From c66241d9acc33ca46fbaf61170bbe6429a1036a3 Mon Sep 17 00:00:00 2001 From: "Pawel Spychalski (DzikuVx)" Date: Sun, 3 Mar 2019 11:15:45 +0100 Subject: [PATCH 001/286] Set max allowed altitude for both FW and MR --- src/main/fc/settings.yaml | 3 +++ src/main/navigation/navigation.c | 15 ++++++++++++++- src/main/navigation/navigation.h | 1 + 3 files changed, 18 insertions(+), 1 deletion(-) diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index bb718961cd..b98e369c35 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -1307,6 +1307,9 @@ groups: - name: nav_max_terrain_follow_alt field: general.max_terrain_follow_altitude max: 1000 + - name: nav_max_altitude + field: general.max_altitude + max: 65000 - name: nav_rth_altitude field: general.rth_altitude max: 65000 diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c index 5575dc8348..da3a484af5 100755 --- a/src/main/navigation/navigation.c +++ b/src/main/navigation/navigation.c @@ -77,7 +77,7 @@ PG_DECLARE_ARRAY(navWaypoint_t, NAV_MAX_WAYPOINTS, nonVolatileWaypointList); PG_REGISTER_ARRAY(navWaypoint_t, NAV_MAX_WAYPOINTS, nonVolatileWaypointList, PG_WAYPOINT_MISSION_STORAGE, 0); #endif -PG_REGISTER_WITH_RESET_TEMPLATE(navConfig_t, navConfig, PG_NAV_CONFIG, 3); +PG_REGISTER_WITH_RESET_TEMPLATE(navConfig_t, navConfig, PG_NAV_CONFIG, 4); PG_RESET_TEMPLATE(navConfig_t, navConfig, .general = { @@ -112,6 +112,7 @@ PG_RESET_TEMPLATE(navConfig_t, navConfig, .rth_home_altitude = 0, .rth_abort_threshold = 50000, // 500m - should be safe for all aircraft .max_terrain_follow_altitude = 100, // max 1m altitude in terrain following mode + .max_altitude = 0, // Altitude limit disabled by default }, // MC-specific @@ -2289,6 +2290,18 @@ void updateClimbRateToAltitudeController(float desiredClimbRate, climbRateToAlti posControl.desiredState.pos.z = altitudeToUse; } else { + + /* + * If max altitude is set, reset climb rate if altitude is reached and climb rate is > 0 + * In other words, when altitude is reached, allow it only to shrink + */ + if (navConfig()->general.max_altitude > 0 && + altitudeToUse >= navConfig()->general.max_altitude && + desiredClimbRate > 0 + ) { + desiredClimbRate = 0; + } + if (STATE(FIXED_WING)) { // Fixed wing climb rate controller is open-loop. We simply move the known altitude target float timeDelta = US2S(currentTimeUs - lastUpdateTimeUs); diff --git a/src/main/navigation/navigation.h b/src/main/navigation/navigation.h index 6a7c9d19e9..989c154c88 100755 --- a/src/main/navigation/navigation.h +++ b/src/main/navigation/navigation.h @@ -153,6 +153,7 @@ typedef struct navConfig_s { uint16_t min_rth_distance; // 0 Disables. Minimal distance for RTH in cm, otherwise it will just autoland uint16_t rth_abort_threshold; // Initiate emergency landing if during RTH we get this much [cm] away from home uint16_t max_terrain_follow_altitude; // Max altitude to be used in SURFACE TRACKING mode + uint16_t max_altitude; // Max altitude when in AltHold mode (not Surface Following) } general; struct { From 683a73378b20bfc117cd49a3d10e5055b6f4ee0a Mon Sep 17 00:00:00 2001 From: "Pawel Spychalski (DzikuVx)" Date: Sun, 3 Mar 2019 11:24:44 +0100 Subject: [PATCH 002/286] Docs update --- docs/Cli.md | 1 + 1 file changed, 1 insertion(+) diff --git a/docs/Cli.md b/docs/Cli.md index 547e1adea5..e57a90f5cb 100644 --- a/docs/Cli.md +++ b/docs/Cli.md @@ -167,6 +167,7 @@ After restoring it's always a good idea to `dump` or `diff` the settings once ag | nav_rth_climb_ignore_emerg | OFF | If set to ON, aircraft will execute initial climb regardless of position sensor (GPS) status. | | nav_rth_alt_mode | AT_LEAST | Configure how the aircraft will manage altitude on the way home, see Navigation modes on wiki for more details | | nav_rth_altitude | 1000 | Used in EXTRA, FIXED and AT_LEAST rth alt modes [cm] (Default 1000 means 10 meters) | +| nav_max_altitude | 0 | Allowed max altitude in [cm] in all cases when Altitude control is enabled. This does not affects manual modes. If AltHold enabled when above allowed altitude, it will not descend, only do not allow to climb higher (Default 0 means disabled) | | nav_rth_home_altitude | 0 | Aircraft will climb/descend to this altitude after reaching home if landing is not enabled. Set to 0 to stay at `nav_rth_altitude` (default) [cm] | | nav_rth_abort_threshold | 50000 | RTH sanity checking feature will notice if distance to home is increasing during RTH and once amount of increase exceeds the threshold defined by this parameter, instead of continuing RTH machine will enter emergency landing, self-level and go down safely. Default is 500m which is safe enough for both multirotor machines and airplanes. [cm] | | nav_mc_bank_angle | 30 | Maximum banking angle (deg) that multicopter navigation is allowed to set. Machine must be able to satisfy this angle without loosing altitude | From 9f2e80029c19e0057551cc0d3cc151a6fd9ba473 Mon Sep 17 00:00:00 2001 From: "Pawel Spychalski (DzikuVx)" Date: Sat, 28 Sep 2019 20:26:58 +0200 Subject: [PATCH 003/286] dzukuvx-bno055-secondary-imu --- make/source.mk | 2 + src/main/build/debug.h | 1 + src/main/drivers/accgyro/accgyro_bno055.c | 77 +++++++++++++++++++++++ src/main/drivers/accgyro/accgyro_bno055.h | 25 ++++++++ src/main/drivers/bus.h | 1 + src/main/fc/fc_core.c | 15 +++++ src/main/fc/fc_core.h | 1 + src/main/fc/fc_tasks.c | 7 +++ src/main/fc/settings.yaml | 2 +- src/main/scheduler/scheduler.h | 1 + src/main/target/common_hardware.c | 4 +- 11 files changed, 134 insertions(+), 2 deletions(-) create mode 100644 src/main/drivers/accgyro/accgyro_bno055.c create mode 100644 src/main/drivers/accgyro/accgyro_bno055.h diff --git a/make/source.mk b/make/source.mk index 3a0d90d1e1..1da9d25bd4 100644 --- a/make/source.mk +++ b/make/source.mk @@ -66,6 +66,8 @@ COMMON_SRC = \ drivers/temperature/ds18b20.c \ drivers/temperature/lm75.c \ drivers/pitotmeter_ms4525.c \ + drivers/pitotmeter_ms4525.c \ + drivers/accgyro/accgyro_bno055.c \ fc/cli.c \ fc/config.c \ fc/controlrate_profile.c \ diff --git a/src/main/build/debug.h b/src/main/build/debug.h index b7453e8836..f4a03274d8 100644 --- a/src/main/build/debug.h +++ b/src/main/build/debug.h @@ -74,5 +74,6 @@ typedef enum { DEBUG_FFT, DEBUG_FFT_TIME, DEBUG_FFT_FREQ, + DEBUG_IMU2, DEBUG_COUNT } debugType_e; diff --git a/src/main/drivers/accgyro/accgyro_bno055.c b/src/main/drivers/accgyro/accgyro_bno055.c new file mode 100644 index 0000000000..ef84a30090 --- /dev/null +++ b/src/main/drivers/accgyro/accgyro_bno055.c @@ -0,0 +1,77 @@ +/* + * This file is part of INAV. + * + * This Source Code Form is subject to the terms of the Mozilla Public + * License, v. 2.0. If a copy of the MPL was not distributed with this file, + * You can obtain one at http://mozilla.org/MPL/2.0/. + * + * Alternatively, the contents of this file may be used under the terms + * of the GNU General Public License Version 3, as described below: + * + * This file is free software: you may copy, redistribute and/or modify + * it under the terms of the GNU General Public License as published by the + * Free Software Foundation, either version 3 of the License, or (at your + * option) any later version. + * + * This file is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General + * Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see http://www.gnu.org/licenses/. + */ + +#include +#include +#include "drivers/bus.h" +#include "drivers/time.h" +#include "build/debug.h" + +static busDevice_t * busDev; + +static bool deviceDetect(busDevice_t * busDev) +{ + for (int retry = 0; retry < 5; retry++) { + uint8_t sig; + + delay(150); + + bool ack = busRead(busDev, 0x00, &sig); + if (ack) { + return true; + } + }; + + return false; +} + +bool bno055Init(void) +{ + busDev = busDeviceInit(BUSTYPE_I2C, DEVHW_BNO055, 0, 0); + if (busDev == NULL) { + DEBUG_SET(DEBUG_IMU2, 2, 1); + return false; + } + + if (!deviceDetect(busDev)) { + DEBUG_SET(DEBUG_IMU2, 2, 2); + busDeviceDeInit(busDev); + return false; + } + + // /* Reset device */ + // busWrite(busDev, PCA9685_MODE1, 0x00); + + // /* Set refresh rate */ + // pca9685setPWMFreq(PCA9685_SERVO_FREQUENCY); + + // delay(1); + + // for (uint8_t i = 0; i < PCA9685_SERVO_COUNT; i++) { + // pca9685setPWMOn(i, 0); + // pca9685setPWMOff(i, 1500); + // } + + return true; +} \ No newline at end of file diff --git a/src/main/drivers/accgyro/accgyro_bno055.h b/src/main/drivers/accgyro/accgyro_bno055.h new file mode 100644 index 0000000000..d62af1d53e --- /dev/null +++ b/src/main/drivers/accgyro/accgyro_bno055.h @@ -0,0 +1,25 @@ +/* + * This file is part of INAV. + * + * This Source Code Form is subject to the terms of the Mozilla Public + * License, v. 2.0. If a copy of the MPL was not distributed with this file, + * You can obtain one at http://mozilla.org/MPL/2.0/. + * + * Alternatively, the contents of this file may be used under the terms + * of the GNU General Public License Version 3, as described below: + * + * This file is free software: you may copy, redistribute and/or modify + * it under the terms of the GNU General Public License as published by the + * Free Software Foundation, either version 3 of the License, or (at your + * option) any later version. + * + * This file is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General + * Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see http://www.gnu.org/licenses/. + */ + +bool bno055Init(void); \ No newline at end of file diff --git a/src/main/drivers/bus.h b/src/main/drivers/bus.h index 283d45e3f4..83a25a6c17 100755 --- a/src/main/drivers/bus.h +++ b/src/main/drivers/bus.h @@ -142,6 +142,7 @@ typedef enum { DEVHW_M25P16, // SPI NOR flash DEVHW_UG2864, // I2C OLED display DEVHW_SDCARD, // Generic SD-Card + DEVHW_BNO055, // BNO055 IMU } devHardwareType_e; typedef enum { diff --git a/src/main/fc/fc_core.c b/src/main/fc/fc_core.c index 497e153be6..7066ed2e06 100755 --- a/src/main/fc/fc_core.c +++ b/src/main/fc/fc_core.c @@ -37,6 +37,7 @@ #include "drivers/time.h" #include "drivers/system.h" #include "drivers/pwm_output.h" +#include "drivers/accgyro/accgyro_bno055.h" #include "sensors/sensors.h" #include "sensors/diagnostics.h" @@ -748,6 +749,20 @@ static float calculateThrottleTiltCompensationFactor(uint8_t throttleTiltCompens } } +void taskSecondaryImu(timeUs_t currentTimeUs) +{ + static bool secondaryImuPresent = false; + static bool secondaryImuChecked = false; + + if (!secondaryImuChecked) { + secondaryImuPresent = bno055Init(); + secondaryImuChecked = true; + } + + DEBUG_SET(DEBUG_IMU2, 0, secondaryImuChecked); + DEBUG_SET(DEBUG_IMU2, 1, secondaryImuPresent); +} + void taskMainPidLoop(timeUs_t currentTimeUs) { cycleTime = getTaskDeltaTime(TASK_SELF); diff --git a/src/main/fc/fc_core.h b/src/main/fc/fc_core.h index a7317e48d1..7caf10d0f5 100644 --- a/src/main/fc/fc_core.h +++ b/src/main/fc/fc_core.h @@ -46,3 +46,4 @@ bool isCalibrating(void); float getFlightTime(void); void fcReboot(bool bootLoader); +void taskSecondaryImu(timeUs_t currentTimeUs); diff --git a/src/main/fc/fc_tasks.c b/src/main/fc/fc_tasks.c index 553c348ab6..26e5b0f903 100755 --- a/src/main/fc/fc_tasks.c +++ b/src/main/fc/fc_tasks.c @@ -343,6 +343,7 @@ void fcTasksInit(void) #ifdef USE_GLOBAL_FUNCTIONS setTaskEnabled(TASK_GLOBAL_FUNCTIONS, true); #endif + setTaskEnabled(TASK_SECONDARY_IMU, true); } cfTask_t cfTasks[TASK_COUNT] = { @@ -564,4 +565,10 @@ cfTask_t cfTasks[TASK_COUNT] = { .staticPriority = TASK_PRIORITY_IDLE, }, #endif + [TASK_SECONDARY_IMU] = { + .taskName = "IMU2", + .taskFunc = taskSecondaryImu, + .desiredPeriod = TASK_PERIOD_HZ(10), // 10Hz @100msec + .staticPriority = TASK_PRIORITY_IDLE, + }, }; diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 65e47fd1d0..fdd241c464 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -78,7 +78,7 @@ tables: values: ["NONE", "GYRO", "NOTCH", "NAV_LANDING", "FW_ALTITUDE", "AGL", "FLOW_RAW", "FLOW", "SBUS", "FPORT", "ALWAYS", "STAGE2", "SAG_COMP_VOLTAGE", "VIBE", "CRUISE", "REM_FLIGHT_TIME", "SMARTAUDIO", "ACC", "GENERIC", "ITERM_RELAX", - "D_BOOST", "ANTIGRAVITY", "FFT", "FFT_TIME", "FFT_FREQ"] + "D_BOOST", "ANTIGRAVITY", "FFT", "FFT_TIME", "FFT_FREQ", "DEBUG_IMU2"] - name: async_mode values: ["NONE", "GYRO", "ALL"] - name: aux_operator diff --git a/src/main/scheduler/scheduler.h b/src/main/scheduler/scheduler.h index 5cf996f02e..4b1d7c252a 100755 --- a/src/main/scheduler/scheduler.h +++ b/src/main/scheduler/scheduler.h @@ -116,6 +116,7 @@ typedef enum { #ifdef USE_GLOBAL_FUNCTIONS TASK_GLOBAL_FUNCTIONS, #endif + TASK_SECONDARY_IMU, /* Count of real tasks */ TASK_COUNT, diff --git a/src/main/target/common_hardware.c b/src/main/target/common_hardware.c index 7c399dbd32..97eec0be1c 100755 --- a/src/main/target/common_hardware.c +++ b/src/main/target/common_hardware.c @@ -312,7 +312,9 @@ #define PCA9685_I2C_BUS BUS_I2C1 #endif BUSDEV_REGISTER_I2C(busdev_pca9685, DEVHW_PCA9685, PCA9685_I2C_BUS, 0x40, NONE, DEVFLAGS_NONE); - #endif + #endif` #endif + BUSDEV_REGISTER_I2C(busdev_bno055, DEVHW_BNO055, BUS_I2C1, 0x29, NONE, DEVFLAGS_NONE); + #endif // USE_TARGET_HARDWARE_DESCRIPTORS From d2bc52d1848185c51d0303b3de30ac0459f1605b Mon Sep 17 00:00:00 2001 From: "Pawel Spychalski (DzikuVx)" Date: Sat, 28 Sep 2019 21:45:16 +0200 Subject: [PATCH 004/286] Read Euler roll --- src/main/drivers/accgyro/accgyro_bno055.c | 31 ++++++++++++++--------- src/main/drivers/accgyro/accgyro_bno055.h | 12 ++++++++- src/main/fc/fc_core.c | 7 +++++ 3 files changed, 37 insertions(+), 13 deletions(-) diff --git a/src/main/drivers/accgyro/accgyro_bno055.c b/src/main/drivers/accgyro/accgyro_bno055.c index ef84a30090..f13f0d66bd 100644 --- a/src/main/drivers/accgyro/accgyro_bno055.c +++ b/src/main/drivers/accgyro/accgyro_bno055.c @@ -27,6 +27,8 @@ #include "drivers/bus.h" #include "drivers/time.h" #include "build/debug.h" +#include "common/vector.h" +#include "drivers/accgyro/accgyro_bno055.h" static busDevice_t * busDev; @@ -60,18 +62,23 @@ bool bno055Init(void) return false; } - // /* Reset device */ - // busWrite(busDev, PCA9685_MODE1, 0x00); - - // /* Set refresh rate */ - // pca9685setPWMFreq(PCA9685_SERVO_FREQUENCY); - - // delay(1); - - // for (uint8_t i = 0; i < PCA9685_SERVO_COUNT; i++) { - // pca9685setPWMOn(i, 0); - // pca9685setPWMOff(i, 1500); - // } + busWrite(busDev, BNO055_ADDR_PWR_MODE, BNO055_PWR_MODE_NORMAL); //Set power mode NORMAL + delay(25); + busWrite(busDev, BNO055_ADDR_OPR_MODE, BNO055_OPR_MODE_NDOF); //Set operational mode NDOF + delay(50); return true; +} + +fpVector3_t bno055GetEurlerAngles(void) +{ + fpVector3_t eurlerAngles; + + int8_t buf; + busRead(busDev, BNO055_ADDR_EUL_ROLL_LSB, &buf); + eurlerAngles.x = buf; + busRead(busDev, BNO055_ADDR_EUL_ROLL_MSB, &buf); + eurlerAngles.x += buf << 8; + + return eurlerAngles; } \ No newline at end of file diff --git a/src/main/drivers/accgyro/accgyro_bno055.h b/src/main/drivers/accgyro/accgyro_bno055.h index d62af1d53e..8493d7fcd4 100644 --- a/src/main/drivers/accgyro/accgyro_bno055.h +++ b/src/main/drivers/accgyro/accgyro_bno055.h @@ -22,4 +22,14 @@ * along with this program. If not, see http://www.gnu.org/licenses/. */ -bool bno055Init(void); \ No newline at end of file +#define BNO055_ADDR_PWR_MODE 0x3E +#define BNO055_ADDR_OPR_MODE 0x3D + +#define BNO055_PWR_MODE_NORMAL 0x00 +#define BNO055_OPR_MODE_NDOF 0x0C + +#define BNO055_ADDR_EUL_ROLL_LSB 0x1C +#define BNO055_ADDR_EUL_ROLL_MSB 0x1D + +bool bno055Init(void); +fpVector3_t bno055GetEurlerAngles(void) \ No newline at end of file diff --git a/src/main/fc/fc_core.c b/src/main/fc/fc_core.c index 7066ed2e06..52eb1a653a 100755 --- a/src/main/fc/fc_core.c +++ b/src/main/fc/fc_core.c @@ -761,6 +761,13 @@ void taskSecondaryImu(timeUs_t currentTimeUs) DEBUG_SET(DEBUG_IMU2, 0, secondaryImuChecked); DEBUG_SET(DEBUG_IMU2, 1, secondaryImuPresent); + + if (secondaryImuPresent) { + fpVector3_t eulerAngles = bno055GetEurlerAngles(); + DEBUG_SET(DEBUG_IMU2, 5, eulerAngles.x); + DEBUG_SET(DEBUG_IMU2, 6, eulerAngles.y); + DEBUG_SET(DEBUG_IMU2, 7, eulerAngles.z); + } } void taskMainPidLoop(timeUs_t currentTimeUs) From 853adf3872a762391988f9eaf08b3e20e85b8c5d Mon Sep 17 00:00:00 2001 From: "Pawel Spychalski (DzikuVx)" Date: Sat, 28 Sep 2019 22:46:36 +0200 Subject: [PATCH 005/286] Read Euler angles from BNO055 --- src/main/drivers/accgyro/accgyro_bno055.c | 11 ++++++----- src/main/drivers/accgyro/accgyro_bno055.h | 12 +++++++++--- src/main/fc/fc_core.c | 1 + 3 files changed, 16 insertions(+), 8 deletions(-) diff --git a/src/main/drivers/accgyro/accgyro_bno055.c b/src/main/drivers/accgyro/accgyro_bno055.c index f13f0d66bd..3e6daab511 100644 --- a/src/main/drivers/accgyro/accgyro_bno055.c +++ b/src/main/drivers/accgyro/accgyro_bno055.c @@ -74,11 +74,12 @@ fpVector3_t bno055GetEurlerAngles(void) { fpVector3_t eurlerAngles; - int8_t buf; - busRead(busDev, BNO055_ADDR_EUL_ROLL_LSB, &buf); - eurlerAngles.x = buf; - busRead(busDev, BNO055_ADDR_EUL_ROLL_MSB, &buf); - eurlerAngles.x += buf << 8; + uint8_t buf[6]; + busReadBuf(busDev, BNO055_ADDR_EUL_YAW_LSB, buf, 6); + + eurlerAngles.x = ((int16_t)((buf[3] << 8) | buf[2])) / 16; + eurlerAngles.y = ((int16_t)((buf[5] << 8) | buf[4])) / 16; + eurlerAngles.z = ((int16_t)((buf[1] << 8) | buf[0])) / 16; return eurlerAngles; } \ No newline at end of file diff --git a/src/main/drivers/accgyro/accgyro_bno055.h b/src/main/drivers/accgyro/accgyro_bno055.h index 8493d7fcd4..227346bd94 100644 --- a/src/main/drivers/accgyro/accgyro_bno055.h +++ b/src/main/drivers/accgyro/accgyro_bno055.h @@ -22,14 +22,20 @@ * along with this program. If not, see http://www.gnu.org/licenses/. */ +#include "common/vector.h" + #define BNO055_ADDR_PWR_MODE 0x3E #define BNO055_ADDR_OPR_MODE 0x3D #define BNO055_PWR_MODE_NORMAL 0x00 #define BNO055_OPR_MODE_NDOF 0x0C -#define BNO055_ADDR_EUL_ROLL_LSB 0x1C -#define BNO055_ADDR_EUL_ROLL_MSB 0x1D +#define BNO055_ADDR_EUL_YAW_LSB 0x1A +#define BNO055_ADDR_EUL_YAW_MSB 0x1B +#define BNO055_ADDR_EUL_ROLL_LSB 0x1C +#define BNO055_ADDR_EUL_ROLL_MSB 0x1D +#define BNO055_ADDR_EUL_PITCH_LSB 0x1E +#define BNO055_ADDR_EUL_PITCH_MSB 0x1F bool bno055Init(void); -fpVector3_t bno055GetEurlerAngles(void) \ No newline at end of file +fpVector3_t bno055GetEurlerAngles(void); \ No newline at end of file diff --git a/src/main/fc/fc_core.c b/src/main/fc/fc_core.c index 52eb1a653a..b8625d603c 100755 --- a/src/main/fc/fc_core.c +++ b/src/main/fc/fc_core.c @@ -88,6 +88,7 @@ #include "flight/failsafe.h" #include "config/feature.h" +#include "common/vector.h" // June 2013 V2.2-dev From 295be6996e67633db508821367268702ca2b8859 Mon Sep 17 00:00:00 2001 From: "Pawel Spychalski (DzikuVx)" Date: Sun, 29 Sep 2019 10:53:15 +0200 Subject: [PATCH 006/286] Read calibration status --- src/main/drivers/accgyro/accgyro_bno055.c | 15 +++++++++++++++ src/main/drivers/accgyro/accgyro_bno055.h | 15 ++++++++++++--- src/main/fc/fc_core.c | 12 +++++++++--- 3 files changed, 36 insertions(+), 6 deletions(-) diff --git a/src/main/drivers/accgyro/accgyro_bno055.c b/src/main/drivers/accgyro/accgyro_bno055.c index 3e6daab511..f06b6f6650 100644 --- a/src/main/drivers/accgyro/accgyro_bno055.c +++ b/src/main/drivers/accgyro/accgyro_bno055.c @@ -82,4 +82,19 @@ fpVector3_t bno055GetEurlerAngles(void) eurlerAngles.z = ((int16_t)((buf[1] << 8) | buf[0])) / 16; return eurlerAngles; +} + +bno055CalibStat_t bno055GetCalibStat(void) +{ + bno055CalibStat_t stats; + uint8_t buf; + + busRead(busDev, BNO055_ADDR_CALIB_STAT, &buf); + + stats.mag = buf & 0b00000011; + stats.acc = (buf >> 2) & 0b00000011; + stats.gyr = (buf >> 4) & 0b00000011; + stats.sys = (buf >> 6) & 0b00000011; + + return stats; } \ No newline at end of file diff --git a/src/main/drivers/accgyro/accgyro_bno055.h b/src/main/drivers/accgyro/accgyro_bno055.h index 227346bd94..f36cb5f7ae 100644 --- a/src/main/drivers/accgyro/accgyro_bno055.h +++ b/src/main/drivers/accgyro/accgyro_bno055.h @@ -24,8 +24,9 @@ #include "common/vector.h" -#define BNO055_ADDR_PWR_MODE 0x3E -#define BNO055_ADDR_OPR_MODE 0x3D +#define BNO055_ADDR_PWR_MODE 0x3E +#define BNO055_ADDR_OPR_MODE 0x3D +#define BNO055_ADDR_CALIB_STAT 0x35 #define BNO055_PWR_MODE_NORMAL 0x00 #define BNO055_OPR_MODE_NDOF 0x0C @@ -37,5 +38,13 @@ #define BNO055_ADDR_EUL_PITCH_LSB 0x1E #define BNO055_ADDR_EUL_PITCH_MSB 0x1F +typedef struct { + uint8_t sys; + uint8_t gyr; + uint8_t acc; + uint8_t mag; +} bno055CalibStat_t; + bool bno055Init(void); -fpVector3_t bno055GetEurlerAngles(void); \ No newline at end of file +fpVector3_t bno055GetEurlerAngles(void); +bno055CalibStat_t bno055GetCalibStat(void); \ No newline at end of file diff --git a/src/main/fc/fc_core.c b/src/main/fc/fc_core.c index b8625d603c..73953e474f 100755 --- a/src/main/fc/fc_core.c +++ b/src/main/fc/fc_core.c @@ -765,9 +765,15 @@ void taskSecondaryImu(timeUs_t currentTimeUs) if (secondaryImuPresent) { fpVector3_t eulerAngles = bno055GetEurlerAngles(); - DEBUG_SET(DEBUG_IMU2, 5, eulerAngles.x); - DEBUG_SET(DEBUG_IMU2, 6, eulerAngles.y); - DEBUG_SET(DEBUG_IMU2, 7, eulerAngles.z); + DEBUG_SET(DEBUG_IMU2, 0, eulerAngles.x); + DEBUG_SET(DEBUG_IMU2, 1, eulerAngles.y); + DEBUG_SET(DEBUG_IMU2, 2, eulerAngles.z); + + bno055CalibStat_t stats = bno055GetCalibStat(); + DEBUG_SET(DEBUG_IMU2, 3, stats.mag); + DEBUG_SET(DEBUG_IMU2, 4, stats.acc); + DEBUG_SET(DEBUG_IMU2, 5, stats.gyr); + DEBUG_SET(DEBUG_IMU2, 6, stats.sys); } } From 0c12340f6f24308611d81758fb86b92d596f9a02 Mon Sep 17 00:00:00 2001 From: "Pawel Spychalski (DzikuVx)" Date: Sun, 29 Sep 2019 18:54:48 +0200 Subject: [PATCH 007/286] Rotate readouts to match body na groud frame --- make/source.mk | 1 + src/main/config/parameter_group_ids.h | 3 +- src/main/drivers/accgyro/accgyro_bno055.c | 30 +++++++- src/main/drivers/accgyro/accgyro_bno055.h | 16 +---- src/main/fc/fc_core.c | 27 -------- src/main/fc/fc_core.h | 3 +- src/main/fc/fc_tasks.c | 7 +- src/main/fc/settings.yaml | 21 ++++++ src/main/flight/secondary_imu.c | 84 +++++++++++++++++++++++ src/main/flight/secondary_imu.h | 43 ++++++++++++ src/main/scheduler/scheduler.h | 2 + src/main/target/common.h | 3 + 12 files changed, 194 insertions(+), 46 deletions(-) create mode 100644 src/main/flight/secondary_imu.c create mode 100644 src/main/flight/secondary_imu.h diff --git a/make/source.mk b/make/source.mk index 1da9d25bd4..303044c25e 100644 --- a/make/source.mk +++ b/make/source.mk @@ -95,6 +95,7 @@ COMMON_SRC = \ flight/servos.c \ flight/wind_estimator.c \ flight/gyroanalyse.c \ + flight/secondary_imu.c \ io/beeper.c \ io/lights.c \ io/pwmdriver_i2c.c \ diff --git a/src/main/config/parameter_group_ids.h b/src/main/config/parameter_group_ids.h index 53c31c3124..811b33028b 100644 --- a/src/main/config/parameter_group_ids.h +++ b/src/main/config/parameter_group_ids.h @@ -108,7 +108,8 @@ #define PG_RCDEVICE_CONFIG 1018 #define PG_GENERAL_SETTINGS 1019 #define PG_GLOBAL_FUNCTIONS 1020 -#define PG_INAV_END 1020 +#define PG_SECONDARY_IMU 1021 +#define PG_INAV_END 1021 // OSD configuration (subject to change) //#define PG_OSD_FONT_CONFIG 2047 diff --git a/src/main/drivers/accgyro/accgyro_bno055.c b/src/main/drivers/accgyro/accgyro_bno055.c index f06b6f6650..97636e9c97 100644 --- a/src/main/drivers/accgyro/accgyro_bno055.c +++ b/src/main/drivers/accgyro/accgyro_bno055.c @@ -30,6 +30,22 @@ #include "common/vector.h" #include "drivers/accgyro/accgyro_bno055.h" +#ifdef USE_IMU_BNO055 + +#define BNO055_ADDR_PWR_MODE 0x3E +#define BNO055_ADDR_OPR_MODE 0x3D +#define BNO055_ADDR_CALIB_STAT 0x35 + +#define BNO055_PWR_MODE_NORMAL 0x00 +#define BNO055_OPR_MODE_NDOF 0x0C + +#define BNO055_ADDR_EUL_YAW_LSB 0x1A +#define BNO055_ADDR_EUL_YAW_MSB 0x1B +#define BNO055_ADDR_EUL_ROLL_LSB 0x1C +#define BNO055_ADDR_EUL_ROLL_MSB 0x1D +#define BNO055_ADDR_EUL_PITCH_LSB 0x1E +#define BNO055_ADDR_EUL_PITCH_MSB 0x1F + static busDevice_t * busDev; static bool deviceDetect(busDevice_t * busDev) @@ -70,6 +86,16 @@ bool bno055Init(void) return true; } +void bno055FetchEulerAngles(int32_t * buffer) +{ + uint8_t buf[6]; + busReadBuf(busDev, BNO055_ADDR_EUL_YAW_LSB, buf, 6); + + buffer[0] = ((int16_t)((buf[3] << 8) | buf[2])) / 16; + buffer[1] = ((int16_t)((buf[5] << 8) | buf[4])) / -16; //Pitch has to be reversed to match INAV notation + buffer[2] = ((int16_t)((buf[1] << 8) | buf[0])) / 16; +} + fpVector3_t bno055GetEurlerAngles(void) { fpVector3_t eurlerAngles; @@ -97,4 +123,6 @@ bno055CalibStat_t bno055GetCalibStat(void) stats.sys = (buf >> 6) & 0b00000011; return stats; -} \ No newline at end of file +} + +#endif \ No newline at end of file diff --git a/src/main/drivers/accgyro/accgyro_bno055.h b/src/main/drivers/accgyro/accgyro_bno055.h index f36cb5f7ae..c2f7ae5b79 100644 --- a/src/main/drivers/accgyro/accgyro_bno055.h +++ b/src/main/drivers/accgyro/accgyro_bno055.h @@ -21,23 +21,10 @@ * You should have received a copy of the GNU General Public License * along with this program. If not, see http://www.gnu.org/licenses/. */ +#pragma once #include "common/vector.h" -#define BNO055_ADDR_PWR_MODE 0x3E -#define BNO055_ADDR_OPR_MODE 0x3D -#define BNO055_ADDR_CALIB_STAT 0x35 - -#define BNO055_PWR_MODE_NORMAL 0x00 -#define BNO055_OPR_MODE_NDOF 0x0C - -#define BNO055_ADDR_EUL_YAW_LSB 0x1A -#define BNO055_ADDR_EUL_YAW_MSB 0x1B -#define BNO055_ADDR_EUL_ROLL_LSB 0x1C -#define BNO055_ADDR_EUL_ROLL_MSB 0x1D -#define BNO055_ADDR_EUL_PITCH_LSB 0x1E -#define BNO055_ADDR_EUL_PITCH_MSB 0x1F - typedef struct { uint8_t sys; uint8_t gyr; @@ -47,4 +34,5 @@ typedef struct { bool bno055Init(void); fpVector3_t bno055GetEurlerAngles(void); +void bno055FetchEulerAngles(int32_t * buffer); bno055CalibStat_t bno055GetCalibStat(void); \ No newline at end of file diff --git a/src/main/fc/fc_core.c b/src/main/fc/fc_core.c index 73953e474f..2cfd5a0ddf 100755 --- a/src/main/fc/fc_core.c +++ b/src/main/fc/fc_core.c @@ -750,33 +750,6 @@ static float calculateThrottleTiltCompensationFactor(uint8_t throttleTiltCompens } } -void taskSecondaryImu(timeUs_t currentTimeUs) -{ - static bool secondaryImuPresent = false; - static bool secondaryImuChecked = false; - - if (!secondaryImuChecked) { - secondaryImuPresent = bno055Init(); - secondaryImuChecked = true; - } - - DEBUG_SET(DEBUG_IMU2, 0, secondaryImuChecked); - DEBUG_SET(DEBUG_IMU2, 1, secondaryImuPresent); - - if (secondaryImuPresent) { - fpVector3_t eulerAngles = bno055GetEurlerAngles(); - DEBUG_SET(DEBUG_IMU2, 0, eulerAngles.x); - DEBUG_SET(DEBUG_IMU2, 1, eulerAngles.y); - DEBUG_SET(DEBUG_IMU2, 2, eulerAngles.z); - - bno055CalibStat_t stats = bno055GetCalibStat(); - DEBUG_SET(DEBUG_IMU2, 3, stats.mag); - DEBUG_SET(DEBUG_IMU2, 4, stats.acc); - DEBUG_SET(DEBUG_IMU2, 5, stats.gyr); - DEBUG_SET(DEBUG_IMU2, 6, stats.sys); - } -} - void taskMainPidLoop(timeUs_t currentTimeUs) { cycleTime = getTaskDeltaTime(TASK_SELF); diff --git a/src/main/fc/fc_core.h b/src/main/fc/fc_core.h index 7caf10d0f5..34eb6fe68b 100644 --- a/src/main/fc/fc_core.h +++ b/src/main/fc/fc_core.h @@ -45,5 +45,4 @@ void emergencyArmingUpdate(bool armingSwitchIsOn); bool isCalibrating(void); float getFlightTime(void); -void fcReboot(bool bootLoader); -void taskSecondaryImu(timeUs_t currentTimeUs); +void fcReboot(bool bootLoader); \ No newline at end of file diff --git a/src/main/fc/fc_tasks.c b/src/main/fc/fc_tasks.c index 26e5b0f903..e00dae77af 100755 --- a/src/main/fc/fc_tasks.c +++ b/src/main/fc/fc_tasks.c @@ -47,6 +47,7 @@ #include "flight/mixer.h" #include "flight/pid.h" #include "flight/wind_estimator.h" +#include "flight/secondary_imu.h" #include "navigation/navigation.h" @@ -343,7 +344,9 @@ void fcTasksInit(void) #ifdef USE_GLOBAL_FUNCTIONS setTaskEnabled(TASK_GLOBAL_FUNCTIONS, true); #endif - setTaskEnabled(TASK_SECONDARY_IMU, true); +#ifdef USE_SECONDARY_IMU + setTaskEnabled(TASK_SECONDARY_IMU, secondaryImuConfig()->enabled); +#endif } cfTask_t cfTasks[TASK_COUNT] = { @@ -565,10 +568,12 @@ cfTask_t cfTasks[TASK_COUNT] = { .staticPriority = TASK_PRIORITY_IDLE, }, #endif +#ifdef USE_SECONDARY_IMU [TASK_SECONDARY_IMU] = { .taskName = "IMU2", .taskFunc = taskSecondaryImu, .desiredPeriod = TASK_PERIOD_HZ(10), // 10Hz @100msec .staticPriority = TASK_PRIORITY_IDLE, }, +#endif }; diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index fdd241c464..b4988d97ae 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -303,6 +303,27 @@ groups: type: uint8_t table: alignment + - name: PG_SECONDARY_IMU + type: secondaryImuConfig_t + headers: ["flight/secondary_imu.h"] + condition: USE_OPFLOW + members: + - name: imu2_enabled + field: enabled + type: bool + - name: imu2_align_roll + field: rollDeciDegrees + min: -1800 + max: 3600 + - name: imu2_align_pitch + field: pitchDeciDegrees + min: -1800 + max: 3600 + - name: imu2_align_yaw + field: yawDeciDegrees + min: -1800 + max: 3600 + - name: PG_COMPASS_CONFIG type: compassConfig_t headers: ["sensors/compass.h"] diff --git a/src/main/flight/secondary_imu.c b/src/main/flight/secondary_imu.c new file mode 100644 index 0000000000..34d494fe1b --- /dev/null +++ b/src/main/flight/secondary_imu.c @@ -0,0 +1,84 @@ +/* + * This file is part of INAV. + * + * This Source Code Form is subject to the terms of the Mozilla Public + * License, v. 2.0. If a copy of the MPL was not distributed with this file, + * You can obtain one at http://mozilla.org/MPL/2.0/. + * + * Alternatively, the contents of this file may be used under the terms + * of the GNU General Public License Version 3, as described below: + * + * This file is free software: you may copy, redistribute and/or modify + * it under the terms of the GNU General Public License as published by the + * Free Software Foundation, either version 3 of the License, or (at your + * option) any later version. + * + * This file is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General + * Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see http://www.gnu.org/licenses/. + */ + +#include "flight/secondary_imu.h" +#include "config/parameter_group_ids.h" +#include "sensors/boardalignment.h" + +#include "build/debug.h" + +#include "drivers/sensor.h" +#include "drivers/accgyro/accgyro_bno055.h" + +PG_REGISTER_WITH_RESET_TEMPLATE(secondaryImuConfig_t, secondaryImuConfig, PG_SECONDARY_IMU, 0); + +PG_RESET_TEMPLATE(secondaryImuConfig_t, secondaryImuConfig, + .enabled = 0, + .rollDeciDegrees = 0, + .pitchDeciDegrees = 0, + .yawDeciDegrees = 0, +); + +EXTENDED_FASTRAM secondaryImuState_t secondaryImuState; + +void taskSecondaryImu(timeUs_t currentTimeUs) +{ + UNUSED(currentTimeUs); + static bool secondaryImuPresent = false; + static bool secondaryImuChecked = false; + + if (!secondaryImuChecked) { + secondaryImuPresent = bno055Init(); + secondaryImuChecked = true; + } + + if (secondaryImuPresent) + { + int32_t angles[3]; + bno055FetchEulerAngles(angles); + + const fpVector3_t v = { + .x = angles[0], + .y = angles[1], + .z = angles[2], + }; + + fpVector3_t rotated; + + fp_angles_t compassAngles = { + .angles.roll = DECIDEGREES_TO_RADIANS(secondaryImuConfig()->rollDeciDegrees), + .angles.pitch = DECIDEGREES_TO_RADIANS(secondaryImuConfig()->pitchDeciDegrees), + .angles.yaw = DECIDEGREES_TO_RADIANS(secondaryImuConfig()->yawDeciDegrees), + }; + fpMat3_t rotationMatrix; + rotationMatrixFromAngles(&rotationMatrix, &compassAngles); + rotationMatrixRotateVector(&rotated, &v, &rotationMatrix); + rotated.z = ((int32_t)(rotated.z + DECIDEGREES_TO_DEGREES(secondaryImuConfig()->yawDeciDegrees))) % 360; + + DEBUG_SET(DEBUG_IMU2, 0, rotated.x); + DEBUG_SET(DEBUG_IMU2, 1, rotated.y); + DEBUG_SET(DEBUG_IMU2, 2, rotated.z); + DEBUG_SET(DEBUG_IMU2, 3, angles[2]); + } +} \ No newline at end of file diff --git a/src/main/flight/secondary_imu.h b/src/main/flight/secondary_imu.h new file mode 100644 index 0000000000..f69edb47d1 --- /dev/null +++ b/src/main/flight/secondary_imu.h @@ -0,0 +1,43 @@ +/* + * This file is part of INAV. + * + * This Source Code Form is subject to the terms of the Mozilla Public + * License, v. 2.0. If a copy of the MPL was not distributed with this file, + * You can obtain one at http://mozilla.org/MPL/2.0/. + * + * Alternatively, the contents of this file may be used under the terms + * of the GNU General Public License Version 3, as described below: + * + * This file is free software: you may copy, redistribute and/or modify + * it under the terms of the GNU General Public License as published by the + * Free Software Foundation, either version 3 of the License, or (at your + * option) any later version. + * + * This file is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General + * Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see http://www.gnu.org/licenses/. + */ + +#pragma once + +#include "config/parameter_group.h" +#include "common/time.h" + +typedef struct secondaryImuConfig_s { + uint8_t enabled; + int16_t rollDeciDegrees; + int16_t pitchDeciDegrees; + int16_t yawDeciDegrees; +} secondaryImuConfig_t; + +typedef struct secondaryImuState_s { + int16_t rawEulerAngles[3]; +} secondaryImuState_t; + +PG_DECLARE(secondaryImuConfig_t, secondaryImuConfig); + +void taskSecondaryImu(timeUs_t currentTimeUs); \ No newline at end of file diff --git a/src/main/scheduler/scheduler.h b/src/main/scheduler/scheduler.h index 4b1d7c252a..b20f3a7066 100755 --- a/src/main/scheduler/scheduler.h +++ b/src/main/scheduler/scheduler.h @@ -116,7 +116,9 @@ typedef enum { #ifdef USE_GLOBAL_FUNCTIONS TASK_GLOBAL_FUNCTIONS, #endif +#ifdef USE_SECONDARY_IMU TASK_SECONDARY_IMU, +#endif /* Count of real tasks */ TASK_COUNT, diff --git a/src/main/target/common.h b/src/main/target/common.h index 29d5598c20..0d01421649 100755 --- a/src/main/target/common.h +++ b/src/main/target/common.h @@ -108,6 +108,9 @@ #define USE_D_BOOST #define USE_ANTIGRAVITY +#define USE_SECONDARY_IMU +#define USE_IMU_BNO055 + #else // FLASH_SIZE < 256 #define LOG_LEVEL_MAXIMUM LOG_LEVEL_ERROR #endif From 73b38494cebccd5bd3842f2f6ede12cab938b646 Mon Sep 17 00:00:00 2001 From: "Pawel Spychalski (DzikuVx)" Date: Mon, 30 Sep 2019 17:12:10 +0200 Subject: [PATCH 008/286] Use secondary IMU for OSD heading and AHI --- src/main/drivers/accgyro/accgyro_bno055.c | 8 ++-- src/main/drivers/accgyro/accgyro_bno055.h | 2 +- src/main/fc/settings.yaml | 6 +++ src/main/flight/secondary_imu.c | 50 +++++++++++++++-------- src/main/flight/secondary_imu.h | 10 ++++- src/main/io/osd.c | 35 ++++++++++++++-- src/main/target/common.h | 3 -- src/main/target/common_hardware.c | 2 +- src/main/target/common_post.h | 5 +++ 9 files changed, 92 insertions(+), 29 deletions(-) diff --git a/src/main/drivers/accgyro/accgyro_bno055.c b/src/main/drivers/accgyro/accgyro_bno055.c index 97636e9c97..94aa3ef218 100644 --- a/src/main/drivers/accgyro/accgyro_bno055.c +++ b/src/main/drivers/accgyro/accgyro_bno055.c @@ -86,14 +86,14 @@ bool bno055Init(void) return true; } -void bno055FetchEulerAngles(int32_t * buffer) +void bno055FetchEulerAngles(int16_t * buffer) { uint8_t buf[6]; busReadBuf(busDev, BNO055_ADDR_EUL_YAW_LSB, buf, 6); - buffer[0] = ((int16_t)((buf[3] << 8) | buf[2])) / 16; - buffer[1] = ((int16_t)((buf[5] << 8) | buf[4])) / -16; //Pitch has to be reversed to match INAV notation - buffer[2] = ((int16_t)((buf[1] << 8) | buf[0])) / 16; + buffer[0] = ((int16_t)((buf[3] << 8) | buf[2])) / 1.6f; + buffer[1] = ((int16_t)((buf[5] << 8) | buf[4])) / -1.6f; //Pitch has to be reversed to match INAV notation + buffer[2] = ((int16_t)((buf[1] << 8) | buf[0])) / 1.6f; } fpVector3_t bno055GetEurlerAngles(void) diff --git a/src/main/drivers/accgyro/accgyro_bno055.h b/src/main/drivers/accgyro/accgyro_bno055.h index c2f7ae5b79..b6f7204cab 100644 --- a/src/main/drivers/accgyro/accgyro_bno055.h +++ b/src/main/drivers/accgyro/accgyro_bno055.h @@ -34,5 +34,5 @@ typedef struct { bool bno055Init(void); fpVector3_t bno055GetEurlerAngles(void); -void bno055FetchEulerAngles(int32_t * buffer); +void bno055FetchEulerAngles(int16_t * buffer); bno055CalibStat_t bno055GetCalibStat(void); \ No newline at end of file diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index b4988d97ae..f64d5ef8e5 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -311,6 +311,12 @@ groups: - name: imu2_enabled field: enabled type: bool + - name: imu2_use_for_osd_heading + field: useForOsdHeading + type: bool + - name: imu2_use_for_osd_ahi + field: useForOsdAHI + type: bool - name: imu2_align_roll field: rollDeciDegrees min: -1800 diff --git a/src/main/flight/secondary_imu.c b/src/main/flight/secondary_imu.c index 34d494fe1b..e1f46d1fe0 100644 --- a/src/main/flight/secondary_imu.c +++ b/src/main/flight/secondary_imu.c @@ -38,6 +38,8 @@ PG_RESET_TEMPLATE(secondaryImuConfig_t, secondaryImuConfig, .rollDeciDegrees = 0, .pitchDeciDegrees = 0, .yawDeciDegrees = 0, + .useForOsdHeading = 0, + .useForOsdAHI = 0, ); EXTENDED_FASTRAM secondaryImuState_t secondaryImuState; @@ -45,40 +47,56 @@ EXTENDED_FASTRAM secondaryImuState_t secondaryImuState; void taskSecondaryImu(timeUs_t currentTimeUs) { UNUSED(currentTimeUs); - static bool secondaryImuPresent = false; static bool secondaryImuChecked = false; if (!secondaryImuChecked) { - secondaryImuPresent = bno055Init(); + secondaryImuState.active = bno055Init(); secondaryImuChecked = true; } - if (secondaryImuPresent) + if (secondaryImuState.active) { - int32_t angles[3]; - bno055FetchEulerAngles(angles); + bno055FetchEulerAngles(secondaryImuState.eulerAngles.raw); + //TODO this way of rotating a vector makes no sense, something simpler have to be developed const fpVector3_t v = { - .x = angles[0], - .y = angles[1], - .z = angles[2], + .x = secondaryImuState.eulerAngles.raw[0], + .y = secondaryImuState.eulerAngles.raw[1], + .z = secondaryImuState.eulerAngles.raw[2], }; fpVector3_t rotated; - fp_angles_t compassAngles = { + fp_angles_t imuAngles = { .angles.roll = DECIDEGREES_TO_RADIANS(secondaryImuConfig()->rollDeciDegrees), .angles.pitch = DECIDEGREES_TO_RADIANS(secondaryImuConfig()->pitchDeciDegrees), .angles.yaw = DECIDEGREES_TO_RADIANS(secondaryImuConfig()->yawDeciDegrees), }; fpMat3_t rotationMatrix; - rotationMatrixFromAngles(&rotationMatrix, &compassAngles); + rotationMatrixFromAngles(&rotationMatrix, &imuAngles); rotationMatrixRotateVector(&rotated, &v, &rotationMatrix); - rotated.z = ((int32_t)(rotated.z + DECIDEGREES_TO_DEGREES(secondaryImuConfig()->yawDeciDegrees))) % 360; + rotated.z = ((int32_t)(rotated.z + secondaryImuConfig()->yawDeciDegrees)) % 3600; + + secondaryImuState.eulerAngles.values.roll = rotated.x; + secondaryImuState.eulerAngles.values.pitch = rotated.y; + secondaryImuState.eulerAngles.values.yaw = rotated.z; + + static uint8_t tick = 0; + tick++; + if (tick == 10) { + secondaryImuState.calibrationStatus = bno055GetCalibStat(); + tick = 0; + } + + DEBUG_SET(DEBUG_IMU2, 0, secondaryImuState.eulerAngles.values.roll); + DEBUG_SET(DEBUG_IMU2, 1, secondaryImuState.eulerAngles.values.pitch); + DEBUG_SET(DEBUG_IMU2, 2, secondaryImuState.eulerAngles.values.yaw); + + DEBUG_SET(DEBUG_IMU2, 3, secondaryImuState.calibrationStatus.mag); + DEBUG_SET(DEBUG_IMU2, 4, secondaryImuState.calibrationStatus.gyr); + DEBUG_SET(DEBUG_IMU2, 5, secondaryImuState.calibrationStatus.acc); + - DEBUG_SET(DEBUG_IMU2, 0, rotated.x); - DEBUG_SET(DEBUG_IMU2, 1, rotated.y); - DEBUG_SET(DEBUG_IMU2, 2, rotated.z); - DEBUG_SET(DEBUG_IMU2, 3, angles[2]); } -} \ No newline at end of file +} + diff --git a/src/main/flight/secondary_imu.h b/src/main/flight/secondary_imu.h index f69edb47d1..d86d9b566e 100644 --- a/src/main/flight/secondary_imu.h +++ b/src/main/flight/secondary_imu.h @@ -26,18 +26,26 @@ #include "config/parameter_group.h" #include "common/time.h" +#include "sensors/sensors.h" +#include "drivers/accgyro/accgyro_bno055.h" typedef struct secondaryImuConfig_s { uint8_t enabled; int16_t rollDeciDegrees; int16_t pitchDeciDegrees; int16_t yawDeciDegrees; + uint8_t useForOsdHeading; + uint8_t useForOsdAHI; } secondaryImuConfig_t; typedef struct secondaryImuState_s { - int16_t rawEulerAngles[3]; + flightDynamicsTrims_t eulerAngles; + bno055CalibStat_t calibrationStatus; + uint8_t active; } secondaryImuState_t; +extern secondaryImuState_t secondaryImuState; + PG_DECLARE(secondaryImuConfig_t, secondaryImuConfig); void taskSecondaryImu(timeUs_t currentTimeUs); \ No newline at end of file diff --git a/src/main/io/osd.c b/src/main/io/osd.c index efca3f7f17..bb16a75d4d 100755 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -81,6 +81,7 @@ #include "flight/pid.h" #include "flight/rth_estimator.h" #include "flight/wind_estimator.h" +#include "flight/secondary_imu.h" #include "navigation/navigation.h" #include "navigation/navigation_private.h" @@ -966,12 +967,28 @@ static uint8_t osdUpdateSidebar(osd_sidebar_scroll_e scroll, osd_sidebar_t *side static bool osdIsHeadingValid(void) { +#ifdef USE_SECONDARY_IMU + if (secondaryImuConfig()->useForOsdHeading) { + return true; + } else { + return isImuHeadingValid(); + } +#else return isImuHeadingValid(); +#endif } int16_t osdGetHeading(void) { - return attitude.values.yaw; +#ifdef USE_SECONDARY_IMU + if (secondaryImuConfig()->useForOsdHeading) { + return secondaryImuState.eulerAngles.values.yaw; + } else { + return attitude.values.yaw; + } +#else + return secondaryImuState.eulerAngles.values.yaw; +#endif } // Returns a heading angle in degrees normalized to [0, 360). @@ -1767,8 +1784,20 @@ static bool osdDrawSingleElement(uint8_t item) float pitch_rad_to_char = (float)(AH_HEIGHT / 2 + 0.5) / DEGREES_TO_RADIANS(osdConfig()->ahi_max_pitch); - float rollAngle = DECIDEGREES_TO_RADIANS(attitude.values.roll); - float pitchAngle = DECIDEGREES_TO_RADIANS(attitude.values.pitch); + float rollAngle; + float pitchAngle; +#ifdef USE_SECONDARY_IMU + if (secondaryImuConfig()->useForOsdAHI) { + rollAngle = DECIDEGREES_TO_RADIANS(secondaryImuState.eulerAngles.values.roll); + pitchAngle = DECIDEGREES_TO_RADIANS(secondaryImuState.eulerAngles.values.pitch); + } else { + rollAngle = DECIDEGREES_TO_RADIANS(attitude.values.roll); + pitchAngle = DECIDEGREES_TO_RADIANS(attitude.values.pitch); + } +#else + rollAngle = DECIDEGREES_TO_RADIANS(attitude.values.roll); + pitchAngle = DECIDEGREES_TO_RADIANS(attitude.values.pitch); +#endif if (osdConfig()->ahi_reverse_roll) { rollAngle = -rollAngle; diff --git a/src/main/target/common.h b/src/main/target/common.h index 0d01421649..29d5598c20 100755 --- a/src/main/target/common.h +++ b/src/main/target/common.h @@ -108,9 +108,6 @@ #define USE_D_BOOST #define USE_ANTIGRAVITY -#define USE_SECONDARY_IMU -#define USE_IMU_BNO055 - #else // FLASH_SIZE < 256 #define LOG_LEVEL_MAXIMUM LOG_LEVEL_ERROR #endif diff --git a/src/main/target/common_hardware.c b/src/main/target/common_hardware.c index 97eec0be1c..5e98cf3e4e 100755 --- a/src/main/target/common_hardware.c +++ b/src/main/target/common_hardware.c @@ -312,7 +312,7 @@ #define PCA9685_I2C_BUS BUS_I2C1 #endif BUSDEV_REGISTER_I2C(busdev_pca9685, DEVHW_PCA9685, PCA9685_I2C_BUS, 0x40, NONE, DEVFLAGS_NONE); - #endif` + #endif #endif BUSDEV_REGISTER_I2C(busdev_bno055, DEVHW_BNO055, BUS_I2C1, 0x29, NONE, DEVFLAGS_NONE); diff --git a/src/main/target/common_post.h b/src/main/target/common_post.h index ca2da612e6..2c7cfe9506 100644 --- a/src/main/target/common_post.h +++ b/src/main/target/common_post.h @@ -52,6 +52,11 @@ #undef USE_SERIALRX_JETIEXBUS #endif +#if defined(USE_I2C) && (FLASH_SIZE > 256) +#define USE_SECONDARY_IMU +#define USE_IMU_BNO055 +#endif + #if defined(SIMULATOR_BUILD) || defined(UNIT_TEST) // This feature uses 'arm_math.h', which does not exist for x86. #undef USE_DYNAMIC_FILTERS From 51f6e0aac507618668fdfd2471461ac22d4bb2c1 Mon Sep 17 00:00:00 2001 From: "Pawel Spychalski (DzikuVx)" Date: Mon, 30 Sep 2019 18:08:40 +0200 Subject: [PATCH 009/286] Fix compilation --- src/main/flight/secondary_imu.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/flight/secondary_imu.c b/src/main/flight/secondary_imu.c index e1f46d1fe0..3f6e734f95 100644 --- a/src/main/flight/secondary_imu.c +++ b/src/main/flight/secondary_imu.c @@ -21,7 +21,7 @@ * You should have received a copy of the GNU General Public License * along with this program. If not, see http://www.gnu.org/licenses/. */ - +#include "common/utils.h" #include "flight/secondary_imu.h" #include "config/parameter_group_ids.h" #include "sensors/boardalignment.h" From ac2f699bf3b5d44d282a100a7050ed9668e077fd Mon Sep 17 00:00:00 2001 From: "Pawel Spychalski (DzikuVx)" Date: Fri, 25 Oct 2019 10:33:47 +0200 Subject: [PATCH 010/286] Enable BNO055 only on selected boards --- src/main/target/MATEKF722/target.h | 4 ++++ src/main/target/common_hardware.c | 7 ++++++- src/main/target/common_post.h | 5 ----- 3 files changed, 10 insertions(+), 6 deletions(-) diff --git a/src/main/target/MATEKF722/target.h b/src/main/target/MATEKF722/target.h index 7f43c4f169..4d8d5cbb7b 100755 --- a/src/main/target/MATEKF722/target.h +++ b/src/main/target/MATEKF722/target.h @@ -162,3 +162,7 @@ #define MAX_PWM_OUTPUT_PORTS 7 #define USE_DSHOT #define USE_SERIALSHOT + +#define USE_SECONDARY_IMU +#define USE_IMU_BNO055 +#define BNO055_I2C_BUS BUS_I2C1 diff --git a/src/main/target/common_hardware.c b/src/main/target/common_hardware.c index 5e98cf3e4e..5ab3a8c161 100755 --- a/src/main/target/common_hardware.c +++ b/src/main/target/common_hardware.c @@ -315,6 +315,11 @@ #endif #endif - BUSDEV_REGISTER_I2C(busdev_bno055, DEVHW_BNO055, BUS_I2C1, 0x29, NONE, DEVFLAGS_NONE); +#ifdef USE_IMU_BNO055 +#ifndef BNO055_I2C_BUS + #define BNO055_I2C_BUS BUS_I2C1 +#endif + BUSDEV_REGISTER_I2C(busdev_bno055, DEVHW_BNO055, BNO055_I2C_BUS, 0x29, NONE, DEVFLAGS_NONE); +#endif #endif // USE_TARGET_HARDWARE_DESCRIPTORS diff --git a/src/main/target/common_post.h b/src/main/target/common_post.h index 2c7cfe9506..ca2da612e6 100644 --- a/src/main/target/common_post.h +++ b/src/main/target/common_post.h @@ -52,11 +52,6 @@ #undef USE_SERIALRX_JETIEXBUS #endif -#if defined(USE_I2C) && (FLASH_SIZE > 256) -#define USE_SECONDARY_IMU -#define USE_IMU_BNO055 -#endif - #if defined(SIMULATOR_BUILD) || defined(UNIT_TEST) // This feature uses 'arm_math.h', which does not exist for x86. #undef USE_DYNAMIC_FILTERS From 9bb7768a66a73edfb78dc168fd48f653a41c79e1 Mon Sep 17 00:00:00 2001 From: "Pawel Spychalski (DzikuVx)" Date: Tue, 10 Dec 2019 14:28:05 +0100 Subject: [PATCH 011/286] Add interface to read and set BNO055 calibration --- src/main/drivers/accgyro/accgyro_bno055.c | 99 ++++++++++++++++++----- src/main/drivers/accgyro/accgyro_bno055.h | 14 +++- src/main/flight/secondary_imu.c | 27 ++++--- src/main/flight/secondary_imu.h | 3 + 4 files changed, 113 insertions(+), 30 deletions(-) diff --git a/src/main/drivers/accgyro/accgyro_bno055.c b/src/main/drivers/accgyro/accgyro_bno055.c index 94aa3ef218..8a372c0c10 100644 --- a/src/main/drivers/accgyro/accgyro_bno055.c +++ b/src/main/drivers/accgyro/accgyro_bno055.c @@ -32,31 +32,54 @@ #ifdef USE_IMU_BNO055 -#define BNO055_ADDR_PWR_MODE 0x3E -#define BNO055_ADDR_OPR_MODE 0x3D -#define BNO055_ADDR_CALIB_STAT 0x35 +#define BNO055_ADDR_PWR_MODE 0x3E +#define BNO055_ADDR_OPR_MODE 0x3D +#define BNO055_ADDR_CALIB_STAT 0x35 -#define BNO055_PWR_MODE_NORMAL 0x00 -#define BNO055_OPR_MODE_NDOF 0x0C +#define BNO055_PWR_MODE_NORMAL 0x00 +#define BNO055_OPR_MODE_NDOF 0x0C -#define BNO055_ADDR_EUL_YAW_LSB 0x1A -#define BNO055_ADDR_EUL_YAW_MSB 0x1B -#define BNO055_ADDR_EUL_ROLL_LSB 0x1C -#define BNO055_ADDR_EUL_ROLL_MSB 0x1D -#define BNO055_ADDR_EUL_PITCH_LSB 0x1E -#define BNO055_ADDR_EUL_PITCH_MSB 0x1F +#define BNO055_ADDR_EUL_YAW_LSB 0x1A +#define BNO055_ADDR_EUL_YAW_MSB 0x1B +#define BNO055_ADDR_EUL_ROLL_LSB 0x1C +#define BNO055_ADDR_EUL_ROLL_MSB 0x1D +#define BNO055_ADDR_EUL_PITCH_LSB 0x1E +#define BNO055_ADDR_EUL_PITCH_MSB 0x1F -static busDevice_t * busDev; +#define BNO055_ADDR_GYR_OFFSET_Z_MSB 0x66 +#define BNO055_ADDR_GYR_OFFSET_Z_LSB 0x65 +#define BNO055_ADDR_GYR_OFFSET_Y_MSB 0x64 +#define BNO055_ADDR_GYR_OFFSET_Y_LSB 0x63 +#define BNO055_ADDR_GYR_OFFSET_X_MSB 0x62 +#define BNO055_ADDR_GYR_OFFSET_X_LSB 0x61 -static bool deviceDetect(busDevice_t * busDev) +#define BNO055_ADDR_MAG_OFFSET_Z_MSB 0x60 +#define BNO055_ADDR_MAG_OFFSET_Z_LSB 0x5F +#define BNO055_ADDR_MAG_OFFSET_Y_MSB 0x5E +#define BNO055_ADDR_MAG_OFFSET_Y_LSB 0x5D +#define BNO055_ADDR_MAG_OFFSET_X_MSB 0x5C +#define BNO055_ADDR_MAG_OFFSET_X_LSB 0x5B + +#define BNO055_ADDR_ACC_OFFSET_Z_MSB 0x5A +#define BNO055_ADDR_ACC_OFFSET_Z_LSB 0x59 +#define BNO055_ADDR_ACC_OFFSET_Y_MSB 0x58 +#define BNO055_ADDR_ACC_OFFSET_Y_LSB 0x57 +#define BNO055_ADDR_ACC_OFFSET_X_MSB 0x56 +#define BNO055_ADDR_ACC_OFFSET_X_LSB 0x55 + +static busDevice_t *busDev; + +static bool deviceDetect(busDevice_t *busDev) { - for (int retry = 0; retry < 5; retry++) { + for (int retry = 0; retry < 5; retry++) + { uint8_t sig; delay(150); bool ack = busRead(busDev, 0x00, &sig); - if (ack) { + if (ack) + { return true; } }; @@ -67,12 +90,14 @@ static bool deviceDetect(busDevice_t * busDev) bool bno055Init(void) { busDev = busDeviceInit(BUSTYPE_I2C, DEVHW_BNO055, 0, 0); - if (busDev == NULL) { + if (busDev == NULL) + { DEBUG_SET(DEBUG_IMU2, 2, 1); return false; } - if (!deviceDetect(busDev)) { + if (!deviceDetect(busDev)) + { DEBUG_SET(DEBUG_IMU2, 2, 2); busDeviceDeInit(busDev); return false; @@ -86,7 +111,7 @@ bool bno055Init(void) return true; } -void bno055FetchEulerAngles(int16_t * buffer) +void bno055FetchEulerAngles(int16_t *buffer) { uint8_t buf[6]; busReadBuf(busDev, BNO055_ADDR_EUL_YAW_LSB, buf, 6); @@ -125,4 +150,42 @@ bno055CalibStat_t bno055GetCalibStat(void) return stats; } +bno055CalibrationData_t bno055GetCalibrationData(void) +{ + bno055CalibrationData_t data; + uint8_t buf[18]; + + busReadBuf(busDev, BNO055_ADDR_ACC_OFFSET_X_LSB, buf, 18); + + uint8_t bufferBit = 0; + for (uint8_t sensorIndex = 0; sensorIndex < 3; sensorIndex++) + { + for (uint8_t axisIndex = 0; axisIndex < 3; axisIndex++) + { + data.raw[sensorIndex][axisIndex] = (int16_t)((buf[bufferBit + 1] << 8) | buf[bufferBit]); + bufferBit += 2; + } + } + + return data; +} + +void bno055SetCalibrationData(bno055CalibrationData_t data) +{ + uint8_t buf[18]; + + uint8_t bufferBit = 0; + for (uint8_t sensorIndex = 0; sensorIndex < 3; sensorIndex++) + { + for (uint8_t axisIndex = 0; axisIndex < 3; axisIndex++) + { + buf[bufferBit] = (uint8_t)(data.raw[sensorIndex][axisIndex] & 0xff); + buf[bufferBit + 1] = (uint8_t)((data.raw[sensorIndex][axisIndex] >> 8 ) & 0xff); + bufferBit += 2; + } + } + + busWriteBuf(busDev, BNO055_ADDR_ACC_OFFSET_X_LSB, buf, 18); +} + #endif \ No newline at end of file diff --git a/src/main/drivers/accgyro/accgyro_bno055.h b/src/main/drivers/accgyro/accgyro_bno055.h index b6f7204cab..e8c2eeb794 100644 --- a/src/main/drivers/accgyro/accgyro_bno055.h +++ b/src/main/drivers/accgyro/accgyro_bno055.h @@ -32,7 +32,19 @@ typedef struct { uint8_t mag; } bno055CalibStat_t; +typedef enum { + ACC = 0, + MAG = 1, + GYRO = 2 +} bno055Sensor_e; + +typedef struct { + int16_t raw[3][3]; +} bno055CalibrationData_t; + bool bno055Init(void); fpVector3_t bno055GetEurlerAngles(void); void bno055FetchEulerAngles(int16_t * buffer); -bno055CalibStat_t bno055GetCalibStat(void); \ No newline at end of file +bno055CalibStat_t bno055GetCalibStat(void); +bno055CalibrationData_t bno055GetCalibrationData(void); +void bno055SetCalibrationData(bno055CalibrationData_t data); \ No newline at end of file diff --git a/src/main/flight/secondary_imu.c b/src/main/flight/secondary_imu.c index 3f6e734f95..e999c48735 100644 --- a/src/main/flight/secondary_imu.c +++ b/src/main/flight/secondary_imu.c @@ -31,16 +31,23 @@ #include "drivers/sensor.h" #include "drivers/accgyro/accgyro_bno055.h" -PG_REGISTER_WITH_RESET_TEMPLATE(secondaryImuConfig_t, secondaryImuConfig, PG_SECONDARY_IMU, 0); +PG_REGISTER_WITH_RESET_FN(secondaryImuConfig_t, secondaryImuConfig, PG_SECONDARY_IMU, 1); -PG_RESET_TEMPLATE(secondaryImuConfig_t, secondaryImuConfig, - .enabled = 0, - .rollDeciDegrees = 0, - .pitchDeciDegrees = 0, - .yawDeciDegrees = 0, - .useForOsdHeading = 0, - .useForOsdAHI = 0, -); +void pgResetFn_secondaryImuConfig(secondaryImuConfig_t *instance) +{ + instance->enabled = 0; + instance->rollDeciDegrees = 0; + instance->pitchDeciDegrees = 0; + instance->yawDeciDegrees = 0; + instance->useForOsdHeading = 0; + instance->useForOsdAHI = 0; + + for (uint8_t i = 0; i < 3 ; i++) { + instance->calibrationAcc[i] = 0; + instance->calibrationMag[i] = 0; + instance->calibrationGyro[i] = 0; + } +} EXTENDED_FASTRAM secondaryImuState_t secondaryImuState; @@ -95,8 +102,6 @@ void taskSecondaryImu(timeUs_t currentTimeUs) DEBUG_SET(DEBUG_IMU2, 3, secondaryImuState.calibrationStatus.mag); DEBUG_SET(DEBUG_IMU2, 4, secondaryImuState.calibrationStatus.gyr); DEBUG_SET(DEBUG_IMU2, 5, secondaryImuState.calibrationStatus.acc); - - } } diff --git a/src/main/flight/secondary_imu.h b/src/main/flight/secondary_imu.h index d86d9b566e..8209685c45 100644 --- a/src/main/flight/secondary_imu.h +++ b/src/main/flight/secondary_imu.h @@ -36,6 +36,9 @@ typedef struct secondaryImuConfig_s { int16_t yawDeciDegrees; uint8_t useForOsdHeading; uint8_t useForOsdAHI; + int16_t calibrationGyro[3]; + int16_t calibrationMag[3]; + int16_t calibrationAcc[3]; } secondaryImuConfig_t; typedef struct secondaryImuState_s { From 4b1acdd3eec8a28d0b6bf72511c987298d83adf0 Mon Sep 17 00:00:00 2001 From: "Pawel Spychalski (DzikuVx)" Date: Tue, 10 Dec 2019 14:59:16 +0100 Subject: [PATCH 012/286] Include radius in BNO055 calibration data --- src/main/drivers/accgyro/accgyro_bno055.c | 30 +++++++++++++++++------ src/main/drivers/accgyro/accgyro_bno055.h | 3 ++- 2 files changed, 25 insertions(+), 8 deletions(-) diff --git a/src/main/drivers/accgyro/accgyro_bno055.c b/src/main/drivers/accgyro/accgyro_bno055.c index 8a372c0c10..92f9e31eb4 100644 --- a/src/main/drivers/accgyro/accgyro_bno055.c +++ b/src/main/drivers/accgyro/accgyro_bno055.c @@ -46,6 +46,11 @@ #define BNO055_ADDR_EUL_PITCH_LSB 0x1E #define BNO055_ADDR_EUL_PITCH_MSB 0x1F +#define BNO055_ADDR_MAG_RADIUS_Z_MSB 0x6A +#define BNO055_ADDR_MAG_RADIUS_Z_LSB 0x69 +#define BNO055_ADDR_ACC_RADIUS_Z_MSB 0x68 +#define BNO055_ADDR_ACC_RADIUS_Z_LSB 0x67 + #define BNO055_ADDR_GYR_OFFSET_Z_MSB 0x66 #define BNO055_ADDR_GYR_OFFSET_Z_LSB 0x65 #define BNO055_ADDR_GYR_OFFSET_Y_MSB 0x64 @@ -153,39 +158,50 @@ bno055CalibStat_t bno055GetCalibStat(void) bno055CalibrationData_t bno055GetCalibrationData(void) { bno055CalibrationData_t data; - uint8_t buf[18]; + uint8_t buf[22]; - busReadBuf(busDev, BNO055_ADDR_ACC_OFFSET_X_LSB, buf, 18); + busReadBuf(busDev, BNO055_ADDR_ACC_OFFSET_X_LSB, buf, 22); uint8_t bufferBit = 0; for (uint8_t sensorIndex = 0; sensorIndex < 3; sensorIndex++) { for (uint8_t axisIndex = 0; axisIndex < 3; axisIndex++) { - data.raw[sensorIndex][axisIndex] = (int16_t)((buf[bufferBit + 1] << 8) | buf[bufferBit]); + data.offset[sensorIndex][axisIndex] = (int16_t)((buf[bufferBit + 1] << 8) | buf[bufferBit]); bufferBit += 2; } } + data.radius[ACC] = (int16_t)((buf[19] << 8) | buf[18]); + data.radius[MAG] = (int16_t)((buf[21] << 8) | buf[20]); + return data; } void bno055SetCalibrationData(bno055CalibrationData_t data) { - uint8_t buf[18]; + uint8_t buf[22]; + //Prepare gains uint8_t bufferBit = 0; for (uint8_t sensorIndex = 0; sensorIndex < 3; sensorIndex++) { for (uint8_t axisIndex = 0; axisIndex < 3; axisIndex++) { - buf[bufferBit] = (uint8_t)(data.raw[sensorIndex][axisIndex] & 0xff); - buf[bufferBit + 1] = (uint8_t)((data.raw[sensorIndex][axisIndex] >> 8 ) & 0xff); + buf[bufferBit] = (uint8_t)(data.offset[sensorIndex][axisIndex] & 0xff); + buf[bufferBit + 1] = (uint8_t)((data.offset[sensorIndex][axisIndex] >> 8 ) & 0xff); bufferBit += 2; } } - busWriteBuf(busDev, BNO055_ADDR_ACC_OFFSET_X_LSB, buf, 18); + //Prepare radius + buf[18] = (uint8_t)(data.radius[ACC] & 0xff); + buf[19] = (uint8_t)((data.radius[ACC] >> 8 ) & 0xff); + buf[20] = (uint8_t)(data.radius[MAG] & 0xff); + buf[21] = (uint8_t)((data.radius[MAG] >> 8 ) & 0xff); + + //Write to the device + busWriteBuf(busDev, BNO055_ADDR_ACC_OFFSET_X_LSB, buf, 22); } #endif \ No newline at end of file diff --git a/src/main/drivers/accgyro/accgyro_bno055.h b/src/main/drivers/accgyro/accgyro_bno055.h index e8c2eeb794..df12fe5527 100644 --- a/src/main/drivers/accgyro/accgyro_bno055.h +++ b/src/main/drivers/accgyro/accgyro_bno055.h @@ -39,7 +39,8 @@ typedef enum { } bno055Sensor_e; typedef struct { - int16_t raw[3][3]; + int16_t radius[2]; + int16_t offset[3][3]; } bno055CalibrationData_t; bool bno055Init(void); From ceaa0593e63d98458a091ca7163b06bf07e66c08 Mon Sep 17 00:00:00 2001 From: "Pawel Spychalski (DzikuVx)" Date: Tue, 10 Dec 2019 16:12:30 +0100 Subject: [PATCH 013/286] Restore calibration profile on device initalization --- src/main/drivers/accgyro/accgyro_bno055.c | 10 ++++++--- src/main/drivers/accgyro/accgyro_bno055.h | 2 +- src/main/fc/settings.yaml | 4 ++++ src/main/flight/secondary_imu.c | 25 +++++++++++++++++++---- src/main/flight/secondary_imu.h | 8 +++++--- 5 files changed, 38 insertions(+), 11 deletions(-) diff --git a/src/main/drivers/accgyro/accgyro_bno055.c b/src/main/drivers/accgyro/accgyro_bno055.c index 92f9e31eb4..698195d008 100644 --- a/src/main/drivers/accgyro/accgyro_bno055.c +++ b/src/main/drivers/accgyro/accgyro_bno055.c @@ -36,8 +36,9 @@ #define BNO055_ADDR_OPR_MODE 0x3D #define BNO055_ADDR_CALIB_STAT 0x35 -#define BNO055_PWR_MODE_NORMAL 0x00 -#define BNO055_OPR_MODE_NDOF 0x0C +#define BNO055_PWR_MODE_NORMAL 0x00 +#define BNO055_OPR_MODE_CONFIG 0x00 +#define BNO055_OPR_MODE_NDOF 0x0C #define BNO055_ADDR_EUL_YAW_LSB 0x1A #define BNO055_ADDR_EUL_YAW_MSB 0x1B @@ -92,7 +93,7 @@ static bool deviceDetect(busDevice_t *busDev) return false; } -bool bno055Init(void) +bool bno055Init(bno055CalibrationData_t calibrationData) { busDev = busDeviceInit(BUSTYPE_I2C, DEVHW_BNO055, 0, 0); if (busDev == NULL) @@ -110,6 +111,9 @@ bool bno055Init(void) busWrite(busDev, BNO055_ADDR_PWR_MODE, BNO055_PWR_MODE_NORMAL); //Set power mode NORMAL delay(25); + busWrite(busDev, BNO055_ADDR_OPR_MODE, BNO055_OPR_MODE_CONFIG); //Set operational mode CONFIG_MODE + delay(30); + bno055SetCalibrationData(calibrationData); busWrite(busDev, BNO055_ADDR_OPR_MODE, BNO055_OPR_MODE_NDOF); //Set operational mode NDOF delay(50); diff --git a/src/main/drivers/accgyro/accgyro_bno055.h b/src/main/drivers/accgyro/accgyro_bno055.h index df12fe5527..86b18beccc 100644 --- a/src/main/drivers/accgyro/accgyro_bno055.h +++ b/src/main/drivers/accgyro/accgyro_bno055.h @@ -43,7 +43,7 @@ typedef struct { int16_t offset[3][3]; } bno055CalibrationData_t; -bool bno055Init(void); +bool bno055Init(bno055CalibrationData_t calibrationData); fpVector3_t bno055GetEurlerAngles(void); void bno055FetchEulerAngles(int16_t * buffer); bno055CalibStat_t bno055GetCalibStat(void); diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 20ab9cd40e..2ccb5bff3f 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -328,6 +328,10 @@ groups: field: yawDeciDegrees min: -1800 max: 3600 + - name: imu2_gain_acc_x + field: calibrationOffsetAcc[X] + min: INT16_MIN + max: INT16_MAX - name: PG_COMPASS_CONFIG type: compassConfig_t diff --git a/src/main/flight/secondary_imu.c b/src/main/flight/secondary_imu.c index e999c48735..a94279b36c 100644 --- a/src/main/flight/secondary_imu.c +++ b/src/main/flight/secondary_imu.c @@ -22,6 +22,7 @@ * along with this program. If not, see http://www.gnu.org/licenses/. */ #include "common/utils.h" +#include "common/axis.h" #include "flight/secondary_imu.h" #include "config/parameter_group_ids.h" #include "sensors/boardalignment.h" @@ -43,10 +44,12 @@ void pgResetFn_secondaryImuConfig(secondaryImuConfig_t *instance) instance->useForOsdAHI = 0; for (uint8_t i = 0; i < 3 ; i++) { - instance->calibrationAcc[i] = 0; - instance->calibrationMag[i] = 0; - instance->calibrationGyro[i] = 0; + instance->calibrationOffsetGyro[i] = 0; + instance->calibrationOffsetMag[i] = 0; + instance->calibrationOffsetAcc[i] = 0; } + instance->calibrationRadiusAcc = 0; + instance->calibrationRadiusMag = 0; } EXTENDED_FASTRAM secondaryImuState_t secondaryImuState; @@ -57,7 +60,21 @@ void taskSecondaryImu(timeUs_t currentTimeUs) static bool secondaryImuChecked = false; if (!secondaryImuChecked) { - secondaryImuState.active = bno055Init(); + + bno055CalibrationData_t calibrationData; + calibrationData.offset[ACC][X] = secondaryImuConfig()->calibrationOffsetAcc[X]; + calibrationData.offset[ACC][Y] = secondaryImuConfig()->calibrationOffsetAcc[Y]; + calibrationData.offset[ACC][Z] = secondaryImuConfig()->calibrationOffsetAcc[Z]; + calibrationData.offset[MAG][X] = secondaryImuConfig()->calibrationOffsetMag[X]; + calibrationData.offset[MAG][Y] = secondaryImuConfig()->calibrationOffsetMag[Y]; + calibrationData.offset[MAG][Z] = secondaryImuConfig()->calibrationOffsetMag[Z]; + calibrationData.offset[GYRO][X] = secondaryImuConfig()->calibrationOffsetGyro[X]; + calibrationData.offset[GYRO][Y] = secondaryImuConfig()->calibrationOffsetGyro[Y]; + calibrationData.offset[GYRO][Z] = secondaryImuConfig()->calibrationOffsetGyro[Z]; + calibrationData.radius[ACC] = secondaryImuConfig()->calibrationRadiusAcc; + calibrationData.radius[MAG] = secondaryImuConfig()->calibrationRadiusMag; + + secondaryImuState.active = bno055Init(calibrationData); secondaryImuChecked = true; } diff --git a/src/main/flight/secondary_imu.h b/src/main/flight/secondary_imu.h index 8209685c45..f0737d821e 100644 --- a/src/main/flight/secondary_imu.h +++ b/src/main/flight/secondary_imu.h @@ -36,9 +36,11 @@ typedef struct secondaryImuConfig_s { int16_t yawDeciDegrees; uint8_t useForOsdHeading; uint8_t useForOsdAHI; - int16_t calibrationGyro[3]; - int16_t calibrationMag[3]; - int16_t calibrationAcc[3]; + int16_t calibrationOffsetGyro[3]; + int16_t calibrationOffsetMag[3]; + int16_t calibrationOffsetAcc[3]; + int16_t calibrationRadiusAcc; + int16_t calibrationRadiusMag; } secondaryImuConfig_t; typedef struct secondaryImuState_s { From 91da35f23480caeadd3642fd214ff19147f80e11 Mon Sep 17 00:00:00 2001 From: "Pawel Spychalski (DzikuVx)" Date: Wed, 11 Dec 2019 11:51:45 +0100 Subject: [PATCH 014/286] Apply manual mag declination on secondary imu data --- src/main/flight/secondary_imu.c | 12 ++++++++++++ src/main/flight/secondary_imu.h | 1 + src/main/io/osd.c | 2 +- 3 files changed, 14 insertions(+), 1 deletion(-) diff --git a/src/main/flight/secondary_imu.c b/src/main/flight/secondary_imu.c index a94279b36c..56e59d9191 100644 --- a/src/main/flight/secondary_imu.c +++ b/src/main/flight/secondary_imu.c @@ -26,6 +26,7 @@ #include "flight/secondary_imu.h" #include "config/parameter_group_ids.h" #include "sensors/boardalignment.h" +#include "sensors/compass.h" #include "build/debug.h" @@ -56,11 +57,19 @@ EXTENDED_FASTRAM secondaryImuState_t secondaryImuState; void taskSecondaryImu(timeUs_t currentTimeUs) { + /* + * Secondary IMU works in decidegrees + */ UNUSED(currentTimeUs); static bool secondaryImuChecked = false; if (!secondaryImuChecked) { + // Create magnetic declination matrix + const int deg = compassConfig()->mag_declination / 100; + const int min = compassConfig()->mag_declination % 100; + secondaryImuState.magDeclination = (deg + min / 60.0f) * 10.0f; + bno055CalibrationData_t calibrationData; calibrationData.offset[ACC][X] = secondaryImuConfig()->calibrationOffsetAcc[X]; calibrationData.offset[ACC][Y] = secondaryImuConfig()->calibrationOffsetAcc[Y]; @@ -82,6 +91,9 @@ void taskSecondaryImu(timeUs_t currentTimeUs) { bno055FetchEulerAngles(secondaryImuState.eulerAngles.raw); + //Apply mag declination + secondaryImuState.eulerAngles.raw[2] += secondaryImuState.magDeclination; + //TODO this way of rotating a vector makes no sense, something simpler have to be developed const fpVector3_t v = { .x = secondaryImuState.eulerAngles.raw[0], diff --git a/src/main/flight/secondary_imu.h b/src/main/flight/secondary_imu.h index f0737d821e..51e917cc2a 100644 --- a/src/main/flight/secondary_imu.h +++ b/src/main/flight/secondary_imu.h @@ -47,6 +47,7 @@ typedef struct secondaryImuState_s { flightDynamicsTrims_t eulerAngles; bno055CalibStat_t calibrationStatus; uint8_t active; + float magDeclination; } secondaryImuState_t; extern secondaryImuState_t secondaryImuState; diff --git a/src/main/io/osd.c b/src/main/io/osd.c index b33a5f4b6f..877597201a 100755 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -1014,7 +1014,7 @@ int16_t osdGetHeading(void) return attitude.values.yaw; } #else - return secondaryImuState.eulerAngles.values.yaw; + return attitude.values.yaw; #endif } From 252894b9a9565cf44a1b3e6055dbbf22b293af82 Mon Sep 17 00:00:00 2001 From: "Pawel Spychalski (DzikuVx)" Date: Wed, 11 Dec 2019 14:25:00 +0100 Subject: [PATCH 015/286] Extract BNO055 initialization to separate routine --- src/main/fc/fc_init.c | 4 + src/main/fc/fc_tasks.c | 2 +- src/main/flight/secondary_imu.c | 137 ++++++++++++++++---------------- src/main/flight/secondary_imu.h | 1 + 4 files changed, 76 insertions(+), 68 deletions(-) diff --git a/src/main/fc/fc_init.c b/src/main/fc/fc_init.c index e1718b26d3..7185a7146a 100644 --- a/src/main/fc/fc_init.c +++ b/src/main/fc/fc_init.c @@ -637,6 +637,10 @@ void init(void) // Latch active features AGAIN since some may be modified by init(). latchActiveFeatures(); motorControlEnable = true; + +#ifdef USE_SECONDARY_IMU + secondaryImuInit(); +#endif fcTasksInit(); #ifdef USE_OSD diff --git a/src/main/fc/fc_tasks.c b/src/main/fc/fc_tasks.c index b5e4f923bd..6ac5200600 100755 --- a/src/main/fc/fc_tasks.c +++ b/src/main/fc/fc_tasks.c @@ -343,7 +343,7 @@ void fcTasksInit(void) setTaskEnabled(TASK_GLOBAL_FUNCTIONS, true); #endif #ifdef USE_SECONDARY_IMU - setTaskEnabled(TASK_SECONDARY_IMU, secondaryImuConfig()->enabled); + setTaskEnabled(TASK_SECONDARY_IMU, secondaryImuConfig()->enabled && secondaryImuState.active); #endif } diff --git a/src/main/flight/secondary_imu.c b/src/main/flight/secondary_imu.c index 56e59d9191..b3fab75338 100644 --- a/src/main/flight/secondary_imu.c +++ b/src/main/flight/secondary_imu.c @@ -35,6 +35,8 @@ PG_REGISTER_WITH_RESET_FN(secondaryImuConfig_t, secondaryImuConfig, PG_SECONDARY_IMU, 1); +EXTENDED_FASTRAM secondaryImuState_t secondaryImuState; + void pgResetFn_secondaryImuConfig(secondaryImuConfig_t *instance) { instance->enabled = 0; @@ -44,7 +46,8 @@ void pgResetFn_secondaryImuConfig(secondaryImuConfig_t *instance) instance->useForOsdHeading = 0; instance->useForOsdAHI = 0; - for (uint8_t i = 0; i < 3 ; i++) { + for (uint8_t i = 0; i < 3; i++) + { instance->calibrationOffsetGyro[i] = 0; instance->calibrationOffsetMag[i] = 0; instance->calibrationOffsetAcc[i] = 0; @@ -53,84 +56,84 @@ void pgResetFn_secondaryImuConfig(secondaryImuConfig_t *instance) instance->calibrationRadiusMag = 0; } -EXTENDED_FASTRAM secondaryImuState_t secondaryImuState; +void secondaryImuInit(void) +{ + // Create magnetic declination matrix + const int deg = compassConfig()->mag_declination / 100; + const int min = compassConfig()->mag_declination % 100; + secondaryImuState.magDeclination = (deg + min / 60.0f) * 10.0f; + + bno055CalibrationData_t calibrationData; + calibrationData.offset[ACC][X] = secondaryImuConfig()->calibrationOffsetAcc[X]; + calibrationData.offset[ACC][Y] = secondaryImuConfig()->calibrationOffsetAcc[Y]; + calibrationData.offset[ACC][Z] = secondaryImuConfig()->calibrationOffsetAcc[Z]; + calibrationData.offset[MAG][X] = secondaryImuConfig()->calibrationOffsetMag[X]; + calibrationData.offset[MAG][Y] = secondaryImuConfig()->calibrationOffsetMag[Y]; + calibrationData.offset[MAG][Z] = secondaryImuConfig()->calibrationOffsetMag[Z]; + calibrationData.offset[GYRO][X] = secondaryImuConfig()->calibrationOffsetGyro[X]; + calibrationData.offset[GYRO][Y] = secondaryImuConfig()->calibrationOffsetGyro[Y]; + calibrationData.offset[GYRO][Z] = secondaryImuConfig()->calibrationOffsetGyro[Z]; + calibrationData.radius[ACC] = secondaryImuConfig()->calibrationRadiusAcc; + calibrationData.radius[MAG] = secondaryImuConfig()->calibrationRadiusMag; + + secondaryImuState.active = bno055Init(calibrationData); +} void taskSecondaryImu(timeUs_t currentTimeUs) { + if (!secondaryImuState.active) + { + return; + } /* * Secondary IMU works in decidegrees */ UNUSED(currentTimeUs); - static bool secondaryImuChecked = false; - if (!secondaryImuChecked) { + bno055FetchEulerAngles(secondaryImuState.eulerAngles.raw); - // Create magnetic declination matrix - const int deg = compassConfig()->mag_declination / 100; - const int min = compassConfig()->mag_declination % 100; - secondaryImuState.magDeclination = (deg + min / 60.0f) * 10.0f; + //Apply mag declination + secondaryImuState.eulerAngles.raw[2] += secondaryImuState.magDeclination; - bno055CalibrationData_t calibrationData; - calibrationData.offset[ACC][X] = secondaryImuConfig()->calibrationOffsetAcc[X]; - calibrationData.offset[ACC][Y] = secondaryImuConfig()->calibrationOffsetAcc[Y]; - calibrationData.offset[ACC][Z] = secondaryImuConfig()->calibrationOffsetAcc[Z]; - calibrationData.offset[MAG][X] = secondaryImuConfig()->calibrationOffsetMag[X]; - calibrationData.offset[MAG][Y] = secondaryImuConfig()->calibrationOffsetMag[Y]; - calibrationData.offset[MAG][Z] = secondaryImuConfig()->calibrationOffsetMag[Z]; - calibrationData.offset[GYRO][X] = secondaryImuConfig()->calibrationOffsetGyro[X]; - calibrationData.offset[GYRO][Y] = secondaryImuConfig()->calibrationOffsetGyro[Y]; - calibrationData.offset[GYRO][Z] = secondaryImuConfig()->calibrationOffsetGyro[Z]; - calibrationData.radius[ACC] = secondaryImuConfig()->calibrationRadiusAcc; - calibrationData.radius[MAG] = secondaryImuConfig()->calibrationRadiusMag; + //TODO this way of rotating a vector makes no sense, something simpler have to be developed + const fpVector3_t v = { + .x = secondaryImuState.eulerAngles.raw[0], + .y = secondaryImuState.eulerAngles.raw[1], + .z = secondaryImuState.eulerAngles.raw[2], + }; - secondaryImuState.active = bno055Init(calibrationData); - secondaryImuChecked = true; - } + fpVector3_t rotated; - if (secondaryImuState.active) + fp_angles_t imuAngles = { + .angles.roll = DECIDEGREES_TO_RADIANS(secondaryImuConfig()->rollDeciDegrees), + .angles.pitch = DECIDEGREES_TO_RADIANS(secondaryImuConfig()->pitchDeciDegrees), + .angles.yaw = DECIDEGREES_TO_RADIANS(secondaryImuConfig()->yawDeciDegrees), + }; + fpMat3_t rotationMatrix; + rotationMatrixFromAngles(&rotationMatrix, &imuAngles); + rotationMatrixRotateVector(&rotated, &v, &rotationMatrix); + rotated.z = ((int32_t)(rotated.z + secondaryImuConfig()->yawDeciDegrees)) % 3600; + + secondaryImuState.eulerAngles.values.roll = rotated.x; + secondaryImuState.eulerAngles.values.pitch = rotated.y; + secondaryImuState.eulerAngles.values.yaw = rotated.z; + + /* + * Every 10 cycles fetch current calibration state + */ + static uint8_t tick = 0; + tick++; + if (tick == 10) { - bno055FetchEulerAngles(secondaryImuState.eulerAngles.raw); - - //Apply mag declination - secondaryImuState.eulerAngles.raw[2] += secondaryImuState.magDeclination; - - //TODO this way of rotating a vector makes no sense, something simpler have to be developed - const fpVector3_t v = { - .x = secondaryImuState.eulerAngles.raw[0], - .y = secondaryImuState.eulerAngles.raw[1], - .z = secondaryImuState.eulerAngles.raw[2], - }; - - fpVector3_t rotated; - - fp_angles_t imuAngles = { - .angles.roll = DECIDEGREES_TO_RADIANS(secondaryImuConfig()->rollDeciDegrees), - .angles.pitch = DECIDEGREES_TO_RADIANS(secondaryImuConfig()->pitchDeciDegrees), - .angles.yaw = DECIDEGREES_TO_RADIANS(secondaryImuConfig()->yawDeciDegrees), - }; - fpMat3_t rotationMatrix; - rotationMatrixFromAngles(&rotationMatrix, &imuAngles); - rotationMatrixRotateVector(&rotated, &v, &rotationMatrix); - rotated.z = ((int32_t)(rotated.z + secondaryImuConfig()->yawDeciDegrees)) % 3600; - - secondaryImuState.eulerAngles.values.roll = rotated.x; - secondaryImuState.eulerAngles.values.pitch = rotated.y; - secondaryImuState.eulerAngles.values.yaw = rotated.z; - - static uint8_t tick = 0; - tick++; - if (tick == 10) { - secondaryImuState.calibrationStatus = bno055GetCalibStat(); - tick = 0; - } - - DEBUG_SET(DEBUG_IMU2, 0, secondaryImuState.eulerAngles.values.roll); - DEBUG_SET(DEBUG_IMU2, 1, secondaryImuState.eulerAngles.values.pitch); - DEBUG_SET(DEBUG_IMU2, 2, secondaryImuState.eulerAngles.values.yaw); - - DEBUG_SET(DEBUG_IMU2, 3, secondaryImuState.calibrationStatus.mag); - DEBUG_SET(DEBUG_IMU2, 4, secondaryImuState.calibrationStatus.gyr); - DEBUG_SET(DEBUG_IMU2, 5, secondaryImuState.calibrationStatus.acc); + secondaryImuState.calibrationStatus = bno055GetCalibStat(); + tick = 0; } -} + DEBUG_SET(DEBUG_IMU2, 0, secondaryImuState.eulerAngles.values.roll); + DEBUG_SET(DEBUG_IMU2, 1, secondaryImuState.eulerAngles.values.pitch); + DEBUG_SET(DEBUG_IMU2, 2, secondaryImuState.eulerAngles.values.yaw); + + DEBUG_SET(DEBUG_IMU2, 3, secondaryImuState.calibrationStatus.mag); + DEBUG_SET(DEBUG_IMU2, 4, secondaryImuState.calibrationStatus.gyr); + DEBUG_SET(DEBUG_IMU2, 5, secondaryImuState.calibrationStatus.acc); +} diff --git a/src/main/flight/secondary_imu.h b/src/main/flight/secondary_imu.h index 51e917cc2a..f1a8f8a76c 100644 --- a/src/main/flight/secondary_imu.h +++ b/src/main/flight/secondary_imu.h @@ -54,4 +54,5 @@ extern secondaryImuState_t secondaryImuState; PG_DECLARE(secondaryImuConfig_t, secondaryImuConfig); +void secondaryImuInit(void); void taskSecondaryImu(timeUs_t currentTimeUs); \ No newline at end of file From 97262b5cbe46d8ab29020ca9283e7e2c66dc1b87 Mon Sep 17 00:00:00 2001 From: "Pawel Spychalski (DzikuVx)" Date: Wed, 11 Dec 2019 15:22:29 +0100 Subject: [PATCH 016/286] CLI interface for BNO055 calibration data --- src/main/fc/cli.c | 52 +++++++++++++++++++++++++++++++++ src/main/flight/secondary_imu.c | 19 ++++++++++++ src/main/flight/secondary_imu.h | 3 +- 3 files changed, 73 insertions(+), 1 deletion(-) diff --git a/src/main/fc/cli.c b/src/main/fc/cli.c index e5ccef4bfa..c6393d4a28 100755 --- a/src/main/fc/cli.c +++ b/src/main/fc/cli.c @@ -82,6 +82,7 @@ extern uint8_t __config_end; #include "flight/mixer.h" #include "flight/pid.h" #include "flight/servos.h" +#include "flight/secondary_imu.h" #include "io/asyncfatfs/asyncfatfs.h" #include "io/beeper.h" @@ -2683,6 +2684,54 @@ static void cliBatch(char *cmdline) } #endif +#ifdef USE_SECONDARY_IMU + +static void printImu2Status(void) +{ + cliPrintLinef("Secondary IMU active: %d", secondaryImuState.active); + cliPrintLine("Calibration status:"); + cliPrintLinef("Sys: %d", secondaryImuState.calibrationStatus.sys); + cliPrintLinef("Gyro: %d", secondaryImuState.calibrationStatus.gyr); + cliPrintLinef("Acc: %d", secondaryImuState.calibrationStatus.acc); + cliPrintLinef("Mag: %d", secondaryImuState.calibrationStatus.mag); + cliPrintLine("Calibration gains:"); + cliPrintLinef( + "Gyro: %d %d %d", + secondaryImuConfig()->calibrationOffsetGyro[X], + secondaryImuConfig()->calibrationOffsetGyro[Y], + secondaryImuConfig()->calibrationOffsetGyro[Z] + ); + cliPrintLinef( + "Acc: %d %d %d", + secondaryImuConfig()->calibrationOffsetAcc[X], + secondaryImuConfig()->calibrationOffsetAcc[Y], + secondaryImuConfig()->calibrationOffsetAcc[Z] + ); + cliPrintLinef( + "Mag: %d %d %d", + secondaryImuConfig()->calibrationOffsetMag[X], + secondaryImuConfig()->calibrationOffsetMag[Y], + secondaryImuConfig()->calibrationOffsetMag[Z] + ); + cliPrintLinef( + "Radius: %d %d", + secondaryImuConfig()->calibrationRadiusAcc, + secondaryImuConfig()->calibrationRadiusMag + ); +} + +static void cliImu2(char *cmdline) +{ + if (sl_strcasecmp(cmdline, "fetch") == 0) { + secondaryImuFetchCalibration(); + printImu2Status(); + } else { + printImu2Status(); + } +} + +#endif + static void cliSave(char *cmdline) { UNUSED(cmdline); @@ -3369,6 +3418,9 @@ const clicmd_t cmdTable[] = { CLI_COMMAND_DEF("resource", "view currently used resources", NULL, cliResource), #endif CLI_COMMAND_DEF("rxrange", "configure rx channel ranges", NULL, cliRxRange), +#ifdef USE_SECONDARY_IMU + CLI_COMMAND_DEF("imu2", "Secondary IMU", NULL, cliImu2), +#endif CLI_COMMAND_DEF("save", "save and reboot", NULL, cliSave), CLI_COMMAND_DEF("serial", "configure serial ports", NULL, cliSerial), #ifdef USE_SERIAL_PASSTHROUGH diff --git a/src/main/flight/secondary_imu.c b/src/main/flight/secondary_imu.c index b3fab75338..51b295751d 100644 --- a/src/main/flight/secondary_imu.c +++ b/src/main/flight/secondary_imu.c @@ -137,3 +137,22 @@ void taskSecondaryImu(timeUs_t currentTimeUs) DEBUG_SET(DEBUG_IMU2, 4, secondaryImuState.calibrationStatus.gyr); DEBUG_SET(DEBUG_IMU2, 5, secondaryImuState.calibrationStatus.acc); } + +void secondaryImuFetchCalibration(void) { + bno055CalibrationData_t calibrationData = bno055GetCalibrationData(); + + secondaryImuConfigMutable()->calibrationOffsetAcc[X] = calibrationData.offset[ACC][X]; + secondaryImuConfigMutable()->calibrationOffsetAcc[Y] = calibrationData.offset[ACC][Y]; + secondaryImuConfigMutable()->calibrationOffsetAcc[Z] = calibrationData.offset[ACC][Z]; + + secondaryImuConfigMutable()->calibrationOffsetMag[X] = calibrationData.offset[MAG][X]; + secondaryImuConfigMutable()->calibrationOffsetMag[Y] = calibrationData.offset[MAG][Y]; + secondaryImuConfigMutable()->calibrationOffsetMag[Z] = calibrationData.offset[MAG][Z]; + + secondaryImuConfigMutable()->calibrationOffsetGyro[X] = calibrationData.offset[GYRO][X]; + secondaryImuConfigMutable()->calibrationOffsetGyro[Y] = calibrationData.offset[GYRO][Y]; + secondaryImuConfigMutable()->calibrationOffsetGyro[Z] = calibrationData.offset[GYRO][Z]; + + secondaryImuConfigMutable()->calibrationRadiusAcc = calibrationData.radius[ACC]; + secondaryImuConfigMutable()->calibrationRadiusMag = calibrationData.radius[MAG]; +} \ No newline at end of file diff --git a/src/main/flight/secondary_imu.h b/src/main/flight/secondary_imu.h index f1a8f8a76c..39c945e59d 100644 --- a/src/main/flight/secondary_imu.h +++ b/src/main/flight/secondary_imu.h @@ -55,4 +55,5 @@ extern secondaryImuState_t secondaryImuState; PG_DECLARE(secondaryImuConfig_t, secondaryImuConfig); void secondaryImuInit(void); -void taskSecondaryImu(timeUs_t currentTimeUs); \ No newline at end of file +void taskSecondaryImu(timeUs_t currentTimeUs); +void secondaryImuFetchCalibration(void); \ No newline at end of file From efeea7a501cdc83bf1706ff09cec087b302ec752 Mon Sep 17 00:00:00 2001 From: "Pawel Spychalski (DzikuVx)" Date: Thu, 12 Dec 2019 22:15:04 +0100 Subject: [PATCH 017/286] Correctly fetch and restore calibration data --- src/main/drivers/accgyro/accgyro_bno055.c | 49 ++++++++++++++--------- src/main/drivers/accgyro/accgyro_bno055.h | 2 +- src/main/fc/cli.c | 1 + src/main/fc/settings.yaml | 28 +++++++++++++ src/main/flight/secondary_imu.c | 25 ++++++------ 5 files changed, 74 insertions(+), 31 deletions(-) diff --git a/src/main/drivers/accgyro/accgyro_bno055.c b/src/main/drivers/accgyro/accgyro_bno055.c index 698195d008..fdb73b7250 100644 --- a/src/main/drivers/accgyro/accgyro_bno055.c +++ b/src/main/drivers/accgyro/accgyro_bno055.c @@ -47,10 +47,10 @@ #define BNO055_ADDR_EUL_PITCH_LSB 0x1E #define BNO055_ADDR_EUL_PITCH_MSB 0x1F -#define BNO055_ADDR_MAG_RADIUS_Z_MSB 0x6A -#define BNO055_ADDR_MAG_RADIUS_Z_LSB 0x69 -#define BNO055_ADDR_ACC_RADIUS_Z_MSB 0x68 -#define BNO055_ADDR_ACC_RADIUS_Z_LSB 0x67 +#define BNO055_ADDR_MAG_RADIUS_MSB 0x6A +#define BNO055_ADDR_MAG_RADIUS_LSB 0x69 +#define BNO055_ADDR_ACC_RADIUS_MSB 0x68 +#define BNO055_ADDR_ACC_RADIUS_LSB 0x67 #define BNO055_ADDR_GYR_OFFSET_Z_MSB 0x66 #define BNO055_ADDR_GYR_OFFSET_Z_LSB 0x65 @@ -93,7 +93,13 @@ static bool deviceDetect(busDevice_t *busDev) return false; } -bool bno055Init(bno055CalibrationData_t calibrationData) +static void bno055SetMode(uint8_t mode) +{ + busWrite(busDev, BNO055_ADDR_OPR_MODE, mode); + delay(25); +} + +bool bno055Init(bno055CalibrationData_t calibrationData, bool setCalibration) { busDev = busDeviceInit(BUSTYPE_I2C, DEVHW_BNO055, 0, 0); if (busDev == NULL) @@ -111,11 +117,11 @@ bool bno055Init(bno055CalibrationData_t calibrationData) busWrite(busDev, BNO055_ADDR_PWR_MODE, BNO055_PWR_MODE_NORMAL); //Set power mode NORMAL delay(25); - busWrite(busDev, BNO055_ADDR_OPR_MODE, BNO055_OPR_MODE_CONFIG); //Set operational mode CONFIG_MODE - delay(30); - bno055SetCalibrationData(calibrationData); - busWrite(busDev, BNO055_ADDR_OPR_MODE, BNO055_OPR_MODE_NDOF); //Set operational mode NDOF - delay(50); + if (setCalibration) { + bno055SetMode(BNO055_OPR_MODE_CONFIG); + bno055SetCalibrationData(calibrationData); + } + bno055SetMode(BNO055_OPR_MODE_NDOF); return true; } @@ -164,8 +170,12 @@ bno055CalibrationData_t bno055GetCalibrationData(void) bno055CalibrationData_t data; uint8_t buf[22]; + bno055SetMode(BNO055_OPR_MODE_CONFIG); + busReadBuf(busDev, BNO055_ADDR_ACC_OFFSET_X_LSB, buf, 22); + bno055SetMode(BNO055_OPR_MODE_NDOF); + uint8_t bufferBit = 0; for (uint8_t sensorIndex = 0; sensorIndex < 3; sensorIndex++) { @@ -182,13 +192,14 @@ bno055CalibrationData_t bno055GetCalibrationData(void) return data; } -void bno055SetCalibrationData(bno055CalibrationData_t data) +void bno055SetCalibrationData(bno055CalibrationData_t data) { - uint8_t buf[22]; + uint8_t buf[12]; //Prepare gains + //We do not restore gyro offsets, they are quickly calibrated at startup uint8_t bufferBit = 0; - for (uint8_t sensorIndex = 0; sensorIndex < 3; sensorIndex++) + for (uint8_t sensorIndex = 0; sensorIndex < 2; sensorIndex++) { for (uint8_t axisIndex = 0; axisIndex < 3; axisIndex++) { @@ -198,14 +209,16 @@ void bno055SetCalibrationData(bno055CalibrationData_t data) } } + busWriteBuf(busDev, BNO055_ADDR_ACC_OFFSET_X_LSB, buf, 12); + //Prepare radius - buf[18] = (uint8_t)(data.radius[ACC] & 0xff); - buf[19] = (uint8_t)((data.radius[ACC] >> 8 ) & 0xff); - buf[20] = (uint8_t)(data.radius[MAG] & 0xff); - buf[21] = (uint8_t)((data.radius[MAG] >> 8 ) & 0xff); + buf[0] = (uint8_t)(data.radius[ACC] & 0xff); + buf[1] = (uint8_t)((data.radius[ACC] >> 8 ) & 0xff); + buf[2] = (uint8_t)(data.radius[MAG] & 0xff); + buf[3] = (uint8_t)((data.radius[MAG] >> 8 ) & 0xff); //Write to the device - busWriteBuf(busDev, BNO055_ADDR_ACC_OFFSET_X_LSB, buf, 22); + busWriteBuf(busDev, BNO055_ADDR_ACC_RADIUS_LSB, buf, 4); } #endif \ No newline at end of file diff --git a/src/main/drivers/accgyro/accgyro_bno055.h b/src/main/drivers/accgyro/accgyro_bno055.h index 86b18beccc..d0c24f6dc4 100644 --- a/src/main/drivers/accgyro/accgyro_bno055.h +++ b/src/main/drivers/accgyro/accgyro_bno055.h @@ -43,7 +43,7 @@ typedef struct { int16_t offset[3][3]; } bno055CalibrationData_t; -bool bno055Init(bno055CalibrationData_t calibrationData); +bool bno055Init(bno055CalibrationData_t calibrationData, bool setCalibration); fpVector3_t bno055GetEurlerAngles(void); void bno055FetchEulerAngles(int16_t * buffer); bno055CalibStat_t bno055GetCalibStat(void); diff --git a/src/main/fc/cli.c b/src/main/fc/cli.c index c6393d4a28..35857499a1 100755 --- a/src/main/fc/cli.c +++ b/src/main/fc/cli.c @@ -2695,6 +2695,7 @@ static void printImu2Status(void) cliPrintLinef("Acc: %d", secondaryImuState.calibrationStatus.acc); cliPrintLinef("Mag: %d", secondaryImuState.calibrationStatus.mag); cliPrintLine("Calibration gains:"); + cliPrintLinef( "Gyro: %d %d %d", secondaryImuConfig()->calibrationOffsetGyro[X], diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 2ccb5bff3f..59ce6987aa 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -332,6 +332,34 @@ groups: field: calibrationOffsetAcc[X] min: INT16_MIN max: INT16_MAX + - name: imu2_gain_acc_y + field: calibrationOffsetAcc[Y] + min: INT16_MIN + max: INT16_MAX + - name: imu2_gain_acc_z + field: calibrationOffsetAcc[Z] + min: INT16_MIN + max: INT16_MAX + - name: imu2_gain_mag_x + field: calibrationOffsetMag[X] + min: INT16_MIN + max: INT16_MAX + - name: imu2_gain_mag_y + field: calibrationOffsetMag[Y] + min: INT16_MIN + max: INT16_MAX + - name: imu2_gain_mag_z + field: calibrationOffsetMag[Z] + min: INT16_MIN + max: INT16_MAX + - name: imu2_radius_acc + field: calibrationRadiusAcc + min: INT16_MIN + max: INT16_MAX + - name: imu2_radius_mag + field: calibrationRadiusMag + min: INT16_MIN + max: INT16_MAX - name: PG_COMPASS_CONFIG type: compassConfig_t diff --git a/src/main/flight/secondary_imu.c b/src/main/flight/secondary_imu.c index 51b295751d..434b5248f6 100644 --- a/src/main/flight/secondary_imu.c +++ b/src/main/flight/secondary_imu.c @@ -21,6 +21,7 @@ * You should have received a copy of the GNU General Public License * along with this program. If not, see http://www.gnu.org/licenses/. */ +#include "stdint.h" #include "common/utils.h" #include "common/axis.h" #include "flight/secondary_imu.h" @@ -64,19 +65,19 @@ void secondaryImuInit(void) secondaryImuState.magDeclination = (deg + min / 60.0f) * 10.0f; bno055CalibrationData_t calibrationData; - calibrationData.offset[ACC][X] = secondaryImuConfig()->calibrationOffsetAcc[X]; - calibrationData.offset[ACC][Y] = secondaryImuConfig()->calibrationOffsetAcc[Y]; - calibrationData.offset[ACC][Z] = secondaryImuConfig()->calibrationOffsetAcc[Z]; - calibrationData.offset[MAG][X] = secondaryImuConfig()->calibrationOffsetMag[X]; - calibrationData.offset[MAG][Y] = secondaryImuConfig()->calibrationOffsetMag[Y]; - calibrationData.offset[MAG][Z] = secondaryImuConfig()->calibrationOffsetMag[Z]; - calibrationData.offset[GYRO][X] = secondaryImuConfig()->calibrationOffsetGyro[X]; - calibrationData.offset[GYRO][Y] = secondaryImuConfig()->calibrationOffsetGyro[Y]; - calibrationData.offset[GYRO][Z] = secondaryImuConfig()->calibrationOffsetGyro[Z]; - calibrationData.radius[ACC] = secondaryImuConfig()->calibrationRadiusAcc; - calibrationData.radius[MAG] = secondaryImuConfig()->calibrationRadiusMag; + calibrationData.offset[ACC][X] = secondaryImuConfig()->calibrationOffsetAcc[X]; + calibrationData.offset[ACC][Y] = secondaryImuConfig()->calibrationOffsetAcc[Y]; + calibrationData.offset[ACC][Z] = secondaryImuConfig()->calibrationOffsetAcc[Z]; + calibrationData.offset[MAG][X] = secondaryImuConfig()->calibrationOffsetMag[X]; + calibrationData.offset[MAG][Y] = secondaryImuConfig()->calibrationOffsetMag[Y]; + calibrationData.offset[MAG][Z] = secondaryImuConfig()->calibrationOffsetMag[Z]; + calibrationData.offset[GYRO][X] = secondaryImuConfig()->calibrationOffsetGyro[X]; + calibrationData.offset[GYRO][Y] = secondaryImuConfig()->calibrationOffsetGyro[Y]; + calibrationData.offset[GYRO][Z] = secondaryImuConfig()->calibrationOffsetGyro[Z]; + calibrationData.radius[ACC] = secondaryImuConfig()->calibrationRadiusAcc; + calibrationData.radius[MAG] = secondaryImuConfig()->calibrationRadiusMag; - secondaryImuState.active = bno055Init(calibrationData); + secondaryImuState.active = bno055Init(calibrationData, (secondaryImuConfig()->calibrationRadiusAcc && secondaryImuConfig()->calibrationRadiusMag)); } void taskSecondaryImu(timeUs_t currentTimeUs) From 22f9c481e8d0fc5d675489177566deb47f771506 Mon Sep 17 00:00:00 2001 From: "Pawel Spychalski (DzikuVx)" Date: Sat, 4 Jan 2020 16:09:43 +0100 Subject: [PATCH 018/286] Enable BNO055 on Kakute F7 --- src/main/target/KAKUTEF7/target.h | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/src/main/target/KAKUTEF7/target.h b/src/main/target/KAKUTEF7/target.h index 01984ed350..6a8dbb95c2 100644 --- a/src/main/target/KAKUTEF7/target.h +++ b/src/main/target/KAKUTEF7/target.h @@ -181,3 +181,7 @@ #define TARGET_IO_PORTE 0xffff #define MAX_PWM_OUTPUT_PORTS 6 + +#define USE_SECONDARY_IMU +#define USE_IMU_BNO055 +#define BNO055_I2C_BUS BUS_I2C1 \ No newline at end of file From 487745c34bd42a9c3dee64090c997ca7ee408f2b Mon Sep 17 00:00:00 2001 From: "Pawel Spychalski (DzikuVx)" Date: Sat, 4 Jan 2020 16:44:55 +0100 Subject: [PATCH 019/286] Use automatic mag declination for secondary IMU --- src/main/flight/secondary_imu.c | 8 +++++++- src/main/flight/secondary_imu.h | 3 ++- src/main/navigation/navigation_pos_estimator.c | 7 ++++++- 3 files changed, 15 insertions(+), 3 deletions(-) diff --git a/src/main/flight/secondary_imu.c b/src/main/flight/secondary_imu.c index 434b5248f6..3879227aba 100644 --- a/src/main/flight/secondary_imu.c +++ b/src/main/flight/secondary_imu.c @@ -62,7 +62,8 @@ void secondaryImuInit(void) // Create magnetic declination matrix const int deg = compassConfig()->mag_declination / 100; const int min = compassConfig()->mag_declination % 100; - secondaryImuState.magDeclination = (deg + min / 60.0f) * 10.0f; + + secondaryImuSetMagneticDeclination(deg + min / 60.0f); bno055CalibrationData_t calibrationData; calibrationData.offset[ACC][X] = secondaryImuConfig()->calibrationOffsetAcc[X]; @@ -137,6 +138,7 @@ void taskSecondaryImu(timeUs_t currentTimeUs) DEBUG_SET(DEBUG_IMU2, 3, secondaryImuState.calibrationStatus.mag); DEBUG_SET(DEBUG_IMU2, 4, secondaryImuState.calibrationStatus.gyr); DEBUG_SET(DEBUG_IMU2, 5, secondaryImuState.calibrationStatus.acc); + DEBUG_SET(DEBUG_IMU2, 6, secondaryImuState.magDeclination); } void secondaryImuFetchCalibration(void) { @@ -156,4 +158,8 @@ void secondaryImuFetchCalibration(void) { secondaryImuConfigMutable()->calibrationRadiusAcc = calibrationData.radius[ACC]; secondaryImuConfigMutable()->calibrationRadiusMag = calibrationData.radius[MAG]; +} + +void secondaryImuSetMagneticDeclination(float declination) { //Incoming units are degrees + secondaryImuState.magDeclination = declination * 10.0f; //Internally declination is stored in decidegrees } \ No newline at end of file diff --git a/src/main/flight/secondary_imu.h b/src/main/flight/secondary_imu.h index 39c945e59d..749b3b575f 100644 --- a/src/main/flight/secondary_imu.h +++ b/src/main/flight/secondary_imu.h @@ -56,4 +56,5 @@ PG_DECLARE(secondaryImuConfig_t, secondaryImuConfig); void secondaryImuInit(void); void taskSecondaryImu(timeUs_t currentTimeUs); -void secondaryImuFetchCalibration(void); \ No newline at end of file +void secondaryImuFetchCalibration(void); +void secondaryImuSetMagneticDeclination(float declination); \ No newline at end of file diff --git a/src/main/navigation/navigation_pos_estimator.c b/src/main/navigation/navigation_pos_estimator.c index d190c0c041..4f3db5ccd2 100755 --- a/src/main/navigation/navigation_pos_estimator.c +++ b/src/main/navigation/navigation_pos_estimator.c @@ -39,6 +39,7 @@ #include "fc/config.h" #include "flight/imu.h" +#include "flight/secondary_imu.h" #include "io/gps.h" @@ -213,7 +214,11 @@ void onNewGPSData(void) if(STATE(GPS_FIX_HOME)){ static bool magDeclinationSet = false; if (positionEstimationConfig()->automatic_mag_declination && !magDeclinationSet) { - imuSetMagneticDeclination(geoCalculateMagDeclination(&newLLH)); + const float declination = geoCalculateMagDeclination(&newLLH); + imuSetMagneticDeclination(declination); +#ifdef USE_SECONDARY_IMU + secondaryImuSetMagneticDeclination(declination); +#endif magDeclinationSet = true; } } From e5b025378273502a202a9815951c4b5d906d91f6 Mon Sep 17 00:00:00 2001 From: "Pawel Spychalski (DzikuVx)" Date: Sat, 4 Jan 2020 17:36:32 +0100 Subject: [PATCH 020/286] Allow BNO055 data to be used in ANGLE mode --- src/main/fc/settings.yaml | 3 +++ src/main/flight/pid.c | 16 ++++++++++++++++ src/main/flight/secondary_imu.c | 1 + src/main/flight/secondary_imu.h | 1 + 4 files changed, 21 insertions(+) diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 3967ac13b5..a68d89a118 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -319,6 +319,9 @@ groups: - name: imu2_use_for_osd_ahi field: useForOsdAHI type: bool + - name: imu2_use_for_stabilized + field: useForStabilized + type: bool - name: imu2_align_roll field: rollDeciDegrees min: -1800 diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index ce7e657982..e5a7eea21a 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -42,6 +42,7 @@ #include "flight/imu.h" #include "flight/mixer.h" #include "flight/rpm_filter.h" +#include "flight/secondary_imu.h" #include "io/gps.h" @@ -533,7 +534,22 @@ static void pidLevel(pidState_t *pidState, flight_dynamics_index_t axis, float h if ((axis == FD_PITCH) && STATE(FIXED_WING) && FLIGHT_MODE(ANGLE_MODE) && !navigationIsControllingThrottle()) angleTarget += scaleRange(MAX(0, navConfig()->fw.cruise_throttle - rcCommand[THROTTLE]), 0, navConfig()->fw.cruise_throttle - PWM_RANGE_MIN, 0, mixerConfig()->fwMinThrottleDownPitchAngle); +#ifdef USE_SECONDARY_IMU + float actual; + if (secondaryImuConfig()->useForStabilized) { + if (axis == FD_ROLL) { + actual = secondaryImuState.eulerAngles.values.roll; + } else { + actual = secondaryImuState.eulerAngles.values.pitch; + } + } else { + actual = attitude.raw[axis]; + } + + const float angleErrorDeg = DECIDEGREES_TO_DEGREES(angleTarget - actual); +#else const float angleErrorDeg = DECIDEGREES_TO_DEGREES(angleTarget - attitude.raw[axis]); +#endif float angleRateTarget = constrainf(angleErrorDeg * (pidBank()->pid[PID_LEVEL].P / FP_PID_LEVEL_P_MULTIPLIER), -currentControlRateProfile->stabilized.rates[axis] * 10.0f, currentControlRateProfile->stabilized.rates[axis] * 10.0f); diff --git a/src/main/flight/secondary_imu.c b/src/main/flight/secondary_imu.c index 3879227aba..e9c8fdb0e8 100644 --- a/src/main/flight/secondary_imu.c +++ b/src/main/flight/secondary_imu.c @@ -46,6 +46,7 @@ void pgResetFn_secondaryImuConfig(secondaryImuConfig_t *instance) instance->yawDeciDegrees = 0; instance->useForOsdHeading = 0; instance->useForOsdAHI = 0; + instance->useForStabilized = 0; for (uint8_t i = 0; i < 3; i++) { diff --git a/src/main/flight/secondary_imu.h b/src/main/flight/secondary_imu.h index 749b3b575f..b3d8f1da29 100644 --- a/src/main/flight/secondary_imu.h +++ b/src/main/flight/secondary_imu.h @@ -36,6 +36,7 @@ typedef struct secondaryImuConfig_s { int16_t yawDeciDegrees; uint8_t useForOsdHeading; uint8_t useForOsdAHI; + uint8_t useForStabilized; int16_t calibrationOffsetGyro[3]; int16_t calibrationOffsetMag[3]; int16_t calibrationOffsetAcc[3]; From 73ca0184ce779678eba589fdcbce3df00fa90bd9 Mon Sep 17 00:00:00 2001 From: breadoven <56191411+breadoven@users.noreply.github.com> Date: Fri, 2 Oct 2020 12:33:26 +0100 Subject: [PATCH 021/286] Fix Naming --- src/main/navigation/navigation.c | 16 ++++++++-------- src/main/navigation/navigation_private.h | 2 +- 2 files changed, 9 insertions(+), 9 deletions(-) diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c index 602de21729..7a88409071 100755 --- a/src/main/navigation/navigation.c +++ b/src/main/navigation/navigation.c @@ -222,7 +222,7 @@ static navigationFSMEvent_t nextForNonGeoStates(void); void initializeRTHSanityChecker(const fpVector3_t * pos); bool validateRTHSanityChecker(void); -static void OverRideRTHAtitudePreset(void); +static void overrideRTHAtitudePreset(void); /*************************************************************************************************/ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_IDLE(navigationFSMState_t previousState); @@ -1086,7 +1086,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_INITIALIZE(navigati { navigationFSMStateFlags_t prevFlags = navGetStateFlags(previousState); - posControl.flags.CanOverRideRTHAlt = false; //prevent unwanted override if AltHold selected at RTH initialize + posControl.flags.canOverrideRTHAlt = false; //prevent unwanted override if AltHold selected at RTH initialize if ((posControl.flags.estHeadingStatus == EST_NONE) || (posControl.flags.estAltStatus == EST_NONE) || (posControl.flags.estPosStatus != EST_TRUSTED) || !STATE(GPS_FIX_HOME)) { // Heading sensor, altitude sensor and HOME fix are mandatory for RTH. If not satisfied - switch to emergency landing @@ -1150,20 +1150,20 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_INITIALIZE(navigati } } -static void OverRideRTHAtitudePreset(void) +static void overrideRTHAtitudePreset(void) { if (!navConfig()->general.flags.rth_alt_control_override) { return; } if (!IS_RC_MODE_ACTIVE(BOXNAVALTHOLD)) { - posControl.flags.CanOverRideRTHAlt = true; + posControl.flags.canOverrideRTHAlt = true; } else { - if (posControl.flags.CanOverRideRTHAlt && !posControl.flags.forcedRTHActivated) { + if (posControl.flags.canOverrideRTHAlt && !posControl.flags.forcedRTHActivated) { posControl.rthState.rthInitialAltitude = posControl.actualState.abs.pos.z; posControl.rthState.rthFinalAltitude = posControl.rthState.rthInitialAltitude; - posControl.flags.CanOverRideRTHAlt = false; + posControl.flags.canOverrideRTHAlt = false; } } } @@ -1172,7 +1172,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_CLIMB_TO_SAFE_ALT(n { UNUSED(previousState); - OverRideRTHAtitudePreset(); + overrideRTHAtitudePreset(); if ((posControl.flags.estHeadingStatus == EST_NONE)) { return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING; @@ -1248,7 +1248,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_HEAD_HOME(navigatio { UNUSED(previousState); - OverRideRTHAtitudePreset(); + overrideRTHAtitudePreset(); if ((posControl.flags.estHeadingStatus == EST_NONE)) { return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING; diff --git a/src/main/navigation/navigation_private.h b/src/main/navigation/navigation_private.h index 8457c46dbb..53583caca1 100755 --- a/src/main/navigation/navigation_private.h +++ b/src/main/navigation/navigation_private.h @@ -87,7 +87,7 @@ typedef struct navigationFlags_s { bool isTerrainFollowEnabled; // Does iNav use rangefinder for terrain following (adjusting baro altitude target according to rangefinders readings) bool forcedRTHActivated; - bool CanOverRideRTHAlt; //RTH climb altitude override possible + bool canOverrideRTHAlt; //RTH climb altitude override is possible } navigationFlags_t; typedef enum { From 181a1ce9e5fde17f73069f4f957a6b7721fc74f9 Mon Sep 17 00:00:00 2001 From: breadoven <56191411+breadoven@users.noreply.github.com> Date: Sun, 15 Nov 2020 21:49:12 +0000 Subject: [PATCH 022/286] Fix Merge Correction Errors --- src/main/navigation/navigation.c | 1 - src/main/navigation/navigation.h | 3 +-- 2 files changed, 1 insertion(+), 3 deletions(-) diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c index 8991966ebf..f42c04fdda 100755 --- a/src/main/navigation/navigation.c +++ b/src/main/navigation/navigation.c @@ -107,7 +107,6 @@ PG_RESET_TEMPLATE(navConfig_t, navConfig, .disarm_on_landing = 0, .rth_allow_landing = NAV_RTH_ALLOW_LANDING_ALWAYS, .rth_alt_control_override = 0, //set using nav_rth_alt_control_override - .auto_overrides_motor_stop = 1, .nav_overrides_motor_stop = NOMS_ALL_NAV, }, diff --git a/src/main/navigation/navigation.h b/src/main/navigation/navigation.h index 4efea10dc7..9457b902ff 100755 --- a/src/main/navigation/navigation.h +++ b/src/main/navigation/navigation.h @@ -182,8 +182,7 @@ typedef struct navConfig_s { uint8_t rth_allow_landing; // Enable landing as last stage of RTH. Use constants in navRTHAllowLanding_e. uint8_t rth_climb_ignore_emerg; // Option to ignore GPS loss on initial climb stage of RTH uint8_t rth_alt_control_override; // Set RTH preset climb altitude to Current altutude during climb using AltHold mode switch - uint8_t auto_overrides_motor_stop; // Autonomous modes override motor_stop setting and user command to stop motor - uint8_t nav_overrides_motor_stop; // Autonomous modes override motor_stop setting and user command to stop motor + uint8_t nav_overrides_motor_stop; // Autonomous modes override motor_stop setting and user command to stop motor } flags; uint8_t pos_failure_timeout; // Time to wait before switching to emergency landing (0 - disable) From 8f2c2620efd46922614f762f2d19a972195aee7a Mon Sep 17 00:00:00 2001 From: breadoven <56191411+breadoven@users.noreply.github.com> Date: Tue, 17 Nov 2020 21:15:37 +0000 Subject: [PATCH 023/286] Change Control to Pitch Stick Override trigger changed from AlfHold mode to pitch stick input. --- src/main/fc/settings.yaml | 2 +- src/main/navigation/navigation.c | 47 ++++++++++++++---------- src/main/navigation/navigation.h | 2 +- src/main/navigation/navigation_private.h | 1 - 4 files changed, 29 insertions(+), 23 deletions(-) diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 38994493e5..60051e6a78 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -2137,7 +2137,7 @@ groups: field: general.flags.rth_alt_control_mode table: nav_rth_alt_mode - name: nav_rth_alt_control_override - description: "If set to ON RTH climb will stop and maintain altitude acheived when AltHold mode is switched ON, overriding preset climb altitude" + description: "If set to ON the preset RTH altitude can be overridden during the climb phase, resetting to the current altitude, by holding the pitch stick in the full pitch down position for longer than 1 second" default_value: "OFF" field: general.flags.rth_alt_control_override type: bool diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c index f42c04fdda..cba13e7a41 100755 --- a/src/main/navigation/navigation.c +++ b/src/main/navigation/navigation.c @@ -1092,8 +1092,6 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_CRUISE_3D_ADJUSTING(nav static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_INITIALIZE(navigationFSMState_t previousState) { navigationFSMStateFlags_t prevFlags = navGetStateFlags(previousState); - - posControl.flags.canOverrideRTHAlt = false; //prevent unwanted override if AltHold selected at RTH initialize if ((posControl.flags.estHeadingStatus == EST_NONE) || (posControl.flags.estAltStatus == EST_NONE) || (posControl.flags.estPosStatus != EST_TRUSTED) || !STATE(GPS_FIX_HOME)) { // Heading sensor, altitude sensor and HOME fix are mandatory for RTH. If not satisfied - switch to emergency landing @@ -1157,24 +1155,6 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_INITIALIZE(navigati } } -static void overrideRTHAtitudePreset(void) -{ - if (!navConfig()->general.flags.rth_alt_control_override) { - return; - } - - if (!IS_RC_MODE_ACTIVE(BOXNAVALTHOLD)) { - posControl.flags.canOverrideRTHAlt = true; - } - else { - if (posControl.flags.canOverrideRTHAlt && !posControl.flags.forcedRTHActivated) { - posControl.rthState.rthInitialAltitude = posControl.actualState.abs.pos.z; - posControl.rthState.rthFinalAltitude = posControl.rthState.rthInitialAltitude; - posControl.flags.canOverrideRTHAlt = false; - } - } -} - static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_CLIMB_TO_SAFE_ALT(navigationFSMState_t previousState) { UNUSED(previousState); @@ -2531,6 +2511,33 @@ void updateHomePosition(void) } } +/* ----------------------------------------------------------- + * Override preset RTH altitude to current altitude + *-----------------------------------------------------------*/ +static void overrideRTHAtitudePreset(void) +{ + if (!navConfig()->general.flags.rth_alt_control_override) { + return; + } + + static timeMs_t PitchStickHoldStartTime; + + if (rxGetChannelValue(PITCH) > 1900) { + if (!PitchStickHoldStartTime) { + PitchStickHoldStartTime = millis(); + } else { + timeMs_t currentTime = millis(); + if (currentTime - PitchStickHoldStartTime > 1000 && !posControl.flags.forcedRTHActivated) { + posControl.rthState.rthInitialAltitude = posControl.actualState.abs.pos.z; + posControl.rthState.rthFinalAltitude = posControl.rthState.rthInitialAltitude; + } + } + } else { + PitchStickHoldStartTime = 0; + } + DEBUG_SET(DEBUG_CRUISE, 1, PitchStickHoldStartTime); +} + /*----------------------------------------------------------- * Update flight statistics *-----------------------------------------------------------*/ diff --git a/src/main/navigation/navigation.h b/src/main/navigation/navigation.h index 9457b902ff..7b9c30606e 100755 --- a/src/main/navigation/navigation.h +++ b/src/main/navigation/navigation.h @@ -181,7 +181,7 @@ typedef struct navConfig_s { uint8_t disarm_on_landing; // uint8_t rth_allow_landing; // Enable landing as last stage of RTH. Use constants in navRTHAllowLanding_e. uint8_t rth_climb_ignore_emerg; // Option to ignore GPS loss on initial climb stage of RTH - uint8_t rth_alt_control_override; // Set RTH preset climb altitude to Current altutude during climb using AltHold mode switch + uint8_t rth_alt_control_override; // Set RTH preset climb altitude to Current altutude during climb using full down pitch stick uint8_t nav_overrides_motor_stop; // Autonomous modes override motor_stop setting and user command to stop motor } flags; diff --git a/src/main/navigation/navigation_private.h b/src/main/navigation/navigation_private.h index 53583caca1..fcae1b7ded 100755 --- a/src/main/navigation/navigation_private.h +++ b/src/main/navigation/navigation_private.h @@ -87,7 +87,6 @@ typedef struct navigationFlags_s { bool isTerrainFollowEnabled; // Does iNav use rangefinder for terrain following (adjusting baro altitude target according to rangefinders readings) bool forcedRTHActivated; - bool canOverrideRTHAlt; //RTH climb altitude override is possible } navigationFlags_t; typedef enum { From 011533be5266d6b811d7a8cebc8a576ec371f0f6 Mon Sep 17 00:00:00 2001 From: breadoven <56191411+breadoven@users.noreply.github.com> Date: Tue, 17 Nov 2020 22:10:27 +0000 Subject: [PATCH 024/286] Update navigation.c Fix tabs --- src/main/navigation/navigation.c | 34 ++++++++++++++++---------------- 1 file changed, 17 insertions(+), 17 deletions(-) diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c index cba13e7a41..b27dc60a89 100755 --- a/src/main/navigation/navigation.c +++ b/src/main/navigation/navigation.c @@ -106,7 +106,7 @@ PG_RESET_TEMPLATE(navConfig_t, navConfig, .rth_tail_first = 0, .disarm_on_landing = 0, .rth_allow_landing = NAV_RTH_ALLOW_LANDING_ALWAYS, - .rth_alt_control_override = 0, //set using nav_rth_alt_control_override + .rth_alt_control_override = 0, //Override the preset RTH altitude to the current altitude .nav_overrides_motor_stop = NOMS_ALL_NAV, }, @@ -2520,22 +2520,22 @@ static void overrideRTHAtitudePreset(void) return; } - static timeMs_t PitchStickHoldStartTime; - - if (rxGetChannelValue(PITCH) > 1900) { - if (!PitchStickHoldStartTime) { - PitchStickHoldStartTime = millis(); - } else { - timeMs_t currentTime = millis(); - if (currentTime - PitchStickHoldStartTime > 1000 && !posControl.flags.forcedRTHActivated) { - posControl.rthState.rthInitialAltitude = posControl.actualState.abs.pos.z; - posControl.rthState.rthFinalAltitude = posControl.rthState.rthInitialAltitude; - } - } - } else { - PitchStickHoldStartTime = 0; - } - DEBUG_SET(DEBUG_CRUISE, 1, PitchStickHoldStartTime); + static timeMs_t PitchStickHoldStartTime; + + if (rxGetChannelValue(PITCH) > 1900) { + if (!PitchStickHoldStartTime) { + PitchStickHoldStartTime = millis(); + } else { + timeMs_t currentTime = millis(); + if (currentTime - PitchStickHoldStartTime > 1000 && !posControl.flags.forcedRTHActivated) { + posControl.rthState.rthInitialAltitude = posControl.actualState.abs.pos.z; + posControl.rthState.rthFinalAltitude = posControl.rthState.rthInitialAltitude; + } + } + } else { + PitchStickHoldStartTime = 0; + } + DEBUG_SET(DEBUG_CRUISE, 1, PitchStickHoldStartTime); } /*----------------------------------------------------------- From 7dbc27c81af7216378c84ad8f6031af7d9d0d858 Mon Sep 17 00:00:00 2001 From: breadoven <56191411+breadoven@users.noreply.github.com> Date: Wed, 25 Nov 2020 18:53:16 +0000 Subject: [PATCH 025/286] Update to add RTH Climb First override RTH Climb First override added using roll stick. RTH altitude override - pitch down stick RTH Climb First override - roll right stick Stick hold for > 2 seconds. --- src/main/navigation/navigation.c | 40 ++++++++++++++---------- src/main/navigation/navigation_private.h | 1 + 2 files changed, 24 insertions(+), 17 deletions(-) diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c index b27dc60a89..067f429e91 100755 --- a/src/main/navigation/navigation.c +++ b/src/main/navigation/navigation.c @@ -106,7 +106,7 @@ PG_RESET_TEMPLATE(navConfig_t, navConfig, .rth_tail_first = 0, .disarm_on_landing = 0, .rth_allow_landing = NAV_RTH_ALLOW_LANDING_ALWAYS, - .rth_alt_control_override = 0, //Override the preset RTH altitude to the current altitude + .rth_alt_control_override = 0, // Override RTH Altitude and rth_climb_first settings using Pitch and Roll stick .nav_overrides_motor_stop = NOMS_ALL_NAV, }, @@ -229,7 +229,7 @@ static navigationFSMEvent_t nextForNonGeoStates(void); void initializeRTHSanityChecker(const fpVector3_t * pos); bool validateRTHSanityChecker(void); -static void overrideRTHAtitudePreset(void); +static void rthAltControlStickOverrideCheck(unsigned axis); /*************************************************************************************************/ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_IDLE(navigationFSMState_t previousState); @@ -1092,6 +1092,9 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_CRUISE_3D_ADJUSTING(nav static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_INITIALIZE(navigationFSMState_t previousState) { navigationFSMStateFlags_t prevFlags = navGetStateFlags(previousState); + + // reset flag to override RTH climb first + posControl.flags.rthClimbFirstOverride = false; if ((posControl.flags.estHeadingStatus == EST_NONE) || (posControl.flags.estAltStatus == EST_NONE) || (posControl.flags.estPosStatus != EST_TRUSTED) || !STATE(GPS_FIX_HOME)) { // Heading sensor, altitude sensor and HOME fix are mandatory for RTH. If not satisfied - switch to emergency landing @@ -1159,7 +1162,8 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_CLIMB_TO_SAFE_ALT(n { UNUSED(previousState); - overrideRTHAtitudePreset(); + rthAltControlStickOverrideCheck(PITCH); + rthAltControlStickOverrideCheck(ROLL); if ((posControl.flags.estHeadingStatus == EST_NONE)) { return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING; @@ -1176,7 +1180,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_CLIMB_TO_SAFE_ALT(n const float rthAltitudeMargin = MAX(FW_RTH_CLIMB_MARGIN_MIN_CM, (rthClimbMarginPercent/100.0) * fabsf(posControl.rthState.rthInitialAltitude - posControl.rthState.homePosition.pos.z)); // If we reached desired initial RTH altitude or we don't want to climb first - if (((navGetCurrentActualPositionAndVelocity()->pos.z - posControl.rthState.rthInitialAltitude) > -rthAltitudeMargin) || (!navConfig()->general.flags.rth_climb_first)) { + if (((navGetCurrentActualPositionAndVelocity()->pos.z - posControl.rthState.rthInitialAltitude) > -rthAltitudeMargin) || (!navConfig()->general.flags.rth_climb_first) || posControl.flags.rthClimbFirstOverride) { // Delayed initialization for RTH sanity check on airplanes - allow to finish climb first as it can take some distance if (STATE(FIXED_WING_LEGACY)) { @@ -1235,7 +1239,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_HEAD_HOME(navigatio { UNUSED(previousState); - overrideRTHAtitudePreset(); + rthAltControlStickOverrideCheck(PITCH); if ((posControl.flags.estHeadingStatus == EST_NONE)) { return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING; @@ -2512,30 +2516,32 @@ void updateHomePosition(void) } /* ----------------------------------------------------------- - * Override preset RTH altitude to current altitude + * Override RTH preset altitude and Climb First option + * Set using Pitch/Roll stick held for > 2 seconds *-----------------------------------------------------------*/ -static void overrideRTHAtitudePreset(void) +static void rthAltControlStickOverrideCheck(unsigned axis) { - if (!navConfig()->general.flags.rth_alt_control_override) { + if (!navConfig()->general.flags.rth_alt_control_override || posControl.flags.forcedRTHActivated) { return; } - static timeMs_t PitchStickHoldStartTime; + static timeMs_t rthOverrideStickHoldStartTime[2]; + timeMs_t currentTime = millis(); - if (rxGetChannelValue(PITCH) > 1900) { - if (!PitchStickHoldStartTime) { - PitchStickHoldStartTime = millis(); - } else { - timeMs_t currentTime = millis(); - if (currentTime - PitchStickHoldStartTime > 1000 && !posControl.flags.forcedRTHActivated) { + if (rxGetChannelValue(axis) > rxConfig()->maxcheck) { + if (!rthOverrideStickHoldStartTime[axis]) { + rthOverrideStickHoldStartTime[axis] = millis(); + } else if (ABS(2500 - (currentTime - rthOverrideStickHoldStartTime[axis])) < 500) { + if (axis == PITCH) { // pitch down to override preset altitude reset to current altitude posControl.rthState.rthInitialAltitude = posControl.actualState.abs.pos.z; posControl.rthState.rthFinalAltitude = posControl.rthState.rthInitialAltitude; + } else { // roll right to override climb first + posControl.flags.rthClimbFirstOverride = true; } } } else { - PitchStickHoldStartTime = 0; + rthOverrideStickHoldStartTime[axis] = 0; } - DEBUG_SET(DEBUG_CRUISE, 1, PitchStickHoldStartTime); } /*----------------------------------------------------------- diff --git a/src/main/navigation/navigation_private.h b/src/main/navigation/navigation_private.h index fcae1b7ded..5d3818d055 100755 --- a/src/main/navigation/navigation_private.h +++ b/src/main/navigation/navigation_private.h @@ -87,6 +87,7 @@ typedef struct navigationFlags_s { bool isTerrainFollowEnabled; // Does iNav use rangefinder for terrain following (adjusting baro altitude target according to rangefinders readings) bool forcedRTHActivated; + bool rthClimbFirstOverride; // RTH Climb First override using Roll stick } navigationFlags_t; typedef enum { From 1b3ebd44754064b5199ccc8fa8145053b2b4f366 Mon Sep 17 00:00:00 2001 From: breadoven <56191411+breadoven@users.noreply.github.com> Date: Wed, 25 Nov 2020 19:14:34 +0000 Subject: [PATCH 026/286] Update settings.yaml --- src/main/fc/settings.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 60051e6a78..f32ae3b172 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -2137,7 +2137,7 @@ groups: field: general.flags.rth_alt_control_mode table: nav_rth_alt_mode - name: nav_rth_alt_control_override - description: "If set to ON the preset RTH altitude can be overridden during the climb phase, resetting to the current altitude, by holding the pitch stick in the full pitch down position for longer than 1 second" + description: "If set to ON RTH altitude and CLIMB FIRST settings can be overridden during the RTH climb phase using full pitch or roll stick held for > 2 seconds. RTH altitude is reset to the current altitude using pitch down stick. RTH CLIMB FIRST is overridden using right roll stick so craft turns and heads directly to home." default_value: "OFF" field: general.flags.rth_alt_control_override type: bool From b3005fbc3c0665128967eebbd392ecdb69d0a05e Mon Sep 17 00:00:00 2001 From: breadoven <56191411+breadoven@users.noreply.github.com> Date: Wed, 25 Nov 2020 22:24:57 +0000 Subject: [PATCH 027/286] Update navigation.c Fix ABS build error --- src/main/navigation/navigation.c | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c index 067f429e91..e1bbfdbfd8 100755 --- a/src/main/navigation/navigation.c +++ b/src/main/navigation/navigation.c @@ -2526,12 +2526,14 @@ static void rthAltControlStickOverrideCheck(unsigned axis) } static timeMs_t rthOverrideStickHoldStartTime[2]; - timeMs_t currentTime = millis(); if (rxGetChannelValue(axis) > rxConfig()->maxcheck) { + + timeDelta_t holdTime = millis() - rthOverrideStickHoldStartTime[axis]; + if (!rthOverrideStickHoldStartTime[axis]) { rthOverrideStickHoldStartTime[axis] = millis(); - } else if (ABS(2500 - (currentTime - rthOverrideStickHoldStartTime[axis])) < 500) { + } else if (ABS(2500 - holdTime) < 500) { if (axis == PITCH) { // pitch down to override preset altitude reset to current altitude posControl.rthState.rthInitialAltitude = posControl.actualState.abs.pos.z; posControl.rthState.rthFinalAltitude = posControl.rthState.rthInitialAltitude; From fcedba8618b201c6047c4fe960db4c98d09d0de7 Mon Sep 17 00:00:00 2001 From: breadoven <56191411+breadoven@users.noreply.github.com> Date: Sun, 29 Nov 2020 13:06:29 +0000 Subject: [PATCH 028/286] Improved Code Cleaned up code removing Climb First flag. Tested in flight, works as expected. --- src/main/navigation/navigation.c | 35 ++++++++++++------------ src/main/navigation/navigation.h | 2 +- src/main/navigation/navigation_private.h | 1 - 3 files changed, 18 insertions(+), 20 deletions(-) diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c index e1bbfdbfd8..cb5c58ddbf 100755 --- a/src/main/navigation/navigation.c +++ b/src/main/navigation/navigation.c @@ -229,7 +229,7 @@ static navigationFSMEvent_t nextForNonGeoStates(void); void initializeRTHSanityChecker(const fpVector3_t * pos); bool validateRTHSanityChecker(void); -static void rthAltControlStickOverrideCheck(unsigned axis); +static bool rthAltControlStickOverrideCheck(unsigned axis); /*************************************************************************************************/ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_IDLE(navigationFSMState_t previousState); @@ -1092,9 +1092,6 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_CRUISE_3D_ADJUSTING(nav static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_INITIALIZE(navigationFSMState_t previousState) { navigationFSMStateFlags_t prevFlags = navGetStateFlags(previousState); - - // reset flag to override RTH climb first - posControl.flags.rthClimbFirstOverride = false; if ((posControl.flags.estHeadingStatus == EST_NONE) || (posControl.flags.estAltStatus == EST_NONE) || (posControl.flags.estPosStatus != EST_TRUSTED) || !STATE(GPS_FIX_HOME)) { // Heading sensor, altitude sensor and HOME fix are mandatory for RTH. If not satisfied - switch to emergency landing @@ -1161,9 +1158,8 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_INITIALIZE(navigati static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_CLIMB_TO_SAFE_ALT(navigationFSMState_t previousState) { UNUSED(previousState); - + rthAltControlStickOverrideCheck(PITCH); - rthAltControlStickOverrideCheck(ROLL); if ((posControl.flags.estHeadingStatus == EST_NONE)) { return NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING; @@ -1180,7 +1176,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_CLIMB_TO_SAFE_ALT(n const float rthAltitudeMargin = MAX(FW_RTH_CLIMB_MARGIN_MIN_CM, (rthClimbMarginPercent/100.0) * fabsf(posControl.rthState.rthInitialAltitude - posControl.rthState.homePosition.pos.z)); // If we reached desired initial RTH altitude or we don't want to climb first - if (((navGetCurrentActualPositionAndVelocity()->pos.z - posControl.rthState.rthInitialAltitude) > -rthAltitudeMargin) || (!navConfig()->general.flags.rth_climb_first) || posControl.flags.rthClimbFirstOverride) { + if (((navGetCurrentActualPositionAndVelocity()->pos.z - posControl.rthState.rthInitialAltitude) > -rthAltitudeMargin) || (!navConfig()->general.flags.rth_climb_first) || rthAltControlStickOverrideCheck(ROLL)) { // Delayed initialization for RTH sanity check on airplanes - allow to finish climb first as it can take some distance if (STATE(FIXED_WING_LEGACY)) { @@ -1238,7 +1234,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_CLIMB_TO_SAFE_ALT(n static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_HEAD_HOME(navigationFSMState_t previousState) { UNUSED(previousState); - + rthAltControlStickOverrideCheck(PITCH); if ((posControl.flags.estHeadingStatus == EST_NONE)) { @@ -2517,33 +2513,36 @@ void updateHomePosition(void) /* ----------------------------------------------------------- * Override RTH preset altitude and Climb First option - * Set using Pitch/Roll stick held for > 2 seconds + * using Pitch/Roll stick held for > 2 seconds *-----------------------------------------------------------*/ -static void rthAltControlStickOverrideCheck(unsigned axis) +static bool rthAltControlStickOverrideCheck(unsigned axis) { if (!navConfig()->general.flags.rth_alt_control_override || posControl.flags.forcedRTHActivated) { - return; + return false; } static timeMs_t rthOverrideStickHoldStartTime[2]; if (rxGetChannelValue(axis) > rxConfig()->maxcheck) { - + timeDelta_t holdTime = millis() - rthOverrideStickHoldStartTime[axis]; - + if (!rthOverrideStickHoldStartTime[axis]) { rthOverrideStickHoldStartTime[axis] = millis(); - } else if (ABS(2500 - holdTime) < 500) { - if (axis == PITCH) { // pitch down to override preset altitude reset to current altitude + } else if (ABS(2500 - holdTime) < 500) { + if (axis == PITCH) { // PITCH down to override preset altitude reset to current altitude posControl.rthState.rthInitialAltitude = posControl.actualState.abs.pos.z; - posControl.rthState.rthFinalAltitude = posControl.rthState.rthInitialAltitude; - } else { // roll right to override climb first - posControl.flags.rthClimbFirstOverride = true; + posControl.rthState.rthFinalAltitude = posControl.rthState.rthInitialAltitude; + return true; + } else if (axis == ROLL) { // ROLL right to override climb first + return true; } } } else { rthOverrideStickHoldStartTime[axis] = 0; } + + return false; } /*----------------------------------------------------------- diff --git a/src/main/navigation/navigation.h b/src/main/navigation/navigation.h index 7b9c30606e..ac07036442 100755 --- a/src/main/navigation/navigation.h +++ b/src/main/navigation/navigation.h @@ -181,7 +181,7 @@ typedef struct navConfig_s { uint8_t disarm_on_landing; // uint8_t rth_allow_landing; // Enable landing as last stage of RTH. Use constants in navRTHAllowLanding_e. uint8_t rth_climb_ignore_emerg; // Option to ignore GPS loss on initial climb stage of RTH - uint8_t rth_alt_control_override; // Set RTH preset climb altitude to Current altutude during climb using full down pitch stick + uint8_t rth_alt_control_override; // Override RTH Altitude and Climb First settings using Pitch and Roll stick uint8_t nav_overrides_motor_stop; // Autonomous modes override motor_stop setting and user command to stop motor } flags; diff --git a/src/main/navigation/navigation_private.h b/src/main/navigation/navigation_private.h index 5d3818d055..fcae1b7ded 100755 --- a/src/main/navigation/navigation_private.h +++ b/src/main/navigation/navigation_private.h @@ -87,7 +87,6 @@ typedef struct navigationFlags_s { bool isTerrainFollowEnabled; // Does iNav use rangefinder for terrain following (adjusting baro altitude target according to rangefinders readings) bool forcedRTHActivated; - bool rthClimbFirstOverride; // RTH Climb First override using Roll stick } navigationFlags_t; typedef enum { From 2e38d12f32adab12d9fa3b61f45e6a4902d6514c Mon Sep 17 00:00:00 2001 From: breadoven <56191411+breadoven@users.noreply.github.com> Date: Thu, 3 Dec 2020 23:31:54 +0000 Subject: [PATCH 029/286] Improve Virtual Current Sensor Virtual current sensor improved by taking into account ability to run motors in Nav modes with throttle low and motor stop active. --- src/main/sensors/battery.c | 12 +++++++++++- 1 file changed, 11 insertions(+), 1 deletion(-) diff --git a/src/main/sensors/battery.c b/src/main/sensors/battery.c index 01d2582ded..c6d7644b7a 100644 --- a/src/main/sensors/battery.c +++ b/src/main/sensors/battery.c @@ -44,6 +44,7 @@ #include "flight/mixer.h" #include "navigation/navigation.h" +#include "navigation/navigation_private.h" #include "config/feature.h" @@ -467,7 +468,16 @@ void currentMeterUpdate(timeUs_t timeDelta) amperage = batteryMetersConfig()->current.offset; if (ARMING_FLAG(ARMED)) { throttleStatus_e throttleStatus = calculateThrottleStatus(THROTTLE_STATUS_TYPE_RC); - int32_t throttleOffset = ((throttleStatus == THROTTLE_LOW) && feature(FEATURE_MOTOR_STOP)) ? 0 : (int32_t)rcCommand[THROTTLE] - 1000; + navigationFSMStateFlags_t stateFlags = navGetCurrentStateFlags(); + bool allNav = navConfig()->general.flags.nav_overrides_motor_stop == NOMS_ALL_NAV && posControl.navState != NAV_STATE_IDLE; + bool autoNav = navConfig()->general.flags.nav_overrides_motor_stop == NOMS_AUTO_ONLY && (stateFlags & (NAV_AUTO_RTH | NAV_AUTO_WP)); + int32_t throttleOffset; + + if (allNav || autoNav) { // account for motors running in Nav modes with throttle low + motor stop + throttleOffset = (int32_t)rcCommand[THROTTLE] - 1000; + } else { + throttleOffset = ((throttleStatus == THROTTLE_LOW) && feature(FEATURE_MOTOR_STOP)) ? 0 : (int32_t)rcCommand[THROTTLE] - 1000; + } int32_t throttleFactor = throttleOffset + (throttleOffset * throttleOffset / 50); amperage += throttleFactor * batteryMetersConfig()->current.scale / 1000; } From d6e3677455830185c263d19e24c340dafa3007f1 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Niccol=C3=B2=20Maggioni?= Date: Thu, 24 Sep 2020 19:40:11 +0200 Subject: [PATCH 030/286] MATEKF411SE_PINIO target LED pad repurposed as 2nd PINIO (USER2) --- src/main/target/MATEKF411SE/CMakeLists.txt | 1 + src/main/target/MATEKF411SE/config.c | 3 +++ src/main/target/MATEKF411SE/target.c | 2 ++ src/main/target/MATEKF411SE/target.h | 5 +++++ 4 files changed, 11 insertions(+) diff --git a/src/main/target/MATEKF411SE/CMakeLists.txt b/src/main/target/MATEKF411SE/CMakeLists.txt index df67c6f00b..919b98fc17 100644 --- a/src/main/target/MATEKF411SE/CMakeLists.txt +++ b/src/main/target/MATEKF411SE/CMakeLists.txt @@ -1 +1,2 @@ target_stm32f411xe(MATEKF411SE) +target_stm32f411xe(MATEKF411SE_PINIO) diff --git a/src/main/target/MATEKF411SE/config.c b/src/main/target/MATEKF411SE/config.c index 07f6de4697..38bf22dd8e 100755 --- a/src/main/target/MATEKF411SE/config.c +++ b/src/main/target/MATEKF411SE/config.c @@ -25,4 +25,7 @@ void targetConfiguration(void) { pinioBoxConfigMutable()->permanentId[0] = BOX_PERMANENT_ID_USER1; +#ifdef MATEKF411SE_PINIO + pinioBoxConfigMutable()->permanentId[1] = BOX_PERMANENT_ID_USER2; +#endif } diff --git a/src/main/target/MATEKF411SE/target.c b/src/main/target/MATEKF411SE/target.c index be71e58f2d..72392c9fa9 100755 --- a/src/main/target/MATEKF411SE/target.c +++ b/src/main/target/MATEKF411SE/target.c @@ -35,7 +35,9 @@ const timerHardware_t timerHardware[] = { DEF_TIM(TIM5, CH3, PA2, TIM_USE_ANY, 0, 0), //TX2 pad -softserial_tx2 DEF_TIM(TIM9, CH2, PA3, TIM_USE_PPM, 0, 0), //RX2 Pad -PPM +#ifndef MATEKF411SE_PINIO DEF_TIM(TIM2, CH3, PB10, TIM_USE_LED, 0, 0), //LED 2812 D(1,1,3) +#endif }; diff --git a/src/main/target/MATEKF411SE/target.h b/src/main/target/MATEKF411SE/target.h index a91f0eb177..46f5c05c99 100755 --- a/src/main/target/MATEKF411SE/target.h +++ b/src/main/target/MATEKF411SE/target.h @@ -131,13 +131,18 @@ #define AIRSPEED_ADC_CHANNEL ADC_CHN_4 // *************** LED2812 ************************ +#ifndef MATEKF411SE_PINIO #define USE_LED_STRIP #define WS2811_PIN PB10 +#endif // *************** PINIO *************************** #define USE_PINIO #define USE_PINIOBOX #define PINIO1_PIN PA13 // Camera switcher +#ifdef MATEKF411SE_PINIO +#define PINIO2_PIN PB10 // External PINIO (LED pad) +#endif // *************** OTHERS ************************* #define DEFAULT_FEATURES (FEATURE_TX_PROF_SEL | FEATURE_OSD | FEATURE_CURRENT_METER | FEATURE_VBAT | FEATURE_TELEMETRY | FEATURE_SOFTSERIAL ) From adf5b38e3d4a13a115039d209cc8afd0cb98aa9b Mon Sep 17 00:00:00 2001 From: breadoven <56191411+breadoven@users.noreply.github.com> Date: Sun, 6 Dec 2020 10:33:42 +0000 Subject: [PATCH 031/286] Update Settings.md --- docs/Settings.md | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/docs/Settings.md b/docs/Settings.md index 00999bf75a..9aaa230afe 100644 --- a/docs/Settings.md +++ b/docs/Settings.md @@ -277,8 +277,8 @@ | nav_fw_max_thr | 1700 | Maximum throttle for flying wing in GPS assisted modes | | nav_fw_min_thr | 1200 | Minimum throttle for flying wing in GPS assisted modes | | nav_fw_pitch2thr | 10 | Amount of throttle applied related to pitch attitude in GPS assisted modes. Throttle = nav_fw_cruise_throttle - (nav_fw_pitch2thr * pitch_angle). (notice that pitch_angle is in degrees and is negative when climbing and positive when diving, and throttle value is constrained between nav_fw_min_thr and nav_fw_max_thr) | -| nav_fw_pitch2thr_smoothing | 0 | How smoothly the autopilot makes pitch to throttle correction inside a deadband defined by pitch_to_throttle_thresh. | -| nav_fw_pitch2thr_threshold | 0 | Threshold from average pitch where momentary pitch_to_throttle correction kicks in. [decidegrees] | +| nav_fw_pitch2thr_smoothing | 6 | How smoothly the autopilot makes pitch to throttle correction inside a deadband defined by pitch_to_throttle_thresh. | +| nav_fw_pitch2thr_threshold | 50 | Threshold from average pitch where momentary pitch_to_throttle correction kicks in. [decidegrees] | | nav_fw_pos_hdg_d | 0 | D gain of heading trajectory PID controller. (Fixedwing, rovers, boats) | | nav_fw_pos_hdg_i | 0 | I gain of heading trajectory PID controller. (Fixedwing, rovers, boats) | | nav_fw_pos_hdg_p | 60 | P gain of heading PID controller. (Fixedwing, rovers, boats) | @@ -324,10 +324,11 @@ | nav_mc_vel_z_i | 50 | I gain of velocity PID controller | | nav_mc_vel_z_p | 100 | P gain of velocity PID controller | | nav_min_rth_distance | 500 | Minimum distance from homepoint when RTH full procedure will be activated [cm]. Below this distance, the mode will activate at the current location and the final phase is executed (loiter / land). Above this distance, the full procedure is activated, which may include initial climb and flying directly to the homepoint before entering the loiter / land phase. | -| nav_overrides_motor_stop | ALL_NAV | When set OFF_ALWAYS the navigation system will not take over the control of the motor if the throttle is low (motor will stop). When set to OFF always the navigation system will not take over the control of the motor if the throttle was low even when failsafe is triggered. When set to AUTO_ONLY the navigation system will only take over the control of the throttle in autonomous navigation modes (NAV WP and NAV RTH). When set to ALL_NAV (default) the navigation system will take over the control of the motor completely and never allow the motor to stop even when the throttle is low. This setting only has an effect combined with MOTOR_STOP and is likely to cause a stall if fw_min_throttle_down_pitch isn't set correctly or the pitch estimation is wrong for fixed wing models when not set to ALL_NAV | +| nav_overrides_motor_stop | ALL_NAV | When set OFF_ALWAYS the navigation system will not take over the control of the motor if the throttle is low (motor will stop). When set to OFF always the navigation system will not take over the control of the motor if the throttle was low even when failsafe is triggered. When set to AUTO_ONLY the navigation system will only take over the control of the throttle in autonomous navigation modes (NAV WP and NAV RTH). When set to ALL_NAV (default) the navigation system will take over the control of the motor completely and never allow the motor to stop even when the throttle is low. This setting only has an effect on NAV modes which take control of the throttle when combined with MOTOR_STOP and is likely to cause a stall if fw_min_throttle_down_pitch isn't set correctly or the pitch estimation is wrong for fixed wing models when not set to ALL_NAV | | nav_position_timeout | 5 | If GPS fails wait for this much seconds before switching to emergency landing mode (0 - disable) | | nav_rth_abort_threshold | 50000 | RTH sanity checking feature will notice if distance to home is increasing during RTH and once amount of increase exceeds the threshold defined by this parameter, instead of continuing RTH machine will enter emergency landing, self-level and go down safely. Default is 500m which is safe enough for both multirotor machines and airplanes. [cm] | | nav_rth_allow_landing | ALWAYS | If set to ON drone will land as a last phase of RTH. | +| nav_rth_alt_control_override | OFF | If set to ON RTH altitude and CLIMB FIRST settings can be overridden during the RTH climb phase using full pitch or roll stick held for > 2 seconds. RTH altitude is reset to the current altitude using pitch down stick. RTH CLIMB FIRST is overridden using right roll stick so craft turns and heads directly to home. | | nav_rth_alt_mode | AT_LEAST | Configure how the aircraft will manage altitude on the way home, see Navigation modes on wiki for more details | | nav_rth_altitude | 1000 | Used in EXTRA, FIXED and AT_LEAST rth alt modes [cm] (Default 1000 means 10 meters) | | nav_rth_climb_first | ON | If set to ON drone will climb to nav_rth_altitude first and head home afterwards. If set to OFF drone will head home instantly and climb on the way. | From a1f9bbdc8c97f549f944161a608ef075bff0dc11 Mon Sep 17 00:00:00 2001 From: breadoven <56191411+breadoven@users.noreply.github.com> Date: Thu, 17 Dec 2020 15:33:27 +0000 Subject: [PATCH 032/286] Fix Nav Launch Related Errors Fixes: Launch detection bug. Repeated redundant code RTH estimator bug - not detecting Nav launch aborted prior to launch detection --- src/main/flight/rth_estimator.c | 2 +- src/main/navigation/navigation_fw_launch.c | 6 +----- 2 files changed, 2 insertions(+), 6 deletions(-) diff --git a/src/main/flight/rth_estimator.c b/src/main/flight/rth_estimator.c index 4ddc719037..47f2937adf 100644 --- a/src/main/flight/rth_estimator.c +++ b/src/main/flight/rth_estimator.c @@ -153,7 +153,7 @@ static float calculateRemainingEnergyBeforeRTH(bool takeWindIntoAccount) { && feature(FEATURE_CURRENT_METER) && (batteryMetersConfig()->cruise_power > 0) && (currentBatteryProfile->capacity.unit == BAT_CAPACITY_UNIT_MWH) && (currentBatteryProfile->capacity.value > 0) && navigationPositionEstimateIsHealthy() && isImuHeadingValid() && (navConfig()->fw.cruise_speed > 0) - && ((!STATE(FIXED_WING_LEGACY)) || !isNavLaunchEnabled() || (isNavLaunchEnabled() && isFixedWingLaunchDetected())) + && ((!STATE(FIXED_WING_LEGACY)) || !isNavLaunchEnabled() || (isNavLaunchEnabled() && (isFixedWingLaunchDetected() || isFixedWingLaunchFinishedOrAborted()))) && (ARMING_FLAG(ARMED)) #ifdef USE_WIND_ESTIMATOR && (!takeWindIntoAccount || isEstimatedWindSpeedValid()) diff --git a/src/main/navigation/navigation_fw_launch.c b/src/main/navigation/navigation_fw_launch.c index 0b3c1633be..a4fd72c500 100755 --- a/src/main/navigation/navigation_fw_launch.c +++ b/src/main/navigation/navigation_fw_launch.c @@ -388,10 +388,6 @@ static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_IN_PROGRESS(timeUs_t return FW_LAUNCH_EVENT_ABORT; // cancel the launch and do the FW_LAUNCH_STATE_IDLE state } - if (isLaunchMaxAltitudeReached()) { - return FW_LAUNCH_EVENT_SUCCESS; // cancel the launch and do the FW_LAUNCH_STATE_FINISH state - } - if (currentStateElapsedMs(currentTimeUs) > navConfig()->fw.launch_timeout) { return FW_LAUNCH_EVENT_SUCCESS; // launch timeout. go to FW_LAUNCH_STATE_FINISH } @@ -453,7 +449,7 @@ void resetFixedWingLaunchController(timeUs_t currentTimeUs) bool isFixedWingLaunchDetected(void) { - return fwLaunch.currentState == FW_LAUNCH_STATE_DETECTED; + return fwLaunch.currentState >= FW_LAUNCH_STATE_DETECTED; } void enableFixedWingLaunchController(timeUs_t currentTimeUs) From e18ecdddaf5d9bde79830f4f8c285872775d234b Mon Sep 17 00:00:00 2001 From: breadoven <56191411+breadoven@users.noreply.github.com> Date: Mon, 21 Dec 2020 09:57:20 +0000 Subject: [PATCH 033/286] FW RTH Spiral Climb Uses loiter climb (spiral) for FW RTH with Climb First option. --- docs/Settings.md | 7 ++++--- src/main/fc/settings.yaml | 5 +++++ src/main/navigation/navigation.c | 19 +++++++++++++++---- src/main/navigation/navigation.h | 1 + 4 files changed, 25 insertions(+), 7 deletions(-) diff --git a/docs/Settings.md b/docs/Settings.md index 00999bf75a..ced9240bd7 100644 --- a/docs/Settings.md +++ b/docs/Settings.md @@ -277,8 +277,8 @@ | nav_fw_max_thr | 1700 | Maximum throttle for flying wing in GPS assisted modes | | nav_fw_min_thr | 1200 | Minimum throttle for flying wing in GPS assisted modes | | nav_fw_pitch2thr | 10 | Amount of throttle applied related to pitch attitude in GPS assisted modes. Throttle = nav_fw_cruise_throttle - (nav_fw_pitch2thr * pitch_angle). (notice that pitch_angle is in degrees and is negative when climbing and positive when diving, and throttle value is constrained between nav_fw_min_thr and nav_fw_max_thr) | -| nav_fw_pitch2thr_smoothing | 0 | How smoothly the autopilot makes pitch to throttle correction inside a deadband defined by pitch_to_throttle_thresh. | -| nav_fw_pitch2thr_threshold | 0 | Threshold from average pitch where momentary pitch_to_throttle correction kicks in. [decidegrees] | +| nav_fw_pitch2thr_smoothing | 6 | How smoothly the autopilot makes pitch to throttle correction inside a deadband defined by pitch_to_throttle_thresh. | +| nav_fw_pitch2thr_threshold | 50 | Threshold from average pitch where momentary pitch_to_throttle correction kicks in. [decidegrees] | | nav_fw_pos_hdg_d | 0 | D gain of heading trajectory PID controller. (Fixedwing, rovers, boats) | | nav_fw_pos_hdg_i | 0 | I gain of heading trajectory PID controller. (Fixedwing, rovers, boats) | | nav_fw_pos_hdg_p | 60 | P gain of heading PID controller. (Fixedwing, rovers, boats) | @@ -324,7 +324,7 @@ | nav_mc_vel_z_i | 50 | I gain of velocity PID controller | | nav_mc_vel_z_p | 100 | P gain of velocity PID controller | | nav_min_rth_distance | 500 | Minimum distance from homepoint when RTH full procedure will be activated [cm]. Below this distance, the mode will activate at the current location and the final phase is executed (loiter / land). Above this distance, the full procedure is activated, which may include initial climb and flying directly to the homepoint before entering the loiter / land phase. | -| nav_overrides_motor_stop | ALL_NAV | When set OFF_ALWAYS the navigation system will not take over the control of the motor if the throttle is low (motor will stop). When set to OFF always the navigation system will not take over the control of the motor if the throttle was low even when failsafe is triggered. When set to AUTO_ONLY the navigation system will only take over the control of the throttle in autonomous navigation modes (NAV WP and NAV RTH). When set to ALL_NAV (default) the navigation system will take over the control of the motor completely and never allow the motor to stop even when the throttle is low. This setting only has an effect combined with MOTOR_STOP and is likely to cause a stall if fw_min_throttle_down_pitch isn't set correctly or the pitch estimation is wrong for fixed wing models when not set to ALL_NAV | +| nav_overrides_motor_stop | ALL_NAV | When set OFF_ALWAYS the navigation system will not take over the control of the motor if the throttle is low (motor will stop). When set to OFF always the navigation system will not take over the control of the motor if the throttle was low even when failsafe is triggered. When set to AUTO_ONLY the navigation system will only take over the control of the throttle in autonomous navigation modes (NAV WP and NAV RTH). When set to ALL_NAV (default) the navigation system will take over the control of the motor completely and never allow the motor to stop even when the throttle is low. This setting only has an effect on NAV modes which take control of the throttle when combined with MOTOR_STOP and is likely to cause a stall if fw_min_throttle_down_pitch isn't set correctly or the pitch estimation is wrong for fixed wing models when not set to ALL_NAV | | nav_position_timeout | 5 | If GPS fails wait for this much seconds before switching to emergency landing mode (0 - disable) | | nav_rth_abort_threshold | 50000 | RTH sanity checking feature will notice if distance to home is increasing during RTH and once amount of increase exceeds the threshold defined by this parameter, instead of continuing RTH machine will enter emergency landing, self-level and go down safely. Default is 500m which is safe enough for both multirotor machines and airplanes. [cm] | | nav_rth_allow_landing | ALWAYS | If set to ON drone will land as a last phase of RTH. | @@ -332,6 +332,7 @@ | nav_rth_altitude | 1000 | Used in EXTRA, FIXED and AT_LEAST rth alt modes [cm] (Default 1000 means 10 meters) | | nav_rth_climb_first | ON | If set to ON drone will climb to nav_rth_altitude first and head home afterwards. If set to OFF drone will head home instantly and climb on the way. | | nav_rth_climb_ignore_emerg | OFF | If set to ON, aircraft will execute initial climb regardless of position sensor (GPS) status. | +| nav_rth_fw_spiral_climb | OFF | Performs RTH loiter climb rather than a linear climb when RTH set to Climb First. Climb rate set by max_auto_climb_rate, turn rate set by nav_fw_loiter_radius. | | nav_rth_home_altitude | 0 | Aircraft will climb/descend to this altitude after reaching home if landing is not enabled. Set to 0 to stay at `nav_rth_altitude` (default) [cm] | | nav_rth_tail_first | OFF | If set to ON drone will return tail-first. Obviously meaningless for airplanes. | | nav_use_fw_yaw_control | OFF | Enables or Disables the use of the heading PID controller on fixed wing. Heading PID controller is always enabled for rovers and boats | diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 7d9104093c..127bc89207 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -2136,6 +2136,11 @@ groups: default_value: "AT_LEAST" field: general.flags.rth_alt_control_mode table: nav_rth_alt_mode + - name: nav_rth_fw_spiral_climb + description: "Performs RTH loiter climb rather than a linear climb when RTH set to Climb First. Climb rate set by max_auto_climb_rate, turn rate set by nav_fw_loiter_radius." + default_value: "OFF" + field: general.flags.rth_fw_spiral_climb + type: bool - name: nav_rth_abort_threshold description: "RTH sanity checking feature will notice if distance to home is increasing during RTH and once amount of increase exceeds the threshold defined by this parameter, instead of continuing RTH machine will enter emergency landing, self-level and go down safely. Default is 500m which is safe enough for both multirotor machines and airplanes. [cm]" default_value: "50000" diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c index 0bcfb50b96..93ec395a8e 100755 --- a/src/main/navigation/navigation.c +++ b/src/main/navigation/navigation.c @@ -106,6 +106,7 @@ PG_RESET_TEMPLATE(navConfig_t, navConfig, .rth_tail_first = 0, .disarm_on_landing = 0, .rth_allow_landing = NAV_RTH_ALLOW_LANDING_ALWAYS, + .rth_fw_spiral_climb = 0, // Loiter climb for FW RTH .nav_overrides_motor_stop = NOMS_ALL_NAV, }, @@ -1150,8 +1151,13 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_INITIALIZE(navigati fpVector3_t targetHoldPos; if (STATE(FIXED_WING_LEGACY)) { - // Airplane - climbout before turning around - calculateFarAwayTarget(&targetHoldPos, posControl.actualState.yaw, 100000.0f); // 1km away + // Airplane - climbout before heading home + if (navConfig()->general.flags.rth_fw_spiral_climb) { + // Spiral climb centered at xy of RTH activation + calculateInitialHoldPosition(&targetHoldPos); + } else { + calculateFarAwayTarget(&targetHoldPos, posControl.actualState.yaw, 100000.0f); // 1km away Linear climb + } } else { // Multicopter, hover and climb calculateInitialHoldPosition(&targetHoldPos); @@ -1226,8 +1232,13 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_CLIMB_TO_SAFE_ALT(n // Climb to safe altitude and turn to correct direction if (STATE(FIXED_WING_LEGACY)) { - tmpHomePos->z += FW_RTH_CLIMB_OVERSHOOT_CM; - setDesiredPosition(tmpHomePos, 0, NAV_POS_UPDATE_Z); + if (navConfig()->general.flags.rth_fw_spiral_climb) { + float altitudeChangeDirection = (tmpHomePos->z += FW_RTH_CLIMB_OVERSHOOT_CM) > navGetCurrentActualPositionAndVelocity()->pos.z ? 1 : -1; + updateClimbRateToAltitudeController(altitudeChangeDirection * navConfig()->general.max_auto_climb_rate, ROC_TO_ALT_NORMAL); + } else { + tmpHomePos->z += FW_RTH_CLIMB_OVERSHOOT_CM; + setDesiredPosition(tmpHomePos, 0, NAV_POS_UPDATE_Z); + } } else { // Until the initial climb phase is complete target slightly *above* the cruise altitude to ensure we actually reach // it in a reasonable time. Immediately after we finish this phase - target the original altitude. diff --git a/src/main/navigation/navigation.h b/src/main/navigation/navigation.h index 5dcd8c71ca..445d2d598d 100755 --- a/src/main/navigation/navigation.h +++ b/src/main/navigation/navigation.h @@ -183,6 +183,7 @@ typedef struct navConfig_s { uint8_t rth_allow_landing; // Enable landing as last stage of RTH. Use constants in navRTHAllowLanding_e. uint8_t rth_climb_ignore_emerg; // Option to ignore GPS loss on initial climb stage of RTH uint8_t nav_overrides_motor_stop; // Autonomous modes override motor_stop setting and user command to stop motor + uint8_t rth_fw_spiral_climb; // Enable RTH spiral climb for FW when "climb first" selected } flags; uint8_t pos_failure_timeout; // Time to wait before switching to emergency landing (0 - disable) From b6279d8a07826feb8284d38467c868c11eec6957 Mon Sep 17 00:00:00 2001 From: breadoven <56191411+breadoven@users.noreply.github.com> Date: Mon, 21 Dec 2020 10:08:50 +0000 Subject: [PATCH 034/286] Update Settings.md --- docs/Settings.md | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/docs/Settings.md b/docs/Settings.md index ced9240bd7..924a3a7f63 100644 --- a/docs/Settings.md +++ b/docs/Settings.md @@ -277,8 +277,8 @@ | nav_fw_max_thr | 1700 | Maximum throttle for flying wing in GPS assisted modes | | nav_fw_min_thr | 1200 | Minimum throttle for flying wing in GPS assisted modes | | nav_fw_pitch2thr | 10 | Amount of throttle applied related to pitch attitude in GPS assisted modes. Throttle = nav_fw_cruise_throttle - (nav_fw_pitch2thr * pitch_angle). (notice that pitch_angle is in degrees and is negative when climbing and positive when diving, and throttle value is constrained between nav_fw_min_thr and nav_fw_max_thr) | -| nav_fw_pitch2thr_smoothing | 6 | How smoothly the autopilot makes pitch to throttle correction inside a deadband defined by pitch_to_throttle_thresh. | -| nav_fw_pitch2thr_threshold | 50 | Threshold from average pitch where momentary pitch_to_throttle correction kicks in. [decidegrees] | +| nav_fw_pitch2thr_smoothing | 0 | How smoothly the autopilot makes pitch to throttle correction inside a deadband defined by pitch_to_throttle_thresh. | +| nav_fw_pitch2thr_threshold | 0 | Threshold from average pitch where momentary pitch_to_throttle correction kicks in. [decidegrees] | | nav_fw_pos_hdg_d | 0 | D gain of heading trajectory PID controller. (Fixedwing, rovers, boats) | | nav_fw_pos_hdg_i | 0 | I gain of heading trajectory PID controller. (Fixedwing, rovers, boats) | | nav_fw_pos_hdg_p | 60 | P gain of heading PID controller. (Fixedwing, rovers, boats) | @@ -324,7 +324,7 @@ | nav_mc_vel_z_i | 50 | I gain of velocity PID controller | | nav_mc_vel_z_p | 100 | P gain of velocity PID controller | | nav_min_rth_distance | 500 | Minimum distance from homepoint when RTH full procedure will be activated [cm]. Below this distance, the mode will activate at the current location and the final phase is executed (loiter / land). Above this distance, the full procedure is activated, which may include initial climb and flying directly to the homepoint before entering the loiter / land phase. | -| nav_overrides_motor_stop | ALL_NAV | When set OFF_ALWAYS the navigation system will not take over the control of the motor if the throttle is low (motor will stop). When set to OFF always the navigation system will not take over the control of the motor if the throttle was low even when failsafe is triggered. When set to AUTO_ONLY the navigation system will only take over the control of the throttle in autonomous navigation modes (NAV WP and NAV RTH). When set to ALL_NAV (default) the navigation system will take over the control of the motor completely and never allow the motor to stop even when the throttle is low. This setting only has an effect on NAV modes which take control of the throttle when combined with MOTOR_STOP and is likely to cause a stall if fw_min_throttle_down_pitch isn't set correctly or the pitch estimation is wrong for fixed wing models when not set to ALL_NAV | +| nav_overrides_motor_stop | ALL_NAV | When set OFF_ALWAYS the navigation system will not take over the control of the motor if the throttle is low (motor will stop). When set to OFF always the navigation system will not take over the control of the motor if the throttle was low even when failsafe is triggered. When set to AUTO_ONLY the navigation system will only take over the control of the throttle in autonomous navigation modes (NAV WP and NAV RTH). When set to ALL_NAV (default) the navigation system will take over the control of the motor completely and never allow the motor to stop even when the throttle is low. This setting only has an effect combined with MOTOR_STOP and is likely to cause a stall if fw_min_throttle_down_pitch isn't set correctly or the pitch estimation is wrong for fixed wing models when not set to ALL_NAV | | nav_position_timeout | 5 | If GPS fails wait for this much seconds before switching to emergency landing mode (0 - disable) | | nav_rth_abort_threshold | 50000 | RTH sanity checking feature will notice if distance to home is increasing during RTH and once amount of increase exceeds the threshold defined by this parameter, instead of continuing RTH machine will enter emergency landing, self-level and go down safely. Default is 500m which is safe enough for both multirotor machines and airplanes. [cm] | | nav_rth_allow_landing | ALWAYS | If set to ON drone will land as a last phase of RTH. | From ec2d8dcb9f824904ddfb6cfa0dcee33c74139dd5 Mon Sep 17 00:00:00 2001 From: breadoven <56191411+breadoven@users.noreply.github.com> Date: Mon, 21 Dec 2020 10:25:53 +0000 Subject: [PATCH 035/286] Revert "Update Settings.md" This reverts commit b6279d8a07826feb8284d38467c868c11eec6957. --- docs/Settings.md | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/docs/Settings.md b/docs/Settings.md index 924a3a7f63..ced9240bd7 100644 --- a/docs/Settings.md +++ b/docs/Settings.md @@ -277,8 +277,8 @@ | nav_fw_max_thr | 1700 | Maximum throttle for flying wing in GPS assisted modes | | nav_fw_min_thr | 1200 | Minimum throttle for flying wing in GPS assisted modes | | nav_fw_pitch2thr | 10 | Amount of throttle applied related to pitch attitude in GPS assisted modes. Throttle = nav_fw_cruise_throttle - (nav_fw_pitch2thr * pitch_angle). (notice that pitch_angle is in degrees and is negative when climbing and positive when diving, and throttle value is constrained between nav_fw_min_thr and nav_fw_max_thr) | -| nav_fw_pitch2thr_smoothing | 0 | How smoothly the autopilot makes pitch to throttle correction inside a deadband defined by pitch_to_throttle_thresh. | -| nav_fw_pitch2thr_threshold | 0 | Threshold from average pitch where momentary pitch_to_throttle correction kicks in. [decidegrees] | +| nav_fw_pitch2thr_smoothing | 6 | How smoothly the autopilot makes pitch to throttle correction inside a deadband defined by pitch_to_throttle_thresh. | +| nav_fw_pitch2thr_threshold | 50 | Threshold from average pitch where momentary pitch_to_throttle correction kicks in. [decidegrees] | | nav_fw_pos_hdg_d | 0 | D gain of heading trajectory PID controller. (Fixedwing, rovers, boats) | | nav_fw_pos_hdg_i | 0 | I gain of heading trajectory PID controller. (Fixedwing, rovers, boats) | | nav_fw_pos_hdg_p | 60 | P gain of heading PID controller. (Fixedwing, rovers, boats) | @@ -324,7 +324,7 @@ | nav_mc_vel_z_i | 50 | I gain of velocity PID controller | | nav_mc_vel_z_p | 100 | P gain of velocity PID controller | | nav_min_rth_distance | 500 | Minimum distance from homepoint when RTH full procedure will be activated [cm]. Below this distance, the mode will activate at the current location and the final phase is executed (loiter / land). Above this distance, the full procedure is activated, which may include initial climb and flying directly to the homepoint before entering the loiter / land phase. | -| nav_overrides_motor_stop | ALL_NAV | When set OFF_ALWAYS the navigation system will not take over the control of the motor if the throttle is low (motor will stop). When set to OFF always the navigation system will not take over the control of the motor if the throttle was low even when failsafe is triggered. When set to AUTO_ONLY the navigation system will only take over the control of the throttle in autonomous navigation modes (NAV WP and NAV RTH). When set to ALL_NAV (default) the navigation system will take over the control of the motor completely and never allow the motor to stop even when the throttle is low. This setting only has an effect combined with MOTOR_STOP and is likely to cause a stall if fw_min_throttle_down_pitch isn't set correctly or the pitch estimation is wrong for fixed wing models when not set to ALL_NAV | +| nav_overrides_motor_stop | ALL_NAV | When set OFF_ALWAYS the navigation system will not take over the control of the motor if the throttle is low (motor will stop). When set to OFF always the navigation system will not take over the control of the motor if the throttle was low even when failsafe is triggered. When set to AUTO_ONLY the navigation system will only take over the control of the throttle in autonomous navigation modes (NAV WP and NAV RTH). When set to ALL_NAV (default) the navigation system will take over the control of the motor completely and never allow the motor to stop even when the throttle is low. This setting only has an effect on NAV modes which take control of the throttle when combined with MOTOR_STOP and is likely to cause a stall if fw_min_throttle_down_pitch isn't set correctly or the pitch estimation is wrong for fixed wing models when not set to ALL_NAV | | nav_position_timeout | 5 | If GPS fails wait for this much seconds before switching to emergency landing mode (0 - disable) | | nav_rth_abort_threshold | 50000 | RTH sanity checking feature will notice if distance to home is increasing during RTH and once amount of increase exceeds the threshold defined by this parameter, instead of continuing RTH machine will enter emergency landing, self-level and go down safely. Default is 500m which is safe enough for both multirotor machines and airplanes. [cm] | | nav_rth_allow_landing | ALWAYS | If set to ON drone will land as a last phase of RTH. | From e0840a4ae6e7784ec46ccff8c61e47b1576dce90 Mon Sep 17 00:00:00 2001 From: breadoven <56191411+breadoven@users.noreply.github.com> Date: Wed, 30 Dec 2020 14:26:19 +0000 Subject: [PATCH 036/286] Update rth_estimator.c --- src/main/flight/rth_estimator.c | 37 +++++++++++++++++++-------------- 1 file changed, 21 insertions(+), 16 deletions(-) diff --git a/src/main/flight/rth_estimator.c b/src/main/flight/rth_estimator.c index 47f2937adf..db6a41adce 100644 --- a/src/main/flight/rth_estimator.c +++ b/src/main/flight/rth_estimator.c @@ -145,21 +145,6 @@ static float estimateRTHEnergyAfterInitialClimb(float distanceToHome, float spee // returns Wh static float calculateRemainingEnergyBeforeRTH(bool takeWindIntoAccount) { - // Fixed wing only for now - if (!STATE(FIXED_WING_LEGACY)) - return -1; - - if (!(feature(FEATURE_VBAT) && batteryWasFullWhenPluggedIn() - && feature(FEATURE_CURRENT_METER) && (batteryMetersConfig()->cruise_power > 0) - && (currentBatteryProfile->capacity.unit == BAT_CAPACITY_UNIT_MWH) && (currentBatteryProfile->capacity.value > 0) - && navigationPositionEstimateIsHealthy() && isImuHeadingValid() && (navConfig()->fw.cruise_speed > 0) - && ((!STATE(FIXED_WING_LEGACY)) || !isNavLaunchEnabled() || (isNavLaunchEnabled() && (isFixedWingLaunchDetected() || isFixedWingLaunchFinishedOrAborted()))) - && (ARMING_FLAG(ARMED)) -#ifdef USE_WIND_ESTIMATOR - && (!takeWindIntoAccount || isEstimatedWindSpeedValid()) -#endif - )) - return -1; const float RTH_initial_altitude_change = MAX(0, (getFinalRTHAltitude() - getEstimatedActualPosition(Z)) / 100); @@ -223,7 +208,27 @@ float calculateRemainingFlightTimeBeforeRTH(bool takeWindIntoAccount) { // returns meters float calculateRemainingDistanceBeforeRTH(bool takeWindIntoAccount) { - + + // Fixed wing only for now + if (!(STATE(FIXED_WING_LEGACY) || ARMING_FLAG(ARMED))) { + return -1; + } + +#ifdef USE_WIND_ESTIMATOR + if (takeWindIntoAccount && !isEstimatedWindSpeedValid()) { + return -1; + } +#endif + + // check requirements + const bool areBatterySettingsOK = feature(FEATURE_VBAT) && feature(FEATURE_CURRENT_METER) && batteryWasFullWhenPluggedIn(); + const bool areRTHEstimatorSettingsOK = batteryMetersConfig()->cruise_power > 0 && currentBatteryProfile->capacity.unit == BAT_CAPACITY_UNIT_MWH &¤tBatteryProfile->capacity.value > 0 && navConfig()->fw.cruise_speed > 0; + const bool isNavigationOK = navigationPositionEstimateIsHealthy() && isImuHeadingValid(); + + if (!(areBatterySettingsOK && areRTHEstimatorSettingsOK && isNavigationOK)) { + return -1; + } + const float remainingFlightTimeBeforeRTH = calculateRemainingFlightTimeBeforeRTH(takeWindIntoAccount); // error: return error code directly From 2d74af63009f3dccc1e6be27a9bf6eb32e2921f9 Mon Sep 17 00:00:00 2001 From: breadoven <56191411+breadoven@users.noreply.github.com> Date: Thu, 31 Dec 2020 12:20:10 +0000 Subject: [PATCH 037/286] Update cms_menu_navigation.c --- src/main/cms/cms_menu_navigation.c | 1 + 1 file changed, 1 insertion(+) diff --git a/src/main/cms/cms_menu_navigation.c b/src/main/cms/cms_menu_navigation.c index d17d3df290..4b5bd24686 100644 --- a/src/main/cms/cms_menu_navigation.c +++ b/src/main/cms/cms_menu_navigation.c @@ -73,6 +73,7 @@ static const CMS_Menu cmsx_menuNavSettings = { OSD_SETTING_ENTRY("RTH ALT MODE", SETTING_NAV_RTH_ALT_MODE), OSD_SETTING_ENTRY("RTH ALT", SETTING_NAV_RTH_ALTITUDE), OSD_SETTING_ENTRY("CLIMB BEFORE RTH", SETTING_NAV_RTH_CLIMB_FIRST), + OSD_SETTING_ENTRY("FW SPIRAL CLIMB", SETTING_NAV_RTH_FW_SPIRAL_CLIMB), OSD_SETTING_ENTRY("TAIL FIRST", SETTING_NAV_RTH_TAIL_FIRST), OSD_SETTING_ENTRY("LAND AFTER RTH", SETTING_NAV_RTH_ALLOW_LANDING), OSD_SETTING_ENTRY("LAND VERT SPEED", SETTING_NAV_LANDING_SPEED), From 3f990f19f28a50f5d6b525f6236e43402d78da45 Mon Sep 17 00:00:00 2001 From: breadoven <56191411+breadoven@users.noreply.github.com> Date: Thu, 31 Dec 2020 12:50:37 +0000 Subject: [PATCH 038/286] Update Settings.md --- docs/Settings.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/docs/Settings.md b/docs/Settings.md index ced9240bd7..68444c5142 100644 --- a/docs/Settings.md +++ b/docs/Settings.md @@ -324,7 +324,7 @@ | nav_mc_vel_z_i | 50 | I gain of velocity PID controller | | nav_mc_vel_z_p | 100 | P gain of velocity PID controller | | nav_min_rth_distance | 500 | Minimum distance from homepoint when RTH full procedure will be activated [cm]. Below this distance, the mode will activate at the current location and the final phase is executed (loiter / land). Above this distance, the full procedure is activated, which may include initial climb and flying directly to the homepoint before entering the loiter / land phase. | -| nav_overrides_motor_stop | ALL_NAV | When set OFF_ALWAYS the navigation system will not take over the control of the motor if the throttle is low (motor will stop). When set to OFF always the navigation system will not take over the control of the motor if the throttle was low even when failsafe is triggered. When set to AUTO_ONLY the navigation system will only take over the control of the throttle in autonomous navigation modes (NAV WP and NAV RTH). When set to ALL_NAV (default) the navigation system will take over the control of the motor completely and never allow the motor to stop even when the throttle is low. This setting only has an effect on NAV modes which take control of the throttle when combined with MOTOR_STOP and is likely to cause a stall if fw_min_throttle_down_pitch isn't set correctly or the pitch estimation is wrong for fixed wing models when not set to ALL_NAV | +| nav_overrides_motor_stop | ALL_NAV | When set to OFF the navigation system will not take over the control of the motor if the throttle is low (motor will stop). When set to OFF_ALWAYS the navigation system will not take over the control of the motor if the throttle was low even when failsafe is triggered. When set to AUTO_ONLY the navigation system will only take over the control of the throttle in autonomous navigation modes (NAV WP and NAV RTH). When set to ALL_NAV (default) the navigation system will take over the control of the motor completely and never allow the motor to stop even when the throttle is low. This setting only has an effect on NAV modes which take control of the throttle when combined with MOTOR_STOP and is likely to cause a stall if fw_min_throttle_down_pitch isn't set correctly or the pitch estimation is wrong for fixed wing models when not set to ALL_NAV | | nav_position_timeout | 5 | If GPS fails wait for this much seconds before switching to emergency landing mode (0 - disable) | | nav_rth_abort_threshold | 50000 | RTH sanity checking feature will notice if distance to home is increasing during RTH and once amount of increase exceeds the threshold defined by this parameter, instead of continuing RTH machine will enter emergency landing, self-level and go down safely. Default is 500m which is safe enough for both multirotor machines and airplanes. [cm] | | nav_rth_allow_landing | ALWAYS | If set to ON drone will land as a last phase of RTH. | From e3e4270713cf0fe2e3bee0ae20a6101d67ae6fb9 Mon Sep 17 00:00:00 2001 From: breadoven <56191411+breadoven@users.noreply.github.com> Date: Wed, 6 Jan 2021 16:12:18 +0000 Subject: [PATCH 039/286] Initial changes --- src/main/io/gps.c | 2 ++ src/main/io/osd.c | 3 +++ 2 files changed, 5 insertions(+) diff --git a/src/main/io/gps.c b/src/main/io/gps.c index fd0294bebc..2dbfd77766 100755 --- a/src/main/io/gps.c +++ b/src/main/io/gps.c @@ -192,6 +192,8 @@ static void gpsResetSolution(void) { gpsSol.eph = 9999; gpsSol.epv = 9999; + gpsSol.numSat = 0; + gpsSol.hdop = 9999; gpsSol.fixType = GPS_NO_FIX; diff --git a/src/main/io/osd.c b/src/main/io/osd.c index 4d21f828ca..79d414c892 100755 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -1307,6 +1307,9 @@ static bool osdDrawSingleElement(uint8_t item) buff[1] = SYM_SAT_R; tfp_sprintf(buff + 2, "%2d", gpsSol.numSat); if (!STATE(GPS_FIX)) { + if (getHwGPSStatus() == HW_SENSOR_UNAVAILABLE || getHwGPSStatus() == HW_SENSOR_UNHEALTHY) { + strcpy(buff + 2, "X!"); + } TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); } break; From bc6438a6f6097c6981f266a7233443277001f055 Mon Sep 17 00:00:00 2001 From: breadoven <56191411+breadoven@users.noreply.github.com> Date: Sat, 16 Jan 2021 12:06:34 +0000 Subject: [PATCH 040/286] Fix setting name error --- docs/Settings.md | 2 +- src/main/fc/settings.yaml | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/docs/Settings.md b/docs/Settings.md index 68444c5142..1f91251f74 100644 --- a/docs/Settings.md +++ b/docs/Settings.md @@ -332,7 +332,7 @@ | nav_rth_altitude | 1000 | Used in EXTRA, FIXED and AT_LEAST rth alt modes [cm] (Default 1000 means 10 meters) | | nav_rth_climb_first | ON | If set to ON drone will climb to nav_rth_altitude first and head home afterwards. If set to OFF drone will head home instantly and climb on the way. | | nav_rth_climb_ignore_emerg | OFF | If set to ON, aircraft will execute initial climb regardless of position sensor (GPS) status. | -| nav_rth_fw_spiral_climb | OFF | Performs RTH loiter climb rather than a linear climb when RTH set to Climb First. Climb rate set by max_auto_climb_rate, turn rate set by nav_fw_loiter_radius. | +| nav_rth_fw_spiral_climb | OFF | Performs RTH loiter climb rather than a linear climb when RTH set to Climb First. Climb rate set by nav_auto_climb_rate, turn rate set by nav_fw_loiter_radius. | | nav_rth_home_altitude | 0 | Aircraft will climb/descend to this altitude after reaching home if landing is not enabled. Set to 0 to stay at `nav_rth_altitude` (default) [cm] | | nav_rth_tail_first | OFF | If set to ON drone will return tail-first. Obviously meaningless for airplanes. | | nav_use_fw_yaw_control | OFF | Enables or Disables the use of the heading PID controller on fixed wing. Heading PID controller is always enabled for rovers and boats | diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 4c67e9be57..a33219eaa4 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -2137,7 +2137,7 @@ groups: field: general.flags.rth_alt_control_mode table: nav_rth_alt_mode - name: nav_rth_fw_spiral_climb - description: "Performs RTH loiter climb rather than a linear climb when RTH set to Climb First. Climb rate set by max_auto_climb_rate, turn rate set by nav_fw_loiter_radius." + description: "Performs RTH loiter climb rather than a linear climb when RTH set to Climb First. Climb rate set by nav_auto_climb_rate, turn rate set by nav_fw_loiter_radius." default_value: "OFF" field: general.flags.rth_fw_spiral_climb type: bool From 55113eea6547c3d3da4c99dd09feeecea58805df Mon Sep 17 00:00:00 2001 From: breadoven <56191411+breadoven@users.noreply.github.com> Date: Tue, 19 Jan 2021 13:35:16 +0000 Subject: [PATCH 041/286] Setting Change Added Spiral setting to nav_rth_climb_first setting. --- docs/Settings.md | 3 +-- src/main/cms/cms_menu_navigation.c | 1 - src/main/fc/settings.yaml | 12 +++++------- src/main/navigation/navigation.c | 7 +++---- src/main/navigation/navigation.h | 7 ++++++- 5 files changed, 15 insertions(+), 15 deletions(-) diff --git a/docs/Settings.md b/docs/Settings.md index 1f91251f74..8e7c689039 100644 --- a/docs/Settings.md +++ b/docs/Settings.md @@ -330,9 +330,8 @@ | nav_rth_allow_landing | ALWAYS | If set to ON drone will land as a last phase of RTH. | | nav_rth_alt_mode | AT_LEAST | Configure how the aircraft will manage altitude on the way home, see Navigation modes on wiki for more details | | nav_rth_altitude | 1000 | Used in EXTRA, FIXED and AT_LEAST rth alt modes [cm] (Default 1000 means 10 meters) | -| nav_rth_climb_first | ON | If set to ON drone will climb to nav_rth_altitude first and head home afterwards. If set to OFF drone will head home instantly and climb on the way. | +| nav_rth_climb_first | ON | If set to ON or ON_FW_SPIRAL aircraft will climb to nav_rth_altitude first before turning to head home. If set to OFF aircraft will turn and head home immediately climbing on the way. For a fixed wing ON will use a linear climb, ON_FW_SPIRAL will use a loiter turning climb with climb rate set by nav_auto_climb_rate, turn rate set by nav_fw_loiter_radius (ON_FW_SPIRAL is a fixed wing setting and behaves the same as ON for a multirotor) | | nav_rth_climb_ignore_emerg | OFF | If set to ON, aircraft will execute initial climb regardless of position sensor (GPS) status. | -| nav_rth_fw_spiral_climb | OFF | Performs RTH loiter climb rather than a linear climb when RTH set to Climb First. Climb rate set by nav_auto_climb_rate, turn rate set by nav_fw_loiter_radius. | | nav_rth_home_altitude | 0 | Aircraft will climb/descend to this altitude after reaching home if landing is not enabled. Set to 0 to stay at `nav_rth_altitude` (default) [cm] | | nav_rth_tail_first | OFF | If set to ON drone will return tail-first. Obviously meaningless for airplanes. | | nav_use_fw_yaw_control | OFF | Enables or Disables the use of the heading PID controller on fixed wing. Heading PID controller is always enabled for rovers and boats | diff --git a/src/main/cms/cms_menu_navigation.c b/src/main/cms/cms_menu_navigation.c index 4b5bd24686..d17d3df290 100644 --- a/src/main/cms/cms_menu_navigation.c +++ b/src/main/cms/cms_menu_navigation.c @@ -73,7 +73,6 @@ static const CMS_Menu cmsx_menuNavSettings = { OSD_SETTING_ENTRY("RTH ALT MODE", SETTING_NAV_RTH_ALT_MODE), OSD_SETTING_ENTRY("RTH ALT", SETTING_NAV_RTH_ALTITUDE), OSD_SETTING_ENTRY("CLIMB BEFORE RTH", SETTING_NAV_RTH_CLIMB_FIRST), - OSD_SETTING_ENTRY("FW SPIRAL CLIMB", SETTING_NAV_RTH_FW_SPIRAL_CLIMB), OSD_SETTING_ENTRY("TAIL FIRST", SETTING_NAV_RTH_TAIL_FIRST), OSD_SETTING_ENTRY("LAND AFTER RTH", SETTING_NAV_RTH_ALLOW_LANDING), OSD_SETTING_ENTRY("LAND VERT SPEED", SETTING_NAV_LANDING_SPEED), diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index a33219eaa4..bcecc1e3b8 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -155,6 +155,9 @@ tables: - name: nav_overrides_motor_stop enum: navOverridesMotorStop_e values: ["OFF_ALWAYS", "OFF", "AUTO_ONLY", "ALL_NAV"] + - name: nav_rth_climb_first + enum: navRTHClimbFirst_e + values: ["OFF", "ON", "ON_FW_SPIRAL"] groups: - name: PG_GYRO_CONFIG @@ -2112,10 +2115,10 @@ groups: field: general.flags.nav_overrides_motor_stop table: nav_overrides_motor_stop - name: nav_rth_climb_first - description: "If set to ON drone will climb to nav_rth_altitude first and head home afterwards. If set to OFF drone will head home instantly and climb on the way." + description: "If set to ON or ON_FW_SPIRAL aircraft will climb to nav_rth_altitude first before turning to head home. If set to OFF aircraft will turn and head home immediately climbing on the way. For a fixed wing ON will use a linear climb, ON_FW_SPIRAL will use a loiter turning climb with climb rate set by nav_auto_climb_rate, turn rate set by nav_fw_loiter_radius (ON_FW_SPIRAL is a fixed wing setting and behaves the same as ON for a multirotor)" default_value: "ON" field: general.flags.rth_climb_first - type: bool + table: nav_rth_climb_first - name: nav_rth_climb_ignore_emerg description: "If set to ON, aircraft will execute initial climb regardless of position sensor (GPS) status." default_value: "OFF" @@ -2136,11 +2139,6 @@ groups: default_value: "AT_LEAST" field: general.flags.rth_alt_control_mode table: nav_rth_alt_mode - - name: nav_rth_fw_spiral_climb - description: "Performs RTH loiter climb rather than a linear climb when RTH set to Climb First. Climb rate set by nav_auto_climb_rate, turn rate set by nav_fw_loiter_radius." - default_value: "OFF" - field: general.flags.rth_fw_spiral_climb - type: bool - name: nav_rth_abort_threshold description: "RTH sanity checking feature will notice if distance to home is increasing during RTH and once amount of increase exceeds the threshold defined by this parameter, instead of continuing RTH machine will enter emergency landing, self-level and go down safely. Default is 500m which is safe enough for both multirotor machines and airplanes. [cm]" default_value: "50000" diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c index 69bf9dc03e..e6086c4064 100755 --- a/src/main/navigation/navigation.c +++ b/src/main/navigation/navigation.c @@ -106,7 +106,6 @@ PG_RESET_TEMPLATE(navConfig_t, navConfig, .rth_tail_first = 0, .disarm_on_landing = 0, .rth_allow_landing = NAV_RTH_ALLOW_LANDING_ALWAYS, - .rth_fw_spiral_climb = 0, // Loiter climb for FW RTH .nav_overrides_motor_stop = NOMS_ALL_NAV, }, @@ -1152,7 +1151,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_INITIALIZE(navigati if (STATE(FIXED_WING_LEGACY)) { // Airplane - climbout before heading home - if (navConfig()->general.flags.rth_fw_spiral_climb) { + if (navConfig()->general.flags.rth_climb_first == ON_FW_SPIRAL) { // Spiral climb centered at xy of RTH activation calculateInitialHoldPosition(&targetHoldPos); } else { @@ -1201,7 +1200,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_CLIMB_TO_SAFE_ALT(n const float rthAltitudeMargin = MAX(FW_RTH_CLIMB_MARGIN_MIN_CM, (rthClimbMarginPercent/100.0) * fabsf(posControl.rthState.rthInitialAltitude - posControl.rthState.homePosition.pos.z)); // If we reached desired initial RTH altitude or we don't want to climb first - if (((navGetCurrentActualPositionAndVelocity()->pos.z - posControl.rthState.rthInitialAltitude) > -rthAltitudeMargin) || (!navConfig()->general.flags.rth_climb_first)) { + if (((navGetCurrentActualPositionAndVelocity()->pos.z - posControl.rthState.rthInitialAltitude) > -rthAltitudeMargin) || (navConfig()->general.flags.rth_climb_first == OFF)) { // Delayed initialization for RTH sanity check on airplanes - allow to finish climb first as it can take some distance if (STATE(FIXED_WING_LEGACY)) { @@ -1232,7 +1231,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_CLIMB_TO_SAFE_ALT(n // Climb to safe altitude and turn to correct direction if (STATE(FIXED_WING_LEGACY)) { - if (navConfig()->general.flags.rth_fw_spiral_climb) { + if (navConfig()->general.flags.rth_climb_first == ON_FW_SPIRAL) { float altitudeChangeDirection = (tmpHomePos->z += FW_RTH_CLIMB_OVERSHOOT_CM) > navGetCurrentActualPositionAndVelocity()->pos.z ? 1 : -1; updateClimbRateToAltitudeController(altitudeChangeDirection * navConfig()->general.max_auto_climb_rate, ROC_TO_ALT_NORMAL); } else { diff --git a/src/main/navigation/navigation.h b/src/main/navigation/navigation.h index 445d2d598d..29914f5877 100755 --- a/src/main/navigation/navigation.h +++ b/src/main/navigation/navigation.h @@ -131,6 +131,12 @@ typedef enum { NOMS_ALL_NAV } navOverridesMotorStop_e; +typedef enum { + OFF, + ON, + ON_FW_SPIRAL, +} navRTHClimbFirst_e; + typedef struct positionEstimationConfig_s { uint8_t automatic_mag_declination; uint8_t reset_altitude_type; // from nav_reset_type_e @@ -183,7 +189,6 @@ typedef struct navConfig_s { uint8_t rth_allow_landing; // Enable landing as last stage of RTH. Use constants in navRTHAllowLanding_e. uint8_t rth_climb_ignore_emerg; // Option to ignore GPS loss on initial climb stage of RTH uint8_t nav_overrides_motor_stop; // Autonomous modes override motor_stop setting and user command to stop motor - uint8_t rth_fw_spiral_climb; // Enable RTH spiral climb for FW when "climb first" selected } flags; uint8_t pos_failure_timeout; // Time to wait before switching to emergency landing (0 - disable) From 337056a8ebdc1c89adb0e93065ed66badb9c6356 Mon Sep 17 00:00:00 2001 From: breadoven <56191411+breadoven@users.noreply.github.com> Date: Sat, 23 Jan 2021 09:28:15 +0000 Subject: [PATCH 042/286] Update cms_menu_navigation.c Change Climb First menu to fit longer Spiral setting option --- src/main/cms/cms_menu_navigation.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/cms/cms_menu_navigation.c b/src/main/cms/cms_menu_navigation.c index d17d3df290..9274f3315a 100644 --- a/src/main/cms/cms_menu_navigation.c +++ b/src/main/cms/cms_menu_navigation.c @@ -72,7 +72,7 @@ static const CMS_Menu cmsx_menuNavSettings = { OSD_SETTING_ENTRY("RTH ALT MODE", SETTING_NAV_RTH_ALT_MODE), OSD_SETTING_ENTRY("RTH ALT", SETTING_NAV_RTH_ALTITUDE), - OSD_SETTING_ENTRY("CLIMB BEFORE RTH", SETTING_NAV_RTH_CLIMB_FIRST), + OSD_SETTING_ENTRY("CLIMB FIRST", SETTING_NAV_RTH_CLIMB_FIRST), OSD_SETTING_ENTRY("TAIL FIRST", SETTING_NAV_RTH_TAIL_FIRST), OSD_SETTING_ENTRY("LAND AFTER RTH", SETTING_NAV_RTH_ALLOW_LANDING), OSD_SETTING_ENTRY("LAND VERT SPEED", SETTING_NAV_LANDING_SPEED), From 4b12ca3326bbfd3feb7ed3f014cdf9c558a95371 Mon Sep 17 00:00:00 2001 From: Marcin Galczynski Date: Fri, 5 Feb 2021 01:22:59 +0100 Subject: [PATCH 043/286] Add support for VL53L1X rangefinder --- src/main/CMakeLists.txt | 2 + src/main/drivers/bus.h | 1 + .../drivers/rangefinder/rangefinder_vl53l1x.c | 1692 +++++++++++++++++ .../drivers/rangefinder/rangefinder_vl53l1x.h | 29 + src/main/fc/settings.yaml | 2 +- src/main/sensors/rangefinder.c | 10 + src/main/sensors/rangefinder.h | 1 + src/main/target/common.h | 1 + src/main/target/common_hardware.c | 9 + 9 files changed, 1746 insertions(+), 1 deletion(-) create mode 100644 src/main/drivers/rangefinder/rangefinder_vl53l1x.c create mode 100644 src/main/drivers/rangefinder/rangefinder_vl53l1x.h diff --git a/src/main/CMakeLists.txt b/src/main/CMakeLists.txt index 364356342f..be87444ed0 100644 --- a/src/main/CMakeLists.txt +++ b/src/main/CMakeLists.txt @@ -224,6 +224,8 @@ main_sources(COMMON_SRC drivers/rangefinder/rangefinder_srf10.h drivers/rangefinder/rangefinder_vl53l0x.c drivers/rangefinder/rangefinder_vl53l0x.h + drivers/rangefinder/rangefinder_vl53l1x.c + drivers/rangefinder/rangefinder_vl53l1x.h drivers/rangefinder/rangefinder_virtual.c drivers/rangefinder/rangefinder_virtual.h diff --git a/src/main/drivers/bus.h b/src/main/drivers/bus.h index 2d7a51fc97..cad41d6440 100755 --- a/src/main/drivers/bus.h +++ b/src/main/drivers/bus.h @@ -137,6 +137,7 @@ typedef enum { DEVHW_SRF10, DEVHW_HCSR04_I2C, // DIY-style adapter DEVHW_VL53L0X, + DEVHW_VL53L1X, /* Other hardware */ DEVHW_MS4525, // Pitot meter diff --git a/src/main/drivers/rangefinder/rangefinder_vl53l1x.c b/src/main/drivers/rangefinder/rangefinder_vl53l1x.c new file mode 100644 index 0000000000..01d6e11955 --- /dev/null +++ b/src/main/drivers/rangefinder/rangefinder_vl53l1x.c @@ -0,0 +1,1692 @@ +/* + * This file is part of INAV. + * + * This Source Code Form is subject to the terms of the Mozilla Public + * License, v. 2.0. If a copy of the MPL was not distributed with this file, + * You can obtain one at http://mozilla.org/MPL/2.0/. + * + * Alternatively, the contents of this file may be used under the terms + * of the GNU General Public License Version 3, as described below: + * + * This file is free software: you may copy, redistribute and/or modify + * it under the terms of the GNU General Public License as published by the + * Free Software Foundation, either version 3 of the License, or (at your + * option) any later version. + * + * This file is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General + * Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see http://www.gnu.org/licenses/. + * + * ================================================================= + * + * Most of the functionality of this library is based on the VL53L1X + * API provided by ST (STSW-IMG009). + * + * The following applies to source code reproduced or derived from + * the API: + * + * ----------------------------------------------------------------- + * + * Copyright (c) 2017, STMicroelectronics - All Rights Reserved + * + * This file : part of VL53L1 Core and : dual licensed, + * either 'STMicroelectronics + * Proprietary license' + * or 'BSD 3-clause "New" or "Revised" License' , at your option. + * + * ******************************************************************************* + * + * 'STMicroelectronics Proprietary license' + * + * ******************************************************************************* + * + * License terms: STMicroelectronics Proprietary in accordance with licensing + * terms at www.st.com/sla0081 + * + * STMicroelectronics confidential + * Reproduction and Communication of this document : strictly prohibited unless + * specifically authorized in writing by STMicroelectronics. + * + * + * ******************************************************************************* + * + * Alternatively, VL53L1 Core may be distributed under the terms of + * 'BSD 3-clause "New" or "Revised" License', in which case the following + * provisions apply instead of the ones mentioned above : + * + * ******************************************************************************* + * + * License terms: BSD 3-clause "New" or "Revised" License. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * 1. Redistributions of source code must retain the above copyright notice, this + * list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * 3. Neither the name of the copyright holder nor the names of its contributors + * may be used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE + * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + * SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER + * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, + * OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + */ + + +#include +#include +#include + +#include "platform.h" + +#if defined(USE_RANGEFINDER) && defined(USE_RANGEFINDER_VL53L1X) + +#include "build/build_config.h" + +#include "drivers/time.h" +#include "drivers/bus_i2c.h" + +#include "drivers/rangefinder/rangefinder.h" +#include "drivers/rangefinder/rangefinder_vl53l1x.h" + +#include "build/debug.h" + +#define VL53L1X_MAX_RANGE_CM (300) +#define VL53L1X_DETECTION_CONE_DECIDEGREES (270) + +#define VL53L1X_IMPLEMENTATION_VER_MAJOR 3 +#define VL53L1X_IMPLEMENTATION_VER_MINOR 4 +#define VL53L1X_IMPLEMENTATION_VER_SUB 0 +#define VL53L1X_IMPLEMENTATION_VER_REVISION 0000 + +typedef int8_t VL53L1X_ERROR; + +#define SOFT_RESET 0x0000 +#define VL53L1_I2C_SLAVE__DEVICE_ADDRESS 0x0001 +#define VL53L1_VHV_CONFIG__TIMEOUT_MACROP_LOOP_BOUND 0x0008 +#define ALGO__CROSSTALK_COMPENSATION_PLANE_OFFSET_KCPS 0x0016 +#define ALGO__CROSSTALK_COMPENSATION_X_PLANE_GRADIENT_KCPS 0x0018 +#define ALGO__CROSSTALK_COMPENSATION_Y_PLANE_GRADIENT_KCPS 0x001A +#define ALGO__PART_TO_PART_RANGE_OFFSET_MM 0x001E +#define MM_CONFIG__INNER_OFFSET_MM 0x0020 +#define MM_CONFIG__OUTER_OFFSET_MM 0x0022 +#define GPIO_HV_MUX__CTRL 0x0030 +#define GPIO__TIO_HV_STATUS 0x0031 +#define SYSTEM__INTERRUPT_CONFIG_GPIO 0x0046 +#define PHASECAL_CONFIG__TIMEOUT_MACROP 0x004B +#define RANGE_CONFIG__TIMEOUT_MACROP_A_HI 0x005E +#define RANGE_CONFIG__VCSEL_PERIOD_A 0x0060 +#define RANGE_CONFIG__VCSEL_PERIOD_B 0x0063 +#define RANGE_CONFIG__TIMEOUT_MACROP_B_HI 0x0061 +#define RANGE_CONFIG__TIMEOUT_MACROP_B_LO 0x0062 +#define RANGE_CONFIG__SIGMA_THRESH 0x0064 +#define RANGE_CONFIG__MIN_COUNT_RATE_RTN_LIMIT_MCPS 0x0066 +#define RANGE_CONFIG__VALID_PHASE_HIGH 0x0069 +#define VL53L1_SYSTEM__INTERMEASUREMENT_PERIOD 0x006C +#define SYSTEM__THRESH_HIGH 0x0072 +#define SYSTEM__THRESH_LOW 0x0074 +#define SD_CONFIG__WOI_SD0 0x0078 +#define SD_CONFIG__INITIAL_PHASE_SD0 0x007A +#define ROI_CONFIG__USER_ROI_CENTRE_SPAD 0x007F +#define ROI_CONFIG__USER_ROI_REQUESTED_GLOBAL_XY_SIZE 0x0080 +#define SYSTEM__SEQUENCE_CONFIG 0x0081 +#define VL53L1_SYSTEM__GROUPED_PARAMETER_HOLD 0x0082 +#define SYSTEM__INTERRUPT_CLEAR 0x0086 +#define SYSTEM__MODE_START 0x0087 +#define VL53L1_RESULT__RANGE_STATUS 0x0089 +#define VL53L1_RESULT__DSS_ACTUAL_EFFECTIVE_SPADS_SD0 0x008C +#define RESULT__AMBIENT_COUNT_RATE_MCPS_SD 0x0090 +#define VL53L1_RESULT__FINAL_CROSSTALK_CORRECTED_RANGE_MM_SD0 0x0096 +#define VL53L1_RESULT__PEAK_SIGNAL_COUNT_RATE_CROSSTALK_CORRECTED_MCPS_SD0 0x0098 +#define VL53L1_RESULT__OSC_CALIBRATE_VAL 0x00DE +#define VL53L1_FIRMWARE__SYSTEM_STATUS 0x00E5 +#define VL53L1_IDENTIFICATION__MODEL_ID 0x010F +#define VL53L1_ROI_CONFIG__MODE_ROI_CENTRE_SPAD 0x013E + +#define ALGO__PART_TO_PART_RANGE_OFFSET_MM 0x001E +#define MM_CONFIG__INNER_OFFSET_MM 0x0020 +#define MM_CONFIG__OUTER_OFFSET_MM 0x0022 + + + +#define VL53L1_ERROR_NONE ((VL53L1X_ERROR) 0) +#define VL53L1_ERROR_CALIBRATION_WARNING ((VL53L1X_ERROR) - 1) + /*!< Warning invalid calibration data may be in used + \a VL53L1_InitData() + \a VL53L1_GetOffsetCalibrationData + \a VL53L1_SetOffsetCalibrationData */ +#define VL53L1_ERROR_MIN_CLIPPED ((VL53L1X_ERROR) - 2) + /*!< Warning parameter passed was clipped to min before to be applied */ + +#define VL53L1_ERROR_UNDEFINED ((VL53L1X_ERROR) - 3) + /*!< Unqualified error */ +#define VL53L1_ERROR_INVALID_PARAMS ((VL53L1X_ERROR) - 4) + /*!< Parameter passed is invalid or out of range */ +#define VL53L1_ERROR_NOT_SUPPORTED ((VL53L1X_ERROR) - 5) + /*!< Function is not supported in current mode or configuration */ +#define VL53L1_ERROR_RANGE_ERROR ((VL53L1X_ERROR) - 6) + /*!< Device report a ranging error interrupt status */ +#define VL53L1_ERROR_TIME_OUT ((VL53L1X_ERROR) - 7) + /*!< Aborted due to time out */ +#define VL53L1_ERROR_MODE_NOT_SUPPORTED ((VL53L1X_ERROR) - 8) + /*!< Asked mode is not supported by the device */ +#define VL53L1_ERROR_BUFFER_TOO_SMALL ((VL53L1X_ERROR) - 9) + /*!< ... */ +#define VL53L1_ERROR_COMMS_BUFFER_TOO_SMALL ((VL53L1X_ERROR) - 10) + /*!< Supplied buffer is larger than I2C supports */ +#define VL53L1_ERROR_GPIO_NOT_EXISTING ((VL53L1X_ERROR) - 11) + /*!< User tried to setup a non-existing GPIO pin */ +#define VL53L1_ERROR_GPIO_FUNCTIONALITY_NOT_SUPPORTED ((VL53L1X_ERROR) - 12) + /*!< unsupported GPIO functionality */ +#define VL53L1_ERROR_CONTROL_INTERFACE ((VL53L1X_ERROR) - 13) + /*!< error reported from IO functions */ +#define VL53L1_ERROR_INVALID_COMMAND ((VL53L1X_ERROR) - 14) + /*!< The command is not allowed in the current device state + * (power down) */ +#define VL53L1_ERROR_DIVISION_BY_ZERO ((VL53L1X_ERROR) - 15) + /*!< In the function a division by zero occurs */ +#define VL53L1_ERROR_REF_SPAD_INIT ((VL53L1X_ERROR) - 16) + /*!< Error during reference SPAD initialization */ +#define VL53L1_ERROR_GPH_SYNC_CHECK_FAIL ((VL53L1X_ERROR) - 17) + /*!< GPH sync interrupt check fail - API out of sync with device*/ +#define VL53L1_ERROR_STREAM_COUNT_CHECK_FAIL ((VL53L1X_ERROR) - 18) + /*!< Stream count check fail - API out of sync with device */ +#define VL53L1_ERROR_GPH_ID_CHECK_FAIL ((VL53L1X_ERROR) - 19) + /*!< GPH ID check fail - API out of sync with device */ +#define VL53L1_ERROR_ZONE_STREAM_COUNT_CHECK_FAIL ((VL53L1X_ERROR) - 20) + /*!< Zone dynamic config stream count check failed - API out of sync */ +#define VL53L1_ERROR_ZONE_GPH_ID_CHECK_FAIL ((VL53L1X_ERROR) - 21) + /*!< Zone dynamic config GPH ID check failed - API out of sync */ + +#define VL53L1_ERROR_XTALK_EXTRACTION_NO_SAMPLE_FAIL ((VL53L1X_ERROR) - 22) + /*!< Thrown when run_xtalk_extraction fn has 0 succesful samples + * when using the full array to sample the xtalk. In this case there is + * not enough information to generate new Xtalk parm info. The function + * will exit and leave the current xtalk parameters unaltered */ +#define VL53L1_ERROR_XTALK_EXTRACTION_SIGMA_LIMIT_FAIL ((VL53L1X_ERROR) - 23) + /*!< Thrown when run_xtalk_extraction fn has found that the + * avg sigma estimate of the full array xtalk sample is > than the + * maximal limit allowed. In this case the xtalk sample is too noisy for + * measurement. The function will exit and leave the current xtalk parameters + * unaltered. */ + + +#define VL53L1_ERROR_OFFSET_CAL_NO_SAMPLE_FAIL ((VL53L1X_ERROR) - 24) + /*!< Thrown if there one of stages has no valid offset calibration + * samples. A fatal error calibration not valid */ +#define VL53L1_ERROR_OFFSET_CAL_NO_SPADS_ENABLED_FAIL ((VL53L1X_ERROR) - 25) + /*!< Thrown if there one of stages has zero effective SPADS + * Traps the case when MM1 SPADs is zero. + * A fatal error calibration not valid */ +#define VL53L1_ERROR_ZONE_CAL_NO_SAMPLE_FAIL ((VL53L1X_ERROR) - 26) + /*!< Thrown if then some of the zones have no valid samples + * A fatal error calibration not valid */ + +#define VL53L1_ERROR_TUNING_PARM_KEY_MISMATCH ((VL53L1X_ERROR) - 27) + /*!< Thrown if the tuning file key table version does not match with + * expected value. The driver expects the key table version to match + * the compiled default version number in the define + * #VL53L1_TUNINGPARM_KEY_TABLE_VERSION_DEFAULT + * */ + +#define VL53L1_WARNING_REF_SPAD_CHAR_NOT_ENOUGH_SPADS ((VL53L1X_ERROR) - 28) + /*!< Thrown if there are less than 5 good SPADs are available. */ +#define VL53L1_WARNING_REF_SPAD_CHAR_RATE_TOO_HIGH ((VL53L1X_ERROR) - 29) + /*!< Thrown if the final reference rate is greater than + the upper reference rate limit - default is 40 Mcps. + Implies a minimum Q3 (x10) SPAD (5) selected */ +#define VL53L1_WARNING_REF_SPAD_CHAR_RATE_TOO_LOW ((VL53L1X_ERROR) - 30) + /*!< Thrown if the final reference rate is less than + the lower reference rate limit - default is 10 Mcps. + Implies maximum Q1 (x1) SPADs selected */ + + +#define VL53L1_WARNING_OFFSET_CAL_MISSING_SAMPLES ((VL53L1X_ERROR) - 31) + /*!< Thrown if there is less than the requested number of + * valid samples. */ +#define VL53L1_WARNING_OFFSET_CAL_SIGMA_TOO_HIGH ((VL53L1X_ERROR) - 32) + /*!< Thrown if the offset calibration range sigma estimate is greater + * than 8.0 mm. This is the recommended min value to yield a stable + * offset measurement */ +#define VL53L1_WARNING_OFFSET_CAL_RATE_TOO_HIGH ((VL53L1X_ERROR) - 33) + /*!< Thrown when VL53L1_run_offset_calibration() peak rate is greater + than that 50.0Mcps. This is the recommended max rate to avoid + pile-up influencing the offset measurement */ +#define VL53L1_WARNING_OFFSET_CAL_SPAD_COUNT_TOO_LOW ((VL53L1X_ERROR) - 34) + /*!< Thrown when VL53L1_run_offset_calibration() when one of stages + range has less that 5.0 effective SPADS. This is the recommended + min value to yield a stable offset */ + + +#define VL53L1_WARNING_ZONE_CAL_MISSING_SAMPLES ((VL53L1X_ERROR) - 35) + /*!< Thrown if one of more of the zones have less than + the requested number of valid samples */ +#define VL53L1_WARNING_ZONE_CAL_SIGMA_TOO_HIGH ((VL53L1X_ERROR) - 36) + /*!< Thrown if one or more zones have sigma estimate value greater + * than 8.0 mm. This is the recommended min value to yield a stable + * offset measurement */ +#define VL53L1_WARNING_ZONE_CAL_RATE_TOO_HIGH ((VL53L1X_ERROR) - 37) + /*!< Thrown if one of more zones have peak rate higher than + that 50.0Mcps. This is the recommended max rate to avoid + pile-up influencing the offset measurement */ + + +#define VL53L1_WARNING_XTALK_MISSING_SAMPLES ((VL53L1X_ERROR) - 38) + /*!< Thrown to notify that some of the xtalk samples did not yield + * valid ranging pulse data while attempting to measure + * the xtalk signal in vl53l1_run_xtalk_extract(). This can signify any of + * the zones are missing samples, for further debug information the + * xtalk_results struct should be referred to. This warning is for + * notification only, the xtalk pulse and shape have still been generated + */ +#define VL53L1_WARNING_XTALK_NO_SAMPLES_FOR_GRADIENT ((VL53L1X_ERROR) - 39) + /*!< Thrown to notify that some of teh xtalk samples used for gradient + * generation did not yield valid ranging pulse data while attempting to + * measure the xtalk signal in vl53l1_run_xtalk_extract(). This can signify + * that any one of the zones 0-3 yielded no successful samples. The + * xtalk_results struct should be referred to for further debug info. + * This warning is for notification only, the xtalk pulse and shape + * have still been generated. + */ +#define VL53L1_WARNING_XTALK_SIGMA_LIMIT_FOR_GRADIENT ((VL53L1X_ERROR) - 40) +/*!< Thrown to notify that some of the xtalk samples used for gradient + * generation did not pass the sigma limit check while attempting to + * measure the xtalk signal in vl53l1_run_xtalk_extract(). This can signify + * that any one of the zones 0-3 yielded an avg sigma_mm value > the limit. + * The xtalk_results struct should be referred to for further debug info. + * This warning is for notification only, the xtalk pulse and shape + * have still been generated. + */ + +#define VL53L1_ERROR_NOT_IMPLEMENTED ((VL53L1X_ERROR) - 41) + /*!< Tells requested functionality has not been implemented yet or + * not compatible with the device */ +#define VL53L1_ERROR_PLATFORM_SPECIFIC_START ((VL53L1X_ERROR) - 60) + /*!< Tells the starting code for platform */ +/** @} VL53L1_define_Error_group */ + +/**************************************** + * PRIVATE define do not edit + ****************************************/ + +/** + * @brief defines SW Version + */ +typedef struct { + uint8_t major; /*!< major number */ + uint8_t minor; /*!< minor number */ + uint8_t build; /*!< build number */ + uint32_t revision; /*!< revision number */ +} VL53L1X_Version_t; + +/** + * @brief defines packed reading results type + */ +typedef struct { + uint8_t Status; /*!< ResultStatus */ + uint16_t Distance; /*!< ResultDistance */ + uint16_t Ambient; /*!< ResultAmbient */ + uint16_t SigPerSPAD;/*!< ResultSignalPerSPAD */ + uint16_t NumSPADs; /*!< ResultNumSPADs */ +} VL53L1X_Result_t; + +/** + * @brief This function returns the SW driver version + */ +VL53L1X_ERROR VL53L1X_GetSWVersion(VL53L1X_Version_t *pVersion); + +/** + * @brief This function sets the sensor I2C address used in case multiple devices application, default address 0x52 + */ +VL53L1X_ERROR VL53L1X_SetI2CAddress(busDevice_t * dev, uint8_t new_address); + +/** + * @brief This function loads the 135 bytes default values to initialize the sensor. + * @param dev Device address + * @return 0:success, != 0:failed + */ +VL53L1X_ERROR VL53L1X_SensorInit(busDevice_t * dev); + +/** + * @brief This function clears the interrupt, to be called after a ranging data reading + * to arm the interrupt for the next data ready event. + */ +VL53L1X_ERROR VL53L1X_ClearInterrupt(busDevice_t * dev); + +/** + * @brief This function programs the interrupt polarity\n + * 1=active high (default), 0=active low + */ +VL53L1X_ERROR VL53L1X_SetInterruptPolarity(busDevice_t * dev, uint8_t IntPol); + +/** + * @brief This function returns the current interrupt polarity\n + * 1=active high (default), 0=active low + */ +VL53L1X_ERROR VL53L1X_GetInterruptPolarity(busDevice_t * dev, uint8_t *pIntPol); + +/** + * @brief This function starts the ranging distance operation\n + * The ranging operation is continuous. The clear interrupt has to be done after each get data to allow the interrupt to raise when the next data is ready\n + * 1=active high (default), 0=active low, use SetInterruptPolarity() to change the interrupt polarity if required. + */ +VL53L1X_ERROR VL53L1X_StartRanging(busDevice_t * dev); + +/** + * @brief This function stops the ranging. + */ +VL53L1X_ERROR VL53L1X_StopRanging(busDevice_t * dev); + +/** + * @brief This function checks if the new ranging data is available by polling the dedicated register. + * @param : isDataReady==0 -> not ready; isDataReady==1 -> ready + */ +VL53L1X_ERROR VL53L1X_CheckForDataReady(busDevice_t * dev, uint8_t *isDataReady); + +/** + * @brief This function programs the timing budget in ms. + * Predefined values = 15, 20, 33, 50, 100(default), 200, 500. + */ +VL53L1X_ERROR VL53L1X_SetTimingBudgetInMs(busDevice_t * dev, uint16_t TimingBudgetInMs); + +/** + * @brief This function returns the current timing budget in ms. + */ +VL53L1X_ERROR VL53L1X_GetTimingBudgetInMs(busDevice_t * dev, uint16_t *pTimingBudgetInMs); + +/** + * @brief This function programs the distance mode (1=short, 2=long(default)). + * Short mode max distance is limited to 1.3 m but better ambient immunity.\n + * Long mode can range up to 4 m in the dark with 200 ms timing budget. + */ +VL53L1X_ERROR VL53L1X_SetDistanceMode(busDevice_t * dev, uint16_t DistanceMode); + +/** + * @brief This function returns the current distance mode (1=short, 2=long). + */ +VL53L1X_ERROR VL53L1X_GetDistanceMode(busDevice_t * dev, uint16_t *pDistanceMode); + +/** + * @brief This function programs the Intermeasurement period in ms\n + * Intermeasurement period must be >/= timing budget. This condition is not checked by the API, + * the customer has the duty to check the condition. Default = 100 ms + */ +VL53L1X_ERROR VL53L1X_SetInterMeasurementInMs(busDevice_t * dev, + uint32_t InterMeasurementInMs); + +/** + * @brief This function returns the Intermeasurement period in ms. + */ +VL53L1X_ERROR VL53L1X_GetInterMeasurementInMs(busDevice_t * dev, uint16_t * pIM); + +/** + * @brief This function returns the boot state of the device (1:booted, 0:not booted) + */ +VL53L1X_ERROR VL53L1X_BootState(busDevice_t * dev, uint8_t *state); + +/** + * @brief This function returns the sensor id, sensor Id must be 0xEEAC + */ +VL53L1X_ERROR VL53L1X_GetSensorId(busDevice_t * dev, uint16_t *id); + +/** + * @brief This function returns the distance measured by the sensor in mm + */ +VL53L1X_ERROR VL53L1X_GetDistance(busDevice_t * dev, uint16_t *distance); + +/** + * @brief This function returns the returned signal per SPAD in kcps/SPAD. + * With kcps stands for Kilo Count Per Second + */ +VL53L1X_ERROR VL53L1X_GetSignalPerSpad(busDevice_t * dev, uint16_t *signalPerSp); + +/** + * @brief This function returns the ambient per SPAD in kcps/SPAD + */ +VL53L1X_ERROR VL53L1X_GetAmbientPerSpad(busDevice_t * dev, uint16_t *amb); + +/** + * @brief This function returns the returned signal in kcps. + */ +VL53L1X_ERROR VL53L1X_GetSignalRate(busDevice_t * dev, uint16_t *signalRate); + +/** + * @brief This function returns the current number of enabled SPADs + */ +VL53L1X_ERROR VL53L1X_GetSpadNb(busDevice_t * dev, uint16_t *spNb); + +/** + * @brief This function returns the ambient rate in kcps + */ +VL53L1X_ERROR VL53L1X_GetAmbientRate(busDevice_t * dev, uint16_t *ambRate); + +/** + * @brief This function returns the ranging status error \n + * (0:no error, 1:sigma failed, 2:signal failed, ..., 7:wrap-around) + */ +VL53L1X_ERROR VL53L1X_GetRangeStatus(busDevice_t * dev, uint8_t *rangeStatus); + +/** + * @brief This function returns measurements and the range status in a single read access + */ +VL53L1X_ERROR VL53L1X_GetResult(busDevice_t * dev, VL53L1X_Result_t *pResult); + +/** + * @brief This function programs the offset correction in mm + * @param OffsetValue:the offset correction value to program in mm + */ +VL53L1X_ERROR VL53L1X_SetOffset(busDevice_t * dev, int16_t OffsetValue); + +/** + * @brief This function returns the programmed offset correction value in mm + */ +VL53L1X_ERROR VL53L1X_GetOffset(busDevice_t * dev, int16_t *Offset); + +/** + * @brief This function programs the xtalk correction value in cps (Count Per Second).\n + * This is the number of photons reflected back from the cover glass in cps. + */ +VL53L1X_ERROR VL53L1X_SetXtalk(busDevice_t * dev, uint16_t XtalkValue); + +/** + * @brief This function returns the current programmed xtalk correction value in cps + */ +VL53L1X_ERROR VL53L1X_GetXtalk(busDevice_t * dev, uint16_t *Xtalk); + +/** + * @brief This function programs the threshold detection mode\n + * Example:\n + * VL53L1X_SetDistanceThreshold(dev,100,300,0,1): Below 100 \n + * VL53L1X_SetDistanceThreshold(dev,100,300,1,1): Above 300 \n + * VL53L1X_SetDistanceThreshold(dev,100,300,2,1): Out of window \n + * VL53L1X_SetDistanceThreshold(dev,100,300,3,1): In window \n + * @param dev : device address + * @param ThreshLow(in mm) : the threshold under which one the device raises an interrupt if Window = 0 + * @param ThreshHigh(in mm) : the threshold above which one the device raises an interrupt if Window = 1 + * @param Window detection mode : 0=below, 1=above, 2=out, 3=in + * @param IntOnNoTarget = 0 (No longer used - just use 0) + */ +VL53L1X_ERROR VL53L1X_SetDistanceThreshold(busDevice_t * dev, uint16_t ThreshLow, + uint16_t ThreshHigh, uint8_t Window, + uint8_t IntOnNoTarget); + +/** + * @brief This function returns the window detection mode (0=below; 1=above; 2=out; 3=in) + */ +VL53L1X_ERROR VL53L1X_GetDistanceThresholdWindow(busDevice_t * dev, uint16_t *window); + +/** + * @brief This function returns the low threshold in mm + */ +VL53L1X_ERROR VL53L1X_GetDistanceThresholdLow(busDevice_t * dev, uint16_t *low); + +/** + * @brief This function returns the high threshold in mm + */ +VL53L1X_ERROR VL53L1X_GetDistanceThresholdHigh(busDevice_t * dev, uint16_t *high); + +/** + * @brief This function programs the ROI (Region of Interest)\n + * The ROI position is centered, only the ROI size can be reprogrammed.\n + * The smallest acceptable ROI size = 4\n + * @param X:ROI Width; Y=ROI Height + */ +VL53L1X_ERROR VL53L1X_SetROI(busDevice_t * dev, uint16_t X, uint16_t Y); + +/** + *@brief This function returns width X and height Y + */ +VL53L1X_ERROR VL53L1X_GetROI_XY(busDevice_t * dev, uint16_t *ROI_X, uint16_t *ROI_Y); + +/** + *@brief This function programs the new user ROI center, please to be aware that there is no check in this function. + *if the ROI center vs ROI size is out of border the ranging function return error #13 + */ +VL53L1X_ERROR VL53L1X_SetROICenter(busDevice_t * dev, uint8_t ROICenter); + +/** + *@brief This function returns the current user ROI center + */ +VL53L1X_ERROR VL53L1X_GetROICenter(busDevice_t * dev, uint8_t *ROICenter); + +/** + * @brief This function programs a new signal threshold in kcps (default=1024 kcps\n + */ +VL53L1X_ERROR VL53L1X_SetSignalThreshold(busDevice_t * dev, uint16_t signal); + +/** + * @brief This function returns the current signal threshold in kcps + */ +VL53L1X_ERROR VL53L1X_GetSignalThreshold(busDevice_t * dev, uint16_t *signal); + +/** + * @brief This function programs a new sigma threshold in mm (default=15 mm) + */ +VL53L1X_ERROR VL53L1X_SetSigmaThreshold(busDevice_t * dev, uint16_t sigma); + +/** + * @brief This function returns the current sigma threshold in mm + */ +VL53L1X_ERROR VL53L1X_GetSigmaThreshold(busDevice_t * dev, uint16_t *signal); + +/** + * @brief This function performs the temperature calibration. + * It is recommended to call this function any time the temperature might have changed by more than 8 deg C + * without sensor ranging activity for an extended period. + */ +VL53L1X_ERROR VL53L1X_StartTemperatureUpdate(busDevice_t * dev); + +/** + * @brief This function performs the offset calibration.\n + * The function returns the offset value found and programs the offset compensation into the device. + * @param TargetDistInMm target distance in mm, ST recommended 100 mm + * Target reflectance = grey17% + * @return 0:success, !=0: failed + * @return offset pointer contains the offset found in mm + */ +int8_t VL53L1X_CalibrateOffset(busDevice_t * dev, uint16_t TargetDistInMm, int16_t *offset); + +/** + * @brief This function performs the xtalk calibration.\n + * The function returns the xtalk value found and programs the xtalk compensation to the device + * @param TargetDistInMm target distance in mm\n + * The target distance : the distance where the sensor start to "under range"\n + * due to the influence of the photons reflected back from the cover glass becoming strong\n + * It's also called inflection point\n + * Target reflectance = grey 17% + * @return 0: success, !=0: failed + * @return xtalk pointer contains the xtalk value found in cps (number of photons in count per second) + */ +int8_t VL53L1X_CalibrateXtalk(busDevice_t * dev, uint16_t TargetDistInMm, uint16_t *xtalk); + + + +const uint8_t VL51L1X_DEFAULT_CONFIGURATION[] = { +0x00, /* 0x2d : set bit 2 and 5 to 1 for fast plus mode (1MHz I2C), else don't touch */ +0x00, /* 0x2e : bit 0 if I2C pulled up at 1.8V, else set bit 0 to 1 (pull up at AVDD) */ +0x00, /* 0x2f : bit 0 if GPIO pulled up at 1.8V, else set bit 0 to 1 (pull up at AVDD) */ +0x01, /* 0x30 : set bit 4 to 0 for active high interrupt and 1 for active low (bits 3:0 must be 0x1), use SetInterruptPolarity() */ +0x02, /* 0x31 : bit 1 = interrupt depending on the polarity, use CheckForDataReady() */ +0x00, /* 0x32 : not user-modifiable */ +0x02, /* 0x33 : not user-modifiable */ +0x08, /* 0x34 : not user-modifiable */ +0x00, /* 0x35 : not user-modifiable */ +0x08, /* 0x36 : not user-modifiable */ +0x10, /* 0x37 : not user-modifiable */ +0x01, /* 0x38 : not user-modifiable */ +0x01, /* 0x39 : not user-modifiable */ +0x00, /* 0x3a : not user-modifiable */ +0x00, /* 0x3b : not user-modifiable */ +0x00, /* 0x3c : not user-modifiable */ +0x00, /* 0x3d : not user-modifiable */ +0xff, /* 0x3e : not user-modifiable */ +0x00, /* 0x3f : not user-modifiable */ +0x0F, /* 0x40 : not user-modifiable */ +0x00, /* 0x41 : not user-modifiable */ +0x00, /* 0x42 : not user-modifiable */ +0x00, /* 0x43 : not user-modifiable */ +0x00, /* 0x44 : not user-modifiable */ +0x00, /* 0x45 : not user-modifiable */ +0x20, /* 0x46 : interrupt configuration 0->level low detection, 1-> level high, 2-> Out of window, 3->In window, 0x20-> New sample ready , TBC */ +0x0b, /* 0x47 : not user-modifiable */ +0x00, /* 0x48 : not user-modifiable */ +0x00, /* 0x49 : not user-modifiable */ +0x02, /* 0x4a : not user-modifiable */ +0x0a, /* 0x4b : not user-modifiable */ +0x21, /* 0x4c : not user-modifiable */ +0x00, /* 0x4d : not user-modifiable */ +0x00, /* 0x4e : not user-modifiable */ +0x05, /* 0x4f : not user-modifiable */ +0x00, /* 0x50 : not user-modifiable */ +0x00, /* 0x51 : not user-modifiable */ +0x00, /* 0x52 : not user-modifiable */ +0x00, /* 0x53 : not user-modifiable */ +0xc8, /* 0x54 : not user-modifiable */ +0x00, /* 0x55 : not user-modifiable */ +0x00, /* 0x56 : not user-modifiable */ +0x38, /* 0x57 : not user-modifiable */ +0xff, /* 0x58 : not user-modifiable */ +0x01, /* 0x59 : not user-modifiable */ +0x00, /* 0x5a : not user-modifiable */ +0x08, /* 0x5b : not user-modifiable */ +0x00, /* 0x5c : not user-modifiable */ +0x00, /* 0x5d : not user-modifiable */ +0x01, /* 0x5e : not user-modifiable */ +0xcc, /* 0x5f : not user-modifiable */ +0x0f, /* 0x60 : not user-modifiable */ +0x01, /* 0x61 : not user-modifiable */ +0xf1, /* 0x62 : not user-modifiable */ +0x0d, /* 0x63 : not user-modifiable */ +0x01, /* 0x64 : Sigma threshold MSB (mm in 14.2 format for MSB+LSB), use SetSigmaThreshold(), default value 90 mm */ +0x68, /* 0x65 : Sigma threshold LSB */ +0x00, /* 0x66 : Min count Rate MSB (MCPS in 9.7 format for MSB+LSB), use SetSignalThreshold() */ +0x80, /* 0x67 : Min count Rate LSB */ +0x08, /* 0x68 : not user-modifiable */ +0xb8, /* 0x69 : not user-modifiable */ +0x00, /* 0x6a : not user-modifiable */ +0x00, /* 0x6b : not user-modifiable */ +0x00, /* 0x6c : Intermeasurement period MSB, 32 bits register, use SetIntermeasurementInMs() */ +0x00, /* 0x6d : Intermeasurement period */ +0x0f, /* 0x6e : Intermeasurement period */ +0x89, /* 0x6f : Intermeasurement period LSB */ +0x00, /* 0x70 : not user-modifiable */ +0x00, /* 0x71 : not user-modifiable */ +0x00, /* 0x72 : distance threshold high MSB (in mm, MSB+LSB), use SetD:tanceThreshold() */ +0x00, /* 0x73 : distance threshold high LSB */ +0x00, /* 0x74 : distance threshold low MSB ( in mm, MSB+LSB), use SetD:tanceThreshold() */ +0x00, /* 0x75 : distance threshold low LSB */ +0x00, /* 0x76 : not user-modifiable */ +0x01, /* 0x77 : not user-modifiable */ +0x0f, /* 0x78 : not user-modifiable */ +0x0d, /* 0x79 : not user-modifiable */ +0x0e, /* 0x7a : not user-modifiable */ +0x0e, /* 0x7b : not user-modifiable */ +0x00, /* 0x7c : not user-modifiable */ +0x00, /* 0x7d : not user-modifiable */ +0x02, /* 0x7e : not user-modifiable */ +0xc7, /* 0x7f : ROI center, use SetROI() */ +0xff, /* 0x80 : XY ROI (X=Width, Y=Height), use SetROI() */ +0x9B, /* 0x81 : not user-modifiable */ +0x00, /* 0x82 : not user-modifiable */ +0x00, /* 0x83 : not user-modifiable */ +0x00, /* 0x84 : not user-modifiable */ +0x01, /* 0x85 : not user-modifiable */ +0x00, /* 0x86 : clear interrupt, use ClearInterrupt() */ +0x00 /* 0x87 : start ranging, use StartRanging() or StopRanging(), If you want an automatic start after VL53L1X_init() call, put 0x40 in location 0x87 */ +}; + +static const uint8_t status_rtn[24] = { 255, 255, 255, 5, 2, 4, 1, 7, 3, 0, + 255, 255, 9, 13, 255, 255, 255, 255, 10, 6, + 255, 255, 11, 12 +}; + +static uint8_t _I2CBuffer[256]; + +static int32_t lastMeasurementCm = RANGEFINDER_OUT_OF_RANGE; +static bool lastMeasurementIsNew = false; +static bool isInitialized = false; +static bool isResponding = true; + +#define _I2CWrite(dev, data, size) \ + (busWriteBuf(dev, 0xFF, data, size) ? 0 : -1) + +#define _I2CRead(dev, data, size) \ + (busReadBuf(dev, 0xFF, data, size) ? 0 : -1) + +VL53L1X_ERROR VL53L1_WriteMulti(busDevice_t * Dev, uint16_t index, uint8_t *pdata, uint32_t count) { + int status_int; + VL53L1X_ERROR Status = VL53L1_ERROR_NONE; + if (count > sizeof(_I2CBuffer) - 1) { + return VL53L1_ERROR_INVALID_PARAMS; + } + _I2CBuffer[0] = index>>8; + _I2CBuffer[1] = index&0xFF; + memcpy(&_I2CBuffer[2], pdata, count); + status_int = _I2CWrite(Dev, _I2CBuffer, count + 2); + if (status_int != 0) { + Status = VL53L1_ERROR_CONTROL_INTERFACE; + } + return Status; +} + +// the ranging_sensor_comms.dll will take care of the page selection +VL53L1X_ERROR VL53L1_ReadMulti(busDevice_t * Dev, uint16_t index, uint8_t *pdata, uint32_t count) { + VL53L1X_ERROR Status = VL53L1_ERROR_NONE; + int32_t status_int; + + _I2CBuffer[0] = index>>8; + _I2CBuffer[1] = index&0xFF; + status_int = _I2CWrite(Dev, _I2CBuffer, 2); + if (status_int != 0) { + Status = VL53L1_ERROR_CONTROL_INTERFACE; + goto done; + } + status_int = _I2CRead(Dev, pdata, count); + if (status_int != 0) { + Status = VL53L1_ERROR_CONTROL_INTERFACE; + } +done: + return Status; +} + +VL53L1X_ERROR VL53L1_WrByte(busDevice_t * Dev, uint16_t index, uint8_t data) { + VL53L1X_ERROR Status = VL53L1_ERROR_NONE; + int32_t status_int; + + _I2CBuffer[0] = index>>8; + _I2CBuffer[1] = index&0xFF; + _I2CBuffer[2] = data; + + status_int = _I2CWrite(Dev, _I2CBuffer, 3); + if (status_int != 0) { + Status = VL53L1_ERROR_CONTROL_INTERFACE; + } + return Status; +} + +VL53L1X_ERROR VL53L1_WrWord(busDevice_t * Dev, uint16_t index, uint16_t data) { + VL53L1X_ERROR Status = VL53L1_ERROR_NONE; + int32_t status_int; + + _I2CBuffer[0] = index>>8; + _I2CBuffer[1] = index&0xFF; + _I2CBuffer[2] = data >> 8; + _I2CBuffer[3] = data & 0x00FF; + + status_int = _I2CWrite(Dev, _I2CBuffer, 4); + if (status_int != 0) { + Status = VL53L1_ERROR_CONTROL_INTERFACE; + } + return Status; +} + +VL53L1X_ERROR VL53L1_WrDWord(busDevice_t * Dev, uint16_t index, uint32_t data) { + VL53L1X_ERROR Status = VL53L1_ERROR_NONE; + int32_t status_int; + _I2CBuffer[0] = index>>8; + _I2CBuffer[1] = index&0xFF; + _I2CBuffer[2] = (data >> 24) & 0xFF; + _I2CBuffer[3] = (data >> 16) & 0xFF; + _I2CBuffer[4] = (data >> 8) & 0xFF; + _I2CBuffer[5] = (data >> 0 ) & 0xFF; + status_int = _I2CWrite(Dev, _I2CBuffer, 6); + if (status_int != 0) { + Status = VL53L1_ERROR_CONTROL_INTERFACE; + } + return Status; +} + +VL53L1X_ERROR VL53L1_RdByte(busDevice_t * Dev, uint16_t index, uint8_t *data) { + VL53L1X_ERROR Status = VL53L1_ERROR_NONE; + int32_t status_int; + + _I2CBuffer[0] = index>>8; + _I2CBuffer[1] = index&0xFF; + status_int = _I2CWrite(Dev, _I2CBuffer, 2); + if( status_int ){ + Status = VL53L1_ERROR_CONTROL_INTERFACE; + goto done; + } + status_int = _I2CRead(Dev, data, 1); + if (status_int != 0) { + Status = VL53L1_ERROR_CONTROL_INTERFACE; + } +done: + return Status; +} + +VL53L1X_ERROR VL53L1_UpdateByte(busDevice_t * Dev, uint16_t index, uint8_t AndData, uint8_t OrData) { + VL53L1X_ERROR Status = VL53L1_ERROR_NONE; + uint8_t data; + + Status = VL53L1_RdByte(Dev, index, &data); + if (Status) { + goto done; + } + data = (data & AndData) | OrData; + Status = VL53L1_WrByte(Dev, index, data); +done: + return Status; +} + +VL53L1X_ERROR VL53L1_RdWord(busDevice_t * Dev, uint16_t index, uint16_t *data) { + VL53L1X_ERROR Status = VL53L1_ERROR_NONE; + int32_t status_int; + + _I2CBuffer[0] = index>>8; + _I2CBuffer[1] = index&0xFF; + status_int = _I2CWrite(Dev, _I2CBuffer, 2); + + if( status_int ){ + Status = VL53L1_ERROR_CONTROL_INTERFACE; + goto done; + } + status_int = _I2CRead(Dev, _I2CBuffer, 2); + if (status_int != 0) { + Status = VL53L1_ERROR_CONTROL_INTERFACE; + goto done; + } + + *data = ((uint16_t)_I2CBuffer[0]<<8) + (uint16_t)_I2CBuffer[1]; +done: + return Status; +} + +VL53L1X_ERROR VL53L1_RdDWord(busDevice_t * Dev, uint16_t index, uint32_t *data) { + VL53L1X_ERROR Status = VL53L1_ERROR_NONE; + int32_t status_int; + + _I2CBuffer[0] = index>>8; + _I2CBuffer[1] = index&0xFF; + status_int = _I2CWrite(Dev, _I2CBuffer, 2); + if (status_int != 0) { + Status = VL53L1_ERROR_CONTROL_INTERFACE; + goto done; + } + status_int = _I2CRead(Dev, _I2CBuffer, 4); + if (status_int != 0) { + Status = VL53L1_ERROR_CONTROL_INTERFACE; + goto done; + } + + *data = ((uint32_t)_I2CBuffer[0]<<24) + ((uint32_t)_I2CBuffer[1]<<16) + ((uint32_t)_I2CBuffer[2]<<8) + (uint32_t)_I2CBuffer[3]; + +done: + return Status; +} + +VL53L1X_ERROR VL53L1X_GetSWVersion(VL53L1X_Version_t *pVersion) +{ + VL53L1X_ERROR Status = 0; + + pVersion->major = VL53L1X_IMPLEMENTATION_VER_MAJOR; + pVersion->minor = VL53L1X_IMPLEMENTATION_VER_MINOR; + pVersion->build = VL53L1X_IMPLEMENTATION_VER_SUB; + pVersion->revision = VL53L1X_IMPLEMENTATION_VER_REVISION; + return Status; +} + +VL53L1X_ERROR VL53L1X_SetI2CAddress(busDevice_t * dev, uint8_t new_address) +{ + VL53L1X_ERROR status = 0; + + status = VL53L1_WrByte(dev, VL53L1_I2C_SLAVE__DEVICE_ADDRESS, new_address >> 1); + return status; +} + +VL53L1X_ERROR VL53L1X_SensorInit(busDevice_t * dev) +{ + VL53L1X_ERROR status = 0; + uint8_t Addr = 0x00, tmp; + + for (Addr = 0x2D; Addr <= 0x87; Addr++){ + status = VL53L1_WrByte(dev, Addr, VL51L1X_DEFAULT_CONFIGURATION[Addr - 0x2D]); + } + status = VL53L1X_StartRanging(dev); + tmp = 0; + while(tmp==0){ + status = VL53L1X_CheckForDataReady(dev, &tmp); + } + status = VL53L1X_ClearInterrupt(dev); + status = VL53L1X_StopRanging(dev); + status = VL53L1_WrByte(dev, VL53L1_VHV_CONFIG__TIMEOUT_MACROP_LOOP_BOUND, 0x09); /* two bounds VHV */ + status = VL53L1_WrByte(dev, 0x0B, 0); /* start VHV from the previous temperature */ + return status; +} + +VL53L1X_ERROR VL53L1X_ClearInterrupt(busDevice_t * dev) +{ + VL53L1X_ERROR status = 0; + + status = VL53L1_WrByte(dev, SYSTEM__INTERRUPT_CLEAR, 0x01); + return status; +} + +VL53L1X_ERROR VL53L1X_SetInterruptPolarity(busDevice_t * dev, uint8_t NewPolarity) +{ + uint8_t Temp; + VL53L1X_ERROR status = 0; + + status = VL53L1_RdByte(dev, GPIO_HV_MUX__CTRL, &Temp); + Temp = Temp & 0xEF; + status = VL53L1_WrByte(dev, GPIO_HV_MUX__CTRL, Temp | (!(NewPolarity & 1)) << 4); + return status; +} + +VL53L1X_ERROR VL53L1X_GetInterruptPolarity(busDevice_t * dev, uint8_t *pInterruptPolarity) +{ + uint8_t Temp; + VL53L1X_ERROR status = 0; + + status = VL53L1_RdByte(dev, GPIO_HV_MUX__CTRL, &Temp); + Temp = Temp & 0x10; + *pInterruptPolarity = !(Temp>>4); + return status; +} + +VL53L1X_ERROR VL53L1X_StartRanging(busDevice_t * dev) +{ + VL53L1X_ERROR status = 0; + + status = VL53L1_WrByte(dev, SYSTEM__MODE_START, 0x40); /* Enable VL53L1X */ + return status; +} + +VL53L1X_ERROR VL53L1X_StopRanging(busDevice_t * dev) +{ + VL53L1X_ERROR status = 0; + + status = VL53L1_WrByte(dev, SYSTEM__MODE_START, 0x00); /* Disable VL53L1X */ + return status; +} + +VL53L1X_ERROR VL53L1X_CheckForDataReady(busDevice_t * dev, uint8_t *isDataReady) +{ + uint8_t Temp; + uint8_t IntPol; + VL53L1X_ERROR status = 0; + + status = VL53L1X_GetInterruptPolarity(dev, &IntPol); + status = VL53L1_RdByte(dev, GPIO__TIO_HV_STATUS, &Temp); + /* Read in the register to check if a new value is available */ + if (status == 0){ + if ((Temp & 1) == IntPol) + *isDataReady = 1; + else + *isDataReady = 0; + } + return status; +} + +VL53L1X_ERROR VL53L1X_SetTimingBudgetInMs(busDevice_t * dev, uint16_t TimingBudgetInMs) +{ + uint16_t DM; + VL53L1X_ERROR status=0; + + status = VL53L1X_GetDistanceMode(dev, &DM); + if (DM == 0) + return 1; + else if (DM == 1) { /* Short DistanceMode */ + switch (TimingBudgetInMs) { + case 15: /* only available in short distance mode */ + VL53L1_WrWord(dev, RANGE_CONFIG__TIMEOUT_MACROP_A_HI, + 0x01D); + VL53L1_WrWord(dev, RANGE_CONFIG__TIMEOUT_MACROP_B_HI, + 0x0027); + break; + case 20: + VL53L1_WrWord(dev, RANGE_CONFIG__TIMEOUT_MACROP_A_HI, + 0x0051); + VL53L1_WrWord(dev, RANGE_CONFIG__TIMEOUT_MACROP_B_HI, + 0x006E); + break; + case 33: + VL53L1_WrWord(dev, RANGE_CONFIG__TIMEOUT_MACROP_A_HI, + 0x00D6); + VL53L1_WrWord(dev, RANGE_CONFIG__TIMEOUT_MACROP_B_HI, + 0x006E); + break; + case 50: + VL53L1_WrWord(dev, RANGE_CONFIG__TIMEOUT_MACROP_A_HI, + 0x1AE); + VL53L1_WrWord(dev, RANGE_CONFIG__TIMEOUT_MACROP_B_HI, + 0x01E8); + break; + case 100: + VL53L1_WrWord(dev, RANGE_CONFIG__TIMEOUT_MACROP_A_HI, + 0x02E1); + VL53L1_WrWord(dev, RANGE_CONFIG__TIMEOUT_MACROP_B_HI, + 0x0388); + break; + case 200: + VL53L1_WrWord(dev, RANGE_CONFIG__TIMEOUT_MACROP_A_HI, + 0x03E1); + VL53L1_WrWord(dev, RANGE_CONFIG__TIMEOUT_MACROP_B_HI, + 0x0496); + break; + case 500: + VL53L1_WrWord(dev, RANGE_CONFIG__TIMEOUT_MACROP_A_HI, + 0x0591); + VL53L1_WrWord(dev, RANGE_CONFIG__TIMEOUT_MACROP_B_HI, + 0x05C1); + break; + default: + status = 1; + break; + } + } else { + switch (TimingBudgetInMs) { + case 20: + VL53L1_WrWord(dev, RANGE_CONFIG__TIMEOUT_MACROP_A_HI, + 0x001E); + VL53L1_WrWord(dev, RANGE_CONFIG__TIMEOUT_MACROP_B_HI, + 0x0022); + break; + case 33: + VL53L1_WrWord(dev, RANGE_CONFIG__TIMEOUT_MACROP_A_HI, + 0x0060); + VL53L1_WrWord(dev, RANGE_CONFIG__TIMEOUT_MACROP_B_HI, + 0x006E); + break; + case 50: + VL53L1_WrWord(dev, RANGE_CONFIG__TIMEOUT_MACROP_A_HI, + 0x00AD); + VL53L1_WrWord(dev, RANGE_CONFIG__TIMEOUT_MACROP_B_HI, + 0x00C6); + break; + case 100: + VL53L1_WrWord(dev, RANGE_CONFIG__TIMEOUT_MACROP_A_HI, + 0x01CC); + VL53L1_WrWord(dev, RANGE_CONFIG__TIMEOUT_MACROP_B_HI, + 0x01EA); + break; + case 200: + VL53L1_WrWord(dev, RANGE_CONFIG__TIMEOUT_MACROP_A_HI, + 0x02D9); + VL53L1_WrWord(dev, RANGE_CONFIG__TIMEOUT_MACROP_B_HI, + 0x02F8); + break; + case 500: + VL53L1_WrWord(dev, RANGE_CONFIG__TIMEOUT_MACROP_A_HI, + 0x048F); + VL53L1_WrWord(dev, RANGE_CONFIG__TIMEOUT_MACROP_B_HI, + 0x04A4); + break; + default: + status = 1; + break; + } + } + return status; +} + +VL53L1X_ERROR VL53L1X_GetTimingBudgetInMs(busDevice_t * dev, uint16_t *pTimingBudget) +{ + uint16_t Temp; + VL53L1X_ERROR status = 0; + + status = VL53L1_RdWord(dev, RANGE_CONFIG__TIMEOUT_MACROP_A_HI, &Temp); + switch (Temp) { + case 0x001D : + *pTimingBudget = 15; + break; + case 0x0051 : + case 0x001E : + *pTimingBudget = 20; + break; + case 0x00D6 : + case 0x0060 : + *pTimingBudget = 33; + break; + case 0x1AE : + case 0x00AD : + *pTimingBudget = 50; + break; + case 0x02E1 : + case 0x01CC : + *pTimingBudget = 100; + break; + case 0x03E1 : + case 0x02D9 : + *pTimingBudget = 200; + break; + case 0x0591 : + case 0x048F : + *pTimingBudget = 500; + break; + default: + status = 1; + *pTimingBudget = 0; + } + return status; +} + +VL53L1X_ERROR VL53L1X_SetDistanceMode(busDevice_t * dev, uint16_t DM) +{ + uint16_t TB; + VL53L1X_ERROR status = 0; + + status = VL53L1X_GetTimingBudgetInMs(dev, &TB); + if (status != 0) + return 1; + switch (DM) { + case 1: + status = VL53L1_WrByte(dev, PHASECAL_CONFIG__TIMEOUT_MACROP, 0x14); + status = VL53L1_WrByte(dev, RANGE_CONFIG__VCSEL_PERIOD_A, 0x07); + status = VL53L1_WrByte(dev, RANGE_CONFIG__VCSEL_PERIOD_B, 0x05); + status = VL53L1_WrByte(dev, RANGE_CONFIG__VALID_PHASE_HIGH, 0x38); + status = VL53L1_WrWord(dev, SD_CONFIG__WOI_SD0, 0x0705); + status = VL53L1_WrWord(dev, SD_CONFIG__INITIAL_PHASE_SD0, 0x0606); + break; + case 2: + status = VL53L1_WrByte(dev, PHASECAL_CONFIG__TIMEOUT_MACROP, 0x0A); + status = VL53L1_WrByte(dev, RANGE_CONFIG__VCSEL_PERIOD_A, 0x0F); + status = VL53L1_WrByte(dev, RANGE_CONFIG__VCSEL_PERIOD_B, 0x0D); + status = VL53L1_WrByte(dev, RANGE_CONFIG__VALID_PHASE_HIGH, 0xB8); + status = VL53L1_WrWord(dev, SD_CONFIG__WOI_SD0, 0x0F0D); + status = VL53L1_WrWord(dev, SD_CONFIG__INITIAL_PHASE_SD0, 0x0E0E); + break; + default: + status = 1; + break; + } + + if (status == 0) + status = VL53L1X_SetTimingBudgetInMs(dev, TB); + return status; +} + +VL53L1X_ERROR VL53L1X_GetDistanceMode(busDevice_t * dev, uint16_t *DM) +{ + uint8_t TempDM, status=0; + + status = VL53L1_RdByte(dev,PHASECAL_CONFIG__TIMEOUT_MACROP, &TempDM); + if (TempDM == 0x14) + *DM=1; + if(TempDM == 0x0A) + *DM=2; + return status; +} + +VL53L1X_ERROR VL53L1X_SetInterMeasurementInMs(busDevice_t * dev, uint32_t InterMeasMs) +{ + uint16_t ClockPLL; + VL53L1X_ERROR status = 0; + + status = VL53L1_RdWord(dev, VL53L1_RESULT__OSC_CALIBRATE_VAL, &ClockPLL); + ClockPLL = ClockPLL&0x3FF; + VL53L1_WrDWord(dev, VL53L1_SYSTEM__INTERMEASUREMENT_PERIOD, + (uint32_t)(ClockPLL * InterMeasMs * 1.075)); + return status; + +} + +VL53L1X_ERROR VL53L1X_GetInterMeasurementInMs(busDevice_t * dev, uint16_t *pIM) +{ + uint16_t ClockPLL; + VL53L1X_ERROR status = 0; + uint32_t tmp; + + status = VL53L1_RdDWord(dev,VL53L1_SYSTEM__INTERMEASUREMENT_PERIOD, &tmp); + *pIM = (uint16_t)tmp; + status = VL53L1_RdWord(dev, VL53L1_RESULT__OSC_CALIBRATE_VAL, &ClockPLL); + ClockPLL = ClockPLL&0x3FF; + *pIM= (uint16_t)(*pIM/(ClockPLL*1.065)); + return status; +} + +VL53L1X_ERROR VL53L1X_BootState(busDevice_t * dev, uint8_t *state) +{ + VL53L1X_ERROR status = 0; + uint8_t tmp = 0; + + status = VL53L1_RdByte(dev,VL53L1_FIRMWARE__SYSTEM_STATUS, &tmp); + *state = tmp; + return status; +} + +VL53L1X_ERROR VL53L1X_GetSensorId(busDevice_t * dev, uint16_t *sensorId) +{ + VL53L1X_ERROR status = 0; + uint16_t tmp = 0; + + status = VL53L1_RdWord(dev, VL53L1_IDENTIFICATION__MODEL_ID, &tmp); + *sensorId = tmp; + return status; +} + +VL53L1X_ERROR VL53L1X_GetDistance(busDevice_t * dev, uint16_t *distance) +{ + VL53L1X_ERROR status = 0; + uint16_t tmp; + + status = (VL53L1_RdWord(dev, + VL53L1_RESULT__FINAL_CROSSTALK_CORRECTED_RANGE_MM_SD0, &tmp)); + *distance = tmp; + return status; +} + +VL53L1X_ERROR VL53L1X_GetSignalPerSpad(busDevice_t * dev, uint16_t *signalRate) +{ + VL53L1X_ERROR status = 0; + uint16_t SpNb=1, signal; + + status = VL53L1_RdWord(dev, + VL53L1_RESULT__PEAK_SIGNAL_COUNT_RATE_CROSSTALK_CORRECTED_MCPS_SD0, &signal); + status = VL53L1_RdWord(dev, + VL53L1_RESULT__DSS_ACTUAL_EFFECTIVE_SPADS_SD0, &SpNb); + *signalRate = (uint16_t) (200.0*signal/SpNb); + return status; +} + +VL53L1X_ERROR VL53L1X_GetAmbientPerSpad(busDevice_t * dev, uint16_t *ambPerSp) +{ + VL53L1X_ERROR status = 0; + uint16_t AmbientRate, SpNb = 1; + + status = VL53L1_RdWord(dev, RESULT__AMBIENT_COUNT_RATE_MCPS_SD, &AmbientRate); + status = VL53L1_RdWord(dev, VL53L1_RESULT__DSS_ACTUAL_EFFECTIVE_SPADS_SD0, &SpNb); + *ambPerSp=(uint16_t) (200.0 * AmbientRate / SpNb); + return status; +} + +VL53L1X_ERROR VL53L1X_GetSignalRate(busDevice_t * dev, uint16_t *signal) +{ + VL53L1X_ERROR status = 0; + uint16_t tmp; + + status = VL53L1_RdWord(dev, + VL53L1_RESULT__PEAK_SIGNAL_COUNT_RATE_CROSSTALK_CORRECTED_MCPS_SD0, &tmp); + *signal = tmp*8; + return status; +} + +VL53L1X_ERROR VL53L1X_GetSpadNb(busDevice_t * dev, uint16_t *spNb) +{ + VL53L1X_ERROR status = 0; + uint16_t tmp; + + status = VL53L1_RdWord(dev, + VL53L1_RESULT__DSS_ACTUAL_EFFECTIVE_SPADS_SD0, &tmp); + *spNb = tmp >> 8; + return status; +} + +VL53L1X_ERROR VL53L1X_GetAmbientRate(busDevice_t * dev, uint16_t *ambRate) +{ + VL53L1X_ERROR status = 0; + uint16_t tmp; + + status = VL53L1_RdWord(dev, RESULT__AMBIENT_COUNT_RATE_MCPS_SD, &tmp); + *ambRate = tmp*8; + return status; +} + +VL53L1X_ERROR VL53L1X_GetRangeStatus(busDevice_t * dev, uint8_t *rangeStatus) +{ + VL53L1X_ERROR status = 0; + uint8_t RgSt; + + *rangeStatus = 255; + status = VL53L1_RdByte(dev, VL53L1_RESULT__RANGE_STATUS, &RgSt); + RgSt = RgSt & 0x1F; + if (RgSt < 24) + *rangeStatus = status_rtn[RgSt]; + return status; +} + +VL53L1X_ERROR VL53L1X_GetResult(busDevice_t * dev, VL53L1X_Result_t *pResult) +{ + VL53L1X_ERROR status = 0; + uint8_t Temp[17]; + uint8_t RgSt = 255; + + status = VL53L1_ReadMulti(dev, VL53L1_RESULT__RANGE_STATUS, Temp, 17); + RgSt = Temp[0] & 0x1F; + if (RgSt < 24) + RgSt = status_rtn[RgSt]; + pResult->Status = RgSt; + pResult->Ambient = (Temp[7] << 8 | Temp[8]) * 8; + pResult->NumSPADs = Temp[3]; + pResult->SigPerSPAD = (Temp[15] << 8 | Temp[16]) * 8; + pResult->Distance = Temp[13] << 8 | Temp[14]; + + return status; +} + +VL53L1X_ERROR VL53L1X_SetOffset(busDevice_t * dev, int16_t OffsetValue) +{ + VL53L1X_ERROR status = 0; + int16_t Temp; + + Temp = (OffsetValue*4); + VL53L1_WrWord(dev, ALGO__PART_TO_PART_RANGE_OFFSET_MM, + (uint16_t)Temp); + VL53L1_WrWord(dev, MM_CONFIG__INNER_OFFSET_MM, 0x0); + VL53L1_WrWord(dev, MM_CONFIG__OUTER_OFFSET_MM, 0x0); + return status; +} + +VL53L1X_ERROR VL53L1X_GetOffset(busDevice_t * dev, int16_t *offset) +{ + VL53L1X_ERROR status = 0; + uint16_t Temp; + + status = VL53L1_RdWord(dev,ALGO__PART_TO_PART_RANGE_OFFSET_MM, &Temp); + Temp = Temp<<3; + Temp = Temp>>5; + *offset = (int16_t)(Temp); + return status; +} + +VL53L1X_ERROR VL53L1X_SetXtalk(busDevice_t * dev, uint16_t XtalkValue) +{ +/* XTalkValue in count per second to avoid float type */ + VL53L1X_ERROR status = 0; + + status = VL53L1_WrWord(dev, + ALGO__CROSSTALK_COMPENSATION_X_PLANE_GRADIENT_KCPS, + 0x0000); + status = VL53L1_WrWord(dev, ALGO__CROSSTALK_COMPENSATION_Y_PLANE_GRADIENT_KCPS, + 0x0000); + status = VL53L1_WrWord(dev, ALGO__CROSSTALK_COMPENSATION_PLANE_OFFSET_KCPS, + (XtalkValue<<9)/1000); /* * << 9 (7.9 format) and /1000 to convert cps to kpcs */ + return status; +} + +VL53L1X_ERROR VL53L1X_GetXtalk(busDevice_t * dev, uint16_t *xtalk ) +{ + VL53L1X_ERROR status = 0; + + status = VL53L1_RdWord(dev,ALGO__CROSSTALK_COMPENSATION_PLANE_OFFSET_KCPS, xtalk); + *xtalk = (uint16_t)((*xtalk*1000)>>9); /* * 1000 to convert kcps to cps and >> 9 (7.9 format) */ + return status; +} + +VL53L1X_ERROR VL53L1X_SetDistanceThreshold(busDevice_t * dev, uint16_t ThreshLow, + uint16_t ThreshHigh, uint8_t Window, + uint8_t IntOnNoTarget) +{ + VL53L1X_ERROR status = 0; + uint8_t Temp = 0; + + status = VL53L1_RdByte(dev, SYSTEM__INTERRUPT_CONFIG_GPIO, &Temp); + Temp = Temp & 0x47; + if (IntOnNoTarget == 0) { + status = VL53L1_WrByte(dev, SYSTEM__INTERRUPT_CONFIG_GPIO, + (Temp | (Window & 0x07))); + } else { + status = VL53L1_WrByte(dev, SYSTEM__INTERRUPT_CONFIG_GPIO, + ((Temp | (Window & 0x07)) | 0x40)); + } + status = VL53L1_WrWord(dev, SYSTEM__THRESH_HIGH, ThreshHigh); + status = VL53L1_WrWord(dev, SYSTEM__THRESH_LOW, ThreshLow); + return status; +} + +VL53L1X_ERROR VL53L1X_GetDistanceThresholdWindow(busDevice_t * dev, uint16_t *window) +{ + VL53L1X_ERROR status = 0; + uint8_t tmp; + status = VL53L1_RdByte(dev,SYSTEM__INTERRUPT_CONFIG_GPIO, &tmp); + *window = (uint16_t)(tmp & 0x7); + return status; +} + +VL53L1X_ERROR VL53L1X_GetDistanceThresholdLow(busDevice_t * dev, uint16_t *low) +{ + VL53L1X_ERROR status = 0; + uint16_t tmp; + + status = VL53L1_RdWord(dev,SYSTEM__THRESH_LOW, &tmp); + *low = tmp; + return status; +} + +VL53L1X_ERROR VL53L1X_GetDistanceThresholdHigh(busDevice_t * dev, uint16_t *high) +{ + VL53L1X_ERROR status = 0; + uint16_t tmp; + + status = VL53L1_RdWord(dev,SYSTEM__THRESH_HIGH, &tmp); + *high = tmp; + return status; +} + +VL53L1X_ERROR VL53L1X_SetROICenter(busDevice_t * dev, uint8_t ROICenter) +{ + VL53L1X_ERROR status = 0; + status = VL53L1_WrByte(dev, ROI_CONFIG__USER_ROI_CENTRE_SPAD, ROICenter); + return status; +} + +VL53L1X_ERROR VL53L1X_GetROICenter(busDevice_t * dev, uint8_t *ROICenter) +{ + VL53L1X_ERROR status = 0; + uint8_t tmp; + status = VL53L1_RdByte(dev, ROI_CONFIG__USER_ROI_CENTRE_SPAD, &tmp); + *ROICenter = tmp; + return status; +} + +VL53L1X_ERROR VL53L1X_SetROI(busDevice_t * dev, uint16_t X, uint16_t Y) +{ + uint8_t OpticalCenter; + VL53L1X_ERROR status = 0; + + status =VL53L1_RdByte(dev, VL53L1_ROI_CONFIG__MODE_ROI_CENTRE_SPAD, &OpticalCenter); + if (X > 16) + X = 16; + if (Y > 16) + Y = 16; + if (X > 10 || Y > 10){ + OpticalCenter = 199; + } + status = VL53L1_WrByte(dev, ROI_CONFIG__USER_ROI_CENTRE_SPAD, OpticalCenter); + status = VL53L1_WrByte(dev, ROI_CONFIG__USER_ROI_REQUESTED_GLOBAL_XY_SIZE, + (Y - 1) << 4 | (X - 1)); + return status; +} + +VL53L1X_ERROR VL53L1X_GetROI_XY(busDevice_t * dev, uint16_t *ROI_X, uint16_t *ROI_Y) +{ + VL53L1X_ERROR status = 0; + uint8_t tmp; + + status = VL53L1_RdByte(dev,ROI_CONFIG__USER_ROI_REQUESTED_GLOBAL_XY_SIZE, &tmp); + *ROI_X = ((uint16_t)tmp & 0x0F) + 1; + *ROI_Y = (((uint16_t)tmp & 0xF0) >> 4) + 1; + return status; +} + +VL53L1X_ERROR VL53L1X_SetSignalThreshold(busDevice_t * dev, uint16_t Signal) +{ + VL53L1X_ERROR status = 0; + + VL53L1_WrWord(dev,RANGE_CONFIG__MIN_COUNT_RATE_RTN_LIMIT_MCPS,Signal>>3); + return status; +} + +VL53L1X_ERROR VL53L1X_GetSignalThreshold(busDevice_t * dev, uint16_t *signal) +{ + VL53L1X_ERROR status = 0; + uint16_t tmp; + + status = VL53L1_RdWord(dev, + RANGE_CONFIG__MIN_COUNT_RATE_RTN_LIMIT_MCPS, &tmp); + *signal = tmp <<3; + return status; +} + +VL53L1X_ERROR VL53L1X_SetSigmaThreshold(busDevice_t * dev, uint16_t Sigma) +{ + VL53L1X_ERROR status = 0; + + if(Sigma>(0xFFFF>>2)){ + return 1; + } + /* 16 bits register 14.2 format */ + status = VL53L1_WrWord(dev,RANGE_CONFIG__SIGMA_THRESH,Sigma<<2); + return status; +} + +VL53L1X_ERROR VL53L1X_GetSigmaThreshold(busDevice_t * dev, uint16_t *sigma) +{ + VL53L1X_ERROR status = 0; + uint16_t tmp; + + status = VL53L1_RdWord(dev,RANGE_CONFIG__SIGMA_THRESH, &tmp); + *sigma = tmp >> 2; + return status; + +} + +VL53L1X_ERROR VL53L1X_StartTemperatureUpdate(busDevice_t * dev) +{ + VL53L1X_ERROR status = 0; + uint8_t tmp=0; + + status = VL53L1_WrByte(dev,VL53L1_VHV_CONFIG__TIMEOUT_MACROP_LOOP_BOUND,0x81); /* full VHV */ + status = VL53L1_WrByte(dev,0x0B,0x92); + status = VL53L1X_StartRanging(dev); + while(tmp==0){ + status = VL53L1X_CheckForDataReady(dev, &tmp); + } + tmp = 0; + status = VL53L1X_ClearInterrupt(dev); + status = VL53L1X_StopRanging(dev); + status = VL53L1_WrByte(dev, VL53L1_VHV_CONFIG__TIMEOUT_MACROP_LOOP_BOUND, 0x09); /* two bounds VHV */ + status = VL53L1_WrByte(dev, 0x0B, 0); /* start VHV from the previous temperature */ + return status; +} + + +int8_t VL53L1X_CalibrateOffset(busDevice_t * dev, uint16_t TargetDistInMm, int16_t *offset) +{ + uint8_t i, tmp; + int16_t AverageDistance = 0; + uint16_t distance; + VL53L1X_ERROR status = 0; + + status = VL53L1_WrWord(dev, ALGO__PART_TO_PART_RANGE_OFFSET_MM, 0x0); + status = VL53L1_WrWord(dev, MM_CONFIG__INNER_OFFSET_MM, 0x0); + status = VL53L1_WrWord(dev, MM_CONFIG__OUTER_OFFSET_MM, 0x0); + status = VL53L1X_StartRanging(dev); /* Enable VL53L1X sensor */ + for (i = 0; i < 50; i++) { + tmp = 0; + while (tmp == 0){ + status = VL53L1X_CheckForDataReady(dev, &tmp); + } + status = VL53L1X_GetDistance(dev, &distance); + status = VL53L1X_ClearInterrupt(dev); + AverageDistance = AverageDistance + distance; + } + status = VL53L1X_StopRanging(dev); + AverageDistance = AverageDistance / 50; + *offset = TargetDistInMm - AverageDistance; + status = VL53L1_WrWord(dev, ALGO__PART_TO_PART_RANGE_OFFSET_MM, *offset*4); + return status; +} + +int8_t VL53L1X_CalibrateXtalk(busDevice_t * dev, uint16_t TargetDistInMm, uint16_t *xtalk) +{ + uint8_t i, tmp; + float AverageSignalRate = 0; + float AverageDistance = 0; + float AverageSpadNb = 0; + uint16_t distance = 0, spadNum; + uint16_t sr; + VL53L1X_ERROR status = 0; + uint32_t calXtalk; + + status = VL53L1_WrWord(dev, 0x0016,0); + status = VL53L1X_StartRanging(dev); + for (i = 0; i < 50; i++) { + tmp = 0; + while (tmp == 0){ + status = VL53L1X_CheckForDataReady(dev, &tmp); + } + status= VL53L1X_GetSignalRate(dev, &sr); + status= VL53L1X_GetDistance(dev, &distance); + status = VL53L1X_ClearInterrupt(dev); + AverageDistance = AverageDistance + distance; + status = VL53L1X_GetSpadNb(dev, &spadNum); + AverageSpadNb = AverageSpadNb + spadNum; + AverageSignalRate = + AverageSignalRate + sr; + } + status = VL53L1X_StopRanging(dev); + AverageDistance = AverageDistance / 50; + AverageSpadNb = AverageSpadNb / 50; + AverageSignalRate = AverageSignalRate / 50; + /* Calculate Xtalk value */ + calXtalk = (uint16_t)(512*(AverageSignalRate*(1-(AverageDistance/TargetDistInMm)))/AverageSpadNb); + *xtalk = (uint16_t)((calXtalk*1000)>>9); + status = VL53L1_WrWord(dev, 0x0016, (uint16_t)calXtalk); + return status; +} + +static void vl53l1x_Init(rangefinderDev_t * rangefinder) +{ + isInitialized = false; + VL53L1X_SensorInit(rangefinder->busDev); + VL53L1X_SetDistanceMode(rangefinder->busDev, 2); /* 1=short, 2=long */ + VL53L1X_SetTimingBudgetInMs(rangefinder->busDev, 33); /* in ms possible values [20, 50, 100, 200, 500] */ + VL53L1X_SetInterMeasurementInMs(rangefinder->busDev, 33); /* in ms, IM must be > = TB */ + VL53L1X_StartRanging(rangefinder->busDev); + isInitialized = true; +} + +void vl53l1x_Update(rangefinderDev_t * rangefinder) +{ + uint16_t Distance; + uint8_t dataReady; + + if (!isInitialized) { + return; + } + + VL53L1X_CheckForDataReady(rangefinder->busDev, &dataReady); + if (dataReady != 0) { + VL53L1X_GetDistance(rangefinder->busDev, &Distance); + lastMeasurementCm = Distance / 10; + lastMeasurementIsNew = true; + } + VL53L1X_ClearInterrupt(rangefinder->busDev); +} + +int32_t vl53l1x_GetDistance(rangefinderDev_t *dev) +{ + UNUSED(dev); + + if (isResponding && isInitialized) { + if (lastMeasurementIsNew) { + lastMeasurementIsNew = false; + return lastMeasurementCm < VL53L1X_MAX_RANGE_CM ? lastMeasurementCm : RANGEFINDER_OUT_OF_RANGE; + } + else { + return RANGEFINDER_NO_NEW_DATA; + } + } + else { + return RANGEFINDER_HARDWARE_FAILURE; + } +} + +static bool deviceDetect(busDevice_t * busDev) +{ + for (int retry = 0; retry < 5; retry++) { + uint8_t model_id; + + delay(150); + + VL53L1X_ERROR err = VL53L1_RdByte(busDev, 0x010F, &model_id); + if (err == 0 && model_id == 0xEA) { + return true; + } + }; + + return false; +} + +bool vl53l1xDetect(rangefinderDev_t * rangefinder) +{ + rangefinder->busDev = busDeviceInit(BUSTYPE_I2C, DEVHW_VL53L1X, 0, OWNER_RANGEFINDER); + if (rangefinder->busDev == NULL) { + return false; + } + + if (!deviceDetect(rangefinder->busDev)) { + busDeviceDeInit(rangefinder->busDev); + return false; + } + + rangefinder->delayMs = RANGEFINDER_VL53L1X_TASK_PERIOD_MS; + rangefinder->maxRangeCm = VL53L1X_MAX_RANGE_CM; + rangefinder->detectionConeDeciDegrees = VL53L1X_DETECTION_CONE_DECIDEGREES; + rangefinder->detectionConeExtendedDeciDegrees = VL53L1X_DETECTION_CONE_DECIDEGREES; + + rangefinder->init = &vl53l1x_Init; + rangefinder->update = &vl53l1x_Update; + rangefinder->read = &vl53l1x_GetDistance; + + return true; +} + +#endif diff --git a/src/main/drivers/rangefinder/rangefinder_vl53l1x.h b/src/main/drivers/rangefinder/rangefinder_vl53l1x.h new file mode 100644 index 0000000000..ad125ce8a0 --- /dev/null +++ b/src/main/drivers/rangefinder/rangefinder_vl53l1x.h @@ -0,0 +1,29 @@ +/* + * This file is part of INAV. + * + * This Source Code Form is subject to the terms of the Mozilla Public + * License, v. 2.0. If a copy of the MPL was not distributed with this file, + * You can obtain one at http://mozilla.org/MPL/2.0/. + * + * Alternatively, the contents of this file may be used under the terms + * of the GNU General Public License Version 3, as described below: + * + * This file is free software: you may copy, redistribute and/or modify + * it under the terms of the GNU General Public License as published by the + * Free Software Foundation, either version 3 of the License, or (at your + * option) any later version. + * + * This file is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General + * Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see http://www.gnu.org/licenses/. + */ + +#pragma once + +#define RANGEFINDER_VL53L1X_TASK_PERIOD_MS 50 + +bool vl53l1xDetect(rangefinderDev_t *dev); diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index f5fde879ef..6dcf22ea14 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -7,7 +7,7 @@ tables: values: ["NONE", "AUTO", "ADXL345", "MPU6050", "MMA845x", "BMA280", "LSM303DLHC", "MPU6000", "MPU6500", "MPU9250", "BMI160", "ICM20689", "FAKE"] enum: accelerationSensor_e - name: rangefinder_hardware - values: ["NONE", "HCSR04", "SRF10", "INAV_I2C", "VL53L0X", "MSP", "UNUSED", "BENEWAKE"] + values: ["NONE", "HCSR04", "SRF10", "INAV_I2C", "VL53L0X", "MSP", "UNUSED", "BENEWAKE", "VL53L1X"] enum: rangefinderType_e - name: mag_hardware values: ["NONE", "AUTO", "HMC5883", "AK8975", "GPSMAG", "MAG3110", "AK8963", "IST8310", "QMC5883", "MPU9250", "IST8308", "LIS3MDL", "MSP", "FAKE"] diff --git a/src/main/sensors/rangefinder.c b/src/main/sensors/rangefinder.c index 29955a42d5..5c7f10c353 100644 --- a/src/main/sensors/rangefinder.c +++ b/src/main/sensors/rangefinder.c @@ -40,6 +40,7 @@ #include "drivers/rangefinder/rangefinder_srf10.h" #include "drivers/rangefinder/rangefinder_hcsr04_i2c.h" #include "drivers/rangefinder/rangefinder_vl53l0x.h" +#include "drivers/rangefinder/rangefinder_vl53l1x.h" #include "drivers/rangefinder/rangefinder_virtual.h" #include "fc/config.h" @@ -131,6 +132,15 @@ static bool rangefinderDetect(rangefinderDev_t * dev, uint8_t rangefinderHardwar #endif break; + case RANGEFINDER_VL53L1X: +#if defined(USE_RANGEFINDER_VL53L1X) + if (vl53l1xDetect(dev)) { + rangefinderHardware = RANGEFINDER_VL53L1X; + rescheduleTask(TASK_RANGEFINDER, TASK_PERIOD_MS(RANGEFINDER_VL53L1X_TASK_PERIOD_MS)); + } +#endif + break; + case RANGEFINDER_MSP: #if defined(USE_RANGEFINDER_MSP) if (virtualRangefinderDetect(dev, &rangefinderMSPVtable)) { diff --git a/src/main/sensors/rangefinder.h b/src/main/sensors/rangefinder.h index 2cd7d01e1c..a814a11423 100644 --- a/src/main/sensors/rangefinder.h +++ b/src/main/sensors/rangefinder.h @@ -30,6 +30,7 @@ typedef enum { RANGEFINDER_MSP = 5, RANGEFINDER_UNUSED = 6, // Was UIB RANGEFINDER_BENEWAKE = 7, + RANGEFINDER_VL53L1X = 8, } rangefinderType_e; typedef struct rangefinderConfig_s { diff --git a/src/main/target/common.h b/src/main/target/common.h index 9d0ddc3346..74752f0172 100755 --- a/src/main/target/common.h +++ b/src/main/target/common.h @@ -97,6 +97,7 @@ #define USE_RANGEFINDER_MSP #define USE_RANGEFINDER_BENEWAKE #define USE_RANGEFINDER_VL53L0X +#define USE_RANGEFINDER_VL53L1X #define USE_RANGEFINDER_HCSR04_I2C // Allow default optic flow boards diff --git a/src/main/target/common_hardware.c b/src/main/target/common_hardware.c index ad8736d0db..7825ac1236 100755 --- a/src/main/target/common_hardware.c +++ b/src/main/target/common_hardware.c @@ -295,6 +295,15 @@ #endif #endif +#if defined(USE_RANGEFINDER_VL53L1X) + #if !defined(VL53L1X_I2C_BUS) && defined(RANGEFINDER_I2C_BUS) + #define VL53L1X_I2C_BUS RANGEFINDER_I2C_BUS + #endif + + #if defined(VL53L1X_I2C_BUS) + BUSDEV_REGISTER_I2C(busdev_vl53l1x, DEVHW_VL53L1X, VL53L1X_I2C_BUS, 0x29, NONE, DEVFLAGS_USE_RAW_REGISTERS, 0); + #endif +#endif /** AIRSPEED SENSORS **/ From ba9fb649e01d6d31e7dd730189eecc2a1d13d108 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Marcin=20Ga=C5=82czy=C5=84ski?= Date: Tue, 9 Feb 2021 00:24:16 +0100 Subject: [PATCH 044/286] optimize timings --- src/main/drivers/rangefinder/rangefinder_vl53l1x.c | 5 +++-- src/main/drivers/rangefinder/rangefinder_vl53l1x.h | 2 +- 2 files changed, 4 insertions(+), 3 deletions(-) diff --git a/src/main/drivers/rangefinder/rangefinder_vl53l1x.c b/src/main/drivers/rangefinder/rangefinder_vl53l1x.c index 01d6e11955..adc251275b 100644 --- a/src/main/drivers/rangefinder/rangefinder_vl53l1x.c +++ b/src/main/drivers/rangefinder/rangefinder_vl53l1x.c @@ -109,6 +109,7 @@ #define VL53L1X_MAX_RANGE_CM (300) #define VL53L1X_DETECTION_CONE_DECIDEGREES (270) +#define VL53L1X_TIMING_BUDGET (33) #define VL53L1X_IMPLEMENTATION_VER_MAJOR 3 #define VL53L1X_IMPLEMENTATION_VER_MINOR 4 @@ -1607,8 +1608,8 @@ static void vl53l1x_Init(rangefinderDev_t * rangefinder) isInitialized = false; VL53L1X_SensorInit(rangefinder->busDev); VL53L1X_SetDistanceMode(rangefinder->busDev, 2); /* 1=short, 2=long */ - VL53L1X_SetTimingBudgetInMs(rangefinder->busDev, 33); /* in ms possible values [20, 50, 100, 200, 500] */ - VL53L1X_SetInterMeasurementInMs(rangefinder->busDev, 33); /* in ms, IM must be > = TB */ + VL53L1X_SetTimingBudgetInMs(rangefinder->busDev, VL53L1X_TIMING_BUDGET); /* in ms possible values [20, 50, 100, 200, 500] */ + VL53L1X_SetInterMeasurementInMs(rangefinder->busDev, RANGEFINDER_VL53L1X_TASK_PERIOD_MS); /* in ms, IM must be > = TB */ VL53L1X_StartRanging(rangefinder->busDev); isInitialized = true; } diff --git a/src/main/drivers/rangefinder/rangefinder_vl53l1x.h b/src/main/drivers/rangefinder/rangefinder_vl53l1x.h index ad125ce8a0..3dc5b63bf0 100644 --- a/src/main/drivers/rangefinder/rangefinder_vl53l1x.h +++ b/src/main/drivers/rangefinder/rangefinder_vl53l1x.h @@ -24,6 +24,6 @@ #pragma once -#define RANGEFINDER_VL53L1X_TASK_PERIOD_MS 50 +#define RANGEFINDER_VL53L1X_TASK_PERIOD_MS (40) bool vl53l1xDetect(rangefinderDev_t *dev); From 3110a80892964e37b120265c7a58445b486cd5eb Mon Sep 17 00:00:00 2001 From: Darren Lines Date: Tue, 9 Feb 2021 15:04:09 +0000 Subject: [PATCH 045/286] Update rc_modes.h Updated the number of modes ranges to 40. More features are being added that require switching, and we have limited RC channels. The extra ranges will allow for some tricks to get multiple independant switches on a single channel. --- src/main/fc/rc_modes.h | 1 - 1 file changed, 1 deletion(-) diff --git a/src/main/fc/rc_modes.h b/src/main/fc/rc_modes.h index 2058c822d3..fc60d7b5be 100755 --- a/src/main/fc/rc_modes.h +++ b/src/main/fc/rc_modes.h @@ -74,7 +74,6 @@ typedef enum { // type to hold enough bits for CHECKBOX_ITEM_COUNT. Struct used for value-like behavior typedef struct boxBitmask_s { BITARRAY_DECLARE(bits, CHECKBOX_ITEM_COUNT); } boxBitmask_t; -#define MAX_MODE_ACTIVATION_CONDITION_COUNT 20 #define CHANNEL_RANGE_MIN 900 #define CHANNEL_RANGE_MAX 2100 From 1a1ae84fc59e57c6746ce739bc1f5068aeebe5d2 Mon Sep 17 00:00:00 2001 From: Darren Lines Date: Tue, 9 Feb 2021 15:08:42 +0000 Subject: [PATCH 046/286] Update rc_modes.h --- src/main/fc/rc_modes.h | 1 + 1 file changed, 1 insertion(+) diff --git a/src/main/fc/rc_modes.h b/src/main/fc/rc_modes.h index fc60d7b5be..6886c830d2 100755 --- a/src/main/fc/rc_modes.h +++ b/src/main/fc/rc_modes.h @@ -74,6 +74,7 @@ typedef enum { // type to hold enough bits for CHECKBOX_ITEM_COUNT. Struct used for value-like behavior typedef struct boxBitmask_s { BITARRAY_DECLARE(bits, CHECKBOX_ITEM_COUNT); } boxBitmask_t; +#define MAX_MODE_ACTIVATION_CONDITION_COUNT 40 #define CHANNEL_RANGE_MIN 900 #define CHANNEL_RANGE_MAX 2100 From d50d3b7d45c977e49ffaa6e33ef4dca83410f211 Mon Sep 17 00:00:00 2001 From: OptimusTi Date: Tue, 9 Feb 2021 13:50:36 -0500 Subject: [PATCH 047/286] Disarm Statistics Pages Divides disarm stats into pages. Roll stick can be used to go back and forth pages. --- src/main/io/osd.c | 71 ++++++++++++++++++++++++++++++----------------- 1 file changed, 45 insertions(+), 26 deletions(-) diff --git a/src/main/io/osd.c b/src/main/io/osd.c index 3612ef00d5..2667c10ca8 100755 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -115,6 +115,8 @@ FILE_COMPILE_FOR_SPEED #define GFORCE_FILTER_TC 0.2 #define DELAYED_REFRESH_RESUME_COMMAND (checkStickPosition(THR_HI) || checkStickPosition(PIT_HI)) +#define STATS_PAGE2 (checkStickPosition(ROL_HI)) +#define STATS_PAGE1 (checkStickPosition(ROL_LO)) #define SPLASH_SCREEN_DISPLAY_TIME 4000 // ms #define ARMED_SCREEN_DISPLAY_TIME 1500 // ms @@ -785,7 +787,7 @@ static const char * navigationStateMessage(void) break; case MW_NAV_STATE_RTH_START: return OSD_MESSAGE_STR(OSD_MSG_STARTING_RTH); - case MW_NAV_STATE_RTH_CLIMB: + case MW_NAV_STATE_RTH_CLIMB: return OSD_MESSAGE_STR(OSD_MSG_RTH_CLIMB); case MW_NAV_STATE_RTH_ENROUTE: return OSD_MESSAGE_STR(OSD_MSG_HEADING_HOME); @@ -2929,8 +2931,7 @@ static void osdUpdateStats(void) stats.max_altitude = MAX(stats.max_altitude, osdGetAltitude()); } -/* Attention: NTSC screen only has 12 fully visible lines - it is FULL now! */ -static void osdShowStats(void) +static void osdShowStatsPage1(void) { const char * disarmReasonStr[DISARM_REASON_COUNT] = { "UNKNOWN", "TIMEOUT", "STICKS", "SWITCH", "SWITCH", "KILLSW", "FAILSAFE", "NAV SYS" }; uint8_t top = 1; /* first fully visible line */ @@ -2940,8 +2941,8 @@ static void osdShowStats(void) displayBeginTransaction(osdDisplayPort, DISPLAY_TRANSACTION_OPT_RESET_DRAWING); displayClearScreen(osdDisplayPort); - if (osdDisplayIsPAL()) - displayWrite(osdDisplayPort, statNameX, top++, " --- STATS ---"); + + displayWrite(osdDisplayPort, statNameX, top++, "--- STATS --- 1/2"); if (feature(FEATURE_GPS)) { displayWrite(osdDisplayPort, statNameX, top, "MAX SPEED :"); @@ -2962,17 +2963,43 @@ static void osdShowStats(void) osdFormatAltitudeStr(buff, stats.max_altitude); displayWrite(osdDisplayPort, statValuesX, top++, buff); + displayWrite(osdDisplayPort, statNameX, top, "MIN RSSI :"); + itoa(stats.min_rssi, buff, 10); + strcat(buff, "%"); + displayWrite(osdDisplayPort, statValuesX, top++, buff); + + displayWrite(osdDisplayPort, statNameX, top, "FLY TIME :"); + uint16_t flySeconds = getFlightTime(); + uint16_t flyMinutes = flySeconds / 60; + flySeconds %= 60; + uint16_t flyHours = flyMinutes / 60; + flyMinutes %= 60; + tfp_sprintf(buff, "%02u:%02u:%02u", flyHours, flyMinutes, flySeconds); + displayWrite(osdDisplayPort, statValuesX, top++, buff); + + displayWrite(osdDisplayPort, statNameX, top, "DISARMED BY :"); + displayWrite(osdDisplayPort, statValuesX, top++, disarmReasonStr[getDisarmReason()]); + displayCommitTransaction(osdDisplayPort); +} + +static void osdShowStatsPage2(void) +{ + uint8_t top = 1; /* first fully visible line */ + const uint8_t statNameX = 1; + const uint8_t statValuesX = 20; + char buff[10]; + + displayBeginTransaction(osdDisplayPort, DISPLAY_TRANSACTION_OPT_RESET_DRAWING); + displayClearScreen(osdDisplayPort); + + displayWrite(osdDisplayPort, statNameX, top++, "--- STATS --- 2/2"); + displayWrite(osdDisplayPort, statNameX, top, "MIN BATTERY VOLT :"); osdFormatCentiNumber(buff, stats.min_voltage, 0, osdConfig()->main_voltage_decimals, 0, osdConfig()->main_voltage_decimals + 2); strcat(buff, "V"); osdLeftAlignString(buff); displayWrite(osdDisplayPort, statValuesX, top++, buff); - displayWrite(osdDisplayPort, statNameX, top, "MIN RSSI :"); - itoa(stats.min_rssi, buff, 10); - strcat(buff, "%"); - displayWrite(osdDisplayPort, statValuesX, top++, buff); - if (feature(FEATURE_CURRENT_METER)) { displayWrite(osdDisplayPort, statNameX, top, "MAX CURRENT :"); itoa(stats.max_current, buff, 10); @@ -3023,15 +3050,6 @@ static void osdShowStats(void) } } - displayWrite(osdDisplayPort, statNameX, top, "FLY TIME :"); - uint16_t flySeconds = getFlightTime(); - uint16_t flyMinutes = flySeconds / 60; - flySeconds %= 60; - uint16_t flyHours = flyMinutes / 60; - flyMinutes %= 60; - tfp_sprintf(buff, "%02u:%02u:%02u", flyHours, flyMinutes, flySeconds); - displayWrite(osdDisplayPort, statValuesX, top++, buff); - const float max_gforce = accGetMeasuredMaxG(); displayWrite(osdDisplayPort, statNameX, top, "MAX G-FORCE :"); osdFormatCentiNumber(buff, max_gforce * 100, 0, 2, 0, 3); @@ -3044,9 +3062,6 @@ static void osdShowStats(void) displayWrite(osdDisplayPort, statValuesX, top, buff); osdFormatCentiNumber(buff, acc_extremes[Z].max * 100, 0, 2, 0, 3); displayWrite(osdDisplayPort, statValuesX + 5, top++, buff); - - displayWrite(osdDisplayPort, statNameX, top, "DISARMED BY :"); - displayWrite(osdDisplayPort, statValuesX, top++, disarmReasonStr[getDisarmReason()]); displayCommitTransaction(osdDisplayPort); } @@ -3165,7 +3180,7 @@ static void osdRefresh(timeUs_t currentTimeUs) #endif osdSetNextRefreshIn(delay); } else { - osdShowStats(); // show statistic + osdShowStatsPage1(); // show first page of statistic osdSetNextRefreshIn(STATS_SCREEN_DISPLAY_TIME); } @@ -3184,6 +3199,10 @@ static void osdRefresh(timeUs_t currentTimeUs) if ((currentTimeUs > resumeRefreshAt) || ((!refreshWaitForResumeCmdRelease) && DELAYED_REFRESH_RESUME_COMMAND)) { displayClearScreen(osdDisplayPort); resumeRefreshAt = 0; + } else if ((currentTimeUs > resumeRefreshAt) || ((!refreshWaitForResumeCmdRelease) && STATS_PAGE1)) { + osdShowStatsPage1(); + } else if ((currentTimeUs > resumeRefreshAt) || ((!refreshWaitForResumeCmdRelease) && STATS_PAGE2)) { + osdShowStatsPage2(); } else { displayHeartbeat(osdDisplayPort); } @@ -3381,7 +3400,7 @@ textAttributes_t osdGetSystemMessage(char *buff, size_t buff_size, bool isCenter if (FLIGHT_MODE(NAV_RTH_MODE) || FLIGHT_MODE(NAV_WP_MODE) || navigationIsExecutingAnEmergencyLanding()) { if (NAV_Status.state == MW_NAV_STATE_WP_ENROUTE) { // Countdown display for remaining Waypoints - tfp_sprintf(messageBuf, "TO WP %u/%u", posControl.activeWaypointIndex + 1, posControl.waypointCount); + tfp_sprintf(messageBuf, "TO WP %u/%u", posControl.activeWaypointIndex + 1, posControl.waypointCount); messages[messageCount++] = messageBuf; } else if (NAV_Status.state == MW_NAV_STATE_HOLD_TIMED) { // WP hold time countdown in seconds @@ -3391,8 +3410,8 @@ textAttributes_t osdGetSystemMessage(char *buff, size_t buff_size, bool isCenter tfp_sprintf(messageBuf, "HOLDING WP FOR %2u S", holdTimeRemaining); messages[messageCount++] = messageBuf; } - } else { - const char *navStateMessage = navigationStateMessage(); + } else { + const char *navStateMessage = navigationStateMessage(); if (navStateMessage) { messages[messageCount++] = navStateMessage; } From d83084e81535b68e9ef296a64263e779796de451 Mon Sep 17 00:00:00 2001 From: OptimusTi Date: Tue, 9 Feb 2021 20:07:05 -0500 Subject: [PATCH 048/286] Update osd.c Puts Current and Power in one line and moves values one char left as suggested on #6045 --- src/main/io/osd.c | 19 ++++++++++--------- 1 file changed, 10 insertions(+), 9 deletions(-) diff --git a/src/main/io/osd.c b/src/main/io/osd.c index 2667c10ca8..5cf8792040 100755 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -2936,7 +2936,7 @@ static void osdShowStatsPage1(void) const char * disarmReasonStr[DISARM_REASON_COUNT] = { "UNKNOWN", "TIMEOUT", "STICKS", "SWITCH", "SWITCH", "KILLSW", "FAILSAFE", "NAV SYS" }; uint8_t top = 1; /* first fully visible line */ const uint8_t statNameX = 1; - const uint8_t statValuesX = 20; + const uint8_t statValuesX = 19; char buff[10]; displayBeginTransaction(osdDisplayPort, DISPLAY_TRANSACTION_OPT_RESET_DRAWING); @@ -2986,7 +2986,7 @@ static void osdShowStatsPage2(void) { uint8_t top = 1; /* first fully visible line */ const uint8_t statNameX = 1; - const uint8_t statValuesX = 20; + const uint8_t statValuesX = 19; char buff[10]; displayBeginTransaction(osdDisplayPort, DISPLAY_TRANSACTION_OPT_RESET_DRAWING); @@ -3001,15 +3001,16 @@ static void osdShowStatsPage2(void) displayWrite(osdDisplayPort, statValuesX, top++, buff); if (feature(FEATURE_CURRENT_METER)) { - displayWrite(osdDisplayPort, statNameX, top, "MAX CURRENT :"); - itoa(stats.max_current, buff, 10); + displayWrite(osdDisplayPort, statNameX, top, "MAX CURRENT/POWER:"); + osdFormatCentiNumber(buff, stats.max_current * 100, 0, 0, 0, 3); strcat(buff, "A"); - displayWrite(osdDisplayPort, statValuesX, top++, buff); - - displayWrite(osdDisplayPort, statNameX, top, "MAX POWER :"); - itoa(stats.max_power, buff, 10); + osdLeftAlignString(buff); + strcat(buff, "/"); + displayWrite(osdDisplayPort, statValuesX, top, buff); + osdFormatCentiNumber(buff, stats.max_power * 100, 0, 0, 0, 4); strcat(buff, "W"); - displayWrite(osdDisplayPort, statValuesX, top++, buff); + osdLeftAlignString(buff); + displayWrite(osdDisplayPort, statValuesX + 5, top++, buff); if (osdConfig()->stats_energy_unit == OSD_STATS_ENERGY_UNIT_MAH) { displayWrite(osdDisplayPort, statNameX, top, "USED MAH :"); From 5f1171418361963360a4a72eb283629830d5b231 Mon Sep 17 00:00:00 2001 From: OptimusTi Date: Tue, 9 Feb 2021 22:57:26 -0500 Subject: [PATCH 049/286] Update osd.c --- src/main/io/osd.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/io/osd.c b/src/main/io/osd.c index 5cf8792040..605497d3ec 100755 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -2936,7 +2936,7 @@ static void osdShowStatsPage1(void) const char * disarmReasonStr[DISARM_REASON_COUNT] = { "UNKNOWN", "TIMEOUT", "STICKS", "SWITCH", "SWITCH", "KILLSW", "FAILSAFE", "NAV SYS" }; uint8_t top = 1; /* first fully visible line */ const uint8_t statNameX = 1; - const uint8_t statValuesX = 19; + const uint8_t statValuesX = 20; char buff[10]; displayBeginTransaction(osdDisplayPort, DISPLAY_TRANSACTION_OPT_RESET_DRAWING); @@ -2986,7 +2986,7 @@ static void osdShowStatsPage2(void) { uint8_t top = 1; /* first fully visible line */ const uint8_t statNameX = 1; - const uint8_t statValuesX = 19; + const uint8_t statValuesX = 20; char buff[10]; displayBeginTransaction(osdDisplayPort, DISPLAY_TRANSACTION_OPT_RESET_DRAWING); From 3706a0928bf9b53547018f812f01ffe9fd560621 Mon Sep 17 00:00:00 2001 From: breadoven <56191411+breadoven@users.noreply.github.com> Date: Wed, 10 Feb 2021 22:45:30 +0000 Subject: [PATCH 050/286] Update navigation.c Remove white space --- src/main/navigation/navigation.c | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c index 7c8652d44d..4a9677bb53 100755 --- a/src/main/navigation/navigation.c +++ b/src/main/navigation/navigation.c @@ -1155,7 +1155,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_INITIALIZE(navigati // Spiral climb centered at xy of RTH activation calculateInitialHoldPosition(&targetHoldPos); } else { - calculateFarAwayTarget(&targetHoldPos, posControl.actualState.yaw, 100000.0f); // 1km away Linear climb + calculateFarAwayTarget(&targetHoldPos, posControl.actualState.yaw, 100000.0f); // 1km away Linear climb } } else { // Multicopter, hover and climb @@ -2003,10 +2003,10 @@ float navPidApply3( pid->integrator = newIntegrator; } } - + if (pidFlags & PID_LIMIT_INTEGRATOR) { pid->integrator = constrainf(pid->integrator, outMin, outMax); - } + } return outValConstrained; } From 49be35296c2de70c77342ef5064d0fb76703f504 Mon Sep 17 00:00:00 2001 From: breadoven <56191411+breadoven@users.noreply.github.com> Date: Wed, 10 Feb 2021 22:55:42 +0000 Subject: [PATCH 051/286] Update rth_estimator.c Remove white space --- src/main/flight/rth_estimator.c | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/src/main/flight/rth_estimator.c b/src/main/flight/rth_estimator.c index db6a41adce..7bae4fa731 100644 --- a/src/main/flight/rth_estimator.c +++ b/src/main/flight/rth_estimator.c @@ -208,27 +208,27 @@ float calculateRemainingFlightTimeBeforeRTH(bool takeWindIntoAccount) { // returns meters float calculateRemainingDistanceBeforeRTH(bool takeWindIntoAccount) { - + // Fixed wing only for now if (!(STATE(FIXED_WING_LEGACY) || ARMING_FLAG(ARMED))) { return -1; } - + #ifdef USE_WIND_ESTIMATOR if (takeWindIntoAccount && !isEstimatedWindSpeedValid()) { return -1; } #endif - + // check requirements const bool areBatterySettingsOK = feature(FEATURE_VBAT) && feature(FEATURE_CURRENT_METER) && batteryWasFullWhenPluggedIn(); - const bool areRTHEstimatorSettingsOK = batteryMetersConfig()->cruise_power > 0 && currentBatteryProfile->capacity.unit == BAT_CAPACITY_UNIT_MWH &¤tBatteryProfile->capacity.value > 0 && navConfig()->fw.cruise_speed > 0; + const bool areRTHEstimatorSettingsOK = batteryMetersConfig()->cruise_power > 0 && currentBatteryProfile->capacity.unit == BAT_CAPACITY_UNIT_MWH &¤tBatteryProfile->capacity.value > 0 && navConfig()->fw.cruise_speed > 0; const bool isNavigationOK = navigationPositionEstimateIsHealthy() && isImuHeadingValid(); - + if (!(areBatterySettingsOK && areRTHEstimatorSettingsOK && isNavigationOK)) { return -1; } - + const float remainingFlightTimeBeforeRTH = calculateRemainingFlightTimeBeforeRTH(takeWindIntoAccount); // error: return error code directly From 0a93b8b64ad58cded87dbcbe6282995c0835b5b4 Mon Sep 17 00:00:00 2001 From: breadoven <56191411+breadoven@users.noreply.github.com> Date: Wed, 10 Feb 2021 23:20:05 +0000 Subject: [PATCH 052/286] Fix white space --- src/main/fc/settings.yaml | 6 +++--- src/main/navigation/navigation.c | 2 +- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index d04e5013f6..36eabf5f55 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -2135,12 +2135,12 @@ groups: description: "Configure how the aircraft will manage altitude on the way home, see Navigation modes on wiki for more details" default_value: "AT_LEAST" field: general.flags.rth_alt_control_mode - table: nav_rth_alt_mode + table: nav_rth_alt_mode - name: nav_rth_alt_control_override description: "If set to ON RTH altitude and CLIMB FIRST settings can be overridden during the RTH climb phase using full pitch or roll stick held for > 2 seconds. RTH altitude is reset to the current altitude using pitch down stick. RTH CLIMB FIRST is overridden using right roll stick so craft turns and heads directly to home." default_value: "OFF" - field: general.flags.rth_alt_control_override - type: bool + field: general.flags.rth_alt_control_override + type: bool - name: nav_rth_abort_threshold description: "RTH sanity checking feature will notice if distance to home is increasing during RTH and once amount of increase exceeds the threshold defined by this parameter, instead of continuing RTH machine will enter emergency landing, self-level and go down safely. Default is 500m which is safe enough for both multirotor machines and airplanes. [cm]" default_value: "50000" diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c index 90786f92a5..5c05bf5e68 100755 --- a/src/main/navigation/navigation.c +++ b/src/main/navigation/navigation.c @@ -2579,7 +2579,7 @@ static bool rthAltControlStickOverrideCheck(unsigned axis) } else if (ABS(2500 - holdTime) < 500) { if (axis == PITCH) { // PITCH down to override preset altitude reset to current altitude posControl.rthState.rthInitialAltitude = posControl.actualState.abs.pos.z; - posControl.rthState.rthFinalAltitude = posControl.rthState.rthInitialAltitude; + posControl.rthState.rthFinalAltitude = posControl.rthState.rthInitialAltitude; return true; } else if (axis == ROLL) { // ROLL right to override climb first return true; From 585e559b618bcca607a01c900f8a9e455d22d3b2 Mon Sep 17 00:00:00 2001 From: breadoven <56191411+breadoven@users.noreply.github.com> Date: Thu, 11 Feb 2021 00:02:22 +0000 Subject: [PATCH 053/286] Revert "Update navigation.c" This reverts commit 3706a0928bf9b53547018f812f01ffe9fd560621. --- src/main/navigation/navigation.c | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c index 4a9677bb53..7c8652d44d 100755 --- a/src/main/navigation/navigation.c +++ b/src/main/navigation/navigation.c @@ -1155,7 +1155,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_INITIALIZE(navigati // Spiral climb centered at xy of RTH activation calculateInitialHoldPosition(&targetHoldPos); } else { - calculateFarAwayTarget(&targetHoldPos, posControl.actualState.yaw, 100000.0f); // 1km away Linear climb + calculateFarAwayTarget(&targetHoldPos, posControl.actualState.yaw, 100000.0f); // 1km away Linear climb } } else { // Multicopter, hover and climb @@ -2003,10 +2003,10 @@ float navPidApply3( pid->integrator = newIntegrator; } } - + if (pidFlags & PID_LIMIT_INTEGRATOR) { pid->integrator = constrainf(pid->integrator, outMin, outMax); - } + } return outValConstrained; } From e48b762d7a72c7c196a3feafc31f8eca044311bf Mon Sep 17 00:00:00 2001 From: breadoven <56191411+breadoven@users.noreply.github.com> Date: Thu, 11 Feb 2021 00:05:36 +0000 Subject: [PATCH 054/286] Update navigation.c Trim white space --- src/main/navigation/navigation.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c index 7c8652d44d..038ebc9264 100755 --- a/src/main/navigation/navigation.c +++ b/src/main/navigation/navigation.c @@ -1155,7 +1155,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_INITIALIZE(navigati // Spiral climb centered at xy of RTH activation calculateInitialHoldPosition(&targetHoldPos); } else { - calculateFarAwayTarget(&targetHoldPos, posControl.actualState.yaw, 100000.0f); // 1km away Linear climb + calculateFarAwayTarget(&targetHoldPos, posControl.actualState.yaw, 100000.0f); // 1km away Linear climb } } else { // Multicopter, hover and climb From d5843958c04886a8cb7cec76202cf2b2f9aaebba Mon Sep 17 00:00:00 2001 From: OptimusTi Date: Thu, 11 Feb 2021 09:37:44 -0500 Subject: [PATCH 055/286] Replaced unit strings to symbols To keep inline with other stats --- src/main/io/osd.c | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/main/io/osd.c b/src/main/io/osd.c index 605497d3ec..d39cebe1bf 100755 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -2996,19 +2996,19 @@ static void osdShowStatsPage2(void) displayWrite(osdDisplayPort, statNameX, top, "MIN BATTERY VOLT :"); osdFormatCentiNumber(buff, stats.min_voltage, 0, osdConfig()->main_voltage_decimals, 0, osdConfig()->main_voltage_decimals + 2); - strcat(buff, "V"); + strcat(buff, "\x06"); osdLeftAlignString(buff); displayWrite(osdDisplayPort, statValuesX, top++, buff); if (feature(FEATURE_CURRENT_METER)) { displayWrite(osdDisplayPort, statNameX, top, "MAX CURRENT/POWER:"); osdFormatCentiNumber(buff, stats.max_current * 100, 0, 0, 0, 3); - strcat(buff, "A"); + strcat(buff, "\x9A"); osdLeftAlignString(buff); strcat(buff, "/"); displayWrite(osdDisplayPort, statValuesX, top, buff); osdFormatCentiNumber(buff, stats.max_power * 100, 0, 0, 0, 4); - strcat(buff, "W"); + strcat(buff, "\xAE"); osdLeftAlignString(buff); displayWrite(osdDisplayPort, statValuesX + 5, top++, buff); From 7d8317cd19534017b186b640194c2c53026ae546 Mon Sep 17 00:00:00 2001 From: OptimusTi Date: Thu, 11 Feb 2021 10:36:53 -0500 Subject: [PATCH 056/286] Reverted to old format for Amps/Watts Reverted to separate lines for these values. Will work on reformatting on a separate PR --- src/main/io/osd.c | 15 +++++++-------- 1 file changed, 7 insertions(+), 8 deletions(-) diff --git a/src/main/io/osd.c b/src/main/io/osd.c index d39cebe1bf..b99d79a155 100755 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -3001,16 +3001,15 @@ static void osdShowStatsPage2(void) displayWrite(osdDisplayPort, statValuesX, top++, buff); if (feature(FEATURE_CURRENT_METER)) { - displayWrite(osdDisplayPort, statNameX, top, "MAX CURRENT/POWER:"); - osdFormatCentiNumber(buff, stats.max_current * 100, 0, 0, 0, 3); + displayWrite(osdDisplayPort, statNameX, top, "MAX CURRENT :"); + itoa(stats.max_current, buff, 10); strcat(buff, "\x9A"); - osdLeftAlignString(buff); - strcat(buff, "/"); - displayWrite(osdDisplayPort, statValuesX, top, buff); - osdFormatCentiNumber(buff, stats.max_power * 100, 0, 0, 0, 4); + displayWrite(osdDisplayPort, statValuesX, top++, buff); + + displayWrite(osdDisplayPort, statNameX, top, "MAX POWER :"); + itoa(stats.max_power, buff, 10); strcat(buff, "\xAE"); - osdLeftAlignString(buff); - displayWrite(osdDisplayPort, statValuesX + 5, top++, buff); + displayWrite(osdDisplayPort, statValuesX, top++, buff); if (osdConfig()->stats_energy_unit == OSD_STATS_ENERGY_UNIT_MAH) { displayWrite(osdDisplayPort, statNameX, top, "USED MAH :"); From d2dc28e084956138d0dbbf6909a08678eb2d70f9 Mon Sep 17 00:00:00 2001 From: OptimusTi Date: Thu, 11 Feb 2021 19:02:00 -0500 Subject: [PATCH 057/286] Using symbol name instead of hex --- src/main/io/osd.c | 13 ++++++------- 1 file changed, 6 insertions(+), 7 deletions(-) diff --git a/src/main/io/osd.c b/src/main/io/osd.c index b99d79a155..edda1479cf 100755 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -2996,19 +2996,18 @@ static void osdShowStatsPage2(void) displayWrite(osdDisplayPort, statNameX, top, "MIN BATTERY VOLT :"); osdFormatCentiNumber(buff, stats.min_voltage, 0, osdConfig()->main_voltage_decimals, 0, osdConfig()->main_voltage_decimals + 2); - strcat(buff, "\x06"); - osdLeftAlignString(buff); + tfp_sprintf(buff, "%s%c", buff, SYM_VOLT); displayWrite(osdDisplayPort, statValuesX, top++, buff); if (feature(FEATURE_CURRENT_METER)) { displayWrite(osdDisplayPort, statNameX, top, "MAX CURRENT :"); itoa(stats.max_current, buff, 10); - strcat(buff, "\x9A"); + tfp_sprintf(buff, "%s%c", buff, SYM_AMP); displayWrite(osdDisplayPort, statValuesX, top++, buff); displayWrite(osdDisplayPort, statNameX, top, "MAX POWER :"); itoa(stats.max_power, buff, 10); - strcat(buff, "\xAE"); + tfp_sprintf(buff, "%s%c", buff, SYM_WATT); displayWrite(osdDisplayPort, statValuesX, top++, buff); if (osdConfig()->stats_energy_unit == OSD_STATS_ENERGY_UNIT_MAH) { @@ -3017,7 +3016,7 @@ static void osdShowStatsPage2(void) } else { displayWrite(osdDisplayPort, statNameX, top, "USED WH :"); osdFormatCentiNumber(buff, getMWhDrawn() / 10, 0, 2, 0, 3); - strcat(buff, "\xAB"); // SYM_WH + tfp_sprintf(buff, "%s%c", buff, SYM_WH); } displayWrite(osdDisplayPort, statValuesX, top++, buff); @@ -3059,9 +3058,9 @@ static void osdShowStatsPage2(void) displayWrite(osdDisplayPort, statNameX, top, "MIN/MAX Z G-FORCE:"); osdFormatCentiNumber(buff, acc_extremes[Z].min * 100, 0, 2, 0, 4); strcat(buff,"/"); - displayWrite(osdDisplayPort, statValuesX, top, buff); + displayWrite(osdDisplayPort, statValuesX - 1, top, buff); osdFormatCentiNumber(buff, acc_extremes[Z].max * 100, 0, 2, 0, 3); - displayWrite(osdDisplayPort, statValuesX + 5, top++, buff); + displayWrite(osdDisplayPort, statValuesX + 4, top++, buff); displayCommitTransaction(osdDisplayPort); } From 3a599d98423a72abae7bfec553fa7ad30d42f6b1 Mon Sep 17 00:00:00 2001 From: Tony Yeung Date: Tue, 23 Feb 2021 00:02:59 +0000 Subject: [PATCH 058/286] New safehome options to control when safehomes are applied --- docs/Safehomes.md | 12 ++++-- docs/Settings.md | 1 + src/main/cms/cms_menu_navigation.c | 1 + src/main/fc/settings.yaml | 8 ++++ src/main/flight/failsafe.c | 14 ++++--- src/main/io/osd.c | 14 +++---- src/main/navigation/navigation.c | 64 ++++++++++++++++++++---------- src/main/navigation/navigation.h | 15 +++++-- 8 files changed, 89 insertions(+), 40 deletions(-) diff --git a/docs/Safehomes.md b/docs/Safehomes.md index b3ed76403c..6402d8275b 100644 --- a/docs/Safehomes.md +++ b/docs/Safehomes.md @@ -12,9 +12,13 @@ One potential risk when landing is that there might be buildings, trees and othe ## Safehome -Safehomes are a list of GPS coordinates that identify safe landing points. When the flight controller is armed, it checks the list of safehomes. The nearest safehome that is enabled and within ```safehome_max_distance``` (default 200m) of the current position will be selected. Otherwise, it reverts to the old behaviour of using your current GPS position as home. +Safehomes are a list of GPS coordinates that identify safe landing points. When the flight controller is armed, it checks the list of safehomes. The nearest safehome that is enabled and within ```safehome_max_distance``` (default 200m) of the current position will be remembered. The arming home location remains as home. -Be aware that the safehome replaces your arming position as home. When flying, RTH will return to the safehome and OSD elements such as distance to home and direction to home will refer to the selected safehome. +When RTH is activated, whether by radio failsafe, or using the RTH radio control mode, the safehome identified during arming will replace the arming home. If RTH is turned off, either by regaining radio control or turning off the RTH radio control mode, the home location will return to arming point. + +The safehome operating mode is set using ```safehome_usage_mode```. If ```OFF```, safehomes will not be used. If ```RTH```, the safehome will replace the arming location when RTH is activated, either manually or because of RX failsafe. If ```RTH_FS```, the safehome will only be used for RX failsafe. This option can be changed using the OSD menu. + +This behavior has changed from the initial release, where the safehome location replaced the arming location during the arming phase. That would result in flight information involving home (home distance, home bearing, etc) using the safehome, instead of the arming location. You can define up to 8 safehomes for different locations you fly at. @@ -36,8 +40,8 @@ If a safehome is selected, an additional message appears: CURRENT DATE CURRENT TIME ``` -The GPS details are those of the selected safehome. -To draw your attention to "HOME" being replaced, the message flashes and stays visible longer. +The GPS details are those of the arming location, not the safehome. +To draw your attention to a safehome being selected, the message flashes and stays visible longer. ## CLI command `safehome` to manage safehomes diff --git a/docs/Settings.md b/docs/Settings.md index e0a8c52242..5ce43ec94c 100644 --- a/docs/Settings.md +++ b/docs/Settings.md @@ -444,6 +444,7 @@ | rx_spi_protocol | | | | rx_spi_rf_channel_count | | | | safehome_max_distance | 20000 | In order for a safehome to be used, it must be less than this distance (in cm) from the arming point. | +| safehome_usage_mode | RTH | Used to control when safehomes will be used. | | sbus_sync_interval | | | | sdcard_detect_inverted | `TARGET dependent` | This setting drives the way SD card is detected in card slot. On some targets (AnyFC F7 clone) different card slot was used and depending of hardware revision ON or OFF setting might be required. If card is not detected, change this value. | | serialrx_halfduplex | AUTO | Allow serial receiver to operate on UART TX pin. With some receivers will allow control and telemetry over a single wire. | diff --git a/src/main/cms/cms_menu_navigation.c b/src/main/cms/cms_menu_navigation.c index d17d3df290..3563889474 100644 --- a/src/main/cms/cms_menu_navigation.c +++ b/src/main/cms/cms_menu_navigation.c @@ -81,6 +81,7 @@ static const CMS_Menu cmsx_menuNavSettings = { OSD_SETTING_ENTRY("MIN RTH DISTANCE", SETTING_NAV_MIN_RTH_DISTANCE), OSD_SETTING_ENTRY("RTH ABORT THRES", SETTING_NAV_RTH_ABORT_THRESHOLD), OSD_SETTING_ENTRY("EMERG LANDING SPEED", SETTING_NAV_EMERG_LANDING_SPEED), + OSD_SETTING_ENTRY("SAFEHOME USAGE MODE", SETTING_SAFEHOME_USAGE_MODE), OSD_BACK_AND_END_ENTRY, }; diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index a49d69ff33..e08b25a6a6 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -157,6 +157,9 @@ tables: values: ["OFF_ALWAYS", "OFF", "AUTO_ONLY", "ALL_NAV"] - name: osd_plus_code_short values: ["0", "2", "4", "6"] + - name: safehome_usage_mode + values: ["OFF", "RTH", "RTH_FS"] + enum: safehomeUsageMode_e groups: - name: PG_GYRO_CONFIG @@ -2191,6 +2194,11 @@ groups: field: general.safehome_max_distance min: 0 max: 65000 + - name: safehome_usage_mode + description: "Used to control when safehomes will be used." + default_value: "RTH" + field: general.flags.safehome_usage_mode + table: safehome_usage_mode - name: nav_mc_bank_angle description: "Maximum banking angle (deg) that multicopter navigation is allowed to set. Machine must be able to satisfy this angle without loosing altitude" default_value: "30" diff --git a/src/main/flight/failsafe.c b/src/main/flight/failsafe.c index 6ac2783c18..8e16842e70 100644 --- a/src/main/flight/failsafe.c +++ b/src/main/flight/failsafe.c @@ -46,6 +46,7 @@ #include "flight/pid.h" #include "navigation/navigation.h" +#include "navigation/navigation_private.h" #include "rx/rx.h" @@ -361,12 +362,15 @@ static failsafeProcedure_e failsafeChooseFailsafeProcedure(void) // Craft is closer than minimum failsafe procedure distance (if set to non-zero) // GPS must also be working, and home position set - if ((failsafeConfig()->failsafe_min_distance > 0) && - ((GPS_distanceToHome * 100) < failsafeConfig()->failsafe_min_distance) && - sensors(SENSOR_GPS) && STATE(GPS_FIX) && STATE(GPS_FIX_HOME)) { + if (failsafeConfig()->failsafe_min_distance > 0 && + sensors(SENSOR_GPS) && STATE(GPS_FIX) && STATE(GPS_FIX_HOME)) { - // Use the alternate, minimum distance failsafe procedure instead - return failsafeConfig()->failsafe_min_distance_procedure; + // get the distance to the original arming point + uint32_t distance = calculateDistanceToDestination(&original_rth_home); + if (distance < failsafeConfig()->failsafe_min_distance) { + // Use the alternate, minimum distance failsafe procedure instead + return failsafeConfig()->failsafe_min_distance_procedure; + } } return failsafeConfig()->failsafe_procedure; diff --git a/src/main/io/osd.c b/src/main/io/osd.c index 3612ef00d5..f1cfe24ea6 100755 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -785,7 +785,7 @@ static const char * navigationStateMessage(void) break; case MW_NAV_STATE_RTH_START: return OSD_MESSAGE_STR(OSD_MSG_STARTING_RTH); - case MW_NAV_STATE_RTH_CLIMB: + case MW_NAV_STATE_RTH_CLIMB: return OSD_MESSAGE_STR(OSD_MSG_RTH_CLIMB); case MW_NAV_STATE_RTH_ENROUTE: return OSD_MESSAGE_STR(OSD_MSG_HEADING_HOME); @@ -3086,11 +3086,11 @@ static void osdShowArmed(void) } y += 4; #if defined (USE_SAFE_HOME) - if (isSafeHomeInUse()) { + if (safehome_distance) { textAttributes_t elemAttr = _TEXT_ATTRIBUTES_BLINK_BIT; char buf2[12]; // format the distance first osdFormatDistanceStr(buf2, safehome_distance); - tfp_sprintf(buf, "%c - %s -> SAFEHOME %u", SYM_HOME, buf2, safehome_used); + tfp_sprintf(buf, "%c - %s -> SAFEHOME %u", SYM_HOME, buf2, safehome_index); // write this message above the ARMED message to make it obvious displayWriteWithAttr(osdDisplayPort, (osdDisplayPort->cols - strlen(buf)) / 2, y - 8, buf, elemAttr); } @@ -3160,7 +3160,7 @@ static void osdRefresh(timeUs_t currentTimeUs) osdShowArmed(); // reset statistic etc uint32_t delay = ARMED_SCREEN_DISPLAY_TIME; #if defined(USE_SAFE_HOME) - if (isSafeHomeInUse()) + if (safehome_distance) delay *= 3; #endif osdSetNextRefreshIn(delay); @@ -3381,7 +3381,7 @@ textAttributes_t osdGetSystemMessage(char *buff, size_t buff_size, bool isCenter if (FLIGHT_MODE(NAV_RTH_MODE) || FLIGHT_MODE(NAV_WP_MODE) || navigationIsExecutingAnEmergencyLanding()) { if (NAV_Status.state == MW_NAV_STATE_WP_ENROUTE) { // Countdown display for remaining Waypoints - tfp_sprintf(messageBuf, "TO WP %u/%u", posControl.activeWaypointIndex + 1, posControl.waypointCount); + tfp_sprintf(messageBuf, "TO WP %u/%u", posControl.activeWaypointIndex + 1, posControl.waypointCount); messages[messageCount++] = messageBuf; } else if (NAV_Status.state == MW_NAV_STATE_HOLD_TIMED) { // WP hold time countdown in seconds @@ -3391,8 +3391,8 @@ textAttributes_t osdGetSystemMessage(char *buff, size_t buff_size, bool isCenter tfp_sprintf(messageBuf, "HOLDING WP FOR %2u S", holdTimeRemaining); messages[messageCount++] = messageBuf; } - } else { - const char *navStateMessage = navigationStateMessage(); + } else { + const char *navStateMessage = navigationStateMessage(); if (navStateMessage) { messages[messageCount++] = navStateMessage; } diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c index 01e83e70b7..cf1b7c1ea6 100755 --- a/src/main/navigation/navigation.c +++ b/src/main/navigation/navigation.c @@ -73,10 +73,14 @@ gpsLocation_t GPS_home; uint32_t GPS_distanceToHome; // distance to home point in meters int16_t GPS_directionToHome; // direction to home point in degrees +fpVector3_t original_rth_home; // the original rth home - save it, since it could be replaced by safehome or HOME_RESET + radar_pois_t radar_pois[RADAR_MAX_POIS]; #if defined(USE_SAFE_HOME) -int8_t safehome_used; // -1 if no safehome, 0 to MAX_SAFEHOMES -1 otherwise -uint32_t safehome_distance; // distance to the selected safehome +int8_t safehome_index; // -1 if no safehome, 0 to MAX_SAFEHOMES -1 otherwise +uint32_t safehome_distance = 0; // distance to the nearest safehome +fpVector3_t nearestSafeHome; // The nearestSafeHome found during arming +bool safehome_applied = false; // whether the safehome has been applied to home. PG_REGISTER_ARRAY(navSafeHome_t, MAX_SAFE_HOMES, safeHomeConfig, PG_SAFE_HOME_CONFIG , 0); @@ -107,6 +111,7 @@ PG_RESET_TEMPLATE(navConfig_t, navConfig, .disarm_on_landing = 0, .rth_allow_landing = NAV_RTH_ALLOW_LANDING_ALWAYS, .nav_overrides_motor_stop = NOMS_ALL_NAV, + .safehome_usage_mode = SAFEHOME_USAGE_RTH, }, // General navigation parameters @@ -2327,26 +2332,40 @@ static navigationHomeFlags_t navigationActualStateHomeValidity(void) #if defined(USE_SAFE_HOME) -/******************************************************* - * Is a safehome being used instead of the arming point? - *******************************************************/ -bool isSafeHomeInUse(void) +void checkSafeHomeState(bool shouldBeEnabled) { - return (safehome_used > -1 && safehome_used < MAX_SAFE_HOMES); + if (navConfig()->general.flags.safehome_usage_mode == SAFEHOME_USAGE_OFF) { + shouldBeEnabled = false; + } else if (navConfig()->general.flags.safehome_usage_mode == SAFEHOME_USAGE_RTH_FS && shouldBeEnabled) { + // if safehomes are only used with failsafe and we're trying to enable safehome + // then enable the safehome only with failsafe + shouldBeEnabled = posControl.flags.forcedRTHActivated; + } + // no safe homes found when arming or safehome feature in the correct state, then we don't need to do anything + if (safehome_distance == 0 || (safehome_applied == shouldBeEnabled)) { + return; + } + if (shouldBeEnabled) { + // set home to safehome + setHomePosition(&nearestSafeHome, 0, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING, navigationActualStateHomeValidity()); + safehome_applied = true; + } else { + // set home to original arming point + setHomePosition(&original_rth_home, 0, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING, navigationActualStateHomeValidity()); + safehome_applied = false; + } } /*********************************************************** * See if there are any safehomes near where we are arming. - * If so, use it instead of the arming point for home. - * Select the nearest safehome + * If so, save the nearest one in case we need it later for RTH. **********************************************************/ -bool foundNearbySafeHome(void) +bool findNearestSafeHome(void) { - safehome_used = -1; + safehome_index = -1; uint32_t nearest_safehome_distance = navConfig()->general.safehome_max_distance + 1; uint32_t distance_to_current; fpVector3_t currentSafeHome; - fpVector3_t nearestSafeHome; gpsLocation_t shLLH; shLLH.alt = 0; for (uint8_t i = 0; i < MAX_SAFE_HOMES; i++) { @@ -2359,20 +2378,19 @@ bool foundNearbySafeHome(void) distance_to_current = calculateDistanceToDestination(¤tSafeHome); if (distance_to_current < nearest_safehome_distance) { // this safehome is the nearest so far - keep track of it. - safehome_used = i; + safehome_index = i; nearest_safehome_distance = distance_to_current; nearestSafeHome.x = currentSafeHome.x; nearestSafeHome.y = currentSafeHome.y; nearestSafeHome.z = currentSafeHome.z; } } - if (safehome_used >= 0) { + if (safehome_index >= 0) { safehome_distance = nearest_safehome_distance; - setHomePosition(&nearestSafeHome, 0, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING, navigationActualStateHomeValidity()); - return true; + } else { + safehome_distance = 0; } - safehome_distance = 0; - return false; + return safehome_distance > 0; } #endif @@ -2398,9 +2416,13 @@ void updateHomePosition(void) } if (setHome) { #if defined(USE_SAFE_HOME) - if (!foundNearbySafeHome()) + findNearestSafeHome(); #endif - setHomePosition(&posControl.actualState.abs.pos, posControl.actualState.yaw, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING, navigationActualStateHomeValidity()); + setHomePosition(&posControl.actualState.abs.pos, posControl.actualState.yaw, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING, navigationActualStateHomeValidity()); + // save the current location in case it is replaced by a safehome or HOME_RESET + original_rth_home.x = posControl.rthState.homePosition.pos.x; + original_rth_home.y = posControl.rthState.homePosition.pos.y; + original_rth_home.z = posControl.rthState.homePosition.pos.z; } } } @@ -3109,6 +3131,7 @@ static navigationFSMEvent_t selectNavEventFromBoxModeInput(void) const bool canActivatePosHold = canActivatePosHoldMode(); const bool canActivateNavigation = canActivateNavigationModes(); const bool isExecutingRTH = navGetStateFlags(posControl.navState) & NAV_AUTO_RTH; + checkSafeHomeState(isExecutingRTH); // Keep canActivateWaypoint flag at FALSE if there is no mission loaded // Also block WP mission if we are executing RTH @@ -3550,6 +3573,7 @@ void abortForcedRTH(void) // Disable failsafe RTH and make sure we back out of navigation mode to IDLE // If any navigation mode was active prior to RTH it will be re-enabled with next RX update posControl.flags.forcedRTHActivated = false; + checkSafeHomeState(false); navProcessFSMEvents(NAV_FSM_EVENT_SWITCH_TO_IDLE); } diff --git a/src/main/navigation/navigation.h b/src/main/navigation/navigation.h index 50ba6b2632..59c784e402 100755 --- a/src/main/navigation/navigation.h +++ b/src/main/navigation/navigation.h @@ -35,6 +35,7 @@ extern gpsLocation_t GPS_home; extern uint32_t GPS_distanceToHome; // distance to home point in meters extern int16_t GPS_directionToHome; // direction to home point in degrees +extern fpVector3_t original_rth_home; // the original rth home - save it, since it could be replaced by safehome or HOME_RESET extern bool autoThrottleManuallyIncreased; @@ -50,14 +51,19 @@ typedef struct { int32_t lon; } navSafeHome_t; +typedef enum { + SAFEHOME_USAGE_OFF = 0, // Don't use safehomes + SAFEHOME_USAGE_RTH = 1, // Default - use safehome for RTH + SAFEHOME_USAGE_RTH_FS = 2, // Use safehomes for RX failsafe only +} safehomeUsageMode_e; + PG_DECLARE_ARRAY(navSafeHome_t, MAX_SAFE_HOMES, safeHomeConfig); -extern int8_t safehome_used; // -1 if no safehome, 0 to MAX_SAFEHOMES -1 otherwise -extern uint32_t safehome_distance; // distance to the selected safehome +extern int8_t safehome_index; // -1 if no safehome, 0 to MAX_SAFEHOMES -1 otherwise +extern uint32_t safehome_distance; // distance to the nearest safehome void resetSafeHomes(void); // remove all safehomes -bool isSafeHomeInUse(void); // Are we using a safehome instead of the arming point? -bool foundNearbySafeHome(void); // Did we find a safehome nearby? +bool findNearestSafeHome(void); // Find nearest safehome #endif // defined(USE_SAFE_HOME) @@ -184,6 +190,7 @@ typedef struct navConfig_s { uint8_t rth_allow_landing; // Enable landing as last stage of RTH. Use constants in navRTHAllowLanding_e. uint8_t rth_climb_ignore_emerg; // Option to ignore GPS loss on initial climb stage of RTH uint8_t nav_overrides_motor_stop; // Autonomous modes override motor_stop setting and user command to stop motor + uint8_t safehome_usage_mode; // Controls when safehomes are used } flags; uint8_t pos_failure_timeout; // Time to wait before switching to emergency landing (0 - disable) From 3aa2edf5d7913ae8f2c545647cd00d263365eb13 Mon Sep 17 00:00:00 2001 From: Tony Yeung Date: Tue, 23 Feb 2021 00:35:27 +0000 Subject: [PATCH 059/286] cleanup --- src/main/navigation/navigation.c | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c index cf1b7c1ea6..9e48563ee7 100755 --- a/src/main/navigation/navigation.c +++ b/src/main/navigation/navigation.c @@ -77,7 +77,7 @@ fpVector3_t original_rth_home; // the original rth home - save it, sin radar_pois_t radar_pois[RADAR_MAX_POIS]; #if defined(USE_SAFE_HOME) -int8_t safehome_index; // -1 if no safehome, 0 to MAX_SAFEHOMES -1 otherwise +int8_t safehome_index = -1; // -1 if no safehome, 0 to MAX_SAFEHOMES -1 otherwise uint32_t safehome_distance = 0; // distance to the nearest safehome fpVector3_t nearestSafeHome; // The nearestSafeHome found during arming bool safehome_applied = false; // whether the safehome has been applied to home. @@ -2416,7 +2416,9 @@ void updateHomePosition(void) } if (setHome) { #if defined(USE_SAFE_HOME) - findNearestSafeHome(); + if (navConfig()->general.flags.safehome_usage_mode != SAFEHOME_USAGE_OFF) { + findNearestSafeHome(); + } #endif setHomePosition(&posControl.actualState.abs.pos, posControl.actualState.yaw, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING, navigationActualStateHomeValidity()); // save the current location in case it is replaced by a safehome or HOME_RESET From aba1790764662969d2ff0da1c4e75fe98361416b Mon Sep 17 00:00:00 2001 From: Tony Yeung Date: Wed, 24 Feb 2021 02:05:36 +0000 Subject: [PATCH 060/286] add OSD messages --- src/main/io/osd.c | 22 ++++++++++++++++++++++ src/main/io/osd.h | 6 +++++- src/main/navigation/navigation.h | 1 + 3 files changed, 28 insertions(+), 1 deletion(-) diff --git a/src/main/io/osd.c b/src/main/io/osd.c index f1cfe24ea6..06b3eedf8e 100755 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -777,6 +777,15 @@ static const char * osdFailsafeInfoMessage(void) } return OSD_MESSAGE_STR(OSD_MSG_RC_RX_LINK_LOST); } +#if defined(USE_SAFE_HOME) +static const char * divertingToSafehomeMessage(void) +{ + if (safehome_applied) { + return OSD_MESSAGE_STR(OSD_MSG_DIVERT_SAFEHOME); + } + return NULL; +} +#endif static const char * navigationStateMessage(void) { @@ -3354,6 +3363,7 @@ textAttributes_t osdGetSystemMessage(char *buff, size_t buff_size, bool isCenter const char *failsafePhaseMessage = osdFailsafePhaseMessage(); const char *failsafeInfoMessage = osdFailsafeInfoMessage(); const char *navStateFSMessage = navigationStateMessage(); + if (failsafePhaseMessage) { messages[messageCount++] = failsafePhaseMessage; } @@ -3363,6 +3373,12 @@ textAttributes_t osdGetSystemMessage(char *buff, size_t buff_size, bool isCenter if (navStateFSMessage) { messages[messageCount++] = navStateFSMessage; } +#if defined(USE_SAFE_HOME) + const char *safehomeMessage = divertingToSafehomeMessage(); + if (safehomeMessage) { + messages[messageCount++] = safehomeMessage; + } +#endif if (messageCount > 0) { message = messages[OSD_ALTERNATING_CHOICES(1000, messageCount)]; if (message == failsafeInfoMessage) { @@ -3397,6 +3413,12 @@ textAttributes_t osdGetSystemMessage(char *buff, size_t buff_size, bool isCenter messages[messageCount++] = navStateMessage; } } +#if defined(USE_SAFE_HOME) + const char *safehomeMessage = divertingToSafehomeMessage(); + if (safehomeMessage) { + messages[messageCount++] = safehomeMessage; + } +#endif } else if (STATE(FIXED_WING_LEGACY) && (navGetCurrentStateFlags() & NAV_CTL_LAUNCH)) { messages[messageCount++] = OSD_MESSAGE_STR(OSD_MSG_AUTOLAUNCH); const char *launchStateMessage = fixedWingLaunchStateMessage(); diff --git a/src/main/io/osd.h b/src/main/io/osd.h index d740b5de52..0433a926fa 100755 --- a/src/main/io/osd.h +++ b/src/main/io/osd.h @@ -98,6 +98,10 @@ #define OSD_MSG_HEADFREE "(HEADFREE)" #define OSD_MSG_UNABLE_ARM "UNABLE TO ARM" +#if defined(USE_SAFE_HOME) +#define OSD_MSG_DIVERT_SAFEHOME "DIVERTING TO SAFEHOME" +#endif + typedef enum { OSD_RSSI_VALUE, OSD_MAIN_BATT_VOLTAGE, @@ -337,7 +341,7 @@ typedef struct osdConfig_s { bool osd_failsafe_switch_layout; uint8_t plus_code_digits; // Number of digits to use in OSD_PLUS_CODE - uint8_t plus_code_short; + uint8_t plus_code_short; uint8_t osd_ahi_style; uint8_t force_grid; // Force a pixel based OSD to use grid mode. uint8_t ahi_bordered; // Only used by the AHI widget diff --git a/src/main/navigation/navigation.h b/src/main/navigation/navigation.h index 59c784e402..baecd6d32b 100755 --- a/src/main/navigation/navigation.h +++ b/src/main/navigation/navigation.h @@ -61,6 +61,7 @@ PG_DECLARE_ARRAY(navSafeHome_t, MAX_SAFE_HOMES, safeHomeConfig); extern int8_t safehome_index; // -1 if no safehome, 0 to MAX_SAFEHOMES -1 otherwise extern uint32_t safehome_distance; // distance to the nearest safehome +extern bool safehome_applied; // whether the safehome has been applied to home. void resetSafeHomes(void); // remove all safehomes bool findNearestSafeHome(void); // Find nearest safehome From f5b62f02832b9b06852b1ace292f93922b90d806 Mon Sep 17 00:00:00 2001 From: Tony Yeung Date: Wed, 24 Feb 2021 22:40:44 +0000 Subject: [PATCH 061/286] bug fixes --- docs/Safehomes.md | 4 ++++ src/main/navigation/navigation.c | 3 ++- 2 files changed, 6 insertions(+), 1 deletion(-) diff --git a/docs/Safehomes.md b/docs/Safehomes.md index 6402d8275b..17c8742073 100644 --- a/docs/Safehomes.md +++ b/docs/Safehomes.md @@ -43,6 +43,10 @@ If a safehome is selected, an additional message appears: The GPS details are those of the arming location, not the safehome. To draw your attention to a safehome being selected, the message flashes and stays visible longer. +## OSD Message during RTH + +If RTH is in progress to a safehome, the message "DIVERTING TO SAFEHOME" will be displayed. + ## CLI command `safehome` to manage safehomes `safehome` - List all safehomes diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c index 9e48563ee7..a23e9eec48 100755 --- a/src/main/navigation/navigation.c +++ b/src/main/navigation/navigation.c @@ -3133,7 +3133,7 @@ static navigationFSMEvent_t selectNavEventFromBoxModeInput(void) const bool canActivatePosHold = canActivatePosHoldMode(); const bool canActivateNavigation = canActivateNavigationModes(); const bool isExecutingRTH = navGetStateFlags(posControl.navState) & NAV_AUTO_RTH; - checkSafeHomeState(isExecutingRTH); + checkSafeHomeState(isExecutingRTH || posControl.flags.forcedRTHActivated); // Keep canActivateWaypoint flag at FALSE if there is no mission loaded // Also block WP mission if we are executing RTH @@ -3567,6 +3567,7 @@ void activateForcedRTH(void) { abortFixedWingLaunch(); posControl.flags.forcedRTHActivated = true; + checkSafeHomeState(true); navProcessFSMEvents(selectNavEventFromBoxModeInput()); } From 34422ffe8799f6ac9a0e07995ccf11168606b7c4 Mon Sep 17 00:00:00 2001 From: Tony Yeung Date: Thu, 25 Feb 2021 14:44:33 +0000 Subject: [PATCH 062/286] warning safehome off --- src/main/io/osd.c | 18 +++++++++++------- src/main/navigation/navigation.c | 4 +--- 2 files changed, 12 insertions(+), 10 deletions(-) diff --git a/src/main/io/osd.c b/src/main/io/osd.c index 06b3eedf8e..89296fb79b 100755 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -3095,13 +3095,17 @@ static void osdShowArmed(void) } y += 4; #if defined (USE_SAFE_HOME) - if (safehome_distance) { - textAttributes_t elemAttr = _TEXT_ATTRIBUTES_BLINK_BIT; - char buf2[12]; // format the distance first - osdFormatDistanceStr(buf2, safehome_distance); - tfp_sprintf(buf, "%c - %s -> SAFEHOME %u", SYM_HOME, buf2, safehome_index); - // write this message above the ARMED message to make it obvious - displayWriteWithAttr(osdDisplayPort, (osdDisplayPort->cols - strlen(buf)) / 2, y - 8, buf, elemAttr); + if (safehome_distance) { // safehome found during arming + if (navConfig()->general.flags.safehome_usage_mode == SAFEHOME_USAGE_OFF) { + strcpy(buf, "SAFEHOME FOUND; MODE OFF"); + } else { + char buf2[12]; // format the distance first + osdFormatDistanceStr(buf2, safehome_distance); + tfp_sprintf(buf, "%c - %s -> SAFEHOME %u", SYM_HOME, buf2, safehome_index); + } + textAttributes_t elemAttr = _TEXT_ATTRIBUTES_BLINK_BIT; + // write this message above the ARMED message to make it obvious + displayWriteWithAttr(osdDisplayPort, (osdDisplayPort->cols - strlen(buf)) / 2, y - 8, buf, elemAttr); } #endif } else { diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c index a23e9eec48..cc6b161e54 100755 --- a/src/main/navigation/navigation.c +++ b/src/main/navigation/navigation.c @@ -2416,9 +2416,7 @@ void updateHomePosition(void) } if (setHome) { #if defined(USE_SAFE_HOME) - if (navConfig()->general.flags.safehome_usage_mode != SAFEHOME_USAGE_OFF) { - findNearestSafeHome(); - } + findNearestSafeHome(); #endif setHomePosition(&posControl.actualState.abs.pos, posControl.actualState.yaw, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING, navigationActualStateHomeValidity()); // save the current location in case it is replaced by a safehome or HOME_RESET From f4e5fd765f8f0a6cf8ad7f264369cf7fd2347aa8 Mon Sep 17 00:00:00 2001 From: Tony Yeung Date: Thu, 25 Feb 2021 15:02:00 +0000 Subject: [PATCH 063/286] add doc changes --- docs/Safehomes.md | 2 ++ 1 file changed, 2 insertions(+) diff --git a/docs/Safehomes.md b/docs/Safehomes.md index 17c8742073..6a5492ec71 100644 --- a/docs/Safehomes.md +++ b/docs/Safehomes.md @@ -43,6 +43,8 @@ If a safehome is selected, an additional message appears: The GPS details are those of the arming location, not the safehome. To draw your attention to a safehome being selected, the message flashes and stays visible longer. +If a safehome was found, but ``safehome_usage_mode``` is ```OFF```, the message ```SAFEHOME FOUND; MODE OFF``` will appear. + ## OSD Message during RTH If RTH is in progress to a safehome, the message "DIVERTING TO SAFEHOME" will be displayed. From e7f6bd8faa0567c1e7dae23d1266780a15df545a Mon Sep 17 00:00:00 2001 From: OptimusTi Date: Sat, 27 Feb 2021 17:42:12 -0500 Subject: [PATCH 064/286] Bug Fix Added check to prevent stats pages outside of disarm screen --- src/main/io/osd.c | 12 ++++++++++-- 1 file changed, 10 insertions(+), 2 deletions(-) diff --git a/src/main/io/osd.c b/src/main/io/osd.c index edda1479cf..b099afceef 100755 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -168,6 +168,7 @@ static bool refreshWaitForResumeCmdRelease; static bool fullRedraw = false; static uint8_t armState; +static uint8_t statsPagesCheck = 0; typedef struct osdMapData_s { uint32_t scale; @@ -2938,6 +2939,7 @@ static void osdShowStatsPage1(void) const uint8_t statNameX = 1; const uint8_t statValuesX = 20; char buff[10]; + statsPagesCheck = 1; displayBeginTransaction(osdDisplayPort, DISPLAY_TRANSACTION_OPT_RESET_DRAWING); displayClearScreen(osdDisplayPort); @@ -2988,6 +2990,7 @@ static void osdShowStatsPage2(void) const uint8_t statNameX = 1; const uint8_t statValuesX = 20; char buff[10]; + statsPagesCheck = 1; displayBeginTransaction(osdDisplayPort, DISPLAY_TRANSACTION_OPT_RESET_DRAWING); displayClearScreen(osdDisplayPort); @@ -3173,6 +3176,7 @@ static void osdRefresh(timeUs_t currentTimeUs) osdResetStats(); osdShowArmed(); // reset statistic etc uint32_t delay = ARMED_SCREEN_DISPLAY_TIME; + statsPagesCheck = 0; #if defined(USE_SAFE_HOME) if (isSafeHomeInUse()) delay *= 3; @@ -3199,9 +3203,13 @@ static void osdRefresh(timeUs_t currentTimeUs) displayClearScreen(osdDisplayPort); resumeRefreshAt = 0; } else if ((currentTimeUs > resumeRefreshAt) || ((!refreshWaitForResumeCmdRelease) && STATS_PAGE1)) { - osdShowStatsPage1(); + if (statsPagesCheck == 1) { + osdShowStatsPage1(); + } } else if ((currentTimeUs > resumeRefreshAt) || ((!refreshWaitForResumeCmdRelease) && STATS_PAGE2)) { - osdShowStatsPage2(); + if (statsPagesCheck == 1) { + osdShowStatsPage2(); + } } else { displayHeartbeat(osdDisplayPort); } From b51ff817cb6ae4baeac3a718004454bf1eccfd72 Mon Sep 17 00:00:00 2001 From: Harry Phillips Date: Sun, 28 Feb 2021 18:05:30 +0000 Subject: [PATCH 065/286] Boilerplate for MAVLink RX --- src/main/CMakeLists.txt | 2 ++ src/main/fc/settings.yaml | 2 +- src/main/rx/mavlink.c | 51 ++++++++++++++++++++++++++++++++++++ src/main/rx/mavlink.h | 20 ++++++++++++++ src/main/rx/rx.c | 6 +++++ src/main/rx/rx.h | 1 + src/main/target/common.h | 1 + src/main/telemetry/mavlink.c | 6 +++++ 8 files changed, 88 insertions(+), 1 deletion(-) create mode 100644 src/main/rx/mavlink.c create mode 100644 src/main/rx/mavlink.h diff --git a/src/main/CMakeLists.txt b/src/main/CMakeLists.txt index d37646abc4..3972b6ec0c 100644 --- a/src/main/CMakeLists.txt +++ b/src/main/CMakeLists.txt @@ -386,6 +386,8 @@ main_sources(COMMON_SRC rx/ibus.h rx/jetiexbus.c rx/jetiexbus.h + rx/mavlink.c + rx/mavlink.h rx/msp.c rx/msp.h rx/msp_override.c diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index d462d72cc8..de05765f53 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -25,7 +25,7 @@ tables: values: ["NONE", "PPM", "SERIAL", "MSP", "SPI", "UNUSED"] enum: rxReceiverType_e - name: serial_rx - values: ["SPEK1024", "SPEK2048", "SBUS", "SUMD", "SUMH", "XB-B", "XB-B-RJ01", "IBUS", "JETIEXBUS", "CRSF", "FPORT", "SBUS_FAST", "FPORT2", "SRXL2", "GHST"] + values: ["SPEK1024", "SPEK2048", "SBUS", "SUMD", "SUMH", "XB-B", "XB-B-RJ01", "IBUS", "JETIEXBUS", "CRSF", "FPORT", "SBUS_FAST", "FPORT2", "SRXL2", "GHST", "MAVLINK"] - name: rx_spi_protocol values: ["ELERES"] enum: rx_spi_protocol_e diff --git a/src/main/rx/mavlink.c b/src/main/rx/mavlink.c new file mode 100644 index 0000000000..5af1acaced --- /dev/null +++ b/src/main/rx/mavlink.c @@ -0,0 +1,51 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Cleanflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Cleanflight. If not, see . + */ + +#include +#include +#include +#include + +#include "platform.h" +FILE_COMPILE_FOR_SPEED +#ifdef USE_SERIALRX_MAVLINK + +#include "build/build_config.h" +#include "build/debug.h" + +#include "common/crc.h" +#include "common/maths.h" +#include "common/utils.h" + +#include "drivers/time.h" +#include "drivers/serial.h" +#include "drivers/serial_uart.h" + +#include "io/serial.h" + +#include "rx/rx.h" +#include "rx/mavlink.h" + +#define MAVLINK_MAX_CHANNEL 18 + +bool mavlinkRxInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig) +{ + // TODO + return false; +} + +#endif diff --git a/src/main/rx/mavlink.h b/src/main/rx/mavlink.h new file mode 100644 index 0000000000..3311b15413 --- /dev/null +++ b/src/main/rx/mavlink.h @@ -0,0 +1,20 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Cleanflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Cleanflight. If not, see . + */ + +#pragma once + +bool mavlinkRxInit(const struct rxConfig_s *initialRxConfig, struct rxRuntimeConfig_s *rxRuntimeConfig); diff --git a/src/main/rx/rx.c b/src/main/rx/rx.c index f028e73330..b01c9e4539 100755 --- a/src/main/rx/rx.c +++ b/src/main/rx/rx.c @@ -68,6 +68,7 @@ #include "rx/sumh.h" #include "rx/xbus.h" #include "rx/ghst.h" +#include "rx/mavlink.h" //#define DEBUG_RX_SIGNAL_LOSS @@ -255,6 +256,11 @@ bool serialRxInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig case SERIALRX_GHST: enabled = ghstRxInit(rxConfig, rxRuntimeConfig); break; +#endif +#ifdef USE_SERIALRX_MAVLINK + case SERIALRX_MAVLINK: + enabled = mavlinkRxInit(rxConfig, rxRuntimeConfig); + break; #endif default: enabled = false; diff --git a/src/main/rx/rx.h b/src/main/rx/rx.h index 0aee1e8353..f35f7bc1f8 100644 --- a/src/main/rx/rx.h +++ b/src/main/rx/rx.h @@ -84,6 +84,7 @@ typedef enum { SERIALRX_FPORT2 = 12, SERIALRX_SRXL2 = 13, SERIALRX_GHST = 14, + SERIALRX_MAVLINK = 15, } rxSerialReceiverType_e; #define MAX_SUPPORTED_RC_PPM_CHANNEL_COUNT 16 diff --git a/src/main/target/common.h b/src/main/target/common.h index 9d0ddc3346..cce269cd7e 100755 --- a/src/main/target/common.h +++ b/src/main/target/common.h @@ -177,6 +177,7 @@ #define USE_SERIALRX_SUMH #define USE_SERIALRX_XBUS #define USE_SERIALRX_JETIEXBUS +#define USE_SERIALRX_MAVLINK #define USE_SERIALRX_CRSF #define USE_PWM_SERVO_DRIVER #define USE_SERIAL_PASSTHROUGH diff --git a/src/main/telemetry/mavlink.c b/src/main/telemetry/mavlink.c index b5725a19b2..272d9c6b06 100644 --- a/src/main/telemetry/mavlink.c +++ b/src/main/telemetry/mavlink.c @@ -1010,6 +1010,10 @@ static bool handleIncoming_MISSION_REQUEST(void) return false; } +static bool handleIncoming_RC_CHANNELS_OVERRIDE(void) { + +} + static bool processMAVLinkIncomingTelemetry(void) { while (serialRxBytesWaiting(mavlinkPort) > 0) { @@ -1030,6 +1034,8 @@ static bool processMAVLinkIncomingTelemetry(void) return handleIncoming_MISSION_REQUEST_LIST(); case MAVLINK_MSG_ID_MISSION_REQUEST: return handleIncoming_MISSION_REQUEST(); + case MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE: + return handleIncoming_RC_CHANNELS_OVERRIDE(); default: return false; } From f58ef63cb40322e88025970f7158d0538f8dcc84 Mon Sep 17 00:00:00 2001 From: Harry Phillips Date: Sun, 28 Feb 2021 18:25:07 +0000 Subject: [PATCH 066/286] mavgen library generator scripts --- lib/main/MAVLink/.gitignore | 1 + lib/main/MAVLink/README.md | 7 +++++++ lib/main/MAVLink/generate.bat | 25 +++++++++++++++++++++++++ lib/main/MAVLink/generate.sh | 25 +++++++++++++++++++++++++ src/main/telemetry/mavlink.c | 4 +++- 5 files changed, 61 insertions(+), 1 deletion(-) create mode 100644 lib/main/MAVLink/.gitignore create mode 100644 lib/main/MAVLink/README.md create mode 100644 lib/main/MAVLink/generate.bat create mode 100644 lib/main/MAVLink/generate.sh diff --git a/lib/main/MAVLink/.gitignore b/lib/main/MAVLink/.gitignore new file mode 100644 index 0000000000..debff4b898 --- /dev/null +++ b/lib/main/MAVLink/.gitignore @@ -0,0 +1 @@ +mavlink-src/ diff --git a/lib/main/MAVLink/README.md b/lib/main/MAVLink/README.md new file mode 100644 index 0000000000..851b32e3e0 --- /dev/null +++ b/lib/main/MAVLink/README.md @@ -0,0 +1,7 @@ +# MAVLink Generator (`mavgen`) + +This directory contains the MAVLink library and scripts to automatically generate it from MAVLink definitions. + +In order to run it, you will need [Python and some other libraries installed](https://mavlink.io/en/getting_started/installation.html). + +Then, run the appropriate script (`generate.sh` for Linux, `generate.bat` for Windows) to automatically re-generate the library. diff --git a/lib/main/MAVLink/generate.bat b/lib/main/MAVLink/generate.bat new file mode 100644 index 0000000000..bb9e7734e7 --- /dev/null +++ b/lib/main/MAVLink/generate.bat @@ -0,0 +1,25 @@ +@echo off + +echo Removing old library... +del /q common\ +del /q minimal\ +del /q standard\ +del /q checksum.h +del /q mavlink_* +del /q protocol.h + +echo Downloading or updating MAVLink sources... + +if exist mavlink-src\.git\ ( + cd mavlink-src + git fetch + git checkout origin/master + cd ../ +) else ( + git clone https://github.com/mavlink/mavlink.git --recursive mavlink-src +) + +set PYTHONPATH=%CD%\mavlink-src + +echo Running MAVLink generator... +python -m pymavlink.tools.mavgen --lang=C --wire-protocol=2.0 --output=. mavlink-src/message_definitions/v1.0/common.xml --no-validate \ No newline at end of file diff --git a/lib/main/MAVLink/generate.sh b/lib/main/MAVLink/generate.sh new file mode 100644 index 0000000000..635473a374 --- /dev/null +++ b/lib/main/MAVLink/generate.sh @@ -0,0 +1,25 @@ +#!/usr/bin/env bash + +echo "Removing old library..." +rm -r common/ +rm -r minimal/ +rm -r standard/ +rm -r checksum.h +rm -r mavlink_* +rm -r protocol.h + +echo "Downloading or updating MAVLink sources..." + +if [ -d "mavlink-src/.git" ]; then + cd mavlink-src || return 1 + git fetch + git checkout origin/master + cd ../ +else + git clone https://github.com/mavlink/mavlink.git --recursive mavlink-src +fi + +PYTHONPATH="$(pwd)/mavlink-src" + +echo "Running MAVLink generator..." +python -m pymavlink.tools.mavgen --lang=C --wire-protocol=2.0 --output=. mavlink-src/message_definitions/v1.0/common.xml --no-validate diff --git a/src/main/telemetry/mavlink.c b/src/main/telemetry/mavlink.c index 272d9c6b06..f6929c7826 100644 --- a/src/main/telemetry/mavlink.c +++ b/src/main/telemetry/mavlink.c @@ -86,8 +86,9 @@ #include "scheduler/scheduler.h" -// mavlink library uses unnames unions that's causes GCC to complain if -Wpedantic is used +// mavlink library uses unnamed unions that's causes GCC to complain if -Wpedantic is used // until this is resolved in mavlink library - ignore -Wpedantic for mavlink code +// TODO check if this is resolved in V2 library #pragma GCC diagnostic push #pragma GCC diagnostic ignored "-Wpedantic" #include "common/mavlink.h" @@ -1023,6 +1024,7 @@ static bool processMAVLinkIncomingTelemetry(void) if (result == MAVLINK_FRAMING_OK) { switch (mavRecvMsg.msgid) { case MAVLINK_MSG_ID_HEARTBEAT: + // TODO failsafe break; case MAVLINK_MSG_ID_MISSION_CLEAR_ALL: return handleIncoming_MISSION_CLEAR_ALL(); From c644fdf9ea0744aa647dc4b788a989eaf611e96d Mon Sep 17 00:00:00 2001 From: Harry Phillips Date: Sun, 28 Feb 2021 18:26:02 +0000 Subject: [PATCH 067/286] MAVLink V2 library (auto-generated) --- lib/main/MAVLink/checksum.h | 23 +- lib/main/MAVLink/common/common.h | 583 +- lib/main/MAVLink/common/mavlink.h | 4 +- .../mavlink_msg_actuator_output_status.h | 255 + .../MAVLink/common/mavlink_msg_ais_vessel.h | 606 +++ .../common/mavlink_msg_att_pos_mocap.h | 48 +- .../common/mavlink_msg_attitude_quaternion.h | 57 +- .../common/mavlink_msg_attitude_target.h | 10 +- ...nk_msg_autopilot_state_for_gimbal_device.h | 480 ++ .../common/mavlink_msg_autopilot_version.h | 48 +- .../common/mavlink_msg_battery_status.h | 152 +- .../common/mavlink_msg_button_change.h | 263 + .../mavlink_msg_camera_capture_status.h | 363 ++ .../common/mavlink_msg_camera_fov_status.h | 430 ++ .../mavlink_msg_camera_image_captured.h | 456 ++ .../common/mavlink_msg_camera_information.h | 507 ++ .../common/mavlink_msg_camera_settings.h | 288 + .../mavlink_msg_camera_tracking_geo_status.h | 513 ++ ...mavlink_msg_camera_tracking_image_status.h | 438 ++ .../common/mavlink_msg_cellular_config.h | 383 ++ .../common/mavlink_msg_cellular_status.h | 363 ++ .../MAVLink/common/mavlink_msg_command_ack.h | 122 +- .../MAVLink/common/mavlink_msg_command_int.h | 20 +- .../mavlink_msg_component_information.h | 331 ++ .../common/mavlink_msg_debug_float_array.h | 281 + .../common/mavlink_msg_distance_sensor.h | 136 +- .../MAVLink/common/mavlink_msg_efi_status.h | 613 +++ .../MAVLink/common/mavlink_msg_esc_info.h | 407 ++ .../MAVLink/common/mavlink_msg_esc_status.h | 307 ++ .../MAVLink/common/mavlink_msg_fence_status.h | 47 +- .../common/mavlink_msg_flight_information.h | 288 + .../common/mavlink_msg_generator_status.h | 463 ++ ...avlink_msg_gimbal_device_attitude_status.h | 405 ++ .../mavlink_msg_gimbal_device_information.h | 557 ++ .../mavlink_msg_gimbal_device_set_attitude.h | 355 ++ .../mavlink_msg_gimbal_manager_information.h | 413 ++ .../mavlink_msg_gimbal_manager_set_attitude.h | 380 ++ ...nk_msg_gimbal_manager_set_manual_control.h | 388 ++ .../mavlink_msg_gimbal_manager_set_pitchyaw.h | 388 ++ .../mavlink_msg_gimbal_manager_status.h | 363 ++ ...link_msg_global_vision_position_estimate.h | 82 +- .../MAVLink/common/mavlink_msg_gps2_raw.h | 71 +- .../common/mavlink_msg_gps_global_origin.h | 51 +- .../MAVLink/common/mavlink_msg_gps_input.h | 71 +- .../MAVLink/common/mavlink_msg_gps_raw_int.h | 176 +- .../MAVLink/common/mavlink_msg_highres_imu.h | 67 +- lib/main/MAVLink/common/mavlink_msg_hil_gps.h | 96 +- .../MAVLink/common/mavlink_msg_hil_sensor.h | 67 +- .../common/mavlink_msg_home_position.h | 51 +- .../common/mavlink_msg_isbd_link_status.h | 388 ++ .../common/mavlink_msg_landing_target.h | 186 +- .../MAVLink/common/mavlink_msg_logging_ack.h | 263 + .../MAVLink/common/mavlink_msg_logging_data.h | 330 ++ .../common/mavlink_msg_logging_data_acked.h | 330 ++ .../common/mavlink_msg_mag_cal_report.h | 638 +++ .../MAVLink/common/mavlink_msg_mission_ack.h | 47 +- .../common/mavlink_msg_mission_clear_all.h | 47 +- .../common/mavlink_msg_mission_count.h | 47 +- .../MAVLink/common/mavlink_msg_mission_item.h | 47 +- .../common/mavlink_msg_mission_item_int.h | 47 +- .../common/mavlink_msg_mission_request.h | 47 +- .../common/mavlink_msg_mission_request_int.h | 47 +- .../common/mavlink_msg_mission_request_list.h | 47 +- ...mavlink_msg_mission_request_partial_list.h | 47 +- .../mavlink_msg_mission_write_partial_list.h | 47 +- .../common/mavlink_msg_mount_orientation.h | 313 ++ .../common/mavlink_msg_obstacle_distance.h | 405 ++ .../MAVLink/common/mavlink_msg_odometry.h | 607 +++ .../mavlink_msg_onboard_computer_status.h | 693 +++ ...mavlink_msg_open_drone_id_authentication.h | 406 ++ .../mavlink_msg_open_drone_id_basic_id.h | 331 ++ .../mavlink_msg_open_drone_id_location.h | 655 +++ .../mavlink_msg_open_drone_id_message_pack.h | 305 ++ .../mavlink_msg_open_drone_id_operator_id.h | 306 ++ .../mavlink_msg_open_drone_id_self_id.h | 306 ++ .../common/mavlink_msg_open_drone_id_system.h | 505 ++ .../MAVLink/common/mavlink_msg_optical_flow.h | 76 +- .../mavlink_msg_orbit_execution_status.h | 338 ++ .../common/mavlink_msg_param_ext_ack.h | 281 + .../mavlink_msg_param_ext_request_list.h | 238 + .../mavlink_msg_param_ext_request_read.h | 280 + .../common/mavlink_msg_param_ext_set.h | 306 ++ .../common/mavlink_msg_param_ext_value.h | 306 ++ .../MAVLink/common/mavlink_msg_play_tune.h | 281 + .../MAVLink/common/mavlink_msg_play_tune_v2.h | 280 + lib/main/MAVLink/common/mavlink_msg_raw_imu.h | 76 +- lib/main/MAVLink/common/mavlink_msg_raw_rpm.h | 238 + .../common/mavlink_msg_rc_channels_override.h | 352 +- .../MAVLink/common/mavlink_msg_scaled_imu.h | 47 +- .../MAVLink/common/mavlink_msg_scaled_imu2.h | 47 +- .../MAVLink/common/mavlink_msg_scaled_imu3.h | 47 +- .../common/mavlink_msg_scaled_pressure.h | 47 +- .../common/mavlink_msg_scaled_pressure2.h | 47 +- .../common/mavlink_msg_scaled_pressure3.h | 47 +- .../common/mavlink_msg_servo_output_raw.h | 226 +- .../common/mavlink_msg_set_attitude_target.h | 10 +- .../mavlink_msg_set_gps_global_origin.h | 51 +- .../common/mavlink_msg_set_home_position.h | 51 +- .../common/mavlink_msg_setup_signing.h | 280 + .../common/mavlink_msg_smart_battery_info.h | 481 ++ .../MAVLink/common/mavlink_msg_statustext.h | 76 +- .../common/mavlink_msg_storage_information.h | 455 ++ .../common/mavlink_msg_supported_tunes.h | 263 + .../mavlink_msg_time_estimate_to_target.h | 313 ++ ...ink_msg_trajectory_representation_bezier.h | 359 ++ ..._msg_trajectory_representation_waypoints.h | 541 ++ lib/main/MAVLink/common/mavlink_msg_tunnel.h | 305 ++ .../common/mavlink_msg_uavcan_node_info.h | 406 ++ .../common/mavlink_msg_uavcan_node_status.h | 338 ++ .../common/mavlink_msg_utm_global_position.h | 630 +++ .../mavlink_msg_vicon_position_estimate.h | 57 +- .../mavlink_msg_video_stream_information.h | 481 ++ .../common/mavlink_msg_video_stream_status.h | 388 ++ .../mavlink_msg_vision_position_estimate.h | 82 +- .../mavlink_msg_vision_speed_estimate.h | 82 +- .../common/mavlink_msg_wheel_distance.h | 255 + .../common/mavlink_msg_wifi_config_ap.h | 281 + .../MAVLink/common/mavlink_msg_winch_status.h | 388 ++ lib/main/MAVLink/common/testsuite.h | 4786 ++++++++++++++++- lib/main/MAVLink/common/version.h | 4 +- lib/main/MAVLink/mavlink_conversions.h | 16 +- lib/main/MAVLink/mavlink_get_info.h | 71 + lib/main/MAVLink/mavlink_helpers.h | 728 ++- lib/main/MAVLink/mavlink_sha256.h | 235 + lib/main/MAVLink/mavlink_types.h | 151 +- lib/main/MAVLink/minimal/mavlink.h | 34 + .../mavlink_msg_heartbeat.h | 0 .../minimal/mavlink_msg_protocol_version.h | 306 ++ lib/main/MAVLink/minimal/minimal.h | 333 ++ lib/main/MAVLink/minimal/testsuite.h | 154 + lib/main/MAVLink/minimal/version.h | 14 + lib/main/MAVLink/protocol.h | 25 +- 132 files changed, 36223 insertions(+), 1470 deletions(-) create mode 100644 lib/main/MAVLink/common/mavlink_msg_actuator_output_status.h create mode 100644 lib/main/MAVLink/common/mavlink_msg_ais_vessel.h create mode 100644 lib/main/MAVLink/common/mavlink_msg_autopilot_state_for_gimbal_device.h create mode 100644 lib/main/MAVLink/common/mavlink_msg_button_change.h create mode 100644 lib/main/MAVLink/common/mavlink_msg_camera_capture_status.h create mode 100644 lib/main/MAVLink/common/mavlink_msg_camera_fov_status.h create mode 100644 lib/main/MAVLink/common/mavlink_msg_camera_image_captured.h create mode 100644 lib/main/MAVLink/common/mavlink_msg_camera_information.h create mode 100644 lib/main/MAVLink/common/mavlink_msg_camera_settings.h create mode 100644 lib/main/MAVLink/common/mavlink_msg_camera_tracking_geo_status.h create mode 100644 lib/main/MAVLink/common/mavlink_msg_camera_tracking_image_status.h create mode 100644 lib/main/MAVLink/common/mavlink_msg_cellular_config.h create mode 100644 lib/main/MAVLink/common/mavlink_msg_cellular_status.h create mode 100644 lib/main/MAVLink/common/mavlink_msg_component_information.h create mode 100644 lib/main/MAVLink/common/mavlink_msg_debug_float_array.h create mode 100644 lib/main/MAVLink/common/mavlink_msg_efi_status.h create mode 100644 lib/main/MAVLink/common/mavlink_msg_esc_info.h create mode 100644 lib/main/MAVLink/common/mavlink_msg_esc_status.h create mode 100644 lib/main/MAVLink/common/mavlink_msg_flight_information.h create mode 100644 lib/main/MAVLink/common/mavlink_msg_generator_status.h create mode 100644 lib/main/MAVLink/common/mavlink_msg_gimbal_device_attitude_status.h create mode 100644 lib/main/MAVLink/common/mavlink_msg_gimbal_device_information.h create mode 100644 lib/main/MAVLink/common/mavlink_msg_gimbal_device_set_attitude.h create mode 100644 lib/main/MAVLink/common/mavlink_msg_gimbal_manager_information.h create mode 100644 lib/main/MAVLink/common/mavlink_msg_gimbal_manager_set_attitude.h create mode 100644 lib/main/MAVLink/common/mavlink_msg_gimbal_manager_set_manual_control.h create mode 100644 lib/main/MAVLink/common/mavlink_msg_gimbal_manager_set_pitchyaw.h create mode 100644 lib/main/MAVLink/common/mavlink_msg_gimbal_manager_status.h create mode 100644 lib/main/MAVLink/common/mavlink_msg_isbd_link_status.h create mode 100644 lib/main/MAVLink/common/mavlink_msg_logging_ack.h create mode 100644 lib/main/MAVLink/common/mavlink_msg_logging_data.h create mode 100644 lib/main/MAVLink/common/mavlink_msg_logging_data_acked.h create mode 100644 lib/main/MAVLink/common/mavlink_msg_mag_cal_report.h create mode 100644 lib/main/MAVLink/common/mavlink_msg_mount_orientation.h create mode 100644 lib/main/MAVLink/common/mavlink_msg_obstacle_distance.h create mode 100644 lib/main/MAVLink/common/mavlink_msg_odometry.h create mode 100644 lib/main/MAVLink/common/mavlink_msg_onboard_computer_status.h create mode 100644 lib/main/MAVLink/common/mavlink_msg_open_drone_id_authentication.h create mode 100644 lib/main/MAVLink/common/mavlink_msg_open_drone_id_basic_id.h create mode 100644 lib/main/MAVLink/common/mavlink_msg_open_drone_id_location.h create mode 100644 lib/main/MAVLink/common/mavlink_msg_open_drone_id_message_pack.h create mode 100644 lib/main/MAVLink/common/mavlink_msg_open_drone_id_operator_id.h create mode 100644 lib/main/MAVLink/common/mavlink_msg_open_drone_id_self_id.h create mode 100644 lib/main/MAVLink/common/mavlink_msg_open_drone_id_system.h create mode 100644 lib/main/MAVLink/common/mavlink_msg_orbit_execution_status.h create mode 100644 lib/main/MAVLink/common/mavlink_msg_param_ext_ack.h create mode 100644 lib/main/MAVLink/common/mavlink_msg_param_ext_request_list.h create mode 100644 lib/main/MAVLink/common/mavlink_msg_param_ext_request_read.h create mode 100644 lib/main/MAVLink/common/mavlink_msg_param_ext_set.h create mode 100644 lib/main/MAVLink/common/mavlink_msg_param_ext_value.h create mode 100644 lib/main/MAVLink/common/mavlink_msg_play_tune.h create mode 100644 lib/main/MAVLink/common/mavlink_msg_play_tune_v2.h create mode 100644 lib/main/MAVLink/common/mavlink_msg_raw_rpm.h create mode 100644 lib/main/MAVLink/common/mavlink_msg_setup_signing.h create mode 100644 lib/main/MAVLink/common/mavlink_msg_smart_battery_info.h create mode 100644 lib/main/MAVLink/common/mavlink_msg_storage_information.h create mode 100644 lib/main/MAVLink/common/mavlink_msg_supported_tunes.h create mode 100644 lib/main/MAVLink/common/mavlink_msg_time_estimate_to_target.h create mode 100644 lib/main/MAVLink/common/mavlink_msg_trajectory_representation_bezier.h create mode 100644 lib/main/MAVLink/common/mavlink_msg_trajectory_representation_waypoints.h create mode 100644 lib/main/MAVLink/common/mavlink_msg_tunnel.h create mode 100644 lib/main/MAVLink/common/mavlink_msg_uavcan_node_info.h create mode 100644 lib/main/MAVLink/common/mavlink_msg_uavcan_node_status.h create mode 100644 lib/main/MAVLink/common/mavlink_msg_utm_global_position.h create mode 100644 lib/main/MAVLink/common/mavlink_msg_video_stream_information.h create mode 100644 lib/main/MAVLink/common/mavlink_msg_video_stream_status.h create mode 100644 lib/main/MAVLink/common/mavlink_msg_wheel_distance.h create mode 100644 lib/main/MAVLink/common/mavlink_msg_wifi_config_ap.h create mode 100644 lib/main/MAVLink/common/mavlink_msg_winch_status.h create mode 100644 lib/main/MAVLink/mavlink_get_info.h create mode 100644 lib/main/MAVLink/mavlink_sha256.h create mode 100644 lib/main/MAVLink/minimal/mavlink.h rename lib/main/MAVLink/{common => minimal}/mavlink_msg_heartbeat.h (100%) mode change 100755 => 100644 create mode 100644 lib/main/MAVLink/minimal/mavlink_msg_protocol_version.h create mode 100644 lib/main/MAVLink/minimal/minimal.h create mode 100644 lib/main/MAVLink/minimal/testsuite.h create mode 100644 lib/main/MAVLink/minimal/version.h diff --git a/lib/main/MAVLink/checksum.h b/lib/main/MAVLink/checksum.h index 0f30b659fa..0a899a6077 100755 --- a/lib/main/MAVLink/checksum.h +++ b/lib/main/MAVLink/checksum.h @@ -1,10 +1,11 @@ -#ifdef __cplusplus +#pragma once + +#if defined(MAVLINK_USE_CXX_NAMESPACE) +namespace mavlink { +#elif defined(__cplusplus) extern "C" { #endif -#ifndef _CHECKSUM_H_ -#define _CHECKSUM_H_ - // Visual Studio versions before 2010 don't have stdint.h, so we just error out. #if (defined _MSC_VER) && (_MSC_VER < 1600) #error "The C-MAVLink implementation requires Visual Studio 2010 or greater" @@ -23,7 +24,7 @@ extern "C" { #ifndef HAVE_CRC_ACCUMULATE /** - * @brief Accumulate the X.25 CRC by adding one char at a time. + * @brief Accumulate the CRC16_MCRF4XX checksum by adding one char at a time. * * The checksum function adds the hash of one char at a time to the * 16 bit checksum (uint16_t). @@ -44,9 +45,9 @@ static inline void crc_accumulate(uint8_t data, uint16_t *crcAccum) /** - * @brief Initiliaze the buffer for the X.25 CRC + * @brief Initiliaze the buffer for the MCRF4XX CRC16 * - * @param crcAccum the 16 bit X.25 CRC + * @param crcAccum the 16 bit MCRF4XX CRC16 */ static inline void crc_init(uint16_t* crcAccum) { @@ -55,7 +56,7 @@ static inline void crc_init(uint16_t* crcAccum) /** - * @brief Calculates the X.25 checksum on a byte buffer + * @brief Calculates the CRC16_MCRF4XX checksum on a byte buffer * * @param pBuffer buffer containing the byte array to hash * @param length length of the byte array @@ -73,7 +74,7 @@ static inline uint16_t crc_calculate(const uint8_t* pBuffer, uint16_t length) /** - * @brief Accumulate the X.25 CRC by adding an array of bytes + * @brief Accumulate the MCRF4XX CRC16 by adding an array of bytes * * The checksum function adds the hash of one char at a time to the * 16 bit checksum (uint16_t). @@ -89,8 +90,6 @@ static inline void crc_accumulate_buffer(uint16_t *crcAccum, const char *pBuffer } } -#endif /* _CHECKSUM_H_ */ - -#ifdef __cplusplus +#if defined(MAVLINK_USE_CXX_NAMESPACE) || defined(__cplusplus) } #endif diff --git a/lib/main/MAVLink/common/common.h b/lib/main/MAVLink/common/common.h index 1014966516..55200c5a84 100755 --- a/lib/main/MAVLink/common/common.h +++ b/lib/main/MAVLink/common/common.h @@ -20,11 +20,11 @@ extern "C" { // MESSAGE LENGTHS AND CRCS #ifndef MAVLINK_MESSAGE_LENGTHS -#define MAVLINK_MESSAGE_LENGTHS {9, 31, 12, 0, 14, 28, 3, 32, 36, 0, 0, 6, 0, 0, 0, 0, 0, 0, 0, 24, 20, 2, 25, 23, 30, 101, 22, 26, 16, 14, 28, 32, 28, 28, 22, 22, 21, 6, 6, 37, 4, 4, 2, 2, 4, 2, 2, 3, 13, 12, 37, 4, 7, 0, 27, 25, 0, 0, 0, 0, 0, 72, 26, 181, 225, 42, 6, 4, 0, 11, 18, 0, 0, 37, 20, 35, 33, 3, 0, 0, 4, 22, 39, 37, 53, 51, 53, 51, 0, 28, 56, 42, 33, 81, 0, 0, 0, 0, 0, 0, 26, 32, 32, 20, 32, 62, 44, 64, 84, 9, 254, 16, 12, 36, 44, 64, 22, 6, 14, 12, 97, 2, 2, 113, 35, 6, 79, 35, 35, 22, 13, 255, 14, 18, 43, 8, 22, 14, 36, 43, 41, 32, 243, 14, 93, 0, 100, 36, 60, 30, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 8, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 42, 40, 63, 182, 40, 42, 0, 0, 0, 0, 0, 32, 52, 53, 6, 2, 38, 19, 254, 36, 30, 18, 18, 51, 9, 0} +#define MAVLINK_MESSAGE_LENGTHS {} #endif #ifndef MAVLINK_MESSAGE_CRCS -#define MAVLINK_MESSAGE_CRCS {50, 124, 137, 0, 237, 217, 104, 119, 117, 0, 0, 89, 0, 0, 0, 0, 0, 0, 0, 137, 214, 159, 220, 168, 24, 23, 170, 144, 67, 115, 39, 246, 185, 104, 237, 244, 222, 212, 9, 254, 230, 28, 28, 132, 221, 232, 11, 153, 41, 39, 78, 196, 132, 0, 15, 3, 0, 0, 0, 0, 0, 167, 183, 119, 191, 118, 148, 21, 0, 243, 124, 0, 0, 38, 20, 158, 152, 143, 0, 0, 14, 106, 49, 22, 143, 140, 5, 150, 0, 231, 183, 63, 54, 47, 0, 0, 0, 0, 0, 0, 175, 102, 158, 208, 56, 93, 138, 108, 32, 185, 84, 34, 174, 124, 237, 4, 76, 128, 56, 116, 134, 237, 203, 250, 87, 203, 220, 25, 226, 46, 29, 223, 85, 6, 229, 203, 1, 195, 109, 168, 181, 47, 72, 131, 127, 0, 103, 154, 178, 200, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 189, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 163, 105, 151, 35, 150, 179, 0, 0, 0, 0, 0, 90, 104, 85, 95, 130, 184, 81, 8, 204, 49, 170, 44, 83, 46, 0} +#define MAVLINK_MESSAGE_CRCS {{0, 50, 9, 9, 0, 0, 0}, {1, 124, 31, 31, 0, 0, 0}, {2, 137, 12, 12, 0, 0, 0}, {4, 237, 14, 14, 3, 12, 13}, {5, 217, 28, 28, 1, 0, 0}, {6, 104, 3, 3, 0, 0, 0}, {7, 119, 32, 32, 0, 0, 0}, {8, 117, 36, 36, 0, 0, 0}, {11, 89, 6, 6, 1, 4, 0}, {19, 137, 24, 24, 3, 4, 5}, {20, 214, 20, 20, 3, 2, 3}, {21, 159, 2, 2, 3, 0, 1}, {22, 220, 25, 25, 0, 0, 0}, {23, 168, 23, 23, 3, 4, 5}, {24, 24, 30, 52, 0, 0, 0}, {25, 23, 101, 101, 0, 0, 0}, {26, 170, 22, 24, 0, 0, 0}, {27, 144, 26, 29, 0, 0, 0}, {28, 67, 16, 16, 0, 0, 0}, {29, 115, 14, 16, 0, 0, 0}, {30, 39, 28, 28, 0, 0, 0}, {31, 246, 32, 48, 0, 0, 0}, {32, 185, 28, 28, 0, 0, 0}, {33, 104, 28, 28, 0, 0, 0}, {34, 237, 22, 22, 0, 0, 0}, {35, 244, 22, 22, 0, 0, 0}, {36, 222, 21, 37, 0, 0, 0}, {37, 212, 6, 7, 3, 4, 5}, {38, 9, 6, 7, 3, 4, 5}, {39, 254, 37, 38, 3, 32, 33}, {40, 230, 4, 5, 3, 2, 3}, {41, 28, 4, 4, 3, 2, 3}, {42, 28, 2, 2, 0, 0, 0}, {43, 132, 2, 3, 3, 0, 1}, {44, 221, 4, 5, 3, 2, 3}, {45, 232, 2, 3, 3, 0, 1}, {46, 11, 2, 2, 0, 0, 0}, {47, 153, 3, 4, 3, 0, 1}, {48, 41, 13, 21, 1, 12, 0}, {49, 39, 12, 20, 0, 0, 0}, {50, 78, 37, 37, 3, 18, 19}, {51, 196, 4, 5, 3, 2, 3}, {52, 132, 7, 7, 0, 0, 0}, {54, 15, 27, 27, 3, 24, 25}, {55, 3, 25, 25, 0, 0, 0}, {61, 167, 72, 72, 0, 0, 0}, {62, 183, 26, 26, 0, 0, 0}, {63, 119, 181, 181, 0, 0, 0}, {64, 191, 225, 225, 0, 0, 0}, {65, 118, 42, 42, 0, 0, 0}, {66, 148, 6, 6, 3, 2, 3}, {67, 21, 4, 4, 0, 0, 0}, {69, 243, 11, 11, 1, 10, 0}, {70, 124, 18, 38, 3, 16, 17}, {73, 38, 37, 38, 3, 32, 33}, {74, 20, 20, 20, 0, 0, 0}, {75, 158, 35, 35, 3, 30, 31}, {76, 152, 33, 33, 3, 30, 31}, {77, 143, 3, 10, 3, 8, 9}, {80, 14, 4, 4, 3, 2, 3}, {81, 106, 22, 22, 0, 0, 0}, {82, 49, 39, 39, 3, 36, 37}, {83, 22, 37, 37, 0, 0, 0}, {84, 143, 53, 53, 3, 50, 51}, {85, 140, 51, 51, 0, 0, 0}, {86, 5, 53, 53, 3, 50, 51}, {87, 150, 51, 51, 0, 0, 0}, {89, 231, 28, 28, 0, 0, 0}, {90, 183, 56, 56, 0, 0, 0}, {91, 63, 42, 42, 0, 0, 0}, {92, 54, 33, 33, 0, 0, 0}, {93, 47, 81, 81, 0, 0, 0}, {100, 175, 26, 34, 0, 0, 0}, {101, 102, 32, 117, 0, 0, 0}, {102, 158, 32, 117, 0, 0, 0}, {103, 208, 20, 57, 0, 0, 0}, {104, 56, 32, 116, 0, 0, 0}, {105, 93, 62, 63, 0, 0, 0}, {106, 138, 44, 44, 0, 0, 0}, {107, 108, 64, 65, 0, 0, 0}, {108, 32, 84, 84, 0, 0, 0}, {109, 185, 9, 9, 0, 0, 0}, {110, 84, 254, 254, 3, 1, 2}, {111, 34, 16, 16, 0, 0, 0}, {112, 174, 12, 12, 0, 0, 0}, {113, 124, 36, 39, 0, 0, 0}, {114, 237, 44, 44, 0, 0, 0}, {115, 4, 64, 64, 0, 0, 0}, {116, 76, 22, 24, 0, 0, 0}, {117, 128, 6, 6, 3, 4, 5}, {118, 56, 14, 14, 0, 0, 0}, {119, 116, 12, 12, 3, 10, 11}, {120, 134, 97, 97, 0, 0, 0}, {121, 237, 2, 2, 3, 0, 1}, {122, 203, 2, 2, 3, 0, 1}, {123, 250, 113, 113, 3, 0, 1}, {124, 87, 35, 37, 0, 0, 0}, {125, 203, 6, 6, 0, 0, 0}, {126, 220, 79, 79, 0, 0, 0}, {127, 25, 35, 35, 0, 0, 0}, {128, 226, 35, 35, 0, 0, 0}, {129, 46, 22, 24, 0, 0, 0}, {130, 29, 13, 13, 0, 0, 0}, {131, 223, 255, 255, 0, 0, 0}, {132, 85, 14, 39, 0, 0, 0}, {133, 6, 18, 18, 0, 0, 0}, {134, 229, 43, 43, 0, 0, 0}, {135, 203, 8, 8, 0, 0, 0}, {136, 1, 22, 22, 0, 0, 0}, {137, 195, 14, 16, 0, 0, 0}, {138, 109, 36, 120, 0, 0, 0}, {139, 168, 43, 43, 3, 41, 42}, {140, 181, 41, 41, 0, 0, 0}, {141, 47, 32, 32, 0, 0, 0}, {142, 72, 243, 243, 0, 0, 0}, {143, 131, 14, 16, 0, 0, 0}, {144, 127, 93, 93, 0, 0, 0}, {146, 103, 100, 100, 0, 0, 0}, {147, 154, 36, 54, 0, 0, 0}, {148, 178, 60, 78, 0, 0, 0}, {149, 200, 30, 60, 0, 0, 0}, {162, 189, 8, 9, 0, 0, 0}, {192, 36, 44, 54, 0, 0, 0}, {225, 208, 65, 65, 0, 0, 0}, {230, 163, 42, 42, 0, 0, 0}, {231, 105, 40, 40, 0, 0, 0}, {232, 151, 63, 65, 0, 0, 0}, {233, 35, 182, 182, 0, 0, 0}, {234, 150, 40, 40, 0, 0, 0}, {235, 179, 42, 42, 0, 0, 0}, {241, 90, 32, 32, 0, 0, 0}, {242, 104, 52, 60, 0, 0, 0}, {243, 85, 53, 61, 1, 52, 0}, {244, 95, 6, 6, 0, 0, 0}, {245, 130, 2, 2, 0, 0, 0}, {246, 184, 38, 38, 0, 0, 0}, {247, 81, 19, 19, 0, 0, 0}, {248, 8, 254, 254, 3, 3, 4}, {249, 204, 36, 36, 0, 0, 0}, {250, 49, 30, 30, 0, 0, 0}, {251, 170, 18, 18, 0, 0, 0}, {252, 44, 18, 18, 0, 0, 0}, {253, 83, 51, 54, 0, 0, 0}, {254, 46, 9, 9, 0, 0, 0}, {256, 71, 42, 42, 3, 8, 9}, {257, 131, 9, 9, 0, 0, 0}, {258, 187, 32, 232, 3, 0, 1}, {259, 92, 235, 235, 0, 0, 0}, {260, 146, 5, 13, 0, 0, 0}, {261, 179, 27, 60, 0, 0, 0}, {262, 12, 18, 22, 0, 0, 0}, {263, 133, 255, 255, 0, 0, 0}, {264, 49, 28, 28, 0, 0, 0}, {265, 26, 16, 20, 0, 0, 0}, {266, 193, 255, 255, 3, 2, 3}, {267, 35, 255, 255, 3, 2, 3}, {268, 14, 4, 4, 3, 2, 3}, {269, 109, 213, 213, 0, 0, 0}, {270, 59, 19, 19, 0, 0, 0}, {271, 22, 52, 52, 0, 0, 0}, {275, 126, 31, 31, 0, 0, 0}, {276, 18, 49, 49, 0, 0, 0}, {280, 70, 33, 33, 0, 0, 0}, {281, 48, 13, 13, 0, 0, 0}, {282, 123, 35, 35, 3, 32, 33}, {283, 74, 144, 144, 0, 0, 0}, {284, 99, 32, 32, 3, 30, 31}, {285, 137, 40, 40, 3, 38, 39}, {286, 210, 53, 53, 3, 50, 51}, {287, 1, 23, 23, 3, 20, 21}, {288, 20, 23, 23, 3, 20, 21}, {290, 221, 42, 42, 0, 0, 0}, {291, 10, 57, 57, 0, 0, 0}, {299, 19, 96, 98, 0, 0, 0}, {300, 217, 22, 22, 0, 0, 0}, {301, 243, 58, 58, 0, 0, 0}, {310, 28, 17, 17, 0, 0, 0}, {311, 95, 116, 116, 0, 0, 0}, {320, 243, 20, 20, 3, 2, 3}, {321, 88, 2, 2, 3, 0, 1}, {322, 243, 149, 149, 0, 0, 0}, {323, 78, 147, 147, 3, 0, 1}, {324, 132, 146, 146, 0, 0, 0}, {330, 23, 158, 167, 0, 0, 0}, {331, 91, 230, 232, 0, 0, 0}, {332, 236, 239, 239, 0, 0, 0}, {333, 231, 109, 109, 0, 0, 0}, {334, 72, 10, 10, 0, 0, 0}, {335, 225, 24, 24, 0, 0, 0}, {336, 245, 84, 84, 0, 0, 0}, {339, 199, 5, 5, 0, 0, 0}, {340, 99, 70, 70, 0, 0, 0}, {350, 232, 20, 252, 0, 0, 0}, {360, 11, 25, 25, 0, 0, 0}, {370, 75, 87, 87, 0, 0, 0}, {373, 117, 42, 42, 0, 0, 0}, {375, 251, 140, 140, 0, 0, 0}, {380, 232, 20, 20, 0, 0, 0}, {385, 147, 133, 133, 3, 2, 3}, {390, 156, 238, 238, 0, 0, 0}, {395, 163, 156, 156, 0, 0, 0}, {400, 110, 254, 254, 3, 4, 5}, {401, 183, 6, 6, 3, 4, 5}, {9000, 113, 137, 137, 0, 0, 0}, {9005, 117, 34, 34, 0, 0, 0}, {12900, 114, 44, 44, 3, 0, 1}, {12901, 254, 59, 59, 3, 30, 31}, {12902, 49, 53, 53, 3, 4, 5}, {12903, 249, 46, 46, 3, 0, 1}, {12904, 203, 46, 46, 3, 20, 21}, {12905, 49, 43, 43, 3, 0, 1}, {12915, 62, 254, 254, 3, 0, 1}} #endif #include "../protocol.h" @@ -34,79 +34,6 @@ extern "C" { // ENUM DEFINITIONS -/** @brief Micro air vehicle / autopilot classes. This identifies the individual model. */ -#ifndef HAVE_ENUM_MAV_AUTOPILOT -#define HAVE_ENUM_MAV_AUTOPILOT -typedef enum MAV_AUTOPILOT -{ - MAV_AUTOPILOT_GENERIC=0, /* Generic autopilot, full support for everything | */ - MAV_AUTOPILOT_RESERVED=1, /* Reserved for future use. | */ - MAV_AUTOPILOT_SLUGS=2, /* SLUGS autopilot, http://slugsuav.soe.ucsc.edu | */ - MAV_AUTOPILOT_ARDUPILOTMEGA=3, /* ArduPilot - Plane/Copter/Rover/Sub/Tracker, https://ardupilot.org | */ - MAV_AUTOPILOT_OPENPILOT=4, /* OpenPilot, http://openpilot.org | */ - MAV_AUTOPILOT_GENERIC_WAYPOINTS_ONLY=5, /* Generic autopilot only supporting simple waypoints | */ - MAV_AUTOPILOT_GENERIC_WAYPOINTS_AND_SIMPLE_NAVIGATION_ONLY=6, /* Generic autopilot supporting waypoints and other simple navigation commands | */ - MAV_AUTOPILOT_GENERIC_MISSION_FULL=7, /* Generic autopilot supporting the full mission command set | */ - MAV_AUTOPILOT_INVALID=8, /* No valid autopilot, e.g. a GCS or other MAVLink component | */ - MAV_AUTOPILOT_PPZ=9, /* PPZ UAV - http://nongnu.org/paparazzi | */ - MAV_AUTOPILOT_UDB=10, /* UAV Dev Board | */ - MAV_AUTOPILOT_FP=11, /* FlexiPilot | */ - MAV_AUTOPILOT_PX4=12, /* PX4 Autopilot - http://px4.io/ | */ - MAV_AUTOPILOT_SMACCMPILOT=13, /* SMACCMPilot - http://smaccmpilot.org | */ - MAV_AUTOPILOT_AUTOQUAD=14, /* AutoQuad -- http://autoquad.org | */ - MAV_AUTOPILOT_ARMAZILA=15, /* Armazila -- http://armazila.com | */ - MAV_AUTOPILOT_AEROB=16, /* Aerob -- http://aerob.ru | */ - MAV_AUTOPILOT_ASLUAV=17, /* ASLUAV autopilot -- http://www.asl.ethz.ch | */ - MAV_AUTOPILOT_SMARTAP=18, /* SmartAP Autopilot - http://sky-drones.com | */ - MAV_AUTOPILOT_AIRRAILS=19, /* AirRails - http://uaventure.com | */ - MAV_AUTOPILOT_ENUM_END=20, /* | */ -} MAV_AUTOPILOT; -#endif - -/** @brief MAVLINK component type reported in HEARTBEAT message. Flight controllers must report the type of the vehicle on which they are mounted (e.g. MAV_TYPE_OCTOROTOR). All other components must report a value appropriate for their type (e.g. a camera must use MAV_TYPE_CAMERA). */ -#ifndef HAVE_ENUM_MAV_TYPE -#define HAVE_ENUM_MAV_TYPE -typedef enum MAV_TYPE -{ - MAV_TYPE_GENERIC=0, /* Generic micro air vehicle | */ - MAV_TYPE_FIXED_WING=1, /* Fixed wing aircraft. | */ - MAV_TYPE_QUADROTOR=2, /* Quadrotor | */ - MAV_TYPE_COAXIAL=3, /* Coaxial helicopter | */ - MAV_TYPE_HELICOPTER=4, /* Normal helicopter with tail rotor. | */ - MAV_TYPE_ANTENNA_TRACKER=5, /* Ground installation | */ - MAV_TYPE_GCS=6, /* Operator control unit / ground control station | */ - MAV_TYPE_AIRSHIP=7, /* Airship, controlled | */ - MAV_TYPE_FREE_BALLOON=8, /* Free balloon, uncontrolled | */ - MAV_TYPE_ROCKET=9, /* Rocket | */ - MAV_TYPE_GROUND_ROVER=10, /* Ground rover | */ - MAV_TYPE_SURFACE_BOAT=11, /* Surface vessel, boat, ship | */ - MAV_TYPE_SUBMARINE=12, /* Submarine | */ - MAV_TYPE_HEXAROTOR=13, /* Hexarotor | */ - MAV_TYPE_OCTOROTOR=14, /* Octorotor | */ - MAV_TYPE_TRICOPTER=15, /* Tricopter | */ - MAV_TYPE_FLAPPING_WING=16, /* Flapping wing | */ - MAV_TYPE_KITE=17, /* Kite | */ - MAV_TYPE_ONBOARD_CONTROLLER=18, /* Onboard companion controller | */ - MAV_TYPE_VTOL_DUOROTOR=19, /* Two-rotor VTOL using control surfaces in vertical operation in addition. Tailsitter. | */ - MAV_TYPE_VTOL_QUADROTOR=20, /* Quad-rotor VTOL using a V-shaped quad config in vertical operation. Tailsitter. | */ - MAV_TYPE_VTOL_TILTROTOR=21, /* Tiltrotor VTOL | */ - MAV_TYPE_VTOL_RESERVED2=22, /* VTOL reserved 2 | */ - MAV_TYPE_VTOL_RESERVED3=23, /* VTOL reserved 3 | */ - MAV_TYPE_VTOL_RESERVED4=24, /* VTOL reserved 4 | */ - MAV_TYPE_VTOL_RESERVED5=25, /* VTOL reserved 5 | */ - MAV_TYPE_GIMBAL=26, /* Gimbal | */ - MAV_TYPE_ADSB=27, /* ADSB system | */ - MAV_TYPE_PARAFOIL=28, /* Steerable, nonrigid airfoil | */ - MAV_TYPE_DODECAROTOR=29, /* Dodecarotor | */ - MAV_TYPE_CAMERA=30, /* Camera | */ - MAV_TYPE_CHARGING_STATION=31, /* Charging station | */ - MAV_TYPE_FLARM=32, /* FLARM collision avoidance system | */ - MAV_TYPE_SERVO=33, /* Servo | */ - MAV_TYPE_ODID=34, /* Open Drone ID. See https://mavlink.io/en/services/opendroneid.html. | */ - MAV_TYPE_ENUM_END=35, /* | */ -} MAV_TYPE; -#endif - /** @brief These values define the type of firmware release. These values indicate the first version or release of this type. For example the first alpha release would be 64, the second would be 65. */ #ifndef HAVE_ENUM_FIRMWARE_VERSION_TYPE #define HAVE_ENUM_FIRMWARE_VERSION_TYPE @@ -144,40 +71,6 @@ typedef enum HL_FAILURE_FLAG } HL_FAILURE_FLAG; #endif -/** @brief These flags encode the MAV mode. */ -#ifndef HAVE_ENUM_MAV_MODE_FLAG -#define HAVE_ENUM_MAV_MODE_FLAG -typedef enum MAV_MODE_FLAG -{ - MAV_MODE_FLAG_CUSTOM_MODE_ENABLED=1, /* 0b00000001 Reserved for future use. | */ - MAV_MODE_FLAG_TEST_ENABLED=2, /* 0b00000010 system has a test mode enabled. This flag is intended for temporary system tests and should not be used for stable implementations. | */ - MAV_MODE_FLAG_AUTO_ENABLED=4, /* 0b00000100 autonomous mode enabled, system finds its own goal positions. Guided flag can be set or not, depends on the actual implementation. | */ - MAV_MODE_FLAG_GUIDED_ENABLED=8, /* 0b00001000 guided mode enabled, system flies waypoints / mission items. | */ - MAV_MODE_FLAG_STABILIZE_ENABLED=16, /* 0b00010000 system stabilizes electronically its attitude (and optionally position). It needs however further control inputs to move around. | */ - MAV_MODE_FLAG_HIL_ENABLED=32, /* 0b00100000 hardware in the loop simulation. All motors / actuators are blocked, but internal software is full operational. | */ - MAV_MODE_FLAG_MANUAL_INPUT_ENABLED=64, /* 0b01000000 remote control input is enabled. | */ - MAV_MODE_FLAG_SAFETY_ARMED=128, /* 0b10000000 MAV safety set to armed. Motors are enabled / running / can start. Ready to fly. Additional note: this flag is to be ignore when sent in the command MAV_CMD_DO_SET_MODE and MAV_CMD_COMPONENT_ARM_DISARM shall be used instead. The flag can still be used to report the armed state. | */ - MAV_MODE_FLAG_ENUM_END=129, /* | */ -} MAV_MODE_FLAG; -#endif - -/** @brief These values encode the bit positions of the decode position. These values can be used to read the value of a flag bit by combining the base_mode variable with AND with the flag position value. The result will be either 0 or 1, depending on if the flag is set or not. */ -#ifndef HAVE_ENUM_MAV_MODE_FLAG_DECODE_POSITION -#define HAVE_ENUM_MAV_MODE_FLAG_DECODE_POSITION -typedef enum MAV_MODE_FLAG_DECODE_POSITION -{ - MAV_MODE_FLAG_DECODE_POSITION_CUSTOM_MODE=1, /* Eighth bit: 00000001 | */ - MAV_MODE_FLAG_DECODE_POSITION_TEST=2, /* Seventh bit: 00000010 | */ - MAV_MODE_FLAG_DECODE_POSITION_AUTO=4, /* Sixth bit: 00000100 | */ - MAV_MODE_FLAG_DECODE_POSITION_GUIDED=8, /* Fifth bit: 00001000 | */ - MAV_MODE_FLAG_DECODE_POSITION_STABILIZE=16, /* Fourth bit: 00010000 | */ - MAV_MODE_FLAG_DECODE_POSITION_HIL=32, /* Third bit: 00100000 | */ - MAV_MODE_FLAG_DECODE_POSITION_MANUAL=64, /* Second bit: 01000000 | */ - MAV_MODE_FLAG_DECODE_POSITION_SAFETY=128, /* First bit: 10000000 | */ - MAV_MODE_FLAG_DECODE_POSITION_ENUM_END=129, /* | */ -} MAV_MODE_FLAG_DECODE_POSITION; -#endif - /** @brief Actions that may be specified in MAV_CMD_OVERRIDE_GOTO to override mission execution. */ #ifndef HAVE_ENUM_MAV_GOTO #define HAVE_ENUM_MAV_GOTO @@ -212,162 +105,6 @@ typedef enum MAV_MODE } MAV_MODE; #endif -/** @brief */ -#ifndef HAVE_ENUM_MAV_STATE -#define HAVE_ENUM_MAV_STATE -typedef enum MAV_STATE -{ - MAV_STATE_UNINIT=0, /* Uninitialized system, state is unknown. | */ - MAV_STATE_BOOT=1, /* System is booting up. | */ - MAV_STATE_CALIBRATING=2, /* System is calibrating and not flight-ready. | */ - MAV_STATE_STANDBY=3, /* System is grounded and on standby. It can be launched any time. | */ - MAV_STATE_ACTIVE=4, /* System is active and might be already airborne. Motors are engaged. | */ - MAV_STATE_CRITICAL=5, /* System is in a non-normal flight mode. It can however still navigate. | */ - MAV_STATE_EMERGENCY=6, /* System is in a non-normal flight mode. It lost control over parts or over the whole airframe. It is in mayday and going down. | */ - MAV_STATE_POWEROFF=7, /* System just initialized its power-down sequence, will shut down now. | */ - MAV_STATE_FLIGHT_TERMINATION=8, /* System is terminating itself. | */ - MAV_STATE_ENUM_END=9, /* | */ -} MAV_STATE; -#endif - -/** @brief Component ids (values) for the different types and instances of onboard hardware/software that might make up a MAVLink system (autopilot, cameras, servos, GPS systems, avoidance systems etc.). - Components must use the appropriate ID in their source address when sending messages. Components can also use IDs to determine if they are the intended recipient of an incoming message. The MAV_COMP_ID_ALL value is used to indicate messages that must be processed by all components. - When creating new entries, components that can have multiple instances (e.g. cameras, servos etc.) should be allocated sequential values. An appropriate number of values should be left free after these components to allow the number of instances to be expanded. */ -#ifndef HAVE_ENUM_MAV_COMPONENT -#define HAVE_ENUM_MAV_COMPONENT -typedef enum MAV_COMPONENT -{ - MAV_COMP_ID_ALL=0, /* Target id (target_component) used to broadcast messages to all components of the receiving system. Components should attempt to process messages with this component ID and forward to components on any other interfaces. Note: This is not a valid *source* component id for a message. | */ - MAV_COMP_ID_AUTOPILOT1=1, /* System flight controller component ("autopilot"). Only one autopilot is expected in a particular system. | */ - MAV_COMP_ID_USER1=25, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ - MAV_COMP_ID_USER2=26, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ - MAV_COMP_ID_USER3=27, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ - MAV_COMP_ID_USER4=28, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ - MAV_COMP_ID_USER5=29, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ - MAV_COMP_ID_USER6=30, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ - MAV_COMP_ID_USER7=31, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ - MAV_COMP_ID_USER8=32, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ - MAV_COMP_ID_USER9=33, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ - MAV_COMP_ID_USER10=34, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ - MAV_COMP_ID_USER11=35, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ - MAV_COMP_ID_USER12=36, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ - MAV_COMP_ID_USER13=37, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ - MAV_COMP_ID_USER14=38, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ - MAV_COMP_ID_USER15=39, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ - MAV_COMP_ID_USER16=40, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ - MAV_COMP_ID_USER17=41, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ - MAV_COMP_ID_USER18=42, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ - MAV_COMP_ID_USER19=43, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ - MAV_COMP_ID_USER20=44, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ - MAV_COMP_ID_USER21=45, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ - MAV_COMP_ID_USER22=46, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ - MAV_COMP_ID_USER23=47, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ - MAV_COMP_ID_USER24=48, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ - MAV_COMP_ID_USER25=49, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ - MAV_COMP_ID_USER26=50, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ - MAV_COMP_ID_USER27=51, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ - MAV_COMP_ID_USER28=52, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ - MAV_COMP_ID_USER29=53, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ - MAV_COMP_ID_USER30=54, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ - MAV_COMP_ID_USER31=55, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ - MAV_COMP_ID_USER32=56, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ - MAV_COMP_ID_USER33=57, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ - MAV_COMP_ID_USER34=58, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ - MAV_COMP_ID_USER35=59, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ - MAV_COMP_ID_USER36=60, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ - MAV_COMP_ID_USER37=61, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ - MAV_COMP_ID_USER38=62, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ - MAV_COMP_ID_USER39=63, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ - MAV_COMP_ID_USER40=64, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ - MAV_COMP_ID_USER41=65, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ - MAV_COMP_ID_USER42=66, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ - MAV_COMP_ID_USER43=67, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ - MAV_COMP_ID_TELEMETRY_RADIO=68, /* Telemetry radio (e.g. SiK radio, or other component that emits RADIO_STATUS messages). | */ - MAV_COMP_ID_USER45=69, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ - MAV_COMP_ID_USER46=70, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ - MAV_COMP_ID_USER47=71, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ - MAV_COMP_ID_USER48=72, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ - MAV_COMP_ID_USER49=73, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ - MAV_COMP_ID_USER50=74, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ - MAV_COMP_ID_USER51=75, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ - MAV_COMP_ID_USER52=76, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ - MAV_COMP_ID_USER53=77, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ - MAV_COMP_ID_USER54=78, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ - MAV_COMP_ID_USER55=79, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ - MAV_COMP_ID_USER56=80, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ - MAV_COMP_ID_USER57=81, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ - MAV_COMP_ID_USER58=82, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ - MAV_COMP_ID_USER59=83, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ - MAV_COMP_ID_USER60=84, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ - MAV_COMP_ID_USER61=85, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ - MAV_COMP_ID_USER62=86, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ - MAV_COMP_ID_USER63=87, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ - MAV_COMP_ID_USER64=88, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ - MAV_COMP_ID_USER65=89, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ - MAV_COMP_ID_USER66=90, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ - MAV_COMP_ID_USER67=91, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ - MAV_COMP_ID_USER68=92, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ - MAV_COMP_ID_USER69=93, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ - MAV_COMP_ID_USER70=94, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ - MAV_COMP_ID_USER71=95, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ - MAV_COMP_ID_USER72=96, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ - MAV_COMP_ID_USER73=97, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ - MAV_COMP_ID_USER74=98, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ - MAV_COMP_ID_USER75=99, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ - MAV_COMP_ID_CAMERA=100, /* Camera #1. | */ - MAV_COMP_ID_CAMERA2=101, /* Camera #2. | */ - MAV_COMP_ID_CAMERA3=102, /* Camera #3. | */ - MAV_COMP_ID_CAMERA4=103, /* Camera #4. | */ - MAV_COMP_ID_CAMERA5=104, /* Camera #5. | */ - MAV_COMP_ID_CAMERA6=105, /* Camera #6. | */ - MAV_COMP_ID_SERVO1=140, /* Servo #1. | */ - MAV_COMP_ID_SERVO2=141, /* Servo #2. | */ - MAV_COMP_ID_SERVO3=142, /* Servo #3. | */ - MAV_COMP_ID_SERVO4=143, /* Servo #4. | */ - MAV_COMP_ID_SERVO5=144, /* Servo #5. | */ - MAV_COMP_ID_SERVO6=145, /* Servo #6. | */ - MAV_COMP_ID_SERVO7=146, /* Servo #7. | */ - MAV_COMP_ID_SERVO8=147, /* Servo #8. | */ - MAV_COMP_ID_SERVO9=148, /* Servo #9. | */ - MAV_COMP_ID_SERVO10=149, /* Servo #10. | */ - MAV_COMP_ID_SERVO11=150, /* Servo #11. | */ - MAV_COMP_ID_SERVO12=151, /* Servo #12. | */ - MAV_COMP_ID_SERVO13=152, /* Servo #13. | */ - MAV_COMP_ID_SERVO14=153, /* Servo #14. | */ - MAV_COMP_ID_GIMBAL=154, /* Gimbal #1. | */ - MAV_COMP_ID_LOG=155, /* Logging component. | */ - MAV_COMP_ID_ADSB=156, /* Automatic Dependent Surveillance-Broadcast (ADS-B) component. | */ - MAV_COMP_ID_OSD=157, /* On Screen Display (OSD) devices for video links. | */ - MAV_COMP_ID_PERIPHERAL=158, /* Generic autopilot peripheral component ID. Meant for devices that do not implement the parameter microservice. | */ - MAV_COMP_ID_QX1_GIMBAL=159, /* Gimbal ID for QX1. | */ - MAV_COMP_ID_FLARM=160, /* FLARM collision alert component. | */ - MAV_COMP_ID_GIMBAL2=171, /* Gimbal #2. | */ - MAV_COMP_ID_GIMBAL3=172, /* Gimbal #3. | */ - MAV_COMP_ID_GIMBAL4=173, /* Gimbal #4 | */ - MAV_COMP_ID_GIMBAL5=174, /* Gimbal #5. | */ - MAV_COMP_ID_GIMBAL6=175, /* Gimbal #6. | */ - MAV_COMP_ID_MISSIONPLANNER=190, /* Component that can generate/supply a mission flight plan (e.g. GCS or developer API). | */ - MAV_COMP_ID_ONBOARD_COMPUTER=191, /* Component that lives on the onboard computer (companion computer) and has some generic functionalities, such as settings system parameters and monitoring the status of some processes that don't directly speak mavlink and so on. | */ - MAV_COMP_ID_PATHPLANNER=195, /* Component that finds an optimal path between points based on a certain constraint (e.g. minimum snap, shortest path, cost, etc.). | */ - MAV_COMP_ID_OBSTACLE_AVOIDANCE=196, /* Component that plans a collision free path between two points. | */ - MAV_COMP_ID_VISUAL_INERTIAL_ODOMETRY=197, /* Component that provides position estimates using VIO techniques. | */ - MAV_COMP_ID_PAIRING_MANAGER=198, /* Component that manages pairing of vehicle and GCS. | */ - MAV_COMP_ID_IMU=200, /* Inertial Measurement Unit (IMU) #1. | */ - MAV_COMP_ID_IMU_2=201, /* Inertial Measurement Unit (IMU) #2. | */ - MAV_COMP_ID_IMU_3=202, /* Inertial Measurement Unit (IMU) #3. | */ - MAV_COMP_ID_GPS=220, /* GPS #1. | */ - MAV_COMP_ID_GPS2=221, /* GPS #2. | */ - MAV_COMP_ID_ODID_TXRX_1=236, /* Open Drone ID transmitter/receiver (Bluetooth/WiFi/Internet). | */ - MAV_COMP_ID_ODID_TXRX_2=237, /* Open Drone ID transmitter/receiver (Bluetooth/WiFi/Internet). | */ - MAV_COMP_ID_ODID_TXRX_3=238, /* Open Drone ID transmitter/receiver (Bluetooth/WiFi/Internet). | */ - MAV_COMP_ID_UDP_BRIDGE=240, /* Component to bridge MAVLink to UDP (i.e. from a UART). | */ - MAV_COMP_ID_UART_BRIDGE=241, /* Component to bridge to UART (i.e. from UDP). | */ - MAV_COMP_ID_TUNNEL_NODE=242, /* Component handling TUNNEL messages (e.g. vendor specific GUI of a component). | */ - MAV_COMP_ID_SYSTEM_CONTROL=250, /* Component for handling system messages (e.g. to ARM, takeoff, etc.). | */ - MAV_COMPONENT_ENUM_END=251, /* | */ -} MAV_COMPONENT; -#endif - /** @brief These encode the sensors whose status is sent as part of the SYS_STATUS message. */ #ifndef HAVE_ENUM_MAV_SYS_STATUS_SENSOR #define HAVE_ENUM_MAV_SYS_STATUS_SENSOR @@ -403,7 +140,8 @@ typedef enum MAV_SYS_STATUS_SENSOR MAV_SYS_STATUS_SENSOR_SATCOM=134217728, /* 0x8000000 Satellite Communication | */ MAV_SYS_STATUS_PREARM_CHECK=268435456, /* 0x10000000 pre-arm check status. Always healthy when armed | */ MAV_SYS_STATUS_OBSTACLE_AVOIDANCE=536870912, /* 0x20000000 Avoidance/collision prevention | */ - MAV_SYS_STATUS_SENSOR_ENUM_END=536870913, /* | */ + MAV_SYS_STATUS_SENSOR_PROPULSION=1073741824, /* 0x40000000 propulsion (actuator, esc, motor or propellor) | */ + MAV_SYS_STATUS_SENSOR_ENUM_END=1073741825, /* | */ } MAV_SYS_STATUS_SENSOR; #endif @@ -443,13 +181,13 @@ typedef enum MAV_FRAME #define HAVE_ENUM_MAVLINK_DATA_STREAM_TYPE typedef enum MAVLINK_DATA_STREAM_TYPE { - MAVLINK_DATA_STREAM_IMG_JPEG=1, /* | */ - MAVLINK_DATA_STREAM_IMG_BMP=2, /* | */ - MAVLINK_DATA_STREAM_IMG_RAW8U=3, /* | */ - MAVLINK_DATA_STREAM_IMG_RAW32U=4, /* | */ - MAVLINK_DATA_STREAM_IMG_PGM=5, /* | */ - MAVLINK_DATA_STREAM_IMG_PNG=6, /* | */ - MAVLINK_DATA_STREAM_TYPE_ENUM_END=7, /* | */ + MAVLINK_DATA_STREAM_IMG_JPEG=0, /* | */ + MAVLINK_DATA_STREAM_IMG_BMP=1, /* | */ + MAVLINK_DATA_STREAM_IMG_RAW8U=2, /* | */ + MAVLINK_DATA_STREAM_IMG_RAW32U=3, /* | */ + MAVLINK_DATA_STREAM_IMG_PGM=4, /* | */ + MAVLINK_DATA_STREAM_IMG_PNG=5, /* | */ + MAVLINK_DATA_STREAM_TYPE_ENUM_END=6, /* | */ } MAVLINK_DATA_STREAM_TYPE; #endif @@ -503,7 +241,8 @@ typedef enum MAV_MOUNT_MODE MAV_MOUNT_MODE_RC_TARGETING=3, /* Load neutral position and start RC Roll,Pitch,Yaw control with stabilization | */ MAV_MOUNT_MODE_GPS_POINT=4, /* Load neutral position and start to point to Lat,Lon,Alt | */ MAV_MOUNT_MODE_SYSID_TARGET=5, /* Gimbal tracks system with specified system ID | */ - MAV_MOUNT_MODE_ENUM_END=6, /* | */ + MAV_MOUNT_MODE_HOME_LOCATION=6, /* Gimbal tracks home location | */ + MAV_MOUNT_MODE_ENUM_END=7, /* | */ } MAV_MOUNT_MODE; #endif @@ -547,10 +286,7 @@ typedef enum GIMBAL_MANAGER_CAP_FLAGS GIMBAL_MANAGER_CAP_FLAGS_SUPPORTS_INFINITE_YAW=2048, /* Based on GIMBAL_DEVICE_CAP_FLAGS_SUPPORTS_INFINITE_YAW. | */ GIMBAL_MANAGER_CAP_FLAGS_CAN_POINT_LOCATION_LOCAL=65536, /* Gimbal manager supports to point to a local position. | */ GIMBAL_MANAGER_CAP_FLAGS_CAN_POINT_LOCATION_GLOBAL=131072, /* Gimbal manager supports to point to a global latitude, longitude, altitude position. | */ - GIMBAL_MANAGER_CAP_FLAGS_SUPPORTS_FOCAL_LENGTH_SCALE=1048576, /* Gimbal manager supports pitching and yawing at an angular velocity scaled by focal length (the more zoomed in, the slower the movement). | */ - GIMBAL_MANAGER_CAP_FLAGS_SUPPORTS_NUDGING=2097152, /* Gimbal manager supports nudging when pointing to a location or tracking. | */ - GIMBAL_MANAGER_CAP_FLAGS_SUPPORTS_OVERRIDE=4194304, /* Gimbal manager supports overriding when pointing to a location or tracking. | */ - GIMBAL_MANAGER_CAP_FLAGS_ENUM_END=4194305, /* | */ + GIMBAL_MANAGER_CAP_FLAGS_ENUM_END=131073, /* | */ } GIMBAL_MANAGER_CAP_FLAGS; #endif @@ -578,11 +314,7 @@ typedef enum GIMBAL_MANAGER_FLAGS GIMBAL_MANAGER_FLAGS_ROLL_LOCK=4, /* Based on GIMBAL_DEVICE_FLAGS_ROLL_LOCK | */ GIMBAL_MANAGER_FLAGS_PITCH_LOCK=8, /* Based on GIMBAL_DEVICE_FLAGS_PITCH_LOCK | */ GIMBAL_MANAGER_FLAGS_YAW_LOCK=16, /* Based on GIMBAL_DEVICE_FLAGS_YAW_LOCK | */ - GIMBAL_MANAGER_FLAGS_ANGULAR_VELOCITY_RELATIVE_TO_FOCAL_LENGTH=1048576, /* Scale angular velocity relative to focal length. This means the gimbal moves slower if it is zoomed in. | */ - GIMBAL_MANAGER_FLAGS_NUDGE=2097152, /* Interpret attitude control on top of pointing to a location or tracking. If this flag is set, the quaternion is relative to the existing tracking angle. | */ - GIMBAL_MANAGER_FLAGS_OVERRIDE=4194304, /* Completely override pointing to a location or tracking. If this flag is set, the quaternion is (as usual) according to GIMBAL_MANAGER_FLAGS_YAW_LOCK. | */ - GIMBAL_MANAGER_FLAGS_NONE=8388608, /* This flag can be set to give up control previously set using MAV_CMD_DO_GIMBAL_MANAGER_ATTITUDE. This flag must not be combined with other flags. | */ - GIMBAL_MANAGER_FLAGS_ENUM_END=8388609, /* | */ + GIMBAL_MANAGER_FLAGS_ENUM_END=17, /* | */ } GIMBAL_MANAGER_FLAGS; #endif @@ -604,6 +336,29 @@ typedef enum GIMBAL_DEVICE_ERROR_FLAGS } GIMBAL_DEVICE_ERROR_FLAGS; #endif +/** @brief Gripper actions. */ +#ifndef HAVE_ENUM_GRIPPER_ACTIONS +#define HAVE_ENUM_GRIPPER_ACTIONS +typedef enum GRIPPER_ACTIONS +{ + GRIPPER_ACTION_RELEASE=0, /* Gripper release cargo. | */ + GRIPPER_ACTION_GRAB=1, /* Gripper grab onto cargo. | */ + GRIPPER_ACTIONS_ENUM_END=2, /* | */ +} GRIPPER_ACTIONS; +#endif + +/** @brief Winch actions. */ +#ifndef HAVE_ENUM_WINCH_ACTIONS +#define HAVE_ENUM_WINCH_ACTIONS +typedef enum WINCH_ACTIONS +{ + WINCH_RELAXED=0, /* Relax winch. | */ + WINCH_RELATIVE_LENGTH_CONTROL=1, /* Wind or unwind specified length of cable, optionally using specified rate. | */ + WINCH_RATE_CONTROL=2, /* Wind or unwind cable at specified rate. | */ + WINCH_ACTIONS_ENUM_END=3, /* | */ +} WINCH_ACTIONS; +#endif + /** @brief Generalized UAVCAN node health */ #ifndef HAVE_ENUM_UAVCAN_NODE_HEALTH #define HAVE_ENUM_UAVCAN_NODE_HEALTH @@ -676,6 +431,24 @@ typedef enum STORAGE_STATUS } STORAGE_STATUS; #endif +/** @brief Flags to indicate the type of storage. */ +#ifndef HAVE_ENUM_STORAGE_TYPE +#define HAVE_ENUM_STORAGE_TYPE +typedef enum STORAGE_TYPE +{ + STORAGE_TYPE_UNKNOWN=0, /* Storage type is not known. | */ + STORAGE_TYPE_USB_STICK=1, /* Storage type is USB device. | */ + STORAGE_TYPE_SD=2, /* Storage type is SD card. | */ + STORAGE_TYPE_MICROSD=3, /* Storage type is microSD card. | */ + STORAGE_TYPE_CF=4, /* Storage type is CFast. | */ + STORAGE_TYPE_CFE=5, /* Storage type is CFexpress. | */ + STORAGE_TYPE_XQD=6, /* Storage type is XQD. | */ + STORAGE_TYPE_HD=7, /* Storage type is HD mass storage type. | */ + STORAGE_TYPE_OTHER=254, /* Storage type is other, not listed type. | */ + STORAGE_TYPE_ENUM_END=255, /* | */ +} STORAGE_TYPE; +#endif + /** @brief Yaw behaviour during orbit flight. */ #ifndef HAVE_ENUM_ORBIT_YAW_BEHAVIOUR #define HAVE_ENUM_ORBIT_YAW_BEHAVIOUR @@ -739,23 +512,11 @@ typedef enum COMP_METADATA_TYPE { COMP_METADATA_TYPE_VERSION=0, /* Version information which also includes information on other optional supported COMP_METADATA_TYPE's. Must be supported. Only downloadable from vehicle. | */ COMP_METADATA_TYPE_PARAMETER=1, /* Parameter meta data. | */ - COMP_METADATA_TYPE_ENUM_END=2, /* | */ + COMP_METADATA_TYPE_COMMANDS=2, /* Meta data which specifies the commands the vehicle supports. (WIP) | */ + COMP_METADATA_TYPE_ENUM_END=3, /* | */ } COMP_METADATA_TYPE; #endif -/** @brief Possible responses from a PARAM_START_TRANSACTION and PARAM_COMMIT_TRANSACTION messages. */ -#ifndef HAVE_ENUM_PARAM_TRANSACTION_RESPONSE -#define HAVE_ENUM_PARAM_TRANSACTION_RESPONSE -typedef enum PARAM_TRANSACTION_RESPONSE -{ - PARAM_TRANSACTION_RESPONSE_ACCEPTED=0, /* Transaction accepted. | */ - PARAM_TRANSACTION_RESPONSE_FAILED=1, /* Transaction failed. | */ - PARAM_TRANSACTION_RESPONSE_UNSUPPORTED=2, /* Transaction unsupported. | */ - PARAM_TRANSACTION_RESPONSE_INPROGRESS=3, /* Transaction in progress. | */ - PARAM_TRANSACTION_RESPONSE_ENUM_END=4, /* | */ -} PARAM_TRANSACTION_RESPONSE; -#endif - /** @brief Possible transport layers to set and get parameters via mavlink during a parameter transaction. */ #ifndef HAVE_ENUM_PARAM_TRANSACTION_TRANSPORT #define HAVE_ENUM_PARAM_TRANSACTION_TRANSPORT @@ -767,14 +528,15 @@ typedef enum PARAM_TRANSACTION_TRANSPORT } PARAM_TRANSACTION_TRANSPORT; #endif -/** @brief Possible parameter transaction action during a commit. */ +/** @brief Possible parameter transaction actions. */ #ifndef HAVE_ENUM_PARAM_TRANSACTION_ACTION #define HAVE_ENUM_PARAM_TRANSACTION_ACTION typedef enum PARAM_TRANSACTION_ACTION { - PARAM_TRANSACTION_ACTION_COMMIT=0, /* Commit the current parameter transaction. | */ - PARAM_TRANSACTION_ACTION_CANCEL=1, /* Cancel the current parameter transaction. | */ - PARAM_TRANSACTION_ACTION_ENUM_END=2, /* | */ + PARAM_TRANSACTION_ACTION_START=0, /* Commit the current parameter transaction. | */ + PARAM_TRANSACTION_ACTION_COMMIT=1, /* Commit the current parameter transaction. | */ + PARAM_TRANSACTION_ACTION_CANCEL=2, /* Cancel the current parameter transaction. | */ + PARAM_TRANSACTION_ACTION_ENUM_END=3, /* | */ } PARAM_TRANSACTION_ACTION; #endif @@ -788,7 +550,7 @@ typedef enum MAV_CMD MAV_CMD_NAV_LOITER_TURNS=18, /* Loiter around this waypoint for X turns |Number of turns.| Leave loiter circle only once heading towards the next waypoint (0 = False)| Loiter radius around waypoint for forward-only moving vehicles (not multicopters). If positive loiter clockwise, else counter-clockwise| Loiter circle exit location and/or path to next waypoint ("xtrack") for forward-only moving vehicles (not multicopters). 0 for the vehicle to converge towards the center xtrack when it leaves the loiter (the line between the centers of the current and next waypoint), 1 to converge to the direct line between the location that the vehicle exits the loiter radius and the next waypoint. Otherwise the angle (in degrees) between the tangent of the loiter circle and the center xtrack at which the vehicle must leave the loiter (and converge to the center xtrack). NaN to use the current system default xtrack behaviour.| Latitude| Longitude| Altitude| */ MAV_CMD_NAV_LOITER_TIME=19, /* Loiter at the specified latitude, longitude and altitude for a certain amount of time. Multicopter vehicles stop at the point (within a vehicle-specific acceptance radius). Forward-only moving vehicles (e.g. fixed-wing) circle the point with the specified radius/direction. If the Heading Required parameter (2) is non-zero forward moving aircraft will only leave the loiter circle once heading towards the next waypoint. |Loiter time (only starts once Lat, Lon and Alt is reached).| Leave loiter circle only once heading towards the next waypoint (0 = False)| Loiter radius around waypoint for forward-only moving vehicles (not multicopters). If positive loiter clockwise, else counter-clockwise.| Loiter circle exit location and/or path to next waypoint ("xtrack") for forward-only moving vehicles (not multicopters). 0 for the vehicle to converge towards the center xtrack when it leaves the loiter (the line between the centers of the current and next waypoint), 1 to converge to the direct line between the location that the vehicle exits the loiter radius and the next waypoint. Otherwise the angle (in degrees) between the tangent of the loiter circle and the center xtrack at which the vehicle must leave the loiter (and converge to the center xtrack). NaN to use the current system default xtrack behaviour.| Latitude| Longitude| Altitude| */ MAV_CMD_NAV_RETURN_TO_LAUNCH=20, /* Return to launch location |Empty| Empty| Empty| Empty| Empty| Empty| Empty| */ - MAV_CMD_NAV_LAND=21, /* Land at location. |Minimum target altitude if landing is aborted (0 = undefined/use system default).| Precision land mode.| Empty| Desired yaw angle. NaN to use the current system yaw heading mode (e.g. yaw towards next waypoint, yaw to home, etc.).| Latitude.| Longitude.| Landing altitude (ground level in current frame).| */ + MAV_CMD_NAV_LAND=21, /* Land at location. |Minimum target altitude if landing is aborted (0 = undefined/use system default).| Precision land mode.| Empty.| Desired yaw angle. NaN to use the current system yaw heading mode (e.g. yaw towards next waypoint, yaw to home, etc.).| Latitude.| Longitude.| Landing altitude (ground level in current frame).| */ MAV_CMD_NAV_TAKEOFF=22, /* Takeoff from ground / hand. Vehicles that support multiple takeoff modes (e.g. VTOL quadplane) should take off using the currently configured mode. |Minimum pitch (if airspeed sensor present), desired pitch without sensor| Empty| Empty| Yaw angle (if magnetometer present), ignored without magnetometer. NaN to use the current system yaw heading mode (e.g. yaw towards next waypoint, yaw to home, etc.).| Latitude| Longitude| Altitude| */ MAV_CMD_NAV_LAND_LOCAL=23, /* Land at local position (local frame only) |Landing target number (if available)| Maximum accepted offset from desired landing position - computed magnitude from spherical coordinates: d = sqrt(x^2 + y^2 + z^2), which gives the maximum accepted distance between the desired landing position and the position where the vehicle is about to land| Landing descend rate| Desired yaw angle| Y-axis position| X-axis position| Z-axis / ground level position| */ MAV_CMD_NAV_TAKEOFF_LOCAL=24, /* Takeoff from local position (local frame only) |Minimum pitch (if airspeed sensor present), desired pitch without sensor| Empty| Takeoff ascend rate| Yaw angle (if magnetometer or another yaw estimation source present), ignored without one of these| Y-axis position| X-axis position| Z-axis position| */ @@ -804,7 +566,7 @@ typedef enum MAV_CMD MAV_CMD_NAV_VTOL_TAKEOFF=84, /* Takeoff from ground using VTOL mode, and transition to forward flight with specified heading. The command should be ignored by vehicles that dont support both VTOL and fixed-wing flight (multicopters, boats,etc.). |Empty| Front transition heading.| Empty| Yaw angle. NaN to use the current system yaw heading mode (e.g. yaw towards next waypoint, yaw to home, etc.).| Latitude| Longitude| Altitude| */ MAV_CMD_NAV_VTOL_LAND=85, /* Land using VTOL mode |Empty| Empty| Approach altitude (with the same reference as the Altitude field). NaN if unspecified.| Yaw angle. NaN to use the current system yaw heading mode (e.g. yaw towards next waypoint, yaw to home, etc.).| Latitude| Longitude| Altitude (ground level)| */ MAV_CMD_NAV_GUIDED_ENABLE=92, /* hand control over to an external controller |On / Off (> 0.5f on)| Empty| Empty| Empty| Empty| Empty| Empty| */ - MAV_CMD_NAV_DELAY=93, /* Delay the next navigation command a number of seconds or until a specified time |Delay (-1 to enable time-of-day fields)| hour (24h format, UTC, -1 to ignore)| minute (24h format, UTC, -1 to ignore)| second (24h format, UTC)| Empty| Empty| Empty| */ + MAV_CMD_NAV_DELAY=93, /* Delay the next navigation command a number of seconds or until a specified time |Delay (-1 to enable time-of-day fields)| hour (24h format, UTC, -1 to ignore)| minute (24h format, UTC, -1 to ignore)| second (24h format, UTC, -1 to ignore)| Empty| Empty| Empty| */ MAV_CMD_NAV_PAYLOAD_PLACE=94, /* Descend and place payload. Vehicle moves to specified location, descends until it detects a hanging payload has reached the ground, and then releases the payload. If ground is not detected before the reaching the maximum descent value (param1), the command will complete without releasing the payload. |Maximum distance to descend.| Empty| Empty| Empty| Latitude| Longitude| Altitude| */ MAV_CMD_NAV_LAST=95, /* NOP - This command is only used to mark the upper limit of the NAV/ACTION commands in the enumeration |Empty| Empty| Empty| Empty| Empty| Empty| Empty| */ MAV_CMD_CONDITION_DELAY=112, /* Delay mission state machine. |Delay| Empty| Empty| Empty| Empty| Empty| Empty| */ @@ -822,7 +584,7 @@ typedef enum MAV_CMD MAV_CMD_DO_SET_SERVO=183, /* Set a servo to a desired PWM value. |Servo instance number.| Pulse Width Modulation.| Empty| Empty| Empty| Empty| Empty| */ MAV_CMD_DO_REPEAT_SERVO=184, /* Cycle a between its nominal setting and a desired PWM for a desired number of cycles with a desired period. |Servo instance number.| Pulse Width Modulation.| Cycle count.| Cycle time.| Empty| Empty| Empty| */ MAV_CMD_DO_FLIGHTTERMINATION=185, /* Terminate flight immediately |Flight termination activated if > 0.5| Empty| Empty| Empty| Empty| Empty| Empty| */ - MAV_CMD_DO_CHANGE_ALTITUDE=186, /* Change altitude set point. |Altitude| Frame of new altitude.| Empty| Empty| Empty| Empty| Empty| */ + MAV_CMD_DO_CHANGE_ALTITUDE=186, /* Change altitude set point. |Altitude.| Frame of new altitude.| Empty| Empty| Empty| Empty| Empty| */ MAV_CMD_DO_SET_ACTUATOR=187, /* Sets actuators (e.g. servos) to a desired value. The actuator numbers are mapped to specific outputs (e.g. on any MAIN or AUX PWM or UAVCAN) using a flight-stack specific mechanism (i.e. a parameter). |Actuator 1 value, scaled from [-1 to 1]. NaN to ignore.| Actuator 2 value, scaled from [-1 to 1]. NaN to ignore.| Actuator 3 value, scaled from [-1 to 1]. NaN to ignore.| Actuator 4 value, scaled from [-1 to 1]. NaN to ignore.| Actuator 5 value, scaled from [-1 to 1]. NaN to ignore.| Actuator 6 value, scaled from [-1 to 1]. NaN to ignore.| Index of actuator set (i.e if set to 1, Actuator 1 becomes Actuator 7)| */ MAV_CMD_DO_LAND_START=189, /* Mission command to perform a landing. This is used as a marker in a mission to tell the autopilot where a sequence of mission items that represents a landing starts. It may also be sent via a COMMAND_LONG to trigger a landing, in which case the nearest (geographically) landing sequence in the mission will be used. The Latitude/Longitude is optional, and may be set to 0 if not needed. If specified then it will be used to help find the closest landing sequence. |Empty| Empty| Empty| Empty| Latitude| Longitude| Empty| */ MAV_CMD_DO_RALLY_LAND=190, /* Mission command to perform a landing from a rally point. |Break altitude| Landing speed| Empty| Empty| Empty| Empty| Empty| */ @@ -830,10 +592,10 @@ typedef enum MAV_CMD MAV_CMD_DO_REPOSITION=192, /* Reposition the vehicle to a specific WGS84 global position. |Ground speed, less than 0 (-1) for default| Bitmask of option flags.| Reserved| Yaw heading. NaN to use the current system yaw heading mode (e.g. yaw towards next waypoint, yaw to home, etc.). For planes indicates loiter direction (0: clockwise, 1: counter clockwise)| Latitude| Longitude| Altitude| */ MAV_CMD_DO_PAUSE_CONTINUE=193, /* If in a GPS controlled position mode, hold the current position or continue. |0: Pause current mission or reposition command, hold current position. 1: Continue mission. A VTOL capable vehicle should enter hover mode (multicopter and VTOL planes). A plane should loiter with the default loiter radius.| Reserved| Reserved| Reserved| Reserved| Reserved| Reserved| */ MAV_CMD_DO_SET_REVERSE=194, /* Set moving direction to forward or reverse. |Direction (0=Forward, 1=Reverse)| Empty| Empty| Empty| Empty| Empty| Empty| */ - MAV_CMD_DO_SET_ROI_LOCATION=195, /* Sets the region of interest (ROI) to a location. This can then be used by the vehicle's control system to control the vehicle attitude and the attitude of various sensors such as cameras. This command can be sent to a gimbal manager but not to a gimbal device. A gimbal is not to react to this message. |Component ID of gimbal device to address (or 1-6 for non-MAVLink gimbal), 0 for all gimbal device components. (Send command multiple times for more than one but not all gimbals.)| Empty| Empty| Empty| Latitude of ROI location| Longitude of ROI location| Altitude of ROI location| */ - MAV_CMD_DO_SET_ROI_WPNEXT_OFFSET=196, /* Sets the region of interest (ROI) to be toward next waypoint, with optional pitch/roll/yaw offset. This can then be used by the vehicle's control system to control the vehicle attitude and the attitude of various sensors such as cameras. This command can be sent to a gimbal manager but not to a gimbal device. A gimbal device is not to react to this message. |Component ID of gimbal device to address (or 1-6 for non-MAVLink gimbal), 0 for all gimbal device components. (Send command multiple times for more than one but not all gimbals.)| Empty| Empty| Empty| Pitch offset from next waypoint, positive tilting up| roll offset from next waypoint, positive banking to the right| yaw offset from next waypoint, positive panning to the right| */ - MAV_CMD_DO_SET_ROI_NONE=197, /* Cancels any previous ROI command returning the vehicle/sensors to default flight characteristics. This can then be used by the vehicle's control system to control the vehicle attitude and the attitude of various sensors such as cameras. This command can be sent to a gimbal manager but not to a gimbal device. A gimbal device is not to react to this message. After this command the gimbal manager should go back to manual input if available, and otherwise assume a neutral position. |Component ID of gimbal device to address (or 1-6 for non-MAVLink gimbal), 0 for all gimbal device components. (Send command multiple times for more than one but not all gimbals.)| Empty| Empty| Empty| Empty| Empty| Empty| */ - MAV_CMD_DO_SET_ROI_SYSID=198, /* Mount tracks system with specified system ID. Determination of target vehicle position may be done with GLOBAL_POSITION_INT or any other means. This command can be sent to a gimbal manager but not to a gimbal device. A gimbal device is not to react to this message. |System ID| Component ID of gimbal device to address (or 1-6 for non-MAVLink gimbal), 0 for all gimbal device components. (Send command multiple times for more than one but not all gimbals.)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| */ + MAV_CMD_DO_SET_ROI_LOCATION=195, /* Sets the region of interest (ROI) to a location. This can then be used by the vehicle's control system to control the vehicle attitude and the attitude of various sensors such as cameras. This command can be sent to a gimbal manager but not to a gimbal device. A gimbal is not to react to this message. |Component ID of gimbal device to address (or 1-6 for non-MAVLink gimbal), 0 for all gimbal device components. Send command multiple times for more than one gimbal (but not all gimbals).| Empty| Empty| Empty| Latitude of ROI location| Longitude of ROI location| Altitude of ROI location| */ + MAV_CMD_DO_SET_ROI_WPNEXT_OFFSET=196, /* Sets the region of interest (ROI) to be toward next waypoint, with optional pitch/roll/yaw offset. This can then be used by the vehicle's control system to control the vehicle attitude and the attitude of various sensors such as cameras. This command can be sent to a gimbal manager but not to a gimbal device. A gimbal device is not to react to this message. |Component ID of gimbal device to address (or 1-6 for non-MAVLink gimbal), 0 for all gimbal device components. Send command multiple times for more than one gimbal (but not all gimbals).| Empty| Empty| Empty| Pitch offset from next waypoint, positive pitching up| Roll offset from next waypoint, positive rolling to the right| Yaw offset from next waypoint, positive yawing to the right| */ + MAV_CMD_DO_SET_ROI_NONE=197, /* Cancels any previous ROI command returning the vehicle/sensors to default flight characteristics. This can then be used by the vehicle's control system to control the vehicle attitude and the attitude of various sensors such as cameras. This command can be sent to a gimbal manager but not to a gimbal device. A gimbal device is not to react to this message. After this command the gimbal manager should go back to manual input if available, and otherwise assume a neutral position. |Component ID of gimbal device to address (or 1-6 for non-MAVLink gimbal), 0 for all gimbal device components. Send command multiple times for more than one gimbal (but not all gimbals).| Empty| Empty| Empty| Empty| Empty| Empty| */ + MAV_CMD_DO_SET_ROI_SYSID=198, /* Mount tracks system with specified system ID. Determination of target vehicle position may be done with GLOBAL_POSITION_INT or any other means. This command can be sent to a gimbal manager but not to a gimbal device. A gimbal device is not to react to this message. |System ID| Component ID of gimbal device to address (or 1-6 for non-MAVLink gimbal), 0 for all gimbal device components. Send command multiple times for more than one gimbal (but not all gimbals).| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| */ MAV_CMD_DO_CONTROL_VIDEO=200, /* Control onboard camera system. |Camera ID (-1 for all)| Transmission: 0: disabled, 1: enabled compressed, 2: enabled raw| Transmission mode: 0: video stream, >0: single images every n seconds| Recording: 0: disabled, 1: enabled compressed, 2: enabled raw| Empty| Empty| Empty| */ MAV_CMD_DO_SET_ROI=201, /* Sets the region of interest (ROI) for a sensor set or the vehicle itself. This can then be used by the vehicle's control system to control the vehicle attitude and the attitude of various sensors such as cameras. |Region of interest mode.| Waypoint index/ target ID (depends on param 1).| Region of interest index. (allows a vehicle to manage multiple ROI's)| Empty| MAV_ROI_WPNEXT: pitch offset from next waypoint, MAV_ROI_LOCATION: latitude| MAV_ROI_WPNEXT: roll offset from next waypoint, MAV_ROI_LOCATION: longitude| MAV_ROI_WPNEXT: yaw offset from next waypoint, MAV_ROI_LOCATION: altitude| */ MAV_CMD_DO_DIGICAM_CONFIGURE=202, /* Configure digital camera. This is a fallback message for systems that have not yet implemented PARAM_EXT_XXX messages and camera definition files (see https://mavlink.io/en/services/camera_def.html ). |Modes: P, TV, AV, M, Etc.| Shutter speed: Divisor number for one second.| Aperture: F stop number.| ISO number e.g. 80, 100, 200, Etc.| Exposure type enumerator.| Command Identity.| Main engine cut-off time before camera trigger. (0 means no cut-off)| */ @@ -845,6 +607,8 @@ typedef enum MAV_CMD MAV_CMD_DO_PARACHUTE=208, /* Mission item/command to release a parachute or enable/disable auto release. |Action| Empty| Empty| Empty| Empty| Empty| Empty| */ MAV_CMD_DO_MOTOR_TEST=209, /* Mission command to perform motor test. |Motor instance number. (from 1 to max number of motors on the vehicle)| Throttle type.| Throttle.| Timeout.| Motor count. (number of motors to test to test in sequence, waiting for the timeout above between them; 0=1 motor, 1=1 motor, 2=2 motors...)| Motor test order.| Empty| */ MAV_CMD_DO_INVERTED_FLIGHT=210, /* Change to/from inverted flight. |Inverted flight. (0=normal, 1=inverted)| Empty| Empty| Empty| Empty| Empty| Empty| */ + MAV_CMD_DO_GRIPPER=211, /* Mission command to operate a gripper. |Gripper instance number.| Gripper action to perform.| Empty| Empty| Empty| Empty| Empty| */ + MAV_CMD_DO_AUTOTUNE_ENABLE=212, /* Enable/disable autotune. |Enable (1: enable, 0:disable).| Empty.| Empty.| Empty.| Empty.| Empty.| Empty.| */ MAV_CMD_NAV_SET_YAW_SPEED=213, /* Sets a desired vehicle turn angle and speed change. |Yaw angle to adjust steering by.| Speed.| Final angle. (0=absolute, 1=relative)| Empty| Empty| Empty| Empty| */ MAV_CMD_DO_SET_CAM_TRIGG_INTERVAL=214, /* Mission command to set camera trigger interval for this flight. If triggering is enabled, the camera is triggered each time this interval expires. This command can also be used to set the shutter integration time for the camera. |Camera trigger cycle time. -1 or 0 to ignore.| Camera shutter integration time. Should be less than trigger cycle time. -1 or 0 to ignore.| Empty| Empty| Empty| Empty| Empty| */ MAV_CMD_DO_MOUNT_CONTROL_QUAT=220, /* Mission command to control a camera or antenna mount, using a quaternion as reference. |quaternion param q1, w (1 in null-rotation)| quaternion param q2, x (0 in null-rotation)| quaternion param q3, y (0 in null-rotation)| quaternion param q4, z (0 in null-rotation)| Empty| Empty| Empty| */ @@ -855,11 +619,12 @@ typedef enum MAV_CMD MAV_CMD_DO_LAST=240, /* NOP - This command is only used to mark the upper limit of the DO commands in the enumeration |Empty| Empty| Empty| Empty| Empty| Empty| Empty| */ MAV_CMD_PREFLIGHT_CALIBRATION=241, /* Trigger calibration. This command will be only accepted if in pre-flight mode. Except for Temperature Calibration, only one sensor should be set in a single message and all others should be zero. |1: gyro calibration, 3: gyro temperature calibration| 1: magnetometer calibration| 1: ground pressure calibration| 1: radio RC calibration, 2: RC trim calibration| 1: accelerometer calibration, 2: board level calibration, 3: accelerometer temperature calibration, 4: simple accelerometer calibration| 1: APM: compass/motor interference calibration (PX4: airspeed calibration, deprecated), 2: airspeed calibration| 1: ESC calibration, 3: barometer temperature calibration| */ MAV_CMD_PREFLIGHT_SET_SENSOR_OFFSETS=242, /* Set sensor offsets. This command will be only accepted if in pre-flight mode. |Sensor to adjust the offsets for: 0: gyros, 1: accelerometer, 2: magnetometer, 3: barometer, 4: optical flow, 5: second magnetometer, 6: third magnetometer| X axis offset (or generic dimension 1), in the sensor's raw units| Y axis offset (or generic dimension 2), in the sensor's raw units| Z axis offset (or generic dimension 3), in the sensor's raw units| Generic dimension 4, in the sensor's raw units| Generic dimension 5, in the sensor's raw units| Generic dimension 6, in the sensor's raw units| */ - MAV_CMD_PREFLIGHT_UAVCAN=243, /* Trigger UAVCAN config. This command will be only accepted if in pre-flight mode. |1: Trigger actuator ID assignment and direction mapping.| Reserved| Reserved| Reserved| Reserved| Reserved| Reserved| */ + MAV_CMD_PREFLIGHT_UAVCAN=243, /* Trigger UAVCAN configuration (actuator ID assignment and direction mapping). Note that this maps to the legacy UAVCAN v0 function UAVCAN_ENUMERATE, which is intended to be executed just once during initial vehicle configuration (it is not a normal pre-flight command and has been poorly named). |1: Trigger actuator ID assignment and direction mapping. 0: Cancel command.| Reserved| Reserved| Reserved| Reserved| Reserved| Reserved| */ MAV_CMD_PREFLIGHT_STORAGE=245, /* Request storage of different parameter values and logs. This command will be only accepted if in pre-flight mode. |Parameter storage: 0: READ FROM FLASH/EEPROM, 1: WRITE CURRENT TO FLASH/EEPROM, 2: Reset to defaults| Mission storage: 0: READ FROM FLASH/EEPROM, 1: WRITE CURRENT TO FLASH/EEPROM, 2: Reset to defaults| Onboard logging: 0: Ignore, 1: Start default rate logging, -1: Stop logging, > 1: logging rate (e.g. set to 1000 for 1000 Hz logging)| Reserved| Empty| Empty| Empty| */ MAV_CMD_PREFLIGHT_REBOOT_SHUTDOWN=246, /* Request the reboot or shutdown of system components. |0: Do nothing for autopilot, 1: Reboot autopilot, 2: Shutdown autopilot, 3: Reboot autopilot and keep it in the bootloader until upgraded.| 0: Do nothing for onboard computer, 1: Reboot onboard computer, 2: Shutdown onboard computer, 3: Reboot onboard computer and keep it in the bootloader until upgraded.| WIP: 0: Do nothing for camera, 1: Reboot onboard camera, 2: Shutdown onboard camera, 3: Reboot onboard camera and keep it in the bootloader until upgraded| WIP: 0: Do nothing for mount (e.g. gimbal), 1: Reboot mount, 2: Shutdown mount, 3: Reboot mount and keep it in the bootloader until upgraded| Reserved (set to 0)| Reserved (set to 0)| WIP: ID (e.g. camera ID -1 for all IDs)| */ MAV_CMD_DO_UPGRADE=247, /* Request a target system to start an upgrade of one (or all) of its components. For example, the command might be sent to a companion computer to cause it to upgrade a connected flight controller. The system doing the upgrade will report progress using the normal command protocol sequence for a long running operation. Command protocol information: https://mavlink.io/en/services/command.html. |Component id of the component to be upgraded. If set to 0, all components should be upgraded.| 0: Do not reboot component after the action is executed, 1: Reboot component after the action is executed.| Reserved| Reserved| Reserved| Reserved| WIP: upgrade progress report rate (can be used for more granular control).| */ MAV_CMD_OVERRIDE_GOTO=252, /* Override current mission with command to pause mission, pause mission and move to position, continue/resume mission. When param 1 indicates that the mission is paused (MAV_GOTO_DO_HOLD), param 2 defines whether it holds in place or moves to another position. |MAV_GOTO_DO_HOLD: pause mission and either hold or move to specified position (depending on param2), MAV_GOTO_DO_CONTINUE: resume mission.| MAV_GOTO_HOLD_AT_CURRENT_POSITION: hold at current position, MAV_GOTO_HOLD_AT_SPECIFIED_POSITION: hold at specified position.| Coordinate frame of hold point.| Desired yaw angle.| Latitude/X position.| Longitude/Y position.| Altitude/Z position.| */ + MAV_CMD_OBLIQUE_SURVEY=260, /* Mission command to set a Camera Auto Mount Pivoting Oblique Survey (Replaces CAM_TRIGG_DIST for this purpose). The camera is triggered each time this distance is exceeded, then the mount moves to the next position. Params 4~6 set-up the angle limits and number of positions for oblique survey, where mount-enabled vehicles automatically roll the camera between shots to emulate an oblique camera setup (providing an increased HFOV). This command can also be used to set the shutter integration time for the camera. |Camera trigger distance. 0 to stop triggering.| Camera shutter integration time. 0 to ignore| The minimum interval in which the camera is capable of taking subsequent pictures repeatedly. 0 to ignore.| Total number of roll positions at which the camera will capture photos (images captures spread evenly across the limits defined by param5).| Angle limits that the camera can be rolled to left and right of center.| Fixed pitch angle that the camera will hold in oblique mode if the mount is actuated in the pitch axis.| Empty| */ MAV_CMD_MISSION_START=300, /* start running a mission |first_item: the first mission item to run| last_item: the last mission item to run (after this item is run, the mission ends)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| */ MAV_CMD_COMPONENT_ARM_DISARM=400, /* Arms / Disarms a component |0: disarm, 1: arm| 0: arm-disarm unless prevented by safety checks (i.e. when landed), 21196: force arming/disarming (e.g. allow arming to override preflight checks and disarming in flight)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| */ MAV_CMD_ILLUMINATOR_ON_OFF=405, /* Turns illuminators ON/OFF. An illuminator is a light source that is used for lighting up dark areas external to the sytstem: e.g. a torch or searchlight (as opposed to a light source for illuminating the system itself, e.g. an indicator light). |0: Illuminators OFF, 1: Illuminators ON| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| */ @@ -883,8 +648,10 @@ typedef enum MAV_CMD MAV_CMD_SET_CAMERA_FOCUS=532, /* Set camera focus. Camera must respond with a CAMERA_SETTINGS message (on success). |Focus type| Focus value| Reserved (default:NaN)| Reserved (default:NaN)| Reserved (default:0)| Reserved (default:0)| Reserved (default:NaN)| */ MAV_CMD_JUMP_TAG=600, /* Tagged jump target. Can be jumped to with MAV_CMD_DO_JUMP_TAG. |Tag.| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| */ MAV_CMD_DO_JUMP_TAG=601, /* Jump to the matching tag in the mission list. Repeat this action for the specified number of times. A mission should contain a single matching tag for each jump. If this is not the case then a jump to a missing tag should complete the mission, and a jump where there are multiple matching tags should always select the one with the lowest mission sequence number. |Target tag to jump to.| Repeat count.| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| */ - MAV_CMD_DO_GIMBAL_MANAGER_TILTPAN=1000, /* High level setpoint to be sent to a gimbal manager to set a gimbal attitude. It is possible to set combinations of the values below. E.g. an angle as well as a desired angular rate can be used to get to this angle at a certain angular rate, or an angular rate only will result in continuous turning. NaN is to be used to signal unset. Note: a gimbal is never to react to this command but only the gimbal manager. |Tilt/pitch rate (positive to tilt up).| Pan/yaw rate (positive to pan to the right).| Tilt/pitch angle (positive to tilt up, relative to vehicle for PAN mode, relative to world horizon for HOLD mode).| Pan/yaw angle (positive to pan to the right, relative to vehicle for PAN mode, absolute to North for HOLD mode).| Gimbal manager flags to use.| Reserved (default:0)| Component ID of gimbal device to address (or 1-6 for non-MAVLink gimbal), 0 for all gimbal device components. (Send command multiple times for more than one but not all gimbals.)| */ - MAV_CMD_IMAGE_START_CAPTURE=2000, /* Start image capture sequence. Sends CAMERA_IMAGE_CAPTURED after each capture. Use NaN for reserved values. |Reserved (Set to 0)| Desired elapsed time between two consecutive pictures (in seconds). Minimum values depend on hardware (typically greater than 2 seconds).| Total number of images to capture. 0 to capture forever/until MAV_CMD_IMAGE_STOP_CAPTURE.| Capture sequence number starting from 1. This is only valid for single-capture (param3 == 1). Increment the capture ID for each capture command to prevent double captures when a command is re-transmitted. Use 0 to ignore it.| Reserved (all remaining params)| Reserved (default:0)| Reserved (default:0)| */ + MAV_CMD_PARAM_TRANSACTION=900, /* Request to start or end a parameter transaction. Multiple kinds of transport layers can be used to exchange parameters in the transaction (param, param_ext and mavftp). The command response can either be a success/failure or an in progress in case the receiving side takes some time to apply the parameters. |Action to be performed (start, commit, cancel, etc.)| Possible transport layers to set and get parameters via mavlink during a parameter transaction.| Identifier for a specific transaction.| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| */ + MAV_CMD_DO_GIMBAL_MANAGER_PITCHYAW=1000, /* High level setpoint to be sent to a gimbal manager to set a gimbal attitude. It is possible to set combinations of the values below. E.g. an angle as well as a desired angular rate can be used to get to this angle at a certain angular rate, or an angular rate only will result in continuous turning. NaN is to be used to signal unset. Note: a gimbal is never to react to this command but only the gimbal manager. |Pitch angle (positive to pitch up, relative to vehicle for FOLLOW mode, relative to world horizon for LOCK mode).| Yaw angle (positive to yaw to the right, relative to vehicle for FOLLOW mode, absolute to North for LOCK mode).| Pitch rate (positive to pitch up).| Yaw rate (positive to yaw to the right).| Gimbal manager flags to use.| Reserved (default:0)| Component ID of gimbal device to address (or 1-6 for non-MAVLink gimbal), 0 for all gimbal device components. Send command multiple times for more than one gimbal (but not all gimbals).| */ + MAV_CMD_DO_GIMBAL_MANAGER_CONFIGURE=1001, /* Gimbal configuration to set which sysid/compid is in primary and secondary control. |Sysid for primary control (0: no one in control, -1: leave unchanged, -2: set itself in control (for missions where the own sysid is still unknown), -3: remove control if currently in control).| Compid for primary control (0: no one in control, -1: leave unchanged, -2: set itself in control (for missions where the own sysid is still unknown), -3: remove control if currently in control).| Sysid for secondary control (0: no one in control, -1: leave unchanged, -2: set itself in control (for missions where the own sysid is still unknown), -3: remove control if currently in control).| Compid for secondary control (0: no one in control, -1: leave unchanged, -2: set itself in control (for missions where the own sysid is still unknown), -3: remove control if currently in control).| Reserved (default:0)| Reserved (default:0)| Component ID of gimbal device to address (or 1-6 for non-MAVLink gimbal), 0 for all gimbal device components. Send command multiple times for more than one gimbal (but not all gimbals).| */ + MAV_CMD_IMAGE_START_CAPTURE=2000, /* Start image capture sequence. Sends CAMERA_IMAGE_CAPTURED after each capture. Use NaN for reserved values. |Reserved (Set to 0)| Desired elapsed time between two consecutive pictures (in seconds). Minimum values depend on hardware (typically greater than 2 seconds).| Total number of images to capture. 0 to capture forever/until MAV_CMD_IMAGE_STOP_CAPTURE.| Capture sequence number starting from 1. This is only valid for single-capture (param3 == 1), otherwise set to 0. Increment the capture ID for each capture command to prevent double captures when a command is re-transmitted.| Reserved (default:NaN)| Reserved (default:NaN)| Reserved (default:NaN)| */ MAV_CMD_IMAGE_STOP_CAPTURE=2001, /* Stop image capture sequence Use NaN for reserved values. |Reserved (Set to 0)| Reserved (default:NaN)| Reserved (default:NaN)| Reserved (default:NaN)| Reserved (default:0)| Reserved (default:0)| Reserved (default:NaN)| */ MAV_CMD_REQUEST_CAMERA_IMAGE_CAPTURE=2002, /* Re-request a CAMERA_IMAGE_CAPTURED message. |Sequence number for missing CAMERA_IMAGE_CAPTURED message| Reserved (default:NaN)| Reserved (default:NaN)| Reserved (default:NaN)| Reserved (default:0)| Reserved (default:0)| Reserved (default:NaN)| */ MAV_CMD_DO_TRIGGER_CONTROL=2003, /* Enable or disable on-board camera triggering system. |Trigger enable/disable (0 for disable, 1 for start), -1 to ignore| 1 to reset the trigger sequence, -1 or 0 to ignore| 1 to pause triggering, but without switching the camera off or retracting it. -1 to ignore| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| */ @@ -910,14 +677,13 @@ typedef enum MAV_CMD MAV_CMD_SET_GUIDED_SUBMODE_CIRCLE=4001, /* This command sets submode circle when vehicle is in guided mode. Vehicle flies along a circle facing the center of the circle. The user can input the velocity along the circle and change the radius. If no input is given the vehicle will hold position. |Radius of desired circle in CIRCLE_MODE| User defined| User defined| User defined| Target latitude of center of circle in CIRCLE_MODE| Target longitude of center of circle in CIRCLE_MODE| Reserved (default:0)| */ MAV_CMD_CONDITION_GATE=4501, /* Delay mission state machine until gate has been reached. |Geometry: 0: orthogonal to path between previous and next waypoint.| Altitude: 0: ignore altitude| Empty| Empty| Latitude| Longitude| Altitude| */ - MAV_CMD_NAV_FENCE_RETURN_POINT=5000, /* Fence return point. There can only be one fence return point. - |Reserved| Reserved| Reserved| Reserved| Latitude| Longitude| Altitude| */ + MAV_CMD_NAV_FENCE_RETURN_POINT=5000, /* Fence return point (there can only be one such point in a geofence definition). If rally points are supported they should be used instead. |Reserved| Reserved| Reserved| Reserved| Latitude| Longitude| Altitude| */ MAV_CMD_NAV_FENCE_POLYGON_VERTEX_INCLUSION=5001, /* Fence vertex for an inclusion polygon (the polygon must not be self-intersecting). The vehicle must stay within this area. Minimum of 3 vertices required. - |Polygon vertex count| Reserved| Reserved| Reserved| Latitude| Longitude| Reserved| */ + |Polygon vertex count| Vehicle must be inside ALL inclusion zones in a single group, vehicle must be inside at least one group, must be the same for all points in each polygon| Reserved| Reserved| Latitude| Longitude| Reserved| */ MAV_CMD_NAV_FENCE_POLYGON_VERTEX_EXCLUSION=5002, /* Fence vertex for an exclusion polygon (the polygon must not be self-intersecting). The vehicle must stay outside this area. Minimum of 3 vertices required. |Polygon vertex count| Reserved| Reserved| Reserved| Latitude| Longitude| Reserved| */ MAV_CMD_NAV_FENCE_CIRCLE_INCLUSION=5003, /* Circular fence area. The vehicle must stay inside this area. - |Radius.| Reserved| Reserved| Reserved| Latitude| Longitude| Reserved| */ + |Radius.| Vehicle must be inside ALL inclusion zones in a single group, vehicle must be inside at least one group| Reserved| Reserved| Latitude| Longitude| Reserved| */ MAV_CMD_NAV_FENCE_CIRCLE_EXCLUSION=5004, /* Circular fence area. The vehicle must stay outside this area. |Radius.| Reserved| Reserved| Reserved| Latitude| Longitude| Reserved| */ MAV_CMD_NAV_RALLY_POINT=5100, /* Rally point. You can have multiple rally points defined. @@ -940,7 +706,9 @@ typedef enum MAV_CMD MAV_CMD_USER_3=31012, /* User defined command. Ground Station will not show the Vehicle as flying through this item. Example: MAV_CMD_DO_SET_PARAMETER item. |User defined| User defined| User defined| User defined| User defined| User defined| User defined| */ MAV_CMD_USER_4=31013, /* User defined command. Ground Station will not show the Vehicle as flying through this item. Example: MAV_CMD_DO_SET_PARAMETER item. |User defined| User defined| User defined| User defined| User defined| User defined| User defined| */ MAV_CMD_USER_5=31014, /* User defined command. Ground Station will not show the Vehicle as flying through this item. Example: MAV_CMD_DO_SET_PARAMETER item. |User defined| User defined| User defined| User defined| User defined| User defined| User defined| */ - MAV_CMD_ENUM_END=31015, /* | */ + MAV_CMD_FIXED_MAG_CAL_YAW=42006, /* Magnetometer calibration based on provided known yaw. This allows for fast calibration using WMM field tables in the vehicle, given only the known yaw of the vehicle. If Latitude and longitude are both zero then use the current vehicle location. |Yaw of vehicle in earth frame.| CompassMask, 0 for all.| Latitude.| Longitude.| Empty.| Empty.| Empty.| */ + MAV_CMD_DO_WINCH=42600, /* Command to operate winch. |Winch instance number.| Action to perform.| Length of cable to release (negative to wind).| Release rate (negative to wind).| Empty.| Empty.| Empty.| */ + MAV_CMD_ENUM_END=42601, /* | */ } MAV_CMD; #endif @@ -985,16 +753,16 @@ typedef enum MAV_ROI #define HAVE_ENUM_MAV_CMD_ACK typedef enum MAV_CMD_ACK { - MAV_CMD_ACK_OK=1, /* Command / mission item is ok. |Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| */ - MAV_CMD_ACK_ERR_FAIL=2, /* Generic error message if none of the other reasons fails or if no detailed error reporting is implemented. |Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| */ - MAV_CMD_ACK_ERR_ACCESS_DENIED=3, /* The system is refusing to accept this command from this source / communication partner. |Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| */ - MAV_CMD_ACK_ERR_NOT_SUPPORTED=4, /* Command or mission item is not supported, other commands would be accepted. |Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| */ - MAV_CMD_ACK_ERR_COORDINATE_FRAME_NOT_SUPPORTED=5, /* The coordinate frame of this command / mission item is not supported. |Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| */ - MAV_CMD_ACK_ERR_COORDINATES_OUT_OF_RANGE=6, /* The coordinate frame of this command is ok, but he coordinate values exceed the safety limits of this system. This is a generic error, please use the more specific error messages below if possible. |Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| */ - MAV_CMD_ACK_ERR_X_LAT_OUT_OF_RANGE=7, /* The X or latitude value is out of range. |Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| */ - MAV_CMD_ACK_ERR_Y_LON_OUT_OF_RANGE=8, /* The Y or longitude value is out of range. |Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| */ - MAV_CMD_ACK_ERR_Z_ALT_OUT_OF_RANGE=9, /* The Z or altitude value is out of range. |Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| */ - MAV_CMD_ACK_ENUM_END=10, /* | */ + MAV_CMD_ACK_OK=0, /* Command / mission item is ok. |Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| */ + MAV_CMD_ACK_ERR_FAIL=1, /* Generic error message if none of the other reasons fails or if no detailed error reporting is implemented. |Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| */ + MAV_CMD_ACK_ERR_ACCESS_DENIED=2, /* The system is refusing to accept this command from this source / communication partner. |Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| */ + MAV_CMD_ACK_ERR_NOT_SUPPORTED=3, /* Command or mission item is not supported, other commands would be accepted. |Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| */ + MAV_CMD_ACK_ERR_COORDINATE_FRAME_NOT_SUPPORTED=4, /* The coordinate frame of this command / mission item is not supported. |Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| */ + MAV_CMD_ACK_ERR_COORDINATES_OUT_OF_RANGE=5, /* The coordinate frame of this command is ok, but he coordinate values exceed the safety limits of this system. This is a generic error, please use the more specific error messages below if possible. |Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| */ + MAV_CMD_ACK_ERR_X_LAT_OUT_OF_RANGE=6, /* The X or latitude value is out of range. |Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| */ + MAV_CMD_ACK_ERR_Y_LON_OUT_OF_RANGE=7, /* The Y or longitude value is out of range. |Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| */ + MAV_CMD_ACK_ERR_Z_ALT_OUT_OF_RANGE=8, /* The Z or altitude value is out of range. |Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| Reserved (default:0)| */ + MAV_CMD_ACK_ENUM_END=9, /* | */ } MAV_CMD_ACK; #endif @@ -1208,7 +976,6 @@ typedef enum MAV_SENSOR_ORIENTATION MAV_SENSOR_ROTATION_ROLL_90_PITCH_68_YAW_293=38, /* Roll: 90, Pitch: 68, Yaw: 293 | */ MAV_SENSOR_ROTATION_PITCH_315=39, /* Pitch: 315 | */ MAV_SENSOR_ROTATION_ROLL_90_PITCH_315=40, /* Roll: 90, Pitch: 315 | */ - MAV_SENSOR_ROTATION_ROLL_270_YAW_180=41, /* Roll: 270, Yaw: 180 | */ MAV_SENSOR_ROTATION_CUSTOM=100, /* Custom orientation | */ MAV_SENSOR_ORIENTATION_ENUM_END=101, /* | */ } MAV_SENSOR_ORIENTATION; @@ -1309,26 +1076,39 @@ typedef enum MAV_BATTERY_CHARGE_STATE MAV_BATTERY_CHARGE_STATE_LOW=2, /* Battery state is low, warn and monitor close. | */ MAV_BATTERY_CHARGE_STATE_CRITICAL=3, /* Battery state is critical, return or abort immediately. | */ MAV_BATTERY_CHARGE_STATE_EMERGENCY=4, /* Battery state is too low for ordinary abort sequence. Perform fastest possible emergency stop to prevent damage. | */ - MAV_BATTERY_CHARGE_STATE_FAILED=5, /* Battery failed, damage unavoidable. | */ - MAV_BATTERY_CHARGE_STATE_UNHEALTHY=6, /* Battery is diagnosed to be defective or an error occurred, usage is discouraged / prohibited. | */ + MAV_BATTERY_CHARGE_STATE_FAILED=5, /* Battery failed, damage unavoidable. Possible causes (faults) are listed in MAV_BATTERY_FAULT. | */ + MAV_BATTERY_CHARGE_STATE_UNHEALTHY=6, /* Battery is diagnosed to be defective or an error occurred, usage is discouraged / prohibited. Possible causes (faults) are listed in MAV_BATTERY_FAULT. | */ MAV_BATTERY_CHARGE_STATE_CHARGING=7, /* Battery is charging. | */ MAV_BATTERY_CHARGE_STATE_ENUM_END=8, /* | */ } MAV_BATTERY_CHARGE_STATE; #endif -/** @brief Smart battery supply status/fault flags (bitmask) for health indication. */ -#ifndef HAVE_ENUM_MAV_SMART_BATTERY_FAULT -#define HAVE_ENUM_MAV_SMART_BATTERY_FAULT -typedef enum MAV_SMART_BATTERY_FAULT +/** @brief Battery mode. Note, the normal operation mode (i.e. when flying) should be reported as MAV_BATTERY_MODE_UNKNOWN to allow message trimming in normal flight. */ +#ifndef HAVE_ENUM_MAV_BATTERY_MODE +#define HAVE_ENUM_MAV_BATTERY_MODE +typedef enum MAV_BATTERY_MODE { - MAV_SMART_BATTERY_FAULT_DEEP_DISCHARGE=1, /* Battery has deep discharged. | */ - MAV_SMART_BATTERY_FAULT_SPIKES=2, /* Voltage spikes. | */ - MAV_SMART_BATTERY_FAULT_SINGLE_CELL_FAIL=4, /* Single cell has failed. | */ - MAV_SMART_BATTERY_FAULT_OVER_CURRENT=8, /* Over-current fault. | */ - MAV_SMART_BATTERY_FAULT_OVER_TEMPERATURE=16, /* Over-temperature fault. | */ - MAV_SMART_BATTERY_FAULT_UNDER_TEMPERATURE=32, /* Under-temperature fault. | */ - MAV_SMART_BATTERY_FAULT_ENUM_END=33, /* | */ -} MAV_SMART_BATTERY_FAULT; + MAV_BATTERY_MODE_UNKNOWN=0, /* Battery mode not supported/unknown battery mode/normal operation. | */ + MAV_BATTERY_MODE_AUTO_DISCHARGING=1, /* Battery is auto discharging (towards storage level). | */ + MAV_BATTERY_MODE_HOT_SWAP=2, /* Battery in hot-swap mode (current limited to prevent spikes that might damage sensitive electrical circuits). | */ + MAV_BATTERY_MODE_ENUM_END=3, /* | */ +} MAV_BATTERY_MODE; +#endif + +/** @brief Smart battery supply status/fault flags (bitmask) for health indication. The battery must also report either MAV_BATTERY_CHARGE_STATE_FAILED or MAV_BATTERY_CHARGE_STATE_UNHEALTHY if any of these are set. */ +#ifndef HAVE_ENUM_MAV_BATTERY_FAULT +#define HAVE_ENUM_MAV_BATTERY_FAULT +typedef enum MAV_BATTERY_FAULT +{ + MAV_BATTERY_FAULT_DEEP_DISCHARGE=1, /* Battery has deep discharged. | */ + MAV_BATTERY_FAULT_SPIKES=2, /* Voltage spikes. | */ + MAV_BATTERY_FAULT_CELL_FAIL=4, /* One or more cells have failed. Battery should also report MAV_BATTERY_CHARGE_STATE_FAILE (and should not be used). | */ + MAV_BATTERY_FAULT_OVER_CURRENT=8, /* Over-current fault. | */ + MAV_BATTERY_FAULT_OVER_TEMPERATURE=16, /* Over-temperature fault. | */ + MAV_BATTERY_FAULT_UNDER_TEMPERATURE=32, /* Under-temperature fault. | */ + MAV_BATTERY_FAULT_INCOMPATIBLE_VOLTAGE=64, /* Vehicle voltage is not compatible with this battery (batteries on same power rail should have similar voltage). | */ + MAV_BATTERY_FAULT_ENUM_END=65, /* | */ +} MAV_BATTERY_FAULT; #endif /** @brief Flags to report status/failure cases for a power generator (used in GENERATOR_STATUS). Note that FAULTS are conditions that cause the generator to fail. Warnings are conditions that require attention before the next use (they indicate the system is not operating properly). */ @@ -1680,9 +1460,9 @@ typedef enum CAMERA_TRACKING_STATUS_FLAGS #define HAVE_ENUM_CAMERA_TRACKING_MODE typedef enum CAMERA_TRACKING_MODE { - CAMERA_TRACKING_NONE=0, /* Not tracking | */ - CAMERA_TRACKING_POINT=1, /* Target is a point | */ - CAMERA_TRACKING_RECTANGLE=2, /* Target is a rectangle | */ + CAMERA_TRACKING_MODE_NONE=0, /* Not tracking | */ + CAMERA_TRACKING_MODE_POINT=1, /* Target is a point | */ + CAMERA_TRACKING_MODE_RECTANGLE=2, /* Target is a rectangle | */ CAMERA_TRACKING_MODE_ENUM_END=3, /* | */ } CAMERA_TRACKING_MODE; #endif @@ -1692,10 +1472,10 @@ typedef enum CAMERA_TRACKING_MODE #define HAVE_ENUM_CAMERA_TRACKING_TARGET_DATA typedef enum CAMERA_TRACKING_TARGET_DATA { - CAMERA_TRACKING_TARGET_NONE=0, /* No target data | */ - CAMERA_TRACKING_TARGET_EMBEDDED=1, /* Target data embedded in image data (proprietary) | */ - CAMERA_TRACKING_TARGET_RENDERED=2, /* Target data rendered in image | */ - CAMERA_TRACKING_TARGET_IN_STATUS=4, /* Target data within status message (Point or Rectangle) | */ + CAMERA_TRACKING_TARGET_DATA_NONE=0, /* No target data | */ + CAMERA_TRACKING_TARGET_DATA_EMBEDDED=1, /* Target data embedded in image data (proprietary) | */ + CAMERA_TRACKING_TARGET_DATA_RENDERED=2, /* Target data rendered in image | */ + CAMERA_TRACKING_TARGET_DATA_IN_STATUS=4, /* Target data within status message (Point or Rectangle) | */ CAMERA_TRACKING_TARGET_DATA_ENUM_END=5, /* | */ } CAMERA_TRACKING_TARGET_DATA; #endif @@ -1798,6 +1578,20 @@ typedef enum POSITION_TARGET_TYPEMASK } POSITION_TARGET_TYPEMASK; #endif +/** @brief Bitmap to indicate which dimensions should be ignored by the vehicle: a value of 0b00000000 indicates that none of the setpoint dimensions should be ignored. */ +#ifndef HAVE_ENUM_ATTITUDE_TARGET_TYPEMASK +#define HAVE_ENUM_ATTITUDE_TARGET_TYPEMASK +typedef enum ATTITUDE_TARGET_TYPEMASK +{ + ATTITUDE_TARGET_TYPEMASK_BODY_ROLL_RATE_IGNORE=1, /* Ignore body roll rate | */ + ATTITUDE_TARGET_TYPEMASK_BODY_PITCH_RATE_IGNORE=2, /* Ignore body pitch rate | */ + ATTITUDE_TARGET_TYPEMASK_BODY_YAW_RATE_IGNORE=4, /* Ignore body yaw rate | */ + ATTITUDE_TARGET_TYPEMASK_THROTTLE_IGNORE=64, /* Ignore throttle | */ + ATTITUDE_TARGET_TYPEMASK_ATTITUDE_IGNORE=128, /* Ignore attitude | */ + ATTITUDE_TARGET_TYPEMASK_ENUM_END=129, /* | */ +} ATTITUDE_TARGET_TYPEMASK; +#endif + /** @brief Airborne status of UAS. */ #ifndef HAVE_ENUM_UTM_FLIGHT_STATE #define HAVE_ENUM_UTM_FLIGHT_STATE @@ -2380,6 +2174,23 @@ typedef enum MAV_WINCH_STATUS_FLAG } MAV_WINCH_STATUS_FLAG; #endif +/** @brief */ +#ifndef HAVE_ENUM_MAG_CAL_STATUS +#define HAVE_ENUM_MAG_CAL_STATUS +typedef enum MAG_CAL_STATUS +{ + MAG_CAL_NOT_STARTED=0, /* | */ + MAG_CAL_WAITING_TO_START=1, /* | */ + MAG_CAL_RUNNING_STEP_ONE=2, /* | */ + MAG_CAL_RUNNING_STEP_TWO=3, /* | */ + MAG_CAL_SUCCESS=4, /* | */ + MAG_CAL_FAILED=5, /* | */ + MAG_CAL_BAD_ORIENTATION=6, /* | */ + MAG_CAL_BAD_RADIUS=7, /* | */ + MAG_CAL_STATUS_ENUM_END=8, /* | */ +} MAG_CAL_STATUS; +#endif + // MAVLINK VERSION #ifndef MAVLINK_VERSION @@ -2392,7 +2203,6 @@ typedef enum MAV_WINCH_STATUS_FLAG #endif // MESSAGE DEFINITIONS -#include "./mavlink_msg_heartbeat.h" #include "./mavlink_msg_sys_status.h" #include "./mavlink_msg_system_time.h" #include "./mavlink_msg_ping.h" @@ -2514,6 +2324,8 @@ typedef enum MAV_WINCH_STATUS_FLAG #include "./mavlink_msg_autopilot_version.h" #include "./mavlink_msg_landing_target.h" #include "./mavlink_msg_fence_status.h" +#include "./mavlink_msg_mag_cal_report.h" +#include "./mavlink_msg_efi_status.h" #include "./mavlink_msg_estimator_status.h" #include "./mavlink_msg_wind_cov.h" #include "./mavlink_msg_gps_input.h" @@ -2534,16 +2346,83 @@ typedef enum MAV_WINCH_STATUS_FLAG #include "./mavlink_msg_named_value_int.h" #include "./mavlink_msg_statustext.h" #include "./mavlink_msg_debug.h" +#include "./mavlink_msg_setup_signing.h" +#include "./mavlink_msg_button_change.h" +#include "./mavlink_msg_play_tune.h" +#include "./mavlink_msg_camera_information.h" +#include "./mavlink_msg_camera_settings.h" +#include "./mavlink_msg_storage_information.h" +#include "./mavlink_msg_camera_capture_status.h" +#include "./mavlink_msg_camera_image_captured.h" +#include "./mavlink_msg_flight_information.h" +#include "./mavlink_msg_mount_orientation.h" +#include "./mavlink_msg_logging_data.h" +#include "./mavlink_msg_logging_data_acked.h" +#include "./mavlink_msg_logging_ack.h" +#include "./mavlink_msg_video_stream_information.h" +#include "./mavlink_msg_video_stream_status.h" +#include "./mavlink_msg_camera_fov_status.h" +#include "./mavlink_msg_camera_tracking_image_status.h" +#include "./mavlink_msg_camera_tracking_geo_status.h" +#include "./mavlink_msg_gimbal_manager_information.h" +#include "./mavlink_msg_gimbal_manager_status.h" +#include "./mavlink_msg_gimbal_manager_set_attitude.h" +#include "./mavlink_msg_gimbal_device_information.h" +#include "./mavlink_msg_gimbal_device_set_attitude.h" +#include "./mavlink_msg_gimbal_device_attitude_status.h" +#include "./mavlink_msg_autopilot_state_for_gimbal_device.h" +#include "./mavlink_msg_gimbal_manager_set_pitchyaw.h" +#include "./mavlink_msg_gimbal_manager_set_manual_control.h" +#include "./mavlink_msg_esc_info.h" +#include "./mavlink_msg_esc_status.h" +#include "./mavlink_msg_wifi_config_ap.h" +#include "./mavlink_msg_ais_vessel.h" +#include "./mavlink_msg_uavcan_node_status.h" +#include "./mavlink_msg_uavcan_node_info.h" +#include "./mavlink_msg_param_ext_request_read.h" +#include "./mavlink_msg_param_ext_request_list.h" +#include "./mavlink_msg_param_ext_value.h" +#include "./mavlink_msg_param_ext_set.h" +#include "./mavlink_msg_param_ext_ack.h" +#include "./mavlink_msg_obstacle_distance.h" +#include "./mavlink_msg_odometry.h" +#include "./mavlink_msg_trajectory_representation_waypoints.h" +#include "./mavlink_msg_trajectory_representation_bezier.h" +#include "./mavlink_msg_cellular_status.h" +#include "./mavlink_msg_isbd_link_status.h" +#include "./mavlink_msg_cellular_config.h" +#include "./mavlink_msg_raw_rpm.h" +#include "./mavlink_msg_utm_global_position.h" +#include "./mavlink_msg_debug_float_array.h" +#include "./mavlink_msg_orbit_execution_status.h" +#include "./mavlink_msg_smart_battery_info.h" +#include "./mavlink_msg_generator_status.h" +#include "./mavlink_msg_actuator_output_status.h" +#include "./mavlink_msg_time_estimate_to_target.h" +#include "./mavlink_msg_tunnel.h" +#include "./mavlink_msg_onboard_computer_status.h" +#include "./mavlink_msg_component_information.h" +#include "./mavlink_msg_play_tune_v2.h" +#include "./mavlink_msg_supported_tunes.h" +#include "./mavlink_msg_wheel_distance.h" +#include "./mavlink_msg_winch_status.h" +#include "./mavlink_msg_open_drone_id_basic_id.h" +#include "./mavlink_msg_open_drone_id_location.h" +#include "./mavlink_msg_open_drone_id_authentication.h" +#include "./mavlink_msg_open_drone_id_self_id.h" +#include "./mavlink_msg_open_drone_id_system.h" +#include "./mavlink_msg_open_drone_id_operator_id.h" +#include "./mavlink_msg_open_drone_id_message_pack.h" // base include - +#include "../minimal/minimal.h" #undef MAVLINK_THIS_XML_IDX #define MAVLINK_THIS_XML_IDX 0 #if MAVLINK_THIS_XML_IDX == MAVLINK_PRIMARY_XML_IDX -# define MAVLINK_MESSAGE_INFO {MAVLINK_MESSAGE_INFO_HEARTBEAT, MAVLINK_MESSAGE_INFO_SYS_STATUS, MAVLINK_MESSAGE_INFO_SYSTEM_TIME, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_PING, MAVLINK_MESSAGE_INFO_CHANGE_OPERATOR_CONTROL, MAVLINK_MESSAGE_INFO_CHANGE_OPERATOR_CONTROL_ACK, MAVLINK_MESSAGE_INFO_AUTH_KEY, MAVLINK_MESSAGE_INFO_LINK_NODE_STATUS, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_SET_MODE, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_PARAM_ACK_TRANSACTION, MAVLINK_MESSAGE_INFO_PARAM_REQUEST_READ, MAVLINK_MESSAGE_INFO_PARAM_REQUEST_LIST, MAVLINK_MESSAGE_INFO_PARAM_VALUE, MAVLINK_MESSAGE_INFO_PARAM_SET, MAVLINK_MESSAGE_INFO_GPS_RAW_INT, MAVLINK_MESSAGE_INFO_GPS_STATUS, MAVLINK_MESSAGE_INFO_SCALED_IMU, MAVLINK_MESSAGE_INFO_RAW_IMU, MAVLINK_MESSAGE_INFO_RAW_PRESSURE, MAVLINK_MESSAGE_INFO_SCALED_PRESSURE, MAVLINK_MESSAGE_INFO_ATTITUDE, MAVLINK_MESSAGE_INFO_ATTITUDE_QUATERNION, MAVLINK_MESSAGE_INFO_LOCAL_POSITION_NED, MAVLINK_MESSAGE_INFO_GLOBAL_POSITION_INT, MAVLINK_MESSAGE_INFO_RC_CHANNELS_SCALED, MAVLINK_MESSAGE_INFO_RC_CHANNELS_RAW, MAVLINK_MESSAGE_INFO_SERVO_OUTPUT_RAW, MAVLINK_MESSAGE_INFO_MISSION_REQUEST_PARTIAL_LIST, MAVLINK_MESSAGE_INFO_MISSION_WRITE_PARTIAL_LIST, MAVLINK_MESSAGE_INFO_MISSION_ITEM, MAVLINK_MESSAGE_INFO_MISSION_REQUEST, MAVLINK_MESSAGE_INFO_MISSION_SET_CURRENT, MAVLINK_MESSAGE_INFO_MISSION_CURRENT, MAVLINK_MESSAGE_INFO_MISSION_REQUEST_LIST, MAVLINK_MESSAGE_INFO_MISSION_COUNT, MAVLINK_MESSAGE_INFO_MISSION_CLEAR_ALL, MAVLINK_MESSAGE_INFO_MISSION_ITEM_REACHED, MAVLINK_MESSAGE_INFO_MISSION_ACK, MAVLINK_MESSAGE_INFO_SET_GPS_GLOBAL_ORIGIN, MAVLINK_MESSAGE_INFO_GPS_GLOBAL_ORIGIN, MAVLINK_MESSAGE_INFO_PARAM_MAP_RC, MAVLINK_MESSAGE_INFO_MISSION_REQUEST_INT, MAVLINK_MESSAGE_INFO_MISSION_CHANGED, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_SAFETY_SET_ALLOWED_AREA, MAVLINK_MESSAGE_INFO_SAFETY_ALLOWED_AREA, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_ATTITUDE_QUATERNION_COV, MAVLINK_MESSAGE_INFO_NAV_CONTROLLER_OUTPUT, MAVLINK_MESSAGE_INFO_GLOBAL_POSITION_INT_COV, MAVLINK_MESSAGE_INFO_LOCAL_POSITION_NED_COV, MAVLINK_MESSAGE_INFO_RC_CHANNELS, MAVLINK_MESSAGE_INFO_REQUEST_DATA_STREAM, MAVLINK_MESSAGE_INFO_DATA_STREAM, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_MANUAL_CONTROL, MAVLINK_MESSAGE_INFO_RC_CHANNELS_OVERRIDE, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_MISSION_ITEM_INT, MAVLINK_MESSAGE_INFO_VFR_HUD, MAVLINK_MESSAGE_INFO_COMMAND_INT, MAVLINK_MESSAGE_INFO_COMMAND_LONG, MAVLINK_MESSAGE_INFO_COMMAND_ACK, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_COMMAND_CANCEL, MAVLINK_MESSAGE_INFO_MANUAL_SETPOINT, MAVLINK_MESSAGE_INFO_SET_ATTITUDE_TARGET, MAVLINK_MESSAGE_INFO_ATTITUDE_TARGET, MAVLINK_MESSAGE_INFO_SET_POSITION_TARGET_LOCAL_NED, MAVLINK_MESSAGE_INFO_POSITION_TARGET_LOCAL_NED, MAVLINK_MESSAGE_INFO_SET_POSITION_TARGET_GLOBAL_INT, MAVLINK_MESSAGE_INFO_POSITION_TARGET_GLOBAL_INT, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_LOCAL_POSITION_NED_SYSTEM_GLOBAL_OFFSET, MAVLINK_MESSAGE_INFO_HIL_STATE, MAVLINK_MESSAGE_INFO_HIL_CONTROLS, MAVLINK_MESSAGE_INFO_HIL_RC_INPUTS_RAW, MAVLINK_MESSAGE_INFO_HIL_ACTUATOR_CONTROLS, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_OPTICAL_FLOW, MAVLINK_MESSAGE_INFO_GLOBAL_VISION_POSITION_ESTIMATE, MAVLINK_MESSAGE_INFO_VISION_POSITION_ESTIMATE, MAVLINK_MESSAGE_INFO_VISION_SPEED_ESTIMATE, MAVLINK_MESSAGE_INFO_VICON_POSITION_ESTIMATE, MAVLINK_MESSAGE_INFO_HIGHRES_IMU, MAVLINK_MESSAGE_INFO_OPTICAL_FLOW_RAD, MAVLINK_MESSAGE_INFO_HIL_SENSOR, MAVLINK_MESSAGE_INFO_SIM_STATE, MAVLINK_MESSAGE_INFO_RADIO_STATUS, MAVLINK_MESSAGE_INFO_FILE_TRANSFER_PROTOCOL, MAVLINK_MESSAGE_INFO_TIMESYNC, MAVLINK_MESSAGE_INFO_CAMERA_TRIGGER, MAVLINK_MESSAGE_INFO_HIL_GPS, MAVLINK_MESSAGE_INFO_HIL_OPTICAL_FLOW, MAVLINK_MESSAGE_INFO_HIL_STATE_QUATERNION, MAVLINK_MESSAGE_INFO_SCALED_IMU2, MAVLINK_MESSAGE_INFO_LOG_REQUEST_LIST, MAVLINK_MESSAGE_INFO_LOG_ENTRY, MAVLINK_MESSAGE_INFO_LOG_REQUEST_DATA, MAVLINK_MESSAGE_INFO_LOG_DATA, MAVLINK_MESSAGE_INFO_LOG_ERASE, MAVLINK_MESSAGE_INFO_LOG_REQUEST_END, MAVLINK_MESSAGE_INFO_GPS_INJECT_DATA, MAVLINK_MESSAGE_INFO_GPS2_RAW, MAVLINK_MESSAGE_INFO_POWER_STATUS, MAVLINK_MESSAGE_INFO_SERIAL_CONTROL, MAVLINK_MESSAGE_INFO_GPS_RTK, MAVLINK_MESSAGE_INFO_GPS2_RTK, MAVLINK_MESSAGE_INFO_SCALED_IMU3, MAVLINK_MESSAGE_INFO_DATA_TRANSMISSION_HANDSHAKE, MAVLINK_MESSAGE_INFO_ENCAPSULATED_DATA, MAVLINK_MESSAGE_INFO_DISTANCE_SENSOR, MAVLINK_MESSAGE_INFO_TERRAIN_REQUEST, MAVLINK_MESSAGE_INFO_TERRAIN_DATA, MAVLINK_MESSAGE_INFO_TERRAIN_CHECK, MAVLINK_MESSAGE_INFO_TERRAIN_REPORT, MAVLINK_MESSAGE_INFO_SCALED_PRESSURE2, MAVLINK_MESSAGE_INFO_ATT_POS_MOCAP, MAVLINK_MESSAGE_INFO_SET_ACTUATOR_CONTROL_TARGET, MAVLINK_MESSAGE_INFO_ACTUATOR_CONTROL_TARGET, MAVLINK_MESSAGE_INFO_ALTITUDE, MAVLINK_MESSAGE_INFO_RESOURCE_REQUEST, MAVLINK_MESSAGE_INFO_SCALED_PRESSURE3, MAVLINK_MESSAGE_INFO_FOLLOW_TARGET, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_CONTROL_SYSTEM_STATE, MAVLINK_MESSAGE_INFO_BATTERY_STATUS, MAVLINK_MESSAGE_INFO_AUTOPILOT_VERSION, MAVLINK_MESSAGE_INFO_LANDING_TARGET, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_FENCE_STATUS, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_ESTIMATOR_STATUS, MAVLINK_MESSAGE_INFO_WIND_COV, MAVLINK_MESSAGE_INFO_GPS_INPUT, MAVLINK_MESSAGE_INFO_GPS_RTCM_DATA, MAVLINK_MESSAGE_INFO_HIGH_LATENCY, MAVLINK_MESSAGE_INFO_HIGH_LATENCY2, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}, MAVLINK_MESSAGE_INFO_VIBRATION, MAVLINK_MESSAGE_INFO_HOME_POSITION, MAVLINK_MESSAGE_INFO_SET_HOME_POSITION, MAVLINK_MESSAGE_INFO_MESSAGE_INTERVAL, MAVLINK_MESSAGE_INFO_EXTENDED_SYS_STATE, MAVLINK_MESSAGE_INFO_ADSB_VEHICLE, MAVLINK_MESSAGE_INFO_COLLISION, MAVLINK_MESSAGE_INFO_V2_EXTENSION, MAVLINK_MESSAGE_INFO_MEMORY_VECT, MAVLINK_MESSAGE_INFO_DEBUG_VECT, MAVLINK_MESSAGE_INFO_NAMED_VALUE_FLOAT, MAVLINK_MESSAGE_INFO_NAMED_VALUE_INT, MAVLINK_MESSAGE_INFO_STATUSTEXT, MAVLINK_MESSAGE_INFO_DEBUG, {"EMPTY",0,{{"","",MAVLINK_TYPE_CHAR,0,0,0}}}} -# define MAVLINK_MESSAGE_NAMES {{ "ACTUATOR_CONTROL_TARGET", 140 }, { "ADSB_VEHICLE", 246 }, { "ALTITUDE", 141 }, { "ATTITUDE", 30 }, { "ATTITUDE_QUATERNION", 31 }, { "ATTITUDE_QUATERNION_COV", 61 }, { "ATTITUDE_TARGET", 83 }, { "ATT_POS_MOCAP", 138 }, { "AUTH_KEY", 7 }, { "AUTOPILOT_VERSION", 148 }, { "BATTERY_STATUS", 147 }, { "CAMERA_TRIGGER", 112 }, { "CHANGE_OPERATOR_CONTROL", 5 }, { "CHANGE_OPERATOR_CONTROL_ACK", 6 }, { "COLLISION", 247 }, { "COMMAND_ACK", 77 }, { "COMMAND_CANCEL", 80 }, { "COMMAND_INT", 75 }, { "COMMAND_LONG", 76 }, { "CONTROL_SYSTEM_STATE", 146 }, { "DATA_STREAM", 67 }, { "DATA_TRANSMISSION_HANDSHAKE", 130 }, { "DEBUG", 254 }, { "DEBUG_VECT", 250 }, { "DISTANCE_SENSOR", 132 }, { "ENCAPSULATED_DATA", 131 }, { "ESTIMATOR_STATUS", 230 }, { "EXTENDED_SYS_STATE", 245 }, { "FENCE_STATUS", 162 }, { "FILE_TRANSFER_PROTOCOL", 110 }, { "FOLLOW_TARGET", 144 }, { "GLOBAL_POSITION_INT", 33 }, { "GLOBAL_POSITION_INT_COV", 63 }, { "GLOBAL_VISION_POSITION_ESTIMATE", 101 }, { "GPS2_RAW", 124 }, { "GPS2_RTK", 128 }, { "GPS_GLOBAL_ORIGIN", 49 }, { "GPS_INJECT_DATA", 123 }, { "GPS_INPUT", 232 }, { "GPS_RAW_INT", 24 }, { "GPS_RTCM_DATA", 233 }, { "GPS_RTK", 127 }, { "GPS_STATUS", 25 }, { "HEARTBEAT", 0 }, { "HIGHRES_IMU", 105 }, { "HIGH_LATENCY", 234 }, { "HIGH_LATENCY2", 235 }, { "HIL_ACTUATOR_CONTROLS", 93 }, { "HIL_CONTROLS", 91 }, { "HIL_GPS", 113 }, { "HIL_OPTICAL_FLOW", 114 }, { "HIL_RC_INPUTS_RAW", 92 }, { "HIL_SENSOR", 107 }, { "HIL_STATE", 90 }, { "HIL_STATE_QUATERNION", 115 }, { "HOME_POSITION", 242 }, { "LANDING_TARGET", 149 }, { "LINK_NODE_STATUS", 8 }, { "LOCAL_POSITION_NED", 32 }, { "LOCAL_POSITION_NED_COV", 64 }, { "LOCAL_POSITION_NED_SYSTEM_GLOBAL_OFFSET", 89 }, { "LOG_DATA", 120 }, { "LOG_ENTRY", 118 }, { "LOG_ERASE", 121 }, { "LOG_REQUEST_DATA", 119 }, { "LOG_REQUEST_END", 122 }, { "LOG_REQUEST_LIST", 117 }, { "MANUAL_CONTROL", 69 }, { "MANUAL_SETPOINT", 81 }, { "MEMORY_VECT", 249 }, { "MESSAGE_INTERVAL", 244 }, { "MISSION_ACK", 47 }, { "MISSION_CHANGED", 52 }, { "MISSION_CLEAR_ALL", 45 }, { "MISSION_COUNT", 44 }, { "MISSION_CURRENT", 42 }, { "MISSION_ITEM", 39 }, { "MISSION_ITEM_INT", 73 }, { "MISSION_ITEM_REACHED", 46 }, { "MISSION_REQUEST", 40 }, { "MISSION_REQUEST_INT", 51 }, { "MISSION_REQUEST_LIST", 43 }, { "MISSION_REQUEST_PARTIAL_LIST", 37 }, { "MISSION_SET_CURRENT", 41 }, { "MISSION_WRITE_PARTIAL_LIST", 38 }, { "NAMED_VALUE_FLOAT", 251 }, { "NAMED_VALUE_INT", 252 }, { "NAV_CONTROLLER_OUTPUT", 62 }, { "OPTICAL_FLOW", 100 }, { "OPTICAL_FLOW_RAD", 106 }, { "PARAM_ACK_TRANSACTION", 19 }, { "PARAM_MAP_RC", 50 }, { "PARAM_REQUEST_LIST", 21 }, { "PARAM_REQUEST_READ", 20 }, { "PARAM_SET", 23 }, { "PARAM_VALUE", 22 }, { "PING", 4 }, { "POSITION_TARGET_GLOBAL_INT", 87 }, { "POSITION_TARGET_LOCAL_NED", 85 }, { "POWER_STATUS", 125 }, { "RADIO_STATUS", 109 }, { "RAW_IMU", 27 }, { "RAW_PRESSURE", 28 }, { "RC_CHANNELS", 65 }, { "RC_CHANNELS_OVERRIDE", 70 }, { "RC_CHANNELS_RAW", 35 }, { "RC_CHANNELS_SCALED", 34 }, { "REQUEST_DATA_STREAM", 66 }, { "RESOURCE_REQUEST", 142 }, { "SAFETY_ALLOWED_AREA", 55 }, { "SAFETY_SET_ALLOWED_AREA", 54 }, { "SCALED_IMU", 26 }, { "SCALED_IMU2", 116 }, { "SCALED_IMU3", 129 }, { "SCALED_PRESSURE", 29 }, { "SCALED_PRESSURE2", 137 }, { "SCALED_PRESSURE3", 143 }, { "SERIAL_CONTROL", 126 }, { "SERVO_OUTPUT_RAW", 36 }, { "SET_ACTUATOR_CONTROL_TARGET", 139 }, { "SET_ATTITUDE_TARGET", 82 }, { "SET_GPS_GLOBAL_ORIGIN", 48 }, { "SET_HOME_POSITION", 243 }, { "SET_MODE", 11 }, { "SET_POSITION_TARGET_GLOBAL_INT", 86 }, { "SET_POSITION_TARGET_LOCAL_NED", 84 }, { "SIM_STATE", 108 }, { "STATUSTEXT", 253 }, { "SYSTEM_TIME", 2 }, { "SYS_STATUS", 1 }, { "TERRAIN_CHECK", 135 }, { "TERRAIN_DATA", 134 }, { "TERRAIN_REPORT", 136 }, { "TERRAIN_REQUEST", 133 }, { "TIMESYNC", 111 }, { "V2_EXTENSION", 248 }, { "VFR_HUD", 74 }, { "VIBRATION", 241 }, { "VICON_POSITION_ESTIMATE", 104 }, { "VISION_POSITION_ESTIMATE", 102 }, { "VISION_SPEED_ESTIMATE", 103 }, { "WIND_COV", 231 }} +# define MAVLINK_MESSAGE_INFO {MAVLINK_MESSAGE_INFO_HEARTBEAT, MAVLINK_MESSAGE_INFO_SYS_STATUS, MAVLINK_MESSAGE_INFO_SYSTEM_TIME, MAVLINK_MESSAGE_INFO_PING, MAVLINK_MESSAGE_INFO_CHANGE_OPERATOR_CONTROL, MAVLINK_MESSAGE_INFO_CHANGE_OPERATOR_CONTROL_ACK, MAVLINK_MESSAGE_INFO_AUTH_KEY, MAVLINK_MESSAGE_INFO_LINK_NODE_STATUS, MAVLINK_MESSAGE_INFO_SET_MODE, MAVLINK_MESSAGE_INFO_PARAM_ACK_TRANSACTION, MAVLINK_MESSAGE_INFO_PARAM_REQUEST_READ, MAVLINK_MESSAGE_INFO_PARAM_REQUEST_LIST, MAVLINK_MESSAGE_INFO_PARAM_VALUE, MAVLINK_MESSAGE_INFO_PARAM_SET, MAVLINK_MESSAGE_INFO_GPS_RAW_INT, MAVLINK_MESSAGE_INFO_GPS_STATUS, MAVLINK_MESSAGE_INFO_SCALED_IMU, MAVLINK_MESSAGE_INFO_RAW_IMU, MAVLINK_MESSAGE_INFO_RAW_PRESSURE, MAVLINK_MESSAGE_INFO_SCALED_PRESSURE, MAVLINK_MESSAGE_INFO_ATTITUDE, MAVLINK_MESSAGE_INFO_ATTITUDE_QUATERNION, MAVLINK_MESSAGE_INFO_LOCAL_POSITION_NED, MAVLINK_MESSAGE_INFO_GLOBAL_POSITION_INT, MAVLINK_MESSAGE_INFO_RC_CHANNELS_SCALED, MAVLINK_MESSAGE_INFO_RC_CHANNELS_RAW, MAVLINK_MESSAGE_INFO_SERVO_OUTPUT_RAW, MAVLINK_MESSAGE_INFO_MISSION_REQUEST_PARTIAL_LIST, MAVLINK_MESSAGE_INFO_MISSION_WRITE_PARTIAL_LIST, MAVLINK_MESSAGE_INFO_MISSION_ITEM, MAVLINK_MESSAGE_INFO_MISSION_REQUEST, MAVLINK_MESSAGE_INFO_MISSION_SET_CURRENT, MAVLINK_MESSAGE_INFO_MISSION_CURRENT, MAVLINK_MESSAGE_INFO_MISSION_REQUEST_LIST, MAVLINK_MESSAGE_INFO_MISSION_COUNT, MAVLINK_MESSAGE_INFO_MISSION_CLEAR_ALL, MAVLINK_MESSAGE_INFO_MISSION_ITEM_REACHED, MAVLINK_MESSAGE_INFO_MISSION_ACK, MAVLINK_MESSAGE_INFO_SET_GPS_GLOBAL_ORIGIN, MAVLINK_MESSAGE_INFO_GPS_GLOBAL_ORIGIN, MAVLINK_MESSAGE_INFO_PARAM_MAP_RC, MAVLINK_MESSAGE_INFO_MISSION_REQUEST_INT, MAVLINK_MESSAGE_INFO_MISSION_CHANGED, MAVLINK_MESSAGE_INFO_SAFETY_SET_ALLOWED_AREA, MAVLINK_MESSAGE_INFO_SAFETY_ALLOWED_AREA, MAVLINK_MESSAGE_INFO_ATTITUDE_QUATERNION_COV, MAVLINK_MESSAGE_INFO_NAV_CONTROLLER_OUTPUT, MAVLINK_MESSAGE_INFO_GLOBAL_POSITION_INT_COV, MAVLINK_MESSAGE_INFO_LOCAL_POSITION_NED_COV, MAVLINK_MESSAGE_INFO_RC_CHANNELS, MAVLINK_MESSAGE_INFO_REQUEST_DATA_STREAM, MAVLINK_MESSAGE_INFO_DATA_STREAM, MAVLINK_MESSAGE_INFO_MANUAL_CONTROL, MAVLINK_MESSAGE_INFO_RC_CHANNELS_OVERRIDE, MAVLINK_MESSAGE_INFO_MISSION_ITEM_INT, MAVLINK_MESSAGE_INFO_VFR_HUD, MAVLINK_MESSAGE_INFO_COMMAND_INT, MAVLINK_MESSAGE_INFO_COMMAND_LONG, MAVLINK_MESSAGE_INFO_COMMAND_ACK, MAVLINK_MESSAGE_INFO_COMMAND_CANCEL, MAVLINK_MESSAGE_INFO_MANUAL_SETPOINT, MAVLINK_MESSAGE_INFO_SET_ATTITUDE_TARGET, MAVLINK_MESSAGE_INFO_ATTITUDE_TARGET, MAVLINK_MESSAGE_INFO_SET_POSITION_TARGET_LOCAL_NED, MAVLINK_MESSAGE_INFO_POSITION_TARGET_LOCAL_NED, MAVLINK_MESSAGE_INFO_SET_POSITION_TARGET_GLOBAL_INT, MAVLINK_MESSAGE_INFO_POSITION_TARGET_GLOBAL_INT, MAVLINK_MESSAGE_INFO_LOCAL_POSITION_NED_SYSTEM_GLOBAL_OFFSET, MAVLINK_MESSAGE_INFO_HIL_STATE, MAVLINK_MESSAGE_INFO_HIL_CONTROLS, MAVLINK_MESSAGE_INFO_HIL_RC_INPUTS_RAW, MAVLINK_MESSAGE_INFO_HIL_ACTUATOR_CONTROLS, MAVLINK_MESSAGE_INFO_OPTICAL_FLOW, MAVLINK_MESSAGE_INFO_GLOBAL_VISION_POSITION_ESTIMATE, MAVLINK_MESSAGE_INFO_VISION_POSITION_ESTIMATE, MAVLINK_MESSAGE_INFO_VISION_SPEED_ESTIMATE, MAVLINK_MESSAGE_INFO_VICON_POSITION_ESTIMATE, MAVLINK_MESSAGE_INFO_HIGHRES_IMU, MAVLINK_MESSAGE_INFO_OPTICAL_FLOW_RAD, MAVLINK_MESSAGE_INFO_HIL_SENSOR, MAVLINK_MESSAGE_INFO_SIM_STATE, MAVLINK_MESSAGE_INFO_RADIO_STATUS, MAVLINK_MESSAGE_INFO_FILE_TRANSFER_PROTOCOL, MAVLINK_MESSAGE_INFO_TIMESYNC, MAVLINK_MESSAGE_INFO_CAMERA_TRIGGER, MAVLINK_MESSAGE_INFO_HIL_GPS, MAVLINK_MESSAGE_INFO_HIL_OPTICAL_FLOW, MAVLINK_MESSAGE_INFO_HIL_STATE_QUATERNION, MAVLINK_MESSAGE_INFO_SCALED_IMU2, MAVLINK_MESSAGE_INFO_LOG_REQUEST_LIST, MAVLINK_MESSAGE_INFO_LOG_ENTRY, MAVLINK_MESSAGE_INFO_LOG_REQUEST_DATA, MAVLINK_MESSAGE_INFO_LOG_DATA, MAVLINK_MESSAGE_INFO_LOG_ERASE, MAVLINK_MESSAGE_INFO_LOG_REQUEST_END, MAVLINK_MESSAGE_INFO_GPS_INJECT_DATA, MAVLINK_MESSAGE_INFO_GPS2_RAW, MAVLINK_MESSAGE_INFO_POWER_STATUS, MAVLINK_MESSAGE_INFO_SERIAL_CONTROL, MAVLINK_MESSAGE_INFO_GPS_RTK, MAVLINK_MESSAGE_INFO_GPS2_RTK, MAVLINK_MESSAGE_INFO_SCALED_IMU3, MAVLINK_MESSAGE_INFO_DATA_TRANSMISSION_HANDSHAKE, MAVLINK_MESSAGE_INFO_ENCAPSULATED_DATA, MAVLINK_MESSAGE_INFO_DISTANCE_SENSOR, MAVLINK_MESSAGE_INFO_TERRAIN_REQUEST, MAVLINK_MESSAGE_INFO_TERRAIN_DATA, MAVLINK_MESSAGE_INFO_TERRAIN_CHECK, MAVLINK_MESSAGE_INFO_TERRAIN_REPORT, MAVLINK_MESSAGE_INFO_SCALED_PRESSURE2, MAVLINK_MESSAGE_INFO_ATT_POS_MOCAP, MAVLINK_MESSAGE_INFO_SET_ACTUATOR_CONTROL_TARGET, MAVLINK_MESSAGE_INFO_ACTUATOR_CONTROL_TARGET, MAVLINK_MESSAGE_INFO_ALTITUDE, MAVLINK_MESSAGE_INFO_RESOURCE_REQUEST, MAVLINK_MESSAGE_INFO_SCALED_PRESSURE3, MAVLINK_MESSAGE_INFO_FOLLOW_TARGET, MAVLINK_MESSAGE_INFO_CONTROL_SYSTEM_STATE, MAVLINK_MESSAGE_INFO_BATTERY_STATUS, MAVLINK_MESSAGE_INFO_AUTOPILOT_VERSION, MAVLINK_MESSAGE_INFO_LANDING_TARGET, MAVLINK_MESSAGE_INFO_FENCE_STATUS, MAVLINK_MESSAGE_INFO_MAG_CAL_REPORT, MAVLINK_MESSAGE_INFO_EFI_STATUS, MAVLINK_MESSAGE_INFO_ESTIMATOR_STATUS, MAVLINK_MESSAGE_INFO_WIND_COV, MAVLINK_MESSAGE_INFO_GPS_INPUT, MAVLINK_MESSAGE_INFO_GPS_RTCM_DATA, MAVLINK_MESSAGE_INFO_HIGH_LATENCY, MAVLINK_MESSAGE_INFO_HIGH_LATENCY2, MAVLINK_MESSAGE_INFO_VIBRATION, MAVLINK_MESSAGE_INFO_HOME_POSITION, MAVLINK_MESSAGE_INFO_SET_HOME_POSITION, MAVLINK_MESSAGE_INFO_MESSAGE_INTERVAL, MAVLINK_MESSAGE_INFO_EXTENDED_SYS_STATE, MAVLINK_MESSAGE_INFO_ADSB_VEHICLE, MAVLINK_MESSAGE_INFO_COLLISION, MAVLINK_MESSAGE_INFO_V2_EXTENSION, MAVLINK_MESSAGE_INFO_MEMORY_VECT, MAVLINK_MESSAGE_INFO_DEBUG_VECT, MAVLINK_MESSAGE_INFO_NAMED_VALUE_FLOAT, MAVLINK_MESSAGE_INFO_NAMED_VALUE_INT, MAVLINK_MESSAGE_INFO_STATUSTEXT, MAVLINK_MESSAGE_INFO_DEBUG, MAVLINK_MESSAGE_INFO_SETUP_SIGNING, MAVLINK_MESSAGE_INFO_BUTTON_CHANGE, MAVLINK_MESSAGE_INFO_PLAY_TUNE, MAVLINK_MESSAGE_INFO_CAMERA_INFORMATION, MAVLINK_MESSAGE_INFO_CAMERA_SETTINGS, MAVLINK_MESSAGE_INFO_STORAGE_INFORMATION, MAVLINK_MESSAGE_INFO_CAMERA_CAPTURE_STATUS, MAVLINK_MESSAGE_INFO_CAMERA_IMAGE_CAPTURED, MAVLINK_MESSAGE_INFO_FLIGHT_INFORMATION, MAVLINK_MESSAGE_INFO_MOUNT_ORIENTATION, MAVLINK_MESSAGE_INFO_LOGGING_DATA, MAVLINK_MESSAGE_INFO_LOGGING_DATA_ACKED, MAVLINK_MESSAGE_INFO_LOGGING_ACK, MAVLINK_MESSAGE_INFO_VIDEO_STREAM_INFORMATION, MAVLINK_MESSAGE_INFO_VIDEO_STREAM_STATUS, MAVLINK_MESSAGE_INFO_CAMERA_FOV_STATUS, MAVLINK_MESSAGE_INFO_CAMERA_TRACKING_IMAGE_STATUS, MAVLINK_MESSAGE_INFO_CAMERA_TRACKING_GEO_STATUS, MAVLINK_MESSAGE_INFO_GIMBAL_MANAGER_INFORMATION, MAVLINK_MESSAGE_INFO_GIMBAL_MANAGER_STATUS, MAVLINK_MESSAGE_INFO_GIMBAL_MANAGER_SET_ATTITUDE, MAVLINK_MESSAGE_INFO_GIMBAL_DEVICE_INFORMATION, MAVLINK_MESSAGE_INFO_GIMBAL_DEVICE_SET_ATTITUDE, MAVLINK_MESSAGE_INFO_GIMBAL_DEVICE_ATTITUDE_STATUS, MAVLINK_MESSAGE_INFO_AUTOPILOT_STATE_FOR_GIMBAL_DEVICE, MAVLINK_MESSAGE_INFO_GIMBAL_MANAGER_SET_PITCHYAW, MAVLINK_MESSAGE_INFO_GIMBAL_MANAGER_SET_MANUAL_CONTROL, MAVLINK_MESSAGE_INFO_ESC_INFO, MAVLINK_MESSAGE_INFO_ESC_STATUS, MAVLINK_MESSAGE_INFO_WIFI_CONFIG_AP, MAVLINK_MESSAGE_INFO_PROTOCOL_VERSION, MAVLINK_MESSAGE_INFO_AIS_VESSEL, MAVLINK_MESSAGE_INFO_UAVCAN_NODE_STATUS, MAVLINK_MESSAGE_INFO_UAVCAN_NODE_INFO, MAVLINK_MESSAGE_INFO_PARAM_EXT_REQUEST_READ, MAVLINK_MESSAGE_INFO_PARAM_EXT_REQUEST_LIST, MAVLINK_MESSAGE_INFO_PARAM_EXT_VALUE, MAVLINK_MESSAGE_INFO_PARAM_EXT_SET, MAVLINK_MESSAGE_INFO_PARAM_EXT_ACK, MAVLINK_MESSAGE_INFO_OBSTACLE_DISTANCE, MAVLINK_MESSAGE_INFO_ODOMETRY, MAVLINK_MESSAGE_INFO_TRAJECTORY_REPRESENTATION_WAYPOINTS, MAVLINK_MESSAGE_INFO_TRAJECTORY_REPRESENTATION_BEZIER, MAVLINK_MESSAGE_INFO_CELLULAR_STATUS, MAVLINK_MESSAGE_INFO_ISBD_LINK_STATUS, MAVLINK_MESSAGE_INFO_CELLULAR_CONFIG, MAVLINK_MESSAGE_INFO_RAW_RPM, MAVLINK_MESSAGE_INFO_UTM_GLOBAL_POSITION, MAVLINK_MESSAGE_INFO_DEBUG_FLOAT_ARRAY, MAVLINK_MESSAGE_INFO_ORBIT_EXECUTION_STATUS, MAVLINK_MESSAGE_INFO_SMART_BATTERY_INFO, MAVLINK_MESSAGE_INFO_GENERATOR_STATUS, MAVLINK_MESSAGE_INFO_ACTUATOR_OUTPUT_STATUS, MAVLINK_MESSAGE_INFO_TIME_ESTIMATE_TO_TARGET, MAVLINK_MESSAGE_INFO_TUNNEL, MAVLINK_MESSAGE_INFO_ONBOARD_COMPUTER_STATUS, MAVLINK_MESSAGE_INFO_COMPONENT_INFORMATION, MAVLINK_MESSAGE_INFO_PLAY_TUNE_V2, MAVLINK_MESSAGE_INFO_SUPPORTED_TUNES, MAVLINK_MESSAGE_INFO_WHEEL_DISTANCE, MAVLINK_MESSAGE_INFO_WINCH_STATUS, MAVLINK_MESSAGE_INFO_OPEN_DRONE_ID_BASIC_ID, MAVLINK_MESSAGE_INFO_OPEN_DRONE_ID_LOCATION, MAVLINK_MESSAGE_INFO_OPEN_DRONE_ID_AUTHENTICATION, MAVLINK_MESSAGE_INFO_OPEN_DRONE_ID_SELF_ID, MAVLINK_MESSAGE_INFO_OPEN_DRONE_ID_SYSTEM, MAVLINK_MESSAGE_INFO_OPEN_DRONE_ID_OPERATOR_ID, MAVLINK_MESSAGE_INFO_OPEN_DRONE_ID_MESSAGE_PACK} +# define MAVLINK_MESSAGE_NAMES {{ "ACTUATOR_CONTROL_TARGET", 140 }, { "ACTUATOR_OUTPUT_STATUS", 375 }, { "ADSB_VEHICLE", 246 }, { "AIS_VESSEL", 301 }, { "ALTITUDE", 141 }, { "ATTITUDE", 30 }, { "ATTITUDE_QUATERNION", 31 }, { "ATTITUDE_QUATERNION_COV", 61 }, { "ATTITUDE_TARGET", 83 }, { "ATT_POS_MOCAP", 138 }, { "AUTH_KEY", 7 }, { "AUTOPILOT_STATE_FOR_GIMBAL_DEVICE", 286 }, { "AUTOPILOT_VERSION", 148 }, { "BATTERY_STATUS", 147 }, { "BUTTON_CHANGE", 257 }, { "CAMERA_CAPTURE_STATUS", 262 }, { "CAMERA_FOV_STATUS", 271 }, { "CAMERA_IMAGE_CAPTURED", 263 }, { "CAMERA_INFORMATION", 259 }, { "CAMERA_SETTINGS", 260 }, { "CAMERA_TRACKING_GEO_STATUS", 276 }, { "CAMERA_TRACKING_IMAGE_STATUS", 275 }, { "CAMERA_TRIGGER", 112 }, { "CELLULAR_CONFIG", 336 }, { "CELLULAR_STATUS", 334 }, { "CHANGE_OPERATOR_CONTROL", 5 }, { "CHANGE_OPERATOR_CONTROL_ACK", 6 }, { "COLLISION", 247 }, { "COMMAND_ACK", 77 }, { "COMMAND_CANCEL", 80 }, { "COMMAND_INT", 75 }, { "COMMAND_LONG", 76 }, { "COMPONENT_INFORMATION", 395 }, { "CONTROL_SYSTEM_STATE", 146 }, { "DATA_STREAM", 67 }, { "DATA_TRANSMISSION_HANDSHAKE", 130 }, { "DEBUG", 254 }, { "DEBUG_FLOAT_ARRAY", 350 }, { "DEBUG_VECT", 250 }, { "DISTANCE_SENSOR", 132 }, { "EFI_STATUS", 225 }, { "ENCAPSULATED_DATA", 131 }, { "ESC_INFO", 290 }, { "ESC_STATUS", 291 }, { "ESTIMATOR_STATUS", 230 }, { "EXTENDED_SYS_STATE", 245 }, { "FENCE_STATUS", 162 }, { "FILE_TRANSFER_PROTOCOL", 110 }, { "FLIGHT_INFORMATION", 264 }, { "FOLLOW_TARGET", 144 }, { "GENERATOR_STATUS", 373 }, { "GIMBAL_DEVICE_ATTITUDE_STATUS", 285 }, { "GIMBAL_DEVICE_INFORMATION", 283 }, { "GIMBAL_DEVICE_SET_ATTITUDE", 284 }, { "GIMBAL_MANAGER_INFORMATION", 280 }, { "GIMBAL_MANAGER_SET_ATTITUDE", 282 }, { "GIMBAL_MANAGER_SET_MANUAL_CONTROL", 288 }, { "GIMBAL_MANAGER_SET_PITCHYAW", 287 }, { "GIMBAL_MANAGER_STATUS", 281 }, { "GLOBAL_POSITION_INT", 33 }, { "GLOBAL_POSITION_INT_COV", 63 }, { "GLOBAL_VISION_POSITION_ESTIMATE", 101 }, { "GPS2_RAW", 124 }, { "GPS2_RTK", 128 }, { "GPS_GLOBAL_ORIGIN", 49 }, { "GPS_INJECT_DATA", 123 }, { "GPS_INPUT", 232 }, { "GPS_RAW_INT", 24 }, { "GPS_RTCM_DATA", 233 }, { "GPS_RTK", 127 }, { "GPS_STATUS", 25 }, { "HEARTBEAT", 0 }, { "HIGHRES_IMU", 105 }, { "HIGH_LATENCY", 234 }, { "HIGH_LATENCY2", 235 }, { "HIL_ACTUATOR_CONTROLS", 93 }, { "HIL_CONTROLS", 91 }, { "HIL_GPS", 113 }, { "HIL_OPTICAL_FLOW", 114 }, { "HIL_RC_INPUTS_RAW", 92 }, { "HIL_SENSOR", 107 }, { "HIL_STATE", 90 }, { "HIL_STATE_QUATERNION", 115 }, { "HOME_POSITION", 242 }, { "ISBD_LINK_STATUS", 335 }, { "LANDING_TARGET", 149 }, { "LINK_NODE_STATUS", 8 }, { "LOCAL_POSITION_NED", 32 }, { "LOCAL_POSITION_NED_COV", 64 }, { "LOCAL_POSITION_NED_SYSTEM_GLOBAL_OFFSET", 89 }, { "LOGGING_ACK", 268 }, { "LOGGING_DATA", 266 }, { "LOGGING_DATA_ACKED", 267 }, { "LOG_DATA", 120 }, { "LOG_ENTRY", 118 }, { "LOG_ERASE", 121 }, { "LOG_REQUEST_DATA", 119 }, { "LOG_REQUEST_END", 122 }, { "LOG_REQUEST_LIST", 117 }, { "MAG_CAL_REPORT", 192 }, { "MANUAL_CONTROL", 69 }, { "MANUAL_SETPOINT", 81 }, { "MEMORY_VECT", 249 }, { "MESSAGE_INTERVAL", 244 }, { "MISSION_ACK", 47 }, { "MISSION_CHANGED", 52 }, { "MISSION_CLEAR_ALL", 45 }, { "MISSION_COUNT", 44 }, { "MISSION_CURRENT", 42 }, { "MISSION_ITEM", 39 }, { "MISSION_ITEM_INT", 73 }, { "MISSION_ITEM_REACHED", 46 }, { "MISSION_REQUEST", 40 }, { "MISSION_REQUEST_INT", 51 }, { "MISSION_REQUEST_LIST", 43 }, { "MISSION_REQUEST_PARTIAL_LIST", 37 }, { "MISSION_SET_CURRENT", 41 }, { "MISSION_WRITE_PARTIAL_LIST", 38 }, { "MOUNT_ORIENTATION", 265 }, { "NAMED_VALUE_FLOAT", 251 }, { "NAMED_VALUE_INT", 252 }, { "NAV_CONTROLLER_OUTPUT", 62 }, { "OBSTACLE_DISTANCE", 330 }, { "ODOMETRY", 331 }, { "ONBOARD_COMPUTER_STATUS", 390 }, { "OPEN_DRONE_ID_AUTHENTICATION", 12902 }, { "OPEN_DRONE_ID_BASIC_ID", 12900 }, { "OPEN_DRONE_ID_LOCATION", 12901 }, { "OPEN_DRONE_ID_MESSAGE_PACK", 12915 }, { "OPEN_DRONE_ID_OPERATOR_ID", 12905 }, { "OPEN_DRONE_ID_SELF_ID", 12903 }, { "OPEN_DRONE_ID_SYSTEM", 12904 }, { "OPTICAL_FLOW", 100 }, { "OPTICAL_FLOW_RAD", 106 }, { "ORBIT_EXECUTION_STATUS", 360 }, { "PARAM_ACK_TRANSACTION", 19 }, { "PARAM_EXT_ACK", 324 }, { "PARAM_EXT_REQUEST_LIST", 321 }, { "PARAM_EXT_REQUEST_READ", 320 }, { "PARAM_EXT_SET", 323 }, { "PARAM_EXT_VALUE", 322 }, { "PARAM_MAP_RC", 50 }, { "PARAM_REQUEST_LIST", 21 }, { "PARAM_REQUEST_READ", 20 }, { "PARAM_SET", 23 }, { "PARAM_VALUE", 22 }, { "PING", 4 }, { "PLAY_TUNE", 258 }, { "PLAY_TUNE_V2", 400 }, { "POSITION_TARGET_GLOBAL_INT", 87 }, { "POSITION_TARGET_LOCAL_NED", 85 }, { "POWER_STATUS", 125 }, { "PROTOCOL_VERSION", 300 }, { "RADIO_STATUS", 109 }, { "RAW_IMU", 27 }, { "RAW_PRESSURE", 28 }, { "RAW_RPM", 339 }, { "RC_CHANNELS", 65 }, { "RC_CHANNELS_OVERRIDE", 70 }, { "RC_CHANNELS_RAW", 35 }, { "RC_CHANNELS_SCALED", 34 }, { "REQUEST_DATA_STREAM", 66 }, { "RESOURCE_REQUEST", 142 }, { "SAFETY_ALLOWED_AREA", 55 }, { "SAFETY_SET_ALLOWED_AREA", 54 }, { "SCALED_IMU", 26 }, { "SCALED_IMU2", 116 }, { "SCALED_IMU3", 129 }, { "SCALED_PRESSURE", 29 }, { "SCALED_PRESSURE2", 137 }, { "SCALED_PRESSURE3", 143 }, { "SERIAL_CONTROL", 126 }, { "SERVO_OUTPUT_RAW", 36 }, { "SETUP_SIGNING", 256 }, { "SET_ACTUATOR_CONTROL_TARGET", 139 }, { "SET_ATTITUDE_TARGET", 82 }, { "SET_GPS_GLOBAL_ORIGIN", 48 }, { "SET_HOME_POSITION", 243 }, { "SET_MODE", 11 }, { "SET_POSITION_TARGET_GLOBAL_INT", 86 }, { "SET_POSITION_TARGET_LOCAL_NED", 84 }, { "SIM_STATE", 108 }, { "SMART_BATTERY_INFO", 370 }, { "STATUSTEXT", 253 }, { "STORAGE_INFORMATION", 261 }, { "SUPPORTED_TUNES", 401 }, { "SYSTEM_TIME", 2 }, { "SYS_STATUS", 1 }, { "TERRAIN_CHECK", 135 }, { "TERRAIN_DATA", 134 }, { "TERRAIN_REPORT", 136 }, { "TERRAIN_REQUEST", 133 }, { "TIMESYNC", 111 }, { "TIME_ESTIMATE_TO_TARGET", 380 }, { "TRAJECTORY_REPRESENTATION_BEZIER", 333 }, { "TRAJECTORY_REPRESENTATION_WAYPOINTS", 332 }, { "TUNNEL", 385 }, { "UAVCAN_NODE_INFO", 311 }, { "UAVCAN_NODE_STATUS", 310 }, { "UTM_GLOBAL_POSITION", 340 }, { "V2_EXTENSION", 248 }, { "VFR_HUD", 74 }, { "VIBRATION", 241 }, { "VICON_POSITION_ESTIMATE", 104 }, { "VIDEO_STREAM_INFORMATION", 269 }, { "VIDEO_STREAM_STATUS", 270 }, { "VISION_POSITION_ESTIMATE", 102 }, { "VISION_SPEED_ESTIMATE", 103 }, { "WHEEL_DISTANCE", 9000 }, { "WIFI_CONFIG_AP", 299 }, { "WINCH_STATUS", 9005 }, { "WIND_COV", 231 }} # if MAVLINK_COMMAND_24BIT # include "../mavlink_get_info.h" # endif diff --git a/lib/main/MAVLink/common/mavlink.h b/lib/main/MAVLink/common/mavlink.h index 35163d3459..39d85d2d5f 100755 --- a/lib/main/MAVLink/common/mavlink.h +++ b/lib/main/MAVLink/common/mavlink.h @@ -9,7 +9,7 @@ #define MAVLINK_PRIMARY_XML_IDX 0 #ifndef MAVLINK_STX -#define MAVLINK_STX 254 +#define MAVLINK_STX 253 #endif #ifndef MAVLINK_ENDIAN @@ -25,7 +25,7 @@ #endif #ifndef MAVLINK_COMMAND_24BIT -#define MAVLINK_COMMAND_24BIT 0 +#define MAVLINK_COMMAND_24BIT 1 #endif #include "version.h" diff --git a/lib/main/MAVLink/common/mavlink_msg_actuator_output_status.h b/lib/main/MAVLink/common/mavlink_msg_actuator_output_status.h new file mode 100644 index 0000000000..52ee04df75 --- /dev/null +++ b/lib/main/MAVLink/common/mavlink_msg_actuator_output_status.h @@ -0,0 +1,255 @@ +#pragma once +// MESSAGE ACTUATOR_OUTPUT_STATUS PACKING + +#define MAVLINK_MSG_ID_ACTUATOR_OUTPUT_STATUS 375 + + +typedef struct __mavlink_actuator_output_status_t { + uint64_t time_usec; /*< [us] Timestamp (since system boot).*/ + uint32_t active; /*< Active outputs*/ + float actuator[32]; /*< Servo / motor output array values. Zero values indicate unused channels.*/ +} mavlink_actuator_output_status_t; + +#define MAVLINK_MSG_ID_ACTUATOR_OUTPUT_STATUS_LEN 140 +#define MAVLINK_MSG_ID_ACTUATOR_OUTPUT_STATUS_MIN_LEN 140 +#define MAVLINK_MSG_ID_375_LEN 140 +#define MAVLINK_MSG_ID_375_MIN_LEN 140 + +#define MAVLINK_MSG_ID_ACTUATOR_OUTPUT_STATUS_CRC 251 +#define MAVLINK_MSG_ID_375_CRC 251 + +#define MAVLINK_MSG_ACTUATOR_OUTPUT_STATUS_FIELD_ACTUATOR_LEN 32 + +#if MAVLINK_COMMAND_24BIT +#define MAVLINK_MESSAGE_INFO_ACTUATOR_OUTPUT_STATUS { \ + 375, \ + "ACTUATOR_OUTPUT_STATUS", \ + 3, \ + { { "time_usec", NULL, MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_actuator_output_status_t, time_usec) }, \ + { "active", NULL, MAVLINK_TYPE_UINT32_T, 0, 8, offsetof(mavlink_actuator_output_status_t, active) }, \ + { "actuator", NULL, MAVLINK_TYPE_FLOAT, 32, 12, offsetof(mavlink_actuator_output_status_t, actuator) }, \ + } \ +} +#else +#define MAVLINK_MESSAGE_INFO_ACTUATOR_OUTPUT_STATUS { \ + "ACTUATOR_OUTPUT_STATUS", \ + 3, \ + { { "time_usec", NULL, MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_actuator_output_status_t, time_usec) }, \ + { "active", NULL, MAVLINK_TYPE_UINT32_T, 0, 8, offsetof(mavlink_actuator_output_status_t, active) }, \ + { "actuator", NULL, MAVLINK_TYPE_FLOAT, 32, 12, offsetof(mavlink_actuator_output_status_t, actuator) }, \ + } \ +} +#endif + +/** + * @brief Pack a actuator_output_status message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param time_usec [us] Timestamp (since system boot). + * @param active Active outputs + * @param actuator Servo / motor output array values. Zero values indicate unused channels. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_actuator_output_status_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint64_t time_usec, uint32_t active, const float *actuator) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_ACTUATOR_OUTPUT_STATUS_LEN]; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_uint32_t(buf, 8, active); + _mav_put_float_array(buf, 12, actuator, 32); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_ACTUATOR_OUTPUT_STATUS_LEN); +#else + mavlink_actuator_output_status_t packet; + packet.time_usec = time_usec; + packet.active = active; + mav_array_memcpy(packet.actuator, actuator, sizeof(float)*32); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_ACTUATOR_OUTPUT_STATUS_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_ACTUATOR_OUTPUT_STATUS; + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_ACTUATOR_OUTPUT_STATUS_MIN_LEN, MAVLINK_MSG_ID_ACTUATOR_OUTPUT_STATUS_LEN, MAVLINK_MSG_ID_ACTUATOR_OUTPUT_STATUS_CRC); +} + +/** + * @brief Pack a actuator_output_status message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param time_usec [us] Timestamp (since system boot). + * @param active Active outputs + * @param actuator Servo / motor output array values. Zero values indicate unused channels. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_actuator_output_status_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint64_t time_usec,uint32_t active,const float *actuator) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_ACTUATOR_OUTPUT_STATUS_LEN]; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_uint32_t(buf, 8, active); + _mav_put_float_array(buf, 12, actuator, 32); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_ACTUATOR_OUTPUT_STATUS_LEN); +#else + mavlink_actuator_output_status_t packet; + packet.time_usec = time_usec; + packet.active = active; + mav_array_memcpy(packet.actuator, actuator, sizeof(float)*32); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_ACTUATOR_OUTPUT_STATUS_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_ACTUATOR_OUTPUT_STATUS; + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_ACTUATOR_OUTPUT_STATUS_MIN_LEN, MAVLINK_MSG_ID_ACTUATOR_OUTPUT_STATUS_LEN, MAVLINK_MSG_ID_ACTUATOR_OUTPUT_STATUS_CRC); +} + +/** + * @brief Encode a actuator_output_status struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param actuator_output_status C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_actuator_output_status_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_actuator_output_status_t* actuator_output_status) +{ + return mavlink_msg_actuator_output_status_pack(system_id, component_id, msg, actuator_output_status->time_usec, actuator_output_status->active, actuator_output_status->actuator); +} + +/** + * @brief Encode a actuator_output_status struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param actuator_output_status C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_actuator_output_status_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_actuator_output_status_t* actuator_output_status) +{ + return mavlink_msg_actuator_output_status_pack_chan(system_id, component_id, chan, msg, actuator_output_status->time_usec, actuator_output_status->active, actuator_output_status->actuator); +} + +/** + * @brief Send a actuator_output_status message + * @param chan MAVLink channel to send the message + * + * @param time_usec [us] Timestamp (since system boot). + * @param active Active outputs + * @param actuator Servo / motor output array values. Zero values indicate unused channels. + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_actuator_output_status_send(mavlink_channel_t chan, uint64_t time_usec, uint32_t active, const float *actuator) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_ACTUATOR_OUTPUT_STATUS_LEN]; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_uint32_t(buf, 8, active); + _mav_put_float_array(buf, 12, actuator, 32); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ACTUATOR_OUTPUT_STATUS, buf, MAVLINK_MSG_ID_ACTUATOR_OUTPUT_STATUS_MIN_LEN, MAVLINK_MSG_ID_ACTUATOR_OUTPUT_STATUS_LEN, MAVLINK_MSG_ID_ACTUATOR_OUTPUT_STATUS_CRC); +#else + mavlink_actuator_output_status_t packet; + packet.time_usec = time_usec; + packet.active = active; + mav_array_memcpy(packet.actuator, actuator, sizeof(float)*32); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ACTUATOR_OUTPUT_STATUS, (const char *)&packet, MAVLINK_MSG_ID_ACTUATOR_OUTPUT_STATUS_MIN_LEN, MAVLINK_MSG_ID_ACTUATOR_OUTPUT_STATUS_LEN, MAVLINK_MSG_ID_ACTUATOR_OUTPUT_STATUS_CRC); +#endif +} + +/** + * @brief Send a actuator_output_status message + * @param chan MAVLink channel to send the message + * @param struct The MAVLink struct to serialize + */ +static inline void mavlink_msg_actuator_output_status_send_struct(mavlink_channel_t chan, const mavlink_actuator_output_status_t* actuator_output_status) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + mavlink_msg_actuator_output_status_send(chan, actuator_output_status->time_usec, actuator_output_status->active, actuator_output_status->actuator); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ACTUATOR_OUTPUT_STATUS, (const char *)actuator_output_status, MAVLINK_MSG_ID_ACTUATOR_OUTPUT_STATUS_MIN_LEN, MAVLINK_MSG_ID_ACTUATOR_OUTPUT_STATUS_LEN, MAVLINK_MSG_ID_ACTUATOR_OUTPUT_STATUS_CRC); +#endif +} + +#if MAVLINK_MSG_ID_ACTUATOR_OUTPUT_STATUS_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_actuator_output_status_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint64_t time_usec, uint32_t active, const float *actuator) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_uint32_t(buf, 8, active); + _mav_put_float_array(buf, 12, actuator, 32); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ACTUATOR_OUTPUT_STATUS, buf, MAVLINK_MSG_ID_ACTUATOR_OUTPUT_STATUS_MIN_LEN, MAVLINK_MSG_ID_ACTUATOR_OUTPUT_STATUS_LEN, MAVLINK_MSG_ID_ACTUATOR_OUTPUT_STATUS_CRC); +#else + mavlink_actuator_output_status_t *packet = (mavlink_actuator_output_status_t *)msgbuf; + packet->time_usec = time_usec; + packet->active = active; + mav_array_memcpy(packet->actuator, actuator, sizeof(float)*32); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ACTUATOR_OUTPUT_STATUS, (const char *)packet, MAVLINK_MSG_ID_ACTUATOR_OUTPUT_STATUS_MIN_LEN, MAVLINK_MSG_ID_ACTUATOR_OUTPUT_STATUS_LEN, MAVLINK_MSG_ID_ACTUATOR_OUTPUT_STATUS_CRC); +#endif +} +#endif + +#endif + +// MESSAGE ACTUATOR_OUTPUT_STATUS UNPACKING + + +/** + * @brief Get field time_usec from actuator_output_status message + * + * @return [us] Timestamp (since system boot). + */ +static inline uint64_t mavlink_msg_actuator_output_status_get_time_usec(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint64_t(msg, 0); +} + +/** + * @brief Get field active from actuator_output_status message + * + * @return Active outputs + */ +static inline uint32_t mavlink_msg_actuator_output_status_get_active(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint32_t(msg, 8); +} + +/** + * @brief Get field actuator from actuator_output_status message + * + * @return Servo / motor output array values. Zero values indicate unused channels. + */ +static inline uint16_t mavlink_msg_actuator_output_status_get_actuator(const mavlink_message_t* msg, float *actuator) +{ + return _MAV_RETURN_float_array(msg, actuator, 32, 12); +} + +/** + * @brief Decode a actuator_output_status message into a struct + * + * @param msg The message to decode + * @param actuator_output_status C-struct to decode the message contents into + */ +static inline void mavlink_msg_actuator_output_status_decode(const mavlink_message_t* msg, mavlink_actuator_output_status_t* actuator_output_status) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + actuator_output_status->time_usec = mavlink_msg_actuator_output_status_get_time_usec(msg); + actuator_output_status->active = mavlink_msg_actuator_output_status_get_active(msg); + mavlink_msg_actuator_output_status_get_actuator(msg, actuator_output_status->actuator); +#else + uint8_t len = msg->len < MAVLINK_MSG_ID_ACTUATOR_OUTPUT_STATUS_LEN? msg->len : MAVLINK_MSG_ID_ACTUATOR_OUTPUT_STATUS_LEN; + memset(actuator_output_status, 0, MAVLINK_MSG_ID_ACTUATOR_OUTPUT_STATUS_LEN); + memcpy(actuator_output_status, _MAV_PAYLOAD(msg), len); +#endif +} diff --git a/lib/main/MAVLink/common/mavlink_msg_ais_vessel.h b/lib/main/MAVLink/common/mavlink_msg_ais_vessel.h new file mode 100644 index 0000000000..e97fe42bcd --- /dev/null +++ b/lib/main/MAVLink/common/mavlink_msg_ais_vessel.h @@ -0,0 +1,606 @@ +#pragma once +// MESSAGE AIS_VESSEL PACKING + +#define MAVLINK_MSG_ID_AIS_VESSEL 301 + + +typedef struct __mavlink_ais_vessel_t { + uint32_t MMSI; /*< Mobile Marine Service Identifier, 9 decimal digits*/ + int32_t lat; /*< [degE7] Latitude*/ + int32_t lon; /*< [degE7] Longitude*/ + uint16_t COG; /*< [cdeg] Course over ground*/ + uint16_t heading; /*< [cdeg] True heading*/ + uint16_t velocity; /*< [cm/s] Speed over ground*/ + uint16_t dimension_bow; /*< [m] Distance from lat/lon location to bow*/ + uint16_t dimension_stern; /*< [m] Distance from lat/lon location to stern*/ + uint16_t tslc; /*< [s] Time since last communication in seconds*/ + uint16_t flags; /*< Bitmask to indicate various statuses including valid data fields*/ + int8_t turn_rate; /*< [cdeg/s] Turn rate*/ + uint8_t navigational_status; /*< Navigational status*/ + uint8_t type; /*< Type of vessels*/ + uint8_t dimension_port; /*< [m] Distance from lat/lon location to port side*/ + uint8_t dimension_starboard; /*< [m] Distance from lat/lon location to starboard side*/ + char callsign[7]; /*< The vessel callsign*/ + char name[20]; /*< The vessel name*/ +} mavlink_ais_vessel_t; + +#define MAVLINK_MSG_ID_AIS_VESSEL_LEN 58 +#define MAVLINK_MSG_ID_AIS_VESSEL_MIN_LEN 58 +#define MAVLINK_MSG_ID_301_LEN 58 +#define MAVLINK_MSG_ID_301_MIN_LEN 58 + +#define MAVLINK_MSG_ID_AIS_VESSEL_CRC 243 +#define MAVLINK_MSG_ID_301_CRC 243 + +#define MAVLINK_MSG_AIS_VESSEL_FIELD_CALLSIGN_LEN 7 +#define MAVLINK_MSG_AIS_VESSEL_FIELD_NAME_LEN 20 + +#if MAVLINK_COMMAND_24BIT +#define MAVLINK_MESSAGE_INFO_AIS_VESSEL { \ + 301, \ + "AIS_VESSEL", \ + 17, \ + { { "MMSI", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_ais_vessel_t, MMSI) }, \ + { "lat", NULL, MAVLINK_TYPE_INT32_T, 0, 4, offsetof(mavlink_ais_vessel_t, lat) }, \ + { "lon", NULL, MAVLINK_TYPE_INT32_T, 0, 8, offsetof(mavlink_ais_vessel_t, lon) }, \ + { "COG", NULL, MAVLINK_TYPE_UINT16_T, 0, 12, offsetof(mavlink_ais_vessel_t, COG) }, \ + { "heading", NULL, MAVLINK_TYPE_UINT16_T, 0, 14, offsetof(mavlink_ais_vessel_t, heading) }, \ + { "velocity", NULL, MAVLINK_TYPE_UINT16_T, 0, 16, offsetof(mavlink_ais_vessel_t, velocity) }, \ + { "turn_rate", NULL, MAVLINK_TYPE_INT8_T, 0, 26, offsetof(mavlink_ais_vessel_t, turn_rate) }, \ + { "navigational_status", NULL, MAVLINK_TYPE_UINT8_T, 0, 27, offsetof(mavlink_ais_vessel_t, navigational_status) }, \ + { "type", NULL, MAVLINK_TYPE_UINT8_T, 0, 28, offsetof(mavlink_ais_vessel_t, type) }, \ + { "dimension_bow", NULL, MAVLINK_TYPE_UINT16_T, 0, 18, offsetof(mavlink_ais_vessel_t, dimension_bow) }, \ + { "dimension_stern", NULL, MAVLINK_TYPE_UINT16_T, 0, 20, offsetof(mavlink_ais_vessel_t, dimension_stern) }, \ + { "dimension_port", NULL, MAVLINK_TYPE_UINT8_T, 0, 29, offsetof(mavlink_ais_vessel_t, dimension_port) }, \ + { "dimension_starboard", NULL, MAVLINK_TYPE_UINT8_T, 0, 30, offsetof(mavlink_ais_vessel_t, dimension_starboard) }, \ + { "callsign", NULL, MAVLINK_TYPE_CHAR, 7, 31, offsetof(mavlink_ais_vessel_t, callsign) }, \ + { "name", NULL, MAVLINK_TYPE_CHAR, 20, 38, offsetof(mavlink_ais_vessel_t, name) }, \ + { "tslc", NULL, MAVLINK_TYPE_UINT16_T, 0, 22, offsetof(mavlink_ais_vessel_t, tslc) }, \ + { "flags", NULL, MAVLINK_TYPE_UINT16_T, 0, 24, offsetof(mavlink_ais_vessel_t, flags) }, \ + } \ +} +#else +#define MAVLINK_MESSAGE_INFO_AIS_VESSEL { \ + "AIS_VESSEL", \ + 17, \ + { { "MMSI", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_ais_vessel_t, MMSI) }, \ + { "lat", NULL, MAVLINK_TYPE_INT32_T, 0, 4, offsetof(mavlink_ais_vessel_t, lat) }, \ + { "lon", NULL, MAVLINK_TYPE_INT32_T, 0, 8, offsetof(mavlink_ais_vessel_t, lon) }, \ + { "COG", NULL, MAVLINK_TYPE_UINT16_T, 0, 12, offsetof(mavlink_ais_vessel_t, COG) }, \ + { "heading", NULL, MAVLINK_TYPE_UINT16_T, 0, 14, offsetof(mavlink_ais_vessel_t, heading) }, \ + { "velocity", NULL, MAVLINK_TYPE_UINT16_T, 0, 16, offsetof(mavlink_ais_vessel_t, velocity) }, \ + { "turn_rate", NULL, MAVLINK_TYPE_INT8_T, 0, 26, offsetof(mavlink_ais_vessel_t, turn_rate) }, \ + { "navigational_status", NULL, MAVLINK_TYPE_UINT8_T, 0, 27, offsetof(mavlink_ais_vessel_t, navigational_status) }, \ + { "type", NULL, MAVLINK_TYPE_UINT8_T, 0, 28, offsetof(mavlink_ais_vessel_t, type) }, \ + { "dimension_bow", NULL, MAVLINK_TYPE_UINT16_T, 0, 18, offsetof(mavlink_ais_vessel_t, dimension_bow) }, \ + { "dimension_stern", NULL, MAVLINK_TYPE_UINT16_T, 0, 20, offsetof(mavlink_ais_vessel_t, dimension_stern) }, \ + { "dimension_port", NULL, MAVLINK_TYPE_UINT8_T, 0, 29, offsetof(mavlink_ais_vessel_t, dimension_port) }, \ + { "dimension_starboard", NULL, MAVLINK_TYPE_UINT8_T, 0, 30, offsetof(mavlink_ais_vessel_t, dimension_starboard) }, \ + { "callsign", NULL, MAVLINK_TYPE_CHAR, 7, 31, offsetof(mavlink_ais_vessel_t, callsign) }, \ + { "name", NULL, MAVLINK_TYPE_CHAR, 20, 38, offsetof(mavlink_ais_vessel_t, name) }, \ + { "tslc", NULL, MAVLINK_TYPE_UINT16_T, 0, 22, offsetof(mavlink_ais_vessel_t, tslc) }, \ + { "flags", NULL, MAVLINK_TYPE_UINT16_T, 0, 24, offsetof(mavlink_ais_vessel_t, flags) }, \ + } \ +} +#endif + +/** + * @brief Pack a ais_vessel message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param MMSI Mobile Marine Service Identifier, 9 decimal digits + * @param lat [degE7] Latitude + * @param lon [degE7] Longitude + * @param COG [cdeg] Course over ground + * @param heading [cdeg] True heading + * @param velocity [cm/s] Speed over ground + * @param turn_rate [cdeg/s] Turn rate + * @param navigational_status Navigational status + * @param type Type of vessels + * @param dimension_bow [m] Distance from lat/lon location to bow + * @param dimension_stern [m] Distance from lat/lon location to stern + * @param dimension_port [m] Distance from lat/lon location to port side + * @param dimension_starboard [m] Distance from lat/lon location to starboard side + * @param callsign The vessel callsign + * @param name The vessel name + * @param tslc [s] Time since last communication in seconds + * @param flags Bitmask to indicate various statuses including valid data fields + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_ais_vessel_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint32_t MMSI, int32_t lat, int32_t lon, uint16_t COG, uint16_t heading, uint16_t velocity, int8_t turn_rate, uint8_t navigational_status, uint8_t type, uint16_t dimension_bow, uint16_t dimension_stern, uint8_t dimension_port, uint8_t dimension_starboard, const char *callsign, const char *name, uint16_t tslc, uint16_t flags) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_AIS_VESSEL_LEN]; + _mav_put_uint32_t(buf, 0, MMSI); + _mav_put_int32_t(buf, 4, lat); + _mav_put_int32_t(buf, 8, lon); + _mav_put_uint16_t(buf, 12, COG); + _mav_put_uint16_t(buf, 14, heading); + _mav_put_uint16_t(buf, 16, velocity); + _mav_put_uint16_t(buf, 18, dimension_bow); + _mav_put_uint16_t(buf, 20, dimension_stern); + _mav_put_uint16_t(buf, 22, tslc); + _mav_put_uint16_t(buf, 24, flags); + _mav_put_int8_t(buf, 26, turn_rate); + _mav_put_uint8_t(buf, 27, navigational_status); + _mav_put_uint8_t(buf, 28, type); + _mav_put_uint8_t(buf, 29, dimension_port); + _mav_put_uint8_t(buf, 30, dimension_starboard); + _mav_put_char_array(buf, 31, callsign, 7); + _mav_put_char_array(buf, 38, name, 20); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_AIS_VESSEL_LEN); +#else + mavlink_ais_vessel_t packet; + packet.MMSI = MMSI; + packet.lat = lat; + packet.lon = lon; + packet.COG = COG; + packet.heading = heading; + packet.velocity = velocity; + packet.dimension_bow = dimension_bow; + packet.dimension_stern = dimension_stern; + packet.tslc = tslc; + packet.flags = flags; + packet.turn_rate = turn_rate; + packet.navigational_status = navigational_status; + packet.type = type; + packet.dimension_port = dimension_port; + packet.dimension_starboard = dimension_starboard; + mav_array_memcpy(packet.callsign, callsign, sizeof(char)*7); + mav_array_memcpy(packet.name, name, sizeof(char)*20); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_AIS_VESSEL_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_AIS_VESSEL; + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_AIS_VESSEL_MIN_LEN, MAVLINK_MSG_ID_AIS_VESSEL_LEN, MAVLINK_MSG_ID_AIS_VESSEL_CRC); +} + +/** + * @brief Pack a ais_vessel message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param MMSI Mobile Marine Service Identifier, 9 decimal digits + * @param lat [degE7] Latitude + * @param lon [degE7] Longitude + * @param COG [cdeg] Course over ground + * @param heading [cdeg] True heading + * @param velocity [cm/s] Speed over ground + * @param turn_rate [cdeg/s] Turn rate + * @param navigational_status Navigational status + * @param type Type of vessels + * @param dimension_bow [m] Distance from lat/lon location to bow + * @param dimension_stern [m] Distance from lat/lon location to stern + * @param dimension_port [m] Distance from lat/lon location to port side + * @param dimension_starboard [m] Distance from lat/lon location to starboard side + * @param callsign The vessel callsign + * @param name The vessel name + * @param tslc [s] Time since last communication in seconds + * @param flags Bitmask to indicate various statuses including valid data fields + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_ais_vessel_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint32_t MMSI,int32_t lat,int32_t lon,uint16_t COG,uint16_t heading,uint16_t velocity,int8_t turn_rate,uint8_t navigational_status,uint8_t type,uint16_t dimension_bow,uint16_t dimension_stern,uint8_t dimension_port,uint8_t dimension_starboard,const char *callsign,const char *name,uint16_t tslc,uint16_t flags) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_AIS_VESSEL_LEN]; + _mav_put_uint32_t(buf, 0, MMSI); + _mav_put_int32_t(buf, 4, lat); + _mav_put_int32_t(buf, 8, lon); + _mav_put_uint16_t(buf, 12, COG); + _mav_put_uint16_t(buf, 14, heading); + _mav_put_uint16_t(buf, 16, velocity); + _mav_put_uint16_t(buf, 18, dimension_bow); + _mav_put_uint16_t(buf, 20, dimension_stern); + _mav_put_uint16_t(buf, 22, tslc); + _mav_put_uint16_t(buf, 24, flags); + _mav_put_int8_t(buf, 26, turn_rate); + _mav_put_uint8_t(buf, 27, navigational_status); + _mav_put_uint8_t(buf, 28, type); + _mav_put_uint8_t(buf, 29, dimension_port); + _mav_put_uint8_t(buf, 30, dimension_starboard); + _mav_put_char_array(buf, 31, callsign, 7); + _mav_put_char_array(buf, 38, name, 20); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_AIS_VESSEL_LEN); +#else + mavlink_ais_vessel_t packet; + packet.MMSI = MMSI; + packet.lat = lat; + packet.lon = lon; + packet.COG = COG; + packet.heading = heading; + packet.velocity = velocity; + packet.dimension_bow = dimension_bow; + packet.dimension_stern = dimension_stern; + packet.tslc = tslc; + packet.flags = flags; + packet.turn_rate = turn_rate; + packet.navigational_status = navigational_status; + packet.type = type; + packet.dimension_port = dimension_port; + packet.dimension_starboard = dimension_starboard; + mav_array_memcpy(packet.callsign, callsign, sizeof(char)*7); + mav_array_memcpy(packet.name, name, sizeof(char)*20); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_AIS_VESSEL_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_AIS_VESSEL; + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_AIS_VESSEL_MIN_LEN, MAVLINK_MSG_ID_AIS_VESSEL_LEN, MAVLINK_MSG_ID_AIS_VESSEL_CRC); +} + +/** + * @brief Encode a ais_vessel struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param ais_vessel C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_ais_vessel_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_ais_vessel_t* ais_vessel) +{ + return mavlink_msg_ais_vessel_pack(system_id, component_id, msg, ais_vessel->MMSI, ais_vessel->lat, ais_vessel->lon, ais_vessel->COG, ais_vessel->heading, ais_vessel->velocity, ais_vessel->turn_rate, ais_vessel->navigational_status, ais_vessel->type, ais_vessel->dimension_bow, ais_vessel->dimension_stern, ais_vessel->dimension_port, ais_vessel->dimension_starboard, ais_vessel->callsign, ais_vessel->name, ais_vessel->tslc, ais_vessel->flags); +} + +/** + * @brief Encode a ais_vessel struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param ais_vessel C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_ais_vessel_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_ais_vessel_t* ais_vessel) +{ + return mavlink_msg_ais_vessel_pack_chan(system_id, component_id, chan, msg, ais_vessel->MMSI, ais_vessel->lat, ais_vessel->lon, ais_vessel->COG, ais_vessel->heading, ais_vessel->velocity, ais_vessel->turn_rate, ais_vessel->navigational_status, ais_vessel->type, ais_vessel->dimension_bow, ais_vessel->dimension_stern, ais_vessel->dimension_port, ais_vessel->dimension_starboard, ais_vessel->callsign, ais_vessel->name, ais_vessel->tslc, ais_vessel->flags); +} + +/** + * @brief Send a ais_vessel message + * @param chan MAVLink channel to send the message + * + * @param MMSI Mobile Marine Service Identifier, 9 decimal digits + * @param lat [degE7] Latitude + * @param lon [degE7] Longitude + * @param COG [cdeg] Course over ground + * @param heading [cdeg] True heading + * @param velocity [cm/s] Speed over ground + * @param turn_rate [cdeg/s] Turn rate + * @param navigational_status Navigational status + * @param type Type of vessels + * @param dimension_bow [m] Distance from lat/lon location to bow + * @param dimension_stern [m] Distance from lat/lon location to stern + * @param dimension_port [m] Distance from lat/lon location to port side + * @param dimension_starboard [m] Distance from lat/lon location to starboard side + * @param callsign The vessel callsign + * @param name The vessel name + * @param tslc [s] Time since last communication in seconds + * @param flags Bitmask to indicate various statuses including valid data fields + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_ais_vessel_send(mavlink_channel_t chan, uint32_t MMSI, int32_t lat, int32_t lon, uint16_t COG, uint16_t heading, uint16_t velocity, int8_t turn_rate, uint8_t navigational_status, uint8_t type, uint16_t dimension_bow, uint16_t dimension_stern, uint8_t dimension_port, uint8_t dimension_starboard, const char *callsign, const char *name, uint16_t tslc, uint16_t flags) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_AIS_VESSEL_LEN]; + _mav_put_uint32_t(buf, 0, MMSI); + _mav_put_int32_t(buf, 4, lat); + _mav_put_int32_t(buf, 8, lon); + _mav_put_uint16_t(buf, 12, COG); + _mav_put_uint16_t(buf, 14, heading); + _mav_put_uint16_t(buf, 16, velocity); + _mav_put_uint16_t(buf, 18, dimension_bow); + _mav_put_uint16_t(buf, 20, dimension_stern); + _mav_put_uint16_t(buf, 22, tslc); + _mav_put_uint16_t(buf, 24, flags); + _mav_put_int8_t(buf, 26, turn_rate); + _mav_put_uint8_t(buf, 27, navigational_status); + _mav_put_uint8_t(buf, 28, type); + _mav_put_uint8_t(buf, 29, dimension_port); + _mav_put_uint8_t(buf, 30, dimension_starboard); + _mav_put_char_array(buf, 31, callsign, 7); + _mav_put_char_array(buf, 38, name, 20); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AIS_VESSEL, buf, MAVLINK_MSG_ID_AIS_VESSEL_MIN_LEN, MAVLINK_MSG_ID_AIS_VESSEL_LEN, MAVLINK_MSG_ID_AIS_VESSEL_CRC); +#else + mavlink_ais_vessel_t packet; + packet.MMSI = MMSI; + packet.lat = lat; + packet.lon = lon; + packet.COG = COG; + packet.heading = heading; + packet.velocity = velocity; + packet.dimension_bow = dimension_bow; + packet.dimension_stern = dimension_stern; + packet.tslc = tslc; + packet.flags = flags; + packet.turn_rate = turn_rate; + packet.navigational_status = navigational_status; + packet.type = type; + packet.dimension_port = dimension_port; + packet.dimension_starboard = dimension_starboard; + mav_array_memcpy(packet.callsign, callsign, sizeof(char)*7); + mav_array_memcpy(packet.name, name, sizeof(char)*20); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AIS_VESSEL, (const char *)&packet, MAVLINK_MSG_ID_AIS_VESSEL_MIN_LEN, MAVLINK_MSG_ID_AIS_VESSEL_LEN, MAVLINK_MSG_ID_AIS_VESSEL_CRC); +#endif +} + +/** + * @brief Send a ais_vessel message + * @param chan MAVLink channel to send the message + * @param struct The MAVLink struct to serialize + */ +static inline void mavlink_msg_ais_vessel_send_struct(mavlink_channel_t chan, const mavlink_ais_vessel_t* ais_vessel) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + mavlink_msg_ais_vessel_send(chan, ais_vessel->MMSI, ais_vessel->lat, ais_vessel->lon, ais_vessel->COG, ais_vessel->heading, ais_vessel->velocity, ais_vessel->turn_rate, ais_vessel->navigational_status, ais_vessel->type, ais_vessel->dimension_bow, ais_vessel->dimension_stern, ais_vessel->dimension_port, ais_vessel->dimension_starboard, ais_vessel->callsign, ais_vessel->name, ais_vessel->tslc, ais_vessel->flags); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AIS_VESSEL, (const char *)ais_vessel, MAVLINK_MSG_ID_AIS_VESSEL_MIN_LEN, MAVLINK_MSG_ID_AIS_VESSEL_LEN, MAVLINK_MSG_ID_AIS_VESSEL_CRC); +#endif +} + +#if MAVLINK_MSG_ID_AIS_VESSEL_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_ais_vessel_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint32_t MMSI, int32_t lat, int32_t lon, uint16_t COG, uint16_t heading, uint16_t velocity, int8_t turn_rate, uint8_t navigational_status, uint8_t type, uint16_t dimension_bow, uint16_t dimension_stern, uint8_t dimension_port, uint8_t dimension_starboard, const char *callsign, const char *name, uint16_t tslc, uint16_t flags) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint32_t(buf, 0, MMSI); + _mav_put_int32_t(buf, 4, lat); + _mav_put_int32_t(buf, 8, lon); + _mav_put_uint16_t(buf, 12, COG); + _mav_put_uint16_t(buf, 14, heading); + _mav_put_uint16_t(buf, 16, velocity); + _mav_put_uint16_t(buf, 18, dimension_bow); + _mav_put_uint16_t(buf, 20, dimension_stern); + _mav_put_uint16_t(buf, 22, tslc); + _mav_put_uint16_t(buf, 24, flags); + _mav_put_int8_t(buf, 26, turn_rate); + _mav_put_uint8_t(buf, 27, navigational_status); + _mav_put_uint8_t(buf, 28, type); + _mav_put_uint8_t(buf, 29, dimension_port); + _mav_put_uint8_t(buf, 30, dimension_starboard); + _mav_put_char_array(buf, 31, callsign, 7); + _mav_put_char_array(buf, 38, name, 20); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AIS_VESSEL, buf, MAVLINK_MSG_ID_AIS_VESSEL_MIN_LEN, MAVLINK_MSG_ID_AIS_VESSEL_LEN, MAVLINK_MSG_ID_AIS_VESSEL_CRC); +#else + mavlink_ais_vessel_t *packet = (mavlink_ais_vessel_t *)msgbuf; + packet->MMSI = MMSI; + packet->lat = lat; + packet->lon = lon; + packet->COG = COG; + packet->heading = heading; + packet->velocity = velocity; + packet->dimension_bow = dimension_bow; + packet->dimension_stern = dimension_stern; + packet->tslc = tslc; + packet->flags = flags; + packet->turn_rate = turn_rate; + packet->navigational_status = navigational_status; + packet->type = type; + packet->dimension_port = dimension_port; + packet->dimension_starboard = dimension_starboard; + mav_array_memcpy(packet->callsign, callsign, sizeof(char)*7); + mav_array_memcpy(packet->name, name, sizeof(char)*20); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AIS_VESSEL, (const char *)packet, MAVLINK_MSG_ID_AIS_VESSEL_MIN_LEN, MAVLINK_MSG_ID_AIS_VESSEL_LEN, MAVLINK_MSG_ID_AIS_VESSEL_CRC); +#endif +} +#endif + +#endif + +// MESSAGE AIS_VESSEL UNPACKING + + +/** + * @brief Get field MMSI from ais_vessel message + * + * @return Mobile Marine Service Identifier, 9 decimal digits + */ +static inline uint32_t mavlink_msg_ais_vessel_get_MMSI(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint32_t(msg, 0); +} + +/** + * @brief Get field lat from ais_vessel message + * + * @return [degE7] Latitude + */ +static inline int32_t mavlink_msg_ais_vessel_get_lat(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int32_t(msg, 4); +} + +/** + * @brief Get field lon from ais_vessel message + * + * @return [degE7] Longitude + */ +static inline int32_t mavlink_msg_ais_vessel_get_lon(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int32_t(msg, 8); +} + +/** + * @brief Get field COG from ais_vessel message + * + * @return [cdeg] Course over ground + */ +static inline uint16_t mavlink_msg_ais_vessel_get_COG(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 12); +} + +/** + * @brief Get field heading from ais_vessel message + * + * @return [cdeg] True heading + */ +static inline uint16_t mavlink_msg_ais_vessel_get_heading(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 14); +} + +/** + * @brief Get field velocity from ais_vessel message + * + * @return [cm/s] Speed over ground + */ +static inline uint16_t mavlink_msg_ais_vessel_get_velocity(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 16); +} + +/** + * @brief Get field turn_rate from ais_vessel message + * + * @return [cdeg/s] Turn rate + */ +static inline int8_t mavlink_msg_ais_vessel_get_turn_rate(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int8_t(msg, 26); +} + +/** + * @brief Get field navigational_status from ais_vessel message + * + * @return Navigational status + */ +static inline uint8_t mavlink_msg_ais_vessel_get_navigational_status(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 27); +} + +/** + * @brief Get field type from ais_vessel message + * + * @return Type of vessels + */ +static inline uint8_t mavlink_msg_ais_vessel_get_type(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 28); +} + +/** + * @brief Get field dimension_bow from ais_vessel message + * + * @return [m] Distance from lat/lon location to bow + */ +static inline uint16_t mavlink_msg_ais_vessel_get_dimension_bow(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 18); +} + +/** + * @brief Get field dimension_stern from ais_vessel message + * + * @return [m] Distance from lat/lon location to stern + */ +static inline uint16_t mavlink_msg_ais_vessel_get_dimension_stern(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 20); +} + +/** + * @brief Get field dimension_port from ais_vessel message + * + * @return [m] Distance from lat/lon location to port side + */ +static inline uint8_t mavlink_msg_ais_vessel_get_dimension_port(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 29); +} + +/** + * @brief Get field dimension_starboard from ais_vessel message + * + * @return [m] Distance from lat/lon location to starboard side + */ +static inline uint8_t mavlink_msg_ais_vessel_get_dimension_starboard(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 30); +} + +/** + * @brief Get field callsign from ais_vessel message + * + * @return The vessel callsign + */ +static inline uint16_t mavlink_msg_ais_vessel_get_callsign(const mavlink_message_t* msg, char *callsign) +{ + return _MAV_RETURN_char_array(msg, callsign, 7, 31); +} + +/** + * @brief Get field name from ais_vessel message + * + * @return The vessel name + */ +static inline uint16_t mavlink_msg_ais_vessel_get_name(const mavlink_message_t* msg, char *name) +{ + return _MAV_RETURN_char_array(msg, name, 20, 38); +} + +/** + * @brief Get field tslc from ais_vessel message + * + * @return [s] Time since last communication in seconds + */ +static inline uint16_t mavlink_msg_ais_vessel_get_tslc(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 22); +} + +/** + * @brief Get field flags from ais_vessel message + * + * @return Bitmask to indicate various statuses including valid data fields + */ +static inline uint16_t mavlink_msg_ais_vessel_get_flags(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 24); +} + +/** + * @brief Decode a ais_vessel message into a struct + * + * @param msg The message to decode + * @param ais_vessel C-struct to decode the message contents into + */ +static inline void mavlink_msg_ais_vessel_decode(const mavlink_message_t* msg, mavlink_ais_vessel_t* ais_vessel) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + ais_vessel->MMSI = mavlink_msg_ais_vessel_get_MMSI(msg); + ais_vessel->lat = mavlink_msg_ais_vessel_get_lat(msg); + ais_vessel->lon = mavlink_msg_ais_vessel_get_lon(msg); + ais_vessel->COG = mavlink_msg_ais_vessel_get_COG(msg); + ais_vessel->heading = mavlink_msg_ais_vessel_get_heading(msg); + ais_vessel->velocity = mavlink_msg_ais_vessel_get_velocity(msg); + ais_vessel->dimension_bow = mavlink_msg_ais_vessel_get_dimension_bow(msg); + ais_vessel->dimension_stern = mavlink_msg_ais_vessel_get_dimension_stern(msg); + ais_vessel->tslc = mavlink_msg_ais_vessel_get_tslc(msg); + ais_vessel->flags = mavlink_msg_ais_vessel_get_flags(msg); + ais_vessel->turn_rate = mavlink_msg_ais_vessel_get_turn_rate(msg); + ais_vessel->navigational_status = mavlink_msg_ais_vessel_get_navigational_status(msg); + ais_vessel->type = mavlink_msg_ais_vessel_get_type(msg); + ais_vessel->dimension_port = mavlink_msg_ais_vessel_get_dimension_port(msg); + ais_vessel->dimension_starboard = mavlink_msg_ais_vessel_get_dimension_starboard(msg); + mavlink_msg_ais_vessel_get_callsign(msg, ais_vessel->callsign); + mavlink_msg_ais_vessel_get_name(msg, ais_vessel->name); +#else + uint8_t len = msg->len < MAVLINK_MSG_ID_AIS_VESSEL_LEN? msg->len : MAVLINK_MSG_ID_AIS_VESSEL_LEN; + memset(ais_vessel, 0, MAVLINK_MSG_ID_AIS_VESSEL_LEN); + memcpy(ais_vessel, _MAV_PAYLOAD(msg), len); +#endif +} diff --git a/lib/main/MAVLink/common/mavlink_msg_att_pos_mocap.h b/lib/main/MAVLink/common/mavlink_msg_att_pos_mocap.h index dab6f557b6..2bf0bfd117 100755 --- a/lib/main/MAVLink/common/mavlink_msg_att_pos_mocap.h +++ b/lib/main/MAVLink/common/mavlink_msg_att_pos_mocap.h @@ -10,39 +10,43 @@ typedef struct __mavlink_att_pos_mocap_t { float x; /*< [m] X position (NED)*/ float y; /*< [m] Y position (NED)*/ float z; /*< [m] Z position (NED)*/ + float covariance[21]; /*< Row-major representation of a pose 6x6 cross-covariance matrix upper right triangle (states: x, y, z, roll, pitch, yaw; first six entries are the first ROW, next five entries are the second ROW, etc.). If unknown, assign NaN value to first element in the array.*/ } mavlink_att_pos_mocap_t; -#define MAVLINK_MSG_ID_ATT_POS_MOCAP_LEN 36 +#define MAVLINK_MSG_ID_ATT_POS_MOCAP_LEN 120 #define MAVLINK_MSG_ID_ATT_POS_MOCAP_MIN_LEN 36 -#define MAVLINK_MSG_ID_138_LEN 36 +#define MAVLINK_MSG_ID_138_LEN 120 #define MAVLINK_MSG_ID_138_MIN_LEN 36 #define MAVLINK_MSG_ID_ATT_POS_MOCAP_CRC 109 #define MAVLINK_MSG_ID_138_CRC 109 #define MAVLINK_MSG_ATT_POS_MOCAP_FIELD_Q_LEN 4 +#define MAVLINK_MSG_ATT_POS_MOCAP_FIELD_COVARIANCE_LEN 21 #if MAVLINK_COMMAND_24BIT #define MAVLINK_MESSAGE_INFO_ATT_POS_MOCAP { \ 138, \ "ATT_POS_MOCAP", \ - 5, \ + 6, \ { { "time_usec", NULL, MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_att_pos_mocap_t, time_usec) }, \ { "q", NULL, MAVLINK_TYPE_FLOAT, 4, 8, offsetof(mavlink_att_pos_mocap_t, q) }, \ { "x", NULL, MAVLINK_TYPE_FLOAT, 0, 24, offsetof(mavlink_att_pos_mocap_t, x) }, \ { "y", NULL, MAVLINK_TYPE_FLOAT, 0, 28, offsetof(mavlink_att_pos_mocap_t, y) }, \ { "z", NULL, MAVLINK_TYPE_FLOAT, 0, 32, offsetof(mavlink_att_pos_mocap_t, z) }, \ + { "covariance", NULL, MAVLINK_TYPE_FLOAT, 21, 36, offsetof(mavlink_att_pos_mocap_t, covariance) }, \ } \ } #else #define MAVLINK_MESSAGE_INFO_ATT_POS_MOCAP { \ "ATT_POS_MOCAP", \ - 5, \ + 6, \ { { "time_usec", NULL, MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_att_pos_mocap_t, time_usec) }, \ { "q", NULL, MAVLINK_TYPE_FLOAT, 4, 8, offsetof(mavlink_att_pos_mocap_t, q) }, \ { "x", NULL, MAVLINK_TYPE_FLOAT, 0, 24, offsetof(mavlink_att_pos_mocap_t, x) }, \ { "y", NULL, MAVLINK_TYPE_FLOAT, 0, 28, offsetof(mavlink_att_pos_mocap_t, y) }, \ { "z", NULL, MAVLINK_TYPE_FLOAT, 0, 32, offsetof(mavlink_att_pos_mocap_t, z) }, \ + { "covariance", NULL, MAVLINK_TYPE_FLOAT, 21, 36, offsetof(mavlink_att_pos_mocap_t, covariance) }, \ } \ } #endif @@ -58,10 +62,11 @@ typedef struct __mavlink_att_pos_mocap_t { * @param x [m] X position (NED) * @param y [m] Y position (NED) * @param z [m] Z position (NED) + * @param covariance Row-major representation of a pose 6x6 cross-covariance matrix upper right triangle (states: x, y, z, roll, pitch, yaw; first six entries are the first ROW, next five entries are the second ROW, etc.). If unknown, assign NaN value to first element in the array. * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_att_pos_mocap_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint64_t time_usec, const float *q, float x, float y, float z) + uint64_t time_usec, const float *q, float x, float y, float z, const float *covariance) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_ATT_POS_MOCAP_LEN]; @@ -70,6 +75,7 @@ static inline uint16_t mavlink_msg_att_pos_mocap_pack(uint8_t system_id, uint8_t _mav_put_float(buf, 28, y); _mav_put_float(buf, 32, z); _mav_put_float_array(buf, 8, q, 4); + _mav_put_float_array(buf, 36, covariance, 21); memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_ATT_POS_MOCAP_LEN); #else mavlink_att_pos_mocap_t packet; @@ -78,6 +84,7 @@ static inline uint16_t mavlink_msg_att_pos_mocap_pack(uint8_t system_id, uint8_t packet.y = y; packet.z = z; mav_array_memcpy(packet.q, q, sizeof(float)*4); + mav_array_memcpy(packet.covariance, covariance, sizeof(float)*21); memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_ATT_POS_MOCAP_LEN); #endif @@ -96,11 +103,12 @@ static inline uint16_t mavlink_msg_att_pos_mocap_pack(uint8_t system_id, uint8_t * @param x [m] X position (NED) * @param y [m] Y position (NED) * @param z [m] Z position (NED) + * @param covariance Row-major representation of a pose 6x6 cross-covariance matrix upper right triangle (states: x, y, z, roll, pitch, yaw; first six entries are the first ROW, next five entries are the second ROW, etc.). If unknown, assign NaN value to first element in the array. * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_att_pos_mocap_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, - uint64_t time_usec,const float *q,float x,float y,float z) + uint64_t time_usec,const float *q,float x,float y,float z,const float *covariance) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_ATT_POS_MOCAP_LEN]; @@ -109,6 +117,7 @@ static inline uint16_t mavlink_msg_att_pos_mocap_pack_chan(uint8_t system_id, ui _mav_put_float(buf, 28, y); _mav_put_float(buf, 32, z); _mav_put_float_array(buf, 8, q, 4); + _mav_put_float_array(buf, 36, covariance, 21); memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_ATT_POS_MOCAP_LEN); #else mavlink_att_pos_mocap_t packet; @@ -117,6 +126,7 @@ static inline uint16_t mavlink_msg_att_pos_mocap_pack_chan(uint8_t system_id, ui packet.y = y; packet.z = z; mav_array_memcpy(packet.q, q, sizeof(float)*4); + mav_array_memcpy(packet.covariance, covariance, sizeof(float)*21); memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_ATT_POS_MOCAP_LEN); #endif @@ -134,7 +144,7 @@ static inline uint16_t mavlink_msg_att_pos_mocap_pack_chan(uint8_t system_id, ui */ static inline uint16_t mavlink_msg_att_pos_mocap_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_att_pos_mocap_t* att_pos_mocap) { - return mavlink_msg_att_pos_mocap_pack(system_id, component_id, msg, att_pos_mocap->time_usec, att_pos_mocap->q, att_pos_mocap->x, att_pos_mocap->y, att_pos_mocap->z); + return mavlink_msg_att_pos_mocap_pack(system_id, component_id, msg, att_pos_mocap->time_usec, att_pos_mocap->q, att_pos_mocap->x, att_pos_mocap->y, att_pos_mocap->z, att_pos_mocap->covariance); } /** @@ -148,7 +158,7 @@ static inline uint16_t mavlink_msg_att_pos_mocap_encode(uint8_t system_id, uint8 */ static inline uint16_t mavlink_msg_att_pos_mocap_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_att_pos_mocap_t* att_pos_mocap) { - return mavlink_msg_att_pos_mocap_pack_chan(system_id, component_id, chan, msg, att_pos_mocap->time_usec, att_pos_mocap->q, att_pos_mocap->x, att_pos_mocap->y, att_pos_mocap->z); + return mavlink_msg_att_pos_mocap_pack_chan(system_id, component_id, chan, msg, att_pos_mocap->time_usec, att_pos_mocap->q, att_pos_mocap->x, att_pos_mocap->y, att_pos_mocap->z, att_pos_mocap->covariance); } /** @@ -160,10 +170,11 @@ static inline uint16_t mavlink_msg_att_pos_mocap_encode_chan(uint8_t system_id, * @param x [m] X position (NED) * @param y [m] Y position (NED) * @param z [m] Z position (NED) + * @param covariance Row-major representation of a pose 6x6 cross-covariance matrix upper right triangle (states: x, y, z, roll, pitch, yaw; first six entries are the first ROW, next five entries are the second ROW, etc.). If unknown, assign NaN value to first element in the array. */ #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS -static inline void mavlink_msg_att_pos_mocap_send(mavlink_channel_t chan, uint64_t time_usec, const float *q, float x, float y, float z) +static inline void mavlink_msg_att_pos_mocap_send(mavlink_channel_t chan, uint64_t time_usec, const float *q, float x, float y, float z, const float *covariance) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_ATT_POS_MOCAP_LEN]; @@ -172,6 +183,7 @@ static inline void mavlink_msg_att_pos_mocap_send(mavlink_channel_t chan, uint64 _mav_put_float(buf, 28, y); _mav_put_float(buf, 32, z); _mav_put_float_array(buf, 8, q, 4); + _mav_put_float_array(buf, 36, covariance, 21); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ATT_POS_MOCAP, buf, MAVLINK_MSG_ID_ATT_POS_MOCAP_MIN_LEN, MAVLINK_MSG_ID_ATT_POS_MOCAP_LEN, MAVLINK_MSG_ID_ATT_POS_MOCAP_CRC); #else mavlink_att_pos_mocap_t packet; @@ -180,6 +192,7 @@ static inline void mavlink_msg_att_pos_mocap_send(mavlink_channel_t chan, uint64 packet.y = y; packet.z = z; mav_array_memcpy(packet.q, q, sizeof(float)*4); + mav_array_memcpy(packet.covariance, covariance, sizeof(float)*21); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ATT_POS_MOCAP, (const char *)&packet, MAVLINK_MSG_ID_ATT_POS_MOCAP_MIN_LEN, MAVLINK_MSG_ID_ATT_POS_MOCAP_LEN, MAVLINK_MSG_ID_ATT_POS_MOCAP_CRC); #endif } @@ -192,7 +205,7 @@ static inline void mavlink_msg_att_pos_mocap_send(mavlink_channel_t chan, uint64 static inline void mavlink_msg_att_pos_mocap_send_struct(mavlink_channel_t chan, const mavlink_att_pos_mocap_t* att_pos_mocap) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - mavlink_msg_att_pos_mocap_send(chan, att_pos_mocap->time_usec, att_pos_mocap->q, att_pos_mocap->x, att_pos_mocap->y, att_pos_mocap->z); + mavlink_msg_att_pos_mocap_send(chan, att_pos_mocap->time_usec, att_pos_mocap->q, att_pos_mocap->x, att_pos_mocap->y, att_pos_mocap->z, att_pos_mocap->covariance); #else _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ATT_POS_MOCAP, (const char *)att_pos_mocap, MAVLINK_MSG_ID_ATT_POS_MOCAP_MIN_LEN, MAVLINK_MSG_ID_ATT_POS_MOCAP_LEN, MAVLINK_MSG_ID_ATT_POS_MOCAP_CRC); #endif @@ -206,7 +219,7 @@ static inline void mavlink_msg_att_pos_mocap_send_struct(mavlink_channel_t chan, is usually the receive buffer for the channel, and allows a reply to an incoming message with minimum stack space usage. */ -static inline void mavlink_msg_att_pos_mocap_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint64_t time_usec, const float *q, float x, float y, float z) +static inline void mavlink_msg_att_pos_mocap_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint64_t time_usec, const float *q, float x, float y, float z, const float *covariance) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char *buf = (char *)msgbuf; @@ -215,6 +228,7 @@ static inline void mavlink_msg_att_pos_mocap_send_buf(mavlink_message_t *msgbuf, _mav_put_float(buf, 28, y); _mav_put_float(buf, 32, z); _mav_put_float_array(buf, 8, q, 4); + _mav_put_float_array(buf, 36, covariance, 21); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ATT_POS_MOCAP, buf, MAVLINK_MSG_ID_ATT_POS_MOCAP_MIN_LEN, MAVLINK_MSG_ID_ATT_POS_MOCAP_LEN, MAVLINK_MSG_ID_ATT_POS_MOCAP_CRC); #else mavlink_att_pos_mocap_t *packet = (mavlink_att_pos_mocap_t *)msgbuf; @@ -223,6 +237,7 @@ static inline void mavlink_msg_att_pos_mocap_send_buf(mavlink_message_t *msgbuf, packet->y = y; packet->z = z; mav_array_memcpy(packet->q, q, sizeof(float)*4); + mav_array_memcpy(packet->covariance, covariance, sizeof(float)*21); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ATT_POS_MOCAP, (const char *)packet, MAVLINK_MSG_ID_ATT_POS_MOCAP_MIN_LEN, MAVLINK_MSG_ID_ATT_POS_MOCAP_LEN, MAVLINK_MSG_ID_ATT_POS_MOCAP_CRC); #endif } @@ -283,6 +298,16 @@ static inline float mavlink_msg_att_pos_mocap_get_z(const mavlink_message_t* msg return _MAV_RETURN_float(msg, 32); } +/** + * @brief Get field covariance from att_pos_mocap message + * + * @return Row-major representation of a pose 6x6 cross-covariance matrix upper right triangle (states: x, y, z, roll, pitch, yaw; first six entries are the first ROW, next five entries are the second ROW, etc.). If unknown, assign NaN value to first element in the array. + */ +static inline uint16_t mavlink_msg_att_pos_mocap_get_covariance(const mavlink_message_t* msg, float *covariance) +{ + return _MAV_RETURN_float_array(msg, covariance, 21, 36); +} + /** * @brief Decode a att_pos_mocap message into a struct * @@ -297,6 +322,7 @@ static inline void mavlink_msg_att_pos_mocap_decode(const mavlink_message_t* msg att_pos_mocap->x = mavlink_msg_att_pos_mocap_get_x(msg); att_pos_mocap->y = mavlink_msg_att_pos_mocap_get_y(msg); att_pos_mocap->z = mavlink_msg_att_pos_mocap_get_z(msg); + mavlink_msg_att_pos_mocap_get_covariance(msg, att_pos_mocap->covariance); #else uint8_t len = msg->len < MAVLINK_MSG_ID_ATT_POS_MOCAP_LEN? msg->len : MAVLINK_MSG_ID_ATT_POS_MOCAP_LEN; memset(att_pos_mocap, 0, MAVLINK_MSG_ID_ATT_POS_MOCAP_LEN); diff --git a/lib/main/MAVLink/common/mavlink_msg_attitude_quaternion.h b/lib/main/MAVLink/common/mavlink_msg_attitude_quaternion.h index 3ef53c4413..a613816221 100755 --- a/lib/main/MAVLink/common/mavlink_msg_attitude_quaternion.h +++ b/lib/main/MAVLink/common/mavlink_msg_attitude_quaternion.h @@ -13,23 +13,24 @@ typedef struct __mavlink_attitude_quaternion_t { float rollspeed; /*< [rad/s] Roll angular speed*/ float pitchspeed; /*< [rad/s] Pitch angular speed*/ float yawspeed; /*< [rad/s] Yaw angular speed*/ + float repr_offset_q[4]; /*< Rotation offset by which the attitude quaternion and angular speed vector should be rotated for user display (quaternion with [w, x, y, z] order, zero-rotation is [1, 0, 0, 0], send [0, 0, 0, 0] if field not supported). This field is intended for systems in which the reference attitude may change during flight. For example, tailsitters VTOLs rotate their reference attitude by 90 degrees between hover mode and fixed wing mode, thus repr_offset_q is equal to [1, 0, 0, 0] in hover mode and equal to [0.7071, 0, 0.7071, 0] in fixed wing mode.*/ } mavlink_attitude_quaternion_t; -#define MAVLINK_MSG_ID_ATTITUDE_QUATERNION_LEN 32 +#define MAVLINK_MSG_ID_ATTITUDE_QUATERNION_LEN 48 #define MAVLINK_MSG_ID_ATTITUDE_QUATERNION_MIN_LEN 32 -#define MAVLINK_MSG_ID_31_LEN 32 +#define MAVLINK_MSG_ID_31_LEN 48 #define MAVLINK_MSG_ID_31_MIN_LEN 32 #define MAVLINK_MSG_ID_ATTITUDE_QUATERNION_CRC 246 #define MAVLINK_MSG_ID_31_CRC 246 - +#define MAVLINK_MSG_ATTITUDE_QUATERNION_FIELD_REPR_OFFSET_Q_LEN 4 #if MAVLINK_COMMAND_24BIT #define MAVLINK_MESSAGE_INFO_ATTITUDE_QUATERNION { \ 31, \ "ATTITUDE_QUATERNION", \ - 8, \ + 9, \ { { "time_boot_ms", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_attitude_quaternion_t, time_boot_ms) }, \ { "q1", NULL, MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_attitude_quaternion_t, q1) }, \ { "q2", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_attitude_quaternion_t, q2) }, \ @@ -38,12 +39,13 @@ typedef struct __mavlink_attitude_quaternion_t { { "rollspeed", NULL, MAVLINK_TYPE_FLOAT, 0, 20, offsetof(mavlink_attitude_quaternion_t, rollspeed) }, \ { "pitchspeed", NULL, MAVLINK_TYPE_FLOAT, 0, 24, offsetof(mavlink_attitude_quaternion_t, pitchspeed) }, \ { "yawspeed", NULL, MAVLINK_TYPE_FLOAT, 0, 28, offsetof(mavlink_attitude_quaternion_t, yawspeed) }, \ + { "repr_offset_q", NULL, MAVLINK_TYPE_FLOAT, 4, 32, offsetof(mavlink_attitude_quaternion_t, repr_offset_q) }, \ } \ } #else #define MAVLINK_MESSAGE_INFO_ATTITUDE_QUATERNION { \ "ATTITUDE_QUATERNION", \ - 8, \ + 9, \ { { "time_boot_ms", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_attitude_quaternion_t, time_boot_ms) }, \ { "q1", NULL, MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_attitude_quaternion_t, q1) }, \ { "q2", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_attitude_quaternion_t, q2) }, \ @@ -52,6 +54,7 @@ typedef struct __mavlink_attitude_quaternion_t { { "rollspeed", NULL, MAVLINK_TYPE_FLOAT, 0, 20, offsetof(mavlink_attitude_quaternion_t, rollspeed) }, \ { "pitchspeed", NULL, MAVLINK_TYPE_FLOAT, 0, 24, offsetof(mavlink_attitude_quaternion_t, pitchspeed) }, \ { "yawspeed", NULL, MAVLINK_TYPE_FLOAT, 0, 28, offsetof(mavlink_attitude_quaternion_t, yawspeed) }, \ + { "repr_offset_q", NULL, MAVLINK_TYPE_FLOAT, 4, 32, offsetof(mavlink_attitude_quaternion_t, repr_offset_q) }, \ } \ } #endif @@ -70,10 +73,11 @@ typedef struct __mavlink_attitude_quaternion_t { * @param rollspeed [rad/s] Roll angular speed * @param pitchspeed [rad/s] Pitch angular speed * @param yawspeed [rad/s] Yaw angular speed + * @param repr_offset_q Rotation offset by which the attitude quaternion and angular speed vector should be rotated for user display (quaternion with [w, x, y, z] order, zero-rotation is [1, 0, 0, 0], send [0, 0, 0, 0] if field not supported). This field is intended for systems in which the reference attitude may change during flight. For example, tailsitters VTOLs rotate their reference attitude by 90 degrees between hover mode and fixed wing mode, thus repr_offset_q is equal to [1, 0, 0, 0] in hover mode and equal to [0.7071, 0, 0.7071, 0] in fixed wing mode. * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_attitude_quaternion_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint32_t time_boot_ms, float q1, float q2, float q3, float q4, float rollspeed, float pitchspeed, float yawspeed) + uint32_t time_boot_ms, float q1, float q2, float q3, float q4, float rollspeed, float pitchspeed, float yawspeed, const float *repr_offset_q) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_ATTITUDE_QUATERNION_LEN]; @@ -85,7 +89,7 @@ static inline uint16_t mavlink_msg_attitude_quaternion_pack(uint8_t system_id, u _mav_put_float(buf, 20, rollspeed); _mav_put_float(buf, 24, pitchspeed); _mav_put_float(buf, 28, yawspeed); - + _mav_put_float_array(buf, 32, repr_offset_q, 4); memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_ATTITUDE_QUATERNION_LEN); #else mavlink_attitude_quaternion_t packet; @@ -97,7 +101,7 @@ static inline uint16_t mavlink_msg_attitude_quaternion_pack(uint8_t system_id, u packet.rollspeed = rollspeed; packet.pitchspeed = pitchspeed; packet.yawspeed = yawspeed; - + mav_array_memcpy(packet.repr_offset_q, repr_offset_q, sizeof(float)*4); memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_ATTITUDE_QUATERNION_LEN); #endif @@ -119,11 +123,12 @@ static inline uint16_t mavlink_msg_attitude_quaternion_pack(uint8_t system_id, u * @param rollspeed [rad/s] Roll angular speed * @param pitchspeed [rad/s] Pitch angular speed * @param yawspeed [rad/s] Yaw angular speed + * @param repr_offset_q Rotation offset by which the attitude quaternion and angular speed vector should be rotated for user display (quaternion with [w, x, y, z] order, zero-rotation is [1, 0, 0, 0], send [0, 0, 0, 0] if field not supported). This field is intended for systems in which the reference attitude may change during flight. For example, tailsitters VTOLs rotate their reference attitude by 90 degrees between hover mode and fixed wing mode, thus repr_offset_q is equal to [1, 0, 0, 0] in hover mode and equal to [0.7071, 0, 0.7071, 0] in fixed wing mode. * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_attitude_quaternion_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, - uint32_t time_boot_ms,float q1,float q2,float q3,float q4,float rollspeed,float pitchspeed,float yawspeed) + uint32_t time_boot_ms,float q1,float q2,float q3,float q4,float rollspeed,float pitchspeed,float yawspeed,const float *repr_offset_q) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_ATTITUDE_QUATERNION_LEN]; @@ -135,7 +140,7 @@ static inline uint16_t mavlink_msg_attitude_quaternion_pack_chan(uint8_t system_ _mav_put_float(buf, 20, rollspeed); _mav_put_float(buf, 24, pitchspeed); _mav_put_float(buf, 28, yawspeed); - + _mav_put_float_array(buf, 32, repr_offset_q, 4); memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_ATTITUDE_QUATERNION_LEN); #else mavlink_attitude_quaternion_t packet; @@ -147,7 +152,7 @@ static inline uint16_t mavlink_msg_attitude_quaternion_pack_chan(uint8_t system_ packet.rollspeed = rollspeed; packet.pitchspeed = pitchspeed; packet.yawspeed = yawspeed; - + mav_array_memcpy(packet.repr_offset_q, repr_offset_q, sizeof(float)*4); memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_ATTITUDE_QUATERNION_LEN); #endif @@ -165,7 +170,7 @@ static inline uint16_t mavlink_msg_attitude_quaternion_pack_chan(uint8_t system_ */ static inline uint16_t mavlink_msg_attitude_quaternion_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_attitude_quaternion_t* attitude_quaternion) { - return mavlink_msg_attitude_quaternion_pack(system_id, component_id, msg, attitude_quaternion->time_boot_ms, attitude_quaternion->q1, attitude_quaternion->q2, attitude_quaternion->q3, attitude_quaternion->q4, attitude_quaternion->rollspeed, attitude_quaternion->pitchspeed, attitude_quaternion->yawspeed); + return mavlink_msg_attitude_quaternion_pack(system_id, component_id, msg, attitude_quaternion->time_boot_ms, attitude_quaternion->q1, attitude_quaternion->q2, attitude_quaternion->q3, attitude_quaternion->q4, attitude_quaternion->rollspeed, attitude_quaternion->pitchspeed, attitude_quaternion->yawspeed, attitude_quaternion->repr_offset_q); } /** @@ -179,7 +184,7 @@ static inline uint16_t mavlink_msg_attitude_quaternion_encode(uint8_t system_id, */ static inline uint16_t mavlink_msg_attitude_quaternion_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_attitude_quaternion_t* attitude_quaternion) { - return mavlink_msg_attitude_quaternion_pack_chan(system_id, component_id, chan, msg, attitude_quaternion->time_boot_ms, attitude_quaternion->q1, attitude_quaternion->q2, attitude_quaternion->q3, attitude_quaternion->q4, attitude_quaternion->rollspeed, attitude_quaternion->pitchspeed, attitude_quaternion->yawspeed); + return mavlink_msg_attitude_quaternion_pack_chan(system_id, component_id, chan, msg, attitude_quaternion->time_boot_ms, attitude_quaternion->q1, attitude_quaternion->q2, attitude_quaternion->q3, attitude_quaternion->q4, attitude_quaternion->rollspeed, attitude_quaternion->pitchspeed, attitude_quaternion->yawspeed, attitude_quaternion->repr_offset_q); } /** @@ -194,10 +199,11 @@ static inline uint16_t mavlink_msg_attitude_quaternion_encode_chan(uint8_t syste * @param rollspeed [rad/s] Roll angular speed * @param pitchspeed [rad/s] Pitch angular speed * @param yawspeed [rad/s] Yaw angular speed + * @param repr_offset_q Rotation offset by which the attitude quaternion and angular speed vector should be rotated for user display (quaternion with [w, x, y, z] order, zero-rotation is [1, 0, 0, 0], send [0, 0, 0, 0] if field not supported). This field is intended for systems in which the reference attitude may change during flight. For example, tailsitters VTOLs rotate their reference attitude by 90 degrees between hover mode and fixed wing mode, thus repr_offset_q is equal to [1, 0, 0, 0] in hover mode and equal to [0.7071, 0, 0.7071, 0] in fixed wing mode. */ #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS -static inline void mavlink_msg_attitude_quaternion_send(mavlink_channel_t chan, uint32_t time_boot_ms, float q1, float q2, float q3, float q4, float rollspeed, float pitchspeed, float yawspeed) +static inline void mavlink_msg_attitude_quaternion_send(mavlink_channel_t chan, uint32_t time_boot_ms, float q1, float q2, float q3, float q4, float rollspeed, float pitchspeed, float yawspeed, const float *repr_offset_q) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_ATTITUDE_QUATERNION_LEN]; @@ -209,7 +215,7 @@ static inline void mavlink_msg_attitude_quaternion_send(mavlink_channel_t chan, _mav_put_float(buf, 20, rollspeed); _mav_put_float(buf, 24, pitchspeed); _mav_put_float(buf, 28, yawspeed); - + _mav_put_float_array(buf, 32, repr_offset_q, 4); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ATTITUDE_QUATERNION, buf, MAVLINK_MSG_ID_ATTITUDE_QUATERNION_MIN_LEN, MAVLINK_MSG_ID_ATTITUDE_QUATERNION_LEN, MAVLINK_MSG_ID_ATTITUDE_QUATERNION_CRC); #else mavlink_attitude_quaternion_t packet; @@ -221,7 +227,7 @@ static inline void mavlink_msg_attitude_quaternion_send(mavlink_channel_t chan, packet.rollspeed = rollspeed; packet.pitchspeed = pitchspeed; packet.yawspeed = yawspeed; - + mav_array_memcpy(packet.repr_offset_q, repr_offset_q, sizeof(float)*4); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ATTITUDE_QUATERNION, (const char *)&packet, MAVLINK_MSG_ID_ATTITUDE_QUATERNION_MIN_LEN, MAVLINK_MSG_ID_ATTITUDE_QUATERNION_LEN, MAVLINK_MSG_ID_ATTITUDE_QUATERNION_CRC); #endif } @@ -234,7 +240,7 @@ static inline void mavlink_msg_attitude_quaternion_send(mavlink_channel_t chan, static inline void mavlink_msg_attitude_quaternion_send_struct(mavlink_channel_t chan, const mavlink_attitude_quaternion_t* attitude_quaternion) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - mavlink_msg_attitude_quaternion_send(chan, attitude_quaternion->time_boot_ms, attitude_quaternion->q1, attitude_quaternion->q2, attitude_quaternion->q3, attitude_quaternion->q4, attitude_quaternion->rollspeed, attitude_quaternion->pitchspeed, attitude_quaternion->yawspeed); + mavlink_msg_attitude_quaternion_send(chan, attitude_quaternion->time_boot_ms, attitude_quaternion->q1, attitude_quaternion->q2, attitude_quaternion->q3, attitude_quaternion->q4, attitude_quaternion->rollspeed, attitude_quaternion->pitchspeed, attitude_quaternion->yawspeed, attitude_quaternion->repr_offset_q); #else _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ATTITUDE_QUATERNION, (const char *)attitude_quaternion, MAVLINK_MSG_ID_ATTITUDE_QUATERNION_MIN_LEN, MAVLINK_MSG_ID_ATTITUDE_QUATERNION_LEN, MAVLINK_MSG_ID_ATTITUDE_QUATERNION_CRC); #endif @@ -248,7 +254,7 @@ static inline void mavlink_msg_attitude_quaternion_send_struct(mavlink_channel_t is usually the receive buffer for the channel, and allows a reply to an incoming message with minimum stack space usage. */ -static inline void mavlink_msg_attitude_quaternion_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint32_t time_boot_ms, float q1, float q2, float q3, float q4, float rollspeed, float pitchspeed, float yawspeed) +static inline void mavlink_msg_attitude_quaternion_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint32_t time_boot_ms, float q1, float q2, float q3, float q4, float rollspeed, float pitchspeed, float yawspeed, const float *repr_offset_q) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char *buf = (char *)msgbuf; @@ -260,7 +266,7 @@ static inline void mavlink_msg_attitude_quaternion_send_buf(mavlink_message_t *m _mav_put_float(buf, 20, rollspeed); _mav_put_float(buf, 24, pitchspeed); _mav_put_float(buf, 28, yawspeed); - + _mav_put_float_array(buf, 32, repr_offset_q, 4); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ATTITUDE_QUATERNION, buf, MAVLINK_MSG_ID_ATTITUDE_QUATERNION_MIN_LEN, MAVLINK_MSG_ID_ATTITUDE_QUATERNION_LEN, MAVLINK_MSG_ID_ATTITUDE_QUATERNION_CRC); #else mavlink_attitude_quaternion_t *packet = (mavlink_attitude_quaternion_t *)msgbuf; @@ -272,7 +278,7 @@ static inline void mavlink_msg_attitude_quaternion_send_buf(mavlink_message_t *m packet->rollspeed = rollspeed; packet->pitchspeed = pitchspeed; packet->yawspeed = yawspeed; - + mav_array_memcpy(packet->repr_offset_q, repr_offset_q, sizeof(float)*4); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ATTITUDE_QUATERNION, (const char *)packet, MAVLINK_MSG_ID_ATTITUDE_QUATERNION_MIN_LEN, MAVLINK_MSG_ID_ATTITUDE_QUATERNION_LEN, MAVLINK_MSG_ID_ATTITUDE_QUATERNION_CRC); #endif } @@ -363,6 +369,16 @@ static inline float mavlink_msg_attitude_quaternion_get_yawspeed(const mavlink_m return _MAV_RETURN_float(msg, 28); } +/** + * @brief Get field repr_offset_q from attitude_quaternion message + * + * @return Rotation offset by which the attitude quaternion and angular speed vector should be rotated for user display (quaternion with [w, x, y, z] order, zero-rotation is [1, 0, 0, 0], send [0, 0, 0, 0] if field not supported). This field is intended for systems in which the reference attitude may change during flight. For example, tailsitters VTOLs rotate their reference attitude by 90 degrees between hover mode and fixed wing mode, thus repr_offset_q is equal to [1, 0, 0, 0] in hover mode and equal to [0.7071, 0, 0.7071, 0] in fixed wing mode. + */ +static inline uint16_t mavlink_msg_attitude_quaternion_get_repr_offset_q(const mavlink_message_t* msg, float *repr_offset_q) +{ + return _MAV_RETURN_float_array(msg, repr_offset_q, 4, 32); +} + /** * @brief Decode a attitude_quaternion message into a struct * @@ -380,6 +396,7 @@ static inline void mavlink_msg_attitude_quaternion_decode(const mavlink_message_ attitude_quaternion->rollspeed = mavlink_msg_attitude_quaternion_get_rollspeed(msg); attitude_quaternion->pitchspeed = mavlink_msg_attitude_quaternion_get_pitchspeed(msg); attitude_quaternion->yawspeed = mavlink_msg_attitude_quaternion_get_yawspeed(msg); + mavlink_msg_attitude_quaternion_get_repr_offset_q(msg, attitude_quaternion->repr_offset_q); #else uint8_t len = msg->len < MAVLINK_MSG_ID_ATTITUDE_QUATERNION_LEN? msg->len : MAVLINK_MSG_ID_ATTITUDE_QUATERNION_LEN; memset(attitude_quaternion, 0, MAVLINK_MSG_ID_ATTITUDE_QUATERNION_LEN); diff --git a/lib/main/MAVLink/common/mavlink_msg_attitude_target.h b/lib/main/MAVLink/common/mavlink_msg_attitude_target.h index af578884f4..bedbb93e9f 100755 --- a/lib/main/MAVLink/common/mavlink_msg_attitude_target.h +++ b/lib/main/MAVLink/common/mavlink_msg_attitude_target.h @@ -11,7 +11,7 @@ typedef struct __mavlink_attitude_target_t { float body_pitch_rate; /*< [rad/s] Body pitch rate*/ float body_yaw_rate; /*< [rad/s] Body yaw rate*/ float thrust; /*< Collective thrust, normalized to 0 .. 1 (-1 .. 1 for vehicles capable of reverse trust)*/ - uint8_t type_mask; /*< Mappings: If any of these bits are set, the corresponding input should be ignored: bit 1: body roll rate, bit 2: body pitch rate, bit 3: body yaw rate. bit 4-bit 7: reserved, bit 8: attitude*/ + uint8_t type_mask; /*< Bitmap to indicate which dimensions should be ignored by the vehicle.*/ } mavlink_attitude_target_t; #define MAVLINK_MSG_ID_ATTITUDE_TARGET_LEN 37 @@ -60,7 +60,7 @@ typedef struct __mavlink_attitude_target_t { * @param msg The MAVLink message to compress the data into * * @param time_boot_ms [ms] Timestamp (time since system boot). - * @param type_mask Mappings: If any of these bits are set, the corresponding input should be ignored: bit 1: body roll rate, bit 2: body pitch rate, bit 3: body yaw rate. bit 4-bit 7: reserved, bit 8: attitude + * @param type_mask Bitmap to indicate which dimensions should be ignored by the vehicle. * @param q Attitude quaternion (w, x, y, z order, zero-rotation is 1, 0, 0, 0) * @param body_roll_rate [rad/s] Body roll rate * @param body_pitch_rate [rad/s] Body pitch rate @@ -104,7 +104,7 @@ static inline uint16_t mavlink_msg_attitude_target_pack(uint8_t system_id, uint8 * @param chan The MAVLink channel this message will be sent over * @param msg The MAVLink message to compress the data into * @param time_boot_ms [ms] Timestamp (time since system boot). - * @param type_mask Mappings: If any of these bits are set, the corresponding input should be ignored: bit 1: body roll rate, bit 2: body pitch rate, bit 3: body yaw rate. bit 4-bit 7: reserved, bit 8: attitude + * @param type_mask Bitmap to indicate which dimensions should be ignored by the vehicle. * @param q Attitude quaternion (w, x, y, z order, zero-rotation is 1, 0, 0, 0) * @param body_roll_rate [rad/s] Body roll rate * @param body_pitch_rate [rad/s] Body pitch rate @@ -174,7 +174,7 @@ static inline uint16_t mavlink_msg_attitude_target_encode_chan(uint8_t system_id * @param chan MAVLink channel to send the message * * @param time_boot_ms [ms] Timestamp (time since system boot). - * @param type_mask Mappings: If any of these bits are set, the corresponding input should be ignored: bit 1: body roll rate, bit 2: body pitch rate, bit 3: body yaw rate. bit 4-bit 7: reserved, bit 8: attitude + * @param type_mask Bitmap to indicate which dimensions should be ignored by the vehicle. * @param q Attitude quaternion (w, x, y, z order, zero-rotation is 1, 0, 0, 0) * @param body_roll_rate [rad/s] Body roll rate * @param body_pitch_rate [rad/s] Body pitch rate @@ -274,7 +274,7 @@ static inline uint32_t mavlink_msg_attitude_target_get_time_boot_ms(const mavlin /** * @brief Get field type_mask from attitude_target message * - * @return Mappings: If any of these bits are set, the corresponding input should be ignored: bit 1: body roll rate, bit 2: body pitch rate, bit 3: body yaw rate. bit 4-bit 7: reserved, bit 8: attitude + * @return Bitmap to indicate which dimensions should be ignored by the vehicle. */ static inline uint8_t mavlink_msg_attitude_target_get_type_mask(const mavlink_message_t* msg) { diff --git a/lib/main/MAVLink/common/mavlink_msg_autopilot_state_for_gimbal_device.h b/lib/main/MAVLink/common/mavlink_msg_autopilot_state_for_gimbal_device.h new file mode 100644 index 0000000000..c9cdce8d54 --- /dev/null +++ b/lib/main/MAVLink/common/mavlink_msg_autopilot_state_for_gimbal_device.h @@ -0,0 +1,480 @@ +#pragma once +// MESSAGE AUTOPILOT_STATE_FOR_GIMBAL_DEVICE PACKING + +#define MAVLINK_MSG_ID_AUTOPILOT_STATE_FOR_GIMBAL_DEVICE 286 + + +typedef struct __mavlink_autopilot_state_for_gimbal_device_t { + uint64_t time_boot_us; /*< [us] Timestamp (time since system boot).*/ + float q[4]; /*< Quaternion components of autopilot attitude: w, x, y, z (1 0 0 0 is the null-rotation, Hamilton convention).*/ + uint32_t q_estimated_delay_us; /*< [us] Estimated delay of the attitude data.*/ + float vx; /*< [m/s] X Speed in NED (North, East, Down).*/ + float vy; /*< [m/s] Y Speed in NED (North, East, Down).*/ + float vz; /*< [m/s] Z Speed in NED (North, East, Down).*/ + uint32_t v_estimated_delay_us; /*< [us] Estimated delay of the speed data.*/ + float feed_forward_angular_velocity_z; /*< [rad/s] Feed forward Z component of angular velocity, positive is yawing to the right, NaN to be ignored. This is to indicate if the autopilot is actively yawing.*/ + uint16_t estimator_status; /*< Bitmap indicating which estimator outputs are valid.*/ + uint8_t target_system; /*< System ID*/ + uint8_t target_component; /*< Component ID*/ + uint8_t landed_state; /*< The landed state. Is set to MAV_LANDED_STATE_UNDEFINED if landed state is unknown.*/ +} mavlink_autopilot_state_for_gimbal_device_t; + +#define MAVLINK_MSG_ID_AUTOPILOT_STATE_FOR_GIMBAL_DEVICE_LEN 53 +#define MAVLINK_MSG_ID_AUTOPILOT_STATE_FOR_GIMBAL_DEVICE_MIN_LEN 53 +#define MAVLINK_MSG_ID_286_LEN 53 +#define MAVLINK_MSG_ID_286_MIN_LEN 53 + +#define MAVLINK_MSG_ID_AUTOPILOT_STATE_FOR_GIMBAL_DEVICE_CRC 210 +#define MAVLINK_MSG_ID_286_CRC 210 + +#define MAVLINK_MSG_AUTOPILOT_STATE_FOR_GIMBAL_DEVICE_FIELD_Q_LEN 4 + +#if MAVLINK_COMMAND_24BIT +#define MAVLINK_MESSAGE_INFO_AUTOPILOT_STATE_FOR_GIMBAL_DEVICE { \ + 286, \ + "AUTOPILOT_STATE_FOR_GIMBAL_DEVICE", \ + 12, \ + { { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 50, offsetof(mavlink_autopilot_state_for_gimbal_device_t, target_system) }, \ + { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 51, offsetof(mavlink_autopilot_state_for_gimbal_device_t, target_component) }, \ + { "time_boot_us", NULL, MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_autopilot_state_for_gimbal_device_t, time_boot_us) }, \ + { "q", NULL, MAVLINK_TYPE_FLOAT, 4, 8, offsetof(mavlink_autopilot_state_for_gimbal_device_t, q) }, \ + { "q_estimated_delay_us", NULL, MAVLINK_TYPE_UINT32_T, 0, 24, offsetof(mavlink_autopilot_state_for_gimbal_device_t, q_estimated_delay_us) }, \ + { "vx", NULL, MAVLINK_TYPE_FLOAT, 0, 28, offsetof(mavlink_autopilot_state_for_gimbal_device_t, vx) }, \ + { "vy", NULL, MAVLINK_TYPE_FLOAT, 0, 32, offsetof(mavlink_autopilot_state_for_gimbal_device_t, vy) }, \ + { "vz", NULL, MAVLINK_TYPE_FLOAT, 0, 36, offsetof(mavlink_autopilot_state_for_gimbal_device_t, vz) }, \ + { "v_estimated_delay_us", NULL, MAVLINK_TYPE_UINT32_T, 0, 40, offsetof(mavlink_autopilot_state_for_gimbal_device_t, v_estimated_delay_us) }, \ + { "feed_forward_angular_velocity_z", NULL, MAVLINK_TYPE_FLOAT, 0, 44, offsetof(mavlink_autopilot_state_for_gimbal_device_t, feed_forward_angular_velocity_z) }, \ + { "estimator_status", NULL, MAVLINK_TYPE_UINT16_T, 0, 48, offsetof(mavlink_autopilot_state_for_gimbal_device_t, estimator_status) }, \ + { "landed_state", NULL, MAVLINK_TYPE_UINT8_T, 0, 52, offsetof(mavlink_autopilot_state_for_gimbal_device_t, landed_state) }, \ + } \ +} +#else +#define MAVLINK_MESSAGE_INFO_AUTOPILOT_STATE_FOR_GIMBAL_DEVICE { \ + "AUTOPILOT_STATE_FOR_GIMBAL_DEVICE", \ + 12, \ + { { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 50, offsetof(mavlink_autopilot_state_for_gimbal_device_t, target_system) }, \ + { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 51, offsetof(mavlink_autopilot_state_for_gimbal_device_t, target_component) }, \ + { "time_boot_us", NULL, MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_autopilot_state_for_gimbal_device_t, time_boot_us) }, \ + { "q", NULL, MAVLINK_TYPE_FLOAT, 4, 8, offsetof(mavlink_autopilot_state_for_gimbal_device_t, q) }, \ + { "q_estimated_delay_us", NULL, MAVLINK_TYPE_UINT32_T, 0, 24, offsetof(mavlink_autopilot_state_for_gimbal_device_t, q_estimated_delay_us) }, \ + { "vx", NULL, MAVLINK_TYPE_FLOAT, 0, 28, offsetof(mavlink_autopilot_state_for_gimbal_device_t, vx) }, \ + { "vy", NULL, MAVLINK_TYPE_FLOAT, 0, 32, offsetof(mavlink_autopilot_state_for_gimbal_device_t, vy) }, \ + { "vz", NULL, MAVLINK_TYPE_FLOAT, 0, 36, offsetof(mavlink_autopilot_state_for_gimbal_device_t, vz) }, \ + { "v_estimated_delay_us", NULL, MAVLINK_TYPE_UINT32_T, 0, 40, offsetof(mavlink_autopilot_state_for_gimbal_device_t, v_estimated_delay_us) }, \ + { "feed_forward_angular_velocity_z", NULL, MAVLINK_TYPE_FLOAT, 0, 44, offsetof(mavlink_autopilot_state_for_gimbal_device_t, feed_forward_angular_velocity_z) }, \ + { "estimator_status", NULL, MAVLINK_TYPE_UINT16_T, 0, 48, offsetof(mavlink_autopilot_state_for_gimbal_device_t, estimator_status) }, \ + { "landed_state", NULL, MAVLINK_TYPE_UINT8_T, 0, 52, offsetof(mavlink_autopilot_state_for_gimbal_device_t, landed_state) }, \ + } \ +} +#endif + +/** + * @brief Pack a autopilot_state_for_gimbal_device message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param target_system System ID + * @param target_component Component ID + * @param time_boot_us [us] Timestamp (time since system boot). + * @param q Quaternion components of autopilot attitude: w, x, y, z (1 0 0 0 is the null-rotation, Hamilton convention). + * @param q_estimated_delay_us [us] Estimated delay of the attitude data. + * @param vx [m/s] X Speed in NED (North, East, Down). + * @param vy [m/s] Y Speed in NED (North, East, Down). + * @param vz [m/s] Z Speed in NED (North, East, Down). + * @param v_estimated_delay_us [us] Estimated delay of the speed data. + * @param feed_forward_angular_velocity_z [rad/s] Feed forward Z component of angular velocity, positive is yawing to the right, NaN to be ignored. This is to indicate if the autopilot is actively yawing. + * @param estimator_status Bitmap indicating which estimator outputs are valid. + * @param landed_state The landed state. Is set to MAV_LANDED_STATE_UNDEFINED if landed state is unknown. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_autopilot_state_for_gimbal_device_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint8_t target_system, uint8_t target_component, uint64_t time_boot_us, const float *q, uint32_t q_estimated_delay_us, float vx, float vy, float vz, uint32_t v_estimated_delay_us, float feed_forward_angular_velocity_z, uint16_t estimator_status, uint8_t landed_state) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_AUTOPILOT_STATE_FOR_GIMBAL_DEVICE_LEN]; + _mav_put_uint64_t(buf, 0, time_boot_us); + _mav_put_uint32_t(buf, 24, q_estimated_delay_us); + _mav_put_float(buf, 28, vx); + _mav_put_float(buf, 32, vy); + _mav_put_float(buf, 36, vz); + _mav_put_uint32_t(buf, 40, v_estimated_delay_us); + _mav_put_float(buf, 44, feed_forward_angular_velocity_z); + _mav_put_uint16_t(buf, 48, estimator_status); + _mav_put_uint8_t(buf, 50, target_system); + _mav_put_uint8_t(buf, 51, target_component); + _mav_put_uint8_t(buf, 52, landed_state); + _mav_put_float_array(buf, 8, q, 4); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_AUTOPILOT_STATE_FOR_GIMBAL_DEVICE_LEN); +#else + mavlink_autopilot_state_for_gimbal_device_t packet; + packet.time_boot_us = time_boot_us; + packet.q_estimated_delay_us = q_estimated_delay_us; + packet.vx = vx; + packet.vy = vy; + packet.vz = vz; + packet.v_estimated_delay_us = v_estimated_delay_us; + packet.feed_forward_angular_velocity_z = feed_forward_angular_velocity_z; + packet.estimator_status = estimator_status; + packet.target_system = target_system; + packet.target_component = target_component; + packet.landed_state = landed_state; + mav_array_memcpy(packet.q, q, sizeof(float)*4); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_AUTOPILOT_STATE_FOR_GIMBAL_DEVICE_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_AUTOPILOT_STATE_FOR_GIMBAL_DEVICE; + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_AUTOPILOT_STATE_FOR_GIMBAL_DEVICE_MIN_LEN, MAVLINK_MSG_ID_AUTOPILOT_STATE_FOR_GIMBAL_DEVICE_LEN, MAVLINK_MSG_ID_AUTOPILOT_STATE_FOR_GIMBAL_DEVICE_CRC); +} + +/** + * @brief Pack a autopilot_state_for_gimbal_device message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param target_system System ID + * @param target_component Component ID + * @param time_boot_us [us] Timestamp (time since system boot). + * @param q Quaternion components of autopilot attitude: w, x, y, z (1 0 0 0 is the null-rotation, Hamilton convention). + * @param q_estimated_delay_us [us] Estimated delay of the attitude data. + * @param vx [m/s] X Speed in NED (North, East, Down). + * @param vy [m/s] Y Speed in NED (North, East, Down). + * @param vz [m/s] Z Speed in NED (North, East, Down). + * @param v_estimated_delay_us [us] Estimated delay of the speed data. + * @param feed_forward_angular_velocity_z [rad/s] Feed forward Z component of angular velocity, positive is yawing to the right, NaN to be ignored. This is to indicate if the autopilot is actively yawing. + * @param estimator_status Bitmap indicating which estimator outputs are valid. + * @param landed_state The landed state. Is set to MAV_LANDED_STATE_UNDEFINED if landed state is unknown. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_autopilot_state_for_gimbal_device_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint8_t target_system,uint8_t target_component,uint64_t time_boot_us,const float *q,uint32_t q_estimated_delay_us,float vx,float vy,float vz,uint32_t v_estimated_delay_us,float feed_forward_angular_velocity_z,uint16_t estimator_status,uint8_t landed_state) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_AUTOPILOT_STATE_FOR_GIMBAL_DEVICE_LEN]; + _mav_put_uint64_t(buf, 0, time_boot_us); + _mav_put_uint32_t(buf, 24, q_estimated_delay_us); + _mav_put_float(buf, 28, vx); + _mav_put_float(buf, 32, vy); + _mav_put_float(buf, 36, vz); + _mav_put_uint32_t(buf, 40, v_estimated_delay_us); + _mav_put_float(buf, 44, feed_forward_angular_velocity_z); + _mav_put_uint16_t(buf, 48, estimator_status); + _mav_put_uint8_t(buf, 50, target_system); + _mav_put_uint8_t(buf, 51, target_component); + _mav_put_uint8_t(buf, 52, landed_state); + _mav_put_float_array(buf, 8, q, 4); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_AUTOPILOT_STATE_FOR_GIMBAL_DEVICE_LEN); +#else + mavlink_autopilot_state_for_gimbal_device_t packet; + packet.time_boot_us = time_boot_us; + packet.q_estimated_delay_us = q_estimated_delay_us; + packet.vx = vx; + packet.vy = vy; + packet.vz = vz; + packet.v_estimated_delay_us = v_estimated_delay_us; + packet.feed_forward_angular_velocity_z = feed_forward_angular_velocity_z; + packet.estimator_status = estimator_status; + packet.target_system = target_system; + packet.target_component = target_component; + packet.landed_state = landed_state; + mav_array_memcpy(packet.q, q, sizeof(float)*4); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_AUTOPILOT_STATE_FOR_GIMBAL_DEVICE_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_AUTOPILOT_STATE_FOR_GIMBAL_DEVICE; + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_AUTOPILOT_STATE_FOR_GIMBAL_DEVICE_MIN_LEN, MAVLINK_MSG_ID_AUTOPILOT_STATE_FOR_GIMBAL_DEVICE_LEN, MAVLINK_MSG_ID_AUTOPILOT_STATE_FOR_GIMBAL_DEVICE_CRC); +} + +/** + * @brief Encode a autopilot_state_for_gimbal_device struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param autopilot_state_for_gimbal_device C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_autopilot_state_for_gimbal_device_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_autopilot_state_for_gimbal_device_t* autopilot_state_for_gimbal_device) +{ + return mavlink_msg_autopilot_state_for_gimbal_device_pack(system_id, component_id, msg, autopilot_state_for_gimbal_device->target_system, autopilot_state_for_gimbal_device->target_component, autopilot_state_for_gimbal_device->time_boot_us, autopilot_state_for_gimbal_device->q, autopilot_state_for_gimbal_device->q_estimated_delay_us, autopilot_state_for_gimbal_device->vx, autopilot_state_for_gimbal_device->vy, autopilot_state_for_gimbal_device->vz, autopilot_state_for_gimbal_device->v_estimated_delay_us, autopilot_state_for_gimbal_device->feed_forward_angular_velocity_z, autopilot_state_for_gimbal_device->estimator_status, autopilot_state_for_gimbal_device->landed_state); +} + +/** + * @brief Encode a autopilot_state_for_gimbal_device struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param autopilot_state_for_gimbal_device C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_autopilot_state_for_gimbal_device_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_autopilot_state_for_gimbal_device_t* autopilot_state_for_gimbal_device) +{ + return mavlink_msg_autopilot_state_for_gimbal_device_pack_chan(system_id, component_id, chan, msg, autopilot_state_for_gimbal_device->target_system, autopilot_state_for_gimbal_device->target_component, autopilot_state_for_gimbal_device->time_boot_us, autopilot_state_for_gimbal_device->q, autopilot_state_for_gimbal_device->q_estimated_delay_us, autopilot_state_for_gimbal_device->vx, autopilot_state_for_gimbal_device->vy, autopilot_state_for_gimbal_device->vz, autopilot_state_for_gimbal_device->v_estimated_delay_us, autopilot_state_for_gimbal_device->feed_forward_angular_velocity_z, autopilot_state_for_gimbal_device->estimator_status, autopilot_state_for_gimbal_device->landed_state); +} + +/** + * @brief Send a autopilot_state_for_gimbal_device message + * @param chan MAVLink channel to send the message + * + * @param target_system System ID + * @param target_component Component ID + * @param time_boot_us [us] Timestamp (time since system boot). + * @param q Quaternion components of autopilot attitude: w, x, y, z (1 0 0 0 is the null-rotation, Hamilton convention). + * @param q_estimated_delay_us [us] Estimated delay of the attitude data. + * @param vx [m/s] X Speed in NED (North, East, Down). + * @param vy [m/s] Y Speed in NED (North, East, Down). + * @param vz [m/s] Z Speed in NED (North, East, Down). + * @param v_estimated_delay_us [us] Estimated delay of the speed data. + * @param feed_forward_angular_velocity_z [rad/s] Feed forward Z component of angular velocity, positive is yawing to the right, NaN to be ignored. This is to indicate if the autopilot is actively yawing. + * @param estimator_status Bitmap indicating which estimator outputs are valid. + * @param landed_state The landed state. Is set to MAV_LANDED_STATE_UNDEFINED if landed state is unknown. + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_autopilot_state_for_gimbal_device_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint64_t time_boot_us, const float *q, uint32_t q_estimated_delay_us, float vx, float vy, float vz, uint32_t v_estimated_delay_us, float feed_forward_angular_velocity_z, uint16_t estimator_status, uint8_t landed_state) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_AUTOPILOT_STATE_FOR_GIMBAL_DEVICE_LEN]; + _mav_put_uint64_t(buf, 0, time_boot_us); + _mav_put_uint32_t(buf, 24, q_estimated_delay_us); + _mav_put_float(buf, 28, vx); + _mav_put_float(buf, 32, vy); + _mav_put_float(buf, 36, vz); + _mav_put_uint32_t(buf, 40, v_estimated_delay_us); + _mav_put_float(buf, 44, feed_forward_angular_velocity_z); + _mav_put_uint16_t(buf, 48, estimator_status); + _mav_put_uint8_t(buf, 50, target_system); + _mav_put_uint8_t(buf, 51, target_component); + _mav_put_uint8_t(buf, 52, landed_state); + _mav_put_float_array(buf, 8, q, 4); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AUTOPILOT_STATE_FOR_GIMBAL_DEVICE, buf, MAVLINK_MSG_ID_AUTOPILOT_STATE_FOR_GIMBAL_DEVICE_MIN_LEN, MAVLINK_MSG_ID_AUTOPILOT_STATE_FOR_GIMBAL_DEVICE_LEN, MAVLINK_MSG_ID_AUTOPILOT_STATE_FOR_GIMBAL_DEVICE_CRC); +#else + mavlink_autopilot_state_for_gimbal_device_t packet; + packet.time_boot_us = time_boot_us; + packet.q_estimated_delay_us = q_estimated_delay_us; + packet.vx = vx; + packet.vy = vy; + packet.vz = vz; + packet.v_estimated_delay_us = v_estimated_delay_us; + packet.feed_forward_angular_velocity_z = feed_forward_angular_velocity_z; + packet.estimator_status = estimator_status; + packet.target_system = target_system; + packet.target_component = target_component; + packet.landed_state = landed_state; + mav_array_memcpy(packet.q, q, sizeof(float)*4); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AUTOPILOT_STATE_FOR_GIMBAL_DEVICE, (const char *)&packet, MAVLINK_MSG_ID_AUTOPILOT_STATE_FOR_GIMBAL_DEVICE_MIN_LEN, MAVLINK_MSG_ID_AUTOPILOT_STATE_FOR_GIMBAL_DEVICE_LEN, MAVLINK_MSG_ID_AUTOPILOT_STATE_FOR_GIMBAL_DEVICE_CRC); +#endif +} + +/** + * @brief Send a autopilot_state_for_gimbal_device message + * @param chan MAVLink channel to send the message + * @param struct The MAVLink struct to serialize + */ +static inline void mavlink_msg_autopilot_state_for_gimbal_device_send_struct(mavlink_channel_t chan, const mavlink_autopilot_state_for_gimbal_device_t* autopilot_state_for_gimbal_device) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + mavlink_msg_autopilot_state_for_gimbal_device_send(chan, autopilot_state_for_gimbal_device->target_system, autopilot_state_for_gimbal_device->target_component, autopilot_state_for_gimbal_device->time_boot_us, autopilot_state_for_gimbal_device->q, autopilot_state_for_gimbal_device->q_estimated_delay_us, autopilot_state_for_gimbal_device->vx, autopilot_state_for_gimbal_device->vy, autopilot_state_for_gimbal_device->vz, autopilot_state_for_gimbal_device->v_estimated_delay_us, autopilot_state_for_gimbal_device->feed_forward_angular_velocity_z, autopilot_state_for_gimbal_device->estimator_status, autopilot_state_for_gimbal_device->landed_state); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AUTOPILOT_STATE_FOR_GIMBAL_DEVICE, (const char *)autopilot_state_for_gimbal_device, MAVLINK_MSG_ID_AUTOPILOT_STATE_FOR_GIMBAL_DEVICE_MIN_LEN, MAVLINK_MSG_ID_AUTOPILOT_STATE_FOR_GIMBAL_DEVICE_LEN, MAVLINK_MSG_ID_AUTOPILOT_STATE_FOR_GIMBAL_DEVICE_CRC); +#endif +} + +#if MAVLINK_MSG_ID_AUTOPILOT_STATE_FOR_GIMBAL_DEVICE_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_autopilot_state_for_gimbal_device_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint64_t time_boot_us, const float *q, uint32_t q_estimated_delay_us, float vx, float vy, float vz, uint32_t v_estimated_delay_us, float feed_forward_angular_velocity_z, uint16_t estimator_status, uint8_t landed_state) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint64_t(buf, 0, time_boot_us); + _mav_put_uint32_t(buf, 24, q_estimated_delay_us); + _mav_put_float(buf, 28, vx); + _mav_put_float(buf, 32, vy); + _mav_put_float(buf, 36, vz); + _mav_put_uint32_t(buf, 40, v_estimated_delay_us); + _mav_put_float(buf, 44, feed_forward_angular_velocity_z); + _mav_put_uint16_t(buf, 48, estimator_status); + _mav_put_uint8_t(buf, 50, target_system); + _mav_put_uint8_t(buf, 51, target_component); + _mav_put_uint8_t(buf, 52, landed_state); + _mav_put_float_array(buf, 8, q, 4); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AUTOPILOT_STATE_FOR_GIMBAL_DEVICE, buf, MAVLINK_MSG_ID_AUTOPILOT_STATE_FOR_GIMBAL_DEVICE_MIN_LEN, MAVLINK_MSG_ID_AUTOPILOT_STATE_FOR_GIMBAL_DEVICE_LEN, MAVLINK_MSG_ID_AUTOPILOT_STATE_FOR_GIMBAL_DEVICE_CRC); +#else + mavlink_autopilot_state_for_gimbal_device_t *packet = (mavlink_autopilot_state_for_gimbal_device_t *)msgbuf; + packet->time_boot_us = time_boot_us; + packet->q_estimated_delay_us = q_estimated_delay_us; + packet->vx = vx; + packet->vy = vy; + packet->vz = vz; + packet->v_estimated_delay_us = v_estimated_delay_us; + packet->feed_forward_angular_velocity_z = feed_forward_angular_velocity_z; + packet->estimator_status = estimator_status; + packet->target_system = target_system; + packet->target_component = target_component; + packet->landed_state = landed_state; + mav_array_memcpy(packet->q, q, sizeof(float)*4); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AUTOPILOT_STATE_FOR_GIMBAL_DEVICE, (const char *)packet, MAVLINK_MSG_ID_AUTOPILOT_STATE_FOR_GIMBAL_DEVICE_MIN_LEN, MAVLINK_MSG_ID_AUTOPILOT_STATE_FOR_GIMBAL_DEVICE_LEN, MAVLINK_MSG_ID_AUTOPILOT_STATE_FOR_GIMBAL_DEVICE_CRC); +#endif +} +#endif + +#endif + +// MESSAGE AUTOPILOT_STATE_FOR_GIMBAL_DEVICE UNPACKING + + +/** + * @brief Get field target_system from autopilot_state_for_gimbal_device message + * + * @return System ID + */ +static inline uint8_t mavlink_msg_autopilot_state_for_gimbal_device_get_target_system(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 50); +} + +/** + * @brief Get field target_component from autopilot_state_for_gimbal_device message + * + * @return Component ID + */ +static inline uint8_t mavlink_msg_autopilot_state_for_gimbal_device_get_target_component(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 51); +} + +/** + * @brief Get field time_boot_us from autopilot_state_for_gimbal_device message + * + * @return [us] Timestamp (time since system boot). + */ +static inline uint64_t mavlink_msg_autopilot_state_for_gimbal_device_get_time_boot_us(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint64_t(msg, 0); +} + +/** + * @brief Get field q from autopilot_state_for_gimbal_device message + * + * @return Quaternion components of autopilot attitude: w, x, y, z (1 0 0 0 is the null-rotation, Hamilton convention). + */ +static inline uint16_t mavlink_msg_autopilot_state_for_gimbal_device_get_q(const mavlink_message_t* msg, float *q) +{ + return _MAV_RETURN_float_array(msg, q, 4, 8); +} + +/** + * @brief Get field q_estimated_delay_us from autopilot_state_for_gimbal_device message + * + * @return [us] Estimated delay of the attitude data. + */ +static inline uint32_t mavlink_msg_autopilot_state_for_gimbal_device_get_q_estimated_delay_us(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint32_t(msg, 24); +} + +/** + * @brief Get field vx from autopilot_state_for_gimbal_device message + * + * @return [m/s] X Speed in NED (North, East, Down). + */ +static inline float mavlink_msg_autopilot_state_for_gimbal_device_get_vx(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 28); +} + +/** + * @brief Get field vy from autopilot_state_for_gimbal_device message + * + * @return [m/s] Y Speed in NED (North, East, Down). + */ +static inline float mavlink_msg_autopilot_state_for_gimbal_device_get_vy(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 32); +} + +/** + * @brief Get field vz from autopilot_state_for_gimbal_device message + * + * @return [m/s] Z Speed in NED (North, East, Down). + */ +static inline float mavlink_msg_autopilot_state_for_gimbal_device_get_vz(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 36); +} + +/** + * @brief Get field v_estimated_delay_us from autopilot_state_for_gimbal_device message + * + * @return [us] Estimated delay of the speed data. + */ +static inline uint32_t mavlink_msg_autopilot_state_for_gimbal_device_get_v_estimated_delay_us(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint32_t(msg, 40); +} + +/** + * @brief Get field feed_forward_angular_velocity_z from autopilot_state_for_gimbal_device message + * + * @return [rad/s] Feed forward Z component of angular velocity, positive is yawing to the right, NaN to be ignored. This is to indicate if the autopilot is actively yawing. + */ +static inline float mavlink_msg_autopilot_state_for_gimbal_device_get_feed_forward_angular_velocity_z(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 44); +} + +/** + * @brief Get field estimator_status from autopilot_state_for_gimbal_device message + * + * @return Bitmap indicating which estimator outputs are valid. + */ +static inline uint16_t mavlink_msg_autopilot_state_for_gimbal_device_get_estimator_status(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 48); +} + +/** + * @brief Get field landed_state from autopilot_state_for_gimbal_device message + * + * @return The landed state. Is set to MAV_LANDED_STATE_UNDEFINED if landed state is unknown. + */ +static inline uint8_t mavlink_msg_autopilot_state_for_gimbal_device_get_landed_state(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 52); +} + +/** + * @brief Decode a autopilot_state_for_gimbal_device message into a struct + * + * @param msg The message to decode + * @param autopilot_state_for_gimbal_device C-struct to decode the message contents into + */ +static inline void mavlink_msg_autopilot_state_for_gimbal_device_decode(const mavlink_message_t* msg, mavlink_autopilot_state_for_gimbal_device_t* autopilot_state_for_gimbal_device) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + autopilot_state_for_gimbal_device->time_boot_us = mavlink_msg_autopilot_state_for_gimbal_device_get_time_boot_us(msg); + mavlink_msg_autopilot_state_for_gimbal_device_get_q(msg, autopilot_state_for_gimbal_device->q); + autopilot_state_for_gimbal_device->q_estimated_delay_us = mavlink_msg_autopilot_state_for_gimbal_device_get_q_estimated_delay_us(msg); + autopilot_state_for_gimbal_device->vx = mavlink_msg_autopilot_state_for_gimbal_device_get_vx(msg); + autopilot_state_for_gimbal_device->vy = mavlink_msg_autopilot_state_for_gimbal_device_get_vy(msg); + autopilot_state_for_gimbal_device->vz = mavlink_msg_autopilot_state_for_gimbal_device_get_vz(msg); + autopilot_state_for_gimbal_device->v_estimated_delay_us = mavlink_msg_autopilot_state_for_gimbal_device_get_v_estimated_delay_us(msg); + autopilot_state_for_gimbal_device->feed_forward_angular_velocity_z = mavlink_msg_autopilot_state_for_gimbal_device_get_feed_forward_angular_velocity_z(msg); + autopilot_state_for_gimbal_device->estimator_status = mavlink_msg_autopilot_state_for_gimbal_device_get_estimator_status(msg); + autopilot_state_for_gimbal_device->target_system = mavlink_msg_autopilot_state_for_gimbal_device_get_target_system(msg); + autopilot_state_for_gimbal_device->target_component = mavlink_msg_autopilot_state_for_gimbal_device_get_target_component(msg); + autopilot_state_for_gimbal_device->landed_state = mavlink_msg_autopilot_state_for_gimbal_device_get_landed_state(msg); +#else + uint8_t len = msg->len < MAVLINK_MSG_ID_AUTOPILOT_STATE_FOR_GIMBAL_DEVICE_LEN? msg->len : MAVLINK_MSG_ID_AUTOPILOT_STATE_FOR_GIMBAL_DEVICE_LEN; + memset(autopilot_state_for_gimbal_device, 0, MAVLINK_MSG_ID_AUTOPILOT_STATE_FOR_GIMBAL_DEVICE_LEN); + memcpy(autopilot_state_for_gimbal_device, _MAV_PAYLOAD(msg), len); +#endif +} diff --git a/lib/main/MAVLink/common/mavlink_msg_autopilot_version.h b/lib/main/MAVLink/common/mavlink_msg_autopilot_version.h index ae0324b75b..37b87be38f 100755 --- a/lib/main/MAVLink/common/mavlink_msg_autopilot_version.h +++ b/lib/main/MAVLink/common/mavlink_msg_autopilot_version.h @@ -16,11 +16,12 @@ typedef struct __mavlink_autopilot_version_t { uint8_t flight_custom_version[8]; /*< Custom version field, commonly the first 8 bytes of the git hash. This is not an unique identifier, but should allow to identify the commit using the main version number even for very large code bases.*/ uint8_t middleware_custom_version[8]; /*< Custom version field, commonly the first 8 bytes of the git hash. This is not an unique identifier, but should allow to identify the commit using the main version number even for very large code bases.*/ uint8_t os_custom_version[8]; /*< Custom version field, commonly the first 8 bytes of the git hash. This is not an unique identifier, but should allow to identify the commit using the main version number even for very large code bases.*/ + uint8_t uid2[18]; /*< UID if provided by hardware (supersedes the uid field. If this is non-zero, use this field, otherwise use uid)*/ } mavlink_autopilot_version_t; -#define MAVLINK_MSG_ID_AUTOPILOT_VERSION_LEN 60 +#define MAVLINK_MSG_ID_AUTOPILOT_VERSION_LEN 78 #define MAVLINK_MSG_ID_AUTOPILOT_VERSION_MIN_LEN 60 -#define MAVLINK_MSG_ID_148_LEN 60 +#define MAVLINK_MSG_ID_148_LEN 78 #define MAVLINK_MSG_ID_148_MIN_LEN 60 #define MAVLINK_MSG_ID_AUTOPILOT_VERSION_CRC 178 @@ -29,12 +30,13 @@ typedef struct __mavlink_autopilot_version_t { #define MAVLINK_MSG_AUTOPILOT_VERSION_FIELD_FLIGHT_CUSTOM_VERSION_LEN 8 #define MAVLINK_MSG_AUTOPILOT_VERSION_FIELD_MIDDLEWARE_CUSTOM_VERSION_LEN 8 #define MAVLINK_MSG_AUTOPILOT_VERSION_FIELD_OS_CUSTOM_VERSION_LEN 8 +#define MAVLINK_MSG_AUTOPILOT_VERSION_FIELD_UID2_LEN 18 #if MAVLINK_COMMAND_24BIT #define MAVLINK_MESSAGE_INFO_AUTOPILOT_VERSION { \ 148, \ "AUTOPILOT_VERSION", \ - 11, \ + 12, \ { { "capabilities", NULL, MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_autopilot_version_t, capabilities) }, \ { "flight_sw_version", NULL, MAVLINK_TYPE_UINT32_T, 0, 16, offsetof(mavlink_autopilot_version_t, flight_sw_version) }, \ { "middleware_sw_version", NULL, MAVLINK_TYPE_UINT32_T, 0, 20, offsetof(mavlink_autopilot_version_t, middleware_sw_version) }, \ @@ -46,12 +48,13 @@ typedef struct __mavlink_autopilot_version_t { { "vendor_id", NULL, MAVLINK_TYPE_UINT16_T, 0, 32, offsetof(mavlink_autopilot_version_t, vendor_id) }, \ { "product_id", NULL, MAVLINK_TYPE_UINT16_T, 0, 34, offsetof(mavlink_autopilot_version_t, product_id) }, \ { "uid", NULL, MAVLINK_TYPE_UINT64_T, 0, 8, offsetof(mavlink_autopilot_version_t, uid) }, \ + { "uid2", NULL, MAVLINK_TYPE_UINT8_T, 18, 60, offsetof(mavlink_autopilot_version_t, uid2) }, \ } \ } #else #define MAVLINK_MESSAGE_INFO_AUTOPILOT_VERSION { \ "AUTOPILOT_VERSION", \ - 11, \ + 12, \ { { "capabilities", NULL, MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_autopilot_version_t, capabilities) }, \ { "flight_sw_version", NULL, MAVLINK_TYPE_UINT32_T, 0, 16, offsetof(mavlink_autopilot_version_t, flight_sw_version) }, \ { "middleware_sw_version", NULL, MAVLINK_TYPE_UINT32_T, 0, 20, offsetof(mavlink_autopilot_version_t, middleware_sw_version) }, \ @@ -63,6 +66,7 @@ typedef struct __mavlink_autopilot_version_t { { "vendor_id", NULL, MAVLINK_TYPE_UINT16_T, 0, 32, offsetof(mavlink_autopilot_version_t, vendor_id) }, \ { "product_id", NULL, MAVLINK_TYPE_UINT16_T, 0, 34, offsetof(mavlink_autopilot_version_t, product_id) }, \ { "uid", NULL, MAVLINK_TYPE_UINT64_T, 0, 8, offsetof(mavlink_autopilot_version_t, uid) }, \ + { "uid2", NULL, MAVLINK_TYPE_UINT8_T, 18, 60, offsetof(mavlink_autopilot_version_t, uid2) }, \ } \ } #endif @@ -84,10 +88,11 @@ typedef struct __mavlink_autopilot_version_t { * @param vendor_id ID of the board vendor * @param product_id ID of the product * @param uid UID if provided by hardware (see uid2) + * @param uid2 UID if provided by hardware (supersedes the uid field. If this is non-zero, use this field, otherwise use uid) * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_autopilot_version_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint64_t capabilities, uint32_t flight_sw_version, uint32_t middleware_sw_version, uint32_t os_sw_version, uint32_t board_version, const uint8_t *flight_custom_version, const uint8_t *middleware_custom_version, const uint8_t *os_custom_version, uint16_t vendor_id, uint16_t product_id, uint64_t uid) + uint64_t capabilities, uint32_t flight_sw_version, uint32_t middleware_sw_version, uint32_t os_sw_version, uint32_t board_version, const uint8_t *flight_custom_version, const uint8_t *middleware_custom_version, const uint8_t *os_custom_version, uint16_t vendor_id, uint16_t product_id, uint64_t uid, const uint8_t *uid2) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_AUTOPILOT_VERSION_LEN]; @@ -102,6 +107,7 @@ static inline uint16_t mavlink_msg_autopilot_version_pack(uint8_t system_id, uin _mav_put_uint8_t_array(buf, 36, flight_custom_version, 8); _mav_put_uint8_t_array(buf, 44, middleware_custom_version, 8); _mav_put_uint8_t_array(buf, 52, os_custom_version, 8); + _mav_put_uint8_t_array(buf, 60, uid2, 18); memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_AUTOPILOT_VERSION_LEN); #else mavlink_autopilot_version_t packet; @@ -116,6 +122,7 @@ static inline uint16_t mavlink_msg_autopilot_version_pack(uint8_t system_id, uin mav_array_memcpy(packet.flight_custom_version, flight_custom_version, sizeof(uint8_t)*8); mav_array_memcpy(packet.middleware_custom_version, middleware_custom_version, sizeof(uint8_t)*8); mav_array_memcpy(packet.os_custom_version, os_custom_version, sizeof(uint8_t)*8); + mav_array_memcpy(packet.uid2, uid2, sizeof(uint8_t)*18); memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_AUTOPILOT_VERSION_LEN); #endif @@ -140,11 +147,12 @@ static inline uint16_t mavlink_msg_autopilot_version_pack(uint8_t system_id, uin * @param vendor_id ID of the board vendor * @param product_id ID of the product * @param uid UID if provided by hardware (see uid2) + * @param uid2 UID if provided by hardware (supersedes the uid field. If this is non-zero, use this field, otherwise use uid) * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_autopilot_version_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, - uint64_t capabilities,uint32_t flight_sw_version,uint32_t middleware_sw_version,uint32_t os_sw_version,uint32_t board_version,const uint8_t *flight_custom_version,const uint8_t *middleware_custom_version,const uint8_t *os_custom_version,uint16_t vendor_id,uint16_t product_id,uint64_t uid) + uint64_t capabilities,uint32_t flight_sw_version,uint32_t middleware_sw_version,uint32_t os_sw_version,uint32_t board_version,const uint8_t *flight_custom_version,const uint8_t *middleware_custom_version,const uint8_t *os_custom_version,uint16_t vendor_id,uint16_t product_id,uint64_t uid,const uint8_t *uid2) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_AUTOPILOT_VERSION_LEN]; @@ -159,6 +167,7 @@ static inline uint16_t mavlink_msg_autopilot_version_pack_chan(uint8_t system_id _mav_put_uint8_t_array(buf, 36, flight_custom_version, 8); _mav_put_uint8_t_array(buf, 44, middleware_custom_version, 8); _mav_put_uint8_t_array(buf, 52, os_custom_version, 8); + _mav_put_uint8_t_array(buf, 60, uid2, 18); memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_AUTOPILOT_VERSION_LEN); #else mavlink_autopilot_version_t packet; @@ -173,6 +182,7 @@ static inline uint16_t mavlink_msg_autopilot_version_pack_chan(uint8_t system_id mav_array_memcpy(packet.flight_custom_version, flight_custom_version, sizeof(uint8_t)*8); mav_array_memcpy(packet.middleware_custom_version, middleware_custom_version, sizeof(uint8_t)*8); mav_array_memcpy(packet.os_custom_version, os_custom_version, sizeof(uint8_t)*8); + mav_array_memcpy(packet.uid2, uid2, sizeof(uint8_t)*18); memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_AUTOPILOT_VERSION_LEN); #endif @@ -190,7 +200,7 @@ static inline uint16_t mavlink_msg_autopilot_version_pack_chan(uint8_t system_id */ static inline uint16_t mavlink_msg_autopilot_version_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_autopilot_version_t* autopilot_version) { - return mavlink_msg_autopilot_version_pack(system_id, component_id, msg, autopilot_version->capabilities, autopilot_version->flight_sw_version, autopilot_version->middleware_sw_version, autopilot_version->os_sw_version, autopilot_version->board_version, autopilot_version->flight_custom_version, autopilot_version->middleware_custom_version, autopilot_version->os_custom_version, autopilot_version->vendor_id, autopilot_version->product_id, autopilot_version->uid); + return mavlink_msg_autopilot_version_pack(system_id, component_id, msg, autopilot_version->capabilities, autopilot_version->flight_sw_version, autopilot_version->middleware_sw_version, autopilot_version->os_sw_version, autopilot_version->board_version, autopilot_version->flight_custom_version, autopilot_version->middleware_custom_version, autopilot_version->os_custom_version, autopilot_version->vendor_id, autopilot_version->product_id, autopilot_version->uid, autopilot_version->uid2); } /** @@ -204,7 +214,7 @@ static inline uint16_t mavlink_msg_autopilot_version_encode(uint8_t system_id, u */ static inline uint16_t mavlink_msg_autopilot_version_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_autopilot_version_t* autopilot_version) { - return mavlink_msg_autopilot_version_pack_chan(system_id, component_id, chan, msg, autopilot_version->capabilities, autopilot_version->flight_sw_version, autopilot_version->middleware_sw_version, autopilot_version->os_sw_version, autopilot_version->board_version, autopilot_version->flight_custom_version, autopilot_version->middleware_custom_version, autopilot_version->os_custom_version, autopilot_version->vendor_id, autopilot_version->product_id, autopilot_version->uid); + return mavlink_msg_autopilot_version_pack_chan(system_id, component_id, chan, msg, autopilot_version->capabilities, autopilot_version->flight_sw_version, autopilot_version->middleware_sw_version, autopilot_version->os_sw_version, autopilot_version->board_version, autopilot_version->flight_custom_version, autopilot_version->middleware_custom_version, autopilot_version->os_custom_version, autopilot_version->vendor_id, autopilot_version->product_id, autopilot_version->uid, autopilot_version->uid2); } /** @@ -222,10 +232,11 @@ static inline uint16_t mavlink_msg_autopilot_version_encode_chan(uint8_t system_ * @param vendor_id ID of the board vendor * @param product_id ID of the product * @param uid UID if provided by hardware (see uid2) + * @param uid2 UID if provided by hardware (supersedes the uid field. If this is non-zero, use this field, otherwise use uid) */ #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS -static inline void mavlink_msg_autopilot_version_send(mavlink_channel_t chan, uint64_t capabilities, uint32_t flight_sw_version, uint32_t middleware_sw_version, uint32_t os_sw_version, uint32_t board_version, const uint8_t *flight_custom_version, const uint8_t *middleware_custom_version, const uint8_t *os_custom_version, uint16_t vendor_id, uint16_t product_id, uint64_t uid) +static inline void mavlink_msg_autopilot_version_send(mavlink_channel_t chan, uint64_t capabilities, uint32_t flight_sw_version, uint32_t middleware_sw_version, uint32_t os_sw_version, uint32_t board_version, const uint8_t *flight_custom_version, const uint8_t *middleware_custom_version, const uint8_t *os_custom_version, uint16_t vendor_id, uint16_t product_id, uint64_t uid, const uint8_t *uid2) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_AUTOPILOT_VERSION_LEN]; @@ -240,6 +251,7 @@ static inline void mavlink_msg_autopilot_version_send(mavlink_channel_t chan, ui _mav_put_uint8_t_array(buf, 36, flight_custom_version, 8); _mav_put_uint8_t_array(buf, 44, middleware_custom_version, 8); _mav_put_uint8_t_array(buf, 52, os_custom_version, 8); + _mav_put_uint8_t_array(buf, 60, uid2, 18); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AUTOPILOT_VERSION, buf, MAVLINK_MSG_ID_AUTOPILOT_VERSION_MIN_LEN, MAVLINK_MSG_ID_AUTOPILOT_VERSION_LEN, MAVLINK_MSG_ID_AUTOPILOT_VERSION_CRC); #else mavlink_autopilot_version_t packet; @@ -254,6 +266,7 @@ static inline void mavlink_msg_autopilot_version_send(mavlink_channel_t chan, ui mav_array_memcpy(packet.flight_custom_version, flight_custom_version, sizeof(uint8_t)*8); mav_array_memcpy(packet.middleware_custom_version, middleware_custom_version, sizeof(uint8_t)*8); mav_array_memcpy(packet.os_custom_version, os_custom_version, sizeof(uint8_t)*8); + mav_array_memcpy(packet.uid2, uid2, sizeof(uint8_t)*18); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AUTOPILOT_VERSION, (const char *)&packet, MAVLINK_MSG_ID_AUTOPILOT_VERSION_MIN_LEN, MAVLINK_MSG_ID_AUTOPILOT_VERSION_LEN, MAVLINK_MSG_ID_AUTOPILOT_VERSION_CRC); #endif } @@ -266,7 +279,7 @@ static inline void mavlink_msg_autopilot_version_send(mavlink_channel_t chan, ui static inline void mavlink_msg_autopilot_version_send_struct(mavlink_channel_t chan, const mavlink_autopilot_version_t* autopilot_version) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - mavlink_msg_autopilot_version_send(chan, autopilot_version->capabilities, autopilot_version->flight_sw_version, autopilot_version->middleware_sw_version, autopilot_version->os_sw_version, autopilot_version->board_version, autopilot_version->flight_custom_version, autopilot_version->middleware_custom_version, autopilot_version->os_custom_version, autopilot_version->vendor_id, autopilot_version->product_id, autopilot_version->uid); + mavlink_msg_autopilot_version_send(chan, autopilot_version->capabilities, autopilot_version->flight_sw_version, autopilot_version->middleware_sw_version, autopilot_version->os_sw_version, autopilot_version->board_version, autopilot_version->flight_custom_version, autopilot_version->middleware_custom_version, autopilot_version->os_custom_version, autopilot_version->vendor_id, autopilot_version->product_id, autopilot_version->uid, autopilot_version->uid2); #else _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AUTOPILOT_VERSION, (const char *)autopilot_version, MAVLINK_MSG_ID_AUTOPILOT_VERSION_MIN_LEN, MAVLINK_MSG_ID_AUTOPILOT_VERSION_LEN, MAVLINK_MSG_ID_AUTOPILOT_VERSION_CRC); #endif @@ -280,7 +293,7 @@ static inline void mavlink_msg_autopilot_version_send_struct(mavlink_channel_t c is usually the receive buffer for the channel, and allows a reply to an incoming message with minimum stack space usage. */ -static inline void mavlink_msg_autopilot_version_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint64_t capabilities, uint32_t flight_sw_version, uint32_t middleware_sw_version, uint32_t os_sw_version, uint32_t board_version, const uint8_t *flight_custom_version, const uint8_t *middleware_custom_version, const uint8_t *os_custom_version, uint16_t vendor_id, uint16_t product_id, uint64_t uid) +static inline void mavlink_msg_autopilot_version_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint64_t capabilities, uint32_t flight_sw_version, uint32_t middleware_sw_version, uint32_t os_sw_version, uint32_t board_version, const uint8_t *flight_custom_version, const uint8_t *middleware_custom_version, const uint8_t *os_custom_version, uint16_t vendor_id, uint16_t product_id, uint64_t uid, const uint8_t *uid2) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char *buf = (char *)msgbuf; @@ -295,6 +308,7 @@ static inline void mavlink_msg_autopilot_version_send_buf(mavlink_message_t *msg _mav_put_uint8_t_array(buf, 36, flight_custom_version, 8); _mav_put_uint8_t_array(buf, 44, middleware_custom_version, 8); _mav_put_uint8_t_array(buf, 52, os_custom_version, 8); + _mav_put_uint8_t_array(buf, 60, uid2, 18); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AUTOPILOT_VERSION, buf, MAVLINK_MSG_ID_AUTOPILOT_VERSION_MIN_LEN, MAVLINK_MSG_ID_AUTOPILOT_VERSION_LEN, MAVLINK_MSG_ID_AUTOPILOT_VERSION_CRC); #else mavlink_autopilot_version_t *packet = (mavlink_autopilot_version_t *)msgbuf; @@ -309,6 +323,7 @@ static inline void mavlink_msg_autopilot_version_send_buf(mavlink_message_t *msg mav_array_memcpy(packet->flight_custom_version, flight_custom_version, sizeof(uint8_t)*8); mav_array_memcpy(packet->middleware_custom_version, middleware_custom_version, sizeof(uint8_t)*8); mav_array_memcpy(packet->os_custom_version, os_custom_version, sizeof(uint8_t)*8); + mav_array_memcpy(packet->uid2, uid2, sizeof(uint8_t)*18); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_AUTOPILOT_VERSION, (const char *)packet, MAVLINK_MSG_ID_AUTOPILOT_VERSION_MIN_LEN, MAVLINK_MSG_ID_AUTOPILOT_VERSION_LEN, MAVLINK_MSG_ID_AUTOPILOT_VERSION_CRC); #endif } @@ -429,6 +444,16 @@ static inline uint64_t mavlink_msg_autopilot_version_get_uid(const mavlink_messa return _MAV_RETURN_uint64_t(msg, 8); } +/** + * @brief Get field uid2 from autopilot_version message + * + * @return UID if provided by hardware (supersedes the uid field. If this is non-zero, use this field, otherwise use uid) + */ +static inline uint16_t mavlink_msg_autopilot_version_get_uid2(const mavlink_message_t* msg, uint8_t *uid2) +{ + return _MAV_RETURN_uint8_t_array(msg, uid2, 18, 60); +} + /** * @brief Decode a autopilot_version message into a struct * @@ -449,6 +474,7 @@ static inline void mavlink_msg_autopilot_version_decode(const mavlink_message_t* mavlink_msg_autopilot_version_get_flight_custom_version(msg, autopilot_version->flight_custom_version); mavlink_msg_autopilot_version_get_middleware_custom_version(msg, autopilot_version->middleware_custom_version); mavlink_msg_autopilot_version_get_os_custom_version(msg, autopilot_version->os_custom_version); + mavlink_msg_autopilot_version_get_uid2(msg, autopilot_version->uid2); #else uint8_t len = msg->len < MAVLINK_MSG_ID_AUTOPILOT_VERSION_LEN? msg->len : MAVLINK_MSG_ID_AUTOPILOT_VERSION_LEN; memset(autopilot_version, 0, MAVLINK_MSG_ID_AUTOPILOT_VERSION_LEN); diff --git a/lib/main/MAVLink/common/mavlink_msg_battery_status.h b/lib/main/MAVLink/common/mavlink_msg_battery_status.h index 9d5ac516b2..084b2a3886 100755 --- a/lib/main/MAVLink/common/mavlink_msg_battery_status.h +++ b/lib/main/MAVLink/common/mavlink_msg_battery_status.h @@ -3,7 +3,7 @@ #define MAVLINK_MSG_ID_BATTERY_STATUS 147 - +MAVPACKED( typedef struct __mavlink_battery_status_t { int32_t current_consumed; /*< [mAh] Consumed charge, -1: autopilot does not provide consumption estimate*/ int32_t energy_consumed; /*< [hJ] Consumed energy, -1: autopilot does not provide energy consumption estimate*/ @@ -14,23 +14,29 @@ typedef struct __mavlink_battery_status_t { uint8_t battery_function; /*< Function of the battery*/ uint8_t type; /*< Type (chemistry) of the battery*/ int8_t battery_remaining; /*< [%] Remaining battery energy. Values: [0-100], -1: autopilot does not estimate the remaining battery.*/ -} mavlink_battery_status_t; + int32_t time_remaining; /*< [s] Remaining battery time, 0: autopilot does not provide remaining battery time estimate*/ + uint8_t charge_state; /*< State for extent of discharge, provided by autopilot for warning or external reactions*/ + uint16_t voltages_ext[4]; /*< [mV] Battery voltages for cells 11 to 14. Cells above the valid cell count for this battery should have a value of 0, where zero indicates not supported (note, this is different than for the voltages field and allows empty byte truncation). If the measured value is 0 then 1 should be sent instead.*/ + uint8_t mode; /*< Battery mode. Default (0) is that battery mode reporting is not supported or battery is in normal-use mode.*/ + uint32_t fault_bitmask; /*< Fault/health indications. These should be set when charge_state is MAV_BATTERY_CHARGE_STATE_FAILED or MAV_BATTERY_CHARGE_STATE_UNHEALTHY (if not, fault reporting is not supported).*/ +}) mavlink_battery_status_t; -#define MAVLINK_MSG_ID_BATTERY_STATUS_LEN 36 +#define MAVLINK_MSG_ID_BATTERY_STATUS_LEN 54 #define MAVLINK_MSG_ID_BATTERY_STATUS_MIN_LEN 36 -#define MAVLINK_MSG_ID_147_LEN 36 +#define MAVLINK_MSG_ID_147_LEN 54 #define MAVLINK_MSG_ID_147_MIN_LEN 36 #define MAVLINK_MSG_ID_BATTERY_STATUS_CRC 154 #define MAVLINK_MSG_ID_147_CRC 154 #define MAVLINK_MSG_BATTERY_STATUS_FIELD_VOLTAGES_LEN 10 +#define MAVLINK_MSG_BATTERY_STATUS_FIELD_VOLTAGES_EXT_LEN 4 #if MAVLINK_COMMAND_24BIT #define MAVLINK_MESSAGE_INFO_BATTERY_STATUS { \ 147, \ "BATTERY_STATUS", \ - 9, \ + 14, \ { { "id", NULL, MAVLINK_TYPE_UINT8_T, 0, 32, offsetof(mavlink_battery_status_t, id) }, \ { "battery_function", NULL, MAVLINK_TYPE_UINT8_T, 0, 33, offsetof(mavlink_battery_status_t, battery_function) }, \ { "type", NULL, MAVLINK_TYPE_UINT8_T, 0, 34, offsetof(mavlink_battery_status_t, type) }, \ @@ -40,12 +46,17 @@ typedef struct __mavlink_battery_status_t { { "current_consumed", NULL, MAVLINK_TYPE_INT32_T, 0, 0, offsetof(mavlink_battery_status_t, current_consumed) }, \ { "energy_consumed", NULL, MAVLINK_TYPE_INT32_T, 0, 4, offsetof(mavlink_battery_status_t, energy_consumed) }, \ { "battery_remaining", NULL, MAVLINK_TYPE_INT8_T, 0, 35, offsetof(mavlink_battery_status_t, battery_remaining) }, \ + { "time_remaining", NULL, MAVLINK_TYPE_INT32_T, 0, 36, offsetof(mavlink_battery_status_t, time_remaining) }, \ + { "charge_state", NULL, MAVLINK_TYPE_UINT8_T, 0, 40, offsetof(mavlink_battery_status_t, charge_state) }, \ + { "voltages_ext", NULL, MAVLINK_TYPE_UINT16_T, 4, 41, offsetof(mavlink_battery_status_t, voltages_ext) }, \ + { "mode", NULL, MAVLINK_TYPE_UINT8_T, 0, 49, offsetof(mavlink_battery_status_t, mode) }, \ + { "fault_bitmask", NULL, MAVLINK_TYPE_UINT32_T, 0, 50, offsetof(mavlink_battery_status_t, fault_bitmask) }, \ } \ } #else #define MAVLINK_MESSAGE_INFO_BATTERY_STATUS { \ "BATTERY_STATUS", \ - 9, \ + 14, \ { { "id", NULL, MAVLINK_TYPE_UINT8_T, 0, 32, offsetof(mavlink_battery_status_t, id) }, \ { "battery_function", NULL, MAVLINK_TYPE_UINT8_T, 0, 33, offsetof(mavlink_battery_status_t, battery_function) }, \ { "type", NULL, MAVLINK_TYPE_UINT8_T, 0, 34, offsetof(mavlink_battery_status_t, type) }, \ @@ -55,6 +66,11 @@ typedef struct __mavlink_battery_status_t { { "current_consumed", NULL, MAVLINK_TYPE_INT32_T, 0, 0, offsetof(mavlink_battery_status_t, current_consumed) }, \ { "energy_consumed", NULL, MAVLINK_TYPE_INT32_T, 0, 4, offsetof(mavlink_battery_status_t, energy_consumed) }, \ { "battery_remaining", NULL, MAVLINK_TYPE_INT8_T, 0, 35, offsetof(mavlink_battery_status_t, battery_remaining) }, \ + { "time_remaining", NULL, MAVLINK_TYPE_INT32_T, 0, 36, offsetof(mavlink_battery_status_t, time_remaining) }, \ + { "charge_state", NULL, MAVLINK_TYPE_UINT8_T, 0, 40, offsetof(mavlink_battery_status_t, charge_state) }, \ + { "voltages_ext", NULL, MAVLINK_TYPE_UINT16_T, 4, 41, offsetof(mavlink_battery_status_t, voltages_ext) }, \ + { "mode", NULL, MAVLINK_TYPE_UINT8_T, 0, 49, offsetof(mavlink_battery_status_t, mode) }, \ + { "fault_bitmask", NULL, MAVLINK_TYPE_UINT32_T, 0, 50, offsetof(mavlink_battery_status_t, fault_bitmask) }, \ } \ } #endif @@ -74,10 +90,15 @@ typedef struct __mavlink_battery_status_t { * @param current_consumed [mAh] Consumed charge, -1: autopilot does not provide consumption estimate * @param energy_consumed [hJ] Consumed energy, -1: autopilot does not provide energy consumption estimate * @param battery_remaining [%] Remaining battery energy. Values: [0-100], -1: autopilot does not estimate the remaining battery. + * @param time_remaining [s] Remaining battery time, 0: autopilot does not provide remaining battery time estimate + * @param charge_state State for extent of discharge, provided by autopilot for warning or external reactions + * @param voltages_ext [mV] Battery voltages for cells 11 to 14. Cells above the valid cell count for this battery should have a value of 0, where zero indicates not supported (note, this is different than for the voltages field and allows empty byte truncation). If the measured value is 0 then 1 should be sent instead. + * @param mode Battery mode. Default (0) is that battery mode reporting is not supported or battery is in normal-use mode. + * @param fault_bitmask Fault/health indications. These should be set when charge_state is MAV_BATTERY_CHARGE_STATE_FAILED or MAV_BATTERY_CHARGE_STATE_UNHEALTHY (if not, fault reporting is not supported). * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_battery_status_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint8_t id, uint8_t battery_function, uint8_t type, int16_t temperature, const uint16_t *voltages, int16_t current_battery, int32_t current_consumed, int32_t energy_consumed, int8_t battery_remaining) + uint8_t id, uint8_t battery_function, uint8_t type, int16_t temperature, const uint16_t *voltages, int16_t current_battery, int32_t current_consumed, int32_t energy_consumed, int8_t battery_remaining, int32_t time_remaining, uint8_t charge_state, const uint16_t *voltages_ext, uint8_t mode, uint32_t fault_bitmask) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_BATTERY_STATUS_LEN]; @@ -89,7 +110,12 @@ static inline uint16_t mavlink_msg_battery_status_pack(uint8_t system_id, uint8_ _mav_put_uint8_t(buf, 33, battery_function); _mav_put_uint8_t(buf, 34, type); _mav_put_int8_t(buf, 35, battery_remaining); + _mav_put_int32_t(buf, 36, time_remaining); + _mav_put_uint8_t(buf, 40, charge_state); + _mav_put_uint8_t(buf, 49, mode); + _mav_put_uint32_t(buf, 50, fault_bitmask); _mav_put_uint16_t_array(buf, 10, voltages, 10); + _mav_put_uint16_t_array(buf, 41, voltages_ext, 4); memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_BATTERY_STATUS_LEN); #else mavlink_battery_status_t packet; @@ -101,7 +127,12 @@ static inline uint16_t mavlink_msg_battery_status_pack(uint8_t system_id, uint8_ packet.battery_function = battery_function; packet.type = type; packet.battery_remaining = battery_remaining; + packet.time_remaining = time_remaining; + packet.charge_state = charge_state; + packet.mode = mode; + packet.fault_bitmask = fault_bitmask; mav_array_memcpy(packet.voltages, voltages, sizeof(uint16_t)*10); + mav_array_memcpy(packet.voltages_ext, voltages_ext, sizeof(uint16_t)*4); memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_BATTERY_STATUS_LEN); #endif @@ -124,11 +155,16 @@ static inline uint16_t mavlink_msg_battery_status_pack(uint8_t system_id, uint8_ * @param current_consumed [mAh] Consumed charge, -1: autopilot does not provide consumption estimate * @param energy_consumed [hJ] Consumed energy, -1: autopilot does not provide energy consumption estimate * @param battery_remaining [%] Remaining battery energy. Values: [0-100], -1: autopilot does not estimate the remaining battery. + * @param time_remaining [s] Remaining battery time, 0: autopilot does not provide remaining battery time estimate + * @param charge_state State for extent of discharge, provided by autopilot for warning or external reactions + * @param voltages_ext [mV] Battery voltages for cells 11 to 14. Cells above the valid cell count for this battery should have a value of 0, where zero indicates not supported (note, this is different than for the voltages field and allows empty byte truncation). If the measured value is 0 then 1 should be sent instead. + * @param mode Battery mode. Default (0) is that battery mode reporting is not supported or battery is in normal-use mode. + * @param fault_bitmask Fault/health indications. These should be set when charge_state is MAV_BATTERY_CHARGE_STATE_FAILED or MAV_BATTERY_CHARGE_STATE_UNHEALTHY (if not, fault reporting is not supported). * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_battery_status_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, - uint8_t id,uint8_t battery_function,uint8_t type,int16_t temperature,const uint16_t *voltages,int16_t current_battery,int32_t current_consumed,int32_t energy_consumed,int8_t battery_remaining) + uint8_t id,uint8_t battery_function,uint8_t type,int16_t temperature,const uint16_t *voltages,int16_t current_battery,int32_t current_consumed,int32_t energy_consumed,int8_t battery_remaining,int32_t time_remaining,uint8_t charge_state,const uint16_t *voltages_ext,uint8_t mode,uint32_t fault_bitmask) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_BATTERY_STATUS_LEN]; @@ -140,7 +176,12 @@ static inline uint16_t mavlink_msg_battery_status_pack_chan(uint8_t system_id, u _mav_put_uint8_t(buf, 33, battery_function); _mav_put_uint8_t(buf, 34, type); _mav_put_int8_t(buf, 35, battery_remaining); + _mav_put_int32_t(buf, 36, time_remaining); + _mav_put_uint8_t(buf, 40, charge_state); + _mav_put_uint8_t(buf, 49, mode); + _mav_put_uint32_t(buf, 50, fault_bitmask); _mav_put_uint16_t_array(buf, 10, voltages, 10); + _mav_put_uint16_t_array(buf, 41, voltages_ext, 4); memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_BATTERY_STATUS_LEN); #else mavlink_battery_status_t packet; @@ -152,7 +193,12 @@ static inline uint16_t mavlink_msg_battery_status_pack_chan(uint8_t system_id, u packet.battery_function = battery_function; packet.type = type; packet.battery_remaining = battery_remaining; + packet.time_remaining = time_remaining; + packet.charge_state = charge_state; + packet.mode = mode; + packet.fault_bitmask = fault_bitmask; mav_array_memcpy(packet.voltages, voltages, sizeof(uint16_t)*10); + mav_array_memcpy(packet.voltages_ext, voltages_ext, sizeof(uint16_t)*4); memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_BATTERY_STATUS_LEN); #endif @@ -170,7 +216,7 @@ static inline uint16_t mavlink_msg_battery_status_pack_chan(uint8_t system_id, u */ static inline uint16_t mavlink_msg_battery_status_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_battery_status_t* battery_status) { - return mavlink_msg_battery_status_pack(system_id, component_id, msg, battery_status->id, battery_status->battery_function, battery_status->type, battery_status->temperature, battery_status->voltages, battery_status->current_battery, battery_status->current_consumed, battery_status->energy_consumed, battery_status->battery_remaining); + return mavlink_msg_battery_status_pack(system_id, component_id, msg, battery_status->id, battery_status->battery_function, battery_status->type, battery_status->temperature, battery_status->voltages, battery_status->current_battery, battery_status->current_consumed, battery_status->energy_consumed, battery_status->battery_remaining, battery_status->time_remaining, battery_status->charge_state, battery_status->voltages_ext, battery_status->mode, battery_status->fault_bitmask); } /** @@ -184,7 +230,7 @@ static inline uint16_t mavlink_msg_battery_status_encode(uint8_t system_id, uint */ static inline uint16_t mavlink_msg_battery_status_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_battery_status_t* battery_status) { - return mavlink_msg_battery_status_pack_chan(system_id, component_id, chan, msg, battery_status->id, battery_status->battery_function, battery_status->type, battery_status->temperature, battery_status->voltages, battery_status->current_battery, battery_status->current_consumed, battery_status->energy_consumed, battery_status->battery_remaining); + return mavlink_msg_battery_status_pack_chan(system_id, component_id, chan, msg, battery_status->id, battery_status->battery_function, battery_status->type, battery_status->temperature, battery_status->voltages, battery_status->current_battery, battery_status->current_consumed, battery_status->energy_consumed, battery_status->battery_remaining, battery_status->time_remaining, battery_status->charge_state, battery_status->voltages_ext, battery_status->mode, battery_status->fault_bitmask); } /** @@ -200,10 +246,15 @@ static inline uint16_t mavlink_msg_battery_status_encode_chan(uint8_t system_id, * @param current_consumed [mAh] Consumed charge, -1: autopilot does not provide consumption estimate * @param energy_consumed [hJ] Consumed energy, -1: autopilot does not provide energy consumption estimate * @param battery_remaining [%] Remaining battery energy. Values: [0-100], -1: autopilot does not estimate the remaining battery. + * @param time_remaining [s] Remaining battery time, 0: autopilot does not provide remaining battery time estimate + * @param charge_state State for extent of discharge, provided by autopilot for warning or external reactions + * @param voltages_ext [mV] Battery voltages for cells 11 to 14. Cells above the valid cell count for this battery should have a value of 0, where zero indicates not supported (note, this is different than for the voltages field and allows empty byte truncation). If the measured value is 0 then 1 should be sent instead. + * @param mode Battery mode. Default (0) is that battery mode reporting is not supported or battery is in normal-use mode. + * @param fault_bitmask Fault/health indications. These should be set when charge_state is MAV_BATTERY_CHARGE_STATE_FAILED or MAV_BATTERY_CHARGE_STATE_UNHEALTHY (if not, fault reporting is not supported). */ #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS -static inline void mavlink_msg_battery_status_send(mavlink_channel_t chan, uint8_t id, uint8_t battery_function, uint8_t type, int16_t temperature, const uint16_t *voltages, int16_t current_battery, int32_t current_consumed, int32_t energy_consumed, int8_t battery_remaining) +static inline void mavlink_msg_battery_status_send(mavlink_channel_t chan, uint8_t id, uint8_t battery_function, uint8_t type, int16_t temperature, const uint16_t *voltages, int16_t current_battery, int32_t current_consumed, int32_t energy_consumed, int8_t battery_remaining, int32_t time_remaining, uint8_t charge_state, const uint16_t *voltages_ext, uint8_t mode, uint32_t fault_bitmask) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_BATTERY_STATUS_LEN]; @@ -215,7 +266,12 @@ static inline void mavlink_msg_battery_status_send(mavlink_channel_t chan, uint8 _mav_put_uint8_t(buf, 33, battery_function); _mav_put_uint8_t(buf, 34, type); _mav_put_int8_t(buf, 35, battery_remaining); + _mav_put_int32_t(buf, 36, time_remaining); + _mav_put_uint8_t(buf, 40, charge_state); + _mav_put_uint8_t(buf, 49, mode); + _mav_put_uint32_t(buf, 50, fault_bitmask); _mav_put_uint16_t_array(buf, 10, voltages, 10); + _mav_put_uint16_t_array(buf, 41, voltages_ext, 4); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_BATTERY_STATUS, buf, MAVLINK_MSG_ID_BATTERY_STATUS_MIN_LEN, MAVLINK_MSG_ID_BATTERY_STATUS_LEN, MAVLINK_MSG_ID_BATTERY_STATUS_CRC); #else mavlink_battery_status_t packet; @@ -227,7 +283,12 @@ static inline void mavlink_msg_battery_status_send(mavlink_channel_t chan, uint8 packet.battery_function = battery_function; packet.type = type; packet.battery_remaining = battery_remaining; + packet.time_remaining = time_remaining; + packet.charge_state = charge_state; + packet.mode = mode; + packet.fault_bitmask = fault_bitmask; mav_array_memcpy(packet.voltages, voltages, sizeof(uint16_t)*10); + mav_array_memcpy(packet.voltages_ext, voltages_ext, sizeof(uint16_t)*4); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_BATTERY_STATUS, (const char *)&packet, MAVLINK_MSG_ID_BATTERY_STATUS_MIN_LEN, MAVLINK_MSG_ID_BATTERY_STATUS_LEN, MAVLINK_MSG_ID_BATTERY_STATUS_CRC); #endif } @@ -240,7 +301,7 @@ static inline void mavlink_msg_battery_status_send(mavlink_channel_t chan, uint8 static inline void mavlink_msg_battery_status_send_struct(mavlink_channel_t chan, const mavlink_battery_status_t* battery_status) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - mavlink_msg_battery_status_send(chan, battery_status->id, battery_status->battery_function, battery_status->type, battery_status->temperature, battery_status->voltages, battery_status->current_battery, battery_status->current_consumed, battery_status->energy_consumed, battery_status->battery_remaining); + mavlink_msg_battery_status_send(chan, battery_status->id, battery_status->battery_function, battery_status->type, battery_status->temperature, battery_status->voltages, battery_status->current_battery, battery_status->current_consumed, battery_status->energy_consumed, battery_status->battery_remaining, battery_status->time_remaining, battery_status->charge_state, battery_status->voltages_ext, battery_status->mode, battery_status->fault_bitmask); #else _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_BATTERY_STATUS, (const char *)battery_status, MAVLINK_MSG_ID_BATTERY_STATUS_MIN_LEN, MAVLINK_MSG_ID_BATTERY_STATUS_LEN, MAVLINK_MSG_ID_BATTERY_STATUS_CRC); #endif @@ -254,7 +315,7 @@ static inline void mavlink_msg_battery_status_send_struct(mavlink_channel_t chan is usually the receive buffer for the channel, and allows a reply to an incoming message with minimum stack space usage. */ -static inline void mavlink_msg_battery_status_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t id, uint8_t battery_function, uint8_t type, int16_t temperature, const uint16_t *voltages, int16_t current_battery, int32_t current_consumed, int32_t energy_consumed, int8_t battery_remaining) +static inline void mavlink_msg_battery_status_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t id, uint8_t battery_function, uint8_t type, int16_t temperature, const uint16_t *voltages, int16_t current_battery, int32_t current_consumed, int32_t energy_consumed, int8_t battery_remaining, int32_t time_remaining, uint8_t charge_state, const uint16_t *voltages_ext, uint8_t mode, uint32_t fault_bitmask) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char *buf = (char *)msgbuf; @@ -266,7 +327,12 @@ static inline void mavlink_msg_battery_status_send_buf(mavlink_message_t *msgbuf _mav_put_uint8_t(buf, 33, battery_function); _mav_put_uint8_t(buf, 34, type); _mav_put_int8_t(buf, 35, battery_remaining); + _mav_put_int32_t(buf, 36, time_remaining); + _mav_put_uint8_t(buf, 40, charge_state); + _mav_put_uint8_t(buf, 49, mode); + _mav_put_uint32_t(buf, 50, fault_bitmask); _mav_put_uint16_t_array(buf, 10, voltages, 10); + _mav_put_uint16_t_array(buf, 41, voltages_ext, 4); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_BATTERY_STATUS, buf, MAVLINK_MSG_ID_BATTERY_STATUS_MIN_LEN, MAVLINK_MSG_ID_BATTERY_STATUS_LEN, MAVLINK_MSG_ID_BATTERY_STATUS_CRC); #else mavlink_battery_status_t *packet = (mavlink_battery_status_t *)msgbuf; @@ -278,7 +344,12 @@ static inline void mavlink_msg_battery_status_send_buf(mavlink_message_t *msgbuf packet->battery_function = battery_function; packet->type = type; packet->battery_remaining = battery_remaining; + packet->time_remaining = time_remaining; + packet->charge_state = charge_state; + packet->mode = mode; + packet->fault_bitmask = fault_bitmask; mav_array_memcpy(packet->voltages, voltages, sizeof(uint16_t)*10); + mav_array_memcpy(packet->voltages_ext, voltages_ext, sizeof(uint16_t)*4); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_BATTERY_STATUS, (const char *)packet, MAVLINK_MSG_ID_BATTERY_STATUS_MIN_LEN, MAVLINK_MSG_ID_BATTERY_STATUS_LEN, MAVLINK_MSG_ID_BATTERY_STATUS_CRC); #endif } @@ -379,6 +450,56 @@ static inline int8_t mavlink_msg_battery_status_get_battery_remaining(const mavl return _MAV_RETURN_int8_t(msg, 35); } +/** + * @brief Get field time_remaining from battery_status message + * + * @return [s] Remaining battery time, 0: autopilot does not provide remaining battery time estimate + */ +static inline int32_t mavlink_msg_battery_status_get_time_remaining(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int32_t(msg, 36); +} + +/** + * @brief Get field charge_state from battery_status message + * + * @return State for extent of discharge, provided by autopilot for warning or external reactions + */ +static inline uint8_t mavlink_msg_battery_status_get_charge_state(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 40); +} + +/** + * @brief Get field voltages_ext from battery_status message + * + * @return [mV] Battery voltages for cells 11 to 14. Cells above the valid cell count for this battery should have a value of 0, where zero indicates not supported (note, this is different than for the voltages field and allows empty byte truncation). If the measured value is 0 then 1 should be sent instead. + */ +static inline uint16_t mavlink_msg_battery_status_get_voltages_ext(const mavlink_message_t* msg, uint16_t *voltages_ext) +{ + return _MAV_RETURN_uint16_t_array(msg, voltages_ext, 4, 41); +} + +/** + * @brief Get field mode from battery_status message + * + * @return Battery mode. Default (0) is that battery mode reporting is not supported or battery is in normal-use mode. + */ +static inline uint8_t mavlink_msg_battery_status_get_mode(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 49); +} + +/** + * @brief Get field fault_bitmask from battery_status message + * + * @return Fault/health indications. These should be set when charge_state is MAV_BATTERY_CHARGE_STATE_FAILED or MAV_BATTERY_CHARGE_STATE_UNHEALTHY (if not, fault reporting is not supported). + */ +static inline uint32_t mavlink_msg_battery_status_get_fault_bitmask(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint32_t(msg, 50); +} + /** * @brief Decode a battery_status message into a struct * @@ -397,6 +518,11 @@ static inline void mavlink_msg_battery_status_decode(const mavlink_message_t* ms battery_status->battery_function = mavlink_msg_battery_status_get_battery_function(msg); battery_status->type = mavlink_msg_battery_status_get_type(msg); battery_status->battery_remaining = mavlink_msg_battery_status_get_battery_remaining(msg); + battery_status->time_remaining = mavlink_msg_battery_status_get_time_remaining(msg); + battery_status->charge_state = mavlink_msg_battery_status_get_charge_state(msg); + mavlink_msg_battery_status_get_voltages_ext(msg, battery_status->voltages_ext); + battery_status->mode = mavlink_msg_battery_status_get_mode(msg); + battery_status->fault_bitmask = mavlink_msg_battery_status_get_fault_bitmask(msg); #else uint8_t len = msg->len < MAVLINK_MSG_ID_BATTERY_STATUS_LEN? msg->len : MAVLINK_MSG_ID_BATTERY_STATUS_LEN; memset(battery_status, 0, MAVLINK_MSG_ID_BATTERY_STATUS_LEN); diff --git a/lib/main/MAVLink/common/mavlink_msg_button_change.h b/lib/main/MAVLink/common/mavlink_msg_button_change.h new file mode 100644 index 0000000000..831bc60fc3 --- /dev/null +++ b/lib/main/MAVLink/common/mavlink_msg_button_change.h @@ -0,0 +1,263 @@ +#pragma once +// MESSAGE BUTTON_CHANGE PACKING + +#define MAVLINK_MSG_ID_BUTTON_CHANGE 257 + + +typedef struct __mavlink_button_change_t { + uint32_t time_boot_ms; /*< [ms] Timestamp (time since system boot).*/ + uint32_t last_change_ms; /*< [ms] Time of last change of button state.*/ + uint8_t state; /*< Bitmap for state of buttons.*/ +} mavlink_button_change_t; + +#define MAVLINK_MSG_ID_BUTTON_CHANGE_LEN 9 +#define MAVLINK_MSG_ID_BUTTON_CHANGE_MIN_LEN 9 +#define MAVLINK_MSG_ID_257_LEN 9 +#define MAVLINK_MSG_ID_257_MIN_LEN 9 + +#define MAVLINK_MSG_ID_BUTTON_CHANGE_CRC 131 +#define MAVLINK_MSG_ID_257_CRC 131 + + + +#if MAVLINK_COMMAND_24BIT +#define MAVLINK_MESSAGE_INFO_BUTTON_CHANGE { \ + 257, \ + "BUTTON_CHANGE", \ + 3, \ + { { "time_boot_ms", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_button_change_t, time_boot_ms) }, \ + { "last_change_ms", NULL, MAVLINK_TYPE_UINT32_T, 0, 4, offsetof(mavlink_button_change_t, last_change_ms) }, \ + { "state", NULL, MAVLINK_TYPE_UINT8_T, 0, 8, offsetof(mavlink_button_change_t, state) }, \ + } \ +} +#else +#define MAVLINK_MESSAGE_INFO_BUTTON_CHANGE { \ + "BUTTON_CHANGE", \ + 3, \ + { { "time_boot_ms", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_button_change_t, time_boot_ms) }, \ + { "last_change_ms", NULL, MAVLINK_TYPE_UINT32_T, 0, 4, offsetof(mavlink_button_change_t, last_change_ms) }, \ + { "state", NULL, MAVLINK_TYPE_UINT8_T, 0, 8, offsetof(mavlink_button_change_t, state) }, \ + } \ +} +#endif + +/** + * @brief Pack a button_change message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param time_boot_ms [ms] Timestamp (time since system boot). + * @param last_change_ms [ms] Time of last change of button state. + * @param state Bitmap for state of buttons. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_button_change_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint32_t time_boot_ms, uint32_t last_change_ms, uint8_t state) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_BUTTON_CHANGE_LEN]; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_uint32_t(buf, 4, last_change_ms); + _mav_put_uint8_t(buf, 8, state); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_BUTTON_CHANGE_LEN); +#else + mavlink_button_change_t packet; + packet.time_boot_ms = time_boot_ms; + packet.last_change_ms = last_change_ms; + packet.state = state; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_BUTTON_CHANGE_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_BUTTON_CHANGE; + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_BUTTON_CHANGE_MIN_LEN, MAVLINK_MSG_ID_BUTTON_CHANGE_LEN, MAVLINK_MSG_ID_BUTTON_CHANGE_CRC); +} + +/** + * @brief Pack a button_change message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param time_boot_ms [ms] Timestamp (time since system boot). + * @param last_change_ms [ms] Time of last change of button state. + * @param state Bitmap for state of buttons. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_button_change_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint32_t time_boot_ms,uint32_t last_change_ms,uint8_t state) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_BUTTON_CHANGE_LEN]; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_uint32_t(buf, 4, last_change_ms); + _mav_put_uint8_t(buf, 8, state); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_BUTTON_CHANGE_LEN); +#else + mavlink_button_change_t packet; + packet.time_boot_ms = time_boot_ms; + packet.last_change_ms = last_change_ms; + packet.state = state; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_BUTTON_CHANGE_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_BUTTON_CHANGE; + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_BUTTON_CHANGE_MIN_LEN, MAVLINK_MSG_ID_BUTTON_CHANGE_LEN, MAVLINK_MSG_ID_BUTTON_CHANGE_CRC); +} + +/** + * @brief Encode a button_change struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param button_change C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_button_change_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_button_change_t* button_change) +{ + return mavlink_msg_button_change_pack(system_id, component_id, msg, button_change->time_boot_ms, button_change->last_change_ms, button_change->state); +} + +/** + * @brief Encode a button_change struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param button_change C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_button_change_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_button_change_t* button_change) +{ + return mavlink_msg_button_change_pack_chan(system_id, component_id, chan, msg, button_change->time_boot_ms, button_change->last_change_ms, button_change->state); +} + +/** + * @brief Send a button_change message + * @param chan MAVLink channel to send the message + * + * @param time_boot_ms [ms] Timestamp (time since system boot). + * @param last_change_ms [ms] Time of last change of button state. + * @param state Bitmap for state of buttons. + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_button_change_send(mavlink_channel_t chan, uint32_t time_boot_ms, uint32_t last_change_ms, uint8_t state) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_BUTTON_CHANGE_LEN]; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_uint32_t(buf, 4, last_change_ms); + _mav_put_uint8_t(buf, 8, state); + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_BUTTON_CHANGE, buf, MAVLINK_MSG_ID_BUTTON_CHANGE_MIN_LEN, MAVLINK_MSG_ID_BUTTON_CHANGE_LEN, MAVLINK_MSG_ID_BUTTON_CHANGE_CRC); +#else + mavlink_button_change_t packet; + packet.time_boot_ms = time_boot_ms; + packet.last_change_ms = last_change_ms; + packet.state = state; + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_BUTTON_CHANGE, (const char *)&packet, MAVLINK_MSG_ID_BUTTON_CHANGE_MIN_LEN, MAVLINK_MSG_ID_BUTTON_CHANGE_LEN, MAVLINK_MSG_ID_BUTTON_CHANGE_CRC); +#endif +} + +/** + * @brief Send a button_change message + * @param chan MAVLink channel to send the message + * @param struct The MAVLink struct to serialize + */ +static inline void mavlink_msg_button_change_send_struct(mavlink_channel_t chan, const mavlink_button_change_t* button_change) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + mavlink_msg_button_change_send(chan, button_change->time_boot_ms, button_change->last_change_ms, button_change->state); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_BUTTON_CHANGE, (const char *)button_change, MAVLINK_MSG_ID_BUTTON_CHANGE_MIN_LEN, MAVLINK_MSG_ID_BUTTON_CHANGE_LEN, MAVLINK_MSG_ID_BUTTON_CHANGE_CRC); +#endif +} + +#if MAVLINK_MSG_ID_BUTTON_CHANGE_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_button_change_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint32_t time_boot_ms, uint32_t last_change_ms, uint8_t state) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_uint32_t(buf, 4, last_change_ms); + _mav_put_uint8_t(buf, 8, state); + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_BUTTON_CHANGE, buf, MAVLINK_MSG_ID_BUTTON_CHANGE_MIN_LEN, MAVLINK_MSG_ID_BUTTON_CHANGE_LEN, MAVLINK_MSG_ID_BUTTON_CHANGE_CRC); +#else + mavlink_button_change_t *packet = (mavlink_button_change_t *)msgbuf; + packet->time_boot_ms = time_boot_ms; + packet->last_change_ms = last_change_ms; + packet->state = state; + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_BUTTON_CHANGE, (const char *)packet, MAVLINK_MSG_ID_BUTTON_CHANGE_MIN_LEN, MAVLINK_MSG_ID_BUTTON_CHANGE_LEN, MAVLINK_MSG_ID_BUTTON_CHANGE_CRC); +#endif +} +#endif + +#endif + +// MESSAGE BUTTON_CHANGE UNPACKING + + +/** + * @brief Get field time_boot_ms from button_change message + * + * @return [ms] Timestamp (time since system boot). + */ +static inline uint32_t mavlink_msg_button_change_get_time_boot_ms(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint32_t(msg, 0); +} + +/** + * @brief Get field last_change_ms from button_change message + * + * @return [ms] Time of last change of button state. + */ +static inline uint32_t mavlink_msg_button_change_get_last_change_ms(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint32_t(msg, 4); +} + +/** + * @brief Get field state from button_change message + * + * @return Bitmap for state of buttons. + */ +static inline uint8_t mavlink_msg_button_change_get_state(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 8); +} + +/** + * @brief Decode a button_change message into a struct + * + * @param msg The message to decode + * @param button_change C-struct to decode the message contents into + */ +static inline void mavlink_msg_button_change_decode(const mavlink_message_t* msg, mavlink_button_change_t* button_change) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + button_change->time_boot_ms = mavlink_msg_button_change_get_time_boot_ms(msg); + button_change->last_change_ms = mavlink_msg_button_change_get_last_change_ms(msg); + button_change->state = mavlink_msg_button_change_get_state(msg); +#else + uint8_t len = msg->len < MAVLINK_MSG_ID_BUTTON_CHANGE_LEN? msg->len : MAVLINK_MSG_ID_BUTTON_CHANGE_LEN; + memset(button_change, 0, MAVLINK_MSG_ID_BUTTON_CHANGE_LEN); + memcpy(button_change, _MAV_PAYLOAD(msg), len); +#endif +} diff --git a/lib/main/MAVLink/common/mavlink_msg_camera_capture_status.h b/lib/main/MAVLink/common/mavlink_msg_camera_capture_status.h new file mode 100644 index 0000000000..8951db6a79 --- /dev/null +++ b/lib/main/MAVLink/common/mavlink_msg_camera_capture_status.h @@ -0,0 +1,363 @@ +#pragma once +// MESSAGE CAMERA_CAPTURE_STATUS PACKING + +#define MAVLINK_MSG_ID_CAMERA_CAPTURE_STATUS 262 + +MAVPACKED( +typedef struct __mavlink_camera_capture_status_t { + uint32_t time_boot_ms; /*< [ms] Timestamp (time since system boot).*/ + float image_interval; /*< [s] Image capture interval*/ + uint32_t recording_time_ms; /*< [ms] Time since recording started*/ + float available_capacity; /*< [MiB] Available storage capacity.*/ + uint8_t image_status; /*< Current status of image capturing (0: idle, 1: capture in progress, 2: interval set but idle, 3: interval set and capture in progress)*/ + uint8_t video_status; /*< Current status of video capturing (0: idle, 1: capture in progress)*/ + int32_t image_count; /*< Total number of images captured ('forever', or until reset using MAV_CMD_STORAGE_FORMAT).*/ +}) mavlink_camera_capture_status_t; + +#define MAVLINK_MSG_ID_CAMERA_CAPTURE_STATUS_LEN 22 +#define MAVLINK_MSG_ID_CAMERA_CAPTURE_STATUS_MIN_LEN 18 +#define MAVLINK_MSG_ID_262_LEN 22 +#define MAVLINK_MSG_ID_262_MIN_LEN 18 + +#define MAVLINK_MSG_ID_CAMERA_CAPTURE_STATUS_CRC 12 +#define MAVLINK_MSG_ID_262_CRC 12 + + + +#if MAVLINK_COMMAND_24BIT +#define MAVLINK_MESSAGE_INFO_CAMERA_CAPTURE_STATUS { \ + 262, \ + "CAMERA_CAPTURE_STATUS", \ + 7, \ + { { "time_boot_ms", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_camera_capture_status_t, time_boot_ms) }, \ + { "image_status", NULL, MAVLINK_TYPE_UINT8_T, 0, 16, offsetof(mavlink_camera_capture_status_t, image_status) }, \ + { "video_status", NULL, MAVLINK_TYPE_UINT8_T, 0, 17, offsetof(mavlink_camera_capture_status_t, video_status) }, \ + { "image_interval", NULL, MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_camera_capture_status_t, image_interval) }, \ + { "recording_time_ms", NULL, MAVLINK_TYPE_UINT32_T, 0, 8, offsetof(mavlink_camera_capture_status_t, recording_time_ms) }, \ + { "available_capacity", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_camera_capture_status_t, available_capacity) }, \ + { "image_count", NULL, MAVLINK_TYPE_INT32_T, 0, 18, offsetof(mavlink_camera_capture_status_t, image_count) }, \ + } \ +} +#else +#define MAVLINK_MESSAGE_INFO_CAMERA_CAPTURE_STATUS { \ + "CAMERA_CAPTURE_STATUS", \ + 7, \ + { { "time_boot_ms", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_camera_capture_status_t, time_boot_ms) }, \ + { "image_status", NULL, MAVLINK_TYPE_UINT8_T, 0, 16, offsetof(mavlink_camera_capture_status_t, image_status) }, \ + { "video_status", NULL, MAVLINK_TYPE_UINT8_T, 0, 17, offsetof(mavlink_camera_capture_status_t, video_status) }, \ + { "image_interval", NULL, MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_camera_capture_status_t, image_interval) }, \ + { "recording_time_ms", NULL, MAVLINK_TYPE_UINT32_T, 0, 8, offsetof(mavlink_camera_capture_status_t, recording_time_ms) }, \ + { "available_capacity", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_camera_capture_status_t, available_capacity) }, \ + { "image_count", NULL, MAVLINK_TYPE_INT32_T, 0, 18, offsetof(mavlink_camera_capture_status_t, image_count) }, \ + } \ +} +#endif + +/** + * @brief Pack a camera_capture_status message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param time_boot_ms [ms] Timestamp (time since system boot). + * @param image_status Current status of image capturing (0: idle, 1: capture in progress, 2: interval set but idle, 3: interval set and capture in progress) + * @param video_status Current status of video capturing (0: idle, 1: capture in progress) + * @param image_interval [s] Image capture interval + * @param recording_time_ms [ms] Time since recording started + * @param available_capacity [MiB] Available storage capacity. + * @param image_count Total number of images captured ('forever', or until reset using MAV_CMD_STORAGE_FORMAT). + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_camera_capture_status_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint32_t time_boot_ms, uint8_t image_status, uint8_t video_status, float image_interval, uint32_t recording_time_ms, float available_capacity, int32_t image_count) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_CAMERA_CAPTURE_STATUS_LEN]; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_float(buf, 4, image_interval); + _mav_put_uint32_t(buf, 8, recording_time_ms); + _mav_put_float(buf, 12, available_capacity); + _mav_put_uint8_t(buf, 16, image_status); + _mav_put_uint8_t(buf, 17, video_status); + _mav_put_int32_t(buf, 18, image_count); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_CAMERA_CAPTURE_STATUS_LEN); +#else + mavlink_camera_capture_status_t packet; + packet.time_boot_ms = time_boot_ms; + packet.image_interval = image_interval; + packet.recording_time_ms = recording_time_ms; + packet.available_capacity = available_capacity; + packet.image_status = image_status; + packet.video_status = video_status; + packet.image_count = image_count; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_CAMERA_CAPTURE_STATUS_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_CAMERA_CAPTURE_STATUS; + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_CAMERA_CAPTURE_STATUS_MIN_LEN, MAVLINK_MSG_ID_CAMERA_CAPTURE_STATUS_LEN, MAVLINK_MSG_ID_CAMERA_CAPTURE_STATUS_CRC); +} + +/** + * @brief Pack a camera_capture_status message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param time_boot_ms [ms] Timestamp (time since system boot). + * @param image_status Current status of image capturing (0: idle, 1: capture in progress, 2: interval set but idle, 3: interval set and capture in progress) + * @param video_status Current status of video capturing (0: idle, 1: capture in progress) + * @param image_interval [s] Image capture interval + * @param recording_time_ms [ms] Time since recording started + * @param available_capacity [MiB] Available storage capacity. + * @param image_count Total number of images captured ('forever', or until reset using MAV_CMD_STORAGE_FORMAT). + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_camera_capture_status_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint32_t time_boot_ms,uint8_t image_status,uint8_t video_status,float image_interval,uint32_t recording_time_ms,float available_capacity,int32_t image_count) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_CAMERA_CAPTURE_STATUS_LEN]; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_float(buf, 4, image_interval); + _mav_put_uint32_t(buf, 8, recording_time_ms); + _mav_put_float(buf, 12, available_capacity); + _mav_put_uint8_t(buf, 16, image_status); + _mav_put_uint8_t(buf, 17, video_status); + _mav_put_int32_t(buf, 18, image_count); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_CAMERA_CAPTURE_STATUS_LEN); +#else + mavlink_camera_capture_status_t packet; + packet.time_boot_ms = time_boot_ms; + packet.image_interval = image_interval; + packet.recording_time_ms = recording_time_ms; + packet.available_capacity = available_capacity; + packet.image_status = image_status; + packet.video_status = video_status; + packet.image_count = image_count; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_CAMERA_CAPTURE_STATUS_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_CAMERA_CAPTURE_STATUS; + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_CAMERA_CAPTURE_STATUS_MIN_LEN, MAVLINK_MSG_ID_CAMERA_CAPTURE_STATUS_LEN, MAVLINK_MSG_ID_CAMERA_CAPTURE_STATUS_CRC); +} + +/** + * @brief Encode a camera_capture_status struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param camera_capture_status C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_camera_capture_status_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_camera_capture_status_t* camera_capture_status) +{ + return mavlink_msg_camera_capture_status_pack(system_id, component_id, msg, camera_capture_status->time_boot_ms, camera_capture_status->image_status, camera_capture_status->video_status, camera_capture_status->image_interval, camera_capture_status->recording_time_ms, camera_capture_status->available_capacity, camera_capture_status->image_count); +} + +/** + * @brief Encode a camera_capture_status struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param camera_capture_status C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_camera_capture_status_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_camera_capture_status_t* camera_capture_status) +{ + return mavlink_msg_camera_capture_status_pack_chan(system_id, component_id, chan, msg, camera_capture_status->time_boot_ms, camera_capture_status->image_status, camera_capture_status->video_status, camera_capture_status->image_interval, camera_capture_status->recording_time_ms, camera_capture_status->available_capacity, camera_capture_status->image_count); +} + +/** + * @brief Send a camera_capture_status message + * @param chan MAVLink channel to send the message + * + * @param time_boot_ms [ms] Timestamp (time since system boot). + * @param image_status Current status of image capturing (0: idle, 1: capture in progress, 2: interval set but idle, 3: interval set and capture in progress) + * @param video_status Current status of video capturing (0: idle, 1: capture in progress) + * @param image_interval [s] Image capture interval + * @param recording_time_ms [ms] Time since recording started + * @param available_capacity [MiB] Available storage capacity. + * @param image_count Total number of images captured ('forever', or until reset using MAV_CMD_STORAGE_FORMAT). + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_camera_capture_status_send(mavlink_channel_t chan, uint32_t time_boot_ms, uint8_t image_status, uint8_t video_status, float image_interval, uint32_t recording_time_ms, float available_capacity, int32_t image_count) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_CAMERA_CAPTURE_STATUS_LEN]; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_float(buf, 4, image_interval); + _mav_put_uint32_t(buf, 8, recording_time_ms); + _mav_put_float(buf, 12, available_capacity); + _mav_put_uint8_t(buf, 16, image_status); + _mav_put_uint8_t(buf, 17, video_status); + _mav_put_int32_t(buf, 18, image_count); + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CAMERA_CAPTURE_STATUS, buf, MAVLINK_MSG_ID_CAMERA_CAPTURE_STATUS_MIN_LEN, MAVLINK_MSG_ID_CAMERA_CAPTURE_STATUS_LEN, MAVLINK_MSG_ID_CAMERA_CAPTURE_STATUS_CRC); +#else + mavlink_camera_capture_status_t packet; + packet.time_boot_ms = time_boot_ms; + packet.image_interval = image_interval; + packet.recording_time_ms = recording_time_ms; + packet.available_capacity = available_capacity; + packet.image_status = image_status; + packet.video_status = video_status; + packet.image_count = image_count; + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CAMERA_CAPTURE_STATUS, (const char *)&packet, MAVLINK_MSG_ID_CAMERA_CAPTURE_STATUS_MIN_LEN, MAVLINK_MSG_ID_CAMERA_CAPTURE_STATUS_LEN, MAVLINK_MSG_ID_CAMERA_CAPTURE_STATUS_CRC); +#endif +} + +/** + * @brief Send a camera_capture_status message + * @param chan MAVLink channel to send the message + * @param struct The MAVLink struct to serialize + */ +static inline void mavlink_msg_camera_capture_status_send_struct(mavlink_channel_t chan, const mavlink_camera_capture_status_t* camera_capture_status) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + mavlink_msg_camera_capture_status_send(chan, camera_capture_status->time_boot_ms, camera_capture_status->image_status, camera_capture_status->video_status, camera_capture_status->image_interval, camera_capture_status->recording_time_ms, camera_capture_status->available_capacity, camera_capture_status->image_count); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CAMERA_CAPTURE_STATUS, (const char *)camera_capture_status, MAVLINK_MSG_ID_CAMERA_CAPTURE_STATUS_MIN_LEN, MAVLINK_MSG_ID_CAMERA_CAPTURE_STATUS_LEN, MAVLINK_MSG_ID_CAMERA_CAPTURE_STATUS_CRC); +#endif +} + +#if MAVLINK_MSG_ID_CAMERA_CAPTURE_STATUS_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_camera_capture_status_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint32_t time_boot_ms, uint8_t image_status, uint8_t video_status, float image_interval, uint32_t recording_time_ms, float available_capacity, int32_t image_count) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_float(buf, 4, image_interval); + _mav_put_uint32_t(buf, 8, recording_time_ms); + _mav_put_float(buf, 12, available_capacity); + _mav_put_uint8_t(buf, 16, image_status); + _mav_put_uint8_t(buf, 17, video_status); + _mav_put_int32_t(buf, 18, image_count); + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CAMERA_CAPTURE_STATUS, buf, MAVLINK_MSG_ID_CAMERA_CAPTURE_STATUS_MIN_LEN, MAVLINK_MSG_ID_CAMERA_CAPTURE_STATUS_LEN, MAVLINK_MSG_ID_CAMERA_CAPTURE_STATUS_CRC); +#else + mavlink_camera_capture_status_t *packet = (mavlink_camera_capture_status_t *)msgbuf; + packet->time_boot_ms = time_boot_ms; + packet->image_interval = image_interval; + packet->recording_time_ms = recording_time_ms; + packet->available_capacity = available_capacity; + packet->image_status = image_status; + packet->video_status = video_status; + packet->image_count = image_count; + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CAMERA_CAPTURE_STATUS, (const char *)packet, MAVLINK_MSG_ID_CAMERA_CAPTURE_STATUS_MIN_LEN, MAVLINK_MSG_ID_CAMERA_CAPTURE_STATUS_LEN, MAVLINK_MSG_ID_CAMERA_CAPTURE_STATUS_CRC); +#endif +} +#endif + +#endif + +// MESSAGE CAMERA_CAPTURE_STATUS UNPACKING + + +/** + * @brief Get field time_boot_ms from camera_capture_status message + * + * @return [ms] Timestamp (time since system boot). + */ +static inline uint32_t mavlink_msg_camera_capture_status_get_time_boot_ms(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint32_t(msg, 0); +} + +/** + * @brief Get field image_status from camera_capture_status message + * + * @return Current status of image capturing (0: idle, 1: capture in progress, 2: interval set but idle, 3: interval set and capture in progress) + */ +static inline uint8_t mavlink_msg_camera_capture_status_get_image_status(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 16); +} + +/** + * @brief Get field video_status from camera_capture_status message + * + * @return Current status of video capturing (0: idle, 1: capture in progress) + */ +static inline uint8_t mavlink_msg_camera_capture_status_get_video_status(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 17); +} + +/** + * @brief Get field image_interval from camera_capture_status message + * + * @return [s] Image capture interval + */ +static inline float mavlink_msg_camera_capture_status_get_image_interval(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 4); +} + +/** + * @brief Get field recording_time_ms from camera_capture_status message + * + * @return [ms] Time since recording started + */ +static inline uint32_t mavlink_msg_camera_capture_status_get_recording_time_ms(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint32_t(msg, 8); +} + +/** + * @brief Get field available_capacity from camera_capture_status message + * + * @return [MiB] Available storage capacity. + */ +static inline float mavlink_msg_camera_capture_status_get_available_capacity(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 12); +} + +/** + * @brief Get field image_count from camera_capture_status message + * + * @return Total number of images captured ('forever', or until reset using MAV_CMD_STORAGE_FORMAT). + */ +static inline int32_t mavlink_msg_camera_capture_status_get_image_count(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int32_t(msg, 18); +} + +/** + * @brief Decode a camera_capture_status message into a struct + * + * @param msg The message to decode + * @param camera_capture_status C-struct to decode the message contents into + */ +static inline void mavlink_msg_camera_capture_status_decode(const mavlink_message_t* msg, mavlink_camera_capture_status_t* camera_capture_status) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + camera_capture_status->time_boot_ms = mavlink_msg_camera_capture_status_get_time_boot_ms(msg); + camera_capture_status->image_interval = mavlink_msg_camera_capture_status_get_image_interval(msg); + camera_capture_status->recording_time_ms = mavlink_msg_camera_capture_status_get_recording_time_ms(msg); + camera_capture_status->available_capacity = mavlink_msg_camera_capture_status_get_available_capacity(msg); + camera_capture_status->image_status = mavlink_msg_camera_capture_status_get_image_status(msg); + camera_capture_status->video_status = mavlink_msg_camera_capture_status_get_video_status(msg); + camera_capture_status->image_count = mavlink_msg_camera_capture_status_get_image_count(msg); +#else + uint8_t len = msg->len < MAVLINK_MSG_ID_CAMERA_CAPTURE_STATUS_LEN? msg->len : MAVLINK_MSG_ID_CAMERA_CAPTURE_STATUS_LEN; + memset(camera_capture_status, 0, MAVLINK_MSG_ID_CAMERA_CAPTURE_STATUS_LEN); + memcpy(camera_capture_status, _MAV_PAYLOAD(msg), len); +#endif +} diff --git a/lib/main/MAVLink/common/mavlink_msg_camera_fov_status.h b/lib/main/MAVLink/common/mavlink_msg_camera_fov_status.h new file mode 100644 index 0000000000..6378e56aa8 --- /dev/null +++ b/lib/main/MAVLink/common/mavlink_msg_camera_fov_status.h @@ -0,0 +1,430 @@ +#pragma once +// MESSAGE CAMERA_FOV_STATUS PACKING + +#define MAVLINK_MSG_ID_CAMERA_FOV_STATUS 271 + + +typedef struct __mavlink_camera_fov_status_t { + uint32_t time_boot_ms; /*< [ms] Timestamp (time since system boot).*/ + int32_t lat_camera; /*< [degE7] Latitude of camera (INT32_MAX if unknown).*/ + int32_t lon_camera; /*< [degE7] Longitude of camera (INT32_MAX if unknown).*/ + int32_t alt_camera; /*< [mm] Altitude (MSL) of camera (INT32_MAX if unknown).*/ + int32_t lat_image; /*< [degE7] Latitude of center of image (INT32_MAX if unknown, INT32_MIN if at infinity, not intersecting with horizon).*/ + int32_t lon_image; /*< [degE7] Longitude of center of image (INT32_MAX if unknown, INT32_MIN if at infinity, not intersecting with horizon).*/ + int32_t alt_image; /*< [mm] Altitude (MSL) of center of image (INT32_MAX if unknown, INT32_MIN if at infinity, not intersecting with horizon).*/ + float q[4]; /*< Quaternion of camera orientation (w, x, y, z order, zero-rotation is 1, 0, 0, 0)*/ + float hfov; /*< [deg] Horizontal field of view (NaN if unknown).*/ + float vfov; /*< [deg] Vertical field of view (NaN if unknown).*/ +} mavlink_camera_fov_status_t; + +#define MAVLINK_MSG_ID_CAMERA_FOV_STATUS_LEN 52 +#define MAVLINK_MSG_ID_CAMERA_FOV_STATUS_MIN_LEN 52 +#define MAVLINK_MSG_ID_271_LEN 52 +#define MAVLINK_MSG_ID_271_MIN_LEN 52 + +#define MAVLINK_MSG_ID_CAMERA_FOV_STATUS_CRC 22 +#define MAVLINK_MSG_ID_271_CRC 22 + +#define MAVLINK_MSG_CAMERA_FOV_STATUS_FIELD_Q_LEN 4 + +#if MAVLINK_COMMAND_24BIT +#define MAVLINK_MESSAGE_INFO_CAMERA_FOV_STATUS { \ + 271, \ + "CAMERA_FOV_STATUS", \ + 10, \ + { { "time_boot_ms", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_camera_fov_status_t, time_boot_ms) }, \ + { "lat_camera", NULL, MAVLINK_TYPE_INT32_T, 0, 4, offsetof(mavlink_camera_fov_status_t, lat_camera) }, \ + { "lon_camera", NULL, MAVLINK_TYPE_INT32_T, 0, 8, offsetof(mavlink_camera_fov_status_t, lon_camera) }, \ + { "alt_camera", NULL, MAVLINK_TYPE_INT32_T, 0, 12, offsetof(mavlink_camera_fov_status_t, alt_camera) }, \ + { "lat_image", NULL, MAVLINK_TYPE_INT32_T, 0, 16, offsetof(mavlink_camera_fov_status_t, lat_image) }, \ + { "lon_image", NULL, MAVLINK_TYPE_INT32_T, 0, 20, offsetof(mavlink_camera_fov_status_t, lon_image) }, \ + { "alt_image", NULL, MAVLINK_TYPE_INT32_T, 0, 24, offsetof(mavlink_camera_fov_status_t, alt_image) }, \ + { "q", NULL, MAVLINK_TYPE_FLOAT, 4, 28, offsetof(mavlink_camera_fov_status_t, q) }, \ + { "hfov", NULL, MAVLINK_TYPE_FLOAT, 0, 44, offsetof(mavlink_camera_fov_status_t, hfov) }, \ + { "vfov", NULL, MAVLINK_TYPE_FLOAT, 0, 48, offsetof(mavlink_camera_fov_status_t, vfov) }, \ + } \ +} +#else +#define MAVLINK_MESSAGE_INFO_CAMERA_FOV_STATUS { \ + "CAMERA_FOV_STATUS", \ + 10, \ + { { "time_boot_ms", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_camera_fov_status_t, time_boot_ms) }, \ + { "lat_camera", NULL, MAVLINK_TYPE_INT32_T, 0, 4, offsetof(mavlink_camera_fov_status_t, lat_camera) }, \ + { "lon_camera", NULL, MAVLINK_TYPE_INT32_T, 0, 8, offsetof(mavlink_camera_fov_status_t, lon_camera) }, \ + { "alt_camera", NULL, MAVLINK_TYPE_INT32_T, 0, 12, offsetof(mavlink_camera_fov_status_t, alt_camera) }, \ + { "lat_image", NULL, MAVLINK_TYPE_INT32_T, 0, 16, offsetof(mavlink_camera_fov_status_t, lat_image) }, \ + { "lon_image", NULL, MAVLINK_TYPE_INT32_T, 0, 20, offsetof(mavlink_camera_fov_status_t, lon_image) }, \ + { "alt_image", NULL, MAVLINK_TYPE_INT32_T, 0, 24, offsetof(mavlink_camera_fov_status_t, alt_image) }, \ + { "q", NULL, MAVLINK_TYPE_FLOAT, 4, 28, offsetof(mavlink_camera_fov_status_t, q) }, \ + { "hfov", NULL, MAVLINK_TYPE_FLOAT, 0, 44, offsetof(mavlink_camera_fov_status_t, hfov) }, \ + { "vfov", NULL, MAVLINK_TYPE_FLOAT, 0, 48, offsetof(mavlink_camera_fov_status_t, vfov) }, \ + } \ +} +#endif + +/** + * @brief Pack a camera_fov_status message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param time_boot_ms [ms] Timestamp (time since system boot). + * @param lat_camera [degE7] Latitude of camera (INT32_MAX if unknown). + * @param lon_camera [degE7] Longitude of camera (INT32_MAX if unknown). + * @param alt_camera [mm] Altitude (MSL) of camera (INT32_MAX if unknown). + * @param lat_image [degE7] Latitude of center of image (INT32_MAX if unknown, INT32_MIN if at infinity, not intersecting with horizon). + * @param lon_image [degE7] Longitude of center of image (INT32_MAX if unknown, INT32_MIN if at infinity, not intersecting with horizon). + * @param alt_image [mm] Altitude (MSL) of center of image (INT32_MAX if unknown, INT32_MIN if at infinity, not intersecting with horizon). + * @param q Quaternion of camera orientation (w, x, y, z order, zero-rotation is 1, 0, 0, 0) + * @param hfov [deg] Horizontal field of view (NaN if unknown). + * @param vfov [deg] Vertical field of view (NaN if unknown). + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_camera_fov_status_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint32_t time_boot_ms, int32_t lat_camera, int32_t lon_camera, int32_t alt_camera, int32_t lat_image, int32_t lon_image, int32_t alt_image, const float *q, float hfov, float vfov) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_CAMERA_FOV_STATUS_LEN]; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_int32_t(buf, 4, lat_camera); + _mav_put_int32_t(buf, 8, lon_camera); + _mav_put_int32_t(buf, 12, alt_camera); + _mav_put_int32_t(buf, 16, lat_image); + _mav_put_int32_t(buf, 20, lon_image); + _mav_put_int32_t(buf, 24, alt_image); + _mav_put_float(buf, 44, hfov); + _mav_put_float(buf, 48, vfov); + _mav_put_float_array(buf, 28, q, 4); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_CAMERA_FOV_STATUS_LEN); +#else + mavlink_camera_fov_status_t packet; + packet.time_boot_ms = time_boot_ms; + packet.lat_camera = lat_camera; + packet.lon_camera = lon_camera; + packet.alt_camera = alt_camera; + packet.lat_image = lat_image; + packet.lon_image = lon_image; + packet.alt_image = alt_image; + packet.hfov = hfov; + packet.vfov = vfov; + mav_array_memcpy(packet.q, q, sizeof(float)*4); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_CAMERA_FOV_STATUS_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_CAMERA_FOV_STATUS; + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_CAMERA_FOV_STATUS_MIN_LEN, MAVLINK_MSG_ID_CAMERA_FOV_STATUS_LEN, MAVLINK_MSG_ID_CAMERA_FOV_STATUS_CRC); +} + +/** + * @brief Pack a camera_fov_status message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param time_boot_ms [ms] Timestamp (time since system boot). + * @param lat_camera [degE7] Latitude of camera (INT32_MAX if unknown). + * @param lon_camera [degE7] Longitude of camera (INT32_MAX if unknown). + * @param alt_camera [mm] Altitude (MSL) of camera (INT32_MAX if unknown). + * @param lat_image [degE7] Latitude of center of image (INT32_MAX if unknown, INT32_MIN if at infinity, not intersecting with horizon). + * @param lon_image [degE7] Longitude of center of image (INT32_MAX if unknown, INT32_MIN if at infinity, not intersecting with horizon). + * @param alt_image [mm] Altitude (MSL) of center of image (INT32_MAX if unknown, INT32_MIN if at infinity, not intersecting with horizon). + * @param q Quaternion of camera orientation (w, x, y, z order, zero-rotation is 1, 0, 0, 0) + * @param hfov [deg] Horizontal field of view (NaN if unknown). + * @param vfov [deg] Vertical field of view (NaN if unknown). + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_camera_fov_status_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint32_t time_boot_ms,int32_t lat_camera,int32_t lon_camera,int32_t alt_camera,int32_t lat_image,int32_t lon_image,int32_t alt_image,const float *q,float hfov,float vfov) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_CAMERA_FOV_STATUS_LEN]; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_int32_t(buf, 4, lat_camera); + _mav_put_int32_t(buf, 8, lon_camera); + _mav_put_int32_t(buf, 12, alt_camera); + _mav_put_int32_t(buf, 16, lat_image); + _mav_put_int32_t(buf, 20, lon_image); + _mav_put_int32_t(buf, 24, alt_image); + _mav_put_float(buf, 44, hfov); + _mav_put_float(buf, 48, vfov); + _mav_put_float_array(buf, 28, q, 4); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_CAMERA_FOV_STATUS_LEN); +#else + mavlink_camera_fov_status_t packet; + packet.time_boot_ms = time_boot_ms; + packet.lat_camera = lat_camera; + packet.lon_camera = lon_camera; + packet.alt_camera = alt_camera; + packet.lat_image = lat_image; + packet.lon_image = lon_image; + packet.alt_image = alt_image; + packet.hfov = hfov; + packet.vfov = vfov; + mav_array_memcpy(packet.q, q, sizeof(float)*4); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_CAMERA_FOV_STATUS_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_CAMERA_FOV_STATUS; + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_CAMERA_FOV_STATUS_MIN_LEN, MAVLINK_MSG_ID_CAMERA_FOV_STATUS_LEN, MAVLINK_MSG_ID_CAMERA_FOV_STATUS_CRC); +} + +/** + * @brief Encode a camera_fov_status struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param camera_fov_status C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_camera_fov_status_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_camera_fov_status_t* camera_fov_status) +{ + return mavlink_msg_camera_fov_status_pack(system_id, component_id, msg, camera_fov_status->time_boot_ms, camera_fov_status->lat_camera, camera_fov_status->lon_camera, camera_fov_status->alt_camera, camera_fov_status->lat_image, camera_fov_status->lon_image, camera_fov_status->alt_image, camera_fov_status->q, camera_fov_status->hfov, camera_fov_status->vfov); +} + +/** + * @brief Encode a camera_fov_status struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param camera_fov_status C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_camera_fov_status_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_camera_fov_status_t* camera_fov_status) +{ + return mavlink_msg_camera_fov_status_pack_chan(system_id, component_id, chan, msg, camera_fov_status->time_boot_ms, camera_fov_status->lat_camera, camera_fov_status->lon_camera, camera_fov_status->alt_camera, camera_fov_status->lat_image, camera_fov_status->lon_image, camera_fov_status->alt_image, camera_fov_status->q, camera_fov_status->hfov, camera_fov_status->vfov); +} + +/** + * @brief Send a camera_fov_status message + * @param chan MAVLink channel to send the message + * + * @param time_boot_ms [ms] Timestamp (time since system boot). + * @param lat_camera [degE7] Latitude of camera (INT32_MAX if unknown). + * @param lon_camera [degE7] Longitude of camera (INT32_MAX if unknown). + * @param alt_camera [mm] Altitude (MSL) of camera (INT32_MAX if unknown). + * @param lat_image [degE7] Latitude of center of image (INT32_MAX if unknown, INT32_MIN if at infinity, not intersecting with horizon). + * @param lon_image [degE7] Longitude of center of image (INT32_MAX if unknown, INT32_MIN if at infinity, not intersecting with horizon). + * @param alt_image [mm] Altitude (MSL) of center of image (INT32_MAX if unknown, INT32_MIN if at infinity, not intersecting with horizon). + * @param q Quaternion of camera orientation (w, x, y, z order, zero-rotation is 1, 0, 0, 0) + * @param hfov [deg] Horizontal field of view (NaN if unknown). + * @param vfov [deg] Vertical field of view (NaN if unknown). + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_camera_fov_status_send(mavlink_channel_t chan, uint32_t time_boot_ms, int32_t lat_camera, int32_t lon_camera, int32_t alt_camera, int32_t lat_image, int32_t lon_image, int32_t alt_image, const float *q, float hfov, float vfov) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_CAMERA_FOV_STATUS_LEN]; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_int32_t(buf, 4, lat_camera); + _mav_put_int32_t(buf, 8, lon_camera); + _mav_put_int32_t(buf, 12, alt_camera); + _mav_put_int32_t(buf, 16, lat_image); + _mav_put_int32_t(buf, 20, lon_image); + _mav_put_int32_t(buf, 24, alt_image); + _mav_put_float(buf, 44, hfov); + _mav_put_float(buf, 48, vfov); + _mav_put_float_array(buf, 28, q, 4); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CAMERA_FOV_STATUS, buf, MAVLINK_MSG_ID_CAMERA_FOV_STATUS_MIN_LEN, MAVLINK_MSG_ID_CAMERA_FOV_STATUS_LEN, MAVLINK_MSG_ID_CAMERA_FOV_STATUS_CRC); +#else + mavlink_camera_fov_status_t packet; + packet.time_boot_ms = time_boot_ms; + packet.lat_camera = lat_camera; + packet.lon_camera = lon_camera; + packet.alt_camera = alt_camera; + packet.lat_image = lat_image; + packet.lon_image = lon_image; + packet.alt_image = alt_image; + packet.hfov = hfov; + packet.vfov = vfov; + mav_array_memcpy(packet.q, q, sizeof(float)*4); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CAMERA_FOV_STATUS, (const char *)&packet, MAVLINK_MSG_ID_CAMERA_FOV_STATUS_MIN_LEN, MAVLINK_MSG_ID_CAMERA_FOV_STATUS_LEN, MAVLINK_MSG_ID_CAMERA_FOV_STATUS_CRC); +#endif +} + +/** + * @brief Send a camera_fov_status message + * @param chan MAVLink channel to send the message + * @param struct The MAVLink struct to serialize + */ +static inline void mavlink_msg_camera_fov_status_send_struct(mavlink_channel_t chan, const mavlink_camera_fov_status_t* camera_fov_status) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + mavlink_msg_camera_fov_status_send(chan, camera_fov_status->time_boot_ms, camera_fov_status->lat_camera, camera_fov_status->lon_camera, camera_fov_status->alt_camera, camera_fov_status->lat_image, camera_fov_status->lon_image, camera_fov_status->alt_image, camera_fov_status->q, camera_fov_status->hfov, camera_fov_status->vfov); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CAMERA_FOV_STATUS, (const char *)camera_fov_status, MAVLINK_MSG_ID_CAMERA_FOV_STATUS_MIN_LEN, MAVLINK_MSG_ID_CAMERA_FOV_STATUS_LEN, MAVLINK_MSG_ID_CAMERA_FOV_STATUS_CRC); +#endif +} + +#if MAVLINK_MSG_ID_CAMERA_FOV_STATUS_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_camera_fov_status_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint32_t time_boot_ms, int32_t lat_camera, int32_t lon_camera, int32_t alt_camera, int32_t lat_image, int32_t lon_image, int32_t alt_image, const float *q, float hfov, float vfov) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_int32_t(buf, 4, lat_camera); + _mav_put_int32_t(buf, 8, lon_camera); + _mav_put_int32_t(buf, 12, alt_camera); + _mav_put_int32_t(buf, 16, lat_image); + _mav_put_int32_t(buf, 20, lon_image); + _mav_put_int32_t(buf, 24, alt_image); + _mav_put_float(buf, 44, hfov); + _mav_put_float(buf, 48, vfov); + _mav_put_float_array(buf, 28, q, 4); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CAMERA_FOV_STATUS, buf, MAVLINK_MSG_ID_CAMERA_FOV_STATUS_MIN_LEN, MAVLINK_MSG_ID_CAMERA_FOV_STATUS_LEN, MAVLINK_MSG_ID_CAMERA_FOV_STATUS_CRC); +#else + mavlink_camera_fov_status_t *packet = (mavlink_camera_fov_status_t *)msgbuf; + packet->time_boot_ms = time_boot_ms; + packet->lat_camera = lat_camera; + packet->lon_camera = lon_camera; + packet->alt_camera = alt_camera; + packet->lat_image = lat_image; + packet->lon_image = lon_image; + packet->alt_image = alt_image; + packet->hfov = hfov; + packet->vfov = vfov; + mav_array_memcpy(packet->q, q, sizeof(float)*4); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CAMERA_FOV_STATUS, (const char *)packet, MAVLINK_MSG_ID_CAMERA_FOV_STATUS_MIN_LEN, MAVLINK_MSG_ID_CAMERA_FOV_STATUS_LEN, MAVLINK_MSG_ID_CAMERA_FOV_STATUS_CRC); +#endif +} +#endif + +#endif + +// MESSAGE CAMERA_FOV_STATUS UNPACKING + + +/** + * @brief Get field time_boot_ms from camera_fov_status message + * + * @return [ms] Timestamp (time since system boot). + */ +static inline uint32_t mavlink_msg_camera_fov_status_get_time_boot_ms(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint32_t(msg, 0); +} + +/** + * @brief Get field lat_camera from camera_fov_status message + * + * @return [degE7] Latitude of camera (INT32_MAX if unknown). + */ +static inline int32_t mavlink_msg_camera_fov_status_get_lat_camera(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int32_t(msg, 4); +} + +/** + * @brief Get field lon_camera from camera_fov_status message + * + * @return [degE7] Longitude of camera (INT32_MAX if unknown). + */ +static inline int32_t mavlink_msg_camera_fov_status_get_lon_camera(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int32_t(msg, 8); +} + +/** + * @brief Get field alt_camera from camera_fov_status message + * + * @return [mm] Altitude (MSL) of camera (INT32_MAX if unknown). + */ +static inline int32_t mavlink_msg_camera_fov_status_get_alt_camera(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int32_t(msg, 12); +} + +/** + * @brief Get field lat_image from camera_fov_status message + * + * @return [degE7] Latitude of center of image (INT32_MAX if unknown, INT32_MIN if at infinity, not intersecting with horizon). + */ +static inline int32_t mavlink_msg_camera_fov_status_get_lat_image(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int32_t(msg, 16); +} + +/** + * @brief Get field lon_image from camera_fov_status message + * + * @return [degE7] Longitude of center of image (INT32_MAX if unknown, INT32_MIN if at infinity, not intersecting with horizon). + */ +static inline int32_t mavlink_msg_camera_fov_status_get_lon_image(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int32_t(msg, 20); +} + +/** + * @brief Get field alt_image from camera_fov_status message + * + * @return [mm] Altitude (MSL) of center of image (INT32_MAX if unknown, INT32_MIN if at infinity, not intersecting with horizon). + */ +static inline int32_t mavlink_msg_camera_fov_status_get_alt_image(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int32_t(msg, 24); +} + +/** + * @brief Get field q from camera_fov_status message + * + * @return Quaternion of camera orientation (w, x, y, z order, zero-rotation is 1, 0, 0, 0) + */ +static inline uint16_t mavlink_msg_camera_fov_status_get_q(const mavlink_message_t* msg, float *q) +{ + return _MAV_RETURN_float_array(msg, q, 4, 28); +} + +/** + * @brief Get field hfov from camera_fov_status message + * + * @return [deg] Horizontal field of view (NaN if unknown). + */ +static inline float mavlink_msg_camera_fov_status_get_hfov(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 44); +} + +/** + * @brief Get field vfov from camera_fov_status message + * + * @return [deg] Vertical field of view (NaN if unknown). + */ +static inline float mavlink_msg_camera_fov_status_get_vfov(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 48); +} + +/** + * @brief Decode a camera_fov_status message into a struct + * + * @param msg The message to decode + * @param camera_fov_status C-struct to decode the message contents into + */ +static inline void mavlink_msg_camera_fov_status_decode(const mavlink_message_t* msg, mavlink_camera_fov_status_t* camera_fov_status) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + camera_fov_status->time_boot_ms = mavlink_msg_camera_fov_status_get_time_boot_ms(msg); + camera_fov_status->lat_camera = mavlink_msg_camera_fov_status_get_lat_camera(msg); + camera_fov_status->lon_camera = mavlink_msg_camera_fov_status_get_lon_camera(msg); + camera_fov_status->alt_camera = mavlink_msg_camera_fov_status_get_alt_camera(msg); + camera_fov_status->lat_image = mavlink_msg_camera_fov_status_get_lat_image(msg); + camera_fov_status->lon_image = mavlink_msg_camera_fov_status_get_lon_image(msg); + camera_fov_status->alt_image = mavlink_msg_camera_fov_status_get_alt_image(msg); + mavlink_msg_camera_fov_status_get_q(msg, camera_fov_status->q); + camera_fov_status->hfov = mavlink_msg_camera_fov_status_get_hfov(msg); + camera_fov_status->vfov = mavlink_msg_camera_fov_status_get_vfov(msg); +#else + uint8_t len = msg->len < MAVLINK_MSG_ID_CAMERA_FOV_STATUS_LEN? msg->len : MAVLINK_MSG_ID_CAMERA_FOV_STATUS_LEN; + memset(camera_fov_status, 0, MAVLINK_MSG_ID_CAMERA_FOV_STATUS_LEN); + memcpy(camera_fov_status, _MAV_PAYLOAD(msg), len); +#endif +} diff --git a/lib/main/MAVLink/common/mavlink_msg_camera_image_captured.h b/lib/main/MAVLink/common/mavlink_msg_camera_image_captured.h new file mode 100644 index 0000000000..01d772b62a --- /dev/null +++ b/lib/main/MAVLink/common/mavlink_msg_camera_image_captured.h @@ -0,0 +1,456 @@ +#pragma once +// MESSAGE CAMERA_IMAGE_CAPTURED PACKING + +#define MAVLINK_MSG_ID_CAMERA_IMAGE_CAPTURED 263 + + +typedef struct __mavlink_camera_image_captured_t { + uint64_t time_utc; /*< [us] Timestamp (time since UNIX epoch) in UTC. 0 for unknown.*/ + uint32_t time_boot_ms; /*< [ms] Timestamp (time since system boot).*/ + int32_t lat; /*< [degE7] Latitude where image was taken*/ + int32_t lon; /*< [degE7] Longitude where capture was taken*/ + int32_t alt; /*< [mm] Altitude (MSL) where image was taken*/ + int32_t relative_alt; /*< [mm] Altitude above ground*/ + float q[4]; /*< Quaternion of camera orientation (w, x, y, z order, zero-rotation is 1, 0, 0, 0)*/ + int32_t image_index; /*< Zero based index of this image (i.e. a new image will have index CAMERA_CAPTURE_STATUS.image count -1)*/ + uint8_t camera_id; /*< Deprecated/unused. Component IDs are used to differentiate multiple cameras.*/ + int8_t capture_result; /*< Boolean indicating success (1) or failure (0) while capturing this image.*/ + char file_url[205]; /*< URL of image taken. Either local storage or http://foo.jpg if camera provides an HTTP interface.*/ +} mavlink_camera_image_captured_t; + +#define MAVLINK_MSG_ID_CAMERA_IMAGE_CAPTURED_LEN 255 +#define MAVLINK_MSG_ID_CAMERA_IMAGE_CAPTURED_MIN_LEN 255 +#define MAVLINK_MSG_ID_263_LEN 255 +#define MAVLINK_MSG_ID_263_MIN_LEN 255 + +#define MAVLINK_MSG_ID_CAMERA_IMAGE_CAPTURED_CRC 133 +#define MAVLINK_MSG_ID_263_CRC 133 + +#define MAVLINK_MSG_CAMERA_IMAGE_CAPTURED_FIELD_Q_LEN 4 +#define MAVLINK_MSG_CAMERA_IMAGE_CAPTURED_FIELD_FILE_URL_LEN 205 + +#if MAVLINK_COMMAND_24BIT +#define MAVLINK_MESSAGE_INFO_CAMERA_IMAGE_CAPTURED { \ + 263, \ + "CAMERA_IMAGE_CAPTURED", \ + 11, \ + { { "time_boot_ms", NULL, MAVLINK_TYPE_UINT32_T, 0, 8, offsetof(mavlink_camera_image_captured_t, time_boot_ms) }, \ + { "time_utc", NULL, MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_camera_image_captured_t, time_utc) }, \ + { "camera_id", NULL, MAVLINK_TYPE_UINT8_T, 0, 48, offsetof(mavlink_camera_image_captured_t, camera_id) }, \ + { "lat", NULL, MAVLINK_TYPE_INT32_T, 0, 12, offsetof(mavlink_camera_image_captured_t, lat) }, \ + { "lon", NULL, MAVLINK_TYPE_INT32_T, 0, 16, offsetof(mavlink_camera_image_captured_t, lon) }, \ + { "alt", NULL, MAVLINK_TYPE_INT32_T, 0, 20, offsetof(mavlink_camera_image_captured_t, alt) }, \ + { "relative_alt", NULL, MAVLINK_TYPE_INT32_T, 0, 24, offsetof(mavlink_camera_image_captured_t, relative_alt) }, \ + { "q", NULL, MAVLINK_TYPE_FLOAT, 4, 28, offsetof(mavlink_camera_image_captured_t, q) }, \ + { "image_index", NULL, MAVLINK_TYPE_INT32_T, 0, 44, offsetof(mavlink_camera_image_captured_t, image_index) }, \ + { "capture_result", NULL, MAVLINK_TYPE_INT8_T, 0, 49, offsetof(mavlink_camera_image_captured_t, capture_result) }, \ + { "file_url", NULL, MAVLINK_TYPE_CHAR, 205, 50, offsetof(mavlink_camera_image_captured_t, file_url) }, \ + } \ +} +#else +#define MAVLINK_MESSAGE_INFO_CAMERA_IMAGE_CAPTURED { \ + "CAMERA_IMAGE_CAPTURED", \ + 11, \ + { { "time_boot_ms", NULL, MAVLINK_TYPE_UINT32_T, 0, 8, offsetof(mavlink_camera_image_captured_t, time_boot_ms) }, \ + { "time_utc", NULL, MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_camera_image_captured_t, time_utc) }, \ + { "camera_id", NULL, MAVLINK_TYPE_UINT8_T, 0, 48, offsetof(mavlink_camera_image_captured_t, camera_id) }, \ + { "lat", NULL, MAVLINK_TYPE_INT32_T, 0, 12, offsetof(mavlink_camera_image_captured_t, lat) }, \ + { "lon", NULL, MAVLINK_TYPE_INT32_T, 0, 16, offsetof(mavlink_camera_image_captured_t, lon) }, \ + { "alt", NULL, MAVLINK_TYPE_INT32_T, 0, 20, offsetof(mavlink_camera_image_captured_t, alt) }, \ + { "relative_alt", NULL, MAVLINK_TYPE_INT32_T, 0, 24, offsetof(mavlink_camera_image_captured_t, relative_alt) }, \ + { "q", NULL, MAVLINK_TYPE_FLOAT, 4, 28, offsetof(mavlink_camera_image_captured_t, q) }, \ + { "image_index", NULL, MAVLINK_TYPE_INT32_T, 0, 44, offsetof(mavlink_camera_image_captured_t, image_index) }, \ + { "capture_result", NULL, MAVLINK_TYPE_INT8_T, 0, 49, offsetof(mavlink_camera_image_captured_t, capture_result) }, \ + { "file_url", NULL, MAVLINK_TYPE_CHAR, 205, 50, offsetof(mavlink_camera_image_captured_t, file_url) }, \ + } \ +} +#endif + +/** + * @brief Pack a camera_image_captured message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param time_boot_ms [ms] Timestamp (time since system boot). + * @param time_utc [us] Timestamp (time since UNIX epoch) in UTC. 0 for unknown. + * @param camera_id Deprecated/unused. Component IDs are used to differentiate multiple cameras. + * @param lat [degE7] Latitude where image was taken + * @param lon [degE7] Longitude where capture was taken + * @param alt [mm] Altitude (MSL) where image was taken + * @param relative_alt [mm] Altitude above ground + * @param q Quaternion of camera orientation (w, x, y, z order, zero-rotation is 1, 0, 0, 0) + * @param image_index Zero based index of this image (i.e. a new image will have index CAMERA_CAPTURE_STATUS.image count -1) + * @param capture_result Boolean indicating success (1) or failure (0) while capturing this image. + * @param file_url URL of image taken. Either local storage or http://foo.jpg if camera provides an HTTP interface. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_camera_image_captured_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint32_t time_boot_ms, uint64_t time_utc, uint8_t camera_id, int32_t lat, int32_t lon, int32_t alt, int32_t relative_alt, const float *q, int32_t image_index, int8_t capture_result, const char *file_url) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_CAMERA_IMAGE_CAPTURED_LEN]; + _mav_put_uint64_t(buf, 0, time_utc); + _mav_put_uint32_t(buf, 8, time_boot_ms); + _mav_put_int32_t(buf, 12, lat); + _mav_put_int32_t(buf, 16, lon); + _mav_put_int32_t(buf, 20, alt); + _mav_put_int32_t(buf, 24, relative_alt); + _mav_put_int32_t(buf, 44, image_index); + _mav_put_uint8_t(buf, 48, camera_id); + _mav_put_int8_t(buf, 49, capture_result); + _mav_put_float_array(buf, 28, q, 4); + _mav_put_char_array(buf, 50, file_url, 205); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_CAMERA_IMAGE_CAPTURED_LEN); +#else + mavlink_camera_image_captured_t packet; + packet.time_utc = time_utc; + packet.time_boot_ms = time_boot_ms; + packet.lat = lat; + packet.lon = lon; + packet.alt = alt; + packet.relative_alt = relative_alt; + packet.image_index = image_index; + packet.camera_id = camera_id; + packet.capture_result = capture_result; + mav_array_memcpy(packet.q, q, sizeof(float)*4); + mav_array_memcpy(packet.file_url, file_url, sizeof(char)*205); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_CAMERA_IMAGE_CAPTURED_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_CAMERA_IMAGE_CAPTURED; + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_CAMERA_IMAGE_CAPTURED_MIN_LEN, MAVLINK_MSG_ID_CAMERA_IMAGE_CAPTURED_LEN, MAVLINK_MSG_ID_CAMERA_IMAGE_CAPTURED_CRC); +} + +/** + * @brief Pack a camera_image_captured message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param time_boot_ms [ms] Timestamp (time since system boot). + * @param time_utc [us] Timestamp (time since UNIX epoch) in UTC. 0 for unknown. + * @param camera_id Deprecated/unused. Component IDs are used to differentiate multiple cameras. + * @param lat [degE7] Latitude where image was taken + * @param lon [degE7] Longitude where capture was taken + * @param alt [mm] Altitude (MSL) where image was taken + * @param relative_alt [mm] Altitude above ground + * @param q Quaternion of camera orientation (w, x, y, z order, zero-rotation is 1, 0, 0, 0) + * @param image_index Zero based index of this image (i.e. a new image will have index CAMERA_CAPTURE_STATUS.image count -1) + * @param capture_result Boolean indicating success (1) or failure (0) while capturing this image. + * @param file_url URL of image taken. Either local storage or http://foo.jpg if camera provides an HTTP interface. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_camera_image_captured_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint32_t time_boot_ms,uint64_t time_utc,uint8_t camera_id,int32_t lat,int32_t lon,int32_t alt,int32_t relative_alt,const float *q,int32_t image_index,int8_t capture_result,const char *file_url) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_CAMERA_IMAGE_CAPTURED_LEN]; + _mav_put_uint64_t(buf, 0, time_utc); + _mav_put_uint32_t(buf, 8, time_boot_ms); + _mav_put_int32_t(buf, 12, lat); + _mav_put_int32_t(buf, 16, lon); + _mav_put_int32_t(buf, 20, alt); + _mav_put_int32_t(buf, 24, relative_alt); + _mav_put_int32_t(buf, 44, image_index); + _mav_put_uint8_t(buf, 48, camera_id); + _mav_put_int8_t(buf, 49, capture_result); + _mav_put_float_array(buf, 28, q, 4); + _mav_put_char_array(buf, 50, file_url, 205); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_CAMERA_IMAGE_CAPTURED_LEN); +#else + mavlink_camera_image_captured_t packet; + packet.time_utc = time_utc; + packet.time_boot_ms = time_boot_ms; + packet.lat = lat; + packet.lon = lon; + packet.alt = alt; + packet.relative_alt = relative_alt; + packet.image_index = image_index; + packet.camera_id = camera_id; + packet.capture_result = capture_result; + mav_array_memcpy(packet.q, q, sizeof(float)*4); + mav_array_memcpy(packet.file_url, file_url, sizeof(char)*205); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_CAMERA_IMAGE_CAPTURED_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_CAMERA_IMAGE_CAPTURED; + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_CAMERA_IMAGE_CAPTURED_MIN_LEN, MAVLINK_MSG_ID_CAMERA_IMAGE_CAPTURED_LEN, MAVLINK_MSG_ID_CAMERA_IMAGE_CAPTURED_CRC); +} + +/** + * @brief Encode a camera_image_captured struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param camera_image_captured C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_camera_image_captured_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_camera_image_captured_t* camera_image_captured) +{ + return mavlink_msg_camera_image_captured_pack(system_id, component_id, msg, camera_image_captured->time_boot_ms, camera_image_captured->time_utc, camera_image_captured->camera_id, camera_image_captured->lat, camera_image_captured->lon, camera_image_captured->alt, camera_image_captured->relative_alt, camera_image_captured->q, camera_image_captured->image_index, camera_image_captured->capture_result, camera_image_captured->file_url); +} + +/** + * @brief Encode a camera_image_captured struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param camera_image_captured C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_camera_image_captured_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_camera_image_captured_t* camera_image_captured) +{ + return mavlink_msg_camera_image_captured_pack_chan(system_id, component_id, chan, msg, camera_image_captured->time_boot_ms, camera_image_captured->time_utc, camera_image_captured->camera_id, camera_image_captured->lat, camera_image_captured->lon, camera_image_captured->alt, camera_image_captured->relative_alt, camera_image_captured->q, camera_image_captured->image_index, camera_image_captured->capture_result, camera_image_captured->file_url); +} + +/** + * @brief Send a camera_image_captured message + * @param chan MAVLink channel to send the message + * + * @param time_boot_ms [ms] Timestamp (time since system boot). + * @param time_utc [us] Timestamp (time since UNIX epoch) in UTC. 0 for unknown. + * @param camera_id Deprecated/unused. Component IDs are used to differentiate multiple cameras. + * @param lat [degE7] Latitude where image was taken + * @param lon [degE7] Longitude where capture was taken + * @param alt [mm] Altitude (MSL) where image was taken + * @param relative_alt [mm] Altitude above ground + * @param q Quaternion of camera orientation (w, x, y, z order, zero-rotation is 1, 0, 0, 0) + * @param image_index Zero based index of this image (i.e. a new image will have index CAMERA_CAPTURE_STATUS.image count -1) + * @param capture_result Boolean indicating success (1) or failure (0) while capturing this image. + * @param file_url URL of image taken. Either local storage or http://foo.jpg if camera provides an HTTP interface. + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_camera_image_captured_send(mavlink_channel_t chan, uint32_t time_boot_ms, uint64_t time_utc, uint8_t camera_id, int32_t lat, int32_t lon, int32_t alt, int32_t relative_alt, const float *q, int32_t image_index, int8_t capture_result, const char *file_url) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_CAMERA_IMAGE_CAPTURED_LEN]; + _mav_put_uint64_t(buf, 0, time_utc); + _mav_put_uint32_t(buf, 8, time_boot_ms); + _mav_put_int32_t(buf, 12, lat); + _mav_put_int32_t(buf, 16, lon); + _mav_put_int32_t(buf, 20, alt); + _mav_put_int32_t(buf, 24, relative_alt); + _mav_put_int32_t(buf, 44, image_index); + _mav_put_uint8_t(buf, 48, camera_id); + _mav_put_int8_t(buf, 49, capture_result); + _mav_put_float_array(buf, 28, q, 4); + _mav_put_char_array(buf, 50, file_url, 205); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CAMERA_IMAGE_CAPTURED, buf, MAVLINK_MSG_ID_CAMERA_IMAGE_CAPTURED_MIN_LEN, MAVLINK_MSG_ID_CAMERA_IMAGE_CAPTURED_LEN, MAVLINK_MSG_ID_CAMERA_IMAGE_CAPTURED_CRC); +#else + mavlink_camera_image_captured_t packet; + packet.time_utc = time_utc; + packet.time_boot_ms = time_boot_ms; + packet.lat = lat; + packet.lon = lon; + packet.alt = alt; + packet.relative_alt = relative_alt; + packet.image_index = image_index; + packet.camera_id = camera_id; + packet.capture_result = capture_result; + mav_array_memcpy(packet.q, q, sizeof(float)*4); + mav_array_memcpy(packet.file_url, file_url, sizeof(char)*205); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CAMERA_IMAGE_CAPTURED, (const char *)&packet, MAVLINK_MSG_ID_CAMERA_IMAGE_CAPTURED_MIN_LEN, MAVLINK_MSG_ID_CAMERA_IMAGE_CAPTURED_LEN, MAVLINK_MSG_ID_CAMERA_IMAGE_CAPTURED_CRC); +#endif +} + +/** + * @brief Send a camera_image_captured message + * @param chan MAVLink channel to send the message + * @param struct The MAVLink struct to serialize + */ +static inline void mavlink_msg_camera_image_captured_send_struct(mavlink_channel_t chan, const mavlink_camera_image_captured_t* camera_image_captured) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + mavlink_msg_camera_image_captured_send(chan, camera_image_captured->time_boot_ms, camera_image_captured->time_utc, camera_image_captured->camera_id, camera_image_captured->lat, camera_image_captured->lon, camera_image_captured->alt, camera_image_captured->relative_alt, camera_image_captured->q, camera_image_captured->image_index, camera_image_captured->capture_result, camera_image_captured->file_url); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CAMERA_IMAGE_CAPTURED, (const char *)camera_image_captured, MAVLINK_MSG_ID_CAMERA_IMAGE_CAPTURED_MIN_LEN, MAVLINK_MSG_ID_CAMERA_IMAGE_CAPTURED_LEN, MAVLINK_MSG_ID_CAMERA_IMAGE_CAPTURED_CRC); +#endif +} + +#if MAVLINK_MSG_ID_CAMERA_IMAGE_CAPTURED_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_camera_image_captured_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint32_t time_boot_ms, uint64_t time_utc, uint8_t camera_id, int32_t lat, int32_t lon, int32_t alt, int32_t relative_alt, const float *q, int32_t image_index, int8_t capture_result, const char *file_url) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint64_t(buf, 0, time_utc); + _mav_put_uint32_t(buf, 8, time_boot_ms); + _mav_put_int32_t(buf, 12, lat); + _mav_put_int32_t(buf, 16, lon); + _mav_put_int32_t(buf, 20, alt); + _mav_put_int32_t(buf, 24, relative_alt); + _mav_put_int32_t(buf, 44, image_index); + _mav_put_uint8_t(buf, 48, camera_id); + _mav_put_int8_t(buf, 49, capture_result); + _mav_put_float_array(buf, 28, q, 4); + _mav_put_char_array(buf, 50, file_url, 205); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CAMERA_IMAGE_CAPTURED, buf, MAVLINK_MSG_ID_CAMERA_IMAGE_CAPTURED_MIN_LEN, MAVLINK_MSG_ID_CAMERA_IMAGE_CAPTURED_LEN, MAVLINK_MSG_ID_CAMERA_IMAGE_CAPTURED_CRC); +#else + mavlink_camera_image_captured_t *packet = (mavlink_camera_image_captured_t *)msgbuf; + packet->time_utc = time_utc; + packet->time_boot_ms = time_boot_ms; + packet->lat = lat; + packet->lon = lon; + packet->alt = alt; + packet->relative_alt = relative_alt; + packet->image_index = image_index; + packet->camera_id = camera_id; + packet->capture_result = capture_result; + mav_array_memcpy(packet->q, q, sizeof(float)*4); + mav_array_memcpy(packet->file_url, file_url, sizeof(char)*205); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CAMERA_IMAGE_CAPTURED, (const char *)packet, MAVLINK_MSG_ID_CAMERA_IMAGE_CAPTURED_MIN_LEN, MAVLINK_MSG_ID_CAMERA_IMAGE_CAPTURED_LEN, MAVLINK_MSG_ID_CAMERA_IMAGE_CAPTURED_CRC); +#endif +} +#endif + +#endif + +// MESSAGE CAMERA_IMAGE_CAPTURED UNPACKING + + +/** + * @brief Get field time_boot_ms from camera_image_captured message + * + * @return [ms] Timestamp (time since system boot). + */ +static inline uint32_t mavlink_msg_camera_image_captured_get_time_boot_ms(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint32_t(msg, 8); +} + +/** + * @brief Get field time_utc from camera_image_captured message + * + * @return [us] Timestamp (time since UNIX epoch) in UTC. 0 for unknown. + */ +static inline uint64_t mavlink_msg_camera_image_captured_get_time_utc(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint64_t(msg, 0); +} + +/** + * @brief Get field camera_id from camera_image_captured message + * + * @return Deprecated/unused. Component IDs are used to differentiate multiple cameras. + */ +static inline uint8_t mavlink_msg_camera_image_captured_get_camera_id(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 48); +} + +/** + * @brief Get field lat from camera_image_captured message + * + * @return [degE7] Latitude where image was taken + */ +static inline int32_t mavlink_msg_camera_image_captured_get_lat(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int32_t(msg, 12); +} + +/** + * @brief Get field lon from camera_image_captured message + * + * @return [degE7] Longitude where capture was taken + */ +static inline int32_t mavlink_msg_camera_image_captured_get_lon(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int32_t(msg, 16); +} + +/** + * @brief Get field alt from camera_image_captured message + * + * @return [mm] Altitude (MSL) where image was taken + */ +static inline int32_t mavlink_msg_camera_image_captured_get_alt(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int32_t(msg, 20); +} + +/** + * @brief Get field relative_alt from camera_image_captured message + * + * @return [mm] Altitude above ground + */ +static inline int32_t mavlink_msg_camera_image_captured_get_relative_alt(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int32_t(msg, 24); +} + +/** + * @brief Get field q from camera_image_captured message + * + * @return Quaternion of camera orientation (w, x, y, z order, zero-rotation is 1, 0, 0, 0) + */ +static inline uint16_t mavlink_msg_camera_image_captured_get_q(const mavlink_message_t* msg, float *q) +{ + return _MAV_RETURN_float_array(msg, q, 4, 28); +} + +/** + * @brief Get field image_index from camera_image_captured message + * + * @return Zero based index of this image (i.e. a new image will have index CAMERA_CAPTURE_STATUS.image count -1) + */ +static inline int32_t mavlink_msg_camera_image_captured_get_image_index(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int32_t(msg, 44); +} + +/** + * @brief Get field capture_result from camera_image_captured message + * + * @return Boolean indicating success (1) or failure (0) while capturing this image. + */ +static inline int8_t mavlink_msg_camera_image_captured_get_capture_result(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int8_t(msg, 49); +} + +/** + * @brief Get field file_url from camera_image_captured message + * + * @return URL of image taken. Either local storage or http://foo.jpg if camera provides an HTTP interface. + */ +static inline uint16_t mavlink_msg_camera_image_captured_get_file_url(const mavlink_message_t* msg, char *file_url) +{ + return _MAV_RETURN_char_array(msg, file_url, 205, 50); +} + +/** + * @brief Decode a camera_image_captured message into a struct + * + * @param msg The message to decode + * @param camera_image_captured C-struct to decode the message contents into + */ +static inline void mavlink_msg_camera_image_captured_decode(const mavlink_message_t* msg, mavlink_camera_image_captured_t* camera_image_captured) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + camera_image_captured->time_utc = mavlink_msg_camera_image_captured_get_time_utc(msg); + camera_image_captured->time_boot_ms = mavlink_msg_camera_image_captured_get_time_boot_ms(msg); + camera_image_captured->lat = mavlink_msg_camera_image_captured_get_lat(msg); + camera_image_captured->lon = mavlink_msg_camera_image_captured_get_lon(msg); + camera_image_captured->alt = mavlink_msg_camera_image_captured_get_alt(msg); + camera_image_captured->relative_alt = mavlink_msg_camera_image_captured_get_relative_alt(msg); + mavlink_msg_camera_image_captured_get_q(msg, camera_image_captured->q); + camera_image_captured->image_index = mavlink_msg_camera_image_captured_get_image_index(msg); + camera_image_captured->camera_id = mavlink_msg_camera_image_captured_get_camera_id(msg); + camera_image_captured->capture_result = mavlink_msg_camera_image_captured_get_capture_result(msg); + mavlink_msg_camera_image_captured_get_file_url(msg, camera_image_captured->file_url); +#else + uint8_t len = msg->len < MAVLINK_MSG_ID_CAMERA_IMAGE_CAPTURED_LEN? msg->len : MAVLINK_MSG_ID_CAMERA_IMAGE_CAPTURED_LEN; + memset(camera_image_captured, 0, MAVLINK_MSG_ID_CAMERA_IMAGE_CAPTURED_LEN); + memcpy(camera_image_captured, _MAV_PAYLOAD(msg), len); +#endif +} diff --git a/lib/main/MAVLink/common/mavlink_msg_camera_information.h b/lib/main/MAVLink/common/mavlink_msg_camera_information.h new file mode 100644 index 0000000000..0ae20c4b3e --- /dev/null +++ b/lib/main/MAVLink/common/mavlink_msg_camera_information.h @@ -0,0 +1,507 @@ +#pragma once +// MESSAGE CAMERA_INFORMATION PACKING + +#define MAVLINK_MSG_ID_CAMERA_INFORMATION 259 + + +typedef struct __mavlink_camera_information_t { + uint32_t time_boot_ms; /*< [ms] Timestamp (time since system boot).*/ + uint32_t firmware_version; /*< Version of the camera firmware, encoded as: (Dev & 0xff) << 24 | (Patch & 0xff) << 16 | (Minor & 0xff) << 8 | (Major & 0xff)*/ + float focal_length; /*< [mm] Focal length*/ + float sensor_size_h; /*< [mm] Image sensor size horizontal*/ + float sensor_size_v; /*< [mm] Image sensor size vertical*/ + uint32_t flags; /*< Bitmap of camera capability flags.*/ + uint16_t resolution_h; /*< [pix] Horizontal image resolution*/ + uint16_t resolution_v; /*< [pix] Vertical image resolution*/ + uint16_t cam_definition_version; /*< Camera definition version (iteration)*/ + uint8_t vendor_name[32]; /*< Name of the camera vendor*/ + uint8_t model_name[32]; /*< Name of the camera model*/ + uint8_t lens_id; /*< Reserved for a lens ID*/ + char cam_definition_uri[140]; /*< Camera definition URI (if any, otherwise only basic functions will be available). HTTP- (http://) and MAVLink FTP- (mavlinkftp://) formatted URIs are allowed (and both must be supported by any GCS that implements the Camera Protocol).*/ +} mavlink_camera_information_t; + +#define MAVLINK_MSG_ID_CAMERA_INFORMATION_LEN 235 +#define MAVLINK_MSG_ID_CAMERA_INFORMATION_MIN_LEN 235 +#define MAVLINK_MSG_ID_259_LEN 235 +#define MAVLINK_MSG_ID_259_MIN_LEN 235 + +#define MAVLINK_MSG_ID_CAMERA_INFORMATION_CRC 92 +#define MAVLINK_MSG_ID_259_CRC 92 + +#define MAVLINK_MSG_CAMERA_INFORMATION_FIELD_VENDOR_NAME_LEN 32 +#define MAVLINK_MSG_CAMERA_INFORMATION_FIELD_MODEL_NAME_LEN 32 +#define MAVLINK_MSG_CAMERA_INFORMATION_FIELD_CAM_DEFINITION_URI_LEN 140 + +#if MAVLINK_COMMAND_24BIT +#define MAVLINK_MESSAGE_INFO_CAMERA_INFORMATION { \ + 259, \ + "CAMERA_INFORMATION", \ + 13, \ + { { "time_boot_ms", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_camera_information_t, time_boot_ms) }, \ + { "vendor_name", NULL, MAVLINK_TYPE_UINT8_T, 32, 30, offsetof(mavlink_camera_information_t, vendor_name) }, \ + { "model_name", NULL, MAVLINK_TYPE_UINT8_T, 32, 62, offsetof(mavlink_camera_information_t, model_name) }, \ + { "firmware_version", NULL, MAVLINK_TYPE_UINT32_T, 0, 4, offsetof(mavlink_camera_information_t, firmware_version) }, \ + { "focal_length", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_camera_information_t, focal_length) }, \ + { "sensor_size_h", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_camera_information_t, sensor_size_h) }, \ + { "sensor_size_v", NULL, MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_camera_information_t, sensor_size_v) }, \ + { "resolution_h", NULL, MAVLINK_TYPE_UINT16_T, 0, 24, offsetof(mavlink_camera_information_t, resolution_h) }, \ + { "resolution_v", NULL, MAVLINK_TYPE_UINT16_T, 0, 26, offsetof(mavlink_camera_information_t, resolution_v) }, \ + { "lens_id", NULL, MAVLINK_TYPE_UINT8_T, 0, 94, offsetof(mavlink_camera_information_t, lens_id) }, \ + { "flags", NULL, MAVLINK_TYPE_UINT32_T, 0, 20, offsetof(mavlink_camera_information_t, flags) }, \ + { "cam_definition_version", NULL, MAVLINK_TYPE_UINT16_T, 0, 28, offsetof(mavlink_camera_information_t, cam_definition_version) }, \ + { "cam_definition_uri", NULL, MAVLINK_TYPE_CHAR, 140, 95, offsetof(mavlink_camera_information_t, cam_definition_uri) }, \ + } \ +} +#else +#define MAVLINK_MESSAGE_INFO_CAMERA_INFORMATION { \ + "CAMERA_INFORMATION", \ + 13, \ + { { "time_boot_ms", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_camera_information_t, time_boot_ms) }, \ + { "vendor_name", NULL, MAVLINK_TYPE_UINT8_T, 32, 30, offsetof(mavlink_camera_information_t, vendor_name) }, \ + { "model_name", NULL, MAVLINK_TYPE_UINT8_T, 32, 62, offsetof(mavlink_camera_information_t, model_name) }, \ + { "firmware_version", NULL, MAVLINK_TYPE_UINT32_T, 0, 4, offsetof(mavlink_camera_information_t, firmware_version) }, \ + { "focal_length", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_camera_information_t, focal_length) }, \ + { "sensor_size_h", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_camera_information_t, sensor_size_h) }, \ + { "sensor_size_v", NULL, MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_camera_information_t, sensor_size_v) }, \ + { "resolution_h", NULL, MAVLINK_TYPE_UINT16_T, 0, 24, offsetof(mavlink_camera_information_t, resolution_h) }, \ + { "resolution_v", NULL, MAVLINK_TYPE_UINT16_T, 0, 26, offsetof(mavlink_camera_information_t, resolution_v) }, \ + { "lens_id", NULL, MAVLINK_TYPE_UINT8_T, 0, 94, offsetof(mavlink_camera_information_t, lens_id) }, \ + { "flags", NULL, MAVLINK_TYPE_UINT32_T, 0, 20, offsetof(mavlink_camera_information_t, flags) }, \ + { "cam_definition_version", NULL, MAVLINK_TYPE_UINT16_T, 0, 28, offsetof(mavlink_camera_information_t, cam_definition_version) }, \ + { "cam_definition_uri", NULL, MAVLINK_TYPE_CHAR, 140, 95, offsetof(mavlink_camera_information_t, cam_definition_uri) }, \ + } \ +} +#endif + +/** + * @brief Pack a camera_information message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param time_boot_ms [ms] Timestamp (time since system boot). + * @param vendor_name Name of the camera vendor + * @param model_name Name of the camera model + * @param firmware_version Version of the camera firmware, encoded as: (Dev & 0xff) << 24 | (Patch & 0xff) << 16 | (Minor & 0xff) << 8 | (Major & 0xff) + * @param focal_length [mm] Focal length + * @param sensor_size_h [mm] Image sensor size horizontal + * @param sensor_size_v [mm] Image sensor size vertical + * @param resolution_h [pix] Horizontal image resolution + * @param resolution_v [pix] Vertical image resolution + * @param lens_id Reserved for a lens ID + * @param flags Bitmap of camera capability flags. + * @param cam_definition_version Camera definition version (iteration) + * @param cam_definition_uri Camera definition URI (if any, otherwise only basic functions will be available). HTTP- (http://) and MAVLink FTP- (mavlinkftp://) formatted URIs are allowed (and both must be supported by any GCS that implements the Camera Protocol). + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_camera_information_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint32_t time_boot_ms, const uint8_t *vendor_name, const uint8_t *model_name, uint32_t firmware_version, float focal_length, float sensor_size_h, float sensor_size_v, uint16_t resolution_h, uint16_t resolution_v, uint8_t lens_id, uint32_t flags, uint16_t cam_definition_version, const char *cam_definition_uri) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_CAMERA_INFORMATION_LEN]; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_uint32_t(buf, 4, firmware_version); + _mav_put_float(buf, 8, focal_length); + _mav_put_float(buf, 12, sensor_size_h); + _mav_put_float(buf, 16, sensor_size_v); + _mav_put_uint32_t(buf, 20, flags); + _mav_put_uint16_t(buf, 24, resolution_h); + _mav_put_uint16_t(buf, 26, resolution_v); + _mav_put_uint16_t(buf, 28, cam_definition_version); + _mav_put_uint8_t(buf, 94, lens_id); + _mav_put_uint8_t_array(buf, 30, vendor_name, 32); + _mav_put_uint8_t_array(buf, 62, model_name, 32); + _mav_put_char_array(buf, 95, cam_definition_uri, 140); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_CAMERA_INFORMATION_LEN); +#else + mavlink_camera_information_t packet; + packet.time_boot_ms = time_boot_ms; + packet.firmware_version = firmware_version; + packet.focal_length = focal_length; + packet.sensor_size_h = sensor_size_h; + packet.sensor_size_v = sensor_size_v; + packet.flags = flags; + packet.resolution_h = resolution_h; + packet.resolution_v = resolution_v; + packet.cam_definition_version = cam_definition_version; + packet.lens_id = lens_id; + mav_array_memcpy(packet.vendor_name, vendor_name, sizeof(uint8_t)*32); + mav_array_memcpy(packet.model_name, model_name, sizeof(uint8_t)*32); + mav_array_memcpy(packet.cam_definition_uri, cam_definition_uri, sizeof(char)*140); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_CAMERA_INFORMATION_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_CAMERA_INFORMATION; + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_CAMERA_INFORMATION_MIN_LEN, MAVLINK_MSG_ID_CAMERA_INFORMATION_LEN, MAVLINK_MSG_ID_CAMERA_INFORMATION_CRC); +} + +/** + * @brief Pack a camera_information message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param time_boot_ms [ms] Timestamp (time since system boot). + * @param vendor_name Name of the camera vendor + * @param model_name Name of the camera model + * @param firmware_version Version of the camera firmware, encoded as: (Dev & 0xff) << 24 | (Patch & 0xff) << 16 | (Minor & 0xff) << 8 | (Major & 0xff) + * @param focal_length [mm] Focal length + * @param sensor_size_h [mm] Image sensor size horizontal + * @param sensor_size_v [mm] Image sensor size vertical + * @param resolution_h [pix] Horizontal image resolution + * @param resolution_v [pix] Vertical image resolution + * @param lens_id Reserved for a lens ID + * @param flags Bitmap of camera capability flags. + * @param cam_definition_version Camera definition version (iteration) + * @param cam_definition_uri Camera definition URI (if any, otherwise only basic functions will be available). HTTP- (http://) and MAVLink FTP- (mavlinkftp://) formatted URIs are allowed (and both must be supported by any GCS that implements the Camera Protocol). + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_camera_information_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint32_t time_boot_ms,const uint8_t *vendor_name,const uint8_t *model_name,uint32_t firmware_version,float focal_length,float sensor_size_h,float sensor_size_v,uint16_t resolution_h,uint16_t resolution_v,uint8_t lens_id,uint32_t flags,uint16_t cam_definition_version,const char *cam_definition_uri) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_CAMERA_INFORMATION_LEN]; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_uint32_t(buf, 4, firmware_version); + _mav_put_float(buf, 8, focal_length); + _mav_put_float(buf, 12, sensor_size_h); + _mav_put_float(buf, 16, sensor_size_v); + _mav_put_uint32_t(buf, 20, flags); + _mav_put_uint16_t(buf, 24, resolution_h); + _mav_put_uint16_t(buf, 26, resolution_v); + _mav_put_uint16_t(buf, 28, cam_definition_version); + _mav_put_uint8_t(buf, 94, lens_id); + _mav_put_uint8_t_array(buf, 30, vendor_name, 32); + _mav_put_uint8_t_array(buf, 62, model_name, 32); + _mav_put_char_array(buf, 95, cam_definition_uri, 140); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_CAMERA_INFORMATION_LEN); +#else + mavlink_camera_information_t packet; + packet.time_boot_ms = time_boot_ms; + packet.firmware_version = firmware_version; + packet.focal_length = focal_length; + packet.sensor_size_h = sensor_size_h; + packet.sensor_size_v = sensor_size_v; + packet.flags = flags; + packet.resolution_h = resolution_h; + packet.resolution_v = resolution_v; + packet.cam_definition_version = cam_definition_version; + packet.lens_id = lens_id; + mav_array_memcpy(packet.vendor_name, vendor_name, sizeof(uint8_t)*32); + mav_array_memcpy(packet.model_name, model_name, sizeof(uint8_t)*32); + mav_array_memcpy(packet.cam_definition_uri, cam_definition_uri, sizeof(char)*140); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_CAMERA_INFORMATION_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_CAMERA_INFORMATION; + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_CAMERA_INFORMATION_MIN_LEN, MAVLINK_MSG_ID_CAMERA_INFORMATION_LEN, MAVLINK_MSG_ID_CAMERA_INFORMATION_CRC); +} + +/** + * @brief Encode a camera_information struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param camera_information C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_camera_information_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_camera_information_t* camera_information) +{ + return mavlink_msg_camera_information_pack(system_id, component_id, msg, camera_information->time_boot_ms, camera_information->vendor_name, camera_information->model_name, camera_information->firmware_version, camera_information->focal_length, camera_information->sensor_size_h, camera_information->sensor_size_v, camera_information->resolution_h, camera_information->resolution_v, camera_information->lens_id, camera_information->flags, camera_information->cam_definition_version, camera_information->cam_definition_uri); +} + +/** + * @brief Encode a camera_information struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param camera_information C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_camera_information_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_camera_information_t* camera_information) +{ + return mavlink_msg_camera_information_pack_chan(system_id, component_id, chan, msg, camera_information->time_boot_ms, camera_information->vendor_name, camera_information->model_name, camera_information->firmware_version, camera_information->focal_length, camera_information->sensor_size_h, camera_information->sensor_size_v, camera_information->resolution_h, camera_information->resolution_v, camera_information->lens_id, camera_information->flags, camera_information->cam_definition_version, camera_information->cam_definition_uri); +} + +/** + * @brief Send a camera_information message + * @param chan MAVLink channel to send the message + * + * @param time_boot_ms [ms] Timestamp (time since system boot). + * @param vendor_name Name of the camera vendor + * @param model_name Name of the camera model + * @param firmware_version Version of the camera firmware, encoded as: (Dev & 0xff) << 24 | (Patch & 0xff) << 16 | (Minor & 0xff) << 8 | (Major & 0xff) + * @param focal_length [mm] Focal length + * @param sensor_size_h [mm] Image sensor size horizontal + * @param sensor_size_v [mm] Image sensor size vertical + * @param resolution_h [pix] Horizontal image resolution + * @param resolution_v [pix] Vertical image resolution + * @param lens_id Reserved for a lens ID + * @param flags Bitmap of camera capability flags. + * @param cam_definition_version Camera definition version (iteration) + * @param cam_definition_uri Camera definition URI (if any, otherwise only basic functions will be available). HTTP- (http://) and MAVLink FTP- (mavlinkftp://) formatted URIs are allowed (and both must be supported by any GCS that implements the Camera Protocol). + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_camera_information_send(mavlink_channel_t chan, uint32_t time_boot_ms, const uint8_t *vendor_name, const uint8_t *model_name, uint32_t firmware_version, float focal_length, float sensor_size_h, float sensor_size_v, uint16_t resolution_h, uint16_t resolution_v, uint8_t lens_id, uint32_t flags, uint16_t cam_definition_version, const char *cam_definition_uri) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_CAMERA_INFORMATION_LEN]; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_uint32_t(buf, 4, firmware_version); + _mav_put_float(buf, 8, focal_length); + _mav_put_float(buf, 12, sensor_size_h); + _mav_put_float(buf, 16, sensor_size_v); + _mav_put_uint32_t(buf, 20, flags); + _mav_put_uint16_t(buf, 24, resolution_h); + _mav_put_uint16_t(buf, 26, resolution_v); + _mav_put_uint16_t(buf, 28, cam_definition_version); + _mav_put_uint8_t(buf, 94, lens_id); + _mav_put_uint8_t_array(buf, 30, vendor_name, 32); + _mav_put_uint8_t_array(buf, 62, model_name, 32); + _mav_put_char_array(buf, 95, cam_definition_uri, 140); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CAMERA_INFORMATION, buf, MAVLINK_MSG_ID_CAMERA_INFORMATION_MIN_LEN, MAVLINK_MSG_ID_CAMERA_INFORMATION_LEN, MAVLINK_MSG_ID_CAMERA_INFORMATION_CRC); +#else + mavlink_camera_information_t packet; + packet.time_boot_ms = time_boot_ms; + packet.firmware_version = firmware_version; + packet.focal_length = focal_length; + packet.sensor_size_h = sensor_size_h; + packet.sensor_size_v = sensor_size_v; + packet.flags = flags; + packet.resolution_h = resolution_h; + packet.resolution_v = resolution_v; + packet.cam_definition_version = cam_definition_version; + packet.lens_id = lens_id; + mav_array_memcpy(packet.vendor_name, vendor_name, sizeof(uint8_t)*32); + mav_array_memcpy(packet.model_name, model_name, sizeof(uint8_t)*32); + mav_array_memcpy(packet.cam_definition_uri, cam_definition_uri, sizeof(char)*140); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CAMERA_INFORMATION, (const char *)&packet, MAVLINK_MSG_ID_CAMERA_INFORMATION_MIN_LEN, MAVLINK_MSG_ID_CAMERA_INFORMATION_LEN, MAVLINK_MSG_ID_CAMERA_INFORMATION_CRC); +#endif +} + +/** + * @brief Send a camera_information message + * @param chan MAVLink channel to send the message + * @param struct The MAVLink struct to serialize + */ +static inline void mavlink_msg_camera_information_send_struct(mavlink_channel_t chan, const mavlink_camera_information_t* camera_information) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + mavlink_msg_camera_information_send(chan, camera_information->time_boot_ms, camera_information->vendor_name, camera_information->model_name, camera_information->firmware_version, camera_information->focal_length, camera_information->sensor_size_h, camera_information->sensor_size_v, camera_information->resolution_h, camera_information->resolution_v, camera_information->lens_id, camera_information->flags, camera_information->cam_definition_version, camera_information->cam_definition_uri); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CAMERA_INFORMATION, (const char *)camera_information, MAVLINK_MSG_ID_CAMERA_INFORMATION_MIN_LEN, MAVLINK_MSG_ID_CAMERA_INFORMATION_LEN, MAVLINK_MSG_ID_CAMERA_INFORMATION_CRC); +#endif +} + +#if MAVLINK_MSG_ID_CAMERA_INFORMATION_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_camera_information_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint32_t time_boot_ms, const uint8_t *vendor_name, const uint8_t *model_name, uint32_t firmware_version, float focal_length, float sensor_size_h, float sensor_size_v, uint16_t resolution_h, uint16_t resolution_v, uint8_t lens_id, uint32_t flags, uint16_t cam_definition_version, const char *cam_definition_uri) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_uint32_t(buf, 4, firmware_version); + _mav_put_float(buf, 8, focal_length); + _mav_put_float(buf, 12, sensor_size_h); + _mav_put_float(buf, 16, sensor_size_v); + _mav_put_uint32_t(buf, 20, flags); + _mav_put_uint16_t(buf, 24, resolution_h); + _mav_put_uint16_t(buf, 26, resolution_v); + _mav_put_uint16_t(buf, 28, cam_definition_version); + _mav_put_uint8_t(buf, 94, lens_id); + _mav_put_uint8_t_array(buf, 30, vendor_name, 32); + _mav_put_uint8_t_array(buf, 62, model_name, 32); + _mav_put_char_array(buf, 95, cam_definition_uri, 140); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CAMERA_INFORMATION, buf, MAVLINK_MSG_ID_CAMERA_INFORMATION_MIN_LEN, MAVLINK_MSG_ID_CAMERA_INFORMATION_LEN, MAVLINK_MSG_ID_CAMERA_INFORMATION_CRC); +#else + mavlink_camera_information_t *packet = (mavlink_camera_information_t *)msgbuf; + packet->time_boot_ms = time_boot_ms; + packet->firmware_version = firmware_version; + packet->focal_length = focal_length; + packet->sensor_size_h = sensor_size_h; + packet->sensor_size_v = sensor_size_v; + packet->flags = flags; + packet->resolution_h = resolution_h; + packet->resolution_v = resolution_v; + packet->cam_definition_version = cam_definition_version; + packet->lens_id = lens_id; + mav_array_memcpy(packet->vendor_name, vendor_name, sizeof(uint8_t)*32); + mav_array_memcpy(packet->model_name, model_name, sizeof(uint8_t)*32); + mav_array_memcpy(packet->cam_definition_uri, cam_definition_uri, sizeof(char)*140); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CAMERA_INFORMATION, (const char *)packet, MAVLINK_MSG_ID_CAMERA_INFORMATION_MIN_LEN, MAVLINK_MSG_ID_CAMERA_INFORMATION_LEN, MAVLINK_MSG_ID_CAMERA_INFORMATION_CRC); +#endif +} +#endif + +#endif + +// MESSAGE CAMERA_INFORMATION UNPACKING + + +/** + * @brief Get field time_boot_ms from camera_information message + * + * @return [ms] Timestamp (time since system boot). + */ +static inline uint32_t mavlink_msg_camera_information_get_time_boot_ms(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint32_t(msg, 0); +} + +/** + * @brief Get field vendor_name from camera_information message + * + * @return Name of the camera vendor + */ +static inline uint16_t mavlink_msg_camera_information_get_vendor_name(const mavlink_message_t* msg, uint8_t *vendor_name) +{ + return _MAV_RETURN_uint8_t_array(msg, vendor_name, 32, 30); +} + +/** + * @brief Get field model_name from camera_information message + * + * @return Name of the camera model + */ +static inline uint16_t mavlink_msg_camera_information_get_model_name(const mavlink_message_t* msg, uint8_t *model_name) +{ + return _MAV_RETURN_uint8_t_array(msg, model_name, 32, 62); +} + +/** + * @brief Get field firmware_version from camera_information message + * + * @return Version of the camera firmware, encoded as: (Dev & 0xff) << 24 | (Patch & 0xff) << 16 | (Minor & 0xff) << 8 | (Major & 0xff) + */ +static inline uint32_t mavlink_msg_camera_information_get_firmware_version(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint32_t(msg, 4); +} + +/** + * @brief Get field focal_length from camera_information message + * + * @return [mm] Focal length + */ +static inline float mavlink_msg_camera_information_get_focal_length(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 8); +} + +/** + * @brief Get field sensor_size_h from camera_information message + * + * @return [mm] Image sensor size horizontal + */ +static inline float mavlink_msg_camera_information_get_sensor_size_h(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 12); +} + +/** + * @brief Get field sensor_size_v from camera_information message + * + * @return [mm] Image sensor size vertical + */ +static inline float mavlink_msg_camera_information_get_sensor_size_v(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 16); +} + +/** + * @brief Get field resolution_h from camera_information message + * + * @return [pix] Horizontal image resolution + */ +static inline uint16_t mavlink_msg_camera_information_get_resolution_h(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 24); +} + +/** + * @brief Get field resolution_v from camera_information message + * + * @return [pix] Vertical image resolution + */ +static inline uint16_t mavlink_msg_camera_information_get_resolution_v(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 26); +} + +/** + * @brief Get field lens_id from camera_information message + * + * @return Reserved for a lens ID + */ +static inline uint8_t mavlink_msg_camera_information_get_lens_id(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 94); +} + +/** + * @brief Get field flags from camera_information message + * + * @return Bitmap of camera capability flags. + */ +static inline uint32_t mavlink_msg_camera_information_get_flags(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint32_t(msg, 20); +} + +/** + * @brief Get field cam_definition_version from camera_information message + * + * @return Camera definition version (iteration) + */ +static inline uint16_t mavlink_msg_camera_information_get_cam_definition_version(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 28); +} + +/** + * @brief Get field cam_definition_uri from camera_information message + * + * @return Camera definition URI (if any, otherwise only basic functions will be available). HTTP- (http://) and MAVLink FTP- (mavlinkftp://) formatted URIs are allowed (and both must be supported by any GCS that implements the Camera Protocol). + */ +static inline uint16_t mavlink_msg_camera_information_get_cam_definition_uri(const mavlink_message_t* msg, char *cam_definition_uri) +{ + return _MAV_RETURN_char_array(msg, cam_definition_uri, 140, 95); +} + +/** + * @brief Decode a camera_information message into a struct + * + * @param msg The message to decode + * @param camera_information C-struct to decode the message contents into + */ +static inline void mavlink_msg_camera_information_decode(const mavlink_message_t* msg, mavlink_camera_information_t* camera_information) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + camera_information->time_boot_ms = mavlink_msg_camera_information_get_time_boot_ms(msg); + camera_information->firmware_version = mavlink_msg_camera_information_get_firmware_version(msg); + camera_information->focal_length = mavlink_msg_camera_information_get_focal_length(msg); + camera_information->sensor_size_h = mavlink_msg_camera_information_get_sensor_size_h(msg); + camera_information->sensor_size_v = mavlink_msg_camera_information_get_sensor_size_v(msg); + camera_information->flags = mavlink_msg_camera_information_get_flags(msg); + camera_information->resolution_h = mavlink_msg_camera_information_get_resolution_h(msg); + camera_information->resolution_v = mavlink_msg_camera_information_get_resolution_v(msg); + camera_information->cam_definition_version = mavlink_msg_camera_information_get_cam_definition_version(msg); + mavlink_msg_camera_information_get_vendor_name(msg, camera_information->vendor_name); + mavlink_msg_camera_information_get_model_name(msg, camera_information->model_name); + camera_information->lens_id = mavlink_msg_camera_information_get_lens_id(msg); + mavlink_msg_camera_information_get_cam_definition_uri(msg, camera_information->cam_definition_uri); +#else + uint8_t len = msg->len < MAVLINK_MSG_ID_CAMERA_INFORMATION_LEN? msg->len : MAVLINK_MSG_ID_CAMERA_INFORMATION_LEN; + memset(camera_information, 0, MAVLINK_MSG_ID_CAMERA_INFORMATION_LEN); + memcpy(camera_information, _MAV_PAYLOAD(msg), len); +#endif +} diff --git a/lib/main/MAVLink/common/mavlink_msg_camera_settings.h b/lib/main/MAVLink/common/mavlink_msg_camera_settings.h new file mode 100644 index 0000000000..8f49780fad --- /dev/null +++ b/lib/main/MAVLink/common/mavlink_msg_camera_settings.h @@ -0,0 +1,288 @@ +#pragma once +// MESSAGE CAMERA_SETTINGS PACKING + +#define MAVLINK_MSG_ID_CAMERA_SETTINGS 260 + +MAVPACKED( +typedef struct __mavlink_camera_settings_t { + uint32_t time_boot_ms; /*< [ms] Timestamp (time since system boot).*/ + uint8_t mode_id; /*< Camera mode*/ + float zoomLevel; /*< Current zoom level (0.0 to 100.0, NaN if not known)*/ + float focusLevel; /*< Current focus level (0.0 to 100.0, NaN if not known)*/ +}) mavlink_camera_settings_t; + +#define MAVLINK_MSG_ID_CAMERA_SETTINGS_LEN 13 +#define MAVLINK_MSG_ID_CAMERA_SETTINGS_MIN_LEN 5 +#define MAVLINK_MSG_ID_260_LEN 13 +#define MAVLINK_MSG_ID_260_MIN_LEN 5 + +#define MAVLINK_MSG_ID_CAMERA_SETTINGS_CRC 146 +#define MAVLINK_MSG_ID_260_CRC 146 + + + +#if MAVLINK_COMMAND_24BIT +#define MAVLINK_MESSAGE_INFO_CAMERA_SETTINGS { \ + 260, \ + "CAMERA_SETTINGS", \ + 4, \ + { { "time_boot_ms", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_camera_settings_t, time_boot_ms) }, \ + { "mode_id", NULL, MAVLINK_TYPE_UINT8_T, 0, 4, offsetof(mavlink_camera_settings_t, mode_id) }, \ + { "zoomLevel", NULL, MAVLINK_TYPE_FLOAT, 0, 5, offsetof(mavlink_camera_settings_t, zoomLevel) }, \ + { "focusLevel", NULL, MAVLINK_TYPE_FLOAT, 0, 9, offsetof(mavlink_camera_settings_t, focusLevel) }, \ + } \ +} +#else +#define MAVLINK_MESSAGE_INFO_CAMERA_SETTINGS { \ + "CAMERA_SETTINGS", \ + 4, \ + { { "time_boot_ms", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_camera_settings_t, time_boot_ms) }, \ + { "mode_id", NULL, MAVLINK_TYPE_UINT8_T, 0, 4, offsetof(mavlink_camera_settings_t, mode_id) }, \ + { "zoomLevel", NULL, MAVLINK_TYPE_FLOAT, 0, 5, offsetof(mavlink_camera_settings_t, zoomLevel) }, \ + { "focusLevel", NULL, MAVLINK_TYPE_FLOAT, 0, 9, offsetof(mavlink_camera_settings_t, focusLevel) }, \ + } \ +} +#endif + +/** + * @brief Pack a camera_settings message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param time_boot_ms [ms] Timestamp (time since system boot). + * @param mode_id Camera mode + * @param zoomLevel Current zoom level (0.0 to 100.0, NaN if not known) + * @param focusLevel Current focus level (0.0 to 100.0, NaN if not known) + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_camera_settings_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint32_t time_boot_ms, uint8_t mode_id, float zoomLevel, float focusLevel) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_CAMERA_SETTINGS_LEN]; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_uint8_t(buf, 4, mode_id); + _mav_put_float(buf, 5, zoomLevel); + _mav_put_float(buf, 9, focusLevel); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_CAMERA_SETTINGS_LEN); +#else + mavlink_camera_settings_t packet; + packet.time_boot_ms = time_boot_ms; + packet.mode_id = mode_id; + packet.zoomLevel = zoomLevel; + packet.focusLevel = focusLevel; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_CAMERA_SETTINGS_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_CAMERA_SETTINGS; + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_CAMERA_SETTINGS_MIN_LEN, MAVLINK_MSG_ID_CAMERA_SETTINGS_LEN, MAVLINK_MSG_ID_CAMERA_SETTINGS_CRC); +} + +/** + * @brief Pack a camera_settings message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param time_boot_ms [ms] Timestamp (time since system boot). + * @param mode_id Camera mode + * @param zoomLevel Current zoom level (0.0 to 100.0, NaN if not known) + * @param focusLevel Current focus level (0.0 to 100.0, NaN if not known) + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_camera_settings_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint32_t time_boot_ms,uint8_t mode_id,float zoomLevel,float focusLevel) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_CAMERA_SETTINGS_LEN]; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_uint8_t(buf, 4, mode_id); + _mav_put_float(buf, 5, zoomLevel); + _mav_put_float(buf, 9, focusLevel); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_CAMERA_SETTINGS_LEN); +#else + mavlink_camera_settings_t packet; + packet.time_boot_ms = time_boot_ms; + packet.mode_id = mode_id; + packet.zoomLevel = zoomLevel; + packet.focusLevel = focusLevel; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_CAMERA_SETTINGS_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_CAMERA_SETTINGS; + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_CAMERA_SETTINGS_MIN_LEN, MAVLINK_MSG_ID_CAMERA_SETTINGS_LEN, MAVLINK_MSG_ID_CAMERA_SETTINGS_CRC); +} + +/** + * @brief Encode a camera_settings struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param camera_settings C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_camera_settings_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_camera_settings_t* camera_settings) +{ + return mavlink_msg_camera_settings_pack(system_id, component_id, msg, camera_settings->time_boot_ms, camera_settings->mode_id, camera_settings->zoomLevel, camera_settings->focusLevel); +} + +/** + * @brief Encode a camera_settings struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param camera_settings C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_camera_settings_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_camera_settings_t* camera_settings) +{ + return mavlink_msg_camera_settings_pack_chan(system_id, component_id, chan, msg, camera_settings->time_boot_ms, camera_settings->mode_id, camera_settings->zoomLevel, camera_settings->focusLevel); +} + +/** + * @brief Send a camera_settings message + * @param chan MAVLink channel to send the message + * + * @param time_boot_ms [ms] Timestamp (time since system boot). + * @param mode_id Camera mode + * @param zoomLevel Current zoom level (0.0 to 100.0, NaN if not known) + * @param focusLevel Current focus level (0.0 to 100.0, NaN if not known) + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_camera_settings_send(mavlink_channel_t chan, uint32_t time_boot_ms, uint8_t mode_id, float zoomLevel, float focusLevel) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_CAMERA_SETTINGS_LEN]; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_uint8_t(buf, 4, mode_id); + _mav_put_float(buf, 5, zoomLevel); + _mav_put_float(buf, 9, focusLevel); + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CAMERA_SETTINGS, buf, MAVLINK_MSG_ID_CAMERA_SETTINGS_MIN_LEN, MAVLINK_MSG_ID_CAMERA_SETTINGS_LEN, MAVLINK_MSG_ID_CAMERA_SETTINGS_CRC); +#else + mavlink_camera_settings_t packet; + packet.time_boot_ms = time_boot_ms; + packet.mode_id = mode_id; + packet.zoomLevel = zoomLevel; + packet.focusLevel = focusLevel; + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CAMERA_SETTINGS, (const char *)&packet, MAVLINK_MSG_ID_CAMERA_SETTINGS_MIN_LEN, MAVLINK_MSG_ID_CAMERA_SETTINGS_LEN, MAVLINK_MSG_ID_CAMERA_SETTINGS_CRC); +#endif +} + +/** + * @brief Send a camera_settings message + * @param chan MAVLink channel to send the message + * @param struct The MAVLink struct to serialize + */ +static inline void mavlink_msg_camera_settings_send_struct(mavlink_channel_t chan, const mavlink_camera_settings_t* camera_settings) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + mavlink_msg_camera_settings_send(chan, camera_settings->time_boot_ms, camera_settings->mode_id, camera_settings->zoomLevel, camera_settings->focusLevel); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CAMERA_SETTINGS, (const char *)camera_settings, MAVLINK_MSG_ID_CAMERA_SETTINGS_MIN_LEN, MAVLINK_MSG_ID_CAMERA_SETTINGS_LEN, MAVLINK_MSG_ID_CAMERA_SETTINGS_CRC); +#endif +} + +#if MAVLINK_MSG_ID_CAMERA_SETTINGS_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_camera_settings_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint32_t time_boot_ms, uint8_t mode_id, float zoomLevel, float focusLevel) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_uint8_t(buf, 4, mode_id); + _mav_put_float(buf, 5, zoomLevel); + _mav_put_float(buf, 9, focusLevel); + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CAMERA_SETTINGS, buf, MAVLINK_MSG_ID_CAMERA_SETTINGS_MIN_LEN, MAVLINK_MSG_ID_CAMERA_SETTINGS_LEN, MAVLINK_MSG_ID_CAMERA_SETTINGS_CRC); +#else + mavlink_camera_settings_t *packet = (mavlink_camera_settings_t *)msgbuf; + packet->time_boot_ms = time_boot_ms; + packet->mode_id = mode_id; + packet->zoomLevel = zoomLevel; + packet->focusLevel = focusLevel; + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CAMERA_SETTINGS, (const char *)packet, MAVLINK_MSG_ID_CAMERA_SETTINGS_MIN_LEN, MAVLINK_MSG_ID_CAMERA_SETTINGS_LEN, MAVLINK_MSG_ID_CAMERA_SETTINGS_CRC); +#endif +} +#endif + +#endif + +// MESSAGE CAMERA_SETTINGS UNPACKING + + +/** + * @brief Get field time_boot_ms from camera_settings message + * + * @return [ms] Timestamp (time since system boot). + */ +static inline uint32_t mavlink_msg_camera_settings_get_time_boot_ms(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint32_t(msg, 0); +} + +/** + * @brief Get field mode_id from camera_settings message + * + * @return Camera mode + */ +static inline uint8_t mavlink_msg_camera_settings_get_mode_id(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 4); +} + +/** + * @brief Get field zoomLevel from camera_settings message + * + * @return Current zoom level (0.0 to 100.0, NaN if not known) + */ +static inline float mavlink_msg_camera_settings_get_zoomLevel(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 5); +} + +/** + * @brief Get field focusLevel from camera_settings message + * + * @return Current focus level (0.0 to 100.0, NaN if not known) + */ +static inline float mavlink_msg_camera_settings_get_focusLevel(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 9); +} + +/** + * @brief Decode a camera_settings message into a struct + * + * @param msg The message to decode + * @param camera_settings C-struct to decode the message contents into + */ +static inline void mavlink_msg_camera_settings_decode(const mavlink_message_t* msg, mavlink_camera_settings_t* camera_settings) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + camera_settings->time_boot_ms = mavlink_msg_camera_settings_get_time_boot_ms(msg); + camera_settings->mode_id = mavlink_msg_camera_settings_get_mode_id(msg); + camera_settings->zoomLevel = mavlink_msg_camera_settings_get_zoomLevel(msg); + camera_settings->focusLevel = mavlink_msg_camera_settings_get_focusLevel(msg); +#else + uint8_t len = msg->len < MAVLINK_MSG_ID_CAMERA_SETTINGS_LEN? msg->len : MAVLINK_MSG_ID_CAMERA_SETTINGS_LEN; + memset(camera_settings, 0, MAVLINK_MSG_ID_CAMERA_SETTINGS_LEN); + memcpy(camera_settings, _MAV_PAYLOAD(msg), len); +#endif +} diff --git a/lib/main/MAVLink/common/mavlink_msg_camera_tracking_geo_status.h b/lib/main/MAVLink/common/mavlink_msg_camera_tracking_geo_status.h new file mode 100644 index 0000000000..f4e22b98f9 --- /dev/null +++ b/lib/main/MAVLink/common/mavlink_msg_camera_tracking_geo_status.h @@ -0,0 +1,513 @@ +#pragma once +// MESSAGE CAMERA_TRACKING_GEO_STATUS PACKING + +#define MAVLINK_MSG_ID_CAMERA_TRACKING_GEO_STATUS 276 + + +typedef struct __mavlink_camera_tracking_geo_status_t { + int32_t lat; /*< [degE7] Latitude of tracked object*/ + int32_t lon; /*< [degE7] Longitude of tracked object*/ + float alt; /*< [m] Altitude of tracked object(AMSL, WGS84)*/ + float h_acc; /*< [m] Horizontal accuracy. NAN if unknown*/ + float v_acc; /*< [m] Vertical accuracy. NAN if unknown*/ + float vel_n; /*< [m/s] North velocity of tracked object. NAN if unknown*/ + float vel_e; /*< [m/s] East velocity of tracked object. NAN if unknown*/ + float vel_d; /*< [m/s] Down velocity of tracked object. NAN if unknown*/ + float vel_acc; /*< [m/s] Velocity accuracy. NAN if unknown*/ + float dist; /*< [m] Distance between camera and tracked object. NAN if unknown*/ + float hdg; /*< [rad] Heading in radians, in NED. NAN if unknown*/ + float hdg_acc; /*< [rad] Accuracy of heading, in NED. NAN if unknown*/ + uint8_t tracking_status; /*< Current tracking status*/ +} mavlink_camera_tracking_geo_status_t; + +#define MAVLINK_MSG_ID_CAMERA_TRACKING_GEO_STATUS_LEN 49 +#define MAVLINK_MSG_ID_CAMERA_TRACKING_GEO_STATUS_MIN_LEN 49 +#define MAVLINK_MSG_ID_276_LEN 49 +#define MAVLINK_MSG_ID_276_MIN_LEN 49 + +#define MAVLINK_MSG_ID_CAMERA_TRACKING_GEO_STATUS_CRC 18 +#define MAVLINK_MSG_ID_276_CRC 18 + + + +#if MAVLINK_COMMAND_24BIT +#define MAVLINK_MESSAGE_INFO_CAMERA_TRACKING_GEO_STATUS { \ + 276, \ + "CAMERA_TRACKING_GEO_STATUS", \ + 13, \ + { { "tracking_status", NULL, MAVLINK_TYPE_UINT8_T, 0, 48, offsetof(mavlink_camera_tracking_geo_status_t, tracking_status) }, \ + { "lat", NULL, MAVLINK_TYPE_INT32_T, 0, 0, offsetof(mavlink_camera_tracking_geo_status_t, lat) }, \ + { "lon", NULL, MAVLINK_TYPE_INT32_T, 0, 4, offsetof(mavlink_camera_tracking_geo_status_t, lon) }, \ + { "alt", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_camera_tracking_geo_status_t, alt) }, \ + { "h_acc", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_camera_tracking_geo_status_t, h_acc) }, \ + { "v_acc", NULL, MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_camera_tracking_geo_status_t, v_acc) }, \ + { "vel_n", NULL, MAVLINK_TYPE_FLOAT, 0, 20, offsetof(mavlink_camera_tracking_geo_status_t, vel_n) }, \ + { "vel_e", NULL, MAVLINK_TYPE_FLOAT, 0, 24, offsetof(mavlink_camera_tracking_geo_status_t, vel_e) }, \ + { "vel_d", NULL, MAVLINK_TYPE_FLOAT, 0, 28, offsetof(mavlink_camera_tracking_geo_status_t, vel_d) }, \ + { "vel_acc", NULL, MAVLINK_TYPE_FLOAT, 0, 32, offsetof(mavlink_camera_tracking_geo_status_t, vel_acc) }, \ + { "dist", NULL, MAVLINK_TYPE_FLOAT, 0, 36, offsetof(mavlink_camera_tracking_geo_status_t, dist) }, \ + { "hdg", NULL, MAVLINK_TYPE_FLOAT, 0, 40, offsetof(mavlink_camera_tracking_geo_status_t, hdg) }, \ + { "hdg_acc", NULL, MAVLINK_TYPE_FLOAT, 0, 44, offsetof(mavlink_camera_tracking_geo_status_t, hdg_acc) }, \ + } \ +} +#else +#define MAVLINK_MESSAGE_INFO_CAMERA_TRACKING_GEO_STATUS { \ + "CAMERA_TRACKING_GEO_STATUS", \ + 13, \ + { { "tracking_status", NULL, MAVLINK_TYPE_UINT8_T, 0, 48, offsetof(mavlink_camera_tracking_geo_status_t, tracking_status) }, \ + { "lat", NULL, MAVLINK_TYPE_INT32_T, 0, 0, offsetof(mavlink_camera_tracking_geo_status_t, lat) }, \ + { "lon", NULL, MAVLINK_TYPE_INT32_T, 0, 4, offsetof(mavlink_camera_tracking_geo_status_t, lon) }, \ + { "alt", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_camera_tracking_geo_status_t, alt) }, \ + { "h_acc", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_camera_tracking_geo_status_t, h_acc) }, \ + { "v_acc", NULL, MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_camera_tracking_geo_status_t, v_acc) }, \ + { "vel_n", NULL, MAVLINK_TYPE_FLOAT, 0, 20, offsetof(mavlink_camera_tracking_geo_status_t, vel_n) }, \ + { "vel_e", NULL, MAVLINK_TYPE_FLOAT, 0, 24, offsetof(mavlink_camera_tracking_geo_status_t, vel_e) }, \ + { "vel_d", NULL, MAVLINK_TYPE_FLOAT, 0, 28, offsetof(mavlink_camera_tracking_geo_status_t, vel_d) }, \ + { "vel_acc", NULL, MAVLINK_TYPE_FLOAT, 0, 32, offsetof(mavlink_camera_tracking_geo_status_t, vel_acc) }, \ + { "dist", NULL, MAVLINK_TYPE_FLOAT, 0, 36, offsetof(mavlink_camera_tracking_geo_status_t, dist) }, \ + { "hdg", NULL, MAVLINK_TYPE_FLOAT, 0, 40, offsetof(mavlink_camera_tracking_geo_status_t, hdg) }, \ + { "hdg_acc", NULL, MAVLINK_TYPE_FLOAT, 0, 44, offsetof(mavlink_camera_tracking_geo_status_t, hdg_acc) }, \ + } \ +} +#endif + +/** + * @brief Pack a camera_tracking_geo_status message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param tracking_status Current tracking status + * @param lat [degE7] Latitude of tracked object + * @param lon [degE7] Longitude of tracked object + * @param alt [m] Altitude of tracked object(AMSL, WGS84) + * @param h_acc [m] Horizontal accuracy. NAN if unknown + * @param v_acc [m] Vertical accuracy. NAN if unknown + * @param vel_n [m/s] North velocity of tracked object. NAN if unknown + * @param vel_e [m/s] East velocity of tracked object. NAN if unknown + * @param vel_d [m/s] Down velocity of tracked object. NAN if unknown + * @param vel_acc [m/s] Velocity accuracy. NAN if unknown + * @param dist [m] Distance between camera and tracked object. NAN if unknown + * @param hdg [rad] Heading in radians, in NED. NAN if unknown + * @param hdg_acc [rad] Accuracy of heading, in NED. NAN if unknown + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_camera_tracking_geo_status_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint8_t tracking_status, int32_t lat, int32_t lon, float alt, float h_acc, float v_acc, float vel_n, float vel_e, float vel_d, float vel_acc, float dist, float hdg, float hdg_acc) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_CAMERA_TRACKING_GEO_STATUS_LEN]; + _mav_put_int32_t(buf, 0, lat); + _mav_put_int32_t(buf, 4, lon); + _mav_put_float(buf, 8, alt); + _mav_put_float(buf, 12, h_acc); + _mav_put_float(buf, 16, v_acc); + _mav_put_float(buf, 20, vel_n); + _mav_put_float(buf, 24, vel_e); + _mav_put_float(buf, 28, vel_d); + _mav_put_float(buf, 32, vel_acc); + _mav_put_float(buf, 36, dist); + _mav_put_float(buf, 40, hdg); + _mav_put_float(buf, 44, hdg_acc); + _mav_put_uint8_t(buf, 48, tracking_status); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_CAMERA_TRACKING_GEO_STATUS_LEN); +#else + mavlink_camera_tracking_geo_status_t packet; + packet.lat = lat; + packet.lon = lon; + packet.alt = alt; + packet.h_acc = h_acc; + packet.v_acc = v_acc; + packet.vel_n = vel_n; + packet.vel_e = vel_e; + packet.vel_d = vel_d; + packet.vel_acc = vel_acc; + packet.dist = dist; + packet.hdg = hdg; + packet.hdg_acc = hdg_acc; + packet.tracking_status = tracking_status; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_CAMERA_TRACKING_GEO_STATUS_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_CAMERA_TRACKING_GEO_STATUS; + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_CAMERA_TRACKING_GEO_STATUS_MIN_LEN, MAVLINK_MSG_ID_CAMERA_TRACKING_GEO_STATUS_LEN, MAVLINK_MSG_ID_CAMERA_TRACKING_GEO_STATUS_CRC); +} + +/** + * @brief Pack a camera_tracking_geo_status message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param tracking_status Current tracking status + * @param lat [degE7] Latitude of tracked object + * @param lon [degE7] Longitude of tracked object + * @param alt [m] Altitude of tracked object(AMSL, WGS84) + * @param h_acc [m] Horizontal accuracy. NAN if unknown + * @param v_acc [m] Vertical accuracy. NAN if unknown + * @param vel_n [m/s] North velocity of tracked object. NAN if unknown + * @param vel_e [m/s] East velocity of tracked object. NAN if unknown + * @param vel_d [m/s] Down velocity of tracked object. NAN if unknown + * @param vel_acc [m/s] Velocity accuracy. NAN if unknown + * @param dist [m] Distance between camera and tracked object. NAN if unknown + * @param hdg [rad] Heading in radians, in NED. NAN if unknown + * @param hdg_acc [rad] Accuracy of heading, in NED. NAN if unknown + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_camera_tracking_geo_status_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint8_t tracking_status,int32_t lat,int32_t lon,float alt,float h_acc,float v_acc,float vel_n,float vel_e,float vel_d,float vel_acc,float dist,float hdg,float hdg_acc) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_CAMERA_TRACKING_GEO_STATUS_LEN]; + _mav_put_int32_t(buf, 0, lat); + _mav_put_int32_t(buf, 4, lon); + _mav_put_float(buf, 8, alt); + _mav_put_float(buf, 12, h_acc); + _mav_put_float(buf, 16, v_acc); + _mav_put_float(buf, 20, vel_n); + _mav_put_float(buf, 24, vel_e); + _mav_put_float(buf, 28, vel_d); + _mav_put_float(buf, 32, vel_acc); + _mav_put_float(buf, 36, dist); + _mav_put_float(buf, 40, hdg); + _mav_put_float(buf, 44, hdg_acc); + _mav_put_uint8_t(buf, 48, tracking_status); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_CAMERA_TRACKING_GEO_STATUS_LEN); +#else + mavlink_camera_tracking_geo_status_t packet; + packet.lat = lat; + packet.lon = lon; + packet.alt = alt; + packet.h_acc = h_acc; + packet.v_acc = v_acc; + packet.vel_n = vel_n; + packet.vel_e = vel_e; + packet.vel_d = vel_d; + packet.vel_acc = vel_acc; + packet.dist = dist; + packet.hdg = hdg; + packet.hdg_acc = hdg_acc; + packet.tracking_status = tracking_status; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_CAMERA_TRACKING_GEO_STATUS_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_CAMERA_TRACKING_GEO_STATUS; + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_CAMERA_TRACKING_GEO_STATUS_MIN_LEN, MAVLINK_MSG_ID_CAMERA_TRACKING_GEO_STATUS_LEN, MAVLINK_MSG_ID_CAMERA_TRACKING_GEO_STATUS_CRC); +} + +/** + * @brief Encode a camera_tracking_geo_status struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param camera_tracking_geo_status C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_camera_tracking_geo_status_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_camera_tracking_geo_status_t* camera_tracking_geo_status) +{ + return mavlink_msg_camera_tracking_geo_status_pack(system_id, component_id, msg, camera_tracking_geo_status->tracking_status, camera_tracking_geo_status->lat, camera_tracking_geo_status->lon, camera_tracking_geo_status->alt, camera_tracking_geo_status->h_acc, camera_tracking_geo_status->v_acc, camera_tracking_geo_status->vel_n, camera_tracking_geo_status->vel_e, camera_tracking_geo_status->vel_d, camera_tracking_geo_status->vel_acc, camera_tracking_geo_status->dist, camera_tracking_geo_status->hdg, camera_tracking_geo_status->hdg_acc); +} + +/** + * @brief Encode a camera_tracking_geo_status struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param camera_tracking_geo_status C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_camera_tracking_geo_status_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_camera_tracking_geo_status_t* camera_tracking_geo_status) +{ + return mavlink_msg_camera_tracking_geo_status_pack_chan(system_id, component_id, chan, msg, camera_tracking_geo_status->tracking_status, camera_tracking_geo_status->lat, camera_tracking_geo_status->lon, camera_tracking_geo_status->alt, camera_tracking_geo_status->h_acc, camera_tracking_geo_status->v_acc, camera_tracking_geo_status->vel_n, camera_tracking_geo_status->vel_e, camera_tracking_geo_status->vel_d, camera_tracking_geo_status->vel_acc, camera_tracking_geo_status->dist, camera_tracking_geo_status->hdg, camera_tracking_geo_status->hdg_acc); +} + +/** + * @brief Send a camera_tracking_geo_status message + * @param chan MAVLink channel to send the message + * + * @param tracking_status Current tracking status + * @param lat [degE7] Latitude of tracked object + * @param lon [degE7] Longitude of tracked object + * @param alt [m] Altitude of tracked object(AMSL, WGS84) + * @param h_acc [m] Horizontal accuracy. NAN if unknown + * @param v_acc [m] Vertical accuracy. NAN if unknown + * @param vel_n [m/s] North velocity of tracked object. NAN if unknown + * @param vel_e [m/s] East velocity of tracked object. NAN if unknown + * @param vel_d [m/s] Down velocity of tracked object. NAN if unknown + * @param vel_acc [m/s] Velocity accuracy. NAN if unknown + * @param dist [m] Distance between camera and tracked object. NAN if unknown + * @param hdg [rad] Heading in radians, in NED. NAN if unknown + * @param hdg_acc [rad] Accuracy of heading, in NED. NAN if unknown + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_camera_tracking_geo_status_send(mavlink_channel_t chan, uint8_t tracking_status, int32_t lat, int32_t lon, float alt, float h_acc, float v_acc, float vel_n, float vel_e, float vel_d, float vel_acc, float dist, float hdg, float hdg_acc) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_CAMERA_TRACKING_GEO_STATUS_LEN]; + _mav_put_int32_t(buf, 0, lat); + _mav_put_int32_t(buf, 4, lon); + _mav_put_float(buf, 8, alt); + _mav_put_float(buf, 12, h_acc); + _mav_put_float(buf, 16, v_acc); + _mav_put_float(buf, 20, vel_n); + _mav_put_float(buf, 24, vel_e); + _mav_put_float(buf, 28, vel_d); + _mav_put_float(buf, 32, vel_acc); + _mav_put_float(buf, 36, dist); + _mav_put_float(buf, 40, hdg); + _mav_put_float(buf, 44, hdg_acc); + _mav_put_uint8_t(buf, 48, tracking_status); + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CAMERA_TRACKING_GEO_STATUS, buf, MAVLINK_MSG_ID_CAMERA_TRACKING_GEO_STATUS_MIN_LEN, MAVLINK_MSG_ID_CAMERA_TRACKING_GEO_STATUS_LEN, MAVLINK_MSG_ID_CAMERA_TRACKING_GEO_STATUS_CRC); +#else + mavlink_camera_tracking_geo_status_t packet; + packet.lat = lat; + packet.lon = lon; + packet.alt = alt; + packet.h_acc = h_acc; + packet.v_acc = v_acc; + packet.vel_n = vel_n; + packet.vel_e = vel_e; + packet.vel_d = vel_d; + packet.vel_acc = vel_acc; + packet.dist = dist; + packet.hdg = hdg; + packet.hdg_acc = hdg_acc; + packet.tracking_status = tracking_status; + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CAMERA_TRACKING_GEO_STATUS, (const char *)&packet, MAVLINK_MSG_ID_CAMERA_TRACKING_GEO_STATUS_MIN_LEN, MAVLINK_MSG_ID_CAMERA_TRACKING_GEO_STATUS_LEN, MAVLINK_MSG_ID_CAMERA_TRACKING_GEO_STATUS_CRC); +#endif +} + +/** + * @brief Send a camera_tracking_geo_status message + * @param chan MAVLink channel to send the message + * @param struct The MAVLink struct to serialize + */ +static inline void mavlink_msg_camera_tracking_geo_status_send_struct(mavlink_channel_t chan, const mavlink_camera_tracking_geo_status_t* camera_tracking_geo_status) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + mavlink_msg_camera_tracking_geo_status_send(chan, camera_tracking_geo_status->tracking_status, camera_tracking_geo_status->lat, camera_tracking_geo_status->lon, camera_tracking_geo_status->alt, camera_tracking_geo_status->h_acc, camera_tracking_geo_status->v_acc, camera_tracking_geo_status->vel_n, camera_tracking_geo_status->vel_e, camera_tracking_geo_status->vel_d, camera_tracking_geo_status->vel_acc, camera_tracking_geo_status->dist, camera_tracking_geo_status->hdg, camera_tracking_geo_status->hdg_acc); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CAMERA_TRACKING_GEO_STATUS, (const char *)camera_tracking_geo_status, MAVLINK_MSG_ID_CAMERA_TRACKING_GEO_STATUS_MIN_LEN, MAVLINK_MSG_ID_CAMERA_TRACKING_GEO_STATUS_LEN, MAVLINK_MSG_ID_CAMERA_TRACKING_GEO_STATUS_CRC); +#endif +} + +#if MAVLINK_MSG_ID_CAMERA_TRACKING_GEO_STATUS_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_camera_tracking_geo_status_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t tracking_status, int32_t lat, int32_t lon, float alt, float h_acc, float v_acc, float vel_n, float vel_e, float vel_d, float vel_acc, float dist, float hdg, float hdg_acc) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_int32_t(buf, 0, lat); + _mav_put_int32_t(buf, 4, lon); + _mav_put_float(buf, 8, alt); + _mav_put_float(buf, 12, h_acc); + _mav_put_float(buf, 16, v_acc); + _mav_put_float(buf, 20, vel_n); + _mav_put_float(buf, 24, vel_e); + _mav_put_float(buf, 28, vel_d); + _mav_put_float(buf, 32, vel_acc); + _mav_put_float(buf, 36, dist); + _mav_put_float(buf, 40, hdg); + _mav_put_float(buf, 44, hdg_acc); + _mav_put_uint8_t(buf, 48, tracking_status); + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CAMERA_TRACKING_GEO_STATUS, buf, MAVLINK_MSG_ID_CAMERA_TRACKING_GEO_STATUS_MIN_LEN, MAVLINK_MSG_ID_CAMERA_TRACKING_GEO_STATUS_LEN, MAVLINK_MSG_ID_CAMERA_TRACKING_GEO_STATUS_CRC); +#else + mavlink_camera_tracking_geo_status_t *packet = (mavlink_camera_tracking_geo_status_t *)msgbuf; + packet->lat = lat; + packet->lon = lon; + packet->alt = alt; + packet->h_acc = h_acc; + packet->v_acc = v_acc; + packet->vel_n = vel_n; + packet->vel_e = vel_e; + packet->vel_d = vel_d; + packet->vel_acc = vel_acc; + packet->dist = dist; + packet->hdg = hdg; + packet->hdg_acc = hdg_acc; + packet->tracking_status = tracking_status; + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CAMERA_TRACKING_GEO_STATUS, (const char *)packet, MAVLINK_MSG_ID_CAMERA_TRACKING_GEO_STATUS_MIN_LEN, MAVLINK_MSG_ID_CAMERA_TRACKING_GEO_STATUS_LEN, MAVLINK_MSG_ID_CAMERA_TRACKING_GEO_STATUS_CRC); +#endif +} +#endif + +#endif + +// MESSAGE CAMERA_TRACKING_GEO_STATUS UNPACKING + + +/** + * @brief Get field tracking_status from camera_tracking_geo_status message + * + * @return Current tracking status + */ +static inline uint8_t mavlink_msg_camera_tracking_geo_status_get_tracking_status(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 48); +} + +/** + * @brief Get field lat from camera_tracking_geo_status message + * + * @return [degE7] Latitude of tracked object + */ +static inline int32_t mavlink_msg_camera_tracking_geo_status_get_lat(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int32_t(msg, 0); +} + +/** + * @brief Get field lon from camera_tracking_geo_status message + * + * @return [degE7] Longitude of tracked object + */ +static inline int32_t mavlink_msg_camera_tracking_geo_status_get_lon(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int32_t(msg, 4); +} + +/** + * @brief Get field alt from camera_tracking_geo_status message + * + * @return [m] Altitude of tracked object(AMSL, WGS84) + */ +static inline float mavlink_msg_camera_tracking_geo_status_get_alt(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 8); +} + +/** + * @brief Get field h_acc from camera_tracking_geo_status message + * + * @return [m] Horizontal accuracy. NAN if unknown + */ +static inline float mavlink_msg_camera_tracking_geo_status_get_h_acc(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 12); +} + +/** + * @brief Get field v_acc from camera_tracking_geo_status message + * + * @return [m] Vertical accuracy. NAN if unknown + */ +static inline float mavlink_msg_camera_tracking_geo_status_get_v_acc(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 16); +} + +/** + * @brief Get field vel_n from camera_tracking_geo_status message + * + * @return [m/s] North velocity of tracked object. NAN if unknown + */ +static inline float mavlink_msg_camera_tracking_geo_status_get_vel_n(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 20); +} + +/** + * @brief Get field vel_e from camera_tracking_geo_status message + * + * @return [m/s] East velocity of tracked object. NAN if unknown + */ +static inline float mavlink_msg_camera_tracking_geo_status_get_vel_e(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 24); +} + +/** + * @brief Get field vel_d from camera_tracking_geo_status message + * + * @return [m/s] Down velocity of tracked object. NAN if unknown + */ +static inline float mavlink_msg_camera_tracking_geo_status_get_vel_d(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 28); +} + +/** + * @brief Get field vel_acc from camera_tracking_geo_status message + * + * @return [m/s] Velocity accuracy. NAN if unknown + */ +static inline float mavlink_msg_camera_tracking_geo_status_get_vel_acc(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 32); +} + +/** + * @brief Get field dist from camera_tracking_geo_status message + * + * @return [m] Distance between camera and tracked object. NAN if unknown + */ +static inline float mavlink_msg_camera_tracking_geo_status_get_dist(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 36); +} + +/** + * @brief Get field hdg from camera_tracking_geo_status message + * + * @return [rad] Heading in radians, in NED. NAN if unknown + */ +static inline float mavlink_msg_camera_tracking_geo_status_get_hdg(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 40); +} + +/** + * @brief Get field hdg_acc from camera_tracking_geo_status message + * + * @return [rad] Accuracy of heading, in NED. NAN if unknown + */ +static inline float mavlink_msg_camera_tracking_geo_status_get_hdg_acc(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 44); +} + +/** + * @brief Decode a camera_tracking_geo_status message into a struct + * + * @param msg The message to decode + * @param camera_tracking_geo_status C-struct to decode the message contents into + */ +static inline void mavlink_msg_camera_tracking_geo_status_decode(const mavlink_message_t* msg, mavlink_camera_tracking_geo_status_t* camera_tracking_geo_status) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + camera_tracking_geo_status->lat = mavlink_msg_camera_tracking_geo_status_get_lat(msg); + camera_tracking_geo_status->lon = mavlink_msg_camera_tracking_geo_status_get_lon(msg); + camera_tracking_geo_status->alt = mavlink_msg_camera_tracking_geo_status_get_alt(msg); + camera_tracking_geo_status->h_acc = mavlink_msg_camera_tracking_geo_status_get_h_acc(msg); + camera_tracking_geo_status->v_acc = mavlink_msg_camera_tracking_geo_status_get_v_acc(msg); + camera_tracking_geo_status->vel_n = mavlink_msg_camera_tracking_geo_status_get_vel_n(msg); + camera_tracking_geo_status->vel_e = mavlink_msg_camera_tracking_geo_status_get_vel_e(msg); + camera_tracking_geo_status->vel_d = mavlink_msg_camera_tracking_geo_status_get_vel_d(msg); + camera_tracking_geo_status->vel_acc = mavlink_msg_camera_tracking_geo_status_get_vel_acc(msg); + camera_tracking_geo_status->dist = mavlink_msg_camera_tracking_geo_status_get_dist(msg); + camera_tracking_geo_status->hdg = mavlink_msg_camera_tracking_geo_status_get_hdg(msg); + camera_tracking_geo_status->hdg_acc = mavlink_msg_camera_tracking_geo_status_get_hdg_acc(msg); + camera_tracking_geo_status->tracking_status = mavlink_msg_camera_tracking_geo_status_get_tracking_status(msg); +#else + uint8_t len = msg->len < MAVLINK_MSG_ID_CAMERA_TRACKING_GEO_STATUS_LEN? msg->len : MAVLINK_MSG_ID_CAMERA_TRACKING_GEO_STATUS_LEN; + memset(camera_tracking_geo_status, 0, MAVLINK_MSG_ID_CAMERA_TRACKING_GEO_STATUS_LEN); + memcpy(camera_tracking_geo_status, _MAV_PAYLOAD(msg), len); +#endif +} diff --git a/lib/main/MAVLink/common/mavlink_msg_camera_tracking_image_status.h b/lib/main/MAVLink/common/mavlink_msg_camera_tracking_image_status.h new file mode 100644 index 0000000000..5f703fce5d --- /dev/null +++ b/lib/main/MAVLink/common/mavlink_msg_camera_tracking_image_status.h @@ -0,0 +1,438 @@ +#pragma once +// MESSAGE CAMERA_TRACKING_IMAGE_STATUS PACKING + +#define MAVLINK_MSG_ID_CAMERA_TRACKING_IMAGE_STATUS 275 + + +typedef struct __mavlink_camera_tracking_image_status_t { + float point_x; /*< Current tracked point x value if CAMERA_TRACKING_MODE_POINT (normalized 0..1, 0 is left, 1 is right), NAN if unknown*/ + float point_y; /*< Current tracked point y value if CAMERA_TRACKING_MODE_POINT (normalized 0..1, 0 is top, 1 is bottom), NAN if unknown*/ + float radius; /*< Current tracked radius if CAMERA_TRACKING_MODE_POINT (normalized 0..1, 0 is image left, 1 is image right), NAN if unknown*/ + float rec_top_x; /*< Current tracked rectangle top x value if CAMERA_TRACKING_MODE_RECTANGLE (normalized 0..1, 0 is left, 1 is right), NAN if unknown*/ + float rec_top_y; /*< Current tracked rectangle top y value if CAMERA_TRACKING_MODE_RECTANGLE (normalized 0..1, 0 is top, 1 is bottom), NAN if unknown*/ + float rec_bottom_x; /*< Current tracked rectangle bottom x value if CAMERA_TRACKING_MODE_RECTANGLE (normalized 0..1, 0 is left, 1 is right), NAN if unknown*/ + float rec_bottom_y; /*< Current tracked rectangle bottom y value if CAMERA_TRACKING_MODE_RECTANGLE (normalized 0..1, 0 is top, 1 is bottom), NAN if unknown*/ + uint8_t tracking_status; /*< Current tracking status*/ + uint8_t tracking_mode; /*< Current tracking mode*/ + uint8_t target_data; /*< Defines location of target data*/ +} mavlink_camera_tracking_image_status_t; + +#define MAVLINK_MSG_ID_CAMERA_TRACKING_IMAGE_STATUS_LEN 31 +#define MAVLINK_MSG_ID_CAMERA_TRACKING_IMAGE_STATUS_MIN_LEN 31 +#define MAVLINK_MSG_ID_275_LEN 31 +#define MAVLINK_MSG_ID_275_MIN_LEN 31 + +#define MAVLINK_MSG_ID_CAMERA_TRACKING_IMAGE_STATUS_CRC 126 +#define MAVLINK_MSG_ID_275_CRC 126 + + + +#if MAVLINK_COMMAND_24BIT +#define MAVLINK_MESSAGE_INFO_CAMERA_TRACKING_IMAGE_STATUS { \ + 275, \ + "CAMERA_TRACKING_IMAGE_STATUS", \ + 10, \ + { { "tracking_status", NULL, MAVLINK_TYPE_UINT8_T, 0, 28, offsetof(mavlink_camera_tracking_image_status_t, tracking_status) }, \ + { "tracking_mode", NULL, MAVLINK_TYPE_UINT8_T, 0, 29, offsetof(mavlink_camera_tracking_image_status_t, tracking_mode) }, \ + { "target_data", NULL, MAVLINK_TYPE_UINT8_T, 0, 30, offsetof(mavlink_camera_tracking_image_status_t, target_data) }, \ + { "point_x", NULL, MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_camera_tracking_image_status_t, point_x) }, \ + { "point_y", NULL, MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_camera_tracking_image_status_t, point_y) }, \ + { "radius", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_camera_tracking_image_status_t, radius) }, \ + { "rec_top_x", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_camera_tracking_image_status_t, rec_top_x) }, \ + { "rec_top_y", NULL, MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_camera_tracking_image_status_t, rec_top_y) }, \ + { "rec_bottom_x", NULL, MAVLINK_TYPE_FLOAT, 0, 20, offsetof(mavlink_camera_tracking_image_status_t, rec_bottom_x) }, \ + { "rec_bottom_y", NULL, MAVLINK_TYPE_FLOAT, 0, 24, offsetof(mavlink_camera_tracking_image_status_t, rec_bottom_y) }, \ + } \ +} +#else +#define MAVLINK_MESSAGE_INFO_CAMERA_TRACKING_IMAGE_STATUS { \ + "CAMERA_TRACKING_IMAGE_STATUS", \ + 10, \ + { { "tracking_status", NULL, MAVLINK_TYPE_UINT8_T, 0, 28, offsetof(mavlink_camera_tracking_image_status_t, tracking_status) }, \ + { "tracking_mode", NULL, MAVLINK_TYPE_UINT8_T, 0, 29, offsetof(mavlink_camera_tracking_image_status_t, tracking_mode) }, \ + { "target_data", NULL, MAVLINK_TYPE_UINT8_T, 0, 30, offsetof(mavlink_camera_tracking_image_status_t, target_data) }, \ + { "point_x", NULL, MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_camera_tracking_image_status_t, point_x) }, \ + { "point_y", NULL, MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_camera_tracking_image_status_t, point_y) }, \ + { "radius", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_camera_tracking_image_status_t, radius) }, \ + { "rec_top_x", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_camera_tracking_image_status_t, rec_top_x) }, \ + { "rec_top_y", NULL, MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_camera_tracking_image_status_t, rec_top_y) }, \ + { "rec_bottom_x", NULL, MAVLINK_TYPE_FLOAT, 0, 20, offsetof(mavlink_camera_tracking_image_status_t, rec_bottom_x) }, \ + { "rec_bottom_y", NULL, MAVLINK_TYPE_FLOAT, 0, 24, offsetof(mavlink_camera_tracking_image_status_t, rec_bottom_y) }, \ + } \ +} +#endif + +/** + * @brief Pack a camera_tracking_image_status message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param tracking_status Current tracking status + * @param tracking_mode Current tracking mode + * @param target_data Defines location of target data + * @param point_x Current tracked point x value if CAMERA_TRACKING_MODE_POINT (normalized 0..1, 0 is left, 1 is right), NAN if unknown + * @param point_y Current tracked point y value if CAMERA_TRACKING_MODE_POINT (normalized 0..1, 0 is top, 1 is bottom), NAN if unknown + * @param radius Current tracked radius if CAMERA_TRACKING_MODE_POINT (normalized 0..1, 0 is image left, 1 is image right), NAN if unknown + * @param rec_top_x Current tracked rectangle top x value if CAMERA_TRACKING_MODE_RECTANGLE (normalized 0..1, 0 is left, 1 is right), NAN if unknown + * @param rec_top_y Current tracked rectangle top y value if CAMERA_TRACKING_MODE_RECTANGLE (normalized 0..1, 0 is top, 1 is bottom), NAN if unknown + * @param rec_bottom_x Current tracked rectangle bottom x value if CAMERA_TRACKING_MODE_RECTANGLE (normalized 0..1, 0 is left, 1 is right), NAN if unknown + * @param rec_bottom_y Current tracked rectangle bottom y value if CAMERA_TRACKING_MODE_RECTANGLE (normalized 0..1, 0 is top, 1 is bottom), NAN if unknown + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_camera_tracking_image_status_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint8_t tracking_status, uint8_t tracking_mode, uint8_t target_data, float point_x, float point_y, float radius, float rec_top_x, float rec_top_y, float rec_bottom_x, float rec_bottom_y) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_CAMERA_TRACKING_IMAGE_STATUS_LEN]; + _mav_put_float(buf, 0, point_x); + _mav_put_float(buf, 4, point_y); + _mav_put_float(buf, 8, radius); + _mav_put_float(buf, 12, rec_top_x); + _mav_put_float(buf, 16, rec_top_y); + _mav_put_float(buf, 20, rec_bottom_x); + _mav_put_float(buf, 24, rec_bottom_y); + _mav_put_uint8_t(buf, 28, tracking_status); + _mav_put_uint8_t(buf, 29, tracking_mode); + _mav_put_uint8_t(buf, 30, target_data); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_CAMERA_TRACKING_IMAGE_STATUS_LEN); +#else + mavlink_camera_tracking_image_status_t packet; + packet.point_x = point_x; + packet.point_y = point_y; + packet.radius = radius; + packet.rec_top_x = rec_top_x; + packet.rec_top_y = rec_top_y; + packet.rec_bottom_x = rec_bottom_x; + packet.rec_bottom_y = rec_bottom_y; + packet.tracking_status = tracking_status; + packet.tracking_mode = tracking_mode; + packet.target_data = target_data; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_CAMERA_TRACKING_IMAGE_STATUS_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_CAMERA_TRACKING_IMAGE_STATUS; + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_CAMERA_TRACKING_IMAGE_STATUS_MIN_LEN, MAVLINK_MSG_ID_CAMERA_TRACKING_IMAGE_STATUS_LEN, MAVLINK_MSG_ID_CAMERA_TRACKING_IMAGE_STATUS_CRC); +} + +/** + * @brief Pack a camera_tracking_image_status message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param tracking_status Current tracking status + * @param tracking_mode Current tracking mode + * @param target_data Defines location of target data + * @param point_x Current tracked point x value if CAMERA_TRACKING_MODE_POINT (normalized 0..1, 0 is left, 1 is right), NAN if unknown + * @param point_y Current tracked point y value if CAMERA_TRACKING_MODE_POINT (normalized 0..1, 0 is top, 1 is bottom), NAN if unknown + * @param radius Current tracked radius if CAMERA_TRACKING_MODE_POINT (normalized 0..1, 0 is image left, 1 is image right), NAN if unknown + * @param rec_top_x Current tracked rectangle top x value if CAMERA_TRACKING_MODE_RECTANGLE (normalized 0..1, 0 is left, 1 is right), NAN if unknown + * @param rec_top_y Current tracked rectangle top y value if CAMERA_TRACKING_MODE_RECTANGLE (normalized 0..1, 0 is top, 1 is bottom), NAN if unknown + * @param rec_bottom_x Current tracked rectangle bottom x value if CAMERA_TRACKING_MODE_RECTANGLE (normalized 0..1, 0 is left, 1 is right), NAN if unknown + * @param rec_bottom_y Current tracked rectangle bottom y value if CAMERA_TRACKING_MODE_RECTANGLE (normalized 0..1, 0 is top, 1 is bottom), NAN if unknown + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_camera_tracking_image_status_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint8_t tracking_status,uint8_t tracking_mode,uint8_t target_data,float point_x,float point_y,float radius,float rec_top_x,float rec_top_y,float rec_bottom_x,float rec_bottom_y) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_CAMERA_TRACKING_IMAGE_STATUS_LEN]; + _mav_put_float(buf, 0, point_x); + _mav_put_float(buf, 4, point_y); + _mav_put_float(buf, 8, radius); + _mav_put_float(buf, 12, rec_top_x); + _mav_put_float(buf, 16, rec_top_y); + _mav_put_float(buf, 20, rec_bottom_x); + _mav_put_float(buf, 24, rec_bottom_y); + _mav_put_uint8_t(buf, 28, tracking_status); + _mav_put_uint8_t(buf, 29, tracking_mode); + _mav_put_uint8_t(buf, 30, target_data); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_CAMERA_TRACKING_IMAGE_STATUS_LEN); +#else + mavlink_camera_tracking_image_status_t packet; + packet.point_x = point_x; + packet.point_y = point_y; + packet.radius = radius; + packet.rec_top_x = rec_top_x; + packet.rec_top_y = rec_top_y; + packet.rec_bottom_x = rec_bottom_x; + packet.rec_bottom_y = rec_bottom_y; + packet.tracking_status = tracking_status; + packet.tracking_mode = tracking_mode; + packet.target_data = target_data; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_CAMERA_TRACKING_IMAGE_STATUS_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_CAMERA_TRACKING_IMAGE_STATUS; + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_CAMERA_TRACKING_IMAGE_STATUS_MIN_LEN, MAVLINK_MSG_ID_CAMERA_TRACKING_IMAGE_STATUS_LEN, MAVLINK_MSG_ID_CAMERA_TRACKING_IMAGE_STATUS_CRC); +} + +/** + * @brief Encode a camera_tracking_image_status struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param camera_tracking_image_status C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_camera_tracking_image_status_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_camera_tracking_image_status_t* camera_tracking_image_status) +{ + return mavlink_msg_camera_tracking_image_status_pack(system_id, component_id, msg, camera_tracking_image_status->tracking_status, camera_tracking_image_status->tracking_mode, camera_tracking_image_status->target_data, camera_tracking_image_status->point_x, camera_tracking_image_status->point_y, camera_tracking_image_status->radius, camera_tracking_image_status->rec_top_x, camera_tracking_image_status->rec_top_y, camera_tracking_image_status->rec_bottom_x, camera_tracking_image_status->rec_bottom_y); +} + +/** + * @brief Encode a camera_tracking_image_status struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param camera_tracking_image_status C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_camera_tracking_image_status_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_camera_tracking_image_status_t* camera_tracking_image_status) +{ + return mavlink_msg_camera_tracking_image_status_pack_chan(system_id, component_id, chan, msg, camera_tracking_image_status->tracking_status, camera_tracking_image_status->tracking_mode, camera_tracking_image_status->target_data, camera_tracking_image_status->point_x, camera_tracking_image_status->point_y, camera_tracking_image_status->radius, camera_tracking_image_status->rec_top_x, camera_tracking_image_status->rec_top_y, camera_tracking_image_status->rec_bottom_x, camera_tracking_image_status->rec_bottom_y); +} + +/** + * @brief Send a camera_tracking_image_status message + * @param chan MAVLink channel to send the message + * + * @param tracking_status Current tracking status + * @param tracking_mode Current tracking mode + * @param target_data Defines location of target data + * @param point_x Current tracked point x value if CAMERA_TRACKING_MODE_POINT (normalized 0..1, 0 is left, 1 is right), NAN if unknown + * @param point_y Current tracked point y value if CAMERA_TRACKING_MODE_POINT (normalized 0..1, 0 is top, 1 is bottom), NAN if unknown + * @param radius Current tracked radius if CAMERA_TRACKING_MODE_POINT (normalized 0..1, 0 is image left, 1 is image right), NAN if unknown + * @param rec_top_x Current tracked rectangle top x value if CAMERA_TRACKING_MODE_RECTANGLE (normalized 0..1, 0 is left, 1 is right), NAN if unknown + * @param rec_top_y Current tracked rectangle top y value if CAMERA_TRACKING_MODE_RECTANGLE (normalized 0..1, 0 is top, 1 is bottom), NAN if unknown + * @param rec_bottom_x Current tracked rectangle bottom x value if CAMERA_TRACKING_MODE_RECTANGLE (normalized 0..1, 0 is left, 1 is right), NAN if unknown + * @param rec_bottom_y Current tracked rectangle bottom y value if CAMERA_TRACKING_MODE_RECTANGLE (normalized 0..1, 0 is top, 1 is bottom), NAN if unknown + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_camera_tracking_image_status_send(mavlink_channel_t chan, uint8_t tracking_status, uint8_t tracking_mode, uint8_t target_data, float point_x, float point_y, float radius, float rec_top_x, float rec_top_y, float rec_bottom_x, float rec_bottom_y) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_CAMERA_TRACKING_IMAGE_STATUS_LEN]; + _mav_put_float(buf, 0, point_x); + _mav_put_float(buf, 4, point_y); + _mav_put_float(buf, 8, radius); + _mav_put_float(buf, 12, rec_top_x); + _mav_put_float(buf, 16, rec_top_y); + _mav_put_float(buf, 20, rec_bottom_x); + _mav_put_float(buf, 24, rec_bottom_y); + _mav_put_uint8_t(buf, 28, tracking_status); + _mav_put_uint8_t(buf, 29, tracking_mode); + _mav_put_uint8_t(buf, 30, target_data); + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CAMERA_TRACKING_IMAGE_STATUS, buf, MAVLINK_MSG_ID_CAMERA_TRACKING_IMAGE_STATUS_MIN_LEN, MAVLINK_MSG_ID_CAMERA_TRACKING_IMAGE_STATUS_LEN, MAVLINK_MSG_ID_CAMERA_TRACKING_IMAGE_STATUS_CRC); +#else + mavlink_camera_tracking_image_status_t packet; + packet.point_x = point_x; + packet.point_y = point_y; + packet.radius = radius; + packet.rec_top_x = rec_top_x; + packet.rec_top_y = rec_top_y; + packet.rec_bottom_x = rec_bottom_x; + packet.rec_bottom_y = rec_bottom_y; + packet.tracking_status = tracking_status; + packet.tracking_mode = tracking_mode; + packet.target_data = target_data; + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CAMERA_TRACKING_IMAGE_STATUS, (const char *)&packet, MAVLINK_MSG_ID_CAMERA_TRACKING_IMAGE_STATUS_MIN_LEN, MAVLINK_MSG_ID_CAMERA_TRACKING_IMAGE_STATUS_LEN, MAVLINK_MSG_ID_CAMERA_TRACKING_IMAGE_STATUS_CRC); +#endif +} + +/** + * @brief Send a camera_tracking_image_status message + * @param chan MAVLink channel to send the message + * @param struct The MAVLink struct to serialize + */ +static inline void mavlink_msg_camera_tracking_image_status_send_struct(mavlink_channel_t chan, const mavlink_camera_tracking_image_status_t* camera_tracking_image_status) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + mavlink_msg_camera_tracking_image_status_send(chan, camera_tracking_image_status->tracking_status, camera_tracking_image_status->tracking_mode, camera_tracking_image_status->target_data, camera_tracking_image_status->point_x, camera_tracking_image_status->point_y, camera_tracking_image_status->radius, camera_tracking_image_status->rec_top_x, camera_tracking_image_status->rec_top_y, camera_tracking_image_status->rec_bottom_x, camera_tracking_image_status->rec_bottom_y); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CAMERA_TRACKING_IMAGE_STATUS, (const char *)camera_tracking_image_status, MAVLINK_MSG_ID_CAMERA_TRACKING_IMAGE_STATUS_MIN_LEN, MAVLINK_MSG_ID_CAMERA_TRACKING_IMAGE_STATUS_LEN, MAVLINK_MSG_ID_CAMERA_TRACKING_IMAGE_STATUS_CRC); +#endif +} + +#if MAVLINK_MSG_ID_CAMERA_TRACKING_IMAGE_STATUS_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_camera_tracking_image_status_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t tracking_status, uint8_t tracking_mode, uint8_t target_data, float point_x, float point_y, float radius, float rec_top_x, float rec_top_y, float rec_bottom_x, float rec_bottom_y) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_float(buf, 0, point_x); + _mav_put_float(buf, 4, point_y); + _mav_put_float(buf, 8, radius); + _mav_put_float(buf, 12, rec_top_x); + _mav_put_float(buf, 16, rec_top_y); + _mav_put_float(buf, 20, rec_bottom_x); + _mav_put_float(buf, 24, rec_bottom_y); + _mav_put_uint8_t(buf, 28, tracking_status); + _mav_put_uint8_t(buf, 29, tracking_mode); + _mav_put_uint8_t(buf, 30, target_data); + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CAMERA_TRACKING_IMAGE_STATUS, buf, MAVLINK_MSG_ID_CAMERA_TRACKING_IMAGE_STATUS_MIN_LEN, MAVLINK_MSG_ID_CAMERA_TRACKING_IMAGE_STATUS_LEN, MAVLINK_MSG_ID_CAMERA_TRACKING_IMAGE_STATUS_CRC); +#else + mavlink_camera_tracking_image_status_t *packet = (mavlink_camera_tracking_image_status_t *)msgbuf; + packet->point_x = point_x; + packet->point_y = point_y; + packet->radius = radius; + packet->rec_top_x = rec_top_x; + packet->rec_top_y = rec_top_y; + packet->rec_bottom_x = rec_bottom_x; + packet->rec_bottom_y = rec_bottom_y; + packet->tracking_status = tracking_status; + packet->tracking_mode = tracking_mode; + packet->target_data = target_data; + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CAMERA_TRACKING_IMAGE_STATUS, (const char *)packet, MAVLINK_MSG_ID_CAMERA_TRACKING_IMAGE_STATUS_MIN_LEN, MAVLINK_MSG_ID_CAMERA_TRACKING_IMAGE_STATUS_LEN, MAVLINK_MSG_ID_CAMERA_TRACKING_IMAGE_STATUS_CRC); +#endif +} +#endif + +#endif + +// MESSAGE CAMERA_TRACKING_IMAGE_STATUS UNPACKING + + +/** + * @brief Get field tracking_status from camera_tracking_image_status message + * + * @return Current tracking status + */ +static inline uint8_t mavlink_msg_camera_tracking_image_status_get_tracking_status(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 28); +} + +/** + * @brief Get field tracking_mode from camera_tracking_image_status message + * + * @return Current tracking mode + */ +static inline uint8_t mavlink_msg_camera_tracking_image_status_get_tracking_mode(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 29); +} + +/** + * @brief Get field target_data from camera_tracking_image_status message + * + * @return Defines location of target data + */ +static inline uint8_t mavlink_msg_camera_tracking_image_status_get_target_data(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 30); +} + +/** + * @brief Get field point_x from camera_tracking_image_status message + * + * @return Current tracked point x value if CAMERA_TRACKING_MODE_POINT (normalized 0..1, 0 is left, 1 is right), NAN if unknown + */ +static inline float mavlink_msg_camera_tracking_image_status_get_point_x(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 0); +} + +/** + * @brief Get field point_y from camera_tracking_image_status message + * + * @return Current tracked point y value if CAMERA_TRACKING_MODE_POINT (normalized 0..1, 0 is top, 1 is bottom), NAN if unknown + */ +static inline float mavlink_msg_camera_tracking_image_status_get_point_y(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 4); +} + +/** + * @brief Get field radius from camera_tracking_image_status message + * + * @return Current tracked radius if CAMERA_TRACKING_MODE_POINT (normalized 0..1, 0 is image left, 1 is image right), NAN if unknown + */ +static inline float mavlink_msg_camera_tracking_image_status_get_radius(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 8); +} + +/** + * @brief Get field rec_top_x from camera_tracking_image_status message + * + * @return Current tracked rectangle top x value if CAMERA_TRACKING_MODE_RECTANGLE (normalized 0..1, 0 is left, 1 is right), NAN if unknown + */ +static inline float mavlink_msg_camera_tracking_image_status_get_rec_top_x(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 12); +} + +/** + * @brief Get field rec_top_y from camera_tracking_image_status message + * + * @return Current tracked rectangle top y value if CAMERA_TRACKING_MODE_RECTANGLE (normalized 0..1, 0 is top, 1 is bottom), NAN if unknown + */ +static inline float mavlink_msg_camera_tracking_image_status_get_rec_top_y(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 16); +} + +/** + * @brief Get field rec_bottom_x from camera_tracking_image_status message + * + * @return Current tracked rectangle bottom x value if CAMERA_TRACKING_MODE_RECTANGLE (normalized 0..1, 0 is left, 1 is right), NAN if unknown + */ +static inline float mavlink_msg_camera_tracking_image_status_get_rec_bottom_x(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 20); +} + +/** + * @brief Get field rec_bottom_y from camera_tracking_image_status message + * + * @return Current tracked rectangle bottom y value if CAMERA_TRACKING_MODE_RECTANGLE (normalized 0..1, 0 is top, 1 is bottom), NAN if unknown + */ +static inline float mavlink_msg_camera_tracking_image_status_get_rec_bottom_y(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 24); +} + +/** + * @brief Decode a camera_tracking_image_status message into a struct + * + * @param msg The message to decode + * @param camera_tracking_image_status C-struct to decode the message contents into + */ +static inline void mavlink_msg_camera_tracking_image_status_decode(const mavlink_message_t* msg, mavlink_camera_tracking_image_status_t* camera_tracking_image_status) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + camera_tracking_image_status->point_x = mavlink_msg_camera_tracking_image_status_get_point_x(msg); + camera_tracking_image_status->point_y = mavlink_msg_camera_tracking_image_status_get_point_y(msg); + camera_tracking_image_status->radius = mavlink_msg_camera_tracking_image_status_get_radius(msg); + camera_tracking_image_status->rec_top_x = mavlink_msg_camera_tracking_image_status_get_rec_top_x(msg); + camera_tracking_image_status->rec_top_y = mavlink_msg_camera_tracking_image_status_get_rec_top_y(msg); + camera_tracking_image_status->rec_bottom_x = mavlink_msg_camera_tracking_image_status_get_rec_bottom_x(msg); + camera_tracking_image_status->rec_bottom_y = mavlink_msg_camera_tracking_image_status_get_rec_bottom_y(msg); + camera_tracking_image_status->tracking_status = mavlink_msg_camera_tracking_image_status_get_tracking_status(msg); + camera_tracking_image_status->tracking_mode = mavlink_msg_camera_tracking_image_status_get_tracking_mode(msg); + camera_tracking_image_status->target_data = mavlink_msg_camera_tracking_image_status_get_target_data(msg); +#else + uint8_t len = msg->len < MAVLINK_MSG_ID_CAMERA_TRACKING_IMAGE_STATUS_LEN? msg->len : MAVLINK_MSG_ID_CAMERA_TRACKING_IMAGE_STATUS_LEN; + memset(camera_tracking_image_status, 0, MAVLINK_MSG_ID_CAMERA_TRACKING_IMAGE_STATUS_LEN); + memcpy(camera_tracking_image_status, _MAV_PAYLOAD(msg), len); +#endif +} diff --git a/lib/main/MAVLink/common/mavlink_msg_cellular_config.h b/lib/main/MAVLink/common/mavlink_msg_cellular_config.h new file mode 100644 index 0000000000..5b8a1fb763 --- /dev/null +++ b/lib/main/MAVLink/common/mavlink_msg_cellular_config.h @@ -0,0 +1,383 @@ +#pragma once +// MESSAGE CELLULAR_CONFIG PACKING + +#define MAVLINK_MSG_ID_CELLULAR_CONFIG 336 + + +typedef struct __mavlink_cellular_config_t { + uint8_t enable_lte; /*< Enable/disable LTE. 0: setting unchanged, 1: disabled, 2: enabled. Current setting when sent back as a response.*/ + uint8_t enable_pin; /*< Enable/disable PIN on the SIM card. 0: setting unchanged, 1: disabled, 2: enabled. Current setting when sent back as a response.*/ + char pin[16]; /*< PIN sent to the SIM card. Blank when PIN is disabled. Empty when message is sent back as a response.*/ + char new_pin[16]; /*< New PIN when changing the PIN. Blank to leave it unchanged. Empty when message is sent back as a response.*/ + char apn[32]; /*< Name of the cellular APN. Blank to leave it unchanged. Current APN when sent back as a response.*/ + char puk[16]; /*< Required PUK code in case the user failed to authenticate 3 times with the PIN. Empty when message is sent back as a response.*/ + uint8_t roaming; /*< Enable/disable roaming. 0: setting unchanged, 1: disabled, 2: enabled. Current setting when sent back as a response.*/ + uint8_t response; /*< Message acceptance response (sent back to GS).*/ +} mavlink_cellular_config_t; + +#define MAVLINK_MSG_ID_CELLULAR_CONFIG_LEN 84 +#define MAVLINK_MSG_ID_CELLULAR_CONFIG_MIN_LEN 84 +#define MAVLINK_MSG_ID_336_LEN 84 +#define MAVLINK_MSG_ID_336_MIN_LEN 84 + +#define MAVLINK_MSG_ID_CELLULAR_CONFIG_CRC 245 +#define MAVLINK_MSG_ID_336_CRC 245 + +#define MAVLINK_MSG_CELLULAR_CONFIG_FIELD_PIN_LEN 16 +#define MAVLINK_MSG_CELLULAR_CONFIG_FIELD_NEW_PIN_LEN 16 +#define MAVLINK_MSG_CELLULAR_CONFIG_FIELD_APN_LEN 32 +#define MAVLINK_MSG_CELLULAR_CONFIG_FIELD_PUK_LEN 16 + +#if MAVLINK_COMMAND_24BIT +#define MAVLINK_MESSAGE_INFO_CELLULAR_CONFIG { \ + 336, \ + "CELLULAR_CONFIG", \ + 8, \ + { { "enable_lte", NULL, MAVLINK_TYPE_UINT8_T, 0, 0, offsetof(mavlink_cellular_config_t, enable_lte) }, \ + { "enable_pin", NULL, MAVLINK_TYPE_UINT8_T, 0, 1, offsetof(mavlink_cellular_config_t, enable_pin) }, \ + { "pin", NULL, MAVLINK_TYPE_CHAR, 16, 2, offsetof(mavlink_cellular_config_t, pin) }, \ + { "new_pin", NULL, MAVLINK_TYPE_CHAR, 16, 18, offsetof(mavlink_cellular_config_t, new_pin) }, \ + { "apn", NULL, MAVLINK_TYPE_CHAR, 32, 34, offsetof(mavlink_cellular_config_t, apn) }, \ + { "puk", NULL, MAVLINK_TYPE_CHAR, 16, 66, offsetof(mavlink_cellular_config_t, puk) }, \ + { "roaming", NULL, MAVLINK_TYPE_UINT8_T, 0, 82, offsetof(mavlink_cellular_config_t, roaming) }, \ + { "response", NULL, MAVLINK_TYPE_UINT8_T, 0, 83, offsetof(mavlink_cellular_config_t, response) }, \ + } \ +} +#else +#define MAVLINK_MESSAGE_INFO_CELLULAR_CONFIG { \ + "CELLULAR_CONFIG", \ + 8, \ + { { "enable_lte", NULL, MAVLINK_TYPE_UINT8_T, 0, 0, offsetof(mavlink_cellular_config_t, enable_lte) }, \ + { "enable_pin", NULL, MAVLINK_TYPE_UINT8_T, 0, 1, offsetof(mavlink_cellular_config_t, enable_pin) }, \ + { "pin", NULL, MAVLINK_TYPE_CHAR, 16, 2, offsetof(mavlink_cellular_config_t, pin) }, \ + { "new_pin", NULL, MAVLINK_TYPE_CHAR, 16, 18, offsetof(mavlink_cellular_config_t, new_pin) }, \ + { "apn", NULL, MAVLINK_TYPE_CHAR, 32, 34, offsetof(mavlink_cellular_config_t, apn) }, \ + { "puk", NULL, MAVLINK_TYPE_CHAR, 16, 66, offsetof(mavlink_cellular_config_t, puk) }, \ + { "roaming", NULL, MAVLINK_TYPE_UINT8_T, 0, 82, offsetof(mavlink_cellular_config_t, roaming) }, \ + { "response", NULL, MAVLINK_TYPE_UINT8_T, 0, 83, offsetof(mavlink_cellular_config_t, response) }, \ + } \ +} +#endif + +/** + * @brief Pack a cellular_config message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param enable_lte Enable/disable LTE. 0: setting unchanged, 1: disabled, 2: enabled. Current setting when sent back as a response. + * @param enable_pin Enable/disable PIN on the SIM card. 0: setting unchanged, 1: disabled, 2: enabled. Current setting when sent back as a response. + * @param pin PIN sent to the SIM card. Blank when PIN is disabled. Empty when message is sent back as a response. + * @param new_pin New PIN when changing the PIN. Blank to leave it unchanged. Empty when message is sent back as a response. + * @param apn Name of the cellular APN. Blank to leave it unchanged. Current APN when sent back as a response. + * @param puk Required PUK code in case the user failed to authenticate 3 times with the PIN. Empty when message is sent back as a response. + * @param roaming Enable/disable roaming. 0: setting unchanged, 1: disabled, 2: enabled. Current setting when sent back as a response. + * @param response Message acceptance response (sent back to GS). + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_cellular_config_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint8_t enable_lte, uint8_t enable_pin, const char *pin, const char *new_pin, const char *apn, const char *puk, uint8_t roaming, uint8_t response) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_CELLULAR_CONFIG_LEN]; + _mav_put_uint8_t(buf, 0, enable_lte); + _mav_put_uint8_t(buf, 1, enable_pin); + _mav_put_uint8_t(buf, 82, roaming); + _mav_put_uint8_t(buf, 83, response); + _mav_put_char_array(buf, 2, pin, 16); + _mav_put_char_array(buf, 18, new_pin, 16); + _mav_put_char_array(buf, 34, apn, 32); + _mav_put_char_array(buf, 66, puk, 16); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_CELLULAR_CONFIG_LEN); +#else + mavlink_cellular_config_t packet; + packet.enable_lte = enable_lte; + packet.enable_pin = enable_pin; + packet.roaming = roaming; + packet.response = response; + mav_array_memcpy(packet.pin, pin, sizeof(char)*16); + mav_array_memcpy(packet.new_pin, new_pin, sizeof(char)*16); + mav_array_memcpy(packet.apn, apn, sizeof(char)*32); + mav_array_memcpy(packet.puk, puk, sizeof(char)*16); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_CELLULAR_CONFIG_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_CELLULAR_CONFIG; + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_CELLULAR_CONFIG_MIN_LEN, MAVLINK_MSG_ID_CELLULAR_CONFIG_LEN, MAVLINK_MSG_ID_CELLULAR_CONFIG_CRC); +} + +/** + * @brief Pack a cellular_config message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param enable_lte Enable/disable LTE. 0: setting unchanged, 1: disabled, 2: enabled. Current setting when sent back as a response. + * @param enable_pin Enable/disable PIN on the SIM card. 0: setting unchanged, 1: disabled, 2: enabled. Current setting when sent back as a response. + * @param pin PIN sent to the SIM card. Blank when PIN is disabled. Empty when message is sent back as a response. + * @param new_pin New PIN when changing the PIN. Blank to leave it unchanged. Empty when message is sent back as a response. + * @param apn Name of the cellular APN. Blank to leave it unchanged. Current APN when sent back as a response. + * @param puk Required PUK code in case the user failed to authenticate 3 times with the PIN. Empty when message is sent back as a response. + * @param roaming Enable/disable roaming. 0: setting unchanged, 1: disabled, 2: enabled. Current setting when sent back as a response. + * @param response Message acceptance response (sent back to GS). + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_cellular_config_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint8_t enable_lte,uint8_t enable_pin,const char *pin,const char *new_pin,const char *apn,const char *puk,uint8_t roaming,uint8_t response) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_CELLULAR_CONFIG_LEN]; + _mav_put_uint8_t(buf, 0, enable_lte); + _mav_put_uint8_t(buf, 1, enable_pin); + _mav_put_uint8_t(buf, 82, roaming); + _mav_put_uint8_t(buf, 83, response); + _mav_put_char_array(buf, 2, pin, 16); + _mav_put_char_array(buf, 18, new_pin, 16); + _mav_put_char_array(buf, 34, apn, 32); + _mav_put_char_array(buf, 66, puk, 16); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_CELLULAR_CONFIG_LEN); +#else + mavlink_cellular_config_t packet; + packet.enable_lte = enable_lte; + packet.enable_pin = enable_pin; + packet.roaming = roaming; + packet.response = response; + mav_array_memcpy(packet.pin, pin, sizeof(char)*16); + mav_array_memcpy(packet.new_pin, new_pin, sizeof(char)*16); + mav_array_memcpy(packet.apn, apn, sizeof(char)*32); + mav_array_memcpy(packet.puk, puk, sizeof(char)*16); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_CELLULAR_CONFIG_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_CELLULAR_CONFIG; + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_CELLULAR_CONFIG_MIN_LEN, MAVLINK_MSG_ID_CELLULAR_CONFIG_LEN, MAVLINK_MSG_ID_CELLULAR_CONFIG_CRC); +} + +/** + * @brief Encode a cellular_config struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param cellular_config C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_cellular_config_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_cellular_config_t* cellular_config) +{ + return mavlink_msg_cellular_config_pack(system_id, component_id, msg, cellular_config->enable_lte, cellular_config->enable_pin, cellular_config->pin, cellular_config->new_pin, cellular_config->apn, cellular_config->puk, cellular_config->roaming, cellular_config->response); +} + +/** + * @brief Encode a cellular_config struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param cellular_config C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_cellular_config_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_cellular_config_t* cellular_config) +{ + return mavlink_msg_cellular_config_pack_chan(system_id, component_id, chan, msg, cellular_config->enable_lte, cellular_config->enable_pin, cellular_config->pin, cellular_config->new_pin, cellular_config->apn, cellular_config->puk, cellular_config->roaming, cellular_config->response); +} + +/** + * @brief Send a cellular_config message + * @param chan MAVLink channel to send the message + * + * @param enable_lte Enable/disable LTE. 0: setting unchanged, 1: disabled, 2: enabled. Current setting when sent back as a response. + * @param enable_pin Enable/disable PIN on the SIM card. 0: setting unchanged, 1: disabled, 2: enabled. Current setting when sent back as a response. + * @param pin PIN sent to the SIM card. Blank when PIN is disabled. Empty when message is sent back as a response. + * @param new_pin New PIN when changing the PIN. Blank to leave it unchanged. Empty when message is sent back as a response. + * @param apn Name of the cellular APN. Blank to leave it unchanged. Current APN when sent back as a response. + * @param puk Required PUK code in case the user failed to authenticate 3 times with the PIN. Empty when message is sent back as a response. + * @param roaming Enable/disable roaming. 0: setting unchanged, 1: disabled, 2: enabled. Current setting when sent back as a response. + * @param response Message acceptance response (sent back to GS). + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_cellular_config_send(mavlink_channel_t chan, uint8_t enable_lte, uint8_t enable_pin, const char *pin, const char *new_pin, const char *apn, const char *puk, uint8_t roaming, uint8_t response) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_CELLULAR_CONFIG_LEN]; + _mav_put_uint8_t(buf, 0, enable_lte); + _mav_put_uint8_t(buf, 1, enable_pin); + _mav_put_uint8_t(buf, 82, roaming); + _mav_put_uint8_t(buf, 83, response); + _mav_put_char_array(buf, 2, pin, 16); + _mav_put_char_array(buf, 18, new_pin, 16); + _mav_put_char_array(buf, 34, apn, 32); + _mav_put_char_array(buf, 66, puk, 16); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CELLULAR_CONFIG, buf, MAVLINK_MSG_ID_CELLULAR_CONFIG_MIN_LEN, MAVLINK_MSG_ID_CELLULAR_CONFIG_LEN, MAVLINK_MSG_ID_CELLULAR_CONFIG_CRC); +#else + mavlink_cellular_config_t packet; + packet.enable_lte = enable_lte; + packet.enable_pin = enable_pin; + packet.roaming = roaming; + packet.response = response; + mav_array_memcpy(packet.pin, pin, sizeof(char)*16); + mav_array_memcpy(packet.new_pin, new_pin, sizeof(char)*16); + mav_array_memcpy(packet.apn, apn, sizeof(char)*32); + mav_array_memcpy(packet.puk, puk, sizeof(char)*16); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CELLULAR_CONFIG, (const char *)&packet, MAVLINK_MSG_ID_CELLULAR_CONFIG_MIN_LEN, MAVLINK_MSG_ID_CELLULAR_CONFIG_LEN, MAVLINK_MSG_ID_CELLULAR_CONFIG_CRC); +#endif +} + +/** + * @brief Send a cellular_config message + * @param chan MAVLink channel to send the message + * @param struct The MAVLink struct to serialize + */ +static inline void mavlink_msg_cellular_config_send_struct(mavlink_channel_t chan, const mavlink_cellular_config_t* cellular_config) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + mavlink_msg_cellular_config_send(chan, cellular_config->enable_lte, cellular_config->enable_pin, cellular_config->pin, cellular_config->new_pin, cellular_config->apn, cellular_config->puk, cellular_config->roaming, cellular_config->response); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CELLULAR_CONFIG, (const char *)cellular_config, MAVLINK_MSG_ID_CELLULAR_CONFIG_MIN_LEN, MAVLINK_MSG_ID_CELLULAR_CONFIG_LEN, MAVLINK_MSG_ID_CELLULAR_CONFIG_CRC); +#endif +} + +#if MAVLINK_MSG_ID_CELLULAR_CONFIG_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_cellular_config_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t enable_lte, uint8_t enable_pin, const char *pin, const char *new_pin, const char *apn, const char *puk, uint8_t roaming, uint8_t response) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint8_t(buf, 0, enable_lte); + _mav_put_uint8_t(buf, 1, enable_pin); + _mav_put_uint8_t(buf, 82, roaming); + _mav_put_uint8_t(buf, 83, response); + _mav_put_char_array(buf, 2, pin, 16); + _mav_put_char_array(buf, 18, new_pin, 16); + _mav_put_char_array(buf, 34, apn, 32); + _mav_put_char_array(buf, 66, puk, 16); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CELLULAR_CONFIG, buf, MAVLINK_MSG_ID_CELLULAR_CONFIG_MIN_LEN, MAVLINK_MSG_ID_CELLULAR_CONFIG_LEN, MAVLINK_MSG_ID_CELLULAR_CONFIG_CRC); +#else + mavlink_cellular_config_t *packet = (mavlink_cellular_config_t *)msgbuf; + packet->enable_lte = enable_lte; + packet->enable_pin = enable_pin; + packet->roaming = roaming; + packet->response = response; + mav_array_memcpy(packet->pin, pin, sizeof(char)*16); + mav_array_memcpy(packet->new_pin, new_pin, sizeof(char)*16); + mav_array_memcpy(packet->apn, apn, sizeof(char)*32); + mav_array_memcpy(packet->puk, puk, sizeof(char)*16); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CELLULAR_CONFIG, (const char *)packet, MAVLINK_MSG_ID_CELLULAR_CONFIG_MIN_LEN, MAVLINK_MSG_ID_CELLULAR_CONFIG_LEN, MAVLINK_MSG_ID_CELLULAR_CONFIG_CRC); +#endif +} +#endif + +#endif + +// MESSAGE CELLULAR_CONFIG UNPACKING + + +/** + * @brief Get field enable_lte from cellular_config message + * + * @return Enable/disable LTE. 0: setting unchanged, 1: disabled, 2: enabled. Current setting when sent back as a response. + */ +static inline uint8_t mavlink_msg_cellular_config_get_enable_lte(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 0); +} + +/** + * @brief Get field enable_pin from cellular_config message + * + * @return Enable/disable PIN on the SIM card. 0: setting unchanged, 1: disabled, 2: enabled. Current setting when sent back as a response. + */ +static inline uint8_t mavlink_msg_cellular_config_get_enable_pin(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 1); +} + +/** + * @brief Get field pin from cellular_config message + * + * @return PIN sent to the SIM card. Blank when PIN is disabled. Empty when message is sent back as a response. + */ +static inline uint16_t mavlink_msg_cellular_config_get_pin(const mavlink_message_t* msg, char *pin) +{ + return _MAV_RETURN_char_array(msg, pin, 16, 2); +} + +/** + * @brief Get field new_pin from cellular_config message + * + * @return New PIN when changing the PIN. Blank to leave it unchanged. Empty when message is sent back as a response. + */ +static inline uint16_t mavlink_msg_cellular_config_get_new_pin(const mavlink_message_t* msg, char *new_pin) +{ + return _MAV_RETURN_char_array(msg, new_pin, 16, 18); +} + +/** + * @brief Get field apn from cellular_config message + * + * @return Name of the cellular APN. Blank to leave it unchanged. Current APN when sent back as a response. + */ +static inline uint16_t mavlink_msg_cellular_config_get_apn(const mavlink_message_t* msg, char *apn) +{ + return _MAV_RETURN_char_array(msg, apn, 32, 34); +} + +/** + * @brief Get field puk from cellular_config message + * + * @return Required PUK code in case the user failed to authenticate 3 times with the PIN. Empty when message is sent back as a response. + */ +static inline uint16_t mavlink_msg_cellular_config_get_puk(const mavlink_message_t* msg, char *puk) +{ + return _MAV_RETURN_char_array(msg, puk, 16, 66); +} + +/** + * @brief Get field roaming from cellular_config message + * + * @return Enable/disable roaming. 0: setting unchanged, 1: disabled, 2: enabled. Current setting when sent back as a response. + */ +static inline uint8_t mavlink_msg_cellular_config_get_roaming(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 82); +} + +/** + * @brief Get field response from cellular_config message + * + * @return Message acceptance response (sent back to GS). + */ +static inline uint8_t mavlink_msg_cellular_config_get_response(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 83); +} + +/** + * @brief Decode a cellular_config message into a struct + * + * @param msg The message to decode + * @param cellular_config C-struct to decode the message contents into + */ +static inline void mavlink_msg_cellular_config_decode(const mavlink_message_t* msg, mavlink_cellular_config_t* cellular_config) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + cellular_config->enable_lte = mavlink_msg_cellular_config_get_enable_lte(msg); + cellular_config->enable_pin = mavlink_msg_cellular_config_get_enable_pin(msg); + mavlink_msg_cellular_config_get_pin(msg, cellular_config->pin); + mavlink_msg_cellular_config_get_new_pin(msg, cellular_config->new_pin); + mavlink_msg_cellular_config_get_apn(msg, cellular_config->apn); + mavlink_msg_cellular_config_get_puk(msg, cellular_config->puk); + cellular_config->roaming = mavlink_msg_cellular_config_get_roaming(msg); + cellular_config->response = mavlink_msg_cellular_config_get_response(msg); +#else + uint8_t len = msg->len < MAVLINK_MSG_ID_CELLULAR_CONFIG_LEN? msg->len : MAVLINK_MSG_ID_CELLULAR_CONFIG_LEN; + memset(cellular_config, 0, MAVLINK_MSG_ID_CELLULAR_CONFIG_LEN); + memcpy(cellular_config, _MAV_PAYLOAD(msg), len); +#endif +} diff --git a/lib/main/MAVLink/common/mavlink_msg_cellular_status.h b/lib/main/MAVLink/common/mavlink_msg_cellular_status.h new file mode 100644 index 0000000000..5b1c195805 --- /dev/null +++ b/lib/main/MAVLink/common/mavlink_msg_cellular_status.h @@ -0,0 +1,363 @@ +#pragma once +// MESSAGE CELLULAR_STATUS PACKING + +#define MAVLINK_MSG_ID_CELLULAR_STATUS 334 + + +typedef struct __mavlink_cellular_status_t { + uint16_t mcc; /*< Mobile country code. If unknown, set to UINT16_MAX*/ + uint16_t mnc; /*< Mobile network code. If unknown, set to UINT16_MAX*/ + uint16_t lac; /*< Location area code. If unknown, set to 0*/ + uint8_t status; /*< Cellular modem status*/ + uint8_t failure_reason; /*< Failure reason when status in in CELLUAR_STATUS_FAILED*/ + uint8_t type; /*< Cellular network radio type: gsm, cdma, lte...*/ + uint8_t quality; /*< Signal quality in percent. If unknown, set to UINT8_MAX*/ +} mavlink_cellular_status_t; + +#define MAVLINK_MSG_ID_CELLULAR_STATUS_LEN 10 +#define MAVLINK_MSG_ID_CELLULAR_STATUS_MIN_LEN 10 +#define MAVLINK_MSG_ID_334_LEN 10 +#define MAVLINK_MSG_ID_334_MIN_LEN 10 + +#define MAVLINK_MSG_ID_CELLULAR_STATUS_CRC 72 +#define MAVLINK_MSG_ID_334_CRC 72 + + + +#if MAVLINK_COMMAND_24BIT +#define MAVLINK_MESSAGE_INFO_CELLULAR_STATUS { \ + 334, \ + "CELLULAR_STATUS", \ + 7, \ + { { "status", NULL, MAVLINK_TYPE_UINT8_T, 0, 6, offsetof(mavlink_cellular_status_t, status) }, \ + { "failure_reason", NULL, MAVLINK_TYPE_UINT8_T, 0, 7, offsetof(mavlink_cellular_status_t, failure_reason) }, \ + { "type", NULL, MAVLINK_TYPE_UINT8_T, 0, 8, offsetof(mavlink_cellular_status_t, type) }, \ + { "quality", NULL, MAVLINK_TYPE_UINT8_T, 0, 9, offsetof(mavlink_cellular_status_t, quality) }, \ + { "mcc", NULL, MAVLINK_TYPE_UINT16_T, 0, 0, offsetof(mavlink_cellular_status_t, mcc) }, \ + { "mnc", NULL, MAVLINK_TYPE_UINT16_T, 0, 2, offsetof(mavlink_cellular_status_t, mnc) }, \ + { "lac", NULL, MAVLINK_TYPE_UINT16_T, 0, 4, offsetof(mavlink_cellular_status_t, lac) }, \ + } \ +} +#else +#define MAVLINK_MESSAGE_INFO_CELLULAR_STATUS { \ + "CELLULAR_STATUS", \ + 7, \ + { { "status", NULL, MAVLINK_TYPE_UINT8_T, 0, 6, offsetof(mavlink_cellular_status_t, status) }, \ + { "failure_reason", NULL, MAVLINK_TYPE_UINT8_T, 0, 7, offsetof(mavlink_cellular_status_t, failure_reason) }, \ + { "type", NULL, MAVLINK_TYPE_UINT8_T, 0, 8, offsetof(mavlink_cellular_status_t, type) }, \ + { "quality", NULL, MAVLINK_TYPE_UINT8_T, 0, 9, offsetof(mavlink_cellular_status_t, quality) }, \ + { "mcc", NULL, MAVLINK_TYPE_UINT16_T, 0, 0, offsetof(mavlink_cellular_status_t, mcc) }, \ + { "mnc", NULL, MAVLINK_TYPE_UINT16_T, 0, 2, offsetof(mavlink_cellular_status_t, mnc) }, \ + { "lac", NULL, MAVLINK_TYPE_UINT16_T, 0, 4, offsetof(mavlink_cellular_status_t, lac) }, \ + } \ +} +#endif + +/** + * @brief Pack a cellular_status message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param status Cellular modem status + * @param failure_reason Failure reason when status in in CELLUAR_STATUS_FAILED + * @param type Cellular network radio type: gsm, cdma, lte... + * @param quality Signal quality in percent. If unknown, set to UINT8_MAX + * @param mcc Mobile country code. If unknown, set to UINT16_MAX + * @param mnc Mobile network code. If unknown, set to UINT16_MAX + * @param lac Location area code. If unknown, set to 0 + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_cellular_status_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint8_t status, uint8_t failure_reason, uint8_t type, uint8_t quality, uint16_t mcc, uint16_t mnc, uint16_t lac) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_CELLULAR_STATUS_LEN]; + _mav_put_uint16_t(buf, 0, mcc); + _mav_put_uint16_t(buf, 2, mnc); + _mav_put_uint16_t(buf, 4, lac); + _mav_put_uint8_t(buf, 6, status); + _mav_put_uint8_t(buf, 7, failure_reason); + _mav_put_uint8_t(buf, 8, type); + _mav_put_uint8_t(buf, 9, quality); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_CELLULAR_STATUS_LEN); +#else + mavlink_cellular_status_t packet; + packet.mcc = mcc; + packet.mnc = mnc; + packet.lac = lac; + packet.status = status; + packet.failure_reason = failure_reason; + packet.type = type; + packet.quality = quality; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_CELLULAR_STATUS_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_CELLULAR_STATUS; + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_CELLULAR_STATUS_MIN_LEN, MAVLINK_MSG_ID_CELLULAR_STATUS_LEN, MAVLINK_MSG_ID_CELLULAR_STATUS_CRC); +} + +/** + * @brief Pack a cellular_status message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param status Cellular modem status + * @param failure_reason Failure reason when status in in CELLUAR_STATUS_FAILED + * @param type Cellular network radio type: gsm, cdma, lte... + * @param quality Signal quality in percent. If unknown, set to UINT8_MAX + * @param mcc Mobile country code. If unknown, set to UINT16_MAX + * @param mnc Mobile network code. If unknown, set to UINT16_MAX + * @param lac Location area code. If unknown, set to 0 + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_cellular_status_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint8_t status,uint8_t failure_reason,uint8_t type,uint8_t quality,uint16_t mcc,uint16_t mnc,uint16_t lac) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_CELLULAR_STATUS_LEN]; + _mav_put_uint16_t(buf, 0, mcc); + _mav_put_uint16_t(buf, 2, mnc); + _mav_put_uint16_t(buf, 4, lac); + _mav_put_uint8_t(buf, 6, status); + _mav_put_uint8_t(buf, 7, failure_reason); + _mav_put_uint8_t(buf, 8, type); + _mav_put_uint8_t(buf, 9, quality); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_CELLULAR_STATUS_LEN); +#else + mavlink_cellular_status_t packet; + packet.mcc = mcc; + packet.mnc = mnc; + packet.lac = lac; + packet.status = status; + packet.failure_reason = failure_reason; + packet.type = type; + packet.quality = quality; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_CELLULAR_STATUS_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_CELLULAR_STATUS; + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_CELLULAR_STATUS_MIN_LEN, MAVLINK_MSG_ID_CELLULAR_STATUS_LEN, MAVLINK_MSG_ID_CELLULAR_STATUS_CRC); +} + +/** + * @brief Encode a cellular_status struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param cellular_status C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_cellular_status_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_cellular_status_t* cellular_status) +{ + return mavlink_msg_cellular_status_pack(system_id, component_id, msg, cellular_status->status, cellular_status->failure_reason, cellular_status->type, cellular_status->quality, cellular_status->mcc, cellular_status->mnc, cellular_status->lac); +} + +/** + * @brief Encode a cellular_status struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param cellular_status C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_cellular_status_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_cellular_status_t* cellular_status) +{ + return mavlink_msg_cellular_status_pack_chan(system_id, component_id, chan, msg, cellular_status->status, cellular_status->failure_reason, cellular_status->type, cellular_status->quality, cellular_status->mcc, cellular_status->mnc, cellular_status->lac); +} + +/** + * @brief Send a cellular_status message + * @param chan MAVLink channel to send the message + * + * @param status Cellular modem status + * @param failure_reason Failure reason when status in in CELLUAR_STATUS_FAILED + * @param type Cellular network radio type: gsm, cdma, lte... + * @param quality Signal quality in percent. If unknown, set to UINT8_MAX + * @param mcc Mobile country code. If unknown, set to UINT16_MAX + * @param mnc Mobile network code. If unknown, set to UINT16_MAX + * @param lac Location area code. If unknown, set to 0 + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_cellular_status_send(mavlink_channel_t chan, uint8_t status, uint8_t failure_reason, uint8_t type, uint8_t quality, uint16_t mcc, uint16_t mnc, uint16_t lac) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_CELLULAR_STATUS_LEN]; + _mav_put_uint16_t(buf, 0, mcc); + _mav_put_uint16_t(buf, 2, mnc); + _mav_put_uint16_t(buf, 4, lac); + _mav_put_uint8_t(buf, 6, status); + _mav_put_uint8_t(buf, 7, failure_reason); + _mav_put_uint8_t(buf, 8, type); + _mav_put_uint8_t(buf, 9, quality); + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CELLULAR_STATUS, buf, MAVLINK_MSG_ID_CELLULAR_STATUS_MIN_LEN, MAVLINK_MSG_ID_CELLULAR_STATUS_LEN, MAVLINK_MSG_ID_CELLULAR_STATUS_CRC); +#else + mavlink_cellular_status_t packet; + packet.mcc = mcc; + packet.mnc = mnc; + packet.lac = lac; + packet.status = status; + packet.failure_reason = failure_reason; + packet.type = type; + packet.quality = quality; + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CELLULAR_STATUS, (const char *)&packet, MAVLINK_MSG_ID_CELLULAR_STATUS_MIN_LEN, MAVLINK_MSG_ID_CELLULAR_STATUS_LEN, MAVLINK_MSG_ID_CELLULAR_STATUS_CRC); +#endif +} + +/** + * @brief Send a cellular_status message + * @param chan MAVLink channel to send the message + * @param struct The MAVLink struct to serialize + */ +static inline void mavlink_msg_cellular_status_send_struct(mavlink_channel_t chan, const mavlink_cellular_status_t* cellular_status) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + mavlink_msg_cellular_status_send(chan, cellular_status->status, cellular_status->failure_reason, cellular_status->type, cellular_status->quality, cellular_status->mcc, cellular_status->mnc, cellular_status->lac); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CELLULAR_STATUS, (const char *)cellular_status, MAVLINK_MSG_ID_CELLULAR_STATUS_MIN_LEN, MAVLINK_MSG_ID_CELLULAR_STATUS_LEN, MAVLINK_MSG_ID_CELLULAR_STATUS_CRC); +#endif +} + +#if MAVLINK_MSG_ID_CELLULAR_STATUS_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_cellular_status_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t status, uint8_t failure_reason, uint8_t type, uint8_t quality, uint16_t mcc, uint16_t mnc, uint16_t lac) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint16_t(buf, 0, mcc); + _mav_put_uint16_t(buf, 2, mnc); + _mav_put_uint16_t(buf, 4, lac); + _mav_put_uint8_t(buf, 6, status); + _mav_put_uint8_t(buf, 7, failure_reason); + _mav_put_uint8_t(buf, 8, type); + _mav_put_uint8_t(buf, 9, quality); + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CELLULAR_STATUS, buf, MAVLINK_MSG_ID_CELLULAR_STATUS_MIN_LEN, MAVLINK_MSG_ID_CELLULAR_STATUS_LEN, MAVLINK_MSG_ID_CELLULAR_STATUS_CRC); +#else + mavlink_cellular_status_t *packet = (mavlink_cellular_status_t *)msgbuf; + packet->mcc = mcc; + packet->mnc = mnc; + packet->lac = lac; + packet->status = status; + packet->failure_reason = failure_reason; + packet->type = type; + packet->quality = quality; + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_CELLULAR_STATUS, (const char *)packet, MAVLINK_MSG_ID_CELLULAR_STATUS_MIN_LEN, MAVLINK_MSG_ID_CELLULAR_STATUS_LEN, MAVLINK_MSG_ID_CELLULAR_STATUS_CRC); +#endif +} +#endif + +#endif + +// MESSAGE CELLULAR_STATUS UNPACKING + + +/** + * @brief Get field status from cellular_status message + * + * @return Cellular modem status + */ +static inline uint8_t mavlink_msg_cellular_status_get_status(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 6); +} + +/** + * @brief Get field failure_reason from cellular_status message + * + * @return Failure reason when status in in CELLUAR_STATUS_FAILED + */ +static inline uint8_t mavlink_msg_cellular_status_get_failure_reason(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 7); +} + +/** + * @brief Get field type from cellular_status message + * + * @return Cellular network radio type: gsm, cdma, lte... + */ +static inline uint8_t mavlink_msg_cellular_status_get_type(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 8); +} + +/** + * @brief Get field quality from cellular_status message + * + * @return Signal quality in percent. If unknown, set to UINT8_MAX + */ +static inline uint8_t mavlink_msg_cellular_status_get_quality(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 9); +} + +/** + * @brief Get field mcc from cellular_status message + * + * @return Mobile country code. If unknown, set to UINT16_MAX + */ +static inline uint16_t mavlink_msg_cellular_status_get_mcc(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 0); +} + +/** + * @brief Get field mnc from cellular_status message + * + * @return Mobile network code. If unknown, set to UINT16_MAX + */ +static inline uint16_t mavlink_msg_cellular_status_get_mnc(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 2); +} + +/** + * @brief Get field lac from cellular_status message + * + * @return Location area code. If unknown, set to 0 + */ +static inline uint16_t mavlink_msg_cellular_status_get_lac(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 4); +} + +/** + * @brief Decode a cellular_status message into a struct + * + * @param msg The message to decode + * @param cellular_status C-struct to decode the message contents into + */ +static inline void mavlink_msg_cellular_status_decode(const mavlink_message_t* msg, mavlink_cellular_status_t* cellular_status) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + cellular_status->mcc = mavlink_msg_cellular_status_get_mcc(msg); + cellular_status->mnc = mavlink_msg_cellular_status_get_mnc(msg); + cellular_status->lac = mavlink_msg_cellular_status_get_lac(msg); + cellular_status->status = mavlink_msg_cellular_status_get_status(msg); + cellular_status->failure_reason = mavlink_msg_cellular_status_get_failure_reason(msg); + cellular_status->type = mavlink_msg_cellular_status_get_type(msg); + cellular_status->quality = mavlink_msg_cellular_status_get_quality(msg); +#else + uint8_t len = msg->len < MAVLINK_MSG_ID_CELLULAR_STATUS_LEN? msg->len : MAVLINK_MSG_ID_CELLULAR_STATUS_LEN; + memset(cellular_status, 0, MAVLINK_MSG_ID_CELLULAR_STATUS_LEN); + memcpy(cellular_status, _MAV_PAYLOAD(msg), len); +#endif +} diff --git a/lib/main/MAVLink/common/mavlink_msg_command_ack.h b/lib/main/MAVLink/common/mavlink_msg_command_ack.h index 52991543c5..58dd1fc6df 100755 --- a/lib/main/MAVLink/common/mavlink_msg_command_ack.h +++ b/lib/main/MAVLink/common/mavlink_msg_command_ack.h @@ -7,11 +7,15 @@ typedef struct __mavlink_command_ack_t { uint16_t command; /*< Command ID (of acknowledged command).*/ uint8_t result; /*< Result of command.*/ + uint8_t progress; /*< WIP: Also used as result_param1, it can be set with an enum containing the errors reasons of why the command was denied, or the progress percentage when result is MAV_RESULT_IN_PROGRESS (255 if the progress is unknown).*/ + int32_t result_param2; /*< WIP: Additional parameter of the result, example: which parameter of MAV_CMD_NAV_WAYPOINT caused it to be denied.*/ + uint8_t target_system; /*< WIP: System ID of the target recipient. This is the ID of the system that sent the command for which this COMMAND_ACK is an acknowledgement.*/ + uint8_t target_component; /*< WIP: Component ID of the target recipient. This is the ID of the system that sent the command for which this COMMAND_ACK is an acknowledgement.*/ } mavlink_command_ack_t; -#define MAVLINK_MSG_ID_COMMAND_ACK_LEN 3 +#define MAVLINK_MSG_ID_COMMAND_ACK_LEN 10 #define MAVLINK_MSG_ID_COMMAND_ACK_MIN_LEN 3 -#define MAVLINK_MSG_ID_77_LEN 3 +#define MAVLINK_MSG_ID_77_LEN 10 #define MAVLINK_MSG_ID_77_MIN_LEN 3 #define MAVLINK_MSG_ID_COMMAND_ACK_CRC 143 @@ -23,17 +27,25 @@ typedef struct __mavlink_command_ack_t { #define MAVLINK_MESSAGE_INFO_COMMAND_ACK { \ 77, \ "COMMAND_ACK", \ - 2, \ + 6, \ { { "command", NULL, MAVLINK_TYPE_UINT16_T, 0, 0, offsetof(mavlink_command_ack_t, command) }, \ { "result", NULL, MAVLINK_TYPE_UINT8_T, 0, 2, offsetof(mavlink_command_ack_t, result) }, \ + { "progress", NULL, MAVLINK_TYPE_UINT8_T, 0, 3, offsetof(mavlink_command_ack_t, progress) }, \ + { "result_param2", NULL, MAVLINK_TYPE_INT32_T, 0, 4, offsetof(mavlink_command_ack_t, result_param2) }, \ + { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 8, offsetof(mavlink_command_ack_t, target_system) }, \ + { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 9, offsetof(mavlink_command_ack_t, target_component) }, \ } \ } #else #define MAVLINK_MESSAGE_INFO_COMMAND_ACK { \ "COMMAND_ACK", \ - 2, \ + 6, \ { { "command", NULL, MAVLINK_TYPE_UINT16_T, 0, 0, offsetof(mavlink_command_ack_t, command) }, \ { "result", NULL, MAVLINK_TYPE_UINT8_T, 0, 2, offsetof(mavlink_command_ack_t, result) }, \ + { "progress", NULL, MAVLINK_TYPE_UINT8_T, 0, 3, offsetof(mavlink_command_ack_t, progress) }, \ + { "result_param2", NULL, MAVLINK_TYPE_INT32_T, 0, 4, offsetof(mavlink_command_ack_t, result_param2) }, \ + { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 8, offsetof(mavlink_command_ack_t, target_system) }, \ + { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 9, offsetof(mavlink_command_ack_t, target_component) }, \ } \ } #endif @@ -46,21 +58,33 @@ typedef struct __mavlink_command_ack_t { * * @param command Command ID (of acknowledged command). * @param result Result of command. + * @param progress WIP: Also used as result_param1, it can be set with an enum containing the errors reasons of why the command was denied, or the progress percentage when result is MAV_RESULT_IN_PROGRESS (255 if the progress is unknown). + * @param result_param2 WIP: Additional parameter of the result, example: which parameter of MAV_CMD_NAV_WAYPOINT caused it to be denied. + * @param target_system WIP: System ID of the target recipient. This is the ID of the system that sent the command for which this COMMAND_ACK is an acknowledgement. + * @param target_component WIP: Component ID of the target recipient. This is the ID of the system that sent the command for which this COMMAND_ACK is an acknowledgement. * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_command_ack_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint16_t command, uint8_t result) + uint16_t command, uint8_t result, uint8_t progress, int32_t result_param2, uint8_t target_system, uint8_t target_component) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_COMMAND_ACK_LEN]; _mav_put_uint16_t(buf, 0, command); _mav_put_uint8_t(buf, 2, result); + _mav_put_uint8_t(buf, 3, progress); + _mav_put_int32_t(buf, 4, result_param2); + _mav_put_uint8_t(buf, 8, target_system); + _mav_put_uint8_t(buf, 9, target_component); memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_COMMAND_ACK_LEN); #else mavlink_command_ack_t packet; packet.command = command; packet.result = result; + packet.progress = progress; + packet.result_param2 = result_param2; + packet.target_system = target_system; + packet.target_component = target_component; memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_COMMAND_ACK_LEN); #endif @@ -77,22 +101,34 @@ static inline uint16_t mavlink_msg_command_ack_pack(uint8_t system_id, uint8_t c * @param msg The MAVLink message to compress the data into * @param command Command ID (of acknowledged command). * @param result Result of command. + * @param progress WIP: Also used as result_param1, it can be set with an enum containing the errors reasons of why the command was denied, or the progress percentage when result is MAV_RESULT_IN_PROGRESS (255 if the progress is unknown). + * @param result_param2 WIP: Additional parameter of the result, example: which parameter of MAV_CMD_NAV_WAYPOINT caused it to be denied. + * @param target_system WIP: System ID of the target recipient. This is the ID of the system that sent the command for which this COMMAND_ACK is an acknowledgement. + * @param target_component WIP: Component ID of the target recipient. This is the ID of the system that sent the command for which this COMMAND_ACK is an acknowledgement. * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_command_ack_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, - uint16_t command,uint8_t result) + uint16_t command,uint8_t result,uint8_t progress,int32_t result_param2,uint8_t target_system,uint8_t target_component) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_COMMAND_ACK_LEN]; _mav_put_uint16_t(buf, 0, command); _mav_put_uint8_t(buf, 2, result); + _mav_put_uint8_t(buf, 3, progress); + _mav_put_int32_t(buf, 4, result_param2); + _mav_put_uint8_t(buf, 8, target_system); + _mav_put_uint8_t(buf, 9, target_component); memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_COMMAND_ACK_LEN); #else mavlink_command_ack_t packet; packet.command = command; packet.result = result; + packet.progress = progress; + packet.result_param2 = result_param2; + packet.target_system = target_system; + packet.target_component = target_component; memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_COMMAND_ACK_LEN); #endif @@ -111,7 +147,7 @@ static inline uint16_t mavlink_msg_command_ack_pack_chan(uint8_t system_id, uint */ static inline uint16_t mavlink_msg_command_ack_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_command_ack_t* command_ack) { - return mavlink_msg_command_ack_pack(system_id, component_id, msg, command_ack->command, command_ack->result); + return mavlink_msg_command_ack_pack(system_id, component_id, msg, command_ack->command, command_ack->result, command_ack->progress, command_ack->result_param2, command_ack->target_system, command_ack->target_component); } /** @@ -125,7 +161,7 @@ static inline uint16_t mavlink_msg_command_ack_encode(uint8_t system_id, uint8_t */ static inline uint16_t mavlink_msg_command_ack_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_command_ack_t* command_ack) { - return mavlink_msg_command_ack_pack_chan(system_id, component_id, chan, msg, command_ack->command, command_ack->result); + return mavlink_msg_command_ack_pack_chan(system_id, component_id, chan, msg, command_ack->command, command_ack->result, command_ack->progress, command_ack->result_param2, command_ack->target_system, command_ack->target_component); } /** @@ -134,21 +170,33 @@ static inline uint16_t mavlink_msg_command_ack_encode_chan(uint8_t system_id, ui * * @param command Command ID (of acknowledged command). * @param result Result of command. + * @param progress WIP: Also used as result_param1, it can be set with an enum containing the errors reasons of why the command was denied, or the progress percentage when result is MAV_RESULT_IN_PROGRESS (255 if the progress is unknown). + * @param result_param2 WIP: Additional parameter of the result, example: which parameter of MAV_CMD_NAV_WAYPOINT caused it to be denied. + * @param target_system WIP: System ID of the target recipient. This is the ID of the system that sent the command for which this COMMAND_ACK is an acknowledgement. + * @param target_component WIP: Component ID of the target recipient. This is the ID of the system that sent the command for which this COMMAND_ACK is an acknowledgement. */ #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS -static inline void mavlink_msg_command_ack_send(mavlink_channel_t chan, uint16_t command, uint8_t result) +static inline void mavlink_msg_command_ack_send(mavlink_channel_t chan, uint16_t command, uint8_t result, uint8_t progress, int32_t result_param2, uint8_t target_system, uint8_t target_component) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_COMMAND_ACK_LEN]; _mav_put_uint16_t(buf, 0, command); _mav_put_uint8_t(buf, 2, result); + _mav_put_uint8_t(buf, 3, progress); + _mav_put_int32_t(buf, 4, result_param2); + _mav_put_uint8_t(buf, 8, target_system); + _mav_put_uint8_t(buf, 9, target_component); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_COMMAND_ACK, buf, MAVLINK_MSG_ID_COMMAND_ACK_MIN_LEN, MAVLINK_MSG_ID_COMMAND_ACK_LEN, MAVLINK_MSG_ID_COMMAND_ACK_CRC); #else mavlink_command_ack_t packet; packet.command = command; packet.result = result; + packet.progress = progress; + packet.result_param2 = result_param2; + packet.target_system = target_system; + packet.target_component = target_component; _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_COMMAND_ACK, (const char *)&packet, MAVLINK_MSG_ID_COMMAND_ACK_MIN_LEN, MAVLINK_MSG_ID_COMMAND_ACK_LEN, MAVLINK_MSG_ID_COMMAND_ACK_CRC); #endif @@ -162,7 +210,7 @@ static inline void mavlink_msg_command_ack_send(mavlink_channel_t chan, uint16_t static inline void mavlink_msg_command_ack_send_struct(mavlink_channel_t chan, const mavlink_command_ack_t* command_ack) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - mavlink_msg_command_ack_send(chan, command_ack->command, command_ack->result); + mavlink_msg_command_ack_send(chan, command_ack->command, command_ack->result, command_ack->progress, command_ack->result_param2, command_ack->target_system, command_ack->target_component); #else _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_COMMAND_ACK, (const char *)command_ack, MAVLINK_MSG_ID_COMMAND_ACK_MIN_LEN, MAVLINK_MSG_ID_COMMAND_ACK_LEN, MAVLINK_MSG_ID_COMMAND_ACK_CRC); #endif @@ -176,18 +224,26 @@ static inline void mavlink_msg_command_ack_send_struct(mavlink_channel_t chan, c is usually the receive buffer for the channel, and allows a reply to an incoming message with minimum stack space usage. */ -static inline void mavlink_msg_command_ack_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint16_t command, uint8_t result) +static inline void mavlink_msg_command_ack_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint16_t command, uint8_t result, uint8_t progress, int32_t result_param2, uint8_t target_system, uint8_t target_component) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char *buf = (char *)msgbuf; _mav_put_uint16_t(buf, 0, command); _mav_put_uint8_t(buf, 2, result); + _mav_put_uint8_t(buf, 3, progress); + _mav_put_int32_t(buf, 4, result_param2); + _mav_put_uint8_t(buf, 8, target_system); + _mav_put_uint8_t(buf, 9, target_component); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_COMMAND_ACK, buf, MAVLINK_MSG_ID_COMMAND_ACK_MIN_LEN, MAVLINK_MSG_ID_COMMAND_ACK_LEN, MAVLINK_MSG_ID_COMMAND_ACK_CRC); #else mavlink_command_ack_t *packet = (mavlink_command_ack_t *)msgbuf; packet->command = command; packet->result = result; + packet->progress = progress; + packet->result_param2 = result_param2; + packet->target_system = target_system; + packet->target_component = target_component; _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_COMMAND_ACK, (const char *)packet, MAVLINK_MSG_ID_COMMAND_ACK_MIN_LEN, MAVLINK_MSG_ID_COMMAND_ACK_LEN, MAVLINK_MSG_ID_COMMAND_ACK_CRC); #endif @@ -219,6 +275,46 @@ static inline uint8_t mavlink_msg_command_ack_get_result(const mavlink_message_t return _MAV_RETURN_uint8_t(msg, 2); } +/** + * @brief Get field progress from command_ack message + * + * @return WIP: Also used as result_param1, it can be set with an enum containing the errors reasons of why the command was denied, or the progress percentage when result is MAV_RESULT_IN_PROGRESS (255 if the progress is unknown). + */ +static inline uint8_t mavlink_msg_command_ack_get_progress(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 3); +} + +/** + * @brief Get field result_param2 from command_ack message + * + * @return WIP: Additional parameter of the result, example: which parameter of MAV_CMD_NAV_WAYPOINT caused it to be denied. + */ +static inline int32_t mavlink_msg_command_ack_get_result_param2(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int32_t(msg, 4); +} + +/** + * @brief Get field target_system from command_ack message + * + * @return WIP: System ID of the target recipient. This is the ID of the system that sent the command for which this COMMAND_ACK is an acknowledgement. + */ +static inline uint8_t mavlink_msg_command_ack_get_target_system(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 8); +} + +/** + * @brief Get field target_component from command_ack message + * + * @return WIP: Component ID of the target recipient. This is the ID of the system that sent the command for which this COMMAND_ACK is an acknowledgement. + */ +static inline uint8_t mavlink_msg_command_ack_get_target_component(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 9); +} + /** * @brief Decode a command_ack message into a struct * @@ -230,6 +326,10 @@ static inline void mavlink_msg_command_ack_decode(const mavlink_message_t* msg, #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS command_ack->command = mavlink_msg_command_ack_get_command(msg); command_ack->result = mavlink_msg_command_ack_get_result(msg); + command_ack->progress = mavlink_msg_command_ack_get_progress(msg); + command_ack->result_param2 = mavlink_msg_command_ack_get_result_param2(msg); + command_ack->target_system = mavlink_msg_command_ack_get_target_system(msg); + command_ack->target_component = mavlink_msg_command_ack_get_target_component(msg); #else uint8_t len = msg->len < MAVLINK_MSG_ID_COMMAND_ACK_LEN? msg->len : MAVLINK_MSG_ID_COMMAND_ACK_LEN; memset(command_ack, 0, MAVLINK_MSG_ID_COMMAND_ACK_LEN); diff --git a/lib/main/MAVLink/common/mavlink_msg_command_int.h b/lib/main/MAVLink/common/mavlink_msg_command_int.h index dcd298a1c9..a23f50895c 100755 --- a/lib/main/MAVLink/common/mavlink_msg_command_int.h +++ b/lib/main/MAVLink/common/mavlink_msg_command_int.h @@ -16,8 +16,8 @@ typedef struct __mavlink_command_int_t { uint8_t target_system; /*< System ID*/ uint8_t target_component; /*< Component ID*/ uint8_t frame; /*< The coordinate system of the COMMAND.*/ - uint8_t current; /*< false:0, true:1*/ - uint8_t autocontinue; /*< autocontinue to next wp*/ + uint8_t current; /*< Not used.*/ + uint8_t autocontinue; /*< Not used (set 0).*/ } mavlink_command_int_t; #define MAVLINK_MSG_ID_COMMAND_INT_LEN 35 @@ -81,8 +81,8 @@ typedef struct __mavlink_command_int_t { * @param target_component Component ID * @param frame The coordinate system of the COMMAND. * @param command The scheduled action for the mission item. - * @param current false:0, true:1 - * @param autocontinue autocontinue to next wp + * @param current Not used. + * @param autocontinue Not used (set 0). * @param param1 PARAM1, see MAV_CMD enum * @param param2 PARAM2, see MAV_CMD enum * @param param3 PARAM3, see MAV_CMD enum @@ -145,8 +145,8 @@ static inline uint16_t mavlink_msg_command_int_pack(uint8_t system_id, uint8_t c * @param target_component Component ID * @param frame The coordinate system of the COMMAND. * @param command The scheduled action for the mission item. - * @param current false:0, true:1 - * @param autocontinue autocontinue to next wp + * @param current Not used. + * @param autocontinue Not used (set 0). * @param param1 PARAM1, see MAV_CMD enum * @param param2 PARAM2, see MAV_CMD enum * @param param3 PARAM3, see MAV_CMD enum @@ -235,8 +235,8 @@ static inline uint16_t mavlink_msg_command_int_encode_chan(uint8_t system_id, ui * @param target_component Component ID * @param frame The coordinate system of the COMMAND. * @param command The scheduled action for the mission item. - * @param current false:0, true:1 - * @param autocontinue autocontinue to next wp + * @param current Not used. + * @param autocontinue Not used (set 0). * @param param1 PARAM1, see MAV_CMD enum * @param param2 PARAM2, see MAV_CMD enum * @param param3 PARAM3, see MAV_CMD enum @@ -396,7 +396,7 @@ static inline uint16_t mavlink_msg_command_int_get_command(const mavlink_message /** * @brief Get field current from command_int message * - * @return false:0, true:1 + * @return Not used. */ static inline uint8_t mavlink_msg_command_int_get_current(const mavlink_message_t* msg) { @@ -406,7 +406,7 @@ static inline uint8_t mavlink_msg_command_int_get_current(const mavlink_message_ /** * @brief Get field autocontinue from command_int message * - * @return autocontinue to next wp + * @return Not used (set 0). */ static inline uint8_t mavlink_msg_command_int_get_autocontinue(const mavlink_message_t* msg) { diff --git a/lib/main/MAVLink/common/mavlink_msg_component_information.h b/lib/main/MAVLink/common/mavlink_msg_component_information.h new file mode 100644 index 0000000000..a916e6bee6 --- /dev/null +++ b/lib/main/MAVLink/common/mavlink_msg_component_information.h @@ -0,0 +1,331 @@ +#pragma once +// MESSAGE COMPONENT_INFORMATION PACKING + +#define MAVLINK_MSG_ID_COMPONENT_INFORMATION 395 + + +typedef struct __mavlink_component_information_t { + uint32_t time_boot_ms; /*< [ms] Timestamp (time since system boot).*/ + uint32_t metadata_type; /*< The type of metadata being requested.*/ + uint32_t metadata_uid; /*< Unique uid for this metadata which a gcs can use for created cached metadata and understanding whether it's cache it up to date or whether it needs to download new data.*/ + uint32_t translation_uid; /*< Unique uid for the translation file associated with the metadata.*/ + char metadata_uri[70]; /*< Component definition URI. If prefix mavlinkftp:// file is downloaded from vehicle over mavlink ftp protocol. If prefix http[s]:// file is downloaded over internet. Files are json format. They can end in .gz to indicate file is in gzip format.*/ + char translation_uri[70]; /*< The translations for strings within the metadata file. If null the either do not exist or may be included in the metadata file itself. The translations specified here supercede any which may be in the metadata file itself. The uri format is the same as component_metadata_uri . Files are in Json Translation spec format. Empty string indicates no tranlsation file.*/ +} mavlink_component_information_t; + +#define MAVLINK_MSG_ID_COMPONENT_INFORMATION_LEN 156 +#define MAVLINK_MSG_ID_COMPONENT_INFORMATION_MIN_LEN 156 +#define MAVLINK_MSG_ID_395_LEN 156 +#define MAVLINK_MSG_ID_395_MIN_LEN 156 + +#define MAVLINK_MSG_ID_COMPONENT_INFORMATION_CRC 163 +#define MAVLINK_MSG_ID_395_CRC 163 + +#define MAVLINK_MSG_COMPONENT_INFORMATION_FIELD_METADATA_URI_LEN 70 +#define MAVLINK_MSG_COMPONENT_INFORMATION_FIELD_TRANSLATION_URI_LEN 70 + +#if MAVLINK_COMMAND_24BIT +#define MAVLINK_MESSAGE_INFO_COMPONENT_INFORMATION { \ + 395, \ + "COMPONENT_INFORMATION", \ + 6, \ + { { "time_boot_ms", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_component_information_t, time_boot_ms) }, \ + { "metadata_type", NULL, MAVLINK_TYPE_UINT32_T, 0, 4, offsetof(mavlink_component_information_t, metadata_type) }, \ + { "metadata_uid", NULL, MAVLINK_TYPE_UINT32_T, 0, 8, offsetof(mavlink_component_information_t, metadata_uid) }, \ + { "metadata_uri", NULL, MAVLINK_TYPE_CHAR, 70, 16, offsetof(mavlink_component_information_t, metadata_uri) }, \ + { "translation_uid", NULL, MAVLINK_TYPE_UINT32_T, 0, 12, offsetof(mavlink_component_information_t, translation_uid) }, \ + { "translation_uri", NULL, MAVLINK_TYPE_CHAR, 70, 86, offsetof(mavlink_component_information_t, translation_uri) }, \ + } \ +} +#else +#define MAVLINK_MESSAGE_INFO_COMPONENT_INFORMATION { \ + "COMPONENT_INFORMATION", \ + 6, \ + { { "time_boot_ms", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_component_information_t, time_boot_ms) }, \ + { "metadata_type", NULL, MAVLINK_TYPE_UINT32_T, 0, 4, offsetof(mavlink_component_information_t, metadata_type) }, \ + { "metadata_uid", NULL, MAVLINK_TYPE_UINT32_T, 0, 8, offsetof(mavlink_component_information_t, metadata_uid) }, \ + { "metadata_uri", NULL, MAVLINK_TYPE_CHAR, 70, 16, offsetof(mavlink_component_information_t, metadata_uri) }, \ + { "translation_uid", NULL, MAVLINK_TYPE_UINT32_T, 0, 12, offsetof(mavlink_component_information_t, translation_uid) }, \ + { "translation_uri", NULL, MAVLINK_TYPE_CHAR, 70, 86, offsetof(mavlink_component_information_t, translation_uri) }, \ + } \ +} +#endif + +/** + * @brief Pack a component_information message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param time_boot_ms [ms] Timestamp (time since system boot). + * @param metadata_type The type of metadata being requested. + * @param metadata_uid Unique uid for this metadata which a gcs can use for created cached metadata and understanding whether it's cache it up to date or whether it needs to download new data. + * @param metadata_uri Component definition URI. If prefix mavlinkftp:// file is downloaded from vehicle over mavlink ftp protocol. If prefix http[s]:// file is downloaded over internet. Files are json format. They can end in .gz to indicate file is in gzip format. + * @param translation_uid Unique uid for the translation file associated with the metadata. + * @param translation_uri The translations for strings within the metadata file. If null the either do not exist or may be included in the metadata file itself. The translations specified here supercede any which may be in the metadata file itself. The uri format is the same as component_metadata_uri . Files are in Json Translation spec format. Empty string indicates no tranlsation file. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_component_information_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint32_t time_boot_ms, uint32_t metadata_type, uint32_t metadata_uid, const char *metadata_uri, uint32_t translation_uid, const char *translation_uri) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_COMPONENT_INFORMATION_LEN]; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_uint32_t(buf, 4, metadata_type); + _mav_put_uint32_t(buf, 8, metadata_uid); + _mav_put_uint32_t(buf, 12, translation_uid); + _mav_put_char_array(buf, 16, metadata_uri, 70); + _mav_put_char_array(buf, 86, translation_uri, 70); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_COMPONENT_INFORMATION_LEN); +#else + mavlink_component_information_t packet; + packet.time_boot_ms = time_boot_ms; + packet.metadata_type = metadata_type; + packet.metadata_uid = metadata_uid; + packet.translation_uid = translation_uid; + mav_array_memcpy(packet.metadata_uri, metadata_uri, sizeof(char)*70); + mav_array_memcpy(packet.translation_uri, translation_uri, sizeof(char)*70); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_COMPONENT_INFORMATION_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_COMPONENT_INFORMATION; + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_COMPONENT_INFORMATION_MIN_LEN, MAVLINK_MSG_ID_COMPONENT_INFORMATION_LEN, MAVLINK_MSG_ID_COMPONENT_INFORMATION_CRC); +} + +/** + * @brief Pack a component_information message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param time_boot_ms [ms] Timestamp (time since system boot). + * @param metadata_type The type of metadata being requested. + * @param metadata_uid Unique uid for this metadata which a gcs can use for created cached metadata and understanding whether it's cache it up to date or whether it needs to download new data. + * @param metadata_uri Component definition URI. If prefix mavlinkftp:// file is downloaded from vehicle over mavlink ftp protocol. If prefix http[s]:// file is downloaded over internet. Files are json format. They can end in .gz to indicate file is in gzip format. + * @param translation_uid Unique uid for the translation file associated with the metadata. + * @param translation_uri The translations for strings within the metadata file. If null the either do not exist or may be included in the metadata file itself. The translations specified here supercede any which may be in the metadata file itself. The uri format is the same as component_metadata_uri . Files are in Json Translation spec format. Empty string indicates no tranlsation file. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_component_information_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint32_t time_boot_ms,uint32_t metadata_type,uint32_t metadata_uid,const char *metadata_uri,uint32_t translation_uid,const char *translation_uri) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_COMPONENT_INFORMATION_LEN]; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_uint32_t(buf, 4, metadata_type); + _mav_put_uint32_t(buf, 8, metadata_uid); + _mav_put_uint32_t(buf, 12, translation_uid); + _mav_put_char_array(buf, 16, metadata_uri, 70); + _mav_put_char_array(buf, 86, translation_uri, 70); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_COMPONENT_INFORMATION_LEN); +#else + mavlink_component_information_t packet; + packet.time_boot_ms = time_boot_ms; + packet.metadata_type = metadata_type; + packet.metadata_uid = metadata_uid; + packet.translation_uid = translation_uid; + mav_array_memcpy(packet.metadata_uri, metadata_uri, sizeof(char)*70); + mav_array_memcpy(packet.translation_uri, translation_uri, sizeof(char)*70); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_COMPONENT_INFORMATION_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_COMPONENT_INFORMATION; + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_COMPONENT_INFORMATION_MIN_LEN, MAVLINK_MSG_ID_COMPONENT_INFORMATION_LEN, MAVLINK_MSG_ID_COMPONENT_INFORMATION_CRC); +} + +/** + * @brief Encode a component_information struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param component_information C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_component_information_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_component_information_t* component_information) +{ + return mavlink_msg_component_information_pack(system_id, component_id, msg, component_information->time_boot_ms, component_information->metadata_type, component_information->metadata_uid, component_information->metadata_uri, component_information->translation_uid, component_information->translation_uri); +} + +/** + * @brief Encode a component_information struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param component_information C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_component_information_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_component_information_t* component_information) +{ + return mavlink_msg_component_information_pack_chan(system_id, component_id, chan, msg, component_information->time_boot_ms, component_information->metadata_type, component_information->metadata_uid, component_information->metadata_uri, component_information->translation_uid, component_information->translation_uri); +} + +/** + * @brief Send a component_information message + * @param chan MAVLink channel to send the message + * + * @param time_boot_ms [ms] Timestamp (time since system boot). + * @param metadata_type The type of metadata being requested. + * @param metadata_uid Unique uid for this metadata which a gcs can use for created cached metadata and understanding whether it's cache it up to date or whether it needs to download new data. + * @param metadata_uri Component definition URI. If prefix mavlinkftp:// file is downloaded from vehicle over mavlink ftp protocol. If prefix http[s]:// file is downloaded over internet. Files are json format. They can end in .gz to indicate file is in gzip format. + * @param translation_uid Unique uid for the translation file associated with the metadata. + * @param translation_uri The translations for strings within the metadata file. If null the either do not exist or may be included in the metadata file itself. The translations specified here supercede any which may be in the metadata file itself. The uri format is the same as component_metadata_uri . Files are in Json Translation spec format. Empty string indicates no tranlsation file. + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_component_information_send(mavlink_channel_t chan, uint32_t time_boot_ms, uint32_t metadata_type, uint32_t metadata_uid, const char *metadata_uri, uint32_t translation_uid, const char *translation_uri) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_COMPONENT_INFORMATION_LEN]; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_uint32_t(buf, 4, metadata_type); + _mav_put_uint32_t(buf, 8, metadata_uid); + _mav_put_uint32_t(buf, 12, translation_uid); + _mav_put_char_array(buf, 16, metadata_uri, 70); + _mav_put_char_array(buf, 86, translation_uri, 70); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_COMPONENT_INFORMATION, buf, MAVLINK_MSG_ID_COMPONENT_INFORMATION_MIN_LEN, MAVLINK_MSG_ID_COMPONENT_INFORMATION_LEN, MAVLINK_MSG_ID_COMPONENT_INFORMATION_CRC); +#else + mavlink_component_information_t packet; + packet.time_boot_ms = time_boot_ms; + packet.metadata_type = metadata_type; + packet.metadata_uid = metadata_uid; + packet.translation_uid = translation_uid; + mav_array_memcpy(packet.metadata_uri, metadata_uri, sizeof(char)*70); + mav_array_memcpy(packet.translation_uri, translation_uri, sizeof(char)*70); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_COMPONENT_INFORMATION, (const char *)&packet, MAVLINK_MSG_ID_COMPONENT_INFORMATION_MIN_LEN, MAVLINK_MSG_ID_COMPONENT_INFORMATION_LEN, MAVLINK_MSG_ID_COMPONENT_INFORMATION_CRC); +#endif +} + +/** + * @brief Send a component_information message + * @param chan MAVLink channel to send the message + * @param struct The MAVLink struct to serialize + */ +static inline void mavlink_msg_component_information_send_struct(mavlink_channel_t chan, const mavlink_component_information_t* component_information) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + mavlink_msg_component_information_send(chan, component_information->time_boot_ms, component_information->metadata_type, component_information->metadata_uid, component_information->metadata_uri, component_information->translation_uid, component_information->translation_uri); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_COMPONENT_INFORMATION, (const char *)component_information, MAVLINK_MSG_ID_COMPONENT_INFORMATION_MIN_LEN, MAVLINK_MSG_ID_COMPONENT_INFORMATION_LEN, MAVLINK_MSG_ID_COMPONENT_INFORMATION_CRC); +#endif +} + +#if MAVLINK_MSG_ID_COMPONENT_INFORMATION_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_component_information_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint32_t time_boot_ms, uint32_t metadata_type, uint32_t metadata_uid, const char *metadata_uri, uint32_t translation_uid, const char *translation_uri) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_uint32_t(buf, 4, metadata_type); + _mav_put_uint32_t(buf, 8, metadata_uid); + _mav_put_uint32_t(buf, 12, translation_uid); + _mav_put_char_array(buf, 16, metadata_uri, 70); + _mav_put_char_array(buf, 86, translation_uri, 70); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_COMPONENT_INFORMATION, buf, MAVLINK_MSG_ID_COMPONENT_INFORMATION_MIN_LEN, MAVLINK_MSG_ID_COMPONENT_INFORMATION_LEN, MAVLINK_MSG_ID_COMPONENT_INFORMATION_CRC); +#else + mavlink_component_information_t *packet = (mavlink_component_information_t *)msgbuf; + packet->time_boot_ms = time_boot_ms; + packet->metadata_type = metadata_type; + packet->metadata_uid = metadata_uid; + packet->translation_uid = translation_uid; + mav_array_memcpy(packet->metadata_uri, metadata_uri, sizeof(char)*70); + mav_array_memcpy(packet->translation_uri, translation_uri, sizeof(char)*70); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_COMPONENT_INFORMATION, (const char *)packet, MAVLINK_MSG_ID_COMPONENT_INFORMATION_MIN_LEN, MAVLINK_MSG_ID_COMPONENT_INFORMATION_LEN, MAVLINK_MSG_ID_COMPONENT_INFORMATION_CRC); +#endif +} +#endif + +#endif + +// MESSAGE COMPONENT_INFORMATION UNPACKING + + +/** + * @brief Get field time_boot_ms from component_information message + * + * @return [ms] Timestamp (time since system boot). + */ +static inline uint32_t mavlink_msg_component_information_get_time_boot_ms(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint32_t(msg, 0); +} + +/** + * @brief Get field metadata_type from component_information message + * + * @return The type of metadata being requested. + */ +static inline uint32_t mavlink_msg_component_information_get_metadata_type(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint32_t(msg, 4); +} + +/** + * @brief Get field metadata_uid from component_information message + * + * @return Unique uid for this metadata which a gcs can use for created cached metadata and understanding whether it's cache it up to date or whether it needs to download new data. + */ +static inline uint32_t mavlink_msg_component_information_get_metadata_uid(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint32_t(msg, 8); +} + +/** + * @brief Get field metadata_uri from component_information message + * + * @return Component definition URI. If prefix mavlinkftp:// file is downloaded from vehicle over mavlink ftp protocol. If prefix http[s]:// file is downloaded over internet. Files are json format. They can end in .gz to indicate file is in gzip format. + */ +static inline uint16_t mavlink_msg_component_information_get_metadata_uri(const mavlink_message_t* msg, char *metadata_uri) +{ + return _MAV_RETURN_char_array(msg, metadata_uri, 70, 16); +} + +/** + * @brief Get field translation_uid from component_information message + * + * @return Unique uid for the translation file associated with the metadata. + */ +static inline uint32_t mavlink_msg_component_information_get_translation_uid(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint32_t(msg, 12); +} + +/** + * @brief Get field translation_uri from component_information message + * + * @return The translations for strings within the metadata file. If null the either do not exist or may be included in the metadata file itself. The translations specified here supercede any which may be in the metadata file itself. The uri format is the same as component_metadata_uri . Files are in Json Translation spec format. Empty string indicates no tranlsation file. + */ +static inline uint16_t mavlink_msg_component_information_get_translation_uri(const mavlink_message_t* msg, char *translation_uri) +{ + return _MAV_RETURN_char_array(msg, translation_uri, 70, 86); +} + +/** + * @brief Decode a component_information message into a struct + * + * @param msg The message to decode + * @param component_information C-struct to decode the message contents into + */ +static inline void mavlink_msg_component_information_decode(const mavlink_message_t* msg, mavlink_component_information_t* component_information) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + component_information->time_boot_ms = mavlink_msg_component_information_get_time_boot_ms(msg); + component_information->metadata_type = mavlink_msg_component_information_get_metadata_type(msg); + component_information->metadata_uid = mavlink_msg_component_information_get_metadata_uid(msg); + component_information->translation_uid = mavlink_msg_component_information_get_translation_uid(msg); + mavlink_msg_component_information_get_metadata_uri(msg, component_information->metadata_uri); + mavlink_msg_component_information_get_translation_uri(msg, component_information->translation_uri); +#else + uint8_t len = msg->len < MAVLINK_MSG_ID_COMPONENT_INFORMATION_LEN? msg->len : MAVLINK_MSG_ID_COMPONENT_INFORMATION_LEN; + memset(component_information, 0, MAVLINK_MSG_ID_COMPONENT_INFORMATION_LEN); + memcpy(component_information, _MAV_PAYLOAD(msg), len); +#endif +} diff --git a/lib/main/MAVLink/common/mavlink_msg_debug_float_array.h b/lib/main/MAVLink/common/mavlink_msg_debug_float_array.h new file mode 100644 index 0000000000..c3a8b31fb8 --- /dev/null +++ b/lib/main/MAVLink/common/mavlink_msg_debug_float_array.h @@ -0,0 +1,281 @@ +#pragma once +// MESSAGE DEBUG_FLOAT_ARRAY PACKING + +#define MAVLINK_MSG_ID_DEBUG_FLOAT_ARRAY 350 + + +typedef struct __mavlink_debug_float_array_t { + uint64_t time_usec; /*< [us] Timestamp (UNIX Epoch time or time since system boot). The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude of the number.*/ + uint16_t array_id; /*< Unique ID used to discriminate between arrays*/ + char name[10]; /*< Name, for human-friendly display in a Ground Control Station*/ + float data[58]; /*< data*/ +} mavlink_debug_float_array_t; + +#define MAVLINK_MSG_ID_DEBUG_FLOAT_ARRAY_LEN 252 +#define MAVLINK_MSG_ID_DEBUG_FLOAT_ARRAY_MIN_LEN 20 +#define MAVLINK_MSG_ID_350_LEN 252 +#define MAVLINK_MSG_ID_350_MIN_LEN 20 + +#define MAVLINK_MSG_ID_DEBUG_FLOAT_ARRAY_CRC 232 +#define MAVLINK_MSG_ID_350_CRC 232 + +#define MAVLINK_MSG_DEBUG_FLOAT_ARRAY_FIELD_NAME_LEN 10 +#define MAVLINK_MSG_DEBUG_FLOAT_ARRAY_FIELD_DATA_LEN 58 + +#if MAVLINK_COMMAND_24BIT +#define MAVLINK_MESSAGE_INFO_DEBUG_FLOAT_ARRAY { \ + 350, \ + "DEBUG_FLOAT_ARRAY", \ + 4, \ + { { "time_usec", NULL, MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_debug_float_array_t, time_usec) }, \ + { "name", NULL, MAVLINK_TYPE_CHAR, 10, 10, offsetof(mavlink_debug_float_array_t, name) }, \ + { "array_id", NULL, MAVLINK_TYPE_UINT16_T, 0, 8, offsetof(mavlink_debug_float_array_t, array_id) }, \ + { "data", NULL, MAVLINK_TYPE_FLOAT, 58, 20, offsetof(mavlink_debug_float_array_t, data) }, \ + } \ +} +#else +#define MAVLINK_MESSAGE_INFO_DEBUG_FLOAT_ARRAY { \ + "DEBUG_FLOAT_ARRAY", \ + 4, \ + { { "time_usec", NULL, MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_debug_float_array_t, time_usec) }, \ + { "name", NULL, MAVLINK_TYPE_CHAR, 10, 10, offsetof(mavlink_debug_float_array_t, name) }, \ + { "array_id", NULL, MAVLINK_TYPE_UINT16_T, 0, 8, offsetof(mavlink_debug_float_array_t, array_id) }, \ + { "data", NULL, MAVLINK_TYPE_FLOAT, 58, 20, offsetof(mavlink_debug_float_array_t, data) }, \ + } \ +} +#endif + +/** + * @brief Pack a debug_float_array message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param time_usec [us] Timestamp (UNIX Epoch time or time since system boot). The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude of the number. + * @param name Name, for human-friendly display in a Ground Control Station + * @param array_id Unique ID used to discriminate between arrays + * @param data data + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_debug_float_array_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint64_t time_usec, const char *name, uint16_t array_id, const float *data) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_DEBUG_FLOAT_ARRAY_LEN]; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_uint16_t(buf, 8, array_id); + _mav_put_char_array(buf, 10, name, 10); + _mav_put_float_array(buf, 20, data, 58); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_DEBUG_FLOAT_ARRAY_LEN); +#else + mavlink_debug_float_array_t packet; + packet.time_usec = time_usec; + packet.array_id = array_id; + mav_array_memcpy(packet.name, name, sizeof(char)*10); + mav_array_memcpy(packet.data, data, sizeof(float)*58); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_DEBUG_FLOAT_ARRAY_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_DEBUG_FLOAT_ARRAY; + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_DEBUG_FLOAT_ARRAY_MIN_LEN, MAVLINK_MSG_ID_DEBUG_FLOAT_ARRAY_LEN, MAVLINK_MSG_ID_DEBUG_FLOAT_ARRAY_CRC); +} + +/** + * @brief Pack a debug_float_array message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param time_usec [us] Timestamp (UNIX Epoch time or time since system boot). The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude of the number. + * @param name Name, for human-friendly display in a Ground Control Station + * @param array_id Unique ID used to discriminate between arrays + * @param data data + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_debug_float_array_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint64_t time_usec,const char *name,uint16_t array_id,const float *data) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_DEBUG_FLOAT_ARRAY_LEN]; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_uint16_t(buf, 8, array_id); + _mav_put_char_array(buf, 10, name, 10); + _mav_put_float_array(buf, 20, data, 58); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_DEBUG_FLOAT_ARRAY_LEN); +#else + mavlink_debug_float_array_t packet; + packet.time_usec = time_usec; + packet.array_id = array_id; + mav_array_memcpy(packet.name, name, sizeof(char)*10); + mav_array_memcpy(packet.data, data, sizeof(float)*58); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_DEBUG_FLOAT_ARRAY_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_DEBUG_FLOAT_ARRAY; + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_DEBUG_FLOAT_ARRAY_MIN_LEN, MAVLINK_MSG_ID_DEBUG_FLOAT_ARRAY_LEN, MAVLINK_MSG_ID_DEBUG_FLOAT_ARRAY_CRC); +} + +/** + * @brief Encode a debug_float_array struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param debug_float_array C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_debug_float_array_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_debug_float_array_t* debug_float_array) +{ + return mavlink_msg_debug_float_array_pack(system_id, component_id, msg, debug_float_array->time_usec, debug_float_array->name, debug_float_array->array_id, debug_float_array->data); +} + +/** + * @brief Encode a debug_float_array struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param debug_float_array C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_debug_float_array_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_debug_float_array_t* debug_float_array) +{ + return mavlink_msg_debug_float_array_pack_chan(system_id, component_id, chan, msg, debug_float_array->time_usec, debug_float_array->name, debug_float_array->array_id, debug_float_array->data); +} + +/** + * @brief Send a debug_float_array message + * @param chan MAVLink channel to send the message + * + * @param time_usec [us] Timestamp (UNIX Epoch time or time since system boot). The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude of the number. + * @param name Name, for human-friendly display in a Ground Control Station + * @param array_id Unique ID used to discriminate between arrays + * @param data data + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_debug_float_array_send(mavlink_channel_t chan, uint64_t time_usec, const char *name, uint16_t array_id, const float *data) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_DEBUG_FLOAT_ARRAY_LEN]; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_uint16_t(buf, 8, array_id); + _mav_put_char_array(buf, 10, name, 10); + _mav_put_float_array(buf, 20, data, 58); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DEBUG_FLOAT_ARRAY, buf, MAVLINK_MSG_ID_DEBUG_FLOAT_ARRAY_MIN_LEN, MAVLINK_MSG_ID_DEBUG_FLOAT_ARRAY_LEN, MAVLINK_MSG_ID_DEBUG_FLOAT_ARRAY_CRC); +#else + mavlink_debug_float_array_t packet; + packet.time_usec = time_usec; + packet.array_id = array_id; + mav_array_memcpy(packet.name, name, sizeof(char)*10); + mav_array_memcpy(packet.data, data, sizeof(float)*58); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DEBUG_FLOAT_ARRAY, (const char *)&packet, MAVLINK_MSG_ID_DEBUG_FLOAT_ARRAY_MIN_LEN, MAVLINK_MSG_ID_DEBUG_FLOAT_ARRAY_LEN, MAVLINK_MSG_ID_DEBUG_FLOAT_ARRAY_CRC); +#endif +} + +/** + * @brief Send a debug_float_array message + * @param chan MAVLink channel to send the message + * @param struct The MAVLink struct to serialize + */ +static inline void mavlink_msg_debug_float_array_send_struct(mavlink_channel_t chan, const mavlink_debug_float_array_t* debug_float_array) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + mavlink_msg_debug_float_array_send(chan, debug_float_array->time_usec, debug_float_array->name, debug_float_array->array_id, debug_float_array->data); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DEBUG_FLOAT_ARRAY, (const char *)debug_float_array, MAVLINK_MSG_ID_DEBUG_FLOAT_ARRAY_MIN_LEN, MAVLINK_MSG_ID_DEBUG_FLOAT_ARRAY_LEN, MAVLINK_MSG_ID_DEBUG_FLOAT_ARRAY_CRC); +#endif +} + +#if MAVLINK_MSG_ID_DEBUG_FLOAT_ARRAY_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_debug_float_array_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint64_t time_usec, const char *name, uint16_t array_id, const float *data) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_uint16_t(buf, 8, array_id); + _mav_put_char_array(buf, 10, name, 10); + _mav_put_float_array(buf, 20, data, 58); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DEBUG_FLOAT_ARRAY, buf, MAVLINK_MSG_ID_DEBUG_FLOAT_ARRAY_MIN_LEN, MAVLINK_MSG_ID_DEBUG_FLOAT_ARRAY_LEN, MAVLINK_MSG_ID_DEBUG_FLOAT_ARRAY_CRC); +#else + mavlink_debug_float_array_t *packet = (mavlink_debug_float_array_t *)msgbuf; + packet->time_usec = time_usec; + packet->array_id = array_id; + mav_array_memcpy(packet->name, name, sizeof(char)*10); + mav_array_memcpy(packet->data, data, sizeof(float)*58); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DEBUG_FLOAT_ARRAY, (const char *)packet, MAVLINK_MSG_ID_DEBUG_FLOAT_ARRAY_MIN_LEN, MAVLINK_MSG_ID_DEBUG_FLOAT_ARRAY_LEN, MAVLINK_MSG_ID_DEBUG_FLOAT_ARRAY_CRC); +#endif +} +#endif + +#endif + +// MESSAGE DEBUG_FLOAT_ARRAY UNPACKING + + +/** + * @brief Get field time_usec from debug_float_array message + * + * @return [us] Timestamp (UNIX Epoch time or time since system boot). The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude of the number. + */ +static inline uint64_t mavlink_msg_debug_float_array_get_time_usec(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint64_t(msg, 0); +} + +/** + * @brief Get field name from debug_float_array message + * + * @return Name, for human-friendly display in a Ground Control Station + */ +static inline uint16_t mavlink_msg_debug_float_array_get_name(const mavlink_message_t* msg, char *name) +{ + return _MAV_RETURN_char_array(msg, name, 10, 10); +} + +/** + * @brief Get field array_id from debug_float_array message + * + * @return Unique ID used to discriminate between arrays + */ +static inline uint16_t mavlink_msg_debug_float_array_get_array_id(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 8); +} + +/** + * @brief Get field data from debug_float_array message + * + * @return data + */ +static inline uint16_t mavlink_msg_debug_float_array_get_data(const mavlink_message_t* msg, float *data) +{ + return _MAV_RETURN_float_array(msg, data, 58, 20); +} + +/** + * @brief Decode a debug_float_array message into a struct + * + * @param msg The message to decode + * @param debug_float_array C-struct to decode the message contents into + */ +static inline void mavlink_msg_debug_float_array_decode(const mavlink_message_t* msg, mavlink_debug_float_array_t* debug_float_array) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + debug_float_array->time_usec = mavlink_msg_debug_float_array_get_time_usec(msg); + debug_float_array->array_id = mavlink_msg_debug_float_array_get_array_id(msg); + mavlink_msg_debug_float_array_get_name(msg, debug_float_array->name); + mavlink_msg_debug_float_array_get_data(msg, debug_float_array->data); +#else + uint8_t len = msg->len < MAVLINK_MSG_ID_DEBUG_FLOAT_ARRAY_LEN? msg->len : MAVLINK_MSG_ID_DEBUG_FLOAT_ARRAY_LEN; + memset(debug_float_array, 0, MAVLINK_MSG_ID_DEBUG_FLOAT_ARRAY_LEN); + memcpy(debug_float_array, _MAV_PAYLOAD(msg), len); +#endif +} diff --git a/lib/main/MAVLink/common/mavlink_msg_distance_sensor.h b/lib/main/MAVLink/common/mavlink_msg_distance_sensor.h index a5b0107be3..7e4e850408 100755 --- a/lib/main/MAVLink/common/mavlink_msg_distance_sensor.h +++ b/lib/main/MAVLink/common/mavlink_msg_distance_sensor.h @@ -3,7 +3,7 @@ #define MAVLINK_MSG_ID_DISTANCE_SENSOR 132 - +MAVPACKED( typedef struct __mavlink_distance_sensor_t { uint32_t time_boot_ms; /*< [ms] Timestamp (time since system boot).*/ uint16_t min_distance; /*< [cm] Minimum distance the sensor can measure*/ @@ -13,23 +13,27 @@ typedef struct __mavlink_distance_sensor_t { uint8_t id; /*< Onboard ID of the sensor*/ uint8_t orientation; /*< Direction the sensor faces. downward-facing: ROTATION_PITCH_270, upward-facing: ROTATION_PITCH_90, backward-facing: ROTATION_PITCH_180, forward-facing: ROTATION_NONE, left-facing: ROTATION_YAW_90, right-facing: ROTATION_YAW_270*/ uint8_t covariance; /*< [cm^2] Measurement variance. Max standard deviation is 6cm. 255 if unknown.*/ -} mavlink_distance_sensor_t; + float horizontal_fov; /*< [rad] Horizontal Field of View (angle) where the distance measurement is valid and the field of view is known. Otherwise this is set to 0.*/ + float vertical_fov; /*< [rad] Vertical Field of View (angle) where the distance measurement is valid and the field of view is known. Otherwise this is set to 0.*/ + float quaternion[4]; /*< Quaternion of the sensor orientation in vehicle body frame (w, x, y, z order, zero-rotation is 1, 0, 0, 0). Zero-rotation is along the vehicle body x-axis. This field is required if the orientation is set to MAV_SENSOR_ROTATION_CUSTOM. Set it to 0 if invalid."*/ + uint8_t signal_quality; /*< [%] Signal quality of the sensor. Specific to each sensor type, representing the relation of the signal strength with the target reflectivity, distance, size or aspect, but normalised as a percentage. 0 = unknown/unset signal quality, 1 = invalid signal, 100 = perfect signal.*/ +}) mavlink_distance_sensor_t; -#define MAVLINK_MSG_ID_DISTANCE_SENSOR_LEN 14 +#define MAVLINK_MSG_ID_DISTANCE_SENSOR_LEN 39 #define MAVLINK_MSG_ID_DISTANCE_SENSOR_MIN_LEN 14 -#define MAVLINK_MSG_ID_132_LEN 14 +#define MAVLINK_MSG_ID_132_LEN 39 #define MAVLINK_MSG_ID_132_MIN_LEN 14 #define MAVLINK_MSG_ID_DISTANCE_SENSOR_CRC 85 #define MAVLINK_MSG_ID_132_CRC 85 - +#define MAVLINK_MSG_DISTANCE_SENSOR_FIELD_QUATERNION_LEN 4 #if MAVLINK_COMMAND_24BIT #define MAVLINK_MESSAGE_INFO_DISTANCE_SENSOR { \ 132, \ "DISTANCE_SENSOR", \ - 8, \ + 12, \ { { "time_boot_ms", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_distance_sensor_t, time_boot_ms) }, \ { "min_distance", NULL, MAVLINK_TYPE_UINT16_T, 0, 4, offsetof(mavlink_distance_sensor_t, min_distance) }, \ { "max_distance", NULL, MAVLINK_TYPE_UINT16_T, 0, 6, offsetof(mavlink_distance_sensor_t, max_distance) }, \ @@ -38,12 +42,16 @@ typedef struct __mavlink_distance_sensor_t { { "id", NULL, MAVLINK_TYPE_UINT8_T, 0, 11, offsetof(mavlink_distance_sensor_t, id) }, \ { "orientation", NULL, MAVLINK_TYPE_UINT8_T, 0, 12, offsetof(mavlink_distance_sensor_t, orientation) }, \ { "covariance", NULL, MAVLINK_TYPE_UINT8_T, 0, 13, offsetof(mavlink_distance_sensor_t, covariance) }, \ + { "horizontal_fov", NULL, MAVLINK_TYPE_FLOAT, 0, 14, offsetof(mavlink_distance_sensor_t, horizontal_fov) }, \ + { "vertical_fov", NULL, MAVLINK_TYPE_FLOAT, 0, 18, offsetof(mavlink_distance_sensor_t, vertical_fov) }, \ + { "quaternion", NULL, MAVLINK_TYPE_FLOAT, 4, 22, offsetof(mavlink_distance_sensor_t, quaternion) }, \ + { "signal_quality", NULL, MAVLINK_TYPE_UINT8_T, 0, 38, offsetof(mavlink_distance_sensor_t, signal_quality) }, \ } \ } #else #define MAVLINK_MESSAGE_INFO_DISTANCE_SENSOR { \ "DISTANCE_SENSOR", \ - 8, \ + 12, \ { { "time_boot_ms", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_distance_sensor_t, time_boot_ms) }, \ { "min_distance", NULL, MAVLINK_TYPE_UINT16_T, 0, 4, offsetof(mavlink_distance_sensor_t, min_distance) }, \ { "max_distance", NULL, MAVLINK_TYPE_UINT16_T, 0, 6, offsetof(mavlink_distance_sensor_t, max_distance) }, \ @@ -52,6 +60,10 @@ typedef struct __mavlink_distance_sensor_t { { "id", NULL, MAVLINK_TYPE_UINT8_T, 0, 11, offsetof(mavlink_distance_sensor_t, id) }, \ { "orientation", NULL, MAVLINK_TYPE_UINT8_T, 0, 12, offsetof(mavlink_distance_sensor_t, orientation) }, \ { "covariance", NULL, MAVLINK_TYPE_UINT8_T, 0, 13, offsetof(mavlink_distance_sensor_t, covariance) }, \ + { "horizontal_fov", NULL, MAVLINK_TYPE_FLOAT, 0, 14, offsetof(mavlink_distance_sensor_t, horizontal_fov) }, \ + { "vertical_fov", NULL, MAVLINK_TYPE_FLOAT, 0, 18, offsetof(mavlink_distance_sensor_t, vertical_fov) }, \ + { "quaternion", NULL, MAVLINK_TYPE_FLOAT, 4, 22, offsetof(mavlink_distance_sensor_t, quaternion) }, \ + { "signal_quality", NULL, MAVLINK_TYPE_UINT8_T, 0, 38, offsetof(mavlink_distance_sensor_t, signal_quality) }, \ } \ } #endif @@ -70,10 +82,14 @@ typedef struct __mavlink_distance_sensor_t { * @param id Onboard ID of the sensor * @param orientation Direction the sensor faces. downward-facing: ROTATION_PITCH_270, upward-facing: ROTATION_PITCH_90, backward-facing: ROTATION_PITCH_180, forward-facing: ROTATION_NONE, left-facing: ROTATION_YAW_90, right-facing: ROTATION_YAW_270 * @param covariance [cm^2] Measurement variance. Max standard deviation is 6cm. 255 if unknown. + * @param horizontal_fov [rad] Horizontal Field of View (angle) where the distance measurement is valid and the field of view is known. Otherwise this is set to 0. + * @param vertical_fov [rad] Vertical Field of View (angle) where the distance measurement is valid and the field of view is known. Otherwise this is set to 0. + * @param quaternion Quaternion of the sensor orientation in vehicle body frame (w, x, y, z order, zero-rotation is 1, 0, 0, 0). Zero-rotation is along the vehicle body x-axis. This field is required if the orientation is set to MAV_SENSOR_ROTATION_CUSTOM. Set it to 0 if invalid." + * @param signal_quality [%] Signal quality of the sensor. Specific to each sensor type, representing the relation of the signal strength with the target reflectivity, distance, size or aspect, but normalised as a percentage. 0 = unknown/unset signal quality, 1 = invalid signal, 100 = perfect signal. * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_distance_sensor_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint32_t time_boot_ms, uint16_t min_distance, uint16_t max_distance, uint16_t current_distance, uint8_t type, uint8_t id, uint8_t orientation, uint8_t covariance) + uint32_t time_boot_ms, uint16_t min_distance, uint16_t max_distance, uint16_t current_distance, uint8_t type, uint8_t id, uint8_t orientation, uint8_t covariance, float horizontal_fov, float vertical_fov, const float *quaternion, uint8_t signal_quality) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_DISTANCE_SENSOR_LEN]; @@ -85,7 +101,10 @@ static inline uint16_t mavlink_msg_distance_sensor_pack(uint8_t system_id, uint8 _mav_put_uint8_t(buf, 11, id); _mav_put_uint8_t(buf, 12, orientation); _mav_put_uint8_t(buf, 13, covariance); - + _mav_put_float(buf, 14, horizontal_fov); + _mav_put_float(buf, 18, vertical_fov); + _mav_put_uint8_t(buf, 38, signal_quality); + _mav_put_float_array(buf, 22, quaternion, 4); memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_DISTANCE_SENSOR_LEN); #else mavlink_distance_sensor_t packet; @@ -97,7 +116,10 @@ static inline uint16_t mavlink_msg_distance_sensor_pack(uint8_t system_id, uint8 packet.id = id; packet.orientation = orientation; packet.covariance = covariance; - + packet.horizontal_fov = horizontal_fov; + packet.vertical_fov = vertical_fov; + packet.signal_quality = signal_quality; + mav_array_memcpy(packet.quaternion, quaternion, sizeof(float)*4); memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_DISTANCE_SENSOR_LEN); #endif @@ -119,11 +141,15 @@ static inline uint16_t mavlink_msg_distance_sensor_pack(uint8_t system_id, uint8 * @param id Onboard ID of the sensor * @param orientation Direction the sensor faces. downward-facing: ROTATION_PITCH_270, upward-facing: ROTATION_PITCH_90, backward-facing: ROTATION_PITCH_180, forward-facing: ROTATION_NONE, left-facing: ROTATION_YAW_90, right-facing: ROTATION_YAW_270 * @param covariance [cm^2] Measurement variance. Max standard deviation is 6cm. 255 if unknown. + * @param horizontal_fov [rad] Horizontal Field of View (angle) where the distance measurement is valid and the field of view is known. Otherwise this is set to 0. + * @param vertical_fov [rad] Vertical Field of View (angle) where the distance measurement is valid and the field of view is known. Otherwise this is set to 0. + * @param quaternion Quaternion of the sensor orientation in vehicle body frame (w, x, y, z order, zero-rotation is 1, 0, 0, 0). Zero-rotation is along the vehicle body x-axis. This field is required if the orientation is set to MAV_SENSOR_ROTATION_CUSTOM. Set it to 0 if invalid." + * @param signal_quality [%] Signal quality of the sensor. Specific to each sensor type, representing the relation of the signal strength with the target reflectivity, distance, size or aspect, but normalised as a percentage. 0 = unknown/unset signal quality, 1 = invalid signal, 100 = perfect signal. * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_distance_sensor_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, - uint32_t time_boot_ms,uint16_t min_distance,uint16_t max_distance,uint16_t current_distance,uint8_t type,uint8_t id,uint8_t orientation,uint8_t covariance) + uint32_t time_boot_ms,uint16_t min_distance,uint16_t max_distance,uint16_t current_distance,uint8_t type,uint8_t id,uint8_t orientation,uint8_t covariance,float horizontal_fov,float vertical_fov,const float *quaternion,uint8_t signal_quality) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_DISTANCE_SENSOR_LEN]; @@ -135,7 +161,10 @@ static inline uint16_t mavlink_msg_distance_sensor_pack_chan(uint8_t system_id, _mav_put_uint8_t(buf, 11, id); _mav_put_uint8_t(buf, 12, orientation); _mav_put_uint8_t(buf, 13, covariance); - + _mav_put_float(buf, 14, horizontal_fov); + _mav_put_float(buf, 18, vertical_fov); + _mav_put_uint8_t(buf, 38, signal_quality); + _mav_put_float_array(buf, 22, quaternion, 4); memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_DISTANCE_SENSOR_LEN); #else mavlink_distance_sensor_t packet; @@ -147,7 +176,10 @@ static inline uint16_t mavlink_msg_distance_sensor_pack_chan(uint8_t system_id, packet.id = id; packet.orientation = orientation; packet.covariance = covariance; - + packet.horizontal_fov = horizontal_fov; + packet.vertical_fov = vertical_fov; + packet.signal_quality = signal_quality; + mav_array_memcpy(packet.quaternion, quaternion, sizeof(float)*4); memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_DISTANCE_SENSOR_LEN); #endif @@ -165,7 +197,7 @@ static inline uint16_t mavlink_msg_distance_sensor_pack_chan(uint8_t system_id, */ static inline uint16_t mavlink_msg_distance_sensor_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_distance_sensor_t* distance_sensor) { - return mavlink_msg_distance_sensor_pack(system_id, component_id, msg, distance_sensor->time_boot_ms, distance_sensor->min_distance, distance_sensor->max_distance, distance_sensor->current_distance, distance_sensor->type, distance_sensor->id, distance_sensor->orientation, distance_sensor->covariance); + return mavlink_msg_distance_sensor_pack(system_id, component_id, msg, distance_sensor->time_boot_ms, distance_sensor->min_distance, distance_sensor->max_distance, distance_sensor->current_distance, distance_sensor->type, distance_sensor->id, distance_sensor->orientation, distance_sensor->covariance, distance_sensor->horizontal_fov, distance_sensor->vertical_fov, distance_sensor->quaternion, distance_sensor->signal_quality); } /** @@ -179,7 +211,7 @@ static inline uint16_t mavlink_msg_distance_sensor_encode(uint8_t system_id, uin */ static inline uint16_t mavlink_msg_distance_sensor_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_distance_sensor_t* distance_sensor) { - return mavlink_msg_distance_sensor_pack_chan(system_id, component_id, chan, msg, distance_sensor->time_boot_ms, distance_sensor->min_distance, distance_sensor->max_distance, distance_sensor->current_distance, distance_sensor->type, distance_sensor->id, distance_sensor->orientation, distance_sensor->covariance); + return mavlink_msg_distance_sensor_pack_chan(system_id, component_id, chan, msg, distance_sensor->time_boot_ms, distance_sensor->min_distance, distance_sensor->max_distance, distance_sensor->current_distance, distance_sensor->type, distance_sensor->id, distance_sensor->orientation, distance_sensor->covariance, distance_sensor->horizontal_fov, distance_sensor->vertical_fov, distance_sensor->quaternion, distance_sensor->signal_quality); } /** @@ -194,10 +226,14 @@ static inline uint16_t mavlink_msg_distance_sensor_encode_chan(uint8_t system_id * @param id Onboard ID of the sensor * @param orientation Direction the sensor faces. downward-facing: ROTATION_PITCH_270, upward-facing: ROTATION_PITCH_90, backward-facing: ROTATION_PITCH_180, forward-facing: ROTATION_NONE, left-facing: ROTATION_YAW_90, right-facing: ROTATION_YAW_270 * @param covariance [cm^2] Measurement variance. Max standard deviation is 6cm. 255 if unknown. + * @param horizontal_fov [rad] Horizontal Field of View (angle) where the distance measurement is valid and the field of view is known. Otherwise this is set to 0. + * @param vertical_fov [rad] Vertical Field of View (angle) where the distance measurement is valid and the field of view is known. Otherwise this is set to 0. + * @param quaternion Quaternion of the sensor orientation in vehicle body frame (w, x, y, z order, zero-rotation is 1, 0, 0, 0). Zero-rotation is along the vehicle body x-axis. This field is required if the orientation is set to MAV_SENSOR_ROTATION_CUSTOM. Set it to 0 if invalid." + * @param signal_quality [%] Signal quality of the sensor. Specific to each sensor type, representing the relation of the signal strength with the target reflectivity, distance, size or aspect, but normalised as a percentage. 0 = unknown/unset signal quality, 1 = invalid signal, 100 = perfect signal. */ #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS -static inline void mavlink_msg_distance_sensor_send(mavlink_channel_t chan, uint32_t time_boot_ms, uint16_t min_distance, uint16_t max_distance, uint16_t current_distance, uint8_t type, uint8_t id, uint8_t orientation, uint8_t covariance) +static inline void mavlink_msg_distance_sensor_send(mavlink_channel_t chan, uint32_t time_boot_ms, uint16_t min_distance, uint16_t max_distance, uint16_t current_distance, uint8_t type, uint8_t id, uint8_t orientation, uint8_t covariance, float horizontal_fov, float vertical_fov, const float *quaternion, uint8_t signal_quality) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_DISTANCE_SENSOR_LEN]; @@ -209,7 +245,10 @@ static inline void mavlink_msg_distance_sensor_send(mavlink_channel_t chan, uint _mav_put_uint8_t(buf, 11, id); _mav_put_uint8_t(buf, 12, orientation); _mav_put_uint8_t(buf, 13, covariance); - + _mav_put_float(buf, 14, horizontal_fov); + _mav_put_float(buf, 18, vertical_fov); + _mav_put_uint8_t(buf, 38, signal_quality); + _mav_put_float_array(buf, 22, quaternion, 4); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DISTANCE_SENSOR, buf, MAVLINK_MSG_ID_DISTANCE_SENSOR_MIN_LEN, MAVLINK_MSG_ID_DISTANCE_SENSOR_LEN, MAVLINK_MSG_ID_DISTANCE_SENSOR_CRC); #else mavlink_distance_sensor_t packet; @@ -221,7 +260,10 @@ static inline void mavlink_msg_distance_sensor_send(mavlink_channel_t chan, uint packet.id = id; packet.orientation = orientation; packet.covariance = covariance; - + packet.horizontal_fov = horizontal_fov; + packet.vertical_fov = vertical_fov; + packet.signal_quality = signal_quality; + mav_array_memcpy(packet.quaternion, quaternion, sizeof(float)*4); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DISTANCE_SENSOR, (const char *)&packet, MAVLINK_MSG_ID_DISTANCE_SENSOR_MIN_LEN, MAVLINK_MSG_ID_DISTANCE_SENSOR_LEN, MAVLINK_MSG_ID_DISTANCE_SENSOR_CRC); #endif } @@ -234,7 +276,7 @@ static inline void mavlink_msg_distance_sensor_send(mavlink_channel_t chan, uint static inline void mavlink_msg_distance_sensor_send_struct(mavlink_channel_t chan, const mavlink_distance_sensor_t* distance_sensor) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - mavlink_msg_distance_sensor_send(chan, distance_sensor->time_boot_ms, distance_sensor->min_distance, distance_sensor->max_distance, distance_sensor->current_distance, distance_sensor->type, distance_sensor->id, distance_sensor->orientation, distance_sensor->covariance); + mavlink_msg_distance_sensor_send(chan, distance_sensor->time_boot_ms, distance_sensor->min_distance, distance_sensor->max_distance, distance_sensor->current_distance, distance_sensor->type, distance_sensor->id, distance_sensor->orientation, distance_sensor->covariance, distance_sensor->horizontal_fov, distance_sensor->vertical_fov, distance_sensor->quaternion, distance_sensor->signal_quality); #else _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DISTANCE_SENSOR, (const char *)distance_sensor, MAVLINK_MSG_ID_DISTANCE_SENSOR_MIN_LEN, MAVLINK_MSG_ID_DISTANCE_SENSOR_LEN, MAVLINK_MSG_ID_DISTANCE_SENSOR_CRC); #endif @@ -248,7 +290,7 @@ static inline void mavlink_msg_distance_sensor_send_struct(mavlink_channel_t cha is usually the receive buffer for the channel, and allows a reply to an incoming message with minimum stack space usage. */ -static inline void mavlink_msg_distance_sensor_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint32_t time_boot_ms, uint16_t min_distance, uint16_t max_distance, uint16_t current_distance, uint8_t type, uint8_t id, uint8_t orientation, uint8_t covariance) +static inline void mavlink_msg_distance_sensor_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint32_t time_boot_ms, uint16_t min_distance, uint16_t max_distance, uint16_t current_distance, uint8_t type, uint8_t id, uint8_t orientation, uint8_t covariance, float horizontal_fov, float vertical_fov, const float *quaternion, uint8_t signal_quality) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char *buf = (char *)msgbuf; @@ -260,7 +302,10 @@ static inline void mavlink_msg_distance_sensor_send_buf(mavlink_message_t *msgbu _mav_put_uint8_t(buf, 11, id); _mav_put_uint8_t(buf, 12, orientation); _mav_put_uint8_t(buf, 13, covariance); - + _mav_put_float(buf, 14, horizontal_fov); + _mav_put_float(buf, 18, vertical_fov); + _mav_put_uint8_t(buf, 38, signal_quality); + _mav_put_float_array(buf, 22, quaternion, 4); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DISTANCE_SENSOR, buf, MAVLINK_MSG_ID_DISTANCE_SENSOR_MIN_LEN, MAVLINK_MSG_ID_DISTANCE_SENSOR_LEN, MAVLINK_MSG_ID_DISTANCE_SENSOR_CRC); #else mavlink_distance_sensor_t *packet = (mavlink_distance_sensor_t *)msgbuf; @@ -272,7 +317,10 @@ static inline void mavlink_msg_distance_sensor_send_buf(mavlink_message_t *msgbu packet->id = id; packet->orientation = orientation; packet->covariance = covariance; - + packet->horizontal_fov = horizontal_fov; + packet->vertical_fov = vertical_fov; + packet->signal_quality = signal_quality; + mav_array_memcpy(packet->quaternion, quaternion, sizeof(float)*4); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_DISTANCE_SENSOR, (const char *)packet, MAVLINK_MSG_ID_DISTANCE_SENSOR_MIN_LEN, MAVLINK_MSG_ID_DISTANCE_SENSOR_LEN, MAVLINK_MSG_ID_DISTANCE_SENSOR_CRC); #endif } @@ -363,6 +411,46 @@ static inline uint8_t mavlink_msg_distance_sensor_get_covariance(const mavlink_m return _MAV_RETURN_uint8_t(msg, 13); } +/** + * @brief Get field horizontal_fov from distance_sensor message + * + * @return [rad] Horizontal Field of View (angle) where the distance measurement is valid and the field of view is known. Otherwise this is set to 0. + */ +static inline float mavlink_msg_distance_sensor_get_horizontal_fov(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 14); +} + +/** + * @brief Get field vertical_fov from distance_sensor message + * + * @return [rad] Vertical Field of View (angle) where the distance measurement is valid and the field of view is known. Otherwise this is set to 0. + */ +static inline float mavlink_msg_distance_sensor_get_vertical_fov(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 18); +} + +/** + * @brief Get field quaternion from distance_sensor message + * + * @return Quaternion of the sensor orientation in vehicle body frame (w, x, y, z order, zero-rotation is 1, 0, 0, 0). Zero-rotation is along the vehicle body x-axis. This field is required if the orientation is set to MAV_SENSOR_ROTATION_CUSTOM. Set it to 0 if invalid." + */ +static inline uint16_t mavlink_msg_distance_sensor_get_quaternion(const mavlink_message_t* msg, float *quaternion) +{ + return _MAV_RETURN_float_array(msg, quaternion, 4, 22); +} + +/** + * @brief Get field signal_quality from distance_sensor message + * + * @return [%] Signal quality of the sensor. Specific to each sensor type, representing the relation of the signal strength with the target reflectivity, distance, size or aspect, but normalised as a percentage. 0 = unknown/unset signal quality, 1 = invalid signal, 100 = perfect signal. + */ +static inline uint8_t mavlink_msg_distance_sensor_get_signal_quality(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 38); +} + /** * @brief Decode a distance_sensor message into a struct * @@ -380,6 +468,10 @@ static inline void mavlink_msg_distance_sensor_decode(const mavlink_message_t* m distance_sensor->id = mavlink_msg_distance_sensor_get_id(msg); distance_sensor->orientation = mavlink_msg_distance_sensor_get_orientation(msg); distance_sensor->covariance = mavlink_msg_distance_sensor_get_covariance(msg); + distance_sensor->horizontal_fov = mavlink_msg_distance_sensor_get_horizontal_fov(msg); + distance_sensor->vertical_fov = mavlink_msg_distance_sensor_get_vertical_fov(msg); + mavlink_msg_distance_sensor_get_quaternion(msg, distance_sensor->quaternion); + distance_sensor->signal_quality = mavlink_msg_distance_sensor_get_signal_quality(msg); #else uint8_t len = msg->len < MAVLINK_MSG_ID_DISTANCE_SENSOR_LEN? msg->len : MAVLINK_MSG_ID_DISTANCE_SENSOR_LEN; memset(distance_sensor, 0, MAVLINK_MSG_ID_DISTANCE_SENSOR_LEN); diff --git a/lib/main/MAVLink/common/mavlink_msg_efi_status.h b/lib/main/MAVLink/common/mavlink_msg_efi_status.h new file mode 100644 index 0000000000..d631024044 --- /dev/null +++ b/lib/main/MAVLink/common/mavlink_msg_efi_status.h @@ -0,0 +1,613 @@ +#pragma once +// MESSAGE EFI_STATUS PACKING + +#define MAVLINK_MSG_ID_EFI_STATUS 225 + + +typedef struct __mavlink_efi_status_t { + float ecu_index; /*< ECU index*/ + float rpm; /*< RPM*/ + float fuel_consumed; /*< [cm^3] Fuel consumed*/ + float fuel_flow; /*< [cm^3/min] Fuel flow rate*/ + float engine_load; /*< [%] Engine load*/ + float throttle_position; /*< [%] Throttle position*/ + float spark_dwell_time; /*< [ms] Spark dwell time*/ + float barometric_pressure; /*< [kPa] Barometric pressure*/ + float intake_manifold_pressure; /*< [kPa] Intake manifold pressure(*/ + float intake_manifold_temperature; /*< [degC] Intake manifold temperature*/ + float cylinder_head_temperature; /*< [degC] Cylinder head temperature*/ + float ignition_timing; /*< [deg] Ignition timing (Crank angle degrees)*/ + float injection_time; /*< [ms] Injection time*/ + float exhaust_gas_temperature; /*< [degC] Exhaust gas temperature*/ + float throttle_out; /*< [%] Output throttle*/ + float pt_compensation; /*< Pressure/temperature compensation*/ + uint8_t health; /*< EFI health status*/ +} mavlink_efi_status_t; + +#define MAVLINK_MSG_ID_EFI_STATUS_LEN 65 +#define MAVLINK_MSG_ID_EFI_STATUS_MIN_LEN 65 +#define MAVLINK_MSG_ID_225_LEN 65 +#define MAVLINK_MSG_ID_225_MIN_LEN 65 + +#define MAVLINK_MSG_ID_EFI_STATUS_CRC 208 +#define MAVLINK_MSG_ID_225_CRC 208 + + + +#if MAVLINK_COMMAND_24BIT +#define MAVLINK_MESSAGE_INFO_EFI_STATUS { \ + 225, \ + "EFI_STATUS", \ + 17, \ + { { "health", NULL, MAVLINK_TYPE_UINT8_T, 0, 64, offsetof(mavlink_efi_status_t, health) }, \ + { "ecu_index", NULL, MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_efi_status_t, ecu_index) }, \ + { "rpm", NULL, MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_efi_status_t, rpm) }, \ + { "fuel_consumed", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_efi_status_t, fuel_consumed) }, \ + { "fuel_flow", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_efi_status_t, fuel_flow) }, \ + { "engine_load", NULL, MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_efi_status_t, engine_load) }, \ + { "throttle_position", NULL, MAVLINK_TYPE_FLOAT, 0, 20, offsetof(mavlink_efi_status_t, throttle_position) }, \ + { "spark_dwell_time", NULL, MAVLINK_TYPE_FLOAT, 0, 24, offsetof(mavlink_efi_status_t, spark_dwell_time) }, \ + { "barometric_pressure", NULL, MAVLINK_TYPE_FLOAT, 0, 28, offsetof(mavlink_efi_status_t, barometric_pressure) }, \ + { "intake_manifold_pressure", NULL, MAVLINK_TYPE_FLOAT, 0, 32, offsetof(mavlink_efi_status_t, intake_manifold_pressure) }, \ + { "intake_manifold_temperature", NULL, MAVLINK_TYPE_FLOAT, 0, 36, offsetof(mavlink_efi_status_t, intake_manifold_temperature) }, \ + { "cylinder_head_temperature", NULL, MAVLINK_TYPE_FLOAT, 0, 40, offsetof(mavlink_efi_status_t, cylinder_head_temperature) }, \ + { "ignition_timing", NULL, MAVLINK_TYPE_FLOAT, 0, 44, offsetof(mavlink_efi_status_t, ignition_timing) }, \ + { "injection_time", NULL, MAVLINK_TYPE_FLOAT, 0, 48, offsetof(mavlink_efi_status_t, injection_time) }, \ + { "exhaust_gas_temperature", NULL, MAVLINK_TYPE_FLOAT, 0, 52, offsetof(mavlink_efi_status_t, exhaust_gas_temperature) }, \ + { "throttle_out", NULL, MAVLINK_TYPE_FLOAT, 0, 56, offsetof(mavlink_efi_status_t, throttle_out) }, \ + { "pt_compensation", NULL, MAVLINK_TYPE_FLOAT, 0, 60, offsetof(mavlink_efi_status_t, pt_compensation) }, \ + } \ +} +#else +#define MAVLINK_MESSAGE_INFO_EFI_STATUS { \ + "EFI_STATUS", \ + 17, \ + { { "health", NULL, MAVLINK_TYPE_UINT8_T, 0, 64, offsetof(mavlink_efi_status_t, health) }, \ + { "ecu_index", NULL, MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_efi_status_t, ecu_index) }, \ + { "rpm", NULL, MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_efi_status_t, rpm) }, \ + { "fuel_consumed", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_efi_status_t, fuel_consumed) }, \ + { "fuel_flow", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_efi_status_t, fuel_flow) }, \ + { "engine_load", NULL, MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_efi_status_t, engine_load) }, \ + { "throttle_position", NULL, MAVLINK_TYPE_FLOAT, 0, 20, offsetof(mavlink_efi_status_t, throttle_position) }, \ + { "spark_dwell_time", NULL, MAVLINK_TYPE_FLOAT, 0, 24, offsetof(mavlink_efi_status_t, spark_dwell_time) }, \ + { "barometric_pressure", NULL, MAVLINK_TYPE_FLOAT, 0, 28, offsetof(mavlink_efi_status_t, barometric_pressure) }, \ + { "intake_manifold_pressure", NULL, MAVLINK_TYPE_FLOAT, 0, 32, offsetof(mavlink_efi_status_t, intake_manifold_pressure) }, \ + { "intake_manifold_temperature", NULL, MAVLINK_TYPE_FLOAT, 0, 36, offsetof(mavlink_efi_status_t, intake_manifold_temperature) }, \ + { "cylinder_head_temperature", NULL, MAVLINK_TYPE_FLOAT, 0, 40, offsetof(mavlink_efi_status_t, cylinder_head_temperature) }, \ + { "ignition_timing", NULL, MAVLINK_TYPE_FLOAT, 0, 44, offsetof(mavlink_efi_status_t, ignition_timing) }, \ + { "injection_time", NULL, MAVLINK_TYPE_FLOAT, 0, 48, offsetof(mavlink_efi_status_t, injection_time) }, \ + { "exhaust_gas_temperature", NULL, MAVLINK_TYPE_FLOAT, 0, 52, offsetof(mavlink_efi_status_t, exhaust_gas_temperature) }, \ + { "throttle_out", NULL, MAVLINK_TYPE_FLOAT, 0, 56, offsetof(mavlink_efi_status_t, throttle_out) }, \ + { "pt_compensation", NULL, MAVLINK_TYPE_FLOAT, 0, 60, offsetof(mavlink_efi_status_t, pt_compensation) }, \ + } \ +} +#endif + +/** + * @brief Pack a efi_status message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param health EFI health status + * @param ecu_index ECU index + * @param rpm RPM + * @param fuel_consumed [cm^3] Fuel consumed + * @param fuel_flow [cm^3/min] Fuel flow rate + * @param engine_load [%] Engine load + * @param throttle_position [%] Throttle position + * @param spark_dwell_time [ms] Spark dwell time + * @param barometric_pressure [kPa] Barometric pressure + * @param intake_manifold_pressure [kPa] Intake manifold pressure( + * @param intake_manifold_temperature [degC] Intake manifold temperature + * @param cylinder_head_temperature [degC] Cylinder head temperature + * @param ignition_timing [deg] Ignition timing (Crank angle degrees) + * @param injection_time [ms] Injection time + * @param exhaust_gas_temperature [degC] Exhaust gas temperature + * @param throttle_out [%] Output throttle + * @param pt_compensation Pressure/temperature compensation + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_efi_status_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint8_t health, float ecu_index, float rpm, float fuel_consumed, float fuel_flow, float engine_load, float throttle_position, float spark_dwell_time, float barometric_pressure, float intake_manifold_pressure, float intake_manifold_temperature, float cylinder_head_temperature, float ignition_timing, float injection_time, float exhaust_gas_temperature, float throttle_out, float pt_compensation) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_EFI_STATUS_LEN]; + _mav_put_float(buf, 0, ecu_index); + _mav_put_float(buf, 4, rpm); + _mav_put_float(buf, 8, fuel_consumed); + _mav_put_float(buf, 12, fuel_flow); + _mav_put_float(buf, 16, engine_load); + _mav_put_float(buf, 20, throttle_position); + _mav_put_float(buf, 24, spark_dwell_time); + _mav_put_float(buf, 28, barometric_pressure); + _mav_put_float(buf, 32, intake_manifold_pressure); + _mav_put_float(buf, 36, intake_manifold_temperature); + _mav_put_float(buf, 40, cylinder_head_temperature); + _mav_put_float(buf, 44, ignition_timing); + _mav_put_float(buf, 48, injection_time); + _mav_put_float(buf, 52, exhaust_gas_temperature); + _mav_put_float(buf, 56, throttle_out); + _mav_put_float(buf, 60, pt_compensation); + _mav_put_uint8_t(buf, 64, health); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_EFI_STATUS_LEN); +#else + mavlink_efi_status_t packet; + packet.ecu_index = ecu_index; + packet.rpm = rpm; + packet.fuel_consumed = fuel_consumed; + packet.fuel_flow = fuel_flow; + packet.engine_load = engine_load; + packet.throttle_position = throttle_position; + packet.spark_dwell_time = spark_dwell_time; + packet.barometric_pressure = barometric_pressure; + packet.intake_manifold_pressure = intake_manifold_pressure; + packet.intake_manifold_temperature = intake_manifold_temperature; + packet.cylinder_head_temperature = cylinder_head_temperature; + packet.ignition_timing = ignition_timing; + packet.injection_time = injection_time; + packet.exhaust_gas_temperature = exhaust_gas_temperature; + packet.throttle_out = throttle_out; + packet.pt_compensation = pt_compensation; + packet.health = health; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_EFI_STATUS_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_EFI_STATUS; + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_EFI_STATUS_MIN_LEN, MAVLINK_MSG_ID_EFI_STATUS_LEN, MAVLINK_MSG_ID_EFI_STATUS_CRC); +} + +/** + * @brief Pack a efi_status message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param health EFI health status + * @param ecu_index ECU index + * @param rpm RPM + * @param fuel_consumed [cm^3] Fuel consumed + * @param fuel_flow [cm^3/min] Fuel flow rate + * @param engine_load [%] Engine load + * @param throttle_position [%] Throttle position + * @param spark_dwell_time [ms] Spark dwell time + * @param barometric_pressure [kPa] Barometric pressure + * @param intake_manifold_pressure [kPa] Intake manifold pressure( + * @param intake_manifold_temperature [degC] Intake manifold temperature + * @param cylinder_head_temperature [degC] Cylinder head temperature + * @param ignition_timing [deg] Ignition timing (Crank angle degrees) + * @param injection_time [ms] Injection time + * @param exhaust_gas_temperature [degC] Exhaust gas temperature + * @param throttle_out [%] Output throttle + * @param pt_compensation Pressure/temperature compensation + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_efi_status_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint8_t health,float ecu_index,float rpm,float fuel_consumed,float fuel_flow,float engine_load,float throttle_position,float spark_dwell_time,float barometric_pressure,float intake_manifold_pressure,float intake_manifold_temperature,float cylinder_head_temperature,float ignition_timing,float injection_time,float exhaust_gas_temperature,float throttle_out,float pt_compensation) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_EFI_STATUS_LEN]; + _mav_put_float(buf, 0, ecu_index); + _mav_put_float(buf, 4, rpm); + _mav_put_float(buf, 8, fuel_consumed); + _mav_put_float(buf, 12, fuel_flow); + _mav_put_float(buf, 16, engine_load); + _mav_put_float(buf, 20, throttle_position); + _mav_put_float(buf, 24, spark_dwell_time); + _mav_put_float(buf, 28, barometric_pressure); + _mav_put_float(buf, 32, intake_manifold_pressure); + _mav_put_float(buf, 36, intake_manifold_temperature); + _mav_put_float(buf, 40, cylinder_head_temperature); + _mav_put_float(buf, 44, ignition_timing); + _mav_put_float(buf, 48, injection_time); + _mav_put_float(buf, 52, exhaust_gas_temperature); + _mav_put_float(buf, 56, throttle_out); + _mav_put_float(buf, 60, pt_compensation); + _mav_put_uint8_t(buf, 64, health); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_EFI_STATUS_LEN); +#else + mavlink_efi_status_t packet; + packet.ecu_index = ecu_index; + packet.rpm = rpm; + packet.fuel_consumed = fuel_consumed; + packet.fuel_flow = fuel_flow; + packet.engine_load = engine_load; + packet.throttle_position = throttle_position; + packet.spark_dwell_time = spark_dwell_time; + packet.barometric_pressure = barometric_pressure; + packet.intake_manifold_pressure = intake_manifold_pressure; + packet.intake_manifold_temperature = intake_manifold_temperature; + packet.cylinder_head_temperature = cylinder_head_temperature; + packet.ignition_timing = ignition_timing; + packet.injection_time = injection_time; + packet.exhaust_gas_temperature = exhaust_gas_temperature; + packet.throttle_out = throttle_out; + packet.pt_compensation = pt_compensation; + packet.health = health; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_EFI_STATUS_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_EFI_STATUS; + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_EFI_STATUS_MIN_LEN, MAVLINK_MSG_ID_EFI_STATUS_LEN, MAVLINK_MSG_ID_EFI_STATUS_CRC); +} + +/** + * @brief Encode a efi_status struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param efi_status C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_efi_status_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_efi_status_t* efi_status) +{ + return mavlink_msg_efi_status_pack(system_id, component_id, msg, efi_status->health, efi_status->ecu_index, efi_status->rpm, efi_status->fuel_consumed, efi_status->fuel_flow, efi_status->engine_load, efi_status->throttle_position, efi_status->spark_dwell_time, efi_status->barometric_pressure, efi_status->intake_manifold_pressure, efi_status->intake_manifold_temperature, efi_status->cylinder_head_temperature, efi_status->ignition_timing, efi_status->injection_time, efi_status->exhaust_gas_temperature, efi_status->throttle_out, efi_status->pt_compensation); +} + +/** + * @brief Encode a efi_status struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param efi_status C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_efi_status_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_efi_status_t* efi_status) +{ + return mavlink_msg_efi_status_pack_chan(system_id, component_id, chan, msg, efi_status->health, efi_status->ecu_index, efi_status->rpm, efi_status->fuel_consumed, efi_status->fuel_flow, efi_status->engine_load, efi_status->throttle_position, efi_status->spark_dwell_time, efi_status->barometric_pressure, efi_status->intake_manifold_pressure, efi_status->intake_manifold_temperature, efi_status->cylinder_head_temperature, efi_status->ignition_timing, efi_status->injection_time, efi_status->exhaust_gas_temperature, efi_status->throttle_out, efi_status->pt_compensation); +} + +/** + * @brief Send a efi_status message + * @param chan MAVLink channel to send the message + * + * @param health EFI health status + * @param ecu_index ECU index + * @param rpm RPM + * @param fuel_consumed [cm^3] Fuel consumed + * @param fuel_flow [cm^3/min] Fuel flow rate + * @param engine_load [%] Engine load + * @param throttle_position [%] Throttle position + * @param spark_dwell_time [ms] Spark dwell time + * @param barometric_pressure [kPa] Barometric pressure + * @param intake_manifold_pressure [kPa] Intake manifold pressure( + * @param intake_manifold_temperature [degC] Intake manifold temperature + * @param cylinder_head_temperature [degC] Cylinder head temperature + * @param ignition_timing [deg] Ignition timing (Crank angle degrees) + * @param injection_time [ms] Injection time + * @param exhaust_gas_temperature [degC] Exhaust gas temperature + * @param throttle_out [%] Output throttle + * @param pt_compensation Pressure/temperature compensation + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_efi_status_send(mavlink_channel_t chan, uint8_t health, float ecu_index, float rpm, float fuel_consumed, float fuel_flow, float engine_load, float throttle_position, float spark_dwell_time, float barometric_pressure, float intake_manifold_pressure, float intake_manifold_temperature, float cylinder_head_temperature, float ignition_timing, float injection_time, float exhaust_gas_temperature, float throttle_out, float pt_compensation) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_EFI_STATUS_LEN]; + _mav_put_float(buf, 0, ecu_index); + _mav_put_float(buf, 4, rpm); + _mav_put_float(buf, 8, fuel_consumed); + _mav_put_float(buf, 12, fuel_flow); + _mav_put_float(buf, 16, engine_load); + _mav_put_float(buf, 20, throttle_position); + _mav_put_float(buf, 24, spark_dwell_time); + _mav_put_float(buf, 28, barometric_pressure); + _mav_put_float(buf, 32, intake_manifold_pressure); + _mav_put_float(buf, 36, intake_manifold_temperature); + _mav_put_float(buf, 40, cylinder_head_temperature); + _mav_put_float(buf, 44, ignition_timing); + _mav_put_float(buf, 48, injection_time); + _mav_put_float(buf, 52, exhaust_gas_temperature); + _mav_put_float(buf, 56, throttle_out); + _mav_put_float(buf, 60, pt_compensation); + _mav_put_uint8_t(buf, 64, health); + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_EFI_STATUS, buf, MAVLINK_MSG_ID_EFI_STATUS_MIN_LEN, MAVLINK_MSG_ID_EFI_STATUS_LEN, MAVLINK_MSG_ID_EFI_STATUS_CRC); +#else + mavlink_efi_status_t packet; + packet.ecu_index = ecu_index; + packet.rpm = rpm; + packet.fuel_consumed = fuel_consumed; + packet.fuel_flow = fuel_flow; + packet.engine_load = engine_load; + packet.throttle_position = throttle_position; + packet.spark_dwell_time = spark_dwell_time; + packet.barometric_pressure = barometric_pressure; + packet.intake_manifold_pressure = intake_manifold_pressure; + packet.intake_manifold_temperature = intake_manifold_temperature; + packet.cylinder_head_temperature = cylinder_head_temperature; + packet.ignition_timing = ignition_timing; + packet.injection_time = injection_time; + packet.exhaust_gas_temperature = exhaust_gas_temperature; + packet.throttle_out = throttle_out; + packet.pt_compensation = pt_compensation; + packet.health = health; + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_EFI_STATUS, (const char *)&packet, MAVLINK_MSG_ID_EFI_STATUS_MIN_LEN, MAVLINK_MSG_ID_EFI_STATUS_LEN, MAVLINK_MSG_ID_EFI_STATUS_CRC); +#endif +} + +/** + * @brief Send a efi_status message + * @param chan MAVLink channel to send the message + * @param struct The MAVLink struct to serialize + */ +static inline void mavlink_msg_efi_status_send_struct(mavlink_channel_t chan, const mavlink_efi_status_t* efi_status) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + mavlink_msg_efi_status_send(chan, efi_status->health, efi_status->ecu_index, efi_status->rpm, efi_status->fuel_consumed, efi_status->fuel_flow, efi_status->engine_load, efi_status->throttle_position, efi_status->spark_dwell_time, efi_status->barometric_pressure, efi_status->intake_manifold_pressure, efi_status->intake_manifold_temperature, efi_status->cylinder_head_temperature, efi_status->ignition_timing, efi_status->injection_time, efi_status->exhaust_gas_temperature, efi_status->throttle_out, efi_status->pt_compensation); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_EFI_STATUS, (const char *)efi_status, MAVLINK_MSG_ID_EFI_STATUS_MIN_LEN, MAVLINK_MSG_ID_EFI_STATUS_LEN, MAVLINK_MSG_ID_EFI_STATUS_CRC); +#endif +} + +#if MAVLINK_MSG_ID_EFI_STATUS_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_efi_status_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t health, float ecu_index, float rpm, float fuel_consumed, float fuel_flow, float engine_load, float throttle_position, float spark_dwell_time, float barometric_pressure, float intake_manifold_pressure, float intake_manifold_temperature, float cylinder_head_temperature, float ignition_timing, float injection_time, float exhaust_gas_temperature, float throttle_out, float pt_compensation) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_float(buf, 0, ecu_index); + _mav_put_float(buf, 4, rpm); + _mav_put_float(buf, 8, fuel_consumed); + _mav_put_float(buf, 12, fuel_flow); + _mav_put_float(buf, 16, engine_load); + _mav_put_float(buf, 20, throttle_position); + _mav_put_float(buf, 24, spark_dwell_time); + _mav_put_float(buf, 28, barometric_pressure); + _mav_put_float(buf, 32, intake_manifold_pressure); + _mav_put_float(buf, 36, intake_manifold_temperature); + _mav_put_float(buf, 40, cylinder_head_temperature); + _mav_put_float(buf, 44, ignition_timing); + _mav_put_float(buf, 48, injection_time); + _mav_put_float(buf, 52, exhaust_gas_temperature); + _mav_put_float(buf, 56, throttle_out); + _mav_put_float(buf, 60, pt_compensation); + _mav_put_uint8_t(buf, 64, health); + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_EFI_STATUS, buf, MAVLINK_MSG_ID_EFI_STATUS_MIN_LEN, MAVLINK_MSG_ID_EFI_STATUS_LEN, MAVLINK_MSG_ID_EFI_STATUS_CRC); +#else + mavlink_efi_status_t *packet = (mavlink_efi_status_t *)msgbuf; + packet->ecu_index = ecu_index; + packet->rpm = rpm; + packet->fuel_consumed = fuel_consumed; + packet->fuel_flow = fuel_flow; + packet->engine_load = engine_load; + packet->throttle_position = throttle_position; + packet->spark_dwell_time = spark_dwell_time; + packet->barometric_pressure = barometric_pressure; + packet->intake_manifold_pressure = intake_manifold_pressure; + packet->intake_manifold_temperature = intake_manifold_temperature; + packet->cylinder_head_temperature = cylinder_head_temperature; + packet->ignition_timing = ignition_timing; + packet->injection_time = injection_time; + packet->exhaust_gas_temperature = exhaust_gas_temperature; + packet->throttle_out = throttle_out; + packet->pt_compensation = pt_compensation; + packet->health = health; + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_EFI_STATUS, (const char *)packet, MAVLINK_MSG_ID_EFI_STATUS_MIN_LEN, MAVLINK_MSG_ID_EFI_STATUS_LEN, MAVLINK_MSG_ID_EFI_STATUS_CRC); +#endif +} +#endif + +#endif + +// MESSAGE EFI_STATUS UNPACKING + + +/** + * @brief Get field health from efi_status message + * + * @return EFI health status + */ +static inline uint8_t mavlink_msg_efi_status_get_health(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 64); +} + +/** + * @brief Get field ecu_index from efi_status message + * + * @return ECU index + */ +static inline float mavlink_msg_efi_status_get_ecu_index(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 0); +} + +/** + * @brief Get field rpm from efi_status message + * + * @return RPM + */ +static inline float mavlink_msg_efi_status_get_rpm(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 4); +} + +/** + * @brief Get field fuel_consumed from efi_status message + * + * @return [cm^3] Fuel consumed + */ +static inline float mavlink_msg_efi_status_get_fuel_consumed(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 8); +} + +/** + * @brief Get field fuel_flow from efi_status message + * + * @return [cm^3/min] Fuel flow rate + */ +static inline float mavlink_msg_efi_status_get_fuel_flow(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 12); +} + +/** + * @brief Get field engine_load from efi_status message + * + * @return [%] Engine load + */ +static inline float mavlink_msg_efi_status_get_engine_load(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 16); +} + +/** + * @brief Get field throttle_position from efi_status message + * + * @return [%] Throttle position + */ +static inline float mavlink_msg_efi_status_get_throttle_position(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 20); +} + +/** + * @brief Get field spark_dwell_time from efi_status message + * + * @return [ms] Spark dwell time + */ +static inline float mavlink_msg_efi_status_get_spark_dwell_time(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 24); +} + +/** + * @brief Get field barometric_pressure from efi_status message + * + * @return [kPa] Barometric pressure + */ +static inline float mavlink_msg_efi_status_get_barometric_pressure(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 28); +} + +/** + * @brief Get field intake_manifold_pressure from efi_status message + * + * @return [kPa] Intake manifold pressure( + */ +static inline float mavlink_msg_efi_status_get_intake_manifold_pressure(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 32); +} + +/** + * @brief Get field intake_manifold_temperature from efi_status message + * + * @return [degC] Intake manifold temperature + */ +static inline float mavlink_msg_efi_status_get_intake_manifold_temperature(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 36); +} + +/** + * @brief Get field cylinder_head_temperature from efi_status message + * + * @return [degC] Cylinder head temperature + */ +static inline float mavlink_msg_efi_status_get_cylinder_head_temperature(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 40); +} + +/** + * @brief Get field ignition_timing from efi_status message + * + * @return [deg] Ignition timing (Crank angle degrees) + */ +static inline float mavlink_msg_efi_status_get_ignition_timing(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 44); +} + +/** + * @brief Get field injection_time from efi_status message + * + * @return [ms] Injection time + */ +static inline float mavlink_msg_efi_status_get_injection_time(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 48); +} + +/** + * @brief Get field exhaust_gas_temperature from efi_status message + * + * @return [degC] Exhaust gas temperature + */ +static inline float mavlink_msg_efi_status_get_exhaust_gas_temperature(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 52); +} + +/** + * @brief Get field throttle_out from efi_status message + * + * @return [%] Output throttle + */ +static inline float mavlink_msg_efi_status_get_throttle_out(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 56); +} + +/** + * @brief Get field pt_compensation from efi_status message + * + * @return Pressure/temperature compensation + */ +static inline float mavlink_msg_efi_status_get_pt_compensation(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 60); +} + +/** + * @brief Decode a efi_status message into a struct + * + * @param msg The message to decode + * @param efi_status C-struct to decode the message contents into + */ +static inline void mavlink_msg_efi_status_decode(const mavlink_message_t* msg, mavlink_efi_status_t* efi_status) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + efi_status->ecu_index = mavlink_msg_efi_status_get_ecu_index(msg); + efi_status->rpm = mavlink_msg_efi_status_get_rpm(msg); + efi_status->fuel_consumed = mavlink_msg_efi_status_get_fuel_consumed(msg); + efi_status->fuel_flow = mavlink_msg_efi_status_get_fuel_flow(msg); + efi_status->engine_load = mavlink_msg_efi_status_get_engine_load(msg); + efi_status->throttle_position = mavlink_msg_efi_status_get_throttle_position(msg); + efi_status->spark_dwell_time = mavlink_msg_efi_status_get_spark_dwell_time(msg); + efi_status->barometric_pressure = mavlink_msg_efi_status_get_barometric_pressure(msg); + efi_status->intake_manifold_pressure = mavlink_msg_efi_status_get_intake_manifold_pressure(msg); + efi_status->intake_manifold_temperature = mavlink_msg_efi_status_get_intake_manifold_temperature(msg); + efi_status->cylinder_head_temperature = mavlink_msg_efi_status_get_cylinder_head_temperature(msg); + efi_status->ignition_timing = mavlink_msg_efi_status_get_ignition_timing(msg); + efi_status->injection_time = mavlink_msg_efi_status_get_injection_time(msg); + efi_status->exhaust_gas_temperature = mavlink_msg_efi_status_get_exhaust_gas_temperature(msg); + efi_status->throttle_out = mavlink_msg_efi_status_get_throttle_out(msg); + efi_status->pt_compensation = mavlink_msg_efi_status_get_pt_compensation(msg); + efi_status->health = mavlink_msg_efi_status_get_health(msg); +#else + uint8_t len = msg->len < MAVLINK_MSG_ID_EFI_STATUS_LEN? msg->len : MAVLINK_MSG_ID_EFI_STATUS_LEN; + memset(efi_status, 0, MAVLINK_MSG_ID_EFI_STATUS_LEN); + memcpy(efi_status, _MAV_PAYLOAD(msg), len); +#endif +} diff --git a/lib/main/MAVLink/common/mavlink_msg_esc_info.h b/lib/main/MAVLink/common/mavlink_msg_esc_info.h new file mode 100644 index 0000000000..d8a8bbcfe1 --- /dev/null +++ b/lib/main/MAVLink/common/mavlink_msg_esc_info.h @@ -0,0 +1,407 @@ +#pragma once +// MESSAGE ESC_INFO PACKING + +#define MAVLINK_MSG_ID_ESC_INFO 290 + + +typedef struct __mavlink_esc_info_t { + uint64_t time_usec; /*< [us] Timestamp (UNIX Epoch time or time since system boot). The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude the number.*/ + uint32_t error_count[4]; /*< Number of reported errors by each ESC since boot.*/ + uint16_t counter; /*< Counter of data packets received.*/ + uint16_t failure_flags[4]; /*< Bitmap of ESC failure flags.*/ + uint8_t index; /*< Index of the first ESC in this message. minValue = 0, maxValue = 60, increment = 4.*/ + uint8_t count; /*< Total number of ESCs in all messages of this type. Message fields with an index higher than this should be ignored because they contain invalid data.*/ + uint8_t connection_type; /*< Connection type protocol for all ESC.*/ + uint8_t info; /*< Information regarding online/offline status of each ESC.*/ + uint8_t temperature[4]; /*< [degC] Temperature measured by each ESC. UINT8_MAX if data not supplied by ESC.*/ +} mavlink_esc_info_t; + +#define MAVLINK_MSG_ID_ESC_INFO_LEN 42 +#define MAVLINK_MSG_ID_ESC_INFO_MIN_LEN 42 +#define MAVLINK_MSG_ID_290_LEN 42 +#define MAVLINK_MSG_ID_290_MIN_LEN 42 + +#define MAVLINK_MSG_ID_ESC_INFO_CRC 221 +#define MAVLINK_MSG_ID_290_CRC 221 + +#define MAVLINK_MSG_ESC_INFO_FIELD_ERROR_COUNT_LEN 4 +#define MAVLINK_MSG_ESC_INFO_FIELD_FAILURE_FLAGS_LEN 4 +#define MAVLINK_MSG_ESC_INFO_FIELD_TEMPERATURE_LEN 4 + +#if MAVLINK_COMMAND_24BIT +#define MAVLINK_MESSAGE_INFO_ESC_INFO { \ + 290, \ + "ESC_INFO", \ + 9, \ + { { "index", NULL, MAVLINK_TYPE_UINT8_T, 0, 34, offsetof(mavlink_esc_info_t, index) }, \ + { "time_usec", NULL, MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_esc_info_t, time_usec) }, \ + { "counter", NULL, MAVLINK_TYPE_UINT16_T, 0, 24, offsetof(mavlink_esc_info_t, counter) }, \ + { "count", NULL, MAVLINK_TYPE_UINT8_T, 0, 35, offsetof(mavlink_esc_info_t, count) }, \ + { "connection_type", NULL, MAVLINK_TYPE_UINT8_T, 0, 36, offsetof(mavlink_esc_info_t, connection_type) }, \ + { "info", NULL, MAVLINK_TYPE_UINT8_T, 0, 37, offsetof(mavlink_esc_info_t, info) }, \ + { "failure_flags", NULL, MAVLINK_TYPE_UINT16_T, 4, 26, offsetof(mavlink_esc_info_t, failure_flags) }, \ + { "error_count", NULL, MAVLINK_TYPE_UINT32_T, 4, 8, offsetof(mavlink_esc_info_t, error_count) }, \ + { "temperature", NULL, MAVLINK_TYPE_UINT8_T, 4, 38, offsetof(mavlink_esc_info_t, temperature) }, \ + } \ +} +#else +#define MAVLINK_MESSAGE_INFO_ESC_INFO { \ + "ESC_INFO", \ + 9, \ + { { "index", NULL, MAVLINK_TYPE_UINT8_T, 0, 34, offsetof(mavlink_esc_info_t, index) }, \ + { "time_usec", NULL, MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_esc_info_t, time_usec) }, \ + { "counter", NULL, MAVLINK_TYPE_UINT16_T, 0, 24, offsetof(mavlink_esc_info_t, counter) }, \ + { "count", NULL, MAVLINK_TYPE_UINT8_T, 0, 35, offsetof(mavlink_esc_info_t, count) }, \ + { "connection_type", NULL, MAVLINK_TYPE_UINT8_T, 0, 36, offsetof(mavlink_esc_info_t, connection_type) }, \ + { "info", NULL, MAVLINK_TYPE_UINT8_T, 0, 37, offsetof(mavlink_esc_info_t, info) }, \ + { "failure_flags", NULL, MAVLINK_TYPE_UINT16_T, 4, 26, offsetof(mavlink_esc_info_t, failure_flags) }, \ + { "error_count", NULL, MAVLINK_TYPE_UINT32_T, 4, 8, offsetof(mavlink_esc_info_t, error_count) }, \ + { "temperature", NULL, MAVLINK_TYPE_UINT8_T, 4, 38, offsetof(mavlink_esc_info_t, temperature) }, \ + } \ +} +#endif + +/** + * @brief Pack a esc_info message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param index Index of the first ESC in this message. minValue = 0, maxValue = 60, increment = 4. + * @param time_usec [us] Timestamp (UNIX Epoch time or time since system boot). The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude the number. + * @param counter Counter of data packets received. + * @param count Total number of ESCs in all messages of this type. Message fields with an index higher than this should be ignored because they contain invalid data. + * @param connection_type Connection type protocol for all ESC. + * @param info Information regarding online/offline status of each ESC. + * @param failure_flags Bitmap of ESC failure flags. + * @param error_count Number of reported errors by each ESC since boot. + * @param temperature [degC] Temperature measured by each ESC. UINT8_MAX if data not supplied by ESC. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_esc_info_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint8_t index, uint64_t time_usec, uint16_t counter, uint8_t count, uint8_t connection_type, uint8_t info, const uint16_t *failure_flags, const uint32_t *error_count, const uint8_t *temperature) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_ESC_INFO_LEN]; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_uint16_t(buf, 24, counter); + _mav_put_uint8_t(buf, 34, index); + _mav_put_uint8_t(buf, 35, count); + _mav_put_uint8_t(buf, 36, connection_type); + _mav_put_uint8_t(buf, 37, info); + _mav_put_uint32_t_array(buf, 8, error_count, 4); + _mav_put_uint16_t_array(buf, 26, failure_flags, 4); + _mav_put_uint8_t_array(buf, 38, temperature, 4); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_ESC_INFO_LEN); +#else + mavlink_esc_info_t packet; + packet.time_usec = time_usec; + packet.counter = counter; + packet.index = index; + packet.count = count; + packet.connection_type = connection_type; + packet.info = info; + mav_array_memcpy(packet.error_count, error_count, sizeof(uint32_t)*4); + mav_array_memcpy(packet.failure_flags, failure_flags, sizeof(uint16_t)*4); + mav_array_memcpy(packet.temperature, temperature, sizeof(uint8_t)*4); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_ESC_INFO_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_ESC_INFO; + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_ESC_INFO_MIN_LEN, MAVLINK_MSG_ID_ESC_INFO_LEN, MAVLINK_MSG_ID_ESC_INFO_CRC); +} + +/** + * @brief Pack a esc_info message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param index Index of the first ESC in this message. minValue = 0, maxValue = 60, increment = 4. + * @param time_usec [us] Timestamp (UNIX Epoch time or time since system boot). The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude the number. + * @param counter Counter of data packets received. + * @param count Total number of ESCs in all messages of this type. Message fields with an index higher than this should be ignored because they contain invalid data. + * @param connection_type Connection type protocol for all ESC. + * @param info Information regarding online/offline status of each ESC. + * @param failure_flags Bitmap of ESC failure flags. + * @param error_count Number of reported errors by each ESC since boot. + * @param temperature [degC] Temperature measured by each ESC. UINT8_MAX if data not supplied by ESC. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_esc_info_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint8_t index,uint64_t time_usec,uint16_t counter,uint8_t count,uint8_t connection_type,uint8_t info,const uint16_t *failure_flags,const uint32_t *error_count,const uint8_t *temperature) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_ESC_INFO_LEN]; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_uint16_t(buf, 24, counter); + _mav_put_uint8_t(buf, 34, index); + _mav_put_uint8_t(buf, 35, count); + _mav_put_uint8_t(buf, 36, connection_type); + _mav_put_uint8_t(buf, 37, info); + _mav_put_uint32_t_array(buf, 8, error_count, 4); + _mav_put_uint16_t_array(buf, 26, failure_flags, 4); + _mav_put_uint8_t_array(buf, 38, temperature, 4); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_ESC_INFO_LEN); +#else + mavlink_esc_info_t packet; + packet.time_usec = time_usec; + packet.counter = counter; + packet.index = index; + packet.count = count; + packet.connection_type = connection_type; + packet.info = info; + mav_array_memcpy(packet.error_count, error_count, sizeof(uint32_t)*4); + mav_array_memcpy(packet.failure_flags, failure_flags, sizeof(uint16_t)*4); + mav_array_memcpy(packet.temperature, temperature, sizeof(uint8_t)*4); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_ESC_INFO_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_ESC_INFO; + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_ESC_INFO_MIN_LEN, MAVLINK_MSG_ID_ESC_INFO_LEN, MAVLINK_MSG_ID_ESC_INFO_CRC); +} + +/** + * @brief Encode a esc_info struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param esc_info C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_esc_info_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_esc_info_t* esc_info) +{ + return mavlink_msg_esc_info_pack(system_id, component_id, msg, esc_info->index, esc_info->time_usec, esc_info->counter, esc_info->count, esc_info->connection_type, esc_info->info, esc_info->failure_flags, esc_info->error_count, esc_info->temperature); +} + +/** + * @brief Encode a esc_info struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param esc_info C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_esc_info_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_esc_info_t* esc_info) +{ + return mavlink_msg_esc_info_pack_chan(system_id, component_id, chan, msg, esc_info->index, esc_info->time_usec, esc_info->counter, esc_info->count, esc_info->connection_type, esc_info->info, esc_info->failure_flags, esc_info->error_count, esc_info->temperature); +} + +/** + * @brief Send a esc_info message + * @param chan MAVLink channel to send the message + * + * @param index Index of the first ESC in this message. minValue = 0, maxValue = 60, increment = 4. + * @param time_usec [us] Timestamp (UNIX Epoch time or time since system boot). The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude the number. + * @param counter Counter of data packets received. + * @param count Total number of ESCs in all messages of this type. Message fields with an index higher than this should be ignored because they contain invalid data. + * @param connection_type Connection type protocol for all ESC. + * @param info Information regarding online/offline status of each ESC. + * @param failure_flags Bitmap of ESC failure flags. + * @param error_count Number of reported errors by each ESC since boot. + * @param temperature [degC] Temperature measured by each ESC. UINT8_MAX if data not supplied by ESC. + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_esc_info_send(mavlink_channel_t chan, uint8_t index, uint64_t time_usec, uint16_t counter, uint8_t count, uint8_t connection_type, uint8_t info, const uint16_t *failure_flags, const uint32_t *error_count, const uint8_t *temperature) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_ESC_INFO_LEN]; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_uint16_t(buf, 24, counter); + _mav_put_uint8_t(buf, 34, index); + _mav_put_uint8_t(buf, 35, count); + _mav_put_uint8_t(buf, 36, connection_type); + _mav_put_uint8_t(buf, 37, info); + _mav_put_uint32_t_array(buf, 8, error_count, 4); + _mav_put_uint16_t_array(buf, 26, failure_flags, 4); + _mav_put_uint8_t_array(buf, 38, temperature, 4); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ESC_INFO, buf, MAVLINK_MSG_ID_ESC_INFO_MIN_LEN, MAVLINK_MSG_ID_ESC_INFO_LEN, MAVLINK_MSG_ID_ESC_INFO_CRC); +#else + mavlink_esc_info_t packet; + packet.time_usec = time_usec; + packet.counter = counter; + packet.index = index; + packet.count = count; + packet.connection_type = connection_type; + packet.info = info; + mav_array_memcpy(packet.error_count, error_count, sizeof(uint32_t)*4); + mav_array_memcpy(packet.failure_flags, failure_flags, sizeof(uint16_t)*4); + mav_array_memcpy(packet.temperature, temperature, sizeof(uint8_t)*4); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ESC_INFO, (const char *)&packet, MAVLINK_MSG_ID_ESC_INFO_MIN_LEN, MAVLINK_MSG_ID_ESC_INFO_LEN, MAVLINK_MSG_ID_ESC_INFO_CRC); +#endif +} + +/** + * @brief Send a esc_info message + * @param chan MAVLink channel to send the message + * @param struct The MAVLink struct to serialize + */ +static inline void mavlink_msg_esc_info_send_struct(mavlink_channel_t chan, const mavlink_esc_info_t* esc_info) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + mavlink_msg_esc_info_send(chan, esc_info->index, esc_info->time_usec, esc_info->counter, esc_info->count, esc_info->connection_type, esc_info->info, esc_info->failure_flags, esc_info->error_count, esc_info->temperature); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ESC_INFO, (const char *)esc_info, MAVLINK_MSG_ID_ESC_INFO_MIN_LEN, MAVLINK_MSG_ID_ESC_INFO_LEN, MAVLINK_MSG_ID_ESC_INFO_CRC); +#endif +} + +#if MAVLINK_MSG_ID_ESC_INFO_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_esc_info_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t index, uint64_t time_usec, uint16_t counter, uint8_t count, uint8_t connection_type, uint8_t info, const uint16_t *failure_flags, const uint32_t *error_count, const uint8_t *temperature) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_uint16_t(buf, 24, counter); + _mav_put_uint8_t(buf, 34, index); + _mav_put_uint8_t(buf, 35, count); + _mav_put_uint8_t(buf, 36, connection_type); + _mav_put_uint8_t(buf, 37, info); + _mav_put_uint32_t_array(buf, 8, error_count, 4); + _mav_put_uint16_t_array(buf, 26, failure_flags, 4); + _mav_put_uint8_t_array(buf, 38, temperature, 4); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ESC_INFO, buf, MAVLINK_MSG_ID_ESC_INFO_MIN_LEN, MAVLINK_MSG_ID_ESC_INFO_LEN, MAVLINK_MSG_ID_ESC_INFO_CRC); +#else + mavlink_esc_info_t *packet = (mavlink_esc_info_t *)msgbuf; + packet->time_usec = time_usec; + packet->counter = counter; + packet->index = index; + packet->count = count; + packet->connection_type = connection_type; + packet->info = info; + mav_array_memcpy(packet->error_count, error_count, sizeof(uint32_t)*4); + mav_array_memcpy(packet->failure_flags, failure_flags, sizeof(uint16_t)*4); + mav_array_memcpy(packet->temperature, temperature, sizeof(uint8_t)*4); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ESC_INFO, (const char *)packet, MAVLINK_MSG_ID_ESC_INFO_MIN_LEN, MAVLINK_MSG_ID_ESC_INFO_LEN, MAVLINK_MSG_ID_ESC_INFO_CRC); +#endif +} +#endif + +#endif + +// MESSAGE ESC_INFO UNPACKING + + +/** + * @brief Get field index from esc_info message + * + * @return Index of the first ESC in this message. minValue = 0, maxValue = 60, increment = 4. + */ +static inline uint8_t mavlink_msg_esc_info_get_index(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 34); +} + +/** + * @brief Get field time_usec from esc_info message + * + * @return [us] Timestamp (UNIX Epoch time or time since system boot). The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude the number. + */ +static inline uint64_t mavlink_msg_esc_info_get_time_usec(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint64_t(msg, 0); +} + +/** + * @brief Get field counter from esc_info message + * + * @return Counter of data packets received. + */ +static inline uint16_t mavlink_msg_esc_info_get_counter(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 24); +} + +/** + * @brief Get field count from esc_info message + * + * @return Total number of ESCs in all messages of this type. Message fields with an index higher than this should be ignored because they contain invalid data. + */ +static inline uint8_t mavlink_msg_esc_info_get_count(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 35); +} + +/** + * @brief Get field connection_type from esc_info message + * + * @return Connection type protocol for all ESC. + */ +static inline uint8_t mavlink_msg_esc_info_get_connection_type(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 36); +} + +/** + * @brief Get field info from esc_info message + * + * @return Information regarding online/offline status of each ESC. + */ +static inline uint8_t mavlink_msg_esc_info_get_info(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 37); +} + +/** + * @brief Get field failure_flags from esc_info message + * + * @return Bitmap of ESC failure flags. + */ +static inline uint16_t mavlink_msg_esc_info_get_failure_flags(const mavlink_message_t* msg, uint16_t *failure_flags) +{ + return _MAV_RETURN_uint16_t_array(msg, failure_flags, 4, 26); +} + +/** + * @brief Get field error_count from esc_info message + * + * @return Number of reported errors by each ESC since boot. + */ +static inline uint16_t mavlink_msg_esc_info_get_error_count(const mavlink_message_t* msg, uint32_t *error_count) +{ + return _MAV_RETURN_uint32_t_array(msg, error_count, 4, 8); +} + +/** + * @brief Get field temperature from esc_info message + * + * @return [degC] Temperature measured by each ESC. UINT8_MAX if data not supplied by ESC. + */ +static inline uint16_t mavlink_msg_esc_info_get_temperature(const mavlink_message_t* msg, uint8_t *temperature) +{ + return _MAV_RETURN_uint8_t_array(msg, temperature, 4, 38); +} + +/** + * @brief Decode a esc_info message into a struct + * + * @param msg The message to decode + * @param esc_info C-struct to decode the message contents into + */ +static inline void mavlink_msg_esc_info_decode(const mavlink_message_t* msg, mavlink_esc_info_t* esc_info) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + esc_info->time_usec = mavlink_msg_esc_info_get_time_usec(msg); + mavlink_msg_esc_info_get_error_count(msg, esc_info->error_count); + esc_info->counter = mavlink_msg_esc_info_get_counter(msg); + mavlink_msg_esc_info_get_failure_flags(msg, esc_info->failure_flags); + esc_info->index = mavlink_msg_esc_info_get_index(msg); + esc_info->count = mavlink_msg_esc_info_get_count(msg); + esc_info->connection_type = mavlink_msg_esc_info_get_connection_type(msg); + esc_info->info = mavlink_msg_esc_info_get_info(msg); + mavlink_msg_esc_info_get_temperature(msg, esc_info->temperature); +#else + uint8_t len = msg->len < MAVLINK_MSG_ID_ESC_INFO_LEN? msg->len : MAVLINK_MSG_ID_ESC_INFO_LEN; + memset(esc_info, 0, MAVLINK_MSG_ID_ESC_INFO_LEN); + memcpy(esc_info, _MAV_PAYLOAD(msg), len); +#endif +} diff --git a/lib/main/MAVLink/common/mavlink_msg_esc_status.h b/lib/main/MAVLink/common/mavlink_msg_esc_status.h new file mode 100644 index 0000000000..3a5fefe20a --- /dev/null +++ b/lib/main/MAVLink/common/mavlink_msg_esc_status.h @@ -0,0 +1,307 @@ +#pragma once +// MESSAGE ESC_STATUS PACKING + +#define MAVLINK_MSG_ID_ESC_STATUS 291 + + +typedef struct __mavlink_esc_status_t { + uint64_t time_usec; /*< [us] Timestamp (UNIX Epoch time or time since system boot). The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude the number.*/ + int32_t rpm[4]; /*< [rpm] Reported motor RPM from each ESC (negative for reverse rotation).*/ + float voltage[4]; /*< [V] Voltage measured from each ESC.*/ + float current[4]; /*< [A] Current measured from each ESC.*/ + uint8_t index; /*< Index of the first ESC in this message. minValue = 0, maxValue = 60, increment = 4.*/ +} mavlink_esc_status_t; + +#define MAVLINK_MSG_ID_ESC_STATUS_LEN 57 +#define MAVLINK_MSG_ID_ESC_STATUS_MIN_LEN 57 +#define MAVLINK_MSG_ID_291_LEN 57 +#define MAVLINK_MSG_ID_291_MIN_LEN 57 + +#define MAVLINK_MSG_ID_ESC_STATUS_CRC 10 +#define MAVLINK_MSG_ID_291_CRC 10 + +#define MAVLINK_MSG_ESC_STATUS_FIELD_RPM_LEN 4 +#define MAVLINK_MSG_ESC_STATUS_FIELD_VOLTAGE_LEN 4 +#define MAVLINK_MSG_ESC_STATUS_FIELD_CURRENT_LEN 4 + +#if MAVLINK_COMMAND_24BIT +#define MAVLINK_MESSAGE_INFO_ESC_STATUS { \ + 291, \ + "ESC_STATUS", \ + 5, \ + { { "index", NULL, MAVLINK_TYPE_UINT8_T, 0, 56, offsetof(mavlink_esc_status_t, index) }, \ + { "time_usec", NULL, MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_esc_status_t, time_usec) }, \ + { "rpm", NULL, MAVLINK_TYPE_INT32_T, 4, 8, offsetof(mavlink_esc_status_t, rpm) }, \ + { "voltage", NULL, MAVLINK_TYPE_FLOAT, 4, 24, offsetof(mavlink_esc_status_t, voltage) }, \ + { "current", NULL, MAVLINK_TYPE_FLOAT, 4, 40, offsetof(mavlink_esc_status_t, current) }, \ + } \ +} +#else +#define MAVLINK_MESSAGE_INFO_ESC_STATUS { \ + "ESC_STATUS", \ + 5, \ + { { "index", NULL, MAVLINK_TYPE_UINT8_T, 0, 56, offsetof(mavlink_esc_status_t, index) }, \ + { "time_usec", NULL, MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_esc_status_t, time_usec) }, \ + { "rpm", NULL, MAVLINK_TYPE_INT32_T, 4, 8, offsetof(mavlink_esc_status_t, rpm) }, \ + { "voltage", NULL, MAVLINK_TYPE_FLOAT, 4, 24, offsetof(mavlink_esc_status_t, voltage) }, \ + { "current", NULL, MAVLINK_TYPE_FLOAT, 4, 40, offsetof(mavlink_esc_status_t, current) }, \ + } \ +} +#endif + +/** + * @brief Pack a esc_status message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param index Index of the first ESC in this message. minValue = 0, maxValue = 60, increment = 4. + * @param time_usec [us] Timestamp (UNIX Epoch time or time since system boot). The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude the number. + * @param rpm [rpm] Reported motor RPM from each ESC (negative for reverse rotation). + * @param voltage [V] Voltage measured from each ESC. + * @param current [A] Current measured from each ESC. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_esc_status_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint8_t index, uint64_t time_usec, const int32_t *rpm, const float *voltage, const float *current) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_ESC_STATUS_LEN]; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_uint8_t(buf, 56, index); + _mav_put_int32_t_array(buf, 8, rpm, 4); + _mav_put_float_array(buf, 24, voltage, 4); + _mav_put_float_array(buf, 40, current, 4); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_ESC_STATUS_LEN); +#else + mavlink_esc_status_t packet; + packet.time_usec = time_usec; + packet.index = index; + mav_array_memcpy(packet.rpm, rpm, sizeof(int32_t)*4); + mav_array_memcpy(packet.voltage, voltage, sizeof(float)*4); + mav_array_memcpy(packet.current, current, sizeof(float)*4); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_ESC_STATUS_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_ESC_STATUS; + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_ESC_STATUS_MIN_LEN, MAVLINK_MSG_ID_ESC_STATUS_LEN, MAVLINK_MSG_ID_ESC_STATUS_CRC); +} + +/** + * @brief Pack a esc_status message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param index Index of the first ESC in this message. minValue = 0, maxValue = 60, increment = 4. + * @param time_usec [us] Timestamp (UNIX Epoch time or time since system boot). The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude the number. + * @param rpm [rpm] Reported motor RPM from each ESC (negative for reverse rotation). + * @param voltage [V] Voltage measured from each ESC. + * @param current [A] Current measured from each ESC. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_esc_status_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint8_t index,uint64_t time_usec,const int32_t *rpm,const float *voltage,const float *current) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_ESC_STATUS_LEN]; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_uint8_t(buf, 56, index); + _mav_put_int32_t_array(buf, 8, rpm, 4); + _mav_put_float_array(buf, 24, voltage, 4); + _mav_put_float_array(buf, 40, current, 4); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_ESC_STATUS_LEN); +#else + mavlink_esc_status_t packet; + packet.time_usec = time_usec; + packet.index = index; + mav_array_memcpy(packet.rpm, rpm, sizeof(int32_t)*4); + mav_array_memcpy(packet.voltage, voltage, sizeof(float)*4); + mav_array_memcpy(packet.current, current, sizeof(float)*4); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_ESC_STATUS_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_ESC_STATUS; + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_ESC_STATUS_MIN_LEN, MAVLINK_MSG_ID_ESC_STATUS_LEN, MAVLINK_MSG_ID_ESC_STATUS_CRC); +} + +/** + * @brief Encode a esc_status struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param esc_status C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_esc_status_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_esc_status_t* esc_status) +{ + return mavlink_msg_esc_status_pack(system_id, component_id, msg, esc_status->index, esc_status->time_usec, esc_status->rpm, esc_status->voltage, esc_status->current); +} + +/** + * @brief Encode a esc_status struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param esc_status C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_esc_status_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_esc_status_t* esc_status) +{ + return mavlink_msg_esc_status_pack_chan(system_id, component_id, chan, msg, esc_status->index, esc_status->time_usec, esc_status->rpm, esc_status->voltage, esc_status->current); +} + +/** + * @brief Send a esc_status message + * @param chan MAVLink channel to send the message + * + * @param index Index of the first ESC in this message. minValue = 0, maxValue = 60, increment = 4. + * @param time_usec [us] Timestamp (UNIX Epoch time or time since system boot). The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude the number. + * @param rpm [rpm] Reported motor RPM from each ESC (negative for reverse rotation). + * @param voltage [V] Voltage measured from each ESC. + * @param current [A] Current measured from each ESC. + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_esc_status_send(mavlink_channel_t chan, uint8_t index, uint64_t time_usec, const int32_t *rpm, const float *voltage, const float *current) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_ESC_STATUS_LEN]; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_uint8_t(buf, 56, index); + _mav_put_int32_t_array(buf, 8, rpm, 4); + _mav_put_float_array(buf, 24, voltage, 4); + _mav_put_float_array(buf, 40, current, 4); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ESC_STATUS, buf, MAVLINK_MSG_ID_ESC_STATUS_MIN_LEN, MAVLINK_MSG_ID_ESC_STATUS_LEN, MAVLINK_MSG_ID_ESC_STATUS_CRC); +#else + mavlink_esc_status_t packet; + packet.time_usec = time_usec; + packet.index = index; + mav_array_memcpy(packet.rpm, rpm, sizeof(int32_t)*4); + mav_array_memcpy(packet.voltage, voltage, sizeof(float)*4); + mav_array_memcpy(packet.current, current, sizeof(float)*4); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ESC_STATUS, (const char *)&packet, MAVLINK_MSG_ID_ESC_STATUS_MIN_LEN, MAVLINK_MSG_ID_ESC_STATUS_LEN, MAVLINK_MSG_ID_ESC_STATUS_CRC); +#endif +} + +/** + * @brief Send a esc_status message + * @param chan MAVLink channel to send the message + * @param struct The MAVLink struct to serialize + */ +static inline void mavlink_msg_esc_status_send_struct(mavlink_channel_t chan, const mavlink_esc_status_t* esc_status) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + mavlink_msg_esc_status_send(chan, esc_status->index, esc_status->time_usec, esc_status->rpm, esc_status->voltage, esc_status->current); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ESC_STATUS, (const char *)esc_status, MAVLINK_MSG_ID_ESC_STATUS_MIN_LEN, MAVLINK_MSG_ID_ESC_STATUS_LEN, MAVLINK_MSG_ID_ESC_STATUS_CRC); +#endif +} + +#if MAVLINK_MSG_ID_ESC_STATUS_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_esc_status_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t index, uint64_t time_usec, const int32_t *rpm, const float *voltage, const float *current) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_uint8_t(buf, 56, index); + _mav_put_int32_t_array(buf, 8, rpm, 4); + _mav_put_float_array(buf, 24, voltage, 4); + _mav_put_float_array(buf, 40, current, 4); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ESC_STATUS, buf, MAVLINK_MSG_ID_ESC_STATUS_MIN_LEN, MAVLINK_MSG_ID_ESC_STATUS_LEN, MAVLINK_MSG_ID_ESC_STATUS_CRC); +#else + mavlink_esc_status_t *packet = (mavlink_esc_status_t *)msgbuf; + packet->time_usec = time_usec; + packet->index = index; + mav_array_memcpy(packet->rpm, rpm, sizeof(int32_t)*4); + mav_array_memcpy(packet->voltage, voltage, sizeof(float)*4); + mav_array_memcpy(packet->current, current, sizeof(float)*4); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ESC_STATUS, (const char *)packet, MAVLINK_MSG_ID_ESC_STATUS_MIN_LEN, MAVLINK_MSG_ID_ESC_STATUS_LEN, MAVLINK_MSG_ID_ESC_STATUS_CRC); +#endif +} +#endif + +#endif + +// MESSAGE ESC_STATUS UNPACKING + + +/** + * @brief Get field index from esc_status message + * + * @return Index of the first ESC in this message. minValue = 0, maxValue = 60, increment = 4. + */ +static inline uint8_t mavlink_msg_esc_status_get_index(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 56); +} + +/** + * @brief Get field time_usec from esc_status message + * + * @return [us] Timestamp (UNIX Epoch time or time since system boot). The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude the number. + */ +static inline uint64_t mavlink_msg_esc_status_get_time_usec(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint64_t(msg, 0); +} + +/** + * @brief Get field rpm from esc_status message + * + * @return [rpm] Reported motor RPM from each ESC (negative for reverse rotation). + */ +static inline uint16_t mavlink_msg_esc_status_get_rpm(const mavlink_message_t* msg, int32_t *rpm) +{ + return _MAV_RETURN_int32_t_array(msg, rpm, 4, 8); +} + +/** + * @brief Get field voltage from esc_status message + * + * @return [V] Voltage measured from each ESC. + */ +static inline uint16_t mavlink_msg_esc_status_get_voltage(const mavlink_message_t* msg, float *voltage) +{ + return _MAV_RETURN_float_array(msg, voltage, 4, 24); +} + +/** + * @brief Get field current from esc_status message + * + * @return [A] Current measured from each ESC. + */ +static inline uint16_t mavlink_msg_esc_status_get_current(const mavlink_message_t* msg, float *current) +{ + return _MAV_RETURN_float_array(msg, current, 4, 40); +} + +/** + * @brief Decode a esc_status message into a struct + * + * @param msg The message to decode + * @param esc_status C-struct to decode the message contents into + */ +static inline void mavlink_msg_esc_status_decode(const mavlink_message_t* msg, mavlink_esc_status_t* esc_status) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + esc_status->time_usec = mavlink_msg_esc_status_get_time_usec(msg); + mavlink_msg_esc_status_get_rpm(msg, esc_status->rpm); + mavlink_msg_esc_status_get_voltage(msg, esc_status->voltage); + mavlink_msg_esc_status_get_current(msg, esc_status->current); + esc_status->index = mavlink_msg_esc_status_get_index(msg); +#else + uint8_t len = msg->len < MAVLINK_MSG_ID_ESC_STATUS_LEN? msg->len : MAVLINK_MSG_ID_ESC_STATUS_LEN; + memset(esc_status, 0, MAVLINK_MSG_ID_ESC_STATUS_LEN); + memcpy(esc_status, _MAV_PAYLOAD(msg), len); +#endif +} diff --git a/lib/main/MAVLink/common/mavlink_msg_fence_status.h b/lib/main/MAVLink/common/mavlink_msg_fence_status.h index d15f768d9b..08ba1555f2 100644 --- a/lib/main/MAVLink/common/mavlink_msg_fence_status.h +++ b/lib/main/MAVLink/common/mavlink_msg_fence_status.h @@ -9,11 +9,12 @@ typedef struct __mavlink_fence_status_t { uint16_t breach_count; /*< Number of fence breaches.*/ uint8_t breach_status; /*< Breach status (0 if currently inside fence, 1 if outside).*/ uint8_t breach_type; /*< Last breach type.*/ + uint8_t breach_mitigation; /*< Active action to prevent fence breach*/ } mavlink_fence_status_t; -#define MAVLINK_MSG_ID_FENCE_STATUS_LEN 8 +#define MAVLINK_MSG_ID_FENCE_STATUS_LEN 9 #define MAVLINK_MSG_ID_FENCE_STATUS_MIN_LEN 8 -#define MAVLINK_MSG_ID_162_LEN 8 +#define MAVLINK_MSG_ID_162_LEN 9 #define MAVLINK_MSG_ID_162_MIN_LEN 8 #define MAVLINK_MSG_ID_FENCE_STATUS_CRC 189 @@ -25,21 +26,23 @@ typedef struct __mavlink_fence_status_t { #define MAVLINK_MESSAGE_INFO_FENCE_STATUS { \ 162, \ "FENCE_STATUS", \ - 4, \ + 5, \ { { "breach_status", NULL, MAVLINK_TYPE_UINT8_T, 0, 6, offsetof(mavlink_fence_status_t, breach_status) }, \ { "breach_count", NULL, MAVLINK_TYPE_UINT16_T, 0, 4, offsetof(mavlink_fence_status_t, breach_count) }, \ { "breach_type", NULL, MAVLINK_TYPE_UINT8_T, 0, 7, offsetof(mavlink_fence_status_t, breach_type) }, \ { "breach_time", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_fence_status_t, breach_time) }, \ + { "breach_mitigation", NULL, MAVLINK_TYPE_UINT8_T, 0, 8, offsetof(mavlink_fence_status_t, breach_mitigation) }, \ } \ } #else #define MAVLINK_MESSAGE_INFO_FENCE_STATUS { \ "FENCE_STATUS", \ - 4, \ + 5, \ { { "breach_status", NULL, MAVLINK_TYPE_UINT8_T, 0, 6, offsetof(mavlink_fence_status_t, breach_status) }, \ { "breach_count", NULL, MAVLINK_TYPE_UINT16_T, 0, 4, offsetof(mavlink_fence_status_t, breach_count) }, \ { "breach_type", NULL, MAVLINK_TYPE_UINT8_T, 0, 7, offsetof(mavlink_fence_status_t, breach_type) }, \ { "breach_time", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_fence_status_t, breach_time) }, \ + { "breach_mitigation", NULL, MAVLINK_TYPE_UINT8_T, 0, 8, offsetof(mavlink_fence_status_t, breach_mitigation) }, \ } \ } #endif @@ -54,10 +57,11 @@ typedef struct __mavlink_fence_status_t { * @param breach_count Number of fence breaches. * @param breach_type Last breach type. * @param breach_time [ms] Time (since boot) of last breach. + * @param breach_mitigation Active action to prevent fence breach * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_fence_status_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint8_t breach_status, uint16_t breach_count, uint8_t breach_type, uint32_t breach_time) + uint8_t breach_status, uint16_t breach_count, uint8_t breach_type, uint32_t breach_time, uint8_t breach_mitigation) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_FENCE_STATUS_LEN]; @@ -65,6 +69,7 @@ static inline uint16_t mavlink_msg_fence_status_pack(uint8_t system_id, uint8_t _mav_put_uint16_t(buf, 4, breach_count); _mav_put_uint8_t(buf, 6, breach_status); _mav_put_uint8_t(buf, 7, breach_type); + _mav_put_uint8_t(buf, 8, breach_mitigation); memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_FENCE_STATUS_LEN); #else @@ -73,6 +78,7 @@ static inline uint16_t mavlink_msg_fence_status_pack(uint8_t system_id, uint8_t packet.breach_count = breach_count; packet.breach_status = breach_status; packet.breach_type = breach_type; + packet.breach_mitigation = breach_mitigation; memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_FENCE_STATUS_LEN); #endif @@ -91,11 +97,12 @@ static inline uint16_t mavlink_msg_fence_status_pack(uint8_t system_id, uint8_t * @param breach_count Number of fence breaches. * @param breach_type Last breach type. * @param breach_time [ms] Time (since boot) of last breach. + * @param breach_mitigation Active action to prevent fence breach * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_fence_status_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, - uint8_t breach_status,uint16_t breach_count,uint8_t breach_type,uint32_t breach_time) + uint8_t breach_status,uint16_t breach_count,uint8_t breach_type,uint32_t breach_time,uint8_t breach_mitigation) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_FENCE_STATUS_LEN]; @@ -103,6 +110,7 @@ static inline uint16_t mavlink_msg_fence_status_pack_chan(uint8_t system_id, uin _mav_put_uint16_t(buf, 4, breach_count); _mav_put_uint8_t(buf, 6, breach_status); _mav_put_uint8_t(buf, 7, breach_type); + _mav_put_uint8_t(buf, 8, breach_mitigation); memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_FENCE_STATUS_LEN); #else @@ -111,6 +119,7 @@ static inline uint16_t mavlink_msg_fence_status_pack_chan(uint8_t system_id, uin packet.breach_count = breach_count; packet.breach_status = breach_status; packet.breach_type = breach_type; + packet.breach_mitigation = breach_mitigation; memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_FENCE_STATUS_LEN); #endif @@ -129,7 +138,7 @@ static inline uint16_t mavlink_msg_fence_status_pack_chan(uint8_t system_id, uin */ static inline uint16_t mavlink_msg_fence_status_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_fence_status_t* fence_status) { - return mavlink_msg_fence_status_pack(system_id, component_id, msg, fence_status->breach_status, fence_status->breach_count, fence_status->breach_type, fence_status->breach_time); + return mavlink_msg_fence_status_pack(system_id, component_id, msg, fence_status->breach_status, fence_status->breach_count, fence_status->breach_type, fence_status->breach_time, fence_status->breach_mitigation); } /** @@ -143,7 +152,7 @@ static inline uint16_t mavlink_msg_fence_status_encode(uint8_t system_id, uint8_ */ static inline uint16_t mavlink_msg_fence_status_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_fence_status_t* fence_status) { - return mavlink_msg_fence_status_pack_chan(system_id, component_id, chan, msg, fence_status->breach_status, fence_status->breach_count, fence_status->breach_type, fence_status->breach_time); + return mavlink_msg_fence_status_pack_chan(system_id, component_id, chan, msg, fence_status->breach_status, fence_status->breach_count, fence_status->breach_type, fence_status->breach_time, fence_status->breach_mitigation); } /** @@ -154,10 +163,11 @@ static inline uint16_t mavlink_msg_fence_status_encode_chan(uint8_t system_id, u * @param breach_count Number of fence breaches. * @param breach_type Last breach type. * @param breach_time [ms] Time (since boot) of last breach. + * @param breach_mitigation Active action to prevent fence breach */ #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS -static inline void mavlink_msg_fence_status_send(mavlink_channel_t chan, uint8_t breach_status, uint16_t breach_count, uint8_t breach_type, uint32_t breach_time) +static inline void mavlink_msg_fence_status_send(mavlink_channel_t chan, uint8_t breach_status, uint16_t breach_count, uint8_t breach_type, uint32_t breach_time, uint8_t breach_mitigation) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_FENCE_STATUS_LEN]; @@ -165,6 +175,7 @@ static inline void mavlink_msg_fence_status_send(mavlink_channel_t chan, uint8_t _mav_put_uint16_t(buf, 4, breach_count); _mav_put_uint8_t(buf, 6, breach_status); _mav_put_uint8_t(buf, 7, breach_type); + _mav_put_uint8_t(buf, 8, breach_mitigation); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FENCE_STATUS, buf, MAVLINK_MSG_ID_FENCE_STATUS_MIN_LEN, MAVLINK_MSG_ID_FENCE_STATUS_LEN, MAVLINK_MSG_ID_FENCE_STATUS_CRC); #else @@ -173,6 +184,7 @@ static inline void mavlink_msg_fence_status_send(mavlink_channel_t chan, uint8_t packet.breach_count = breach_count; packet.breach_status = breach_status; packet.breach_type = breach_type; + packet.breach_mitigation = breach_mitigation; _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FENCE_STATUS, (const char *)&packet, MAVLINK_MSG_ID_FENCE_STATUS_MIN_LEN, MAVLINK_MSG_ID_FENCE_STATUS_LEN, MAVLINK_MSG_ID_FENCE_STATUS_CRC); #endif @@ -186,7 +198,7 @@ static inline void mavlink_msg_fence_status_send(mavlink_channel_t chan, uint8_t static inline void mavlink_msg_fence_status_send_struct(mavlink_channel_t chan, const mavlink_fence_status_t* fence_status) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - mavlink_msg_fence_status_send(chan, fence_status->breach_status, fence_status->breach_count, fence_status->breach_type, fence_status->breach_time); + mavlink_msg_fence_status_send(chan, fence_status->breach_status, fence_status->breach_count, fence_status->breach_type, fence_status->breach_time, fence_status->breach_mitigation); #else _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FENCE_STATUS, (const char *)fence_status, MAVLINK_MSG_ID_FENCE_STATUS_MIN_LEN, MAVLINK_MSG_ID_FENCE_STATUS_LEN, MAVLINK_MSG_ID_FENCE_STATUS_CRC); #endif @@ -200,7 +212,7 @@ static inline void mavlink_msg_fence_status_send_struct(mavlink_channel_t chan, is usually the receive buffer for the channel, and allows a reply to an incoming message with minimum stack space usage. */ -static inline void mavlink_msg_fence_status_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t breach_status, uint16_t breach_count, uint8_t breach_type, uint32_t breach_time) +static inline void mavlink_msg_fence_status_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t breach_status, uint16_t breach_count, uint8_t breach_type, uint32_t breach_time, uint8_t breach_mitigation) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char *buf = (char *)msgbuf; @@ -208,6 +220,7 @@ static inline void mavlink_msg_fence_status_send_buf(mavlink_message_t *msgbuf, _mav_put_uint16_t(buf, 4, breach_count); _mav_put_uint8_t(buf, 6, breach_status); _mav_put_uint8_t(buf, 7, breach_type); + _mav_put_uint8_t(buf, 8, breach_mitigation); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FENCE_STATUS, buf, MAVLINK_MSG_ID_FENCE_STATUS_MIN_LEN, MAVLINK_MSG_ID_FENCE_STATUS_LEN, MAVLINK_MSG_ID_FENCE_STATUS_CRC); #else @@ -216,6 +229,7 @@ static inline void mavlink_msg_fence_status_send_buf(mavlink_message_t *msgbuf, packet->breach_count = breach_count; packet->breach_status = breach_status; packet->breach_type = breach_type; + packet->breach_mitigation = breach_mitigation; _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FENCE_STATUS, (const char *)packet, MAVLINK_MSG_ID_FENCE_STATUS_MIN_LEN, MAVLINK_MSG_ID_FENCE_STATUS_LEN, MAVLINK_MSG_ID_FENCE_STATUS_CRC); #endif @@ -267,6 +281,16 @@ static inline uint32_t mavlink_msg_fence_status_get_breach_time(const mavlink_me return _MAV_RETURN_uint32_t(msg, 0); } +/** + * @brief Get field breach_mitigation from fence_status message + * + * @return Active action to prevent fence breach + */ +static inline uint8_t mavlink_msg_fence_status_get_breach_mitigation(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 8); +} + /** * @brief Decode a fence_status message into a struct * @@ -280,6 +304,7 @@ static inline void mavlink_msg_fence_status_decode(const mavlink_message_t* msg, fence_status->breach_count = mavlink_msg_fence_status_get_breach_count(msg); fence_status->breach_status = mavlink_msg_fence_status_get_breach_status(msg); fence_status->breach_type = mavlink_msg_fence_status_get_breach_type(msg); + fence_status->breach_mitigation = mavlink_msg_fence_status_get_breach_mitigation(msg); #else uint8_t len = msg->len < MAVLINK_MSG_ID_FENCE_STATUS_LEN? msg->len : MAVLINK_MSG_ID_FENCE_STATUS_LEN; memset(fence_status, 0, MAVLINK_MSG_ID_FENCE_STATUS_LEN); diff --git a/lib/main/MAVLink/common/mavlink_msg_flight_information.h b/lib/main/MAVLink/common/mavlink_msg_flight_information.h new file mode 100644 index 0000000000..78513ff593 --- /dev/null +++ b/lib/main/MAVLink/common/mavlink_msg_flight_information.h @@ -0,0 +1,288 @@ +#pragma once +// MESSAGE FLIGHT_INFORMATION PACKING + +#define MAVLINK_MSG_ID_FLIGHT_INFORMATION 264 + + +typedef struct __mavlink_flight_information_t { + uint64_t arming_time_utc; /*< [us] Timestamp at arming (time since UNIX epoch) in UTC, 0 for unknown*/ + uint64_t takeoff_time_utc; /*< [us] Timestamp at takeoff (time since UNIX epoch) in UTC, 0 for unknown*/ + uint64_t flight_uuid; /*< Universally unique identifier (UUID) of flight, should correspond to name of log files*/ + uint32_t time_boot_ms; /*< [ms] Timestamp (time since system boot).*/ +} mavlink_flight_information_t; + +#define MAVLINK_MSG_ID_FLIGHT_INFORMATION_LEN 28 +#define MAVLINK_MSG_ID_FLIGHT_INFORMATION_MIN_LEN 28 +#define MAVLINK_MSG_ID_264_LEN 28 +#define MAVLINK_MSG_ID_264_MIN_LEN 28 + +#define MAVLINK_MSG_ID_FLIGHT_INFORMATION_CRC 49 +#define MAVLINK_MSG_ID_264_CRC 49 + + + +#if MAVLINK_COMMAND_24BIT +#define MAVLINK_MESSAGE_INFO_FLIGHT_INFORMATION { \ + 264, \ + "FLIGHT_INFORMATION", \ + 4, \ + { { "time_boot_ms", NULL, MAVLINK_TYPE_UINT32_T, 0, 24, offsetof(mavlink_flight_information_t, time_boot_ms) }, \ + { "arming_time_utc", NULL, MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_flight_information_t, arming_time_utc) }, \ + { "takeoff_time_utc", NULL, MAVLINK_TYPE_UINT64_T, 0, 8, offsetof(mavlink_flight_information_t, takeoff_time_utc) }, \ + { "flight_uuid", NULL, MAVLINK_TYPE_UINT64_T, 0, 16, offsetof(mavlink_flight_information_t, flight_uuid) }, \ + } \ +} +#else +#define MAVLINK_MESSAGE_INFO_FLIGHT_INFORMATION { \ + "FLIGHT_INFORMATION", \ + 4, \ + { { "time_boot_ms", NULL, MAVLINK_TYPE_UINT32_T, 0, 24, offsetof(mavlink_flight_information_t, time_boot_ms) }, \ + { "arming_time_utc", NULL, MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_flight_information_t, arming_time_utc) }, \ + { "takeoff_time_utc", NULL, MAVLINK_TYPE_UINT64_T, 0, 8, offsetof(mavlink_flight_information_t, takeoff_time_utc) }, \ + { "flight_uuid", NULL, MAVLINK_TYPE_UINT64_T, 0, 16, offsetof(mavlink_flight_information_t, flight_uuid) }, \ + } \ +} +#endif + +/** + * @brief Pack a flight_information message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param time_boot_ms [ms] Timestamp (time since system boot). + * @param arming_time_utc [us] Timestamp at arming (time since UNIX epoch) in UTC, 0 for unknown + * @param takeoff_time_utc [us] Timestamp at takeoff (time since UNIX epoch) in UTC, 0 for unknown + * @param flight_uuid Universally unique identifier (UUID) of flight, should correspond to name of log files + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_flight_information_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint32_t time_boot_ms, uint64_t arming_time_utc, uint64_t takeoff_time_utc, uint64_t flight_uuid) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_FLIGHT_INFORMATION_LEN]; + _mav_put_uint64_t(buf, 0, arming_time_utc); + _mav_put_uint64_t(buf, 8, takeoff_time_utc); + _mav_put_uint64_t(buf, 16, flight_uuid); + _mav_put_uint32_t(buf, 24, time_boot_ms); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_FLIGHT_INFORMATION_LEN); +#else + mavlink_flight_information_t packet; + packet.arming_time_utc = arming_time_utc; + packet.takeoff_time_utc = takeoff_time_utc; + packet.flight_uuid = flight_uuid; + packet.time_boot_ms = time_boot_ms; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_FLIGHT_INFORMATION_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_FLIGHT_INFORMATION; + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_FLIGHT_INFORMATION_MIN_LEN, MAVLINK_MSG_ID_FLIGHT_INFORMATION_LEN, MAVLINK_MSG_ID_FLIGHT_INFORMATION_CRC); +} + +/** + * @brief Pack a flight_information message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param time_boot_ms [ms] Timestamp (time since system boot). + * @param arming_time_utc [us] Timestamp at arming (time since UNIX epoch) in UTC, 0 for unknown + * @param takeoff_time_utc [us] Timestamp at takeoff (time since UNIX epoch) in UTC, 0 for unknown + * @param flight_uuid Universally unique identifier (UUID) of flight, should correspond to name of log files + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_flight_information_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint32_t time_boot_ms,uint64_t arming_time_utc,uint64_t takeoff_time_utc,uint64_t flight_uuid) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_FLIGHT_INFORMATION_LEN]; + _mav_put_uint64_t(buf, 0, arming_time_utc); + _mav_put_uint64_t(buf, 8, takeoff_time_utc); + _mav_put_uint64_t(buf, 16, flight_uuid); + _mav_put_uint32_t(buf, 24, time_boot_ms); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_FLIGHT_INFORMATION_LEN); +#else + mavlink_flight_information_t packet; + packet.arming_time_utc = arming_time_utc; + packet.takeoff_time_utc = takeoff_time_utc; + packet.flight_uuid = flight_uuid; + packet.time_boot_ms = time_boot_ms; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_FLIGHT_INFORMATION_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_FLIGHT_INFORMATION; + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_FLIGHT_INFORMATION_MIN_LEN, MAVLINK_MSG_ID_FLIGHT_INFORMATION_LEN, MAVLINK_MSG_ID_FLIGHT_INFORMATION_CRC); +} + +/** + * @brief Encode a flight_information struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param flight_information C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_flight_information_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_flight_information_t* flight_information) +{ + return mavlink_msg_flight_information_pack(system_id, component_id, msg, flight_information->time_boot_ms, flight_information->arming_time_utc, flight_information->takeoff_time_utc, flight_information->flight_uuid); +} + +/** + * @brief Encode a flight_information struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param flight_information C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_flight_information_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_flight_information_t* flight_information) +{ + return mavlink_msg_flight_information_pack_chan(system_id, component_id, chan, msg, flight_information->time_boot_ms, flight_information->arming_time_utc, flight_information->takeoff_time_utc, flight_information->flight_uuid); +} + +/** + * @brief Send a flight_information message + * @param chan MAVLink channel to send the message + * + * @param time_boot_ms [ms] Timestamp (time since system boot). + * @param arming_time_utc [us] Timestamp at arming (time since UNIX epoch) in UTC, 0 for unknown + * @param takeoff_time_utc [us] Timestamp at takeoff (time since UNIX epoch) in UTC, 0 for unknown + * @param flight_uuid Universally unique identifier (UUID) of flight, should correspond to name of log files + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_flight_information_send(mavlink_channel_t chan, uint32_t time_boot_ms, uint64_t arming_time_utc, uint64_t takeoff_time_utc, uint64_t flight_uuid) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_FLIGHT_INFORMATION_LEN]; + _mav_put_uint64_t(buf, 0, arming_time_utc); + _mav_put_uint64_t(buf, 8, takeoff_time_utc); + _mav_put_uint64_t(buf, 16, flight_uuid); + _mav_put_uint32_t(buf, 24, time_boot_ms); + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FLIGHT_INFORMATION, buf, MAVLINK_MSG_ID_FLIGHT_INFORMATION_MIN_LEN, MAVLINK_MSG_ID_FLIGHT_INFORMATION_LEN, MAVLINK_MSG_ID_FLIGHT_INFORMATION_CRC); +#else + mavlink_flight_information_t packet; + packet.arming_time_utc = arming_time_utc; + packet.takeoff_time_utc = takeoff_time_utc; + packet.flight_uuid = flight_uuid; + packet.time_boot_ms = time_boot_ms; + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FLIGHT_INFORMATION, (const char *)&packet, MAVLINK_MSG_ID_FLIGHT_INFORMATION_MIN_LEN, MAVLINK_MSG_ID_FLIGHT_INFORMATION_LEN, MAVLINK_MSG_ID_FLIGHT_INFORMATION_CRC); +#endif +} + +/** + * @brief Send a flight_information message + * @param chan MAVLink channel to send the message + * @param struct The MAVLink struct to serialize + */ +static inline void mavlink_msg_flight_information_send_struct(mavlink_channel_t chan, const mavlink_flight_information_t* flight_information) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + mavlink_msg_flight_information_send(chan, flight_information->time_boot_ms, flight_information->arming_time_utc, flight_information->takeoff_time_utc, flight_information->flight_uuid); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FLIGHT_INFORMATION, (const char *)flight_information, MAVLINK_MSG_ID_FLIGHT_INFORMATION_MIN_LEN, MAVLINK_MSG_ID_FLIGHT_INFORMATION_LEN, MAVLINK_MSG_ID_FLIGHT_INFORMATION_CRC); +#endif +} + +#if MAVLINK_MSG_ID_FLIGHT_INFORMATION_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_flight_information_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint32_t time_boot_ms, uint64_t arming_time_utc, uint64_t takeoff_time_utc, uint64_t flight_uuid) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint64_t(buf, 0, arming_time_utc); + _mav_put_uint64_t(buf, 8, takeoff_time_utc); + _mav_put_uint64_t(buf, 16, flight_uuid); + _mav_put_uint32_t(buf, 24, time_boot_ms); + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FLIGHT_INFORMATION, buf, MAVLINK_MSG_ID_FLIGHT_INFORMATION_MIN_LEN, MAVLINK_MSG_ID_FLIGHT_INFORMATION_LEN, MAVLINK_MSG_ID_FLIGHT_INFORMATION_CRC); +#else + mavlink_flight_information_t *packet = (mavlink_flight_information_t *)msgbuf; + packet->arming_time_utc = arming_time_utc; + packet->takeoff_time_utc = takeoff_time_utc; + packet->flight_uuid = flight_uuid; + packet->time_boot_ms = time_boot_ms; + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_FLIGHT_INFORMATION, (const char *)packet, MAVLINK_MSG_ID_FLIGHT_INFORMATION_MIN_LEN, MAVLINK_MSG_ID_FLIGHT_INFORMATION_LEN, MAVLINK_MSG_ID_FLIGHT_INFORMATION_CRC); +#endif +} +#endif + +#endif + +// MESSAGE FLIGHT_INFORMATION UNPACKING + + +/** + * @brief Get field time_boot_ms from flight_information message + * + * @return [ms] Timestamp (time since system boot). + */ +static inline uint32_t mavlink_msg_flight_information_get_time_boot_ms(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint32_t(msg, 24); +} + +/** + * @brief Get field arming_time_utc from flight_information message + * + * @return [us] Timestamp at arming (time since UNIX epoch) in UTC, 0 for unknown + */ +static inline uint64_t mavlink_msg_flight_information_get_arming_time_utc(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint64_t(msg, 0); +} + +/** + * @brief Get field takeoff_time_utc from flight_information message + * + * @return [us] Timestamp at takeoff (time since UNIX epoch) in UTC, 0 for unknown + */ +static inline uint64_t mavlink_msg_flight_information_get_takeoff_time_utc(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint64_t(msg, 8); +} + +/** + * @brief Get field flight_uuid from flight_information message + * + * @return Universally unique identifier (UUID) of flight, should correspond to name of log files + */ +static inline uint64_t mavlink_msg_flight_information_get_flight_uuid(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint64_t(msg, 16); +} + +/** + * @brief Decode a flight_information message into a struct + * + * @param msg The message to decode + * @param flight_information C-struct to decode the message contents into + */ +static inline void mavlink_msg_flight_information_decode(const mavlink_message_t* msg, mavlink_flight_information_t* flight_information) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + flight_information->arming_time_utc = mavlink_msg_flight_information_get_arming_time_utc(msg); + flight_information->takeoff_time_utc = mavlink_msg_flight_information_get_takeoff_time_utc(msg); + flight_information->flight_uuid = mavlink_msg_flight_information_get_flight_uuid(msg); + flight_information->time_boot_ms = mavlink_msg_flight_information_get_time_boot_ms(msg); +#else + uint8_t len = msg->len < MAVLINK_MSG_ID_FLIGHT_INFORMATION_LEN? msg->len : MAVLINK_MSG_ID_FLIGHT_INFORMATION_LEN; + memset(flight_information, 0, MAVLINK_MSG_ID_FLIGHT_INFORMATION_LEN); + memcpy(flight_information, _MAV_PAYLOAD(msg), len); +#endif +} diff --git a/lib/main/MAVLink/common/mavlink_msg_generator_status.h b/lib/main/MAVLink/common/mavlink_msg_generator_status.h new file mode 100644 index 0000000000..ef960e13cf --- /dev/null +++ b/lib/main/MAVLink/common/mavlink_msg_generator_status.h @@ -0,0 +1,463 @@ +#pragma once +// MESSAGE GENERATOR_STATUS PACKING + +#define MAVLINK_MSG_ID_GENERATOR_STATUS 373 + + +typedef struct __mavlink_generator_status_t { + uint64_t status; /*< Status flags.*/ + float battery_current; /*< [A] Current into/out of battery. Positive for out. Negative for in. NaN: field not provided.*/ + float load_current; /*< [A] Current going to the UAV. If battery current not available this is the DC current from the generator. Positive for out. Negative for in. NaN: field not provided*/ + float power_generated; /*< [W] The power being generated. NaN: field not provided*/ + float bus_voltage; /*< [V] Voltage of the bus seen at the generator, or battery bus if battery bus is controlled by generator and at a different voltage to main bus.*/ + float bat_current_setpoint; /*< [A] The target battery current. Positive for out. Negative for in. NaN: field not provided*/ + uint32_t runtime; /*< [s] Seconds this generator has run since it was rebooted. UINT32_MAX: field not provided.*/ + int32_t time_until_maintenance; /*< [s] Seconds until this generator requires maintenance. A negative value indicates maintenance is past-due. INT32_MAX: field not provided.*/ + uint16_t generator_speed; /*< [rpm] Speed of electrical generator or alternator. UINT16_MAX: field not provided.*/ + int16_t rectifier_temperature; /*< [degC] The temperature of the rectifier or power converter. INT16_MAX: field not provided.*/ + int16_t generator_temperature; /*< [degC] The temperature of the mechanical motor, fuel cell core or generator. INT16_MAX: field not provided.*/ +} mavlink_generator_status_t; + +#define MAVLINK_MSG_ID_GENERATOR_STATUS_LEN 42 +#define MAVLINK_MSG_ID_GENERATOR_STATUS_MIN_LEN 42 +#define MAVLINK_MSG_ID_373_LEN 42 +#define MAVLINK_MSG_ID_373_MIN_LEN 42 + +#define MAVLINK_MSG_ID_GENERATOR_STATUS_CRC 117 +#define MAVLINK_MSG_ID_373_CRC 117 + + + +#if MAVLINK_COMMAND_24BIT +#define MAVLINK_MESSAGE_INFO_GENERATOR_STATUS { \ + 373, \ + "GENERATOR_STATUS", \ + 11, \ + { { "status", NULL, MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_generator_status_t, status) }, \ + { "generator_speed", NULL, MAVLINK_TYPE_UINT16_T, 0, 36, offsetof(mavlink_generator_status_t, generator_speed) }, \ + { "battery_current", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_generator_status_t, battery_current) }, \ + { "load_current", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_generator_status_t, load_current) }, \ + { "power_generated", NULL, MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_generator_status_t, power_generated) }, \ + { "bus_voltage", NULL, MAVLINK_TYPE_FLOAT, 0, 20, offsetof(mavlink_generator_status_t, bus_voltage) }, \ + { "rectifier_temperature", NULL, MAVLINK_TYPE_INT16_T, 0, 38, offsetof(mavlink_generator_status_t, rectifier_temperature) }, \ + { "bat_current_setpoint", NULL, MAVLINK_TYPE_FLOAT, 0, 24, offsetof(mavlink_generator_status_t, bat_current_setpoint) }, \ + { "generator_temperature", NULL, MAVLINK_TYPE_INT16_T, 0, 40, offsetof(mavlink_generator_status_t, generator_temperature) }, \ + { "runtime", NULL, MAVLINK_TYPE_UINT32_T, 0, 28, offsetof(mavlink_generator_status_t, runtime) }, \ + { "time_until_maintenance", NULL, MAVLINK_TYPE_INT32_T, 0, 32, offsetof(mavlink_generator_status_t, time_until_maintenance) }, \ + } \ +} +#else +#define MAVLINK_MESSAGE_INFO_GENERATOR_STATUS { \ + "GENERATOR_STATUS", \ + 11, \ + { { "status", NULL, MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_generator_status_t, status) }, \ + { "generator_speed", NULL, MAVLINK_TYPE_UINT16_T, 0, 36, offsetof(mavlink_generator_status_t, generator_speed) }, \ + { "battery_current", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_generator_status_t, battery_current) }, \ + { "load_current", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_generator_status_t, load_current) }, \ + { "power_generated", NULL, MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_generator_status_t, power_generated) }, \ + { "bus_voltage", NULL, MAVLINK_TYPE_FLOAT, 0, 20, offsetof(mavlink_generator_status_t, bus_voltage) }, \ + { "rectifier_temperature", NULL, MAVLINK_TYPE_INT16_T, 0, 38, offsetof(mavlink_generator_status_t, rectifier_temperature) }, \ + { "bat_current_setpoint", NULL, MAVLINK_TYPE_FLOAT, 0, 24, offsetof(mavlink_generator_status_t, bat_current_setpoint) }, \ + { "generator_temperature", NULL, MAVLINK_TYPE_INT16_T, 0, 40, offsetof(mavlink_generator_status_t, generator_temperature) }, \ + { "runtime", NULL, MAVLINK_TYPE_UINT32_T, 0, 28, offsetof(mavlink_generator_status_t, runtime) }, \ + { "time_until_maintenance", NULL, MAVLINK_TYPE_INT32_T, 0, 32, offsetof(mavlink_generator_status_t, time_until_maintenance) }, \ + } \ +} +#endif + +/** + * @brief Pack a generator_status message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param status Status flags. + * @param generator_speed [rpm] Speed of electrical generator or alternator. UINT16_MAX: field not provided. + * @param battery_current [A] Current into/out of battery. Positive for out. Negative for in. NaN: field not provided. + * @param load_current [A] Current going to the UAV. If battery current not available this is the DC current from the generator. Positive for out. Negative for in. NaN: field not provided + * @param power_generated [W] The power being generated. NaN: field not provided + * @param bus_voltage [V] Voltage of the bus seen at the generator, or battery bus if battery bus is controlled by generator and at a different voltage to main bus. + * @param rectifier_temperature [degC] The temperature of the rectifier or power converter. INT16_MAX: field not provided. + * @param bat_current_setpoint [A] The target battery current. Positive for out. Negative for in. NaN: field not provided + * @param generator_temperature [degC] The temperature of the mechanical motor, fuel cell core or generator. INT16_MAX: field not provided. + * @param runtime [s] Seconds this generator has run since it was rebooted. UINT32_MAX: field not provided. + * @param time_until_maintenance [s] Seconds until this generator requires maintenance. A negative value indicates maintenance is past-due. INT32_MAX: field not provided. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_generator_status_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint64_t status, uint16_t generator_speed, float battery_current, float load_current, float power_generated, float bus_voltage, int16_t rectifier_temperature, float bat_current_setpoint, int16_t generator_temperature, uint32_t runtime, int32_t time_until_maintenance) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_GENERATOR_STATUS_LEN]; + _mav_put_uint64_t(buf, 0, status); + _mav_put_float(buf, 8, battery_current); + _mav_put_float(buf, 12, load_current); + _mav_put_float(buf, 16, power_generated); + _mav_put_float(buf, 20, bus_voltage); + _mav_put_float(buf, 24, bat_current_setpoint); + _mav_put_uint32_t(buf, 28, runtime); + _mav_put_int32_t(buf, 32, time_until_maintenance); + _mav_put_uint16_t(buf, 36, generator_speed); + _mav_put_int16_t(buf, 38, rectifier_temperature); + _mav_put_int16_t(buf, 40, generator_temperature); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_GENERATOR_STATUS_LEN); +#else + mavlink_generator_status_t packet; + packet.status = status; + packet.battery_current = battery_current; + packet.load_current = load_current; + packet.power_generated = power_generated; + packet.bus_voltage = bus_voltage; + packet.bat_current_setpoint = bat_current_setpoint; + packet.runtime = runtime; + packet.time_until_maintenance = time_until_maintenance; + packet.generator_speed = generator_speed; + packet.rectifier_temperature = rectifier_temperature; + packet.generator_temperature = generator_temperature; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_GENERATOR_STATUS_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_GENERATOR_STATUS; + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_GENERATOR_STATUS_MIN_LEN, MAVLINK_MSG_ID_GENERATOR_STATUS_LEN, MAVLINK_MSG_ID_GENERATOR_STATUS_CRC); +} + +/** + * @brief Pack a generator_status message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param status Status flags. + * @param generator_speed [rpm] Speed of electrical generator or alternator. UINT16_MAX: field not provided. + * @param battery_current [A] Current into/out of battery. Positive for out. Negative for in. NaN: field not provided. + * @param load_current [A] Current going to the UAV. If battery current not available this is the DC current from the generator. Positive for out. Negative for in. NaN: field not provided + * @param power_generated [W] The power being generated. NaN: field not provided + * @param bus_voltage [V] Voltage of the bus seen at the generator, or battery bus if battery bus is controlled by generator and at a different voltage to main bus. + * @param rectifier_temperature [degC] The temperature of the rectifier or power converter. INT16_MAX: field not provided. + * @param bat_current_setpoint [A] The target battery current. Positive for out. Negative for in. NaN: field not provided + * @param generator_temperature [degC] The temperature of the mechanical motor, fuel cell core or generator. INT16_MAX: field not provided. + * @param runtime [s] Seconds this generator has run since it was rebooted. UINT32_MAX: field not provided. + * @param time_until_maintenance [s] Seconds until this generator requires maintenance. A negative value indicates maintenance is past-due. INT32_MAX: field not provided. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_generator_status_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint64_t status,uint16_t generator_speed,float battery_current,float load_current,float power_generated,float bus_voltage,int16_t rectifier_temperature,float bat_current_setpoint,int16_t generator_temperature,uint32_t runtime,int32_t time_until_maintenance) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_GENERATOR_STATUS_LEN]; + _mav_put_uint64_t(buf, 0, status); + _mav_put_float(buf, 8, battery_current); + _mav_put_float(buf, 12, load_current); + _mav_put_float(buf, 16, power_generated); + _mav_put_float(buf, 20, bus_voltage); + _mav_put_float(buf, 24, bat_current_setpoint); + _mav_put_uint32_t(buf, 28, runtime); + _mav_put_int32_t(buf, 32, time_until_maintenance); + _mav_put_uint16_t(buf, 36, generator_speed); + _mav_put_int16_t(buf, 38, rectifier_temperature); + _mav_put_int16_t(buf, 40, generator_temperature); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_GENERATOR_STATUS_LEN); +#else + mavlink_generator_status_t packet; + packet.status = status; + packet.battery_current = battery_current; + packet.load_current = load_current; + packet.power_generated = power_generated; + packet.bus_voltage = bus_voltage; + packet.bat_current_setpoint = bat_current_setpoint; + packet.runtime = runtime; + packet.time_until_maintenance = time_until_maintenance; + packet.generator_speed = generator_speed; + packet.rectifier_temperature = rectifier_temperature; + packet.generator_temperature = generator_temperature; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_GENERATOR_STATUS_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_GENERATOR_STATUS; + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_GENERATOR_STATUS_MIN_LEN, MAVLINK_MSG_ID_GENERATOR_STATUS_LEN, MAVLINK_MSG_ID_GENERATOR_STATUS_CRC); +} + +/** + * @brief Encode a generator_status struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param generator_status C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_generator_status_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_generator_status_t* generator_status) +{ + return mavlink_msg_generator_status_pack(system_id, component_id, msg, generator_status->status, generator_status->generator_speed, generator_status->battery_current, generator_status->load_current, generator_status->power_generated, generator_status->bus_voltage, generator_status->rectifier_temperature, generator_status->bat_current_setpoint, generator_status->generator_temperature, generator_status->runtime, generator_status->time_until_maintenance); +} + +/** + * @brief Encode a generator_status struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param generator_status C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_generator_status_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_generator_status_t* generator_status) +{ + return mavlink_msg_generator_status_pack_chan(system_id, component_id, chan, msg, generator_status->status, generator_status->generator_speed, generator_status->battery_current, generator_status->load_current, generator_status->power_generated, generator_status->bus_voltage, generator_status->rectifier_temperature, generator_status->bat_current_setpoint, generator_status->generator_temperature, generator_status->runtime, generator_status->time_until_maintenance); +} + +/** + * @brief Send a generator_status message + * @param chan MAVLink channel to send the message + * + * @param status Status flags. + * @param generator_speed [rpm] Speed of electrical generator or alternator. UINT16_MAX: field not provided. + * @param battery_current [A] Current into/out of battery. Positive for out. Negative for in. NaN: field not provided. + * @param load_current [A] Current going to the UAV. If battery current not available this is the DC current from the generator. Positive for out. Negative for in. NaN: field not provided + * @param power_generated [W] The power being generated. NaN: field not provided + * @param bus_voltage [V] Voltage of the bus seen at the generator, or battery bus if battery bus is controlled by generator and at a different voltage to main bus. + * @param rectifier_temperature [degC] The temperature of the rectifier or power converter. INT16_MAX: field not provided. + * @param bat_current_setpoint [A] The target battery current. Positive for out. Negative for in. NaN: field not provided + * @param generator_temperature [degC] The temperature of the mechanical motor, fuel cell core or generator. INT16_MAX: field not provided. + * @param runtime [s] Seconds this generator has run since it was rebooted. UINT32_MAX: field not provided. + * @param time_until_maintenance [s] Seconds until this generator requires maintenance. A negative value indicates maintenance is past-due. INT32_MAX: field not provided. + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_generator_status_send(mavlink_channel_t chan, uint64_t status, uint16_t generator_speed, float battery_current, float load_current, float power_generated, float bus_voltage, int16_t rectifier_temperature, float bat_current_setpoint, int16_t generator_temperature, uint32_t runtime, int32_t time_until_maintenance) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_GENERATOR_STATUS_LEN]; + _mav_put_uint64_t(buf, 0, status); + _mav_put_float(buf, 8, battery_current); + _mav_put_float(buf, 12, load_current); + _mav_put_float(buf, 16, power_generated); + _mav_put_float(buf, 20, bus_voltage); + _mav_put_float(buf, 24, bat_current_setpoint); + _mav_put_uint32_t(buf, 28, runtime); + _mav_put_int32_t(buf, 32, time_until_maintenance); + _mav_put_uint16_t(buf, 36, generator_speed); + _mav_put_int16_t(buf, 38, rectifier_temperature); + _mav_put_int16_t(buf, 40, generator_temperature); + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GENERATOR_STATUS, buf, MAVLINK_MSG_ID_GENERATOR_STATUS_MIN_LEN, MAVLINK_MSG_ID_GENERATOR_STATUS_LEN, MAVLINK_MSG_ID_GENERATOR_STATUS_CRC); +#else + mavlink_generator_status_t packet; + packet.status = status; + packet.battery_current = battery_current; + packet.load_current = load_current; + packet.power_generated = power_generated; + packet.bus_voltage = bus_voltage; + packet.bat_current_setpoint = bat_current_setpoint; + packet.runtime = runtime; + packet.time_until_maintenance = time_until_maintenance; + packet.generator_speed = generator_speed; + packet.rectifier_temperature = rectifier_temperature; + packet.generator_temperature = generator_temperature; + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GENERATOR_STATUS, (const char *)&packet, MAVLINK_MSG_ID_GENERATOR_STATUS_MIN_LEN, MAVLINK_MSG_ID_GENERATOR_STATUS_LEN, MAVLINK_MSG_ID_GENERATOR_STATUS_CRC); +#endif +} + +/** + * @brief Send a generator_status message + * @param chan MAVLink channel to send the message + * @param struct The MAVLink struct to serialize + */ +static inline void mavlink_msg_generator_status_send_struct(mavlink_channel_t chan, const mavlink_generator_status_t* generator_status) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + mavlink_msg_generator_status_send(chan, generator_status->status, generator_status->generator_speed, generator_status->battery_current, generator_status->load_current, generator_status->power_generated, generator_status->bus_voltage, generator_status->rectifier_temperature, generator_status->bat_current_setpoint, generator_status->generator_temperature, generator_status->runtime, generator_status->time_until_maintenance); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GENERATOR_STATUS, (const char *)generator_status, MAVLINK_MSG_ID_GENERATOR_STATUS_MIN_LEN, MAVLINK_MSG_ID_GENERATOR_STATUS_LEN, MAVLINK_MSG_ID_GENERATOR_STATUS_CRC); +#endif +} + +#if MAVLINK_MSG_ID_GENERATOR_STATUS_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_generator_status_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint64_t status, uint16_t generator_speed, float battery_current, float load_current, float power_generated, float bus_voltage, int16_t rectifier_temperature, float bat_current_setpoint, int16_t generator_temperature, uint32_t runtime, int32_t time_until_maintenance) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint64_t(buf, 0, status); + _mav_put_float(buf, 8, battery_current); + _mav_put_float(buf, 12, load_current); + _mav_put_float(buf, 16, power_generated); + _mav_put_float(buf, 20, bus_voltage); + _mav_put_float(buf, 24, bat_current_setpoint); + _mav_put_uint32_t(buf, 28, runtime); + _mav_put_int32_t(buf, 32, time_until_maintenance); + _mav_put_uint16_t(buf, 36, generator_speed); + _mav_put_int16_t(buf, 38, rectifier_temperature); + _mav_put_int16_t(buf, 40, generator_temperature); + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GENERATOR_STATUS, buf, MAVLINK_MSG_ID_GENERATOR_STATUS_MIN_LEN, MAVLINK_MSG_ID_GENERATOR_STATUS_LEN, MAVLINK_MSG_ID_GENERATOR_STATUS_CRC); +#else + mavlink_generator_status_t *packet = (mavlink_generator_status_t *)msgbuf; + packet->status = status; + packet->battery_current = battery_current; + packet->load_current = load_current; + packet->power_generated = power_generated; + packet->bus_voltage = bus_voltage; + packet->bat_current_setpoint = bat_current_setpoint; + packet->runtime = runtime; + packet->time_until_maintenance = time_until_maintenance; + packet->generator_speed = generator_speed; + packet->rectifier_temperature = rectifier_temperature; + packet->generator_temperature = generator_temperature; + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GENERATOR_STATUS, (const char *)packet, MAVLINK_MSG_ID_GENERATOR_STATUS_MIN_LEN, MAVLINK_MSG_ID_GENERATOR_STATUS_LEN, MAVLINK_MSG_ID_GENERATOR_STATUS_CRC); +#endif +} +#endif + +#endif + +// MESSAGE GENERATOR_STATUS UNPACKING + + +/** + * @brief Get field status from generator_status message + * + * @return Status flags. + */ +static inline uint64_t mavlink_msg_generator_status_get_status(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint64_t(msg, 0); +} + +/** + * @brief Get field generator_speed from generator_status message + * + * @return [rpm] Speed of electrical generator or alternator. UINT16_MAX: field not provided. + */ +static inline uint16_t mavlink_msg_generator_status_get_generator_speed(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 36); +} + +/** + * @brief Get field battery_current from generator_status message + * + * @return [A] Current into/out of battery. Positive for out. Negative for in. NaN: field not provided. + */ +static inline float mavlink_msg_generator_status_get_battery_current(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 8); +} + +/** + * @brief Get field load_current from generator_status message + * + * @return [A] Current going to the UAV. If battery current not available this is the DC current from the generator. Positive for out. Negative for in. NaN: field not provided + */ +static inline float mavlink_msg_generator_status_get_load_current(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 12); +} + +/** + * @brief Get field power_generated from generator_status message + * + * @return [W] The power being generated. NaN: field not provided + */ +static inline float mavlink_msg_generator_status_get_power_generated(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 16); +} + +/** + * @brief Get field bus_voltage from generator_status message + * + * @return [V] Voltage of the bus seen at the generator, or battery bus if battery bus is controlled by generator and at a different voltage to main bus. + */ +static inline float mavlink_msg_generator_status_get_bus_voltage(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 20); +} + +/** + * @brief Get field rectifier_temperature from generator_status message + * + * @return [degC] The temperature of the rectifier or power converter. INT16_MAX: field not provided. + */ +static inline int16_t mavlink_msg_generator_status_get_rectifier_temperature(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int16_t(msg, 38); +} + +/** + * @brief Get field bat_current_setpoint from generator_status message + * + * @return [A] The target battery current. Positive for out. Negative for in. NaN: field not provided + */ +static inline float mavlink_msg_generator_status_get_bat_current_setpoint(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 24); +} + +/** + * @brief Get field generator_temperature from generator_status message + * + * @return [degC] The temperature of the mechanical motor, fuel cell core or generator. INT16_MAX: field not provided. + */ +static inline int16_t mavlink_msg_generator_status_get_generator_temperature(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int16_t(msg, 40); +} + +/** + * @brief Get field runtime from generator_status message + * + * @return [s] Seconds this generator has run since it was rebooted. UINT32_MAX: field not provided. + */ +static inline uint32_t mavlink_msg_generator_status_get_runtime(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint32_t(msg, 28); +} + +/** + * @brief Get field time_until_maintenance from generator_status message + * + * @return [s] Seconds until this generator requires maintenance. A negative value indicates maintenance is past-due. INT32_MAX: field not provided. + */ +static inline int32_t mavlink_msg_generator_status_get_time_until_maintenance(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int32_t(msg, 32); +} + +/** + * @brief Decode a generator_status message into a struct + * + * @param msg The message to decode + * @param generator_status C-struct to decode the message contents into + */ +static inline void mavlink_msg_generator_status_decode(const mavlink_message_t* msg, mavlink_generator_status_t* generator_status) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + generator_status->status = mavlink_msg_generator_status_get_status(msg); + generator_status->battery_current = mavlink_msg_generator_status_get_battery_current(msg); + generator_status->load_current = mavlink_msg_generator_status_get_load_current(msg); + generator_status->power_generated = mavlink_msg_generator_status_get_power_generated(msg); + generator_status->bus_voltage = mavlink_msg_generator_status_get_bus_voltage(msg); + generator_status->bat_current_setpoint = mavlink_msg_generator_status_get_bat_current_setpoint(msg); + generator_status->runtime = mavlink_msg_generator_status_get_runtime(msg); + generator_status->time_until_maintenance = mavlink_msg_generator_status_get_time_until_maintenance(msg); + generator_status->generator_speed = mavlink_msg_generator_status_get_generator_speed(msg); + generator_status->rectifier_temperature = mavlink_msg_generator_status_get_rectifier_temperature(msg); + generator_status->generator_temperature = mavlink_msg_generator_status_get_generator_temperature(msg); +#else + uint8_t len = msg->len < MAVLINK_MSG_ID_GENERATOR_STATUS_LEN? msg->len : MAVLINK_MSG_ID_GENERATOR_STATUS_LEN; + memset(generator_status, 0, MAVLINK_MSG_ID_GENERATOR_STATUS_LEN); + memcpy(generator_status, _MAV_PAYLOAD(msg), len); +#endif +} diff --git a/lib/main/MAVLink/common/mavlink_msg_gimbal_device_attitude_status.h b/lib/main/MAVLink/common/mavlink_msg_gimbal_device_attitude_status.h new file mode 100644 index 0000000000..63275480b4 --- /dev/null +++ b/lib/main/MAVLink/common/mavlink_msg_gimbal_device_attitude_status.h @@ -0,0 +1,405 @@ +#pragma once +// MESSAGE GIMBAL_DEVICE_ATTITUDE_STATUS PACKING + +#define MAVLINK_MSG_ID_GIMBAL_DEVICE_ATTITUDE_STATUS 285 + + +typedef struct __mavlink_gimbal_device_attitude_status_t { + uint32_t time_boot_ms; /*< [ms] Timestamp (time since system boot).*/ + float q[4]; /*< Quaternion components, w, x, y, z (1 0 0 0 is the null-rotation, the frame is depends on whether the flag GIMBAL_DEVICE_FLAGS_YAW_LOCK is set)*/ + float angular_velocity_x; /*< [rad/s] X component of angular velocity (NaN if unknown)*/ + float angular_velocity_y; /*< [rad/s] Y component of angular velocity (NaN if unknown)*/ + float angular_velocity_z; /*< [rad/s] Z component of angular velocity (NaN if unknown)*/ + uint32_t failure_flags; /*< Failure flags (0 for no failure)*/ + uint16_t flags; /*< Current gimbal flags set.*/ + uint8_t target_system; /*< System ID*/ + uint8_t target_component; /*< Component ID*/ +} mavlink_gimbal_device_attitude_status_t; + +#define MAVLINK_MSG_ID_GIMBAL_DEVICE_ATTITUDE_STATUS_LEN 40 +#define MAVLINK_MSG_ID_GIMBAL_DEVICE_ATTITUDE_STATUS_MIN_LEN 40 +#define MAVLINK_MSG_ID_285_LEN 40 +#define MAVLINK_MSG_ID_285_MIN_LEN 40 + +#define MAVLINK_MSG_ID_GIMBAL_DEVICE_ATTITUDE_STATUS_CRC 137 +#define MAVLINK_MSG_ID_285_CRC 137 + +#define MAVLINK_MSG_GIMBAL_DEVICE_ATTITUDE_STATUS_FIELD_Q_LEN 4 + +#if MAVLINK_COMMAND_24BIT +#define MAVLINK_MESSAGE_INFO_GIMBAL_DEVICE_ATTITUDE_STATUS { \ + 285, \ + "GIMBAL_DEVICE_ATTITUDE_STATUS", \ + 9, \ + { { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 38, offsetof(mavlink_gimbal_device_attitude_status_t, target_system) }, \ + { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 39, offsetof(mavlink_gimbal_device_attitude_status_t, target_component) }, \ + { "time_boot_ms", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_gimbal_device_attitude_status_t, time_boot_ms) }, \ + { "flags", NULL, MAVLINK_TYPE_UINT16_T, 0, 36, offsetof(mavlink_gimbal_device_attitude_status_t, flags) }, \ + { "q", NULL, MAVLINK_TYPE_FLOAT, 4, 4, offsetof(mavlink_gimbal_device_attitude_status_t, q) }, \ + { "angular_velocity_x", NULL, MAVLINK_TYPE_FLOAT, 0, 20, offsetof(mavlink_gimbal_device_attitude_status_t, angular_velocity_x) }, \ + { "angular_velocity_y", NULL, MAVLINK_TYPE_FLOAT, 0, 24, offsetof(mavlink_gimbal_device_attitude_status_t, angular_velocity_y) }, \ + { "angular_velocity_z", NULL, MAVLINK_TYPE_FLOAT, 0, 28, offsetof(mavlink_gimbal_device_attitude_status_t, angular_velocity_z) }, \ + { "failure_flags", NULL, MAVLINK_TYPE_UINT32_T, 0, 32, offsetof(mavlink_gimbal_device_attitude_status_t, failure_flags) }, \ + } \ +} +#else +#define MAVLINK_MESSAGE_INFO_GIMBAL_DEVICE_ATTITUDE_STATUS { \ + "GIMBAL_DEVICE_ATTITUDE_STATUS", \ + 9, \ + { { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 38, offsetof(mavlink_gimbal_device_attitude_status_t, target_system) }, \ + { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 39, offsetof(mavlink_gimbal_device_attitude_status_t, target_component) }, \ + { "time_boot_ms", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_gimbal_device_attitude_status_t, time_boot_ms) }, \ + { "flags", NULL, MAVLINK_TYPE_UINT16_T, 0, 36, offsetof(mavlink_gimbal_device_attitude_status_t, flags) }, \ + { "q", NULL, MAVLINK_TYPE_FLOAT, 4, 4, offsetof(mavlink_gimbal_device_attitude_status_t, q) }, \ + { "angular_velocity_x", NULL, MAVLINK_TYPE_FLOAT, 0, 20, offsetof(mavlink_gimbal_device_attitude_status_t, angular_velocity_x) }, \ + { "angular_velocity_y", NULL, MAVLINK_TYPE_FLOAT, 0, 24, offsetof(mavlink_gimbal_device_attitude_status_t, angular_velocity_y) }, \ + { "angular_velocity_z", NULL, MAVLINK_TYPE_FLOAT, 0, 28, offsetof(mavlink_gimbal_device_attitude_status_t, angular_velocity_z) }, \ + { "failure_flags", NULL, MAVLINK_TYPE_UINT32_T, 0, 32, offsetof(mavlink_gimbal_device_attitude_status_t, failure_flags) }, \ + } \ +} +#endif + +/** + * @brief Pack a gimbal_device_attitude_status message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param target_system System ID + * @param target_component Component ID + * @param time_boot_ms [ms] Timestamp (time since system boot). + * @param flags Current gimbal flags set. + * @param q Quaternion components, w, x, y, z (1 0 0 0 is the null-rotation, the frame is depends on whether the flag GIMBAL_DEVICE_FLAGS_YAW_LOCK is set) + * @param angular_velocity_x [rad/s] X component of angular velocity (NaN if unknown) + * @param angular_velocity_y [rad/s] Y component of angular velocity (NaN if unknown) + * @param angular_velocity_z [rad/s] Z component of angular velocity (NaN if unknown) + * @param failure_flags Failure flags (0 for no failure) + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_gimbal_device_attitude_status_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint8_t target_system, uint8_t target_component, uint32_t time_boot_ms, uint16_t flags, const float *q, float angular_velocity_x, float angular_velocity_y, float angular_velocity_z, uint32_t failure_flags) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_GIMBAL_DEVICE_ATTITUDE_STATUS_LEN]; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_float(buf, 20, angular_velocity_x); + _mav_put_float(buf, 24, angular_velocity_y); + _mav_put_float(buf, 28, angular_velocity_z); + _mav_put_uint32_t(buf, 32, failure_flags); + _mav_put_uint16_t(buf, 36, flags); + _mav_put_uint8_t(buf, 38, target_system); + _mav_put_uint8_t(buf, 39, target_component); + _mav_put_float_array(buf, 4, q, 4); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_GIMBAL_DEVICE_ATTITUDE_STATUS_LEN); +#else + mavlink_gimbal_device_attitude_status_t packet; + packet.time_boot_ms = time_boot_ms; + packet.angular_velocity_x = angular_velocity_x; + packet.angular_velocity_y = angular_velocity_y; + packet.angular_velocity_z = angular_velocity_z; + packet.failure_flags = failure_flags; + packet.flags = flags; + packet.target_system = target_system; + packet.target_component = target_component; + mav_array_memcpy(packet.q, q, sizeof(float)*4); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_GIMBAL_DEVICE_ATTITUDE_STATUS_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_GIMBAL_DEVICE_ATTITUDE_STATUS; + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_GIMBAL_DEVICE_ATTITUDE_STATUS_MIN_LEN, MAVLINK_MSG_ID_GIMBAL_DEVICE_ATTITUDE_STATUS_LEN, MAVLINK_MSG_ID_GIMBAL_DEVICE_ATTITUDE_STATUS_CRC); +} + +/** + * @brief Pack a gimbal_device_attitude_status message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param target_system System ID + * @param target_component Component ID + * @param time_boot_ms [ms] Timestamp (time since system boot). + * @param flags Current gimbal flags set. + * @param q Quaternion components, w, x, y, z (1 0 0 0 is the null-rotation, the frame is depends on whether the flag GIMBAL_DEVICE_FLAGS_YAW_LOCK is set) + * @param angular_velocity_x [rad/s] X component of angular velocity (NaN if unknown) + * @param angular_velocity_y [rad/s] Y component of angular velocity (NaN if unknown) + * @param angular_velocity_z [rad/s] Z component of angular velocity (NaN if unknown) + * @param failure_flags Failure flags (0 for no failure) + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_gimbal_device_attitude_status_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint8_t target_system,uint8_t target_component,uint32_t time_boot_ms,uint16_t flags,const float *q,float angular_velocity_x,float angular_velocity_y,float angular_velocity_z,uint32_t failure_flags) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_GIMBAL_DEVICE_ATTITUDE_STATUS_LEN]; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_float(buf, 20, angular_velocity_x); + _mav_put_float(buf, 24, angular_velocity_y); + _mav_put_float(buf, 28, angular_velocity_z); + _mav_put_uint32_t(buf, 32, failure_flags); + _mav_put_uint16_t(buf, 36, flags); + _mav_put_uint8_t(buf, 38, target_system); + _mav_put_uint8_t(buf, 39, target_component); + _mav_put_float_array(buf, 4, q, 4); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_GIMBAL_DEVICE_ATTITUDE_STATUS_LEN); +#else + mavlink_gimbal_device_attitude_status_t packet; + packet.time_boot_ms = time_boot_ms; + packet.angular_velocity_x = angular_velocity_x; + packet.angular_velocity_y = angular_velocity_y; + packet.angular_velocity_z = angular_velocity_z; + packet.failure_flags = failure_flags; + packet.flags = flags; + packet.target_system = target_system; + packet.target_component = target_component; + mav_array_memcpy(packet.q, q, sizeof(float)*4); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_GIMBAL_DEVICE_ATTITUDE_STATUS_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_GIMBAL_DEVICE_ATTITUDE_STATUS; + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_GIMBAL_DEVICE_ATTITUDE_STATUS_MIN_LEN, MAVLINK_MSG_ID_GIMBAL_DEVICE_ATTITUDE_STATUS_LEN, MAVLINK_MSG_ID_GIMBAL_DEVICE_ATTITUDE_STATUS_CRC); +} + +/** + * @brief Encode a gimbal_device_attitude_status struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param gimbal_device_attitude_status C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_gimbal_device_attitude_status_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_gimbal_device_attitude_status_t* gimbal_device_attitude_status) +{ + return mavlink_msg_gimbal_device_attitude_status_pack(system_id, component_id, msg, gimbal_device_attitude_status->target_system, gimbal_device_attitude_status->target_component, gimbal_device_attitude_status->time_boot_ms, gimbal_device_attitude_status->flags, gimbal_device_attitude_status->q, gimbal_device_attitude_status->angular_velocity_x, gimbal_device_attitude_status->angular_velocity_y, gimbal_device_attitude_status->angular_velocity_z, gimbal_device_attitude_status->failure_flags); +} + +/** + * @brief Encode a gimbal_device_attitude_status struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param gimbal_device_attitude_status C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_gimbal_device_attitude_status_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_gimbal_device_attitude_status_t* gimbal_device_attitude_status) +{ + return mavlink_msg_gimbal_device_attitude_status_pack_chan(system_id, component_id, chan, msg, gimbal_device_attitude_status->target_system, gimbal_device_attitude_status->target_component, gimbal_device_attitude_status->time_boot_ms, gimbal_device_attitude_status->flags, gimbal_device_attitude_status->q, gimbal_device_attitude_status->angular_velocity_x, gimbal_device_attitude_status->angular_velocity_y, gimbal_device_attitude_status->angular_velocity_z, gimbal_device_attitude_status->failure_flags); +} + +/** + * @brief Send a gimbal_device_attitude_status message + * @param chan MAVLink channel to send the message + * + * @param target_system System ID + * @param target_component Component ID + * @param time_boot_ms [ms] Timestamp (time since system boot). + * @param flags Current gimbal flags set. + * @param q Quaternion components, w, x, y, z (1 0 0 0 is the null-rotation, the frame is depends on whether the flag GIMBAL_DEVICE_FLAGS_YAW_LOCK is set) + * @param angular_velocity_x [rad/s] X component of angular velocity (NaN if unknown) + * @param angular_velocity_y [rad/s] Y component of angular velocity (NaN if unknown) + * @param angular_velocity_z [rad/s] Z component of angular velocity (NaN if unknown) + * @param failure_flags Failure flags (0 for no failure) + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_gimbal_device_attitude_status_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint32_t time_boot_ms, uint16_t flags, const float *q, float angular_velocity_x, float angular_velocity_y, float angular_velocity_z, uint32_t failure_flags) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_GIMBAL_DEVICE_ATTITUDE_STATUS_LEN]; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_float(buf, 20, angular_velocity_x); + _mav_put_float(buf, 24, angular_velocity_y); + _mav_put_float(buf, 28, angular_velocity_z); + _mav_put_uint32_t(buf, 32, failure_flags); + _mav_put_uint16_t(buf, 36, flags); + _mav_put_uint8_t(buf, 38, target_system); + _mav_put_uint8_t(buf, 39, target_component); + _mav_put_float_array(buf, 4, q, 4); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GIMBAL_DEVICE_ATTITUDE_STATUS, buf, MAVLINK_MSG_ID_GIMBAL_DEVICE_ATTITUDE_STATUS_MIN_LEN, MAVLINK_MSG_ID_GIMBAL_DEVICE_ATTITUDE_STATUS_LEN, MAVLINK_MSG_ID_GIMBAL_DEVICE_ATTITUDE_STATUS_CRC); +#else + mavlink_gimbal_device_attitude_status_t packet; + packet.time_boot_ms = time_boot_ms; + packet.angular_velocity_x = angular_velocity_x; + packet.angular_velocity_y = angular_velocity_y; + packet.angular_velocity_z = angular_velocity_z; + packet.failure_flags = failure_flags; + packet.flags = flags; + packet.target_system = target_system; + packet.target_component = target_component; + mav_array_memcpy(packet.q, q, sizeof(float)*4); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GIMBAL_DEVICE_ATTITUDE_STATUS, (const char *)&packet, MAVLINK_MSG_ID_GIMBAL_DEVICE_ATTITUDE_STATUS_MIN_LEN, MAVLINK_MSG_ID_GIMBAL_DEVICE_ATTITUDE_STATUS_LEN, MAVLINK_MSG_ID_GIMBAL_DEVICE_ATTITUDE_STATUS_CRC); +#endif +} + +/** + * @brief Send a gimbal_device_attitude_status message + * @param chan MAVLink channel to send the message + * @param struct The MAVLink struct to serialize + */ +static inline void mavlink_msg_gimbal_device_attitude_status_send_struct(mavlink_channel_t chan, const mavlink_gimbal_device_attitude_status_t* gimbal_device_attitude_status) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + mavlink_msg_gimbal_device_attitude_status_send(chan, gimbal_device_attitude_status->target_system, gimbal_device_attitude_status->target_component, gimbal_device_attitude_status->time_boot_ms, gimbal_device_attitude_status->flags, gimbal_device_attitude_status->q, gimbal_device_attitude_status->angular_velocity_x, gimbal_device_attitude_status->angular_velocity_y, gimbal_device_attitude_status->angular_velocity_z, gimbal_device_attitude_status->failure_flags); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GIMBAL_DEVICE_ATTITUDE_STATUS, (const char *)gimbal_device_attitude_status, MAVLINK_MSG_ID_GIMBAL_DEVICE_ATTITUDE_STATUS_MIN_LEN, MAVLINK_MSG_ID_GIMBAL_DEVICE_ATTITUDE_STATUS_LEN, MAVLINK_MSG_ID_GIMBAL_DEVICE_ATTITUDE_STATUS_CRC); +#endif +} + +#if MAVLINK_MSG_ID_GIMBAL_DEVICE_ATTITUDE_STATUS_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_gimbal_device_attitude_status_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint32_t time_boot_ms, uint16_t flags, const float *q, float angular_velocity_x, float angular_velocity_y, float angular_velocity_z, uint32_t failure_flags) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_float(buf, 20, angular_velocity_x); + _mav_put_float(buf, 24, angular_velocity_y); + _mav_put_float(buf, 28, angular_velocity_z); + _mav_put_uint32_t(buf, 32, failure_flags); + _mav_put_uint16_t(buf, 36, flags); + _mav_put_uint8_t(buf, 38, target_system); + _mav_put_uint8_t(buf, 39, target_component); + _mav_put_float_array(buf, 4, q, 4); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GIMBAL_DEVICE_ATTITUDE_STATUS, buf, MAVLINK_MSG_ID_GIMBAL_DEVICE_ATTITUDE_STATUS_MIN_LEN, MAVLINK_MSG_ID_GIMBAL_DEVICE_ATTITUDE_STATUS_LEN, MAVLINK_MSG_ID_GIMBAL_DEVICE_ATTITUDE_STATUS_CRC); +#else + mavlink_gimbal_device_attitude_status_t *packet = (mavlink_gimbal_device_attitude_status_t *)msgbuf; + packet->time_boot_ms = time_boot_ms; + packet->angular_velocity_x = angular_velocity_x; + packet->angular_velocity_y = angular_velocity_y; + packet->angular_velocity_z = angular_velocity_z; + packet->failure_flags = failure_flags; + packet->flags = flags; + packet->target_system = target_system; + packet->target_component = target_component; + mav_array_memcpy(packet->q, q, sizeof(float)*4); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GIMBAL_DEVICE_ATTITUDE_STATUS, (const char *)packet, MAVLINK_MSG_ID_GIMBAL_DEVICE_ATTITUDE_STATUS_MIN_LEN, MAVLINK_MSG_ID_GIMBAL_DEVICE_ATTITUDE_STATUS_LEN, MAVLINK_MSG_ID_GIMBAL_DEVICE_ATTITUDE_STATUS_CRC); +#endif +} +#endif + +#endif + +// MESSAGE GIMBAL_DEVICE_ATTITUDE_STATUS UNPACKING + + +/** + * @brief Get field target_system from gimbal_device_attitude_status message + * + * @return System ID + */ +static inline uint8_t mavlink_msg_gimbal_device_attitude_status_get_target_system(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 38); +} + +/** + * @brief Get field target_component from gimbal_device_attitude_status message + * + * @return Component ID + */ +static inline uint8_t mavlink_msg_gimbal_device_attitude_status_get_target_component(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 39); +} + +/** + * @brief Get field time_boot_ms from gimbal_device_attitude_status message + * + * @return [ms] Timestamp (time since system boot). + */ +static inline uint32_t mavlink_msg_gimbal_device_attitude_status_get_time_boot_ms(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint32_t(msg, 0); +} + +/** + * @brief Get field flags from gimbal_device_attitude_status message + * + * @return Current gimbal flags set. + */ +static inline uint16_t mavlink_msg_gimbal_device_attitude_status_get_flags(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 36); +} + +/** + * @brief Get field q from gimbal_device_attitude_status message + * + * @return Quaternion components, w, x, y, z (1 0 0 0 is the null-rotation, the frame is depends on whether the flag GIMBAL_DEVICE_FLAGS_YAW_LOCK is set) + */ +static inline uint16_t mavlink_msg_gimbal_device_attitude_status_get_q(const mavlink_message_t* msg, float *q) +{ + return _MAV_RETURN_float_array(msg, q, 4, 4); +} + +/** + * @brief Get field angular_velocity_x from gimbal_device_attitude_status message + * + * @return [rad/s] X component of angular velocity (NaN if unknown) + */ +static inline float mavlink_msg_gimbal_device_attitude_status_get_angular_velocity_x(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 20); +} + +/** + * @brief Get field angular_velocity_y from gimbal_device_attitude_status message + * + * @return [rad/s] Y component of angular velocity (NaN if unknown) + */ +static inline float mavlink_msg_gimbal_device_attitude_status_get_angular_velocity_y(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 24); +} + +/** + * @brief Get field angular_velocity_z from gimbal_device_attitude_status message + * + * @return [rad/s] Z component of angular velocity (NaN if unknown) + */ +static inline float mavlink_msg_gimbal_device_attitude_status_get_angular_velocity_z(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 28); +} + +/** + * @brief Get field failure_flags from gimbal_device_attitude_status message + * + * @return Failure flags (0 for no failure) + */ +static inline uint32_t mavlink_msg_gimbal_device_attitude_status_get_failure_flags(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint32_t(msg, 32); +} + +/** + * @brief Decode a gimbal_device_attitude_status message into a struct + * + * @param msg The message to decode + * @param gimbal_device_attitude_status C-struct to decode the message contents into + */ +static inline void mavlink_msg_gimbal_device_attitude_status_decode(const mavlink_message_t* msg, mavlink_gimbal_device_attitude_status_t* gimbal_device_attitude_status) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + gimbal_device_attitude_status->time_boot_ms = mavlink_msg_gimbal_device_attitude_status_get_time_boot_ms(msg); + mavlink_msg_gimbal_device_attitude_status_get_q(msg, gimbal_device_attitude_status->q); + gimbal_device_attitude_status->angular_velocity_x = mavlink_msg_gimbal_device_attitude_status_get_angular_velocity_x(msg); + gimbal_device_attitude_status->angular_velocity_y = mavlink_msg_gimbal_device_attitude_status_get_angular_velocity_y(msg); + gimbal_device_attitude_status->angular_velocity_z = mavlink_msg_gimbal_device_attitude_status_get_angular_velocity_z(msg); + gimbal_device_attitude_status->failure_flags = mavlink_msg_gimbal_device_attitude_status_get_failure_flags(msg); + gimbal_device_attitude_status->flags = mavlink_msg_gimbal_device_attitude_status_get_flags(msg); + gimbal_device_attitude_status->target_system = mavlink_msg_gimbal_device_attitude_status_get_target_system(msg); + gimbal_device_attitude_status->target_component = mavlink_msg_gimbal_device_attitude_status_get_target_component(msg); +#else + uint8_t len = msg->len < MAVLINK_MSG_ID_GIMBAL_DEVICE_ATTITUDE_STATUS_LEN? msg->len : MAVLINK_MSG_ID_GIMBAL_DEVICE_ATTITUDE_STATUS_LEN; + memset(gimbal_device_attitude_status, 0, MAVLINK_MSG_ID_GIMBAL_DEVICE_ATTITUDE_STATUS_LEN); + memcpy(gimbal_device_attitude_status, _MAV_PAYLOAD(msg), len); +#endif +} diff --git a/lib/main/MAVLink/common/mavlink_msg_gimbal_device_information.h b/lib/main/MAVLink/common/mavlink_msg_gimbal_device_information.h new file mode 100644 index 0000000000..2f0c86bdc9 --- /dev/null +++ b/lib/main/MAVLink/common/mavlink_msg_gimbal_device_information.h @@ -0,0 +1,557 @@ +#pragma once +// MESSAGE GIMBAL_DEVICE_INFORMATION PACKING + +#define MAVLINK_MSG_ID_GIMBAL_DEVICE_INFORMATION 283 + + +typedef struct __mavlink_gimbal_device_information_t { + uint64_t uid; /*< UID of gimbal hardware (0 if unknown).*/ + uint32_t time_boot_ms; /*< [ms] Timestamp (time since system boot).*/ + uint32_t firmware_version; /*< Version of the gimbal firmware, encoded as: (Dev & 0xff) << 24 | (Patch & 0xff) << 16 | (Minor & 0xff) << 8 | (Major & 0xff).*/ + uint32_t hardware_version; /*< Version of the gimbal hardware, encoded as: (Dev & 0xff) << 24 | (Patch & 0xff) << 16 | (Minor & 0xff) << 8 | (Major & 0xff).*/ + float roll_min; /*< [rad] Minimum hardware roll angle (positive: rolling to the right, negative: rolling to the left)*/ + float roll_max; /*< [rad] Maximum hardware roll angle (positive: rolling to the right, negative: rolling to the left)*/ + float pitch_min; /*< [rad] Minimum hardware pitch angle (positive: up, negative: down)*/ + float pitch_max; /*< [rad] Maximum hardware pitch angle (positive: up, negative: down)*/ + float yaw_min; /*< [rad] Minimum hardware yaw angle (positive: to the right, negative: to the left)*/ + float yaw_max; /*< [rad] Maximum hardware yaw angle (positive: to the right, negative: to the left)*/ + uint16_t cap_flags; /*< Bitmap of gimbal capability flags.*/ + uint16_t custom_cap_flags; /*< Bitmap for use for gimbal-specific capability flags.*/ + char vendor_name[32]; /*< Name of the gimbal vendor.*/ + char model_name[32]; /*< Name of the gimbal model.*/ + char custom_name[32]; /*< Custom name of the gimbal given to it by the user.*/ +} mavlink_gimbal_device_information_t; + +#define MAVLINK_MSG_ID_GIMBAL_DEVICE_INFORMATION_LEN 144 +#define MAVLINK_MSG_ID_GIMBAL_DEVICE_INFORMATION_MIN_LEN 144 +#define MAVLINK_MSG_ID_283_LEN 144 +#define MAVLINK_MSG_ID_283_MIN_LEN 144 + +#define MAVLINK_MSG_ID_GIMBAL_DEVICE_INFORMATION_CRC 74 +#define MAVLINK_MSG_ID_283_CRC 74 + +#define MAVLINK_MSG_GIMBAL_DEVICE_INFORMATION_FIELD_VENDOR_NAME_LEN 32 +#define MAVLINK_MSG_GIMBAL_DEVICE_INFORMATION_FIELD_MODEL_NAME_LEN 32 +#define MAVLINK_MSG_GIMBAL_DEVICE_INFORMATION_FIELD_CUSTOM_NAME_LEN 32 + +#if MAVLINK_COMMAND_24BIT +#define MAVLINK_MESSAGE_INFO_GIMBAL_DEVICE_INFORMATION { \ + 283, \ + "GIMBAL_DEVICE_INFORMATION", \ + 15, \ + { { "time_boot_ms", NULL, MAVLINK_TYPE_UINT32_T, 0, 8, offsetof(mavlink_gimbal_device_information_t, time_boot_ms) }, \ + { "vendor_name", NULL, MAVLINK_TYPE_CHAR, 32, 48, offsetof(mavlink_gimbal_device_information_t, vendor_name) }, \ + { "model_name", NULL, MAVLINK_TYPE_CHAR, 32, 80, offsetof(mavlink_gimbal_device_information_t, model_name) }, \ + { "custom_name", NULL, MAVLINK_TYPE_CHAR, 32, 112, offsetof(mavlink_gimbal_device_information_t, custom_name) }, \ + { "firmware_version", NULL, MAVLINK_TYPE_UINT32_T, 0, 12, offsetof(mavlink_gimbal_device_information_t, firmware_version) }, \ + { "hardware_version", NULL, MAVLINK_TYPE_UINT32_T, 0, 16, offsetof(mavlink_gimbal_device_information_t, hardware_version) }, \ + { "uid", NULL, MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_gimbal_device_information_t, uid) }, \ + { "cap_flags", NULL, MAVLINK_TYPE_UINT16_T, 0, 44, offsetof(mavlink_gimbal_device_information_t, cap_flags) }, \ + { "custom_cap_flags", NULL, MAVLINK_TYPE_UINT16_T, 0, 46, offsetof(mavlink_gimbal_device_information_t, custom_cap_flags) }, \ + { "roll_min", NULL, MAVLINK_TYPE_FLOAT, 0, 20, offsetof(mavlink_gimbal_device_information_t, roll_min) }, \ + { "roll_max", NULL, MAVLINK_TYPE_FLOAT, 0, 24, offsetof(mavlink_gimbal_device_information_t, roll_max) }, \ + { "pitch_min", NULL, MAVLINK_TYPE_FLOAT, 0, 28, offsetof(mavlink_gimbal_device_information_t, pitch_min) }, \ + { "pitch_max", NULL, MAVLINK_TYPE_FLOAT, 0, 32, offsetof(mavlink_gimbal_device_information_t, pitch_max) }, \ + { "yaw_min", NULL, MAVLINK_TYPE_FLOAT, 0, 36, offsetof(mavlink_gimbal_device_information_t, yaw_min) }, \ + { "yaw_max", NULL, MAVLINK_TYPE_FLOAT, 0, 40, offsetof(mavlink_gimbal_device_information_t, yaw_max) }, \ + } \ +} +#else +#define MAVLINK_MESSAGE_INFO_GIMBAL_DEVICE_INFORMATION { \ + "GIMBAL_DEVICE_INFORMATION", \ + 15, \ + { { "time_boot_ms", NULL, MAVLINK_TYPE_UINT32_T, 0, 8, offsetof(mavlink_gimbal_device_information_t, time_boot_ms) }, \ + { "vendor_name", NULL, MAVLINK_TYPE_CHAR, 32, 48, offsetof(mavlink_gimbal_device_information_t, vendor_name) }, \ + { "model_name", NULL, MAVLINK_TYPE_CHAR, 32, 80, offsetof(mavlink_gimbal_device_information_t, model_name) }, \ + { "custom_name", NULL, MAVLINK_TYPE_CHAR, 32, 112, offsetof(mavlink_gimbal_device_information_t, custom_name) }, \ + { "firmware_version", NULL, MAVLINK_TYPE_UINT32_T, 0, 12, offsetof(mavlink_gimbal_device_information_t, firmware_version) }, \ + { "hardware_version", NULL, MAVLINK_TYPE_UINT32_T, 0, 16, offsetof(mavlink_gimbal_device_information_t, hardware_version) }, \ + { "uid", NULL, MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_gimbal_device_information_t, uid) }, \ + { "cap_flags", NULL, MAVLINK_TYPE_UINT16_T, 0, 44, offsetof(mavlink_gimbal_device_information_t, cap_flags) }, \ + { "custom_cap_flags", NULL, MAVLINK_TYPE_UINT16_T, 0, 46, offsetof(mavlink_gimbal_device_information_t, custom_cap_flags) }, \ + { "roll_min", NULL, MAVLINK_TYPE_FLOAT, 0, 20, offsetof(mavlink_gimbal_device_information_t, roll_min) }, \ + { "roll_max", NULL, MAVLINK_TYPE_FLOAT, 0, 24, offsetof(mavlink_gimbal_device_information_t, roll_max) }, \ + { "pitch_min", NULL, MAVLINK_TYPE_FLOAT, 0, 28, offsetof(mavlink_gimbal_device_information_t, pitch_min) }, \ + { "pitch_max", NULL, MAVLINK_TYPE_FLOAT, 0, 32, offsetof(mavlink_gimbal_device_information_t, pitch_max) }, \ + { "yaw_min", NULL, MAVLINK_TYPE_FLOAT, 0, 36, offsetof(mavlink_gimbal_device_information_t, yaw_min) }, \ + { "yaw_max", NULL, MAVLINK_TYPE_FLOAT, 0, 40, offsetof(mavlink_gimbal_device_information_t, yaw_max) }, \ + } \ +} +#endif + +/** + * @brief Pack a gimbal_device_information message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param time_boot_ms [ms] Timestamp (time since system boot). + * @param vendor_name Name of the gimbal vendor. + * @param model_name Name of the gimbal model. + * @param custom_name Custom name of the gimbal given to it by the user. + * @param firmware_version Version of the gimbal firmware, encoded as: (Dev & 0xff) << 24 | (Patch & 0xff) << 16 | (Minor & 0xff) << 8 | (Major & 0xff). + * @param hardware_version Version of the gimbal hardware, encoded as: (Dev & 0xff) << 24 | (Patch & 0xff) << 16 | (Minor & 0xff) << 8 | (Major & 0xff). + * @param uid UID of gimbal hardware (0 if unknown). + * @param cap_flags Bitmap of gimbal capability flags. + * @param custom_cap_flags Bitmap for use for gimbal-specific capability flags. + * @param roll_min [rad] Minimum hardware roll angle (positive: rolling to the right, negative: rolling to the left) + * @param roll_max [rad] Maximum hardware roll angle (positive: rolling to the right, negative: rolling to the left) + * @param pitch_min [rad] Minimum hardware pitch angle (positive: up, negative: down) + * @param pitch_max [rad] Maximum hardware pitch angle (positive: up, negative: down) + * @param yaw_min [rad] Minimum hardware yaw angle (positive: to the right, negative: to the left) + * @param yaw_max [rad] Maximum hardware yaw angle (positive: to the right, negative: to the left) + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_gimbal_device_information_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint32_t time_boot_ms, const char *vendor_name, const char *model_name, const char *custom_name, uint32_t firmware_version, uint32_t hardware_version, uint64_t uid, uint16_t cap_flags, uint16_t custom_cap_flags, float roll_min, float roll_max, float pitch_min, float pitch_max, float yaw_min, float yaw_max) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_GIMBAL_DEVICE_INFORMATION_LEN]; + _mav_put_uint64_t(buf, 0, uid); + _mav_put_uint32_t(buf, 8, time_boot_ms); + _mav_put_uint32_t(buf, 12, firmware_version); + _mav_put_uint32_t(buf, 16, hardware_version); + _mav_put_float(buf, 20, roll_min); + _mav_put_float(buf, 24, roll_max); + _mav_put_float(buf, 28, pitch_min); + _mav_put_float(buf, 32, pitch_max); + _mav_put_float(buf, 36, yaw_min); + _mav_put_float(buf, 40, yaw_max); + _mav_put_uint16_t(buf, 44, cap_flags); + _mav_put_uint16_t(buf, 46, custom_cap_flags); + _mav_put_char_array(buf, 48, vendor_name, 32); + _mav_put_char_array(buf, 80, model_name, 32); + _mav_put_char_array(buf, 112, custom_name, 32); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_GIMBAL_DEVICE_INFORMATION_LEN); +#else + mavlink_gimbal_device_information_t packet; + packet.uid = uid; + packet.time_boot_ms = time_boot_ms; + packet.firmware_version = firmware_version; + packet.hardware_version = hardware_version; + packet.roll_min = roll_min; + packet.roll_max = roll_max; + packet.pitch_min = pitch_min; + packet.pitch_max = pitch_max; + packet.yaw_min = yaw_min; + packet.yaw_max = yaw_max; + packet.cap_flags = cap_flags; + packet.custom_cap_flags = custom_cap_flags; + mav_array_memcpy(packet.vendor_name, vendor_name, sizeof(char)*32); + mav_array_memcpy(packet.model_name, model_name, sizeof(char)*32); + mav_array_memcpy(packet.custom_name, custom_name, sizeof(char)*32); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_GIMBAL_DEVICE_INFORMATION_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_GIMBAL_DEVICE_INFORMATION; + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_GIMBAL_DEVICE_INFORMATION_MIN_LEN, MAVLINK_MSG_ID_GIMBAL_DEVICE_INFORMATION_LEN, MAVLINK_MSG_ID_GIMBAL_DEVICE_INFORMATION_CRC); +} + +/** + * @brief Pack a gimbal_device_information message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param time_boot_ms [ms] Timestamp (time since system boot). + * @param vendor_name Name of the gimbal vendor. + * @param model_name Name of the gimbal model. + * @param custom_name Custom name of the gimbal given to it by the user. + * @param firmware_version Version of the gimbal firmware, encoded as: (Dev & 0xff) << 24 | (Patch & 0xff) << 16 | (Minor & 0xff) << 8 | (Major & 0xff). + * @param hardware_version Version of the gimbal hardware, encoded as: (Dev & 0xff) << 24 | (Patch & 0xff) << 16 | (Minor & 0xff) << 8 | (Major & 0xff). + * @param uid UID of gimbal hardware (0 if unknown). + * @param cap_flags Bitmap of gimbal capability flags. + * @param custom_cap_flags Bitmap for use for gimbal-specific capability flags. + * @param roll_min [rad] Minimum hardware roll angle (positive: rolling to the right, negative: rolling to the left) + * @param roll_max [rad] Maximum hardware roll angle (positive: rolling to the right, negative: rolling to the left) + * @param pitch_min [rad] Minimum hardware pitch angle (positive: up, negative: down) + * @param pitch_max [rad] Maximum hardware pitch angle (positive: up, negative: down) + * @param yaw_min [rad] Minimum hardware yaw angle (positive: to the right, negative: to the left) + * @param yaw_max [rad] Maximum hardware yaw angle (positive: to the right, negative: to the left) + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_gimbal_device_information_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint32_t time_boot_ms,const char *vendor_name,const char *model_name,const char *custom_name,uint32_t firmware_version,uint32_t hardware_version,uint64_t uid,uint16_t cap_flags,uint16_t custom_cap_flags,float roll_min,float roll_max,float pitch_min,float pitch_max,float yaw_min,float yaw_max) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_GIMBAL_DEVICE_INFORMATION_LEN]; + _mav_put_uint64_t(buf, 0, uid); + _mav_put_uint32_t(buf, 8, time_boot_ms); + _mav_put_uint32_t(buf, 12, firmware_version); + _mav_put_uint32_t(buf, 16, hardware_version); + _mav_put_float(buf, 20, roll_min); + _mav_put_float(buf, 24, roll_max); + _mav_put_float(buf, 28, pitch_min); + _mav_put_float(buf, 32, pitch_max); + _mav_put_float(buf, 36, yaw_min); + _mav_put_float(buf, 40, yaw_max); + _mav_put_uint16_t(buf, 44, cap_flags); + _mav_put_uint16_t(buf, 46, custom_cap_flags); + _mav_put_char_array(buf, 48, vendor_name, 32); + _mav_put_char_array(buf, 80, model_name, 32); + _mav_put_char_array(buf, 112, custom_name, 32); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_GIMBAL_DEVICE_INFORMATION_LEN); +#else + mavlink_gimbal_device_information_t packet; + packet.uid = uid; + packet.time_boot_ms = time_boot_ms; + packet.firmware_version = firmware_version; + packet.hardware_version = hardware_version; + packet.roll_min = roll_min; + packet.roll_max = roll_max; + packet.pitch_min = pitch_min; + packet.pitch_max = pitch_max; + packet.yaw_min = yaw_min; + packet.yaw_max = yaw_max; + packet.cap_flags = cap_flags; + packet.custom_cap_flags = custom_cap_flags; + mav_array_memcpy(packet.vendor_name, vendor_name, sizeof(char)*32); + mav_array_memcpy(packet.model_name, model_name, sizeof(char)*32); + mav_array_memcpy(packet.custom_name, custom_name, sizeof(char)*32); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_GIMBAL_DEVICE_INFORMATION_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_GIMBAL_DEVICE_INFORMATION; + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_GIMBAL_DEVICE_INFORMATION_MIN_LEN, MAVLINK_MSG_ID_GIMBAL_DEVICE_INFORMATION_LEN, MAVLINK_MSG_ID_GIMBAL_DEVICE_INFORMATION_CRC); +} + +/** + * @brief Encode a gimbal_device_information struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param gimbal_device_information C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_gimbal_device_information_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_gimbal_device_information_t* gimbal_device_information) +{ + return mavlink_msg_gimbal_device_information_pack(system_id, component_id, msg, gimbal_device_information->time_boot_ms, gimbal_device_information->vendor_name, gimbal_device_information->model_name, gimbal_device_information->custom_name, gimbal_device_information->firmware_version, gimbal_device_information->hardware_version, gimbal_device_information->uid, gimbal_device_information->cap_flags, gimbal_device_information->custom_cap_flags, gimbal_device_information->roll_min, gimbal_device_information->roll_max, gimbal_device_information->pitch_min, gimbal_device_information->pitch_max, gimbal_device_information->yaw_min, gimbal_device_information->yaw_max); +} + +/** + * @brief Encode a gimbal_device_information struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param gimbal_device_information C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_gimbal_device_information_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_gimbal_device_information_t* gimbal_device_information) +{ + return mavlink_msg_gimbal_device_information_pack_chan(system_id, component_id, chan, msg, gimbal_device_information->time_boot_ms, gimbal_device_information->vendor_name, gimbal_device_information->model_name, gimbal_device_information->custom_name, gimbal_device_information->firmware_version, gimbal_device_information->hardware_version, gimbal_device_information->uid, gimbal_device_information->cap_flags, gimbal_device_information->custom_cap_flags, gimbal_device_information->roll_min, gimbal_device_information->roll_max, gimbal_device_information->pitch_min, gimbal_device_information->pitch_max, gimbal_device_information->yaw_min, gimbal_device_information->yaw_max); +} + +/** + * @brief Send a gimbal_device_information message + * @param chan MAVLink channel to send the message + * + * @param time_boot_ms [ms] Timestamp (time since system boot). + * @param vendor_name Name of the gimbal vendor. + * @param model_name Name of the gimbal model. + * @param custom_name Custom name of the gimbal given to it by the user. + * @param firmware_version Version of the gimbal firmware, encoded as: (Dev & 0xff) << 24 | (Patch & 0xff) << 16 | (Minor & 0xff) << 8 | (Major & 0xff). + * @param hardware_version Version of the gimbal hardware, encoded as: (Dev & 0xff) << 24 | (Patch & 0xff) << 16 | (Minor & 0xff) << 8 | (Major & 0xff). + * @param uid UID of gimbal hardware (0 if unknown). + * @param cap_flags Bitmap of gimbal capability flags. + * @param custom_cap_flags Bitmap for use for gimbal-specific capability flags. + * @param roll_min [rad] Minimum hardware roll angle (positive: rolling to the right, negative: rolling to the left) + * @param roll_max [rad] Maximum hardware roll angle (positive: rolling to the right, negative: rolling to the left) + * @param pitch_min [rad] Minimum hardware pitch angle (positive: up, negative: down) + * @param pitch_max [rad] Maximum hardware pitch angle (positive: up, negative: down) + * @param yaw_min [rad] Minimum hardware yaw angle (positive: to the right, negative: to the left) + * @param yaw_max [rad] Maximum hardware yaw angle (positive: to the right, negative: to the left) + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_gimbal_device_information_send(mavlink_channel_t chan, uint32_t time_boot_ms, const char *vendor_name, const char *model_name, const char *custom_name, uint32_t firmware_version, uint32_t hardware_version, uint64_t uid, uint16_t cap_flags, uint16_t custom_cap_flags, float roll_min, float roll_max, float pitch_min, float pitch_max, float yaw_min, float yaw_max) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_GIMBAL_DEVICE_INFORMATION_LEN]; + _mav_put_uint64_t(buf, 0, uid); + _mav_put_uint32_t(buf, 8, time_boot_ms); + _mav_put_uint32_t(buf, 12, firmware_version); + _mav_put_uint32_t(buf, 16, hardware_version); + _mav_put_float(buf, 20, roll_min); + _mav_put_float(buf, 24, roll_max); + _mav_put_float(buf, 28, pitch_min); + _mav_put_float(buf, 32, pitch_max); + _mav_put_float(buf, 36, yaw_min); + _mav_put_float(buf, 40, yaw_max); + _mav_put_uint16_t(buf, 44, cap_flags); + _mav_put_uint16_t(buf, 46, custom_cap_flags); + _mav_put_char_array(buf, 48, vendor_name, 32); + _mav_put_char_array(buf, 80, model_name, 32); + _mav_put_char_array(buf, 112, custom_name, 32); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GIMBAL_DEVICE_INFORMATION, buf, MAVLINK_MSG_ID_GIMBAL_DEVICE_INFORMATION_MIN_LEN, MAVLINK_MSG_ID_GIMBAL_DEVICE_INFORMATION_LEN, MAVLINK_MSG_ID_GIMBAL_DEVICE_INFORMATION_CRC); +#else + mavlink_gimbal_device_information_t packet; + packet.uid = uid; + packet.time_boot_ms = time_boot_ms; + packet.firmware_version = firmware_version; + packet.hardware_version = hardware_version; + packet.roll_min = roll_min; + packet.roll_max = roll_max; + packet.pitch_min = pitch_min; + packet.pitch_max = pitch_max; + packet.yaw_min = yaw_min; + packet.yaw_max = yaw_max; + packet.cap_flags = cap_flags; + packet.custom_cap_flags = custom_cap_flags; + mav_array_memcpy(packet.vendor_name, vendor_name, sizeof(char)*32); + mav_array_memcpy(packet.model_name, model_name, sizeof(char)*32); + mav_array_memcpy(packet.custom_name, custom_name, sizeof(char)*32); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GIMBAL_DEVICE_INFORMATION, (const char *)&packet, MAVLINK_MSG_ID_GIMBAL_DEVICE_INFORMATION_MIN_LEN, MAVLINK_MSG_ID_GIMBAL_DEVICE_INFORMATION_LEN, MAVLINK_MSG_ID_GIMBAL_DEVICE_INFORMATION_CRC); +#endif +} + +/** + * @brief Send a gimbal_device_information message + * @param chan MAVLink channel to send the message + * @param struct The MAVLink struct to serialize + */ +static inline void mavlink_msg_gimbal_device_information_send_struct(mavlink_channel_t chan, const mavlink_gimbal_device_information_t* gimbal_device_information) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + mavlink_msg_gimbal_device_information_send(chan, gimbal_device_information->time_boot_ms, gimbal_device_information->vendor_name, gimbal_device_information->model_name, gimbal_device_information->custom_name, gimbal_device_information->firmware_version, gimbal_device_information->hardware_version, gimbal_device_information->uid, gimbal_device_information->cap_flags, gimbal_device_information->custom_cap_flags, gimbal_device_information->roll_min, gimbal_device_information->roll_max, gimbal_device_information->pitch_min, gimbal_device_information->pitch_max, gimbal_device_information->yaw_min, gimbal_device_information->yaw_max); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GIMBAL_DEVICE_INFORMATION, (const char *)gimbal_device_information, MAVLINK_MSG_ID_GIMBAL_DEVICE_INFORMATION_MIN_LEN, MAVLINK_MSG_ID_GIMBAL_DEVICE_INFORMATION_LEN, MAVLINK_MSG_ID_GIMBAL_DEVICE_INFORMATION_CRC); +#endif +} + +#if MAVLINK_MSG_ID_GIMBAL_DEVICE_INFORMATION_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_gimbal_device_information_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint32_t time_boot_ms, const char *vendor_name, const char *model_name, const char *custom_name, uint32_t firmware_version, uint32_t hardware_version, uint64_t uid, uint16_t cap_flags, uint16_t custom_cap_flags, float roll_min, float roll_max, float pitch_min, float pitch_max, float yaw_min, float yaw_max) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint64_t(buf, 0, uid); + _mav_put_uint32_t(buf, 8, time_boot_ms); + _mav_put_uint32_t(buf, 12, firmware_version); + _mav_put_uint32_t(buf, 16, hardware_version); + _mav_put_float(buf, 20, roll_min); + _mav_put_float(buf, 24, roll_max); + _mav_put_float(buf, 28, pitch_min); + _mav_put_float(buf, 32, pitch_max); + _mav_put_float(buf, 36, yaw_min); + _mav_put_float(buf, 40, yaw_max); + _mav_put_uint16_t(buf, 44, cap_flags); + _mav_put_uint16_t(buf, 46, custom_cap_flags); + _mav_put_char_array(buf, 48, vendor_name, 32); + _mav_put_char_array(buf, 80, model_name, 32); + _mav_put_char_array(buf, 112, custom_name, 32); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GIMBAL_DEVICE_INFORMATION, buf, MAVLINK_MSG_ID_GIMBAL_DEVICE_INFORMATION_MIN_LEN, MAVLINK_MSG_ID_GIMBAL_DEVICE_INFORMATION_LEN, MAVLINK_MSG_ID_GIMBAL_DEVICE_INFORMATION_CRC); +#else + mavlink_gimbal_device_information_t *packet = (mavlink_gimbal_device_information_t *)msgbuf; + packet->uid = uid; + packet->time_boot_ms = time_boot_ms; + packet->firmware_version = firmware_version; + packet->hardware_version = hardware_version; + packet->roll_min = roll_min; + packet->roll_max = roll_max; + packet->pitch_min = pitch_min; + packet->pitch_max = pitch_max; + packet->yaw_min = yaw_min; + packet->yaw_max = yaw_max; + packet->cap_flags = cap_flags; + packet->custom_cap_flags = custom_cap_flags; + mav_array_memcpy(packet->vendor_name, vendor_name, sizeof(char)*32); + mav_array_memcpy(packet->model_name, model_name, sizeof(char)*32); + mav_array_memcpy(packet->custom_name, custom_name, sizeof(char)*32); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GIMBAL_DEVICE_INFORMATION, (const char *)packet, MAVLINK_MSG_ID_GIMBAL_DEVICE_INFORMATION_MIN_LEN, MAVLINK_MSG_ID_GIMBAL_DEVICE_INFORMATION_LEN, MAVLINK_MSG_ID_GIMBAL_DEVICE_INFORMATION_CRC); +#endif +} +#endif + +#endif + +// MESSAGE GIMBAL_DEVICE_INFORMATION UNPACKING + + +/** + * @brief Get field time_boot_ms from gimbal_device_information message + * + * @return [ms] Timestamp (time since system boot). + */ +static inline uint32_t mavlink_msg_gimbal_device_information_get_time_boot_ms(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint32_t(msg, 8); +} + +/** + * @brief Get field vendor_name from gimbal_device_information message + * + * @return Name of the gimbal vendor. + */ +static inline uint16_t mavlink_msg_gimbal_device_information_get_vendor_name(const mavlink_message_t* msg, char *vendor_name) +{ + return _MAV_RETURN_char_array(msg, vendor_name, 32, 48); +} + +/** + * @brief Get field model_name from gimbal_device_information message + * + * @return Name of the gimbal model. + */ +static inline uint16_t mavlink_msg_gimbal_device_information_get_model_name(const mavlink_message_t* msg, char *model_name) +{ + return _MAV_RETURN_char_array(msg, model_name, 32, 80); +} + +/** + * @brief Get field custom_name from gimbal_device_information message + * + * @return Custom name of the gimbal given to it by the user. + */ +static inline uint16_t mavlink_msg_gimbal_device_information_get_custom_name(const mavlink_message_t* msg, char *custom_name) +{ + return _MAV_RETURN_char_array(msg, custom_name, 32, 112); +} + +/** + * @brief Get field firmware_version from gimbal_device_information message + * + * @return Version of the gimbal firmware, encoded as: (Dev & 0xff) << 24 | (Patch & 0xff) << 16 | (Minor & 0xff) << 8 | (Major & 0xff). + */ +static inline uint32_t mavlink_msg_gimbal_device_information_get_firmware_version(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint32_t(msg, 12); +} + +/** + * @brief Get field hardware_version from gimbal_device_information message + * + * @return Version of the gimbal hardware, encoded as: (Dev & 0xff) << 24 | (Patch & 0xff) << 16 | (Minor & 0xff) << 8 | (Major & 0xff). + */ +static inline uint32_t mavlink_msg_gimbal_device_information_get_hardware_version(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint32_t(msg, 16); +} + +/** + * @brief Get field uid from gimbal_device_information message + * + * @return UID of gimbal hardware (0 if unknown). + */ +static inline uint64_t mavlink_msg_gimbal_device_information_get_uid(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint64_t(msg, 0); +} + +/** + * @brief Get field cap_flags from gimbal_device_information message + * + * @return Bitmap of gimbal capability flags. + */ +static inline uint16_t mavlink_msg_gimbal_device_information_get_cap_flags(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 44); +} + +/** + * @brief Get field custom_cap_flags from gimbal_device_information message + * + * @return Bitmap for use for gimbal-specific capability flags. + */ +static inline uint16_t mavlink_msg_gimbal_device_information_get_custom_cap_flags(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 46); +} + +/** + * @brief Get field roll_min from gimbal_device_information message + * + * @return [rad] Minimum hardware roll angle (positive: rolling to the right, negative: rolling to the left) + */ +static inline float mavlink_msg_gimbal_device_information_get_roll_min(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 20); +} + +/** + * @brief Get field roll_max from gimbal_device_information message + * + * @return [rad] Maximum hardware roll angle (positive: rolling to the right, negative: rolling to the left) + */ +static inline float mavlink_msg_gimbal_device_information_get_roll_max(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 24); +} + +/** + * @brief Get field pitch_min from gimbal_device_information message + * + * @return [rad] Minimum hardware pitch angle (positive: up, negative: down) + */ +static inline float mavlink_msg_gimbal_device_information_get_pitch_min(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 28); +} + +/** + * @brief Get field pitch_max from gimbal_device_information message + * + * @return [rad] Maximum hardware pitch angle (positive: up, negative: down) + */ +static inline float mavlink_msg_gimbal_device_information_get_pitch_max(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 32); +} + +/** + * @brief Get field yaw_min from gimbal_device_information message + * + * @return [rad] Minimum hardware yaw angle (positive: to the right, negative: to the left) + */ +static inline float mavlink_msg_gimbal_device_information_get_yaw_min(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 36); +} + +/** + * @brief Get field yaw_max from gimbal_device_information message + * + * @return [rad] Maximum hardware yaw angle (positive: to the right, negative: to the left) + */ +static inline float mavlink_msg_gimbal_device_information_get_yaw_max(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 40); +} + +/** + * @brief Decode a gimbal_device_information message into a struct + * + * @param msg The message to decode + * @param gimbal_device_information C-struct to decode the message contents into + */ +static inline void mavlink_msg_gimbal_device_information_decode(const mavlink_message_t* msg, mavlink_gimbal_device_information_t* gimbal_device_information) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + gimbal_device_information->uid = mavlink_msg_gimbal_device_information_get_uid(msg); + gimbal_device_information->time_boot_ms = mavlink_msg_gimbal_device_information_get_time_boot_ms(msg); + gimbal_device_information->firmware_version = mavlink_msg_gimbal_device_information_get_firmware_version(msg); + gimbal_device_information->hardware_version = mavlink_msg_gimbal_device_information_get_hardware_version(msg); + gimbal_device_information->roll_min = mavlink_msg_gimbal_device_information_get_roll_min(msg); + gimbal_device_information->roll_max = mavlink_msg_gimbal_device_information_get_roll_max(msg); + gimbal_device_information->pitch_min = mavlink_msg_gimbal_device_information_get_pitch_min(msg); + gimbal_device_information->pitch_max = mavlink_msg_gimbal_device_information_get_pitch_max(msg); + gimbal_device_information->yaw_min = mavlink_msg_gimbal_device_information_get_yaw_min(msg); + gimbal_device_information->yaw_max = mavlink_msg_gimbal_device_information_get_yaw_max(msg); + gimbal_device_information->cap_flags = mavlink_msg_gimbal_device_information_get_cap_flags(msg); + gimbal_device_information->custom_cap_flags = mavlink_msg_gimbal_device_information_get_custom_cap_flags(msg); + mavlink_msg_gimbal_device_information_get_vendor_name(msg, gimbal_device_information->vendor_name); + mavlink_msg_gimbal_device_information_get_model_name(msg, gimbal_device_information->model_name); + mavlink_msg_gimbal_device_information_get_custom_name(msg, gimbal_device_information->custom_name); +#else + uint8_t len = msg->len < MAVLINK_MSG_ID_GIMBAL_DEVICE_INFORMATION_LEN? msg->len : MAVLINK_MSG_ID_GIMBAL_DEVICE_INFORMATION_LEN; + memset(gimbal_device_information, 0, MAVLINK_MSG_ID_GIMBAL_DEVICE_INFORMATION_LEN); + memcpy(gimbal_device_information, _MAV_PAYLOAD(msg), len); +#endif +} diff --git a/lib/main/MAVLink/common/mavlink_msg_gimbal_device_set_attitude.h b/lib/main/MAVLink/common/mavlink_msg_gimbal_device_set_attitude.h new file mode 100644 index 0000000000..1cfda32666 --- /dev/null +++ b/lib/main/MAVLink/common/mavlink_msg_gimbal_device_set_attitude.h @@ -0,0 +1,355 @@ +#pragma once +// MESSAGE GIMBAL_DEVICE_SET_ATTITUDE PACKING + +#define MAVLINK_MSG_ID_GIMBAL_DEVICE_SET_ATTITUDE 284 + + +typedef struct __mavlink_gimbal_device_set_attitude_t { + float q[4]; /*< Quaternion components, w, x, y, z (1 0 0 0 is the null-rotation, the frame is depends on whether the flag GIMBAL_DEVICE_FLAGS_YAW_LOCK is set, set all fields to NaN if only angular velocity should be used)*/ + float angular_velocity_x; /*< [rad/s] X component of angular velocity, positive is rolling to the right, NaN to be ignored.*/ + float angular_velocity_y; /*< [rad/s] Y component of angular velocity, positive is pitching up, NaN to be ignored.*/ + float angular_velocity_z; /*< [rad/s] Z component of angular velocity, positive is yawing to the right, NaN to be ignored.*/ + uint16_t flags; /*< Low level gimbal flags.*/ + uint8_t target_system; /*< System ID*/ + uint8_t target_component; /*< Component ID*/ +} mavlink_gimbal_device_set_attitude_t; + +#define MAVLINK_MSG_ID_GIMBAL_DEVICE_SET_ATTITUDE_LEN 32 +#define MAVLINK_MSG_ID_GIMBAL_DEVICE_SET_ATTITUDE_MIN_LEN 32 +#define MAVLINK_MSG_ID_284_LEN 32 +#define MAVLINK_MSG_ID_284_MIN_LEN 32 + +#define MAVLINK_MSG_ID_GIMBAL_DEVICE_SET_ATTITUDE_CRC 99 +#define MAVLINK_MSG_ID_284_CRC 99 + +#define MAVLINK_MSG_GIMBAL_DEVICE_SET_ATTITUDE_FIELD_Q_LEN 4 + +#if MAVLINK_COMMAND_24BIT +#define MAVLINK_MESSAGE_INFO_GIMBAL_DEVICE_SET_ATTITUDE { \ + 284, \ + "GIMBAL_DEVICE_SET_ATTITUDE", \ + 7, \ + { { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 30, offsetof(mavlink_gimbal_device_set_attitude_t, target_system) }, \ + { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 31, offsetof(mavlink_gimbal_device_set_attitude_t, target_component) }, \ + { "flags", NULL, MAVLINK_TYPE_UINT16_T, 0, 28, offsetof(mavlink_gimbal_device_set_attitude_t, flags) }, \ + { "q", NULL, MAVLINK_TYPE_FLOAT, 4, 0, offsetof(mavlink_gimbal_device_set_attitude_t, q) }, \ + { "angular_velocity_x", NULL, MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_gimbal_device_set_attitude_t, angular_velocity_x) }, \ + { "angular_velocity_y", NULL, MAVLINK_TYPE_FLOAT, 0, 20, offsetof(mavlink_gimbal_device_set_attitude_t, angular_velocity_y) }, \ + { "angular_velocity_z", NULL, MAVLINK_TYPE_FLOAT, 0, 24, offsetof(mavlink_gimbal_device_set_attitude_t, angular_velocity_z) }, \ + } \ +} +#else +#define MAVLINK_MESSAGE_INFO_GIMBAL_DEVICE_SET_ATTITUDE { \ + "GIMBAL_DEVICE_SET_ATTITUDE", \ + 7, \ + { { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 30, offsetof(mavlink_gimbal_device_set_attitude_t, target_system) }, \ + { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 31, offsetof(mavlink_gimbal_device_set_attitude_t, target_component) }, \ + { "flags", NULL, MAVLINK_TYPE_UINT16_T, 0, 28, offsetof(mavlink_gimbal_device_set_attitude_t, flags) }, \ + { "q", NULL, MAVLINK_TYPE_FLOAT, 4, 0, offsetof(mavlink_gimbal_device_set_attitude_t, q) }, \ + { "angular_velocity_x", NULL, MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_gimbal_device_set_attitude_t, angular_velocity_x) }, \ + { "angular_velocity_y", NULL, MAVLINK_TYPE_FLOAT, 0, 20, offsetof(mavlink_gimbal_device_set_attitude_t, angular_velocity_y) }, \ + { "angular_velocity_z", NULL, MAVLINK_TYPE_FLOAT, 0, 24, offsetof(mavlink_gimbal_device_set_attitude_t, angular_velocity_z) }, \ + } \ +} +#endif + +/** + * @brief Pack a gimbal_device_set_attitude message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param target_system System ID + * @param target_component Component ID + * @param flags Low level gimbal flags. + * @param q Quaternion components, w, x, y, z (1 0 0 0 is the null-rotation, the frame is depends on whether the flag GIMBAL_DEVICE_FLAGS_YAW_LOCK is set, set all fields to NaN if only angular velocity should be used) + * @param angular_velocity_x [rad/s] X component of angular velocity, positive is rolling to the right, NaN to be ignored. + * @param angular_velocity_y [rad/s] Y component of angular velocity, positive is pitching up, NaN to be ignored. + * @param angular_velocity_z [rad/s] Z component of angular velocity, positive is yawing to the right, NaN to be ignored. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_gimbal_device_set_attitude_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint8_t target_system, uint8_t target_component, uint16_t flags, const float *q, float angular_velocity_x, float angular_velocity_y, float angular_velocity_z) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_GIMBAL_DEVICE_SET_ATTITUDE_LEN]; + _mav_put_float(buf, 16, angular_velocity_x); + _mav_put_float(buf, 20, angular_velocity_y); + _mav_put_float(buf, 24, angular_velocity_z); + _mav_put_uint16_t(buf, 28, flags); + _mav_put_uint8_t(buf, 30, target_system); + _mav_put_uint8_t(buf, 31, target_component); + _mav_put_float_array(buf, 0, q, 4); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_GIMBAL_DEVICE_SET_ATTITUDE_LEN); +#else + mavlink_gimbal_device_set_attitude_t packet; + packet.angular_velocity_x = angular_velocity_x; + packet.angular_velocity_y = angular_velocity_y; + packet.angular_velocity_z = angular_velocity_z; + packet.flags = flags; + packet.target_system = target_system; + packet.target_component = target_component; + mav_array_memcpy(packet.q, q, sizeof(float)*4); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_GIMBAL_DEVICE_SET_ATTITUDE_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_GIMBAL_DEVICE_SET_ATTITUDE; + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_GIMBAL_DEVICE_SET_ATTITUDE_MIN_LEN, MAVLINK_MSG_ID_GIMBAL_DEVICE_SET_ATTITUDE_LEN, MAVLINK_MSG_ID_GIMBAL_DEVICE_SET_ATTITUDE_CRC); +} + +/** + * @brief Pack a gimbal_device_set_attitude message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param target_system System ID + * @param target_component Component ID + * @param flags Low level gimbal flags. + * @param q Quaternion components, w, x, y, z (1 0 0 0 is the null-rotation, the frame is depends on whether the flag GIMBAL_DEVICE_FLAGS_YAW_LOCK is set, set all fields to NaN if only angular velocity should be used) + * @param angular_velocity_x [rad/s] X component of angular velocity, positive is rolling to the right, NaN to be ignored. + * @param angular_velocity_y [rad/s] Y component of angular velocity, positive is pitching up, NaN to be ignored. + * @param angular_velocity_z [rad/s] Z component of angular velocity, positive is yawing to the right, NaN to be ignored. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_gimbal_device_set_attitude_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint8_t target_system,uint8_t target_component,uint16_t flags,const float *q,float angular_velocity_x,float angular_velocity_y,float angular_velocity_z) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_GIMBAL_DEVICE_SET_ATTITUDE_LEN]; + _mav_put_float(buf, 16, angular_velocity_x); + _mav_put_float(buf, 20, angular_velocity_y); + _mav_put_float(buf, 24, angular_velocity_z); + _mav_put_uint16_t(buf, 28, flags); + _mav_put_uint8_t(buf, 30, target_system); + _mav_put_uint8_t(buf, 31, target_component); + _mav_put_float_array(buf, 0, q, 4); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_GIMBAL_DEVICE_SET_ATTITUDE_LEN); +#else + mavlink_gimbal_device_set_attitude_t packet; + packet.angular_velocity_x = angular_velocity_x; + packet.angular_velocity_y = angular_velocity_y; + packet.angular_velocity_z = angular_velocity_z; + packet.flags = flags; + packet.target_system = target_system; + packet.target_component = target_component; + mav_array_memcpy(packet.q, q, sizeof(float)*4); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_GIMBAL_DEVICE_SET_ATTITUDE_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_GIMBAL_DEVICE_SET_ATTITUDE; + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_GIMBAL_DEVICE_SET_ATTITUDE_MIN_LEN, MAVLINK_MSG_ID_GIMBAL_DEVICE_SET_ATTITUDE_LEN, MAVLINK_MSG_ID_GIMBAL_DEVICE_SET_ATTITUDE_CRC); +} + +/** + * @brief Encode a gimbal_device_set_attitude struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param gimbal_device_set_attitude C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_gimbal_device_set_attitude_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_gimbal_device_set_attitude_t* gimbal_device_set_attitude) +{ + return mavlink_msg_gimbal_device_set_attitude_pack(system_id, component_id, msg, gimbal_device_set_attitude->target_system, gimbal_device_set_attitude->target_component, gimbal_device_set_attitude->flags, gimbal_device_set_attitude->q, gimbal_device_set_attitude->angular_velocity_x, gimbal_device_set_attitude->angular_velocity_y, gimbal_device_set_attitude->angular_velocity_z); +} + +/** + * @brief Encode a gimbal_device_set_attitude struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param gimbal_device_set_attitude C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_gimbal_device_set_attitude_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_gimbal_device_set_attitude_t* gimbal_device_set_attitude) +{ + return mavlink_msg_gimbal_device_set_attitude_pack_chan(system_id, component_id, chan, msg, gimbal_device_set_attitude->target_system, gimbal_device_set_attitude->target_component, gimbal_device_set_attitude->flags, gimbal_device_set_attitude->q, gimbal_device_set_attitude->angular_velocity_x, gimbal_device_set_attitude->angular_velocity_y, gimbal_device_set_attitude->angular_velocity_z); +} + +/** + * @brief Send a gimbal_device_set_attitude message + * @param chan MAVLink channel to send the message + * + * @param target_system System ID + * @param target_component Component ID + * @param flags Low level gimbal flags. + * @param q Quaternion components, w, x, y, z (1 0 0 0 is the null-rotation, the frame is depends on whether the flag GIMBAL_DEVICE_FLAGS_YAW_LOCK is set, set all fields to NaN if only angular velocity should be used) + * @param angular_velocity_x [rad/s] X component of angular velocity, positive is rolling to the right, NaN to be ignored. + * @param angular_velocity_y [rad/s] Y component of angular velocity, positive is pitching up, NaN to be ignored. + * @param angular_velocity_z [rad/s] Z component of angular velocity, positive is yawing to the right, NaN to be ignored. + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_gimbal_device_set_attitude_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint16_t flags, const float *q, float angular_velocity_x, float angular_velocity_y, float angular_velocity_z) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_GIMBAL_DEVICE_SET_ATTITUDE_LEN]; + _mav_put_float(buf, 16, angular_velocity_x); + _mav_put_float(buf, 20, angular_velocity_y); + _mav_put_float(buf, 24, angular_velocity_z); + _mav_put_uint16_t(buf, 28, flags); + _mav_put_uint8_t(buf, 30, target_system); + _mav_put_uint8_t(buf, 31, target_component); + _mav_put_float_array(buf, 0, q, 4); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GIMBAL_DEVICE_SET_ATTITUDE, buf, MAVLINK_MSG_ID_GIMBAL_DEVICE_SET_ATTITUDE_MIN_LEN, MAVLINK_MSG_ID_GIMBAL_DEVICE_SET_ATTITUDE_LEN, MAVLINK_MSG_ID_GIMBAL_DEVICE_SET_ATTITUDE_CRC); +#else + mavlink_gimbal_device_set_attitude_t packet; + packet.angular_velocity_x = angular_velocity_x; + packet.angular_velocity_y = angular_velocity_y; + packet.angular_velocity_z = angular_velocity_z; + packet.flags = flags; + packet.target_system = target_system; + packet.target_component = target_component; + mav_array_memcpy(packet.q, q, sizeof(float)*4); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GIMBAL_DEVICE_SET_ATTITUDE, (const char *)&packet, MAVLINK_MSG_ID_GIMBAL_DEVICE_SET_ATTITUDE_MIN_LEN, MAVLINK_MSG_ID_GIMBAL_DEVICE_SET_ATTITUDE_LEN, MAVLINK_MSG_ID_GIMBAL_DEVICE_SET_ATTITUDE_CRC); +#endif +} + +/** + * @brief Send a gimbal_device_set_attitude message + * @param chan MAVLink channel to send the message + * @param struct The MAVLink struct to serialize + */ +static inline void mavlink_msg_gimbal_device_set_attitude_send_struct(mavlink_channel_t chan, const mavlink_gimbal_device_set_attitude_t* gimbal_device_set_attitude) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + mavlink_msg_gimbal_device_set_attitude_send(chan, gimbal_device_set_attitude->target_system, gimbal_device_set_attitude->target_component, gimbal_device_set_attitude->flags, gimbal_device_set_attitude->q, gimbal_device_set_attitude->angular_velocity_x, gimbal_device_set_attitude->angular_velocity_y, gimbal_device_set_attitude->angular_velocity_z); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GIMBAL_DEVICE_SET_ATTITUDE, (const char *)gimbal_device_set_attitude, MAVLINK_MSG_ID_GIMBAL_DEVICE_SET_ATTITUDE_MIN_LEN, MAVLINK_MSG_ID_GIMBAL_DEVICE_SET_ATTITUDE_LEN, MAVLINK_MSG_ID_GIMBAL_DEVICE_SET_ATTITUDE_CRC); +#endif +} + +#if MAVLINK_MSG_ID_GIMBAL_DEVICE_SET_ATTITUDE_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_gimbal_device_set_attitude_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint16_t flags, const float *q, float angular_velocity_x, float angular_velocity_y, float angular_velocity_z) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_float(buf, 16, angular_velocity_x); + _mav_put_float(buf, 20, angular_velocity_y); + _mav_put_float(buf, 24, angular_velocity_z); + _mav_put_uint16_t(buf, 28, flags); + _mav_put_uint8_t(buf, 30, target_system); + _mav_put_uint8_t(buf, 31, target_component); + _mav_put_float_array(buf, 0, q, 4); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GIMBAL_DEVICE_SET_ATTITUDE, buf, MAVLINK_MSG_ID_GIMBAL_DEVICE_SET_ATTITUDE_MIN_LEN, MAVLINK_MSG_ID_GIMBAL_DEVICE_SET_ATTITUDE_LEN, MAVLINK_MSG_ID_GIMBAL_DEVICE_SET_ATTITUDE_CRC); +#else + mavlink_gimbal_device_set_attitude_t *packet = (mavlink_gimbal_device_set_attitude_t *)msgbuf; + packet->angular_velocity_x = angular_velocity_x; + packet->angular_velocity_y = angular_velocity_y; + packet->angular_velocity_z = angular_velocity_z; + packet->flags = flags; + packet->target_system = target_system; + packet->target_component = target_component; + mav_array_memcpy(packet->q, q, sizeof(float)*4); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GIMBAL_DEVICE_SET_ATTITUDE, (const char *)packet, MAVLINK_MSG_ID_GIMBAL_DEVICE_SET_ATTITUDE_MIN_LEN, MAVLINK_MSG_ID_GIMBAL_DEVICE_SET_ATTITUDE_LEN, MAVLINK_MSG_ID_GIMBAL_DEVICE_SET_ATTITUDE_CRC); +#endif +} +#endif + +#endif + +// MESSAGE GIMBAL_DEVICE_SET_ATTITUDE UNPACKING + + +/** + * @brief Get field target_system from gimbal_device_set_attitude message + * + * @return System ID + */ +static inline uint8_t mavlink_msg_gimbal_device_set_attitude_get_target_system(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 30); +} + +/** + * @brief Get field target_component from gimbal_device_set_attitude message + * + * @return Component ID + */ +static inline uint8_t mavlink_msg_gimbal_device_set_attitude_get_target_component(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 31); +} + +/** + * @brief Get field flags from gimbal_device_set_attitude message + * + * @return Low level gimbal flags. + */ +static inline uint16_t mavlink_msg_gimbal_device_set_attitude_get_flags(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 28); +} + +/** + * @brief Get field q from gimbal_device_set_attitude message + * + * @return Quaternion components, w, x, y, z (1 0 0 0 is the null-rotation, the frame is depends on whether the flag GIMBAL_DEVICE_FLAGS_YAW_LOCK is set, set all fields to NaN if only angular velocity should be used) + */ +static inline uint16_t mavlink_msg_gimbal_device_set_attitude_get_q(const mavlink_message_t* msg, float *q) +{ + return _MAV_RETURN_float_array(msg, q, 4, 0); +} + +/** + * @brief Get field angular_velocity_x from gimbal_device_set_attitude message + * + * @return [rad/s] X component of angular velocity, positive is rolling to the right, NaN to be ignored. + */ +static inline float mavlink_msg_gimbal_device_set_attitude_get_angular_velocity_x(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 16); +} + +/** + * @brief Get field angular_velocity_y from gimbal_device_set_attitude message + * + * @return [rad/s] Y component of angular velocity, positive is pitching up, NaN to be ignored. + */ +static inline float mavlink_msg_gimbal_device_set_attitude_get_angular_velocity_y(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 20); +} + +/** + * @brief Get field angular_velocity_z from gimbal_device_set_attitude message + * + * @return [rad/s] Z component of angular velocity, positive is yawing to the right, NaN to be ignored. + */ +static inline float mavlink_msg_gimbal_device_set_attitude_get_angular_velocity_z(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 24); +} + +/** + * @brief Decode a gimbal_device_set_attitude message into a struct + * + * @param msg The message to decode + * @param gimbal_device_set_attitude C-struct to decode the message contents into + */ +static inline void mavlink_msg_gimbal_device_set_attitude_decode(const mavlink_message_t* msg, mavlink_gimbal_device_set_attitude_t* gimbal_device_set_attitude) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + mavlink_msg_gimbal_device_set_attitude_get_q(msg, gimbal_device_set_attitude->q); + gimbal_device_set_attitude->angular_velocity_x = mavlink_msg_gimbal_device_set_attitude_get_angular_velocity_x(msg); + gimbal_device_set_attitude->angular_velocity_y = mavlink_msg_gimbal_device_set_attitude_get_angular_velocity_y(msg); + gimbal_device_set_attitude->angular_velocity_z = mavlink_msg_gimbal_device_set_attitude_get_angular_velocity_z(msg); + gimbal_device_set_attitude->flags = mavlink_msg_gimbal_device_set_attitude_get_flags(msg); + gimbal_device_set_attitude->target_system = mavlink_msg_gimbal_device_set_attitude_get_target_system(msg); + gimbal_device_set_attitude->target_component = mavlink_msg_gimbal_device_set_attitude_get_target_component(msg); +#else + uint8_t len = msg->len < MAVLINK_MSG_ID_GIMBAL_DEVICE_SET_ATTITUDE_LEN? msg->len : MAVLINK_MSG_ID_GIMBAL_DEVICE_SET_ATTITUDE_LEN; + memset(gimbal_device_set_attitude, 0, MAVLINK_MSG_ID_GIMBAL_DEVICE_SET_ATTITUDE_LEN); + memcpy(gimbal_device_set_attitude, _MAV_PAYLOAD(msg), len); +#endif +} diff --git a/lib/main/MAVLink/common/mavlink_msg_gimbal_manager_information.h b/lib/main/MAVLink/common/mavlink_msg_gimbal_manager_information.h new file mode 100644 index 0000000000..58c8c59d97 --- /dev/null +++ b/lib/main/MAVLink/common/mavlink_msg_gimbal_manager_information.h @@ -0,0 +1,413 @@ +#pragma once +// MESSAGE GIMBAL_MANAGER_INFORMATION PACKING + +#define MAVLINK_MSG_ID_GIMBAL_MANAGER_INFORMATION 280 + + +typedef struct __mavlink_gimbal_manager_information_t { + uint32_t time_boot_ms; /*< [ms] Timestamp (time since system boot).*/ + uint32_t cap_flags; /*< Bitmap of gimbal capability flags.*/ + float roll_min; /*< [rad] Minimum hardware roll angle (positive: rolling to the right, negative: rolling to the left)*/ + float roll_max; /*< [rad] Maximum hardware roll angle (positive: rolling to the right, negative: rolling to the left)*/ + float pitch_min; /*< [rad] Minimum pitch angle (positive: up, negative: down)*/ + float pitch_max; /*< [rad] Maximum pitch angle (positive: up, negative: down)*/ + float yaw_min; /*< [rad] Minimum yaw angle (positive: to the right, negative: to the left)*/ + float yaw_max; /*< [rad] Maximum yaw angle (positive: to the right, negative: to the left)*/ + uint8_t gimbal_device_id; /*< Gimbal device ID that this gimbal manager is responsible for.*/ +} mavlink_gimbal_manager_information_t; + +#define MAVLINK_MSG_ID_GIMBAL_MANAGER_INFORMATION_LEN 33 +#define MAVLINK_MSG_ID_GIMBAL_MANAGER_INFORMATION_MIN_LEN 33 +#define MAVLINK_MSG_ID_280_LEN 33 +#define MAVLINK_MSG_ID_280_MIN_LEN 33 + +#define MAVLINK_MSG_ID_GIMBAL_MANAGER_INFORMATION_CRC 70 +#define MAVLINK_MSG_ID_280_CRC 70 + + + +#if MAVLINK_COMMAND_24BIT +#define MAVLINK_MESSAGE_INFO_GIMBAL_MANAGER_INFORMATION { \ + 280, \ + "GIMBAL_MANAGER_INFORMATION", \ + 9, \ + { { "time_boot_ms", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_gimbal_manager_information_t, time_boot_ms) }, \ + { "cap_flags", NULL, MAVLINK_TYPE_UINT32_T, 0, 4, offsetof(mavlink_gimbal_manager_information_t, cap_flags) }, \ + { "gimbal_device_id", NULL, MAVLINK_TYPE_UINT8_T, 0, 32, offsetof(mavlink_gimbal_manager_information_t, gimbal_device_id) }, \ + { "roll_min", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_gimbal_manager_information_t, roll_min) }, \ + { "roll_max", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_gimbal_manager_information_t, roll_max) }, \ + { "pitch_min", NULL, MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_gimbal_manager_information_t, pitch_min) }, \ + { "pitch_max", NULL, MAVLINK_TYPE_FLOAT, 0, 20, offsetof(mavlink_gimbal_manager_information_t, pitch_max) }, \ + { "yaw_min", NULL, MAVLINK_TYPE_FLOAT, 0, 24, offsetof(mavlink_gimbal_manager_information_t, yaw_min) }, \ + { "yaw_max", NULL, MAVLINK_TYPE_FLOAT, 0, 28, offsetof(mavlink_gimbal_manager_information_t, yaw_max) }, \ + } \ +} +#else +#define MAVLINK_MESSAGE_INFO_GIMBAL_MANAGER_INFORMATION { \ + "GIMBAL_MANAGER_INFORMATION", \ + 9, \ + { { "time_boot_ms", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_gimbal_manager_information_t, time_boot_ms) }, \ + { "cap_flags", NULL, MAVLINK_TYPE_UINT32_T, 0, 4, offsetof(mavlink_gimbal_manager_information_t, cap_flags) }, \ + { "gimbal_device_id", NULL, MAVLINK_TYPE_UINT8_T, 0, 32, offsetof(mavlink_gimbal_manager_information_t, gimbal_device_id) }, \ + { "roll_min", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_gimbal_manager_information_t, roll_min) }, \ + { "roll_max", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_gimbal_manager_information_t, roll_max) }, \ + { "pitch_min", NULL, MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_gimbal_manager_information_t, pitch_min) }, \ + { "pitch_max", NULL, MAVLINK_TYPE_FLOAT, 0, 20, offsetof(mavlink_gimbal_manager_information_t, pitch_max) }, \ + { "yaw_min", NULL, MAVLINK_TYPE_FLOAT, 0, 24, offsetof(mavlink_gimbal_manager_information_t, yaw_min) }, \ + { "yaw_max", NULL, MAVLINK_TYPE_FLOAT, 0, 28, offsetof(mavlink_gimbal_manager_information_t, yaw_max) }, \ + } \ +} +#endif + +/** + * @brief Pack a gimbal_manager_information message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param time_boot_ms [ms] Timestamp (time since system boot). + * @param cap_flags Bitmap of gimbal capability flags. + * @param gimbal_device_id Gimbal device ID that this gimbal manager is responsible for. + * @param roll_min [rad] Minimum hardware roll angle (positive: rolling to the right, negative: rolling to the left) + * @param roll_max [rad] Maximum hardware roll angle (positive: rolling to the right, negative: rolling to the left) + * @param pitch_min [rad] Minimum pitch angle (positive: up, negative: down) + * @param pitch_max [rad] Maximum pitch angle (positive: up, negative: down) + * @param yaw_min [rad] Minimum yaw angle (positive: to the right, negative: to the left) + * @param yaw_max [rad] Maximum yaw angle (positive: to the right, negative: to the left) + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_gimbal_manager_information_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint32_t time_boot_ms, uint32_t cap_flags, uint8_t gimbal_device_id, float roll_min, float roll_max, float pitch_min, float pitch_max, float yaw_min, float yaw_max) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_GIMBAL_MANAGER_INFORMATION_LEN]; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_uint32_t(buf, 4, cap_flags); + _mav_put_float(buf, 8, roll_min); + _mav_put_float(buf, 12, roll_max); + _mav_put_float(buf, 16, pitch_min); + _mav_put_float(buf, 20, pitch_max); + _mav_put_float(buf, 24, yaw_min); + _mav_put_float(buf, 28, yaw_max); + _mav_put_uint8_t(buf, 32, gimbal_device_id); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_GIMBAL_MANAGER_INFORMATION_LEN); +#else + mavlink_gimbal_manager_information_t packet; + packet.time_boot_ms = time_boot_ms; + packet.cap_flags = cap_flags; + packet.roll_min = roll_min; + packet.roll_max = roll_max; + packet.pitch_min = pitch_min; + packet.pitch_max = pitch_max; + packet.yaw_min = yaw_min; + packet.yaw_max = yaw_max; + packet.gimbal_device_id = gimbal_device_id; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_GIMBAL_MANAGER_INFORMATION_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_GIMBAL_MANAGER_INFORMATION; + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_GIMBAL_MANAGER_INFORMATION_MIN_LEN, MAVLINK_MSG_ID_GIMBAL_MANAGER_INFORMATION_LEN, MAVLINK_MSG_ID_GIMBAL_MANAGER_INFORMATION_CRC); +} + +/** + * @brief Pack a gimbal_manager_information message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param time_boot_ms [ms] Timestamp (time since system boot). + * @param cap_flags Bitmap of gimbal capability flags. + * @param gimbal_device_id Gimbal device ID that this gimbal manager is responsible for. + * @param roll_min [rad] Minimum hardware roll angle (positive: rolling to the right, negative: rolling to the left) + * @param roll_max [rad] Maximum hardware roll angle (positive: rolling to the right, negative: rolling to the left) + * @param pitch_min [rad] Minimum pitch angle (positive: up, negative: down) + * @param pitch_max [rad] Maximum pitch angle (positive: up, negative: down) + * @param yaw_min [rad] Minimum yaw angle (positive: to the right, negative: to the left) + * @param yaw_max [rad] Maximum yaw angle (positive: to the right, negative: to the left) + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_gimbal_manager_information_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint32_t time_boot_ms,uint32_t cap_flags,uint8_t gimbal_device_id,float roll_min,float roll_max,float pitch_min,float pitch_max,float yaw_min,float yaw_max) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_GIMBAL_MANAGER_INFORMATION_LEN]; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_uint32_t(buf, 4, cap_flags); + _mav_put_float(buf, 8, roll_min); + _mav_put_float(buf, 12, roll_max); + _mav_put_float(buf, 16, pitch_min); + _mav_put_float(buf, 20, pitch_max); + _mav_put_float(buf, 24, yaw_min); + _mav_put_float(buf, 28, yaw_max); + _mav_put_uint8_t(buf, 32, gimbal_device_id); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_GIMBAL_MANAGER_INFORMATION_LEN); +#else + mavlink_gimbal_manager_information_t packet; + packet.time_boot_ms = time_boot_ms; + packet.cap_flags = cap_flags; + packet.roll_min = roll_min; + packet.roll_max = roll_max; + packet.pitch_min = pitch_min; + packet.pitch_max = pitch_max; + packet.yaw_min = yaw_min; + packet.yaw_max = yaw_max; + packet.gimbal_device_id = gimbal_device_id; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_GIMBAL_MANAGER_INFORMATION_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_GIMBAL_MANAGER_INFORMATION; + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_GIMBAL_MANAGER_INFORMATION_MIN_LEN, MAVLINK_MSG_ID_GIMBAL_MANAGER_INFORMATION_LEN, MAVLINK_MSG_ID_GIMBAL_MANAGER_INFORMATION_CRC); +} + +/** + * @brief Encode a gimbal_manager_information struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param gimbal_manager_information C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_gimbal_manager_information_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_gimbal_manager_information_t* gimbal_manager_information) +{ + return mavlink_msg_gimbal_manager_information_pack(system_id, component_id, msg, gimbal_manager_information->time_boot_ms, gimbal_manager_information->cap_flags, gimbal_manager_information->gimbal_device_id, gimbal_manager_information->roll_min, gimbal_manager_information->roll_max, gimbal_manager_information->pitch_min, gimbal_manager_information->pitch_max, gimbal_manager_information->yaw_min, gimbal_manager_information->yaw_max); +} + +/** + * @brief Encode a gimbal_manager_information struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param gimbal_manager_information C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_gimbal_manager_information_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_gimbal_manager_information_t* gimbal_manager_information) +{ + return mavlink_msg_gimbal_manager_information_pack_chan(system_id, component_id, chan, msg, gimbal_manager_information->time_boot_ms, gimbal_manager_information->cap_flags, gimbal_manager_information->gimbal_device_id, gimbal_manager_information->roll_min, gimbal_manager_information->roll_max, gimbal_manager_information->pitch_min, gimbal_manager_information->pitch_max, gimbal_manager_information->yaw_min, gimbal_manager_information->yaw_max); +} + +/** + * @brief Send a gimbal_manager_information message + * @param chan MAVLink channel to send the message + * + * @param time_boot_ms [ms] Timestamp (time since system boot). + * @param cap_flags Bitmap of gimbal capability flags. + * @param gimbal_device_id Gimbal device ID that this gimbal manager is responsible for. + * @param roll_min [rad] Minimum hardware roll angle (positive: rolling to the right, negative: rolling to the left) + * @param roll_max [rad] Maximum hardware roll angle (positive: rolling to the right, negative: rolling to the left) + * @param pitch_min [rad] Minimum pitch angle (positive: up, negative: down) + * @param pitch_max [rad] Maximum pitch angle (positive: up, negative: down) + * @param yaw_min [rad] Minimum yaw angle (positive: to the right, negative: to the left) + * @param yaw_max [rad] Maximum yaw angle (positive: to the right, negative: to the left) + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_gimbal_manager_information_send(mavlink_channel_t chan, uint32_t time_boot_ms, uint32_t cap_flags, uint8_t gimbal_device_id, float roll_min, float roll_max, float pitch_min, float pitch_max, float yaw_min, float yaw_max) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_GIMBAL_MANAGER_INFORMATION_LEN]; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_uint32_t(buf, 4, cap_flags); + _mav_put_float(buf, 8, roll_min); + _mav_put_float(buf, 12, roll_max); + _mav_put_float(buf, 16, pitch_min); + _mav_put_float(buf, 20, pitch_max); + _mav_put_float(buf, 24, yaw_min); + _mav_put_float(buf, 28, yaw_max); + _mav_put_uint8_t(buf, 32, gimbal_device_id); + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GIMBAL_MANAGER_INFORMATION, buf, MAVLINK_MSG_ID_GIMBAL_MANAGER_INFORMATION_MIN_LEN, MAVLINK_MSG_ID_GIMBAL_MANAGER_INFORMATION_LEN, MAVLINK_MSG_ID_GIMBAL_MANAGER_INFORMATION_CRC); +#else + mavlink_gimbal_manager_information_t packet; + packet.time_boot_ms = time_boot_ms; + packet.cap_flags = cap_flags; + packet.roll_min = roll_min; + packet.roll_max = roll_max; + packet.pitch_min = pitch_min; + packet.pitch_max = pitch_max; + packet.yaw_min = yaw_min; + packet.yaw_max = yaw_max; + packet.gimbal_device_id = gimbal_device_id; + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GIMBAL_MANAGER_INFORMATION, (const char *)&packet, MAVLINK_MSG_ID_GIMBAL_MANAGER_INFORMATION_MIN_LEN, MAVLINK_MSG_ID_GIMBAL_MANAGER_INFORMATION_LEN, MAVLINK_MSG_ID_GIMBAL_MANAGER_INFORMATION_CRC); +#endif +} + +/** + * @brief Send a gimbal_manager_information message + * @param chan MAVLink channel to send the message + * @param struct The MAVLink struct to serialize + */ +static inline void mavlink_msg_gimbal_manager_information_send_struct(mavlink_channel_t chan, const mavlink_gimbal_manager_information_t* gimbal_manager_information) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + mavlink_msg_gimbal_manager_information_send(chan, gimbal_manager_information->time_boot_ms, gimbal_manager_information->cap_flags, gimbal_manager_information->gimbal_device_id, gimbal_manager_information->roll_min, gimbal_manager_information->roll_max, gimbal_manager_information->pitch_min, gimbal_manager_information->pitch_max, gimbal_manager_information->yaw_min, gimbal_manager_information->yaw_max); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GIMBAL_MANAGER_INFORMATION, (const char *)gimbal_manager_information, MAVLINK_MSG_ID_GIMBAL_MANAGER_INFORMATION_MIN_LEN, MAVLINK_MSG_ID_GIMBAL_MANAGER_INFORMATION_LEN, MAVLINK_MSG_ID_GIMBAL_MANAGER_INFORMATION_CRC); +#endif +} + +#if MAVLINK_MSG_ID_GIMBAL_MANAGER_INFORMATION_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_gimbal_manager_information_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint32_t time_boot_ms, uint32_t cap_flags, uint8_t gimbal_device_id, float roll_min, float roll_max, float pitch_min, float pitch_max, float yaw_min, float yaw_max) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_uint32_t(buf, 4, cap_flags); + _mav_put_float(buf, 8, roll_min); + _mav_put_float(buf, 12, roll_max); + _mav_put_float(buf, 16, pitch_min); + _mav_put_float(buf, 20, pitch_max); + _mav_put_float(buf, 24, yaw_min); + _mav_put_float(buf, 28, yaw_max); + _mav_put_uint8_t(buf, 32, gimbal_device_id); + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GIMBAL_MANAGER_INFORMATION, buf, MAVLINK_MSG_ID_GIMBAL_MANAGER_INFORMATION_MIN_LEN, MAVLINK_MSG_ID_GIMBAL_MANAGER_INFORMATION_LEN, MAVLINK_MSG_ID_GIMBAL_MANAGER_INFORMATION_CRC); +#else + mavlink_gimbal_manager_information_t *packet = (mavlink_gimbal_manager_information_t *)msgbuf; + packet->time_boot_ms = time_boot_ms; + packet->cap_flags = cap_flags; + packet->roll_min = roll_min; + packet->roll_max = roll_max; + packet->pitch_min = pitch_min; + packet->pitch_max = pitch_max; + packet->yaw_min = yaw_min; + packet->yaw_max = yaw_max; + packet->gimbal_device_id = gimbal_device_id; + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GIMBAL_MANAGER_INFORMATION, (const char *)packet, MAVLINK_MSG_ID_GIMBAL_MANAGER_INFORMATION_MIN_LEN, MAVLINK_MSG_ID_GIMBAL_MANAGER_INFORMATION_LEN, MAVLINK_MSG_ID_GIMBAL_MANAGER_INFORMATION_CRC); +#endif +} +#endif + +#endif + +// MESSAGE GIMBAL_MANAGER_INFORMATION UNPACKING + + +/** + * @brief Get field time_boot_ms from gimbal_manager_information message + * + * @return [ms] Timestamp (time since system boot). + */ +static inline uint32_t mavlink_msg_gimbal_manager_information_get_time_boot_ms(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint32_t(msg, 0); +} + +/** + * @brief Get field cap_flags from gimbal_manager_information message + * + * @return Bitmap of gimbal capability flags. + */ +static inline uint32_t mavlink_msg_gimbal_manager_information_get_cap_flags(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint32_t(msg, 4); +} + +/** + * @brief Get field gimbal_device_id from gimbal_manager_information message + * + * @return Gimbal device ID that this gimbal manager is responsible for. + */ +static inline uint8_t mavlink_msg_gimbal_manager_information_get_gimbal_device_id(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 32); +} + +/** + * @brief Get field roll_min from gimbal_manager_information message + * + * @return [rad] Minimum hardware roll angle (positive: rolling to the right, negative: rolling to the left) + */ +static inline float mavlink_msg_gimbal_manager_information_get_roll_min(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 8); +} + +/** + * @brief Get field roll_max from gimbal_manager_information message + * + * @return [rad] Maximum hardware roll angle (positive: rolling to the right, negative: rolling to the left) + */ +static inline float mavlink_msg_gimbal_manager_information_get_roll_max(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 12); +} + +/** + * @brief Get field pitch_min from gimbal_manager_information message + * + * @return [rad] Minimum pitch angle (positive: up, negative: down) + */ +static inline float mavlink_msg_gimbal_manager_information_get_pitch_min(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 16); +} + +/** + * @brief Get field pitch_max from gimbal_manager_information message + * + * @return [rad] Maximum pitch angle (positive: up, negative: down) + */ +static inline float mavlink_msg_gimbal_manager_information_get_pitch_max(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 20); +} + +/** + * @brief Get field yaw_min from gimbal_manager_information message + * + * @return [rad] Minimum yaw angle (positive: to the right, negative: to the left) + */ +static inline float mavlink_msg_gimbal_manager_information_get_yaw_min(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 24); +} + +/** + * @brief Get field yaw_max from gimbal_manager_information message + * + * @return [rad] Maximum yaw angle (positive: to the right, negative: to the left) + */ +static inline float mavlink_msg_gimbal_manager_information_get_yaw_max(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 28); +} + +/** + * @brief Decode a gimbal_manager_information message into a struct + * + * @param msg The message to decode + * @param gimbal_manager_information C-struct to decode the message contents into + */ +static inline void mavlink_msg_gimbal_manager_information_decode(const mavlink_message_t* msg, mavlink_gimbal_manager_information_t* gimbal_manager_information) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + gimbal_manager_information->time_boot_ms = mavlink_msg_gimbal_manager_information_get_time_boot_ms(msg); + gimbal_manager_information->cap_flags = mavlink_msg_gimbal_manager_information_get_cap_flags(msg); + gimbal_manager_information->roll_min = mavlink_msg_gimbal_manager_information_get_roll_min(msg); + gimbal_manager_information->roll_max = mavlink_msg_gimbal_manager_information_get_roll_max(msg); + gimbal_manager_information->pitch_min = mavlink_msg_gimbal_manager_information_get_pitch_min(msg); + gimbal_manager_information->pitch_max = mavlink_msg_gimbal_manager_information_get_pitch_max(msg); + gimbal_manager_information->yaw_min = mavlink_msg_gimbal_manager_information_get_yaw_min(msg); + gimbal_manager_information->yaw_max = mavlink_msg_gimbal_manager_information_get_yaw_max(msg); + gimbal_manager_information->gimbal_device_id = mavlink_msg_gimbal_manager_information_get_gimbal_device_id(msg); +#else + uint8_t len = msg->len < MAVLINK_MSG_ID_GIMBAL_MANAGER_INFORMATION_LEN? msg->len : MAVLINK_MSG_ID_GIMBAL_MANAGER_INFORMATION_LEN; + memset(gimbal_manager_information, 0, MAVLINK_MSG_ID_GIMBAL_MANAGER_INFORMATION_LEN); + memcpy(gimbal_manager_information, _MAV_PAYLOAD(msg), len); +#endif +} diff --git a/lib/main/MAVLink/common/mavlink_msg_gimbal_manager_set_attitude.h b/lib/main/MAVLink/common/mavlink_msg_gimbal_manager_set_attitude.h new file mode 100644 index 0000000000..828f1e6a3f --- /dev/null +++ b/lib/main/MAVLink/common/mavlink_msg_gimbal_manager_set_attitude.h @@ -0,0 +1,380 @@ +#pragma once +// MESSAGE GIMBAL_MANAGER_SET_ATTITUDE PACKING + +#define MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_ATTITUDE 282 + + +typedef struct __mavlink_gimbal_manager_set_attitude_t { + uint32_t flags; /*< High level gimbal manager flags to use.*/ + float q[4]; /*< Quaternion components, w, x, y, z (1 0 0 0 is the null-rotation, the frame is depends on whether the flag GIMBAL_MANAGER_FLAGS_YAW_LOCK is set)*/ + float angular_velocity_x; /*< [rad/s] X component of angular velocity, positive is rolling to the right, NaN to be ignored.*/ + float angular_velocity_y; /*< [rad/s] Y component of angular velocity, positive is pitching up, NaN to be ignored.*/ + float angular_velocity_z; /*< [rad/s] Z component of angular velocity, positive is yawing to the right, NaN to be ignored.*/ + uint8_t target_system; /*< System ID*/ + uint8_t target_component; /*< Component ID*/ + uint8_t gimbal_device_id; /*< Component ID of gimbal device to address (or 1-6 for non-MAVLink gimbal), 0 for all gimbal device components. Send command multiple times for more than one gimbal (but not all gimbals).*/ +} mavlink_gimbal_manager_set_attitude_t; + +#define MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_ATTITUDE_LEN 35 +#define MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_ATTITUDE_MIN_LEN 35 +#define MAVLINK_MSG_ID_282_LEN 35 +#define MAVLINK_MSG_ID_282_MIN_LEN 35 + +#define MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_ATTITUDE_CRC 123 +#define MAVLINK_MSG_ID_282_CRC 123 + +#define MAVLINK_MSG_GIMBAL_MANAGER_SET_ATTITUDE_FIELD_Q_LEN 4 + +#if MAVLINK_COMMAND_24BIT +#define MAVLINK_MESSAGE_INFO_GIMBAL_MANAGER_SET_ATTITUDE { \ + 282, \ + "GIMBAL_MANAGER_SET_ATTITUDE", \ + 8, \ + { { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 32, offsetof(mavlink_gimbal_manager_set_attitude_t, target_system) }, \ + { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 33, offsetof(mavlink_gimbal_manager_set_attitude_t, target_component) }, \ + { "flags", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_gimbal_manager_set_attitude_t, flags) }, \ + { "gimbal_device_id", NULL, MAVLINK_TYPE_UINT8_T, 0, 34, offsetof(mavlink_gimbal_manager_set_attitude_t, gimbal_device_id) }, \ + { "q", NULL, MAVLINK_TYPE_FLOAT, 4, 4, offsetof(mavlink_gimbal_manager_set_attitude_t, q) }, \ + { "angular_velocity_x", NULL, MAVLINK_TYPE_FLOAT, 0, 20, offsetof(mavlink_gimbal_manager_set_attitude_t, angular_velocity_x) }, \ + { "angular_velocity_y", NULL, MAVLINK_TYPE_FLOAT, 0, 24, offsetof(mavlink_gimbal_manager_set_attitude_t, angular_velocity_y) }, \ + { "angular_velocity_z", NULL, MAVLINK_TYPE_FLOAT, 0, 28, offsetof(mavlink_gimbal_manager_set_attitude_t, angular_velocity_z) }, \ + } \ +} +#else +#define MAVLINK_MESSAGE_INFO_GIMBAL_MANAGER_SET_ATTITUDE { \ + "GIMBAL_MANAGER_SET_ATTITUDE", \ + 8, \ + { { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 32, offsetof(mavlink_gimbal_manager_set_attitude_t, target_system) }, \ + { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 33, offsetof(mavlink_gimbal_manager_set_attitude_t, target_component) }, \ + { "flags", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_gimbal_manager_set_attitude_t, flags) }, \ + { "gimbal_device_id", NULL, MAVLINK_TYPE_UINT8_T, 0, 34, offsetof(mavlink_gimbal_manager_set_attitude_t, gimbal_device_id) }, \ + { "q", NULL, MAVLINK_TYPE_FLOAT, 4, 4, offsetof(mavlink_gimbal_manager_set_attitude_t, q) }, \ + { "angular_velocity_x", NULL, MAVLINK_TYPE_FLOAT, 0, 20, offsetof(mavlink_gimbal_manager_set_attitude_t, angular_velocity_x) }, \ + { "angular_velocity_y", NULL, MAVLINK_TYPE_FLOAT, 0, 24, offsetof(mavlink_gimbal_manager_set_attitude_t, angular_velocity_y) }, \ + { "angular_velocity_z", NULL, MAVLINK_TYPE_FLOAT, 0, 28, offsetof(mavlink_gimbal_manager_set_attitude_t, angular_velocity_z) }, \ + } \ +} +#endif + +/** + * @brief Pack a gimbal_manager_set_attitude message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param target_system System ID + * @param target_component Component ID + * @param flags High level gimbal manager flags to use. + * @param gimbal_device_id Component ID of gimbal device to address (or 1-6 for non-MAVLink gimbal), 0 for all gimbal device components. Send command multiple times for more than one gimbal (but not all gimbals). + * @param q Quaternion components, w, x, y, z (1 0 0 0 is the null-rotation, the frame is depends on whether the flag GIMBAL_MANAGER_FLAGS_YAW_LOCK is set) + * @param angular_velocity_x [rad/s] X component of angular velocity, positive is rolling to the right, NaN to be ignored. + * @param angular_velocity_y [rad/s] Y component of angular velocity, positive is pitching up, NaN to be ignored. + * @param angular_velocity_z [rad/s] Z component of angular velocity, positive is yawing to the right, NaN to be ignored. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_gimbal_manager_set_attitude_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint8_t target_system, uint8_t target_component, uint32_t flags, uint8_t gimbal_device_id, const float *q, float angular_velocity_x, float angular_velocity_y, float angular_velocity_z) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_ATTITUDE_LEN]; + _mav_put_uint32_t(buf, 0, flags); + _mav_put_float(buf, 20, angular_velocity_x); + _mav_put_float(buf, 24, angular_velocity_y); + _mav_put_float(buf, 28, angular_velocity_z); + _mav_put_uint8_t(buf, 32, target_system); + _mav_put_uint8_t(buf, 33, target_component); + _mav_put_uint8_t(buf, 34, gimbal_device_id); + _mav_put_float_array(buf, 4, q, 4); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_ATTITUDE_LEN); +#else + mavlink_gimbal_manager_set_attitude_t packet; + packet.flags = flags; + packet.angular_velocity_x = angular_velocity_x; + packet.angular_velocity_y = angular_velocity_y; + packet.angular_velocity_z = angular_velocity_z; + packet.target_system = target_system; + packet.target_component = target_component; + packet.gimbal_device_id = gimbal_device_id; + mav_array_memcpy(packet.q, q, sizeof(float)*4); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_ATTITUDE_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_ATTITUDE; + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_ATTITUDE_MIN_LEN, MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_ATTITUDE_LEN, MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_ATTITUDE_CRC); +} + +/** + * @brief Pack a gimbal_manager_set_attitude message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param target_system System ID + * @param target_component Component ID + * @param flags High level gimbal manager flags to use. + * @param gimbal_device_id Component ID of gimbal device to address (or 1-6 for non-MAVLink gimbal), 0 for all gimbal device components. Send command multiple times for more than one gimbal (but not all gimbals). + * @param q Quaternion components, w, x, y, z (1 0 0 0 is the null-rotation, the frame is depends on whether the flag GIMBAL_MANAGER_FLAGS_YAW_LOCK is set) + * @param angular_velocity_x [rad/s] X component of angular velocity, positive is rolling to the right, NaN to be ignored. + * @param angular_velocity_y [rad/s] Y component of angular velocity, positive is pitching up, NaN to be ignored. + * @param angular_velocity_z [rad/s] Z component of angular velocity, positive is yawing to the right, NaN to be ignored. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_gimbal_manager_set_attitude_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint8_t target_system,uint8_t target_component,uint32_t flags,uint8_t gimbal_device_id,const float *q,float angular_velocity_x,float angular_velocity_y,float angular_velocity_z) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_ATTITUDE_LEN]; + _mav_put_uint32_t(buf, 0, flags); + _mav_put_float(buf, 20, angular_velocity_x); + _mav_put_float(buf, 24, angular_velocity_y); + _mav_put_float(buf, 28, angular_velocity_z); + _mav_put_uint8_t(buf, 32, target_system); + _mav_put_uint8_t(buf, 33, target_component); + _mav_put_uint8_t(buf, 34, gimbal_device_id); + _mav_put_float_array(buf, 4, q, 4); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_ATTITUDE_LEN); +#else + mavlink_gimbal_manager_set_attitude_t packet; + packet.flags = flags; + packet.angular_velocity_x = angular_velocity_x; + packet.angular_velocity_y = angular_velocity_y; + packet.angular_velocity_z = angular_velocity_z; + packet.target_system = target_system; + packet.target_component = target_component; + packet.gimbal_device_id = gimbal_device_id; + mav_array_memcpy(packet.q, q, sizeof(float)*4); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_ATTITUDE_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_ATTITUDE; + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_ATTITUDE_MIN_LEN, MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_ATTITUDE_LEN, MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_ATTITUDE_CRC); +} + +/** + * @brief Encode a gimbal_manager_set_attitude struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param gimbal_manager_set_attitude C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_gimbal_manager_set_attitude_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_gimbal_manager_set_attitude_t* gimbal_manager_set_attitude) +{ + return mavlink_msg_gimbal_manager_set_attitude_pack(system_id, component_id, msg, gimbal_manager_set_attitude->target_system, gimbal_manager_set_attitude->target_component, gimbal_manager_set_attitude->flags, gimbal_manager_set_attitude->gimbal_device_id, gimbal_manager_set_attitude->q, gimbal_manager_set_attitude->angular_velocity_x, gimbal_manager_set_attitude->angular_velocity_y, gimbal_manager_set_attitude->angular_velocity_z); +} + +/** + * @brief Encode a gimbal_manager_set_attitude struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param gimbal_manager_set_attitude C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_gimbal_manager_set_attitude_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_gimbal_manager_set_attitude_t* gimbal_manager_set_attitude) +{ + return mavlink_msg_gimbal_manager_set_attitude_pack_chan(system_id, component_id, chan, msg, gimbal_manager_set_attitude->target_system, gimbal_manager_set_attitude->target_component, gimbal_manager_set_attitude->flags, gimbal_manager_set_attitude->gimbal_device_id, gimbal_manager_set_attitude->q, gimbal_manager_set_attitude->angular_velocity_x, gimbal_manager_set_attitude->angular_velocity_y, gimbal_manager_set_attitude->angular_velocity_z); +} + +/** + * @brief Send a gimbal_manager_set_attitude message + * @param chan MAVLink channel to send the message + * + * @param target_system System ID + * @param target_component Component ID + * @param flags High level gimbal manager flags to use. + * @param gimbal_device_id Component ID of gimbal device to address (or 1-6 for non-MAVLink gimbal), 0 for all gimbal device components. Send command multiple times for more than one gimbal (but not all gimbals). + * @param q Quaternion components, w, x, y, z (1 0 0 0 is the null-rotation, the frame is depends on whether the flag GIMBAL_MANAGER_FLAGS_YAW_LOCK is set) + * @param angular_velocity_x [rad/s] X component of angular velocity, positive is rolling to the right, NaN to be ignored. + * @param angular_velocity_y [rad/s] Y component of angular velocity, positive is pitching up, NaN to be ignored. + * @param angular_velocity_z [rad/s] Z component of angular velocity, positive is yawing to the right, NaN to be ignored. + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_gimbal_manager_set_attitude_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint32_t flags, uint8_t gimbal_device_id, const float *q, float angular_velocity_x, float angular_velocity_y, float angular_velocity_z) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_ATTITUDE_LEN]; + _mav_put_uint32_t(buf, 0, flags); + _mav_put_float(buf, 20, angular_velocity_x); + _mav_put_float(buf, 24, angular_velocity_y); + _mav_put_float(buf, 28, angular_velocity_z); + _mav_put_uint8_t(buf, 32, target_system); + _mav_put_uint8_t(buf, 33, target_component); + _mav_put_uint8_t(buf, 34, gimbal_device_id); + _mav_put_float_array(buf, 4, q, 4); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_ATTITUDE, buf, MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_ATTITUDE_MIN_LEN, MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_ATTITUDE_LEN, MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_ATTITUDE_CRC); +#else + mavlink_gimbal_manager_set_attitude_t packet; + packet.flags = flags; + packet.angular_velocity_x = angular_velocity_x; + packet.angular_velocity_y = angular_velocity_y; + packet.angular_velocity_z = angular_velocity_z; + packet.target_system = target_system; + packet.target_component = target_component; + packet.gimbal_device_id = gimbal_device_id; + mav_array_memcpy(packet.q, q, sizeof(float)*4); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_ATTITUDE, (const char *)&packet, MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_ATTITUDE_MIN_LEN, MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_ATTITUDE_LEN, MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_ATTITUDE_CRC); +#endif +} + +/** + * @brief Send a gimbal_manager_set_attitude message + * @param chan MAVLink channel to send the message + * @param struct The MAVLink struct to serialize + */ +static inline void mavlink_msg_gimbal_manager_set_attitude_send_struct(mavlink_channel_t chan, const mavlink_gimbal_manager_set_attitude_t* gimbal_manager_set_attitude) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + mavlink_msg_gimbal_manager_set_attitude_send(chan, gimbal_manager_set_attitude->target_system, gimbal_manager_set_attitude->target_component, gimbal_manager_set_attitude->flags, gimbal_manager_set_attitude->gimbal_device_id, gimbal_manager_set_attitude->q, gimbal_manager_set_attitude->angular_velocity_x, gimbal_manager_set_attitude->angular_velocity_y, gimbal_manager_set_attitude->angular_velocity_z); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_ATTITUDE, (const char *)gimbal_manager_set_attitude, MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_ATTITUDE_MIN_LEN, MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_ATTITUDE_LEN, MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_ATTITUDE_CRC); +#endif +} + +#if MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_ATTITUDE_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_gimbal_manager_set_attitude_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint32_t flags, uint8_t gimbal_device_id, const float *q, float angular_velocity_x, float angular_velocity_y, float angular_velocity_z) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint32_t(buf, 0, flags); + _mav_put_float(buf, 20, angular_velocity_x); + _mav_put_float(buf, 24, angular_velocity_y); + _mav_put_float(buf, 28, angular_velocity_z); + _mav_put_uint8_t(buf, 32, target_system); + _mav_put_uint8_t(buf, 33, target_component); + _mav_put_uint8_t(buf, 34, gimbal_device_id); + _mav_put_float_array(buf, 4, q, 4); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_ATTITUDE, buf, MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_ATTITUDE_MIN_LEN, MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_ATTITUDE_LEN, MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_ATTITUDE_CRC); +#else + mavlink_gimbal_manager_set_attitude_t *packet = (mavlink_gimbal_manager_set_attitude_t *)msgbuf; + packet->flags = flags; + packet->angular_velocity_x = angular_velocity_x; + packet->angular_velocity_y = angular_velocity_y; + packet->angular_velocity_z = angular_velocity_z; + packet->target_system = target_system; + packet->target_component = target_component; + packet->gimbal_device_id = gimbal_device_id; + mav_array_memcpy(packet->q, q, sizeof(float)*4); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_ATTITUDE, (const char *)packet, MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_ATTITUDE_MIN_LEN, MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_ATTITUDE_LEN, MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_ATTITUDE_CRC); +#endif +} +#endif + +#endif + +// MESSAGE GIMBAL_MANAGER_SET_ATTITUDE UNPACKING + + +/** + * @brief Get field target_system from gimbal_manager_set_attitude message + * + * @return System ID + */ +static inline uint8_t mavlink_msg_gimbal_manager_set_attitude_get_target_system(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 32); +} + +/** + * @brief Get field target_component from gimbal_manager_set_attitude message + * + * @return Component ID + */ +static inline uint8_t mavlink_msg_gimbal_manager_set_attitude_get_target_component(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 33); +} + +/** + * @brief Get field flags from gimbal_manager_set_attitude message + * + * @return High level gimbal manager flags to use. + */ +static inline uint32_t mavlink_msg_gimbal_manager_set_attitude_get_flags(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint32_t(msg, 0); +} + +/** + * @brief Get field gimbal_device_id from gimbal_manager_set_attitude message + * + * @return Component ID of gimbal device to address (or 1-6 for non-MAVLink gimbal), 0 for all gimbal device components. Send command multiple times for more than one gimbal (but not all gimbals). + */ +static inline uint8_t mavlink_msg_gimbal_manager_set_attitude_get_gimbal_device_id(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 34); +} + +/** + * @brief Get field q from gimbal_manager_set_attitude message + * + * @return Quaternion components, w, x, y, z (1 0 0 0 is the null-rotation, the frame is depends on whether the flag GIMBAL_MANAGER_FLAGS_YAW_LOCK is set) + */ +static inline uint16_t mavlink_msg_gimbal_manager_set_attitude_get_q(const mavlink_message_t* msg, float *q) +{ + return _MAV_RETURN_float_array(msg, q, 4, 4); +} + +/** + * @brief Get field angular_velocity_x from gimbal_manager_set_attitude message + * + * @return [rad/s] X component of angular velocity, positive is rolling to the right, NaN to be ignored. + */ +static inline float mavlink_msg_gimbal_manager_set_attitude_get_angular_velocity_x(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 20); +} + +/** + * @brief Get field angular_velocity_y from gimbal_manager_set_attitude message + * + * @return [rad/s] Y component of angular velocity, positive is pitching up, NaN to be ignored. + */ +static inline float mavlink_msg_gimbal_manager_set_attitude_get_angular_velocity_y(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 24); +} + +/** + * @brief Get field angular_velocity_z from gimbal_manager_set_attitude message + * + * @return [rad/s] Z component of angular velocity, positive is yawing to the right, NaN to be ignored. + */ +static inline float mavlink_msg_gimbal_manager_set_attitude_get_angular_velocity_z(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 28); +} + +/** + * @brief Decode a gimbal_manager_set_attitude message into a struct + * + * @param msg The message to decode + * @param gimbal_manager_set_attitude C-struct to decode the message contents into + */ +static inline void mavlink_msg_gimbal_manager_set_attitude_decode(const mavlink_message_t* msg, mavlink_gimbal_manager_set_attitude_t* gimbal_manager_set_attitude) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + gimbal_manager_set_attitude->flags = mavlink_msg_gimbal_manager_set_attitude_get_flags(msg); + mavlink_msg_gimbal_manager_set_attitude_get_q(msg, gimbal_manager_set_attitude->q); + gimbal_manager_set_attitude->angular_velocity_x = mavlink_msg_gimbal_manager_set_attitude_get_angular_velocity_x(msg); + gimbal_manager_set_attitude->angular_velocity_y = mavlink_msg_gimbal_manager_set_attitude_get_angular_velocity_y(msg); + gimbal_manager_set_attitude->angular_velocity_z = mavlink_msg_gimbal_manager_set_attitude_get_angular_velocity_z(msg); + gimbal_manager_set_attitude->target_system = mavlink_msg_gimbal_manager_set_attitude_get_target_system(msg); + gimbal_manager_set_attitude->target_component = mavlink_msg_gimbal_manager_set_attitude_get_target_component(msg); + gimbal_manager_set_attitude->gimbal_device_id = mavlink_msg_gimbal_manager_set_attitude_get_gimbal_device_id(msg); +#else + uint8_t len = msg->len < MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_ATTITUDE_LEN? msg->len : MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_ATTITUDE_LEN; + memset(gimbal_manager_set_attitude, 0, MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_ATTITUDE_LEN); + memcpy(gimbal_manager_set_attitude, _MAV_PAYLOAD(msg), len); +#endif +} diff --git a/lib/main/MAVLink/common/mavlink_msg_gimbal_manager_set_manual_control.h b/lib/main/MAVLink/common/mavlink_msg_gimbal_manager_set_manual_control.h new file mode 100644 index 0000000000..d3096c49bb --- /dev/null +++ b/lib/main/MAVLink/common/mavlink_msg_gimbal_manager_set_manual_control.h @@ -0,0 +1,388 @@ +#pragma once +// MESSAGE GIMBAL_MANAGER_SET_MANUAL_CONTROL PACKING + +#define MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_MANUAL_CONTROL 288 + + +typedef struct __mavlink_gimbal_manager_set_manual_control_t { + uint32_t flags; /*< High level gimbal manager flags.*/ + float pitch; /*< Pitch angle unitless (-1..1, positive: up, negative: down, NaN to be ignored).*/ + float yaw; /*< Yaw angle unitless (-1..1, positive: to the right, negative: to the left, NaN to be ignored).*/ + float pitch_rate; /*< Pitch angular rate unitless (-1..1, positive: up, negative: down, NaN to be ignored).*/ + float yaw_rate; /*< Yaw angular rate unitless (-1..1, positive: to the right, negative: to the left, NaN to be ignored).*/ + uint8_t target_system; /*< System ID*/ + uint8_t target_component; /*< Component ID*/ + uint8_t gimbal_device_id; /*< Component ID of gimbal device to address (or 1-6 for non-MAVLink gimbal), 0 for all gimbal device components. Send command multiple times for more than one gimbal (but not all gimbals).*/ +} mavlink_gimbal_manager_set_manual_control_t; + +#define MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_MANUAL_CONTROL_LEN 23 +#define MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_MANUAL_CONTROL_MIN_LEN 23 +#define MAVLINK_MSG_ID_288_LEN 23 +#define MAVLINK_MSG_ID_288_MIN_LEN 23 + +#define MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_MANUAL_CONTROL_CRC 20 +#define MAVLINK_MSG_ID_288_CRC 20 + + + +#if MAVLINK_COMMAND_24BIT +#define MAVLINK_MESSAGE_INFO_GIMBAL_MANAGER_SET_MANUAL_CONTROL { \ + 288, \ + "GIMBAL_MANAGER_SET_MANUAL_CONTROL", \ + 8, \ + { { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 20, offsetof(mavlink_gimbal_manager_set_manual_control_t, target_system) }, \ + { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 21, offsetof(mavlink_gimbal_manager_set_manual_control_t, target_component) }, \ + { "flags", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_gimbal_manager_set_manual_control_t, flags) }, \ + { "gimbal_device_id", NULL, MAVLINK_TYPE_UINT8_T, 0, 22, offsetof(mavlink_gimbal_manager_set_manual_control_t, gimbal_device_id) }, \ + { "pitch", NULL, MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_gimbal_manager_set_manual_control_t, pitch) }, \ + { "yaw", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_gimbal_manager_set_manual_control_t, yaw) }, \ + { "pitch_rate", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_gimbal_manager_set_manual_control_t, pitch_rate) }, \ + { "yaw_rate", NULL, MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_gimbal_manager_set_manual_control_t, yaw_rate) }, \ + } \ +} +#else +#define MAVLINK_MESSAGE_INFO_GIMBAL_MANAGER_SET_MANUAL_CONTROL { \ + "GIMBAL_MANAGER_SET_MANUAL_CONTROL", \ + 8, \ + { { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 20, offsetof(mavlink_gimbal_manager_set_manual_control_t, target_system) }, \ + { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 21, offsetof(mavlink_gimbal_manager_set_manual_control_t, target_component) }, \ + { "flags", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_gimbal_manager_set_manual_control_t, flags) }, \ + { "gimbal_device_id", NULL, MAVLINK_TYPE_UINT8_T, 0, 22, offsetof(mavlink_gimbal_manager_set_manual_control_t, gimbal_device_id) }, \ + { "pitch", NULL, MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_gimbal_manager_set_manual_control_t, pitch) }, \ + { "yaw", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_gimbal_manager_set_manual_control_t, yaw) }, \ + { "pitch_rate", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_gimbal_manager_set_manual_control_t, pitch_rate) }, \ + { "yaw_rate", NULL, MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_gimbal_manager_set_manual_control_t, yaw_rate) }, \ + } \ +} +#endif + +/** + * @brief Pack a gimbal_manager_set_manual_control message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param target_system System ID + * @param target_component Component ID + * @param flags High level gimbal manager flags. + * @param gimbal_device_id Component ID of gimbal device to address (or 1-6 for non-MAVLink gimbal), 0 for all gimbal device components. Send command multiple times for more than one gimbal (but not all gimbals). + * @param pitch Pitch angle unitless (-1..1, positive: up, negative: down, NaN to be ignored). + * @param yaw Yaw angle unitless (-1..1, positive: to the right, negative: to the left, NaN to be ignored). + * @param pitch_rate Pitch angular rate unitless (-1..1, positive: up, negative: down, NaN to be ignored). + * @param yaw_rate Yaw angular rate unitless (-1..1, positive: to the right, negative: to the left, NaN to be ignored). + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_gimbal_manager_set_manual_control_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint8_t target_system, uint8_t target_component, uint32_t flags, uint8_t gimbal_device_id, float pitch, float yaw, float pitch_rate, float yaw_rate) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_MANUAL_CONTROL_LEN]; + _mav_put_uint32_t(buf, 0, flags); + _mav_put_float(buf, 4, pitch); + _mav_put_float(buf, 8, yaw); + _mav_put_float(buf, 12, pitch_rate); + _mav_put_float(buf, 16, yaw_rate); + _mav_put_uint8_t(buf, 20, target_system); + _mav_put_uint8_t(buf, 21, target_component); + _mav_put_uint8_t(buf, 22, gimbal_device_id); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_MANUAL_CONTROL_LEN); +#else + mavlink_gimbal_manager_set_manual_control_t packet; + packet.flags = flags; + packet.pitch = pitch; + packet.yaw = yaw; + packet.pitch_rate = pitch_rate; + packet.yaw_rate = yaw_rate; + packet.target_system = target_system; + packet.target_component = target_component; + packet.gimbal_device_id = gimbal_device_id; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_MANUAL_CONTROL_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_MANUAL_CONTROL; + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_MANUAL_CONTROL_MIN_LEN, MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_MANUAL_CONTROL_LEN, MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_MANUAL_CONTROL_CRC); +} + +/** + * @brief Pack a gimbal_manager_set_manual_control message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param target_system System ID + * @param target_component Component ID + * @param flags High level gimbal manager flags. + * @param gimbal_device_id Component ID of gimbal device to address (or 1-6 for non-MAVLink gimbal), 0 for all gimbal device components. Send command multiple times for more than one gimbal (but not all gimbals). + * @param pitch Pitch angle unitless (-1..1, positive: up, negative: down, NaN to be ignored). + * @param yaw Yaw angle unitless (-1..1, positive: to the right, negative: to the left, NaN to be ignored). + * @param pitch_rate Pitch angular rate unitless (-1..1, positive: up, negative: down, NaN to be ignored). + * @param yaw_rate Yaw angular rate unitless (-1..1, positive: to the right, negative: to the left, NaN to be ignored). + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_gimbal_manager_set_manual_control_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint8_t target_system,uint8_t target_component,uint32_t flags,uint8_t gimbal_device_id,float pitch,float yaw,float pitch_rate,float yaw_rate) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_MANUAL_CONTROL_LEN]; + _mav_put_uint32_t(buf, 0, flags); + _mav_put_float(buf, 4, pitch); + _mav_put_float(buf, 8, yaw); + _mav_put_float(buf, 12, pitch_rate); + _mav_put_float(buf, 16, yaw_rate); + _mav_put_uint8_t(buf, 20, target_system); + _mav_put_uint8_t(buf, 21, target_component); + _mav_put_uint8_t(buf, 22, gimbal_device_id); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_MANUAL_CONTROL_LEN); +#else + mavlink_gimbal_manager_set_manual_control_t packet; + packet.flags = flags; + packet.pitch = pitch; + packet.yaw = yaw; + packet.pitch_rate = pitch_rate; + packet.yaw_rate = yaw_rate; + packet.target_system = target_system; + packet.target_component = target_component; + packet.gimbal_device_id = gimbal_device_id; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_MANUAL_CONTROL_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_MANUAL_CONTROL; + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_MANUAL_CONTROL_MIN_LEN, MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_MANUAL_CONTROL_LEN, MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_MANUAL_CONTROL_CRC); +} + +/** + * @brief Encode a gimbal_manager_set_manual_control struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param gimbal_manager_set_manual_control C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_gimbal_manager_set_manual_control_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_gimbal_manager_set_manual_control_t* gimbal_manager_set_manual_control) +{ + return mavlink_msg_gimbal_manager_set_manual_control_pack(system_id, component_id, msg, gimbal_manager_set_manual_control->target_system, gimbal_manager_set_manual_control->target_component, gimbal_manager_set_manual_control->flags, gimbal_manager_set_manual_control->gimbal_device_id, gimbal_manager_set_manual_control->pitch, gimbal_manager_set_manual_control->yaw, gimbal_manager_set_manual_control->pitch_rate, gimbal_manager_set_manual_control->yaw_rate); +} + +/** + * @brief Encode a gimbal_manager_set_manual_control struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param gimbal_manager_set_manual_control C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_gimbal_manager_set_manual_control_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_gimbal_manager_set_manual_control_t* gimbal_manager_set_manual_control) +{ + return mavlink_msg_gimbal_manager_set_manual_control_pack_chan(system_id, component_id, chan, msg, gimbal_manager_set_manual_control->target_system, gimbal_manager_set_manual_control->target_component, gimbal_manager_set_manual_control->flags, gimbal_manager_set_manual_control->gimbal_device_id, gimbal_manager_set_manual_control->pitch, gimbal_manager_set_manual_control->yaw, gimbal_manager_set_manual_control->pitch_rate, gimbal_manager_set_manual_control->yaw_rate); +} + +/** + * @brief Send a gimbal_manager_set_manual_control message + * @param chan MAVLink channel to send the message + * + * @param target_system System ID + * @param target_component Component ID + * @param flags High level gimbal manager flags. + * @param gimbal_device_id Component ID of gimbal device to address (or 1-6 for non-MAVLink gimbal), 0 for all gimbal device components. Send command multiple times for more than one gimbal (but not all gimbals). + * @param pitch Pitch angle unitless (-1..1, positive: up, negative: down, NaN to be ignored). + * @param yaw Yaw angle unitless (-1..1, positive: to the right, negative: to the left, NaN to be ignored). + * @param pitch_rate Pitch angular rate unitless (-1..1, positive: up, negative: down, NaN to be ignored). + * @param yaw_rate Yaw angular rate unitless (-1..1, positive: to the right, negative: to the left, NaN to be ignored). + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_gimbal_manager_set_manual_control_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint32_t flags, uint8_t gimbal_device_id, float pitch, float yaw, float pitch_rate, float yaw_rate) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_MANUAL_CONTROL_LEN]; + _mav_put_uint32_t(buf, 0, flags); + _mav_put_float(buf, 4, pitch); + _mav_put_float(buf, 8, yaw); + _mav_put_float(buf, 12, pitch_rate); + _mav_put_float(buf, 16, yaw_rate); + _mav_put_uint8_t(buf, 20, target_system); + _mav_put_uint8_t(buf, 21, target_component); + _mav_put_uint8_t(buf, 22, gimbal_device_id); + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_MANUAL_CONTROL, buf, MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_MANUAL_CONTROL_MIN_LEN, MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_MANUAL_CONTROL_LEN, MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_MANUAL_CONTROL_CRC); +#else + mavlink_gimbal_manager_set_manual_control_t packet; + packet.flags = flags; + packet.pitch = pitch; + packet.yaw = yaw; + packet.pitch_rate = pitch_rate; + packet.yaw_rate = yaw_rate; + packet.target_system = target_system; + packet.target_component = target_component; + packet.gimbal_device_id = gimbal_device_id; + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_MANUAL_CONTROL, (const char *)&packet, MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_MANUAL_CONTROL_MIN_LEN, MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_MANUAL_CONTROL_LEN, MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_MANUAL_CONTROL_CRC); +#endif +} + +/** + * @brief Send a gimbal_manager_set_manual_control message + * @param chan MAVLink channel to send the message + * @param struct The MAVLink struct to serialize + */ +static inline void mavlink_msg_gimbal_manager_set_manual_control_send_struct(mavlink_channel_t chan, const mavlink_gimbal_manager_set_manual_control_t* gimbal_manager_set_manual_control) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + mavlink_msg_gimbal_manager_set_manual_control_send(chan, gimbal_manager_set_manual_control->target_system, gimbal_manager_set_manual_control->target_component, gimbal_manager_set_manual_control->flags, gimbal_manager_set_manual_control->gimbal_device_id, gimbal_manager_set_manual_control->pitch, gimbal_manager_set_manual_control->yaw, gimbal_manager_set_manual_control->pitch_rate, gimbal_manager_set_manual_control->yaw_rate); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_MANUAL_CONTROL, (const char *)gimbal_manager_set_manual_control, MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_MANUAL_CONTROL_MIN_LEN, MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_MANUAL_CONTROL_LEN, MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_MANUAL_CONTROL_CRC); +#endif +} + +#if MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_MANUAL_CONTROL_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_gimbal_manager_set_manual_control_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint32_t flags, uint8_t gimbal_device_id, float pitch, float yaw, float pitch_rate, float yaw_rate) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint32_t(buf, 0, flags); + _mav_put_float(buf, 4, pitch); + _mav_put_float(buf, 8, yaw); + _mav_put_float(buf, 12, pitch_rate); + _mav_put_float(buf, 16, yaw_rate); + _mav_put_uint8_t(buf, 20, target_system); + _mav_put_uint8_t(buf, 21, target_component); + _mav_put_uint8_t(buf, 22, gimbal_device_id); + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_MANUAL_CONTROL, buf, MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_MANUAL_CONTROL_MIN_LEN, MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_MANUAL_CONTROL_LEN, MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_MANUAL_CONTROL_CRC); +#else + mavlink_gimbal_manager_set_manual_control_t *packet = (mavlink_gimbal_manager_set_manual_control_t *)msgbuf; + packet->flags = flags; + packet->pitch = pitch; + packet->yaw = yaw; + packet->pitch_rate = pitch_rate; + packet->yaw_rate = yaw_rate; + packet->target_system = target_system; + packet->target_component = target_component; + packet->gimbal_device_id = gimbal_device_id; + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_MANUAL_CONTROL, (const char *)packet, MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_MANUAL_CONTROL_MIN_LEN, MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_MANUAL_CONTROL_LEN, MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_MANUAL_CONTROL_CRC); +#endif +} +#endif + +#endif + +// MESSAGE GIMBAL_MANAGER_SET_MANUAL_CONTROL UNPACKING + + +/** + * @brief Get field target_system from gimbal_manager_set_manual_control message + * + * @return System ID + */ +static inline uint8_t mavlink_msg_gimbal_manager_set_manual_control_get_target_system(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 20); +} + +/** + * @brief Get field target_component from gimbal_manager_set_manual_control message + * + * @return Component ID + */ +static inline uint8_t mavlink_msg_gimbal_manager_set_manual_control_get_target_component(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 21); +} + +/** + * @brief Get field flags from gimbal_manager_set_manual_control message + * + * @return High level gimbal manager flags. + */ +static inline uint32_t mavlink_msg_gimbal_manager_set_manual_control_get_flags(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint32_t(msg, 0); +} + +/** + * @brief Get field gimbal_device_id from gimbal_manager_set_manual_control message + * + * @return Component ID of gimbal device to address (or 1-6 for non-MAVLink gimbal), 0 for all gimbal device components. Send command multiple times for more than one gimbal (but not all gimbals). + */ +static inline uint8_t mavlink_msg_gimbal_manager_set_manual_control_get_gimbal_device_id(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 22); +} + +/** + * @brief Get field pitch from gimbal_manager_set_manual_control message + * + * @return Pitch angle unitless (-1..1, positive: up, negative: down, NaN to be ignored). + */ +static inline float mavlink_msg_gimbal_manager_set_manual_control_get_pitch(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 4); +} + +/** + * @brief Get field yaw from gimbal_manager_set_manual_control message + * + * @return Yaw angle unitless (-1..1, positive: to the right, negative: to the left, NaN to be ignored). + */ +static inline float mavlink_msg_gimbal_manager_set_manual_control_get_yaw(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 8); +} + +/** + * @brief Get field pitch_rate from gimbal_manager_set_manual_control message + * + * @return Pitch angular rate unitless (-1..1, positive: up, negative: down, NaN to be ignored). + */ +static inline float mavlink_msg_gimbal_manager_set_manual_control_get_pitch_rate(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 12); +} + +/** + * @brief Get field yaw_rate from gimbal_manager_set_manual_control message + * + * @return Yaw angular rate unitless (-1..1, positive: to the right, negative: to the left, NaN to be ignored). + */ +static inline float mavlink_msg_gimbal_manager_set_manual_control_get_yaw_rate(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 16); +} + +/** + * @brief Decode a gimbal_manager_set_manual_control message into a struct + * + * @param msg The message to decode + * @param gimbal_manager_set_manual_control C-struct to decode the message contents into + */ +static inline void mavlink_msg_gimbal_manager_set_manual_control_decode(const mavlink_message_t* msg, mavlink_gimbal_manager_set_manual_control_t* gimbal_manager_set_manual_control) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + gimbal_manager_set_manual_control->flags = mavlink_msg_gimbal_manager_set_manual_control_get_flags(msg); + gimbal_manager_set_manual_control->pitch = mavlink_msg_gimbal_manager_set_manual_control_get_pitch(msg); + gimbal_manager_set_manual_control->yaw = mavlink_msg_gimbal_manager_set_manual_control_get_yaw(msg); + gimbal_manager_set_manual_control->pitch_rate = mavlink_msg_gimbal_manager_set_manual_control_get_pitch_rate(msg); + gimbal_manager_set_manual_control->yaw_rate = mavlink_msg_gimbal_manager_set_manual_control_get_yaw_rate(msg); + gimbal_manager_set_manual_control->target_system = mavlink_msg_gimbal_manager_set_manual_control_get_target_system(msg); + gimbal_manager_set_manual_control->target_component = mavlink_msg_gimbal_manager_set_manual_control_get_target_component(msg); + gimbal_manager_set_manual_control->gimbal_device_id = mavlink_msg_gimbal_manager_set_manual_control_get_gimbal_device_id(msg); +#else + uint8_t len = msg->len < MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_MANUAL_CONTROL_LEN? msg->len : MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_MANUAL_CONTROL_LEN; + memset(gimbal_manager_set_manual_control, 0, MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_MANUAL_CONTROL_LEN); + memcpy(gimbal_manager_set_manual_control, _MAV_PAYLOAD(msg), len); +#endif +} diff --git a/lib/main/MAVLink/common/mavlink_msg_gimbal_manager_set_pitchyaw.h b/lib/main/MAVLink/common/mavlink_msg_gimbal_manager_set_pitchyaw.h new file mode 100644 index 0000000000..de6a43b1d3 --- /dev/null +++ b/lib/main/MAVLink/common/mavlink_msg_gimbal_manager_set_pitchyaw.h @@ -0,0 +1,388 @@ +#pragma once +// MESSAGE GIMBAL_MANAGER_SET_PITCHYAW PACKING + +#define MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_PITCHYAW 287 + + +typedef struct __mavlink_gimbal_manager_set_pitchyaw_t { + uint32_t flags; /*< High level gimbal manager flags to use.*/ + float pitch; /*< [rad] Pitch angle (positive: up, negative: down, NaN to be ignored).*/ + float yaw; /*< [rad] Yaw angle (positive: to the right, negative: to the left, NaN to be ignored).*/ + float pitch_rate; /*< [rad/s] Pitch angular rate (positive: up, negative: down, NaN to be ignored).*/ + float yaw_rate; /*< [rad/s] Yaw angular rate (positive: to the right, negative: to the left, NaN to be ignored).*/ + uint8_t target_system; /*< System ID*/ + uint8_t target_component; /*< Component ID*/ + uint8_t gimbal_device_id; /*< Component ID of gimbal device to address (or 1-6 for non-MAVLink gimbal), 0 for all gimbal device components. Send command multiple times for more than one gimbal (but not all gimbals).*/ +} mavlink_gimbal_manager_set_pitchyaw_t; + +#define MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_PITCHYAW_LEN 23 +#define MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_PITCHYAW_MIN_LEN 23 +#define MAVLINK_MSG_ID_287_LEN 23 +#define MAVLINK_MSG_ID_287_MIN_LEN 23 + +#define MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_PITCHYAW_CRC 1 +#define MAVLINK_MSG_ID_287_CRC 1 + + + +#if MAVLINK_COMMAND_24BIT +#define MAVLINK_MESSAGE_INFO_GIMBAL_MANAGER_SET_PITCHYAW { \ + 287, \ + "GIMBAL_MANAGER_SET_PITCHYAW", \ + 8, \ + { { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 20, offsetof(mavlink_gimbal_manager_set_pitchyaw_t, target_system) }, \ + { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 21, offsetof(mavlink_gimbal_manager_set_pitchyaw_t, target_component) }, \ + { "flags", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_gimbal_manager_set_pitchyaw_t, flags) }, \ + { "gimbal_device_id", NULL, MAVLINK_TYPE_UINT8_T, 0, 22, offsetof(mavlink_gimbal_manager_set_pitchyaw_t, gimbal_device_id) }, \ + { "pitch", NULL, MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_gimbal_manager_set_pitchyaw_t, pitch) }, \ + { "yaw", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_gimbal_manager_set_pitchyaw_t, yaw) }, \ + { "pitch_rate", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_gimbal_manager_set_pitchyaw_t, pitch_rate) }, \ + { "yaw_rate", NULL, MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_gimbal_manager_set_pitchyaw_t, yaw_rate) }, \ + } \ +} +#else +#define MAVLINK_MESSAGE_INFO_GIMBAL_MANAGER_SET_PITCHYAW { \ + "GIMBAL_MANAGER_SET_PITCHYAW", \ + 8, \ + { { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 20, offsetof(mavlink_gimbal_manager_set_pitchyaw_t, target_system) }, \ + { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 21, offsetof(mavlink_gimbal_manager_set_pitchyaw_t, target_component) }, \ + { "flags", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_gimbal_manager_set_pitchyaw_t, flags) }, \ + { "gimbal_device_id", NULL, MAVLINK_TYPE_UINT8_T, 0, 22, offsetof(mavlink_gimbal_manager_set_pitchyaw_t, gimbal_device_id) }, \ + { "pitch", NULL, MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_gimbal_manager_set_pitchyaw_t, pitch) }, \ + { "yaw", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_gimbal_manager_set_pitchyaw_t, yaw) }, \ + { "pitch_rate", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_gimbal_manager_set_pitchyaw_t, pitch_rate) }, \ + { "yaw_rate", NULL, MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_gimbal_manager_set_pitchyaw_t, yaw_rate) }, \ + } \ +} +#endif + +/** + * @brief Pack a gimbal_manager_set_pitchyaw message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param target_system System ID + * @param target_component Component ID + * @param flags High level gimbal manager flags to use. + * @param gimbal_device_id Component ID of gimbal device to address (or 1-6 for non-MAVLink gimbal), 0 for all gimbal device components. Send command multiple times for more than one gimbal (but not all gimbals). + * @param pitch [rad] Pitch angle (positive: up, negative: down, NaN to be ignored). + * @param yaw [rad] Yaw angle (positive: to the right, negative: to the left, NaN to be ignored). + * @param pitch_rate [rad/s] Pitch angular rate (positive: up, negative: down, NaN to be ignored). + * @param yaw_rate [rad/s] Yaw angular rate (positive: to the right, negative: to the left, NaN to be ignored). + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_gimbal_manager_set_pitchyaw_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint8_t target_system, uint8_t target_component, uint32_t flags, uint8_t gimbal_device_id, float pitch, float yaw, float pitch_rate, float yaw_rate) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_PITCHYAW_LEN]; + _mav_put_uint32_t(buf, 0, flags); + _mav_put_float(buf, 4, pitch); + _mav_put_float(buf, 8, yaw); + _mav_put_float(buf, 12, pitch_rate); + _mav_put_float(buf, 16, yaw_rate); + _mav_put_uint8_t(buf, 20, target_system); + _mav_put_uint8_t(buf, 21, target_component); + _mav_put_uint8_t(buf, 22, gimbal_device_id); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_PITCHYAW_LEN); +#else + mavlink_gimbal_manager_set_pitchyaw_t packet; + packet.flags = flags; + packet.pitch = pitch; + packet.yaw = yaw; + packet.pitch_rate = pitch_rate; + packet.yaw_rate = yaw_rate; + packet.target_system = target_system; + packet.target_component = target_component; + packet.gimbal_device_id = gimbal_device_id; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_PITCHYAW_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_PITCHYAW; + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_PITCHYAW_MIN_LEN, MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_PITCHYAW_LEN, MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_PITCHYAW_CRC); +} + +/** + * @brief Pack a gimbal_manager_set_pitchyaw message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param target_system System ID + * @param target_component Component ID + * @param flags High level gimbal manager flags to use. + * @param gimbal_device_id Component ID of gimbal device to address (or 1-6 for non-MAVLink gimbal), 0 for all gimbal device components. Send command multiple times for more than one gimbal (but not all gimbals). + * @param pitch [rad] Pitch angle (positive: up, negative: down, NaN to be ignored). + * @param yaw [rad] Yaw angle (positive: to the right, negative: to the left, NaN to be ignored). + * @param pitch_rate [rad/s] Pitch angular rate (positive: up, negative: down, NaN to be ignored). + * @param yaw_rate [rad/s] Yaw angular rate (positive: to the right, negative: to the left, NaN to be ignored). + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_gimbal_manager_set_pitchyaw_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint8_t target_system,uint8_t target_component,uint32_t flags,uint8_t gimbal_device_id,float pitch,float yaw,float pitch_rate,float yaw_rate) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_PITCHYAW_LEN]; + _mav_put_uint32_t(buf, 0, flags); + _mav_put_float(buf, 4, pitch); + _mav_put_float(buf, 8, yaw); + _mav_put_float(buf, 12, pitch_rate); + _mav_put_float(buf, 16, yaw_rate); + _mav_put_uint8_t(buf, 20, target_system); + _mav_put_uint8_t(buf, 21, target_component); + _mav_put_uint8_t(buf, 22, gimbal_device_id); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_PITCHYAW_LEN); +#else + mavlink_gimbal_manager_set_pitchyaw_t packet; + packet.flags = flags; + packet.pitch = pitch; + packet.yaw = yaw; + packet.pitch_rate = pitch_rate; + packet.yaw_rate = yaw_rate; + packet.target_system = target_system; + packet.target_component = target_component; + packet.gimbal_device_id = gimbal_device_id; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_PITCHYAW_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_PITCHYAW; + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_PITCHYAW_MIN_LEN, MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_PITCHYAW_LEN, MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_PITCHYAW_CRC); +} + +/** + * @brief Encode a gimbal_manager_set_pitchyaw struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param gimbal_manager_set_pitchyaw C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_gimbal_manager_set_pitchyaw_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_gimbal_manager_set_pitchyaw_t* gimbal_manager_set_pitchyaw) +{ + return mavlink_msg_gimbal_manager_set_pitchyaw_pack(system_id, component_id, msg, gimbal_manager_set_pitchyaw->target_system, gimbal_manager_set_pitchyaw->target_component, gimbal_manager_set_pitchyaw->flags, gimbal_manager_set_pitchyaw->gimbal_device_id, gimbal_manager_set_pitchyaw->pitch, gimbal_manager_set_pitchyaw->yaw, gimbal_manager_set_pitchyaw->pitch_rate, gimbal_manager_set_pitchyaw->yaw_rate); +} + +/** + * @brief Encode a gimbal_manager_set_pitchyaw struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param gimbal_manager_set_pitchyaw C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_gimbal_manager_set_pitchyaw_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_gimbal_manager_set_pitchyaw_t* gimbal_manager_set_pitchyaw) +{ + return mavlink_msg_gimbal_manager_set_pitchyaw_pack_chan(system_id, component_id, chan, msg, gimbal_manager_set_pitchyaw->target_system, gimbal_manager_set_pitchyaw->target_component, gimbal_manager_set_pitchyaw->flags, gimbal_manager_set_pitchyaw->gimbal_device_id, gimbal_manager_set_pitchyaw->pitch, gimbal_manager_set_pitchyaw->yaw, gimbal_manager_set_pitchyaw->pitch_rate, gimbal_manager_set_pitchyaw->yaw_rate); +} + +/** + * @brief Send a gimbal_manager_set_pitchyaw message + * @param chan MAVLink channel to send the message + * + * @param target_system System ID + * @param target_component Component ID + * @param flags High level gimbal manager flags to use. + * @param gimbal_device_id Component ID of gimbal device to address (or 1-6 for non-MAVLink gimbal), 0 for all gimbal device components. Send command multiple times for more than one gimbal (but not all gimbals). + * @param pitch [rad] Pitch angle (positive: up, negative: down, NaN to be ignored). + * @param yaw [rad] Yaw angle (positive: to the right, negative: to the left, NaN to be ignored). + * @param pitch_rate [rad/s] Pitch angular rate (positive: up, negative: down, NaN to be ignored). + * @param yaw_rate [rad/s] Yaw angular rate (positive: to the right, negative: to the left, NaN to be ignored). + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_gimbal_manager_set_pitchyaw_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint32_t flags, uint8_t gimbal_device_id, float pitch, float yaw, float pitch_rate, float yaw_rate) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_PITCHYAW_LEN]; + _mav_put_uint32_t(buf, 0, flags); + _mav_put_float(buf, 4, pitch); + _mav_put_float(buf, 8, yaw); + _mav_put_float(buf, 12, pitch_rate); + _mav_put_float(buf, 16, yaw_rate); + _mav_put_uint8_t(buf, 20, target_system); + _mav_put_uint8_t(buf, 21, target_component); + _mav_put_uint8_t(buf, 22, gimbal_device_id); + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_PITCHYAW, buf, MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_PITCHYAW_MIN_LEN, MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_PITCHYAW_LEN, MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_PITCHYAW_CRC); +#else + mavlink_gimbal_manager_set_pitchyaw_t packet; + packet.flags = flags; + packet.pitch = pitch; + packet.yaw = yaw; + packet.pitch_rate = pitch_rate; + packet.yaw_rate = yaw_rate; + packet.target_system = target_system; + packet.target_component = target_component; + packet.gimbal_device_id = gimbal_device_id; + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_PITCHYAW, (const char *)&packet, MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_PITCHYAW_MIN_LEN, MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_PITCHYAW_LEN, MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_PITCHYAW_CRC); +#endif +} + +/** + * @brief Send a gimbal_manager_set_pitchyaw message + * @param chan MAVLink channel to send the message + * @param struct The MAVLink struct to serialize + */ +static inline void mavlink_msg_gimbal_manager_set_pitchyaw_send_struct(mavlink_channel_t chan, const mavlink_gimbal_manager_set_pitchyaw_t* gimbal_manager_set_pitchyaw) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + mavlink_msg_gimbal_manager_set_pitchyaw_send(chan, gimbal_manager_set_pitchyaw->target_system, gimbal_manager_set_pitchyaw->target_component, gimbal_manager_set_pitchyaw->flags, gimbal_manager_set_pitchyaw->gimbal_device_id, gimbal_manager_set_pitchyaw->pitch, gimbal_manager_set_pitchyaw->yaw, gimbal_manager_set_pitchyaw->pitch_rate, gimbal_manager_set_pitchyaw->yaw_rate); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_PITCHYAW, (const char *)gimbal_manager_set_pitchyaw, MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_PITCHYAW_MIN_LEN, MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_PITCHYAW_LEN, MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_PITCHYAW_CRC); +#endif +} + +#if MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_PITCHYAW_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_gimbal_manager_set_pitchyaw_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint32_t flags, uint8_t gimbal_device_id, float pitch, float yaw, float pitch_rate, float yaw_rate) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint32_t(buf, 0, flags); + _mav_put_float(buf, 4, pitch); + _mav_put_float(buf, 8, yaw); + _mav_put_float(buf, 12, pitch_rate); + _mav_put_float(buf, 16, yaw_rate); + _mav_put_uint8_t(buf, 20, target_system); + _mav_put_uint8_t(buf, 21, target_component); + _mav_put_uint8_t(buf, 22, gimbal_device_id); + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_PITCHYAW, buf, MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_PITCHYAW_MIN_LEN, MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_PITCHYAW_LEN, MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_PITCHYAW_CRC); +#else + mavlink_gimbal_manager_set_pitchyaw_t *packet = (mavlink_gimbal_manager_set_pitchyaw_t *)msgbuf; + packet->flags = flags; + packet->pitch = pitch; + packet->yaw = yaw; + packet->pitch_rate = pitch_rate; + packet->yaw_rate = yaw_rate; + packet->target_system = target_system; + packet->target_component = target_component; + packet->gimbal_device_id = gimbal_device_id; + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_PITCHYAW, (const char *)packet, MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_PITCHYAW_MIN_LEN, MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_PITCHYAW_LEN, MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_PITCHYAW_CRC); +#endif +} +#endif + +#endif + +// MESSAGE GIMBAL_MANAGER_SET_PITCHYAW UNPACKING + + +/** + * @brief Get field target_system from gimbal_manager_set_pitchyaw message + * + * @return System ID + */ +static inline uint8_t mavlink_msg_gimbal_manager_set_pitchyaw_get_target_system(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 20); +} + +/** + * @brief Get field target_component from gimbal_manager_set_pitchyaw message + * + * @return Component ID + */ +static inline uint8_t mavlink_msg_gimbal_manager_set_pitchyaw_get_target_component(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 21); +} + +/** + * @brief Get field flags from gimbal_manager_set_pitchyaw message + * + * @return High level gimbal manager flags to use. + */ +static inline uint32_t mavlink_msg_gimbal_manager_set_pitchyaw_get_flags(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint32_t(msg, 0); +} + +/** + * @brief Get field gimbal_device_id from gimbal_manager_set_pitchyaw message + * + * @return Component ID of gimbal device to address (or 1-6 for non-MAVLink gimbal), 0 for all gimbal device components. Send command multiple times for more than one gimbal (but not all gimbals). + */ +static inline uint8_t mavlink_msg_gimbal_manager_set_pitchyaw_get_gimbal_device_id(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 22); +} + +/** + * @brief Get field pitch from gimbal_manager_set_pitchyaw message + * + * @return [rad] Pitch angle (positive: up, negative: down, NaN to be ignored). + */ +static inline float mavlink_msg_gimbal_manager_set_pitchyaw_get_pitch(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 4); +} + +/** + * @brief Get field yaw from gimbal_manager_set_pitchyaw message + * + * @return [rad] Yaw angle (positive: to the right, negative: to the left, NaN to be ignored). + */ +static inline float mavlink_msg_gimbal_manager_set_pitchyaw_get_yaw(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 8); +} + +/** + * @brief Get field pitch_rate from gimbal_manager_set_pitchyaw message + * + * @return [rad/s] Pitch angular rate (positive: up, negative: down, NaN to be ignored). + */ +static inline float mavlink_msg_gimbal_manager_set_pitchyaw_get_pitch_rate(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 12); +} + +/** + * @brief Get field yaw_rate from gimbal_manager_set_pitchyaw message + * + * @return [rad/s] Yaw angular rate (positive: to the right, negative: to the left, NaN to be ignored). + */ +static inline float mavlink_msg_gimbal_manager_set_pitchyaw_get_yaw_rate(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 16); +} + +/** + * @brief Decode a gimbal_manager_set_pitchyaw message into a struct + * + * @param msg The message to decode + * @param gimbal_manager_set_pitchyaw C-struct to decode the message contents into + */ +static inline void mavlink_msg_gimbal_manager_set_pitchyaw_decode(const mavlink_message_t* msg, mavlink_gimbal_manager_set_pitchyaw_t* gimbal_manager_set_pitchyaw) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + gimbal_manager_set_pitchyaw->flags = mavlink_msg_gimbal_manager_set_pitchyaw_get_flags(msg); + gimbal_manager_set_pitchyaw->pitch = mavlink_msg_gimbal_manager_set_pitchyaw_get_pitch(msg); + gimbal_manager_set_pitchyaw->yaw = mavlink_msg_gimbal_manager_set_pitchyaw_get_yaw(msg); + gimbal_manager_set_pitchyaw->pitch_rate = mavlink_msg_gimbal_manager_set_pitchyaw_get_pitch_rate(msg); + gimbal_manager_set_pitchyaw->yaw_rate = mavlink_msg_gimbal_manager_set_pitchyaw_get_yaw_rate(msg); + gimbal_manager_set_pitchyaw->target_system = mavlink_msg_gimbal_manager_set_pitchyaw_get_target_system(msg); + gimbal_manager_set_pitchyaw->target_component = mavlink_msg_gimbal_manager_set_pitchyaw_get_target_component(msg); + gimbal_manager_set_pitchyaw->gimbal_device_id = mavlink_msg_gimbal_manager_set_pitchyaw_get_gimbal_device_id(msg); +#else + uint8_t len = msg->len < MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_PITCHYAW_LEN? msg->len : MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_PITCHYAW_LEN; + memset(gimbal_manager_set_pitchyaw, 0, MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_PITCHYAW_LEN); + memcpy(gimbal_manager_set_pitchyaw, _MAV_PAYLOAD(msg), len); +#endif +} diff --git a/lib/main/MAVLink/common/mavlink_msg_gimbal_manager_status.h b/lib/main/MAVLink/common/mavlink_msg_gimbal_manager_status.h new file mode 100644 index 0000000000..4eace068bb --- /dev/null +++ b/lib/main/MAVLink/common/mavlink_msg_gimbal_manager_status.h @@ -0,0 +1,363 @@ +#pragma once +// MESSAGE GIMBAL_MANAGER_STATUS PACKING + +#define MAVLINK_MSG_ID_GIMBAL_MANAGER_STATUS 281 + + +typedef struct __mavlink_gimbal_manager_status_t { + uint32_t time_boot_ms; /*< [ms] Timestamp (time since system boot).*/ + uint32_t flags; /*< High level gimbal manager flags currently applied.*/ + uint8_t gimbal_device_id; /*< Gimbal device ID that this gimbal manager is responsible for.*/ + uint8_t primary_control_sysid; /*< System ID of MAVLink component with primary control, 0 for none.*/ + uint8_t primary_control_compid; /*< Component ID of MAVLink component with primary control, 0 for none.*/ + uint8_t secondary_control_sysid; /*< System ID of MAVLink component with secondary control, 0 for none.*/ + uint8_t secondary_control_compid; /*< Component ID of MAVLink component with secondary control, 0 for none.*/ +} mavlink_gimbal_manager_status_t; + +#define MAVLINK_MSG_ID_GIMBAL_MANAGER_STATUS_LEN 13 +#define MAVLINK_MSG_ID_GIMBAL_MANAGER_STATUS_MIN_LEN 13 +#define MAVLINK_MSG_ID_281_LEN 13 +#define MAVLINK_MSG_ID_281_MIN_LEN 13 + +#define MAVLINK_MSG_ID_GIMBAL_MANAGER_STATUS_CRC 48 +#define MAVLINK_MSG_ID_281_CRC 48 + + + +#if MAVLINK_COMMAND_24BIT +#define MAVLINK_MESSAGE_INFO_GIMBAL_MANAGER_STATUS { \ + 281, \ + "GIMBAL_MANAGER_STATUS", \ + 7, \ + { { "time_boot_ms", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_gimbal_manager_status_t, time_boot_ms) }, \ + { "flags", NULL, MAVLINK_TYPE_UINT32_T, 0, 4, offsetof(mavlink_gimbal_manager_status_t, flags) }, \ + { "gimbal_device_id", NULL, MAVLINK_TYPE_UINT8_T, 0, 8, offsetof(mavlink_gimbal_manager_status_t, gimbal_device_id) }, \ + { "primary_control_sysid", NULL, MAVLINK_TYPE_UINT8_T, 0, 9, offsetof(mavlink_gimbal_manager_status_t, primary_control_sysid) }, \ + { "primary_control_compid", NULL, MAVLINK_TYPE_UINT8_T, 0, 10, offsetof(mavlink_gimbal_manager_status_t, primary_control_compid) }, \ + { "secondary_control_sysid", NULL, MAVLINK_TYPE_UINT8_T, 0, 11, offsetof(mavlink_gimbal_manager_status_t, secondary_control_sysid) }, \ + { "secondary_control_compid", NULL, MAVLINK_TYPE_UINT8_T, 0, 12, offsetof(mavlink_gimbal_manager_status_t, secondary_control_compid) }, \ + } \ +} +#else +#define MAVLINK_MESSAGE_INFO_GIMBAL_MANAGER_STATUS { \ + "GIMBAL_MANAGER_STATUS", \ + 7, \ + { { "time_boot_ms", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_gimbal_manager_status_t, time_boot_ms) }, \ + { "flags", NULL, MAVLINK_TYPE_UINT32_T, 0, 4, offsetof(mavlink_gimbal_manager_status_t, flags) }, \ + { "gimbal_device_id", NULL, MAVLINK_TYPE_UINT8_T, 0, 8, offsetof(mavlink_gimbal_manager_status_t, gimbal_device_id) }, \ + { "primary_control_sysid", NULL, MAVLINK_TYPE_UINT8_T, 0, 9, offsetof(mavlink_gimbal_manager_status_t, primary_control_sysid) }, \ + { "primary_control_compid", NULL, MAVLINK_TYPE_UINT8_T, 0, 10, offsetof(mavlink_gimbal_manager_status_t, primary_control_compid) }, \ + { "secondary_control_sysid", NULL, MAVLINK_TYPE_UINT8_T, 0, 11, offsetof(mavlink_gimbal_manager_status_t, secondary_control_sysid) }, \ + { "secondary_control_compid", NULL, MAVLINK_TYPE_UINT8_T, 0, 12, offsetof(mavlink_gimbal_manager_status_t, secondary_control_compid) }, \ + } \ +} +#endif + +/** + * @brief Pack a gimbal_manager_status message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param time_boot_ms [ms] Timestamp (time since system boot). + * @param flags High level gimbal manager flags currently applied. + * @param gimbal_device_id Gimbal device ID that this gimbal manager is responsible for. + * @param primary_control_sysid System ID of MAVLink component with primary control, 0 for none. + * @param primary_control_compid Component ID of MAVLink component with primary control, 0 for none. + * @param secondary_control_sysid System ID of MAVLink component with secondary control, 0 for none. + * @param secondary_control_compid Component ID of MAVLink component with secondary control, 0 for none. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_gimbal_manager_status_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint32_t time_boot_ms, uint32_t flags, uint8_t gimbal_device_id, uint8_t primary_control_sysid, uint8_t primary_control_compid, uint8_t secondary_control_sysid, uint8_t secondary_control_compid) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_GIMBAL_MANAGER_STATUS_LEN]; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_uint32_t(buf, 4, flags); + _mav_put_uint8_t(buf, 8, gimbal_device_id); + _mav_put_uint8_t(buf, 9, primary_control_sysid); + _mav_put_uint8_t(buf, 10, primary_control_compid); + _mav_put_uint8_t(buf, 11, secondary_control_sysid); + _mav_put_uint8_t(buf, 12, secondary_control_compid); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_GIMBAL_MANAGER_STATUS_LEN); +#else + mavlink_gimbal_manager_status_t packet; + packet.time_boot_ms = time_boot_ms; + packet.flags = flags; + packet.gimbal_device_id = gimbal_device_id; + packet.primary_control_sysid = primary_control_sysid; + packet.primary_control_compid = primary_control_compid; + packet.secondary_control_sysid = secondary_control_sysid; + packet.secondary_control_compid = secondary_control_compid; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_GIMBAL_MANAGER_STATUS_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_GIMBAL_MANAGER_STATUS; + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_GIMBAL_MANAGER_STATUS_MIN_LEN, MAVLINK_MSG_ID_GIMBAL_MANAGER_STATUS_LEN, MAVLINK_MSG_ID_GIMBAL_MANAGER_STATUS_CRC); +} + +/** + * @brief Pack a gimbal_manager_status message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param time_boot_ms [ms] Timestamp (time since system boot). + * @param flags High level gimbal manager flags currently applied. + * @param gimbal_device_id Gimbal device ID that this gimbal manager is responsible for. + * @param primary_control_sysid System ID of MAVLink component with primary control, 0 for none. + * @param primary_control_compid Component ID of MAVLink component with primary control, 0 for none. + * @param secondary_control_sysid System ID of MAVLink component with secondary control, 0 for none. + * @param secondary_control_compid Component ID of MAVLink component with secondary control, 0 for none. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_gimbal_manager_status_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint32_t time_boot_ms,uint32_t flags,uint8_t gimbal_device_id,uint8_t primary_control_sysid,uint8_t primary_control_compid,uint8_t secondary_control_sysid,uint8_t secondary_control_compid) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_GIMBAL_MANAGER_STATUS_LEN]; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_uint32_t(buf, 4, flags); + _mav_put_uint8_t(buf, 8, gimbal_device_id); + _mav_put_uint8_t(buf, 9, primary_control_sysid); + _mav_put_uint8_t(buf, 10, primary_control_compid); + _mav_put_uint8_t(buf, 11, secondary_control_sysid); + _mav_put_uint8_t(buf, 12, secondary_control_compid); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_GIMBAL_MANAGER_STATUS_LEN); +#else + mavlink_gimbal_manager_status_t packet; + packet.time_boot_ms = time_boot_ms; + packet.flags = flags; + packet.gimbal_device_id = gimbal_device_id; + packet.primary_control_sysid = primary_control_sysid; + packet.primary_control_compid = primary_control_compid; + packet.secondary_control_sysid = secondary_control_sysid; + packet.secondary_control_compid = secondary_control_compid; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_GIMBAL_MANAGER_STATUS_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_GIMBAL_MANAGER_STATUS; + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_GIMBAL_MANAGER_STATUS_MIN_LEN, MAVLINK_MSG_ID_GIMBAL_MANAGER_STATUS_LEN, MAVLINK_MSG_ID_GIMBAL_MANAGER_STATUS_CRC); +} + +/** + * @brief Encode a gimbal_manager_status struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param gimbal_manager_status C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_gimbal_manager_status_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_gimbal_manager_status_t* gimbal_manager_status) +{ + return mavlink_msg_gimbal_manager_status_pack(system_id, component_id, msg, gimbal_manager_status->time_boot_ms, gimbal_manager_status->flags, gimbal_manager_status->gimbal_device_id, gimbal_manager_status->primary_control_sysid, gimbal_manager_status->primary_control_compid, gimbal_manager_status->secondary_control_sysid, gimbal_manager_status->secondary_control_compid); +} + +/** + * @brief Encode a gimbal_manager_status struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param gimbal_manager_status C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_gimbal_manager_status_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_gimbal_manager_status_t* gimbal_manager_status) +{ + return mavlink_msg_gimbal_manager_status_pack_chan(system_id, component_id, chan, msg, gimbal_manager_status->time_boot_ms, gimbal_manager_status->flags, gimbal_manager_status->gimbal_device_id, gimbal_manager_status->primary_control_sysid, gimbal_manager_status->primary_control_compid, gimbal_manager_status->secondary_control_sysid, gimbal_manager_status->secondary_control_compid); +} + +/** + * @brief Send a gimbal_manager_status message + * @param chan MAVLink channel to send the message + * + * @param time_boot_ms [ms] Timestamp (time since system boot). + * @param flags High level gimbal manager flags currently applied. + * @param gimbal_device_id Gimbal device ID that this gimbal manager is responsible for. + * @param primary_control_sysid System ID of MAVLink component with primary control, 0 for none. + * @param primary_control_compid Component ID of MAVLink component with primary control, 0 for none. + * @param secondary_control_sysid System ID of MAVLink component with secondary control, 0 for none. + * @param secondary_control_compid Component ID of MAVLink component with secondary control, 0 for none. + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_gimbal_manager_status_send(mavlink_channel_t chan, uint32_t time_boot_ms, uint32_t flags, uint8_t gimbal_device_id, uint8_t primary_control_sysid, uint8_t primary_control_compid, uint8_t secondary_control_sysid, uint8_t secondary_control_compid) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_GIMBAL_MANAGER_STATUS_LEN]; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_uint32_t(buf, 4, flags); + _mav_put_uint8_t(buf, 8, gimbal_device_id); + _mav_put_uint8_t(buf, 9, primary_control_sysid); + _mav_put_uint8_t(buf, 10, primary_control_compid); + _mav_put_uint8_t(buf, 11, secondary_control_sysid); + _mav_put_uint8_t(buf, 12, secondary_control_compid); + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GIMBAL_MANAGER_STATUS, buf, MAVLINK_MSG_ID_GIMBAL_MANAGER_STATUS_MIN_LEN, MAVLINK_MSG_ID_GIMBAL_MANAGER_STATUS_LEN, MAVLINK_MSG_ID_GIMBAL_MANAGER_STATUS_CRC); +#else + mavlink_gimbal_manager_status_t packet; + packet.time_boot_ms = time_boot_ms; + packet.flags = flags; + packet.gimbal_device_id = gimbal_device_id; + packet.primary_control_sysid = primary_control_sysid; + packet.primary_control_compid = primary_control_compid; + packet.secondary_control_sysid = secondary_control_sysid; + packet.secondary_control_compid = secondary_control_compid; + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GIMBAL_MANAGER_STATUS, (const char *)&packet, MAVLINK_MSG_ID_GIMBAL_MANAGER_STATUS_MIN_LEN, MAVLINK_MSG_ID_GIMBAL_MANAGER_STATUS_LEN, MAVLINK_MSG_ID_GIMBAL_MANAGER_STATUS_CRC); +#endif +} + +/** + * @brief Send a gimbal_manager_status message + * @param chan MAVLink channel to send the message + * @param struct The MAVLink struct to serialize + */ +static inline void mavlink_msg_gimbal_manager_status_send_struct(mavlink_channel_t chan, const mavlink_gimbal_manager_status_t* gimbal_manager_status) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + mavlink_msg_gimbal_manager_status_send(chan, gimbal_manager_status->time_boot_ms, gimbal_manager_status->flags, gimbal_manager_status->gimbal_device_id, gimbal_manager_status->primary_control_sysid, gimbal_manager_status->primary_control_compid, gimbal_manager_status->secondary_control_sysid, gimbal_manager_status->secondary_control_compid); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GIMBAL_MANAGER_STATUS, (const char *)gimbal_manager_status, MAVLINK_MSG_ID_GIMBAL_MANAGER_STATUS_MIN_LEN, MAVLINK_MSG_ID_GIMBAL_MANAGER_STATUS_LEN, MAVLINK_MSG_ID_GIMBAL_MANAGER_STATUS_CRC); +#endif +} + +#if MAVLINK_MSG_ID_GIMBAL_MANAGER_STATUS_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_gimbal_manager_status_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint32_t time_boot_ms, uint32_t flags, uint8_t gimbal_device_id, uint8_t primary_control_sysid, uint8_t primary_control_compid, uint8_t secondary_control_sysid, uint8_t secondary_control_compid) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_uint32_t(buf, 4, flags); + _mav_put_uint8_t(buf, 8, gimbal_device_id); + _mav_put_uint8_t(buf, 9, primary_control_sysid); + _mav_put_uint8_t(buf, 10, primary_control_compid); + _mav_put_uint8_t(buf, 11, secondary_control_sysid); + _mav_put_uint8_t(buf, 12, secondary_control_compid); + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GIMBAL_MANAGER_STATUS, buf, MAVLINK_MSG_ID_GIMBAL_MANAGER_STATUS_MIN_LEN, MAVLINK_MSG_ID_GIMBAL_MANAGER_STATUS_LEN, MAVLINK_MSG_ID_GIMBAL_MANAGER_STATUS_CRC); +#else + mavlink_gimbal_manager_status_t *packet = (mavlink_gimbal_manager_status_t *)msgbuf; + packet->time_boot_ms = time_boot_ms; + packet->flags = flags; + packet->gimbal_device_id = gimbal_device_id; + packet->primary_control_sysid = primary_control_sysid; + packet->primary_control_compid = primary_control_compid; + packet->secondary_control_sysid = secondary_control_sysid; + packet->secondary_control_compid = secondary_control_compid; + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GIMBAL_MANAGER_STATUS, (const char *)packet, MAVLINK_MSG_ID_GIMBAL_MANAGER_STATUS_MIN_LEN, MAVLINK_MSG_ID_GIMBAL_MANAGER_STATUS_LEN, MAVLINK_MSG_ID_GIMBAL_MANAGER_STATUS_CRC); +#endif +} +#endif + +#endif + +// MESSAGE GIMBAL_MANAGER_STATUS UNPACKING + + +/** + * @brief Get field time_boot_ms from gimbal_manager_status message + * + * @return [ms] Timestamp (time since system boot). + */ +static inline uint32_t mavlink_msg_gimbal_manager_status_get_time_boot_ms(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint32_t(msg, 0); +} + +/** + * @brief Get field flags from gimbal_manager_status message + * + * @return High level gimbal manager flags currently applied. + */ +static inline uint32_t mavlink_msg_gimbal_manager_status_get_flags(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint32_t(msg, 4); +} + +/** + * @brief Get field gimbal_device_id from gimbal_manager_status message + * + * @return Gimbal device ID that this gimbal manager is responsible for. + */ +static inline uint8_t mavlink_msg_gimbal_manager_status_get_gimbal_device_id(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 8); +} + +/** + * @brief Get field primary_control_sysid from gimbal_manager_status message + * + * @return System ID of MAVLink component with primary control, 0 for none. + */ +static inline uint8_t mavlink_msg_gimbal_manager_status_get_primary_control_sysid(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 9); +} + +/** + * @brief Get field primary_control_compid from gimbal_manager_status message + * + * @return Component ID of MAVLink component with primary control, 0 for none. + */ +static inline uint8_t mavlink_msg_gimbal_manager_status_get_primary_control_compid(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 10); +} + +/** + * @brief Get field secondary_control_sysid from gimbal_manager_status message + * + * @return System ID of MAVLink component with secondary control, 0 for none. + */ +static inline uint8_t mavlink_msg_gimbal_manager_status_get_secondary_control_sysid(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 11); +} + +/** + * @brief Get field secondary_control_compid from gimbal_manager_status message + * + * @return Component ID of MAVLink component with secondary control, 0 for none. + */ +static inline uint8_t mavlink_msg_gimbal_manager_status_get_secondary_control_compid(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 12); +} + +/** + * @brief Decode a gimbal_manager_status message into a struct + * + * @param msg The message to decode + * @param gimbal_manager_status C-struct to decode the message contents into + */ +static inline void mavlink_msg_gimbal_manager_status_decode(const mavlink_message_t* msg, mavlink_gimbal_manager_status_t* gimbal_manager_status) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + gimbal_manager_status->time_boot_ms = mavlink_msg_gimbal_manager_status_get_time_boot_ms(msg); + gimbal_manager_status->flags = mavlink_msg_gimbal_manager_status_get_flags(msg); + gimbal_manager_status->gimbal_device_id = mavlink_msg_gimbal_manager_status_get_gimbal_device_id(msg); + gimbal_manager_status->primary_control_sysid = mavlink_msg_gimbal_manager_status_get_primary_control_sysid(msg); + gimbal_manager_status->primary_control_compid = mavlink_msg_gimbal_manager_status_get_primary_control_compid(msg); + gimbal_manager_status->secondary_control_sysid = mavlink_msg_gimbal_manager_status_get_secondary_control_sysid(msg); + gimbal_manager_status->secondary_control_compid = mavlink_msg_gimbal_manager_status_get_secondary_control_compid(msg); +#else + uint8_t len = msg->len < MAVLINK_MSG_ID_GIMBAL_MANAGER_STATUS_LEN? msg->len : MAVLINK_MSG_ID_GIMBAL_MANAGER_STATUS_LEN; + memset(gimbal_manager_status, 0, MAVLINK_MSG_ID_GIMBAL_MANAGER_STATUS_LEN); + memcpy(gimbal_manager_status, _MAV_PAYLOAD(msg), len); +#endif +} diff --git a/lib/main/MAVLink/common/mavlink_msg_global_vision_position_estimate.h b/lib/main/MAVLink/common/mavlink_msg_global_vision_position_estimate.h index 5f191fe3ad..94a3d05dd4 100755 --- a/lib/main/MAVLink/common/mavlink_msg_global_vision_position_estimate.h +++ b/lib/main/MAVLink/common/mavlink_msg_global_vision_position_estimate.h @@ -12,23 +12,25 @@ typedef struct __mavlink_global_vision_position_estimate_t { float roll; /*< [rad] Roll angle*/ float pitch; /*< [rad] Pitch angle*/ float yaw; /*< [rad] Yaw angle*/ + float covariance[21]; /*< Row-major representation of pose 6x6 cross-covariance matrix upper right triangle (states: x_global, y_global, z_global, roll, pitch, yaw; first six entries are the first ROW, next five entries are the second ROW, etc.). If unknown, assign NaN value to first element in the array.*/ + uint8_t reset_counter; /*< Estimate reset counter. This should be incremented when the estimate resets in any of the dimensions (position, velocity, attitude, angular speed). This is designed to be used when e.g an external SLAM system detects a loop-closure and the estimate jumps.*/ } mavlink_global_vision_position_estimate_t; -#define MAVLINK_MSG_ID_GLOBAL_VISION_POSITION_ESTIMATE_LEN 32 +#define MAVLINK_MSG_ID_GLOBAL_VISION_POSITION_ESTIMATE_LEN 117 #define MAVLINK_MSG_ID_GLOBAL_VISION_POSITION_ESTIMATE_MIN_LEN 32 -#define MAVLINK_MSG_ID_101_LEN 32 +#define MAVLINK_MSG_ID_101_LEN 117 #define MAVLINK_MSG_ID_101_MIN_LEN 32 #define MAVLINK_MSG_ID_GLOBAL_VISION_POSITION_ESTIMATE_CRC 102 #define MAVLINK_MSG_ID_101_CRC 102 - +#define MAVLINK_MSG_GLOBAL_VISION_POSITION_ESTIMATE_FIELD_COVARIANCE_LEN 21 #if MAVLINK_COMMAND_24BIT #define MAVLINK_MESSAGE_INFO_GLOBAL_VISION_POSITION_ESTIMATE { \ 101, \ "GLOBAL_VISION_POSITION_ESTIMATE", \ - 7, \ + 9, \ { { "usec", NULL, MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_global_vision_position_estimate_t, usec) }, \ { "x", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_global_vision_position_estimate_t, x) }, \ { "y", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_global_vision_position_estimate_t, y) }, \ @@ -36,12 +38,14 @@ typedef struct __mavlink_global_vision_position_estimate_t { { "roll", NULL, MAVLINK_TYPE_FLOAT, 0, 20, offsetof(mavlink_global_vision_position_estimate_t, roll) }, \ { "pitch", NULL, MAVLINK_TYPE_FLOAT, 0, 24, offsetof(mavlink_global_vision_position_estimate_t, pitch) }, \ { "yaw", NULL, MAVLINK_TYPE_FLOAT, 0, 28, offsetof(mavlink_global_vision_position_estimate_t, yaw) }, \ + { "covariance", NULL, MAVLINK_TYPE_FLOAT, 21, 32, offsetof(mavlink_global_vision_position_estimate_t, covariance) }, \ + { "reset_counter", NULL, MAVLINK_TYPE_UINT8_T, 0, 116, offsetof(mavlink_global_vision_position_estimate_t, reset_counter) }, \ } \ } #else #define MAVLINK_MESSAGE_INFO_GLOBAL_VISION_POSITION_ESTIMATE { \ "GLOBAL_VISION_POSITION_ESTIMATE", \ - 7, \ + 9, \ { { "usec", NULL, MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_global_vision_position_estimate_t, usec) }, \ { "x", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_global_vision_position_estimate_t, x) }, \ { "y", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_global_vision_position_estimate_t, y) }, \ @@ -49,6 +53,8 @@ typedef struct __mavlink_global_vision_position_estimate_t { { "roll", NULL, MAVLINK_TYPE_FLOAT, 0, 20, offsetof(mavlink_global_vision_position_estimate_t, roll) }, \ { "pitch", NULL, MAVLINK_TYPE_FLOAT, 0, 24, offsetof(mavlink_global_vision_position_estimate_t, pitch) }, \ { "yaw", NULL, MAVLINK_TYPE_FLOAT, 0, 28, offsetof(mavlink_global_vision_position_estimate_t, yaw) }, \ + { "covariance", NULL, MAVLINK_TYPE_FLOAT, 21, 32, offsetof(mavlink_global_vision_position_estimate_t, covariance) }, \ + { "reset_counter", NULL, MAVLINK_TYPE_UINT8_T, 0, 116, offsetof(mavlink_global_vision_position_estimate_t, reset_counter) }, \ } \ } #endif @@ -66,10 +72,12 @@ typedef struct __mavlink_global_vision_position_estimate_t { * @param roll [rad] Roll angle * @param pitch [rad] Pitch angle * @param yaw [rad] Yaw angle + * @param covariance Row-major representation of pose 6x6 cross-covariance matrix upper right triangle (states: x_global, y_global, z_global, roll, pitch, yaw; first six entries are the first ROW, next five entries are the second ROW, etc.). If unknown, assign NaN value to first element in the array. + * @param reset_counter Estimate reset counter. This should be incremented when the estimate resets in any of the dimensions (position, velocity, attitude, angular speed). This is designed to be used when e.g an external SLAM system detects a loop-closure and the estimate jumps. * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_global_vision_position_estimate_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint64_t usec, float x, float y, float z, float roll, float pitch, float yaw) + uint64_t usec, float x, float y, float z, float roll, float pitch, float yaw, const float *covariance, uint8_t reset_counter) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_GLOBAL_VISION_POSITION_ESTIMATE_LEN]; @@ -80,7 +88,8 @@ static inline uint16_t mavlink_msg_global_vision_position_estimate_pack(uint8_t _mav_put_float(buf, 20, roll); _mav_put_float(buf, 24, pitch); _mav_put_float(buf, 28, yaw); - + _mav_put_uint8_t(buf, 116, reset_counter); + _mav_put_float_array(buf, 32, covariance, 21); memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_GLOBAL_VISION_POSITION_ESTIMATE_LEN); #else mavlink_global_vision_position_estimate_t packet; @@ -91,7 +100,8 @@ static inline uint16_t mavlink_msg_global_vision_position_estimate_pack(uint8_t packet.roll = roll; packet.pitch = pitch; packet.yaw = yaw; - + packet.reset_counter = reset_counter; + mav_array_memcpy(packet.covariance, covariance, sizeof(float)*21); memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_GLOBAL_VISION_POSITION_ESTIMATE_LEN); #endif @@ -112,11 +122,13 @@ static inline uint16_t mavlink_msg_global_vision_position_estimate_pack(uint8_t * @param roll [rad] Roll angle * @param pitch [rad] Pitch angle * @param yaw [rad] Yaw angle + * @param covariance Row-major representation of pose 6x6 cross-covariance matrix upper right triangle (states: x_global, y_global, z_global, roll, pitch, yaw; first six entries are the first ROW, next five entries are the second ROW, etc.). If unknown, assign NaN value to first element in the array. + * @param reset_counter Estimate reset counter. This should be incremented when the estimate resets in any of the dimensions (position, velocity, attitude, angular speed). This is designed to be used when e.g an external SLAM system detects a loop-closure and the estimate jumps. * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_global_vision_position_estimate_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, - uint64_t usec,float x,float y,float z,float roll,float pitch,float yaw) + uint64_t usec,float x,float y,float z,float roll,float pitch,float yaw,const float *covariance,uint8_t reset_counter) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_GLOBAL_VISION_POSITION_ESTIMATE_LEN]; @@ -127,7 +139,8 @@ static inline uint16_t mavlink_msg_global_vision_position_estimate_pack_chan(uin _mav_put_float(buf, 20, roll); _mav_put_float(buf, 24, pitch); _mav_put_float(buf, 28, yaw); - + _mav_put_uint8_t(buf, 116, reset_counter); + _mav_put_float_array(buf, 32, covariance, 21); memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_GLOBAL_VISION_POSITION_ESTIMATE_LEN); #else mavlink_global_vision_position_estimate_t packet; @@ -138,7 +151,8 @@ static inline uint16_t mavlink_msg_global_vision_position_estimate_pack_chan(uin packet.roll = roll; packet.pitch = pitch; packet.yaw = yaw; - + packet.reset_counter = reset_counter; + mav_array_memcpy(packet.covariance, covariance, sizeof(float)*21); memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_GLOBAL_VISION_POSITION_ESTIMATE_LEN); #endif @@ -156,7 +170,7 @@ static inline uint16_t mavlink_msg_global_vision_position_estimate_pack_chan(uin */ static inline uint16_t mavlink_msg_global_vision_position_estimate_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_global_vision_position_estimate_t* global_vision_position_estimate) { - return mavlink_msg_global_vision_position_estimate_pack(system_id, component_id, msg, global_vision_position_estimate->usec, global_vision_position_estimate->x, global_vision_position_estimate->y, global_vision_position_estimate->z, global_vision_position_estimate->roll, global_vision_position_estimate->pitch, global_vision_position_estimate->yaw); + return mavlink_msg_global_vision_position_estimate_pack(system_id, component_id, msg, global_vision_position_estimate->usec, global_vision_position_estimate->x, global_vision_position_estimate->y, global_vision_position_estimate->z, global_vision_position_estimate->roll, global_vision_position_estimate->pitch, global_vision_position_estimate->yaw, global_vision_position_estimate->covariance, global_vision_position_estimate->reset_counter); } /** @@ -170,7 +184,7 @@ static inline uint16_t mavlink_msg_global_vision_position_estimate_encode(uint8_ */ static inline uint16_t mavlink_msg_global_vision_position_estimate_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_global_vision_position_estimate_t* global_vision_position_estimate) { - return mavlink_msg_global_vision_position_estimate_pack_chan(system_id, component_id, chan, msg, global_vision_position_estimate->usec, global_vision_position_estimate->x, global_vision_position_estimate->y, global_vision_position_estimate->z, global_vision_position_estimate->roll, global_vision_position_estimate->pitch, global_vision_position_estimate->yaw); + return mavlink_msg_global_vision_position_estimate_pack_chan(system_id, component_id, chan, msg, global_vision_position_estimate->usec, global_vision_position_estimate->x, global_vision_position_estimate->y, global_vision_position_estimate->z, global_vision_position_estimate->roll, global_vision_position_estimate->pitch, global_vision_position_estimate->yaw, global_vision_position_estimate->covariance, global_vision_position_estimate->reset_counter); } /** @@ -184,10 +198,12 @@ static inline uint16_t mavlink_msg_global_vision_position_estimate_encode_chan(u * @param roll [rad] Roll angle * @param pitch [rad] Pitch angle * @param yaw [rad] Yaw angle + * @param covariance Row-major representation of pose 6x6 cross-covariance matrix upper right triangle (states: x_global, y_global, z_global, roll, pitch, yaw; first six entries are the first ROW, next five entries are the second ROW, etc.). If unknown, assign NaN value to first element in the array. + * @param reset_counter Estimate reset counter. This should be incremented when the estimate resets in any of the dimensions (position, velocity, attitude, angular speed). This is designed to be used when e.g an external SLAM system detects a loop-closure and the estimate jumps. */ #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS -static inline void mavlink_msg_global_vision_position_estimate_send(mavlink_channel_t chan, uint64_t usec, float x, float y, float z, float roll, float pitch, float yaw) +static inline void mavlink_msg_global_vision_position_estimate_send(mavlink_channel_t chan, uint64_t usec, float x, float y, float z, float roll, float pitch, float yaw, const float *covariance, uint8_t reset_counter) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_GLOBAL_VISION_POSITION_ESTIMATE_LEN]; @@ -198,7 +214,8 @@ static inline void mavlink_msg_global_vision_position_estimate_send(mavlink_chan _mav_put_float(buf, 20, roll); _mav_put_float(buf, 24, pitch); _mav_put_float(buf, 28, yaw); - + _mav_put_uint8_t(buf, 116, reset_counter); + _mav_put_float_array(buf, 32, covariance, 21); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GLOBAL_VISION_POSITION_ESTIMATE, buf, MAVLINK_MSG_ID_GLOBAL_VISION_POSITION_ESTIMATE_MIN_LEN, MAVLINK_MSG_ID_GLOBAL_VISION_POSITION_ESTIMATE_LEN, MAVLINK_MSG_ID_GLOBAL_VISION_POSITION_ESTIMATE_CRC); #else mavlink_global_vision_position_estimate_t packet; @@ -209,7 +226,8 @@ static inline void mavlink_msg_global_vision_position_estimate_send(mavlink_chan packet.roll = roll; packet.pitch = pitch; packet.yaw = yaw; - + packet.reset_counter = reset_counter; + mav_array_memcpy(packet.covariance, covariance, sizeof(float)*21); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GLOBAL_VISION_POSITION_ESTIMATE, (const char *)&packet, MAVLINK_MSG_ID_GLOBAL_VISION_POSITION_ESTIMATE_MIN_LEN, MAVLINK_MSG_ID_GLOBAL_VISION_POSITION_ESTIMATE_LEN, MAVLINK_MSG_ID_GLOBAL_VISION_POSITION_ESTIMATE_CRC); #endif } @@ -222,7 +240,7 @@ static inline void mavlink_msg_global_vision_position_estimate_send(mavlink_chan static inline void mavlink_msg_global_vision_position_estimate_send_struct(mavlink_channel_t chan, const mavlink_global_vision_position_estimate_t* global_vision_position_estimate) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - mavlink_msg_global_vision_position_estimate_send(chan, global_vision_position_estimate->usec, global_vision_position_estimate->x, global_vision_position_estimate->y, global_vision_position_estimate->z, global_vision_position_estimate->roll, global_vision_position_estimate->pitch, global_vision_position_estimate->yaw); + mavlink_msg_global_vision_position_estimate_send(chan, global_vision_position_estimate->usec, global_vision_position_estimate->x, global_vision_position_estimate->y, global_vision_position_estimate->z, global_vision_position_estimate->roll, global_vision_position_estimate->pitch, global_vision_position_estimate->yaw, global_vision_position_estimate->covariance, global_vision_position_estimate->reset_counter); #else _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GLOBAL_VISION_POSITION_ESTIMATE, (const char *)global_vision_position_estimate, MAVLINK_MSG_ID_GLOBAL_VISION_POSITION_ESTIMATE_MIN_LEN, MAVLINK_MSG_ID_GLOBAL_VISION_POSITION_ESTIMATE_LEN, MAVLINK_MSG_ID_GLOBAL_VISION_POSITION_ESTIMATE_CRC); #endif @@ -236,7 +254,7 @@ static inline void mavlink_msg_global_vision_position_estimate_send_struct(mavli is usually the receive buffer for the channel, and allows a reply to an incoming message with minimum stack space usage. */ -static inline void mavlink_msg_global_vision_position_estimate_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint64_t usec, float x, float y, float z, float roll, float pitch, float yaw) +static inline void mavlink_msg_global_vision_position_estimate_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint64_t usec, float x, float y, float z, float roll, float pitch, float yaw, const float *covariance, uint8_t reset_counter) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char *buf = (char *)msgbuf; @@ -247,7 +265,8 @@ static inline void mavlink_msg_global_vision_position_estimate_send_buf(mavlink_ _mav_put_float(buf, 20, roll); _mav_put_float(buf, 24, pitch); _mav_put_float(buf, 28, yaw); - + _mav_put_uint8_t(buf, 116, reset_counter); + _mav_put_float_array(buf, 32, covariance, 21); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GLOBAL_VISION_POSITION_ESTIMATE, buf, MAVLINK_MSG_ID_GLOBAL_VISION_POSITION_ESTIMATE_MIN_LEN, MAVLINK_MSG_ID_GLOBAL_VISION_POSITION_ESTIMATE_LEN, MAVLINK_MSG_ID_GLOBAL_VISION_POSITION_ESTIMATE_CRC); #else mavlink_global_vision_position_estimate_t *packet = (mavlink_global_vision_position_estimate_t *)msgbuf; @@ -258,7 +277,8 @@ static inline void mavlink_msg_global_vision_position_estimate_send_buf(mavlink_ packet->roll = roll; packet->pitch = pitch; packet->yaw = yaw; - + packet->reset_counter = reset_counter; + mav_array_memcpy(packet->covariance, covariance, sizeof(float)*21); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GLOBAL_VISION_POSITION_ESTIMATE, (const char *)packet, MAVLINK_MSG_ID_GLOBAL_VISION_POSITION_ESTIMATE_MIN_LEN, MAVLINK_MSG_ID_GLOBAL_VISION_POSITION_ESTIMATE_LEN, MAVLINK_MSG_ID_GLOBAL_VISION_POSITION_ESTIMATE_CRC); #endif } @@ -339,6 +359,26 @@ static inline float mavlink_msg_global_vision_position_estimate_get_yaw(const ma return _MAV_RETURN_float(msg, 28); } +/** + * @brief Get field covariance from global_vision_position_estimate message + * + * @return Row-major representation of pose 6x6 cross-covariance matrix upper right triangle (states: x_global, y_global, z_global, roll, pitch, yaw; first six entries are the first ROW, next five entries are the second ROW, etc.). If unknown, assign NaN value to first element in the array. + */ +static inline uint16_t mavlink_msg_global_vision_position_estimate_get_covariance(const mavlink_message_t* msg, float *covariance) +{ + return _MAV_RETURN_float_array(msg, covariance, 21, 32); +} + +/** + * @brief Get field reset_counter from global_vision_position_estimate message + * + * @return Estimate reset counter. This should be incremented when the estimate resets in any of the dimensions (position, velocity, attitude, angular speed). This is designed to be used when e.g an external SLAM system detects a loop-closure and the estimate jumps. + */ +static inline uint8_t mavlink_msg_global_vision_position_estimate_get_reset_counter(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 116); +} + /** * @brief Decode a global_vision_position_estimate message into a struct * @@ -355,6 +395,8 @@ static inline void mavlink_msg_global_vision_position_estimate_decode(const mavl global_vision_position_estimate->roll = mavlink_msg_global_vision_position_estimate_get_roll(msg); global_vision_position_estimate->pitch = mavlink_msg_global_vision_position_estimate_get_pitch(msg); global_vision_position_estimate->yaw = mavlink_msg_global_vision_position_estimate_get_yaw(msg); + mavlink_msg_global_vision_position_estimate_get_covariance(msg, global_vision_position_estimate->covariance); + global_vision_position_estimate->reset_counter = mavlink_msg_global_vision_position_estimate_get_reset_counter(msg); #else uint8_t len = msg->len < MAVLINK_MSG_ID_GLOBAL_VISION_POSITION_ESTIMATE_LEN? msg->len : MAVLINK_MSG_ID_GLOBAL_VISION_POSITION_ESTIMATE_LEN; memset(global_vision_position_estimate, 0, MAVLINK_MSG_ID_GLOBAL_VISION_POSITION_ESTIMATE_LEN); diff --git a/lib/main/MAVLink/common/mavlink_msg_gps2_raw.h b/lib/main/MAVLink/common/mavlink_msg_gps2_raw.h index 330b7e96dd..5e741ef493 100755 --- a/lib/main/MAVLink/common/mavlink_msg_gps2_raw.h +++ b/lib/main/MAVLink/common/mavlink_msg_gps2_raw.h @@ -3,25 +3,26 @@ #define MAVLINK_MSG_ID_GPS2_RAW 124 - +MAVPACKED( typedef struct __mavlink_gps2_raw_t { uint64_t time_usec; /*< [us] Timestamp (UNIX Epoch time or time since system boot). The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude of the number.*/ int32_t lat; /*< [degE7] Latitude (WGS84)*/ int32_t lon; /*< [degE7] Longitude (WGS84)*/ int32_t alt; /*< [mm] Altitude (MSL). Positive for up.*/ uint32_t dgps_age; /*< [ms] Age of DGPS info*/ - uint16_t eph; /*< [cm] GPS HDOP horizontal dilution of position. If unknown, set to: UINT16_MAX*/ - uint16_t epv; /*< [cm] GPS VDOP vertical dilution of position. If unknown, set to: UINT16_MAX*/ + uint16_t eph; /*< GPS HDOP horizontal dilution of position (unitless). If unknown, set to: UINT16_MAX*/ + uint16_t epv; /*< GPS VDOP vertical dilution of position (unitless). If unknown, set to: UINT16_MAX*/ uint16_t vel; /*< [cm/s] GPS ground speed. If unknown, set to: UINT16_MAX*/ uint16_t cog; /*< [cdeg] Course over ground (NOT heading, but direction of movement): 0.0..359.99 degrees. If unknown, set to: UINT16_MAX*/ uint8_t fix_type; /*< GPS fix type.*/ uint8_t satellites_visible; /*< Number of satellites visible. If unknown, set to 255*/ uint8_t dgps_numch; /*< Number of DGPS satellites*/ -} mavlink_gps2_raw_t; + uint16_t yaw; /*< [cdeg] Yaw in earth frame from north. Use 0 if this GPS does not provide yaw. Use 65535 if this GPS is configured to provide yaw and is currently unable to provide it. Use 36000 for north.*/ +}) mavlink_gps2_raw_t; -#define MAVLINK_MSG_ID_GPS2_RAW_LEN 35 +#define MAVLINK_MSG_ID_GPS2_RAW_LEN 37 #define MAVLINK_MSG_ID_GPS2_RAW_MIN_LEN 35 -#define MAVLINK_MSG_ID_124_LEN 35 +#define MAVLINK_MSG_ID_124_LEN 37 #define MAVLINK_MSG_ID_124_MIN_LEN 35 #define MAVLINK_MSG_ID_GPS2_RAW_CRC 87 @@ -33,7 +34,7 @@ typedef struct __mavlink_gps2_raw_t { #define MAVLINK_MESSAGE_INFO_GPS2_RAW { \ 124, \ "GPS2_RAW", \ - 12, \ + 13, \ { { "time_usec", NULL, MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_gps2_raw_t, time_usec) }, \ { "fix_type", NULL, MAVLINK_TYPE_UINT8_T, 0, 32, offsetof(mavlink_gps2_raw_t, fix_type) }, \ { "lat", NULL, MAVLINK_TYPE_INT32_T, 0, 8, offsetof(mavlink_gps2_raw_t, lat) }, \ @@ -46,12 +47,13 @@ typedef struct __mavlink_gps2_raw_t { { "satellites_visible", NULL, MAVLINK_TYPE_UINT8_T, 0, 33, offsetof(mavlink_gps2_raw_t, satellites_visible) }, \ { "dgps_numch", NULL, MAVLINK_TYPE_UINT8_T, 0, 34, offsetof(mavlink_gps2_raw_t, dgps_numch) }, \ { "dgps_age", NULL, MAVLINK_TYPE_UINT32_T, 0, 20, offsetof(mavlink_gps2_raw_t, dgps_age) }, \ + { "yaw", NULL, MAVLINK_TYPE_UINT16_T, 0, 35, offsetof(mavlink_gps2_raw_t, yaw) }, \ } \ } #else #define MAVLINK_MESSAGE_INFO_GPS2_RAW { \ "GPS2_RAW", \ - 12, \ + 13, \ { { "time_usec", NULL, MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_gps2_raw_t, time_usec) }, \ { "fix_type", NULL, MAVLINK_TYPE_UINT8_T, 0, 32, offsetof(mavlink_gps2_raw_t, fix_type) }, \ { "lat", NULL, MAVLINK_TYPE_INT32_T, 0, 8, offsetof(mavlink_gps2_raw_t, lat) }, \ @@ -64,6 +66,7 @@ typedef struct __mavlink_gps2_raw_t { { "satellites_visible", NULL, MAVLINK_TYPE_UINT8_T, 0, 33, offsetof(mavlink_gps2_raw_t, satellites_visible) }, \ { "dgps_numch", NULL, MAVLINK_TYPE_UINT8_T, 0, 34, offsetof(mavlink_gps2_raw_t, dgps_numch) }, \ { "dgps_age", NULL, MAVLINK_TYPE_UINT32_T, 0, 20, offsetof(mavlink_gps2_raw_t, dgps_age) }, \ + { "yaw", NULL, MAVLINK_TYPE_UINT16_T, 0, 35, offsetof(mavlink_gps2_raw_t, yaw) }, \ } \ } #endif @@ -79,17 +82,18 @@ typedef struct __mavlink_gps2_raw_t { * @param lat [degE7] Latitude (WGS84) * @param lon [degE7] Longitude (WGS84) * @param alt [mm] Altitude (MSL). Positive for up. - * @param eph [cm] GPS HDOP horizontal dilution of position. If unknown, set to: UINT16_MAX - * @param epv [cm] GPS VDOP vertical dilution of position. If unknown, set to: UINT16_MAX + * @param eph GPS HDOP horizontal dilution of position (unitless). If unknown, set to: UINT16_MAX + * @param epv GPS VDOP vertical dilution of position (unitless). If unknown, set to: UINT16_MAX * @param vel [cm/s] GPS ground speed. If unknown, set to: UINT16_MAX * @param cog [cdeg] Course over ground (NOT heading, but direction of movement): 0.0..359.99 degrees. If unknown, set to: UINT16_MAX * @param satellites_visible Number of satellites visible. If unknown, set to 255 * @param dgps_numch Number of DGPS satellites * @param dgps_age [ms] Age of DGPS info + * @param yaw [cdeg] Yaw in earth frame from north. Use 0 if this GPS does not provide yaw. Use 65535 if this GPS is configured to provide yaw and is currently unable to provide it. Use 36000 for north. * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_gps2_raw_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint64_t time_usec, uint8_t fix_type, int32_t lat, int32_t lon, int32_t alt, uint16_t eph, uint16_t epv, uint16_t vel, uint16_t cog, uint8_t satellites_visible, uint8_t dgps_numch, uint32_t dgps_age) + uint64_t time_usec, uint8_t fix_type, int32_t lat, int32_t lon, int32_t alt, uint16_t eph, uint16_t epv, uint16_t vel, uint16_t cog, uint8_t satellites_visible, uint8_t dgps_numch, uint32_t dgps_age, uint16_t yaw) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_GPS2_RAW_LEN]; @@ -105,6 +109,7 @@ static inline uint16_t mavlink_msg_gps2_raw_pack(uint8_t system_id, uint8_t comp _mav_put_uint8_t(buf, 32, fix_type); _mav_put_uint8_t(buf, 33, satellites_visible); _mav_put_uint8_t(buf, 34, dgps_numch); + _mav_put_uint16_t(buf, 35, yaw); memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_GPS2_RAW_LEN); #else @@ -121,6 +126,7 @@ static inline uint16_t mavlink_msg_gps2_raw_pack(uint8_t system_id, uint8_t comp packet.fix_type = fix_type; packet.satellites_visible = satellites_visible; packet.dgps_numch = dgps_numch; + packet.yaw = yaw; memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_GPS2_RAW_LEN); #endif @@ -140,18 +146,19 @@ static inline uint16_t mavlink_msg_gps2_raw_pack(uint8_t system_id, uint8_t comp * @param lat [degE7] Latitude (WGS84) * @param lon [degE7] Longitude (WGS84) * @param alt [mm] Altitude (MSL). Positive for up. - * @param eph [cm] GPS HDOP horizontal dilution of position. If unknown, set to: UINT16_MAX - * @param epv [cm] GPS VDOP vertical dilution of position. If unknown, set to: UINT16_MAX + * @param eph GPS HDOP horizontal dilution of position (unitless). If unknown, set to: UINT16_MAX + * @param epv GPS VDOP vertical dilution of position (unitless). If unknown, set to: UINT16_MAX * @param vel [cm/s] GPS ground speed. If unknown, set to: UINT16_MAX * @param cog [cdeg] Course over ground (NOT heading, but direction of movement): 0.0..359.99 degrees. If unknown, set to: UINT16_MAX * @param satellites_visible Number of satellites visible. If unknown, set to 255 * @param dgps_numch Number of DGPS satellites * @param dgps_age [ms] Age of DGPS info + * @param yaw [cdeg] Yaw in earth frame from north. Use 0 if this GPS does not provide yaw. Use 65535 if this GPS is configured to provide yaw and is currently unable to provide it. Use 36000 for north. * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_gps2_raw_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, - uint64_t time_usec,uint8_t fix_type,int32_t lat,int32_t lon,int32_t alt,uint16_t eph,uint16_t epv,uint16_t vel,uint16_t cog,uint8_t satellites_visible,uint8_t dgps_numch,uint32_t dgps_age) + uint64_t time_usec,uint8_t fix_type,int32_t lat,int32_t lon,int32_t alt,uint16_t eph,uint16_t epv,uint16_t vel,uint16_t cog,uint8_t satellites_visible,uint8_t dgps_numch,uint32_t dgps_age,uint16_t yaw) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_GPS2_RAW_LEN]; @@ -167,6 +174,7 @@ static inline uint16_t mavlink_msg_gps2_raw_pack_chan(uint8_t system_id, uint8_t _mav_put_uint8_t(buf, 32, fix_type); _mav_put_uint8_t(buf, 33, satellites_visible); _mav_put_uint8_t(buf, 34, dgps_numch); + _mav_put_uint16_t(buf, 35, yaw); memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_GPS2_RAW_LEN); #else @@ -183,6 +191,7 @@ static inline uint16_t mavlink_msg_gps2_raw_pack_chan(uint8_t system_id, uint8_t packet.fix_type = fix_type; packet.satellites_visible = satellites_visible; packet.dgps_numch = dgps_numch; + packet.yaw = yaw; memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_GPS2_RAW_LEN); #endif @@ -201,7 +210,7 @@ static inline uint16_t mavlink_msg_gps2_raw_pack_chan(uint8_t system_id, uint8_t */ static inline uint16_t mavlink_msg_gps2_raw_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_gps2_raw_t* gps2_raw) { - return mavlink_msg_gps2_raw_pack(system_id, component_id, msg, gps2_raw->time_usec, gps2_raw->fix_type, gps2_raw->lat, gps2_raw->lon, gps2_raw->alt, gps2_raw->eph, gps2_raw->epv, gps2_raw->vel, gps2_raw->cog, gps2_raw->satellites_visible, gps2_raw->dgps_numch, gps2_raw->dgps_age); + return mavlink_msg_gps2_raw_pack(system_id, component_id, msg, gps2_raw->time_usec, gps2_raw->fix_type, gps2_raw->lat, gps2_raw->lon, gps2_raw->alt, gps2_raw->eph, gps2_raw->epv, gps2_raw->vel, gps2_raw->cog, gps2_raw->satellites_visible, gps2_raw->dgps_numch, gps2_raw->dgps_age, gps2_raw->yaw); } /** @@ -215,7 +224,7 @@ static inline uint16_t mavlink_msg_gps2_raw_encode(uint8_t system_id, uint8_t co */ static inline uint16_t mavlink_msg_gps2_raw_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_gps2_raw_t* gps2_raw) { - return mavlink_msg_gps2_raw_pack_chan(system_id, component_id, chan, msg, gps2_raw->time_usec, gps2_raw->fix_type, gps2_raw->lat, gps2_raw->lon, gps2_raw->alt, gps2_raw->eph, gps2_raw->epv, gps2_raw->vel, gps2_raw->cog, gps2_raw->satellites_visible, gps2_raw->dgps_numch, gps2_raw->dgps_age); + return mavlink_msg_gps2_raw_pack_chan(system_id, component_id, chan, msg, gps2_raw->time_usec, gps2_raw->fix_type, gps2_raw->lat, gps2_raw->lon, gps2_raw->alt, gps2_raw->eph, gps2_raw->epv, gps2_raw->vel, gps2_raw->cog, gps2_raw->satellites_visible, gps2_raw->dgps_numch, gps2_raw->dgps_age, gps2_raw->yaw); } /** @@ -227,17 +236,18 @@ static inline uint16_t mavlink_msg_gps2_raw_encode_chan(uint8_t system_id, uint8 * @param lat [degE7] Latitude (WGS84) * @param lon [degE7] Longitude (WGS84) * @param alt [mm] Altitude (MSL). Positive for up. - * @param eph [cm] GPS HDOP horizontal dilution of position. If unknown, set to: UINT16_MAX - * @param epv [cm] GPS VDOP vertical dilution of position. If unknown, set to: UINT16_MAX + * @param eph GPS HDOP horizontal dilution of position (unitless). If unknown, set to: UINT16_MAX + * @param epv GPS VDOP vertical dilution of position (unitless). If unknown, set to: UINT16_MAX * @param vel [cm/s] GPS ground speed. If unknown, set to: UINT16_MAX * @param cog [cdeg] Course over ground (NOT heading, but direction of movement): 0.0..359.99 degrees. If unknown, set to: UINT16_MAX * @param satellites_visible Number of satellites visible. If unknown, set to 255 * @param dgps_numch Number of DGPS satellites * @param dgps_age [ms] Age of DGPS info + * @param yaw [cdeg] Yaw in earth frame from north. Use 0 if this GPS does not provide yaw. Use 65535 if this GPS is configured to provide yaw and is currently unable to provide it. Use 36000 for north. */ #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS -static inline void mavlink_msg_gps2_raw_send(mavlink_channel_t chan, uint64_t time_usec, uint8_t fix_type, int32_t lat, int32_t lon, int32_t alt, uint16_t eph, uint16_t epv, uint16_t vel, uint16_t cog, uint8_t satellites_visible, uint8_t dgps_numch, uint32_t dgps_age) +static inline void mavlink_msg_gps2_raw_send(mavlink_channel_t chan, uint64_t time_usec, uint8_t fix_type, int32_t lat, int32_t lon, int32_t alt, uint16_t eph, uint16_t epv, uint16_t vel, uint16_t cog, uint8_t satellites_visible, uint8_t dgps_numch, uint32_t dgps_age, uint16_t yaw) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_GPS2_RAW_LEN]; @@ -253,6 +263,7 @@ static inline void mavlink_msg_gps2_raw_send(mavlink_channel_t chan, uint64_t ti _mav_put_uint8_t(buf, 32, fix_type); _mav_put_uint8_t(buf, 33, satellites_visible); _mav_put_uint8_t(buf, 34, dgps_numch); + _mav_put_uint16_t(buf, 35, yaw); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GPS2_RAW, buf, MAVLINK_MSG_ID_GPS2_RAW_MIN_LEN, MAVLINK_MSG_ID_GPS2_RAW_LEN, MAVLINK_MSG_ID_GPS2_RAW_CRC); #else @@ -269,6 +280,7 @@ static inline void mavlink_msg_gps2_raw_send(mavlink_channel_t chan, uint64_t ti packet.fix_type = fix_type; packet.satellites_visible = satellites_visible; packet.dgps_numch = dgps_numch; + packet.yaw = yaw; _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GPS2_RAW, (const char *)&packet, MAVLINK_MSG_ID_GPS2_RAW_MIN_LEN, MAVLINK_MSG_ID_GPS2_RAW_LEN, MAVLINK_MSG_ID_GPS2_RAW_CRC); #endif @@ -282,7 +294,7 @@ static inline void mavlink_msg_gps2_raw_send(mavlink_channel_t chan, uint64_t ti static inline void mavlink_msg_gps2_raw_send_struct(mavlink_channel_t chan, const mavlink_gps2_raw_t* gps2_raw) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - mavlink_msg_gps2_raw_send(chan, gps2_raw->time_usec, gps2_raw->fix_type, gps2_raw->lat, gps2_raw->lon, gps2_raw->alt, gps2_raw->eph, gps2_raw->epv, gps2_raw->vel, gps2_raw->cog, gps2_raw->satellites_visible, gps2_raw->dgps_numch, gps2_raw->dgps_age); + mavlink_msg_gps2_raw_send(chan, gps2_raw->time_usec, gps2_raw->fix_type, gps2_raw->lat, gps2_raw->lon, gps2_raw->alt, gps2_raw->eph, gps2_raw->epv, gps2_raw->vel, gps2_raw->cog, gps2_raw->satellites_visible, gps2_raw->dgps_numch, gps2_raw->dgps_age, gps2_raw->yaw); #else _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GPS2_RAW, (const char *)gps2_raw, MAVLINK_MSG_ID_GPS2_RAW_MIN_LEN, MAVLINK_MSG_ID_GPS2_RAW_LEN, MAVLINK_MSG_ID_GPS2_RAW_CRC); #endif @@ -296,7 +308,7 @@ static inline void mavlink_msg_gps2_raw_send_struct(mavlink_channel_t chan, cons is usually the receive buffer for the channel, and allows a reply to an incoming message with minimum stack space usage. */ -static inline void mavlink_msg_gps2_raw_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint64_t time_usec, uint8_t fix_type, int32_t lat, int32_t lon, int32_t alt, uint16_t eph, uint16_t epv, uint16_t vel, uint16_t cog, uint8_t satellites_visible, uint8_t dgps_numch, uint32_t dgps_age) +static inline void mavlink_msg_gps2_raw_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint64_t time_usec, uint8_t fix_type, int32_t lat, int32_t lon, int32_t alt, uint16_t eph, uint16_t epv, uint16_t vel, uint16_t cog, uint8_t satellites_visible, uint8_t dgps_numch, uint32_t dgps_age, uint16_t yaw) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char *buf = (char *)msgbuf; @@ -312,6 +324,7 @@ static inline void mavlink_msg_gps2_raw_send_buf(mavlink_message_t *msgbuf, mavl _mav_put_uint8_t(buf, 32, fix_type); _mav_put_uint8_t(buf, 33, satellites_visible); _mav_put_uint8_t(buf, 34, dgps_numch); + _mav_put_uint16_t(buf, 35, yaw); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GPS2_RAW, buf, MAVLINK_MSG_ID_GPS2_RAW_MIN_LEN, MAVLINK_MSG_ID_GPS2_RAW_LEN, MAVLINK_MSG_ID_GPS2_RAW_CRC); #else @@ -328,6 +341,7 @@ static inline void mavlink_msg_gps2_raw_send_buf(mavlink_message_t *msgbuf, mavl packet->fix_type = fix_type; packet->satellites_visible = satellites_visible; packet->dgps_numch = dgps_numch; + packet->yaw = yaw; _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GPS2_RAW, (const char *)packet, MAVLINK_MSG_ID_GPS2_RAW_MIN_LEN, MAVLINK_MSG_ID_GPS2_RAW_LEN, MAVLINK_MSG_ID_GPS2_RAW_CRC); #endif @@ -392,7 +406,7 @@ static inline int32_t mavlink_msg_gps2_raw_get_alt(const mavlink_message_t* msg) /** * @brief Get field eph from gps2_raw message * - * @return [cm] GPS HDOP horizontal dilution of position. If unknown, set to: UINT16_MAX + * @return GPS HDOP horizontal dilution of position (unitless). If unknown, set to: UINT16_MAX */ static inline uint16_t mavlink_msg_gps2_raw_get_eph(const mavlink_message_t* msg) { @@ -402,7 +416,7 @@ static inline uint16_t mavlink_msg_gps2_raw_get_eph(const mavlink_message_t* msg /** * @brief Get field epv from gps2_raw message * - * @return [cm] GPS VDOP vertical dilution of position. If unknown, set to: UINT16_MAX + * @return GPS VDOP vertical dilution of position (unitless). If unknown, set to: UINT16_MAX */ static inline uint16_t mavlink_msg_gps2_raw_get_epv(const mavlink_message_t* msg) { @@ -459,6 +473,16 @@ static inline uint32_t mavlink_msg_gps2_raw_get_dgps_age(const mavlink_message_t return _MAV_RETURN_uint32_t(msg, 20); } +/** + * @brief Get field yaw from gps2_raw message + * + * @return [cdeg] Yaw in earth frame from north. Use 0 if this GPS does not provide yaw. Use 65535 if this GPS is configured to provide yaw and is currently unable to provide it. Use 36000 for north. + */ +static inline uint16_t mavlink_msg_gps2_raw_get_yaw(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 35); +} + /** * @brief Decode a gps2_raw message into a struct * @@ -480,6 +504,7 @@ static inline void mavlink_msg_gps2_raw_decode(const mavlink_message_t* msg, mav gps2_raw->fix_type = mavlink_msg_gps2_raw_get_fix_type(msg); gps2_raw->satellites_visible = mavlink_msg_gps2_raw_get_satellites_visible(msg); gps2_raw->dgps_numch = mavlink_msg_gps2_raw_get_dgps_numch(msg); + gps2_raw->yaw = mavlink_msg_gps2_raw_get_yaw(msg); #else uint8_t len = msg->len < MAVLINK_MSG_ID_GPS2_RAW_LEN? msg->len : MAVLINK_MSG_ID_GPS2_RAW_LEN; memset(gps2_raw, 0, MAVLINK_MSG_ID_GPS2_RAW_LEN); diff --git a/lib/main/MAVLink/common/mavlink_msg_gps_global_origin.h b/lib/main/MAVLink/common/mavlink_msg_gps_global_origin.h index 7719543ee2..ccbde8fd14 100755 --- a/lib/main/MAVLink/common/mavlink_msg_gps_global_origin.h +++ b/lib/main/MAVLink/common/mavlink_msg_gps_global_origin.h @@ -3,16 +3,17 @@ #define MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN 49 - +MAVPACKED( typedef struct __mavlink_gps_global_origin_t { int32_t latitude; /*< [degE7] Latitude (WGS84)*/ int32_t longitude; /*< [degE7] Longitude (WGS84)*/ int32_t altitude; /*< [mm] Altitude (MSL). Positive for up.*/ -} mavlink_gps_global_origin_t; + uint64_t time_usec; /*< [us] Timestamp (UNIX Epoch time or time since system boot). The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude of the number.*/ +}) mavlink_gps_global_origin_t; -#define MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN_LEN 12 +#define MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN_LEN 20 #define MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN_MIN_LEN 12 -#define MAVLINK_MSG_ID_49_LEN 12 +#define MAVLINK_MSG_ID_49_LEN 20 #define MAVLINK_MSG_ID_49_MIN_LEN 12 #define MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN_CRC 39 @@ -24,19 +25,21 @@ typedef struct __mavlink_gps_global_origin_t { #define MAVLINK_MESSAGE_INFO_GPS_GLOBAL_ORIGIN { \ 49, \ "GPS_GLOBAL_ORIGIN", \ - 3, \ + 4, \ { { "latitude", NULL, MAVLINK_TYPE_INT32_T, 0, 0, offsetof(mavlink_gps_global_origin_t, latitude) }, \ { "longitude", NULL, MAVLINK_TYPE_INT32_T, 0, 4, offsetof(mavlink_gps_global_origin_t, longitude) }, \ { "altitude", NULL, MAVLINK_TYPE_INT32_T, 0, 8, offsetof(mavlink_gps_global_origin_t, altitude) }, \ + { "time_usec", NULL, MAVLINK_TYPE_UINT64_T, 0, 12, offsetof(mavlink_gps_global_origin_t, time_usec) }, \ } \ } #else #define MAVLINK_MESSAGE_INFO_GPS_GLOBAL_ORIGIN { \ "GPS_GLOBAL_ORIGIN", \ - 3, \ + 4, \ { { "latitude", NULL, MAVLINK_TYPE_INT32_T, 0, 0, offsetof(mavlink_gps_global_origin_t, latitude) }, \ { "longitude", NULL, MAVLINK_TYPE_INT32_T, 0, 4, offsetof(mavlink_gps_global_origin_t, longitude) }, \ { "altitude", NULL, MAVLINK_TYPE_INT32_T, 0, 8, offsetof(mavlink_gps_global_origin_t, altitude) }, \ + { "time_usec", NULL, MAVLINK_TYPE_UINT64_T, 0, 12, offsetof(mavlink_gps_global_origin_t, time_usec) }, \ } \ } #endif @@ -50,16 +53,18 @@ typedef struct __mavlink_gps_global_origin_t { * @param latitude [degE7] Latitude (WGS84) * @param longitude [degE7] Longitude (WGS84) * @param altitude [mm] Altitude (MSL). Positive for up. + * @param time_usec [us] Timestamp (UNIX Epoch time or time since system boot). The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude of the number. * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_gps_global_origin_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - int32_t latitude, int32_t longitude, int32_t altitude) + int32_t latitude, int32_t longitude, int32_t altitude, uint64_t time_usec) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN_LEN]; _mav_put_int32_t(buf, 0, latitude); _mav_put_int32_t(buf, 4, longitude); _mav_put_int32_t(buf, 8, altitude); + _mav_put_uint64_t(buf, 12, time_usec); memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN_LEN); #else @@ -67,6 +72,7 @@ static inline uint16_t mavlink_msg_gps_global_origin_pack(uint8_t system_id, uin packet.latitude = latitude; packet.longitude = longitude; packet.altitude = altitude; + packet.time_usec = time_usec; memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN_LEN); #endif @@ -84,17 +90,19 @@ static inline uint16_t mavlink_msg_gps_global_origin_pack(uint8_t system_id, uin * @param latitude [degE7] Latitude (WGS84) * @param longitude [degE7] Longitude (WGS84) * @param altitude [mm] Altitude (MSL). Positive for up. + * @param time_usec [us] Timestamp (UNIX Epoch time or time since system boot). The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude of the number. * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_gps_global_origin_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, - int32_t latitude,int32_t longitude,int32_t altitude) + int32_t latitude,int32_t longitude,int32_t altitude,uint64_t time_usec) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN_LEN]; _mav_put_int32_t(buf, 0, latitude); _mav_put_int32_t(buf, 4, longitude); _mav_put_int32_t(buf, 8, altitude); + _mav_put_uint64_t(buf, 12, time_usec); memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN_LEN); #else @@ -102,6 +110,7 @@ static inline uint16_t mavlink_msg_gps_global_origin_pack_chan(uint8_t system_id packet.latitude = latitude; packet.longitude = longitude; packet.altitude = altitude; + packet.time_usec = time_usec; memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN_LEN); #endif @@ -120,7 +129,7 @@ static inline uint16_t mavlink_msg_gps_global_origin_pack_chan(uint8_t system_id */ static inline uint16_t mavlink_msg_gps_global_origin_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_gps_global_origin_t* gps_global_origin) { - return mavlink_msg_gps_global_origin_pack(system_id, component_id, msg, gps_global_origin->latitude, gps_global_origin->longitude, gps_global_origin->altitude); + return mavlink_msg_gps_global_origin_pack(system_id, component_id, msg, gps_global_origin->latitude, gps_global_origin->longitude, gps_global_origin->altitude, gps_global_origin->time_usec); } /** @@ -134,7 +143,7 @@ static inline uint16_t mavlink_msg_gps_global_origin_encode(uint8_t system_id, u */ static inline uint16_t mavlink_msg_gps_global_origin_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_gps_global_origin_t* gps_global_origin) { - return mavlink_msg_gps_global_origin_pack_chan(system_id, component_id, chan, msg, gps_global_origin->latitude, gps_global_origin->longitude, gps_global_origin->altitude); + return mavlink_msg_gps_global_origin_pack_chan(system_id, component_id, chan, msg, gps_global_origin->latitude, gps_global_origin->longitude, gps_global_origin->altitude, gps_global_origin->time_usec); } /** @@ -144,16 +153,18 @@ static inline uint16_t mavlink_msg_gps_global_origin_encode_chan(uint8_t system_ * @param latitude [degE7] Latitude (WGS84) * @param longitude [degE7] Longitude (WGS84) * @param altitude [mm] Altitude (MSL). Positive for up. + * @param time_usec [us] Timestamp (UNIX Epoch time or time since system boot). The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude of the number. */ #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS -static inline void mavlink_msg_gps_global_origin_send(mavlink_channel_t chan, int32_t latitude, int32_t longitude, int32_t altitude) +static inline void mavlink_msg_gps_global_origin_send(mavlink_channel_t chan, int32_t latitude, int32_t longitude, int32_t altitude, uint64_t time_usec) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN_LEN]; _mav_put_int32_t(buf, 0, latitude); _mav_put_int32_t(buf, 4, longitude); _mav_put_int32_t(buf, 8, altitude); + _mav_put_uint64_t(buf, 12, time_usec); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN, buf, MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN_MIN_LEN, MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN_LEN, MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN_CRC); #else @@ -161,6 +172,7 @@ static inline void mavlink_msg_gps_global_origin_send(mavlink_channel_t chan, in packet.latitude = latitude; packet.longitude = longitude; packet.altitude = altitude; + packet.time_usec = time_usec; _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN, (const char *)&packet, MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN_MIN_LEN, MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN_LEN, MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN_CRC); #endif @@ -174,7 +186,7 @@ static inline void mavlink_msg_gps_global_origin_send(mavlink_channel_t chan, in static inline void mavlink_msg_gps_global_origin_send_struct(mavlink_channel_t chan, const mavlink_gps_global_origin_t* gps_global_origin) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - mavlink_msg_gps_global_origin_send(chan, gps_global_origin->latitude, gps_global_origin->longitude, gps_global_origin->altitude); + mavlink_msg_gps_global_origin_send(chan, gps_global_origin->latitude, gps_global_origin->longitude, gps_global_origin->altitude, gps_global_origin->time_usec); #else _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN, (const char *)gps_global_origin, MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN_MIN_LEN, MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN_LEN, MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN_CRC); #endif @@ -188,13 +200,14 @@ static inline void mavlink_msg_gps_global_origin_send_struct(mavlink_channel_t c is usually the receive buffer for the channel, and allows a reply to an incoming message with minimum stack space usage. */ -static inline void mavlink_msg_gps_global_origin_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, int32_t latitude, int32_t longitude, int32_t altitude) +static inline void mavlink_msg_gps_global_origin_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, int32_t latitude, int32_t longitude, int32_t altitude, uint64_t time_usec) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char *buf = (char *)msgbuf; _mav_put_int32_t(buf, 0, latitude); _mav_put_int32_t(buf, 4, longitude); _mav_put_int32_t(buf, 8, altitude); + _mav_put_uint64_t(buf, 12, time_usec); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN, buf, MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN_MIN_LEN, MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN_LEN, MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN_CRC); #else @@ -202,6 +215,7 @@ static inline void mavlink_msg_gps_global_origin_send_buf(mavlink_message_t *msg packet->latitude = latitude; packet->longitude = longitude; packet->altitude = altitude; + packet->time_usec = time_usec; _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN, (const char *)packet, MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN_MIN_LEN, MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN_LEN, MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN_CRC); #endif @@ -243,6 +257,16 @@ static inline int32_t mavlink_msg_gps_global_origin_get_altitude(const mavlink_m return _MAV_RETURN_int32_t(msg, 8); } +/** + * @brief Get field time_usec from gps_global_origin message + * + * @return [us] Timestamp (UNIX Epoch time or time since system boot). The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude of the number. + */ +static inline uint64_t mavlink_msg_gps_global_origin_get_time_usec(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint64_t(msg, 12); +} + /** * @brief Decode a gps_global_origin message into a struct * @@ -255,6 +279,7 @@ static inline void mavlink_msg_gps_global_origin_decode(const mavlink_message_t* gps_global_origin->latitude = mavlink_msg_gps_global_origin_get_latitude(msg); gps_global_origin->longitude = mavlink_msg_gps_global_origin_get_longitude(msg); gps_global_origin->altitude = mavlink_msg_gps_global_origin_get_altitude(msg); + gps_global_origin->time_usec = mavlink_msg_gps_global_origin_get_time_usec(msg); #else uint8_t len = msg->len < MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN_LEN? msg->len : MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN_LEN; memset(gps_global_origin, 0, MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN_LEN); diff --git a/lib/main/MAVLink/common/mavlink_msg_gps_input.h b/lib/main/MAVLink/common/mavlink_msg_gps_input.h index 6268b11af3..de76ddbf1d 100755 --- a/lib/main/MAVLink/common/mavlink_msg_gps_input.h +++ b/lib/main/MAVLink/common/mavlink_msg_gps_input.h @@ -3,15 +3,15 @@ #define MAVLINK_MSG_ID_GPS_INPUT 232 - +MAVPACKED( typedef struct __mavlink_gps_input_t { uint64_t time_usec; /*< [us] Timestamp (UNIX Epoch time or time since system boot). The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude of the number.*/ uint32_t time_week_ms; /*< [ms] GPS time (from start of GPS week)*/ int32_t lat; /*< [degE7] Latitude (WGS84)*/ int32_t lon; /*< [degE7] Longitude (WGS84)*/ float alt; /*< [m] Altitude (MSL). Positive for up.*/ - float hdop; /*< [m] GPS HDOP horizontal dilution of position*/ - float vdop; /*< [m] GPS VDOP vertical dilution of position*/ + float hdop; /*< GPS HDOP horizontal dilution of position (unitless). If unknown, set to: UINT16_MAX*/ + float vdop; /*< GPS VDOP vertical dilution of position (unitless). If unknown, set to: UINT16_MAX*/ float vn; /*< [m/s] GPS velocity in north direction in earth-fixed NED frame*/ float ve; /*< [m/s] GPS velocity in east direction in earth-fixed NED frame*/ float vd; /*< [m/s] GPS velocity in down direction in earth-fixed NED frame*/ @@ -23,11 +23,12 @@ typedef struct __mavlink_gps_input_t { uint8_t gps_id; /*< ID of the GPS for multiple GPS inputs*/ uint8_t fix_type; /*< 0-1: no fix, 2: 2D fix, 3: 3D fix. 4: 3D with DGPS. 5: 3D with RTK*/ uint8_t satellites_visible; /*< Number of satellites visible.*/ -} mavlink_gps_input_t; + uint16_t yaw; /*< [cdeg] Yaw of vehicle relative to Earth's North, zero means not available, use 36000 for north*/ +}) mavlink_gps_input_t; -#define MAVLINK_MSG_ID_GPS_INPUT_LEN 63 +#define MAVLINK_MSG_ID_GPS_INPUT_LEN 65 #define MAVLINK_MSG_ID_GPS_INPUT_MIN_LEN 63 -#define MAVLINK_MSG_ID_232_LEN 63 +#define MAVLINK_MSG_ID_232_LEN 65 #define MAVLINK_MSG_ID_232_MIN_LEN 63 #define MAVLINK_MSG_ID_GPS_INPUT_CRC 151 @@ -39,7 +40,7 @@ typedef struct __mavlink_gps_input_t { #define MAVLINK_MESSAGE_INFO_GPS_INPUT { \ 232, \ "GPS_INPUT", \ - 18, \ + 19, \ { { "time_usec", NULL, MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_gps_input_t, time_usec) }, \ { "gps_id", NULL, MAVLINK_TYPE_UINT8_T, 0, 60, offsetof(mavlink_gps_input_t, gps_id) }, \ { "ignore_flags", NULL, MAVLINK_TYPE_UINT16_T, 0, 56, offsetof(mavlink_gps_input_t, ignore_flags) }, \ @@ -58,12 +59,13 @@ typedef struct __mavlink_gps_input_t { { "horiz_accuracy", NULL, MAVLINK_TYPE_FLOAT, 0, 48, offsetof(mavlink_gps_input_t, horiz_accuracy) }, \ { "vert_accuracy", NULL, MAVLINK_TYPE_FLOAT, 0, 52, offsetof(mavlink_gps_input_t, vert_accuracy) }, \ { "satellites_visible", NULL, MAVLINK_TYPE_UINT8_T, 0, 62, offsetof(mavlink_gps_input_t, satellites_visible) }, \ + { "yaw", NULL, MAVLINK_TYPE_UINT16_T, 0, 63, offsetof(mavlink_gps_input_t, yaw) }, \ } \ } #else #define MAVLINK_MESSAGE_INFO_GPS_INPUT { \ "GPS_INPUT", \ - 18, \ + 19, \ { { "time_usec", NULL, MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_gps_input_t, time_usec) }, \ { "gps_id", NULL, MAVLINK_TYPE_UINT8_T, 0, 60, offsetof(mavlink_gps_input_t, gps_id) }, \ { "ignore_flags", NULL, MAVLINK_TYPE_UINT16_T, 0, 56, offsetof(mavlink_gps_input_t, ignore_flags) }, \ @@ -82,6 +84,7 @@ typedef struct __mavlink_gps_input_t { { "horiz_accuracy", NULL, MAVLINK_TYPE_FLOAT, 0, 48, offsetof(mavlink_gps_input_t, horiz_accuracy) }, \ { "vert_accuracy", NULL, MAVLINK_TYPE_FLOAT, 0, 52, offsetof(mavlink_gps_input_t, vert_accuracy) }, \ { "satellites_visible", NULL, MAVLINK_TYPE_UINT8_T, 0, 62, offsetof(mavlink_gps_input_t, satellites_visible) }, \ + { "yaw", NULL, MAVLINK_TYPE_UINT16_T, 0, 63, offsetof(mavlink_gps_input_t, yaw) }, \ } \ } #endif @@ -101,8 +104,8 @@ typedef struct __mavlink_gps_input_t { * @param lat [degE7] Latitude (WGS84) * @param lon [degE7] Longitude (WGS84) * @param alt [m] Altitude (MSL). Positive for up. - * @param hdop [m] GPS HDOP horizontal dilution of position - * @param vdop [m] GPS VDOP vertical dilution of position + * @param hdop GPS HDOP horizontal dilution of position (unitless). If unknown, set to: UINT16_MAX + * @param vdop GPS VDOP vertical dilution of position (unitless). If unknown, set to: UINT16_MAX * @param vn [m/s] GPS velocity in north direction in earth-fixed NED frame * @param ve [m/s] GPS velocity in east direction in earth-fixed NED frame * @param vd [m/s] GPS velocity in down direction in earth-fixed NED frame @@ -110,10 +113,11 @@ typedef struct __mavlink_gps_input_t { * @param horiz_accuracy [m] GPS horizontal accuracy * @param vert_accuracy [m] GPS vertical accuracy * @param satellites_visible Number of satellites visible. + * @param yaw [cdeg] Yaw of vehicle relative to Earth's North, zero means not available, use 36000 for north * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_gps_input_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint64_t time_usec, uint8_t gps_id, uint16_t ignore_flags, uint32_t time_week_ms, uint16_t time_week, uint8_t fix_type, int32_t lat, int32_t lon, float alt, float hdop, float vdop, float vn, float ve, float vd, float speed_accuracy, float horiz_accuracy, float vert_accuracy, uint8_t satellites_visible) + uint64_t time_usec, uint8_t gps_id, uint16_t ignore_flags, uint32_t time_week_ms, uint16_t time_week, uint8_t fix_type, int32_t lat, int32_t lon, float alt, float hdop, float vdop, float vn, float ve, float vd, float speed_accuracy, float horiz_accuracy, float vert_accuracy, uint8_t satellites_visible, uint16_t yaw) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_GPS_INPUT_LEN]; @@ -135,6 +139,7 @@ static inline uint16_t mavlink_msg_gps_input_pack(uint8_t system_id, uint8_t com _mav_put_uint8_t(buf, 60, gps_id); _mav_put_uint8_t(buf, 61, fix_type); _mav_put_uint8_t(buf, 62, satellites_visible); + _mav_put_uint16_t(buf, 63, yaw); memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_GPS_INPUT_LEN); #else @@ -157,6 +162,7 @@ static inline uint16_t mavlink_msg_gps_input_pack(uint8_t system_id, uint8_t com packet.gps_id = gps_id; packet.fix_type = fix_type; packet.satellites_visible = satellites_visible; + packet.yaw = yaw; memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_GPS_INPUT_LEN); #endif @@ -180,8 +186,8 @@ static inline uint16_t mavlink_msg_gps_input_pack(uint8_t system_id, uint8_t com * @param lat [degE7] Latitude (WGS84) * @param lon [degE7] Longitude (WGS84) * @param alt [m] Altitude (MSL). Positive for up. - * @param hdop [m] GPS HDOP horizontal dilution of position - * @param vdop [m] GPS VDOP vertical dilution of position + * @param hdop GPS HDOP horizontal dilution of position (unitless). If unknown, set to: UINT16_MAX + * @param vdop GPS VDOP vertical dilution of position (unitless). If unknown, set to: UINT16_MAX * @param vn [m/s] GPS velocity in north direction in earth-fixed NED frame * @param ve [m/s] GPS velocity in east direction in earth-fixed NED frame * @param vd [m/s] GPS velocity in down direction in earth-fixed NED frame @@ -189,11 +195,12 @@ static inline uint16_t mavlink_msg_gps_input_pack(uint8_t system_id, uint8_t com * @param horiz_accuracy [m] GPS horizontal accuracy * @param vert_accuracy [m] GPS vertical accuracy * @param satellites_visible Number of satellites visible. + * @param yaw [cdeg] Yaw of vehicle relative to Earth's North, zero means not available, use 36000 for north * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_gps_input_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, - uint64_t time_usec,uint8_t gps_id,uint16_t ignore_flags,uint32_t time_week_ms,uint16_t time_week,uint8_t fix_type,int32_t lat,int32_t lon,float alt,float hdop,float vdop,float vn,float ve,float vd,float speed_accuracy,float horiz_accuracy,float vert_accuracy,uint8_t satellites_visible) + uint64_t time_usec,uint8_t gps_id,uint16_t ignore_flags,uint32_t time_week_ms,uint16_t time_week,uint8_t fix_type,int32_t lat,int32_t lon,float alt,float hdop,float vdop,float vn,float ve,float vd,float speed_accuracy,float horiz_accuracy,float vert_accuracy,uint8_t satellites_visible,uint16_t yaw) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_GPS_INPUT_LEN]; @@ -215,6 +222,7 @@ static inline uint16_t mavlink_msg_gps_input_pack_chan(uint8_t system_id, uint8_ _mav_put_uint8_t(buf, 60, gps_id); _mav_put_uint8_t(buf, 61, fix_type); _mav_put_uint8_t(buf, 62, satellites_visible); + _mav_put_uint16_t(buf, 63, yaw); memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_GPS_INPUT_LEN); #else @@ -237,6 +245,7 @@ static inline uint16_t mavlink_msg_gps_input_pack_chan(uint8_t system_id, uint8_ packet.gps_id = gps_id; packet.fix_type = fix_type; packet.satellites_visible = satellites_visible; + packet.yaw = yaw; memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_GPS_INPUT_LEN); #endif @@ -255,7 +264,7 @@ static inline uint16_t mavlink_msg_gps_input_pack_chan(uint8_t system_id, uint8_ */ static inline uint16_t mavlink_msg_gps_input_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_gps_input_t* gps_input) { - return mavlink_msg_gps_input_pack(system_id, component_id, msg, gps_input->time_usec, gps_input->gps_id, gps_input->ignore_flags, gps_input->time_week_ms, gps_input->time_week, gps_input->fix_type, gps_input->lat, gps_input->lon, gps_input->alt, gps_input->hdop, gps_input->vdop, gps_input->vn, gps_input->ve, gps_input->vd, gps_input->speed_accuracy, gps_input->horiz_accuracy, gps_input->vert_accuracy, gps_input->satellites_visible); + return mavlink_msg_gps_input_pack(system_id, component_id, msg, gps_input->time_usec, gps_input->gps_id, gps_input->ignore_flags, gps_input->time_week_ms, gps_input->time_week, gps_input->fix_type, gps_input->lat, gps_input->lon, gps_input->alt, gps_input->hdop, gps_input->vdop, gps_input->vn, gps_input->ve, gps_input->vd, gps_input->speed_accuracy, gps_input->horiz_accuracy, gps_input->vert_accuracy, gps_input->satellites_visible, gps_input->yaw); } /** @@ -269,7 +278,7 @@ static inline uint16_t mavlink_msg_gps_input_encode(uint8_t system_id, uint8_t c */ static inline uint16_t mavlink_msg_gps_input_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_gps_input_t* gps_input) { - return mavlink_msg_gps_input_pack_chan(system_id, component_id, chan, msg, gps_input->time_usec, gps_input->gps_id, gps_input->ignore_flags, gps_input->time_week_ms, gps_input->time_week, gps_input->fix_type, gps_input->lat, gps_input->lon, gps_input->alt, gps_input->hdop, gps_input->vdop, gps_input->vn, gps_input->ve, gps_input->vd, gps_input->speed_accuracy, gps_input->horiz_accuracy, gps_input->vert_accuracy, gps_input->satellites_visible); + return mavlink_msg_gps_input_pack_chan(system_id, component_id, chan, msg, gps_input->time_usec, gps_input->gps_id, gps_input->ignore_flags, gps_input->time_week_ms, gps_input->time_week, gps_input->fix_type, gps_input->lat, gps_input->lon, gps_input->alt, gps_input->hdop, gps_input->vdop, gps_input->vn, gps_input->ve, gps_input->vd, gps_input->speed_accuracy, gps_input->horiz_accuracy, gps_input->vert_accuracy, gps_input->satellites_visible, gps_input->yaw); } /** @@ -285,8 +294,8 @@ static inline uint16_t mavlink_msg_gps_input_encode_chan(uint8_t system_id, uint * @param lat [degE7] Latitude (WGS84) * @param lon [degE7] Longitude (WGS84) * @param alt [m] Altitude (MSL). Positive for up. - * @param hdop [m] GPS HDOP horizontal dilution of position - * @param vdop [m] GPS VDOP vertical dilution of position + * @param hdop GPS HDOP horizontal dilution of position (unitless). If unknown, set to: UINT16_MAX + * @param vdop GPS VDOP vertical dilution of position (unitless). If unknown, set to: UINT16_MAX * @param vn [m/s] GPS velocity in north direction in earth-fixed NED frame * @param ve [m/s] GPS velocity in east direction in earth-fixed NED frame * @param vd [m/s] GPS velocity in down direction in earth-fixed NED frame @@ -294,10 +303,11 @@ static inline uint16_t mavlink_msg_gps_input_encode_chan(uint8_t system_id, uint * @param horiz_accuracy [m] GPS horizontal accuracy * @param vert_accuracy [m] GPS vertical accuracy * @param satellites_visible Number of satellites visible. + * @param yaw [cdeg] Yaw of vehicle relative to Earth's North, zero means not available, use 36000 for north */ #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS -static inline void mavlink_msg_gps_input_send(mavlink_channel_t chan, uint64_t time_usec, uint8_t gps_id, uint16_t ignore_flags, uint32_t time_week_ms, uint16_t time_week, uint8_t fix_type, int32_t lat, int32_t lon, float alt, float hdop, float vdop, float vn, float ve, float vd, float speed_accuracy, float horiz_accuracy, float vert_accuracy, uint8_t satellites_visible) +static inline void mavlink_msg_gps_input_send(mavlink_channel_t chan, uint64_t time_usec, uint8_t gps_id, uint16_t ignore_flags, uint32_t time_week_ms, uint16_t time_week, uint8_t fix_type, int32_t lat, int32_t lon, float alt, float hdop, float vdop, float vn, float ve, float vd, float speed_accuracy, float horiz_accuracy, float vert_accuracy, uint8_t satellites_visible, uint16_t yaw) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_GPS_INPUT_LEN]; @@ -319,6 +329,7 @@ static inline void mavlink_msg_gps_input_send(mavlink_channel_t chan, uint64_t t _mav_put_uint8_t(buf, 60, gps_id); _mav_put_uint8_t(buf, 61, fix_type); _mav_put_uint8_t(buf, 62, satellites_visible); + _mav_put_uint16_t(buf, 63, yaw); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GPS_INPUT, buf, MAVLINK_MSG_ID_GPS_INPUT_MIN_LEN, MAVLINK_MSG_ID_GPS_INPUT_LEN, MAVLINK_MSG_ID_GPS_INPUT_CRC); #else @@ -341,6 +352,7 @@ static inline void mavlink_msg_gps_input_send(mavlink_channel_t chan, uint64_t t packet.gps_id = gps_id; packet.fix_type = fix_type; packet.satellites_visible = satellites_visible; + packet.yaw = yaw; _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GPS_INPUT, (const char *)&packet, MAVLINK_MSG_ID_GPS_INPUT_MIN_LEN, MAVLINK_MSG_ID_GPS_INPUT_LEN, MAVLINK_MSG_ID_GPS_INPUT_CRC); #endif @@ -354,7 +366,7 @@ static inline void mavlink_msg_gps_input_send(mavlink_channel_t chan, uint64_t t static inline void mavlink_msg_gps_input_send_struct(mavlink_channel_t chan, const mavlink_gps_input_t* gps_input) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - mavlink_msg_gps_input_send(chan, gps_input->time_usec, gps_input->gps_id, gps_input->ignore_flags, gps_input->time_week_ms, gps_input->time_week, gps_input->fix_type, gps_input->lat, gps_input->lon, gps_input->alt, gps_input->hdop, gps_input->vdop, gps_input->vn, gps_input->ve, gps_input->vd, gps_input->speed_accuracy, gps_input->horiz_accuracy, gps_input->vert_accuracy, gps_input->satellites_visible); + mavlink_msg_gps_input_send(chan, gps_input->time_usec, gps_input->gps_id, gps_input->ignore_flags, gps_input->time_week_ms, gps_input->time_week, gps_input->fix_type, gps_input->lat, gps_input->lon, gps_input->alt, gps_input->hdop, gps_input->vdop, gps_input->vn, gps_input->ve, gps_input->vd, gps_input->speed_accuracy, gps_input->horiz_accuracy, gps_input->vert_accuracy, gps_input->satellites_visible, gps_input->yaw); #else _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GPS_INPUT, (const char *)gps_input, MAVLINK_MSG_ID_GPS_INPUT_MIN_LEN, MAVLINK_MSG_ID_GPS_INPUT_LEN, MAVLINK_MSG_ID_GPS_INPUT_CRC); #endif @@ -368,7 +380,7 @@ static inline void mavlink_msg_gps_input_send_struct(mavlink_channel_t chan, con is usually the receive buffer for the channel, and allows a reply to an incoming message with minimum stack space usage. */ -static inline void mavlink_msg_gps_input_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint64_t time_usec, uint8_t gps_id, uint16_t ignore_flags, uint32_t time_week_ms, uint16_t time_week, uint8_t fix_type, int32_t lat, int32_t lon, float alt, float hdop, float vdop, float vn, float ve, float vd, float speed_accuracy, float horiz_accuracy, float vert_accuracy, uint8_t satellites_visible) +static inline void mavlink_msg_gps_input_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint64_t time_usec, uint8_t gps_id, uint16_t ignore_flags, uint32_t time_week_ms, uint16_t time_week, uint8_t fix_type, int32_t lat, int32_t lon, float alt, float hdop, float vdop, float vn, float ve, float vd, float speed_accuracy, float horiz_accuracy, float vert_accuracy, uint8_t satellites_visible, uint16_t yaw) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char *buf = (char *)msgbuf; @@ -390,6 +402,7 @@ static inline void mavlink_msg_gps_input_send_buf(mavlink_message_t *msgbuf, mav _mav_put_uint8_t(buf, 60, gps_id); _mav_put_uint8_t(buf, 61, fix_type); _mav_put_uint8_t(buf, 62, satellites_visible); + _mav_put_uint16_t(buf, 63, yaw); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GPS_INPUT, buf, MAVLINK_MSG_ID_GPS_INPUT_MIN_LEN, MAVLINK_MSG_ID_GPS_INPUT_LEN, MAVLINK_MSG_ID_GPS_INPUT_CRC); #else @@ -412,6 +425,7 @@ static inline void mavlink_msg_gps_input_send_buf(mavlink_message_t *msgbuf, mav packet->gps_id = gps_id; packet->fix_type = fix_type; packet->satellites_visible = satellites_visible; + packet->yaw = yaw; _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GPS_INPUT, (const char *)packet, MAVLINK_MSG_ID_GPS_INPUT_MIN_LEN, MAVLINK_MSG_ID_GPS_INPUT_LEN, MAVLINK_MSG_ID_GPS_INPUT_CRC); #endif @@ -516,7 +530,7 @@ static inline float mavlink_msg_gps_input_get_alt(const mavlink_message_t* msg) /** * @brief Get field hdop from gps_input message * - * @return [m] GPS HDOP horizontal dilution of position + * @return GPS HDOP horizontal dilution of position (unitless). If unknown, set to: UINT16_MAX */ static inline float mavlink_msg_gps_input_get_hdop(const mavlink_message_t* msg) { @@ -526,7 +540,7 @@ static inline float mavlink_msg_gps_input_get_hdop(const mavlink_message_t* msg) /** * @brief Get field vdop from gps_input message * - * @return [m] GPS VDOP vertical dilution of position + * @return GPS VDOP vertical dilution of position (unitless). If unknown, set to: UINT16_MAX */ static inline float mavlink_msg_gps_input_get_vdop(const mavlink_message_t* msg) { @@ -603,6 +617,16 @@ static inline uint8_t mavlink_msg_gps_input_get_satellites_visible(const mavlink return _MAV_RETURN_uint8_t(msg, 62); } +/** + * @brief Get field yaw from gps_input message + * + * @return [cdeg] Yaw of vehicle relative to Earth's North, zero means not available, use 36000 for north + */ +static inline uint16_t mavlink_msg_gps_input_get_yaw(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 63); +} + /** * @brief Decode a gps_input message into a struct * @@ -630,6 +654,7 @@ static inline void mavlink_msg_gps_input_decode(const mavlink_message_t* msg, ma gps_input->gps_id = mavlink_msg_gps_input_get_gps_id(msg); gps_input->fix_type = mavlink_msg_gps_input_get_fix_type(msg); gps_input->satellites_visible = mavlink_msg_gps_input_get_satellites_visible(msg); + gps_input->yaw = mavlink_msg_gps_input_get_yaw(msg); #else uint8_t len = msg->len < MAVLINK_MSG_ID_GPS_INPUT_LEN? msg->len : MAVLINK_MSG_ID_GPS_INPUT_LEN; memset(gps_input, 0, MAVLINK_MSG_ID_GPS_INPUT_LEN); diff --git a/lib/main/MAVLink/common/mavlink_msg_gps_raw_int.h b/lib/main/MAVLink/common/mavlink_msg_gps_raw_int.h index 6e22537c93..b87d82e489 100755 --- a/lib/main/MAVLink/common/mavlink_msg_gps_raw_int.h +++ b/lib/main/MAVLink/common/mavlink_msg_gps_raw_int.h @@ -3,7 +3,7 @@ #define MAVLINK_MSG_ID_GPS_RAW_INT 24 - +MAVPACKED( typedef struct __mavlink_gps_raw_int_t { uint64_t time_usec; /*< [us] Timestamp (UNIX Epoch time or time since system boot). The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude of the number.*/ int32_t lat; /*< [degE7] Latitude (WGS84, EGM96 ellipsoid)*/ @@ -15,11 +15,17 @@ typedef struct __mavlink_gps_raw_int_t { uint16_t cog; /*< [cdeg] Course over ground (NOT heading, but direction of movement) in degrees * 100, 0.0..359.99 degrees. If unknown, set to: UINT16_MAX*/ uint8_t fix_type; /*< GPS fix type.*/ uint8_t satellites_visible; /*< Number of satellites visible. If unknown, set to 255*/ -} mavlink_gps_raw_int_t; + int32_t alt_ellipsoid; /*< [mm] Altitude (above WGS84, EGM96 ellipsoid). Positive for up.*/ + uint32_t h_acc; /*< [mm] Position uncertainty.*/ + uint32_t v_acc; /*< [mm] Altitude uncertainty.*/ + uint32_t vel_acc; /*< [mm] Speed uncertainty.*/ + uint32_t hdg_acc; /*< [degE5] Heading / track uncertainty*/ + uint16_t yaw; /*< [cdeg] Yaw in earth frame from north. Use 0 if this GPS does not provide yaw. Use 65535 if this GPS is configured to provide yaw and is currently unable to provide it. Use 36000 for north.*/ +}) mavlink_gps_raw_int_t; -#define MAVLINK_MSG_ID_GPS_RAW_INT_LEN 30 +#define MAVLINK_MSG_ID_GPS_RAW_INT_LEN 52 #define MAVLINK_MSG_ID_GPS_RAW_INT_MIN_LEN 30 -#define MAVLINK_MSG_ID_24_LEN 30 +#define MAVLINK_MSG_ID_24_LEN 52 #define MAVLINK_MSG_ID_24_MIN_LEN 30 #define MAVLINK_MSG_ID_GPS_RAW_INT_CRC 24 @@ -31,7 +37,7 @@ typedef struct __mavlink_gps_raw_int_t { #define MAVLINK_MESSAGE_INFO_GPS_RAW_INT { \ 24, \ "GPS_RAW_INT", \ - 10, \ + 16, \ { { "time_usec", NULL, MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_gps_raw_int_t, time_usec) }, \ { "fix_type", NULL, MAVLINK_TYPE_UINT8_T, 0, 28, offsetof(mavlink_gps_raw_int_t, fix_type) }, \ { "lat", NULL, MAVLINK_TYPE_INT32_T, 0, 8, offsetof(mavlink_gps_raw_int_t, lat) }, \ @@ -42,12 +48,18 @@ typedef struct __mavlink_gps_raw_int_t { { "vel", NULL, MAVLINK_TYPE_UINT16_T, 0, 24, offsetof(mavlink_gps_raw_int_t, vel) }, \ { "cog", NULL, MAVLINK_TYPE_UINT16_T, 0, 26, offsetof(mavlink_gps_raw_int_t, cog) }, \ { "satellites_visible", NULL, MAVLINK_TYPE_UINT8_T, 0, 29, offsetof(mavlink_gps_raw_int_t, satellites_visible) }, \ + { "alt_ellipsoid", NULL, MAVLINK_TYPE_INT32_T, 0, 30, offsetof(mavlink_gps_raw_int_t, alt_ellipsoid) }, \ + { "h_acc", NULL, MAVLINK_TYPE_UINT32_T, 0, 34, offsetof(mavlink_gps_raw_int_t, h_acc) }, \ + { "v_acc", NULL, MAVLINK_TYPE_UINT32_T, 0, 38, offsetof(mavlink_gps_raw_int_t, v_acc) }, \ + { "vel_acc", NULL, MAVLINK_TYPE_UINT32_T, 0, 42, offsetof(mavlink_gps_raw_int_t, vel_acc) }, \ + { "hdg_acc", NULL, MAVLINK_TYPE_UINT32_T, 0, 46, offsetof(mavlink_gps_raw_int_t, hdg_acc) }, \ + { "yaw", NULL, MAVLINK_TYPE_UINT16_T, 0, 50, offsetof(mavlink_gps_raw_int_t, yaw) }, \ } \ } #else #define MAVLINK_MESSAGE_INFO_GPS_RAW_INT { \ "GPS_RAW_INT", \ - 10, \ + 16, \ { { "time_usec", NULL, MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_gps_raw_int_t, time_usec) }, \ { "fix_type", NULL, MAVLINK_TYPE_UINT8_T, 0, 28, offsetof(mavlink_gps_raw_int_t, fix_type) }, \ { "lat", NULL, MAVLINK_TYPE_INT32_T, 0, 8, offsetof(mavlink_gps_raw_int_t, lat) }, \ @@ -58,6 +70,12 @@ typedef struct __mavlink_gps_raw_int_t { { "vel", NULL, MAVLINK_TYPE_UINT16_T, 0, 24, offsetof(mavlink_gps_raw_int_t, vel) }, \ { "cog", NULL, MAVLINK_TYPE_UINT16_T, 0, 26, offsetof(mavlink_gps_raw_int_t, cog) }, \ { "satellites_visible", NULL, MAVLINK_TYPE_UINT8_T, 0, 29, offsetof(mavlink_gps_raw_int_t, satellites_visible) }, \ + { "alt_ellipsoid", NULL, MAVLINK_TYPE_INT32_T, 0, 30, offsetof(mavlink_gps_raw_int_t, alt_ellipsoid) }, \ + { "h_acc", NULL, MAVLINK_TYPE_UINT32_T, 0, 34, offsetof(mavlink_gps_raw_int_t, h_acc) }, \ + { "v_acc", NULL, MAVLINK_TYPE_UINT32_T, 0, 38, offsetof(mavlink_gps_raw_int_t, v_acc) }, \ + { "vel_acc", NULL, MAVLINK_TYPE_UINT32_T, 0, 42, offsetof(mavlink_gps_raw_int_t, vel_acc) }, \ + { "hdg_acc", NULL, MAVLINK_TYPE_UINT32_T, 0, 46, offsetof(mavlink_gps_raw_int_t, hdg_acc) }, \ + { "yaw", NULL, MAVLINK_TYPE_UINT16_T, 0, 50, offsetof(mavlink_gps_raw_int_t, yaw) }, \ } \ } #endif @@ -78,10 +96,16 @@ typedef struct __mavlink_gps_raw_int_t { * @param vel [cm/s] GPS ground speed. If unknown, set to: UINT16_MAX * @param cog [cdeg] Course over ground (NOT heading, but direction of movement) in degrees * 100, 0.0..359.99 degrees. If unknown, set to: UINT16_MAX * @param satellites_visible Number of satellites visible. If unknown, set to 255 + * @param alt_ellipsoid [mm] Altitude (above WGS84, EGM96 ellipsoid). Positive for up. + * @param h_acc [mm] Position uncertainty. + * @param v_acc [mm] Altitude uncertainty. + * @param vel_acc [mm] Speed uncertainty. + * @param hdg_acc [degE5] Heading / track uncertainty + * @param yaw [cdeg] Yaw in earth frame from north. Use 0 if this GPS does not provide yaw. Use 65535 if this GPS is configured to provide yaw and is currently unable to provide it. Use 36000 for north. * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_gps_raw_int_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint64_t time_usec, uint8_t fix_type, int32_t lat, int32_t lon, int32_t alt, uint16_t eph, uint16_t epv, uint16_t vel, uint16_t cog, uint8_t satellites_visible) + uint64_t time_usec, uint8_t fix_type, int32_t lat, int32_t lon, int32_t alt, uint16_t eph, uint16_t epv, uint16_t vel, uint16_t cog, uint8_t satellites_visible, int32_t alt_ellipsoid, uint32_t h_acc, uint32_t v_acc, uint32_t vel_acc, uint32_t hdg_acc, uint16_t yaw) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_GPS_RAW_INT_LEN]; @@ -95,6 +119,12 @@ static inline uint16_t mavlink_msg_gps_raw_int_pack(uint8_t system_id, uint8_t c _mav_put_uint16_t(buf, 26, cog); _mav_put_uint8_t(buf, 28, fix_type); _mav_put_uint8_t(buf, 29, satellites_visible); + _mav_put_int32_t(buf, 30, alt_ellipsoid); + _mav_put_uint32_t(buf, 34, h_acc); + _mav_put_uint32_t(buf, 38, v_acc); + _mav_put_uint32_t(buf, 42, vel_acc); + _mav_put_uint32_t(buf, 46, hdg_acc); + _mav_put_uint16_t(buf, 50, yaw); memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_GPS_RAW_INT_LEN); #else @@ -109,6 +139,12 @@ static inline uint16_t mavlink_msg_gps_raw_int_pack(uint8_t system_id, uint8_t c packet.cog = cog; packet.fix_type = fix_type; packet.satellites_visible = satellites_visible; + packet.alt_ellipsoid = alt_ellipsoid; + packet.h_acc = h_acc; + packet.v_acc = v_acc; + packet.vel_acc = vel_acc; + packet.hdg_acc = hdg_acc; + packet.yaw = yaw; memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_GPS_RAW_INT_LEN); #endif @@ -133,11 +169,17 @@ static inline uint16_t mavlink_msg_gps_raw_int_pack(uint8_t system_id, uint8_t c * @param vel [cm/s] GPS ground speed. If unknown, set to: UINT16_MAX * @param cog [cdeg] Course over ground (NOT heading, but direction of movement) in degrees * 100, 0.0..359.99 degrees. If unknown, set to: UINT16_MAX * @param satellites_visible Number of satellites visible. If unknown, set to 255 + * @param alt_ellipsoid [mm] Altitude (above WGS84, EGM96 ellipsoid). Positive for up. + * @param h_acc [mm] Position uncertainty. + * @param v_acc [mm] Altitude uncertainty. + * @param vel_acc [mm] Speed uncertainty. + * @param hdg_acc [degE5] Heading / track uncertainty + * @param yaw [cdeg] Yaw in earth frame from north. Use 0 if this GPS does not provide yaw. Use 65535 if this GPS is configured to provide yaw and is currently unable to provide it. Use 36000 for north. * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_gps_raw_int_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, - uint64_t time_usec,uint8_t fix_type,int32_t lat,int32_t lon,int32_t alt,uint16_t eph,uint16_t epv,uint16_t vel,uint16_t cog,uint8_t satellites_visible) + uint64_t time_usec,uint8_t fix_type,int32_t lat,int32_t lon,int32_t alt,uint16_t eph,uint16_t epv,uint16_t vel,uint16_t cog,uint8_t satellites_visible,int32_t alt_ellipsoid,uint32_t h_acc,uint32_t v_acc,uint32_t vel_acc,uint32_t hdg_acc,uint16_t yaw) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_GPS_RAW_INT_LEN]; @@ -151,6 +193,12 @@ static inline uint16_t mavlink_msg_gps_raw_int_pack_chan(uint8_t system_id, uint _mav_put_uint16_t(buf, 26, cog); _mav_put_uint8_t(buf, 28, fix_type); _mav_put_uint8_t(buf, 29, satellites_visible); + _mav_put_int32_t(buf, 30, alt_ellipsoid); + _mav_put_uint32_t(buf, 34, h_acc); + _mav_put_uint32_t(buf, 38, v_acc); + _mav_put_uint32_t(buf, 42, vel_acc); + _mav_put_uint32_t(buf, 46, hdg_acc); + _mav_put_uint16_t(buf, 50, yaw); memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_GPS_RAW_INT_LEN); #else @@ -165,6 +213,12 @@ static inline uint16_t mavlink_msg_gps_raw_int_pack_chan(uint8_t system_id, uint packet.cog = cog; packet.fix_type = fix_type; packet.satellites_visible = satellites_visible; + packet.alt_ellipsoid = alt_ellipsoid; + packet.h_acc = h_acc; + packet.v_acc = v_acc; + packet.vel_acc = vel_acc; + packet.hdg_acc = hdg_acc; + packet.yaw = yaw; memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_GPS_RAW_INT_LEN); #endif @@ -183,7 +237,7 @@ static inline uint16_t mavlink_msg_gps_raw_int_pack_chan(uint8_t system_id, uint */ static inline uint16_t mavlink_msg_gps_raw_int_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_gps_raw_int_t* gps_raw_int) { - return mavlink_msg_gps_raw_int_pack(system_id, component_id, msg, gps_raw_int->time_usec, gps_raw_int->fix_type, gps_raw_int->lat, gps_raw_int->lon, gps_raw_int->alt, gps_raw_int->eph, gps_raw_int->epv, gps_raw_int->vel, gps_raw_int->cog, gps_raw_int->satellites_visible); + return mavlink_msg_gps_raw_int_pack(system_id, component_id, msg, gps_raw_int->time_usec, gps_raw_int->fix_type, gps_raw_int->lat, gps_raw_int->lon, gps_raw_int->alt, gps_raw_int->eph, gps_raw_int->epv, gps_raw_int->vel, gps_raw_int->cog, gps_raw_int->satellites_visible, gps_raw_int->alt_ellipsoid, gps_raw_int->h_acc, gps_raw_int->v_acc, gps_raw_int->vel_acc, gps_raw_int->hdg_acc, gps_raw_int->yaw); } /** @@ -197,7 +251,7 @@ static inline uint16_t mavlink_msg_gps_raw_int_encode(uint8_t system_id, uint8_t */ static inline uint16_t mavlink_msg_gps_raw_int_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_gps_raw_int_t* gps_raw_int) { - return mavlink_msg_gps_raw_int_pack_chan(system_id, component_id, chan, msg, gps_raw_int->time_usec, gps_raw_int->fix_type, gps_raw_int->lat, gps_raw_int->lon, gps_raw_int->alt, gps_raw_int->eph, gps_raw_int->epv, gps_raw_int->vel, gps_raw_int->cog, gps_raw_int->satellites_visible); + return mavlink_msg_gps_raw_int_pack_chan(system_id, component_id, chan, msg, gps_raw_int->time_usec, gps_raw_int->fix_type, gps_raw_int->lat, gps_raw_int->lon, gps_raw_int->alt, gps_raw_int->eph, gps_raw_int->epv, gps_raw_int->vel, gps_raw_int->cog, gps_raw_int->satellites_visible, gps_raw_int->alt_ellipsoid, gps_raw_int->h_acc, gps_raw_int->v_acc, gps_raw_int->vel_acc, gps_raw_int->hdg_acc, gps_raw_int->yaw); } /** @@ -214,10 +268,16 @@ static inline uint16_t mavlink_msg_gps_raw_int_encode_chan(uint8_t system_id, ui * @param vel [cm/s] GPS ground speed. If unknown, set to: UINT16_MAX * @param cog [cdeg] Course over ground (NOT heading, but direction of movement) in degrees * 100, 0.0..359.99 degrees. If unknown, set to: UINT16_MAX * @param satellites_visible Number of satellites visible. If unknown, set to 255 + * @param alt_ellipsoid [mm] Altitude (above WGS84, EGM96 ellipsoid). Positive for up. + * @param h_acc [mm] Position uncertainty. + * @param v_acc [mm] Altitude uncertainty. + * @param vel_acc [mm] Speed uncertainty. + * @param hdg_acc [degE5] Heading / track uncertainty + * @param yaw [cdeg] Yaw in earth frame from north. Use 0 if this GPS does not provide yaw. Use 65535 if this GPS is configured to provide yaw and is currently unable to provide it. Use 36000 for north. */ #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS -static inline void mavlink_msg_gps_raw_int_send(mavlink_channel_t chan, uint64_t time_usec, uint8_t fix_type, int32_t lat, int32_t lon, int32_t alt, uint16_t eph, uint16_t epv, uint16_t vel, uint16_t cog, uint8_t satellites_visible) +static inline void mavlink_msg_gps_raw_int_send(mavlink_channel_t chan, uint64_t time_usec, uint8_t fix_type, int32_t lat, int32_t lon, int32_t alt, uint16_t eph, uint16_t epv, uint16_t vel, uint16_t cog, uint8_t satellites_visible, int32_t alt_ellipsoid, uint32_t h_acc, uint32_t v_acc, uint32_t vel_acc, uint32_t hdg_acc, uint16_t yaw) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_GPS_RAW_INT_LEN]; @@ -231,6 +291,12 @@ static inline void mavlink_msg_gps_raw_int_send(mavlink_channel_t chan, uint64_t _mav_put_uint16_t(buf, 26, cog); _mav_put_uint8_t(buf, 28, fix_type); _mav_put_uint8_t(buf, 29, satellites_visible); + _mav_put_int32_t(buf, 30, alt_ellipsoid); + _mav_put_uint32_t(buf, 34, h_acc); + _mav_put_uint32_t(buf, 38, v_acc); + _mav_put_uint32_t(buf, 42, vel_acc); + _mav_put_uint32_t(buf, 46, hdg_acc); + _mav_put_uint16_t(buf, 50, yaw); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GPS_RAW_INT, buf, MAVLINK_MSG_ID_GPS_RAW_INT_MIN_LEN, MAVLINK_MSG_ID_GPS_RAW_INT_LEN, MAVLINK_MSG_ID_GPS_RAW_INT_CRC); #else @@ -245,6 +311,12 @@ static inline void mavlink_msg_gps_raw_int_send(mavlink_channel_t chan, uint64_t packet.cog = cog; packet.fix_type = fix_type; packet.satellites_visible = satellites_visible; + packet.alt_ellipsoid = alt_ellipsoid; + packet.h_acc = h_acc; + packet.v_acc = v_acc; + packet.vel_acc = vel_acc; + packet.hdg_acc = hdg_acc; + packet.yaw = yaw; _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GPS_RAW_INT, (const char *)&packet, MAVLINK_MSG_ID_GPS_RAW_INT_MIN_LEN, MAVLINK_MSG_ID_GPS_RAW_INT_LEN, MAVLINK_MSG_ID_GPS_RAW_INT_CRC); #endif @@ -258,7 +330,7 @@ static inline void mavlink_msg_gps_raw_int_send(mavlink_channel_t chan, uint64_t static inline void mavlink_msg_gps_raw_int_send_struct(mavlink_channel_t chan, const mavlink_gps_raw_int_t* gps_raw_int) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - mavlink_msg_gps_raw_int_send(chan, gps_raw_int->time_usec, gps_raw_int->fix_type, gps_raw_int->lat, gps_raw_int->lon, gps_raw_int->alt, gps_raw_int->eph, gps_raw_int->epv, gps_raw_int->vel, gps_raw_int->cog, gps_raw_int->satellites_visible); + mavlink_msg_gps_raw_int_send(chan, gps_raw_int->time_usec, gps_raw_int->fix_type, gps_raw_int->lat, gps_raw_int->lon, gps_raw_int->alt, gps_raw_int->eph, gps_raw_int->epv, gps_raw_int->vel, gps_raw_int->cog, gps_raw_int->satellites_visible, gps_raw_int->alt_ellipsoid, gps_raw_int->h_acc, gps_raw_int->v_acc, gps_raw_int->vel_acc, gps_raw_int->hdg_acc, gps_raw_int->yaw); #else _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GPS_RAW_INT, (const char *)gps_raw_int, MAVLINK_MSG_ID_GPS_RAW_INT_MIN_LEN, MAVLINK_MSG_ID_GPS_RAW_INT_LEN, MAVLINK_MSG_ID_GPS_RAW_INT_CRC); #endif @@ -272,7 +344,7 @@ static inline void mavlink_msg_gps_raw_int_send_struct(mavlink_channel_t chan, c is usually the receive buffer for the channel, and allows a reply to an incoming message with minimum stack space usage. */ -static inline void mavlink_msg_gps_raw_int_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint64_t time_usec, uint8_t fix_type, int32_t lat, int32_t lon, int32_t alt, uint16_t eph, uint16_t epv, uint16_t vel, uint16_t cog, uint8_t satellites_visible) +static inline void mavlink_msg_gps_raw_int_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint64_t time_usec, uint8_t fix_type, int32_t lat, int32_t lon, int32_t alt, uint16_t eph, uint16_t epv, uint16_t vel, uint16_t cog, uint8_t satellites_visible, int32_t alt_ellipsoid, uint32_t h_acc, uint32_t v_acc, uint32_t vel_acc, uint32_t hdg_acc, uint16_t yaw) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char *buf = (char *)msgbuf; @@ -286,6 +358,12 @@ static inline void mavlink_msg_gps_raw_int_send_buf(mavlink_message_t *msgbuf, m _mav_put_uint16_t(buf, 26, cog); _mav_put_uint8_t(buf, 28, fix_type); _mav_put_uint8_t(buf, 29, satellites_visible); + _mav_put_int32_t(buf, 30, alt_ellipsoid); + _mav_put_uint32_t(buf, 34, h_acc); + _mav_put_uint32_t(buf, 38, v_acc); + _mav_put_uint32_t(buf, 42, vel_acc); + _mav_put_uint32_t(buf, 46, hdg_acc); + _mav_put_uint16_t(buf, 50, yaw); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GPS_RAW_INT, buf, MAVLINK_MSG_ID_GPS_RAW_INT_MIN_LEN, MAVLINK_MSG_ID_GPS_RAW_INT_LEN, MAVLINK_MSG_ID_GPS_RAW_INT_CRC); #else @@ -300,6 +378,12 @@ static inline void mavlink_msg_gps_raw_int_send_buf(mavlink_message_t *msgbuf, m packet->cog = cog; packet->fix_type = fix_type; packet->satellites_visible = satellites_visible; + packet->alt_ellipsoid = alt_ellipsoid; + packet->h_acc = h_acc; + packet->v_acc = v_acc; + packet->vel_acc = vel_acc; + packet->hdg_acc = hdg_acc; + packet->yaw = yaw; _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_GPS_RAW_INT, (const char *)packet, MAVLINK_MSG_ID_GPS_RAW_INT_MIN_LEN, MAVLINK_MSG_ID_GPS_RAW_INT_LEN, MAVLINK_MSG_ID_GPS_RAW_INT_CRC); #endif @@ -411,6 +495,66 @@ static inline uint8_t mavlink_msg_gps_raw_int_get_satellites_visible(const mavli return _MAV_RETURN_uint8_t(msg, 29); } +/** + * @brief Get field alt_ellipsoid from gps_raw_int message + * + * @return [mm] Altitude (above WGS84, EGM96 ellipsoid). Positive for up. + */ +static inline int32_t mavlink_msg_gps_raw_int_get_alt_ellipsoid(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int32_t(msg, 30); +} + +/** + * @brief Get field h_acc from gps_raw_int message + * + * @return [mm] Position uncertainty. + */ +static inline uint32_t mavlink_msg_gps_raw_int_get_h_acc(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint32_t(msg, 34); +} + +/** + * @brief Get field v_acc from gps_raw_int message + * + * @return [mm] Altitude uncertainty. + */ +static inline uint32_t mavlink_msg_gps_raw_int_get_v_acc(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint32_t(msg, 38); +} + +/** + * @brief Get field vel_acc from gps_raw_int message + * + * @return [mm] Speed uncertainty. + */ +static inline uint32_t mavlink_msg_gps_raw_int_get_vel_acc(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint32_t(msg, 42); +} + +/** + * @brief Get field hdg_acc from gps_raw_int message + * + * @return [degE5] Heading / track uncertainty + */ +static inline uint32_t mavlink_msg_gps_raw_int_get_hdg_acc(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint32_t(msg, 46); +} + +/** + * @brief Get field yaw from gps_raw_int message + * + * @return [cdeg] Yaw in earth frame from north. Use 0 if this GPS does not provide yaw. Use 65535 if this GPS is configured to provide yaw and is currently unable to provide it. Use 36000 for north. + */ +static inline uint16_t mavlink_msg_gps_raw_int_get_yaw(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 50); +} + /** * @brief Decode a gps_raw_int message into a struct * @@ -430,6 +574,12 @@ static inline void mavlink_msg_gps_raw_int_decode(const mavlink_message_t* msg, gps_raw_int->cog = mavlink_msg_gps_raw_int_get_cog(msg); gps_raw_int->fix_type = mavlink_msg_gps_raw_int_get_fix_type(msg); gps_raw_int->satellites_visible = mavlink_msg_gps_raw_int_get_satellites_visible(msg); + gps_raw_int->alt_ellipsoid = mavlink_msg_gps_raw_int_get_alt_ellipsoid(msg); + gps_raw_int->h_acc = mavlink_msg_gps_raw_int_get_h_acc(msg); + gps_raw_int->v_acc = mavlink_msg_gps_raw_int_get_v_acc(msg); + gps_raw_int->vel_acc = mavlink_msg_gps_raw_int_get_vel_acc(msg); + gps_raw_int->hdg_acc = mavlink_msg_gps_raw_int_get_hdg_acc(msg); + gps_raw_int->yaw = mavlink_msg_gps_raw_int_get_yaw(msg); #else uint8_t len = msg->len < MAVLINK_MSG_ID_GPS_RAW_INT_LEN? msg->len : MAVLINK_MSG_ID_GPS_RAW_INT_LEN; memset(gps_raw_int, 0, MAVLINK_MSG_ID_GPS_RAW_INT_LEN); diff --git a/lib/main/MAVLink/common/mavlink_msg_highres_imu.h b/lib/main/MAVLink/common/mavlink_msg_highres_imu.h index 59718e30cd..a8e0899923 100755 --- a/lib/main/MAVLink/common/mavlink_msg_highres_imu.h +++ b/lib/main/MAVLink/common/mavlink_msg_highres_imu.h @@ -15,16 +15,17 @@ typedef struct __mavlink_highres_imu_t { float xmag; /*< [gauss] X Magnetic field*/ float ymag; /*< [gauss] Y Magnetic field*/ float zmag; /*< [gauss] Z Magnetic field*/ - float abs_pressure; /*< [mbar] Absolute pressure*/ - float diff_pressure; /*< [mbar] Differential pressure*/ + float abs_pressure; /*< [hPa] Absolute pressure*/ + float diff_pressure; /*< [hPa] Differential pressure*/ float pressure_alt; /*< Altitude calculated from pressure*/ float temperature; /*< [degC] Temperature*/ uint16_t fields_updated; /*< Bitmap for fields that have updated since last message, bit 0 = xacc, bit 12: temperature*/ + uint8_t id; /*< Id. Ids are numbered from 0 and map to IMUs numbered from 1 (e.g. IMU1 will have a message with id=0)*/ } mavlink_highres_imu_t; -#define MAVLINK_MSG_ID_HIGHRES_IMU_LEN 62 +#define MAVLINK_MSG_ID_HIGHRES_IMU_LEN 63 #define MAVLINK_MSG_ID_HIGHRES_IMU_MIN_LEN 62 -#define MAVLINK_MSG_ID_105_LEN 62 +#define MAVLINK_MSG_ID_105_LEN 63 #define MAVLINK_MSG_ID_105_MIN_LEN 62 #define MAVLINK_MSG_ID_HIGHRES_IMU_CRC 93 @@ -36,7 +37,7 @@ typedef struct __mavlink_highres_imu_t { #define MAVLINK_MESSAGE_INFO_HIGHRES_IMU { \ 105, \ "HIGHRES_IMU", \ - 15, \ + 16, \ { { "time_usec", NULL, MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_highres_imu_t, time_usec) }, \ { "xacc", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_highres_imu_t, xacc) }, \ { "yacc", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_highres_imu_t, yacc) }, \ @@ -52,12 +53,13 @@ typedef struct __mavlink_highres_imu_t { { "pressure_alt", NULL, MAVLINK_TYPE_FLOAT, 0, 52, offsetof(mavlink_highres_imu_t, pressure_alt) }, \ { "temperature", NULL, MAVLINK_TYPE_FLOAT, 0, 56, offsetof(mavlink_highres_imu_t, temperature) }, \ { "fields_updated", NULL, MAVLINK_TYPE_UINT16_T, 0, 60, offsetof(mavlink_highres_imu_t, fields_updated) }, \ + { "id", NULL, MAVLINK_TYPE_UINT8_T, 0, 62, offsetof(mavlink_highres_imu_t, id) }, \ } \ } #else #define MAVLINK_MESSAGE_INFO_HIGHRES_IMU { \ "HIGHRES_IMU", \ - 15, \ + 16, \ { { "time_usec", NULL, MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_highres_imu_t, time_usec) }, \ { "xacc", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_highres_imu_t, xacc) }, \ { "yacc", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_highres_imu_t, yacc) }, \ @@ -73,6 +75,7 @@ typedef struct __mavlink_highres_imu_t { { "pressure_alt", NULL, MAVLINK_TYPE_FLOAT, 0, 52, offsetof(mavlink_highres_imu_t, pressure_alt) }, \ { "temperature", NULL, MAVLINK_TYPE_FLOAT, 0, 56, offsetof(mavlink_highres_imu_t, temperature) }, \ { "fields_updated", NULL, MAVLINK_TYPE_UINT16_T, 0, 60, offsetof(mavlink_highres_imu_t, fields_updated) }, \ + { "id", NULL, MAVLINK_TYPE_UINT8_T, 0, 62, offsetof(mavlink_highres_imu_t, id) }, \ } \ } #endif @@ -93,15 +96,16 @@ typedef struct __mavlink_highres_imu_t { * @param xmag [gauss] X Magnetic field * @param ymag [gauss] Y Magnetic field * @param zmag [gauss] Z Magnetic field - * @param abs_pressure [mbar] Absolute pressure - * @param diff_pressure [mbar] Differential pressure + * @param abs_pressure [hPa] Absolute pressure + * @param diff_pressure [hPa] Differential pressure * @param pressure_alt Altitude calculated from pressure * @param temperature [degC] Temperature * @param fields_updated Bitmap for fields that have updated since last message, bit 0 = xacc, bit 12: temperature + * @param id Id. Ids are numbered from 0 and map to IMUs numbered from 1 (e.g. IMU1 will have a message with id=0) * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_highres_imu_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint64_t time_usec, float xacc, float yacc, float zacc, float xgyro, float ygyro, float zgyro, float xmag, float ymag, float zmag, float abs_pressure, float diff_pressure, float pressure_alt, float temperature, uint16_t fields_updated) + uint64_t time_usec, float xacc, float yacc, float zacc, float xgyro, float ygyro, float zgyro, float xmag, float ymag, float zmag, float abs_pressure, float diff_pressure, float pressure_alt, float temperature, uint16_t fields_updated, uint8_t id) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_HIGHRES_IMU_LEN]; @@ -120,6 +124,7 @@ static inline uint16_t mavlink_msg_highres_imu_pack(uint8_t system_id, uint8_t c _mav_put_float(buf, 52, pressure_alt); _mav_put_float(buf, 56, temperature); _mav_put_uint16_t(buf, 60, fields_updated); + _mav_put_uint8_t(buf, 62, id); memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_HIGHRES_IMU_LEN); #else @@ -139,6 +144,7 @@ static inline uint16_t mavlink_msg_highres_imu_pack(uint8_t system_id, uint8_t c packet.pressure_alt = pressure_alt; packet.temperature = temperature; packet.fields_updated = fields_updated; + packet.id = id; memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_HIGHRES_IMU_LEN); #endif @@ -163,16 +169,17 @@ static inline uint16_t mavlink_msg_highres_imu_pack(uint8_t system_id, uint8_t c * @param xmag [gauss] X Magnetic field * @param ymag [gauss] Y Magnetic field * @param zmag [gauss] Z Magnetic field - * @param abs_pressure [mbar] Absolute pressure - * @param diff_pressure [mbar] Differential pressure + * @param abs_pressure [hPa] Absolute pressure + * @param diff_pressure [hPa] Differential pressure * @param pressure_alt Altitude calculated from pressure * @param temperature [degC] Temperature * @param fields_updated Bitmap for fields that have updated since last message, bit 0 = xacc, bit 12: temperature + * @param id Id. Ids are numbered from 0 and map to IMUs numbered from 1 (e.g. IMU1 will have a message with id=0) * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_highres_imu_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, - uint64_t time_usec,float xacc,float yacc,float zacc,float xgyro,float ygyro,float zgyro,float xmag,float ymag,float zmag,float abs_pressure,float diff_pressure,float pressure_alt,float temperature,uint16_t fields_updated) + uint64_t time_usec,float xacc,float yacc,float zacc,float xgyro,float ygyro,float zgyro,float xmag,float ymag,float zmag,float abs_pressure,float diff_pressure,float pressure_alt,float temperature,uint16_t fields_updated,uint8_t id) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_HIGHRES_IMU_LEN]; @@ -191,6 +198,7 @@ static inline uint16_t mavlink_msg_highres_imu_pack_chan(uint8_t system_id, uint _mav_put_float(buf, 52, pressure_alt); _mav_put_float(buf, 56, temperature); _mav_put_uint16_t(buf, 60, fields_updated); + _mav_put_uint8_t(buf, 62, id); memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_HIGHRES_IMU_LEN); #else @@ -210,6 +218,7 @@ static inline uint16_t mavlink_msg_highres_imu_pack_chan(uint8_t system_id, uint packet.pressure_alt = pressure_alt; packet.temperature = temperature; packet.fields_updated = fields_updated; + packet.id = id; memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_HIGHRES_IMU_LEN); #endif @@ -228,7 +237,7 @@ static inline uint16_t mavlink_msg_highres_imu_pack_chan(uint8_t system_id, uint */ static inline uint16_t mavlink_msg_highres_imu_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_highres_imu_t* highres_imu) { - return mavlink_msg_highres_imu_pack(system_id, component_id, msg, highres_imu->time_usec, highres_imu->xacc, highres_imu->yacc, highres_imu->zacc, highres_imu->xgyro, highres_imu->ygyro, highres_imu->zgyro, highres_imu->xmag, highres_imu->ymag, highres_imu->zmag, highres_imu->abs_pressure, highres_imu->diff_pressure, highres_imu->pressure_alt, highres_imu->temperature, highres_imu->fields_updated); + return mavlink_msg_highres_imu_pack(system_id, component_id, msg, highres_imu->time_usec, highres_imu->xacc, highres_imu->yacc, highres_imu->zacc, highres_imu->xgyro, highres_imu->ygyro, highres_imu->zgyro, highres_imu->xmag, highres_imu->ymag, highres_imu->zmag, highres_imu->abs_pressure, highres_imu->diff_pressure, highres_imu->pressure_alt, highres_imu->temperature, highres_imu->fields_updated, highres_imu->id); } /** @@ -242,7 +251,7 @@ static inline uint16_t mavlink_msg_highres_imu_encode(uint8_t system_id, uint8_t */ static inline uint16_t mavlink_msg_highres_imu_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_highres_imu_t* highres_imu) { - return mavlink_msg_highres_imu_pack_chan(system_id, component_id, chan, msg, highres_imu->time_usec, highres_imu->xacc, highres_imu->yacc, highres_imu->zacc, highres_imu->xgyro, highres_imu->ygyro, highres_imu->zgyro, highres_imu->xmag, highres_imu->ymag, highres_imu->zmag, highres_imu->abs_pressure, highres_imu->diff_pressure, highres_imu->pressure_alt, highres_imu->temperature, highres_imu->fields_updated); + return mavlink_msg_highres_imu_pack_chan(system_id, component_id, chan, msg, highres_imu->time_usec, highres_imu->xacc, highres_imu->yacc, highres_imu->zacc, highres_imu->xgyro, highres_imu->ygyro, highres_imu->zgyro, highres_imu->xmag, highres_imu->ymag, highres_imu->zmag, highres_imu->abs_pressure, highres_imu->diff_pressure, highres_imu->pressure_alt, highres_imu->temperature, highres_imu->fields_updated, highres_imu->id); } /** @@ -259,15 +268,16 @@ static inline uint16_t mavlink_msg_highres_imu_encode_chan(uint8_t system_id, ui * @param xmag [gauss] X Magnetic field * @param ymag [gauss] Y Magnetic field * @param zmag [gauss] Z Magnetic field - * @param abs_pressure [mbar] Absolute pressure - * @param diff_pressure [mbar] Differential pressure + * @param abs_pressure [hPa] Absolute pressure + * @param diff_pressure [hPa] Differential pressure * @param pressure_alt Altitude calculated from pressure * @param temperature [degC] Temperature * @param fields_updated Bitmap for fields that have updated since last message, bit 0 = xacc, bit 12: temperature + * @param id Id. Ids are numbered from 0 and map to IMUs numbered from 1 (e.g. IMU1 will have a message with id=0) */ #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS -static inline void mavlink_msg_highres_imu_send(mavlink_channel_t chan, uint64_t time_usec, float xacc, float yacc, float zacc, float xgyro, float ygyro, float zgyro, float xmag, float ymag, float zmag, float abs_pressure, float diff_pressure, float pressure_alt, float temperature, uint16_t fields_updated) +static inline void mavlink_msg_highres_imu_send(mavlink_channel_t chan, uint64_t time_usec, float xacc, float yacc, float zacc, float xgyro, float ygyro, float zgyro, float xmag, float ymag, float zmag, float abs_pressure, float diff_pressure, float pressure_alt, float temperature, uint16_t fields_updated, uint8_t id) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_HIGHRES_IMU_LEN]; @@ -286,6 +296,7 @@ static inline void mavlink_msg_highres_imu_send(mavlink_channel_t chan, uint64_t _mav_put_float(buf, 52, pressure_alt); _mav_put_float(buf, 56, temperature); _mav_put_uint16_t(buf, 60, fields_updated); + _mav_put_uint8_t(buf, 62, id); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HIGHRES_IMU, buf, MAVLINK_MSG_ID_HIGHRES_IMU_MIN_LEN, MAVLINK_MSG_ID_HIGHRES_IMU_LEN, MAVLINK_MSG_ID_HIGHRES_IMU_CRC); #else @@ -305,6 +316,7 @@ static inline void mavlink_msg_highres_imu_send(mavlink_channel_t chan, uint64_t packet.pressure_alt = pressure_alt; packet.temperature = temperature; packet.fields_updated = fields_updated; + packet.id = id; _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HIGHRES_IMU, (const char *)&packet, MAVLINK_MSG_ID_HIGHRES_IMU_MIN_LEN, MAVLINK_MSG_ID_HIGHRES_IMU_LEN, MAVLINK_MSG_ID_HIGHRES_IMU_CRC); #endif @@ -318,7 +330,7 @@ static inline void mavlink_msg_highres_imu_send(mavlink_channel_t chan, uint64_t static inline void mavlink_msg_highres_imu_send_struct(mavlink_channel_t chan, const mavlink_highres_imu_t* highres_imu) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - mavlink_msg_highres_imu_send(chan, highres_imu->time_usec, highres_imu->xacc, highres_imu->yacc, highres_imu->zacc, highres_imu->xgyro, highres_imu->ygyro, highres_imu->zgyro, highres_imu->xmag, highres_imu->ymag, highres_imu->zmag, highres_imu->abs_pressure, highres_imu->diff_pressure, highres_imu->pressure_alt, highres_imu->temperature, highres_imu->fields_updated); + mavlink_msg_highres_imu_send(chan, highres_imu->time_usec, highres_imu->xacc, highres_imu->yacc, highres_imu->zacc, highres_imu->xgyro, highres_imu->ygyro, highres_imu->zgyro, highres_imu->xmag, highres_imu->ymag, highres_imu->zmag, highres_imu->abs_pressure, highres_imu->diff_pressure, highres_imu->pressure_alt, highres_imu->temperature, highres_imu->fields_updated, highres_imu->id); #else _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HIGHRES_IMU, (const char *)highres_imu, MAVLINK_MSG_ID_HIGHRES_IMU_MIN_LEN, MAVLINK_MSG_ID_HIGHRES_IMU_LEN, MAVLINK_MSG_ID_HIGHRES_IMU_CRC); #endif @@ -332,7 +344,7 @@ static inline void mavlink_msg_highres_imu_send_struct(mavlink_channel_t chan, c is usually the receive buffer for the channel, and allows a reply to an incoming message with minimum stack space usage. */ -static inline void mavlink_msg_highres_imu_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint64_t time_usec, float xacc, float yacc, float zacc, float xgyro, float ygyro, float zgyro, float xmag, float ymag, float zmag, float abs_pressure, float diff_pressure, float pressure_alt, float temperature, uint16_t fields_updated) +static inline void mavlink_msg_highres_imu_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint64_t time_usec, float xacc, float yacc, float zacc, float xgyro, float ygyro, float zgyro, float xmag, float ymag, float zmag, float abs_pressure, float diff_pressure, float pressure_alt, float temperature, uint16_t fields_updated, uint8_t id) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char *buf = (char *)msgbuf; @@ -351,6 +363,7 @@ static inline void mavlink_msg_highres_imu_send_buf(mavlink_message_t *msgbuf, m _mav_put_float(buf, 52, pressure_alt); _mav_put_float(buf, 56, temperature); _mav_put_uint16_t(buf, 60, fields_updated); + _mav_put_uint8_t(buf, 62, id); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HIGHRES_IMU, buf, MAVLINK_MSG_ID_HIGHRES_IMU_MIN_LEN, MAVLINK_MSG_ID_HIGHRES_IMU_LEN, MAVLINK_MSG_ID_HIGHRES_IMU_CRC); #else @@ -370,6 +383,7 @@ static inline void mavlink_msg_highres_imu_send_buf(mavlink_message_t *msgbuf, m packet->pressure_alt = pressure_alt; packet->temperature = temperature; packet->fields_updated = fields_updated; + packet->id = id; _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HIGHRES_IMU, (const char *)packet, MAVLINK_MSG_ID_HIGHRES_IMU_MIN_LEN, MAVLINK_MSG_ID_HIGHRES_IMU_LEN, MAVLINK_MSG_ID_HIGHRES_IMU_CRC); #endif @@ -484,7 +498,7 @@ static inline float mavlink_msg_highres_imu_get_zmag(const mavlink_message_t* ms /** * @brief Get field abs_pressure from highres_imu message * - * @return [mbar] Absolute pressure + * @return [hPa] Absolute pressure */ static inline float mavlink_msg_highres_imu_get_abs_pressure(const mavlink_message_t* msg) { @@ -494,7 +508,7 @@ static inline float mavlink_msg_highres_imu_get_abs_pressure(const mavlink_messa /** * @brief Get field diff_pressure from highres_imu message * - * @return [mbar] Differential pressure + * @return [hPa] Differential pressure */ static inline float mavlink_msg_highres_imu_get_diff_pressure(const mavlink_message_t* msg) { @@ -531,6 +545,16 @@ static inline uint16_t mavlink_msg_highres_imu_get_fields_updated(const mavlink_ return _MAV_RETURN_uint16_t(msg, 60); } +/** + * @brief Get field id from highres_imu message + * + * @return Id. Ids are numbered from 0 and map to IMUs numbered from 1 (e.g. IMU1 will have a message with id=0) + */ +static inline uint8_t mavlink_msg_highres_imu_get_id(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 62); +} + /** * @brief Decode a highres_imu message into a struct * @@ -555,6 +579,7 @@ static inline void mavlink_msg_highres_imu_decode(const mavlink_message_t* msg, highres_imu->pressure_alt = mavlink_msg_highres_imu_get_pressure_alt(msg); highres_imu->temperature = mavlink_msg_highres_imu_get_temperature(msg); highres_imu->fields_updated = mavlink_msg_highres_imu_get_fields_updated(msg); + highres_imu->id = mavlink_msg_highres_imu_get_id(msg); #else uint8_t len = msg->len < MAVLINK_MSG_ID_HIGHRES_IMU_LEN? msg->len : MAVLINK_MSG_ID_HIGHRES_IMU_LEN; memset(highres_imu, 0, MAVLINK_MSG_ID_HIGHRES_IMU_LEN); diff --git a/lib/main/MAVLink/common/mavlink_msg_hil_gps.h b/lib/main/MAVLink/common/mavlink_msg_hil_gps.h index 1d1c2575c8..4b8ff2b85e 100755 --- a/lib/main/MAVLink/common/mavlink_msg_hil_gps.h +++ b/lib/main/MAVLink/common/mavlink_msg_hil_gps.h @@ -3,14 +3,14 @@ #define MAVLINK_MSG_ID_HIL_GPS 113 - +MAVPACKED( typedef struct __mavlink_hil_gps_t { uint64_t time_usec; /*< [us] Timestamp (UNIX Epoch time or time since system boot). The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude of the number.*/ int32_t lat; /*< [degE7] Latitude (WGS84)*/ int32_t lon; /*< [degE7] Longitude (WGS84)*/ int32_t alt; /*< [mm] Altitude (MSL). Positive for up.*/ - uint16_t eph; /*< [cm] GPS HDOP horizontal dilution of position. If unknown, set to: 65535*/ - uint16_t epv; /*< [cm] GPS VDOP vertical dilution of position. If unknown, set to: 65535*/ + uint16_t eph; /*< GPS HDOP horizontal dilution of position (unitless). If unknown, set to: UINT16_MAX*/ + uint16_t epv; /*< GPS VDOP vertical dilution of position (unitless). If unknown, set to: UINT16_MAX*/ uint16_t vel; /*< [cm/s] GPS ground speed. If unknown, set to: 65535*/ int16_t vn; /*< [cm/s] GPS velocity in north direction in earth-fixed NED frame*/ int16_t ve; /*< [cm/s] GPS velocity in east direction in earth-fixed NED frame*/ @@ -18,11 +18,13 @@ typedef struct __mavlink_hil_gps_t { uint16_t cog; /*< [cdeg] Course over ground (NOT heading, but direction of movement), 0.0..359.99 degrees. If unknown, set to: 65535*/ uint8_t fix_type; /*< 0-1: no fix, 2: 2D fix, 3: 3D fix. Some applications will not use the value of this field unless it is at least two, so always correctly fill in the fix.*/ uint8_t satellites_visible; /*< Number of satellites visible. If unknown, set to 255*/ -} mavlink_hil_gps_t; + uint8_t id; /*< GPS ID (zero indexed). Used for multiple GPS inputs*/ + uint16_t yaw; /*< [cdeg] Yaw of vehicle relative to Earth's North, zero means not available, use 36000 for north*/ +}) mavlink_hil_gps_t; -#define MAVLINK_MSG_ID_HIL_GPS_LEN 36 +#define MAVLINK_MSG_ID_HIL_GPS_LEN 39 #define MAVLINK_MSG_ID_HIL_GPS_MIN_LEN 36 -#define MAVLINK_MSG_ID_113_LEN 36 +#define MAVLINK_MSG_ID_113_LEN 39 #define MAVLINK_MSG_ID_113_MIN_LEN 36 #define MAVLINK_MSG_ID_HIL_GPS_CRC 124 @@ -34,7 +36,7 @@ typedef struct __mavlink_hil_gps_t { #define MAVLINK_MESSAGE_INFO_HIL_GPS { \ 113, \ "HIL_GPS", \ - 13, \ + 15, \ { { "time_usec", NULL, MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_hil_gps_t, time_usec) }, \ { "fix_type", NULL, MAVLINK_TYPE_UINT8_T, 0, 34, offsetof(mavlink_hil_gps_t, fix_type) }, \ { "lat", NULL, MAVLINK_TYPE_INT32_T, 0, 8, offsetof(mavlink_hil_gps_t, lat) }, \ @@ -48,12 +50,14 @@ typedef struct __mavlink_hil_gps_t { { "vd", NULL, MAVLINK_TYPE_INT16_T, 0, 30, offsetof(mavlink_hil_gps_t, vd) }, \ { "cog", NULL, MAVLINK_TYPE_UINT16_T, 0, 32, offsetof(mavlink_hil_gps_t, cog) }, \ { "satellites_visible", NULL, MAVLINK_TYPE_UINT8_T, 0, 35, offsetof(mavlink_hil_gps_t, satellites_visible) }, \ + { "id", NULL, MAVLINK_TYPE_UINT8_T, 0, 36, offsetof(mavlink_hil_gps_t, id) }, \ + { "yaw", NULL, MAVLINK_TYPE_UINT16_T, 0, 37, offsetof(mavlink_hil_gps_t, yaw) }, \ } \ } #else #define MAVLINK_MESSAGE_INFO_HIL_GPS { \ "HIL_GPS", \ - 13, \ + 15, \ { { "time_usec", NULL, MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_hil_gps_t, time_usec) }, \ { "fix_type", NULL, MAVLINK_TYPE_UINT8_T, 0, 34, offsetof(mavlink_hil_gps_t, fix_type) }, \ { "lat", NULL, MAVLINK_TYPE_INT32_T, 0, 8, offsetof(mavlink_hil_gps_t, lat) }, \ @@ -67,6 +71,8 @@ typedef struct __mavlink_hil_gps_t { { "vd", NULL, MAVLINK_TYPE_INT16_T, 0, 30, offsetof(mavlink_hil_gps_t, vd) }, \ { "cog", NULL, MAVLINK_TYPE_UINT16_T, 0, 32, offsetof(mavlink_hil_gps_t, cog) }, \ { "satellites_visible", NULL, MAVLINK_TYPE_UINT8_T, 0, 35, offsetof(mavlink_hil_gps_t, satellites_visible) }, \ + { "id", NULL, MAVLINK_TYPE_UINT8_T, 0, 36, offsetof(mavlink_hil_gps_t, id) }, \ + { "yaw", NULL, MAVLINK_TYPE_UINT16_T, 0, 37, offsetof(mavlink_hil_gps_t, yaw) }, \ } \ } #endif @@ -82,18 +88,20 @@ typedef struct __mavlink_hil_gps_t { * @param lat [degE7] Latitude (WGS84) * @param lon [degE7] Longitude (WGS84) * @param alt [mm] Altitude (MSL). Positive for up. - * @param eph [cm] GPS HDOP horizontal dilution of position. If unknown, set to: 65535 - * @param epv [cm] GPS VDOP vertical dilution of position. If unknown, set to: 65535 + * @param eph GPS HDOP horizontal dilution of position (unitless). If unknown, set to: UINT16_MAX + * @param epv GPS VDOP vertical dilution of position (unitless). If unknown, set to: UINT16_MAX * @param vel [cm/s] GPS ground speed. If unknown, set to: 65535 * @param vn [cm/s] GPS velocity in north direction in earth-fixed NED frame * @param ve [cm/s] GPS velocity in east direction in earth-fixed NED frame * @param vd [cm/s] GPS velocity in down direction in earth-fixed NED frame * @param cog [cdeg] Course over ground (NOT heading, but direction of movement), 0.0..359.99 degrees. If unknown, set to: 65535 * @param satellites_visible Number of satellites visible. If unknown, set to 255 + * @param id GPS ID (zero indexed). Used for multiple GPS inputs + * @param yaw [cdeg] Yaw of vehicle relative to Earth's North, zero means not available, use 36000 for north * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_hil_gps_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint64_t time_usec, uint8_t fix_type, int32_t lat, int32_t lon, int32_t alt, uint16_t eph, uint16_t epv, uint16_t vel, int16_t vn, int16_t ve, int16_t vd, uint16_t cog, uint8_t satellites_visible) + uint64_t time_usec, uint8_t fix_type, int32_t lat, int32_t lon, int32_t alt, uint16_t eph, uint16_t epv, uint16_t vel, int16_t vn, int16_t ve, int16_t vd, uint16_t cog, uint8_t satellites_visible, uint8_t id, uint16_t yaw) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_HIL_GPS_LEN]; @@ -110,6 +118,8 @@ static inline uint16_t mavlink_msg_hil_gps_pack(uint8_t system_id, uint8_t compo _mav_put_uint16_t(buf, 32, cog); _mav_put_uint8_t(buf, 34, fix_type); _mav_put_uint8_t(buf, 35, satellites_visible); + _mav_put_uint8_t(buf, 36, id); + _mav_put_uint16_t(buf, 37, yaw); memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_HIL_GPS_LEN); #else @@ -127,6 +137,8 @@ static inline uint16_t mavlink_msg_hil_gps_pack(uint8_t system_id, uint8_t compo packet.cog = cog; packet.fix_type = fix_type; packet.satellites_visible = satellites_visible; + packet.id = id; + packet.yaw = yaw; memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_HIL_GPS_LEN); #endif @@ -146,19 +158,21 @@ static inline uint16_t mavlink_msg_hil_gps_pack(uint8_t system_id, uint8_t compo * @param lat [degE7] Latitude (WGS84) * @param lon [degE7] Longitude (WGS84) * @param alt [mm] Altitude (MSL). Positive for up. - * @param eph [cm] GPS HDOP horizontal dilution of position. If unknown, set to: 65535 - * @param epv [cm] GPS VDOP vertical dilution of position. If unknown, set to: 65535 + * @param eph GPS HDOP horizontal dilution of position (unitless). If unknown, set to: UINT16_MAX + * @param epv GPS VDOP vertical dilution of position (unitless). If unknown, set to: UINT16_MAX * @param vel [cm/s] GPS ground speed. If unknown, set to: 65535 * @param vn [cm/s] GPS velocity in north direction in earth-fixed NED frame * @param ve [cm/s] GPS velocity in east direction in earth-fixed NED frame * @param vd [cm/s] GPS velocity in down direction in earth-fixed NED frame * @param cog [cdeg] Course over ground (NOT heading, but direction of movement), 0.0..359.99 degrees. If unknown, set to: 65535 * @param satellites_visible Number of satellites visible. If unknown, set to 255 + * @param id GPS ID (zero indexed). Used for multiple GPS inputs + * @param yaw [cdeg] Yaw of vehicle relative to Earth's North, zero means not available, use 36000 for north * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_hil_gps_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, - uint64_t time_usec,uint8_t fix_type,int32_t lat,int32_t lon,int32_t alt,uint16_t eph,uint16_t epv,uint16_t vel,int16_t vn,int16_t ve,int16_t vd,uint16_t cog,uint8_t satellites_visible) + uint64_t time_usec,uint8_t fix_type,int32_t lat,int32_t lon,int32_t alt,uint16_t eph,uint16_t epv,uint16_t vel,int16_t vn,int16_t ve,int16_t vd,uint16_t cog,uint8_t satellites_visible,uint8_t id,uint16_t yaw) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_HIL_GPS_LEN]; @@ -175,6 +189,8 @@ static inline uint16_t mavlink_msg_hil_gps_pack_chan(uint8_t system_id, uint8_t _mav_put_uint16_t(buf, 32, cog); _mav_put_uint8_t(buf, 34, fix_type); _mav_put_uint8_t(buf, 35, satellites_visible); + _mav_put_uint8_t(buf, 36, id); + _mav_put_uint16_t(buf, 37, yaw); memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_HIL_GPS_LEN); #else @@ -192,6 +208,8 @@ static inline uint16_t mavlink_msg_hil_gps_pack_chan(uint8_t system_id, uint8_t packet.cog = cog; packet.fix_type = fix_type; packet.satellites_visible = satellites_visible; + packet.id = id; + packet.yaw = yaw; memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_HIL_GPS_LEN); #endif @@ -210,7 +228,7 @@ static inline uint16_t mavlink_msg_hil_gps_pack_chan(uint8_t system_id, uint8_t */ static inline uint16_t mavlink_msg_hil_gps_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_hil_gps_t* hil_gps) { - return mavlink_msg_hil_gps_pack(system_id, component_id, msg, hil_gps->time_usec, hil_gps->fix_type, hil_gps->lat, hil_gps->lon, hil_gps->alt, hil_gps->eph, hil_gps->epv, hil_gps->vel, hil_gps->vn, hil_gps->ve, hil_gps->vd, hil_gps->cog, hil_gps->satellites_visible); + return mavlink_msg_hil_gps_pack(system_id, component_id, msg, hil_gps->time_usec, hil_gps->fix_type, hil_gps->lat, hil_gps->lon, hil_gps->alt, hil_gps->eph, hil_gps->epv, hil_gps->vel, hil_gps->vn, hil_gps->ve, hil_gps->vd, hil_gps->cog, hil_gps->satellites_visible, hil_gps->id, hil_gps->yaw); } /** @@ -224,7 +242,7 @@ static inline uint16_t mavlink_msg_hil_gps_encode(uint8_t system_id, uint8_t com */ static inline uint16_t mavlink_msg_hil_gps_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_hil_gps_t* hil_gps) { - return mavlink_msg_hil_gps_pack_chan(system_id, component_id, chan, msg, hil_gps->time_usec, hil_gps->fix_type, hil_gps->lat, hil_gps->lon, hil_gps->alt, hil_gps->eph, hil_gps->epv, hil_gps->vel, hil_gps->vn, hil_gps->ve, hil_gps->vd, hil_gps->cog, hil_gps->satellites_visible); + return mavlink_msg_hil_gps_pack_chan(system_id, component_id, chan, msg, hil_gps->time_usec, hil_gps->fix_type, hil_gps->lat, hil_gps->lon, hil_gps->alt, hil_gps->eph, hil_gps->epv, hil_gps->vel, hil_gps->vn, hil_gps->ve, hil_gps->vd, hil_gps->cog, hil_gps->satellites_visible, hil_gps->id, hil_gps->yaw); } /** @@ -236,18 +254,20 @@ static inline uint16_t mavlink_msg_hil_gps_encode_chan(uint8_t system_id, uint8_ * @param lat [degE7] Latitude (WGS84) * @param lon [degE7] Longitude (WGS84) * @param alt [mm] Altitude (MSL). Positive for up. - * @param eph [cm] GPS HDOP horizontal dilution of position. If unknown, set to: 65535 - * @param epv [cm] GPS VDOP vertical dilution of position. If unknown, set to: 65535 + * @param eph GPS HDOP horizontal dilution of position (unitless). If unknown, set to: UINT16_MAX + * @param epv GPS VDOP vertical dilution of position (unitless). If unknown, set to: UINT16_MAX * @param vel [cm/s] GPS ground speed. If unknown, set to: 65535 * @param vn [cm/s] GPS velocity in north direction in earth-fixed NED frame * @param ve [cm/s] GPS velocity in east direction in earth-fixed NED frame * @param vd [cm/s] GPS velocity in down direction in earth-fixed NED frame * @param cog [cdeg] Course over ground (NOT heading, but direction of movement), 0.0..359.99 degrees. If unknown, set to: 65535 * @param satellites_visible Number of satellites visible. If unknown, set to 255 + * @param id GPS ID (zero indexed). Used for multiple GPS inputs + * @param yaw [cdeg] Yaw of vehicle relative to Earth's North, zero means not available, use 36000 for north */ #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS -static inline void mavlink_msg_hil_gps_send(mavlink_channel_t chan, uint64_t time_usec, uint8_t fix_type, int32_t lat, int32_t lon, int32_t alt, uint16_t eph, uint16_t epv, uint16_t vel, int16_t vn, int16_t ve, int16_t vd, uint16_t cog, uint8_t satellites_visible) +static inline void mavlink_msg_hil_gps_send(mavlink_channel_t chan, uint64_t time_usec, uint8_t fix_type, int32_t lat, int32_t lon, int32_t alt, uint16_t eph, uint16_t epv, uint16_t vel, int16_t vn, int16_t ve, int16_t vd, uint16_t cog, uint8_t satellites_visible, uint8_t id, uint16_t yaw) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_HIL_GPS_LEN]; @@ -264,6 +284,8 @@ static inline void mavlink_msg_hil_gps_send(mavlink_channel_t chan, uint64_t tim _mav_put_uint16_t(buf, 32, cog); _mav_put_uint8_t(buf, 34, fix_type); _mav_put_uint8_t(buf, 35, satellites_visible); + _mav_put_uint8_t(buf, 36, id); + _mav_put_uint16_t(buf, 37, yaw); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HIL_GPS, buf, MAVLINK_MSG_ID_HIL_GPS_MIN_LEN, MAVLINK_MSG_ID_HIL_GPS_LEN, MAVLINK_MSG_ID_HIL_GPS_CRC); #else @@ -281,6 +303,8 @@ static inline void mavlink_msg_hil_gps_send(mavlink_channel_t chan, uint64_t tim packet.cog = cog; packet.fix_type = fix_type; packet.satellites_visible = satellites_visible; + packet.id = id; + packet.yaw = yaw; _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HIL_GPS, (const char *)&packet, MAVLINK_MSG_ID_HIL_GPS_MIN_LEN, MAVLINK_MSG_ID_HIL_GPS_LEN, MAVLINK_MSG_ID_HIL_GPS_CRC); #endif @@ -294,7 +318,7 @@ static inline void mavlink_msg_hil_gps_send(mavlink_channel_t chan, uint64_t tim static inline void mavlink_msg_hil_gps_send_struct(mavlink_channel_t chan, const mavlink_hil_gps_t* hil_gps) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - mavlink_msg_hil_gps_send(chan, hil_gps->time_usec, hil_gps->fix_type, hil_gps->lat, hil_gps->lon, hil_gps->alt, hil_gps->eph, hil_gps->epv, hil_gps->vel, hil_gps->vn, hil_gps->ve, hil_gps->vd, hil_gps->cog, hil_gps->satellites_visible); + mavlink_msg_hil_gps_send(chan, hil_gps->time_usec, hil_gps->fix_type, hil_gps->lat, hil_gps->lon, hil_gps->alt, hil_gps->eph, hil_gps->epv, hil_gps->vel, hil_gps->vn, hil_gps->ve, hil_gps->vd, hil_gps->cog, hil_gps->satellites_visible, hil_gps->id, hil_gps->yaw); #else _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HIL_GPS, (const char *)hil_gps, MAVLINK_MSG_ID_HIL_GPS_MIN_LEN, MAVLINK_MSG_ID_HIL_GPS_LEN, MAVLINK_MSG_ID_HIL_GPS_CRC); #endif @@ -308,7 +332,7 @@ static inline void mavlink_msg_hil_gps_send_struct(mavlink_channel_t chan, const is usually the receive buffer for the channel, and allows a reply to an incoming message with minimum stack space usage. */ -static inline void mavlink_msg_hil_gps_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint64_t time_usec, uint8_t fix_type, int32_t lat, int32_t lon, int32_t alt, uint16_t eph, uint16_t epv, uint16_t vel, int16_t vn, int16_t ve, int16_t vd, uint16_t cog, uint8_t satellites_visible) +static inline void mavlink_msg_hil_gps_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint64_t time_usec, uint8_t fix_type, int32_t lat, int32_t lon, int32_t alt, uint16_t eph, uint16_t epv, uint16_t vel, int16_t vn, int16_t ve, int16_t vd, uint16_t cog, uint8_t satellites_visible, uint8_t id, uint16_t yaw) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char *buf = (char *)msgbuf; @@ -325,6 +349,8 @@ static inline void mavlink_msg_hil_gps_send_buf(mavlink_message_t *msgbuf, mavli _mav_put_uint16_t(buf, 32, cog); _mav_put_uint8_t(buf, 34, fix_type); _mav_put_uint8_t(buf, 35, satellites_visible); + _mav_put_uint8_t(buf, 36, id); + _mav_put_uint16_t(buf, 37, yaw); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HIL_GPS, buf, MAVLINK_MSG_ID_HIL_GPS_MIN_LEN, MAVLINK_MSG_ID_HIL_GPS_LEN, MAVLINK_MSG_ID_HIL_GPS_CRC); #else @@ -342,6 +368,8 @@ static inline void mavlink_msg_hil_gps_send_buf(mavlink_message_t *msgbuf, mavli packet->cog = cog; packet->fix_type = fix_type; packet->satellites_visible = satellites_visible; + packet->id = id; + packet->yaw = yaw; _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HIL_GPS, (const char *)packet, MAVLINK_MSG_ID_HIL_GPS_MIN_LEN, MAVLINK_MSG_ID_HIL_GPS_LEN, MAVLINK_MSG_ID_HIL_GPS_CRC); #endif @@ -406,7 +434,7 @@ static inline int32_t mavlink_msg_hil_gps_get_alt(const mavlink_message_t* msg) /** * @brief Get field eph from hil_gps message * - * @return [cm] GPS HDOP horizontal dilution of position. If unknown, set to: 65535 + * @return GPS HDOP horizontal dilution of position (unitless). If unknown, set to: UINT16_MAX */ static inline uint16_t mavlink_msg_hil_gps_get_eph(const mavlink_message_t* msg) { @@ -416,7 +444,7 @@ static inline uint16_t mavlink_msg_hil_gps_get_eph(const mavlink_message_t* msg) /** * @brief Get field epv from hil_gps message * - * @return [cm] GPS VDOP vertical dilution of position. If unknown, set to: 65535 + * @return GPS VDOP vertical dilution of position (unitless). If unknown, set to: UINT16_MAX */ static inline uint16_t mavlink_msg_hil_gps_get_epv(const mavlink_message_t* msg) { @@ -483,6 +511,26 @@ static inline uint8_t mavlink_msg_hil_gps_get_satellites_visible(const mavlink_m return _MAV_RETURN_uint8_t(msg, 35); } +/** + * @brief Get field id from hil_gps message + * + * @return GPS ID (zero indexed). Used for multiple GPS inputs + */ +static inline uint8_t mavlink_msg_hil_gps_get_id(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 36); +} + +/** + * @brief Get field yaw from hil_gps message + * + * @return [cdeg] Yaw of vehicle relative to Earth's North, zero means not available, use 36000 for north + */ +static inline uint16_t mavlink_msg_hil_gps_get_yaw(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 37); +} + /** * @brief Decode a hil_gps message into a struct * @@ -505,6 +553,8 @@ static inline void mavlink_msg_hil_gps_decode(const mavlink_message_t* msg, mavl hil_gps->cog = mavlink_msg_hil_gps_get_cog(msg); hil_gps->fix_type = mavlink_msg_hil_gps_get_fix_type(msg); hil_gps->satellites_visible = mavlink_msg_hil_gps_get_satellites_visible(msg); + hil_gps->id = mavlink_msg_hil_gps_get_id(msg); + hil_gps->yaw = mavlink_msg_hil_gps_get_yaw(msg); #else uint8_t len = msg->len < MAVLINK_MSG_ID_HIL_GPS_LEN? msg->len : MAVLINK_MSG_ID_HIL_GPS_LEN; memset(hil_gps, 0, MAVLINK_MSG_ID_HIL_GPS_LEN); diff --git a/lib/main/MAVLink/common/mavlink_msg_hil_sensor.h b/lib/main/MAVLink/common/mavlink_msg_hil_sensor.h index e9d03cc70e..007c5a6989 100755 --- a/lib/main/MAVLink/common/mavlink_msg_hil_sensor.h +++ b/lib/main/MAVLink/common/mavlink_msg_hil_sensor.h @@ -15,16 +15,17 @@ typedef struct __mavlink_hil_sensor_t { float xmag; /*< [gauss] X Magnetic field*/ float ymag; /*< [gauss] Y Magnetic field*/ float zmag; /*< [gauss] Z Magnetic field*/ - float abs_pressure; /*< [mbar] Absolute pressure*/ - float diff_pressure; /*< [mbar] Differential pressure (airspeed)*/ + float abs_pressure; /*< [hPa] Absolute pressure*/ + float diff_pressure; /*< [hPa] Differential pressure (airspeed)*/ float pressure_alt; /*< Altitude calculated from pressure*/ float temperature; /*< [degC] Temperature*/ uint32_t fields_updated; /*< Bitmap for fields that have updated since last message, bit 0 = xacc, bit 12: temperature, bit 31: full reset of attitude/position/velocities/etc was performed in sim.*/ + uint8_t id; /*< Sensor ID (zero indexed). Used for multiple sensor inputs*/ } mavlink_hil_sensor_t; -#define MAVLINK_MSG_ID_HIL_SENSOR_LEN 64 +#define MAVLINK_MSG_ID_HIL_SENSOR_LEN 65 #define MAVLINK_MSG_ID_HIL_SENSOR_MIN_LEN 64 -#define MAVLINK_MSG_ID_107_LEN 64 +#define MAVLINK_MSG_ID_107_LEN 65 #define MAVLINK_MSG_ID_107_MIN_LEN 64 #define MAVLINK_MSG_ID_HIL_SENSOR_CRC 108 @@ -36,7 +37,7 @@ typedef struct __mavlink_hil_sensor_t { #define MAVLINK_MESSAGE_INFO_HIL_SENSOR { \ 107, \ "HIL_SENSOR", \ - 15, \ + 16, \ { { "time_usec", NULL, MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_hil_sensor_t, time_usec) }, \ { "xacc", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_hil_sensor_t, xacc) }, \ { "yacc", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_hil_sensor_t, yacc) }, \ @@ -52,12 +53,13 @@ typedef struct __mavlink_hil_sensor_t { { "pressure_alt", NULL, MAVLINK_TYPE_FLOAT, 0, 52, offsetof(mavlink_hil_sensor_t, pressure_alt) }, \ { "temperature", NULL, MAVLINK_TYPE_FLOAT, 0, 56, offsetof(mavlink_hil_sensor_t, temperature) }, \ { "fields_updated", NULL, MAVLINK_TYPE_UINT32_T, 0, 60, offsetof(mavlink_hil_sensor_t, fields_updated) }, \ + { "id", NULL, MAVLINK_TYPE_UINT8_T, 0, 64, offsetof(mavlink_hil_sensor_t, id) }, \ } \ } #else #define MAVLINK_MESSAGE_INFO_HIL_SENSOR { \ "HIL_SENSOR", \ - 15, \ + 16, \ { { "time_usec", NULL, MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_hil_sensor_t, time_usec) }, \ { "xacc", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_hil_sensor_t, xacc) }, \ { "yacc", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_hil_sensor_t, yacc) }, \ @@ -73,6 +75,7 @@ typedef struct __mavlink_hil_sensor_t { { "pressure_alt", NULL, MAVLINK_TYPE_FLOAT, 0, 52, offsetof(mavlink_hil_sensor_t, pressure_alt) }, \ { "temperature", NULL, MAVLINK_TYPE_FLOAT, 0, 56, offsetof(mavlink_hil_sensor_t, temperature) }, \ { "fields_updated", NULL, MAVLINK_TYPE_UINT32_T, 0, 60, offsetof(mavlink_hil_sensor_t, fields_updated) }, \ + { "id", NULL, MAVLINK_TYPE_UINT8_T, 0, 64, offsetof(mavlink_hil_sensor_t, id) }, \ } \ } #endif @@ -93,15 +96,16 @@ typedef struct __mavlink_hil_sensor_t { * @param xmag [gauss] X Magnetic field * @param ymag [gauss] Y Magnetic field * @param zmag [gauss] Z Magnetic field - * @param abs_pressure [mbar] Absolute pressure - * @param diff_pressure [mbar] Differential pressure (airspeed) + * @param abs_pressure [hPa] Absolute pressure + * @param diff_pressure [hPa] Differential pressure (airspeed) * @param pressure_alt Altitude calculated from pressure * @param temperature [degC] Temperature * @param fields_updated Bitmap for fields that have updated since last message, bit 0 = xacc, bit 12: temperature, bit 31: full reset of attitude/position/velocities/etc was performed in sim. + * @param id Sensor ID (zero indexed). Used for multiple sensor inputs * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_hil_sensor_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint64_t time_usec, float xacc, float yacc, float zacc, float xgyro, float ygyro, float zgyro, float xmag, float ymag, float zmag, float abs_pressure, float diff_pressure, float pressure_alt, float temperature, uint32_t fields_updated) + uint64_t time_usec, float xacc, float yacc, float zacc, float xgyro, float ygyro, float zgyro, float xmag, float ymag, float zmag, float abs_pressure, float diff_pressure, float pressure_alt, float temperature, uint32_t fields_updated, uint8_t id) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_HIL_SENSOR_LEN]; @@ -120,6 +124,7 @@ static inline uint16_t mavlink_msg_hil_sensor_pack(uint8_t system_id, uint8_t co _mav_put_float(buf, 52, pressure_alt); _mav_put_float(buf, 56, temperature); _mav_put_uint32_t(buf, 60, fields_updated); + _mav_put_uint8_t(buf, 64, id); memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_HIL_SENSOR_LEN); #else @@ -139,6 +144,7 @@ static inline uint16_t mavlink_msg_hil_sensor_pack(uint8_t system_id, uint8_t co packet.pressure_alt = pressure_alt; packet.temperature = temperature; packet.fields_updated = fields_updated; + packet.id = id; memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_HIL_SENSOR_LEN); #endif @@ -163,16 +169,17 @@ static inline uint16_t mavlink_msg_hil_sensor_pack(uint8_t system_id, uint8_t co * @param xmag [gauss] X Magnetic field * @param ymag [gauss] Y Magnetic field * @param zmag [gauss] Z Magnetic field - * @param abs_pressure [mbar] Absolute pressure - * @param diff_pressure [mbar] Differential pressure (airspeed) + * @param abs_pressure [hPa] Absolute pressure + * @param diff_pressure [hPa] Differential pressure (airspeed) * @param pressure_alt Altitude calculated from pressure * @param temperature [degC] Temperature * @param fields_updated Bitmap for fields that have updated since last message, bit 0 = xacc, bit 12: temperature, bit 31: full reset of attitude/position/velocities/etc was performed in sim. + * @param id Sensor ID (zero indexed). Used for multiple sensor inputs * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_hil_sensor_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, - uint64_t time_usec,float xacc,float yacc,float zacc,float xgyro,float ygyro,float zgyro,float xmag,float ymag,float zmag,float abs_pressure,float diff_pressure,float pressure_alt,float temperature,uint32_t fields_updated) + uint64_t time_usec,float xacc,float yacc,float zacc,float xgyro,float ygyro,float zgyro,float xmag,float ymag,float zmag,float abs_pressure,float diff_pressure,float pressure_alt,float temperature,uint32_t fields_updated,uint8_t id) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_HIL_SENSOR_LEN]; @@ -191,6 +198,7 @@ static inline uint16_t mavlink_msg_hil_sensor_pack_chan(uint8_t system_id, uint8 _mav_put_float(buf, 52, pressure_alt); _mav_put_float(buf, 56, temperature); _mav_put_uint32_t(buf, 60, fields_updated); + _mav_put_uint8_t(buf, 64, id); memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_HIL_SENSOR_LEN); #else @@ -210,6 +218,7 @@ static inline uint16_t mavlink_msg_hil_sensor_pack_chan(uint8_t system_id, uint8 packet.pressure_alt = pressure_alt; packet.temperature = temperature; packet.fields_updated = fields_updated; + packet.id = id; memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_HIL_SENSOR_LEN); #endif @@ -228,7 +237,7 @@ static inline uint16_t mavlink_msg_hil_sensor_pack_chan(uint8_t system_id, uint8 */ static inline uint16_t mavlink_msg_hil_sensor_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_hil_sensor_t* hil_sensor) { - return mavlink_msg_hil_sensor_pack(system_id, component_id, msg, hil_sensor->time_usec, hil_sensor->xacc, hil_sensor->yacc, hil_sensor->zacc, hil_sensor->xgyro, hil_sensor->ygyro, hil_sensor->zgyro, hil_sensor->xmag, hil_sensor->ymag, hil_sensor->zmag, hil_sensor->abs_pressure, hil_sensor->diff_pressure, hil_sensor->pressure_alt, hil_sensor->temperature, hil_sensor->fields_updated); + return mavlink_msg_hil_sensor_pack(system_id, component_id, msg, hil_sensor->time_usec, hil_sensor->xacc, hil_sensor->yacc, hil_sensor->zacc, hil_sensor->xgyro, hil_sensor->ygyro, hil_sensor->zgyro, hil_sensor->xmag, hil_sensor->ymag, hil_sensor->zmag, hil_sensor->abs_pressure, hil_sensor->diff_pressure, hil_sensor->pressure_alt, hil_sensor->temperature, hil_sensor->fields_updated, hil_sensor->id); } /** @@ -242,7 +251,7 @@ static inline uint16_t mavlink_msg_hil_sensor_encode(uint8_t system_id, uint8_t */ static inline uint16_t mavlink_msg_hil_sensor_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_hil_sensor_t* hil_sensor) { - return mavlink_msg_hil_sensor_pack_chan(system_id, component_id, chan, msg, hil_sensor->time_usec, hil_sensor->xacc, hil_sensor->yacc, hil_sensor->zacc, hil_sensor->xgyro, hil_sensor->ygyro, hil_sensor->zgyro, hil_sensor->xmag, hil_sensor->ymag, hil_sensor->zmag, hil_sensor->abs_pressure, hil_sensor->diff_pressure, hil_sensor->pressure_alt, hil_sensor->temperature, hil_sensor->fields_updated); + return mavlink_msg_hil_sensor_pack_chan(system_id, component_id, chan, msg, hil_sensor->time_usec, hil_sensor->xacc, hil_sensor->yacc, hil_sensor->zacc, hil_sensor->xgyro, hil_sensor->ygyro, hil_sensor->zgyro, hil_sensor->xmag, hil_sensor->ymag, hil_sensor->zmag, hil_sensor->abs_pressure, hil_sensor->diff_pressure, hil_sensor->pressure_alt, hil_sensor->temperature, hil_sensor->fields_updated, hil_sensor->id); } /** @@ -259,15 +268,16 @@ static inline uint16_t mavlink_msg_hil_sensor_encode_chan(uint8_t system_id, uin * @param xmag [gauss] X Magnetic field * @param ymag [gauss] Y Magnetic field * @param zmag [gauss] Z Magnetic field - * @param abs_pressure [mbar] Absolute pressure - * @param diff_pressure [mbar] Differential pressure (airspeed) + * @param abs_pressure [hPa] Absolute pressure + * @param diff_pressure [hPa] Differential pressure (airspeed) * @param pressure_alt Altitude calculated from pressure * @param temperature [degC] Temperature * @param fields_updated Bitmap for fields that have updated since last message, bit 0 = xacc, bit 12: temperature, bit 31: full reset of attitude/position/velocities/etc was performed in sim. + * @param id Sensor ID (zero indexed). Used for multiple sensor inputs */ #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS -static inline void mavlink_msg_hil_sensor_send(mavlink_channel_t chan, uint64_t time_usec, float xacc, float yacc, float zacc, float xgyro, float ygyro, float zgyro, float xmag, float ymag, float zmag, float abs_pressure, float diff_pressure, float pressure_alt, float temperature, uint32_t fields_updated) +static inline void mavlink_msg_hil_sensor_send(mavlink_channel_t chan, uint64_t time_usec, float xacc, float yacc, float zacc, float xgyro, float ygyro, float zgyro, float xmag, float ymag, float zmag, float abs_pressure, float diff_pressure, float pressure_alt, float temperature, uint32_t fields_updated, uint8_t id) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_HIL_SENSOR_LEN]; @@ -286,6 +296,7 @@ static inline void mavlink_msg_hil_sensor_send(mavlink_channel_t chan, uint64_t _mav_put_float(buf, 52, pressure_alt); _mav_put_float(buf, 56, temperature); _mav_put_uint32_t(buf, 60, fields_updated); + _mav_put_uint8_t(buf, 64, id); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HIL_SENSOR, buf, MAVLINK_MSG_ID_HIL_SENSOR_MIN_LEN, MAVLINK_MSG_ID_HIL_SENSOR_LEN, MAVLINK_MSG_ID_HIL_SENSOR_CRC); #else @@ -305,6 +316,7 @@ static inline void mavlink_msg_hil_sensor_send(mavlink_channel_t chan, uint64_t packet.pressure_alt = pressure_alt; packet.temperature = temperature; packet.fields_updated = fields_updated; + packet.id = id; _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HIL_SENSOR, (const char *)&packet, MAVLINK_MSG_ID_HIL_SENSOR_MIN_LEN, MAVLINK_MSG_ID_HIL_SENSOR_LEN, MAVLINK_MSG_ID_HIL_SENSOR_CRC); #endif @@ -318,7 +330,7 @@ static inline void mavlink_msg_hil_sensor_send(mavlink_channel_t chan, uint64_t static inline void mavlink_msg_hil_sensor_send_struct(mavlink_channel_t chan, const mavlink_hil_sensor_t* hil_sensor) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - mavlink_msg_hil_sensor_send(chan, hil_sensor->time_usec, hil_sensor->xacc, hil_sensor->yacc, hil_sensor->zacc, hil_sensor->xgyro, hil_sensor->ygyro, hil_sensor->zgyro, hil_sensor->xmag, hil_sensor->ymag, hil_sensor->zmag, hil_sensor->abs_pressure, hil_sensor->diff_pressure, hil_sensor->pressure_alt, hil_sensor->temperature, hil_sensor->fields_updated); + mavlink_msg_hil_sensor_send(chan, hil_sensor->time_usec, hil_sensor->xacc, hil_sensor->yacc, hil_sensor->zacc, hil_sensor->xgyro, hil_sensor->ygyro, hil_sensor->zgyro, hil_sensor->xmag, hil_sensor->ymag, hil_sensor->zmag, hil_sensor->abs_pressure, hil_sensor->diff_pressure, hil_sensor->pressure_alt, hil_sensor->temperature, hil_sensor->fields_updated, hil_sensor->id); #else _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HIL_SENSOR, (const char *)hil_sensor, MAVLINK_MSG_ID_HIL_SENSOR_MIN_LEN, MAVLINK_MSG_ID_HIL_SENSOR_LEN, MAVLINK_MSG_ID_HIL_SENSOR_CRC); #endif @@ -332,7 +344,7 @@ static inline void mavlink_msg_hil_sensor_send_struct(mavlink_channel_t chan, co is usually the receive buffer for the channel, and allows a reply to an incoming message with minimum stack space usage. */ -static inline void mavlink_msg_hil_sensor_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint64_t time_usec, float xacc, float yacc, float zacc, float xgyro, float ygyro, float zgyro, float xmag, float ymag, float zmag, float abs_pressure, float diff_pressure, float pressure_alt, float temperature, uint32_t fields_updated) +static inline void mavlink_msg_hil_sensor_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint64_t time_usec, float xacc, float yacc, float zacc, float xgyro, float ygyro, float zgyro, float xmag, float ymag, float zmag, float abs_pressure, float diff_pressure, float pressure_alt, float temperature, uint32_t fields_updated, uint8_t id) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char *buf = (char *)msgbuf; @@ -351,6 +363,7 @@ static inline void mavlink_msg_hil_sensor_send_buf(mavlink_message_t *msgbuf, ma _mav_put_float(buf, 52, pressure_alt); _mav_put_float(buf, 56, temperature); _mav_put_uint32_t(buf, 60, fields_updated); + _mav_put_uint8_t(buf, 64, id); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HIL_SENSOR, buf, MAVLINK_MSG_ID_HIL_SENSOR_MIN_LEN, MAVLINK_MSG_ID_HIL_SENSOR_LEN, MAVLINK_MSG_ID_HIL_SENSOR_CRC); #else @@ -370,6 +383,7 @@ static inline void mavlink_msg_hil_sensor_send_buf(mavlink_message_t *msgbuf, ma packet->pressure_alt = pressure_alt; packet->temperature = temperature; packet->fields_updated = fields_updated; + packet->id = id; _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HIL_SENSOR, (const char *)packet, MAVLINK_MSG_ID_HIL_SENSOR_MIN_LEN, MAVLINK_MSG_ID_HIL_SENSOR_LEN, MAVLINK_MSG_ID_HIL_SENSOR_CRC); #endif @@ -484,7 +498,7 @@ static inline float mavlink_msg_hil_sensor_get_zmag(const mavlink_message_t* msg /** * @brief Get field abs_pressure from hil_sensor message * - * @return [mbar] Absolute pressure + * @return [hPa] Absolute pressure */ static inline float mavlink_msg_hil_sensor_get_abs_pressure(const mavlink_message_t* msg) { @@ -494,7 +508,7 @@ static inline float mavlink_msg_hil_sensor_get_abs_pressure(const mavlink_messag /** * @brief Get field diff_pressure from hil_sensor message * - * @return [mbar] Differential pressure (airspeed) + * @return [hPa] Differential pressure (airspeed) */ static inline float mavlink_msg_hil_sensor_get_diff_pressure(const mavlink_message_t* msg) { @@ -531,6 +545,16 @@ static inline uint32_t mavlink_msg_hil_sensor_get_fields_updated(const mavlink_m return _MAV_RETURN_uint32_t(msg, 60); } +/** + * @brief Get field id from hil_sensor message + * + * @return Sensor ID (zero indexed). Used for multiple sensor inputs + */ +static inline uint8_t mavlink_msg_hil_sensor_get_id(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 64); +} + /** * @brief Decode a hil_sensor message into a struct * @@ -555,6 +579,7 @@ static inline void mavlink_msg_hil_sensor_decode(const mavlink_message_t* msg, m hil_sensor->pressure_alt = mavlink_msg_hil_sensor_get_pressure_alt(msg); hil_sensor->temperature = mavlink_msg_hil_sensor_get_temperature(msg); hil_sensor->fields_updated = mavlink_msg_hil_sensor_get_fields_updated(msg); + hil_sensor->id = mavlink_msg_hil_sensor_get_id(msg); #else uint8_t len = msg->len < MAVLINK_MSG_ID_HIL_SENSOR_LEN? msg->len : MAVLINK_MSG_ID_HIL_SENSOR_LEN; memset(hil_sensor, 0, MAVLINK_MSG_ID_HIL_SENSOR_LEN); diff --git a/lib/main/MAVLink/common/mavlink_msg_home_position.h b/lib/main/MAVLink/common/mavlink_msg_home_position.h index d275d1d630..ab5130b810 100755 --- a/lib/main/MAVLink/common/mavlink_msg_home_position.h +++ b/lib/main/MAVLink/common/mavlink_msg_home_position.h @@ -3,7 +3,7 @@ #define MAVLINK_MSG_ID_HOME_POSITION 242 - +MAVPACKED( typedef struct __mavlink_home_position_t { int32_t latitude; /*< [degE7] Latitude (WGS84)*/ int32_t longitude; /*< [degE7] Longitude (WGS84)*/ @@ -15,11 +15,12 @@ typedef struct __mavlink_home_position_t { float approach_x; /*< [m] Local X position of the end of the approach vector. Multicopters should set this position based on their takeoff path. Grass-landing fixed wing aircraft should set it the same way as multicopters. Runway-landing fixed wing aircraft should set it to the opposite direction of the takeoff, assuming the takeoff happened from the threshold / touchdown zone.*/ float approach_y; /*< [m] Local Y position of the end of the approach vector. Multicopters should set this position based on their takeoff path. Grass-landing fixed wing aircraft should set it the same way as multicopters. Runway-landing fixed wing aircraft should set it to the opposite direction of the takeoff, assuming the takeoff happened from the threshold / touchdown zone.*/ float approach_z; /*< [m] Local Z position of the end of the approach vector. Multicopters should set this position based on their takeoff path. Grass-landing fixed wing aircraft should set it the same way as multicopters. Runway-landing fixed wing aircraft should set it to the opposite direction of the takeoff, assuming the takeoff happened from the threshold / touchdown zone.*/ -} mavlink_home_position_t; + uint64_t time_usec; /*< [us] Timestamp (UNIX Epoch time or time since system boot). The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude of the number.*/ +}) mavlink_home_position_t; -#define MAVLINK_MSG_ID_HOME_POSITION_LEN 52 +#define MAVLINK_MSG_ID_HOME_POSITION_LEN 60 #define MAVLINK_MSG_ID_HOME_POSITION_MIN_LEN 52 -#define MAVLINK_MSG_ID_242_LEN 52 +#define MAVLINK_MSG_ID_242_LEN 60 #define MAVLINK_MSG_ID_242_MIN_LEN 52 #define MAVLINK_MSG_ID_HOME_POSITION_CRC 104 @@ -31,7 +32,7 @@ typedef struct __mavlink_home_position_t { #define MAVLINK_MESSAGE_INFO_HOME_POSITION { \ 242, \ "HOME_POSITION", \ - 10, \ + 11, \ { { "latitude", NULL, MAVLINK_TYPE_INT32_T, 0, 0, offsetof(mavlink_home_position_t, latitude) }, \ { "longitude", NULL, MAVLINK_TYPE_INT32_T, 0, 4, offsetof(mavlink_home_position_t, longitude) }, \ { "altitude", NULL, MAVLINK_TYPE_INT32_T, 0, 8, offsetof(mavlink_home_position_t, altitude) }, \ @@ -42,12 +43,13 @@ typedef struct __mavlink_home_position_t { { "approach_x", NULL, MAVLINK_TYPE_FLOAT, 0, 40, offsetof(mavlink_home_position_t, approach_x) }, \ { "approach_y", NULL, MAVLINK_TYPE_FLOAT, 0, 44, offsetof(mavlink_home_position_t, approach_y) }, \ { "approach_z", NULL, MAVLINK_TYPE_FLOAT, 0, 48, offsetof(mavlink_home_position_t, approach_z) }, \ + { "time_usec", NULL, MAVLINK_TYPE_UINT64_T, 0, 52, offsetof(mavlink_home_position_t, time_usec) }, \ } \ } #else #define MAVLINK_MESSAGE_INFO_HOME_POSITION { \ "HOME_POSITION", \ - 10, \ + 11, \ { { "latitude", NULL, MAVLINK_TYPE_INT32_T, 0, 0, offsetof(mavlink_home_position_t, latitude) }, \ { "longitude", NULL, MAVLINK_TYPE_INT32_T, 0, 4, offsetof(mavlink_home_position_t, longitude) }, \ { "altitude", NULL, MAVLINK_TYPE_INT32_T, 0, 8, offsetof(mavlink_home_position_t, altitude) }, \ @@ -58,6 +60,7 @@ typedef struct __mavlink_home_position_t { { "approach_x", NULL, MAVLINK_TYPE_FLOAT, 0, 40, offsetof(mavlink_home_position_t, approach_x) }, \ { "approach_y", NULL, MAVLINK_TYPE_FLOAT, 0, 44, offsetof(mavlink_home_position_t, approach_y) }, \ { "approach_z", NULL, MAVLINK_TYPE_FLOAT, 0, 48, offsetof(mavlink_home_position_t, approach_z) }, \ + { "time_usec", NULL, MAVLINK_TYPE_UINT64_T, 0, 52, offsetof(mavlink_home_position_t, time_usec) }, \ } \ } #endif @@ -78,10 +81,11 @@ typedef struct __mavlink_home_position_t { * @param approach_x [m] Local X position of the end of the approach vector. Multicopters should set this position based on their takeoff path. Grass-landing fixed wing aircraft should set it the same way as multicopters. Runway-landing fixed wing aircraft should set it to the opposite direction of the takeoff, assuming the takeoff happened from the threshold / touchdown zone. * @param approach_y [m] Local Y position of the end of the approach vector. Multicopters should set this position based on their takeoff path. Grass-landing fixed wing aircraft should set it the same way as multicopters. Runway-landing fixed wing aircraft should set it to the opposite direction of the takeoff, assuming the takeoff happened from the threshold / touchdown zone. * @param approach_z [m] Local Z position of the end of the approach vector. Multicopters should set this position based on their takeoff path. Grass-landing fixed wing aircraft should set it the same way as multicopters. Runway-landing fixed wing aircraft should set it to the opposite direction of the takeoff, assuming the takeoff happened from the threshold / touchdown zone. + * @param time_usec [us] Timestamp (UNIX Epoch time or time since system boot). The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude of the number. * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_home_position_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - int32_t latitude, int32_t longitude, int32_t altitude, float x, float y, float z, const float *q, float approach_x, float approach_y, float approach_z) + int32_t latitude, int32_t longitude, int32_t altitude, float x, float y, float z, const float *q, float approach_x, float approach_y, float approach_z, uint64_t time_usec) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_HOME_POSITION_LEN]; @@ -94,6 +98,7 @@ static inline uint16_t mavlink_msg_home_position_pack(uint8_t system_id, uint8_t _mav_put_float(buf, 40, approach_x); _mav_put_float(buf, 44, approach_y); _mav_put_float(buf, 48, approach_z); + _mav_put_uint64_t(buf, 52, time_usec); _mav_put_float_array(buf, 24, q, 4); memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_HOME_POSITION_LEN); #else @@ -107,6 +112,7 @@ static inline uint16_t mavlink_msg_home_position_pack(uint8_t system_id, uint8_t packet.approach_x = approach_x; packet.approach_y = approach_y; packet.approach_z = approach_z; + packet.time_usec = time_usec; mav_array_memcpy(packet.q, q, sizeof(float)*4); memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_HOME_POSITION_LEN); #endif @@ -131,11 +137,12 @@ static inline uint16_t mavlink_msg_home_position_pack(uint8_t system_id, uint8_t * @param approach_x [m] Local X position of the end of the approach vector. Multicopters should set this position based on their takeoff path. Grass-landing fixed wing aircraft should set it the same way as multicopters. Runway-landing fixed wing aircraft should set it to the opposite direction of the takeoff, assuming the takeoff happened from the threshold / touchdown zone. * @param approach_y [m] Local Y position of the end of the approach vector. Multicopters should set this position based on their takeoff path. Grass-landing fixed wing aircraft should set it the same way as multicopters. Runway-landing fixed wing aircraft should set it to the opposite direction of the takeoff, assuming the takeoff happened from the threshold / touchdown zone. * @param approach_z [m] Local Z position of the end of the approach vector. Multicopters should set this position based on their takeoff path. Grass-landing fixed wing aircraft should set it the same way as multicopters. Runway-landing fixed wing aircraft should set it to the opposite direction of the takeoff, assuming the takeoff happened from the threshold / touchdown zone. + * @param time_usec [us] Timestamp (UNIX Epoch time or time since system boot). The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude of the number. * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_home_position_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, - int32_t latitude,int32_t longitude,int32_t altitude,float x,float y,float z,const float *q,float approach_x,float approach_y,float approach_z) + int32_t latitude,int32_t longitude,int32_t altitude,float x,float y,float z,const float *q,float approach_x,float approach_y,float approach_z,uint64_t time_usec) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_HOME_POSITION_LEN]; @@ -148,6 +155,7 @@ static inline uint16_t mavlink_msg_home_position_pack_chan(uint8_t system_id, ui _mav_put_float(buf, 40, approach_x); _mav_put_float(buf, 44, approach_y); _mav_put_float(buf, 48, approach_z); + _mav_put_uint64_t(buf, 52, time_usec); _mav_put_float_array(buf, 24, q, 4); memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_HOME_POSITION_LEN); #else @@ -161,6 +169,7 @@ static inline uint16_t mavlink_msg_home_position_pack_chan(uint8_t system_id, ui packet.approach_x = approach_x; packet.approach_y = approach_y; packet.approach_z = approach_z; + packet.time_usec = time_usec; mav_array_memcpy(packet.q, q, sizeof(float)*4); memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_HOME_POSITION_LEN); #endif @@ -179,7 +188,7 @@ static inline uint16_t mavlink_msg_home_position_pack_chan(uint8_t system_id, ui */ static inline uint16_t mavlink_msg_home_position_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_home_position_t* home_position) { - return mavlink_msg_home_position_pack(system_id, component_id, msg, home_position->latitude, home_position->longitude, home_position->altitude, home_position->x, home_position->y, home_position->z, home_position->q, home_position->approach_x, home_position->approach_y, home_position->approach_z); + return mavlink_msg_home_position_pack(system_id, component_id, msg, home_position->latitude, home_position->longitude, home_position->altitude, home_position->x, home_position->y, home_position->z, home_position->q, home_position->approach_x, home_position->approach_y, home_position->approach_z, home_position->time_usec); } /** @@ -193,7 +202,7 @@ static inline uint16_t mavlink_msg_home_position_encode(uint8_t system_id, uint8 */ static inline uint16_t mavlink_msg_home_position_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_home_position_t* home_position) { - return mavlink_msg_home_position_pack_chan(system_id, component_id, chan, msg, home_position->latitude, home_position->longitude, home_position->altitude, home_position->x, home_position->y, home_position->z, home_position->q, home_position->approach_x, home_position->approach_y, home_position->approach_z); + return mavlink_msg_home_position_pack_chan(system_id, component_id, chan, msg, home_position->latitude, home_position->longitude, home_position->altitude, home_position->x, home_position->y, home_position->z, home_position->q, home_position->approach_x, home_position->approach_y, home_position->approach_z, home_position->time_usec); } /** @@ -210,10 +219,11 @@ static inline uint16_t mavlink_msg_home_position_encode_chan(uint8_t system_id, * @param approach_x [m] Local X position of the end of the approach vector. Multicopters should set this position based on their takeoff path. Grass-landing fixed wing aircraft should set it the same way as multicopters. Runway-landing fixed wing aircraft should set it to the opposite direction of the takeoff, assuming the takeoff happened from the threshold / touchdown zone. * @param approach_y [m] Local Y position of the end of the approach vector. Multicopters should set this position based on their takeoff path. Grass-landing fixed wing aircraft should set it the same way as multicopters. Runway-landing fixed wing aircraft should set it to the opposite direction of the takeoff, assuming the takeoff happened from the threshold / touchdown zone. * @param approach_z [m] Local Z position of the end of the approach vector. Multicopters should set this position based on their takeoff path. Grass-landing fixed wing aircraft should set it the same way as multicopters. Runway-landing fixed wing aircraft should set it to the opposite direction of the takeoff, assuming the takeoff happened from the threshold / touchdown zone. + * @param time_usec [us] Timestamp (UNIX Epoch time or time since system boot). The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude of the number. */ #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS -static inline void mavlink_msg_home_position_send(mavlink_channel_t chan, int32_t latitude, int32_t longitude, int32_t altitude, float x, float y, float z, const float *q, float approach_x, float approach_y, float approach_z) +static inline void mavlink_msg_home_position_send(mavlink_channel_t chan, int32_t latitude, int32_t longitude, int32_t altitude, float x, float y, float z, const float *q, float approach_x, float approach_y, float approach_z, uint64_t time_usec) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_HOME_POSITION_LEN]; @@ -226,6 +236,7 @@ static inline void mavlink_msg_home_position_send(mavlink_channel_t chan, int32_ _mav_put_float(buf, 40, approach_x); _mav_put_float(buf, 44, approach_y); _mav_put_float(buf, 48, approach_z); + _mav_put_uint64_t(buf, 52, time_usec); _mav_put_float_array(buf, 24, q, 4); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HOME_POSITION, buf, MAVLINK_MSG_ID_HOME_POSITION_MIN_LEN, MAVLINK_MSG_ID_HOME_POSITION_LEN, MAVLINK_MSG_ID_HOME_POSITION_CRC); #else @@ -239,6 +250,7 @@ static inline void mavlink_msg_home_position_send(mavlink_channel_t chan, int32_ packet.approach_x = approach_x; packet.approach_y = approach_y; packet.approach_z = approach_z; + packet.time_usec = time_usec; mav_array_memcpy(packet.q, q, sizeof(float)*4); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HOME_POSITION, (const char *)&packet, MAVLINK_MSG_ID_HOME_POSITION_MIN_LEN, MAVLINK_MSG_ID_HOME_POSITION_LEN, MAVLINK_MSG_ID_HOME_POSITION_CRC); #endif @@ -252,7 +264,7 @@ static inline void mavlink_msg_home_position_send(mavlink_channel_t chan, int32_ static inline void mavlink_msg_home_position_send_struct(mavlink_channel_t chan, const mavlink_home_position_t* home_position) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - mavlink_msg_home_position_send(chan, home_position->latitude, home_position->longitude, home_position->altitude, home_position->x, home_position->y, home_position->z, home_position->q, home_position->approach_x, home_position->approach_y, home_position->approach_z); + mavlink_msg_home_position_send(chan, home_position->latitude, home_position->longitude, home_position->altitude, home_position->x, home_position->y, home_position->z, home_position->q, home_position->approach_x, home_position->approach_y, home_position->approach_z, home_position->time_usec); #else _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HOME_POSITION, (const char *)home_position, MAVLINK_MSG_ID_HOME_POSITION_MIN_LEN, MAVLINK_MSG_ID_HOME_POSITION_LEN, MAVLINK_MSG_ID_HOME_POSITION_CRC); #endif @@ -266,7 +278,7 @@ static inline void mavlink_msg_home_position_send_struct(mavlink_channel_t chan, is usually the receive buffer for the channel, and allows a reply to an incoming message with minimum stack space usage. */ -static inline void mavlink_msg_home_position_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, int32_t latitude, int32_t longitude, int32_t altitude, float x, float y, float z, const float *q, float approach_x, float approach_y, float approach_z) +static inline void mavlink_msg_home_position_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, int32_t latitude, int32_t longitude, int32_t altitude, float x, float y, float z, const float *q, float approach_x, float approach_y, float approach_z, uint64_t time_usec) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char *buf = (char *)msgbuf; @@ -279,6 +291,7 @@ static inline void mavlink_msg_home_position_send_buf(mavlink_message_t *msgbuf, _mav_put_float(buf, 40, approach_x); _mav_put_float(buf, 44, approach_y); _mav_put_float(buf, 48, approach_z); + _mav_put_uint64_t(buf, 52, time_usec); _mav_put_float_array(buf, 24, q, 4); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HOME_POSITION, buf, MAVLINK_MSG_ID_HOME_POSITION_MIN_LEN, MAVLINK_MSG_ID_HOME_POSITION_LEN, MAVLINK_MSG_ID_HOME_POSITION_CRC); #else @@ -292,6 +305,7 @@ static inline void mavlink_msg_home_position_send_buf(mavlink_message_t *msgbuf, packet->approach_x = approach_x; packet->approach_y = approach_y; packet->approach_z = approach_z; + packet->time_usec = time_usec; mav_array_memcpy(packet->q, q, sizeof(float)*4); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_HOME_POSITION, (const char *)packet, MAVLINK_MSG_ID_HOME_POSITION_MIN_LEN, MAVLINK_MSG_ID_HOME_POSITION_LEN, MAVLINK_MSG_ID_HOME_POSITION_CRC); #endif @@ -403,6 +417,16 @@ static inline float mavlink_msg_home_position_get_approach_z(const mavlink_messa return _MAV_RETURN_float(msg, 48); } +/** + * @brief Get field time_usec from home_position message + * + * @return [us] Timestamp (UNIX Epoch time or time since system boot). The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude of the number. + */ +static inline uint64_t mavlink_msg_home_position_get_time_usec(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint64_t(msg, 52); +} + /** * @brief Decode a home_position message into a struct * @@ -422,6 +446,7 @@ static inline void mavlink_msg_home_position_decode(const mavlink_message_t* msg home_position->approach_x = mavlink_msg_home_position_get_approach_x(msg); home_position->approach_y = mavlink_msg_home_position_get_approach_y(msg); home_position->approach_z = mavlink_msg_home_position_get_approach_z(msg); + home_position->time_usec = mavlink_msg_home_position_get_time_usec(msg); #else uint8_t len = msg->len < MAVLINK_MSG_ID_HOME_POSITION_LEN? msg->len : MAVLINK_MSG_ID_HOME_POSITION_LEN; memset(home_position, 0, MAVLINK_MSG_ID_HOME_POSITION_LEN); diff --git a/lib/main/MAVLink/common/mavlink_msg_isbd_link_status.h b/lib/main/MAVLink/common/mavlink_msg_isbd_link_status.h new file mode 100644 index 0000000000..a0c53745f9 --- /dev/null +++ b/lib/main/MAVLink/common/mavlink_msg_isbd_link_status.h @@ -0,0 +1,388 @@ +#pragma once +// MESSAGE ISBD_LINK_STATUS PACKING + +#define MAVLINK_MSG_ID_ISBD_LINK_STATUS 335 + + +typedef struct __mavlink_isbd_link_status_t { + uint64_t timestamp; /*< [us] Timestamp (UNIX Epoch time or time since system boot). The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude of the number.*/ + uint64_t last_heartbeat; /*< [us] Timestamp of the last successful sbd session. The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude of the number.*/ + uint16_t failed_sessions; /*< Number of failed SBD sessions.*/ + uint16_t successful_sessions; /*< Number of successful SBD sessions.*/ + uint8_t signal_quality; /*< Signal quality equal to the number of bars displayed on the ISU signal strength indicator. Range is 0 to 5, where 0 indicates no signal and 5 indicates maximum signal strength.*/ + uint8_t ring_pending; /*< 1: Ring call pending, 0: No call pending.*/ + uint8_t tx_session_pending; /*< 1: Transmission session pending, 0: No transmission session pending.*/ + uint8_t rx_session_pending; /*< 1: Receiving session pending, 0: No receiving session pending.*/ +} mavlink_isbd_link_status_t; + +#define MAVLINK_MSG_ID_ISBD_LINK_STATUS_LEN 24 +#define MAVLINK_MSG_ID_ISBD_LINK_STATUS_MIN_LEN 24 +#define MAVLINK_MSG_ID_335_LEN 24 +#define MAVLINK_MSG_ID_335_MIN_LEN 24 + +#define MAVLINK_MSG_ID_ISBD_LINK_STATUS_CRC 225 +#define MAVLINK_MSG_ID_335_CRC 225 + + + +#if MAVLINK_COMMAND_24BIT +#define MAVLINK_MESSAGE_INFO_ISBD_LINK_STATUS { \ + 335, \ + "ISBD_LINK_STATUS", \ + 8, \ + { { "timestamp", NULL, MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_isbd_link_status_t, timestamp) }, \ + { "last_heartbeat", NULL, MAVLINK_TYPE_UINT64_T, 0, 8, offsetof(mavlink_isbd_link_status_t, last_heartbeat) }, \ + { "failed_sessions", NULL, MAVLINK_TYPE_UINT16_T, 0, 16, offsetof(mavlink_isbd_link_status_t, failed_sessions) }, \ + { "successful_sessions", NULL, MAVLINK_TYPE_UINT16_T, 0, 18, offsetof(mavlink_isbd_link_status_t, successful_sessions) }, \ + { "signal_quality", NULL, MAVLINK_TYPE_UINT8_T, 0, 20, offsetof(mavlink_isbd_link_status_t, signal_quality) }, \ + { "ring_pending", NULL, MAVLINK_TYPE_UINT8_T, 0, 21, offsetof(mavlink_isbd_link_status_t, ring_pending) }, \ + { "tx_session_pending", NULL, MAVLINK_TYPE_UINT8_T, 0, 22, offsetof(mavlink_isbd_link_status_t, tx_session_pending) }, \ + { "rx_session_pending", NULL, MAVLINK_TYPE_UINT8_T, 0, 23, offsetof(mavlink_isbd_link_status_t, rx_session_pending) }, \ + } \ +} +#else +#define MAVLINK_MESSAGE_INFO_ISBD_LINK_STATUS { \ + "ISBD_LINK_STATUS", \ + 8, \ + { { "timestamp", NULL, MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_isbd_link_status_t, timestamp) }, \ + { "last_heartbeat", NULL, MAVLINK_TYPE_UINT64_T, 0, 8, offsetof(mavlink_isbd_link_status_t, last_heartbeat) }, \ + { "failed_sessions", NULL, MAVLINK_TYPE_UINT16_T, 0, 16, offsetof(mavlink_isbd_link_status_t, failed_sessions) }, \ + { "successful_sessions", NULL, MAVLINK_TYPE_UINT16_T, 0, 18, offsetof(mavlink_isbd_link_status_t, successful_sessions) }, \ + { "signal_quality", NULL, MAVLINK_TYPE_UINT8_T, 0, 20, offsetof(mavlink_isbd_link_status_t, signal_quality) }, \ + { "ring_pending", NULL, MAVLINK_TYPE_UINT8_T, 0, 21, offsetof(mavlink_isbd_link_status_t, ring_pending) }, \ + { "tx_session_pending", NULL, MAVLINK_TYPE_UINT8_T, 0, 22, offsetof(mavlink_isbd_link_status_t, tx_session_pending) }, \ + { "rx_session_pending", NULL, MAVLINK_TYPE_UINT8_T, 0, 23, offsetof(mavlink_isbd_link_status_t, rx_session_pending) }, \ + } \ +} +#endif + +/** + * @brief Pack a isbd_link_status message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param timestamp [us] Timestamp (UNIX Epoch time or time since system boot). The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude of the number. + * @param last_heartbeat [us] Timestamp of the last successful sbd session. The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude of the number. + * @param failed_sessions Number of failed SBD sessions. + * @param successful_sessions Number of successful SBD sessions. + * @param signal_quality Signal quality equal to the number of bars displayed on the ISU signal strength indicator. Range is 0 to 5, where 0 indicates no signal and 5 indicates maximum signal strength. + * @param ring_pending 1: Ring call pending, 0: No call pending. + * @param tx_session_pending 1: Transmission session pending, 0: No transmission session pending. + * @param rx_session_pending 1: Receiving session pending, 0: No receiving session pending. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_isbd_link_status_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint64_t timestamp, uint64_t last_heartbeat, uint16_t failed_sessions, uint16_t successful_sessions, uint8_t signal_quality, uint8_t ring_pending, uint8_t tx_session_pending, uint8_t rx_session_pending) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_ISBD_LINK_STATUS_LEN]; + _mav_put_uint64_t(buf, 0, timestamp); + _mav_put_uint64_t(buf, 8, last_heartbeat); + _mav_put_uint16_t(buf, 16, failed_sessions); + _mav_put_uint16_t(buf, 18, successful_sessions); + _mav_put_uint8_t(buf, 20, signal_quality); + _mav_put_uint8_t(buf, 21, ring_pending); + _mav_put_uint8_t(buf, 22, tx_session_pending); + _mav_put_uint8_t(buf, 23, rx_session_pending); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_ISBD_LINK_STATUS_LEN); +#else + mavlink_isbd_link_status_t packet; + packet.timestamp = timestamp; + packet.last_heartbeat = last_heartbeat; + packet.failed_sessions = failed_sessions; + packet.successful_sessions = successful_sessions; + packet.signal_quality = signal_quality; + packet.ring_pending = ring_pending; + packet.tx_session_pending = tx_session_pending; + packet.rx_session_pending = rx_session_pending; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_ISBD_LINK_STATUS_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_ISBD_LINK_STATUS; + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_ISBD_LINK_STATUS_MIN_LEN, MAVLINK_MSG_ID_ISBD_LINK_STATUS_LEN, MAVLINK_MSG_ID_ISBD_LINK_STATUS_CRC); +} + +/** + * @brief Pack a isbd_link_status message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param timestamp [us] Timestamp (UNIX Epoch time or time since system boot). The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude of the number. + * @param last_heartbeat [us] Timestamp of the last successful sbd session. The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude of the number. + * @param failed_sessions Number of failed SBD sessions. + * @param successful_sessions Number of successful SBD sessions. + * @param signal_quality Signal quality equal to the number of bars displayed on the ISU signal strength indicator. Range is 0 to 5, where 0 indicates no signal and 5 indicates maximum signal strength. + * @param ring_pending 1: Ring call pending, 0: No call pending. + * @param tx_session_pending 1: Transmission session pending, 0: No transmission session pending. + * @param rx_session_pending 1: Receiving session pending, 0: No receiving session pending. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_isbd_link_status_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint64_t timestamp,uint64_t last_heartbeat,uint16_t failed_sessions,uint16_t successful_sessions,uint8_t signal_quality,uint8_t ring_pending,uint8_t tx_session_pending,uint8_t rx_session_pending) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_ISBD_LINK_STATUS_LEN]; + _mav_put_uint64_t(buf, 0, timestamp); + _mav_put_uint64_t(buf, 8, last_heartbeat); + _mav_put_uint16_t(buf, 16, failed_sessions); + _mav_put_uint16_t(buf, 18, successful_sessions); + _mav_put_uint8_t(buf, 20, signal_quality); + _mav_put_uint8_t(buf, 21, ring_pending); + _mav_put_uint8_t(buf, 22, tx_session_pending); + _mav_put_uint8_t(buf, 23, rx_session_pending); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_ISBD_LINK_STATUS_LEN); +#else + mavlink_isbd_link_status_t packet; + packet.timestamp = timestamp; + packet.last_heartbeat = last_heartbeat; + packet.failed_sessions = failed_sessions; + packet.successful_sessions = successful_sessions; + packet.signal_quality = signal_quality; + packet.ring_pending = ring_pending; + packet.tx_session_pending = tx_session_pending; + packet.rx_session_pending = rx_session_pending; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_ISBD_LINK_STATUS_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_ISBD_LINK_STATUS; + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_ISBD_LINK_STATUS_MIN_LEN, MAVLINK_MSG_ID_ISBD_LINK_STATUS_LEN, MAVLINK_MSG_ID_ISBD_LINK_STATUS_CRC); +} + +/** + * @brief Encode a isbd_link_status struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param isbd_link_status C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_isbd_link_status_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_isbd_link_status_t* isbd_link_status) +{ + return mavlink_msg_isbd_link_status_pack(system_id, component_id, msg, isbd_link_status->timestamp, isbd_link_status->last_heartbeat, isbd_link_status->failed_sessions, isbd_link_status->successful_sessions, isbd_link_status->signal_quality, isbd_link_status->ring_pending, isbd_link_status->tx_session_pending, isbd_link_status->rx_session_pending); +} + +/** + * @brief Encode a isbd_link_status struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param isbd_link_status C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_isbd_link_status_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_isbd_link_status_t* isbd_link_status) +{ + return mavlink_msg_isbd_link_status_pack_chan(system_id, component_id, chan, msg, isbd_link_status->timestamp, isbd_link_status->last_heartbeat, isbd_link_status->failed_sessions, isbd_link_status->successful_sessions, isbd_link_status->signal_quality, isbd_link_status->ring_pending, isbd_link_status->tx_session_pending, isbd_link_status->rx_session_pending); +} + +/** + * @brief Send a isbd_link_status message + * @param chan MAVLink channel to send the message + * + * @param timestamp [us] Timestamp (UNIX Epoch time or time since system boot). The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude of the number. + * @param last_heartbeat [us] Timestamp of the last successful sbd session. The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude of the number. + * @param failed_sessions Number of failed SBD sessions. + * @param successful_sessions Number of successful SBD sessions. + * @param signal_quality Signal quality equal to the number of bars displayed on the ISU signal strength indicator. Range is 0 to 5, where 0 indicates no signal and 5 indicates maximum signal strength. + * @param ring_pending 1: Ring call pending, 0: No call pending. + * @param tx_session_pending 1: Transmission session pending, 0: No transmission session pending. + * @param rx_session_pending 1: Receiving session pending, 0: No receiving session pending. + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_isbd_link_status_send(mavlink_channel_t chan, uint64_t timestamp, uint64_t last_heartbeat, uint16_t failed_sessions, uint16_t successful_sessions, uint8_t signal_quality, uint8_t ring_pending, uint8_t tx_session_pending, uint8_t rx_session_pending) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_ISBD_LINK_STATUS_LEN]; + _mav_put_uint64_t(buf, 0, timestamp); + _mav_put_uint64_t(buf, 8, last_heartbeat); + _mav_put_uint16_t(buf, 16, failed_sessions); + _mav_put_uint16_t(buf, 18, successful_sessions); + _mav_put_uint8_t(buf, 20, signal_quality); + _mav_put_uint8_t(buf, 21, ring_pending); + _mav_put_uint8_t(buf, 22, tx_session_pending); + _mav_put_uint8_t(buf, 23, rx_session_pending); + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ISBD_LINK_STATUS, buf, MAVLINK_MSG_ID_ISBD_LINK_STATUS_MIN_LEN, MAVLINK_MSG_ID_ISBD_LINK_STATUS_LEN, MAVLINK_MSG_ID_ISBD_LINK_STATUS_CRC); +#else + mavlink_isbd_link_status_t packet; + packet.timestamp = timestamp; + packet.last_heartbeat = last_heartbeat; + packet.failed_sessions = failed_sessions; + packet.successful_sessions = successful_sessions; + packet.signal_quality = signal_quality; + packet.ring_pending = ring_pending; + packet.tx_session_pending = tx_session_pending; + packet.rx_session_pending = rx_session_pending; + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ISBD_LINK_STATUS, (const char *)&packet, MAVLINK_MSG_ID_ISBD_LINK_STATUS_MIN_LEN, MAVLINK_MSG_ID_ISBD_LINK_STATUS_LEN, MAVLINK_MSG_ID_ISBD_LINK_STATUS_CRC); +#endif +} + +/** + * @brief Send a isbd_link_status message + * @param chan MAVLink channel to send the message + * @param struct The MAVLink struct to serialize + */ +static inline void mavlink_msg_isbd_link_status_send_struct(mavlink_channel_t chan, const mavlink_isbd_link_status_t* isbd_link_status) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + mavlink_msg_isbd_link_status_send(chan, isbd_link_status->timestamp, isbd_link_status->last_heartbeat, isbd_link_status->failed_sessions, isbd_link_status->successful_sessions, isbd_link_status->signal_quality, isbd_link_status->ring_pending, isbd_link_status->tx_session_pending, isbd_link_status->rx_session_pending); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ISBD_LINK_STATUS, (const char *)isbd_link_status, MAVLINK_MSG_ID_ISBD_LINK_STATUS_MIN_LEN, MAVLINK_MSG_ID_ISBD_LINK_STATUS_LEN, MAVLINK_MSG_ID_ISBD_LINK_STATUS_CRC); +#endif +} + +#if MAVLINK_MSG_ID_ISBD_LINK_STATUS_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_isbd_link_status_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint64_t timestamp, uint64_t last_heartbeat, uint16_t failed_sessions, uint16_t successful_sessions, uint8_t signal_quality, uint8_t ring_pending, uint8_t tx_session_pending, uint8_t rx_session_pending) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint64_t(buf, 0, timestamp); + _mav_put_uint64_t(buf, 8, last_heartbeat); + _mav_put_uint16_t(buf, 16, failed_sessions); + _mav_put_uint16_t(buf, 18, successful_sessions); + _mav_put_uint8_t(buf, 20, signal_quality); + _mav_put_uint8_t(buf, 21, ring_pending); + _mav_put_uint8_t(buf, 22, tx_session_pending); + _mav_put_uint8_t(buf, 23, rx_session_pending); + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ISBD_LINK_STATUS, buf, MAVLINK_MSG_ID_ISBD_LINK_STATUS_MIN_LEN, MAVLINK_MSG_ID_ISBD_LINK_STATUS_LEN, MAVLINK_MSG_ID_ISBD_LINK_STATUS_CRC); +#else + mavlink_isbd_link_status_t *packet = (mavlink_isbd_link_status_t *)msgbuf; + packet->timestamp = timestamp; + packet->last_heartbeat = last_heartbeat; + packet->failed_sessions = failed_sessions; + packet->successful_sessions = successful_sessions; + packet->signal_quality = signal_quality; + packet->ring_pending = ring_pending; + packet->tx_session_pending = tx_session_pending; + packet->rx_session_pending = rx_session_pending; + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ISBD_LINK_STATUS, (const char *)packet, MAVLINK_MSG_ID_ISBD_LINK_STATUS_MIN_LEN, MAVLINK_MSG_ID_ISBD_LINK_STATUS_LEN, MAVLINK_MSG_ID_ISBD_LINK_STATUS_CRC); +#endif +} +#endif + +#endif + +// MESSAGE ISBD_LINK_STATUS UNPACKING + + +/** + * @brief Get field timestamp from isbd_link_status message + * + * @return [us] Timestamp (UNIX Epoch time or time since system boot). The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude of the number. + */ +static inline uint64_t mavlink_msg_isbd_link_status_get_timestamp(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint64_t(msg, 0); +} + +/** + * @brief Get field last_heartbeat from isbd_link_status message + * + * @return [us] Timestamp of the last successful sbd session. The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude of the number. + */ +static inline uint64_t mavlink_msg_isbd_link_status_get_last_heartbeat(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint64_t(msg, 8); +} + +/** + * @brief Get field failed_sessions from isbd_link_status message + * + * @return Number of failed SBD sessions. + */ +static inline uint16_t mavlink_msg_isbd_link_status_get_failed_sessions(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 16); +} + +/** + * @brief Get field successful_sessions from isbd_link_status message + * + * @return Number of successful SBD sessions. + */ +static inline uint16_t mavlink_msg_isbd_link_status_get_successful_sessions(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 18); +} + +/** + * @brief Get field signal_quality from isbd_link_status message + * + * @return Signal quality equal to the number of bars displayed on the ISU signal strength indicator. Range is 0 to 5, where 0 indicates no signal and 5 indicates maximum signal strength. + */ +static inline uint8_t mavlink_msg_isbd_link_status_get_signal_quality(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 20); +} + +/** + * @brief Get field ring_pending from isbd_link_status message + * + * @return 1: Ring call pending, 0: No call pending. + */ +static inline uint8_t mavlink_msg_isbd_link_status_get_ring_pending(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 21); +} + +/** + * @brief Get field tx_session_pending from isbd_link_status message + * + * @return 1: Transmission session pending, 0: No transmission session pending. + */ +static inline uint8_t mavlink_msg_isbd_link_status_get_tx_session_pending(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 22); +} + +/** + * @brief Get field rx_session_pending from isbd_link_status message + * + * @return 1: Receiving session pending, 0: No receiving session pending. + */ +static inline uint8_t mavlink_msg_isbd_link_status_get_rx_session_pending(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 23); +} + +/** + * @brief Decode a isbd_link_status message into a struct + * + * @param msg The message to decode + * @param isbd_link_status C-struct to decode the message contents into + */ +static inline void mavlink_msg_isbd_link_status_decode(const mavlink_message_t* msg, mavlink_isbd_link_status_t* isbd_link_status) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + isbd_link_status->timestamp = mavlink_msg_isbd_link_status_get_timestamp(msg); + isbd_link_status->last_heartbeat = mavlink_msg_isbd_link_status_get_last_heartbeat(msg); + isbd_link_status->failed_sessions = mavlink_msg_isbd_link_status_get_failed_sessions(msg); + isbd_link_status->successful_sessions = mavlink_msg_isbd_link_status_get_successful_sessions(msg); + isbd_link_status->signal_quality = mavlink_msg_isbd_link_status_get_signal_quality(msg); + isbd_link_status->ring_pending = mavlink_msg_isbd_link_status_get_ring_pending(msg); + isbd_link_status->tx_session_pending = mavlink_msg_isbd_link_status_get_tx_session_pending(msg); + isbd_link_status->rx_session_pending = mavlink_msg_isbd_link_status_get_rx_session_pending(msg); +#else + uint8_t len = msg->len < MAVLINK_MSG_ID_ISBD_LINK_STATUS_LEN? msg->len : MAVLINK_MSG_ID_ISBD_LINK_STATUS_LEN; + memset(isbd_link_status, 0, MAVLINK_MSG_ID_ISBD_LINK_STATUS_LEN); + memcpy(isbd_link_status, _MAV_PAYLOAD(msg), len); +#endif +} diff --git a/lib/main/MAVLink/common/mavlink_msg_landing_target.h b/lib/main/MAVLink/common/mavlink_msg_landing_target.h index b591cb3cfe..1d1b41ed8e 100755 --- a/lib/main/MAVLink/common/mavlink_msg_landing_target.h +++ b/lib/main/MAVLink/common/mavlink_msg_landing_target.h @@ -3,7 +3,7 @@ #define MAVLINK_MSG_ID_LANDING_TARGET 149 - +MAVPACKED( typedef struct __mavlink_landing_target_t { uint64_t time_usec; /*< [us] Timestamp (UNIX Epoch time or time since system boot). The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude of the number.*/ float angle_x; /*< [rad] X-axis angular offset of the target from the center of the image*/ @@ -13,23 +13,29 @@ typedef struct __mavlink_landing_target_t { float size_y; /*< [rad] Size of target along y-axis*/ uint8_t target_num; /*< The ID of the target if multiple targets are present*/ uint8_t frame; /*< Coordinate frame used for following fields.*/ -} mavlink_landing_target_t; + float x; /*< [m] X Position of the landing target in MAV_FRAME*/ + float y; /*< [m] Y Position of the landing target in MAV_FRAME*/ + float z; /*< [m] Z Position of the landing target in MAV_FRAME*/ + float q[4]; /*< Quaternion of landing target orientation (w, x, y, z order, zero-rotation is 1, 0, 0, 0)*/ + uint8_t type; /*< Type of landing target*/ + uint8_t position_valid; /*< Boolean indicating whether the position fields (x, y, z, q, type) contain valid target position information (valid: 1, invalid: 0). Default is 0 (invalid).*/ +}) mavlink_landing_target_t; -#define MAVLINK_MSG_ID_LANDING_TARGET_LEN 30 +#define MAVLINK_MSG_ID_LANDING_TARGET_LEN 60 #define MAVLINK_MSG_ID_LANDING_TARGET_MIN_LEN 30 -#define MAVLINK_MSG_ID_149_LEN 30 +#define MAVLINK_MSG_ID_149_LEN 60 #define MAVLINK_MSG_ID_149_MIN_LEN 30 #define MAVLINK_MSG_ID_LANDING_TARGET_CRC 200 #define MAVLINK_MSG_ID_149_CRC 200 - +#define MAVLINK_MSG_LANDING_TARGET_FIELD_Q_LEN 4 #if MAVLINK_COMMAND_24BIT #define MAVLINK_MESSAGE_INFO_LANDING_TARGET { \ 149, \ "LANDING_TARGET", \ - 8, \ + 14, \ { { "time_usec", NULL, MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_landing_target_t, time_usec) }, \ { "target_num", NULL, MAVLINK_TYPE_UINT8_T, 0, 28, offsetof(mavlink_landing_target_t, target_num) }, \ { "frame", NULL, MAVLINK_TYPE_UINT8_T, 0, 29, offsetof(mavlink_landing_target_t, frame) }, \ @@ -38,12 +44,18 @@ typedef struct __mavlink_landing_target_t { { "distance", NULL, MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_landing_target_t, distance) }, \ { "size_x", NULL, MAVLINK_TYPE_FLOAT, 0, 20, offsetof(mavlink_landing_target_t, size_x) }, \ { "size_y", NULL, MAVLINK_TYPE_FLOAT, 0, 24, offsetof(mavlink_landing_target_t, size_y) }, \ + { "x", NULL, MAVLINK_TYPE_FLOAT, 0, 30, offsetof(mavlink_landing_target_t, x) }, \ + { "y", NULL, MAVLINK_TYPE_FLOAT, 0, 34, offsetof(mavlink_landing_target_t, y) }, \ + { "z", NULL, MAVLINK_TYPE_FLOAT, 0, 38, offsetof(mavlink_landing_target_t, z) }, \ + { "q", NULL, MAVLINK_TYPE_FLOAT, 4, 42, offsetof(mavlink_landing_target_t, q) }, \ + { "type", NULL, MAVLINK_TYPE_UINT8_T, 0, 58, offsetof(mavlink_landing_target_t, type) }, \ + { "position_valid", NULL, MAVLINK_TYPE_UINT8_T, 0, 59, offsetof(mavlink_landing_target_t, position_valid) }, \ } \ } #else #define MAVLINK_MESSAGE_INFO_LANDING_TARGET { \ "LANDING_TARGET", \ - 8, \ + 14, \ { { "time_usec", NULL, MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_landing_target_t, time_usec) }, \ { "target_num", NULL, MAVLINK_TYPE_UINT8_T, 0, 28, offsetof(mavlink_landing_target_t, target_num) }, \ { "frame", NULL, MAVLINK_TYPE_UINT8_T, 0, 29, offsetof(mavlink_landing_target_t, frame) }, \ @@ -52,6 +64,12 @@ typedef struct __mavlink_landing_target_t { { "distance", NULL, MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_landing_target_t, distance) }, \ { "size_x", NULL, MAVLINK_TYPE_FLOAT, 0, 20, offsetof(mavlink_landing_target_t, size_x) }, \ { "size_y", NULL, MAVLINK_TYPE_FLOAT, 0, 24, offsetof(mavlink_landing_target_t, size_y) }, \ + { "x", NULL, MAVLINK_TYPE_FLOAT, 0, 30, offsetof(mavlink_landing_target_t, x) }, \ + { "y", NULL, MAVLINK_TYPE_FLOAT, 0, 34, offsetof(mavlink_landing_target_t, y) }, \ + { "z", NULL, MAVLINK_TYPE_FLOAT, 0, 38, offsetof(mavlink_landing_target_t, z) }, \ + { "q", NULL, MAVLINK_TYPE_FLOAT, 4, 42, offsetof(mavlink_landing_target_t, q) }, \ + { "type", NULL, MAVLINK_TYPE_UINT8_T, 0, 58, offsetof(mavlink_landing_target_t, type) }, \ + { "position_valid", NULL, MAVLINK_TYPE_UINT8_T, 0, 59, offsetof(mavlink_landing_target_t, position_valid) }, \ } \ } #endif @@ -70,10 +88,16 @@ typedef struct __mavlink_landing_target_t { * @param distance [m] Distance to the target from the vehicle * @param size_x [rad] Size of target along x-axis * @param size_y [rad] Size of target along y-axis + * @param x [m] X Position of the landing target in MAV_FRAME + * @param y [m] Y Position of the landing target in MAV_FRAME + * @param z [m] Z Position of the landing target in MAV_FRAME + * @param q Quaternion of landing target orientation (w, x, y, z order, zero-rotation is 1, 0, 0, 0) + * @param type Type of landing target + * @param position_valid Boolean indicating whether the position fields (x, y, z, q, type) contain valid target position information (valid: 1, invalid: 0). Default is 0 (invalid). * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_landing_target_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint64_t time_usec, uint8_t target_num, uint8_t frame, float angle_x, float angle_y, float distance, float size_x, float size_y) + uint64_t time_usec, uint8_t target_num, uint8_t frame, float angle_x, float angle_y, float distance, float size_x, float size_y, float x, float y, float z, const float *q, uint8_t type, uint8_t position_valid) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_LANDING_TARGET_LEN]; @@ -85,7 +109,12 @@ static inline uint16_t mavlink_msg_landing_target_pack(uint8_t system_id, uint8_ _mav_put_float(buf, 24, size_y); _mav_put_uint8_t(buf, 28, target_num); _mav_put_uint8_t(buf, 29, frame); - + _mav_put_float(buf, 30, x); + _mav_put_float(buf, 34, y); + _mav_put_float(buf, 38, z); + _mav_put_uint8_t(buf, 58, type); + _mav_put_uint8_t(buf, 59, position_valid); + _mav_put_float_array(buf, 42, q, 4); memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_LANDING_TARGET_LEN); #else mavlink_landing_target_t packet; @@ -97,7 +126,12 @@ static inline uint16_t mavlink_msg_landing_target_pack(uint8_t system_id, uint8_ packet.size_y = size_y; packet.target_num = target_num; packet.frame = frame; - + packet.x = x; + packet.y = y; + packet.z = z; + packet.type = type; + packet.position_valid = position_valid; + mav_array_memcpy(packet.q, q, sizeof(float)*4); memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_LANDING_TARGET_LEN); #endif @@ -119,11 +153,17 @@ static inline uint16_t mavlink_msg_landing_target_pack(uint8_t system_id, uint8_ * @param distance [m] Distance to the target from the vehicle * @param size_x [rad] Size of target along x-axis * @param size_y [rad] Size of target along y-axis + * @param x [m] X Position of the landing target in MAV_FRAME + * @param y [m] Y Position of the landing target in MAV_FRAME + * @param z [m] Z Position of the landing target in MAV_FRAME + * @param q Quaternion of landing target orientation (w, x, y, z order, zero-rotation is 1, 0, 0, 0) + * @param type Type of landing target + * @param position_valid Boolean indicating whether the position fields (x, y, z, q, type) contain valid target position information (valid: 1, invalid: 0). Default is 0 (invalid). * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_landing_target_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, - uint64_t time_usec,uint8_t target_num,uint8_t frame,float angle_x,float angle_y,float distance,float size_x,float size_y) + uint64_t time_usec,uint8_t target_num,uint8_t frame,float angle_x,float angle_y,float distance,float size_x,float size_y,float x,float y,float z,const float *q,uint8_t type,uint8_t position_valid) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_LANDING_TARGET_LEN]; @@ -135,7 +175,12 @@ static inline uint16_t mavlink_msg_landing_target_pack_chan(uint8_t system_id, u _mav_put_float(buf, 24, size_y); _mav_put_uint8_t(buf, 28, target_num); _mav_put_uint8_t(buf, 29, frame); - + _mav_put_float(buf, 30, x); + _mav_put_float(buf, 34, y); + _mav_put_float(buf, 38, z); + _mav_put_uint8_t(buf, 58, type); + _mav_put_uint8_t(buf, 59, position_valid); + _mav_put_float_array(buf, 42, q, 4); memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_LANDING_TARGET_LEN); #else mavlink_landing_target_t packet; @@ -147,7 +192,12 @@ static inline uint16_t mavlink_msg_landing_target_pack_chan(uint8_t system_id, u packet.size_y = size_y; packet.target_num = target_num; packet.frame = frame; - + packet.x = x; + packet.y = y; + packet.z = z; + packet.type = type; + packet.position_valid = position_valid; + mav_array_memcpy(packet.q, q, sizeof(float)*4); memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_LANDING_TARGET_LEN); #endif @@ -165,7 +215,7 @@ static inline uint16_t mavlink_msg_landing_target_pack_chan(uint8_t system_id, u */ static inline uint16_t mavlink_msg_landing_target_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_landing_target_t* landing_target) { - return mavlink_msg_landing_target_pack(system_id, component_id, msg, landing_target->time_usec, landing_target->target_num, landing_target->frame, landing_target->angle_x, landing_target->angle_y, landing_target->distance, landing_target->size_x, landing_target->size_y); + return mavlink_msg_landing_target_pack(system_id, component_id, msg, landing_target->time_usec, landing_target->target_num, landing_target->frame, landing_target->angle_x, landing_target->angle_y, landing_target->distance, landing_target->size_x, landing_target->size_y, landing_target->x, landing_target->y, landing_target->z, landing_target->q, landing_target->type, landing_target->position_valid); } /** @@ -179,7 +229,7 @@ static inline uint16_t mavlink_msg_landing_target_encode(uint8_t system_id, uint */ static inline uint16_t mavlink_msg_landing_target_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_landing_target_t* landing_target) { - return mavlink_msg_landing_target_pack_chan(system_id, component_id, chan, msg, landing_target->time_usec, landing_target->target_num, landing_target->frame, landing_target->angle_x, landing_target->angle_y, landing_target->distance, landing_target->size_x, landing_target->size_y); + return mavlink_msg_landing_target_pack_chan(system_id, component_id, chan, msg, landing_target->time_usec, landing_target->target_num, landing_target->frame, landing_target->angle_x, landing_target->angle_y, landing_target->distance, landing_target->size_x, landing_target->size_y, landing_target->x, landing_target->y, landing_target->z, landing_target->q, landing_target->type, landing_target->position_valid); } /** @@ -194,10 +244,16 @@ static inline uint16_t mavlink_msg_landing_target_encode_chan(uint8_t system_id, * @param distance [m] Distance to the target from the vehicle * @param size_x [rad] Size of target along x-axis * @param size_y [rad] Size of target along y-axis + * @param x [m] X Position of the landing target in MAV_FRAME + * @param y [m] Y Position of the landing target in MAV_FRAME + * @param z [m] Z Position of the landing target in MAV_FRAME + * @param q Quaternion of landing target orientation (w, x, y, z order, zero-rotation is 1, 0, 0, 0) + * @param type Type of landing target + * @param position_valid Boolean indicating whether the position fields (x, y, z, q, type) contain valid target position information (valid: 1, invalid: 0). Default is 0 (invalid). */ #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS -static inline void mavlink_msg_landing_target_send(mavlink_channel_t chan, uint64_t time_usec, uint8_t target_num, uint8_t frame, float angle_x, float angle_y, float distance, float size_x, float size_y) +static inline void mavlink_msg_landing_target_send(mavlink_channel_t chan, uint64_t time_usec, uint8_t target_num, uint8_t frame, float angle_x, float angle_y, float distance, float size_x, float size_y, float x, float y, float z, const float *q, uint8_t type, uint8_t position_valid) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_LANDING_TARGET_LEN]; @@ -209,7 +265,12 @@ static inline void mavlink_msg_landing_target_send(mavlink_channel_t chan, uint6 _mav_put_float(buf, 24, size_y); _mav_put_uint8_t(buf, 28, target_num); _mav_put_uint8_t(buf, 29, frame); - + _mav_put_float(buf, 30, x); + _mav_put_float(buf, 34, y); + _mav_put_float(buf, 38, z); + _mav_put_uint8_t(buf, 58, type); + _mav_put_uint8_t(buf, 59, position_valid); + _mav_put_float_array(buf, 42, q, 4); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LANDING_TARGET, buf, MAVLINK_MSG_ID_LANDING_TARGET_MIN_LEN, MAVLINK_MSG_ID_LANDING_TARGET_LEN, MAVLINK_MSG_ID_LANDING_TARGET_CRC); #else mavlink_landing_target_t packet; @@ -221,7 +282,12 @@ static inline void mavlink_msg_landing_target_send(mavlink_channel_t chan, uint6 packet.size_y = size_y; packet.target_num = target_num; packet.frame = frame; - + packet.x = x; + packet.y = y; + packet.z = z; + packet.type = type; + packet.position_valid = position_valid; + mav_array_memcpy(packet.q, q, sizeof(float)*4); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LANDING_TARGET, (const char *)&packet, MAVLINK_MSG_ID_LANDING_TARGET_MIN_LEN, MAVLINK_MSG_ID_LANDING_TARGET_LEN, MAVLINK_MSG_ID_LANDING_TARGET_CRC); #endif } @@ -234,7 +300,7 @@ static inline void mavlink_msg_landing_target_send(mavlink_channel_t chan, uint6 static inline void mavlink_msg_landing_target_send_struct(mavlink_channel_t chan, const mavlink_landing_target_t* landing_target) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - mavlink_msg_landing_target_send(chan, landing_target->time_usec, landing_target->target_num, landing_target->frame, landing_target->angle_x, landing_target->angle_y, landing_target->distance, landing_target->size_x, landing_target->size_y); + mavlink_msg_landing_target_send(chan, landing_target->time_usec, landing_target->target_num, landing_target->frame, landing_target->angle_x, landing_target->angle_y, landing_target->distance, landing_target->size_x, landing_target->size_y, landing_target->x, landing_target->y, landing_target->z, landing_target->q, landing_target->type, landing_target->position_valid); #else _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LANDING_TARGET, (const char *)landing_target, MAVLINK_MSG_ID_LANDING_TARGET_MIN_LEN, MAVLINK_MSG_ID_LANDING_TARGET_LEN, MAVLINK_MSG_ID_LANDING_TARGET_CRC); #endif @@ -248,7 +314,7 @@ static inline void mavlink_msg_landing_target_send_struct(mavlink_channel_t chan is usually the receive buffer for the channel, and allows a reply to an incoming message with minimum stack space usage. */ -static inline void mavlink_msg_landing_target_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint64_t time_usec, uint8_t target_num, uint8_t frame, float angle_x, float angle_y, float distance, float size_x, float size_y) +static inline void mavlink_msg_landing_target_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint64_t time_usec, uint8_t target_num, uint8_t frame, float angle_x, float angle_y, float distance, float size_x, float size_y, float x, float y, float z, const float *q, uint8_t type, uint8_t position_valid) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char *buf = (char *)msgbuf; @@ -260,7 +326,12 @@ static inline void mavlink_msg_landing_target_send_buf(mavlink_message_t *msgbuf _mav_put_float(buf, 24, size_y); _mav_put_uint8_t(buf, 28, target_num); _mav_put_uint8_t(buf, 29, frame); - + _mav_put_float(buf, 30, x); + _mav_put_float(buf, 34, y); + _mav_put_float(buf, 38, z); + _mav_put_uint8_t(buf, 58, type); + _mav_put_uint8_t(buf, 59, position_valid); + _mav_put_float_array(buf, 42, q, 4); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LANDING_TARGET, buf, MAVLINK_MSG_ID_LANDING_TARGET_MIN_LEN, MAVLINK_MSG_ID_LANDING_TARGET_LEN, MAVLINK_MSG_ID_LANDING_TARGET_CRC); #else mavlink_landing_target_t *packet = (mavlink_landing_target_t *)msgbuf; @@ -272,7 +343,12 @@ static inline void mavlink_msg_landing_target_send_buf(mavlink_message_t *msgbuf packet->size_y = size_y; packet->target_num = target_num; packet->frame = frame; - + packet->x = x; + packet->y = y; + packet->z = z; + packet->type = type; + packet->position_valid = position_valid; + mav_array_memcpy(packet->q, q, sizeof(float)*4); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LANDING_TARGET, (const char *)packet, MAVLINK_MSG_ID_LANDING_TARGET_MIN_LEN, MAVLINK_MSG_ID_LANDING_TARGET_LEN, MAVLINK_MSG_ID_LANDING_TARGET_CRC); #endif } @@ -363,6 +439,66 @@ static inline float mavlink_msg_landing_target_get_size_y(const mavlink_message_ return _MAV_RETURN_float(msg, 24); } +/** + * @brief Get field x from landing_target message + * + * @return [m] X Position of the landing target in MAV_FRAME + */ +static inline float mavlink_msg_landing_target_get_x(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 30); +} + +/** + * @brief Get field y from landing_target message + * + * @return [m] Y Position of the landing target in MAV_FRAME + */ +static inline float mavlink_msg_landing_target_get_y(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 34); +} + +/** + * @brief Get field z from landing_target message + * + * @return [m] Z Position of the landing target in MAV_FRAME + */ +static inline float mavlink_msg_landing_target_get_z(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 38); +} + +/** + * @brief Get field q from landing_target message + * + * @return Quaternion of landing target orientation (w, x, y, z order, zero-rotation is 1, 0, 0, 0) + */ +static inline uint16_t mavlink_msg_landing_target_get_q(const mavlink_message_t* msg, float *q) +{ + return _MAV_RETURN_float_array(msg, q, 4, 42); +} + +/** + * @brief Get field type from landing_target message + * + * @return Type of landing target + */ +static inline uint8_t mavlink_msg_landing_target_get_type(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 58); +} + +/** + * @brief Get field position_valid from landing_target message + * + * @return Boolean indicating whether the position fields (x, y, z, q, type) contain valid target position information (valid: 1, invalid: 0). Default is 0 (invalid). + */ +static inline uint8_t mavlink_msg_landing_target_get_position_valid(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 59); +} + /** * @brief Decode a landing_target message into a struct * @@ -380,6 +516,12 @@ static inline void mavlink_msg_landing_target_decode(const mavlink_message_t* ms landing_target->size_y = mavlink_msg_landing_target_get_size_y(msg); landing_target->target_num = mavlink_msg_landing_target_get_target_num(msg); landing_target->frame = mavlink_msg_landing_target_get_frame(msg); + landing_target->x = mavlink_msg_landing_target_get_x(msg); + landing_target->y = mavlink_msg_landing_target_get_y(msg); + landing_target->z = mavlink_msg_landing_target_get_z(msg); + mavlink_msg_landing_target_get_q(msg, landing_target->q); + landing_target->type = mavlink_msg_landing_target_get_type(msg); + landing_target->position_valid = mavlink_msg_landing_target_get_position_valid(msg); #else uint8_t len = msg->len < MAVLINK_MSG_ID_LANDING_TARGET_LEN? msg->len : MAVLINK_MSG_ID_LANDING_TARGET_LEN; memset(landing_target, 0, MAVLINK_MSG_ID_LANDING_TARGET_LEN); diff --git a/lib/main/MAVLink/common/mavlink_msg_logging_ack.h b/lib/main/MAVLink/common/mavlink_msg_logging_ack.h new file mode 100644 index 0000000000..56dab387de --- /dev/null +++ b/lib/main/MAVLink/common/mavlink_msg_logging_ack.h @@ -0,0 +1,263 @@ +#pragma once +// MESSAGE LOGGING_ACK PACKING + +#define MAVLINK_MSG_ID_LOGGING_ACK 268 + + +typedef struct __mavlink_logging_ack_t { + uint16_t sequence; /*< sequence number (must match the one in LOGGING_DATA_ACKED)*/ + uint8_t target_system; /*< system ID of the target*/ + uint8_t target_component; /*< component ID of the target*/ +} mavlink_logging_ack_t; + +#define MAVLINK_MSG_ID_LOGGING_ACK_LEN 4 +#define MAVLINK_MSG_ID_LOGGING_ACK_MIN_LEN 4 +#define MAVLINK_MSG_ID_268_LEN 4 +#define MAVLINK_MSG_ID_268_MIN_LEN 4 + +#define MAVLINK_MSG_ID_LOGGING_ACK_CRC 14 +#define MAVLINK_MSG_ID_268_CRC 14 + + + +#if MAVLINK_COMMAND_24BIT +#define MAVLINK_MESSAGE_INFO_LOGGING_ACK { \ + 268, \ + "LOGGING_ACK", \ + 3, \ + { { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 2, offsetof(mavlink_logging_ack_t, target_system) }, \ + { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 3, offsetof(mavlink_logging_ack_t, target_component) }, \ + { "sequence", NULL, MAVLINK_TYPE_UINT16_T, 0, 0, offsetof(mavlink_logging_ack_t, sequence) }, \ + } \ +} +#else +#define MAVLINK_MESSAGE_INFO_LOGGING_ACK { \ + "LOGGING_ACK", \ + 3, \ + { { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 2, offsetof(mavlink_logging_ack_t, target_system) }, \ + { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 3, offsetof(mavlink_logging_ack_t, target_component) }, \ + { "sequence", NULL, MAVLINK_TYPE_UINT16_T, 0, 0, offsetof(mavlink_logging_ack_t, sequence) }, \ + } \ +} +#endif + +/** + * @brief Pack a logging_ack message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param target_system system ID of the target + * @param target_component component ID of the target + * @param sequence sequence number (must match the one in LOGGING_DATA_ACKED) + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_logging_ack_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint8_t target_system, uint8_t target_component, uint16_t sequence) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_LOGGING_ACK_LEN]; + _mav_put_uint16_t(buf, 0, sequence); + _mav_put_uint8_t(buf, 2, target_system); + _mav_put_uint8_t(buf, 3, target_component); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_LOGGING_ACK_LEN); +#else + mavlink_logging_ack_t packet; + packet.sequence = sequence; + packet.target_system = target_system; + packet.target_component = target_component; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_LOGGING_ACK_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_LOGGING_ACK; + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_LOGGING_ACK_MIN_LEN, MAVLINK_MSG_ID_LOGGING_ACK_LEN, MAVLINK_MSG_ID_LOGGING_ACK_CRC); +} + +/** + * @brief Pack a logging_ack message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param target_system system ID of the target + * @param target_component component ID of the target + * @param sequence sequence number (must match the one in LOGGING_DATA_ACKED) + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_logging_ack_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint8_t target_system,uint8_t target_component,uint16_t sequence) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_LOGGING_ACK_LEN]; + _mav_put_uint16_t(buf, 0, sequence); + _mav_put_uint8_t(buf, 2, target_system); + _mav_put_uint8_t(buf, 3, target_component); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_LOGGING_ACK_LEN); +#else + mavlink_logging_ack_t packet; + packet.sequence = sequence; + packet.target_system = target_system; + packet.target_component = target_component; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_LOGGING_ACK_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_LOGGING_ACK; + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_LOGGING_ACK_MIN_LEN, MAVLINK_MSG_ID_LOGGING_ACK_LEN, MAVLINK_MSG_ID_LOGGING_ACK_CRC); +} + +/** + * @brief Encode a logging_ack struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param logging_ack C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_logging_ack_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_logging_ack_t* logging_ack) +{ + return mavlink_msg_logging_ack_pack(system_id, component_id, msg, logging_ack->target_system, logging_ack->target_component, logging_ack->sequence); +} + +/** + * @brief Encode a logging_ack struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param logging_ack C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_logging_ack_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_logging_ack_t* logging_ack) +{ + return mavlink_msg_logging_ack_pack_chan(system_id, component_id, chan, msg, logging_ack->target_system, logging_ack->target_component, logging_ack->sequence); +} + +/** + * @brief Send a logging_ack message + * @param chan MAVLink channel to send the message + * + * @param target_system system ID of the target + * @param target_component component ID of the target + * @param sequence sequence number (must match the one in LOGGING_DATA_ACKED) + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_logging_ack_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint16_t sequence) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_LOGGING_ACK_LEN]; + _mav_put_uint16_t(buf, 0, sequence); + _mav_put_uint8_t(buf, 2, target_system); + _mav_put_uint8_t(buf, 3, target_component); + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOGGING_ACK, buf, MAVLINK_MSG_ID_LOGGING_ACK_MIN_LEN, MAVLINK_MSG_ID_LOGGING_ACK_LEN, MAVLINK_MSG_ID_LOGGING_ACK_CRC); +#else + mavlink_logging_ack_t packet; + packet.sequence = sequence; + packet.target_system = target_system; + packet.target_component = target_component; + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOGGING_ACK, (const char *)&packet, MAVLINK_MSG_ID_LOGGING_ACK_MIN_LEN, MAVLINK_MSG_ID_LOGGING_ACK_LEN, MAVLINK_MSG_ID_LOGGING_ACK_CRC); +#endif +} + +/** + * @brief Send a logging_ack message + * @param chan MAVLink channel to send the message + * @param struct The MAVLink struct to serialize + */ +static inline void mavlink_msg_logging_ack_send_struct(mavlink_channel_t chan, const mavlink_logging_ack_t* logging_ack) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + mavlink_msg_logging_ack_send(chan, logging_ack->target_system, logging_ack->target_component, logging_ack->sequence); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOGGING_ACK, (const char *)logging_ack, MAVLINK_MSG_ID_LOGGING_ACK_MIN_LEN, MAVLINK_MSG_ID_LOGGING_ACK_LEN, MAVLINK_MSG_ID_LOGGING_ACK_CRC); +#endif +} + +#if MAVLINK_MSG_ID_LOGGING_ACK_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_logging_ack_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint16_t sequence) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint16_t(buf, 0, sequence); + _mav_put_uint8_t(buf, 2, target_system); + _mav_put_uint8_t(buf, 3, target_component); + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOGGING_ACK, buf, MAVLINK_MSG_ID_LOGGING_ACK_MIN_LEN, MAVLINK_MSG_ID_LOGGING_ACK_LEN, MAVLINK_MSG_ID_LOGGING_ACK_CRC); +#else + mavlink_logging_ack_t *packet = (mavlink_logging_ack_t *)msgbuf; + packet->sequence = sequence; + packet->target_system = target_system; + packet->target_component = target_component; + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOGGING_ACK, (const char *)packet, MAVLINK_MSG_ID_LOGGING_ACK_MIN_LEN, MAVLINK_MSG_ID_LOGGING_ACK_LEN, MAVLINK_MSG_ID_LOGGING_ACK_CRC); +#endif +} +#endif + +#endif + +// MESSAGE LOGGING_ACK UNPACKING + + +/** + * @brief Get field target_system from logging_ack message + * + * @return system ID of the target + */ +static inline uint8_t mavlink_msg_logging_ack_get_target_system(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 2); +} + +/** + * @brief Get field target_component from logging_ack message + * + * @return component ID of the target + */ +static inline uint8_t mavlink_msg_logging_ack_get_target_component(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 3); +} + +/** + * @brief Get field sequence from logging_ack message + * + * @return sequence number (must match the one in LOGGING_DATA_ACKED) + */ +static inline uint16_t mavlink_msg_logging_ack_get_sequence(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 0); +} + +/** + * @brief Decode a logging_ack message into a struct + * + * @param msg The message to decode + * @param logging_ack C-struct to decode the message contents into + */ +static inline void mavlink_msg_logging_ack_decode(const mavlink_message_t* msg, mavlink_logging_ack_t* logging_ack) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + logging_ack->sequence = mavlink_msg_logging_ack_get_sequence(msg); + logging_ack->target_system = mavlink_msg_logging_ack_get_target_system(msg); + logging_ack->target_component = mavlink_msg_logging_ack_get_target_component(msg); +#else + uint8_t len = msg->len < MAVLINK_MSG_ID_LOGGING_ACK_LEN? msg->len : MAVLINK_MSG_ID_LOGGING_ACK_LEN; + memset(logging_ack, 0, MAVLINK_MSG_ID_LOGGING_ACK_LEN); + memcpy(logging_ack, _MAV_PAYLOAD(msg), len); +#endif +} diff --git a/lib/main/MAVLink/common/mavlink_msg_logging_data.h b/lib/main/MAVLink/common/mavlink_msg_logging_data.h new file mode 100644 index 0000000000..c9bf0559de --- /dev/null +++ b/lib/main/MAVLink/common/mavlink_msg_logging_data.h @@ -0,0 +1,330 @@ +#pragma once +// MESSAGE LOGGING_DATA PACKING + +#define MAVLINK_MSG_ID_LOGGING_DATA 266 + + +typedef struct __mavlink_logging_data_t { + uint16_t sequence; /*< sequence number (can wrap)*/ + uint8_t target_system; /*< system ID of the target*/ + uint8_t target_component; /*< component ID of the target*/ + uint8_t length; /*< [bytes] data length*/ + uint8_t first_message_offset; /*< [bytes] offset into data where first message starts. This can be used for recovery, when a previous message got lost (set to 255 if no start exists).*/ + uint8_t data[249]; /*< logged data*/ +} mavlink_logging_data_t; + +#define MAVLINK_MSG_ID_LOGGING_DATA_LEN 255 +#define MAVLINK_MSG_ID_LOGGING_DATA_MIN_LEN 255 +#define MAVLINK_MSG_ID_266_LEN 255 +#define MAVLINK_MSG_ID_266_MIN_LEN 255 + +#define MAVLINK_MSG_ID_LOGGING_DATA_CRC 193 +#define MAVLINK_MSG_ID_266_CRC 193 + +#define MAVLINK_MSG_LOGGING_DATA_FIELD_DATA_LEN 249 + +#if MAVLINK_COMMAND_24BIT +#define MAVLINK_MESSAGE_INFO_LOGGING_DATA { \ + 266, \ + "LOGGING_DATA", \ + 6, \ + { { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 2, offsetof(mavlink_logging_data_t, target_system) }, \ + { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 3, offsetof(mavlink_logging_data_t, target_component) }, \ + { "sequence", NULL, MAVLINK_TYPE_UINT16_T, 0, 0, offsetof(mavlink_logging_data_t, sequence) }, \ + { "length", NULL, MAVLINK_TYPE_UINT8_T, 0, 4, offsetof(mavlink_logging_data_t, length) }, \ + { "first_message_offset", NULL, MAVLINK_TYPE_UINT8_T, 0, 5, offsetof(mavlink_logging_data_t, first_message_offset) }, \ + { "data", NULL, MAVLINK_TYPE_UINT8_T, 249, 6, offsetof(mavlink_logging_data_t, data) }, \ + } \ +} +#else +#define MAVLINK_MESSAGE_INFO_LOGGING_DATA { \ + "LOGGING_DATA", \ + 6, \ + { { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 2, offsetof(mavlink_logging_data_t, target_system) }, \ + { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 3, offsetof(mavlink_logging_data_t, target_component) }, \ + { "sequence", NULL, MAVLINK_TYPE_UINT16_T, 0, 0, offsetof(mavlink_logging_data_t, sequence) }, \ + { "length", NULL, MAVLINK_TYPE_UINT8_T, 0, 4, offsetof(mavlink_logging_data_t, length) }, \ + { "first_message_offset", NULL, MAVLINK_TYPE_UINT8_T, 0, 5, offsetof(mavlink_logging_data_t, first_message_offset) }, \ + { "data", NULL, MAVLINK_TYPE_UINT8_T, 249, 6, offsetof(mavlink_logging_data_t, data) }, \ + } \ +} +#endif + +/** + * @brief Pack a logging_data message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param target_system system ID of the target + * @param target_component component ID of the target + * @param sequence sequence number (can wrap) + * @param length [bytes] data length + * @param first_message_offset [bytes] offset into data where first message starts. This can be used for recovery, when a previous message got lost (set to 255 if no start exists). + * @param data logged data + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_logging_data_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint8_t target_system, uint8_t target_component, uint16_t sequence, uint8_t length, uint8_t first_message_offset, const uint8_t *data) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_LOGGING_DATA_LEN]; + _mav_put_uint16_t(buf, 0, sequence); + _mav_put_uint8_t(buf, 2, target_system); + _mav_put_uint8_t(buf, 3, target_component); + _mav_put_uint8_t(buf, 4, length); + _mav_put_uint8_t(buf, 5, first_message_offset); + _mav_put_uint8_t_array(buf, 6, data, 249); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_LOGGING_DATA_LEN); +#else + mavlink_logging_data_t packet; + packet.sequence = sequence; + packet.target_system = target_system; + packet.target_component = target_component; + packet.length = length; + packet.first_message_offset = first_message_offset; + mav_array_memcpy(packet.data, data, sizeof(uint8_t)*249); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_LOGGING_DATA_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_LOGGING_DATA; + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_LOGGING_DATA_MIN_LEN, MAVLINK_MSG_ID_LOGGING_DATA_LEN, MAVLINK_MSG_ID_LOGGING_DATA_CRC); +} + +/** + * @brief Pack a logging_data message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param target_system system ID of the target + * @param target_component component ID of the target + * @param sequence sequence number (can wrap) + * @param length [bytes] data length + * @param first_message_offset [bytes] offset into data where first message starts. This can be used for recovery, when a previous message got lost (set to 255 if no start exists). + * @param data logged data + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_logging_data_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint8_t target_system,uint8_t target_component,uint16_t sequence,uint8_t length,uint8_t first_message_offset,const uint8_t *data) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_LOGGING_DATA_LEN]; + _mav_put_uint16_t(buf, 0, sequence); + _mav_put_uint8_t(buf, 2, target_system); + _mav_put_uint8_t(buf, 3, target_component); + _mav_put_uint8_t(buf, 4, length); + _mav_put_uint8_t(buf, 5, first_message_offset); + _mav_put_uint8_t_array(buf, 6, data, 249); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_LOGGING_DATA_LEN); +#else + mavlink_logging_data_t packet; + packet.sequence = sequence; + packet.target_system = target_system; + packet.target_component = target_component; + packet.length = length; + packet.first_message_offset = first_message_offset; + mav_array_memcpy(packet.data, data, sizeof(uint8_t)*249); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_LOGGING_DATA_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_LOGGING_DATA; + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_LOGGING_DATA_MIN_LEN, MAVLINK_MSG_ID_LOGGING_DATA_LEN, MAVLINK_MSG_ID_LOGGING_DATA_CRC); +} + +/** + * @brief Encode a logging_data struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param logging_data C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_logging_data_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_logging_data_t* logging_data) +{ + return mavlink_msg_logging_data_pack(system_id, component_id, msg, logging_data->target_system, logging_data->target_component, logging_data->sequence, logging_data->length, logging_data->first_message_offset, logging_data->data); +} + +/** + * @brief Encode a logging_data struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param logging_data C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_logging_data_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_logging_data_t* logging_data) +{ + return mavlink_msg_logging_data_pack_chan(system_id, component_id, chan, msg, logging_data->target_system, logging_data->target_component, logging_data->sequence, logging_data->length, logging_data->first_message_offset, logging_data->data); +} + +/** + * @brief Send a logging_data message + * @param chan MAVLink channel to send the message + * + * @param target_system system ID of the target + * @param target_component component ID of the target + * @param sequence sequence number (can wrap) + * @param length [bytes] data length + * @param first_message_offset [bytes] offset into data where first message starts. This can be used for recovery, when a previous message got lost (set to 255 if no start exists). + * @param data logged data + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_logging_data_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint16_t sequence, uint8_t length, uint8_t first_message_offset, const uint8_t *data) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_LOGGING_DATA_LEN]; + _mav_put_uint16_t(buf, 0, sequence); + _mav_put_uint8_t(buf, 2, target_system); + _mav_put_uint8_t(buf, 3, target_component); + _mav_put_uint8_t(buf, 4, length); + _mav_put_uint8_t(buf, 5, first_message_offset); + _mav_put_uint8_t_array(buf, 6, data, 249); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOGGING_DATA, buf, MAVLINK_MSG_ID_LOGGING_DATA_MIN_LEN, MAVLINK_MSG_ID_LOGGING_DATA_LEN, MAVLINK_MSG_ID_LOGGING_DATA_CRC); +#else + mavlink_logging_data_t packet; + packet.sequence = sequence; + packet.target_system = target_system; + packet.target_component = target_component; + packet.length = length; + packet.first_message_offset = first_message_offset; + mav_array_memcpy(packet.data, data, sizeof(uint8_t)*249); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOGGING_DATA, (const char *)&packet, MAVLINK_MSG_ID_LOGGING_DATA_MIN_LEN, MAVLINK_MSG_ID_LOGGING_DATA_LEN, MAVLINK_MSG_ID_LOGGING_DATA_CRC); +#endif +} + +/** + * @brief Send a logging_data message + * @param chan MAVLink channel to send the message + * @param struct The MAVLink struct to serialize + */ +static inline void mavlink_msg_logging_data_send_struct(mavlink_channel_t chan, const mavlink_logging_data_t* logging_data) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + mavlink_msg_logging_data_send(chan, logging_data->target_system, logging_data->target_component, logging_data->sequence, logging_data->length, logging_data->first_message_offset, logging_data->data); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOGGING_DATA, (const char *)logging_data, MAVLINK_MSG_ID_LOGGING_DATA_MIN_LEN, MAVLINK_MSG_ID_LOGGING_DATA_LEN, MAVLINK_MSG_ID_LOGGING_DATA_CRC); +#endif +} + +#if MAVLINK_MSG_ID_LOGGING_DATA_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_logging_data_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint16_t sequence, uint8_t length, uint8_t first_message_offset, const uint8_t *data) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint16_t(buf, 0, sequence); + _mav_put_uint8_t(buf, 2, target_system); + _mav_put_uint8_t(buf, 3, target_component); + _mav_put_uint8_t(buf, 4, length); + _mav_put_uint8_t(buf, 5, first_message_offset); + _mav_put_uint8_t_array(buf, 6, data, 249); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOGGING_DATA, buf, MAVLINK_MSG_ID_LOGGING_DATA_MIN_LEN, MAVLINK_MSG_ID_LOGGING_DATA_LEN, MAVLINK_MSG_ID_LOGGING_DATA_CRC); +#else + mavlink_logging_data_t *packet = (mavlink_logging_data_t *)msgbuf; + packet->sequence = sequence; + packet->target_system = target_system; + packet->target_component = target_component; + packet->length = length; + packet->first_message_offset = first_message_offset; + mav_array_memcpy(packet->data, data, sizeof(uint8_t)*249); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOGGING_DATA, (const char *)packet, MAVLINK_MSG_ID_LOGGING_DATA_MIN_LEN, MAVLINK_MSG_ID_LOGGING_DATA_LEN, MAVLINK_MSG_ID_LOGGING_DATA_CRC); +#endif +} +#endif + +#endif + +// MESSAGE LOGGING_DATA UNPACKING + + +/** + * @brief Get field target_system from logging_data message + * + * @return system ID of the target + */ +static inline uint8_t mavlink_msg_logging_data_get_target_system(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 2); +} + +/** + * @brief Get field target_component from logging_data message + * + * @return component ID of the target + */ +static inline uint8_t mavlink_msg_logging_data_get_target_component(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 3); +} + +/** + * @brief Get field sequence from logging_data message + * + * @return sequence number (can wrap) + */ +static inline uint16_t mavlink_msg_logging_data_get_sequence(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 0); +} + +/** + * @brief Get field length from logging_data message + * + * @return [bytes] data length + */ +static inline uint8_t mavlink_msg_logging_data_get_length(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 4); +} + +/** + * @brief Get field first_message_offset from logging_data message + * + * @return [bytes] offset into data where first message starts. This can be used for recovery, when a previous message got lost (set to 255 if no start exists). + */ +static inline uint8_t mavlink_msg_logging_data_get_first_message_offset(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 5); +} + +/** + * @brief Get field data from logging_data message + * + * @return logged data + */ +static inline uint16_t mavlink_msg_logging_data_get_data(const mavlink_message_t* msg, uint8_t *data) +{ + return _MAV_RETURN_uint8_t_array(msg, data, 249, 6); +} + +/** + * @brief Decode a logging_data message into a struct + * + * @param msg The message to decode + * @param logging_data C-struct to decode the message contents into + */ +static inline void mavlink_msg_logging_data_decode(const mavlink_message_t* msg, mavlink_logging_data_t* logging_data) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + logging_data->sequence = mavlink_msg_logging_data_get_sequence(msg); + logging_data->target_system = mavlink_msg_logging_data_get_target_system(msg); + logging_data->target_component = mavlink_msg_logging_data_get_target_component(msg); + logging_data->length = mavlink_msg_logging_data_get_length(msg); + logging_data->first_message_offset = mavlink_msg_logging_data_get_first_message_offset(msg); + mavlink_msg_logging_data_get_data(msg, logging_data->data); +#else + uint8_t len = msg->len < MAVLINK_MSG_ID_LOGGING_DATA_LEN? msg->len : MAVLINK_MSG_ID_LOGGING_DATA_LEN; + memset(logging_data, 0, MAVLINK_MSG_ID_LOGGING_DATA_LEN); + memcpy(logging_data, _MAV_PAYLOAD(msg), len); +#endif +} diff --git a/lib/main/MAVLink/common/mavlink_msg_logging_data_acked.h b/lib/main/MAVLink/common/mavlink_msg_logging_data_acked.h new file mode 100644 index 0000000000..a179387c72 --- /dev/null +++ b/lib/main/MAVLink/common/mavlink_msg_logging_data_acked.h @@ -0,0 +1,330 @@ +#pragma once +// MESSAGE LOGGING_DATA_ACKED PACKING + +#define MAVLINK_MSG_ID_LOGGING_DATA_ACKED 267 + + +typedef struct __mavlink_logging_data_acked_t { + uint16_t sequence; /*< sequence number (can wrap)*/ + uint8_t target_system; /*< system ID of the target*/ + uint8_t target_component; /*< component ID of the target*/ + uint8_t length; /*< [bytes] data length*/ + uint8_t first_message_offset; /*< [bytes] offset into data where first message starts. This can be used for recovery, when a previous message got lost (set to 255 if no start exists).*/ + uint8_t data[249]; /*< logged data*/ +} mavlink_logging_data_acked_t; + +#define MAVLINK_MSG_ID_LOGGING_DATA_ACKED_LEN 255 +#define MAVLINK_MSG_ID_LOGGING_DATA_ACKED_MIN_LEN 255 +#define MAVLINK_MSG_ID_267_LEN 255 +#define MAVLINK_MSG_ID_267_MIN_LEN 255 + +#define MAVLINK_MSG_ID_LOGGING_DATA_ACKED_CRC 35 +#define MAVLINK_MSG_ID_267_CRC 35 + +#define MAVLINK_MSG_LOGGING_DATA_ACKED_FIELD_DATA_LEN 249 + +#if MAVLINK_COMMAND_24BIT +#define MAVLINK_MESSAGE_INFO_LOGGING_DATA_ACKED { \ + 267, \ + "LOGGING_DATA_ACKED", \ + 6, \ + { { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 2, offsetof(mavlink_logging_data_acked_t, target_system) }, \ + { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 3, offsetof(mavlink_logging_data_acked_t, target_component) }, \ + { "sequence", NULL, MAVLINK_TYPE_UINT16_T, 0, 0, offsetof(mavlink_logging_data_acked_t, sequence) }, \ + { "length", NULL, MAVLINK_TYPE_UINT8_T, 0, 4, offsetof(mavlink_logging_data_acked_t, length) }, \ + { "first_message_offset", NULL, MAVLINK_TYPE_UINT8_T, 0, 5, offsetof(mavlink_logging_data_acked_t, first_message_offset) }, \ + { "data", NULL, MAVLINK_TYPE_UINT8_T, 249, 6, offsetof(mavlink_logging_data_acked_t, data) }, \ + } \ +} +#else +#define MAVLINK_MESSAGE_INFO_LOGGING_DATA_ACKED { \ + "LOGGING_DATA_ACKED", \ + 6, \ + { { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 2, offsetof(mavlink_logging_data_acked_t, target_system) }, \ + { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 3, offsetof(mavlink_logging_data_acked_t, target_component) }, \ + { "sequence", NULL, MAVLINK_TYPE_UINT16_T, 0, 0, offsetof(mavlink_logging_data_acked_t, sequence) }, \ + { "length", NULL, MAVLINK_TYPE_UINT8_T, 0, 4, offsetof(mavlink_logging_data_acked_t, length) }, \ + { "first_message_offset", NULL, MAVLINK_TYPE_UINT8_T, 0, 5, offsetof(mavlink_logging_data_acked_t, first_message_offset) }, \ + { "data", NULL, MAVLINK_TYPE_UINT8_T, 249, 6, offsetof(mavlink_logging_data_acked_t, data) }, \ + } \ +} +#endif + +/** + * @brief Pack a logging_data_acked message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param target_system system ID of the target + * @param target_component component ID of the target + * @param sequence sequence number (can wrap) + * @param length [bytes] data length + * @param first_message_offset [bytes] offset into data where first message starts. This can be used for recovery, when a previous message got lost (set to 255 if no start exists). + * @param data logged data + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_logging_data_acked_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint8_t target_system, uint8_t target_component, uint16_t sequence, uint8_t length, uint8_t first_message_offset, const uint8_t *data) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_LOGGING_DATA_ACKED_LEN]; + _mav_put_uint16_t(buf, 0, sequence); + _mav_put_uint8_t(buf, 2, target_system); + _mav_put_uint8_t(buf, 3, target_component); + _mav_put_uint8_t(buf, 4, length); + _mav_put_uint8_t(buf, 5, first_message_offset); + _mav_put_uint8_t_array(buf, 6, data, 249); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_LOGGING_DATA_ACKED_LEN); +#else + mavlink_logging_data_acked_t packet; + packet.sequence = sequence; + packet.target_system = target_system; + packet.target_component = target_component; + packet.length = length; + packet.first_message_offset = first_message_offset; + mav_array_memcpy(packet.data, data, sizeof(uint8_t)*249); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_LOGGING_DATA_ACKED_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_LOGGING_DATA_ACKED; + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_LOGGING_DATA_ACKED_MIN_LEN, MAVLINK_MSG_ID_LOGGING_DATA_ACKED_LEN, MAVLINK_MSG_ID_LOGGING_DATA_ACKED_CRC); +} + +/** + * @brief Pack a logging_data_acked message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param target_system system ID of the target + * @param target_component component ID of the target + * @param sequence sequence number (can wrap) + * @param length [bytes] data length + * @param first_message_offset [bytes] offset into data where first message starts. This can be used for recovery, when a previous message got lost (set to 255 if no start exists). + * @param data logged data + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_logging_data_acked_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint8_t target_system,uint8_t target_component,uint16_t sequence,uint8_t length,uint8_t first_message_offset,const uint8_t *data) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_LOGGING_DATA_ACKED_LEN]; + _mav_put_uint16_t(buf, 0, sequence); + _mav_put_uint8_t(buf, 2, target_system); + _mav_put_uint8_t(buf, 3, target_component); + _mav_put_uint8_t(buf, 4, length); + _mav_put_uint8_t(buf, 5, first_message_offset); + _mav_put_uint8_t_array(buf, 6, data, 249); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_LOGGING_DATA_ACKED_LEN); +#else + mavlink_logging_data_acked_t packet; + packet.sequence = sequence; + packet.target_system = target_system; + packet.target_component = target_component; + packet.length = length; + packet.first_message_offset = first_message_offset; + mav_array_memcpy(packet.data, data, sizeof(uint8_t)*249); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_LOGGING_DATA_ACKED_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_LOGGING_DATA_ACKED; + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_LOGGING_DATA_ACKED_MIN_LEN, MAVLINK_MSG_ID_LOGGING_DATA_ACKED_LEN, MAVLINK_MSG_ID_LOGGING_DATA_ACKED_CRC); +} + +/** + * @brief Encode a logging_data_acked struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param logging_data_acked C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_logging_data_acked_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_logging_data_acked_t* logging_data_acked) +{ + return mavlink_msg_logging_data_acked_pack(system_id, component_id, msg, logging_data_acked->target_system, logging_data_acked->target_component, logging_data_acked->sequence, logging_data_acked->length, logging_data_acked->first_message_offset, logging_data_acked->data); +} + +/** + * @brief Encode a logging_data_acked struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param logging_data_acked C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_logging_data_acked_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_logging_data_acked_t* logging_data_acked) +{ + return mavlink_msg_logging_data_acked_pack_chan(system_id, component_id, chan, msg, logging_data_acked->target_system, logging_data_acked->target_component, logging_data_acked->sequence, logging_data_acked->length, logging_data_acked->first_message_offset, logging_data_acked->data); +} + +/** + * @brief Send a logging_data_acked message + * @param chan MAVLink channel to send the message + * + * @param target_system system ID of the target + * @param target_component component ID of the target + * @param sequence sequence number (can wrap) + * @param length [bytes] data length + * @param first_message_offset [bytes] offset into data where first message starts. This can be used for recovery, when a previous message got lost (set to 255 if no start exists). + * @param data logged data + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_logging_data_acked_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint16_t sequence, uint8_t length, uint8_t first_message_offset, const uint8_t *data) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_LOGGING_DATA_ACKED_LEN]; + _mav_put_uint16_t(buf, 0, sequence); + _mav_put_uint8_t(buf, 2, target_system); + _mav_put_uint8_t(buf, 3, target_component); + _mav_put_uint8_t(buf, 4, length); + _mav_put_uint8_t(buf, 5, first_message_offset); + _mav_put_uint8_t_array(buf, 6, data, 249); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOGGING_DATA_ACKED, buf, MAVLINK_MSG_ID_LOGGING_DATA_ACKED_MIN_LEN, MAVLINK_MSG_ID_LOGGING_DATA_ACKED_LEN, MAVLINK_MSG_ID_LOGGING_DATA_ACKED_CRC); +#else + mavlink_logging_data_acked_t packet; + packet.sequence = sequence; + packet.target_system = target_system; + packet.target_component = target_component; + packet.length = length; + packet.first_message_offset = first_message_offset; + mav_array_memcpy(packet.data, data, sizeof(uint8_t)*249); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOGGING_DATA_ACKED, (const char *)&packet, MAVLINK_MSG_ID_LOGGING_DATA_ACKED_MIN_LEN, MAVLINK_MSG_ID_LOGGING_DATA_ACKED_LEN, MAVLINK_MSG_ID_LOGGING_DATA_ACKED_CRC); +#endif +} + +/** + * @brief Send a logging_data_acked message + * @param chan MAVLink channel to send the message + * @param struct The MAVLink struct to serialize + */ +static inline void mavlink_msg_logging_data_acked_send_struct(mavlink_channel_t chan, const mavlink_logging_data_acked_t* logging_data_acked) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + mavlink_msg_logging_data_acked_send(chan, logging_data_acked->target_system, logging_data_acked->target_component, logging_data_acked->sequence, logging_data_acked->length, logging_data_acked->first_message_offset, logging_data_acked->data); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOGGING_DATA_ACKED, (const char *)logging_data_acked, MAVLINK_MSG_ID_LOGGING_DATA_ACKED_MIN_LEN, MAVLINK_MSG_ID_LOGGING_DATA_ACKED_LEN, MAVLINK_MSG_ID_LOGGING_DATA_ACKED_CRC); +#endif +} + +#if MAVLINK_MSG_ID_LOGGING_DATA_ACKED_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_logging_data_acked_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint16_t sequence, uint8_t length, uint8_t first_message_offset, const uint8_t *data) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint16_t(buf, 0, sequence); + _mav_put_uint8_t(buf, 2, target_system); + _mav_put_uint8_t(buf, 3, target_component); + _mav_put_uint8_t(buf, 4, length); + _mav_put_uint8_t(buf, 5, first_message_offset); + _mav_put_uint8_t_array(buf, 6, data, 249); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOGGING_DATA_ACKED, buf, MAVLINK_MSG_ID_LOGGING_DATA_ACKED_MIN_LEN, MAVLINK_MSG_ID_LOGGING_DATA_ACKED_LEN, MAVLINK_MSG_ID_LOGGING_DATA_ACKED_CRC); +#else + mavlink_logging_data_acked_t *packet = (mavlink_logging_data_acked_t *)msgbuf; + packet->sequence = sequence; + packet->target_system = target_system; + packet->target_component = target_component; + packet->length = length; + packet->first_message_offset = first_message_offset; + mav_array_memcpy(packet->data, data, sizeof(uint8_t)*249); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_LOGGING_DATA_ACKED, (const char *)packet, MAVLINK_MSG_ID_LOGGING_DATA_ACKED_MIN_LEN, MAVLINK_MSG_ID_LOGGING_DATA_ACKED_LEN, MAVLINK_MSG_ID_LOGGING_DATA_ACKED_CRC); +#endif +} +#endif + +#endif + +// MESSAGE LOGGING_DATA_ACKED UNPACKING + + +/** + * @brief Get field target_system from logging_data_acked message + * + * @return system ID of the target + */ +static inline uint8_t mavlink_msg_logging_data_acked_get_target_system(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 2); +} + +/** + * @brief Get field target_component from logging_data_acked message + * + * @return component ID of the target + */ +static inline uint8_t mavlink_msg_logging_data_acked_get_target_component(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 3); +} + +/** + * @brief Get field sequence from logging_data_acked message + * + * @return sequence number (can wrap) + */ +static inline uint16_t mavlink_msg_logging_data_acked_get_sequence(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 0); +} + +/** + * @brief Get field length from logging_data_acked message + * + * @return [bytes] data length + */ +static inline uint8_t mavlink_msg_logging_data_acked_get_length(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 4); +} + +/** + * @brief Get field first_message_offset from logging_data_acked message + * + * @return [bytes] offset into data where first message starts. This can be used for recovery, when a previous message got lost (set to 255 if no start exists). + */ +static inline uint8_t mavlink_msg_logging_data_acked_get_first_message_offset(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 5); +} + +/** + * @brief Get field data from logging_data_acked message + * + * @return logged data + */ +static inline uint16_t mavlink_msg_logging_data_acked_get_data(const mavlink_message_t* msg, uint8_t *data) +{ + return _MAV_RETURN_uint8_t_array(msg, data, 249, 6); +} + +/** + * @brief Decode a logging_data_acked message into a struct + * + * @param msg The message to decode + * @param logging_data_acked C-struct to decode the message contents into + */ +static inline void mavlink_msg_logging_data_acked_decode(const mavlink_message_t* msg, mavlink_logging_data_acked_t* logging_data_acked) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + logging_data_acked->sequence = mavlink_msg_logging_data_acked_get_sequence(msg); + logging_data_acked->target_system = mavlink_msg_logging_data_acked_get_target_system(msg); + logging_data_acked->target_component = mavlink_msg_logging_data_acked_get_target_component(msg); + logging_data_acked->length = mavlink_msg_logging_data_acked_get_length(msg); + logging_data_acked->first_message_offset = mavlink_msg_logging_data_acked_get_first_message_offset(msg); + mavlink_msg_logging_data_acked_get_data(msg, logging_data_acked->data); +#else + uint8_t len = msg->len < MAVLINK_MSG_ID_LOGGING_DATA_ACKED_LEN? msg->len : MAVLINK_MSG_ID_LOGGING_DATA_ACKED_LEN; + memset(logging_data_acked, 0, MAVLINK_MSG_ID_LOGGING_DATA_ACKED_LEN); + memcpy(logging_data_acked, _MAV_PAYLOAD(msg), len); +#endif +} diff --git a/lib/main/MAVLink/common/mavlink_msg_mag_cal_report.h b/lib/main/MAVLink/common/mavlink_msg_mag_cal_report.h new file mode 100644 index 0000000000..6694501e17 --- /dev/null +++ b/lib/main/MAVLink/common/mavlink_msg_mag_cal_report.h @@ -0,0 +1,638 @@ +#pragma once +// MESSAGE MAG_CAL_REPORT PACKING + +#define MAVLINK_MSG_ID_MAG_CAL_REPORT 192 + +MAVPACKED( +typedef struct __mavlink_mag_cal_report_t { + float fitness; /*< [mgauss] RMS milligauss residuals.*/ + float ofs_x; /*< X offset.*/ + float ofs_y; /*< Y offset.*/ + float ofs_z; /*< Z offset.*/ + float diag_x; /*< X diagonal (matrix 11).*/ + float diag_y; /*< Y diagonal (matrix 22).*/ + float diag_z; /*< Z diagonal (matrix 33).*/ + float offdiag_x; /*< X off-diagonal (matrix 12 and 21).*/ + float offdiag_y; /*< Y off-diagonal (matrix 13 and 31).*/ + float offdiag_z; /*< Z off-diagonal (matrix 32 and 23).*/ + uint8_t compass_id; /*< Compass being calibrated.*/ + uint8_t cal_mask; /*< Bitmask of compasses being calibrated.*/ + uint8_t cal_status; /*< Calibration Status.*/ + uint8_t autosaved; /*< 0=requires a MAV_CMD_DO_ACCEPT_MAG_CAL, 1=saved to parameters.*/ + float orientation_confidence; /*< Confidence in orientation (higher is better).*/ + uint8_t old_orientation; /*< orientation before calibration.*/ + uint8_t new_orientation; /*< orientation after calibration.*/ + float scale_factor; /*< field radius correction factor*/ +}) mavlink_mag_cal_report_t; + +#define MAVLINK_MSG_ID_MAG_CAL_REPORT_LEN 54 +#define MAVLINK_MSG_ID_MAG_CAL_REPORT_MIN_LEN 44 +#define MAVLINK_MSG_ID_192_LEN 54 +#define MAVLINK_MSG_ID_192_MIN_LEN 44 + +#define MAVLINK_MSG_ID_MAG_CAL_REPORT_CRC 36 +#define MAVLINK_MSG_ID_192_CRC 36 + + + +#if MAVLINK_COMMAND_24BIT +#define MAVLINK_MESSAGE_INFO_MAG_CAL_REPORT { \ + 192, \ + "MAG_CAL_REPORT", \ + 18, \ + { { "compass_id", NULL, MAVLINK_TYPE_UINT8_T, 0, 40, offsetof(mavlink_mag_cal_report_t, compass_id) }, \ + { "cal_mask", NULL, MAVLINK_TYPE_UINT8_T, 0, 41, offsetof(mavlink_mag_cal_report_t, cal_mask) }, \ + { "cal_status", NULL, MAVLINK_TYPE_UINT8_T, 0, 42, offsetof(mavlink_mag_cal_report_t, cal_status) }, \ + { "autosaved", NULL, MAVLINK_TYPE_UINT8_T, 0, 43, offsetof(mavlink_mag_cal_report_t, autosaved) }, \ + { "fitness", NULL, MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_mag_cal_report_t, fitness) }, \ + { "ofs_x", NULL, MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_mag_cal_report_t, ofs_x) }, \ + { "ofs_y", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_mag_cal_report_t, ofs_y) }, \ + { "ofs_z", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_mag_cal_report_t, ofs_z) }, \ + { "diag_x", NULL, MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_mag_cal_report_t, diag_x) }, \ + { "diag_y", NULL, MAVLINK_TYPE_FLOAT, 0, 20, offsetof(mavlink_mag_cal_report_t, diag_y) }, \ + { "diag_z", NULL, MAVLINK_TYPE_FLOAT, 0, 24, offsetof(mavlink_mag_cal_report_t, diag_z) }, \ + { "offdiag_x", NULL, MAVLINK_TYPE_FLOAT, 0, 28, offsetof(mavlink_mag_cal_report_t, offdiag_x) }, \ + { "offdiag_y", NULL, MAVLINK_TYPE_FLOAT, 0, 32, offsetof(mavlink_mag_cal_report_t, offdiag_y) }, \ + { "offdiag_z", NULL, MAVLINK_TYPE_FLOAT, 0, 36, offsetof(mavlink_mag_cal_report_t, offdiag_z) }, \ + { "orientation_confidence", NULL, MAVLINK_TYPE_FLOAT, 0, 44, offsetof(mavlink_mag_cal_report_t, orientation_confidence) }, \ + { "old_orientation", NULL, MAVLINK_TYPE_UINT8_T, 0, 48, offsetof(mavlink_mag_cal_report_t, old_orientation) }, \ + { "new_orientation", NULL, MAVLINK_TYPE_UINT8_T, 0, 49, offsetof(mavlink_mag_cal_report_t, new_orientation) }, \ + { "scale_factor", NULL, MAVLINK_TYPE_FLOAT, 0, 50, offsetof(mavlink_mag_cal_report_t, scale_factor) }, \ + } \ +} +#else +#define MAVLINK_MESSAGE_INFO_MAG_CAL_REPORT { \ + "MAG_CAL_REPORT", \ + 18, \ + { { "compass_id", NULL, MAVLINK_TYPE_UINT8_T, 0, 40, offsetof(mavlink_mag_cal_report_t, compass_id) }, \ + { "cal_mask", NULL, MAVLINK_TYPE_UINT8_T, 0, 41, offsetof(mavlink_mag_cal_report_t, cal_mask) }, \ + { "cal_status", NULL, MAVLINK_TYPE_UINT8_T, 0, 42, offsetof(mavlink_mag_cal_report_t, cal_status) }, \ + { "autosaved", NULL, MAVLINK_TYPE_UINT8_T, 0, 43, offsetof(mavlink_mag_cal_report_t, autosaved) }, \ + { "fitness", NULL, MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_mag_cal_report_t, fitness) }, \ + { "ofs_x", NULL, MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_mag_cal_report_t, ofs_x) }, \ + { "ofs_y", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_mag_cal_report_t, ofs_y) }, \ + { "ofs_z", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_mag_cal_report_t, ofs_z) }, \ + { "diag_x", NULL, MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_mag_cal_report_t, diag_x) }, \ + { "diag_y", NULL, MAVLINK_TYPE_FLOAT, 0, 20, offsetof(mavlink_mag_cal_report_t, diag_y) }, \ + { "diag_z", NULL, MAVLINK_TYPE_FLOAT, 0, 24, offsetof(mavlink_mag_cal_report_t, diag_z) }, \ + { "offdiag_x", NULL, MAVLINK_TYPE_FLOAT, 0, 28, offsetof(mavlink_mag_cal_report_t, offdiag_x) }, \ + { "offdiag_y", NULL, MAVLINK_TYPE_FLOAT, 0, 32, offsetof(mavlink_mag_cal_report_t, offdiag_y) }, \ + { "offdiag_z", NULL, MAVLINK_TYPE_FLOAT, 0, 36, offsetof(mavlink_mag_cal_report_t, offdiag_z) }, \ + { "orientation_confidence", NULL, MAVLINK_TYPE_FLOAT, 0, 44, offsetof(mavlink_mag_cal_report_t, orientation_confidence) }, \ + { "old_orientation", NULL, MAVLINK_TYPE_UINT8_T, 0, 48, offsetof(mavlink_mag_cal_report_t, old_orientation) }, \ + { "new_orientation", NULL, MAVLINK_TYPE_UINT8_T, 0, 49, offsetof(mavlink_mag_cal_report_t, new_orientation) }, \ + { "scale_factor", NULL, MAVLINK_TYPE_FLOAT, 0, 50, offsetof(mavlink_mag_cal_report_t, scale_factor) }, \ + } \ +} +#endif + +/** + * @brief Pack a mag_cal_report message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param compass_id Compass being calibrated. + * @param cal_mask Bitmask of compasses being calibrated. + * @param cal_status Calibration Status. + * @param autosaved 0=requires a MAV_CMD_DO_ACCEPT_MAG_CAL, 1=saved to parameters. + * @param fitness [mgauss] RMS milligauss residuals. + * @param ofs_x X offset. + * @param ofs_y Y offset. + * @param ofs_z Z offset. + * @param diag_x X diagonal (matrix 11). + * @param diag_y Y diagonal (matrix 22). + * @param diag_z Z diagonal (matrix 33). + * @param offdiag_x X off-diagonal (matrix 12 and 21). + * @param offdiag_y Y off-diagonal (matrix 13 and 31). + * @param offdiag_z Z off-diagonal (matrix 32 and 23). + * @param orientation_confidence Confidence in orientation (higher is better). + * @param old_orientation orientation before calibration. + * @param new_orientation orientation after calibration. + * @param scale_factor field radius correction factor + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_mag_cal_report_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint8_t compass_id, uint8_t cal_mask, uint8_t cal_status, uint8_t autosaved, float fitness, float ofs_x, float ofs_y, float ofs_z, float diag_x, float diag_y, float diag_z, float offdiag_x, float offdiag_y, float offdiag_z, float orientation_confidence, uint8_t old_orientation, uint8_t new_orientation, float scale_factor) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_MAG_CAL_REPORT_LEN]; + _mav_put_float(buf, 0, fitness); + _mav_put_float(buf, 4, ofs_x); + _mav_put_float(buf, 8, ofs_y); + _mav_put_float(buf, 12, ofs_z); + _mav_put_float(buf, 16, diag_x); + _mav_put_float(buf, 20, diag_y); + _mav_put_float(buf, 24, diag_z); + _mav_put_float(buf, 28, offdiag_x); + _mav_put_float(buf, 32, offdiag_y); + _mav_put_float(buf, 36, offdiag_z); + _mav_put_uint8_t(buf, 40, compass_id); + _mav_put_uint8_t(buf, 41, cal_mask); + _mav_put_uint8_t(buf, 42, cal_status); + _mav_put_uint8_t(buf, 43, autosaved); + _mav_put_float(buf, 44, orientation_confidence); + _mav_put_uint8_t(buf, 48, old_orientation); + _mav_put_uint8_t(buf, 49, new_orientation); + _mav_put_float(buf, 50, scale_factor); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_MAG_CAL_REPORT_LEN); +#else + mavlink_mag_cal_report_t packet; + packet.fitness = fitness; + packet.ofs_x = ofs_x; + packet.ofs_y = ofs_y; + packet.ofs_z = ofs_z; + packet.diag_x = diag_x; + packet.diag_y = diag_y; + packet.diag_z = diag_z; + packet.offdiag_x = offdiag_x; + packet.offdiag_y = offdiag_y; + packet.offdiag_z = offdiag_z; + packet.compass_id = compass_id; + packet.cal_mask = cal_mask; + packet.cal_status = cal_status; + packet.autosaved = autosaved; + packet.orientation_confidence = orientation_confidence; + packet.old_orientation = old_orientation; + packet.new_orientation = new_orientation; + packet.scale_factor = scale_factor; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_MAG_CAL_REPORT_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_MAG_CAL_REPORT; + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_MAG_CAL_REPORT_MIN_LEN, MAVLINK_MSG_ID_MAG_CAL_REPORT_LEN, MAVLINK_MSG_ID_MAG_CAL_REPORT_CRC); +} + +/** + * @brief Pack a mag_cal_report message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param compass_id Compass being calibrated. + * @param cal_mask Bitmask of compasses being calibrated. + * @param cal_status Calibration Status. + * @param autosaved 0=requires a MAV_CMD_DO_ACCEPT_MAG_CAL, 1=saved to parameters. + * @param fitness [mgauss] RMS milligauss residuals. + * @param ofs_x X offset. + * @param ofs_y Y offset. + * @param ofs_z Z offset. + * @param diag_x X diagonal (matrix 11). + * @param diag_y Y diagonal (matrix 22). + * @param diag_z Z diagonal (matrix 33). + * @param offdiag_x X off-diagonal (matrix 12 and 21). + * @param offdiag_y Y off-diagonal (matrix 13 and 31). + * @param offdiag_z Z off-diagonal (matrix 32 and 23). + * @param orientation_confidence Confidence in orientation (higher is better). + * @param old_orientation orientation before calibration. + * @param new_orientation orientation after calibration. + * @param scale_factor field radius correction factor + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_mag_cal_report_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint8_t compass_id,uint8_t cal_mask,uint8_t cal_status,uint8_t autosaved,float fitness,float ofs_x,float ofs_y,float ofs_z,float diag_x,float diag_y,float diag_z,float offdiag_x,float offdiag_y,float offdiag_z,float orientation_confidence,uint8_t old_orientation,uint8_t new_orientation,float scale_factor) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_MAG_CAL_REPORT_LEN]; + _mav_put_float(buf, 0, fitness); + _mav_put_float(buf, 4, ofs_x); + _mav_put_float(buf, 8, ofs_y); + _mav_put_float(buf, 12, ofs_z); + _mav_put_float(buf, 16, diag_x); + _mav_put_float(buf, 20, diag_y); + _mav_put_float(buf, 24, diag_z); + _mav_put_float(buf, 28, offdiag_x); + _mav_put_float(buf, 32, offdiag_y); + _mav_put_float(buf, 36, offdiag_z); + _mav_put_uint8_t(buf, 40, compass_id); + _mav_put_uint8_t(buf, 41, cal_mask); + _mav_put_uint8_t(buf, 42, cal_status); + _mav_put_uint8_t(buf, 43, autosaved); + _mav_put_float(buf, 44, orientation_confidence); + _mav_put_uint8_t(buf, 48, old_orientation); + _mav_put_uint8_t(buf, 49, new_orientation); + _mav_put_float(buf, 50, scale_factor); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_MAG_CAL_REPORT_LEN); +#else + mavlink_mag_cal_report_t packet; + packet.fitness = fitness; + packet.ofs_x = ofs_x; + packet.ofs_y = ofs_y; + packet.ofs_z = ofs_z; + packet.diag_x = diag_x; + packet.diag_y = diag_y; + packet.diag_z = diag_z; + packet.offdiag_x = offdiag_x; + packet.offdiag_y = offdiag_y; + packet.offdiag_z = offdiag_z; + packet.compass_id = compass_id; + packet.cal_mask = cal_mask; + packet.cal_status = cal_status; + packet.autosaved = autosaved; + packet.orientation_confidence = orientation_confidence; + packet.old_orientation = old_orientation; + packet.new_orientation = new_orientation; + packet.scale_factor = scale_factor; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_MAG_CAL_REPORT_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_MAG_CAL_REPORT; + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_MAG_CAL_REPORT_MIN_LEN, MAVLINK_MSG_ID_MAG_CAL_REPORT_LEN, MAVLINK_MSG_ID_MAG_CAL_REPORT_CRC); +} + +/** + * @brief Encode a mag_cal_report struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param mag_cal_report C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_mag_cal_report_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_mag_cal_report_t* mag_cal_report) +{ + return mavlink_msg_mag_cal_report_pack(system_id, component_id, msg, mag_cal_report->compass_id, mag_cal_report->cal_mask, mag_cal_report->cal_status, mag_cal_report->autosaved, mag_cal_report->fitness, mag_cal_report->ofs_x, mag_cal_report->ofs_y, mag_cal_report->ofs_z, mag_cal_report->diag_x, mag_cal_report->diag_y, mag_cal_report->diag_z, mag_cal_report->offdiag_x, mag_cal_report->offdiag_y, mag_cal_report->offdiag_z, mag_cal_report->orientation_confidence, mag_cal_report->old_orientation, mag_cal_report->new_orientation, mag_cal_report->scale_factor); +} + +/** + * @brief Encode a mag_cal_report struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param mag_cal_report C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_mag_cal_report_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_mag_cal_report_t* mag_cal_report) +{ + return mavlink_msg_mag_cal_report_pack_chan(system_id, component_id, chan, msg, mag_cal_report->compass_id, mag_cal_report->cal_mask, mag_cal_report->cal_status, mag_cal_report->autosaved, mag_cal_report->fitness, mag_cal_report->ofs_x, mag_cal_report->ofs_y, mag_cal_report->ofs_z, mag_cal_report->diag_x, mag_cal_report->diag_y, mag_cal_report->diag_z, mag_cal_report->offdiag_x, mag_cal_report->offdiag_y, mag_cal_report->offdiag_z, mag_cal_report->orientation_confidence, mag_cal_report->old_orientation, mag_cal_report->new_orientation, mag_cal_report->scale_factor); +} + +/** + * @brief Send a mag_cal_report message + * @param chan MAVLink channel to send the message + * + * @param compass_id Compass being calibrated. + * @param cal_mask Bitmask of compasses being calibrated. + * @param cal_status Calibration Status. + * @param autosaved 0=requires a MAV_CMD_DO_ACCEPT_MAG_CAL, 1=saved to parameters. + * @param fitness [mgauss] RMS milligauss residuals. + * @param ofs_x X offset. + * @param ofs_y Y offset. + * @param ofs_z Z offset. + * @param diag_x X diagonal (matrix 11). + * @param diag_y Y diagonal (matrix 22). + * @param diag_z Z diagonal (matrix 33). + * @param offdiag_x X off-diagonal (matrix 12 and 21). + * @param offdiag_y Y off-diagonal (matrix 13 and 31). + * @param offdiag_z Z off-diagonal (matrix 32 and 23). + * @param orientation_confidence Confidence in orientation (higher is better). + * @param old_orientation orientation before calibration. + * @param new_orientation orientation after calibration. + * @param scale_factor field radius correction factor + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_mag_cal_report_send(mavlink_channel_t chan, uint8_t compass_id, uint8_t cal_mask, uint8_t cal_status, uint8_t autosaved, float fitness, float ofs_x, float ofs_y, float ofs_z, float diag_x, float diag_y, float diag_z, float offdiag_x, float offdiag_y, float offdiag_z, float orientation_confidence, uint8_t old_orientation, uint8_t new_orientation, float scale_factor) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_MAG_CAL_REPORT_LEN]; + _mav_put_float(buf, 0, fitness); + _mav_put_float(buf, 4, ofs_x); + _mav_put_float(buf, 8, ofs_y); + _mav_put_float(buf, 12, ofs_z); + _mav_put_float(buf, 16, diag_x); + _mav_put_float(buf, 20, diag_y); + _mav_put_float(buf, 24, diag_z); + _mav_put_float(buf, 28, offdiag_x); + _mav_put_float(buf, 32, offdiag_y); + _mav_put_float(buf, 36, offdiag_z); + _mav_put_uint8_t(buf, 40, compass_id); + _mav_put_uint8_t(buf, 41, cal_mask); + _mav_put_uint8_t(buf, 42, cal_status); + _mav_put_uint8_t(buf, 43, autosaved); + _mav_put_float(buf, 44, orientation_confidence); + _mav_put_uint8_t(buf, 48, old_orientation); + _mav_put_uint8_t(buf, 49, new_orientation); + _mav_put_float(buf, 50, scale_factor); + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MAG_CAL_REPORT, buf, MAVLINK_MSG_ID_MAG_CAL_REPORT_MIN_LEN, MAVLINK_MSG_ID_MAG_CAL_REPORT_LEN, MAVLINK_MSG_ID_MAG_CAL_REPORT_CRC); +#else + mavlink_mag_cal_report_t packet; + packet.fitness = fitness; + packet.ofs_x = ofs_x; + packet.ofs_y = ofs_y; + packet.ofs_z = ofs_z; + packet.diag_x = diag_x; + packet.diag_y = diag_y; + packet.diag_z = diag_z; + packet.offdiag_x = offdiag_x; + packet.offdiag_y = offdiag_y; + packet.offdiag_z = offdiag_z; + packet.compass_id = compass_id; + packet.cal_mask = cal_mask; + packet.cal_status = cal_status; + packet.autosaved = autosaved; + packet.orientation_confidence = orientation_confidence; + packet.old_orientation = old_orientation; + packet.new_orientation = new_orientation; + packet.scale_factor = scale_factor; + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MAG_CAL_REPORT, (const char *)&packet, MAVLINK_MSG_ID_MAG_CAL_REPORT_MIN_LEN, MAVLINK_MSG_ID_MAG_CAL_REPORT_LEN, MAVLINK_MSG_ID_MAG_CAL_REPORT_CRC); +#endif +} + +/** + * @brief Send a mag_cal_report message + * @param chan MAVLink channel to send the message + * @param struct The MAVLink struct to serialize + */ +static inline void mavlink_msg_mag_cal_report_send_struct(mavlink_channel_t chan, const mavlink_mag_cal_report_t* mag_cal_report) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + mavlink_msg_mag_cal_report_send(chan, mag_cal_report->compass_id, mag_cal_report->cal_mask, mag_cal_report->cal_status, mag_cal_report->autosaved, mag_cal_report->fitness, mag_cal_report->ofs_x, mag_cal_report->ofs_y, mag_cal_report->ofs_z, mag_cal_report->diag_x, mag_cal_report->diag_y, mag_cal_report->diag_z, mag_cal_report->offdiag_x, mag_cal_report->offdiag_y, mag_cal_report->offdiag_z, mag_cal_report->orientation_confidence, mag_cal_report->old_orientation, mag_cal_report->new_orientation, mag_cal_report->scale_factor); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MAG_CAL_REPORT, (const char *)mag_cal_report, MAVLINK_MSG_ID_MAG_CAL_REPORT_MIN_LEN, MAVLINK_MSG_ID_MAG_CAL_REPORT_LEN, MAVLINK_MSG_ID_MAG_CAL_REPORT_CRC); +#endif +} + +#if MAVLINK_MSG_ID_MAG_CAL_REPORT_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_mag_cal_report_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t compass_id, uint8_t cal_mask, uint8_t cal_status, uint8_t autosaved, float fitness, float ofs_x, float ofs_y, float ofs_z, float diag_x, float diag_y, float diag_z, float offdiag_x, float offdiag_y, float offdiag_z, float orientation_confidence, uint8_t old_orientation, uint8_t new_orientation, float scale_factor) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_float(buf, 0, fitness); + _mav_put_float(buf, 4, ofs_x); + _mav_put_float(buf, 8, ofs_y); + _mav_put_float(buf, 12, ofs_z); + _mav_put_float(buf, 16, diag_x); + _mav_put_float(buf, 20, diag_y); + _mav_put_float(buf, 24, diag_z); + _mav_put_float(buf, 28, offdiag_x); + _mav_put_float(buf, 32, offdiag_y); + _mav_put_float(buf, 36, offdiag_z); + _mav_put_uint8_t(buf, 40, compass_id); + _mav_put_uint8_t(buf, 41, cal_mask); + _mav_put_uint8_t(buf, 42, cal_status); + _mav_put_uint8_t(buf, 43, autosaved); + _mav_put_float(buf, 44, orientation_confidence); + _mav_put_uint8_t(buf, 48, old_orientation); + _mav_put_uint8_t(buf, 49, new_orientation); + _mav_put_float(buf, 50, scale_factor); + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MAG_CAL_REPORT, buf, MAVLINK_MSG_ID_MAG_CAL_REPORT_MIN_LEN, MAVLINK_MSG_ID_MAG_CAL_REPORT_LEN, MAVLINK_MSG_ID_MAG_CAL_REPORT_CRC); +#else + mavlink_mag_cal_report_t *packet = (mavlink_mag_cal_report_t *)msgbuf; + packet->fitness = fitness; + packet->ofs_x = ofs_x; + packet->ofs_y = ofs_y; + packet->ofs_z = ofs_z; + packet->diag_x = diag_x; + packet->diag_y = diag_y; + packet->diag_z = diag_z; + packet->offdiag_x = offdiag_x; + packet->offdiag_y = offdiag_y; + packet->offdiag_z = offdiag_z; + packet->compass_id = compass_id; + packet->cal_mask = cal_mask; + packet->cal_status = cal_status; + packet->autosaved = autosaved; + packet->orientation_confidence = orientation_confidence; + packet->old_orientation = old_orientation; + packet->new_orientation = new_orientation; + packet->scale_factor = scale_factor; + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MAG_CAL_REPORT, (const char *)packet, MAVLINK_MSG_ID_MAG_CAL_REPORT_MIN_LEN, MAVLINK_MSG_ID_MAG_CAL_REPORT_LEN, MAVLINK_MSG_ID_MAG_CAL_REPORT_CRC); +#endif +} +#endif + +#endif + +// MESSAGE MAG_CAL_REPORT UNPACKING + + +/** + * @brief Get field compass_id from mag_cal_report message + * + * @return Compass being calibrated. + */ +static inline uint8_t mavlink_msg_mag_cal_report_get_compass_id(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 40); +} + +/** + * @brief Get field cal_mask from mag_cal_report message + * + * @return Bitmask of compasses being calibrated. + */ +static inline uint8_t mavlink_msg_mag_cal_report_get_cal_mask(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 41); +} + +/** + * @brief Get field cal_status from mag_cal_report message + * + * @return Calibration Status. + */ +static inline uint8_t mavlink_msg_mag_cal_report_get_cal_status(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 42); +} + +/** + * @brief Get field autosaved from mag_cal_report message + * + * @return 0=requires a MAV_CMD_DO_ACCEPT_MAG_CAL, 1=saved to parameters. + */ +static inline uint8_t mavlink_msg_mag_cal_report_get_autosaved(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 43); +} + +/** + * @brief Get field fitness from mag_cal_report message + * + * @return [mgauss] RMS milligauss residuals. + */ +static inline float mavlink_msg_mag_cal_report_get_fitness(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 0); +} + +/** + * @brief Get field ofs_x from mag_cal_report message + * + * @return X offset. + */ +static inline float mavlink_msg_mag_cal_report_get_ofs_x(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 4); +} + +/** + * @brief Get field ofs_y from mag_cal_report message + * + * @return Y offset. + */ +static inline float mavlink_msg_mag_cal_report_get_ofs_y(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 8); +} + +/** + * @brief Get field ofs_z from mag_cal_report message + * + * @return Z offset. + */ +static inline float mavlink_msg_mag_cal_report_get_ofs_z(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 12); +} + +/** + * @brief Get field diag_x from mag_cal_report message + * + * @return X diagonal (matrix 11). + */ +static inline float mavlink_msg_mag_cal_report_get_diag_x(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 16); +} + +/** + * @brief Get field diag_y from mag_cal_report message + * + * @return Y diagonal (matrix 22). + */ +static inline float mavlink_msg_mag_cal_report_get_diag_y(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 20); +} + +/** + * @brief Get field diag_z from mag_cal_report message + * + * @return Z diagonal (matrix 33). + */ +static inline float mavlink_msg_mag_cal_report_get_diag_z(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 24); +} + +/** + * @brief Get field offdiag_x from mag_cal_report message + * + * @return X off-diagonal (matrix 12 and 21). + */ +static inline float mavlink_msg_mag_cal_report_get_offdiag_x(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 28); +} + +/** + * @brief Get field offdiag_y from mag_cal_report message + * + * @return Y off-diagonal (matrix 13 and 31). + */ +static inline float mavlink_msg_mag_cal_report_get_offdiag_y(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 32); +} + +/** + * @brief Get field offdiag_z from mag_cal_report message + * + * @return Z off-diagonal (matrix 32 and 23). + */ +static inline float mavlink_msg_mag_cal_report_get_offdiag_z(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 36); +} + +/** + * @brief Get field orientation_confidence from mag_cal_report message + * + * @return Confidence in orientation (higher is better). + */ +static inline float mavlink_msg_mag_cal_report_get_orientation_confidence(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 44); +} + +/** + * @brief Get field old_orientation from mag_cal_report message + * + * @return orientation before calibration. + */ +static inline uint8_t mavlink_msg_mag_cal_report_get_old_orientation(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 48); +} + +/** + * @brief Get field new_orientation from mag_cal_report message + * + * @return orientation after calibration. + */ +static inline uint8_t mavlink_msg_mag_cal_report_get_new_orientation(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 49); +} + +/** + * @brief Get field scale_factor from mag_cal_report message + * + * @return field radius correction factor + */ +static inline float mavlink_msg_mag_cal_report_get_scale_factor(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 50); +} + +/** + * @brief Decode a mag_cal_report message into a struct + * + * @param msg The message to decode + * @param mag_cal_report C-struct to decode the message contents into + */ +static inline void mavlink_msg_mag_cal_report_decode(const mavlink_message_t* msg, mavlink_mag_cal_report_t* mag_cal_report) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + mag_cal_report->fitness = mavlink_msg_mag_cal_report_get_fitness(msg); + mag_cal_report->ofs_x = mavlink_msg_mag_cal_report_get_ofs_x(msg); + mag_cal_report->ofs_y = mavlink_msg_mag_cal_report_get_ofs_y(msg); + mag_cal_report->ofs_z = mavlink_msg_mag_cal_report_get_ofs_z(msg); + mag_cal_report->diag_x = mavlink_msg_mag_cal_report_get_diag_x(msg); + mag_cal_report->diag_y = mavlink_msg_mag_cal_report_get_diag_y(msg); + mag_cal_report->diag_z = mavlink_msg_mag_cal_report_get_diag_z(msg); + mag_cal_report->offdiag_x = mavlink_msg_mag_cal_report_get_offdiag_x(msg); + mag_cal_report->offdiag_y = mavlink_msg_mag_cal_report_get_offdiag_y(msg); + mag_cal_report->offdiag_z = mavlink_msg_mag_cal_report_get_offdiag_z(msg); + mag_cal_report->compass_id = mavlink_msg_mag_cal_report_get_compass_id(msg); + mag_cal_report->cal_mask = mavlink_msg_mag_cal_report_get_cal_mask(msg); + mag_cal_report->cal_status = mavlink_msg_mag_cal_report_get_cal_status(msg); + mag_cal_report->autosaved = mavlink_msg_mag_cal_report_get_autosaved(msg); + mag_cal_report->orientation_confidence = mavlink_msg_mag_cal_report_get_orientation_confidence(msg); + mag_cal_report->old_orientation = mavlink_msg_mag_cal_report_get_old_orientation(msg); + mag_cal_report->new_orientation = mavlink_msg_mag_cal_report_get_new_orientation(msg); + mag_cal_report->scale_factor = mavlink_msg_mag_cal_report_get_scale_factor(msg); +#else + uint8_t len = msg->len < MAVLINK_MSG_ID_MAG_CAL_REPORT_LEN? msg->len : MAVLINK_MSG_ID_MAG_CAL_REPORT_LEN; + memset(mag_cal_report, 0, MAVLINK_MSG_ID_MAG_CAL_REPORT_LEN); + memcpy(mag_cal_report, _MAV_PAYLOAD(msg), len); +#endif +} diff --git a/lib/main/MAVLink/common/mavlink_msg_mission_ack.h b/lib/main/MAVLink/common/mavlink_msg_mission_ack.h index 182825a5ea..916b94d38d 100755 --- a/lib/main/MAVLink/common/mavlink_msg_mission_ack.h +++ b/lib/main/MAVLink/common/mavlink_msg_mission_ack.h @@ -8,11 +8,12 @@ typedef struct __mavlink_mission_ack_t { uint8_t target_system; /*< System ID*/ uint8_t target_component; /*< Component ID*/ uint8_t type; /*< Mission result.*/ + uint8_t mission_type; /*< Mission type.*/ } mavlink_mission_ack_t; -#define MAVLINK_MSG_ID_MISSION_ACK_LEN 3 +#define MAVLINK_MSG_ID_MISSION_ACK_LEN 4 #define MAVLINK_MSG_ID_MISSION_ACK_MIN_LEN 3 -#define MAVLINK_MSG_ID_47_LEN 3 +#define MAVLINK_MSG_ID_47_LEN 4 #define MAVLINK_MSG_ID_47_MIN_LEN 3 #define MAVLINK_MSG_ID_MISSION_ACK_CRC 153 @@ -24,19 +25,21 @@ typedef struct __mavlink_mission_ack_t { #define MAVLINK_MESSAGE_INFO_MISSION_ACK { \ 47, \ "MISSION_ACK", \ - 3, \ + 4, \ { { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 0, offsetof(mavlink_mission_ack_t, target_system) }, \ { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 1, offsetof(mavlink_mission_ack_t, target_component) }, \ { "type", NULL, MAVLINK_TYPE_UINT8_T, 0, 2, offsetof(mavlink_mission_ack_t, type) }, \ + { "mission_type", NULL, MAVLINK_TYPE_UINT8_T, 0, 3, offsetof(mavlink_mission_ack_t, mission_type) }, \ } \ } #else #define MAVLINK_MESSAGE_INFO_MISSION_ACK { \ "MISSION_ACK", \ - 3, \ + 4, \ { { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 0, offsetof(mavlink_mission_ack_t, target_system) }, \ { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 1, offsetof(mavlink_mission_ack_t, target_component) }, \ { "type", NULL, MAVLINK_TYPE_UINT8_T, 0, 2, offsetof(mavlink_mission_ack_t, type) }, \ + { "mission_type", NULL, MAVLINK_TYPE_UINT8_T, 0, 3, offsetof(mavlink_mission_ack_t, mission_type) }, \ } \ } #endif @@ -50,16 +53,18 @@ typedef struct __mavlink_mission_ack_t { * @param target_system System ID * @param target_component Component ID * @param type Mission result. + * @param mission_type Mission type. * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_mission_ack_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint8_t target_system, uint8_t target_component, uint8_t type) + uint8_t target_system, uint8_t target_component, uint8_t type, uint8_t mission_type) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_MISSION_ACK_LEN]; _mav_put_uint8_t(buf, 0, target_system); _mav_put_uint8_t(buf, 1, target_component); _mav_put_uint8_t(buf, 2, type); + _mav_put_uint8_t(buf, 3, mission_type); memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_MISSION_ACK_LEN); #else @@ -67,6 +72,7 @@ static inline uint16_t mavlink_msg_mission_ack_pack(uint8_t system_id, uint8_t c packet.target_system = target_system; packet.target_component = target_component; packet.type = type; + packet.mission_type = mission_type; memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_MISSION_ACK_LEN); #endif @@ -84,17 +90,19 @@ static inline uint16_t mavlink_msg_mission_ack_pack(uint8_t system_id, uint8_t c * @param target_system System ID * @param target_component Component ID * @param type Mission result. + * @param mission_type Mission type. * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_mission_ack_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, - uint8_t target_system,uint8_t target_component,uint8_t type) + uint8_t target_system,uint8_t target_component,uint8_t type,uint8_t mission_type) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_MISSION_ACK_LEN]; _mav_put_uint8_t(buf, 0, target_system); _mav_put_uint8_t(buf, 1, target_component); _mav_put_uint8_t(buf, 2, type); + _mav_put_uint8_t(buf, 3, mission_type); memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_MISSION_ACK_LEN); #else @@ -102,6 +110,7 @@ static inline uint16_t mavlink_msg_mission_ack_pack_chan(uint8_t system_id, uint packet.target_system = target_system; packet.target_component = target_component; packet.type = type; + packet.mission_type = mission_type; memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_MISSION_ACK_LEN); #endif @@ -120,7 +129,7 @@ static inline uint16_t mavlink_msg_mission_ack_pack_chan(uint8_t system_id, uint */ static inline uint16_t mavlink_msg_mission_ack_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_mission_ack_t* mission_ack) { - return mavlink_msg_mission_ack_pack(system_id, component_id, msg, mission_ack->target_system, mission_ack->target_component, mission_ack->type); + return mavlink_msg_mission_ack_pack(system_id, component_id, msg, mission_ack->target_system, mission_ack->target_component, mission_ack->type, mission_ack->mission_type); } /** @@ -134,7 +143,7 @@ static inline uint16_t mavlink_msg_mission_ack_encode(uint8_t system_id, uint8_t */ static inline uint16_t mavlink_msg_mission_ack_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_mission_ack_t* mission_ack) { - return mavlink_msg_mission_ack_pack_chan(system_id, component_id, chan, msg, mission_ack->target_system, mission_ack->target_component, mission_ack->type); + return mavlink_msg_mission_ack_pack_chan(system_id, component_id, chan, msg, mission_ack->target_system, mission_ack->target_component, mission_ack->type, mission_ack->mission_type); } /** @@ -144,16 +153,18 @@ static inline uint16_t mavlink_msg_mission_ack_encode_chan(uint8_t system_id, ui * @param target_system System ID * @param target_component Component ID * @param type Mission result. + * @param mission_type Mission type. */ #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS -static inline void mavlink_msg_mission_ack_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint8_t type) +static inline void mavlink_msg_mission_ack_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint8_t type, uint8_t mission_type) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_MISSION_ACK_LEN]; _mav_put_uint8_t(buf, 0, target_system); _mav_put_uint8_t(buf, 1, target_component); _mav_put_uint8_t(buf, 2, type); + _mav_put_uint8_t(buf, 3, mission_type); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_ACK, buf, MAVLINK_MSG_ID_MISSION_ACK_MIN_LEN, MAVLINK_MSG_ID_MISSION_ACK_LEN, MAVLINK_MSG_ID_MISSION_ACK_CRC); #else @@ -161,6 +172,7 @@ static inline void mavlink_msg_mission_ack_send(mavlink_channel_t chan, uint8_t packet.target_system = target_system; packet.target_component = target_component; packet.type = type; + packet.mission_type = mission_type; _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_ACK, (const char *)&packet, MAVLINK_MSG_ID_MISSION_ACK_MIN_LEN, MAVLINK_MSG_ID_MISSION_ACK_LEN, MAVLINK_MSG_ID_MISSION_ACK_CRC); #endif @@ -174,7 +186,7 @@ static inline void mavlink_msg_mission_ack_send(mavlink_channel_t chan, uint8_t static inline void mavlink_msg_mission_ack_send_struct(mavlink_channel_t chan, const mavlink_mission_ack_t* mission_ack) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - mavlink_msg_mission_ack_send(chan, mission_ack->target_system, mission_ack->target_component, mission_ack->type); + mavlink_msg_mission_ack_send(chan, mission_ack->target_system, mission_ack->target_component, mission_ack->type, mission_ack->mission_type); #else _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_ACK, (const char *)mission_ack, MAVLINK_MSG_ID_MISSION_ACK_MIN_LEN, MAVLINK_MSG_ID_MISSION_ACK_LEN, MAVLINK_MSG_ID_MISSION_ACK_CRC); #endif @@ -188,13 +200,14 @@ static inline void mavlink_msg_mission_ack_send_struct(mavlink_channel_t chan, c is usually the receive buffer for the channel, and allows a reply to an incoming message with minimum stack space usage. */ -static inline void mavlink_msg_mission_ack_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint8_t type) +static inline void mavlink_msg_mission_ack_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint8_t type, uint8_t mission_type) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char *buf = (char *)msgbuf; _mav_put_uint8_t(buf, 0, target_system); _mav_put_uint8_t(buf, 1, target_component); _mav_put_uint8_t(buf, 2, type); + _mav_put_uint8_t(buf, 3, mission_type); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_ACK, buf, MAVLINK_MSG_ID_MISSION_ACK_MIN_LEN, MAVLINK_MSG_ID_MISSION_ACK_LEN, MAVLINK_MSG_ID_MISSION_ACK_CRC); #else @@ -202,6 +215,7 @@ static inline void mavlink_msg_mission_ack_send_buf(mavlink_message_t *msgbuf, m packet->target_system = target_system; packet->target_component = target_component; packet->type = type; + packet->mission_type = mission_type; _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_ACK, (const char *)packet, MAVLINK_MSG_ID_MISSION_ACK_MIN_LEN, MAVLINK_MSG_ID_MISSION_ACK_LEN, MAVLINK_MSG_ID_MISSION_ACK_CRC); #endif @@ -243,6 +257,16 @@ static inline uint8_t mavlink_msg_mission_ack_get_type(const mavlink_message_t* return _MAV_RETURN_uint8_t(msg, 2); } +/** + * @brief Get field mission_type from mission_ack message + * + * @return Mission type. + */ +static inline uint8_t mavlink_msg_mission_ack_get_mission_type(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 3); +} + /** * @brief Decode a mission_ack message into a struct * @@ -255,6 +279,7 @@ static inline void mavlink_msg_mission_ack_decode(const mavlink_message_t* msg, mission_ack->target_system = mavlink_msg_mission_ack_get_target_system(msg); mission_ack->target_component = mavlink_msg_mission_ack_get_target_component(msg); mission_ack->type = mavlink_msg_mission_ack_get_type(msg); + mission_ack->mission_type = mavlink_msg_mission_ack_get_mission_type(msg); #else uint8_t len = msg->len < MAVLINK_MSG_ID_MISSION_ACK_LEN? msg->len : MAVLINK_MSG_ID_MISSION_ACK_LEN; memset(mission_ack, 0, MAVLINK_MSG_ID_MISSION_ACK_LEN); diff --git a/lib/main/MAVLink/common/mavlink_msg_mission_clear_all.h b/lib/main/MAVLink/common/mavlink_msg_mission_clear_all.h index bfed2d3fc2..1e585f9fca 100755 --- a/lib/main/MAVLink/common/mavlink_msg_mission_clear_all.h +++ b/lib/main/MAVLink/common/mavlink_msg_mission_clear_all.h @@ -7,11 +7,12 @@ typedef struct __mavlink_mission_clear_all_t { uint8_t target_system; /*< System ID*/ uint8_t target_component; /*< Component ID*/ + uint8_t mission_type; /*< Mission type.*/ } mavlink_mission_clear_all_t; -#define MAVLINK_MSG_ID_MISSION_CLEAR_ALL_LEN 2 +#define MAVLINK_MSG_ID_MISSION_CLEAR_ALL_LEN 3 #define MAVLINK_MSG_ID_MISSION_CLEAR_ALL_MIN_LEN 2 -#define MAVLINK_MSG_ID_45_LEN 2 +#define MAVLINK_MSG_ID_45_LEN 3 #define MAVLINK_MSG_ID_45_MIN_LEN 2 #define MAVLINK_MSG_ID_MISSION_CLEAR_ALL_CRC 232 @@ -23,17 +24,19 @@ typedef struct __mavlink_mission_clear_all_t { #define MAVLINK_MESSAGE_INFO_MISSION_CLEAR_ALL { \ 45, \ "MISSION_CLEAR_ALL", \ - 2, \ + 3, \ { { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 0, offsetof(mavlink_mission_clear_all_t, target_system) }, \ { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 1, offsetof(mavlink_mission_clear_all_t, target_component) }, \ + { "mission_type", NULL, MAVLINK_TYPE_UINT8_T, 0, 2, offsetof(mavlink_mission_clear_all_t, mission_type) }, \ } \ } #else #define MAVLINK_MESSAGE_INFO_MISSION_CLEAR_ALL { \ "MISSION_CLEAR_ALL", \ - 2, \ + 3, \ { { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 0, offsetof(mavlink_mission_clear_all_t, target_system) }, \ { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 1, offsetof(mavlink_mission_clear_all_t, target_component) }, \ + { "mission_type", NULL, MAVLINK_TYPE_UINT8_T, 0, 2, offsetof(mavlink_mission_clear_all_t, mission_type) }, \ } \ } #endif @@ -46,21 +49,24 @@ typedef struct __mavlink_mission_clear_all_t { * * @param target_system System ID * @param target_component Component ID + * @param mission_type Mission type. * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_mission_clear_all_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint8_t target_system, uint8_t target_component) + uint8_t target_system, uint8_t target_component, uint8_t mission_type) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_MISSION_CLEAR_ALL_LEN]; _mav_put_uint8_t(buf, 0, target_system); _mav_put_uint8_t(buf, 1, target_component); + _mav_put_uint8_t(buf, 2, mission_type); memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_MISSION_CLEAR_ALL_LEN); #else mavlink_mission_clear_all_t packet; packet.target_system = target_system; packet.target_component = target_component; + packet.mission_type = mission_type; memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_MISSION_CLEAR_ALL_LEN); #endif @@ -77,22 +83,25 @@ static inline uint16_t mavlink_msg_mission_clear_all_pack(uint8_t system_id, uin * @param msg The MAVLink message to compress the data into * @param target_system System ID * @param target_component Component ID + * @param mission_type Mission type. * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_mission_clear_all_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, - uint8_t target_system,uint8_t target_component) + uint8_t target_system,uint8_t target_component,uint8_t mission_type) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_MISSION_CLEAR_ALL_LEN]; _mav_put_uint8_t(buf, 0, target_system); _mav_put_uint8_t(buf, 1, target_component); + _mav_put_uint8_t(buf, 2, mission_type); memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_MISSION_CLEAR_ALL_LEN); #else mavlink_mission_clear_all_t packet; packet.target_system = target_system; packet.target_component = target_component; + packet.mission_type = mission_type; memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_MISSION_CLEAR_ALL_LEN); #endif @@ -111,7 +120,7 @@ static inline uint16_t mavlink_msg_mission_clear_all_pack_chan(uint8_t system_id */ static inline uint16_t mavlink_msg_mission_clear_all_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_mission_clear_all_t* mission_clear_all) { - return mavlink_msg_mission_clear_all_pack(system_id, component_id, msg, mission_clear_all->target_system, mission_clear_all->target_component); + return mavlink_msg_mission_clear_all_pack(system_id, component_id, msg, mission_clear_all->target_system, mission_clear_all->target_component, mission_clear_all->mission_type); } /** @@ -125,7 +134,7 @@ static inline uint16_t mavlink_msg_mission_clear_all_encode(uint8_t system_id, u */ static inline uint16_t mavlink_msg_mission_clear_all_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_mission_clear_all_t* mission_clear_all) { - return mavlink_msg_mission_clear_all_pack_chan(system_id, component_id, chan, msg, mission_clear_all->target_system, mission_clear_all->target_component); + return mavlink_msg_mission_clear_all_pack_chan(system_id, component_id, chan, msg, mission_clear_all->target_system, mission_clear_all->target_component, mission_clear_all->mission_type); } /** @@ -134,21 +143,24 @@ static inline uint16_t mavlink_msg_mission_clear_all_encode_chan(uint8_t system_ * * @param target_system System ID * @param target_component Component ID + * @param mission_type Mission type. */ #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS -static inline void mavlink_msg_mission_clear_all_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component) +static inline void mavlink_msg_mission_clear_all_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint8_t mission_type) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_MISSION_CLEAR_ALL_LEN]; _mav_put_uint8_t(buf, 0, target_system); _mav_put_uint8_t(buf, 1, target_component); + _mav_put_uint8_t(buf, 2, mission_type); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_CLEAR_ALL, buf, MAVLINK_MSG_ID_MISSION_CLEAR_ALL_MIN_LEN, MAVLINK_MSG_ID_MISSION_CLEAR_ALL_LEN, MAVLINK_MSG_ID_MISSION_CLEAR_ALL_CRC); #else mavlink_mission_clear_all_t packet; packet.target_system = target_system; packet.target_component = target_component; + packet.mission_type = mission_type; _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_CLEAR_ALL, (const char *)&packet, MAVLINK_MSG_ID_MISSION_CLEAR_ALL_MIN_LEN, MAVLINK_MSG_ID_MISSION_CLEAR_ALL_LEN, MAVLINK_MSG_ID_MISSION_CLEAR_ALL_CRC); #endif @@ -162,7 +174,7 @@ static inline void mavlink_msg_mission_clear_all_send(mavlink_channel_t chan, ui static inline void mavlink_msg_mission_clear_all_send_struct(mavlink_channel_t chan, const mavlink_mission_clear_all_t* mission_clear_all) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - mavlink_msg_mission_clear_all_send(chan, mission_clear_all->target_system, mission_clear_all->target_component); + mavlink_msg_mission_clear_all_send(chan, mission_clear_all->target_system, mission_clear_all->target_component, mission_clear_all->mission_type); #else _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_CLEAR_ALL, (const char *)mission_clear_all, MAVLINK_MSG_ID_MISSION_CLEAR_ALL_MIN_LEN, MAVLINK_MSG_ID_MISSION_CLEAR_ALL_LEN, MAVLINK_MSG_ID_MISSION_CLEAR_ALL_CRC); #endif @@ -176,18 +188,20 @@ static inline void mavlink_msg_mission_clear_all_send_struct(mavlink_channel_t c is usually the receive buffer for the channel, and allows a reply to an incoming message with minimum stack space usage. */ -static inline void mavlink_msg_mission_clear_all_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target_system, uint8_t target_component) +static inline void mavlink_msg_mission_clear_all_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint8_t mission_type) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char *buf = (char *)msgbuf; _mav_put_uint8_t(buf, 0, target_system); _mav_put_uint8_t(buf, 1, target_component); + _mav_put_uint8_t(buf, 2, mission_type); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_CLEAR_ALL, buf, MAVLINK_MSG_ID_MISSION_CLEAR_ALL_MIN_LEN, MAVLINK_MSG_ID_MISSION_CLEAR_ALL_LEN, MAVLINK_MSG_ID_MISSION_CLEAR_ALL_CRC); #else mavlink_mission_clear_all_t *packet = (mavlink_mission_clear_all_t *)msgbuf; packet->target_system = target_system; packet->target_component = target_component; + packet->mission_type = mission_type; _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_CLEAR_ALL, (const char *)packet, MAVLINK_MSG_ID_MISSION_CLEAR_ALL_MIN_LEN, MAVLINK_MSG_ID_MISSION_CLEAR_ALL_LEN, MAVLINK_MSG_ID_MISSION_CLEAR_ALL_CRC); #endif @@ -219,6 +233,16 @@ static inline uint8_t mavlink_msg_mission_clear_all_get_target_component(const m return _MAV_RETURN_uint8_t(msg, 1); } +/** + * @brief Get field mission_type from mission_clear_all message + * + * @return Mission type. + */ +static inline uint8_t mavlink_msg_mission_clear_all_get_mission_type(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 2); +} + /** * @brief Decode a mission_clear_all message into a struct * @@ -230,6 +254,7 @@ static inline void mavlink_msg_mission_clear_all_decode(const mavlink_message_t* #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS mission_clear_all->target_system = mavlink_msg_mission_clear_all_get_target_system(msg); mission_clear_all->target_component = mavlink_msg_mission_clear_all_get_target_component(msg); + mission_clear_all->mission_type = mavlink_msg_mission_clear_all_get_mission_type(msg); #else uint8_t len = msg->len < MAVLINK_MSG_ID_MISSION_CLEAR_ALL_LEN? msg->len : MAVLINK_MSG_ID_MISSION_CLEAR_ALL_LEN; memset(mission_clear_all, 0, MAVLINK_MSG_ID_MISSION_CLEAR_ALL_LEN); diff --git a/lib/main/MAVLink/common/mavlink_msg_mission_count.h b/lib/main/MAVLink/common/mavlink_msg_mission_count.h index 8c0cd1c6d0..53afd49002 100755 --- a/lib/main/MAVLink/common/mavlink_msg_mission_count.h +++ b/lib/main/MAVLink/common/mavlink_msg_mission_count.h @@ -8,11 +8,12 @@ typedef struct __mavlink_mission_count_t { uint16_t count; /*< Number of mission items in the sequence*/ uint8_t target_system; /*< System ID*/ uint8_t target_component; /*< Component ID*/ + uint8_t mission_type; /*< Mission type.*/ } mavlink_mission_count_t; -#define MAVLINK_MSG_ID_MISSION_COUNT_LEN 4 +#define MAVLINK_MSG_ID_MISSION_COUNT_LEN 5 #define MAVLINK_MSG_ID_MISSION_COUNT_MIN_LEN 4 -#define MAVLINK_MSG_ID_44_LEN 4 +#define MAVLINK_MSG_ID_44_LEN 5 #define MAVLINK_MSG_ID_44_MIN_LEN 4 #define MAVLINK_MSG_ID_MISSION_COUNT_CRC 221 @@ -24,19 +25,21 @@ typedef struct __mavlink_mission_count_t { #define MAVLINK_MESSAGE_INFO_MISSION_COUNT { \ 44, \ "MISSION_COUNT", \ - 3, \ + 4, \ { { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 2, offsetof(mavlink_mission_count_t, target_system) }, \ { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 3, offsetof(mavlink_mission_count_t, target_component) }, \ { "count", NULL, MAVLINK_TYPE_UINT16_T, 0, 0, offsetof(mavlink_mission_count_t, count) }, \ + { "mission_type", NULL, MAVLINK_TYPE_UINT8_T, 0, 4, offsetof(mavlink_mission_count_t, mission_type) }, \ } \ } #else #define MAVLINK_MESSAGE_INFO_MISSION_COUNT { \ "MISSION_COUNT", \ - 3, \ + 4, \ { { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 2, offsetof(mavlink_mission_count_t, target_system) }, \ { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 3, offsetof(mavlink_mission_count_t, target_component) }, \ { "count", NULL, MAVLINK_TYPE_UINT16_T, 0, 0, offsetof(mavlink_mission_count_t, count) }, \ + { "mission_type", NULL, MAVLINK_TYPE_UINT8_T, 0, 4, offsetof(mavlink_mission_count_t, mission_type) }, \ } \ } #endif @@ -50,16 +53,18 @@ typedef struct __mavlink_mission_count_t { * @param target_system System ID * @param target_component Component ID * @param count Number of mission items in the sequence + * @param mission_type Mission type. * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_mission_count_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint8_t target_system, uint8_t target_component, uint16_t count) + uint8_t target_system, uint8_t target_component, uint16_t count, uint8_t mission_type) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_MISSION_COUNT_LEN]; _mav_put_uint16_t(buf, 0, count); _mav_put_uint8_t(buf, 2, target_system); _mav_put_uint8_t(buf, 3, target_component); + _mav_put_uint8_t(buf, 4, mission_type); memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_MISSION_COUNT_LEN); #else @@ -67,6 +72,7 @@ static inline uint16_t mavlink_msg_mission_count_pack(uint8_t system_id, uint8_t packet.count = count; packet.target_system = target_system; packet.target_component = target_component; + packet.mission_type = mission_type; memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_MISSION_COUNT_LEN); #endif @@ -84,17 +90,19 @@ static inline uint16_t mavlink_msg_mission_count_pack(uint8_t system_id, uint8_t * @param target_system System ID * @param target_component Component ID * @param count Number of mission items in the sequence + * @param mission_type Mission type. * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_mission_count_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, - uint8_t target_system,uint8_t target_component,uint16_t count) + uint8_t target_system,uint8_t target_component,uint16_t count,uint8_t mission_type) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_MISSION_COUNT_LEN]; _mav_put_uint16_t(buf, 0, count); _mav_put_uint8_t(buf, 2, target_system); _mav_put_uint8_t(buf, 3, target_component); + _mav_put_uint8_t(buf, 4, mission_type); memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_MISSION_COUNT_LEN); #else @@ -102,6 +110,7 @@ static inline uint16_t mavlink_msg_mission_count_pack_chan(uint8_t system_id, ui packet.count = count; packet.target_system = target_system; packet.target_component = target_component; + packet.mission_type = mission_type; memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_MISSION_COUNT_LEN); #endif @@ -120,7 +129,7 @@ static inline uint16_t mavlink_msg_mission_count_pack_chan(uint8_t system_id, ui */ static inline uint16_t mavlink_msg_mission_count_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_mission_count_t* mission_count) { - return mavlink_msg_mission_count_pack(system_id, component_id, msg, mission_count->target_system, mission_count->target_component, mission_count->count); + return mavlink_msg_mission_count_pack(system_id, component_id, msg, mission_count->target_system, mission_count->target_component, mission_count->count, mission_count->mission_type); } /** @@ -134,7 +143,7 @@ static inline uint16_t mavlink_msg_mission_count_encode(uint8_t system_id, uint8 */ static inline uint16_t mavlink_msg_mission_count_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_mission_count_t* mission_count) { - return mavlink_msg_mission_count_pack_chan(system_id, component_id, chan, msg, mission_count->target_system, mission_count->target_component, mission_count->count); + return mavlink_msg_mission_count_pack_chan(system_id, component_id, chan, msg, mission_count->target_system, mission_count->target_component, mission_count->count, mission_count->mission_type); } /** @@ -144,16 +153,18 @@ static inline uint16_t mavlink_msg_mission_count_encode_chan(uint8_t system_id, * @param target_system System ID * @param target_component Component ID * @param count Number of mission items in the sequence + * @param mission_type Mission type. */ #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS -static inline void mavlink_msg_mission_count_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint16_t count) +static inline void mavlink_msg_mission_count_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint16_t count, uint8_t mission_type) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_MISSION_COUNT_LEN]; _mav_put_uint16_t(buf, 0, count); _mav_put_uint8_t(buf, 2, target_system); _mav_put_uint8_t(buf, 3, target_component); + _mav_put_uint8_t(buf, 4, mission_type); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_COUNT, buf, MAVLINK_MSG_ID_MISSION_COUNT_MIN_LEN, MAVLINK_MSG_ID_MISSION_COUNT_LEN, MAVLINK_MSG_ID_MISSION_COUNT_CRC); #else @@ -161,6 +172,7 @@ static inline void mavlink_msg_mission_count_send(mavlink_channel_t chan, uint8_ packet.count = count; packet.target_system = target_system; packet.target_component = target_component; + packet.mission_type = mission_type; _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_COUNT, (const char *)&packet, MAVLINK_MSG_ID_MISSION_COUNT_MIN_LEN, MAVLINK_MSG_ID_MISSION_COUNT_LEN, MAVLINK_MSG_ID_MISSION_COUNT_CRC); #endif @@ -174,7 +186,7 @@ static inline void mavlink_msg_mission_count_send(mavlink_channel_t chan, uint8_ static inline void mavlink_msg_mission_count_send_struct(mavlink_channel_t chan, const mavlink_mission_count_t* mission_count) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - mavlink_msg_mission_count_send(chan, mission_count->target_system, mission_count->target_component, mission_count->count); + mavlink_msg_mission_count_send(chan, mission_count->target_system, mission_count->target_component, mission_count->count, mission_count->mission_type); #else _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_COUNT, (const char *)mission_count, MAVLINK_MSG_ID_MISSION_COUNT_MIN_LEN, MAVLINK_MSG_ID_MISSION_COUNT_LEN, MAVLINK_MSG_ID_MISSION_COUNT_CRC); #endif @@ -188,13 +200,14 @@ static inline void mavlink_msg_mission_count_send_struct(mavlink_channel_t chan, is usually the receive buffer for the channel, and allows a reply to an incoming message with minimum stack space usage. */ -static inline void mavlink_msg_mission_count_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint16_t count) +static inline void mavlink_msg_mission_count_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint16_t count, uint8_t mission_type) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char *buf = (char *)msgbuf; _mav_put_uint16_t(buf, 0, count); _mav_put_uint8_t(buf, 2, target_system); _mav_put_uint8_t(buf, 3, target_component); + _mav_put_uint8_t(buf, 4, mission_type); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_COUNT, buf, MAVLINK_MSG_ID_MISSION_COUNT_MIN_LEN, MAVLINK_MSG_ID_MISSION_COUNT_LEN, MAVLINK_MSG_ID_MISSION_COUNT_CRC); #else @@ -202,6 +215,7 @@ static inline void mavlink_msg_mission_count_send_buf(mavlink_message_t *msgbuf, packet->count = count; packet->target_system = target_system; packet->target_component = target_component; + packet->mission_type = mission_type; _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_COUNT, (const char *)packet, MAVLINK_MSG_ID_MISSION_COUNT_MIN_LEN, MAVLINK_MSG_ID_MISSION_COUNT_LEN, MAVLINK_MSG_ID_MISSION_COUNT_CRC); #endif @@ -243,6 +257,16 @@ static inline uint16_t mavlink_msg_mission_count_get_count(const mavlink_message return _MAV_RETURN_uint16_t(msg, 0); } +/** + * @brief Get field mission_type from mission_count message + * + * @return Mission type. + */ +static inline uint8_t mavlink_msg_mission_count_get_mission_type(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 4); +} + /** * @brief Decode a mission_count message into a struct * @@ -255,6 +279,7 @@ static inline void mavlink_msg_mission_count_decode(const mavlink_message_t* msg mission_count->count = mavlink_msg_mission_count_get_count(msg); mission_count->target_system = mavlink_msg_mission_count_get_target_system(msg); mission_count->target_component = mavlink_msg_mission_count_get_target_component(msg); + mission_count->mission_type = mavlink_msg_mission_count_get_mission_type(msg); #else uint8_t len = msg->len < MAVLINK_MSG_ID_MISSION_COUNT_LEN? msg->len : MAVLINK_MSG_ID_MISSION_COUNT_LEN; memset(mission_count, 0, MAVLINK_MSG_ID_MISSION_COUNT_LEN); diff --git a/lib/main/MAVLink/common/mavlink_msg_mission_item.h b/lib/main/MAVLink/common/mavlink_msg_mission_item.h index 0a50b542a5..b14d151b28 100755 --- a/lib/main/MAVLink/common/mavlink_msg_mission_item.h +++ b/lib/main/MAVLink/common/mavlink_msg_mission_item.h @@ -19,11 +19,12 @@ typedef struct __mavlink_mission_item_t { uint8_t frame; /*< The coordinate system of the waypoint.*/ uint8_t current; /*< false:0, true:1*/ uint8_t autocontinue; /*< Autocontinue to next waypoint*/ + uint8_t mission_type; /*< Mission type.*/ } mavlink_mission_item_t; -#define MAVLINK_MSG_ID_MISSION_ITEM_LEN 37 +#define MAVLINK_MSG_ID_MISSION_ITEM_LEN 38 #define MAVLINK_MSG_ID_MISSION_ITEM_MIN_LEN 37 -#define MAVLINK_MSG_ID_39_LEN 37 +#define MAVLINK_MSG_ID_39_LEN 38 #define MAVLINK_MSG_ID_39_MIN_LEN 37 #define MAVLINK_MSG_ID_MISSION_ITEM_CRC 254 @@ -35,7 +36,7 @@ typedef struct __mavlink_mission_item_t { #define MAVLINK_MESSAGE_INFO_MISSION_ITEM { \ 39, \ "MISSION_ITEM", \ - 14, \ + 15, \ { { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 32, offsetof(mavlink_mission_item_t, target_system) }, \ { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 33, offsetof(mavlink_mission_item_t, target_component) }, \ { "seq", NULL, MAVLINK_TYPE_UINT16_T, 0, 28, offsetof(mavlink_mission_item_t, seq) }, \ @@ -50,12 +51,13 @@ typedef struct __mavlink_mission_item_t { { "x", NULL, MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_mission_item_t, x) }, \ { "y", NULL, MAVLINK_TYPE_FLOAT, 0, 20, offsetof(mavlink_mission_item_t, y) }, \ { "z", NULL, MAVLINK_TYPE_FLOAT, 0, 24, offsetof(mavlink_mission_item_t, z) }, \ + { "mission_type", NULL, MAVLINK_TYPE_UINT8_T, 0, 37, offsetof(mavlink_mission_item_t, mission_type) }, \ } \ } #else #define MAVLINK_MESSAGE_INFO_MISSION_ITEM { \ "MISSION_ITEM", \ - 14, \ + 15, \ { { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 32, offsetof(mavlink_mission_item_t, target_system) }, \ { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 33, offsetof(mavlink_mission_item_t, target_component) }, \ { "seq", NULL, MAVLINK_TYPE_UINT16_T, 0, 28, offsetof(mavlink_mission_item_t, seq) }, \ @@ -70,6 +72,7 @@ typedef struct __mavlink_mission_item_t { { "x", NULL, MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_mission_item_t, x) }, \ { "y", NULL, MAVLINK_TYPE_FLOAT, 0, 20, offsetof(mavlink_mission_item_t, y) }, \ { "z", NULL, MAVLINK_TYPE_FLOAT, 0, 24, offsetof(mavlink_mission_item_t, z) }, \ + { "mission_type", NULL, MAVLINK_TYPE_UINT8_T, 0, 37, offsetof(mavlink_mission_item_t, mission_type) }, \ } \ } #endif @@ -94,10 +97,11 @@ typedef struct __mavlink_mission_item_t { * @param x PARAM5 / local: X coordinate, global: latitude * @param y PARAM6 / local: Y coordinate, global: longitude * @param z PARAM7 / local: Z coordinate, global: altitude (relative or absolute, depending on frame). + * @param mission_type Mission type. * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_mission_item_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint8_t target_system, uint8_t target_component, uint16_t seq, uint8_t frame, uint16_t command, uint8_t current, uint8_t autocontinue, float param1, float param2, float param3, float param4, float x, float y, float z) + uint8_t target_system, uint8_t target_component, uint16_t seq, uint8_t frame, uint16_t command, uint8_t current, uint8_t autocontinue, float param1, float param2, float param3, float param4, float x, float y, float z, uint8_t mission_type) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_MISSION_ITEM_LEN]; @@ -115,6 +119,7 @@ static inline uint16_t mavlink_msg_mission_item_pack(uint8_t system_id, uint8_t _mav_put_uint8_t(buf, 34, frame); _mav_put_uint8_t(buf, 35, current); _mav_put_uint8_t(buf, 36, autocontinue); + _mav_put_uint8_t(buf, 37, mission_type); memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_MISSION_ITEM_LEN); #else @@ -133,6 +138,7 @@ static inline uint16_t mavlink_msg_mission_item_pack(uint8_t system_id, uint8_t packet.frame = frame; packet.current = current; packet.autocontinue = autocontinue; + packet.mission_type = mission_type; memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_MISSION_ITEM_LEN); #endif @@ -161,11 +167,12 @@ static inline uint16_t mavlink_msg_mission_item_pack(uint8_t system_id, uint8_t * @param x PARAM5 / local: X coordinate, global: latitude * @param y PARAM6 / local: Y coordinate, global: longitude * @param z PARAM7 / local: Z coordinate, global: altitude (relative or absolute, depending on frame). + * @param mission_type Mission type. * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_mission_item_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, - uint8_t target_system,uint8_t target_component,uint16_t seq,uint8_t frame,uint16_t command,uint8_t current,uint8_t autocontinue,float param1,float param2,float param3,float param4,float x,float y,float z) + uint8_t target_system,uint8_t target_component,uint16_t seq,uint8_t frame,uint16_t command,uint8_t current,uint8_t autocontinue,float param1,float param2,float param3,float param4,float x,float y,float z,uint8_t mission_type) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_MISSION_ITEM_LEN]; @@ -183,6 +190,7 @@ static inline uint16_t mavlink_msg_mission_item_pack_chan(uint8_t system_id, uin _mav_put_uint8_t(buf, 34, frame); _mav_put_uint8_t(buf, 35, current); _mav_put_uint8_t(buf, 36, autocontinue); + _mav_put_uint8_t(buf, 37, mission_type); memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_MISSION_ITEM_LEN); #else @@ -201,6 +209,7 @@ static inline uint16_t mavlink_msg_mission_item_pack_chan(uint8_t system_id, uin packet.frame = frame; packet.current = current; packet.autocontinue = autocontinue; + packet.mission_type = mission_type; memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_MISSION_ITEM_LEN); #endif @@ -219,7 +228,7 @@ static inline uint16_t mavlink_msg_mission_item_pack_chan(uint8_t system_id, uin */ static inline uint16_t mavlink_msg_mission_item_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_mission_item_t* mission_item) { - return mavlink_msg_mission_item_pack(system_id, component_id, msg, mission_item->target_system, mission_item->target_component, mission_item->seq, mission_item->frame, mission_item->command, mission_item->current, mission_item->autocontinue, mission_item->param1, mission_item->param2, mission_item->param3, mission_item->param4, mission_item->x, mission_item->y, mission_item->z); + return mavlink_msg_mission_item_pack(system_id, component_id, msg, mission_item->target_system, mission_item->target_component, mission_item->seq, mission_item->frame, mission_item->command, mission_item->current, mission_item->autocontinue, mission_item->param1, mission_item->param2, mission_item->param3, mission_item->param4, mission_item->x, mission_item->y, mission_item->z, mission_item->mission_type); } /** @@ -233,7 +242,7 @@ static inline uint16_t mavlink_msg_mission_item_encode(uint8_t system_id, uint8_ */ static inline uint16_t mavlink_msg_mission_item_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_mission_item_t* mission_item) { - return mavlink_msg_mission_item_pack_chan(system_id, component_id, chan, msg, mission_item->target_system, mission_item->target_component, mission_item->seq, mission_item->frame, mission_item->command, mission_item->current, mission_item->autocontinue, mission_item->param1, mission_item->param2, mission_item->param3, mission_item->param4, mission_item->x, mission_item->y, mission_item->z); + return mavlink_msg_mission_item_pack_chan(system_id, component_id, chan, msg, mission_item->target_system, mission_item->target_component, mission_item->seq, mission_item->frame, mission_item->command, mission_item->current, mission_item->autocontinue, mission_item->param1, mission_item->param2, mission_item->param3, mission_item->param4, mission_item->x, mission_item->y, mission_item->z, mission_item->mission_type); } /** @@ -254,10 +263,11 @@ static inline uint16_t mavlink_msg_mission_item_encode_chan(uint8_t system_id, u * @param x PARAM5 / local: X coordinate, global: latitude * @param y PARAM6 / local: Y coordinate, global: longitude * @param z PARAM7 / local: Z coordinate, global: altitude (relative or absolute, depending on frame). + * @param mission_type Mission type. */ #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS -static inline void mavlink_msg_mission_item_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint16_t seq, uint8_t frame, uint16_t command, uint8_t current, uint8_t autocontinue, float param1, float param2, float param3, float param4, float x, float y, float z) +static inline void mavlink_msg_mission_item_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint16_t seq, uint8_t frame, uint16_t command, uint8_t current, uint8_t autocontinue, float param1, float param2, float param3, float param4, float x, float y, float z, uint8_t mission_type) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_MISSION_ITEM_LEN]; @@ -275,6 +285,7 @@ static inline void mavlink_msg_mission_item_send(mavlink_channel_t chan, uint8_t _mav_put_uint8_t(buf, 34, frame); _mav_put_uint8_t(buf, 35, current); _mav_put_uint8_t(buf, 36, autocontinue); + _mav_put_uint8_t(buf, 37, mission_type); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_ITEM, buf, MAVLINK_MSG_ID_MISSION_ITEM_MIN_LEN, MAVLINK_MSG_ID_MISSION_ITEM_LEN, MAVLINK_MSG_ID_MISSION_ITEM_CRC); #else @@ -293,6 +304,7 @@ static inline void mavlink_msg_mission_item_send(mavlink_channel_t chan, uint8_t packet.frame = frame; packet.current = current; packet.autocontinue = autocontinue; + packet.mission_type = mission_type; _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_ITEM, (const char *)&packet, MAVLINK_MSG_ID_MISSION_ITEM_MIN_LEN, MAVLINK_MSG_ID_MISSION_ITEM_LEN, MAVLINK_MSG_ID_MISSION_ITEM_CRC); #endif @@ -306,7 +318,7 @@ static inline void mavlink_msg_mission_item_send(mavlink_channel_t chan, uint8_t static inline void mavlink_msg_mission_item_send_struct(mavlink_channel_t chan, const mavlink_mission_item_t* mission_item) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - mavlink_msg_mission_item_send(chan, mission_item->target_system, mission_item->target_component, mission_item->seq, mission_item->frame, mission_item->command, mission_item->current, mission_item->autocontinue, mission_item->param1, mission_item->param2, mission_item->param3, mission_item->param4, mission_item->x, mission_item->y, mission_item->z); + mavlink_msg_mission_item_send(chan, mission_item->target_system, mission_item->target_component, mission_item->seq, mission_item->frame, mission_item->command, mission_item->current, mission_item->autocontinue, mission_item->param1, mission_item->param2, mission_item->param3, mission_item->param4, mission_item->x, mission_item->y, mission_item->z, mission_item->mission_type); #else _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_ITEM, (const char *)mission_item, MAVLINK_MSG_ID_MISSION_ITEM_MIN_LEN, MAVLINK_MSG_ID_MISSION_ITEM_LEN, MAVLINK_MSG_ID_MISSION_ITEM_CRC); #endif @@ -320,7 +332,7 @@ static inline void mavlink_msg_mission_item_send_struct(mavlink_channel_t chan, is usually the receive buffer for the channel, and allows a reply to an incoming message with minimum stack space usage. */ -static inline void mavlink_msg_mission_item_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint16_t seq, uint8_t frame, uint16_t command, uint8_t current, uint8_t autocontinue, float param1, float param2, float param3, float param4, float x, float y, float z) +static inline void mavlink_msg_mission_item_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint16_t seq, uint8_t frame, uint16_t command, uint8_t current, uint8_t autocontinue, float param1, float param2, float param3, float param4, float x, float y, float z, uint8_t mission_type) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char *buf = (char *)msgbuf; @@ -338,6 +350,7 @@ static inline void mavlink_msg_mission_item_send_buf(mavlink_message_t *msgbuf, _mav_put_uint8_t(buf, 34, frame); _mav_put_uint8_t(buf, 35, current); _mav_put_uint8_t(buf, 36, autocontinue); + _mav_put_uint8_t(buf, 37, mission_type); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_ITEM, buf, MAVLINK_MSG_ID_MISSION_ITEM_MIN_LEN, MAVLINK_MSG_ID_MISSION_ITEM_LEN, MAVLINK_MSG_ID_MISSION_ITEM_CRC); #else @@ -356,6 +369,7 @@ static inline void mavlink_msg_mission_item_send_buf(mavlink_message_t *msgbuf, packet->frame = frame; packet->current = current; packet->autocontinue = autocontinue; + packet->mission_type = mission_type; _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_ITEM, (const char *)packet, MAVLINK_MSG_ID_MISSION_ITEM_MIN_LEN, MAVLINK_MSG_ID_MISSION_ITEM_LEN, MAVLINK_MSG_ID_MISSION_ITEM_CRC); #endif @@ -507,6 +521,16 @@ static inline float mavlink_msg_mission_item_get_z(const mavlink_message_t* msg) return _MAV_RETURN_float(msg, 24); } +/** + * @brief Get field mission_type from mission_item message + * + * @return Mission type. + */ +static inline uint8_t mavlink_msg_mission_item_get_mission_type(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 37); +} + /** * @brief Decode a mission_item message into a struct * @@ -530,6 +554,7 @@ static inline void mavlink_msg_mission_item_decode(const mavlink_message_t* msg, mission_item->frame = mavlink_msg_mission_item_get_frame(msg); mission_item->current = mavlink_msg_mission_item_get_current(msg); mission_item->autocontinue = mavlink_msg_mission_item_get_autocontinue(msg); + mission_item->mission_type = mavlink_msg_mission_item_get_mission_type(msg); #else uint8_t len = msg->len < MAVLINK_MSG_ID_MISSION_ITEM_LEN? msg->len : MAVLINK_MSG_ID_MISSION_ITEM_LEN; memset(mission_item, 0, MAVLINK_MSG_ID_MISSION_ITEM_LEN); diff --git a/lib/main/MAVLink/common/mavlink_msg_mission_item_int.h b/lib/main/MAVLink/common/mavlink_msg_mission_item_int.h index 64572671c9..74ad3a5ed5 100755 --- a/lib/main/MAVLink/common/mavlink_msg_mission_item_int.h +++ b/lib/main/MAVLink/common/mavlink_msg_mission_item_int.h @@ -19,11 +19,12 @@ typedef struct __mavlink_mission_item_int_t { uint8_t frame; /*< The coordinate system of the waypoint.*/ uint8_t current; /*< false:0, true:1*/ uint8_t autocontinue; /*< Autocontinue to next waypoint*/ + uint8_t mission_type; /*< Mission type.*/ } mavlink_mission_item_int_t; -#define MAVLINK_MSG_ID_MISSION_ITEM_INT_LEN 37 +#define MAVLINK_MSG_ID_MISSION_ITEM_INT_LEN 38 #define MAVLINK_MSG_ID_MISSION_ITEM_INT_MIN_LEN 37 -#define MAVLINK_MSG_ID_73_LEN 37 +#define MAVLINK_MSG_ID_73_LEN 38 #define MAVLINK_MSG_ID_73_MIN_LEN 37 #define MAVLINK_MSG_ID_MISSION_ITEM_INT_CRC 38 @@ -35,7 +36,7 @@ typedef struct __mavlink_mission_item_int_t { #define MAVLINK_MESSAGE_INFO_MISSION_ITEM_INT { \ 73, \ "MISSION_ITEM_INT", \ - 14, \ + 15, \ { { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 32, offsetof(mavlink_mission_item_int_t, target_system) }, \ { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 33, offsetof(mavlink_mission_item_int_t, target_component) }, \ { "seq", NULL, MAVLINK_TYPE_UINT16_T, 0, 28, offsetof(mavlink_mission_item_int_t, seq) }, \ @@ -50,12 +51,13 @@ typedef struct __mavlink_mission_item_int_t { { "x", NULL, MAVLINK_TYPE_INT32_T, 0, 16, offsetof(mavlink_mission_item_int_t, x) }, \ { "y", NULL, MAVLINK_TYPE_INT32_T, 0, 20, offsetof(mavlink_mission_item_int_t, y) }, \ { "z", NULL, MAVLINK_TYPE_FLOAT, 0, 24, offsetof(mavlink_mission_item_int_t, z) }, \ + { "mission_type", NULL, MAVLINK_TYPE_UINT8_T, 0, 37, offsetof(mavlink_mission_item_int_t, mission_type) }, \ } \ } #else #define MAVLINK_MESSAGE_INFO_MISSION_ITEM_INT { \ "MISSION_ITEM_INT", \ - 14, \ + 15, \ { { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 32, offsetof(mavlink_mission_item_int_t, target_system) }, \ { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 33, offsetof(mavlink_mission_item_int_t, target_component) }, \ { "seq", NULL, MAVLINK_TYPE_UINT16_T, 0, 28, offsetof(mavlink_mission_item_int_t, seq) }, \ @@ -70,6 +72,7 @@ typedef struct __mavlink_mission_item_int_t { { "x", NULL, MAVLINK_TYPE_INT32_T, 0, 16, offsetof(mavlink_mission_item_int_t, x) }, \ { "y", NULL, MAVLINK_TYPE_INT32_T, 0, 20, offsetof(mavlink_mission_item_int_t, y) }, \ { "z", NULL, MAVLINK_TYPE_FLOAT, 0, 24, offsetof(mavlink_mission_item_int_t, z) }, \ + { "mission_type", NULL, MAVLINK_TYPE_UINT8_T, 0, 37, offsetof(mavlink_mission_item_int_t, mission_type) }, \ } \ } #endif @@ -94,10 +97,11 @@ typedef struct __mavlink_mission_item_int_t { * @param x PARAM5 / local: x position in meters * 1e4, global: latitude in degrees * 10^7 * @param y PARAM6 / y position: local: x position in meters * 1e4, global: longitude in degrees *10^7 * @param z PARAM7 / z position: global: altitude in meters (relative or absolute, depending on frame. + * @param mission_type Mission type. * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_mission_item_int_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint8_t target_system, uint8_t target_component, uint16_t seq, uint8_t frame, uint16_t command, uint8_t current, uint8_t autocontinue, float param1, float param2, float param3, float param4, int32_t x, int32_t y, float z) + uint8_t target_system, uint8_t target_component, uint16_t seq, uint8_t frame, uint16_t command, uint8_t current, uint8_t autocontinue, float param1, float param2, float param3, float param4, int32_t x, int32_t y, float z, uint8_t mission_type) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_MISSION_ITEM_INT_LEN]; @@ -115,6 +119,7 @@ static inline uint16_t mavlink_msg_mission_item_int_pack(uint8_t system_id, uint _mav_put_uint8_t(buf, 34, frame); _mav_put_uint8_t(buf, 35, current); _mav_put_uint8_t(buf, 36, autocontinue); + _mav_put_uint8_t(buf, 37, mission_type); memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_MISSION_ITEM_INT_LEN); #else @@ -133,6 +138,7 @@ static inline uint16_t mavlink_msg_mission_item_int_pack(uint8_t system_id, uint packet.frame = frame; packet.current = current; packet.autocontinue = autocontinue; + packet.mission_type = mission_type; memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_MISSION_ITEM_INT_LEN); #endif @@ -161,11 +167,12 @@ static inline uint16_t mavlink_msg_mission_item_int_pack(uint8_t system_id, uint * @param x PARAM5 / local: x position in meters * 1e4, global: latitude in degrees * 10^7 * @param y PARAM6 / y position: local: x position in meters * 1e4, global: longitude in degrees *10^7 * @param z PARAM7 / z position: global: altitude in meters (relative or absolute, depending on frame. + * @param mission_type Mission type. * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_mission_item_int_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, - uint8_t target_system,uint8_t target_component,uint16_t seq,uint8_t frame,uint16_t command,uint8_t current,uint8_t autocontinue,float param1,float param2,float param3,float param4,int32_t x,int32_t y,float z) + uint8_t target_system,uint8_t target_component,uint16_t seq,uint8_t frame,uint16_t command,uint8_t current,uint8_t autocontinue,float param1,float param2,float param3,float param4,int32_t x,int32_t y,float z,uint8_t mission_type) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_MISSION_ITEM_INT_LEN]; @@ -183,6 +190,7 @@ static inline uint16_t mavlink_msg_mission_item_int_pack_chan(uint8_t system_id, _mav_put_uint8_t(buf, 34, frame); _mav_put_uint8_t(buf, 35, current); _mav_put_uint8_t(buf, 36, autocontinue); + _mav_put_uint8_t(buf, 37, mission_type); memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_MISSION_ITEM_INT_LEN); #else @@ -201,6 +209,7 @@ static inline uint16_t mavlink_msg_mission_item_int_pack_chan(uint8_t system_id, packet.frame = frame; packet.current = current; packet.autocontinue = autocontinue; + packet.mission_type = mission_type; memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_MISSION_ITEM_INT_LEN); #endif @@ -219,7 +228,7 @@ static inline uint16_t mavlink_msg_mission_item_int_pack_chan(uint8_t system_id, */ static inline uint16_t mavlink_msg_mission_item_int_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_mission_item_int_t* mission_item_int) { - return mavlink_msg_mission_item_int_pack(system_id, component_id, msg, mission_item_int->target_system, mission_item_int->target_component, mission_item_int->seq, mission_item_int->frame, mission_item_int->command, mission_item_int->current, mission_item_int->autocontinue, mission_item_int->param1, mission_item_int->param2, mission_item_int->param3, mission_item_int->param4, mission_item_int->x, mission_item_int->y, mission_item_int->z); + return mavlink_msg_mission_item_int_pack(system_id, component_id, msg, mission_item_int->target_system, mission_item_int->target_component, mission_item_int->seq, mission_item_int->frame, mission_item_int->command, mission_item_int->current, mission_item_int->autocontinue, mission_item_int->param1, mission_item_int->param2, mission_item_int->param3, mission_item_int->param4, mission_item_int->x, mission_item_int->y, mission_item_int->z, mission_item_int->mission_type); } /** @@ -233,7 +242,7 @@ static inline uint16_t mavlink_msg_mission_item_int_encode(uint8_t system_id, ui */ static inline uint16_t mavlink_msg_mission_item_int_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_mission_item_int_t* mission_item_int) { - return mavlink_msg_mission_item_int_pack_chan(system_id, component_id, chan, msg, mission_item_int->target_system, mission_item_int->target_component, mission_item_int->seq, mission_item_int->frame, mission_item_int->command, mission_item_int->current, mission_item_int->autocontinue, mission_item_int->param1, mission_item_int->param2, mission_item_int->param3, mission_item_int->param4, mission_item_int->x, mission_item_int->y, mission_item_int->z); + return mavlink_msg_mission_item_int_pack_chan(system_id, component_id, chan, msg, mission_item_int->target_system, mission_item_int->target_component, mission_item_int->seq, mission_item_int->frame, mission_item_int->command, mission_item_int->current, mission_item_int->autocontinue, mission_item_int->param1, mission_item_int->param2, mission_item_int->param3, mission_item_int->param4, mission_item_int->x, mission_item_int->y, mission_item_int->z, mission_item_int->mission_type); } /** @@ -254,10 +263,11 @@ static inline uint16_t mavlink_msg_mission_item_int_encode_chan(uint8_t system_i * @param x PARAM5 / local: x position in meters * 1e4, global: latitude in degrees * 10^7 * @param y PARAM6 / y position: local: x position in meters * 1e4, global: longitude in degrees *10^7 * @param z PARAM7 / z position: global: altitude in meters (relative or absolute, depending on frame. + * @param mission_type Mission type. */ #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS -static inline void mavlink_msg_mission_item_int_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint16_t seq, uint8_t frame, uint16_t command, uint8_t current, uint8_t autocontinue, float param1, float param2, float param3, float param4, int32_t x, int32_t y, float z) +static inline void mavlink_msg_mission_item_int_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint16_t seq, uint8_t frame, uint16_t command, uint8_t current, uint8_t autocontinue, float param1, float param2, float param3, float param4, int32_t x, int32_t y, float z, uint8_t mission_type) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_MISSION_ITEM_INT_LEN]; @@ -275,6 +285,7 @@ static inline void mavlink_msg_mission_item_int_send(mavlink_channel_t chan, uin _mav_put_uint8_t(buf, 34, frame); _mav_put_uint8_t(buf, 35, current); _mav_put_uint8_t(buf, 36, autocontinue); + _mav_put_uint8_t(buf, 37, mission_type); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_ITEM_INT, buf, MAVLINK_MSG_ID_MISSION_ITEM_INT_MIN_LEN, MAVLINK_MSG_ID_MISSION_ITEM_INT_LEN, MAVLINK_MSG_ID_MISSION_ITEM_INT_CRC); #else @@ -293,6 +304,7 @@ static inline void mavlink_msg_mission_item_int_send(mavlink_channel_t chan, uin packet.frame = frame; packet.current = current; packet.autocontinue = autocontinue; + packet.mission_type = mission_type; _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_ITEM_INT, (const char *)&packet, MAVLINK_MSG_ID_MISSION_ITEM_INT_MIN_LEN, MAVLINK_MSG_ID_MISSION_ITEM_INT_LEN, MAVLINK_MSG_ID_MISSION_ITEM_INT_CRC); #endif @@ -306,7 +318,7 @@ static inline void mavlink_msg_mission_item_int_send(mavlink_channel_t chan, uin static inline void mavlink_msg_mission_item_int_send_struct(mavlink_channel_t chan, const mavlink_mission_item_int_t* mission_item_int) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - mavlink_msg_mission_item_int_send(chan, mission_item_int->target_system, mission_item_int->target_component, mission_item_int->seq, mission_item_int->frame, mission_item_int->command, mission_item_int->current, mission_item_int->autocontinue, mission_item_int->param1, mission_item_int->param2, mission_item_int->param3, mission_item_int->param4, mission_item_int->x, mission_item_int->y, mission_item_int->z); + mavlink_msg_mission_item_int_send(chan, mission_item_int->target_system, mission_item_int->target_component, mission_item_int->seq, mission_item_int->frame, mission_item_int->command, mission_item_int->current, mission_item_int->autocontinue, mission_item_int->param1, mission_item_int->param2, mission_item_int->param3, mission_item_int->param4, mission_item_int->x, mission_item_int->y, mission_item_int->z, mission_item_int->mission_type); #else _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_ITEM_INT, (const char *)mission_item_int, MAVLINK_MSG_ID_MISSION_ITEM_INT_MIN_LEN, MAVLINK_MSG_ID_MISSION_ITEM_INT_LEN, MAVLINK_MSG_ID_MISSION_ITEM_INT_CRC); #endif @@ -320,7 +332,7 @@ static inline void mavlink_msg_mission_item_int_send_struct(mavlink_channel_t ch is usually the receive buffer for the channel, and allows a reply to an incoming message with minimum stack space usage. */ -static inline void mavlink_msg_mission_item_int_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint16_t seq, uint8_t frame, uint16_t command, uint8_t current, uint8_t autocontinue, float param1, float param2, float param3, float param4, int32_t x, int32_t y, float z) +static inline void mavlink_msg_mission_item_int_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint16_t seq, uint8_t frame, uint16_t command, uint8_t current, uint8_t autocontinue, float param1, float param2, float param3, float param4, int32_t x, int32_t y, float z, uint8_t mission_type) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char *buf = (char *)msgbuf; @@ -338,6 +350,7 @@ static inline void mavlink_msg_mission_item_int_send_buf(mavlink_message_t *msgb _mav_put_uint8_t(buf, 34, frame); _mav_put_uint8_t(buf, 35, current); _mav_put_uint8_t(buf, 36, autocontinue); + _mav_put_uint8_t(buf, 37, mission_type); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_ITEM_INT, buf, MAVLINK_MSG_ID_MISSION_ITEM_INT_MIN_LEN, MAVLINK_MSG_ID_MISSION_ITEM_INT_LEN, MAVLINK_MSG_ID_MISSION_ITEM_INT_CRC); #else @@ -356,6 +369,7 @@ static inline void mavlink_msg_mission_item_int_send_buf(mavlink_message_t *msgb packet->frame = frame; packet->current = current; packet->autocontinue = autocontinue; + packet->mission_type = mission_type; _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_ITEM_INT, (const char *)packet, MAVLINK_MSG_ID_MISSION_ITEM_INT_MIN_LEN, MAVLINK_MSG_ID_MISSION_ITEM_INT_LEN, MAVLINK_MSG_ID_MISSION_ITEM_INT_CRC); #endif @@ -507,6 +521,16 @@ static inline float mavlink_msg_mission_item_int_get_z(const mavlink_message_t* return _MAV_RETURN_float(msg, 24); } +/** + * @brief Get field mission_type from mission_item_int message + * + * @return Mission type. + */ +static inline uint8_t mavlink_msg_mission_item_int_get_mission_type(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 37); +} + /** * @brief Decode a mission_item_int message into a struct * @@ -530,6 +554,7 @@ static inline void mavlink_msg_mission_item_int_decode(const mavlink_message_t* mission_item_int->frame = mavlink_msg_mission_item_int_get_frame(msg); mission_item_int->current = mavlink_msg_mission_item_int_get_current(msg); mission_item_int->autocontinue = mavlink_msg_mission_item_int_get_autocontinue(msg); + mission_item_int->mission_type = mavlink_msg_mission_item_int_get_mission_type(msg); #else uint8_t len = msg->len < MAVLINK_MSG_ID_MISSION_ITEM_INT_LEN? msg->len : MAVLINK_MSG_ID_MISSION_ITEM_INT_LEN; memset(mission_item_int, 0, MAVLINK_MSG_ID_MISSION_ITEM_INT_LEN); diff --git a/lib/main/MAVLink/common/mavlink_msg_mission_request.h b/lib/main/MAVLink/common/mavlink_msg_mission_request.h index e3b33cc3ec..4f8c0fe77c 100755 --- a/lib/main/MAVLink/common/mavlink_msg_mission_request.h +++ b/lib/main/MAVLink/common/mavlink_msg_mission_request.h @@ -8,11 +8,12 @@ typedef struct __mavlink_mission_request_t { uint16_t seq; /*< Sequence*/ uint8_t target_system; /*< System ID*/ uint8_t target_component; /*< Component ID*/ + uint8_t mission_type; /*< Mission type.*/ } mavlink_mission_request_t; -#define MAVLINK_MSG_ID_MISSION_REQUEST_LEN 4 +#define MAVLINK_MSG_ID_MISSION_REQUEST_LEN 5 #define MAVLINK_MSG_ID_MISSION_REQUEST_MIN_LEN 4 -#define MAVLINK_MSG_ID_40_LEN 4 +#define MAVLINK_MSG_ID_40_LEN 5 #define MAVLINK_MSG_ID_40_MIN_LEN 4 #define MAVLINK_MSG_ID_MISSION_REQUEST_CRC 230 @@ -24,19 +25,21 @@ typedef struct __mavlink_mission_request_t { #define MAVLINK_MESSAGE_INFO_MISSION_REQUEST { \ 40, \ "MISSION_REQUEST", \ - 3, \ + 4, \ { { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 2, offsetof(mavlink_mission_request_t, target_system) }, \ { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 3, offsetof(mavlink_mission_request_t, target_component) }, \ { "seq", NULL, MAVLINK_TYPE_UINT16_T, 0, 0, offsetof(mavlink_mission_request_t, seq) }, \ + { "mission_type", NULL, MAVLINK_TYPE_UINT8_T, 0, 4, offsetof(mavlink_mission_request_t, mission_type) }, \ } \ } #else #define MAVLINK_MESSAGE_INFO_MISSION_REQUEST { \ "MISSION_REQUEST", \ - 3, \ + 4, \ { { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 2, offsetof(mavlink_mission_request_t, target_system) }, \ { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 3, offsetof(mavlink_mission_request_t, target_component) }, \ { "seq", NULL, MAVLINK_TYPE_UINT16_T, 0, 0, offsetof(mavlink_mission_request_t, seq) }, \ + { "mission_type", NULL, MAVLINK_TYPE_UINT8_T, 0, 4, offsetof(mavlink_mission_request_t, mission_type) }, \ } \ } #endif @@ -50,16 +53,18 @@ typedef struct __mavlink_mission_request_t { * @param target_system System ID * @param target_component Component ID * @param seq Sequence + * @param mission_type Mission type. * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_mission_request_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint8_t target_system, uint8_t target_component, uint16_t seq) + uint8_t target_system, uint8_t target_component, uint16_t seq, uint8_t mission_type) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_MISSION_REQUEST_LEN]; _mav_put_uint16_t(buf, 0, seq); _mav_put_uint8_t(buf, 2, target_system); _mav_put_uint8_t(buf, 3, target_component); + _mav_put_uint8_t(buf, 4, mission_type); memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_MISSION_REQUEST_LEN); #else @@ -67,6 +72,7 @@ static inline uint16_t mavlink_msg_mission_request_pack(uint8_t system_id, uint8 packet.seq = seq; packet.target_system = target_system; packet.target_component = target_component; + packet.mission_type = mission_type; memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_MISSION_REQUEST_LEN); #endif @@ -84,17 +90,19 @@ static inline uint16_t mavlink_msg_mission_request_pack(uint8_t system_id, uint8 * @param target_system System ID * @param target_component Component ID * @param seq Sequence + * @param mission_type Mission type. * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_mission_request_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, - uint8_t target_system,uint8_t target_component,uint16_t seq) + uint8_t target_system,uint8_t target_component,uint16_t seq,uint8_t mission_type) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_MISSION_REQUEST_LEN]; _mav_put_uint16_t(buf, 0, seq); _mav_put_uint8_t(buf, 2, target_system); _mav_put_uint8_t(buf, 3, target_component); + _mav_put_uint8_t(buf, 4, mission_type); memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_MISSION_REQUEST_LEN); #else @@ -102,6 +110,7 @@ static inline uint16_t mavlink_msg_mission_request_pack_chan(uint8_t system_id, packet.seq = seq; packet.target_system = target_system; packet.target_component = target_component; + packet.mission_type = mission_type; memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_MISSION_REQUEST_LEN); #endif @@ -120,7 +129,7 @@ static inline uint16_t mavlink_msg_mission_request_pack_chan(uint8_t system_id, */ static inline uint16_t mavlink_msg_mission_request_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_mission_request_t* mission_request) { - return mavlink_msg_mission_request_pack(system_id, component_id, msg, mission_request->target_system, mission_request->target_component, mission_request->seq); + return mavlink_msg_mission_request_pack(system_id, component_id, msg, mission_request->target_system, mission_request->target_component, mission_request->seq, mission_request->mission_type); } /** @@ -134,7 +143,7 @@ static inline uint16_t mavlink_msg_mission_request_encode(uint8_t system_id, uin */ static inline uint16_t mavlink_msg_mission_request_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_mission_request_t* mission_request) { - return mavlink_msg_mission_request_pack_chan(system_id, component_id, chan, msg, mission_request->target_system, mission_request->target_component, mission_request->seq); + return mavlink_msg_mission_request_pack_chan(system_id, component_id, chan, msg, mission_request->target_system, mission_request->target_component, mission_request->seq, mission_request->mission_type); } /** @@ -144,16 +153,18 @@ static inline uint16_t mavlink_msg_mission_request_encode_chan(uint8_t system_id * @param target_system System ID * @param target_component Component ID * @param seq Sequence + * @param mission_type Mission type. */ #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS -static inline void mavlink_msg_mission_request_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint16_t seq) +static inline void mavlink_msg_mission_request_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint16_t seq, uint8_t mission_type) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_MISSION_REQUEST_LEN]; _mav_put_uint16_t(buf, 0, seq); _mav_put_uint8_t(buf, 2, target_system); _mav_put_uint8_t(buf, 3, target_component); + _mav_put_uint8_t(buf, 4, mission_type); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_REQUEST, buf, MAVLINK_MSG_ID_MISSION_REQUEST_MIN_LEN, MAVLINK_MSG_ID_MISSION_REQUEST_LEN, MAVLINK_MSG_ID_MISSION_REQUEST_CRC); #else @@ -161,6 +172,7 @@ static inline void mavlink_msg_mission_request_send(mavlink_channel_t chan, uint packet.seq = seq; packet.target_system = target_system; packet.target_component = target_component; + packet.mission_type = mission_type; _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_REQUEST, (const char *)&packet, MAVLINK_MSG_ID_MISSION_REQUEST_MIN_LEN, MAVLINK_MSG_ID_MISSION_REQUEST_LEN, MAVLINK_MSG_ID_MISSION_REQUEST_CRC); #endif @@ -174,7 +186,7 @@ static inline void mavlink_msg_mission_request_send(mavlink_channel_t chan, uint static inline void mavlink_msg_mission_request_send_struct(mavlink_channel_t chan, const mavlink_mission_request_t* mission_request) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - mavlink_msg_mission_request_send(chan, mission_request->target_system, mission_request->target_component, mission_request->seq); + mavlink_msg_mission_request_send(chan, mission_request->target_system, mission_request->target_component, mission_request->seq, mission_request->mission_type); #else _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_REQUEST, (const char *)mission_request, MAVLINK_MSG_ID_MISSION_REQUEST_MIN_LEN, MAVLINK_MSG_ID_MISSION_REQUEST_LEN, MAVLINK_MSG_ID_MISSION_REQUEST_CRC); #endif @@ -188,13 +200,14 @@ static inline void mavlink_msg_mission_request_send_struct(mavlink_channel_t cha is usually the receive buffer for the channel, and allows a reply to an incoming message with minimum stack space usage. */ -static inline void mavlink_msg_mission_request_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint16_t seq) +static inline void mavlink_msg_mission_request_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint16_t seq, uint8_t mission_type) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char *buf = (char *)msgbuf; _mav_put_uint16_t(buf, 0, seq); _mav_put_uint8_t(buf, 2, target_system); _mav_put_uint8_t(buf, 3, target_component); + _mav_put_uint8_t(buf, 4, mission_type); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_REQUEST, buf, MAVLINK_MSG_ID_MISSION_REQUEST_MIN_LEN, MAVLINK_MSG_ID_MISSION_REQUEST_LEN, MAVLINK_MSG_ID_MISSION_REQUEST_CRC); #else @@ -202,6 +215,7 @@ static inline void mavlink_msg_mission_request_send_buf(mavlink_message_t *msgbu packet->seq = seq; packet->target_system = target_system; packet->target_component = target_component; + packet->mission_type = mission_type; _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_REQUEST, (const char *)packet, MAVLINK_MSG_ID_MISSION_REQUEST_MIN_LEN, MAVLINK_MSG_ID_MISSION_REQUEST_LEN, MAVLINK_MSG_ID_MISSION_REQUEST_CRC); #endif @@ -243,6 +257,16 @@ static inline uint16_t mavlink_msg_mission_request_get_seq(const mavlink_message return _MAV_RETURN_uint16_t(msg, 0); } +/** + * @brief Get field mission_type from mission_request message + * + * @return Mission type. + */ +static inline uint8_t mavlink_msg_mission_request_get_mission_type(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 4); +} + /** * @brief Decode a mission_request message into a struct * @@ -255,6 +279,7 @@ static inline void mavlink_msg_mission_request_decode(const mavlink_message_t* m mission_request->seq = mavlink_msg_mission_request_get_seq(msg); mission_request->target_system = mavlink_msg_mission_request_get_target_system(msg); mission_request->target_component = mavlink_msg_mission_request_get_target_component(msg); + mission_request->mission_type = mavlink_msg_mission_request_get_mission_type(msg); #else uint8_t len = msg->len < MAVLINK_MSG_ID_MISSION_REQUEST_LEN? msg->len : MAVLINK_MSG_ID_MISSION_REQUEST_LEN; memset(mission_request, 0, MAVLINK_MSG_ID_MISSION_REQUEST_LEN); diff --git a/lib/main/MAVLink/common/mavlink_msg_mission_request_int.h b/lib/main/MAVLink/common/mavlink_msg_mission_request_int.h index a6c2df9119..c131f78f7b 100755 --- a/lib/main/MAVLink/common/mavlink_msg_mission_request_int.h +++ b/lib/main/MAVLink/common/mavlink_msg_mission_request_int.h @@ -8,11 +8,12 @@ typedef struct __mavlink_mission_request_int_t { uint16_t seq; /*< Sequence*/ uint8_t target_system; /*< System ID*/ uint8_t target_component; /*< Component ID*/ + uint8_t mission_type; /*< Mission type.*/ } mavlink_mission_request_int_t; -#define MAVLINK_MSG_ID_MISSION_REQUEST_INT_LEN 4 +#define MAVLINK_MSG_ID_MISSION_REQUEST_INT_LEN 5 #define MAVLINK_MSG_ID_MISSION_REQUEST_INT_MIN_LEN 4 -#define MAVLINK_MSG_ID_51_LEN 4 +#define MAVLINK_MSG_ID_51_LEN 5 #define MAVLINK_MSG_ID_51_MIN_LEN 4 #define MAVLINK_MSG_ID_MISSION_REQUEST_INT_CRC 196 @@ -24,19 +25,21 @@ typedef struct __mavlink_mission_request_int_t { #define MAVLINK_MESSAGE_INFO_MISSION_REQUEST_INT { \ 51, \ "MISSION_REQUEST_INT", \ - 3, \ + 4, \ { { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 2, offsetof(mavlink_mission_request_int_t, target_system) }, \ { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 3, offsetof(mavlink_mission_request_int_t, target_component) }, \ { "seq", NULL, MAVLINK_TYPE_UINT16_T, 0, 0, offsetof(mavlink_mission_request_int_t, seq) }, \ + { "mission_type", NULL, MAVLINK_TYPE_UINT8_T, 0, 4, offsetof(mavlink_mission_request_int_t, mission_type) }, \ } \ } #else #define MAVLINK_MESSAGE_INFO_MISSION_REQUEST_INT { \ "MISSION_REQUEST_INT", \ - 3, \ + 4, \ { { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 2, offsetof(mavlink_mission_request_int_t, target_system) }, \ { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 3, offsetof(mavlink_mission_request_int_t, target_component) }, \ { "seq", NULL, MAVLINK_TYPE_UINT16_T, 0, 0, offsetof(mavlink_mission_request_int_t, seq) }, \ + { "mission_type", NULL, MAVLINK_TYPE_UINT8_T, 0, 4, offsetof(mavlink_mission_request_int_t, mission_type) }, \ } \ } #endif @@ -50,16 +53,18 @@ typedef struct __mavlink_mission_request_int_t { * @param target_system System ID * @param target_component Component ID * @param seq Sequence + * @param mission_type Mission type. * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_mission_request_int_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint8_t target_system, uint8_t target_component, uint16_t seq) + uint8_t target_system, uint8_t target_component, uint16_t seq, uint8_t mission_type) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_MISSION_REQUEST_INT_LEN]; _mav_put_uint16_t(buf, 0, seq); _mav_put_uint8_t(buf, 2, target_system); _mav_put_uint8_t(buf, 3, target_component); + _mav_put_uint8_t(buf, 4, mission_type); memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_MISSION_REQUEST_INT_LEN); #else @@ -67,6 +72,7 @@ static inline uint16_t mavlink_msg_mission_request_int_pack(uint8_t system_id, u packet.seq = seq; packet.target_system = target_system; packet.target_component = target_component; + packet.mission_type = mission_type; memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_MISSION_REQUEST_INT_LEN); #endif @@ -84,17 +90,19 @@ static inline uint16_t mavlink_msg_mission_request_int_pack(uint8_t system_id, u * @param target_system System ID * @param target_component Component ID * @param seq Sequence + * @param mission_type Mission type. * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_mission_request_int_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, - uint8_t target_system,uint8_t target_component,uint16_t seq) + uint8_t target_system,uint8_t target_component,uint16_t seq,uint8_t mission_type) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_MISSION_REQUEST_INT_LEN]; _mav_put_uint16_t(buf, 0, seq); _mav_put_uint8_t(buf, 2, target_system); _mav_put_uint8_t(buf, 3, target_component); + _mav_put_uint8_t(buf, 4, mission_type); memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_MISSION_REQUEST_INT_LEN); #else @@ -102,6 +110,7 @@ static inline uint16_t mavlink_msg_mission_request_int_pack_chan(uint8_t system_ packet.seq = seq; packet.target_system = target_system; packet.target_component = target_component; + packet.mission_type = mission_type; memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_MISSION_REQUEST_INT_LEN); #endif @@ -120,7 +129,7 @@ static inline uint16_t mavlink_msg_mission_request_int_pack_chan(uint8_t system_ */ static inline uint16_t mavlink_msg_mission_request_int_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_mission_request_int_t* mission_request_int) { - return mavlink_msg_mission_request_int_pack(system_id, component_id, msg, mission_request_int->target_system, mission_request_int->target_component, mission_request_int->seq); + return mavlink_msg_mission_request_int_pack(system_id, component_id, msg, mission_request_int->target_system, mission_request_int->target_component, mission_request_int->seq, mission_request_int->mission_type); } /** @@ -134,7 +143,7 @@ static inline uint16_t mavlink_msg_mission_request_int_encode(uint8_t system_id, */ static inline uint16_t mavlink_msg_mission_request_int_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_mission_request_int_t* mission_request_int) { - return mavlink_msg_mission_request_int_pack_chan(system_id, component_id, chan, msg, mission_request_int->target_system, mission_request_int->target_component, mission_request_int->seq); + return mavlink_msg_mission_request_int_pack_chan(system_id, component_id, chan, msg, mission_request_int->target_system, mission_request_int->target_component, mission_request_int->seq, mission_request_int->mission_type); } /** @@ -144,16 +153,18 @@ static inline uint16_t mavlink_msg_mission_request_int_encode_chan(uint8_t syste * @param target_system System ID * @param target_component Component ID * @param seq Sequence + * @param mission_type Mission type. */ #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS -static inline void mavlink_msg_mission_request_int_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint16_t seq) +static inline void mavlink_msg_mission_request_int_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint16_t seq, uint8_t mission_type) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_MISSION_REQUEST_INT_LEN]; _mav_put_uint16_t(buf, 0, seq); _mav_put_uint8_t(buf, 2, target_system); _mav_put_uint8_t(buf, 3, target_component); + _mav_put_uint8_t(buf, 4, mission_type); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_REQUEST_INT, buf, MAVLINK_MSG_ID_MISSION_REQUEST_INT_MIN_LEN, MAVLINK_MSG_ID_MISSION_REQUEST_INT_LEN, MAVLINK_MSG_ID_MISSION_REQUEST_INT_CRC); #else @@ -161,6 +172,7 @@ static inline void mavlink_msg_mission_request_int_send(mavlink_channel_t chan, packet.seq = seq; packet.target_system = target_system; packet.target_component = target_component; + packet.mission_type = mission_type; _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_REQUEST_INT, (const char *)&packet, MAVLINK_MSG_ID_MISSION_REQUEST_INT_MIN_LEN, MAVLINK_MSG_ID_MISSION_REQUEST_INT_LEN, MAVLINK_MSG_ID_MISSION_REQUEST_INT_CRC); #endif @@ -174,7 +186,7 @@ static inline void mavlink_msg_mission_request_int_send(mavlink_channel_t chan, static inline void mavlink_msg_mission_request_int_send_struct(mavlink_channel_t chan, const mavlink_mission_request_int_t* mission_request_int) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - mavlink_msg_mission_request_int_send(chan, mission_request_int->target_system, mission_request_int->target_component, mission_request_int->seq); + mavlink_msg_mission_request_int_send(chan, mission_request_int->target_system, mission_request_int->target_component, mission_request_int->seq, mission_request_int->mission_type); #else _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_REQUEST_INT, (const char *)mission_request_int, MAVLINK_MSG_ID_MISSION_REQUEST_INT_MIN_LEN, MAVLINK_MSG_ID_MISSION_REQUEST_INT_LEN, MAVLINK_MSG_ID_MISSION_REQUEST_INT_CRC); #endif @@ -188,13 +200,14 @@ static inline void mavlink_msg_mission_request_int_send_struct(mavlink_channel_t is usually the receive buffer for the channel, and allows a reply to an incoming message with minimum stack space usage. */ -static inline void mavlink_msg_mission_request_int_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint16_t seq) +static inline void mavlink_msg_mission_request_int_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint16_t seq, uint8_t mission_type) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char *buf = (char *)msgbuf; _mav_put_uint16_t(buf, 0, seq); _mav_put_uint8_t(buf, 2, target_system); _mav_put_uint8_t(buf, 3, target_component); + _mav_put_uint8_t(buf, 4, mission_type); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_REQUEST_INT, buf, MAVLINK_MSG_ID_MISSION_REQUEST_INT_MIN_LEN, MAVLINK_MSG_ID_MISSION_REQUEST_INT_LEN, MAVLINK_MSG_ID_MISSION_REQUEST_INT_CRC); #else @@ -202,6 +215,7 @@ static inline void mavlink_msg_mission_request_int_send_buf(mavlink_message_t *m packet->seq = seq; packet->target_system = target_system; packet->target_component = target_component; + packet->mission_type = mission_type; _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_REQUEST_INT, (const char *)packet, MAVLINK_MSG_ID_MISSION_REQUEST_INT_MIN_LEN, MAVLINK_MSG_ID_MISSION_REQUEST_INT_LEN, MAVLINK_MSG_ID_MISSION_REQUEST_INT_CRC); #endif @@ -243,6 +257,16 @@ static inline uint16_t mavlink_msg_mission_request_int_get_seq(const mavlink_mes return _MAV_RETURN_uint16_t(msg, 0); } +/** + * @brief Get field mission_type from mission_request_int message + * + * @return Mission type. + */ +static inline uint8_t mavlink_msg_mission_request_int_get_mission_type(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 4); +} + /** * @brief Decode a mission_request_int message into a struct * @@ -255,6 +279,7 @@ static inline void mavlink_msg_mission_request_int_decode(const mavlink_message_ mission_request_int->seq = mavlink_msg_mission_request_int_get_seq(msg); mission_request_int->target_system = mavlink_msg_mission_request_int_get_target_system(msg); mission_request_int->target_component = mavlink_msg_mission_request_int_get_target_component(msg); + mission_request_int->mission_type = mavlink_msg_mission_request_int_get_mission_type(msg); #else uint8_t len = msg->len < MAVLINK_MSG_ID_MISSION_REQUEST_INT_LEN? msg->len : MAVLINK_MSG_ID_MISSION_REQUEST_INT_LEN; memset(mission_request_int, 0, MAVLINK_MSG_ID_MISSION_REQUEST_INT_LEN); diff --git a/lib/main/MAVLink/common/mavlink_msg_mission_request_list.h b/lib/main/MAVLink/common/mavlink_msg_mission_request_list.h index 8b6659fde7..a1633f43f4 100755 --- a/lib/main/MAVLink/common/mavlink_msg_mission_request_list.h +++ b/lib/main/MAVLink/common/mavlink_msg_mission_request_list.h @@ -7,11 +7,12 @@ typedef struct __mavlink_mission_request_list_t { uint8_t target_system; /*< System ID*/ uint8_t target_component; /*< Component ID*/ + uint8_t mission_type; /*< Mission type.*/ } mavlink_mission_request_list_t; -#define MAVLINK_MSG_ID_MISSION_REQUEST_LIST_LEN 2 +#define MAVLINK_MSG_ID_MISSION_REQUEST_LIST_LEN 3 #define MAVLINK_MSG_ID_MISSION_REQUEST_LIST_MIN_LEN 2 -#define MAVLINK_MSG_ID_43_LEN 2 +#define MAVLINK_MSG_ID_43_LEN 3 #define MAVLINK_MSG_ID_43_MIN_LEN 2 #define MAVLINK_MSG_ID_MISSION_REQUEST_LIST_CRC 132 @@ -23,17 +24,19 @@ typedef struct __mavlink_mission_request_list_t { #define MAVLINK_MESSAGE_INFO_MISSION_REQUEST_LIST { \ 43, \ "MISSION_REQUEST_LIST", \ - 2, \ + 3, \ { { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 0, offsetof(mavlink_mission_request_list_t, target_system) }, \ { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 1, offsetof(mavlink_mission_request_list_t, target_component) }, \ + { "mission_type", NULL, MAVLINK_TYPE_UINT8_T, 0, 2, offsetof(mavlink_mission_request_list_t, mission_type) }, \ } \ } #else #define MAVLINK_MESSAGE_INFO_MISSION_REQUEST_LIST { \ "MISSION_REQUEST_LIST", \ - 2, \ + 3, \ { { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 0, offsetof(mavlink_mission_request_list_t, target_system) }, \ { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 1, offsetof(mavlink_mission_request_list_t, target_component) }, \ + { "mission_type", NULL, MAVLINK_TYPE_UINT8_T, 0, 2, offsetof(mavlink_mission_request_list_t, mission_type) }, \ } \ } #endif @@ -46,21 +49,24 @@ typedef struct __mavlink_mission_request_list_t { * * @param target_system System ID * @param target_component Component ID + * @param mission_type Mission type. * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_mission_request_list_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint8_t target_system, uint8_t target_component) + uint8_t target_system, uint8_t target_component, uint8_t mission_type) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_MISSION_REQUEST_LIST_LEN]; _mav_put_uint8_t(buf, 0, target_system); _mav_put_uint8_t(buf, 1, target_component); + _mav_put_uint8_t(buf, 2, mission_type); memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_MISSION_REQUEST_LIST_LEN); #else mavlink_mission_request_list_t packet; packet.target_system = target_system; packet.target_component = target_component; + packet.mission_type = mission_type; memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_MISSION_REQUEST_LIST_LEN); #endif @@ -77,22 +83,25 @@ static inline uint16_t mavlink_msg_mission_request_list_pack(uint8_t system_id, * @param msg The MAVLink message to compress the data into * @param target_system System ID * @param target_component Component ID + * @param mission_type Mission type. * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_mission_request_list_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, - uint8_t target_system,uint8_t target_component) + uint8_t target_system,uint8_t target_component,uint8_t mission_type) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_MISSION_REQUEST_LIST_LEN]; _mav_put_uint8_t(buf, 0, target_system); _mav_put_uint8_t(buf, 1, target_component); + _mav_put_uint8_t(buf, 2, mission_type); memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_MISSION_REQUEST_LIST_LEN); #else mavlink_mission_request_list_t packet; packet.target_system = target_system; packet.target_component = target_component; + packet.mission_type = mission_type; memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_MISSION_REQUEST_LIST_LEN); #endif @@ -111,7 +120,7 @@ static inline uint16_t mavlink_msg_mission_request_list_pack_chan(uint8_t system */ static inline uint16_t mavlink_msg_mission_request_list_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_mission_request_list_t* mission_request_list) { - return mavlink_msg_mission_request_list_pack(system_id, component_id, msg, mission_request_list->target_system, mission_request_list->target_component); + return mavlink_msg_mission_request_list_pack(system_id, component_id, msg, mission_request_list->target_system, mission_request_list->target_component, mission_request_list->mission_type); } /** @@ -125,7 +134,7 @@ static inline uint16_t mavlink_msg_mission_request_list_encode(uint8_t system_id */ static inline uint16_t mavlink_msg_mission_request_list_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_mission_request_list_t* mission_request_list) { - return mavlink_msg_mission_request_list_pack_chan(system_id, component_id, chan, msg, mission_request_list->target_system, mission_request_list->target_component); + return mavlink_msg_mission_request_list_pack_chan(system_id, component_id, chan, msg, mission_request_list->target_system, mission_request_list->target_component, mission_request_list->mission_type); } /** @@ -134,21 +143,24 @@ static inline uint16_t mavlink_msg_mission_request_list_encode_chan(uint8_t syst * * @param target_system System ID * @param target_component Component ID + * @param mission_type Mission type. */ #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS -static inline void mavlink_msg_mission_request_list_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component) +static inline void mavlink_msg_mission_request_list_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint8_t mission_type) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_MISSION_REQUEST_LIST_LEN]; _mav_put_uint8_t(buf, 0, target_system); _mav_put_uint8_t(buf, 1, target_component); + _mav_put_uint8_t(buf, 2, mission_type); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_REQUEST_LIST, buf, MAVLINK_MSG_ID_MISSION_REQUEST_LIST_MIN_LEN, MAVLINK_MSG_ID_MISSION_REQUEST_LIST_LEN, MAVLINK_MSG_ID_MISSION_REQUEST_LIST_CRC); #else mavlink_mission_request_list_t packet; packet.target_system = target_system; packet.target_component = target_component; + packet.mission_type = mission_type; _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_REQUEST_LIST, (const char *)&packet, MAVLINK_MSG_ID_MISSION_REQUEST_LIST_MIN_LEN, MAVLINK_MSG_ID_MISSION_REQUEST_LIST_LEN, MAVLINK_MSG_ID_MISSION_REQUEST_LIST_CRC); #endif @@ -162,7 +174,7 @@ static inline void mavlink_msg_mission_request_list_send(mavlink_channel_t chan, static inline void mavlink_msg_mission_request_list_send_struct(mavlink_channel_t chan, const mavlink_mission_request_list_t* mission_request_list) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - mavlink_msg_mission_request_list_send(chan, mission_request_list->target_system, mission_request_list->target_component); + mavlink_msg_mission_request_list_send(chan, mission_request_list->target_system, mission_request_list->target_component, mission_request_list->mission_type); #else _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_REQUEST_LIST, (const char *)mission_request_list, MAVLINK_MSG_ID_MISSION_REQUEST_LIST_MIN_LEN, MAVLINK_MSG_ID_MISSION_REQUEST_LIST_LEN, MAVLINK_MSG_ID_MISSION_REQUEST_LIST_CRC); #endif @@ -176,18 +188,20 @@ static inline void mavlink_msg_mission_request_list_send_struct(mavlink_channel_ is usually the receive buffer for the channel, and allows a reply to an incoming message with minimum stack space usage. */ -static inline void mavlink_msg_mission_request_list_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target_system, uint8_t target_component) +static inline void mavlink_msg_mission_request_list_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint8_t mission_type) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char *buf = (char *)msgbuf; _mav_put_uint8_t(buf, 0, target_system); _mav_put_uint8_t(buf, 1, target_component); + _mav_put_uint8_t(buf, 2, mission_type); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_REQUEST_LIST, buf, MAVLINK_MSG_ID_MISSION_REQUEST_LIST_MIN_LEN, MAVLINK_MSG_ID_MISSION_REQUEST_LIST_LEN, MAVLINK_MSG_ID_MISSION_REQUEST_LIST_CRC); #else mavlink_mission_request_list_t *packet = (mavlink_mission_request_list_t *)msgbuf; packet->target_system = target_system; packet->target_component = target_component; + packet->mission_type = mission_type; _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_REQUEST_LIST, (const char *)packet, MAVLINK_MSG_ID_MISSION_REQUEST_LIST_MIN_LEN, MAVLINK_MSG_ID_MISSION_REQUEST_LIST_LEN, MAVLINK_MSG_ID_MISSION_REQUEST_LIST_CRC); #endif @@ -219,6 +233,16 @@ static inline uint8_t mavlink_msg_mission_request_list_get_target_component(cons return _MAV_RETURN_uint8_t(msg, 1); } +/** + * @brief Get field mission_type from mission_request_list message + * + * @return Mission type. + */ +static inline uint8_t mavlink_msg_mission_request_list_get_mission_type(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 2); +} + /** * @brief Decode a mission_request_list message into a struct * @@ -230,6 +254,7 @@ static inline void mavlink_msg_mission_request_list_decode(const mavlink_message #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS mission_request_list->target_system = mavlink_msg_mission_request_list_get_target_system(msg); mission_request_list->target_component = mavlink_msg_mission_request_list_get_target_component(msg); + mission_request_list->mission_type = mavlink_msg_mission_request_list_get_mission_type(msg); #else uint8_t len = msg->len < MAVLINK_MSG_ID_MISSION_REQUEST_LIST_LEN? msg->len : MAVLINK_MSG_ID_MISSION_REQUEST_LIST_LEN; memset(mission_request_list, 0, MAVLINK_MSG_ID_MISSION_REQUEST_LIST_LEN); diff --git a/lib/main/MAVLink/common/mavlink_msg_mission_request_partial_list.h b/lib/main/MAVLink/common/mavlink_msg_mission_request_partial_list.h index 0eda74772e..93906934b9 100755 --- a/lib/main/MAVLink/common/mavlink_msg_mission_request_partial_list.h +++ b/lib/main/MAVLink/common/mavlink_msg_mission_request_partial_list.h @@ -9,11 +9,12 @@ typedef struct __mavlink_mission_request_partial_list_t { int16_t end_index; /*< End index, -1 by default (-1: send list to end). Else a valid index of the list*/ uint8_t target_system; /*< System ID*/ uint8_t target_component; /*< Component ID*/ + uint8_t mission_type; /*< Mission type.*/ } mavlink_mission_request_partial_list_t; -#define MAVLINK_MSG_ID_MISSION_REQUEST_PARTIAL_LIST_LEN 6 +#define MAVLINK_MSG_ID_MISSION_REQUEST_PARTIAL_LIST_LEN 7 #define MAVLINK_MSG_ID_MISSION_REQUEST_PARTIAL_LIST_MIN_LEN 6 -#define MAVLINK_MSG_ID_37_LEN 6 +#define MAVLINK_MSG_ID_37_LEN 7 #define MAVLINK_MSG_ID_37_MIN_LEN 6 #define MAVLINK_MSG_ID_MISSION_REQUEST_PARTIAL_LIST_CRC 212 @@ -25,21 +26,23 @@ typedef struct __mavlink_mission_request_partial_list_t { #define MAVLINK_MESSAGE_INFO_MISSION_REQUEST_PARTIAL_LIST { \ 37, \ "MISSION_REQUEST_PARTIAL_LIST", \ - 4, \ + 5, \ { { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 4, offsetof(mavlink_mission_request_partial_list_t, target_system) }, \ { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 5, offsetof(mavlink_mission_request_partial_list_t, target_component) }, \ { "start_index", NULL, MAVLINK_TYPE_INT16_T, 0, 0, offsetof(mavlink_mission_request_partial_list_t, start_index) }, \ { "end_index", NULL, MAVLINK_TYPE_INT16_T, 0, 2, offsetof(mavlink_mission_request_partial_list_t, end_index) }, \ + { "mission_type", NULL, MAVLINK_TYPE_UINT8_T, 0, 6, offsetof(mavlink_mission_request_partial_list_t, mission_type) }, \ } \ } #else #define MAVLINK_MESSAGE_INFO_MISSION_REQUEST_PARTIAL_LIST { \ "MISSION_REQUEST_PARTIAL_LIST", \ - 4, \ + 5, \ { { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 4, offsetof(mavlink_mission_request_partial_list_t, target_system) }, \ { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 5, offsetof(mavlink_mission_request_partial_list_t, target_component) }, \ { "start_index", NULL, MAVLINK_TYPE_INT16_T, 0, 0, offsetof(mavlink_mission_request_partial_list_t, start_index) }, \ { "end_index", NULL, MAVLINK_TYPE_INT16_T, 0, 2, offsetof(mavlink_mission_request_partial_list_t, end_index) }, \ + { "mission_type", NULL, MAVLINK_TYPE_UINT8_T, 0, 6, offsetof(mavlink_mission_request_partial_list_t, mission_type) }, \ } \ } #endif @@ -54,10 +57,11 @@ typedef struct __mavlink_mission_request_partial_list_t { * @param target_component Component ID * @param start_index Start index * @param end_index End index, -1 by default (-1: send list to end). Else a valid index of the list + * @param mission_type Mission type. * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_mission_request_partial_list_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint8_t target_system, uint8_t target_component, int16_t start_index, int16_t end_index) + uint8_t target_system, uint8_t target_component, int16_t start_index, int16_t end_index, uint8_t mission_type) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_MISSION_REQUEST_PARTIAL_LIST_LEN]; @@ -65,6 +69,7 @@ static inline uint16_t mavlink_msg_mission_request_partial_list_pack(uint8_t sys _mav_put_int16_t(buf, 2, end_index); _mav_put_uint8_t(buf, 4, target_system); _mav_put_uint8_t(buf, 5, target_component); + _mav_put_uint8_t(buf, 6, mission_type); memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_MISSION_REQUEST_PARTIAL_LIST_LEN); #else @@ -73,6 +78,7 @@ static inline uint16_t mavlink_msg_mission_request_partial_list_pack(uint8_t sys packet.end_index = end_index; packet.target_system = target_system; packet.target_component = target_component; + packet.mission_type = mission_type; memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_MISSION_REQUEST_PARTIAL_LIST_LEN); #endif @@ -91,11 +97,12 @@ static inline uint16_t mavlink_msg_mission_request_partial_list_pack(uint8_t sys * @param target_component Component ID * @param start_index Start index * @param end_index End index, -1 by default (-1: send list to end). Else a valid index of the list + * @param mission_type Mission type. * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_mission_request_partial_list_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, - uint8_t target_system,uint8_t target_component,int16_t start_index,int16_t end_index) + uint8_t target_system,uint8_t target_component,int16_t start_index,int16_t end_index,uint8_t mission_type) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_MISSION_REQUEST_PARTIAL_LIST_LEN]; @@ -103,6 +110,7 @@ static inline uint16_t mavlink_msg_mission_request_partial_list_pack_chan(uint8_ _mav_put_int16_t(buf, 2, end_index); _mav_put_uint8_t(buf, 4, target_system); _mav_put_uint8_t(buf, 5, target_component); + _mav_put_uint8_t(buf, 6, mission_type); memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_MISSION_REQUEST_PARTIAL_LIST_LEN); #else @@ -111,6 +119,7 @@ static inline uint16_t mavlink_msg_mission_request_partial_list_pack_chan(uint8_ packet.end_index = end_index; packet.target_system = target_system; packet.target_component = target_component; + packet.mission_type = mission_type; memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_MISSION_REQUEST_PARTIAL_LIST_LEN); #endif @@ -129,7 +138,7 @@ static inline uint16_t mavlink_msg_mission_request_partial_list_pack_chan(uint8_ */ static inline uint16_t mavlink_msg_mission_request_partial_list_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_mission_request_partial_list_t* mission_request_partial_list) { - return mavlink_msg_mission_request_partial_list_pack(system_id, component_id, msg, mission_request_partial_list->target_system, mission_request_partial_list->target_component, mission_request_partial_list->start_index, mission_request_partial_list->end_index); + return mavlink_msg_mission_request_partial_list_pack(system_id, component_id, msg, mission_request_partial_list->target_system, mission_request_partial_list->target_component, mission_request_partial_list->start_index, mission_request_partial_list->end_index, mission_request_partial_list->mission_type); } /** @@ -143,7 +152,7 @@ static inline uint16_t mavlink_msg_mission_request_partial_list_encode(uint8_t s */ static inline uint16_t mavlink_msg_mission_request_partial_list_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_mission_request_partial_list_t* mission_request_partial_list) { - return mavlink_msg_mission_request_partial_list_pack_chan(system_id, component_id, chan, msg, mission_request_partial_list->target_system, mission_request_partial_list->target_component, mission_request_partial_list->start_index, mission_request_partial_list->end_index); + return mavlink_msg_mission_request_partial_list_pack_chan(system_id, component_id, chan, msg, mission_request_partial_list->target_system, mission_request_partial_list->target_component, mission_request_partial_list->start_index, mission_request_partial_list->end_index, mission_request_partial_list->mission_type); } /** @@ -154,10 +163,11 @@ static inline uint16_t mavlink_msg_mission_request_partial_list_encode_chan(uint * @param target_component Component ID * @param start_index Start index * @param end_index End index, -1 by default (-1: send list to end). Else a valid index of the list + * @param mission_type Mission type. */ #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS -static inline void mavlink_msg_mission_request_partial_list_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, int16_t start_index, int16_t end_index) +static inline void mavlink_msg_mission_request_partial_list_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, int16_t start_index, int16_t end_index, uint8_t mission_type) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_MISSION_REQUEST_PARTIAL_LIST_LEN]; @@ -165,6 +175,7 @@ static inline void mavlink_msg_mission_request_partial_list_send(mavlink_channel _mav_put_int16_t(buf, 2, end_index); _mav_put_uint8_t(buf, 4, target_system); _mav_put_uint8_t(buf, 5, target_component); + _mav_put_uint8_t(buf, 6, mission_type); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_REQUEST_PARTIAL_LIST, buf, MAVLINK_MSG_ID_MISSION_REQUEST_PARTIAL_LIST_MIN_LEN, MAVLINK_MSG_ID_MISSION_REQUEST_PARTIAL_LIST_LEN, MAVLINK_MSG_ID_MISSION_REQUEST_PARTIAL_LIST_CRC); #else @@ -173,6 +184,7 @@ static inline void mavlink_msg_mission_request_partial_list_send(mavlink_channel packet.end_index = end_index; packet.target_system = target_system; packet.target_component = target_component; + packet.mission_type = mission_type; _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_REQUEST_PARTIAL_LIST, (const char *)&packet, MAVLINK_MSG_ID_MISSION_REQUEST_PARTIAL_LIST_MIN_LEN, MAVLINK_MSG_ID_MISSION_REQUEST_PARTIAL_LIST_LEN, MAVLINK_MSG_ID_MISSION_REQUEST_PARTIAL_LIST_CRC); #endif @@ -186,7 +198,7 @@ static inline void mavlink_msg_mission_request_partial_list_send(mavlink_channel static inline void mavlink_msg_mission_request_partial_list_send_struct(mavlink_channel_t chan, const mavlink_mission_request_partial_list_t* mission_request_partial_list) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - mavlink_msg_mission_request_partial_list_send(chan, mission_request_partial_list->target_system, mission_request_partial_list->target_component, mission_request_partial_list->start_index, mission_request_partial_list->end_index); + mavlink_msg_mission_request_partial_list_send(chan, mission_request_partial_list->target_system, mission_request_partial_list->target_component, mission_request_partial_list->start_index, mission_request_partial_list->end_index, mission_request_partial_list->mission_type); #else _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_REQUEST_PARTIAL_LIST, (const char *)mission_request_partial_list, MAVLINK_MSG_ID_MISSION_REQUEST_PARTIAL_LIST_MIN_LEN, MAVLINK_MSG_ID_MISSION_REQUEST_PARTIAL_LIST_LEN, MAVLINK_MSG_ID_MISSION_REQUEST_PARTIAL_LIST_CRC); #endif @@ -200,7 +212,7 @@ static inline void mavlink_msg_mission_request_partial_list_send_struct(mavlink_ is usually the receive buffer for the channel, and allows a reply to an incoming message with minimum stack space usage. */ -static inline void mavlink_msg_mission_request_partial_list_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, int16_t start_index, int16_t end_index) +static inline void mavlink_msg_mission_request_partial_list_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, int16_t start_index, int16_t end_index, uint8_t mission_type) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char *buf = (char *)msgbuf; @@ -208,6 +220,7 @@ static inline void mavlink_msg_mission_request_partial_list_send_buf(mavlink_mes _mav_put_int16_t(buf, 2, end_index); _mav_put_uint8_t(buf, 4, target_system); _mav_put_uint8_t(buf, 5, target_component); + _mav_put_uint8_t(buf, 6, mission_type); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_REQUEST_PARTIAL_LIST, buf, MAVLINK_MSG_ID_MISSION_REQUEST_PARTIAL_LIST_MIN_LEN, MAVLINK_MSG_ID_MISSION_REQUEST_PARTIAL_LIST_LEN, MAVLINK_MSG_ID_MISSION_REQUEST_PARTIAL_LIST_CRC); #else @@ -216,6 +229,7 @@ static inline void mavlink_msg_mission_request_partial_list_send_buf(mavlink_mes packet->end_index = end_index; packet->target_system = target_system; packet->target_component = target_component; + packet->mission_type = mission_type; _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_REQUEST_PARTIAL_LIST, (const char *)packet, MAVLINK_MSG_ID_MISSION_REQUEST_PARTIAL_LIST_MIN_LEN, MAVLINK_MSG_ID_MISSION_REQUEST_PARTIAL_LIST_LEN, MAVLINK_MSG_ID_MISSION_REQUEST_PARTIAL_LIST_CRC); #endif @@ -267,6 +281,16 @@ static inline int16_t mavlink_msg_mission_request_partial_list_get_end_index(con return _MAV_RETURN_int16_t(msg, 2); } +/** + * @brief Get field mission_type from mission_request_partial_list message + * + * @return Mission type. + */ +static inline uint8_t mavlink_msg_mission_request_partial_list_get_mission_type(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 6); +} + /** * @brief Decode a mission_request_partial_list message into a struct * @@ -280,6 +304,7 @@ static inline void mavlink_msg_mission_request_partial_list_decode(const mavlink mission_request_partial_list->end_index = mavlink_msg_mission_request_partial_list_get_end_index(msg); mission_request_partial_list->target_system = mavlink_msg_mission_request_partial_list_get_target_system(msg); mission_request_partial_list->target_component = mavlink_msg_mission_request_partial_list_get_target_component(msg); + mission_request_partial_list->mission_type = mavlink_msg_mission_request_partial_list_get_mission_type(msg); #else uint8_t len = msg->len < MAVLINK_MSG_ID_MISSION_REQUEST_PARTIAL_LIST_LEN? msg->len : MAVLINK_MSG_ID_MISSION_REQUEST_PARTIAL_LIST_LEN; memset(mission_request_partial_list, 0, MAVLINK_MSG_ID_MISSION_REQUEST_PARTIAL_LIST_LEN); diff --git a/lib/main/MAVLink/common/mavlink_msg_mission_write_partial_list.h b/lib/main/MAVLink/common/mavlink_msg_mission_write_partial_list.h index c1ede1b215..9f8bd08f8c 100755 --- a/lib/main/MAVLink/common/mavlink_msg_mission_write_partial_list.h +++ b/lib/main/MAVLink/common/mavlink_msg_mission_write_partial_list.h @@ -9,11 +9,12 @@ typedef struct __mavlink_mission_write_partial_list_t { int16_t end_index; /*< End index, equal or greater than start index.*/ uint8_t target_system; /*< System ID*/ uint8_t target_component; /*< Component ID*/ + uint8_t mission_type; /*< Mission type.*/ } mavlink_mission_write_partial_list_t; -#define MAVLINK_MSG_ID_MISSION_WRITE_PARTIAL_LIST_LEN 6 +#define MAVLINK_MSG_ID_MISSION_WRITE_PARTIAL_LIST_LEN 7 #define MAVLINK_MSG_ID_MISSION_WRITE_PARTIAL_LIST_MIN_LEN 6 -#define MAVLINK_MSG_ID_38_LEN 6 +#define MAVLINK_MSG_ID_38_LEN 7 #define MAVLINK_MSG_ID_38_MIN_LEN 6 #define MAVLINK_MSG_ID_MISSION_WRITE_PARTIAL_LIST_CRC 9 @@ -25,21 +26,23 @@ typedef struct __mavlink_mission_write_partial_list_t { #define MAVLINK_MESSAGE_INFO_MISSION_WRITE_PARTIAL_LIST { \ 38, \ "MISSION_WRITE_PARTIAL_LIST", \ - 4, \ + 5, \ { { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 4, offsetof(mavlink_mission_write_partial_list_t, target_system) }, \ { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 5, offsetof(mavlink_mission_write_partial_list_t, target_component) }, \ { "start_index", NULL, MAVLINK_TYPE_INT16_T, 0, 0, offsetof(mavlink_mission_write_partial_list_t, start_index) }, \ { "end_index", NULL, MAVLINK_TYPE_INT16_T, 0, 2, offsetof(mavlink_mission_write_partial_list_t, end_index) }, \ + { "mission_type", NULL, MAVLINK_TYPE_UINT8_T, 0, 6, offsetof(mavlink_mission_write_partial_list_t, mission_type) }, \ } \ } #else #define MAVLINK_MESSAGE_INFO_MISSION_WRITE_PARTIAL_LIST { \ "MISSION_WRITE_PARTIAL_LIST", \ - 4, \ + 5, \ { { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 4, offsetof(mavlink_mission_write_partial_list_t, target_system) }, \ { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 5, offsetof(mavlink_mission_write_partial_list_t, target_component) }, \ { "start_index", NULL, MAVLINK_TYPE_INT16_T, 0, 0, offsetof(mavlink_mission_write_partial_list_t, start_index) }, \ { "end_index", NULL, MAVLINK_TYPE_INT16_T, 0, 2, offsetof(mavlink_mission_write_partial_list_t, end_index) }, \ + { "mission_type", NULL, MAVLINK_TYPE_UINT8_T, 0, 6, offsetof(mavlink_mission_write_partial_list_t, mission_type) }, \ } \ } #endif @@ -54,10 +57,11 @@ typedef struct __mavlink_mission_write_partial_list_t { * @param target_component Component ID * @param start_index Start index. Must be smaller / equal to the largest index of the current onboard list. * @param end_index End index, equal or greater than start index. + * @param mission_type Mission type. * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_mission_write_partial_list_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint8_t target_system, uint8_t target_component, int16_t start_index, int16_t end_index) + uint8_t target_system, uint8_t target_component, int16_t start_index, int16_t end_index, uint8_t mission_type) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_MISSION_WRITE_PARTIAL_LIST_LEN]; @@ -65,6 +69,7 @@ static inline uint16_t mavlink_msg_mission_write_partial_list_pack(uint8_t syste _mav_put_int16_t(buf, 2, end_index); _mav_put_uint8_t(buf, 4, target_system); _mav_put_uint8_t(buf, 5, target_component); + _mav_put_uint8_t(buf, 6, mission_type); memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_MISSION_WRITE_PARTIAL_LIST_LEN); #else @@ -73,6 +78,7 @@ static inline uint16_t mavlink_msg_mission_write_partial_list_pack(uint8_t syste packet.end_index = end_index; packet.target_system = target_system; packet.target_component = target_component; + packet.mission_type = mission_type; memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_MISSION_WRITE_PARTIAL_LIST_LEN); #endif @@ -91,11 +97,12 @@ static inline uint16_t mavlink_msg_mission_write_partial_list_pack(uint8_t syste * @param target_component Component ID * @param start_index Start index. Must be smaller / equal to the largest index of the current onboard list. * @param end_index End index, equal or greater than start index. + * @param mission_type Mission type. * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_mission_write_partial_list_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, - uint8_t target_system,uint8_t target_component,int16_t start_index,int16_t end_index) + uint8_t target_system,uint8_t target_component,int16_t start_index,int16_t end_index,uint8_t mission_type) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_MISSION_WRITE_PARTIAL_LIST_LEN]; @@ -103,6 +110,7 @@ static inline uint16_t mavlink_msg_mission_write_partial_list_pack_chan(uint8_t _mav_put_int16_t(buf, 2, end_index); _mav_put_uint8_t(buf, 4, target_system); _mav_put_uint8_t(buf, 5, target_component); + _mav_put_uint8_t(buf, 6, mission_type); memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_MISSION_WRITE_PARTIAL_LIST_LEN); #else @@ -111,6 +119,7 @@ static inline uint16_t mavlink_msg_mission_write_partial_list_pack_chan(uint8_t packet.end_index = end_index; packet.target_system = target_system; packet.target_component = target_component; + packet.mission_type = mission_type; memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_MISSION_WRITE_PARTIAL_LIST_LEN); #endif @@ -129,7 +138,7 @@ static inline uint16_t mavlink_msg_mission_write_partial_list_pack_chan(uint8_t */ static inline uint16_t mavlink_msg_mission_write_partial_list_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_mission_write_partial_list_t* mission_write_partial_list) { - return mavlink_msg_mission_write_partial_list_pack(system_id, component_id, msg, mission_write_partial_list->target_system, mission_write_partial_list->target_component, mission_write_partial_list->start_index, mission_write_partial_list->end_index); + return mavlink_msg_mission_write_partial_list_pack(system_id, component_id, msg, mission_write_partial_list->target_system, mission_write_partial_list->target_component, mission_write_partial_list->start_index, mission_write_partial_list->end_index, mission_write_partial_list->mission_type); } /** @@ -143,7 +152,7 @@ static inline uint16_t mavlink_msg_mission_write_partial_list_encode(uint8_t sys */ static inline uint16_t mavlink_msg_mission_write_partial_list_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_mission_write_partial_list_t* mission_write_partial_list) { - return mavlink_msg_mission_write_partial_list_pack_chan(system_id, component_id, chan, msg, mission_write_partial_list->target_system, mission_write_partial_list->target_component, mission_write_partial_list->start_index, mission_write_partial_list->end_index); + return mavlink_msg_mission_write_partial_list_pack_chan(system_id, component_id, chan, msg, mission_write_partial_list->target_system, mission_write_partial_list->target_component, mission_write_partial_list->start_index, mission_write_partial_list->end_index, mission_write_partial_list->mission_type); } /** @@ -154,10 +163,11 @@ static inline uint16_t mavlink_msg_mission_write_partial_list_encode_chan(uint8_ * @param target_component Component ID * @param start_index Start index. Must be smaller / equal to the largest index of the current onboard list. * @param end_index End index, equal or greater than start index. + * @param mission_type Mission type. */ #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS -static inline void mavlink_msg_mission_write_partial_list_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, int16_t start_index, int16_t end_index) +static inline void mavlink_msg_mission_write_partial_list_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, int16_t start_index, int16_t end_index, uint8_t mission_type) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_MISSION_WRITE_PARTIAL_LIST_LEN]; @@ -165,6 +175,7 @@ static inline void mavlink_msg_mission_write_partial_list_send(mavlink_channel_t _mav_put_int16_t(buf, 2, end_index); _mav_put_uint8_t(buf, 4, target_system); _mav_put_uint8_t(buf, 5, target_component); + _mav_put_uint8_t(buf, 6, mission_type); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_WRITE_PARTIAL_LIST, buf, MAVLINK_MSG_ID_MISSION_WRITE_PARTIAL_LIST_MIN_LEN, MAVLINK_MSG_ID_MISSION_WRITE_PARTIAL_LIST_LEN, MAVLINK_MSG_ID_MISSION_WRITE_PARTIAL_LIST_CRC); #else @@ -173,6 +184,7 @@ static inline void mavlink_msg_mission_write_partial_list_send(mavlink_channel_t packet.end_index = end_index; packet.target_system = target_system; packet.target_component = target_component; + packet.mission_type = mission_type; _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_WRITE_PARTIAL_LIST, (const char *)&packet, MAVLINK_MSG_ID_MISSION_WRITE_PARTIAL_LIST_MIN_LEN, MAVLINK_MSG_ID_MISSION_WRITE_PARTIAL_LIST_LEN, MAVLINK_MSG_ID_MISSION_WRITE_PARTIAL_LIST_CRC); #endif @@ -186,7 +198,7 @@ static inline void mavlink_msg_mission_write_partial_list_send(mavlink_channel_t static inline void mavlink_msg_mission_write_partial_list_send_struct(mavlink_channel_t chan, const mavlink_mission_write_partial_list_t* mission_write_partial_list) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - mavlink_msg_mission_write_partial_list_send(chan, mission_write_partial_list->target_system, mission_write_partial_list->target_component, mission_write_partial_list->start_index, mission_write_partial_list->end_index); + mavlink_msg_mission_write_partial_list_send(chan, mission_write_partial_list->target_system, mission_write_partial_list->target_component, mission_write_partial_list->start_index, mission_write_partial_list->end_index, mission_write_partial_list->mission_type); #else _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_WRITE_PARTIAL_LIST, (const char *)mission_write_partial_list, MAVLINK_MSG_ID_MISSION_WRITE_PARTIAL_LIST_MIN_LEN, MAVLINK_MSG_ID_MISSION_WRITE_PARTIAL_LIST_LEN, MAVLINK_MSG_ID_MISSION_WRITE_PARTIAL_LIST_CRC); #endif @@ -200,7 +212,7 @@ static inline void mavlink_msg_mission_write_partial_list_send_struct(mavlink_ch is usually the receive buffer for the channel, and allows a reply to an incoming message with minimum stack space usage. */ -static inline void mavlink_msg_mission_write_partial_list_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, int16_t start_index, int16_t end_index) +static inline void mavlink_msg_mission_write_partial_list_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, int16_t start_index, int16_t end_index, uint8_t mission_type) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char *buf = (char *)msgbuf; @@ -208,6 +220,7 @@ static inline void mavlink_msg_mission_write_partial_list_send_buf(mavlink_messa _mav_put_int16_t(buf, 2, end_index); _mav_put_uint8_t(buf, 4, target_system); _mav_put_uint8_t(buf, 5, target_component); + _mav_put_uint8_t(buf, 6, mission_type); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_WRITE_PARTIAL_LIST, buf, MAVLINK_MSG_ID_MISSION_WRITE_PARTIAL_LIST_MIN_LEN, MAVLINK_MSG_ID_MISSION_WRITE_PARTIAL_LIST_LEN, MAVLINK_MSG_ID_MISSION_WRITE_PARTIAL_LIST_CRC); #else @@ -216,6 +229,7 @@ static inline void mavlink_msg_mission_write_partial_list_send_buf(mavlink_messa packet->end_index = end_index; packet->target_system = target_system; packet->target_component = target_component; + packet->mission_type = mission_type; _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MISSION_WRITE_PARTIAL_LIST, (const char *)packet, MAVLINK_MSG_ID_MISSION_WRITE_PARTIAL_LIST_MIN_LEN, MAVLINK_MSG_ID_MISSION_WRITE_PARTIAL_LIST_LEN, MAVLINK_MSG_ID_MISSION_WRITE_PARTIAL_LIST_CRC); #endif @@ -267,6 +281,16 @@ static inline int16_t mavlink_msg_mission_write_partial_list_get_end_index(const return _MAV_RETURN_int16_t(msg, 2); } +/** + * @brief Get field mission_type from mission_write_partial_list message + * + * @return Mission type. + */ +static inline uint8_t mavlink_msg_mission_write_partial_list_get_mission_type(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 6); +} + /** * @brief Decode a mission_write_partial_list message into a struct * @@ -280,6 +304,7 @@ static inline void mavlink_msg_mission_write_partial_list_decode(const mavlink_m mission_write_partial_list->end_index = mavlink_msg_mission_write_partial_list_get_end_index(msg); mission_write_partial_list->target_system = mavlink_msg_mission_write_partial_list_get_target_system(msg); mission_write_partial_list->target_component = mavlink_msg_mission_write_partial_list_get_target_component(msg); + mission_write_partial_list->mission_type = mavlink_msg_mission_write_partial_list_get_mission_type(msg); #else uint8_t len = msg->len < MAVLINK_MSG_ID_MISSION_WRITE_PARTIAL_LIST_LEN? msg->len : MAVLINK_MSG_ID_MISSION_WRITE_PARTIAL_LIST_LEN; memset(mission_write_partial_list, 0, MAVLINK_MSG_ID_MISSION_WRITE_PARTIAL_LIST_LEN); diff --git a/lib/main/MAVLink/common/mavlink_msg_mount_orientation.h b/lib/main/MAVLink/common/mavlink_msg_mount_orientation.h new file mode 100644 index 0000000000..96bfda2501 --- /dev/null +++ b/lib/main/MAVLink/common/mavlink_msg_mount_orientation.h @@ -0,0 +1,313 @@ +#pragma once +// MESSAGE MOUNT_ORIENTATION PACKING + +#define MAVLINK_MSG_ID_MOUNT_ORIENTATION 265 + + +typedef struct __mavlink_mount_orientation_t { + uint32_t time_boot_ms; /*< [ms] Timestamp (time since system boot).*/ + float roll; /*< [deg] Roll in global frame (set to NaN for invalid).*/ + float pitch; /*< [deg] Pitch in global frame (set to NaN for invalid).*/ + float yaw; /*< [deg] Yaw relative to vehicle (set to NaN for invalid).*/ + float yaw_absolute; /*< [deg] Yaw in absolute frame relative to Earth's North, north is 0 (set to NaN for invalid).*/ +} mavlink_mount_orientation_t; + +#define MAVLINK_MSG_ID_MOUNT_ORIENTATION_LEN 20 +#define MAVLINK_MSG_ID_MOUNT_ORIENTATION_MIN_LEN 16 +#define MAVLINK_MSG_ID_265_LEN 20 +#define MAVLINK_MSG_ID_265_MIN_LEN 16 + +#define MAVLINK_MSG_ID_MOUNT_ORIENTATION_CRC 26 +#define MAVLINK_MSG_ID_265_CRC 26 + + + +#if MAVLINK_COMMAND_24BIT +#define MAVLINK_MESSAGE_INFO_MOUNT_ORIENTATION { \ + 265, \ + "MOUNT_ORIENTATION", \ + 5, \ + { { "time_boot_ms", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_mount_orientation_t, time_boot_ms) }, \ + { "roll", NULL, MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_mount_orientation_t, roll) }, \ + { "pitch", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_mount_orientation_t, pitch) }, \ + { "yaw", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_mount_orientation_t, yaw) }, \ + { "yaw_absolute", NULL, MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_mount_orientation_t, yaw_absolute) }, \ + } \ +} +#else +#define MAVLINK_MESSAGE_INFO_MOUNT_ORIENTATION { \ + "MOUNT_ORIENTATION", \ + 5, \ + { { "time_boot_ms", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_mount_orientation_t, time_boot_ms) }, \ + { "roll", NULL, MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_mount_orientation_t, roll) }, \ + { "pitch", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_mount_orientation_t, pitch) }, \ + { "yaw", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_mount_orientation_t, yaw) }, \ + { "yaw_absolute", NULL, MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_mount_orientation_t, yaw_absolute) }, \ + } \ +} +#endif + +/** + * @brief Pack a mount_orientation message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param time_boot_ms [ms] Timestamp (time since system boot). + * @param roll [deg] Roll in global frame (set to NaN for invalid). + * @param pitch [deg] Pitch in global frame (set to NaN for invalid). + * @param yaw [deg] Yaw relative to vehicle (set to NaN for invalid). + * @param yaw_absolute [deg] Yaw in absolute frame relative to Earth's North, north is 0 (set to NaN for invalid). + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_mount_orientation_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint32_t time_boot_ms, float roll, float pitch, float yaw, float yaw_absolute) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_MOUNT_ORIENTATION_LEN]; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_float(buf, 4, roll); + _mav_put_float(buf, 8, pitch); + _mav_put_float(buf, 12, yaw); + _mav_put_float(buf, 16, yaw_absolute); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_MOUNT_ORIENTATION_LEN); +#else + mavlink_mount_orientation_t packet; + packet.time_boot_ms = time_boot_ms; + packet.roll = roll; + packet.pitch = pitch; + packet.yaw = yaw; + packet.yaw_absolute = yaw_absolute; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_MOUNT_ORIENTATION_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_MOUNT_ORIENTATION; + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_MOUNT_ORIENTATION_MIN_LEN, MAVLINK_MSG_ID_MOUNT_ORIENTATION_LEN, MAVLINK_MSG_ID_MOUNT_ORIENTATION_CRC); +} + +/** + * @brief Pack a mount_orientation message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param time_boot_ms [ms] Timestamp (time since system boot). + * @param roll [deg] Roll in global frame (set to NaN for invalid). + * @param pitch [deg] Pitch in global frame (set to NaN for invalid). + * @param yaw [deg] Yaw relative to vehicle (set to NaN for invalid). + * @param yaw_absolute [deg] Yaw in absolute frame relative to Earth's North, north is 0 (set to NaN for invalid). + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_mount_orientation_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint32_t time_boot_ms,float roll,float pitch,float yaw,float yaw_absolute) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_MOUNT_ORIENTATION_LEN]; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_float(buf, 4, roll); + _mav_put_float(buf, 8, pitch); + _mav_put_float(buf, 12, yaw); + _mav_put_float(buf, 16, yaw_absolute); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_MOUNT_ORIENTATION_LEN); +#else + mavlink_mount_orientation_t packet; + packet.time_boot_ms = time_boot_ms; + packet.roll = roll; + packet.pitch = pitch; + packet.yaw = yaw; + packet.yaw_absolute = yaw_absolute; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_MOUNT_ORIENTATION_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_MOUNT_ORIENTATION; + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_MOUNT_ORIENTATION_MIN_LEN, MAVLINK_MSG_ID_MOUNT_ORIENTATION_LEN, MAVLINK_MSG_ID_MOUNT_ORIENTATION_CRC); +} + +/** + * @brief Encode a mount_orientation struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param mount_orientation C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_mount_orientation_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_mount_orientation_t* mount_orientation) +{ + return mavlink_msg_mount_orientation_pack(system_id, component_id, msg, mount_orientation->time_boot_ms, mount_orientation->roll, mount_orientation->pitch, mount_orientation->yaw, mount_orientation->yaw_absolute); +} + +/** + * @brief Encode a mount_orientation struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param mount_orientation C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_mount_orientation_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_mount_orientation_t* mount_orientation) +{ + return mavlink_msg_mount_orientation_pack_chan(system_id, component_id, chan, msg, mount_orientation->time_boot_ms, mount_orientation->roll, mount_orientation->pitch, mount_orientation->yaw, mount_orientation->yaw_absolute); +} + +/** + * @brief Send a mount_orientation message + * @param chan MAVLink channel to send the message + * + * @param time_boot_ms [ms] Timestamp (time since system boot). + * @param roll [deg] Roll in global frame (set to NaN for invalid). + * @param pitch [deg] Pitch in global frame (set to NaN for invalid). + * @param yaw [deg] Yaw relative to vehicle (set to NaN for invalid). + * @param yaw_absolute [deg] Yaw in absolute frame relative to Earth's North, north is 0 (set to NaN for invalid). + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_mount_orientation_send(mavlink_channel_t chan, uint32_t time_boot_ms, float roll, float pitch, float yaw, float yaw_absolute) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_MOUNT_ORIENTATION_LEN]; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_float(buf, 4, roll); + _mav_put_float(buf, 8, pitch); + _mav_put_float(buf, 12, yaw); + _mav_put_float(buf, 16, yaw_absolute); + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MOUNT_ORIENTATION, buf, MAVLINK_MSG_ID_MOUNT_ORIENTATION_MIN_LEN, MAVLINK_MSG_ID_MOUNT_ORIENTATION_LEN, MAVLINK_MSG_ID_MOUNT_ORIENTATION_CRC); +#else + mavlink_mount_orientation_t packet; + packet.time_boot_ms = time_boot_ms; + packet.roll = roll; + packet.pitch = pitch; + packet.yaw = yaw; + packet.yaw_absolute = yaw_absolute; + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MOUNT_ORIENTATION, (const char *)&packet, MAVLINK_MSG_ID_MOUNT_ORIENTATION_MIN_LEN, MAVLINK_MSG_ID_MOUNT_ORIENTATION_LEN, MAVLINK_MSG_ID_MOUNT_ORIENTATION_CRC); +#endif +} + +/** + * @brief Send a mount_orientation message + * @param chan MAVLink channel to send the message + * @param struct The MAVLink struct to serialize + */ +static inline void mavlink_msg_mount_orientation_send_struct(mavlink_channel_t chan, const mavlink_mount_orientation_t* mount_orientation) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + mavlink_msg_mount_orientation_send(chan, mount_orientation->time_boot_ms, mount_orientation->roll, mount_orientation->pitch, mount_orientation->yaw, mount_orientation->yaw_absolute); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MOUNT_ORIENTATION, (const char *)mount_orientation, MAVLINK_MSG_ID_MOUNT_ORIENTATION_MIN_LEN, MAVLINK_MSG_ID_MOUNT_ORIENTATION_LEN, MAVLINK_MSG_ID_MOUNT_ORIENTATION_CRC); +#endif +} + +#if MAVLINK_MSG_ID_MOUNT_ORIENTATION_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_mount_orientation_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint32_t time_boot_ms, float roll, float pitch, float yaw, float yaw_absolute) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_float(buf, 4, roll); + _mav_put_float(buf, 8, pitch); + _mav_put_float(buf, 12, yaw); + _mav_put_float(buf, 16, yaw_absolute); + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MOUNT_ORIENTATION, buf, MAVLINK_MSG_ID_MOUNT_ORIENTATION_MIN_LEN, MAVLINK_MSG_ID_MOUNT_ORIENTATION_LEN, MAVLINK_MSG_ID_MOUNT_ORIENTATION_CRC); +#else + mavlink_mount_orientation_t *packet = (mavlink_mount_orientation_t *)msgbuf; + packet->time_boot_ms = time_boot_ms; + packet->roll = roll; + packet->pitch = pitch; + packet->yaw = yaw; + packet->yaw_absolute = yaw_absolute; + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_MOUNT_ORIENTATION, (const char *)packet, MAVLINK_MSG_ID_MOUNT_ORIENTATION_MIN_LEN, MAVLINK_MSG_ID_MOUNT_ORIENTATION_LEN, MAVLINK_MSG_ID_MOUNT_ORIENTATION_CRC); +#endif +} +#endif + +#endif + +// MESSAGE MOUNT_ORIENTATION UNPACKING + + +/** + * @brief Get field time_boot_ms from mount_orientation message + * + * @return [ms] Timestamp (time since system boot). + */ +static inline uint32_t mavlink_msg_mount_orientation_get_time_boot_ms(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint32_t(msg, 0); +} + +/** + * @brief Get field roll from mount_orientation message + * + * @return [deg] Roll in global frame (set to NaN for invalid). + */ +static inline float mavlink_msg_mount_orientation_get_roll(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 4); +} + +/** + * @brief Get field pitch from mount_orientation message + * + * @return [deg] Pitch in global frame (set to NaN for invalid). + */ +static inline float mavlink_msg_mount_orientation_get_pitch(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 8); +} + +/** + * @brief Get field yaw from mount_orientation message + * + * @return [deg] Yaw relative to vehicle (set to NaN for invalid). + */ +static inline float mavlink_msg_mount_orientation_get_yaw(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 12); +} + +/** + * @brief Get field yaw_absolute from mount_orientation message + * + * @return [deg] Yaw in absolute frame relative to Earth's North, north is 0 (set to NaN for invalid). + */ +static inline float mavlink_msg_mount_orientation_get_yaw_absolute(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 16); +} + +/** + * @brief Decode a mount_orientation message into a struct + * + * @param msg The message to decode + * @param mount_orientation C-struct to decode the message contents into + */ +static inline void mavlink_msg_mount_orientation_decode(const mavlink_message_t* msg, mavlink_mount_orientation_t* mount_orientation) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + mount_orientation->time_boot_ms = mavlink_msg_mount_orientation_get_time_boot_ms(msg); + mount_orientation->roll = mavlink_msg_mount_orientation_get_roll(msg); + mount_orientation->pitch = mavlink_msg_mount_orientation_get_pitch(msg); + mount_orientation->yaw = mavlink_msg_mount_orientation_get_yaw(msg); + mount_orientation->yaw_absolute = mavlink_msg_mount_orientation_get_yaw_absolute(msg); +#else + uint8_t len = msg->len < MAVLINK_MSG_ID_MOUNT_ORIENTATION_LEN? msg->len : MAVLINK_MSG_ID_MOUNT_ORIENTATION_LEN; + memset(mount_orientation, 0, MAVLINK_MSG_ID_MOUNT_ORIENTATION_LEN); + memcpy(mount_orientation, _MAV_PAYLOAD(msg), len); +#endif +} diff --git a/lib/main/MAVLink/common/mavlink_msg_obstacle_distance.h b/lib/main/MAVLink/common/mavlink_msg_obstacle_distance.h new file mode 100644 index 0000000000..641f2627fe --- /dev/null +++ b/lib/main/MAVLink/common/mavlink_msg_obstacle_distance.h @@ -0,0 +1,405 @@ +#pragma once +// MESSAGE OBSTACLE_DISTANCE PACKING + +#define MAVLINK_MSG_ID_OBSTACLE_DISTANCE 330 + +MAVPACKED( +typedef struct __mavlink_obstacle_distance_t { + uint64_t time_usec; /*< [us] Timestamp (UNIX Epoch time or time since system boot). The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude of the number.*/ + uint16_t distances[72]; /*< [cm] Distance of obstacles around the vehicle with index 0 corresponding to north + angle_offset, unless otherwise specified in the frame. A value of 0 is valid and means that the obstacle is practically touching the sensor. A value of max_distance +1 means no obstacle is present. A value of UINT16_MAX for unknown/not used. In a array element, one unit corresponds to 1cm.*/ + uint16_t min_distance; /*< [cm] Minimum distance the sensor can measure.*/ + uint16_t max_distance; /*< [cm] Maximum distance the sensor can measure.*/ + uint8_t sensor_type; /*< Class id of the distance sensor type.*/ + uint8_t increment; /*< [deg] Angular width in degrees of each array element. Increment direction is clockwise. This field is ignored if increment_f is non-zero.*/ + float increment_f; /*< [deg] Angular width in degrees of each array element as a float. If non-zero then this value is used instead of the uint8_t increment field. Positive is clockwise direction, negative is counter-clockwise.*/ + float angle_offset; /*< [deg] Relative angle offset of the 0-index element in the distances array. Value of 0 corresponds to forward. Positive is clockwise direction, negative is counter-clockwise.*/ + uint8_t frame; /*< Coordinate frame of reference for the yaw rotation and offset of the sensor data. Defaults to MAV_FRAME_GLOBAL, which is north aligned. For body-mounted sensors use MAV_FRAME_BODY_FRD, which is vehicle front aligned.*/ +}) mavlink_obstacle_distance_t; + +#define MAVLINK_MSG_ID_OBSTACLE_DISTANCE_LEN 167 +#define MAVLINK_MSG_ID_OBSTACLE_DISTANCE_MIN_LEN 158 +#define MAVLINK_MSG_ID_330_LEN 167 +#define MAVLINK_MSG_ID_330_MIN_LEN 158 + +#define MAVLINK_MSG_ID_OBSTACLE_DISTANCE_CRC 23 +#define MAVLINK_MSG_ID_330_CRC 23 + +#define MAVLINK_MSG_OBSTACLE_DISTANCE_FIELD_DISTANCES_LEN 72 + +#if MAVLINK_COMMAND_24BIT +#define MAVLINK_MESSAGE_INFO_OBSTACLE_DISTANCE { \ + 330, \ + "OBSTACLE_DISTANCE", \ + 9, \ + { { "time_usec", NULL, MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_obstacle_distance_t, time_usec) }, \ + { "sensor_type", NULL, MAVLINK_TYPE_UINT8_T, 0, 156, offsetof(mavlink_obstacle_distance_t, sensor_type) }, \ + { "distances", NULL, MAVLINK_TYPE_UINT16_T, 72, 8, offsetof(mavlink_obstacle_distance_t, distances) }, \ + { "increment", NULL, MAVLINK_TYPE_UINT8_T, 0, 157, offsetof(mavlink_obstacle_distance_t, increment) }, \ + { "min_distance", NULL, MAVLINK_TYPE_UINT16_T, 0, 152, offsetof(mavlink_obstacle_distance_t, min_distance) }, \ + { "max_distance", NULL, MAVLINK_TYPE_UINT16_T, 0, 154, offsetof(mavlink_obstacle_distance_t, max_distance) }, \ + { "increment_f", NULL, MAVLINK_TYPE_FLOAT, 0, 158, offsetof(mavlink_obstacle_distance_t, increment_f) }, \ + { "angle_offset", NULL, MAVLINK_TYPE_FLOAT, 0, 162, offsetof(mavlink_obstacle_distance_t, angle_offset) }, \ + { "frame", NULL, MAVLINK_TYPE_UINT8_T, 0, 166, offsetof(mavlink_obstacle_distance_t, frame) }, \ + } \ +} +#else +#define MAVLINK_MESSAGE_INFO_OBSTACLE_DISTANCE { \ + "OBSTACLE_DISTANCE", \ + 9, \ + { { "time_usec", NULL, MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_obstacle_distance_t, time_usec) }, \ + { "sensor_type", NULL, MAVLINK_TYPE_UINT8_T, 0, 156, offsetof(mavlink_obstacle_distance_t, sensor_type) }, \ + { "distances", NULL, MAVLINK_TYPE_UINT16_T, 72, 8, offsetof(mavlink_obstacle_distance_t, distances) }, \ + { "increment", NULL, MAVLINK_TYPE_UINT8_T, 0, 157, offsetof(mavlink_obstacle_distance_t, increment) }, \ + { "min_distance", NULL, MAVLINK_TYPE_UINT16_T, 0, 152, offsetof(mavlink_obstacle_distance_t, min_distance) }, \ + { "max_distance", NULL, MAVLINK_TYPE_UINT16_T, 0, 154, offsetof(mavlink_obstacle_distance_t, max_distance) }, \ + { "increment_f", NULL, MAVLINK_TYPE_FLOAT, 0, 158, offsetof(mavlink_obstacle_distance_t, increment_f) }, \ + { "angle_offset", NULL, MAVLINK_TYPE_FLOAT, 0, 162, offsetof(mavlink_obstacle_distance_t, angle_offset) }, \ + { "frame", NULL, MAVLINK_TYPE_UINT8_T, 0, 166, offsetof(mavlink_obstacle_distance_t, frame) }, \ + } \ +} +#endif + +/** + * @brief Pack a obstacle_distance message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param time_usec [us] Timestamp (UNIX Epoch time or time since system boot). The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude of the number. + * @param sensor_type Class id of the distance sensor type. + * @param distances [cm] Distance of obstacles around the vehicle with index 0 corresponding to north + angle_offset, unless otherwise specified in the frame. A value of 0 is valid and means that the obstacle is practically touching the sensor. A value of max_distance +1 means no obstacle is present. A value of UINT16_MAX for unknown/not used. In a array element, one unit corresponds to 1cm. + * @param increment [deg] Angular width in degrees of each array element. Increment direction is clockwise. This field is ignored if increment_f is non-zero. + * @param min_distance [cm] Minimum distance the sensor can measure. + * @param max_distance [cm] Maximum distance the sensor can measure. + * @param increment_f [deg] Angular width in degrees of each array element as a float. If non-zero then this value is used instead of the uint8_t increment field. Positive is clockwise direction, negative is counter-clockwise. + * @param angle_offset [deg] Relative angle offset of the 0-index element in the distances array. Value of 0 corresponds to forward. Positive is clockwise direction, negative is counter-clockwise. + * @param frame Coordinate frame of reference for the yaw rotation and offset of the sensor data. Defaults to MAV_FRAME_GLOBAL, which is north aligned. For body-mounted sensors use MAV_FRAME_BODY_FRD, which is vehicle front aligned. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_obstacle_distance_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint64_t time_usec, uint8_t sensor_type, const uint16_t *distances, uint8_t increment, uint16_t min_distance, uint16_t max_distance, float increment_f, float angle_offset, uint8_t frame) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_OBSTACLE_DISTANCE_LEN]; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_uint16_t(buf, 152, min_distance); + _mav_put_uint16_t(buf, 154, max_distance); + _mav_put_uint8_t(buf, 156, sensor_type); + _mav_put_uint8_t(buf, 157, increment); + _mav_put_float(buf, 158, increment_f); + _mav_put_float(buf, 162, angle_offset); + _mav_put_uint8_t(buf, 166, frame); + _mav_put_uint16_t_array(buf, 8, distances, 72); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_OBSTACLE_DISTANCE_LEN); +#else + mavlink_obstacle_distance_t packet; + packet.time_usec = time_usec; + packet.min_distance = min_distance; + packet.max_distance = max_distance; + packet.sensor_type = sensor_type; + packet.increment = increment; + packet.increment_f = increment_f; + packet.angle_offset = angle_offset; + packet.frame = frame; + mav_array_memcpy(packet.distances, distances, sizeof(uint16_t)*72); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_OBSTACLE_DISTANCE_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_OBSTACLE_DISTANCE; + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_OBSTACLE_DISTANCE_MIN_LEN, MAVLINK_MSG_ID_OBSTACLE_DISTANCE_LEN, MAVLINK_MSG_ID_OBSTACLE_DISTANCE_CRC); +} + +/** + * @brief Pack a obstacle_distance message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param time_usec [us] Timestamp (UNIX Epoch time or time since system boot). The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude of the number. + * @param sensor_type Class id of the distance sensor type. + * @param distances [cm] Distance of obstacles around the vehicle with index 0 corresponding to north + angle_offset, unless otherwise specified in the frame. A value of 0 is valid and means that the obstacle is practically touching the sensor. A value of max_distance +1 means no obstacle is present. A value of UINT16_MAX for unknown/not used. In a array element, one unit corresponds to 1cm. + * @param increment [deg] Angular width in degrees of each array element. Increment direction is clockwise. This field is ignored if increment_f is non-zero. + * @param min_distance [cm] Minimum distance the sensor can measure. + * @param max_distance [cm] Maximum distance the sensor can measure. + * @param increment_f [deg] Angular width in degrees of each array element as a float. If non-zero then this value is used instead of the uint8_t increment field. Positive is clockwise direction, negative is counter-clockwise. + * @param angle_offset [deg] Relative angle offset of the 0-index element in the distances array. Value of 0 corresponds to forward. Positive is clockwise direction, negative is counter-clockwise. + * @param frame Coordinate frame of reference for the yaw rotation and offset of the sensor data. Defaults to MAV_FRAME_GLOBAL, which is north aligned. For body-mounted sensors use MAV_FRAME_BODY_FRD, which is vehicle front aligned. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_obstacle_distance_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint64_t time_usec,uint8_t sensor_type,const uint16_t *distances,uint8_t increment,uint16_t min_distance,uint16_t max_distance,float increment_f,float angle_offset,uint8_t frame) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_OBSTACLE_DISTANCE_LEN]; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_uint16_t(buf, 152, min_distance); + _mav_put_uint16_t(buf, 154, max_distance); + _mav_put_uint8_t(buf, 156, sensor_type); + _mav_put_uint8_t(buf, 157, increment); + _mav_put_float(buf, 158, increment_f); + _mav_put_float(buf, 162, angle_offset); + _mav_put_uint8_t(buf, 166, frame); + _mav_put_uint16_t_array(buf, 8, distances, 72); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_OBSTACLE_DISTANCE_LEN); +#else + mavlink_obstacle_distance_t packet; + packet.time_usec = time_usec; + packet.min_distance = min_distance; + packet.max_distance = max_distance; + packet.sensor_type = sensor_type; + packet.increment = increment; + packet.increment_f = increment_f; + packet.angle_offset = angle_offset; + packet.frame = frame; + mav_array_memcpy(packet.distances, distances, sizeof(uint16_t)*72); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_OBSTACLE_DISTANCE_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_OBSTACLE_DISTANCE; + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_OBSTACLE_DISTANCE_MIN_LEN, MAVLINK_MSG_ID_OBSTACLE_DISTANCE_LEN, MAVLINK_MSG_ID_OBSTACLE_DISTANCE_CRC); +} + +/** + * @brief Encode a obstacle_distance struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param obstacle_distance C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_obstacle_distance_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_obstacle_distance_t* obstacle_distance) +{ + return mavlink_msg_obstacle_distance_pack(system_id, component_id, msg, obstacle_distance->time_usec, obstacle_distance->sensor_type, obstacle_distance->distances, obstacle_distance->increment, obstacle_distance->min_distance, obstacle_distance->max_distance, obstacle_distance->increment_f, obstacle_distance->angle_offset, obstacle_distance->frame); +} + +/** + * @brief Encode a obstacle_distance struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param obstacle_distance C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_obstacle_distance_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_obstacle_distance_t* obstacle_distance) +{ + return mavlink_msg_obstacle_distance_pack_chan(system_id, component_id, chan, msg, obstacle_distance->time_usec, obstacle_distance->sensor_type, obstacle_distance->distances, obstacle_distance->increment, obstacle_distance->min_distance, obstacle_distance->max_distance, obstacle_distance->increment_f, obstacle_distance->angle_offset, obstacle_distance->frame); +} + +/** + * @brief Send a obstacle_distance message + * @param chan MAVLink channel to send the message + * + * @param time_usec [us] Timestamp (UNIX Epoch time or time since system boot). The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude of the number. + * @param sensor_type Class id of the distance sensor type. + * @param distances [cm] Distance of obstacles around the vehicle with index 0 corresponding to north + angle_offset, unless otherwise specified in the frame. A value of 0 is valid and means that the obstacle is practically touching the sensor. A value of max_distance +1 means no obstacle is present. A value of UINT16_MAX for unknown/not used. In a array element, one unit corresponds to 1cm. + * @param increment [deg] Angular width in degrees of each array element. Increment direction is clockwise. This field is ignored if increment_f is non-zero. + * @param min_distance [cm] Minimum distance the sensor can measure. + * @param max_distance [cm] Maximum distance the sensor can measure. + * @param increment_f [deg] Angular width in degrees of each array element as a float. If non-zero then this value is used instead of the uint8_t increment field. Positive is clockwise direction, negative is counter-clockwise. + * @param angle_offset [deg] Relative angle offset of the 0-index element in the distances array. Value of 0 corresponds to forward. Positive is clockwise direction, negative is counter-clockwise. + * @param frame Coordinate frame of reference for the yaw rotation and offset of the sensor data. Defaults to MAV_FRAME_GLOBAL, which is north aligned. For body-mounted sensors use MAV_FRAME_BODY_FRD, which is vehicle front aligned. + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_obstacle_distance_send(mavlink_channel_t chan, uint64_t time_usec, uint8_t sensor_type, const uint16_t *distances, uint8_t increment, uint16_t min_distance, uint16_t max_distance, float increment_f, float angle_offset, uint8_t frame) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_OBSTACLE_DISTANCE_LEN]; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_uint16_t(buf, 152, min_distance); + _mav_put_uint16_t(buf, 154, max_distance); + _mav_put_uint8_t(buf, 156, sensor_type); + _mav_put_uint8_t(buf, 157, increment); + _mav_put_float(buf, 158, increment_f); + _mav_put_float(buf, 162, angle_offset); + _mav_put_uint8_t(buf, 166, frame); + _mav_put_uint16_t_array(buf, 8, distances, 72); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OBSTACLE_DISTANCE, buf, MAVLINK_MSG_ID_OBSTACLE_DISTANCE_MIN_LEN, MAVLINK_MSG_ID_OBSTACLE_DISTANCE_LEN, MAVLINK_MSG_ID_OBSTACLE_DISTANCE_CRC); +#else + mavlink_obstacle_distance_t packet; + packet.time_usec = time_usec; + packet.min_distance = min_distance; + packet.max_distance = max_distance; + packet.sensor_type = sensor_type; + packet.increment = increment; + packet.increment_f = increment_f; + packet.angle_offset = angle_offset; + packet.frame = frame; + mav_array_memcpy(packet.distances, distances, sizeof(uint16_t)*72); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OBSTACLE_DISTANCE, (const char *)&packet, MAVLINK_MSG_ID_OBSTACLE_DISTANCE_MIN_LEN, MAVLINK_MSG_ID_OBSTACLE_DISTANCE_LEN, MAVLINK_MSG_ID_OBSTACLE_DISTANCE_CRC); +#endif +} + +/** + * @brief Send a obstacle_distance message + * @param chan MAVLink channel to send the message + * @param struct The MAVLink struct to serialize + */ +static inline void mavlink_msg_obstacle_distance_send_struct(mavlink_channel_t chan, const mavlink_obstacle_distance_t* obstacle_distance) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + mavlink_msg_obstacle_distance_send(chan, obstacle_distance->time_usec, obstacle_distance->sensor_type, obstacle_distance->distances, obstacle_distance->increment, obstacle_distance->min_distance, obstacle_distance->max_distance, obstacle_distance->increment_f, obstacle_distance->angle_offset, obstacle_distance->frame); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OBSTACLE_DISTANCE, (const char *)obstacle_distance, MAVLINK_MSG_ID_OBSTACLE_DISTANCE_MIN_LEN, MAVLINK_MSG_ID_OBSTACLE_DISTANCE_LEN, MAVLINK_MSG_ID_OBSTACLE_DISTANCE_CRC); +#endif +} + +#if MAVLINK_MSG_ID_OBSTACLE_DISTANCE_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_obstacle_distance_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint64_t time_usec, uint8_t sensor_type, const uint16_t *distances, uint8_t increment, uint16_t min_distance, uint16_t max_distance, float increment_f, float angle_offset, uint8_t frame) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_uint16_t(buf, 152, min_distance); + _mav_put_uint16_t(buf, 154, max_distance); + _mav_put_uint8_t(buf, 156, sensor_type); + _mav_put_uint8_t(buf, 157, increment); + _mav_put_float(buf, 158, increment_f); + _mav_put_float(buf, 162, angle_offset); + _mav_put_uint8_t(buf, 166, frame); + _mav_put_uint16_t_array(buf, 8, distances, 72); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OBSTACLE_DISTANCE, buf, MAVLINK_MSG_ID_OBSTACLE_DISTANCE_MIN_LEN, MAVLINK_MSG_ID_OBSTACLE_DISTANCE_LEN, MAVLINK_MSG_ID_OBSTACLE_DISTANCE_CRC); +#else + mavlink_obstacle_distance_t *packet = (mavlink_obstacle_distance_t *)msgbuf; + packet->time_usec = time_usec; + packet->min_distance = min_distance; + packet->max_distance = max_distance; + packet->sensor_type = sensor_type; + packet->increment = increment; + packet->increment_f = increment_f; + packet->angle_offset = angle_offset; + packet->frame = frame; + mav_array_memcpy(packet->distances, distances, sizeof(uint16_t)*72); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OBSTACLE_DISTANCE, (const char *)packet, MAVLINK_MSG_ID_OBSTACLE_DISTANCE_MIN_LEN, MAVLINK_MSG_ID_OBSTACLE_DISTANCE_LEN, MAVLINK_MSG_ID_OBSTACLE_DISTANCE_CRC); +#endif +} +#endif + +#endif + +// MESSAGE OBSTACLE_DISTANCE UNPACKING + + +/** + * @brief Get field time_usec from obstacle_distance message + * + * @return [us] Timestamp (UNIX Epoch time or time since system boot). The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude of the number. + */ +static inline uint64_t mavlink_msg_obstacle_distance_get_time_usec(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint64_t(msg, 0); +} + +/** + * @brief Get field sensor_type from obstacle_distance message + * + * @return Class id of the distance sensor type. + */ +static inline uint8_t mavlink_msg_obstacle_distance_get_sensor_type(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 156); +} + +/** + * @brief Get field distances from obstacle_distance message + * + * @return [cm] Distance of obstacles around the vehicle with index 0 corresponding to north + angle_offset, unless otherwise specified in the frame. A value of 0 is valid and means that the obstacle is practically touching the sensor. A value of max_distance +1 means no obstacle is present. A value of UINT16_MAX for unknown/not used. In a array element, one unit corresponds to 1cm. + */ +static inline uint16_t mavlink_msg_obstacle_distance_get_distances(const mavlink_message_t* msg, uint16_t *distances) +{ + return _MAV_RETURN_uint16_t_array(msg, distances, 72, 8); +} + +/** + * @brief Get field increment from obstacle_distance message + * + * @return [deg] Angular width in degrees of each array element. Increment direction is clockwise. This field is ignored if increment_f is non-zero. + */ +static inline uint8_t mavlink_msg_obstacle_distance_get_increment(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 157); +} + +/** + * @brief Get field min_distance from obstacle_distance message + * + * @return [cm] Minimum distance the sensor can measure. + */ +static inline uint16_t mavlink_msg_obstacle_distance_get_min_distance(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 152); +} + +/** + * @brief Get field max_distance from obstacle_distance message + * + * @return [cm] Maximum distance the sensor can measure. + */ +static inline uint16_t mavlink_msg_obstacle_distance_get_max_distance(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 154); +} + +/** + * @brief Get field increment_f from obstacle_distance message + * + * @return [deg] Angular width in degrees of each array element as a float. If non-zero then this value is used instead of the uint8_t increment field. Positive is clockwise direction, negative is counter-clockwise. + */ +static inline float mavlink_msg_obstacle_distance_get_increment_f(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 158); +} + +/** + * @brief Get field angle_offset from obstacle_distance message + * + * @return [deg] Relative angle offset of the 0-index element in the distances array. Value of 0 corresponds to forward. Positive is clockwise direction, negative is counter-clockwise. + */ +static inline float mavlink_msg_obstacle_distance_get_angle_offset(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 162); +} + +/** + * @brief Get field frame from obstacle_distance message + * + * @return Coordinate frame of reference for the yaw rotation and offset of the sensor data. Defaults to MAV_FRAME_GLOBAL, which is north aligned. For body-mounted sensors use MAV_FRAME_BODY_FRD, which is vehicle front aligned. + */ +static inline uint8_t mavlink_msg_obstacle_distance_get_frame(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 166); +} + +/** + * @brief Decode a obstacle_distance message into a struct + * + * @param msg The message to decode + * @param obstacle_distance C-struct to decode the message contents into + */ +static inline void mavlink_msg_obstacle_distance_decode(const mavlink_message_t* msg, mavlink_obstacle_distance_t* obstacle_distance) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + obstacle_distance->time_usec = mavlink_msg_obstacle_distance_get_time_usec(msg); + mavlink_msg_obstacle_distance_get_distances(msg, obstacle_distance->distances); + obstacle_distance->min_distance = mavlink_msg_obstacle_distance_get_min_distance(msg); + obstacle_distance->max_distance = mavlink_msg_obstacle_distance_get_max_distance(msg); + obstacle_distance->sensor_type = mavlink_msg_obstacle_distance_get_sensor_type(msg); + obstacle_distance->increment = mavlink_msg_obstacle_distance_get_increment(msg); + obstacle_distance->increment_f = mavlink_msg_obstacle_distance_get_increment_f(msg); + obstacle_distance->angle_offset = mavlink_msg_obstacle_distance_get_angle_offset(msg); + obstacle_distance->frame = mavlink_msg_obstacle_distance_get_frame(msg); +#else + uint8_t len = msg->len < MAVLINK_MSG_ID_OBSTACLE_DISTANCE_LEN? msg->len : MAVLINK_MSG_ID_OBSTACLE_DISTANCE_LEN; + memset(obstacle_distance, 0, MAVLINK_MSG_ID_OBSTACLE_DISTANCE_LEN); + memcpy(obstacle_distance, _MAV_PAYLOAD(msg), len); +#endif +} diff --git a/lib/main/MAVLink/common/mavlink_msg_odometry.h b/lib/main/MAVLink/common/mavlink_msg_odometry.h new file mode 100644 index 0000000000..93869b7916 --- /dev/null +++ b/lib/main/MAVLink/common/mavlink_msg_odometry.h @@ -0,0 +1,607 @@ +#pragma once +// MESSAGE ODOMETRY PACKING + +#define MAVLINK_MSG_ID_ODOMETRY 331 + + +typedef struct __mavlink_odometry_t { + uint64_t time_usec; /*< [us] Timestamp (UNIX Epoch time or time since system boot). The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude of the number.*/ + float x; /*< [m] X Position*/ + float y; /*< [m] Y Position*/ + float z; /*< [m] Z Position*/ + float q[4]; /*< Quaternion components, w, x, y, z (1 0 0 0 is the null-rotation)*/ + float vx; /*< [m/s] X linear speed*/ + float vy; /*< [m/s] Y linear speed*/ + float vz; /*< [m/s] Z linear speed*/ + float rollspeed; /*< [rad/s] Roll angular speed*/ + float pitchspeed; /*< [rad/s] Pitch angular speed*/ + float yawspeed; /*< [rad/s] Yaw angular speed*/ + float pose_covariance[21]; /*< Row-major representation of a 6x6 pose cross-covariance matrix upper right triangle (states: x, y, z, roll, pitch, yaw; first six entries are the first ROW, next five entries are the second ROW, etc.). If unknown, assign NaN value to first element in the array.*/ + float velocity_covariance[21]; /*< Row-major representation of a 6x6 velocity cross-covariance matrix upper right triangle (states: vx, vy, vz, rollspeed, pitchspeed, yawspeed; first six entries are the first ROW, next five entries are the second ROW, etc.). If unknown, assign NaN value to first element in the array.*/ + uint8_t frame_id; /*< Coordinate frame of reference for the pose data.*/ + uint8_t child_frame_id; /*< Coordinate frame of reference for the velocity in free space (twist) data.*/ + uint8_t reset_counter; /*< Estimate reset counter. This should be incremented when the estimate resets in any of the dimensions (position, velocity, attitude, angular speed). This is designed to be used when e.g an external SLAM system detects a loop-closure and the estimate jumps.*/ + uint8_t estimator_type; /*< Type of estimator that is providing the odometry.*/ +} mavlink_odometry_t; + +#define MAVLINK_MSG_ID_ODOMETRY_LEN 232 +#define MAVLINK_MSG_ID_ODOMETRY_MIN_LEN 230 +#define MAVLINK_MSG_ID_331_LEN 232 +#define MAVLINK_MSG_ID_331_MIN_LEN 230 + +#define MAVLINK_MSG_ID_ODOMETRY_CRC 91 +#define MAVLINK_MSG_ID_331_CRC 91 + +#define MAVLINK_MSG_ODOMETRY_FIELD_Q_LEN 4 +#define MAVLINK_MSG_ODOMETRY_FIELD_POSE_COVARIANCE_LEN 21 +#define MAVLINK_MSG_ODOMETRY_FIELD_VELOCITY_COVARIANCE_LEN 21 + +#if MAVLINK_COMMAND_24BIT +#define MAVLINK_MESSAGE_INFO_ODOMETRY { \ + 331, \ + "ODOMETRY", \ + 17, \ + { { "time_usec", NULL, MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_odometry_t, time_usec) }, \ + { "frame_id", NULL, MAVLINK_TYPE_UINT8_T, 0, 228, offsetof(mavlink_odometry_t, frame_id) }, \ + { "child_frame_id", NULL, MAVLINK_TYPE_UINT8_T, 0, 229, offsetof(mavlink_odometry_t, child_frame_id) }, \ + { "x", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_odometry_t, x) }, \ + { "y", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_odometry_t, y) }, \ + { "z", NULL, MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_odometry_t, z) }, \ + { "q", NULL, MAVLINK_TYPE_FLOAT, 4, 20, offsetof(mavlink_odometry_t, q) }, \ + { "vx", NULL, MAVLINK_TYPE_FLOAT, 0, 36, offsetof(mavlink_odometry_t, vx) }, \ + { "vy", NULL, MAVLINK_TYPE_FLOAT, 0, 40, offsetof(mavlink_odometry_t, vy) }, \ + { "vz", NULL, MAVLINK_TYPE_FLOAT, 0, 44, offsetof(mavlink_odometry_t, vz) }, \ + { "rollspeed", NULL, MAVLINK_TYPE_FLOAT, 0, 48, offsetof(mavlink_odometry_t, rollspeed) }, \ + { "pitchspeed", NULL, MAVLINK_TYPE_FLOAT, 0, 52, offsetof(mavlink_odometry_t, pitchspeed) }, \ + { "yawspeed", NULL, MAVLINK_TYPE_FLOAT, 0, 56, offsetof(mavlink_odometry_t, yawspeed) }, \ + { "pose_covariance", NULL, MAVLINK_TYPE_FLOAT, 21, 60, offsetof(mavlink_odometry_t, pose_covariance) }, \ + { "velocity_covariance", NULL, MAVLINK_TYPE_FLOAT, 21, 144, offsetof(mavlink_odometry_t, velocity_covariance) }, \ + { "reset_counter", NULL, MAVLINK_TYPE_UINT8_T, 0, 230, offsetof(mavlink_odometry_t, reset_counter) }, \ + { "estimator_type", NULL, MAVLINK_TYPE_UINT8_T, 0, 231, offsetof(mavlink_odometry_t, estimator_type) }, \ + } \ +} +#else +#define MAVLINK_MESSAGE_INFO_ODOMETRY { \ + "ODOMETRY", \ + 17, \ + { { "time_usec", NULL, MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_odometry_t, time_usec) }, \ + { "frame_id", NULL, MAVLINK_TYPE_UINT8_T, 0, 228, offsetof(mavlink_odometry_t, frame_id) }, \ + { "child_frame_id", NULL, MAVLINK_TYPE_UINT8_T, 0, 229, offsetof(mavlink_odometry_t, child_frame_id) }, \ + { "x", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_odometry_t, x) }, \ + { "y", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_odometry_t, y) }, \ + { "z", NULL, MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_odometry_t, z) }, \ + { "q", NULL, MAVLINK_TYPE_FLOAT, 4, 20, offsetof(mavlink_odometry_t, q) }, \ + { "vx", NULL, MAVLINK_TYPE_FLOAT, 0, 36, offsetof(mavlink_odometry_t, vx) }, \ + { "vy", NULL, MAVLINK_TYPE_FLOAT, 0, 40, offsetof(mavlink_odometry_t, vy) }, \ + { "vz", NULL, MAVLINK_TYPE_FLOAT, 0, 44, offsetof(mavlink_odometry_t, vz) }, \ + { "rollspeed", NULL, MAVLINK_TYPE_FLOAT, 0, 48, offsetof(mavlink_odometry_t, rollspeed) }, \ + { "pitchspeed", NULL, MAVLINK_TYPE_FLOAT, 0, 52, offsetof(mavlink_odometry_t, pitchspeed) }, \ + { "yawspeed", NULL, MAVLINK_TYPE_FLOAT, 0, 56, offsetof(mavlink_odometry_t, yawspeed) }, \ + { "pose_covariance", NULL, MAVLINK_TYPE_FLOAT, 21, 60, offsetof(mavlink_odometry_t, pose_covariance) }, \ + { "velocity_covariance", NULL, MAVLINK_TYPE_FLOAT, 21, 144, offsetof(mavlink_odometry_t, velocity_covariance) }, \ + { "reset_counter", NULL, MAVLINK_TYPE_UINT8_T, 0, 230, offsetof(mavlink_odometry_t, reset_counter) }, \ + { "estimator_type", NULL, MAVLINK_TYPE_UINT8_T, 0, 231, offsetof(mavlink_odometry_t, estimator_type) }, \ + } \ +} +#endif + +/** + * @brief Pack a odometry message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param time_usec [us] Timestamp (UNIX Epoch time or time since system boot). The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude of the number. + * @param frame_id Coordinate frame of reference for the pose data. + * @param child_frame_id Coordinate frame of reference for the velocity in free space (twist) data. + * @param x [m] X Position + * @param y [m] Y Position + * @param z [m] Z Position + * @param q Quaternion components, w, x, y, z (1 0 0 0 is the null-rotation) + * @param vx [m/s] X linear speed + * @param vy [m/s] Y linear speed + * @param vz [m/s] Z linear speed + * @param rollspeed [rad/s] Roll angular speed + * @param pitchspeed [rad/s] Pitch angular speed + * @param yawspeed [rad/s] Yaw angular speed + * @param pose_covariance Row-major representation of a 6x6 pose cross-covariance matrix upper right triangle (states: x, y, z, roll, pitch, yaw; first six entries are the first ROW, next five entries are the second ROW, etc.). If unknown, assign NaN value to first element in the array. + * @param velocity_covariance Row-major representation of a 6x6 velocity cross-covariance matrix upper right triangle (states: vx, vy, vz, rollspeed, pitchspeed, yawspeed; first six entries are the first ROW, next five entries are the second ROW, etc.). If unknown, assign NaN value to first element in the array. + * @param reset_counter Estimate reset counter. This should be incremented when the estimate resets in any of the dimensions (position, velocity, attitude, angular speed). This is designed to be used when e.g an external SLAM system detects a loop-closure and the estimate jumps. + * @param estimator_type Type of estimator that is providing the odometry. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_odometry_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint64_t time_usec, uint8_t frame_id, uint8_t child_frame_id, float x, float y, float z, const float *q, float vx, float vy, float vz, float rollspeed, float pitchspeed, float yawspeed, const float *pose_covariance, const float *velocity_covariance, uint8_t reset_counter, uint8_t estimator_type) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_ODOMETRY_LEN]; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_float(buf, 8, x); + _mav_put_float(buf, 12, y); + _mav_put_float(buf, 16, z); + _mav_put_float(buf, 36, vx); + _mav_put_float(buf, 40, vy); + _mav_put_float(buf, 44, vz); + _mav_put_float(buf, 48, rollspeed); + _mav_put_float(buf, 52, pitchspeed); + _mav_put_float(buf, 56, yawspeed); + _mav_put_uint8_t(buf, 228, frame_id); + _mav_put_uint8_t(buf, 229, child_frame_id); + _mav_put_uint8_t(buf, 230, reset_counter); + _mav_put_uint8_t(buf, 231, estimator_type); + _mav_put_float_array(buf, 20, q, 4); + _mav_put_float_array(buf, 60, pose_covariance, 21); + _mav_put_float_array(buf, 144, velocity_covariance, 21); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_ODOMETRY_LEN); +#else + mavlink_odometry_t packet; + packet.time_usec = time_usec; + packet.x = x; + packet.y = y; + packet.z = z; + packet.vx = vx; + packet.vy = vy; + packet.vz = vz; + packet.rollspeed = rollspeed; + packet.pitchspeed = pitchspeed; + packet.yawspeed = yawspeed; + packet.frame_id = frame_id; + packet.child_frame_id = child_frame_id; + packet.reset_counter = reset_counter; + packet.estimator_type = estimator_type; + mav_array_memcpy(packet.q, q, sizeof(float)*4); + mav_array_memcpy(packet.pose_covariance, pose_covariance, sizeof(float)*21); + mav_array_memcpy(packet.velocity_covariance, velocity_covariance, sizeof(float)*21); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_ODOMETRY_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_ODOMETRY; + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_ODOMETRY_MIN_LEN, MAVLINK_MSG_ID_ODOMETRY_LEN, MAVLINK_MSG_ID_ODOMETRY_CRC); +} + +/** + * @brief Pack a odometry message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param time_usec [us] Timestamp (UNIX Epoch time or time since system boot). The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude of the number. + * @param frame_id Coordinate frame of reference for the pose data. + * @param child_frame_id Coordinate frame of reference for the velocity in free space (twist) data. + * @param x [m] X Position + * @param y [m] Y Position + * @param z [m] Z Position + * @param q Quaternion components, w, x, y, z (1 0 0 0 is the null-rotation) + * @param vx [m/s] X linear speed + * @param vy [m/s] Y linear speed + * @param vz [m/s] Z linear speed + * @param rollspeed [rad/s] Roll angular speed + * @param pitchspeed [rad/s] Pitch angular speed + * @param yawspeed [rad/s] Yaw angular speed + * @param pose_covariance Row-major representation of a 6x6 pose cross-covariance matrix upper right triangle (states: x, y, z, roll, pitch, yaw; first six entries are the first ROW, next five entries are the second ROW, etc.). If unknown, assign NaN value to first element in the array. + * @param velocity_covariance Row-major representation of a 6x6 velocity cross-covariance matrix upper right triangle (states: vx, vy, vz, rollspeed, pitchspeed, yawspeed; first six entries are the first ROW, next five entries are the second ROW, etc.). If unknown, assign NaN value to first element in the array. + * @param reset_counter Estimate reset counter. This should be incremented when the estimate resets in any of the dimensions (position, velocity, attitude, angular speed). This is designed to be used when e.g an external SLAM system detects a loop-closure and the estimate jumps. + * @param estimator_type Type of estimator that is providing the odometry. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_odometry_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint64_t time_usec,uint8_t frame_id,uint8_t child_frame_id,float x,float y,float z,const float *q,float vx,float vy,float vz,float rollspeed,float pitchspeed,float yawspeed,const float *pose_covariance,const float *velocity_covariance,uint8_t reset_counter,uint8_t estimator_type) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_ODOMETRY_LEN]; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_float(buf, 8, x); + _mav_put_float(buf, 12, y); + _mav_put_float(buf, 16, z); + _mav_put_float(buf, 36, vx); + _mav_put_float(buf, 40, vy); + _mav_put_float(buf, 44, vz); + _mav_put_float(buf, 48, rollspeed); + _mav_put_float(buf, 52, pitchspeed); + _mav_put_float(buf, 56, yawspeed); + _mav_put_uint8_t(buf, 228, frame_id); + _mav_put_uint8_t(buf, 229, child_frame_id); + _mav_put_uint8_t(buf, 230, reset_counter); + _mav_put_uint8_t(buf, 231, estimator_type); + _mav_put_float_array(buf, 20, q, 4); + _mav_put_float_array(buf, 60, pose_covariance, 21); + _mav_put_float_array(buf, 144, velocity_covariance, 21); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_ODOMETRY_LEN); +#else + mavlink_odometry_t packet; + packet.time_usec = time_usec; + packet.x = x; + packet.y = y; + packet.z = z; + packet.vx = vx; + packet.vy = vy; + packet.vz = vz; + packet.rollspeed = rollspeed; + packet.pitchspeed = pitchspeed; + packet.yawspeed = yawspeed; + packet.frame_id = frame_id; + packet.child_frame_id = child_frame_id; + packet.reset_counter = reset_counter; + packet.estimator_type = estimator_type; + mav_array_memcpy(packet.q, q, sizeof(float)*4); + mav_array_memcpy(packet.pose_covariance, pose_covariance, sizeof(float)*21); + mav_array_memcpy(packet.velocity_covariance, velocity_covariance, sizeof(float)*21); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_ODOMETRY_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_ODOMETRY; + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_ODOMETRY_MIN_LEN, MAVLINK_MSG_ID_ODOMETRY_LEN, MAVLINK_MSG_ID_ODOMETRY_CRC); +} + +/** + * @brief Encode a odometry struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param odometry C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_odometry_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_odometry_t* odometry) +{ + return mavlink_msg_odometry_pack(system_id, component_id, msg, odometry->time_usec, odometry->frame_id, odometry->child_frame_id, odometry->x, odometry->y, odometry->z, odometry->q, odometry->vx, odometry->vy, odometry->vz, odometry->rollspeed, odometry->pitchspeed, odometry->yawspeed, odometry->pose_covariance, odometry->velocity_covariance, odometry->reset_counter, odometry->estimator_type); +} + +/** + * @brief Encode a odometry struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param odometry C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_odometry_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_odometry_t* odometry) +{ + return mavlink_msg_odometry_pack_chan(system_id, component_id, chan, msg, odometry->time_usec, odometry->frame_id, odometry->child_frame_id, odometry->x, odometry->y, odometry->z, odometry->q, odometry->vx, odometry->vy, odometry->vz, odometry->rollspeed, odometry->pitchspeed, odometry->yawspeed, odometry->pose_covariance, odometry->velocity_covariance, odometry->reset_counter, odometry->estimator_type); +} + +/** + * @brief Send a odometry message + * @param chan MAVLink channel to send the message + * + * @param time_usec [us] Timestamp (UNIX Epoch time or time since system boot). The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude of the number. + * @param frame_id Coordinate frame of reference for the pose data. + * @param child_frame_id Coordinate frame of reference for the velocity in free space (twist) data. + * @param x [m] X Position + * @param y [m] Y Position + * @param z [m] Z Position + * @param q Quaternion components, w, x, y, z (1 0 0 0 is the null-rotation) + * @param vx [m/s] X linear speed + * @param vy [m/s] Y linear speed + * @param vz [m/s] Z linear speed + * @param rollspeed [rad/s] Roll angular speed + * @param pitchspeed [rad/s] Pitch angular speed + * @param yawspeed [rad/s] Yaw angular speed + * @param pose_covariance Row-major representation of a 6x6 pose cross-covariance matrix upper right triangle (states: x, y, z, roll, pitch, yaw; first six entries are the first ROW, next five entries are the second ROW, etc.). If unknown, assign NaN value to first element in the array. + * @param velocity_covariance Row-major representation of a 6x6 velocity cross-covariance matrix upper right triangle (states: vx, vy, vz, rollspeed, pitchspeed, yawspeed; first six entries are the first ROW, next five entries are the second ROW, etc.). If unknown, assign NaN value to first element in the array. + * @param reset_counter Estimate reset counter. This should be incremented when the estimate resets in any of the dimensions (position, velocity, attitude, angular speed). This is designed to be used when e.g an external SLAM system detects a loop-closure and the estimate jumps. + * @param estimator_type Type of estimator that is providing the odometry. + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_odometry_send(mavlink_channel_t chan, uint64_t time_usec, uint8_t frame_id, uint8_t child_frame_id, float x, float y, float z, const float *q, float vx, float vy, float vz, float rollspeed, float pitchspeed, float yawspeed, const float *pose_covariance, const float *velocity_covariance, uint8_t reset_counter, uint8_t estimator_type) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_ODOMETRY_LEN]; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_float(buf, 8, x); + _mav_put_float(buf, 12, y); + _mav_put_float(buf, 16, z); + _mav_put_float(buf, 36, vx); + _mav_put_float(buf, 40, vy); + _mav_put_float(buf, 44, vz); + _mav_put_float(buf, 48, rollspeed); + _mav_put_float(buf, 52, pitchspeed); + _mav_put_float(buf, 56, yawspeed); + _mav_put_uint8_t(buf, 228, frame_id); + _mav_put_uint8_t(buf, 229, child_frame_id); + _mav_put_uint8_t(buf, 230, reset_counter); + _mav_put_uint8_t(buf, 231, estimator_type); + _mav_put_float_array(buf, 20, q, 4); + _mav_put_float_array(buf, 60, pose_covariance, 21); + _mav_put_float_array(buf, 144, velocity_covariance, 21); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ODOMETRY, buf, MAVLINK_MSG_ID_ODOMETRY_MIN_LEN, MAVLINK_MSG_ID_ODOMETRY_LEN, MAVLINK_MSG_ID_ODOMETRY_CRC); +#else + mavlink_odometry_t packet; + packet.time_usec = time_usec; + packet.x = x; + packet.y = y; + packet.z = z; + packet.vx = vx; + packet.vy = vy; + packet.vz = vz; + packet.rollspeed = rollspeed; + packet.pitchspeed = pitchspeed; + packet.yawspeed = yawspeed; + packet.frame_id = frame_id; + packet.child_frame_id = child_frame_id; + packet.reset_counter = reset_counter; + packet.estimator_type = estimator_type; + mav_array_memcpy(packet.q, q, sizeof(float)*4); + mav_array_memcpy(packet.pose_covariance, pose_covariance, sizeof(float)*21); + mav_array_memcpy(packet.velocity_covariance, velocity_covariance, sizeof(float)*21); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ODOMETRY, (const char *)&packet, MAVLINK_MSG_ID_ODOMETRY_MIN_LEN, MAVLINK_MSG_ID_ODOMETRY_LEN, MAVLINK_MSG_ID_ODOMETRY_CRC); +#endif +} + +/** + * @brief Send a odometry message + * @param chan MAVLink channel to send the message + * @param struct The MAVLink struct to serialize + */ +static inline void mavlink_msg_odometry_send_struct(mavlink_channel_t chan, const mavlink_odometry_t* odometry) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + mavlink_msg_odometry_send(chan, odometry->time_usec, odometry->frame_id, odometry->child_frame_id, odometry->x, odometry->y, odometry->z, odometry->q, odometry->vx, odometry->vy, odometry->vz, odometry->rollspeed, odometry->pitchspeed, odometry->yawspeed, odometry->pose_covariance, odometry->velocity_covariance, odometry->reset_counter, odometry->estimator_type); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ODOMETRY, (const char *)odometry, MAVLINK_MSG_ID_ODOMETRY_MIN_LEN, MAVLINK_MSG_ID_ODOMETRY_LEN, MAVLINK_MSG_ID_ODOMETRY_CRC); +#endif +} + +#if MAVLINK_MSG_ID_ODOMETRY_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_odometry_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint64_t time_usec, uint8_t frame_id, uint8_t child_frame_id, float x, float y, float z, const float *q, float vx, float vy, float vz, float rollspeed, float pitchspeed, float yawspeed, const float *pose_covariance, const float *velocity_covariance, uint8_t reset_counter, uint8_t estimator_type) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_float(buf, 8, x); + _mav_put_float(buf, 12, y); + _mav_put_float(buf, 16, z); + _mav_put_float(buf, 36, vx); + _mav_put_float(buf, 40, vy); + _mav_put_float(buf, 44, vz); + _mav_put_float(buf, 48, rollspeed); + _mav_put_float(buf, 52, pitchspeed); + _mav_put_float(buf, 56, yawspeed); + _mav_put_uint8_t(buf, 228, frame_id); + _mav_put_uint8_t(buf, 229, child_frame_id); + _mav_put_uint8_t(buf, 230, reset_counter); + _mav_put_uint8_t(buf, 231, estimator_type); + _mav_put_float_array(buf, 20, q, 4); + _mav_put_float_array(buf, 60, pose_covariance, 21); + _mav_put_float_array(buf, 144, velocity_covariance, 21); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ODOMETRY, buf, MAVLINK_MSG_ID_ODOMETRY_MIN_LEN, MAVLINK_MSG_ID_ODOMETRY_LEN, MAVLINK_MSG_ID_ODOMETRY_CRC); +#else + mavlink_odometry_t *packet = (mavlink_odometry_t *)msgbuf; + packet->time_usec = time_usec; + packet->x = x; + packet->y = y; + packet->z = z; + packet->vx = vx; + packet->vy = vy; + packet->vz = vz; + packet->rollspeed = rollspeed; + packet->pitchspeed = pitchspeed; + packet->yawspeed = yawspeed; + packet->frame_id = frame_id; + packet->child_frame_id = child_frame_id; + packet->reset_counter = reset_counter; + packet->estimator_type = estimator_type; + mav_array_memcpy(packet->q, q, sizeof(float)*4); + mav_array_memcpy(packet->pose_covariance, pose_covariance, sizeof(float)*21); + mav_array_memcpy(packet->velocity_covariance, velocity_covariance, sizeof(float)*21); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ODOMETRY, (const char *)packet, MAVLINK_MSG_ID_ODOMETRY_MIN_LEN, MAVLINK_MSG_ID_ODOMETRY_LEN, MAVLINK_MSG_ID_ODOMETRY_CRC); +#endif +} +#endif + +#endif + +// MESSAGE ODOMETRY UNPACKING + + +/** + * @brief Get field time_usec from odometry message + * + * @return [us] Timestamp (UNIX Epoch time or time since system boot). The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude of the number. + */ +static inline uint64_t mavlink_msg_odometry_get_time_usec(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint64_t(msg, 0); +} + +/** + * @brief Get field frame_id from odometry message + * + * @return Coordinate frame of reference for the pose data. + */ +static inline uint8_t mavlink_msg_odometry_get_frame_id(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 228); +} + +/** + * @brief Get field child_frame_id from odometry message + * + * @return Coordinate frame of reference for the velocity in free space (twist) data. + */ +static inline uint8_t mavlink_msg_odometry_get_child_frame_id(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 229); +} + +/** + * @brief Get field x from odometry message + * + * @return [m] X Position + */ +static inline float mavlink_msg_odometry_get_x(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 8); +} + +/** + * @brief Get field y from odometry message + * + * @return [m] Y Position + */ +static inline float mavlink_msg_odometry_get_y(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 12); +} + +/** + * @brief Get field z from odometry message + * + * @return [m] Z Position + */ +static inline float mavlink_msg_odometry_get_z(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 16); +} + +/** + * @brief Get field q from odometry message + * + * @return Quaternion components, w, x, y, z (1 0 0 0 is the null-rotation) + */ +static inline uint16_t mavlink_msg_odometry_get_q(const mavlink_message_t* msg, float *q) +{ + return _MAV_RETURN_float_array(msg, q, 4, 20); +} + +/** + * @brief Get field vx from odometry message + * + * @return [m/s] X linear speed + */ +static inline float mavlink_msg_odometry_get_vx(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 36); +} + +/** + * @brief Get field vy from odometry message + * + * @return [m/s] Y linear speed + */ +static inline float mavlink_msg_odometry_get_vy(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 40); +} + +/** + * @brief Get field vz from odometry message + * + * @return [m/s] Z linear speed + */ +static inline float mavlink_msg_odometry_get_vz(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 44); +} + +/** + * @brief Get field rollspeed from odometry message + * + * @return [rad/s] Roll angular speed + */ +static inline float mavlink_msg_odometry_get_rollspeed(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 48); +} + +/** + * @brief Get field pitchspeed from odometry message + * + * @return [rad/s] Pitch angular speed + */ +static inline float mavlink_msg_odometry_get_pitchspeed(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 52); +} + +/** + * @brief Get field yawspeed from odometry message + * + * @return [rad/s] Yaw angular speed + */ +static inline float mavlink_msg_odometry_get_yawspeed(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 56); +} + +/** + * @brief Get field pose_covariance from odometry message + * + * @return Row-major representation of a 6x6 pose cross-covariance matrix upper right triangle (states: x, y, z, roll, pitch, yaw; first six entries are the first ROW, next five entries are the second ROW, etc.). If unknown, assign NaN value to first element in the array. + */ +static inline uint16_t mavlink_msg_odometry_get_pose_covariance(const mavlink_message_t* msg, float *pose_covariance) +{ + return _MAV_RETURN_float_array(msg, pose_covariance, 21, 60); +} + +/** + * @brief Get field velocity_covariance from odometry message + * + * @return Row-major representation of a 6x6 velocity cross-covariance matrix upper right triangle (states: vx, vy, vz, rollspeed, pitchspeed, yawspeed; first six entries are the first ROW, next five entries are the second ROW, etc.). If unknown, assign NaN value to first element in the array. + */ +static inline uint16_t mavlink_msg_odometry_get_velocity_covariance(const mavlink_message_t* msg, float *velocity_covariance) +{ + return _MAV_RETURN_float_array(msg, velocity_covariance, 21, 144); +} + +/** + * @brief Get field reset_counter from odometry message + * + * @return Estimate reset counter. This should be incremented when the estimate resets in any of the dimensions (position, velocity, attitude, angular speed). This is designed to be used when e.g an external SLAM system detects a loop-closure and the estimate jumps. + */ +static inline uint8_t mavlink_msg_odometry_get_reset_counter(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 230); +} + +/** + * @brief Get field estimator_type from odometry message + * + * @return Type of estimator that is providing the odometry. + */ +static inline uint8_t mavlink_msg_odometry_get_estimator_type(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 231); +} + +/** + * @brief Decode a odometry message into a struct + * + * @param msg The message to decode + * @param odometry C-struct to decode the message contents into + */ +static inline void mavlink_msg_odometry_decode(const mavlink_message_t* msg, mavlink_odometry_t* odometry) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + odometry->time_usec = mavlink_msg_odometry_get_time_usec(msg); + odometry->x = mavlink_msg_odometry_get_x(msg); + odometry->y = mavlink_msg_odometry_get_y(msg); + odometry->z = mavlink_msg_odometry_get_z(msg); + mavlink_msg_odometry_get_q(msg, odometry->q); + odometry->vx = mavlink_msg_odometry_get_vx(msg); + odometry->vy = mavlink_msg_odometry_get_vy(msg); + odometry->vz = mavlink_msg_odometry_get_vz(msg); + odometry->rollspeed = mavlink_msg_odometry_get_rollspeed(msg); + odometry->pitchspeed = mavlink_msg_odometry_get_pitchspeed(msg); + odometry->yawspeed = mavlink_msg_odometry_get_yawspeed(msg); + mavlink_msg_odometry_get_pose_covariance(msg, odometry->pose_covariance); + mavlink_msg_odometry_get_velocity_covariance(msg, odometry->velocity_covariance); + odometry->frame_id = mavlink_msg_odometry_get_frame_id(msg); + odometry->child_frame_id = mavlink_msg_odometry_get_child_frame_id(msg); + odometry->reset_counter = mavlink_msg_odometry_get_reset_counter(msg); + odometry->estimator_type = mavlink_msg_odometry_get_estimator_type(msg); +#else + uint8_t len = msg->len < MAVLINK_MSG_ID_ODOMETRY_LEN? msg->len : MAVLINK_MSG_ID_ODOMETRY_LEN; + memset(odometry, 0, MAVLINK_MSG_ID_ODOMETRY_LEN); + memcpy(odometry, _MAV_PAYLOAD(msg), len); +#endif +} diff --git a/lib/main/MAVLink/common/mavlink_msg_onboard_computer_status.h b/lib/main/MAVLink/common/mavlink_msg_onboard_computer_status.h new file mode 100644 index 0000000000..6da2581423 --- /dev/null +++ b/lib/main/MAVLink/common/mavlink_msg_onboard_computer_status.h @@ -0,0 +1,693 @@ +#pragma once +// MESSAGE ONBOARD_COMPUTER_STATUS PACKING + +#define MAVLINK_MSG_ID_ONBOARD_COMPUTER_STATUS 390 + + +typedef struct __mavlink_onboard_computer_status_t { + uint64_t time_usec; /*< [us] Timestamp (UNIX Epoch time or time since system boot). The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude of the number.*/ + uint32_t uptime; /*< [ms] Time since system boot.*/ + uint32_t ram_usage; /*< [MiB] Amount of used RAM on the component system. A value of UINT32_MAX implies the field is unused.*/ + uint32_t ram_total; /*< [MiB] Total amount of RAM on the component system. A value of UINT32_MAX implies the field is unused.*/ + uint32_t storage_type[4]; /*< Storage type: 0: HDD, 1: SSD, 2: EMMC, 3: SD card (non-removable), 4: SD card (removable). A value of UINT32_MAX implies the field is unused.*/ + uint32_t storage_usage[4]; /*< [MiB] Amount of used storage space on the component system. A value of UINT32_MAX implies the field is unused.*/ + uint32_t storage_total[4]; /*< [MiB] Total amount of storage space on the component system. A value of UINT32_MAX implies the field is unused.*/ + uint32_t link_type[6]; /*< Link type: 0-9: UART, 10-19: Wired network, 20-29: Wifi, 30-39: Point-to-point proprietary, 40-49: Mesh proprietary*/ + uint32_t link_tx_rate[6]; /*< [KiB/s] Network traffic from the component system. A value of UINT32_MAX implies the field is unused.*/ + uint32_t link_rx_rate[6]; /*< [KiB/s] Network traffic to the component system. A value of UINT32_MAX implies the field is unused.*/ + uint32_t link_tx_max[6]; /*< [KiB/s] Network capacity from the component system. A value of UINT32_MAX implies the field is unused.*/ + uint32_t link_rx_max[6]; /*< [KiB/s] Network capacity to the component system. A value of UINT32_MAX implies the field is unused.*/ + int16_t fan_speed[4]; /*< [rpm] Fan speeds. A value of INT16_MAX implies the field is unused.*/ + uint8_t type; /*< Type of the onboard computer: 0: Mission computer primary, 1: Mission computer backup 1, 2: Mission computer backup 2, 3: Compute node, 4-5: Compute spares, 6-9: Payload computers.*/ + uint8_t cpu_cores[8]; /*< CPU usage on the component in percent (100 - idle). A value of UINT8_MAX implies the field is unused.*/ + uint8_t cpu_combined[10]; /*< Combined CPU usage as the last 10 slices of 100 MS (a histogram). This allows to identify spikes in load that max out the system, but only for a short amount of time. A value of UINT8_MAX implies the field is unused.*/ + uint8_t gpu_cores[4]; /*< GPU usage on the component in percent (100 - idle). A value of UINT8_MAX implies the field is unused.*/ + uint8_t gpu_combined[10]; /*< Combined GPU usage as the last 10 slices of 100 MS (a histogram). This allows to identify spikes in load that max out the system, but only for a short amount of time. A value of UINT8_MAX implies the field is unused.*/ + int8_t temperature_board; /*< [degC] Temperature of the board. A value of INT8_MAX implies the field is unused.*/ + int8_t temperature_core[8]; /*< [degC] Temperature of the CPU core. A value of INT8_MAX implies the field is unused.*/ +} mavlink_onboard_computer_status_t; + +#define MAVLINK_MSG_ID_ONBOARD_COMPUTER_STATUS_LEN 238 +#define MAVLINK_MSG_ID_ONBOARD_COMPUTER_STATUS_MIN_LEN 238 +#define MAVLINK_MSG_ID_390_LEN 238 +#define MAVLINK_MSG_ID_390_MIN_LEN 238 + +#define MAVLINK_MSG_ID_ONBOARD_COMPUTER_STATUS_CRC 156 +#define MAVLINK_MSG_ID_390_CRC 156 + +#define MAVLINK_MSG_ONBOARD_COMPUTER_STATUS_FIELD_STORAGE_TYPE_LEN 4 +#define MAVLINK_MSG_ONBOARD_COMPUTER_STATUS_FIELD_STORAGE_USAGE_LEN 4 +#define MAVLINK_MSG_ONBOARD_COMPUTER_STATUS_FIELD_STORAGE_TOTAL_LEN 4 +#define MAVLINK_MSG_ONBOARD_COMPUTER_STATUS_FIELD_LINK_TYPE_LEN 6 +#define MAVLINK_MSG_ONBOARD_COMPUTER_STATUS_FIELD_LINK_TX_RATE_LEN 6 +#define MAVLINK_MSG_ONBOARD_COMPUTER_STATUS_FIELD_LINK_RX_RATE_LEN 6 +#define MAVLINK_MSG_ONBOARD_COMPUTER_STATUS_FIELD_LINK_TX_MAX_LEN 6 +#define MAVLINK_MSG_ONBOARD_COMPUTER_STATUS_FIELD_LINK_RX_MAX_LEN 6 +#define MAVLINK_MSG_ONBOARD_COMPUTER_STATUS_FIELD_FAN_SPEED_LEN 4 +#define MAVLINK_MSG_ONBOARD_COMPUTER_STATUS_FIELD_CPU_CORES_LEN 8 +#define MAVLINK_MSG_ONBOARD_COMPUTER_STATUS_FIELD_CPU_COMBINED_LEN 10 +#define MAVLINK_MSG_ONBOARD_COMPUTER_STATUS_FIELD_GPU_CORES_LEN 4 +#define MAVLINK_MSG_ONBOARD_COMPUTER_STATUS_FIELD_GPU_COMBINED_LEN 10 +#define MAVLINK_MSG_ONBOARD_COMPUTER_STATUS_FIELD_TEMPERATURE_CORE_LEN 8 + +#if MAVLINK_COMMAND_24BIT +#define MAVLINK_MESSAGE_INFO_ONBOARD_COMPUTER_STATUS { \ + 390, \ + "ONBOARD_COMPUTER_STATUS", \ + 20, \ + { { "time_usec", NULL, MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_onboard_computer_status_t, time_usec) }, \ + { "uptime", NULL, MAVLINK_TYPE_UINT32_T, 0, 8, offsetof(mavlink_onboard_computer_status_t, uptime) }, \ + { "type", NULL, MAVLINK_TYPE_UINT8_T, 0, 196, offsetof(mavlink_onboard_computer_status_t, type) }, \ + { "cpu_cores", NULL, MAVLINK_TYPE_UINT8_T, 8, 197, offsetof(mavlink_onboard_computer_status_t, cpu_cores) }, \ + { "cpu_combined", NULL, MAVLINK_TYPE_UINT8_T, 10, 205, offsetof(mavlink_onboard_computer_status_t, cpu_combined) }, \ + { "gpu_cores", NULL, MAVLINK_TYPE_UINT8_T, 4, 215, offsetof(mavlink_onboard_computer_status_t, gpu_cores) }, \ + { "gpu_combined", NULL, MAVLINK_TYPE_UINT8_T, 10, 219, offsetof(mavlink_onboard_computer_status_t, gpu_combined) }, \ + { "temperature_board", NULL, MAVLINK_TYPE_INT8_T, 0, 229, offsetof(mavlink_onboard_computer_status_t, temperature_board) }, \ + { "temperature_core", NULL, MAVLINK_TYPE_INT8_T, 8, 230, offsetof(mavlink_onboard_computer_status_t, temperature_core) }, \ + { "fan_speed", NULL, MAVLINK_TYPE_INT16_T, 4, 188, offsetof(mavlink_onboard_computer_status_t, fan_speed) }, \ + { "ram_usage", NULL, MAVLINK_TYPE_UINT32_T, 0, 12, offsetof(mavlink_onboard_computer_status_t, ram_usage) }, \ + { "ram_total", NULL, MAVLINK_TYPE_UINT32_T, 0, 16, offsetof(mavlink_onboard_computer_status_t, ram_total) }, \ + { "storage_type", NULL, MAVLINK_TYPE_UINT32_T, 4, 20, offsetof(mavlink_onboard_computer_status_t, storage_type) }, \ + { "storage_usage", NULL, MAVLINK_TYPE_UINT32_T, 4, 36, offsetof(mavlink_onboard_computer_status_t, storage_usage) }, \ + { "storage_total", NULL, MAVLINK_TYPE_UINT32_T, 4, 52, offsetof(mavlink_onboard_computer_status_t, storage_total) }, \ + { "link_type", NULL, MAVLINK_TYPE_UINT32_T, 6, 68, offsetof(mavlink_onboard_computer_status_t, link_type) }, \ + { "link_tx_rate", NULL, MAVLINK_TYPE_UINT32_T, 6, 92, offsetof(mavlink_onboard_computer_status_t, link_tx_rate) }, \ + { "link_rx_rate", NULL, MAVLINK_TYPE_UINT32_T, 6, 116, offsetof(mavlink_onboard_computer_status_t, link_rx_rate) }, \ + { "link_tx_max", NULL, MAVLINK_TYPE_UINT32_T, 6, 140, offsetof(mavlink_onboard_computer_status_t, link_tx_max) }, \ + { "link_rx_max", NULL, MAVLINK_TYPE_UINT32_T, 6, 164, offsetof(mavlink_onboard_computer_status_t, link_rx_max) }, \ + } \ +} +#else +#define MAVLINK_MESSAGE_INFO_ONBOARD_COMPUTER_STATUS { \ + "ONBOARD_COMPUTER_STATUS", \ + 20, \ + { { "time_usec", NULL, MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_onboard_computer_status_t, time_usec) }, \ + { "uptime", NULL, MAVLINK_TYPE_UINT32_T, 0, 8, offsetof(mavlink_onboard_computer_status_t, uptime) }, \ + { "type", NULL, MAVLINK_TYPE_UINT8_T, 0, 196, offsetof(mavlink_onboard_computer_status_t, type) }, \ + { "cpu_cores", NULL, MAVLINK_TYPE_UINT8_T, 8, 197, offsetof(mavlink_onboard_computer_status_t, cpu_cores) }, \ + { "cpu_combined", NULL, MAVLINK_TYPE_UINT8_T, 10, 205, offsetof(mavlink_onboard_computer_status_t, cpu_combined) }, \ + { "gpu_cores", NULL, MAVLINK_TYPE_UINT8_T, 4, 215, offsetof(mavlink_onboard_computer_status_t, gpu_cores) }, \ + { "gpu_combined", NULL, MAVLINK_TYPE_UINT8_T, 10, 219, offsetof(mavlink_onboard_computer_status_t, gpu_combined) }, \ + { "temperature_board", NULL, MAVLINK_TYPE_INT8_T, 0, 229, offsetof(mavlink_onboard_computer_status_t, temperature_board) }, \ + { "temperature_core", NULL, MAVLINK_TYPE_INT8_T, 8, 230, offsetof(mavlink_onboard_computer_status_t, temperature_core) }, \ + { "fan_speed", NULL, MAVLINK_TYPE_INT16_T, 4, 188, offsetof(mavlink_onboard_computer_status_t, fan_speed) }, \ + { "ram_usage", NULL, MAVLINK_TYPE_UINT32_T, 0, 12, offsetof(mavlink_onboard_computer_status_t, ram_usage) }, \ + { "ram_total", NULL, MAVLINK_TYPE_UINT32_T, 0, 16, offsetof(mavlink_onboard_computer_status_t, ram_total) }, \ + { "storage_type", NULL, MAVLINK_TYPE_UINT32_T, 4, 20, offsetof(mavlink_onboard_computer_status_t, storage_type) }, \ + { "storage_usage", NULL, MAVLINK_TYPE_UINT32_T, 4, 36, offsetof(mavlink_onboard_computer_status_t, storage_usage) }, \ + { "storage_total", NULL, MAVLINK_TYPE_UINT32_T, 4, 52, offsetof(mavlink_onboard_computer_status_t, storage_total) }, \ + { "link_type", NULL, MAVLINK_TYPE_UINT32_T, 6, 68, offsetof(mavlink_onboard_computer_status_t, link_type) }, \ + { "link_tx_rate", NULL, MAVLINK_TYPE_UINT32_T, 6, 92, offsetof(mavlink_onboard_computer_status_t, link_tx_rate) }, \ + { "link_rx_rate", NULL, MAVLINK_TYPE_UINT32_T, 6, 116, offsetof(mavlink_onboard_computer_status_t, link_rx_rate) }, \ + { "link_tx_max", NULL, MAVLINK_TYPE_UINT32_T, 6, 140, offsetof(mavlink_onboard_computer_status_t, link_tx_max) }, \ + { "link_rx_max", NULL, MAVLINK_TYPE_UINT32_T, 6, 164, offsetof(mavlink_onboard_computer_status_t, link_rx_max) }, \ + } \ +} +#endif + +/** + * @brief Pack a onboard_computer_status message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param time_usec [us] Timestamp (UNIX Epoch time or time since system boot). The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude of the number. + * @param uptime [ms] Time since system boot. + * @param type Type of the onboard computer: 0: Mission computer primary, 1: Mission computer backup 1, 2: Mission computer backup 2, 3: Compute node, 4-5: Compute spares, 6-9: Payload computers. + * @param cpu_cores CPU usage on the component in percent (100 - idle). A value of UINT8_MAX implies the field is unused. + * @param cpu_combined Combined CPU usage as the last 10 slices of 100 MS (a histogram). This allows to identify spikes in load that max out the system, but only for a short amount of time. A value of UINT8_MAX implies the field is unused. + * @param gpu_cores GPU usage on the component in percent (100 - idle). A value of UINT8_MAX implies the field is unused. + * @param gpu_combined Combined GPU usage as the last 10 slices of 100 MS (a histogram). This allows to identify spikes in load that max out the system, but only for a short amount of time. A value of UINT8_MAX implies the field is unused. + * @param temperature_board [degC] Temperature of the board. A value of INT8_MAX implies the field is unused. + * @param temperature_core [degC] Temperature of the CPU core. A value of INT8_MAX implies the field is unused. + * @param fan_speed [rpm] Fan speeds. A value of INT16_MAX implies the field is unused. + * @param ram_usage [MiB] Amount of used RAM on the component system. A value of UINT32_MAX implies the field is unused. + * @param ram_total [MiB] Total amount of RAM on the component system. A value of UINT32_MAX implies the field is unused. + * @param storage_type Storage type: 0: HDD, 1: SSD, 2: EMMC, 3: SD card (non-removable), 4: SD card (removable). A value of UINT32_MAX implies the field is unused. + * @param storage_usage [MiB] Amount of used storage space on the component system. A value of UINT32_MAX implies the field is unused. + * @param storage_total [MiB] Total amount of storage space on the component system. A value of UINT32_MAX implies the field is unused. + * @param link_type Link type: 0-9: UART, 10-19: Wired network, 20-29: Wifi, 30-39: Point-to-point proprietary, 40-49: Mesh proprietary + * @param link_tx_rate [KiB/s] Network traffic from the component system. A value of UINT32_MAX implies the field is unused. + * @param link_rx_rate [KiB/s] Network traffic to the component system. A value of UINT32_MAX implies the field is unused. + * @param link_tx_max [KiB/s] Network capacity from the component system. A value of UINT32_MAX implies the field is unused. + * @param link_rx_max [KiB/s] Network capacity to the component system. A value of UINT32_MAX implies the field is unused. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_onboard_computer_status_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint64_t time_usec, uint32_t uptime, uint8_t type, const uint8_t *cpu_cores, const uint8_t *cpu_combined, const uint8_t *gpu_cores, const uint8_t *gpu_combined, int8_t temperature_board, const int8_t *temperature_core, const int16_t *fan_speed, uint32_t ram_usage, uint32_t ram_total, const uint32_t *storage_type, const uint32_t *storage_usage, const uint32_t *storage_total, const uint32_t *link_type, const uint32_t *link_tx_rate, const uint32_t *link_rx_rate, const uint32_t *link_tx_max, const uint32_t *link_rx_max) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_ONBOARD_COMPUTER_STATUS_LEN]; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_uint32_t(buf, 8, uptime); + _mav_put_uint32_t(buf, 12, ram_usage); + _mav_put_uint32_t(buf, 16, ram_total); + _mav_put_uint8_t(buf, 196, type); + _mav_put_int8_t(buf, 229, temperature_board); + _mav_put_uint32_t_array(buf, 20, storage_type, 4); + _mav_put_uint32_t_array(buf, 36, storage_usage, 4); + _mav_put_uint32_t_array(buf, 52, storage_total, 4); + _mav_put_uint32_t_array(buf, 68, link_type, 6); + _mav_put_uint32_t_array(buf, 92, link_tx_rate, 6); + _mav_put_uint32_t_array(buf, 116, link_rx_rate, 6); + _mav_put_uint32_t_array(buf, 140, link_tx_max, 6); + _mav_put_uint32_t_array(buf, 164, link_rx_max, 6); + _mav_put_int16_t_array(buf, 188, fan_speed, 4); + _mav_put_uint8_t_array(buf, 197, cpu_cores, 8); + _mav_put_uint8_t_array(buf, 205, cpu_combined, 10); + _mav_put_uint8_t_array(buf, 215, gpu_cores, 4); + _mav_put_uint8_t_array(buf, 219, gpu_combined, 10); + _mav_put_int8_t_array(buf, 230, temperature_core, 8); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_ONBOARD_COMPUTER_STATUS_LEN); +#else + mavlink_onboard_computer_status_t packet; + packet.time_usec = time_usec; + packet.uptime = uptime; + packet.ram_usage = ram_usage; + packet.ram_total = ram_total; + packet.type = type; + packet.temperature_board = temperature_board; + mav_array_memcpy(packet.storage_type, storage_type, sizeof(uint32_t)*4); + mav_array_memcpy(packet.storage_usage, storage_usage, sizeof(uint32_t)*4); + mav_array_memcpy(packet.storage_total, storage_total, sizeof(uint32_t)*4); + mav_array_memcpy(packet.link_type, link_type, sizeof(uint32_t)*6); + mav_array_memcpy(packet.link_tx_rate, link_tx_rate, sizeof(uint32_t)*6); + mav_array_memcpy(packet.link_rx_rate, link_rx_rate, sizeof(uint32_t)*6); + mav_array_memcpy(packet.link_tx_max, link_tx_max, sizeof(uint32_t)*6); + mav_array_memcpy(packet.link_rx_max, link_rx_max, sizeof(uint32_t)*6); + mav_array_memcpy(packet.fan_speed, fan_speed, sizeof(int16_t)*4); + mav_array_memcpy(packet.cpu_cores, cpu_cores, sizeof(uint8_t)*8); + mav_array_memcpy(packet.cpu_combined, cpu_combined, sizeof(uint8_t)*10); + mav_array_memcpy(packet.gpu_cores, gpu_cores, sizeof(uint8_t)*4); + mav_array_memcpy(packet.gpu_combined, gpu_combined, sizeof(uint8_t)*10); + mav_array_memcpy(packet.temperature_core, temperature_core, sizeof(int8_t)*8); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_ONBOARD_COMPUTER_STATUS_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_ONBOARD_COMPUTER_STATUS; + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_ONBOARD_COMPUTER_STATUS_MIN_LEN, MAVLINK_MSG_ID_ONBOARD_COMPUTER_STATUS_LEN, MAVLINK_MSG_ID_ONBOARD_COMPUTER_STATUS_CRC); +} + +/** + * @brief Pack a onboard_computer_status message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param time_usec [us] Timestamp (UNIX Epoch time or time since system boot). The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude of the number. + * @param uptime [ms] Time since system boot. + * @param type Type of the onboard computer: 0: Mission computer primary, 1: Mission computer backup 1, 2: Mission computer backup 2, 3: Compute node, 4-5: Compute spares, 6-9: Payload computers. + * @param cpu_cores CPU usage on the component in percent (100 - idle). A value of UINT8_MAX implies the field is unused. + * @param cpu_combined Combined CPU usage as the last 10 slices of 100 MS (a histogram). This allows to identify spikes in load that max out the system, but only for a short amount of time. A value of UINT8_MAX implies the field is unused. + * @param gpu_cores GPU usage on the component in percent (100 - idle). A value of UINT8_MAX implies the field is unused. + * @param gpu_combined Combined GPU usage as the last 10 slices of 100 MS (a histogram). This allows to identify spikes in load that max out the system, but only for a short amount of time. A value of UINT8_MAX implies the field is unused. + * @param temperature_board [degC] Temperature of the board. A value of INT8_MAX implies the field is unused. + * @param temperature_core [degC] Temperature of the CPU core. A value of INT8_MAX implies the field is unused. + * @param fan_speed [rpm] Fan speeds. A value of INT16_MAX implies the field is unused. + * @param ram_usage [MiB] Amount of used RAM on the component system. A value of UINT32_MAX implies the field is unused. + * @param ram_total [MiB] Total amount of RAM on the component system. A value of UINT32_MAX implies the field is unused. + * @param storage_type Storage type: 0: HDD, 1: SSD, 2: EMMC, 3: SD card (non-removable), 4: SD card (removable). A value of UINT32_MAX implies the field is unused. + * @param storage_usage [MiB] Amount of used storage space on the component system. A value of UINT32_MAX implies the field is unused. + * @param storage_total [MiB] Total amount of storage space on the component system. A value of UINT32_MAX implies the field is unused. + * @param link_type Link type: 0-9: UART, 10-19: Wired network, 20-29: Wifi, 30-39: Point-to-point proprietary, 40-49: Mesh proprietary + * @param link_tx_rate [KiB/s] Network traffic from the component system. A value of UINT32_MAX implies the field is unused. + * @param link_rx_rate [KiB/s] Network traffic to the component system. A value of UINT32_MAX implies the field is unused. + * @param link_tx_max [KiB/s] Network capacity from the component system. A value of UINT32_MAX implies the field is unused. + * @param link_rx_max [KiB/s] Network capacity to the component system. A value of UINT32_MAX implies the field is unused. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_onboard_computer_status_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint64_t time_usec,uint32_t uptime,uint8_t type,const uint8_t *cpu_cores,const uint8_t *cpu_combined,const uint8_t *gpu_cores,const uint8_t *gpu_combined,int8_t temperature_board,const int8_t *temperature_core,const int16_t *fan_speed,uint32_t ram_usage,uint32_t ram_total,const uint32_t *storage_type,const uint32_t *storage_usage,const uint32_t *storage_total,const uint32_t *link_type,const uint32_t *link_tx_rate,const uint32_t *link_rx_rate,const uint32_t *link_tx_max,const uint32_t *link_rx_max) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_ONBOARD_COMPUTER_STATUS_LEN]; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_uint32_t(buf, 8, uptime); + _mav_put_uint32_t(buf, 12, ram_usage); + _mav_put_uint32_t(buf, 16, ram_total); + _mav_put_uint8_t(buf, 196, type); + _mav_put_int8_t(buf, 229, temperature_board); + _mav_put_uint32_t_array(buf, 20, storage_type, 4); + _mav_put_uint32_t_array(buf, 36, storage_usage, 4); + _mav_put_uint32_t_array(buf, 52, storage_total, 4); + _mav_put_uint32_t_array(buf, 68, link_type, 6); + _mav_put_uint32_t_array(buf, 92, link_tx_rate, 6); + _mav_put_uint32_t_array(buf, 116, link_rx_rate, 6); + _mav_put_uint32_t_array(buf, 140, link_tx_max, 6); + _mav_put_uint32_t_array(buf, 164, link_rx_max, 6); + _mav_put_int16_t_array(buf, 188, fan_speed, 4); + _mav_put_uint8_t_array(buf, 197, cpu_cores, 8); + _mav_put_uint8_t_array(buf, 205, cpu_combined, 10); + _mav_put_uint8_t_array(buf, 215, gpu_cores, 4); + _mav_put_uint8_t_array(buf, 219, gpu_combined, 10); + _mav_put_int8_t_array(buf, 230, temperature_core, 8); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_ONBOARD_COMPUTER_STATUS_LEN); +#else + mavlink_onboard_computer_status_t packet; + packet.time_usec = time_usec; + packet.uptime = uptime; + packet.ram_usage = ram_usage; + packet.ram_total = ram_total; + packet.type = type; + packet.temperature_board = temperature_board; + mav_array_memcpy(packet.storage_type, storage_type, sizeof(uint32_t)*4); + mav_array_memcpy(packet.storage_usage, storage_usage, sizeof(uint32_t)*4); + mav_array_memcpy(packet.storage_total, storage_total, sizeof(uint32_t)*4); + mav_array_memcpy(packet.link_type, link_type, sizeof(uint32_t)*6); + mav_array_memcpy(packet.link_tx_rate, link_tx_rate, sizeof(uint32_t)*6); + mav_array_memcpy(packet.link_rx_rate, link_rx_rate, sizeof(uint32_t)*6); + mav_array_memcpy(packet.link_tx_max, link_tx_max, sizeof(uint32_t)*6); + mav_array_memcpy(packet.link_rx_max, link_rx_max, sizeof(uint32_t)*6); + mav_array_memcpy(packet.fan_speed, fan_speed, sizeof(int16_t)*4); + mav_array_memcpy(packet.cpu_cores, cpu_cores, sizeof(uint8_t)*8); + mav_array_memcpy(packet.cpu_combined, cpu_combined, sizeof(uint8_t)*10); + mav_array_memcpy(packet.gpu_cores, gpu_cores, sizeof(uint8_t)*4); + mav_array_memcpy(packet.gpu_combined, gpu_combined, sizeof(uint8_t)*10); + mav_array_memcpy(packet.temperature_core, temperature_core, sizeof(int8_t)*8); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_ONBOARD_COMPUTER_STATUS_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_ONBOARD_COMPUTER_STATUS; + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_ONBOARD_COMPUTER_STATUS_MIN_LEN, MAVLINK_MSG_ID_ONBOARD_COMPUTER_STATUS_LEN, MAVLINK_MSG_ID_ONBOARD_COMPUTER_STATUS_CRC); +} + +/** + * @brief Encode a onboard_computer_status struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param onboard_computer_status C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_onboard_computer_status_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_onboard_computer_status_t* onboard_computer_status) +{ + return mavlink_msg_onboard_computer_status_pack(system_id, component_id, msg, onboard_computer_status->time_usec, onboard_computer_status->uptime, onboard_computer_status->type, onboard_computer_status->cpu_cores, onboard_computer_status->cpu_combined, onboard_computer_status->gpu_cores, onboard_computer_status->gpu_combined, onboard_computer_status->temperature_board, onboard_computer_status->temperature_core, onboard_computer_status->fan_speed, onboard_computer_status->ram_usage, onboard_computer_status->ram_total, onboard_computer_status->storage_type, onboard_computer_status->storage_usage, onboard_computer_status->storage_total, onboard_computer_status->link_type, onboard_computer_status->link_tx_rate, onboard_computer_status->link_rx_rate, onboard_computer_status->link_tx_max, onboard_computer_status->link_rx_max); +} + +/** + * @brief Encode a onboard_computer_status struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param onboard_computer_status C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_onboard_computer_status_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_onboard_computer_status_t* onboard_computer_status) +{ + return mavlink_msg_onboard_computer_status_pack_chan(system_id, component_id, chan, msg, onboard_computer_status->time_usec, onboard_computer_status->uptime, onboard_computer_status->type, onboard_computer_status->cpu_cores, onboard_computer_status->cpu_combined, onboard_computer_status->gpu_cores, onboard_computer_status->gpu_combined, onboard_computer_status->temperature_board, onboard_computer_status->temperature_core, onboard_computer_status->fan_speed, onboard_computer_status->ram_usage, onboard_computer_status->ram_total, onboard_computer_status->storage_type, onboard_computer_status->storage_usage, onboard_computer_status->storage_total, onboard_computer_status->link_type, onboard_computer_status->link_tx_rate, onboard_computer_status->link_rx_rate, onboard_computer_status->link_tx_max, onboard_computer_status->link_rx_max); +} + +/** + * @brief Send a onboard_computer_status message + * @param chan MAVLink channel to send the message + * + * @param time_usec [us] Timestamp (UNIX Epoch time or time since system boot). The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude of the number. + * @param uptime [ms] Time since system boot. + * @param type Type of the onboard computer: 0: Mission computer primary, 1: Mission computer backup 1, 2: Mission computer backup 2, 3: Compute node, 4-5: Compute spares, 6-9: Payload computers. + * @param cpu_cores CPU usage on the component in percent (100 - idle). A value of UINT8_MAX implies the field is unused. + * @param cpu_combined Combined CPU usage as the last 10 slices of 100 MS (a histogram). This allows to identify spikes in load that max out the system, but only for a short amount of time. A value of UINT8_MAX implies the field is unused. + * @param gpu_cores GPU usage on the component in percent (100 - idle). A value of UINT8_MAX implies the field is unused. + * @param gpu_combined Combined GPU usage as the last 10 slices of 100 MS (a histogram). This allows to identify spikes in load that max out the system, but only for a short amount of time. A value of UINT8_MAX implies the field is unused. + * @param temperature_board [degC] Temperature of the board. A value of INT8_MAX implies the field is unused. + * @param temperature_core [degC] Temperature of the CPU core. A value of INT8_MAX implies the field is unused. + * @param fan_speed [rpm] Fan speeds. A value of INT16_MAX implies the field is unused. + * @param ram_usage [MiB] Amount of used RAM on the component system. A value of UINT32_MAX implies the field is unused. + * @param ram_total [MiB] Total amount of RAM on the component system. A value of UINT32_MAX implies the field is unused. + * @param storage_type Storage type: 0: HDD, 1: SSD, 2: EMMC, 3: SD card (non-removable), 4: SD card (removable). A value of UINT32_MAX implies the field is unused. + * @param storage_usage [MiB] Amount of used storage space on the component system. A value of UINT32_MAX implies the field is unused. + * @param storage_total [MiB] Total amount of storage space on the component system. A value of UINT32_MAX implies the field is unused. + * @param link_type Link type: 0-9: UART, 10-19: Wired network, 20-29: Wifi, 30-39: Point-to-point proprietary, 40-49: Mesh proprietary + * @param link_tx_rate [KiB/s] Network traffic from the component system. A value of UINT32_MAX implies the field is unused. + * @param link_rx_rate [KiB/s] Network traffic to the component system. A value of UINT32_MAX implies the field is unused. + * @param link_tx_max [KiB/s] Network capacity from the component system. A value of UINT32_MAX implies the field is unused. + * @param link_rx_max [KiB/s] Network capacity to the component system. A value of UINT32_MAX implies the field is unused. + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_onboard_computer_status_send(mavlink_channel_t chan, uint64_t time_usec, uint32_t uptime, uint8_t type, const uint8_t *cpu_cores, const uint8_t *cpu_combined, const uint8_t *gpu_cores, const uint8_t *gpu_combined, int8_t temperature_board, const int8_t *temperature_core, const int16_t *fan_speed, uint32_t ram_usage, uint32_t ram_total, const uint32_t *storage_type, const uint32_t *storage_usage, const uint32_t *storage_total, const uint32_t *link_type, const uint32_t *link_tx_rate, const uint32_t *link_rx_rate, const uint32_t *link_tx_max, const uint32_t *link_rx_max) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_ONBOARD_COMPUTER_STATUS_LEN]; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_uint32_t(buf, 8, uptime); + _mav_put_uint32_t(buf, 12, ram_usage); + _mav_put_uint32_t(buf, 16, ram_total); + _mav_put_uint8_t(buf, 196, type); + _mav_put_int8_t(buf, 229, temperature_board); + _mav_put_uint32_t_array(buf, 20, storage_type, 4); + _mav_put_uint32_t_array(buf, 36, storage_usage, 4); + _mav_put_uint32_t_array(buf, 52, storage_total, 4); + _mav_put_uint32_t_array(buf, 68, link_type, 6); + _mav_put_uint32_t_array(buf, 92, link_tx_rate, 6); + _mav_put_uint32_t_array(buf, 116, link_rx_rate, 6); + _mav_put_uint32_t_array(buf, 140, link_tx_max, 6); + _mav_put_uint32_t_array(buf, 164, link_rx_max, 6); + _mav_put_int16_t_array(buf, 188, fan_speed, 4); + _mav_put_uint8_t_array(buf, 197, cpu_cores, 8); + _mav_put_uint8_t_array(buf, 205, cpu_combined, 10); + _mav_put_uint8_t_array(buf, 215, gpu_cores, 4); + _mav_put_uint8_t_array(buf, 219, gpu_combined, 10); + _mav_put_int8_t_array(buf, 230, temperature_core, 8); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ONBOARD_COMPUTER_STATUS, buf, MAVLINK_MSG_ID_ONBOARD_COMPUTER_STATUS_MIN_LEN, MAVLINK_MSG_ID_ONBOARD_COMPUTER_STATUS_LEN, MAVLINK_MSG_ID_ONBOARD_COMPUTER_STATUS_CRC); +#else + mavlink_onboard_computer_status_t packet; + packet.time_usec = time_usec; + packet.uptime = uptime; + packet.ram_usage = ram_usage; + packet.ram_total = ram_total; + packet.type = type; + packet.temperature_board = temperature_board; + mav_array_memcpy(packet.storage_type, storage_type, sizeof(uint32_t)*4); + mav_array_memcpy(packet.storage_usage, storage_usage, sizeof(uint32_t)*4); + mav_array_memcpy(packet.storage_total, storage_total, sizeof(uint32_t)*4); + mav_array_memcpy(packet.link_type, link_type, sizeof(uint32_t)*6); + mav_array_memcpy(packet.link_tx_rate, link_tx_rate, sizeof(uint32_t)*6); + mav_array_memcpy(packet.link_rx_rate, link_rx_rate, sizeof(uint32_t)*6); + mav_array_memcpy(packet.link_tx_max, link_tx_max, sizeof(uint32_t)*6); + mav_array_memcpy(packet.link_rx_max, link_rx_max, sizeof(uint32_t)*6); + mav_array_memcpy(packet.fan_speed, fan_speed, sizeof(int16_t)*4); + mav_array_memcpy(packet.cpu_cores, cpu_cores, sizeof(uint8_t)*8); + mav_array_memcpy(packet.cpu_combined, cpu_combined, sizeof(uint8_t)*10); + mav_array_memcpy(packet.gpu_cores, gpu_cores, sizeof(uint8_t)*4); + mav_array_memcpy(packet.gpu_combined, gpu_combined, sizeof(uint8_t)*10); + mav_array_memcpy(packet.temperature_core, temperature_core, sizeof(int8_t)*8); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ONBOARD_COMPUTER_STATUS, (const char *)&packet, MAVLINK_MSG_ID_ONBOARD_COMPUTER_STATUS_MIN_LEN, MAVLINK_MSG_ID_ONBOARD_COMPUTER_STATUS_LEN, MAVLINK_MSG_ID_ONBOARD_COMPUTER_STATUS_CRC); +#endif +} + +/** + * @brief Send a onboard_computer_status message + * @param chan MAVLink channel to send the message + * @param struct The MAVLink struct to serialize + */ +static inline void mavlink_msg_onboard_computer_status_send_struct(mavlink_channel_t chan, const mavlink_onboard_computer_status_t* onboard_computer_status) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + mavlink_msg_onboard_computer_status_send(chan, onboard_computer_status->time_usec, onboard_computer_status->uptime, onboard_computer_status->type, onboard_computer_status->cpu_cores, onboard_computer_status->cpu_combined, onboard_computer_status->gpu_cores, onboard_computer_status->gpu_combined, onboard_computer_status->temperature_board, onboard_computer_status->temperature_core, onboard_computer_status->fan_speed, onboard_computer_status->ram_usage, onboard_computer_status->ram_total, onboard_computer_status->storage_type, onboard_computer_status->storage_usage, onboard_computer_status->storage_total, onboard_computer_status->link_type, onboard_computer_status->link_tx_rate, onboard_computer_status->link_rx_rate, onboard_computer_status->link_tx_max, onboard_computer_status->link_rx_max); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ONBOARD_COMPUTER_STATUS, (const char *)onboard_computer_status, MAVLINK_MSG_ID_ONBOARD_COMPUTER_STATUS_MIN_LEN, MAVLINK_MSG_ID_ONBOARD_COMPUTER_STATUS_LEN, MAVLINK_MSG_ID_ONBOARD_COMPUTER_STATUS_CRC); +#endif +} + +#if MAVLINK_MSG_ID_ONBOARD_COMPUTER_STATUS_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_onboard_computer_status_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint64_t time_usec, uint32_t uptime, uint8_t type, const uint8_t *cpu_cores, const uint8_t *cpu_combined, const uint8_t *gpu_cores, const uint8_t *gpu_combined, int8_t temperature_board, const int8_t *temperature_core, const int16_t *fan_speed, uint32_t ram_usage, uint32_t ram_total, const uint32_t *storage_type, const uint32_t *storage_usage, const uint32_t *storage_total, const uint32_t *link_type, const uint32_t *link_tx_rate, const uint32_t *link_rx_rate, const uint32_t *link_tx_max, const uint32_t *link_rx_max) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_uint32_t(buf, 8, uptime); + _mav_put_uint32_t(buf, 12, ram_usage); + _mav_put_uint32_t(buf, 16, ram_total); + _mav_put_uint8_t(buf, 196, type); + _mav_put_int8_t(buf, 229, temperature_board); + _mav_put_uint32_t_array(buf, 20, storage_type, 4); + _mav_put_uint32_t_array(buf, 36, storage_usage, 4); + _mav_put_uint32_t_array(buf, 52, storage_total, 4); + _mav_put_uint32_t_array(buf, 68, link_type, 6); + _mav_put_uint32_t_array(buf, 92, link_tx_rate, 6); + _mav_put_uint32_t_array(buf, 116, link_rx_rate, 6); + _mav_put_uint32_t_array(buf, 140, link_tx_max, 6); + _mav_put_uint32_t_array(buf, 164, link_rx_max, 6); + _mav_put_int16_t_array(buf, 188, fan_speed, 4); + _mav_put_uint8_t_array(buf, 197, cpu_cores, 8); + _mav_put_uint8_t_array(buf, 205, cpu_combined, 10); + _mav_put_uint8_t_array(buf, 215, gpu_cores, 4); + _mav_put_uint8_t_array(buf, 219, gpu_combined, 10); + _mav_put_int8_t_array(buf, 230, temperature_core, 8); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ONBOARD_COMPUTER_STATUS, buf, MAVLINK_MSG_ID_ONBOARD_COMPUTER_STATUS_MIN_LEN, MAVLINK_MSG_ID_ONBOARD_COMPUTER_STATUS_LEN, MAVLINK_MSG_ID_ONBOARD_COMPUTER_STATUS_CRC); +#else + mavlink_onboard_computer_status_t *packet = (mavlink_onboard_computer_status_t *)msgbuf; + packet->time_usec = time_usec; + packet->uptime = uptime; + packet->ram_usage = ram_usage; + packet->ram_total = ram_total; + packet->type = type; + packet->temperature_board = temperature_board; + mav_array_memcpy(packet->storage_type, storage_type, sizeof(uint32_t)*4); + mav_array_memcpy(packet->storage_usage, storage_usage, sizeof(uint32_t)*4); + mav_array_memcpy(packet->storage_total, storage_total, sizeof(uint32_t)*4); + mav_array_memcpy(packet->link_type, link_type, sizeof(uint32_t)*6); + mav_array_memcpy(packet->link_tx_rate, link_tx_rate, sizeof(uint32_t)*6); + mav_array_memcpy(packet->link_rx_rate, link_rx_rate, sizeof(uint32_t)*6); + mav_array_memcpy(packet->link_tx_max, link_tx_max, sizeof(uint32_t)*6); + mav_array_memcpy(packet->link_rx_max, link_rx_max, sizeof(uint32_t)*6); + mav_array_memcpy(packet->fan_speed, fan_speed, sizeof(int16_t)*4); + mav_array_memcpy(packet->cpu_cores, cpu_cores, sizeof(uint8_t)*8); + mav_array_memcpy(packet->cpu_combined, cpu_combined, sizeof(uint8_t)*10); + mav_array_memcpy(packet->gpu_cores, gpu_cores, sizeof(uint8_t)*4); + mav_array_memcpy(packet->gpu_combined, gpu_combined, sizeof(uint8_t)*10); + mav_array_memcpy(packet->temperature_core, temperature_core, sizeof(int8_t)*8); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ONBOARD_COMPUTER_STATUS, (const char *)packet, MAVLINK_MSG_ID_ONBOARD_COMPUTER_STATUS_MIN_LEN, MAVLINK_MSG_ID_ONBOARD_COMPUTER_STATUS_LEN, MAVLINK_MSG_ID_ONBOARD_COMPUTER_STATUS_CRC); +#endif +} +#endif + +#endif + +// MESSAGE ONBOARD_COMPUTER_STATUS UNPACKING + + +/** + * @brief Get field time_usec from onboard_computer_status message + * + * @return [us] Timestamp (UNIX Epoch time or time since system boot). The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude of the number. + */ +static inline uint64_t mavlink_msg_onboard_computer_status_get_time_usec(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint64_t(msg, 0); +} + +/** + * @brief Get field uptime from onboard_computer_status message + * + * @return [ms] Time since system boot. + */ +static inline uint32_t mavlink_msg_onboard_computer_status_get_uptime(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint32_t(msg, 8); +} + +/** + * @brief Get field type from onboard_computer_status message + * + * @return Type of the onboard computer: 0: Mission computer primary, 1: Mission computer backup 1, 2: Mission computer backup 2, 3: Compute node, 4-5: Compute spares, 6-9: Payload computers. + */ +static inline uint8_t mavlink_msg_onboard_computer_status_get_type(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 196); +} + +/** + * @brief Get field cpu_cores from onboard_computer_status message + * + * @return CPU usage on the component in percent (100 - idle). A value of UINT8_MAX implies the field is unused. + */ +static inline uint16_t mavlink_msg_onboard_computer_status_get_cpu_cores(const mavlink_message_t* msg, uint8_t *cpu_cores) +{ + return _MAV_RETURN_uint8_t_array(msg, cpu_cores, 8, 197); +} + +/** + * @brief Get field cpu_combined from onboard_computer_status message + * + * @return Combined CPU usage as the last 10 slices of 100 MS (a histogram). This allows to identify spikes in load that max out the system, but only for a short amount of time. A value of UINT8_MAX implies the field is unused. + */ +static inline uint16_t mavlink_msg_onboard_computer_status_get_cpu_combined(const mavlink_message_t* msg, uint8_t *cpu_combined) +{ + return _MAV_RETURN_uint8_t_array(msg, cpu_combined, 10, 205); +} + +/** + * @brief Get field gpu_cores from onboard_computer_status message + * + * @return GPU usage on the component in percent (100 - idle). A value of UINT8_MAX implies the field is unused. + */ +static inline uint16_t mavlink_msg_onboard_computer_status_get_gpu_cores(const mavlink_message_t* msg, uint8_t *gpu_cores) +{ + return _MAV_RETURN_uint8_t_array(msg, gpu_cores, 4, 215); +} + +/** + * @brief Get field gpu_combined from onboard_computer_status message + * + * @return Combined GPU usage as the last 10 slices of 100 MS (a histogram). This allows to identify spikes in load that max out the system, but only for a short amount of time. A value of UINT8_MAX implies the field is unused. + */ +static inline uint16_t mavlink_msg_onboard_computer_status_get_gpu_combined(const mavlink_message_t* msg, uint8_t *gpu_combined) +{ + return _MAV_RETURN_uint8_t_array(msg, gpu_combined, 10, 219); +} + +/** + * @brief Get field temperature_board from onboard_computer_status message + * + * @return [degC] Temperature of the board. A value of INT8_MAX implies the field is unused. + */ +static inline int8_t mavlink_msg_onboard_computer_status_get_temperature_board(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int8_t(msg, 229); +} + +/** + * @brief Get field temperature_core from onboard_computer_status message + * + * @return [degC] Temperature of the CPU core. A value of INT8_MAX implies the field is unused. + */ +static inline uint16_t mavlink_msg_onboard_computer_status_get_temperature_core(const mavlink_message_t* msg, int8_t *temperature_core) +{ + return _MAV_RETURN_int8_t_array(msg, temperature_core, 8, 230); +} + +/** + * @brief Get field fan_speed from onboard_computer_status message + * + * @return [rpm] Fan speeds. A value of INT16_MAX implies the field is unused. + */ +static inline uint16_t mavlink_msg_onboard_computer_status_get_fan_speed(const mavlink_message_t* msg, int16_t *fan_speed) +{ + return _MAV_RETURN_int16_t_array(msg, fan_speed, 4, 188); +} + +/** + * @brief Get field ram_usage from onboard_computer_status message + * + * @return [MiB] Amount of used RAM on the component system. A value of UINT32_MAX implies the field is unused. + */ +static inline uint32_t mavlink_msg_onboard_computer_status_get_ram_usage(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint32_t(msg, 12); +} + +/** + * @brief Get field ram_total from onboard_computer_status message + * + * @return [MiB] Total amount of RAM on the component system. A value of UINT32_MAX implies the field is unused. + */ +static inline uint32_t mavlink_msg_onboard_computer_status_get_ram_total(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint32_t(msg, 16); +} + +/** + * @brief Get field storage_type from onboard_computer_status message + * + * @return Storage type: 0: HDD, 1: SSD, 2: EMMC, 3: SD card (non-removable), 4: SD card (removable). A value of UINT32_MAX implies the field is unused. + */ +static inline uint16_t mavlink_msg_onboard_computer_status_get_storage_type(const mavlink_message_t* msg, uint32_t *storage_type) +{ + return _MAV_RETURN_uint32_t_array(msg, storage_type, 4, 20); +} + +/** + * @brief Get field storage_usage from onboard_computer_status message + * + * @return [MiB] Amount of used storage space on the component system. A value of UINT32_MAX implies the field is unused. + */ +static inline uint16_t mavlink_msg_onboard_computer_status_get_storage_usage(const mavlink_message_t* msg, uint32_t *storage_usage) +{ + return _MAV_RETURN_uint32_t_array(msg, storage_usage, 4, 36); +} + +/** + * @brief Get field storage_total from onboard_computer_status message + * + * @return [MiB] Total amount of storage space on the component system. A value of UINT32_MAX implies the field is unused. + */ +static inline uint16_t mavlink_msg_onboard_computer_status_get_storage_total(const mavlink_message_t* msg, uint32_t *storage_total) +{ + return _MAV_RETURN_uint32_t_array(msg, storage_total, 4, 52); +} + +/** + * @brief Get field link_type from onboard_computer_status message + * + * @return Link type: 0-9: UART, 10-19: Wired network, 20-29: Wifi, 30-39: Point-to-point proprietary, 40-49: Mesh proprietary + */ +static inline uint16_t mavlink_msg_onboard_computer_status_get_link_type(const mavlink_message_t* msg, uint32_t *link_type) +{ + return _MAV_RETURN_uint32_t_array(msg, link_type, 6, 68); +} + +/** + * @brief Get field link_tx_rate from onboard_computer_status message + * + * @return [KiB/s] Network traffic from the component system. A value of UINT32_MAX implies the field is unused. + */ +static inline uint16_t mavlink_msg_onboard_computer_status_get_link_tx_rate(const mavlink_message_t* msg, uint32_t *link_tx_rate) +{ + return _MAV_RETURN_uint32_t_array(msg, link_tx_rate, 6, 92); +} + +/** + * @brief Get field link_rx_rate from onboard_computer_status message + * + * @return [KiB/s] Network traffic to the component system. A value of UINT32_MAX implies the field is unused. + */ +static inline uint16_t mavlink_msg_onboard_computer_status_get_link_rx_rate(const mavlink_message_t* msg, uint32_t *link_rx_rate) +{ + return _MAV_RETURN_uint32_t_array(msg, link_rx_rate, 6, 116); +} + +/** + * @brief Get field link_tx_max from onboard_computer_status message + * + * @return [KiB/s] Network capacity from the component system. A value of UINT32_MAX implies the field is unused. + */ +static inline uint16_t mavlink_msg_onboard_computer_status_get_link_tx_max(const mavlink_message_t* msg, uint32_t *link_tx_max) +{ + return _MAV_RETURN_uint32_t_array(msg, link_tx_max, 6, 140); +} + +/** + * @brief Get field link_rx_max from onboard_computer_status message + * + * @return [KiB/s] Network capacity to the component system. A value of UINT32_MAX implies the field is unused. + */ +static inline uint16_t mavlink_msg_onboard_computer_status_get_link_rx_max(const mavlink_message_t* msg, uint32_t *link_rx_max) +{ + return _MAV_RETURN_uint32_t_array(msg, link_rx_max, 6, 164); +} + +/** + * @brief Decode a onboard_computer_status message into a struct + * + * @param msg The message to decode + * @param onboard_computer_status C-struct to decode the message contents into + */ +static inline void mavlink_msg_onboard_computer_status_decode(const mavlink_message_t* msg, mavlink_onboard_computer_status_t* onboard_computer_status) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + onboard_computer_status->time_usec = mavlink_msg_onboard_computer_status_get_time_usec(msg); + onboard_computer_status->uptime = mavlink_msg_onboard_computer_status_get_uptime(msg); + onboard_computer_status->ram_usage = mavlink_msg_onboard_computer_status_get_ram_usage(msg); + onboard_computer_status->ram_total = mavlink_msg_onboard_computer_status_get_ram_total(msg); + mavlink_msg_onboard_computer_status_get_storage_type(msg, onboard_computer_status->storage_type); + mavlink_msg_onboard_computer_status_get_storage_usage(msg, onboard_computer_status->storage_usage); + mavlink_msg_onboard_computer_status_get_storage_total(msg, onboard_computer_status->storage_total); + mavlink_msg_onboard_computer_status_get_link_type(msg, onboard_computer_status->link_type); + mavlink_msg_onboard_computer_status_get_link_tx_rate(msg, onboard_computer_status->link_tx_rate); + mavlink_msg_onboard_computer_status_get_link_rx_rate(msg, onboard_computer_status->link_rx_rate); + mavlink_msg_onboard_computer_status_get_link_tx_max(msg, onboard_computer_status->link_tx_max); + mavlink_msg_onboard_computer_status_get_link_rx_max(msg, onboard_computer_status->link_rx_max); + mavlink_msg_onboard_computer_status_get_fan_speed(msg, onboard_computer_status->fan_speed); + onboard_computer_status->type = mavlink_msg_onboard_computer_status_get_type(msg); + mavlink_msg_onboard_computer_status_get_cpu_cores(msg, onboard_computer_status->cpu_cores); + mavlink_msg_onboard_computer_status_get_cpu_combined(msg, onboard_computer_status->cpu_combined); + mavlink_msg_onboard_computer_status_get_gpu_cores(msg, onboard_computer_status->gpu_cores); + mavlink_msg_onboard_computer_status_get_gpu_combined(msg, onboard_computer_status->gpu_combined); + onboard_computer_status->temperature_board = mavlink_msg_onboard_computer_status_get_temperature_board(msg); + mavlink_msg_onboard_computer_status_get_temperature_core(msg, onboard_computer_status->temperature_core); +#else + uint8_t len = msg->len < MAVLINK_MSG_ID_ONBOARD_COMPUTER_STATUS_LEN? msg->len : MAVLINK_MSG_ID_ONBOARD_COMPUTER_STATUS_LEN; + memset(onboard_computer_status, 0, MAVLINK_MSG_ID_ONBOARD_COMPUTER_STATUS_LEN); + memcpy(onboard_computer_status, _MAV_PAYLOAD(msg), len); +#endif +} diff --git a/lib/main/MAVLink/common/mavlink_msg_open_drone_id_authentication.h b/lib/main/MAVLink/common/mavlink_msg_open_drone_id_authentication.h new file mode 100644 index 0000000000..cd7b68e026 --- /dev/null +++ b/lib/main/MAVLink/common/mavlink_msg_open_drone_id_authentication.h @@ -0,0 +1,406 @@ +#pragma once +// MESSAGE OPEN_DRONE_ID_AUTHENTICATION PACKING + +#define MAVLINK_MSG_ID_OPEN_DRONE_ID_AUTHENTICATION 12902 + + +typedef struct __mavlink_open_drone_id_authentication_t { + uint32_t timestamp; /*< [s] This field is only present for page 0. 32 bit Unix Timestamp in seconds since 00:00:00 01/01/2019.*/ + uint8_t target_system; /*< System ID (0 for broadcast).*/ + uint8_t target_component; /*< Component ID (0 for broadcast).*/ + uint8_t id_or_mac[20]; /*< Only used for drone ID data received from other UAs. See detailed description at https://mavlink.io/en/services/opendroneid.html. */ + uint8_t authentication_type; /*< Indicates the type of authentication.*/ + uint8_t data_page; /*< Allowed range is 0 - 4.*/ + uint8_t page_count; /*< This field is only present for page 0. Allowed range is 0 - 5.*/ + uint8_t length; /*< [bytes] This field is only present for page 0. Total bytes of authentication_data from all data pages. Allowed range is 0 - 109 (17 + 23*4).*/ + uint8_t authentication_data[23]; /*< Opaque authentication data. For page 0, the size is only 17 bytes. For other pages, the size is 23 bytes. Shall be filled with nulls in the unused portion of the field.*/ +} mavlink_open_drone_id_authentication_t; + +#define MAVLINK_MSG_ID_OPEN_DRONE_ID_AUTHENTICATION_LEN 53 +#define MAVLINK_MSG_ID_OPEN_DRONE_ID_AUTHENTICATION_MIN_LEN 53 +#define MAVLINK_MSG_ID_12902_LEN 53 +#define MAVLINK_MSG_ID_12902_MIN_LEN 53 + +#define MAVLINK_MSG_ID_OPEN_DRONE_ID_AUTHENTICATION_CRC 49 +#define MAVLINK_MSG_ID_12902_CRC 49 + +#define MAVLINK_MSG_OPEN_DRONE_ID_AUTHENTICATION_FIELD_ID_OR_MAC_LEN 20 +#define MAVLINK_MSG_OPEN_DRONE_ID_AUTHENTICATION_FIELD_AUTHENTICATION_DATA_LEN 23 + +#if MAVLINK_COMMAND_24BIT +#define MAVLINK_MESSAGE_INFO_OPEN_DRONE_ID_AUTHENTICATION { \ + 12902, \ + "OPEN_DRONE_ID_AUTHENTICATION", \ + 9, \ + { { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 4, offsetof(mavlink_open_drone_id_authentication_t, target_system) }, \ + { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 5, offsetof(mavlink_open_drone_id_authentication_t, target_component) }, \ + { "id_or_mac", NULL, MAVLINK_TYPE_UINT8_T, 20, 6, offsetof(mavlink_open_drone_id_authentication_t, id_or_mac) }, \ + { "authentication_type", NULL, MAVLINK_TYPE_UINT8_T, 0, 26, offsetof(mavlink_open_drone_id_authentication_t, authentication_type) }, \ + { "data_page", NULL, MAVLINK_TYPE_UINT8_T, 0, 27, offsetof(mavlink_open_drone_id_authentication_t, data_page) }, \ + { "page_count", NULL, MAVLINK_TYPE_UINT8_T, 0, 28, offsetof(mavlink_open_drone_id_authentication_t, page_count) }, \ + { "length", NULL, MAVLINK_TYPE_UINT8_T, 0, 29, offsetof(mavlink_open_drone_id_authentication_t, length) }, \ + { "timestamp", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_open_drone_id_authentication_t, timestamp) }, \ + { "authentication_data", NULL, MAVLINK_TYPE_UINT8_T, 23, 30, offsetof(mavlink_open_drone_id_authentication_t, authentication_data) }, \ + } \ +} +#else +#define MAVLINK_MESSAGE_INFO_OPEN_DRONE_ID_AUTHENTICATION { \ + "OPEN_DRONE_ID_AUTHENTICATION", \ + 9, \ + { { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 4, offsetof(mavlink_open_drone_id_authentication_t, target_system) }, \ + { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 5, offsetof(mavlink_open_drone_id_authentication_t, target_component) }, \ + { "id_or_mac", NULL, MAVLINK_TYPE_UINT8_T, 20, 6, offsetof(mavlink_open_drone_id_authentication_t, id_or_mac) }, \ + { "authentication_type", NULL, MAVLINK_TYPE_UINT8_T, 0, 26, offsetof(mavlink_open_drone_id_authentication_t, authentication_type) }, \ + { "data_page", NULL, MAVLINK_TYPE_UINT8_T, 0, 27, offsetof(mavlink_open_drone_id_authentication_t, data_page) }, \ + { "page_count", NULL, MAVLINK_TYPE_UINT8_T, 0, 28, offsetof(mavlink_open_drone_id_authentication_t, page_count) }, \ + { "length", NULL, MAVLINK_TYPE_UINT8_T, 0, 29, offsetof(mavlink_open_drone_id_authentication_t, length) }, \ + { "timestamp", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_open_drone_id_authentication_t, timestamp) }, \ + { "authentication_data", NULL, MAVLINK_TYPE_UINT8_T, 23, 30, offsetof(mavlink_open_drone_id_authentication_t, authentication_data) }, \ + } \ +} +#endif + +/** + * @brief Pack a open_drone_id_authentication message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param target_system System ID (0 for broadcast). + * @param target_component Component ID (0 for broadcast). + * @param id_or_mac Only used for drone ID data received from other UAs. See detailed description at https://mavlink.io/en/services/opendroneid.html. + * @param authentication_type Indicates the type of authentication. + * @param data_page Allowed range is 0 - 4. + * @param page_count This field is only present for page 0. Allowed range is 0 - 5. + * @param length [bytes] This field is only present for page 0. Total bytes of authentication_data from all data pages. Allowed range is 0 - 109 (17 + 23*4). + * @param timestamp [s] This field is only present for page 0. 32 bit Unix Timestamp in seconds since 00:00:00 01/01/2019. + * @param authentication_data Opaque authentication data. For page 0, the size is only 17 bytes. For other pages, the size is 23 bytes. Shall be filled with nulls in the unused portion of the field. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_open_drone_id_authentication_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint8_t target_system, uint8_t target_component, const uint8_t *id_or_mac, uint8_t authentication_type, uint8_t data_page, uint8_t page_count, uint8_t length, uint32_t timestamp, const uint8_t *authentication_data) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_OPEN_DRONE_ID_AUTHENTICATION_LEN]; + _mav_put_uint32_t(buf, 0, timestamp); + _mav_put_uint8_t(buf, 4, target_system); + _mav_put_uint8_t(buf, 5, target_component); + _mav_put_uint8_t(buf, 26, authentication_type); + _mav_put_uint8_t(buf, 27, data_page); + _mav_put_uint8_t(buf, 28, page_count); + _mav_put_uint8_t(buf, 29, length); + _mav_put_uint8_t_array(buf, 6, id_or_mac, 20); + _mav_put_uint8_t_array(buf, 30, authentication_data, 23); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_OPEN_DRONE_ID_AUTHENTICATION_LEN); +#else + mavlink_open_drone_id_authentication_t packet; + packet.timestamp = timestamp; + packet.target_system = target_system; + packet.target_component = target_component; + packet.authentication_type = authentication_type; + packet.data_page = data_page; + packet.page_count = page_count; + packet.length = length; + mav_array_memcpy(packet.id_or_mac, id_or_mac, sizeof(uint8_t)*20); + mav_array_memcpy(packet.authentication_data, authentication_data, sizeof(uint8_t)*23); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_OPEN_DRONE_ID_AUTHENTICATION_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_OPEN_DRONE_ID_AUTHENTICATION; + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_OPEN_DRONE_ID_AUTHENTICATION_MIN_LEN, MAVLINK_MSG_ID_OPEN_DRONE_ID_AUTHENTICATION_LEN, MAVLINK_MSG_ID_OPEN_DRONE_ID_AUTHENTICATION_CRC); +} + +/** + * @brief Pack a open_drone_id_authentication message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param target_system System ID (0 for broadcast). + * @param target_component Component ID (0 for broadcast). + * @param id_or_mac Only used for drone ID data received from other UAs. See detailed description at https://mavlink.io/en/services/opendroneid.html. + * @param authentication_type Indicates the type of authentication. + * @param data_page Allowed range is 0 - 4. + * @param page_count This field is only present for page 0. Allowed range is 0 - 5. + * @param length [bytes] This field is only present for page 0. Total bytes of authentication_data from all data pages. Allowed range is 0 - 109 (17 + 23*4). + * @param timestamp [s] This field is only present for page 0. 32 bit Unix Timestamp in seconds since 00:00:00 01/01/2019. + * @param authentication_data Opaque authentication data. For page 0, the size is only 17 bytes. For other pages, the size is 23 bytes. Shall be filled with nulls in the unused portion of the field. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_open_drone_id_authentication_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint8_t target_system,uint8_t target_component,const uint8_t *id_or_mac,uint8_t authentication_type,uint8_t data_page,uint8_t page_count,uint8_t length,uint32_t timestamp,const uint8_t *authentication_data) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_OPEN_DRONE_ID_AUTHENTICATION_LEN]; + _mav_put_uint32_t(buf, 0, timestamp); + _mav_put_uint8_t(buf, 4, target_system); + _mav_put_uint8_t(buf, 5, target_component); + _mav_put_uint8_t(buf, 26, authentication_type); + _mav_put_uint8_t(buf, 27, data_page); + _mav_put_uint8_t(buf, 28, page_count); + _mav_put_uint8_t(buf, 29, length); + _mav_put_uint8_t_array(buf, 6, id_or_mac, 20); + _mav_put_uint8_t_array(buf, 30, authentication_data, 23); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_OPEN_DRONE_ID_AUTHENTICATION_LEN); +#else + mavlink_open_drone_id_authentication_t packet; + packet.timestamp = timestamp; + packet.target_system = target_system; + packet.target_component = target_component; + packet.authentication_type = authentication_type; + packet.data_page = data_page; + packet.page_count = page_count; + packet.length = length; + mav_array_memcpy(packet.id_or_mac, id_or_mac, sizeof(uint8_t)*20); + mav_array_memcpy(packet.authentication_data, authentication_data, sizeof(uint8_t)*23); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_OPEN_DRONE_ID_AUTHENTICATION_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_OPEN_DRONE_ID_AUTHENTICATION; + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_OPEN_DRONE_ID_AUTHENTICATION_MIN_LEN, MAVLINK_MSG_ID_OPEN_DRONE_ID_AUTHENTICATION_LEN, MAVLINK_MSG_ID_OPEN_DRONE_ID_AUTHENTICATION_CRC); +} + +/** + * @brief Encode a open_drone_id_authentication struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param open_drone_id_authentication C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_open_drone_id_authentication_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_open_drone_id_authentication_t* open_drone_id_authentication) +{ + return mavlink_msg_open_drone_id_authentication_pack(system_id, component_id, msg, open_drone_id_authentication->target_system, open_drone_id_authentication->target_component, open_drone_id_authentication->id_or_mac, open_drone_id_authentication->authentication_type, open_drone_id_authentication->data_page, open_drone_id_authentication->page_count, open_drone_id_authentication->length, open_drone_id_authentication->timestamp, open_drone_id_authentication->authentication_data); +} + +/** + * @brief Encode a open_drone_id_authentication struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param open_drone_id_authentication C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_open_drone_id_authentication_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_open_drone_id_authentication_t* open_drone_id_authentication) +{ + return mavlink_msg_open_drone_id_authentication_pack_chan(system_id, component_id, chan, msg, open_drone_id_authentication->target_system, open_drone_id_authentication->target_component, open_drone_id_authentication->id_or_mac, open_drone_id_authentication->authentication_type, open_drone_id_authentication->data_page, open_drone_id_authentication->page_count, open_drone_id_authentication->length, open_drone_id_authentication->timestamp, open_drone_id_authentication->authentication_data); +} + +/** + * @brief Send a open_drone_id_authentication message + * @param chan MAVLink channel to send the message + * + * @param target_system System ID (0 for broadcast). + * @param target_component Component ID (0 for broadcast). + * @param id_or_mac Only used for drone ID data received from other UAs. See detailed description at https://mavlink.io/en/services/opendroneid.html. + * @param authentication_type Indicates the type of authentication. + * @param data_page Allowed range is 0 - 4. + * @param page_count This field is only present for page 0. Allowed range is 0 - 5. + * @param length [bytes] This field is only present for page 0. Total bytes of authentication_data from all data pages. Allowed range is 0 - 109 (17 + 23*4). + * @param timestamp [s] This field is only present for page 0. 32 bit Unix Timestamp in seconds since 00:00:00 01/01/2019. + * @param authentication_data Opaque authentication data. For page 0, the size is only 17 bytes. For other pages, the size is 23 bytes. Shall be filled with nulls in the unused portion of the field. + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_open_drone_id_authentication_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, const uint8_t *id_or_mac, uint8_t authentication_type, uint8_t data_page, uint8_t page_count, uint8_t length, uint32_t timestamp, const uint8_t *authentication_data) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_OPEN_DRONE_ID_AUTHENTICATION_LEN]; + _mav_put_uint32_t(buf, 0, timestamp); + _mav_put_uint8_t(buf, 4, target_system); + _mav_put_uint8_t(buf, 5, target_component); + _mav_put_uint8_t(buf, 26, authentication_type); + _mav_put_uint8_t(buf, 27, data_page); + _mav_put_uint8_t(buf, 28, page_count); + _mav_put_uint8_t(buf, 29, length); + _mav_put_uint8_t_array(buf, 6, id_or_mac, 20); + _mav_put_uint8_t_array(buf, 30, authentication_data, 23); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OPEN_DRONE_ID_AUTHENTICATION, buf, MAVLINK_MSG_ID_OPEN_DRONE_ID_AUTHENTICATION_MIN_LEN, MAVLINK_MSG_ID_OPEN_DRONE_ID_AUTHENTICATION_LEN, MAVLINK_MSG_ID_OPEN_DRONE_ID_AUTHENTICATION_CRC); +#else + mavlink_open_drone_id_authentication_t packet; + packet.timestamp = timestamp; + packet.target_system = target_system; + packet.target_component = target_component; + packet.authentication_type = authentication_type; + packet.data_page = data_page; + packet.page_count = page_count; + packet.length = length; + mav_array_memcpy(packet.id_or_mac, id_or_mac, sizeof(uint8_t)*20); + mav_array_memcpy(packet.authentication_data, authentication_data, sizeof(uint8_t)*23); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OPEN_DRONE_ID_AUTHENTICATION, (const char *)&packet, MAVLINK_MSG_ID_OPEN_DRONE_ID_AUTHENTICATION_MIN_LEN, MAVLINK_MSG_ID_OPEN_DRONE_ID_AUTHENTICATION_LEN, MAVLINK_MSG_ID_OPEN_DRONE_ID_AUTHENTICATION_CRC); +#endif +} + +/** + * @brief Send a open_drone_id_authentication message + * @param chan MAVLink channel to send the message + * @param struct The MAVLink struct to serialize + */ +static inline void mavlink_msg_open_drone_id_authentication_send_struct(mavlink_channel_t chan, const mavlink_open_drone_id_authentication_t* open_drone_id_authentication) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + mavlink_msg_open_drone_id_authentication_send(chan, open_drone_id_authentication->target_system, open_drone_id_authentication->target_component, open_drone_id_authentication->id_or_mac, open_drone_id_authentication->authentication_type, open_drone_id_authentication->data_page, open_drone_id_authentication->page_count, open_drone_id_authentication->length, open_drone_id_authentication->timestamp, open_drone_id_authentication->authentication_data); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OPEN_DRONE_ID_AUTHENTICATION, (const char *)open_drone_id_authentication, MAVLINK_MSG_ID_OPEN_DRONE_ID_AUTHENTICATION_MIN_LEN, MAVLINK_MSG_ID_OPEN_DRONE_ID_AUTHENTICATION_LEN, MAVLINK_MSG_ID_OPEN_DRONE_ID_AUTHENTICATION_CRC); +#endif +} + +#if MAVLINK_MSG_ID_OPEN_DRONE_ID_AUTHENTICATION_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_open_drone_id_authentication_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, const uint8_t *id_or_mac, uint8_t authentication_type, uint8_t data_page, uint8_t page_count, uint8_t length, uint32_t timestamp, const uint8_t *authentication_data) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint32_t(buf, 0, timestamp); + _mav_put_uint8_t(buf, 4, target_system); + _mav_put_uint8_t(buf, 5, target_component); + _mav_put_uint8_t(buf, 26, authentication_type); + _mav_put_uint8_t(buf, 27, data_page); + _mav_put_uint8_t(buf, 28, page_count); + _mav_put_uint8_t(buf, 29, length); + _mav_put_uint8_t_array(buf, 6, id_or_mac, 20); + _mav_put_uint8_t_array(buf, 30, authentication_data, 23); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OPEN_DRONE_ID_AUTHENTICATION, buf, MAVLINK_MSG_ID_OPEN_DRONE_ID_AUTHENTICATION_MIN_LEN, MAVLINK_MSG_ID_OPEN_DRONE_ID_AUTHENTICATION_LEN, MAVLINK_MSG_ID_OPEN_DRONE_ID_AUTHENTICATION_CRC); +#else + mavlink_open_drone_id_authentication_t *packet = (mavlink_open_drone_id_authentication_t *)msgbuf; + packet->timestamp = timestamp; + packet->target_system = target_system; + packet->target_component = target_component; + packet->authentication_type = authentication_type; + packet->data_page = data_page; + packet->page_count = page_count; + packet->length = length; + mav_array_memcpy(packet->id_or_mac, id_or_mac, sizeof(uint8_t)*20); + mav_array_memcpy(packet->authentication_data, authentication_data, sizeof(uint8_t)*23); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OPEN_DRONE_ID_AUTHENTICATION, (const char *)packet, MAVLINK_MSG_ID_OPEN_DRONE_ID_AUTHENTICATION_MIN_LEN, MAVLINK_MSG_ID_OPEN_DRONE_ID_AUTHENTICATION_LEN, MAVLINK_MSG_ID_OPEN_DRONE_ID_AUTHENTICATION_CRC); +#endif +} +#endif + +#endif + +// MESSAGE OPEN_DRONE_ID_AUTHENTICATION UNPACKING + + +/** + * @brief Get field target_system from open_drone_id_authentication message + * + * @return System ID (0 for broadcast). + */ +static inline uint8_t mavlink_msg_open_drone_id_authentication_get_target_system(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 4); +} + +/** + * @brief Get field target_component from open_drone_id_authentication message + * + * @return Component ID (0 for broadcast). + */ +static inline uint8_t mavlink_msg_open_drone_id_authentication_get_target_component(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 5); +} + +/** + * @brief Get field id_or_mac from open_drone_id_authentication message + * + * @return Only used for drone ID data received from other UAs. See detailed description at https://mavlink.io/en/services/opendroneid.html. + */ +static inline uint16_t mavlink_msg_open_drone_id_authentication_get_id_or_mac(const mavlink_message_t* msg, uint8_t *id_or_mac) +{ + return _MAV_RETURN_uint8_t_array(msg, id_or_mac, 20, 6); +} + +/** + * @brief Get field authentication_type from open_drone_id_authentication message + * + * @return Indicates the type of authentication. + */ +static inline uint8_t mavlink_msg_open_drone_id_authentication_get_authentication_type(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 26); +} + +/** + * @brief Get field data_page from open_drone_id_authentication message + * + * @return Allowed range is 0 - 4. + */ +static inline uint8_t mavlink_msg_open_drone_id_authentication_get_data_page(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 27); +} + +/** + * @brief Get field page_count from open_drone_id_authentication message + * + * @return This field is only present for page 0. Allowed range is 0 - 5. + */ +static inline uint8_t mavlink_msg_open_drone_id_authentication_get_page_count(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 28); +} + +/** + * @brief Get field length from open_drone_id_authentication message + * + * @return [bytes] This field is only present for page 0. Total bytes of authentication_data from all data pages. Allowed range is 0 - 109 (17 + 23*4). + */ +static inline uint8_t mavlink_msg_open_drone_id_authentication_get_length(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 29); +} + +/** + * @brief Get field timestamp from open_drone_id_authentication message + * + * @return [s] This field is only present for page 0. 32 bit Unix Timestamp in seconds since 00:00:00 01/01/2019. + */ +static inline uint32_t mavlink_msg_open_drone_id_authentication_get_timestamp(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint32_t(msg, 0); +} + +/** + * @brief Get field authentication_data from open_drone_id_authentication message + * + * @return Opaque authentication data. For page 0, the size is only 17 bytes. For other pages, the size is 23 bytes. Shall be filled with nulls in the unused portion of the field. + */ +static inline uint16_t mavlink_msg_open_drone_id_authentication_get_authentication_data(const mavlink_message_t* msg, uint8_t *authentication_data) +{ + return _MAV_RETURN_uint8_t_array(msg, authentication_data, 23, 30); +} + +/** + * @brief Decode a open_drone_id_authentication message into a struct + * + * @param msg The message to decode + * @param open_drone_id_authentication C-struct to decode the message contents into + */ +static inline void mavlink_msg_open_drone_id_authentication_decode(const mavlink_message_t* msg, mavlink_open_drone_id_authentication_t* open_drone_id_authentication) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + open_drone_id_authentication->timestamp = mavlink_msg_open_drone_id_authentication_get_timestamp(msg); + open_drone_id_authentication->target_system = mavlink_msg_open_drone_id_authentication_get_target_system(msg); + open_drone_id_authentication->target_component = mavlink_msg_open_drone_id_authentication_get_target_component(msg); + mavlink_msg_open_drone_id_authentication_get_id_or_mac(msg, open_drone_id_authentication->id_or_mac); + open_drone_id_authentication->authentication_type = mavlink_msg_open_drone_id_authentication_get_authentication_type(msg); + open_drone_id_authentication->data_page = mavlink_msg_open_drone_id_authentication_get_data_page(msg); + open_drone_id_authentication->page_count = mavlink_msg_open_drone_id_authentication_get_page_count(msg); + open_drone_id_authentication->length = mavlink_msg_open_drone_id_authentication_get_length(msg); + mavlink_msg_open_drone_id_authentication_get_authentication_data(msg, open_drone_id_authentication->authentication_data); +#else + uint8_t len = msg->len < MAVLINK_MSG_ID_OPEN_DRONE_ID_AUTHENTICATION_LEN? msg->len : MAVLINK_MSG_ID_OPEN_DRONE_ID_AUTHENTICATION_LEN; + memset(open_drone_id_authentication, 0, MAVLINK_MSG_ID_OPEN_DRONE_ID_AUTHENTICATION_LEN); + memcpy(open_drone_id_authentication, _MAV_PAYLOAD(msg), len); +#endif +} diff --git a/lib/main/MAVLink/common/mavlink_msg_open_drone_id_basic_id.h b/lib/main/MAVLink/common/mavlink_msg_open_drone_id_basic_id.h new file mode 100644 index 0000000000..1102960b71 --- /dev/null +++ b/lib/main/MAVLink/common/mavlink_msg_open_drone_id_basic_id.h @@ -0,0 +1,331 @@ +#pragma once +// MESSAGE OPEN_DRONE_ID_BASIC_ID PACKING + +#define MAVLINK_MSG_ID_OPEN_DRONE_ID_BASIC_ID 12900 + + +typedef struct __mavlink_open_drone_id_basic_id_t { + uint8_t target_system; /*< System ID (0 for broadcast).*/ + uint8_t target_component; /*< Component ID (0 for broadcast).*/ + uint8_t id_or_mac[20]; /*< Only used for drone ID data received from other UAs. See detailed description at https://mavlink.io/en/services/opendroneid.html. */ + uint8_t id_type; /*< Indicates the format for the uas_id field of this message.*/ + uint8_t ua_type; /*< Indicates the type of UA (Unmanned Aircraft).*/ + uint8_t uas_id[20]; /*< UAS (Unmanned Aircraft System) ID following the format specified by id_type. Shall be filled with nulls in the unused portion of the field.*/ +} mavlink_open_drone_id_basic_id_t; + +#define MAVLINK_MSG_ID_OPEN_DRONE_ID_BASIC_ID_LEN 44 +#define MAVLINK_MSG_ID_OPEN_DRONE_ID_BASIC_ID_MIN_LEN 44 +#define MAVLINK_MSG_ID_12900_LEN 44 +#define MAVLINK_MSG_ID_12900_MIN_LEN 44 + +#define MAVLINK_MSG_ID_OPEN_DRONE_ID_BASIC_ID_CRC 114 +#define MAVLINK_MSG_ID_12900_CRC 114 + +#define MAVLINK_MSG_OPEN_DRONE_ID_BASIC_ID_FIELD_ID_OR_MAC_LEN 20 +#define MAVLINK_MSG_OPEN_DRONE_ID_BASIC_ID_FIELD_UAS_ID_LEN 20 + +#if MAVLINK_COMMAND_24BIT +#define MAVLINK_MESSAGE_INFO_OPEN_DRONE_ID_BASIC_ID { \ + 12900, \ + "OPEN_DRONE_ID_BASIC_ID", \ + 6, \ + { { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 0, offsetof(mavlink_open_drone_id_basic_id_t, target_system) }, \ + { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 1, offsetof(mavlink_open_drone_id_basic_id_t, target_component) }, \ + { "id_or_mac", NULL, MAVLINK_TYPE_UINT8_T, 20, 2, offsetof(mavlink_open_drone_id_basic_id_t, id_or_mac) }, \ + { "id_type", NULL, MAVLINK_TYPE_UINT8_T, 0, 22, offsetof(mavlink_open_drone_id_basic_id_t, id_type) }, \ + { "ua_type", NULL, MAVLINK_TYPE_UINT8_T, 0, 23, offsetof(mavlink_open_drone_id_basic_id_t, ua_type) }, \ + { "uas_id", NULL, MAVLINK_TYPE_UINT8_T, 20, 24, offsetof(mavlink_open_drone_id_basic_id_t, uas_id) }, \ + } \ +} +#else +#define MAVLINK_MESSAGE_INFO_OPEN_DRONE_ID_BASIC_ID { \ + "OPEN_DRONE_ID_BASIC_ID", \ + 6, \ + { { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 0, offsetof(mavlink_open_drone_id_basic_id_t, target_system) }, \ + { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 1, offsetof(mavlink_open_drone_id_basic_id_t, target_component) }, \ + { "id_or_mac", NULL, MAVLINK_TYPE_UINT8_T, 20, 2, offsetof(mavlink_open_drone_id_basic_id_t, id_or_mac) }, \ + { "id_type", NULL, MAVLINK_TYPE_UINT8_T, 0, 22, offsetof(mavlink_open_drone_id_basic_id_t, id_type) }, \ + { "ua_type", NULL, MAVLINK_TYPE_UINT8_T, 0, 23, offsetof(mavlink_open_drone_id_basic_id_t, ua_type) }, \ + { "uas_id", NULL, MAVLINK_TYPE_UINT8_T, 20, 24, offsetof(mavlink_open_drone_id_basic_id_t, uas_id) }, \ + } \ +} +#endif + +/** + * @brief Pack a open_drone_id_basic_id message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param target_system System ID (0 for broadcast). + * @param target_component Component ID (0 for broadcast). + * @param id_or_mac Only used for drone ID data received from other UAs. See detailed description at https://mavlink.io/en/services/opendroneid.html. + * @param id_type Indicates the format for the uas_id field of this message. + * @param ua_type Indicates the type of UA (Unmanned Aircraft). + * @param uas_id UAS (Unmanned Aircraft System) ID following the format specified by id_type. Shall be filled with nulls in the unused portion of the field. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_open_drone_id_basic_id_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint8_t target_system, uint8_t target_component, const uint8_t *id_or_mac, uint8_t id_type, uint8_t ua_type, const uint8_t *uas_id) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_OPEN_DRONE_ID_BASIC_ID_LEN]; + _mav_put_uint8_t(buf, 0, target_system); + _mav_put_uint8_t(buf, 1, target_component); + _mav_put_uint8_t(buf, 22, id_type); + _mav_put_uint8_t(buf, 23, ua_type); + _mav_put_uint8_t_array(buf, 2, id_or_mac, 20); + _mav_put_uint8_t_array(buf, 24, uas_id, 20); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_OPEN_DRONE_ID_BASIC_ID_LEN); +#else + mavlink_open_drone_id_basic_id_t packet; + packet.target_system = target_system; + packet.target_component = target_component; + packet.id_type = id_type; + packet.ua_type = ua_type; + mav_array_memcpy(packet.id_or_mac, id_or_mac, sizeof(uint8_t)*20); + mav_array_memcpy(packet.uas_id, uas_id, sizeof(uint8_t)*20); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_OPEN_DRONE_ID_BASIC_ID_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_OPEN_DRONE_ID_BASIC_ID; + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_OPEN_DRONE_ID_BASIC_ID_MIN_LEN, MAVLINK_MSG_ID_OPEN_DRONE_ID_BASIC_ID_LEN, MAVLINK_MSG_ID_OPEN_DRONE_ID_BASIC_ID_CRC); +} + +/** + * @brief Pack a open_drone_id_basic_id message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param target_system System ID (0 for broadcast). + * @param target_component Component ID (0 for broadcast). + * @param id_or_mac Only used for drone ID data received from other UAs. See detailed description at https://mavlink.io/en/services/opendroneid.html. + * @param id_type Indicates the format for the uas_id field of this message. + * @param ua_type Indicates the type of UA (Unmanned Aircraft). + * @param uas_id UAS (Unmanned Aircraft System) ID following the format specified by id_type. Shall be filled with nulls in the unused portion of the field. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_open_drone_id_basic_id_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint8_t target_system,uint8_t target_component,const uint8_t *id_or_mac,uint8_t id_type,uint8_t ua_type,const uint8_t *uas_id) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_OPEN_DRONE_ID_BASIC_ID_LEN]; + _mav_put_uint8_t(buf, 0, target_system); + _mav_put_uint8_t(buf, 1, target_component); + _mav_put_uint8_t(buf, 22, id_type); + _mav_put_uint8_t(buf, 23, ua_type); + _mav_put_uint8_t_array(buf, 2, id_or_mac, 20); + _mav_put_uint8_t_array(buf, 24, uas_id, 20); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_OPEN_DRONE_ID_BASIC_ID_LEN); +#else + mavlink_open_drone_id_basic_id_t packet; + packet.target_system = target_system; + packet.target_component = target_component; + packet.id_type = id_type; + packet.ua_type = ua_type; + mav_array_memcpy(packet.id_or_mac, id_or_mac, sizeof(uint8_t)*20); + mav_array_memcpy(packet.uas_id, uas_id, sizeof(uint8_t)*20); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_OPEN_DRONE_ID_BASIC_ID_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_OPEN_DRONE_ID_BASIC_ID; + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_OPEN_DRONE_ID_BASIC_ID_MIN_LEN, MAVLINK_MSG_ID_OPEN_DRONE_ID_BASIC_ID_LEN, MAVLINK_MSG_ID_OPEN_DRONE_ID_BASIC_ID_CRC); +} + +/** + * @brief Encode a open_drone_id_basic_id struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param open_drone_id_basic_id C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_open_drone_id_basic_id_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_open_drone_id_basic_id_t* open_drone_id_basic_id) +{ + return mavlink_msg_open_drone_id_basic_id_pack(system_id, component_id, msg, open_drone_id_basic_id->target_system, open_drone_id_basic_id->target_component, open_drone_id_basic_id->id_or_mac, open_drone_id_basic_id->id_type, open_drone_id_basic_id->ua_type, open_drone_id_basic_id->uas_id); +} + +/** + * @brief Encode a open_drone_id_basic_id struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param open_drone_id_basic_id C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_open_drone_id_basic_id_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_open_drone_id_basic_id_t* open_drone_id_basic_id) +{ + return mavlink_msg_open_drone_id_basic_id_pack_chan(system_id, component_id, chan, msg, open_drone_id_basic_id->target_system, open_drone_id_basic_id->target_component, open_drone_id_basic_id->id_or_mac, open_drone_id_basic_id->id_type, open_drone_id_basic_id->ua_type, open_drone_id_basic_id->uas_id); +} + +/** + * @brief Send a open_drone_id_basic_id message + * @param chan MAVLink channel to send the message + * + * @param target_system System ID (0 for broadcast). + * @param target_component Component ID (0 for broadcast). + * @param id_or_mac Only used for drone ID data received from other UAs. See detailed description at https://mavlink.io/en/services/opendroneid.html. + * @param id_type Indicates the format for the uas_id field of this message. + * @param ua_type Indicates the type of UA (Unmanned Aircraft). + * @param uas_id UAS (Unmanned Aircraft System) ID following the format specified by id_type. Shall be filled with nulls in the unused portion of the field. + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_open_drone_id_basic_id_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, const uint8_t *id_or_mac, uint8_t id_type, uint8_t ua_type, const uint8_t *uas_id) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_OPEN_DRONE_ID_BASIC_ID_LEN]; + _mav_put_uint8_t(buf, 0, target_system); + _mav_put_uint8_t(buf, 1, target_component); + _mav_put_uint8_t(buf, 22, id_type); + _mav_put_uint8_t(buf, 23, ua_type); + _mav_put_uint8_t_array(buf, 2, id_or_mac, 20); + _mav_put_uint8_t_array(buf, 24, uas_id, 20); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OPEN_DRONE_ID_BASIC_ID, buf, MAVLINK_MSG_ID_OPEN_DRONE_ID_BASIC_ID_MIN_LEN, MAVLINK_MSG_ID_OPEN_DRONE_ID_BASIC_ID_LEN, MAVLINK_MSG_ID_OPEN_DRONE_ID_BASIC_ID_CRC); +#else + mavlink_open_drone_id_basic_id_t packet; + packet.target_system = target_system; + packet.target_component = target_component; + packet.id_type = id_type; + packet.ua_type = ua_type; + mav_array_memcpy(packet.id_or_mac, id_or_mac, sizeof(uint8_t)*20); + mav_array_memcpy(packet.uas_id, uas_id, sizeof(uint8_t)*20); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OPEN_DRONE_ID_BASIC_ID, (const char *)&packet, MAVLINK_MSG_ID_OPEN_DRONE_ID_BASIC_ID_MIN_LEN, MAVLINK_MSG_ID_OPEN_DRONE_ID_BASIC_ID_LEN, MAVLINK_MSG_ID_OPEN_DRONE_ID_BASIC_ID_CRC); +#endif +} + +/** + * @brief Send a open_drone_id_basic_id message + * @param chan MAVLink channel to send the message + * @param struct The MAVLink struct to serialize + */ +static inline void mavlink_msg_open_drone_id_basic_id_send_struct(mavlink_channel_t chan, const mavlink_open_drone_id_basic_id_t* open_drone_id_basic_id) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + mavlink_msg_open_drone_id_basic_id_send(chan, open_drone_id_basic_id->target_system, open_drone_id_basic_id->target_component, open_drone_id_basic_id->id_or_mac, open_drone_id_basic_id->id_type, open_drone_id_basic_id->ua_type, open_drone_id_basic_id->uas_id); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OPEN_DRONE_ID_BASIC_ID, (const char *)open_drone_id_basic_id, MAVLINK_MSG_ID_OPEN_DRONE_ID_BASIC_ID_MIN_LEN, MAVLINK_MSG_ID_OPEN_DRONE_ID_BASIC_ID_LEN, MAVLINK_MSG_ID_OPEN_DRONE_ID_BASIC_ID_CRC); +#endif +} + +#if MAVLINK_MSG_ID_OPEN_DRONE_ID_BASIC_ID_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_open_drone_id_basic_id_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, const uint8_t *id_or_mac, uint8_t id_type, uint8_t ua_type, const uint8_t *uas_id) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint8_t(buf, 0, target_system); + _mav_put_uint8_t(buf, 1, target_component); + _mav_put_uint8_t(buf, 22, id_type); + _mav_put_uint8_t(buf, 23, ua_type); + _mav_put_uint8_t_array(buf, 2, id_or_mac, 20); + _mav_put_uint8_t_array(buf, 24, uas_id, 20); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OPEN_DRONE_ID_BASIC_ID, buf, MAVLINK_MSG_ID_OPEN_DRONE_ID_BASIC_ID_MIN_LEN, MAVLINK_MSG_ID_OPEN_DRONE_ID_BASIC_ID_LEN, MAVLINK_MSG_ID_OPEN_DRONE_ID_BASIC_ID_CRC); +#else + mavlink_open_drone_id_basic_id_t *packet = (mavlink_open_drone_id_basic_id_t *)msgbuf; + packet->target_system = target_system; + packet->target_component = target_component; + packet->id_type = id_type; + packet->ua_type = ua_type; + mav_array_memcpy(packet->id_or_mac, id_or_mac, sizeof(uint8_t)*20); + mav_array_memcpy(packet->uas_id, uas_id, sizeof(uint8_t)*20); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OPEN_DRONE_ID_BASIC_ID, (const char *)packet, MAVLINK_MSG_ID_OPEN_DRONE_ID_BASIC_ID_MIN_LEN, MAVLINK_MSG_ID_OPEN_DRONE_ID_BASIC_ID_LEN, MAVLINK_MSG_ID_OPEN_DRONE_ID_BASIC_ID_CRC); +#endif +} +#endif + +#endif + +// MESSAGE OPEN_DRONE_ID_BASIC_ID UNPACKING + + +/** + * @brief Get field target_system from open_drone_id_basic_id message + * + * @return System ID (0 for broadcast). + */ +static inline uint8_t mavlink_msg_open_drone_id_basic_id_get_target_system(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 0); +} + +/** + * @brief Get field target_component from open_drone_id_basic_id message + * + * @return Component ID (0 for broadcast). + */ +static inline uint8_t mavlink_msg_open_drone_id_basic_id_get_target_component(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 1); +} + +/** + * @brief Get field id_or_mac from open_drone_id_basic_id message + * + * @return Only used for drone ID data received from other UAs. See detailed description at https://mavlink.io/en/services/opendroneid.html. + */ +static inline uint16_t mavlink_msg_open_drone_id_basic_id_get_id_or_mac(const mavlink_message_t* msg, uint8_t *id_or_mac) +{ + return _MAV_RETURN_uint8_t_array(msg, id_or_mac, 20, 2); +} + +/** + * @brief Get field id_type from open_drone_id_basic_id message + * + * @return Indicates the format for the uas_id field of this message. + */ +static inline uint8_t mavlink_msg_open_drone_id_basic_id_get_id_type(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 22); +} + +/** + * @brief Get field ua_type from open_drone_id_basic_id message + * + * @return Indicates the type of UA (Unmanned Aircraft). + */ +static inline uint8_t mavlink_msg_open_drone_id_basic_id_get_ua_type(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 23); +} + +/** + * @brief Get field uas_id from open_drone_id_basic_id message + * + * @return UAS (Unmanned Aircraft System) ID following the format specified by id_type. Shall be filled with nulls in the unused portion of the field. + */ +static inline uint16_t mavlink_msg_open_drone_id_basic_id_get_uas_id(const mavlink_message_t* msg, uint8_t *uas_id) +{ + return _MAV_RETURN_uint8_t_array(msg, uas_id, 20, 24); +} + +/** + * @brief Decode a open_drone_id_basic_id message into a struct + * + * @param msg The message to decode + * @param open_drone_id_basic_id C-struct to decode the message contents into + */ +static inline void mavlink_msg_open_drone_id_basic_id_decode(const mavlink_message_t* msg, mavlink_open_drone_id_basic_id_t* open_drone_id_basic_id) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + open_drone_id_basic_id->target_system = mavlink_msg_open_drone_id_basic_id_get_target_system(msg); + open_drone_id_basic_id->target_component = mavlink_msg_open_drone_id_basic_id_get_target_component(msg); + mavlink_msg_open_drone_id_basic_id_get_id_or_mac(msg, open_drone_id_basic_id->id_or_mac); + open_drone_id_basic_id->id_type = mavlink_msg_open_drone_id_basic_id_get_id_type(msg); + open_drone_id_basic_id->ua_type = mavlink_msg_open_drone_id_basic_id_get_ua_type(msg); + mavlink_msg_open_drone_id_basic_id_get_uas_id(msg, open_drone_id_basic_id->uas_id); +#else + uint8_t len = msg->len < MAVLINK_MSG_ID_OPEN_DRONE_ID_BASIC_ID_LEN? msg->len : MAVLINK_MSG_ID_OPEN_DRONE_ID_BASIC_ID_LEN; + memset(open_drone_id_basic_id, 0, MAVLINK_MSG_ID_OPEN_DRONE_ID_BASIC_ID_LEN); + memcpy(open_drone_id_basic_id, _MAV_PAYLOAD(msg), len); +#endif +} diff --git a/lib/main/MAVLink/common/mavlink_msg_open_drone_id_location.h b/lib/main/MAVLink/common/mavlink_msg_open_drone_id_location.h new file mode 100644 index 0000000000..0c28343933 --- /dev/null +++ b/lib/main/MAVLink/common/mavlink_msg_open_drone_id_location.h @@ -0,0 +1,655 @@ +#pragma once +// MESSAGE OPEN_DRONE_ID_LOCATION PACKING + +#define MAVLINK_MSG_ID_OPEN_DRONE_ID_LOCATION 12901 + + +typedef struct __mavlink_open_drone_id_location_t { + int32_t latitude; /*< [degE7] Current latitude of the unmanned aircraft. If unknown: 0 (both Lat/Lon).*/ + int32_t longitude; /*< [degE7] Current longitude of the unmanned aircraft. If unknown: 0 (both Lat/Lon).*/ + float altitude_barometric; /*< [m] The altitude calculated from the barometric pressue. Reference is against 29.92inHg or 1013.2mb. If unknown: -1000 m.*/ + float altitude_geodetic; /*< [m] The geodetic altitude as defined by WGS84. If unknown: -1000 m.*/ + float height; /*< [m] The current height of the unmanned aircraft above the take-off location or the ground as indicated by height_reference. If unknown: -1000 m.*/ + float timestamp; /*< [s] Seconds after the full hour with reference to UTC time. Typically the GPS outputs a time-of-week value in milliseconds. First convert that to UTC and then convert for this field using ((float) (time_week_ms % (60*60*1000))) / 1000.*/ + uint16_t direction; /*< [cdeg] Direction over ground (not heading, but direction of movement) measured clockwise from true North: 0 - 35999 centi-degrees. If unknown: 36100 centi-degrees.*/ + uint16_t speed_horizontal; /*< [cm/s] Ground speed. Positive only. If unknown: 25500 cm/s. If speed is larger than 25425 cm/s, use 25425 cm/s.*/ + int16_t speed_vertical; /*< [cm/s] The vertical speed. Up is positive. If unknown: 6300 cm/s. If speed is larger than 6200 cm/s, use 6200 cm/s. If lower than -6200 cm/s, use -6200 cm/s.*/ + uint8_t target_system; /*< System ID (0 for broadcast).*/ + uint8_t target_component; /*< Component ID (0 for broadcast).*/ + uint8_t id_or_mac[20]; /*< Only used for drone ID data received from other UAs. See detailed description at https://mavlink.io/en/services/opendroneid.html. */ + uint8_t status; /*< Indicates whether the unmanned aircraft is on the ground or in the air.*/ + uint8_t height_reference; /*< Indicates the reference point for the height field.*/ + uint8_t horizontal_accuracy; /*< The accuracy of the horizontal position.*/ + uint8_t vertical_accuracy; /*< The accuracy of the vertical position.*/ + uint8_t barometer_accuracy; /*< The accuracy of the barometric altitude.*/ + uint8_t speed_accuracy; /*< The accuracy of the horizontal and vertical speed.*/ + uint8_t timestamp_accuracy; /*< The accuracy of the timestamps.*/ +} mavlink_open_drone_id_location_t; + +#define MAVLINK_MSG_ID_OPEN_DRONE_ID_LOCATION_LEN 59 +#define MAVLINK_MSG_ID_OPEN_DRONE_ID_LOCATION_MIN_LEN 59 +#define MAVLINK_MSG_ID_12901_LEN 59 +#define MAVLINK_MSG_ID_12901_MIN_LEN 59 + +#define MAVLINK_MSG_ID_OPEN_DRONE_ID_LOCATION_CRC 254 +#define MAVLINK_MSG_ID_12901_CRC 254 + +#define MAVLINK_MSG_OPEN_DRONE_ID_LOCATION_FIELD_ID_OR_MAC_LEN 20 + +#if MAVLINK_COMMAND_24BIT +#define MAVLINK_MESSAGE_INFO_OPEN_DRONE_ID_LOCATION { \ + 12901, \ + "OPEN_DRONE_ID_LOCATION", \ + 19, \ + { { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 30, offsetof(mavlink_open_drone_id_location_t, target_system) }, \ + { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 31, offsetof(mavlink_open_drone_id_location_t, target_component) }, \ + { "id_or_mac", NULL, MAVLINK_TYPE_UINT8_T, 20, 32, offsetof(mavlink_open_drone_id_location_t, id_or_mac) }, \ + { "status", NULL, MAVLINK_TYPE_UINT8_T, 0, 52, offsetof(mavlink_open_drone_id_location_t, status) }, \ + { "direction", NULL, MAVLINK_TYPE_UINT16_T, 0, 24, offsetof(mavlink_open_drone_id_location_t, direction) }, \ + { "speed_horizontal", NULL, MAVLINK_TYPE_UINT16_T, 0, 26, offsetof(mavlink_open_drone_id_location_t, speed_horizontal) }, \ + { "speed_vertical", NULL, MAVLINK_TYPE_INT16_T, 0, 28, offsetof(mavlink_open_drone_id_location_t, speed_vertical) }, \ + { "latitude", NULL, MAVLINK_TYPE_INT32_T, 0, 0, offsetof(mavlink_open_drone_id_location_t, latitude) }, \ + { "longitude", NULL, MAVLINK_TYPE_INT32_T, 0, 4, offsetof(mavlink_open_drone_id_location_t, longitude) }, \ + { "altitude_barometric", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_open_drone_id_location_t, altitude_barometric) }, \ + { "altitude_geodetic", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_open_drone_id_location_t, altitude_geodetic) }, \ + { "height_reference", NULL, MAVLINK_TYPE_UINT8_T, 0, 53, offsetof(mavlink_open_drone_id_location_t, height_reference) }, \ + { "height", NULL, MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_open_drone_id_location_t, height) }, \ + { "horizontal_accuracy", NULL, MAVLINK_TYPE_UINT8_T, 0, 54, offsetof(mavlink_open_drone_id_location_t, horizontal_accuracy) }, \ + { "vertical_accuracy", NULL, MAVLINK_TYPE_UINT8_T, 0, 55, offsetof(mavlink_open_drone_id_location_t, vertical_accuracy) }, \ + { "barometer_accuracy", NULL, MAVLINK_TYPE_UINT8_T, 0, 56, offsetof(mavlink_open_drone_id_location_t, barometer_accuracy) }, \ + { "speed_accuracy", NULL, MAVLINK_TYPE_UINT8_T, 0, 57, offsetof(mavlink_open_drone_id_location_t, speed_accuracy) }, \ + { "timestamp", NULL, MAVLINK_TYPE_FLOAT, 0, 20, offsetof(mavlink_open_drone_id_location_t, timestamp) }, \ + { "timestamp_accuracy", NULL, MAVLINK_TYPE_UINT8_T, 0, 58, offsetof(mavlink_open_drone_id_location_t, timestamp_accuracy) }, \ + } \ +} +#else +#define MAVLINK_MESSAGE_INFO_OPEN_DRONE_ID_LOCATION { \ + "OPEN_DRONE_ID_LOCATION", \ + 19, \ + { { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 30, offsetof(mavlink_open_drone_id_location_t, target_system) }, \ + { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 31, offsetof(mavlink_open_drone_id_location_t, target_component) }, \ + { "id_or_mac", NULL, MAVLINK_TYPE_UINT8_T, 20, 32, offsetof(mavlink_open_drone_id_location_t, id_or_mac) }, \ + { "status", NULL, MAVLINK_TYPE_UINT8_T, 0, 52, offsetof(mavlink_open_drone_id_location_t, status) }, \ + { "direction", NULL, MAVLINK_TYPE_UINT16_T, 0, 24, offsetof(mavlink_open_drone_id_location_t, direction) }, \ + { "speed_horizontal", NULL, MAVLINK_TYPE_UINT16_T, 0, 26, offsetof(mavlink_open_drone_id_location_t, speed_horizontal) }, \ + { "speed_vertical", NULL, MAVLINK_TYPE_INT16_T, 0, 28, offsetof(mavlink_open_drone_id_location_t, speed_vertical) }, \ + { "latitude", NULL, MAVLINK_TYPE_INT32_T, 0, 0, offsetof(mavlink_open_drone_id_location_t, latitude) }, \ + { "longitude", NULL, MAVLINK_TYPE_INT32_T, 0, 4, offsetof(mavlink_open_drone_id_location_t, longitude) }, \ + { "altitude_barometric", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_open_drone_id_location_t, altitude_barometric) }, \ + { "altitude_geodetic", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_open_drone_id_location_t, altitude_geodetic) }, \ + { "height_reference", NULL, MAVLINK_TYPE_UINT8_T, 0, 53, offsetof(mavlink_open_drone_id_location_t, height_reference) }, \ + { "height", NULL, MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_open_drone_id_location_t, height) }, \ + { "horizontal_accuracy", NULL, MAVLINK_TYPE_UINT8_T, 0, 54, offsetof(mavlink_open_drone_id_location_t, horizontal_accuracy) }, \ + { "vertical_accuracy", NULL, MAVLINK_TYPE_UINT8_T, 0, 55, offsetof(mavlink_open_drone_id_location_t, vertical_accuracy) }, \ + { "barometer_accuracy", NULL, MAVLINK_TYPE_UINT8_T, 0, 56, offsetof(mavlink_open_drone_id_location_t, barometer_accuracy) }, \ + { "speed_accuracy", NULL, MAVLINK_TYPE_UINT8_T, 0, 57, offsetof(mavlink_open_drone_id_location_t, speed_accuracy) }, \ + { "timestamp", NULL, MAVLINK_TYPE_FLOAT, 0, 20, offsetof(mavlink_open_drone_id_location_t, timestamp) }, \ + { "timestamp_accuracy", NULL, MAVLINK_TYPE_UINT8_T, 0, 58, offsetof(mavlink_open_drone_id_location_t, timestamp_accuracy) }, \ + } \ +} +#endif + +/** + * @brief Pack a open_drone_id_location message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param target_system System ID (0 for broadcast). + * @param target_component Component ID (0 for broadcast). + * @param id_or_mac Only used for drone ID data received from other UAs. See detailed description at https://mavlink.io/en/services/opendroneid.html. + * @param status Indicates whether the unmanned aircraft is on the ground or in the air. + * @param direction [cdeg] Direction over ground (not heading, but direction of movement) measured clockwise from true North: 0 - 35999 centi-degrees. If unknown: 36100 centi-degrees. + * @param speed_horizontal [cm/s] Ground speed. Positive only. If unknown: 25500 cm/s. If speed is larger than 25425 cm/s, use 25425 cm/s. + * @param speed_vertical [cm/s] The vertical speed. Up is positive. If unknown: 6300 cm/s. If speed is larger than 6200 cm/s, use 6200 cm/s. If lower than -6200 cm/s, use -6200 cm/s. + * @param latitude [degE7] Current latitude of the unmanned aircraft. If unknown: 0 (both Lat/Lon). + * @param longitude [degE7] Current longitude of the unmanned aircraft. If unknown: 0 (both Lat/Lon). + * @param altitude_barometric [m] The altitude calculated from the barometric pressue. Reference is against 29.92inHg or 1013.2mb. If unknown: -1000 m. + * @param altitude_geodetic [m] The geodetic altitude as defined by WGS84. If unknown: -1000 m. + * @param height_reference Indicates the reference point for the height field. + * @param height [m] The current height of the unmanned aircraft above the take-off location or the ground as indicated by height_reference. If unknown: -1000 m. + * @param horizontal_accuracy The accuracy of the horizontal position. + * @param vertical_accuracy The accuracy of the vertical position. + * @param barometer_accuracy The accuracy of the barometric altitude. + * @param speed_accuracy The accuracy of the horizontal and vertical speed. + * @param timestamp [s] Seconds after the full hour with reference to UTC time. Typically the GPS outputs a time-of-week value in milliseconds. First convert that to UTC and then convert for this field using ((float) (time_week_ms % (60*60*1000))) / 1000. + * @param timestamp_accuracy The accuracy of the timestamps. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_open_drone_id_location_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint8_t target_system, uint8_t target_component, const uint8_t *id_or_mac, uint8_t status, uint16_t direction, uint16_t speed_horizontal, int16_t speed_vertical, int32_t latitude, int32_t longitude, float altitude_barometric, float altitude_geodetic, uint8_t height_reference, float height, uint8_t horizontal_accuracy, uint8_t vertical_accuracy, uint8_t barometer_accuracy, uint8_t speed_accuracy, float timestamp, uint8_t timestamp_accuracy) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_OPEN_DRONE_ID_LOCATION_LEN]; + _mav_put_int32_t(buf, 0, latitude); + _mav_put_int32_t(buf, 4, longitude); + _mav_put_float(buf, 8, altitude_barometric); + _mav_put_float(buf, 12, altitude_geodetic); + _mav_put_float(buf, 16, height); + _mav_put_float(buf, 20, timestamp); + _mav_put_uint16_t(buf, 24, direction); + _mav_put_uint16_t(buf, 26, speed_horizontal); + _mav_put_int16_t(buf, 28, speed_vertical); + _mav_put_uint8_t(buf, 30, target_system); + _mav_put_uint8_t(buf, 31, target_component); + _mav_put_uint8_t(buf, 52, status); + _mav_put_uint8_t(buf, 53, height_reference); + _mav_put_uint8_t(buf, 54, horizontal_accuracy); + _mav_put_uint8_t(buf, 55, vertical_accuracy); + _mav_put_uint8_t(buf, 56, barometer_accuracy); + _mav_put_uint8_t(buf, 57, speed_accuracy); + _mav_put_uint8_t(buf, 58, timestamp_accuracy); + _mav_put_uint8_t_array(buf, 32, id_or_mac, 20); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_OPEN_DRONE_ID_LOCATION_LEN); +#else + mavlink_open_drone_id_location_t packet; + packet.latitude = latitude; + packet.longitude = longitude; + packet.altitude_barometric = altitude_barometric; + packet.altitude_geodetic = altitude_geodetic; + packet.height = height; + packet.timestamp = timestamp; + packet.direction = direction; + packet.speed_horizontal = speed_horizontal; + packet.speed_vertical = speed_vertical; + packet.target_system = target_system; + packet.target_component = target_component; + packet.status = status; + packet.height_reference = height_reference; + packet.horizontal_accuracy = horizontal_accuracy; + packet.vertical_accuracy = vertical_accuracy; + packet.barometer_accuracy = barometer_accuracy; + packet.speed_accuracy = speed_accuracy; + packet.timestamp_accuracy = timestamp_accuracy; + mav_array_memcpy(packet.id_or_mac, id_or_mac, sizeof(uint8_t)*20); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_OPEN_DRONE_ID_LOCATION_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_OPEN_DRONE_ID_LOCATION; + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_OPEN_DRONE_ID_LOCATION_MIN_LEN, MAVLINK_MSG_ID_OPEN_DRONE_ID_LOCATION_LEN, MAVLINK_MSG_ID_OPEN_DRONE_ID_LOCATION_CRC); +} + +/** + * @brief Pack a open_drone_id_location message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param target_system System ID (0 for broadcast). + * @param target_component Component ID (0 for broadcast). + * @param id_or_mac Only used for drone ID data received from other UAs. See detailed description at https://mavlink.io/en/services/opendroneid.html. + * @param status Indicates whether the unmanned aircraft is on the ground or in the air. + * @param direction [cdeg] Direction over ground (not heading, but direction of movement) measured clockwise from true North: 0 - 35999 centi-degrees. If unknown: 36100 centi-degrees. + * @param speed_horizontal [cm/s] Ground speed. Positive only. If unknown: 25500 cm/s. If speed is larger than 25425 cm/s, use 25425 cm/s. + * @param speed_vertical [cm/s] The vertical speed. Up is positive. If unknown: 6300 cm/s. If speed is larger than 6200 cm/s, use 6200 cm/s. If lower than -6200 cm/s, use -6200 cm/s. + * @param latitude [degE7] Current latitude of the unmanned aircraft. If unknown: 0 (both Lat/Lon). + * @param longitude [degE7] Current longitude of the unmanned aircraft. If unknown: 0 (both Lat/Lon). + * @param altitude_barometric [m] The altitude calculated from the barometric pressue. Reference is against 29.92inHg or 1013.2mb. If unknown: -1000 m. + * @param altitude_geodetic [m] The geodetic altitude as defined by WGS84. If unknown: -1000 m. + * @param height_reference Indicates the reference point for the height field. + * @param height [m] The current height of the unmanned aircraft above the take-off location or the ground as indicated by height_reference. If unknown: -1000 m. + * @param horizontal_accuracy The accuracy of the horizontal position. + * @param vertical_accuracy The accuracy of the vertical position. + * @param barometer_accuracy The accuracy of the barometric altitude. + * @param speed_accuracy The accuracy of the horizontal and vertical speed. + * @param timestamp [s] Seconds after the full hour with reference to UTC time. Typically the GPS outputs a time-of-week value in milliseconds. First convert that to UTC and then convert for this field using ((float) (time_week_ms % (60*60*1000))) / 1000. + * @param timestamp_accuracy The accuracy of the timestamps. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_open_drone_id_location_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint8_t target_system,uint8_t target_component,const uint8_t *id_or_mac,uint8_t status,uint16_t direction,uint16_t speed_horizontal,int16_t speed_vertical,int32_t latitude,int32_t longitude,float altitude_barometric,float altitude_geodetic,uint8_t height_reference,float height,uint8_t horizontal_accuracy,uint8_t vertical_accuracy,uint8_t barometer_accuracy,uint8_t speed_accuracy,float timestamp,uint8_t timestamp_accuracy) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_OPEN_DRONE_ID_LOCATION_LEN]; + _mav_put_int32_t(buf, 0, latitude); + _mav_put_int32_t(buf, 4, longitude); + _mav_put_float(buf, 8, altitude_barometric); + _mav_put_float(buf, 12, altitude_geodetic); + _mav_put_float(buf, 16, height); + _mav_put_float(buf, 20, timestamp); + _mav_put_uint16_t(buf, 24, direction); + _mav_put_uint16_t(buf, 26, speed_horizontal); + _mav_put_int16_t(buf, 28, speed_vertical); + _mav_put_uint8_t(buf, 30, target_system); + _mav_put_uint8_t(buf, 31, target_component); + _mav_put_uint8_t(buf, 52, status); + _mav_put_uint8_t(buf, 53, height_reference); + _mav_put_uint8_t(buf, 54, horizontal_accuracy); + _mav_put_uint8_t(buf, 55, vertical_accuracy); + _mav_put_uint8_t(buf, 56, barometer_accuracy); + _mav_put_uint8_t(buf, 57, speed_accuracy); + _mav_put_uint8_t(buf, 58, timestamp_accuracy); + _mav_put_uint8_t_array(buf, 32, id_or_mac, 20); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_OPEN_DRONE_ID_LOCATION_LEN); +#else + mavlink_open_drone_id_location_t packet; + packet.latitude = latitude; + packet.longitude = longitude; + packet.altitude_barometric = altitude_barometric; + packet.altitude_geodetic = altitude_geodetic; + packet.height = height; + packet.timestamp = timestamp; + packet.direction = direction; + packet.speed_horizontal = speed_horizontal; + packet.speed_vertical = speed_vertical; + packet.target_system = target_system; + packet.target_component = target_component; + packet.status = status; + packet.height_reference = height_reference; + packet.horizontal_accuracy = horizontal_accuracy; + packet.vertical_accuracy = vertical_accuracy; + packet.barometer_accuracy = barometer_accuracy; + packet.speed_accuracy = speed_accuracy; + packet.timestamp_accuracy = timestamp_accuracy; + mav_array_memcpy(packet.id_or_mac, id_or_mac, sizeof(uint8_t)*20); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_OPEN_DRONE_ID_LOCATION_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_OPEN_DRONE_ID_LOCATION; + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_OPEN_DRONE_ID_LOCATION_MIN_LEN, MAVLINK_MSG_ID_OPEN_DRONE_ID_LOCATION_LEN, MAVLINK_MSG_ID_OPEN_DRONE_ID_LOCATION_CRC); +} + +/** + * @brief Encode a open_drone_id_location struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param open_drone_id_location C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_open_drone_id_location_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_open_drone_id_location_t* open_drone_id_location) +{ + return mavlink_msg_open_drone_id_location_pack(system_id, component_id, msg, open_drone_id_location->target_system, open_drone_id_location->target_component, open_drone_id_location->id_or_mac, open_drone_id_location->status, open_drone_id_location->direction, open_drone_id_location->speed_horizontal, open_drone_id_location->speed_vertical, open_drone_id_location->latitude, open_drone_id_location->longitude, open_drone_id_location->altitude_barometric, open_drone_id_location->altitude_geodetic, open_drone_id_location->height_reference, open_drone_id_location->height, open_drone_id_location->horizontal_accuracy, open_drone_id_location->vertical_accuracy, open_drone_id_location->barometer_accuracy, open_drone_id_location->speed_accuracy, open_drone_id_location->timestamp, open_drone_id_location->timestamp_accuracy); +} + +/** + * @brief Encode a open_drone_id_location struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param open_drone_id_location C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_open_drone_id_location_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_open_drone_id_location_t* open_drone_id_location) +{ + return mavlink_msg_open_drone_id_location_pack_chan(system_id, component_id, chan, msg, open_drone_id_location->target_system, open_drone_id_location->target_component, open_drone_id_location->id_or_mac, open_drone_id_location->status, open_drone_id_location->direction, open_drone_id_location->speed_horizontal, open_drone_id_location->speed_vertical, open_drone_id_location->latitude, open_drone_id_location->longitude, open_drone_id_location->altitude_barometric, open_drone_id_location->altitude_geodetic, open_drone_id_location->height_reference, open_drone_id_location->height, open_drone_id_location->horizontal_accuracy, open_drone_id_location->vertical_accuracy, open_drone_id_location->barometer_accuracy, open_drone_id_location->speed_accuracy, open_drone_id_location->timestamp, open_drone_id_location->timestamp_accuracy); +} + +/** + * @brief Send a open_drone_id_location message + * @param chan MAVLink channel to send the message + * + * @param target_system System ID (0 for broadcast). + * @param target_component Component ID (0 for broadcast). + * @param id_or_mac Only used for drone ID data received from other UAs. See detailed description at https://mavlink.io/en/services/opendroneid.html. + * @param status Indicates whether the unmanned aircraft is on the ground or in the air. + * @param direction [cdeg] Direction over ground (not heading, but direction of movement) measured clockwise from true North: 0 - 35999 centi-degrees. If unknown: 36100 centi-degrees. + * @param speed_horizontal [cm/s] Ground speed. Positive only. If unknown: 25500 cm/s. If speed is larger than 25425 cm/s, use 25425 cm/s. + * @param speed_vertical [cm/s] The vertical speed. Up is positive. If unknown: 6300 cm/s. If speed is larger than 6200 cm/s, use 6200 cm/s. If lower than -6200 cm/s, use -6200 cm/s. + * @param latitude [degE7] Current latitude of the unmanned aircraft. If unknown: 0 (both Lat/Lon). + * @param longitude [degE7] Current longitude of the unmanned aircraft. If unknown: 0 (both Lat/Lon). + * @param altitude_barometric [m] The altitude calculated from the barometric pressue. Reference is against 29.92inHg or 1013.2mb. If unknown: -1000 m. + * @param altitude_geodetic [m] The geodetic altitude as defined by WGS84. If unknown: -1000 m. + * @param height_reference Indicates the reference point for the height field. + * @param height [m] The current height of the unmanned aircraft above the take-off location or the ground as indicated by height_reference. If unknown: -1000 m. + * @param horizontal_accuracy The accuracy of the horizontal position. + * @param vertical_accuracy The accuracy of the vertical position. + * @param barometer_accuracy The accuracy of the barometric altitude. + * @param speed_accuracy The accuracy of the horizontal and vertical speed. + * @param timestamp [s] Seconds after the full hour with reference to UTC time. Typically the GPS outputs a time-of-week value in milliseconds. First convert that to UTC and then convert for this field using ((float) (time_week_ms % (60*60*1000))) / 1000. + * @param timestamp_accuracy The accuracy of the timestamps. + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_open_drone_id_location_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, const uint8_t *id_or_mac, uint8_t status, uint16_t direction, uint16_t speed_horizontal, int16_t speed_vertical, int32_t latitude, int32_t longitude, float altitude_barometric, float altitude_geodetic, uint8_t height_reference, float height, uint8_t horizontal_accuracy, uint8_t vertical_accuracy, uint8_t barometer_accuracy, uint8_t speed_accuracy, float timestamp, uint8_t timestamp_accuracy) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_OPEN_DRONE_ID_LOCATION_LEN]; + _mav_put_int32_t(buf, 0, latitude); + _mav_put_int32_t(buf, 4, longitude); + _mav_put_float(buf, 8, altitude_barometric); + _mav_put_float(buf, 12, altitude_geodetic); + _mav_put_float(buf, 16, height); + _mav_put_float(buf, 20, timestamp); + _mav_put_uint16_t(buf, 24, direction); + _mav_put_uint16_t(buf, 26, speed_horizontal); + _mav_put_int16_t(buf, 28, speed_vertical); + _mav_put_uint8_t(buf, 30, target_system); + _mav_put_uint8_t(buf, 31, target_component); + _mav_put_uint8_t(buf, 52, status); + _mav_put_uint8_t(buf, 53, height_reference); + _mav_put_uint8_t(buf, 54, horizontal_accuracy); + _mav_put_uint8_t(buf, 55, vertical_accuracy); + _mav_put_uint8_t(buf, 56, barometer_accuracy); + _mav_put_uint8_t(buf, 57, speed_accuracy); + _mav_put_uint8_t(buf, 58, timestamp_accuracy); + _mav_put_uint8_t_array(buf, 32, id_or_mac, 20); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OPEN_DRONE_ID_LOCATION, buf, MAVLINK_MSG_ID_OPEN_DRONE_ID_LOCATION_MIN_LEN, MAVLINK_MSG_ID_OPEN_DRONE_ID_LOCATION_LEN, MAVLINK_MSG_ID_OPEN_DRONE_ID_LOCATION_CRC); +#else + mavlink_open_drone_id_location_t packet; + packet.latitude = latitude; + packet.longitude = longitude; + packet.altitude_barometric = altitude_barometric; + packet.altitude_geodetic = altitude_geodetic; + packet.height = height; + packet.timestamp = timestamp; + packet.direction = direction; + packet.speed_horizontal = speed_horizontal; + packet.speed_vertical = speed_vertical; + packet.target_system = target_system; + packet.target_component = target_component; + packet.status = status; + packet.height_reference = height_reference; + packet.horizontal_accuracy = horizontal_accuracy; + packet.vertical_accuracy = vertical_accuracy; + packet.barometer_accuracy = barometer_accuracy; + packet.speed_accuracy = speed_accuracy; + packet.timestamp_accuracy = timestamp_accuracy; + mav_array_memcpy(packet.id_or_mac, id_or_mac, sizeof(uint8_t)*20); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OPEN_DRONE_ID_LOCATION, (const char *)&packet, MAVLINK_MSG_ID_OPEN_DRONE_ID_LOCATION_MIN_LEN, MAVLINK_MSG_ID_OPEN_DRONE_ID_LOCATION_LEN, MAVLINK_MSG_ID_OPEN_DRONE_ID_LOCATION_CRC); +#endif +} + +/** + * @brief Send a open_drone_id_location message + * @param chan MAVLink channel to send the message + * @param struct The MAVLink struct to serialize + */ +static inline void mavlink_msg_open_drone_id_location_send_struct(mavlink_channel_t chan, const mavlink_open_drone_id_location_t* open_drone_id_location) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + mavlink_msg_open_drone_id_location_send(chan, open_drone_id_location->target_system, open_drone_id_location->target_component, open_drone_id_location->id_or_mac, open_drone_id_location->status, open_drone_id_location->direction, open_drone_id_location->speed_horizontal, open_drone_id_location->speed_vertical, open_drone_id_location->latitude, open_drone_id_location->longitude, open_drone_id_location->altitude_barometric, open_drone_id_location->altitude_geodetic, open_drone_id_location->height_reference, open_drone_id_location->height, open_drone_id_location->horizontal_accuracy, open_drone_id_location->vertical_accuracy, open_drone_id_location->barometer_accuracy, open_drone_id_location->speed_accuracy, open_drone_id_location->timestamp, open_drone_id_location->timestamp_accuracy); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OPEN_DRONE_ID_LOCATION, (const char *)open_drone_id_location, MAVLINK_MSG_ID_OPEN_DRONE_ID_LOCATION_MIN_LEN, MAVLINK_MSG_ID_OPEN_DRONE_ID_LOCATION_LEN, MAVLINK_MSG_ID_OPEN_DRONE_ID_LOCATION_CRC); +#endif +} + +#if MAVLINK_MSG_ID_OPEN_DRONE_ID_LOCATION_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_open_drone_id_location_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, const uint8_t *id_or_mac, uint8_t status, uint16_t direction, uint16_t speed_horizontal, int16_t speed_vertical, int32_t latitude, int32_t longitude, float altitude_barometric, float altitude_geodetic, uint8_t height_reference, float height, uint8_t horizontal_accuracy, uint8_t vertical_accuracy, uint8_t barometer_accuracy, uint8_t speed_accuracy, float timestamp, uint8_t timestamp_accuracy) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_int32_t(buf, 0, latitude); + _mav_put_int32_t(buf, 4, longitude); + _mav_put_float(buf, 8, altitude_barometric); + _mav_put_float(buf, 12, altitude_geodetic); + _mav_put_float(buf, 16, height); + _mav_put_float(buf, 20, timestamp); + _mav_put_uint16_t(buf, 24, direction); + _mav_put_uint16_t(buf, 26, speed_horizontal); + _mav_put_int16_t(buf, 28, speed_vertical); + _mav_put_uint8_t(buf, 30, target_system); + _mav_put_uint8_t(buf, 31, target_component); + _mav_put_uint8_t(buf, 52, status); + _mav_put_uint8_t(buf, 53, height_reference); + _mav_put_uint8_t(buf, 54, horizontal_accuracy); + _mav_put_uint8_t(buf, 55, vertical_accuracy); + _mav_put_uint8_t(buf, 56, barometer_accuracy); + _mav_put_uint8_t(buf, 57, speed_accuracy); + _mav_put_uint8_t(buf, 58, timestamp_accuracy); + _mav_put_uint8_t_array(buf, 32, id_or_mac, 20); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OPEN_DRONE_ID_LOCATION, buf, MAVLINK_MSG_ID_OPEN_DRONE_ID_LOCATION_MIN_LEN, MAVLINK_MSG_ID_OPEN_DRONE_ID_LOCATION_LEN, MAVLINK_MSG_ID_OPEN_DRONE_ID_LOCATION_CRC); +#else + mavlink_open_drone_id_location_t *packet = (mavlink_open_drone_id_location_t *)msgbuf; + packet->latitude = latitude; + packet->longitude = longitude; + packet->altitude_barometric = altitude_barometric; + packet->altitude_geodetic = altitude_geodetic; + packet->height = height; + packet->timestamp = timestamp; + packet->direction = direction; + packet->speed_horizontal = speed_horizontal; + packet->speed_vertical = speed_vertical; + packet->target_system = target_system; + packet->target_component = target_component; + packet->status = status; + packet->height_reference = height_reference; + packet->horizontal_accuracy = horizontal_accuracy; + packet->vertical_accuracy = vertical_accuracy; + packet->barometer_accuracy = barometer_accuracy; + packet->speed_accuracy = speed_accuracy; + packet->timestamp_accuracy = timestamp_accuracy; + mav_array_memcpy(packet->id_or_mac, id_or_mac, sizeof(uint8_t)*20); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OPEN_DRONE_ID_LOCATION, (const char *)packet, MAVLINK_MSG_ID_OPEN_DRONE_ID_LOCATION_MIN_LEN, MAVLINK_MSG_ID_OPEN_DRONE_ID_LOCATION_LEN, MAVLINK_MSG_ID_OPEN_DRONE_ID_LOCATION_CRC); +#endif +} +#endif + +#endif + +// MESSAGE OPEN_DRONE_ID_LOCATION UNPACKING + + +/** + * @brief Get field target_system from open_drone_id_location message + * + * @return System ID (0 for broadcast). + */ +static inline uint8_t mavlink_msg_open_drone_id_location_get_target_system(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 30); +} + +/** + * @brief Get field target_component from open_drone_id_location message + * + * @return Component ID (0 for broadcast). + */ +static inline uint8_t mavlink_msg_open_drone_id_location_get_target_component(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 31); +} + +/** + * @brief Get field id_or_mac from open_drone_id_location message + * + * @return Only used for drone ID data received from other UAs. See detailed description at https://mavlink.io/en/services/opendroneid.html. + */ +static inline uint16_t mavlink_msg_open_drone_id_location_get_id_or_mac(const mavlink_message_t* msg, uint8_t *id_or_mac) +{ + return _MAV_RETURN_uint8_t_array(msg, id_or_mac, 20, 32); +} + +/** + * @brief Get field status from open_drone_id_location message + * + * @return Indicates whether the unmanned aircraft is on the ground or in the air. + */ +static inline uint8_t mavlink_msg_open_drone_id_location_get_status(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 52); +} + +/** + * @brief Get field direction from open_drone_id_location message + * + * @return [cdeg] Direction over ground (not heading, but direction of movement) measured clockwise from true North: 0 - 35999 centi-degrees. If unknown: 36100 centi-degrees. + */ +static inline uint16_t mavlink_msg_open_drone_id_location_get_direction(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 24); +} + +/** + * @brief Get field speed_horizontal from open_drone_id_location message + * + * @return [cm/s] Ground speed. Positive only. If unknown: 25500 cm/s. If speed is larger than 25425 cm/s, use 25425 cm/s. + */ +static inline uint16_t mavlink_msg_open_drone_id_location_get_speed_horizontal(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 26); +} + +/** + * @brief Get field speed_vertical from open_drone_id_location message + * + * @return [cm/s] The vertical speed. Up is positive. If unknown: 6300 cm/s. If speed is larger than 6200 cm/s, use 6200 cm/s. If lower than -6200 cm/s, use -6200 cm/s. + */ +static inline int16_t mavlink_msg_open_drone_id_location_get_speed_vertical(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int16_t(msg, 28); +} + +/** + * @brief Get field latitude from open_drone_id_location message + * + * @return [degE7] Current latitude of the unmanned aircraft. If unknown: 0 (both Lat/Lon). + */ +static inline int32_t mavlink_msg_open_drone_id_location_get_latitude(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int32_t(msg, 0); +} + +/** + * @brief Get field longitude from open_drone_id_location message + * + * @return [degE7] Current longitude of the unmanned aircraft. If unknown: 0 (both Lat/Lon). + */ +static inline int32_t mavlink_msg_open_drone_id_location_get_longitude(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int32_t(msg, 4); +} + +/** + * @brief Get field altitude_barometric from open_drone_id_location message + * + * @return [m] The altitude calculated from the barometric pressue. Reference is against 29.92inHg or 1013.2mb. If unknown: -1000 m. + */ +static inline float mavlink_msg_open_drone_id_location_get_altitude_barometric(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 8); +} + +/** + * @brief Get field altitude_geodetic from open_drone_id_location message + * + * @return [m] The geodetic altitude as defined by WGS84. If unknown: -1000 m. + */ +static inline float mavlink_msg_open_drone_id_location_get_altitude_geodetic(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 12); +} + +/** + * @brief Get field height_reference from open_drone_id_location message + * + * @return Indicates the reference point for the height field. + */ +static inline uint8_t mavlink_msg_open_drone_id_location_get_height_reference(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 53); +} + +/** + * @brief Get field height from open_drone_id_location message + * + * @return [m] The current height of the unmanned aircraft above the take-off location or the ground as indicated by height_reference. If unknown: -1000 m. + */ +static inline float mavlink_msg_open_drone_id_location_get_height(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 16); +} + +/** + * @brief Get field horizontal_accuracy from open_drone_id_location message + * + * @return The accuracy of the horizontal position. + */ +static inline uint8_t mavlink_msg_open_drone_id_location_get_horizontal_accuracy(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 54); +} + +/** + * @brief Get field vertical_accuracy from open_drone_id_location message + * + * @return The accuracy of the vertical position. + */ +static inline uint8_t mavlink_msg_open_drone_id_location_get_vertical_accuracy(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 55); +} + +/** + * @brief Get field barometer_accuracy from open_drone_id_location message + * + * @return The accuracy of the barometric altitude. + */ +static inline uint8_t mavlink_msg_open_drone_id_location_get_barometer_accuracy(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 56); +} + +/** + * @brief Get field speed_accuracy from open_drone_id_location message + * + * @return The accuracy of the horizontal and vertical speed. + */ +static inline uint8_t mavlink_msg_open_drone_id_location_get_speed_accuracy(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 57); +} + +/** + * @brief Get field timestamp from open_drone_id_location message + * + * @return [s] Seconds after the full hour with reference to UTC time. Typically the GPS outputs a time-of-week value in milliseconds. First convert that to UTC and then convert for this field using ((float) (time_week_ms % (60*60*1000))) / 1000. + */ +static inline float mavlink_msg_open_drone_id_location_get_timestamp(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 20); +} + +/** + * @brief Get field timestamp_accuracy from open_drone_id_location message + * + * @return The accuracy of the timestamps. + */ +static inline uint8_t mavlink_msg_open_drone_id_location_get_timestamp_accuracy(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 58); +} + +/** + * @brief Decode a open_drone_id_location message into a struct + * + * @param msg The message to decode + * @param open_drone_id_location C-struct to decode the message contents into + */ +static inline void mavlink_msg_open_drone_id_location_decode(const mavlink_message_t* msg, mavlink_open_drone_id_location_t* open_drone_id_location) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + open_drone_id_location->latitude = mavlink_msg_open_drone_id_location_get_latitude(msg); + open_drone_id_location->longitude = mavlink_msg_open_drone_id_location_get_longitude(msg); + open_drone_id_location->altitude_barometric = mavlink_msg_open_drone_id_location_get_altitude_barometric(msg); + open_drone_id_location->altitude_geodetic = mavlink_msg_open_drone_id_location_get_altitude_geodetic(msg); + open_drone_id_location->height = mavlink_msg_open_drone_id_location_get_height(msg); + open_drone_id_location->timestamp = mavlink_msg_open_drone_id_location_get_timestamp(msg); + open_drone_id_location->direction = mavlink_msg_open_drone_id_location_get_direction(msg); + open_drone_id_location->speed_horizontal = mavlink_msg_open_drone_id_location_get_speed_horizontal(msg); + open_drone_id_location->speed_vertical = mavlink_msg_open_drone_id_location_get_speed_vertical(msg); + open_drone_id_location->target_system = mavlink_msg_open_drone_id_location_get_target_system(msg); + open_drone_id_location->target_component = mavlink_msg_open_drone_id_location_get_target_component(msg); + mavlink_msg_open_drone_id_location_get_id_or_mac(msg, open_drone_id_location->id_or_mac); + open_drone_id_location->status = mavlink_msg_open_drone_id_location_get_status(msg); + open_drone_id_location->height_reference = mavlink_msg_open_drone_id_location_get_height_reference(msg); + open_drone_id_location->horizontal_accuracy = mavlink_msg_open_drone_id_location_get_horizontal_accuracy(msg); + open_drone_id_location->vertical_accuracy = mavlink_msg_open_drone_id_location_get_vertical_accuracy(msg); + open_drone_id_location->barometer_accuracy = mavlink_msg_open_drone_id_location_get_barometer_accuracy(msg); + open_drone_id_location->speed_accuracy = mavlink_msg_open_drone_id_location_get_speed_accuracy(msg); + open_drone_id_location->timestamp_accuracy = mavlink_msg_open_drone_id_location_get_timestamp_accuracy(msg); +#else + uint8_t len = msg->len < MAVLINK_MSG_ID_OPEN_DRONE_ID_LOCATION_LEN? msg->len : MAVLINK_MSG_ID_OPEN_DRONE_ID_LOCATION_LEN; + memset(open_drone_id_location, 0, MAVLINK_MSG_ID_OPEN_DRONE_ID_LOCATION_LEN); + memcpy(open_drone_id_location, _MAV_PAYLOAD(msg), len); +#endif +} diff --git a/lib/main/MAVLink/common/mavlink_msg_open_drone_id_message_pack.h b/lib/main/MAVLink/common/mavlink_msg_open_drone_id_message_pack.h new file mode 100644 index 0000000000..16407cfb35 --- /dev/null +++ b/lib/main/MAVLink/common/mavlink_msg_open_drone_id_message_pack.h @@ -0,0 +1,305 @@ +#pragma once +// MESSAGE OPEN_DRONE_ID_MESSAGE_PACK PACKING + +#define MAVLINK_MSG_ID_OPEN_DRONE_ID_MESSAGE_PACK 12915 + + +typedef struct __mavlink_open_drone_id_message_pack_t { + uint8_t target_system; /*< System ID (0 for broadcast).*/ + uint8_t target_component; /*< Component ID (0 for broadcast).*/ + uint8_t single_message_size; /*< [bytes] This field must currently always be equal to 25 (bytes), since all encoded OpenDroneID messages are specificed to have this length.*/ + uint8_t msg_pack_size; /*< Number of encoded messages in the pack (not the number of bytes). Allowed range is 1 - 10.*/ + uint8_t messages[250]; /*< Concatenation of encoded OpenDroneID messages. Shall be filled with nulls in the unused portion of the field.*/ +} mavlink_open_drone_id_message_pack_t; + +#define MAVLINK_MSG_ID_OPEN_DRONE_ID_MESSAGE_PACK_LEN 254 +#define MAVLINK_MSG_ID_OPEN_DRONE_ID_MESSAGE_PACK_MIN_LEN 254 +#define MAVLINK_MSG_ID_12915_LEN 254 +#define MAVLINK_MSG_ID_12915_MIN_LEN 254 + +#define MAVLINK_MSG_ID_OPEN_DRONE_ID_MESSAGE_PACK_CRC 62 +#define MAVLINK_MSG_ID_12915_CRC 62 + +#define MAVLINK_MSG_OPEN_DRONE_ID_MESSAGE_PACK_FIELD_MESSAGES_LEN 250 + +#if MAVLINK_COMMAND_24BIT +#define MAVLINK_MESSAGE_INFO_OPEN_DRONE_ID_MESSAGE_PACK { \ + 12915, \ + "OPEN_DRONE_ID_MESSAGE_PACK", \ + 5, \ + { { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 0, offsetof(mavlink_open_drone_id_message_pack_t, target_system) }, \ + { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 1, offsetof(mavlink_open_drone_id_message_pack_t, target_component) }, \ + { "single_message_size", NULL, MAVLINK_TYPE_UINT8_T, 0, 2, offsetof(mavlink_open_drone_id_message_pack_t, single_message_size) }, \ + { "msg_pack_size", NULL, MAVLINK_TYPE_UINT8_T, 0, 3, offsetof(mavlink_open_drone_id_message_pack_t, msg_pack_size) }, \ + { "messages", NULL, MAVLINK_TYPE_UINT8_T, 250, 4, offsetof(mavlink_open_drone_id_message_pack_t, messages) }, \ + } \ +} +#else +#define MAVLINK_MESSAGE_INFO_OPEN_DRONE_ID_MESSAGE_PACK { \ + "OPEN_DRONE_ID_MESSAGE_PACK", \ + 5, \ + { { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 0, offsetof(mavlink_open_drone_id_message_pack_t, target_system) }, \ + { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 1, offsetof(mavlink_open_drone_id_message_pack_t, target_component) }, \ + { "single_message_size", NULL, MAVLINK_TYPE_UINT8_T, 0, 2, offsetof(mavlink_open_drone_id_message_pack_t, single_message_size) }, \ + { "msg_pack_size", NULL, MAVLINK_TYPE_UINT8_T, 0, 3, offsetof(mavlink_open_drone_id_message_pack_t, msg_pack_size) }, \ + { "messages", NULL, MAVLINK_TYPE_UINT8_T, 250, 4, offsetof(mavlink_open_drone_id_message_pack_t, messages) }, \ + } \ +} +#endif + +/** + * @brief Pack a open_drone_id_message_pack message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param target_system System ID (0 for broadcast). + * @param target_component Component ID (0 for broadcast). + * @param single_message_size [bytes] This field must currently always be equal to 25 (bytes), since all encoded OpenDroneID messages are specificed to have this length. + * @param msg_pack_size Number of encoded messages in the pack (not the number of bytes). Allowed range is 1 - 10. + * @param messages Concatenation of encoded OpenDroneID messages. Shall be filled with nulls in the unused portion of the field. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_open_drone_id_message_pack_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint8_t target_system, uint8_t target_component, uint8_t single_message_size, uint8_t msg_pack_size, const uint8_t *messages) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_OPEN_DRONE_ID_MESSAGE_PACK_LEN]; + _mav_put_uint8_t(buf, 0, target_system); + _mav_put_uint8_t(buf, 1, target_component); + _mav_put_uint8_t(buf, 2, single_message_size); + _mav_put_uint8_t(buf, 3, msg_pack_size); + _mav_put_uint8_t_array(buf, 4, messages, 250); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_OPEN_DRONE_ID_MESSAGE_PACK_LEN); +#else + mavlink_open_drone_id_message_pack_t packet; + packet.target_system = target_system; + packet.target_component = target_component; + packet.single_message_size = single_message_size; + packet.msg_pack_size = msg_pack_size; + mav_array_memcpy(packet.messages, messages, sizeof(uint8_t)*250); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_OPEN_DRONE_ID_MESSAGE_PACK_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_OPEN_DRONE_ID_MESSAGE_PACK; + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_OPEN_DRONE_ID_MESSAGE_PACK_MIN_LEN, MAVLINK_MSG_ID_OPEN_DRONE_ID_MESSAGE_PACK_LEN, MAVLINK_MSG_ID_OPEN_DRONE_ID_MESSAGE_PACK_CRC); +} + +/** + * @brief Pack a open_drone_id_message_pack message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param target_system System ID (0 for broadcast). + * @param target_component Component ID (0 for broadcast). + * @param single_message_size [bytes] This field must currently always be equal to 25 (bytes), since all encoded OpenDroneID messages are specificed to have this length. + * @param msg_pack_size Number of encoded messages in the pack (not the number of bytes). Allowed range is 1 - 10. + * @param messages Concatenation of encoded OpenDroneID messages. Shall be filled with nulls in the unused portion of the field. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_open_drone_id_message_pack_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint8_t target_system,uint8_t target_component,uint8_t single_message_size,uint8_t msg_pack_size,const uint8_t *messages) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_OPEN_DRONE_ID_MESSAGE_PACK_LEN]; + _mav_put_uint8_t(buf, 0, target_system); + _mav_put_uint8_t(buf, 1, target_component); + _mav_put_uint8_t(buf, 2, single_message_size); + _mav_put_uint8_t(buf, 3, msg_pack_size); + _mav_put_uint8_t_array(buf, 4, messages, 250); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_OPEN_DRONE_ID_MESSAGE_PACK_LEN); +#else + mavlink_open_drone_id_message_pack_t packet; + packet.target_system = target_system; + packet.target_component = target_component; + packet.single_message_size = single_message_size; + packet.msg_pack_size = msg_pack_size; + mav_array_memcpy(packet.messages, messages, sizeof(uint8_t)*250); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_OPEN_DRONE_ID_MESSAGE_PACK_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_OPEN_DRONE_ID_MESSAGE_PACK; + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_OPEN_DRONE_ID_MESSAGE_PACK_MIN_LEN, MAVLINK_MSG_ID_OPEN_DRONE_ID_MESSAGE_PACK_LEN, MAVLINK_MSG_ID_OPEN_DRONE_ID_MESSAGE_PACK_CRC); +} + +/** + * @brief Encode a open_drone_id_message_pack struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param open_drone_id_message_pack C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_open_drone_id_message_pack_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_open_drone_id_message_pack_t* open_drone_id_message_pack) +{ + return mavlink_msg_open_drone_id_message_pack_pack(system_id, component_id, msg, open_drone_id_message_pack->target_system, open_drone_id_message_pack->target_component, open_drone_id_message_pack->single_message_size, open_drone_id_message_pack->msg_pack_size, open_drone_id_message_pack->messages); +} + +/** + * @brief Encode a open_drone_id_message_pack struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param open_drone_id_message_pack C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_open_drone_id_message_pack_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_open_drone_id_message_pack_t* open_drone_id_message_pack) +{ + return mavlink_msg_open_drone_id_message_pack_pack_chan(system_id, component_id, chan, msg, open_drone_id_message_pack->target_system, open_drone_id_message_pack->target_component, open_drone_id_message_pack->single_message_size, open_drone_id_message_pack->msg_pack_size, open_drone_id_message_pack->messages); +} + +/** + * @brief Send a open_drone_id_message_pack message + * @param chan MAVLink channel to send the message + * + * @param target_system System ID (0 for broadcast). + * @param target_component Component ID (0 for broadcast). + * @param single_message_size [bytes] This field must currently always be equal to 25 (bytes), since all encoded OpenDroneID messages are specificed to have this length. + * @param msg_pack_size Number of encoded messages in the pack (not the number of bytes). Allowed range is 1 - 10. + * @param messages Concatenation of encoded OpenDroneID messages. Shall be filled with nulls in the unused portion of the field. + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_open_drone_id_message_pack_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint8_t single_message_size, uint8_t msg_pack_size, const uint8_t *messages) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_OPEN_DRONE_ID_MESSAGE_PACK_LEN]; + _mav_put_uint8_t(buf, 0, target_system); + _mav_put_uint8_t(buf, 1, target_component); + _mav_put_uint8_t(buf, 2, single_message_size); + _mav_put_uint8_t(buf, 3, msg_pack_size); + _mav_put_uint8_t_array(buf, 4, messages, 250); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OPEN_DRONE_ID_MESSAGE_PACK, buf, MAVLINK_MSG_ID_OPEN_DRONE_ID_MESSAGE_PACK_MIN_LEN, MAVLINK_MSG_ID_OPEN_DRONE_ID_MESSAGE_PACK_LEN, MAVLINK_MSG_ID_OPEN_DRONE_ID_MESSAGE_PACK_CRC); +#else + mavlink_open_drone_id_message_pack_t packet; + packet.target_system = target_system; + packet.target_component = target_component; + packet.single_message_size = single_message_size; + packet.msg_pack_size = msg_pack_size; + mav_array_memcpy(packet.messages, messages, sizeof(uint8_t)*250); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OPEN_DRONE_ID_MESSAGE_PACK, (const char *)&packet, MAVLINK_MSG_ID_OPEN_DRONE_ID_MESSAGE_PACK_MIN_LEN, MAVLINK_MSG_ID_OPEN_DRONE_ID_MESSAGE_PACK_LEN, MAVLINK_MSG_ID_OPEN_DRONE_ID_MESSAGE_PACK_CRC); +#endif +} + +/** + * @brief Send a open_drone_id_message_pack message + * @param chan MAVLink channel to send the message + * @param struct The MAVLink struct to serialize + */ +static inline void mavlink_msg_open_drone_id_message_pack_send_struct(mavlink_channel_t chan, const mavlink_open_drone_id_message_pack_t* open_drone_id_message_pack) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + mavlink_msg_open_drone_id_message_pack_send(chan, open_drone_id_message_pack->target_system, open_drone_id_message_pack->target_component, open_drone_id_message_pack->single_message_size, open_drone_id_message_pack->msg_pack_size, open_drone_id_message_pack->messages); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OPEN_DRONE_ID_MESSAGE_PACK, (const char *)open_drone_id_message_pack, MAVLINK_MSG_ID_OPEN_DRONE_ID_MESSAGE_PACK_MIN_LEN, MAVLINK_MSG_ID_OPEN_DRONE_ID_MESSAGE_PACK_LEN, MAVLINK_MSG_ID_OPEN_DRONE_ID_MESSAGE_PACK_CRC); +#endif +} + +#if MAVLINK_MSG_ID_OPEN_DRONE_ID_MESSAGE_PACK_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_open_drone_id_message_pack_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint8_t single_message_size, uint8_t msg_pack_size, const uint8_t *messages) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint8_t(buf, 0, target_system); + _mav_put_uint8_t(buf, 1, target_component); + _mav_put_uint8_t(buf, 2, single_message_size); + _mav_put_uint8_t(buf, 3, msg_pack_size); + _mav_put_uint8_t_array(buf, 4, messages, 250); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OPEN_DRONE_ID_MESSAGE_PACK, buf, MAVLINK_MSG_ID_OPEN_DRONE_ID_MESSAGE_PACK_MIN_LEN, MAVLINK_MSG_ID_OPEN_DRONE_ID_MESSAGE_PACK_LEN, MAVLINK_MSG_ID_OPEN_DRONE_ID_MESSAGE_PACK_CRC); +#else + mavlink_open_drone_id_message_pack_t *packet = (mavlink_open_drone_id_message_pack_t *)msgbuf; + packet->target_system = target_system; + packet->target_component = target_component; + packet->single_message_size = single_message_size; + packet->msg_pack_size = msg_pack_size; + mav_array_memcpy(packet->messages, messages, sizeof(uint8_t)*250); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OPEN_DRONE_ID_MESSAGE_PACK, (const char *)packet, MAVLINK_MSG_ID_OPEN_DRONE_ID_MESSAGE_PACK_MIN_LEN, MAVLINK_MSG_ID_OPEN_DRONE_ID_MESSAGE_PACK_LEN, MAVLINK_MSG_ID_OPEN_DRONE_ID_MESSAGE_PACK_CRC); +#endif +} +#endif + +#endif + +// MESSAGE OPEN_DRONE_ID_MESSAGE_PACK UNPACKING + + +/** + * @brief Get field target_system from open_drone_id_message_pack message + * + * @return System ID (0 for broadcast). + */ +static inline uint8_t mavlink_msg_open_drone_id_message_pack_get_target_system(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 0); +} + +/** + * @brief Get field target_component from open_drone_id_message_pack message + * + * @return Component ID (0 for broadcast). + */ +static inline uint8_t mavlink_msg_open_drone_id_message_pack_get_target_component(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 1); +} + +/** + * @brief Get field single_message_size from open_drone_id_message_pack message + * + * @return [bytes] This field must currently always be equal to 25 (bytes), since all encoded OpenDroneID messages are specificed to have this length. + */ +static inline uint8_t mavlink_msg_open_drone_id_message_pack_get_single_message_size(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 2); +} + +/** + * @brief Get field msg_pack_size from open_drone_id_message_pack message + * + * @return Number of encoded messages in the pack (not the number of bytes). Allowed range is 1 - 10. + */ +static inline uint8_t mavlink_msg_open_drone_id_message_pack_get_msg_pack_size(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 3); +} + +/** + * @brief Get field messages from open_drone_id_message_pack message + * + * @return Concatenation of encoded OpenDroneID messages. Shall be filled with nulls in the unused portion of the field. + */ +static inline uint16_t mavlink_msg_open_drone_id_message_pack_get_messages(const mavlink_message_t* msg, uint8_t *messages) +{ + return _MAV_RETURN_uint8_t_array(msg, messages, 250, 4); +} + +/** + * @brief Decode a open_drone_id_message_pack message into a struct + * + * @param msg The message to decode + * @param open_drone_id_message_pack C-struct to decode the message contents into + */ +static inline void mavlink_msg_open_drone_id_message_pack_decode(const mavlink_message_t* msg, mavlink_open_drone_id_message_pack_t* open_drone_id_message_pack) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + open_drone_id_message_pack->target_system = mavlink_msg_open_drone_id_message_pack_get_target_system(msg); + open_drone_id_message_pack->target_component = mavlink_msg_open_drone_id_message_pack_get_target_component(msg); + open_drone_id_message_pack->single_message_size = mavlink_msg_open_drone_id_message_pack_get_single_message_size(msg); + open_drone_id_message_pack->msg_pack_size = mavlink_msg_open_drone_id_message_pack_get_msg_pack_size(msg); + mavlink_msg_open_drone_id_message_pack_get_messages(msg, open_drone_id_message_pack->messages); +#else + uint8_t len = msg->len < MAVLINK_MSG_ID_OPEN_DRONE_ID_MESSAGE_PACK_LEN? msg->len : MAVLINK_MSG_ID_OPEN_DRONE_ID_MESSAGE_PACK_LEN; + memset(open_drone_id_message_pack, 0, MAVLINK_MSG_ID_OPEN_DRONE_ID_MESSAGE_PACK_LEN); + memcpy(open_drone_id_message_pack, _MAV_PAYLOAD(msg), len); +#endif +} diff --git a/lib/main/MAVLink/common/mavlink_msg_open_drone_id_operator_id.h b/lib/main/MAVLink/common/mavlink_msg_open_drone_id_operator_id.h new file mode 100644 index 0000000000..f3bf5aff29 --- /dev/null +++ b/lib/main/MAVLink/common/mavlink_msg_open_drone_id_operator_id.h @@ -0,0 +1,306 @@ +#pragma once +// MESSAGE OPEN_DRONE_ID_OPERATOR_ID PACKING + +#define MAVLINK_MSG_ID_OPEN_DRONE_ID_OPERATOR_ID 12905 + + +typedef struct __mavlink_open_drone_id_operator_id_t { + uint8_t target_system; /*< System ID (0 for broadcast).*/ + uint8_t target_component; /*< Component ID (0 for broadcast).*/ + uint8_t id_or_mac[20]; /*< Only used for drone ID data received from other UAs. See detailed description at https://mavlink.io/en/services/opendroneid.html. */ + uint8_t operator_id_type; /*< Indicates the type of the operator_id field.*/ + char operator_id[20]; /*< Text description or numeric value expressed as ASCII characters. Shall be filled with nulls in the unused portion of the field.*/ +} mavlink_open_drone_id_operator_id_t; + +#define MAVLINK_MSG_ID_OPEN_DRONE_ID_OPERATOR_ID_LEN 43 +#define MAVLINK_MSG_ID_OPEN_DRONE_ID_OPERATOR_ID_MIN_LEN 43 +#define MAVLINK_MSG_ID_12905_LEN 43 +#define MAVLINK_MSG_ID_12905_MIN_LEN 43 + +#define MAVLINK_MSG_ID_OPEN_DRONE_ID_OPERATOR_ID_CRC 49 +#define MAVLINK_MSG_ID_12905_CRC 49 + +#define MAVLINK_MSG_OPEN_DRONE_ID_OPERATOR_ID_FIELD_ID_OR_MAC_LEN 20 +#define MAVLINK_MSG_OPEN_DRONE_ID_OPERATOR_ID_FIELD_OPERATOR_ID_LEN 20 + +#if MAVLINK_COMMAND_24BIT +#define MAVLINK_MESSAGE_INFO_OPEN_DRONE_ID_OPERATOR_ID { \ + 12905, \ + "OPEN_DRONE_ID_OPERATOR_ID", \ + 5, \ + { { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 0, offsetof(mavlink_open_drone_id_operator_id_t, target_system) }, \ + { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 1, offsetof(mavlink_open_drone_id_operator_id_t, target_component) }, \ + { "id_or_mac", NULL, MAVLINK_TYPE_UINT8_T, 20, 2, offsetof(mavlink_open_drone_id_operator_id_t, id_or_mac) }, \ + { "operator_id_type", NULL, MAVLINK_TYPE_UINT8_T, 0, 22, offsetof(mavlink_open_drone_id_operator_id_t, operator_id_type) }, \ + { "operator_id", NULL, MAVLINK_TYPE_CHAR, 20, 23, offsetof(mavlink_open_drone_id_operator_id_t, operator_id) }, \ + } \ +} +#else +#define MAVLINK_MESSAGE_INFO_OPEN_DRONE_ID_OPERATOR_ID { \ + "OPEN_DRONE_ID_OPERATOR_ID", \ + 5, \ + { { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 0, offsetof(mavlink_open_drone_id_operator_id_t, target_system) }, \ + { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 1, offsetof(mavlink_open_drone_id_operator_id_t, target_component) }, \ + { "id_or_mac", NULL, MAVLINK_TYPE_UINT8_T, 20, 2, offsetof(mavlink_open_drone_id_operator_id_t, id_or_mac) }, \ + { "operator_id_type", NULL, MAVLINK_TYPE_UINT8_T, 0, 22, offsetof(mavlink_open_drone_id_operator_id_t, operator_id_type) }, \ + { "operator_id", NULL, MAVLINK_TYPE_CHAR, 20, 23, offsetof(mavlink_open_drone_id_operator_id_t, operator_id) }, \ + } \ +} +#endif + +/** + * @brief Pack a open_drone_id_operator_id message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param target_system System ID (0 for broadcast). + * @param target_component Component ID (0 for broadcast). + * @param id_or_mac Only used for drone ID data received from other UAs. See detailed description at https://mavlink.io/en/services/opendroneid.html. + * @param operator_id_type Indicates the type of the operator_id field. + * @param operator_id Text description or numeric value expressed as ASCII characters. Shall be filled with nulls in the unused portion of the field. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_open_drone_id_operator_id_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint8_t target_system, uint8_t target_component, const uint8_t *id_or_mac, uint8_t operator_id_type, const char *operator_id) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_OPEN_DRONE_ID_OPERATOR_ID_LEN]; + _mav_put_uint8_t(buf, 0, target_system); + _mav_put_uint8_t(buf, 1, target_component); + _mav_put_uint8_t(buf, 22, operator_id_type); + _mav_put_uint8_t_array(buf, 2, id_or_mac, 20); + _mav_put_char_array(buf, 23, operator_id, 20); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_OPEN_DRONE_ID_OPERATOR_ID_LEN); +#else + mavlink_open_drone_id_operator_id_t packet; + packet.target_system = target_system; + packet.target_component = target_component; + packet.operator_id_type = operator_id_type; + mav_array_memcpy(packet.id_or_mac, id_or_mac, sizeof(uint8_t)*20); + mav_array_memcpy(packet.operator_id, operator_id, sizeof(char)*20); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_OPEN_DRONE_ID_OPERATOR_ID_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_OPEN_DRONE_ID_OPERATOR_ID; + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_OPEN_DRONE_ID_OPERATOR_ID_MIN_LEN, MAVLINK_MSG_ID_OPEN_DRONE_ID_OPERATOR_ID_LEN, MAVLINK_MSG_ID_OPEN_DRONE_ID_OPERATOR_ID_CRC); +} + +/** + * @brief Pack a open_drone_id_operator_id message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param target_system System ID (0 for broadcast). + * @param target_component Component ID (0 for broadcast). + * @param id_or_mac Only used for drone ID data received from other UAs. See detailed description at https://mavlink.io/en/services/opendroneid.html. + * @param operator_id_type Indicates the type of the operator_id field. + * @param operator_id Text description or numeric value expressed as ASCII characters. Shall be filled with nulls in the unused portion of the field. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_open_drone_id_operator_id_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint8_t target_system,uint8_t target_component,const uint8_t *id_or_mac,uint8_t operator_id_type,const char *operator_id) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_OPEN_DRONE_ID_OPERATOR_ID_LEN]; + _mav_put_uint8_t(buf, 0, target_system); + _mav_put_uint8_t(buf, 1, target_component); + _mav_put_uint8_t(buf, 22, operator_id_type); + _mav_put_uint8_t_array(buf, 2, id_or_mac, 20); + _mav_put_char_array(buf, 23, operator_id, 20); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_OPEN_DRONE_ID_OPERATOR_ID_LEN); +#else + mavlink_open_drone_id_operator_id_t packet; + packet.target_system = target_system; + packet.target_component = target_component; + packet.operator_id_type = operator_id_type; + mav_array_memcpy(packet.id_or_mac, id_or_mac, sizeof(uint8_t)*20); + mav_array_memcpy(packet.operator_id, operator_id, sizeof(char)*20); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_OPEN_DRONE_ID_OPERATOR_ID_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_OPEN_DRONE_ID_OPERATOR_ID; + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_OPEN_DRONE_ID_OPERATOR_ID_MIN_LEN, MAVLINK_MSG_ID_OPEN_DRONE_ID_OPERATOR_ID_LEN, MAVLINK_MSG_ID_OPEN_DRONE_ID_OPERATOR_ID_CRC); +} + +/** + * @brief Encode a open_drone_id_operator_id struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param open_drone_id_operator_id C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_open_drone_id_operator_id_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_open_drone_id_operator_id_t* open_drone_id_operator_id) +{ + return mavlink_msg_open_drone_id_operator_id_pack(system_id, component_id, msg, open_drone_id_operator_id->target_system, open_drone_id_operator_id->target_component, open_drone_id_operator_id->id_or_mac, open_drone_id_operator_id->operator_id_type, open_drone_id_operator_id->operator_id); +} + +/** + * @brief Encode a open_drone_id_operator_id struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param open_drone_id_operator_id C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_open_drone_id_operator_id_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_open_drone_id_operator_id_t* open_drone_id_operator_id) +{ + return mavlink_msg_open_drone_id_operator_id_pack_chan(system_id, component_id, chan, msg, open_drone_id_operator_id->target_system, open_drone_id_operator_id->target_component, open_drone_id_operator_id->id_or_mac, open_drone_id_operator_id->operator_id_type, open_drone_id_operator_id->operator_id); +} + +/** + * @brief Send a open_drone_id_operator_id message + * @param chan MAVLink channel to send the message + * + * @param target_system System ID (0 for broadcast). + * @param target_component Component ID (0 for broadcast). + * @param id_or_mac Only used for drone ID data received from other UAs. See detailed description at https://mavlink.io/en/services/opendroneid.html. + * @param operator_id_type Indicates the type of the operator_id field. + * @param operator_id Text description or numeric value expressed as ASCII characters. Shall be filled with nulls in the unused portion of the field. + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_open_drone_id_operator_id_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, const uint8_t *id_or_mac, uint8_t operator_id_type, const char *operator_id) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_OPEN_DRONE_ID_OPERATOR_ID_LEN]; + _mav_put_uint8_t(buf, 0, target_system); + _mav_put_uint8_t(buf, 1, target_component); + _mav_put_uint8_t(buf, 22, operator_id_type); + _mav_put_uint8_t_array(buf, 2, id_or_mac, 20); + _mav_put_char_array(buf, 23, operator_id, 20); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OPEN_DRONE_ID_OPERATOR_ID, buf, MAVLINK_MSG_ID_OPEN_DRONE_ID_OPERATOR_ID_MIN_LEN, MAVLINK_MSG_ID_OPEN_DRONE_ID_OPERATOR_ID_LEN, MAVLINK_MSG_ID_OPEN_DRONE_ID_OPERATOR_ID_CRC); +#else + mavlink_open_drone_id_operator_id_t packet; + packet.target_system = target_system; + packet.target_component = target_component; + packet.operator_id_type = operator_id_type; + mav_array_memcpy(packet.id_or_mac, id_or_mac, sizeof(uint8_t)*20); + mav_array_memcpy(packet.operator_id, operator_id, sizeof(char)*20); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OPEN_DRONE_ID_OPERATOR_ID, (const char *)&packet, MAVLINK_MSG_ID_OPEN_DRONE_ID_OPERATOR_ID_MIN_LEN, MAVLINK_MSG_ID_OPEN_DRONE_ID_OPERATOR_ID_LEN, MAVLINK_MSG_ID_OPEN_DRONE_ID_OPERATOR_ID_CRC); +#endif +} + +/** + * @brief Send a open_drone_id_operator_id message + * @param chan MAVLink channel to send the message + * @param struct The MAVLink struct to serialize + */ +static inline void mavlink_msg_open_drone_id_operator_id_send_struct(mavlink_channel_t chan, const mavlink_open_drone_id_operator_id_t* open_drone_id_operator_id) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + mavlink_msg_open_drone_id_operator_id_send(chan, open_drone_id_operator_id->target_system, open_drone_id_operator_id->target_component, open_drone_id_operator_id->id_or_mac, open_drone_id_operator_id->operator_id_type, open_drone_id_operator_id->operator_id); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OPEN_DRONE_ID_OPERATOR_ID, (const char *)open_drone_id_operator_id, MAVLINK_MSG_ID_OPEN_DRONE_ID_OPERATOR_ID_MIN_LEN, MAVLINK_MSG_ID_OPEN_DRONE_ID_OPERATOR_ID_LEN, MAVLINK_MSG_ID_OPEN_DRONE_ID_OPERATOR_ID_CRC); +#endif +} + +#if MAVLINK_MSG_ID_OPEN_DRONE_ID_OPERATOR_ID_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_open_drone_id_operator_id_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, const uint8_t *id_or_mac, uint8_t operator_id_type, const char *operator_id) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint8_t(buf, 0, target_system); + _mav_put_uint8_t(buf, 1, target_component); + _mav_put_uint8_t(buf, 22, operator_id_type); + _mav_put_uint8_t_array(buf, 2, id_or_mac, 20); + _mav_put_char_array(buf, 23, operator_id, 20); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OPEN_DRONE_ID_OPERATOR_ID, buf, MAVLINK_MSG_ID_OPEN_DRONE_ID_OPERATOR_ID_MIN_LEN, MAVLINK_MSG_ID_OPEN_DRONE_ID_OPERATOR_ID_LEN, MAVLINK_MSG_ID_OPEN_DRONE_ID_OPERATOR_ID_CRC); +#else + mavlink_open_drone_id_operator_id_t *packet = (mavlink_open_drone_id_operator_id_t *)msgbuf; + packet->target_system = target_system; + packet->target_component = target_component; + packet->operator_id_type = operator_id_type; + mav_array_memcpy(packet->id_or_mac, id_or_mac, sizeof(uint8_t)*20); + mav_array_memcpy(packet->operator_id, operator_id, sizeof(char)*20); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OPEN_DRONE_ID_OPERATOR_ID, (const char *)packet, MAVLINK_MSG_ID_OPEN_DRONE_ID_OPERATOR_ID_MIN_LEN, MAVLINK_MSG_ID_OPEN_DRONE_ID_OPERATOR_ID_LEN, MAVLINK_MSG_ID_OPEN_DRONE_ID_OPERATOR_ID_CRC); +#endif +} +#endif + +#endif + +// MESSAGE OPEN_DRONE_ID_OPERATOR_ID UNPACKING + + +/** + * @brief Get field target_system from open_drone_id_operator_id message + * + * @return System ID (0 for broadcast). + */ +static inline uint8_t mavlink_msg_open_drone_id_operator_id_get_target_system(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 0); +} + +/** + * @brief Get field target_component from open_drone_id_operator_id message + * + * @return Component ID (0 for broadcast). + */ +static inline uint8_t mavlink_msg_open_drone_id_operator_id_get_target_component(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 1); +} + +/** + * @brief Get field id_or_mac from open_drone_id_operator_id message + * + * @return Only used for drone ID data received from other UAs. See detailed description at https://mavlink.io/en/services/opendroneid.html. + */ +static inline uint16_t mavlink_msg_open_drone_id_operator_id_get_id_or_mac(const mavlink_message_t* msg, uint8_t *id_or_mac) +{ + return _MAV_RETURN_uint8_t_array(msg, id_or_mac, 20, 2); +} + +/** + * @brief Get field operator_id_type from open_drone_id_operator_id message + * + * @return Indicates the type of the operator_id field. + */ +static inline uint8_t mavlink_msg_open_drone_id_operator_id_get_operator_id_type(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 22); +} + +/** + * @brief Get field operator_id from open_drone_id_operator_id message + * + * @return Text description or numeric value expressed as ASCII characters. Shall be filled with nulls in the unused portion of the field. + */ +static inline uint16_t mavlink_msg_open_drone_id_operator_id_get_operator_id(const mavlink_message_t* msg, char *operator_id) +{ + return _MAV_RETURN_char_array(msg, operator_id, 20, 23); +} + +/** + * @brief Decode a open_drone_id_operator_id message into a struct + * + * @param msg The message to decode + * @param open_drone_id_operator_id C-struct to decode the message contents into + */ +static inline void mavlink_msg_open_drone_id_operator_id_decode(const mavlink_message_t* msg, mavlink_open_drone_id_operator_id_t* open_drone_id_operator_id) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + open_drone_id_operator_id->target_system = mavlink_msg_open_drone_id_operator_id_get_target_system(msg); + open_drone_id_operator_id->target_component = mavlink_msg_open_drone_id_operator_id_get_target_component(msg); + mavlink_msg_open_drone_id_operator_id_get_id_or_mac(msg, open_drone_id_operator_id->id_or_mac); + open_drone_id_operator_id->operator_id_type = mavlink_msg_open_drone_id_operator_id_get_operator_id_type(msg); + mavlink_msg_open_drone_id_operator_id_get_operator_id(msg, open_drone_id_operator_id->operator_id); +#else + uint8_t len = msg->len < MAVLINK_MSG_ID_OPEN_DRONE_ID_OPERATOR_ID_LEN? msg->len : MAVLINK_MSG_ID_OPEN_DRONE_ID_OPERATOR_ID_LEN; + memset(open_drone_id_operator_id, 0, MAVLINK_MSG_ID_OPEN_DRONE_ID_OPERATOR_ID_LEN); + memcpy(open_drone_id_operator_id, _MAV_PAYLOAD(msg), len); +#endif +} diff --git a/lib/main/MAVLink/common/mavlink_msg_open_drone_id_self_id.h b/lib/main/MAVLink/common/mavlink_msg_open_drone_id_self_id.h new file mode 100644 index 0000000000..0750472ace --- /dev/null +++ b/lib/main/MAVLink/common/mavlink_msg_open_drone_id_self_id.h @@ -0,0 +1,306 @@ +#pragma once +// MESSAGE OPEN_DRONE_ID_SELF_ID PACKING + +#define MAVLINK_MSG_ID_OPEN_DRONE_ID_SELF_ID 12903 + + +typedef struct __mavlink_open_drone_id_self_id_t { + uint8_t target_system; /*< System ID (0 for broadcast).*/ + uint8_t target_component; /*< Component ID (0 for broadcast).*/ + uint8_t id_or_mac[20]; /*< Only used for drone ID data received from other UAs. See detailed description at https://mavlink.io/en/services/opendroneid.html. */ + uint8_t description_type; /*< Indicates the type of the description field.*/ + char description[23]; /*< Text description or numeric value expressed as ASCII characters. Shall be filled with nulls in the unused portion of the field.*/ +} mavlink_open_drone_id_self_id_t; + +#define MAVLINK_MSG_ID_OPEN_DRONE_ID_SELF_ID_LEN 46 +#define MAVLINK_MSG_ID_OPEN_DRONE_ID_SELF_ID_MIN_LEN 46 +#define MAVLINK_MSG_ID_12903_LEN 46 +#define MAVLINK_MSG_ID_12903_MIN_LEN 46 + +#define MAVLINK_MSG_ID_OPEN_DRONE_ID_SELF_ID_CRC 249 +#define MAVLINK_MSG_ID_12903_CRC 249 + +#define MAVLINK_MSG_OPEN_DRONE_ID_SELF_ID_FIELD_ID_OR_MAC_LEN 20 +#define MAVLINK_MSG_OPEN_DRONE_ID_SELF_ID_FIELD_DESCRIPTION_LEN 23 + +#if MAVLINK_COMMAND_24BIT +#define MAVLINK_MESSAGE_INFO_OPEN_DRONE_ID_SELF_ID { \ + 12903, \ + "OPEN_DRONE_ID_SELF_ID", \ + 5, \ + { { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 0, offsetof(mavlink_open_drone_id_self_id_t, target_system) }, \ + { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 1, offsetof(mavlink_open_drone_id_self_id_t, target_component) }, \ + { "id_or_mac", NULL, MAVLINK_TYPE_UINT8_T, 20, 2, offsetof(mavlink_open_drone_id_self_id_t, id_or_mac) }, \ + { "description_type", NULL, MAVLINK_TYPE_UINT8_T, 0, 22, offsetof(mavlink_open_drone_id_self_id_t, description_type) }, \ + { "description", NULL, MAVLINK_TYPE_CHAR, 23, 23, offsetof(mavlink_open_drone_id_self_id_t, description) }, \ + } \ +} +#else +#define MAVLINK_MESSAGE_INFO_OPEN_DRONE_ID_SELF_ID { \ + "OPEN_DRONE_ID_SELF_ID", \ + 5, \ + { { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 0, offsetof(mavlink_open_drone_id_self_id_t, target_system) }, \ + { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 1, offsetof(mavlink_open_drone_id_self_id_t, target_component) }, \ + { "id_or_mac", NULL, MAVLINK_TYPE_UINT8_T, 20, 2, offsetof(mavlink_open_drone_id_self_id_t, id_or_mac) }, \ + { "description_type", NULL, MAVLINK_TYPE_UINT8_T, 0, 22, offsetof(mavlink_open_drone_id_self_id_t, description_type) }, \ + { "description", NULL, MAVLINK_TYPE_CHAR, 23, 23, offsetof(mavlink_open_drone_id_self_id_t, description) }, \ + } \ +} +#endif + +/** + * @brief Pack a open_drone_id_self_id message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param target_system System ID (0 for broadcast). + * @param target_component Component ID (0 for broadcast). + * @param id_or_mac Only used for drone ID data received from other UAs. See detailed description at https://mavlink.io/en/services/opendroneid.html. + * @param description_type Indicates the type of the description field. + * @param description Text description or numeric value expressed as ASCII characters. Shall be filled with nulls in the unused portion of the field. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_open_drone_id_self_id_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint8_t target_system, uint8_t target_component, const uint8_t *id_or_mac, uint8_t description_type, const char *description) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_OPEN_DRONE_ID_SELF_ID_LEN]; + _mav_put_uint8_t(buf, 0, target_system); + _mav_put_uint8_t(buf, 1, target_component); + _mav_put_uint8_t(buf, 22, description_type); + _mav_put_uint8_t_array(buf, 2, id_or_mac, 20); + _mav_put_char_array(buf, 23, description, 23); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_OPEN_DRONE_ID_SELF_ID_LEN); +#else + mavlink_open_drone_id_self_id_t packet; + packet.target_system = target_system; + packet.target_component = target_component; + packet.description_type = description_type; + mav_array_memcpy(packet.id_or_mac, id_or_mac, sizeof(uint8_t)*20); + mav_array_memcpy(packet.description, description, sizeof(char)*23); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_OPEN_DRONE_ID_SELF_ID_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_OPEN_DRONE_ID_SELF_ID; + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_OPEN_DRONE_ID_SELF_ID_MIN_LEN, MAVLINK_MSG_ID_OPEN_DRONE_ID_SELF_ID_LEN, MAVLINK_MSG_ID_OPEN_DRONE_ID_SELF_ID_CRC); +} + +/** + * @brief Pack a open_drone_id_self_id message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param target_system System ID (0 for broadcast). + * @param target_component Component ID (0 for broadcast). + * @param id_or_mac Only used for drone ID data received from other UAs. See detailed description at https://mavlink.io/en/services/opendroneid.html. + * @param description_type Indicates the type of the description field. + * @param description Text description or numeric value expressed as ASCII characters. Shall be filled with nulls in the unused portion of the field. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_open_drone_id_self_id_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint8_t target_system,uint8_t target_component,const uint8_t *id_or_mac,uint8_t description_type,const char *description) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_OPEN_DRONE_ID_SELF_ID_LEN]; + _mav_put_uint8_t(buf, 0, target_system); + _mav_put_uint8_t(buf, 1, target_component); + _mav_put_uint8_t(buf, 22, description_type); + _mav_put_uint8_t_array(buf, 2, id_or_mac, 20); + _mav_put_char_array(buf, 23, description, 23); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_OPEN_DRONE_ID_SELF_ID_LEN); +#else + mavlink_open_drone_id_self_id_t packet; + packet.target_system = target_system; + packet.target_component = target_component; + packet.description_type = description_type; + mav_array_memcpy(packet.id_or_mac, id_or_mac, sizeof(uint8_t)*20); + mav_array_memcpy(packet.description, description, sizeof(char)*23); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_OPEN_DRONE_ID_SELF_ID_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_OPEN_DRONE_ID_SELF_ID; + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_OPEN_DRONE_ID_SELF_ID_MIN_LEN, MAVLINK_MSG_ID_OPEN_DRONE_ID_SELF_ID_LEN, MAVLINK_MSG_ID_OPEN_DRONE_ID_SELF_ID_CRC); +} + +/** + * @brief Encode a open_drone_id_self_id struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param open_drone_id_self_id C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_open_drone_id_self_id_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_open_drone_id_self_id_t* open_drone_id_self_id) +{ + return mavlink_msg_open_drone_id_self_id_pack(system_id, component_id, msg, open_drone_id_self_id->target_system, open_drone_id_self_id->target_component, open_drone_id_self_id->id_or_mac, open_drone_id_self_id->description_type, open_drone_id_self_id->description); +} + +/** + * @brief Encode a open_drone_id_self_id struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param open_drone_id_self_id C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_open_drone_id_self_id_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_open_drone_id_self_id_t* open_drone_id_self_id) +{ + return mavlink_msg_open_drone_id_self_id_pack_chan(system_id, component_id, chan, msg, open_drone_id_self_id->target_system, open_drone_id_self_id->target_component, open_drone_id_self_id->id_or_mac, open_drone_id_self_id->description_type, open_drone_id_self_id->description); +} + +/** + * @brief Send a open_drone_id_self_id message + * @param chan MAVLink channel to send the message + * + * @param target_system System ID (0 for broadcast). + * @param target_component Component ID (0 for broadcast). + * @param id_or_mac Only used for drone ID data received from other UAs. See detailed description at https://mavlink.io/en/services/opendroneid.html. + * @param description_type Indicates the type of the description field. + * @param description Text description or numeric value expressed as ASCII characters. Shall be filled with nulls in the unused portion of the field. + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_open_drone_id_self_id_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, const uint8_t *id_or_mac, uint8_t description_type, const char *description) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_OPEN_DRONE_ID_SELF_ID_LEN]; + _mav_put_uint8_t(buf, 0, target_system); + _mav_put_uint8_t(buf, 1, target_component); + _mav_put_uint8_t(buf, 22, description_type); + _mav_put_uint8_t_array(buf, 2, id_or_mac, 20); + _mav_put_char_array(buf, 23, description, 23); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OPEN_DRONE_ID_SELF_ID, buf, MAVLINK_MSG_ID_OPEN_DRONE_ID_SELF_ID_MIN_LEN, MAVLINK_MSG_ID_OPEN_DRONE_ID_SELF_ID_LEN, MAVLINK_MSG_ID_OPEN_DRONE_ID_SELF_ID_CRC); +#else + mavlink_open_drone_id_self_id_t packet; + packet.target_system = target_system; + packet.target_component = target_component; + packet.description_type = description_type; + mav_array_memcpy(packet.id_or_mac, id_or_mac, sizeof(uint8_t)*20); + mav_array_memcpy(packet.description, description, sizeof(char)*23); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OPEN_DRONE_ID_SELF_ID, (const char *)&packet, MAVLINK_MSG_ID_OPEN_DRONE_ID_SELF_ID_MIN_LEN, MAVLINK_MSG_ID_OPEN_DRONE_ID_SELF_ID_LEN, MAVLINK_MSG_ID_OPEN_DRONE_ID_SELF_ID_CRC); +#endif +} + +/** + * @brief Send a open_drone_id_self_id message + * @param chan MAVLink channel to send the message + * @param struct The MAVLink struct to serialize + */ +static inline void mavlink_msg_open_drone_id_self_id_send_struct(mavlink_channel_t chan, const mavlink_open_drone_id_self_id_t* open_drone_id_self_id) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + mavlink_msg_open_drone_id_self_id_send(chan, open_drone_id_self_id->target_system, open_drone_id_self_id->target_component, open_drone_id_self_id->id_or_mac, open_drone_id_self_id->description_type, open_drone_id_self_id->description); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OPEN_DRONE_ID_SELF_ID, (const char *)open_drone_id_self_id, MAVLINK_MSG_ID_OPEN_DRONE_ID_SELF_ID_MIN_LEN, MAVLINK_MSG_ID_OPEN_DRONE_ID_SELF_ID_LEN, MAVLINK_MSG_ID_OPEN_DRONE_ID_SELF_ID_CRC); +#endif +} + +#if MAVLINK_MSG_ID_OPEN_DRONE_ID_SELF_ID_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_open_drone_id_self_id_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, const uint8_t *id_or_mac, uint8_t description_type, const char *description) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint8_t(buf, 0, target_system); + _mav_put_uint8_t(buf, 1, target_component); + _mav_put_uint8_t(buf, 22, description_type); + _mav_put_uint8_t_array(buf, 2, id_or_mac, 20); + _mav_put_char_array(buf, 23, description, 23); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OPEN_DRONE_ID_SELF_ID, buf, MAVLINK_MSG_ID_OPEN_DRONE_ID_SELF_ID_MIN_LEN, MAVLINK_MSG_ID_OPEN_DRONE_ID_SELF_ID_LEN, MAVLINK_MSG_ID_OPEN_DRONE_ID_SELF_ID_CRC); +#else + mavlink_open_drone_id_self_id_t *packet = (mavlink_open_drone_id_self_id_t *)msgbuf; + packet->target_system = target_system; + packet->target_component = target_component; + packet->description_type = description_type; + mav_array_memcpy(packet->id_or_mac, id_or_mac, sizeof(uint8_t)*20); + mav_array_memcpy(packet->description, description, sizeof(char)*23); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OPEN_DRONE_ID_SELF_ID, (const char *)packet, MAVLINK_MSG_ID_OPEN_DRONE_ID_SELF_ID_MIN_LEN, MAVLINK_MSG_ID_OPEN_DRONE_ID_SELF_ID_LEN, MAVLINK_MSG_ID_OPEN_DRONE_ID_SELF_ID_CRC); +#endif +} +#endif + +#endif + +// MESSAGE OPEN_DRONE_ID_SELF_ID UNPACKING + + +/** + * @brief Get field target_system from open_drone_id_self_id message + * + * @return System ID (0 for broadcast). + */ +static inline uint8_t mavlink_msg_open_drone_id_self_id_get_target_system(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 0); +} + +/** + * @brief Get field target_component from open_drone_id_self_id message + * + * @return Component ID (0 for broadcast). + */ +static inline uint8_t mavlink_msg_open_drone_id_self_id_get_target_component(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 1); +} + +/** + * @brief Get field id_or_mac from open_drone_id_self_id message + * + * @return Only used for drone ID data received from other UAs. See detailed description at https://mavlink.io/en/services/opendroneid.html. + */ +static inline uint16_t mavlink_msg_open_drone_id_self_id_get_id_or_mac(const mavlink_message_t* msg, uint8_t *id_or_mac) +{ + return _MAV_RETURN_uint8_t_array(msg, id_or_mac, 20, 2); +} + +/** + * @brief Get field description_type from open_drone_id_self_id message + * + * @return Indicates the type of the description field. + */ +static inline uint8_t mavlink_msg_open_drone_id_self_id_get_description_type(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 22); +} + +/** + * @brief Get field description from open_drone_id_self_id message + * + * @return Text description or numeric value expressed as ASCII characters. Shall be filled with nulls in the unused portion of the field. + */ +static inline uint16_t mavlink_msg_open_drone_id_self_id_get_description(const mavlink_message_t* msg, char *description) +{ + return _MAV_RETURN_char_array(msg, description, 23, 23); +} + +/** + * @brief Decode a open_drone_id_self_id message into a struct + * + * @param msg The message to decode + * @param open_drone_id_self_id C-struct to decode the message contents into + */ +static inline void mavlink_msg_open_drone_id_self_id_decode(const mavlink_message_t* msg, mavlink_open_drone_id_self_id_t* open_drone_id_self_id) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + open_drone_id_self_id->target_system = mavlink_msg_open_drone_id_self_id_get_target_system(msg); + open_drone_id_self_id->target_component = mavlink_msg_open_drone_id_self_id_get_target_component(msg); + mavlink_msg_open_drone_id_self_id_get_id_or_mac(msg, open_drone_id_self_id->id_or_mac); + open_drone_id_self_id->description_type = mavlink_msg_open_drone_id_self_id_get_description_type(msg); + mavlink_msg_open_drone_id_self_id_get_description(msg, open_drone_id_self_id->description); +#else + uint8_t len = msg->len < MAVLINK_MSG_ID_OPEN_DRONE_ID_SELF_ID_LEN? msg->len : MAVLINK_MSG_ID_OPEN_DRONE_ID_SELF_ID_LEN; + memset(open_drone_id_self_id, 0, MAVLINK_MSG_ID_OPEN_DRONE_ID_SELF_ID_LEN); + memcpy(open_drone_id_self_id, _MAV_PAYLOAD(msg), len); +#endif +} diff --git a/lib/main/MAVLink/common/mavlink_msg_open_drone_id_system.h b/lib/main/MAVLink/common/mavlink_msg_open_drone_id_system.h new file mode 100644 index 0000000000..2af999649f --- /dev/null +++ b/lib/main/MAVLink/common/mavlink_msg_open_drone_id_system.h @@ -0,0 +1,505 @@ +#pragma once +// MESSAGE OPEN_DRONE_ID_SYSTEM PACKING + +#define MAVLINK_MSG_ID_OPEN_DRONE_ID_SYSTEM 12904 + + +typedef struct __mavlink_open_drone_id_system_t { + int32_t operator_latitude; /*< [degE7] Latitude of the operator. If unknown: 0 (both Lat/Lon).*/ + int32_t operator_longitude; /*< [degE7] Longitude of the operator. If unknown: 0 (both Lat/Lon).*/ + float area_ceiling; /*< [m] Area Operations Ceiling relative to WGS84. If unknown: -1000 m.*/ + float area_floor; /*< [m] Area Operations Floor relative to WGS84. If unknown: -1000 m.*/ + uint16_t area_count; /*< Number of aircraft in the area, group or formation (default 1).*/ + uint16_t area_radius; /*< [m] Radius of the cylindrical area of the group or formation (default 0).*/ + uint8_t target_system; /*< System ID (0 for broadcast).*/ + uint8_t target_component; /*< Component ID (0 for broadcast).*/ + uint8_t id_or_mac[20]; /*< Only used for drone ID data received from other UAs. See detailed description at https://mavlink.io/en/services/opendroneid.html. */ + uint8_t operator_location_type; /*< Specifies the operator location type.*/ + uint8_t classification_type; /*< Specifies the classification type of the UA.*/ + uint8_t category_eu; /*< When classification_type is MAV_ODID_CLASSIFICATION_TYPE_EU, specifies the category of the UA.*/ + uint8_t class_eu; /*< When classification_type is MAV_ODID_CLASSIFICATION_TYPE_EU, specifies the class of the UA.*/ +} mavlink_open_drone_id_system_t; + +#define MAVLINK_MSG_ID_OPEN_DRONE_ID_SYSTEM_LEN 46 +#define MAVLINK_MSG_ID_OPEN_DRONE_ID_SYSTEM_MIN_LEN 46 +#define MAVLINK_MSG_ID_12904_LEN 46 +#define MAVLINK_MSG_ID_12904_MIN_LEN 46 + +#define MAVLINK_MSG_ID_OPEN_DRONE_ID_SYSTEM_CRC 203 +#define MAVLINK_MSG_ID_12904_CRC 203 + +#define MAVLINK_MSG_OPEN_DRONE_ID_SYSTEM_FIELD_ID_OR_MAC_LEN 20 + +#if MAVLINK_COMMAND_24BIT +#define MAVLINK_MESSAGE_INFO_OPEN_DRONE_ID_SYSTEM { \ + 12904, \ + "OPEN_DRONE_ID_SYSTEM", \ + 13, \ + { { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 20, offsetof(mavlink_open_drone_id_system_t, target_system) }, \ + { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 21, offsetof(mavlink_open_drone_id_system_t, target_component) }, \ + { "id_or_mac", NULL, MAVLINK_TYPE_UINT8_T, 20, 22, offsetof(mavlink_open_drone_id_system_t, id_or_mac) }, \ + { "operator_location_type", NULL, MAVLINK_TYPE_UINT8_T, 0, 42, offsetof(mavlink_open_drone_id_system_t, operator_location_type) }, \ + { "classification_type", NULL, MAVLINK_TYPE_UINT8_T, 0, 43, offsetof(mavlink_open_drone_id_system_t, classification_type) }, \ + { "operator_latitude", NULL, MAVLINK_TYPE_INT32_T, 0, 0, offsetof(mavlink_open_drone_id_system_t, operator_latitude) }, \ + { "operator_longitude", NULL, MAVLINK_TYPE_INT32_T, 0, 4, offsetof(mavlink_open_drone_id_system_t, operator_longitude) }, \ + { "area_count", NULL, MAVLINK_TYPE_UINT16_T, 0, 16, offsetof(mavlink_open_drone_id_system_t, area_count) }, \ + { "area_radius", NULL, MAVLINK_TYPE_UINT16_T, 0, 18, offsetof(mavlink_open_drone_id_system_t, area_radius) }, \ + { "area_ceiling", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_open_drone_id_system_t, area_ceiling) }, \ + { "area_floor", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_open_drone_id_system_t, area_floor) }, \ + { "category_eu", NULL, MAVLINK_TYPE_UINT8_T, 0, 44, offsetof(mavlink_open_drone_id_system_t, category_eu) }, \ + { "class_eu", NULL, MAVLINK_TYPE_UINT8_T, 0, 45, offsetof(mavlink_open_drone_id_system_t, class_eu) }, \ + } \ +} +#else +#define MAVLINK_MESSAGE_INFO_OPEN_DRONE_ID_SYSTEM { \ + "OPEN_DRONE_ID_SYSTEM", \ + 13, \ + { { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 20, offsetof(mavlink_open_drone_id_system_t, target_system) }, \ + { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 21, offsetof(mavlink_open_drone_id_system_t, target_component) }, \ + { "id_or_mac", NULL, MAVLINK_TYPE_UINT8_T, 20, 22, offsetof(mavlink_open_drone_id_system_t, id_or_mac) }, \ + { "operator_location_type", NULL, MAVLINK_TYPE_UINT8_T, 0, 42, offsetof(mavlink_open_drone_id_system_t, operator_location_type) }, \ + { "classification_type", NULL, MAVLINK_TYPE_UINT8_T, 0, 43, offsetof(mavlink_open_drone_id_system_t, classification_type) }, \ + { "operator_latitude", NULL, MAVLINK_TYPE_INT32_T, 0, 0, offsetof(mavlink_open_drone_id_system_t, operator_latitude) }, \ + { "operator_longitude", NULL, MAVLINK_TYPE_INT32_T, 0, 4, offsetof(mavlink_open_drone_id_system_t, operator_longitude) }, \ + { "area_count", NULL, MAVLINK_TYPE_UINT16_T, 0, 16, offsetof(mavlink_open_drone_id_system_t, area_count) }, \ + { "area_radius", NULL, MAVLINK_TYPE_UINT16_T, 0, 18, offsetof(mavlink_open_drone_id_system_t, area_radius) }, \ + { "area_ceiling", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_open_drone_id_system_t, area_ceiling) }, \ + { "area_floor", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_open_drone_id_system_t, area_floor) }, \ + { "category_eu", NULL, MAVLINK_TYPE_UINT8_T, 0, 44, offsetof(mavlink_open_drone_id_system_t, category_eu) }, \ + { "class_eu", NULL, MAVLINK_TYPE_UINT8_T, 0, 45, offsetof(mavlink_open_drone_id_system_t, class_eu) }, \ + } \ +} +#endif + +/** + * @brief Pack a open_drone_id_system message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param target_system System ID (0 for broadcast). + * @param target_component Component ID (0 for broadcast). + * @param id_or_mac Only used for drone ID data received from other UAs. See detailed description at https://mavlink.io/en/services/opendroneid.html. + * @param operator_location_type Specifies the operator location type. + * @param classification_type Specifies the classification type of the UA. + * @param operator_latitude [degE7] Latitude of the operator. If unknown: 0 (both Lat/Lon). + * @param operator_longitude [degE7] Longitude of the operator. If unknown: 0 (both Lat/Lon). + * @param area_count Number of aircraft in the area, group or formation (default 1). + * @param area_radius [m] Radius of the cylindrical area of the group or formation (default 0). + * @param area_ceiling [m] Area Operations Ceiling relative to WGS84. If unknown: -1000 m. + * @param area_floor [m] Area Operations Floor relative to WGS84. If unknown: -1000 m. + * @param category_eu When classification_type is MAV_ODID_CLASSIFICATION_TYPE_EU, specifies the category of the UA. + * @param class_eu When classification_type is MAV_ODID_CLASSIFICATION_TYPE_EU, specifies the class of the UA. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_open_drone_id_system_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint8_t target_system, uint8_t target_component, const uint8_t *id_or_mac, uint8_t operator_location_type, uint8_t classification_type, int32_t operator_latitude, int32_t operator_longitude, uint16_t area_count, uint16_t area_radius, float area_ceiling, float area_floor, uint8_t category_eu, uint8_t class_eu) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_OPEN_DRONE_ID_SYSTEM_LEN]; + _mav_put_int32_t(buf, 0, operator_latitude); + _mav_put_int32_t(buf, 4, operator_longitude); + _mav_put_float(buf, 8, area_ceiling); + _mav_put_float(buf, 12, area_floor); + _mav_put_uint16_t(buf, 16, area_count); + _mav_put_uint16_t(buf, 18, area_radius); + _mav_put_uint8_t(buf, 20, target_system); + _mav_put_uint8_t(buf, 21, target_component); + _mav_put_uint8_t(buf, 42, operator_location_type); + _mav_put_uint8_t(buf, 43, classification_type); + _mav_put_uint8_t(buf, 44, category_eu); + _mav_put_uint8_t(buf, 45, class_eu); + _mav_put_uint8_t_array(buf, 22, id_or_mac, 20); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_OPEN_DRONE_ID_SYSTEM_LEN); +#else + mavlink_open_drone_id_system_t packet; + packet.operator_latitude = operator_latitude; + packet.operator_longitude = operator_longitude; + packet.area_ceiling = area_ceiling; + packet.area_floor = area_floor; + packet.area_count = area_count; + packet.area_radius = area_radius; + packet.target_system = target_system; + packet.target_component = target_component; + packet.operator_location_type = operator_location_type; + packet.classification_type = classification_type; + packet.category_eu = category_eu; + packet.class_eu = class_eu; + mav_array_memcpy(packet.id_or_mac, id_or_mac, sizeof(uint8_t)*20); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_OPEN_DRONE_ID_SYSTEM_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_OPEN_DRONE_ID_SYSTEM; + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_OPEN_DRONE_ID_SYSTEM_MIN_LEN, MAVLINK_MSG_ID_OPEN_DRONE_ID_SYSTEM_LEN, MAVLINK_MSG_ID_OPEN_DRONE_ID_SYSTEM_CRC); +} + +/** + * @brief Pack a open_drone_id_system message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param target_system System ID (0 for broadcast). + * @param target_component Component ID (0 for broadcast). + * @param id_or_mac Only used for drone ID data received from other UAs. See detailed description at https://mavlink.io/en/services/opendroneid.html. + * @param operator_location_type Specifies the operator location type. + * @param classification_type Specifies the classification type of the UA. + * @param operator_latitude [degE7] Latitude of the operator. If unknown: 0 (both Lat/Lon). + * @param operator_longitude [degE7] Longitude of the operator. If unknown: 0 (both Lat/Lon). + * @param area_count Number of aircraft in the area, group or formation (default 1). + * @param area_radius [m] Radius of the cylindrical area of the group or formation (default 0). + * @param area_ceiling [m] Area Operations Ceiling relative to WGS84. If unknown: -1000 m. + * @param area_floor [m] Area Operations Floor relative to WGS84. If unknown: -1000 m. + * @param category_eu When classification_type is MAV_ODID_CLASSIFICATION_TYPE_EU, specifies the category of the UA. + * @param class_eu When classification_type is MAV_ODID_CLASSIFICATION_TYPE_EU, specifies the class of the UA. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_open_drone_id_system_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint8_t target_system,uint8_t target_component,const uint8_t *id_or_mac,uint8_t operator_location_type,uint8_t classification_type,int32_t operator_latitude,int32_t operator_longitude,uint16_t area_count,uint16_t area_radius,float area_ceiling,float area_floor,uint8_t category_eu,uint8_t class_eu) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_OPEN_DRONE_ID_SYSTEM_LEN]; + _mav_put_int32_t(buf, 0, operator_latitude); + _mav_put_int32_t(buf, 4, operator_longitude); + _mav_put_float(buf, 8, area_ceiling); + _mav_put_float(buf, 12, area_floor); + _mav_put_uint16_t(buf, 16, area_count); + _mav_put_uint16_t(buf, 18, area_radius); + _mav_put_uint8_t(buf, 20, target_system); + _mav_put_uint8_t(buf, 21, target_component); + _mav_put_uint8_t(buf, 42, operator_location_type); + _mav_put_uint8_t(buf, 43, classification_type); + _mav_put_uint8_t(buf, 44, category_eu); + _mav_put_uint8_t(buf, 45, class_eu); + _mav_put_uint8_t_array(buf, 22, id_or_mac, 20); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_OPEN_DRONE_ID_SYSTEM_LEN); +#else + mavlink_open_drone_id_system_t packet; + packet.operator_latitude = operator_latitude; + packet.operator_longitude = operator_longitude; + packet.area_ceiling = area_ceiling; + packet.area_floor = area_floor; + packet.area_count = area_count; + packet.area_radius = area_radius; + packet.target_system = target_system; + packet.target_component = target_component; + packet.operator_location_type = operator_location_type; + packet.classification_type = classification_type; + packet.category_eu = category_eu; + packet.class_eu = class_eu; + mav_array_memcpy(packet.id_or_mac, id_or_mac, sizeof(uint8_t)*20); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_OPEN_DRONE_ID_SYSTEM_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_OPEN_DRONE_ID_SYSTEM; + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_OPEN_DRONE_ID_SYSTEM_MIN_LEN, MAVLINK_MSG_ID_OPEN_DRONE_ID_SYSTEM_LEN, MAVLINK_MSG_ID_OPEN_DRONE_ID_SYSTEM_CRC); +} + +/** + * @brief Encode a open_drone_id_system struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param open_drone_id_system C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_open_drone_id_system_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_open_drone_id_system_t* open_drone_id_system) +{ + return mavlink_msg_open_drone_id_system_pack(system_id, component_id, msg, open_drone_id_system->target_system, open_drone_id_system->target_component, open_drone_id_system->id_or_mac, open_drone_id_system->operator_location_type, open_drone_id_system->classification_type, open_drone_id_system->operator_latitude, open_drone_id_system->operator_longitude, open_drone_id_system->area_count, open_drone_id_system->area_radius, open_drone_id_system->area_ceiling, open_drone_id_system->area_floor, open_drone_id_system->category_eu, open_drone_id_system->class_eu); +} + +/** + * @brief Encode a open_drone_id_system struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param open_drone_id_system C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_open_drone_id_system_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_open_drone_id_system_t* open_drone_id_system) +{ + return mavlink_msg_open_drone_id_system_pack_chan(system_id, component_id, chan, msg, open_drone_id_system->target_system, open_drone_id_system->target_component, open_drone_id_system->id_or_mac, open_drone_id_system->operator_location_type, open_drone_id_system->classification_type, open_drone_id_system->operator_latitude, open_drone_id_system->operator_longitude, open_drone_id_system->area_count, open_drone_id_system->area_radius, open_drone_id_system->area_ceiling, open_drone_id_system->area_floor, open_drone_id_system->category_eu, open_drone_id_system->class_eu); +} + +/** + * @brief Send a open_drone_id_system message + * @param chan MAVLink channel to send the message + * + * @param target_system System ID (0 for broadcast). + * @param target_component Component ID (0 for broadcast). + * @param id_or_mac Only used for drone ID data received from other UAs. See detailed description at https://mavlink.io/en/services/opendroneid.html. + * @param operator_location_type Specifies the operator location type. + * @param classification_type Specifies the classification type of the UA. + * @param operator_latitude [degE7] Latitude of the operator. If unknown: 0 (both Lat/Lon). + * @param operator_longitude [degE7] Longitude of the operator. If unknown: 0 (both Lat/Lon). + * @param area_count Number of aircraft in the area, group or formation (default 1). + * @param area_radius [m] Radius of the cylindrical area of the group or formation (default 0). + * @param area_ceiling [m] Area Operations Ceiling relative to WGS84. If unknown: -1000 m. + * @param area_floor [m] Area Operations Floor relative to WGS84. If unknown: -1000 m. + * @param category_eu When classification_type is MAV_ODID_CLASSIFICATION_TYPE_EU, specifies the category of the UA. + * @param class_eu When classification_type is MAV_ODID_CLASSIFICATION_TYPE_EU, specifies the class of the UA. + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_open_drone_id_system_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, const uint8_t *id_or_mac, uint8_t operator_location_type, uint8_t classification_type, int32_t operator_latitude, int32_t operator_longitude, uint16_t area_count, uint16_t area_radius, float area_ceiling, float area_floor, uint8_t category_eu, uint8_t class_eu) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_OPEN_DRONE_ID_SYSTEM_LEN]; + _mav_put_int32_t(buf, 0, operator_latitude); + _mav_put_int32_t(buf, 4, operator_longitude); + _mav_put_float(buf, 8, area_ceiling); + _mav_put_float(buf, 12, area_floor); + _mav_put_uint16_t(buf, 16, area_count); + _mav_put_uint16_t(buf, 18, area_radius); + _mav_put_uint8_t(buf, 20, target_system); + _mav_put_uint8_t(buf, 21, target_component); + _mav_put_uint8_t(buf, 42, operator_location_type); + _mav_put_uint8_t(buf, 43, classification_type); + _mav_put_uint8_t(buf, 44, category_eu); + _mav_put_uint8_t(buf, 45, class_eu); + _mav_put_uint8_t_array(buf, 22, id_or_mac, 20); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OPEN_DRONE_ID_SYSTEM, buf, MAVLINK_MSG_ID_OPEN_DRONE_ID_SYSTEM_MIN_LEN, MAVLINK_MSG_ID_OPEN_DRONE_ID_SYSTEM_LEN, MAVLINK_MSG_ID_OPEN_DRONE_ID_SYSTEM_CRC); +#else + mavlink_open_drone_id_system_t packet; + packet.operator_latitude = operator_latitude; + packet.operator_longitude = operator_longitude; + packet.area_ceiling = area_ceiling; + packet.area_floor = area_floor; + packet.area_count = area_count; + packet.area_radius = area_radius; + packet.target_system = target_system; + packet.target_component = target_component; + packet.operator_location_type = operator_location_type; + packet.classification_type = classification_type; + packet.category_eu = category_eu; + packet.class_eu = class_eu; + mav_array_memcpy(packet.id_or_mac, id_or_mac, sizeof(uint8_t)*20); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OPEN_DRONE_ID_SYSTEM, (const char *)&packet, MAVLINK_MSG_ID_OPEN_DRONE_ID_SYSTEM_MIN_LEN, MAVLINK_MSG_ID_OPEN_DRONE_ID_SYSTEM_LEN, MAVLINK_MSG_ID_OPEN_DRONE_ID_SYSTEM_CRC); +#endif +} + +/** + * @brief Send a open_drone_id_system message + * @param chan MAVLink channel to send the message + * @param struct The MAVLink struct to serialize + */ +static inline void mavlink_msg_open_drone_id_system_send_struct(mavlink_channel_t chan, const mavlink_open_drone_id_system_t* open_drone_id_system) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + mavlink_msg_open_drone_id_system_send(chan, open_drone_id_system->target_system, open_drone_id_system->target_component, open_drone_id_system->id_or_mac, open_drone_id_system->operator_location_type, open_drone_id_system->classification_type, open_drone_id_system->operator_latitude, open_drone_id_system->operator_longitude, open_drone_id_system->area_count, open_drone_id_system->area_radius, open_drone_id_system->area_ceiling, open_drone_id_system->area_floor, open_drone_id_system->category_eu, open_drone_id_system->class_eu); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OPEN_DRONE_ID_SYSTEM, (const char *)open_drone_id_system, MAVLINK_MSG_ID_OPEN_DRONE_ID_SYSTEM_MIN_LEN, MAVLINK_MSG_ID_OPEN_DRONE_ID_SYSTEM_LEN, MAVLINK_MSG_ID_OPEN_DRONE_ID_SYSTEM_CRC); +#endif +} + +#if MAVLINK_MSG_ID_OPEN_DRONE_ID_SYSTEM_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_open_drone_id_system_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, const uint8_t *id_or_mac, uint8_t operator_location_type, uint8_t classification_type, int32_t operator_latitude, int32_t operator_longitude, uint16_t area_count, uint16_t area_radius, float area_ceiling, float area_floor, uint8_t category_eu, uint8_t class_eu) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_int32_t(buf, 0, operator_latitude); + _mav_put_int32_t(buf, 4, operator_longitude); + _mav_put_float(buf, 8, area_ceiling); + _mav_put_float(buf, 12, area_floor); + _mav_put_uint16_t(buf, 16, area_count); + _mav_put_uint16_t(buf, 18, area_radius); + _mav_put_uint8_t(buf, 20, target_system); + _mav_put_uint8_t(buf, 21, target_component); + _mav_put_uint8_t(buf, 42, operator_location_type); + _mav_put_uint8_t(buf, 43, classification_type); + _mav_put_uint8_t(buf, 44, category_eu); + _mav_put_uint8_t(buf, 45, class_eu); + _mav_put_uint8_t_array(buf, 22, id_or_mac, 20); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OPEN_DRONE_ID_SYSTEM, buf, MAVLINK_MSG_ID_OPEN_DRONE_ID_SYSTEM_MIN_LEN, MAVLINK_MSG_ID_OPEN_DRONE_ID_SYSTEM_LEN, MAVLINK_MSG_ID_OPEN_DRONE_ID_SYSTEM_CRC); +#else + mavlink_open_drone_id_system_t *packet = (mavlink_open_drone_id_system_t *)msgbuf; + packet->operator_latitude = operator_latitude; + packet->operator_longitude = operator_longitude; + packet->area_ceiling = area_ceiling; + packet->area_floor = area_floor; + packet->area_count = area_count; + packet->area_radius = area_radius; + packet->target_system = target_system; + packet->target_component = target_component; + packet->operator_location_type = operator_location_type; + packet->classification_type = classification_type; + packet->category_eu = category_eu; + packet->class_eu = class_eu; + mav_array_memcpy(packet->id_or_mac, id_or_mac, sizeof(uint8_t)*20); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OPEN_DRONE_ID_SYSTEM, (const char *)packet, MAVLINK_MSG_ID_OPEN_DRONE_ID_SYSTEM_MIN_LEN, MAVLINK_MSG_ID_OPEN_DRONE_ID_SYSTEM_LEN, MAVLINK_MSG_ID_OPEN_DRONE_ID_SYSTEM_CRC); +#endif +} +#endif + +#endif + +// MESSAGE OPEN_DRONE_ID_SYSTEM UNPACKING + + +/** + * @brief Get field target_system from open_drone_id_system message + * + * @return System ID (0 for broadcast). + */ +static inline uint8_t mavlink_msg_open_drone_id_system_get_target_system(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 20); +} + +/** + * @brief Get field target_component from open_drone_id_system message + * + * @return Component ID (0 for broadcast). + */ +static inline uint8_t mavlink_msg_open_drone_id_system_get_target_component(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 21); +} + +/** + * @brief Get field id_or_mac from open_drone_id_system message + * + * @return Only used for drone ID data received from other UAs. See detailed description at https://mavlink.io/en/services/opendroneid.html. + */ +static inline uint16_t mavlink_msg_open_drone_id_system_get_id_or_mac(const mavlink_message_t* msg, uint8_t *id_or_mac) +{ + return _MAV_RETURN_uint8_t_array(msg, id_or_mac, 20, 22); +} + +/** + * @brief Get field operator_location_type from open_drone_id_system message + * + * @return Specifies the operator location type. + */ +static inline uint8_t mavlink_msg_open_drone_id_system_get_operator_location_type(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 42); +} + +/** + * @brief Get field classification_type from open_drone_id_system message + * + * @return Specifies the classification type of the UA. + */ +static inline uint8_t mavlink_msg_open_drone_id_system_get_classification_type(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 43); +} + +/** + * @brief Get field operator_latitude from open_drone_id_system message + * + * @return [degE7] Latitude of the operator. If unknown: 0 (both Lat/Lon). + */ +static inline int32_t mavlink_msg_open_drone_id_system_get_operator_latitude(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int32_t(msg, 0); +} + +/** + * @brief Get field operator_longitude from open_drone_id_system message + * + * @return [degE7] Longitude of the operator. If unknown: 0 (both Lat/Lon). + */ +static inline int32_t mavlink_msg_open_drone_id_system_get_operator_longitude(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int32_t(msg, 4); +} + +/** + * @brief Get field area_count from open_drone_id_system message + * + * @return Number of aircraft in the area, group or formation (default 1). + */ +static inline uint16_t mavlink_msg_open_drone_id_system_get_area_count(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 16); +} + +/** + * @brief Get field area_radius from open_drone_id_system message + * + * @return [m] Radius of the cylindrical area of the group or formation (default 0). + */ +static inline uint16_t mavlink_msg_open_drone_id_system_get_area_radius(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 18); +} + +/** + * @brief Get field area_ceiling from open_drone_id_system message + * + * @return [m] Area Operations Ceiling relative to WGS84. If unknown: -1000 m. + */ +static inline float mavlink_msg_open_drone_id_system_get_area_ceiling(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 8); +} + +/** + * @brief Get field area_floor from open_drone_id_system message + * + * @return [m] Area Operations Floor relative to WGS84. If unknown: -1000 m. + */ +static inline float mavlink_msg_open_drone_id_system_get_area_floor(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 12); +} + +/** + * @brief Get field category_eu from open_drone_id_system message + * + * @return When classification_type is MAV_ODID_CLASSIFICATION_TYPE_EU, specifies the category of the UA. + */ +static inline uint8_t mavlink_msg_open_drone_id_system_get_category_eu(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 44); +} + +/** + * @brief Get field class_eu from open_drone_id_system message + * + * @return When classification_type is MAV_ODID_CLASSIFICATION_TYPE_EU, specifies the class of the UA. + */ +static inline uint8_t mavlink_msg_open_drone_id_system_get_class_eu(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 45); +} + +/** + * @brief Decode a open_drone_id_system message into a struct + * + * @param msg The message to decode + * @param open_drone_id_system C-struct to decode the message contents into + */ +static inline void mavlink_msg_open_drone_id_system_decode(const mavlink_message_t* msg, mavlink_open_drone_id_system_t* open_drone_id_system) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + open_drone_id_system->operator_latitude = mavlink_msg_open_drone_id_system_get_operator_latitude(msg); + open_drone_id_system->operator_longitude = mavlink_msg_open_drone_id_system_get_operator_longitude(msg); + open_drone_id_system->area_ceiling = mavlink_msg_open_drone_id_system_get_area_ceiling(msg); + open_drone_id_system->area_floor = mavlink_msg_open_drone_id_system_get_area_floor(msg); + open_drone_id_system->area_count = mavlink_msg_open_drone_id_system_get_area_count(msg); + open_drone_id_system->area_radius = mavlink_msg_open_drone_id_system_get_area_radius(msg); + open_drone_id_system->target_system = mavlink_msg_open_drone_id_system_get_target_system(msg); + open_drone_id_system->target_component = mavlink_msg_open_drone_id_system_get_target_component(msg); + mavlink_msg_open_drone_id_system_get_id_or_mac(msg, open_drone_id_system->id_or_mac); + open_drone_id_system->operator_location_type = mavlink_msg_open_drone_id_system_get_operator_location_type(msg); + open_drone_id_system->classification_type = mavlink_msg_open_drone_id_system_get_classification_type(msg); + open_drone_id_system->category_eu = mavlink_msg_open_drone_id_system_get_category_eu(msg); + open_drone_id_system->class_eu = mavlink_msg_open_drone_id_system_get_class_eu(msg); +#else + uint8_t len = msg->len < MAVLINK_MSG_ID_OPEN_DRONE_ID_SYSTEM_LEN? msg->len : MAVLINK_MSG_ID_OPEN_DRONE_ID_SYSTEM_LEN; + memset(open_drone_id_system, 0, MAVLINK_MSG_ID_OPEN_DRONE_ID_SYSTEM_LEN); + memcpy(open_drone_id_system, _MAV_PAYLOAD(msg), len); +#endif +} diff --git a/lib/main/MAVLink/common/mavlink_msg_optical_flow.h b/lib/main/MAVLink/common/mavlink_msg_optical_flow.h index 56082216f9..25c8886421 100755 --- a/lib/main/MAVLink/common/mavlink_msg_optical_flow.h +++ b/lib/main/MAVLink/common/mavlink_msg_optical_flow.h @@ -3,7 +3,7 @@ #define MAVLINK_MSG_ID_OPTICAL_FLOW 100 - +MAVPACKED( typedef struct __mavlink_optical_flow_t { uint64_t time_usec; /*< [us] Timestamp (UNIX Epoch time or time since system boot). The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude of the number.*/ float flow_comp_m_x; /*< [m/s] Flow in x-sensor direction, angular-speed compensated*/ @@ -13,11 +13,13 @@ typedef struct __mavlink_optical_flow_t { int16_t flow_y; /*< [dpix] Flow in y-sensor direction*/ uint8_t sensor_id; /*< Sensor ID*/ uint8_t quality; /*< Optical flow quality / confidence. 0: bad, 255: maximum quality*/ -} mavlink_optical_flow_t; + float flow_rate_x; /*< [rad/s] Flow rate about X axis*/ + float flow_rate_y; /*< [rad/s] Flow rate about Y axis*/ +}) mavlink_optical_flow_t; -#define MAVLINK_MSG_ID_OPTICAL_FLOW_LEN 26 +#define MAVLINK_MSG_ID_OPTICAL_FLOW_LEN 34 #define MAVLINK_MSG_ID_OPTICAL_FLOW_MIN_LEN 26 -#define MAVLINK_MSG_ID_100_LEN 26 +#define MAVLINK_MSG_ID_100_LEN 34 #define MAVLINK_MSG_ID_100_MIN_LEN 26 #define MAVLINK_MSG_ID_OPTICAL_FLOW_CRC 175 @@ -29,7 +31,7 @@ typedef struct __mavlink_optical_flow_t { #define MAVLINK_MESSAGE_INFO_OPTICAL_FLOW { \ 100, \ "OPTICAL_FLOW", \ - 8, \ + 10, \ { { "time_usec", NULL, MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_optical_flow_t, time_usec) }, \ { "sensor_id", NULL, MAVLINK_TYPE_UINT8_T, 0, 24, offsetof(mavlink_optical_flow_t, sensor_id) }, \ { "flow_x", NULL, MAVLINK_TYPE_INT16_T, 0, 20, offsetof(mavlink_optical_flow_t, flow_x) }, \ @@ -38,12 +40,14 @@ typedef struct __mavlink_optical_flow_t { { "flow_comp_m_y", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_optical_flow_t, flow_comp_m_y) }, \ { "quality", NULL, MAVLINK_TYPE_UINT8_T, 0, 25, offsetof(mavlink_optical_flow_t, quality) }, \ { "ground_distance", NULL, MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_optical_flow_t, ground_distance) }, \ + { "flow_rate_x", NULL, MAVLINK_TYPE_FLOAT, 0, 26, offsetof(mavlink_optical_flow_t, flow_rate_x) }, \ + { "flow_rate_y", NULL, MAVLINK_TYPE_FLOAT, 0, 30, offsetof(mavlink_optical_flow_t, flow_rate_y) }, \ } \ } #else #define MAVLINK_MESSAGE_INFO_OPTICAL_FLOW { \ "OPTICAL_FLOW", \ - 8, \ + 10, \ { { "time_usec", NULL, MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_optical_flow_t, time_usec) }, \ { "sensor_id", NULL, MAVLINK_TYPE_UINT8_T, 0, 24, offsetof(mavlink_optical_flow_t, sensor_id) }, \ { "flow_x", NULL, MAVLINK_TYPE_INT16_T, 0, 20, offsetof(mavlink_optical_flow_t, flow_x) }, \ @@ -52,6 +56,8 @@ typedef struct __mavlink_optical_flow_t { { "flow_comp_m_y", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_optical_flow_t, flow_comp_m_y) }, \ { "quality", NULL, MAVLINK_TYPE_UINT8_T, 0, 25, offsetof(mavlink_optical_flow_t, quality) }, \ { "ground_distance", NULL, MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_optical_flow_t, ground_distance) }, \ + { "flow_rate_x", NULL, MAVLINK_TYPE_FLOAT, 0, 26, offsetof(mavlink_optical_flow_t, flow_rate_x) }, \ + { "flow_rate_y", NULL, MAVLINK_TYPE_FLOAT, 0, 30, offsetof(mavlink_optical_flow_t, flow_rate_y) }, \ } \ } #endif @@ -70,10 +76,12 @@ typedef struct __mavlink_optical_flow_t { * @param flow_comp_m_y [m/s] Flow in y-sensor direction, angular-speed compensated * @param quality Optical flow quality / confidence. 0: bad, 255: maximum quality * @param ground_distance [m] Ground distance. Positive value: distance known. Negative value: Unknown distance + * @param flow_rate_x [rad/s] Flow rate about X axis + * @param flow_rate_y [rad/s] Flow rate about Y axis * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_optical_flow_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint64_t time_usec, uint8_t sensor_id, int16_t flow_x, int16_t flow_y, float flow_comp_m_x, float flow_comp_m_y, uint8_t quality, float ground_distance) + uint64_t time_usec, uint8_t sensor_id, int16_t flow_x, int16_t flow_y, float flow_comp_m_x, float flow_comp_m_y, uint8_t quality, float ground_distance, float flow_rate_x, float flow_rate_y) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_OPTICAL_FLOW_LEN]; @@ -85,6 +93,8 @@ static inline uint16_t mavlink_msg_optical_flow_pack(uint8_t system_id, uint8_t _mav_put_int16_t(buf, 22, flow_y); _mav_put_uint8_t(buf, 24, sensor_id); _mav_put_uint8_t(buf, 25, quality); + _mav_put_float(buf, 26, flow_rate_x); + _mav_put_float(buf, 30, flow_rate_y); memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_OPTICAL_FLOW_LEN); #else @@ -97,6 +107,8 @@ static inline uint16_t mavlink_msg_optical_flow_pack(uint8_t system_id, uint8_t packet.flow_y = flow_y; packet.sensor_id = sensor_id; packet.quality = quality; + packet.flow_rate_x = flow_rate_x; + packet.flow_rate_y = flow_rate_y; memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_OPTICAL_FLOW_LEN); #endif @@ -119,11 +131,13 @@ static inline uint16_t mavlink_msg_optical_flow_pack(uint8_t system_id, uint8_t * @param flow_comp_m_y [m/s] Flow in y-sensor direction, angular-speed compensated * @param quality Optical flow quality / confidence. 0: bad, 255: maximum quality * @param ground_distance [m] Ground distance. Positive value: distance known. Negative value: Unknown distance + * @param flow_rate_x [rad/s] Flow rate about X axis + * @param flow_rate_y [rad/s] Flow rate about Y axis * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_optical_flow_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, - uint64_t time_usec,uint8_t sensor_id,int16_t flow_x,int16_t flow_y,float flow_comp_m_x,float flow_comp_m_y,uint8_t quality,float ground_distance) + uint64_t time_usec,uint8_t sensor_id,int16_t flow_x,int16_t flow_y,float flow_comp_m_x,float flow_comp_m_y,uint8_t quality,float ground_distance,float flow_rate_x,float flow_rate_y) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_OPTICAL_FLOW_LEN]; @@ -135,6 +149,8 @@ static inline uint16_t mavlink_msg_optical_flow_pack_chan(uint8_t system_id, uin _mav_put_int16_t(buf, 22, flow_y); _mav_put_uint8_t(buf, 24, sensor_id); _mav_put_uint8_t(buf, 25, quality); + _mav_put_float(buf, 26, flow_rate_x); + _mav_put_float(buf, 30, flow_rate_y); memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_OPTICAL_FLOW_LEN); #else @@ -147,6 +163,8 @@ static inline uint16_t mavlink_msg_optical_flow_pack_chan(uint8_t system_id, uin packet.flow_y = flow_y; packet.sensor_id = sensor_id; packet.quality = quality; + packet.flow_rate_x = flow_rate_x; + packet.flow_rate_y = flow_rate_y; memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_OPTICAL_FLOW_LEN); #endif @@ -165,7 +183,7 @@ static inline uint16_t mavlink_msg_optical_flow_pack_chan(uint8_t system_id, uin */ static inline uint16_t mavlink_msg_optical_flow_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_optical_flow_t* optical_flow) { - return mavlink_msg_optical_flow_pack(system_id, component_id, msg, optical_flow->time_usec, optical_flow->sensor_id, optical_flow->flow_x, optical_flow->flow_y, optical_flow->flow_comp_m_x, optical_flow->flow_comp_m_y, optical_flow->quality, optical_flow->ground_distance); + return mavlink_msg_optical_flow_pack(system_id, component_id, msg, optical_flow->time_usec, optical_flow->sensor_id, optical_flow->flow_x, optical_flow->flow_y, optical_flow->flow_comp_m_x, optical_flow->flow_comp_m_y, optical_flow->quality, optical_flow->ground_distance, optical_flow->flow_rate_x, optical_flow->flow_rate_y); } /** @@ -179,7 +197,7 @@ static inline uint16_t mavlink_msg_optical_flow_encode(uint8_t system_id, uint8_ */ static inline uint16_t mavlink_msg_optical_flow_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_optical_flow_t* optical_flow) { - return mavlink_msg_optical_flow_pack_chan(system_id, component_id, chan, msg, optical_flow->time_usec, optical_flow->sensor_id, optical_flow->flow_x, optical_flow->flow_y, optical_flow->flow_comp_m_x, optical_flow->flow_comp_m_y, optical_flow->quality, optical_flow->ground_distance); + return mavlink_msg_optical_flow_pack_chan(system_id, component_id, chan, msg, optical_flow->time_usec, optical_flow->sensor_id, optical_flow->flow_x, optical_flow->flow_y, optical_flow->flow_comp_m_x, optical_flow->flow_comp_m_y, optical_flow->quality, optical_flow->ground_distance, optical_flow->flow_rate_x, optical_flow->flow_rate_y); } /** @@ -194,10 +212,12 @@ static inline uint16_t mavlink_msg_optical_flow_encode_chan(uint8_t system_id, u * @param flow_comp_m_y [m/s] Flow in y-sensor direction, angular-speed compensated * @param quality Optical flow quality / confidence. 0: bad, 255: maximum quality * @param ground_distance [m] Ground distance. Positive value: distance known. Negative value: Unknown distance + * @param flow_rate_x [rad/s] Flow rate about X axis + * @param flow_rate_y [rad/s] Flow rate about Y axis */ #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS -static inline void mavlink_msg_optical_flow_send(mavlink_channel_t chan, uint64_t time_usec, uint8_t sensor_id, int16_t flow_x, int16_t flow_y, float flow_comp_m_x, float flow_comp_m_y, uint8_t quality, float ground_distance) +static inline void mavlink_msg_optical_flow_send(mavlink_channel_t chan, uint64_t time_usec, uint8_t sensor_id, int16_t flow_x, int16_t flow_y, float flow_comp_m_x, float flow_comp_m_y, uint8_t quality, float ground_distance, float flow_rate_x, float flow_rate_y) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_OPTICAL_FLOW_LEN]; @@ -209,6 +229,8 @@ static inline void mavlink_msg_optical_flow_send(mavlink_channel_t chan, uint64_ _mav_put_int16_t(buf, 22, flow_y); _mav_put_uint8_t(buf, 24, sensor_id); _mav_put_uint8_t(buf, 25, quality); + _mav_put_float(buf, 26, flow_rate_x); + _mav_put_float(buf, 30, flow_rate_y); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OPTICAL_FLOW, buf, MAVLINK_MSG_ID_OPTICAL_FLOW_MIN_LEN, MAVLINK_MSG_ID_OPTICAL_FLOW_LEN, MAVLINK_MSG_ID_OPTICAL_FLOW_CRC); #else @@ -221,6 +243,8 @@ static inline void mavlink_msg_optical_flow_send(mavlink_channel_t chan, uint64_ packet.flow_y = flow_y; packet.sensor_id = sensor_id; packet.quality = quality; + packet.flow_rate_x = flow_rate_x; + packet.flow_rate_y = flow_rate_y; _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OPTICAL_FLOW, (const char *)&packet, MAVLINK_MSG_ID_OPTICAL_FLOW_MIN_LEN, MAVLINK_MSG_ID_OPTICAL_FLOW_LEN, MAVLINK_MSG_ID_OPTICAL_FLOW_CRC); #endif @@ -234,7 +258,7 @@ static inline void mavlink_msg_optical_flow_send(mavlink_channel_t chan, uint64_ static inline void mavlink_msg_optical_flow_send_struct(mavlink_channel_t chan, const mavlink_optical_flow_t* optical_flow) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - mavlink_msg_optical_flow_send(chan, optical_flow->time_usec, optical_flow->sensor_id, optical_flow->flow_x, optical_flow->flow_y, optical_flow->flow_comp_m_x, optical_flow->flow_comp_m_y, optical_flow->quality, optical_flow->ground_distance); + mavlink_msg_optical_flow_send(chan, optical_flow->time_usec, optical_flow->sensor_id, optical_flow->flow_x, optical_flow->flow_y, optical_flow->flow_comp_m_x, optical_flow->flow_comp_m_y, optical_flow->quality, optical_flow->ground_distance, optical_flow->flow_rate_x, optical_flow->flow_rate_y); #else _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OPTICAL_FLOW, (const char *)optical_flow, MAVLINK_MSG_ID_OPTICAL_FLOW_MIN_LEN, MAVLINK_MSG_ID_OPTICAL_FLOW_LEN, MAVLINK_MSG_ID_OPTICAL_FLOW_CRC); #endif @@ -248,7 +272,7 @@ static inline void mavlink_msg_optical_flow_send_struct(mavlink_channel_t chan, is usually the receive buffer for the channel, and allows a reply to an incoming message with minimum stack space usage. */ -static inline void mavlink_msg_optical_flow_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint64_t time_usec, uint8_t sensor_id, int16_t flow_x, int16_t flow_y, float flow_comp_m_x, float flow_comp_m_y, uint8_t quality, float ground_distance) +static inline void mavlink_msg_optical_flow_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint64_t time_usec, uint8_t sensor_id, int16_t flow_x, int16_t flow_y, float flow_comp_m_x, float flow_comp_m_y, uint8_t quality, float ground_distance, float flow_rate_x, float flow_rate_y) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char *buf = (char *)msgbuf; @@ -260,6 +284,8 @@ static inline void mavlink_msg_optical_flow_send_buf(mavlink_message_t *msgbuf, _mav_put_int16_t(buf, 22, flow_y); _mav_put_uint8_t(buf, 24, sensor_id); _mav_put_uint8_t(buf, 25, quality); + _mav_put_float(buf, 26, flow_rate_x); + _mav_put_float(buf, 30, flow_rate_y); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OPTICAL_FLOW, buf, MAVLINK_MSG_ID_OPTICAL_FLOW_MIN_LEN, MAVLINK_MSG_ID_OPTICAL_FLOW_LEN, MAVLINK_MSG_ID_OPTICAL_FLOW_CRC); #else @@ -272,6 +298,8 @@ static inline void mavlink_msg_optical_flow_send_buf(mavlink_message_t *msgbuf, packet->flow_y = flow_y; packet->sensor_id = sensor_id; packet->quality = quality; + packet->flow_rate_x = flow_rate_x; + packet->flow_rate_y = flow_rate_y; _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_OPTICAL_FLOW, (const char *)packet, MAVLINK_MSG_ID_OPTICAL_FLOW_MIN_LEN, MAVLINK_MSG_ID_OPTICAL_FLOW_LEN, MAVLINK_MSG_ID_OPTICAL_FLOW_CRC); #endif @@ -363,6 +391,26 @@ static inline float mavlink_msg_optical_flow_get_ground_distance(const mavlink_m return _MAV_RETURN_float(msg, 16); } +/** + * @brief Get field flow_rate_x from optical_flow message + * + * @return [rad/s] Flow rate about X axis + */ +static inline float mavlink_msg_optical_flow_get_flow_rate_x(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 26); +} + +/** + * @brief Get field flow_rate_y from optical_flow message + * + * @return [rad/s] Flow rate about Y axis + */ +static inline float mavlink_msg_optical_flow_get_flow_rate_y(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 30); +} + /** * @brief Decode a optical_flow message into a struct * @@ -380,6 +428,8 @@ static inline void mavlink_msg_optical_flow_decode(const mavlink_message_t* msg, optical_flow->flow_y = mavlink_msg_optical_flow_get_flow_y(msg); optical_flow->sensor_id = mavlink_msg_optical_flow_get_sensor_id(msg); optical_flow->quality = mavlink_msg_optical_flow_get_quality(msg); + optical_flow->flow_rate_x = mavlink_msg_optical_flow_get_flow_rate_x(msg); + optical_flow->flow_rate_y = mavlink_msg_optical_flow_get_flow_rate_y(msg); #else uint8_t len = msg->len < MAVLINK_MSG_ID_OPTICAL_FLOW_LEN? msg->len : MAVLINK_MSG_ID_OPTICAL_FLOW_LEN; memset(optical_flow, 0, MAVLINK_MSG_ID_OPTICAL_FLOW_LEN); diff --git a/lib/main/MAVLink/common/mavlink_msg_orbit_execution_status.h b/lib/main/MAVLink/common/mavlink_msg_orbit_execution_status.h new file mode 100644 index 0000000000..29e2915a74 --- /dev/null +++ b/lib/main/MAVLink/common/mavlink_msg_orbit_execution_status.h @@ -0,0 +1,338 @@ +#pragma once +// MESSAGE ORBIT_EXECUTION_STATUS PACKING + +#define MAVLINK_MSG_ID_ORBIT_EXECUTION_STATUS 360 + + +typedef struct __mavlink_orbit_execution_status_t { + uint64_t time_usec; /*< [us] Timestamp (UNIX Epoch time or time since system boot). The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude of the number.*/ + float radius; /*< [m] Radius of the orbit circle. Positive values orbit clockwise, negative values orbit counter-clockwise.*/ + int32_t x; /*< X coordinate of center point. Coordinate system depends on frame field: local = x position in meters * 1e4, global = latitude in degrees * 1e7.*/ + int32_t y; /*< Y coordinate of center point. Coordinate system depends on frame field: local = x position in meters * 1e4, global = latitude in degrees * 1e7.*/ + float z; /*< [m] Altitude of center point. Coordinate system depends on frame field.*/ + uint8_t frame; /*< The coordinate system of the fields: x, y, z.*/ +} mavlink_orbit_execution_status_t; + +#define MAVLINK_MSG_ID_ORBIT_EXECUTION_STATUS_LEN 25 +#define MAVLINK_MSG_ID_ORBIT_EXECUTION_STATUS_MIN_LEN 25 +#define MAVLINK_MSG_ID_360_LEN 25 +#define MAVLINK_MSG_ID_360_MIN_LEN 25 + +#define MAVLINK_MSG_ID_ORBIT_EXECUTION_STATUS_CRC 11 +#define MAVLINK_MSG_ID_360_CRC 11 + + + +#if MAVLINK_COMMAND_24BIT +#define MAVLINK_MESSAGE_INFO_ORBIT_EXECUTION_STATUS { \ + 360, \ + "ORBIT_EXECUTION_STATUS", \ + 6, \ + { { "time_usec", NULL, MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_orbit_execution_status_t, time_usec) }, \ + { "radius", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_orbit_execution_status_t, radius) }, \ + { "frame", NULL, MAVLINK_TYPE_UINT8_T, 0, 24, offsetof(mavlink_orbit_execution_status_t, frame) }, \ + { "x", NULL, MAVLINK_TYPE_INT32_T, 0, 12, offsetof(mavlink_orbit_execution_status_t, x) }, \ + { "y", NULL, MAVLINK_TYPE_INT32_T, 0, 16, offsetof(mavlink_orbit_execution_status_t, y) }, \ + { "z", NULL, MAVLINK_TYPE_FLOAT, 0, 20, offsetof(mavlink_orbit_execution_status_t, z) }, \ + } \ +} +#else +#define MAVLINK_MESSAGE_INFO_ORBIT_EXECUTION_STATUS { \ + "ORBIT_EXECUTION_STATUS", \ + 6, \ + { { "time_usec", NULL, MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_orbit_execution_status_t, time_usec) }, \ + { "radius", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_orbit_execution_status_t, radius) }, \ + { "frame", NULL, MAVLINK_TYPE_UINT8_T, 0, 24, offsetof(mavlink_orbit_execution_status_t, frame) }, \ + { "x", NULL, MAVLINK_TYPE_INT32_T, 0, 12, offsetof(mavlink_orbit_execution_status_t, x) }, \ + { "y", NULL, MAVLINK_TYPE_INT32_T, 0, 16, offsetof(mavlink_orbit_execution_status_t, y) }, \ + { "z", NULL, MAVLINK_TYPE_FLOAT, 0, 20, offsetof(mavlink_orbit_execution_status_t, z) }, \ + } \ +} +#endif + +/** + * @brief Pack a orbit_execution_status message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param time_usec [us] Timestamp (UNIX Epoch time or time since system boot). The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude of the number. + * @param radius [m] Radius of the orbit circle. Positive values orbit clockwise, negative values orbit counter-clockwise. + * @param frame The coordinate system of the fields: x, y, z. + * @param x X coordinate of center point. Coordinate system depends on frame field: local = x position in meters * 1e4, global = latitude in degrees * 1e7. + * @param y Y coordinate of center point. Coordinate system depends on frame field: local = x position in meters * 1e4, global = latitude in degrees * 1e7. + * @param z [m] Altitude of center point. Coordinate system depends on frame field. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_orbit_execution_status_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint64_t time_usec, float radius, uint8_t frame, int32_t x, int32_t y, float z) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_ORBIT_EXECUTION_STATUS_LEN]; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_float(buf, 8, radius); + _mav_put_int32_t(buf, 12, x); + _mav_put_int32_t(buf, 16, y); + _mav_put_float(buf, 20, z); + _mav_put_uint8_t(buf, 24, frame); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_ORBIT_EXECUTION_STATUS_LEN); +#else + mavlink_orbit_execution_status_t packet; + packet.time_usec = time_usec; + packet.radius = radius; + packet.x = x; + packet.y = y; + packet.z = z; + packet.frame = frame; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_ORBIT_EXECUTION_STATUS_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_ORBIT_EXECUTION_STATUS; + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_ORBIT_EXECUTION_STATUS_MIN_LEN, MAVLINK_MSG_ID_ORBIT_EXECUTION_STATUS_LEN, MAVLINK_MSG_ID_ORBIT_EXECUTION_STATUS_CRC); +} + +/** + * @brief Pack a orbit_execution_status message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param time_usec [us] Timestamp (UNIX Epoch time or time since system boot). The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude of the number. + * @param radius [m] Radius of the orbit circle. Positive values orbit clockwise, negative values orbit counter-clockwise. + * @param frame The coordinate system of the fields: x, y, z. + * @param x X coordinate of center point. Coordinate system depends on frame field: local = x position in meters * 1e4, global = latitude in degrees * 1e7. + * @param y Y coordinate of center point. Coordinate system depends on frame field: local = x position in meters * 1e4, global = latitude in degrees * 1e7. + * @param z [m] Altitude of center point. Coordinate system depends on frame field. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_orbit_execution_status_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint64_t time_usec,float radius,uint8_t frame,int32_t x,int32_t y,float z) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_ORBIT_EXECUTION_STATUS_LEN]; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_float(buf, 8, radius); + _mav_put_int32_t(buf, 12, x); + _mav_put_int32_t(buf, 16, y); + _mav_put_float(buf, 20, z); + _mav_put_uint8_t(buf, 24, frame); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_ORBIT_EXECUTION_STATUS_LEN); +#else + mavlink_orbit_execution_status_t packet; + packet.time_usec = time_usec; + packet.radius = radius; + packet.x = x; + packet.y = y; + packet.z = z; + packet.frame = frame; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_ORBIT_EXECUTION_STATUS_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_ORBIT_EXECUTION_STATUS; + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_ORBIT_EXECUTION_STATUS_MIN_LEN, MAVLINK_MSG_ID_ORBIT_EXECUTION_STATUS_LEN, MAVLINK_MSG_ID_ORBIT_EXECUTION_STATUS_CRC); +} + +/** + * @brief Encode a orbit_execution_status struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param orbit_execution_status C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_orbit_execution_status_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_orbit_execution_status_t* orbit_execution_status) +{ + return mavlink_msg_orbit_execution_status_pack(system_id, component_id, msg, orbit_execution_status->time_usec, orbit_execution_status->radius, orbit_execution_status->frame, orbit_execution_status->x, orbit_execution_status->y, orbit_execution_status->z); +} + +/** + * @brief Encode a orbit_execution_status struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param orbit_execution_status C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_orbit_execution_status_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_orbit_execution_status_t* orbit_execution_status) +{ + return mavlink_msg_orbit_execution_status_pack_chan(system_id, component_id, chan, msg, orbit_execution_status->time_usec, orbit_execution_status->radius, orbit_execution_status->frame, orbit_execution_status->x, orbit_execution_status->y, orbit_execution_status->z); +} + +/** + * @brief Send a orbit_execution_status message + * @param chan MAVLink channel to send the message + * + * @param time_usec [us] Timestamp (UNIX Epoch time or time since system boot). The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude of the number. + * @param radius [m] Radius of the orbit circle. Positive values orbit clockwise, negative values orbit counter-clockwise. + * @param frame The coordinate system of the fields: x, y, z. + * @param x X coordinate of center point. Coordinate system depends on frame field: local = x position in meters * 1e4, global = latitude in degrees * 1e7. + * @param y Y coordinate of center point. Coordinate system depends on frame field: local = x position in meters * 1e4, global = latitude in degrees * 1e7. + * @param z [m] Altitude of center point. Coordinate system depends on frame field. + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_orbit_execution_status_send(mavlink_channel_t chan, uint64_t time_usec, float radius, uint8_t frame, int32_t x, int32_t y, float z) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_ORBIT_EXECUTION_STATUS_LEN]; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_float(buf, 8, radius); + _mav_put_int32_t(buf, 12, x); + _mav_put_int32_t(buf, 16, y); + _mav_put_float(buf, 20, z); + _mav_put_uint8_t(buf, 24, frame); + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ORBIT_EXECUTION_STATUS, buf, MAVLINK_MSG_ID_ORBIT_EXECUTION_STATUS_MIN_LEN, MAVLINK_MSG_ID_ORBIT_EXECUTION_STATUS_LEN, MAVLINK_MSG_ID_ORBIT_EXECUTION_STATUS_CRC); +#else + mavlink_orbit_execution_status_t packet; + packet.time_usec = time_usec; + packet.radius = radius; + packet.x = x; + packet.y = y; + packet.z = z; + packet.frame = frame; + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ORBIT_EXECUTION_STATUS, (const char *)&packet, MAVLINK_MSG_ID_ORBIT_EXECUTION_STATUS_MIN_LEN, MAVLINK_MSG_ID_ORBIT_EXECUTION_STATUS_LEN, MAVLINK_MSG_ID_ORBIT_EXECUTION_STATUS_CRC); +#endif +} + +/** + * @brief Send a orbit_execution_status message + * @param chan MAVLink channel to send the message + * @param struct The MAVLink struct to serialize + */ +static inline void mavlink_msg_orbit_execution_status_send_struct(mavlink_channel_t chan, const mavlink_orbit_execution_status_t* orbit_execution_status) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + mavlink_msg_orbit_execution_status_send(chan, orbit_execution_status->time_usec, orbit_execution_status->radius, orbit_execution_status->frame, orbit_execution_status->x, orbit_execution_status->y, orbit_execution_status->z); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ORBIT_EXECUTION_STATUS, (const char *)orbit_execution_status, MAVLINK_MSG_ID_ORBIT_EXECUTION_STATUS_MIN_LEN, MAVLINK_MSG_ID_ORBIT_EXECUTION_STATUS_LEN, MAVLINK_MSG_ID_ORBIT_EXECUTION_STATUS_CRC); +#endif +} + +#if MAVLINK_MSG_ID_ORBIT_EXECUTION_STATUS_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_orbit_execution_status_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint64_t time_usec, float radius, uint8_t frame, int32_t x, int32_t y, float z) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_float(buf, 8, radius); + _mav_put_int32_t(buf, 12, x); + _mav_put_int32_t(buf, 16, y); + _mav_put_float(buf, 20, z); + _mav_put_uint8_t(buf, 24, frame); + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ORBIT_EXECUTION_STATUS, buf, MAVLINK_MSG_ID_ORBIT_EXECUTION_STATUS_MIN_LEN, MAVLINK_MSG_ID_ORBIT_EXECUTION_STATUS_LEN, MAVLINK_MSG_ID_ORBIT_EXECUTION_STATUS_CRC); +#else + mavlink_orbit_execution_status_t *packet = (mavlink_orbit_execution_status_t *)msgbuf; + packet->time_usec = time_usec; + packet->radius = radius; + packet->x = x; + packet->y = y; + packet->z = z; + packet->frame = frame; + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_ORBIT_EXECUTION_STATUS, (const char *)packet, MAVLINK_MSG_ID_ORBIT_EXECUTION_STATUS_MIN_LEN, MAVLINK_MSG_ID_ORBIT_EXECUTION_STATUS_LEN, MAVLINK_MSG_ID_ORBIT_EXECUTION_STATUS_CRC); +#endif +} +#endif + +#endif + +// MESSAGE ORBIT_EXECUTION_STATUS UNPACKING + + +/** + * @brief Get field time_usec from orbit_execution_status message + * + * @return [us] Timestamp (UNIX Epoch time or time since system boot). The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude of the number. + */ +static inline uint64_t mavlink_msg_orbit_execution_status_get_time_usec(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint64_t(msg, 0); +} + +/** + * @brief Get field radius from orbit_execution_status message + * + * @return [m] Radius of the orbit circle. Positive values orbit clockwise, negative values orbit counter-clockwise. + */ +static inline float mavlink_msg_orbit_execution_status_get_radius(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 8); +} + +/** + * @brief Get field frame from orbit_execution_status message + * + * @return The coordinate system of the fields: x, y, z. + */ +static inline uint8_t mavlink_msg_orbit_execution_status_get_frame(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 24); +} + +/** + * @brief Get field x from orbit_execution_status message + * + * @return X coordinate of center point. Coordinate system depends on frame field: local = x position in meters * 1e4, global = latitude in degrees * 1e7. + */ +static inline int32_t mavlink_msg_orbit_execution_status_get_x(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int32_t(msg, 12); +} + +/** + * @brief Get field y from orbit_execution_status message + * + * @return Y coordinate of center point. Coordinate system depends on frame field: local = x position in meters * 1e4, global = latitude in degrees * 1e7. + */ +static inline int32_t mavlink_msg_orbit_execution_status_get_y(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int32_t(msg, 16); +} + +/** + * @brief Get field z from orbit_execution_status message + * + * @return [m] Altitude of center point. Coordinate system depends on frame field. + */ +static inline float mavlink_msg_orbit_execution_status_get_z(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 20); +} + +/** + * @brief Decode a orbit_execution_status message into a struct + * + * @param msg The message to decode + * @param orbit_execution_status C-struct to decode the message contents into + */ +static inline void mavlink_msg_orbit_execution_status_decode(const mavlink_message_t* msg, mavlink_orbit_execution_status_t* orbit_execution_status) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + orbit_execution_status->time_usec = mavlink_msg_orbit_execution_status_get_time_usec(msg); + orbit_execution_status->radius = mavlink_msg_orbit_execution_status_get_radius(msg); + orbit_execution_status->x = mavlink_msg_orbit_execution_status_get_x(msg); + orbit_execution_status->y = mavlink_msg_orbit_execution_status_get_y(msg); + orbit_execution_status->z = mavlink_msg_orbit_execution_status_get_z(msg); + orbit_execution_status->frame = mavlink_msg_orbit_execution_status_get_frame(msg); +#else + uint8_t len = msg->len < MAVLINK_MSG_ID_ORBIT_EXECUTION_STATUS_LEN? msg->len : MAVLINK_MSG_ID_ORBIT_EXECUTION_STATUS_LEN; + memset(orbit_execution_status, 0, MAVLINK_MSG_ID_ORBIT_EXECUTION_STATUS_LEN); + memcpy(orbit_execution_status, _MAV_PAYLOAD(msg), len); +#endif +} diff --git a/lib/main/MAVLink/common/mavlink_msg_param_ext_ack.h b/lib/main/MAVLink/common/mavlink_msg_param_ext_ack.h new file mode 100644 index 0000000000..3a560d2b8f --- /dev/null +++ b/lib/main/MAVLink/common/mavlink_msg_param_ext_ack.h @@ -0,0 +1,281 @@ +#pragma once +// MESSAGE PARAM_EXT_ACK PACKING + +#define MAVLINK_MSG_ID_PARAM_EXT_ACK 324 + + +typedef struct __mavlink_param_ext_ack_t { + char param_id[16]; /*< Parameter id, terminated by NULL if the length is less than 16 human-readable chars and WITHOUT null termination (NULL) byte if the length is exactly 16 chars - applications have to provide 16+1 bytes storage if the ID is stored as string*/ + char param_value[128]; /*< Parameter value (new value if PARAM_ACK_ACCEPTED, current value otherwise)*/ + uint8_t param_type; /*< Parameter type.*/ + uint8_t param_result; /*< Result code.*/ +} mavlink_param_ext_ack_t; + +#define MAVLINK_MSG_ID_PARAM_EXT_ACK_LEN 146 +#define MAVLINK_MSG_ID_PARAM_EXT_ACK_MIN_LEN 146 +#define MAVLINK_MSG_ID_324_LEN 146 +#define MAVLINK_MSG_ID_324_MIN_LEN 146 + +#define MAVLINK_MSG_ID_PARAM_EXT_ACK_CRC 132 +#define MAVLINK_MSG_ID_324_CRC 132 + +#define MAVLINK_MSG_PARAM_EXT_ACK_FIELD_PARAM_ID_LEN 16 +#define MAVLINK_MSG_PARAM_EXT_ACK_FIELD_PARAM_VALUE_LEN 128 + +#if MAVLINK_COMMAND_24BIT +#define MAVLINK_MESSAGE_INFO_PARAM_EXT_ACK { \ + 324, \ + "PARAM_EXT_ACK", \ + 4, \ + { { "param_id", NULL, MAVLINK_TYPE_CHAR, 16, 0, offsetof(mavlink_param_ext_ack_t, param_id) }, \ + { "param_value", NULL, MAVLINK_TYPE_CHAR, 128, 16, offsetof(mavlink_param_ext_ack_t, param_value) }, \ + { "param_type", NULL, MAVLINK_TYPE_UINT8_T, 0, 144, offsetof(mavlink_param_ext_ack_t, param_type) }, \ + { "param_result", NULL, MAVLINK_TYPE_UINT8_T, 0, 145, offsetof(mavlink_param_ext_ack_t, param_result) }, \ + } \ +} +#else +#define MAVLINK_MESSAGE_INFO_PARAM_EXT_ACK { \ + "PARAM_EXT_ACK", \ + 4, \ + { { "param_id", NULL, MAVLINK_TYPE_CHAR, 16, 0, offsetof(mavlink_param_ext_ack_t, param_id) }, \ + { "param_value", NULL, MAVLINK_TYPE_CHAR, 128, 16, offsetof(mavlink_param_ext_ack_t, param_value) }, \ + { "param_type", NULL, MAVLINK_TYPE_UINT8_T, 0, 144, offsetof(mavlink_param_ext_ack_t, param_type) }, \ + { "param_result", NULL, MAVLINK_TYPE_UINT8_T, 0, 145, offsetof(mavlink_param_ext_ack_t, param_result) }, \ + } \ +} +#endif + +/** + * @brief Pack a param_ext_ack message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param param_id Parameter id, terminated by NULL if the length is less than 16 human-readable chars and WITHOUT null termination (NULL) byte if the length is exactly 16 chars - applications have to provide 16+1 bytes storage if the ID is stored as string + * @param param_value Parameter value (new value if PARAM_ACK_ACCEPTED, current value otherwise) + * @param param_type Parameter type. + * @param param_result Result code. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_param_ext_ack_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + const char *param_id, const char *param_value, uint8_t param_type, uint8_t param_result) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_PARAM_EXT_ACK_LEN]; + _mav_put_uint8_t(buf, 144, param_type); + _mav_put_uint8_t(buf, 145, param_result); + _mav_put_char_array(buf, 0, param_id, 16); + _mav_put_char_array(buf, 16, param_value, 128); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_PARAM_EXT_ACK_LEN); +#else + mavlink_param_ext_ack_t packet; + packet.param_type = param_type; + packet.param_result = param_result; + mav_array_memcpy(packet.param_id, param_id, sizeof(char)*16); + mav_array_memcpy(packet.param_value, param_value, sizeof(char)*128); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_PARAM_EXT_ACK_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_PARAM_EXT_ACK; + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_PARAM_EXT_ACK_MIN_LEN, MAVLINK_MSG_ID_PARAM_EXT_ACK_LEN, MAVLINK_MSG_ID_PARAM_EXT_ACK_CRC); +} + +/** + * @brief Pack a param_ext_ack message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param param_id Parameter id, terminated by NULL if the length is less than 16 human-readable chars and WITHOUT null termination (NULL) byte if the length is exactly 16 chars - applications have to provide 16+1 bytes storage if the ID is stored as string + * @param param_value Parameter value (new value if PARAM_ACK_ACCEPTED, current value otherwise) + * @param param_type Parameter type. + * @param param_result Result code. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_param_ext_ack_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + const char *param_id,const char *param_value,uint8_t param_type,uint8_t param_result) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_PARAM_EXT_ACK_LEN]; + _mav_put_uint8_t(buf, 144, param_type); + _mav_put_uint8_t(buf, 145, param_result); + _mav_put_char_array(buf, 0, param_id, 16); + _mav_put_char_array(buf, 16, param_value, 128); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_PARAM_EXT_ACK_LEN); +#else + mavlink_param_ext_ack_t packet; + packet.param_type = param_type; + packet.param_result = param_result; + mav_array_memcpy(packet.param_id, param_id, sizeof(char)*16); + mav_array_memcpy(packet.param_value, param_value, sizeof(char)*128); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_PARAM_EXT_ACK_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_PARAM_EXT_ACK; + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_PARAM_EXT_ACK_MIN_LEN, MAVLINK_MSG_ID_PARAM_EXT_ACK_LEN, MAVLINK_MSG_ID_PARAM_EXT_ACK_CRC); +} + +/** + * @brief Encode a param_ext_ack struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param param_ext_ack C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_param_ext_ack_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_param_ext_ack_t* param_ext_ack) +{ + return mavlink_msg_param_ext_ack_pack(system_id, component_id, msg, param_ext_ack->param_id, param_ext_ack->param_value, param_ext_ack->param_type, param_ext_ack->param_result); +} + +/** + * @brief Encode a param_ext_ack struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param param_ext_ack C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_param_ext_ack_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_param_ext_ack_t* param_ext_ack) +{ + return mavlink_msg_param_ext_ack_pack_chan(system_id, component_id, chan, msg, param_ext_ack->param_id, param_ext_ack->param_value, param_ext_ack->param_type, param_ext_ack->param_result); +} + +/** + * @brief Send a param_ext_ack message + * @param chan MAVLink channel to send the message + * + * @param param_id Parameter id, terminated by NULL if the length is less than 16 human-readable chars and WITHOUT null termination (NULL) byte if the length is exactly 16 chars - applications have to provide 16+1 bytes storage if the ID is stored as string + * @param param_value Parameter value (new value if PARAM_ACK_ACCEPTED, current value otherwise) + * @param param_type Parameter type. + * @param param_result Result code. + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_param_ext_ack_send(mavlink_channel_t chan, const char *param_id, const char *param_value, uint8_t param_type, uint8_t param_result) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_PARAM_EXT_ACK_LEN]; + _mav_put_uint8_t(buf, 144, param_type); + _mav_put_uint8_t(buf, 145, param_result); + _mav_put_char_array(buf, 0, param_id, 16); + _mav_put_char_array(buf, 16, param_value, 128); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PARAM_EXT_ACK, buf, MAVLINK_MSG_ID_PARAM_EXT_ACK_MIN_LEN, MAVLINK_MSG_ID_PARAM_EXT_ACK_LEN, MAVLINK_MSG_ID_PARAM_EXT_ACK_CRC); +#else + mavlink_param_ext_ack_t packet; + packet.param_type = param_type; + packet.param_result = param_result; + mav_array_memcpy(packet.param_id, param_id, sizeof(char)*16); + mav_array_memcpy(packet.param_value, param_value, sizeof(char)*128); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PARAM_EXT_ACK, (const char *)&packet, MAVLINK_MSG_ID_PARAM_EXT_ACK_MIN_LEN, MAVLINK_MSG_ID_PARAM_EXT_ACK_LEN, MAVLINK_MSG_ID_PARAM_EXT_ACK_CRC); +#endif +} + +/** + * @brief Send a param_ext_ack message + * @param chan MAVLink channel to send the message + * @param struct The MAVLink struct to serialize + */ +static inline void mavlink_msg_param_ext_ack_send_struct(mavlink_channel_t chan, const mavlink_param_ext_ack_t* param_ext_ack) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + mavlink_msg_param_ext_ack_send(chan, param_ext_ack->param_id, param_ext_ack->param_value, param_ext_ack->param_type, param_ext_ack->param_result); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PARAM_EXT_ACK, (const char *)param_ext_ack, MAVLINK_MSG_ID_PARAM_EXT_ACK_MIN_LEN, MAVLINK_MSG_ID_PARAM_EXT_ACK_LEN, MAVLINK_MSG_ID_PARAM_EXT_ACK_CRC); +#endif +} + +#if MAVLINK_MSG_ID_PARAM_EXT_ACK_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_param_ext_ack_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, const char *param_id, const char *param_value, uint8_t param_type, uint8_t param_result) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint8_t(buf, 144, param_type); + _mav_put_uint8_t(buf, 145, param_result); + _mav_put_char_array(buf, 0, param_id, 16); + _mav_put_char_array(buf, 16, param_value, 128); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PARAM_EXT_ACK, buf, MAVLINK_MSG_ID_PARAM_EXT_ACK_MIN_LEN, MAVLINK_MSG_ID_PARAM_EXT_ACK_LEN, MAVLINK_MSG_ID_PARAM_EXT_ACK_CRC); +#else + mavlink_param_ext_ack_t *packet = (mavlink_param_ext_ack_t *)msgbuf; + packet->param_type = param_type; + packet->param_result = param_result; + mav_array_memcpy(packet->param_id, param_id, sizeof(char)*16); + mav_array_memcpy(packet->param_value, param_value, sizeof(char)*128); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PARAM_EXT_ACK, (const char *)packet, MAVLINK_MSG_ID_PARAM_EXT_ACK_MIN_LEN, MAVLINK_MSG_ID_PARAM_EXT_ACK_LEN, MAVLINK_MSG_ID_PARAM_EXT_ACK_CRC); +#endif +} +#endif + +#endif + +// MESSAGE PARAM_EXT_ACK UNPACKING + + +/** + * @brief Get field param_id from param_ext_ack message + * + * @return Parameter id, terminated by NULL if the length is less than 16 human-readable chars and WITHOUT null termination (NULL) byte if the length is exactly 16 chars - applications have to provide 16+1 bytes storage if the ID is stored as string + */ +static inline uint16_t mavlink_msg_param_ext_ack_get_param_id(const mavlink_message_t* msg, char *param_id) +{ + return _MAV_RETURN_char_array(msg, param_id, 16, 0); +} + +/** + * @brief Get field param_value from param_ext_ack message + * + * @return Parameter value (new value if PARAM_ACK_ACCEPTED, current value otherwise) + */ +static inline uint16_t mavlink_msg_param_ext_ack_get_param_value(const mavlink_message_t* msg, char *param_value) +{ + return _MAV_RETURN_char_array(msg, param_value, 128, 16); +} + +/** + * @brief Get field param_type from param_ext_ack message + * + * @return Parameter type. + */ +static inline uint8_t mavlink_msg_param_ext_ack_get_param_type(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 144); +} + +/** + * @brief Get field param_result from param_ext_ack message + * + * @return Result code. + */ +static inline uint8_t mavlink_msg_param_ext_ack_get_param_result(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 145); +} + +/** + * @brief Decode a param_ext_ack message into a struct + * + * @param msg The message to decode + * @param param_ext_ack C-struct to decode the message contents into + */ +static inline void mavlink_msg_param_ext_ack_decode(const mavlink_message_t* msg, mavlink_param_ext_ack_t* param_ext_ack) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + mavlink_msg_param_ext_ack_get_param_id(msg, param_ext_ack->param_id); + mavlink_msg_param_ext_ack_get_param_value(msg, param_ext_ack->param_value); + param_ext_ack->param_type = mavlink_msg_param_ext_ack_get_param_type(msg); + param_ext_ack->param_result = mavlink_msg_param_ext_ack_get_param_result(msg); +#else + uint8_t len = msg->len < MAVLINK_MSG_ID_PARAM_EXT_ACK_LEN? msg->len : MAVLINK_MSG_ID_PARAM_EXT_ACK_LEN; + memset(param_ext_ack, 0, MAVLINK_MSG_ID_PARAM_EXT_ACK_LEN); + memcpy(param_ext_ack, _MAV_PAYLOAD(msg), len); +#endif +} diff --git a/lib/main/MAVLink/common/mavlink_msg_param_ext_request_list.h b/lib/main/MAVLink/common/mavlink_msg_param_ext_request_list.h new file mode 100644 index 0000000000..a3983dfb85 --- /dev/null +++ b/lib/main/MAVLink/common/mavlink_msg_param_ext_request_list.h @@ -0,0 +1,238 @@ +#pragma once +// MESSAGE PARAM_EXT_REQUEST_LIST PACKING + +#define MAVLINK_MSG_ID_PARAM_EXT_REQUEST_LIST 321 + + +typedef struct __mavlink_param_ext_request_list_t { + uint8_t target_system; /*< System ID*/ + uint8_t target_component; /*< Component ID*/ +} mavlink_param_ext_request_list_t; + +#define MAVLINK_MSG_ID_PARAM_EXT_REQUEST_LIST_LEN 2 +#define MAVLINK_MSG_ID_PARAM_EXT_REQUEST_LIST_MIN_LEN 2 +#define MAVLINK_MSG_ID_321_LEN 2 +#define MAVLINK_MSG_ID_321_MIN_LEN 2 + +#define MAVLINK_MSG_ID_PARAM_EXT_REQUEST_LIST_CRC 88 +#define MAVLINK_MSG_ID_321_CRC 88 + + + +#if MAVLINK_COMMAND_24BIT +#define MAVLINK_MESSAGE_INFO_PARAM_EXT_REQUEST_LIST { \ + 321, \ + "PARAM_EXT_REQUEST_LIST", \ + 2, \ + { { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 0, offsetof(mavlink_param_ext_request_list_t, target_system) }, \ + { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 1, offsetof(mavlink_param_ext_request_list_t, target_component) }, \ + } \ +} +#else +#define MAVLINK_MESSAGE_INFO_PARAM_EXT_REQUEST_LIST { \ + "PARAM_EXT_REQUEST_LIST", \ + 2, \ + { { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 0, offsetof(mavlink_param_ext_request_list_t, target_system) }, \ + { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 1, offsetof(mavlink_param_ext_request_list_t, target_component) }, \ + } \ +} +#endif + +/** + * @brief Pack a param_ext_request_list message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param target_system System ID + * @param target_component Component ID + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_param_ext_request_list_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint8_t target_system, uint8_t target_component) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_PARAM_EXT_REQUEST_LIST_LEN]; + _mav_put_uint8_t(buf, 0, target_system); + _mav_put_uint8_t(buf, 1, target_component); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_PARAM_EXT_REQUEST_LIST_LEN); +#else + mavlink_param_ext_request_list_t packet; + packet.target_system = target_system; + packet.target_component = target_component; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_PARAM_EXT_REQUEST_LIST_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_PARAM_EXT_REQUEST_LIST; + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_PARAM_EXT_REQUEST_LIST_MIN_LEN, MAVLINK_MSG_ID_PARAM_EXT_REQUEST_LIST_LEN, MAVLINK_MSG_ID_PARAM_EXT_REQUEST_LIST_CRC); +} + +/** + * @brief Pack a param_ext_request_list message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param target_system System ID + * @param target_component Component ID + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_param_ext_request_list_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint8_t target_system,uint8_t target_component) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_PARAM_EXT_REQUEST_LIST_LEN]; + _mav_put_uint8_t(buf, 0, target_system); + _mav_put_uint8_t(buf, 1, target_component); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_PARAM_EXT_REQUEST_LIST_LEN); +#else + mavlink_param_ext_request_list_t packet; + packet.target_system = target_system; + packet.target_component = target_component; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_PARAM_EXT_REQUEST_LIST_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_PARAM_EXT_REQUEST_LIST; + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_PARAM_EXT_REQUEST_LIST_MIN_LEN, MAVLINK_MSG_ID_PARAM_EXT_REQUEST_LIST_LEN, MAVLINK_MSG_ID_PARAM_EXT_REQUEST_LIST_CRC); +} + +/** + * @brief Encode a param_ext_request_list struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param param_ext_request_list C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_param_ext_request_list_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_param_ext_request_list_t* param_ext_request_list) +{ + return mavlink_msg_param_ext_request_list_pack(system_id, component_id, msg, param_ext_request_list->target_system, param_ext_request_list->target_component); +} + +/** + * @brief Encode a param_ext_request_list struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param param_ext_request_list C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_param_ext_request_list_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_param_ext_request_list_t* param_ext_request_list) +{ + return mavlink_msg_param_ext_request_list_pack_chan(system_id, component_id, chan, msg, param_ext_request_list->target_system, param_ext_request_list->target_component); +} + +/** + * @brief Send a param_ext_request_list message + * @param chan MAVLink channel to send the message + * + * @param target_system System ID + * @param target_component Component ID + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_param_ext_request_list_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_PARAM_EXT_REQUEST_LIST_LEN]; + _mav_put_uint8_t(buf, 0, target_system); + _mav_put_uint8_t(buf, 1, target_component); + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PARAM_EXT_REQUEST_LIST, buf, MAVLINK_MSG_ID_PARAM_EXT_REQUEST_LIST_MIN_LEN, MAVLINK_MSG_ID_PARAM_EXT_REQUEST_LIST_LEN, MAVLINK_MSG_ID_PARAM_EXT_REQUEST_LIST_CRC); +#else + mavlink_param_ext_request_list_t packet; + packet.target_system = target_system; + packet.target_component = target_component; + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PARAM_EXT_REQUEST_LIST, (const char *)&packet, MAVLINK_MSG_ID_PARAM_EXT_REQUEST_LIST_MIN_LEN, MAVLINK_MSG_ID_PARAM_EXT_REQUEST_LIST_LEN, MAVLINK_MSG_ID_PARAM_EXT_REQUEST_LIST_CRC); +#endif +} + +/** + * @brief Send a param_ext_request_list message + * @param chan MAVLink channel to send the message + * @param struct The MAVLink struct to serialize + */ +static inline void mavlink_msg_param_ext_request_list_send_struct(mavlink_channel_t chan, const mavlink_param_ext_request_list_t* param_ext_request_list) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + mavlink_msg_param_ext_request_list_send(chan, param_ext_request_list->target_system, param_ext_request_list->target_component); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PARAM_EXT_REQUEST_LIST, (const char *)param_ext_request_list, MAVLINK_MSG_ID_PARAM_EXT_REQUEST_LIST_MIN_LEN, MAVLINK_MSG_ID_PARAM_EXT_REQUEST_LIST_LEN, MAVLINK_MSG_ID_PARAM_EXT_REQUEST_LIST_CRC); +#endif +} + +#if MAVLINK_MSG_ID_PARAM_EXT_REQUEST_LIST_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_param_ext_request_list_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target_system, uint8_t target_component) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint8_t(buf, 0, target_system); + _mav_put_uint8_t(buf, 1, target_component); + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PARAM_EXT_REQUEST_LIST, buf, MAVLINK_MSG_ID_PARAM_EXT_REQUEST_LIST_MIN_LEN, MAVLINK_MSG_ID_PARAM_EXT_REQUEST_LIST_LEN, MAVLINK_MSG_ID_PARAM_EXT_REQUEST_LIST_CRC); +#else + mavlink_param_ext_request_list_t *packet = (mavlink_param_ext_request_list_t *)msgbuf; + packet->target_system = target_system; + packet->target_component = target_component; + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PARAM_EXT_REQUEST_LIST, (const char *)packet, MAVLINK_MSG_ID_PARAM_EXT_REQUEST_LIST_MIN_LEN, MAVLINK_MSG_ID_PARAM_EXT_REQUEST_LIST_LEN, MAVLINK_MSG_ID_PARAM_EXT_REQUEST_LIST_CRC); +#endif +} +#endif + +#endif + +// MESSAGE PARAM_EXT_REQUEST_LIST UNPACKING + + +/** + * @brief Get field target_system from param_ext_request_list message + * + * @return System ID + */ +static inline uint8_t mavlink_msg_param_ext_request_list_get_target_system(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 0); +} + +/** + * @brief Get field target_component from param_ext_request_list message + * + * @return Component ID + */ +static inline uint8_t mavlink_msg_param_ext_request_list_get_target_component(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 1); +} + +/** + * @brief Decode a param_ext_request_list message into a struct + * + * @param msg The message to decode + * @param param_ext_request_list C-struct to decode the message contents into + */ +static inline void mavlink_msg_param_ext_request_list_decode(const mavlink_message_t* msg, mavlink_param_ext_request_list_t* param_ext_request_list) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + param_ext_request_list->target_system = mavlink_msg_param_ext_request_list_get_target_system(msg); + param_ext_request_list->target_component = mavlink_msg_param_ext_request_list_get_target_component(msg); +#else + uint8_t len = msg->len < MAVLINK_MSG_ID_PARAM_EXT_REQUEST_LIST_LEN? msg->len : MAVLINK_MSG_ID_PARAM_EXT_REQUEST_LIST_LEN; + memset(param_ext_request_list, 0, MAVLINK_MSG_ID_PARAM_EXT_REQUEST_LIST_LEN); + memcpy(param_ext_request_list, _MAV_PAYLOAD(msg), len); +#endif +} diff --git a/lib/main/MAVLink/common/mavlink_msg_param_ext_request_read.h b/lib/main/MAVLink/common/mavlink_msg_param_ext_request_read.h new file mode 100644 index 0000000000..974f68566e --- /dev/null +++ b/lib/main/MAVLink/common/mavlink_msg_param_ext_request_read.h @@ -0,0 +1,280 @@ +#pragma once +// MESSAGE PARAM_EXT_REQUEST_READ PACKING + +#define MAVLINK_MSG_ID_PARAM_EXT_REQUEST_READ 320 + + +typedef struct __mavlink_param_ext_request_read_t { + int16_t param_index; /*< Parameter index. Set to -1 to use the Parameter ID field as identifier (else param_id will be ignored)*/ + uint8_t target_system; /*< System ID*/ + uint8_t target_component; /*< Component ID*/ + char param_id[16]; /*< Parameter id, terminated by NULL if the length is less than 16 human-readable chars and WITHOUT null termination (NULL) byte if the length is exactly 16 chars - applications have to provide 16+1 bytes storage if the ID is stored as string*/ +} mavlink_param_ext_request_read_t; + +#define MAVLINK_MSG_ID_PARAM_EXT_REQUEST_READ_LEN 20 +#define MAVLINK_MSG_ID_PARAM_EXT_REQUEST_READ_MIN_LEN 20 +#define MAVLINK_MSG_ID_320_LEN 20 +#define MAVLINK_MSG_ID_320_MIN_LEN 20 + +#define MAVLINK_MSG_ID_PARAM_EXT_REQUEST_READ_CRC 243 +#define MAVLINK_MSG_ID_320_CRC 243 + +#define MAVLINK_MSG_PARAM_EXT_REQUEST_READ_FIELD_PARAM_ID_LEN 16 + +#if MAVLINK_COMMAND_24BIT +#define MAVLINK_MESSAGE_INFO_PARAM_EXT_REQUEST_READ { \ + 320, \ + "PARAM_EXT_REQUEST_READ", \ + 4, \ + { { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 2, offsetof(mavlink_param_ext_request_read_t, target_system) }, \ + { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 3, offsetof(mavlink_param_ext_request_read_t, target_component) }, \ + { "param_id", NULL, MAVLINK_TYPE_CHAR, 16, 4, offsetof(mavlink_param_ext_request_read_t, param_id) }, \ + { "param_index", NULL, MAVLINK_TYPE_INT16_T, 0, 0, offsetof(mavlink_param_ext_request_read_t, param_index) }, \ + } \ +} +#else +#define MAVLINK_MESSAGE_INFO_PARAM_EXT_REQUEST_READ { \ + "PARAM_EXT_REQUEST_READ", \ + 4, \ + { { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 2, offsetof(mavlink_param_ext_request_read_t, target_system) }, \ + { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 3, offsetof(mavlink_param_ext_request_read_t, target_component) }, \ + { "param_id", NULL, MAVLINK_TYPE_CHAR, 16, 4, offsetof(mavlink_param_ext_request_read_t, param_id) }, \ + { "param_index", NULL, MAVLINK_TYPE_INT16_T, 0, 0, offsetof(mavlink_param_ext_request_read_t, param_index) }, \ + } \ +} +#endif + +/** + * @brief Pack a param_ext_request_read message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param target_system System ID + * @param target_component Component ID + * @param param_id Parameter id, terminated by NULL if the length is less than 16 human-readable chars and WITHOUT null termination (NULL) byte if the length is exactly 16 chars - applications have to provide 16+1 bytes storage if the ID is stored as string + * @param param_index Parameter index. Set to -1 to use the Parameter ID field as identifier (else param_id will be ignored) + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_param_ext_request_read_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint8_t target_system, uint8_t target_component, const char *param_id, int16_t param_index) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_PARAM_EXT_REQUEST_READ_LEN]; + _mav_put_int16_t(buf, 0, param_index); + _mav_put_uint8_t(buf, 2, target_system); + _mav_put_uint8_t(buf, 3, target_component); + _mav_put_char_array(buf, 4, param_id, 16); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_PARAM_EXT_REQUEST_READ_LEN); +#else + mavlink_param_ext_request_read_t packet; + packet.param_index = param_index; + packet.target_system = target_system; + packet.target_component = target_component; + mav_array_memcpy(packet.param_id, param_id, sizeof(char)*16); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_PARAM_EXT_REQUEST_READ_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_PARAM_EXT_REQUEST_READ; + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_PARAM_EXT_REQUEST_READ_MIN_LEN, MAVLINK_MSG_ID_PARAM_EXT_REQUEST_READ_LEN, MAVLINK_MSG_ID_PARAM_EXT_REQUEST_READ_CRC); +} + +/** + * @brief Pack a param_ext_request_read message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param target_system System ID + * @param target_component Component ID + * @param param_id Parameter id, terminated by NULL if the length is less than 16 human-readable chars and WITHOUT null termination (NULL) byte if the length is exactly 16 chars - applications have to provide 16+1 bytes storage if the ID is stored as string + * @param param_index Parameter index. Set to -1 to use the Parameter ID field as identifier (else param_id will be ignored) + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_param_ext_request_read_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint8_t target_system,uint8_t target_component,const char *param_id,int16_t param_index) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_PARAM_EXT_REQUEST_READ_LEN]; + _mav_put_int16_t(buf, 0, param_index); + _mav_put_uint8_t(buf, 2, target_system); + _mav_put_uint8_t(buf, 3, target_component); + _mav_put_char_array(buf, 4, param_id, 16); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_PARAM_EXT_REQUEST_READ_LEN); +#else + mavlink_param_ext_request_read_t packet; + packet.param_index = param_index; + packet.target_system = target_system; + packet.target_component = target_component; + mav_array_memcpy(packet.param_id, param_id, sizeof(char)*16); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_PARAM_EXT_REQUEST_READ_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_PARAM_EXT_REQUEST_READ; + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_PARAM_EXT_REQUEST_READ_MIN_LEN, MAVLINK_MSG_ID_PARAM_EXT_REQUEST_READ_LEN, MAVLINK_MSG_ID_PARAM_EXT_REQUEST_READ_CRC); +} + +/** + * @brief Encode a param_ext_request_read struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param param_ext_request_read C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_param_ext_request_read_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_param_ext_request_read_t* param_ext_request_read) +{ + return mavlink_msg_param_ext_request_read_pack(system_id, component_id, msg, param_ext_request_read->target_system, param_ext_request_read->target_component, param_ext_request_read->param_id, param_ext_request_read->param_index); +} + +/** + * @brief Encode a param_ext_request_read struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param param_ext_request_read C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_param_ext_request_read_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_param_ext_request_read_t* param_ext_request_read) +{ + return mavlink_msg_param_ext_request_read_pack_chan(system_id, component_id, chan, msg, param_ext_request_read->target_system, param_ext_request_read->target_component, param_ext_request_read->param_id, param_ext_request_read->param_index); +} + +/** + * @brief Send a param_ext_request_read message + * @param chan MAVLink channel to send the message + * + * @param target_system System ID + * @param target_component Component ID + * @param param_id Parameter id, terminated by NULL if the length is less than 16 human-readable chars and WITHOUT null termination (NULL) byte if the length is exactly 16 chars - applications have to provide 16+1 bytes storage if the ID is stored as string + * @param param_index Parameter index. Set to -1 to use the Parameter ID field as identifier (else param_id will be ignored) + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_param_ext_request_read_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, const char *param_id, int16_t param_index) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_PARAM_EXT_REQUEST_READ_LEN]; + _mav_put_int16_t(buf, 0, param_index); + _mav_put_uint8_t(buf, 2, target_system); + _mav_put_uint8_t(buf, 3, target_component); + _mav_put_char_array(buf, 4, param_id, 16); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PARAM_EXT_REQUEST_READ, buf, MAVLINK_MSG_ID_PARAM_EXT_REQUEST_READ_MIN_LEN, MAVLINK_MSG_ID_PARAM_EXT_REQUEST_READ_LEN, MAVLINK_MSG_ID_PARAM_EXT_REQUEST_READ_CRC); +#else + mavlink_param_ext_request_read_t packet; + packet.param_index = param_index; + packet.target_system = target_system; + packet.target_component = target_component; + mav_array_memcpy(packet.param_id, param_id, sizeof(char)*16); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PARAM_EXT_REQUEST_READ, (const char *)&packet, MAVLINK_MSG_ID_PARAM_EXT_REQUEST_READ_MIN_LEN, MAVLINK_MSG_ID_PARAM_EXT_REQUEST_READ_LEN, MAVLINK_MSG_ID_PARAM_EXT_REQUEST_READ_CRC); +#endif +} + +/** + * @brief Send a param_ext_request_read message + * @param chan MAVLink channel to send the message + * @param struct The MAVLink struct to serialize + */ +static inline void mavlink_msg_param_ext_request_read_send_struct(mavlink_channel_t chan, const mavlink_param_ext_request_read_t* param_ext_request_read) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + mavlink_msg_param_ext_request_read_send(chan, param_ext_request_read->target_system, param_ext_request_read->target_component, param_ext_request_read->param_id, param_ext_request_read->param_index); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PARAM_EXT_REQUEST_READ, (const char *)param_ext_request_read, MAVLINK_MSG_ID_PARAM_EXT_REQUEST_READ_MIN_LEN, MAVLINK_MSG_ID_PARAM_EXT_REQUEST_READ_LEN, MAVLINK_MSG_ID_PARAM_EXT_REQUEST_READ_CRC); +#endif +} + +#if MAVLINK_MSG_ID_PARAM_EXT_REQUEST_READ_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_param_ext_request_read_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, const char *param_id, int16_t param_index) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_int16_t(buf, 0, param_index); + _mav_put_uint8_t(buf, 2, target_system); + _mav_put_uint8_t(buf, 3, target_component); + _mav_put_char_array(buf, 4, param_id, 16); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PARAM_EXT_REQUEST_READ, buf, MAVLINK_MSG_ID_PARAM_EXT_REQUEST_READ_MIN_LEN, MAVLINK_MSG_ID_PARAM_EXT_REQUEST_READ_LEN, MAVLINK_MSG_ID_PARAM_EXT_REQUEST_READ_CRC); +#else + mavlink_param_ext_request_read_t *packet = (mavlink_param_ext_request_read_t *)msgbuf; + packet->param_index = param_index; + packet->target_system = target_system; + packet->target_component = target_component; + mav_array_memcpy(packet->param_id, param_id, sizeof(char)*16); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PARAM_EXT_REQUEST_READ, (const char *)packet, MAVLINK_MSG_ID_PARAM_EXT_REQUEST_READ_MIN_LEN, MAVLINK_MSG_ID_PARAM_EXT_REQUEST_READ_LEN, MAVLINK_MSG_ID_PARAM_EXT_REQUEST_READ_CRC); +#endif +} +#endif + +#endif + +// MESSAGE PARAM_EXT_REQUEST_READ UNPACKING + + +/** + * @brief Get field target_system from param_ext_request_read message + * + * @return System ID + */ +static inline uint8_t mavlink_msg_param_ext_request_read_get_target_system(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 2); +} + +/** + * @brief Get field target_component from param_ext_request_read message + * + * @return Component ID + */ +static inline uint8_t mavlink_msg_param_ext_request_read_get_target_component(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 3); +} + +/** + * @brief Get field param_id from param_ext_request_read message + * + * @return Parameter id, terminated by NULL if the length is less than 16 human-readable chars and WITHOUT null termination (NULL) byte if the length is exactly 16 chars - applications have to provide 16+1 bytes storage if the ID is stored as string + */ +static inline uint16_t mavlink_msg_param_ext_request_read_get_param_id(const mavlink_message_t* msg, char *param_id) +{ + return _MAV_RETURN_char_array(msg, param_id, 16, 4); +} + +/** + * @brief Get field param_index from param_ext_request_read message + * + * @return Parameter index. Set to -1 to use the Parameter ID field as identifier (else param_id will be ignored) + */ +static inline int16_t mavlink_msg_param_ext_request_read_get_param_index(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int16_t(msg, 0); +} + +/** + * @brief Decode a param_ext_request_read message into a struct + * + * @param msg The message to decode + * @param param_ext_request_read C-struct to decode the message contents into + */ +static inline void mavlink_msg_param_ext_request_read_decode(const mavlink_message_t* msg, mavlink_param_ext_request_read_t* param_ext_request_read) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + param_ext_request_read->param_index = mavlink_msg_param_ext_request_read_get_param_index(msg); + param_ext_request_read->target_system = mavlink_msg_param_ext_request_read_get_target_system(msg); + param_ext_request_read->target_component = mavlink_msg_param_ext_request_read_get_target_component(msg); + mavlink_msg_param_ext_request_read_get_param_id(msg, param_ext_request_read->param_id); +#else + uint8_t len = msg->len < MAVLINK_MSG_ID_PARAM_EXT_REQUEST_READ_LEN? msg->len : MAVLINK_MSG_ID_PARAM_EXT_REQUEST_READ_LEN; + memset(param_ext_request_read, 0, MAVLINK_MSG_ID_PARAM_EXT_REQUEST_READ_LEN); + memcpy(param_ext_request_read, _MAV_PAYLOAD(msg), len); +#endif +} diff --git a/lib/main/MAVLink/common/mavlink_msg_param_ext_set.h b/lib/main/MAVLink/common/mavlink_msg_param_ext_set.h new file mode 100644 index 0000000000..bb4c7ccf04 --- /dev/null +++ b/lib/main/MAVLink/common/mavlink_msg_param_ext_set.h @@ -0,0 +1,306 @@ +#pragma once +// MESSAGE PARAM_EXT_SET PACKING + +#define MAVLINK_MSG_ID_PARAM_EXT_SET 323 + + +typedef struct __mavlink_param_ext_set_t { + uint8_t target_system; /*< System ID*/ + uint8_t target_component; /*< Component ID*/ + char param_id[16]; /*< Parameter id, terminated by NULL if the length is less than 16 human-readable chars and WITHOUT null termination (NULL) byte if the length is exactly 16 chars - applications have to provide 16+1 bytes storage if the ID is stored as string*/ + char param_value[128]; /*< Parameter value*/ + uint8_t param_type; /*< Parameter type.*/ +} mavlink_param_ext_set_t; + +#define MAVLINK_MSG_ID_PARAM_EXT_SET_LEN 147 +#define MAVLINK_MSG_ID_PARAM_EXT_SET_MIN_LEN 147 +#define MAVLINK_MSG_ID_323_LEN 147 +#define MAVLINK_MSG_ID_323_MIN_LEN 147 + +#define MAVLINK_MSG_ID_PARAM_EXT_SET_CRC 78 +#define MAVLINK_MSG_ID_323_CRC 78 + +#define MAVLINK_MSG_PARAM_EXT_SET_FIELD_PARAM_ID_LEN 16 +#define MAVLINK_MSG_PARAM_EXT_SET_FIELD_PARAM_VALUE_LEN 128 + +#if MAVLINK_COMMAND_24BIT +#define MAVLINK_MESSAGE_INFO_PARAM_EXT_SET { \ + 323, \ + "PARAM_EXT_SET", \ + 5, \ + { { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 0, offsetof(mavlink_param_ext_set_t, target_system) }, \ + { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 1, offsetof(mavlink_param_ext_set_t, target_component) }, \ + { "param_id", NULL, MAVLINK_TYPE_CHAR, 16, 2, offsetof(mavlink_param_ext_set_t, param_id) }, \ + { "param_value", NULL, MAVLINK_TYPE_CHAR, 128, 18, offsetof(mavlink_param_ext_set_t, param_value) }, \ + { "param_type", NULL, MAVLINK_TYPE_UINT8_T, 0, 146, offsetof(mavlink_param_ext_set_t, param_type) }, \ + } \ +} +#else +#define MAVLINK_MESSAGE_INFO_PARAM_EXT_SET { \ + "PARAM_EXT_SET", \ + 5, \ + { { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 0, offsetof(mavlink_param_ext_set_t, target_system) }, \ + { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 1, offsetof(mavlink_param_ext_set_t, target_component) }, \ + { "param_id", NULL, MAVLINK_TYPE_CHAR, 16, 2, offsetof(mavlink_param_ext_set_t, param_id) }, \ + { "param_value", NULL, MAVLINK_TYPE_CHAR, 128, 18, offsetof(mavlink_param_ext_set_t, param_value) }, \ + { "param_type", NULL, MAVLINK_TYPE_UINT8_T, 0, 146, offsetof(mavlink_param_ext_set_t, param_type) }, \ + } \ +} +#endif + +/** + * @brief Pack a param_ext_set message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param target_system System ID + * @param target_component Component ID + * @param param_id Parameter id, terminated by NULL if the length is less than 16 human-readable chars and WITHOUT null termination (NULL) byte if the length is exactly 16 chars - applications have to provide 16+1 bytes storage if the ID is stored as string + * @param param_value Parameter value + * @param param_type Parameter type. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_param_ext_set_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint8_t target_system, uint8_t target_component, const char *param_id, const char *param_value, uint8_t param_type) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_PARAM_EXT_SET_LEN]; + _mav_put_uint8_t(buf, 0, target_system); + _mav_put_uint8_t(buf, 1, target_component); + _mav_put_uint8_t(buf, 146, param_type); + _mav_put_char_array(buf, 2, param_id, 16); + _mav_put_char_array(buf, 18, param_value, 128); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_PARAM_EXT_SET_LEN); +#else + mavlink_param_ext_set_t packet; + packet.target_system = target_system; + packet.target_component = target_component; + packet.param_type = param_type; + mav_array_memcpy(packet.param_id, param_id, sizeof(char)*16); + mav_array_memcpy(packet.param_value, param_value, sizeof(char)*128); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_PARAM_EXT_SET_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_PARAM_EXT_SET; + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_PARAM_EXT_SET_MIN_LEN, MAVLINK_MSG_ID_PARAM_EXT_SET_LEN, MAVLINK_MSG_ID_PARAM_EXT_SET_CRC); +} + +/** + * @brief Pack a param_ext_set message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param target_system System ID + * @param target_component Component ID + * @param param_id Parameter id, terminated by NULL if the length is less than 16 human-readable chars and WITHOUT null termination (NULL) byte if the length is exactly 16 chars - applications have to provide 16+1 bytes storage if the ID is stored as string + * @param param_value Parameter value + * @param param_type Parameter type. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_param_ext_set_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint8_t target_system,uint8_t target_component,const char *param_id,const char *param_value,uint8_t param_type) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_PARAM_EXT_SET_LEN]; + _mav_put_uint8_t(buf, 0, target_system); + _mav_put_uint8_t(buf, 1, target_component); + _mav_put_uint8_t(buf, 146, param_type); + _mav_put_char_array(buf, 2, param_id, 16); + _mav_put_char_array(buf, 18, param_value, 128); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_PARAM_EXT_SET_LEN); +#else + mavlink_param_ext_set_t packet; + packet.target_system = target_system; + packet.target_component = target_component; + packet.param_type = param_type; + mav_array_memcpy(packet.param_id, param_id, sizeof(char)*16); + mav_array_memcpy(packet.param_value, param_value, sizeof(char)*128); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_PARAM_EXT_SET_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_PARAM_EXT_SET; + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_PARAM_EXT_SET_MIN_LEN, MAVLINK_MSG_ID_PARAM_EXT_SET_LEN, MAVLINK_MSG_ID_PARAM_EXT_SET_CRC); +} + +/** + * @brief Encode a param_ext_set struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param param_ext_set C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_param_ext_set_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_param_ext_set_t* param_ext_set) +{ + return mavlink_msg_param_ext_set_pack(system_id, component_id, msg, param_ext_set->target_system, param_ext_set->target_component, param_ext_set->param_id, param_ext_set->param_value, param_ext_set->param_type); +} + +/** + * @brief Encode a param_ext_set struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param param_ext_set C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_param_ext_set_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_param_ext_set_t* param_ext_set) +{ + return mavlink_msg_param_ext_set_pack_chan(system_id, component_id, chan, msg, param_ext_set->target_system, param_ext_set->target_component, param_ext_set->param_id, param_ext_set->param_value, param_ext_set->param_type); +} + +/** + * @brief Send a param_ext_set message + * @param chan MAVLink channel to send the message + * + * @param target_system System ID + * @param target_component Component ID + * @param param_id Parameter id, terminated by NULL if the length is less than 16 human-readable chars and WITHOUT null termination (NULL) byte if the length is exactly 16 chars - applications have to provide 16+1 bytes storage if the ID is stored as string + * @param param_value Parameter value + * @param param_type Parameter type. + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_param_ext_set_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, const char *param_id, const char *param_value, uint8_t param_type) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_PARAM_EXT_SET_LEN]; + _mav_put_uint8_t(buf, 0, target_system); + _mav_put_uint8_t(buf, 1, target_component); + _mav_put_uint8_t(buf, 146, param_type); + _mav_put_char_array(buf, 2, param_id, 16); + _mav_put_char_array(buf, 18, param_value, 128); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PARAM_EXT_SET, buf, MAVLINK_MSG_ID_PARAM_EXT_SET_MIN_LEN, MAVLINK_MSG_ID_PARAM_EXT_SET_LEN, MAVLINK_MSG_ID_PARAM_EXT_SET_CRC); +#else + mavlink_param_ext_set_t packet; + packet.target_system = target_system; + packet.target_component = target_component; + packet.param_type = param_type; + mav_array_memcpy(packet.param_id, param_id, sizeof(char)*16); + mav_array_memcpy(packet.param_value, param_value, sizeof(char)*128); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PARAM_EXT_SET, (const char *)&packet, MAVLINK_MSG_ID_PARAM_EXT_SET_MIN_LEN, MAVLINK_MSG_ID_PARAM_EXT_SET_LEN, MAVLINK_MSG_ID_PARAM_EXT_SET_CRC); +#endif +} + +/** + * @brief Send a param_ext_set message + * @param chan MAVLink channel to send the message + * @param struct The MAVLink struct to serialize + */ +static inline void mavlink_msg_param_ext_set_send_struct(mavlink_channel_t chan, const mavlink_param_ext_set_t* param_ext_set) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + mavlink_msg_param_ext_set_send(chan, param_ext_set->target_system, param_ext_set->target_component, param_ext_set->param_id, param_ext_set->param_value, param_ext_set->param_type); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PARAM_EXT_SET, (const char *)param_ext_set, MAVLINK_MSG_ID_PARAM_EXT_SET_MIN_LEN, MAVLINK_MSG_ID_PARAM_EXT_SET_LEN, MAVLINK_MSG_ID_PARAM_EXT_SET_CRC); +#endif +} + +#if MAVLINK_MSG_ID_PARAM_EXT_SET_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_param_ext_set_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, const char *param_id, const char *param_value, uint8_t param_type) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint8_t(buf, 0, target_system); + _mav_put_uint8_t(buf, 1, target_component); + _mav_put_uint8_t(buf, 146, param_type); + _mav_put_char_array(buf, 2, param_id, 16); + _mav_put_char_array(buf, 18, param_value, 128); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PARAM_EXT_SET, buf, MAVLINK_MSG_ID_PARAM_EXT_SET_MIN_LEN, MAVLINK_MSG_ID_PARAM_EXT_SET_LEN, MAVLINK_MSG_ID_PARAM_EXT_SET_CRC); +#else + mavlink_param_ext_set_t *packet = (mavlink_param_ext_set_t *)msgbuf; + packet->target_system = target_system; + packet->target_component = target_component; + packet->param_type = param_type; + mav_array_memcpy(packet->param_id, param_id, sizeof(char)*16); + mav_array_memcpy(packet->param_value, param_value, sizeof(char)*128); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PARAM_EXT_SET, (const char *)packet, MAVLINK_MSG_ID_PARAM_EXT_SET_MIN_LEN, MAVLINK_MSG_ID_PARAM_EXT_SET_LEN, MAVLINK_MSG_ID_PARAM_EXT_SET_CRC); +#endif +} +#endif + +#endif + +// MESSAGE PARAM_EXT_SET UNPACKING + + +/** + * @brief Get field target_system from param_ext_set message + * + * @return System ID + */ +static inline uint8_t mavlink_msg_param_ext_set_get_target_system(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 0); +} + +/** + * @brief Get field target_component from param_ext_set message + * + * @return Component ID + */ +static inline uint8_t mavlink_msg_param_ext_set_get_target_component(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 1); +} + +/** + * @brief Get field param_id from param_ext_set message + * + * @return Parameter id, terminated by NULL if the length is less than 16 human-readable chars and WITHOUT null termination (NULL) byte if the length is exactly 16 chars - applications have to provide 16+1 bytes storage if the ID is stored as string + */ +static inline uint16_t mavlink_msg_param_ext_set_get_param_id(const mavlink_message_t* msg, char *param_id) +{ + return _MAV_RETURN_char_array(msg, param_id, 16, 2); +} + +/** + * @brief Get field param_value from param_ext_set message + * + * @return Parameter value + */ +static inline uint16_t mavlink_msg_param_ext_set_get_param_value(const mavlink_message_t* msg, char *param_value) +{ + return _MAV_RETURN_char_array(msg, param_value, 128, 18); +} + +/** + * @brief Get field param_type from param_ext_set message + * + * @return Parameter type. + */ +static inline uint8_t mavlink_msg_param_ext_set_get_param_type(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 146); +} + +/** + * @brief Decode a param_ext_set message into a struct + * + * @param msg The message to decode + * @param param_ext_set C-struct to decode the message contents into + */ +static inline void mavlink_msg_param_ext_set_decode(const mavlink_message_t* msg, mavlink_param_ext_set_t* param_ext_set) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + param_ext_set->target_system = mavlink_msg_param_ext_set_get_target_system(msg); + param_ext_set->target_component = mavlink_msg_param_ext_set_get_target_component(msg); + mavlink_msg_param_ext_set_get_param_id(msg, param_ext_set->param_id); + mavlink_msg_param_ext_set_get_param_value(msg, param_ext_set->param_value); + param_ext_set->param_type = mavlink_msg_param_ext_set_get_param_type(msg); +#else + uint8_t len = msg->len < MAVLINK_MSG_ID_PARAM_EXT_SET_LEN? msg->len : MAVLINK_MSG_ID_PARAM_EXT_SET_LEN; + memset(param_ext_set, 0, MAVLINK_MSG_ID_PARAM_EXT_SET_LEN); + memcpy(param_ext_set, _MAV_PAYLOAD(msg), len); +#endif +} diff --git a/lib/main/MAVLink/common/mavlink_msg_param_ext_value.h b/lib/main/MAVLink/common/mavlink_msg_param_ext_value.h new file mode 100644 index 0000000000..1bf131c390 --- /dev/null +++ b/lib/main/MAVLink/common/mavlink_msg_param_ext_value.h @@ -0,0 +1,306 @@ +#pragma once +// MESSAGE PARAM_EXT_VALUE PACKING + +#define MAVLINK_MSG_ID_PARAM_EXT_VALUE 322 + + +typedef struct __mavlink_param_ext_value_t { + uint16_t param_count; /*< Total number of parameters*/ + uint16_t param_index; /*< Index of this parameter*/ + char param_id[16]; /*< Parameter id, terminated by NULL if the length is less than 16 human-readable chars and WITHOUT null termination (NULL) byte if the length is exactly 16 chars - applications have to provide 16+1 bytes storage if the ID is stored as string*/ + char param_value[128]; /*< Parameter value*/ + uint8_t param_type; /*< Parameter type.*/ +} mavlink_param_ext_value_t; + +#define MAVLINK_MSG_ID_PARAM_EXT_VALUE_LEN 149 +#define MAVLINK_MSG_ID_PARAM_EXT_VALUE_MIN_LEN 149 +#define MAVLINK_MSG_ID_322_LEN 149 +#define MAVLINK_MSG_ID_322_MIN_LEN 149 + +#define MAVLINK_MSG_ID_PARAM_EXT_VALUE_CRC 243 +#define MAVLINK_MSG_ID_322_CRC 243 + +#define MAVLINK_MSG_PARAM_EXT_VALUE_FIELD_PARAM_ID_LEN 16 +#define MAVLINK_MSG_PARAM_EXT_VALUE_FIELD_PARAM_VALUE_LEN 128 + +#if MAVLINK_COMMAND_24BIT +#define MAVLINK_MESSAGE_INFO_PARAM_EXT_VALUE { \ + 322, \ + "PARAM_EXT_VALUE", \ + 5, \ + { { "param_id", NULL, MAVLINK_TYPE_CHAR, 16, 4, offsetof(mavlink_param_ext_value_t, param_id) }, \ + { "param_value", NULL, MAVLINK_TYPE_CHAR, 128, 20, offsetof(mavlink_param_ext_value_t, param_value) }, \ + { "param_type", NULL, MAVLINK_TYPE_UINT8_T, 0, 148, offsetof(mavlink_param_ext_value_t, param_type) }, \ + { "param_count", NULL, MAVLINK_TYPE_UINT16_T, 0, 0, offsetof(mavlink_param_ext_value_t, param_count) }, \ + { "param_index", NULL, MAVLINK_TYPE_UINT16_T, 0, 2, offsetof(mavlink_param_ext_value_t, param_index) }, \ + } \ +} +#else +#define MAVLINK_MESSAGE_INFO_PARAM_EXT_VALUE { \ + "PARAM_EXT_VALUE", \ + 5, \ + { { "param_id", NULL, MAVLINK_TYPE_CHAR, 16, 4, offsetof(mavlink_param_ext_value_t, param_id) }, \ + { "param_value", NULL, MAVLINK_TYPE_CHAR, 128, 20, offsetof(mavlink_param_ext_value_t, param_value) }, \ + { "param_type", NULL, MAVLINK_TYPE_UINT8_T, 0, 148, offsetof(mavlink_param_ext_value_t, param_type) }, \ + { "param_count", NULL, MAVLINK_TYPE_UINT16_T, 0, 0, offsetof(mavlink_param_ext_value_t, param_count) }, \ + { "param_index", NULL, MAVLINK_TYPE_UINT16_T, 0, 2, offsetof(mavlink_param_ext_value_t, param_index) }, \ + } \ +} +#endif + +/** + * @brief Pack a param_ext_value message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param param_id Parameter id, terminated by NULL if the length is less than 16 human-readable chars and WITHOUT null termination (NULL) byte if the length is exactly 16 chars - applications have to provide 16+1 bytes storage if the ID is stored as string + * @param param_value Parameter value + * @param param_type Parameter type. + * @param param_count Total number of parameters + * @param param_index Index of this parameter + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_param_ext_value_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + const char *param_id, const char *param_value, uint8_t param_type, uint16_t param_count, uint16_t param_index) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_PARAM_EXT_VALUE_LEN]; + _mav_put_uint16_t(buf, 0, param_count); + _mav_put_uint16_t(buf, 2, param_index); + _mav_put_uint8_t(buf, 148, param_type); + _mav_put_char_array(buf, 4, param_id, 16); + _mav_put_char_array(buf, 20, param_value, 128); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_PARAM_EXT_VALUE_LEN); +#else + mavlink_param_ext_value_t packet; + packet.param_count = param_count; + packet.param_index = param_index; + packet.param_type = param_type; + mav_array_memcpy(packet.param_id, param_id, sizeof(char)*16); + mav_array_memcpy(packet.param_value, param_value, sizeof(char)*128); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_PARAM_EXT_VALUE_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_PARAM_EXT_VALUE; + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_PARAM_EXT_VALUE_MIN_LEN, MAVLINK_MSG_ID_PARAM_EXT_VALUE_LEN, MAVLINK_MSG_ID_PARAM_EXT_VALUE_CRC); +} + +/** + * @brief Pack a param_ext_value message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param param_id Parameter id, terminated by NULL if the length is less than 16 human-readable chars and WITHOUT null termination (NULL) byte if the length is exactly 16 chars - applications have to provide 16+1 bytes storage if the ID is stored as string + * @param param_value Parameter value + * @param param_type Parameter type. + * @param param_count Total number of parameters + * @param param_index Index of this parameter + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_param_ext_value_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + const char *param_id,const char *param_value,uint8_t param_type,uint16_t param_count,uint16_t param_index) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_PARAM_EXT_VALUE_LEN]; + _mav_put_uint16_t(buf, 0, param_count); + _mav_put_uint16_t(buf, 2, param_index); + _mav_put_uint8_t(buf, 148, param_type); + _mav_put_char_array(buf, 4, param_id, 16); + _mav_put_char_array(buf, 20, param_value, 128); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_PARAM_EXT_VALUE_LEN); +#else + mavlink_param_ext_value_t packet; + packet.param_count = param_count; + packet.param_index = param_index; + packet.param_type = param_type; + mav_array_memcpy(packet.param_id, param_id, sizeof(char)*16); + mav_array_memcpy(packet.param_value, param_value, sizeof(char)*128); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_PARAM_EXT_VALUE_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_PARAM_EXT_VALUE; + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_PARAM_EXT_VALUE_MIN_LEN, MAVLINK_MSG_ID_PARAM_EXT_VALUE_LEN, MAVLINK_MSG_ID_PARAM_EXT_VALUE_CRC); +} + +/** + * @brief Encode a param_ext_value struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param param_ext_value C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_param_ext_value_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_param_ext_value_t* param_ext_value) +{ + return mavlink_msg_param_ext_value_pack(system_id, component_id, msg, param_ext_value->param_id, param_ext_value->param_value, param_ext_value->param_type, param_ext_value->param_count, param_ext_value->param_index); +} + +/** + * @brief Encode a param_ext_value struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param param_ext_value C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_param_ext_value_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_param_ext_value_t* param_ext_value) +{ + return mavlink_msg_param_ext_value_pack_chan(system_id, component_id, chan, msg, param_ext_value->param_id, param_ext_value->param_value, param_ext_value->param_type, param_ext_value->param_count, param_ext_value->param_index); +} + +/** + * @brief Send a param_ext_value message + * @param chan MAVLink channel to send the message + * + * @param param_id Parameter id, terminated by NULL if the length is less than 16 human-readable chars and WITHOUT null termination (NULL) byte if the length is exactly 16 chars - applications have to provide 16+1 bytes storage if the ID is stored as string + * @param param_value Parameter value + * @param param_type Parameter type. + * @param param_count Total number of parameters + * @param param_index Index of this parameter + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_param_ext_value_send(mavlink_channel_t chan, const char *param_id, const char *param_value, uint8_t param_type, uint16_t param_count, uint16_t param_index) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_PARAM_EXT_VALUE_LEN]; + _mav_put_uint16_t(buf, 0, param_count); + _mav_put_uint16_t(buf, 2, param_index); + _mav_put_uint8_t(buf, 148, param_type); + _mav_put_char_array(buf, 4, param_id, 16); + _mav_put_char_array(buf, 20, param_value, 128); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PARAM_EXT_VALUE, buf, MAVLINK_MSG_ID_PARAM_EXT_VALUE_MIN_LEN, MAVLINK_MSG_ID_PARAM_EXT_VALUE_LEN, MAVLINK_MSG_ID_PARAM_EXT_VALUE_CRC); +#else + mavlink_param_ext_value_t packet; + packet.param_count = param_count; + packet.param_index = param_index; + packet.param_type = param_type; + mav_array_memcpy(packet.param_id, param_id, sizeof(char)*16); + mav_array_memcpy(packet.param_value, param_value, sizeof(char)*128); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PARAM_EXT_VALUE, (const char *)&packet, MAVLINK_MSG_ID_PARAM_EXT_VALUE_MIN_LEN, MAVLINK_MSG_ID_PARAM_EXT_VALUE_LEN, MAVLINK_MSG_ID_PARAM_EXT_VALUE_CRC); +#endif +} + +/** + * @brief Send a param_ext_value message + * @param chan MAVLink channel to send the message + * @param struct The MAVLink struct to serialize + */ +static inline void mavlink_msg_param_ext_value_send_struct(mavlink_channel_t chan, const mavlink_param_ext_value_t* param_ext_value) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + mavlink_msg_param_ext_value_send(chan, param_ext_value->param_id, param_ext_value->param_value, param_ext_value->param_type, param_ext_value->param_count, param_ext_value->param_index); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PARAM_EXT_VALUE, (const char *)param_ext_value, MAVLINK_MSG_ID_PARAM_EXT_VALUE_MIN_LEN, MAVLINK_MSG_ID_PARAM_EXT_VALUE_LEN, MAVLINK_MSG_ID_PARAM_EXT_VALUE_CRC); +#endif +} + +#if MAVLINK_MSG_ID_PARAM_EXT_VALUE_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_param_ext_value_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, const char *param_id, const char *param_value, uint8_t param_type, uint16_t param_count, uint16_t param_index) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint16_t(buf, 0, param_count); + _mav_put_uint16_t(buf, 2, param_index); + _mav_put_uint8_t(buf, 148, param_type); + _mav_put_char_array(buf, 4, param_id, 16); + _mav_put_char_array(buf, 20, param_value, 128); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PARAM_EXT_VALUE, buf, MAVLINK_MSG_ID_PARAM_EXT_VALUE_MIN_LEN, MAVLINK_MSG_ID_PARAM_EXT_VALUE_LEN, MAVLINK_MSG_ID_PARAM_EXT_VALUE_CRC); +#else + mavlink_param_ext_value_t *packet = (mavlink_param_ext_value_t *)msgbuf; + packet->param_count = param_count; + packet->param_index = param_index; + packet->param_type = param_type; + mav_array_memcpy(packet->param_id, param_id, sizeof(char)*16); + mav_array_memcpy(packet->param_value, param_value, sizeof(char)*128); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PARAM_EXT_VALUE, (const char *)packet, MAVLINK_MSG_ID_PARAM_EXT_VALUE_MIN_LEN, MAVLINK_MSG_ID_PARAM_EXT_VALUE_LEN, MAVLINK_MSG_ID_PARAM_EXT_VALUE_CRC); +#endif +} +#endif + +#endif + +// MESSAGE PARAM_EXT_VALUE UNPACKING + + +/** + * @brief Get field param_id from param_ext_value message + * + * @return Parameter id, terminated by NULL if the length is less than 16 human-readable chars and WITHOUT null termination (NULL) byte if the length is exactly 16 chars - applications have to provide 16+1 bytes storage if the ID is stored as string + */ +static inline uint16_t mavlink_msg_param_ext_value_get_param_id(const mavlink_message_t* msg, char *param_id) +{ + return _MAV_RETURN_char_array(msg, param_id, 16, 4); +} + +/** + * @brief Get field param_value from param_ext_value message + * + * @return Parameter value + */ +static inline uint16_t mavlink_msg_param_ext_value_get_param_value(const mavlink_message_t* msg, char *param_value) +{ + return _MAV_RETURN_char_array(msg, param_value, 128, 20); +} + +/** + * @brief Get field param_type from param_ext_value message + * + * @return Parameter type. + */ +static inline uint8_t mavlink_msg_param_ext_value_get_param_type(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 148); +} + +/** + * @brief Get field param_count from param_ext_value message + * + * @return Total number of parameters + */ +static inline uint16_t mavlink_msg_param_ext_value_get_param_count(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 0); +} + +/** + * @brief Get field param_index from param_ext_value message + * + * @return Index of this parameter + */ +static inline uint16_t mavlink_msg_param_ext_value_get_param_index(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 2); +} + +/** + * @brief Decode a param_ext_value message into a struct + * + * @param msg The message to decode + * @param param_ext_value C-struct to decode the message contents into + */ +static inline void mavlink_msg_param_ext_value_decode(const mavlink_message_t* msg, mavlink_param_ext_value_t* param_ext_value) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + param_ext_value->param_count = mavlink_msg_param_ext_value_get_param_count(msg); + param_ext_value->param_index = mavlink_msg_param_ext_value_get_param_index(msg); + mavlink_msg_param_ext_value_get_param_id(msg, param_ext_value->param_id); + mavlink_msg_param_ext_value_get_param_value(msg, param_ext_value->param_value); + param_ext_value->param_type = mavlink_msg_param_ext_value_get_param_type(msg); +#else + uint8_t len = msg->len < MAVLINK_MSG_ID_PARAM_EXT_VALUE_LEN? msg->len : MAVLINK_MSG_ID_PARAM_EXT_VALUE_LEN; + memset(param_ext_value, 0, MAVLINK_MSG_ID_PARAM_EXT_VALUE_LEN); + memcpy(param_ext_value, _MAV_PAYLOAD(msg), len); +#endif +} diff --git a/lib/main/MAVLink/common/mavlink_msg_play_tune.h b/lib/main/MAVLink/common/mavlink_msg_play_tune.h new file mode 100644 index 0000000000..c318448e24 --- /dev/null +++ b/lib/main/MAVLink/common/mavlink_msg_play_tune.h @@ -0,0 +1,281 @@ +#pragma once +// MESSAGE PLAY_TUNE PACKING + +#define MAVLINK_MSG_ID_PLAY_TUNE 258 + + +typedef struct __mavlink_play_tune_t { + uint8_t target_system; /*< System ID*/ + uint8_t target_component; /*< Component ID*/ + char tune[30]; /*< tune in board specific format*/ + char tune2[200]; /*< tune extension (appended to tune)*/ +} mavlink_play_tune_t; + +#define MAVLINK_MSG_ID_PLAY_TUNE_LEN 232 +#define MAVLINK_MSG_ID_PLAY_TUNE_MIN_LEN 32 +#define MAVLINK_MSG_ID_258_LEN 232 +#define MAVLINK_MSG_ID_258_MIN_LEN 32 + +#define MAVLINK_MSG_ID_PLAY_TUNE_CRC 187 +#define MAVLINK_MSG_ID_258_CRC 187 + +#define MAVLINK_MSG_PLAY_TUNE_FIELD_TUNE_LEN 30 +#define MAVLINK_MSG_PLAY_TUNE_FIELD_TUNE2_LEN 200 + +#if MAVLINK_COMMAND_24BIT +#define MAVLINK_MESSAGE_INFO_PLAY_TUNE { \ + 258, \ + "PLAY_TUNE", \ + 4, \ + { { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 0, offsetof(mavlink_play_tune_t, target_system) }, \ + { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 1, offsetof(mavlink_play_tune_t, target_component) }, \ + { "tune", NULL, MAVLINK_TYPE_CHAR, 30, 2, offsetof(mavlink_play_tune_t, tune) }, \ + { "tune2", NULL, MAVLINK_TYPE_CHAR, 200, 32, offsetof(mavlink_play_tune_t, tune2) }, \ + } \ +} +#else +#define MAVLINK_MESSAGE_INFO_PLAY_TUNE { \ + "PLAY_TUNE", \ + 4, \ + { { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 0, offsetof(mavlink_play_tune_t, target_system) }, \ + { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 1, offsetof(mavlink_play_tune_t, target_component) }, \ + { "tune", NULL, MAVLINK_TYPE_CHAR, 30, 2, offsetof(mavlink_play_tune_t, tune) }, \ + { "tune2", NULL, MAVLINK_TYPE_CHAR, 200, 32, offsetof(mavlink_play_tune_t, tune2) }, \ + } \ +} +#endif + +/** + * @brief Pack a play_tune message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param target_system System ID + * @param target_component Component ID + * @param tune tune in board specific format + * @param tune2 tune extension (appended to tune) + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_play_tune_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint8_t target_system, uint8_t target_component, const char *tune, const char *tune2) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_PLAY_TUNE_LEN]; + _mav_put_uint8_t(buf, 0, target_system); + _mav_put_uint8_t(buf, 1, target_component); + _mav_put_char_array(buf, 2, tune, 30); + _mav_put_char_array(buf, 32, tune2, 200); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_PLAY_TUNE_LEN); +#else + mavlink_play_tune_t packet; + packet.target_system = target_system; + packet.target_component = target_component; + mav_array_memcpy(packet.tune, tune, sizeof(char)*30); + mav_array_memcpy(packet.tune2, tune2, sizeof(char)*200); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_PLAY_TUNE_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_PLAY_TUNE; + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_PLAY_TUNE_MIN_LEN, MAVLINK_MSG_ID_PLAY_TUNE_LEN, MAVLINK_MSG_ID_PLAY_TUNE_CRC); +} + +/** + * @brief Pack a play_tune message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param target_system System ID + * @param target_component Component ID + * @param tune tune in board specific format + * @param tune2 tune extension (appended to tune) + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_play_tune_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint8_t target_system,uint8_t target_component,const char *tune,const char *tune2) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_PLAY_TUNE_LEN]; + _mav_put_uint8_t(buf, 0, target_system); + _mav_put_uint8_t(buf, 1, target_component); + _mav_put_char_array(buf, 2, tune, 30); + _mav_put_char_array(buf, 32, tune2, 200); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_PLAY_TUNE_LEN); +#else + mavlink_play_tune_t packet; + packet.target_system = target_system; + packet.target_component = target_component; + mav_array_memcpy(packet.tune, tune, sizeof(char)*30); + mav_array_memcpy(packet.tune2, tune2, sizeof(char)*200); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_PLAY_TUNE_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_PLAY_TUNE; + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_PLAY_TUNE_MIN_LEN, MAVLINK_MSG_ID_PLAY_TUNE_LEN, MAVLINK_MSG_ID_PLAY_TUNE_CRC); +} + +/** + * @brief Encode a play_tune struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param play_tune C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_play_tune_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_play_tune_t* play_tune) +{ + return mavlink_msg_play_tune_pack(system_id, component_id, msg, play_tune->target_system, play_tune->target_component, play_tune->tune, play_tune->tune2); +} + +/** + * @brief Encode a play_tune struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param play_tune C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_play_tune_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_play_tune_t* play_tune) +{ + return mavlink_msg_play_tune_pack_chan(system_id, component_id, chan, msg, play_tune->target_system, play_tune->target_component, play_tune->tune, play_tune->tune2); +} + +/** + * @brief Send a play_tune message + * @param chan MAVLink channel to send the message + * + * @param target_system System ID + * @param target_component Component ID + * @param tune tune in board specific format + * @param tune2 tune extension (appended to tune) + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_play_tune_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, const char *tune, const char *tune2) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_PLAY_TUNE_LEN]; + _mav_put_uint8_t(buf, 0, target_system); + _mav_put_uint8_t(buf, 1, target_component); + _mav_put_char_array(buf, 2, tune, 30); + _mav_put_char_array(buf, 32, tune2, 200); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PLAY_TUNE, buf, MAVLINK_MSG_ID_PLAY_TUNE_MIN_LEN, MAVLINK_MSG_ID_PLAY_TUNE_LEN, MAVLINK_MSG_ID_PLAY_TUNE_CRC); +#else + mavlink_play_tune_t packet; + packet.target_system = target_system; + packet.target_component = target_component; + mav_array_memcpy(packet.tune, tune, sizeof(char)*30); + mav_array_memcpy(packet.tune2, tune2, sizeof(char)*200); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PLAY_TUNE, (const char *)&packet, MAVLINK_MSG_ID_PLAY_TUNE_MIN_LEN, MAVLINK_MSG_ID_PLAY_TUNE_LEN, MAVLINK_MSG_ID_PLAY_TUNE_CRC); +#endif +} + +/** + * @brief Send a play_tune message + * @param chan MAVLink channel to send the message + * @param struct The MAVLink struct to serialize + */ +static inline void mavlink_msg_play_tune_send_struct(mavlink_channel_t chan, const mavlink_play_tune_t* play_tune) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + mavlink_msg_play_tune_send(chan, play_tune->target_system, play_tune->target_component, play_tune->tune, play_tune->tune2); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PLAY_TUNE, (const char *)play_tune, MAVLINK_MSG_ID_PLAY_TUNE_MIN_LEN, MAVLINK_MSG_ID_PLAY_TUNE_LEN, MAVLINK_MSG_ID_PLAY_TUNE_CRC); +#endif +} + +#if MAVLINK_MSG_ID_PLAY_TUNE_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_play_tune_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, const char *tune, const char *tune2) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint8_t(buf, 0, target_system); + _mav_put_uint8_t(buf, 1, target_component); + _mav_put_char_array(buf, 2, tune, 30); + _mav_put_char_array(buf, 32, tune2, 200); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PLAY_TUNE, buf, MAVLINK_MSG_ID_PLAY_TUNE_MIN_LEN, MAVLINK_MSG_ID_PLAY_TUNE_LEN, MAVLINK_MSG_ID_PLAY_TUNE_CRC); +#else + mavlink_play_tune_t *packet = (mavlink_play_tune_t *)msgbuf; + packet->target_system = target_system; + packet->target_component = target_component; + mav_array_memcpy(packet->tune, tune, sizeof(char)*30); + mav_array_memcpy(packet->tune2, tune2, sizeof(char)*200); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PLAY_TUNE, (const char *)packet, MAVLINK_MSG_ID_PLAY_TUNE_MIN_LEN, MAVLINK_MSG_ID_PLAY_TUNE_LEN, MAVLINK_MSG_ID_PLAY_TUNE_CRC); +#endif +} +#endif + +#endif + +// MESSAGE PLAY_TUNE UNPACKING + + +/** + * @brief Get field target_system from play_tune message + * + * @return System ID + */ +static inline uint8_t mavlink_msg_play_tune_get_target_system(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 0); +} + +/** + * @brief Get field target_component from play_tune message + * + * @return Component ID + */ +static inline uint8_t mavlink_msg_play_tune_get_target_component(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 1); +} + +/** + * @brief Get field tune from play_tune message + * + * @return tune in board specific format + */ +static inline uint16_t mavlink_msg_play_tune_get_tune(const mavlink_message_t* msg, char *tune) +{ + return _MAV_RETURN_char_array(msg, tune, 30, 2); +} + +/** + * @brief Get field tune2 from play_tune message + * + * @return tune extension (appended to tune) + */ +static inline uint16_t mavlink_msg_play_tune_get_tune2(const mavlink_message_t* msg, char *tune2) +{ + return _MAV_RETURN_char_array(msg, tune2, 200, 32); +} + +/** + * @brief Decode a play_tune message into a struct + * + * @param msg The message to decode + * @param play_tune C-struct to decode the message contents into + */ +static inline void mavlink_msg_play_tune_decode(const mavlink_message_t* msg, mavlink_play_tune_t* play_tune) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + play_tune->target_system = mavlink_msg_play_tune_get_target_system(msg); + play_tune->target_component = mavlink_msg_play_tune_get_target_component(msg); + mavlink_msg_play_tune_get_tune(msg, play_tune->tune); + mavlink_msg_play_tune_get_tune2(msg, play_tune->tune2); +#else + uint8_t len = msg->len < MAVLINK_MSG_ID_PLAY_TUNE_LEN? msg->len : MAVLINK_MSG_ID_PLAY_TUNE_LEN; + memset(play_tune, 0, MAVLINK_MSG_ID_PLAY_TUNE_LEN); + memcpy(play_tune, _MAV_PAYLOAD(msg), len); +#endif +} diff --git a/lib/main/MAVLink/common/mavlink_msg_play_tune_v2.h b/lib/main/MAVLink/common/mavlink_msg_play_tune_v2.h new file mode 100644 index 0000000000..c47d6e6a8a --- /dev/null +++ b/lib/main/MAVLink/common/mavlink_msg_play_tune_v2.h @@ -0,0 +1,280 @@ +#pragma once +// MESSAGE PLAY_TUNE_V2 PACKING + +#define MAVLINK_MSG_ID_PLAY_TUNE_V2 400 + + +typedef struct __mavlink_play_tune_v2_t { + uint32_t format; /*< Tune format*/ + uint8_t target_system; /*< System ID*/ + uint8_t target_component; /*< Component ID*/ + char tune[248]; /*< Tune definition as a NULL-terminated string.*/ +} mavlink_play_tune_v2_t; + +#define MAVLINK_MSG_ID_PLAY_TUNE_V2_LEN 254 +#define MAVLINK_MSG_ID_PLAY_TUNE_V2_MIN_LEN 254 +#define MAVLINK_MSG_ID_400_LEN 254 +#define MAVLINK_MSG_ID_400_MIN_LEN 254 + +#define MAVLINK_MSG_ID_PLAY_TUNE_V2_CRC 110 +#define MAVLINK_MSG_ID_400_CRC 110 + +#define MAVLINK_MSG_PLAY_TUNE_V2_FIELD_TUNE_LEN 248 + +#if MAVLINK_COMMAND_24BIT +#define MAVLINK_MESSAGE_INFO_PLAY_TUNE_V2 { \ + 400, \ + "PLAY_TUNE_V2", \ + 4, \ + { { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 4, offsetof(mavlink_play_tune_v2_t, target_system) }, \ + { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 5, offsetof(mavlink_play_tune_v2_t, target_component) }, \ + { "format", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_play_tune_v2_t, format) }, \ + { "tune", NULL, MAVLINK_TYPE_CHAR, 248, 6, offsetof(mavlink_play_tune_v2_t, tune) }, \ + } \ +} +#else +#define MAVLINK_MESSAGE_INFO_PLAY_TUNE_V2 { \ + "PLAY_TUNE_V2", \ + 4, \ + { { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 4, offsetof(mavlink_play_tune_v2_t, target_system) }, \ + { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 5, offsetof(mavlink_play_tune_v2_t, target_component) }, \ + { "format", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_play_tune_v2_t, format) }, \ + { "tune", NULL, MAVLINK_TYPE_CHAR, 248, 6, offsetof(mavlink_play_tune_v2_t, tune) }, \ + } \ +} +#endif + +/** + * @brief Pack a play_tune_v2 message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param target_system System ID + * @param target_component Component ID + * @param format Tune format + * @param tune Tune definition as a NULL-terminated string. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_play_tune_v2_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint8_t target_system, uint8_t target_component, uint32_t format, const char *tune) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_PLAY_TUNE_V2_LEN]; + _mav_put_uint32_t(buf, 0, format); + _mav_put_uint8_t(buf, 4, target_system); + _mav_put_uint8_t(buf, 5, target_component); + _mav_put_char_array(buf, 6, tune, 248); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_PLAY_TUNE_V2_LEN); +#else + mavlink_play_tune_v2_t packet; + packet.format = format; + packet.target_system = target_system; + packet.target_component = target_component; + mav_array_memcpy(packet.tune, tune, sizeof(char)*248); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_PLAY_TUNE_V2_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_PLAY_TUNE_V2; + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_PLAY_TUNE_V2_MIN_LEN, MAVLINK_MSG_ID_PLAY_TUNE_V2_LEN, MAVLINK_MSG_ID_PLAY_TUNE_V2_CRC); +} + +/** + * @brief Pack a play_tune_v2 message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param target_system System ID + * @param target_component Component ID + * @param format Tune format + * @param tune Tune definition as a NULL-terminated string. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_play_tune_v2_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint8_t target_system,uint8_t target_component,uint32_t format,const char *tune) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_PLAY_TUNE_V2_LEN]; + _mav_put_uint32_t(buf, 0, format); + _mav_put_uint8_t(buf, 4, target_system); + _mav_put_uint8_t(buf, 5, target_component); + _mav_put_char_array(buf, 6, tune, 248); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_PLAY_TUNE_V2_LEN); +#else + mavlink_play_tune_v2_t packet; + packet.format = format; + packet.target_system = target_system; + packet.target_component = target_component; + mav_array_memcpy(packet.tune, tune, sizeof(char)*248); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_PLAY_TUNE_V2_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_PLAY_TUNE_V2; + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_PLAY_TUNE_V2_MIN_LEN, MAVLINK_MSG_ID_PLAY_TUNE_V2_LEN, MAVLINK_MSG_ID_PLAY_TUNE_V2_CRC); +} + +/** + * @brief Encode a play_tune_v2 struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param play_tune_v2 C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_play_tune_v2_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_play_tune_v2_t* play_tune_v2) +{ + return mavlink_msg_play_tune_v2_pack(system_id, component_id, msg, play_tune_v2->target_system, play_tune_v2->target_component, play_tune_v2->format, play_tune_v2->tune); +} + +/** + * @brief Encode a play_tune_v2 struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param play_tune_v2 C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_play_tune_v2_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_play_tune_v2_t* play_tune_v2) +{ + return mavlink_msg_play_tune_v2_pack_chan(system_id, component_id, chan, msg, play_tune_v2->target_system, play_tune_v2->target_component, play_tune_v2->format, play_tune_v2->tune); +} + +/** + * @brief Send a play_tune_v2 message + * @param chan MAVLink channel to send the message + * + * @param target_system System ID + * @param target_component Component ID + * @param format Tune format + * @param tune Tune definition as a NULL-terminated string. + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_play_tune_v2_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint32_t format, const char *tune) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_PLAY_TUNE_V2_LEN]; + _mav_put_uint32_t(buf, 0, format); + _mav_put_uint8_t(buf, 4, target_system); + _mav_put_uint8_t(buf, 5, target_component); + _mav_put_char_array(buf, 6, tune, 248); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PLAY_TUNE_V2, buf, MAVLINK_MSG_ID_PLAY_TUNE_V2_MIN_LEN, MAVLINK_MSG_ID_PLAY_TUNE_V2_LEN, MAVLINK_MSG_ID_PLAY_TUNE_V2_CRC); +#else + mavlink_play_tune_v2_t packet; + packet.format = format; + packet.target_system = target_system; + packet.target_component = target_component; + mav_array_memcpy(packet.tune, tune, sizeof(char)*248); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PLAY_TUNE_V2, (const char *)&packet, MAVLINK_MSG_ID_PLAY_TUNE_V2_MIN_LEN, MAVLINK_MSG_ID_PLAY_TUNE_V2_LEN, MAVLINK_MSG_ID_PLAY_TUNE_V2_CRC); +#endif +} + +/** + * @brief Send a play_tune_v2 message + * @param chan MAVLink channel to send the message + * @param struct The MAVLink struct to serialize + */ +static inline void mavlink_msg_play_tune_v2_send_struct(mavlink_channel_t chan, const mavlink_play_tune_v2_t* play_tune_v2) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + mavlink_msg_play_tune_v2_send(chan, play_tune_v2->target_system, play_tune_v2->target_component, play_tune_v2->format, play_tune_v2->tune); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PLAY_TUNE_V2, (const char *)play_tune_v2, MAVLINK_MSG_ID_PLAY_TUNE_V2_MIN_LEN, MAVLINK_MSG_ID_PLAY_TUNE_V2_LEN, MAVLINK_MSG_ID_PLAY_TUNE_V2_CRC); +#endif +} + +#if MAVLINK_MSG_ID_PLAY_TUNE_V2_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_play_tune_v2_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint32_t format, const char *tune) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint32_t(buf, 0, format); + _mav_put_uint8_t(buf, 4, target_system); + _mav_put_uint8_t(buf, 5, target_component); + _mav_put_char_array(buf, 6, tune, 248); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PLAY_TUNE_V2, buf, MAVLINK_MSG_ID_PLAY_TUNE_V2_MIN_LEN, MAVLINK_MSG_ID_PLAY_TUNE_V2_LEN, MAVLINK_MSG_ID_PLAY_TUNE_V2_CRC); +#else + mavlink_play_tune_v2_t *packet = (mavlink_play_tune_v2_t *)msgbuf; + packet->format = format; + packet->target_system = target_system; + packet->target_component = target_component; + mav_array_memcpy(packet->tune, tune, sizeof(char)*248); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PLAY_TUNE_V2, (const char *)packet, MAVLINK_MSG_ID_PLAY_TUNE_V2_MIN_LEN, MAVLINK_MSG_ID_PLAY_TUNE_V2_LEN, MAVLINK_MSG_ID_PLAY_TUNE_V2_CRC); +#endif +} +#endif + +#endif + +// MESSAGE PLAY_TUNE_V2 UNPACKING + + +/** + * @brief Get field target_system from play_tune_v2 message + * + * @return System ID + */ +static inline uint8_t mavlink_msg_play_tune_v2_get_target_system(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 4); +} + +/** + * @brief Get field target_component from play_tune_v2 message + * + * @return Component ID + */ +static inline uint8_t mavlink_msg_play_tune_v2_get_target_component(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 5); +} + +/** + * @brief Get field format from play_tune_v2 message + * + * @return Tune format + */ +static inline uint32_t mavlink_msg_play_tune_v2_get_format(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint32_t(msg, 0); +} + +/** + * @brief Get field tune from play_tune_v2 message + * + * @return Tune definition as a NULL-terminated string. + */ +static inline uint16_t mavlink_msg_play_tune_v2_get_tune(const mavlink_message_t* msg, char *tune) +{ + return _MAV_RETURN_char_array(msg, tune, 248, 6); +} + +/** + * @brief Decode a play_tune_v2 message into a struct + * + * @param msg The message to decode + * @param play_tune_v2 C-struct to decode the message contents into + */ +static inline void mavlink_msg_play_tune_v2_decode(const mavlink_message_t* msg, mavlink_play_tune_v2_t* play_tune_v2) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + play_tune_v2->format = mavlink_msg_play_tune_v2_get_format(msg); + play_tune_v2->target_system = mavlink_msg_play_tune_v2_get_target_system(msg); + play_tune_v2->target_component = mavlink_msg_play_tune_v2_get_target_component(msg); + mavlink_msg_play_tune_v2_get_tune(msg, play_tune_v2->tune); +#else + uint8_t len = msg->len < MAVLINK_MSG_ID_PLAY_TUNE_V2_LEN? msg->len : MAVLINK_MSG_ID_PLAY_TUNE_V2_LEN; + memset(play_tune_v2, 0, MAVLINK_MSG_ID_PLAY_TUNE_V2_LEN); + memcpy(play_tune_v2, _MAV_PAYLOAD(msg), len); +#endif +} diff --git a/lib/main/MAVLink/common/mavlink_msg_raw_imu.h b/lib/main/MAVLink/common/mavlink_msg_raw_imu.h index 0e0cf7141c..eeb4eacb2e 100755 --- a/lib/main/MAVLink/common/mavlink_msg_raw_imu.h +++ b/lib/main/MAVLink/common/mavlink_msg_raw_imu.h @@ -3,7 +3,7 @@ #define MAVLINK_MSG_ID_RAW_IMU 27 - +MAVPACKED( typedef struct __mavlink_raw_imu_t { uint64_t time_usec; /*< [us] Timestamp (UNIX Epoch time or time since system boot). The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude of the number.*/ int16_t xacc; /*< X acceleration (raw)*/ @@ -15,11 +15,13 @@ typedef struct __mavlink_raw_imu_t { int16_t xmag; /*< X Magnetic field (raw)*/ int16_t ymag; /*< Y Magnetic field (raw)*/ int16_t zmag; /*< Z Magnetic field (raw)*/ -} mavlink_raw_imu_t; + uint8_t id; /*< Id. Ids are numbered from 0 and map to IMUs numbered from 1 (e.g. IMU1 will have a message with id=0)*/ + int16_t temperature; /*< [cdegC] Temperature, 0: IMU does not provide temperature values. If the IMU is at 0C it must send 1 (0.01C).*/ +}) mavlink_raw_imu_t; -#define MAVLINK_MSG_ID_RAW_IMU_LEN 26 +#define MAVLINK_MSG_ID_RAW_IMU_LEN 29 #define MAVLINK_MSG_ID_RAW_IMU_MIN_LEN 26 -#define MAVLINK_MSG_ID_27_LEN 26 +#define MAVLINK_MSG_ID_27_LEN 29 #define MAVLINK_MSG_ID_27_MIN_LEN 26 #define MAVLINK_MSG_ID_RAW_IMU_CRC 144 @@ -31,7 +33,7 @@ typedef struct __mavlink_raw_imu_t { #define MAVLINK_MESSAGE_INFO_RAW_IMU { \ 27, \ "RAW_IMU", \ - 10, \ + 12, \ { { "time_usec", NULL, MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_raw_imu_t, time_usec) }, \ { "xacc", NULL, MAVLINK_TYPE_INT16_T, 0, 8, offsetof(mavlink_raw_imu_t, xacc) }, \ { "yacc", NULL, MAVLINK_TYPE_INT16_T, 0, 10, offsetof(mavlink_raw_imu_t, yacc) }, \ @@ -42,12 +44,14 @@ typedef struct __mavlink_raw_imu_t { { "xmag", NULL, MAVLINK_TYPE_INT16_T, 0, 20, offsetof(mavlink_raw_imu_t, xmag) }, \ { "ymag", NULL, MAVLINK_TYPE_INT16_T, 0, 22, offsetof(mavlink_raw_imu_t, ymag) }, \ { "zmag", NULL, MAVLINK_TYPE_INT16_T, 0, 24, offsetof(mavlink_raw_imu_t, zmag) }, \ + { "id", NULL, MAVLINK_TYPE_UINT8_T, 0, 26, offsetof(mavlink_raw_imu_t, id) }, \ + { "temperature", NULL, MAVLINK_TYPE_INT16_T, 0, 27, offsetof(mavlink_raw_imu_t, temperature) }, \ } \ } #else #define MAVLINK_MESSAGE_INFO_RAW_IMU { \ "RAW_IMU", \ - 10, \ + 12, \ { { "time_usec", NULL, MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_raw_imu_t, time_usec) }, \ { "xacc", NULL, MAVLINK_TYPE_INT16_T, 0, 8, offsetof(mavlink_raw_imu_t, xacc) }, \ { "yacc", NULL, MAVLINK_TYPE_INT16_T, 0, 10, offsetof(mavlink_raw_imu_t, yacc) }, \ @@ -58,6 +62,8 @@ typedef struct __mavlink_raw_imu_t { { "xmag", NULL, MAVLINK_TYPE_INT16_T, 0, 20, offsetof(mavlink_raw_imu_t, xmag) }, \ { "ymag", NULL, MAVLINK_TYPE_INT16_T, 0, 22, offsetof(mavlink_raw_imu_t, ymag) }, \ { "zmag", NULL, MAVLINK_TYPE_INT16_T, 0, 24, offsetof(mavlink_raw_imu_t, zmag) }, \ + { "id", NULL, MAVLINK_TYPE_UINT8_T, 0, 26, offsetof(mavlink_raw_imu_t, id) }, \ + { "temperature", NULL, MAVLINK_TYPE_INT16_T, 0, 27, offsetof(mavlink_raw_imu_t, temperature) }, \ } \ } #endif @@ -78,10 +84,12 @@ typedef struct __mavlink_raw_imu_t { * @param xmag X Magnetic field (raw) * @param ymag Y Magnetic field (raw) * @param zmag Z Magnetic field (raw) + * @param id Id. Ids are numbered from 0 and map to IMUs numbered from 1 (e.g. IMU1 will have a message with id=0) + * @param temperature [cdegC] Temperature, 0: IMU does not provide temperature values. If the IMU is at 0C it must send 1 (0.01C). * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_raw_imu_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint64_t time_usec, int16_t xacc, int16_t yacc, int16_t zacc, int16_t xgyro, int16_t ygyro, int16_t zgyro, int16_t xmag, int16_t ymag, int16_t zmag) + uint64_t time_usec, int16_t xacc, int16_t yacc, int16_t zacc, int16_t xgyro, int16_t ygyro, int16_t zgyro, int16_t xmag, int16_t ymag, int16_t zmag, uint8_t id, int16_t temperature) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_RAW_IMU_LEN]; @@ -95,6 +103,8 @@ static inline uint16_t mavlink_msg_raw_imu_pack(uint8_t system_id, uint8_t compo _mav_put_int16_t(buf, 20, xmag); _mav_put_int16_t(buf, 22, ymag); _mav_put_int16_t(buf, 24, zmag); + _mav_put_uint8_t(buf, 26, id); + _mav_put_int16_t(buf, 27, temperature); memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_RAW_IMU_LEN); #else @@ -109,6 +119,8 @@ static inline uint16_t mavlink_msg_raw_imu_pack(uint8_t system_id, uint8_t compo packet.xmag = xmag; packet.ymag = ymag; packet.zmag = zmag; + packet.id = id; + packet.temperature = temperature; memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_RAW_IMU_LEN); #endif @@ -133,11 +145,13 @@ static inline uint16_t mavlink_msg_raw_imu_pack(uint8_t system_id, uint8_t compo * @param xmag X Magnetic field (raw) * @param ymag Y Magnetic field (raw) * @param zmag Z Magnetic field (raw) + * @param id Id. Ids are numbered from 0 and map to IMUs numbered from 1 (e.g. IMU1 will have a message with id=0) + * @param temperature [cdegC] Temperature, 0: IMU does not provide temperature values. If the IMU is at 0C it must send 1 (0.01C). * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_raw_imu_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, - uint64_t time_usec,int16_t xacc,int16_t yacc,int16_t zacc,int16_t xgyro,int16_t ygyro,int16_t zgyro,int16_t xmag,int16_t ymag,int16_t zmag) + uint64_t time_usec,int16_t xacc,int16_t yacc,int16_t zacc,int16_t xgyro,int16_t ygyro,int16_t zgyro,int16_t xmag,int16_t ymag,int16_t zmag,uint8_t id,int16_t temperature) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_RAW_IMU_LEN]; @@ -151,6 +165,8 @@ static inline uint16_t mavlink_msg_raw_imu_pack_chan(uint8_t system_id, uint8_t _mav_put_int16_t(buf, 20, xmag); _mav_put_int16_t(buf, 22, ymag); _mav_put_int16_t(buf, 24, zmag); + _mav_put_uint8_t(buf, 26, id); + _mav_put_int16_t(buf, 27, temperature); memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_RAW_IMU_LEN); #else @@ -165,6 +181,8 @@ static inline uint16_t mavlink_msg_raw_imu_pack_chan(uint8_t system_id, uint8_t packet.xmag = xmag; packet.ymag = ymag; packet.zmag = zmag; + packet.id = id; + packet.temperature = temperature; memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_RAW_IMU_LEN); #endif @@ -183,7 +201,7 @@ static inline uint16_t mavlink_msg_raw_imu_pack_chan(uint8_t system_id, uint8_t */ static inline uint16_t mavlink_msg_raw_imu_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_raw_imu_t* raw_imu) { - return mavlink_msg_raw_imu_pack(system_id, component_id, msg, raw_imu->time_usec, raw_imu->xacc, raw_imu->yacc, raw_imu->zacc, raw_imu->xgyro, raw_imu->ygyro, raw_imu->zgyro, raw_imu->xmag, raw_imu->ymag, raw_imu->zmag); + return mavlink_msg_raw_imu_pack(system_id, component_id, msg, raw_imu->time_usec, raw_imu->xacc, raw_imu->yacc, raw_imu->zacc, raw_imu->xgyro, raw_imu->ygyro, raw_imu->zgyro, raw_imu->xmag, raw_imu->ymag, raw_imu->zmag, raw_imu->id, raw_imu->temperature); } /** @@ -197,7 +215,7 @@ static inline uint16_t mavlink_msg_raw_imu_encode(uint8_t system_id, uint8_t com */ static inline uint16_t mavlink_msg_raw_imu_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_raw_imu_t* raw_imu) { - return mavlink_msg_raw_imu_pack_chan(system_id, component_id, chan, msg, raw_imu->time_usec, raw_imu->xacc, raw_imu->yacc, raw_imu->zacc, raw_imu->xgyro, raw_imu->ygyro, raw_imu->zgyro, raw_imu->xmag, raw_imu->ymag, raw_imu->zmag); + return mavlink_msg_raw_imu_pack_chan(system_id, component_id, chan, msg, raw_imu->time_usec, raw_imu->xacc, raw_imu->yacc, raw_imu->zacc, raw_imu->xgyro, raw_imu->ygyro, raw_imu->zgyro, raw_imu->xmag, raw_imu->ymag, raw_imu->zmag, raw_imu->id, raw_imu->temperature); } /** @@ -214,10 +232,12 @@ static inline uint16_t mavlink_msg_raw_imu_encode_chan(uint8_t system_id, uint8_ * @param xmag X Magnetic field (raw) * @param ymag Y Magnetic field (raw) * @param zmag Z Magnetic field (raw) + * @param id Id. Ids are numbered from 0 and map to IMUs numbered from 1 (e.g. IMU1 will have a message with id=0) + * @param temperature [cdegC] Temperature, 0: IMU does not provide temperature values. If the IMU is at 0C it must send 1 (0.01C). */ #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS -static inline void mavlink_msg_raw_imu_send(mavlink_channel_t chan, uint64_t time_usec, int16_t xacc, int16_t yacc, int16_t zacc, int16_t xgyro, int16_t ygyro, int16_t zgyro, int16_t xmag, int16_t ymag, int16_t zmag) +static inline void mavlink_msg_raw_imu_send(mavlink_channel_t chan, uint64_t time_usec, int16_t xacc, int16_t yacc, int16_t zacc, int16_t xgyro, int16_t ygyro, int16_t zgyro, int16_t xmag, int16_t ymag, int16_t zmag, uint8_t id, int16_t temperature) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_RAW_IMU_LEN]; @@ -231,6 +251,8 @@ static inline void mavlink_msg_raw_imu_send(mavlink_channel_t chan, uint64_t tim _mav_put_int16_t(buf, 20, xmag); _mav_put_int16_t(buf, 22, ymag); _mav_put_int16_t(buf, 24, zmag); + _mav_put_uint8_t(buf, 26, id); + _mav_put_int16_t(buf, 27, temperature); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RAW_IMU, buf, MAVLINK_MSG_ID_RAW_IMU_MIN_LEN, MAVLINK_MSG_ID_RAW_IMU_LEN, MAVLINK_MSG_ID_RAW_IMU_CRC); #else @@ -245,6 +267,8 @@ static inline void mavlink_msg_raw_imu_send(mavlink_channel_t chan, uint64_t tim packet.xmag = xmag; packet.ymag = ymag; packet.zmag = zmag; + packet.id = id; + packet.temperature = temperature; _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RAW_IMU, (const char *)&packet, MAVLINK_MSG_ID_RAW_IMU_MIN_LEN, MAVLINK_MSG_ID_RAW_IMU_LEN, MAVLINK_MSG_ID_RAW_IMU_CRC); #endif @@ -258,7 +282,7 @@ static inline void mavlink_msg_raw_imu_send(mavlink_channel_t chan, uint64_t tim static inline void mavlink_msg_raw_imu_send_struct(mavlink_channel_t chan, const mavlink_raw_imu_t* raw_imu) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - mavlink_msg_raw_imu_send(chan, raw_imu->time_usec, raw_imu->xacc, raw_imu->yacc, raw_imu->zacc, raw_imu->xgyro, raw_imu->ygyro, raw_imu->zgyro, raw_imu->xmag, raw_imu->ymag, raw_imu->zmag); + mavlink_msg_raw_imu_send(chan, raw_imu->time_usec, raw_imu->xacc, raw_imu->yacc, raw_imu->zacc, raw_imu->xgyro, raw_imu->ygyro, raw_imu->zgyro, raw_imu->xmag, raw_imu->ymag, raw_imu->zmag, raw_imu->id, raw_imu->temperature); #else _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RAW_IMU, (const char *)raw_imu, MAVLINK_MSG_ID_RAW_IMU_MIN_LEN, MAVLINK_MSG_ID_RAW_IMU_LEN, MAVLINK_MSG_ID_RAW_IMU_CRC); #endif @@ -272,7 +296,7 @@ static inline void mavlink_msg_raw_imu_send_struct(mavlink_channel_t chan, const is usually the receive buffer for the channel, and allows a reply to an incoming message with minimum stack space usage. */ -static inline void mavlink_msg_raw_imu_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint64_t time_usec, int16_t xacc, int16_t yacc, int16_t zacc, int16_t xgyro, int16_t ygyro, int16_t zgyro, int16_t xmag, int16_t ymag, int16_t zmag) +static inline void mavlink_msg_raw_imu_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint64_t time_usec, int16_t xacc, int16_t yacc, int16_t zacc, int16_t xgyro, int16_t ygyro, int16_t zgyro, int16_t xmag, int16_t ymag, int16_t zmag, uint8_t id, int16_t temperature) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char *buf = (char *)msgbuf; @@ -286,6 +310,8 @@ static inline void mavlink_msg_raw_imu_send_buf(mavlink_message_t *msgbuf, mavli _mav_put_int16_t(buf, 20, xmag); _mav_put_int16_t(buf, 22, ymag); _mav_put_int16_t(buf, 24, zmag); + _mav_put_uint8_t(buf, 26, id); + _mav_put_int16_t(buf, 27, temperature); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RAW_IMU, buf, MAVLINK_MSG_ID_RAW_IMU_MIN_LEN, MAVLINK_MSG_ID_RAW_IMU_LEN, MAVLINK_MSG_ID_RAW_IMU_CRC); #else @@ -300,6 +326,8 @@ static inline void mavlink_msg_raw_imu_send_buf(mavlink_message_t *msgbuf, mavli packet->xmag = xmag; packet->ymag = ymag; packet->zmag = zmag; + packet->id = id; + packet->temperature = temperature; _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RAW_IMU, (const char *)packet, MAVLINK_MSG_ID_RAW_IMU_MIN_LEN, MAVLINK_MSG_ID_RAW_IMU_LEN, MAVLINK_MSG_ID_RAW_IMU_CRC); #endif @@ -411,6 +439,26 @@ static inline int16_t mavlink_msg_raw_imu_get_zmag(const mavlink_message_t* msg) return _MAV_RETURN_int16_t(msg, 24); } +/** + * @brief Get field id from raw_imu message + * + * @return Id. Ids are numbered from 0 and map to IMUs numbered from 1 (e.g. IMU1 will have a message with id=0) + */ +static inline uint8_t mavlink_msg_raw_imu_get_id(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 26); +} + +/** + * @brief Get field temperature from raw_imu message + * + * @return [cdegC] Temperature, 0: IMU does not provide temperature values. If the IMU is at 0C it must send 1 (0.01C). + */ +static inline int16_t mavlink_msg_raw_imu_get_temperature(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int16_t(msg, 27); +} + /** * @brief Decode a raw_imu message into a struct * @@ -430,6 +478,8 @@ static inline void mavlink_msg_raw_imu_decode(const mavlink_message_t* msg, mavl raw_imu->xmag = mavlink_msg_raw_imu_get_xmag(msg); raw_imu->ymag = mavlink_msg_raw_imu_get_ymag(msg); raw_imu->zmag = mavlink_msg_raw_imu_get_zmag(msg); + raw_imu->id = mavlink_msg_raw_imu_get_id(msg); + raw_imu->temperature = mavlink_msg_raw_imu_get_temperature(msg); #else uint8_t len = msg->len < MAVLINK_MSG_ID_RAW_IMU_LEN? msg->len : MAVLINK_MSG_ID_RAW_IMU_LEN; memset(raw_imu, 0, MAVLINK_MSG_ID_RAW_IMU_LEN); diff --git a/lib/main/MAVLink/common/mavlink_msg_raw_rpm.h b/lib/main/MAVLink/common/mavlink_msg_raw_rpm.h new file mode 100644 index 0000000000..0fa4f1a0b5 --- /dev/null +++ b/lib/main/MAVLink/common/mavlink_msg_raw_rpm.h @@ -0,0 +1,238 @@ +#pragma once +// MESSAGE RAW_RPM PACKING + +#define MAVLINK_MSG_ID_RAW_RPM 339 + + +typedef struct __mavlink_raw_rpm_t { + float frequency; /*< [rpm] Indicated rate*/ + uint8_t index; /*< Index of this RPM sensor (0-indexed)*/ +} mavlink_raw_rpm_t; + +#define MAVLINK_MSG_ID_RAW_RPM_LEN 5 +#define MAVLINK_MSG_ID_RAW_RPM_MIN_LEN 5 +#define MAVLINK_MSG_ID_339_LEN 5 +#define MAVLINK_MSG_ID_339_MIN_LEN 5 + +#define MAVLINK_MSG_ID_RAW_RPM_CRC 199 +#define MAVLINK_MSG_ID_339_CRC 199 + + + +#if MAVLINK_COMMAND_24BIT +#define MAVLINK_MESSAGE_INFO_RAW_RPM { \ + 339, \ + "RAW_RPM", \ + 2, \ + { { "index", NULL, MAVLINK_TYPE_UINT8_T, 0, 4, offsetof(mavlink_raw_rpm_t, index) }, \ + { "frequency", NULL, MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_raw_rpm_t, frequency) }, \ + } \ +} +#else +#define MAVLINK_MESSAGE_INFO_RAW_RPM { \ + "RAW_RPM", \ + 2, \ + { { "index", NULL, MAVLINK_TYPE_UINT8_T, 0, 4, offsetof(mavlink_raw_rpm_t, index) }, \ + { "frequency", NULL, MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_raw_rpm_t, frequency) }, \ + } \ +} +#endif + +/** + * @brief Pack a raw_rpm message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param index Index of this RPM sensor (0-indexed) + * @param frequency [rpm] Indicated rate + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_raw_rpm_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint8_t index, float frequency) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_RAW_RPM_LEN]; + _mav_put_float(buf, 0, frequency); + _mav_put_uint8_t(buf, 4, index); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_RAW_RPM_LEN); +#else + mavlink_raw_rpm_t packet; + packet.frequency = frequency; + packet.index = index; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_RAW_RPM_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_RAW_RPM; + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_RAW_RPM_MIN_LEN, MAVLINK_MSG_ID_RAW_RPM_LEN, MAVLINK_MSG_ID_RAW_RPM_CRC); +} + +/** + * @brief Pack a raw_rpm message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param index Index of this RPM sensor (0-indexed) + * @param frequency [rpm] Indicated rate + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_raw_rpm_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint8_t index,float frequency) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_RAW_RPM_LEN]; + _mav_put_float(buf, 0, frequency); + _mav_put_uint8_t(buf, 4, index); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_RAW_RPM_LEN); +#else + mavlink_raw_rpm_t packet; + packet.frequency = frequency; + packet.index = index; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_RAW_RPM_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_RAW_RPM; + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_RAW_RPM_MIN_LEN, MAVLINK_MSG_ID_RAW_RPM_LEN, MAVLINK_MSG_ID_RAW_RPM_CRC); +} + +/** + * @brief Encode a raw_rpm struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param raw_rpm C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_raw_rpm_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_raw_rpm_t* raw_rpm) +{ + return mavlink_msg_raw_rpm_pack(system_id, component_id, msg, raw_rpm->index, raw_rpm->frequency); +} + +/** + * @brief Encode a raw_rpm struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param raw_rpm C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_raw_rpm_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_raw_rpm_t* raw_rpm) +{ + return mavlink_msg_raw_rpm_pack_chan(system_id, component_id, chan, msg, raw_rpm->index, raw_rpm->frequency); +} + +/** + * @brief Send a raw_rpm message + * @param chan MAVLink channel to send the message + * + * @param index Index of this RPM sensor (0-indexed) + * @param frequency [rpm] Indicated rate + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_raw_rpm_send(mavlink_channel_t chan, uint8_t index, float frequency) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_RAW_RPM_LEN]; + _mav_put_float(buf, 0, frequency); + _mav_put_uint8_t(buf, 4, index); + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RAW_RPM, buf, MAVLINK_MSG_ID_RAW_RPM_MIN_LEN, MAVLINK_MSG_ID_RAW_RPM_LEN, MAVLINK_MSG_ID_RAW_RPM_CRC); +#else + mavlink_raw_rpm_t packet; + packet.frequency = frequency; + packet.index = index; + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RAW_RPM, (const char *)&packet, MAVLINK_MSG_ID_RAW_RPM_MIN_LEN, MAVLINK_MSG_ID_RAW_RPM_LEN, MAVLINK_MSG_ID_RAW_RPM_CRC); +#endif +} + +/** + * @brief Send a raw_rpm message + * @param chan MAVLink channel to send the message + * @param struct The MAVLink struct to serialize + */ +static inline void mavlink_msg_raw_rpm_send_struct(mavlink_channel_t chan, const mavlink_raw_rpm_t* raw_rpm) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + mavlink_msg_raw_rpm_send(chan, raw_rpm->index, raw_rpm->frequency); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RAW_RPM, (const char *)raw_rpm, MAVLINK_MSG_ID_RAW_RPM_MIN_LEN, MAVLINK_MSG_ID_RAW_RPM_LEN, MAVLINK_MSG_ID_RAW_RPM_CRC); +#endif +} + +#if MAVLINK_MSG_ID_RAW_RPM_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_raw_rpm_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t index, float frequency) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_float(buf, 0, frequency); + _mav_put_uint8_t(buf, 4, index); + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RAW_RPM, buf, MAVLINK_MSG_ID_RAW_RPM_MIN_LEN, MAVLINK_MSG_ID_RAW_RPM_LEN, MAVLINK_MSG_ID_RAW_RPM_CRC); +#else + mavlink_raw_rpm_t *packet = (mavlink_raw_rpm_t *)msgbuf; + packet->frequency = frequency; + packet->index = index; + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RAW_RPM, (const char *)packet, MAVLINK_MSG_ID_RAW_RPM_MIN_LEN, MAVLINK_MSG_ID_RAW_RPM_LEN, MAVLINK_MSG_ID_RAW_RPM_CRC); +#endif +} +#endif + +#endif + +// MESSAGE RAW_RPM UNPACKING + + +/** + * @brief Get field index from raw_rpm message + * + * @return Index of this RPM sensor (0-indexed) + */ +static inline uint8_t mavlink_msg_raw_rpm_get_index(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 4); +} + +/** + * @brief Get field frequency from raw_rpm message + * + * @return [rpm] Indicated rate + */ +static inline float mavlink_msg_raw_rpm_get_frequency(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 0); +} + +/** + * @brief Decode a raw_rpm message into a struct + * + * @param msg The message to decode + * @param raw_rpm C-struct to decode the message contents into + */ +static inline void mavlink_msg_raw_rpm_decode(const mavlink_message_t* msg, mavlink_raw_rpm_t* raw_rpm) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + raw_rpm->frequency = mavlink_msg_raw_rpm_get_frequency(msg); + raw_rpm->index = mavlink_msg_raw_rpm_get_index(msg); +#else + uint8_t len = msg->len < MAVLINK_MSG_ID_RAW_RPM_LEN? msg->len : MAVLINK_MSG_ID_RAW_RPM_LEN; + memset(raw_rpm, 0, MAVLINK_MSG_ID_RAW_RPM_LEN); + memcpy(raw_rpm, _MAV_PAYLOAD(msg), len); +#endif +} diff --git a/lib/main/MAVLink/common/mavlink_msg_rc_channels_override.h b/lib/main/MAVLink/common/mavlink_msg_rc_channels_override.h index b45c6ae85b..a0d2a767ec 100755 --- a/lib/main/MAVLink/common/mavlink_msg_rc_channels_override.h +++ b/lib/main/MAVLink/common/mavlink_msg_rc_channels_override.h @@ -5,21 +5,31 @@ typedef struct __mavlink_rc_channels_override_t { - uint16_t chan1_raw; /*< [us] RC channel 1 value. A value of UINT16_MAX means to ignore this field.*/ - uint16_t chan2_raw; /*< [us] RC channel 2 value. A value of UINT16_MAX means to ignore this field.*/ - uint16_t chan3_raw; /*< [us] RC channel 3 value. A value of UINT16_MAX means to ignore this field.*/ - uint16_t chan4_raw; /*< [us] RC channel 4 value. A value of UINT16_MAX means to ignore this field.*/ - uint16_t chan5_raw; /*< [us] RC channel 5 value. A value of UINT16_MAX means to ignore this field.*/ - uint16_t chan6_raw; /*< [us] RC channel 6 value. A value of UINT16_MAX means to ignore this field.*/ - uint16_t chan7_raw; /*< [us] RC channel 7 value. A value of UINT16_MAX means to ignore this field.*/ - uint16_t chan8_raw; /*< [us] RC channel 8 value. A value of UINT16_MAX means to ignore this field.*/ + uint16_t chan1_raw; /*< [us] RC channel 1 value. A value of UINT16_MAX means to ignore this field. A value of 0 means to release this channel back to the RC radio.*/ + uint16_t chan2_raw; /*< [us] RC channel 2 value. A value of UINT16_MAX means to ignore this field. A value of 0 means to release this channel back to the RC radio.*/ + uint16_t chan3_raw; /*< [us] RC channel 3 value. A value of UINT16_MAX means to ignore this field. A value of 0 means to release this channel back to the RC radio.*/ + uint16_t chan4_raw; /*< [us] RC channel 4 value. A value of UINT16_MAX means to ignore this field. A value of 0 means to release this channel back to the RC radio.*/ + uint16_t chan5_raw; /*< [us] RC channel 5 value. A value of UINT16_MAX means to ignore this field. A value of 0 means to release this channel back to the RC radio.*/ + uint16_t chan6_raw; /*< [us] RC channel 6 value. A value of UINT16_MAX means to ignore this field. A value of 0 means to release this channel back to the RC radio.*/ + uint16_t chan7_raw; /*< [us] RC channel 7 value. A value of UINT16_MAX means to ignore this field. A value of 0 means to release this channel back to the RC radio.*/ + uint16_t chan8_raw; /*< [us] RC channel 8 value. A value of UINT16_MAX means to ignore this field. A value of 0 means to release this channel back to the RC radio.*/ uint8_t target_system; /*< System ID*/ uint8_t target_component; /*< Component ID*/ + uint16_t chan9_raw; /*< [us] RC channel 9 value. A value of 0 or UINT16_MAX means to ignore this field. A value of UINT16_MAX-1 means to release this channel back to the RC radio.*/ + uint16_t chan10_raw; /*< [us] RC channel 10 value. A value of 0 or UINT16_MAX means to ignore this field. A value of UINT16_MAX-1 means to release this channel back to the RC radio.*/ + uint16_t chan11_raw; /*< [us] RC channel 11 value. A value of 0 or UINT16_MAX means to ignore this field. A value of UINT16_MAX-1 means to release this channel back to the RC radio.*/ + uint16_t chan12_raw; /*< [us] RC channel 12 value. A value of 0 or UINT16_MAX means to ignore this field. A value of UINT16_MAX-1 means to release this channel back to the RC radio.*/ + uint16_t chan13_raw; /*< [us] RC channel 13 value. A value of 0 or UINT16_MAX means to ignore this field. A value of UINT16_MAX-1 means to release this channel back to the RC radio.*/ + uint16_t chan14_raw; /*< [us] RC channel 14 value. A value of 0 or UINT16_MAX means to ignore this field. A value of UINT16_MAX-1 means to release this channel back to the RC radio.*/ + uint16_t chan15_raw; /*< [us] RC channel 15 value. A value of 0 or UINT16_MAX means to ignore this field. A value of UINT16_MAX-1 means to release this channel back to the RC radio.*/ + uint16_t chan16_raw; /*< [us] RC channel 16 value. A value of 0 or UINT16_MAX means to ignore this field. A value of UINT16_MAX-1 means to release this channel back to the RC radio.*/ + uint16_t chan17_raw; /*< [us] RC channel 17 value. A value of 0 or UINT16_MAX means to ignore this field. A value of UINT16_MAX-1 means to release this channel back to the RC radio.*/ + uint16_t chan18_raw; /*< [us] RC channel 18 value. A value of 0 or UINT16_MAX means to ignore this field. A value of UINT16_MAX-1 means to release this channel back to the RC radio.*/ } mavlink_rc_channels_override_t; -#define MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE_LEN 18 +#define MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE_LEN 38 #define MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE_MIN_LEN 18 -#define MAVLINK_MSG_ID_70_LEN 18 +#define MAVLINK_MSG_ID_70_LEN 38 #define MAVLINK_MSG_ID_70_MIN_LEN 18 #define MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE_CRC 124 @@ -31,7 +41,7 @@ typedef struct __mavlink_rc_channels_override_t { #define MAVLINK_MESSAGE_INFO_RC_CHANNELS_OVERRIDE { \ 70, \ "RC_CHANNELS_OVERRIDE", \ - 10, \ + 20, \ { { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 16, offsetof(mavlink_rc_channels_override_t, target_system) }, \ { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 17, offsetof(mavlink_rc_channels_override_t, target_component) }, \ { "chan1_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 0, offsetof(mavlink_rc_channels_override_t, chan1_raw) }, \ @@ -42,12 +52,22 @@ typedef struct __mavlink_rc_channels_override_t { { "chan6_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 10, offsetof(mavlink_rc_channels_override_t, chan6_raw) }, \ { "chan7_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 12, offsetof(mavlink_rc_channels_override_t, chan7_raw) }, \ { "chan8_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 14, offsetof(mavlink_rc_channels_override_t, chan8_raw) }, \ + { "chan9_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 18, offsetof(mavlink_rc_channels_override_t, chan9_raw) }, \ + { "chan10_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 20, offsetof(mavlink_rc_channels_override_t, chan10_raw) }, \ + { "chan11_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 22, offsetof(mavlink_rc_channels_override_t, chan11_raw) }, \ + { "chan12_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 24, offsetof(mavlink_rc_channels_override_t, chan12_raw) }, \ + { "chan13_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 26, offsetof(mavlink_rc_channels_override_t, chan13_raw) }, \ + { "chan14_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 28, offsetof(mavlink_rc_channels_override_t, chan14_raw) }, \ + { "chan15_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 30, offsetof(mavlink_rc_channels_override_t, chan15_raw) }, \ + { "chan16_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 32, offsetof(mavlink_rc_channels_override_t, chan16_raw) }, \ + { "chan17_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 34, offsetof(mavlink_rc_channels_override_t, chan17_raw) }, \ + { "chan18_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 36, offsetof(mavlink_rc_channels_override_t, chan18_raw) }, \ } \ } #else #define MAVLINK_MESSAGE_INFO_RC_CHANNELS_OVERRIDE { \ "RC_CHANNELS_OVERRIDE", \ - 10, \ + 20, \ { { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 16, offsetof(mavlink_rc_channels_override_t, target_system) }, \ { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 17, offsetof(mavlink_rc_channels_override_t, target_component) }, \ { "chan1_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 0, offsetof(mavlink_rc_channels_override_t, chan1_raw) }, \ @@ -58,6 +78,16 @@ typedef struct __mavlink_rc_channels_override_t { { "chan6_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 10, offsetof(mavlink_rc_channels_override_t, chan6_raw) }, \ { "chan7_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 12, offsetof(mavlink_rc_channels_override_t, chan7_raw) }, \ { "chan8_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 14, offsetof(mavlink_rc_channels_override_t, chan8_raw) }, \ + { "chan9_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 18, offsetof(mavlink_rc_channels_override_t, chan9_raw) }, \ + { "chan10_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 20, offsetof(mavlink_rc_channels_override_t, chan10_raw) }, \ + { "chan11_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 22, offsetof(mavlink_rc_channels_override_t, chan11_raw) }, \ + { "chan12_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 24, offsetof(mavlink_rc_channels_override_t, chan12_raw) }, \ + { "chan13_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 26, offsetof(mavlink_rc_channels_override_t, chan13_raw) }, \ + { "chan14_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 28, offsetof(mavlink_rc_channels_override_t, chan14_raw) }, \ + { "chan15_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 30, offsetof(mavlink_rc_channels_override_t, chan15_raw) }, \ + { "chan16_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 32, offsetof(mavlink_rc_channels_override_t, chan16_raw) }, \ + { "chan17_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 34, offsetof(mavlink_rc_channels_override_t, chan17_raw) }, \ + { "chan18_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 36, offsetof(mavlink_rc_channels_override_t, chan18_raw) }, \ } \ } #endif @@ -70,18 +100,28 @@ typedef struct __mavlink_rc_channels_override_t { * * @param target_system System ID * @param target_component Component ID - * @param chan1_raw [us] RC channel 1 value. A value of UINT16_MAX means to ignore this field. - * @param chan2_raw [us] RC channel 2 value. A value of UINT16_MAX means to ignore this field. - * @param chan3_raw [us] RC channel 3 value. A value of UINT16_MAX means to ignore this field. - * @param chan4_raw [us] RC channel 4 value. A value of UINT16_MAX means to ignore this field. - * @param chan5_raw [us] RC channel 5 value. A value of UINT16_MAX means to ignore this field. - * @param chan6_raw [us] RC channel 6 value. A value of UINT16_MAX means to ignore this field. - * @param chan7_raw [us] RC channel 7 value. A value of UINT16_MAX means to ignore this field. - * @param chan8_raw [us] RC channel 8 value. A value of UINT16_MAX means to ignore this field. + * @param chan1_raw [us] RC channel 1 value. A value of UINT16_MAX means to ignore this field. A value of 0 means to release this channel back to the RC radio. + * @param chan2_raw [us] RC channel 2 value. A value of UINT16_MAX means to ignore this field. A value of 0 means to release this channel back to the RC radio. + * @param chan3_raw [us] RC channel 3 value. A value of UINT16_MAX means to ignore this field. A value of 0 means to release this channel back to the RC radio. + * @param chan4_raw [us] RC channel 4 value. A value of UINT16_MAX means to ignore this field. A value of 0 means to release this channel back to the RC radio. + * @param chan5_raw [us] RC channel 5 value. A value of UINT16_MAX means to ignore this field. A value of 0 means to release this channel back to the RC radio. + * @param chan6_raw [us] RC channel 6 value. A value of UINT16_MAX means to ignore this field. A value of 0 means to release this channel back to the RC radio. + * @param chan7_raw [us] RC channel 7 value. A value of UINT16_MAX means to ignore this field. A value of 0 means to release this channel back to the RC radio. + * @param chan8_raw [us] RC channel 8 value. A value of UINT16_MAX means to ignore this field. A value of 0 means to release this channel back to the RC radio. + * @param chan9_raw [us] RC channel 9 value. A value of 0 or UINT16_MAX means to ignore this field. A value of UINT16_MAX-1 means to release this channel back to the RC radio. + * @param chan10_raw [us] RC channel 10 value. A value of 0 or UINT16_MAX means to ignore this field. A value of UINT16_MAX-1 means to release this channel back to the RC radio. + * @param chan11_raw [us] RC channel 11 value. A value of 0 or UINT16_MAX means to ignore this field. A value of UINT16_MAX-1 means to release this channel back to the RC radio. + * @param chan12_raw [us] RC channel 12 value. A value of 0 or UINT16_MAX means to ignore this field. A value of UINT16_MAX-1 means to release this channel back to the RC radio. + * @param chan13_raw [us] RC channel 13 value. A value of 0 or UINT16_MAX means to ignore this field. A value of UINT16_MAX-1 means to release this channel back to the RC radio. + * @param chan14_raw [us] RC channel 14 value. A value of 0 or UINT16_MAX means to ignore this field. A value of UINT16_MAX-1 means to release this channel back to the RC radio. + * @param chan15_raw [us] RC channel 15 value. A value of 0 or UINT16_MAX means to ignore this field. A value of UINT16_MAX-1 means to release this channel back to the RC radio. + * @param chan16_raw [us] RC channel 16 value. A value of 0 or UINT16_MAX means to ignore this field. A value of UINT16_MAX-1 means to release this channel back to the RC radio. + * @param chan17_raw [us] RC channel 17 value. A value of 0 or UINT16_MAX means to ignore this field. A value of UINT16_MAX-1 means to release this channel back to the RC radio. + * @param chan18_raw [us] RC channel 18 value. A value of 0 or UINT16_MAX means to ignore this field. A value of UINT16_MAX-1 means to release this channel back to the RC radio. * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_rc_channels_override_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint8_t target_system, uint8_t target_component, uint16_t chan1_raw, uint16_t chan2_raw, uint16_t chan3_raw, uint16_t chan4_raw, uint16_t chan5_raw, uint16_t chan6_raw, uint16_t chan7_raw, uint16_t chan8_raw) + uint8_t target_system, uint8_t target_component, uint16_t chan1_raw, uint16_t chan2_raw, uint16_t chan3_raw, uint16_t chan4_raw, uint16_t chan5_raw, uint16_t chan6_raw, uint16_t chan7_raw, uint16_t chan8_raw, uint16_t chan9_raw, uint16_t chan10_raw, uint16_t chan11_raw, uint16_t chan12_raw, uint16_t chan13_raw, uint16_t chan14_raw, uint16_t chan15_raw, uint16_t chan16_raw, uint16_t chan17_raw, uint16_t chan18_raw) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE_LEN]; @@ -95,6 +135,16 @@ static inline uint16_t mavlink_msg_rc_channels_override_pack(uint8_t system_id, _mav_put_uint16_t(buf, 14, chan8_raw); _mav_put_uint8_t(buf, 16, target_system); _mav_put_uint8_t(buf, 17, target_component); + _mav_put_uint16_t(buf, 18, chan9_raw); + _mav_put_uint16_t(buf, 20, chan10_raw); + _mav_put_uint16_t(buf, 22, chan11_raw); + _mav_put_uint16_t(buf, 24, chan12_raw); + _mav_put_uint16_t(buf, 26, chan13_raw); + _mav_put_uint16_t(buf, 28, chan14_raw); + _mav_put_uint16_t(buf, 30, chan15_raw); + _mav_put_uint16_t(buf, 32, chan16_raw); + _mav_put_uint16_t(buf, 34, chan17_raw); + _mav_put_uint16_t(buf, 36, chan18_raw); memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE_LEN); #else @@ -109,6 +159,16 @@ static inline uint16_t mavlink_msg_rc_channels_override_pack(uint8_t system_id, packet.chan8_raw = chan8_raw; packet.target_system = target_system; packet.target_component = target_component; + packet.chan9_raw = chan9_raw; + packet.chan10_raw = chan10_raw; + packet.chan11_raw = chan11_raw; + packet.chan12_raw = chan12_raw; + packet.chan13_raw = chan13_raw; + packet.chan14_raw = chan14_raw; + packet.chan15_raw = chan15_raw; + packet.chan16_raw = chan16_raw; + packet.chan17_raw = chan17_raw; + packet.chan18_raw = chan18_raw; memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE_LEN); #endif @@ -125,19 +185,29 @@ static inline uint16_t mavlink_msg_rc_channels_override_pack(uint8_t system_id, * @param msg The MAVLink message to compress the data into * @param target_system System ID * @param target_component Component ID - * @param chan1_raw [us] RC channel 1 value. A value of UINT16_MAX means to ignore this field. - * @param chan2_raw [us] RC channel 2 value. A value of UINT16_MAX means to ignore this field. - * @param chan3_raw [us] RC channel 3 value. A value of UINT16_MAX means to ignore this field. - * @param chan4_raw [us] RC channel 4 value. A value of UINT16_MAX means to ignore this field. - * @param chan5_raw [us] RC channel 5 value. A value of UINT16_MAX means to ignore this field. - * @param chan6_raw [us] RC channel 6 value. A value of UINT16_MAX means to ignore this field. - * @param chan7_raw [us] RC channel 7 value. A value of UINT16_MAX means to ignore this field. - * @param chan8_raw [us] RC channel 8 value. A value of UINT16_MAX means to ignore this field. + * @param chan1_raw [us] RC channel 1 value. A value of UINT16_MAX means to ignore this field. A value of 0 means to release this channel back to the RC radio. + * @param chan2_raw [us] RC channel 2 value. A value of UINT16_MAX means to ignore this field. A value of 0 means to release this channel back to the RC radio. + * @param chan3_raw [us] RC channel 3 value. A value of UINT16_MAX means to ignore this field. A value of 0 means to release this channel back to the RC radio. + * @param chan4_raw [us] RC channel 4 value. A value of UINT16_MAX means to ignore this field. A value of 0 means to release this channel back to the RC radio. + * @param chan5_raw [us] RC channel 5 value. A value of UINT16_MAX means to ignore this field. A value of 0 means to release this channel back to the RC radio. + * @param chan6_raw [us] RC channel 6 value. A value of UINT16_MAX means to ignore this field. A value of 0 means to release this channel back to the RC radio. + * @param chan7_raw [us] RC channel 7 value. A value of UINT16_MAX means to ignore this field. A value of 0 means to release this channel back to the RC radio. + * @param chan8_raw [us] RC channel 8 value. A value of UINT16_MAX means to ignore this field. A value of 0 means to release this channel back to the RC radio. + * @param chan9_raw [us] RC channel 9 value. A value of 0 or UINT16_MAX means to ignore this field. A value of UINT16_MAX-1 means to release this channel back to the RC radio. + * @param chan10_raw [us] RC channel 10 value. A value of 0 or UINT16_MAX means to ignore this field. A value of UINT16_MAX-1 means to release this channel back to the RC radio. + * @param chan11_raw [us] RC channel 11 value. A value of 0 or UINT16_MAX means to ignore this field. A value of UINT16_MAX-1 means to release this channel back to the RC radio. + * @param chan12_raw [us] RC channel 12 value. A value of 0 or UINT16_MAX means to ignore this field. A value of UINT16_MAX-1 means to release this channel back to the RC radio. + * @param chan13_raw [us] RC channel 13 value. A value of 0 or UINT16_MAX means to ignore this field. A value of UINT16_MAX-1 means to release this channel back to the RC radio. + * @param chan14_raw [us] RC channel 14 value. A value of 0 or UINT16_MAX means to ignore this field. A value of UINT16_MAX-1 means to release this channel back to the RC radio. + * @param chan15_raw [us] RC channel 15 value. A value of 0 or UINT16_MAX means to ignore this field. A value of UINT16_MAX-1 means to release this channel back to the RC radio. + * @param chan16_raw [us] RC channel 16 value. A value of 0 or UINT16_MAX means to ignore this field. A value of UINT16_MAX-1 means to release this channel back to the RC radio. + * @param chan17_raw [us] RC channel 17 value. A value of 0 or UINT16_MAX means to ignore this field. A value of UINT16_MAX-1 means to release this channel back to the RC radio. + * @param chan18_raw [us] RC channel 18 value. A value of 0 or UINT16_MAX means to ignore this field. A value of UINT16_MAX-1 means to release this channel back to the RC radio. * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_rc_channels_override_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, - uint8_t target_system,uint8_t target_component,uint16_t chan1_raw,uint16_t chan2_raw,uint16_t chan3_raw,uint16_t chan4_raw,uint16_t chan5_raw,uint16_t chan6_raw,uint16_t chan7_raw,uint16_t chan8_raw) + uint8_t target_system,uint8_t target_component,uint16_t chan1_raw,uint16_t chan2_raw,uint16_t chan3_raw,uint16_t chan4_raw,uint16_t chan5_raw,uint16_t chan6_raw,uint16_t chan7_raw,uint16_t chan8_raw,uint16_t chan9_raw,uint16_t chan10_raw,uint16_t chan11_raw,uint16_t chan12_raw,uint16_t chan13_raw,uint16_t chan14_raw,uint16_t chan15_raw,uint16_t chan16_raw,uint16_t chan17_raw,uint16_t chan18_raw) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE_LEN]; @@ -151,6 +221,16 @@ static inline uint16_t mavlink_msg_rc_channels_override_pack_chan(uint8_t system _mav_put_uint16_t(buf, 14, chan8_raw); _mav_put_uint8_t(buf, 16, target_system); _mav_put_uint8_t(buf, 17, target_component); + _mav_put_uint16_t(buf, 18, chan9_raw); + _mav_put_uint16_t(buf, 20, chan10_raw); + _mav_put_uint16_t(buf, 22, chan11_raw); + _mav_put_uint16_t(buf, 24, chan12_raw); + _mav_put_uint16_t(buf, 26, chan13_raw); + _mav_put_uint16_t(buf, 28, chan14_raw); + _mav_put_uint16_t(buf, 30, chan15_raw); + _mav_put_uint16_t(buf, 32, chan16_raw); + _mav_put_uint16_t(buf, 34, chan17_raw); + _mav_put_uint16_t(buf, 36, chan18_raw); memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE_LEN); #else @@ -165,6 +245,16 @@ static inline uint16_t mavlink_msg_rc_channels_override_pack_chan(uint8_t system packet.chan8_raw = chan8_raw; packet.target_system = target_system; packet.target_component = target_component; + packet.chan9_raw = chan9_raw; + packet.chan10_raw = chan10_raw; + packet.chan11_raw = chan11_raw; + packet.chan12_raw = chan12_raw; + packet.chan13_raw = chan13_raw; + packet.chan14_raw = chan14_raw; + packet.chan15_raw = chan15_raw; + packet.chan16_raw = chan16_raw; + packet.chan17_raw = chan17_raw; + packet.chan18_raw = chan18_raw; memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE_LEN); #endif @@ -183,7 +273,7 @@ static inline uint16_t mavlink_msg_rc_channels_override_pack_chan(uint8_t system */ static inline uint16_t mavlink_msg_rc_channels_override_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_rc_channels_override_t* rc_channels_override) { - return mavlink_msg_rc_channels_override_pack(system_id, component_id, msg, rc_channels_override->target_system, rc_channels_override->target_component, rc_channels_override->chan1_raw, rc_channels_override->chan2_raw, rc_channels_override->chan3_raw, rc_channels_override->chan4_raw, rc_channels_override->chan5_raw, rc_channels_override->chan6_raw, rc_channels_override->chan7_raw, rc_channels_override->chan8_raw); + return mavlink_msg_rc_channels_override_pack(system_id, component_id, msg, rc_channels_override->target_system, rc_channels_override->target_component, rc_channels_override->chan1_raw, rc_channels_override->chan2_raw, rc_channels_override->chan3_raw, rc_channels_override->chan4_raw, rc_channels_override->chan5_raw, rc_channels_override->chan6_raw, rc_channels_override->chan7_raw, rc_channels_override->chan8_raw, rc_channels_override->chan9_raw, rc_channels_override->chan10_raw, rc_channels_override->chan11_raw, rc_channels_override->chan12_raw, rc_channels_override->chan13_raw, rc_channels_override->chan14_raw, rc_channels_override->chan15_raw, rc_channels_override->chan16_raw, rc_channels_override->chan17_raw, rc_channels_override->chan18_raw); } /** @@ -197,7 +287,7 @@ static inline uint16_t mavlink_msg_rc_channels_override_encode(uint8_t system_id */ static inline uint16_t mavlink_msg_rc_channels_override_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_rc_channels_override_t* rc_channels_override) { - return mavlink_msg_rc_channels_override_pack_chan(system_id, component_id, chan, msg, rc_channels_override->target_system, rc_channels_override->target_component, rc_channels_override->chan1_raw, rc_channels_override->chan2_raw, rc_channels_override->chan3_raw, rc_channels_override->chan4_raw, rc_channels_override->chan5_raw, rc_channels_override->chan6_raw, rc_channels_override->chan7_raw, rc_channels_override->chan8_raw); + return mavlink_msg_rc_channels_override_pack_chan(system_id, component_id, chan, msg, rc_channels_override->target_system, rc_channels_override->target_component, rc_channels_override->chan1_raw, rc_channels_override->chan2_raw, rc_channels_override->chan3_raw, rc_channels_override->chan4_raw, rc_channels_override->chan5_raw, rc_channels_override->chan6_raw, rc_channels_override->chan7_raw, rc_channels_override->chan8_raw, rc_channels_override->chan9_raw, rc_channels_override->chan10_raw, rc_channels_override->chan11_raw, rc_channels_override->chan12_raw, rc_channels_override->chan13_raw, rc_channels_override->chan14_raw, rc_channels_override->chan15_raw, rc_channels_override->chan16_raw, rc_channels_override->chan17_raw, rc_channels_override->chan18_raw); } /** @@ -206,18 +296,28 @@ static inline uint16_t mavlink_msg_rc_channels_override_encode_chan(uint8_t syst * * @param target_system System ID * @param target_component Component ID - * @param chan1_raw [us] RC channel 1 value. A value of UINT16_MAX means to ignore this field. - * @param chan2_raw [us] RC channel 2 value. A value of UINT16_MAX means to ignore this field. - * @param chan3_raw [us] RC channel 3 value. A value of UINT16_MAX means to ignore this field. - * @param chan4_raw [us] RC channel 4 value. A value of UINT16_MAX means to ignore this field. - * @param chan5_raw [us] RC channel 5 value. A value of UINT16_MAX means to ignore this field. - * @param chan6_raw [us] RC channel 6 value. A value of UINT16_MAX means to ignore this field. - * @param chan7_raw [us] RC channel 7 value. A value of UINT16_MAX means to ignore this field. - * @param chan8_raw [us] RC channel 8 value. A value of UINT16_MAX means to ignore this field. + * @param chan1_raw [us] RC channel 1 value. A value of UINT16_MAX means to ignore this field. A value of 0 means to release this channel back to the RC radio. + * @param chan2_raw [us] RC channel 2 value. A value of UINT16_MAX means to ignore this field. A value of 0 means to release this channel back to the RC radio. + * @param chan3_raw [us] RC channel 3 value. A value of UINT16_MAX means to ignore this field. A value of 0 means to release this channel back to the RC radio. + * @param chan4_raw [us] RC channel 4 value. A value of UINT16_MAX means to ignore this field. A value of 0 means to release this channel back to the RC radio. + * @param chan5_raw [us] RC channel 5 value. A value of UINT16_MAX means to ignore this field. A value of 0 means to release this channel back to the RC radio. + * @param chan6_raw [us] RC channel 6 value. A value of UINT16_MAX means to ignore this field. A value of 0 means to release this channel back to the RC radio. + * @param chan7_raw [us] RC channel 7 value. A value of UINT16_MAX means to ignore this field. A value of 0 means to release this channel back to the RC radio. + * @param chan8_raw [us] RC channel 8 value. A value of UINT16_MAX means to ignore this field. A value of 0 means to release this channel back to the RC radio. + * @param chan9_raw [us] RC channel 9 value. A value of 0 or UINT16_MAX means to ignore this field. A value of UINT16_MAX-1 means to release this channel back to the RC radio. + * @param chan10_raw [us] RC channel 10 value. A value of 0 or UINT16_MAX means to ignore this field. A value of UINT16_MAX-1 means to release this channel back to the RC radio. + * @param chan11_raw [us] RC channel 11 value. A value of 0 or UINT16_MAX means to ignore this field. A value of UINT16_MAX-1 means to release this channel back to the RC radio. + * @param chan12_raw [us] RC channel 12 value. A value of 0 or UINT16_MAX means to ignore this field. A value of UINT16_MAX-1 means to release this channel back to the RC radio. + * @param chan13_raw [us] RC channel 13 value. A value of 0 or UINT16_MAX means to ignore this field. A value of UINT16_MAX-1 means to release this channel back to the RC radio. + * @param chan14_raw [us] RC channel 14 value. A value of 0 or UINT16_MAX means to ignore this field. A value of UINT16_MAX-1 means to release this channel back to the RC radio. + * @param chan15_raw [us] RC channel 15 value. A value of 0 or UINT16_MAX means to ignore this field. A value of UINT16_MAX-1 means to release this channel back to the RC radio. + * @param chan16_raw [us] RC channel 16 value. A value of 0 or UINT16_MAX means to ignore this field. A value of UINT16_MAX-1 means to release this channel back to the RC radio. + * @param chan17_raw [us] RC channel 17 value. A value of 0 or UINT16_MAX means to ignore this field. A value of UINT16_MAX-1 means to release this channel back to the RC radio. + * @param chan18_raw [us] RC channel 18 value. A value of 0 or UINT16_MAX means to ignore this field. A value of UINT16_MAX-1 means to release this channel back to the RC radio. */ #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS -static inline void mavlink_msg_rc_channels_override_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint16_t chan1_raw, uint16_t chan2_raw, uint16_t chan3_raw, uint16_t chan4_raw, uint16_t chan5_raw, uint16_t chan6_raw, uint16_t chan7_raw, uint16_t chan8_raw) +static inline void mavlink_msg_rc_channels_override_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint16_t chan1_raw, uint16_t chan2_raw, uint16_t chan3_raw, uint16_t chan4_raw, uint16_t chan5_raw, uint16_t chan6_raw, uint16_t chan7_raw, uint16_t chan8_raw, uint16_t chan9_raw, uint16_t chan10_raw, uint16_t chan11_raw, uint16_t chan12_raw, uint16_t chan13_raw, uint16_t chan14_raw, uint16_t chan15_raw, uint16_t chan16_raw, uint16_t chan17_raw, uint16_t chan18_raw) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE_LEN]; @@ -231,6 +331,16 @@ static inline void mavlink_msg_rc_channels_override_send(mavlink_channel_t chan, _mav_put_uint16_t(buf, 14, chan8_raw); _mav_put_uint8_t(buf, 16, target_system); _mav_put_uint8_t(buf, 17, target_component); + _mav_put_uint16_t(buf, 18, chan9_raw); + _mav_put_uint16_t(buf, 20, chan10_raw); + _mav_put_uint16_t(buf, 22, chan11_raw); + _mav_put_uint16_t(buf, 24, chan12_raw); + _mav_put_uint16_t(buf, 26, chan13_raw); + _mav_put_uint16_t(buf, 28, chan14_raw); + _mav_put_uint16_t(buf, 30, chan15_raw); + _mav_put_uint16_t(buf, 32, chan16_raw); + _mav_put_uint16_t(buf, 34, chan17_raw); + _mav_put_uint16_t(buf, 36, chan18_raw); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE, buf, MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE_MIN_LEN, MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE_LEN, MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE_CRC); #else @@ -245,6 +355,16 @@ static inline void mavlink_msg_rc_channels_override_send(mavlink_channel_t chan, packet.chan8_raw = chan8_raw; packet.target_system = target_system; packet.target_component = target_component; + packet.chan9_raw = chan9_raw; + packet.chan10_raw = chan10_raw; + packet.chan11_raw = chan11_raw; + packet.chan12_raw = chan12_raw; + packet.chan13_raw = chan13_raw; + packet.chan14_raw = chan14_raw; + packet.chan15_raw = chan15_raw; + packet.chan16_raw = chan16_raw; + packet.chan17_raw = chan17_raw; + packet.chan18_raw = chan18_raw; _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE, (const char *)&packet, MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE_MIN_LEN, MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE_LEN, MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE_CRC); #endif @@ -258,7 +378,7 @@ static inline void mavlink_msg_rc_channels_override_send(mavlink_channel_t chan, static inline void mavlink_msg_rc_channels_override_send_struct(mavlink_channel_t chan, const mavlink_rc_channels_override_t* rc_channels_override) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - mavlink_msg_rc_channels_override_send(chan, rc_channels_override->target_system, rc_channels_override->target_component, rc_channels_override->chan1_raw, rc_channels_override->chan2_raw, rc_channels_override->chan3_raw, rc_channels_override->chan4_raw, rc_channels_override->chan5_raw, rc_channels_override->chan6_raw, rc_channels_override->chan7_raw, rc_channels_override->chan8_raw); + mavlink_msg_rc_channels_override_send(chan, rc_channels_override->target_system, rc_channels_override->target_component, rc_channels_override->chan1_raw, rc_channels_override->chan2_raw, rc_channels_override->chan3_raw, rc_channels_override->chan4_raw, rc_channels_override->chan5_raw, rc_channels_override->chan6_raw, rc_channels_override->chan7_raw, rc_channels_override->chan8_raw, rc_channels_override->chan9_raw, rc_channels_override->chan10_raw, rc_channels_override->chan11_raw, rc_channels_override->chan12_raw, rc_channels_override->chan13_raw, rc_channels_override->chan14_raw, rc_channels_override->chan15_raw, rc_channels_override->chan16_raw, rc_channels_override->chan17_raw, rc_channels_override->chan18_raw); #else _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE, (const char *)rc_channels_override, MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE_MIN_LEN, MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE_LEN, MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE_CRC); #endif @@ -272,7 +392,7 @@ static inline void mavlink_msg_rc_channels_override_send_struct(mavlink_channel_ is usually the receive buffer for the channel, and allows a reply to an incoming message with minimum stack space usage. */ -static inline void mavlink_msg_rc_channels_override_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint16_t chan1_raw, uint16_t chan2_raw, uint16_t chan3_raw, uint16_t chan4_raw, uint16_t chan5_raw, uint16_t chan6_raw, uint16_t chan7_raw, uint16_t chan8_raw) +static inline void mavlink_msg_rc_channels_override_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint16_t chan1_raw, uint16_t chan2_raw, uint16_t chan3_raw, uint16_t chan4_raw, uint16_t chan5_raw, uint16_t chan6_raw, uint16_t chan7_raw, uint16_t chan8_raw, uint16_t chan9_raw, uint16_t chan10_raw, uint16_t chan11_raw, uint16_t chan12_raw, uint16_t chan13_raw, uint16_t chan14_raw, uint16_t chan15_raw, uint16_t chan16_raw, uint16_t chan17_raw, uint16_t chan18_raw) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char *buf = (char *)msgbuf; @@ -286,6 +406,16 @@ static inline void mavlink_msg_rc_channels_override_send_buf(mavlink_message_t * _mav_put_uint16_t(buf, 14, chan8_raw); _mav_put_uint8_t(buf, 16, target_system); _mav_put_uint8_t(buf, 17, target_component); + _mav_put_uint16_t(buf, 18, chan9_raw); + _mav_put_uint16_t(buf, 20, chan10_raw); + _mav_put_uint16_t(buf, 22, chan11_raw); + _mav_put_uint16_t(buf, 24, chan12_raw); + _mav_put_uint16_t(buf, 26, chan13_raw); + _mav_put_uint16_t(buf, 28, chan14_raw); + _mav_put_uint16_t(buf, 30, chan15_raw); + _mav_put_uint16_t(buf, 32, chan16_raw); + _mav_put_uint16_t(buf, 34, chan17_raw); + _mav_put_uint16_t(buf, 36, chan18_raw); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE, buf, MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE_MIN_LEN, MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE_LEN, MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE_CRC); #else @@ -300,6 +430,16 @@ static inline void mavlink_msg_rc_channels_override_send_buf(mavlink_message_t * packet->chan8_raw = chan8_raw; packet->target_system = target_system; packet->target_component = target_component; + packet->chan9_raw = chan9_raw; + packet->chan10_raw = chan10_raw; + packet->chan11_raw = chan11_raw; + packet->chan12_raw = chan12_raw; + packet->chan13_raw = chan13_raw; + packet->chan14_raw = chan14_raw; + packet->chan15_raw = chan15_raw; + packet->chan16_raw = chan16_raw; + packet->chan17_raw = chan17_raw; + packet->chan18_raw = chan18_raw; _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE, (const char *)packet, MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE_MIN_LEN, MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE_LEN, MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE_CRC); #endif @@ -334,7 +474,7 @@ static inline uint8_t mavlink_msg_rc_channels_override_get_target_component(cons /** * @brief Get field chan1_raw from rc_channels_override message * - * @return [us] RC channel 1 value. A value of UINT16_MAX means to ignore this field. + * @return [us] RC channel 1 value. A value of UINT16_MAX means to ignore this field. A value of 0 means to release this channel back to the RC radio. */ static inline uint16_t mavlink_msg_rc_channels_override_get_chan1_raw(const mavlink_message_t* msg) { @@ -344,7 +484,7 @@ static inline uint16_t mavlink_msg_rc_channels_override_get_chan1_raw(const mavl /** * @brief Get field chan2_raw from rc_channels_override message * - * @return [us] RC channel 2 value. A value of UINT16_MAX means to ignore this field. + * @return [us] RC channel 2 value. A value of UINT16_MAX means to ignore this field. A value of 0 means to release this channel back to the RC radio. */ static inline uint16_t mavlink_msg_rc_channels_override_get_chan2_raw(const mavlink_message_t* msg) { @@ -354,7 +494,7 @@ static inline uint16_t mavlink_msg_rc_channels_override_get_chan2_raw(const mavl /** * @brief Get field chan3_raw from rc_channels_override message * - * @return [us] RC channel 3 value. A value of UINT16_MAX means to ignore this field. + * @return [us] RC channel 3 value. A value of UINT16_MAX means to ignore this field. A value of 0 means to release this channel back to the RC radio. */ static inline uint16_t mavlink_msg_rc_channels_override_get_chan3_raw(const mavlink_message_t* msg) { @@ -364,7 +504,7 @@ static inline uint16_t mavlink_msg_rc_channels_override_get_chan3_raw(const mavl /** * @brief Get field chan4_raw from rc_channels_override message * - * @return [us] RC channel 4 value. A value of UINT16_MAX means to ignore this field. + * @return [us] RC channel 4 value. A value of UINT16_MAX means to ignore this field. A value of 0 means to release this channel back to the RC radio. */ static inline uint16_t mavlink_msg_rc_channels_override_get_chan4_raw(const mavlink_message_t* msg) { @@ -374,7 +514,7 @@ static inline uint16_t mavlink_msg_rc_channels_override_get_chan4_raw(const mavl /** * @brief Get field chan5_raw from rc_channels_override message * - * @return [us] RC channel 5 value. A value of UINT16_MAX means to ignore this field. + * @return [us] RC channel 5 value. A value of UINT16_MAX means to ignore this field. A value of 0 means to release this channel back to the RC radio. */ static inline uint16_t mavlink_msg_rc_channels_override_get_chan5_raw(const mavlink_message_t* msg) { @@ -384,7 +524,7 @@ static inline uint16_t mavlink_msg_rc_channels_override_get_chan5_raw(const mavl /** * @brief Get field chan6_raw from rc_channels_override message * - * @return [us] RC channel 6 value. A value of UINT16_MAX means to ignore this field. + * @return [us] RC channel 6 value. A value of UINT16_MAX means to ignore this field. A value of 0 means to release this channel back to the RC radio. */ static inline uint16_t mavlink_msg_rc_channels_override_get_chan6_raw(const mavlink_message_t* msg) { @@ -394,7 +534,7 @@ static inline uint16_t mavlink_msg_rc_channels_override_get_chan6_raw(const mavl /** * @brief Get field chan7_raw from rc_channels_override message * - * @return [us] RC channel 7 value. A value of UINT16_MAX means to ignore this field. + * @return [us] RC channel 7 value. A value of UINT16_MAX means to ignore this field. A value of 0 means to release this channel back to the RC radio. */ static inline uint16_t mavlink_msg_rc_channels_override_get_chan7_raw(const mavlink_message_t* msg) { @@ -404,13 +544,113 @@ static inline uint16_t mavlink_msg_rc_channels_override_get_chan7_raw(const mavl /** * @brief Get field chan8_raw from rc_channels_override message * - * @return [us] RC channel 8 value. A value of UINT16_MAX means to ignore this field. + * @return [us] RC channel 8 value. A value of UINT16_MAX means to ignore this field. A value of 0 means to release this channel back to the RC radio. */ static inline uint16_t mavlink_msg_rc_channels_override_get_chan8_raw(const mavlink_message_t* msg) { return _MAV_RETURN_uint16_t(msg, 14); } +/** + * @brief Get field chan9_raw from rc_channels_override message + * + * @return [us] RC channel 9 value. A value of 0 or UINT16_MAX means to ignore this field. A value of UINT16_MAX-1 means to release this channel back to the RC radio. + */ +static inline uint16_t mavlink_msg_rc_channels_override_get_chan9_raw(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 18); +} + +/** + * @brief Get field chan10_raw from rc_channels_override message + * + * @return [us] RC channel 10 value. A value of 0 or UINT16_MAX means to ignore this field. A value of UINT16_MAX-1 means to release this channel back to the RC radio. + */ +static inline uint16_t mavlink_msg_rc_channels_override_get_chan10_raw(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 20); +} + +/** + * @brief Get field chan11_raw from rc_channels_override message + * + * @return [us] RC channel 11 value. A value of 0 or UINT16_MAX means to ignore this field. A value of UINT16_MAX-1 means to release this channel back to the RC radio. + */ +static inline uint16_t mavlink_msg_rc_channels_override_get_chan11_raw(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 22); +} + +/** + * @brief Get field chan12_raw from rc_channels_override message + * + * @return [us] RC channel 12 value. A value of 0 or UINT16_MAX means to ignore this field. A value of UINT16_MAX-1 means to release this channel back to the RC radio. + */ +static inline uint16_t mavlink_msg_rc_channels_override_get_chan12_raw(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 24); +} + +/** + * @brief Get field chan13_raw from rc_channels_override message + * + * @return [us] RC channel 13 value. A value of 0 or UINT16_MAX means to ignore this field. A value of UINT16_MAX-1 means to release this channel back to the RC radio. + */ +static inline uint16_t mavlink_msg_rc_channels_override_get_chan13_raw(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 26); +} + +/** + * @brief Get field chan14_raw from rc_channels_override message + * + * @return [us] RC channel 14 value. A value of 0 or UINT16_MAX means to ignore this field. A value of UINT16_MAX-1 means to release this channel back to the RC radio. + */ +static inline uint16_t mavlink_msg_rc_channels_override_get_chan14_raw(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 28); +} + +/** + * @brief Get field chan15_raw from rc_channels_override message + * + * @return [us] RC channel 15 value. A value of 0 or UINT16_MAX means to ignore this field. A value of UINT16_MAX-1 means to release this channel back to the RC radio. + */ +static inline uint16_t mavlink_msg_rc_channels_override_get_chan15_raw(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 30); +} + +/** + * @brief Get field chan16_raw from rc_channels_override message + * + * @return [us] RC channel 16 value. A value of 0 or UINT16_MAX means to ignore this field. A value of UINT16_MAX-1 means to release this channel back to the RC radio. + */ +static inline uint16_t mavlink_msg_rc_channels_override_get_chan16_raw(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 32); +} + +/** + * @brief Get field chan17_raw from rc_channels_override message + * + * @return [us] RC channel 17 value. A value of 0 or UINT16_MAX means to ignore this field. A value of UINT16_MAX-1 means to release this channel back to the RC radio. + */ +static inline uint16_t mavlink_msg_rc_channels_override_get_chan17_raw(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 34); +} + +/** + * @brief Get field chan18_raw from rc_channels_override message + * + * @return [us] RC channel 18 value. A value of 0 or UINT16_MAX means to ignore this field. A value of UINT16_MAX-1 means to release this channel back to the RC radio. + */ +static inline uint16_t mavlink_msg_rc_channels_override_get_chan18_raw(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 36); +} + /** * @brief Decode a rc_channels_override message into a struct * @@ -430,6 +670,16 @@ static inline void mavlink_msg_rc_channels_override_decode(const mavlink_message rc_channels_override->chan8_raw = mavlink_msg_rc_channels_override_get_chan8_raw(msg); rc_channels_override->target_system = mavlink_msg_rc_channels_override_get_target_system(msg); rc_channels_override->target_component = mavlink_msg_rc_channels_override_get_target_component(msg); + rc_channels_override->chan9_raw = mavlink_msg_rc_channels_override_get_chan9_raw(msg); + rc_channels_override->chan10_raw = mavlink_msg_rc_channels_override_get_chan10_raw(msg); + rc_channels_override->chan11_raw = mavlink_msg_rc_channels_override_get_chan11_raw(msg); + rc_channels_override->chan12_raw = mavlink_msg_rc_channels_override_get_chan12_raw(msg); + rc_channels_override->chan13_raw = mavlink_msg_rc_channels_override_get_chan13_raw(msg); + rc_channels_override->chan14_raw = mavlink_msg_rc_channels_override_get_chan14_raw(msg); + rc_channels_override->chan15_raw = mavlink_msg_rc_channels_override_get_chan15_raw(msg); + rc_channels_override->chan16_raw = mavlink_msg_rc_channels_override_get_chan16_raw(msg); + rc_channels_override->chan17_raw = mavlink_msg_rc_channels_override_get_chan17_raw(msg); + rc_channels_override->chan18_raw = mavlink_msg_rc_channels_override_get_chan18_raw(msg); #else uint8_t len = msg->len < MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE_LEN? msg->len : MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE_LEN; memset(rc_channels_override, 0, MAVLINK_MSG_ID_RC_CHANNELS_OVERRIDE_LEN); diff --git a/lib/main/MAVLink/common/mavlink_msg_scaled_imu.h b/lib/main/MAVLink/common/mavlink_msg_scaled_imu.h index f94b146bf2..14b1257ce9 100755 --- a/lib/main/MAVLink/common/mavlink_msg_scaled_imu.h +++ b/lib/main/MAVLink/common/mavlink_msg_scaled_imu.h @@ -15,11 +15,12 @@ typedef struct __mavlink_scaled_imu_t { int16_t xmag; /*< [mgauss] X Magnetic field*/ int16_t ymag; /*< [mgauss] Y Magnetic field*/ int16_t zmag; /*< [mgauss] Z Magnetic field*/ + int16_t temperature; /*< [cdegC] Temperature, 0: IMU does not provide temperature values. If the IMU is at 0C it must send 1 (0.01C).*/ } mavlink_scaled_imu_t; -#define MAVLINK_MSG_ID_SCALED_IMU_LEN 22 +#define MAVLINK_MSG_ID_SCALED_IMU_LEN 24 #define MAVLINK_MSG_ID_SCALED_IMU_MIN_LEN 22 -#define MAVLINK_MSG_ID_26_LEN 22 +#define MAVLINK_MSG_ID_26_LEN 24 #define MAVLINK_MSG_ID_26_MIN_LEN 22 #define MAVLINK_MSG_ID_SCALED_IMU_CRC 170 @@ -31,7 +32,7 @@ typedef struct __mavlink_scaled_imu_t { #define MAVLINK_MESSAGE_INFO_SCALED_IMU { \ 26, \ "SCALED_IMU", \ - 10, \ + 11, \ { { "time_boot_ms", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_scaled_imu_t, time_boot_ms) }, \ { "xacc", NULL, MAVLINK_TYPE_INT16_T, 0, 4, offsetof(mavlink_scaled_imu_t, xacc) }, \ { "yacc", NULL, MAVLINK_TYPE_INT16_T, 0, 6, offsetof(mavlink_scaled_imu_t, yacc) }, \ @@ -42,12 +43,13 @@ typedef struct __mavlink_scaled_imu_t { { "xmag", NULL, MAVLINK_TYPE_INT16_T, 0, 16, offsetof(mavlink_scaled_imu_t, xmag) }, \ { "ymag", NULL, MAVLINK_TYPE_INT16_T, 0, 18, offsetof(mavlink_scaled_imu_t, ymag) }, \ { "zmag", NULL, MAVLINK_TYPE_INT16_T, 0, 20, offsetof(mavlink_scaled_imu_t, zmag) }, \ + { "temperature", NULL, MAVLINK_TYPE_INT16_T, 0, 22, offsetof(mavlink_scaled_imu_t, temperature) }, \ } \ } #else #define MAVLINK_MESSAGE_INFO_SCALED_IMU { \ "SCALED_IMU", \ - 10, \ + 11, \ { { "time_boot_ms", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_scaled_imu_t, time_boot_ms) }, \ { "xacc", NULL, MAVLINK_TYPE_INT16_T, 0, 4, offsetof(mavlink_scaled_imu_t, xacc) }, \ { "yacc", NULL, MAVLINK_TYPE_INT16_T, 0, 6, offsetof(mavlink_scaled_imu_t, yacc) }, \ @@ -58,6 +60,7 @@ typedef struct __mavlink_scaled_imu_t { { "xmag", NULL, MAVLINK_TYPE_INT16_T, 0, 16, offsetof(mavlink_scaled_imu_t, xmag) }, \ { "ymag", NULL, MAVLINK_TYPE_INT16_T, 0, 18, offsetof(mavlink_scaled_imu_t, ymag) }, \ { "zmag", NULL, MAVLINK_TYPE_INT16_T, 0, 20, offsetof(mavlink_scaled_imu_t, zmag) }, \ + { "temperature", NULL, MAVLINK_TYPE_INT16_T, 0, 22, offsetof(mavlink_scaled_imu_t, temperature) }, \ } \ } #endif @@ -78,10 +81,11 @@ typedef struct __mavlink_scaled_imu_t { * @param xmag [mgauss] X Magnetic field * @param ymag [mgauss] Y Magnetic field * @param zmag [mgauss] Z Magnetic field + * @param temperature [cdegC] Temperature, 0: IMU does not provide temperature values. If the IMU is at 0C it must send 1 (0.01C). * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_scaled_imu_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint32_t time_boot_ms, int16_t xacc, int16_t yacc, int16_t zacc, int16_t xgyro, int16_t ygyro, int16_t zgyro, int16_t xmag, int16_t ymag, int16_t zmag) + uint32_t time_boot_ms, int16_t xacc, int16_t yacc, int16_t zacc, int16_t xgyro, int16_t ygyro, int16_t zgyro, int16_t xmag, int16_t ymag, int16_t zmag, int16_t temperature) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_SCALED_IMU_LEN]; @@ -95,6 +99,7 @@ static inline uint16_t mavlink_msg_scaled_imu_pack(uint8_t system_id, uint8_t co _mav_put_int16_t(buf, 16, xmag); _mav_put_int16_t(buf, 18, ymag); _mav_put_int16_t(buf, 20, zmag); + _mav_put_int16_t(buf, 22, temperature); memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SCALED_IMU_LEN); #else @@ -109,6 +114,7 @@ static inline uint16_t mavlink_msg_scaled_imu_pack(uint8_t system_id, uint8_t co packet.xmag = xmag; packet.ymag = ymag; packet.zmag = zmag; + packet.temperature = temperature; memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SCALED_IMU_LEN); #endif @@ -133,11 +139,12 @@ static inline uint16_t mavlink_msg_scaled_imu_pack(uint8_t system_id, uint8_t co * @param xmag [mgauss] X Magnetic field * @param ymag [mgauss] Y Magnetic field * @param zmag [mgauss] Z Magnetic field + * @param temperature [cdegC] Temperature, 0: IMU does not provide temperature values. If the IMU is at 0C it must send 1 (0.01C). * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_scaled_imu_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, - uint32_t time_boot_ms,int16_t xacc,int16_t yacc,int16_t zacc,int16_t xgyro,int16_t ygyro,int16_t zgyro,int16_t xmag,int16_t ymag,int16_t zmag) + uint32_t time_boot_ms,int16_t xacc,int16_t yacc,int16_t zacc,int16_t xgyro,int16_t ygyro,int16_t zgyro,int16_t xmag,int16_t ymag,int16_t zmag,int16_t temperature) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_SCALED_IMU_LEN]; @@ -151,6 +158,7 @@ static inline uint16_t mavlink_msg_scaled_imu_pack_chan(uint8_t system_id, uint8 _mav_put_int16_t(buf, 16, xmag); _mav_put_int16_t(buf, 18, ymag); _mav_put_int16_t(buf, 20, zmag); + _mav_put_int16_t(buf, 22, temperature); memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SCALED_IMU_LEN); #else @@ -165,6 +173,7 @@ static inline uint16_t mavlink_msg_scaled_imu_pack_chan(uint8_t system_id, uint8 packet.xmag = xmag; packet.ymag = ymag; packet.zmag = zmag; + packet.temperature = temperature; memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SCALED_IMU_LEN); #endif @@ -183,7 +192,7 @@ static inline uint16_t mavlink_msg_scaled_imu_pack_chan(uint8_t system_id, uint8 */ static inline uint16_t mavlink_msg_scaled_imu_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_scaled_imu_t* scaled_imu) { - return mavlink_msg_scaled_imu_pack(system_id, component_id, msg, scaled_imu->time_boot_ms, scaled_imu->xacc, scaled_imu->yacc, scaled_imu->zacc, scaled_imu->xgyro, scaled_imu->ygyro, scaled_imu->zgyro, scaled_imu->xmag, scaled_imu->ymag, scaled_imu->zmag); + return mavlink_msg_scaled_imu_pack(system_id, component_id, msg, scaled_imu->time_boot_ms, scaled_imu->xacc, scaled_imu->yacc, scaled_imu->zacc, scaled_imu->xgyro, scaled_imu->ygyro, scaled_imu->zgyro, scaled_imu->xmag, scaled_imu->ymag, scaled_imu->zmag, scaled_imu->temperature); } /** @@ -197,7 +206,7 @@ static inline uint16_t mavlink_msg_scaled_imu_encode(uint8_t system_id, uint8_t */ static inline uint16_t mavlink_msg_scaled_imu_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_scaled_imu_t* scaled_imu) { - return mavlink_msg_scaled_imu_pack_chan(system_id, component_id, chan, msg, scaled_imu->time_boot_ms, scaled_imu->xacc, scaled_imu->yacc, scaled_imu->zacc, scaled_imu->xgyro, scaled_imu->ygyro, scaled_imu->zgyro, scaled_imu->xmag, scaled_imu->ymag, scaled_imu->zmag); + return mavlink_msg_scaled_imu_pack_chan(system_id, component_id, chan, msg, scaled_imu->time_boot_ms, scaled_imu->xacc, scaled_imu->yacc, scaled_imu->zacc, scaled_imu->xgyro, scaled_imu->ygyro, scaled_imu->zgyro, scaled_imu->xmag, scaled_imu->ymag, scaled_imu->zmag, scaled_imu->temperature); } /** @@ -214,10 +223,11 @@ static inline uint16_t mavlink_msg_scaled_imu_encode_chan(uint8_t system_id, uin * @param xmag [mgauss] X Magnetic field * @param ymag [mgauss] Y Magnetic field * @param zmag [mgauss] Z Magnetic field + * @param temperature [cdegC] Temperature, 0: IMU does not provide temperature values. If the IMU is at 0C it must send 1 (0.01C). */ #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS -static inline void mavlink_msg_scaled_imu_send(mavlink_channel_t chan, uint32_t time_boot_ms, int16_t xacc, int16_t yacc, int16_t zacc, int16_t xgyro, int16_t ygyro, int16_t zgyro, int16_t xmag, int16_t ymag, int16_t zmag) +static inline void mavlink_msg_scaled_imu_send(mavlink_channel_t chan, uint32_t time_boot_ms, int16_t xacc, int16_t yacc, int16_t zacc, int16_t xgyro, int16_t ygyro, int16_t zgyro, int16_t xmag, int16_t ymag, int16_t zmag, int16_t temperature) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_SCALED_IMU_LEN]; @@ -231,6 +241,7 @@ static inline void mavlink_msg_scaled_imu_send(mavlink_channel_t chan, uint32_t _mav_put_int16_t(buf, 16, xmag); _mav_put_int16_t(buf, 18, ymag); _mav_put_int16_t(buf, 20, zmag); + _mav_put_int16_t(buf, 22, temperature); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SCALED_IMU, buf, MAVLINK_MSG_ID_SCALED_IMU_MIN_LEN, MAVLINK_MSG_ID_SCALED_IMU_LEN, MAVLINK_MSG_ID_SCALED_IMU_CRC); #else @@ -245,6 +256,7 @@ static inline void mavlink_msg_scaled_imu_send(mavlink_channel_t chan, uint32_t packet.xmag = xmag; packet.ymag = ymag; packet.zmag = zmag; + packet.temperature = temperature; _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SCALED_IMU, (const char *)&packet, MAVLINK_MSG_ID_SCALED_IMU_MIN_LEN, MAVLINK_MSG_ID_SCALED_IMU_LEN, MAVLINK_MSG_ID_SCALED_IMU_CRC); #endif @@ -258,7 +270,7 @@ static inline void mavlink_msg_scaled_imu_send(mavlink_channel_t chan, uint32_t static inline void mavlink_msg_scaled_imu_send_struct(mavlink_channel_t chan, const mavlink_scaled_imu_t* scaled_imu) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - mavlink_msg_scaled_imu_send(chan, scaled_imu->time_boot_ms, scaled_imu->xacc, scaled_imu->yacc, scaled_imu->zacc, scaled_imu->xgyro, scaled_imu->ygyro, scaled_imu->zgyro, scaled_imu->xmag, scaled_imu->ymag, scaled_imu->zmag); + mavlink_msg_scaled_imu_send(chan, scaled_imu->time_boot_ms, scaled_imu->xacc, scaled_imu->yacc, scaled_imu->zacc, scaled_imu->xgyro, scaled_imu->ygyro, scaled_imu->zgyro, scaled_imu->xmag, scaled_imu->ymag, scaled_imu->zmag, scaled_imu->temperature); #else _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SCALED_IMU, (const char *)scaled_imu, MAVLINK_MSG_ID_SCALED_IMU_MIN_LEN, MAVLINK_MSG_ID_SCALED_IMU_LEN, MAVLINK_MSG_ID_SCALED_IMU_CRC); #endif @@ -272,7 +284,7 @@ static inline void mavlink_msg_scaled_imu_send_struct(mavlink_channel_t chan, co is usually the receive buffer for the channel, and allows a reply to an incoming message with minimum stack space usage. */ -static inline void mavlink_msg_scaled_imu_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint32_t time_boot_ms, int16_t xacc, int16_t yacc, int16_t zacc, int16_t xgyro, int16_t ygyro, int16_t zgyro, int16_t xmag, int16_t ymag, int16_t zmag) +static inline void mavlink_msg_scaled_imu_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint32_t time_boot_ms, int16_t xacc, int16_t yacc, int16_t zacc, int16_t xgyro, int16_t ygyro, int16_t zgyro, int16_t xmag, int16_t ymag, int16_t zmag, int16_t temperature) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char *buf = (char *)msgbuf; @@ -286,6 +298,7 @@ static inline void mavlink_msg_scaled_imu_send_buf(mavlink_message_t *msgbuf, ma _mav_put_int16_t(buf, 16, xmag); _mav_put_int16_t(buf, 18, ymag); _mav_put_int16_t(buf, 20, zmag); + _mav_put_int16_t(buf, 22, temperature); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SCALED_IMU, buf, MAVLINK_MSG_ID_SCALED_IMU_MIN_LEN, MAVLINK_MSG_ID_SCALED_IMU_LEN, MAVLINK_MSG_ID_SCALED_IMU_CRC); #else @@ -300,6 +313,7 @@ static inline void mavlink_msg_scaled_imu_send_buf(mavlink_message_t *msgbuf, ma packet->xmag = xmag; packet->ymag = ymag; packet->zmag = zmag; + packet->temperature = temperature; _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SCALED_IMU, (const char *)packet, MAVLINK_MSG_ID_SCALED_IMU_MIN_LEN, MAVLINK_MSG_ID_SCALED_IMU_LEN, MAVLINK_MSG_ID_SCALED_IMU_CRC); #endif @@ -411,6 +425,16 @@ static inline int16_t mavlink_msg_scaled_imu_get_zmag(const mavlink_message_t* m return _MAV_RETURN_int16_t(msg, 20); } +/** + * @brief Get field temperature from scaled_imu message + * + * @return [cdegC] Temperature, 0: IMU does not provide temperature values. If the IMU is at 0C it must send 1 (0.01C). + */ +static inline int16_t mavlink_msg_scaled_imu_get_temperature(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int16_t(msg, 22); +} + /** * @brief Decode a scaled_imu message into a struct * @@ -430,6 +454,7 @@ static inline void mavlink_msg_scaled_imu_decode(const mavlink_message_t* msg, m scaled_imu->xmag = mavlink_msg_scaled_imu_get_xmag(msg); scaled_imu->ymag = mavlink_msg_scaled_imu_get_ymag(msg); scaled_imu->zmag = mavlink_msg_scaled_imu_get_zmag(msg); + scaled_imu->temperature = mavlink_msg_scaled_imu_get_temperature(msg); #else uint8_t len = msg->len < MAVLINK_MSG_ID_SCALED_IMU_LEN? msg->len : MAVLINK_MSG_ID_SCALED_IMU_LEN; memset(scaled_imu, 0, MAVLINK_MSG_ID_SCALED_IMU_LEN); diff --git a/lib/main/MAVLink/common/mavlink_msg_scaled_imu2.h b/lib/main/MAVLink/common/mavlink_msg_scaled_imu2.h index 297874cb4a..2106cefd1e 100755 --- a/lib/main/MAVLink/common/mavlink_msg_scaled_imu2.h +++ b/lib/main/MAVLink/common/mavlink_msg_scaled_imu2.h @@ -15,11 +15,12 @@ typedef struct __mavlink_scaled_imu2_t { int16_t xmag; /*< [mgauss] X Magnetic field*/ int16_t ymag; /*< [mgauss] Y Magnetic field*/ int16_t zmag; /*< [mgauss] Z Magnetic field*/ + int16_t temperature; /*< [cdegC] Temperature, 0: IMU does not provide temperature values. If the IMU is at 0C it must send 1 (0.01C).*/ } mavlink_scaled_imu2_t; -#define MAVLINK_MSG_ID_SCALED_IMU2_LEN 22 +#define MAVLINK_MSG_ID_SCALED_IMU2_LEN 24 #define MAVLINK_MSG_ID_SCALED_IMU2_MIN_LEN 22 -#define MAVLINK_MSG_ID_116_LEN 22 +#define MAVLINK_MSG_ID_116_LEN 24 #define MAVLINK_MSG_ID_116_MIN_LEN 22 #define MAVLINK_MSG_ID_SCALED_IMU2_CRC 76 @@ -31,7 +32,7 @@ typedef struct __mavlink_scaled_imu2_t { #define MAVLINK_MESSAGE_INFO_SCALED_IMU2 { \ 116, \ "SCALED_IMU2", \ - 10, \ + 11, \ { { "time_boot_ms", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_scaled_imu2_t, time_boot_ms) }, \ { "xacc", NULL, MAVLINK_TYPE_INT16_T, 0, 4, offsetof(mavlink_scaled_imu2_t, xacc) }, \ { "yacc", NULL, MAVLINK_TYPE_INT16_T, 0, 6, offsetof(mavlink_scaled_imu2_t, yacc) }, \ @@ -42,12 +43,13 @@ typedef struct __mavlink_scaled_imu2_t { { "xmag", NULL, MAVLINK_TYPE_INT16_T, 0, 16, offsetof(mavlink_scaled_imu2_t, xmag) }, \ { "ymag", NULL, MAVLINK_TYPE_INT16_T, 0, 18, offsetof(mavlink_scaled_imu2_t, ymag) }, \ { "zmag", NULL, MAVLINK_TYPE_INT16_T, 0, 20, offsetof(mavlink_scaled_imu2_t, zmag) }, \ + { "temperature", NULL, MAVLINK_TYPE_INT16_T, 0, 22, offsetof(mavlink_scaled_imu2_t, temperature) }, \ } \ } #else #define MAVLINK_MESSAGE_INFO_SCALED_IMU2 { \ "SCALED_IMU2", \ - 10, \ + 11, \ { { "time_boot_ms", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_scaled_imu2_t, time_boot_ms) }, \ { "xacc", NULL, MAVLINK_TYPE_INT16_T, 0, 4, offsetof(mavlink_scaled_imu2_t, xacc) }, \ { "yacc", NULL, MAVLINK_TYPE_INT16_T, 0, 6, offsetof(mavlink_scaled_imu2_t, yacc) }, \ @@ -58,6 +60,7 @@ typedef struct __mavlink_scaled_imu2_t { { "xmag", NULL, MAVLINK_TYPE_INT16_T, 0, 16, offsetof(mavlink_scaled_imu2_t, xmag) }, \ { "ymag", NULL, MAVLINK_TYPE_INT16_T, 0, 18, offsetof(mavlink_scaled_imu2_t, ymag) }, \ { "zmag", NULL, MAVLINK_TYPE_INT16_T, 0, 20, offsetof(mavlink_scaled_imu2_t, zmag) }, \ + { "temperature", NULL, MAVLINK_TYPE_INT16_T, 0, 22, offsetof(mavlink_scaled_imu2_t, temperature) }, \ } \ } #endif @@ -78,10 +81,11 @@ typedef struct __mavlink_scaled_imu2_t { * @param xmag [mgauss] X Magnetic field * @param ymag [mgauss] Y Magnetic field * @param zmag [mgauss] Z Magnetic field + * @param temperature [cdegC] Temperature, 0: IMU does not provide temperature values. If the IMU is at 0C it must send 1 (0.01C). * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_scaled_imu2_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint32_t time_boot_ms, int16_t xacc, int16_t yacc, int16_t zacc, int16_t xgyro, int16_t ygyro, int16_t zgyro, int16_t xmag, int16_t ymag, int16_t zmag) + uint32_t time_boot_ms, int16_t xacc, int16_t yacc, int16_t zacc, int16_t xgyro, int16_t ygyro, int16_t zgyro, int16_t xmag, int16_t ymag, int16_t zmag, int16_t temperature) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_SCALED_IMU2_LEN]; @@ -95,6 +99,7 @@ static inline uint16_t mavlink_msg_scaled_imu2_pack(uint8_t system_id, uint8_t c _mav_put_int16_t(buf, 16, xmag); _mav_put_int16_t(buf, 18, ymag); _mav_put_int16_t(buf, 20, zmag); + _mav_put_int16_t(buf, 22, temperature); memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SCALED_IMU2_LEN); #else @@ -109,6 +114,7 @@ static inline uint16_t mavlink_msg_scaled_imu2_pack(uint8_t system_id, uint8_t c packet.xmag = xmag; packet.ymag = ymag; packet.zmag = zmag; + packet.temperature = temperature; memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SCALED_IMU2_LEN); #endif @@ -133,11 +139,12 @@ static inline uint16_t mavlink_msg_scaled_imu2_pack(uint8_t system_id, uint8_t c * @param xmag [mgauss] X Magnetic field * @param ymag [mgauss] Y Magnetic field * @param zmag [mgauss] Z Magnetic field + * @param temperature [cdegC] Temperature, 0: IMU does not provide temperature values. If the IMU is at 0C it must send 1 (0.01C). * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_scaled_imu2_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, - uint32_t time_boot_ms,int16_t xacc,int16_t yacc,int16_t zacc,int16_t xgyro,int16_t ygyro,int16_t zgyro,int16_t xmag,int16_t ymag,int16_t zmag) + uint32_t time_boot_ms,int16_t xacc,int16_t yacc,int16_t zacc,int16_t xgyro,int16_t ygyro,int16_t zgyro,int16_t xmag,int16_t ymag,int16_t zmag,int16_t temperature) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_SCALED_IMU2_LEN]; @@ -151,6 +158,7 @@ static inline uint16_t mavlink_msg_scaled_imu2_pack_chan(uint8_t system_id, uint _mav_put_int16_t(buf, 16, xmag); _mav_put_int16_t(buf, 18, ymag); _mav_put_int16_t(buf, 20, zmag); + _mav_put_int16_t(buf, 22, temperature); memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SCALED_IMU2_LEN); #else @@ -165,6 +173,7 @@ static inline uint16_t mavlink_msg_scaled_imu2_pack_chan(uint8_t system_id, uint packet.xmag = xmag; packet.ymag = ymag; packet.zmag = zmag; + packet.temperature = temperature; memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SCALED_IMU2_LEN); #endif @@ -183,7 +192,7 @@ static inline uint16_t mavlink_msg_scaled_imu2_pack_chan(uint8_t system_id, uint */ static inline uint16_t mavlink_msg_scaled_imu2_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_scaled_imu2_t* scaled_imu2) { - return mavlink_msg_scaled_imu2_pack(system_id, component_id, msg, scaled_imu2->time_boot_ms, scaled_imu2->xacc, scaled_imu2->yacc, scaled_imu2->zacc, scaled_imu2->xgyro, scaled_imu2->ygyro, scaled_imu2->zgyro, scaled_imu2->xmag, scaled_imu2->ymag, scaled_imu2->zmag); + return mavlink_msg_scaled_imu2_pack(system_id, component_id, msg, scaled_imu2->time_boot_ms, scaled_imu2->xacc, scaled_imu2->yacc, scaled_imu2->zacc, scaled_imu2->xgyro, scaled_imu2->ygyro, scaled_imu2->zgyro, scaled_imu2->xmag, scaled_imu2->ymag, scaled_imu2->zmag, scaled_imu2->temperature); } /** @@ -197,7 +206,7 @@ static inline uint16_t mavlink_msg_scaled_imu2_encode(uint8_t system_id, uint8_t */ static inline uint16_t mavlink_msg_scaled_imu2_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_scaled_imu2_t* scaled_imu2) { - return mavlink_msg_scaled_imu2_pack_chan(system_id, component_id, chan, msg, scaled_imu2->time_boot_ms, scaled_imu2->xacc, scaled_imu2->yacc, scaled_imu2->zacc, scaled_imu2->xgyro, scaled_imu2->ygyro, scaled_imu2->zgyro, scaled_imu2->xmag, scaled_imu2->ymag, scaled_imu2->zmag); + return mavlink_msg_scaled_imu2_pack_chan(system_id, component_id, chan, msg, scaled_imu2->time_boot_ms, scaled_imu2->xacc, scaled_imu2->yacc, scaled_imu2->zacc, scaled_imu2->xgyro, scaled_imu2->ygyro, scaled_imu2->zgyro, scaled_imu2->xmag, scaled_imu2->ymag, scaled_imu2->zmag, scaled_imu2->temperature); } /** @@ -214,10 +223,11 @@ static inline uint16_t mavlink_msg_scaled_imu2_encode_chan(uint8_t system_id, ui * @param xmag [mgauss] X Magnetic field * @param ymag [mgauss] Y Magnetic field * @param zmag [mgauss] Z Magnetic field + * @param temperature [cdegC] Temperature, 0: IMU does not provide temperature values. If the IMU is at 0C it must send 1 (0.01C). */ #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS -static inline void mavlink_msg_scaled_imu2_send(mavlink_channel_t chan, uint32_t time_boot_ms, int16_t xacc, int16_t yacc, int16_t zacc, int16_t xgyro, int16_t ygyro, int16_t zgyro, int16_t xmag, int16_t ymag, int16_t zmag) +static inline void mavlink_msg_scaled_imu2_send(mavlink_channel_t chan, uint32_t time_boot_ms, int16_t xacc, int16_t yacc, int16_t zacc, int16_t xgyro, int16_t ygyro, int16_t zgyro, int16_t xmag, int16_t ymag, int16_t zmag, int16_t temperature) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_SCALED_IMU2_LEN]; @@ -231,6 +241,7 @@ static inline void mavlink_msg_scaled_imu2_send(mavlink_channel_t chan, uint32_t _mav_put_int16_t(buf, 16, xmag); _mav_put_int16_t(buf, 18, ymag); _mav_put_int16_t(buf, 20, zmag); + _mav_put_int16_t(buf, 22, temperature); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SCALED_IMU2, buf, MAVLINK_MSG_ID_SCALED_IMU2_MIN_LEN, MAVLINK_MSG_ID_SCALED_IMU2_LEN, MAVLINK_MSG_ID_SCALED_IMU2_CRC); #else @@ -245,6 +256,7 @@ static inline void mavlink_msg_scaled_imu2_send(mavlink_channel_t chan, uint32_t packet.xmag = xmag; packet.ymag = ymag; packet.zmag = zmag; + packet.temperature = temperature; _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SCALED_IMU2, (const char *)&packet, MAVLINK_MSG_ID_SCALED_IMU2_MIN_LEN, MAVLINK_MSG_ID_SCALED_IMU2_LEN, MAVLINK_MSG_ID_SCALED_IMU2_CRC); #endif @@ -258,7 +270,7 @@ static inline void mavlink_msg_scaled_imu2_send(mavlink_channel_t chan, uint32_t static inline void mavlink_msg_scaled_imu2_send_struct(mavlink_channel_t chan, const mavlink_scaled_imu2_t* scaled_imu2) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - mavlink_msg_scaled_imu2_send(chan, scaled_imu2->time_boot_ms, scaled_imu2->xacc, scaled_imu2->yacc, scaled_imu2->zacc, scaled_imu2->xgyro, scaled_imu2->ygyro, scaled_imu2->zgyro, scaled_imu2->xmag, scaled_imu2->ymag, scaled_imu2->zmag); + mavlink_msg_scaled_imu2_send(chan, scaled_imu2->time_boot_ms, scaled_imu2->xacc, scaled_imu2->yacc, scaled_imu2->zacc, scaled_imu2->xgyro, scaled_imu2->ygyro, scaled_imu2->zgyro, scaled_imu2->xmag, scaled_imu2->ymag, scaled_imu2->zmag, scaled_imu2->temperature); #else _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SCALED_IMU2, (const char *)scaled_imu2, MAVLINK_MSG_ID_SCALED_IMU2_MIN_LEN, MAVLINK_MSG_ID_SCALED_IMU2_LEN, MAVLINK_MSG_ID_SCALED_IMU2_CRC); #endif @@ -272,7 +284,7 @@ static inline void mavlink_msg_scaled_imu2_send_struct(mavlink_channel_t chan, c is usually the receive buffer for the channel, and allows a reply to an incoming message with minimum stack space usage. */ -static inline void mavlink_msg_scaled_imu2_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint32_t time_boot_ms, int16_t xacc, int16_t yacc, int16_t zacc, int16_t xgyro, int16_t ygyro, int16_t zgyro, int16_t xmag, int16_t ymag, int16_t zmag) +static inline void mavlink_msg_scaled_imu2_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint32_t time_boot_ms, int16_t xacc, int16_t yacc, int16_t zacc, int16_t xgyro, int16_t ygyro, int16_t zgyro, int16_t xmag, int16_t ymag, int16_t zmag, int16_t temperature) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char *buf = (char *)msgbuf; @@ -286,6 +298,7 @@ static inline void mavlink_msg_scaled_imu2_send_buf(mavlink_message_t *msgbuf, m _mav_put_int16_t(buf, 16, xmag); _mav_put_int16_t(buf, 18, ymag); _mav_put_int16_t(buf, 20, zmag); + _mav_put_int16_t(buf, 22, temperature); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SCALED_IMU2, buf, MAVLINK_MSG_ID_SCALED_IMU2_MIN_LEN, MAVLINK_MSG_ID_SCALED_IMU2_LEN, MAVLINK_MSG_ID_SCALED_IMU2_CRC); #else @@ -300,6 +313,7 @@ static inline void mavlink_msg_scaled_imu2_send_buf(mavlink_message_t *msgbuf, m packet->xmag = xmag; packet->ymag = ymag; packet->zmag = zmag; + packet->temperature = temperature; _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SCALED_IMU2, (const char *)packet, MAVLINK_MSG_ID_SCALED_IMU2_MIN_LEN, MAVLINK_MSG_ID_SCALED_IMU2_LEN, MAVLINK_MSG_ID_SCALED_IMU2_CRC); #endif @@ -411,6 +425,16 @@ static inline int16_t mavlink_msg_scaled_imu2_get_zmag(const mavlink_message_t* return _MAV_RETURN_int16_t(msg, 20); } +/** + * @brief Get field temperature from scaled_imu2 message + * + * @return [cdegC] Temperature, 0: IMU does not provide temperature values. If the IMU is at 0C it must send 1 (0.01C). + */ +static inline int16_t mavlink_msg_scaled_imu2_get_temperature(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int16_t(msg, 22); +} + /** * @brief Decode a scaled_imu2 message into a struct * @@ -430,6 +454,7 @@ static inline void mavlink_msg_scaled_imu2_decode(const mavlink_message_t* msg, scaled_imu2->xmag = mavlink_msg_scaled_imu2_get_xmag(msg); scaled_imu2->ymag = mavlink_msg_scaled_imu2_get_ymag(msg); scaled_imu2->zmag = mavlink_msg_scaled_imu2_get_zmag(msg); + scaled_imu2->temperature = mavlink_msg_scaled_imu2_get_temperature(msg); #else uint8_t len = msg->len < MAVLINK_MSG_ID_SCALED_IMU2_LEN? msg->len : MAVLINK_MSG_ID_SCALED_IMU2_LEN; memset(scaled_imu2, 0, MAVLINK_MSG_ID_SCALED_IMU2_LEN); diff --git a/lib/main/MAVLink/common/mavlink_msg_scaled_imu3.h b/lib/main/MAVLink/common/mavlink_msg_scaled_imu3.h index d01ac948fd..cd20f23566 100755 --- a/lib/main/MAVLink/common/mavlink_msg_scaled_imu3.h +++ b/lib/main/MAVLink/common/mavlink_msg_scaled_imu3.h @@ -15,11 +15,12 @@ typedef struct __mavlink_scaled_imu3_t { int16_t xmag; /*< [mgauss] X Magnetic field*/ int16_t ymag; /*< [mgauss] Y Magnetic field*/ int16_t zmag; /*< [mgauss] Z Magnetic field*/ + int16_t temperature; /*< [cdegC] Temperature, 0: IMU does not provide temperature values. If the IMU is at 0C it must send 1 (0.01C).*/ } mavlink_scaled_imu3_t; -#define MAVLINK_MSG_ID_SCALED_IMU3_LEN 22 +#define MAVLINK_MSG_ID_SCALED_IMU3_LEN 24 #define MAVLINK_MSG_ID_SCALED_IMU3_MIN_LEN 22 -#define MAVLINK_MSG_ID_129_LEN 22 +#define MAVLINK_MSG_ID_129_LEN 24 #define MAVLINK_MSG_ID_129_MIN_LEN 22 #define MAVLINK_MSG_ID_SCALED_IMU3_CRC 46 @@ -31,7 +32,7 @@ typedef struct __mavlink_scaled_imu3_t { #define MAVLINK_MESSAGE_INFO_SCALED_IMU3 { \ 129, \ "SCALED_IMU3", \ - 10, \ + 11, \ { { "time_boot_ms", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_scaled_imu3_t, time_boot_ms) }, \ { "xacc", NULL, MAVLINK_TYPE_INT16_T, 0, 4, offsetof(mavlink_scaled_imu3_t, xacc) }, \ { "yacc", NULL, MAVLINK_TYPE_INT16_T, 0, 6, offsetof(mavlink_scaled_imu3_t, yacc) }, \ @@ -42,12 +43,13 @@ typedef struct __mavlink_scaled_imu3_t { { "xmag", NULL, MAVLINK_TYPE_INT16_T, 0, 16, offsetof(mavlink_scaled_imu3_t, xmag) }, \ { "ymag", NULL, MAVLINK_TYPE_INT16_T, 0, 18, offsetof(mavlink_scaled_imu3_t, ymag) }, \ { "zmag", NULL, MAVLINK_TYPE_INT16_T, 0, 20, offsetof(mavlink_scaled_imu3_t, zmag) }, \ + { "temperature", NULL, MAVLINK_TYPE_INT16_T, 0, 22, offsetof(mavlink_scaled_imu3_t, temperature) }, \ } \ } #else #define MAVLINK_MESSAGE_INFO_SCALED_IMU3 { \ "SCALED_IMU3", \ - 10, \ + 11, \ { { "time_boot_ms", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_scaled_imu3_t, time_boot_ms) }, \ { "xacc", NULL, MAVLINK_TYPE_INT16_T, 0, 4, offsetof(mavlink_scaled_imu3_t, xacc) }, \ { "yacc", NULL, MAVLINK_TYPE_INT16_T, 0, 6, offsetof(mavlink_scaled_imu3_t, yacc) }, \ @@ -58,6 +60,7 @@ typedef struct __mavlink_scaled_imu3_t { { "xmag", NULL, MAVLINK_TYPE_INT16_T, 0, 16, offsetof(mavlink_scaled_imu3_t, xmag) }, \ { "ymag", NULL, MAVLINK_TYPE_INT16_T, 0, 18, offsetof(mavlink_scaled_imu3_t, ymag) }, \ { "zmag", NULL, MAVLINK_TYPE_INT16_T, 0, 20, offsetof(mavlink_scaled_imu3_t, zmag) }, \ + { "temperature", NULL, MAVLINK_TYPE_INT16_T, 0, 22, offsetof(mavlink_scaled_imu3_t, temperature) }, \ } \ } #endif @@ -78,10 +81,11 @@ typedef struct __mavlink_scaled_imu3_t { * @param xmag [mgauss] X Magnetic field * @param ymag [mgauss] Y Magnetic field * @param zmag [mgauss] Z Magnetic field + * @param temperature [cdegC] Temperature, 0: IMU does not provide temperature values. If the IMU is at 0C it must send 1 (0.01C). * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_scaled_imu3_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint32_t time_boot_ms, int16_t xacc, int16_t yacc, int16_t zacc, int16_t xgyro, int16_t ygyro, int16_t zgyro, int16_t xmag, int16_t ymag, int16_t zmag) + uint32_t time_boot_ms, int16_t xacc, int16_t yacc, int16_t zacc, int16_t xgyro, int16_t ygyro, int16_t zgyro, int16_t xmag, int16_t ymag, int16_t zmag, int16_t temperature) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_SCALED_IMU3_LEN]; @@ -95,6 +99,7 @@ static inline uint16_t mavlink_msg_scaled_imu3_pack(uint8_t system_id, uint8_t c _mav_put_int16_t(buf, 16, xmag); _mav_put_int16_t(buf, 18, ymag); _mav_put_int16_t(buf, 20, zmag); + _mav_put_int16_t(buf, 22, temperature); memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SCALED_IMU3_LEN); #else @@ -109,6 +114,7 @@ static inline uint16_t mavlink_msg_scaled_imu3_pack(uint8_t system_id, uint8_t c packet.xmag = xmag; packet.ymag = ymag; packet.zmag = zmag; + packet.temperature = temperature; memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SCALED_IMU3_LEN); #endif @@ -133,11 +139,12 @@ static inline uint16_t mavlink_msg_scaled_imu3_pack(uint8_t system_id, uint8_t c * @param xmag [mgauss] X Magnetic field * @param ymag [mgauss] Y Magnetic field * @param zmag [mgauss] Z Magnetic field + * @param temperature [cdegC] Temperature, 0: IMU does not provide temperature values. If the IMU is at 0C it must send 1 (0.01C). * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_scaled_imu3_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, - uint32_t time_boot_ms,int16_t xacc,int16_t yacc,int16_t zacc,int16_t xgyro,int16_t ygyro,int16_t zgyro,int16_t xmag,int16_t ymag,int16_t zmag) + uint32_t time_boot_ms,int16_t xacc,int16_t yacc,int16_t zacc,int16_t xgyro,int16_t ygyro,int16_t zgyro,int16_t xmag,int16_t ymag,int16_t zmag,int16_t temperature) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_SCALED_IMU3_LEN]; @@ -151,6 +158,7 @@ static inline uint16_t mavlink_msg_scaled_imu3_pack_chan(uint8_t system_id, uint _mav_put_int16_t(buf, 16, xmag); _mav_put_int16_t(buf, 18, ymag); _mav_put_int16_t(buf, 20, zmag); + _mav_put_int16_t(buf, 22, temperature); memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SCALED_IMU3_LEN); #else @@ -165,6 +173,7 @@ static inline uint16_t mavlink_msg_scaled_imu3_pack_chan(uint8_t system_id, uint packet.xmag = xmag; packet.ymag = ymag; packet.zmag = zmag; + packet.temperature = temperature; memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SCALED_IMU3_LEN); #endif @@ -183,7 +192,7 @@ static inline uint16_t mavlink_msg_scaled_imu3_pack_chan(uint8_t system_id, uint */ static inline uint16_t mavlink_msg_scaled_imu3_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_scaled_imu3_t* scaled_imu3) { - return mavlink_msg_scaled_imu3_pack(system_id, component_id, msg, scaled_imu3->time_boot_ms, scaled_imu3->xacc, scaled_imu3->yacc, scaled_imu3->zacc, scaled_imu3->xgyro, scaled_imu3->ygyro, scaled_imu3->zgyro, scaled_imu3->xmag, scaled_imu3->ymag, scaled_imu3->zmag); + return mavlink_msg_scaled_imu3_pack(system_id, component_id, msg, scaled_imu3->time_boot_ms, scaled_imu3->xacc, scaled_imu3->yacc, scaled_imu3->zacc, scaled_imu3->xgyro, scaled_imu3->ygyro, scaled_imu3->zgyro, scaled_imu3->xmag, scaled_imu3->ymag, scaled_imu3->zmag, scaled_imu3->temperature); } /** @@ -197,7 +206,7 @@ static inline uint16_t mavlink_msg_scaled_imu3_encode(uint8_t system_id, uint8_t */ static inline uint16_t mavlink_msg_scaled_imu3_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_scaled_imu3_t* scaled_imu3) { - return mavlink_msg_scaled_imu3_pack_chan(system_id, component_id, chan, msg, scaled_imu3->time_boot_ms, scaled_imu3->xacc, scaled_imu3->yacc, scaled_imu3->zacc, scaled_imu3->xgyro, scaled_imu3->ygyro, scaled_imu3->zgyro, scaled_imu3->xmag, scaled_imu3->ymag, scaled_imu3->zmag); + return mavlink_msg_scaled_imu3_pack_chan(system_id, component_id, chan, msg, scaled_imu3->time_boot_ms, scaled_imu3->xacc, scaled_imu3->yacc, scaled_imu3->zacc, scaled_imu3->xgyro, scaled_imu3->ygyro, scaled_imu3->zgyro, scaled_imu3->xmag, scaled_imu3->ymag, scaled_imu3->zmag, scaled_imu3->temperature); } /** @@ -214,10 +223,11 @@ static inline uint16_t mavlink_msg_scaled_imu3_encode_chan(uint8_t system_id, ui * @param xmag [mgauss] X Magnetic field * @param ymag [mgauss] Y Magnetic field * @param zmag [mgauss] Z Magnetic field + * @param temperature [cdegC] Temperature, 0: IMU does not provide temperature values. If the IMU is at 0C it must send 1 (0.01C). */ #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS -static inline void mavlink_msg_scaled_imu3_send(mavlink_channel_t chan, uint32_t time_boot_ms, int16_t xacc, int16_t yacc, int16_t zacc, int16_t xgyro, int16_t ygyro, int16_t zgyro, int16_t xmag, int16_t ymag, int16_t zmag) +static inline void mavlink_msg_scaled_imu3_send(mavlink_channel_t chan, uint32_t time_boot_ms, int16_t xacc, int16_t yacc, int16_t zacc, int16_t xgyro, int16_t ygyro, int16_t zgyro, int16_t xmag, int16_t ymag, int16_t zmag, int16_t temperature) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_SCALED_IMU3_LEN]; @@ -231,6 +241,7 @@ static inline void mavlink_msg_scaled_imu3_send(mavlink_channel_t chan, uint32_t _mav_put_int16_t(buf, 16, xmag); _mav_put_int16_t(buf, 18, ymag); _mav_put_int16_t(buf, 20, zmag); + _mav_put_int16_t(buf, 22, temperature); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SCALED_IMU3, buf, MAVLINK_MSG_ID_SCALED_IMU3_MIN_LEN, MAVLINK_MSG_ID_SCALED_IMU3_LEN, MAVLINK_MSG_ID_SCALED_IMU3_CRC); #else @@ -245,6 +256,7 @@ static inline void mavlink_msg_scaled_imu3_send(mavlink_channel_t chan, uint32_t packet.xmag = xmag; packet.ymag = ymag; packet.zmag = zmag; + packet.temperature = temperature; _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SCALED_IMU3, (const char *)&packet, MAVLINK_MSG_ID_SCALED_IMU3_MIN_LEN, MAVLINK_MSG_ID_SCALED_IMU3_LEN, MAVLINK_MSG_ID_SCALED_IMU3_CRC); #endif @@ -258,7 +270,7 @@ static inline void mavlink_msg_scaled_imu3_send(mavlink_channel_t chan, uint32_t static inline void mavlink_msg_scaled_imu3_send_struct(mavlink_channel_t chan, const mavlink_scaled_imu3_t* scaled_imu3) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - mavlink_msg_scaled_imu3_send(chan, scaled_imu3->time_boot_ms, scaled_imu3->xacc, scaled_imu3->yacc, scaled_imu3->zacc, scaled_imu3->xgyro, scaled_imu3->ygyro, scaled_imu3->zgyro, scaled_imu3->xmag, scaled_imu3->ymag, scaled_imu3->zmag); + mavlink_msg_scaled_imu3_send(chan, scaled_imu3->time_boot_ms, scaled_imu3->xacc, scaled_imu3->yacc, scaled_imu3->zacc, scaled_imu3->xgyro, scaled_imu3->ygyro, scaled_imu3->zgyro, scaled_imu3->xmag, scaled_imu3->ymag, scaled_imu3->zmag, scaled_imu3->temperature); #else _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SCALED_IMU3, (const char *)scaled_imu3, MAVLINK_MSG_ID_SCALED_IMU3_MIN_LEN, MAVLINK_MSG_ID_SCALED_IMU3_LEN, MAVLINK_MSG_ID_SCALED_IMU3_CRC); #endif @@ -272,7 +284,7 @@ static inline void mavlink_msg_scaled_imu3_send_struct(mavlink_channel_t chan, c is usually the receive buffer for the channel, and allows a reply to an incoming message with minimum stack space usage. */ -static inline void mavlink_msg_scaled_imu3_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint32_t time_boot_ms, int16_t xacc, int16_t yacc, int16_t zacc, int16_t xgyro, int16_t ygyro, int16_t zgyro, int16_t xmag, int16_t ymag, int16_t zmag) +static inline void mavlink_msg_scaled_imu3_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint32_t time_boot_ms, int16_t xacc, int16_t yacc, int16_t zacc, int16_t xgyro, int16_t ygyro, int16_t zgyro, int16_t xmag, int16_t ymag, int16_t zmag, int16_t temperature) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char *buf = (char *)msgbuf; @@ -286,6 +298,7 @@ static inline void mavlink_msg_scaled_imu3_send_buf(mavlink_message_t *msgbuf, m _mav_put_int16_t(buf, 16, xmag); _mav_put_int16_t(buf, 18, ymag); _mav_put_int16_t(buf, 20, zmag); + _mav_put_int16_t(buf, 22, temperature); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SCALED_IMU3, buf, MAVLINK_MSG_ID_SCALED_IMU3_MIN_LEN, MAVLINK_MSG_ID_SCALED_IMU3_LEN, MAVLINK_MSG_ID_SCALED_IMU3_CRC); #else @@ -300,6 +313,7 @@ static inline void mavlink_msg_scaled_imu3_send_buf(mavlink_message_t *msgbuf, m packet->xmag = xmag; packet->ymag = ymag; packet->zmag = zmag; + packet->temperature = temperature; _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SCALED_IMU3, (const char *)packet, MAVLINK_MSG_ID_SCALED_IMU3_MIN_LEN, MAVLINK_MSG_ID_SCALED_IMU3_LEN, MAVLINK_MSG_ID_SCALED_IMU3_CRC); #endif @@ -411,6 +425,16 @@ static inline int16_t mavlink_msg_scaled_imu3_get_zmag(const mavlink_message_t* return _MAV_RETURN_int16_t(msg, 20); } +/** + * @brief Get field temperature from scaled_imu3 message + * + * @return [cdegC] Temperature, 0: IMU does not provide temperature values. If the IMU is at 0C it must send 1 (0.01C). + */ +static inline int16_t mavlink_msg_scaled_imu3_get_temperature(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int16_t(msg, 22); +} + /** * @brief Decode a scaled_imu3 message into a struct * @@ -430,6 +454,7 @@ static inline void mavlink_msg_scaled_imu3_decode(const mavlink_message_t* msg, scaled_imu3->xmag = mavlink_msg_scaled_imu3_get_xmag(msg); scaled_imu3->ymag = mavlink_msg_scaled_imu3_get_ymag(msg); scaled_imu3->zmag = mavlink_msg_scaled_imu3_get_zmag(msg); + scaled_imu3->temperature = mavlink_msg_scaled_imu3_get_temperature(msg); #else uint8_t len = msg->len < MAVLINK_MSG_ID_SCALED_IMU3_LEN? msg->len : MAVLINK_MSG_ID_SCALED_IMU3_LEN; memset(scaled_imu3, 0, MAVLINK_MSG_ID_SCALED_IMU3_LEN); diff --git a/lib/main/MAVLink/common/mavlink_msg_scaled_pressure.h b/lib/main/MAVLink/common/mavlink_msg_scaled_pressure.h index 9889c4676a..13344a8a68 100755 --- a/lib/main/MAVLink/common/mavlink_msg_scaled_pressure.h +++ b/lib/main/MAVLink/common/mavlink_msg_scaled_pressure.h @@ -9,11 +9,12 @@ typedef struct __mavlink_scaled_pressure_t { float press_abs; /*< [hPa] Absolute pressure*/ float press_diff; /*< [hPa] Differential pressure 1*/ int16_t temperature; /*< [cdegC] Absolute pressure temperature*/ + int16_t temperature_press_diff; /*< [cdegC] Differential pressure temperature (0, if not available). Report values of 0 (or 1) as 1 cdegC.*/ } mavlink_scaled_pressure_t; -#define MAVLINK_MSG_ID_SCALED_PRESSURE_LEN 14 +#define MAVLINK_MSG_ID_SCALED_PRESSURE_LEN 16 #define MAVLINK_MSG_ID_SCALED_PRESSURE_MIN_LEN 14 -#define MAVLINK_MSG_ID_29_LEN 14 +#define MAVLINK_MSG_ID_29_LEN 16 #define MAVLINK_MSG_ID_29_MIN_LEN 14 #define MAVLINK_MSG_ID_SCALED_PRESSURE_CRC 115 @@ -25,21 +26,23 @@ typedef struct __mavlink_scaled_pressure_t { #define MAVLINK_MESSAGE_INFO_SCALED_PRESSURE { \ 29, \ "SCALED_PRESSURE", \ - 4, \ + 5, \ { { "time_boot_ms", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_scaled_pressure_t, time_boot_ms) }, \ { "press_abs", NULL, MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_scaled_pressure_t, press_abs) }, \ { "press_diff", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_scaled_pressure_t, press_diff) }, \ { "temperature", NULL, MAVLINK_TYPE_INT16_T, 0, 12, offsetof(mavlink_scaled_pressure_t, temperature) }, \ + { "temperature_press_diff", NULL, MAVLINK_TYPE_INT16_T, 0, 14, offsetof(mavlink_scaled_pressure_t, temperature_press_diff) }, \ } \ } #else #define MAVLINK_MESSAGE_INFO_SCALED_PRESSURE { \ "SCALED_PRESSURE", \ - 4, \ + 5, \ { { "time_boot_ms", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_scaled_pressure_t, time_boot_ms) }, \ { "press_abs", NULL, MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_scaled_pressure_t, press_abs) }, \ { "press_diff", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_scaled_pressure_t, press_diff) }, \ { "temperature", NULL, MAVLINK_TYPE_INT16_T, 0, 12, offsetof(mavlink_scaled_pressure_t, temperature) }, \ + { "temperature_press_diff", NULL, MAVLINK_TYPE_INT16_T, 0, 14, offsetof(mavlink_scaled_pressure_t, temperature_press_diff) }, \ } \ } #endif @@ -54,10 +57,11 @@ typedef struct __mavlink_scaled_pressure_t { * @param press_abs [hPa] Absolute pressure * @param press_diff [hPa] Differential pressure 1 * @param temperature [cdegC] Absolute pressure temperature + * @param temperature_press_diff [cdegC] Differential pressure temperature (0, if not available). Report values of 0 (or 1) as 1 cdegC. * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_scaled_pressure_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint32_t time_boot_ms, float press_abs, float press_diff, int16_t temperature) + uint32_t time_boot_ms, float press_abs, float press_diff, int16_t temperature, int16_t temperature_press_diff) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_SCALED_PRESSURE_LEN]; @@ -65,6 +69,7 @@ static inline uint16_t mavlink_msg_scaled_pressure_pack(uint8_t system_id, uint8 _mav_put_float(buf, 4, press_abs); _mav_put_float(buf, 8, press_diff); _mav_put_int16_t(buf, 12, temperature); + _mav_put_int16_t(buf, 14, temperature_press_diff); memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SCALED_PRESSURE_LEN); #else @@ -73,6 +78,7 @@ static inline uint16_t mavlink_msg_scaled_pressure_pack(uint8_t system_id, uint8 packet.press_abs = press_abs; packet.press_diff = press_diff; packet.temperature = temperature; + packet.temperature_press_diff = temperature_press_diff; memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SCALED_PRESSURE_LEN); #endif @@ -91,11 +97,12 @@ static inline uint16_t mavlink_msg_scaled_pressure_pack(uint8_t system_id, uint8 * @param press_abs [hPa] Absolute pressure * @param press_diff [hPa] Differential pressure 1 * @param temperature [cdegC] Absolute pressure temperature + * @param temperature_press_diff [cdegC] Differential pressure temperature (0, if not available). Report values of 0 (or 1) as 1 cdegC. * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_scaled_pressure_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, - uint32_t time_boot_ms,float press_abs,float press_diff,int16_t temperature) + uint32_t time_boot_ms,float press_abs,float press_diff,int16_t temperature,int16_t temperature_press_diff) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_SCALED_PRESSURE_LEN]; @@ -103,6 +110,7 @@ static inline uint16_t mavlink_msg_scaled_pressure_pack_chan(uint8_t system_id, _mav_put_float(buf, 4, press_abs); _mav_put_float(buf, 8, press_diff); _mav_put_int16_t(buf, 12, temperature); + _mav_put_int16_t(buf, 14, temperature_press_diff); memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SCALED_PRESSURE_LEN); #else @@ -111,6 +119,7 @@ static inline uint16_t mavlink_msg_scaled_pressure_pack_chan(uint8_t system_id, packet.press_abs = press_abs; packet.press_diff = press_diff; packet.temperature = temperature; + packet.temperature_press_diff = temperature_press_diff; memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SCALED_PRESSURE_LEN); #endif @@ -129,7 +138,7 @@ static inline uint16_t mavlink_msg_scaled_pressure_pack_chan(uint8_t system_id, */ static inline uint16_t mavlink_msg_scaled_pressure_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_scaled_pressure_t* scaled_pressure) { - return mavlink_msg_scaled_pressure_pack(system_id, component_id, msg, scaled_pressure->time_boot_ms, scaled_pressure->press_abs, scaled_pressure->press_diff, scaled_pressure->temperature); + return mavlink_msg_scaled_pressure_pack(system_id, component_id, msg, scaled_pressure->time_boot_ms, scaled_pressure->press_abs, scaled_pressure->press_diff, scaled_pressure->temperature, scaled_pressure->temperature_press_diff); } /** @@ -143,7 +152,7 @@ static inline uint16_t mavlink_msg_scaled_pressure_encode(uint8_t system_id, uin */ static inline uint16_t mavlink_msg_scaled_pressure_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_scaled_pressure_t* scaled_pressure) { - return mavlink_msg_scaled_pressure_pack_chan(system_id, component_id, chan, msg, scaled_pressure->time_boot_ms, scaled_pressure->press_abs, scaled_pressure->press_diff, scaled_pressure->temperature); + return mavlink_msg_scaled_pressure_pack_chan(system_id, component_id, chan, msg, scaled_pressure->time_boot_ms, scaled_pressure->press_abs, scaled_pressure->press_diff, scaled_pressure->temperature, scaled_pressure->temperature_press_diff); } /** @@ -154,10 +163,11 @@ static inline uint16_t mavlink_msg_scaled_pressure_encode_chan(uint8_t system_id * @param press_abs [hPa] Absolute pressure * @param press_diff [hPa] Differential pressure 1 * @param temperature [cdegC] Absolute pressure temperature + * @param temperature_press_diff [cdegC] Differential pressure temperature (0, if not available). Report values of 0 (or 1) as 1 cdegC. */ #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS -static inline void mavlink_msg_scaled_pressure_send(mavlink_channel_t chan, uint32_t time_boot_ms, float press_abs, float press_diff, int16_t temperature) +static inline void mavlink_msg_scaled_pressure_send(mavlink_channel_t chan, uint32_t time_boot_ms, float press_abs, float press_diff, int16_t temperature, int16_t temperature_press_diff) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_SCALED_PRESSURE_LEN]; @@ -165,6 +175,7 @@ static inline void mavlink_msg_scaled_pressure_send(mavlink_channel_t chan, uint _mav_put_float(buf, 4, press_abs); _mav_put_float(buf, 8, press_diff); _mav_put_int16_t(buf, 12, temperature); + _mav_put_int16_t(buf, 14, temperature_press_diff); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SCALED_PRESSURE, buf, MAVLINK_MSG_ID_SCALED_PRESSURE_MIN_LEN, MAVLINK_MSG_ID_SCALED_PRESSURE_LEN, MAVLINK_MSG_ID_SCALED_PRESSURE_CRC); #else @@ -173,6 +184,7 @@ static inline void mavlink_msg_scaled_pressure_send(mavlink_channel_t chan, uint packet.press_abs = press_abs; packet.press_diff = press_diff; packet.temperature = temperature; + packet.temperature_press_diff = temperature_press_diff; _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SCALED_PRESSURE, (const char *)&packet, MAVLINK_MSG_ID_SCALED_PRESSURE_MIN_LEN, MAVLINK_MSG_ID_SCALED_PRESSURE_LEN, MAVLINK_MSG_ID_SCALED_PRESSURE_CRC); #endif @@ -186,7 +198,7 @@ static inline void mavlink_msg_scaled_pressure_send(mavlink_channel_t chan, uint static inline void mavlink_msg_scaled_pressure_send_struct(mavlink_channel_t chan, const mavlink_scaled_pressure_t* scaled_pressure) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - mavlink_msg_scaled_pressure_send(chan, scaled_pressure->time_boot_ms, scaled_pressure->press_abs, scaled_pressure->press_diff, scaled_pressure->temperature); + mavlink_msg_scaled_pressure_send(chan, scaled_pressure->time_boot_ms, scaled_pressure->press_abs, scaled_pressure->press_diff, scaled_pressure->temperature, scaled_pressure->temperature_press_diff); #else _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SCALED_PRESSURE, (const char *)scaled_pressure, MAVLINK_MSG_ID_SCALED_PRESSURE_MIN_LEN, MAVLINK_MSG_ID_SCALED_PRESSURE_LEN, MAVLINK_MSG_ID_SCALED_PRESSURE_CRC); #endif @@ -200,7 +212,7 @@ static inline void mavlink_msg_scaled_pressure_send_struct(mavlink_channel_t cha is usually the receive buffer for the channel, and allows a reply to an incoming message with minimum stack space usage. */ -static inline void mavlink_msg_scaled_pressure_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint32_t time_boot_ms, float press_abs, float press_diff, int16_t temperature) +static inline void mavlink_msg_scaled_pressure_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint32_t time_boot_ms, float press_abs, float press_diff, int16_t temperature, int16_t temperature_press_diff) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char *buf = (char *)msgbuf; @@ -208,6 +220,7 @@ static inline void mavlink_msg_scaled_pressure_send_buf(mavlink_message_t *msgbu _mav_put_float(buf, 4, press_abs); _mav_put_float(buf, 8, press_diff); _mav_put_int16_t(buf, 12, temperature); + _mav_put_int16_t(buf, 14, temperature_press_diff); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SCALED_PRESSURE, buf, MAVLINK_MSG_ID_SCALED_PRESSURE_MIN_LEN, MAVLINK_MSG_ID_SCALED_PRESSURE_LEN, MAVLINK_MSG_ID_SCALED_PRESSURE_CRC); #else @@ -216,6 +229,7 @@ static inline void mavlink_msg_scaled_pressure_send_buf(mavlink_message_t *msgbu packet->press_abs = press_abs; packet->press_diff = press_diff; packet->temperature = temperature; + packet->temperature_press_diff = temperature_press_diff; _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SCALED_PRESSURE, (const char *)packet, MAVLINK_MSG_ID_SCALED_PRESSURE_MIN_LEN, MAVLINK_MSG_ID_SCALED_PRESSURE_LEN, MAVLINK_MSG_ID_SCALED_PRESSURE_CRC); #endif @@ -267,6 +281,16 @@ static inline int16_t mavlink_msg_scaled_pressure_get_temperature(const mavlink_ return _MAV_RETURN_int16_t(msg, 12); } +/** + * @brief Get field temperature_press_diff from scaled_pressure message + * + * @return [cdegC] Differential pressure temperature (0, if not available). Report values of 0 (or 1) as 1 cdegC. + */ +static inline int16_t mavlink_msg_scaled_pressure_get_temperature_press_diff(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int16_t(msg, 14); +} + /** * @brief Decode a scaled_pressure message into a struct * @@ -280,6 +304,7 @@ static inline void mavlink_msg_scaled_pressure_decode(const mavlink_message_t* m scaled_pressure->press_abs = mavlink_msg_scaled_pressure_get_press_abs(msg); scaled_pressure->press_diff = mavlink_msg_scaled_pressure_get_press_diff(msg); scaled_pressure->temperature = mavlink_msg_scaled_pressure_get_temperature(msg); + scaled_pressure->temperature_press_diff = mavlink_msg_scaled_pressure_get_temperature_press_diff(msg); #else uint8_t len = msg->len < MAVLINK_MSG_ID_SCALED_PRESSURE_LEN? msg->len : MAVLINK_MSG_ID_SCALED_PRESSURE_LEN; memset(scaled_pressure, 0, MAVLINK_MSG_ID_SCALED_PRESSURE_LEN); diff --git a/lib/main/MAVLink/common/mavlink_msg_scaled_pressure2.h b/lib/main/MAVLink/common/mavlink_msg_scaled_pressure2.h index 7ac74c6758..8dc28850e8 100755 --- a/lib/main/MAVLink/common/mavlink_msg_scaled_pressure2.h +++ b/lib/main/MAVLink/common/mavlink_msg_scaled_pressure2.h @@ -9,11 +9,12 @@ typedef struct __mavlink_scaled_pressure2_t { float press_abs; /*< [hPa] Absolute pressure*/ float press_diff; /*< [hPa] Differential pressure*/ int16_t temperature; /*< [cdegC] Absolute pressure temperature*/ + int16_t temperature_press_diff; /*< [cdegC] Differential pressure temperature (0, if not available). Report values of 0 (or 1) as 1 cdegC.*/ } mavlink_scaled_pressure2_t; -#define MAVLINK_MSG_ID_SCALED_PRESSURE2_LEN 14 +#define MAVLINK_MSG_ID_SCALED_PRESSURE2_LEN 16 #define MAVLINK_MSG_ID_SCALED_PRESSURE2_MIN_LEN 14 -#define MAVLINK_MSG_ID_137_LEN 14 +#define MAVLINK_MSG_ID_137_LEN 16 #define MAVLINK_MSG_ID_137_MIN_LEN 14 #define MAVLINK_MSG_ID_SCALED_PRESSURE2_CRC 195 @@ -25,21 +26,23 @@ typedef struct __mavlink_scaled_pressure2_t { #define MAVLINK_MESSAGE_INFO_SCALED_PRESSURE2 { \ 137, \ "SCALED_PRESSURE2", \ - 4, \ + 5, \ { { "time_boot_ms", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_scaled_pressure2_t, time_boot_ms) }, \ { "press_abs", NULL, MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_scaled_pressure2_t, press_abs) }, \ { "press_diff", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_scaled_pressure2_t, press_diff) }, \ { "temperature", NULL, MAVLINK_TYPE_INT16_T, 0, 12, offsetof(mavlink_scaled_pressure2_t, temperature) }, \ + { "temperature_press_diff", NULL, MAVLINK_TYPE_INT16_T, 0, 14, offsetof(mavlink_scaled_pressure2_t, temperature_press_diff) }, \ } \ } #else #define MAVLINK_MESSAGE_INFO_SCALED_PRESSURE2 { \ "SCALED_PRESSURE2", \ - 4, \ + 5, \ { { "time_boot_ms", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_scaled_pressure2_t, time_boot_ms) }, \ { "press_abs", NULL, MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_scaled_pressure2_t, press_abs) }, \ { "press_diff", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_scaled_pressure2_t, press_diff) }, \ { "temperature", NULL, MAVLINK_TYPE_INT16_T, 0, 12, offsetof(mavlink_scaled_pressure2_t, temperature) }, \ + { "temperature_press_diff", NULL, MAVLINK_TYPE_INT16_T, 0, 14, offsetof(mavlink_scaled_pressure2_t, temperature_press_diff) }, \ } \ } #endif @@ -54,10 +57,11 @@ typedef struct __mavlink_scaled_pressure2_t { * @param press_abs [hPa] Absolute pressure * @param press_diff [hPa] Differential pressure * @param temperature [cdegC] Absolute pressure temperature + * @param temperature_press_diff [cdegC] Differential pressure temperature (0, if not available). Report values of 0 (or 1) as 1 cdegC. * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_scaled_pressure2_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint32_t time_boot_ms, float press_abs, float press_diff, int16_t temperature) + uint32_t time_boot_ms, float press_abs, float press_diff, int16_t temperature, int16_t temperature_press_diff) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_SCALED_PRESSURE2_LEN]; @@ -65,6 +69,7 @@ static inline uint16_t mavlink_msg_scaled_pressure2_pack(uint8_t system_id, uint _mav_put_float(buf, 4, press_abs); _mav_put_float(buf, 8, press_diff); _mav_put_int16_t(buf, 12, temperature); + _mav_put_int16_t(buf, 14, temperature_press_diff); memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SCALED_PRESSURE2_LEN); #else @@ -73,6 +78,7 @@ static inline uint16_t mavlink_msg_scaled_pressure2_pack(uint8_t system_id, uint packet.press_abs = press_abs; packet.press_diff = press_diff; packet.temperature = temperature; + packet.temperature_press_diff = temperature_press_diff; memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SCALED_PRESSURE2_LEN); #endif @@ -91,11 +97,12 @@ static inline uint16_t mavlink_msg_scaled_pressure2_pack(uint8_t system_id, uint * @param press_abs [hPa] Absolute pressure * @param press_diff [hPa] Differential pressure * @param temperature [cdegC] Absolute pressure temperature + * @param temperature_press_diff [cdegC] Differential pressure temperature (0, if not available). Report values of 0 (or 1) as 1 cdegC. * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_scaled_pressure2_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, - uint32_t time_boot_ms,float press_abs,float press_diff,int16_t temperature) + uint32_t time_boot_ms,float press_abs,float press_diff,int16_t temperature,int16_t temperature_press_diff) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_SCALED_PRESSURE2_LEN]; @@ -103,6 +110,7 @@ static inline uint16_t mavlink_msg_scaled_pressure2_pack_chan(uint8_t system_id, _mav_put_float(buf, 4, press_abs); _mav_put_float(buf, 8, press_diff); _mav_put_int16_t(buf, 12, temperature); + _mav_put_int16_t(buf, 14, temperature_press_diff); memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SCALED_PRESSURE2_LEN); #else @@ -111,6 +119,7 @@ static inline uint16_t mavlink_msg_scaled_pressure2_pack_chan(uint8_t system_id, packet.press_abs = press_abs; packet.press_diff = press_diff; packet.temperature = temperature; + packet.temperature_press_diff = temperature_press_diff; memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SCALED_PRESSURE2_LEN); #endif @@ -129,7 +138,7 @@ static inline uint16_t mavlink_msg_scaled_pressure2_pack_chan(uint8_t system_id, */ static inline uint16_t mavlink_msg_scaled_pressure2_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_scaled_pressure2_t* scaled_pressure2) { - return mavlink_msg_scaled_pressure2_pack(system_id, component_id, msg, scaled_pressure2->time_boot_ms, scaled_pressure2->press_abs, scaled_pressure2->press_diff, scaled_pressure2->temperature); + return mavlink_msg_scaled_pressure2_pack(system_id, component_id, msg, scaled_pressure2->time_boot_ms, scaled_pressure2->press_abs, scaled_pressure2->press_diff, scaled_pressure2->temperature, scaled_pressure2->temperature_press_diff); } /** @@ -143,7 +152,7 @@ static inline uint16_t mavlink_msg_scaled_pressure2_encode(uint8_t system_id, ui */ static inline uint16_t mavlink_msg_scaled_pressure2_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_scaled_pressure2_t* scaled_pressure2) { - return mavlink_msg_scaled_pressure2_pack_chan(system_id, component_id, chan, msg, scaled_pressure2->time_boot_ms, scaled_pressure2->press_abs, scaled_pressure2->press_diff, scaled_pressure2->temperature); + return mavlink_msg_scaled_pressure2_pack_chan(system_id, component_id, chan, msg, scaled_pressure2->time_boot_ms, scaled_pressure2->press_abs, scaled_pressure2->press_diff, scaled_pressure2->temperature, scaled_pressure2->temperature_press_diff); } /** @@ -154,10 +163,11 @@ static inline uint16_t mavlink_msg_scaled_pressure2_encode_chan(uint8_t system_i * @param press_abs [hPa] Absolute pressure * @param press_diff [hPa] Differential pressure * @param temperature [cdegC] Absolute pressure temperature + * @param temperature_press_diff [cdegC] Differential pressure temperature (0, if not available). Report values of 0 (or 1) as 1 cdegC. */ #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS -static inline void mavlink_msg_scaled_pressure2_send(mavlink_channel_t chan, uint32_t time_boot_ms, float press_abs, float press_diff, int16_t temperature) +static inline void mavlink_msg_scaled_pressure2_send(mavlink_channel_t chan, uint32_t time_boot_ms, float press_abs, float press_diff, int16_t temperature, int16_t temperature_press_diff) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_SCALED_PRESSURE2_LEN]; @@ -165,6 +175,7 @@ static inline void mavlink_msg_scaled_pressure2_send(mavlink_channel_t chan, uin _mav_put_float(buf, 4, press_abs); _mav_put_float(buf, 8, press_diff); _mav_put_int16_t(buf, 12, temperature); + _mav_put_int16_t(buf, 14, temperature_press_diff); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SCALED_PRESSURE2, buf, MAVLINK_MSG_ID_SCALED_PRESSURE2_MIN_LEN, MAVLINK_MSG_ID_SCALED_PRESSURE2_LEN, MAVLINK_MSG_ID_SCALED_PRESSURE2_CRC); #else @@ -173,6 +184,7 @@ static inline void mavlink_msg_scaled_pressure2_send(mavlink_channel_t chan, uin packet.press_abs = press_abs; packet.press_diff = press_diff; packet.temperature = temperature; + packet.temperature_press_diff = temperature_press_diff; _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SCALED_PRESSURE2, (const char *)&packet, MAVLINK_MSG_ID_SCALED_PRESSURE2_MIN_LEN, MAVLINK_MSG_ID_SCALED_PRESSURE2_LEN, MAVLINK_MSG_ID_SCALED_PRESSURE2_CRC); #endif @@ -186,7 +198,7 @@ static inline void mavlink_msg_scaled_pressure2_send(mavlink_channel_t chan, uin static inline void mavlink_msg_scaled_pressure2_send_struct(mavlink_channel_t chan, const mavlink_scaled_pressure2_t* scaled_pressure2) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - mavlink_msg_scaled_pressure2_send(chan, scaled_pressure2->time_boot_ms, scaled_pressure2->press_abs, scaled_pressure2->press_diff, scaled_pressure2->temperature); + mavlink_msg_scaled_pressure2_send(chan, scaled_pressure2->time_boot_ms, scaled_pressure2->press_abs, scaled_pressure2->press_diff, scaled_pressure2->temperature, scaled_pressure2->temperature_press_diff); #else _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SCALED_PRESSURE2, (const char *)scaled_pressure2, MAVLINK_MSG_ID_SCALED_PRESSURE2_MIN_LEN, MAVLINK_MSG_ID_SCALED_PRESSURE2_LEN, MAVLINK_MSG_ID_SCALED_PRESSURE2_CRC); #endif @@ -200,7 +212,7 @@ static inline void mavlink_msg_scaled_pressure2_send_struct(mavlink_channel_t ch is usually the receive buffer for the channel, and allows a reply to an incoming message with minimum stack space usage. */ -static inline void mavlink_msg_scaled_pressure2_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint32_t time_boot_ms, float press_abs, float press_diff, int16_t temperature) +static inline void mavlink_msg_scaled_pressure2_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint32_t time_boot_ms, float press_abs, float press_diff, int16_t temperature, int16_t temperature_press_diff) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char *buf = (char *)msgbuf; @@ -208,6 +220,7 @@ static inline void mavlink_msg_scaled_pressure2_send_buf(mavlink_message_t *msgb _mav_put_float(buf, 4, press_abs); _mav_put_float(buf, 8, press_diff); _mav_put_int16_t(buf, 12, temperature); + _mav_put_int16_t(buf, 14, temperature_press_diff); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SCALED_PRESSURE2, buf, MAVLINK_MSG_ID_SCALED_PRESSURE2_MIN_LEN, MAVLINK_MSG_ID_SCALED_PRESSURE2_LEN, MAVLINK_MSG_ID_SCALED_PRESSURE2_CRC); #else @@ -216,6 +229,7 @@ static inline void mavlink_msg_scaled_pressure2_send_buf(mavlink_message_t *msgb packet->press_abs = press_abs; packet->press_diff = press_diff; packet->temperature = temperature; + packet->temperature_press_diff = temperature_press_diff; _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SCALED_PRESSURE2, (const char *)packet, MAVLINK_MSG_ID_SCALED_PRESSURE2_MIN_LEN, MAVLINK_MSG_ID_SCALED_PRESSURE2_LEN, MAVLINK_MSG_ID_SCALED_PRESSURE2_CRC); #endif @@ -267,6 +281,16 @@ static inline int16_t mavlink_msg_scaled_pressure2_get_temperature(const mavlink return _MAV_RETURN_int16_t(msg, 12); } +/** + * @brief Get field temperature_press_diff from scaled_pressure2 message + * + * @return [cdegC] Differential pressure temperature (0, if not available). Report values of 0 (or 1) as 1 cdegC. + */ +static inline int16_t mavlink_msg_scaled_pressure2_get_temperature_press_diff(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int16_t(msg, 14); +} + /** * @brief Decode a scaled_pressure2 message into a struct * @@ -280,6 +304,7 @@ static inline void mavlink_msg_scaled_pressure2_decode(const mavlink_message_t* scaled_pressure2->press_abs = mavlink_msg_scaled_pressure2_get_press_abs(msg); scaled_pressure2->press_diff = mavlink_msg_scaled_pressure2_get_press_diff(msg); scaled_pressure2->temperature = mavlink_msg_scaled_pressure2_get_temperature(msg); + scaled_pressure2->temperature_press_diff = mavlink_msg_scaled_pressure2_get_temperature_press_diff(msg); #else uint8_t len = msg->len < MAVLINK_MSG_ID_SCALED_PRESSURE2_LEN? msg->len : MAVLINK_MSG_ID_SCALED_PRESSURE2_LEN; memset(scaled_pressure2, 0, MAVLINK_MSG_ID_SCALED_PRESSURE2_LEN); diff --git a/lib/main/MAVLink/common/mavlink_msg_scaled_pressure3.h b/lib/main/MAVLink/common/mavlink_msg_scaled_pressure3.h index e0007cf8ed..34368b1efb 100755 --- a/lib/main/MAVLink/common/mavlink_msg_scaled_pressure3.h +++ b/lib/main/MAVLink/common/mavlink_msg_scaled_pressure3.h @@ -9,11 +9,12 @@ typedef struct __mavlink_scaled_pressure3_t { float press_abs; /*< [hPa] Absolute pressure*/ float press_diff; /*< [hPa] Differential pressure*/ int16_t temperature; /*< [cdegC] Absolute pressure temperature*/ + int16_t temperature_press_diff; /*< [cdegC] Differential pressure temperature (0, if not available). Report values of 0 (or 1) as 1 cdegC.*/ } mavlink_scaled_pressure3_t; -#define MAVLINK_MSG_ID_SCALED_PRESSURE3_LEN 14 +#define MAVLINK_MSG_ID_SCALED_PRESSURE3_LEN 16 #define MAVLINK_MSG_ID_SCALED_PRESSURE3_MIN_LEN 14 -#define MAVLINK_MSG_ID_143_LEN 14 +#define MAVLINK_MSG_ID_143_LEN 16 #define MAVLINK_MSG_ID_143_MIN_LEN 14 #define MAVLINK_MSG_ID_SCALED_PRESSURE3_CRC 131 @@ -25,21 +26,23 @@ typedef struct __mavlink_scaled_pressure3_t { #define MAVLINK_MESSAGE_INFO_SCALED_PRESSURE3 { \ 143, \ "SCALED_PRESSURE3", \ - 4, \ + 5, \ { { "time_boot_ms", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_scaled_pressure3_t, time_boot_ms) }, \ { "press_abs", NULL, MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_scaled_pressure3_t, press_abs) }, \ { "press_diff", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_scaled_pressure3_t, press_diff) }, \ { "temperature", NULL, MAVLINK_TYPE_INT16_T, 0, 12, offsetof(mavlink_scaled_pressure3_t, temperature) }, \ + { "temperature_press_diff", NULL, MAVLINK_TYPE_INT16_T, 0, 14, offsetof(mavlink_scaled_pressure3_t, temperature_press_diff) }, \ } \ } #else #define MAVLINK_MESSAGE_INFO_SCALED_PRESSURE3 { \ "SCALED_PRESSURE3", \ - 4, \ + 5, \ { { "time_boot_ms", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_scaled_pressure3_t, time_boot_ms) }, \ { "press_abs", NULL, MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_scaled_pressure3_t, press_abs) }, \ { "press_diff", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_scaled_pressure3_t, press_diff) }, \ { "temperature", NULL, MAVLINK_TYPE_INT16_T, 0, 12, offsetof(mavlink_scaled_pressure3_t, temperature) }, \ + { "temperature_press_diff", NULL, MAVLINK_TYPE_INT16_T, 0, 14, offsetof(mavlink_scaled_pressure3_t, temperature_press_diff) }, \ } \ } #endif @@ -54,10 +57,11 @@ typedef struct __mavlink_scaled_pressure3_t { * @param press_abs [hPa] Absolute pressure * @param press_diff [hPa] Differential pressure * @param temperature [cdegC] Absolute pressure temperature + * @param temperature_press_diff [cdegC] Differential pressure temperature (0, if not available). Report values of 0 (or 1) as 1 cdegC. * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_scaled_pressure3_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint32_t time_boot_ms, float press_abs, float press_diff, int16_t temperature) + uint32_t time_boot_ms, float press_abs, float press_diff, int16_t temperature, int16_t temperature_press_diff) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_SCALED_PRESSURE3_LEN]; @@ -65,6 +69,7 @@ static inline uint16_t mavlink_msg_scaled_pressure3_pack(uint8_t system_id, uint _mav_put_float(buf, 4, press_abs); _mav_put_float(buf, 8, press_diff); _mav_put_int16_t(buf, 12, temperature); + _mav_put_int16_t(buf, 14, temperature_press_diff); memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SCALED_PRESSURE3_LEN); #else @@ -73,6 +78,7 @@ static inline uint16_t mavlink_msg_scaled_pressure3_pack(uint8_t system_id, uint packet.press_abs = press_abs; packet.press_diff = press_diff; packet.temperature = temperature; + packet.temperature_press_diff = temperature_press_diff; memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SCALED_PRESSURE3_LEN); #endif @@ -91,11 +97,12 @@ static inline uint16_t mavlink_msg_scaled_pressure3_pack(uint8_t system_id, uint * @param press_abs [hPa] Absolute pressure * @param press_diff [hPa] Differential pressure * @param temperature [cdegC] Absolute pressure temperature + * @param temperature_press_diff [cdegC] Differential pressure temperature (0, if not available). Report values of 0 (or 1) as 1 cdegC. * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_scaled_pressure3_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, - uint32_t time_boot_ms,float press_abs,float press_diff,int16_t temperature) + uint32_t time_boot_ms,float press_abs,float press_diff,int16_t temperature,int16_t temperature_press_diff) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_SCALED_PRESSURE3_LEN]; @@ -103,6 +110,7 @@ static inline uint16_t mavlink_msg_scaled_pressure3_pack_chan(uint8_t system_id, _mav_put_float(buf, 4, press_abs); _mav_put_float(buf, 8, press_diff); _mav_put_int16_t(buf, 12, temperature); + _mav_put_int16_t(buf, 14, temperature_press_diff); memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SCALED_PRESSURE3_LEN); #else @@ -111,6 +119,7 @@ static inline uint16_t mavlink_msg_scaled_pressure3_pack_chan(uint8_t system_id, packet.press_abs = press_abs; packet.press_diff = press_diff; packet.temperature = temperature; + packet.temperature_press_diff = temperature_press_diff; memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SCALED_PRESSURE3_LEN); #endif @@ -129,7 +138,7 @@ static inline uint16_t mavlink_msg_scaled_pressure3_pack_chan(uint8_t system_id, */ static inline uint16_t mavlink_msg_scaled_pressure3_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_scaled_pressure3_t* scaled_pressure3) { - return mavlink_msg_scaled_pressure3_pack(system_id, component_id, msg, scaled_pressure3->time_boot_ms, scaled_pressure3->press_abs, scaled_pressure3->press_diff, scaled_pressure3->temperature); + return mavlink_msg_scaled_pressure3_pack(system_id, component_id, msg, scaled_pressure3->time_boot_ms, scaled_pressure3->press_abs, scaled_pressure3->press_diff, scaled_pressure3->temperature, scaled_pressure3->temperature_press_diff); } /** @@ -143,7 +152,7 @@ static inline uint16_t mavlink_msg_scaled_pressure3_encode(uint8_t system_id, ui */ static inline uint16_t mavlink_msg_scaled_pressure3_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_scaled_pressure3_t* scaled_pressure3) { - return mavlink_msg_scaled_pressure3_pack_chan(system_id, component_id, chan, msg, scaled_pressure3->time_boot_ms, scaled_pressure3->press_abs, scaled_pressure3->press_diff, scaled_pressure3->temperature); + return mavlink_msg_scaled_pressure3_pack_chan(system_id, component_id, chan, msg, scaled_pressure3->time_boot_ms, scaled_pressure3->press_abs, scaled_pressure3->press_diff, scaled_pressure3->temperature, scaled_pressure3->temperature_press_diff); } /** @@ -154,10 +163,11 @@ static inline uint16_t mavlink_msg_scaled_pressure3_encode_chan(uint8_t system_i * @param press_abs [hPa] Absolute pressure * @param press_diff [hPa] Differential pressure * @param temperature [cdegC] Absolute pressure temperature + * @param temperature_press_diff [cdegC] Differential pressure temperature (0, if not available). Report values of 0 (or 1) as 1 cdegC. */ #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS -static inline void mavlink_msg_scaled_pressure3_send(mavlink_channel_t chan, uint32_t time_boot_ms, float press_abs, float press_diff, int16_t temperature) +static inline void mavlink_msg_scaled_pressure3_send(mavlink_channel_t chan, uint32_t time_boot_ms, float press_abs, float press_diff, int16_t temperature, int16_t temperature_press_diff) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_SCALED_PRESSURE3_LEN]; @@ -165,6 +175,7 @@ static inline void mavlink_msg_scaled_pressure3_send(mavlink_channel_t chan, uin _mav_put_float(buf, 4, press_abs); _mav_put_float(buf, 8, press_diff); _mav_put_int16_t(buf, 12, temperature); + _mav_put_int16_t(buf, 14, temperature_press_diff); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SCALED_PRESSURE3, buf, MAVLINK_MSG_ID_SCALED_PRESSURE3_MIN_LEN, MAVLINK_MSG_ID_SCALED_PRESSURE3_LEN, MAVLINK_MSG_ID_SCALED_PRESSURE3_CRC); #else @@ -173,6 +184,7 @@ static inline void mavlink_msg_scaled_pressure3_send(mavlink_channel_t chan, uin packet.press_abs = press_abs; packet.press_diff = press_diff; packet.temperature = temperature; + packet.temperature_press_diff = temperature_press_diff; _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SCALED_PRESSURE3, (const char *)&packet, MAVLINK_MSG_ID_SCALED_PRESSURE3_MIN_LEN, MAVLINK_MSG_ID_SCALED_PRESSURE3_LEN, MAVLINK_MSG_ID_SCALED_PRESSURE3_CRC); #endif @@ -186,7 +198,7 @@ static inline void mavlink_msg_scaled_pressure3_send(mavlink_channel_t chan, uin static inline void mavlink_msg_scaled_pressure3_send_struct(mavlink_channel_t chan, const mavlink_scaled_pressure3_t* scaled_pressure3) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - mavlink_msg_scaled_pressure3_send(chan, scaled_pressure3->time_boot_ms, scaled_pressure3->press_abs, scaled_pressure3->press_diff, scaled_pressure3->temperature); + mavlink_msg_scaled_pressure3_send(chan, scaled_pressure3->time_boot_ms, scaled_pressure3->press_abs, scaled_pressure3->press_diff, scaled_pressure3->temperature, scaled_pressure3->temperature_press_diff); #else _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SCALED_PRESSURE3, (const char *)scaled_pressure3, MAVLINK_MSG_ID_SCALED_PRESSURE3_MIN_LEN, MAVLINK_MSG_ID_SCALED_PRESSURE3_LEN, MAVLINK_MSG_ID_SCALED_PRESSURE3_CRC); #endif @@ -200,7 +212,7 @@ static inline void mavlink_msg_scaled_pressure3_send_struct(mavlink_channel_t ch is usually the receive buffer for the channel, and allows a reply to an incoming message with minimum stack space usage. */ -static inline void mavlink_msg_scaled_pressure3_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint32_t time_boot_ms, float press_abs, float press_diff, int16_t temperature) +static inline void mavlink_msg_scaled_pressure3_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint32_t time_boot_ms, float press_abs, float press_diff, int16_t temperature, int16_t temperature_press_diff) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char *buf = (char *)msgbuf; @@ -208,6 +220,7 @@ static inline void mavlink_msg_scaled_pressure3_send_buf(mavlink_message_t *msgb _mav_put_float(buf, 4, press_abs); _mav_put_float(buf, 8, press_diff); _mav_put_int16_t(buf, 12, temperature); + _mav_put_int16_t(buf, 14, temperature_press_diff); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SCALED_PRESSURE3, buf, MAVLINK_MSG_ID_SCALED_PRESSURE3_MIN_LEN, MAVLINK_MSG_ID_SCALED_PRESSURE3_LEN, MAVLINK_MSG_ID_SCALED_PRESSURE3_CRC); #else @@ -216,6 +229,7 @@ static inline void mavlink_msg_scaled_pressure3_send_buf(mavlink_message_t *msgb packet->press_abs = press_abs; packet->press_diff = press_diff; packet->temperature = temperature; + packet->temperature_press_diff = temperature_press_diff; _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SCALED_PRESSURE3, (const char *)packet, MAVLINK_MSG_ID_SCALED_PRESSURE3_MIN_LEN, MAVLINK_MSG_ID_SCALED_PRESSURE3_LEN, MAVLINK_MSG_ID_SCALED_PRESSURE3_CRC); #endif @@ -267,6 +281,16 @@ static inline int16_t mavlink_msg_scaled_pressure3_get_temperature(const mavlink return _MAV_RETURN_int16_t(msg, 12); } +/** + * @brief Get field temperature_press_diff from scaled_pressure3 message + * + * @return [cdegC] Differential pressure temperature (0, if not available). Report values of 0 (or 1) as 1 cdegC. + */ +static inline int16_t mavlink_msg_scaled_pressure3_get_temperature_press_diff(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int16_t(msg, 14); +} + /** * @brief Decode a scaled_pressure3 message into a struct * @@ -280,6 +304,7 @@ static inline void mavlink_msg_scaled_pressure3_decode(const mavlink_message_t* scaled_pressure3->press_abs = mavlink_msg_scaled_pressure3_get_press_abs(msg); scaled_pressure3->press_diff = mavlink_msg_scaled_pressure3_get_press_diff(msg); scaled_pressure3->temperature = mavlink_msg_scaled_pressure3_get_temperature(msg); + scaled_pressure3->temperature_press_diff = mavlink_msg_scaled_pressure3_get_temperature_press_diff(msg); #else uint8_t len = msg->len < MAVLINK_MSG_ID_SCALED_PRESSURE3_LEN? msg->len : MAVLINK_MSG_ID_SCALED_PRESSURE3_LEN; memset(scaled_pressure3, 0, MAVLINK_MSG_ID_SCALED_PRESSURE3_LEN); diff --git a/lib/main/MAVLink/common/mavlink_msg_servo_output_raw.h b/lib/main/MAVLink/common/mavlink_msg_servo_output_raw.h index 840c6a25f6..4b76d20a8f 100755 --- a/lib/main/MAVLink/common/mavlink_msg_servo_output_raw.h +++ b/lib/main/MAVLink/common/mavlink_msg_servo_output_raw.h @@ -3,7 +3,7 @@ #define MAVLINK_MSG_ID_SERVO_OUTPUT_RAW 36 - +MAVPACKED( typedef struct __mavlink_servo_output_raw_t { uint32_t time_usec; /*< [us] Timestamp (UNIX Epoch time or time since system boot). The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude of the number.*/ uint16_t servo1_raw; /*< [us] Servo output 1 value*/ @@ -15,11 +15,19 @@ typedef struct __mavlink_servo_output_raw_t { uint16_t servo7_raw; /*< [us] Servo output 7 value*/ uint16_t servo8_raw; /*< [us] Servo output 8 value*/ uint8_t port; /*< Servo output port (set of 8 outputs = 1 port). Flight stacks running on Pixhawk should use: 0 = MAIN, 1 = AUX.*/ -} mavlink_servo_output_raw_t; + uint16_t servo9_raw; /*< [us] Servo output 9 value*/ + uint16_t servo10_raw; /*< [us] Servo output 10 value*/ + uint16_t servo11_raw; /*< [us] Servo output 11 value*/ + uint16_t servo12_raw; /*< [us] Servo output 12 value*/ + uint16_t servo13_raw; /*< [us] Servo output 13 value*/ + uint16_t servo14_raw; /*< [us] Servo output 14 value*/ + uint16_t servo15_raw; /*< [us] Servo output 15 value*/ + uint16_t servo16_raw; /*< [us] Servo output 16 value*/ +}) mavlink_servo_output_raw_t; -#define MAVLINK_MSG_ID_SERVO_OUTPUT_RAW_LEN 21 +#define MAVLINK_MSG_ID_SERVO_OUTPUT_RAW_LEN 37 #define MAVLINK_MSG_ID_SERVO_OUTPUT_RAW_MIN_LEN 21 -#define MAVLINK_MSG_ID_36_LEN 21 +#define MAVLINK_MSG_ID_36_LEN 37 #define MAVLINK_MSG_ID_36_MIN_LEN 21 #define MAVLINK_MSG_ID_SERVO_OUTPUT_RAW_CRC 222 @@ -31,7 +39,7 @@ typedef struct __mavlink_servo_output_raw_t { #define MAVLINK_MESSAGE_INFO_SERVO_OUTPUT_RAW { \ 36, \ "SERVO_OUTPUT_RAW", \ - 10, \ + 18, \ { { "time_usec", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_servo_output_raw_t, time_usec) }, \ { "port", NULL, MAVLINK_TYPE_UINT8_T, 0, 20, offsetof(mavlink_servo_output_raw_t, port) }, \ { "servo1_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 4, offsetof(mavlink_servo_output_raw_t, servo1_raw) }, \ @@ -42,12 +50,20 @@ typedef struct __mavlink_servo_output_raw_t { { "servo6_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 14, offsetof(mavlink_servo_output_raw_t, servo6_raw) }, \ { "servo7_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 16, offsetof(mavlink_servo_output_raw_t, servo7_raw) }, \ { "servo8_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 18, offsetof(mavlink_servo_output_raw_t, servo8_raw) }, \ + { "servo9_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 21, offsetof(mavlink_servo_output_raw_t, servo9_raw) }, \ + { "servo10_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 23, offsetof(mavlink_servo_output_raw_t, servo10_raw) }, \ + { "servo11_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 25, offsetof(mavlink_servo_output_raw_t, servo11_raw) }, \ + { "servo12_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 27, offsetof(mavlink_servo_output_raw_t, servo12_raw) }, \ + { "servo13_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 29, offsetof(mavlink_servo_output_raw_t, servo13_raw) }, \ + { "servo14_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 31, offsetof(mavlink_servo_output_raw_t, servo14_raw) }, \ + { "servo15_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 33, offsetof(mavlink_servo_output_raw_t, servo15_raw) }, \ + { "servo16_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 35, offsetof(mavlink_servo_output_raw_t, servo16_raw) }, \ } \ } #else #define MAVLINK_MESSAGE_INFO_SERVO_OUTPUT_RAW { \ "SERVO_OUTPUT_RAW", \ - 10, \ + 18, \ { { "time_usec", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_servo_output_raw_t, time_usec) }, \ { "port", NULL, MAVLINK_TYPE_UINT8_T, 0, 20, offsetof(mavlink_servo_output_raw_t, port) }, \ { "servo1_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 4, offsetof(mavlink_servo_output_raw_t, servo1_raw) }, \ @@ -58,6 +74,14 @@ typedef struct __mavlink_servo_output_raw_t { { "servo6_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 14, offsetof(mavlink_servo_output_raw_t, servo6_raw) }, \ { "servo7_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 16, offsetof(mavlink_servo_output_raw_t, servo7_raw) }, \ { "servo8_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 18, offsetof(mavlink_servo_output_raw_t, servo8_raw) }, \ + { "servo9_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 21, offsetof(mavlink_servo_output_raw_t, servo9_raw) }, \ + { "servo10_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 23, offsetof(mavlink_servo_output_raw_t, servo10_raw) }, \ + { "servo11_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 25, offsetof(mavlink_servo_output_raw_t, servo11_raw) }, \ + { "servo12_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 27, offsetof(mavlink_servo_output_raw_t, servo12_raw) }, \ + { "servo13_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 29, offsetof(mavlink_servo_output_raw_t, servo13_raw) }, \ + { "servo14_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 31, offsetof(mavlink_servo_output_raw_t, servo14_raw) }, \ + { "servo15_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 33, offsetof(mavlink_servo_output_raw_t, servo15_raw) }, \ + { "servo16_raw", NULL, MAVLINK_TYPE_UINT16_T, 0, 35, offsetof(mavlink_servo_output_raw_t, servo16_raw) }, \ } \ } #endif @@ -78,10 +102,18 @@ typedef struct __mavlink_servo_output_raw_t { * @param servo6_raw [us] Servo output 6 value * @param servo7_raw [us] Servo output 7 value * @param servo8_raw [us] Servo output 8 value + * @param servo9_raw [us] Servo output 9 value + * @param servo10_raw [us] Servo output 10 value + * @param servo11_raw [us] Servo output 11 value + * @param servo12_raw [us] Servo output 12 value + * @param servo13_raw [us] Servo output 13 value + * @param servo14_raw [us] Servo output 14 value + * @param servo15_raw [us] Servo output 15 value + * @param servo16_raw [us] Servo output 16 value * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_servo_output_raw_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint32_t time_usec, uint8_t port, uint16_t servo1_raw, uint16_t servo2_raw, uint16_t servo3_raw, uint16_t servo4_raw, uint16_t servo5_raw, uint16_t servo6_raw, uint16_t servo7_raw, uint16_t servo8_raw) + uint32_t time_usec, uint8_t port, uint16_t servo1_raw, uint16_t servo2_raw, uint16_t servo3_raw, uint16_t servo4_raw, uint16_t servo5_raw, uint16_t servo6_raw, uint16_t servo7_raw, uint16_t servo8_raw, uint16_t servo9_raw, uint16_t servo10_raw, uint16_t servo11_raw, uint16_t servo12_raw, uint16_t servo13_raw, uint16_t servo14_raw, uint16_t servo15_raw, uint16_t servo16_raw) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_SERVO_OUTPUT_RAW_LEN]; @@ -95,6 +127,14 @@ static inline uint16_t mavlink_msg_servo_output_raw_pack(uint8_t system_id, uint _mav_put_uint16_t(buf, 16, servo7_raw); _mav_put_uint16_t(buf, 18, servo8_raw); _mav_put_uint8_t(buf, 20, port); + _mav_put_uint16_t(buf, 21, servo9_raw); + _mav_put_uint16_t(buf, 23, servo10_raw); + _mav_put_uint16_t(buf, 25, servo11_raw); + _mav_put_uint16_t(buf, 27, servo12_raw); + _mav_put_uint16_t(buf, 29, servo13_raw); + _mav_put_uint16_t(buf, 31, servo14_raw); + _mav_put_uint16_t(buf, 33, servo15_raw); + _mav_put_uint16_t(buf, 35, servo16_raw); memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW_LEN); #else @@ -109,6 +149,14 @@ static inline uint16_t mavlink_msg_servo_output_raw_pack(uint8_t system_id, uint packet.servo7_raw = servo7_raw; packet.servo8_raw = servo8_raw; packet.port = port; + packet.servo9_raw = servo9_raw; + packet.servo10_raw = servo10_raw; + packet.servo11_raw = servo11_raw; + packet.servo12_raw = servo12_raw; + packet.servo13_raw = servo13_raw; + packet.servo14_raw = servo14_raw; + packet.servo15_raw = servo15_raw; + packet.servo16_raw = servo16_raw; memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW_LEN); #endif @@ -133,11 +181,19 @@ static inline uint16_t mavlink_msg_servo_output_raw_pack(uint8_t system_id, uint * @param servo6_raw [us] Servo output 6 value * @param servo7_raw [us] Servo output 7 value * @param servo8_raw [us] Servo output 8 value + * @param servo9_raw [us] Servo output 9 value + * @param servo10_raw [us] Servo output 10 value + * @param servo11_raw [us] Servo output 11 value + * @param servo12_raw [us] Servo output 12 value + * @param servo13_raw [us] Servo output 13 value + * @param servo14_raw [us] Servo output 14 value + * @param servo15_raw [us] Servo output 15 value + * @param servo16_raw [us] Servo output 16 value * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_servo_output_raw_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, - uint32_t time_usec,uint8_t port,uint16_t servo1_raw,uint16_t servo2_raw,uint16_t servo3_raw,uint16_t servo4_raw,uint16_t servo5_raw,uint16_t servo6_raw,uint16_t servo7_raw,uint16_t servo8_raw) + uint32_t time_usec,uint8_t port,uint16_t servo1_raw,uint16_t servo2_raw,uint16_t servo3_raw,uint16_t servo4_raw,uint16_t servo5_raw,uint16_t servo6_raw,uint16_t servo7_raw,uint16_t servo8_raw,uint16_t servo9_raw,uint16_t servo10_raw,uint16_t servo11_raw,uint16_t servo12_raw,uint16_t servo13_raw,uint16_t servo14_raw,uint16_t servo15_raw,uint16_t servo16_raw) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_SERVO_OUTPUT_RAW_LEN]; @@ -151,6 +207,14 @@ static inline uint16_t mavlink_msg_servo_output_raw_pack_chan(uint8_t system_id, _mav_put_uint16_t(buf, 16, servo7_raw); _mav_put_uint16_t(buf, 18, servo8_raw); _mav_put_uint8_t(buf, 20, port); + _mav_put_uint16_t(buf, 21, servo9_raw); + _mav_put_uint16_t(buf, 23, servo10_raw); + _mav_put_uint16_t(buf, 25, servo11_raw); + _mav_put_uint16_t(buf, 27, servo12_raw); + _mav_put_uint16_t(buf, 29, servo13_raw); + _mav_put_uint16_t(buf, 31, servo14_raw); + _mav_put_uint16_t(buf, 33, servo15_raw); + _mav_put_uint16_t(buf, 35, servo16_raw); memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW_LEN); #else @@ -165,6 +229,14 @@ static inline uint16_t mavlink_msg_servo_output_raw_pack_chan(uint8_t system_id, packet.servo7_raw = servo7_raw; packet.servo8_raw = servo8_raw; packet.port = port; + packet.servo9_raw = servo9_raw; + packet.servo10_raw = servo10_raw; + packet.servo11_raw = servo11_raw; + packet.servo12_raw = servo12_raw; + packet.servo13_raw = servo13_raw; + packet.servo14_raw = servo14_raw; + packet.servo15_raw = servo15_raw; + packet.servo16_raw = servo16_raw; memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW_LEN); #endif @@ -183,7 +255,7 @@ static inline uint16_t mavlink_msg_servo_output_raw_pack_chan(uint8_t system_id, */ static inline uint16_t mavlink_msg_servo_output_raw_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_servo_output_raw_t* servo_output_raw) { - return mavlink_msg_servo_output_raw_pack(system_id, component_id, msg, servo_output_raw->time_usec, servo_output_raw->port, servo_output_raw->servo1_raw, servo_output_raw->servo2_raw, servo_output_raw->servo3_raw, servo_output_raw->servo4_raw, servo_output_raw->servo5_raw, servo_output_raw->servo6_raw, servo_output_raw->servo7_raw, servo_output_raw->servo8_raw); + return mavlink_msg_servo_output_raw_pack(system_id, component_id, msg, servo_output_raw->time_usec, servo_output_raw->port, servo_output_raw->servo1_raw, servo_output_raw->servo2_raw, servo_output_raw->servo3_raw, servo_output_raw->servo4_raw, servo_output_raw->servo5_raw, servo_output_raw->servo6_raw, servo_output_raw->servo7_raw, servo_output_raw->servo8_raw, servo_output_raw->servo9_raw, servo_output_raw->servo10_raw, servo_output_raw->servo11_raw, servo_output_raw->servo12_raw, servo_output_raw->servo13_raw, servo_output_raw->servo14_raw, servo_output_raw->servo15_raw, servo_output_raw->servo16_raw); } /** @@ -197,7 +269,7 @@ static inline uint16_t mavlink_msg_servo_output_raw_encode(uint8_t system_id, ui */ static inline uint16_t mavlink_msg_servo_output_raw_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_servo_output_raw_t* servo_output_raw) { - return mavlink_msg_servo_output_raw_pack_chan(system_id, component_id, chan, msg, servo_output_raw->time_usec, servo_output_raw->port, servo_output_raw->servo1_raw, servo_output_raw->servo2_raw, servo_output_raw->servo3_raw, servo_output_raw->servo4_raw, servo_output_raw->servo5_raw, servo_output_raw->servo6_raw, servo_output_raw->servo7_raw, servo_output_raw->servo8_raw); + return mavlink_msg_servo_output_raw_pack_chan(system_id, component_id, chan, msg, servo_output_raw->time_usec, servo_output_raw->port, servo_output_raw->servo1_raw, servo_output_raw->servo2_raw, servo_output_raw->servo3_raw, servo_output_raw->servo4_raw, servo_output_raw->servo5_raw, servo_output_raw->servo6_raw, servo_output_raw->servo7_raw, servo_output_raw->servo8_raw, servo_output_raw->servo9_raw, servo_output_raw->servo10_raw, servo_output_raw->servo11_raw, servo_output_raw->servo12_raw, servo_output_raw->servo13_raw, servo_output_raw->servo14_raw, servo_output_raw->servo15_raw, servo_output_raw->servo16_raw); } /** @@ -214,10 +286,18 @@ static inline uint16_t mavlink_msg_servo_output_raw_encode_chan(uint8_t system_i * @param servo6_raw [us] Servo output 6 value * @param servo7_raw [us] Servo output 7 value * @param servo8_raw [us] Servo output 8 value + * @param servo9_raw [us] Servo output 9 value + * @param servo10_raw [us] Servo output 10 value + * @param servo11_raw [us] Servo output 11 value + * @param servo12_raw [us] Servo output 12 value + * @param servo13_raw [us] Servo output 13 value + * @param servo14_raw [us] Servo output 14 value + * @param servo15_raw [us] Servo output 15 value + * @param servo16_raw [us] Servo output 16 value */ #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS -static inline void mavlink_msg_servo_output_raw_send(mavlink_channel_t chan, uint32_t time_usec, uint8_t port, uint16_t servo1_raw, uint16_t servo2_raw, uint16_t servo3_raw, uint16_t servo4_raw, uint16_t servo5_raw, uint16_t servo6_raw, uint16_t servo7_raw, uint16_t servo8_raw) +static inline void mavlink_msg_servo_output_raw_send(mavlink_channel_t chan, uint32_t time_usec, uint8_t port, uint16_t servo1_raw, uint16_t servo2_raw, uint16_t servo3_raw, uint16_t servo4_raw, uint16_t servo5_raw, uint16_t servo6_raw, uint16_t servo7_raw, uint16_t servo8_raw, uint16_t servo9_raw, uint16_t servo10_raw, uint16_t servo11_raw, uint16_t servo12_raw, uint16_t servo13_raw, uint16_t servo14_raw, uint16_t servo15_raw, uint16_t servo16_raw) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_SERVO_OUTPUT_RAW_LEN]; @@ -231,6 +311,14 @@ static inline void mavlink_msg_servo_output_raw_send(mavlink_channel_t chan, uin _mav_put_uint16_t(buf, 16, servo7_raw); _mav_put_uint16_t(buf, 18, servo8_raw); _mav_put_uint8_t(buf, 20, port); + _mav_put_uint16_t(buf, 21, servo9_raw); + _mav_put_uint16_t(buf, 23, servo10_raw); + _mav_put_uint16_t(buf, 25, servo11_raw); + _mav_put_uint16_t(buf, 27, servo12_raw); + _mav_put_uint16_t(buf, 29, servo13_raw); + _mav_put_uint16_t(buf, 31, servo14_raw); + _mav_put_uint16_t(buf, 33, servo15_raw); + _mav_put_uint16_t(buf, 35, servo16_raw); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, buf, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW_MIN_LEN, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW_LEN, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW_CRC); #else @@ -245,6 +333,14 @@ static inline void mavlink_msg_servo_output_raw_send(mavlink_channel_t chan, uin packet.servo7_raw = servo7_raw; packet.servo8_raw = servo8_raw; packet.port = port; + packet.servo9_raw = servo9_raw; + packet.servo10_raw = servo10_raw; + packet.servo11_raw = servo11_raw; + packet.servo12_raw = servo12_raw; + packet.servo13_raw = servo13_raw; + packet.servo14_raw = servo14_raw; + packet.servo15_raw = servo15_raw; + packet.servo16_raw = servo16_raw; _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, (const char *)&packet, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW_MIN_LEN, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW_LEN, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW_CRC); #endif @@ -258,7 +354,7 @@ static inline void mavlink_msg_servo_output_raw_send(mavlink_channel_t chan, uin static inline void mavlink_msg_servo_output_raw_send_struct(mavlink_channel_t chan, const mavlink_servo_output_raw_t* servo_output_raw) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - mavlink_msg_servo_output_raw_send(chan, servo_output_raw->time_usec, servo_output_raw->port, servo_output_raw->servo1_raw, servo_output_raw->servo2_raw, servo_output_raw->servo3_raw, servo_output_raw->servo4_raw, servo_output_raw->servo5_raw, servo_output_raw->servo6_raw, servo_output_raw->servo7_raw, servo_output_raw->servo8_raw); + mavlink_msg_servo_output_raw_send(chan, servo_output_raw->time_usec, servo_output_raw->port, servo_output_raw->servo1_raw, servo_output_raw->servo2_raw, servo_output_raw->servo3_raw, servo_output_raw->servo4_raw, servo_output_raw->servo5_raw, servo_output_raw->servo6_raw, servo_output_raw->servo7_raw, servo_output_raw->servo8_raw, servo_output_raw->servo9_raw, servo_output_raw->servo10_raw, servo_output_raw->servo11_raw, servo_output_raw->servo12_raw, servo_output_raw->servo13_raw, servo_output_raw->servo14_raw, servo_output_raw->servo15_raw, servo_output_raw->servo16_raw); #else _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, (const char *)servo_output_raw, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW_MIN_LEN, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW_LEN, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW_CRC); #endif @@ -272,7 +368,7 @@ static inline void mavlink_msg_servo_output_raw_send_struct(mavlink_channel_t ch is usually the receive buffer for the channel, and allows a reply to an incoming message with minimum stack space usage. */ -static inline void mavlink_msg_servo_output_raw_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint32_t time_usec, uint8_t port, uint16_t servo1_raw, uint16_t servo2_raw, uint16_t servo3_raw, uint16_t servo4_raw, uint16_t servo5_raw, uint16_t servo6_raw, uint16_t servo7_raw, uint16_t servo8_raw) +static inline void mavlink_msg_servo_output_raw_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint32_t time_usec, uint8_t port, uint16_t servo1_raw, uint16_t servo2_raw, uint16_t servo3_raw, uint16_t servo4_raw, uint16_t servo5_raw, uint16_t servo6_raw, uint16_t servo7_raw, uint16_t servo8_raw, uint16_t servo9_raw, uint16_t servo10_raw, uint16_t servo11_raw, uint16_t servo12_raw, uint16_t servo13_raw, uint16_t servo14_raw, uint16_t servo15_raw, uint16_t servo16_raw) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char *buf = (char *)msgbuf; @@ -286,6 +382,14 @@ static inline void mavlink_msg_servo_output_raw_send_buf(mavlink_message_t *msgb _mav_put_uint16_t(buf, 16, servo7_raw); _mav_put_uint16_t(buf, 18, servo8_raw); _mav_put_uint8_t(buf, 20, port); + _mav_put_uint16_t(buf, 21, servo9_raw); + _mav_put_uint16_t(buf, 23, servo10_raw); + _mav_put_uint16_t(buf, 25, servo11_raw); + _mav_put_uint16_t(buf, 27, servo12_raw); + _mav_put_uint16_t(buf, 29, servo13_raw); + _mav_put_uint16_t(buf, 31, servo14_raw); + _mav_put_uint16_t(buf, 33, servo15_raw); + _mav_put_uint16_t(buf, 35, servo16_raw); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, buf, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW_MIN_LEN, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW_LEN, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW_CRC); #else @@ -300,6 +404,14 @@ static inline void mavlink_msg_servo_output_raw_send_buf(mavlink_message_t *msgb packet->servo7_raw = servo7_raw; packet->servo8_raw = servo8_raw; packet->port = port; + packet->servo9_raw = servo9_raw; + packet->servo10_raw = servo10_raw; + packet->servo11_raw = servo11_raw; + packet->servo12_raw = servo12_raw; + packet->servo13_raw = servo13_raw; + packet->servo14_raw = servo14_raw; + packet->servo15_raw = servo15_raw; + packet->servo16_raw = servo16_raw; _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW, (const char *)packet, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW_MIN_LEN, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW_LEN, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW_CRC); #endif @@ -411,6 +523,86 @@ static inline uint16_t mavlink_msg_servo_output_raw_get_servo8_raw(const mavlink return _MAV_RETURN_uint16_t(msg, 18); } +/** + * @brief Get field servo9_raw from servo_output_raw message + * + * @return [us] Servo output 9 value + */ +static inline uint16_t mavlink_msg_servo_output_raw_get_servo9_raw(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 21); +} + +/** + * @brief Get field servo10_raw from servo_output_raw message + * + * @return [us] Servo output 10 value + */ +static inline uint16_t mavlink_msg_servo_output_raw_get_servo10_raw(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 23); +} + +/** + * @brief Get field servo11_raw from servo_output_raw message + * + * @return [us] Servo output 11 value + */ +static inline uint16_t mavlink_msg_servo_output_raw_get_servo11_raw(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 25); +} + +/** + * @brief Get field servo12_raw from servo_output_raw message + * + * @return [us] Servo output 12 value + */ +static inline uint16_t mavlink_msg_servo_output_raw_get_servo12_raw(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 27); +} + +/** + * @brief Get field servo13_raw from servo_output_raw message + * + * @return [us] Servo output 13 value + */ +static inline uint16_t mavlink_msg_servo_output_raw_get_servo13_raw(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 29); +} + +/** + * @brief Get field servo14_raw from servo_output_raw message + * + * @return [us] Servo output 14 value + */ +static inline uint16_t mavlink_msg_servo_output_raw_get_servo14_raw(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 31); +} + +/** + * @brief Get field servo15_raw from servo_output_raw message + * + * @return [us] Servo output 15 value + */ +static inline uint16_t mavlink_msg_servo_output_raw_get_servo15_raw(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 33); +} + +/** + * @brief Get field servo16_raw from servo_output_raw message + * + * @return [us] Servo output 16 value + */ +static inline uint16_t mavlink_msg_servo_output_raw_get_servo16_raw(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 35); +} + /** * @brief Decode a servo_output_raw message into a struct * @@ -430,6 +622,14 @@ static inline void mavlink_msg_servo_output_raw_decode(const mavlink_message_t* servo_output_raw->servo7_raw = mavlink_msg_servo_output_raw_get_servo7_raw(msg); servo_output_raw->servo8_raw = mavlink_msg_servo_output_raw_get_servo8_raw(msg); servo_output_raw->port = mavlink_msg_servo_output_raw_get_port(msg); + servo_output_raw->servo9_raw = mavlink_msg_servo_output_raw_get_servo9_raw(msg); + servo_output_raw->servo10_raw = mavlink_msg_servo_output_raw_get_servo10_raw(msg); + servo_output_raw->servo11_raw = mavlink_msg_servo_output_raw_get_servo11_raw(msg); + servo_output_raw->servo12_raw = mavlink_msg_servo_output_raw_get_servo12_raw(msg); + servo_output_raw->servo13_raw = mavlink_msg_servo_output_raw_get_servo13_raw(msg); + servo_output_raw->servo14_raw = mavlink_msg_servo_output_raw_get_servo14_raw(msg); + servo_output_raw->servo15_raw = mavlink_msg_servo_output_raw_get_servo15_raw(msg); + servo_output_raw->servo16_raw = mavlink_msg_servo_output_raw_get_servo16_raw(msg); #else uint8_t len = msg->len < MAVLINK_MSG_ID_SERVO_OUTPUT_RAW_LEN? msg->len : MAVLINK_MSG_ID_SERVO_OUTPUT_RAW_LEN; memset(servo_output_raw, 0, MAVLINK_MSG_ID_SERVO_OUTPUT_RAW_LEN); diff --git a/lib/main/MAVLink/common/mavlink_msg_set_attitude_target.h b/lib/main/MAVLink/common/mavlink_msg_set_attitude_target.h index 94a2e856b8..df1e851ebe 100755 --- a/lib/main/MAVLink/common/mavlink_msg_set_attitude_target.h +++ b/lib/main/MAVLink/common/mavlink_msg_set_attitude_target.h @@ -13,7 +13,7 @@ typedef struct __mavlink_set_attitude_target_t { float thrust; /*< Collective thrust, normalized to 0 .. 1 (-1 .. 1 for vehicles capable of reverse trust)*/ uint8_t target_system; /*< System ID*/ uint8_t target_component; /*< Component ID*/ - uint8_t type_mask; /*< Mappings: If any of these bits are set, the corresponding input should be ignored: bit 1: body roll rate, bit 2: body pitch rate, bit 3: body yaw rate. bit 4-bit 6: reserved, bit 7: throttle, bit 8: attitude*/ + uint8_t type_mask; /*< Bitmap to indicate which dimensions should be ignored by the vehicle.*/ } mavlink_set_attitude_target_t; #define MAVLINK_MSG_ID_SET_ATTITUDE_TARGET_LEN 39 @@ -68,7 +68,7 @@ typedef struct __mavlink_set_attitude_target_t { * @param time_boot_ms [ms] Timestamp (time since system boot). * @param target_system System ID * @param target_component Component ID - * @param type_mask Mappings: If any of these bits are set, the corresponding input should be ignored: bit 1: body roll rate, bit 2: body pitch rate, bit 3: body yaw rate. bit 4-bit 6: reserved, bit 7: throttle, bit 8: attitude + * @param type_mask Bitmap to indicate which dimensions should be ignored by the vehicle. * @param q Attitude quaternion (w, x, y, z order, zero-rotation is 1, 0, 0, 0) * @param body_roll_rate [rad/s] Body roll rate * @param body_pitch_rate [rad/s] Body pitch rate @@ -118,7 +118,7 @@ static inline uint16_t mavlink_msg_set_attitude_target_pack(uint8_t system_id, u * @param time_boot_ms [ms] Timestamp (time since system boot). * @param target_system System ID * @param target_component Component ID - * @param type_mask Mappings: If any of these bits are set, the corresponding input should be ignored: bit 1: body roll rate, bit 2: body pitch rate, bit 3: body yaw rate. bit 4-bit 6: reserved, bit 7: throttle, bit 8: attitude + * @param type_mask Bitmap to indicate which dimensions should be ignored by the vehicle. * @param q Attitude quaternion (w, x, y, z order, zero-rotation is 1, 0, 0, 0) * @param body_roll_rate [rad/s] Body roll rate * @param body_pitch_rate [rad/s] Body pitch rate @@ -194,7 +194,7 @@ static inline uint16_t mavlink_msg_set_attitude_target_encode_chan(uint8_t syste * @param time_boot_ms [ms] Timestamp (time since system boot). * @param target_system System ID * @param target_component Component ID - * @param type_mask Mappings: If any of these bits are set, the corresponding input should be ignored: bit 1: body roll rate, bit 2: body pitch rate, bit 3: body yaw rate. bit 4-bit 6: reserved, bit 7: throttle, bit 8: attitude + * @param type_mask Bitmap to indicate which dimensions should be ignored by the vehicle. * @param q Attitude quaternion (w, x, y, z order, zero-rotation is 1, 0, 0, 0) * @param body_roll_rate [rad/s] Body roll rate * @param body_pitch_rate [rad/s] Body pitch rate @@ -322,7 +322,7 @@ static inline uint8_t mavlink_msg_set_attitude_target_get_target_component(const /** * @brief Get field type_mask from set_attitude_target message * - * @return Mappings: If any of these bits are set, the corresponding input should be ignored: bit 1: body roll rate, bit 2: body pitch rate, bit 3: body yaw rate. bit 4-bit 6: reserved, bit 7: throttle, bit 8: attitude + * @return Bitmap to indicate which dimensions should be ignored by the vehicle. */ static inline uint8_t mavlink_msg_set_attitude_target_get_type_mask(const mavlink_message_t* msg) { diff --git a/lib/main/MAVLink/common/mavlink_msg_set_gps_global_origin.h b/lib/main/MAVLink/common/mavlink_msg_set_gps_global_origin.h index 7da7fbb510..62a2f448f8 100755 --- a/lib/main/MAVLink/common/mavlink_msg_set_gps_global_origin.h +++ b/lib/main/MAVLink/common/mavlink_msg_set_gps_global_origin.h @@ -3,17 +3,18 @@ #define MAVLINK_MSG_ID_SET_GPS_GLOBAL_ORIGIN 48 - +MAVPACKED( typedef struct __mavlink_set_gps_global_origin_t { int32_t latitude; /*< [degE7] Latitude (WGS84)*/ int32_t longitude; /*< [degE7] Longitude (WGS84)*/ int32_t altitude; /*< [mm] Altitude (MSL). Positive for up.*/ uint8_t target_system; /*< System ID*/ -} mavlink_set_gps_global_origin_t; + uint64_t time_usec; /*< [us] Timestamp (UNIX Epoch time or time since system boot). The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude of the number.*/ +}) mavlink_set_gps_global_origin_t; -#define MAVLINK_MSG_ID_SET_GPS_GLOBAL_ORIGIN_LEN 13 +#define MAVLINK_MSG_ID_SET_GPS_GLOBAL_ORIGIN_LEN 21 #define MAVLINK_MSG_ID_SET_GPS_GLOBAL_ORIGIN_MIN_LEN 13 -#define MAVLINK_MSG_ID_48_LEN 13 +#define MAVLINK_MSG_ID_48_LEN 21 #define MAVLINK_MSG_ID_48_MIN_LEN 13 #define MAVLINK_MSG_ID_SET_GPS_GLOBAL_ORIGIN_CRC 41 @@ -25,21 +26,23 @@ typedef struct __mavlink_set_gps_global_origin_t { #define MAVLINK_MESSAGE_INFO_SET_GPS_GLOBAL_ORIGIN { \ 48, \ "SET_GPS_GLOBAL_ORIGIN", \ - 4, \ + 5, \ { { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 12, offsetof(mavlink_set_gps_global_origin_t, target_system) }, \ { "latitude", NULL, MAVLINK_TYPE_INT32_T, 0, 0, offsetof(mavlink_set_gps_global_origin_t, latitude) }, \ { "longitude", NULL, MAVLINK_TYPE_INT32_T, 0, 4, offsetof(mavlink_set_gps_global_origin_t, longitude) }, \ { "altitude", NULL, MAVLINK_TYPE_INT32_T, 0, 8, offsetof(mavlink_set_gps_global_origin_t, altitude) }, \ + { "time_usec", NULL, MAVLINK_TYPE_UINT64_T, 0, 13, offsetof(mavlink_set_gps_global_origin_t, time_usec) }, \ } \ } #else #define MAVLINK_MESSAGE_INFO_SET_GPS_GLOBAL_ORIGIN { \ "SET_GPS_GLOBAL_ORIGIN", \ - 4, \ + 5, \ { { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 12, offsetof(mavlink_set_gps_global_origin_t, target_system) }, \ { "latitude", NULL, MAVLINK_TYPE_INT32_T, 0, 0, offsetof(mavlink_set_gps_global_origin_t, latitude) }, \ { "longitude", NULL, MAVLINK_TYPE_INT32_T, 0, 4, offsetof(mavlink_set_gps_global_origin_t, longitude) }, \ { "altitude", NULL, MAVLINK_TYPE_INT32_T, 0, 8, offsetof(mavlink_set_gps_global_origin_t, altitude) }, \ + { "time_usec", NULL, MAVLINK_TYPE_UINT64_T, 0, 13, offsetof(mavlink_set_gps_global_origin_t, time_usec) }, \ } \ } #endif @@ -54,10 +57,11 @@ typedef struct __mavlink_set_gps_global_origin_t { * @param latitude [degE7] Latitude (WGS84) * @param longitude [degE7] Longitude (WGS84) * @param altitude [mm] Altitude (MSL). Positive for up. + * @param time_usec [us] Timestamp (UNIX Epoch time or time since system boot). The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude of the number. * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_set_gps_global_origin_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint8_t target_system, int32_t latitude, int32_t longitude, int32_t altitude) + uint8_t target_system, int32_t latitude, int32_t longitude, int32_t altitude, uint64_t time_usec) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_SET_GPS_GLOBAL_ORIGIN_LEN]; @@ -65,6 +69,7 @@ static inline uint16_t mavlink_msg_set_gps_global_origin_pack(uint8_t system_id, _mav_put_int32_t(buf, 4, longitude); _mav_put_int32_t(buf, 8, altitude); _mav_put_uint8_t(buf, 12, target_system); + _mav_put_uint64_t(buf, 13, time_usec); memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SET_GPS_GLOBAL_ORIGIN_LEN); #else @@ -73,6 +78,7 @@ static inline uint16_t mavlink_msg_set_gps_global_origin_pack(uint8_t system_id, packet.longitude = longitude; packet.altitude = altitude; packet.target_system = target_system; + packet.time_usec = time_usec; memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SET_GPS_GLOBAL_ORIGIN_LEN); #endif @@ -91,11 +97,12 @@ static inline uint16_t mavlink_msg_set_gps_global_origin_pack(uint8_t system_id, * @param latitude [degE7] Latitude (WGS84) * @param longitude [degE7] Longitude (WGS84) * @param altitude [mm] Altitude (MSL). Positive for up. + * @param time_usec [us] Timestamp (UNIX Epoch time or time since system boot). The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude of the number. * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_set_gps_global_origin_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, - uint8_t target_system,int32_t latitude,int32_t longitude,int32_t altitude) + uint8_t target_system,int32_t latitude,int32_t longitude,int32_t altitude,uint64_t time_usec) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_SET_GPS_GLOBAL_ORIGIN_LEN]; @@ -103,6 +110,7 @@ static inline uint16_t mavlink_msg_set_gps_global_origin_pack_chan(uint8_t syste _mav_put_int32_t(buf, 4, longitude); _mav_put_int32_t(buf, 8, altitude); _mav_put_uint8_t(buf, 12, target_system); + _mav_put_uint64_t(buf, 13, time_usec); memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SET_GPS_GLOBAL_ORIGIN_LEN); #else @@ -111,6 +119,7 @@ static inline uint16_t mavlink_msg_set_gps_global_origin_pack_chan(uint8_t syste packet.longitude = longitude; packet.altitude = altitude; packet.target_system = target_system; + packet.time_usec = time_usec; memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SET_GPS_GLOBAL_ORIGIN_LEN); #endif @@ -129,7 +138,7 @@ static inline uint16_t mavlink_msg_set_gps_global_origin_pack_chan(uint8_t syste */ static inline uint16_t mavlink_msg_set_gps_global_origin_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_set_gps_global_origin_t* set_gps_global_origin) { - return mavlink_msg_set_gps_global_origin_pack(system_id, component_id, msg, set_gps_global_origin->target_system, set_gps_global_origin->latitude, set_gps_global_origin->longitude, set_gps_global_origin->altitude); + return mavlink_msg_set_gps_global_origin_pack(system_id, component_id, msg, set_gps_global_origin->target_system, set_gps_global_origin->latitude, set_gps_global_origin->longitude, set_gps_global_origin->altitude, set_gps_global_origin->time_usec); } /** @@ -143,7 +152,7 @@ static inline uint16_t mavlink_msg_set_gps_global_origin_encode(uint8_t system_i */ static inline uint16_t mavlink_msg_set_gps_global_origin_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_set_gps_global_origin_t* set_gps_global_origin) { - return mavlink_msg_set_gps_global_origin_pack_chan(system_id, component_id, chan, msg, set_gps_global_origin->target_system, set_gps_global_origin->latitude, set_gps_global_origin->longitude, set_gps_global_origin->altitude); + return mavlink_msg_set_gps_global_origin_pack_chan(system_id, component_id, chan, msg, set_gps_global_origin->target_system, set_gps_global_origin->latitude, set_gps_global_origin->longitude, set_gps_global_origin->altitude, set_gps_global_origin->time_usec); } /** @@ -154,10 +163,11 @@ static inline uint16_t mavlink_msg_set_gps_global_origin_encode_chan(uint8_t sys * @param latitude [degE7] Latitude (WGS84) * @param longitude [degE7] Longitude (WGS84) * @param altitude [mm] Altitude (MSL). Positive for up. + * @param time_usec [us] Timestamp (UNIX Epoch time or time since system boot). The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude of the number. */ #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS -static inline void mavlink_msg_set_gps_global_origin_send(mavlink_channel_t chan, uint8_t target_system, int32_t latitude, int32_t longitude, int32_t altitude) +static inline void mavlink_msg_set_gps_global_origin_send(mavlink_channel_t chan, uint8_t target_system, int32_t latitude, int32_t longitude, int32_t altitude, uint64_t time_usec) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_SET_GPS_GLOBAL_ORIGIN_LEN]; @@ -165,6 +175,7 @@ static inline void mavlink_msg_set_gps_global_origin_send(mavlink_channel_t chan _mav_put_int32_t(buf, 4, longitude); _mav_put_int32_t(buf, 8, altitude); _mav_put_uint8_t(buf, 12, target_system); + _mav_put_uint64_t(buf, 13, time_usec); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_GPS_GLOBAL_ORIGIN, buf, MAVLINK_MSG_ID_SET_GPS_GLOBAL_ORIGIN_MIN_LEN, MAVLINK_MSG_ID_SET_GPS_GLOBAL_ORIGIN_LEN, MAVLINK_MSG_ID_SET_GPS_GLOBAL_ORIGIN_CRC); #else @@ -173,6 +184,7 @@ static inline void mavlink_msg_set_gps_global_origin_send(mavlink_channel_t chan packet.longitude = longitude; packet.altitude = altitude; packet.target_system = target_system; + packet.time_usec = time_usec; _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_GPS_GLOBAL_ORIGIN, (const char *)&packet, MAVLINK_MSG_ID_SET_GPS_GLOBAL_ORIGIN_MIN_LEN, MAVLINK_MSG_ID_SET_GPS_GLOBAL_ORIGIN_LEN, MAVLINK_MSG_ID_SET_GPS_GLOBAL_ORIGIN_CRC); #endif @@ -186,7 +198,7 @@ static inline void mavlink_msg_set_gps_global_origin_send(mavlink_channel_t chan static inline void mavlink_msg_set_gps_global_origin_send_struct(mavlink_channel_t chan, const mavlink_set_gps_global_origin_t* set_gps_global_origin) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - mavlink_msg_set_gps_global_origin_send(chan, set_gps_global_origin->target_system, set_gps_global_origin->latitude, set_gps_global_origin->longitude, set_gps_global_origin->altitude); + mavlink_msg_set_gps_global_origin_send(chan, set_gps_global_origin->target_system, set_gps_global_origin->latitude, set_gps_global_origin->longitude, set_gps_global_origin->altitude, set_gps_global_origin->time_usec); #else _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_GPS_GLOBAL_ORIGIN, (const char *)set_gps_global_origin, MAVLINK_MSG_ID_SET_GPS_GLOBAL_ORIGIN_MIN_LEN, MAVLINK_MSG_ID_SET_GPS_GLOBAL_ORIGIN_LEN, MAVLINK_MSG_ID_SET_GPS_GLOBAL_ORIGIN_CRC); #endif @@ -200,7 +212,7 @@ static inline void mavlink_msg_set_gps_global_origin_send_struct(mavlink_channel is usually the receive buffer for the channel, and allows a reply to an incoming message with minimum stack space usage. */ -static inline void mavlink_msg_set_gps_global_origin_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target_system, int32_t latitude, int32_t longitude, int32_t altitude) +static inline void mavlink_msg_set_gps_global_origin_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target_system, int32_t latitude, int32_t longitude, int32_t altitude, uint64_t time_usec) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char *buf = (char *)msgbuf; @@ -208,6 +220,7 @@ static inline void mavlink_msg_set_gps_global_origin_send_buf(mavlink_message_t _mav_put_int32_t(buf, 4, longitude); _mav_put_int32_t(buf, 8, altitude); _mav_put_uint8_t(buf, 12, target_system); + _mav_put_uint64_t(buf, 13, time_usec); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_GPS_GLOBAL_ORIGIN, buf, MAVLINK_MSG_ID_SET_GPS_GLOBAL_ORIGIN_MIN_LEN, MAVLINK_MSG_ID_SET_GPS_GLOBAL_ORIGIN_LEN, MAVLINK_MSG_ID_SET_GPS_GLOBAL_ORIGIN_CRC); #else @@ -216,6 +229,7 @@ static inline void mavlink_msg_set_gps_global_origin_send_buf(mavlink_message_t packet->longitude = longitude; packet->altitude = altitude; packet->target_system = target_system; + packet->time_usec = time_usec; _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_GPS_GLOBAL_ORIGIN, (const char *)packet, MAVLINK_MSG_ID_SET_GPS_GLOBAL_ORIGIN_MIN_LEN, MAVLINK_MSG_ID_SET_GPS_GLOBAL_ORIGIN_LEN, MAVLINK_MSG_ID_SET_GPS_GLOBAL_ORIGIN_CRC); #endif @@ -267,6 +281,16 @@ static inline int32_t mavlink_msg_set_gps_global_origin_get_altitude(const mavli return _MAV_RETURN_int32_t(msg, 8); } +/** + * @brief Get field time_usec from set_gps_global_origin message + * + * @return [us] Timestamp (UNIX Epoch time or time since system boot). The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude of the number. + */ +static inline uint64_t mavlink_msg_set_gps_global_origin_get_time_usec(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint64_t(msg, 13); +} + /** * @brief Decode a set_gps_global_origin message into a struct * @@ -280,6 +304,7 @@ static inline void mavlink_msg_set_gps_global_origin_decode(const mavlink_messag set_gps_global_origin->longitude = mavlink_msg_set_gps_global_origin_get_longitude(msg); set_gps_global_origin->altitude = mavlink_msg_set_gps_global_origin_get_altitude(msg); set_gps_global_origin->target_system = mavlink_msg_set_gps_global_origin_get_target_system(msg); + set_gps_global_origin->time_usec = mavlink_msg_set_gps_global_origin_get_time_usec(msg); #else uint8_t len = msg->len < MAVLINK_MSG_ID_SET_GPS_GLOBAL_ORIGIN_LEN? msg->len : MAVLINK_MSG_ID_SET_GPS_GLOBAL_ORIGIN_LEN; memset(set_gps_global_origin, 0, MAVLINK_MSG_ID_SET_GPS_GLOBAL_ORIGIN_LEN); diff --git a/lib/main/MAVLink/common/mavlink_msg_set_home_position.h b/lib/main/MAVLink/common/mavlink_msg_set_home_position.h index 7ddcae5a09..ea7d2d0f26 100755 --- a/lib/main/MAVLink/common/mavlink_msg_set_home_position.h +++ b/lib/main/MAVLink/common/mavlink_msg_set_home_position.h @@ -3,7 +3,7 @@ #define MAVLINK_MSG_ID_SET_HOME_POSITION 243 - +MAVPACKED( typedef struct __mavlink_set_home_position_t { int32_t latitude; /*< [degE7] Latitude (WGS84)*/ int32_t longitude; /*< [degE7] Longitude (WGS84)*/ @@ -16,11 +16,12 @@ typedef struct __mavlink_set_home_position_t { float approach_y; /*< [m] Local Y position of the end of the approach vector. Multicopters should set this position based on their takeoff path. Grass-landing fixed wing aircraft should set it the same way as multicopters. Runway-landing fixed wing aircraft should set it to the opposite direction of the takeoff, assuming the takeoff happened from the threshold / touchdown zone.*/ float approach_z; /*< [m] Local Z position of the end of the approach vector. Multicopters should set this position based on their takeoff path. Grass-landing fixed wing aircraft should set it the same way as multicopters. Runway-landing fixed wing aircraft should set it to the opposite direction of the takeoff, assuming the takeoff happened from the threshold / touchdown zone.*/ uint8_t target_system; /*< System ID.*/ -} mavlink_set_home_position_t; + uint64_t time_usec; /*< [us] Timestamp (UNIX Epoch time or time since system boot). The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude of the number.*/ +}) mavlink_set_home_position_t; -#define MAVLINK_MSG_ID_SET_HOME_POSITION_LEN 53 +#define MAVLINK_MSG_ID_SET_HOME_POSITION_LEN 61 #define MAVLINK_MSG_ID_SET_HOME_POSITION_MIN_LEN 53 -#define MAVLINK_MSG_ID_243_LEN 53 +#define MAVLINK_MSG_ID_243_LEN 61 #define MAVLINK_MSG_ID_243_MIN_LEN 53 #define MAVLINK_MSG_ID_SET_HOME_POSITION_CRC 85 @@ -32,7 +33,7 @@ typedef struct __mavlink_set_home_position_t { #define MAVLINK_MESSAGE_INFO_SET_HOME_POSITION { \ 243, \ "SET_HOME_POSITION", \ - 11, \ + 12, \ { { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 52, offsetof(mavlink_set_home_position_t, target_system) }, \ { "latitude", NULL, MAVLINK_TYPE_INT32_T, 0, 0, offsetof(mavlink_set_home_position_t, latitude) }, \ { "longitude", NULL, MAVLINK_TYPE_INT32_T, 0, 4, offsetof(mavlink_set_home_position_t, longitude) }, \ @@ -44,12 +45,13 @@ typedef struct __mavlink_set_home_position_t { { "approach_x", NULL, MAVLINK_TYPE_FLOAT, 0, 40, offsetof(mavlink_set_home_position_t, approach_x) }, \ { "approach_y", NULL, MAVLINK_TYPE_FLOAT, 0, 44, offsetof(mavlink_set_home_position_t, approach_y) }, \ { "approach_z", NULL, MAVLINK_TYPE_FLOAT, 0, 48, offsetof(mavlink_set_home_position_t, approach_z) }, \ + { "time_usec", NULL, MAVLINK_TYPE_UINT64_T, 0, 53, offsetof(mavlink_set_home_position_t, time_usec) }, \ } \ } #else #define MAVLINK_MESSAGE_INFO_SET_HOME_POSITION { \ "SET_HOME_POSITION", \ - 11, \ + 12, \ { { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 52, offsetof(mavlink_set_home_position_t, target_system) }, \ { "latitude", NULL, MAVLINK_TYPE_INT32_T, 0, 0, offsetof(mavlink_set_home_position_t, latitude) }, \ { "longitude", NULL, MAVLINK_TYPE_INT32_T, 0, 4, offsetof(mavlink_set_home_position_t, longitude) }, \ @@ -61,6 +63,7 @@ typedef struct __mavlink_set_home_position_t { { "approach_x", NULL, MAVLINK_TYPE_FLOAT, 0, 40, offsetof(mavlink_set_home_position_t, approach_x) }, \ { "approach_y", NULL, MAVLINK_TYPE_FLOAT, 0, 44, offsetof(mavlink_set_home_position_t, approach_y) }, \ { "approach_z", NULL, MAVLINK_TYPE_FLOAT, 0, 48, offsetof(mavlink_set_home_position_t, approach_z) }, \ + { "time_usec", NULL, MAVLINK_TYPE_UINT64_T, 0, 53, offsetof(mavlink_set_home_position_t, time_usec) }, \ } \ } #endif @@ -82,10 +85,11 @@ typedef struct __mavlink_set_home_position_t { * @param approach_x [m] Local X position of the end of the approach vector. Multicopters should set this position based on their takeoff path. Grass-landing fixed wing aircraft should set it the same way as multicopters. Runway-landing fixed wing aircraft should set it to the opposite direction of the takeoff, assuming the takeoff happened from the threshold / touchdown zone. * @param approach_y [m] Local Y position of the end of the approach vector. Multicopters should set this position based on their takeoff path. Grass-landing fixed wing aircraft should set it the same way as multicopters. Runway-landing fixed wing aircraft should set it to the opposite direction of the takeoff, assuming the takeoff happened from the threshold / touchdown zone. * @param approach_z [m] Local Z position of the end of the approach vector. Multicopters should set this position based on their takeoff path. Grass-landing fixed wing aircraft should set it the same way as multicopters. Runway-landing fixed wing aircraft should set it to the opposite direction of the takeoff, assuming the takeoff happened from the threshold / touchdown zone. + * @param time_usec [us] Timestamp (UNIX Epoch time or time since system boot). The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude of the number. * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_set_home_position_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint8_t target_system, int32_t latitude, int32_t longitude, int32_t altitude, float x, float y, float z, const float *q, float approach_x, float approach_y, float approach_z) + uint8_t target_system, int32_t latitude, int32_t longitude, int32_t altitude, float x, float y, float z, const float *q, float approach_x, float approach_y, float approach_z, uint64_t time_usec) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_SET_HOME_POSITION_LEN]; @@ -99,6 +103,7 @@ static inline uint16_t mavlink_msg_set_home_position_pack(uint8_t system_id, uin _mav_put_float(buf, 44, approach_y); _mav_put_float(buf, 48, approach_z); _mav_put_uint8_t(buf, 52, target_system); + _mav_put_uint64_t(buf, 53, time_usec); _mav_put_float_array(buf, 24, q, 4); memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SET_HOME_POSITION_LEN); #else @@ -113,6 +118,7 @@ static inline uint16_t mavlink_msg_set_home_position_pack(uint8_t system_id, uin packet.approach_y = approach_y; packet.approach_z = approach_z; packet.target_system = target_system; + packet.time_usec = time_usec; mav_array_memcpy(packet.q, q, sizeof(float)*4); memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SET_HOME_POSITION_LEN); #endif @@ -138,11 +144,12 @@ static inline uint16_t mavlink_msg_set_home_position_pack(uint8_t system_id, uin * @param approach_x [m] Local X position of the end of the approach vector. Multicopters should set this position based on their takeoff path. Grass-landing fixed wing aircraft should set it the same way as multicopters. Runway-landing fixed wing aircraft should set it to the opposite direction of the takeoff, assuming the takeoff happened from the threshold / touchdown zone. * @param approach_y [m] Local Y position of the end of the approach vector. Multicopters should set this position based on their takeoff path. Grass-landing fixed wing aircraft should set it the same way as multicopters. Runway-landing fixed wing aircraft should set it to the opposite direction of the takeoff, assuming the takeoff happened from the threshold / touchdown zone. * @param approach_z [m] Local Z position of the end of the approach vector. Multicopters should set this position based on their takeoff path. Grass-landing fixed wing aircraft should set it the same way as multicopters. Runway-landing fixed wing aircraft should set it to the opposite direction of the takeoff, assuming the takeoff happened from the threshold / touchdown zone. + * @param time_usec [us] Timestamp (UNIX Epoch time or time since system boot). The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude of the number. * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_set_home_position_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, - uint8_t target_system,int32_t latitude,int32_t longitude,int32_t altitude,float x,float y,float z,const float *q,float approach_x,float approach_y,float approach_z) + uint8_t target_system,int32_t latitude,int32_t longitude,int32_t altitude,float x,float y,float z,const float *q,float approach_x,float approach_y,float approach_z,uint64_t time_usec) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_SET_HOME_POSITION_LEN]; @@ -156,6 +163,7 @@ static inline uint16_t mavlink_msg_set_home_position_pack_chan(uint8_t system_id _mav_put_float(buf, 44, approach_y); _mav_put_float(buf, 48, approach_z); _mav_put_uint8_t(buf, 52, target_system); + _mav_put_uint64_t(buf, 53, time_usec); _mav_put_float_array(buf, 24, q, 4); memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SET_HOME_POSITION_LEN); #else @@ -170,6 +178,7 @@ static inline uint16_t mavlink_msg_set_home_position_pack_chan(uint8_t system_id packet.approach_y = approach_y; packet.approach_z = approach_z; packet.target_system = target_system; + packet.time_usec = time_usec; mav_array_memcpy(packet.q, q, sizeof(float)*4); memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SET_HOME_POSITION_LEN); #endif @@ -188,7 +197,7 @@ static inline uint16_t mavlink_msg_set_home_position_pack_chan(uint8_t system_id */ static inline uint16_t mavlink_msg_set_home_position_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_set_home_position_t* set_home_position) { - return mavlink_msg_set_home_position_pack(system_id, component_id, msg, set_home_position->target_system, set_home_position->latitude, set_home_position->longitude, set_home_position->altitude, set_home_position->x, set_home_position->y, set_home_position->z, set_home_position->q, set_home_position->approach_x, set_home_position->approach_y, set_home_position->approach_z); + return mavlink_msg_set_home_position_pack(system_id, component_id, msg, set_home_position->target_system, set_home_position->latitude, set_home_position->longitude, set_home_position->altitude, set_home_position->x, set_home_position->y, set_home_position->z, set_home_position->q, set_home_position->approach_x, set_home_position->approach_y, set_home_position->approach_z, set_home_position->time_usec); } /** @@ -202,7 +211,7 @@ static inline uint16_t mavlink_msg_set_home_position_encode(uint8_t system_id, u */ static inline uint16_t mavlink_msg_set_home_position_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_set_home_position_t* set_home_position) { - return mavlink_msg_set_home_position_pack_chan(system_id, component_id, chan, msg, set_home_position->target_system, set_home_position->latitude, set_home_position->longitude, set_home_position->altitude, set_home_position->x, set_home_position->y, set_home_position->z, set_home_position->q, set_home_position->approach_x, set_home_position->approach_y, set_home_position->approach_z); + return mavlink_msg_set_home_position_pack_chan(system_id, component_id, chan, msg, set_home_position->target_system, set_home_position->latitude, set_home_position->longitude, set_home_position->altitude, set_home_position->x, set_home_position->y, set_home_position->z, set_home_position->q, set_home_position->approach_x, set_home_position->approach_y, set_home_position->approach_z, set_home_position->time_usec); } /** @@ -220,10 +229,11 @@ static inline uint16_t mavlink_msg_set_home_position_encode_chan(uint8_t system_ * @param approach_x [m] Local X position of the end of the approach vector. Multicopters should set this position based on their takeoff path. Grass-landing fixed wing aircraft should set it the same way as multicopters. Runway-landing fixed wing aircraft should set it to the opposite direction of the takeoff, assuming the takeoff happened from the threshold / touchdown zone. * @param approach_y [m] Local Y position of the end of the approach vector. Multicopters should set this position based on their takeoff path. Grass-landing fixed wing aircraft should set it the same way as multicopters. Runway-landing fixed wing aircraft should set it to the opposite direction of the takeoff, assuming the takeoff happened from the threshold / touchdown zone. * @param approach_z [m] Local Z position of the end of the approach vector. Multicopters should set this position based on their takeoff path. Grass-landing fixed wing aircraft should set it the same way as multicopters. Runway-landing fixed wing aircraft should set it to the opposite direction of the takeoff, assuming the takeoff happened from the threshold / touchdown zone. + * @param time_usec [us] Timestamp (UNIX Epoch time or time since system boot). The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude of the number. */ #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS -static inline void mavlink_msg_set_home_position_send(mavlink_channel_t chan, uint8_t target_system, int32_t latitude, int32_t longitude, int32_t altitude, float x, float y, float z, const float *q, float approach_x, float approach_y, float approach_z) +static inline void mavlink_msg_set_home_position_send(mavlink_channel_t chan, uint8_t target_system, int32_t latitude, int32_t longitude, int32_t altitude, float x, float y, float z, const float *q, float approach_x, float approach_y, float approach_z, uint64_t time_usec) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_SET_HOME_POSITION_LEN]; @@ -237,6 +247,7 @@ static inline void mavlink_msg_set_home_position_send(mavlink_channel_t chan, ui _mav_put_float(buf, 44, approach_y); _mav_put_float(buf, 48, approach_z); _mav_put_uint8_t(buf, 52, target_system); + _mav_put_uint64_t(buf, 53, time_usec); _mav_put_float_array(buf, 24, q, 4); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_HOME_POSITION, buf, MAVLINK_MSG_ID_SET_HOME_POSITION_MIN_LEN, MAVLINK_MSG_ID_SET_HOME_POSITION_LEN, MAVLINK_MSG_ID_SET_HOME_POSITION_CRC); #else @@ -251,6 +262,7 @@ static inline void mavlink_msg_set_home_position_send(mavlink_channel_t chan, ui packet.approach_y = approach_y; packet.approach_z = approach_z; packet.target_system = target_system; + packet.time_usec = time_usec; mav_array_memcpy(packet.q, q, sizeof(float)*4); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_HOME_POSITION, (const char *)&packet, MAVLINK_MSG_ID_SET_HOME_POSITION_MIN_LEN, MAVLINK_MSG_ID_SET_HOME_POSITION_LEN, MAVLINK_MSG_ID_SET_HOME_POSITION_CRC); #endif @@ -264,7 +276,7 @@ static inline void mavlink_msg_set_home_position_send(mavlink_channel_t chan, ui static inline void mavlink_msg_set_home_position_send_struct(mavlink_channel_t chan, const mavlink_set_home_position_t* set_home_position) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - mavlink_msg_set_home_position_send(chan, set_home_position->target_system, set_home_position->latitude, set_home_position->longitude, set_home_position->altitude, set_home_position->x, set_home_position->y, set_home_position->z, set_home_position->q, set_home_position->approach_x, set_home_position->approach_y, set_home_position->approach_z); + mavlink_msg_set_home_position_send(chan, set_home_position->target_system, set_home_position->latitude, set_home_position->longitude, set_home_position->altitude, set_home_position->x, set_home_position->y, set_home_position->z, set_home_position->q, set_home_position->approach_x, set_home_position->approach_y, set_home_position->approach_z, set_home_position->time_usec); #else _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_HOME_POSITION, (const char *)set_home_position, MAVLINK_MSG_ID_SET_HOME_POSITION_MIN_LEN, MAVLINK_MSG_ID_SET_HOME_POSITION_LEN, MAVLINK_MSG_ID_SET_HOME_POSITION_CRC); #endif @@ -278,7 +290,7 @@ static inline void mavlink_msg_set_home_position_send_struct(mavlink_channel_t c is usually the receive buffer for the channel, and allows a reply to an incoming message with minimum stack space usage. */ -static inline void mavlink_msg_set_home_position_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target_system, int32_t latitude, int32_t longitude, int32_t altitude, float x, float y, float z, const float *q, float approach_x, float approach_y, float approach_z) +static inline void mavlink_msg_set_home_position_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target_system, int32_t latitude, int32_t longitude, int32_t altitude, float x, float y, float z, const float *q, float approach_x, float approach_y, float approach_z, uint64_t time_usec) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char *buf = (char *)msgbuf; @@ -292,6 +304,7 @@ static inline void mavlink_msg_set_home_position_send_buf(mavlink_message_t *msg _mav_put_float(buf, 44, approach_y); _mav_put_float(buf, 48, approach_z); _mav_put_uint8_t(buf, 52, target_system); + _mav_put_uint64_t(buf, 53, time_usec); _mav_put_float_array(buf, 24, q, 4); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_HOME_POSITION, buf, MAVLINK_MSG_ID_SET_HOME_POSITION_MIN_LEN, MAVLINK_MSG_ID_SET_HOME_POSITION_LEN, MAVLINK_MSG_ID_SET_HOME_POSITION_CRC); #else @@ -306,6 +319,7 @@ static inline void mavlink_msg_set_home_position_send_buf(mavlink_message_t *msg packet->approach_y = approach_y; packet->approach_z = approach_z; packet->target_system = target_system; + packet->time_usec = time_usec; mav_array_memcpy(packet->q, q, sizeof(float)*4); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SET_HOME_POSITION, (const char *)packet, MAVLINK_MSG_ID_SET_HOME_POSITION_MIN_LEN, MAVLINK_MSG_ID_SET_HOME_POSITION_LEN, MAVLINK_MSG_ID_SET_HOME_POSITION_CRC); #endif @@ -427,6 +441,16 @@ static inline float mavlink_msg_set_home_position_get_approach_z(const mavlink_m return _MAV_RETURN_float(msg, 48); } +/** + * @brief Get field time_usec from set_home_position message + * + * @return [us] Timestamp (UNIX Epoch time or time since system boot). The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude of the number. + */ +static inline uint64_t mavlink_msg_set_home_position_get_time_usec(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint64_t(msg, 53); +} + /** * @brief Decode a set_home_position message into a struct * @@ -447,6 +471,7 @@ static inline void mavlink_msg_set_home_position_decode(const mavlink_message_t* set_home_position->approach_y = mavlink_msg_set_home_position_get_approach_y(msg); set_home_position->approach_z = mavlink_msg_set_home_position_get_approach_z(msg); set_home_position->target_system = mavlink_msg_set_home_position_get_target_system(msg); + set_home_position->time_usec = mavlink_msg_set_home_position_get_time_usec(msg); #else uint8_t len = msg->len < MAVLINK_MSG_ID_SET_HOME_POSITION_LEN? msg->len : MAVLINK_MSG_ID_SET_HOME_POSITION_LEN; memset(set_home_position, 0, MAVLINK_MSG_ID_SET_HOME_POSITION_LEN); diff --git a/lib/main/MAVLink/common/mavlink_msg_setup_signing.h b/lib/main/MAVLink/common/mavlink_msg_setup_signing.h new file mode 100644 index 0000000000..e9bab0cbaa --- /dev/null +++ b/lib/main/MAVLink/common/mavlink_msg_setup_signing.h @@ -0,0 +1,280 @@ +#pragma once +// MESSAGE SETUP_SIGNING PACKING + +#define MAVLINK_MSG_ID_SETUP_SIGNING 256 + + +typedef struct __mavlink_setup_signing_t { + uint64_t initial_timestamp; /*< initial timestamp*/ + uint8_t target_system; /*< system id of the target*/ + uint8_t target_component; /*< component ID of the target*/ + uint8_t secret_key[32]; /*< signing key*/ +} mavlink_setup_signing_t; + +#define MAVLINK_MSG_ID_SETUP_SIGNING_LEN 42 +#define MAVLINK_MSG_ID_SETUP_SIGNING_MIN_LEN 42 +#define MAVLINK_MSG_ID_256_LEN 42 +#define MAVLINK_MSG_ID_256_MIN_LEN 42 + +#define MAVLINK_MSG_ID_SETUP_SIGNING_CRC 71 +#define MAVLINK_MSG_ID_256_CRC 71 + +#define MAVLINK_MSG_SETUP_SIGNING_FIELD_SECRET_KEY_LEN 32 + +#if MAVLINK_COMMAND_24BIT +#define MAVLINK_MESSAGE_INFO_SETUP_SIGNING { \ + 256, \ + "SETUP_SIGNING", \ + 4, \ + { { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 8, offsetof(mavlink_setup_signing_t, target_system) }, \ + { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 9, offsetof(mavlink_setup_signing_t, target_component) }, \ + { "secret_key", NULL, MAVLINK_TYPE_UINT8_T, 32, 10, offsetof(mavlink_setup_signing_t, secret_key) }, \ + { "initial_timestamp", NULL, MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_setup_signing_t, initial_timestamp) }, \ + } \ +} +#else +#define MAVLINK_MESSAGE_INFO_SETUP_SIGNING { \ + "SETUP_SIGNING", \ + 4, \ + { { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 8, offsetof(mavlink_setup_signing_t, target_system) }, \ + { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 9, offsetof(mavlink_setup_signing_t, target_component) }, \ + { "secret_key", NULL, MAVLINK_TYPE_UINT8_T, 32, 10, offsetof(mavlink_setup_signing_t, secret_key) }, \ + { "initial_timestamp", NULL, MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_setup_signing_t, initial_timestamp) }, \ + } \ +} +#endif + +/** + * @brief Pack a setup_signing message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param target_system system id of the target + * @param target_component component ID of the target + * @param secret_key signing key + * @param initial_timestamp initial timestamp + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_setup_signing_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint8_t target_system, uint8_t target_component, const uint8_t *secret_key, uint64_t initial_timestamp) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_SETUP_SIGNING_LEN]; + _mav_put_uint64_t(buf, 0, initial_timestamp); + _mav_put_uint8_t(buf, 8, target_system); + _mav_put_uint8_t(buf, 9, target_component); + _mav_put_uint8_t_array(buf, 10, secret_key, 32); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SETUP_SIGNING_LEN); +#else + mavlink_setup_signing_t packet; + packet.initial_timestamp = initial_timestamp; + packet.target_system = target_system; + packet.target_component = target_component; + mav_array_memcpy(packet.secret_key, secret_key, sizeof(uint8_t)*32); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SETUP_SIGNING_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_SETUP_SIGNING; + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SETUP_SIGNING_MIN_LEN, MAVLINK_MSG_ID_SETUP_SIGNING_LEN, MAVLINK_MSG_ID_SETUP_SIGNING_CRC); +} + +/** + * @brief Pack a setup_signing message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param target_system system id of the target + * @param target_component component ID of the target + * @param secret_key signing key + * @param initial_timestamp initial timestamp + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_setup_signing_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint8_t target_system,uint8_t target_component,const uint8_t *secret_key,uint64_t initial_timestamp) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_SETUP_SIGNING_LEN]; + _mav_put_uint64_t(buf, 0, initial_timestamp); + _mav_put_uint8_t(buf, 8, target_system); + _mav_put_uint8_t(buf, 9, target_component); + _mav_put_uint8_t_array(buf, 10, secret_key, 32); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SETUP_SIGNING_LEN); +#else + mavlink_setup_signing_t packet; + packet.initial_timestamp = initial_timestamp; + packet.target_system = target_system; + packet.target_component = target_component; + mav_array_memcpy(packet.secret_key, secret_key, sizeof(uint8_t)*32); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SETUP_SIGNING_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_SETUP_SIGNING; + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SETUP_SIGNING_MIN_LEN, MAVLINK_MSG_ID_SETUP_SIGNING_LEN, MAVLINK_MSG_ID_SETUP_SIGNING_CRC); +} + +/** + * @brief Encode a setup_signing struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param setup_signing C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_setup_signing_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_setup_signing_t* setup_signing) +{ + return mavlink_msg_setup_signing_pack(system_id, component_id, msg, setup_signing->target_system, setup_signing->target_component, setup_signing->secret_key, setup_signing->initial_timestamp); +} + +/** + * @brief Encode a setup_signing struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param setup_signing C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_setup_signing_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_setup_signing_t* setup_signing) +{ + return mavlink_msg_setup_signing_pack_chan(system_id, component_id, chan, msg, setup_signing->target_system, setup_signing->target_component, setup_signing->secret_key, setup_signing->initial_timestamp); +} + +/** + * @brief Send a setup_signing message + * @param chan MAVLink channel to send the message + * + * @param target_system system id of the target + * @param target_component component ID of the target + * @param secret_key signing key + * @param initial_timestamp initial timestamp + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_setup_signing_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, const uint8_t *secret_key, uint64_t initial_timestamp) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_SETUP_SIGNING_LEN]; + _mav_put_uint64_t(buf, 0, initial_timestamp); + _mav_put_uint8_t(buf, 8, target_system); + _mav_put_uint8_t(buf, 9, target_component); + _mav_put_uint8_t_array(buf, 10, secret_key, 32); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SETUP_SIGNING, buf, MAVLINK_MSG_ID_SETUP_SIGNING_MIN_LEN, MAVLINK_MSG_ID_SETUP_SIGNING_LEN, MAVLINK_MSG_ID_SETUP_SIGNING_CRC); +#else + mavlink_setup_signing_t packet; + packet.initial_timestamp = initial_timestamp; + packet.target_system = target_system; + packet.target_component = target_component; + mav_array_memcpy(packet.secret_key, secret_key, sizeof(uint8_t)*32); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SETUP_SIGNING, (const char *)&packet, MAVLINK_MSG_ID_SETUP_SIGNING_MIN_LEN, MAVLINK_MSG_ID_SETUP_SIGNING_LEN, MAVLINK_MSG_ID_SETUP_SIGNING_CRC); +#endif +} + +/** + * @brief Send a setup_signing message + * @param chan MAVLink channel to send the message + * @param struct The MAVLink struct to serialize + */ +static inline void mavlink_msg_setup_signing_send_struct(mavlink_channel_t chan, const mavlink_setup_signing_t* setup_signing) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + mavlink_msg_setup_signing_send(chan, setup_signing->target_system, setup_signing->target_component, setup_signing->secret_key, setup_signing->initial_timestamp); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SETUP_SIGNING, (const char *)setup_signing, MAVLINK_MSG_ID_SETUP_SIGNING_MIN_LEN, MAVLINK_MSG_ID_SETUP_SIGNING_LEN, MAVLINK_MSG_ID_SETUP_SIGNING_CRC); +#endif +} + +#if MAVLINK_MSG_ID_SETUP_SIGNING_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_setup_signing_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, const uint8_t *secret_key, uint64_t initial_timestamp) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint64_t(buf, 0, initial_timestamp); + _mav_put_uint8_t(buf, 8, target_system); + _mav_put_uint8_t(buf, 9, target_component); + _mav_put_uint8_t_array(buf, 10, secret_key, 32); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SETUP_SIGNING, buf, MAVLINK_MSG_ID_SETUP_SIGNING_MIN_LEN, MAVLINK_MSG_ID_SETUP_SIGNING_LEN, MAVLINK_MSG_ID_SETUP_SIGNING_CRC); +#else + mavlink_setup_signing_t *packet = (mavlink_setup_signing_t *)msgbuf; + packet->initial_timestamp = initial_timestamp; + packet->target_system = target_system; + packet->target_component = target_component; + mav_array_memcpy(packet->secret_key, secret_key, sizeof(uint8_t)*32); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SETUP_SIGNING, (const char *)packet, MAVLINK_MSG_ID_SETUP_SIGNING_MIN_LEN, MAVLINK_MSG_ID_SETUP_SIGNING_LEN, MAVLINK_MSG_ID_SETUP_SIGNING_CRC); +#endif +} +#endif + +#endif + +// MESSAGE SETUP_SIGNING UNPACKING + + +/** + * @brief Get field target_system from setup_signing message + * + * @return system id of the target + */ +static inline uint8_t mavlink_msg_setup_signing_get_target_system(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 8); +} + +/** + * @brief Get field target_component from setup_signing message + * + * @return component ID of the target + */ +static inline uint8_t mavlink_msg_setup_signing_get_target_component(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 9); +} + +/** + * @brief Get field secret_key from setup_signing message + * + * @return signing key + */ +static inline uint16_t mavlink_msg_setup_signing_get_secret_key(const mavlink_message_t* msg, uint8_t *secret_key) +{ + return _MAV_RETURN_uint8_t_array(msg, secret_key, 32, 10); +} + +/** + * @brief Get field initial_timestamp from setup_signing message + * + * @return initial timestamp + */ +static inline uint64_t mavlink_msg_setup_signing_get_initial_timestamp(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint64_t(msg, 0); +} + +/** + * @brief Decode a setup_signing message into a struct + * + * @param msg The message to decode + * @param setup_signing C-struct to decode the message contents into + */ +static inline void mavlink_msg_setup_signing_decode(const mavlink_message_t* msg, mavlink_setup_signing_t* setup_signing) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + setup_signing->initial_timestamp = mavlink_msg_setup_signing_get_initial_timestamp(msg); + setup_signing->target_system = mavlink_msg_setup_signing_get_target_system(msg); + setup_signing->target_component = mavlink_msg_setup_signing_get_target_component(msg); + mavlink_msg_setup_signing_get_secret_key(msg, setup_signing->secret_key); +#else + uint8_t len = msg->len < MAVLINK_MSG_ID_SETUP_SIGNING_LEN? msg->len : MAVLINK_MSG_ID_SETUP_SIGNING_LEN; + memset(setup_signing, 0, MAVLINK_MSG_ID_SETUP_SIGNING_LEN); + memcpy(setup_signing, _MAV_PAYLOAD(msg), len); +#endif +} diff --git a/lib/main/MAVLink/common/mavlink_msg_smart_battery_info.h b/lib/main/MAVLink/common/mavlink_msg_smart_battery_info.h new file mode 100644 index 0000000000..174c197f31 --- /dev/null +++ b/lib/main/MAVLink/common/mavlink_msg_smart_battery_info.h @@ -0,0 +1,481 @@ +#pragma once +// MESSAGE SMART_BATTERY_INFO PACKING + +#define MAVLINK_MSG_ID_SMART_BATTERY_INFO 370 + + +typedef struct __mavlink_smart_battery_info_t { + int32_t capacity_full_specification; /*< [mAh] Capacity when full according to manufacturer, -1: field not provided.*/ + int32_t capacity_full; /*< [mAh] Capacity when full (accounting for battery degradation), -1: field not provided.*/ + uint16_t cycle_count; /*< Charge/discharge cycle count. UINT16_MAX: field not provided.*/ + uint16_t weight; /*< [g] Battery weight. 0: field not provided.*/ + uint16_t discharge_minimum_voltage; /*< [mV] Minimum per-cell voltage when discharging. If not supplied set to UINT16_MAX value.*/ + uint16_t charging_minimum_voltage; /*< [mV] Minimum per-cell voltage when charging. If not supplied set to UINT16_MAX value.*/ + uint16_t resting_minimum_voltage; /*< [mV] Minimum per-cell voltage when resting. If not supplied set to UINT16_MAX value.*/ + uint8_t id; /*< Battery ID*/ + uint8_t battery_function; /*< Function of the battery*/ + uint8_t type; /*< Type (chemistry) of the battery*/ + char serial_number[16]; /*< Serial number in ASCII characters, 0 terminated. All 0: field not provided.*/ + char device_name[50]; /*< Static device name. Encode as manufacturer and product names separated using an underscore.*/ +} mavlink_smart_battery_info_t; + +#define MAVLINK_MSG_ID_SMART_BATTERY_INFO_LEN 87 +#define MAVLINK_MSG_ID_SMART_BATTERY_INFO_MIN_LEN 87 +#define MAVLINK_MSG_ID_370_LEN 87 +#define MAVLINK_MSG_ID_370_MIN_LEN 87 + +#define MAVLINK_MSG_ID_SMART_BATTERY_INFO_CRC 75 +#define MAVLINK_MSG_ID_370_CRC 75 + +#define MAVLINK_MSG_SMART_BATTERY_INFO_FIELD_SERIAL_NUMBER_LEN 16 +#define MAVLINK_MSG_SMART_BATTERY_INFO_FIELD_DEVICE_NAME_LEN 50 + +#if MAVLINK_COMMAND_24BIT +#define MAVLINK_MESSAGE_INFO_SMART_BATTERY_INFO { \ + 370, \ + "SMART_BATTERY_INFO", \ + 12, \ + { { "id", NULL, MAVLINK_TYPE_UINT8_T, 0, 18, offsetof(mavlink_smart_battery_info_t, id) }, \ + { "battery_function", NULL, MAVLINK_TYPE_UINT8_T, 0, 19, offsetof(mavlink_smart_battery_info_t, battery_function) }, \ + { "type", NULL, MAVLINK_TYPE_UINT8_T, 0, 20, offsetof(mavlink_smart_battery_info_t, type) }, \ + { "capacity_full_specification", NULL, MAVLINK_TYPE_INT32_T, 0, 0, offsetof(mavlink_smart_battery_info_t, capacity_full_specification) }, \ + { "capacity_full", NULL, MAVLINK_TYPE_INT32_T, 0, 4, offsetof(mavlink_smart_battery_info_t, capacity_full) }, \ + { "cycle_count", NULL, MAVLINK_TYPE_UINT16_T, 0, 8, offsetof(mavlink_smart_battery_info_t, cycle_count) }, \ + { "serial_number", NULL, MAVLINK_TYPE_CHAR, 16, 21, offsetof(mavlink_smart_battery_info_t, serial_number) }, \ + { "device_name", NULL, MAVLINK_TYPE_CHAR, 50, 37, offsetof(mavlink_smart_battery_info_t, device_name) }, \ + { "weight", NULL, MAVLINK_TYPE_UINT16_T, 0, 10, offsetof(mavlink_smart_battery_info_t, weight) }, \ + { "discharge_minimum_voltage", NULL, MAVLINK_TYPE_UINT16_T, 0, 12, offsetof(mavlink_smart_battery_info_t, discharge_minimum_voltage) }, \ + { "charging_minimum_voltage", NULL, MAVLINK_TYPE_UINT16_T, 0, 14, offsetof(mavlink_smart_battery_info_t, charging_minimum_voltage) }, \ + { "resting_minimum_voltage", NULL, MAVLINK_TYPE_UINT16_T, 0, 16, offsetof(mavlink_smart_battery_info_t, resting_minimum_voltage) }, \ + } \ +} +#else +#define MAVLINK_MESSAGE_INFO_SMART_BATTERY_INFO { \ + "SMART_BATTERY_INFO", \ + 12, \ + { { "id", NULL, MAVLINK_TYPE_UINT8_T, 0, 18, offsetof(mavlink_smart_battery_info_t, id) }, \ + { "battery_function", NULL, MAVLINK_TYPE_UINT8_T, 0, 19, offsetof(mavlink_smart_battery_info_t, battery_function) }, \ + { "type", NULL, MAVLINK_TYPE_UINT8_T, 0, 20, offsetof(mavlink_smart_battery_info_t, type) }, \ + { "capacity_full_specification", NULL, MAVLINK_TYPE_INT32_T, 0, 0, offsetof(mavlink_smart_battery_info_t, capacity_full_specification) }, \ + { "capacity_full", NULL, MAVLINK_TYPE_INT32_T, 0, 4, offsetof(mavlink_smart_battery_info_t, capacity_full) }, \ + { "cycle_count", NULL, MAVLINK_TYPE_UINT16_T, 0, 8, offsetof(mavlink_smart_battery_info_t, cycle_count) }, \ + { "serial_number", NULL, MAVLINK_TYPE_CHAR, 16, 21, offsetof(mavlink_smart_battery_info_t, serial_number) }, \ + { "device_name", NULL, MAVLINK_TYPE_CHAR, 50, 37, offsetof(mavlink_smart_battery_info_t, device_name) }, \ + { "weight", NULL, MAVLINK_TYPE_UINT16_T, 0, 10, offsetof(mavlink_smart_battery_info_t, weight) }, \ + { "discharge_minimum_voltage", NULL, MAVLINK_TYPE_UINT16_T, 0, 12, offsetof(mavlink_smart_battery_info_t, discharge_minimum_voltage) }, \ + { "charging_minimum_voltage", NULL, MAVLINK_TYPE_UINT16_T, 0, 14, offsetof(mavlink_smart_battery_info_t, charging_minimum_voltage) }, \ + { "resting_minimum_voltage", NULL, MAVLINK_TYPE_UINT16_T, 0, 16, offsetof(mavlink_smart_battery_info_t, resting_minimum_voltage) }, \ + } \ +} +#endif + +/** + * @brief Pack a smart_battery_info message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param id Battery ID + * @param battery_function Function of the battery + * @param type Type (chemistry) of the battery + * @param capacity_full_specification [mAh] Capacity when full according to manufacturer, -1: field not provided. + * @param capacity_full [mAh] Capacity when full (accounting for battery degradation), -1: field not provided. + * @param cycle_count Charge/discharge cycle count. UINT16_MAX: field not provided. + * @param serial_number Serial number in ASCII characters, 0 terminated. All 0: field not provided. + * @param device_name Static device name. Encode as manufacturer and product names separated using an underscore. + * @param weight [g] Battery weight. 0: field not provided. + * @param discharge_minimum_voltage [mV] Minimum per-cell voltage when discharging. If not supplied set to UINT16_MAX value. + * @param charging_minimum_voltage [mV] Minimum per-cell voltage when charging. If not supplied set to UINT16_MAX value. + * @param resting_minimum_voltage [mV] Minimum per-cell voltage when resting. If not supplied set to UINT16_MAX value. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_smart_battery_info_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint8_t id, uint8_t battery_function, uint8_t type, int32_t capacity_full_specification, int32_t capacity_full, uint16_t cycle_count, const char *serial_number, const char *device_name, uint16_t weight, uint16_t discharge_minimum_voltage, uint16_t charging_minimum_voltage, uint16_t resting_minimum_voltage) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_SMART_BATTERY_INFO_LEN]; + _mav_put_int32_t(buf, 0, capacity_full_specification); + _mav_put_int32_t(buf, 4, capacity_full); + _mav_put_uint16_t(buf, 8, cycle_count); + _mav_put_uint16_t(buf, 10, weight); + _mav_put_uint16_t(buf, 12, discharge_minimum_voltage); + _mav_put_uint16_t(buf, 14, charging_minimum_voltage); + _mav_put_uint16_t(buf, 16, resting_minimum_voltage); + _mav_put_uint8_t(buf, 18, id); + _mav_put_uint8_t(buf, 19, battery_function); + _mav_put_uint8_t(buf, 20, type); + _mav_put_char_array(buf, 21, serial_number, 16); + _mav_put_char_array(buf, 37, device_name, 50); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SMART_BATTERY_INFO_LEN); +#else + mavlink_smart_battery_info_t packet; + packet.capacity_full_specification = capacity_full_specification; + packet.capacity_full = capacity_full; + packet.cycle_count = cycle_count; + packet.weight = weight; + packet.discharge_minimum_voltage = discharge_minimum_voltage; + packet.charging_minimum_voltage = charging_minimum_voltage; + packet.resting_minimum_voltage = resting_minimum_voltage; + packet.id = id; + packet.battery_function = battery_function; + packet.type = type; + mav_array_memcpy(packet.serial_number, serial_number, sizeof(char)*16); + mav_array_memcpy(packet.device_name, device_name, sizeof(char)*50); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SMART_BATTERY_INFO_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_SMART_BATTERY_INFO; + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SMART_BATTERY_INFO_MIN_LEN, MAVLINK_MSG_ID_SMART_BATTERY_INFO_LEN, MAVLINK_MSG_ID_SMART_BATTERY_INFO_CRC); +} + +/** + * @brief Pack a smart_battery_info message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param id Battery ID + * @param battery_function Function of the battery + * @param type Type (chemistry) of the battery + * @param capacity_full_specification [mAh] Capacity when full according to manufacturer, -1: field not provided. + * @param capacity_full [mAh] Capacity when full (accounting for battery degradation), -1: field not provided. + * @param cycle_count Charge/discharge cycle count. UINT16_MAX: field not provided. + * @param serial_number Serial number in ASCII characters, 0 terminated. All 0: field not provided. + * @param device_name Static device name. Encode as manufacturer and product names separated using an underscore. + * @param weight [g] Battery weight. 0: field not provided. + * @param discharge_minimum_voltage [mV] Minimum per-cell voltage when discharging. If not supplied set to UINT16_MAX value. + * @param charging_minimum_voltage [mV] Minimum per-cell voltage when charging. If not supplied set to UINT16_MAX value. + * @param resting_minimum_voltage [mV] Minimum per-cell voltage when resting. If not supplied set to UINT16_MAX value. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_smart_battery_info_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint8_t id,uint8_t battery_function,uint8_t type,int32_t capacity_full_specification,int32_t capacity_full,uint16_t cycle_count,const char *serial_number,const char *device_name,uint16_t weight,uint16_t discharge_minimum_voltage,uint16_t charging_minimum_voltage,uint16_t resting_minimum_voltage) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_SMART_BATTERY_INFO_LEN]; + _mav_put_int32_t(buf, 0, capacity_full_specification); + _mav_put_int32_t(buf, 4, capacity_full); + _mav_put_uint16_t(buf, 8, cycle_count); + _mav_put_uint16_t(buf, 10, weight); + _mav_put_uint16_t(buf, 12, discharge_minimum_voltage); + _mav_put_uint16_t(buf, 14, charging_minimum_voltage); + _mav_put_uint16_t(buf, 16, resting_minimum_voltage); + _mav_put_uint8_t(buf, 18, id); + _mav_put_uint8_t(buf, 19, battery_function); + _mav_put_uint8_t(buf, 20, type); + _mav_put_char_array(buf, 21, serial_number, 16); + _mav_put_char_array(buf, 37, device_name, 50); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SMART_BATTERY_INFO_LEN); +#else + mavlink_smart_battery_info_t packet; + packet.capacity_full_specification = capacity_full_specification; + packet.capacity_full = capacity_full; + packet.cycle_count = cycle_count; + packet.weight = weight; + packet.discharge_minimum_voltage = discharge_minimum_voltage; + packet.charging_minimum_voltage = charging_minimum_voltage; + packet.resting_minimum_voltage = resting_minimum_voltage; + packet.id = id; + packet.battery_function = battery_function; + packet.type = type; + mav_array_memcpy(packet.serial_number, serial_number, sizeof(char)*16); + mav_array_memcpy(packet.device_name, device_name, sizeof(char)*50); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SMART_BATTERY_INFO_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_SMART_BATTERY_INFO; + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SMART_BATTERY_INFO_MIN_LEN, MAVLINK_MSG_ID_SMART_BATTERY_INFO_LEN, MAVLINK_MSG_ID_SMART_BATTERY_INFO_CRC); +} + +/** + * @brief Encode a smart_battery_info struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param smart_battery_info C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_smart_battery_info_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_smart_battery_info_t* smart_battery_info) +{ + return mavlink_msg_smart_battery_info_pack(system_id, component_id, msg, smart_battery_info->id, smart_battery_info->battery_function, smart_battery_info->type, smart_battery_info->capacity_full_specification, smart_battery_info->capacity_full, smart_battery_info->cycle_count, smart_battery_info->serial_number, smart_battery_info->device_name, smart_battery_info->weight, smart_battery_info->discharge_minimum_voltage, smart_battery_info->charging_minimum_voltage, smart_battery_info->resting_minimum_voltage); +} + +/** + * @brief Encode a smart_battery_info struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param smart_battery_info C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_smart_battery_info_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_smart_battery_info_t* smart_battery_info) +{ + return mavlink_msg_smart_battery_info_pack_chan(system_id, component_id, chan, msg, smart_battery_info->id, smart_battery_info->battery_function, smart_battery_info->type, smart_battery_info->capacity_full_specification, smart_battery_info->capacity_full, smart_battery_info->cycle_count, smart_battery_info->serial_number, smart_battery_info->device_name, smart_battery_info->weight, smart_battery_info->discharge_minimum_voltage, smart_battery_info->charging_minimum_voltage, smart_battery_info->resting_minimum_voltage); +} + +/** + * @brief Send a smart_battery_info message + * @param chan MAVLink channel to send the message + * + * @param id Battery ID + * @param battery_function Function of the battery + * @param type Type (chemistry) of the battery + * @param capacity_full_specification [mAh] Capacity when full according to manufacturer, -1: field not provided. + * @param capacity_full [mAh] Capacity when full (accounting for battery degradation), -1: field not provided. + * @param cycle_count Charge/discharge cycle count. UINT16_MAX: field not provided. + * @param serial_number Serial number in ASCII characters, 0 terminated. All 0: field not provided. + * @param device_name Static device name. Encode as manufacturer and product names separated using an underscore. + * @param weight [g] Battery weight. 0: field not provided. + * @param discharge_minimum_voltage [mV] Minimum per-cell voltage when discharging. If not supplied set to UINT16_MAX value. + * @param charging_minimum_voltage [mV] Minimum per-cell voltage when charging. If not supplied set to UINT16_MAX value. + * @param resting_minimum_voltage [mV] Minimum per-cell voltage when resting. If not supplied set to UINT16_MAX value. + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_smart_battery_info_send(mavlink_channel_t chan, uint8_t id, uint8_t battery_function, uint8_t type, int32_t capacity_full_specification, int32_t capacity_full, uint16_t cycle_count, const char *serial_number, const char *device_name, uint16_t weight, uint16_t discharge_minimum_voltage, uint16_t charging_minimum_voltage, uint16_t resting_minimum_voltage) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_SMART_BATTERY_INFO_LEN]; + _mav_put_int32_t(buf, 0, capacity_full_specification); + _mav_put_int32_t(buf, 4, capacity_full); + _mav_put_uint16_t(buf, 8, cycle_count); + _mav_put_uint16_t(buf, 10, weight); + _mav_put_uint16_t(buf, 12, discharge_minimum_voltage); + _mav_put_uint16_t(buf, 14, charging_minimum_voltage); + _mav_put_uint16_t(buf, 16, resting_minimum_voltage); + _mav_put_uint8_t(buf, 18, id); + _mav_put_uint8_t(buf, 19, battery_function); + _mav_put_uint8_t(buf, 20, type); + _mav_put_char_array(buf, 21, serial_number, 16); + _mav_put_char_array(buf, 37, device_name, 50); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SMART_BATTERY_INFO, buf, MAVLINK_MSG_ID_SMART_BATTERY_INFO_MIN_LEN, MAVLINK_MSG_ID_SMART_BATTERY_INFO_LEN, MAVLINK_MSG_ID_SMART_BATTERY_INFO_CRC); +#else + mavlink_smart_battery_info_t packet; + packet.capacity_full_specification = capacity_full_specification; + packet.capacity_full = capacity_full; + packet.cycle_count = cycle_count; + packet.weight = weight; + packet.discharge_minimum_voltage = discharge_minimum_voltage; + packet.charging_minimum_voltage = charging_minimum_voltage; + packet.resting_minimum_voltage = resting_minimum_voltage; + packet.id = id; + packet.battery_function = battery_function; + packet.type = type; + mav_array_memcpy(packet.serial_number, serial_number, sizeof(char)*16); + mav_array_memcpy(packet.device_name, device_name, sizeof(char)*50); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SMART_BATTERY_INFO, (const char *)&packet, MAVLINK_MSG_ID_SMART_BATTERY_INFO_MIN_LEN, MAVLINK_MSG_ID_SMART_BATTERY_INFO_LEN, MAVLINK_MSG_ID_SMART_BATTERY_INFO_CRC); +#endif +} + +/** + * @brief Send a smart_battery_info message + * @param chan MAVLink channel to send the message + * @param struct The MAVLink struct to serialize + */ +static inline void mavlink_msg_smart_battery_info_send_struct(mavlink_channel_t chan, const mavlink_smart_battery_info_t* smart_battery_info) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + mavlink_msg_smart_battery_info_send(chan, smart_battery_info->id, smart_battery_info->battery_function, smart_battery_info->type, smart_battery_info->capacity_full_specification, smart_battery_info->capacity_full, smart_battery_info->cycle_count, smart_battery_info->serial_number, smart_battery_info->device_name, smart_battery_info->weight, smart_battery_info->discharge_minimum_voltage, smart_battery_info->charging_minimum_voltage, smart_battery_info->resting_minimum_voltage); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SMART_BATTERY_INFO, (const char *)smart_battery_info, MAVLINK_MSG_ID_SMART_BATTERY_INFO_MIN_LEN, MAVLINK_MSG_ID_SMART_BATTERY_INFO_LEN, MAVLINK_MSG_ID_SMART_BATTERY_INFO_CRC); +#endif +} + +#if MAVLINK_MSG_ID_SMART_BATTERY_INFO_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_smart_battery_info_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t id, uint8_t battery_function, uint8_t type, int32_t capacity_full_specification, int32_t capacity_full, uint16_t cycle_count, const char *serial_number, const char *device_name, uint16_t weight, uint16_t discharge_minimum_voltage, uint16_t charging_minimum_voltage, uint16_t resting_minimum_voltage) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_int32_t(buf, 0, capacity_full_specification); + _mav_put_int32_t(buf, 4, capacity_full); + _mav_put_uint16_t(buf, 8, cycle_count); + _mav_put_uint16_t(buf, 10, weight); + _mav_put_uint16_t(buf, 12, discharge_minimum_voltage); + _mav_put_uint16_t(buf, 14, charging_minimum_voltage); + _mav_put_uint16_t(buf, 16, resting_minimum_voltage); + _mav_put_uint8_t(buf, 18, id); + _mav_put_uint8_t(buf, 19, battery_function); + _mav_put_uint8_t(buf, 20, type); + _mav_put_char_array(buf, 21, serial_number, 16); + _mav_put_char_array(buf, 37, device_name, 50); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SMART_BATTERY_INFO, buf, MAVLINK_MSG_ID_SMART_BATTERY_INFO_MIN_LEN, MAVLINK_MSG_ID_SMART_BATTERY_INFO_LEN, MAVLINK_MSG_ID_SMART_BATTERY_INFO_CRC); +#else + mavlink_smart_battery_info_t *packet = (mavlink_smart_battery_info_t *)msgbuf; + packet->capacity_full_specification = capacity_full_specification; + packet->capacity_full = capacity_full; + packet->cycle_count = cycle_count; + packet->weight = weight; + packet->discharge_minimum_voltage = discharge_minimum_voltage; + packet->charging_minimum_voltage = charging_minimum_voltage; + packet->resting_minimum_voltage = resting_minimum_voltage; + packet->id = id; + packet->battery_function = battery_function; + packet->type = type; + mav_array_memcpy(packet->serial_number, serial_number, sizeof(char)*16); + mav_array_memcpy(packet->device_name, device_name, sizeof(char)*50); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SMART_BATTERY_INFO, (const char *)packet, MAVLINK_MSG_ID_SMART_BATTERY_INFO_MIN_LEN, MAVLINK_MSG_ID_SMART_BATTERY_INFO_LEN, MAVLINK_MSG_ID_SMART_BATTERY_INFO_CRC); +#endif +} +#endif + +#endif + +// MESSAGE SMART_BATTERY_INFO UNPACKING + + +/** + * @brief Get field id from smart_battery_info message + * + * @return Battery ID + */ +static inline uint8_t mavlink_msg_smart_battery_info_get_id(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 18); +} + +/** + * @brief Get field battery_function from smart_battery_info message + * + * @return Function of the battery + */ +static inline uint8_t mavlink_msg_smart_battery_info_get_battery_function(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 19); +} + +/** + * @brief Get field type from smart_battery_info message + * + * @return Type (chemistry) of the battery + */ +static inline uint8_t mavlink_msg_smart_battery_info_get_type(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 20); +} + +/** + * @brief Get field capacity_full_specification from smart_battery_info message + * + * @return [mAh] Capacity when full according to manufacturer, -1: field not provided. + */ +static inline int32_t mavlink_msg_smart_battery_info_get_capacity_full_specification(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int32_t(msg, 0); +} + +/** + * @brief Get field capacity_full from smart_battery_info message + * + * @return [mAh] Capacity when full (accounting for battery degradation), -1: field not provided. + */ +static inline int32_t mavlink_msg_smart_battery_info_get_capacity_full(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int32_t(msg, 4); +} + +/** + * @brief Get field cycle_count from smart_battery_info message + * + * @return Charge/discharge cycle count. UINT16_MAX: field not provided. + */ +static inline uint16_t mavlink_msg_smart_battery_info_get_cycle_count(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 8); +} + +/** + * @brief Get field serial_number from smart_battery_info message + * + * @return Serial number in ASCII characters, 0 terminated. All 0: field not provided. + */ +static inline uint16_t mavlink_msg_smart_battery_info_get_serial_number(const mavlink_message_t* msg, char *serial_number) +{ + return _MAV_RETURN_char_array(msg, serial_number, 16, 21); +} + +/** + * @brief Get field device_name from smart_battery_info message + * + * @return Static device name. Encode as manufacturer and product names separated using an underscore. + */ +static inline uint16_t mavlink_msg_smart_battery_info_get_device_name(const mavlink_message_t* msg, char *device_name) +{ + return _MAV_RETURN_char_array(msg, device_name, 50, 37); +} + +/** + * @brief Get field weight from smart_battery_info message + * + * @return [g] Battery weight. 0: field not provided. + */ +static inline uint16_t mavlink_msg_smart_battery_info_get_weight(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 10); +} + +/** + * @brief Get field discharge_minimum_voltage from smart_battery_info message + * + * @return [mV] Minimum per-cell voltage when discharging. If not supplied set to UINT16_MAX value. + */ +static inline uint16_t mavlink_msg_smart_battery_info_get_discharge_minimum_voltage(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 12); +} + +/** + * @brief Get field charging_minimum_voltage from smart_battery_info message + * + * @return [mV] Minimum per-cell voltage when charging. If not supplied set to UINT16_MAX value. + */ +static inline uint16_t mavlink_msg_smart_battery_info_get_charging_minimum_voltage(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 14); +} + +/** + * @brief Get field resting_minimum_voltage from smart_battery_info message + * + * @return [mV] Minimum per-cell voltage when resting. If not supplied set to UINT16_MAX value. + */ +static inline uint16_t mavlink_msg_smart_battery_info_get_resting_minimum_voltage(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 16); +} + +/** + * @brief Decode a smart_battery_info message into a struct + * + * @param msg The message to decode + * @param smart_battery_info C-struct to decode the message contents into + */ +static inline void mavlink_msg_smart_battery_info_decode(const mavlink_message_t* msg, mavlink_smart_battery_info_t* smart_battery_info) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + smart_battery_info->capacity_full_specification = mavlink_msg_smart_battery_info_get_capacity_full_specification(msg); + smart_battery_info->capacity_full = mavlink_msg_smart_battery_info_get_capacity_full(msg); + smart_battery_info->cycle_count = mavlink_msg_smart_battery_info_get_cycle_count(msg); + smart_battery_info->weight = mavlink_msg_smart_battery_info_get_weight(msg); + smart_battery_info->discharge_minimum_voltage = mavlink_msg_smart_battery_info_get_discharge_minimum_voltage(msg); + smart_battery_info->charging_minimum_voltage = mavlink_msg_smart_battery_info_get_charging_minimum_voltage(msg); + smart_battery_info->resting_minimum_voltage = mavlink_msg_smart_battery_info_get_resting_minimum_voltage(msg); + smart_battery_info->id = mavlink_msg_smart_battery_info_get_id(msg); + smart_battery_info->battery_function = mavlink_msg_smart_battery_info_get_battery_function(msg); + smart_battery_info->type = mavlink_msg_smart_battery_info_get_type(msg); + mavlink_msg_smart_battery_info_get_serial_number(msg, smart_battery_info->serial_number); + mavlink_msg_smart_battery_info_get_device_name(msg, smart_battery_info->device_name); +#else + uint8_t len = msg->len < MAVLINK_MSG_ID_SMART_BATTERY_INFO_LEN? msg->len : MAVLINK_MSG_ID_SMART_BATTERY_INFO_LEN; + memset(smart_battery_info, 0, MAVLINK_MSG_ID_SMART_BATTERY_INFO_LEN); + memcpy(smart_battery_info, _MAV_PAYLOAD(msg), len); +#endif +} diff --git a/lib/main/MAVLink/common/mavlink_msg_statustext.h b/lib/main/MAVLink/common/mavlink_msg_statustext.h index 2193fac839..28e0c0b0fd 100755 --- a/lib/main/MAVLink/common/mavlink_msg_statustext.h +++ b/lib/main/MAVLink/common/mavlink_msg_statustext.h @@ -3,15 +3,17 @@ #define MAVLINK_MSG_ID_STATUSTEXT 253 - +MAVPACKED( typedef struct __mavlink_statustext_t { uint8_t severity; /*< Severity of status. Relies on the definitions within RFC-5424.*/ char text[50]; /*< Status text message, without null termination character*/ -} mavlink_statustext_t; + uint16_t id; /*< Unique (opaque) identifier for this statustext message. May be used to reassemble a logical long-statustext message from a sequence of chunks. A value of zero indicates this is the only chunk in the sequence and the message can be emitted immediately.*/ + uint8_t chunk_seq; /*< This chunk's sequence number; indexing is from zero. Any null character in the text field is taken to mean this was the last chunk.*/ +}) mavlink_statustext_t; -#define MAVLINK_MSG_ID_STATUSTEXT_LEN 51 +#define MAVLINK_MSG_ID_STATUSTEXT_LEN 54 #define MAVLINK_MSG_ID_STATUSTEXT_MIN_LEN 51 -#define MAVLINK_MSG_ID_253_LEN 51 +#define MAVLINK_MSG_ID_253_LEN 54 #define MAVLINK_MSG_ID_253_MIN_LEN 51 #define MAVLINK_MSG_ID_STATUSTEXT_CRC 83 @@ -23,17 +25,21 @@ typedef struct __mavlink_statustext_t { #define MAVLINK_MESSAGE_INFO_STATUSTEXT { \ 253, \ "STATUSTEXT", \ - 2, \ + 4, \ { { "severity", NULL, MAVLINK_TYPE_UINT8_T, 0, 0, offsetof(mavlink_statustext_t, severity) }, \ { "text", NULL, MAVLINK_TYPE_CHAR, 50, 1, offsetof(mavlink_statustext_t, text) }, \ + { "id", NULL, MAVLINK_TYPE_UINT16_T, 0, 51, offsetof(mavlink_statustext_t, id) }, \ + { "chunk_seq", NULL, MAVLINK_TYPE_UINT8_T, 0, 53, offsetof(mavlink_statustext_t, chunk_seq) }, \ } \ } #else #define MAVLINK_MESSAGE_INFO_STATUSTEXT { \ "STATUSTEXT", \ - 2, \ + 4, \ { { "severity", NULL, MAVLINK_TYPE_UINT8_T, 0, 0, offsetof(mavlink_statustext_t, severity) }, \ { "text", NULL, MAVLINK_TYPE_CHAR, 50, 1, offsetof(mavlink_statustext_t, text) }, \ + { "id", NULL, MAVLINK_TYPE_UINT16_T, 0, 51, offsetof(mavlink_statustext_t, id) }, \ + { "chunk_seq", NULL, MAVLINK_TYPE_UINT8_T, 0, 53, offsetof(mavlink_statustext_t, chunk_seq) }, \ } \ } #endif @@ -46,19 +52,25 @@ typedef struct __mavlink_statustext_t { * * @param severity Severity of status. Relies on the definitions within RFC-5424. * @param text Status text message, without null termination character + * @param id Unique (opaque) identifier for this statustext message. May be used to reassemble a logical long-statustext message from a sequence of chunks. A value of zero indicates this is the only chunk in the sequence and the message can be emitted immediately. + * @param chunk_seq This chunk's sequence number; indexing is from zero. Any null character in the text field is taken to mean this was the last chunk. * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_statustext_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint8_t severity, const char *text) + uint8_t severity, const char *text, uint16_t id, uint8_t chunk_seq) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_STATUSTEXT_LEN]; _mav_put_uint8_t(buf, 0, severity); + _mav_put_uint16_t(buf, 51, id); + _mav_put_uint8_t(buf, 53, chunk_seq); _mav_put_char_array(buf, 1, text, 50); memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_STATUSTEXT_LEN); #else mavlink_statustext_t packet; packet.severity = severity; + packet.id = id; + packet.chunk_seq = chunk_seq; mav_array_memcpy(packet.text, text, sizeof(char)*50); memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_STATUSTEXT_LEN); #endif @@ -75,20 +87,26 @@ static inline uint16_t mavlink_msg_statustext_pack(uint8_t system_id, uint8_t co * @param msg The MAVLink message to compress the data into * @param severity Severity of status. Relies on the definitions within RFC-5424. * @param text Status text message, without null termination character + * @param id Unique (opaque) identifier for this statustext message. May be used to reassemble a logical long-statustext message from a sequence of chunks. A value of zero indicates this is the only chunk in the sequence and the message can be emitted immediately. + * @param chunk_seq This chunk's sequence number; indexing is from zero. Any null character in the text field is taken to mean this was the last chunk. * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_statustext_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, - uint8_t severity,const char *text) + uint8_t severity,const char *text,uint16_t id,uint8_t chunk_seq) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_STATUSTEXT_LEN]; _mav_put_uint8_t(buf, 0, severity); + _mav_put_uint16_t(buf, 51, id); + _mav_put_uint8_t(buf, 53, chunk_seq); _mav_put_char_array(buf, 1, text, 50); memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_STATUSTEXT_LEN); #else mavlink_statustext_t packet; packet.severity = severity; + packet.id = id; + packet.chunk_seq = chunk_seq; mav_array_memcpy(packet.text, text, sizeof(char)*50); memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_STATUSTEXT_LEN); #endif @@ -107,7 +125,7 @@ static inline uint16_t mavlink_msg_statustext_pack_chan(uint8_t system_id, uint8 */ static inline uint16_t mavlink_msg_statustext_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_statustext_t* statustext) { - return mavlink_msg_statustext_pack(system_id, component_id, msg, statustext->severity, statustext->text); + return mavlink_msg_statustext_pack(system_id, component_id, msg, statustext->severity, statustext->text, statustext->id, statustext->chunk_seq); } /** @@ -121,7 +139,7 @@ static inline uint16_t mavlink_msg_statustext_encode(uint8_t system_id, uint8_t */ static inline uint16_t mavlink_msg_statustext_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_statustext_t* statustext) { - return mavlink_msg_statustext_pack_chan(system_id, component_id, chan, msg, statustext->severity, statustext->text); + return mavlink_msg_statustext_pack_chan(system_id, component_id, chan, msg, statustext->severity, statustext->text, statustext->id, statustext->chunk_seq); } /** @@ -130,19 +148,25 @@ static inline uint16_t mavlink_msg_statustext_encode_chan(uint8_t system_id, uin * * @param severity Severity of status. Relies on the definitions within RFC-5424. * @param text Status text message, without null termination character + * @param id Unique (opaque) identifier for this statustext message. May be used to reassemble a logical long-statustext message from a sequence of chunks. A value of zero indicates this is the only chunk in the sequence and the message can be emitted immediately. + * @param chunk_seq This chunk's sequence number; indexing is from zero. Any null character in the text field is taken to mean this was the last chunk. */ #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS -static inline void mavlink_msg_statustext_send(mavlink_channel_t chan, uint8_t severity, const char *text) +static inline void mavlink_msg_statustext_send(mavlink_channel_t chan, uint8_t severity, const char *text, uint16_t id, uint8_t chunk_seq) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_STATUSTEXT_LEN]; _mav_put_uint8_t(buf, 0, severity); + _mav_put_uint16_t(buf, 51, id); + _mav_put_uint8_t(buf, 53, chunk_seq); _mav_put_char_array(buf, 1, text, 50); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_STATUSTEXT, buf, MAVLINK_MSG_ID_STATUSTEXT_MIN_LEN, MAVLINK_MSG_ID_STATUSTEXT_LEN, MAVLINK_MSG_ID_STATUSTEXT_CRC); #else mavlink_statustext_t packet; packet.severity = severity; + packet.id = id; + packet.chunk_seq = chunk_seq; mav_array_memcpy(packet.text, text, sizeof(char)*50); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_STATUSTEXT, (const char *)&packet, MAVLINK_MSG_ID_STATUSTEXT_MIN_LEN, MAVLINK_MSG_ID_STATUSTEXT_LEN, MAVLINK_MSG_ID_STATUSTEXT_CRC); #endif @@ -156,7 +180,7 @@ static inline void mavlink_msg_statustext_send(mavlink_channel_t chan, uint8_t s static inline void mavlink_msg_statustext_send_struct(mavlink_channel_t chan, const mavlink_statustext_t* statustext) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - mavlink_msg_statustext_send(chan, statustext->severity, statustext->text); + mavlink_msg_statustext_send(chan, statustext->severity, statustext->text, statustext->id, statustext->chunk_seq); #else _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_STATUSTEXT, (const char *)statustext, MAVLINK_MSG_ID_STATUSTEXT_MIN_LEN, MAVLINK_MSG_ID_STATUSTEXT_LEN, MAVLINK_MSG_ID_STATUSTEXT_CRC); #endif @@ -170,16 +194,20 @@ static inline void mavlink_msg_statustext_send_struct(mavlink_channel_t chan, co is usually the receive buffer for the channel, and allows a reply to an incoming message with minimum stack space usage. */ -static inline void mavlink_msg_statustext_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t severity, const char *text) +static inline void mavlink_msg_statustext_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t severity, const char *text, uint16_t id, uint8_t chunk_seq) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char *buf = (char *)msgbuf; _mav_put_uint8_t(buf, 0, severity); + _mav_put_uint16_t(buf, 51, id); + _mav_put_uint8_t(buf, 53, chunk_seq); _mav_put_char_array(buf, 1, text, 50); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_STATUSTEXT, buf, MAVLINK_MSG_ID_STATUSTEXT_MIN_LEN, MAVLINK_MSG_ID_STATUSTEXT_LEN, MAVLINK_MSG_ID_STATUSTEXT_CRC); #else mavlink_statustext_t *packet = (mavlink_statustext_t *)msgbuf; packet->severity = severity; + packet->id = id; + packet->chunk_seq = chunk_seq; mav_array_memcpy(packet->text, text, sizeof(char)*50); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_STATUSTEXT, (const char *)packet, MAVLINK_MSG_ID_STATUSTEXT_MIN_LEN, MAVLINK_MSG_ID_STATUSTEXT_LEN, MAVLINK_MSG_ID_STATUSTEXT_CRC); #endif @@ -211,6 +239,26 @@ static inline uint16_t mavlink_msg_statustext_get_text(const mavlink_message_t* return _MAV_RETURN_char_array(msg, text, 50, 1); } +/** + * @brief Get field id from statustext message + * + * @return Unique (opaque) identifier for this statustext message. May be used to reassemble a logical long-statustext message from a sequence of chunks. A value of zero indicates this is the only chunk in the sequence and the message can be emitted immediately. + */ +static inline uint16_t mavlink_msg_statustext_get_id(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 51); +} + +/** + * @brief Get field chunk_seq from statustext message + * + * @return This chunk's sequence number; indexing is from zero. Any null character in the text field is taken to mean this was the last chunk. + */ +static inline uint8_t mavlink_msg_statustext_get_chunk_seq(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 53); +} + /** * @brief Decode a statustext message into a struct * @@ -222,6 +270,8 @@ static inline void mavlink_msg_statustext_decode(const mavlink_message_t* msg, m #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS statustext->severity = mavlink_msg_statustext_get_severity(msg); mavlink_msg_statustext_get_text(msg, statustext->text); + statustext->id = mavlink_msg_statustext_get_id(msg); + statustext->chunk_seq = mavlink_msg_statustext_get_chunk_seq(msg); #else uint8_t len = msg->len < MAVLINK_MSG_ID_STATUSTEXT_LEN? msg->len : MAVLINK_MSG_ID_STATUSTEXT_LEN; memset(statustext, 0, MAVLINK_MSG_ID_STATUSTEXT_LEN); diff --git a/lib/main/MAVLink/common/mavlink_msg_storage_information.h b/lib/main/MAVLink/common/mavlink_msg_storage_information.h new file mode 100644 index 0000000000..76d8afd57b --- /dev/null +++ b/lib/main/MAVLink/common/mavlink_msg_storage_information.h @@ -0,0 +1,455 @@ +#pragma once +// MESSAGE STORAGE_INFORMATION PACKING + +#define MAVLINK_MSG_ID_STORAGE_INFORMATION 261 + + +typedef struct __mavlink_storage_information_t { + uint32_t time_boot_ms; /*< [ms] Timestamp (time since system boot).*/ + float total_capacity; /*< [MiB] Total capacity. If storage is not ready (STORAGE_STATUS_READY) value will be ignored.*/ + float used_capacity; /*< [MiB] Used capacity. If storage is not ready (STORAGE_STATUS_READY) value will be ignored.*/ + float available_capacity; /*< [MiB] Available storage capacity. If storage is not ready (STORAGE_STATUS_READY) value will be ignored.*/ + float read_speed; /*< [MiB/s] Read speed.*/ + float write_speed; /*< [MiB/s] Write speed.*/ + uint8_t storage_id; /*< Storage ID (1 for first, 2 for second, etc.)*/ + uint8_t storage_count; /*< Number of storage devices*/ + uint8_t status; /*< Status of storage*/ + uint8_t type; /*< Type of storage*/ + char name[32]; /*< Textual storage name to be used in UI (microSD 1, Internal Memory, etc.) This is a NULL terminated string. If it is exactly 32 characters long, add a terminating NULL. If this string is empty, the generic type is shown to the user.*/ +} mavlink_storage_information_t; + +#define MAVLINK_MSG_ID_STORAGE_INFORMATION_LEN 60 +#define MAVLINK_MSG_ID_STORAGE_INFORMATION_MIN_LEN 27 +#define MAVLINK_MSG_ID_261_LEN 60 +#define MAVLINK_MSG_ID_261_MIN_LEN 27 + +#define MAVLINK_MSG_ID_STORAGE_INFORMATION_CRC 179 +#define MAVLINK_MSG_ID_261_CRC 179 + +#define MAVLINK_MSG_STORAGE_INFORMATION_FIELD_NAME_LEN 32 + +#if MAVLINK_COMMAND_24BIT +#define MAVLINK_MESSAGE_INFO_STORAGE_INFORMATION { \ + 261, \ + "STORAGE_INFORMATION", \ + 11, \ + { { "time_boot_ms", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_storage_information_t, time_boot_ms) }, \ + { "storage_id", NULL, MAVLINK_TYPE_UINT8_T, 0, 24, offsetof(mavlink_storage_information_t, storage_id) }, \ + { "storage_count", NULL, MAVLINK_TYPE_UINT8_T, 0, 25, offsetof(mavlink_storage_information_t, storage_count) }, \ + { "status", NULL, MAVLINK_TYPE_UINT8_T, 0, 26, offsetof(mavlink_storage_information_t, status) }, \ + { "total_capacity", NULL, MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_storage_information_t, total_capacity) }, \ + { "used_capacity", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_storage_information_t, used_capacity) }, \ + { "available_capacity", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_storage_information_t, available_capacity) }, \ + { "read_speed", NULL, MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_storage_information_t, read_speed) }, \ + { "write_speed", NULL, MAVLINK_TYPE_FLOAT, 0, 20, offsetof(mavlink_storage_information_t, write_speed) }, \ + { "type", NULL, MAVLINK_TYPE_UINT8_T, 0, 27, offsetof(mavlink_storage_information_t, type) }, \ + { "name", NULL, MAVLINK_TYPE_CHAR, 32, 28, offsetof(mavlink_storage_information_t, name) }, \ + } \ +} +#else +#define MAVLINK_MESSAGE_INFO_STORAGE_INFORMATION { \ + "STORAGE_INFORMATION", \ + 11, \ + { { "time_boot_ms", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_storage_information_t, time_boot_ms) }, \ + { "storage_id", NULL, MAVLINK_TYPE_UINT8_T, 0, 24, offsetof(mavlink_storage_information_t, storage_id) }, \ + { "storage_count", NULL, MAVLINK_TYPE_UINT8_T, 0, 25, offsetof(mavlink_storage_information_t, storage_count) }, \ + { "status", NULL, MAVLINK_TYPE_UINT8_T, 0, 26, offsetof(mavlink_storage_information_t, status) }, \ + { "total_capacity", NULL, MAVLINK_TYPE_FLOAT, 0, 4, offsetof(mavlink_storage_information_t, total_capacity) }, \ + { "used_capacity", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_storage_information_t, used_capacity) }, \ + { "available_capacity", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_storage_information_t, available_capacity) }, \ + { "read_speed", NULL, MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_storage_information_t, read_speed) }, \ + { "write_speed", NULL, MAVLINK_TYPE_FLOAT, 0, 20, offsetof(mavlink_storage_information_t, write_speed) }, \ + { "type", NULL, MAVLINK_TYPE_UINT8_T, 0, 27, offsetof(mavlink_storage_information_t, type) }, \ + { "name", NULL, MAVLINK_TYPE_CHAR, 32, 28, offsetof(mavlink_storage_information_t, name) }, \ + } \ +} +#endif + +/** + * @brief Pack a storage_information message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param time_boot_ms [ms] Timestamp (time since system boot). + * @param storage_id Storage ID (1 for first, 2 for second, etc.) + * @param storage_count Number of storage devices + * @param status Status of storage + * @param total_capacity [MiB] Total capacity. If storage is not ready (STORAGE_STATUS_READY) value will be ignored. + * @param used_capacity [MiB] Used capacity. If storage is not ready (STORAGE_STATUS_READY) value will be ignored. + * @param available_capacity [MiB] Available storage capacity. If storage is not ready (STORAGE_STATUS_READY) value will be ignored. + * @param read_speed [MiB/s] Read speed. + * @param write_speed [MiB/s] Write speed. + * @param type Type of storage + * @param name Textual storage name to be used in UI (microSD 1, Internal Memory, etc.) This is a NULL terminated string. If it is exactly 32 characters long, add a terminating NULL. If this string is empty, the generic type is shown to the user. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_storage_information_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint32_t time_boot_ms, uint8_t storage_id, uint8_t storage_count, uint8_t status, float total_capacity, float used_capacity, float available_capacity, float read_speed, float write_speed, uint8_t type, const char *name) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_STORAGE_INFORMATION_LEN]; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_float(buf, 4, total_capacity); + _mav_put_float(buf, 8, used_capacity); + _mav_put_float(buf, 12, available_capacity); + _mav_put_float(buf, 16, read_speed); + _mav_put_float(buf, 20, write_speed); + _mav_put_uint8_t(buf, 24, storage_id); + _mav_put_uint8_t(buf, 25, storage_count); + _mav_put_uint8_t(buf, 26, status); + _mav_put_uint8_t(buf, 27, type); + _mav_put_char_array(buf, 28, name, 32); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_STORAGE_INFORMATION_LEN); +#else + mavlink_storage_information_t packet; + packet.time_boot_ms = time_boot_ms; + packet.total_capacity = total_capacity; + packet.used_capacity = used_capacity; + packet.available_capacity = available_capacity; + packet.read_speed = read_speed; + packet.write_speed = write_speed; + packet.storage_id = storage_id; + packet.storage_count = storage_count; + packet.status = status; + packet.type = type; + mav_array_memcpy(packet.name, name, sizeof(char)*32); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_STORAGE_INFORMATION_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_STORAGE_INFORMATION; + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_STORAGE_INFORMATION_MIN_LEN, MAVLINK_MSG_ID_STORAGE_INFORMATION_LEN, MAVLINK_MSG_ID_STORAGE_INFORMATION_CRC); +} + +/** + * @brief Pack a storage_information message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param time_boot_ms [ms] Timestamp (time since system boot). + * @param storage_id Storage ID (1 for first, 2 for second, etc.) + * @param storage_count Number of storage devices + * @param status Status of storage + * @param total_capacity [MiB] Total capacity. If storage is not ready (STORAGE_STATUS_READY) value will be ignored. + * @param used_capacity [MiB] Used capacity. If storage is not ready (STORAGE_STATUS_READY) value will be ignored. + * @param available_capacity [MiB] Available storage capacity. If storage is not ready (STORAGE_STATUS_READY) value will be ignored. + * @param read_speed [MiB/s] Read speed. + * @param write_speed [MiB/s] Write speed. + * @param type Type of storage + * @param name Textual storage name to be used in UI (microSD 1, Internal Memory, etc.) This is a NULL terminated string. If it is exactly 32 characters long, add a terminating NULL. If this string is empty, the generic type is shown to the user. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_storage_information_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint32_t time_boot_ms,uint8_t storage_id,uint8_t storage_count,uint8_t status,float total_capacity,float used_capacity,float available_capacity,float read_speed,float write_speed,uint8_t type,const char *name) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_STORAGE_INFORMATION_LEN]; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_float(buf, 4, total_capacity); + _mav_put_float(buf, 8, used_capacity); + _mav_put_float(buf, 12, available_capacity); + _mav_put_float(buf, 16, read_speed); + _mav_put_float(buf, 20, write_speed); + _mav_put_uint8_t(buf, 24, storage_id); + _mav_put_uint8_t(buf, 25, storage_count); + _mav_put_uint8_t(buf, 26, status); + _mav_put_uint8_t(buf, 27, type); + _mav_put_char_array(buf, 28, name, 32); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_STORAGE_INFORMATION_LEN); +#else + mavlink_storage_information_t packet; + packet.time_boot_ms = time_boot_ms; + packet.total_capacity = total_capacity; + packet.used_capacity = used_capacity; + packet.available_capacity = available_capacity; + packet.read_speed = read_speed; + packet.write_speed = write_speed; + packet.storage_id = storage_id; + packet.storage_count = storage_count; + packet.status = status; + packet.type = type; + mav_array_memcpy(packet.name, name, sizeof(char)*32); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_STORAGE_INFORMATION_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_STORAGE_INFORMATION; + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_STORAGE_INFORMATION_MIN_LEN, MAVLINK_MSG_ID_STORAGE_INFORMATION_LEN, MAVLINK_MSG_ID_STORAGE_INFORMATION_CRC); +} + +/** + * @brief Encode a storage_information struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param storage_information C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_storage_information_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_storage_information_t* storage_information) +{ + return mavlink_msg_storage_information_pack(system_id, component_id, msg, storage_information->time_boot_ms, storage_information->storage_id, storage_information->storage_count, storage_information->status, storage_information->total_capacity, storage_information->used_capacity, storage_information->available_capacity, storage_information->read_speed, storage_information->write_speed, storage_information->type, storage_information->name); +} + +/** + * @brief Encode a storage_information struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param storage_information C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_storage_information_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_storage_information_t* storage_information) +{ + return mavlink_msg_storage_information_pack_chan(system_id, component_id, chan, msg, storage_information->time_boot_ms, storage_information->storage_id, storage_information->storage_count, storage_information->status, storage_information->total_capacity, storage_information->used_capacity, storage_information->available_capacity, storage_information->read_speed, storage_information->write_speed, storage_information->type, storage_information->name); +} + +/** + * @brief Send a storage_information message + * @param chan MAVLink channel to send the message + * + * @param time_boot_ms [ms] Timestamp (time since system boot). + * @param storage_id Storage ID (1 for first, 2 for second, etc.) + * @param storage_count Number of storage devices + * @param status Status of storage + * @param total_capacity [MiB] Total capacity. If storage is not ready (STORAGE_STATUS_READY) value will be ignored. + * @param used_capacity [MiB] Used capacity. If storage is not ready (STORAGE_STATUS_READY) value will be ignored. + * @param available_capacity [MiB] Available storage capacity. If storage is not ready (STORAGE_STATUS_READY) value will be ignored. + * @param read_speed [MiB/s] Read speed. + * @param write_speed [MiB/s] Write speed. + * @param type Type of storage + * @param name Textual storage name to be used in UI (microSD 1, Internal Memory, etc.) This is a NULL terminated string. If it is exactly 32 characters long, add a terminating NULL. If this string is empty, the generic type is shown to the user. + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_storage_information_send(mavlink_channel_t chan, uint32_t time_boot_ms, uint8_t storage_id, uint8_t storage_count, uint8_t status, float total_capacity, float used_capacity, float available_capacity, float read_speed, float write_speed, uint8_t type, const char *name) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_STORAGE_INFORMATION_LEN]; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_float(buf, 4, total_capacity); + _mav_put_float(buf, 8, used_capacity); + _mav_put_float(buf, 12, available_capacity); + _mav_put_float(buf, 16, read_speed); + _mav_put_float(buf, 20, write_speed); + _mav_put_uint8_t(buf, 24, storage_id); + _mav_put_uint8_t(buf, 25, storage_count); + _mav_put_uint8_t(buf, 26, status); + _mav_put_uint8_t(buf, 27, type); + _mav_put_char_array(buf, 28, name, 32); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_STORAGE_INFORMATION, buf, MAVLINK_MSG_ID_STORAGE_INFORMATION_MIN_LEN, MAVLINK_MSG_ID_STORAGE_INFORMATION_LEN, MAVLINK_MSG_ID_STORAGE_INFORMATION_CRC); +#else + mavlink_storage_information_t packet; + packet.time_boot_ms = time_boot_ms; + packet.total_capacity = total_capacity; + packet.used_capacity = used_capacity; + packet.available_capacity = available_capacity; + packet.read_speed = read_speed; + packet.write_speed = write_speed; + packet.storage_id = storage_id; + packet.storage_count = storage_count; + packet.status = status; + packet.type = type; + mav_array_memcpy(packet.name, name, sizeof(char)*32); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_STORAGE_INFORMATION, (const char *)&packet, MAVLINK_MSG_ID_STORAGE_INFORMATION_MIN_LEN, MAVLINK_MSG_ID_STORAGE_INFORMATION_LEN, MAVLINK_MSG_ID_STORAGE_INFORMATION_CRC); +#endif +} + +/** + * @brief Send a storage_information message + * @param chan MAVLink channel to send the message + * @param struct The MAVLink struct to serialize + */ +static inline void mavlink_msg_storage_information_send_struct(mavlink_channel_t chan, const mavlink_storage_information_t* storage_information) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + mavlink_msg_storage_information_send(chan, storage_information->time_boot_ms, storage_information->storage_id, storage_information->storage_count, storage_information->status, storage_information->total_capacity, storage_information->used_capacity, storage_information->available_capacity, storage_information->read_speed, storage_information->write_speed, storage_information->type, storage_information->name); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_STORAGE_INFORMATION, (const char *)storage_information, MAVLINK_MSG_ID_STORAGE_INFORMATION_MIN_LEN, MAVLINK_MSG_ID_STORAGE_INFORMATION_LEN, MAVLINK_MSG_ID_STORAGE_INFORMATION_CRC); +#endif +} + +#if MAVLINK_MSG_ID_STORAGE_INFORMATION_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_storage_information_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint32_t time_boot_ms, uint8_t storage_id, uint8_t storage_count, uint8_t status, float total_capacity, float used_capacity, float available_capacity, float read_speed, float write_speed, uint8_t type, const char *name) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint32_t(buf, 0, time_boot_ms); + _mav_put_float(buf, 4, total_capacity); + _mav_put_float(buf, 8, used_capacity); + _mav_put_float(buf, 12, available_capacity); + _mav_put_float(buf, 16, read_speed); + _mav_put_float(buf, 20, write_speed); + _mav_put_uint8_t(buf, 24, storage_id); + _mav_put_uint8_t(buf, 25, storage_count); + _mav_put_uint8_t(buf, 26, status); + _mav_put_uint8_t(buf, 27, type); + _mav_put_char_array(buf, 28, name, 32); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_STORAGE_INFORMATION, buf, MAVLINK_MSG_ID_STORAGE_INFORMATION_MIN_LEN, MAVLINK_MSG_ID_STORAGE_INFORMATION_LEN, MAVLINK_MSG_ID_STORAGE_INFORMATION_CRC); +#else + mavlink_storage_information_t *packet = (mavlink_storage_information_t *)msgbuf; + packet->time_boot_ms = time_boot_ms; + packet->total_capacity = total_capacity; + packet->used_capacity = used_capacity; + packet->available_capacity = available_capacity; + packet->read_speed = read_speed; + packet->write_speed = write_speed; + packet->storage_id = storage_id; + packet->storage_count = storage_count; + packet->status = status; + packet->type = type; + mav_array_memcpy(packet->name, name, sizeof(char)*32); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_STORAGE_INFORMATION, (const char *)packet, MAVLINK_MSG_ID_STORAGE_INFORMATION_MIN_LEN, MAVLINK_MSG_ID_STORAGE_INFORMATION_LEN, MAVLINK_MSG_ID_STORAGE_INFORMATION_CRC); +#endif +} +#endif + +#endif + +// MESSAGE STORAGE_INFORMATION UNPACKING + + +/** + * @brief Get field time_boot_ms from storage_information message + * + * @return [ms] Timestamp (time since system boot). + */ +static inline uint32_t mavlink_msg_storage_information_get_time_boot_ms(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint32_t(msg, 0); +} + +/** + * @brief Get field storage_id from storage_information message + * + * @return Storage ID (1 for first, 2 for second, etc.) + */ +static inline uint8_t mavlink_msg_storage_information_get_storage_id(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 24); +} + +/** + * @brief Get field storage_count from storage_information message + * + * @return Number of storage devices + */ +static inline uint8_t mavlink_msg_storage_information_get_storage_count(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 25); +} + +/** + * @brief Get field status from storage_information message + * + * @return Status of storage + */ +static inline uint8_t mavlink_msg_storage_information_get_status(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 26); +} + +/** + * @brief Get field total_capacity from storage_information message + * + * @return [MiB] Total capacity. If storage is not ready (STORAGE_STATUS_READY) value will be ignored. + */ +static inline float mavlink_msg_storage_information_get_total_capacity(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 4); +} + +/** + * @brief Get field used_capacity from storage_information message + * + * @return [MiB] Used capacity. If storage is not ready (STORAGE_STATUS_READY) value will be ignored. + */ +static inline float mavlink_msg_storage_information_get_used_capacity(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 8); +} + +/** + * @brief Get field available_capacity from storage_information message + * + * @return [MiB] Available storage capacity. If storage is not ready (STORAGE_STATUS_READY) value will be ignored. + */ +static inline float mavlink_msg_storage_information_get_available_capacity(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 12); +} + +/** + * @brief Get field read_speed from storage_information message + * + * @return [MiB/s] Read speed. + */ +static inline float mavlink_msg_storage_information_get_read_speed(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 16); +} + +/** + * @brief Get field write_speed from storage_information message + * + * @return [MiB/s] Write speed. + */ +static inline float mavlink_msg_storage_information_get_write_speed(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 20); +} + +/** + * @brief Get field type from storage_information message + * + * @return Type of storage + */ +static inline uint8_t mavlink_msg_storage_information_get_type(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 27); +} + +/** + * @brief Get field name from storage_information message + * + * @return Textual storage name to be used in UI (microSD 1, Internal Memory, etc.) This is a NULL terminated string. If it is exactly 32 characters long, add a terminating NULL. If this string is empty, the generic type is shown to the user. + */ +static inline uint16_t mavlink_msg_storage_information_get_name(const mavlink_message_t* msg, char *name) +{ + return _MAV_RETURN_char_array(msg, name, 32, 28); +} + +/** + * @brief Decode a storage_information message into a struct + * + * @param msg The message to decode + * @param storage_information C-struct to decode the message contents into + */ +static inline void mavlink_msg_storage_information_decode(const mavlink_message_t* msg, mavlink_storage_information_t* storage_information) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + storage_information->time_boot_ms = mavlink_msg_storage_information_get_time_boot_ms(msg); + storage_information->total_capacity = mavlink_msg_storage_information_get_total_capacity(msg); + storage_information->used_capacity = mavlink_msg_storage_information_get_used_capacity(msg); + storage_information->available_capacity = mavlink_msg_storage_information_get_available_capacity(msg); + storage_information->read_speed = mavlink_msg_storage_information_get_read_speed(msg); + storage_information->write_speed = mavlink_msg_storage_information_get_write_speed(msg); + storage_information->storage_id = mavlink_msg_storage_information_get_storage_id(msg); + storage_information->storage_count = mavlink_msg_storage_information_get_storage_count(msg); + storage_information->status = mavlink_msg_storage_information_get_status(msg); + storage_information->type = mavlink_msg_storage_information_get_type(msg); + mavlink_msg_storage_information_get_name(msg, storage_information->name); +#else + uint8_t len = msg->len < MAVLINK_MSG_ID_STORAGE_INFORMATION_LEN? msg->len : MAVLINK_MSG_ID_STORAGE_INFORMATION_LEN; + memset(storage_information, 0, MAVLINK_MSG_ID_STORAGE_INFORMATION_LEN); + memcpy(storage_information, _MAV_PAYLOAD(msg), len); +#endif +} diff --git a/lib/main/MAVLink/common/mavlink_msg_supported_tunes.h b/lib/main/MAVLink/common/mavlink_msg_supported_tunes.h new file mode 100644 index 0000000000..a8fb9a3abf --- /dev/null +++ b/lib/main/MAVLink/common/mavlink_msg_supported_tunes.h @@ -0,0 +1,263 @@ +#pragma once +// MESSAGE SUPPORTED_TUNES PACKING + +#define MAVLINK_MSG_ID_SUPPORTED_TUNES 401 + + +typedef struct __mavlink_supported_tunes_t { + uint32_t format; /*< Bitfield of supported tune formats.*/ + uint8_t target_system; /*< System ID*/ + uint8_t target_component; /*< Component ID*/ +} mavlink_supported_tunes_t; + +#define MAVLINK_MSG_ID_SUPPORTED_TUNES_LEN 6 +#define MAVLINK_MSG_ID_SUPPORTED_TUNES_MIN_LEN 6 +#define MAVLINK_MSG_ID_401_LEN 6 +#define MAVLINK_MSG_ID_401_MIN_LEN 6 + +#define MAVLINK_MSG_ID_SUPPORTED_TUNES_CRC 183 +#define MAVLINK_MSG_ID_401_CRC 183 + + + +#if MAVLINK_COMMAND_24BIT +#define MAVLINK_MESSAGE_INFO_SUPPORTED_TUNES { \ + 401, \ + "SUPPORTED_TUNES", \ + 3, \ + { { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 4, offsetof(mavlink_supported_tunes_t, target_system) }, \ + { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 5, offsetof(mavlink_supported_tunes_t, target_component) }, \ + { "format", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_supported_tunes_t, format) }, \ + } \ +} +#else +#define MAVLINK_MESSAGE_INFO_SUPPORTED_TUNES { \ + "SUPPORTED_TUNES", \ + 3, \ + { { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 4, offsetof(mavlink_supported_tunes_t, target_system) }, \ + { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 5, offsetof(mavlink_supported_tunes_t, target_component) }, \ + { "format", NULL, MAVLINK_TYPE_UINT32_T, 0, 0, offsetof(mavlink_supported_tunes_t, format) }, \ + } \ +} +#endif + +/** + * @brief Pack a supported_tunes message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param target_system System ID + * @param target_component Component ID + * @param format Bitfield of supported tune formats. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_supported_tunes_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint8_t target_system, uint8_t target_component, uint32_t format) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_SUPPORTED_TUNES_LEN]; + _mav_put_uint32_t(buf, 0, format); + _mav_put_uint8_t(buf, 4, target_system); + _mav_put_uint8_t(buf, 5, target_component); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SUPPORTED_TUNES_LEN); +#else + mavlink_supported_tunes_t packet; + packet.format = format; + packet.target_system = target_system; + packet.target_component = target_component; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SUPPORTED_TUNES_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_SUPPORTED_TUNES; + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_SUPPORTED_TUNES_MIN_LEN, MAVLINK_MSG_ID_SUPPORTED_TUNES_LEN, MAVLINK_MSG_ID_SUPPORTED_TUNES_CRC); +} + +/** + * @brief Pack a supported_tunes message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param target_system System ID + * @param target_component Component ID + * @param format Bitfield of supported tune formats. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_supported_tunes_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint8_t target_system,uint8_t target_component,uint32_t format) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_SUPPORTED_TUNES_LEN]; + _mav_put_uint32_t(buf, 0, format); + _mav_put_uint8_t(buf, 4, target_system); + _mav_put_uint8_t(buf, 5, target_component); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_SUPPORTED_TUNES_LEN); +#else + mavlink_supported_tunes_t packet; + packet.format = format; + packet.target_system = target_system; + packet.target_component = target_component; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_SUPPORTED_TUNES_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_SUPPORTED_TUNES; + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_SUPPORTED_TUNES_MIN_LEN, MAVLINK_MSG_ID_SUPPORTED_TUNES_LEN, MAVLINK_MSG_ID_SUPPORTED_TUNES_CRC); +} + +/** + * @brief Encode a supported_tunes struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param supported_tunes C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_supported_tunes_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_supported_tunes_t* supported_tunes) +{ + return mavlink_msg_supported_tunes_pack(system_id, component_id, msg, supported_tunes->target_system, supported_tunes->target_component, supported_tunes->format); +} + +/** + * @brief Encode a supported_tunes struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param supported_tunes C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_supported_tunes_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_supported_tunes_t* supported_tunes) +{ + return mavlink_msg_supported_tunes_pack_chan(system_id, component_id, chan, msg, supported_tunes->target_system, supported_tunes->target_component, supported_tunes->format); +} + +/** + * @brief Send a supported_tunes message + * @param chan MAVLink channel to send the message + * + * @param target_system System ID + * @param target_component Component ID + * @param format Bitfield of supported tune formats. + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_supported_tunes_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint32_t format) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_SUPPORTED_TUNES_LEN]; + _mav_put_uint32_t(buf, 0, format); + _mav_put_uint8_t(buf, 4, target_system); + _mav_put_uint8_t(buf, 5, target_component); + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SUPPORTED_TUNES, buf, MAVLINK_MSG_ID_SUPPORTED_TUNES_MIN_LEN, MAVLINK_MSG_ID_SUPPORTED_TUNES_LEN, MAVLINK_MSG_ID_SUPPORTED_TUNES_CRC); +#else + mavlink_supported_tunes_t packet; + packet.format = format; + packet.target_system = target_system; + packet.target_component = target_component; + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SUPPORTED_TUNES, (const char *)&packet, MAVLINK_MSG_ID_SUPPORTED_TUNES_MIN_LEN, MAVLINK_MSG_ID_SUPPORTED_TUNES_LEN, MAVLINK_MSG_ID_SUPPORTED_TUNES_CRC); +#endif +} + +/** + * @brief Send a supported_tunes message + * @param chan MAVLink channel to send the message + * @param struct The MAVLink struct to serialize + */ +static inline void mavlink_msg_supported_tunes_send_struct(mavlink_channel_t chan, const mavlink_supported_tunes_t* supported_tunes) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + mavlink_msg_supported_tunes_send(chan, supported_tunes->target_system, supported_tunes->target_component, supported_tunes->format); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SUPPORTED_TUNES, (const char *)supported_tunes, MAVLINK_MSG_ID_SUPPORTED_TUNES_MIN_LEN, MAVLINK_MSG_ID_SUPPORTED_TUNES_LEN, MAVLINK_MSG_ID_SUPPORTED_TUNES_CRC); +#endif +} + +#if MAVLINK_MSG_ID_SUPPORTED_TUNES_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_supported_tunes_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint32_t format) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint32_t(buf, 0, format); + _mav_put_uint8_t(buf, 4, target_system); + _mav_put_uint8_t(buf, 5, target_component); + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SUPPORTED_TUNES, buf, MAVLINK_MSG_ID_SUPPORTED_TUNES_MIN_LEN, MAVLINK_MSG_ID_SUPPORTED_TUNES_LEN, MAVLINK_MSG_ID_SUPPORTED_TUNES_CRC); +#else + mavlink_supported_tunes_t *packet = (mavlink_supported_tunes_t *)msgbuf; + packet->format = format; + packet->target_system = target_system; + packet->target_component = target_component; + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_SUPPORTED_TUNES, (const char *)packet, MAVLINK_MSG_ID_SUPPORTED_TUNES_MIN_LEN, MAVLINK_MSG_ID_SUPPORTED_TUNES_LEN, MAVLINK_MSG_ID_SUPPORTED_TUNES_CRC); +#endif +} +#endif + +#endif + +// MESSAGE SUPPORTED_TUNES UNPACKING + + +/** + * @brief Get field target_system from supported_tunes message + * + * @return System ID + */ +static inline uint8_t mavlink_msg_supported_tunes_get_target_system(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 4); +} + +/** + * @brief Get field target_component from supported_tunes message + * + * @return Component ID + */ +static inline uint8_t mavlink_msg_supported_tunes_get_target_component(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 5); +} + +/** + * @brief Get field format from supported_tunes message + * + * @return Bitfield of supported tune formats. + */ +static inline uint32_t mavlink_msg_supported_tunes_get_format(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint32_t(msg, 0); +} + +/** + * @brief Decode a supported_tunes message into a struct + * + * @param msg The message to decode + * @param supported_tunes C-struct to decode the message contents into + */ +static inline void mavlink_msg_supported_tunes_decode(const mavlink_message_t* msg, mavlink_supported_tunes_t* supported_tunes) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + supported_tunes->format = mavlink_msg_supported_tunes_get_format(msg); + supported_tunes->target_system = mavlink_msg_supported_tunes_get_target_system(msg); + supported_tunes->target_component = mavlink_msg_supported_tunes_get_target_component(msg); +#else + uint8_t len = msg->len < MAVLINK_MSG_ID_SUPPORTED_TUNES_LEN? msg->len : MAVLINK_MSG_ID_SUPPORTED_TUNES_LEN; + memset(supported_tunes, 0, MAVLINK_MSG_ID_SUPPORTED_TUNES_LEN); + memcpy(supported_tunes, _MAV_PAYLOAD(msg), len); +#endif +} diff --git a/lib/main/MAVLink/common/mavlink_msg_time_estimate_to_target.h b/lib/main/MAVLink/common/mavlink_msg_time_estimate_to_target.h new file mode 100644 index 0000000000..29d03d5946 --- /dev/null +++ b/lib/main/MAVLink/common/mavlink_msg_time_estimate_to_target.h @@ -0,0 +1,313 @@ +#pragma once +// MESSAGE TIME_ESTIMATE_TO_TARGET PACKING + +#define MAVLINK_MSG_ID_TIME_ESTIMATE_TO_TARGET 380 + + +typedef struct __mavlink_time_estimate_to_target_t { + int32_t safe_return; /*< [s] Estimated time to complete the vehicle's configured "safe return" action from its current position (e.g. RTL, Smart RTL, etc.). -1 indicates that the vehicle is landed, or that no time estimate available.*/ + int32_t land; /*< [s] Estimated time for vehicle to complete the LAND action from its current position. -1 indicates that the vehicle is landed, or that no time estimate available.*/ + int32_t mission_next_item; /*< [s] Estimated time for reaching/completing the currently active mission item. -1 means no time estimate available.*/ + int32_t mission_end; /*< [s] Estimated time for completing the current mission. -1 means no mission active and/or no estimate available.*/ + int32_t commanded_action; /*< [s] Estimated time for completing the current commanded action (i.e. Go To, Takeoff, Land, etc.). -1 means no action active and/or no estimate available.*/ +} mavlink_time_estimate_to_target_t; + +#define MAVLINK_MSG_ID_TIME_ESTIMATE_TO_TARGET_LEN 20 +#define MAVLINK_MSG_ID_TIME_ESTIMATE_TO_TARGET_MIN_LEN 20 +#define MAVLINK_MSG_ID_380_LEN 20 +#define MAVLINK_MSG_ID_380_MIN_LEN 20 + +#define MAVLINK_MSG_ID_TIME_ESTIMATE_TO_TARGET_CRC 232 +#define MAVLINK_MSG_ID_380_CRC 232 + + + +#if MAVLINK_COMMAND_24BIT +#define MAVLINK_MESSAGE_INFO_TIME_ESTIMATE_TO_TARGET { \ + 380, \ + "TIME_ESTIMATE_TO_TARGET", \ + 5, \ + { { "safe_return", NULL, MAVLINK_TYPE_INT32_T, 0, 0, offsetof(mavlink_time_estimate_to_target_t, safe_return) }, \ + { "land", NULL, MAVLINK_TYPE_INT32_T, 0, 4, offsetof(mavlink_time_estimate_to_target_t, land) }, \ + { "mission_next_item", NULL, MAVLINK_TYPE_INT32_T, 0, 8, offsetof(mavlink_time_estimate_to_target_t, mission_next_item) }, \ + { "mission_end", NULL, MAVLINK_TYPE_INT32_T, 0, 12, offsetof(mavlink_time_estimate_to_target_t, mission_end) }, \ + { "commanded_action", NULL, MAVLINK_TYPE_INT32_T, 0, 16, offsetof(mavlink_time_estimate_to_target_t, commanded_action) }, \ + } \ +} +#else +#define MAVLINK_MESSAGE_INFO_TIME_ESTIMATE_TO_TARGET { \ + "TIME_ESTIMATE_TO_TARGET", \ + 5, \ + { { "safe_return", NULL, MAVLINK_TYPE_INT32_T, 0, 0, offsetof(mavlink_time_estimate_to_target_t, safe_return) }, \ + { "land", NULL, MAVLINK_TYPE_INT32_T, 0, 4, offsetof(mavlink_time_estimate_to_target_t, land) }, \ + { "mission_next_item", NULL, MAVLINK_TYPE_INT32_T, 0, 8, offsetof(mavlink_time_estimate_to_target_t, mission_next_item) }, \ + { "mission_end", NULL, MAVLINK_TYPE_INT32_T, 0, 12, offsetof(mavlink_time_estimate_to_target_t, mission_end) }, \ + { "commanded_action", NULL, MAVLINK_TYPE_INT32_T, 0, 16, offsetof(mavlink_time_estimate_to_target_t, commanded_action) }, \ + } \ +} +#endif + +/** + * @brief Pack a time_estimate_to_target message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param safe_return [s] Estimated time to complete the vehicle's configured "safe return" action from its current position (e.g. RTL, Smart RTL, etc.). -1 indicates that the vehicle is landed, or that no time estimate available. + * @param land [s] Estimated time for vehicle to complete the LAND action from its current position. -1 indicates that the vehicle is landed, or that no time estimate available. + * @param mission_next_item [s] Estimated time for reaching/completing the currently active mission item. -1 means no time estimate available. + * @param mission_end [s] Estimated time for completing the current mission. -1 means no mission active and/or no estimate available. + * @param commanded_action [s] Estimated time for completing the current commanded action (i.e. Go To, Takeoff, Land, etc.). -1 means no action active and/or no estimate available. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_time_estimate_to_target_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + int32_t safe_return, int32_t land, int32_t mission_next_item, int32_t mission_end, int32_t commanded_action) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_TIME_ESTIMATE_TO_TARGET_LEN]; + _mav_put_int32_t(buf, 0, safe_return); + _mav_put_int32_t(buf, 4, land); + _mav_put_int32_t(buf, 8, mission_next_item); + _mav_put_int32_t(buf, 12, mission_end); + _mav_put_int32_t(buf, 16, commanded_action); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_TIME_ESTIMATE_TO_TARGET_LEN); +#else + mavlink_time_estimate_to_target_t packet; + packet.safe_return = safe_return; + packet.land = land; + packet.mission_next_item = mission_next_item; + packet.mission_end = mission_end; + packet.commanded_action = commanded_action; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_TIME_ESTIMATE_TO_TARGET_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_TIME_ESTIMATE_TO_TARGET; + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_TIME_ESTIMATE_TO_TARGET_MIN_LEN, MAVLINK_MSG_ID_TIME_ESTIMATE_TO_TARGET_LEN, MAVLINK_MSG_ID_TIME_ESTIMATE_TO_TARGET_CRC); +} + +/** + * @brief Pack a time_estimate_to_target message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param safe_return [s] Estimated time to complete the vehicle's configured "safe return" action from its current position (e.g. RTL, Smart RTL, etc.). -1 indicates that the vehicle is landed, or that no time estimate available. + * @param land [s] Estimated time for vehicle to complete the LAND action from its current position. -1 indicates that the vehicle is landed, or that no time estimate available. + * @param mission_next_item [s] Estimated time for reaching/completing the currently active mission item. -1 means no time estimate available. + * @param mission_end [s] Estimated time for completing the current mission. -1 means no mission active and/or no estimate available. + * @param commanded_action [s] Estimated time for completing the current commanded action (i.e. Go To, Takeoff, Land, etc.). -1 means no action active and/or no estimate available. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_time_estimate_to_target_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + int32_t safe_return,int32_t land,int32_t mission_next_item,int32_t mission_end,int32_t commanded_action) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_TIME_ESTIMATE_TO_TARGET_LEN]; + _mav_put_int32_t(buf, 0, safe_return); + _mav_put_int32_t(buf, 4, land); + _mav_put_int32_t(buf, 8, mission_next_item); + _mav_put_int32_t(buf, 12, mission_end); + _mav_put_int32_t(buf, 16, commanded_action); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_TIME_ESTIMATE_TO_TARGET_LEN); +#else + mavlink_time_estimate_to_target_t packet; + packet.safe_return = safe_return; + packet.land = land; + packet.mission_next_item = mission_next_item; + packet.mission_end = mission_end; + packet.commanded_action = commanded_action; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_TIME_ESTIMATE_TO_TARGET_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_TIME_ESTIMATE_TO_TARGET; + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_TIME_ESTIMATE_TO_TARGET_MIN_LEN, MAVLINK_MSG_ID_TIME_ESTIMATE_TO_TARGET_LEN, MAVLINK_MSG_ID_TIME_ESTIMATE_TO_TARGET_CRC); +} + +/** + * @brief Encode a time_estimate_to_target struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param time_estimate_to_target C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_time_estimate_to_target_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_time_estimate_to_target_t* time_estimate_to_target) +{ + return mavlink_msg_time_estimate_to_target_pack(system_id, component_id, msg, time_estimate_to_target->safe_return, time_estimate_to_target->land, time_estimate_to_target->mission_next_item, time_estimate_to_target->mission_end, time_estimate_to_target->commanded_action); +} + +/** + * @brief Encode a time_estimate_to_target struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param time_estimate_to_target C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_time_estimate_to_target_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_time_estimate_to_target_t* time_estimate_to_target) +{ + return mavlink_msg_time_estimate_to_target_pack_chan(system_id, component_id, chan, msg, time_estimate_to_target->safe_return, time_estimate_to_target->land, time_estimate_to_target->mission_next_item, time_estimate_to_target->mission_end, time_estimate_to_target->commanded_action); +} + +/** + * @brief Send a time_estimate_to_target message + * @param chan MAVLink channel to send the message + * + * @param safe_return [s] Estimated time to complete the vehicle's configured "safe return" action from its current position (e.g. RTL, Smart RTL, etc.). -1 indicates that the vehicle is landed, or that no time estimate available. + * @param land [s] Estimated time for vehicle to complete the LAND action from its current position. -1 indicates that the vehicle is landed, or that no time estimate available. + * @param mission_next_item [s] Estimated time for reaching/completing the currently active mission item. -1 means no time estimate available. + * @param mission_end [s] Estimated time for completing the current mission. -1 means no mission active and/or no estimate available. + * @param commanded_action [s] Estimated time for completing the current commanded action (i.e. Go To, Takeoff, Land, etc.). -1 means no action active and/or no estimate available. + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_time_estimate_to_target_send(mavlink_channel_t chan, int32_t safe_return, int32_t land, int32_t mission_next_item, int32_t mission_end, int32_t commanded_action) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_TIME_ESTIMATE_TO_TARGET_LEN]; + _mav_put_int32_t(buf, 0, safe_return); + _mav_put_int32_t(buf, 4, land); + _mav_put_int32_t(buf, 8, mission_next_item); + _mav_put_int32_t(buf, 12, mission_end); + _mav_put_int32_t(buf, 16, commanded_action); + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_TIME_ESTIMATE_TO_TARGET, buf, MAVLINK_MSG_ID_TIME_ESTIMATE_TO_TARGET_MIN_LEN, MAVLINK_MSG_ID_TIME_ESTIMATE_TO_TARGET_LEN, MAVLINK_MSG_ID_TIME_ESTIMATE_TO_TARGET_CRC); +#else + mavlink_time_estimate_to_target_t packet; + packet.safe_return = safe_return; + packet.land = land; + packet.mission_next_item = mission_next_item; + packet.mission_end = mission_end; + packet.commanded_action = commanded_action; + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_TIME_ESTIMATE_TO_TARGET, (const char *)&packet, MAVLINK_MSG_ID_TIME_ESTIMATE_TO_TARGET_MIN_LEN, MAVLINK_MSG_ID_TIME_ESTIMATE_TO_TARGET_LEN, MAVLINK_MSG_ID_TIME_ESTIMATE_TO_TARGET_CRC); +#endif +} + +/** + * @brief Send a time_estimate_to_target message + * @param chan MAVLink channel to send the message + * @param struct The MAVLink struct to serialize + */ +static inline void mavlink_msg_time_estimate_to_target_send_struct(mavlink_channel_t chan, const mavlink_time_estimate_to_target_t* time_estimate_to_target) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + mavlink_msg_time_estimate_to_target_send(chan, time_estimate_to_target->safe_return, time_estimate_to_target->land, time_estimate_to_target->mission_next_item, time_estimate_to_target->mission_end, time_estimate_to_target->commanded_action); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_TIME_ESTIMATE_TO_TARGET, (const char *)time_estimate_to_target, MAVLINK_MSG_ID_TIME_ESTIMATE_TO_TARGET_MIN_LEN, MAVLINK_MSG_ID_TIME_ESTIMATE_TO_TARGET_LEN, MAVLINK_MSG_ID_TIME_ESTIMATE_TO_TARGET_CRC); +#endif +} + +#if MAVLINK_MSG_ID_TIME_ESTIMATE_TO_TARGET_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_time_estimate_to_target_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, int32_t safe_return, int32_t land, int32_t mission_next_item, int32_t mission_end, int32_t commanded_action) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_int32_t(buf, 0, safe_return); + _mav_put_int32_t(buf, 4, land); + _mav_put_int32_t(buf, 8, mission_next_item); + _mav_put_int32_t(buf, 12, mission_end); + _mav_put_int32_t(buf, 16, commanded_action); + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_TIME_ESTIMATE_TO_TARGET, buf, MAVLINK_MSG_ID_TIME_ESTIMATE_TO_TARGET_MIN_LEN, MAVLINK_MSG_ID_TIME_ESTIMATE_TO_TARGET_LEN, MAVLINK_MSG_ID_TIME_ESTIMATE_TO_TARGET_CRC); +#else + mavlink_time_estimate_to_target_t *packet = (mavlink_time_estimate_to_target_t *)msgbuf; + packet->safe_return = safe_return; + packet->land = land; + packet->mission_next_item = mission_next_item; + packet->mission_end = mission_end; + packet->commanded_action = commanded_action; + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_TIME_ESTIMATE_TO_TARGET, (const char *)packet, MAVLINK_MSG_ID_TIME_ESTIMATE_TO_TARGET_MIN_LEN, MAVLINK_MSG_ID_TIME_ESTIMATE_TO_TARGET_LEN, MAVLINK_MSG_ID_TIME_ESTIMATE_TO_TARGET_CRC); +#endif +} +#endif + +#endif + +// MESSAGE TIME_ESTIMATE_TO_TARGET UNPACKING + + +/** + * @brief Get field safe_return from time_estimate_to_target message + * + * @return [s] Estimated time to complete the vehicle's configured "safe return" action from its current position (e.g. RTL, Smart RTL, etc.). -1 indicates that the vehicle is landed, or that no time estimate available. + */ +static inline int32_t mavlink_msg_time_estimate_to_target_get_safe_return(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int32_t(msg, 0); +} + +/** + * @brief Get field land from time_estimate_to_target message + * + * @return [s] Estimated time for vehicle to complete the LAND action from its current position. -1 indicates that the vehicle is landed, or that no time estimate available. + */ +static inline int32_t mavlink_msg_time_estimate_to_target_get_land(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int32_t(msg, 4); +} + +/** + * @brief Get field mission_next_item from time_estimate_to_target message + * + * @return [s] Estimated time for reaching/completing the currently active mission item. -1 means no time estimate available. + */ +static inline int32_t mavlink_msg_time_estimate_to_target_get_mission_next_item(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int32_t(msg, 8); +} + +/** + * @brief Get field mission_end from time_estimate_to_target message + * + * @return [s] Estimated time for completing the current mission. -1 means no mission active and/or no estimate available. + */ +static inline int32_t mavlink_msg_time_estimate_to_target_get_mission_end(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int32_t(msg, 12); +} + +/** + * @brief Get field commanded_action from time_estimate_to_target message + * + * @return [s] Estimated time for completing the current commanded action (i.e. Go To, Takeoff, Land, etc.). -1 means no action active and/or no estimate available. + */ +static inline int32_t mavlink_msg_time_estimate_to_target_get_commanded_action(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int32_t(msg, 16); +} + +/** + * @brief Decode a time_estimate_to_target message into a struct + * + * @param msg The message to decode + * @param time_estimate_to_target C-struct to decode the message contents into + */ +static inline void mavlink_msg_time_estimate_to_target_decode(const mavlink_message_t* msg, mavlink_time_estimate_to_target_t* time_estimate_to_target) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + time_estimate_to_target->safe_return = mavlink_msg_time_estimate_to_target_get_safe_return(msg); + time_estimate_to_target->land = mavlink_msg_time_estimate_to_target_get_land(msg); + time_estimate_to_target->mission_next_item = mavlink_msg_time_estimate_to_target_get_mission_next_item(msg); + time_estimate_to_target->mission_end = mavlink_msg_time_estimate_to_target_get_mission_end(msg); + time_estimate_to_target->commanded_action = mavlink_msg_time_estimate_to_target_get_commanded_action(msg); +#else + uint8_t len = msg->len < MAVLINK_MSG_ID_TIME_ESTIMATE_TO_TARGET_LEN? msg->len : MAVLINK_MSG_ID_TIME_ESTIMATE_TO_TARGET_LEN; + memset(time_estimate_to_target, 0, MAVLINK_MSG_ID_TIME_ESTIMATE_TO_TARGET_LEN); + memcpy(time_estimate_to_target, _MAV_PAYLOAD(msg), len); +#endif +} diff --git a/lib/main/MAVLink/common/mavlink_msg_trajectory_representation_bezier.h b/lib/main/MAVLink/common/mavlink_msg_trajectory_representation_bezier.h new file mode 100644 index 0000000000..c535fd4416 --- /dev/null +++ b/lib/main/MAVLink/common/mavlink_msg_trajectory_representation_bezier.h @@ -0,0 +1,359 @@ +#pragma once +// MESSAGE TRAJECTORY_REPRESENTATION_BEZIER PACKING + +#define MAVLINK_MSG_ID_TRAJECTORY_REPRESENTATION_BEZIER 333 + + +typedef struct __mavlink_trajectory_representation_bezier_t { + uint64_t time_usec; /*< [us] Timestamp (UNIX Epoch time or time since system boot). The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude of the number.*/ + float pos_x[5]; /*< [m] X-coordinate of bezier control points. Set to NaN if not being used*/ + float pos_y[5]; /*< [m] Y-coordinate of bezier control points. Set to NaN if not being used*/ + float pos_z[5]; /*< [m] Z-coordinate of bezier control points. Set to NaN if not being used*/ + float delta[5]; /*< [s] Bezier time horizon. Set to NaN if velocity/acceleration should not be incorporated*/ + float pos_yaw[5]; /*< [rad] Yaw. Set to NaN for unchanged*/ + uint8_t valid_points; /*< Number of valid control points (up-to 5 points are possible)*/ +} mavlink_trajectory_representation_bezier_t; + +#define MAVLINK_MSG_ID_TRAJECTORY_REPRESENTATION_BEZIER_LEN 109 +#define MAVLINK_MSG_ID_TRAJECTORY_REPRESENTATION_BEZIER_MIN_LEN 109 +#define MAVLINK_MSG_ID_333_LEN 109 +#define MAVLINK_MSG_ID_333_MIN_LEN 109 + +#define MAVLINK_MSG_ID_TRAJECTORY_REPRESENTATION_BEZIER_CRC 231 +#define MAVLINK_MSG_ID_333_CRC 231 + +#define MAVLINK_MSG_TRAJECTORY_REPRESENTATION_BEZIER_FIELD_POS_X_LEN 5 +#define MAVLINK_MSG_TRAJECTORY_REPRESENTATION_BEZIER_FIELD_POS_Y_LEN 5 +#define MAVLINK_MSG_TRAJECTORY_REPRESENTATION_BEZIER_FIELD_POS_Z_LEN 5 +#define MAVLINK_MSG_TRAJECTORY_REPRESENTATION_BEZIER_FIELD_DELTA_LEN 5 +#define MAVLINK_MSG_TRAJECTORY_REPRESENTATION_BEZIER_FIELD_POS_YAW_LEN 5 + +#if MAVLINK_COMMAND_24BIT +#define MAVLINK_MESSAGE_INFO_TRAJECTORY_REPRESENTATION_BEZIER { \ + 333, \ + "TRAJECTORY_REPRESENTATION_BEZIER", \ + 7, \ + { { "time_usec", NULL, MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_trajectory_representation_bezier_t, time_usec) }, \ + { "valid_points", NULL, MAVLINK_TYPE_UINT8_T, 0, 108, offsetof(mavlink_trajectory_representation_bezier_t, valid_points) }, \ + { "pos_x", NULL, MAVLINK_TYPE_FLOAT, 5, 8, offsetof(mavlink_trajectory_representation_bezier_t, pos_x) }, \ + { "pos_y", NULL, MAVLINK_TYPE_FLOAT, 5, 28, offsetof(mavlink_trajectory_representation_bezier_t, pos_y) }, \ + { "pos_z", NULL, MAVLINK_TYPE_FLOAT, 5, 48, offsetof(mavlink_trajectory_representation_bezier_t, pos_z) }, \ + { "delta", NULL, MAVLINK_TYPE_FLOAT, 5, 68, offsetof(mavlink_trajectory_representation_bezier_t, delta) }, \ + { "pos_yaw", NULL, MAVLINK_TYPE_FLOAT, 5, 88, offsetof(mavlink_trajectory_representation_bezier_t, pos_yaw) }, \ + } \ +} +#else +#define MAVLINK_MESSAGE_INFO_TRAJECTORY_REPRESENTATION_BEZIER { \ + "TRAJECTORY_REPRESENTATION_BEZIER", \ + 7, \ + { { "time_usec", NULL, MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_trajectory_representation_bezier_t, time_usec) }, \ + { "valid_points", NULL, MAVLINK_TYPE_UINT8_T, 0, 108, offsetof(mavlink_trajectory_representation_bezier_t, valid_points) }, \ + { "pos_x", NULL, MAVLINK_TYPE_FLOAT, 5, 8, offsetof(mavlink_trajectory_representation_bezier_t, pos_x) }, \ + { "pos_y", NULL, MAVLINK_TYPE_FLOAT, 5, 28, offsetof(mavlink_trajectory_representation_bezier_t, pos_y) }, \ + { "pos_z", NULL, MAVLINK_TYPE_FLOAT, 5, 48, offsetof(mavlink_trajectory_representation_bezier_t, pos_z) }, \ + { "delta", NULL, MAVLINK_TYPE_FLOAT, 5, 68, offsetof(mavlink_trajectory_representation_bezier_t, delta) }, \ + { "pos_yaw", NULL, MAVLINK_TYPE_FLOAT, 5, 88, offsetof(mavlink_trajectory_representation_bezier_t, pos_yaw) }, \ + } \ +} +#endif + +/** + * @brief Pack a trajectory_representation_bezier message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param time_usec [us] Timestamp (UNIX Epoch time or time since system boot). The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude of the number. + * @param valid_points Number of valid control points (up-to 5 points are possible) + * @param pos_x [m] X-coordinate of bezier control points. Set to NaN if not being used + * @param pos_y [m] Y-coordinate of bezier control points. Set to NaN if not being used + * @param pos_z [m] Z-coordinate of bezier control points. Set to NaN if not being used + * @param delta [s] Bezier time horizon. Set to NaN if velocity/acceleration should not be incorporated + * @param pos_yaw [rad] Yaw. Set to NaN for unchanged + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_trajectory_representation_bezier_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint64_t time_usec, uint8_t valid_points, const float *pos_x, const float *pos_y, const float *pos_z, const float *delta, const float *pos_yaw) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_TRAJECTORY_REPRESENTATION_BEZIER_LEN]; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_uint8_t(buf, 108, valid_points); + _mav_put_float_array(buf, 8, pos_x, 5); + _mav_put_float_array(buf, 28, pos_y, 5); + _mav_put_float_array(buf, 48, pos_z, 5); + _mav_put_float_array(buf, 68, delta, 5); + _mav_put_float_array(buf, 88, pos_yaw, 5); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_TRAJECTORY_REPRESENTATION_BEZIER_LEN); +#else + mavlink_trajectory_representation_bezier_t packet; + packet.time_usec = time_usec; + packet.valid_points = valid_points; + mav_array_memcpy(packet.pos_x, pos_x, sizeof(float)*5); + mav_array_memcpy(packet.pos_y, pos_y, sizeof(float)*5); + mav_array_memcpy(packet.pos_z, pos_z, sizeof(float)*5); + mav_array_memcpy(packet.delta, delta, sizeof(float)*5); + mav_array_memcpy(packet.pos_yaw, pos_yaw, sizeof(float)*5); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_TRAJECTORY_REPRESENTATION_BEZIER_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_TRAJECTORY_REPRESENTATION_BEZIER; + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_TRAJECTORY_REPRESENTATION_BEZIER_MIN_LEN, MAVLINK_MSG_ID_TRAJECTORY_REPRESENTATION_BEZIER_LEN, MAVLINK_MSG_ID_TRAJECTORY_REPRESENTATION_BEZIER_CRC); +} + +/** + * @brief Pack a trajectory_representation_bezier message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param time_usec [us] Timestamp (UNIX Epoch time or time since system boot). The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude of the number. + * @param valid_points Number of valid control points (up-to 5 points are possible) + * @param pos_x [m] X-coordinate of bezier control points. Set to NaN if not being used + * @param pos_y [m] Y-coordinate of bezier control points. Set to NaN if not being used + * @param pos_z [m] Z-coordinate of bezier control points. Set to NaN if not being used + * @param delta [s] Bezier time horizon. Set to NaN if velocity/acceleration should not be incorporated + * @param pos_yaw [rad] Yaw. Set to NaN for unchanged + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_trajectory_representation_bezier_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint64_t time_usec,uint8_t valid_points,const float *pos_x,const float *pos_y,const float *pos_z,const float *delta,const float *pos_yaw) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_TRAJECTORY_REPRESENTATION_BEZIER_LEN]; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_uint8_t(buf, 108, valid_points); + _mav_put_float_array(buf, 8, pos_x, 5); + _mav_put_float_array(buf, 28, pos_y, 5); + _mav_put_float_array(buf, 48, pos_z, 5); + _mav_put_float_array(buf, 68, delta, 5); + _mav_put_float_array(buf, 88, pos_yaw, 5); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_TRAJECTORY_REPRESENTATION_BEZIER_LEN); +#else + mavlink_trajectory_representation_bezier_t packet; + packet.time_usec = time_usec; + packet.valid_points = valid_points; + mav_array_memcpy(packet.pos_x, pos_x, sizeof(float)*5); + mav_array_memcpy(packet.pos_y, pos_y, sizeof(float)*5); + mav_array_memcpy(packet.pos_z, pos_z, sizeof(float)*5); + mav_array_memcpy(packet.delta, delta, sizeof(float)*5); + mav_array_memcpy(packet.pos_yaw, pos_yaw, sizeof(float)*5); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_TRAJECTORY_REPRESENTATION_BEZIER_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_TRAJECTORY_REPRESENTATION_BEZIER; + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_TRAJECTORY_REPRESENTATION_BEZIER_MIN_LEN, MAVLINK_MSG_ID_TRAJECTORY_REPRESENTATION_BEZIER_LEN, MAVLINK_MSG_ID_TRAJECTORY_REPRESENTATION_BEZIER_CRC); +} + +/** + * @brief Encode a trajectory_representation_bezier struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param trajectory_representation_bezier C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_trajectory_representation_bezier_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_trajectory_representation_bezier_t* trajectory_representation_bezier) +{ + return mavlink_msg_trajectory_representation_bezier_pack(system_id, component_id, msg, trajectory_representation_bezier->time_usec, trajectory_representation_bezier->valid_points, trajectory_representation_bezier->pos_x, trajectory_representation_bezier->pos_y, trajectory_representation_bezier->pos_z, trajectory_representation_bezier->delta, trajectory_representation_bezier->pos_yaw); +} + +/** + * @brief Encode a trajectory_representation_bezier struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param trajectory_representation_bezier C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_trajectory_representation_bezier_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_trajectory_representation_bezier_t* trajectory_representation_bezier) +{ + return mavlink_msg_trajectory_representation_bezier_pack_chan(system_id, component_id, chan, msg, trajectory_representation_bezier->time_usec, trajectory_representation_bezier->valid_points, trajectory_representation_bezier->pos_x, trajectory_representation_bezier->pos_y, trajectory_representation_bezier->pos_z, trajectory_representation_bezier->delta, trajectory_representation_bezier->pos_yaw); +} + +/** + * @brief Send a trajectory_representation_bezier message + * @param chan MAVLink channel to send the message + * + * @param time_usec [us] Timestamp (UNIX Epoch time or time since system boot). The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude of the number. + * @param valid_points Number of valid control points (up-to 5 points are possible) + * @param pos_x [m] X-coordinate of bezier control points. Set to NaN if not being used + * @param pos_y [m] Y-coordinate of bezier control points. Set to NaN if not being used + * @param pos_z [m] Z-coordinate of bezier control points. Set to NaN if not being used + * @param delta [s] Bezier time horizon. Set to NaN if velocity/acceleration should not be incorporated + * @param pos_yaw [rad] Yaw. Set to NaN for unchanged + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_trajectory_representation_bezier_send(mavlink_channel_t chan, uint64_t time_usec, uint8_t valid_points, const float *pos_x, const float *pos_y, const float *pos_z, const float *delta, const float *pos_yaw) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_TRAJECTORY_REPRESENTATION_BEZIER_LEN]; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_uint8_t(buf, 108, valid_points); + _mav_put_float_array(buf, 8, pos_x, 5); + _mav_put_float_array(buf, 28, pos_y, 5); + _mav_put_float_array(buf, 48, pos_z, 5); + _mav_put_float_array(buf, 68, delta, 5); + _mav_put_float_array(buf, 88, pos_yaw, 5); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_TRAJECTORY_REPRESENTATION_BEZIER, buf, MAVLINK_MSG_ID_TRAJECTORY_REPRESENTATION_BEZIER_MIN_LEN, MAVLINK_MSG_ID_TRAJECTORY_REPRESENTATION_BEZIER_LEN, MAVLINK_MSG_ID_TRAJECTORY_REPRESENTATION_BEZIER_CRC); +#else + mavlink_trajectory_representation_bezier_t packet; + packet.time_usec = time_usec; + packet.valid_points = valid_points; + mav_array_memcpy(packet.pos_x, pos_x, sizeof(float)*5); + mav_array_memcpy(packet.pos_y, pos_y, sizeof(float)*5); + mav_array_memcpy(packet.pos_z, pos_z, sizeof(float)*5); + mav_array_memcpy(packet.delta, delta, sizeof(float)*5); + mav_array_memcpy(packet.pos_yaw, pos_yaw, sizeof(float)*5); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_TRAJECTORY_REPRESENTATION_BEZIER, (const char *)&packet, MAVLINK_MSG_ID_TRAJECTORY_REPRESENTATION_BEZIER_MIN_LEN, MAVLINK_MSG_ID_TRAJECTORY_REPRESENTATION_BEZIER_LEN, MAVLINK_MSG_ID_TRAJECTORY_REPRESENTATION_BEZIER_CRC); +#endif +} + +/** + * @brief Send a trajectory_representation_bezier message + * @param chan MAVLink channel to send the message + * @param struct The MAVLink struct to serialize + */ +static inline void mavlink_msg_trajectory_representation_bezier_send_struct(mavlink_channel_t chan, const mavlink_trajectory_representation_bezier_t* trajectory_representation_bezier) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + mavlink_msg_trajectory_representation_bezier_send(chan, trajectory_representation_bezier->time_usec, trajectory_representation_bezier->valid_points, trajectory_representation_bezier->pos_x, trajectory_representation_bezier->pos_y, trajectory_representation_bezier->pos_z, trajectory_representation_bezier->delta, trajectory_representation_bezier->pos_yaw); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_TRAJECTORY_REPRESENTATION_BEZIER, (const char *)trajectory_representation_bezier, MAVLINK_MSG_ID_TRAJECTORY_REPRESENTATION_BEZIER_MIN_LEN, MAVLINK_MSG_ID_TRAJECTORY_REPRESENTATION_BEZIER_LEN, MAVLINK_MSG_ID_TRAJECTORY_REPRESENTATION_BEZIER_CRC); +#endif +} + +#if MAVLINK_MSG_ID_TRAJECTORY_REPRESENTATION_BEZIER_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_trajectory_representation_bezier_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint64_t time_usec, uint8_t valid_points, const float *pos_x, const float *pos_y, const float *pos_z, const float *delta, const float *pos_yaw) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_uint8_t(buf, 108, valid_points); + _mav_put_float_array(buf, 8, pos_x, 5); + _mav_put_float_array(buf, 28, pos_y, 5); + _mav_put_float_array(buf, 48, pos_z, 5); + _mav_put_float_array(buf, 68, delta, 5); + _mav_put_float_array(buf, 88, pos_yaw, 5); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_TRAJECTORY_REPRESENTATION_BEZIER, buf, MAVLINK_MSG_ID_TRAJECTORY_REPRESENTATION_BEZIER_MIN_LEN, MAVLINK_MSG_ID_TRAJECTORY_REPRESENTATION_BEZIER_LEN, MAVLINK_MSG_ID_TRAJECTORY_REPRESENTATION_BEZIER_CRC); +#else + mavlink_trajectory_representation_bezier_t *packet = (mavlink_trajectory_representation_bezier_t *)msgbuf; + packet->time_usec = time_usec; + packet->valid_points = valid_points; + mav_array_memcpy(packet->pos_x, pos_x, sizeof(float)*5); + mav_array_memcpy(packet->pos_y, pos_y, sizeof(float)*5); + mav_array_memcpy(packet->pos_z, pos_z, sizeof(float)*5); + mav_array_memcpy(packet->delta, delta, sizeof(float)*5); + mav_array_memcpy(packet->pos_yaw, pos_yaw, sizeof(float)*5); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_TRAJECTORY_REPRESENTATION_BEZIER, (const char *)packet, MAVLINK_MSG_ID_TRAJECTORY_REPRESENTATION_BEZIER_MIN_LEN, MAVLINK_MSG_ID_TRAJECTORY_REPRESENTATION_BEZIER_LEN, MAVLINK_MSG_ID_TRAJECTORY_REPRESENTATION_BEZIER_CRC); +#endif +} +#endif + +#endif + +// MESSAGE TRAJECTORY_REPRESENTATION_BEZIER UNPACKING + + +/** + * @brief Get field time_usec from trajectory_representation_bezier message + * + * @return [us] Timestamp (UNIX Epoch time or time since system boot). The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude of the number. + */ +static inline uint64_t mavlink_msg_trajectory_representation_bezier_get_time_usec(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint64_t(msg, 0); +} + +/** + * @brief Get field valid_points from trajectory_representation_bezier message + * + * @return Number of valid control points (up-to 5 points are possible) + */ +static inline uint8_t mavlink_msg_trajectory_representation_bezier_get_valid_points(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 108); +} + +/** + * @brief Get field pos_x from trajectory_representation_bezier message + * + * @return [m] X-coordinate of bezier control points. Set to NaN if not being used + */ +static inline uint16_t mavlink_msg_trajectory_representation_bezier_get_pos_x(const mavlink_message_t* msg, float *pos_x) +{ + return _MAV_RETURN_float_array(msg, pos_x, 5, 8); +} + +/** + * @brief Get field pos_y from trajectory_representation_bezier message + * + * @return [m] Y-coordinate of bezier control points. Set to NaN if not being used + */ +static inline uint16_t mavlink_msg_trajectory_representation_bezier_get_pos_y(const mavlink_message_t* msg, float *pos_y) +{ + return _MAV_RETURN_float_array(msg, pos_y, 5, 28); +} + +/** + * @brief Get field pos_z from trajectory_representation_bezier message + * + * @return [m] Z-coordinate of bezier control points. Set to NaN if not being used + */ +static inline uint16_t mavlink_msg_trajectory_representation_bezier_get_pos_z(const mavlink_message_t* msg, float *pos_z) +{ + return _MAV_RETURN_float_array(msg, pos_z, 5, 48); +} + +/** + * @brief Get field delta from trajectory_representation_bezier message + * + * @return [s] Bezier time horizon. Set to NaN if velocity/acceleration should not be incorporated + */ +static inline uint16_t mavlink_msg_trajectory_representation_bezier_get_delta(const mavlink_message_t* msg, float *delta) +{ + return _MAV_RETURN_float_array(msg, delta, 5, 68); +} + +/** + * @brief Get field pos_yaw from trajectory_representation_bezier message + * + * @return [rad] Yaw. Set to NaN for unchanged + */ +static inline uint16_t mavlink_msg_trajectory_representation_bezier_get_pos_yaw(const mavlink_message_t* msg, float *pos_yaw) +{ + return _MAV_RETURN_float_array(msg, pos_yaw, 5, 88); +} + +/** + * @brief Decode a trajectory_representation_bezier message into a struct + * + * @param msg The message to decode + * @param trajectory_representation_bezier C-struct to decode the message contents into + */ +static inline void mavlink_msg_trajectory_representation_bezier_decode(const mavlink_message_t* msg, mavlink_trajectory_representation_bezier_t* trajectory_representation_bezier) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + trajectory_representation_bezier->time_usec = mavlink_msg_trajectory_representation_bezier_get_time_usec(msg); + mavlink_msg_trajectory_representation_bezier_get_pos_x(msg, trajectory_representation_bezier->pos_x); + mavlink_msg_trajectory_representation_bezier_get_pos_y(msg, trajectory_representation_bezier->pos_y); + mavlink_msg_trajectory_representation_bezier_get_pos_z(msg, trajectory_representation_bezier->pos_z); + mavlink_msg_trajectory_representation_bezier_get_delta(msg, trajectory_representation_bezier->delta); + mavlink_msg_trajectory_representation_bezier_get_pos_yaw(msg, trajectory_representation_bezier->pos_yaw); + trajectory_representation_bezier->valid_points = mavlink_msg_trajectory_representation_bezier_get_valid_points(msg); +#else + uint8_t len = msg->len < MAVLINK_MSG_ID_TRAJECTORY_REPRESENTATION_BEZIER_LEN? msg->len : MAVLINK_MSG_ID_TRAJECTORY_REPRESENTATION_BEZIER_LEN; + memset(trajectory_representation_bezier, 0, MAVLINK_MSG_ID_TRAJECTORY_REPRESENTATION_BEZIER_LEN); + memcpy(trajectory_representation_bezier, _MAV_PAYLOAD(msg), len); +#endif +} diff --git a/lib/main/MAVLink/common/mavlink_msg_trajectory_representation_waypoints.h b/lib/main/MAVLink/common/mavlink_msg_trajectory_representation_waypoints.h new file mode 100644 index 0000000000..470c4626fa --- /dev/null +++ b/lib/main/MAVLink/common/mavlink_msg_trajectory_representation_waypoints.h @@ -0,0 +1,541 @@ +#pragma once +// MESSAGE TRAJECTORY_REPRESENTATION_WAYPOINTS PACKING + +#define MAVLINK_MSG_ID_TRAJECTORY_REPRESENTATION_WAYPOINTS 332 + + +typedef struct __mavlink_trajectory_representation_waypoints_t { + uint64_t time_usec; /*< [us] Timestamp (UNIX Epoch time or time since system boot). The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude of the number.*/ + float pos_x[5]; /*< [m] X-coordinate of waypoint, set to NaN if not being used*/ + float pos_y[5]; /*< [m] Y-coordinate of waypoint, set to NaN if not being used*/ + float pos_z[5]; /*< [m] Z-coordinate of waypoint, set to NaN if not being used*/ + float vel_x[5]; /*< [m/s] X-velocity of waypoint, set to NaN if not being used*/ + float vel_y[5]; /*< [m/s] Y-velocity of waypoint, set to NaN if not being used*/ + float vel_z[5]; /*< [m/s] Z-velocity of waypoint, set to NaN if not being used*/ + float acc_x[5]; /*< [m/s/s] X-acceleration of waypoint, set to NaN if not being used*/ + float acc_y[5]; /*< [m/s/s] Y-acceleration of waypoint, set to NaN if not being used*/ + float acc_z[5]; /*< [m/s/s] Z-acceleration of waypoint, set to NaN if not being used*/ + float pos_yaw[5]; /*< [rad] Yaw angle, set to NaN if not being used*/ + float vel_yaw[5]; /*< [rad/s] Yaw rate, set to NaN if not being used*/ + uint16_t command[5]; /*< Scheduled action for each waypoint, UINT16_MAX if not being used.*/ + uint8_t valid_points; /*< Number of valid points (up-to 5 waypoints are possible)*/ +} mavlink_trajectory_representation_waypoints_t; + +#define MAVLINK_MSG_ID_TRAJECTORY_REPRESENTATION_WAYPOINTS_LEN 239 +#define MAVLINK_MSG_ID_TRAJECTORY_REPRESENTATION_WAYPOINTS_MIN_LEN 239 +#define MAVLINK_MSG_ID_332_LEN 239 +#define MAVLINK_MSG_ID_332_MIN_LEN 239 + +#define MAVLINK_MSG_ID_TRAJECTORY_REPRESENTATION_WAYPOINTS_CRC 236 +#define MAVLINK_MSG_ID_332_CRC 236 + +#define MAVLINK_MSG_TRAJECTORY_REPRESENTATION_WAYPOINTS_FIELD_POS_X_LEN 5 +#define MAVLINK_MSG_TRAJECTORY_REPRESENTATION_WAYPOINTS_FIELD_POS_Y_LEN 5 +#define MAVLINK_MSG_TRAJECTORY_REPRESENTATION_WAYPOINTS_FIELD_POS_Z_LEN 5 +#define MAVLINK_MSG_TRAJECTORY_REPRESENTATION_WAYPOINTS_FIELD_VEL_X_LEN 5 +#define MAVLINK_MSG_TRAJECTORY_REPRESENTATION_WAYPOINTS_FIELD_VEL_Y_LEN 5 +#define MAVLINK_MSG_TRAJECTORY_REPRESENTATION_WAYPOINTS_FIELD_VEL_Z_LEN 5 +#define MAVLINK_MSG_TRAJECTORY_REPRESENTATION_WAYPOINTS_FIELD_ACC_X_LEN 5 +#define MAVLINK_MSG_TRAJECTORY_REPRESENTATION_WAYPOINTS_FIELD_ACC_Y_LEN 5 +#define MAVLINK_MSG_TRAJECTORY_REPRESENTATION_WAYPOINTS_FIELD_ACC_Z_LEN 5 +#define MAVLINK_MSG_TRAJECTORY_REPRESENTATION_WAYPOINTS_FIELD_POS_YAW_LEN 5 +#define MAVLINK_MSG_TRAJECTORY_REPRESENTATION_WAYPOINTS_FIELD_VEL_YAW_LEN 5 +#define MAVLINK_MSG_TRAJECTORY_REPRESENTATION_WAYPOINTS_FIELD_COMMAND_LEN 5 + +#if MAVLINK_COMMAND_24BIT +#define MAVLINK_MESSAGE_INFO_TRAJECTORY_REPRESENTATION_WAYPOINTS { \ + 332, \ + "TRAJECTORY_REPRESENTATION_WAYPOINTS", \ + 14, \ + { { "time_usec", NULL, MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_trajectory_representation_waypoints_t, time_usec) }, \ + { "valid_points", NULL, MAVLINK_TYPE_UINT8_T, 0, 238, offsetof(mavlink_trajectory_representation_waypoints_t, valid_points) }, \ + { "pos_x", NULL, MAVLINK_TYPE_FLOAT, 5, 8, offsetof(mavlink_trajectory_representation_waypoints_t, pos_x) }, \ + { "pos_y", NULL, MAVLINK_TYPE_FLOAT, 5, 28, offsetof(mavlink_trajectory_representation_waypoints_t, pos_y) }, \ + { "pos_z", NULL, MAVLINK_TYPE_FLOAT, 5, 48, offsetof(mavlink_trajectory_representation_waypoints_t, pos_z) }, \ + { "vel_x", NULL, MAVLINK_TYPE_FLOAT, 5, 68, offsetof(mavlink_trajectory_representation_waypoints_t, vel_x) }, \ + { "vel_y", NULL, MAVLINK_TYPE_FLOAT, 5, 88, offsetof(mavlink_trajectory_representation_waypoints_t, vel_y) }, \ + { "vel_z", NULL, MAVLINK_TYPE_FLOAT, 5, 108, offsetof(mavlink_trajectory_representation_waypoints_t, vel_z) }, \ + { "acc_x", NULL, MAVLINK_TYPE_FLOAT, 5, 128, offsetof(mavlink_trajectory_representation_waypoints_t, acc_x) }, \ + { "acc_y", NULL, MAVLINK_TYPE_FLOAT, 5, 148, offsetof(mavlink_trajectory_representation_waypoints_t, acc_y) }, \ + { "acc_z", NULL, MAVLINK_TYPE_FLOAT, 5, 168, offsetof(mavlink_trajectory_representation_waypoints_t, acc_z) }, \ + { "pos_yaw", NULL, MAVLINK_TYPE_FLOAT, 5, 188, offsetof(mavlink_trajectory_representation_waypoints_t, pos_yaw) }, \ + { "vel_yaw", NULL, MAVLINK_TYPE_FLOAT, 5, 208, offsetof(mavlink_trajectory_representation_waypoints_t, vel_yaw) }, \ + { "command", NULL, MAVLINK_TYPE_UINT16_T, 5, 228, offsetof(mavlink_trajectory_representation_waypoints_t, command) }, \ + } \ +} +#else +#define MAVLINK_MESSAGE_INFO_TRAJECTORY_REPRESENTATION_WAYPOINTS { \ + "TRAJECTORY_REPRESENTATION_WAYPOINTS", \ + 14, \ + { { "time_usec", NULL, MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_trajectory_representation_waypoints_t, time_usec) }, \ + { "valid_points", NULL, MAVLINK_TYPE_UINT8_T, 0, 238, offsetof(mavlink_trajectory_representation_waypoints_t, valid_points) }, \ + { "pos_x", NULL, MAVLINK_TYPE_FLOAT, 5, 8, offsetof(mavlink_trajectory_representation_waypoints_t, pos_x) }, \ + { "pos_y", NULL, MAVLINK_TYPE_FLOAT, 5, 28, offsetof(mavlink_trajectory_representation_waypoints_t, pos_y) }, \ + { "pos_z", NULL, MAVLINK_TYPE_FLOAT, 5, 48, offsetof(mavlink_trajectory_representation_waypoints_t, pos_z) }, \ + { "vel_x", NULL, MAVLINK_TYPE_FLOAT, 5, 68, offsetof(mavlink_trajectory_representation_waypoints_t, vel_x) }, \ + { "vel_y", NULL, MAVLINK_TYPE_FLOAT, 5, 88, offsetof(mavlink_trajectory_representation_waypoints_t, vel_y) }, \ + { "vel_z", NULL, MAVLINK_TYPE_FLOAT, 5, 108, offsetof(mavlink_trajectory_representation_waypoints_t, vel_z) }, \ + { "acc_x", NULL, MAVLINK_TYPE_FLOAT, 5, 128, offsetof(mavlink_trajectory_representation_waypoints_t, acc_x) }, \ + { "acc_y", NULL, MAVLINK_TYPE_FLOAT, 5, 148, offsetof(mavlink_trajectory_representation_waypoints_t, acc_y) }, \ + { "acc_z", NULL, MAVLINK_TYPE_FLOAT, 5, 168, offsetof(mavlink_trajectory_representation_waypoints_t, acc_z) }, \ + { "pos_yaw", NULL, MAVLINK_TYPE_FLOAT, 5, 188, offsetof(mavlink_trajectory_representation_waypoints_t, pos_yaw) }, \ + { "vel_yaw", NULL, MAVLINK_TYPE_FLOAT, 5, 208, offsetof(mavlink_trajectory_representation_waypoints_t, vel_yaw) }, \ + { "command", NULL, MAVLINK_TYPE_UINT16_T, 5, 228, offsetof(mavlink_trajectory_representation_waypoints_t, command) }, \ + } \ +} +#endif + +/** + * @brief Pack a trajectory_representation_waypoints message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param time_usec [us] Timestamp (UNIX Epoch time or time since system boot). The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude of the number. + * @param valid_points Number of valid points (up-to 5 waypoints are possible) + * @param pos_x [m] X-coordinate of waypoint, set to NaN if not being used + * @param pos_y [m] Y-coordinate of waypoint, set to NaN if not being used + * @param pos_z [m] Z-coordinate of waypoint, set to NaN if not being used + * @param vel_x [m/s] X-velocity of waypoint, set to NaN if not being used + * @param vel_y [m/s] Y-velocity of waypoint, set to NaN if not being used + * @param vel_z [m/s] Z-velocity of waypoint, set to NaN if not being used + * @param acc_x [m/s/s] X-acceleration of waypoint, set to NaN if not being used + * @param acc_y [m/s/s] Y-acceleration of waypoint, set to NaN if not being used + * @param acc_z [m/s/s] Z-acceleration of waypoint, set to NaN if not being used + * @param pos_yaw [rad] Yaw angle, set to NaN if not being used + * @param vel_yaw [rad/s] Yaw rate, set to NaN if not being used + * @param command Scheduled action for each waypoint, UINT16_MAX if not being used. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_trajectory_representation_waypoints_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint64_t time_usec, uint8_t valid_points, const float *pos_x, const float *pos_y, const float *pos_z, const float *vel_x, const float *vel_y, const float *vel_z, const float *acc_x, const float *acc_y, const float *acc_z, const float *pos_yaw, const float *vel_yaw, const uint16_t *command) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_TRAJECTORY_REPRESENTATION_WAYPOINTS_LEN]; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_uint8_t(buf, 238, valid_points); + _mav_put_float_array(buf, 8, pos_x, 5); + _mav_put_float_array(buf, 28, pos_y, 5); + _mav_put_float_array(buf, 48, pos_z, 5); + _mav_put_float_array(buf, 68, vel_x, 5); + _mav_put_float_array(buf, 88, vel_y, 5); + _mav_put_float_array(buf, 108, vel_z, 5); + _mav_put_float_array(buf, 128, acc_x, 5); + _mav_put_float_array(buf, 148, acc_y, 5); + _mav_put_float_array(buf, 168, acc_z, 5); + _mav_put_float_array(buf, 188, pos_yaw, 5); + _mav_put_float_array(buf, 208, vel_yaw, 5); + _mav_put_uint16_t_array(buf, 228, command, 5); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_TRAJECTORY_REPRESENTATION_WAYPOINTS_LEN); +#else + mavlink_trajectory_representation_waypoints_t packet; + packet.time_usec = time_usec; + packet.valid_points = valid_points; + mav_array_memcpy(packet.pos_x, pos_x, sizeof(float)*5); + mav_array_memcpy(packet.pos_y, pos_y, sizeof(float)*5); + mav_array_memcpy(packet.pos_z, pos_z, sizeof(float)*5); + mav_array_memcpy(packet.vel_x, vel_x, sizeof(float)*5); + mav_array_memcpy(packet.vel_y, vel_y, sizeof(float)*5); + mav_array_memcpy(packet.vel_z, vel_z, sizeof(float)*5); + mav_array_memcpy(packet.acc_x, acc_x, sizeof(float)*5); + mav_array_memcpy(packet.acc_y, acc_y, sizeof(float)*5); + mav_array_memcpy(packet.acc_z, acc_z, sizeof(float)*5); + mav_array_memcpy(packet.pos_yaw, pos_yaw, sizeof(float)*5); + mav_array_memcpy(packet.vel_yaw, vel_yaw, sizeof(float)*5); + mav_array_memcpy(packet.command, command, sizeof(uint16_t)*5); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_TRAJECTORY_REPRESENTATION_WAYPOINTS_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_TRAJECTORY_REPRESENTATION_WAYPOINTS; + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_TRAJECTORY_REPRESENTATION_WAYPOINTS_MIN_LEN, MAVLINK_MSG_ID_TRAJECTORY_REPRESENTATION_WAYPOINTS_LEN, MAVLINK_MSG_ID_TRAJECTORY_REPRESENTATION_WAYPOINTS_CRC); +} + +/** + * @brief Pack a trajectory_representation_waypoints message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param time_usec [us] Timestamp (UNIX Epoch time or time since system boot). The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude of the number. + * @param valid_points Number of valid points (up-to 5 waypoints are possible) + * @param pos_x [m] X-coordinate of waypoint, set to NaN if not being used + * @param pos_y [m] Y-coordinate of waypoint, set to NaN if not being used + * @param pos_z [m] Z-coordinate of waypoint, set to NaN if not being used + * @param vel_x [m/s] X-velocity of waypoint, set to NaN if not being used + * @param vel_y [m/s] Y-velocity of waypoint, set to NaN if not being used + * @param vel_z [m/s] Z-velocity of waypoint, set to NaN if not being used + * @param acc_x [m/s/s] X-acceleration of waypoint, set to NaN if not being used + * @param acc_y [m/s/s] Y-acceleration of waypoint, set to NaN if not being used + * @param acc_z [m/s/s] Z-acceleration of waypoint, set to NaN if not being used + * @param pos_yaw [rad] Yaw angle, set to NaN if not being used + * @param vel_yaw [rad/s] Yaw rate, set to NaN if not being used + * @param command Scheduled action for each waypoint, UINT16_MAX if not being used. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_trajectory_representation_waypoints_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint64_t time_usec,uint8_t valid_points,const float *pos_x,const float *pos_y,const float *pos_z,const float *vel_x,const float *vel_y,const float *vel_z,const float *acc_x,const float *acc_y,const float *acc_z,const float *pos_yaw,const float *vel_yaw,const uint16_t *command) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_TRAJECTORY_REPRESENTATION_WAYPOINTS_LEN]; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_uint8_t(buf, 238, valid_points); + _mav_put_float_array(buf, 8, pos_x, 5); + _mav_put_float_array(buf, 28, pos_y, 5); + _mav_put_float_array(buf, 48, pos_z, 5); + _mav_put_float_array(buf, 68, vel_x, 5); + _mav_put_float_array(buf, 88, vel_y, 5); + _mav_put_float_array(buf, 108, vel_z, 5); + _mav_put_float_array(buf, 128, acc_x, 5); + _mav_put_float_array(buf, 148, acc_y, 5); + _mav_put_float_array(buf, 168, acc_z, 5); + _mav_put_float_array(buf, 188, pos_yaw, 5); + _mav_put_float_array(buf, 208, vel_yaw, 5); + _mav_put_uint16_t_array(buf, 228, command, 5); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_TRAJECTORY_REPRESENTATION_WAYPOINTS_LEN); +#else + mavlink_trajectory_representation_waypoints_t packet; + packet.time_usec = time_usec; + packet.valid_points = valid_points; + mav_array_memcpy(packet.pos_x, pos_x, sizeof(float)*5); + mav_array_memcpy(packet.pos_y, pos_y, sizeof(float)*5); + mav_array_memcpy(packet.pos_z, pos_z, sizeof(float)*5); + mav_array_memcpy(packet.vel_x, vel_x, sizeof(float)*5); + mav_array_memcpy(packet.vel_y, vel_y, sizeof(float)*5); + mav_array_memcpy(packet.vel_z, vel_z, sizeof(float)*5); + mav_array_memcpy(packet.acc_x, acc_x, sizeof(float)*5); + mav_array_memcpy(packet.acc_y, acc_y, sizeof(float)*5); + mav_array_memcpy(packet.acc_z, acc_z, sizeof(float)*5); + mav_array_memcpy(packet.pos_yaw, pos_yaw, sizeof(float)*5); + mav_array_memcpy(packet.vel_yaw, vel_yaw, sizeof(float)*5); + mav_array_memcpy(packet.command, command, sizeof(uint16_t)*5); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_TRAJECTORY_REPRESENTATION_WAYPOINTS_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_TRAJECTORY_REPRESENTATION_WAYPOINTS; + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_TRAJECTORY_REPRESENTATION_WAYPOINTS_MIN_LEN, MAVLINK_MSG_ID_TRAJECTORY_REPRESENTATION_WAYPOINTS_LEN, MAVLINK_MSG_ID_TRAJECTORY_REPRESENTATION_WAYPOINTS_CRC); +} + +/** + * @brief Encode a trajectory_representation_waypoints struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param trajectory_representation_waypoints C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_trajectory_representation_waypoints_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_trajectory_representation_waypoints_t* trajectory_representation_waypoints) +{ + return mavlink_msg_trajectory_representation_waypoints_pack(system_id, component_id, msg, trajectory_representation_waypoints->time_usec, trajectory_representation_waypoints->valid_points, trajectory_representation_waypoints->pos_x, trajectory_representation_waypoints->pos_y, trajectory_representation_waypoints->pos_z, trajectory_representation_waypoints->vel_x, trajectory_representation_waypoints->vel_y, trajectory_representation_waypoints->vel_z, trajectory_representation_waypoints->acc_x, trajectory_representation_waypoints->acc_y, trajectory_representation_waypoints->acc_z, trajectory_representation_waypoints->pos_yaw, trajectory_representation_waypoints->vel_yaw, trajectory_representation_waypoints->command); +} + +/** + * @brief Encode a trajectory_representation_waypoints struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param trajectory_representation_waypoints C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_trajectory_representation_waypoints_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_trajectory_representation_waypoints_t* trajectory_representation_waypoints) +{ + return mavlink_msg_trajectory_representation_waypoints_pack_chan(system_id, component_id, chan, msg, trajectory_representation_waypoints->time_usec, trajectory_representation_waypoints->valid_points, trajectory_representation_waypoints->pos_x, trajectory_representation_waypoints->pos_y, trajectory_representation_waypoints->pos_z, trajectory_representation_waypoints->vel_x, trajectory_representation_waypoints->vel_y, trajectory_representation_waypoints->vel_z, trajectory_representation_waypoints->acc_x, trajectory_representation_waypoints->acc_y, trajectory_representation_waypoints->acc_z, trajectory_representation_waypoints->pos_yaw, trajectory_representation_waypoints->vel_yaw, trajectory_representation_waypoints->command); +} + +/** + * @brief Send a trajectory_representation_waypoints message + * @param chan MAVLink channel to send the message + * + * @param time_usec [us] Timestamp (UNIX Epoch time or time since system boot). The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude of the number. + * @param valid_points Number of valid points (up-to 5 waypoints are possible) + * @param pos_x [m] X-coordinate of waypoint, set to NaN if not being used + * @param pos_y [m] Y-coordinate of waypoint, set to NaN if not being used + * @param pos_z [m] Z-coordinate of waypoint, set to NaN if not being used + * @param vel_x [m/s] X-velocity of waypoint, set to NaN if not being used + * @param vel_y [m/s] Y-velocity of waypoint, set to NaN if not being used + * @param vel_z [m/s] Z-velocity of waypoint, set to NaN if not being used + * @param acc_x [m/s/s] X-acceleration of waypoint, set to NaN if not being used + * @param acc_y [m/s/s] Y-acceleration of waypoint, set to NaN if not being used + * @param acc_z [m/s/s] Z-acceleration of waypoint, set to NaN if not being used + * @param pos_yaw [rad] Yaw angle, set to NaN if not being used + * @param vel_yaw [rad/s] Yaw rate, set to NaN if not being used + * @param command Scheduled action for each waypoint, UINT16_MAX if not being used. + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_trajectory_representation_waypoints_send(mavlink_channel_t chan, uint64_t time_usec, uint8_t valid_points, const float *pos_x, const float *pos_y, const float *pos_z, const float *vel_x, const float *vel_y, const float *vel_z, const float *acc_x, const float *acc_y, const float *acc_z, const float *pos_yaw, const float *vel_yaw, const uint16_t *command) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_TRAJECTORY_REPRESENTATION_WAYPOINTS_LEN]; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_uint8_t(buf, 238, valid_points); + _mav_put_float_array(buf, 8, pos_x, 5); + _mav_put_float_array(buf, 28, pos_y, 5); + _mav_put_float_array(buf, 48, pos_z, 5); + _mav_put_float_array(buf, 68, vel_x, 5); + _mav_put_float_array(buf, 88, vel_y, 5); + _mav_put_float_array(buf, 108, vel_z, 5); + _mav_put_float_array(buf, 128, acc_x, 5); + _mav_put_float_array(buf, 148, acc_y, 5); + _mav_put_float_array(buf, 168, acc_z, 5); + _mav_put_float_array(buf, 188, pos_yaw, 5); + _mav_put_float_array(buf, 208, vel_yaw, 5); + _mav_put_uint16_t_array(buf, 228, command, 5); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_TRAJECTORY_REPRESENTATION_WAYPOINTS, buf, MAVLINK_MSG_ID_TRAJECTORY_REPRESENTATION_WAYPOINTS_MIN_LEN, MAVLINK_MSG_ID_TRAJECTORY_REPRESENTATION_WAYPOINTS_LEN, MAVLINK_MSG_ID_TRAJECTORY_REPRESENTATION_WAYPOINTS_CRC); +#else + mavlink_trajectory_representation_waypoints_t packet; + packet.time_usec = time_usec; + packet.valid_points = valid_points; + mav_array_memcpy(packet.pos_x, pos_x, sizeof(float)*5); + mav_array_memcpy(packet.pos_y, pos_y, sizeof(float)*5); + mav_array_memcpy(packet.pos_z, pos_z, sizeof(float)*5); + mav_array_memcpy(packet.vel_x, vel_x, sizeof(float)*5); + mav_array_memcpy(packet.vel_y, vel_y, sizeof(float)*5); + mav_array_memcpy(packet.vel_z, vel_z, sizeof(float)*5); + mav_array_memcpy(packet.acc_x, acc_x, sizeof(float)*5); + mav_array_memcpy(packet.acc_y, acc_y, sizeof(float)*5); + mav_array_memcpy(packet.acc_z, acc_z, sizeof(float)*5); + mav_array_memcpy(packet.pos_yaw, pos_yaw, sizeof(float)*5); + mav_array_memcpy(packet.vel_yaw, vel_yaw, sizeof(float)*5); + mav_array_memcpy(packet.command, command, sizeof(uint16_t)*5); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_TRAJECTORY_REPRESENTATION_WAYPOINTS, (const char *)&packet, MAVLINK_MSG_ID_TRAJECTORY_REPRESENTATION_WAYPOINTS_MIN_LEN, MAVLINK_MSG_ID_TRAJECTORY_REPRESENTATION_WAYPOINTS_LEN, MAVLINK_MSG_ID_TRAJECTORY_REPRESENTATION_WAYPOINTS_CRC); +#endif +} + +/** + * @brief Send a trajectory_representation_waypoints message + * @param chan MAVLink channel to send the message + * @param struct The MAVLink struct to serialize + */ +static inline void mavlink_msg_trajectory_representation_waypoints_send_struct(mavlink_channel_t chan, const mavlink_trajectory_representation_waypoints_t* trajectory_representation_waypoints) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + mavlink_msg_trajectory_representation_waypoints_send(chan, trajectory_representation_waypoints->time_usec, trajectory_representation_waypoints->valid_points, trajectory_representation_waypoints->pos_x, trajectory_representation_waypoints->pos_y, trajectory_representation_waypoints->pos_z, trajectory_representation_waypoints->vel_x, trajectory_representation_waypoints->vel_y, trajectory_representation_waypoints->vel_z, trajectory_representation_waypoints->acc_x, trajectory_representation_waypoints->acc_y, trajectory_representation_waypoints->acc_z, trajectory_representation_waypoints->pos_yaw, trajectory_representation_waypoints->vel_yaw, trajectory_representation_waypoints->command); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_TRAJECTORY_REPRESENTATION_WAYPOINTS, (const char *)trajectory_representation_waypoints, MAVLINK_MSG_ID_TRAJECTORY_REPRESENTATION_WAYPOINTS_MIN_LEN, MAVLINK_MSG_ID_TRAJECTORY_REPRESENTATION_WAYPOINTS_LEN, MAVLINK_MSG_ID_TRAJECTORY_REPRESENTATION_WAYPOINTS_CRC); +#endif +} + +#if MAVLINK_MSG_ID_TRAJECTORY_REPRESENTATION_WAYPOINTS_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_trajectory_representation_waypoints_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint64_t time_usec, uint8_t valid_points, const float *pos_x, const float *pos_y, const float *pos_z, const float *vel_x, const float *vel_y, const float *vel_z, const float *acc_x, const float *acc_y, const float *acc_z, const float *pos_yaw, const float *vel_yaw, const uint16_t *command) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_uint8_t(buf, 238, valid_points); + _mav_put_float_array(buf, 8, pos_x, 5); + _mav_put_float_array(buf, 28, pos_y, 5); + _mav_put_float_array(buf, 48, pos_z, 5); + _mav_put_float_array(buf, 68, vel_x, 5); + _mav_put_float_array(buf, 88, vel_y, 5); + _mav_put_float_array(buf, 108, vel_z, 5); + _mav_put_float_array(buf, 128, acc_x, 5); + _mav_put_float_array(buf, 148, acc_y, 5); + _mav_put_float_array(buf, 168, acc_z, 5); + _mav_put_float_array(buf, 188, pos_yaw, 5); + _mav_put_float_array(buf, 208, vel_yaw, 5); + _mav_put_uint16_t_array(buf, 228, command, 5); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_TRAJECTORY_REPRESENTATION_WAYPOINTS, buf, MAVLINK_MSG_ID_TRAJECTORY_REPRESENTATION_WAYPOINTS_MIN_LEN, MAVLINK_MSG_ID_TRAJECTORY_REPRESENTATION_WAYPOINTS_LEN, MAVLINK_MSG_ID_TRAJECTORY_REPRESENTATION_WAYPOINTS_CRC); +#else + mavlink_trajectory_representation_waypoints_t *packet = (mavlink_trajectory_representation_waypoints_t *)msgbuf; + packet->time_usec = time_usec; + packet->valid_points = valid_points; + mav_array_memcpy(packet->pos_x, pos_x, sizeof(float)*5); + mav_array_memcpy(packet->pos_y, pos_y, sizeof(float)*5); + mav_array_memcpy(packet->pos_z, pos_z, sizeof(float)*5); + mav_array_memcpy(packet->vel_x, vel_x, sizeof(float)*5); + mav_array_memcpy(packet->vel_y, vel_y, sizeof(float)*5); + mav_array_memcpy(packet->vel_z, vel_z, sizeof(float)*5); + mav_array_memcpy(packet->acc_x, acc_x, sizeof(float)*5); + mav_array_memcpy(packet->acc_y, acc_y, sizeof(float)*5); + mav_array_memcpy(packet->acc_z, acc_z, sizeof(float)*5); + mav_array_memcpy(packet->pos_yaw, pos_yaw, sizeof(float)*5); + mav_array_memcpy(packet->vel_yaw, vel_yaw, sizeof(float)*5); + mav_array_memcpy(packet->command, command, sizeof(uint16_t)*5); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_TRAJECTORY_REPRESENTATION_WAYPOINTS, (const char *)packet, MAVLINK_MSG_ID_TRAJECTORY_REPRESENTATION_WAYPOINTS_MIN_LEN, MAVLINK_MSG_ID_TRAJECTORY_REPRESENTATION_WAYPOINTS_LEN, MAVLINK_MSG_ID_TRAJECTORY_REPRESENTATION_WAYPOINTS_CRC); +#endif +} +#endif + +#endif + +// MESSAGE TRAJECTORY_REPRESENTATION_WAYPOINTS UNPACKING + + +/** + * @brief Get field time_usec from trajectory_representation_waypoints message + * + * @return [us] Timestamp (UNIX Epoch time or time since system boot). The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude of the number. + */ +static inline uint64_t mavlink_msg_trajectory_representation_waypoints_get_time_usec(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint64_t(msg, 0); +} + +/** + * @brief Get field valid_points from trajectory_representation_waypoints message + * + * @return Number of valid points (up-to 5 waypoints are possible) + */ +static inline uint8_t mavlink_msg_trajectory_representation_waypoints_get_valid_points(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 238); +} + +/** + * @brief Get field pos_x from trajectory_representation_waypoints message + * + * @return [m] X-coordinate of waypoint, set to NaN if not being used + */ +static inline uint16_t mavlink_msg_trajectory_representation_waypoints_get_pos_x(const mavlink_message_t* msg, float *pos_x) +{ + return _MAV_RETURN_float_array(msg, pos_x, 5, 8); +} + +/** + * @brief Get field pos_y from trajectory_representation_waypoints message + * + * @return [m] Y-coordinate of waypoint, set to NaN if not being used + */ +static inline uint16_t mavlink_msg_trajectory_representation_waypoints_get_pos_y(const mavlink_message_t* msg, float *pos_y) +{ + return _MAV_RETURN_float_array(msg, pos_y, 5, 28); +} + +/** + * @brief Get field pos_z from trajectory_representation_waypoints message + * + * @return [m] Z-coordinate of waypoint, set to NaN if not being used + */ +static inline uint16_t mavlink_msg_trajectory_representation_waypoints_get_pos_z(const mavlink_message_t* msg, float *pos_z) +{ + return _MAV_RETURN_float_array(msg, pos_z, 5, 48); +} + +/** + * @brief Get field vel_x from trajectory_representation_waypoints message + * + * @return [m/s] X-velocity of waypoint, set to NaN if not being used + */ +static inline uint16_t mavlink_msg_trajectory_representation_waypoints_get_vel_x(const mavlink_message_t* msg, float *vel_x) +{ + return _MAV_RETURN_float_array(msg, vel_x, 5, 68); +} + +/** + * @brief Get field vel_y from trajectory_representation_waypoints message + * + * @return [m/s] Y-velocity of waypoint, set to NaN if not being used + */ +static inline uint16_t mavlink_msg_trajectory_representation_waypoints_get_vel_y(const mavlink_message_t* msg, float *vel_y) +{ + return _MAV_RETURN_float_array(msg, vel_y, 5, 88); +} + +/** + * @brief Get field vel_z from trajectory_representation_waypoints message + * + * @return [m/s] Z-velocity of waypoint, set to NaN if not being used + */ +static inline uint16_t mavlink_msg_trajectory_representation_waypoints_get_vel_z(const mavlink_message_t* msg, float *vel_z) +{ + return _MAV_RETURN_float_array(msg, vel_z, 5, 108); +} + +/** + * @brief Get field acc_x from trajectory_representation_waypoints message + * + * @return [m/s/s] X-acceleration of waypoint, set to NaN if not being used + */ +static inline uint16_t mavlink_msg_trajectory_representation_waypoints_get_acc_x(const mavlink_message_t* msg, float *acc_x) +{ + return _MAV_RETURN_float_array(msg, acc_x, 5, 128); +} + +/** + * @brief Get field acc_y from trajectory_representation_waypoints message + * + * @return [m/s/s] Y-acceleration of waypoint, set to NaN if not being used + */ +static inline uint16_t mavlink_msg_trajectory_representation_waypoints_get_acc_y(const mavlink_message_t* msg, float *acc_y) +{ + return _MAV_RETURN_float_array(msg, acc_y, 5, 148); +} + +/** + * @brief Get field acc_z from trajectory_representation_waypoints message + * + * @return [m/s/s] Z-acceleration of waypoint, set to NaN if not being used + */ +static inline uint16_t mavlink_msg_trajectory_representation_waypoints_get_acc_z(const mavlink_message_t* msg, float *acc_z) +{ + return _MAV_RETURN_float_array(msg, acc_z, 5, 168); +} + +/** + * @brief Get field pos_yaw from trajectory_representation_waypoints message + * + * @return [rad] Yaw angle, set to NaN if not being used + */ +static inline uint16_t mavlink_msg_trajectory_representation_waypoints_get_pos_yaw(const mavlink_message_t* msg, float *pos_yaw) +{ + return _MAV_RETURN_float_array(msg, pos_yaw, 5, 188); +} + +/** + * @brief Get field vel_yaw from trajectory_representation_waypoints message + * + * @return [rad/s] Yaw rate, set to NaN if not being used + */ +static inline uint16_t mavlink_msg_trajectory_representation_waypoints_get_vel_yaw(const mavlink_message_t* msg, float *vel_yaw) +{ + return _MAV_RETURN_float_array(msg, vel_yaw, 5, 208); +} + +/** + * @brief Get field command from trajectory_representation_waypoints message + * + * @return Scheduled action for each waypoint, UINT16_MAX if not being used. + */ +static inline uint16_t mavlink_msg_trajectory_representation_waypoints_get_command(const mavlink_message_t* msg, uint16_t *command) +{ + return _MAV_RETURN_uint16_t_array(msg, command, 5, 228); +} + +/** + * @brief Decode a trajectory_representation_waypoints message into a struct + * + * @param msg The message to decode + * @param trajectory_representation_waypoints C-struct to decode the message contents into + */ +static inline void mavlink_msg_trajectory_representation_waypoints_decode(const mavlink_message_t* msg, mavlink_trajectory_representation_waypoints_t* trajectory_representation_waypoints) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + trajectory_representation_waypoints->time_usec = mavlink_msg_trajectory_representation_waypoints_get_time_usec(msg); + mavlink_msg_trajectory_representation_waypoints_get_pos_x(msg, trajectory_representation_waypoints->pos_x); + mavlink_msg_trajectory_representation_waypoints_get_pos_y(msg, trajectory_representation_waypoints->pos_y); + mavlink_msg_trajectory_representation_waypoints_get_pos_z(msg, trajectory_representation_waypoints->pos_z); + mavlink_msg_trajectory_representation_waypoints_get_vel_x(msg, trajectory_representation_waypoints->vel_x); + mavlink_msg_trajectory_representation_waypoints_get_vel_y(msg, trajectory_representation_waypoints->vel_y); + mavlink_msg_trajectory_representation_waypoints_get_vel_z(msg, trajectory_representation_waypoints->vel_z); + mavlink_msg_trajectory_representation_waypoints_get_acc_x(msg, trajectory_representation_waypoints->acc_x); + mavlink_msg_trajectory_representation_waypoints_get_acc_y(msg, trajectory_representation_waypoints->acc_y); + mavlink_msg_trajectory_representation_waypoints_get_acc_z(msg, trajectory_representation_waypoints->acc_z); + mavlink_msg_trajectory_representation_waypoints_get_pos_yaw(msg, trajectory_representation_waypoints->pos_yaw); + mavlink_msg_trajectory_representation_waypoints_get_vel_yaw(msg, trajectory_representation_waypoints->vel_yaw); + mavlink_msg_trajectory_representation_waypoints_get_command(msg, trajectory_representation_waypoints->command); + trajectory_representation_waypoints->valid_points = mavlink_msg_trajectory_representation_waypoints_get_valid_points(msg); +#else + uint8_t len = msg->len < MAVLINK_MSG_ID_TRAJECTORY_REPRESENTATION_WAYPOINTS_LEN? msg->len : MAVLINK_MSG_ID_TRAJECTORY_REPRESENTATION_WAYPOINTS_LEN; + memset(trajectory_representation_waypoints, 0, MAVLINK_MSG_ID_TRAJECTORY_REPRESENTATION_WAYPOINTS_LEN); + memcpy(trajectory_representation_waypoints, _MAV_PAYLOAD(msg), len); +#endif +} diff --git a/lib/main/MAVLink/common/mavlink_msg_tunnel.h b/lib/main/MAVLink/common/mavlink_msg_tunnel.h new file mode 100644 index 0000000000..871315882d --- /dev/null +++ b/lib/main/MAVLink/common/mavlink_msg_tunnel.h @@ -0,0 +1,305 @@ +#pragma once +// MESSAGE TUNNEL PACKING + +#define MAVLINK_MSG_ID_TUNNEL 385 + + +typedef struct __mavlink_tunnel_t { + uint16_t payload_type; /*< A code that identifies the content of the payload (0 for unknown, which is the default). If this code is less than 32768, it is a 'registered' payload type and the corresponding code should be added to the MAV_TUNNEL_PAYLOAD_TYPE enum. Software creators can register blocks of types as needed. Codes greater than 32767 are considered local experiments and should not be checked in to any widely distributed codebase.*/ + uint8_t target_system; /*< System ID (can be 0 for broadcast, but this is discouraged)*/ + uint8_t target_component; /*< Component ID (can be 0 for broadcast, but this is discouraged)*/ + uint8_t payload_length; /*< Length of the data transported in payload*/ + uint8_t payload[128]; /*< Variable length payload. The payload length is defined by payload_length. The entire content of this block is opaque unless you understand the encoding specified by payload_type.*/ +} mavlink_tunnel_t; + +#define MAVLINK_MSG_ID_TUNNEL_LEN 133 +#define MAVLINK_MSG_ID_TUNNEL_MIN_LEN 133 +#define MAVLINK_MSG_ID_385_LEN 133 +#define MAVLINK_MSG_ID_385_MIN_LEN 133 + +#define MAVLINK_MSG_ID_TUNNEL_CRC 147 +#define MAVLINK_MSG_ID_385_CRC 147 + +#define MAVLINK_MSG_TUNNEL_FIELD_PAYLOAD_LEN 128 + +#if MAVLINK_COMMAND_24BIT +#define MAVLINK_MESSAGE_INFO_TUNNEL { \ + 385, \ + "TUNNEL", \ + 5, \ + { { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 2, offsetof(mavlink_tunnel_t, target_system) }, \ + { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 3, offsetof(mavlink_tunnel_t, target_component) }, \ + { "payload_type", NULL, MAVLINK_TYPE_UINT16_T, 0, 0, offsetof(mavlink_tunnel_t, payload_type) }, \ + { "payload_length", NULL, MAVLINK_TYPE_UINT8_T, 0, 4, offsetof(mavlink_tunnel_t, payload_length) }, \ + { "payload", NULL, MAVLINK_TYPE_UINT8_T, 128, 5, offsetof(mavlink_tunnel_t, payload) }, \ + } \ +} +#else +#define MAVLINK_MESSAGE_INFO_TUNNEL { \ + "TUNNEL", \ + 5, \ + { { "target_system", NULL, MAVLINK_TYPE_UINT8_T, 0, 2, offsetof(mavlink_tunnel_t, target_system) }, \ + { "target_component", NULL, MAVLINK_TYPE_UINT8_T, 0, 3, offsetof(mavlink_tunnel_t, target_component) }, \ + { "payload_type", NULL, MAVLINK_TYPE_UINT16_T, 0, 0, offsetof(mavlink_tunnel_t, payload_type) }, \ + { "payload_length", NULL, MAVLINK_TYPE_UINT8_T, 0, 4, offsetof(mavlink_tunnel_t, payload_length) }, \ + { "payload", NULL, MAVLINK_TYPE_UINT8_T, 128, 5, offsetof(mavlink_tunnel_t, payload) }, \ + } \ +} +#endif + +/** + * @brief Pack a tunnel message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param target_system System ID (can be 0 for broadcast, but this is discouraged) + * @param target_component Component ID (can be 0 for broadcast, but this is discouraged) + * @param payload_type A code that identifies the content of the payload (0 for unknown, which is the default). If this code is less than 32768, it is a 'registered' payload type and the corresponding code should be added to the MAV_TUNNEL_PAYLOAD_TYPE enum. Software creators can register blocks of types as needed. Codes greater than 32767 are considered local experiments and should not be checked in to any widely distributed codebase. + * @param payload_length Length of the data transported in payload + * @param payload Variable length payload. The payload length is defined by payload_length. The entire content of this block is opaque unless you understand the encoding specified by payload_type. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_tunnel_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint8_t target_system, uint8_t target_component, uint16_t payload_type, uint8_t payload_length, const uint8_t *payload) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_TUNNEL_LEN]; + _mav_put_uint16_t(buf, 0, payload_type); + _mav_put_uint8_t(buf, 2, target_system); + _mav_put_uint8_t(buf, 3, target_component); + _mav_put_uint8_t(buf, 4, payload_length); + _mav_put_uint8_t_array(buf, 5, payload, 128); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_TUNNEL_LEN); +#else + mavlink_tunnel_t packet; + packet.payload_type = payload_type; + packet.target_system = target_system; + packet.target_component = target_component; + packet.payload_length = payload_length; + mav_array_memcpy(packet.payload, payload, sizeof(uint8_t)*128); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_TUNNEL_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_TUNNEL; + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_TUNNEL_MIN_LEN, MAVLINK_MSG_ID_TUNNEL_LEN, MAVLINK_MSG_ID_TUNNEL_CRC); +} + +/** + * @brief Pack a tunnel message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param target_system System ID (can be 0 for broadcast, but this is discouraged) + * @param target_component Component ID (can be 0 for broadcast, but this is discouraged) + * @param payload_type A code that identifies the content of the payload (0 for unknown, which is the default). If this code is less than 32768, it is a 'registered' payload type and the corresponding code should be added to the MAV_TUNNEL_PAYLOAD_TYPE enum. Software creators can register blocks of types as needed. Codes greater than 32767 are considered local experiments and should not be checked in to any widely distributed codebase. + * @param payload_length Length of the data transported in payload + * @param payload Variable length payload. The payload length is defined by payload_length. The entire content of this block is opaque unless you understand the encoding specified by payload_type. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_tunnel_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint8_t target_system,uint8_t target_component,uint16_t payload_type,uint8_t payload_length,const uint8_t *payload) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_TUNNEL_LEN]; + _mav_put_uint16_t(buf, 0, payload_type); + _mav_put_uint8_t(buf, 2, target_system); + _mav_put_uint8_t(buf, 3, target_component); + _mav_put_uint8_t(buf, 4, payload_length); + _mav_put_uint8_t_array(buf, 5, payload, 128); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_TUNNEL_LEN); +#else + mavlink_tunnel_t packet; + packet.payload_type = payload_type; + packet.target_system = target_system; + packet.target_component = target_component; + packet.payload_length = payload_length; + mav_array_memcpy(packet.payload, payload, sizeof(uint8_t)*128); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_TUNNEL_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_TUNNEL; + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_TUNNEL_MIN_LEN, MAVLINK_MSG_ID_TUNNEL_LEN, MAVLINK_MSG_ID_TUNNEL_CRC); +} + +/** + * @brief Encode a tunnel struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param tunnel C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_tunnel_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_tunnel_t* tunnel) +{ + return mavlink_msg_tunnel_pack(system_id, component_id, msg, tunnel->target_system, tunnel->target_component, tunnel->payload_type, tunnel->payload_length, tunnel->payload); +} + +/** + * @brief Encode a tunnel struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param tunnel C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_tunnel_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_tunnel_t* tunnel) +{ + return mavlink_msg_tunnel_pack_chan(system_id, component_id, chan, msg, tunnel->target_system, tunnel->target_component, tunnel->payload_type, tunnel->payload_length, tunnel->payload); +} + +/** + * @brief Send a tunnel message + * @param chan MAVLink channel to send the message + * + * @param target_system System ID (can be 0 for broadcast, but this is discouraged) + * @param target_component Component ID (can be 0 for broadcast, but this is discouraged) + * @param payload_type A code that identifies the content of the payload (0 for unknown, which is the default). If this code is less than 32768, it is a 'registered' payload type and the corresponding code should be added to the MAV_TUNNEL_PAYLOAD_TYPE enum. Software creators can register blocks of types as needed. Codes greater than 32767 are considered local experiments and should not be checked in to any widely distributed codebase. + * @param payload_length Length of the data transported in payload + * @param payload Variable length payload. The payload length is defined by payload_length. The entire content of this block is opaque unless you understand the encoding specified by payload_type. + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_tunnel_send(mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint16_t payload_type, uint8_t payload_length, const uint8_t *payload) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_TUNNEL_LEN]; + _mav_put_uint16_t(buf, 0, payload_type); + _mav_put_uint8_t(buf, 2, target_system); + _mav_put_uint8_t(buf, 3, target_component); + _mav_put_uint8_t(buf, 4, payload_length); + _mav_put_uint8_t_array(buf, 5, payload, 128); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_TUNNEL, buf, MAVLINK_MSG_ID_TUNNEL_MIN_LEN, MAVLINK_MSG_ID_TUNNEL_LEN, MAVLINK_MSG_ID_TUNNEL_CRC); +#else + mavlink_tunnel_t packet; + packet.payload_type = payload_type; + packet.target_system = target_system; + packet.target_component = target_component; + packet.payload_length = payload_length; + mav_array_memcpy(packet.payload, payload, sizeof(uint8_t)*128); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_TUNNEL, (const char *)&packet, MAVLINK_MSG_ID_TUNNEL_MIN_LEN, MAVLINK_MSG_ID_TUNNEL_LEN, MAVLINK_MSG_ID_TUNNEL_CRC); +#endif +} + +/** + * @brief Send a tunnel message + * @param chan MAVLink channel to send the message + * @param struct The MAVLink struct to serialize + */ +static inline void mavlink_msg_tunnel_send_struct(mavlink_channel_t chan, const mavlink_tunnel_t* tunnel) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + mavlink_msg_tunnel_send(chan, tunnel->target_system, tunnel->target_component, tunnel->payload_type, tunnel->payload_length, tunnel->payload); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_TUNNEL, (const char *)tunnel, MAVLINK_MSG_ID_TUNNEL_MIN_LEN, MAVLINK_MSG_ID_TUNNEL_LEN, MAVLINK_MSG_ID_TUNNEL_CRC); +#endif +} + +#if MAVLINK_MSG_ID_TUNNEL_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_tunnel_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t target_system, uint8_t target_component, uint16_t payload_type, uint8_t payload_length, const uint8_t *payload) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint16_t(buf, 0, payload_type); + _mav_put_uint8_t(buf, 2, target_system); + _mav_put_uint8_t(buf, 3, target_component); + _mav_put_uint8_t(buf, 4, payload_length); + _mav_put_uint8_t_array(buf, 5, payload, 128); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_TUNNEL, buf, MAVLINK_MSG_ID_TUNNEL_MIN_LEN, MAVLINK_MSG_ID_TUNNEL_LEN, MAVLINK_MSG_ID_TUNNEL_CRC); +#else + mavlink_tunnel_t *packet = (mavlink_tunnel_t *)msgbuf; + packet->payload_type = payload_type; + packet->target_system = target_system; + packet->target_component = target_component; + packet->payload_length = payload_length; + mav_array_memcpy(packet->payload, payload, sizeof(uint8_t)*128); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_TUNNEL, (const char *)packet, MAVLINK_MSG_ID_TUNNEL_MIN_LEN, MAVLINK_MSG_ID_TUNNEL_LEN, MAVLINK_MSG_ID_TUNNEL_CRC); +#endif +} +#endif + +#endif + +// MESSAGE TUNNEL UNPACKING + + +/** + * @brief Get field target_system from tunnel message + * + * @return System ID (can be 0 for broadcast, but this is discouraged) + */ +static inline uint8_t mavlink_msg_tunnel_get_target_system(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 2); +} + +/** + * @brief Get field target_component from tunnel message + * + * @return Component ID (can be 0 for broadcast, but this is discouraged) + */ +static inline uint8_t mavlink_msg_tunnel_get_target_component(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 3); +} + +/** + * @brief Get field payload_type from tunnel message + * + * @return A code that identifies the content of the payload (0 for unknown, which is the default). If this code is less than 32768, it is a 'registered' payload type and the corresponding code should be added to the MAV_TUNNEL_PAYLOAD_TYPE enum. Software creators can register blocks of types as needed. Codes greater than 32767 are considered local experiments and should not be checked in to any widely distributed codebase. + */ +static inline uint16_t mavlink_msg_tunnel_get_payload_type(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 0); +} + +/** + * @brief Get field payload_length from tunnel message + * + * @return Length of the data transported in payload + */ +static inline uint8_t mavlink_msg_tunnel_get_payload_length(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 4); +} + +/** + * @brief Get field payload from tunnel message + * + * @return Variable length payload. The payload length is defined by payload_length. The entire content of this block is opaque unless you understand the encoding specified by payload_type. + */ +static inline uint16_t mavlink_msg_tunnel_get_payload(const mavlink_message_t* msg, uint8_t *payload) +{ + return _MAV_RETURN_uint8_t_array(msg, payload, 128, 5); +} + +/** + * @brief Decode a tunnel message into a struct + * + * @param msg The message to decode + * @param tunnel C-struct to decode the message contents into + */ +static inline void mavlink_msg_tunnel_decode(const mavlink_message_t* msg, mavlink_tunnel_t* tunnel) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + tunnel->payload_type = mavlink_msg_tunnel_get_payload_type(msg); + tunnel->target_system = mavlink_msg_tunnel_get_target_system(msg); + tunnel->target_component = mavlink_msg_tunnel_get_target_component(msg); + tunnel->payload_length = mavlink_msg_tunnel_get_payload_length(msg); + mavlink_msg_tunnel_get_payload(msg, tunnel->payload); +#else + uint8_t len = msg->len < MAVLINK_MSG_ID_TUNNEL_LEN? msg->len : MAVLINK_MSG_ID_TUNNEL_LEN; + memset(tunnel, 0, MAVLINK_MSG_ID_TUNNEL_LEN); + memcpy(tunnel, _MAV_PAYLOAD(msg), len); +#endif +} diff --git a/lib/main/MAVLink/common/mavlink_msg_uavcan_node_info.h b/lib/main/MAVLink/common/mavlink_msg_uavcan_node_info.h new file mode 100644 index 0000000000..a042c5c4e5 --- /dev/null +++ b/lib/main/MAVLink/common/mavlink_msg_uavcan_node_info.h @@ -0,0 +1,406 @@ +#pragma once +// MESSAGE UAVCAN_NODE_INFO PACKING + +#define MAVLINK_MSG_ID_UAVCAN_NODE_INFO 311 + + +typedef struct __mavlink_uavcan_node_info_t { + uint64_t time_usec; /*< [us] Timestamp (UNIX Epoch time or time since system boot). The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude of the number.*/ + uint32_t uptime_sec; /*< [s] Time since the start-up of the node.*/ + uint32_t sw_vcs_commit; /*< Version control system (VCS) revision identifier (e.g. git short commit hash). Zero if unknown.*/ + char name[80]; /*< Node name string. For example, "sapog.px4.io".*/ + uint8_t hw_version_major; /*< Hardware major version number.*/ + uint8_t hw_version_minor; /*< Hardware minor version number.*/ + uint8_t hw_unique_id[16]; /*< Hardware unique 128-bit ID.*/ + uint8_t sw_version_major; /*< Software major version number.*/ + uint8_t sw_version_minor; /*< Software minor version number.*/ +} mavlink_uavcan_node_info_t; + +#define MAVLINK_MSG_ID_UAVCAN_NODE_INFO_LEN 116 +#define MAVLINK_MSG_ID_UAVCAN_NODE_INFO_MIN_LEN 116 +#define MAVLINK_MSG_ID_311_LEN 116 +#define MAVLINK_MSG_ID_311_MIN_LEN 116 + +#define MAVLINK_MSG_ID_UAVCAN_NODE_INFO_CRC 95 +#define MAVLINK_MSG_ID_311_CRC 95 + +#define MAVLINK_MSG_UAVCAN_NODE_INFO_FIELD_NAME_LEN 80 +#define MAVLINK_MSG_UAVCAN_NODE_INFO_FIELD_HW_UNIQUE_ID_LEN 16 + +#if MAVLINK_COMMAND_24BIT +#define MAVLINK_MESSAGE_INFO_UAVCAN_NODE_INFO { \ + 311, \ + "UAVCAN_NODE_INFO", \ + 9, \ + { { "time_usec", NULL, MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_uavcan_node_info_t, time_usec) }, \ + { "uptime_sec", NULL, MAVLINK_TYPE_UINT32_T, 0, 8, offsetof(mavlink_uavcan_node_info_t, uptime_sec) }, \ + { "name", NULL, MAVLINK_TYPE_CHAR, 80, 16, offsetof(mavlink_uavcan_node_info_t, name) }, \ + { "hw_version_major", NULL, MAVLINK_TYPE_UINT8_T, 0, 96, offsetof(mavlink_uavcan_node_info_t, hw_version_major) }, \ + { "hw_version_minor", NULL, MAVLINK_TYPE_UINT8_T, 0, 97, offsetof(mavlink_uavcan_node_info_t, hw_version_minor) }, \ + { "hw_unique_id", NULL, MAVLINK_TYPE_UINT8_T, 16, 98, offsetof(mavlink_uavcan_node_info_t, hw_unique_id) }, \ + { "sw_version_major", NULL, MAVLINK_TYPE_UINT8_T, 0, 114, offsetof(mavlink_uavcan_node_info_t, sw_version_major) }, \ + { "sw_version_minor", NULL, MAVLINK_TYPE_UINT8_T, 0, 115, offsetof(mavlink_uavcan_node_info_t, sw_version_minor) }, \ + { "sw_vcs_commit", NULL, MAVLINK_TYPE_UINT32_T, 0, 12, offsetof(mavlink_uavcan_node_info_t, sw_vcs_commit) }, \ + } \ +} +#else +#define MAVLINK_MESSAGE_INFO_UAVCAN_NODE_INFO { \ + "UAVCAN_NODE_INFO", \ + 9, \ + { { "time_usec", NULL, MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_uavcan_node_info_t, time_usec) }, \ + { "uptime_sec", NULL, MAVLINK_TYPE_UINT32_T, 0, 8, offsetof(mavlink_uavcan_node_info_t, uptime_sec) }, \ + { "name", NULL, MAVLINK_TYPE_CHAR, 80, 16, offsetof(mavlink_uavcan_node_info_t, name) }, \ + { "hw_version_major", NULL, MAVLINK_TYPE_UINT8_T, 0, 96, offsetof(mavlink_uavcan_node_info_t, hw_version_major) }, \ + { "hw_version_minor", NULL, MAVLINK_TYPE_UINT8_T, 0, 97, offsetof(mavlink_uavcan_node_info_t, hw_version_minor) }, \ + { "hw_unique_id", NULL, MAVLINK_TYPE_UINT8_T, 16, 98, offsetof(mavlink_uavcan_node_info_t, hw_unique_id) }, \ + { "sw_version_major", NULL, MAVLINK_TYPE_UINT8_T, 0, 114, offsetof(mavlink_uavcan_node_info_t, sw_version_major) }, \ + { "sw_version_minor", NULL, MAVLINK_TYPE_UINT8_T, 0, 115, offsetof(mavlink_uavcan_node_info_t, sw_version_minor) }, \ + { "sw_vcs_commit", NULL, MAVLINK_TYPE_UINT32_T, 0, 12, offsetof(mavlink_uavcan_node_info_t, sw_vcs_commit) }, \ + } \ +} +#endif + +/** + * @brief Pack a uavcan_node_info message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param time_usec [us] Timestamp (UNIX Epoch time or time since system boot). The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude of the number. + * @param uptime_sec [s] Time since the start-up of the node. + * @param name Node name string. For example, "sapog.px4.io". + * @param hw_version_major Hardware major version number. + * @param hw_version_minor Hardware minor version number. + * @param hw_unique_id Hardware unique 128-bit ID. + * @param sw_version_major Software major version number. + * @param sw_version_minor Software minor version number. + * @param sw_vcs_commit Version control system (VCS) revision identifier (e.g. git short commit hash). Zero if unknown. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_uavcan_node_info_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint64_t time_usec, uint32_t uptime_sec, const char *name, uint8_t hw_version_major, uint8_t hw_version_minor, const uint8_t *hw_unique_id, uint8_t sw_version_major, uint8_t sw_version_minor, uint32_t sw_vcs_commit) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_UAVCAN_NODE_INFO_LEN]; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_uint32_t(buf, 8, uptime_sec); + _mav_put_uint32_t(buf, 12, sw_vcs_commit); + _mav_put_uint8_t(buf, 96, hw_version_major); + _mav_put_uint8_t(buf, 97, hw_version_minor); + _mav_put_uint8_t(buf, 114, sw_version_major); + _mav_put_uint8_t(buf, 115, sw_version_minor); + _mav_put_char_array(buf, 16, name, 80); + _mav_put_uint8_t_array(buf, 98, hw_unique_id, 16); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_UAVCAN_NODE_INFO_LEN); +#else + mavlink_uavcan_node_info_t packet; + packet.time_usec = time_usec; + packet.uptime_sec = uptime_sec; + packet.sw_vcs_commit = sw_vcs_commit; + packet.hw_version_major = hw_version_major; + packet.hw_version_minor = hw_version_minor; + packet.sw_version_major = sw_version_major; + packet.sw_version_minor = sw_version_minor; + mav_array_memcpy(packet.name, name, sizeof(char)*80); + mav_array_memcpy(packet.hw_unique_id, hw_unique_id, sizeof(uint8_t)*16); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_UAVCAN_NODE_INFO_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_UAVCAN_NODE_INFO; + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_UAVCAN_NODE_INFO_MIN_LEN, MAVLINK_MSG_ID_UAVCAN_NODE_INFO_LEN, MAVLINK_MSG_ID_UAVCAN_NODE_INFO_CRC); +} + +/** + * @brief Pack a uavcan_node_info message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param time_usec [us] Timestamp (UNIX Epoch time or time since system boot). The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude of the number. + * @param uptime_sec [s] Time since the start-up of the node. + * @param name Node name string. For example, "sapog.px4.io". + * @param hw_version_major Hardware major version number. + * @param hw_version_minor Hardware minor version number. + * @param hw_unique_id Hardware unique 128-bit ID. + * @param sw_version_major Software major version number. + * @param sw_version_minor Software minor version number. + * @param sw_vcs_commit Version control system (VCS) revision identifier (e.g. git short commit hash). Zero if unknown. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_uavcan_node_info_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint64_t time_usec,uint32_t uptime_sec,const char *name,uint8_t hw_version_major,uint8_t hw_version_minor,const uint8_t *hw_unique_id,uint8_t sw_version_major,uint8_t sw_version_minor,uint32_t sw_vcs_commit) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_UAVCAN_NODE_INFO_LEN]; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_uint32_t(buf, 8, uptime_sec); + _mav_put_uint32_t(buf, 12, sw_vcs_commit); + _mav_put_uint8_t(buf, 96, hw_version_major); + _mav_put_uint8_t(buf, 97, hw_version_minor); + _mav_put_uint8_t(buf, 114, sw_version_major); + _mav_put_uint8_t(buf, 115, sw_version_minor); + _mav_put_char_array(buf, 16, name, 80); + _mav_put_uint8_t_array(buf, 98, hw_unique_id, 16); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_UAVCAN_NODE_INFO_LEN); +#else + mavlink_uavcan_node_info_t packet; + packet.time_usec = time_usec; + packet.uptime_sec = uptime_sec; + packet.sw_vcs_commit = sw_vcs_commit; + packet.hw_version_major = hw_version_major; + packet.hw_version_minor = hw_version_minor; + packet.sw_version_major = sw_version_major; + packet.sw_version_minor = sw_version_minor; + mav_array_memcpy(packet.name, name, sizeof(char)*80); + mav_array_memcpy(packet.hw_unique_id, hw_unique_id, sizeof(uint8_t)*16); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_UAVCAN_NODE_INFO_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_UAVCAN_NODE_INFO; + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_UAVCAN_NODE_INFO_MIN_LEN, MAVLINK_MSG_ID_UAVCAN_NODE_INFO_LEN, MAVLINK_MSG_ID_UAVCAN_NODE_INFO_CRC); +} + +/** + * @brief Encode a uavcan_node_info struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param uavcan_node_info C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_uavcan_node_info_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_uavcan_node_info_t* uavcan_node_info) +{ + return mavlink_msg_uavcan_node_info_pack(system_id, component_id, msg, uavcan_node_info->time_usec, uavcan_node_info->uptime_sec, uavcan_node_info->name, uavcan_node_info->hw_version_major, uavcan_node_info->hw_version_minor, uavcan_node_info->hw_unique_id, uavcan_node_info->sw_version_major, uavcan_node_info->sw_version_minor, uavcan_node_info->sw_vcs_commit); +} + +/** + * @brief Encode a uavcan_node_info struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param uavcan_node_info C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_uavcan_node_info_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_uavcan_node_info_t* uavcan_node_info) +{ + return mavlink_msg_uavcan_node_info_pack_chan(system_id, component_id, chan, msg, uavcan_node_info->time_usec, uavcan_node_info->uptime_sec, uavcan_node_info->name, uavcan_node_info->hw_version_major, uavcan_node_info->hw_version_minor, uavcan_node_info->hw_unique_id, uavcan_node_info->sw_version_major, uavcan_node_info->sw_version_minor, uavcan_node_info->sw_vcs_commit); +} + +/** + * @brief Send a uavcan_node_info message + * @param chan MAVLink channel to send the message + * + * @param time_usec [us] Timestamp (UNIX Epoch time or time since system boot). The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude of the number. + * @param uptime_sec [s] Time since the start-up of the node. + * @param name Node name string. For example, "sapog.px4.io". + * @param hw_version_major Hardware major version number. + * @param hw_version_minor Hardware minor version number. + * @param hw_unique_id Hardware unique 128-bit ID. + * @param sw_version_major Software major version number. + * @param sw_version_minor Software minor version number. + * @param sw_vcs_commit Version control system (VCS) revision identifier (e.g. git short commit hash). Zero if unknown. + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_uavcan_node_info_send(mavlink_channel_t chan, uint64_t time_usec, uint32_t uptime_sec, const char *name, uint8_t hw_version_major, uint8_t hw_version_minor, const uint8_t *hw_unique_id, uint8_t sw_version_major, uint8_t sw_version_minor, uint32_t sw_vcs_commit) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_UAVCAN_NODE_INFO_LEN]; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_uint32_t(buf, 8, uptime_sec); + _mav_put_uint32_t(buf, 12, sw_vcs_commit); + _mav_put_uint8_t(buf, 96, hw_version_major); + _mav_put_uint8_t(buf, 97, hw_version_minor); + _mav_put_uint8_t(buf, 114, sw_version_major); + _mav_put_uint8_t(buf, 115, sw_version_minor); + _mav_put_char_array(buf, 16, name, 80); + _mav_put_uint8_t_array(buf, 98, hw_unique_id, 16); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_UAVCAN_NODE_INFO, buf, MAVLINK_MSG_ID_UAVCAN_NODE_INFO_MIN_LEN, MAVLINK_MSG_ID_UAVCAN_NODE_INFO_LEN, MAVLINK_MSG_ID_UAVCAN_NODE_INFO_CRC); +#else + mavlink_uavcan_node_info_t packet; + packet.time_usec = time_usec; + packet.uptime_sec = uptime_sec; + packet.sw_vcs_commit = sw_vcs_commit; + packet.hw_version_major = hw_version_major; + packet.hw_version_minor = hw_version_minor; + packet.sw_version_major = sw_version_major; + packet.sw_version_minor = sw_version_minor; + mav_array_memcpy(packet.name, name, sizeof(char)*80); + mav_array_memcpy(packet.hw_unique_id, hw_unique_id, sizeof(uint8_t)*16); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_UAVCAN_NODE_INFO, (const char *)&packet, MAVLINK_MSG_ID_UAVCAN_NODE_INFO_MIN_LEN, MAVLINK_MSG_ID_UAVCAN_NODE_INFO_LEN, MAVLINK_MSG_ID_UAVCAN_NODE_INFO_CRC); +#endif +} + +/** + * @brief Send a uavcan_node_info message + * @param chan MAVLink channel to send the message + * @param struct The MAVLink struct to serialize + */ +static inline void mavlink_msg_uavcan_node_info_send_struct(mavlink_channel_t chan, const mavlink_uavcan_node_info_t* uavcan_node_info) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + mavlink_msg_uavcan_node_info_send(chan, uavcan_node_info->time_usec, uavcan_node_info->uptime_sec, uavcan_node_info->name, uavcan_node_info->hw_version_major, uavcan_node_info->hw_version_minor, uavcan_node_info->hw_unique_id, uavcan_node_info->sw_version_major, uavcan_node_info->sw_version_minor, uavcan_node_info->sw_vcs_commit); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_UAVCAN_NODE_INFO, (const char *)uavcan_node_info, MAVLINK_MSG_ID_UAVCAN_NODE_INFO_MIN_LEN, MAVLINK_MSG_ID_UAVCAN_NODE_INFO_LEN, MAVLINK_MSG_ID_UAVCAN_NODE_INFO_CRC); +#endif +} + +#if MAVLINK_MSG_ID_UAVCAN_NODE_INFO_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_uavcan_node_info_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint64_t time_usec, uint32_t uptime_sec, const char *name, uint8_t hw_version_major, uint8_t hw_version_minor, const uint8_t *hw_unique_id, uint8_t sw_version_major, uint8_t sw_version_minor, uint32_t sw_vcs_commit) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_uint32_t(buf, 8, uptime_sec); + _mav_put_uint32_t(buf, 12, sw_vcs_commit); + _mav_put_uint8_t(buf, 96, hw_version_major); + _mav_put_uint8_t(buf, 97, hw_version_minor); + _mav_put_uint8_t(buf, 114, sw_version_major); + _mav_put_uint8_t(buf, 115, sw_version_minor); + _mav_put_char_array(buf, 16, name, 80); + _mav_put_uint8_t_array(buf, 98, hw_unique_id, 16); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_UAVCAN_NODE_INFO, buf, MAVLINK_MSG_ID_UAVCAN_NODE_INFO_MIN_LEN, MAVLINK_MSG_ID_UAVCAN_NODE_INFO_LEN, MAVLINK_MSG_ID_UAVCAN_NODE_INFO_CRC); +#else + mavlink_uavcan_node_info_t *packet = (mavlink_uavcan_node_info_t *)msgbuf; + packet->time_usec = time_usec; + packet->uptime_sec = uptime_sec; + packet->sw_vcs_commit = sw_vcs_commit; + packet->hw_version_major = hw_version_major; + packet->hw_version_minor = hw_version_minor; + packet->sw_version_major = sw_version_major; + packet->sw_version_minor = sw_version_minor; + mav_array_memcpy(packet->name, name, sizeof(char)*80); + mav_array_memcpy(packet->hw_unique_id, hw_unique_id, sizeof(uint8_t)*16); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_UAVCAN_NODE_INFO, (const char *)packet, MAVLINK_MSG_ID_UAVCAN_NODE_INFO_MIN_LEN, MAVLINK_MSG_ID_UAVCAN_NODE_INFO_LEN, MAVLINK_MSG_ID_UAVCAN_NODE_INFO_CRC); +#endif +} +#endif + +#endif + +// MESSAGE UAVCAN_NODE_INFO UNPACKING + + +/** + * @brief Get field time_usec from uavcan_node_info message + * + * @return [us] Timestamp (UNIX Epoch time or time since system boot). The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude of the number. + */ +static inline uint64_t mavlink_msg_uavcan_node_info_get_time_usec(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint64_t(msg, 0); +} + +/** + * @brief Get field uptime_sec from uavcan_node_info message + * + * @return [s] Time since the start-up of the node. + */ +static inline uint32_t mavlink_msg_uavcan_node_info_get_uptime_sec(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint32_t(msg, 8); +} + +/** + * @brief Get field name from uavcan_node_info message + * + * @return Node name string. For example, "sapog.px4.io". + */ +static inline uint16_t mavlink_msg_uavcan_node_info_get_name(const mavlink_message_t* msg, char *name) +{ + return _MAV_RETURN_char_array(msg, name, 80, 16); +} + +/** + * @brief Get field hw_version_major from uavcan_node_info message + * + * @return Hardware major version number. + */ +static inline uint8_t mavlink_msg_uavcan_node_info_get_hw_version_major(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 96); +} + +/** + * @brief Get field hw_version_minor from uavcan_node_info message + * + * @return Hardware minor version number. + */ +static inline uint8_t mavlink_msg_uavcan_node_info_get_hw_version_minor(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 97); +} + +/** + * @brief Get field hw_unique_id from uavcan_node_info message + * + * @return Hardware unique 128-bit ID. + */ +static inline uint16_t mavlink_msg_uavcan_node_info_get_hw_unique_id(const mavlink_message_t* msg, uint8_t *hw_unique_id) +{ + return _MAV_RETURN_uint8_t_array(msg, hw_unique_id, 16, 98); +} + +/** + * @brief Get field sw_version_major from uavcan_node_info message + * + * @return Software major version number. + */ +static inline uint8_t mavlink_msg_uavcan_node_info_get_sw_version_major(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 114); +} + +/** + * @brief Get field sw_version_minor from uavcan_node_info message + * + * @return Software minor version number. + */ +static inline uint8_t mavlink_msg_uavcan_node_info_get_sw_version_minor(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 115); +} + +/** + * @brief Get field sw_vcs_commit from uavcan_node_info message + * + * @return Version control system (VCS) revision identifier (e.g. git short commit hash). Zero if unknown. + */ +static inline uint32_t mavlink_msg_uavcan_node_info_get_sw_vcs_commit(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint32_t(msg, 12); +} + +/** + * @brief Decode a uavcan_node_info message into a struct + * + * @param msg The message to decode + * @param uavcan_node_info C-struct to decode the message contents into + */ +static inline void mavlink_msg_uavcan_node_info_decode(const mavlink_message_t* msg, mavlink_uavcan_node_info_t* uavcan_node_info) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + uavcan_node_info->time_usec = mavlink_msg_uavcan_node_info_get_time_usec(msg); + uavcan_node_info->uptime_sec = mavlink_msg_uavcan_node_info_get_uptime_sec(msg); + uavcan_node_info->sw_vcs_commit = mavlink_msg_uavcan_node_info_get_sw_vcs_commit(msg); + mavlink_msg_uavcan_node_info_get_name(msg, uavcan_node_info->name); + uavcan_node_info->hw_version_major = mavlink_msg_uavcan_node_info_get_hw_version_major(msg); + uavcan_node_info->hw_version_minor = mavlink_msg_uavcan_node_info_get_hw_version_minor(msg); + mavlink_msg_uavcan_node_info_get_hw_unique_id(msg, uavcan_node_info->hw_unique_id); + uavcan_node_info->sw_version_major = mavlink_msg_uavcan_node_info_get_sw_version_major(msg); + uavcan_node_info->sw_version_minor = mavlink_msg_uavcan_node_info_get_sw_version_minor(msg); +#else + uint8_t len = msg->len < MAVLINK_MSG_ID_UAVCAN_NODE_INFO_LEN? msg->len : MAVLINK_MSG_ID_UAVCAN_NODE_INFO_LEN; + memset(uavcan_node_info, 0, MAVLINK_MSG_ID_UAVCAN_NODE_INFO_LEN); + memcpy(uavcan_node_info, _MAV_PAYLOAD(msg), len); +#endif +} diff --git a/lib/main/MAVLink/common/mavlink_msg_uavcan_node_status.h b/lib/main/MAVLink/common/mavlink_msg_uavcan_node_status.h new file mode 100644 index 0000000000..6f1d8d8fd3 --- /dev/null +++ b/lib/main/MAVLink/common/mavlink_msg_uavcan_node_status.h @@ -0,0 +1,338 @@ +#pragma once +// MESSAGE UAVCAN_NODE_STATUS PACKING + +#define MAVLINK_MSG_ID_UAVCAN_NODE_STATUS 310 + + +typedef struct __mavlink_uavcan_node_status_t { + uint64_t time_usec; /*< [us] Timestamp (UNIX Epoch time or time since system boot). The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude of the number.*/ + uint32_t uptime_sec; /*< [s] Time since the start-up of the node.*/ + uint16_t vendor_specific_status_code; /*< Vendor-specific status information.*/ + uint8_t health; /*< Generalized node health status.*/ + uint8_t mode; /*< Generalized operating mode.*/ + uint8_t sub_mode; /*< Not used currently.*/ +} mavlink_uavcan_node_status_t; + +#define MAVLINK_MSG_ID_UAVCAN_NODE_STATUS_LEN 17 +#define MAVLINK_MSG_ID_UAVCAN_NODE_STATUS_MIN_LEN 17 +#define MAVLINK_MSG_ID_310_LEN 17 +#define MAVLINK_MSG_ID_310_MIN_LEN 17 + +#define MAVLINK_MSG_ID_UAVCAN_NODE_STATUS_CRC 28 +#define MAVLINK_MSG_ID_310_CRC 28 + + + +#if MAVLINK_COMMAND_24BIT +#define MAVLINK_MESSAGE_INFO_UAVCAN_NODE_STATUS { \ + 310, \ + "UAVCAN_NODE_STATUS", \ + 6, \ + { { "time_usec", NULL, MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_uavcan_node_status_t, time_usec) }, \ + { "uptime_sec", NULL, MAVLINK_TYPE_UINT32_T, 0, 8, offsetof(mavlink_uavcan_node_status_t, uptime_sec) }, \ + { "health", NULL, MAVLINK_TYPE_UINT8_T, 0, 14, offsetof(mavlink_uavcan_node_status_t, health) }, \ + { "mode", NULL, MAVLINK_TYPE_UINT8_T, 0, 15, offsetof(mavlink_uavcan_node_status_t, mode) }, \ + { "sub_mode", NULL, MAVLINK_TYPE_UINT8_T, 0, 16, offsetof(mavlink_uavcan_node_status_t, sub_mode) }, \ + { "vendor_specific_status_code", NULL, MAVLINK_TYPE_UINT16_T, 0, 12, offsetof(mavlink_uavcan_node_status_t, vendor_specific_status_code) }, \ + } \ +} +#else +#define MAVLINK_MESSAGE_INFO_UAVCAN_NODE_STATUS { \ + "UAVCAN_NODE_STATUS", \ + 6, \ + { { "time_usec", NULL, MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_uavcan_node_status_t, time_usec) }, \ + { "uptime_sec", NULL, MAVLINK_TYPE_UINT32_T, 0, 8, offsetof(mavlink_uavcan_node_status_t, uptime_sec) }, \ + { "health", NULL, MAVLINK_TYPE_UINT8_T, 0, 14, offsetof(mavlink_uavcan_node_status_t, health) }, \ + { "mode", NULL, MAVLINK_TYPE_UINT8_T, 0, 15, offsetof(mavlink_uavcan_node_status_t, mode) }, \ + { "sub_mode", NULL, MAVLINK_TYPE_UINT8_T, 0, 16, offsetof(mavlink_uavcan_node_status_t, sub_mode) }, \ + { "vendor_specific_status_code", NULL, MAVLINK_TYPE_UINT16_T, 0, 12, offsetof(mavlink_uavcan_node_status_t, vendor_specific_status_code) }, \ + } \ +} +#endif + +/** + * @brief Pack a uavcan_node_status message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param time_usec [us] Timestamp (UNIX Epoch time or time since system boot). The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude of the number. + * @param uptime_sec [s] Time since the start-up of the node. + * @param health Generalized node health status. + * @param mode Generalized operating mode. + * @param sub_mode Not used currently. + * @param vendor_specific_status_code Vendor-specific status information. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_uavcan_node_status_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint64_t time_usec, uint32_t uptime_sec, uint8_t health, uint8_t mode, uint8_t sub_mode, uint16_t vendor_specific_status_code) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_UAVCAN_NODE_STATUS_LEN]; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_uint32_t(buf, 8, uptime_sec); + _mav_put_uint16_t(buf, 12, vendor_specific_status_code); + _mav_put_uint8_t(buf, 14, health); + _mav_put_uint8_t(buf, 15, mode); + _mav_put_uint8_t(buf, 16, sub_mode); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_UAVCAN_NODE_STATUS_LEN); +#else + mavlink_uavcan_node_status_t packet; + packet.time_usec = time_usec; + packet.uptime_sec = uptime_sec; + packet.vendor_specific_status_code = vendor_specific_status_code; + packet.health = health; + packet.mode = mode; + packet.sub_mode = sub_mode; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_UAVCAN_NODE_STATUS_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_UAVCAN_NODE_STATUS; + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_UAVCAN_NODE_STATUS_MIN_LEN, MAVLINK_MSG_ID_UAVCAN_NODE_STATUS_LEN, MAVLINK_MSG_ID_UAVCAN_NODE_STATUS_CRC); +} + +/** + * @brief Pack a uavcan_node_status message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param time_usec [us] Timestamp (UNIX Epoch time or time since system boot). The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude of the number. + * @param uptime_sec [s] Time since the start-up of the node. + * @param health Generalized node health status. + * @param mode Generalized operating mode. + * @param sub_mode Not used currently. + * @param vendor_specific_status_code Vendor-specific status information. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_uavcan_node_status_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint64_t time_usec,uint32_t uptime_sec,uint8_t health,uint8_t mode,uint8_t sub_mode,uint16_t vendor_specific_status_code) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_UAVCAN_NODE_STATUS_LEN]; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_uint32_t(buf, 8, uptime_sec); + _mav_put_uint16_t(buf, 12, vendor_specific_status_code); + _mav_put_uint8_t(buf, 14, health); + _mav_put_uint8_t(buf, 15, mode); + _mav_put_uint8_t(buf, 16, sub_mode); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_UAVCAN_NODE_STATUS_LEN); +#else + mavlink_uavcan_node_status_t packet; + packet.time_usec = time_usec; + packet.uptime_sec = uptime_sec; + packet.vendor_specific_status_code = vendor_specific_status_code; + packet.health = health; + packet.mode = mode; + packet.sub_mode = sub_mode; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_UAVCAN_NODE_STATUS_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_UAVCAN_NODE_STATUS; + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_UAVCAN_NODE_STATUS_MIN_LEN, MAVLINK_MSG_ID_UAVCAN_NODE_STATUS_LEN, MAVLINK_MSG_ID_UAVCAN_NODE_STATUS_CRC); +} + +/** + * @brief Encode a uavcan_node_status struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param uavcan_node_status C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_uavcan_node_status_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_uavcan_node_status_t* uavcan_node_status) +{ + return mavlink_msg_uavcan_node_status_pack(system_id, component_id, msg, uavcan_node_status->time_usec, uavcan_node_status->uptime_sec, uavcan_node_status->health, uavcan_node_status->mode, uavcan_node_status->sub_mode, uavcan_node_status->vendor_specific_status_code); +} + +/** + * @brief Encode a uavcan_node_status struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param uavcan_node_status C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_uavcan_node_status_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_uavcan_node_status_t* uavcan_node_status) +{ + return mavlink_msg_uavcan_node_status_pack_chan(system_id, component_id, chan, msg, uavcan_node_status->time_usec, uavcan_node_status->uptime_sec, uavcan_node_status->health, uavcan_node_status->mode, uavcan_node_status->sub_mode, uavcan_node_status->vendor_specific_status_code); +} + +/** + * @brief Send a uavcan_node_status message + * @param chan MAVLink channel to send the message + * + * @param time_usec [us] Timestamp (UNIX Epoch time or time since system boot). The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude of the number. + * @param uptime_sec [s] Time since the start-up of the node. + * @param health Generalized node health status. + * @param mode Generalized operating mode. + * @param sub_mode Not used currently. + * @param vendor_specific_status_code Vendor-specific status information. + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_uavcan_node_status_send(mavlink_channel_t chan, uint64_t time_usec, uint32_t uptime_sec, uint8_t health, uint8_t mode, uint8_t sub_mode, uint16_t vendor_specific_status_code) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_UAVCAN_NODE_STATUS_LEN]; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_uint32_t(buf, 8, uptime_sec); + _mav_put_uint16_t(buf, 12, vendor_specific_status_code); + _mav_put_uint8_t(buf, 14, health); + _mav_put_uint8_t(buf, 15, mode); + _mav_put_uint8_t(buf, 16, sub_mode); + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_UAVCAN_NODE_STATUS, buf, MAVLINK_MSG_ID_UAVCAN_NODE_STATUS_MIN_LEN, MAVLINK_MSG_ID_UAVCAN_NODE_STATUS_LEN, MAVLINK_MSG_ID_UAVCAN_NODE_STATUS_CRC); +#else + mavlink_uavcan_node_status_t packet; + packet.time_usec = time_usec; + packet.uptime_sec = uptime_sec; + packet.vendor_specific_status_code = vendor_specific_status_code; + packet.health = health; + packet.mode = mode; + packet.sub_mode = sub_mode; + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_UAVCAN_NODE_STATUS, (const char *)&packet, MAVLINK_MSG_ID_UAVCAN_NODE_STATUS_MIN_LEN, MAVLINK_MSG_ID_UAVCAN_NODE_STATUS_LEN, MAVLINK_MSG_ID_UAVCAN_NODE_STATUS_CRC); +#endif +} + +/** + * @brief Send a uavcan_node_status message + * @param chan MAVLink channel to send the message + * @param struct The MAVLink struct to serialize + */ +static inline void mavlink_msg_uavcan_node_status_send_struct(mavlink_channel_t chan, const mavlink_uavcan_node_status_t* uavcan_node_status) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + mavlink_msg_uavcan_node_status_send(chan, uavcan_node_status->time_usec, uavcan_node_status->uptime_sec, uavcan_node_status->health, uavcan_node_status->mode, uavcan_node_status->sub_mode, uavcan_node_status->vendor_specific_status_code); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_UAVCAN_NODE_STATUS, (const char *)uavcan_node_status, MAVLINK_MSG_ID_UAVCAN_NODE_STATUS_MIN_LEN, MAVLINK_MSG_ID_UAVCAN_NODE_STATUS_LEN, MAVLINK_MSG_ID_UAVCAN_NODE_STATUS_CRC); +#endif +} + +#if MAVLINK_MSG_ID_UAVCAN_NODE_STATUS_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_uavcan_node_status_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint64_t time_usec, uint32_t uptime_sec, uint8_t health, uint8_t mode, uint8_t sub_mode, uint16_t vendor_specific_status_code) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_uint32_t(buf, 8, uptime_sec); + _mav_put_uint16_t(buf, 12, vendor_specific_status_code); + _mav_put_uint8_t(buf, 14, health); + _mav_put_uint8_t(buf, 15, mode); + _mav_put_uint8_t(buf, 16, sub_mode); + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_UAVCAN_NODE_STATUS, buf, MAVLINK_MSG_ID_UAVCAN_NODE_STATUS_MIN_LEN, MAVLINK_MSG_ID_UAVCAN_NODE_STATUS_LEN, MAVLINK_MSG_ID_UAVCAN_NODE_STATUS_CRC); +#else + mavlink_uavcan_node_status_t *packet = (mavlink_uavcan_node_status_t *)msgbuf; + packet->time_usec = time_usec; + packet->uptime_sec = uptime_sec; + packet->vendor_specific_status_code = vendor_specific_status_code; + packet->health = health; + packet->mode = mode; + packet->sub_mode = sub_mode; + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_UAVCAN_NODE_STATUS, (const char *)packet, MAVLINK_MSG_ID_UAVCAN_NODE_STATUS_MIN_LEN, MAVLINK_MSG_ID_UAVCAN_NODE_STATUS_LEN, MAVLINK_MSG_ID_UAVCAN_NODE_STATUS_CRC); +#endif +} +#endif + +#endif + +// MESSAGE UAVCAN_NODE_STATUS UNPACKING + + +/** + * @brief Get field time_usec from uavcan_node_status message + * + * @return [us] Timestamp (UNIX Epoch time or time since system boot). The receiving end can infer timestamp format (since 1.1.1970 or since system boot) by checking for the magnitude of the number. + */ +static inline uint64_t mavlink_msg_uavcan_node_status_get_time_usec(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint64_t(msg, 0); +} + +/** + * @brief Get field uptime_sec from uavcan_node_status message + * + * @return [s] Time since the start-up of the node. + */ +static inline uint32_t mavlink_msg_uavcan_node_status_get_uptime_sec(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint32_t(msg, 8); +} + +/** + * @brief Get field health from uavcan_node_status message + * + * @return Generalized node health status. + */ +static inline uint8_t mavlink_msg_uavcan_node_status_get_health(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 14); +} + +/** + * @brief Get field mode from uavcan_node_status message + * + * @return Generalized operating mode. + */ +static inline uint8_t mavlink_msg_uavcan_node_status_get_mode(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 15); +} + +/** + * @brief Get field sub_mode from uavcan_node_status message + * + * @return Not used currently. + */ +static inline uint8_t mavlink_msg_uavcan_node_status_get_sub_mode(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 16); +} + +/** + * @brief Get field vendor_specific_status_code from uavcan_node_status message + * + * @return Vendor-specific status information. + */ +static inline uint16_t mavlink_msg_uavcan_node_status_get_vendor_specific_status_code(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 12); +} + +/** + * @brief Decode a uavcan_node_status message into a struct + * + * @param msg The message to decode + * @param uavcan_node_status C-struct to decode the message contents into + */ +static inline void mavlink_msg_uavcan_node_status_decode(const mavlink_message_t* msg, mavlink_uavcan_node_status_t* uavcan_node_status) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + uavcan_node_status->time_usec = mavlink_msg_uavcan_node_status_get_time_usec(msg); + uavcan_node_status->uptime_sec = mavlink_msg_uavcan_node_status_get_uptime_sec(msg); + uavcan_node_status->vendor_specific_status_code = mavlink_msg_uavcan_node_status_get_vendor_specific_status_code(msg); + uavcan_node_status->health = mavlink_msg_uavcan_node_status_get_health(msg); + uavcan_node_status->mode = mavlink_msg_uavcan_node_status_get_mode(msg); + uavcan_node_status->sub_mode = mavlink_msg_uavcan_node_status_get_sub_mode(msg); +#else + uint8_t len = msg->len < MAVLINK_MSG_ID_UAVCAN_NODE_STATUS_LEN? msg->len : MAVLINK_MSG_ID_UAVCAN_NODE_STATUS_LEN; + memset(uavcan_node_status, 0, MAVLINK_MSG_ID_UAVCAN_NODE_STATUS_LEN); + memcpy(uavcan_node_status, _MAV_PAYLOAD(msg), len); +#endif +} diff --git a/lib/main/MAVLink/common/mavlink_msg_utm_global_position.h b/lib/main/MAVLink/common/mavlink_msg_utm_global_position.h new file mode 100644 index 0000000000..0c8ee72263 --- /dev/null +++ b/lib/main/MAVLink/common/mavlink_msg_utm_global_position.h @@ -0,0 +1,630 @@ +#pragma once +// MESSAGE UTM_GLOBAL_POSITION PACKING + +#define MAVLINK_MSG_ID_UTM_GLOBAL_POSITION 340 + + +typedef struct __mavlink_utm_global_position_t { + uint64_t time; /*< [us] Time of applicability of position (microseconds since UNIX epoch).*/ + int32_t lat; /*< [degE7] Latitude (WGS84)*/ + int32_t lon; /*< [degE7] Longitude (WGS84)*/ + int32_t alt; /*< [mm] Altitude (WGS84)*/ + int32_t relative_alt; /*< [mm] Altitude above ground*/ + int32_t next_lat; /*< [degE7] Next waypoint, latitude (WGS84)*/ + int32_t next_lon; /*< [degE7] Next waypoint, longitude (WGS84)*/ + int32_t next_alt; /*< [mm] Next waypoint, altitude (WGS84)*/ + int16_t vx; /*< [cm/s] Ground X speed (latitude, positive north)*/ + int16_t vy; /*< [cm/s] Ground Y speed (longitude, positive east)*/ + int16_t vz; /*< [cm/s] Ground Z speed (altitude, positive down)*/ + uint16_t h_acc; /*< [mm] Horizontal position uncertainty (standard deviation)*/ + uint16_t v_acc; /*< [mm] Altitude uncertainty (standard deviation)*/ + uint16_t vel_acc; /*< [cm/s] Speed uncertainty (standard deviation)*/ + uint16_t update_rate; /*< [cs] Time until next update. Set to 0 if unknown or in data driven mode.*/ + uint8_t uas_id[18]; /*< Unique UAS ID.*/ + uint8_t flight_state; /*< Flight state*/ + uint8_t flags; /*< Bitwise OR combination of the data available flags.*/ +} mavlink_utm_global_position_t; + +#define MAVLINK_MSG_ID_UTM_GLOBAL_POSITION_LEN 70 +#define MAVLINK_MSG_ID_UTM_GLOBAL_POSITION_MIN_LEN 70 +#define MAVLINK_MSG_ID_340_LEN 70 +#define MAVLINK_MSG_ID_340_MIN_LEN 70 + +#define MAVLINK_MSG_ID_UTM_GLOBAL_POSITION_CRC 99 +#define MAVLINK_MSG_ID_340_CRC 99 + +#define MAVLINK_MSG_UTM_GLOBAL_POSITION_FIELD_UAS_ID_LEN 18 + +#if MAVLINK_COMMAND_24BIT +#define MAVLINK_MESSAGE_INFO_UTM_GLOBAL_POSITION { \ + 340, \ + "UTM_GLOBAL_POSITION", \ + 18, \ + { { "time", NULL, MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_utm_global_position_t, time) }, \ + { "uas_id", NULL, MAVLINK_TYPE_UINT8_T, 18, 50, offsetof(mavlink_utm_global_position_t, uas_id) }, \ + { "lat", NULL, MAVLINK_TYPE_INT32_T, 0, 8, offsetof(mavlink_utm_global_position_t, lat) }, \ + { "lon", NULL, MAVLINK_TYPE_INT32_T, 0, 12, offsetof(mavlink_utm_global_position_t, lon) }, \ + { "alt", NULL, MAVLINK_TYPE_INT32_T, 0, 16, offsetof(mavlink_utm_global_position_t, alt) }, \ + { "relative_alt", NULL, MAVLINK_TYPE_INT32_T, 0, 20, offsetof(mavlink_utm_global_position_t, relative_alt) }, \ + { "vx", NULL, MAVLINK_TYPE_INT16_T, 0, 36, offsetof(mavlink_utm_global_position_t, vx) }, \ + { "vy", NULL, MAVLINK_TYPE_INT16_T, 0, 38, offsetof(mavlink_utm_global_position_t, vy) }, \ + { "vz", NULL, MAVLINK_TYPE_INT16_T, 0, 40, offsetof(mavlink_utm_global_position_t, vz) }, \ + { "h_acc", NULL, MAVLINK_TYPE_UINT16_T, 0, 42, offsetof(mavlink_utm_global_position_t, h_acc) }, \ + { "v_acc", NULL, MAVLINK_TYPE_UINT16_T, 0, 44, offsetof(mavlink_utm_global_position_t, v_acc) }, \ + { "vel_acc", NULL, MAVLINK_TYPE_UINT16_T, 0, 46, offsetof(mavlink_utm_global_position_t, vel_acc) }, \ + { "next_lat", NULL, MAVLINK_TYPE_INT32_T, 0, 24, offsetof(mavlink_utm_global_position_t, next_lat) }, \ + { "next_lon", NULL, MAVLINK_TYPE_INT32_T, 0, 28, offsetof(mavlink_utm_global_position_t, next_lon) }, \ + { "next_alt", NULL, MAVLINK_TYPE_INT32_T, 0, 32, offsetof(mavlink_utm_global_position_t, next_alt) }, \ + { "update_rate", NULL, MAVLINK_TYPE_UINT16_T, 0, 48, offsetof(mavlink_utm_global_position_t, update_rate) }, \ + { "flight_state", NULL, MAVLINK_TYPE_UINT8_T, 0, 68, offsetof(mavlink_utm_global_position_t, flight_state) }, \ + { "flags", NULL, MAVLINK_TYPE_UINT8_T, 0, 69, offsetof(mavlink_utm_global_position_t, flags) }, \ + } \ +} +#else +#define MAVLINK_MESSAGE_INFO_UTM_GLOBAL_POSITION { \ + "UTM_GLOBAL_POSITION", \ + 18, \ + { { "time", NULL, MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_utm_global_position_t, time) }, \ + { "uas_id", NULL, MAVLINK_TYPE_UINT8_T, 18, 50, offsetof(mavlink_utm_global_position_t, uas_id) }, \ + { "lat", NULL, MAVLINK_TYPE_INT32_T, 0, 8, offsetof(mavlink_utm_global_position_t, lat) }, \ + { "lon", NULL, MAVLINK_TYPE_INT32_T, 0, 12, offsetof(mavlink_utm_global_position_t, lon) }, \ + { "alt", NULL, MAVLINK_TYPE_INT32_T, 0, 16, offsetof(mavlink_utm_global_position_t, alt) }, \ + { "relative_alt", NULL, MAVLINK_TYPE_INT32_T, 0, 20, offsetof(mavlink_utm_global_position_t, relative_alt) }, \ + { "vx", NULL, MAVLINK_TYPE_INT16_T, 0, 36, offsetof(mavlink_utm_global_position_t, vx) }, \ + { "vy", NULL, MAVLINK_TYPE_INT16_T, 0, 38, offsetof(mavlink_utm_global_position_t, vy) }, \ + { "vz", NULL, MAVLINK_TYPE_INT16_T, 0, 40, offsetof(mavlink_utm_global_position_t, vz) }, \ + { "h_acc", NULL, MAVLINK_TYPE_UINT16_T, 0, 42, offsetof(mavlink_utm_global_position_t, h_acc) }, \ + { "v_acc", NULL, MAVLINK_TYPE_UINT16_T, 0, 44, offsetof(mavlink_utm_global_position_t, v_acc) }, \ + { "vel_acc", NULL, MAVLINK_TYPE_UINT16_T, 0, 46, offsetof(mavlink_utm_global_position_t, vel_acc) }, \ + { "next_lat", NULL, MAVLINK_TYPE_INT32_T, 0, 24, offsetof(mavlink_utm_global_position_t, next_lat) }, \ + { "next_lon", NULL, MAVLINK_TYPE_INT32_T, 0, 28, offsetof(mavlink_utm_global_position_t, next_lon) }, \ + { "next_alt", NULL, MAVLINK_TYPE_INT32_T, 0, 32, offsetof(mavlink_utm_global_position_t, next_alt) }, \ + { "update_rate", NULL, MAVLINK_TYPE_UINT16_T, 0, 48, offsetof(mavlink_utm_global_position_t, update_rate) }, \ + { "flight_state", NULL, MAVLINK_TYPE_UINT8_T, 0, 68, offsetof(mavlink_utm_global_position_t, flight_state) }, \ + { "flags", NULL, MAVLINK_TYPE_UINT8_T, 0, 69, offsetof(mavlink_utm_global_position_t, flags) }, \ + } \ +} +#endif + +/** + * @brief Pack a utm_global_position message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param time [us] Time of applicability of position (microseconds since UNIX epoch). + * @param uas_id Unique UAS ID. + * @param lat [degE7] Latitude (WGS84) + * @param lon [degE7] Longitude (WGS84) + * @param alt [mm] Altitude (WGS84) + * @param relative_alt [mm] Altitude above ground + * @param vx [cm/s] Ground X speed (latitude, positive north) + * @param vy [cm/s] Ground Y speed (longitude, positive east) + * @param vz [cm/s] Ground Z speed (altitude, positive down) + * @param h_acc [mm] Horizontal position uncertainty (standard deviation) + * @param v_acc [mm] Altitude uncertainty (standard deviation) + * @param vel_acc [cm/s] Speed uncertainty (standard deviation) + * @param next_lat [degE7] Next waypoint, latitude (WGS84) + * @param next_lon [degE7] Next waypoint, longitude (WGS84) + * @param next_alt [mm] Next waypoint, altitude (WGS84) + * @param update_rate [cs] Time until next update. Set to 0 if unknown or in data driven mode. + * @param flight_state Flight state + * @param flags Bitwise OR combination of the data available flags. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_utm_global_position_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint64_t time, const uint8_t *uas_id, int32_t lat, int32_t lon, int32_t alt, int32_t relative_alt, int16_t vx, int16_t vy, int16_t vz, uint16_t h_acc, uint16_t v_acc, uint16_t vel_acc, int32_t next_lat, int32_t next_lon, int32_t next_alt, uint16_t update_rate, uint8_t flight_state, uint8_t flags) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_UTM_GLOBAL_POSITION_LEN]; + _mav_put_uint64_t(buf, 0, time); + _mav_put_int32_t(buf, 8, lat); + _mav_put_int32_t(buf, 12, lon); + _mav_put_int32_t(buf, 16, alt); + _mav_put_int32_t(buf, 20, relative_alt); + _mav_put_int32_t(buf, 24, next_lat); + _mav_put_int32_t(buf, 28, next_lon); + _mav_put_int32_t(buf, 32, next_alt); + _mav_put_int16_t(buf, 36, vx); + _mav_put_int16_t(buf, 38, vy); + _mav_put_int16_t(buf, 40, vz); + _mav_put_uint16_t(buf, 42, h_acc); + _mav_put_uint16_t(buf, 44, v_acc); + _mav_put_uint16_t(buf, 46, vel_acc); + _mav_put_uint16_t(buf, 48, update_rate); + _mav_put_uint8_t(buf, 68, flight_state); + _mav_put_uint8_t(buf, 69, flags); + _mav_put_uint8_t_array(buf, 50, uas_id, 18); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_UTM_GLOBAL_POSITION_LEN); +#else + mavlink_utm_global_position_t packet; + packet.time = time; + packet.lat = lat; + packet.lon = lon; + packet.alt = alt; + packet.relative_alt = relative_alt; + packet.next_lat = next_lat; + packet.next_lon = next_lon; + packet.next_alt = next_alt; + packet.vx = vx; + packet.vy = vy; + packet.vz = vz; + packet.h_acc = h_acc; + packet.v_acc = v_acc; + packet.vel_acc = vel_acc; + packet.update_rate = update_rate; + packet.flight_state = flight_state; + packet.flags = flags; + mav_array_memcpy(packet.uas_id, uas_id, sizeof(uint8_t)*18); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_UTM_GLOBAL_POSITION_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_UTM_GLOBAL_POSITION; + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_UTM_GLOBAL_POSITION_MIN_LEN, MAVLINK_MSG_ID_UTM_GLOBAL_POSITION_LEN, MAVLINK_MSG_ID_UTM_GLOBAL_POSITION_CRC); +} + +/** + * @brief Pack a utm_global_position message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param time [us] Time of applicability of position (microseconds since UNIX epoch). + * @param uas_id Unique UAS ID. + * @param lat [degE7] Latitude (WGS84) + * @param lon [degE7] Longitude (WGS84) + * @param alt [mm] Altitude (WGS84) + * @param relative_alt [mm] Altitude above ground + * @param vx [cm/s] Ground X speed (latitude, positive north) + * @param vy [cm/s] Ground Y speed (longitude, positive east) + * @param vz [cm/s] Ground Z speed (altitude, positive down) + * @param h_acc [mm] Horizontal position uncertainty (standard deviation) + * @param v_acc [mm] Altitude uncertainty (standard deviation) + * @param vel_acc [cm/s] Speed uncertainty (standard deviation) + * @param next_lat [degE7] Next waypoint, latitude (WGS84) + * @param next_lon [degE7] Next waypoint, longitude (WGS84) + * @param next_alt [mm] Next waypoint, altitude (WGS84) + * @param update_rate [cs] Time until next update. Set to 0 if unknown or in data driven mode. + * @param flight_state Flight state + * @param flags Bitwise OR combination of the data available flags. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_utm_global_position_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint64_t time,const uint8_t *uas_id,int32_t lat,int32_t lon,int32_t alt,int32_t relative_alt,int16_t vx,int16_t vy,int16_t vz,uint16_t h_acc,uint16_t v_acc,uint16_t vel_acc,int32_t next_lat,int32_t next_lon,int32_t next_alt,uint16_t update_rate,uint8_t flight_state,uint8_t flags) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_UTM_GLOBAL_POSITION_LEN]; + _mav_put_uint64_t(buf, 0, time); + _mav_put_int32_t(buf, 8, lat); + _mav_put_int32_t(buf, 12, lon); + _mav_put_int32_t(buf, 16, alt); + _mav_put_int32_t(buf, 20, relative_alt); + _mav_put_int32_t(buf, 24, next_lat); + _mav_put_int32_t(buf, 28, next_lon); + _mav_put_int32_t(buf, 32, next_alt); + _mav_put_int16_t(buf, 36, vx); + _mav_put_int16_t(buf, 38, vy); + _mav_put_int16_t(buf, 40, vz); + _mav_put_uint16_t(buf, 42, h_acc); + _mav_put_uint16_t(buf, 44, v_acc); + _mav_put_uint16_t(buf, 46, vel_acc); + _mav_put_uint16_t(buf, 48, update_rate); + _mav_put_uint8_t(buf, 68, flight_state); + _mav_put_uint8_t(buf, 69, flags); + _mav_put_uint8_t_array(buf, 50, uas_id, 18); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_UTM_GLOBAL_POSITION_LEN); +#else + mavlink_utm_global_position_t packet; + packet.time = time; + packet.lat = lat; + packet.lon = lon; + packet.alt = alt; + packet.relative_alt = relative_alt; + packet.next_lat = next_lat; + packet.next_lon = next_lon; + packet.next_alt = next_alt; + packet.vx = vx; + packet.vy = vy; + packet.vz = vz; + packet.h_acc = h_acc; + packet.v_acc = v_acc; + packet.vel_acc = vel_acc; + packet.update_rate = update_rate; + packet.flight_state = flight_state; + packet.flags = flags; + mav_array_memcpy(packet.uas_id, uas_id, sizeof(uint8_t)*18); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_UTM_GLOBAL_POSITION_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_UTM_GLOBAL_POSITION; + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_UTM_GLOBAL_POSITION_MIN_LEN, MAVLINK_MSG_ID_UTM_GLOBAL_POSITION_LEN, MAVLINK_MSG_ID_UTM_GLOBAL_POSITION_CRC); +} + +/** + * @brief Encode a utm_global_position struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param utm_global_position C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_utm_global_position_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_utm_global_position_t* utm_global_position) +{ + return mavlink_msg_utm_global_position_pack(system_id, component_id, msg, utm_global_position->time, utm_global_position->uas_id, utm_global_position->lat, utm_global_position->lon, utm_global_position->alt, utm_global_position->relative_alt, utm_global_position->vx, utm_global_position->vy, utm_global_position->vz, utm_global_position->h_acc, utm_global_position->v_acc, utm_global_position->vel_acc, utm_global_position->next_lat, utm_global_position->next_lon, utm_global_position->next_alt, utm_global_position->update_rate, utm_global_position->flight_state, utm_global_position->flags); +} + +/** + * @brief Encode a utm_global_position struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param utm_global_position C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_utm_global_position_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_utm_global_position_t* utm_global_position) +{ + return mavlink_msg_utm_global_position_pack_chan(system_id, component_id, chan, msg, utm_global_position->time, utm_global_position->uas_id, utm_global_position->lat, utm_global_position->lon, utm_global_position->alt, utm_global_position->relative_alt, utm_global_position->vx, utm_global_position->vy, utm_global_position->vz, utm_global_position->h_acc, utm_global_position->v_acc, utm_global_position->vel_acc, utm_global_position->next_lat, utm_global_position->next_lon, utm_global_position->next_alt, utm_global_position->update_rate, utm_global_position->flight_state, utm_global_position->flags); +} + +/** + * @brief Send a utm_global_position message + * @param chan MAVLink channel to send the message + * + * @param time [us] Time of applicability of position (microseconds since UNIX epoch). + * @param uas_id Unique UAS ID. + * @param lat [degE7] Latitude (WGS84) + * @param lon [degE7] Longitude (WGS84) + * @param alt [mm] Altitude (WGS84) + * @param relative_alt [mm] Altitude above ground + * @param vx [cm/s] Ground X speed (latitude, positive north) + * @param vy [cm/s] Ground Y speed (longitude, positive east) + * @param vz [cm/s] Ground Z speed (altitude, positive down) + * @param h_acc [mm] Horizontal position uncertainty (standard deviation) + * @param v_acc [mm] Altitude uncertainty (standard deviation) + * @param vel_acc [cm/s] Speed uncertainty (standard deviation) + * @param next_lat [degE7] Next waypoint, latitude (WGS84) + * @param next_lon [degE7] Next waypoint, longitude (WGS84) + * @param next_alt [mm] Next waypoint, altitude (WGS84) + * @param update_rate [cs] Time until next update. Set to 0 if unknown or in data driven mode. + * @param flight_state Flight state + * @param flags Bitwise OR combination of the data available flags. + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_utm_global_position_send(mavlink_channel_t chan, uint64_t time, const uint8_t *uas_id, int32_t lat, int32_t lon, int32_t alt, int32_t relative_alt, int16_t vx, int16_t vy, int16_t vz, uint16_t h_acc, uint16_t v_acc, uint16_t vel_acc, int32_t next_lat, int32_t next_lon, int32_t next_alt, uint16_t update_rate, uint8_t flight_state, uint8_t flags) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_UTM_GLOBAL_POSITION_LEN]; + _mav_put_uint64_t(buf, 0, time); + _mav_put_int32_t(buf, 8, lat); + _mav_put_int32_t(buf, 12, lon); + _mav_put_int32_t(buf, 16, alt); + _mav_put_int32_t(buf, 20, relative_alt); + _mav_put_int32_t(buf, 24, next_lat); + _mav_put_int32_t(buf, 28, next_lon); + _mav_put_int32_t(buf, 32, next_alt); + _mav_put_int16_t(buf, 36, vx); + _mav_put_int16_t(buf, 38, vy); + _mav_put_int16_t(buf, 40, vz); + _mav_put_uint16_t(buf, 42, h_acc); + _mav_put_uint16_t(buf, 44, v_acc); + _mav_put_uint16_t(buf, 46, vel_acc); + _mav_put_uint16_t(buf, 48, update_rate); + _mav_put_uint8_t(buf, 68, flight_state); + _mav_put_uint8_t(buf, 69, flags); + _mav_put_uint8_t_array(buf, 50, uas_id, 18); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_UTM_GLOBAL_POSITION, buf, MAVLINK_MSG_ID_UTM_GLOBAL_POSITION_MIN_LEN, MAVLINK_MSG_ID_UTM_GLOBAL_POSITION_LEN, MAVLINK_MSG_ID_UTM_GLOBAL_POSITION_CRC); +#else + mavlink_utm_global_position_t packet; + packet.time = time; + packet.lat = lat; + packet.lon = lon; + packet.alt = alt; + packet.relative_alt = relative_alt; + packet.next_lat = next_lat; + packet.next_lon = next_lon; + packet.next_alt = next_alt; + packet.vx = vx; + packet.vy = vy; + packet.vz = vz; + packet.h_acc = h_acc; + packet.v_acc = v_acc; + packet.vel_acc = vel_acc; + packet.update_rate = update_rate; + packet.flight_state = flight_state; + packet.flags = flags; + mav_array_memcpy(packet.uas_id, uas_id, sizeof(uint8_t)*18); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_UTM_GLOBAL_POSITION, (const char *)&packet, MAVLINK_MSG_ID_UTM_GLOBAL_POSITION_MIN_LEN, MAVLINK_MSG_ID_UTM_GLOBAL_POSITION_LEN, MAVLINK_MSG_ID_UTM_GLOBAL_POSITION_CRC); +#endif +} + +/** + * @brief Send a utm_global_position message + * @param chan MAVLink channel to send the message + * @param struct The MAVLink struct to serialize + */ +static inline void mavlink_msg_utm_global_position_send_struct(mavlink_channel_t chan, const mavlink_utm_global_position_t* utm_global_position) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + mavlink_msg_utm_global_position_send(chan, utm_global_position->time, utm_global_position->uas_id, utm_global_position->lat, utm_global_position->lon, utm_global_position->alt, utm_global_position->relative_alt, utm_global_position->vx, utm_global_position->vy, utm_global_position->vz, utm_global_position->h_acc, utm_global_position->v_acc, utm_global_position->vel_acc, utm_global_position->next_lat, utm_global_position->next_lon, utm_global_position->next_alt, utm_global_position->update_rate, utm_global_position->flight_state, utm_global_position->flags); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_UTM_GLOBAL_POSITION, (const char *)utm_global_position, MAVLINK_MSG_ID_UTM_GLOBAL_POSITION_MIN_LEN, MAVLINK_MSG_ID_UTM_GLOBAL_POSITION_LEN, MAVLINK_MSG_ID_UTM_GLOBAL_POSITION_CRC); +#endif +} + +#if MAVLINK_MSG_ID_UTM_GLOBAL_POSITION_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_utm_global_position_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint64_t time, const uint8_t *uas_id, int32_t lat, int32_t lon, int32_t alt, int32_t relative_alt, int16_t vx, int16_t vy, int16_t vz, uint16_t h_acc, uint16_t v_acc, uint16_t vel_acc, int32_t next_lat, int32_t next_lon, int32_t next_alt, uint16_t update_rate, uint8_t flight_state, uint8_t flags) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint64_t(buf, 0, time); + _mav_put_int32_t(buf, 8, lat); + _mav_put_int32_t(buf, 12, lon); + _mav_put_int32_t(buf, 16, alt); + _mav_put_int32_t(buf, 20, relative_alt); + _mav_put_int32_t(buf, 24, next_lat); + _mav_put_int32_t(buf, 28, next_lon); + _mav_put_int32_t(buf, 32, next_alt); + _mav_put_int16_t(buf, 36, vx); + _mav_put_int16_t(buf, 38, vy); + _mav_put_int16_t(buf, 40, vz); + _mav_put_uint16_t(buf, 42, h_acc); + _mav_put_uint16_t(buf, 44, v_acc); + _mav_put_uint16_t(buf, 46, vel_acc); + _mav_put_uint16_t(buf, 48, update_rate); + _mav_put_uint8_t(buf, 68, flight_state); + _mav_put_uint8_t(buf, 69, flags); + _mav_put_uint8_t_array(buf, 50, uas_id, 18); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_UTM_GLOBAL_POSITION, buf, MAVLINK_MSG_ID_UTM_GLOBAL_POSITION_MIN_LEN, MAVLINK_MSG_ID_UTM_GLOBAL_POSITION_LEN, MAVLINK_MSG_ID_UTM_GLOBAL_POSITION_CRC); +#else + mavlink_utm_global_position_t *packet = (mavlink_utm_global_position_t *)msgbuf; + packet->time = time; + packet->lat = lat; + packet->lon = lon; + packet->alt = alt; + packet->relative_alt = relative_alt; + packet->next_lat = next_lat; + packet->next_lon = next_lon; + packet->next_alt = next_alt; + packet->vx = vx; + packet->vy = vy; + packet->vz = vz; + packet->h_acc = h_acc; + packet->v_acc = v_acc; + packet->vel_acc = vel_acc; + packet->update_rate = update_rate; + packet->flight_state = flight_state; + packet->flags = flags; + mav_array_memcpy(packet->uas_id, uas_id, sizeof(uint8_t)*18); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_UTM_GLOBAL_POSITION, (const char *)packet, MAVLINK_MSG_ID_UTM_GLOBAL_POSITION_MIN_LEN, MAVLINK_MSG_ID_UTM_GLOBAL_POSITION_LEN, MAVLINK_MSG_ID_UTM_GLOBAL_POSITION_CRC); +#endif +} +#endif + +#endif + +// MESSAGE UTM_GLOBAL_POSITION UNPACKING + + +/** + * @brief Get field time from utm_global_position message + * + * @return [us] Time of applicability of position (microseconds since UNIX epoch). + */ +static inline uint64_t mavlink_msg_utm_global_position_get_time(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint64_t(msg, 0); +} + +/** + * @brief Get field uas_id from utm_global_position message + * + * @return Unique UAS ID. + */ +static inline uint16_t mavlink_msg_utm_global_position_get_uas_id(const mavlink_message_t* msg, uint8_t *uas_id) +{ + return _MAV_RETURN_uint8_t_array(msg, uas_id, 18, 50); +} + +/** + * @brief Get field lat from utm_global_position message + * + * @return [degE7] Latitude (WGS84) + */ +static inline int32_t mavlink_msg_utm_global_position_get_lat(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int32_t(msg, 8); +} + +/** + * @brief Get field lon from utm_global_position message + * + * @return [degE7] Longitude (WGS84) + */ +static inline int32_t mavlink_msg_utm_global_position_get_lon(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int32_t(msg, 12); +} + +/** + * @brief Get field alt from utm_global_position message + * + * @return [mm] Altitude (WGS84) + */ +static inline int32_t mavlink_msg_utm_global_position_get_alt(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int32_t(msg, 16); +} + +/** + * @brief Get field relative_alt from utm_global_position message + * + * @return [mm] Altitude above ground + */ +static inline int32_t mavlink_msg_utm_global_position_get_relative_alt(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int32_t(msg, 20); +} + +/** + * @brief Get field vx from utm_global_position message + * + * @return [cm/s] Ground X speed (latitude, positive north) + */ +static inline int16_t mavlink_msg_utm_global_position_get_vx(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int16_t(msg, 36); +} + +/** + * @brief Get field vy from utm_global_position message + * + * @return [cm/s] Ground Y speed (longitude, positive east) + */ +static inline int16_t mavlink_msg_utm_global_position_get_vy(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int16_t(msg, 38); +} + +/** + * @brief Get field vz from utm_global_position message + * + * @return [cm/s] Ground Z speed (altitude, positive down) + */ +static inline int16_t mavlink_msg_utm_global_position_get_vz(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int16_t(msg, 40); +} + +/** + * @brief Get field h_acc from utm_global_position message + * + * @return [mm] Horizontal position uncertainty (standard deviation) + */ +static inline uint16_t mavlink_msg_utm_global_position_get_h_acc(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 42); +} + +/** + * @brief Get field v_acc from utm_global_position message + * + * @return [mm] Altitude uncertainty (standard deviation) + */ +static inline uint16_t mavlink_msg_utm_global_position_get_v_acc(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 44); +} + +/** + * @brief Get field vel_acc from utm_global_position message + * + * @return [cm/s] Speed uncertainty (standard deviation) + */ +static inline uint16_t mavlink_msg_utm_global_position_get_vel_acc(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 46); +} + +/** + * @brief Get field next_lat from utm_global_position message + * + * @return [degE7] Next waypoint, latitude (WGS84) + */ +static inline int32_t mavlink_msg_utm_global_position_get_next_lat(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int32_t(msg, 24); +} + +/** + * @brief Get field next_lon from utm_global_position message + * + * @return [degE7] Next waypoint, longitude (WGS84) + */ +static inline int32_t mavlink_msg_utm_global_position_get_next_lon(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int32_t(msg, 28); +} + +/** + * @brief Get field next_alt from utm_global_position message + * + * @return [mm] Next waypoint, altitude (WGS84) + */ +static inline int32_t mavlink_msg_utm_global_position_get_next_alt(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int32_t(msg, 32); +} + +/** + * @brief Get field update_rate from utm_global_position message + * + * @return [cs] Time until next update. Set to 0 if unknown or in data driven mode. + */ +static inline uint16_t mavlink_msg_utm_global_position_get_update_rate(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 48); +} + +/** + * @brief Get field flight_state from utm_global_position message + * + * @return Flight state + */ +static inline uint8_t mavlink_msg_utm_global_position_get_flight_state(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 68); +} + +/** + * @brief Get field flags from utm_global_position message + * + * @return Bitwise OR combination of the data available flags. + */ +static inline uint8_t mavlink_msg_utm_global_position_get_flags(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 69); +} + +/** + * @brief Decode a utm_global_position message into a struct + * + * @param msg The message to decode + * @param utm_global_position C-struct to decode the message contents into + */ +static inline void mavlink_msg_utm_global_position_decode(const mavlink_message_t* msg, mavlink_utm_global_position_t* utm_global_position) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + utm_global_position->time = mavlink_msg_utm_global_position_get_time(msg); + utm_global_position->lat = mavlink_msg_utm_global_position_get_lat(msg); + utm_global_position->lon = mavlink_msg_utm_global_position_get_lon(msg); + utm_global_position->alt = mavlink_msg_utm_global_position_get_alt(msg); + utm_global_position->relative_alt = mavlink_msg_utm_global_position_get_relative_alt(msg); + utm_global_position->next_lat = mavlink_msg_utm_global_position_get_next_lat(msg); + utm_global_position->next_lon = mavlink_msg_utm_global_position_get_next_lon(msg); + utm_global_position->next_alt = mavlink_msg_utm_global_position_get_next_alt(msg); + utm_global_position->vx = mavlink_msg_utm_global_position_get_vx(msg); + utm_global_position->vy = mavlink_msg_utm_global_position_get_vy(msg); + utm_global_position->vz = mavlink_msg_utm_global_position_get_vz(msg); + utm_global_position->h_acc = mavlink_msg_utm_global_position_get_h_acc(msg); + utm_global_position->v_acc = mavlink_msg_utm_global_position_get_v_acc(msg); + utm_global_position->vel_acc = mavlink_msg_utm_global_position_get_vel_acc(msg); + utm_global_position->update_rate = mavlink_msg_utm_global_position_get_update_rate(msg); + mavlink_msg_utm_global_position_get_uas_id(msg, utm_global_position->uas_id); + utm_global_position->flight_state = mavlink_msg_utm_global_position_get_flight_state(msg); + utm_global_position->flags = mavlink_msg_utm_global_position_get_flags(msg); +#else + uint8_t len = msg->len < MAVLINK_MSG_ID_UTM_GLOBAL_POSITION_LEN? msg->len : MAVLINK_MSG_ID_UTM_GLOBAL_POSITION_LEN; + memset(utm_global_position, 0, MAVLINK_MSG_ID_UTM_GLOBAL_POSITION_LEN); + memcpy(utm_global_position, _MAV_PAYLOAD(msg), len); +#endif +} diff --git a/lib/main/MAVLink/common/mavlink_msg_vicon_position_estimate.h b/lib/main/MAVLink/common/mavlink_msg_vicon_position_estimate.h index 4ad2a73885..753f526037 100755 --- a/lib/main/MAVLink/common/mavlink_msg_vicon_position_estimate.h +++ b/lib/main/MAVLink/common/mavlink_msg_vicon_position_estimate.h @@ -12,23 +12,24 @@ typedef struct __mavlink_vicon_position_estimate_t { float roll; /*< [rad] Roll angle*/ float pitch; /*< [rad] Pitch angle*/ float yaw; /*< [rad] Yaw angle*/ + float covariance[21]; /*< Row-major representation of 6x6 pose cross-covariance matrix upper right triangle (states: x, y, z, roll, pitch, yaw; first six entries are the first ROW, next five entries are the second ROW, etc.). If unknown, assign NaN value to first element in the array.*/ } mavlink_vicon_position_estimate_t; -#define MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE_LEN 32 +#define MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE_LEN 116 #define MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE_MIN_LEN 32 -#define MAVLINK_MSG_ID_104_LEN 32 +#define MAVLINK_MSG_ID_104_LEN 116 #define MAVLINK_MSG_ID_104_MIN_LEN 32 #define MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE_CRC 56 #define MAVLINK_MSG_ID_104_CRC 56 - +#define MAVLINK_MSG_VICON_POSITION_ESTIMATE_FIELD_COVARIANCE_LEN 21 #if MAVLINK_COMMAND_24BIT #define MAVLINK_MESSAGE_INFO_VICON_POSITION_ESTIMATE { \ 104, \ "VICON_POSITION_ESTIMATE", \ - 7, \ + 8, \ { { "usec", NULL, MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_vicon_position_estimate_t, usec) }, \ { "x", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_vicon_position_estimate_t, x) }, \ { "y", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_vicon_position_estimate_t, y) }, \ @@ -36,12 +37,13 @@ typedef struct __mavlink_vicon_position_estimate_t { { "roll", NULL, MAVLINK_TYPE_FLOAT, 0, 20, offsetof(mavlink_vicon_position_estimate_t, roll) }, \ { "pitch", NULL, MAVLINK_TYPE_FLOAT, 0, 24, offsetof(mavlink_vicon_position_estimate_t, pitch) }, \ { "yaw", NULL, MAVLINK_TYPE_FLOAT, 0, 28, offsetof(mavlink_vicon_position_estimate_t, yaw) }, \ + { "covariance", NULL, MAVLINK_TYPE_FLOAT, 21, 32, offsetof(mavlink_vicon_position_estimate_t, covariance) }, \ } \ } #else #define MAVLINK_MESSAGE_INFO_VICON_POSITION_ESTIMATE { \ "VICON_POSITION_ESTIMATE", \ - 7, \ + 8, \ { { "usec", NULL, MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_vicon_position_estimate_t, usec) }, \ { "x", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_vicon_position_estimate_t, x) }, \ { "y", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_vicon_position_estimate_t, y) }, \ @@ -49,6 +51,7 @@ typedef struct __mavlink_vicon_position_estimate_t { { "roll", NULL, MAVLINK_TYPE_FLOAT, 0, 20, offsetof(mavlink_vicon_position_estimate_t, roll) }, \ { "pitch", NULL, MAVLINK_TYPE_FLOAT, 0, 24, offsetof(mavlink_vicon_position_estimate_t, pitch) }, \ { "yaw", NULL, MAVLINK_TYPE_FLOAT, 0, 28, offsetof(mavlink_vicon_position_estimate_t, yaw) }, \ + { "covariance", NULL, MAVLINK_TYPE_FLOAT, 21, 32, offsetof(mavlink_vicon_position_estimate_t, covariance) }, \ } \ } #endif @@ -66,10 +69,11 @@ typedef struct __mavlink_vicon_position_estimate_t { * @param roll [rad] Roll angle * @param pitch [rad] Pitch angle * @param yaw [rad] Yaw angle + * @param covariance Row-major representation of 6x6 pose cross-covariance matrix upper right triangle (states: x, y, z, roll, pitch, yaw; first six entries are the first ROW, next five entries are the second ROW, etc.). If unknown, assign NaN value to first element in the array. * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_vicon_position_estimate_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint64_t usec, float x, float y, float z, float roll, float pitch, float yaw) + uint64_t usec, float x, float y, float z, float roll, float pitch, float yaw, const float *covariance) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE_LEN]; @@ -80,7 +84,7 @@ static inline uint16_t mavlink_msg_vicon_position_estimate_pack(uint8_t system_i _mav_put_float(buf, 20, roll); _mav_put_float(buf, 24, pitch); _mav_put_float(buf, 28, yaw); - + _mav_put_float_array(buf, 32, covariance, 21); memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE_LEN); #else mavlink_vicon_position_estimate_t packet; @@ -91,7 +95,7 @@ static inline uint16_t mavlink_msg_vicon_position_estimate_pack(uint8_t system_i packet.roll = roll; packet.pitch = pitch; packet.yaw = yaw; - + mav_array_memcpy(packet.covariance, covariance, sizeof(float)*21); memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE_LEN); #endif @@ -112,11 +116,12 @@ static inline uint16_t mavlink_msg_vicon_position_estimate_pack(uint8_t system_i * @param roll [rad] Roll angle * @param pitch [rad] Pitch angle * @param yaw [rad] Yaw angle + * @param covariance Row-major representation of 6x6 pose cross-covariance matrix upper right triangle (states: x, y, z, roll, pitch, yaw; first six entries are the first ROW, next five entries are the second ROW, etc.). If unknown, assign NaN value to first element in the array. * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_vicon_position_estimate_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, - uint64_t usec,float x,float y,float z,float roll,float pitch,float yaw) + uint64_t usec,float x,float y,float z,float roll,float pitch,float yaw,const float *covariance) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE_LEN]; @@ -127,7 +132,7 @@ static inline uint16_t mavlink_msg_vicon_position_estimate_pack_chan(uint8_t sys _mav_put_float(buf, 20, roll); _mav_put_float(buf, 24, pitch); _mav_put_float(buf, 28, yaw); - + _mav_put_float_array(buf, 32, covariance, 21); memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE_LEN); #else mavlink_vicon_position_estimate_t packet; @@ -138,7 +143,7 @@ static inline uint16_t mavlink_msg_vicon_position_estimate_pack_chan(uint8_t sys packet.roll = roll; packet.pitch = pitch; packet.yaw = yaw; - + mav_array_memcpy(packet.covariance, covariance, sizeof(float)*21); memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE_LEN); #endif @@ -156,7 +161,7 @@ static inline uint16_t mavlink_msg_vicon_position_estimate_pack_chan(uint8_t sys */ static inline uint16_t mavlink_msg_vicon_position_estimate_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_vicon_position_estimate_t* vicon_position_estimate) { - return mavlink_msg_vicon_position_estimate_pack(system_id, component_id, msg, vicon_position_estimate->usec, vicon_position_estimate->x, vicon_position_estimate->y, vicon_position_estimate->z, vicon_position_estimate->roll, vicon_position_estimate->pitch, vicon_position_estimate->yaw); + return mavlink_msg_vicon_position_estimate_pack(system_id, component_id, msg, vicon_position_estimate->usec, vicon_position_estimate->x, vicon_position_estimate->y, vicon_position_estimate->z, vicon_position_estimate->roll, vicon_position_estimate->pitch, vicon_position_estimate->yaw, vicon_position_estimate->covariance); } /** @@ -170,7 +175,7 @@ static inline uint16_t mavlink_msg_vicon_position_estimate_encode(uint8_t system */ static inline uint16_t mavlink_msg_vicon_position_estimate_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_vicon_position_estimate_t* vicon_position_estimate) { - return mavlink_msg_vicon_position_estimate_pack_chan(system_id, component_id, chan, msg, vicon_position_estimate->usec, vicon_position_estimate->x, vicon_position_estimate->y, vicon_position_estimate->z, vicon_position_estimate->roll, vicon_position_estimate->pitch, vicon_position_estimate->yaw); + return mavlink_msg_vicon_position_estimate_pack_chan(system_id, component_id, chan, msg, vicon_position_estimate->usec, vicon_position_estimate->x, vicon_position_estimate->y, vicon_position_estimate->z, vicon_position_estimate->roll, vicon_position_estimate->pitch, vicon_position_estimate->yaw, vicon_position_estimate->covariance); } /** @@ -184,10 +189,11 @@ static inline uint16_t mavlink_msg_vicon_position_estimate_encode_chan(uint8_t s * @param roll [rad] Roll angle * @param pitch [rad] Pitch angle * @param yaw [rad] Yaw angle + * @param covariance Row-major representation of 6x6 pose cross-covariance matrix upper right triangle (states: x, y, z, roll, pitch, yaw; first six entries are the first ROW, next five entries are the second ROW, etc.). If unknown, assign NaN value to first element in the array. */ #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS -static inline void mavlink_msg_vicon_position_estimate_send(mavlink_channel_t chan, uint64_t usec, float x, float y, float z, float roll, float pitch, float yaw) +static inline void mavlink_msg_vicon_position_estimate_send(mavlink_channel_t chan, uint64_t usec, float x, float y, float z, float roll, float pitch, float yaw, const float *covariance) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE_LEN]; @@ -198,7 +204,7 @@ static inline void mavlink_msg_vicon_position_estimate_send(mavlink_channel_t ch _mav_put_float(buf, 20, roll); _mav_put_float(buf, 24, pitch); _mav_put_float(buf, 28, yaw); - + _mav_put_float_array(buf, 32, covariance, 21); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE, buf, MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE_MIN_LEN, MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE_LEN, MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE_CRC); #else mavlink_vicon_position_estimate_t packet; @@ -209,7 +215,7 @@ static inline void mavlink_msg_vicon_position_estimate_send(mavlink_channel_t ch packet.roll = roll; packet.pitch = pitch; packet.yaw = yaw; - + mav_array_memcpy(packet.covariance, covariance, sizeof(float)*21); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE, (const char *)&packet, MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE_MIN_LEN, MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE_LEN, MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE_CRC); #endif } @@ -222,7 +228,7 @@ static inline void mavlink_msg_vicon_position_estimate_send(mavlink_channel_t ch static inline void mavlink_msg_vicon_position_estimate_send_struct(mavlink_channel_t chan, const mavlink_vicon_position_estimate_t* vicon_position_estimate) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - mavlink_msg_vicon_position_estimate_send(chan, vicon_position_estimate->usec, vicon_position_estimate->x, vicon_position_estimate->y, vicon_position_estimate->z, vicon_position_estimate->roll, vicon_position_estimate->pitch, vicon_position_estimate->yaw); + mavlink_msg_vicon_position_estimate_send(chan, vicon_position_estimate->usec, vicon_position_estimate->x, vicon_position_estimate->y, vicon_position_estimate->z, vicon_position_estimate->roll, vicon_position_estimate->pitch, vicon_position_estimate->yaw, vicon_position_estimate->covariance); #else _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE, (const char *)vicon_position_estimate, MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE_MIN_LEN, MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE_LEN, MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE_CRC); #endif @@ -236,7 +242,7 @@ static inline void mavlink_msg_vicon_position_estimate_send_struct(mavlink_chann is usually the receive buffer for the channel, and allows a reply to an incoming message with minimum stack space usage. */ -static inline void mavlink_msg_vicon_position_estimate_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint64_t usec, float x, float y, float z, float roll, float pitch, float yaw) +static inline void mavlink_msg_vicon_position_estimate_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint64_t usec, float x, float y, float z, float roll, float pitch, float yaw, const float *covariance) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char *buf = (char *)msgbuf; @@ -247,7 +253,7 @@ static inline void mavlink_msg_vicon_position_estimate_send_buf(mavlink_message_ _mav_put_float(buf, 20, roll); _mav_put_float(buf, 24, pitch); _mav_put_float(buf, 28, yaw); - + _mav_put_float_array(buf, 32, covariance, 21); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE, buf, MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE_MIN_LEN, MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE_LEN, MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE_CRC); #else mavlink_vicon_position_estimate_t *packet = (mavlink_vicon_position_estimate_t *)msgbuf; @@ -258,7 +264,7 @@ static inline void mavlink_msg_vicon_position_estimate_send_buf(mavlink_message_ packet->roll = roll; packet->pitch = pitch; packet->yaw = yaw; - + mav_array_memcpy(packet->covariance, covariance, sizeof(float)*21); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE, (const char *)packet, MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE_MIN_LEN, MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE_LEN, MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE_CRC); #endif } @@ -339,6 +345,16 @@ static inline float mavlink_msg_vicon_position_estimate_get_yaw(const mavlink_me return _MAV_RETURN_float(msg, 28); } +/** + * @brief Get field covariance from vicon_position_estimate message + * + * @return Row-major representation of 6x6 pose cross-covariance matrix upper right triangle (states: x, y, z, roll, pitch, yaw; first six entries are the first ROW, next five entries are the second ROW, etc.). If unknown, assign NaN value to first element in the array. + */ +static inline uint16_t mavlink_msg_vicon_position_estimate_get_covariance(const mavlink_message_t* msg, float *covariance) +{ + return _MAV_RETURN_float_array(msg, covariance, 21, 32); +} + /** * @brief Decode a vicon_position_estimate message into a struct * @@ -355,6 +371,7 @@ static inline void mavlink_msg_vicon_position_estimate_decode(const mavlink_mess vicon_position_estimate->roll = mavlink_msg_vicon_position_estimate_get_roll(msg); vicon_position_estimate->pitch = mavlink_msg_vicon_position_estimate_get_pitch(msg); vicon_position_estimate->yaw = mavlink_msg_vicon_position_estimate_get_yaw(msg); + mavlink_msg_vicon_position_estimate_get_covariance(msg, vicon_position_estimate->covariance); #else uint8_t len = msg->len < MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE_LEN? msg->len : MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE_LEN; memset(vicon_position_estimate, 0, MAVLINK_MSG_ID_VICON_POSITION_ESTIMATE_LEN); diff --git a/lib/main/MAVLink/common/mavlink_msg_video_stream_information.h b/lib/main/MAVLink/common/mavlink_msg_video_stream_information.h new file mode 100644 index 0000000000..aa6d84b743 --- /dev/null +++ b/lib/main/MAVLink/common/mavlink_msg_video_stream_information.h @@ -0,0 +1,481 @@ +#pragma once +// MESSAGE VIDEO_STREAM_INFORMATION PACKING + +#define MAVLINK_MSG_ID_VIDEO_STREAM_INFORMATION 269 + + +typedef struct __mavlink_video_stream_information_t { + float framerate; /*< [Hz] Frame rate.*/ + uint32_t bitrate; /*< [bits/s] Bit rate.*/ + uint16_t flags; /*< Bitmap of stream status flags.*/ + uint16_t resolution_h; /*< [pix] Horizontal resolution.*/ + uint16_t resolution_v; /*< [pix] Vertical resolution.*/ + uint16_t rotation; /*< [deg] Video image rotation clockwise.*/ + uint16_t hfov; /*< [deg] Horizontal Field of view.*/ + uint8_t stream_id; /*< Video Stream ID (1 for first, 2 for second, etc.)*/ + uint8_t count; /*< Number of streams available.*/ + uint8_t type; /*< Type of stream.*/ + char name[32]; /*< Stream name.*/ + char uri[160]; /*< Video stream URI (TCP or RTSP URI ground station should connect to) or port number (UDP port ground station should listen to).*/ +} mavlink_video_stream_information_t; + +#define MAVLINK_MSG_ID_VIDEO_STREAM_INFORMATION_LEN 213 +#define MAVLINK_MSG_ID_VIDEO_STREAM_INFORMATION_MIN_LEN 213 +#define MAVLINK_MSG_ID_269_LEN 213 +#define MAVLINK_MSG_ID_269_MIN_LEN 213 + +#define MAVLINK_MSG_ID_VIDEO_STREAM_INFORMATION_CRC 109 +#define MAVLINK_MSG_ID_269_CRC 109 + +#define MAVLINK_MSG_VIDEO_STREAM_INFORMATION_FIELD_NAME_LEN 32 +#define MAVLINK_MSG_VIDEO_STREAM_INFORMATION_FIELD_URI_LEN 160 + +#if MAVLINK_COMMAND_24BIT +#define MAVLINK_MESSAGE_INFO_VIDEO_STREAM_INFORMATION { \ + 269, \ + "VIDEO_STREAM_INFORMATION", \ + 12, \ + { { "stream_id", NULL, MAVLINK_TYPE_UINT8_T, 0, 18, offsetof(mavlink_video_stream_information_t, stream_id) }, \ + { "count", NULL, MAVLINK_TYPE_UINT8_T, 0, 19, offsetof(mavlink_video_stream_information_t, count) }, \ + { "type", NULL, MAVLINK_TYPE_UINT8_T, 0, 20, offsetof(mavlink_video_stream_information_t, type) }, \ + { "flags", NULL, MAVLINK_TYPE_UINT16_T, 0, 8, offsetof(mavlink_video_stream_information_t, flags) }, \ + { "framerate", NULL, MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_video_stream_information_t, framerate) }, \ + { "resolution_h", NULL, MAVLINK_TYPE_UINT16_T, 0, 10, offsetof(mavlink_video_stream_information_t, resolution_h) }, \ + { "resolution_v", NULL, MAVLINK_TYPE_UINT16_T, 0, 12, offsetof(mavlink_video_stream_information_t, resolution_v) }, \ + { "bitrate", NULL, MAVLINK_TYPE_UINT32_T, 0, 4, offsetof(mavlink_video_stream_information_t, bitrate) }, \ + { "rotation", NULL, MAVLINK_TYPE_UINT16_T, 0, 14, offsetof(mavlink_video_stream_information_t, rotation) }, \ + { "hfov", NULL, MAVLINK_TYPE_UINT16_T, 0, 16, offsetof(mavlink_video_stream_information_t, hfov) }, \ + { "name", NULL, MAVLINK_TYPE_CHAR, 32, 21, offsetof(mavlink_video_stream_information_t, name) }, \ + { "uri", NULL, MAVLINK_TYPE_CHAR, 160, 53, offsetof(mavlink_video_stream_information_t, uri) }, \ + } \ +} +#else +#define MAVLINK_MESSAGE_INFO_VIDEO_STREAM_INFORMATION { \ + "VIDEO_STREAM_INFORMATION", \ + 12, \ + { { "stream_id", NULL, MAVLINK_TYPE_UINT8_T, 0, 18, offsetof(mavlink_video_stream_information_t, stream_id) }, \ + { "count", NULL, MAVLINK_TYPE_UINT8_T, 0, 19, offsetof(mavlink_video_stream_information_t, count) }, \ + { "type", NULL, MAVLINK_TYPE_UINT8_T, 0, 20, offsetof(mavlink_video_stream_information_t, type) }, \ + { "flags", NULL, MAVLINK_TYPE_UINT16_T, 0, 8, offsetof(mavlink_video_stream_information_t, flags) }, \ + { "framerate", NULL, MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_video_stream_information_t, framerate) }, \ + { "resolution_h", NULL, MAVLINK_TYPE_UINT16_T, 0, 10, offsetof(mavlink_video_stream_information_t, resolution_h) }, \ + { "resolution_v", NULL, MAVLINK_TYPE_UINT16_T, 0, 12, offsetof(mavlink_video_stream_information_t, resolution_v) }, \ + { "bitrate", NULL, MAVLINK_TYPE_UINT32_T, 0, 4, offsetof(mavlink_video_stream_information_t, bitrate) }, \ + { "rotation", NULL, MAVLINK_TYPE_UINT16_T, 0, 14, offsetof(mavlink_video_stream_information_t, rotation) }, \ + { "hfov", NULL, MAVLINK_TYPE_UINT16_T, 0, 16, offsetof(mavlink_video_stream_information_t, hfov) }, \ + { "name", NULL, MAVLINK_TYPE_CHAR, 32, 21, offsetof(mavlink_video_stream_information_t, name) }, \ + { "uri", NULL, MAVLINK_TYPE_CHAR, 160, 53, offsetof(mavlink_video_stream_information_t, uri) }, \ + } \ +} +#endif + +/** + * @brief Pack a video_stream_information message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param stream_id Video Stream ID (1 for first, 2 for second, etc.) + * @param count Number of streams available. + * @param type Type of stream. + * @param flags Bitmap of stream status flags. + * @param framerate [Hz] Frame rate. + * @param resolution_h [pix] Horizontal resolution. + * @param resolution_v [pix] Vertical resolution. + * @param bitrate [bits/s] Bit rate. + * @param rotation [deg] Video image rotation clockwise. + * @param hfov [deg] Horizontal Field of view. + * @param name Stream name. + * @param uri Video stream URI (TCP or RTSP URI ground station should connect to) or port number (UDP port ground station should listen to). + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_video_stream_information_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint8_t stream_id, uint8_t count, uint8_t type, uint16_t flags, float framerate, uint16_t resolution_h, uint16_t resolution_v, uint32_t bitrate, uint16_t rotation, uint16_t hfov, const char *name, const char *uri) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_VIDEO_STREAM_INFORMATION_LEN]; + _mav_put_float(buf, 0, framerate); + _mav_put_uint32_t(buf, 4, bitrate); + _mav_put_uint16_t(buf, 8, flags); + _mav_put_uint16_t(buf, 10, resolution_h); + _mav_put_uint16_t(buf, 12, resolution_v); + _mav_put_uint16_t(buf, 14, rotation); + _mav_put_uint16_t(buf, 16, hfov); + _mav_put_uint8_t(buf, 18, stream_id); + _mav_put_uint8_t(buf, 19, count); + _mav_put_uint8_t(buf, 20, type); + _mav_put_char_array(buf, 21, name, 32); + _mav_put_char_array(buf, 53, uri, 160); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_VIDEO_STREAM_INFORMATION_LEN); +#else + mavlink_video_stream_information_t packet; + packet.framerate = framerate; + packet.bitrate = bitrate; + packet.flags = flags; + packet.resolution_h = resolution_h; + packet.resolution_v = resolution_v; + packet.rotation = rotation; + packet.hfov = hfov; + packet.stream_id = stream_id; + packet.count = count; + packet.type = type; + mav_array_memcpy(packet.name, name, sizeof(char)*32); + mav_array_memcpy(packet.uri, uri, sizeof(char)*160); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_VIDEO_STREAM_INFORMATION_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_VIDEO_STREAM_INFORMATION; + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_VIDEO_STREAM_INFORMATION_MIN_LEN, MAVLINK_MSG_ID_VIDEO_STREAM_INFORMATION_LEN, MAVLINK_MSG_ID_VIDEO_STREAM_INFORMATION_CRC); +} + +/** + * @brief Pack a video_stream_information message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param stream_id Video Stream ID (1 for first, 2 for second, etc.) + * @param count Number of streams available. + * @param type Type of stream. + * @param flags Bitmap of stream status flags. + * @param framerate [Hz] Frame rate. + * @param resolution_h [pix] Horizontal resolution. + * @param resolution_v [pix] Vertical resolution. + * @param bitrate [bits/s] Bit rate. + * @param rotation [deg] Video image rotation clockwise. + * @param hfov [deg] Horizontal Field of view. + * @param name Stream name. + * @param uri Video stream URI (TCP or RTSP URI ground station should connect to) or port number (UDP port ground station should listen to). + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_video_stream_information_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint8_t stream_id,uint8_t count,uint8_t type,uint16_t flags,float framerate,uint16_t resolution_h,uint16_t resolution_v,uint32_t bitrate,uint16_t rotation,uint16_t hfov,const char *name,const char *uri) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_VIDEO_STREAM_INFORMATION_LEN]; + _mav_put_float(buf, 0, framerate); + _mav_put_uint32_t(buf, 4, bitrate); + _mav_put_uint16_t(buf, 8, flags); + _mav_put_uint16_t(buf, 10, resolution_h); + _mav_put_uint16_t(buf, 12, resolution_v); + _mav_put_uint16_t(buf, 14, rotation); + _mav_put_uint16_t(buf, 16, hfov); + _mav_put_uint8_t(buf, 18, stream_id); + _mav_put_uint8_t(buf, 19, count); + _mav_put_uint8_t(buf, 20, type); + _mav_put_char_array(buf, 21, name, 32); + _mav_put_char_array(buf, 53, uri, 160); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_VIDEO_STREAM_INFORMATION_LEN); +#else + mavlink_video_stream_information_t packet; + packet.framerate = framerate; + packet.bitrate = bitrate; + packet.flags = flags; + packet.resolution_h = resolution_h; + packet.resolution_v = resolution_v; + packet.rotation = rotation; + packet.hfov = hfov; + packet.stream_id = stream_id; + packet.count = count; + packet.type = type; + mav_array_memcpy(packet.name, name, sizeof(char)*32); + mav_array_memcpy(packet.uri, uri, sizeof(char)*160); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_VIDEO_STREAM_INFORMATION_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_VIDEO_STREAM_INFORMATION; + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_VIDEO_STREAM_INFORMATION_MIN_LEN, MAVLINK_MSG_ID_VIDEO_STREAM_INFORMATION_LEN, MAVLINK_MSG_ID_VIDEO_STREAM_INFORMATION_CRC); +} + +/** + * @brief Encode a video_stream_information struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param video_stream_information C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_video_stream_information_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_video_stream_information_t* video_stream_information) +{ + return mavlink_msg_video_stream_information_pack(system_id, component_id, msg, video_stream_information->stream_id, video_stream_information->count, video_stream_information->type, video_stream_information->flags, video_stream_information->framerate, video_stream_information->resolution_h, video_stream_information->resolution_v, video_stream_information->bitrate, video_stream_information->rotation, video_stream_information->hfov, video_stream_information->name, video_stream_information->uri); +} + +/** + * @brief Encode a video_stream_information struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param video_stream_information C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_video_stream_information_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_video_stream_information_t* video_stream_information) +{ + return mavlink_msg_video_stream_information_pack_chan(system_id, component_id, chan, msg, video_stream_information->stream_id, video_stream_information->count, video_stream_information->type, video_stream_information->flags, video_stream_information->framerate, video_stream_information->resolution_h, video_stream_information->resolution_v, video_stream_information->bitrate, video_stream_information->rotation, video_stream_information->hfov, video_stream_information->name, video_stream_information->uri); +} + +/** + * @brief Send a video_stream_information message + * @param chan MAVLink channel to send the message + * + * @param stream_id Video Stream ID (1 for first, 2 for second, etc.) + * @param count Number of streams available. + * @param type Type of stream. + * @param flags Bitmap of stream status flags. + * @param framerate [Hz] Frame rate. + * @param resolution_h [pix] Horizontal resolution. + * @param resolution_v [pix] Vertical resolution. + * @param bitrate [bits/s] Bit rate. + * @param rotation [deg] Video image rotation clockwise. + * @param hfov [deg] Horizontal Field of view. + * @param name Stream name. + * @param uri Video stream URI (TCP or RTSP URI ground station should connect to) or port number (UDP port ground station should listen to). + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_video_stream_information_send(mavlink_channel_t chan, uint8_t stream_id, uint8_t count, uint8_t type, uint16_t flags, float framerate, uint16_t resolution_h, uint16_t resolution_v, uint32_t bitrate, uint16_t rotation, uint16_t hfov, const char *name, const char *uri) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_VIDEO_STREAM_INFORMATION_LEN]; + _mav_put_float(buf, 0, framerate); + _mav_put_uint32_t(buf, 4, bitrate); + _mav_put_uint16_t(buf, 8, flags); + _mav_put_uint16_t(buf, 10, resolution_h); + _mav_put_uint16_t(buf, 12, resolution_v); + _mav_put_uint16_t(buf, 14, rotation); + _mav_put_uint16_t(buf, 16, hfov); + _mav_put_uint8_t(buf, 18, stream_id); + _mav_put_uint8_t(buf, 19, count); + _mav_put_uint8_t(buf, 20, type); + _mav_put_char_array(buf, 21, name, 32); + _mav_put_char_array(buf, 53, uri, 160); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_VIDEO_STREAM_INFORMATION, buf, MAVLINK_MSG_ID_VIDEO_STREAM_INFORMATION_MIN_LEN, MAVLINK_MSG_ID_VIDEO_STREAM_INFORMATION_LEN, MAVLINK_MSG_ID_VIDEO_STREAM_INFORMATION_CRC); +#else + mavlink_video_stream_information_t packet; + packet.framerate = framerate; + packet.bitrate = bitrate; + packet.flags = flags; + packet.resolution_h = resolution_h; + packet.resolution_v = resolution_v; + packet.rotation = rotation; + packet.hfov = hfov; + packet.stream_id = stream_id; + packet.count = count; + packet.type = type; + mav_array_memcpy(packet.name, name, sizeof(char)*32); + mav_array_memcpy(packet.uri, uri, sizeof(char)*160); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_VIDEO_STREAM_INFORMATION, (const char *)&packet, MAVLINK_MSG_ID_VIDEO_STREAM_INFORMATION_MIN_LEN, MAVLINK_MSG_ID_VIDEO_STREAM_INFORMATION_LEN, MAVLINK_MSG_ID_VIDEO_STREAM_INFORMATION_CRC); +#endif +} + +/** + * @brief Send a video_stream_information message + * @param chan MAVLink channel to send the message + * @param struct The MAVLink struct to serialize + */ +static inline void mavlink_msg_video_stream_information_send_struct(mavlink_channel_t chan, const mavlink_video_stream_information_t* video_stream_information) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + mavlink_msg_video_stream_information_send(chan, video_stream_information->stream_id, video_stream_information->count, video_stream_information->type, video_stream_information->flags, video_stream_information->framerate, video_stream_information->resolution_h, video_stream_information->resolution_v, video_stream_information->bitrate, video_stream_information->rotation, video_stream_information->hfov, video_stream_information->name, video_stream_information->uri); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_VIDEO_STREAM_INFORMATION, (const char *)video_stream_information, MAVLINK_MSG_ID_VIDEO_STREAM_INFORMATION_MIN_LEN, MAVLINK_MSG_ID_VIDEO_STREAM_INFORMATION_LEN, MAVLINK_MSG_ID_VIDEO_STREAM_INFORMATION_CRC); +#endif +} + +#if MAVLINK_MSG_ID_VIDEO_STREAM_INFORMATION_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_video_stream_information_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t stream_id, uint8_t count, uint8_t type, uint16_t flags, float framerate, uint16_t resolution_h, uint16_t resolution_v, uint32_t bitrate, uint16_t rotation, uint16_t hfov, const char *name, const char *uri) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_float(buf, 0, framerate); + _mav_put_uint32_t(buf, 4, bitrate); + _mav_put_uint16_t(buf, 8, flags); + _mav_put_uint16_t(buf, 10, resolution_h); + _mav_put_uint16_t(buf, 12, resolution_v); + _mav_put_uint16_t(buf, 14, rotation); + _mav_put_uint16_t(buf, 16, hfov); + _mav_put_uint8_t(buf, 18, stream_id); + _mav_put_uint8_t(buf, 19, count); + _mav_put_uint8_t(buf, 20, type); + _mav_put_char_array(buf, 21, name, 32); + _mav_put_char_array(buf, 53, uri, 160); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_VIDEO_STREAM_INFORMATION, buf, MAVLINK_MSG_ID_VIDEO_STREAM_INFORMATION_MIN_LEN, MAVLINK_MSG_ID_VIDEO_STREAM_INFORMATION_LEN, MAVLINK_MSG_ID_VIDEO_STREAM_INFORMATION_CRC); +#else + mavlink_video_stream_information_t *packet = (mavlink_video_stream_information_t *)msgbuf; + packet->framerate = framerate; + packet->bitrate = bitrate; + packet->flags = flags; + packet->resolution_h = resolution_h; + packet->resolution_v = resolution_v; + packet->rotation = rotation; + packet->hfov = hfov; + packet->stream_id = stream_id; + packet->count = count; + packet->type = type; + mav_array_memcpy(packet->name, name, sizeof(char)*32); + mav_array_memcpy(packet->uri, uri, sizeof(char)*160); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_VIDEO_STREAM_INFORMATION, (const char *)packet, MAVLINK_MSG_ID_VIDEO_STREAM_INFORMATION_MIN_LEN, MAVLINK_MSG_ID_VIDEO_STREAM_INFORMATION_LEN, MAVLINK_MSG_ID_VIDEO_STREAM_INFORMATION_CRC); +#endif +} +#endif + +#endif + +// MESSAGE VIDEO_STREAM_INFORMATION UNPACKING + + +/** + * @brief Get field stream_id from video_stream_information message + * + * @return Video Stream ID (1 for first, 2 for second, etc.) + */ +static inline uint8_t mavlink_msg_video_stream_information_get_stream_id(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 18); +} + +/** + * @brief Get field count from video_stream_information message + * + * @return Number of streams available. + */ +static inline uint8_t mavlink_msg_video_stream_information_get_count(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 19); +} + +/** + * @brief Get field type from video_stream_information message + * + * @return Type of stream. + */ +static inline uint8_t mavlink_msg_video_stream_information_get_type(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 20); +} + +/** + * @brief Get field flags from video_stream_information message + * + * @return Bitmap of stream status flags. + */ +static inline uint16_t mavlink_msg_video_stream_information_get_flags(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 8); +} + +/** + * @brief Get field framerate from video_stream_information message + * + * @return [Hz] Frame rate. + */ +static inline float mavlink_msg_video_stream_information_get_framerate(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 0); +} + +/** + * @brief Get field resolution_h from video_stream_information message + * + * @return [pix] Horizontal resolution. + */ +static inline uint16_t mavlink_msg_video_stream_information_get_resolution_h(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 10); +} + +/** + * @brief Get field resolution_v from video_stream_information message + * + * @return [pix] Vertical resolution. + */ +static inline uint16_t mavlink_msg_video_stream_information_get_resolution_v(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 12); +} + +/** + * @brief Get field bitrate from video_stream_information message + * + * @return [bits/s] Bit rate. + */ +static inline uint32_t mavlink_msg_video_stream_information_get_bitrate(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint32_t(msg, 4); +} + +/** + * @brief Get field rotation from video_stream_information message + * + * @return [deg] Video image rotation clockwise. + */ +static inline uint16_t mavlink_msg_video_stream_information_get_rotation(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 14); +} + +/** + * @brief Get field hfov from video_stream_information message + * + * @return [deg] Horizontal Field of view. + */ +static inline uint16_t mavlink_msg_video_stream_information_get_hfov(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 16); +} + +/** + * @brief Get field name from video_stream_information message + * + * @return Stream name. + */ +static inline uint16_t mavlink_msg_video_stream_information_get_name(const mavlink_message_t* msg, char *name) +{ + return _MAV_RETURN_char_array(msg, name, 32, 21); +} + +/** + * @brief Get field uri from video_stream_information message + * + * @return Video stream URI (TCP or RTSP URI ground station should connect to) or port number (UDP port ground station should listen to). + */ +static inline uint16_t mavlink_msg_video_stream_information_get_uri(const mavlink_message_t* msg, char *uri) +{ + return _MAV_RETURN_char_array(msg, uri, 160, 53); +} + +/** + * @brief Decode a video_stream_information message into a struct + * + * @param msg The message to decode + * @param video_stream_information C-struct to decode the message contents into + */ +static inline void mavlink_msg_video_stream_information_decode(const mavlink_message_t* msg, mavlink_video_stream_information_t* video_stream_information) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + video_stream_information->framerate = mavlink_msg_video_stream_information_get_framerate(msg); + video_stream_information->bitrate = mavlink_msg_video_stream_information_get_bitrate(msg); + video_stream_information->flags = mavlink_msg_video_stream_information_get_flags(msg); + video_stream_information->resolution_h = mavlink_msg_video_stream_information_get_resolution_h(msg); + video_stream_information->resolution_v = mavlink_msg_video_stream_information_get_resolution_v(msg); + video_stream_information->rotation = mavlink_msg_video_stream_information_get_rotation(msg); + video_stream_information->hfov = mavlink_msg_video_stream_information_get_hfov(msg); + video_stream_information->stream_id = mavlink_msg_video_stream_information_get_stream_id(msg); + video_stream_information->count = mavlink_msg_video_stream_information_get_count(msg); + video_stream_information->type = mavlink_msg_video_stream_information_get_type(msg); + mavlink_msg_video_stream_information_get_name(msg, video_stream_information->name); + mavlink_msg_video_stream_information_get_uri(msg, video_stream_information->uri); +#else + uint8_t len = msg->len < MAVLINK_MSG_ID_VIDEO_STREAM_INFORMATION_LEN? msg->len : MAVLINK_MSG_ID_VIDEO_STREAM_INFORMATION_LEN; + memset(video_stream_information, 0, MAVLINK_MSG_ID_VIDEO_STREAM_INFORMATION_LEN); + memcpy(video_stream_information, _MAV_PAYLOAD(msg), len); +#endif +} diff --git a/lib/main/MAVLink/common/mavlink_msg_video_stream_status.h b/lib/main/MAVLink/common/mavlink_msg_video_stream_status.h new file mode 100644 index 0000000000..380941005e --- /dev/null +++ b/lib/main/MAVLink/common/mavlink_msg_video_stream_status.h @@ -0,0 +1,388 @@ +#pragma once +// MESSAGE VIDEO_STREAM_STATUS PACKING + +#define MAVLINK_MSG_ID_VIDEO_STREAM_STATUS 270 + + +typedef struct __mavlink_video_stream_status_t { + float framerate; /*< [Hz] Frame rate*/ + uint32_t bitrate; /*< [bits/s] Bit rate*/ + uint16_t flags; /*< Bitmap of stream status flags*/ + uint16_t resolution_h; /*< [pix] Horizontal resolution*/ + uint16_t resolution_v; /*< [pix] Vertical resolution*/ + uint16_t rotation; /*< [deg] Video image rotation clockwise*/ + uint16_t hfov; /*< [deg] Horizontal Field of view*/ + uint8_t stream_id; /*< Video Stream ID (1 for first, 2 for second, etc.)*/ +} mavlink_video_stream_status_t; + +#define MAVLINK_MSG_ID_VIDEO_STREAM_STATUS_LEN 19 +#define MAVLINK_MSG_ID_VIDEO_STREAM_STATUS_MIN_LEN 19 +#define MAVLINK_MSG_ID_270_LEN 19 +#define MAVLINK_MSG_ID_270_MIN_LEN 19 + +#define MAVLINK_MSG_ID_VIDEO_STREAM_STATUS_CRC 59 +#define MAVLINK_MSG_ID_270_CRC 59 + + + +#if MAVLINK_COMMAND_24BIT +#define MAVLINK_MESSAGE_INFO_VIDEO_STREAM_STATUS { \ + 270, \ + "VIDEO_STREAM_STATUS", \ + 8, \ + { { "stream_id", NULL, MAVLINK_TYPE_UINT8_T, 0, 18, offsetof(mavlink_video_stream_status_t, stream_id) }, \ + { "flags", NULL, MAVLINK_TYPE_UINT16_T, 0, 8, offsetof(mavlink_video_stream_status_t, flags) }, \ + { "framerate", NULL, MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_video_stream_status_t, framerate) }, \ + { "resolution_h", NULL, MAVLINK_TYPE_UINT16_T, 0, 10, offsetof(mavlink_video_stream_status_t, resolution_h) }, \ + { "resolution_v", NULL, MAVLINK_TYPE_UINT16_T, 0, 12, offsetof(mavlink_video_stream_status_t, resolution_v) }, \ + { "bitrate", NULL, MAVLINK_TYPE_UINT32_T, 0, 4, offsetof(mavlink_video_stream_status_t, bitrate) }, \ + { "rotation", NULL, MAVLINK_TYPE_UINT16_T, 0, 14, offsetof(mavlink_video_stream_status_t, rotation) }, \ + { "hfov", NULL, MAVLINK_TYPE_UINT16_T, 0, 16, offsetof(mavlink_video_stream_status_t, hfov) }, \ + } \ +} +#else +#define MAVLINK_MESSAGE_INFO_VIDEO_STREAM_STATUS { \ + "VIDEO_STREAM_STATUS", \ + 8, \ + { { "stream_id", NULL, MAVLINK_TYPE_UINT8_T, 0, 18, offsetof(mavlink_video_stream_status_t, stream_id) }, \ + { "flags", NULL, MAVLINK_TYPE_UINT16_T, 0, 8, offsetof(mavlink_video_stream_status_t, flags) }, \ + { "framerate", NULL, MAVLINK_TYPE_FLOAT, 0, 0, offsetof(mavlink_video_stream_status_t, framerate) }, \ + { "resolution_h", NULL, MAVLINK_TYPE_UINT16_T, 0, 10, offsetof(mavlink_video_stream_status_t, resolution_h) }, \ + { "resolution_v", NULL, MAVLINK_TYPE_UINT16_T, 0, 12, offsetof(mavlink_video_stream_status_t, resolution_v) }, \ + { "bitrate", NULL, MAVLINK_TYPE_UINT32_T, 0, 4, offsetof(mavlink_video_stream_status_t, bitrate) }, \ + { "rotation", NULL, MAVLINK_TYPE_UINT16_T, 0, 14, offsetof(mavlink_video_stream_status_t, rotation) }, \ + { "hfov", NULL, MAVLINK_TYPE_UINT16_T, 0, 16, offsetof(mavlink_video_stream_status_t, hfov) }, \ + } \ +} +#endif + +/** + * @brief Pack a video_stream_status message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param stream_id Video Stream ID (1 for first, 2 for second, etc.) + * @param flags Bitmap of stream status flags + * @param framerate [Hz] Frame rate + * @param resolution_h [pix] Horizontal resolution + * @param resolution_v [pix] Vertical resolution + * @param bitrate [bits/s] Bit rate + * @param rotation [deg] Video image rotation clockwise + * @param hfov [deg] Horizontal Field of view + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_video_stream_status_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint8_t stream_id, uint16_t flags, float framerate, uint16_t resolution_h, uint16_t resolution_v, uint32_t bitrate, uint16_t rotation, uint16_t hfov) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_VIDEO_STREAM_STATUS_LEN]; + _mav_put_float(buf, 0, framerate); + _mav_put_uint32_t(buf, 4, bitrate); + _mav_put_uint16_t(buf, 8, flags); + _mav_put_uint16_t(buf, 10, resolution_h); + _mav_put_uint16_t(buf, 12, resolution_v); + _mav_put_uint16_t(buf, 14, rotation); + _mav_put_uint16_t(buf, 16, hfov); + _mav_put_uint8_t(buf, 18, stream_id); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_VIDEO_STREAM_STATUS_LEN); +#else + mavlink_video_stream_status_t packet; + packet.framerate = framerate; + packet.bitrate = bitrate; + packet.flags = flags; + packet.resolution_h = resolution_h; + packet.resolution_v = resolution_v; + packet.rotation = rotation; + packet.hfov = hfov; + packet.stream_id = stream_id; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_VIDEO_STREAM_STATUS_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_VIDEO_STREAM_STATUS; + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_VIDEO_STREAM_STATUS_MIN_LEN, MAVLINK_MSG_ID_VIDEO_STREAM_STATUS_LEN, MAVLINK_MSG_ID_VIDEO_STREAM_STATUS_CRC); +} + +/** + * @brief Pack a video_stream_status message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param stream_id Video Stream ID (1 for first, 2 for second, etc.) + * @param flags Bitmap of stream status flags + * @param framerate [Hz] Frame rate + * @param resolution_h [pix] Horizontal resolution + * @param resolution_v [pix] Vertical resolution + * @param bitrate [bits/s] Bit rate + * @param rotation [deg] Video image rotation clockwise + * @param hfov [deg] Horizontal Field of view + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_video_stream_status_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint8_t stream_id,uint16_t flags,float framerate,uint16_t resolution_h,uint16_t resolution_v,uint32_t bitrate,uint16_t rotation,uint16_t hfov) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_VIDEO_STREAM_STATUS_LEN]; + _mav_put_float(buf, 0, framerate); + _mav_put_uint32_t(buf, 4, bitrate); + _mav_put_uint16_t(buf, 8, flags); + _mav_put_uint16_t(buf, 10, resolution_h); + _mav_put_uint16_t(buf, 12, resolution_v); + _mav_put_uint16_t(buf, 14, rotation); + _mav_put_uint16_t(buf, 16, hfov); + _mav_put_uint8_t(buf, 18, stream_id); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_VIDEO_STREAM_STATUS_LEN); +#else + mavlink_video_stream_status_t packet; + packet.framerate = framerate; + packet.bitrate = bitrate; + packet.flags = flags; + packet.resolution_h = resolution_h; + packet.resolution_v = resolution_v; + packet.rotation = rotation; + packet.hfov = hfov; + packet.stream_id = stream_id; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_VIDEO_STREAM_STATUS_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_VIDEO_STREAM_STATUS; + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_VIDEO_STREAM_STATUS_MIN_LEN, MAVLINK_MSG_ID_VIDEO_STREAM_STATUS_LEN, MAVLINK_MSG_ID_VIDEO_STREAM_STATUS_CRC); +} + +/** + * @brief Encode a video_stream_status struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param video_stream_status C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_video_stream_status_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_video_stream_status_t* video_stream_status) +{ + return mavlink_msg_video_stream_status_pack(system_id, component_id, msg, video_stream_status->stream_id, video_stream_status->flags, video_stream_status->framerate, video_stream_status->resolution_h, video_stream_status->resolution_v, video_stream_status->bitrate, video_stream_status->rotation, video_stream_status->hfov); +} + +/** + * @brief Encode a video_stream_status struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param video_stream_status C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_video_stream_status_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_video_stream_status_t* video_stream_status) +{ + return mavlink_msg_video_stream_status_pack_chan(system_id, component_id, chan, msg, video_stream_status->stream_id, video_stream_status->flags, video_stream_status->framerate, video_stream_status->resolution_h, video_stream_status->resolution_v, video_stream_status->bitrate, video_stream_status->rotation, video_stream_status->hfov); +} + +/** + * @brief Send a video_stream_status message + * @param chan MAVLink channel to send the message + * + * @param stream_id Video Stream ID (1 for first, 2 for second, etc.) + * @param flags Bitmap of stream status flags + * @param framerate [Hz] Frame rate + * @param resolution_h [pix] Horizontal resolution + * @param resolution_v [pix] Vertical resolution + * @param bitrate [bits/s] Bit rate + * @param rotation [deg] Video image rotation clockwise + * @param hfov [deg] Horizontal Field of view + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_video_stream_status_send(mavlink_channel_t chan, uint8_t stream_id, uint16_t flags, float framerate, uint16_t resolution_h, uint16_t resolution_v, uint32_t bitrate, uint16_t rotation, uint16_t hfov) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_VIDEO_STREAM_STATUS_LEN]; + _mav_put_float(buf, 0, framerate); + _mav_put_uint32_t(buf, 4, bitrate); + _mav_put_uint16_t(buf, 8, flags); + _mav_put_uint16_t(buf, 10, resolution_h); + _mav_put_uint16_t(buf, 12, resolution_v); + _mav_put_uint16_t(buf, 14, rotation); + _mav_put_uint16_t(buf, 16, hfov); + _mav_put_uint8_t(buf, 18, stream_id); + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_VIDEO_STREAM_STATUS, buf, MAVLINK_MSG_ID_VIDEO_STREAM_STATUS_MIN_LEN, MAVLINK_MSG_ID_VIDEO_STREAM_STATUS_LEN, MAVLINK_MSG_ID_VIDEO_STREAM_STATUS_CRC); +#else + mavlink_video_stream_status_t packet; + packet.framerate = framerate; + packet.bitrate = bitrate; + packet.flags = flags; + packet.resolution_h = resolution_h; + packet.resolution_v = resolution_v; + packet.rotation = rotation; + packet.hfov = hfov; + packet.stream_id = stream_id; + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_VIDEO_STREAM_STATUS, (const char *)&packet, MAVLINK_MSG_ID_VIDEO_STREAM_STATUS_MIN_LEN, MAVLINK_MSG_ID_VIDEO_STREAM_STATUS_LEN, MAVLINK_MSG_ID_VIDEO_STREAM_STATUS_CRC); +#endif +} + +/** + * @brief Send a video_stream_status message + * @param chan MAVLink channel to send the message + * @param struct The MAVLink struct to serialize + */ +static inline void mavlink_msg_video_stream_status_send_struct(mavlink_channel_t chan, const mavlink_video_stream_status_t* video_stream_status) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + mavlink_msg_video_stream_status_send(chan, video_stream_status->stream_id, video_stream_status->flags, video_stream_status->framerate, video_stream_status->resolution_h, video_stream_status->resolution_v, video_stream_status->bitrate, video_stream_status->rotation, video_stream_status->hfov); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_VIDEO_STREAM_STATUS, (const char *)video_stream_status, MAVLINK_MSG_ID_VIDEO_STREAM_STATUS_MIN_LEN, MAVLINK_MSG_ID_VIDEO_STREAM_STATUS_LEN, MAVLINK_MSG_ID_VIDEO_STREAM_STATUS_CRC); +#endif +} + +#if MAVLINK_MSG_ID_VIDEO_STREAM_STATUS_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_video_stream_status_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint8_t stream_id, uint16_t flags, float framerate, uint16_t resolution_h, uint16_t resolution_v, uint32_t bitrate, uint16_t rotation, uint16_t hfov) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_float(buf, 0, framerate); + _mav_put_uint32_t(buf, 4, bitrate); + _mav_put_uint16_t(buf, 8, flags); + _mav_put_uint16_t(buf, 10, resolution_h); + _mav_put_uint16_t(buf, 12, resolution_v); + _mav_put_uint16_t(buf, 14, rotation); + _mav_put_uint16_t(buf, 16, hfov); + _mav_put_uint8_t(buf, 18, stream_id); + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_VIDEO_STREAM_STATUS, buf, MAVLINK_MSG_ID_VIDEO_STREAM_STATUS_MIN_LEN, MAVLINK_MSG_ID_VIDEO_STREAM_STATUS_LEN, MAVLINK_MSG_ID_VIDEO_STREAM_STATUS_CRC); +#else + mavlink_video_stream_status_t *packet = (mavlink_video_stream_status_t *)msgbuf; + packet->framerate = framerate; + packet->bitrate = bitrate; + packet->flags = flags; + packet->resolution_h = resolution_h; + packet->resolution_v = resolution_v; + packet->rotation = rotation; + packet->hfov = hfov; + packet->stream_id = stream_id; + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_VIDEO_STREAM_STATUS, (const char *)packet, MAVLINK_MSG_ID_VIDEO_STREAM_STATUS_MIN_LEN, MAVLINK_MSG_ID_VIDEO_STREAM_STATUS_LEN, MAVLINK_MSG_ID_VIDEO_STREAM_STATUS_CRC); +#endif +} +#endif + +#endif + +// MESSAGE VIDEO_STREAM_STATUS UNPACKING + + +/** + * @brief Get field stream_id from video_stream_status message + * + * @return Video Stream ID (1 for first, 2 for second, etc.) + */ +static inline uint8_t mavlink_msg_video_stream_status_get_stream_id(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 18); +} + +/** + * @brief Get field flags from video_stream_status message + * + * @return Bitmap of stream status flags + */ +static inline uint16_t mavlink_msg_video_stream_status_get_flags(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 8); +} + +/** + * @brief Get field framerate from video_stream_status message + * + * @return [Hz] Frame rate + */ +static inline float mavlink_msg_video_stream_status_get_framerate(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 0); +} + +/** + * @brief Get field resolution_h from video_stream_status message + * + * @return [pix] Horizontal resolution + */ +static inline uint16_t mavlink_msg_video_stream_status_get_resolution_h(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 10); +} + +/** + * @brief Get field resolution_v from video_stream_status message + * + * @return [pix] Vertical resolution + */ +static inline uint16_t mavlink_msg_video_stream_status_get_resolution_v(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 12); +} + +/** + * @brief Get field bitrate from video_stream_status message + * + * @return [bits/s] Bit rate + */ +static inline uint32_t mavlink_msg_video_stream_status_get_bitrate(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint32_t(msg, 4); +} + +/** + * @brief Get field rotation from video_stream_status message + * + * @return [deg] Video image rotation clockwise + */ +static inline uint16_t mavlink_msg_video_stream_status_get_rotation(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 14); +} + +/** + * @brief Get field hfov from video_stream_status message + * + * @return [deg] Horizontal Field of view + */ +static inline uint16_t mavlink_msg_video_stream_status_get_hfov(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 16); +} + +/** + * @brief Decode a video_stream_status message into a struct + * + * @param msg The message to decode + * @param video_stream_status C-struct to decode the message contents into + */ +static inline void mavlink_msg_video_stream_status_decode(const mavlink_message_t* msg, mavlink_video_stream_status_t* video_stream_status) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + video_stream_status->framerate = mavlink_msg_video_stream_status_get_framerate(msg); + video_stream_status->bitrate = mavlink_msg_video_stream_status_get_bitrate(msg); + video_stream_status->flags = mavlink_msg_video_stream_status_get_flags(msg); + video_stream_status->resolution_h = mavlink_msg_video_stream_status_get_resolution_h(msg); + video_stream_status->resolution_v = mavlink_msg_video_stream_status_get_resolution_v(msg); + video_stream_status->rotation = mavlink_msg_video_stream_status_get_rotation(msg); + video_stream_status->hfov = mavlink_msg_video_stream_status_get_hfov(msg); + video_stream_status->stream_id = mavlink_msg_video_stream_status_get_stream_id(msg); +#else + uint8_t len = msg->len < MAVLINK_MSG_ID_VIDEO_STREAM_STATUS_LEN? msg->len : MAVLINK_MSG_ID_VIDEO_STREAM_STATUS_LEN; + memset(video_stream_status, 0, MAVLINK_MSG_ID_VIDEO_STREAM_STATUS_LEN); + memcpy(video_stream_status, _MAV_PAYLOAD(msg), len); +#endif +} diff --git a/lib/main/MAVLink/common/mavlink_msg_vision_position_estimate.h b/lib/main/MAVLink/common/mavlink_msg_vision_position_estimate.h index 8c5d5d2e96..438b8c5bb3 100755 --- a/lib/main/MAVLink/common/mavlink_msg_vision_position_estimate.h +++ b/lib/main/MAVLink/common/mavlink_msg_vision_position_estimate.h @@ -12,23 +12,25 @@ typedef struct __mavlink_vision_position_estimate_t { float roll; /*< [rad] Roll angle*/ float pitch; /*< [rad] Pitch angle*/ float yaw; /*< [rad] Yaw angle*/ + float covariance[21]; /*< Row-major representation of pose 6x6 cross-covariance matrix upper right triangle (states: x, y, z, roll, pitch, yaw; first six entries are the first ROW, next five entries are the second ROW, etc.). If unknown, assign NaN value to first element in the array.*/ + uint8_t reset_counter; /*< Estimate reset counter. This should be incremented when the estimate resets in any of the dimensions (position, velocity, attitude, angular speed). This is designed to be used when e.g an external SLAM system detects a loop-closure and the estimate jumps.*/ } mavlink_vision_position_estimate_t; -#define MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE_LEN 32 +#define MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE_LEN 117 #define MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE_MIN_LEN 32 -#define MAVLINK_MSG_ID_102_LEN 32 +#define MAVLINK_MSG_ID_102_LEN 117 #define MAVLINK_MSG_ID_102_MIN_LEN 32 #define MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE_CRC 158 #define MAVLINK_MSG_ID_102_CRC 158 - +#define MAVLINK_MSG_VISION_POSITION_ESTIMATE_FIELD_COVARIANCE_LEN 21 #if MAVLINK_COMMAND_24BIT #define MAVLINK_MESSAGE_INFO_VISION_POSITION_ESTIMATE { \ 102, \ "VISION_POSITION_ESTIMATE", \ - 7, \ + 9, \ { { "usec", NULL, MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_vision_position_estimate_t, usec) }, \ { "x", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_vision_position_estimate_t, x) }, \ { "y", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_vision_position_estimate_t, y) }, \ @@ -36,12 +38,14 @@ typedef struct __mavlink_vision_position_estimate_t { { "roll", NULL, MAVLINK_TYPE_FLOAT, 0, 20, offsetof(mavlink_vision_position_estimate_t, roll) }, \ { "pitch", NULL, MAVLINK_TYPE_FLOAT, 0, 24, offsetof(mavlink_vision_position_estimate_t, pitch) }, \ { "yaw", NULL, MAVLINK_TYPE_FLOAT, 0, 28, offsetof(mavlink_vision_position_estimate_t, yaw) }, \ + { "covariance", NULL, MAVLINK_TYPE_FLOAT, 21, 32, offsetof(mavlink_vision_position_estimate_t, covariance) }, \ + { "reset_counter", NULL, MAVLINK_TYPE_UINT8_T, 0, 116, offsetof(mavlink_vision_position_estimate_t, reset_counter) }, \ } \ } #else #define MAVLINK_MESSAGE_INFO_VISION_POSITION_ESTIMATE { \ "VISION_POSITION_ESTIMATE", \ - 7, \ + 9, \ { { "usec", NULL, MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_vision_position_estimate_t, usec) }, \ { "x", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_vision_position_estimate_t, x) }, \ { "y", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_vision_position_estimate_t, y) }, \ @@ -49,6 +53,8 @@ typedef struct __mavlink_vision_position_estimate_t { { "roll", NULL, MAVLINK_TYPE_FLOAT, 0, 20, offsetof(mavlink_vision_position_estimate_t, roll) }, \ { "pitch", NULL, MAVLINK_TYPE_FLOAT, 0, 24, offsetof(mavlink_vision_position_estimate_t, pitch) }, \ { "yaw", NULL, MAVLINK_TYPE_FLOAT, 0, 28, offsetof(mavlink_vision_position_estimate_t, yaw) }, \ + { "covariance", NULL, MAVLINK_TYPE_FLOAT, 21, 32, offsetof(mavlink_vision_position_estimate_t, covariance) }, \ + { "reset_counter", NULL, MAVLINK_TYPE_UINT8_T, 0, 116, offsetof(mavlink_vision_position_estimate_t, reset_counter) }, \ } \ } #endif @@ -66,10 +72,12 @@ typedef struct __mavlink_vision_position_estimate_t { * @param roll [rad] Roll angle * @param pitch [rad] Pitch angle * @param yaw [rad] Yaw angle + * @param covariance Row-major representation of pose 6x6 cross-covariance matrix upper right triangle (states: x, y, z, roll, pitch, yaw; first six entries are the first ROW, next five entries are the second ROW, etc.). If unknown, assign NaN value to first element in the array. + * @param reset_counter Estimate reset counter. This should be incremented when the estimate resets in any of the dimensions (position, velocity, attitude, angular speed). This is designed to be used when e.g an external SLAM system detects a loop-closure and the estimate jumps. * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_vision_position_estimate_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint64_t usec, float x, float y, float z, float roll, float pitch, float yaw) + uint64_t usec, float x, float y, float z, float roll, float pitch, float yaw, const float *covariance, uint8_t reset_counter) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE_LEN]; @@ -80,7 +88,8 @@ static inline uint16_t mavlink_msg_vision_position_estimate_pack(uint8_t system_ _mav_put_float(buf, 20, roll); _mav_put_float(buf, 24, pitch); _mav_put_float(buf, 28, yaw); - + _mav_put_uint8_t(buf, 116, reset_counter); + _mav_put_float_array(buf, 32, covariance, 21); memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE_LEN); #else mavlink_vision_position_estimate_t packet; @@ -91,7 +100,8 @@ static inline uint16_t mavlink_msg_vision_position_estimate_pack(uint8_t system_ packet.roll = roll; packet.pitch = pitch; packet.yaw = yaw; - + packet.reset_counter = reset_counter; + mav_array_memcpy(packet.covariance, covariance, sizeof(float)*21); memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE_LEN); #endif @@ -112,11 +122,13 @@ static inline uint16_t mavlink_msg_vision_position_estimate_pack(uint8_t system_ * @param roll [rad] Roll angle * @param pitch [rad] Pitch angle * @param yaw [rad] Yaw angle + * @param covariance Row-major representation of pose 6x6 cross-covariance matrix upper right triangle (states: x, y, z, roll, pitch, yaw; first six entries are the first ROW, next five entries are the second ROW, etc.). If unknown, assign NaN value to first element in the array. + * @param reset_counter Estimate reset counter. This should be incremented when the estimate resets in any of the dimensions (position, velocity, attitude, angular speed). This is designed to be used when e.g an external SLAM system detects a loop-closure and the estimate jumps. * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_vision_position_estimate_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, - uint64_t usec,float x,float y,float z,float roll,float pitch,float yaw) + uint64_t usec,float x,float y,float z,float roll,float pitch,float yaw,const float *covariance,uint8_t reset_counter) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE_LEN]; @@ -127,7 +139,8 @@ static inline uint16_t mavlink_msg_vision_position_estimate_pack_chan(uint8_t sy _mav_put_float(buf, 20, roll); _mav_put_float(buf, 24, pitch); _mav_put_float(buf, 28, yaw); - + _mav_put_uint8_t(buf, 116, reset_counter); + _mav_put_float_array(buf, 32, covariance, 21); memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE_LEN); #else mavlink_vision_position_estimate_t packet; @@ -138,7 +151,8 @@ static inline uint16_t mavlink_msg_vision_position_estimate_pack_chan(uint8_t sy packet.roll = roll; packet.pitch = pitch; packet.yaw = yaw; - + packet.reset_counter = reset_counter; + mav_array_memcpy(packet.covariance, covariance, sizeof(float)*21); memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE_LEN); #endif @@ -156,7 +170,7 @@ static inline uint16_t mavlink_msg_vision_position_estimate_pack_chan(uint8_t sy */ static inline uint16_t mavlink_msg_vision_position_estimate_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_vision_position_estimate_t* vision_position_estimate) { - return mavlink_msg_vision_position_estimate_pack(system_id, component_id, msg, vision_position_estimate->usec, vision_position_estimate->x, vision_position_estimate->y, vision_position_estimate->z, vision_position_estimate->roll, vision_position_estimate->pitch, vision_position_estimate->yaw); + return mavlink_msg_vision_position_estimate_pack(system_id, component_id, msg, vision_position_estimate->usec, vision_position_estimate->x, vision_position_estimate->y, vision_position_estimate->z, vision_position_estimate->roll, vision_position_estimate->pitch, vision_position_estimate->yaw, vision_position_estimate->covariance, vision_position_estimate->reset_counter); } /** @@ -170,7 +184,7 @@ static inline uint16_t mavlink_msg_vision_position_estimate_encode(uint8_t syste */ static inline uint16_t mavlink_msg_vision_position_estimate_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_vision_position_estimate_t* vision_position_estimate) { - return mavlink_msg_vision_position_estimate_pack_chan(system_id, component_id, chan, msg, vision_position_estimate->usec, vision_position_estimate->x, vision_position_estimate->y, vision_position_estimate->z, vision_position_estimate->roll, vision_position_estimate->pitch, vision_position_estimate->yaw); + return mavlink_msg_vision_position_estimate_pack_chan(system_id, component_id, chan, msg, vision_position_estimate->usec, vision_position_estimate->x, vision_position_estimate->y, vision_position_estimate->z, vision_position_estimate->roll, vision_position_estimate->pitch, vision_position_estimate->yaw, vision_position_estimate->covariance, vision_position_estimate->reset_counter); } /** @@ -184,10 +198,12 @@ static inline uint16_t mavlink_msg_vision_position_estimate_encode_chan(uint8_t * @param roll [rad] Roll angle * @param pitch [rad] Pitch angle * @param yaw [rad] Yaw angle + * @param covariance Row-major representation of pose 6x6 cross-covariance matrix upper right triangle (states: x, y, z, roll, pitch, yaw; first six entries are the first ROW, next five entries are the second ROW, etc.). If unknown, assign NaN value to first element in the array. + * @param reset_counter Estimate reset counter. This should be incremented when the estimate resets in any of the dimensions (position, velocity, attitude, angular speed). This is designed to be used when e.g an external SLAM system detects a loop-closure and the estimate jumps. */ #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS -static inline void mavlink_msg_vision_position_estimate_send(mavlink_channel_t chan, uint64_t usec, float x, float y, float z, float roll, float pitch, float yaw) +static inline void mavlink_msg_vision_position_estimate_send(mavlink_channel_t chan, uint64_t usec, float x, float y, float z, float roll, float pitch, float yaw, const float *covariance, uint8_t reset_counter) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE_LEN]; @@ -198,7 +214,8 @@ static inline void mavlink_msg_vision_position_estimate_send(mavlink_channel_t c _mav_put_float(buf, 20, roll); _mav_put_float(buf, 24, pitch); _mav_put_float(buf, 28, yaw); - + _mav_put_uint8_t(buf, 116, reset_counter); + _mav_put_float_array(buf, 32, covariance, 21); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE, buf, MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE_MIN_LEN, MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE_LEN, MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE_CRC); #else mavlink_vision_position_estimate_t packet; @@ -209,7 +226,8 @@ static inline void mavlink_msg_vision_position_estimate_send(mavlink_channel_t c packet.roll = roll; packet.pitch = pitch; packet.yaw = yaw; - + packet.reset_counter = reset_counter; + mav_array_memcpy(packet.covariance, covariance, sizeof(float)*21); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE, (const char *)&packet, MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE_MIN_LEN, MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE_LEN, MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE_CRC); #endif } @@ -222,7 +240,7 @@ static inline void mavlink_msg_vision_position_estimate_send(mavlink_channel_t c static inline void mavlink_msg_vision_position_estimate_send_struct(mavlink_channel_t chan, const mavlink_vision_position_estimate_t* vision_position_estimate) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - mavlink_msg_vision_position_estimate_send(chan, vision_position_estimate->usec, vision_position_estimate->x, vision_position_estimate->y, vision_position_estimate->z, vision_position_estimate->roll, vision_position_estimate->pitch, vision_position_estimate->yaw); + mavlink_msg_vision_position_estimate_send(chan, vision_position_estimate->usec, vision_position_estimate->x, vision_position_estimate->y, vision_position_estimate->z, vision_position_estimate->roll, vision_position_estimate->pitch, vision_position_estimate->yaw, vision_position_estimate->covariance, vision_position_estimate->reset_counter); #else _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE, (const char *)vision_position_estimate, MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE_MIN_LEN, MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE_LEN, MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE_CRC); #endif @@ -236,7 +254,7 @@ static inline void mavlink_msg_vision_position_estimate_send_struct(mavlink_chan is usually the receive buffer for the channel, and allows a reply to an incoming message with minimum stack space usage. */ -static inline void mavlink_msg_vision_position_estimate_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint64_t usec, float x, float y, float z, float roll, float pitch, float yaw) +static inline void mavlink_msg_vision_position_estimate_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint64_t usec, float x, float y, float z, float roll, float pitch, float yaw, const float *covariance, uint8_t reset_counter) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char *buf = (char *)msgbuf; @@ -247,7 +265,8 @@ static inline void mavlink_msg_vision_position_estimate_send_buf(mavlink_message _mav_put_float(buf, 20, roll); _mav_put_float(buf, 24, pitch); _mav_put_float(buf, 28, yaw); - + _mav_put_uint8_t(buf, 116, reset_counter); + _mav_put_float_array(buf, 32, covariance, 21); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE, buf, MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE_MIN_LEN, MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE_LEN, MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE_CRC); #else mavlink_vision_position_estimate_t *packet = (mavlink_vision_position_estimate_t *)msgbuf; @@ -258,7 +277,8 @@ static inline void mavlink_msg_vision_position_estimate_send_buf(mavlink_message packet->roll = roll; packet->pitch = pitch; packet->yaw = yaw; - + packet->reset_counter = reset_counter; + mav_array_memcpy(packet->covariance, covariance, sizeof(float)*21); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE, (const char *)packet, MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE_MIN_LEN, MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE_LEN, MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE_CRC); #endif } @@ -339,6 +359,26 @@ static inline float mavlink_msg_vision_position_estimate_get_yaw(const mavlink_m return _MAV_RETURN_float(msg, 28); } +/** + * @brief Get field covariance from vision_position_estimate message + * + * @return Row-major representation of pose 6x6 cross-covariance matrix upper right triangle (states: x, y, z, roll, pitch, yaw; first six entries are the first ROW, next five entries are the second ROW, etc.). If unknown, assign NaN value to first element in the array. + */ +static inline uint16_t mavlink_msg_vision_position_estimate_get_covariance(const mavlink_message_t* msg, float *covariance) +{ + return _MAV_RETURN_float_array(msg, covariance, 21, 32); +} + +/** + * @brief Get field reset_counter from vision_position_estimate message + * + * @return Estimate reset counter. This should be incremented when the estimate resets in any of the dimensions (position, velocity, attitude, angular speed). This is designed to be used when e.g an external SLAM system detects a loop-closure and the estimate jumps. + */ +static inline uint8_t mavlink_msg_vision_position_estimate_get_reset_counter(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 116); +} + /** * @brief Decode a vision_position_estimate message into a struct * @@ -355,6 +395,8 @@ static inline void mavlink_msg_vision_position_estimate_decode(const mavlink_mes vision_position_estimate->roll = mavlink_msg_vision_position_estimate_get_roll(msg); vision_position_estimate->pitch = mavlink_msg_vision_position_estimate_get_pitch(msg); vision_position_estimate->yaw = mavlink_msg_vision_position_estimate_get_yaw(msg); + mavlink_msg_vision_position_estimate_get_covariance(msg, vision_position_estimate->covariance); + vision_position_estimate->reset_counter = mavlink_msg_vision_position_estimate_get_reset_counter(msg); #else uint8_t len = msg->len < MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE_LEN? msg->len : MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE_LEN; memset(vision_position_estimate, 0, MAVLINK_MSG_ID_VISION_POSITION_ESTIMATE_LEN); diff --git a/lib/main/MAVLink/common/mavlink_msg_vision_speed_estimate.h b/lib/main/MAVLink/common/mavlink_msg_vision_speed_estimate.h index b3f3003636..7c993e8fd6 100755 --- a/lib/main/MAVLink/common/mavlink_msg_vision_speed_estimate.h +++ b/lib/main/MAVLink/common/mavlink_msg_vision_speed_estimate.h @@ -9,37 +9,43 @@ typedef struct __mavlink_vision_speed_estimate_t { float x; /*< [m/s] Global X speed*/ float y; /*< [m/s] Global Y speed*/ float z; /*< [m/s] Global Z speed*/ + float covariance[9]; /*< Row-major representation of 3x3 linear velocity covariance matrix (states: vx, vy, vz; 1st three entries - 1st row, etc.). If unknown, assign NaN value to first element in the array.*/ + uint8_t reset_counter; /*< Estimate reset counter. This should be incremented when the estimate resets in any of the dimensions (position, velocity, attitude, angular speed). This is designed to be used when e.g an external SLAM system detects a loop-closure and the estimate jumps.*/ } mavlink_vision_speed_estimate_t; -#define MAVLINK_MSG_ID_VISION_SPEED_ESTIMATE_LEN 20 +#define MAVLINK_MSG_ID_VISION_SPEED_ESTIMATE_LEN 57 #define MAVLINK_MSG_ID_VISION_SPEED_ESTIMATE_MIN_LEN 20 -#define MAVLINK_MSG_ID_103_LEN 20 +#define MAVLINK_MSG_ID_103_LEN 57 #define MAVLINK_MSG_ID_103_MIN_LEN 20 #define MAVLINK_MSG_ID_VISION_SPEED_ESTIMATE_CRC 208 #define MAVLINK_MSG_ID_103_CRC 208 - +#define MAVLINK_MSG_VISION_SPEED_ESTIMATE_FIELD_COVARIANCE_LEN 9 #if MAVLINK_COMMAND_24BIT #define MAVLINK_MESSAGE_INFO_VISION_SPEED_ESTIMATE { \ 103, \ "VISION_SPEED_ESTIMATE", \ - 4, \ + 6, \ { { "usec", NULL, MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_vision_speed_estimate_t, usec) }, \ { "x", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_vision_speed_estimate_t, x) }, \ { "y", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_vision_speed_estimate_t, y) }, \ { "z", NULL, MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_vision_speed_estimate_t, z) }, \ + { "covariance", NULL, MAVLINK_TYPE_FLOAT, 9, 20, offsetof(mavlink_vision_speed_estimate_t, covariance) }, \ + { "reset_counter", NULL, MAVLINK_TYPE_UINT8_T, 0, 56, offsetof(mavlink_vision_speed_estimate_t, reset_counter) }, \ } \ } #else #define MAVLINK_MESSAGE_INFO_VISION_SPEED_ESTIMATE { \ "VISION_SPEED_ESTIMATE", \ - 4, \ + 6, \ { { "usec", NULL, MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_vision_speed_estimate_t, usec) }, \ { "x", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_vision_speed_estimate_t, x) }, \ { "y", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_vision_speed_estimate_t, y) }, \ { "z", NULL, MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_vision_speed_estimate_t, z) }, \ + { "covariance", NULL, MAVLINK_TYPE_FLOAT, 9, 20, offsetof(mavlink_vision_speed_estimate_t, covariance) }, \ + { "reset_counter", NULL, MAVLINK_TYPE_UINT8_T, 0, 56, offsetof(mavlink_vision_speed_estimate_t, reset_counter) }, \ } \ } #endif @@ -54,10 +60,12 @@ typedef struct __mavlink_vision_speed_estimate_t { * @param x [m/s] Global X speed * @param y [m/s] Global Y speed * @param z [m/s] Global Z speed + * @param covariance Row-major representation of 3x3 linear velocity covariance matrix (states: vx, vy, vz; 1st three entries - 1st row, etc.). If unknown, assign NaN value to first element in the array. + * @param reset_counter Estimate reset counter. This should be incremented when the estimate resets in any of the dimensions (position, velocity, attitude, angular speed). This is designed to be used when e.g an external SLAM system detects a loop-closure and the estimate jumps. * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_vision_speed_estimate_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, - uint64_t usec, float x, float y, float z) + uint64_t usec, float x, float y, float z, const float *covariance, uint8_t reset_counter) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_VISION_SPEED_ESTIMATE_LEN]; @@ -65,7 +73,8 @@ static inline uint16_t mavlink_msg_vision_speed_estimate_pack(uint8_t system_id, _mav_put_float(buf, 8, x); _mav_put_float(buf, 12, y); _mav_put_float(buf, 16, z); - + _mav_put_uint8_t(buf, 56, reset_counter); + _mav_put_float_array(buf, 20, covariance, 9); memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_VISION_SPEED_ESTIMATE_LEN); #else mavlink_vision_speed_estimate_t packet; @@ -73,7 +82,8 @@ static inline uint16_t mavlink_msg_vision_speed_estimate_pack(uint8_t system_id, packet.x = x; packet.y = y; packet.z = z; - + packet.reset_counter = reset_counter; + mav_array_memcpy(packet.covariance, covariance, sizeof(float)*9); memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_VISION_SPEED_ESTIMATE_LEN); #endif @@ -91,11 +101,13 @@ static inline uint16_t mavlink_msg_vision_speed_estimate_pack(uint8_t system_id, * @param x [m/s] Global X speed * @param y [m/s] Global Y speed * @param z [m/s] Global Z speed + * @param covariance Row-major representation of 3x3 linear velocity covariance matrix (states: vx, vy, vz; 1st three entries - 1st row, etc.). If unknown, assign NaN value to first element in the array. + * @param reset_counter Estimate reset counter. This should be incremented when the estimate resets in any of the dimensions (position, velocity, attitude, angular speed). This is designed to be used when e.g an external SLAM system detects a loop-closure and the estimate jumps. * @return length of the message in bytes (excluding serial stream start sign) */ static inline uint16_t mavlink_msg_vision_speed_estimate_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, - uint64_t usec,float x,float y,float z) + uint64_t usec,float x,float y,float z,const float *covariance,uint8_t reset_counter) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_VISION_SPEED_ESTIMATE_LEN]; @@ -103,7 +115,8 @@ static inline uint16_t mavlink_msg_vision_speed_estimate_pack_chan(uint8_t syste _mav_put_float(buf, 8, x); _mav_put_float(buf, 12, y); _mav_put_float(buf, 16, z); - + _mav_put_uint8_t(buf, 56, reset_counter); + _mav_put_float_array(buf, 20, covariance, 9); memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_VISION_SPEED_ESTIMATE_LEN); #else mavlink_vision_speed_estimate_t packet; @@ -111,7 +124,8 @@ static inline uint16_t mavlink_msg_vision_speed_estimate_pack_chan(uint8_t syste packet.x = x; packet.y = y; packet.z = z; - + packet.reset_counter = reset_counter; + mav_array_memcpy(packet.covariance, covariance, sizeof(float)*9); memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_VISION_SPEED_ESTIMATE_LEN); #endif @@ -129,7 +143,7 @@ static inline uint16_t mavlink_msg_vision_speed_estimate_pack_chan(uint8_t syste */ static inline uint16_t mavlink_msg_vision_speed_estimate_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_vision_speed_estimate_t* vision_speed_estimate) { - return mavlink_msg_vision_speed_estimate_pack(system_id, component_id, msg, vision_speed_estimate->usec, vision_speed_estimate->x, vision_speed_estimate->y, vision_speed_estimate->z); + return mavlink_msg_vision_speed_estimate_pack(system_id, component_id, msg, vision_speed_estimate->usec, vision_speed_estimate->x, vision_speed_estimate->y, vision_speed_estimate->z, vision_speed_estimate->covariance, vision_speed_estimate->reset_counter); } /** @@ -143,7 +157,7 @@ static inline uint16_t mavlink_msg_vision_speed_estimate_encode(uint8_t system_i */ static inline uint16_t mavlink_msg_vision_speed_estimate_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_vision_speed_estimate_t* vision_speed_estimate) { - return mavlink_msg_vision_speed_estimate_pack_chan(system_id, component_id, chan, msg, vision_speed_estimate->usec, vision_speed_estimate->x, vision_speed_estimate->y, vision_speed_estimate->z); + return mavlink_msg_vision_speed_estimate_pack_chan(system_id, component_id, chan, msg, vision_speed_estimate->usec, vision_speed_estimate->x, vision_speed_estimate->y, vision_speed_estimate->z, vision_speed_estimate->covariance, vision_speed_estimate->reset_counter); } /** @@ -154,10 +168,12 @@ static inline uint16_t mavlink_msg_vision_speed_estimate_encode_chan(uint8_t sys * @param x [m/s] Global X speed * @param y [m/s] Global Y speed * @param z [m/s] Global Z speed + * @param covariance Row-major representation of 3x3 linear velocity covariance matrix (states: vx, vy, vz; 1st three entries - 1st row, etc.). If unknown, assign NaN value to first element in the array. + * @param reset_counter Estimate reset counter. This should be incremented when the estimate resets in any of the dimensions (position, velocity, attitude, angular speed). This is designed to be used when e.g an external SLAM system detects a loop-closure and the estimate jumps. */ #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS -static inline void mavlink_msg_vision_speed_estimate_send(mavlink_channel_t chan, uint64_t usec, float x, float y, float z) +static inline void mavlink_msg_vision_speed_estimate_send(mavlink_channel_t chan, uint64_t usec, float x, float y, float z, const float *covariance, uint8_t reset_counter) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char buf[MAVLINK_MSG_ID_VISION_SPEED_ESTIMATE_LEN]; @@ -165,7 +181,8 @@ static inline void mavlink_msg_vision_speed_estimate_send(mavlink_channel_t chan _mav_put_float(buf, 8, x); _mav_put_float(buf, 12, y); _mav_put_float(buf, 16, z); - + _mav_put_uint8_t(buf, 56, reset_counter); + _mav_put_float_array(buf, 20, covariance, 9); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_VISION_SPEED_ESTIMATE, buf, MAVLINK_MSG_ID_VISION_SPEED_ESTIMATE_MIN_LEN, MAVLINK_MSG_ID_VISION_SPEED_ESTIMATE_LEN, MAVLINK_MSG_ID_VISION_SPEED_ESTIMATE_CRC); #else mavlink_vision_speed_estimate_t packet; @@ -173,7 +190,8 @@ static inline void mavlink_msg_vision_speed_estimate_send(mavlink_channel_t chan packet.x = x; packet.y = y; packet.z = z; - + packet.reset_counter = reset_counter; + mav_array_memcpy(packet.covariance, covariance, sizeof(float)*9); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_VISION_SPEED_ESTIMATE, (const char *)&packet, MAVLINK_MSG_ID_VISION_SPEED_ESTIMATE_MIN_LEN, MAVLINK_MSG_ID_VISION_SPEED_ESTIMATE_LEN, MAVLINK_MSG_ID_VISION_SPEED_ESTIMATE_CRC); #endif } @@ -186,7 +204,7 @@ static inline void mavlink_msg_vision_speed_estimate_send(mavlink_channel_t chan static inline void mavlink_msg_vision_speed_estimate_send_struct(mavlink_channel_t chan, const mavlink_vision_speed_estimate_t* vision_speed_estimate) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS - mavlink_msg_vision_speed_estimate_send(chan, vision_speed_estimate->usec, vision_speed_estimate->x, vision_speed_estimate->y, vision_speed_estimate->z); + mavlink_msg_vision_speed_estimate_send(chan, vision_speed_estimate->usec, vision_speed_estimate->x, vision_speed_estimate->y, vision_speed_estimate->z, vision_speed_estimate->covariance, vision_speed_estimate->reset_counter); #else _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_VISION_SPEED_ESTIMATE, (const char *)vision_speed_estimate, MAVLINK_MSG_ID_VISION_SPEED_ESTIMATE_MIN_LEN, MAVLINK_MSG_ID_VISION_SPEED_ESTIMATE_LEN, MAVLINK_MSG_ID_VISION_SPEED_ESTIMATE_CRC); #endif @@ -200,7 +218,7 @@ static inline void mavlink_msg_vision_speed_estimate_send_struct(mavlink_channel is usually the receive buffer for the channel, and allows a reply to an incoming message with minimum stack space usage. */ -static inline void mavlink_msg_vision_speed_estimate_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint64_t usec, float x, float y, float z) +static inline void mavlink_msg_vision_speed_estimate_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint64_t usec, float x, float y, float z, const float *covariance, uint8_t reset_counter) { #if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS char *buf = (char *)msgbuf; @@ -208,7 +226,8 @@ static inline void mavlink_msg_vision_speed_estimate_send_buf(mavlink_message_t _mav_put_float(buf, 8, x); _mav_put_float(buf, 12, y); _mav_put_float(buf, 16, z); - + _mav_put_uint8_t(buf, 56, reset_counter); + _mav_put_float_array(buf, 20, covariance, 9); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_VISION_SPEED_ESTIMATE, buf, MAVLINK_MSG_ID_VISION_SPEED_ESTIMATE_MIN_LEN, MAVLINK_MSG_ID_VISION_SPEED_ESTIMATE_LEN, MAVLINK_MSG_ID_VISION_SPEED_ESTIMATE_CRC); #else mavlink_vision_speed_estimate_t *packet = (mavlink_vision_speed_estimate_t *)msgbuf; @@ -216,7 +235,8 @@ static inline void mavlink_msg_vision_speed_estimate_send_buf(mavlink_message_t packet->x = x; packet->y = y; packet->z = z; - + packet->reset_counter = reset_counter; + mav_array_memcpy(packet->covariance, covariance, sizeof(float)*9); _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_VISION_SPEED_ESTIMATE, (const char *)packet, MAVLINK_MSG_ID_VISION_SPEED_ESTIMATE_MIN_LEN, MAVLINK_MSG_ID_VISION_SPEED_ESTIMATE_LEN, MAVLINK_MSG_ID_VISION_SPEED_ESTIMATE_CRC); #endif } @@ -267,6 +287,26 @@ static inline float mavlink_msg_vision_speed_estimate_get_z(const mavlink_messag return _MAV_RETURN_float(msg, 16); } +/** + * @brief Get field covariance from vision_speed_estimate message + * + * @return Row-major representation of 3x3 linear velocity covariance matrix (states: vx, vy, vz; 1st three entries - 1st row, etc.). If unknown, assign NaN value to first element in the array. + */ +static inline uint16_t mavlink_msg_vision_speed_estimate_get_covariance(const mavlink_message_t* msg, float *covariance) +{ + return _MAV_RETURN_float_array(msg, covariance, 9, 20); +} + +/** + * @brief Get field reset_counter from vision_speed_estimate message + * + * @return Estimate reset counter. This should be incremented when the estimate resets in any of the dimensions (position, velocity, attitude, angular speed). This is designed to be used when e.g an external SLAM system detects a loop-closure and the estimate jumps. + */ +static inline uint8_t mavlink_msg_vision_speed_estimate_get_reset_counter(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 56); +} + /** * @brief Decode a vision_speed_estimate message into a struct * @@ -280,6 +320,8 @@ static inline void mavlink_msg_vision_speed_estimate_decode(const mavlink_messag vision_speed_estimate->x = mavlink_msg_vision_speed_estimate_get_x(msg); vision_speed_estimate->y = mavlink_msg_vision_speed_estimate_get_y(msg); vision_speed_estimate->z = mavlink_msg_vision_speed_estimate_get_z(msg); + mavlink_msg_vision_speed_estimate_get_covariance(msg, vision_speed_estimate->covariance); + vision_speed_estimate->reset_counter = mavlink_msg_vision_speed_estimate_get_reset_counter(msg); #else uint8_t len = msg->len < MAVLINK_MSG_ID_VISION_SPEED_ESTIMATE_LEN? msg->len : MAVLINK_MSG_ID_VISION_SPEED_ESTIMATE_LEN; memset(vision_speed_estimate, 0, MAVLINK_MSG_ID_VISION_SPEED_ESTIMATE_LEN); diff --git a/lib/main/MAVLink/common/mavlink_msg_wheel_distance.h b/lib/main/MAVLink/common/mavlink_msg_wheel_distance.h new file mode 100644 index 0000000000..d506f00360 --- /dev/null +++ b/lib/main/MAVLink/common/mavlink_msg_wheel_distance.h @@ -0,0 +1,255 @@ +#pragma once +// MESSAGE WHEEL_DISTANCE PACKING + +#define MAVLINK_MSG_ID_WHEEL_DISTANCE 9000 + + +typedef struct __mavlink_wheel_distance_t { + uint64_t time_usec; /*< [us] Timestamp (synced to UNIX time or since system boot).*/ + double distance[16]; /*< [m] Distance reported by individual wheel encoders. Forward rotations increase values, reverse rotations decrease them. Not all wheels will necessarily have wheel encoders; the mapping of encoders to wheel positions must be agreed/understood by the endpoints.*/ + uint8_t count; /*< Number of wheels reported.*/ +} mavlink_wheel_distance_t; + +#define MAVLINK_MSG_ID_WHEEL_DISTANCE_LEN 137 +#define MAVLINK_MSG_ID_WHEEL_DISTANCE_MIN_LEN 137 +#define MAVLINK_MSG_ID_9000_LEN 137 +#define MAVLINK_MSG_ID_9000_MIN_LEN 137 + +#define MAVLINK_MSG_ID_WHEEL_DISTANCE_CRC 113 +#define MAVLINK_MSG_ID_9000_CRC 113 + +#define MAVLINK_MSG_WHEEL_DISTANCE_FIELD_DISTANCE_LEN 16 + +#if MAVLINK_COMMAND_24BIT +#define MAVLINK_MESSAGE_INFO_WHEEL_DISTANCE { \ + 9000, \ + "WHEEL_DISTANCE", \ + 3, \ + { { "time_usec", NULL, MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_wheel_distance_t, time_usec) }, \ + { "count", NULL, MAVLINK_TYPE_UINT8_T, 0, 136, offsetof(mavlink_wheel_distance_t, count) }, \ + { "distance", NULL, MAVLINK_TYPE_DOUBLE, 16, 8, offsetof(mavlink_wheel_distance_t, distance) }, \ + } \ +} +#else +#define MAVLINK_MESSAGE_INFO_WHEEL_DISTANCE { \ + "WHEEL_DISTANCE", \ + 3, \ + { { "time_usec", NULL, MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_wheel_distance_t, time_usec) }, \ + { "count", NULL, MAVLINK_TYPE_UINT8_T, 0, 136, offsetof(mavlink_wheel_distance_t, count) }, \ + { "distance", NULL, MAVLINK_TYPE_DOUBLE, 16, 8, offsetof(mavlink_wheel_distance_t, distance) }, \ + } \ +} +#endif + +/** + * @brief Pack a wheel_distance message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param time_usec [us] Timestamp (synced to UNIX time or since system boot). + * @param count Number of wheels reported. + * @param distance [m] Distance reported by individual wheel encoders. Forward rotations increase values, reverse rotations decrease them. Not all wheels will necessarily have wheel encoders; the mapping of encoders to wheel positions must be agreed/understood by the endpoints. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_wheel_distance_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint64_t time_usec, uint8_t count, const double *distance) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_WHEEL_DISTANCE_LEN]; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_uint8_t(buf, 136, count); + _mav_put_double_array(buf, 8, distance, 16); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_WHEEL_DISTANCE_LEN); +#else + mavlink_wheel_distance_t packet; + packet.time_usec = time_usec; + packet.count = count; + mav_array_memcpy(packet.distance, distance, sizeof(double)*16); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_WHEEL_DISTANCE_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_WHEEL_DISTANCE; + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_WHEEL_DISTANCE_MIN_LEN, MAVLINK_MSG_ID_WHEEL_DISTANCE_LEN, MAVLINK_MSG_ID_WHEEL_DISTANCE_CRC); +} + +/** + * @brief Pack a wheel_distance message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param time_usec [us] Timestamp (synced to UNIX time or since system boot). + * @param count Number of wheels reported. + * @param distance [m] Distance reported by individual wheel encoders. Forward rotations increase values, reverse rotations decrease them. Not all wheels will necessarily have wheel encoders; the mapping of encoders to wheel positions must be agreed/understood by the endpoints. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_wheel_distance_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint64_t time_usec,uint8_t count,const double *distance) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_WHEEL_DISTANCE_LEN]; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_uint8_t(buf, 136, count); + _mav_put_double_array(buf, 8, distance, 16); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_WHEEL_DISTANCE_LEN); +#else + mavlink_wheel_distance_t packet; + packet.time_usec = time_usec; + packet.count = count; + mav_array_memcpy(packet.distance, distance, sizeof(double)*16); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_WHEEL_DISTANCE_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_WHEEL_DISTANCE; + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_WHEEL_DISTANCE_MIN_LEN, MAVLINK_MSG_ID_WHEEL_DISTANCE_LEN, MAVLINK_MSG_ID_WHEEL_DISTANCE_CRC); +} + +/** + * @brief Encode a wheel_distance struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param wheel_distance C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_wheel_distance_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_wheel_distance_t* wheel_distance) +{ + return mavlink_msg_wheel_distance_pack(system_id, component_id, msg, wheel_distance->time_usec, wheel_distance->count, wheel_distance->distance); +} + +/** + * @brief Encode a wheel_distance struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param wheel_distance C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_wheel_distance_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_wheel_distance_t* wheel_distance) +{ + return mavlink_msg_wheel_distance_pack_chan(system_id, component_id, chan, msg, wheel_distance->time_usec, wheel_distance->count, wheel_distance->distance); +} + +/** + * @brief Send a wheel_distance message + * @param chan MAVLink channel to send the message + * + * @param time_usec [us] Timestamp (synced to UNIX time or since system boot). + * @param count Number of wheels reported. + * @param distance [m] Distance reported by individual wheel encoders. Forward rotations increase values, reverse rotations decrease them. Not all wheels will necessarily have wheel encoders; the mapping of encoders to wheel positions must be agreed/understood by the endpoints. + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_wheel_distance_send(mavlink_channel_t chan, uint64_t time_usec, uint8_t count, const double *distance) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_WHEEL_DISTANCE_LEN]; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_uint8_t(buf, 136, count); + _mav_put_double_array(buf, 8, distance, 16); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_WHEEL_DISTANCE, buf, MAVLINK_MSG_ID_WHEEL_DISTANCE_MIN_LEN, MAVLINK_MSG_ID_WHEEL_DISTANCE_LEN, MAVLINK_MSG_ID_WHEEL_DISTANCE_CRC); +#else + mavlink_wheel_distance_t packet; + packet.time_usec = time_usec; + packet.count = count; + mav_array_memcpy(packet.distance, distance, sizeof(double)*16); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_WHEEL_DISTANCE, (const char *)&packet, MAVLINK_MSG_ID_WHEEL_DISTANCE_MIN_LEN, MAVLINK_MSG_ID_WHEEL_DISTANCE_LEN, MAVLINK_MSG_ID_WHEEL_DISTANCE_CRC); +#endif +} + +/** + * @brief Send a wheel_distance message + * @param chan MAVLink channel to send the message + * @param struct The MAVLink struct to serialize + */ +static inline void mavlink_msg_wheel_distance_send_struct(mavlink_channel_t chan, const mavlink_wheel_distance_t* wheel_distance) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + mavlink_msg_wheel_distance_send(chan, wheel_distance->time_usec, wheel_distance->count, wheel_distance->distance); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_WHEEL_DISTANCE, (const char *)wheel_distance, MAVLINK_MSG_ID_WHEEL_DISTANCE_MIN_LEN, MAVLINK_MSG_ID_WHEEL_DISTANCE_LEN, MAVLINK_MSG_ID_WHEEL_DISTANCE_CRC); +#endif +} + +#if MAVLINK_MSG_ID_WHEEL_DISTANCE_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_wheel_distance_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint64_t time_usec, uint8_t count, const double *distance) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_uint8_t(buf, 136, count); + _mav_put_double_array(buf, 8, distance, 16); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_WHEEL_DISTANCE, buf, MAVLINK_MSG_ID_WHEEL_DISTANCE_MIN_LEN, MAVLINK_MSG_ID_WHEEL_DISTANCE_LEN, MAVLINK_MSG_ID_WHEEL_DISTANCE_CRC); +#else + mavlink_wheel_distance_t *packet = (mavlink_wheel_distance_t *)msgbuf; + packet->time_usec = time_usec; + packet->count = count; + mav_array_memcpy(packet->distance, distance, sizeof(double)*16); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_WHEEL_DISTANCE, (const char *)packet, MAVLINK_MSG_ID_WHEEL_DISTANCE_MIN_LEN, MAVLINK_MSG_ID_WHEEL_DISTANCE_LEN, MAVLINK_MSG_ID_WHEEL_DISTANCE_CRC); +#endif +} +#endif + +#endif + +// MESSAGE WHEEL_DISTANCE UNPACKING + + +/** + * @brief Get field time_usec from wheel_distance message + * + * @return [us] Timestamp (synced to UNIX time or since system boot). + */ +static inline uint64_t mavlink_msg_wheel_distance_get_time_usec(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint64_t(msg, 0); +} + +/** + * @brief Get field count from wheel_distance message + * + * @return Number of wheels reported. + */ +static inline uint8_t mavlink_msg_wheel_distance_get_count(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint8_t(msg, 136); +} + +/** + * @brief Get field distance from wheel_distance message + * + * @return [m] Distance reported by individual wheel encoders. Forward rotations increase values, reverse rotations decrease them. Not all wheels will necessarily have wheel encoders; the mapping of encoders to wheel positions must be agreed/understood by the endpoints. + */ +static inline uint16_t mavlink_msg_wheel_distance_get_distance(const mavlink_message_t* msg, double *distance) +{ + return _MAV_RETURN_double_array(msg, distance, 16, 8); +} + +/** + * @brief Decode a wheel_distance message into a struct + * + * @param msg The message to decode + * @param wheel_distance C-struct to decode the message contents into + */ +static inline void mavlink_msg_wheel_distance_decode(const mavlink_message_t* msg, mavlink_wheel_distance_t* wheel_distance) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + wheel_distance->time_usec = mavlink_msg_wheel_distance_get_time_usec(msg); + mavlink_msg_wheel_distance_get_distance(msg, wheel_distance->distance); + wheel_distance->count = mavlink_msg_wheel_distance_get_count(msg); +#else + uint8_t len = msg->len < MAVLINK_MSG_ID_WHEEL_DISTANCE_LEN? msg->len : MAVLINK_MSG_ID_WHEEL_DISTANCE_LEN; + memset(wheel_distance, 0, MAVLINK_MSG_ID_WHEEL_DISTANCE_LEN); + memcpy(wheel_distance, _MAV_PAYLOAD(msg), len); +#endif +} diff --git a/lib/main/MAVLink/common/mavlink_msg_wifi_config_ap.h b/lib/main/MAVLink/common/mavlink_msg_wifi_config_ap.h new file mode 100644 index 0000000000..f5abc2473b --- /dev/null +++ b/lib/main/MAVLink/common/mavlink_msg_wifi_config_ap.h @@ -0,0 +1,281 @@ +#pragma once +// MESSAGE WIFI_CONFIG_AP PACKING + +#define MAVLINK_MSG_ID_WIFI_CONFIG_AP 299 + + +typedef struct __mavlink_wifi_config_ap_t { + char ssid[32]; /*< Name of Wi-Fi network (SSID). Blank to leave it unchanged when setting. Current SSID when sent back as a response.*/ + char password[64]; /*< Password. Blank for an open AP. MD5 hash when message is sent back as a response.*/ + int8_t mode; /*< WiFi Mode.*/ + int8_t response; /*< Message acceptance response (sent back to GS).*/ +} mavlink_wifi_config_ap_t; + +#define MAVLINK_MSG_ID_WIFI_CONFIG_AP_LEN 98 +#define MAVLINK_MSG_ID_WIFI_CONFIG_AP_MIN_LEN 96 +#define MAVLINK_MSG_ID_299_LEN 98 +#define MAVLINK_MSG_ID_299_MIN_LEN 96 + +#define MAVLINK_MSG_ID_WIFI_CONFIG_AP_CRC 19 +#define MAVLINK_MSG_ID_299_CRC 19 + +#define MAVLINK_MSG_WIFI_CONFIG_AP_FIELD_SSID_LEN 32 +#define MAVLINK_MSG_WIFI_CONFIG_AP_FIELD_PASSWORD_LEN 64 + +#if MAVLINK_COMMAND_24BIT +#define MAVLINK_MESSAGE_INFO_WIFI_CONFIG_AP { \ + 299, \ + "WIFI_CONFIG_AP", \ + 4, \ + { { "ssid", NULL, MAVLINK_TYPE_CHAR, 32, 0, offsetof(mavlink_wifi_config_ap_t, ssid) }, \ + { "password", NULL, MAVLINK_TYPE_CHAR, 64, 32, offsetof(mavlink_wifi_config_ap_t, password) }, \ + { "mode", NULL, MAVLINK_TYPE_INT8_T, 0, 96, offsetof(mavlink_wifi_config_ap_t, mode) }, \ + { "response", NULL, MAVLINK_TYPE_INT8_T, 0, 97, offsetof(mavlink_wifi_config_ap_t, response) }, \ + } \ +} +#else +#define MAVLINK_MESSAGE_INFO_WIFI_CONFIG_AP { \ + "WIFI_CONFIG_AP", \ + 4, \ + { { "ssid", NULL, MAVLINK_TYPE_CHAR, 32, 0, offsetof(mavlink_wifi_config_ap_t, ssid) }, \ + { "password", NULL, MAVLINK_TYPE_CHAR, 64, 32, offsetof(mavlink_wifi_config_ap_t, password) }, \ + { "mode", NULL, MAVLINK_TYPE_INT8_T, 0, 96, offsetof(mavlink_wifi_config_ap_t, mode) }, \ + { "response", NULL, MAVLINK_TYPE_INT8_T, 0, 97, offsetof(mavlink_wifi_config_ap_t, response) }, \ + } \ +} +#endif + +/** + * @brief Pack a wifi_config_ap message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param ssid Name of Wi-Fi network (SSID). Blank to leave it unchanged when setting. Current SSID when sent back as a response. + * @param password Password. Blank for an open AP. MD5 hash when message is sent back as a response. + * @param mode WiFi Mode. + * @param response Message acceptance response (sent back to GS). + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_wifi_config_ap_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + const char *ssid, const char *password, int8_t mode, int8_t response) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_WIFI_CONFIG_AP_LEN]; + _mav_put_int8_t(buf, 96, mode); + _mav_put_int8_t(buf, 97, response); + _mav_put_char_array(buf, 0, ssid, 32); + _mav_put_char_array(buf, 32, password, 64); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_WIFI_CONFIG_AP_LEN); +#else + mavlink_wifi_config_ap_t packet; + packet.mode = mode; + packet.response = response; + mav_array_memcpy(packet.ssid, ssid, sizeof(char)*32); + mav_array_memcpy(packet.password, password, sizeof(char)*64); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_WIFI_CONFIG_AP_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_WIFI_CONFIG_AP; + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_WIFI_CONFIG_AP_MIN_LEN, MAVLINK_MSG_ID_WIFI_CONFIG_AP_LEN, MAVLINK_MSG_ID_WIFI_CONFIG_AP_CRC); +} + +/** + * @brief Pack a wifi_config_ap message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param ssid Name of Wi-Fi network (SSID). Blank to leave it unchanged when setting. Current SSID when sent back as a response. + * @param password Password. Blank for an open AP. MD5 hash when message is sent back as a response. + * @param mode WiFi Mode. + * @param response Message acceptance response (sent back to GS). + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_wifi_config_ap_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + const char *ssid,const char *password,int8_t mode,int8_t response) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_WIFI_CONFIG_AP_LEN]; + _mav_put_int8_t(buf, 96, mode); + _mav_put_int8_t(buf, 97, response); + _mav_put_char_array(buf, 0, ssid, 32); + _mav_put_char_array(buf, 32, password, 64); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_WIFI_CONFIG_AP_LEN); +#else + mavlink_wifi_config_ap_t packet; + packet.mode = mode; + packet.response = response; + mav_array_memcpy(packet.ssid, ssid, sizeof(char)*32); + mav_array_memcpy(packet.password, password, sizeof(char)*64); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_WIFI_CONFIG_AP_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_WIFI_CONFIG_AP; + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_WIFI_CONFIG_AP_MIN_LEN, MAVLINK_MSG_ID_WIFI_CONFIG_AP_LEN, MAVLINK_MSG_ID_WIFI_CONFIG_AP_CRC); +} + +/** + * @brief Encode a wifi_config_ap struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param wifi_config_ap C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_wifi_config_ap_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_wifi_config_ap_t* wifi_config_ap) +{ + return mavlink_msg_wifi_config_ap_pack(system_id, component_id, msg, wifi_config_ap->ssid, wifi_config_ap->password, wifi_config_ap->mode, wifi_config_ap->response); +} + +/** + * @brief Encode a wifi_config_ap struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param wifi_config_ap C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_wifi_config_ap_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_wifi_config_ap_t* wifi_config_ap) +{ + return mavlink_msg_wifi_config_ap_pack_chan(system_id, component_id, chan, msg, wifi_config_ap->ssid, wifi_config_ap->password, wifi_config_ap->mode, wifi_config_ap->response); +} + +/** + * @brief Send a wifi_config_ap message + * @param chan MAVLink channel to send the message + * + * @param ssid Name of Wi-Fi network (SSID). Blank to leave it unchanged when setting. Current SSID when sent back as a response. + * @param password Password. Blank for an open AP. MD5 hash when message is sent back as a response. + * @param mode WiFi Mode. + * @param response Message acceptance response (sent back to GS). + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_wifi_config_ap_send(mavlink_channel_t chan, const char *ssid, const char *password, int8_t mode, int8_t response) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_WIFI_CONFIG_AP_LEN]; + _mav_put_int8_t(buf, 96, mode); + _mav_put_int8_t(buf, 97, response); + _mav_put_char_array(buf, 0, ssid, 32); + _mav_put_char_array(buf, 32, password, 64); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_WIFI_CONFIG_AP, buf, MAVLINK_MSG_ID_WIFI_CONFIG_AP_MIN_LEN, MAVLINK_MSG_ID_WIFI_CONFIG_AP_LEN, MAVLINK_MSG_ID_WIFI_CONFIG_AP_CRC); +#else + mavlink_wifi_config_ap_t packet; + packet.mode = mode; + packet.response = response; + mav_array_memcpy(packet.ssid, ssid, sizeof(char)*32); + mav_array_memcpy(packet.password, password, sizeof(char)*64); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_WIFI_CONFIG_AP, (const char *)&packet, MAVLINK_MSG_ID_WIFI_CONFIG_AP_MIN_LEN, MAVLINK_MSG_ID_WIFI_CONFIG_AP_LEN, MAVLINK_MSG_ID_WIFI_CONFIG_AP_CRC); +#endif +} + +/** + * @brief Send a wifi_config_ap message + * @param chan MAVLink channel to send the message + * @param struct The MAVLink struct to serialize + */ +static inline void mavlink_msg_wifi_config_ap_send_struct(mavlink_channel_t chan, const mavlink_wifi_config_ap_t* wifi_config_ap) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + mavlink_msg_wifi_config_ap_send(chan, wifi_config_ap->ssid, wifi_config_ap->password, wifi_config_ap->mode, wifi_config_ap->response); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_WIFI_CONFIG_AP, (const char *)wifi_config_ap, MAVLINK_MSG_ID_WIFI_CONFIG_AP_MIN_LEN, MAVLINK_MSG_ID_WIFI_CONFIG_AP_LEN, MAVLINK_MSG_ID_WIFI_CONFIG_AP_CRC); +#endif +} + +#if MAVLINK_MSG_ID_WIFI_CONFIG_AP_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_wifi_config_ap_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, const char *ssid, const char *password, int8_t mode, int8_t response) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_int8_t(buf, 96, mode); + _mav_put_int8_t(buf, 97, response); + _mav_put_char_array(buf, 0, ssid, 32); + _mav_put_char_array(buf, 32, password, 64); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_WIFI_CONFIG_AP, buf, MAVLINK_MSG_ID_WIFI_CONFIG_AP_MIN_LEN, MAVLINK_MSG_ID_WIFI_CONFIG_AP_LEN, MAVLINK_MSG_ID_WIFI_CONFIG_AP_CRC); +#else + mavlink_wifi_config_ap_t *packet = (mavlink_wifi_config_ap_t *)msgbuf; + packet->mode = mode; + packet->response = response; + mav_array_memcpy(packet->ssid, ssid, sizeof(char)*32); + mav_array_memcpy(packet->password, password, sizeof(char)*64); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_WIFI_CONFIG_AP, (const char *)packet, MAVLINK_MSG_ID_WIFI_CONFIG_AP_MIN_LEN, MAVLINK_MSG_ID_WIFI_CONFIG_AP_LEN, MAVLINK_MSG_ID_WIFI_CONFIG_AP_CRC); +#endif +} +#endif + +#endif + +// MESSAGE WIFI_CONFIG_AP UNPACKING + + +/** + * @brief Get field ssid from wifi_config_ap message + * + * @return Name of Wi-Fi network (SSID). Blank to leave it unchanged when setting. Current SSID when sent back as a response. + */ +static inline uint16_t mavlink_msg_wifi_config_ap_get_ssid(const mavlink_message_t* msg, char *ssid) +{ + return _MAV_RETURN_char_array(msg, ssid, 32, 0); +} + +/** + * @brief Get field password from wifi_config_ap message + * + * @return Password. Blank for an open AP. MD5 hash when message is sent back as a response. + */ +static inline uint16_t mavlink_msg_wifi_config_ap_get_password(const mavlink_message_t* msg, char *password) +{ + return _MAV_RETURN_char_array(msg, password, 64, 32); +} + +/** + * @brief Get field mode from wifi_config_ap message + * + * @return WiFi Mode. + */ +static inline int8_t mavlink_msg_wifi_config_ap_get_mode(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int8_t(msg, 96); +} + +/** + * @brief Get field response from wifi_config_ap message + * + * @return Message acceptance response (sent back to GS). + */ +static inline int8_t mavlink_msg_wifi_config_ap_get_response(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int8_t(msg, 97); +} + +/** + * @brief Decode a wifi_config_ap message into a struct + * + * @param msg The message to decode + * @param wifi_config_ap C-struct to decode the message contents into + */ +static inline void mavlink_msg_wifi_config_ap_decode(const mavlink_message_t* msg, mavlink_wifi_config_ap_t* wifi_config_ap) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + mavlink_msg_wifi_config_ap_get_ssid(msg, wifi_config_ap->ssid); + mavlink_msg_wifi_config_ap_get_password(msg, wifi_config_ap->password); + wifi_config_ap->mode = mavlink_msg_wifi_config_ap_get_mode(msg); + wifi_config_ap->response = mavlink_msg_wifi_config_ap_get_response(msg); +#else + uint8_t len = msg->len < MAVLINK_MSG_ID_WIFI_CONFIG_AP_LEN? msg->len : MAVLINK_MSG_ID_WIFI_CONFIG_AP_LEN; + memset(wifi_config_ap, 0, MAVLINK_MSG_ID_WIFI_CONFIG_AP_LEN); + memcpy(wifi_config_ap, _MAV_PAYLOAD(msg), len); +#endif +} diff --git a/lib/main/MAVLink/common/mavlink_msg_winch_status.h b/lib/main/MAVLink/common/mavlink_msg_winch_status.h new file mode 100644 index 0000000000..62bd61695f --- /dev/null +++ b/lib/main/MAVLink/common/mavlink_msg_winch_status.h @@ -0,0 +1,388 @@ +#pragma once +// MESSAGE WINCH_STATUS PACKING + +#define MAVLINK_MSG_ID_WINCH_STATUS 9005 + + +typedef struct __mavlink_winch_status_t { + uint64_t time_usec; /*< [us] Timestamp (synced to UNIX time or since system boot).*/ + float line_length; /*< [m] Length of line released. NaN if unknown*/ + float speed; /*< [m/s] Speed line is being released or retracted. Positive values if being released, negative values if being retracted, NaN if unknown*/ + float tension; /*< [kg] Tension on the line. NaN if unknown*/ + float voltage; /*< [V] Voltage of the battery supplying the winch. NaN if unknown*/ + float current; /*< [A] Current draw from the winch. NaN if unknown*/ + uint32_t status; /*< Status flags*/ + int16_t temperature; /*< [degC] Temperature of the motor. INT16_MAX if unknown*/ +} mavlink_winch_status_t; + +#define MAVLINK_MSG_ID_WINCH_STATUS_LEN 34 +#define MAVLINK_MSG_ID_WINCH_STATUS_MIN_LEN 34 +#define MAVLINK_MSG_ID_9005_LEN 34 +#define MAVLINK_MSG_ID_9005_MIN_LEN 34 + +#define MAVLINK_MSG_ID_WINCH_STATUS_CRC 117 +#define MAVLINK_MSG_ID_9005_CRC 117 + + + +#if MAVLINK_COMMAND_24BIT +#define MAVLINK_MESSAGE_INFO_WINCH_STATUS { \ + 9005, \ + "WINCH_STATUS", \ + 8, \ + { { "time_usec", NULL, MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_winch_status_t, time_usec) }, \ + { "line_length", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_winch_status_t, line_length) }, \ + { "speed", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_winch_status_t, speed) }, \ + { "tension", NULL, MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_winch_status_t, tension) }, \ + { "voltage", NULL, MAVLINK_TYPE_FLOAT, 0, 20, offsetof(mavlink_winch_status_t, voltage) }, \ + { "current", NULL, MAVLINK_TYPE_FLOAT, 0, 24, offsetof(mavlink_winch_status_t, current) }, \ + { "temperature", NULL, MAVLINK_TYPE_INT16_T, 0, 32, offsetof(mavlink_winch_status_t, temperature) }, \ + { "status", NULL, MAVLINK_TYPE_UINT32_T, 0, 28, offsetof(mavlink_winch_status_t, status) }, \ + } \ +} +#else +#define MAVLINK_MESSAGE_INFO_WINCH_STATUS { \ + "WINCH_STATUS", \ + 8, \ + { { "time_usec", NULL, MAVLINK_TYPE_UINT64_T, 0, 0, offsetof(mavlink_winch_status_t, time_usec) }, \ + { "line_length", NULL, MAVLINK_TYPE_FLOAT, 0, 8, offsetof(mavlink_winch_status_t, line_length) }, \ + { "speed", NULL, MAVLINK_TYPE_FLOAT, 0, 12, offsetof(mavlink_winch_status_t, speed) }, \ + { "tension", NULL, MAVLINK_TYPE_FLOAT, 0, 16, offsetof(mavlink_winch_status_t, tension) }, \ + { "voltage", NULL, MAVLINK_TYPE_FLOAT, 0, 20, offsetof(mavlink_winch_status_t, voltage) }, \ + { "current", NULL, MAVLINK_TYPE_FLOAT, 0, 24, offsetof(mavlink_winch_status_t, current) }, \ + { "temperature", NULL, MAVLINK_TYPE_INT16_T, 0, 32, offsetof(mavlink_winch_status_t, temperature) }, \ + { "status", NULL, MAVLINK_TYPE_UINT32_T, 0, 28, offsetof(mavlink_winch_status_t, status) }, \ + } \ +} +#endif + +/** + * @brief Pack a winch_status message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param time_usec [us] Timestamp (synced to UNIX time or since system boot). + * @param line_length [m] Length of line released. NaN if unknown + * @param speed [m/s] Speed line is being released or retracted. Positive values if being released, negative values if being retracted, NaN if unknown + * @param tension [kg] Tension on the line. NaN if unknown + * @param voltage [V] Voltage of the battery supplying the winch. NaN if unknown + * @param current [A] Current draw from the winch. NaN if unknown + * @param temperature [degC] Temperature of the motor. INT16_MAX if unknown + * @param status Status flags + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_winch_status_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint64_t time_usec, float line_length, float speed, float tension, float voltage, float current, int16_t temperature, uint32_t status) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_WINCH_STATUS_LEN]; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_float(buf, 8, line_length); + _mav_put_float(buf, 12, speed); + _mav_put_float(buf, 16, tension); + _mav_put_float(buf, 20, voltage); + _mav_put_float(buf, 24, current); + _mav_put_uint32_t(buf, 28, status); + _mav_put_int16_t(buf, 32, temperature); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_WINCH_STATUS_LEN); +#else + mavlink_winch_status_t packet; + packet.time_usec = time_usec; + packet.line_length = line_length; + packet.speed = speed; + packet.tension = tension; + packet.voltage = voltage; + packet.current = current; + packet.status = status; + packet.temperature = temperature; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_WINCH_STATUS_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_WINCH_STATUS; + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_WINCH_STATUS_MIN_LEN, MAVLINK_MSG_ID_WINCH_STATUS_LEN, MAVLINK_MSG_ID_WINCH_STATUS_CRC); +} + +/** + * @brief Pack a winch_status message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param time_usec [us] Timestamp (synced to UNIX time or since system boot). + * @param line_length [m] Length of line released. NaN if unknown + * @param speed [m/s] Speed line is being released or retracted. Positive values if being released, negative values if being retracted, NaN if unknown + * @param tension [kg] Tension on the line. NaN if unknown + * @param voltage [V] Voltage of the battery supplying the winch. NaN if unknown + * @param current [A] Current draw from the winch. NaN if unknown + * @param temperature [degC] Temperature of the motor. INT16_MAX if unknown + * @param status Status flags + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_winch_status_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint64_t time_usec,float line_length,float speed,float tension,float voltage,float current,int16_t temperature,uint32_t status) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_WINCH_STATUS_LEN]; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_float(buf, 8, line_length); + _mav_put_float(buf, 12, speed); + _mav_put_float(buf, 16, tension); + _mav_put_float(buf, 20, voltage); + _mav_put_float(buf, 24, current); + _mav_put_uint32_t(buf, 28, status); + _mav_put_int16_t(buf, 32, temperature); + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_WINCH_STATUS_LEN); +#else + mavlink_winch_status_t packet; + packet.time_usec = time_usec; + packet.line_length = line_length; + packet.speed = speed; + packet.tension = tension; + packet.voltage = voltage; + packet.current = current; + packet.status = status; + packet.temperature = temperature; + + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_WINCH_STATUS_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_WINCH_STATUS; + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_WINCH_STATUS_MIN_LEN, MAVLINK_MSG_ID_WINCH_STATUS_LEN, MAVLINK_MSG_ID_WINCH_STATUS_CRC); +} + +/** + * @brief Encode a winch_status struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param winch_status C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_winch_status_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_winch_status_t* winch_status) +{ + return mavlink_msg_winch_status_pack(system_id, component_id, msg, winch_status->time_usec, winch_status->line_length, winch_status->speed, winch_status->tension, winch_status->voltage, winch_status->current, winch_status->temperature, winch_status->status); +} + +/** + * @brief Encode a winch_status struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param winch_status C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_winch_status_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_winch_status_t* winch_status) +{ + return mavlink_msg_winch_status_pack_chan(system_id, component_id, chan, msg, winch_status->time_usec, winch_status->line_length, winch_status->speed, winch_status->tension, winch_status->voltage, winch_status->current, winch_status->temperature, winch_status->status); +} + +/** + * @brief Send a winch_status message + * @param chan MAVLink channel to send the message + * + * @param time_usec [us] Timestamp (synced to UNIX time or since system boot). + * @param line_length [m] Length of line released. NaN if unknown + * @param speed [m/s] Speed line is being released or retracted. Positive values if being released, negative values if being retracted, NaN if unknown + * @param tension [kg] Tension on the line. NaN if unknown + * @param voltage [V] Voltage of the battery supplying the winch. NaN if unknown + * @param current [A] Current draw from the winch. NaN if unknown + * @param temperature [degC] Temperature of the motor. INT16_MAX if unknown + * @param status Status flags + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_winch_status_send(mavlink_channel_t chan, uint64_t time_usec, float line_length, float speed, float tension, float voltage, float current, int16_t temperature, uint32_t status) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_WINCH_STATUS_LEN]; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_float(buf, 8, line_length); + _mav_put_float(buf, 12, speed); + _mav_put_float(buf, 16, tension); + _mav_put_float(buf, 20, voltage); + _mav_put_float(buf, 24, current); + _mav_put_uint32_t(buf, 28, status); + _mav_put_int16_t(buf, 32, temperature); + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_WINCH_STATUS, buf, MAVLINK_MSG_ID_WINCH_STATUS_MIN_LEN, MAVLINK_MSG_ID_WINCH_STATUS_LEN, MAVLINK_MSG_ID_WINCH_STATUS_CRC); +#else + mavlink_winch_status_t packet; + packet.time_usec = time_usec; + packet.line_length = line_length; + packet.speed = speed; + packet.tension = tension; + packet.voltage = voltage; + packet.current = current; + packet.status = status; + packet.temperature = temperature; + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_WINCH_STATUS, (const char *)&packet, MAVLINK_MSG_ID_WINCH_STATUS_MIN_LEN, MAVLINK_MSG_ID_WINCH_STATUS_LEN, MAVLINK_MSG_ID_WINCH_STATUS_CRC); +#endif +} + +/** + * @brief Send a winch_status message + * @param chan MAVLink channel to send the message + * @param struct The MAVLink struct to serialize + */ +static inline void mavlink_msg_winch_status_send_struct(mavlink_channel_t chan, const mavlink_winch_status_t* winch_status) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + mavlink_msg_winch_status_send(chan, winch_status->time_usec, winch_status->line_length, winch_status->speed, winch_status->tension, winch_status->voltage, winch_status->current, winch_status->temperature, winch_status->status); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_WINCH_STATUS, (const char *)winch_status, MAVLINK_MSG_ID_WINCH_STATUS_MIN_LEN, MAVLINK_MSG_ID_WINCH_STATUS_LEN, MAVLINK_MSG_ID_WINCH_STATUS_CRC); +#endif +} + +#if MAVLINK_MSG_ID_WINCH_STATUS_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_winch_status_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint64_t time_usec, float line_length, float speed, float tension, float voltage, float current, int16_t temperature, uint32_t status) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint64_t(buf, 0, time_usec); + _mav_put_float(buf, 8, line_length); + _mav_put_float(buf, 12, speed); + _mav_put_float(buf, 16, tension); + _mav_put_float(buf, 20, voltage); + _mav_put_float(buf, 24, current); + _mav_put_uint32_t(buf, 28, status); + _mav_put_int16_t(buf, 32, temperature); + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_WINCH_STATUS, buf, MAVLINK_MSG_ID_WINCH_STATUS_MIN_LEN, MAVLINK_MSG_ID_WINCH_STATUS_LEN, MAVLINK_MSG_ID_WINCH_STATUS_CRC); +#else + mavlink_winch_status_t *packet = (mavlink_winch_status_t *)msgbuf; + packet->time_usec = time_usec; + packet->line_length = line_length; + packet->speed = speed; + packet->tension = tension; + packet->voltage = voltage; + packet->current = current; + packet->status = status; + packet->temperature = temperature; + + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_WINCH_STATUS, (const char *)packet, MAVLINK_MSG_ID_WINCH_STATUS_MIN_LEN, MAVLINK_MSG_ID_WINCH_STATUS_LEN, MAVLINK_MSG_ID_WINCH_STATUS_CRC); +#endif +} +#endif + +#endif + +// MESSAGE WINCH_STATUS UNPACKING + + +/** + * @brief Get field time_usec from winch_status message + * + * @return [us] Timestamp (synced to UNIX time or since system boot). + */ +static inline uint64_t mavlink_msg_winch_status_get_time_usec(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint64_t(msg, 0); +} + +/** + * @brief Get field line_length from winch_status message + * + * @return [m] Length of line released. NaN if unknown + */ +static inline float mavlink_msg_winch_status_get_line_length(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 8); +} + +/** + * @brief Get field speed from winch_status message + * + * @return [m/s] Speed line is being released or retracted. Positive values if being released, negative values if being retracted, NaN if unknown + */ +static inline float mavlink_msg_winch_status_get_speed(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 12); +} + +/** + * @brief Get field tension from winch_status message + * + * @return [kg] Tension on the line. NaN if unknown + */ +static inline float mavlink_msg_winch_status_get_tension(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 16); +} + +/** + * @brief Get field voltage from winch_status message + * + * @return [V] Voltage of the battery supplying the winch. NaN if unknown + */ +static inline float mavlink_msg_winch_status_get_voltage(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 20); +} + +/** + * @brief Get field current from winch_status message + * + * @return [A] Current draw from the winch. NaN if unknown + */ +static inline float mavlink_msg_winch_status_get_current(const mavlink_message_t* msg) +{ + return _MAV_RETURN_float(msg, 24); +} + +/** + * @brief Get field temperature from winch_status message + * + * @return [degC] Temperature of the motor. INT16_MAX if unknown + */ +static inline int16_t mavlink_msg_winch_status_get_temperature(const mavlink_message_t* msg) +{ + return _MAV_RETURN_int16_t(msg, 32); +} + +/** + * @brief Get field status from winch_status message + * + * @return Status flags + */ +static inline uint32_t mavlink_msg_winch_status_get_status(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint32_t(msg, 28); +} + +/** + * @brief Decode a winch_status message into a struct + * + * @param msg The message to decode + * @param winch_status C-struct to decode the message contents into + */ +static inline void mavlink_msg_winch_status_decode(const mavlink_message_t* msg, mavlink_winch_status_t* winch_status) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + winch_status->time_usec = mavlink_msg_winch_status_get_time_usec(msg); + winch_status->line_length = mavlink_msg_winch_status_get_line_length(msg); + winch_status->speed = mavlink_msg_winch_status_get_speed(msg); + winch_status->tension = mavlink_msg_winch_status_get_tension(msg); + winch_status->voltage = mavlink_msg_winch_status_get_voltage(msg); + winch_status->current = mavlink_msg_winch_status_get_current(msg); + winch_status->status = mavlink_msg_winch_status_get_status(msg); + winch_status->temperature = mavlink_msg_winch_status_get_temperature(msg); +#else + uint8_t len = msg->len < MAVLINK_MSG_ID_WINCH_STATUS_LEN? msg->len : MAVLINK_MSG_ID_WINCH_STATUS_LEN; + memset(winch_status, 0, MAVLINK_MSG_ID_WINCH_STATUS_LEN); + memcpy(winch_status, _MAV_PAYLOAD(msg), len); +#endif +} diff --git a/lib/main/MAVLink/common/testsuite.h b/lib/main/MAVLink/common/testsuite.h index a450230f3d..8cea375547 100755 --- a/lib/main/MAVLink/common/testsuite.h +++ b/lib/main/MAVLink/common/testsuite.h @@ -12,78 +12,19 @@ extern "C" { #ifndef MAVLINK_TEST_ALL #define MAVLINK_TEST_ALL - +static void mavlink_test_minimal(uint8_t, uint8_t, mavlink_message_t *last_msg); static void mavlink_test_common(uint8_t, uint8_t, mavlink_message_t *last_msg); static void mavlink_test_all(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg) { - + mavlink_test_minimal(system_id, component_id, last_msg); mavlink_test_common(system_id, component_id, last_msg); } #endif +#include "../minimal/testsuite.h" - -static void mavlink_test_heartbeat(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg) -{ -#ifdef MAVLINK_STATUS_FLAG_OUT_MAVLINK1 - mavlink_status_t *status = mavlink_get_channel_status(MAVLINK_COMM_0); - if ((status->flags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) && MAVLINK_MSG_ID_HEARTBEAT >= 256) { - return; - } -#endif - mavlink_message_t msg; - uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; - uint16_t i; - mavlink_heartbeat_t packet_in = { - 963497464,17,84,151,218,3 - }; - mavlink_heartbeat_t packet1, packet2; - memset(&packet1, 0, sizeof(packet1)); - packet1.custom_mode = packet_in.custom_mode; - packet1.type = packet_in.type; - packet1.autopilot = packet_in.autopilot; - packet1.base_mode = packet_in.base_mode; - packet1.system_status = packet_in.system_status; - packet1.mavlink_version = packet_in.mavlink_version; - - -#ifdef MAVLINK_STATUS_FLAG_OUT_MAVLINK1 - if (status->flags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) { - // cope with extensions - memset(MAVLINK_MSG_ID_HEARTBEAT_MIN_LEN + (char *)&packet1, 0, sizeof(packet1)-MAVLINK_MSG_ID_HEARTBEAT_MIN_LEN); - } -#endif - memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_heartbeat_encode(system_id, component_id, &msg, &packet1); - mavlink_msg_heartbeat_decode(&msg, &packet2); - MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); - - memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_heartbeat_pack(system_id, component_id, &msg , packet1.type , packet1.autopilot , packet1.base_mode , packet1.custom_mode , packet1.system_status ); - mavlink_msg_heartbeat_decode(&msg, &packet2); - MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); - - memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_heartbeat_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.type , packet1.autopilot , packet1.base_mode , packet1.custom_mode , packet1.system_status ); - mavlink_msg_heartbeat_decode(&msg, &packet2); - MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); - - memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_to_send_buffer(buffer, &msg); - for (i=0; iflags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) { @@ -1297,12 +1249,12 @@ static void mavlink_test_attitude_quaternion(uint8_t system_id, uint8_t componen MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_attitude_quaternion_pack(system_id, component_id, &msg , packet1.time_boot_ms , packet1.q1 , packet1.q2 , packet1.q3 , packet1.q4 , packet1.rollspeed , packet1.pitchspeed , packet1.yawspeed ); + mavlink_msg_attitude_quaternion_pack(system_id, component_id, &msg , packet1.time_boot_ms , packet1.q1 , packet1.q2 , packet1.q3 , packet1.q4 , packet1.rollspeed , packet1.pitchspeed , packet1.yawspeed , packet1.repr_offset_q ); mavlink_msg_attitude_quaternion_decode(&msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_attitude_quaternion_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.time_boot_ms , packet1.q1 , packet1.q2 , packet1.q3 , packet1.q4 , packet1.rollspeed , packet1.pitchspeed , packet1.yawspeed ); + mavlink_msg_attitude_quaternion_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.time_boot_ms , packet1.q1 , packet1.q2 , packet1.q3 , packet1.q4 , packet1.rollspeed , packet1.pitchspeed , packet1.yawspeed , packet1.repr_offset_q ); mavlink_msg_attitude_quaternion_decode(&msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); @@ -1315,7 +1267,7 @@ static void mavlink_test_attitude_quaternion(uint8_t system_id, uint8_t componen MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_attitude_quaternion_send(MAVLINK_COMM_1 , packet1.time_boot_ms , packet1.q1 , packet1.q2 , packet1.q3 , packet1.q4 , packet1.rollspeed , packet1.pitchspeed , packet1.yawspeed ); + mavlink_msg_attitude_quaternion_send(MAVLINK_COMM_1 , packet1.time_boot_ms , packet1.q1 , packet1.q2 , packet1.q3 , packet1.q4 , packet1.rollspeed , packet1.pitchspeed , packet1.yawspeed , packet1.repr_offset_q ); mavlink_msg_attitude_quaternion_decode(last_msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); } @@ -1582,7 +1534,7 @@ static void mavlink_test_servo_output_raw(uint8_t system_id, uint8_t component_i uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; uint16_t i; mavlink_servo_output_raw_t packet_in = { - 963497464,17443,17547,17651,17755,17859,17963,18067,18171,65 + 963497464,17443,17547,17651,17755,17859,17963,18067,18171,65,18327,18431,18535,18639,18743,18847,18951,19055 }; mavlink_servo_output_raw_t packet1, packet2; memset(&packet1, 0, sizeof(packet1)); @@ -1596,6 +1548,14 @@ static void mavlink_test_servo_output_raw(uint8_t system_id, uint8_t component_i packet1.servo7_raw = packet_in.servo7_raw; packet1.servo8_raw = packet_in.servo8_raw; packet1.port = packet_in.port; + packet1.servo9_raw = packet_in.servo9_raw; + packet1.servo10_raw = packet_in.servo10_raw; + packet1.servo11_raw = packet_in.servo11_raw; + packet1.servo12_raw = packet_in.servo12_raw; + packet1.servo13_raw = packet_in.servo13_raw; + packet1.servo14_raw = packet_in.servo14_raw; + packet1.servo15_raw = packet_in.servo15_raw; + packet1.servo16_raw = packet_in.servo16_raw; #ifdef MAVLINK_STATUS_FLAG_OUT_MAVLINK1 @@ -1610,12 +1570,12 @@ static void mavlink_test_servo_output_raw(uint8_t system_id, uint8_t component_i MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_servo_output_raw_pack(system_id, component_id, &msg , packet1.time_usec , packet1.port , packet1.servo1_raw , packet1.servo2_raw , packet1.servo3_raw , packet1.servo4_raw , packet1.servo5_raw , packet1.servo6_raw , packet1.servo7_raw , packet1.servo8_raw ); + mavlink_msg_servo_output_raw_pack(system_id, component_id, &msg , packet1.time_usec , packet1.port , packet1.servo1_raw , packet1.servo2_raw , packet1.servo3_raw , packet1.servo4_raw , packet1.servo5_raw , packet1.servo6_raw , packet1.servo7_raw , packet1.servo8_raw , packet1.servo9_raw , packet1.servo10_raw , packet1.servo11_raw , packet1.servo12_raw , packet1.servo13_raw , packet1.servo14_raw , packet1.servo15_raw , packet1.servo16_raw ); mavlink_msg_servo_output_raw_decode(&msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_servo_output_raw_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.time_usec , packet1.port , packet1.servo1_raw , packet1.servo2_raw , packet1.servo3_raw , packet1.servo4_raw , packet1.servo5_raw , packet1.servo6_raw , packet1.servo7_raw , packet1.servo8_raw ); + mavlink_msg_servo_output_raw_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.time_usec , packet1.port , packet1.servo1_raw , packet1.servo2_raw , packet1.servo3_raw , packet1.servo4_raw , packet1.servo5_raw , packet1.servo6_raw , packet1.servo7_raw , packet1.servo8_raw , packet1.servo9_raw , packet1.servo10_raw , packet1.servo11_raw , packet1.servo12_raw , packet1.servo13_raw , packet1.servo14_raw , packet1.servo15_raw , packet1.servo16_raw ); mavlink_msg_servo_output_raw_decode(&msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); @@ -1628,7 +1588,7 @@ static void mavlink_test_servo_output_raw(uint8_t system_id, uint8_t component_i MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_servo_output_raw_send(MAVLINK_COMM_1 , packet1.time_usec , packet1.port , packet1.servo1_raw , packet1.servo2_raw , packet1.servo3_raw , packet1.servo4_raw , packet1.servo5_raw , packet1.servo6_raw , packet1.servo7_raw , packet1.servo8_raw ); + mavlink_msg_servo_output_raw_send(MAVLINK_COMM_1 , packet1.time_usec , packet1.port , packet1.servo1_raw , packet1.servo2_raw , packet1.servo3_raw , packet1.servo4_raw , packet1.servo5_raw , packet1.servo6_raw , packet1.servo7_raw , packet1.servo8_raw , packet1.servo9_raw , packet1.servo10_raw , packet1.servo11_raw , packet1.servo12_raw , packet1.servo13_raw , packet1.servo14_raw , packet1.servo15_raw , packet1.servo16_raw ); mavlink_msg_servo_output_raw_decode(last_msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); } @@ -1645,7 +1605,7 @@ static void mavlink_test_mission_request_partial_list(uint8_t system_id, uint8_t uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; uint16_t i; mavlink_mission_request_partial_list_t packet_in = { - 17235,17339,17,84 + 17235,17339,17,84,151 }; mavlink_mission_request_partial_list_t packet1, packet2; memset(&packet1, 0, sizeof(packet1)); @@ -1653,6 +1613,7 @@ static void mavlink_test_mission_request_partial_list(uint8_t system_id, uint8_t packet1.end_index = packet_in.end_index; packet1.target_system = packet_in.target_system; packet1.target_component = packet_in.target_component; + packet1.mission_type = packet_in.mission_type; #ifdef MAVLINK_STATUS_FLAG_OUT_MAVLINK1 @@ -1667,12 +1628,12 @@ static void mavlink_test_mission_request_partial_list(uint8_t system_id, uint8_t MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_mission_request_partial_list_pack(system_id, component_id, &msg , packet1.target_system , packet1.target_component , packet1.start_index , packet1.end_index ); + mavlink_msg_mission_request_partial_list_pack(system_id, component_id, &msg , packet1.target_system , packet1.target_component , packet1.start_index , packet1.end_index , packet1.mission_type ); mavlink_msg_mission_request_partial_list_decode(&msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_mission_request_partial_list_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.target_system , packet1.target_component , packet1.start_index , packet1.end_index ); + mavlink_msg_mission_request_partial_list_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.target_system , packet1.target_component , packet1.start_index , packet1.end_index , packet1.mission_type ); mavlink_msg_mission_request_partial_list_decode(&msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); @@ -1685,7 +1646,7 @@ static void mavlink_test_mission_request_partial_list(uint8_t system_id, uint8_t MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_mission_request_partial_list_send(MAVLINK_COMM_1 , packet1.target_system , packet1.target_component , packet1.start_index , packet1.end_index ); + mavlink_msg_mission_request_partial_list_send(MAVLINK_COMM_1 , packet1.target_system , packet1.target_component , packet1.start_index , packet1.end_index , packet1.mission_type ); mavlink_msg_mission_request_partial_list_decode(last_msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); } @@ -1702,7 +1663,7 @@ static void mavlink_test_mission_write_partial_list(uint8_t system_id, uint8_t c uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; uint16_t i; mavlink_mission_write_partial_list_t packet_in = { - 17235,17339,17,84 + 17235,17339,17,84,151 }; mavlink_mission_write_partial_list_t packet1, packet2; memset(&packet1, 0, sizeof(packet1)); @@ -1710,6 +1671,7 @@ static void mavlink_test_mission_write_partial_list(uint8_t system_id, uint8_t c packet1.end_index = packet_in.end_index; packet1.target_system = packet_in.target_system; packet1.target_component = packet_in.target_component; + packet1.mission_type = packet_in.mission_type; #ifdef MAVLINK_STATUS_FLAG_OUT_MAVLINK1 @@ -1724,12 +1686,12 @@ static void mavlink_test_mission_write_partial_list(uint8_t system_id, uint8_t c MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_mission_write_partial_list_pack(system_id, component_id, &msg , packet1.target_system , packet1.target_component , packet1.start_index , packet1.end_index ); + mavlink_msg_mission_write_partial_list_pack(system_id, component_id, &msg , packet1.target_system , packet1.target_component , packet1.start_index , packet1.end_index , packet1.mission_type ); mavlink_msg_mission_write_partial_list_decode(&msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_mission_write_partial_list_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.target_system , packet1.target_component , packet1.start_index , packet1.end_index ); + mavlink_msg_mission_write_partial_list_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.target_system , packet1.target_component , packet1.start_index , packet1.end_index , packet1.mission_type ); mavlink_msg_mission_write_partial_list_decode(&msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); @@ -1742,7 +1704,7 @@ static void mavlink_test_mission_write_partial_list(uint8_t system_id, uint8_t c MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_mission_write_partial_list_send(MAVLINK_COMM_1 , packet1.target_system , packet1.target_component , packet1.start_index , packet1.end_index ); + mavlink_msg_mission_write_partial_list_send(MAVLINK_COMM_1 , packet1.target_system , packet1.target_component , packet1.start_index , packet1.end_index , packet1.mission_type ); mavlink_msg_mission_write_partial_list_decode(last_msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); } @@ -1759,7 +1721,7 @@ static void mavlink_test_mission_item(uint8_t system_id, uint8_t component_id, m uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; uint16_t i; mavlink_mission_item_t packet_in = { - 17.0,45.0,73.0,101.0,129.0,157.0,185.0,18691,18795,101,168,235,46,113 + 17.0,45.0,73.0,101.0,129.0,157.0,185.0,18691,18795,101,168,235,46,113,180 }; mavlink_mission_item_t packet1, packet2; memset(&packet1, 0, sizeof(packet1)); @@ -1777,6 +1739,7 @@ static void mavlink_test_mission_item(uint8_t system_id, uint8_t component_id, m packet1.frame = packet_in.frame; packet1.current = packet_in.current; packet1.autocontinue = packet_in.autocontinue; + packet1.mission_type = packet_in.mission_type; #ifdef MAVLINK_STATUS_FLAG_OUT_MAVLINK1 @@ -1791,12 +1754,12 @@ static void mavlink_test_mission_item(uint8_t system_id, uint8_t component_id, m MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_mission_item_pack(system_id, component_id, &msg , packet1.target_system , packet1.target_component , packet1.seq , packet1.frame , packet1.command , packet1.current , packet1.autocontinue , packet1.param1 , packet1.param2 , packet1.param3 , packet1.param4 , packet1.x , packet1.y , packet1.z ); + mavlink_msg_mission_item_pack(system_id, component_id, &msg , packet1.target_system , packet1.target_component , packet1.seq , packet1.frame , packet1.command , packet1.current , packet1.autocontinue , packet1.param1 , packet1.param2 , packet1.param3 , packet1.param4 , packet1.x , packet1.y , packet1.z , packet1.mission_type ); mavlink_msg_mission_item_decode(&msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_mission_item_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.target_system , packet1.target_component , packet1.seq , packet1.frame , packet1.command , packet1.current , packet1.autocontinue , packet1.param1 , packet1.param2 , packet1.param3 , packet1.param4 , packet1.x , packet1.y , packet1.z ); + mavlink_msg_mission_item_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.target_system , packet1.target_component , packet1.seq , packet1.frame , packet1.command , packet1.current , packet1.autocontinue , packet1.param1 , packet1.param2 , packet1.param3 , packet1.param4 , packet1.x , packet1.y , packet1.z , packet1.mission_type ); mavlink_msg_mission_item_decode(&msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); @@ -1809,7 +1772,7 @@ static void mavlink_test_mission_item(uint8_t system_id, uint8_t component_id, m MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_mission_item_send(MAVLINK_COMM_1 , packet1.target_system , packet1.target_component , packet1.seq , packet1.frame , packet1.command , packet1.current , packet1.autocontinue , packet1.param1 , packet1.param2 , packet1.param3 , packet1.param4 , packet1.x , packet1.y , packet1.z ); + mavlink_msg_mission_item_send(MAVLINK_COMM_1 , packet1.target_system , packet1.target_component , packet1.seq , packet1.frame , packet1.command , packet1.current , packet1.autocontinue , packet1.param1 , packet1.param2 , packet1.param3 , packet1.param4 , packet1.x , packet1.y , packet1.z , packet1.mission_type ); mavlink_msg_mission_item_decode(last_msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); } @@ -1826,13 +1789,14 @@ static void mavlink_test_mission_request(uint8_t system_id, uint8_t component_id uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; uint16_t i; mavlink_mission_request_t packet_in = { - 17235,139,206 + 17235,139,206,17 }; mavlink_mission_request_t packet1, packet2; memset(&packet1, 0, sizeof(packet1)); packet1.seq = packet_in.seq; packet1.target_system = packet_in.target_system; packet1.target_component = packet_in.target_component; + packet1.mission_type = packet_in.mission_type; #ifdef MAVLINK_STATUS_FLAG_OUT_MAVLINK1 @@ -1847,12 +1811,12 @@ static void mavlink_test_mission_request(uint8_t system_id, uint8_t component_id MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_mission_request_pack(system_id, component_id, &msg , packet1.target_system , packet1.target_component , packet1.seq ); + mavlink_msg_mission_request_pack(system_id, component_id, &msg , packet1.target_system , packet1.target_component , packet1.seq , packet1.mission_type ); mavlink_msg_mission_request_decode(&msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_mission_request_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.target_system , packet1.target_component , packet1.seq ); + mavlink_msg_mission_request_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.target_system , packet1.target_component , packet1.seq , packet1.mission_type ); mavlink_msg_mission_request_decode(&msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); @@ -1865,7 +1829,7 @@ static void mavlink_test_mission_request(uint8_t system_id, uint8_t component_id MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_mission_request_send(MAVLINK_COMM_1 , packet1.target_system , packet1.target_component , packet1.seq ); + mavlink_msg_mission_request_send(MAVLINK_COMM_1 , packet1.target_system , packet1.target_component , packet1.seq , packet1.mission_type ); mavlink_msg_mission_request_decode(last_msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); } @@ -1992,12 +1956,13 @@ static void mavlink_test_mission_request_list(uint8_t system_id, uint8_t compone uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; uint16_t i; mavlink_mission_request_list_t packet_in = { - 5,72 + 5,72,139 }; mavlink_mission_request_list_t packet1, packet2; memset(&packet1, 0, sizeof(packet1)); packet1.target_system = packet_in.target_system; packet1.target_component = packet_in.target_component; + packet1.mission_type = packet_in.mission_type; #ifdef MAVLINK_STATUS_FLAG_OUT_MAVLINK1 @@ -2012,12 +1977,12 @@ static void mavlink_test_mission_request_list(uint8_t system_id, uint8_t compone MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_mission_request_list_pack(system_id, component_id, &msg , packet1.target_system , packet1.target_component ); + mavlink_msg_mission_request_list_pack(system_id, component_id, &msg , packet1.target_system , packet1.target_component , packet1.mission_type ); mavlink_msg_mission_request_list_decode(&msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_mission_request_list_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.target_system , packet1.target_component ); + mavlink_msg_mission_request_list_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.target_system , packet1.target_component , packet1.mission_type ); mavlink_msg_mission_request_list_decode(&msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); @@ -2030,7 +1995,7 @@ static void mavlink_test_mission_request_list(uint8_t system_id, uint8_t compone MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_mission_request_list_send(MAVLINK_COMM_1 , packet1.target_system , packet1.target_component ); + mavlink_msg_mission_request_list_send(MAVLINK_COMM_1 , packet1.target_system , packet1.target_component , packet1.mission_type ); mavlink_msg_mission_request_list_decode(last_msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); } @@ -2047,13 +2012,14 @@ static void mavlink_test_mission_count(uint8_t system_id, uint8_t component_id, uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; uint16_t i; mavlink_mission_count_t packet_in = { - 17235,139,206 + 17235,139,206,17 }; mavlink_mission_count_t packet1, packet2; memset(&packet1, 0, sizeof(packet1)); packet1.count = packet_in.count; packet1.target_system = packet_in.target_system; packet1.target_component = packet_in.target_component; + packet1.mission_type = packet_in.mission_type; #ifdef MAVLINK_STATUS_FLAG_OUT_MAVLINK1 @@ -2068,12 +2034,12 @@ static void mavlink_test_mission_count(uint8_t system_id, uint8_t component_id, MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_mission_count_pack(system_id, component_id, &msg , packet1.target_system , packet1.target_component , packet1.count ); + mavlink_msg_mission_count_pack(system_id, component_id, &msg , packet1.target_system , packet1.target_component , packet1.count , packet1.mission_type ); mavlink_msg_mission_count_decode(&msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_mission_count_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.target_system , packet1.target_component , packet1.count ); + mavlink_msg_mission_count_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.target_system , packet1.target_component , packet1.count , packet1.mission_type ); mavlink_msg_mission_count_decode(&msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); @@ -2086,7 +2052,7 @@ static void mavlink_test_mission_count(uint8_t system_id, uint8_t component_id, MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_mission_count_send(MAVLINK_COMM_1 , packet1.target_system , packet1.target_component , packet1.count ); + mavlink_msg_mission_count_send(MAVLINK_COMM_1 , packet1.target_system , packet1.target_component , packet1.count , packet1.mission_type ); mavlink_msg_mission_count_decode(last_msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); } @@ -2103,12 +2069,13 @@ static void mavlink_test_mission_clear_all(uint8_t system_id, uint8_t component_ uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; uint16_t i; mavlink_mission_clear_all_t packet_in = { - 5,72 + 5,72,139 }; mavlink_mission_clear_all_t packet1, packet2; memset(&packet1, 0, sizeof(packet1)); packet1.target_system = packet_in.target_system; packet1.target_component = packet_in.target_component; + packet1.mission_type = packet_in.mission_type; #ifdef MAVLINK_STATUS_FLAG_OUT_MAVLINK1 @@ -2123,12 +2090,12 @@ static void mavlink_test_mission_clear_all(uint8_t system_id, uint8_t component_ MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_mission_clear_all_pack(system_id, component_id, &msg , packet1.target_system , packet1.target_component ); + mavlink_msg_mission_clear_all_pack(system_id, component_id, &msg , packet1.target_system , packet1.target_component , packet1.mission_type ); mavlink_msg_mission_clear_all_decode(&msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_mission_clear_all_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.target_system , packet1.target_component ); + mavlink_msg_mission_clear_all_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.target_system , packet1.target_component , packet1.mission_type ); mavlink_msg_mission_clear_all_decode(&msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); @@ -2141,7 +2108,7 @@ static void mavlink_test_mission_clear_all(uint8_t system_id, uint8_t component_ MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_mission_clear_all_send(MAVLINK_COMM_1 , packet1.target_system , packet1.target_component ); + mavlink_msg_mission_clear_all_send(MAVLINK_COMM_1 , packet1.target_system , packet1.target_component , packet1.mission_type ); mavlink_msg_mission_clear_all_decode(last_msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); } @@ -2212,13 +2179,14 @@ static void mavlink_test_mission_ack(uint8_t system_id, uint8_t component_id, ma uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; uint16_t i; mavlink_mission_ack_t packet_in = { - 5,72,139 + 5,72,139,206 }; mavlink_mission_ack_t packet1, packet2; memset(&packet1, 0, sizeof(packet1)); packet1.target_system = packet_in.target_system; packet1.target_component = packet_in.target_component; packet1.type = packet_in.type; + packet1.mission_type = packet_in.mission_type; #ifdef MAVLINK_STATUS_FLAG_OUT_MAVLINK1 @@ -2233,12 +2201,12 @@ static void mavlink_test_mission_ack(uint8_t system_id, uint8_t component_id, ma MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_mission_ack_pack(system_id, component_id, &msg , packet1.target_system , packet1.target_component , packet1.type ); + mavlink_msg_mission_ack_pack(system_id, component_id, &msg , packet1.target_system , packet1.target_component , packet1.type , packet1.mission_type ); mavlink_msg_mission_ack_decode(&msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_mission_ack_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.target_system , packet1.target_component , packet1.type ); + mavlink_msg_mission_ack_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.target_system , packet1.target_component , packet1.type , packet1.mission_type ); mavlink_msg_mission_ack_decode(&msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); @@ -2251,7 +2219,7 @@ static void mavlink_test_mission_ack(uint8_t system_id, uint8_t component_id, ma MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_mission_ack_send(MAVLINK_COMM_1 , packet1.target_system , packet1.target_component , packet1.type ); + mavlink_msg_mission_ack_send(MAVLINK_COMM_1 , packet1.target_system , packet1.target_component , packet1.type , packet1.mission_type ); mavlink_msg_mission_ack_decode(last_msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); } @@ -2268,7 +2236,7 @@ static void mavlink_test_set_gps_global_origin(uint8_t system_id, uint8_t compon uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; uint16_t i; mavlink_set_gps_global_origin_t packet_in = { - 963497464,963497672,963497880,41 + 963497464,963497672,963497880,41,93372036854776626ULL }; mavlink_set_gps_global_origin_t packet1, packet2; memset(&packet1, 0, sizeof(packet1)); @@ -2276,6 +2244,7 @@ static void mavlink_test_set_gps_global_origin(uint8_t system_id, uint8_t compon packet1.longitude = packet_in.longitude; packet1.altitude = packet_in.altitude; packet1.target_system = packet_in.target_system; + packet1.time_usec = packet_in.time_usec; #ifdef MAVLINK_STATUS_FLAG_OUT_MAVLINK1 @@ -2290,12 +2259,12 @@ static void mavlink_test_set_gps_global_origin(uint8_t system_id, uint8_t compon MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_set_gps_global_origin_pack(system_id, component_id, &msg , packet1.target_system , packet1.latitude , packet1.longitude , packet1.altitude ); + mavlink_msg_set_gps_global_origin_pack(system_id, component_id, &msg , packet1.target_system , packet1.latitude , packet1.longitude , packet1.altitude , packet1.time_usec ); mavlink_msg_set_gps_global_origin_decode(&msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_set_gps_global_origin_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.target_system , packet1.latitude , packet1.longitude , packet1.altitude ); + mavlink_msg_set_gps_global_origin_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.target_system , packet1.latitude , packet1.longitude , packet1.altitude , packet1.time_usec ); mavlink_msg_set_gps_global_origin_decode(&msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); @@ -2308,7 +2277,7 @@ static void mavlink_test_set_gps_global_origin(uint8_t system_id, uint8_t compon MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_set_gps_global_origin_send(MAVLINK_COMM_1 , packet1.target_system , packet1.latitude , packet1.longitude , packet1.altitude ); + mavlink_msg_set_gps_global_origin_send(MAVLINK_COMM_1 , packet1.target_system , packet1.latitude , packet1.longitude , packet1.altitude , packet1.time_usec ); mavlink_msg_set_gps_global_origin_decode(last_msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); } @@ -2325,13 +2294,14 @@ static void mavlink_test_gps_global_origin(uint8_t system_id, uint8_t component_ uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; uint16_t i; mavlink_gps_global_origin_t packet_in = { - 963497464,963497672,963497880 + 963497464,963497672,963497880,93372036854776563ULL }; mavlink_gps_global_origin_t packet1, packet2; memset(&packet1, 0, sizeof(packet1)); packet1.latitude = packet_in.latitude; packet1.longitude = packet_in.longitude; packet1.altitude = packet_in.altitude; + packet1.time_usec = packet_in.time_usec; #ifdef MAVLINK_STATUS_FLAG_OUT_MAVLINK1 @@ -2346,12 +2316,12 @@ static void mavlink_test_gps_global_origin(uint8_t system_id, uint8_t component_ MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_gps_global_origin_pack(system_id, component_id, &msg , packet1.latitude , packet1.longitude , packet1.altitude ); + mavlink_msg_gps_global_origin_pack(system_id, component_id, &msg , packet1.latitude , packet1.longitude , packet1.altitude , packet1.time_usec ); mavlink_msg_gps_global_origin_decode(&msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_gps_global_origin_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.latitude , packet1.longitude , packet1.altitude ); + mavlink_msg_gps_global_origin_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.latitude , packet1.longitude , packet1.altitude , packet1.time_usec ); mavlink_msg_gps_global_origin_decode(&msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); @@ -2364,7 +2334,7 @@ static void mavlink_test_gps_global_origin(uint8_t system_id, uint8_t component_ MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_gps_global_origin_send(MAVLINK_COMM_1 , packet1.latitude , packet1.longitude , packet1.altitude ); + mavlink_msg_gps_global_origin_send(MAVLINK_COMM_1 , packet1.latitude , packet1.longitude , packet1.altitude , packet1.time_usec ); mavlink_msg_gps_global_origin_decode(last_msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); } @@ -2443,13 +2413,14 @@ static void mavlink_test_mission_request_int(uint8_t system_id, uint8_t componen uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; uint16_t i; mavlink_mission_request_int_t packet_in = { - 17235,139,206 + 17235,139,206,17 }; mavlink_mission_request_int_t packet1, packet2; memset(&packet1, 0, sizeof(packet1)); packet1.seq = packet_in.seq; packet1.target_system = packet_in.target_system; packet1.target_component = packet_in.target_component; + packet1.mission_type = packet_in.mission_type; #ifdef MAVLINK_STATUS_FLAG_OUT_MAVLINK1 @@ -2464,12 +2435,12 @@ static void mavlink_test_mission_request_int(uint8_t system_id, uint8_t componen MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_mission_request_int_pack(system_id, component_id, &msg , packet1.target_system , packet1.target_component , packet1.seq ); + mavlink_msg_mission_request_int_pack(system_id, component_id, &msg , packet1.target_system , packet1.target_component , packet1.seq , packet1.mission_type ); mavlink_msg_mission_request_int_decode(&msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_mission_request_int_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.target_system , packet1.target_component , packet1.seq ); + mavlink_msg_mission_request_int_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.target_system , packet1.target_component , packet1.seq , packet1.mission_type ); mavlink_msg_mission_request_int_decode(&msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); @@ -2482,7 +2453,7 @@ static void mavlink_test_mission_request_int(uint8_t system_id, uint8_t componen MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_mission_request_int_send(MAVLINK_COMM_1 , packet1.target_system , packet1.target_component , packet1.seq ); + mavlink_msg_mission_request_int_send(MAVLINK_COMM_1 , packet1.target_system , packet1.target_component , packet1.seq , packet1.mission_type ); mavlink_msg_mission_request_int_decode(last_msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); } @@ -3174,7 +3145,7 @@ static void mavlink_test_rc_channels_override(uint8_t system_id, uint8_t compone uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; uint16_t i; mavlink_rc_channels_override_t packet_in = { - 17235,17339,17443,17547,17651,17755,17859,17963,53,120 + 17235,17339,17443,17547,17651,17755,17859,17963,53,120,18171,18275,18379,18483,18587,18691,18795,18899,19003,19107 }; mavlink_rc_channels_override_t packet1, packet2; memset(&packet1, 0, sizeof(packet1)); @@ -3188,6 +3159,16 @@ static void mavlink_test_rc_channels_override(uint8_t system_id, uint8_t compone packet1.chan8_raw = packet_in.chan8_raw; packet1.target_system = packet_in.target_system; packet1.target_component = packet_in.target_component; + packet1.chan9_raw = packet_in.chan9_raw; + packet1.chan10_raw = packet_in.chan10_raw; + packet1.chan11_raw = packet_in.chan11_raw; + packet1.chan12_raw = packet_in.chan12_raw; + packet1.chan13_raw = packet_in.chan13_raw; + packet1.chan14_raw = packet_in.chan14_raw; + packet1.chan15_raw = packet_in.chan15_raw; + packet1.chan16_raw = packet_in.chan16_raw; + packet1.chan17_raw = packet_in.chan17_raw; + packet1.chan18_raw = packet_in.chan18_raw; #ifdef MAVLINK_STATUS_FLAG_OUT_MAVLINK1 @@ -3202,12 +3183,12 @@ static void mavlink_test_rc_channels_override(uint8_t system_id, uint8_t compone MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_rc_channels_override_pack(system_id, component_id, &msg , packet1.target_system , packet1.target_component , packet1.chan1_raw , packet1.chan2_raw , packet1.chan3_raw , packet1.chan4_raw , packet1.chan5_raw , packet1.chan6_raw , packet1.chan7_raw , packet1.chan8_raw ); + mavlink_msg_rc_channels_override_pack(system_id, component_id, &msg , packet1.target_system , packet1.target_component , packet1.chan1_raw , packet1.chan2_raw , packet1.chan3_raw , packet1.chan4_raw , packet1.chan5_raw , packet1.chan6_raw , packet1.chan7_raw , packet1.chan8_raw , packet1.chan9_raw , packet1.chan10_raw , packet1.chan11_raw , packet1.chan12_raw , packet1.chan13_raw , packet1.chan14_raw , packet1.chan15_raw , packet1.chan16_raw , packet1.chan17_raw , packet1.chan18_raw ); mavlink_msg_rc_channels_override_decode(&msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_rc_channels_override_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.target_system , packet1.target_component , packet1.chan1_raw , packet1.chan2_raw , packet1.chan3_raw , packet1.chan4_raw , packet1.chan5_raw , packet1.chan6_raw , packet1.chan7_raw , packet1.chan8_raw ); + mavlink_msg_rc_channels_override_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.target_system , packet1.target_component , packet1.chan1_raw , packet1.chan2_raw , packet1.chan3_raw , packet1.chan4_raw , packet1.chan5_raw , packet1.chan6_raw , packet1.chan7_raw , packet1.chan8_raw , packet1.chan9_raw , packet1.chan10_raw , packet1.chan11_raw , packet1.chan12_raw , packet1.chan13_raw , packet1.chan14_raw , packet1.chan15_raw , packet1.chan16_raw , packet1.chan17_raw , packet1.chan18_raw ); mavlink_msg_rc_channels_override_decode(&msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); @@ -3220,7 +3201,7 @@ static void mavlink_test_rc_channels_override(uint8_t system_id, uint8_t compone MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_rc_channels_override_send(MAVLINK_COMM_1 , packet1.target_system , packet1.target_component , packet1.chan1_raw , packet1.chan2_raw , packet1.chan3_raw , packet1.chan4_raw , packet1.chan5_raw , packet1.chan6_raw , packet1.chan7_raw , packet1.chan8_raw ); + mavlink_msg_rc_channels_override_send(MAVLINK_COMM_1 , packet1.target_system , packet1.target_component , packet1.chan1_raw , packet1.chan2_raw , packet1.chan3_raw , packet1.chan4_raw , packet1.chan5_raw , packet1.chan6_raw , packet1.chan7_raw , packet1.chan8_raw , packet1.chan9_raw , packet1.chan10_raw , packet1.chan11_raw , packet1.chan12_raw , packet1.chan13_raw , packet1.chan14_raw , packet1.chan15_raw , packet1.chan16_raw , packet1.chan17_raw , packet1.chan18_raw ); mavlink_msg_rc_channels_override_decode(last_msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); } @@ -3237,7 +3218,7 @@ static void mavlink_test_mission_item_int(uint8_t system_id, uint8_t component_i uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; uint16_t i; mavlink_mission_item_int_t packet_in = { - 17.0,45.0,73.0,101.0,963498296,963498504,185.0,18691,18795,101,168,235,46,113 + 17.0,45.0,73.0,101.0,963498296,963498504,185.0,18691,18795,101,168,235,46,113,180 }; mavlink_mission_item_int_t packet1, packet2; memset(&packet1, 0, sizeof(packet1)); @@ -3255,6 +3236,7 @@ static void mavlink_test_mission_item_int(uint8_t system_id, uint8_t component_i packet1.frame = packet_in.frame; packet1.current = packet_in.current; packet1.autocontinue = packet_in.autocontinue; + packet1.mission_type = packet_in.mission_type; #ifdef MAVLINK_STATUS_FLAG_OUT_MAVLINK1 @@ -3269,12 +3251,12 @@ static void mavlink_test_mission_item_int(uint8_t system_id, uint8_t component_i MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_mission_item_int_pack(system_id, component_id, &msg , packet1.target_system , packet1.target_component , packet1.seq , packet1.frame , packet1.command , packet1.current , packet1.autocontinue , packet1.param1 , packet1.param2 , packet1.param3 , packet1.param4 , packet1.x , packet1.y , packet1.z ); + mavlink_msg_mission_item_int_pack(system_id, component_id, &msg , packet1.target_system , packet1.target_component , packet1.seq , packet1.frame , packet1.command , packet1.current , packet1.autocontinue , packet1.param1 , packet1.param2 , packet1.param3 , packet1.param4 , packet1.x , packet1.y , packet1.z , packet1.mission_type ); mavlink_msg_mission_item_int_decode(&msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_mission_item_int_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.target_system , packet1.target_component , packet1.seq , packet1.frame , packet1.command , packet1.current , packet1.autocontinue , packet1.param1 , packet1.param2 , packet1.param3 , packet1.param4 , packet1.x , packet1.y , packet1.z ); + mavlink_msg_mission_item_int_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.target_system , packet1.target_component , packet1.seq , packet1.frame , packet1.command , packet1.current , packet1.autocontinue , packet1.param1 , packet1.param2 , packet1.param3 , packet1.param4 , packet1.x , packet1.y , packet1.z , packet1.mission_type ); mavlink_msg_mission_item_int_decode(&msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); @@ -3287,7 +3269,7 @@ static void mavlink_test_mission_item_int(uint8_t system_id, uint8_t component_i MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_mission_item_int_send(MAVLINK_COMM_1 , packet1.target_system , packet1.target_component , packet1.seq , packet1.frame , packet1.command , packet1.current , packet1.autocontinue , packet1.param1 , packet1.param2 , packet1.param3 , packet1.param4 , packet1.x , packet1.y , packet1.z ); + mavlink_msg_mission_item_int_send(MAVLINK_COMM_1 , packet1.target_system , packet1.target_component , packet1.seq , packet1.frame , packet1.command , packet1.current , packet1.autocontinue , packet1.param1 , packet1.param2 , packet1.param3 , packet1.param4 , packet1.x , packet1.y , packet1.z , packet1.mission_type ); mavlink_msg_mission_item_int_decode(last_msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); } @@ -3493,12 +3475,16 @@ static void mavlink_test_command_ack(uint8_t system_id, uint8_t component_id, ma uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; uint16_t i; mavlink_command_ack_t packet_in = { - 17235,139 + 17235,139,206,963497672,29,96 }; mavlink_command_ack_t packet1, packet2; memset(&packet1, 0, sizeof(packet1)); packet1.command = packet_in.command; packet1.result = packet_in.result; + packet1.progress = packet_in.progress; + packet1.result_param2 = packet_in.result_param2; + packet1.target_system = packet_in.target_system; + packet1.target_component = packet_in.target_component; #ifdef MAVLINK_STATUS_FLAG_OUT_MAVLINK1 @@ -3513,12 +3499,12 @@ static void mavlink_test_command_ack(uint8_t system_id, uint8_t component_id, ma MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_command_ack_pack(system_id, component_id, &msg , packet1.command , packet1.result ); + mavlink_msg_command_ack_pack(system_id, component_id, &msg , packet1.command , packet1.result , packet1.progress , packet1.result_param2 , packet1.target_system , packet1.target_component ); mavlink_msg_command_ack_decode(&msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_command_ack_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.command , packet1.result ); + mavlink_msg_command_ack_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.command , packet1.result , packet1.progress , packet1.result_param2 , packet1.target_system , packet1.target_component ); mavlink_msg_command_ack_decode(&msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); @@ -3531,7 +3517,7 @@ static void mavlink_test_command_ack(uint8_t system_id, uint8_t component_id, ma MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_command_ack_send(MAVLINK_COMM_1 , packet1.command , packet1.result ); + mavlink_msg_command_ack_send(MAVLINK_COMM_1 , packet1.command , packet1.result , packet1.progress , packet1.result_param2 , packet1.target_system , packet1.target_component ); mavlink_msg_command_ack_decode(last_msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); } @@ -4375,7 +4361,7 @@ static void mavlink_test_optical_flow(uint8_t system_id, uint8_t component_id, m uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; uint16_t i; mavlink_optical_flow_t packet_in = { - 93372036854775807ULL,73.0,101.0,129.0,18275,18379,77,144 + 93372036854775807ULL,73.0,101.0,129.0,18275,18379,77,144,199.0,227.0 }; mavlink_optical_flow_t packet1, packet2; memset(&packet1, 0, sizeof(packet1)); @@ -4387,6 +4373,8 @@ static void mavlink_test_optical_flow(uint8_t system_id, uint8_t component_id, m packet1.flow_y = packet_in.flow_y; packet1.sensor_id = packet_in.sensor_id; packet1.quality = packet_in.quality; + packet1.flow_rate_x = packet_in.flow_rate_x; + packet1.flow_rate_y = packet_in.flow_rate_y; #ifdef MAVLINK_STATUS_FLAG_OUT_MAVLINK1 @@ -4401,12 +4389,12 @@ static void mavlink_test_optical_flow(uint8_t system_id, uint8_t component_id, m MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_optical_flow_pack(system_id, component_id, &msg , packet1.time_usec , packet1.sensor_id , packet1.flow_x , packet1.flow_y , packet1.flow_comp_m_x , packet1.flow_comp_m_y , packet1.quality , packet1.ground_distance ); + mavlink_msg_optical_flow_pack(system_id, component_id, &msg , packet1.time_usec , packet1.sensor_id , packet1.flow_x , packet1.flow_y , packet1.flow_comp_m_x , packet1.flow_comp_m_y , packet1.quality , packet1.ground_distance , packet1.flow_rate_x , packet1.flow_rate_y ); mavlink_msg_optical_flow_decode(&msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_optical_flow_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.time_usec , packet1.sensor_id , packet1.flow_x , packet1.flow_y , packet1.flow_comp_m_x , packet1.flow_comp_m_y , packet1.quality , packet1.ground_distance ); + mavlink_msg_optical_flow_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.time_usec , packet1.sensor_id , packet1.flow_x , packet1.flow_y , packet1.flow_comp_m_x , packet1.flow_comp_m_y , packet1.quality , packet1.ground_distance , packet1.flow_rate_x , packet1.flow_rate_y ); mavlink_msg_optical_flow_decode(&msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); @@ -4419,7 +4407,7 @@ static void mavlink_test_optical_flow(uint8_t system_id, uint8_t component_id, m MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_optical_flow_send(MAVLINK_COMM_1 , packet1.time_usec , packet1.sensor_id , packet1.flow_x , packet1.flow_y , packet1.flow_comp_m_x , packet1.flow_comp_m_y , packet1.quality , packet1.ground_distance ); + mavlink_msg_optical_flow_send(MAVLINK_COMM_1 , packet1.time_usec , packet1.sensor_id , packet1.flow_x , packet1.flow_y , packet1.flow_comp_m_x , packet1.flow_comp_m_y , packet1.quality , packet1.ground_distance , packet1.flow_rate_x , packet1.flow_rate_y ); mavlink_msg_optical_flow_decode(last_msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); } @@ -4436,7 +4424,7 @@ static void mavlink_test_global_vision_position_estimate(uint8_t system_id, uint uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; uint16_t i; mavlink_global_vision_position_estimate_t packet_in = { - 93372036854775807ULL,73.0,101.0,129.0,157.0,185.0,213.0 + 93372036854775807ULL,73.0,101.0,129.0,157.0,185.0,213.0,{ 241.0, 242.0, 243.0, 244.0, 245.0, 246.0, 247.0, 248.0, 249.0, 250.0, 251.0, 252.0, 253.0, 254.0, 255.0, 256.0, 257.0, 258.0, 259.0, 260.0, 261.0 },97 }; mavlink_global_vision_position_estimate_t packet1, packet2; memset(&packet1, 0, sizeof(packet1)); @@ -4447,7 +4435,9 @@ static void mavlink_test_global_vision_position_estimate(uint8_t system_id, uint packet1.roll = packet_in.roll; packet1.pitch = packet_in.pitch; packet1.yaw = packet_in.yaw; + packet1.reset_counter = packet_in.reset_counter; + mav_array_memcpy(packet1.covariance, packet_in.covariance, sizeof(float)*21); #ifdef MAVLINK_STATUS_FLAG_OUT_MAVLINK1 if (status->flags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) { @@ -4461,12 +4451,12 @@ static void mavlink_test_global_vision_position_estimate(uint8_t system_id, uint MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_global_vision_position_estimate_pack(system_id, component_id, &msg , packet1.usec , packet1.x , packet1.y , packet1.z , packet1.roll , packet1.pitch , packet1.yaw ); + mavlink_msg_global_vision_position_estimate_pack(system_id, component_id, &msg , packet1.usec , packet1.x , packet1.y , packet1.z , packet1.roll , packet1.pitch , packet1.yaw , packet1.covariance , packet1.reset_counter ); mavlink_msg_global_vision_position_estimate_decode(&msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_global_vision_position_estimate_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.usec , packet1.x , packet1.y , packet1.z , packet1.roll , packet1.pitch , packet1.yaw ); + mavlink_msg_global_vision_position_estimate_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.usec , packet1.x , packet1.y , packet1.z , packet1.roll , packet1.pitch , packet1.yaw , packet1.covariance , packet1.reset_counter ); mavlink_msg_global_vision_position_estimate_decode(&msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); @@ -4479,7 +4469,7 @@ static void mavlink_test_global_vision_position_estimate(uint8_t system_id, uint MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_global_vision_position_estimate_send(MAVLINK_COMM_1 , packet1.usec , packet1.x , packet1.y , packet1.z , packet1.roll , packet1.pitch , packet1.yaw ); + mavlink_msg_global_vision_position_estimate_send(MAVLINK_COMM_1 , packet1.usec , packet1.x , packet1.y , packet1.z , packet1.roll , packet1.pitch , packet1.yaw , packet1.covariance , packet1.reset_counter ); mavlink_msg_global_vision_position_estimate_decode(last_msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); } @@ -4496,7 +4486,7 @@ static void mavlink_test_vision_position_estimate(uint8_t system_id, uint8_t com uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; uint16_t i; mavlink_vision_position_estimate_t packet_in = { - 93372036854775807ULL,73.0,101.0,129.0,157.0,185.0,213.0 + 93372036854775807ULL,73.0,101.0,129.0,157.0,185.0,213.0,{ 241.0, 242.0, 243.0, 244.0, 245.0, 246.0, 247.0, 248.0, 249.0, 250.0, 251.0, 252.0, 253.0, 254.0, 255.0, 256.0, 257.0, 258.0, 259.0, 260.0, 261.0 },97 }; mavlink_vision_position_estimate_t packet1, packet2; memset(&packet1, 0, sizeof(packet1)); @@ -4507,7 +4497,9 @@ static void mavlink_test_vision_position_estimate(uint8_t system_id, uint8_t com packet1.roll = packet_in.roll; packet1.pitch = packet_in.pitch; packet1.yaw = packet_in.yaw; + packet1.reset_counter = packet_in.reset_counter; + mav_array_memcpy(packet1.covariance, packet_in.covariance, sizeof(float)*21); #ifdef MAVLINK_STATUS_FLAG_OUT_MAVLINK1 if (status->flags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) { @@ -4521,12 +4513,12 @@ static void mavlink_test_vision_position_estimate(uint8_t system_id, uint8_t com MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_vision_position_estimate_pack(system_id, component_id, &msg , packet1.usec , packet1.x , packet1.y , packet1.z , packet1.roll , packet1.pitch , packet1.yaw ); + mavlink_msg_vision_position_estimate_pack(system_id, component_id, &msg , packet1.usec , packet1.x , packet1.y , packet1.z , packet1.roll , packet1.pitch , packet1.yaw , packet1.covariance , packet1.reset_counter ); mavlink_msg_vision_position_estimate_decode(&msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_vision_position_estimate_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.usec , packet1.x , packet1.y , packet1.z , packet1.roll , packet1.pitch , packet1.yaw ); + mavlink_msg_vision_position_estimate_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.usec , packet1.x , packet1.y , packet1.z , packet1.roll , packet1.pitch , packet1.yaw , packet1.covariance , packet1.reset_counter ); mavlink_msg_vision_position_estimate_decode(&msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); @@ -4539,7 +4531,7 @@ static void mavlink_test_vision_position_estimate(uint8_t system_id, uint8_t com MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_vision_position_estimate_send(MAVLINK_COMM_1 , packet1.usec , packet1.x , packet1.y , packet1.z , packet1.roll , packet1.pitch , packet1.yaw ); + mavlink_msg_vision_position_estimate_send(MAVLINK_COMM_1 , packet1.usec , packet1.x , packet1.y , packet1.z , packet1.roll , packet1.pitch , packet1.yaw , packet1.covariance , packet1.reset_counter ); mavlink_msg_vision_position_estimate_decode(last_msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); } @@ -4556,7 +4548,7 @@ static void mavlink_test_vision_speed_estimate(uint8_t system_id, uint8_t compon uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; uint16_t i; mavlink_vision_speed_estimate_t packet_in = { - 93372036854775807ULL,73.0,101.0,129.0 + 93372036854775807ULL,73.0,101.0,129.0,{ 157.0, 158.0, 159.0, 160.0, 161.0, 162.0, 163.0, 164.0, 165.0 },173 }; mavlink_vision_speed_estimate_t packet1, packet2; memset(&packet1, 0, sizeof(packet1)); @@ -4564,7 +4556,9 @@ static void mavlink_test_vision_speed_estimate(uint8_t system_id, uint8_t compon packet1.x = packet_in.x; packet1.y = packet_in.y; packet1.z = packet_in.z; + packet1.reset_counter = packet_in.reset_counter; + mav_array_memcpy(packet1.covariance, packet_in.covariance, sizeof(float)*9); #ifdef MAVLINK_STATUS_FLAG_OUT_MAVLINK1 if (status->flags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) { @@ -4578,12 +4572,12 @@ static void mavlink_test_vision_speed_estimate(uint8_t system_id, uint8_t compon MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_vision_speed_estimate_pack(system_id, component_id, &msg , packet1.usec , packet1.x , packet1.y , packet1.z ); + mavlink_msg_vision_speed_estimate_pack(system_id, component_id, &msg , packet1.usec , packet1.x , packet1.y , packet1.z , packet1.covariance , packet1.reset_counter ); mavlink_msg_vision_speed_estimate_decode(&msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_vision_speed_estimate_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.usec , packet1.x , packet1.y , packet1.z ); + mavlink_msg_vision_speed_estimate_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.usec , packet1.x , packet1.y , packet1.z , packet1.covariance , packet1.reset_counter ); mavlink_msg_vision_speed_estimate_decode(&msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); @@ -4596,7 +4590,7 @@ static void mavlink_test_vision_speed_estimate(uint8_t system_id, uint8_t compon MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_vision_speed_estimate_send(MAVLINK_COMM_1 , packet1.usec , packet1.x , packet1.y , packet1.z ); + mavlink_msg_vision_speed_estimate_send(MAVLINK_COMM_1 , packet1.usec , packet1.x , packet1.y , packet1.z , packet1.covariance , packet1.reset_counter ); mavlink_msg_vision_speed_estimate_decode(last_msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); } @@ -4613,7 +4607,7 @@ static void mavlink_test_vicon_position_estimate(uint8_t system_id, uint8_t comp uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; uint16_t i; mavlink_vicon_position_estimate_t packet_in = { - 93372036854775807ULL,73.0,101.0,129.0,157.0,185.0,213.0 + 93372036854775807ULL,73.0,101.0,129.0,157.0,185.0,213.0,{ 241.0, 242.0, 243.0, 244.0, 245.0, 246.0, 247.0, 248.0, 249.0, 250.0, 251.0, 252.0, 253.0, 254.0, 255.0, 256.0, 257.0, 258.0, 259.0, 260.0, 261.0 } }; mavlink_vicon_position_estimate_t packet1, packet2; memset(&packet1, 0, sizeof(packet1)); @@ -4625,6 +4619,7 @@ static void mavlink_test_vicon_position_estimate(uint8_t system_id, uint8_t comp packet1.pitch = packet_in.pitch; packet1.yaw = packet_in.yaw; + mav_array_memcpy(packet1.covariance, packet_in.covariance, sizeof(float)*21); #ifdef MAVLINK_STATUS_FLAG_OUT_MAVLINK1 if (status->flags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) { @@ -4638,12 +4633,12 @@ static void mavlink_test_vicon_position_estimate(uint8_t system_id, uint8_t comp MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_vicon_position_estimate_pack(system_id, component_id, &msg , packet1.usec , packet1.x , packet1.y , packet1.z , packet1.roll , packet1.pitch , packet1.yaw ); + mavlink_msg_vicon_position_estimate_pack(system_id, component_id, &msg , packet1.usec , packet1.x , packet1.y , packet1.z , packet1.roll , packet1.pitch , packet1.yaw , packet1.covariance ); mavlink_msg_vicon_position_estimate_decode(&msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_vicon_position_estimate_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.usec , packet1.x , packet1.y , packet1.z , packet1.roll , packet1.pitch , packet1.yaw ); + mavlink_msg_vicon_position_estimate_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.usec , packet1.x , packet1.y , packet1.z , packet1.roll , packet1.pitch , packet1.yaw , packet1.covariance ); mavlink_msg_vicon_position_estimate_decode(&msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); @@ -4656,7 +4651,7 @@ static void mavlink_test_vicon_position_estimate(uint8_t system_id, uint8_t comp MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_vicon_position_estimate_send(MAVLINK_COMM_1 , packet1.usec , packet1.x , packet1.y , packet1.z , packet1.roll , packet1.pitch , packet1.yaw ); + mavlink_msg_vicon_position_estimate_send(MAVLINK_COMM_1 , packet1.usec , packet1.x , packet1.y , packet1.z , packet1.roll , packet1.pitch , packet1.yaw , packet1.covariance ); mavlink_msg_vicon_position_estimate_decode(last_msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); } @@ -4673,7 +4668,7 @@ static void mavlink_test_highres_imu(uint8_t system_id, uint8_t component_id, ma uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; uint16_t i; mavlink_highres_imu_t packet_in = { - 93372036854775807ULL,73.0,101.0,129.0,157.0,185.0,213.0,241.0,269.0,297.0,325.0,353.0,381.0,409.0,20355 + 93372036854775807ULL,73.0,101.0,129.0,157.0,185.0,213.0,241.0,269.0,297.0,325.0,353.0,381.0,409.0,20355,63 }; mavlink_highres_imu_t packet1, packet2; memset(&packet1, 0, sizeof(packet1)); @@ -4692,6 +4687,7 @@ static void mavlink_test_highres_imu(uint8_t system_id, uint8_t component_id, ma packet1.pressure_alt = packet_in.pressure_alt; packet1.temperature = packet_in.temperature; packet1.fields_updated = packet_in.fields_updated; + packet1.id = packet_in.id; #ifdef MAVLINK_STATUS_FLAG_OUT_MAVLINK1 @@ -4706,12 +4702,12 @@ static void mavlink_test_highres_imu(uint8_t system_id, uint8_t component_id, ma MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_highres_imu_pack(system_id, component_id, &msg , packet1.time_usec , packet1.xacc , packet1.yacc , packet1.zacc , packet1.xgyro , packet1.ygyro , packet1.zgyro , packet1.xmag , packet1.ymag , packet1.zmag , packet1.abs_pressure , packet1.diff_pressure , packet1.pressure_alt , packet1.temperature , packet1.fields_updated ); + mavlink_msg_highres_imu_pack(system_id, component_id, &msg , packet1.time_usec , packet1.xacc , packet1.yacc , packet1.zacc , packet1.xgyro , packet1.ygyro , packet1.zgyro , packet1.xmag , packet1.ymag , packet1.zmag , packet1.abs_pressure , packet1.diff_pressure , packet1.pressure_alt , packet1.temperature , packet1.fields_updated , packet1.id ); mavlink_msg_highres_imu_decode(&msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_highres_imu_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.time_usec , packet1.xacc , packet1.yacc , packet1.zacc , packet1.xgyro , packet1.ygyro , packet1.zgyro , packet1.xmag , packet1.ymag , packet1.zmag , packet1.abs_pressure , packet1.diff_pressure , packet1.pressure_alt , packet1.temperature , packet1.fields_updated ); + mavlink_msg_highres_imu_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.time_usec , packet1.xacc , packet1.yacc , packet1.zacc , packet1.xgyro , packet1.ygyro , packet1.zgyro , packet1.xmag , packet1.ymag , packet1.zmag , packet1.abs_pressure , packet1.diff_pressure , packet1.pressure_alt , packet1.temperature , packet1.fields_updated , packet1.id ); mavlink_msg_highres_imu_decode(&msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); @@ -4724,7 +4720,7 @@ static void mavlink_test_highres_imu(uint8_t system_id, uint8_t component_id, ma MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_highres_imu_send(MAVLINK_COMM_1 , packet1.time_usec , packet1.xacc , packet1.yacc , packet1.zacc , packet1.xgyro , packet1.ygyro , packet1.zgyro , packet1.xmag , packet1.ymag , packet1.zmag , packet1.abs_pressure , packet1.diff_pressure , packet1.pressure_alt , packet1.temperature , packet1.fields_updated ); + mavlink_msg_highres_imu_send(MAVLINK_COMM_1 , packet1.time_usec , packet1.xacc , packet1.yacc , packet1.zacc , packet1.xgyro , packet1.ygyro , packet1.zgyro , packet1.xmag , packet1.ymag , packet1.zmag , packet1.abs_pressure , packet1.diff_pressure , packet1.pressure_alt , packet1.temperature , packet1.fields_updated , packet1.id ); mavlink_msg_highres_imu_decode(last_msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); } @@ -4806,7 +4802,7 @@ static void mavlink_test_hil_sensor(uint8_t system_id, uint8_t component_id, mav uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; uint16_t i; mavlink_hil_sensor_t packet_in = { - 93372036854775807ULL,73.0,101.0,129.0,157.0,185.0,213.0,241.0,269.0,297.0,325.0,353.0,381.0,409.0,963500584 + 93372036854775807ULL,73.0,101.0,129.0,157.0,185.0,213.0,241.0,269.0,297.0,325.0,353.0,381.0,409.0,963500584,197 }; mavlink_hil_sensor_t packet1, packet2; memset(&packet1, 0, sizeof(packet1)); @@ -4825,6 +4821,7 @@ static void mavlink_test_hil_sensor(uint8_t system_id, uint8_t component_id, mav packet1.pressure_alt = packet_in.pressure_alt; packet1.temperature = packet_in.temperature; packet1.fields_updated = packet_in.fields_updated; + packet1.id = packet_in.id; #ifdef MAVLINK_STATUS_FLAG_OUT_MAVLINK1 @@ -4839,12 +4836,12 @@ static void mavlink_test_hil_sensor(uint8_t system_id, uint8_t component_id, mav MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_hil_sensor_pack(system_id, component_id, &msg , packet1.time_usec , packet1.xacc , packet1.yacc , packet1.zacc , packet1.xgyro , packet1.ygyro , packet1.zgyro , packet1.xmag , packet1.ymag , packet1.zmag , packet1.abs_pressure , packet1.diff_pressure , packet1.pressure_alt , packet1.temperature , packet1.fields_updated ); + mavlink_msg_hil_sensor_pack(system_id, component_id, &msg , packet1.time_usec , packet1.xacc , packet1.yacc , packet1.zacc , packet1.xgyro , packet1.ygyro , packet1.zgyro , packet1.xmag , packet1.ymag , packet1.zmag , packet1.abs_pressure , packet1.diff_pressure , packet1.pressure_alt , packet1.temperature , packet1.fields_updated , packet1.id ); mavlink_msg_hil_sensor_decode(&msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_hil_sensor_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.time_usec , packet1.xacc , packet1.yacc , packet1.zacc , packet1.xgyro , packet1.ygyro , packet1.zgyro , packet1.xmag , packet1.ymag , packet1.zmag , packet1.abs_pressure , packet1.diff_pressure , packet1.pressure_alt , packet1.temperature , packet1.fields_updated ); + mavlink_msg_hil_sensor_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.time_usec , packet1.xacc , packet1.yacc , packet1.zacc , packet1.xgyro , packet1.ygyro , packet1.zgyro , packet1.xmag , packet1.ymag , packet1.zmag , packet1.abs_pressure , packet1.diff_pressure , packet1.pressure_alt , packet1.temperature , packet1.fields_updated , packet1.id ); mavlink_msg_hil_sensor_decode(&msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); @@ -4857,7 +4854,7 @@ static void mavlink_test_hil_sensor(uint8_t system_id, uint8_t component_id, mav MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_hil_sensor_send(MAVLINK_COMM_1 , packet1.time_usec , packet1.xacc , packet1.yacc , packet1.zacc , packet1.xgyro , packet1.ygyro , packet1.zgyro , packet1.xmag , packet1.ymag , packet1.zmag , packet1.abs_pressure , packet1.diff_pressure , packet1.pressure_alt , packet1.temperature , packet1.fields_updated ); + mavlink_msg_hil_sensor_send(MAVLINK_COMM_1 , packet1.time_usec , packet1.xacc , packet1.yacc , packet1.zacc , packet1.xgyro , packet1.ygyro , packet1.zgyro , packet1.xmag , packet1.ymag , packet1.zmag , packet1.abs_pressure , packet1.diff_pressure , packet1.pressure_alt , packet1.temperature , packet1.fields_updated , packet1.id ); mavlink_msg_hil_sensor_decode(last_msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); } @@ -5175,7 +5172,7 @@ static void mavlink_test_hil_gps(uint8_t system_id, uint8_t component_id, mavlin uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; uint16_t i; mavlink_hil_gps_t packet_in = { - 93372036854775807ULL,963497880,963498088,963498296,18275,18379,18483,18587,18691,18795,18899,235,46 + 93372036854775807ULL,963497880,963498088,963498296,18275,18379,18483,18587,18691,18795,18899,235,46,113,19159 }; mavlink_hil_gps_t packet1, packet2; memset(&packet1, 0, sizeof(packet1)); @@ -5192,6 +5189,8 @@ static void mavlink_test_hil_gps(uint8_t system_id, uint8_t component_id, mavlin packet1.cog = packet_in.cog; packet1.fix_type = packet_in.fix_type; packet1.satellites_visible = packet_in.satellites_visible; + packet1.id = packet_in.id; + packet1.yaw = packet_in.yaw; #ifdef MAVLINK_STATUS_FLAG_OUT_MAVLINK1 @@ -5206,12 +5205,12 @@ static void mavlink_test_hil_gps(uint8_t system_id, uint8_t component_id, mavlin MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_hil_gps_pack(system_id, component_id, &msg , packet1.time_usec , packet1.fix_type , packet1.lat , packet1.lon , packet1.alt , packet1.eph , packet1.epv , packet1.vel , packet1.vn , packet1.ve , packet1.vd , packet1.cog , packet1.satellites_visible ); + mavlink_msg_hil_gps_pack(system_id, component_id, &msg , packet1.time_usec , packet1.fix_type , packet1.lat , packet1.lon , packet1.alt , packet1.eph , packet1.epv , packet1.vel , packet1.vn , packet1.ve , packet1.vd , packet1.cog , packet1.satellites_visible , packet1.id , packet1.yaw ); mavlink_msg_hil_gps_decode(&msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_hil_gps_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.time_usec , packet1.fix_type , packet1.lat , packet1.lon , packet1.alt , packet1.eph , packet1.epv , packet1.vel , packet1.vn , packet1.ve , packet1.vd , packet1.cog , packet1.satellites_visible ); + mavlink_msg_hil_gps_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.time_usec , packet1.fix_type , packet1.lat , packet1.lon , packet1.alt , packet1.eph , packet1.epv , packet1.vel , packet1.vn , packet1.ve , packet1.vd , packet1.cog , packet1.satellites_visible , packet1.id , packet1.yaw ); mavlink_msg_hil_gps_decode(&msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); @@ -5224,7 +5223,7 @@ static void mavlink_test_hil_gps(uint8_t system_id, uint8_t component_id, mavlin MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_hil_gps_send(MAVLINK_COMM_1 , packet1.time_usec , packet1.fix_type , packet1.lat , packet1.lon , packet1.alt , packet1.eph , packet1.epv , packet1.vel , packet1.vn , packet1.ve , packet1.vd , packet1.cog , packet1.satellites_visible ); + mavlink_msg_hil_gps_send(MAVLINK_COMM_1 , packet1.time_usec , packet1.fix_type , packet1.lat , packet1.lon , packet1.alt , packet1.eph , packet1.epv , packet1.vel , packet1.vn , packet1.ve , packet1.vd , packet1.cog , packet1.satellites_visible , packet1.id , packet1.yaw ); mavlink_msg_hil_gps_decode(last_msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); } @@ -5375,7 +5374,7 @@ static void mavlink_test_scaled_imu2(uint8_t system_id, uint8_t component_id, ma uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; uint16_t i; mavlink_scaled_imu2_t packet_in = { - 963497464,17443,17547,17651,17755,17859,17963,18067,18171,18275 + 963497464,17443,17547,17651,17755,17859,17963,18067,18171,18275,18379 }; mavlink_scaled_imu2_t packet1, packet2; memset(&packet1, 0, sizeof(packet1)); @@ -5389,6 +5388,7 @@ static void mavlink_test_scaled_imu2(uint8_t system_id, uint8_t component_id, ma packet1.xmag = packet_in.xmag; packet1.ymag = packet_in.ymag; packet1.zmag = packet_in.zmag; + packet1.temperature = packet_in.temperature; #ifdef MAVLINK_STATUS_FLAG_OUT_MAVLINK1 @@ -5403,12 +5403,12 @@ static void mavlink_test_scaled_imu2(uint8_t system_id, uint8_t component_id, ma MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_scaled_imu2_pack(system_id, component_id, &msg , packet1.time_boot_ms , packet1.xacc , packet1.yacc , packet1.zacc , packet1.xgyro , packet1.ygyro , packet1.zgyro , packet1.xmag , packet1.ymag , packet1.zmag ); + mavlink_msg_scaled_imu2_pack(system_id, component_id, &msg , packet1.time_boot_ms , packet1.xacc , packet1.yacc , packet1.zacc , packet1.xgyro , packet1.ygyro , packet1.zgyro , packet1.xmag , packet1.ymag , packet1.zmag , packet1.temperature ); mavlink_msg_scaled_imu2_decode(&msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_scaled_imu2_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.time_boot_ms , packet1.xacc , packet1.yacc , packet1.zacc , packet1.xgyro , packet1.ygyro , packet1.zgyro , packet1.xmag , packet1.ymag , packet1.zmag ); + mavlink_msg_scaled_imu2_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.time_boot_ms , packet1.xacc , packet1.yacc , packet1.zacc , packet1.xgyro , packet1.ygyro , packet1.zgyro , packet1.xmag , packet1.ymag , packet1.zmag , packet1.temperature ); mavlink_msg_scaled_imu2_decode(&msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); @@ -5421,7 +5421,7 @@ static void mavlink_test_scaled_imu2(uint8_t system_id, uint8_t component_id, ma MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_scaled_imu2_send(MAVLINK_COMM_1 , packet1.time_boot_ms , packet1.xacc , packet1.yacc , packet1.zacc , packet1.xgyro , packet1.ygyro , packet1.zgyro , packet1.xmag , packet1.ymag , packet1.zmag ); + mavlink_msg_scaled_imu2_send(MAVLINK_COMM_1 , packet1.time_boot_ms , packet1.xacc , packet1.yacc , packet1.zacc , packet1.xgyro , packet1.ygyro , packet1.zgyro , packet1.xmag , packet1.ymag , packet1.zmag , packet1.temperature ); mavlink_msg_scaled_imu2_decode(last_msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); } @@ -5835,7 +5835,7 @@ static void mavlink_test_gps2_raw(uint8_t system_id, uint8_t component_id, mavli uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; uint16_t i; mavlink_gps2_raw_t packet_in = { - 93372036854775807ULL,963497880,963498088,963498296,963498504,18483,18587,18691,18795,101,168,235 + 93372036854775807ULL,963497880,963498088,963498296,963498504,18483,18587,18691,18795,101,168,235,19055 }; mavlink_gps2_raw_t packet1, packet2; memset(&packet1, 0, sizeof(packet1)); @@ -5851,6 +5851,7 @@ static void mavlink_test_gps2_raw(uint8_t system_id, uint8_t component_id, mavli packet1.fix_type = packet_in.fix_type; packet1.satellites_visible = packet_in.satellites_visible; packet1.dgps_numch = packet_in.dgps_numch; + packet1.yaw = packet_in.yaw; #ifdef MAVLINK_STATUS_FLAG_OUT_MAVLINK1 @@ -5865,12 +5866,12 @@ static void mavlink_test_gps2_raw(uint8_t system_id, uint8_t component_id, mavli MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_gps2_raw_pack(system_id, component_id, &msg , packet1.time_usec , packet1.fix_type , packet1.lat , packet1.lon , packet1.alt , packet1.eph , packet1.epv , packet1.vel , packet1.cog , packet1.satellites_visible , packet1.dgps_numch , packet1.dgps_age ); + mavlink_msg_gps2_raw_pack(system_id, component_id, &msg , packet1.time_usec , packet1.fix_type , packet1.lat , packet1.lon , packet1.alt , packet1.eph , packet1.epv , packet1.vel , packet1.cog , packet1.satellites_visible , packet1.dgps_numch , packet1.dgps_age , packet1.yaw ); mavlink_msg_gps2_raw_decode(&msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_gps2_raw_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.time_usec , packet1.fix_type , packet1.lat , packet1.lon , packet1.alt , packet1.eph , packet1.epv , packet1.vel , packet1.cog , packet1.satellites_visible , packet1.dgps_numch , packet1.dgps_age ); + mavlink_msg_gps2_raw_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.time_usec , packet1.fix_type , packet1.lat , packet1.lon , packet1.alt , packet1.eph , packet1.epv , packet1.vel , packet1.cog , packet1.satellites_visible , packet1.dgps_numch , packet1.dgps_age , packet1.yaw ); mavlink_msg_gps2_raw_decode(&msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); @@ -5883,7 +5884,7 @@ static void mavlink_test_gps2_raw(uint8_t system_id, uint8_t component_id, mavli MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_gps2_raw_send(MAVLINK_COMM_1 , packet1.time_usec , packet1.fix_type , packet1.lat , packet1.lon , packet1.alt , packet1.eph , packet1.epv , packet1.vel , packet1.cog , packet1.satellites_visible , packet1.dgps_numch , packet1.dgps_age ); + mavlink_msg_gps2_raw_send(MAVLINK_COMM_1 , packet1.time_usec , packet1.fix_type , packet1.lat , packet1.lon , packet1.alt , packet1.eph , packet1.epv , packet1.vel , packet1.cog , packet1.satellites_visible , packet1.dgps_numch , packet1.dgps_age , packet1.yaw ); mavlink_msg_gps2_raw_decode(last_msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); } @@ -6147,7 +6148,7 @@ static void mavlink_test_scaled_imu3(uint8_t system_id, uint8_t component_id, ma uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; uint16_t i; mavlink_scaled_imu3_t packet_in = { - 963497464,17443,17547,17651,17755,17859,17963,18067,18171,18275 + 963497464,17443,17547,17651,17755,17859,17963,18067,18171,18275,18379 }; mavlink_scaled_imu3_t packet1, packet2; memset(&packet1, 0, sizeof(packet1)); @@ -6161,6 +6162,7 @@ static void mavlink_test_scaled_imu3(uint8_t system_id, uint8_t component_id, ma packet1.xmag = packet_in.xmag; packet1.ymag = packet_in.ymag; packet1.zmag = packet_in.zmag; + packet1.temperature = packet_in.temperature; #ifdef MAVLINK_STATUS_FLAG_OUT_MAVLINK1 @@ -6175,12 +6177,12 @@ static void mavlink_test_scaled_imu3(uint8_t system_id, uint8_t component_id, ma MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_scaled_imu3_pack(system_id, component_id, &msg , packet1.time_boot_ms , packet1.xacc , packet1.yacc , packet1.zacc , packet1.xgyro , packet1.ygyro , packet1.zgyro , packet1.xmag , packet1.ymag , packet1.zmag ); + mavlink_msg_scaled_imu3_pack(system_id, component_id, &msg , packet1.time_boot_ms , packet1.xacc , packet1.yacc , packet1.zacc , packet1.xgyro , packet1.ygyro , packet1.zgyro , packet1.xmag , packet1.ymag , packet1.zmag , packet1.temperature ); mavlink_msg_scaled_imu3_decode(&msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_scaled_imu3_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.time_boot_ms , packet1.xacc , packet1.yacc , packet1.zacc , packet1.xgyro , packet1.ygyro , packet1.zgyro , packet1.xmag , packet1.ymag , packet1.zmag ); + mavlink_msg_scaled_imu3_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.time_boot_ms , packet1.xacc , packet1.yacc , packet1.zacc , packet1.xgyro , packet1.ygyro , packet1.zgyro , packet1.xmag , packet1.ymag , packet1.zmag , packet1.temperature ); mavlink_msg_scaled_imu3_decode(&msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); @@ -6193,7 +6195,7 @@ static void mavlink_test_scaled_imu3(uint8_t system_id, uint8_t component_id, ma MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_scaled_imu3_send(MAVLINK_COMM_1 , packet1.time_boot_ms , packet1.xacc , packet1.yacc , packet1.zacc , packet1.xgyro , packet1.ygyro , packet1.zgyro , packet1.xmag , packet1.ymag , packet1.zmag ); + mavlink_msg_scaled_imu3_send(MAVLINK_COMM_1 , packet1.time_boot_ms , packet1.xacc , packet1.yacc , packet1.zacc , packet1.xgyro , packet1.ygyro , packet1.zgyro , packet1.xmag , packet1.ymag , packet1.zmag , packet1.temperature ); mavlink_msg_scaled_imu3_decode(last_msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); } @@ -6325,7 +6327,7 @@ static void mavlink_test_distance_sensor(uint8_t system_id, uint8_t component_id uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; uint16_t i; mavlink_distance_sensor_t packet_in = { - 963497464,17443,17547,17651,163,230,41,108 + 963497464,17443,17547,17651,163,230,41,108,115.0,143.0,{ 171.0, 172.0, 173.0, 174.0 },247 }; mavlink_distance_sensor_t packet1, packet2; memset(&packet1, 0, sizeof(packet1)); @@ -6337,7 +6339,11 @@ static void mavlink_test_distance_sensor(uint8_t system_id, uint8_t component_id packet1.id = packet_in.id; packet1.orientation = packet_in.orientation; packet1.covariance = packet_in.covariance; + packet1.horizontal_fov = packet_in.horizontal_fov; + packet1.vertical_fov = packet_in.vertical_fov; + packet1.signal_quality = packet_in.signal_quality; + mav_array_memcpy(packet1.quaternion, packet_in.quaternion, sizeof(float)*4); #ifdef MAVLINK_STATUS_FLAG_OUT_MAVLINK1 if (status->flags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) { @@ -6351,12 +6357,12 @@ static void mavlink_test_distance_sensor(uint8_t system_id, uint8_t component_id MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_distance_sensor_pack(system_id, component_id, &msg , packet1.time_boot_ms , packet1.min_distance , packet1.max_distance , packet1.current_distance , packet1.type , packet1.id , packet1.orientation , packet1.covariance ); + mavlink_msg_distance_sensor_pack(system_id, component_id, &msg , packet1.time_boot_ms , packet1.min_distance , packet1.max_distance , packet1.current_distance , packet1.type , packet1.id , packet1.orientation , packet1.covariance , packet1.horizontal_fov , packet1.vertical_fov , packet1.quaternion , packet1.signal_quality ); mavlink_msg_distance_sensor_decode(&msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_distance_sensor_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.time_boot_ms , packet1.min_distance , packet1.max_distance , packet1.current_distance , packet1.type , packet1.id , packet1.orientation , packet1.covariance ); + mavlink_msg_distance_sensor_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.time_boot_ms , packet1.min_distance , packet1.max_distance , packet1.current_distance , packet1.type , packet1.id , packet1.orientation , packet1.covariance , packet1.horizontal_fov , packet1.vertical_fov , packet1.quaternion , packet1.signal_quality ); mavlink_msg_distance_sensor_decode(&msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); @@ -6369,7 +6375,7 @@ static void mavlink_test_distance_sensor(uint8_t system_id, uint8_t component_id MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_distance_sensor_send(MAVLINK_COMM_1 , packet1.time_boot_ms , packet1.min_distance , packet1.max_distance , packet1.current_distance , packet1.type , packet1.id , packet1.orientation , packet1.covariance ); + mavlink_msg_distance_sensor_send(MAVLINK_COMM_1 , packet1.time_boot_ms , packet1.min_distance , packet1.max_distance , packet1.current_distance , packet1.type , packet1.id , packet1.orientation , packet1.covariance , packet1.horizontal_fov , packet1.vertical_fov , packet1.quaternion , packet1.signal_quality ); mavlink_msg_distance_sensor_decode(last_msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); } @@ -6616,7 +6622,7 @@ static void mavlink_test_scaled_pressure2(uint8_t system_id, uint8_t component_i uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; uint16_t i; mavlink_scaled_pressure2_t packet_in = { - 963497464,45.0,73.0,17859 + 963497464,45.0,73.0,17859,17963 }; mavlink_scaled_pressure2_t packet1, packet2; memset(&packet1, 0, sizeof(packet1)); @@ -6624,6 +6630,7 @@ static void mavlink_test_scaled_pressure2(uint8_t system_id, uint8_t component_i packet1.press_abs = packet_in.press_abs; packet1.press_diff = packet_in.press_diff; packet1.temperature = packet_in.temperature; + packet1.temperature_press_diff = packet_in.temperature_press_diff; #ifdef MAVLINK_STATUS_FLAG_OUT_MAVLINK1 @@ -6638,12 +6645,12 @@ static void mavlink_test_scaled_pressure2(uint8_t system_id, uint8_t component_i MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_scaled_pressure2_pack(system_id, component_id, &msg , packet1.time_boot_ms , packet1.press_abs , packet1.press_diff , packet1.temperature ); + mavlink_msg_scaled_pressure2_pack(system_id, component_id, &msg , packet1.time_boot_ms , packet1.press_abs , packet1.press_diff , packet1.temperature , packet1.temperature_press_diff ); mavlink_msg_scaled_pressure2_decode(&msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_scaled_pressure2_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.time_boot_ms , packet1.press_abs , packet1.press_diff , packet1.temperature ); + mavlink_msg_scaled_pressure2_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.time_boot_ms , packet1.press_abs , packet1.press_diff , packet1.temperature , packet1.temperature_press_diff ); mavlink_msg_scaled_pressure2_decode(&msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); @@ -6656,7 +6663,7 @@ static void mavlink_test_scaled_pressure2(uint8_t system_id, uint8_t component_i MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_scaled_pressure2_send(MAVLINK_COMM_1 , packet1.time_boot_ms , packet1.press_abs , packet1.press_diff , packet1.temperature ); + mavlink_msg_scaled_pressure2_send(MAVLINK_COMM_1 , packet1.time_boot_ms , packet1.press_abs , packet1.press_diff , packet1.temperature , packet1.temperature_press_diff ); mavlink_msg_scaled_pressure2_decode(last_msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); } @@ -6673,7 +6680,7 @@ static void mavlink_test_att_pos_mocap(uint8_t system_id, uint8_t component_id, uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; uint16_t i; mavlink_att_pos_mocap_t packet_in = { - 93372036854775807ULL,{ 73.0, 74.0, 75.0, 76.0 },185.0,213.0,241.0 + 93372036854775807ULL,{ 73.0, 74.0, 75.0, 76.0 },185.0,213.0,241.0,{ 269.0, 270.0, 271.0, 272.0, 273.0, 274.0, 275.0, 276.0, 277.0, 278.0, 279.0, 280.0, 281.0, 282.0, 283.0, 284.0, 285.0, 286.0, 287.0, 288.0, 289.0 } }; mavlink_att_pos_mocap_t packet1, packet2; memset(&packet1, 0, sizeof(packet1)); @@ -6683,6 +6690,7 @@ static void mavlink_test_att_pos_mocap(uint8_t system_id, uint8_t component_id, packet1.z = packet_in.z; mav_array_memcpy(packet1.q, packet_in.q, sizeof(float)*4); + mav_array_memcpy(packet1.covariance, packet_in.covariance, sizeof(float)*21); #ifdef MAVLINK_STATUS_FLAG_OUT_MAVLINK1 if (status->flags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) { @@ -6696,12 +6704,12 @@ static void mavlink_test_att_pos_mocap(uint8_t system_id, uint8_t component_id, MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_att_pos_mocap_pack(system_id, component_id, &msg , packet1.time_usec , packet1.q , packet1.x , packet1.y , packet1.z ); + mavlink_msg_att_pos_mocap_pack(system_id, component_id, &msg , packet1.time_usec , packet1.q , packet1.x , packet1.y , packet1.z , packet1.covariance ); mavlink_msg_att_pos_mocap_decode(&msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_att_pos_mocap_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.time_usec , packet1.q , packet1.x , packet1.y , packet1.z ); + mavlink_msg_att_pos_mocap_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.time_usec , packet1.q , packet1.x , packet1.y , packet1.z , packet1.covariance ); mavlink_msg_att_pos_mocap_decode(&msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); @@ -6714,7 +6722,7 @@ static void mavlink_test_att_pos_mocap(uint8_t system_id, uint8_t component_id, MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_att_pos_mocap_send(MAVLINK_COMM_1 , packet1.time_usec , packet1.q , packet1.x , packet1.y , packet1.z ); + mavlink_msg_att_pos_mocap_send(MAVLINK_COMM_1 , packet1.time_usec , packet1.q , packet1.x , packet1.y , packet1.z , packet1.covariance ); mavlink_msg_att_pos_mocap_decode(last_msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); } @@ -6963,7 +6971,7 @@ static void mavlink_test_scaled_pressure3(uint8_t system_id, uint8_t component_i uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; uint16_t i; mavlink_scaled_pressure3_t packet_in = { - 963497464,45.0,73.0,17859 + 963497464,45.0,73.0,17859,17963 }; mavlink_scaled_pressure3_t packet1, packet2; memset(&packet1, 0, sizeof(packet1)); @@ -6971,6 +6979,7 @@ static void mavlink_test_scaled_pressure3(uint8_t system_id, uint8_t component_i packet1.press_abs = packet_in.press_abs; packet1.press_diff = packet_in.press_diff; packet1.temperature = packet_in.temperature; + packet1.temperature_press_diff = packet_in.temperature_press_diff; #ifdef MAVLINK_STATUS_FLAG_OUT_MAVLINK1 @@ -6985,12 +6994,12 @@ static void mavlink_test_scaled_pressure3(uint8_t system_id, uint8_t component_i MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_scaled_pressure3_pack(system_id, component_id, &msg , packet1.time_boot_ms , packet1.press_abs , packet1.press_diff , packet1.temperature ); + mavlink_msg_scaled_pressure3_pack(system_id, component_id, &msg , packet1.time_boot_ms , packet1.press_abs , packet1.press_diff , packet1.temperature , packet1.temperature_press_diff ); mavlink_msg_scaled_pressure3_decode(&msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_scaled_pressure3_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.time_boot_ms , packet1.press_abs , packet1.press_diff , packet1.temperature ); + mavlink_msg_scaled_pressure3_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.time_boot_ms , packet1.press_abs , packet1.press_diff , packet1.temperature , packet1.temperature_press_diff ); mavlink_msg_scaled_pressure3_decode(&msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); @@ -7003,7 +7012,7 @@ static void mavlink_test_scaled_pressure3(uint8_t system_id, uint8_t component_i MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_scaled_pressure3_send(MAVLINK_COMM_1 , packet1.time_boot_ms , packet1.press_abs , packet1.press_diff , packet1.temperature ); + mavlink_msg_scaled_pressure3_send(MAVLINK_COMM_1 , packet1.time_boot_ms , packet1.press_abs , packet1.press_diff , packet1.temperature , packet1.temperature_press_diff ); mavlink_msg_scaled_pressure3_decode(last_msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); } @@ -7154,7 +7163,7 @@ static void mavlink_test_battery_status(uint8_t system_id, uint8_t component_id, uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; uint16_t i; mavlink_battery_status_t packet_in = { - 963497464,963497672,17651,{ 17755, 17756, 17757, 17758, 17759, 17760, 17761, 17762, 17763, 17764 },18795,101,168,235,46 + 963497464,963497672,17651,{ 17755, 17756, 17757, 17758, 17759, 17760, 17761, 17762, 17763, 17764 },18795,101,168,235,46,963499336,125,{ 19367, 19368, 19369, 19370 },216,963500064 }; mavlink_battery_status_t packet1, packet2; memset(&packet1, 0, sizeof(packet1)); @@ -7166,8 +7175,13 @@ static void mavlink_test_battery_status(uint8_t system_id, uint8_t component_id, packet1.battery_function = packet_in.battery_function; packet1.type = packet_in.type; packet1.battery_remaining = packet_in.battery_remaining; + packet1.time_remaining = packet_in.time_remaining; + packet1.charge_state = packet_in.charge_state; + packet1.mode = packet_in.mode; + packet1.fault_bitmask = packet_in.fault_bitmask; mav_array_memcpy(packet1.voltages, packet_in.voltages, sizeof(uint16_t)*10); + mav_array_memcpy(packet1.voltages_ext, packet_in.voltages_ext, sizeof(uint16_t)*4); #ifdef MAVLINK_STATUS_FLAG_OUT_MAVLINK1 if (status->flags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) { @@ -7181,12 +7195,12 @@ static void mavlink_test_battery_status(uint8_t system_id, uint8_t component_id, MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_battery_status_pack(system_id, component_id, &msg , packet1.id , packet1.battery_function , packet1.type , packet1.temperature , packet1.voltages , packet1.current_battery , packet1.current_consumed , packet1.energy_consumed , packet1.battery_remaining ); + mavlink_msg_battery_status_pack(system_id, component_id, &msg , packet1.id , packet1.battery_function , packet1.type , packet1.temperature , packet1.voltages , packet1.current_battery , packet1.current_consumed , packet1.energy_consumed , packet1.battery_remaining , packet1.time_remaining , packet1.charge_state , packet1.voltages_ext , packet1.mode , packet1.fault_bitmask ); mavlink_msg_battery_status_decode(&msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_battery_status_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.id , packet1.battery_function , packet1.type , packet1.temperature , packet1.voltages , packet1.current_battery , packet1.current_consumed , packet1.energy_consumed , packet1.battery_remaining ); + mavlink_msg_battery_status_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.id , packet1.battery_function , packet1.type , packet1.temperature , packet1.voltages , packet1.current_battery , packet1.current_consumed , packet1.energy_consumed , packet1.battery_remaining , packet1.time_remaining , packet1.charge_state , packet1.voltages_ext , packet1.mode , packet1.fault_bitmask ); mavlink_msg_battery_status_decode(&msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); @@ -7199,7 +7213,7 @@ static void mavlink_test_battery_status(uint8_t system_id, uint8_t component_id, MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_battery_status_send(MAVLINK_COMM_1 , packet1.id , packet1.battery_function , packet1.type , packet1.temperature , packet1.voltages , packet1.current_battery , packet1.current_consumed , packet1.energy_consumed , packet1.battery_remaining ); + mavlink_msg_battery_status_send(MAVLINK_COMM_1 , packet1.id , packet1.battery_function , packet1.type , packet1.temperature , packet1.voltages , packet1.current_battery , packet1.current_consumed , packet1.energy_consumed , packet1.battery_remaining , packet1.time_remaining , packet1.charge_state , packet1.voltages_ext , packet1.mode , packet1.fault_bitmask ); mavlink_msg_battery_status_decode(last_msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); } @@ -7216,7 +7230,7 @@ static void mavlink_test_autopilot_version(uint8_t system_id, uint8_t component_ uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; uint16_t i; mavlink_autopilot_version_t packet_in = { - 93372036854775807ULL,93372036854776311ULL,963498296,963498504,963498712,963498920,18899,19003,{ 113, 114, 115, 116, 117, 118, 119, 120 },{ 137, 138, 139, 140, 141, 142, 143, 144 },{ 161, 162, 163, 164, 165, 166, 167, 168 } + 93372036854775807ULL,93372036854776311ULL,963498296,963498504,963498712,963498920,18899,19003,{ 113, 114, 115, 116, 117, 118, 119, 120 },{ 137, 138, 139, 140, 141, 142, 143, 144 },{ 161, 162, 163, 164, 165, 166, 167, 168 },{ 185, 186, 187, 188, 189, 190, 191, 192, 193, 194, 195, 196, 197, 198, 199, 200, 201, 202 } }; mavlink_autopilot_version_t packet1, packet2; memset(&packet1, 0, sizeof(packet1)); @@ -7232,6 +7246,7 @@ static void mavlink_test_autopilot_version(uint8_t system_id, uint8_t component_ mav_array_memcpy(packet1.flight_custom_version, packet_in.flight_custom_version, sizeof(uint8_t)*8); mav_array_memcpy(packet1.middleware_custom_version, packet_in.middleware_custom_version, sizeof(uint8_t)*8); mav_array_memcpy(packet1.os_custom_version, packet_in.os_custom_version, sizeof(uint8_t)*8); + mav_array_memcpy(packet1.uid2, packet_in.uid2, sizeof(uint8_t)*18); #ifdef MAVLINK_STATUS_FLAG_OUT_MAVLINK1 if (status->flags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) { @@ -7245,12 +7260,12 @@ static void mavlink_test_autopilot_version(uint8_t system_id, uint8_t component_ MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_autopilot_version_pack(system_id, component_id, &msg , packet1.capabilities , packet1.flight_sw_version , packet1.middleware_sw_version , packet1.os_sw_version , packet1.board_version , packet1.flight_custom_version , packet1.middleware_custom_version , packet1.os_custom_version , packet1.vendor_id , packet1.product_id , packet1.uid ); + mavlink_msg_autopilot_version_pack(system_id, component_id, &msg , packet1.capabilities , packet1.flight_sw_version , packet1.middleware_sw_version , packet1.os_sw_version , packet1.board_version , packet1.flight_custom_version , packet1.middleware_custom_version , packet1.os_custom_version , packet1.vendor_id , packet1.product_id , packet1.uid , packet1.uid2 ); mavlink_msg_autopilot_version_decode(&msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_autopilot_version_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.capabilities , packet1.flight_sw_version , packet1.middleware_sw_version , packet1.os_sw_version , packet1.board_version , packet1.flight_custom_version , packet1.middleware_custom_version , packet1.os_custom_version , packet1.vendor_id , packet1.product_id , packet1.uid ); + mavlink_msg_autopilot_version_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.capabilities , packet1.flight_sw_version , packet1.middleware_sw_version , packet1.os_sw_version , packet1.board_version , packet1.flight_custom_version , packet1.middleware_custom_version , packet1.os_custom_version , packet1.vendor_id , packet1.product_id , packet1.uid , packet1.uid2 ); mavlink_msg_autopilot_version_decode(&msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); @@ -7263,7 +7278,7 @@ static void mavlink_test_autopilot_version(uint8_t system_id, uint8_t component_ MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_autopilot_version_send(MAVLINK_COMM_1 , packet1.capabilities , packet1.flight_sw_version , packet1.middleware_sw_version , packet1.os_sw_version , packet1.board_version , packet1.flight_custom_version , packet1.middleware_custom_version , packet1.os_custom_version , packet1.vendor_id , packet1.product_id , packet1.uid ); + mavlink_msg_autopilot_version_send(MAVLINK_COMM_1 , packet1.capabilities , packet1.flight_sw_version , packet1.middleware_sw_version , packet1.os_sw_version , packet1.board_version , packet1.flight_custom_version , packet1.middleware_custom_version , packet1.os_custom_version , packet1.vendor_id , packet1.product_id , packet1.uid , packet1.uid2 ); mavlink_msg_autopilot_version_decode(last_msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); } @@ -7280,7 +7295,7 @@ static void mavlink_test_landing_target(uint8_t system_id, uint8_t component_id, uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; uint16_t i; mavlink_landing_target_t packet_in = { - 93372036854775807ULL,73.0,101.0,129.0,157.0,185.0,89,156 + 93372036854775807ULL,73.0,101.0,129.0,157.0,185.0,89,156,227.0,255.0,283.0,{ 311.0, 312.0, 313.0, 314.0 },51,118 }; mavlink_landing_target_t packet1, packet2; memset(&packet1, 0, sizeof(packet1)); @@ -7292,7 +7307,13 @@ static void mavlink_test_landing_target(uint8_t system_id, uint8_t component_id, packet1.size_y = packet_in.size_y; packet1.target_num = packet_in.target_num; packet1.frame = packet_in.frame; + packet1.x = packet_in.x; + packet1.y = packet_in.y; + packet1.z = packet_in.z; + packet1.type = packet_in.type; + packet1.position_valid = packet_in.position_valid; + mav_array_memcpy(packet1.q, packet_in.q, sizeof(float)*4); #ifdef MAVLINK_STATUS_FLAG_OUT_MAVLINK1 if (status->flags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) { @@ -7306,12 +7327,12 @@ static void mavlink_test_landing_target(uint8_t system_id, uint8_t component_id, MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_landing_target_pack(system_id, component_id, &msg , packet1.time_usec , packet1.target_num , packet1.frame , packet1.angle_x , packet1.angle_y , packet1.distance , packet1.size_x , packet1.size_y ); + mavlink_msg_landing_target_pack(system_id, component_id, &msg , packet1.time_usec , packet1.target_num , packet1.frame , packet1.angle_x , packet1.angle_y , packet1.distance , packet1.size_x , packet1.size_y , packet1.x , packet1.y , packet1.z , packet1.q , packet1.type , packet1.position_valid ); mavlink_msg_landing_target_decode(&msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_landing_target_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.time_usec , packet1.target_num , packet1.frame , packet1.angle_x , packet1.angle_y , packet1.distance , packet1.size_x , packet1.size_y ); + mavlink_msg_landing_target_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.time_usec , packet1.target_num , packet1.frame , packet1.angle_x , packet1.angle_y , packet1.distance , packet1.size_x , packet1.size_y , packet1.x , packet1.y , packet1.z , packet1.q , packet1.type , packet1.position_valid ); mavlink_msg_landing_target_decode(&msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); @@ -7324,7 +7345,7 @@ static void mavlink_test_landing_target(uint8_t system_id, uint8_t component_id, MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_landing_target_send(MAVLINK_COMM_1 , packet1.time_usec , packet1.target_num , packet1.frame , packet1.angle_x , packet1.angle_y , packet1.distance , packet1.size_x , packet1.size_y ); + mavlink_msg_landing_target_send(MAVLINK_COMM_1 , packet1.time_usec , packet1.target_num , packet1.frame , packet1.angle_x , packet1.angle_y , packet1.distance , packet1.size_x , packet1.size_y , packet1.x , packet1.y , packet1.z , packet1.q , packet1.type , packet1.position_valid ); mavlink_msg_landing_target_decode(last_msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); } @@ -7341,7 +7362,7 @@ static void mavlink_test_fence_status(uint8_t system_id, uint8_t component_id, m uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; uint16_t i; mavlink_fence_status_t packet_in = { - 963497464,17443,151,218 + 963497464,17443,151,218,29 }; mavlink_fence_status_t packet1, packet2; memset(&packet1, 0, sizeof(packet1)); @@ -7349,6 +7370,7 @@ static void mavlink_test_fence_status(uint8_t system_id, uint8_t component_id, m packet1.breach_count = packet_in.breach_count; packet1.breach_status = packet_in.breach_status; packet1.breach_type = packet_in.breach_type; + packet1.breach_mitigation = packet_in.breach_mitigation; #ifdef MAVLINK_STATUS_FLAG_OUT_MAVLINK1 @@ -7363,12 +7385,12 @@ static void mavlink_test_fence_status(uint8_t system_id, uint8_t component_id, m MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_fence_status_pack(system_id, component_id, &msg , packet1.breach_status , packet1.breach_count , packet1.breach_type , packet1.breach_time ); + mavlink_msg_fence_status_pack(system_id, component_id, &msg , packet1.breach_status , packet1.breach_count , packet1.breach_type , packet1.breach_time , packet1.breach_mitigation ); mavlink_msg_fence_status_decode(&msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_fence_status_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.breach_status , packet1.breach_count , packet1.breach_type , packet1.breach_time ); + mavlink_msg_fence_status_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.breach_status , packet1.breach_count , packet1.breach_type , packet1.breach_time , packet1.breach_mitigation ); mavlink_msg_fence_status_decode(&msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); @@ -7381,11 +7403,152 @@ static void mavlink_test_fence_status(uint8_t system_id, uint8_t component_id, m MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); memset(&packet2, 0, sizeof(packet2)); - mavlink_msg_fence_status_send(MAVLINK_COMM_1 , packet1.breach_status , packet1.breach_count , packet1.breach_type , packet1.breach_time ); + mavlink_msg_fence_status_send(MAVLINK_COMM_1 , packet1.breach_status , packet1.breach_count , packet1.breach_type , packet1.breach_time , packet1.breach_mitigation ); mavlink_msg_fence_status_decode(last_msg, &packet2); MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); } +static void mavlink_test_mag_cal_report(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg) +{ +#ifdef MAVLINK_STATUS_FLAG_OUT_MAVLINK1 + mavlink_status_t *status = mavlink_get_channel_status(MAVLINK_COMM_0); + if ((status->flags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) && MAVLINK_MSG_ID_MAG_CAL_REPORT >= 256) { + return; + } +#endif + mavlink_message_t msg; + uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; + uint16_t i; + mavlink_mag_cal_report_t packet_in = { + 17.0,45.0,73.0,101.0,129.0,157.0,185.0,213.0,241.0,269.0,125,192,3,70,325.0,149,216,367.0 + }; + mavlink_mag_cal_report_t packet1, packet2; + memset(&packet1, 0, sizeof(packet1)); + packet1.fitness = packet_in.fitness; + packet1.ofs_x = packet_in.ofs_x; + packet1.ofs_y = packet_in.ofs_y; + packet1.ofs_z = packet_in.ofs_z; + packet1.diag_x = packet_in.diag_x; + packet1.diag_y = packet_in.diag_y; + packet1.diag_z = packet_in.diag_z; + packet1.offdiag_x = packet_in.offdiag_x; + packet1.offdiag_y = packet_in.offdiag_y; + packet1.offdiag_z = packet_in.offdiag_z; + packet1.compass_id = packet_in.compass_id; + packet1.cal_mask = packet_in.cal_mask; + packet1.cal_status = packet_in.cal_status; + packet1.autosaved = packet_in.autosaved; + packet1.orientation_confidence = packet_in.orientation_confidence; + packet1.old_orientation = packet_in.old_orientation; + packet1.new_orientation = packet_in.new_orientation; + packet1.scale_factor = packet_in.scale_factor; + + +#ifdef MAVLINK_STATUS_FLAG_OUT_MAVLINK1 + if (status->flags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) { + // cope with extensions + memset(MAVLINK_MSG_ID_MAG_CAL_REPORT_MIN_LEN + (char *)&packet1, 0, sizeof(packet1)-MAVLINK_MSG_ID_MAG_CAL_REPORT_MIN_LEN); + } +#endif + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_mag_cal_report_encode(system_id, component_id, &msg, &packet1); + mavlink_msg_mag_cal_report_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_mag_cal_report_pack(system_id, component_id, &msg , packet1.compass_id , packet1.cal_mask , packet1.cal_status , packet1.autosaved , packet1.fitness , packet1.ofs_x , packet1.ofs_y , packet1.ofs_z , packet1.diag_x , packet1.diag_y , packet1.diag_z , packet1.offdiag_x , packet1.offdiag_y , packet1.offdiag_z , packet1.orientation_confidence , packet1.old_orientation , packet1.new_orientation , packet1.scale_factor ); + mavlink_msg_mag_cal_report_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_mag_cal_report_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.compass_id , packet1.cal_mask , packet1.cal_status , packet1.autosaved , packet1.fitness , packet1.ofs_x , packet1.ofs_y , packet1.ofs_z , packet1.diag_x , packet1.diag_y , packet1.diag_z , packet1.offdiag_x , packet1.offdiag_y , packet1.offdiag_z , packet1.orientation_confidence , packet1.old_orientation , packet1.new_orientation , packet1.scale_factor ); + mavlink_msg_mag_cal_report_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_to_send_buffer(buffer, &msg); + for (i=0; iflags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) && MAVLINK_MSG_ID_EFI_STATUS >= 256) { + return; + } +#endif + mavlink_message_t msg; + uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; + uint16_t i; + mavlink_efi_status_t packet_in = { + 17.0,45.0,73.0,101.0,129.0,157.0,185.0,213.0,241.0,269.0,297.0,325.0,353.0,381.0,409.0,437.0,197 + }; + mavlink_efi_status_t packet1, packet2; + memset(&packet1, 0, sizeof(packet1)); + packet1.ecu_index = packet_in.ecu_index; + packet1.rpm = packet_in.rpm; + packet1.fuel_consumed = packet_in.fuel_consumed; + packet1.fuel_flow = packet_in.fuel_flow; + packet1.engine_load = packet_in.engine_load; + packet1.throttle_position = packet_in.throttle_position; + packet1.spark_dwell_time = packet_in.spark_dwell_time; + packet1.barometric_pressure = packet_in.barometric_pressure; + packet1.intake_manifold_pressure = packet_in.intake_manifold_pressure; + packet1.intake_manifold_temperature = packet_in.intake_manifold_temperature; + packet1.cylinder_head_temperature = packet_in.cylinder_head_temperature; + packet1.ignition_timing = packet_in.ignition_timing; + packet1.injection_time = packet_in.injection_time; + packet1.exhaust_gas_temperature = packet_in.exhaust_gas_temperature; + packet1.throttle_out = packet_in.throttle_out; + packet1.pt_compensation = packet_in.pt_compensation; + packet1.health = packet_in.health; + + +#ifdef MAVLINK_STATUS_FLAG_OUT_MAVLINK1 + if (status->flags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) { + // cope with extensions + memset(MAVLINK_MSG_ID_EFI_STATUS_MIN_LEN + (char *)&packet1, 0, sizeof(packet1)-MAVLINK_MSG_ID_EFI_STATUS_MIN_LEN); + } +#endif + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_efi_status_encode(system_id, component_id, &msg, &packet1); + mavlink_msg_efi_status_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_efi_status_pack(system_id, component_id, &msg , packet1.health , packet1.ecu_index , packet1.rpm , packet1.fuel_consumed , packet1.fuel_flow , packet1.engine_load , packet1.throttle_position , packet1.spark_dwell_time , packet1.barometric_pressure , packet1.intake_manifold_pressure , packet1.intake_manifold_temperature , packet1.cylinder_head_temperature , packet1.ignition_timing , packet1.injection_time , packet1.exhaust_gas_temperature , packet1.throttle_out , packet1.pt_compensation ); + mavlink_msg_efi_status_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_efi_status_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.health , packet1.ecu_index , packet1.rpm , packet1.fuel_consumed , packet1.fuel_flow , packet1.engine_load , packet1.throttle_position , packet1.spark_dwell_time , packet1.barometric_pressure , packet1.intake_manifold_pressure , packet1.intake_manifold_temperature , packet1.cylinder_head_temperature , packet1.ignition_timing , packet1.injection_time , packet1.exhaust_gas_temperature , packet1.throttle_out , packet1.pt_compensation ); + mavlink_msg_efi_status_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_to_send_buffer(buffer, &msg); + for (i=0; iflags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) && MAVLINK_MSG_ID_SETUP_SIGNING >= 256) { + return; + } +#endif + mavlink_message_t msg; + uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; + uint16_t i; + mavlink_setup_signing_t packet_in = { + 93372036854775807ULL,29,96,{ 163, 164, 165, 166, 167, 168, 169, 170, 171, 172, 173, 174, 175, 176, 177, 178, 179, 180, 181, 182, 183, 184, 185, 186, 187, 188, 189, 190, 191, 192, 193, 194 } + }; + mavlink_setup_signing_t packet1, packet2; + memset(&packet1, 0, sizeof(packet1)); + packet1.initial_timestamp = packet_in.initial_timestamp; + packet1.target_system = packet_in.target_system; + packet1.target_component = packet_in.target_component; + + mav_array_memcpy(packet1.secret_key, packet_in.secret_key, sizeof(uint8_t)*32); + +#ifdef MAVLINK_STATUS_FLAG_OUT_MAVLINK1 + if (status->flags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) { + // cope with extensions + memset(MAVLINK_MSG_ID_SETUP_SIGNING_MIN_LEN + (char *)&packet1, 0, sizeof(packet1)-MAVLINK_MSG_ID_SETUP_SIGNING_MIN_LEN); + } +#endif + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_setup_signing_encode(system_id, component_id, &msg, &packet1); + mavlink_msg_setup_signing_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_setup_signing_pack(system_id, component_id, &msg , packet1.target_system , packet1.target_component , packet1.secret_key , packet1.initial_timestamp ); + mavlink_msg_setup_signing_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_setup_signing_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.target_system , packet1.target_component , packet1.secret_key , packet1.initial_timestamp ); + mavlink_msg_setup_signing_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_to_send_buffer(buffer, &msg); + for (i=0; iflags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) && MAVLINK_MSG_ID_BUTTON_CHANGE >= 256) { + return; + } +#endif + mavlink_message_t msg; + uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; + uint16_t i; + mavlink_button_change_t packet_in = { + 963497464,963497672,29 + }; + mavlink_button_change_t packet1, packet2; + memset(&packet1, 0, sizeof(packet1)); + packet1.time_boot_ms = packet_in.time_boot_ms; + packet1.last_change_ms = packet_in.last_change_ms; + packet1.state = packet_in.state; + + +#ifdef MAVLINK_STATUS_FLAG_OUT_MAVLINK1 + if (status->flags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) { + // cope with extensions + memset(MAVLINK_MSG_ID_BUTTON_CHANGE_MIN_LEN + (char *)&packet1, 0, sizeof(packet1)-MAVLINK_MSG_ID_BUTTON_CHANGE_MIN_LEN); + } +#endif + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_button_change_encode(system_id, component_id, &msg, &packet1); + mavlink_msg_button_change_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_button_change_pack(system_id, component_id, &msg , packet1.time_boot_ms , packet1.last_change_ms , packet1.state ); + mavlink_msg_button_change_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_button_change_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.time_boot_ms , packet1.last_change_ms , packet1.state ); + mavlink_msg_button_change_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_to_send_buffer(buffer, &msg); + for (i=0; iflags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) && MAVLINK_MSG_ID_PLAY_TUNE >= 256) { + return; + } +#endif + mavlink_message_t msg; + uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; + uint16_t i; + mavlink_play_tune_t packet_in = { + 5,72,"CDEFGHIJKLMNOPQRSTUVWXYZABCDE","GHIJKLMNOPQRSTUVWXYZABCDEFGHIJKLMNOPQRSTUVWXYZABCDEFGHIJKLMNOPQRSTUVWXYZABCDEFGHIJKLMNOPQRSTUVWXYZABCDEFGHIJKLMNOPQRSTUVWXYZABCDEFGHIJKLMNOPQRSTUVWXYZABCDEFGHIJKLMNOPQRSTUVWXYZABCDEFGHIJKLMNOPQRSTUVW" + }; + mavlink_play_tune_t packet1, packet2; + memset(&packet1, 0, sizeof(packet1)); + packet1.target_system = packet_in.target_system; + packet1.target_component = packet_in.target_component; + + mav_array_memcpy(packet1.tune, packet_in.tune, sizeof(char)*30); + mav_array_memcpy(packet1.tune2, packet_in.tune2, sizeof(char)*200); + +#ifdef MAVLINK_STATUS_FLAG_OUT_MAVLINK1 + if (status->flags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) { + // cope with extensions + memset(MAVLINK_MSG_ID_PLAY_TUNE_MIN_LEN + (char *)&packet1, 0, sizeof(packet1)-MAVLINK_MSG_ID_PLAY_TUNE_MIN_LEN); + } +#endif + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_play_tune_encode(system_id, component_id, &msg, &packet1); + mavlink_msg_play_tune_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_play_tune_pack(system_id, component_id, &msg , packet1.target_system , packet1.target_component , packet1.tune , packet1.tune2 ); + mavlink_msg_play_tune_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_play_tune_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.target_system , packet1.target_component , packet1.tune , packet1.tune2 ); + mavlink_msg_play_tune_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_to_send_buffer(buffer, &msg); + for (i=0; iflags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) && MAVLINK_MSG_ID_CAMERA_INFORMATION >= 256) { + return; + } +#endif + mavlink_message_t msg; + uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; + uint16_t i; + mavlink_camera_information_t packet_in = { + 963497464,963497672,73.0,101.0,129.0,963498504,18483,18587,18691,{ 223, 224, 225, 226, 227, 228, 229, 230, 231, 232, 233, 234, 235, 236, 237, 238, 239, 240, 241, 242, 243, 244, 245, 246, 247, 248, 249, 250, 251, 252, 253, 254 },{ 63, 64, 65, 66, 67, 68, 69, 70, 71, 72, 73, 74, 75, 76, 77, 78, 79, 80, 81, 82, 83, 84, 85, 86, 87, 88, 89, 90, 91, 92, 93, 94 },159,"RSTUVWXYZABCDEFGHIJKLMNOPQRSTUVWXYZABCDEFGHIJKLMNOPQRSTUVWXYZABCDEFGHIJKLMNOPQRSTUVWXYZABCDEFGHIJKLMNOPQRSTUVWXYZABCDEFGHIJKLMNOPQRSTUVWXYZ" + }; + mavlink_camera_information_t packet1, packet2; + memset(&packet1, 0, sizeof(packet1)); + packet1.time_boot_ms = packet_in.time_boot_ms; + packet1.firmware_version = packet_in.firmware_version; + packet1.focal_length = packet_in.focal_length; + packet1.sensor_size_h = packet_in.sensor_size_h; + packet1.sensor_size_v = packet_in.sensor_size_v; + packet1.flags = packet_in.flags; + packet1.resolution_h = packet_in.resolution_h; + packet1.resolution_v = packet_in.resolution_v; + packet1.cam_definition_version = packet_in.cam_definition_version; + packet1.lens_id = packet_in.lens_id; + + mav_array_memcpy(packet1.vendor_name, packet_in.vendor_name, sizeof(uint8_t)*32); + mav_array_memcpy(packet1.model_name, packet_in.model_name, sizeof(uint8_t)*32); + mav_array_memcpy(packet1.cam_definition_uri, packet_in.cam_definition_uri, sizeof(char)*140); + +#ifdef MAVLINK_STATUS_FLAG_OUT_MAVLINK1 + if (status->flags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) { + // cope with extensions + memset(MAVLINK_MSG_ID_CAMERA_INFORMATION_MIN_LEN + (char *)&packet1, 0, sizeof(packet1)-MAVLINK_MSG_ID_CAMERA_INFORMATION_MIN_LEN); + } +#endif + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_camera_information_encode(system_id, component_id, &msg, &packet1); + mavlink_msg_camera_information_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_camera_information_pack(system_id, component_id, &msg , packet1.time_boot_ms , packet1.vendor_name , packet1.model_name , packet1.firmware_version , packet1.focal_length , packet1.sensor_size_h , packet1.sensor_size_v , packet1.resolution_h , packet1.resolution_v , packet1.lens_id , packet1.flags , packet1.cam_definition_version , packet1.cam_definition_uri ); + mavlink_msg_camera_information_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_camera_information_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.time_boot_ms , packet1.vendor_name , packet1.model_name , packet1.firmware_version , packet1.focal_length , packet1.sensor_size_h , packet1.sensor_size_v , packet1.resolution_h , packet1.resolution_v , packet1.lens_id , packet1.flags , packet1.cam_definition_version , packet1.cam_definition_uri ); + mavlink_msg_camera_information_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_to_send_buffer(buffer, &msg); + for (i=0; iflags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) && MAVLINK_MSG_ID_CAMERA_SETTINGS >= 256) { + return; + } +#endif + mavlink_message_t msg; + uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; + uint16_t i; + mavlink_camera_settings_t packet_in = { + 963497464,17,52.0,80.0 + }; + mavlink_camera_settings_t packet1, packet2; + memset(&packet1, 0, sizeof(packet1)); + packet1.time_boot_ms = packet_in.time_boot_ms; + packet1.mode_id = packet_in.mode_id; + packet1.zoomLevel = packet_in.zoomLevel; + packet1.focusLevel = packet_in.focusLevel; + + +#ifdef MAVLINK_STATUS_FLAG_OUT_MAVLINK1 + if (status->flags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) { + // cope with extensions + memset(MAVLINK_MSG_ID_CAMERA_SETTINGS_MIN_LEN + (char *)&packet1, 0, sizeof(packet1)-MAVLINK_MSG_ID_CAMERA_SETTINGS_MIN_LEN); + } +#endif + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_camera_settings_encode(system_id, component_id, &msg, &packet1); + mavlink_msg_camera_settings_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_camera_settings_pack(system_id, component_id, &msg , packet1.time_boot_ms , packet1.mode_id , packet1.zoomLevel , packet1.focusLevel ); + mavlink_msg_camera_settings_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_camera_settings_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.time_boot_ms , packet1.mode_id , packet1.zoomLevel , packet1.focusLevel ); + mavlink_msg_camera_settings_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_to_send_buffer(buffer, &msg); + for (i=0; iflags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) && MAVLINK_MSG_ID_STORAGE_INFORMATION >= 256) { + return; + } +#endif + mavlink_message_t msg; + uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; + uint16_t i; + mavlink_storage_information_t packet_in = { + 963497464,45.0,73.0,101.0,129.0,157.0,77,144,211,22,"CDEFGHIJKLMNOPQRSTUVWXYZABCDEFG" + }; + mavlink_storage_information_t packet1, packet2; + memset(&packet1, 0, sizeof(packet1)); + packet1.time_boot_ms = packet_in.time_boot_ms; + packet1.total_capacity = packet_in.total_capacity; + packet1.used_capacity = packet_in.used_capacity; + packet1.available_capacity = packet_in.available_capacity; + packet1.read_speed = packet_in.read_speed; + packet1.write_speed = packet_in.write_speed; + packet1.storage_id = packet_in.storage_id; + packet1.storage_count = packet_in.storage_count; + packet1.status = packet_in.status; + packet1.type = packet_in.type; + + mav_array_memcpy(packet1.name, packet_in.name, sizeof(char)*32); + +#ifdef MAVLINK_STATUS_FLAG_OUT_MAVLINK1 + if (status->flags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) { + // cope with extensions + memset(MAVLINK_MSG_ID_STORAGE_INFORMATION_MIN_LEN + (char *)&packet1, 0, sizeof(packet1)-MAVLINK_MSG_ID_STORAGE_INFORMATION_MIN_LEN); + } +#endif + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_storage_information_encode(system_id, component_id, &msg, &packet1); + mavlink_msg_storage_information_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_storage_information_pack(system_id, component_id, &msg , packet1.time_boot_ms , packet1.storage_id , packet1.storage_count , packet1.status , packet1.total_capacity , packet1.used_capacity , packet1.available_capacity , packet1.read_speed , packet1.write_speed , packet1.type , packet1.name ); + mavlink_msg_storage_information_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_storage_information_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.time_boot_ms , packet1.storage_id , packet1.storage_count , packet1.status , packet1.total_capacity , packet1.used_capacity , packet1.available_capacity , packet1.read_speed , packet1.write_speed , packet1.type , packet1.name ); + mavlink_msg_storage_information_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_to_send_buffer(buffer, &msg); + for (i=0; iflags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) && MAVLINK_MSG_ID_CAMERA_CAPTURE_STATUS >= 256) { + return; + } +#endif + mavlink_message_t msg; + uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; + uint16_t i; + mavlink_camera_capture_status_t packet_in = { + 963497464,45.0,963497880,101.0,53,120,963498400 + }; + mavlink_camera_capture_status_t packet1, packet2; + memset(&packet1, 0, sizeof(packet1)); + packet1.time_boot_ms = packet_in.time_boot_ms; + packet1.image_interval = packet_in.image_interval; + packet1.recording_time_ms = packet_in.recording_time_ms; + packet1.available_capacity = packet_in.available_capacity; + packet1.image_status = packet_in.image_status; + packet1.video_status = packet_in.video_status; + packet1.image_count = packet_in.image_count; + + +#ifdef MAVLINK_STATUS_FLAG_OUT_MAVLINK1 + if (status->flags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) { + // cope with extensions + memset(MAVLINK_MSG_ID_CAMERA_CAPTURE_STATUS_MIN_LEN + (char *)&packet1, 0, sizeof(packet1)-MAVLINK_MSG_ID_CAMERA_CAPTURE_STATUS_MIN_LEN); + } +#endif + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_camera_capture_status_encode(system_id, component_id, &msg, &packet1); + mavlink_msg_camera_capture_status_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_camera_capture_status_pack(system_id, component_id, &msg , packet1.time_boot_ms , packet1.image_status , packet1.video_status , packet1.image_interval , packet1.recording_time_ms , packet1.available_capacity , packet1.image_count ); + mavlink_msg_camera_capture_status_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_camera_capture_status_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.time_boot_ms , packet1.image_status , packet1.video_status , packet1.image_interval , packet1.recording_time_ms , packet1.available_capacity , packet1.image_count ); + mavlink_msg_camera_capture_status_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_to_send_buffer(buffer, &msg); + for (i=0; iflags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) && MAVLINK_MSG_ID_CAMERA_IMAGE_CAPTURED >= 256) { + return; + } +#endif + mavlink_message_t msg; + uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; + uint16_t i; + mavlink_camera_image_captured_t packet_in = { + 93372036854775807ULL,963497880,963498088,963498296,963498504,963498712,{ 213.0, 214.0, 215.0, 216.0 },963499752,149,216,"YZABCDEFGHIJKLMNOPQRSTUVWXYZABCDEFGHIJKLMNOPQRSTUVWXYZABCDEFGHIJKLMNOPQRSTUVWXYZABCDEFGHIJKLMNOPQRSTUVWXYZABCDEFGHIJKLMNOPQRSTUVWXYZABCDEFGHIJKLMNOPQRSTUVWXYZABCDEFGHIJKLMNOPQRSTUVWXYZABCDEFGHIJKLMNOPQRST" + }; + mavlink_camera_image_captured_t packet1, packet2; + memset(&packet1, 0, sizeof(packet1)); + packet1.time_utc = packet_in.time_utc; + packet1.time_boot_ms = packet_in.time_boot_ms; + packet1.lat = packet_in.lat; + packet1.lon = packet_in.lon; + packet1.alt = packet_in.alt; + packet1.relative_alt = packet_in.relative_alt; + packet1.image_index = packet_in.image_index; + packet1.camera_id = packet_in.camera_id; + packet1.capture_result = packet_in.capture_result; + + mav_array_memcpy(packet1.q, packet_in.q, sizeof(float)*4); + mav_array_memcpy(packet1.file_url, packet_in.file_url, sizeof(char)*205); + +#ifdef MAVLINK_STATUS_FLAG_OUT_MAVLINK1 + if (status->flags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) { + // cope with extensions + memset(MAVLINK_MSG_ID_CAMERA_IMAGE_CAPTURED_MIN_LEN + (char *)&packet1, 0, sizeof(packet1)-MAVLINK_MSG_ID_CAMERA_IMAGE_CAPTURED_MIN_LEN); + } +#endif + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_camera_image_captured_encode(system_id, component_id, &msg, &packet1); + mavlink_msg_camera_image_captured_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_camera_image_captured_pack(system_id, component_id, &msg , packet1.time_boot_ms , packet1.time_utc , packet1.camera_id , packet1.lat , packet1.lon , packet1.alt , packet1.relative_alt , packet1.q , packet1.image_index , packet1.capture_result , packet1.file_url ); + mavlink_msg_camera_image_captured_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_camera_image_captured_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.time_boot_ms , packet1.time_utc , packet1.camera_id , packet1.lat , packet1.lon , packet1.alt , packet1.relative_alt , packet1.q , packet1.image_index , packet1.capture_result , packet1.file_url ); + mavlink_msg_camera_image_captured_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_to_send_buffer(buffer, &msg); + for (i=0; iflags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) && MAVLINK_MSG_ID_FLIGHT_INFORMATION >= 256) { + return; + } +#endif + mavlink_message_t msg; + uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; + uint16_t i; + mavlink_flight_information_t packet_in = { + 93372036854775807ULL,93372036854776311ULL,93372036854776815ULL,963498712 + }; + mavlink_flight_information_t packet1, packet2; + memset(&packet1, 0, sizeof(packet1)); + packet1.arming_time_utc = packet_in.arming_time_utc; + packet1.takeoff_time_utc = packet_in.takeoff_time_utc; + packet1.flight_uuid = packet_in.flight_uuid; + packet1.time_boot_ms = packet_in.time_boot_ms; + + +#ifdef MAVLINK_STATUS_FLAG_OUT_MAVLINK1 + if (status->flags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) { + // cope with extensions + memset(MAVLINK_MSG_ID_FLIGHT_INFORMATION_MIN_LEN + (char *)&packet1, 0, sizeof(packet1)-MAVLINK_MSG_ID_FLIGHT_INFORMATION_MIN_LEN); + } +#endif + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_flight_information_encode(system_id, component_id, &msg, &packet1); + mavlink_msg_flight_information_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_flight_information_pack(system_id, component_id, &msg , packet1.time_boot_ms , packet1.arming_time_utc , packet1.takeoff_time_utc , packet1.flight_uuid ); + mavlink_msg_flight_information_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_flight_information_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.time_boot_ms , packet1.arming_time_utc , packet1.takeoff_time_utc , packet1.flight_uuid ); + mavlink_msg_flight_information_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_to_send_buffer(buffer, &msg); + for (i=0; iflags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) && MAVLINK_MSG_ID_MOUNT_ORIENTATION >= 256) { + return; + } +#endif + mavlink_message_t msg; + uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; + uint16_t i; + mavlink_mount_orientation_t packet_in = { + 963497464,45.0,73.0,101.0,129.0 + }; + mavlink_mount_orientation_t packet1, packet2; + memset(&packet1, 0, sizeof(packet1)); + packet1.time_boot_ms = packet_in.time_boot_ms; + packet1.roll = packet_in.roll; + packet1.pitch = packet_in.pitch; + packet1.yaw = packet_in.yaw; + packet1.yaw_absolute = packet_in.yaw_absolute; + + +#ifdef MAVLINK_STATUS_FLAG_OUT_MAVLINK1 + if (status->flags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) { + // cope with extensions + memset(MAVLINK_MSG_ID_MOUNT_ORIENTATION_MIN_LEN + (char *)&packet1, 0, sizeof(packet1)-MAVLINK_MSG_ID_MOUNT_ORIENTATION_MIN_LEN); + } +#endif + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_mount_orientation_encode(system_id, component_id, &msg, &packet1); + mavlink_msg_mount_orientation_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_mount_orientation_pack(system_id, component_id, &msg , packet1.time_boot_ms , packet1.roll , packet1.pitch , packet1.yaw , packet1.yaw_absolute ); + mavlink_msg_mount_orientation_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_mount_orientation_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.time_boot_ms , packet1.roll , packet1.pitch , packet1.yaw , packet1.yaw_absolute ); + mavlink_msg_mount_orientation_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_to_send_buffer(buffer, &msg); + for (i=0; iflags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) && MAVLINK_MSG_ID_LOGGING_DATA >= 256) { + return; + } +#endif + mavlink_message_t msg; + uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; + uint16_t i; + mavlink_logging_data_t packet_in = { + 17235,139,206,17,84,{ 151, 152, 153, 154, 155, 156, 157, 158, 159, 160, 161, 162, 163, 164, 165, 166, 167, 168, 169, 170, 171, 172, 173, 174, 175, 176, 177, 178, 179, 180, 181, 182, 183, 184, 185, 186, 187, 188, 189, 190, 191, 192, 193, 194, 195, 196, 197, 198, 199, 200, 201, 202, 203, 204, 205, 206, 207, 208, 209, 210, 211, 212, 213, 214, 215, 216, 217, 218, 219, 220, 221, 222, 223, 224, 225, 226, 227, 228, 229, 230, 231, 232, 233, 234, 235, 236, 237, 238, 239, 240, 241, 242, 243, 244, 245, 246, 247, 248, 249, 250, 251, 252, 253, 254, 255, 0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16, 17, 18, 19, 20, 21, 22, 23, 24, 25, 26, 27, 28, 29, 30, 31, 32, 33, 34, 35, 36, 37, 38, 39, 40, 41, 42, 43, 44, 45, 46, 47, 48, 49, 50, 51, 52, 53, 54, 55, 56, 57, 58, 59, 60, 61, 62, 63, 64, 65, 66, 67, 68, 69, 70, 71, 72, 73, 74, 75, 76, 77, 78, 79, 80, 81, 82, 83, 84, 85, 86, 87, 88, 89, 90, 91, 92, 93, 94, 95, 96, 97, 98, 99, 100, 101, 102, 103, 104, 105, 106, 107, 108, 109, 110, 111, 112, 113, 114, 115, 116, 117, 118, 119, 120, 121, 122, 123, 124, 125, 126, 127, 128, 129, 130, 131, 132, 133, 134, 135, 136, 137, 138, 139, 140, 141, 142, 143 } + }; + mavlink_logging_data_t packet1, packet2; + memset(&packet1, 0, sizeof(packet1)); + packet1.sequence = packet_in.sequence; + packet1.target_system = packet_in.target_system; + packet1.target_component = packet_in.target_component; + packet1.length = packet_in.length; + packet1.first_message_offset = packet_in.first_message_offset; + + mav_array_memcpy(packet1.data, packet_in.data, sizeof(uint8_t)*249); + +#ifdef MAVLINK_STATUS_FLAG_OUT_MAVLINK1 + if (status->flags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) { + // cope with extensions + memset(MAVLINK_MSG_ID_LOGGING_DATA_MIN_LEN + (char *)&packet1, 0, sizeof(packet1)-MAVLINK_MSG_ID_LOGGING_DATA_MIN_LEN); + } +#endif + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_logging_data_encode(system_id, component_id, &msg, &packet1); + mavlink_msg_logging_data_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_logging_data_pack(system_id, component_id, &msg , packet1.target_system , packet1.target_component , packet1.sequence , packet1.length , packet1.first_message_offset , packet1.data ); + mavlink_msg_logging_data_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_logging_data_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.target_system , packet1.target_component , packet1.sequence , packet1.length , packet1.first_message_offset , packet1.data ); + mavlink_msg_logging_data_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_to_send_buffer(buffer, &msg); + for (i=0; iflags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) && MAVLINK_MSG_ID_LOGGING_DATA_ACKED >= 256) { + return; + } +#endif + mavlink_message_t msg; + uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; + uint16_t i; + mavlink_logging_data_acked_t packet_in = { + 17235,139,206,17,84,{ 151, 152, 153, 154, 155, 156, 157, 158, 159, 160, 161, 162, 163, 164, 165, 166, 167, 168, 169, 170, 171, 172, 173, 174, 175, 176, 177, 178, 179, 180, 181, 182, 183, 184, 185, 186, 187, 188, 189, 190, 191, 192, 193, 194, 195, 196, 197, 198, 199, 200, 201, 202, 203, 204, 205, 206, 207, 208, 209, 210, 211, 212, 213, 214, 215, 216, 217, 218, 219, 220, 221, 222, 223, 224, 225, 226, 227, 228, 229, 230, 231, 232, 233, 234, 235, 236, 237, 238, 239, 240, 241, 242, 243, 244, 245, 246, 247, 248, 249, 250, 251, 252, 253, 254, 255, 0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16, 17, 18, 19, 20, 21, 22, 23, 24, 25, 26, 27, 28, 29, 30, 31, 32, 33, 34, 35, 36, 37, 38, 39, 40, 41, 42, 43, 44, 45, 46, 47, 48, 49, 50, 51, 52, 53, 54, 55, 56, 57, 58, 59, 60, 61, 62, 63, 64, 65, 66, 67, 68, 69, 70, 71, 72, 73, 74, 75, 76, 77, 78, 79, 80, 81, 82, 83, 84, 85, 86, 87, 88, 89, 90, 91, 92, 93, 94, 95, 96, 97, 98, 99, 100, 101, 102, 103, 104, 105, 106, 107, 108, 109, 110, 111, 112, 113, 114, 115, 116, 117, 118, 119, 120, 121, 122, 123, 124, 125, 126, 127, 128, 129, 130, 131, 132, 133, 134, 135, 136, 137, 138, 139, 140, 141, 142, 143 } + }; + mavlink_logging_data_acked_t packet1, packet2; + memset(&packet1, 0, sizeof(packet1)); + packet1.sequence = packet_in.sequence; + packet1.target_system = packet_in.target_system; + packet1.target_component = packet_in.target_component; + packet1.length = packet_in.length; + packet1.first_message_offset = packet_in.first_message_offset; + + mav_array_memcpy(packet1.data, packet_in.data, sizeof(uint8_t)*249); + +#ifdef MAVLINK_STATUS_FLAG_OUT_MAVLINK1 + if (status->flags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) { + // cope with extensions + memset(MAVLINK_MSG_ID_LOGGING_DATA_ACKED_MIN_LEN + (char *)&packet1, 0, sizeof(packet1)-MAVLINK_MSG_ID_LOGGING_DATA_ACKED_MIN_LEN); + } +#endif + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_logging_data_acked_encode(system_id, component_id, &msg, &packet1); + mavlink_msg_logging_data_acked_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_logging_data_acked_pack(system_id, component_id, &msg , packet1.target_system , packet1.target_component , packet1.sequence , packet1.length , packet1.first_message_offset , packet1.data ); + mavlink_msg_logging_data_acked_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_logging_data_acked_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.target_system , packet1.target_component , packet1.sequence , packet1.length , packet1.first_message_offset , packet1.data ); + mavlink_msg_logging_data_acked_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_to_send_buffer(buffer, &msg); + for (i=0; iflags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) && MAVLINK_MSG_ID_LOGGING_ACK >= 256) { + return; + } +#endif + mavlink_message_t msg; + uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; + uint16_t i; + mavlink_logging_ack_t packet_in = { + 17235,139,206 + }; + mavlink_logging_ack_t packet1, packet2; + memset(&packet1, 0, sizeof(packet1)); + packet1.sequence = packet_in.sequence; + packet1.target_system = packet_in.target_system; + packet1.target_component = packet_in.target_component; + + +#ifdef MAVLINK_STATUS_FLAG_OUT_MAVLINK1 + if (status->flags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) { + // cope with extensions + memset(MAVLINK_MSG_ID_LOGGING_ACK_MIN_LEN + (char *)&packet1, 0, sizeof(packet1)-MAVLINK_MSG_ID_LOGGING_ACK_MIN_LEN); + } +#endif + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_logging_ack_encode(system_id, component_id, &msg, &packet1); + mavlink_msg_logging_ack_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_logging_ack_pack(system_id, component_id, &msg , packet1.target_system , packet1.target_component , packet1.sequence ); + mavlink_msg_logging_ack_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_logging_ack_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.target_system , packet1.target_component , packet1.sequence ); + mavlink_msg_logging_ack_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_to_send_buffer(buffer, &msg); + for (i=0; iflags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) && MAVLINK_MSG_ID_VIDEO_STREAM_INFORMATION >= 256) { + return; + } +#endif + mavlink_message_t msg; + uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; + uint16_t i; + mavlink_video_stream_information_t packet_in = { + 17.0,963497672,17651,17755,17859,17963,18067,187,254,65,"VWXYZABCDEFGHIJKLMNOPQRSTUVWXYZ","BCDEFGHIJKLMNOPQRSTUVWXYZABCDEFGHIJKLMNOPQRSTUVWXYZABCDEFGHIJKLMNOPQRSTUVWXYZABCDEFGHIJKLMNOPQRSTUVWXYZABCDEFGHIJKLMNOPQRSTUVWXYZABCDEFGHIJKLMNOPQRSTUVWXYZABCD" + }; + mavlink_video_stream_information_t packet1, packet2; + memset(&packet1, 0, sizeof(packet1)); + packet1.framerate = packet_in.framerate; + packet1.bitrate = packet_in.bitrate; + packet1.flags = packet_in.flags; + packet1.resolution_h = packet_in.resolution_h; + packet1.resolution_v = packet_in.resolution_v; + packet1.rotation = packet_in.rotation; + packet1.hfov = packet_in.hfov; + packet1.stream_id = packet_in.stream_id; + packet1.count = packet_in.count; + packet1.type = packet_in.type; + + mav_array_memcpy(packet1.name, packet_in.name, sizeof(char)*32); + mav_array_memcpy(packet1.uri, packet_in.uri, sizeof(char)*160); + +#ifdef MAVLINK_STATUS_FLAG_OUT_MAVLINK1 + if (status->flags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) { + // cope with extensions + memset(MAVLINK_MSG_ID_VIDEO_STREAM_INFORMATION_MIN_LEN + (char *)&packet1, 0, sizeof(packet1)-MAVLINK_MSG_ID_VIDEO_STREAM_INFORMATION_MIN_LEN); + } +#endif + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_video_stream_information_encode(system_id, component_id, &msg, &packet1); + mavlink_msg_video_stream_information_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_video_stream_information_pack(system_id, component_id, &msg , packet1.stream_id , packet1.count , packet1.type , packet1.flags , packet1.framerate , packet1.resolution_h , packet1.resolution_v , packet1.bitrate , packet1.rotation , packet1.hfov , packet1.name , packet1.uri ); + mavlink_msg_video_stream_information_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_video_stream_information_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.stream_id , packet1.count , packet1.type , packet1.flags , packet1.framerate , packet1.resolution_h , packet1.resolution_v , packet1.bitrate , packet1.rotation , packet1.hfov , packet1.name , packet1.uri ); + mavlink_msg_video_stream_information_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_to_send_buffer(buffer, &msg); + for (i=0; iflags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) && MAVLINK_MSG_ID_VIDEO_STREAM_STATUS >= 256) { + return; + } +#endif + mavlink_message_t msg; + uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; + uint16_t i; + mavlink_video_stream_status_t packet_in = { + 17.0,963497672,17651,17755,17859,17963,18067,187 + }; + mavlink_video_stream_status_t packet1, packet2; + memset(&packet1, 0, sizeof(packet1)); + packet1.framerate = packet_in.framerate; + packet1.bitrate = packet_in.bitrate; + packet1.flags = packet_in.flags; + packet1.resolution_h = packet_in.resolution_h; + packet1.resolution_v = packet_in.resolution_v; + packet1.rotation = packet_in.rotation; + packet1.hfov = packet_in.hfov; + packet1.stream_id = packet_in.stream_id; + + +#ifdef MAVLINK_STATUS_FLAG_OUT_MAVLINK1 + if (status->flags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) { + // cope with extensions + memset(MAVLINK_MSG_ID_VIDEO_STREAM_STATUS_MIN_LEN + (char *)&packet1, 0, sizeof(packet1)-MAVLINK_MSG_ID_VIDEO_STREAM_STATUS_MIN_LEN); + } +#endif + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_video_stream_status_encode(system_id, component_id, &msg, &packet1); + mavlink_msg_video_stream_status_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_video_stream_status_pack(system_id, component_id, &msg , packet1.stream_id , packet1.flags , packet1.framerate , packet1.resolution_h , packet1.resolution_v , packet1.bitrate , packet1.rotation , packet1.hfov ); + mavlink_msg_video_stream_status_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_video_stream_status_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.stream_id , packet1.flags , packet1.framerate , packet1.resolution_h , packet1.resolution_v , packet1.bitrate , packet1.rotation , packet1.hfov ); + mavlink_msg_video_stream_status_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_to_send_buffer(buffer, &msg); + for (i=0; iflags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) && MAVLINK_MSG_ID_CAMERA_FOV_STATUS >= 256) { + return; + } +#endif + mavlink_message_t msg; + uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; + uint16_t i; + mavlink_camera_fov_status_t packet_in = { + 963497464,963497672,963497880,963498088,963498296,963498504,963498712,{ 213.0, 214.0, 215.0, 216.0 },325.0,353.0 + }; + mavlink_camera_fov_status_t packet1, packet2; + memset(&packet1, 0, sizeof(packet1)); + packet1.time_boot_ms = packet_in.time_boot_ms; + packet1.lat_camera = packet_in.lat_camera; + packet1.lon_camera = packet_in.lon_camera; + packet1.alt_camera = packet_in.alt_camera; + packet1.lat_image = packet_in.lat_image; + packet1.lon_image = packet_in.lon_image; + packet1.alt_image = packet_in.alt_image; + packet1.hfov = packet_in.hfov; + packet1.vfov = packet_in.vfov; + + mav_array_memcpy(packet1.q, packet_in.q, sizeof(float)*4); + +#ifdef MAVLINK_STATUS_FLAG_OUT_MAVLINK1 + if (status->flags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) { + // cope with extensions + memset(MAVLINK_MSG_ID_CAMERA_FOV_STATUS_MIN_LEN + (char *)&packet1, 0, sizeof(packet1)-MAVLINK_MSG_ID_CAMERA_FOV_STATUS_MIN_LEN); + } +#endif + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_camera_fov_status_encode(system_id, component_id, &msg, &packet1); + mavlink_msg_camera_fov_status_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_camera_fov_status_pack(system_id, component_id, &msg , packet1.time_boot_ms , packet1.lat_camera , packet1.lon_camera , packet1.alt_camera , packet1.lat_image , packet1.lon_image , packet1.alt_image , packet1.q , packet1.hfov , packet1.vfov ); + mavlink_msg_camera_fov_status_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_camera_fov_status_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.time_boot_ms , packet1.lat_camera , packet1.lon_camera , packet1.alt_camera , packet1.lat_image , packet1.lon_image , packet1.alt_image , packet1.q , packet1.hfov , packet1.vfov ); + mavlink_msg_camera_fov_status_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_to_send_buffer(buffer, &msg); + for (i=0; iflags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) && MAVLINK_MSG_ID_CAMERA_TRACKING_IMAGE_STATUS >= 256) { + return; + } +#endif + mavlink_message_t msg; + uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; + uint16_t i; + mavlink_camera_tracking_image_status_t packet_in = { + 17.0,45.0,73.0,101.0,129.0,157.0,185.0,89,156,223 + }; + mavlink_camera_tracking_image_status_t packet1, packet2; + memset(&packet1, 0, sizeof(packet1)); + packet1.point_x = packet_in.point_x; + packet1.point_y = packet_in.point_y; + packet1.radius = packet_in.radius; + packet1.rec_top_x = packet_in.rec_top_x; + packet1.rec_top_y = packet_in.rec_top_y; + packet1.rec_bottom_x = packet_in.rec_bottom_x; + packet1.rec_bottom_y = packet_in.rec_bottom_y; + packet1.tracking_status = packet_in.tracking_status; + packet1.tracking_mode = packet_in.tracking_mode; + packet1.target_data = packet_in.target_data; + + +#ifdef MAVLINK_STATUS_FLAG_OUT_MAVLINK1 + if (status->flags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) { + // cope with extensions + memset(MAVLINK_MSG_ID_CAMERA_TRACKING_IMAGE_STATUS_MIN_LEN + (char *)&packet1, 0, sizeof(packet1)-MAVLINK_MSG_ID_CAMERA_TRACKING_IMAGE_STATUS_MIN_LEN); + } +#endif + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_camera_tracking_image_status_encode(system_id, component_id, &msg, &packet1); + mavlink_msg_camera_tracking_image_status_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_camera_tracking_image_status_pack(system_id, component_id, &msg , packet1.tracking_status , packet1.tracking_mode , packet1.target_data , packet1.point_x , packet1.point_y , packet1.radius , packet1.rec_top_x , packet1.rec_top_y , packet1.rec_bottom_x , packet1.rec_bottom_y ); + mavlink_msg_camera_tracking_image_status_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_camera_tracking_image_status_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.tracking_status , packet1.tracking_mode , packet1.target_data , packet1.point_x , packet1.point_y , packet1.radius , packet1.rec_top_x , packet1.rec_top_y , packet1.rec_bottom_x , packet1.rec_bottom_y ); + mavlink_msg_camera_tracking_image_status_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_to_send_buffer(buffer, &msg); + for (i=0; iflags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) && MAVLINK_MSG_ID_CAMERA_TRACKING_GEO_STATUS >= 256) { + return; + } +#endif + mavlink_message_t msg; + uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; + uint16_t i; + mavlink_camera_tracking_geo_status_t packet_in = { + 963497464,963497672,73.0,101.0,129.0,157.0,185.0,213.0,241.0,269.0,297.0,325.0,149 + }; + mavlink_camera_tracking_geo_status_t packet1, packet2; + memset(&packet1, 0, sizeof(packet1)); + packet1.lat = packet_in.lat; + packet1.lon = packet_in.lon; + packet1.alt = packet_in.alt; + packet1.h_acc = packet_in.h_acc; + packet1.v_acc = packet_in.v_acc; + packet1.vel_n = packet_in.vel_n; + packet1.vel_e = packet_in.vel_e; + packet1.vel_d = packet_in.vel_d; + packet1.vel_acc = packet_in.vel_acc; + packet1.dist = packet_in.dist; + packet1.hdg = packet_in.hdg; + packet1.hdg_acc = packet_in.hdg_acc; + packet1.tracking_status = packet_in.tracking_status; + + +#ifdef MAVLINK_STATUS_FLAG_OUT_MAVLINK1 + if (status->flags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) { + // cope with extensions + memset(MAVLINK_MSG_ID_CAMERA_TRACKING_GEO_STATUS_MIN_LEN + (char *)&packet1, 0, sizeof(packet1)-MAVLINK_MSG_ID_CAMERA_TRACKING_GEO_STATUS_MIN_LEN); + } +#endif + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_camera_tracking_geo_status_encode(system_id, component_id, &msg, &packet1); + mavlink_msg_camera_tracking_geo_status_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_camera_tracking_geo_status_pack(system_id, component_id, &msg , packet1.tracking_status , packet1.lat , packet1.lon , packet1.alt , packet1.h_acc , packet1.v_acc , packet1.vel_n , packet1.vel_e , packet1.vel_d , packet1.vel_acc , packet1.dist , packet1.hdg , packet1.hdg_acc ); + mavlink_msg_camera_tracking_geo_status_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_camera_tracking_geo_status_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.tracking_status , packet1.lat , packet1.lon , packet1.alt , packet1.h_acc , packet1.v_acc , packet1.vel_n , packet1.vel_e , packet1.vel_d , packet1.vel_acc , packet1.dist , packet1.hdg , packet1.hdg_acc ); + mavlink_msg_camera_tracking_geo_status_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_to_send_buffer(buffer, &msg); + for (i=0; iflags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) && MAVLINK_MSG_ID_GIMBAL_MANAGER_INFORMATION >= 256) { + return; + } +#endif + mavlink_message_t msg; + uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; + uint16_t i; + mavlink_gimbal_manager_information_t packet_in = { + 963497464,963497672,73.0,101.0,129.0,157.0,185.0,213.0,101 + }; + mavlink_gimbal_manager_information_t packet1, packet2; + memset(&packet1, 0, sizeof(packet1)); + packet1.time_boot_ms = packet_in.time_boot_ms; + packet1.cap_flags = packet_in.cap_flags; + packet1.roll_min = packet_in.roll_min; + packet1.roll_max = packet_in.roll_max; + packet1.pitch_min = packet_in.pitch_min; + packet1.pitch_max = packet_in.pitch_max; + packet1.yaw_min = packet_in.yaw_min; + packet1.yaw_max = packet_in.yaw_max; + packet1.gimbal_device_id = packet_in.gimbal_device_id; + + +#ifdef MAVLINK_STATUS_FLAG_OUT_MAVLINK1 + if (status->flags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) { + // cope with extensions + memset(MAVLINK_MSG_ID_GIMBAL_MANAGER_INFORMATION_MIN_LEN + (char *)&packet1, 0, sizeof(packet1)-MAVLINK_MSG_ID_GIMBAL_MANAGER_INFORMATION_MIN_LEN); + } +#endif + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_gimbal_manager_information_encode(system_id, component_id, &msg, &packet1); + mavlink_msg_gimbal_manager_information_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_gimbal_manager_information_pack(system_id, component_id, &msg , packet1.time_boot_ms , packet1.cap_flags , packet1.gimbal_device_id , packet1.roll_min , packet1.roll_max , packet1.pitch_min , packet1.pitch_max , packet1.yaw_min , packet1.yaw_max ); + mavlink_msg_gimbal_manager_information_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_gimbal_manager_information_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.time_boot_ms , packet1.cap_flags , packet1.gimbal_device_id , packet1.roll_min , packet1.roll_max , packet1.pitch_min , packet1.pitch_max , packet1.yaw_min , packet1.yaw_max ); + mavlink_msg_gimbal_manager_information_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_to_send_buffer(buffer, &msg); + for (i=0; iflags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) && MAVLINK_MSG_ID_GIMBAL_MANAGER_STATUS >= 256) { + return; + } +#endif + mavlink_message_t msg; + uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; + uint16_t i; + mavlink_gimbal_manager_status_t packet_in = { + 963497464,963497672,29,96,163,230,41 + }; + mavlink_gimbal_manager_status_t packet1, packet2; + memset(&packet1, 0, sizeof(packet1)); + packet1.time_boot_ms = packet_in.time_boot_ms; + packet1.flags = packet_in.flags; + packet1.gimbal_device_id = packet_in.gimbal_device_id; + packet1.primary_control_sysid = packet_in.primary_control_sysid; + packet1.primary_control_compid = packet_in.primary_control_compid; + packet1.secondary_control_sysid = packet_in.secondary_control_sysid; + packet1.secondary_control_compid = packet_in.secondary_control_compid; + + +#ifdef MAVLINK_STATUS_FLAG_OUT_MAVLINK1 + if (status->flags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) { + // cope with extensions + memset(MAVLINK_MSG_ID_GIMBAL_MANAGER_STATUS_MIN_LEN + (char *)&packet1, 0, sizeof(packet1)-MAVLINK_MSG_ID_GIMBAL_MANAGER_STATUS_MIN_LEN); + } +#endif + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_gimbal_manager_status_encode(system_id, component_id, &msg, &packet1); + mavlink_msg_gimbal_manager_status_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_gimbal_manager_status_pack(system_id, component_id, &msg , packet1.time_boot_ms , packet1.flags , packet1.gimbal_device_id , packet1.primary_control_sysid , packet1.primary_control_compid , packet1.secondary_control_sysid , packet1.secondary_control_compid ); + mavlink_msg_gimbal_manager_status_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_gimbal_manager_status_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.time_boot_ms , packet1.flags , packet1.gimbal_device_id , packet1.primary_control_sysid , packet1.primary_control_compid , packet1.secondary_control_sysid , packet1.secondary_control_compid ); + mavlink_msg_gimbal_manager_status_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_to_send_buffer(buffer, &msg); + for (i=0; iflags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) && MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_ATTITUDE >= 256) { + return; + } +#endif + mavlink_message_t msg; + uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; + uint16_t i; + mavlink_gimbal_manager_set_attitude_t packet_in = { + 963497464,{ 45.0, 46.0, 47.0, 48.0 },157.0,185.0,213.0,101,168,235 + }; + mavlink_gimbal_manager_set_attitude_t packet1, packet2; + memset(&packet1, 0, sizeof(packet1)); + packet1.flags = packet_in.flags; + packet1.angular_velocity_x = packet_in.angular_velocity_x; + packet1.angular_velocity_y = packet_in.angular_velocity_y; + packet1.angular_velocity_z = packet_in.angular_velocity_z; + packet1.target_system = packet_in.target_system; + packet1.target_component = packet_in.target_component; + packet1.gimbal_device_id = packet_in.gimbal_device_id; + + mav_array_memcpy(packet1.q, packet_in.q, sizeof(float)*4); + +#ifdef MAVLINK_STATUS_FLAG_OUT_MAVLINK1 + if (status->flags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) { + // cope with extensions + memset(MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_ATTITUDE_MIN_LEN + (char *)&packet1, 0, sizeof(packet1)-MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_ATTITUDE_MIN_LEN); + } +#endif + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_gimbal_manager_set_attitude_encode(system_id, component_id, &msg, &packet1); + mavlink_msg_gimbal_manager_set_attitude_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_gimbal_manager_set_attitude_pack(system_id, component_id, &msg , packet1.target_system , packet1.target_component , packet1.flags , packet1.gimbal_device_id , packet1.q , packet1.angular_velocity_x , packet1.angular_velocity_y , packet1.angular_velocity_z ); + mavlink_msg_gimbal_manager_set_attitude_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_gimbal_manager_set_attitude_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.target_system , packet1.target_component , packet1.flags , packet1.gimbal_device_id , packet1.q , packet1.angular_velocity_x , packet1.angular_velocity_y , packet1.angular_velocity_z ); + mavlink_msg_gimbal_manager_set_attitude_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_to_send_buffer(buffer, &msg); + for (i=0; iflags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) && MAVLINK_MSG_ID_GIMBAL_DEVICE_INFORMATION >= 256) { + return; + } +#endif + mavlink_message_t msg; + uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; + uint16_t i; + mavlink_gimbal_device_information_t packet_in = { + 93372036854775807ULL,963497880,963498088,963498296,157.0,185.0,213.0,241.0,269.0,297.0,19523,19627,"WXYZABCDEFGHIJKLMNOPQRSTUVWXYZA","CDEFGHIJKLMNOPQRSTUVWXYZABCDEFG","IJKLMNOPQRSTUVWXYZABCDEFGHIJKLM" + }; + mavlink_gimbal_device_information_t packet1, packet2; + memset(&packet1, 0, sizeof(packet1)); + packet1.uid = packet_in.uid; + packet1.time_boot_ms = packet_in.time_boot_ms; + packet1.firmware_version = packet_in.firmware_version; + packet1.hardware_version = packet_in.hardware_version; + packet1.roll_min = packet_in.roll_min; + packet1.roll_max = packet_in.roll_max; + packet1.pitch_min = packet_in.pitch_min; + packet1.pitch_max = packet_in.pitch_max; + packet1.yaw_min = packet_in.yaw_min; + packet1.yaw_max = packet_in.yaw_max; + packet1.cap_flags = packet_in.cap_flags; + packet1.custom_cap_flags = packet_in.custom_cap_flags; + + mav_array_memcpy(packet1.vendor_name, packet_in.vendor_name, sizeof(char)*32); + mav_array_memcpy(packet1.model_name, packet_in.model_name, sizeof(char)*32); + mav_array_memcpy(packet1.custom_name, packet_in.custom_name, sizeof(char)*32); + +#ifdef MAVLINK_STATUS_FLAG_OUT_MAVLINK1 + if (status->flags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) { + // cope with extensions + memset(MAVLINK_MSG_ID_GIMBAL_DEVICE_INFORMATION_MIN_LEN + (char *)&packet1, 0, sizeof(packet1)-MAVLINK_MSG_ID_GIMBAL_DEVICE_INFORMATION_MIN_LEN); + } +#endif + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_gimbal_device_information_encode(system_id, component_id, &msg, &packet1); + mavlink_msg_gimbal_device_information_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_gimbal_device_information_pack(system_id, component_id, &msg , packet1.time_boot_ms , packet1.vendor_name , packet1.model_name , packet1.custom_name , packet1.firmware_version , packet1.hardware_version , packet1.uid , packet1.cap_flags , packet1.custom_cap_flags , packet1.roll_min , packet1.roll_max , packet1.pitch_min , packet1.pitch_max , packet1.yaw_min , packet1.yaw_max ); + mavlink_msg_gimbal_device_information_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_gimbal_device_information_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.time_boot_ms , packet1.vendor_name , packet1.model_name , packet1.custom_name , packet1.firmware_version , packet1.hardware_version , packet1.uid , packet1.cap_flags , packet1.custom_cap_flags , packet1.roll_min , packet1.roll_max , packet1.pitch_min , packet1.pitch_max , packet1.yaw_min , packet1.yaw_max ); + mavlink_msg_gimbal_device_information_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_to_send_buffer(buffer, &msg); + for (i=0; iflags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) && MAVLINK_MSG_ID_GIMBAL_DEVICE_SET_ATTITUDE >= 256) { + return; + } +#endif + mavlink_message_t msg; + uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; + uint16_t i; + mavlink_gimbal_device_set_attitude_t packet_in = { + { 17.0, 18.0, 19.0, 20.0 },129.0,157.0,185.0,18691,223,34 + }; + mavlink_gimbal_device_set_attitude_t packet1, packet2; + memset(&packet1, 0, sizeof(packet1)); + packet1.angular_velocity_x = packet_in.angular_velocity_x; + packet1.angular_velocity_y = packet_in.angular_velocity_y; + packet1.angular_velocity_z = packet_in.angular_velocity_z; + packet1.flags = packet_in.flags; + packet1.target_system = packet_in.target_system; + packet1.target_component = packet_in.target_component; + + mav_array_memcpy(packet1.q, packet_in.q, sizeof(float)*4); + +#ifdef MAVLINK_STATUS_FLAG_OUT_MAVLINK1 + if (status->flags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) { + // cope with extensions + memset(MAVLINK_MSG_ID_GIMBAL_DEVICE_SET_ATTITUDE_MIN_LEN + (char *)&packet1, 0, sizeof(packet1)-MAVLINK_MSG_ID_GIMBAL_DEVICE_SET_ATTITUDE_MIN_LEN); + } +#endif + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_gimbal_device_set_attitude_encode(system_id, component_id, &msg, &packet1); + mavlink_msg_gimbal_device_set_attitude_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_gimbal_device_set_attitude_pack(system_id, component_id, &msg , packet1.target_system , packet1.target_component , packet1.flags , packet1.q , packet1.angular_velocity_x , packet1.angular_velocity_y , packet1.angular_velocity_z ); + mavlink_msg_gimbal_device_set_attitude_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_gimbal_device_set_attitude_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.target_system , packet1.target_component , packet1.flags , packet1.q , packet1.angular_velocity_x , packet1.angular_velocity_y , packet1.angular_velocity_z ); + mavlink_msg_gimbal_device_set_attitude_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_to_send_buffer(buffer, &msg); + for (i=0; iflags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) && MAVLINK_MSG_ID_GIMBAL_DEVICE_ATTITUDE_STATUS >= 256) { + return; + } +#endif + mavlink_message_t msg; + uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; + uint16_t i; + mavlink_gimbal_device_attitude_status_t packet_in = { + 963497464,{ 45.0, 46.0, 47.0, 48.0 },157.0,185.0,213.0,963499128,19107,247,58 + }; + mavlink_gimbal_device_attitude_status_t packet1, packet2; + memset(&packet1, 0, sizeof(packet1)); + packet1.time_boot_ms = packet_in.time_boot_ms; + packet1.angular_velocity_x = packet_in.angular_velocity_x; + packet1.angular_velocity_y = packet_in.angular_velocity_y; + packet1.angular_velocity_z = packet_in.angular_velocity_z; + packet1.failure_flags = packet_in.failure_flags; + packet1.flags = packet_in.flags; + packet1.target_system = packet_in.target_system; + packet1.target_component = packet_in.target_component; + + mav_array_memcpy(packet1.q, packet_in.q, sizeof(float)*4); + +#ifdef MAVLINK_STATUS_FLAG_OUT_MAVLINK1 + if (status->flags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) { + // cope with extensions + memset(MAVLINK_MSG_ID_GIMBAL_DEVICE_ATTITUDE_STATUS_MIN_LEN + (char *)&packet1, 0, sizeof(packet1)-MAVLINK_MSG_ID_GIMBAL_DEVICE_ATTITUDE_STATUS_MIN_LEN); + } +#endif + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_gimbal_device_attitude_status_encode(system_id, component_id, &msg, &packet1); + mavlink_msg_gimbal_device_attitude_status_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_gimbal_device_attitude_status_pack(system_id, component_id, &msg , packet1.target_system , packet1.target_component , packet1.time_boot_ms , packet1.flags , packet1.q , packet1.angular_velocity_x , packet1.angular_velocity_y , packet1.angular_velocity_z , packet1.failure_flags ); + mavlink_msg_gimbal_device_attitude_status_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_gimbal_device_attitude_status_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.target_system , packet1.target_component , packet1.time_boot_ms , packet1.flags , packet1.q , packet1.angular_velocity_x , packet1.angular_velocity_y , packet1.angular_velocity_z , packet1.failure_flags ); + mavlink_msg_gimbal_device_attitude_status_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_to_send_buffer(buffer, &msg); + for (i=0; iflags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) && MAVLINK_MSG_ID_AUTOPILOT_STATE_FOR_GIMBAL_DEVICE >= 256) { + return; + } +#endif + mavlink_message_t msg; + uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; + uint16_t i; + mavlink_autopilot_state_for_gimbal_device_t packet_in = { + 93372036854775807ULL,{ 73.0, 74.0, 75.0, 76.0 },963498712,213.0,241.0,269.0,963499544,325.0,19731,27,94,161 + }; + mavlink_autopilot_state_for_gimbal_device_t packet1, packet2; + memset(&packet1, 0, sizeof(packet1)); + packet1.time_boot_us = packet_in.time_boot_us; + packet1.q_estimated_delay_us = packet_in.q_estimated_delay_us; + packet1.vx = packet_in.vx; + packet1.vy = packet_in.vy; + packet1.vz = packet_in.vz; + packet1.v_estimated_delay_us = packet_in.v_estimated_delay_us; + packet1.feed_forward_angular_velocity_z = packet_in.feed_forward_angular_velocity_z; + packet1.estimator_status = packet_in.estimator_status; + packet1.target_system = packet_in.target_system; + packet1.target_component = packet_in.target_component; + packet1.landed_state = packet_in.landed_state; + + mav_array_memcpy(packet1.q, packet_in.q, sizeof(float)*4); + +#ifdef MAVLINK_STATUS_FLAG_OUT_MAVLINK1 + if (status->flags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) { + // cope with extensions + memset(MAVLINK_MSG_ID_AUTOPILOT_STATE_FOR_GIMBAL_DEVICE_MIN_LEN + (char *)&packet1, 0, sizeof(packet1)-MAVLINK_MSG_ID_AUTOPILOT_STATE_FOR_GIMBAL_DEVICE_MIN_LEN); + } +#endif + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_autopilot_state_for_gimbal_device_encode(system_id, component_id, &msg, &packet1); + mavlink_msg_autopilot_state_for_gimbal_device_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_autopilot_state_for_gimbal_device_pack(system_id, component_id, &msg , packet1.target_system , packet1.target_component , packet1.time_boot_us , packet1.q , packet1.q_estimated_delay_us , packet1.vx , packet1.vy , packet1.vz , packet1.v_estimated_delay_us , packet1.feed_forward_angular_velocity_z , packet1.estimator_status , packet1.landed_state ); + mavlink_msg_autopilot_state_for_gimbal_device_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_autopilot_state_for_gimbal_device_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.target_system , packet1.target_component , packet1.time_boot_us , packet1.q , packet1.q_estimated_delay_us , packet1.vx , packet1.vy , packet1.vz , packet1.v_estimated_delay_us , packet1.feed_forward_angular_velocity_z , packet1.estimator_status , packet1.landed_state ); + mavlink_msg_autopilot_state_for_gimbal_device_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_to_send_buffer(buffer, &msg); + for (i=0; iflags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) && MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_PITCHYAW >= 256) { + return; + } +#endif + mavlink_message_t msg; + uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; + uint16_t i; + mavlink_gimbal_manager_set_pitchyaw_t packet_in = { + 963497464,45.0,73.0,101.0,129.0,65,132,199 + }; + mavlink_gimbal_manager_set_pitchyaw_t packet1, packet2; + memset(&packet1, 0, sizeof(packet1)); + packet1.flags = packet_in.flags; + packet1.pitch = packet_in.pitch; + packet1.yaw = packet_in.yaw; + packet1.pitch_rate = packet_in.pitch_rate; + packet1.yaw_rate = packet_in.yaw_rate; + packet1.target_system = packet_in.target_system; + packet1.target_component = packet_in.target_component; + packet1.gimbal_device_id = packet_in.gimbal_device_id; + + +#ifdef MAVLINK_STATUS_FLAG_OUT_MAVLINK1 + if (status->flags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) { + // cope with extensions + memset(MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_PITCHYAW_MIN_LEN + (char *)&packet1, 0, sizeof(packet1)-MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_PITCHYAW_MIN_LEN); + } +#endif + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_gimbal_manager_set_pitchyaw_encode(system_id, component_id, &msg, &packet1); + mavlink_msg_gimbal_manager_set_pitchyaw_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_gimbal_manager_set_pitchyaw_pack(system_id, component_id, &msg , packet1.target_system , packet1.target_component , packet1.flags , packet1.gimbal_device_id , packet1.pitch , packet1.yaw , packet1.pitch_rate , packet1.yaw_rate ); + mavlink_msg_gimbal_manager_set_pitchyaw_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_gimbal_manager_set_pitchyaw_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.target_system , packet1.target_component , packet1.flags , packet1.gimbal_device_id , packet1.pitch , packet1.yaw , packet1.pitch_rate , packet1.yaw_rate ); + mavlink_msg_gimbal_manager_set_pitchyaw_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_to_send_buffer(buffer, &msg); + for (i=0; iflags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) && MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_MANUAL_CONTROL >= 256) { + return; + } +#endif + mavlink_message_t msg; + uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; + uint16_t i; + mavlink_gimbal_manager_set_manual_control_t packet_in = { + 963497464,45.0,73.0,101.0,129.0,65,132,199 + }; + mavlink_gimbal_manager_set_manual_control_t packet1, packet2; + memset(&packet1, 0, sizeof(packet1)); + packet1.flags = packet_in.flags; + packet1.pitch = packet_in.pitch; + packet1.yaw = packet_in.yaw; + packet1.pitch_rate = packet_in.pitch_rate; + packet1.yaw_rate = packet_in.yaw_rate; + packet1.target_system = packet_in.target_system; + packet1.target_component = packet_in.target_component; + packet1.gimbal_device_id = packet_in.gimbal_device_id; + + +#ifdef MAVLINK_STATUS_FLAG_OUT_MAVLINK1 + if (status->flags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) { + // cope with extensions + memset(MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_MANUAL_CONTROL_MIN_LEN + (char *)&packet1, 0, sizeof(packet1)-MAVLINK_MSG_ID_GIMBAL_MANAGER_SET_MANUAL_CONTROL_MIN_LEN); + } +#endif + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_gimbal_manager_set_manual_control_encode(system_id, component_id, &msg, &packet1); + mavlink_msg_gimbal_manager_set_manual_control_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_gimbal_manager_set_manual_control_pack(system_id, component_id, &msg , packet1.target_system , packet1.target_component , packet1.flags , packet1.gimbal_device_id , packet1.pitch , packet1.yaw , packet1.pitch_rate , packet1.yaw_rate ); + mavlink_msg_gimbal_manager_set_manual_control_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_gimbal_manager_set_manual_control_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.target_system , packet1.target_component , packet1.flags , packet1.gimbal_device_id , packet1.pitch , packet1.yaw , packet1.pitch_rate , packet1.yaw_rate ); + mavlink_msg_gimbal_manager_set_manual_control_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_to_send_buffer(buffer, &msg); + for (i=0; iflags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) && MAVLINK_MSG_ID_ESC_INFO >= 256) { + return; + } +#endif + mavlink_message_t msg; + uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; + uint16_t i; + mavlink_esc_info_t packet_in = { + 93372036854775807ULL,{ 963497880, 963497881, 963497882, 963497883 },18483,{ 18587, 18588, 18589, 18590 },235,46,113,180,{ 247, 248, 249, 250 } + }; + mavlink_esc_info_t packet1, packet2; + memset(&packet1, 0, sizeof(packet1)); + packet1.time_usec = packet_in.time_usec; + packet1.counter = packet_in.counter; + packet1.index = packet_in.index; + packet1.count = packet_in.count; + packet1.connection_type = packet_in.connection_type; + packet1.info = packet_in.info; + + mav_array_memcpy(packet1.error_count, packet_in.error_count, sizeof(uint32_t)*4); + mav_array_memcpy(packet1.failure_flags, packet_in.failure_flags, sizeof(uint16_t)*4); + mav_array_memcpy(packet1.temperature, packet_in.temperature, sizeof(uint8_t)*4); + +#ifdef MAVLINK_STATUS_FLAG_OUT_MAVLINK1 + if (status->flags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) { + // cope with extensions + memset(MAVLINK_MSG_ID_ESC_INFO_MIN_LEN + (char *)&packet1, 0, sizeof(packet1)-MAVLINK_MSG_ID_ESC_INFO_MIN_LEN); + } +#endif + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_esc_info_encode(system_id, component_id, &msg, &packet1); + mavlink_msg_esc_info_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_esc_info_pack(system_id, component_id, &msg , packet1.index , packet1.time_usec , packet1.counter , packet1.count , packet1.connection_type , packet1.info , packet1.failure_flags , packet1.error_count , packet1.temperature ); + mavlink_msg_esc_info_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_esc_info_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.index , packet1.time_usec , packet1.counter , packet1.count , packet1.connection_type , packet1.info , packet1.failure_flags , packet1.error_count , packet1.temperature ); + mavlink_msg_esc_info_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_to_send_buffer(buffer, &msg); + for (i=0; iflags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) && MAVLINK_MSG_ID_ESC_STATUS >= 256) { + return; + } +#endif + mavlink_message_t msg; + uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; + uint16_t i; + mavlink_esc_status_t packet_in = { + 93372036854775807ULL,{ 963497880, 963497881, 963497882, 963497883 },{ 185.0, 186.0, 187.0, 188.0 },{ 297.0, 298.0, 299.0, 300.0 },173 + }; + mavlink_esc_status_t packet1, packet2; + memset(&packet1, 0, sizeof(packet1)); + packet1.time_usec = packet_in.time_usec; + packet1.index = packet_in.index; + + mav_array_memcpy(packet1.rpm, packet_in.rpm, sizeof(int32_t)*4); + mav_array_memcpy(packet1.voltage, packet_in.voltage, sizeof(float)*4); + mav_array_memcpy(packet1.current, packet_in.current, sizeof(float)*4); + +#ifdef MAVLINK_STATUS_FLAG_OUT_MAVLINK1 + if (status->flags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) { + // cope with extensions + memset(MAVLINK_MSG_ID_ESC_STATUS_MIN_LEN + (char *)&packet1, 0, sizeof(packet1)-MAVLINK_MSG_ID_ESC_STATUS_MIN_LEN); + } +#endif + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_esc_status_encode(system_id, component_id, &msg, &packet1); + mavlink_msg_esc_status_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_esc_status_pack(system_id, component_id, &msg , packet1.index , packet1.time_usec , packet1.rpm , packet1.voltage , packet1.current ); + mavlink_msg_esc_status_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_esc_status_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.index , packet1.time_usec , packet1.rpm , packet1.voltage , packet1.current ); + mavlink_msg_esc_status_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_to_send_buffer(buffer, &msg); + for (i=0; iflags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) && MAVLINK_MSG_ID_WIFI_CONFIG_AP >= 256) { + return; + } +#endif + mavlink_message_t msg; + uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; + uint16_t i; + mavlink_wifi_config_ap_t packet_in = { + "ABCDEFGHIJKLMNOPQRSTUVWXYZABCDE","GHIJKLMNOPQRSTUVWXYZABCDEFGHIJKLMNOPQRSTUVWXYZABCDEFGHIJKLMNOPQ",37,104 + }; + mavlink_wifi_config_ap_t packet1, packet2; + memset(&packet1, 0, sizeof(packet1)); + packet1.mode = packet_in.mode; + packet1.response = packet_in.response; + + mav_array_memcpy(packet1.ssid, packet_in.ssid, sizeof(char)*32); + mav_array_memcpy(packet1.password, packet_in.password, sizeof(char)*64); + +#ifdef MAVLINK_STATUS_FLAG_OUT_MAVLINK1 + if (status->flags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) { + // cope with extensions + memset(MAVLINK_MSG_ID_WIFI_CONFIG_AP_MIN_LEN + (char *)&packet1, 0, sizeof(packet1)-MAVLINK_MSG_ID_WIFI_CONFIG_AP_MIN_LEN); + } +#endif + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_wifi_config_ap_encode(system_id, component_id, &msg, &packet1); + mavlink_msg_wifi_config_ap_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_wifi_config_ap_pack(system_id, component_id, &msg , packet1.ssid , packet1.password , packet1.mode , packet1.response ); + mavlink_msg_wifi_config_ap_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_wifi_config_ap_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.ssid , packet1.password , packet1.mode , packet1.response ); + mavlink_msg_wifi_config_ap_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_to_send_buffer(buffer, &msg); + for (i=0; iflags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) && MAVLINK_MSG_ID_AIS_VESSEL >= 256) { + return; + } +#endif + mavlink_message_t msg; + uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; + uint16_t i; + mavlink_ais_vessel_t packet_in = { + 963497464,963497672,963497880,17859,17963,18067,18171,18275,18379,18483,211,22,89,156,223,"FGHIJK","MNOPQRSTUVWXYZABCDE" + }; + mavlink_ais_vessel_t packet1, packet2; + memset(&packet1, 0, sizeof(packet1)); + packet1.MMSI = packet_in.MMSI; + packet1.lat = packet_in.lat; + packet1.lon = packet_in.lon; + packet1.COG = packet_in.COG; + packet1.heading = packet_in.heading; + packet1.velocity = packet_in.velocity; + packet1.dimension_bow = packet_in.dimension_bow; + packet1.dimension_stern = packet_in.dimension_stern; + packet1.tslc = packet_in.tslc; + packet1.flags = packet_in.flags; + packet1.turn_rate = packet_in.turn_rate; + packet1.navigational_status = packet_in.navigational_status; + packet1.type = packet_in.type; + packet1.dimension_port = packet_in.dimension_port; + packet1.dimension_starboard = packet_in.dimension_starboard; + + mav_array_memcpy(packet1.callsign, packet_in.callsign, sizeof(char)*7); + mav_array_memcpy(packet1.name, packet_in.name, sizeof(char)*20); + +#ifdef MAVLINK_STATUS_FLAG_OUT_MAVLINK1 + if (status->flags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) { + // cope with extensions + memset(MAVLINK_MSG_ID_AIS_VESSEL_MIN_LEN + (char *)&packet1, 0, sizeof(packet1)-MAVLINK_MSG_ID_AIS_VESSEL_MIN_LEN); + } +#endif + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_ais_vessel_encode(system_id, component_id, &msg, &packet1); + mavlink_msg_ais_vessel_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_ais_vessel_pack(system_id, component_id, &msg , packet1.MMSI , packet1.lat , packet1.lon , packet1.COG , packet1.heading , packet1.velocity , packet1.turn_rate , packet1.navigational_status , packet1.type , packet1.dimension_bow , packet1.dimension_stern , packet1.dimension_port , packet1.dimension_starboard , packet1.callsign , packet1.name , packet1.tslc , packet1.flags ); + mavlink_msg_ais_vessel_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_ais_vessel_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.MMSI , packet1.lat , packet1.lon , packet1.COG , packet1.heading , packet1.velocity , packet1.turn_rate , packet1.navigational_status , packet1.type , packet1.dimension_bow , packet1.dimension_stern , packet1.dimension_port , packet1.dimension_starboard , packet1.callsign , packet1.name , packet1.tslc , packet1.flags ); + mavlink_msg_ais_vessel_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_to_send_buffer(buffer, &msg); + for (i=0; iflags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) && MAVLINK_MSG_ID_UAVCAN_NODE_STATUS >= 256) { + return; + } +#endif + mavlink_message_t msg; + uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; + uint16_t i; + mavlink_uavcan_node_status_t packet_in = { + 93372036854775807ULL,963497880,17859,175,242,53 + }; + mavlink_uavcan_node_status_t packet1, packet2; + memset(&packet1, 0, sizeof(packet1)); + packet1.time_usec = packet_in.time_usec; + packet1.uptime_sec = packet_in.uptime_sec; + packet1.vendor_specific_status_code = packet_in.vendor_specific_status_code; + packet1.health = packet_in.health; + packet1.mode = packet_in.mode; + packet1.sub_mode = packet_in.sub_mode; + + +#ifdef MAVLINK_STATUS_FLAG_OUT_MAVLINK1 + if (status->flags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) { + // cope with extensions + memset(MAVLINK_MSG_ID_UAVCAN_NODE_STATUS_MIN_LEN + (char *)&packet1, 0, sizeof(packet1)-MAVLINK_MSG_ID_UAVCAN_NODE_STATUS_MIN_LEN); + } +#endif + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_uavcan_node_status_encode(system_id, component_id, &msg, &packet1); + mavlink_msg_uavcan_node_status_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_uavcan_node_status_pack(system_id, component_id, &msg , packet1.time_usec , packet1.uptime_sec , packet1.health , packet1.mode , packet1.sub_mode , packet1.vendor_specific_status_code ); + mavlink_msg_uavcan_node_status_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_uavcan_node_status_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.time_usec , packet1.uptime_sec , packet1.health , packet1.mode , packet1.sub_mode , packet1.vendor_specific_status_code ); + mavlink_msg_uavcan_node_status_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_to_send_buffer(buffer, &msg); + for (i=0; iflags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) && MAVLINK_MSG_ID_UAVCAN_NODE_INFO >= 256) { + return; + } +#endif + mavlink_message_t msg; + uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; + uint16_t i; + mavlink_uavcan_node_info_t packet_in = { + 93372036854775807ULL,963497880,963498088,"QRSTUVWXYZABCDEFGHIJKLMNOPQRSTUVWXYZABCDEFGHIJKLMNOPQRSTUVWXYZABCDEFGHIJKLMNOPQ",37,104,{ 171, 172, 173, 174, 175, 176, 177, 178, 179, 180, 181, 182, 183, 184, 185, 186 },219,30 + }; + mavlink_uavcan_node_info_t packet1, packet2; + memset(&packet1, 0, sizeof(packet1)); + packet1.time_usec = packet_in.time_usec; + packet1.uptime_sec = packet_in.uptime_sec; + packet1.sw_vcs_commit = packet_in.sw_vcs_commit; + packet1.hw_version_major = packet_in.hw_version_major; + packet1.hw_version_minor = packet_in.hw_version_minor; + packet1.sw_version_major = packet_in.sw_version_major; + packet1.sw_version_minor = packet_in.sw_version_minor; + + mav_array_memcpy(packet1.name, packet_in.name, sizeof(char)*80); + mav_array_memcpy(packet1.hw_unique_id, packet_in.hw_unique_id, sizeof(uint8_t)*16); + +#ifdef MAVLINK_STATUS_FLAG_OUT_MAVLINK1 + if (status->flags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) { + // cope with extensions + memset(MAVLINK_MSG_ID_UAVCAN_NODE_INFO_MIN_LEN + (char *)&packet1, 0, sizeof(packet1)-MAVLINK_MSG_ID_UAVCAN_NODE_INFO_MIN_LEN); + } +#endif + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_uavcan_node_info_encode(system_id, component_id, &msg, &packet1); + mavlink_msg_uavcan_node_info_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_uavcan_node_info_pack(system_id, component_id, &msg , packet1.time_usec , packet1.uptime_sec , packet1.name , packet1.hw_version_major , packet1.hw_version_minor , packet1.hw_unique_id , packet1.sw_version_major , packet1.sw_version_minor , packet1.sw_vcs_commit ); + mavlink_msg_uavcan_node_info_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_uavcan_node_info_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.time_usec , packet1.uptime_sec , packet1.name , packet1.hw_version_major , packet1.hw_version_minor , packet1.hw_unique_id , packet1.sw_version_major , packet1.sw_version_minor , packet1.sw_vcs_commit ); + mavlink_msg_uavcan_node_info_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_to_send_buffer(buffer, &msg); + for (i=0; iflags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) && MAVLINK_MSG_ID_PARAM_EXT_REQUEST_READ >= 256) { + return; + } +#endif + mavlink_message_t msg; + uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; + uint16_t i; + mavlink_param_ext_request_read_t packet_in = { + 17235,139,206,"EFGHIJKLMNOPQRS" + }; + mavlink_param_ext_request_read_t packet1, packet2; + memset(&packet1, 0, sizeof(packet1)); + packet1.param_index = packet_in.param_index; + packet1.target_system = packet_in.target_system; + packet1.target_component = packet_in.target_component; + + mav_array_memcpy(packet1.param_id, packet_in.param_id, sizeof(char)*16); + +#ifdef MAVLINK_STATUS_FLAG_OUT_MAVLINK1 + if (status->flags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) { + // cope with extensions + memset(MAVLINK_MSG_ID_PARAM_EXT_REQUEST_READ_MIN_LEN + (char *)&packet1, 0, sizeof(packet1)-MAVLINK_MSG_ID_PARAM_EXT_REQUEST_READ_MIN_LEN); + } +#endif + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_param_ext_request_read_encode(system_id, component_id, &msg, &packet1); + mavlink_msg_param_ext_request_read_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_param_ext_request_read_pack(system_id, component_id, &msg , packet1.target_system , packet1.target_component , packet1.param_id , packet1.param_index ); + mavlink_msg_param_ext_request_read_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_param_ext_request_read_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.target_system , packet1.target_component , packet1.param_id , packet1.param_index ); + mavlink_msg_param_ext_request_read_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_to_send_buffer(buffer, &msg); + for (i=0; iflags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) && MAVLINK_MSG_ID_PARAM_EXT_REQUEST_LIST >= 256) { + return; + } +#endif + mavlink_message_t msg; + uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; + uint16_t i; + mavlink_param_ext_request_list_t packet_in = { + 5,72 + }; + mavlink_param_ext_request_list_t packet1, packet2; + memset(&packet1, 0, sizeof(packet1)); + packet1.target_system = packet_in.target_system; + packet1.target_component = packet_in.target_component; + + +#ifdef MAVLINK_STATUS_FLAG_OUT_MAVLINK1 + if (status->flags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) { + // cope with extensions + memset(MAVLINK_MSG_ID_PARAM_EXT_REQUEST_LIST_MIN_LEN + (char *)&packet1, 0, sizeof(packet1)-MAVLINK_MSG_ID_PARAM_EXT_REQUEST_LIST_MIN_LEN); + } +#endif + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_param_ext_request_list_encode(system_id, component_id, &msg, &packet1); + mavlink_msg_param_ext_request_list_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_param_ext_request_list_pack(system_id, component_id, &msg , packet1.target_system , packet1.target_component ); + mavlink_msg_param_ext_request_list_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_param_ext_request_list_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.target_system , packet1.target_component ); + mavlink_msg_param_ext_request_list_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_to_send_buffer(buffer, &msg); + for (i=0; iflags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) && MAVLINK_MSG_ID_PARAM_EXT_VALUE >= 256) { + return; + } +#endif + mavlink_message_t msg; + uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; + uint16_t i; + mavlink_param_ext_value_t packet_in = { + 17235,17339,"EFGHIJKLMNOPQRS","UVWXYZABCDEFGHIJKLMNOPQRSTUVWXYZABCDEFGHIJKLMNOPQRSTUVWXYZABCDEFGHIJKLMNOPQRSTUVWXYZABCDEFGHIJKLMNOPQRSTUVWXYZABCDEFGHIJKLMNOPQ",193 + }; + mavlink_param_ext_value_t packet1, packet2; + memset(&packet1, 0, sizeof(packet1)); + packet1.param_count = packet_in.param_count; + packet1.param_index = packet_in.param_index; + packet1.param_type = packet_in.param_type; + + mav_array_memcpy(packet1.param_id, packet_in.param_id, sizeof(char)*16); + mav_array_memcpy(packet1.param_value, packet_in.param_value, sizeof(char)*128); + +#ifdef MAVLINK_STATUS_FLAG_OUT_MAVLINK1 + if (status->flags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) { + // cope with extensions + memset(MAVLINK_MSG_ID_PARAM_EXT_VALUE_MIN_LEN + (char *)&packet1, 0, sizeof(packet1)-MAVLINK_MSG_ID_PARAM_EXT_VALUE_MIN_LEN); + } +#endif + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_param_ext_value_encode(system_id, component_id, &msg, &packet1); + mavlink_msg_param_ext_value_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_param_ext_value_pack(system_id, component_id, &msg , packet1.param_id , packet1.param_value , packet1.param_type , packet1.param_count , packet1.param_index ); + mavlink_msg_param_ext_value_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_param_ext_value_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.param_id , packet1.param_value , packet1.param_type , packet1.param_count , packet1.param_index ); + mavlink_msg_param_ext_value_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_to_send_buffer(buffer, &msg); + for (i=0; iflags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) && MAVLINK_MSG_ID_PARAM_EXT_SET >= 256) { + return; + } +#endif + mavlink_message_t msg; + uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; + uint16_t i; + mavlink_param_ext_set_t packet_in = { + 5,72,"CDEFGHIJKLMNOPQ","STUVWXYZABCDEFGHIJKLMNOPQRSTUVWXYZABCDEFGHIJKLMNOPQRSTUVWXYZABCDEFGHIJKLMNOPQRSTUVWXYZABCDEFGHIJKLMNOPQRSTUVWXYZABCDEFGHIJKLMNO",59 + }; + mavlink_param_ext_set_t packet1, packet2; + memset(&packet1, 0, sizeof(packet1)); + packet1.target_system = packet_in.target_system; + packet1.target_component = packet_in.target_component; + packet1.param_type = packet_in.param_type; + + mav_array_memcpy(packet1.param_id, packet_in.param_id, sizeof(char)*16); + mav_array_memcpy(packet1.param_value, packet_in.param_value, sizeof(char)*128); + +#ifdef MAVLINK_STATUS_FLAG_OUT_MAVLINK1 + if (status->flags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) { + // cope with extensions + memset(MAVLINK_MSG_ID_PARAM_EXT_SET_MIN_LEN + (char *)&packet1, 0, sizeof(packet1)-MAVLINK_MSG_ID_PARAM_EXT_SET_MIN_LEN); + } +#endif + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_param_ext_set_encode(system_id, component_id, &msg, &packet1); + mavlink_msg_param_ext_set_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_param_ext_set_pack(system_id, component_id, &msg , packet1.target_system , packet1.target_component , packet1.param_id , packet1.param_value , packet1.param_type ); + mavlink_msg_param_ext_set_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_param_ext_set_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.target_system , packet1.target_component , packet1.param_id , packet1.param_value , packet1.param_type ); + mavlink_msg_param_ext_set_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_to_send_buffer(buffer, &msg); + for (i=0; iflags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) && MAVLINK_MSG_ID_PARAM_EXT_ACK >= 256) { + return; + } +#endif + mavlink_message_t msg; + uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; + uint16_t i; + mavlink_param_ext_ack_t packet_in = { + "ABCDEFGHIJKLMNO","QRSTUVWXYZABCDEFGHIJKLMNOPQRSTUVWXYZABCDEFGHIJKLMNOPQRSTUVWXYZABCDEFGHIJKLMNOPQRSTUVWXYZABCDEFGHIJKLMNOPQRSTUVWXYZABCDEFGHIJKLM",181,248 + }; + mavlink_param_ext_ack_t packet1, packet2; + memset(&packet1, 0, sizeof(packet1)); + packet1.param_type = packet_in.param_type; + packet1.param_result = packet_in.param_result; + + mav_array_memcpy(packet1.param_id, packet_in.param_id, sizeof(char)*16); + mav_array_memcpy(packet1.param_value, packet_in.param_value, sizeof(char)*128); + +#ifdef MAVLINK_STATUS_FLAG_OUT_MAVLINK1 + if (status->flags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) { + // cope with extensions + memset(MAVLINK_MSG_ID_PARAM_EXT_ACK_MIN_LEN + (char *)&packet1, 0, sizeof(packet1)-MAVLINK_MSG_ID_PARAM_EXT_ACK_MIN_LEN); + } +#endif + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_param_ext_ack_encode(system_id, component_id, &msg, &packet1); + mavlink_msg_param_ext_ack_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_param_ext_ack_pack(system_id, component_id, &msg , packet1.param_id , packet1.param_value , packet1.param_type , packet1.param_result ); + mavlink_msg_param_ext_ack_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_param_ext_ack_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.param_id , packet1.param_value , packet1.param_type , packet1.param_result ); + mavlink_msg_param_ext_ack_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_to_send_buffer(buffer, &msg); + for (i=0; iflags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) && MAVLINK_MSG_ID_OBSTACLE_DISTANCE >= 256) { + return; + } +#endif + mavlink_message_t msg; + uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; + uint16_t i; + mavlink_obstacle_distance_t packet_in = { + 93372036854775807ULL,{ 17651, 17652, 17653, 17654, 17655, 17656, 17657, 17658, 17659, 17660, 17661, 17662, 17663, 17664, 17665, 17666, 17667, 17668, 17669, 17670, 17671, 17672, 17673, 17674, 17675, 17676, 17677, 17678, 17679, 17680, 17681, 17682, 17683, 17684, 17685, 17686, 17687, 17688, 17689, 17690, 17691, 17692, 17693, 17694, 17695, 17696, 17697, 17698, 17699, 17700, 17701, 17702, 17703, 17704, 17705, 17706, 17707, 17708, 17709, 17710, 17711, 17712, 17713, 17714, 17715, 17716, 17717, 17718, 17719, 17720, 17721, 17722 },25139,25243,217,28,1123.0,1151.0,119 + }; + mavlink_obstacle_distance_t packet1, packet2; + memset(&packet1, 0, sizeof(packet1)); + packet1.time_usec = packet_in.time_usec; + packet1.min_distance = packet_in.min_distance; + packet1.max_distance = packet_in.max_distance; + packet1.sensor_type = packet_in.sensor_type; + packet1.increment = packet_in.increment; + packet1.increment_f = packet_in.increment_f; + packet1.angle_offset = packet_in.angle_offset; + packet1.frame = packet_in.frame; + + mav_array_memcpy(packet1.distances, packet_in.distances, sizeof(uint16_t)*72); + +#ifdef MAVLINK_STATUS_FLAG_OUT_MAVLINK1 + if (status->flags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) { + // cope with extensions + memset(MAVLINK_MSG_ID_OBSTACLE_DISTANCE_MIN_LEN + (char *)&packet1, 0, sizeof(packet1)-MAVLINK_MSG_ID_OBSTACLE_DISTANCE_MIN_LEN); + } +#endif + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_obstacle_distance_encode(system_id, component_id, &msg, &packet1); + mavlink_msg_obstacle_distance_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_obstacle_distance_pack(system_id, component_id, &msg , packet1.time_usec , packet1.sensor_type , packet1.distances , packet1.increment , packet1.min_distance , packet1.max_distance , packet1.increment_f , packet1.angle_offset , packet1.frame ); + mavlink_msg_obstacle_distance_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_obstacle_distance_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.time_usec , packet1.sensor_type , packet1.distances , packet1.increment , packet1.min_distance , packet1.max_distance , packet1.increment_f , packet1.angle_offset , packet1.frame ); + mavlink_msg_obstacle_distance_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_to_send_buffer(buffer, &msg); + for (i=0; iflags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) && MAVLINK_MSG_ID_ODOMETRY >= 256) { + return; + } +#endif + mavlink_message_t msg; + uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; + uint16_t i; + mavlink_odometry_t packet_in = { + 93372036854775807ULL,73.0,101.0,129.0,{ 157.0, 158.0, 159.0, 160.0 },269.0,297.0,325.0,353.0,381.0,409.0,{ 437.0, 438.0, 439.0, 440.0, 441.0, 442.0, 443.0, 444.0, 445.0, 446.0, 447.0, 448.0, 449.0, 450.0, 451.0, 452.0, 453.0, 454.0, 455.0, 456.0, 457.0 },{ 1025.0, 1026.0, 1027.0, 1028.0, 1029.0, 1030.0, 1031.0, 1032.0, 1033.0, 1034.0, 1035.0, 1036.0, 1037.0, 1038.0, 1039.0, 1040.0, 1041.0, 1042.0, 1043.0, 1044.0, 1045.0 },177,244,55,122 + }; + mavlink_odometry_t packet1, packet2; + memset(&packet1, 0, sizeof(packet1)); + packet1.time_usec = packet_in.time_usec; + packet1.x = packet_in.x; + packet1.y = packet_in.y; + packet1.z = packet_in.z; + packet1.vx = packet_in.vx; + packet1.vy = packet_in.vy; + packet1.vz = packet_in.vz; + packet1.rollspeed = packet_in.rollspeed; + packet1.pitchspeed = packet_in.pitchspeed; + packet1.yawspeed = packet_in.yawspeed; + packet1.frame_id = packet_in.frame_id; + packet1.child_frame_id = packet_in.child_frame_id; + packet1.reset_counter = packet_in.reset_counter; + packet1.estimator_type = packet_in.estimator_type; + + mav_array_memcpy(packet1.q, packet_in.q, sizeof(float)*4); + mav_array_memcpy(packet1.pose_covariance, packet_in.pose_covariance, sizeof(float)*21); + mav_array_memcpy(packet1.velocity_covariance, packet_in.velocity_covariance, sizeof(float)*21); + +#ifdef MAVLINK_STATUS_FLAG_OUT_MAVLINK1 + if (status->flags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) { + // cope with extensions + memset(MAVLINK_MSG_ID_ODOMETRY_MIN_LEN + (char *)&packet1, 0, sizeof(packet1)-MAVLINK_MSG_ID_ODOMETRY_MIN_LEN); + } +#endif + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_odometry_encode(system_id, component_id, &msg, &packet1); + mavlink_msg_odometry_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_odometry_pack(system_id, component_id, &msg , packet1.time_usec , packet1.frame_id , packet1.child_frame_id , packet1.x , packet1.y , packet1.z , packet1.q , packet1.vx , packet1.vy , packet1.vz , packet1.rollspeed , packet1.pitchspeed , packet1.yawspeed , packet1.pose_covariance , packet1.velocity_covariance , packet1.reset_counter , packet1.estimator_type ); + mavlink_msg_odometry_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_odometry_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.time_usec , packet1.frame_id , packet1.child_frame_id , packet1.x , packet1.y , packet1.z , packet1.q , packet1.vx , packet1.vy , packet1.vz , packet1.rollspeed , packet1.pitchspeed , packet1.yawspeed , packet1.pose_covariance , packet1.velocity_covariance , packet1.reset_counter , packet1.estimator_type ); + mavlink_msg_odometry_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_to_send_buffer(buffer, &msg); + for (i=0; iflags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) && MAVLINK_MSG_ID_TRAJECTORY_REPRESENTATION_WAYPOINTS >= 256) { + return; + } +#endif + mavlink_message_t msg; + uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; + uint16_t i; + mavlink_trajectory_representation_waypoints_t packet_in = { + 93372036854775807ULL,{ 73.0, 74.0, 75.0, 76.0, 77.0 },{ 213.0, 214.0, 215.0, 216.0, 217.0 },{ 353.0, 354.0, 355.0, 356.0, 357.0 },{ 493.0, 494.0, 495.0, 496.0, 497.0 },{ 633.0, 634.0, 635.0, 636.0, 637.0 },{ 773.0, 774.0, 775.0, 776.0, 777.0 },{ 913.0, 914.0, 915.0, 916.0, 917.0 },{ 1053.0, 1054.0, 1055.0, 1056.0, 1057.0 },{ 1193.0, 1194.0, 1195.0, 1196.0, 1197.0 },{ 1333.0, 1334.0, 1335.0, 1336.0, 1337.0 },{ 1473.0, 1474.0, 1475.0, 1476.0, 1477.0 },{ 29091, 29092, 29093, 29094, 29095 },79 + }; + mavlink_trajectory_representation_waypoints_t packet1, packet2; + memset(&packet1, 0, sizeof(packet1)); + packet1.time_usec = packet_in.time_usec; + packet1.valid_points = packet_in.valid_points; + + mav_array_memcpy(packet1.pos_x, packet_in.pos_x, sizeof(float)*5); + mav_array_memcpy(packet1.pos_y, packet_in.pos_y, sizeof(float)*5); + mav_array_memcpy(packet1.pos_z, packet_in.pos_z, sizeof(float)*5); + mav_array_memcpy(packet1.vel_x, packet_in.vel_x, sizeof(float)*5); + mav_array_memcpy(packet1.vel_y, packet_in.vel_y, sizeof(float)*5); + mav_array_memcpy(packet1.vel_z, packet_in.vel_z, sizeof(float)*5); + mav_array_memcpy(packet1.acc_x, packet_in.acc_x, sizeof(float)*5); + mav_array_memcpy(packet1.acc_y, packet_in.acc_y, sizeof(float)*5); + mav_array_memcpy(packet1.acc_z, packet_in.acc_z, sizeof(float)*5); + mav_array_memcpy(packet1.pos_yaw, packet_in.pos_yaw, sizeof(float)*5); + mav_array_memcpy(packet1.vel_yaw, packet_in.vel_yaw, sizeof(float)*5); + mav_array_memcpy(packet1.command, packet_in.command, sizeof(uint16_t)*5); + +#ifdef MAVLINK_STATUS_FLAG_OUT_MAVLINK1 + if (status->flags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) { + // cope with extensions + memset(MAVLINK_MSG_ID_TRAJECTORY_REPRESENTATION_WAYPOINTS_MIN_LEN + (char *)&packet1, 0, sizeof(packet1)-MAVLINK_MSG_ID_TRAJECTORY_REPRESENTATION_WAYPOINTS_MIN_LEN); + } +#endif + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_trajectory_representation_waypoints_encode(system_id, component_id, &msg, &packet1); + mavlink_msg_trajectory_representation_waypoints_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_trajectory_representation_waypoints_pack(system_id, component_id, &msg , packet1.time_usec , packet1.valid_points , packet1.pos_x , packet1.pos_y , packet1.pos_z , packet1.vel_x , packet1.vel_y , packet1.vel_z , packet1.acc_x , packet1.acc_y , packet1.acc_z , packet1.pos_yaw , packet1.vel_yaw , packet1.command ); + mavlink_msg_trajectory_representation_waypoints_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_trajectory_representation_waypoints_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.time_usec , packet1.valid_points , packet1.pos_x , packet1.pos_y , packet1.pos_z , packet1.vel_x , packet1.vel_y , packet1.vel_z , packet1.acc_x , packet1.acc_y , packet1.acc_z , packet1.pos_yaw , packet1.vel_yaw , packet1.command ); + mavlink_msg_trajectory_representation_waypoints_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_to_send_buffer(buffer, &msg); + for (i=0; iflags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) && MAVLINK_MSG_ID_TRAJECTORY_REPRESENTATION_BEZIER >= 256) { + return; + } +#endif + mavlink_message_t msg; + uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; + uint16_t i; + mavlink_trajectory_representation_bezier_t packet_in = { + 93372036854775807ULL,{ 73.0, 74.0, 75.0, 76.0, 77.0 },{ 213.0, 214.0, 215.0, 216.0, 217.0 },{ 353.0, 354.0, 355.0, 356.0, 357.0 },{ 493.0, 494.0, 495.0, 496.0, 497.0 },{ 633.0, 634.0, 635.0, 636.0, 637.0 },73 + }; + mavlink_trajectory_representation_bezier_t packet1, packet2; + memset(&packet1, 0, sizeof(packet1)); + packet1.time_usec = packet_in.time_usec; + packet1.valid_points = packet_in.valid_points; + + mav_array_memcpy(packet1.pos_x, packet_in.pos_x, sizeof(float)*5); + mav_array_memcpy(packet1.pos_y, packet_in.pos_y, sizeof(float)*5); + mav_array_memcpy(packet1.pos_z, packet_in.pos_z, sizeof(float)*5); + mav_array_memcpy(packet1.delta, packet_in.delta, sizeof(float)*5); + mav_array_memcpy(packet1.pos_yaw, packet_in.pos_yaw, sizeof(float)*5); + +#ifdef MAVLINK_STATUS_FLAG_OUT_MAVLINK1 + if (status->flags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) { + // cope with extensions + memset(MAVLINK_MSG_ID_TRAJECTORY_REPRESENTATION_BEZIER_MIN_LEN + (char *)&packet1, 0, sizeof(packet1)-MAVLINK_MSG_ID_TRAJECTORY_REPRESENTATION_BEZIER_MIN_LEN); + } +#endif + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_trajectory_representation_bezier_encode(system_id, component_id, &msg, &packet1); + mavlink_msg_trajectory_representation_bezier_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_trajectory_representation_bezier_pack(system_id, component_id, &msg , packet1.time_usec , packet1.valid_points , packet1.pos_x , packet1.pos_y , packet1.pos_z , packet1.delta , packet1.pos_yaw ); + mavlink_msg_trajectory_representation_bezier_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_trajectory_representation_bezier_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.time_usec , packet1.valid_points , packet1.pos_x , packet1.pos_y , packet1.pos_z , packet1.delta , packet1.pos_yaw ); + mavlink_msg_trajectory_representation_bezier_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_to_send_buffer(buffer, &msg); + for (i=0; iflags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) && MAVLINK_MSG_ID_CELLULAR_STATUS >= 256) { + return; + } +#endif + mavlink_message_t msg; + uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; + uint16_t i; + mavlink_cellular_status_t packet_in = { + 17235,17339,17443,151,218,29,96 + }; + mavlink_cellular_status_t packet1, packet2; + memset(&packet1, 0, sizeof(packet1)); + packet1.mcc = packet_in.mcc; + packet1.mnc = packet_in.mnc; + packet1.lac = packet_in.lac; + packet1.status = packet_in.status; + packet1.failure_reason = packet_in.failure_reason; + packet1.type = packet_in.type; + packet1.quality = packet_in.quality; + + +#ifdef MAVLINK_STATUS_FLAG_OUT_MAVLINK1 + if (status->flags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) { + // cope with extensions + memset(MAVLINK_MSG_ID_CELLULAR_STATUS_MIN_LEN + (char *)&packet1, 0, sizeof(packet1)-MAVLINK_MSG_ID_CELLULAR_STATUS_MIN_LEN); + } +#endif + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_cellular_status_encode(system_id, component_id, &msg, &packet1); + mavlink_msg_cellular_status_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_cellular_status_pack(system_id, component_id, &msg , packet1.status , packet1.failure_reason , packet1.type , packet1.quality , packet1.mcc , packet1.mnc , packet1.lac ); + mavlink_msg_cellular_status_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_cellular_status_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.status , packet1.failure_reason , packet1.type , packet1.quality , packet1.mcc , packet1.mnc , packet1.lac ); + mavlink_msg_cellular_status_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_to_send_buffer(buffer, &msg); + for (i=0; iflags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) && MAVLINK_MSG_ID_ISBD_LINK_STATUS >= 256) { + return; + } +#endif + mavlink_message_t msg; + uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; + uint16_t i; + mavlink_isbd_link_status_t packet_in = { + 93372036854775807ULL,93372036854776311ULL,18067,18171,65,132,199,10 + }; + mavlink_isbd_link_status_t packet1, packet2; + memset(&packet1, 0, sizeof(packet1)); + packet1.timestamp = packet_in.timestamp; + packet1.last_heartbeat = packet_in.last_heartbeat; + packet1.failed_sessions = packet_in.failed_sessions; + packet1.successful_sessions = packet_in.successful_sessions; + packet1.signal_quality = packet_in.signal_quality; + packet1.ring_pending = packet_in.ring_pending; + packet1.tx_session_pending = packet_in.tx_session_pending; + packet1.rx_session_pending = packet_in.rx_session_pending; + + +#ifdef MAVLINK_STATUS_FLAG_OUT_MAVLINK1 + if (status->flags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) { + // cope with extensions + memset(MAVLINK_MSG_ID_ISBD_LINK_STATUS_MIN_LEN + (char *)&packet1, 0, sizeof(packet1)-MAVLINK_MSG_ID_ISBD_LINK_STATUS_MIN_LEN); + } +#endif + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_isbd_link_status_encode(system_id, component_id, &msg, &packet1); + mavlink_msg_isbd_link_status_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_isbd_link_status_pack(system_id, component_id, &msg , packet1.timestamp , packet1.last_heartbeat , packet1.failed_sessions , packet1.successful_sessions , packet1.signal_quality , packet1.ring_pending , packet1.tx_session_pending , packet1.rx_session_pending ); + mavlink_msg_isbd_link_status_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_isbd_link_status_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.timestamp , packet1.last_heartbeat , packet1.failed_sessions , packet1.successful_sessions , packet1.signal_quality , packet1.ring_pending , packet1.tx_session_pending , packet1.rx_session_pending ); + mavlink_msg_isbd_link_status_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_to_send_buffer(buffer, &msg); + for (i=0; iflags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) && MAVLINK_MSG_ID_CELLULAR_CONFIG >= 256) { + return; + } +#endif + mavlink_message_t msg; + uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; + uint16_t i; + mavlink_cellular_config_t packet_in = { + 5,72,"CDEFGHIJKLMNOPQ","STUVWXYZABCDEFG","IJKLMNOPQRSTUVWXYZABCDEFGHIJKLM","OPQRSTUVWXYZABC",123,190 + }; + mavlink_cellular_config_t packet1, packet2; + memset(&packet1, 0, sizeof(packet1)); + packet1.enable_lte = packet_in.enable_lte; + packet1.enable_pin = packet_in.enable_pin; + packet1.roaming = packet_in.roaming; + packet1.response = packet_in.response; + + mav_array_memcpy(packet1.pin, packet_in.pin, sizeof(char)*16); + mav_array_memcpy(packet1.new_pin, packet_in.new_pin, sizeof(char)*16); + mav_array_memcpy(packet1.apn, packet_in.apn, sizeof(char)*32); + mav_array_memcpy(packet1.puk, packet_in.puk, sizeof(char)*16); + +#ifdef MAVLINK_STATUS_FLAG_OUT_MAVLINK1 + if (status->flags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) { + // cope with extensions + memset(MAVLINK_MSG_ID_CELLULAR_CONFIG_MIN_LEN + (char *)&packet1, 0, sizeof(packet1)-MAVLINK_MSG_ID_CELLULAR_CONFIG_MIN_LEN); + } +#endif + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_cellular_config_encode(system_id, component_id, &msg, &packet1); + mavlink_msg_cellular_config_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_cellular_config_pack(system_id, component_id, &msg , packet1.enable_lte , packet1.enable_pin , packet1.pin , packet1.new_pin , packet1.apn , packet1.puk , packet1.roaming , packet1.response ); + mavlink_msg_cellular_config_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_cellular_config_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.enable_lte , packet1.enable_pin , packet1.pin , packet1.new_pin , packet1.apn , packet1.puk , packet1.roaming , packet1.response ); + mavlink_msg_cellular_config_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_to_send_buffer(buffer, &msg); + for (i=0; iflags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) && MAVLINK_MSG_ID_RAW_RPM >= 256) { + return; + } +#endif + mavlink_message_t msg; + uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; + uint16_t i; + mavlink_raw_rpm_t packet_in = { + 17.0,17 + }; + mavlink_raw_rpm_t packet1, packet2; + memset(&packet1, 0, sizeof(packet1)); + packet1.frequency = packet_in.frequency; + packet1.index = packet_in.index; + + +#ifdef MAVLINK_STATUS_FLAG_OUT_MAVLINK1 + if (status->flags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) { + // cope with extensions + memset(MAVLINK_MSG_ID_RAW_RPM_MIN_LEN + (char *)&packet1, 0, sizeof(packet1)-MAVLINK_MSG_ID_RAW_RPM_MIN_LEN); + } +#endif + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_raw_rpm_encode(system_id, component_id, &msg, &packet1); + mavlink_msg_raw_rpm_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_raw_rpm_pack(system_id, component_id, &msg , packet1.index , packet1.frequency ); + mavlink_msg_raw_rpm_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_raw_rpm_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.index , packet1.frequency ); + mavlink_msg_raw_rpm_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_to_send_buffer(buffer, &msg); + for (i=0; iflags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) && MAVLINK_MSG_ID_UTM_GLOBAL_POSITION >= 256) { + return; + } +#endif + mavlink_message_t msg; + uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; + uint16_t i; + mavlink_utm_global_position_t packet_in = { + 93372036854775807ULL,963497880,963498088,963498296,963498504,963498712,963498920,963499128,19107,19211,19315,19419,19523,19627,19731,{ 27, 28, 29, 30, 31, 32, 33, 34, 35, 36, 37, 38, 39, 40, 41, 42, 43, 44 },209,20 + }; + mavlink_utm_global_position_t packet1, packet2; + memset(&packet1, 0, sizeof(packet1)); + packet1.time = packet_in.time; + packet1.lat = packet_in.lat; + packet1.lon = packet_in.lon; + packet1.alt = packet_in.alt; + packet1.relative_alt = packet_in.relative_alt; + packet1.next_lat = packet_in.next_lat; + packet1.next_lon = packet_in.next_lon; + packet1.next_alt = packet_in.next_alt; + packet1.vx = packet_in.vx; + packet1.vy = packet_in.vy; + packet1.vz = packet_in.vz; + packet1.h_acc = packet_in.h_acc; + packet1.v_acc = packet_in.v_acc; + packet1.vel_acc = packet_in.vel_acc; + packet1.update_rate = packet_in.update_rate; + packet1.flight_state = packet_in.flight_state; + packet1.flags = packet_in.flags; + + mav_array_memcpy(packet1.uas_id, packet_in.uas_id, sizeof(uint8_t)*18); + +#ifdef MAVLINK_STATUS_FLAG_OUT_MAVLINK1 + if (status->flags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) { + // cope with extensions + memset(MAVLINK_MSG_ID_UTM_GLOBAL_POSITION_MIN_LEN + (char *)&packet1, 0, sizeof(packet1)-MAVLINK_MSG_ID_UTM_GLOBAL_POSITION_MIN_LEN); + } +#endif + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_utm_global_position_encode(system_id, component_id, &msg, &packet1); + mavlink_msg_utm_global_position_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_utm_global_position_pack(system_id, component_id, &msg , packet1.time , packet1.uas_id , packet1.lat , packet1.lon , packet1.alt , packet1.relative_alt , packet1.vx , packet1.vy , packet1.vz , packet1.h_acc , packet1.v_acc , packet1.vel_acc , packet1.next_lat , packet1.next_lon , packet1.next_alt , packet1.update_rate , packet1.flight_state , packet1.flags ); + mavlink_msg_utm_global_position_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_utm_global_position_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.time , packet1.uas_id , packet1.lat , packet1.lon , packet1.alt , packet1.relative_alt , packet1.vx , packet1.vy , packet1.vz , packet1.h_acc , packet1.v_acc , packet1.vel_acc , packet1.next_lat , packet1.next_lon , packet1.next_alt , packet1.update_rate , packet1.flight_state , packet1.flags ); + mavlink_msg_utm_global_position_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_to_send_buffer(buffer, &msg); + for (i=0; iflags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) && MAVLINK_MSG_ID_DEBUG_FLOAT_ARRAY >= 256) { + return; + } +#endif + mavlink_message_t msg; + uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; + uint16_t i; + mavlink_debug_float_array_t packet_in = { + 93372036854775807ULL,17651,"KLMNOPQRS",{ 157.0, 158.0, 159.0, 160.0, 161.0, 162.0, 163.0, 164.0, 165.0, 166.0, 167.0, 168.0, 169.0, 170.0, 171.0, 172.0, 173.0, 174.0, 175.0, 176.0, 177.0, 178.0, 179.0, 180.0, 181.0, 182.0, 183.0, 184.0, 185.0, 186.0, 187.0, 188.0, 189.0, 190.0, 191.0, 192.0, 193.0, 194.0, 195.0, 196.0, 197.0, 198.0, 199.0, 200.0, 201.0, 202.0, 203.0, 204.0, 205.0, 206.0, 207.0, 208.0, 209.0, 210.0, 211.0, 212.0, 213.0, 214.0 } + }; + mavlink_debug_float_array_t packet1, packet2; + memset(&packet1, 0, sizeof(packet1)); + packet1.time_usec = packet_in.time_usec; + packet1.array_id = packet_in.array_id; + + mav_array_memcpy(packet1.name, packet_in.name, sizeof(char)*10); + mav_array_memcpy(packet1.data, packet_in.data, sizeof(float)*58); + +#ifdef MAVLINK_STATUS_FLAG_OUT_MAVLINK1 + if (status->flags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) { + // cope with extensions + memset(MAVLINK_MSG_ID_DEBUG_FLOAT_ARRAY_MIN_LEN + (char *)&packet1, 0, sizeof(packet1)-MAVLINK_MSG_ID_DEBUG_FLOAT_ARRAY_MIN_LEN); + } +#endif + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_debug_float_array_encode(system_id, component_id, &msg, &packet1); + mavlink_msg_debug_float_array_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_debug_float_array_pack(system_id, component_id, &msg , packet1.time_usec , packet1.name , packet1.array_id , packet1.data ); + mavlink_msg_debug_float_array_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_debug_float_array_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.time_usec , packet1.name , packet1.array_id , packet1.data ); + mavlink_msg_debug_float_array_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_to_send_buffer(buffer, &msg); + for (i=0; iflags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) && MAVLINK_MSG_ID_ORBIT_EXECUTION_STATUS >= 256) { + return; + } +#endif + mavlink_message_t msg; + uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; + uint16_t i; + mavlink_orbit_execution_status_t packet_in = { + 93372036854775807ULL,73.0,963498088,963498296,157.0,77 + }; + mavlink_orbit_execution_status_t packet1, packet2; + memset(&packet1, 0, sizeof(packet1)); + packet1.time_usec = packet_in.time_usec; + packet1.radius = packet_in.radius; + packet1.x = packet_in.x; + packet1.y = packet_in.y; + packet1.z = packet_in.z; + packet1.frame = packet_in.frame; + + +#ifdef MAVLINK_STATUS_FLAG_OUT_MAVLINK1 + if (status->flags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) { + // cope with extensions + memset(MAVLINK_MSG_ID_ORBIT_EXECUTION_STATUS_MIN_LEN + (char *)&packet1, 0, sizeof(packet1)-MAVLINK_MSG_ID_ORBIT_EXECUTION_STATUS_MIN_LEN); + } +#endif + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_orbit_execution_status_encode(system_id, component_id, &msg, &packet1); + mavlink_msg_orbit_execution_status_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_orbit_execution_status_pack(system_id, component_id, &msg , packet1.time_usec , packet1.radius , packet1.frame , packet1.x , packet1.y , packet1.z ); + mavlink_msg_orbit_execution_status_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_orbit_execution_status_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.time_usec , packet1.radius , packet1.frame , packet1.x , packet1.y , packet1.z ); + mavlink_msg_orbit_execution_status_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_to_send_buffer(buffer, &msg); + for (i=0; iflags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) && MAVLINK_MSG_ID_SMART_BATTERY_INFO >= 256) { + return; + } +#endif + mavlink_message_t msg; + uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; + uint16_t i; + mavlink_smart_battery_info_t packet_in = { + 963497464,963497672,17651,17755,17859,17963,18067,187,254,65,"VWXYZABCDEFGHIJ","LMNOPQRSTUVWXYZABCDEFGHIJKLMNOPQRSTUVWXYZABCDEFGH" + }; + mavlink_smart_battery_info_t packet1, packet2; + memset(&packet1, 0, sizeof(packet1)); + packet1.capacity_full_specification = packet_in.capacity_full_specification; + packet1.capacity_full = packet_in.capacity_full; + packet1.cycle_count = packet_in.cycle_count; + packet1.weight = packet_in.weight; + packet1.discharge_minimum_voltage = packet_in.discharge_minimum_voltage; + packet1.charging_minimum_voltage = packet_in.charging_minimum_voltage; + packet1.resting_minimum_voltage = packet_in.resting_minimum_voltage; + packet1.id = packet_in.id; + packet1.battery_function = packet_in.battery_function; + packet1.type = packet_in.type; + + mav_array_memcpy(packet1.serial_number, packet_in.serial_number, sizeof(char)*16); + mav_array_memcpy(packet1.device_name, packet_in.device_name, sizeof(char)*50); + +#ifdef MAVLINK_STATUS_FLAG_OUT_MAVLINK1 + if (status->flags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) { + // cope with extensions + memset(MAVLINK_MSG_ID_SMART_BATTERY_INFO_MIN_LEN + (char *)&packet1, 0, sizeof(packet1)-MAVLINK_MSG_ID_SMART_BATTERY_INFO_MIN_LEN); + } +#endif + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_smart_battery_info_encode(system_id, component_id, &msg, &packet1); + mavlink_msg_smart_battery_info_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_smart_battery_info_pack(system_id, component_id, &msg , packet1.id , packet1.battery_function , packet1.type , packet1.capacity_full_specification , packet1.capacity_full , packet1.cycle_count , packet1.serial_number , packet1.device_name , packet1.weight , packet1.discharge_minimum_voltage , packet1.charging_minimum_voltage , packet1.resting_minimum_voltage ); + mavlink_msg_smart_battery_info_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_smart_battery_info_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.id , packet1.battery_function , packet1.type , packet1.capacity_full_specification , packet1.capacity_full , packet1.cycle_count , packet1.serial_number , packet1.device_name , packet1.weight , packet1.discharge_minimum_voltage , packet1.charging_minimum_voltage , packet1.resting_minimum_voltage ); + mavlink_msg_smart_battery_info_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_to_send_buffer(buffer, &msg); + for (i=0; iflags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) && MAVLINK_MSG_ID_GENERATOR_STATUS >= 256) { + return; + } +#endif + mavlink_message_t msg; + uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; + uint16_t i; + mavlink_generator_status_t packet_in = { + 93372036854775807ULL,73.0,101.0,129.0,157.0,185.0,963498920,963499128,19107,19211,19315 + }; + mavlink_generator_status_t packet1, packet2; + memset(&packet1, 0, sizeof(packet1)); + packet1.status = packet_in.status; + packet1.battery_current = packet_in.battery_current; + packet1.load_current = packet_in.load_current; + packet1.power_generated = packet_in.power_generated; + packet1.bus_voltage = packet_in.bus_voltage; + packet1.bat_current_setpoint = packet_in.bat_current_setpoint; + packet1.runtime = packet_in.runtime; + packet1.time_until_maintenance = packet_in.time_until_maintenance; + packet1.generator_speed = packet_in.generator_speed; + packet1.rectifier_temperature = packet_in.rectifier_temperature; + packet1.generator_temperature = packet_in.generator_temperature; + + +#ifdef MAVLINK_STATUS_FLAG_OUT_MAVLINK1 + if (status->flags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) { + // cope with extensions + memset(MAVLINK_MSG_ID_GENERATOR_STATUS_MIN_LEN + (char *)&packet1, 0, sizeof(packet1)-MAVLINK_MSG_ID_GENERATOR_STATUS_MIN_LEN); + } +#endif + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_generator_status_encode(system_id, component_id, &msg, &packet1); + mavlink_msg_generator_status_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_generator_status_pack(system_id, component_id, &msg , packet1.status , packet1.generator_speed , packet1.battery_current , packet1.load_current , packet1.power_generated , packet1.bus_voltage , packet1.rectifier_temperature , packet1.bat_current_setpoint , packet1.generator_temperature , packet1.runtime , packet1.time_until_maintenance ); + mavlink_msg_generator_status_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_generator_status_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.status , packet1.generator_speed , packet1.battery_current , packet1.load_current , packet1.power_generated , packet1.bus_voltage , packet1.rectifier_temperature , packet1.bat_current_setpoint , packet1.generator_temperature , packet1.runtime , packet1.time_until_maintenance ); + mavlink_msg_generator_status_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_to_send_buffer(buffer, &msg); + for (i=0; iflags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) && MAVLINK_MSG_ID_ACTUATOR_OUTPUT_STATUS >= 256) { + return; + } +#endif + mavlink_message_t msg; + uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; + uint16_t i; + mavlink_actuator_output_status_t packet_in = { + 93372036854775807ULL,963497880,{ 101.0, 102.0, 103.0, 104.0, 105.0, 106.0, 107.0, 108.0, 109.0, 110.0, 111.0, 112.0, 113.0, 114.0, 115.0, 116.0, 117.0, 118.0, 119.0, 120.0, 121.0, 122.0, 123.0, 124.0, 125.0, 126.0, 127.0, 128.0, 129.0, 130.0, 131.0, 132.0 } + }; + mavlink_actuator_output_status_t packet1, packet2; + memset(&packet1, 0, sizeof(packet1)); + packet1.time_usec = packet_in.time_usec; + packet1.active = packet_in.active; + + mav_array_memcpy(packet1.actuator, packet_in.actuator, sizeof(float)*32); + +#ifdef MAVLINK_STATUS_FLAG_OUT_MAVLINK1 + if (status->flags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) { + // cope with extensions + memset(MAVLINK_MSG_ID_ACTUATOR_OUTPUT_STATUS_MIN_LEN + (char *)&packet1, 0, sizeof(packet1)-MAVLINK_MSG_ID_ACTUATOR_OUTPUT_STATUS_MIN_LEN); + } +#endif + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_actuator_output_status_encode(system_id, component_id, &msg, &packet1); + mavlink_msg_actuator_output_status_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_actuator_output_status_pack(system_id, component_id, &msg , packet1.time_usec , packet1.active , packet1.actuator ); + mavlink_msg_actuator_output_status_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_actuator_output_status_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.time_usec , packet1.active , packet1.actuator ); + mavlink_msg_actuator_output_status_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_to_send_buffer(buffer, &msg); + for (i=0; iflags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) && MAVLINK_MSG_ID_TIME_ESTIMATE_TO_TARGET >= 256) { + return; + } +#endif + mavlink_message_t msg; + uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; + uint16_t i; + mavlink_time_estimate_to_target_t packet_in = { + 963497464,963497672,963497880,963498088,963498296 + }; + mavlink_time_estimate_to_target_t packet1, packet2; + memset(&packet1, 0, sizeof(packet1)); + packet1.safe_return = packet_in.safe_return; + packet1.land = packet_in.land; + packet1.mission_next_item = packet_in.mission_next_item; + packet1.mission_end = packet_in.mission_end; + packet1.commanded_action = packet_in.commanded_action; + + +#ifdef MAVLINK_STATUS_FLAG_OUT_MAVLINK1 + if (status->flags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) { + // cope with extensions + memset(MAVLINK_MSG_ID_TIME_ESTIMATE_TO_TARGET_MIN_LEN + (char *)&packet1, 0, sizeof(packet1)-MAVLINK_MSG_ID_TIME_ESTIMATE_TO_TARGET_MIN_LEN); + } +#endif + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_time_estimate_to_target_encode(system_id, component_id, &msg, &packet1); + mavlink_msg_time_estimate_to_target_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_time_estimate_to_target_pack(system_id, component_id, &msg , packet1.safe_return , packet1.land , packet1.mission_next_item , packet1.mission_end , packet1.commanded_action ); + mavlink_msg_time_estimate_to_target_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_time_estimate_to_target_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.safe_return , packet1.land , packet1.mission_next_item , packet1.mission_end , packet1.commanded_action ); + mavlink_msg_time_estimate_to_target_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_to_send_buffer(buffer, &msg); + for (i=0; iflags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) && MAVLINK_MSG_ID_TUNNEL >= 256) { + return; + } +#endif + mavlink_message_t msg; + uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; + uint16_t i; + mavlink_tunnel_t packet_in = { + 17235,139,206,17,{ 84, 85, 86, 87, 88, 89, 90, 91, 92, 93, 94, 95, 96, 97, 98, 99, 100, 101, 102, 103, 104, 105, 106, 107, 108, 109, 110, 111, 112, 113, 114, 115, 116, 117, 118, 119, 120, 121, 122, 123, 124, 125, 126, 127, 128, 129, 130, 131, 132, 133, 134, 135, 136, 137, 138, 139, 140, 141, 142, 143, 144, 145, 146, 147, 148, 149, 150, 151, 152, 153, 154, 155, 156, 157, 158, 159, 160, 161, 162, 163, 164, 165, 166, 167, 168, 169, 170, 171, 172, 173, 174, 175, 176, 177, 178, 179, 180, 181, 182, 183, 184, 185, 186, 187, 188, 189, 190, 191, 192, 193, 194, 195, 196, 197, 198, 199, 200, 201, 202, 203, 204, 205, 206, 207, 208, 209, 210, 211 } + }; + mavlink_tunnel_t packet1, packet2; + memset(&packet1, 0, sizeof(packet1)); + packet1.payload_type = packet_in.payload_type; + packet1.target_system = packet_in.target_system; + packet1.target_component = packet_in.target_component; + packet1.payload_length = packet_in.payload_length; + + mav_array_memcpy(packet1.payload, packet_in.payload, sizeof(uint8_t)*128); + +#ifdef MAVLINK_STATUS_FLAG_OUT_MAVLINK1 + if (status->flags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) { + // cope with extensions + memset(MAVLINK_MSG_ID_TUNNEL_MIN_LEN + (char *)&packet1, 0, sizeof(packet1)-MAVLINK_MSG_ID_TUNNEL_MIN_LEN); + } +#endif + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_tunnel_encode(system_id, component_id, &msg, &packet1); + mavlink_msg_tunnel_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_tunnel_pack(system_id, component_id, &msg , packet1.target_system , packet1.target_component , packet1.payload_type , packet1.payload_length , packet1.payload ); + mavlink_msg_tunnel_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_tunnel_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.target_system , packet1.target_component , packet1.payload_type , packet1.payload_length , packet1.payload ); + mavlink_msg_tunnel_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_to_send_buffer(buffer, &msg); + for (i=0; iflags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) && MAVLINK_MSG_ID_ONBOARD_COMPUTER_STATUS >= 256) { + return; + } +#endif + mavlink_message_t msg; + uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; + uint16_t i; + mavlink_onboard_computer_status_t packet_in = { + 93372036854775807ULL,963497880,963498088,963498296,{ 963498504, 963498505, 963498506, 963498507 },{ 963499336, 963499337, 963499338, 963499339 },{ 963500168, 963500169, 963500170, 963500171 },{ 963501000, 963501001, 963501002, 963501003, 963501004, 963501005 },{ 963502248, 963502249, 963502250, 963502251, 963502252, 963502253 },{ 963503496, 963503497, 963503498, 963503499, 963503500, 963503501 },{ 963504744, 963504745, 963504746, 963504747, 963504748, 963504749 },{ 963505992, 963505993, 963505994, 963505995, 963505996, 963505997 },{ 27011, 27012, 27013, 27014 },81,{ 148, 149, 150, 151, 152, 153, 154, 155 },{ 172, 173, 174, 175, 176, 177, 178, 179, 180, 181 },{ 74, 75, 76, 77 },{ 86, 87, 88, 89, 90, 91, 92, 93, 94, 95 },244,{ 55, 56, 57, 58, 59, 60, 61, 62 } + }; + mavlink_onboard_computer_status_t packet1, packet2; + memset(&packet1, 0, sizeof(packet1)); + packet1.time_usec = packet_in.time_usec; + packet1.uptime = packet_in.uptime; + packet1.ram_usage = packet_in.ram_usage; + packet1.ram_total = packet_in.ram_total; + packet1.type = packet_in.type; + packet1.temperature_board = packet_in.temperature_board; + + mav_array_memcpy(packet1.storage_type, packet_in.storage_type, sizeof(uint32_t)*4); + mav_array_memcpy(packet1.storage_usage, packet_in.storage_usage, sizeof(uint32_t)*4); + mav_array_memcpy(packet1.storage_total, packet_in.storage_total, sizeof(uint32_t)*4); + mav_array_memcpy(packet1.link_type, packet_in.link_type, sizeof(uint32_t)*6); + mav_array_memcpy(packet1.link_tx_rate, packet_in.link_tx_rate, sizeof(uint32_t)*6); + mav_array_memcpy(packet1.link_rx_rate, packet_in.link_rx_rate, sizeof(uint32_t)*6); + mav_array_memcpy(packet1.link_tx_max, packet_in.link_tx_max, sizeof(uint32_t)*6); + mav_array_memcpy(packet1.link_rx_max, packet_in.link_rx_max, sizeof(uint32_t)*6); + mav_array_memcpy(packet1.fan_speed, packet_in.fan_speed, sizeof(int16_t)*4); + mav_array_memcpy(packet1.cpu_cores, packet_in.cpu_cores, sizeof(uint8_t)*8); + mav_array_memcpy(packet1.cpu_combined, packet_in.cpu_combined, sizeof(uint8_t)*10); + mav_array_memcpy(packet1.gpu_cores, packet_in.gpu_cores, sizeof(uint8_t)*4); + mav_array_memcpy(packet1.gpu_combined, packet_in.gpu_combined, sizeof(uint8_t)*10); + mav_array_memcpy(packet1.temperature_core, packet_in.temperature_core, sizeof(int8_t)*8); + +#ifdef MAVLINK_STATUS_FLAG_OUT_MAVLINK1 + if (status->flags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) { + // cope with extensions + memset(MAVLINK_MSG_ID_ONBOARD_COMPUTER_STATUS_MIN_LEN + (char *)&packet1, 0, sizeof(packet1)-MAVLINK_MSG_ID_ONBOARD_COMPUTER_STATUS_MIN_LEN); + } +#endif + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_onboard_computer_status_encode(system_id, component_id, &msg, &packet1); + mavlink_msg_onboard_computer_status_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_onboard_computer_status_pack(system_id, component_id, &msg , packet1.time_usec , packet1.uptime , packet1.type , packet1.cpu_cores , packet1.cpu_combined , packet1.gpu_cores , packet1.gpu_combined , packet1.temperature_board , packet1.temperature_core , packet1.fan_speed , packet1.ram_usage , packet1.ram_total , packet1.storage_type , packet1.storage_usage , packet1.storage_total , packet1.link_type , packet1.link_tx_rate , packet1.link_rx_rate , packet1.link_tx_max , packet1.link_rx_max ); + mavlink_msg_onboard_computer_status_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_onboard_computer_status_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.time_usec , packet1.uptime , packet1.type , packet1.cpu_cores , packet1.cpu_combined , packet1.gpu_cores , packet1.gpu_combined , packet1.temperature_board , packet1.temperature_core , packet1.fan_speed , packet1.ram_usage , packet1.ram_total , packet1.storage_type , packet1.storage_usage , packet1.storage_total , packet1.link_type , packet1.link_tx_rate , packet1.link_rx_rate , packet1.link_tx_max , packet1.link_rx_max ); + mavlink_msg_onboard_computer_status_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_to_send_buffer(buffer, &msg); + for (i=0; iflags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) && MAVLINK_MSG_ID_COMPONENT_INFORMATION >= 256) { + return; + } +#endif + mavlink_message_t msg; + uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; + uint16_t i; + mavlink_component_information_t packet_in = { + 963497464,963497672,963497880,963498088,"QRSTUVWXYZABCDEFGHIJKLMNOPQRSTUVWXYZABCDEFGHIJKLMNOPQRSTUVWXYZABCDEFG","IJKLMNOPQRSTUVWXYZABCDEFGHIJKLMNOPQRSTUVWXYZABCDEFGHIJKLMNOPQRSTUVWXY" + }; + mavlink_component_information_t packet1, packet2; + memset(&packet1, 0, sizeof(packet1)); + packet1.time_boot_ms = packet_in.time_boot_ms; + packet1.metadata_type = packet_in.metadata_type; + packet1.metadata_uid = packet_in.metadata_uid; + packet1.translation_uid = packet_in.translation_uid; + + mav_array_memcpy(packet1.metadata_uri, packet_in.metadata_uri, sizeof(char)*70); + mav_array_memcpy(packet1.translation_uri, packet_in.translation_uri, sizeof(char)*70); + +#ifdef MAVLINK_STATUS_FLAG_OUT_MAVLINK1 + if (status->flags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) { + // cope with extensions + memset(MAVLINK_MSG_ID_COMPONENT_INFORMATION_MIN_LEN + (char *)&packet1, 0, sizeof(packet1)-MAVLINK_MSG_ID_COMPONENT_INFORMATION_MIN_LEN); + } +#endif + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_component_information_encode(system_id, component_id, &msg, &packet1); + mavlink_msg_component_information_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_component_information_pack(system_id, component_id, &msg , packet1.time_boot_ms , packet1.metadata_type , packet1.metadata_uid , packet1.metadata_uri , packet1.translation_uid , packet1.translation_uri ); + mavlink_msg_component_information_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_component_information_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.time_boot_ms , packet1.metadata_type , packet1.metadata_uid , packet1.metadata_uri , packet1.translation_uid , packet1.translation_uri ); + mavlink_msg_component_information_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_to_send_buffer(buffer, &msg); + for (i=0; iflags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) && MAVLINK_MSG_ID_PLAY_TUNE_V2 >= 256) { + return; + } +#endif + mavlink_message_t msg; + uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; + uint16_t i; + mavlink_play_tune_v2_t packet_in = { + 963497464,17,84,"GHIJKLMNOPQRSTUVWXYZABCDEFGHIJKLMNOPQRSTUVWXYZABCDEFGHIJKLMNOPQRSTUVWXYZABCDEFGHIJKLMNOPQRSTUVWXYZABCDEFGHIJKLMNOPQRSTUVWXYZABCDEFGHIJKLMNOPQRSTUVWXYZABCDEFGHIJKLMNOPQRSTUVWXYZABCDEFGHIJKLMNOPQRSTUVWXYZABCDEFGHIJKLMNOPQRSTUVWXYZABCDEFGHIJKLMNOPQRS" + }; + mavlink_play_tune_v2_t packet1, packet2; + memset(&packet1, 0, sizeof(packet1)); + packet1.format = packet_in.format; + packet1.target_system = packet_in.target_system; + packet1.target_component = packet_in.target_component; + + mav_array_memcpy(packet1.tune, packet_in.tune, sizeof(char)*248); + +#ifdef MAVLINK_STATUS_FLAG_OUT_MAVLINK1 + if (status->flags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) { + // cope with extensions + memset(MAVLINK_MSG_ID_PLAY_TUNE_V2_MIN_LEN + (char *)&packet1, 0, sizeof(packet1)-MAVLINK_MSG_ID_PLAY_TUNE_V2_MIN_LEN); + } +#endif + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_play_tune_v2_encode(system_id, component_id, &msg, &packet1); + mavlink_msg_play_tune_v2_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_play_tune_v2_pack(system_id, component_id, &msg , packet1.target_system , packet1.target_component , packet1.format , packet1.tune ); + mavlink_msg_play_tune_v2_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_play_tune_v2_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.target_system , packet1.target_component , packet1.format , packet1.tune ); + mavlink_msg_play_tune_v2_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_to_send_buffer(buffer, &msg); + for (i=0; iflags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) && MAVLINK_MSG_ID_SUPPORTED_TUNES >= 256) { + return; + } +#endif + mavlink_message_t msg; + uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; + uint16_t i; + mavlink_supported_tunes_t packet_in = { + 963497464,17,84 + }; + mavlink_supported_tunes_t packet1, packet2; + memset(&packet1, 0, sizeof(packet1)); + packet1.format = packet_in.format; + packet1.target_system = packet_in.target_system; + packet1.target_component = packet_in.target_component; + + +#ifdef MAVLINK_STATUS_FLAG_OUT_MAVLINK1 + if (status->flags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) { + // cope with extensions + memset(MAVLINK_MSG_ID_SUPPORTED_TUNES_MIN_LEN + (char *)&packet1, 0, sizeof(packet1)-MAVLINK_MSG_ID_SUPPORTED_TUNES_MIN_LEN); + } +#endif + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_supported_tunes_encode(system_id, component_id, &msg, &packet1); + mavlink_msg_supported_tunes_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_supported_tunes_pack(system_id, component_id, &msg , packet1.target_system , packet1.target_component , packet1.format ); + mavlink_msg_supported_tunes_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_supported_tunes_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.target_system , packet1.target_component , packet1.format ); + mavlink_msg_supported_tunes_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_to_send_buffer(buffer, &msg); + for (i=0; iflags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) && MAVLINK_MSG_ID_WHEEL_DISTANCE >= 256) { + return; + } +#endif + mavlink_message_t msg; + uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; + uint16_t i; + mavlink_wheel_distance_t packet_in = { + 93372036854775807ULL,{ 179.0, 180.0, 181.0, 182.0, 183.0, 184.0, 185.0, 186.0, 187.0, 188.0, 189.0, 190.0, 191.0, 192.0, 193.0, 194.0 },157 + }; + mavlink_wheel_distance_t packet1, packet2; + memset(&packet1, 0, sizeof(packet1)); + packet1.time_usec = packet_in.time_usec; + packet1.count = packet_in.count; + + mav_array_memcpy(packet1.distance, packet_in.distance, sizeof(double)*16); + +#ifdef MAVLINK_STATUS_FLAG_OUT_MAVLINK1 + if (status->flags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) { + // cope with extensions + memset(MAVLINK_MSG_ID_WHEEL_DISTANCE_MIN_LEN + (char *)&packet1, 0, sizeof(packet1)-MAVLINK_MSG_ID_WHEEL_DISTANCE_MIN_LEN); + } +#endif + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_wheel_distance_encode(system_id, component_id, &msg, &packet1); + mavlink_msg_wheel_distance_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_wheel_distance_pack(system_id, component_id, &msg , packet1.time_usec , packet1.count , packet1.distance ); + mavlink_msg_wheel_distance_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_wheel_distance_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.time_usec , packet1.count , packet1.distance ); + mavlink_msg_wheel_distance_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_to_send_buffer(buffer, &msg); + for (i=0; iflags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) && MAVLINK_MSG_ID_WINCH_STATUS >= 256) { + return; + } +#endif + mavlink_message_t msg; + uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; + uint16_t i; + mavlink_winch_status_t packet_in = { + 93372036854775807ULL,73.0,101.0,129.0,157.0,185.0,963498920,18899 + }; + mavlink_winch_status_t packet1, packet2; + memset(&packet1, 0, sizeof(packet1)); + packet1.time_usec = packet_in.time_usec; + packet1.line_length = packet_in.line_length; + packet1.speed = packet_in.speed; + packet1.tension = packet_in.tension; + packet1.voltage = packet_in.voltage; + packet1.current = packet_in.current; + packet1.status = packet_in.status; + packet1.temperature = packet_in.temperature; + + +#ifdef MAVLINK_STATUS_FLAG_OUT_MAVLINK1 + if (status->flags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) { + // cope with extensions + memset(MAVLINK_MSG_ID_WINCH_STATUS_MIN_LEN + (char *)&packet1, 0, sizeof(packet1)-MAVLINK_MSG_ID_WINCH_STATUS_MIN_LEN); + } +#endif + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_winch_status_encode(system_id, component_id, &msg, &packet1); + mavlink_msg_winch_status_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_winch_status_pack(system_id, component_id, &msg , packet1.time_usec , packet1.line_length , packet1.speed , packet1.tension , packet1.voltage , packet1.current , packet1.temperature , packet1.status ); + mavlink_msg_winch_status_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_winch_status_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.time_usec , packet1.line_length , packet1.speed , packet1.tension , packet1.voltage , packet1.current , packet1.temperature , packet1.status ); + mavlink_msg_winch_status_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_to_send_buffer(buffer, &msg); + for (i=0; iflags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) && MAVLINK_MSG_ID_OPEN_DRONE_ID_BASIC_ID >= 256) { + return; + } +#endif + mavlink_message_t msg; + uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; + uint16_t i; + mavlink_open_drone_id_basic_id_t packet_in = { + 5,72,{ 139, 140, 141, 142, 143, 144, 145, 146, 147, 148, 149, 150, 151, 152, 153, 154, 155, 156, 157, 158 },199,10,{ 77, 78, 79, 80, 81, 82, 83, 84, 85, 86, 87, 88, 89, 90, 91, 92, 93, 94, 95, 96 } + }; + mavlink_open_drone_id_basic_id_t packet1, packet2; + memset(&packet1, 0, sizeof(packet1)); + packet1.target_system = packet_in.target_system; + packet1.target_component = packet_in.target_component; + packet1.id_type = packet_in.id_type; + packet1.ua_type = packet_in.ua_type; + + mav_array_memcpy(packet1.id_or_mac, packet_in.id_or_mac, sizeof(uint8_t)*20); + mav_array_memcpy(packet1.uas_id, packet_in.uas_id, sizeof(uint8_t)*20); + +#ifdef MAVLINK_STATUS_FLAG_OUT_MAVLINK1 + if (status->flags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) { + // cope with extensions + memset(MAVLINK_MSG_ID_OPEN_DRONE_ID_BASIC_ID_MIN_LEN + (char *)&packet1, 0, sizeof(packet1)-MAVLINK_MSG_ID_OPEN_DRONE_ID_BASIC_ID_MIN_LEN); + } +#endif + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_open_drone_id_basic_id_encode(system_id, component_id, &msg, &packet1); + mavlink_msg_open_drone_id_basic_id_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_open_drone_id_basic_id_pack(system_id, component_id, &msg , packet1.target_system , packet1.target_component , packet1.id_or_mac , packet1.id_type , packet1.ua_type , packet1.uas_id ); + mavlink_msg_open_drone_id_basic_id_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_open_drone_id_basic_id_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.target_system , packet1.target_component , packet1.id_or_mac , packet1.id_type , packet1.ua_type , packet1.uas_id ); + mavlink_msg_open_drone_id_basic_id_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_to_send_buffer(buffer, &msg); + for (i=0; iflags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) && MAVLINK_MSG_ID_OPEN_DRONE_ID_LOCATION >= 256) { + return; + } +#endif + mavlink_message_t msg; + uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; + uint16_t i; + mavlink_open_drone_id_location_t packet_in = { + 963497464,963497672,73.0,101.0,129.0,157.0,18483,18587,18691,223,34,{ 101, 102, 103, 104, 105, 106, 107, 108, 109, 110, 111, 112, 113, 114, 115, 116, 117, 118, 119, 120 },161,228,39,106,173,240,51 + }; + mavlink_open_drone_id_location_t packet1, packet2; + memset(&packet1, 0, sizeof(packet1)); + packet1.latitude = packet_in.latitude; + packet1.longitude = packet_in.longitude; + packet1.altitude_barometric = packet_in.altitude_barometric; + packet1.altitude_geodetic = packet_in.altitude_geodetic; + packet1.height = packet_in.height; + packet1.timestamp = packet_in.timestamp; + packet1.direction = packet_in.direction; + packet1.speed_horizontal = packet_in.speed_horizontal; + packet1.speed_vertical = packet_in.speed_vertical; + packet1.target_system = packet_in.target_system; + packet1.target_component = packet_in.target_component; + packet1.status = packet_in.status; + packet1.height_reference = packet_in.height_reference; + packet1.horizontal_accuracy = packet_in.horizontal_accuracy; + packet1.vertical_accuracy = packet_in.vertical_accuracy; + packet1.barometer_accuracy = packet_in.barometer_accuracy; + packet1.speed_accuracy = packet_in.speed_accuracy; + packet1.timestamp_accuracy = packet_in.timestamp_accuracy; + + mav_array_memcpy(packet1.id_or_mac, packet_in.id_or_mac, sizeof(uint8_t)*20); + +#ifdef MAVLINK_STATUS_FLAG_OUT_MAVLINK1 + if (status->flags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) { + // cope with extensions + memset(MAVLINK_MSG_ID_OPEN_DRONE_ID_LOCATION_MIN_LEN + (char *)&packet1, 0, sizeof(packet1)-MAVLINK_MSG_ID_OPEN_DRONE_ID_LOCATION_MIN_LEN); + } +#endif + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_open_drone_id_location_encode(system_id, component_id, &msg, &packet1); + mavlink_msg_open_drone_id_location_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_open_drone_id_location_pack(system_id, component_id, &msg , packet1.target_system , packet1.target_component , packet1.id_or_mac , packet1.status , packet1.direction , packet1.speed_horizontal , packet1.speed_vertical , packet1.latitude , packet1.longitude , packet1.altitude_barometric , packet1.altitude_geodetic , packet1.height_reference , packet1.height , packet1.horizontal_accuracy , packet1.vertical_accuracy , packet1.barometer_accuracy , packet1.speed_accuracy , packet1.timestamp , packet1.timestamp_accuracy ); + mavlink_msg_open_drone_id_location_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_open_drone_id_location_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.target_system , packet1.target_component , packet1.id_or_mac , packet1.status , packet1.direction , packet1.speed_horizontal , packet1.speed_vertical , packet1.latitude , packet1.longitude , packet1.altitude_barometric , packet1.altitude_geodetic , packet1.height_reference , packet1.height , packet1.horizontal_accuracy , packet1.vertical_accuracy , packet1.barometer_accuracy , packet1.speed_accuracy , packet1.timestamp , packet1.timestamp_accuracy ); + mavlink_msg_open_drone_id_location_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_to_send_buffer(buffer, &msg); + for (i=0; iflags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) && MAVLINK_MSG_ID_OPEN_DRONE_ID_AUTHENTICATION >= 256) { + return; + } +#endif + mavlink_message_t msg; + uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; + uint16_t i; + mavlink_open_drone_id_authentication_t packet_in = { + 963497464,17,84,{ 151, 152, 153, 154, 155, 156, 157, 158, 159, 160, 161, 162, 163, 164, 165, 166, 167, 168, 169, 170 },211,22,89,156,{ 223, 224, 225, 226, 227, 228, 229, 230, 231, 232, 233, 234, 235, 236, 237, 238, 239, 240, 241, 242, 243, 244, 245 } + }; + mavlink_open_drone_id_authentication_t packet1, packet2; + memset(&packet1, 0, sizeof(packet1)); + packet1.timestamp = packet_in.timestamp; + packet1.target_system = packet_in.target_system; + packet1.target_component = packet_in.target_component; + packet1.authentication_type = packet_in.authentication_type; + packet1.data_page = packet_in.data_page; + packet1.page_count = packet_in.page_count; + packet1.length = packet_in.length; + + mav_array_memcpy(packet1.id_or_mac, packet_in.id_or_mac, sizeof(uint8_t)*20); + mav_array_memcpy(packet1.authentication_data, packet_in.authentication_data, sizeof(uint8_t)*23); + +#ifdef MAVLINK_STATUS_FLAG_OUT_MAVLINK1 + if (status->flags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) { + // cope with extensions + memset(MAVLINK_MSG_ID_OPEN_DRONE_ID_AUTHENTICATION_MIN_LEN + (char *)&packet1, 0, sizeof(packet1)-MAVLINK_MSG_ID_OPEN_DRONE_ID_AUTHENTICATION_MIN_LEN); + } +#endif + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_open_drone_id_authentication_encode(system_id, component_id, &msg, &packet1); + mavlink_msg_open_drone_id_authentication_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_open_drone_id_authentication_pack(system_id, component_id, &msg , packet1.target_system , packet1.target_component , packet1.id_or_mac , packet1.authentication_type , packet1.data_page , packet1.page_count , packet1.length , packet1.timestamp , packet1.authentication_data ); + mavlink_msg_open_drone_id_authentication_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_open_drone_id_authentication_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.target_system , packet1.target_component , packet1.id_or_mac , packet1.authentication_type , packet1.data_page , packet1.page_count , packet1.length , packet1.timestamp , packet1.authentication_data ); + mavlink_msg_open_drone_id_authentication_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_to_send_buffer(buffer, &msg); + for (i=0; iflags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) && MAVLINK_MSG_ID_OPEN_DRONE_ID_SELF_ID >= 256) { + return; + } +#endif + mavlink_message_t msg; + uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; + uint16_t i; + mavlink_open_drone_id_self_id_t packet_in = { + 5,72,{ 139, 140, 141, 142, 143, 144, 145, 146, 147, 148, 149, 150, 151, 152, 153, 154, 155, 156, 157, 158 },199,"XYZABCDEFGHIJKLMNOPQRS" + }; + mavlink_open_drone_id_self_id_t packet1, packet2; + memset(&packet1, 0, sizeof(packet1)); + packet1.target_system = packet_in.target_system; + packet1.target_component = packet_in.target_component; + packet1.description_type = packet_in.description_type; + + mav_array_memcpy(packet1.id_or_mac, packet_in.id_or_mac, sizeof(uint8_t)*20); + mav_array_memcpy(packet1.description, packet_in.description, sizeof(char)*23); + +#ifdef MAVLINK_STATUS_FLAG_OUT_MAVLINK1 + if (status->flags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) { + // cope with extensions + memset(MAVLINK_MSG_ID_OPEN_DRONE_ID_SELF_ID_MIN_LEN + (char *)&packet1, 0, sizeof(packet1)-MAVLINK_MSG_ID_OPEN_DRONE_ID_SELF_ID_MIN_LEN); + } +#endif + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_open_drone_id_self_id_encode(system_id, component_id, &msg, &packet1); + mavlink_msg_open_drone_id_self_id_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_open_drone_id_self_id_pack(system_id, component_id, &msg , packet1.target_system , packet1.target_component , packet1.id_or_mac , packet1.description_type , packet1.description ); + mavlink_msg_open_drone_id_self_id_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_open_drone_id_self_id_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.target_system , packet1.target_component , packet1.id_or_mac , packet1.description_type , packet1.description ); + mavlink_msg_open_drone_id_self_id_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_to_send_buffer(buffer, &msg); + for (i=0; iflags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) && MAVLINK_MSG_ID_OPEN_DRONE_ID_SYSTEM >= 256) { + return; + } +#endif + mavlink_message_t msg; + uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; + uint16_t i; + mavlink_open_drone_id_system_t packet_in = { + 963497464,963497672,73.0,101.0,18067,18171,65,132,{ 199, 200, 201, 202, 203, 204, 205, 206, 207, 208, 209, 210, 211, 212, 213, 214, 215, 216, 217, 218 },3,70,137,204 + }; + mavlink_open_drone_id_system_t packet1, packet2; + memset(&packet1, 0, sizeof(packet1)); + packet1.operator_latitude = packet_in.operator_latitude; + packet1.operator_longitude = packet_in.operator_longitude; + packet1.area_ceiling = packet_in.area_ceiling; + packet1.area_floor = packet_in.area_floor; + packet1.area_count = packet_in.area_count; + packet1.area_radius = packet_in.area_radius; + packet1.target_system = packet_in.target_system; + packet1.target_component = packet_in.target_component; + packet1.operator_location_type = packet_in.operator_location_type; + packet1.classification_type = packet_in.classification_type; + packet1.category_eu = packet_in.category_eu; + packet1.class_eu = packet_in.class_eu; + + mav_array_memcpy(packet1.id_or_mac, packet_in.id_or_mac, sizeof(uint8_t)*20); + +#ifdef MAVLINK_STATUS_FLAG_OUT_MAVLINK1 + if (status->flags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) { + // cope with extensions + memset(MAVLINK_MSG_ID_OPEN_DRONE_ID_SYSTEM_MIN_LEN + (char *)&packet1, 0, sizeof(packet1)-MAVLINK_MSG_ID_OPEN_DRONE_ID_SYSTEM_MIN_LEN); + } +#endif + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_open_drone_id_system_encode(system_id, component_id, &msg, &packet1); + mavlink_msg_open_drone_id_system_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_open_drone_id_system_pack(system_id, component_id, &msg , packet1.target_system , packet1.target_component , packet1.id_or_mac , packet1.operator_location_type , packet1.classification_type , packet1.operator_latitude , packet1.operator_longitude , packet1.area_count , packet1.area_radius , packet1.area_ceiling , packet1.area_floor , packet1.category_eu , packet1.class_eu ); + mavlink_msg_open_drone_id_system_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_open_drone_id_system_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.target_system , packet1.target_component , packet1.id_or_mac , packet1.operator_location_type , packet1.classification_type , packet1.operator_latitude , packet1.operator_longitude , packet1.area_count , packet1.area_radius , packet1.area_ceiling , packet1.area_floor , packet1.category_eu , packet1.class_eu ); + mavlink_msg_open_drone_id_system_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_to_send_buffer(buffer, &msg); + for (i=0; iflags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) && MAVLINK_MSG_ID_OPEN_DRONE_ID_OPERATOR_ID >= 256) { + return; + } +#endif + mavlink_message_t msg; + uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; + uint16_t i; + mavlink_open_drone_id_operator_id_t packet_in = { + 5,72,{ 139, 140, 141, 142, 143, 144, 145, 146, 147, 148, 149, 150, 151, 152, 153, 154, 155, 156, 157, 158 },199,"XYZABCDEFGHIJKLMNOP" + }; + mavlink_open_drone_id_operator_id_t packet1, packet2; + memset(&packet1, 0, sizeof(packet1)); + packet1.target_system = packet_in.target_system; + packet1.target_component = packet_in.target_component; + packet1.operator_id_type = packet_in.operator_id_type; + + mav_array_memcpy(packet1.id_or_mac, packet_in.id_or_mac, sizeof(uint8_t)*20); + mav_array_memcpy(packet1.operator_id, packet_in.operator_id, sizeof(char)*20); + +#ifdef MAVLINK_STATUS_FLAG_OUT_MAVLINK1 + if (status->flags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) { + // cope with extensions + memset(MAVLINK_MSG_ID_OPEN_DRONE_ID_OPERATOR_ID_MIN_LEN + (char *)&packet1, 0, sizeof(packet1)-MAVLINK_MSG_ID_OPEN_DRONE_ID_OPERATOR_ID_MIN_LEN); + } +#endif + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_open_drone_id_operator_id_encode(system_id, component_id, &msg, &packet1); + mavlink_msg_open_drone_id_operator_id_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_open_drone_id_operator_id_pack(system_id, component_id, &msg , packet1.target_system , packet1.target_component , packet1.id_or_mac , packet1.operator_id_type , packet1.operator_id ); + mavlink_msg_open_drone_id_operator_id_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_open_drone_id_operator_id_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.target_system , packet1.target_component , packet1.id_or_mac , packet1.operator_id_type , packet1.operator_id ); + mavlink_msg_open_drone_id_operator_id_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_to_send_buffer(buffer, &msg); + for (i=0; iflags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) && MAVLINK_MSG_ID_OPEN_DRONE_ID_MESSAGE_PACK >= 256) { + return; + } +#endif + mavlink_message_t msg; + uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; + uint16_t i; + mavlink_open_drone_id_message_pack_t packet_in = { + 5,72,139,206,{ 17, 18, 19, 20, 21, 22, 23, 24, 25, 26, 27, 28, 29, 30, 31, 32, 33, 34, 35, 36, 37, 38, 39, 40, 41, 42, 43, 44, 45, 46, 47, 48, 49, 50, 51, 52, 53, 54, 55, 56, 57, 58, 59, 60, 61, 62, 63, 64, 65, 66, 67, 68, 69, 70, 71, 72, 73, 74, 75, 76, 77, 78, 79, 80, 81, 82, 83, 84, 85, 86, 87, 88, 89, 90, 91, 92, 93, 94, 95, 96, 97, 98, 99, 100, 101, 102, 103, 104, 105, 106, 107, 108, 109, 110, 111, 112, 113, 114, 115, 116, 117, 118, 119, 120, 121, 122, 123, 124, 125, 126, 127, 128, 129, 130, 131, 132, 133, 134, 135, 136, 137, 138, 139, 140, 141, 142, 143, 144, 145, 146, 147, 148, 149, 150, 151, 152, 153, 154, 155, 156, 157, 158, 159, 160, 161, 162, 163, 164, 165, 166, 167, 168, 169, 170, 171, 172, 173, 174, 175, 176, 177, 178, 179, 180, 181, 182, 183, 184, 185, 186, 187, 188, 189, 190, 191, 192, 193, 194, 195, 196, 197, 198, 199, 200, 201, 202, 203, 204, 205, 206, 207, 208, 209, 210, 211, 212, 213, 214, 215, 216, 217, 218, 219, 220, 221, 222, 223, 224, 225, 226, 227, 228, 229, 230, 231, 232, 233, 234, 235, 236, 237, 238, 239, 240, 241, 242, 243, 244, 245, 246, 247, 248, 249, 250, 251, 252, 253, 254, 255, 0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10 } + }; + mavlink_open_drone_id_message_pack_t packet1, packet2; + memset(&packet1, 0, sizeof(packet1)); + packet1.target_system = packet_in.target_system; + packet1.target_component = packet_in.target_component; + packet1.single_message_size = packet_in.single_message_size; + packet1.msg_pack_size = packet_in.msg_pack_size; + + mav_array_memcpy(packet1.messages, packet_in.messages, sizeof(uint8_t)*250); + +#ifdef MAVLINK_STATUS_FLAG_OUT_MAVLINK1 + if (status->flags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) { + // cope with extensions + memset(MAVLINK_MSG_ID_OPEN_DRONE_ID_MESSAGE_PACK_MIN_LEN + (char *)&packet1, 0, sizeof(packet1)-MAVLINK_MSG_ID_OPEN_DRONE_ID_MESSAGE_PACK_MIN_LEN); + } +#endif + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_open_drone_id_message_pack_encode(system_id, component_id, &msg, &packet1); + mavlink_msg_open_drone_id_message_pack_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_open_drone_id_message_pack_pack(system_id, component_id, &msg , packet1.target_system , packet1.target_component , packet1.single_message_size , packet1.msg_pack_size , packet1.messages ); + mavlink_msg_open_drone_id_message_pack_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_open_drone_id_message_pack_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.target_system , packet1.target_component , packet1.single_message_size , packet1.msg_pack_size , packet1.messages ); + mavlink_msg_open_drone_id_message_pack_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_to_send_buffer(buffer, &msg); + for (i=0; i mavlink_message_info[mid].msgid) { + low = mid; + continue; + } + low = mid; + break; + } + if (mavlink_message_info[low].msgid == msgid) { + return &mavlink_message_info[low]; + } + return NULL; +} + +/* + return the message_info struct for a message +*/ +MAVLINK_HELPER const mavlink_message_info_t *mavlink_get_message_info(const mavlink_message_t *msg) +{ + return mavlink_get_message_info_by_id(msg->msgid); +} + +/* + return the message_info struct for a message +*/ +MAVLINK_HELPER const mavlink_message_info_t *mavlink_get_message_info_by_name(const char *name) +{ + static const struct { const char *name; uint32_t msgid; } mavlink_message_names[] = MAVLINK_MESSAGE_NAMES; + /* + use a bisection search to find the right entry. A perfect hash may be better + Note that this assumes the table is sorted with primary key name + */ + uint32_t low=0, high=sizeof(mavlink_message_names)/sizeof(mavlink_message_names[0]); + while (low < high) { + uint32_t mid = (low+1+high)/2; + int cmp = strcmp(mavlink_message_names[mid].name, name); + if (cmp == 0) { + return mavlink_get_message_info_by_id(mavlink_message_names[mid].msgid); + } + if (cmp > 0) { + high = mid-1; + } else { + low = mid; + } + } + return NULL; +} +#endif // MAVLINK_USE_MESSAGE_INFO + + diff --git a/lib/main/MAVLink/mavlink_helpers.h b/lib/main/MAVLink/mavlink_helpers.h index 2f30cb7ad7..fed04d6885 100755 --- a/lib/main/MAVLink/mavlink_helpers.h +++ b/lib/main/MAVLink/mavlink_helpers.h @@ -1,15 +1,21 @@ -#ifndef _MAVLINK_HELPERS_H_ -#define _MAVLINK_HELPERS_H_ +#pragma once #include "string.h" #include "checksum.h" #include "mavlink_types.h" #include "mavlink_conversions.h" +#include #ifndef MAVLINK_HELPER #define MAVLINK_HELPER #endif +#include "mavlink_sha256.h" + +#ifdef MAVLINK_USE_CXX_NAMESPACE +namespace mavlink { +#endif + /* * Internal function to give access to the channel status for each channel */ @@ -41,7 +47,15 @@ MAVLINK_HELPER mavlink_message_t* mavlink_get_channel_buffer(uint8_t chan) #endif return &m_mavlink_buffer[chan]; } -#endif +#endif // MAVLINK_GET_CHANNEL_BUFFER + +/* Enable this option to check the length of each message. + This allows invalid messages to be caught much sooner. Use if the transmission + medium is prone to missing (or extra) characters (e.g. a radio that fades in + and out). Only use if the channel will only contain messages types listed in + the headers. +*/ +//#define MAVLINK_CHECK_MESSAGE_LENGTH /** * @brief Reset the status of a channel. @@ -52,6 +66,137 @@ MAVLINK_HELPER void mavlink_reset_channel_status(uint8_t chan) status->parse_state = MAVLINK_PARSE_STATE_IDLE; } +/** + * @brief create a signature block for a packet + */ +MAVLINK_HELPER uint8_t mavlink_sign_packet(mavlink_signing_t *signing, + uint8_t signature[MAVLINK_SIGNATURE_BLOCK_LEN], + const uint8_t *header, uint8_t header_len, + const uint8_t *packet, uint8_t packet_len, + const uint8_t crc[2]) +{ + mavlink_sha256_ctx ctx; + union { + uint64_t t64; + uint8_t t8[8]; + } tstamp; + if (signing == NULL || !(signing->flags & MAVLINK_SIGNING_FLAG_SIGN_OUTGOING)) { + return 0; + } + signature[0] = signing->link_id; + tstamp.t64 = signing->timestamp; + memcpy(&signature[1], tstamp.t8, 6); + signing->timestamp++; + + mavlink_sha256_init(&ctx); + mavlink_sha256_update(&ctx, signing->secret_key, sizeof(signing->secret_key)); + mavlink_sha256_update(&ctx, header, header_len); + mavlink_sha256_update(&ctx, packet, packet_len); + mavlink_sha256_update(&ctx, crc, 2); + mavlink_sha256_update(&ctx, signature, 7); + mavlink_sha256_final_48(&ctx, &signature[7]); + + return MAVLINK_SIGNATURE_BLOCK_LEN; +} + +/** + * @brief Trim payload of any trailing zero-populated bytes (MAVLink 2 only). + * + * @param payload Serialised payload buffer. + * @param length Length of full-width payload buffer. + * @return Length of payload after zero-filled bytes are trimmed. + */ +MAVLINK_HELPER uint8_t _mav_trim_payload(const char *payload, uint8_t length) +{ + while (length > 1 && payload[length-1] == 0) { + length--; + } + return length; +} + +/** + * @brief check a signature block for a packet + */ +MAVLINK_HELPER bool mavlink_signature_check(mavlink_signing_t *signing, + mavlink_signing_streams_t *signing_streams, + const mavlink_message_t *msg) +{ + if (signing == NULL) { + return true; + } + const uint8_t *p = (const uint8_t *)&msg->magic; + const uint8_t *psig = msg->signature; + const uint8_t *incoming_signature = psig+7; + mavlink_sha256_ctx ctx; + uint8_t signature[6]; + uint16_t i; + + mavlink_sha256_init(&ctx); + mavlink_sha256_update(&ctx, signing->secret_key, sizeof(signing->secret_key)); + mavlink_sha256_update(&ctx, p, MAVLINK_CORE_HEADER_LEN+1+msg->len); + mavlink_sha256_update(&ctx, msg->ck, 2); + mavlink_sha256_update(&ctx, psig, 1+6); + mavlink_sha256_final_48(&ctx, signature); + if (memcmp(signature, incoming_signature, 6) != 0) { + return false; + } + + // now check timestamp + union tstamp { + uint64_t t64; + uint8_t t8[8]; + } tstamp; + uint8_t link_id = psig[0]; + tstamp.t64 = 0; + memcpy(tstamp.t8, psig+1, 6); + + if (signing_streams == NULL) { + return false; + } + + // find stream + for (i=0; inum_signing_streams; i++) { + if (msg->sysid == signing_streams->stream[i].sysid && + msg->compid == signing_streams->stream[i].compid && + link_id == signing_streams->stream[i].link_id) { + break; + } + } + if (i == signing_streams->num_signing_streams) { + if (signing_streams->num_signing_streams >= MAVLINK_MAX_SIGNING_STREAMS) { + // over max number of streams + return false; + } + // new stream. Only accept if timestamp is not more than 1 minute old + if (tstamp.t64 + 6000*1000UL < signing->timestamp) { + return false; + } + // add new stream + signing_streams->stream[i].sysid = msg->sysid; + signing_streams->stream[i].compid = msg->compid; + signing_streams->stream[i].link_id = link_id; + signing_streams->num_signing_streams++; + } else { + union tstamp last_tstamp; + last_tstamp.t64 = 0; + memcpy(last_tstamp.t8, signing_streams->stream[i].timestamp_bytes, 6); + if (tstamp.t64 <= last_tstamp.t64) { + // repeating old timestamp + return false; + } + } + + // remember last timestamp + memcpy(signing_streams->stream[i].timestamp_bytes, psig+1, 6); + + // our next timestamp must be at least this timestamp + if (tstamp.t64 > signing->timestamp) { + signing->timestamp = tstamp.t64; + } + return true; +} + + /** * @brief Finalize a MAVLink message with channel assignment * @@ -64,52 +209,89 @@ MAVLINK_HELPER void mavlink_reset_channel_status(uint8_t chan) * @param system_id Id of the sending (this) system, 1-127 * @param length Message length */ -#if MAVLINK_CRC_EXTRA -MAVLINK_HELPER uint16_t mavlink_finalize_message_chan(mavlink_message_t* msg, uint8_t system_id, uint8_t component_id, - uint8_t chan, uint8_t min_length, uint8_t length, uint8_t crc_extra) -#else -MAVLINK_HELPER uint16_t mavlink_finalize_message_chan(mavlink_message_t* msg, uint8_t system_id, uint8_t component_id, - uint8_t chan, uint8_t length) -#endif +MAVLINK_HELPER uint16_t mavlink_finalize_message_buffer(mavlink_message_t* msg, uint8_t system_id, uint8_t component_id, + mavlink_status_t* status, uint8_t min_length, uint8_t length, uint8_t crc_extra) { - // This is only used for the v2 protocol and we silence it here - (void)min_length; - // This code part is the same for all messages; - msg->magic = MAVLINK_STX; - msg->len = length; + bool mavlink1 = (status->flags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) != 0; + bool signing = (!mavlink1) && status->signing && (status->signing->flags & MAVLINK_SIGNING_FLAG_SIGN_OUTGOING); + uint8_t signature_len = signing? MAVLINK_SIGNATURE_BLOCK_LEN : 0; + uint8_t header_len = MAVLINK_CORE_HEADER_LEN+1; + uint8_t buf[MAVLINK_CORE_HEADER_LEN+1]; + if (mavlink1) { + msg->magic = MAVLINK_STX_MAVLINK1; + header_len = MAVLINK_CORE_HEADER_MAVLINK1_LEN+1; + } else { + msg->magic = MAVLINK_STX; + } + msg->len = mavlink1?min_length:_mav_trim_payload(_MAV_PAYLOAD(msg), length); msg->sysid = system_id; msg->compid = component_id; - // One sequence number per channel - msg->seq = mavlink_get_channel_status(chan)->current_tx_seq; - mavlink_get_channel_status(chan)->current_tx_seq = mavlink_get_channel_status(chan)->current_tx_seq+1; - msg->checksum = crc_calculate(((const uint8_t*)(msg)) + 3, MAVLINK_CORE_HEADER_LEN); - crc_accumulate_buffer(&msg->checksum, _MAV_PAYLOAD(msg), msg->len); -#if MAVLINK_CRC_EXTRA - crc_accumulate(crc_extra, &msg->checksum); -#endif - mavlink_ck_a(msg) = (uint8_t)(msg->checksum & 0xFF); - mavlink_ck_b(msg) = (uint8_t)(msg->checksum >> 8); + msg->incompat_flags = 0; + if (signing) { + msg->incompat_flags |= MAVLINK_IFLAG_SIGNED; + } + msg->compat_flags = 0; + msg->seq = status->current_tx_seq; + status->current_tx_seq = status->current_tx_seq + 1; - return length + MAVLINK_NUM_NON_PAYLOAD_BYTES; + // form the header as a byte array for the crc + buf[0] = msg->magic; + buf[1] = msg->len; + if (mavlink1) { + buf[2] = msg->seq; + buf[3] = msg->sysid; + buf[4] = msg->compid; + buf[5] = msg->msgid & 0xFF; + } else { + buf[2] = msg->incompat_flags; + buf[3] = msg->compat_flags; + buf[4] = msg->seq; + buf[5] = msg->sysid; + buf[6] = msg->compid; + buf[7] = msg->msgid & 0xFF; + buf[8] = (msg->msgid >> 8) & 0xFF; + buf[9] = (msg->msgid >> 16) & 0xFF; + } + + uint16_t checksum = crc_calculate(&buf[1], header_len-1); + crc_accumulate_buffer(&checksum, _MAV_PAYLOAD(msg), msg->len); + crc_accumulate(crc_extra, &checksum); + mavlink_ck_a(msg) = (uint8_t)(checksum & 0xFF); + mavlink_ck_b(msg) = (uint8_t)(checksum >> 8); + + msg->checksum = checksum; + + if (signing) { + mavlink_sign_packet(status->signing, + msg->signature, + (const uint8_t *)buf, header_len, + (const uint8_t *)_MAV_PAYLOAD(msg), msg->len, + (const uint8_t *)_MAV_PAYLOAD(msg)+(uint16_t)msg->len); + } + + return msg->len + header_len + 2 + signature_len; } +MAVLINK_HELPER uint16_t mavlink_finalize_message_chan(mavlink_message_t* msg, uint8_t system_id, uint8_t component_id, + uint8_t chan, uint8_t min_length, uint8_t length, uint8_t crc_extra) +{ + mavlink_status_t *status = mavlink_get_channel_status(chan); + return mavlink_finalize_message_buffer(msg, system_id, component_id, status, min_length, length, crc_extra); +} /** * @brief Finalize a MAVLink message with MAVLINK_COMM_0 as default channel */ -#if MAVLINK_CRC_EXTRA MAVLINK_HELPER uint16_t mavlink_finalize_message(mavlink_message_t* msg, uint8_t system_id, uint8_t component_id, uint8_t min_length, uint8_t length, uint8_t crc_extra) { return mavlink_finalize_message_chan(msg, system_id, component_id, MAVLINK_COMM_0, min_length, length, crc_extra); } -#else -MAVLINK_HELPER uint16_t mavlink_finalize_message(mavlink_message_t* msg, uint8_t system_id, uint8_t component_id, - uint8_t length) + +static inline void _mav_parse_error(mavlink_status_t *status) { - return mavlink_finalize_message_chan(msg, system_id, component_id, MAVLINK_COMM_0, length); + status->parse_error++; } -#endif #ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS MAVLINK_HELPER void _mavlink_send_uart(mavlink_channel_t chan, const char *buf, uint16_t len); @@ -117,42 +299,78 @@ MAVLINK_HELPER void _mavlink_send_uart(mavlink_channel_t chan, const char *buf, /** * @brief Finalize a MAVLink message with channel assignment and send */ -#if MAVLINK_CRC_EXTRA -MAVLINK_HELPER void _mav_finalize_message_chan_send(mavlink_channel_t chan, uint8_t msgid, const char *packet, +MAVLINK_HELPER void _mav_finalize_message_chan_send(mavlink_channel_t chan, uint32_t msgid, + const char *packet, uint8_t min_length, uint8_t length, uint8_t crc_extra) -#else -MAVLINK_HELPER void _mav_finalize_message_chan_send(mavlink_channel_t chan, uint8_t msgid, const char *packet, uint8_t length) -#endif { uint16_t checksum; uint8_t buf[MAVLINK_NUM_HEADER_BYTES]; uint8_t ck[2]; mavlink_status_t *status = mavlink_get_channel_status(chan); - buf[0] = MAVLINK_STX; - buf[1] = length; - buf[2] = status->current_tx_seq; - buf[3] = mavlink_system.sysid; - buf[4] = mavlink_system.compid; - buf[5] = msgid; + uint8_t header_len = MAVLINK_CORE_HEADER_LEN; + uint8_t signature_len = 0; + uint8_t signature[MAVLINK_SIGNATURE_BLOCK_LEN]; + bool mavlink1 = (status->flags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) != 0; + bool signing = (!mavlink1) && status->signing && (status->signing->flags & MAVLINK_SIGNING_FLAG_SIGN_OUTGOING); + + if (mavlink1) { + length = min_length; + if (msgid > 255) { + // can't send 16 bit messages + _mav_parse_error(status); + return; + } + header_len = MAVLINK_CORE_HEADER_MAVLINK1_LEN; + buf[0] = MAVLINK_STX_MAVLINK1; + buf[1] = length; + buf[2] = status->current_tx_seq; + buf[3] = mavlink_system.sysid; + buf[4] = mavlink_system.compid; + buf[5] = msgid & 0xFF; + } else { + uint8_t incompat_flags = 0; + if (signing) { + incompat_flags |= MAVLINK_IFLAG_SIGNED; + } + length = _mav_trim_payload(packet, length); + buf[0] = MAVLINK_STX; + buf[1] = length; + buf[2] = incompat_flags; + buf[3] = 0; // compat_flags + buf[4] = status->current_tx_seq; + buf[5] = mavlink_system.sysid; + buf[6] = mavlink_system.compid; + buf[7] = msgid & 0xFF; + buf[8] = (msgid >> 8) & 0xFF; + buf[9] = (msgid >> 16) & 0xFF; + } status->current_tx_seq++; - checksum = crc_calculate((const uint8_t*)&buf[1], MAVLINK_CORE_HEADER_LEN); + checksum = crc_calculate((const uint8_t*)&buf[1], header_len); crc_accumulate_buffer(&checksum, packet, length); -#if MAVLINK_CRC_EXTRA crc_accumulate(crc_extra, &checksum); -#endif ck[0] = (uint8_t)(checksum & 0xFF); ck[1] = (uint8_t)(checksum >> 8); - MAVLINK_START_UART_SEND(chan, MAVLINK_NUM_NON_PAYLOAD_BYTES + (uint16_t)length); - _mavlink_send_uart(chan, (const char *)buf, MAVLINK_NUM_HEADER_BYTES); + if (signing) { + // possibly add a signature + signature_len = mavlink_sign_packet(status->signing, signature, buf, header_len+1, + (const uint8_t *)packet, length, ck); + } + + MAVLINK_START_UART_SEND(chan, header_len + 3 + (uint16_t)length + (uint16_t)signature_len); + _mavlink_send_uart(chan, (const char *)buf, header_len+1); _mavlink_send_uart(chan, packet, length); _mavlink_send_uart(chan, (const char *)ck, 2); - MAVLINK_END_UART_SEND(chan, MAVLINK_NUM_NON_PAYLOAD_BYTES + (uint16_t)length); + if (signature_len != 0) { + _mavlink_send_uart(chan, (const char *)signature, signature_len); + } + MAVLINK_END_UART_SEND(chan, header_len + 3 + (uint16_t)length + (uint16_t)signature_len); } /** * @brief re-send a message over a uart channel * this is more stack efficient than re-marshalling the message + * If the message is signed then the original signature is also sent */ MAVLINK_HELPER void _mavlink_resend_uart(mavlink_channel_t chan, const mavlink_message_t *msg) { @@ -162,27 +380,92 @@ MAVLINK_HELPER void _mavlink_resend_uart(mavlink_channel_t chan, const mavlink_m ck[1] = (uint8_t)(msg->checksum >> 8); // XXX use the right sequence here - MAVLINK_START_UART_SEND(chan, MAVLINK_NUM_NON_PAYLOAD_BYTES + msg->len); - _mavlink_send_uart(chan, (const char *)&msg->magic, MAVLINK_NUM_HEADER_BYTES); + uint8_t header_len; + uint8_t signature_len; + + if (msg->magic == MAVLINK_STX_MAVLINK1) { + header_len = MAVLINK_CORE_HEADER_MAVLINK1_LEN + 1; + signature_len = 0; + MAVLINK_START_UART_SEND(chan, header_len + msg->len + 2 + signature_len); + // we can't send the structure directly as it has extra mavlink2 elements in it + uint8_t buf[MAVLINK_CORE_HEADER_MAVLINK1_LEN + 1]; + buf[0] = msg->magic; + buf[1] = msg->len; + buf[2] = msg->seq; + buf[3] = msg->sysid; + buf[4] = msg->compid; + buf[5] = msg->msgid & 0xFF; + _mavlink_send_uart(chan, (const char*)buf, header_len); + } else { + header_len = MAVLINK_CORE_HEADER_LEN + 1; + signature_len = (msg->incompat_flags & MAVLINK_IFLAG_SIGNED)?MAVLINK_SIGNATURE_BLOCK_LEN:0; + MAVLINK_START_UART_SEND(chan, header_len + msg->len + 2 + signature_len); + uint8_t buf[MAVLINK_CORE_HEADER_LEN + 1]; + buf[0] = msg->magic; + buf[1] = msg->len; + buf[2] = msg->incompat_flags; + buf[3] = msg->compat_flags; + buf[4] = msg->seq; + buf[5] = msg->sysid; + buf[6] = msg->compid; + buf[7] = msg->msgid & 0xFF; + buf[8] = (msg->msgid >> 8) & 0xFF; + buf[9] = (msg->msgid >> 16) & 0xFF; + _mavlink_send_uart(chan, (const char *)buf, header_len); + } _mavlink_send_uart(chan, _MAV_PAYLOAD(msg), msg->len); _mavlink_send_uart(chan, (const char *)ck, 2); - MAVLINK_END_UART_SEND(chan, MAVLINK_NUM_NON_PAYLOAD_BYTES + msg->len); + if (signature_len != 0) { + _mavlink_send_uart(chan, (const char *)msg->signature, MAVLINK_SIGNATURE_BLOCK_LEN); + } + MAVLINK_END_UART_SEND(chan, header_len + msg->len + 2 + signature_len); } #endif // MAVLINK_USE_CONVENIENCE_FUNCTIONS /** * @brief Pack a message to send it over a serial byte stream */ -MAVLINK_HELPER uint16_t mavlink_msg_to_send_buffer(uint8_t *buffer, const mavlink_message_t *msg) +MAVLINK_HELPER uint16_t mavlink_msg_to_send_buffer(uint8_t *buf, const mavlink_message_t *msg) { - memcpy(buffer, (const uint8_t *)&msg->magic, MAVLINK_NUM_HEADER_BYTES + (uint16_t)msg->len); - - uint8_t *ck = buffer + (MAVLINK_NUM_HEADER_BYTES + (uint16_t)msg->len); - + uint8_t signature_len, header_len; + uint8_t *ck; + uint8_t length = msg->len; + + if (msg->magic == MAVLINK_STX_MAVLINK1) { + signature_len = 0; + header_len = MAVLINK_CORE_HEADER_MAVLINK1_LEN; + buf[0] = msg->magic; + buf[1] = length; + buf[2] = msg->seq; + buf[3] = msg->sysid; + buf[4] = msg->compid; + buf[5] = msg->msgid & 0xFF; + memcpy(&buf[6], _MAV_PAYLOAD(msg), msg->len); + ck = buf + header_len + 1 + (uint16_t)msg->len; + } else { + length = _mav_trim_payload(_MAV_PAYLOAD(msg), length); + header_len = MAVLINK_CORE_HEADER_LEN; + buf[0] = msg->magic; + buf[1] = length; + buf[2] = msg->incompat_flags; + buf[3] = msg->compat_flags; + buf[4] = msg->seq; + buf[5] = msg->sysid; + buf[6] = msg->compid; + buf[7] = msg->msgid & 0xFF; + buf[8] = (msg->msgid >> 8) & 0xFF; + buf[9] = (msg->msgid >> 16) & 0xFF; + memcpy(&buf[10], _MAV_PAYLOAD(msg), length); + ck = buf + header_len + 1 + (uint16_t)length; + signature_len = (msg->incompat_flags & MAVLINK_IFLAG_SIGNED)?MAVLINK_SIGNATURE_BLOCK_LEN:0; + } ck[0] = (uint8_t)(msg->checksum & 0xFF); ck[1] = (uint8_t)(msg->checksum >> 8); + if (signature_len > 0) { + memcpy(&ck[2], msg->signature, signature_len); + } - return MAVLINK_NUM_NON_PAYLOAD_BYTES + (uint16_t)msg->len; + return header_len + 1 + 2 + (uint16_t)length + (uint16_t)signature_len; } union __mavlink_bitfield { @@ -197,12 +480,78 @@ union __mavlink_bitfield { MAVLINK_HELPER void mavlink_start_checksum(mavlink_message_t* msg) { - crc_init(&msg->checksum); + uint16_t crcTmp = 0; + crc_init(&crcTmp); + msg->checksum = crcTmp; } MAVLINK_HELPER void mavlink_update_checksum(mavlink_message_t* msg, uint8_t c) { - crc_accumulate(c, &msg->checksum); + uint16_t checksum = msg->checksum; + crc_accumulate(c, &checksum); + msg->checksum = checksum; +} + +/* + return the crc_entry value for a msgid +*/ +#ifndef MAVLINK_GET_MSG_ENTRY +MAVLINK_HELPER const mavlink_msg_entry_t *mavlink_get_msg_entry(uint32_t msgid) +{ + static const mavlink_msg_entry_t mavlink_message_crcs[] = MAVLINK_MESSAGE_CRCS; + /* + use a bisection search to find the right entry. A perfect hash may be better + Note that this assumes the table is sorted by msgid + */ + uint32_t low=0, high=sizeof(mavlink_message_crcs)/sizeof(mavlink_message_crcs[0]) - 1; + while (low < high) { + uint32_t mid = (low+1+high)/2; + if (msgid < mavlink_message_crcs[mid].msgid) { + high = mid-1; + continue; + } + if (msgid > mavlink_message_crcs[mid].msgid) { + low = mid; + continue; + } + low = mid; + break; + } + if (mavlink_message_crcs[low].msgid != msgid) { + // msgid is not in the table + return NULL; + } + return &mavlink_message_crcs[low]; +} +#endif // MAVLINK_GET_MSG_ENTRY + +/* + return the crc_extra value for a message +*/ +MAVLINK_HELPER uint8_t mavlink_get_crc_extra(const mavlink_message_t *msg) +{ + const mavlink_msg_entry_t *e = mavlink_get_msg_entry(msg->msgid); + return e?e->crc_extra:0; +} + +/* + return the min message length +*/ +#define MAVLINK_HAVE_MIN_MESSAGE_LENGTH +MAVLINK_HELPER uint8_t mavlink_min_message_length(const mavlink_message_t *msg) +{ + const mavlink_msg_entry_t *e = mavlink_get_msg_entry(msg->msgid); + return e?e->min_msg_len:0; +} + +/* + return the max message length (including extensions) +*/ +#define MAVLINK_HAVE_MAX_MESSAGE_LENGTH +MAVLINK_HELPER uint8_t mavlink_max_message_length(const mavlink_message_t *msg) +{ + const mavlink_msg_entry_t *e = mavlink_get_msg_entry(msg->msgid); + return e?e->max_msg_len:0; } /** @@ -211,12 +560,13 @@ MAVLINK_HELPER void mavlink_update_checksum(mavlink_message_t* msg, uint8_t c) * parser in a library that doesn't use any global variables * * @param rxmsg parsing message buffer - * @param status parsing status buffer + * @param status parsing status buffer * @param c The char to parse * * @param r_message NULL if no message could be decoded, otherwise the message data * @param r_mavlink_status if a message was decoded, this is filled with the channel's stats * @return 0 if no message could be decoded, 1 on good message and CRC, 2 on bad CRC + * */ MAVLINK_HELPER uint8_t mavlink_frame_char_buffer(mavlink_message_t* rxmsg, mavlink_status_t* status, @@ -224,30 +574,6 @@ MAVLINK_HELPER uint8_t mavlink_frame_char_buffer(mavlink_message_t* rxmsg, mavlink_message_t* r_message, mavlink_status_t* r_mavlink_status) { - /* - default message crc function. You can override this per-system to - put this data in a different memory segment - */ -#if MAVLINK_CRC_EXTRA -#ifndef MAVLINK_MESSAGE_CRC - static const uint8_t mavlink_message_crcs[256] = MAVLINK_MESSAGE_CRCS; -#define MAVLINK_MESSAGE_CRC(msgid) mavlink_message_crcs[msgid] -#endif -#endif - - /* Enable this option to check the length of each message. - This allows invalid messages to be caught much sooner. Use if the transmission - medium is prone to missing (or extra) characters (e.g. a radio that fades in - and out). Only use if the channel will only contain messages types listed in - the headers. - */ -#ifdef MAVLINK_CHECK_MESSAGE_LENGTH -#ifndef MAVLINK_MESSAGE_LENGTH - static const uint8_t mavlink_message_lengths[256] = MAVLINK_MESSAGE_LENGTHS; -#define MAVLINK_MESSAGE_LENGTH(msgid) mavlink_message_lengths[msgid] -#endif -#endif - int bufferIndex = 0; status->msg_received = MAVLINK_FRAMING_INCOMPLETE; @@ -261,6 +587,14 @@ MAVLINK_HELPER uint8_t mavlink_frame_char_buffer(mavlink_message_t* rxmsg, status->parse_state = MAVLINK_PARSE_STATE_GOT_STX; rxmsg->len = 0; rxmsg->magic = c; + status->flags &= ~MAVLINK_STATUS_FLAG_IN_MAVLINK1; + mavlink_start_checksum(rxmsg); + } else if (c == MAVLINK_STX_MAVLINK1) + { + status->parse_state = MAVLINK_PARSE_STATE_GOT_STX; + rxmsg->len = 0; + rxmsg->magic = c; + status->flags |= MAVLINK_STATUS_FLAG_IN_MAVLINK1; mavlink_start_checksum(rxmsg); } break; @@ -275,7 +609,7 @@ MAVLINK_HELPER uint8_t mavlink_frame_char_buffer(mavlink_message_t* rxmsg, ) { status->buffer_overrun++; - status->parse_error++; + _mav_parse_error(status); status->msg_received = 0; status->parse_state = MAVLINK_PARSE_STATE_IDLE; } @@ -285,16 +619,41 @@ MAVLINK_HELPER uint8_t mavlink_frame_char_buffer(mavlink_message_t* rxmsg, rxmsg->len = c; status->packet_idx = 0; mavlink_update_checksum(rxmsg, c); - status->parse_state = MAVLINK_PARSE_STATE_GOT_LENGTH; + if (status->flags & MAVLINK_STATUS_FLAG_IN_MAVLINK1) { + rxmsg->incompat_flags = 0; + rxmsg->compat_flags = 0; + status->parse_state = MAVLINK_PARSE_STATE_GOT_COMPAT_FLAGS; + } else { + status->parse_state = MAVLINK_PARSE_STATE_GOT_LENGTH; + } } break; case MAVLINK_PARSE_STATE_GOT_LENGTH: + rxmsg->incompat_flags = c; + if ((rxmsg->incompat_flags & ~MAVLINK_IFLAG_MASK) != 0) { + // message includes an incompatible feature flag + _mav_parse_error(status); + status->msg_received = 0; + status->parse_state = MAVLINK_PARSE_STATE_IDLE; + break; + } + mavlink_update_checksum(rxmsg, c); + status->parse_state = MAVLINK_PARSE_STATE_GOT_INCOMPAT_FLAGS; + break; + + case MAVLINK_PARSE_STATE_GOT_INCOMPAT_FLAGS: + rxmsg->compat_flags = c; + mavlink_update_checksum(rxmsg, c); + status->parse_state = MAVLINK_PARSE_STATE_GOT_COMPAT_FLAGS; + break; + + case MAVLINK_PARSE_STATE_GOT_COMPAT_FLAGS: rxmsg->seq = c; mavlink_update_checksum(rxmsg, c); status->parse_state = MAVLINK_PARSE_STATE_GOT_SEQ; break; - + case MAVLINK_PARSE_STATE_GOT_SEQ: rxmsg->sysid = c; mavlink_update_checksum(rxmsg, c); @@ -304,31 +663,57 @@ MAVLINK_HELPER uint8_t mavlink_frame_char_buffer(mavlink_message_t* rxmsg, case MAVLINK_PARSE_STATE_GOT_SYSID: rxmsg->compid = c; mavlink_update_checksum(rxmsg, c); - status->parse_state = MAVLINK_PARSE_STATE_GOT_COMPID; + status->parse_state = MAVLINK_PARSE_STATE_GOT_COMPID; break; case MAVLINK_PARSE_STATE_GOT_COMPID: -#ifdef MAVLINK_CHECK_MESSAGE_LENGTH - if (rxmsg->len != MAVLINK_MESSAGE_LENGTH(c)) - { - status->parse_error++; - status->parse_state = MAVLINK_PARSE_STATE_IDLE; - break; - } -#endif rxmsg->msgid = c; mavlink_update_checksum(rxmsg, c); - if (rxmsg->len == 0) - { - status->parse_state = MAVLINK_PARSE_STATE_GOT_PAYLOAD; - } - else - { - status->parse_state = MAVLINK_PARSE_STATE_GOT_MSGID; + if (status->flags & MAVLINK_STATUS_FLAG_IN_MAVLINK1) { + if(rxmsg->len > 0) { + status->parse_state = MAVLINK_PARSE_STATE_GOT_MSGID3; + } else { + status->parse_state = MAVLINK_PARSE_STATE_GOT_PAYLOAD; + } +#ifdef MAVLINK_CHECK_MESSAGE_LENGTH + if (rxmsg->len < mavlink_min_message_length(rxmsg) || + rxmsg->len > mavlink_max_message_length(rxmsg)) { + _mav_parse_error(status); + status->parse_state = MAVLINK_PARSE_STATE_IDLE; + break; + } +#endif + } else { + status->parse_state = MAVLINK_PARSE_STATE_GOT_MSGID1; } break; - case MAVLINK_PARSE_STATE_GOT_MSGID: + case MAVLINK_PARSE_STATE_GOT_MSGID1: + rxmsg->msgid |= c<<8; + mavlink_update_checksum(rxmsg, c); + status->parse_state = MAVLINK_PARSE_STATE_GOT_MSGID2; + break; + + case MAVLINK_PARSE_STATE_GOT_MSGID2: + rxmsg->msgid |= ((uint32_t)c)<<16; + mavlink_update_checksum(rxmsg, c); + if(rxmsg->len > 0){ + status->parse_state = MAVLINK_PARSE_STATE_GOT_MSGID3; + } else { + status->parse_state = MAVLINK_PARSE_STATE_GOT_PAYLOAD; + } +#ifdef MAVLINK_CHECK_MESSAGE_LENGTH + if (rxmsg->len < mavlink_min_message_length(rxmsg) || + rxmsg->len > mavlink_max_message_length(rxmsg)) + { + _mav_parse_error(status); + status->parse_state = MAVLINK_PARSE_STATE_IDLE; + break; + } +#endif + break; + + case MAVLINK_PARSE_STATE_GOT_MSGID3: _MAV_PAYLOAD_NON_CONST(rxmsg)[status->packet_idx++] = (char)c; mavlink_update_checksum(rxmsg, c); if (status->packet_idx == rxmsg->len) @@ -337,17 +722,23 @@ MAVLINK_HELPER uint8_t mavlink_frame_char_buffer(mavlink_message_t* rxmsg, } break; - case MAVLINK_PARSE_STATE_GOT_PAYLOAD: -#if MAVLINK_CRC_EXTRA - mavlink_update_checksum(rxmsg, MAVLINK_MESSAGE_CRC(rxmsg->msgid)); -#endif + case MAVLINK_PARSE_STATE_GOT_PAYLOAD: { + const mavlink_msg_entry_t *e = mavlink_get_msg_entry(rxmsg->msgid); + uint8_t crc_extra = e?e->crc_extra:0; + mavlink_update_checksum(rxmsg, crc_extra); if (c != (rxmsg->checksum & 0xFF)) { status->parse_state = MAVLINK_PARSE_STATE_GOT_BAD_CRC1; } else { status->parse_state = MAVLINK_PARSE_STATE_GOT_CRC1; } - _MAV_PAYLOAD_NON_CONST(rxmsg)[status->packet_idx] = (char)c; + rxmsg->ck[0] = c; + + // zero-fill the packet to cope with short incoming packets + if (e && status->packet_idx < e->max_msg_len) { + memset(&_MAV_PAYLOAD_NON_CONST(rxmsg)[status->packet_idx], 0, e->max_msg_len - status->packet_idx); + } break; + } case MAVLINK_PARSE_STATE_GOT_CRC1: case MAVLINK_PARSE_STATE_GOT_BAD_CRC1: @@ -357,10 +748,56 @@ MAVLINK_HELPER uint8_t mavlink_frame_char_buffer(mavlink_message_t* rxmsg, } else { // Successfully got message status->msg_received = MAVLINK_FRAMING_OK; - } - status->parse_state = MAVLINK_PARSE_STATE_IDLE; - _MAV_PAYLOAD_NON_CONST(rxmsg)[status->packet_idx+1] = (char)c; - memcpy(r_message, rxmsg, sizeof(mavlink_message_t)); + } + rxmsg->ck[1] = c; + + if (rxmsg->incompat_flags & MAVLINK_IFLAG_SIGNED) { + status->parse_state = MAVLINK_PARSE_STATE_SIGNATURE_WAIT; + status->signature_wait = MAVLINK_SIGNATURE_BLOCK_LEN; + + // If the CRC is already wrong, don't overwrite msg_received, + // otherwise we can end up with garbage flagged as valid. + if (status->msg_received != MAVLINK_FRAMING_BAD_CRC) { + status->msg_received = MAVLINK_FRAMING_INCOMPLETE; + } + } else { + if (status->signing && + (status->signing->accept_unsigned_callback == NULL || + !status->signing->accept_unsigned_callback(status, rxmsg->msgid))) { + + // If the CRC is already wrong, don't overwrite msg_received. + if (status->msg_received != MAVLINK_FRAMING_BAD_CRC) { + status->msg_received = MAVLINK_FRAMING_BAD_SIGNATURE; + } + } + status->parse_state = MAVLINK_PARSE_STATE_IDLE; + if (r_message != NULL) { + memcpy(r_message, rxmsg, sizeof(mavlink_message_t)); + } + } + break; + case MAVLINK_PARSE_STATE_SIGNATURE_WAIT: + rxmsg->signature[MAVLINK_SIGNATURE_BLOCK_LEN-status->signature_wait] = c; + status->signature_wait--; + if (status->signature_wait == 0) { + // we have the whole signature, check it is OK + bool sig_ok = mavlink_signature_check(status->signing, status->signing_streams, rxmsg); + if (!sig_ok && + (status->signing->accept_unsigned_callback && + status->signing->accept_unsigned_callback(status, rxmsg->msgid))) { + // accepted via application level override + sig_ok = true; + } + if (sig_ok) { + status->msg_received = MAVLINK_FRAMING_OK; + } else { + status->msg_received = MAVLINK_FRAMING_BAD_SIGNATURE; + } + status->parse_state = MAVLINK_PARSE_STATE_IDLE; + if (r_message !=NULL) { + memcpy(r_message, rxmsg, sizeof(mavlink_message_t)); + } + } break; } @@ -380,13 +817,18 @@ MAVLINK_HELPER uint8_t mavlink_frame_char_buffer(mavlink_message_t* rxmsg, status->packet_rx_success_count++; } - r_message->len = rxmsg->len; // Provide visibility on how far we are into current msg - r_mavlink_status->parse_state = status->parse_state; - r_mavlink_status->packet_idx = status->packet_idx; - r_mavlink_status->current_rx_seq = status->current_rx_seq+1; - r_mavlink_status->packet_rx_success_count = status->packet_rx_success_count; - r_mavlink_status->packet_rx_drop_count = status->parse_error; - status->parse_error = 0; + if (r_message != NULL) { + r_message->len = rxmsg->len; // Provide visibility on how far we are into current msg + } + if (r_mavlink_status != NULL) { + r_mavlink_status->parse_state = status->parse_state; + r_mavlink_status->packet_idx = status->packet_idx; + r_mavlink_status->current_rx_seq = status->current_rx_seq+1; + r_mavlink_status->packet_rx_success_count = status->packet_rx_success_count; + r_mavlink_status->packet_rx_drop_count = status->parse_error; + r_mavlink_status->flags = status->flags; + } + status->parse_error = 0; if (status->msg_received == MAVLINK_FRAMING_BAD_CRC) { /* @@ -396,7 +838,9 @@ MAVLINK_HELPER uint8_t mavlink_frame_char_buffer(mavlink_message_t* rxmsg, mavlink_msg_to_send_buffer() won't overwrite the checksum */ - r_message->checksum = _MAV_PAYLOAD(rxmsg)[status->packet_idx] | (_MAV_PAYLOAD(rxmsg)[status->packet_idx+1]<<8); + if (r_message != NULL) { + r_message->checksum = rxmsg->ck[0] | (rxmsg->ck[1]<<8); + } } return status->msg_received; @@ -418,7 +862,7 @@ MAVLINK_HELPER uint8_t mavlink_frame_char_buffer(mavlink_message_t* rxmsg, * on MCU (e.g. ARM7), while COMM_NB_HIGH is the limit for the number of channels in Linux/Windows * @param c The char to parse * - * @param r_message NULL if no message could be decoded, the message data else + * @param r_message NULL if no message could be decoded, otherwise the message data * @param r_mavlink_status if a message was decoded, this is filled with the channel's stats * @return 0 if no message could be decoded, 1 on good message and CRC, 2 on bad CRC * @@ -453,6 +897,33 @@ MAVLINK_HELPER uint8_t mavlink_frame_char(uint8_t chan, uint8_t c, mavlink_messa r_mavlink_status); } +/** + * Set the protocol version + */ +MAVLINK_HELPER void mavlink_set_proto_version(uint8_t chan, unsigned int version) +{ + mavlink_status_t *status = mavlink_get_channel_status(chan); + if (version > 1) { + status->flags &= ~(MAVLINK_STATUS_FLAG_OUT_MAVLINK1); + } else { + status->flags |= MAVLINK_STATUS_FLAG_OUT_MAVLINK1; + } +} + +/** + * Get the protocol version + * + * @return 1 for v1, 2 for v2 + */ +MAVLINK_HELPER unsigned int mavlink_get_proto_version(uint8_t chan) +{ + mavlink_status_t *status = mavlink_get_channel_status(chan); + if ((status->flags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) > 0) { + return 1; + } else { + return 2; + } +} /** * This is a convenience function which handles the complete MAVLink parsing. @@ -469,7 +940,7 @@ MAVLINK_HELPER uint8_t mavlink_frame_char(uint8_t chan, uint8_t c, mavlink_messa * on MCU (e.g. ARM7), while COMM_NB_HIGH is the limit for the number of channels in Linux/Windows * @param c The char to parse * - * @param r_message NULL if no message could be decoded, otherwise the message data. + * @param r_message NULL if no message could be decoded, otherwise the message data * @param r_mavlink_status if a message was decoded, this is filled with the channel's stats * @return 0 if no message could be decoded or bad CRC, 1 on good message and CRC * @@ -498,11 +969,12 @@ MAVLINK_HELPER uint8_t mavlink_frame_char(uint8_t chan, uint8_t c, mavlink_messa MAVLINK_HELPER uint8_t mavlink_parse_char(uint8_t chan, uint8_t c, mavlink_message_t* r_message, mavlink_status_t* r_mavlink_status) { uint8_t msg_received = mavlink_frame_char(chan, c, r_message, r_mavlink_status); - if (msg_received == MAVLINK_FRAMING_BAD_CRC) { + if (msg_received == MAVLINK_FRAMING_BAD_CRC || + msg_received == MAVLINK_FRAMING_BAD_SIGNATURE) { // we got a bad CRC. Treat as a parse failure mavlink_message_t* rxmsg = mavlink_get_channel_buffer(chan); mavlink_status_t* status = mavlink_get_channel_status(chan); - status->parse_error++; + _mav_parse_error(status); status->msg_received = MAVLINK_FRAMING_INCOMPLETE; status->parse_state = MAVLINK_PARSE_STATE_IDLE; if (c == MAVLINK_STX) @@ -656,4 +1128,6 @@ MAVLINK_HELPER void _mavlink_send_uart(mavlink_channel_t chan, const char *buf, } #endif // MAVLINK_USE_CONVENIENCE_FUNCTIONS -#endif /* _MAVLINK_HELPERS_H_ */ +#ifdef MAVLINK_USE_CXX_NAMESPACE +} // namespace mavlink +#endif diff --git a/lib/main/MAVLink/mavlink_sha256.h b/lib/main/MAVLink/mavlink_sha256.h new file mode 100644 index 0000000000..7accd03566 --- /dev/null +++ b/lib/main/MAVLink/mavlink_sha256.h @@ -0,0 +1,235 @@ +#pragma once + +/* + sha-256 implementation for MAVLink based on Heimdal sources, with + modifications to suit mavlink headers + */ +/* + * Copyright (c) 1995 - 2001 Kungliga Tekniska Högskolan + * (Royal Institute of Technology, Stockholm, Sweden). + * All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in the + * documentation and/or other materials provided with the distribution. + * + * 3. Neither the name of the Institute nor the names of its contributors + * may be used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE INSTITUTE AND CONTRIBUTORS ``AS IS'' AND + * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE INSTITUTE OR CONTRIBUTORS BE LIABLE + * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL + * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS + * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) + * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY + * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF + * SUCH DAMAGE. + */ + +/* + allow implementation to provide their own sha256 with the same API +*/ +#ifndef HAVE_MAVLINK_SHA256 + +#ifdef MAVLINK_USE_CXX_NAMESPACE +namespace mavlink { +#endif + +#ifndef MAVLINK_HELPER +#define MAVLINK_HELPER +#endif + +typedef struct { + uint32_t sz[2]; + uint32_t counter[8]; + union { + unsigned char save_bytes[64]; + uint32_t save_u32[16]; + } u; +} mavlink_sha256_ctx; + +#define Ch(x,y,z) (((x) & (y)) ^ ((~(x)) & (z))) +#define Maj(x,y,z) (((x) & (y)) ^ ((x) & (z)) ^ ((y) & (z))) + +#define ROTR(x,n) (((x)>>(n)) | ((x) << (32 - (n)))) + +#define Sigma0(x) (ROTR(x,2) ^ ROTR(x,13) ^ ROTR(x,22)) +#define Sigma1(x) (ROTR(x,6) ^ ROTR(x,11) ^ ROTR(x,25)) +#define sigma0(x) (ROTR(x,7) ^ ROTR(x,18) ^ ((x)>>3)) +#define sigma1(x) (ROTR(x,17) ^ ROTR(x,19) ^ ((x)>>10)) + +static const uint32_t mavlink_sha256_constant_256[64] = { + 0x428a2f98, 0x71374491, 0xb5c0fbcf, 0xe9b5dba5, + 0x3956c25b, 0x59f111f1, 0x923f82a4, 0xab1c5ed5, + 0xd807aa98, 0x12835b01, 0x243185be, 0x550c7dc3, + 0x72be5d74, 0x80deb1fe, 0x9bdc06a7, 0xc19bf174, + 0xe49b69c1, 0xefbe4786, 0x0fc19dc6, 0x240ca1cc, + 0x2de92c6f, 0x4a7484aa, 0x5cb0a9dc, 0x76f988da, + 0x983e5152, 0xa831c66d, 0xb00327c8, 0xbf597fc7, + 0xc6e00bf3, 0xd5a79147, 0x06ca6351, 0x14292967, + 0x27b70a85, 0x2e1b2138, 0x4d2c6dfc, 0x53380d13, + 0x650a7354, 0x766a0abb, 0x81c2c92e, 0x92722c85, + 0xa2bfe8a1, 0xa81a664b, 0xc24b8b70, 0xc76c51a3, + 0xd192e819, 0xd6990624, 0xf40e3585, 0x106aa070, + 0x19a4c116, 0x1e376c08, 0x2748774c, 0x34b0bcb5, + 0x391c0cb3, 0x4ed8aa4a, 0x5b9cca4f, 0x682e6ff3, + 0x748f82ee, 0x78a5636f, 0x84c87814, 0x8cc70208, + 0x90befffa, 0xa4506ceb, 0xbef9a3f7, 0xc67178f2 +}; + +MAVLINK_HELPER void mavlink_sha256_init(mavlink_sha256_ctx *m) +{ + m->sz[0] = 0; + m->sz[1] = 0; + m->counter[0] = 0x6a09e667; + m->counter[1] = 0xbb67ae85; + m->counter[2] = 0x3c6ef372; + m->counter[3] = 0xa54ff53a; + m->counter[4] = 0x510e527f; + m->counter[5] = 0x9b05688c; + m->counter[6] = 0x1f83d9ab; + m->counter[7] = 0x5be0cd19; +} + +static inline void mavlink_sha256_calc(mavlink_sha256_ctx *m, uint32_t *in) +{ + uint32_t AA, BB, CC, DD, EE, FF, GG, HH; + uint32_t data[64]; + int i; + + AA = m->counter[0]; + BB = m->counter[1]; + CC = m->counter[2]; + DD = m->counter[3]; + EE = m->counter[4]; + FF = m->counter[5]; + GG = m->counter[6]; + HH = m->counter[7]; + + for (i = 0; i < 16; ++i) + data[i] = in[i]; + for (i = 16; i < 64; ++i) + data[i] = sigma1(data[i-2]) + data[i-7] + + sigma0(data[i-15]) + data[i - 16]; + + for (i = 0; i < 64; i++) { + uint32_t T1, T2; + + T1 = HH + Sigma1(EE) + Ch(EE, FF, GG) + mavlink_sha256_constant_256[i] + data[i]; + T2 = Sigma0(AA) + Maj(AA,BB,CC); + + HH = GG; + GG = FF; + FF = EE; + EE = DD + T1; + DD = CC; + CC = BB; + BB = AA; + AA = T1 + T2; + } + + m->counter[0] += AA; + m->counter[1] += BB; + m->counter[2] += CC; + m->counter[3] += DD; + m->counter[4] += EE; + m->counter[5] += FF; + m->counter[6] += GG; + m->counter[7] += HH; +} + +MAVLINK_HELPER void mavlink_sha256_update(mavlink_sha256_ctx *m, const void *v, uint32_t len) +{ + const unsigned char *p = (const unsigned char *)v; + uint32_t old_sz = m->sz[0]; + uint32_t offset; + + m->sz[0] += len * 8; + if (m->sz[0] < old_sz) + ++m->sz[1]; + offset = (old_sz / 8) % 64; + while(len > 0){ + uint32_t l = 64 - offset; + if (len < l) { + l = len; + } + memcpy(m->u.save_bytes + offset, p, l); + offset += l; + p += l; + len -= l; + if(offset == 64){ + int i; + uint32_t current[16]; + const uint32_t *u = m->u.save_u32; + for (i = 0; i < 16; i++){ + const uint8_t *p1 = (const uint8_t *)&u[i]; + uint8_t *p2 = (uint8_t *)¤t[i]; + p2[0] = p1[3]; + p2[1] = p1[2]; + p2[2] = p1[1]; + p2[3] = p1[0]; + } + mavlink_sha256_calc(m, current); + offset = 0; + } + } +} + +/* + get first 48 bits of final sha256 hash + */ +MAVLINK_HELPER void mavlink_sha256_final_48(mavlink_sha256_ctx *m, uint8_t result[6]) +{ + unsigned char zeros[72]; + unsigned offset = (m->sz[0] / 8) % 64; + unsigned int dstart = (120 - offset - 1) % 64 + 1; + uint8_t *p = (uint8_t *)&m->counter[0]; + + *zeros = 0x80; + memset (zeros + 1, 0, sizeof(zeros) - 1); + zeros[dstart+7] = (m->sz[0] >> 0) & 0xff; + zeros[dstart+6] = (m->sz[0] >> 8) & 0xff; + zeros[dstart+5] = (m->sz[0] >> 16) & 0xff; + zeros[dstart+4] = (m->sz[0] >> 24) & 0xff; + zeros[dstart+3] = (m->sz[1] >> 0) & 0xff; + zeros[dstart+2] = (m->sz[1] >> 8) & 0xff; + zeros[dstart+1] = (m->sz[1] >> 16) & 0xff; + zeros[dstart+0] = (m->sz[1] >> 24) & 0xff; + + mavlink_sha256_update(m, zeros, dstart + 8); + + // this ordering makes the result consistent with taking the first + // 6 bytes of more conventional sha256 functions. It assumes + // little-endian ordering of m->counter + result[0] = p[3]; + result[1] = p[2]; + result[2] = p[1]; + result[3] = p[0]; + result[4] = p[7]; + result[5] = p[6]; +} + +// prevent conflicts with users of the header +#undef Ch +#undef ROTR +#undef Sigma0 +#undef Sigma1 +#undef sigma0 +#undef sigma1 + +#ifdef MAVLINK_USE_CXX_NAMESPACE +} // namespace mavlink +#endif + +#endif // HAVE_MAVLINK_SHA256 diff --git a/lib/main/MAVLink/mavlink_types.h b/lib/main/MAVLink/mavlink_types.h index 0a98ccc087..8bdf0b51c7 100755 --- a/lib/main/MAVLink/mavlink_types.h +++ b/lib/main/MAVLink/mavlink_types.h @@ -1,13 +1,17 @@ -#ifndef MAVLINK_TYPES_H_ -#define MAVLINK_TYPES_H_ +#pragma once // Visual Studio versions before 2010 don't have stdint.h, so we just error out. #if (defined _MSC_VER) && (_MSC_VER < 1600) #error "The C-MAVLink implementation requires Visual Studio 2010 or greater" #endif +#include #include +#ifdef MAVLINK_USE_CXX_NAMESPACE +namespace mavlink { +#endif + // Macro to define packed structures #ifdef __GNUC__ #define MAVPACKED( __Declaration__ ) __Declaration__ __attribute__((packed)) @@ -20,26 +24,15 @@ #define MAVLINK_MAX_PAYLOAD_LEN 255 ///< Maximum payload length #endif -#define MAVLINK_CORE_HEADER_LEN 5 ///< Length of core header (of the comm. layer): message length (1 byte) + message sequence (1 byte) + message system id (1 byte) + message component id (1 byte) + message type id (1 byte) -#define MAVLINK_NUM_HEADER_BYTES (MAVLINK_CORE_HEADER_LEN + 1) ///< Length of all header bytes, including core and checksum +#define MAVLINK_CORE_HEADER_LEN 9 ///< Length of core header (of the comm. layer) +#define MAVLINK_CORE_HEADER_MAVLINK1_LEN 5 ///< Length of MAVLink1 core header (of the comm. layer) +#define MAVLINK_NUM_HEADER_BYTES (MAVLINK_CORE_HEADER_LEN + 1) ///< Length of all header bytes, including core and stx #define MAVLINK_NUM_CHECKSUM_BYTES 2 #define MAVLINK_NUM_NON_PAYLOAD_BYTES (MAVLINK_NUM_HEADER_BYTES + MAVLINK_NUM_CHECKSUM_BYTES) -#define MAVLINK_MAX_PACKET_LEN (MAVLINK_MAX_PAYLOAD_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES) ///< Maximum packet length - -#define MAVLINK_MSG_ID_EXTENDED_MESSAGE 255 -#define MAVLINK_EXTENDED_HEADER_LEN 14 - -#if (defined _MSC_VER) || ((defined __APPLE__) && (defined __MACH__)) || (defined __linux__) - /* full fledged 32bit++ OS */ - #define MAVLINK_MAX_EXTENDED_PACKET_LEN 65507 -#else - /* small microcontrollers */ - #define MAVLINK_MAX_EXTENDED_PACKET_LEN 2048 -#endif - -#define MAVLINK_MAX_EXTENDED_PAYLOAD_LEN (MAVLINK_MAX_EXTENDED_PACKET_LEN - MAVLINK_EXTENDED_HEADER_LEN - MAVLINK_NUM_NON_PAYLOAD_BYTES) +#define MAVLINK_SIGNATURE_BLOCK_LEN 13 +#define MAVLINK_MAX_PACKET_LEN (MAVLINK_MAX_PAYLOAD_LEN + MAVLINK_NUM_NON_PAYLOAD_BYTES + MAVLINK_SIGNATURE_BLOCK_LEN) ///< Maximum packet length /** * Old-style 4 byte param union @@ -75,7 +68,7 @@ typedef struct param_union { * The intention is that by replacing the is_double bit with 0 the type can be directly used as a double (as the is_double bit corresponds to the * lowest mantissa bit of a double). If is_double is 0 then mavlink_type gives the type in the union. * The mavlink_types.h header will also need to have shifts/masks to define the bit boundaries in the above, - * as bitfield ordering isn’t consistent between platforms. The above is intended to be for gcc on x86, + * as bitfield ordering isn't consistent between platforms. The above is intended to be for gcc on x86, * which should be the same as gcc on little-endian arm. When using shifts/masks the value will be treated as a 64 bit unsigned number, * and the bits pulled out using the shifts/masks. */ @@ -113,23 +106,20 @@ typedef struct __mavlink_system { MAVPACKED( typedef struct __mavlink_message { - uint16_t checksum; ///< sent at end of packet - uint8_t magic; ///< protocol magic marker - uint8_t len; ///< Length of payload - uint8_t seq; ///< Sequence of packet - uint8_t sysid; ///< ID of message sender system/aircraft - uint8_t compid; ///< ID of the message sender component - uint8_t msgid; ///< ID of message in payload + uint16_t checksum; ///< sent at end of packet + uint8_t magic; ///< protocol magic marker + uint8_t len; ///< Length of payload + uint8_t incompat_flags; ///< flags that must be understood + uint8_t compat_flags; ///< flags that can be ignored if not understood + uint8_t seq; ///< Sequence of packet + uint8_t sysid; ///< ID of message sender system/aircraft + uint8_t compid; ///< ID of the message sender component + uint32_t msgid:24; ///< ID of message in payload uint64_t payload64[(MAVLINK_MAX_PAYLOAD_LEN+MAVLINK_NUM_CHECKSUM_BYTES+7)/8]; + uint8_t ck[2]; ///< incoming checksum bytes + uint8_t signature[MAVLINK_SIGNATURE_BLOCK_LEN]; }) mavlink_message_t; -MAVPACKED( -typedef struct __mavlink_extended_message { - mavlink_message_t base_msg; - int32_t extended_payload_len; ///< Length of extended payload if any - uint8_t extended_payload[MAVLINK_MAX_EXTENDED_PAYLOAD_LEN]; -}) mavlink_extended_message_t; - typedef enum { MAVLINK_TYPE_CHAR = 0, MAVLINK_TYPE_UINT8_T = 1, @@ -147,7 +137,7 @@ typedef enum { #define MAVLINK_MAX_FIELDS 64 typedef struct __mavlink_field_info { - const char *name; // name of this field + const char *name; // name of this field const char *print_format; // printing format hint, or NULL mavlink_message_type_t type; // type of this field unsigned int array_length; // if non-zero, field is an array @@ -158,6 +148,7 @@ typedef struct __mavlink_field_info { // note that in this structure the order of fields is the order // in the XML file, not necessary the wire order typedef struct __mavlink_message_info { + uint32_t msgid; // message ID const char *name; // name of the message unsigned num_fields; // how many fields in this message mavlink_field_info_t fields[MAVLINK_MAX_FIELDS]; // field information @@ -170,12 +161,14 @@ typedef struct __mavlink_message_info { #define mavlink_ck_a(msg) *((msg)->len + (uint8_t *)_MAV_PAYLOAD_NON_CONST(msg)) #define mavlink_ck_b(msg) *(((msg)->len+(uint16_t)1) + (uint8_t *)_MAV_PAYLOAD_NON_CONST(msg)) +#ifndef HAVE_MAVLINK_CHANNEL_T typedef enum { MAVLINK_COMM_0, MAVLINK_COMM_1, MAVLINK_COMM_2, MAVLINK_COMM_3 } mavlink_channel_t; +#endif /* * applications can set MAVLINK_COMM_NUM_BUFFERS to the maximum number @@ -194,22 +187,35 @@ typedef enum { MAVLINK_PARSE_STATE_UNINIT=0, MAVLINK_PARSE_STATE_IDLE, MAVLINK_PARSE_STATE_GOT_STX, - MAVLINK_PARSE_STATE_GOT_SEQ, MAVLINK_PARSE_STATE_GOT_LENGTH, + MAVLINK_PARSE_STATE_GOT_INCOMPAT_FLAGS, + MAVLINK_PARSE_STATE_GOT_COMPAT_FLAGS, + MAVLINK_PARSE_STATE_GOT_SEQ, MAVLINK_PARSE_STATE_GOT_SYSID, MAVLINK_PARSE_STATE_GOT_COMPID, - MAVLINK_PARSE_STATE_GOT_MSGID, + MAVLINK_PARSE_STATE_GOT_MSGID1, + MAVLINK_PARSE_STATE_GOT_MSGID2, + MAVLINK_PARSE_STATE_GOT_MSGID3, MAVLINK_PARSE_STATE_GOT_PAYLOAD, MAVLINK_PARSE_STATE_GOT_CRC1, - MAVLINK_PARSE_STATE_GOT_BAD_CRC1 + MAVLINK_PARSE_STATE_GOT_BAD_CRC1, + MAVLINK_PARSE_STATE_SIGNATURE_WAIT } mavlink_parse_state_t; ///< The state machine for the comm parser typedef enum { MAVLINK_FRAMING_INCOMPLETE=0, MAVLINK_FRAMING_OK=1, - MAVLINK_FRAMING_BAD_CRC=2 + MAVLINK_FRAMING_BAD_CRC=2, + MAVLINK_FRAMING_BAD_SIGNATURE=3 } mavlink_framing_t; +#define MAVLINK_STATUS_FLAG_IN_MAVLINK1 1 // last incoming packet was MAVLink1 +#define MAVLINK_STATUS_FLAG_OUT_MAVLINK1 2 // generate MAVLink1 by default +#define MAVLINK_STATUS_FLAG_IN_SIGNED 4 // last incoming packet was signed and validated +#define MAVLINK_STATUS_FLAG_IN_BADSIG 8 // last incoming packet had a bad signature + +#define MAVLINK_STX_MAVLINK1 0xFE // marker for old protocol + typedef struct __mavlink_status { uint8_t msg_received; ///< Number of received messages uint8_t buffer_overrun; ///< Number of buffer overruns @@ -220,9 +226,76 @@ typedef struct __mavlink_status { uint8_t current_tx_seq; ///< Sequence number of last packet sent uint16_t packet_rx_success_count; ///< Received packets uint16_t packet_rx_drop_count; ///< Number of packet drops + uint8_t flags; ///< MAVLINK_STATUS_FLAG_* + uint8_t signature_wait; ///< number of signature bytes left to receive + struct __mavlink_signing *signing; ///< optional signing state + struct __mavlink_signing_streams *signing_streams; ///< global record of stream timestamps } mavlink_status_t; +/* + a callback function to allow for accepting unsigned packets + */ +typedef bool (*mavlink_accept_unsigned_t)(const mavlink_status_t *status, uint32_t msgid); + +/* + flags controlling signing + */ +#define MAVLINK_SIGNING_FLAG_SIGN_OUTGOING 1 ///< Enable outgoing signing + +/* + state of MAVLink signing for this channel + */ +typedef struct __mavlink_signing { + uint8_t flags; ///< MAVLINK_SIGNING_FLAG_* + uint8_t link_id; ///< Same as MAVLINK_CHANNEL + uint64_t timestamp; ///< Timestamp, in microseconds since UNIX epoch GMT + uint8_t secret_key[32]; + mavlink_accept_unsigned_t accept_unsigned_callback; +} mavlink_signing_t; + +/* + timestamp state of each logical signing stream. This needs to be the same structure for all + connections in order to be secure + */ +#ifndef MAVLINK_MAX_SIGNING_STREAMS +#define MAVLINK_MAX_SIGNING_STREAMS 16 +#endif +typedef struct __mavlink_signing_streams { + uint16_t num_signing_streams; + struct __mavlink_signing_stream { + uint8_t link_id; ///< ID of the link (MAVLINK_CHANNEL) + uint8_t sysid; ///< Remote system ID + uint8_t compid; ///< Remote component ID + uint8_t timestamp_bytes[6]; ///< Timestamp, in microseconds since UNIX epoch GMT + } stream[MAVLINK_MAX_SIGNING_STREAMS]; +} mavlink_signing_streams_t; + + #define MAVLINK_BIG_ENDIAN 0 #define MAVLINK_LITTLE_ENDIAN 1 -#endif /* MAVLINK_TYPES_H_ */ +#define MAV_MSG_ENTRY_FLAG_HAVE_TARGET_SYSTEM 1 +#define MAV_MSG_ENTRY_FLAG_HAVE_TARGET_COMPONENT 2 + +/* + entry in table of information about each message type + */ +typedef struct __mavlink_msg_entry { + uint32_t msgid; + uint8_t crc_extra; + uint8_t min_msg_len; // minimum message length + uint8_t max_msg_len; // maximum message length (e.g. including mavlink2 extensions) + uint8_t flags; // MAV_MSG_ENTRY_FLAG_* + uint8_t target_system_ofs; // payload offset to target_system, or 0 + uint8_t target_component_ofs; // payload offset to target_component, or 0 +} mavlink_msg_entry_t; + +/* + incompat_flags bits + */ +#define MAVLINK_IFLAG_SIGNED 0x01 +#define MAVLINK_IFLAG_MASK 0x01 // mask of all understood bits + +#ifdef MAVLINK_USE_CXX_NAMESPACE +} // namespace mavlink +#endif diff --git a/lib/main/MAVLink/minimal/mavlink.h b/lib/main/MAVLink/minimal/mavlink.h new file mode 100644 index 0000000000..32c6c97c68 --- /dev/null +++ b/lib/main/MAVLink/minimal/mavlink.h @@ -0,0 +1,34 @@ +/** @file + * @brief MAVLink comm protocol built from minimal.xml + * @see http://mavlink.org + */ +#pragma once +#ifndef MAVLINK_H +#define MAVLINK_H + +#define MAVLINK_PRIMARY_XML_IDX 1 + +#ifndef MAVLINK_STX +#define MAVLINK_STX 253 +#endif + +#ifndef MAVLINK_ENDIAN +#define MAVLINK_ENDIAN MAVLINK_LITTLE_ENDIAN +#endif + +#ifndef MAVLINK_ALIGNED_FIELDS +#define MAVLINK_ALIGNED_FIELDS 1 +#endif + +#ifndef MAVLINK_CRC_EXTRA +#define MAVLINK_CRC_EXTRA 1 +#endif + +#ifndef MAVLINK_COMMAND_24BIT +#define MAVLINK_COMMAND_24BIT 1 +#endif + +#include "version.h" +#include "minimal.h" + +#endif // MAVLINK_H diff --git a/lib/main/MAVLink/common/mavlink_msg_heartbeat.h b/lib/main/MAVLink/minimal/mavlink_msg_heartbeat.h old mode 100755 new mode 100644 similarity index 100% rename from lib/main/MAVLink/common/mavlink_msg_heartbeat.h rename to lib/main/MAVLink/minimal/mavlink_msg_heartbeat.h diff --git a/lib/main/MAVLink/minimal/mavlink_msg_protocol_version.h b/lib/main/MAVLink/minimal/mavlink_msg_protocol_version.h new file mode 100644 index 0000000000..d795ff28c6 --- /dev/null +++ b/lib/main/MAVLink/minimal/mavlink_msg_protocol_version.h @@ -0,0 +1,306 @@ +#pragma once +// MESSAGE PROTOCOL_VERSION PACKING + +#define MAVLINK_MSG_ID_PROTOCOL_VERSION 300 + + +typedef struct __mavlink_protocol_version_t { + uint16_t version; /*< Currently active MAVLink version number * 100: v1.0 is 100, v2.0 is 200, etc.*/ + uint16_t min_version; /*< Minimum MAVLink version supported*/ + uint16_t max_version; /*< Maximum MAVLink version supported (set to the same value as version by default)*/ + uint8_t spec_version_hash[8]; /*< The first 8 bytes (not characters printed in hex!) of the git hash.*/ + uint8_t library_version_hash[8]; /*< The first 8 bytes (not characters printed in hex!) of the git hash.*/ +} mavlink_protocol_version_t; + +#define MAVLINK_MSG_ID_PROTOCOL_VERSION_LEN 22 +#define MAVLINK_MSG_ID_PROTOCOL_VERSION_MIN_LEN 22 +#define MAVLINK_MSG_ID_300_LEN 22 +#define MAVLINK_MSG_ID_300_MIN_LEN 22 + +#define MAVLINK_MSG_ID_PROTOCOL_VERSION_CRC 217 +#define MAVLINK_MSG_ID_300_CRC 217 + +#define MAVLINK_MSG_PROTOCOL_VERSION_FIELD_SPEC_VERSION_HASH_LEN 8 +#define MAVLINK_MSG_PROTOCOL_VERSION_FIELD_LIBRARY_VERSION_HASH_LEN 8 + +#if MAVLINK_COMMAND_24BIT +#define MAVLINK_MESSAGE_INFO_PROTOCOL_VERSION { \ + 300, \ + "PROTOCOL_VERSION", \ + 5, \ + { { "version", NULL, MAVLINK_TYPE_UINT16_T, 0, 0, offsetof(mavlink_protocol_version_t, version) }, \ + { "min_version", NULL, MAVLINK_TYPE_UINT16_T, 0, 2, offsetof(mavlink_protocol_version_t, min_version) }, \ + { "max_version", NULL, MAVLINK_TYPE_UINT16_T, 0, 4, offsetof(mavlink_protocol_version_t, max_version) }, \ + { "spec_version_hash", NULL, MAVLINK_TYPE_UINT8_T, 8, 6, offsetof(mavlink_protocol_version_t, spec_version_hash) }, \ + { "library_version_hash", NULL, MAVLINK_TYPE_UINT8_T, 8, 14, offsetof(mavlink_protocol_version_t, library_version_hash) }, \ + } \ +} +#else +#define MAVLINK_MESSAGE_INFO_PROTOCOL_VERSION { \ + "PROTOCOL_VERSION", \ + 5, \ + { { "version", NULL, MAVLINK_TYPE_UINT16_T, 0, 0, offsetof(mavlink_protocol_version_t, version) }, \ + { "min_version", NULL, MAVLINK_TYPE_UINT16_T, 0, 2, offsetof(mavlink_protocol_version_t, min_version) }, \ + { "max_version", NULL, MAVLINK_TYPE_UINT16_T, 0, 4, offsetof(mavlink_protocol_version_t, max_version) }, \ + { "spec_version_hash", NULL, MAVLINK_TYPE_UINT8_T, 8, 6, offsetof(mavlink_protocol_version_t, spec_version_hash) }, \ + { "library_version_hash", NULL, MAVLINK_TYPE_UINT8_T, 8, 14, offsetof(mavlink_protocol_version_t, library_version_hash) }, \ + } \ +} +#endif + +/** + * @brief Pack a protocol_version message + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * + * @param version Currently active MAVLink version number * 100: v1.0 is 100, v2.0 is 200, etc. + * @param min_version Minimum MAVLink version supported + * @param max_version Maximum MAVLink version supported (set to the same value as version by default) + * @param spec_version_hash The first 8 bytes (not characters printed in hex!) of the git hash. + * @param library_version_hash The first 8 bytes (not characters printed in hex!) of the git hash. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_protocol_version_pack(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, + uint16_t version, uint16_t min_version, uint16_t max_version, const uint8_t *spec_version_hash, const uint8_t *library_version_hash) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_PROTOCOL_VERSION_LEN]; + _mav_put_uint16_t(buf, 0, version); + _mav_put_uint16_t(buf, 2, min_version); + _mav_put_uint16_t(buf, 4, max_version); + _mav_put_uint8_t_array(buf, 6, spec_version_hash, 8); + _mav_put_uint8_t_array(buf, 14, library_version_hash, 8); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_PROTOCOL_VERSION_LEN); +#else + mavlink_protocol_version_t packet; + packet.version = version; + packet.min_version = min_version; + packet.max_version = max_version; + mav_array_memcpy(packet.spec_version_hash, spec_version_hash, sizeof(uint8_t)*8); + mav_array_memcpy(packet.library_version_hash, library_version_hash, sizeof(uint8_t)*8); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_PROTOCOL_VERSION_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_PROTOCOL_VERSION; + return mavlink_finalize_message(msg, system_id, component_id, MAVLINK_MSG_ID_PROTOCOL_VERSION_MIN_LEN, MAVLINK_MSG_ID_PROTOCOL_VERSION_LEN, MAVLINK_MSG_ID_PROTOCOL_VERSION_CRC); +} + +/** + * @brief Pack a protocol_version message on a channel + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param version Currently active MAVLink version number * 100: v1.0 is 100, v2.0 is 200, etc. + * @param min_version Minimum MAVLink version supported + * @param max_version Maximum MAVLink version supported (set to the same value as version by default) + * @param spec_version_hash The first 8 bytes (not characters printed in hex!) of the git hash. + * @param library_version_hash The first 8 bytes (not characters printed in hex!) of the git hash. + * @return length of the message in bytes (excluding serial stream start sign) + */ +static inline uint16_t mavlink_msg_protocol_version_pack_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, + mavlink_message_t* msg, + uint16_t version,uint16_t min_version,uint16_t max_version,const uint8_t *spec_version_hash,const uint8_t *library_version_hash) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_PROTOCOL_VERSION_LEN]; + _mav_put_uint16_t(buf, 0, version); + _mav_put_uint16_t(buf, 2, min_version); + _mav_put_uint16_t(buf, 4, max_version); + _mav_put_uint8_t_array(buf, 6, spec_version_hash, 8); + _mav_put_uint8_t_array(buf, 14, library_version_hash, 8); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), buf, MAVLINK_MSG_ID_PROTOCOL_VERSION_LEN); +#else + mavlink_protocol_version_t packet; + packet.version = version; + packet.min_version = min_version; + packet.max_version = max_version; + mav_array_memcpy(packet.spec_version_hash, spec_version_hash, sizeof(uint8_t)*8); + mav_array_memcpy(packet.library_version_hash, library_version_hash, sizeof(uint8_t)*8); + memcpy(_MAV_PAYLOAD_NON_CONST(msg), &packet, MAVLINK_MSG_ID_PROTOCOL_VERSION_LEN); +#endif + + msg->msgid = MAVLINK_MSG_ID_PROTOCOL_VERSION; + return mavlink_finalize_message_chan(msg, system_id, component_id, chan, MAVLINK_MSG_ID_PROTOCOL_VERSION_MIN_LEN, MAVLINK_MSG_ID_PROTOCOL_VERSION_LEN, MAVLINK_MSG_ID_PROTOCOL_VERSION_CRC); +} + +/** + * @brief Encode a protocol_version struct + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param msg The MAVLink message to compress the data into + * @param protocol_version C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_protocol_version_encode(uint8_t system_id, uint8_t component_id, mavlink_message_t* msg, const mavlink_protocol_version_t* protocol_version) +{ + return mavlink_msg_protocol_version_pack(system_id, component_id, msg, protocol_version->version, protocol_version->min_version, protocol_version->max_version, protocol_version->spec_version_hash, protocol_version->library_version_hash); +} + +/** + * @brief Encode a protocol_version struct on a channel + * + * @param system_id ID of this system + * @param component_id ID of this component (e.g. 200 for IMU) + * @param chan The MAVLink channel this message will be sent over + * @param msg The MAVLink message to compress the data into + * @param protocol_version C-struct to read the message contents from + */ +static inline uint16_t mavlink_msg_protocol_version_encode_chan(uint8_t system_id, uint8_t component_id, uint8_t chan, mavlink_message_t* msg, const mavlink_protocol_version_t* protocol_version) +{ + return mavlink_msg_protocol_version_pack_chan(system_id, component_id, chan, msg, protocol_version->version, protocol_version->min_version, protocol_version->max_version, protocol_version->spec_version_hash, protocol_version->library_version_hash); +} + +/** + * @brief Send a protocol_version message + * @param chan MAVLink channel to send the message + * + * @param version Currently active MAVLink version number * 100: v1.0 is 100, v2.0 is 200, etc. + * @param min_version Minimum MAVLink version supported + * @param max_version Maximum MAVLink version supported (set to the same value as version by default) + * @param spec_version_hash The first 8 bytes (not characters printed in hex!) of the git hash. + * @param library_version_hash The first 8 bytes (not characters printed in hex!) of the git hash. + */ +#ifdef MAVLINK_USE_CONVENIENCE_FUNCTIONS + +static inline void mavlink_msg_protocol_version_send(mavlink_channel_t chan, uint16_t version, uint16_t min_version, uint16_t max_version, const uint8_t *spec_version_hash, const uint8_t *library_version_hash) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char buf[MAVLINK_MSG_ID_PROTOCOL_VERSION_LEN]; + _mav_put_uint16_t(buf, 0, version); + _mav_put_uint16_t(buf, 2, min_version); + _mav_put_uint16_t(buf, 4, max_version); + _mav_put_uint8_t_array(buf, 6, spec_version_hash, 8); + _mav_put_uint8_t_array(buf, 14, library_version_hash, 8); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PROTOCOL_VERSION, buf, MAVLINK_MSG_ID_PROTOCOL_VERSION_MIN_LEN, MAVLINK_MSG_ID_PROTOCOL_VERSION_LEN, MAVLINK_MSG_ID_PROTOCOL_VERSION_CRC); +#else + mavlink_protocol_version_t packet; + packet.version = version; + packet.min_version = min_version; + packet.max_version = max_version; + mav_array_memcpy(packet.spec_version_hash, spec_version_hash, sizeof(uint8_t)*8); + mav_array_memcpy(packet.library_version_hash, library_version_hash, sizeof(uint8_t)*8); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PROTOCOL_VERSION, (const char *)&packet, MAVLINK_MSG_ID_PROTOCOL_VERSION_MIN_LEN, MAVLINK_MSG_ID_PROTOCOL_VERSION_LEN, MAVLINK_MSG_ID_PROTOCOL_VERSION_CRC); +#endif +} + +/** + * @brief Send a protocol_version message + * @param chan MAVLink channel to send the message + * @param struct The MAVLink struct to serialize + */ +static inline void mavlink_msg_protocol_version_send_struct(mavlink_channel_t chan, const mavlink_protocol_version_t* protocol_version) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + mavlink_msg_protocol_version_send(chan, protocol_version->version, protocol_version->min_version, protocol_version->max_version, protocol_version->spec_version_hash, protocol_version->library_version_hash); +#else + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PROTOCOL_VERSION, (const char *)protocol_version, MAVLINK_MSG_ID_PROTOCOL_VERSION_MIN_LEN, MAVLINK_MSG_ID_PROTOCOL_VERSION_LEN, MAVLINK_MSG_ID_PROTOCOL_VERSION_CRC); +#endif +} + +#if MAVLINK_MSG_ID_PROTOCOL_VERSION_LEN <= MAVLINK_MAX_PAYLOAD_LEN +/* + This varient of _send() can be used to save stack space by re-using + memory from the receive buffer. The caller provides a + mavlink_message_t which is the size of a full mavlink message. This + is usually the receive buffer for the channel, and allows a reply to an + incoming message with minimum stack space usage. + */ +static inline void mavlink_msg_protocol_version_send_buf(mavlink_message_t *msgbuf, mavlink_channel_t chan, uint16_t version, uint16_t min_version, uint16_t max_version, const uint8_t *spec_version_hash, const uint8_t *library_version_hash) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + char *buf = (char *)msgbuf; + _mav_put_uint16_t(buf, 0, version); + _mav_put_uint16_t(buf, 2, min_version); + _mav_put_uint16_t(buf, 4, max_version); + _mav_put_uint8_t_array(buf, 6, spec_version_hash, 8); + _mav_put_uint8_t_array(buf, 14, library_version_hash, 8); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PROTOCOL_VERSION, buf, MAVLINK_MSG_ID_PROTOCOL_VERSION_MIN_LEN, MAVLINK_MSG_ID_PROTOCOL_VERSION_LEN, MAVLINK_MSG_ID_PROTOCOL_VERSION_CRC); +#else + mavlink_protocol_version_t *packet = (mavlink_protocol_version_t *)msgbuf; + packet->version = version; + packet->min_version = min_version; + packet->max_version = max_version; + mav_array_memcpy(packet->spec_version_hash, spec_version_hash, sizeof(uint8_t)*8); + mav_array_memcpy(packet->library_version_hash, library_version_hash, sizeof(uint8_t)*8); + _mav_finalize_message_chan_send(chan, MAVLINK_MSG_ID_PROTOCOL_VERSION, (const char *)packet, MAVLINK_MSG_ID_PROTOCOL_VERSION_MIN_LEN, MAVLINK_MSG_ID_PROTOCOL_VERSION_LEN, MAVLINK_MSG_ID_PROTOCOL_VERSION_CRC); +#endif +} +#endif + +#endif + +// MESSAGE PROTOCOL_VERSION UNPACKING + + +/** + * @brief Get field version from protocol_version message + * + * @return Currently active MAVLink version number * 100: v1.0 is 100, v2.0 is 200, etc. + */ +static inline uint16_t mavlink_msg_protocol_version_get_version(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 0); +} + +/** + * @brief Get field min_version from protocol_version message + * + * @return Minimum MAVLink version supported + */ +static inline uint16_t mavlink_msg_protocol_version_get_min_version(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 2); +} + +/** + * @brief Get field max_version from protocol_version message + * + * @return Maximum MAVLink version supported (set to the same value as version by default) + */ +static inline uint16_t mavlink_msg_protocol_version_get_max_version(const mavlink_message_t* msg) +{ + return _MAV_RETURN_uint16_t(msg, 4); +} + +/** + * @brief Get field spec_version_hash from protocol_version message + * + * @return The first 8 bytes (not characters printed in hex!) of the git hash. + */ +static inline uint16_t mavlink_msg_protocol_version_get_spec_version_hash(const mavlink_message_t* msg, uint8_t *spec_version_hash) +{ + return _MAV_RETURN_uint8_t_array(msg, spec_version_hash, 8, 6); +} + +/** + * @brief Get field library_version_hash from protocol_version message + * + * @return The first 8 bytes (not characters printed in hex!) of the git hash. + */ +static inline uint16_t mavlink_msg_protocol_version_get_library_version_hash(const mavlink_message_t* msg, uint8_t *library_version_hash) +{ + return _MAV_RETURN_uint8_t_array(msg, library_version_hash, 8, 14); +} + +/** + * @brief Decode a protocol_version message into a struct + * + * @param msg The message to decode + * @param protocol_version C-struct to decode the message contents into + */ +static inline void mavlink_msg_protocol_version_decode(const mavlink_message_t* msg, mavlink_protocol_version_t* protocol_version) +{ +#if MAVLINK_NEED_BYTE_SWAP || !MAVLINK_ALIGNED_FIELDS + protocol_version->version = mavlink_msg_protocol_version_get_version(msg); + protocol_version->min_version = mavlink_msg_protocol_version_get_min_version(msg); + protocol_version->max_version = mavlink_msg_protocol_version_get_max_version(msg); + mavlink_msg_protocol_version_get_spec_version_hash(msg, protocol_version->spec_version_hash); + mavlink_msg_protocol_version_get_library_version_hash(msg, protocol_version->library_version_hash); +#else + uint8_t len = msg->len < MAVLINK_MSG_ID_PROTOCOL_VERSION_LEN? msg->len : MAVLINK_MSG_ID_PROTOCOL_VERSION_LEN; + memset(protocol_version, 0, MAVLINK_MSG_ID_PROTOCOL_VERSION_LEN); + memcpy(protocol_version, _MAV_PAYLOAD(msg), len); +#endif +} diff --git a/lib/main/MAVLink/minimal/minimal.h b/lib/main/MAVLink/minimal/minimal.h new file mode 100644 index 0000000000..755eeba888 --- /dev/null +++ b/lib/main/MAVLink/minimal/minimal.h @@ -0,0 +1,333 @@ +/** @file + * @brief MAVLink comm protocol generated from minimal.xml + * @see http://mavlink.org + */ +#pragma once +#ifndef MAVLINK_MINIMAL_H +#define MAVLINK_MINIMAL_H + +#ifndef MAVLINK_H + #error Wrong include order: MAVLINK_MINIMAL.H MUST NOT BE DIRECTLY USED. Include mavlink.h from the same directory instead or set ALL AND EVERY defines from MAVLINK.H manually accordingly, including the #define MAVLINK_H call. +#endif + +#undef MAVLINK_THIS_XML_IDX +#define MAVLINK_THIS_XML_IDX 1 + +#ifdef __cplusplus +extern "C" { +#endif + +// MESSAGE LENGTHS AND CRCS + +#ifndef MAVLINK_MESSAGE_LENGTHS +#define MAVLINK_MESSAGE_LENGTHS {} +#endif + +#ifndef MAVLINK_MESSAGE_CRCS +#define MAVLINK_MESSAGE_CRCS {{0, 50, 9, 9, 0, 0, 0}, {300, 217, 22, 22, 0, 0, 0}} +#endif + +#include "../protocol.h" + +#define MAVLINK_ENABLED_MINIMAL + +// ENUM DEFINITIONS + + +/** @brief Micro air vehicle / autopilot classes. This identifies the individual model. */ +#ifndef HAVE_ENUM_MAV_AUTOPILOT +#define HAVE_ENUM_MAV_AUTOPILOT +typedef enum MAV_AUTOPILOT +{ + MAV_AUTOPILOT_GENERIC=0, /* Generic autopilot, full support for everything | */ + MAV_AUTOPILOT_RESERVED=1, /* Reserved for future use. | */ + MAV_AUTOPILOT_SLUGS=2, /* SLUGS autopilot, http://slugsuav.soe.ucsc.edu | */ + MAV_AUTOPILOT_ARDUPILOTMEGA=3, /* ArduPilot - Plane/Copter/Rover/Sub/Tracker, https://ardupilot.org | */ + MAV_AUTOPILOT_OPENPILOT=4, /* OpenPilot, http://openpilot.org | */ + MAV_AUTOPILOT_GENERIC_WAYPOINTS_ONLY=5, /* Generic autopilot only supporting simple waypoints | */ + MAV_AUTOPILOT_GENERIC_WAYPOINTS_AND_SIMPLE_NAVIGATION_ONLY=6, /* Generic autopilot supporting waypoints and other simple navigation commands | */ + MAV_AUTOPILOT_GENERIC_MISSION_FULL=7, /* Generic autopilot supporting the full mission command set | */ + MAV_AUTOPILOT_INVALID=8, /* No valid autopilot, e.g. a GCS or other MAVLink component | */ + MAV_AUTOPILOT_PPZ=9, /* PPZ UAV - http://nongnu.org/paparazzi | */ + MAV_AUTOPILOT_UDB=10, /* UAV Dev Board | */ + MAV_AUTOPILOT_FP=11, /* FlexiPilot | */ + MAV_AUTOPILOT_PX4=12, /* PX4 Autopilot - http://px4.io/ | */ + MAV_AUTOPILOT_SMACCMPILOT=13, /* SMACCMPilot - http://smaccmpilot.org | */ + MAV_AUTOPILOT_AUTOQUAD=14, /* AutoQuad -- http://autoquad.org | */ + MAV_AUTOPILOT_ARMAZILA=15, /* Armazila -- http://armazila.com | */ + MAV_AUTOPILOT_AEROB=16, /* Aerob -- http://aerob.ru | */ + MAV_AUTOPILOT_ASLUAV=17, /* ASLUAV autopilot -- http://www.asl.ethz.ch | */ + MAV_AUTOPILOT_SMARTAP=18, /* SmartAP Autopilot - http://sky-drones.com | */ + MAV_AUTOPILOT_AIRRAILS=19, /* AirRails - http://uaventure.com | */ + MAV_AUTOPILOT_ENUM_END=20, /* | */ +} MAV_AUTOPILOT; +#endif + +/** @brief MAVLINK component type reported in HEARTBEAT message. Flight controllers must report the type of the vehicle on which they are mounted (e.g. MAV_TYPE_OCTOROTOR). All other components must report a value appropriate for their type (e.g. a camera must use MAV_TYPE_CAMERA). */ +#ifndef HAVE_ENUM_MAV_TYPE +#define HAVE_ENUM_MAV_TYPE +typedef enum MAV_TYPE +{ + MAV_TYPE_GENERIC=0, /* Generic micro air vehicle | */ + MAV_TYPE_FIXED_WING=1, /* Fixed wing aircraft. | */ + MAV_TYPE_QUADROTOR=2, /* Quadrotor | */ + MAV_TYPE_COAXIAL=3, /* Coaxial helicopter | */ + MAV_TYPE_HELICOPTER=4, /* Normal helicopter with tail rotor. | */ + MAV_TYPE_ANTENNA_TRACKER=5, /* Ground installation | */ + MAV_TYPE_GCS=6, /* Operator control unit / ground control station | */ + MAV_TYPE_AIRSHIP=7, /* Airship, controlled | */ + MAV_TYPE_FREE_BALLOON=8, /* Free balloon, uncontrolled | */ + MAV_TYPE_ROCKET=9, /* Rocket | */ + MAV_TYPE_GROUND_ROVER=10, /* Ground rover | */ + MAV_TYPE_SURFACE_BOAT=11, /* Surface vessel, boat, ship | */ + MAV_TYPE_SUBMARINE=12, /* Submarine | */ + MAV_TYPE_HEXAROTOR=13, /* Hexarotor | */ + MAV_TYPE_OCTOROTOR=14, /* Octorotor | */ + MAV_TYPE_TRICOPTER=15, /* Tricopter | */ + MAV_TYPE_FLAPPING_WING=16, /* Flapping wing | */ + MAV_TYPE_KITE=17, /* Kite | */ + MAV_TYPE_ONBOARD_CONTROLLER=18, /* Onboard companion controller | */ + MAV_TYPE_VTOL_DUOROTOR=19, /* Two-rotor VTOL using control surfaces in vertical operation in addition. Tailsitter. | */ + MAV_TYPE_VTOL_QUADROTOR=20, /* Quad-rotor VTOL using a V-shaped quad config in vertical operation. Tailsitter. | */ + MAV_TYPE_VTOL_TILTROTOR=21, /* Tiltrotor VTOL | */ + MAV_TYPE_VTOL_RESERVED2=22, /* VTOL reserved 2 | */ + MAV_TYPE_VTOL_RESERVED3=23, /* VTOL reserved 3 | */ + MAV_TYPE_VTOL_RESERVED4=24, /* VTOL reserved 4 | */ + MAV_TYPE_VTOL_RESERVED5=25, /* VTOL reserved 5 | */ + MAV_TYPE_GIMBAL=26, /* Gimbal | */ + MAV_TYPE_ADSB=27, /* ADSB system | */ + MAV_TYPE_PARAFOIL=28, /* Steerable, nonrigid airfoil | */ + MAV_TYPE_DODECAROTOR=29, /* Dodecarotor | */ + MAV_TYPE_CAMERA=30, /* Camera | */ + MAV_TYPE_CHARGING_STATION=31, /* Charging station | */ + MAV_TYPE_FLARM=32, /* FLARM collision avoidance system | */ + MAV_TYPE_SERVO=33, /* Servo | */ + MAV_TYPE_ODID=34, /* Open Drone ID. See https://mavlink.io/en/services/opendroneid.html. | */ + MAV_TYPE_DECAROTOR=35, /* Decarotor | */ + MAV_TYPE_ENUM_END=36, /* | */ +} MAV_TYPE; +#endif + +/** @brief These flags encode the MAV mode. */ +#ifndef HAVE_ENUM_MAV_MODE_FLAG +#define HAVE_ENUM_MAV_MODE_FLAG +typedef enum MAV_MODE_FLAG +{ + MAV_MODE_FLAG_CUSTOM_MODE_ENABLED=1, /* 0b00000001 Reserved for future use. | */ + MAV_MODE_FLAG_TEST_ENABLED=2, /* 0b00000010 system has a test mode enabled. This flag is intended for temporary system tests and should not be used for stable implementations. | */ + MAV_MODE_FLAG_AUTO_ENABLED=4, /* 0b00000100 autonomous mode enabled, system finds its own goal positions. Guided flag can be set or not, depends on the actual implementation. | */ + MAV_MODE_FLAG_GUIDED_ENABLED=8, /* 0b00001000 guided mode enabled, system flies waypoints / mission items. | */ + MAV_MODE_FLAG_STABILIZE_ENABLED=16, /* 0b00010000 system stabilizes electronically its attitude (and optionally position). It needs however further control inputs to move around. | */ + MAV_MODE_FLAG_HIL_ENABLED=32, /* 0b00100000 hardware in the loop simulation. All motors / actuators are blocked, but internal software is full operational. | */ + MAV_MODE_FLAG_MANUAL_INPUT_ENABLED=64, /* 0b01000000 remote control input is enabled. | */ + MAV_MODE_FLAG_SAFETY_ARMED=128, /* 0b10000000 MAV safety set to armed. Motors are enabled / running / can start. Ready to fly. Additional note: this flag is to be ignore when sent in the command MAV_CMD_DO_SET_MODE and MAV_CMD_COMPONENT_ARM_DISARM shall be used instead. The flag can still be used to report the armed state. | */ + MAV_MODE_FLAG_ENUM_END=129, /* | */ +} MAV_MODE_FLAG; +#endif + +/** @brief These values encode the bit positions of the decode position. These values can be used to read the value of a flag bit by combining the base_mode variable with AND with the flag position value. The result will be either 0 or 1, depending on if the flag is set or not. */ +#ifndef HAVE_ENUM_MAV_MODE_FLAG_DECODE_POSITION +#define HAVE_ENUM_MAV_MODE_FLAG_DECODE_POSITION +typedef enum MAV_MODE_FLAG_DECODE_POSITION +{ + MAV_MODE_FLAG_DECODE_POSITION_CUSTOM_MODE=1, /* Eighth bit: 00000001 | */ + MAV_MODE_FLAG_DECODE_POSITION_TEST=2, /* Seventh bit: 00000010 | */ + MAV_MODE_FLAG_DECODE_POSITION_AUTO=4, /* Sixth bit: 00000100 | */ + MAV_MODE_FLAG_DECODE_POSITION_GUIDED=8, /* Fifth bit: 00001000 | */ + MAV_MODE_FLAG_DECODE_POSITION_STABILIZE=16, /* Fourth bit: 00010000 | */ + MAV_MODE_FLAG_DECODE_POSITION_HIL=32, /* Third bit: 00100000 | */ + MAV_MODE_FLAG_DECODE_POSITION_MANUAL=64, /* Second bit: 01000000 | */ + MAV_MODE_FLAG_DECODE_POSITION_SAFETY=128, /* First bit: 10000000 | */ + MAV_MODE_FLAG_DECODE_POSITION_ENUM_END=129, /* | */ +} MAV_MODE_FLAG_DECODE_POSITION; +#endif + +/** @brief */ +#ifndef HAVE_ENUM_MAV_STATE +#define HAVE_ENUM_MAV_STATE +typedef enum MAV_STATE +{ + MAV_STATE_UNINIT=0, /* Uninitialized system, state is unknown. | */ + MAV_STATE_BOOT=1, /* System is booting up. | */ + MAV_STATE_CALIBRATING=2, /* System is calibrating and not flight-ready. | */ + MAV_STATE_STANDBY=3, /* System is grounded and on standby. It can be launched any time. | */ + MAV_STATE_ACTIVE=4, /* System is active and might be already airborne. Motors are engaged. | */ + MAV_STATE_CRITICAL=5, /* System is in a non-normal flight mode. It can however still navigate. | */ + MAV_STATE_EMERGENCY=6, /* System is in a non-normal flight mode. It lost control over parts or over the whole airframe. It is in mayday and going down. | */ + MAV_STATE_POWEROFF=7, /* System just initialized its power-down sequence, will shut down now. | */ + MAV_STATE_FLIGHT_TERMINATION=8, /* System is terminating itself. | */ + MAV_STATE_ENUM_END=9, /* | */ +} MAV_STATE; +#endif + +/** @brief Component ids (values) for the different types and instances of onboard hardware/software that might make up a MAVLink system (autopilot, cameras, servos, GPS systems, avoidance systems etc.). + Components must use the appropriate ID in their source address when sending messages. Components can also use IDs to determine if they are the intended recipient of an incoming message. The MAV_COMP_ID_ALL value is used to indicate messages that must be processed by all components. + When creating new entries, components that can have multiple instances (e.g. cameras, servos etc.) should be allocated sequential values. An appropriate number of values should be left free after these components to allow the number of instances to be expanded. */ +#ifndef HAVE_ENUM_MAV_COMPONENT +#define HAVE_ENUM_MAV_COMPONENT +typedef enum MAV_COMPONENT +{ + MAV_COMP_ID_ALL=0, /* Target id (target_component) used to broadcast messages to all components of the receiving system. Components should attempt to process messages with this component ID and forward to components on any other interfaces. Note: This is not a valid *source* component id for a message. | */ + MAV_COMP_ID_AUTOPILOT1=1, /* System flight controller component ("autopilot"). Only one autopilot is expected in a particular system. | */ + MAV_COMP_ID_USER1=25, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ + MAV_COMP_ID_USER2=26, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ + MAV_COMP_ID_USER3=27, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ + MAV_COMP_ID_USER4=28, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ + MAV_COMP_ID_USER5=29, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ + MAV_COMP_ID_USER6=30, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ + MAV_COMP_ID_USER7=31, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ + MAV_COMP_ID_USER8=32, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ + MAV_COMP_ID_USER9=33, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ + MAV_COMP_ID_USER10=34, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ + MAV_COMP_ID_USER11=35, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ + MAV_COMP_ID_USER12=36, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ + MAV_COMP_ID_USER13=37, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ + MAV_COMP_ID_USER14=38, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ + MAV_COMP_ID_USER15=39, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ + MAV_COMP_ID_USER16=40, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ + MAV_COMP_ID_USER17=41, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ + MAV_COMP_ID_USER18=42, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ + MAV_COMP_ID_USER19=43, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ + MAV_COMP_ID_USER20=44, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ + MAV_COMP_ID_USER21=45, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ + MAV_COMP_ID_USER22=46, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ + MAV_COMP_ID_USER23=47, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ + MAV_COMP_ID_USER24=48, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ + MAV_COMP_ID_USER25=49, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ + MAV_COMP_ID_USER26=50, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ + MAV_COMP_ID_USER27=51, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ + MAV_COMP_ID_USER28=52, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ + MAV_COMP_ID_USER29=53, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ + MAV_COMP_ID_USER30=54, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ + MAV_COMP_ID_USER31=55, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ + MAV_COMP_ID_USER32=56, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ + MAV_COMP_ID_USER33=57, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ + MAV_COMP_ID_USER34=58, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ + MAV_COMP_ID_USER35=59, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ + MAV_COMP_ID_USER36=60, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ + MAV_COMP_ID_USER37=61, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ + MAV_COMP_ID_USER38=62, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ + MAV_COMP_ID_USER39=63, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ + MAV_COMP_ID_USER40=64, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ + MAV_COMP_ID_USER41=65, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ + MAV_COMP_ID_USER42=66, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ + MAV_COMP_ID_USER43=67, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ + MAV_COMP_ID_TELEMETRY_RADIO=68, /* Telemetry radio (e.g. SiK radio, or other component that emits RADIO_STATUS messages). | */ + MAV_COMP_ID_USER45=69, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ + MAV_COMP_ID_USER46=70, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ + MAV_COMP_ID_USER47=71, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ + MAV_COMP_ID_USER48=72, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ + MAV_COMP_ID_USER49=73, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ + MAV_COMP_ID_USER50=74, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ + MAV_COMP_ID_USER51=75, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ + MAV_COMP_ID_USER52=76, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ + MAV_COMP_ID_USER53=77, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ + MAV_COMP_ID_USER54=78, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ + MAV_COMP_ID_USER55=79, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ + MAV_COMP_ID_USER56=80, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ + MAV_COMP_ID_USER57=81, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ + MAV_COMP_ID_USER58=82, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ + MAV_COMP_ID_USER59=83, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ + MAV_COMP_ID_USER60=84, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ + MAV_COMP_ID_USER61=85, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ + MAV_COMP_ID_USER62=86, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ + MAV_COMP_ID_USER63=87, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ + MAV_COMP_ID_USER64=88, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ + MAV_COMP_ID_USER65=89, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ + MAV_COMP_ID_USER66=90, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ + MAV_COMP_ID_USER67=91, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ + MAV_COMP_ID_USER68=92, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ + MAV_COMP_ID_USER69=93, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ + MAV_COMP_ID_USER70=94, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ + MAV_COMP_ID_USER71=95, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ + MAV_COMP_ID_USER72=96, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ + MAV_COMP_ID_USER73=97, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ + MAV_COMP_ID_USER74=98, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ + MAV_COMP_ID_USER75=99, /* Id for a component on privately managed MAVLink network. Can be used for any purpose but may not be published by components outside of the private network. | */ + MAV_COMP_ID_CAMERA=100, /* Camera #1. | */ + MAV_COMP_ID_CAMERA2=101, /* Camera #2. | */ + MAV_COMP_ID_CAMERA3=102, /* Camera #3. | */ + MAV_COMP_ID_CAMERA4=103, /* Camera #4. | */ + MAV_COMP_ID_CAMERA5=104, /* Camera #5. | */ + MAV_COMP_ID_CAMERA6=105, /* Camera #6. | */ + MAV_COMP_ID_SERVO1=140, /* Servo #1. | */ + MAV_COMP_ID_SERVO2=141, /* Servo #2. | */ + MAV_COMP_ID_SERVO3=142, /* Servo #3. | */ + MAV_COMP_ID_SERVO4=143, /* Servo #4. | */ + MAV_COMP_ID_SERVO5=144, /* Servo #5. | */ + MAV_COMP_ID_SERVO6=145, /* Servo #6. | */ + MAV_COMP_ID_SERVO7=146, /* Servo #7. | */ + MAV_COMP_ID_SERVO8=147, /* Servo #8. | */ + MAV_COMP_ID_SERVO9=148, /* Servo #9. | */ + MAV_COMP_ID_SERVO10=149, /* Servo #10. | */ + MAV_COMP_ID_SERVO11=150, /* Servo #11. | */ + MAV_COMP_ID_SERVO12=151, /* Servo #12. | */ + MAV_COMP_ID_SERVO13=152, /* Servo #13. | */ + MAV_COMP_ID_SERVO14=153, /* Servo #14. | */ + MAV_COMP_ID_GIMBAL=154, /* Gimbal #1. | */ + MAV_COMP_ID_LOG=155, /* Logging component. | */ + MAV_COMP_ID_ADSB=156, /* Automatic Dependent Surveillance-Broadcast (ADS-B) component. | */ + MAV_COMP_ID_OSD=157, /* On Screen Display (OSD) devices for video links. | */ + MAV_COMP_ID_PERIPHERAL=158, /* Generic autopilot peripheral component ID. Meant for devices that do not implement the parameter microservice. | */ + MAV_COMP_ID_QX1_GIMBAL=159, /* Gimbal ID for QX1. | */ + MAV_COMP_ID_FLARM=160, /* FLARM collision alert component. | */ + MAV_COMP_ID_GIMBAL2=171, /* Gimbal #2. | */ + MAV_COMP_ID_GIMBAL3=172, /* Gimbal #3. | */ + MAV_COMP_ID_GIMBAL4=173, /* Gimbal #4 | */ + MAV_COMP_ID_GIMBAL5=174, /* Gimbal #5. | */ + MAV_COMP_ID_GIMBAL6=175, /* Gimbal #6. | */ + MAV_COMP_ID_MISSIONPLANNER=190, /* Component that can generate/supply a mission flight plan (e.g. GCS or developer API). | */ + MAV_COMP_ID_ONBOARD_COMPUTER=191, /* Component that lives on the onboard computer (companion computer) and has some generic functionalities, such as settings system parameters and monitoring the status of some processes that don't directly speak mavlink and so on. | */ + MAV_COMP_ID_PATHPLANNER=195, /* Component that finds an optimal path between points based on a certain constraint (e.g. minimum snap, shortest path, cost, etc.). | */ + MAV_COMP_ID_OBSTACLE_AVOIDANCE=196, /* Component that plans a collision free path between two points. | */ + MAV_COMP_ID_VISUAL_INERTIAL_ODOMETRY=197, /* Component that provides position estimates using VIO techniques. | */ + MAV_COMP_ID_PAIRING_MANAGER=198, /* Component that manages pairing of vehicle and GCS. | */ + MAV_COMP_ID_IMU=200, /* Inertial Measurement Unit (IMU) #1. | */ + MAV_COMP_ID_IMU_2=201, /* Inertial Measurement Unit (IMU) #2. | */ + MAV_COMP_ID_IMU_3=202, /* Inertial Measurement Unit (IMU) #3. | */ + MAV_COMP_ID_GPS=220, /* GPS #1. | */ + MAV_COMP_ID_GPS2=221, /* GPS #2. | */ + MAV_COMP_ID_ODID_TXRX_1=236, /* Open Drone ID transmitter/receiver (Bluetooth/WiFi/Internet). | */ + MAV_COMP_ID_ODID_TXRX_2=237, /* Open Drone ID transmitter/receiver (Bluetooth/WiFi/Internet). | */ + MAV_COMP_ID_ODID_TXRX_3=238, /* Open Drone ID transmitter/receiver (Bluetooth/WiFi/Internet). | */ + MAV_COMP_ID_UDP_BRIDGE=240, /* Component to bridge MAVLink to UDP (i.e. from a UART). | */ + MAV_COMP_ID_UART_BRIDGE=241, /* Component to bridge to UART (i.e. from UDP). | */ + MAV_COMP_ID_TUNNEL_NODE=242, /* Component handling TUNNEL messages (e.g. vendor specific GUI of a component). | */ + MAV_COMP_ID_SYSTEM_CONTROL=250, /* Component for handling system messages (e.g. to ARM, takeoff, etc.). | */ + MAV_COMPONENT_ENUM_END=251, /* | */ +} MAV_COMPONENT; +#endif + +// MAVLINK VERSION + +#ifndef MAVLINK_VERSION +#define MAVLINK_VERSION 3 +#endif + +#if (MAVLINK_VERSION == 0) +#undef MAVLINK_VERSION +#define MAVLINK_VERSION 3 +#endif + +// MESSAGE DEFINITIONS +#include "./mavlink_msg_heartbeat.h" +#include "./mavlink_msg_protocol_version.h" + +// base include + + +#undef MAVLINK_THIS_XML_IDX +#define MAVLINK_THIS_XML_IDX 1 + +#if MAVLINK_THIS_XML_IDX == MAVLINK_PRIMARY_XML_IDX +# define MAVLINK_MESSAGE_INFO {MAVLINK_MESSAGE_INFO_HEARTBEAT, MAVLINK_MESSAGE_INFO_PROTOCOL_VERSION} +# define MAVLINK_MESSAGE_NAMES {{ "HEARTBEAT", 0 }, { "PROTOCOL_VERSION", 300 }} +# if MAVLINK_COMMAND_24BIT +# include "../mavlink_get_info.h" +# endif +#endif + +#ifdef __cplusplus +} +#endif // __cplusplus +#endif // MAVLINK_MINIMAL_H diff --git a/lib/main/MAVLink/minimal/testsuite.h b/lib/main/MAVLink/minimal/testsuite.h new file mode 100644 index 0000000000..fd9c37f414 --- /dev/null +++ b/lib/main/MAVLink/minimal/testsuite.h @@ -0,0 +1,154 @@ +/** @file + * @brief MAVLink comm protocol testsuite generated from minimal.xml + * @see http://qgroundcontrol.org/mavlink/ + */ +#pragma once +#ifndef MINIMAL_TESTSUITE_H +#define MINIMAL_TESTSUITE_H + +#ifdef __cplusplus +extern "C" { +#endif + +#ifndef MAVLINK_TEST_ALL +#define MAVLINK_TEST_ALL + +static void mavlink_test_minimal(uint8_t, uint8_t, mavlink_message_t *last_msg); + +static void mavlink_test_all(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg) +{ + + mavlink_test_minimal(system_id, component_id, last_msg); +} +#endif + + + + +static void mavlink_test_heartbeat(uint8_t system_id, uint8_t component_id, mavlink_message_t *last_msg) +{ +#ifdef MAVLINK_STATUS_FLAG_OUT_MAVLINK1 + mavlink_status_t *status = mavlink_get_channel_status(MAVLINK_COMM_0); + if ((status->flags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) && MAVLINK_MSG_ID_HEARTBEAT >= 256) { + return; + } +#endif + mavlink_message_t msg; + uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; + uint16_t i; + mavlink_heartbeat_t packet_in = { + 963497464,17,84,151,218,3 + }; + mavlink_heartbeat_t packet1, packet2; + memset(&packet1, 0, sizeof(packet1)); + packet1.custom_mode = packet_in.custom_mode; + packet1.type = packet_in.type; + packet1.autopilot = packet_in.autopilot; + packet1.base_mode = packet_in.base_mode; + packet1.system_status = packet_in.system_status; + packet1.mavlink_version = packet_in.mavlink_version; + + +#ifdef MAVLINK_STATUS_FLAG_OUT_MAVLINK1 + if (status->flags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) { + // cope with extensions + memset(MAVLINK_MSG_ID_HEARTBEAT_MIN_LEN + (char *)&packet1, 0, sizeof(packet1)-MAVLINK_MSG_ID_HEARTBEAT_MIN_LEN); + } +#endif + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_heartbeat_encode(system_id, component_id, &msg, &packet1); + mavlink_msg_heartbeat_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_heartbeat_pack(system_id, component_id, &msg , packet1.type , packet1.autopilot , packet1.base_mode , packet1.custom_mode , packet1.system_status ); + mavlink_msg_heartbeat_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_heartbeat_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.type , packet1.autopilot , packet1.base_mode , packet1.custom_mode , packet1.system_status ); + mavlink_msg_heartbeat_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_to_send_buffer(buffer, &msg); + for (i=0; iflags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) && MAVLINK_MSG_ID_PROTOCOL_VERSION >= 256) { + return; + } +#endif + mavlink_message_t msg; + uint8_t buffer[MAVLINK_MAX_PACKET_LEN]; + uint16_t i; + mavlink_protocol_version_t packet_in = { + 17235,17339,17443,{ 151, 152, 153, 154, 155, 156, 157, 158 },{ 175, 176, 177, 178, 179, 180, 181, 182 } + }; + mavlink_protocol_version_t packet1, packet2; + memset(&packet1, 0, sizeof(packet1)); + packet1.version = packet_in.version; + packet1.min_version = packet_in.min_version; + packet1.max_version = packet_in.max_version; + + mav_array_memcpy(packet1.spec_version_hash, packet_in.spec_version_hash, sizeof(uint8_t)*8); + mav_array_memcpy(packet1.library_version_hash, packet_in.library_version_hash, sizeof(uint8_t)*8); + +#ifdef MAVLINK_STATUS_FLAG_OUT_MAVLINK1 + if (status->flags & MAVLINK_STATUS_FLAG_OUT_MAVLINK1) { + // cope with extensions + memset(MAVLINK_MSG_ID_PROTOCOL_VERSION_MIN_LEN + (char *)&packet1, 0, sizeof(packet1)-MAVLINK_MSG_ID_PROTOCOL_VERSION_MIN_LEN); + } +#endif + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_protocol_version_encode(system_id, component_id, &msg, &packet1); + mavlink_msg_protocol_version_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_protocol_version_pack(system_id, component_id, &msg , packet1.version , packet1.min_version , packet1.max_version , packet1.spec_version_hash , packet1.library_version_hash ); + mavlink_msg_protocol_version_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_protocol_version_pack_chan(system_id, component_id, MAVLINK_COMM_0, &msg , packet1.version , packet1.min_version , packet1.max_version , packet1.spec_version_hash , packet1.library_version_hash ); + mavlink_msg_protocol_version_decode(&msg, &packet2); + MAVLINK_ASSERT(memcmp(&packet1, &packet2, sizeof(packet1)) == 0); + + memset(&packet2, 0, sizeof(packet2)); + mavlink_msg_to_send_buffer(buffer, &msg); + for (i=0; ilen + MAVLINK_NUM_NON_PAYLOAD_BYTES; + if (msg->magic == MAVLINK_STX_MAVLINK1) { + return msg->len + MAVLINK_CORE_HEADER_MAVLINK1_LEN+1 + 2; + } + uint16_t signature_len = (msg->incompat_flags & MAVLINK_IFLAG_SIGNED)?MAVLINK_SIGNATURE_BLOCK_LEN:0; + return msg->len + MAVLINK_NUM_NON_PAYLOAD_BYTES + signature_len; } #if MAVLINK_NEED_BYTE_SWAP @@ -336,4 +331,4 @@ _MAV_RETURN_ARRAY(int64_t, i64) _MAV_RETURN_ARRAY(float, f) _MAV_RETURN_ARRAY(double, d) -#endif // _MAVLINK_PROTOCOL_H_ + From 9cb3057ed703a6547d3c93fa9c99103cb2a8a602 Mon Sep 17 00:00:00 2001 From: Harry Phillips Date: Sun, 28 Feb 2021 19:05:59 +0000 Subject: [PATCH 068/286] MAVLink V2 implementation --- src/main/telemetry/mavlink.c | 84 +++++++++++++++++++++++++----------- 1 file changed, 60 insertions(+), 24 deletions(-) diff --git a/src/main/telemetry/mavlink.c b/src/main/telemetry/mavlink.c index f6929c7826..c4efcfe70d 100644 --- a/src/main/telemetry/mavlink.c +++ b/src/main/telemetry/mavlink.c @@ -323,6 +323,7 @@ void checkMAVLinkTelemetryState(void) static void mavlinkSendMessage(void) { uint8_t mavBuffer[MAVLINK_MAX_PACKET_LEN]; + // TODO encode message as MAVLink v1 if required0 int msgLength = mavlink_msg_to_send_buffer(mavBuffer, &mavSendMsg); for (int i = 0; i < msgLength; i++) { @@ -540,7 +541,19 @@ void mavlinkSendPosition(timeUs_t currentTimeUs) // cog Course over ground (NOT heading, but direction of movement) in degrees * 100, 0.0..359.99 degrees. If unknown, set to: 65535 gpsSol.groundCourse * 10, // satellites_visible Number of satellites visible. If unknown, set to 255 - gpsSol.numSat); + gpsSol.numSat, + // alt_ellipsoid Altitude (above WGS84, EGM96 ellipsoid). Positive for up + 0, + // h_acc Position uncertainty in mm, + gpsSol.eph * 10, + // v_acc Altitude uncertainty in mm, + gpsSol.epv * 10, + // vel_acc Speed uncertainty in mm (??) + 0, + // hdg_acc Heading uncertainty in degE5 + 0, + // yaw Yaw in earth frame from north. Use 0 if this GPS does not provide yaw. Use 65535 if this GPS is configured to provide yaw and is currently unable to provide it. Use 36000 for north. + 0); mavlinkSendMessage(); @@ -578,7 +591,10 @@ void mavlinkSendPosition(timeUs_t currentTimeUs) // longitude Longitude (WGS84), expressed as * 1E7 GPS_home.lon, // altitude Altitude(WGS84), expressed as * 1000 - GPS_home.alt * 10); // FIXME + GPS_home.alt * 10, // FIXME + // time_usec Timestamp (microseconds since system boot) + // Use millis() * 1000 as micros() will overflow after 1.19 hours. + ((uint64_t) millis()) * 1000); mavlinkSendMessage(); } @@ -596,11 +612,11 @@ void mavlinkSendAttitude(void) // yaw Yaw angle (rad) DECIDEGREES_TO_RADIANS(attitude.values.yaw), // rollspeed Roll angular speed (rad/s) - 0, + imuMeasuredRotationBF.x, // TODO check if this is the correct axis // pitchspeed Pitch angular speed (rad/s) - 0, + imuMeasuredRotationBF.y, // TODO check if this is the correct axis // yawspeed Yaw angular speed (rad/s) - 0); + imuMeasuredRotationBF.z); // TODO check if this is the correct axis mavlinkSendMessage(); } @@ -739,12 +755,18 @@ void mavlinkSendHUDAndHeartbeat(void) void mavlinkSendBatteryTemperatureStatusText(void) { uint16_t batteryVoltages[MAVLINK_MSG_BATTERY_STATUS_FIELD_VOLTAGES_LEN]; + uint16_t batteryVoltagesExt[MAVLINK_MSG_BATTERY_STATUS_FIELD_VOLTAGES_EXT_LEN]; memset(batteryVoltages, UINT16_MAX, sizeof(batteryVoltages)); + memset(batteryVoltagesExt, 0, sizeof(batteryVoltagesExt)); if (feature(FEATURE_VBAT)) { uint8_t batteryCellCount = getBatteryCellCount(); if (batteryCellCount > 0) { - for (int cell=0; (cell < batteryCellCount) && (cell < MAVLINK_MSG_BATTERY_STATUS_FIELD_VOLTAGES_LEN); cell++) { - batteryVoltages[cell] = getBatteryAverageCellVoltage() * 10; + for (int cell=0; cell < batteryCellCount && cell < MAVLINK_MSG_BATTERY_STATUS_FIELD_VOLTAGES_LEN + MAVLINK_MSG_BATTERY_STATUS_FIELD_VOLTAGES_EXT_LEN; cell++) { + if (cell < MAVLINK_MSG_BATTERY_STATUS_FIELD_VOLTAGES_LEN) { + batteryVoltages[cell] = getBatteryAverageCellVoltage() * 10; + } else { + batteryVoltagesExt[cell] = getBatteryAverageCellVoltage() * 10; + } } } else { @@ -773,7 +795,17 @@ void mavlinkSendBatteryTemperatureStatusText(void) // energy_consumed Consumed energy, in 100*Joules (intergrated U*I*dt) (1 = 100 Joule), -1: autopilot does not provide energy consumption estimate isAmperageConfigured() ? getMWhDrawn()*36 : -1, // battery_remaining Remaining battery energy: (0%: 0, 100%: 100), -1: autopilot does not estimate the remaining battery); - feature(FEATURE_VBAT) ? calculateBatteryPercentage() : -1); + feature(FEATURE_VBAT) ? calculateBatteryPercentage() : -1, + // time_remaining Remaining battery time, 0: autopilot does not provide remaining battery time estimate + 0, // TODO this could easily be implemented + // charge_state State for extent of discharge, provided by autopilot for warning or external reactions + 0, + // voltages_ext Battery voltages for cells 11 to 14. Cells above the valid cell count for this battery should have a value of 0, where zero indicates not supported (note, this is different than for the voltages field and allows empty byte truncation). If the measured value is 0 then 1 should be sent instead. + batteryVoltagesExt, + // mode Battery mode. Default (0) is that battery mode reporting is not supported or battery is in normal-use mode. + 0, + // fault_bitmask Fault/health indications. These should be set when charge_state is MAV_BATTERY_CHARGE_STATE_FAILED or MAV_BATTERY_CHARGE_STATE_UNHEALTHY (if not, fault reporting is not supported). + 0); mavlinkSendMessage(); @@ -784,7 +816,8 @@ void mavlinkSendBatteryTemperatureStatusText(void) millis(), 0, 0, - temperature * 10); + temperature * 10, + 0); mavlinkSendMessage(); @@ -803,7 +836,9 @@ void mavlinkSendBatteryTemperatureStatusText(void) mavlink_msg_statustext_pack(mavSystemId, mavComponentId, &mavSendMsg, (uint8_t)severity, - buff); + buff, + 0, + 0); mavlinkSendMessage(); } @@ -851,7 +886,7 @@ static bool handleIncoming_MISSION_CLEAR_ALL(void) // Check if this message is for us if (msg.target_system == mavSystemId) { resetWaypointList(); - mavlink_msg_mission_ack_pack(mavSystemId, mavComponentId, &mavSendMsg, mavRecvMsg.sysid, mavRecvMsg.compid, MAV_MISSION_ACCEPTED); + mavlink_msg_mission_ack_pack(mavSystemId, mavComponentId, &mavSendMsg, mavRecvMsg.sysid, mavRecvMsg.compid, MAV_MISSION_ACCEPTED, MAV_MISSION_TYPE_MISSION); mavlinkSendMessage(); return true; } @@ -873,17 +908,17 @@ static bool handleIncoming_MISSION_COUNT(void) if (msg.count <= NAV_MAX_WAYPOINTS) { incomingMissionWpCount = msg.count; // We need to know how many items to request incomingMissionWpSequence = 0; - mavlink_msg_mission_request_pack(mavSystemId, mavComponentId, &mavSendMsg, mavRecvMsg.sysid, mavRecvMsg.compid, incomingMissionWpSequence); + mavlink_msg_mission_request_pack(mavSystemId, mavComponentId, &mavSendMsg, mavRecvMsg.sysid, mavRecvMsg.compid, incomingMissionWpSequence, MAV_MISSION_TYPE_MISSION); mavlinkSendMessage(); return true; } else if (ARMING_FLAG(ARMED)) { - mavlink_msg_mission_ack_pack(mavSystemId, mavComponentId, &mavSendMsg, mavRecvMsg.sysid, mavRecvMsg.compid, MAV_MISSION_ERROR); + mavlink_msg_mission_ack_pack(mavSystemId, mavComponentId, &mavSendMsg, mavRecvMsg.sysid, mavRecvMsg.compid, MAV_MISSION_ERROR, MAV_MISSION_TYPE_MISSION); mavlinkSendMessage(); return true; } else { - mavlink_msg_mission_ack_pack(mavSystemId, mavComponentId, &mavSendMsg, mavRecvMsg.sysid, mavRecvMsg.compid, MAV_MISSION_NO_SPACE); + mavlink_msg_mission_ack_pack(mavSystemId, mavComponentId, &mavSendMsg, mavRecvMsg.sysid, mavRecvMsg.compid, MAV_MISSION_NO_SPACE, MAV_MISSION_TYPE_MISSION); mavlinkSendMessage(); return true; } @@ -901,19 +936,19 @@ static bool handleIncoming_MISSION_ITEM(void) if (msg.target_system == mavSystemId) { // Check supported values first if (ARMING_FLAG(ARMED)) { - mavlink_msg_mission_ack_pack(mavSystemId, mavComponentId, &mavSendMsg, mavRecvMsg.sysid, mavRecvMsg.compid, MAV_MISSION_ERROR); + mavlink_msg_mission_ack_pack(mavSystemId, mavComponentId, &mavSendMsg, mavRecvMsg.sysid, mavRecvMsg.compid, MAV_MISSION_ERROR, MAV_MISSION_TYPE_MISSION); mavlinkSendMessage(); return true; } if ((msg.autocontinue == 0) || (msg.command != MAV_CMD_NAV_WAYPOINT && msg.command != MAV_CMD_NAV_RETURN_TO_LAUNCH)) { - mavlink_msg_mission_ack_pack(mavSystemId, mavComponentId, &mavSendMsg, mavRecvMsg.sysid, mavRecvMsg.compid, MAV_MISSION_UNSUPPORTED); + mavlink_msg_mission_ack_pack(mavSystemId, mavComponentId, &mavSendMsg, mavRecvMsg.sysid, mavRecvMsg.compid, MAV_MISSION_UNSUPPORTED, MAV_MISSION_TYPE_MISSION); mavlinkSendMessage(); return true; } if ((msg.frame != MAV_FRAME_GLOBAL_RELATIVE_ALT) && !(msg.frame == MAV_FRAME_MISSION && msg.command == MAV_CMD_NAV_RETURN_TO_LAUNCH)) { - mavlink_msg_mission_ack_pack(mavSystemId, mavComponentId, &mavSendMsg, mavRecvMsg.sysid, mavRecvMsg.compid, MAV_MISSION_UNSUPPORTED_FRAME); + mavlink_msg_mission_ack_pack(mavSystemId, mavComponentId, &mavSendMsg, mavRecvMsg.sysid, mavRecvMsg.compid, MAV_MISSION_UNSUPPORTED_FRAME, MAV_MISSION_TYPE_MISSION); mavlinkSendMessage(); return true; } @@ -935,22 +970,22 @@ static bool handleIncoming_MISSION_ITEM(void) if (incomingMissionWpSequence >= incomingMissionWpCount) { if (isWaypointListValid()) { - mavlink_msg_mission_ack_pack(mavSystemId, mavComponentId, &mavSendMsg, mavRecvMsg.sysid, mavRecvMsg.compid, MAV_MISSION_ACCEPTED); + mavlink_msg_mission_ack_pack(mavSystemId, mavComponentId, &mavSendMsg, mavRecvMsg.sysid, mavRecvMsg.compid, MAV_MISSION_ACCEPTED, MAV_MISSION_TYPE_MISSION); mavlinkSendMessage(); } else { - mavlink_msg_mission_ack_pack(mavSystemId, mavComponentId, &mavSendMsg, mavRecvMsg.sysid, mavRecvMsg.compid, MAV_MISSION_INVALID); + mavlink_msg_mission_ack_pack(mavSystemId, mavComponentId, &mavSendMsg, mavRecvMsg.sysid, mavRecvMsg.compid, MAV_MISSION_INVALID, MAV_MISSION_TYPE_MISSION); mavlinkSendMessage(); } } else { - mavlink_msg_mission_request_pack(mavSystemId, mavComponentId, &mavSendMsg, mavRecvMsg.sysid, mavRecvMsg.compid, incomingMissionWpSequence); + mavlink_msg_mission_request_pack(mavSystemId, mavComponentId, &mavSendMsg, mavRecvMsg.sysid, mavRecvMsg.compid, incomingMissionWpSequence, MAV_MISSION_TYPE_MISSION); mavlinkSendMessage(); } } else { // Wrong sequence number received - mavlink_msg_mission_ack_pack(mavSystemId, mavComponentId, &mavSendMsg, mavRecvMsg.sysid, mavRecvMsg.compid, MAV_MISSION_INVALID_SEQUENCE); + mavlink_msg_mission_ack_pack(mavSystemId, mavComponentId, &mavSendMsg, mavRecvMsg.sysid, mavRecvMsg.compid, MAV_MISSION_INVALID_SEQUENCE, MAV_MISSION_TYPE_MISSION); mavlinkSendMessage(); } @@ -967,7 +1002,7 @@ static bool handleIncoming_MISSION_REQUEST_LIST(void) // Check if this message is for us if (msg.target_system == mavSystemId) { - mavlink_msg_mission_count_pack(mavSystemId, mavComponentId, &mavSendMsg, mavRecvMsg.sysid, mavRecvMsg.compid, getWaypointCount()); + mavlink_msg_mission_count_pack(mavSystemId, mavComponentId, &mavSendMsg, mavRecvMsg.sysid, mavRecvMsg.compid, getWaypointCount(), MAV_MISSION_TYPE_MISSION); mavlinkSendMessage(); return true; } @@ -997,11 +1032,12 @@ static bool handleIncoming_MISSION_REQUEST(void) 0, 0, 0, 0, wp.lat / 1e7f, wp.lon / 1e7f, - wp.alt / 100.0f); + wp.alt / 100.0f, + MAV_MISSION_TYPE_MISSION); mavlinkSendMessage(); } else { - mavlink_msg_mission_ack_pack(mavSystemId, mavComponentId, &mavSendMsg, mavRecvMsg.sysid, mavRecvMsg.compid, MAV_MISSION_INVALID_SEQUENCE); + mavlink_msg_mission_ack_pack(mavSystemId, mavComponentId, &mavSendMsg, mavRecvMsg.sysid, mavRecvMsg.compid, MAV_MISSION_INVALID_SEQUENCE, MAV_MISSION_TYPE_MISSION); mavlinkSendMessage(); } From a7d14877410b51a39e020c5f07a6a939a60291be Mon Sep 17 00:00:00 2001 From: Harry Phillips Date: Sun, 28 Feb 2021 21:28:46 +0000 Subject: [PATCH 069/286] Basic implementation of RC over MAVLink --- src/main/rx/mavlink.c | 53 ++++++++++++++++++++++++++++++++++-- src/main/rx/mavlink.h | 3 ++ src/main/telemetry/mavlink.c | 17 ++++++++---- 3 files changed, 64 insertions(+), 9 deletions(-) diff --git a/src/main/rx/mavlink.c b/src/main/rx/mavlink.c index 5af1acaced..480b3ad723 100644 --- a/src/main/rx/mavlink.c +++ b/src/main/rx/mavlink.c @@ -40,12 +40,59 @@ FILE_COMPILE_FOR_SPEED #include "rx/rx.h" #include "rx/mavlink.h" -#define MAVLINK_MAX_CHANNEL 18 +#define MAVLINK_CHANNEL_COUNT 18 +static uint16_t mavlinkChannelData[MAVLINK_CHANNEL_COUNT]; + +void mavlinkRxHandleMessage(const mavlink_rc_channels_override_t *msg) { + mavlinkChannelData[0] = msg->chan1_raw; + mavlinkChannelData[1] = msg->chan2_raw; + mavlinkChannelData[2] = msg->chan3_raw; + mavlinkChannelData[3] = msg->chan4_raw; + mavlinkChannelData[4] = msg->chan5_raw; + mavlinkChannelData[5] = msg->chan6_raw; + mavlinkChannelData[6] = msg->chan7_raw; + mavlinkChannelData[7] = msg->chan8_raw; + mavlinkChannelData[8] = msg->chan9_raw; + mavlinkChannelData[9] = msg->chan10_raw; + mavlinkChannelData[10] = msg->chan11_raw; + mavlinkChannelData[11] = msg->chan12_raw; + mavlinkChannelData[12] = msg->chan13_raw; + mavlinkChannelData[13] = msg->chan14_raw; + mavlinkChannelData[14] = msg->chan15_raw; + mavlinkChannelData[15] = msg->chan16_raw; + mavlinkChannelData[16] = msg->chan17_raw; + mavlinkChannelData[17] = msg->chan18_raw; +} + +static uint8_t mavlinkFrameStatus(rxRuntimeConfig_t *rxRuntimeConfig) +{ + return RX_FRAME_COMPLETE; +} + +static uint16_t mavlinkReadRawRC(const rxRuntimeConfig_t *rxRuntimeConfig, uint8_t chan) +{ + UNUSED(rxRuntimeConfig); + // MAVLink values are sent as PWM values in microseconds so no conversion is needed + return mavlinkChannelData[chan]; +} bool mavlinkRxInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig) { - // TODO - return false; + // TODO failsafe + + UNUSED(rxConfig); + + rxRuntimeConfig->channelData = mavlinkChannelData; + rxRuntimeConfig->channelCount = MAVLINK_CHANNEL_COUNT; + rxRuntimeConfig->rxRefreshRate = 11000; + rxRuntimeConfig->rcReadRawFn = mavlinkReadRawRC; + rxRuntimeConfig->rcFrameStatusFn = mavlinkFrameStatus; + + for (int ii = 0; ii < MAVLINK_CHANNEL_COUNT; ++ii) { + mavlinkChannelData[ii] = (16 * PWM_RANGE_MIDDLE) / 10 - 1408; + } + + return true; } #endif diff --git a/src/main/rx/mavlink.h b/src/main/rx/mavlink.h index 3311b15413..1519149237 100644 --- a/src/main/rx/mavlink.h +++ b/src/main/rx/mavlink.h @@ -17,4 +17,7 @@ #pragma once +#include "common/mavlink.h" + +void mavlinkRxHandleMessage(const mavlink_rc_channels_override_t *msg); bool mavlinkRxInit(const struct rxConfig_s *initialRxConfig, struct rxRuntimeConfig_s *rxRuntimeConfig); diff --git a/src/main/telemetry/mavlink.c b/src/main/telemetry/mavlink.c index c4efcfe70d..ddeb3a3158 100644 --- a/src/main/telemetry/mavlink.c +++ b/src/main/telemetry/mavlink.c @@ -67,6 +67,7 @@ #include "navigation/navigation_private.h" #include "rx/rx.h" +#include "rx/mavlink.h" #include "sensors/acceleration.h" #include "sensors/barometer.h" @@ -86,13 +87,7 @@ #include "scheduler/scheduler.h" -// mavlink library uses unnamed unions that's causes GCC to complain if -Wpedantic is used -// until this is resolved in mavlink library - ignore -Wpedantic for mavlink code -// TODO check if this is resolved in V2 library -#pragma GCC diagnostic push -#pragma GCC diagnostic ignored "-Wpedantic" #include "common/mavlink.h" -#pragma GCC diagnostic pop #define TELEMETRY_MAVLINK_PORT_MODE MODE_RXTX #define TELEMETRY_MAVLINK_MAXRATE 50 @@ -1048,7 +1043,17 @@ static bool handleIncoming_MISSION_REQUEST(void) } static bool handleIncoming_RC_CHANNELS_OVERRIDE(void) { + mavlink_rc_channels_override_t msg; + mavlink_msg_rc_channels_override_decode(&mavRecvMsg, &msg); + // Check if this message is for us + if (msg.target_system == mavSystemId) { + mavlinkRxHandleMessage(&msg); + // TODO do we need to send an ack? + return true; + } + + return false; } static bool processMAVLinkIncomingTelemetry(void) From 1feb1c318ef2e5c90ca194e4e4bca75c8ed98fdd Mon Sep 17 00:00:00 2001 From: Tony Yeung Date: Sun, 28 Feb 2021 23:04:24 +0000 Subject: [PATCH 070/286] set home position info after updating it --- src/main/navigation/navigation.c | 3 +++ 1 file changed, 3 insertions(+) diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c index cc6b161e54..6172a9a1b8 100755 --- a/src/main/navigation/navigation.c +++ b/src/main/navigation/navigation.c @@ -233,6 +233,7 @@ static navigationFSMEvent_t nextForNonGeoStates(void); void initializeRTHSanityChecker(const fpVector3_t * pos); bool validateRTHSanityChecker(void); +void updateHomePosition(void); /*************************************************************************************************/ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_IDLE(navigationFSMState_t previousState); @@ -2354,6 +2355,8 @@ void checkSafeHomeState(bool shouldBeEnabled) setHomePosition(&original_rth_home, 0, NAV_POS_UPDATE_XY | NAV_POS_UPDATE_Z | NAV_POS_UPDATE_HEADING, navigationActualStateHomeValidity()); safehome_applied = false; } + // if we've changed the home position, update the distance and direction + updateHomePosition(); } /*********************************************************** From 8be957c512b199b1096a116dc577c9304c4babd6 Mon Sep 17 00:00:00 2001 From: Harry Phillips Date: Mon, 1 Mar 2021 01:42:46 +0000 Subject: [PATCH 071/286] Correctly report frame status (for failsafe) --- src/main/flight/failsafe.h | 2 +- src/main/rx/mavlink.c | 13 ++++++++++--- src/main/telemetry/mavlink.c | 2 -- 3 files changed, 11 insertions(+), 6 deletions(-) diff --git a/src/main/flight/failsafe.h b/src/main/flight/failsafe.h index 042a52186f..cafc01b007 100644 --- a/src/main/flight/failsafe.h +++ b/src/main/flight/failsafe.h @@ -57,7 +57,7 @@ typedef enum { /* In this phase, the connection from the receiver * has been confirmed as lost and it will either * transition into FAILSAFE_RX_LOSS_RECOVERED if the - * RX link is recovered inmmediately or one of the + * RX link is recovered immediately or one of the * recovery phases otherwise (as configured via * failsafe_procedure) or into FAILSAFE_RX_LOSS_IDLE * if failsafe_procedure is NONE. diff --git a/src/main/rx/mavlink.c b/src/main/rx/mavlink.c index 480b3ad723..6923421caa 100644 --- a/src/main/rx/mavlink.c +++ b/src/main/rx/mavlink.c @@ -42,8 +42,10 @@ FILE_COMPILE_FOR_SPEED #define MAVLINK_CHANNEL_COUNT 18 static uint16_t mavlinkChannelData[MAVLINK_CHANNEL_COUNT]; +static bool frameReceived; void mavlinkRxHandleMessage(const mavlink_rc_channels_override_t *msg) { + // TODO handle non-present channels mavlinkChannelData[0] = msg->chan1_raw; mavlinkChannelData[1] = msg->chan2_raw; mavlinkChannelData[2] = msg->chan3_raw; @@ -62,11 +64,16 @@ void mavlinkRxHandleMessage(const mavlink_rc_channels_override_t *msg) { mavlinkChannelData[15] = msg->chan16_raw; mavlinkChannelData[16] = msg->chan17_raw; mavlinkChannelData[17] = msg->chan18_raw; + frameReceived = true; } static uint8_t mavlinkFrameStatus(rxRuntimeConfig_t *rxRuntimeConfig) { - return RX_FRAME_COMPLETE; + if (frameReceived) { + frameReceived = false; + return RX_FRAME_COMPLETE; + } + return RX_FRAME_PENDING; } static uint16_t mavlinkReadRawRC(const rxRuntimeConfig_t *rxRuntimeConfig, uint8_t chan) @@ -78,10 +85,10 @@ static uint16_t mavlinkReadRawRC(const rxRuntimeConfig_t *rxRuntimeConfig, uint8 bool mavlinkRxInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig) { - // TODO failsafe - UNUSED(rxConfig); + frameReceived = false; + rxRuntimeConfig->channelData = mavlinkChannelData; rxRuntimeConfig->channelCount = MAVLINK_CHANNEL_COUNT; rxRuntimeConfig->rxRefreshRate = 11000; diff --git a/src/main/telemetry/mavlink.c b/src/main/telemetry/mavlink.c index ddeb3a3158..461d36d7bc 100644 --- a/src/main/telemetry/mavlink.c +++ b/src/main/telemetry/mavlink.c @@ -1049,7 +1049,6 @@ static bool handleIncoming_RC_CHANNELS_OVERRIDE(void) { // Check if this message is for us if (msg.target_system == mavSystemId) { mavlinkRxHandleMessage(&msg); - // TODO do we need to send an ack? return true; } @@ -1065,7 +1064,6 @@ static bool processMAVLinkIncomingTelemetry(void) if (result == MAVLINK_FRAMING_OK) { switch (mavRecvMsg.msgid) { case MAVLINK_MSG_ID_HEARTBEAT: - // TODO failsafe break; case MAVLINK_MSG_ID_MISSION_CLEAR_ALL: return handleIncoming_MISSION_CLEAR_ALL(); From 36dc2bc6c1db1b429c76beec51953a4df4f721b2 Mon Sep 17 00:00:00 2001 From: Harry Phillips Date: Mon, 1 Mar 2021 01:49:37 +0000 Subject: [PATCH 072/286] Messy way of checking channel validity --- src/main/rx/mavlink.c | 50 +++++++++++++++++-------------------------- 1 file changed, 20 insertions(+), 30 deletions(-) diff --git a/src/main/rx/mavlink.c b/src/main/rx/mavlink.c index 6923421caa..7c349e962f 100644 --- a/src/main/rx/mavlink.c +++ b/src/main/rx/mavlink.c @@ -24,19 +24,10 @@ FILE_COMPILE_FOR_SPEED #ifdef USE_SERIALRX_MAVLINK -#include "build/build_config.h" #include "build/debug.h" -#include "common/crc.h" -#include "common/maths.h" #include "common/utils.h" -#include "drivers/time.h" -#include "drivers/serial.h" -#include "drivers/serial_uart.h" - -#include "io/serial.h" - #include "rx/rx.h" #include "rx/mavlink.h" @@ -45,25 +36,24 @@ static uint16_t mavlinkChannelData[MAVLINK_CHANNEL_COUNT]; static bool frameReceived; void mavlinkRxHandleMessage(const mavlink_rc_channels_override_t *msg) { - // TODO handle non-present channels - mavlinkChannelData[0] = msg->chan1_raw; - mavlinkChannelData[1] = msg->chan2_raw; - mavlinkChannelData[2] = msg->chan3_raw; - mavlinkChannelData[3] = msg->chan4_raw; - mavlinkChannelData[4] = msg->chan5_raw; - mavlinkChannelData[5] = msg->chan6_raw; - mavlinkChannelData[6] = msg->chan7_raw; - mavlinkChannelData[7] = msg->chan8_raw; - mavlinkChannelData[8] = msg->chan9_raw; - mavlinkChannelData[9] = msg->chan10_raw; - mavlinkChannelData[10] = msg->chan11_raw; - mavlinkChannelData[11] = msg->chan12_raw; - mavlinkChannelData[12] = msg->chan13_raw; - mavlinkChannelData[13] = msg->chan14_raw; - mavlinkChannelData[14] = msg->chan15_raw; - mavlinkChannelData[15] = msg->chan16_raw; - mavlinkChannelData[16] = msg->chan17_raw; - mavlinkChannelData[17] = msg->chan18_raw; + if (msg->chan1_raw != 0 && msg->chan1_raw != UINT16_MAX) mavlinkChannelData[0] = msg->chan1_raw; + if (msg->chan2_raw != 0 && msg->chan2_raw != UINT16_MAX) mavlinkChannelData[1] = msg->chan2_raw; + if (msg->chan3_raw != 0 && msg->chan3_raw != UINT16_MAX) mavlinkChannelData[2] = msg->chan3_raw; + if (msg->chan4_raw != 0 && msg->chan4_raw != UINT16_MAX) mavlinkChannelData[3] = msg->chan4_raw; + if (msg->chan5_raw != 0 && msg->chan5_raw != UINT16_MAX) mavlinkChannelData[4] = msg->chan5_raw; + if (msg->chan6_raw != 0 && msg->chan6_raw != UINT16_MAX) mavlinkChannelData[5] = msg->chan6_raw; + if (msg->chan7_raw != 0 && msg->chan7_raw != UINT16_MAX) mavlinkChannelData[6] = msg->chan7_raw; + if (msg->chan8_raw != 0 && msg->chan8_raw != UINT16_MAX) mavlinkChannelData[7] = msg->chan8_raw; + if (msg->chan9_raw != 0 && msg->chan9_raw < UINT16_MAX - 1) mavlinkChannelData[8] = msg->chan9_raw; + if (msg->chan10_raw != 0 && msg->chan10_raw < UINT16_MAX - 1) mavlinkChannelData[9] = msg->chan10_raw; + if (msg->chan11_raw != 0 && msg->chan11_raw < UINT16_MAX - 1) mavlinkChannelData[10] = msg->chan11_raw; + if (msg->chan12_raw != 0 && msg->chan12_raw < UINT16_MAX - 1) mavlinkChannelData[11] = msg->chan12_raw; + if (msg->chan13_raw != 0 && msg->chan13_raw < UINT16_MAX - 1) mavlinkChannelData[12] = msg->chan13_raw; + if (msg->chan14_raw != 0 && msg->chan14_raw < UINT16_MAX - 1) mavlinkChannelData[13] = msg->chan14_raw; + if (msg->chan15_raw != 0 && msg->chan15_raw < UINT16_MAX - 1) mavlinkChannelData[14] = msg->chan15_raw; + if (msg->chan16_raw != 0 && msg->chan16_raw < UINT16_MAX - 1) mavlinkChannelData[15] = msg->chan16_raw; + if (msg->chan17_raw != 0 && msg->chan17_raw < UINT16_MAX - 1) mavlinkChannelData[16] = msg->chan17_raw; + if (msg->chan18_raw != 0 && msg->chan18_raw < UINT16_MAX - 1) mavlinkChannelData[17] = msg->chan18_raw; frameReceived = true; } @@ -76,11 +66,11 @@ static uint8_t mavlinkFrameStatus(rxRuntimeConfig_t *rxRuntimeConfig) return RX_FRAME_PENDING; } -static uint16_t mavlinkReadRawRC(const rxRuntimeConfig_t *rxRuntimeConfig, uint8_t chan) +static uint16_t mavlinkReadRawRC(const rxRuntimeConfig_t *rxRuntimeConfig, uint8_t channel) { UNUSED(rxRuntimeConfig); // MAVLink values are sent as PWM values in microseconds so no conversion is needed - return mavlinkChannelData[chan]; + return mavlinkChannelData[channel]; } bool mavlinkRxInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig) From 81a41f89882765322483c9bf184fa8ac94435fc8 Mon Sep 17 00:00:00 2001 From: Tony Yeung Date: Mon, 1 Mar 2021 13:35:55 +0000 Subject: [PATCH 073/286] add v2 changes --- docs/Safehomes.md | 10 ++++++---- 1 file changed, 6 insertions(+), 4 deletions(-) diff --git a/docs/Safehomes.md b/docs/Safehomes.md index 6a5492ec71..e805659518 100644 --- a/docs/Safehomes.md +++ b/docs/Safehomes.md @@ -12,18 +12,20 @@ One potential risk when landing is that there might be buildings, trees and othe ## Safehome -Safehomes are a list of GPS coordinates that identify safe landing points. When the flight controller is armed, it checks the list of safehomes. The nearest safehome that is enabled and within ```safehome_max_distance``` (default 200m) of the current position will be remembered. The arming home location remains as home. +Safehomes are a list of GPS coordinates that identify safe landing points. You can define up to 8 safehomes for different locations you fly at. When the flight controller is armed, it checks the list of safehomes. The nearest safehome that is enabled and within ```safehome_max_distance``` (default 200m) of the current position is identified. The arming home location remains as home. -When RTH is activated, whether by radio failsafe, or using the RTH radio control mode, the safehome identified during arming will replace the arming home. If RTH is turned off, either by regaining radio control or turning off the RTH radio control mode, the home location will return to arming point. +When RTH is activated, whether by radio failsafe, or using the RTH radio control mode, the safehome identified during arming will replace the home location. If RTH is turned off, either by regaining radio control or turning off the RTH radio control mode, the home location will return back to arming point. The safehome operating mode is set using ```safehome_usage_mode```. If ```OFF```, safehomes will not be used. If ```RTH```, the safehome will replace the arming location when RTH is activated, either manually or because of RX failsafe. If ```RTH_FS```, the safehome will only be used for RX failsafe. This option can be changed using the OSD menu. -This behavior has changed from the initial release, where the safehome location replaced the arming location during the arming phase. That would result in flight information involving home (home distance, home bearing, etc) using the safehome, instead of the arming location. +If you frequently use RTH to return back to the arming point, you may not want the aircraft to fly to the safehome. Let it do this at least once to confirm safehomes is working as expected. Afterward, `set safehome_usage_mode = RTH_FS` and the safehome will only be used for failsafe. -You can define up to 8 safehomes for different locations you fly at. +When using mode `RTH_FS`, you should confirm that your radio's failsafe configuration triggers the iNav failsafe mode. With many receivers, you have the ability to specify what signal to output during failsafe conditions. When you are choosing safehome locations, ensure that the location is clear of obstructions for a radius more than 50m (`nav_fw_loiter_radius`). As the plane descends, the circles aren't always symmetrical, as wind direction could result in some wider or tighter turns. Also, the direction and length of the final landing stage is also unknown. You want to choose a point that has provides a margin for variation and the final landing. +If your safehome is not visible from your current location, use extra caution. A visual check of the safehome is recommend prior to flying. If the safehome is in use, you can use the OSD menu to disable safehome usage prior to your flight. + ## OSD Message when Armed When the aircraft is armed, the OSD briefly shows `ARMED` and the current GPS position and current date and time. From 5ec6ee97442bb0740cb3e54afe84f1a12888f82f Mon Sep 17 00:00:00 2001 From: OptimusTi Date: Sun, 7 Mar 2021 15:33:26 -0500 Subject: [PATCH 074/286] Adds CRSF Min RSSI and LQ Adds CRSF Min RSSI and LQ to Disarm Stats pages --- src/main/io/osd.c | 53 ++++++++++++++++++++++++++++++++++++++++------- 1 file changed, 46 insertions(+), 7 deletions(-) diff --git a/src/main/io/osd.c b/src/main/io/osd.c index a02d79720b..4d0bf2f191 100755 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -155,6 +155,8 @@ typedef struct statistic_s { int16_t max_current; // /100 int16_t max_power; // /100 int16_t min_rssi; + int16_t min_lq; // for CRSF + int16_t min_rssi_dbm; // for CRSF int32_t max_altitude; uint32_t max_distance; } statistic_t; @@ -527,6 +529,21 @@ static uint16_t osdConvertRSSI(void) return constrain(getRSSI() * 100 / RSSI_MAX_VALUE, 0, 99); } +static uint16_t osdGetLQ(void) +{ + int16_t statsLQ = rxLinkStatistics.uplinkLQ; + int16_t scaledLQ = scaleRange(constrain(statsLQ, 0, 100), 0, 100, 170, 300); + if (rxLinkStatistics.rfMode == 2) { + return scaledLQ; + } else { + return statsLQ; + } +} + +static uint16_t osdGetdBm(void) +{ + return rxLinkStatistics.uplinkRSSI; +} /** * Displays a temperature postfixed with a symbol depending on the current unit system * @param label to display @@ -2196,17 +2213,17 @@ static bool osdDrawSingleElement(uint8_t item) } case OSD_DEBUG: { - /* - * Longest representable string is -2147483648 does not fit in the screen. + /* + * Longest representable string is -2147483648 does not fit in the screen. * Only 7 digits for negative and 8 digits for positive values allowed */ for (uint8_t bufferIndex = 0; bufferIndex < DEBUG32_VALUE_COUNT; ++elemPosY, bufferIndex += 2) { tfp_sprintf( - buff, - "[%u]=%8ld [%u]=%8ld", - bufferIndex, - constrain(debug[bufferIndex], -9999999, 99999999), - bufferIndex+1, + buff, + "[%u]=%8ld [%u]=%8ld", + bufferIndex, + constrain(debug[bufferIndex], -9999999, 99999999), + bufferIndex+1, constrain(debug[bufferIndex+1], -9999999, 99999999) ); displayWrite(osdDisplayPort, elemPosX, elemPosY, buff); @@ -2910,6 +2927,8 @@ static void osdResetStats(void) stats.max_speed = 0; stats.min_voltage = 5000; stats.min_rssi = 99; + stats.min_lq = 300; + stats.min_rssi_dbm = 0; stats.max_altitude = 0; } @@ -2942,6 +2961,14 @@ static void osdUpdateStats(void) if (stats.min_rssi > value) stats.min_rssi = value; + value = osdGetLQ(); + if (stats.min_lq > value) + stats.min_lq = value; + + value = osdGetdBm(); + if (stats.min_rssi_dbm > value) + stats.min_rssi_dbm = value; + stats.max_altitude = MAX(stats.max_altitude, osdGetAltitude()); } @@ -2984,10 +3011,22 @@ static void osdShowStats(void) osdLeftAlignString(buff); displayWrite(osdDisplayPort, statValuesX, top++, buff); +#if defined(USE_SERIALRX_CRSF) + displayWrite(osdDisplayPort, statNameX, top, "MIN LQ :"); + itoa(stats.min_lq, buff, 10); + strcat(buff, "%"); + displayWrite(osdDisplayPort, statValuesX, top++, buff); + + displayWrite(osdDisplayPort, statNameX, top, "MIN RSSI :"); + itoa(stats.min_rssi_dbm, buff, 10); + tfp_sprintf(buff, "%s%c", buff, SYM_DBM); + displayWrite(osdDisplayPort, statValuesX, top++, buff); +#else displayWrite(osdDisplayPort, statNameX, top, "MIN RSSI :"); itoa(stats.min_rssi, buff, 10); strcat(buff, "%"); displayWrite(osdDisplayPort, statValuesX, top++, buff); +#endif if (feature(FEATURE_CURRENT_METER)) { displayWrite(osdDisplayPort, statNameX, top, "MAX CURRENT :"); From fca5a2f76ee530800e988ba12fef705e87a8662a Mon Sep 17 00:00:00 2001 From: root Date: Sun, 7 Mar 2021 22:51:12 +0100 Subject: [PATCH 075/286] Fix Serial RX and ICM20869 Target YUPIF7 --- src/main/target/YUPIF7/target.h | 15 ++++++++------- 1 file changed, 8 insertions(+), 7 deletions(-) diff --git a/src/main/target/YUPIF7/target.h b/src/main/target/YUPIF7/target.h index b7799bfb81..eda2d7cbbf 100644 --- a/src/main/target/YUPIF7/target.h +++ b/src/main/target/YUPIF7/target.h @@ -15,7 +15,6 @@ * along with Cleanflight. If not, see . */ - #pragma once #define TARGET_BOARD_IDENTIFIER "YPF7" #define USBD_PRODUCT_STRING "YUPIF7" @@ -37,18 +36,16 @@ #define ENSURE_MPU_DATA_READY_IS_LOW // ICM 20689 -#define MPU6500_CS_PIN SPI1_NSS_PIN -#define MPU6500_SPI_BUS BUS_SPI1 - -#define USE_IMU_MPU6500 -#define IMU_MPU6500_ALIGN CW90_DEG +#define USE_IMU_ICM20689 +#define IMU_ICM20689_ALIGN CW90_DEG +#define ICM20689_CS_PIN SPI1_NSS_PIN +#define ICM20689_SPI_BUS BUS_SPI1 #define USE_MAG #define MAG_I2C_BUS BUS_I2C1 #define USE_MAG_HMC5883 #define MAG_HMC5883_ALIGN CW270_DEG_FLIP #define USE_MAG_QMC5883 - #define TEMPERATURE_I2C_BUS BUS_I2C1 #define USE_BARO @@ -57,8 +54,10 @@ #define USE_BARO_BMP280 // Serial ports +#define USB_IO #define USE_VCP #define VBUS_SENSING_PIN PA8 +#define VBUS_SENSING_ENABLED #define USE_UART1 #define UART1_RX_PIN PA10 @@ -147,6 +146,7 @@ #define USE_ESC_SENSOR // Default configuration +#define DEFAULT_RX_TYPE RX_TYPE_SERIAL #define SERIALRX_PROVIDER SERIALRX_SBUS #define SERIALRX_UART SERIAL_PORT_USART6 #define TELEMETRY_UART SERIAL_PORT_USART1 @@ -162,3 +162,4 @@ #define TARGET_IO_PORTB 0xffff #define TARGET_IO_PORTC 0xffff #define TARGET_IO_PORTD (BIT(2)) + From 77d5093a1015f02c732f8ad4e123ae1cf2b5d4b1 Mon Sep 17 00:00:00 2001 From: OptimusTi Date: Mon, 8 Mar 2021 19:16:50 -0500 Subject: [PATCH 076/286] Removed duplicate Min Voltage. --- src/main/io/osd.c | 6 ------ 1 file changed, 6 deletions(-) diff --git a/src/main/io/osd.c b/src/main/io/osd.c index f41b6c2d66..6fbc13fc2c 100644 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -3008,12 +3008,6 @@ static void osdShowStatsPage1(void) osdFormatAltitudeStr(buff, stats.max_altitude); displayWrite(osdDisplayPort, statValuesX, top++, buff); - displayWrite(osdDisplayPort, statNameX, top, "MIN BATTERY VOLT :"); - osdFormatCentiNumber(buff, stats.min_voltage, 0, osdConfig()->main_voltage_decimals, 0, osdConfig()->main_voltage_decimals + 2); - strcat(buff, "V"); - osdLeftAlignString(buff); - displayWrite(osdDisplayPort, statValuesX, top++, buff); - #if defined(USE_SERIALRX_CRSF) displayWrite(osdDisplayPort, statNameX, top, "MIN LQ :"); itoa(stats.min_lq, buff, 10); From 2a5ce2e0b31ff2eff44b273d4dd37567058baa50 Mon Sep 17 00:00:00 2001 From: "Pawel Spychalski (DzikuVx)" Date: Tue, 9 Mar 2021 22:08:07 +0100 Subject: [PATCH 077/286] Do not slow down in WP mission when approaching a waypoint --- src/main/navigation/navigation_multicopter.c | 14 +++++++++++++- 1 file changed, 13 insertions(+), 1 deletion(-) diff --git a/src/main/navigation/navigation_multicopter.c b/src/main/navigation/navigation_multicopter.c index adfdb28ec0..bb8d4d5105 100755 --- a/src/main/navigation/navigation_multicopter.c +++ b/src/main/navigation/navigation_multicopter.c @@ -438,7 +438,19 @@ static void updatePositionVelocityController_MC(const float maxSpeed) // Scale velocity to respect max_speed float newVelTotal = sqrtf(sq(newVelX) + sq(newVelY)); - if (newVelTotal > maxSpeed) { + + /* + * We override computed speed with max speed in following cases: + * 1 - computed velocity is > maxSpeed + * 2 - in WP mission when: slowDownForTurning is OFF, we do not fly towards na last waypoint and computed speed is < maxSpeed + */ + if ( + (navGetStateFlags(posControl.navState) & NAV_AUTO_WP && + !isApproachingLastWaypoint() && + newVelTotal < maxSpeed && + !navConfig()->mc.slowDownForTurning + ) || newVelTotal > maxSpeed + ) { newVelX = maxSpeed * (newVelX / newVelTotal); newVelY = maxSpeed * (newVelY / newVelTotal); newVelTotal = maxSpeed; From 7363df0a7926095541a90d95074dc12bfd393024 Mon Sep 17 00:00:00 2001 From: "Pawel Spychalski (DzikuVx)" Date: Tue, 9 Mar 2021 22:15:52 +0100 Subject: [PATCH 078/286] Fix obvious complitation error --- src/main/navigation/navigation_multicopter.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/navigation/navigation_multicopter.c b/src/main/navigation/navigation_multicopter.c index bb8d4d5105..a0beb4149c 100755 --- a/src/main/navigation/navigation_multicopter.c +++ b/src/main/navigation/navigation_multicopter.c @@ -445,7 +445,7 @@ static void updatePositionVelocityController_MC(const float maxSpeed) * 2 - in WP mission when: slowDownForTurning is OFF, we do not fly towards na last waypoint and computed speed is < maxSpeed */ if ( - (navGetStateFlags(posControl.navState) & NAV_AUTO_WP && + (navGetCurrentStateFlags() & NAV_AUTO_WP && !isApproachingLastWaypoint() && newVelTotal < maxSpeed && !navConfig()->mc.slowDownForTurning From b2a77fe98612ba84a33629cf19b276b98d5b0c85 Mon Sep 17 00:00:00 2001 From: breadoven <56191411+breadoven@users.noreply.github.com> Date: Fri, 12 Mar 2021 10:06:37 +0000 Subject: [PATCH 079/286] Update osd.c --- src/main/io/osd.c | 27 ++++++++++++++++----------- 1 file changed, 16 insertions(+), 11 deletions(-) diff --git a/src/main/io/osd.c b/src/main/io/osd.c index e40749735e..9f72c01ae3 100755 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -1199,7 +1199,7 @@ static void osdDisplayFlightPIDValues(uint8_t elemPosX, uint8_t elemPosY, const if ((isAdjustmentFunctionSelected(adjFuncD)) || (((adjFuncD == ADJUSTMENT_ROLL_D) || (adjFuncD == ADJUSTMENT_PITCH_D)) && (isAdjustmentFunctionSelected(ADJUSTMENT_PITCH_ROLL_D)))) TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); displayWriteWithAttr(osdDisplayPort, elemPosX + 12, elemPosY, buff, elemAttr); - + elemAttr = TEXT_ATTRIBUTES_NONE; tfp_sprintf(buff, "%3d", pid->FF); if ((isAdjustmentFunctionSelected(adjFuncFF)) || (((adjFuncFF == ADJUSTMENT_ROLL_FF) || (adjFuncFF == ADJUSTMENT_PITCH_FF)) && (isAdjustmentFunctionSelected(ADJUSTMENT_PITCH_ROLL_FF)))) @@ -2238,17 +2238,17 @@ static bool osdDrawSingleElement(uint8_t item) } case OSD_DEBUG: { - /* - * Longest representable string is -2147483648 does not fit in the screen. + /* + * Longest representable string is -2147483648 does not fit in the screen. * Only 7 digits for negative and 8 digits for positive values allowed */ for (uint8_t bufferIndex = 0; bufferIndex < DEBUG32_VALUE_COUNT; ++elemPosY, bufferIndex += 2) { tfp_sprintf( - buff, - "[%u]=%8ld [%u]=%8ld", - bufferIndex, - constrain(debug[bufferIndex], -9999999, 99999999), - bufferIndex+1, + buff, + "[%u]=%8ld [%u]=%8ld", + bufferIndex, + constrain(debug[bufferIndex], -9999999, 99999999), + bufferIndex+1, constrain(debug[bufferIndex+1], -9999999, 99999999) ); displayWrite(osdDisplayPort, elemPosX, elemPosY, buff); @@ -3117,8 +3117,8 @@ static void osdShowArmed(void) char versionBuf[30]; char *date; char *time; - // We need 12 visible rows - uint8_t y = MIN((osdDisplayPort->rows / 2) - 1, osdDisplayPort->rows - 12 - 1); + // We need 12 visible rows, start row never < first fully visible row 1 + uint8_t y = osdDisplayPort->rows > 13 ? (osdDisplayPort->rows - 12) / 2 : 1; displayClearScreen(osdDisplayPort); displayWrite(osdDisplayPort, 12, y, "ARMED"); @@ -3127,9 +3127,14 @@ static void osdShowArmed(void) if (strlen(systemConfig()->name) > 0) { osdFormatCraftName(craftNameBuf); displayWrite(osdDisplayPort, (osdDisplayPort->cols - strlen(systemConfig() -> name)) / 2, y, craftNameBuf ); - y += 2; + y += 1; } + if (posControl.waypointListValid && posControl.waypointCount > 0) { + displayWrite(osdDisplayPort, 7, y, "*MISSION LOADED*"); + } + y += 1; + #if defined(USE_GPS) if (feature(FEATURE_GPS)) { if (STATE(GPS_FIX_HOME)) { From 2e2d48655cb8996cb7211c528e3d8ece4075106d Mon Sep 17 00:00:00 2001 From: Jeff Kubascik Date: Thu, 11 Mar 2021 23:08:20 -0500 Subject: [PATCH 080/286] Enable report_cell_voltage support for CRSF telemetry --- src/main/telemetry/crsf.c | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/src/main/telemetry/crsf.c b/src/main/telemetry/crsf.c index 8ac00c39f9..d72175cf24 100755 --- a/src/main/telemetry/crsf.c +++ b/src/main/telemetry/crsf.c @@ -246,7 +246,11 @@ static void crsfFrameBatterySensor(sbuf_t *dst) // use sbufWrite since CRC does not include frame length sbufWriteU8(dst, CRSF_FRAME_BATTERY_SENSOR_PAYLOAD_SIZE + CRSF_FRAME_LENGTH_TYPE_CRC); crsfSerialize8(dst, CRSF_FRAMETYPE_BATTERY_SENSOR); - crsfSerialize16(dst, getBatteryVoltage() / 10); // vbat is in units of 0.01V + if (telemetryConfig()->report_cell_voltage) { + crsfSerialize16(dst, getBatteryAverageCellVoltage() / 10); + } else { + crsfSerialize16(dst, getBatteryVoltage() / 10); // vbat is in units of 0.01V + } crsfSerialize16(dst, getAmperage() / 10); const uint8_t batteryRemainingPercentage = calculateBatteryPercentage(); crsfSerialize8(dst, (getMAhDrawn() >> 16)); From fb4a1307269b149a6a8d15b51f8bb65bb0072460 Mon Sep 17 00:00:00 2001 From: OptimusTi Date: Tue, 16 Mar 2021 12:40:44 -0400 Subject: [PATCH 081/286] Typo Fix There was a typo in the SNR symbol name --- src/main/drivers/osd_symbols.h | 2 +- src/main/io/osd.c | 4 ++-- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/src/main/drivers/osd_symbols.h b/src/main/drivers/osd_symbols.h index 244decd89f..d8accf4c31 100644 --- a/src/main/drivers/osd_symbols.h +++ b/src/main/drivers/osd_symbols.h @@ -243,7 +243,7 @@ #define SYM_2RSS 0xEA // RSSI 2 #define SYM_DB 0xEB // dB #define SYM_DBM 0xEC // dBm -#define SYM_SRN 0xEE // SNR +#define SYM_SNR 0xEE // SNR #define SYM_MW 0xED // mW #else diff --git a/src/main/io/osd.c b/src/main/io/osd.c index 6fbc13fc2c..2886c7930c 100644 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -1717,12 +1717,12 @@ static bool osdDrawSingleElement(uint8_t item) const char* hidesnr = " "; int16_t osdSNR_Alarm = rxLinkStatistics.uplinkSNR; if (osdSNR_Alarm <= osdConfig()->snr_alarm) { - buff[0] = SYM_SRN; + buff[0] = SYM_SNR; tfp_sprintf(buff + 1, "%3d%c", rxLinkStatistics.uplinkSNR, SYM_DB); } else if (osdSNR_Alarm > osdConfig()->snr_alarm) { if (cmsInMenu) { - buff[0] = SYM_SRN; + buff[0] = SYM_SNR; tfp_sprintf(buff + 1, "%s%c", showsnr, SYM_DB); } else { buff[0] = SYM_BLANK; From 9cd36256145f9e4c9cef0e9fc4e6eef9016f5d99 Mon Sep 17 00:00:00 2001 From: OptimusTi Date: Thu, 18 Mar 2021 12:45:37 -0400 Subject: [PATCH 082/286] Fixed CRSF SNR Simplified to make it look pretty --- src/main/io/osd.c | 32 +++++++++++++++----------------- 1 file changed, 15 insertions(+), 17 deletions(-) diff --git a/src/main/io/osd.c b/src/main/io/osd.c index 2886c7930c..b123c1b494 100644 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -1712,25 +1712,23 @@ static bool osdDrawSingleElement(uint8_t item) break; } - case OSD_CRSF_SNR_DB: { - const char* showsnr = "-20"; - const char* hidesnr = " "; - int16_t osdSNR_Alarm = rxLinkStatistics.uplinkSNR; - if (osdSNR_Alarm <= osdConfig()->snr_alarm) { - buff[0] = SYM_SNR; - tfp_sprintf(buff + 1, "%3d%c", rxLinkStatistics.uplinkSNR, SYM_DB); - } - else if (osdSNR_Alarm > osdConfig()->snr_alarm) { - if (cmsInMenu) { + case OSD_CRSF_SNR_DB: { + const char* showsnr = "-20"; + const char* hidesnr = " "; + int16_t osdSNR_Alarm = rxLinkStatistics.uplinkSNR; + buff[0] = SYM_BLANK; + tfp_sprintf(buff + 1, "%s%c", hidesnr, SYM_BLANK); + if (osdSNR_Alarm <= osdConfig()->snr_alarm) { buff[0] = SYM_SNR; - tfp_sprintf(buff + 1, "%s%c", showsnr, SYM_DB); - } else { - buff[0] = SYM_BLANK; - tfp_sprintf(buff + 1, "%s%c", hidesnr, SYM_BLANK); + tfp_sprintf(buff + 1, "%3d%c", rxLinkStatistics.uplinkSNR, SYM_DB); + } else if (osdSNR_Alarm > osdConfig()->snr_alarm) { + if (cmsInMenu) { + buff[0] = SYM_SNR; + tfp_sprintf(buff + 1, "%s%c", showsnr, SYM_DB); + } } - } - break; - } + break; + } #endif case OSD_CRSF_TX_POWER: { From cd46c4c8d19663cb1e05d40ddf722430a332f282 Mon Sep 17 00:00:00 2001 From: OptimusTi Date: Fri, 19 Mar 2021 00:27:59 -0400 Subject: [PATCH 083/286] More simplifying removed a couple of unneeded lines --- src/main/io/osd.c | 10 ++++------ 1 file changed, 4 insertions(+), 6 deletions(-) diff --git a/src/main/io/osd.c b/src/main/io/osd.c index b123c1b494..127631a149 100644 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -1721,14 +1721,12 @@ static bool osdDrawSingleElement(uint8_t item) if (osdSNR_Alarm <= osdConfig()->snr_alarm) { buff[0] = SYM_SNR; tfp_sprintf(buff + 1, "%3d%c", rxLinkStatistics.uplinkSNR, SYM_DB); - } else if (osdSNR_Alarm > osdConfig()->snr_alarm) { - if (cmsInMenu) { - buff[0] = SYM_SNR; - tfp_sprintf(buff + 1, "%s%c", showsnr, SYM_DB); - } + } else if (cmsInMenu) { + buff[0] = SYM_SNR; + tfp_sprintf(buff + 1, "%s%c", showsnr, SYM_DB); } break; - } + } #endif case OSD_CRSF_TX_POWER: { From 94aff24b93be0b0cc60e38e6e7034592c630d524 Mon Sep 17 00:00:00 2001 From: Alexander van Saase Date: Fri, 19 Mar 2021 22:39:22 +0100 Subject: [PATCH 084/286] detect launch on gps speed --- src/main/navigation/navigation_fw_launch.c | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/main/navigation/navigation_fw_launch.c b/src/main/navigation/navigation_fw_launch.c index 0b3c1633be..2ddc0e56bc 100755 --- a/src/main/navigation/navigation_fw_launch.c +++ b/src/main/navigation/navigation_fw_launch.c @@ -316,10 +316,11 @@ static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_WAIT_DETECTION(timeU const bool isBungeeLaunched = isForwardAccelerationHigh && isAircraftAlmostLevel; const bool isSwingLaunched = (swingVelocity > navConfig()->fw.launch_velocity_thresh) && (imuMeasuredAccelBF.x > 0); + const bool isForwardLaunched = (gpsSol.groundSpeed > navConfig()->fw.launch_velocity_thresh); applyThrottleIdleLogic(false); - if (isBungeeLaunched || isSwingLaunched) { + if (isBungeeLaunched || isSwingLaunched || isForwardLaunched) { if (currentStateElapsedMs(currentTimeUs) > navConfig()->fw.launch_time_thresh) { return FW_LAUNCH_EVENT_SUCCESS; // the launch is detected now, go to FW_LAUNCH_STATE_DETECTED } From 749111df549e5e22da61243a7ff5309c087caa84 Mon Sep 17 00:00:00 2001 From: Alexander van Saase Date: Fri, 19 Mar 2021 22:46:43 +0100 Subject: [PATCH 085/286] add acceleration check --- src/main/navigation/navigation_fw_launch.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/navigation/navigation_fw_launch.c b/src/main/navigation/navigation_fw_launch.c index 2ddc0e56bc..8dc9cb68a9 100755 --- a/src/main/navigation/navigation_fw_launch.c +++ b/src/main/navigation/navigation_fw_launch.c @@ -316,7 +316,7 @@ static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_WAIT_DETECTION(timeU const bool isBungeeLaunched = isForwardAccelerationHigh && isAircraftAlmostLevel; const bool isSwingLaunched = (swingVelocity > navConfig()->fw.launch_velocity_thresh) && (imuMeasuredAccelBF.x > 0); - const bool isForwardLaunched = (gpsSol.groundSpeed > navConfig()->fw.launch_velocity_thresh); + const bool isForwardLaunched = (gpsSol.groundSpeed > navConfig()->fw.launch_velocity_thresh) && (imuMeasuredAccelBF.x > 0); applyThrottleIdleLogic(false); From 23ef95f17e9076864cf08cf58b10465a9756a3d4 Mon Sep 17 00:00:00 2001 From: luca Date: Sat, 20 Mar 2021 18:37:18 +0100 Subject: [PATCH 086/286] Failsafe procedure from OSD --- src/main/cms/cms_menu_misc.c | 1 + 1 file changed, 1 insertion(+) diff --git a/src/main/cms/cms_menu_misc.c b/src/main/cms/cms_menu_misc.c index 15916826fa..1180932505 100644 --- a/src/main/cms/cms_menu_misc.c +++ b/src/main/cms/cms_menu_misc.c @@ -47,6 +47,7 @@ static const OSD_Entry menuMiscEntries[]= OSD_SETTING_ENTRY("OSD VOLT DECIMALS", SETTING_OSD_MAIN_VOLTAGE_DECIMALS), OSD_SETTING_ENTRY("STATS ENERGY UNIT", SETTING_OSD_STATS_ENERGY_UNIT), #endif + OSD_SETTING_ENTRY("FS PROCEDURE", SETTING_FAILSAFE_PROCEDURE), OSD_BACK_AND_END_ENTRY, }; From 6e9ccb93fcbd1540cb12cd05a4f30f1151387e67 Mon Sep 17 00:00:00 2001 From: luca Date: Sat, 20 Mar 2021 19:35:42 +0100 Subject: [PATCH 087/286] trigger GitHub actions From c2a98768c0ae0362e00d60f6b595a5fce0dcbcb2 Mon Sep 17 00:00:00 2001 From: Alexander van Saase Date: Sat, 20 Mar 2021 20:49:48 +0100 Subject: [PATCH 088/286] check gps --- src/main/navigation/navigation_fw_launch.c | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/src/main/navigation/navigation_fw_launch.c b/src/main/navigation/navigation_fw_launch.c index 8dc9cb68a9..6db4f8f849 100755 --- a/src/main/navigation/navigation_fw_launch.c +++ b/src/main/navigation/navigation_fw_launch.c @@ -48,6 +48,8 @@ #include "navigation/navigation.h" #include "navigation/navigation_private.h" +#include "io/gps.h" + #define SWING_LAUNCH_MIN_ROTATION_RATE DEGREES_TO_RADIANS(100) // expect minimum 100dps rotation rate #define LAUNCH_MOTOR_IDLE_SPINUP_TIME 1500 // ms #define UNUSED(x) ((void)(x)) @@ -316,7 +318,7 @@ static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_WAIT_DETECTION(timeU const bool isBungeeLaunched = isForwardAccelerationHigh && isAircraftAlmostLevel; const bool isSwingLaunched = (swingVelocity > navConfig()->fw.launch_velocity_thresh) && (imuMeasuredAccelBF.x > 0); - const bool isForwardLaunched = (gpsSol.groundSpeed > navConfig()->fw.launch_velocity_thresh) && (imuMeasuredAccelBF.x > 0); + const bool isForwardLaunched = isGPSHeadingValid() && (imuMeasuredAccelBF.x > 0); applyThrottleIdleLogic(false); From 48a250b35f05ba6ae61a103d02fe7502b2d28bc2 Mon Sep 17 00:00:00 2001 From: OptimusTi Date: Sun, 21 Mar 2021 00:36:05 -0400 Subject: [PATCH 089/286] More formatting and fixed IF statement --- src/main/io/osd.c | 49 +++++++++++++++++++++++++++-------------------- 1 file changed, 28 insertions(+), 21 deletions(-) diff --git a/src/main/io/osd.c b/src/main/io/osd.c index 127631a149..c7fb9aa003 100644 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -1670,7 +1670,9 @@ static bool osdDrawSingleElement(uint8_t item) return true; } +#if defined(USE_SERIALRX_CRSF) case OSD_CRSF_RSSI_DBM: + { if (rxLinkStatistics.activeAnt == 0) { buff[0] = SYM_RSSI; tfp_sprintf(buff + 1, "%4d%c", rxLinkStatistics.uplinkRSSI, SYM_DBM); @@ -1685,12 +1687,12 @@ static bool osdDrawSingleElement(uint8_t item) } } break; - -#if defined(USE_SERIALRX_CRSF) - case OSD_CRSF_LQ: { - buff[0] = SYM_BLANK; - int16_t statsLQ = rxLinkStatistics.uplinkLQ; - int16_t scaledLQ = scaleRange(constrain(statsLQ, 0, 100), 0, 100, 170, 300); + } + case OSD_CRSF_LQ: + { + buff[0] = SYM_BLANK; + int16_t statsLQ = rxLinkStatistics.uplinkLQ; + int16_t scaledLQ = scaleRange(constrain(statsLQ, 0, 100), 0, 100, 170, 300); if (rxLinkStatistics.rfMode == 2) { if (osdConfig()->crsf_lq_format == OSD_CRSF_LQ_TYPE1) { tfp_sprintf(buff, "%5d%s", scaledLQ, "%"); @@ -1700,9 +1702,9 @@ static bool osdDrawSingleElement(uint8_t item) } else { if (osdConfig()->crsf_lq_format == OSD_CRSF_LQ_TYPE1) { tfp_sprintf(buff, "%5d%s", rxLinkStatistics.uplinkLQ, "%"); - } else { + } else { tfp_sprintf(buff, "%d:%3d%s", rxLinkStatistics.rfMode, rxLinkStatistics.uplinkLQ, "%"); - } + } } if (!failsafeIsReceivingRxData()){ TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); @@ -1712,27 +1714,32 @@ static bool osdDrawSingleElement(uint8_t item) break; } - case OSD_CRSF_SNR_DB: { + case OSD_CRSF_SNR_DB: + { const char* showsnr = "-20"; const char* hidesnr = " "; int16_t osdSNR_Alarm = rxLinkStatistics.uplinkSNR; - buff[0] = SYM_BLANK; - tfp_sprintf(buff + 1, "%s%c", hidesnr, SYM_BLANK); - if (osdSNR_Alarm <= osdConfig()->snr_alarm) { + if (osdSNR_Alarm > osdConfig()->snr_alarm) { + if (cmsInMenu) { + buff[0] = SYM_SNR; + tfp_sprintf(buff + 1, "%s%c", showsnr, SYM_DB); + } else { + buff[0] = SYM_BLANK; + tfp_sprintf(buff + 1, "%s%c", hidesnr, SYM_BLANK); + } + } else if (osdSNR_Alarm <= osdConfig()->snr_alarm) { buff[0] = SYM_SNR; tfp_sprintf(buff + 1, "%3d%c", rxLinkStatistics.uplinkSNR, SYM_DB); - } else if (cmsInMenu) { - buff[0] = SYM_SNR; - tfp_sprintf(buff + 1, "%s%c", showsnr, SYM_DB); } break; - } -#endif + } - case OSD_CRSF_TX_POWER: { - tfp_sprintf(buff, "%4d%c", rxLinkStatistics.uplinkTXPower, SYM_MW); - break; - } + case OSD_CRSF_TX_POWER: + { + tfp_sprintf(buff, "%4d%c", rxLinkStatistics.uplinkTXPower, SYM_MW); + break; + } +#endif case OSD_CROSSHAIRS: // Hud is a sub-element of the crosshair From 3a82e26b8bdefa00df6b30aaee78ab703778508b Mon Sep 17 00:00:00 2001 From: Alexander van Saase Date: Mon, 22 Mar 2021 21:58:23 +0100 Subject: [PATCH 090/286] add groundspeed check --- src/main/navigation/navigation_fw_launch.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/navigation/navigation_fw_launch.c b/src/main/navigation/navigation_fw_launch.c index 6db4f8f849..66ee070c1f 100755 --- a/src/main/navigation/navigation_fw_launch.c +++ b/src/main/navigation/navigation_fw_launch.c @@ -318,7 +318,7 @@ static fixedWingLaunchEvent_t fwLaunchState_FW_LAUNCH_STATE_WAIT_DETECTION(timeU const bool isBungeeLaunched = isForwardAccelerationHigh && isAircraftAlmostLevel; const bool isSwingLaunched = (swingVelocity > navConfig()->fw.launch_velocity_thresh) && (imuMeasuredAccelBF.x > 0); - const bool isForwardLaunched = isGPSHeadingValid() && (imuMeasuredAccelBF.x > 0); + const bool isForwardLaunched = isGPSHeadingValid() && (gpsSol.groundSpeed > navConfig()->fw.launch_velocity_thresh) && (imuMeasuredAccelBF.x > 0); applyThrottleIdleLogic(false); From ef90e360e2c1a238afc4a2be7315eabd2ec23c24 Mon Sep 17 00:00:00 2001 From: Harry Phillips Date: Mon, 1 Mar 2021 01:59:06 +0000 Subject: [PATCH 091/286] MAVLink V1/V2 switch --- src/main/fc/settings.yaml | 6 ++++++ src/main/telemetry/mavlink.c | 6 ++---- src/main/telemetry/telemetry.c | 3 ++- src/main/telemetry/telemetry.h | 1 + 4 files changed, 11 insertions(+), 5 deletions(-) diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index de05765f53..d4477da31f 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -2640,6 +2640,12 @@ groups: type: uint8_t min: 0 max: 255 + - name: mavlink_version + field: mavlink.version + type: uint8_t + min: 1 + max: 2 + - name: PG_ELERES_CONFIG type: eleresConfig_t headers: ["rx/eleres.h"] diff --git a/src/main/telemetry/mavlink.c b/src/main/telemetry/mavlink.c index 461d36d7bc..3166fb99f7 100644 --- a/src/main/telemetry/mavlink.c +++ b/src/main/telemetry/mavlink.c @@ -318,7 +318,7 @@ void checkMAVLinkTelemetryState(void) static void mavlinkSendMessage(void) { uint8_t mavBuffer[MAVLINK_MAX_PACKET_LEN]; - // TODO encode message as MAVLink v1 if required0 + if (telemetryConfig()->mavlink.version == 1) mavSendMsg.magic = MAVLINK_STX_MAVLINK1; // TODO test this switches to MAVLink v1 int msgLength = mavlink_msg_to_send_buffer(mavBuffer, &mavSendMsg); for (int i = 0; i < msgLength; i++) { @@ -330,7 +330,7 @@ void mavlinkSendSystemStatus(void) { // Receiver is assumed to be always present uint32_t onboard_control_sensors_present = (MAV_SYS_STATUS_SENSOR_RC_RECEIVER); - // GYRO and RC are assumed as minimium requirements + // GYRO and RC are assumed as minimum requirements uint32_t onboard_control_sensors_enabled = (MAV_SYS_STATUS_SENSOR_3D_GYRO | MAV_SYS_STATUS_SENSOR_RC_RECEIVER); uint32_t onboard_control_sensors_health = 0; @@ -1111,8 +1111,6 @@ void handleMAVLinkTelemetry(timeUs_t currentTimeUs) lastMavlinkMessage = currentTimeUs; incomingRequestServed = false; } - - } #endif diff --git a/src/main/telemetry/telemetry.c b/src/main/telemetry/telemetry.c index 98531e65e3..6495adee43 100644 --- a/src/main/telemetry/telemetry.c +++ b/src/main/telemetry/telemetry.c @@ -84,7 +84,8 @@ PG_RESET_TEMPLATE(telemetryConfig_t, telemetryConfig, .position_rate = 2, .extra1_rate = 10, .extra2_rate = 2, - .extra3_rate = 1 + .extra3_rate = 1, + .version = 2 } ); diff --git a/src/main/telemetry/telemetry.h b/src/main/telemetry/telemetry.h index 717e9fec03..3e10e44700 100644 --- a/src/main/telemetry/telemetry.h +++ b/src/main/telemetry/telemetry.h @@ -83,6 +83,7 @@ typedef struct telemetryConfig_s { uint8_t extra1_rate; uint8_t extra2_rate; uint8_t extra3_rate; + uint8_t version; } mavlink; } telemetryConfig_t; From bac73924795b801b16c35b01e5e3b183f7754236 Mon Sep 17 00:00:00 2001 From: Harry Phillips Date: Wed, 24 Mar 2021 16:21:11 +0000 Subject: [PATCH 092/286] Add "prearm was reset" flag to prevent rearming without resetting prearm --- src/main/fc/fc_core.c | 10 +++++++++- 1 file changed, 9 insertions(+), 1 deletion(-) diff --git a/src/main/fc/fc_core.c b/src/main/fc/fc_core.c index 9126728f2a..3e1b24385c 100755 --- a/src/main/fc/fc_core.c +++ b/src/main/fc/fc_core.c @@ -130,6 +130,7 @@ static bool isRXDataNew; static uint32_t gyroSyncFailureCount; static disarmReason_t lastDisarmReason = DISARM_NONE; static emergencyArmingState_t emergencyArming; +static bool prearmWasReset = false; // Prearm must be reset (RC Mode not active) before arming is possible bool isCalibrating(void) { @@ -297,8 +298,13 @@ static void updateArmingStatus(void) if (isModeActivationConditionPresent(BOXPREARM)) { if (IS_RC_MODE_ACTIVE(BOXPREARM)) { - DISABLE_ARMING_FLAG(ARMING_DISABLED_NO_PREARM); + if (prearmWasReset) { + DISABLE_ARMING_FLAG(ARMING_DISABLED_NO_PREARM); + } else { + ENABLE_ARMING_FLAG(ARMING_DISABLED_NO_PREARM); + } } else { + prearmWasReset = true; ENABLE_ARMING_FLAG(ARMING_DISABLED_NO_PREARM); } } else { @@ -410,6 +416,8 @@ void disarm(disarmReason_t disarmReason) programmingPidReset(); #endif beeper(BEEPER_DISARMING); // emit disarm tone + + prearmWasReset = false; } } From b3dc93299cf1b0e4782476d4a72e4f1f10531d16 Mon Sep 17 00:00:00 2001 From: Harry Phillips Date: Wed, 24 Mar 2021 21:00:47 +0000 Subject: [PATCH 093/286] Bugfix --- src/main/telemetry/mavlink.c | 19 +++++++------------ 1 file changed, 7 insertions(+), 12 deletions(-) diff --git a/src/main/telemetry/mavlink.c b/src/main/telemetry/mavlink.c index 3166fb99f7..0ad0cc9279 100644 --- a/src/main/telemetry/mavlink.c +++ b/src/main/telemetry/mavlink.c @@ -318,7 +318,7 @@ void checkMAVLinkTelemetryState(void) static void mavlinkSendMessage(void) { uint8_t mavBuffer[MAVLINK_MAX_PACKET_LEN]; - if (telemetryConfig()->mavlink.version == 1) mavSendMsg.magic = MAVLINK_STX_MAVLINK1; // TODO test this switches to MAVLink v1 + if (telemetryConfig()->mavlink.version == 1) mavSendMsg.magic = MAVLINK_STX_MAVLINK1; int msgLength = mavlink_msg_to_send_buffer(mavBuffer, &mavSendMsg); for (int i = 0; i < msgLength; i++) { @@ -607,11 +607,11 @@ void mavlinkSendAttitude(void) // yaw Yaw angle (rad) DECIDEGREES_TO_RADIANS(attitude.values.yaw), // rollspeed Roll angular speed (rad/s) - imuMeasuredRotationBF.x, // TODO check if this is the correct axis + gyro.gyroADCf[FD_ROLL], // pitchspeed Pitch angular speed (rad/s) - imuMeasuredRotationBF.y, // TODO check if this is the correct axis + gyro.gyroADCf[FD_PITCH], // yawspeed Yaw angular speed (rad/s) - imuMeasuredRotationBF.z); // TODO check if this is the correct axis + gyro.gyroADCf[FD_YAW]); mavlinkSendMessage(); } @@ -1045,14 +1045,9 @@ static bool handleIncoming_MISSION_REQUEST(void) static bool handleIncoming_RC_CHANNELS_OVERRIDE(void) { mavlink_rc_channels_override_t msg; mavlink_msg_rc_channels_override_decode(&mavRecvMsg, &msg); - - // Check if this message is for us - if (msg.target_system == mavSystemId) { - mavlinkRxHandleMessage(&msg); - return true; - } - - return false; + // Don't check system ID because it's not configurable with systems like Crossfire + mavlinkRxHandleMessage(&msg); + return true; } static bool processMAVLinkIncomingTelemetry(void) From 0db5d891eb01c20b3b7b4df868d877d0abfa90a0 Mon Sep 17 00:00:00 2001 From: Harry Phillips Date: Wed, 24 Mar 2021 21:35:17 +0000 Subject: [PATCH 094/286] Fix generate script permissions --- lib/main/MAVLink/generate.sh | 0 1 file changed, 0 insertions(+), 0 deletions(-) mode change 100644 => 100755 lib/main/MAVLink/generate.sh diff --git a/lib/main/MAVLink/generate.sh b/lib/main/MAVLink/generate.sh old mode 100644 new mode 100755 From 80c1d488a95e27e6d4ed5d7ae257d16c9646a388 Mon Sep 17 00:00:00 2001 From: Harry Phillips Date: Wed, 24 Mar 2021 21:59:21 +0000 Subject: [PATCH 095/286] Add optional Prearm timeout --- src/main/fc/fc_core.c | 5 ++++- src/main/fc/rc_controls.h | 1 + src/main/fc/settings.yaml | 6 ++++++ 3 files changed, 11 insertions(+), 1 deletion(-) diff --git a/src/main/fc/fc_core.c b/src/main/fc/fc_core.c index 3e1b24385c..69dd548a7b 100755 --- a/src/main/fc/fc_core.c +++ b/src/main/fc/fc_core.c @@ -130,7 +130,9 @@ static bool isRXDataNew; static uint32_t gyroSyncFailureCount; static disarmReason_t lastDisarmReason = DISARM_NONE; static emergencyArmingState_t emergencyArming; + static bool prearmWasReset = false; // Prearm must be reset (RC Mode not active) before arming is possible +static timeMs_t prearmActivationTime = 0; bool isCalibrating(void) { @@ -298,13 +300,14 @@ static void updateArmingStatus(void) if (isModeActivationConditionPresent(BOXPREARM)) { if (IS_RC_MODE_ACTIVE(BOXPREARM)) { - if (prearmWasReset) { + if (prearmWasReset && (armingConfig()->prearmTimeoutMs == 0 || millis() - prearmActivationTime < armingConfig()->prearmTimeoutMs)) { DISABLE_ARMING_FLAG(ARMING_DISABLED_NO_PREARM); } else { ENABLE_ARMING_FLAG(ARMING_DISABLED_NO_PREARM); } } else { prearmWasReset = true; + prearmActivationTime = millis(); ENABLE_ARMING_FLAG(ARMING_DISABLED_NO_PREARM); } } else { diff --git a/src/main/fc/rc_controls.h b/src/main/fc/rc_controls.h index b229de2311..f5258a9c9e 100644 --- a/src/main/fc/rc_controls.h +++ b/src/main/fc/rc_controls.h @@ -97,6 +97,7 @@ typedef struct armingConfig_s { uint8_t fixed_wing_auto_arm; // Auto-arm fixed wing aircraft on throttle up and never disarm uint8_t disarm_kill_switch; // allow disarm via AUX switch regardless of throttle value uint16_t switchDisarmDelayMs; // additional delay between ARM box going off and actual disarm + uint16_t prearmTimeoutMs; // duration for which Prearm being activated is valid. after this, Prearm needs to be reset. 0 means Prearm does not timeout. } armingConfig_t; PG_DECLARE(armingConfig_t, armingConfig); diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index df53038e56..5a3f0d7b55 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -1236,6 +1236,12 @@ groups: field: switchDisarmDelayMs min: 0 max: 1000 + - name: prearm_timeout + description: "Duration (ms) for which Prearm being activated is valid. after this, Prearm needs to be reset. 0 means Prearm does not timeout." + default_value: "0" + field: prearmTimeoutMs + min: 0 + max: 10000 - name: PG_GENERAL_SETTINGS headers: ["config/general_settings.h"] From 18ece44e83fbb4cf5a9a37b6f3d5aae0363e0b32 Mon Sep 17 00:00:00 2001 From: Harry Phillips Date: Wed, 24 Mar 2021 23:33:25 +0000 Subject: [PATCH 096/286] Update Docs --- docs/Settings.md | 1 + src/main/fc/settings.yaml | 2 ++ 2 files changed, 3 insertions(+) diff --git a/docs/Settings.md b/docs/Settings.md index 8e5d3a9caf..dbc2c626f1 100644 --- a/docs/Settings.md +++ b/docs/Settings.md @@ -221,6 +221,7 @@ | mavlink_extra3_rate | | | | mavlink_pos_rate | | | | mavlink_rc_chan_rate | | | +| mavlink_version | 2 | Version of MAVLink to use | | max_angle_inclination_pit | 300 | Maximum inclination in level (angle) mode (PITCH axis). 100=10° | | max_angle_inclination_rll | 300 | Maximum inclination in level (angle) mode (ROLL axis). 100=10° | | max_check | 1900 | These are min/max values (in us) which, when a channel is smaller (min) or larger (max) than the value will activate various RC commands, such as arming, or stick configuration. Normally, every RC channel should be set so that min = 1000us, max = 2000us. On most transmitters this usually means 125% endpoints. Default check values are 100us above/below this value. | diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 457f864ebd..b875f62e5e 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -2676,6 +2676,8 @@ groups: max: 255 - name: mavlink_version field: mavlink.version + description: "Version of MAVLink to use" + default_value: "2" type: uint8_t min: 1 max: 2 From 39a6fabc8cef3bb8d4829cb0d85ee87ed48f6c95 Mon Sep 17 00:00:00 2001 From: Harry Phillips Date: Wed, 24 Mar 2021 23:38:10 +0000 Subject: [PATCH 097/286] Add unused marker --- src/main/rx/mavlink.c | 1 + 1 file changed, 1 insertion(+) diff --git a/src/main/rx/mavlink.c b/src/main/rx/mavlink.c index 7c349e962f..8b0c46a0f3 100644 --- a/src/main/rx/mavlink.c +++ b/src/main/rx/mavlink.c @@ -59,6 +59,7 @@ void mavlinkRxHandleMessage(const mavlink_rc_channels_override_t *msg) { static uint8_t mavlinkFrameStatus(rxRuntimeConfig_t *rxRuntimeConfig) { + UNUSED(rxRuntimeConfig); if (frameReceived) { frameReceived = false; return RX_FRAME_COMPLETE; From ec0b9a91848804f4d563aaebefeb33879ef5d816 Mon Sep 17 00:00:00 2001 From: Harry Phillips Date: Wed, 24 Mar 2021 23:32:00 +0000 Subject: [PATCH 098/286] Update Docs --- docs/Settings.md | 1 + src/main/fc/settings.yaml | 2 +- 2 files changed, 2 insertions(+), 1 deletion(-) diff --git a/docs/Settings.md b/docs/Settings.md index 8e5d3a9caf..144739d4b7 100644 --- a/docs/Settings.md +++ b/docs/Settings.md @@ -426,6 +426,7 @@ | pitot_scale | | | | platform_type | "MULTIROTOR" | Defines UAV platform type. Allowed values: "MULTIROTOR", "AIRPLANE", "HELICOPTER", "TRICOPTER", "ROVER", "BOAT". Currently only MULTIROTOR, AIRPLANE and TRICOPTER types are implemented | | pos_hold_deadband | 20 | Stick deadband in [r/c points], applied after r/c deadband and expo | +| prearm_timeout | 10000 | Duration (ms) for which Prearm being activated is valid. after this, Prearm needs to be reset. 0 means Prearm does not timeout. | | rangefinder_hardware | NONE | Selection of rangefinder hardware. | | rangefinder_median_filter | OFF | 3-point median filtering for rangefinder readouts | | rate_accel_limit_roll_pitch | 0 | Limits acceleration of ROLL/PITCH rotation speed that can be requested by stick input. In degrees-per-second-squared. Small and powerful UAV flies great with high acceleration limit ( > 5000 dps^2 and even > 10000 dps^2). Big and heavy multirotors will benefit from low acceleration limit (~ 360 dps^2). When set correctly, it greatly improves stopping performance. Value of 0 disables limiting. | diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 5a3f0d7b55..93805fa1a5 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -1238,7 +1238,7 @@ groups: max: 1000 - name: prearm_timeout description: "Duration (ms) for which Prearm being activated is valid. after this, Prearm needs to be reset. 0 means Prearm does not timeout." - default_value: "0" + default_value: "10000" field: prearmTimeoutMs min: 0 max: 10000 From dbe39d3f3bc7d964ccd3acdab56035b61a5b3ab9 Mon Sep 17 00:00:00 2001 From: Tony Yeung Date: Thu, 25 Mar 2021 17:53:24 +0000 Subject: [PATCH 099/286] add more info to safehome_usage_mode --- docs/Settings.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/docs/Settings.md b/docs/Settings.md index 5ce43ec94c..ffe03cd497 100644 --- a/docs/Settings.md +++ b/docs/Settings.md @@ -444,7 +444,7 @@ | rx_spi_protocol | | | | rx_spi_rf_channel_count | | | | safehome_max_distance | 20000 | In order for a safehome to be used, it must be less than this distance (in cm) from the arming point. | -| safehome_usage_mode | RTH | Used to control when safehomes will be used. | +| safehome_usage_mode | RTH | Used to control when safehomes will be used. Possible values are `OFF`, `RTH` and `RTH_FS`. See [Safehome documentation](Safehomes.md#Safehome) for more information. | | sbus_sync_interval | | | | sdcard_detect_inverted | `TARGET dependent` | This setting drives the way SD card is detected in card slot. On some targets (AnyFC F7 clone) different card slot was used and depending of hardware revision ON or OFF setting might be required. If card is not detected, change this value. | | serialrx_halfduplex | AUTO | Allow serial receiver to operate on UART TX pin. With some receivers will allow control and telemetry over a single wire. | From c7e6a74c6d97a2df1914c80f4051fa8869c26088 Mon Sep 17 00:00:00 2001 From: Tony Yeung Date: Thu, 25 Mar 2021 18:05:29 +0000 Subject: [PATCH 100/286] put changes in settings.yaml --- src/main/fc/settings.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index e08b25a6a6..81b5c771db 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -2195,7 +2195,7 @@ groups: min: 0 max: 65000 - name: safehome_usage_mode - description: "Used to control when safehomes will be used." + description: "Used to control when safehomes will be used. Possible values are `OFF`, `RTH` and `RTH_FS`. See [Safehome documentation](Safehomes.md#Safehome) for more information." default_value: "RTH" field: general.flags.safehome_usage_mode table: safehome_usage_mode From 0af4298d136e9de2aa3b5a3e661f03b60e191138 Mon Sep 17 00:00:00 2001 From: Jonathan Hudson Date: Thu, 25 Mar 2021 20:30:55 +0000 Subject: [PATCH 101/286] fix incorrect sdmmc_sdio_f4xx error state (#6748) --- src/main/drivers/sdcard/sdmmc_sdio_f4xx.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/drivers/sdcard/sdmmc_sdio_f4xx.c b/src/main/drivers/sdcard/sdmmc_sdio_f4xx.c index 2734de00ad..592ce4cf64 100644 --- a/src/main/drivers/sdcard/sdmmc_sdio_f4xx.c +++ b/src/main/drivers/sdcard/sdmmc_sdio_f4xx.c @@ -1081,7 +1081,7 @@ SD_Error_t SD_GetStatus(void) } } else { - ErrorState = SD_CARD_ERROR; + ErrorState = SD_ERROR; } return ErrorState; From 8e33c9c5808304c2b64dd7c4f45f9a6a27b508ce Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Marcin=20Ga=C5=82czy=C5=84ski?= Date: Fri, 26 Mar 2021 19:54:42 +0100 Subject: [PATCH 102/286] remove unused functions, add static, check init status --- .../drivers/rangefinder/rangefinder_vl53l1x.c | 867 +++++++++--------- 1 file changed, 435 insertions(+), 432 deletions(-) diff --git a/src/main/drivers/rangefinder/rangefinder_vl53l1x.c b/src/main/drivers/rangefinder/rangefinder_vl53l1x.c index adc251275b..839d06e9dc 100644 --- a/src/main/drivers/rangefinder/rangefinder_vl53l1x.c +++ b/src/main/drivers/rangefinder/rangefinder_vl53l1x.c @@ -350,165 +350,165 @@ typedef struct { /** * @brief This function returns the SW driver version */ -VL53L1X_ERROR VL53L1X_GetSWVersion(VL53L1X_Version_t *pVersion); +// VL53L1X_ERROR VL53L1X_GetSWVersion(VL53L1X_Version_t *pVersion); /** * @brief This function sets the sensor I2C address used in case multiple devices application, default address 0x52 */ -VL53L1X_ERROR VL53L1X_SetI2CAddress(busDevice_t * dev, uint8_t new_address); +// // VL53L1X_ERROR VL53L1X_SetI2CAddress(busDevice_t * dev, uint8_t new_address); -/** - * @brief This function loads the 135 bytes default values to initialize the sensor. - * @param dev Device address - * @return 0:success, != 0:failed - */ +// /** +// * @brief This function loads the 135 bytes default values to initialize the sensor. +// * @param dev Device address +// * @return 0:success, != 0:failed +// */ VL53L1X_ERROR VL53L1X_SensorInit(busDevice_t * dev); /** * @brief This function clears the interrupt, to be called after a ranging data reading * to arm the interrupt for the next data ready event. */ -VL53L1X_ERROR VL53L1X_ClearInterrupt(busDevice_t * dev); +static VL53L1X_ERROR VL53L1X_ClearInterrupt(busDevice_t * dev); /** * @brief This function programs the interrupt polarity\n * 1=active high (default), 0=active low */ -VL53L1X_ERROR VL53L1X_SetInterruptPolarity(busDevice_t * dev, uint8_t IntPol); +// VL53L1X_ERROR VL53L1X_SetInterruptPolarity(busDevice_t * dev, uint8_t IntPol); /** * @brief This function returns the current interrupt polarity\n * 1=active high (default), 0=active low */ -VL53L1X_ERROR VL53L1X_GetInterruptPolarity(busDevice_t * dev, uint8_t *pIntPol); +static VL53L1X_ERROR VL53L1X_GetInterruptPolarity(busDevice_t * dev, uint8_t *pIntPol); /** * @brief This function starts the ranging distance operation\n * The ranging operation is continuous. The clear interrupt has to be done after each get data to allow the interrupt to raise when the next data is ready\n * 1=active high (default), 0=active low, use SetInterruptPolarity() to change the interrupt polarity if required. */ -VL53L1X_ERROR VL53L1X_StartRanging(busDevice_t * dev); +static VL53L1X_ERROR VL53L1X_StartRanging(busDevice_t * dev); /** * @brief This function stops the ranging. */ -VL53L1X_ERROR VL53L1X_StopRanging(busDevice_t * dev); +static VL53L1X_ERROR VL53L1X_StopRanging(busDevice_t * dev); /** * @brief This function checks if the new ranging data is available by polling the dedicated register. * @param : isDataReady==0 -> not ready; isDataReady==1 -> ready */ -VL53L1X_ERROR VL53L1X_CheckForDataReady(busDevice_t * dev, uint8_t *isDataReady); +static VL53L1X_ERROR VL53L1X_CheckForDataReady(busDevice_t * dev, uint8_t *isDataReady); /** * @brief This function programs the timing budget in ms. * Predefined values = 15, 20, 33, 50, 100(default), 200, 500. */ -VL53L1X_ERROR VL53L1X_SetTimingBudgetInMs(busDevice_t * dev, uint16_t TimingBudgetInMs); +static VL53L1X_ERROR VL53L1X_SetTimingBudgetInMs(busDevice_t * dev, uint16_t TimingBudgetInMs); /** * @brief This function returns the current timing budget in ms. */ -VL53L1X_ERROR VL53L1X_GetTimingBudgetInMs(busDevice_t * dev, uint16_t *pTimingBudgetInMs); +static VL53L1X_ERROR VL53L1X_GetTimingBudgetInMs(busDevice_t * dev, uint16_t *pTimingBudgetInMs); /** * @brief This function programs the distance mode (1=short, 2=long(default)). * Short mode max distance is limited to 1.3 m but better ambient immunity.\n * Long mode can range up to 4 m in the dark with 200 ms timing budget. */ -VL53L1X_ERROR VL53L1X_SetDistanceMode(busDevice_t * dev, uint16_t DistanceMode); +static VL53L1X_ERROR VL53L1X_SetDistanceMode(busDevice_t * dev, uint16_t DistanceMode); /** * @brief This function returns the current distance mode (1=short, 2=long). */ -VL53L1X_ERROR VL53L1X_GetDistanceMode(busDevice_t * dev, uint16_t *pDistanceMode); +static VL53L1X_ERROR VL53L1X_GetDistanceMode(busDevice_t * dev, uint16_t *pDistanceMode); /** * @brief This function programs the Intermeasurement period in ms\n * Intermeasurement period must be >/= timing budget. This condition is not checked by the API, * the customer has the duty to check the condition. Default = 100 ms */ -VL53L1X_ERROR VL53L1X_SetInterMeasurementInMs(busDevice_t * dev, +static VL53L1X_ERROR VL53L1X_SetInterMeasurementInMs(busDevice_t * dev, uint32_t InterMeasurementInMs); /** * @brief This function returns the Intermeasurement period in ms. */ -VL53L1X_ERROR VL53L1X_GetInterMeasurementInMs(busDevice_t * dev, uint16_t * pIM); +// VL53L1X_ERROR VL53L1X_GetInterMeasurementInMs(busDevice_t * dev, uint16_t * pIM); /** * @brief This function returns the boot state of the device (1:booted, 0:not booted) */ -VL53L1X_ERROR VL53L1X_BootState(busDevice_t * dev, uint8_t *state); +// VL53L1X_ERROR VL53L1X_BootState(busDevice_t * dev, uint8_t *state); /** * @brief This function returns the sensor id, sensor Id must be 0xEEAC */ -VL53L1X_ERROR VL53L1X_GetSensorId(busDevice_t * dev, uint16_t *id); +// VL53L1X_ERROR VL53L1X_GetSensorId(busDevice_t * dev, uint16_t *id); /** * @brief This function returns the distance measured by the sensor in mm */ -VL53L1X_ERROR VL53L1X_GetDistance(busDevice_t * dev, uint16_t *distance); +static VL53L1X_ERROR VL53L1X_GetDistance(busDevice_t * dev, uint16_t *distance); /** * @brief This function returns the returned signal per SPAD in kcps/SPAD. * With kcps stands for Kilo Count Per Second */ -VL53L1X_ERROR VL53L1X_GetSignalPerSpad(busDevice_t * dev, uint16_t *signalPerSp); +// VL53L1X_ERROR VL53L1X_GetSignalPerSpad(busDevice_t * dev, uint16_t *signalPerSp); /** * @brief This function returns the ambient per SPAD in kcps/SPAD */ -VL53L1X_ERROR VL53L1X_GetAmbientPerSpad(busDevice_t * dev, uint16_t *amb); +// VL53L1X_ERROR VL53L1X_GetAmbientPerSpad(busDevice_t * dev, uint16_t *amb); /** * @brief This function returns the returned signal in kcps. */ -VL53L1X_ERROR VL53L1X_GetSignalRate(busDevice_t * dev, uint16_t *signalRate); +// static VL53L1X_ERROR VL53L1X_GetSignalRate(busDevice_t * dev, uint16_t *signalRate); /** * @brief This function returns the current number of enabled SPADs */ -VL53L1X_ERROR VL53L1X_GetSpadNb(busDevice_t * dev, uint16_t *spNb); +// static VL53L1X_ERROR VL53L1X_GetSpadNb(busDevice_t * dev, uint16_t *spNb); /** * @brief This function returns the ambient rate in kcps */ -VL53L1X_ERROR VL53L1X_GetAmbientRate(busDevice_t * dev, uint16_t *ambRate); +// VL53L1X_ERROR VL53L1X_GetAmbientRate(busDevice_t * dev, uint16_t *ambRate); /** * @brief This function returns the ranging status error \n * (0:no error, 1:sigma failed, 2:signal failed, ..., 7:wrap-around) */ -VL53L1X_ERROR VL53L1X_GetRangeStatus(busDevice_t * dev, uint8_t *rangeStatus); +// VL53L1X_ERROR VL53L1X_GetRangeStatus(busDevice_t * dev, uint8_t *rangeStatus); /** * @brief This function returns measurements and the range status in a single read access */ -VL53L1X_ERROR VL53L1X_GetResult(busDevice_t * dev, VL53L1X_Result_t *pResult); +// VL53L1X_ERROR VL53L1X_GetResult(busDevice_t * dev, VL53L1X_Result_t *pResult); /** * @brief This function programs the offset correction in mm * @param OffsetValue:the offset correction value to program in mm */ -VL53L1X_ERROR VL53L1X_SetOffset(busDevice_t * dev, int16_t OffsetValue); +// VL53L1X_ERROR VL53L1X_SetOffset(busDevice_t * dev, int16_t OffsetValue); /** * @brief This function returns the programmed offset correction value in mm */ -VL53L1X_ERROR VL53L1X_GetOffset(busDevice_t * dev, int16_t *Offset); +// VL53L1X_ERROR VL53L1X_GetOffset(busDevice_t * dev, int16_t *Offset); /** * @brief This function programs the xtalk correction value in cps (Count Per Second).\n * This is the number of photons reflected back from the cover glass in cps. */ -VL53L1X_ERROR VL53L1X_SetXtalk(busDevice_t * dev, uint16_t XtalkValue); +// VL53L1X_ERROR VL53L1X_SetXtalk(busDevice_t * dev, uint16_t XtalkValue); /** * @brief This function returns the current programmed xtalk correction value in cps */ -VL53L1X_ERROR VL53L1X_GetXtalk(busDevice_t * dev, uint16_t *Xtalk); +// VL53L1X_ERROR VL53L1X_GetXtalk(busDevice_t * dev, uint16_t *Xtalk); /** * @brief This function programs the threshold detection mode\n @@ -523,24 +523,24 @@ VL53L1X_ERROR VL53L1X_GetXtalk(busDevice_t * dev, uint16_t *Xtalk); * @param Window detection mode : 0=below, 1=above, 2=out, 3=in * @param IntOnNoTarget = 0 (No longer used - just use 0) */ -VL53L1X_ERROR VL53L1X_SetDistanceThreshold(busDevice_t * dev, uint16_t ThreshLow, - uint16_t ThreshHigh, uint8_t Window, - uint8_t IntOnNoTarget); +// VL53L1X_ERROR VL53L1X_SetDistanceThreshold(busDevice_t * dev, uint16_t ThreshLow, +// uint16_t ThreshHigh, uint8_t Window, +// uint8_t IntOnNoTarget); /** * @brief This function returns the window detection mode (0=below; 1=above; 2=out; 3=in) */ -VL53L1X_ERROR VL53L1X_GetDistanceThresholdWindow(busDevice_t * dev, uint16_t *window); +// VL53L1X_ERROR VL53L1X_GetDistanceThresholdWindow(busDevice_t * dev, uint16_t *window); /** * @brief This function returns the low threshold in mm */ -VL53L1X_ERROR VL53L1X_GetDistanceThresholdLow(busDevice_t * dev, uint16_t *low); +// VL53L1X_ERROR VL53L1X_GetDistanceThresholdLow(busDevice_t * dev, uint16_t *low); /** * @brief This function returns the high threshold in mm */ -VL53L1X_ERROR VL53L1X_GetDistanceThresholdHigh(busDevice_t * dev, uint16_t *high); +// VL53L1X_ERROR VL53L1X_GetDistanceThresholdHigh(busDevice_t * dev, uint16_t *high); /** * @brief This function programs the ROI (Region of Interest)\n @@ -548,50 +548,50 @@ VL53L1X_ERROR VL53L1X_GetDistanceThresholdHigh(busDevice_t * dev, uint16_t *high * The smallest acceptable ROI size = 4\n * @param X:ROI Width; Y=ROI Height */ -VL53L1X_ERROR VL53L1X_SetROI(busDevice_t * dev, uint16_t X, uint16_t Y); +// VL53L1X_ERROR VL53L1X_SetROI(busDevice_t * dev, uint16_t X, uint16_t Y); /** *@brief This function returns width X and height Y */ -VL53L1X_ERROR VL53L1X_GetROI_XY(busDevice_t * dev, uint16_t *ROI_X, uint16_t *ROI_Y); +// VL53L1X_ERROR VL53L1X_GetROI_XY(busDevice_t * dev, uint16_t *ROI_X, uint16_t *ROI_Y); /** *@brief This function programs the new user ROI center, please to be aware that there is no check in this function. *if the ROI center vs ROI size is out of border the ranging function return error #13 */ -VL53L1X_ERROR VL53L1X_SetROICenter(busDevice_t * dev, uint8_t ROICenter); +// VL53L1X_ERROR VL53L1X_SetROICenter(busDevice_t * dev, uint8_t ROICenter); /** *@brief This function returns the current user ROI center */ -VL53L1X_ERROR VL53L1X_GetROICenter(busDevice_t * dev, uint8_t *ROICenter); +// VL53L1X_ERROR VL53L1X_GetROICenter(busDevice_t * dev, uint8_t *ROICenter); /** * @brief This function programs a new signal threshold in kcps (default=1024 kcps\n */ -VL53L1X_ERROR VL53L1X_SetSignalThreshold(busDevice_t * dev, uint16_t signal); +// VL53L1X_ERROR VL53L1X_SetSignalThreshold(busDevice_t * dev, uint16_t signal); /** * @brief This function returns the current signal threshold in kcps */ -VL53L1X_ERROR VL53L1X_GetSignalThreshold(busDevice_t * dev, uint16_t *signal); +// VL53L1X_ERROR VL53L1X_GetSignalThreshold(busDevice_t * dev, uint16_t *signal); /** * @brief This function programs a new sigma threshold in mm (default=15 mm) */ -VL53L1X_ERROR VL53L1X_SetSigmaThreshold(busDevice_t * dev, uint16_t sigma); +// VL53L1X_ERROR VL53L1X_SetSigmaThreshold(busDevice_t * dev, uint16_t sigma); /** * @brief This function returns the current sigma threshold in mm */ -VL53L1X_ERROR VL53L1X_GetSigmaThreshold(busDevice_t * dev, uint16_t *signal); +// VL53L1X_ERROR VL53L1X_GetSigmaThreshold(busDevice_t * dev, uint16_t *signal); /** * @brief This function performs the temperature calibration. * It is recommended to call this function any time the temperature might have changed by more than 8 deg C * without sensor ranging activity for an extended period. */ -VL53L1X_ERROR VL53L1X_StartTemperatureUpdate(busDevice_t * dev); +// VL53L1X_ERROR VL53L1X_StartTemperatureUpdate(busDevice_t * dev); /** * @brief This function performs the offset calibration.\n @@ -601,7 +601,7 @@ VL53L1X_ERROR VL53L1X_StartTemperatureUpdate(busDevice_t * dev); * @return 0:success, !=0: failed * @return offset pointer contains the offset found in mm */ -int8_t VL53L1X_CalibrateOffset(busDevice_t * dev, uint16_t TargetDistInMm, int16_t *offset); +// int8_t VL53L1X_CalibrateOffset(busDevice_t * dev, uint16_t TargetDistInMm, int16_t *offset); /** * @brief This function performs the xtalk calibration.\n @@ -614,7 +614,7 @@ int8_t VL53L1X_CalibrateOffset(busDevice_t * dev, uint16_t TargetDistInMm, int16 * @return 0: success, !=0: failed * @return xtalk pointer contains the xtalk value found in cps (number of photons in count per second) */ -int8_t VL53L1X_CalibrateXtalk(busDevice_t * dev, uint16_t TargetDistInMm, uint16_t *xtalk); +// int8_t VL53L1X_CalibrateXtalk(busDevice_t * dev, uint16_t TargetDistInMm, uint16_t *xtalk); @@ -712,10 +712,10 @@ const uint8_t VL51L1X_DEFAULT_CONFIGURATION[] = { 0x00 /* 0x87 : start ranging, use StartRanging() or StopRanging(), If you want an automatic start after VL53L1X_init() call, put 0x40 in location 0x87 */ }; -static const uint8_t status_rtn[24] = { 255, 255, 255, 5, 2, 4, 1, 7, 3, 0, - 255, 255, 9, 13, 255, 255, 255, 255, 10, 6, - 255, 255, 11, 12 -}; +// static const uint8_t status_rtn[24] = { 255, 255, 255, 5, 2, 4, 1, 7, 3, 0, +// 255, 255, 9, 13, 255, 255, 255, 255, 10, 6, +// 255, 255, 11, 12 +// }; static uint8_t _I2CBuffer[256]; @@ -892,24 +892,24 @@ done: return Status; } -VL53L1X_ERROR VL53L1X_GetSWVersion(VL53L1X_Version_t *pVersion) -{ - VL53L1X_ERROR Status = 0; +// VL53L1X_ERROR VL53L1X_GetSWVersion(VL53L1X_Version_t *pVersion) +// { +// VL53L1X_ERROR Status = 0; - pVersion->major = VL53L1X_IMPLEMENTATION_VER_MAJOR; - pVersion->minor = VL53L1X_IMPLEMENTATION_VER_MINOR; - pVersion->build = VL53L1X_IMPLEMENTATION_VER_SUB; - pVersion->revision = VL53L1X_IMPLEMENTATION_VER_REVISION; - return Status; -} +// pVersion->major = VL53L1X_IMPLEMENTATION_VER_MAJOR; +// pVersion->minor = VL53L1X_IMPLEMENTATION_VER_MINOR; +// pVersion->build = VL53L1X_IMPLEMENTATION_VER_SUB; +// pVersion->revision = VL53L1X_IMPLEMENTATION_VER_REVISION; +// return Status; +// } -VL53L1X_ERROR VL53L1X_SetI2CAddress(busDevice_t * dev, uint8_t new_address) -{ - VL53L1X_ERROR status = 0; +// VL53L1X_ERROR VL53L1X_SetI2CAddress(busDevice_t * dev, uint8_t new_address) +// { +// VL53L1X_ERROR status = 0; - status = VL53L1_WrByte(dev, VL53L1_I2C_SLAVE__DEVICE_ADDRESS, new_address >> 1); - return status; -} +// status = VL53L1_WrByte(dev, VL53L1_I2C_SLAVE__DEVICE_ADDRESS, new_address >> 1); +// return status; +// } VL53L1X_ERROR VL53L1X_SensorInit(busDevice_t * dev) { @@ -931,7 +931,7 @@ VL53L1X_ERROR VL53L1X_SensorInit(busDevice_t * dev) return status; } -VL53L1X_ERROR VL53L1X_ClearInterrupt(busDevice_t * dev) +static VL53L1X_ERROR VL53L1X_ClearInterrupt(busDevice_t * dev) { VL53L1X_ERROR status = 0; @@ -939,18 +939,18 @@ VL53L1X_ERROR VL53L1X_ClearInterrupt(busDevice_t * dev) return status; } -VL53L1X_ERROR VL53L1X_SetInterruptPolarity(busDevice_t * dev, uint8_t NewPolarity) -{ - uint8_t Temp; - VL53L1X_ERROR status = 0; +// VL53L1X_ERROR VL53L1X_SetInterruptPolarity(busDevice_t * dev, uint8_t NewPolarity) +// { +// uint8_t Temp; +// VL53L1X_ERROR status = 0; - status = VL53L1_RdByte(dev, GPIO_HV_MUX__CTRL, &Temp); - Temp = Temp & 0xEF; - status = VL53L1_WrByte(dev, GPIO_HV_MUX__CTRL, Temp | (!(NewPolarity & 1)) << 4); - return status; -} +// status = VL53L1_RdByte(dev, GPIO_HV_MUX__CTRL, &Temp); +// Temp = Temp & 0xEF; +// status = VL53L1_WrByte(dev, GPIO_HV_MUX__CTRL, Temp | (!(NewPolarity & 1)) << 4); +// return status; +// } -VL53L1X_ERROR VL53L1X_GetInterruptPolarity(busDevice_t * dev, uint8_t *pInterruptPolarity) +static VL53L1X_ERROR VL53L1X_GetInterruptPolarity(busDevice_t * dev, uint8_t *pInterruptPolarity) { uint8_t Temp; VL53L1X_ERROR status = 0; @@ -961,7 +961,7 @@ VL53L1X_ERROR VL53L1X_GetInterruptPolarity(busDevice_t * dev, uint8_t *pInterrup return status; } -VL53L1X_ERROR VL53L1X_StartRanging(busDevice_t * dev) +static VL53L1X_ERROR VL53L1X_StartRanging(busDevice_t * dev) { VL53L1X_ERROR status = 0; @@ -969,7 +969,7 @@ VL53L1X_ERROR VL53L1X_StartRanging(busDevice_t * dev) return status; } -VL53L1X_ERROR VL53L1X_StopRanging(busDevice_t * dev) +static VL53L1X_ERROR VL53L1X_StopRanging(busDevice_t * dev) { VL53L1X_ERROR status = 0; @@ -977,7 +977,7 @@ VL53L1X_ERROR VL53L1X_StopRanging(busDevice_t * dev) return status; } -VL53L1X_ERROR VL53L1X_CheckForDataReady(busDevice_t * dev, uint8_t *isDataReady) +static VL53L1X_ERROR VL53L1X_CheckForDataReady(busDevice_t * dev, uint8_t *isDataReady) { uint8_t Temp; uint8_t IntPol; @@ -995,7 +995,7 @@ VL53L1X_ERROR VL53L1X_CheckForDataReady(busDevice_t * dev, uint8_t *isDataReady) return status; } -VL53L1X_ERROR VL53L1X_SetTimingBudgetInMs(busDevice_t * dev, uint16_t TimingBudgetInMs) +static VL53L1X_ERROR VL53L1X_SetTimingBudgetInMs(busDevice_t * dev, uint16_t TimingBudgetInMs) { uint16_t DM; VL53L1X_ERROR status=0; @@ -1097,7 +1097,7 @@ VL53L1X_ERROR VL53L1X_SetTimingBudgetInMs(busDevice_t * dev, uint16_t TimingBudg return status; } -VL53L1X_ERROR VL53L1X_GetTimingBudgetInMs(busDevice_t * dev, uint16_t *pTimingBudget) +static VL53L1X_ERROR VL53L1X_GetTimingBudgetInMs(busDevice_t * dev, uint16_t *pTimingBudget) { uint16_t Temp; VL53L1X_ERROR status = 0; @@ -1138,7 +1138,7 @@ VL53L1X_ERROR VL53L1X_GetTimingBudgetInMs(busDevice_t * dev, uint16_t *pTimingBu return status; } -VL53L1X_ERROR VL53L1X_SetDistanceMode(busDevice_t * dev, uint16_t DM) +static VL53L1X_ERROR VL53L1X_SetDistanceMode(busDevice_t * dev, uint16_t DM) { uint16_t TB; VL53L1X_ERROR status = 0; @@ -1173,7 +1173,7 @@ VL53L1X_ERROR VL53L1X_SetDistanceMode(busDevice_t * dev, uint16_t DM) return status; } -VL53L1X_ERROR VL53L1X_GetDistanceMode(busDevice_t * dev, uint16_t *DM) +static VL53L1X_ERROR VL53L1X_GetDistanceMode(busDevice_t * dev, uint16_t *DM) { uint8_t TempDM, status=0; @@ -1185,7 +1185,7 @@ VL53L1X_ERROR VL53L1X_GetDistanceMode(busDevice_t * dev, uint16_t *DM) return status; } -VL53L1X_ERROR VL53L1X_SetInterMeasurementInMs(busDevice_t * dev, uint32_t InterMeasMs) +static VL53L1X_ERROR VL53L1X_SetInterMeasurementInMs(busDevice_t * dev, uint32_t InterMeasMs) { uint16_t ClockPLL; VL53L1X_ERROR status = 0; @@ -1198,41 +1198,41 @@ VL53L1X_ERROR VL53L1X_SetInterMeasurementInMs(busDevice_t * dev, uint32_t InterM } -VL53L1X_ERROR VL53L1X_GetInterMeasurementInMs(busDevice_t * dev, uint16_t *pIM) -{ - uint16_t ClockPLL; - VL53L1X_ERROR status = 0; - uint32_t tmp; +// VL53L1X_ERROR VL53L1X_GetInterMeasurementInMs(busDevice_t * dev, uint16_t *pIM) +// { +// uint16_t ClockPLL; +// VL53L1X_ERROR status = 0; +// uint32_t tmp; - status = VL53L1_RdDWord(dev,VL53L1_SYSTEM__INTERMEASUREMENT_PERIOD, &tmp); - *pIM = (uint16_t)tmp; - status = VL53L1_RdWord(dev, VL53L1_RESULT__OSC_CALIBRATE_VAL, &ClockPLL); - ClockPLL = ClockPLL&0x3FF; - *pIM= (uint16_t)(*pIM/(ClockPLL*1.065)); - return status; -} +// status = VL53L1_RdDWord(dev,VL53L1_SYSTEM__INTERMEASUREMENT_PERIOD, &tmp); +// *pIM = (uint16_t)tmp; +// status = VL53L1_RdWord(dev, VL53L1_RESULT__OSC_CALIBRATE_VAL, &ClockPLL); +// ClockPLL = ClockPLL&0x3FF; +// *pIM= (uint16_t)(*pIM/(ClockPLL*1.065)); +// return status; +// } -VL53L1X_ERROR VL53L1X_BootState(busDevice_t * dev, uint8_t *state) -{ - VL53L1X_ERROR status = 0; - uint8_t tmp = 0; +// VL53L1X_ERROR VL53L1X_BootState(busDevice_t * dev, uint8_t *state) +// { +// VL53L1X_ERROR status = 0; +// uint8_t tmp = 0; - status = VL53L1_RdByte(dev,VL53L1_FIRMWARE__SYSTEM_STATUS, &tmp); - *state = tmp; - return status; -} +// status = VL53L1_RdByte(dev,VL53L1_FIRMWARE__SYSTEM_STATUS, &tmp); +// *state = tmp; +// return status; +// } -VL53L1X_ERROR VL53L1X_GetSensorId(busDevice_t * dev, uint16_t *sensorId) -{ - VL53L1X_ERROR status = 0; - uint16_t tmp = 0; +// VL53L1X_ERROR VL53L1X_GetSensorId(busDevice_t * dev, uint16_t *sensorId) +// { +// VL53L1X_ERROR status = 0; +// uint16_t tmp = 0; - status = VL53L1_RdWord(dev, VL53L1_IDENTIFICATION__MODEL_ID, &tmp); - *sensorId = tmp; - return status; -} +// status = VL53L1_RdWord(dev, VL53L1_IDENTIFICATION__MODEL_ID, &tmp); +// *sensorId = tmp; +// return status; +// } -VL53L1X_ERROR VL53L1X_GetDistance(busDevice_t * dev, uint16_t *distance) +static VL53L1X_ERROR VL53L1X_GetDistance(busDevice_t * dev, uint16_t *distance) { VL53L1X_ERROR status = 0; uint16_t tmp; @@ -1243,375 +1243,378 @@ VL53L1X_ERROR VL53L1X_GetDistance(busDevice_t * dev, uint16_t *distance) return status; } -VL53L1X_ERROR VL53L1X_GetSignalPerSpad(busDevice_t * dev, uint16_t *signalRate) -{ - VL53L1X_ERROR status = 0; - uint16_t SpNb=1, signal; +// VL53L1X_ERROR VL53L1X_GetSignalPerSpad(busDevice_t * dev, uint16_t *signalRate) +// { +// VL53L1X_ERROR status = 0; +// uint16_t SpNb=1, signal; - status = VL53L1_RdWord(dev, - VL53L1_RESULT__PEAK_SIGNAL_COUNT_RATE_CROSSTALK_CORRECTED_MCPS_SD0, &signal); - status = VL53L1_RdWord(dev, - VL53L1_RESULT__DSS_ACTUAL_EFFECTIVE_SPADS_SD0, &SpNb); - *signalRate = (uint16_t) (200.0*signal/SpNb); - return status; -} +// status = VL53L1_RdWord(dev, +// VL53L1_RESULT__PEAK_SIGNAL_COUNT_RATE_CROSSTALK_CORRECTED_MCPS_SD0, &signal); +// status = VL53L1_RdWord(dev, +// VL53L1_RESULT__DSS_ACTUAL_EFFECTIVE_SPADS_SD0, &SpNb); +// *signalRate = (uint16_t) (200.0*signal/SpNb); +// return status; +// } -VL53L1X_ERROR VL53L1X_GetAmbientPerSpad(busDevice_t * dev, uint16_t *ambPerSp) -{ - VL53L1X_ERROR status = 0; - uint16_t AmbientRate, SpNb = 1; +// VL53L1X_ERROR VL53L1X_GetAmbientPerSpad(busDevice_t * dev, uint16_t *ambPerSp) +// { +// VL53L1X_ERROR status = 0; +// uint16_t AmbientRate, SpNb = 1; - status = VL53L1_RdWord(dev, RESULT__AMBIENT_COUNT_RATE_MCPS_SD, &AmbientRate); - status = VL53L1_RdWord(dev, VL53L1_RESULT__DSS_ACTUAL_EFFECTIVE_SPADS_SD0, &SpNb); - *ambPerSp=(uint16_t) (200.0 * AmbientRate / SpNb); - return status; -} +// status = VL53L1_RdWord(dev, RESULT__AMBIENT_COUNT_RATE_MCPS_SD, &AmbientRate); +// status = VL53L1_RdWord(dev, VL53L1_RESULT__DSS_ACTUAL_EFFECTIVE_SPADS_SD0, &SpNb); +// *ambPerSp=(uint16_t) (200.0 * AmbientRate / SpNb); +// return status; +// } -VL53L1X_ERROR VL53L1X_GetSignalRate(busDevice_t * dev, uint16_t *signal) -{ - VL53L1X_ERROR status = 0; - uint16_t tmp; +// static VL53L1X_ERROR VL53L1X_GetSignalRate(busDevice_t * dev, uint16_t *signal) +// { +// VL53L1X_ERROR status = 0; +// uint16_t tmp; - status = VL53L1_RdWord(dev, - VL53L1_RESULT__PEAK_SIGNAL_COUNT_RATE_CROSSTALK_CORRECTED_MCPS_SD0, &tmp); - *signal = tmp*8; - return status; -} +// status = VL53L1_RdWord(dev, +// VL53L1_RESULT__PEAK_SIGNAL_COUNT_RATE_CROSSTALK_CORRECTED_MCPS_SD0, &tmp); +// *signal = tmp*8; +// return status; +// } -VL53L1X_ERROR VL53L1X_GetSpadNb(busDevice_t * dev, uint16_t *spNb) -{ - VL53L1X_ERROR status = 0; - uint16_t tmp; +// static VL53L1X_ERROR VL53L1X_GetSpadNb(busDevice_t * dev, uint16_t *spNb) +// { +// VL53L1X_ERROR status = 0; +// uint16_t tmp; - status = VL53L1_RdWord(dev, - VL53L1_RESULT__DSS_ACTUAL_EFFECTIVE_SPADS_SD0, &tmp); - *spNb = tmp >> 8; - return status; -} +// status = VL53L1_RdWord(dev, +// VL53L1_RESULT__DSS_ACTUAL_EFFECTIVE_SPADS_SD0, &tmp); +// *spNb = tmp >> 8; +// return status; +// } -VL53L1X_ERROR VL53L1X_GetAmbientRate(busDevice_t * dev, uint16_t *ambRate) -{ - VL53L1X_ERROR status = 0; - uint16_t tmp; +// VL53L1X_ERROR VL53L1X_GetAmbientRate(busDevice_t * dev, uint16_t *ambRate) +// { +// VL53L1X_ERROR status = 0; +// uint16_t tmp; - status = VL53L1_RdWord(dev, RESULT__AMBIENT_COUNT_RATE_MCPS_SD, &tmp); - *ambRate = tmp*8; - return status; -} +// status = VL53L1_RdWord(dev, RESULT__AMBIENT_COUNT_RATE_MCPS_SD, &tmp); +// *ambRate = tmp*8; +// return status; +// } -VL53L1X_ERROR VL53L1X_GetRangeStatus(busDevice_t * dev, uint8_t *rangeStatus) -{ - VL53L1X_ERROR status = 0; - uint8_t RgSt; +// VL53L1X_ERROR VL53L1X_GetRangeStatus(busDevice_t * dev, uint8_t *rangeStatus) +// { +// VL53L1X_ERROR status = 0; +// uint8_t RgSt; - *rangeStatus = 255; - status = VL53L1_RdByte(dev, VL53L1_RESULT__RANGE_STATUS, &RgSt); - RgSt = RgSt & 0x1F; - if (RgSt < 24) - *rangeStatus = status_rtn[RgSt]; - return status; -} +// *rangeStatus = 255; +// status = VL53L1_RdByte(dev, VL53L1_RESULT__RANGE_STATUS, &RgSt); +// RgSt = RgSt & 0x1F; +// if (RgSt < 24) +// *rangeStatus = status_rtn[RgSt]; +// return status; +// } -VL53L1X_ERROR VL53L1X_GetResult(busDevice_t * dev, VL53L1X_Result_t *pResult) -{ - VL53L1X_ERROR status = 0; - uint8_t Temp[17]; - uint8_t RgSt = 255; +// VL53L1X_ERROR VL53L1X_GetResult(busDevice_t * dev, VL53L1X_Result_t *pResult) +// { +// VL53L1X_ERROR status = 0; +// uint8_t Temp[17]; +// uint8_t RgSt = 255; - status = VL53L1_ReadMulti(dev, VL53L1_RESULT__RANGE_STATUS, Temp, 17); - RgSt = Temp[0] & 0x1F; - if (RgSt < 24) - RgSt = status_rtn[RgSt]; - pResult->Status = RgSt; - pResult->Ambient = (Temp[7] << 8 | Temp[8]) * 8; - pResult->NumSPADs = Temp[3]; - pResult->SigPerSPAD = (Temp[15] << 8 | Temp[16]) * 8; - pResult->Distance = Temp[13] << 8 | Temp[14]; +// status = VL53L1_ReadMulti(dev, VL53L1_RESULT__RANGE_STATUS, Temp, 17); +// RgSt = Temp[0] & 0x1F; +// if (RgSt < 24) +// RgSt = status_rtn[RgSt]; +// pResult->Status = RgSt; +// pResult->Ambient = (Temp[7] << 8 | Temp[8]) * 8; +// pResult->NumSPADs = Temp[3]; +// pResult->SigPerSPAD = (Temp[15] << 8 | Temp[16]) * 8; +// pResult->Distance = Temp[13] << 8 | Temp[14]; - return status; -} +// return status; +// } -VL53L1X_ERROR VL53L1X_SetOffset(busDevice_t * dev, int16_t OffsetValue) -{ - VL53L1X_ERROR status = 0; - int16_t Temp; +// VL53L1X_ERROR VL53L1X_SetOffset(busDevice_t * dev, int16_t OffsetValue) +// { +// VL53L1X_ERROR status = 0; +// int16_t Temp; - Temp = (OffsetValue*4); - VL53L1_WrWord(dev, ALGO__PART_TO_PART_RANGE_OFFSET_MM, - (uint16_t)Temp); - VL53L1_WrWord(dev, MM_CONFIG__INNER_OFFSET_MM, 0x0); - VL53L1_WrWord(dev, MM_CONFIG__OUTER_OFFSET_MM, 0x0); - return status; -} +// Temp = (OffsetValue*4); +// VL53L1_WrWord(dev, ALGO__PART_TO_PART_RANGE_OFFSET_MM, +// (uint16_t)Temp); +// VL53L1_WrWord(dev, MM_CONFIG__INNER_OFFSET_MM, 0x0); +// VL53L1_WrWord(dev, MM_CONFIG__OUTER_OFFSET_MM, 0x0); +// return status; +// } -VL53L1X_ERROR VL53L1X_GetOffset(busDevice_t * dev, int16_t *offset) -{ - VL53L1X_ERROR status = 0; - uint16_t Temp; +// VL53L1X_ERROR VL53L1X_GetOffset(busDevice_t * dev, int16_t *offset) +// { +// VL53L1X_ERROR status = 0; +// uint16_t Temp; - status = VL53L1_RdWord(dev,ALGO__PART_TO_PART_RANGE_OFFSET_MM, &Temp); - Temp = Temp<<3; - Temp = Temp>>5; - *offset = (int16_t)(Temp); - return status; -} +// status = VL53L1_RdWord(dev,ALGO__PART_TO_PART_RANGE_OFFSET_MM, &Temp); +// Temp = Temp<<3; +// Temp = Temp>>5; +// *offset = (int16_t)(Temp); +// return status; +// } -VL53L1X_ERROR VL53L1X_SetXtalk(busDevice_t * dev, uint16_t XtalkValue) -{ -/* XTalkValue in count per second to avoid float type */ - VL53L1X_ERROR status = 0; +// VL53L1X_ERROR VL53L1X_SetXtalk(busDevice_t * dev, uint16_t XtalkValue) +// { +// /* XTalkValue in count per second to avoid float type */ +// VL53L1X_ERROR status = 0; - status = VL53L1_WrWord(dev, - ALGO__CROSSTALK_COMPENSATION_X_PLANE_GRADIENT_KCPS, - 0x0000); - status = VL53L1_WrWord(dev, ALGO__CROSSTALK_COMPENSATION_Y_PLANE_GRADIENT_KCPS, - 0x0000); - status = VL53L1_WrWord(dev, ALGO__CROSSTALK_COMPENSATION_PLANE_OFFSET_KCPS, - (XtalkValue<<9)/1000); /* * << 9 (7.9 format) and /1000 to convert cps to kpcs */ - return status; -} +// status = VL53L1_WrWord(dev, +// ALGO__CROSSTALK_COMPENSATION_X_PLANE_GRADIENT_KCPS, +// 0x0000); +// status = VL53L1_WrWord(dev, ALGO__CROSSTALK_COMPENSATION_Y_PLANE_GRADIENT_KCPS, +// 0x0000); +// status = VL53L1_WrWord(dev, ALGO__CROSSTALK_COMPENSATION_PLANE_OFFSET_KCPS, +// (XtalkValue<<9)/1000); /* * << 9 (7.9 format) and /1000 to convert cps to kpcs */ +// return status; +// } -VL53L1X_ERROR VL53L1X_GetXtalk(busDevice_t * dev, uint16_t *xtalk ) -{ - VL53L1X_ERROR status = 0; +// VL53L1X_ERROR VL53L1X_GetXtalk(busDevice_t * dev, uint16_t *xtalk ) +// { +// VL53L1X_ERROR status = 0; - status = VL53L1_RdWord(dev,ALGO__CROSSTALK_COMPENSATION_PLANE_OFFSET_KCPS, xtalk); - *xtalk = (uint16_t)((*xtalk*1000)>>9); /* * 1000 to convert kcps to cps and >> 9 (7.9 format) */ - return status; -} +// status = VL53L1_RdWord(dev,ALGO__CROSSTALK_COMPENSATION_PLANE_OFFSET_KCPS, xtalk); +// *xtalk = (uint16_t)((*xtalk*1000)>>9); /* * 1000 to convert kcps to cps and >> 9 (7.9 format) */ +// return status; +// } -VL53L1X_ERROR VL53L1X_SetDistanceThreshold(busDevice_t * dev, uint16_t ThreshLow, - uint16_t ThreshHigh, uint8_t Window, - uint8_t IntOnNoTarget) -{ - VL53L1X_ERROR status = 0; - uint8_t Temp = 0; +// VL53L1X_ERROR VL53L1X_SetDistanceThreshold(busDevice_t * dev, uint16_t ThreshLow, +// uint16_t ThreshHigh, uint8_t Window, +// uint8_t IntOnNoTarget) +// { +// VL53L1X_ERROR status = 0; +// uint8_t Temp = 0; - status = VL53L1_RdByte(dev, SYSTEM__INTERRUPT_CONFIG_GPIO, &Temp); - Temp = Temp & 0x47; - if (IntOnNoTarget == 0) { - status = VL53L1_WrByte(dev, SYSTEM__INTERRUPT_CONFIG_GPIO, - (Temp | (Window & 0x07))); - } else { - status = VL53L1_WrByte(dev, SYSTEM__INTERRUPT_CONFIG_GPIO, - ((Temp | (Window & 0x07)) | 0x40)); - } - status = VL53L1_WrWord(dev, SYSTEM__THRESH_HIGH, ThreshHigh); - status = VL53L1_WrWord(dev, SYSTEM__THRESH_LOW, ThreshLow); - return status; -} +// status = VL53L1_RdByte(dev, SYSTEM__INTERRUPT_CONFIG_GPIO, &Temp); +// Temp = Temp & 0x47; +// if (IntOnNoTarget == 0) { +// status = VL53L1_WrByte(dev, SYSTEM__INTERRUPT_CONFIG_GPIO, +// (Temp | (Window & 0x07))); +// } else { +// status = VL53L1_WrByte(dev, SYSTEM__INTERRUPT_CONFIG_GPIO, +// ((Temp | (Window & 0x07)) | 0x40)); +// } +// status = VL53L1_WrWord(dev, SYSTEM__THRESH_HIGH, ThreshHigh); +// status = VL53L1_WrWord(dev, SYSTEM__THRESH_LOW, ThreshLow); +// return status; +// } -VL53L1X_ERROR VL53L1X_GetDistanceThresholdWindow(busDevice_t * dev, uint16_t *window) -{ - VL53L1X_ERROR status = 0; - uint8_t tmp; - status = VL53L1_RdByte(dev,SYSTEM__INTERRUPT_CONFIG_GPIO, &tmp); - *window = (uint16_t)(tmp & 0x7); - return status; -} +// VL53L1X_ERROR VL53L1X_GetDistanceThresholdWindow(busDevice_t * dev, uint16_t *window) +// { +// VL53L1X_ERROR status = 0; +// uint8_t tmp; +// status = VL53L1_RdByte(dev,SYSTEM__INTERRUPT_CONFIG_GPIO, &tmp); +// *window = (uint16_t)(tmp & 0x7); +// return status; +// } -VL53L1X_ERROR VL53L1X_GetDistanceThresholdLow(busDevice_t * dev, uint16_t *low) -{ - VL53L1X_ERROR status = 0; - uint16_t tmp; +// VL53L1X_ERROR VL53L1X_GetDistanceThresholdLow(busDevice_t * dev, uint16_t *low) +// { +// VL53L1X_ERROR status = 0; +// uint16_t tmp; - status = VL53L1_RdWord(dev,SYSTEM__THRESH_LOW, &tmp); - *low = tmp; - return status; -} +// status = VL53L1_RdWord(dev,SYSTEM__THRESH_LOW, &tmp); +// *low = tmp; +// return status; +// } -VL53L1X_ERROR VL53L1X_GetDistanceThresholdHigh(busDevice_t * dev, uint16_t *high) -{ - VL53L1X_ERROR status = 0; - uint16_t tmp; +// VL53L1X_ERROR VL53L1X_GetDistanceThresholdHigh(busDevice_t * dev, uint16_t *high) +// { +// VL53L1X_ERROR status = 0; +// uint16_t tmp; - status = VL53L1_RdWord(dev,SYSTEM__THRESH_HIGH, &tmp); - *high = tmp; - return status; -} +// status = VL53L1_RdWord(dev,SYSTEM__THRESH_HIGH, &tmp); +// *high = tmp; +// return status; +// } -VL53L1X_ERROR VL53L1X_SetROICenter(busDevice_t * dev, uint8_t ROICenter) -{ - VL53L1X_ERROR status = 0; - status = VL53L1_WrByte(dev, ROI_CONFIG__USER_ROI_CENTRE_SPAD, ROICenter); - return status; -} +// VL53L1X_ERROR VL53L1X_SetROICenter(busDevice_t * dev, uint8_t ROICenter) +// { +// VL53L1X_ERROR status = 0; +// status = VL53L1_WrByte(dev, ROI_CONFIG__USER_ROI_CENTRE_SPAD, ROICenter); +// return status; +// } -VL53L1X_ERROR VL53L1X_GetROICenter(busDevice_t * dev, uint8_t *ROICenter) -{ - VL53L1X_ERROR status = 0; - uint8_t tmp; - status = VL53L1_RdByte(dev, ROI_CONFIG__USER_ROI_CENTRE_SPAD, &tmp); - *ROICenter = tmp; - return status; -} +// VL53L1X_ERROR VL53L1X_GetROICenter(busDevice_t * dev, uint8_t *ROICenter) +// { +// VL53L1X_ERROR status = 0; +// uint8_t tmp; +// status = VL53L1_RdByte(dev, ROI_CONFIG__USER_ROI_CENTRE_SPAD, &tmp); +// *ROICenter = tmp; +// return status; +// } -VL53L1X_ERROR VL53L1X_SetROI(busDevice_t * dev, uint16_t X, uint16_t Y) -{ - uint8_t OpticalCenter; - VL53L1X_ERROR status = 0; +// VL53L1X_ERROR VL53L1X_SetROI(busDevice_t * dev, uint16_t X, uint16_t Y) +// { +// uint8_t OpticalCenter; +// VL53L1X_ERROR status = 0; - status =VL53L1_RdByte(dev, VL53L1_ROI_CONFIG__MODE_ROI_CENTRE_SPAD, &OpticalCenter); - if (X > 16) - X = 16; - if (Y > 16) - Y = 16; - if (X > 10 || Y > 10){ - OpticalCenter = 199; - } - status = VL53L1_WrByte(dev, ROI_CONFIG__USER_ROI_CENTRE_SPAD, OpticalCenter); - status = VL53L1_WrByte(dev, ROI_CONFIG__USER_ROI_REQUESTED_GLOBAL_XY_SIZE, - (Y - 1) << 4 | (X - 1)); - return status; -} +// status =VL53L1_RdByte(dev, VL53L1_ROI_CONFIG__MODE_ROI_CENTRE_SPAD, &OpticalCenter); +// if (X > 16) +// X = 16; +// if (Y > 16) +// Y = 16; +// if (X > 10 || Y > 10){ +// OpticalCenter = 199; +// } +// status = VL53L1_WrByte(dev, ROI_CONFIG__USER_ROI_CENTRE_SPAD, OpticalCenter); +// status = VL53L1_WrByte(dev, ROI_CONFIG__USER_ROI_REQUESTED_GLOBAL_XY_SIZE, +// (Y - 1) << 4 | (X - 1)); +// return status; +// } -VL53L1X_ERROR VL53L1X_GetROI_XY(busDevice_t * dev, uint16_t *ROI_X, uint16_t *ROI_Y) -{ - VL53L1X_ERROR status = 0; - uint8_t tmp; +// VL53L1X_ERROR VL53L1X_GetROI_XY(busDevice_t * dev, uint16_t *ROI_X, uint16_t *ROI_Y) +// { +// VL53L1X_ERROR status = 0; +// uint8_t tmp; - status = VL53L1_RdByte(dev,ROI_CONFIG__USER_ROI_REQUESTED_GLOBAL_XY_SIZE, &tmp); - *ROI_X = ((uint16_t)tmp & 0x0F) + 1; - *ROI_Y = (((uint16_t)tmp & 0xF0) >> 4) + 1; - return status; -} +// status = VL53L1_RdByte(dev,ROI_CONFIG__USER_ROI_REQUESTED_GLOBAL_XY_SIZE, &tmp); +// *ROI_X = ((uint16_t)tmp & 0x0F) + 1; +// *ROI_Y = (((uint16_t)tmp & 0xF0) >> 4) + 1; +// return status; +// } -VL53L1X_ERROR VL53L1X_SetSignalThreshold(busDevice_t * dev, uint16_t Signal) -{ - VL53L1X_ERROR status = 0; +// VL53L1X_ERROR VL53L1X_SetSignalThreshold(busDevice_t * dev, uint16_t Signal) +// { +// VL53L1X_ERROR status = 0; - VL53L1_WrWord(dev,RANGE_CONFIG__MIN_COUNT_RATE_RTN_LIMIT_MCPS,Signal>>3); - return status; -} +// VL53L1_WrWord(dev,RANGE_CONFIG__MIN_COUNT_RATE_RTN_LIMIT_MCPS,Signal>>3); +// return status; +// } -VL53L1X_ERROR VL53L1X_GetSignalThreshold(busDevice_t * dev, uint16_t *signal) -{ - VL53L1X_ERROR status = 0; - uint16_t tmp; +// VL53L1X_ERROR VL53L1X_GetSignalThreshold(busDevice_t * dev, uint16_t *signal) +// { +// VL53L1X_ERROR status = 0; +// uint16_t tmp; - status = VL53L1_RdWord(dev, - RANGE_CONFIG__MIN_COUNT_RATE_RTN_LIMIT_MCPS, &tmp); - *signal = tmp <<3; - return status; -} +// status = VL53L1_RdWord(dev, +// RANGE_CONFIG__MIN_COUNT_RATE_RTN_LIMIT_MCPS, &tmp); +// *signal = tmp <<3; +// return status; +// } -VL53L1X_ERROR VL53L1X_SetSigmaThreshold(busDevice_t * dev, uint16_t Sigma) -{ - VL53L1X_ERROR status = 0; +// VL53L1X_ERROR VL53L1X_SetSigmaThreshold(busDevice_t * dev, uint16_t Sigma) +// { +// VL53L1X_ERROR status = 0; - if(Sigma>(0xFFFF>>2)){ - return 1; - } - /* 16 bits register 14.2 format */ - status = VL53L1_WrWord(dev,RANGE_CONFIG__SIGMA_THRESH,Sigma<<2); - return status; -} +// if(Sigma>(0xFFFF>>2)){ +// return 1; +// } +// /* 16 bits register 14.2 format */ +// status = VL53L1_WrWord(dev,RANGE_CONFIG__SIGMA_THRESH,Sigma<<2); +// return status; +// } -VL53L1X_ERROR VL53L1X_GetSigmaThreshold(busDevice_t * dev, uint16_t *sigma) -{ - VL53L1X_ERROR status = 0; - uint16_t tmp; +// VL53L1X_ERROR VL53L1X_GetSigmaThreshold(busDevice_t * dev, uint16_t *sigma) +// { +// VL53L1X_ERROR status = 0; +// uint16_t tmp; - status = VL53L1_RdWord(dev,RANGE_CONFIG__SIGMA_THRESH, &tmp); - *sigma = tmp >> 2; - return status; +// status = VL53L1_RdWord(dev,RANGE_CONFIG__SIGMA_THRESH, &tmp); +// *sigma = tmp >> 2; +// return status; -} +// } -VL53L1X_ERROR VL53L1X_StartTemperatureUpdate(busDevice_t * dev) -{ - VL53L1X_ERROR status = 0; - uint8_t tmp=0; +// VL53L1X_ERROR VL53L1X_StartTemperatureUpdate(busDevice_t * dev) +// { +// VL53L1X_ERROR status = 0; +// uint8_t tmp=0; - status = VL53L1_WrByte(dev,VL53L1_VHV_CONFIG__TIMEOUT_MACROP_LOOP_BOUND,0x81); /* full VHV */ - status = VL53L1_WrByte(dev,0x0B,0x92); - status = VL53L1X_StartRanging(dev); - while(tmp==0){ - status = VL53L1X_CheckForDataReady(dev, &tmp); - } - tmp = 0; - status = VL53L1X_ClearInterrupt(dev); - status = VL53L1X_StopRanging(dev); - status = VL53L1_WrByte(dev, VL53L1_VHV_CONFIG__TIMEOUT_MACROP_LOOP_BOUND, 0x09); /* two bounds VHV */ - status = VL53L1_WrByte(dev, 0x0B, 0); /* start VHV from the previous temperature */ - return status; -} +// status = VL53L1_WrByte(dev,VL53L1_VHV_CONFIG__TIMEOUT_MACROP_LOOP_BOUND,0x81); /* full VHV */ +// status = VL53L1_WrByte(dev,0x0B,0x92); +// status = VL53L1X_StartRanging(dev); +// while(tmp==0){ +// status = VL53L1X_CheckForDataReady(dev, &tmp); +// } +// tmp = 0; +// status = VL53L1X_ClearInterrupt(dev); +// status = VL53L1X_StopRanging(dev); +// status = VL53L1_WrByte(dev, VL53L1_VHV_CONFIG__TIMEOUT_MACROP_LOOP_BOUND, 0x09); /* two bounds VHV */ +// status = VL53L1_WrByte(dev, 0x0B, 0); /* start VHV from the previous temperature */ +// return status; +// } -int8_t VL53L1X_CalibrateOffset(busDevice_t * dev, uint16_t TargetDistInMm, int16_t *offset) -{ - uint8_t i, tmp; - int16_t AverageDistance = 0; - uint16_t distance; - VL53L1X_ERROR status = 0; +// int8_t VL53L1X_CalibrateOffset(busDevice_t * dev, uint16_t TargetDistInMm, int16_t *offset) +// { +// uint8_t i, tmp; +// int16_t AverageDistance = 0; +// uint16_t distance; +// VL53L1X_ERROR status = 0; - status = VL53L1_WrWord(dev, ALGO__PART_TO_PART_RANGE_OFFSET_MM, 0x0); - status = VL53L1_WrWord(dev, MM_CONFIG__INNER_OFFSET_MM, 0x0); - status = VL53L1_WrWord(dev, MM_CONFIG__OUTER_OFFSET_MM, 0x0); - status = VL53L1X_StartRanging(dev); /* Enable VL53L1X sensor */ - for (i = 0; i < 50; i++) { - tmp = 0; - while (tmp == 0){ - status = VL53L1X_CheckForDataReady(dev, &tmp); - } - status = VL53L1X_GetDistance(dev, &distance); - status = VL53L1X_ClearInterrupt(dev); - AverageDistance = AverageDistance + distance; - } - status = VL53L1X_StopRanging(dev); - AverageDistance = AverageDistance / 50; - *offset = TargetDistInMm - AverageDistance; - status = VL53L1_WrWord(dev, ALGO__PART_TO_PART_RANGE_OFFSET_MM, *offset*4); - return status; -} +// status = VL53L1_WrWord(dev, ALGO__PART_TO_PART_RANGE_OFFSET_MM, 0x0); +// status = VL53L1_WrWord(dev, MM_CONFIG__INNER_OFFSET_MM, 0x0); +// status = VL53L1_WrWord(dev, MM_CONFIG__OUTER_OFFSET_MM, 0x0); +// status = VL53L1X_StartRanging(dev); /* Enable VL53L1X sensor */ +// for (i = 0; i < 50; i++) { +// tmp = 0; +// while (tmp == 0){ +// status = VL53L1X_CheckForDataReady(dev, &tmp); +// } +// status = VL53L1X_GetDistance(dev, &distance); +// status = VL53L1X_ClearInterrupt(dev); +// AverageDistance = AverageDistance + distance; +// } +// status = VL53L1X_StopRanging(dev); +// AverageDistance = AverageDistance / 50; +// *offset = TargetDistInMm - AverageDistance; +// status = VL53L1_WrWord(dev, ALGO__PART_TO_PART_RANGE_OFFSET_MM, *offset*4); +// return status; +// } -int8_t VL53L1X_CalibrateXtalk(busDevice_t * dev, uint16_t TargetDistInMm, uint16_t *xtalk) -{ - uint8_t i, tmp; - float AverageSignalRate = 0; - float AverageDistance = 0; - float AverageSpadNb = 0; - uint16_t distance = 0, spadNum; - uint16_t sr; - VL53L1X_ERROR status = 0; - uint32_t calXtalk; +// int8_t VL53L1X_CalibrateXtalk(busDevice_t * dev, uint16_t TargetDistInMm, uint16_t *xtalk) +// { +// uint8_t i, tmp; +// float AverageSignalRate = 0; +// float AverageDistance = 0; +// float AverageSpadNb = 0; +// uint16_t distance = 0, spadNum; +// uint16_t sr; +// VL53L1X_ERROR status = 0; +// uint32_t calXtalk; - status = VL53L1_WrWord(dev, 0x0016,0); - status = VL53L1X_StartRanging(dev); - for (i = 0; i < 50; i++) { - tmp = 0; - while (tmp == 0){ - status = VL53L1X_CheckForDataReady(dev, &tmp); - } - status= VL53L1X_GetSignalRate(dev, &sr); - status= VL53L1X_GetDistance(dev, &distance); - status = VL53L1X_ClearInterrupt(dev); - AverageDistance = AverageDistance + distance; - status = VL53L1X_GetSpadNb(dev, &spadNum); - AverageSpadNb = AverageSpadNb + spadNum; - AverageSignalRate = - AverageSignalRate + sr; - } - status = VL53L1X_StopRanging(dev); - AverageDistance = AverageDistance / 50; - AverageSpadNb = AverageSpadNb / 50; - AverageSignalRate = AverageSignalRate / 50; - /* Calculate Xtalk value */ - calXtalk = (uint16_t)(512*(AverageSignalRate*(1-(AverageDistance/TargetDistInMm)))/AverageSpadNb); - *xtalk = (uint16_t)((calXtalk*1000)>>9); - status = VL53L1_WrWord(dev, 0x0016, (uint16_t)calXtalk); - return status; -} +// status = VL53L1_WrWord(dev, 0x0016,0); +// status = VL53L1X_StartRanging(dev); +// for (i = 0; i < 50; i++) { +// tmp = 0; +// while (tmp == 0){ +// status = VL53L1X_CheckForDataReady(dev, &tmp); +// } +// status= VL53L1X_GetSignalRate(dev, &sr); +// status= VL53L1X_GetDistance(dev, &distance); +// status = VL53L1X_ClearInterrupt(dev); +// AverageDistance = AverageDistance + distance; +// status = VL53L1X_GetSpadNb(dev, &spadNum); +// AverageSpadNb = AverageSpadNb + spadNum; +// AverageSignalRate = +// AverageSignalRate + sr; +// } +// status = VL53L1X_StopRanging(dev); +// AverageDistance = AverageDistance / 50; +// AverageSpadNb = AverageSpadNb / 50; +// AverageSignalRate = AverageSignalRate / 50; +// /* Calculate Xtalk value */ +// calXtalk = (uint16_t)(512*(AverageSignalRate*(1-(AverageDistance/TargetDistInMm)))/AverageSpadNb); +// *xtalk = (uint16_t)((calXtalk*1000)>>9); +// status = VL53L1_WrWord(dev, 0x0016, (uint16_t)calXtalk); +// return status; +// } static void vl53l1x_Init(rangefinderDev_t * rangefinder) { + VL53L1X_ERROR status = VL53L1_ERROR_NONE; isInitialized = false; - VL53L1X_SensorInit(rangefinder->busDev); - VL53L1X_SetDistanceMode(rangefinder->busDev, 2); /* 1=short, 2=long */ - VL53L1X_SetTimingBudgetInMs(rangefinder->busDev, VL53L1X_TIMING_BUDGET); /* in ms possible values [20, 50, 100, 200, 500] */ - VL53L1X_SetInterMeasurementInMs(rangefinder->busDev, RANGEFINDER_VL53L1X_TASK_PERIOD_MS); /* in ms, IM must be > = TB */ - VL53L1X_StartRanging(rangefinder->busDev); - isInitialized = true; + status = VL53L1X_SensorInit(rangefinder->busDev); + if (status == VL53L1_ERROR_NONE) { + VL53L1X_SetDistanceMode(rangefinder->busDev, 2); /* 1=short, 2=long */ + VL53L1X_SetTimingBudgetInMs(rangefinder->busDev, 33); /* in ms possible values [20, 50, 100, 200, 500] */ + VL53L1X_SetInterMeasurementInMs(rangefinder->busDev, RANGEFINDER_VL53L1X_TASK_PERIOD_MS); /* in ms, IM must be > = TB */ + status = VL53L1X_StartRanging(rangefinder->busDev); + } + isInitialized = (status == VL53L1_ERROR_NONE); } void vl53l1x_Update(rangefinderDev_t * rangefinder) @@ -1639,7 +1642,7 @@ int32_t vl53l1x_GetDistance(rangefinderDev_t *dev) if (isResponding && isInitialized) { if (lastMeasurementIsNew) { lastMeasurementIsNew = false; - return lastMeasurementCm < VL53L1X_MAX_RANGE_CM ? lastMeasurementCm : RANGEFINDER_OUT_OF_RANGE; + return (lastMeasurementCm < VL53L1X_MAX_RANGE_CM) ? lastMeasurementCm : RANGEFINDER_OUT_OF_RANGE; } else { return RANGEFINDER_NO_NEW_DATA; From f6d252ad74415a61a3892acd593163d0e2f56070 Mon Sep 17 00:00:00 2001 From: Tony Yeung Date: Sat, 27 Mar 2021 00:35:53 +0000 Subject: [PATCH 103/286] change messages for loitering over safehome --- src/main/io/osd.c | 7 ++++++- src/main/io/osd.h | 1 + 2 files changed, 7 insertions(+), 1 deletion(-) diff --git a/src/main/io/osd.c b/src/main/io/osd.c index 89296fb79b..3255ae79e2 100755 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -780,7 +780,7 @@ static const char * osdFailsafeInfoMessage(void) #if defined(USE_SAFE_HOME) static const char * divertingToSafehomeMessage(void) { - if (safehome_applied) { + if (NAV_Status.state != MW_NAV_STATE_HOVER_ABOVE_HOME && safehome_applied) { return OSD_MESSAGE_STR(OSD_MSG_DIVERT_SAFEHOME); } return NULL; @@ -821,6 +821,11 @@ static const char * navigationStateMessage(void) return OSD_MESSAGE_STR(OSD_MSG_LANDING); case MW_NAV_STATE_HOVER_ABOVE_HOME: if (STATE(FIXED_WING_LEGACY)) { +#if defined(USE_SAFE_HOME) + if (safehome_applied) { + return OSD_MESSAGE_STR(OSD_MSG_LOITERING_SAFEHOME); + } +#endif return OSD_MESSAGE_STR(OSD_MSG_LOITERING_HOME); } return OSD_MESSAGE_STR(OSD_MSG_HOVERING); diff --git a/src/main/io/osd.h b/src/main/io/osd.h index 0433a926fa..aeca8340b5 100755 --- a/src/main/io/osd.h +++ b/src/main/io/osd.h @@ -100,6 +100,7 @@ #if defined(USE_SAFE_HOME) #define OSD_MSG_DIVERT_SAFEHOME "DIVERTING TO SAFEHOME" +#define OSD_MSG_LOITERING_SAFEHOME "LOITERING AROUND SAFEHOME" #endif typedef enum { From 2d07330d429a367672e0823be03003695e0c33ac Mon Sep 17 00:00:00 2001 From: Darren Lines Date: Sat, 27 Mar 2021 10:17:09 +0000 Subject: [PATCH 104/286] Update logic_condition.h Added GPS Valid Fix to flight parameters --- src/main/programming/logic_condition.h | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/main/programming/logic_condition.h b/src/main/programming/logic_condition.h index 28cbdbb4dc..005c7a85f4 100644 --- a/src/main/programming/logic_condition.h +++ b/src/main/programming/logic_condition.h @@ -119,6 +119,7 @@ typedef enum { LOGIC_CONDITION_OPERAND_FLIGHT_3D_HOME_DISTANCE, // 31 LOGIC_CONDITION_OPERAND_FLIGHT_CRSF_LQ, // 32 LOGIC_CONDITION_OPERAND_FLIGHT_CRSF_SNR, // 33 + LOGIC_CONDITION_OPERAND_FLIGHT_GPS_VALID, // 0/1 // 34 } logicFlightOperands_e; @@ -195,4 +196,4 @@ void logicConditionReset(void); float getThrottleScale(float globalThrottleScale); int16_t getRcCommandOverride(int16_t command[], uint8_t axis); -int16_t getRcChannelOverride(uint8_t channel, int16_t originalValue); \ No newline at end of file +int16_t getRcChannelOverride(uint8_t channel, int16_t originalValue); From 4306010588122afff21fa895d1c731972f27e0c7 Mon Sep 17 00:00:00 2001 From: Darren Lines Date: Sat, 27 Mar 2021 10:19:04 +0000 Subject: [PATCH 105/286] Added GPS Valid Fix to programming Added GPS Valid fix flight parameter. Also have set the GPS sats to 0 when a GPS sensor loss occurs. This is much more useful than a false reading. --- src/main/programming/logic_condition.c | 11 ++++++++++- 1 file changed, 10 insertions(+), 1 deletion(-) diff --git a/src/main/programming/logic_condition.c b/src/main/programming/logic_condition.c index b1b9cefaee..017d585b0a 100644 --- a/src/main/programming/logic_condition.c +++ b/src/main/programming/logic_condition.c @@ -45,6 +45,7 @@ #include "flight/pid.h" #include "drivers/io_port_expander.h" #include "io/osd_common.h" +#include "sensors/diagnostics.h" #include "navigation/navigation.h" #include "navigation/navigation_private.h" @@ -408,7 +409,15 @@ static int logicConditionGetFlightOperandValue(int operand) { break; case LOGIC_CONDITION_OPERAND_FLIGHT_GPS_SATS: - return gpsSol.numSat; + if (getHwGPSStatus() == HW_SENSOR_UNAVAILABLE || getHwGPSStatus() == HW_SENSOR_UNHEALTHY) { + return 0; + } else { + return gpsSol.numSat; + } + break; + + case LOGIC_CONDITION_OPERAND_FLIGHT_GPS_VALID: // 0/1 + return STATE(GPS_FIX) ? 1 : 0; break; case LOGIC_CONDITION_OPERAND_FLIGHT_GROUD_SPEED: // cm/s From 2a69ee3107577a320f592eebb411b1a7040d5694 Mon Sep 17 00:00:00 2001 From: Darren Lines Date: Sat, 27 Mar 2021 10:21:14 +0000 Subject: [PATCH 106/286] Added GPS Valid Fix --- docs/Programming Framework.md | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/docs/Programming Framework.md b/docs/Programming Framework.md index 6d258ba8c9..ca5cc65f7f 100644 --- a/docs/Programming Framework.md +++ b/docs/Programming Framework.md @@ -125,7 +125,8 @@ IPF can be edited using INAV Configurator user interface, of via CLI | 30 | ACTIVE_WAYPOINT_ACTION | See ACTIVE_WAYPOINT_ACTION paragraph | | 31 | 3D HOME_DISTANCE | in `meters`, calculated from HOME_DISTANCE and ALTITUDE using Pythagorean theorem | | 32 | CROSSFIRE LQ | Crossfire Link quality as returned by the CRSF protocol | -| 33 | CROSSFIRE SNR | Crossfire SNR as returned by the CRSF protocol | +| 33 | CROSSFIRE SNR | Crossfire SNR as returned by the CRSF protocol | +| 34 | GPS_VALID | boolean `0`/`1`. True when the GPS has a valid 3D Fix | #### ACTIVE_WAYPOINT_ACTION @@ -261,4 +262,4 @@ Steps: 1. Normalize range [1000:2000] to [0:1000] by substracting `1000` 2. Scale range [0:1000] to [0:3] 3. Increase range by `1` to have the range of [1:4] -4. Assign LC#2 to VTX power function \ No newline at end of file +4. Assign LC#2 to VTX power function From 91a5a51cc09836a4745397bad1a84f9eb8176b50 Mon Sep 17 00:00:00 2001 From: "Pawel Spychalski (DzikuVx)" Date: Mon, 29 Mar 2021 12:08:39 +0200 Subject: [PATCH 107/286] Version bump to 3.0 --- CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index dc9a5385d3..b73616735b 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -29,7 +29,7 @@ else() include("${CMAKE_CURRENT_SOURCE_DIR}/cmake/${TOOLCHAIN}-checks.cmake") endif() -project(INAV VERSION 2.7.0) +project(INAV VERSION 3.0.0) enable_language(ASM) From b1c0f473ed674e5315874453cd81843183f23841 Mon Sep 17 00:00:00 2001 From: Jonathan Hudson Date: Mon, 29 Mar 2021 15:29:45 +0100 Subject: [PATCH 108/286] add additional MC_MOTORS (#6758) --- src/main/target/HGLRCF722/target.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/target/HGLRCF722/target.c b/src/main/target/HGLRCF722/target.c index ebabbd28a1..6924d5ac93 100644 --- a/src/main/target/HGLRCF722/target.c +++ b/src/main/target/HGLRCF722/target.c @@ -42,8 +42,8 @@ const timerHardware_t timerHardware[] = { DEF_TIM(TIM3, CH4, PB1, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S4 UP1-2 D(1, 2, 5) DEF_TIM(TIM2, CH1, PA15, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S5 UP1-7 D(1, 5, 3) - clash with S2 DEF_TIM(TIM2, CH2, PB3, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0, 0), // S6 UP1-7 D(1, 6, 3) - DEF_TIM(TIM4, CH1, PB6, TIM_USE_MC_SERVO | TIM_USE_FW_MOTOR, 0, 0), // S7 UP1-6 D(1, 0, 2) - DEF_TIM(TIM4, CH2, PB7, TIM_USE_MC_SERVO | TIM_USE_FW_MOTOR, 0, 0), // S8 UP1-6 D(1, 3, 2) + DEF_TIM(TIM4, CH1, PB6, TIM_USE_MC_MOTOR | TIM_USE_MC_SERVO | TIM_USE_FW_MOTOR, 0, 0), // S7 UP1-6 D(1, 0, 2) + DEF_TIM(TIM4, CH2, PB7, TIM_USE_MC_MOTOR | TIM_USE_MC_SERVO | TIM_USE_FW_MOTOR, 0, 0), // S8 UP1-6 D(1, 3, 2) DEF_TIM(TIM1, CH1, PA8, TIM_USE_LED, 0, 2), // LED D(2, 6, 0) DEF_TIM(TIM9, CH2, PA3, TIM_USE_PPM, 0, 0), // PPM, RX2 From 4408ed7af77997e34fcde71a16b3f2e97579c357 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Alberto=20Garci=CC=81a=20Hierro?= Date: Sat, 3 Apr 2021 20:55:09 +0100 Subject: [PATCH 109/286] canvas: Honor ahi_max_pitch when drawing the horizon line Map our real pitch to a value that causes the specified limit in the setting to place the horizon bar at the top (or bottom) when ahi_max_pitch is reached. --- src/main/io/osd_canvas.c | 7 ++++++- 1 file changed, 6 insertions(+), 1 deletion(-) diff --git a/src/main/io/osd_canvas.c b/src/main/io/osd_canvas.c index a13e66bc47..541914ca81 100644 --- a/src/main/io/osd_canvas.c +++ b/src/main/io/osd_canvas.c @@ -377,8 +377,13 @@ static bool osdCanvasDrawArtificialHorizonWidget(displayPort_t *display, display } configured = true; } + // The widget displays 270degs before fixing the bar at the top/bottom + // so that's 135degs each direction. Map that to the configured limit. + const float halfRange = 135.0f; + const float limit = halfRange / 180.0f * M_PIf; + float multiplier = halfRange / osdConfig()->ahi_max_pitch; widgetAHIData_t data = { - .pitch = pitchAngle, + .pitch = constrainf(pitchAngle * multiplier, -limit, limit), .roll = rollAngle, }; if (displayWidgetsDrawAHI(&widgets, instance, &data)) { From 8eef9a5731cfc77fb5e78db59df3ada37b8266a6 Mon Sep 17 00:00:00 2001 From: "Pawel Spychalski (DzikuVx)" Date: Sat, 3 Apr 2021 22:28:39 +0200 Subject: [PATCH 110/286] Define BNO055 I2C on all supported targets --- src/main/target/AIKONF4/target.h | 1 + src/main/target/AIRBOTF4/target.h | 2 +- src/main/target/AIRBOTF7/target.h | 2 +- src/main/target/ALIENFLIGHTF4/target.h | 1 + src/main/target/ALIENFLIGHTNGF7/target.h | 1 + src/main/target/ANYFC/target.h | 1 + src/main/target/ANYFCF7/target.h | 1 + src/main/target/ANYFCM7/target.h | 1 + src/main/target/ASGARD32F4/target.h | 1 + src/main/target/ASGARD32F7/target.h | 1 + src/main/target/BEEROTORF4/target.h | 1 + src/main/target/BETAFLIGHTF4/target.h | 1 + src/main/target/BLUEJAYF4/target.h | 1 + src/main/target/CLRACINGF4AIR/target.h | 1 + src/main/target/COLIBRI/target.h | 1 + src/main/target/COLIBRI_RACE/target.h | 3 ++- src/main/target/DALRCF405/target.h | 1 + src/main/target/DALRCF722DUAL/target.h | 2 +- src/main/target/F4BY/target.h | 1 + src/main/target/FALCORE/target.h | 3 ++- src/main/target/FF_F35_LIGHTNING/target.h | 1 + src/main/target/FIREWORKSV2/target.h | 2 ++ src/main/target/FISHDRONEF4/target.h | 2 ++ src/main/target/FLYWOOF411/target.h | 2 ++ src/main/target/FLYWOOF7DUAL/target.h | 2 ++ src/main/target/FOXEERF405/target.h | 2 ++ src/main/target/FOXEERF722DUAL/target.h | 2 ++ src/main/target/FRSKYPILOT/target.h | 2 +- src/main/target/FRSKY_ROVERF7/target.h | 2 +- src/main/target/FURYF4OSD/target.h | 1 + src/main/target/HGLRCF722/target.h | 2 +- src/main/target/IFLIGHTF4_SUCCEXD/target.h | 1 + src/main/target/IFLIGHTF4_TWING/target.h | 1 + src/main/target/IFLIGHTF7_TWING/target.h | 1 + src/main/target/KAKUTEF4/target.h | 1 + src/main/target/KAKUTEF7/target.h | 2 -- src/main/target/KAKUTEF7MINIV3/target.h | 1 + src/main/target/MAMBAF405US/target.h | 2 ++ src/main/target/MAMBAF722/target.h | 2 ++ src/main/target/MATEKF405/target.h | 2 ++ src/main/target/MATEKF405CAN/target.h | 1 + src/main/target/MATEKF405SE/target.h | 3 +-- src/main/target/MATEKF411/target.h | 1 + src/main/target/MATEKF411SE/target.h | 1 + src/main/target/MATEKF722/target.h | 2 -- src/main/target/MATEKF722PX/target.h | 2 +- src/main/target/MATEKF722SE/target.h | 2 -- src/main/target/MATEKF765/target.h | 2 +- src/main/target/MATEKH743/target.h | 2 +- src/main/target/MOTOLAB/target.h | 3 ++- src/main/target/OMNIBUSF4/target.h | 1 + src/main/target/OMNIBUSF7/target.h | 1 + src/main/target/OMNIBUSF7NXT/target.h | 1 + src/main/target/REVO/target.h | 1 + src/main/target/RUSH_BLADE_F7/target.h | 2 +- src/main/target/SPARKY2/target.h | 1 + src/main/target/SPEEDYBEEF4/target.h | 2 ++ src/main/target/SPEEDYBEEF7/target.h | 2 +- src/main/target/SPRACINGF4EVO/target.h | 1 + src/main/target/SPRACINGF7DUAL/target.h | 2 ++ src/main/target/YUPIF4/target.h | 1 + src/main/target/YUPIF7/target.h | 1 + src/main/target/common.h | 3 +++ 63 files changed, 76 insertions(+), 22 deletions(-) diff --git a/src/main/target/AIKONF4/target.h b/src/main/target/AIKONF4/target.h index 6e77fde6ad..d0e13b8a12 100644 --- a/src/main/target/AIKONF4/target.h +++ b/src/main/target/AIKONF4/target.h @@ -103,6 +103,7 @@ #define TEMPERATURE_I2C_BUS DEFAULT_I2C_BUS #define PITOT_I2C_BUS DEFAULT_I2C_BUS #define PCA9685_I2C_BUS DEFAULT_I2C_BUS +#define BNO055_I2C_BUS DEFAULT_I2C_BUS #define DEFAULT_RX_TYPE RX_TYPE_SERIAL #define SERIALRX_PROVIDER SERIALRX_SBUS diff --git a/src/main/target/AIRBOTF4/target.h b/src/main/target/AIRBOTF4/target.h index edf6973b54..596a0b54fd 100644 --- a/src/main/target/AIRBOTF4/target.h +++ b/src/main/target/AIRBOTF4/target.h @@ -64,8 +64,8 @@ #define USE_PITOT_ADC #define PITOT_I2C_BUS BUS_I2C2 - #define TEMPERATURE_I2C_BUS BUS_I2C2 +#define BNO055_I2C_BUS BUS_I2C2 #define M25P16_CS_PIN PB3 #define M25P16_SPI_BUS BUS_SPI3 diff --git a/src/main/target/AIRBOTF7/target.h b/src/main/target/AIRBOTF7/target.h index 8b4a267710..4a554cdd56 100644 --- a/src/main/target/AIRBOTF7/target.h +++ b/src/main/target/AIRBOTF7/target.h @@ -79,7 +79,7 @@ #define USE_MAG_LIS3MDL #define TEMPERATURE_I2C_BUS BUS_I2C1 - +#define BNO055_I2C_BUS BUS_I2C1 #define PITOT_I2C_BUS BUS_I2C1 #define USE_RANGEFINDER diff --git a/src/main/target/ALIENFLIGHTF4/target.h b/src/main/target/ALIENFLIGHTF4/target.h index 0cae3b81cb..9d7718d640 100644 --- a/src/main/target/ALIENFLIGHTF4/target.h +++ b/src/main/target/ALIENFLIGHTF4/target.h @@ -64,6 +64,7 @@ #define MAG_MPU9250_ALIGN CW0_DEG #define TEMPERATURE_I2C_BUS BUS_I2C1 +#define BNO055_I2C_BUS BUS_I2C1 #define USE_BARO #define BARO_I2C_BUS BUS_I2C1 diff --git a/src/main/target/ALIENFLIGHTNGF7/target.h b/src/main/target/ALIENFLIGHTNGF7/target.h index 45e4fe08d9..0a4522d312 100644 --- a/src/main/target/ALIENFLIGHTNGF7/target.h +++ b/src/main/target/ALIENFLIGHTNGF7/target.h @@ -66,6 +66,7 @@ #define AK8963_SPI_BUS BUS_SPI3 #define TEMPERATURE_I2C_BUS BUS_I2C1 +#define BNO055_I2C_BUS BUS_I2C1 #define USE_BARO #define BARO_I2C_BUS BUS_I2C1 diff --git a/src/main/target/ANYFC/target.h b/src/main/target/ANYFC/target.h index cfd82fc9c7..ad5fdf17a7 100644 --- a/src/main/target/ANYFC/target.h +++ b/src/main/target/ANYFC/target.h @@ -53,6 +53,7 @@ #define USE_RANGEFINDER #define RANGEFINDER_I2C_BUS BUS_I2C2 +#define BNO055_I2C_BUS BUS_I2C2 #define USE_BARO #define BARO_I2C_BUS BUS_I2C1 diff --git a/src/main/target/ANYFCF7/target.h b/src/main/target/ANYFCF7/target.h index 16baaca9cc..1e6d6c9f42 100644 --- a/src/main/target/ANYFCF7/target.h +++ b/src/main/target/ANYFCF7/target.h @@ -49,6 +49,7 @@ #define USE_MAG_LIS3MDL #define TEMPERATURE_I2C_BUS BUS_I2C2 +#define BNO055_I2C_BUS BUS_I2C2 #define USE_BARO diff --git a/src/main/target/ANYFCM7/target.h b/src/main/target/ANYFCM7/target.h index ca42a0ad2e..07e09434a1 100644 --- a/src/main/target/ANYFCM7/target.h +++ b/src/main/target/ANYFCM7/target.h @@ -49,6 +49,7 @@ #define USE_MAG_LIS3MDL #define TEMPERATURE_I2C_BUS BUS_I2C2 +#define BNO055_I2C_BUS BUS_I2C2 #define USE_BARO #define BARO_I2C_BUS BUS_I2C2 diff --git a/src/main/target/ASGARD32F4/target.h b/src/main/target/ASGARD32F4/target.h index 49c8dd10c3..d9018a5cd9 100644 --- a/src/main/target/ASGARD32F4/target.h +++ b/src/main/target/ASGARD32F4/target.h @@ -168,3 +168,4 @@ #define PITOT_I2C_BUS BUS_I2C2 #define PCA9685_I2C_BUS BUS_I2C2 #define TEMPERATURE_I2C_BUS BUS_I2C2 +#define BNO055_I2C_BUS BUS_I2C2 diff --git a/src/main/target/ASGARD32F7/target.h b/src/main/target/ASGARD32F7/target.h index f26650eeeb..97e2ad210e 100644 --- a/src/main/target/ASGARD32F7/target.h +++ b/src/main/target/ASGARD32F7/target.h @@ -172,3 +172,4 @@ #define PCA9685_I2C_BUS BUS_I2C2 #define PITOT_I2C_BUS BUS_I2C2 +#define BNO055_I2C_BUS BUS_I2C2 diff --git a/src/main/target/BEEROTORF4/target.h b/src/main/target/BEEROTORF4/target.h index 7f7ed3dea6..7963cae436 100644 --- a/src/main/target/BEEROTORF4/target.h +++ b/src/main/target/BEEROTORF4/target.h @@ -56,6 +56,7 @@ #define USE_MAG_LIS3MDL #define TEMPERATURE_I2C_BUS BUS_I2C1 +#define BNO055_I2C_BUS BUS_I2C1 #define USE_OSD #define USE_MAX7456 diff --git a/src/main/target/BETAFLIGHTF4/target.h b/src/main/target/BETAFLIGHTF4/target.h index cf4727c7b3..0b33a58755 100755 --- a/src/main/target/BETAFLIGHTF4/target.h +++ b/src/main/target/BETAFLIGHTF4/target.h @@ -130,6 +130,7 @@ #define USE_MAG_LIS3MDL #define TEMPERATURE_I2C_BUS BUS_I2C2 +#define BNO055_I2C_BUS BUS_I2C2 #define USE_BARO #define BARO_I2C_BUS BUS_I2C2 diff --git a/src/main/target/BLUEJAYF4/target.h b/src/main/target/BLUEJAYF4/target.h index 06cca4cf11..321b5e280a 100644 --- a/src/main/target/BLUEJAYF4/target.h +++ b/src/main/target/BLUEJAYF4/target.h @@ -53,6 +53,7 @@ #define USE_MAG_LIS3MDL #define TEMPERATURE_I2C_BUS BUS_I2C1 +#define BNO055_I2C_BUS BUS_I2C1 #define USE_BARO #define BARO_I2C_BUS BUS_I2C1 diff --git a/src/main/target/CLRACINGF4AIR/target.h b/src/main/target/CLRACINGF4AIR/target.h index 2b8f3c0145..e5f5aa5a4f 100644 --- a/src/main/target/CLRACINGF4AIR/target.h +++ b/src/main/target/CLRACINGF4AIR/target.h @@ -116,6 +116,7 @@ #define I2C2_SDA PB11 #define TEMPERATURE_I2C_BUS BUS_I2C2 +#define BNO055_I2C_BUS BUS_I2C2 #endif #define USE_ADC diff --git a/src/main/target/COLIBRI/target.h b/src/main/target/COLIBRI/target.h index d852c19a06..eff71edd24 100755 --- a/src/main/target/COLIBRI/target.h +++ b/src/main/target/COLIBRI/target.h @@ -56,6 +56,7 @@ #define USE_MAG_LIS3MDL #define TEMPERATURE_I2C_BUS BUS_I2C3 +#define BNO055_I2C_BUS BUS_I2C3 #ifdef QUANTON #define IMU_MPU6000_ALIGN CW90_DEG diff --git a/src/main/target/COLIBRI_RACE/target.h b/src/main/target/COLIBRI_RACE/target.h index 57e6a98eee..8347b58cd3 100755 --- a/src/main/target/COLIBRI_RACE/target.h +++ b/src/main/target/COLIBRI_RACE/target.h @@ -124,4 +124,5 @@ #define TARGET_IO_PORTD (BIT(2)) #define TARGET_IO_PORTF (BIT(0)|BIT(1)|BIT(4)) -#define PCA9685_I2C_BUS BUS_I2C2 \ No newline at end of file +#define PCA9685_I2C_BUS BUS_I2C2 +#define BNO055_I2C_BUS BUS_I2C2 \ No newline at end of file diff --git a/src/main/target/DALRCF405/target.h b/src/main/target/DALRCF405/target.h index b478f8e238..0b37dc2c11 100644 --- a/src/main/target/DALRCF405/target.h +++ b/src/main/target/DALRCF405/target.h @@ -96,6 +96,7 @@ #define USE_MAG_LIS3MDL #define TEMPERATURE_I2C_BUS BUS_I2C1 +#define BNO055_I2C_BUS BUS_I2C1 #define USE_RANGEFINDER #define USE_RANGEFINDER_HCSR04_I2C diff --git a/src/main/target/DALRCF722DUAL/target.h b/src/main/target/DALRCF722DUAL/target.h index 534042b8da..ca29604c6c 100644 --- a/src/main/target/DALRCF722DUAL/target.h +++ b/src/main/target/DALRCF722DUAL/target.h @@ -82,7 +82,7 @@ #define USE_MAG_LIS3MDL #define TEMPERATURE_I2C_BUS BUS_I2C1 - +#define BNO055_I2C_BUS BUS_I2C1 #define PITOT_I2C_BUS BUS_I2C1 #define USE_SPI diff --git a/src/main/target/F4BY/target.h b/src/main/target/F4BY/target.h index 6c2ebc7925..6dbb58442f 100644 --- a/src/main/target/F4BY/target.h +++ b/src/main/target/F4BY/target.h @@ -56,6 +56,7 @@ #define USE_MAG_LIS3MDL #define TEMPERATURE_I2C_BUS BUS_I2C2 +#define BNO055_I2C_BUS BUS_I2C2 #define USE_BARO #define BARO_I2C_BUS BUS_I2C2 diff --git a/src/main/target/FALCORE/target.h b/src/main/target/FALCORE/target.h index d8480eda73..cdb8ba4636 100755 --- a/src/main/target/FALCORE/target.h +++ b/src/main/target/FALCORE/target.h @@ -135,4 +135,5 @@ #define TARGET_IO_PORTD 0xFFFF #define TARGET_IO_PORTF (BIT(0)|BIT(1)|BIT(4)) -#define PCA9685_I2C_BUS BUS_I2C2 \ No newline at end of file +#define PCA9685_I2C_BUS BUS_I2C2 +#define BNO055_I2C_BUS BUS_I2C2 \ No newline at end of file diff --git a/src/main/target/FF_F35_LIGHTNING/target.h b/src/main/target/FF_F35_LIGHTNING/target.h index d2285e8a38..73e3e0fee9 100644 --- a/src/main/target/FF_F35_LIGHTNING/target.h +++ b/src/main/target/FF_F35_LIGHTNING/target.h @@ -127,6 +127,7 @@ #define PITOT_I2C_BUS BUS_I2C1 #define TEMPERATURE_I2C_BUS BUS_I2C1 +#define BNO055_I2C_BUS BUS_I2C1 #define DEFAULT_FEATURES (FEATURE_TX_PROF_SEL | FEATURE_VBAT | FEATURE_CURRENT_METER | FEATURE_OSD | FEATURE_GPS | FEATURE_TELEMETRY) diff --git a/src/main/target/FIREWORKSV2/target.h b/src/main/target/FIREWORKSV2/target.h index c3818458fd..a0ea4428da 100644 --- a/src/main/target/FIREWORKSV2/target.h +++ b/src/main/target/FIREWORKSV2/target.h @@ -231,6 +231,8 @@ #if defined(OMNIBUSF4V6) #define PCA9685_I2C_BUS BUS_I2C1 +#define BNO055_I2C_BUS BUS_I2C1 #else #define PCA9685_I2C_BUS BUS_I2C2 +#define BNO055_I2C_BUS BUS_I2C2 #endif diff --git a/src/main/target/FISHDRONEF4/target.h b/src/main/target/FISHDRONEF4/target.h index 91fd29491b..40c0fd199e 100644 --- a/src/main/target/FISHDRONEF4/target.h +++ b/src/main/target/FISHDRONEF4/target.h @@ -65,6 +65,8 @@ // *************** Temperature sensor ***************** #define TEMPERATURE_I2C_BUS BUS_I2C1 +#define BNO055_I2C_BUS BUS_I2C1 + // *************** BARO ***************************** #define USE_BARO #define BARO_I2C_BUS BUS_I2C1 diff --git a/src/main/target/FLYWOOF411/target.h b/src/main/target/FLYWOOF411/target.h index 675513def2..34d110d7a5 100644 --- a/src/main/target/FLYWOOF411/target.h +++ b/src/main/target/FLYWOOF411/target.h @@ -76,6 +76,8 @@ #define USE_BARO_BMP280 #define USE_BARO_MS5611 +#define BNO055_I2C_BUS BUS_I2C1 + #define USE_MAG #define MAG_I2C_BUS BUS_I2C1 #define USE_MAG_HMC5883 diff --git a/src/main/target/FLYWOOF7DUAL/target.h b/src/main/target/FLYWOOF7DUAL/target.h index 419d5ba9e4..10b8d285bb 100644 --- a/src/main/target/FLYWOOF7DUAL/target.h +++ b/src/main/target/FLYWOOF7DUAL/target.h @@ -113,6 +113,8 @@ #define USE_BARO_BMP280 #define USE_BARO_MS5611 +#define BNO055_I2C_BUS BUS_I2C1 + #define USE_MAG #define MAG_I2C_BUS BUS_I2C1 #define USE_MAG_HMC5883 diff --git a/src/main/target/FOXEERF405/target.h b/src/main/target/FOXEERF405/target.h index c4c38aa3c7..2b8d4e2ac0 100644 --- a/src/main/target/FOXEERF405/target.h +++ b/src/main/target/FOXEERF405/target.h @@ -117,6 +117,8 @@ #define USE_MAG_MAG3110 #define USE_MAG_LIS3MDL +#define BNO055_I2C_BUS BUS_I2C1 + /*** ADC ***/ #define USE_ADC #define ADC_CHANNEL_1_PIN PC0 diff --git a/src/main/target/FOXEERF722DUAL/target.h b/src/main/target/FOXEERF722DUAL/target.h index e1ac91c271..b339cef7a9 100644 --- a/src/main/target/FOXEERF722DUAL/target.h +++ b/src/main/target/FOXEERF722DUAL/target.h @@ -125,6 +125,8 @@ #define USE_MAG_MAG3110 #define USE_MAG_LIS3MDL +#define BNO055_I2C_BUS BUS_I2C1 + /*** ADC ***/ #define USE_ADC #define ADC_CHANNEL_1_PIN PC0 diff --git a/src/main/target/FRSKYPILOT/target.h b/src/main/target/FRSKYPILOT/target.h index a92057f4ee..87bd0b072b 100644 --- a/src/main/target/FRSKYPILOT/target.h +++ b/src/main/target/FRSKYPILOT/target.h @@ -123,7 +123,7 @@ #define USE_MAG_LIS3MDL #define TEMPERATURE_I2C_BUS BUS_I2C3 - +#define BNO055_I2C_BUS BUS_I2C3 #define PITOT_I2C_BUS BUS_I2C3 #define USE_RANGEFINDER diff --git a/src/main/target/FRSKY_ROVERF7/target.h b/src/main/target/FRSKY_ROVERF7/target.h index 30955d0bfd..6347cd9bd9 100755 --- a/src/main/target/FRSKY_ROVERF7/target.h +++ b/src/main/target/FRSKY_ROVERF7/target.h @@ -66,7 +66,7 @@ #define USE_MAG_LIS3MDL #define TEMPERATURE_I2C_BUS BUS_I2C1 - +#define BNO055_I2C_BUS BUS_I2C1 #define PITOT_I2C_BUS BUS_I2C1 // *************** SPI2 Flash *********************** diff --git a/src/main/target/FURYF4OSD/target.h b/src/main/target/FURYF4OSD/target.h index c445959610..d5ccf15a95 100644 --- a/src/main/target/FURYF4OSD/target.h +++ b/src/main/target/FURYF4OSD/target.h @@ -134,6 +134,7 @@ #define USE_MAG_LIS3MDL #define TEMPERATURE_I2C_BUS DEFAULT_I2C_BUS +#define BNO055_I2C_BUS DEFAULT_I2C_BUS #define USE_RANGEFINDER #define USE_RANGEFINDER_MSP diff --git a/src/main/target/HGLRCF722/target.h b/src/main/target/HGLRCF722/target.h index c745a853be..440a657c1b 100644 --- a/src/main/target/HGLRCF722/target.h +++ b/src/main/target/HGLRCF722/target.h @@ -80,7 +80,7 @@ #define USE_MAG_LIS3MDL #define TEMPERATURE_I2C_BUS BUS_I2C1 - +#define BNO055_I2C_BUS BUS_I2C1 #define PITOT_I2C_BUS BUS_I2C1 #define USE_RANGEFINDER diff --git a/src/main/target/IFLIGHTF4_SUCCEXD/target.h b/src/main/target/IFLIGHTF4_SUCCEXD/target.h index e47b9c7638..aee436548c 100644 --- a/src/main/target/IFLIGHTF4_SUCCEXD/target.h +++ b/src/main/target/IFLIGHTF4_SUCCEXD/target.h @@ -112,6 +112,7 @@ #define PITOT_I2C_BUS BUS_I2C1 #define TEMPERATURE_I2C_BUS BUS_I2C1 +#define BNO055_I2C_BUS BUS_I2C1 #define USE_RANGEFINDER #define USE_RANGEFINDER_MSP diff --git a/src/main/target/IFLIGHTF4_TWING/target.h b/src/main/target/IFLIGHTF4_TWING/target.h index cbb9d29428..ab5d0bf5f3 100644 --- a/src/main/target/IFLIGHTF4_TWING/target.h +++ b/src/main/target/IFLIGHTF4_TWING/target.h @@ -79,6 +79,7 @@ #define RANGEFINDER_I2C_BUS BUS_I2C1 #define PCA9685_I2C_BUS BUS_I2C1 +#define BNO055_I2C_BUS BUS_I2C1 // *************** OSD ***************************** #define USE_SPI_DEVICE_2 diff --git a/src/main/target/IFLIGHTF7_TWING/target.h b/src/main/target/IFLIGHTF7_TWING/target.h index bac28ceea0..2fb765bce7 100644 --- a/src/main/target/IFLIGHTF7_TWING/target.h +++ b/src/main/target/IFLIGHTF7_TWING/target.h @@ -80,6 +80,7 @@ #define USE_RANGEFINDER #define RANGEFINDER_I2C_BUS BUS_I2C2 +#define BNO055_I2C_BUS BUS_I2C2 // *************** FLASH ************************** #define M25P16_CS_PIN PB9 diff --git a/src/main/target/KAKUTEF4/target.h b/src/main/target/KAKUTEF4/target.h index 80013075a3..134889a0f3 100755 --- a/src/main/target/KAKUTEF4/target.h +++ b/src/main/target/KAKUTEF4/target.h @@ -65,6 +65,7 @@ # define USE_MAG_LIS3MDL # define TEMPERATURE_I2C_BUS BUS_I2C1 +#define BNO055_I2C_BUS BUS_I2C1 # define USE_BARO # define BARO_I2C_BUS BUS_I2C1 diff --git a/src/main/target/KAKUTEF7/target.h b/src/main/target/KAKUTEF7/target.h index 0889e6d94f..da8f8c9d1a 100644 --- a/src/main/target/KAKUTEF7/target.h +++ b/src/main/target/KAKUTEF7/target.h @@ -180,6 +180,4 @@ #define MAX_PWM_OUTPUT_PORTS 6 -#define USE_SECONDARY_IMU -#define USE_IMU_BNO055 #define BNO055_I2C_BUS BUS_I2C1 \ No newline at end of file diff --git a/src/main/target/KAKUTEF7MINIV3/target.h b/src/main/target/KAKUTEF7MINIV3/target.h index a32f9655c9..7ecc7a3d1e 100644 --- a/src/main/target/KAKUTEF7MINIV3/target.h +++ b/src/main/target/KAKUTEF7MINIV3/target.h @@ -167,6 +167,7 @@ #define RANGEFINDER_I2C_BUS BUS_I2C1 #define TEMPERATURE_I2C_BUS BUS_I2C1 #define PITOT_I2C_BUS BUS_I2C1 +#define BNO055_I2C_BUS BUS_I2C1 /*** Used pins ***/ #define TARGET_IO_PORTA 0xffff diff --git a/src/main/target/MAMBAF405US/target.h b/src/main/target/MAMBAF405US/target.h index f4312c06fc..f1ca6b7b00 100644 --- a/src/main/target/MAMBAF405US/target.h +++ b/src/main/target/MAMBAF405US/target.h @@ -56,6 +56,8 @@ #define USE_BARO_BMP280 #define USE_BARO_MS5611 +#define BNO055_I2C_BUS BUS_I2C2 + //*********** Magnetometer / Compass ************* #define USE_MAG #define MAG_I2C_BUS DEFAULT_I2C_BUS diff --git a/src/main/target/MAMBAF722/target.h b/src/main/target/MAMBAF722/target.h index aa324c685c..20bb2e4873 100644 --- a/src/main/target/MAMBAF722/target.h +++ b/src/main/target/MAMBAF722/target.h @@ -56,6 +56,8 @@ #define USE_BARO_BMP280 #define USE_BARO_MS5611 +#define BNO055_I2C_BUS BUS_I2C2 + //*********** Magnetometer / Compass ************* #define USE_MAG #define MAG_I2C_BUS DEFAULT_I2C_BUS diff --git a/src/main/target/MATEKF405/target.h b/src/main/target/MATEKF405/target.h index 37e7f78096..a70cc55583 100644 --- a/src/main/target/MATEKF405/target.h +++ b/src/main/target/MATEKF405/target.h @@ -162,6 +162,8 @@ #define TEMPERATURE_I2C_BUS DEFAULT_I2C_BUS +#define BNO055_I2C_BUS DEFAULT_I2C_BUS + #define USE_RANGEFINDER #define USE_RANGEFINDER_MSP #define USE_RANGEFINDER_HCSR04_I2C diff --git a/src/main/target/MATEKF405CAN/target.h b/src/main/target/MATEKF405CAN/target.h index 057bf922e4..f2c0999ee8 100644 --- a/src/main/target/MATEKF405CAN/target.h +++ b/src/main/target/MATEKF405CAN/target.h @@ -75,6 +75,7 @@ #define PITOT_I2C_BUS BUS_I2C2 #define TEMPERATURE_I2C_BUS BUS_I2C2 #define PCA9685_I2C_BUS BUS_I2C2 +#define BNO055_I2C_BUS BUS_I2C2 // *************** SPI2 RM3100 ************************** diff --git a/src/main/target/MATEKF405SE/target.h b/src/main/target/MATEKF405SE/target.h index 81681b94f5..79fc334751 100644 --- a/src/main/target/MATEKF405SE/target.h +++ b/src/main/target/MATEKF405SE/target.h @@ -73,10 +73,9 @@ #define USE_RANGEFINDER #define USE_RANGEFINDER_HCSR04_I2C #define RANGEFINDER_I2C_BUS BUS_I2C2 - #define PITOT_I2C_BUS BUS_I2C2 - #define TEMPERATURE_I2C_BUS BUS_I2C2 +#define BNO055_I2C_BUS BUS_I2C2 // *************** SPI2 OSD *************************** diff --git a/src/main/target/MATEKF411/target.h b/src/main/target/MATEKF411/target.h index b12f1e596a..ac6962a350 100755 --- a/src/main/target/MATEKF411/target.h +++ b/src/main/target/MATEKF411/target.h @@ -121,6 +121,7 @@ #define USE_BARO_DPS310 #define TEMPERATURE_I2C_BUS BUS_I2C1 +#define BNO055_I2C_BUS BUS_I2C1 #define USE_MAG #define MAG_I2C_BUS BUS_I2C1 diff --git a/src/main/target/MATEKF411SE/target.h b/src/main/target/MATEKF411SE/target.h index de42b9c1eb..59d59e42de 100755 --- a/src/main/target/MATEKF411SE/target.h +++ b/src/main/target/MATEKF411SE/target.h @@ -113,6 +113,7 @@ #define PITOT_I2C_BUS BUS_I2C1 #define TEMPERATURE_I2C_BUS BUS_I2C1 +#define BNO055_I2C_BUS BUS_I2C1 #define USE_RANGEFINDER #define USE_RANGEFINDER_MSP diff --git a/src/main/target/MATEKF722/target.h b/src/main/target/MATEKF722/target.h index 317074b45b..331ad65821 100755 --- a/src/main/target/MATEKF722/target.h +++ b/src/main/target/MATEKF722/target.h @@ -160,6 +160,4 @@ #define USE_ESC_SENSOR #define USE_SERIALSHOT -#define USE_SECONDARY_IMU -#define USE_IMU_BNO055 #define BNO055_I2C_BUS BUS_I2C1 diff --git a/src/main/target/MATEKF722PX/target.h b/src/main/target/MATEKF722PX/target.h index 24c471860e..ca9dd843a6 100755 --- a/src/main/target/MATEKF722PX/target.h +++ b/src/main/target/MATEKF722PX/target.h @@ -66,7 +66,7 @@ #define USE_MAG_LIS3MDL #define TEMPERATURE_I2C_BUS BUS_I2C1 - +#define BNO055_I2C_BUS BUS_I2C1 #define PITOT_I2C_BUS BUS_I2C1 #define USE_RANGEFINDER diff --git a/src/main/target/MATEKF722SE/target.h b/src/main/target/MATEKF722SE/target.h index fc4a9b10e4..ecff68fa66 100644 --- a/src/main/target/MATEKF722SE/target.h +++ b/src/main/target/MATEKF722SE/target.h @@ -195,6 +195,4 @@ #define USE_SERIALSHOT #define USE_ESC_SENSOR -#define USE_SECONDARY_IMU -#define USE_IMU_BNO055 #define BNO055_I2C_BUS BUS_I2C1 \ No newline at end of file diff --git a/src/main/target/MATEKF765/target.h b/src/main/target/MATEKF765/target.h index 48dde29e1e..8f5b256026 100644 --- a/src/main/target/MATEKF765/target.h +++ b/src/main/target/MATEKF765/target.h @@ -86,7 +86,7 @@ #define USE_MAG_LIS3MDL #define TEMPERATURE_I2C_BUS BUS_I2C2 - +#define BNO055_I2C_BUS BUS_I2C2 #define PITOT_I2C_BUS BUS_I2C2 #define USE_RANGEFINDER diff --git a/src/main/target/MATEKH743/target.h b/src/main/target/MATEKH743/target.h index b8ac4303eb..cc3409c8f5 100644 --- a/src/main/target/MATEKH743/target.h +++ b/src/main/target/MATEKH743/target.h @@ -157,7 +157,7 @@ #define USE_MAG_LIS3MDL #define TEMPERATURE_I2C_BUS BUS_I2C2 - +#define BNO055_I2C_BUS BUS_I2C2 #define PITOT_I2C_BUS BUS_I2C2 #define USE_RANGEFINDER diff --git a/src/main/target/MOTOLAB/target.h b/src/main/target/MOTOLAB/target.h index 386eadf120..81d44eb6a2 100644 --- a/src/main/target/MOTOLAB/target.h +++ b/src/main/target/MOTOLAB/target.h @@ -120,4 +120,5 @@ // !!TODO - check the following line is correct #define TARGET_IO_PORTF (BIT(0)|BIT(1)|BIT(3)|BIT(4)) -#define PCA9685_I2C_BUS BUS_I2C2 \ No newline at end of file +#define PCA9685_I2C_BUS BUS_I2C2 +#define BNO055_I2C_BUS BUS_I2C2 \ No newline at end of file diff --git a/src/main/target/OMNIBUSF4/target.h b/src/main/target/OMNIBUSF4/target.h index 0c3666a9d3..2a4383ab4d 100644 --- a/src/main/target/OMNIBUSF4/target.h +++ b/src/main/target/OMNIBUSF4/target.h @@ -100,6 +100,7 @@ #define USE_MAG_AK8975 #define TEMPERATURE_I2C_BUS I2C_EXT_BUS +#define BNO055_I2C_BUS I2C_EXT_BUS #define USE_BARO diff --git a/src/main/target/OMNIBUSF7/target.h b/src/main/target/OMNIBUSF7/target.h index d013a36a6c..d82ba047b1 100644 --- a/src/main/target/OMNIBUSF7/target.h +++ b/src/main/target/OMNIBUSF7/target.h @@ -158,6 +158,7 @@ #define USE_MAG_LIS3MDL #define TEMPERATURE_I2C_BUS BUS_I2C2 +#define BNO055_I2C_BUS BUS_I2C2 #define USE_RANGEFINDER #define USE_RANGEFINDER_HCSR04_I2C diff --git a/src/main/target/OMNIBUSF7NXT/target.h b/src/main/target/OMNIBUSF7NXT/target.h index b14e0c219e..241322b5d6 100644 --- a/src/main/target/OMNIBUSF7NXT/target.h +++ b/src/main/target/OMNIBUSF7NXT/target.h @@ -59,6 +59,7 @@ #define USE_MAG_LIS3MDL #define TEMPERATURE_I2C_BUS BUS_I2C1 +#define BNO055_I2C_BUS BUS_I2C1 #define USE_BARO #define USE_BARO_LPS25H diff --git a/src/main/target/REVO/target.h b/src/main/target/REVO/target.h index be2c511c19..360d097c69 100644 --- a/src/main/target/REVO/target.h +++ b/src/main/target/REVO/target.h @@ -56,6 +56,7 @@ #define USE_MAG_LIS3MDL #define TEMPERATURE_I2C_BUS BUS_I2C2 +#define BNO055_I2C_BUS BUS_I2C2 #define USE_BARO #define BARO_I2C_BUS BUS_I2C1 diff --git a/src/main/target/RUSH_BLADE_F7/target.h b/src/main/target/RUSH_BLADE_F7/target.h index ca0175f10d..cdae1860b3 100644 --- a/src/main/target/RUSH_BLADE_F7/target.h +++ b/src/main/target/RUSH_BLADE_F7/target.h @@ -66,7 +66,7 @@ #define USE_MAG_LIS3MDL #define TEMPERATURE_I2C_BUS BUS_I2C1 - +#define BNO055_I2C_BUS BUS_I2C1 #define PITOT_I2C_BUS BUS_I2C1 #define USE_RANGEFINDER diff --git a/src/main/target/SPARKY2/target.h b/src/main/target/SPARKY2/target.h index f716369b74..c1d18603d5 100644 --- a/src/main/target/SPARKY2/target.h +++ b/src/main/target/SPARKY2/target.h @@ -53,6 +53,7 @@ #define USE_MAG_LIS3MDL #define TEMPERATURE_I2C_BUS BUS_I2C1 +#define BNO055_I2C_BUS BUS_I2C1 #define USE_BARO #define BARO_I2C_BUS BUS_I2C1 diff --git a/src/main/target/SPEEDYBEEF4/target.h b/src/main/target/SPEEDYBEEF4/target.h index 1365b7aec5..c51b558832 100644 --- a/src/main/target/SPEEDYBEEF4/target.h +++ b/src/main/target/SPEEDYBEEF4/target.h @@ -124,6 +124,8 @@ #define SERIAL_PORT_COUNT 6 #endif +#define BNO055_I2C_BUS BUS_I2C1 + /*** BARO & MAG ***/ #define USE_BARO #define BARO_I2C_BUS BUS_I2C1 diff --git a/src/main/target/SPEEDYBEEF7/target.h b/src/main/target/SPEEDYBEEF7/target.h index a5c8b9150e..71e855af0a 100644 --- a/src/main/target/SPEEDYBEEF7/target.h +++ b/src/main/target/SPEEDYBEEF7/target.h @@ -120,7 +120,7 @@ #define USE_MAG_LIS3MDL #define TEMPERATURE_I2C_BUS BUS_I2C2 - +#define BNO055_I2C_BUS BUS_I2C2 #define PITOT_I2C_BUS BUS_I2C2 #define USE_RANGEFINDER diff --git a/src/main/target/SPRACINGF4EVO/target.h b/src/main/target/SPRACINGF4EVO/target.h index c1da04f60b..efe89add1e 100755 --- a/src/main/target/SPRACINGF4EVO/target.h +++ b/src/main/target/SPRACINGF4EVO/target.h @@ -59,6 +59,7 @@ #define USE_MAG_LIS3MDL #define TEMPERATURE_I2C_BUS BUS_I2C1 +#define BNO055_I2C_BUS BUS_I2C1 #define USE_VCP diff --git a/src/main/target/SPRACINGF7DUAL/target.h b/src/main/target/SPRACINGF7DUAL/target.h index aead3cbe59..9bbc2a7455 100644 --- a/src/main/target/SPRACINGF7DUAL/target.h +++ b/src/main/target/SPRACINGF7DUAL/target.h @@ -62,6 +62,8 @@ #define USE_BARO_BMP280 #define USE_BARO_MS5611 +#define BNO055_I2C_BUS BUS_I2C1 + #define MAG_I2C_BUS BUS_I2C1 #define USE_MAG #define USE_MAG_HMC5883 diff --git a/src/main/target/YUPIF4/target.h b/src/main/target/YUPIF4/target.h index dc423616f2..7f05ef7ef4 100644 --- a/src/main/target/YUPIF4/target.h +++ b/src/main/target/YUPIF4/target.h @@ -68,6 +68,7 @@ #define USE_MAG_QMC5883 #define TEMPERATURE_I2C_BUS BUS_I2C2 +#define BNO055_I2C_BUS BUS_I2C2 #define USE_BARO #define BARO_I2C_BUS BUS_I2C2 diff --git a/src/main/target/YUPIF7/target.h b/src/main/target/YUPIF7/target.h index b7799bfb81..87f600f32d 100644 --- a/src/main/target/YUPIF7/target.h +++ b/src/main/target/YUPIF7/target.h @@ -50,6 +50,7 @@ #define USE_MAG_QMC5883 #define TEMPERATURE_I2C_BUS BUS_I2C1 +#define BNO055_I2C_BUS BUS_I2C1 #define USE_BARO #define BARO_I2C_BUS BUS_I2C1 diff --git a/src/main/target/common.h b/src/main/target/common.h index 9d0ddc3346..18780d4f07 100755 --- a/src/main/target/common.h +++ b/src/main/target/common.h @@ -148,6 +148,9 @@ #define USE_SERIALRX_GHST #define USE_TELEMETRY_GHST +#define USE_SECONDARY_IMU +#define USE_IMU_BNO055 + #else // MCU_FLASH_SIZE < 256 #define LOG_LEVEL_MAXIMUM LOG_LEVEL_ERROR #endif From b61c1590f795e930366805d36b541a07900a4edc Mon Sep 17 00:00:00 2001 From: Michel Pastor Date: Sat, 3 Apr 2021 22:38:21 +0200 Subject: [PATCH 111/286] Homogenize OSD AHI settings prefix --- docs/Settings.md | 4 ++-- src/main/fc/settings.yaml | 4 ++-- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/docs/Settings.md b/docs/Settings.md index 8e5d3a9caf..016b319ee2 100644 --- a/docs/Settings.md +++ b/docs/Settings.md @@ -355,12 +355,12 @@ | opflow_scale | | | | osd_ahi_bordered | OFF | Shows a border/corners around the AHI region (pixel OSD only) | | osd_ahi_height | 162 | AHI height in pixels (pixel OSD only) | +| osd_ahi_max_pitch | 20 | Max pitch, in degrees, for OSD artificial horizon | +| osd_ahi_reverse_roll | | | | osd_ahi_style | DEFAULT | Sets OSD Artificial Horizon style "DEFAULT" or "LINE" for the FrSky Graphical OSD. | | osd_ahi_vertical_offset | 0 | AHI vertical offset from center (pixel OSD only) | | osd_ahi_width | 132 | AHI width in pixels (pixel OSD only) | | osd_alt_alarm | 100 | Value above which to make the OSD relative altitude indicator blink (meters) | -| osd_artificial_horizon_max_pitch | 20 | Max pitch, in degrees, for OSD artificial horizon | -| osd_artificial_horizon_reverse_roll | | | | osd_baro_temp_alarm_max | 600 | Temperature above which the baro temperature OSD element will start blinking (decidegrees centigrade) | | osd_baro_temp_alarm_min | -200 | Temperature under which the baro temperature OSD element will start blinking (decidegrees centigrade) | | osd_camera_fov_h | 135 | Horizontal field of view for the camera in degres | diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index b8992bcd22..c49fc2183c 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -2859,10 +2859,10 @@ groups: table: osd_alignment type: uint8_t - - name: osd_artificial_horizon_reverse_roll + - name: osd_ahi_reverse_roll field: ahi_reverse_roll type: bool - - name: osd_artificial_horizon_max_pitch + - name: osd_ahi_max_pitch description: "Max pitch, in degrees, for OSD artificial horizon" default_value: "20" field: ahi_max_pitch From 94316ceb42731c1cbd3403819cc3e1cb223afbb1 Mon Sep 17 00:00:00 2001 From: Michel Pastor Date: Sat, 3 Apr 2021 22:49:34 +0200 Subject: [PATCH 112/286] canvas: fix multiplier for default OSD AHI style --- src/main/io/osd_canvas.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/io/osd_canvas.c b/src/main/io/osd_canvas.c index 541914ca81..cab358f09c 100644 --- a/src/main/io/osd_canvas.c +++ b/src/main/io/osd_canvas.c @@ -381,7 +381,7 @@ static bool osdCanvasDrawArtificialHorizonWidget(displayPort_t *display, display // so that's 135degs each direction. Map that to the configured limit. const float halfRange = 135.0f; const float limit = halfRange / 180.0f * M_PIf; - float multiplier = halfRange / osdConfig()->ahi_max_pitch; + float multiplier = osdConfig()->osd_ahi_style == OSD_AHI_STYLE_DEFAULT ? 1.0f : halfRange / osdConfig()->ahi_max_pitch; widgetAHIData_t data = { .pitch = constrainf(pitchAngle * multiplier, -limit, limit), .roll = rollAngle, From 44d2c08a819fed21381d7482a3664530ad81e667 Mon Sep 17 00:00:00 2001 From: "Pawel Spychalski (DzikuVx)" Date: Sun, 4 Apr 2021 15:24:35 +0200 Subject: [PATCH 113/286] make BNO055 one of avalable secondary imus --- docs/Settings.md | 30 ++++++++++++------------- src/main/fc/fc_tasks.c | 2 +- src/main/fc/settings.yaml | 39 ++++++++++++++++++++++++++++++--- src/main/flight/pid.c | 2 +- src/main/flight/secondary_imu.c | 12 ++++++++-- src/main/flight/secondary_imu.h | 7 +++++- src/main/io/osd.c | 6 ++--- 7 files changed, 72 insertions(+), 26 deletions(-) diff --git a/docs/Settings.md b/docs/Settings.md index 2f7054dc2c..b775bb70ce 100644 --- a/docs/Settings.md +++ b/docs/Settings.md @@ -165,21 +165,21 @@ | i2c_speed | 400KHZ | This setting controls the clock speed of I2C bus. 400KHZ is the default that most setups are able to use. Some noise-free setups may be overclocked to 800KHZ. Some sensor chips or setups with long wires may work unreliably at 400KHZ - user can try lowering the clock speed to 200KHZ or even 100KHZ. User need to bear in mind that lower clock speeds might require higher looptimes (lower looptime rate) | | ibus_telemetry_type | 0 | Type compatibility ibus telemetry for transmitters. See Telemetry.md label IBUS for details. | | idle_power | 0 | Power draw at zero throttle used for remaining flight time/distance estimation in 0.01W unit | -| imu2_align_pitch | | | -| imu2_align_roll | | | -| imu2_align_yaw | | | -| imu2_enabled | | | -| imu2_gain_acc_x | | | -| imu2_gain_acc_y | | | -| imu2_gain_acc_z | | | -| imu2_gain_mag_x | | | -| imu2_gain_mag_y | | | -| imu2_gain_mag_z | | | -| imu2_radius_acc | | | -| imu2_radius_mag | | | -| imu2_use_for_osd_ahi | | | -| imu2_use_for_osd_heading | | | -| imu2_use_for_stabilized | | | +| imu2_align_pitch | 0 | Pitch alignment for Secondary IMU. 1/10 of a degree | +| imu2_align_roll | 0 | Roll alignment for Secondary IMU. 1/10 of a degree | +| imu2_align_yaw | 0 | Yaw alignment for Secondary IMU. 1/10 of a degree | +| imu2_gain_acc_x | 0 | Secondary IMU ACC calibration data | +| imu2_gain_acc_y | 0 | Secondary IMU ACC calibration data | +| imu2_gain_acc_z | 0 | Secondary IMU ACC calibration data | +| imu2_gain_mag_x | 0 | Secondary IMU MAG calibration data | +| imu2_gain_mag_y | 0 | Secondary IMU MAG calibration data | +| imu2_gain_mag_z | 0 | Secondary IMU MAG calibration data | +| imu2_hardware | NONE | Selection of a Secondary IMU hardware type. NONE disables this functionality | +| imu2_radius_acc | 0 | Secondary IMU MAG calibration data | +| imu2_radius_mag | 0 | Secondary IMU MAG calibration data | +| imu2_use_for_osd_ahi | OFF | If set to ON, Secondary IMU data will be used for Analog OSD Artificial Horizon | +| imu2_use_for_osd_heading | OFF | If set to ON, Secondary IMU data will be used for Analog OSD heading | +| imu2_use_for_stabilized | OFF | If set to ON, Secondary IMU data will be used for Angle, Horizon and all other modes that control attitude (PosHold, WP, RTH) | | imu_acc_ignore_rate | 0 | Total gyro rotation rate threshold [deg/s] to consider accelerometer trustworthy on airplanes | | imu_acc_ignore_slope | 0 | Half-width of the interval to gradually reduce accelerometer weight. Centered at `imu_acc_ignore_rate` (exactly 50% weight) | | imu_dcm_ki | 50 | Inertial Measurement Unit KI Gain for accelerometer measurements | diff --git a/src/main/fc/fc_tasks.c b/src/main/fc/fc_tasks.c index 761483f718..33d417a1d6 100755 --- a/src/main/fc/fc_tasks.c +++ b/src/main/fc/fc_tasks.c @@ -377,7 +377,7 @@ void fcTasksInit(void) setTaskEnabled(TASK_SMARTPORT_MASTER, true); #endif #ifdef USE_SECONDARY_IMU - setTaskEnabled(TASK_SECONDARY_IMU, secondaryImuConfig()->enabled && secondaryImuState.active); + setTaskEnabled(TASK_SECONDARY_IMU, secondaryImuConfig()->hardwareType != SECONDARY_IMU_NONE && secondaryImuState.active); #endif } diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 1dd75328be..d95bb7e0ba 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -9,6 +9,9 @@ tables: - name: rangefinder_hardware values: ["NONE", "HCSR04", "SRF10", "INAV_I2C", "VL53L0X", "MSP", "UNUSED", "BENEWAKE"] enum: rangefinderType_e + - name: secondary_imu_hardware + values: ["NONE", "BNO055"] + enum: secondaryImuType_e - name: mag_hardware values: ["NONE", "AUTO", "HMC5883", "AK8975", "GPSMAG", "MAG3110", "AK8963", "IST8310", "QMC5883", "MPU9250", "IST8308", "LIS3MDL", "MSP", "RM3100", FAKE"] enum: magSensor_e @@ -409,60 +412,90 @@ groups: headers: ["flight/secondary_imu.h"] condition: USE_SECONDARY_IMU members: - - name: imu2_enabled - field: enabled - type: bool + - name: imu2_hardware + description: "Selection of a Secondary IMU hardware type. NONE disables this functionality" + default_value: "NONE" + field: hardwareType + table: secondary_imu_hardware - name: imu2_use_for_osd_heading + description: "If set to ON, Secondary IMU data will be used for Analog OSD heading" + default_value: "OFF" field: useForOsdHeading type: bool - name: imu2_use_for_osd_ahi + description: "If set to ON, Secondary IMU data will be used for Analog OSD Artificial Horizon" field: useForOsdAHI + default_value: "OFF" type: bool - name: imu2_use_for_stabilized + description: "If set to ON, Secondary IMU data will be used for Angle, Horizon and all other modes that control attitude (PosHold, WP, RTH)" field: useForStabilized + default_value: "OFF" type: bool - name: imu2_align_roll + description: "Roll alignment for Secondary IMU. 1/10 of a degree" field: rollDeciDegrees + default_value: "0" min: -1800 max: 3600 - name: imu2_align_pitch + description: "Pitch alignment for Secondary IMU. 1/10 of a degree" field: pitchDeciDegrees + default_value: "0" min: -1800 max: 3600 - name: imu2_align_yaw + description: "Yaw alignment for Secondary IMU. 1/10 of a degree" field: yawDeciDegrees + default_value: "0" min: -1800 max: 3600 - name: imu2_gain_acc_x + description: "Secondary IMU ACC calibration data" field: calibrationOffsetAcc[X] + default_value: "0" min: INT16_MIN max: INT16_MAX - name: imu2_gain_acc_y field: calibrationOffsetAcc[Y] + description: "Secondary IMU ACC calibration data" + default_value: "0" min: INT16_MIN max: INT16_MAX - name: imu2_gain_acc_z field: calibrationOffsetAcc[Z] + description: "Secondary IMU ACC calibration data" + default_value: "0" min: INT16_MIN max: INT16_MAX - name: imu2_gain_mag_x field: calibrationOffsetMag[X] + description: "Secondary IMU MAG calibration data" + default_value: "0" min: INT16_MIN max: INT16_MAX - name: imu2_gain_mag_y field: calibrationOffsetMag[Y] + description: "Secondary IMU MAG calibration data" + default_value: "0" min: INT16_MIN max: INT16_MAX - name: imu2_gain_mag_z field: calibrationOffsetMag[Z] + description: "Secondary IMU MAG calibration data" + default_value: "0" min: INT16_MIN max: INT16_MAX - name: imu2_radius_acc field: calibrationRadiusAcc + description: "Secondary IMU MAG calibration data" + default_value: "0" min: INT16_MIN max: INT16_MAX - name: imu2_radius_mag field: calibrationRadiusMag + description: "Secondary IMU MAG calibration data" + default_value: "0" min: INT16_MIN max: INT16_MAX diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index aa170855ae..2bb7c4a204 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -555,7 +555,7 @@ static void pidLevel(pidState_t *pidState, flight_dynamics_index_t axis, float h #ifdef USE_SECONDARY_IMU float actual; - if (secondaryImuConfig()->useForStabilized) { + if (secondaryImuState.active && secondaryImuConfig()->useForStabilized) { if (axis == FD_ROLL) { actual = secondaryImuState.eulerAngles.values.roll; } else { diff --git a/src/main/flight/secondary_imu.c b/src/main/flight/secondary_imu.c index e9c8fdb0e8..da3a4dfde7 100644 --- a/src/main/flight/secondary_imu.c +++ b/src/main/flight/secondary_imu.c @@ -40,7 +40,7 @@ EXTENDED_FASTRAM secondaryImuState_t secondaryImuState; void pgResetFn_secondaryImuConfig(secondaryImuConfig_t *instance) { - instance->enabled = 0; + instance->hardwareType = SECONDARY_IMU_NONE, instance->rollDeciDegrees = 0; instance->pitchDeciDegrees = 0; instance->yawDeciDegrees = 0; @@ -60,6 +60,7 @@ void pgResetFn_secondaryImuConfig(secondaryImuConfig_t *instance) void secondaryImuInit(void) { + secondaryImuState.active = false; // Create magnetic declination matrix const int deg = compassConfig()->mag_declination / 100; const int min = compassConfig()->mag_declination % 100; @@ -79,7 +80,14 @@ void secondaryImuInit(void) calibrationData.radius[ACC] = secondaryImuConfig()->calibrationRadiusAcc; calibrationData.radius[MAG] = secondaryImuConfig()->calibrationRadiusMag; - secondaryImuState.active = bno055Init(calibrationData, (secondaryImuConfig()->calibrationRadiusAcc && secondaryImuConfig()->calibrationRadiusMag)); + if (secondaryImuConfig()->hardwareType == SECONDARY_IMU_BNO055) { + secondaryImuState.active = bno055Init(calibrationData, (secondaryImuConfig()->calibrationRadiusAcc && secondaryImuConfig()->calibrationRadiusMag)); + } + + if (!secondaryImuState.active) { + secondaryImuConfigMutable()->hardwareType = SECONDARY_IMU_NONE; + } + } void taskSecondaryImu(timeUs_t currentTimeUs) diff --git a/src/main/flight/secondary_imu.h b/src/main/flight/secondary_imu.h index b3d8f1da29..b674bc9c93 100644 --- a/src/main/flight/secondary_imu.h +++ b/src/main/flight/secondary_imu.h @@ -29,8 +29,13 @@ #include "sensors/sensors.h" #include "drivers/accgyro/accgyro_bno055.h" +typedef enum { + SECONDARY_IMU_NONE = 0, + SECONDARY_IMU_BNO055 = 1, +} secondaryImuType_e; + typedef struct secondaryImuConfig_s { - uint8_t enabled; + uint8_t hardwareType; int16_t rollDeciDegrees; int16_t pitchDeciDegrees; int16_t yawDeciDegrees; diff --git a/src/main/io/osd.c b/src/main/io/osd.c index 75d0bf1044..ae636cba9c 100755 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -942,7 +942,7 @@ static inline int32_t osdGetAltitudeMsl(void) static bool osdIsHeadingValid(void) { #ifdef USE_SECONDARY_IMU - if (secondaryImuConfig()->useForOsdHeading) { + if (secondaryImuState.active && secondaryImuConfig()->useForOsdHeading) { return true; } else { return isImuHeadingValid(); @@ -955,7 +955,7 @@ static bool osdIsHeadingValid(void) int16_t osdGetHeading(void) { #ifdef USE_SECONDARY_IMU - if (secondaryImuConfig()->useForOsdHeading) { + if (secondaryImuState.active && secondaryImuConfig()->useForOsdHeading) { return secondaryImuState.eulerAngles.values.yaw; } else { return attitude.values.yaw; @@ -1872,7 +1872,7 @@ static bool osdDrawSingleElement(uint8_t item) float pitchAngle; #ifdef USE_SECONDARY_IMU - if (secondaryImuConfig()->useForOsdAHI) { + if (secondaryImuState.active && secondaryImuConfig()->useForOsdAHI) { rollAngle = DECIDEGREES_TO_RADIANS(secondaryImuState.eulerAngles.values.roll); pitchAngle = DECIDEGREES_TO_RADIANS(secondaryImuState.eulerAngles.values.pitch); } else { From f567a1b8e2b18f8195eb650f7b16bdfd2bf04146 Mon Sep 17 00:00:00 2001 From: "Pawel Spychalski (DzikuVx)" Date: Sun, 4 Apr 2021 17:24:58 +0200 Subject: [PATCH 114/286] drop legacy file --- make/source.mk | 282 ------------------------------------------------- 1 file changed, 282 deletions(-) delete mode 100644 make/source.mk diff --git a/make/source.mk b/make/source.mk deleted file mode 100644 index e3a58d96db..0000000000 --- a/make/source.mk +++ /dev/null @@ -1,282 +0,0 @@ -COMMON_SRC = \ - $(TARGET_DIR_SRC) \ - main.c \ - target/common_hardware.c \ - build/assert.c \ - build/build_config.c \ - build/debug.c \ - build/version.c \ - common/bitarray.c \ - common/calibration.c \ - common/colorconversion.c \ - common/crc.c \ - common/encoding.c \ - common/filter.c \ - common/gps_conversion.c \ - common/log.c \ - common/logic_condition.c \ - common/global_functions.c \ - common/maths.c \ - common/memory.c \ - common/olc.c \ - common/printf.c \ - common/streambuf.c \ - common/string_light.c \ - common/time.c \ - common/typeconversion.c \ - common/uvarint.c \ - config/config_eeprom.c \ - config/config_streamer.c \ - config/feature.c \ - config/parameter_group.c \ - config/general_settings.c \ - drivers/adc.c \ - drivers/buf_writer.c \ - drivers/bus.c \ - drivers/bus_busdev_i2c.c \ - drivers/bus_busdev_spi.c \ - drivers/bus_i2c_soft.c \ - drivers/bus_spi.c \ - drivers/display.c \ - drivers/display_canvas.c \ - drivers/display_font_metadata.c \ - drivers/exti.c \ - drivers/io.c \ - drivers/io_pca9685.c \ - drivers/light_led.c \ - drivers/osd.c \ - drivers/resource.c \ - drivers/rx_nrf24l01.c \ - drivers/rx_spi.c \ - drivers/rx_xn297.c \ - drivers/pitotmeter_adc.c \ - drivers/pitotmeter_virtual.c \ - drivers/pwm_esc_detect.c \ - drivers/pwm_mapping.c \ - drivers/pwm_output.c \ - drivers/pinio.c \ - drivers/rcc.c \ - drivers/rx_pwm.c \ - drivers/serial.c \ - drivers/serial_uart.c \ - drivers/sound_beeper.c \ - drivers/stack_check.c \ - drivers/system.c \ - drivers/timer.c \ - drivers/lights_io.c \ - drivers/1-wire.c \ - drivers/1-wire/ds_crc.c \ - drivers/1-wire/ds2482.c \ - drivers/temperature/ds18b20.c \ - drivers/temperature/lm75.c \ - drivers/pitotmeter_ms4525.c \ - drivers/pitotmeter_ms4525.c \ - drivers/accgyro/accgyro_bno055.c \ - fc/cli.c \ - fc/config.c \ - fc/controlrate_profile.c \ - fc/fc_core.c \ - fc/fc_init.c \ - fc/fc_tasks.c \ - fc/fc_hardfaults.c \ - fc/fc_msp.c \ - fc/fc_msp_box.c \ - fc/rc_smoothing.c \ - fc/rc_adjustments.c \ - fc/rc_controls.c \ - fc/rc_curves.c \ - fc/rc_modes.c \ - fc/runtime_config.c \ - fc/settings.c \ - fc/stats.c \ - flight/failsafe.c \ - flight/hil.c \ - flight/imu.c \ - flight/mixer.c \ - flight/pid.c \ - flight/pid_autotune.c \ - flight/rth_estimator.c \ - flight/servos.c \ - flight/wind_estimator.c \ - flight/gyroanalyse.c \ - flight/secondary_imu.c \ - flight/rpm_filter.c \ - flight/dynamic_gyro_notch.c \ - io/beeper.c \ - io/esc_serialshot.c \ - io/frsky_osd.c \ - io/osd_dji_hd.c \ - io/lights.c \ - io/piniobox.c \ - io/pwmdriver_i2c.c \ - io/serial.c \ - io/serial_4way.c \ - io/serial_4way_avrootloader.c \ - io/serial_4way_stk500v2.c \ - io/statusindicator.c \ - io/rcdevice.c \ - io/rcdevice_cam.c \ - msp/msp_serial.c \ - rx/crsf.c \ - rx/eleres.c \ - rx/fport.c \ - rx/ibus.c \ - rx/jetiexbus.c \ - rx/msp.c \ - rx/msp_override.c \ - rx/nrf24_cx10.c \ - rx/nrf24_inav.c \ - rx/nrf24_h8_3d.c \ - rx/nrf24_syma.c \ - rx/nrf24_v202.c \ - rx/pwm.c \ - rx/rx.c \ - rx/rx_spi.c \ - rx/sbus.c \ - rx/sbus_channels.c \ - rx/spektrum.c \ - rx/sumd.c \ - rx/sumh.c \ - rx/uib_rx.c \ - rx/xbus.c \ - scheduler/scheduler.c \ - sensors/acceleration.c \ - sensors/battery.c \ - sensors/temperature.c \ - sensors/boardalignment.c \ - sensors/compass.c \ - sensors/diagnostics.c \ - sensors/gyro.c \ - sensors/initialisation.c \ - sensors/esc_sensor.c \ - uav_interconnect/uav_interconnect_bus.c \ - uav_interconnect/uav_interconnect_rangefinder.c \ - blackbox/blackbox.c \ - blackbox/blackbox_encoding.c \ - blackbox/blackbox_io.c \ - cms/cms.c \ - cms/cms_menu_battery.c \ - cms/cms_menu_blackbox.c \ - cms/cms_menu_builtin.c \ - cms/cms_menu_imu.c \ - cms/cms_menu_ledstrip.c \ - cms/cms_menu_misc.c \ - cms/cms_menu_mixer_servo.c \ - cms/cms_menu_navigation.c \ - cms/cms_menu_osd.c \ - cms/cms_menu_saveexit.c \ - cms/cms_menu_vtx.c \ - drivers/display_ug2864hsweg01.c \ - drivers/rangefinder/rangefinder_hcsr04.c \ - drivers/rangefinder/rangefinder_hcsr04_i2c.c \ - drivers/rangefinder/rangefinder_srf10.c \ - drivers/rangefinder/rangefinder_vl53l0x.c \ - drivers/rangefinder/rangefinder_virtual.c \ - drivers/opflow/opflow_fake.c \ - drivers/opflow/opflow_virtual.c \ - drivers/vtx_common.c \ - io/rangefinder_msp.c \ - io/rangefinder_benewake.c \ - io/opflow_cxof.c \ - io/opflow_msp.c \ - io/dashboard.c \ - io/displayport_frsky_osd.c \ - io/displayport_max7456.c \ - io/displayport_msp.c \ - io/displayport_oled.c \ - io/displayport_hott.c \ - io/gps.c \ - io/gps_ublox.c \ - io/gps_nmea.c \ - io/gps_naza.c \ - io/ledstrip.c \ - io/osd.c \ - io/osd_canvas.c \ - io/osd_common.c \ - io/osd_grid.c \ - io/osd_hud.c \ - navigation/navigation.c \ - navigation/navigation_fixedwing.c \ - navigation/navigation_fw_launch.c \ - navigation/navigation_geo.c \ - navigation/navigation_multicopter.c \ - navigation/navigation_pos_estimator.c \ - navigation/navigation_pos_estimator_agl.c \ - navigation/navigation_pos_estimator_flow.c \ - sensors/barometer.c \ - sensors/pitotmeter.c \ - sensors/rangefinder.c \ - sensors/opflow.c \ - telemetry/crsf.c \ - telemetry/frsky.c \ - telemetry/frsky_d.c \ - telemetry/hott.c \ - telemetry/ibus_shared.c \ - telemetry/ibus.c \ - telemetry/ltm.c \ - telemetry/mavlink.c \ - telemetry/msp_shared.c \ - telemetry/smartport.c \ - telemetry/sim.c \ - telemetry/telemetry.c \ - io/vtx.c \ - io/vtx_string.c \ - io/vtx_smartaudio.c \ - io/vtx_tramp.c \ - io/vtx_ffpv24g.c \ - io/vtx_control.c - -COMMON_DEVICE_SRC = \ - $(CMSIS_SRC) \ - $(DEVICE_STDPERIPH_SRC) - -TARGET_SRC := $(STARTUP_SRC) $(COMMON_DEVICE_SRC) $(COMMON_SRC) $(MCU_COMMON_SRC) $(TARGET_SRC) - -#excludes -TARGET_SRC := $(filter-out $(MCU_EXCLUDES), $(TARGET_SRC)) - -ifneq ($(filter ONBOARDFLASH,$(FEATURES)),) -TARGET_SRC += \ - drivers/flash_m25p16.c \ - io/flashfs.c \ - $(MSC_SRC) -endif - -ifneq ($(filter SDCARD,$(FEATURES)),) -TARGET_SRC += \ - drivers/sdcard/sdcard.c \ - drivers/sdcard/sdcard_spi.c \ - drivers/sdcard/sdcard_sdio.c \ - drivers/sdcard/sdcard_standard.c \ - io/asyncfatfs/asyncfatfs.c \ - io/asyncfatfs/fat_standard.c \ - $(MSC_SRC) -endif - -ifneq ($(filter VCP,$(FEATURES)),) -TARGET_SRC += $(VCP_SRC) -endif - -ifneq ($(filter MSC,$(FEATURES)),) -TARGET_SRC += $(MSC_SRC) -endif - -ifneq ($(DSP_LIB),) - -INCLUDE_DIRS += $(DSP_LIB)/Include - -TARGET_SRC += $(DSP_LIB)/Source/BasicMathFunctions/arm_mult_f32.c -TARGET_SRC += $(DSP_LIB)/Source/TransformFunctions/arm_rfft_fast_f32.c -TARGET_SRC += $(DSP_LIB)/Source/TransformFunctions/arm_cfft_f32.c -TARGET_SRC += $(DSP_LIB)/Source/TransformFunctions/arm_rfft_fast_init_f32.c -TARGET_SRC += $(DSP_LIB)/Source/TransformFunctions/arm_cfft_radix8_f32.c -TARGET_SRC += $(DSP_LIB)/Source/CommonTables/arm_common_tables.c - -TARGET_SRC += $(DSP_LIB)/Source/ComplexMathFunctions/arm_cmplx_mag_f32.c -TARGET_SRC += $(DSP_LIB)/Source/StatisticsFunctions/arm_max_f32.c - -TARGET_SRC += $(wildcard $(DSP_LIB)/Source/*/*.S) -endif - -# Search path and source files for the ST stdperiph library -VPATH := $(VPATH):$(STDPERIPH_DIR)/src From 99a8084125034fe960173a3035944cce752f95e8 Mon Sep 17 00:00:00 2001 From: Harry Phillips Date: Sun, 4 Apr 2021 22:46:22 +0100 Subject: [PATCH 115/286] Bump parameter group version, add default value --- src/main/fc/rc_controls.c | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/src/main/fc/rc_controls.c b/src/main/fc/rc_controls.c index 1b8664972d..51b9b2d605 100644 --- a/src/main/fc/rc_controls.c +++ b/src/main/fc/rc_controls.c @@ -64,12 +64,13 @@ #define AIRMODE_DEADBAND 25 #define MIN_RC_TICK_INTERVAL_MS 20 #define DEFAULT_RC_SWITCH_DISARM_DELAY_MS 250 // Wait at least 250ms before disarming via switch +#define DEFAULT_PREARM_TIMEOUT 10000 // Prearm is invalidated after 10 seconds stickPositions_e rcStickPositions; FASTRAM int16_t rcCommand[4]; // interval [1000;2000] for THROTTLE and [-500;+500] for ROLL/PITCH/YAW -PG_REGISTER_WITH_RESET_TEMPLATE(rcControlsConfig_t, rcControlsConfig, PG_RC_CONTROLS_CONFIG, 1); +PG_REGISTER_WITH_RESET_TEMPLATE(rcControlsConfig_t, rcControlsConfig, PG_RC_CONTROLS_CONFIG, 2); PG_RESET_TEMPLATE(rcControlsConfig_t, rcControlsConfig, .deadband = 5, @@ -87,6 +88,7 @@ PG_RESET_TEMPLATE(armingConfig_t, armingConfig, .fixed_wing_auto_arm = 0, .disarm_kill_switch = 1, .switchDisarmDelayMs = DEFAULT_RC_SWITCH_DISARM_DELAY_MS, + .prearmTimeoutMs = DEFAULT_PREARM_TIMEOUT, ); bool areSticksInApModePosition(uint16_t ap_mode) From 4e5964e77c49f75b92911cd7f332d1d301cce5a0 Mon Sep 17 00:00:00 2001 From: emtrax-ltd Date: Mon, 5 Apr 2021 21:50:54 +0200 Subject: [PATCH 116/286] Added rangefinder "GY-US42(v2) Ultrasonic Range Sensor" --- src/main/CMakeLists.txt | 4 +- src/main/drivers/bus.h | 1 + src/main/drivers/bus_i2c_hal.c | 2 +- .../drivers/rangefinder/rangefinder_us42.c | 132 ++++++++++++++++++ .../drivers/rangefinder/rangefinder_us42.h | 22 +++ src/main/fc/settings.yaml | 2 +- src/main/sensors/rangefinder.c | 12 +- src/main/sensors/rangefinder.h | 1 + src/main/target/MATEKF405SE/target.h | 1 + src/main/target/common.h | 1 + src/main/target/common_hardware.c | 9 ++ 11 files changed, 183 insertions(+), 4 deletions(-) create mode 100644 src/main/drivers/rangefinder/rangefinder_us42.c create mode 100644 src/main/drivers/rangefinder/rangefinder_us42.h diff --git a/src/main/CMakeLists.txt b/src/main/CMakeLists.txt index 47cb31eea8..6e2d28c864 100644 --- a/src/main/CMakeLists.txt +++ b/src/main/CMakeLists.txt @@ -238,7 +238,9 @@ main_sources(COMMON_SRC drivers/rangefinder/rangefinder_vl53l1x.h drivers/rangefinder/rangefinder_virtual.c drivers/rangefinder/rangefinder_virtual.h - + drivers/rangefinder/rangefinder_us42.c + drivers/rangefinder/rangefinder_us42.h + drivers/resource.c drivers/resource.h drivers/rcc.c diff --git a/src/main/drivers/bus.h b/src/main/drivers/bus.h index fd8239964c..eefa81eb10 100755 --- a/src/main/drivers/bus.h +++ b/src/main/drivers/bus.h @@ -141,6 +141,7 @@ typedef enum { DEVHW_HCSR04_I2C, // DIY-style adapter DEVHW_VL53L0X, DEVHW_VL53L1X, + DEVHW_US42, /* Other hardware */ DEVHW_MS4525, // Pitot meter diff --git a/src/main/drivers/bus_i2c_hal.c b/src/main/drivers/bus_i2c_hal.c index 0531f2af5b..91644b895a 100644 --- a/src/main/drivers/bus_i2c_hal.c +++ b/src/main/drivers/bus_i2c_hal.c @@ -174,7 +174,7 @@ bool i2cWriteBuffer(I2CDevice device, uint8_t addr_, uint8_t reg_, uint8_t len_, HAL_StatusTypeDef status; - if (reg_ == 0xFF && allowRawAccess) { + if ((reg_ == 0xFF || len_ == 0) && allowRawAccess) { status = HAL_I2C_Master_Transmit(&state->handle, addr_ << 1, (uint8_t *)data, len_, I2C_DEFAULT_TIMEOUT); } else { diff --git a/src/main/drivers/rangefinder/rangefinder_us42.c b/src/main/drivers/rangefinder/rangefinder_us42.c new file mode 100644 index 0000000000..eabb571d92 --- /dev/null +++ b/src/main/drivers/rangefinder/rangefinder_us42.c @@ -0,0 +1,132 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Cleanflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Cleanflight. If not, see . + */ + +#include +#include + +#include "platform.h" + +#if defined(USE_RANGEFINDER) && defined(USE_RANGEFINDER_US42) + +#include "build/build_config.h" + +#include "drivers/time.h" +#include "drivers/bus_i2c.h" + +#include "drivers/rangefinder/rangefinder.h" +#include "drivers/rangefinder/rangefinder_us42.h" + +#include "build/debug.h" + +// GY-US42(v2) Ultrasonic Range Sensor +#define US42_MAX_RANGE_CM 400 // vcc=3.3v -> 500cm; vcc=5v -> 700cm; Ardupilot recommends a maximum of 400cm +#define US42_DETECTION_CONE_DECIDEGREES 250 +#define US42_DETECTION_CONE_EXTENDED_DECIDEGREES 300 +#define US42_MIN_PROBE_INTERVAL 50 + +#define US42_I2C_ADDRESS 0x70 +#define US42_I2C_REGISTRY_BASE 0x00 +#define US42_I2C_REGISTRY_PROBE 0x51 +#define US42_I2C_REGISTRY_STATUS_OK 0x00 +#define US42_I2C_REGISTRY_DISTANCE_HIGH 0x00 +#define US42_I2C_REGISTRY_DISTANCE_LOW 0x01 + +volatile int32_t us42MeasurementCm = RANGEFINDER_OUT_OF_RANGE; +static int16_t minimumReadingIntervalMs = US42_MIN_PROBE_INTERVAL; +static uint32_t timeOfLastMeasurementMs; +uint8_t nullProbeCommandValue[0]; +static bool isUs42Responding = false; + +static void us42Init(rangefinderDev_t *rangefinder) +{ + busWriteBuf(rangefinder->busDev, US42_I2C_REGISTRY_PROBE, nullProbeCommandValue, 0); +} + +void us42Update(rangefinderDev_t *rangefinder) +{ + uint8_t data[2]; + isUs42Responding = busReadBuf(rangefinder->busDev, US42_I2C_REGISTRY_PROBE, data, 2); + + if (isUs42Responding) { + us42MeasurementCm = (int32_t)data[0] << 8 | (int32_t)data[1]; + + if (us42MeasurementCm > US42_MAX_RANGE_CM) { + us42MeasurementCm = RANGEFINDER_OUT_OF_RANGE; + } + + } else { + us42MeasurementCm = RANGEFINDER_HARDWARE_FAILURE; + } + + const timeMs_t timeNowMs = millis(); + if (timeNowMs > timeOfLastMeasurementMs + minimumReadingIntervalMs) { + // measurement repeat interval should be greater than minimumReadingIntervalMs + // to avoid interference between connective measurements. + timeOfLastMeasurementMs = timeNowMs; + busWriteBuf(rangefinder->busDev, US42_I2C_REGISTRY_PROBE, nullProbeCommandValue, 0); + } +} + +/** + * Get the distance that was measured by the last pulse, in centimeters. + */ +int32_t us42GetDistance(rangefinderDev_t *rangefinder) +{ + UNUSED(rangefinder); + return us42MeasurementCm; +} + +static bool deviceDetect(busDevice_t * busDev) +{ + for (int retry = 0; retry < 5; retry++) { + uint8_t inquiryResult; + + delay(150); + + bool ack = busRead(busDev, US42_I2C_REGISTRY_BASE, &inquiryResult); + if (ack && inquiryResult == US42_I2C_REGISTRY_STATUS_OK) { + return true; + } + }; + + return false; +} + +bool us42Detect(rangefinderDev_t *rangefinder) +{ + rangefinder->busDev = busDeviceInit(BUSTYPE_I2C, DEVHW_US42, 0, OWNER_RANGEFINDER); + if (rangefinder->busDev == NULL) { + return false; + } + + if (!deviceDetect(rangefinder->busDev)) { + busDeviceDeInit(rangefinder->busDev); + return false; + } + + rangefinder->delayMs = RANGEFINDER_US42_TASK_PERIOD_MS; + rangefinder->maxRangeCm = US42_MAX_RANGE_CM; + rangefinder->detectionConeDeciDegrees = US42_DETECTION_CONE_DECIDEGREES; + rangefinder->detectionConeExtendedDeciDegrees = US42_DETECTION_CONE_EXTENDED_DECIDEGREES; + + rangefinder->init = &us42Init; + rangefinder->update = &us42Update; + rangefinder->read = &us42GetDistance; + + return true; +} +#endif \ No newline at end of file diff --git a/src/main/drivers/rangefinder/rangefinder_us42.h b/src/main/drivers/rangefinder/rangefinder_us42.h new file mode 100644 index 0000000000..e6424c6c85 --- /dev/null +++ b/src/main/drivers/rangefinder/rangefinder_us42.h @@ -0,0 +1,22 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Cleanflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Cleanflight. If not, see . + */ + +#pragma once + +#define RANGEFINDER_US42_TASK_PERIOD_MS 100 + +bool us42Detect(rangefinderDev_t *dev); \ No newline at end of file diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 5be6038db3..16e267df7b 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -7,7 +7,7 @@ tables: values: ["NONE", "AUTO", "ADXL345", "MPU6050", "MMA845x", "BMA280", "LSM303DLHC", "MPU6000", "MPU6500", "MPU9250", "BMI160", "ICM20689", "BMI088", "FAKE"] enum: accelerationSensor_e - name: rangefinder_hardware - values: ["NONE", "HCSR04", "SRF10", "INAV_I2C", "VL53L0X", "MSP", "UNUSED", "BENEWAKE", "VL53L1X"] + values: ["NONE", "HCSR04", "SRF10", "INAV_I2C", "VL53L0X", "MSP", "UNUSED", "BENEWAKE", "VL53L1X", "US42"] enum: rangefinderType_e - name: secondary_imu_hardware values: ["NONE", "BNO055"] diff --git a/src/main/sensors/rangefinder.c b/src/main/sensors/rangefinder.c index 5c7f10c353..4d360c9a22 100644 --- a/src/main/sensors/rangefinder.c +++ b/src/main/sensors/rangefinder.c @@ -42,6 +42,7 @@ #include "drivers/rangefinder/rangefinder_vl53l0x.h" #include "drivers/rangefinder/rangefinder_vl53l1x.h" #include "drivers/rangefinder/rangefinder_virtual.h" +#include "drivers/rangefinder/rangefinder_us42.h" #include "fc/config.h" #include "fc/runtime_config.h" @@ -159,7 +160,16 @@ static bool rangefinderDetect(rangefinderDev_t * dev, uint8_t rangefinderHardwar #endif break; - case RANGEFINDER_NONE: + case RANGEFINDER_US42: +#ifdef USE_RANGEFINDER_US42 + if (us42Detect(dev)) { + rangefinderHardware = RANGEFINDER_US42; + rescheduleTask(TASK_RANGEFINDER, TASK_PERIOD_MS(RANGEFINDER_US42_TASK_PERIOD_MS)); + } +#endif + break; + + case RANGEFINDER_NONE: rangefinderHardware = RANGEFINDER_NONE; break; } diff --git a/src/main/sensors/rangefinder.h b/src/main/sensors/rangefinder.h index a814a11423..568c01ae12 100644 --- a/src/main/sensors/rangefinder.h +++ b/src/main/sensors/rangefinder.h @@ -31,6 +31,7 @@ typedef enum { RANGEFINDER_UNUSED = 6, // Was UIB RANGEFINDER_BENEWAKE = 7, RANGEFINDER_VL53L1X = 8, + RANGEFINDER_US42 = 9, } rangefinderType_e; typedef struct rangefinderConfig_s { diff --git a/src/main/target/MATEKF405SE/target.h b/src/main/target/MATEKF405SE/target.h index 79fc334751..75afdea322 100644 --- a/src/main/target/MATEKF405SE/target.h +++ b/src/main/target/MATEKF405SE/target.h @@ -72,6 +72,7 @@ #define USE_RANGEFINDER #define USE_RANGEFINDER_HCSR04_I2C +#define USE_RANGEFINDER_US42 #define RANGEFINDER_I2C_BUS BUS_I2C2 #define PITOT_I2C_BUS BUS_I2C2 #define TEMPERATURE_I2C_BUS BUS_I2C2 diff --git a/src/main/target/common.h b/src/main/target/common.h index abe10db98a..2c619084de 100755 --- a/src/main/target/common.h +++ b/src/main/target/common.h @@ -99,6 +99,7 @@ #define USE_RANGEFINDER_VL53L0X #define USE_RANGEFINDER_VL53L1X #define USE_RANGEFINDER_HCSR04_I2C +#define USE_RANGEFINDER_US42 // Allow default optic flow boards #define USE_OPFLOW diff --git a/src/main/target/common_hardware.c b/src/main/target/common_hardware.c index 437e9c7de5..5227932418 100755 --- a/src/main/target/common_hardware.c +++ b/src/main/target/common_hardware.c @@ -320,6 +320,15 @@ #endif #endif +#if defined(USE_RANGEFINDER_US42) + #if !defined(US42_I2C_BUS) + #define US42_I2C_BUS RANGEFINDER_I2C_BUS + #endif + #if defined(US42_I2C_BUS) + BUSDEV_REGISTER_I2C(busdev_us42, DEVHW_US42, US42_I2C_BUS, 0x70, NONE, DEVFLAGS_USE_RAW_REGISTERS, 0); // Requires null data to passthrough + #endif +#endif + /** AIRSPEED SENSORS **/ #if defined(PITOT_I2C_BUS) && !defined(MS4525_I2C_BUS) From 537e4eff4c0571975a7905423c1cb7b46ffaedc9 Mon Sep 17 00:00:00 2001 From: Alexander van Saase Date: Mon, 5 Apr 2021 22:00:34 +0200 Subject: [PATCH 117/286] Increase max cell count to 12 --- src/main/fc/settings.yaml | 4 ++-- src/main/sensors/battery.c | 6 ++++-- 2 files changed, 6 insertions(+), 4 deletions(-) diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 5be6038db3..ab27d8c51c 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -1015,10 +1015,10 @@ groups: field: cells condition: USE_ADC min: 0 - max: 8 + max: 12 - name: vbat_cell_detect_voltage description: "Maximum voltage per cell, used for auto-detecting the number of cells of the battery in 0.01V units, default is 4.30V" - default_value: "430" + default_value: "425" field: voltage.cellDetect condition: USE_ADC min: 100 diff --git a/src/main/sensors/battery.c b/src/main/sensors/battery.c index 01d2582ded..420a9790e9 100644 --- a/src/main/sensors/battery.c +++ b/src/main/sensors/battery.c @@ -100,7 +100,7 @@ void pgResetFn_batteryProfiles(batteryProfile_t *instance) .cells = 0, .voltage = { - .cellDetect = 430, + .cellDetect = 425, .cellMax = 420, .cellMin = 330, .cellWarning = 350 @@ -266,7 +266,9 @@ void batteryUpdate(timeUs_t timeDelta) batteryCellCount = currentBatteryProfile->cells; else { batteryCellCount = (vbat / currentBatteryProfile->voltage.cellDetect) + 1; - if (batteryCellCount > 8) batteryCellCount = 8; // something is wrong, we expect 8 cells maximum (and autodetection will be problematic at 6+ cells) + // Assume there are no 7S, 9S and 11S batteries so round up to 8S, 10S and 12S + batteryCellCount = ((batteryCellCount > 6) && (batteryCellCount & 2) == 0) ? batteryCellCount : batteryCellCount + 1; + batteryCellCount = MIN(batteryCellCount, 12); } batteryFullVoltage = batteryCellCount * currentBatteryProfile->voltage.cellMax; From 6130dcc3200dd51b54065463e95adf30100576b0 Mon Sep 17 00:00:00 2001 From: Alexander van Saase Date: Tue, 6 Apr 2021 12:12:32 +0200 Subject: [PATCH 118/286] docs --- docs/Settings.md | 4 ++-- src/main/fc/settings.yaml | 4 ++-- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/docs/Settings.md b/docs/Settings.md index f8cc0672cb..e5d99e738b 100644 --- a/docs/Settings.md +++ b/docs/Settings.md @@ -41,7 +41,7 @@ | baro_cal_tolerance | 150 | Baro calibration tolerance in cm. The default should allow the noisiest baro to complete calibration [cm]. | | baro_hardware | AUTO | Selection of baro hardware. See Wiki Sensor auto detect and hardware failure detection for more info | | baro_median_filter | ON | 3-point median filtering for barometer readouts. No reason to change this setting | -| bat_cells | 0 | Number of cells of the battery (0 = autodetect), see battery documentation | +| bat_cells | 0 | Number of cells of the battery (0 = autodetect), see battery documentation. 7S, 9S and 11S batteries cannot be autodetected. | | bat_voltage_src | RAW | Chose between raw and sag compensated battery voltage to use for battery alarms and telemetry. Possible values are `RAW` and `SAG_COMP` | | battery_capacity | 0 | Set the battery capacity in mAh or mWh (see `battery_capacity_unit`). Used to calculate the remaining battery capacity. | | battery_capacity_critical | 0 | If the remaining battery capacity goes below this threshold the battery is considered empty and the beeper will emit long beeps. | @@ -514,7 +514,7 @@ | tz_automatic_dst | OFF | Automatically add Daylight Saving Time to the GPS time when needed or simply ignore it. Includes presets for EU and the USA - if you live outside these areas it is suggested to manage DST manually via `tz_offset`. | | tz_offset | 0 | Time zone offset from UTC, in minutes. This is applied to the GPS time for logging and time-stamping of Blackbox logs | | vbat_adc_channel | - | ADC channel to use for battery voltage sensor. Defaults to board VBAT input (if available). 0 = disabled | -| vbat_cell_detect_voltage | 430 | Maximum voltage per cell, used for auto-detecting the number of cells of the battery in 0.01V units, default is 4.30V | +| vbat_cell_detect_voltage | 425 | Maximum voltage per cell, used for auto-detecting the number of cells of the battery in 0.01V units, default is 4.30V. | | vbat_max_cell_voltage | 420 | Maximum voltage per cell in 0.01V units, default is 4.20V | | vbat_meter_type | `ADC` | Vbat voltage source. Possible values: `NONE`, `ADC`, `ESC`. `ESC` required ESC telemetry enebled and running | | vbat_min_cell_voltage | 330 | Minimum voltage per cell, this triggers battery out alarms, in 0.01V units, default is 330 (3.3V) | diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index ab27d8c51c..3a446cf6b6 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -1010,14 +1010,14 @@ groups: value_type: BATTERY_CONFIG_VALUE members: - name: bat_cells - description: "Number of cells of the battery (0 = autodetect), see battery documentation" + description: "Number of cells of the battery (0 = autodetect), see battery documentation. 7S, 9S and 11S batteries cannot be autodetected." default_value: "0" field: cells condition: USE_ADC min: 0 max: 12 - name: vbat_cell_detect_voltage - description: "Maximum voltage per cell, used for auto-detecting the number of cells of the battery in 0.01V units, default is 4.30V" + description: "Maximum voltage per cell, used for auto-detecting the number of cells of the battery in 0.01V units, default is 4.30V." default_value: "425" field: voltage.cellDetect condition: USE_ADC From 943290b2ebd7ab5d05310d272d1cec6e50b58a96 Mon Sep 17 00:00:00 2001 From: Harry Phillips Date: Tue, 6 Apr 2021 11:42:25 +0100 Subject: [PATCH 119/286] Remove MAVLink inline modifiers --- lib/main/MAVLink/README.md | 6 +++++ lib/main/MAVLink/generate.sh | 3 +++ lib/main/MAVLink/protocol.h | 44 ++++++++++++++++++------------------ 3 files changed, 31 insertions(+), 22 deletions(-) diff --git a/lib/main/MAVLink/README.md b/lib/main/MAVLink/README.md index 851b32e3e0..51e912f99c 100644 --- a/lib/main/MAVLink/README.md +++ b/lib/main/MAVLink/README.md @@ -5,3 +5,9 @@ This directory contains the MAVLink library and scripts to automatically generat In order to run it, you will need [Python and some other libraries installed](https://mavlink.io/en/getting_started/installation.html). Then, run the appropriate script (`generate.sh` for Linux, `generate.bat` for Windows) to automatically re-generate the library. + +## IMPORTANT NOTE + +By default, the MAVLink library declares all of its functions as `inline`, which results in common functions being duplicated many times. +After generating the library, `protocol.h` must be modified, and all `inline` keywords removed. +This is performed automatically by `generate.sh`, but not by `generate.bat` as Windows batch files do not have an equivalent to `sed`. So, this must be done manually on Windows. diff --git a/lib/main/MAVLink/generate.sh b/lib/main/MAVLink/generate.sh index 635473a374..f753448aa5 100755 --- a/lib/main/MAVLink/generate.sh +++ b/lib/main/MAVLink/generate.sh @@ -23,3 +23,6 @@ PYTHONPATH="$(pwd)/mavlink-src" echo "Running MAVLink generator..." python -m pymavlink.tools.mavgen --lang=C --wire-protocol=2.0 --output=. mavlink-src/message_definitions/v1.0/common.xml --no-validate + +echo "Removing inlines..." +sed -i 's/ inline//' protocol.h diff --git a/lib/main/MAVLink/protocol.h b/lib/main/MAVLink/protocol.h index d9227303ff..6feca0104c 100755 --- a/lib/main/MAVLink/protocol.h +++ b/lib/main/MAVLink/protocol.h @@ -71,7 +71,7 @@ #else - #define MAVLINK_HELPER static inline + #define MAVLINK_HELPER static #include "mavlink_helpers.h" #endif // MAVLINK_SEPARATE_HELPERS @@ -80,7 +80,7 @@ /** * @brief Get the required buffer size for this message */ -static inline uint16_t mavlink_msg_get_send_buffer_length(const mavlink_message_t* msg) +static uint16_t mavlink_msg_get_send_buffer_length(const mavlink_message_t* msg) { if (msg->magic == MAVLINK_STX_MAVLINK1) { return msg->len + MAVLINK_CORE_HEADER_MAVLINK1_LEN+1 + 2; @@ -90,19 +90,19 @@ static inline uint16_t mavlink_msg_get_send_buffer_length(const mavlink_message_ } #if MAVLINK_NEED_BYTE_SWAP -static inline void byte_swap_2(char *dst, const char *src) +static void byte_swap_2(char *dst, const char *src) { dst[0] = src[1]; dst[1] = src[0]; } -static inline void byte_swap_4(char *dst, const char *src) +static void byte_swap_4(char *dst, const char *src) { dst[0] = src[3]; dst[1] = src[2]; dst[2] = src[1]; dst[3] = src[0]; } -static inline void byte_swap_8(char *dst, const char *src) +static void byte_swap_8(char *dst, const char *src) { dst[0] = src[7]; dst[1] = src[6]; @@ -114,19 +114,19 @@ static inline void byte_swap_8(char *dst, const char *src) dst[7] = src[0]; } #elif !MAVLINK_ALIGNED_FIELDS -static inline void byte_copy_2(char *dst, const char *src) +static void byte_copy_2(char *dst, const char *src) { dst[0] = src[0]; dst[1] = src[1]; } -static inline void byte_copy_4(char *dst, const char *src) +static void byte_copy_4(char *dst, const char *src) { dst[0] = src[0]; dst[1] = src[1]; dst[2] = src[2]; dst[3] = src[3]; } -static inline void byte_copy_8(char *dst, const char *src) +static void byte_copy_8(char *dst, const char *src) { memcpy(dst, src, 8); } @@ -168,7 +168,7 @@ static inline void byte_copy_8(char *dst, const char *src) /* like memcpy(), but if src is NULL, do a memset to zero */ -static inline void mav_array_memcpy(void *dest, const void *src, size_t n) +static void mav_array_memcpy(void *dest, const void *src, size_t n) { if (src == NULL) { memset(dest, 0, n); @@ -180,7 +180,7 @@ static inline void mav_array_memcpy(void *dest, const void *src, size_t n) /* * Place a char array into a buffer */ -static inline void _mav_put_char_array(char *buf, uint8_t wire_offset, const char *b, uint8_t array_length) +static void _mav_put_char_array(char *buf, uint8_t wire_offset, const char *b, uint8_t array_length) { mav_array_memcpy(&buf[wire_offset], b, array_length); @@ -189,7 +189,7 @@ static inline void _mav_put_char_array(char *buf, uint8_t wire_offset, const cha /* * Place a uint8_t array into a buffer */ -static inline void _mav_put_uint8_t_array(char *buf, uint8_t wire_offset, const uint8_t *b, uint8_t array_length) +static void _mav_put_uint8_t_array(char *buf, uint8_t wire_offset, const uint8_t *b, uint8_t array_length) { mav_array_memcpy(&buf[wire_offset], b, array_length); @@ -198,7 +198,7 @@ static inline void _mav_put_uint8_t_array(char *buf, uint8_t wire_offset, const /* * Place a int8_t array into a buffer */ -static inline void _mav_put_int8_t_array(char *buf, uint8_t wire_offset, const int8_t *b, uint8_t array_length) +static void _mav_put_int8_t_array(char *buf, uint8_t wire_offset, const int8_t *b, uint8_t array_length) { mav_array_memcpy(&buf[wire_offset], b, array_length); @@ -206,7 +206,7 @@ static inline void _mav_put_int8_t_array(char *buf, uint8_t wire_offset, const i #if MAVLINK_NEED_BYTE_SWAP #define _MAV_PUT_ARRAY(TYPE, V) \ -static inline void _mav_put_ ## TYPE ##_array(char *buf, uint8_t wire_offset, const TYPE *b, uint8_t array_length) \ +static void _mav_put_ ## TYPE ##_array(char *buf, uint8_t wire_offset, const TYPE *b, uint8_t array_length) \ { \ if (b == NULL) { \ memset(&buf[wire_offset], 0, array_length*sizeof(TYPE)); \ @@ -219,7 +219,7 @@ static inline void _mav_put_ ## TYPE ##_array(char *buf, uint8_t wire_offset, co } #else #define _MAV_PUT_ARRAY(TYPE, V) \ -static inline void _mav_put_ ## TYPE ##_array(char *buf, uint8_t wire_offset, const TYPE *b, uint8_t array_length) \ +static void _mav_put_ ## TYPE ##_array(char *buf, uint8_t wire_offset, const TYPE *b, uint8_t array_length) \ { \ mav_array_memcpy(&buf[wire_offset], b, array_length*sizeof(TYPE)); \ } @@ -240,7 +240,7 @@ _MAV_PUT_ARRAY(double, d) #if MAVLINK_NEED_BYTE_SWAP #define _MAV_MSG_RETURN_TYPE(TYPE, SIZE) \ -static inline TYPE _MAV_RETURN_## TYPE(const mavlink_message_t *msg, uint8_t ofs) \ +static TYPE _MAV_RETURN_## TYPE(const mavlink_message_t *msg, uint8_t ofs) \ { TYPE r; byte_swap_## SIZE((char*)&r, &_MAV_PAYLOAD(msg)[ofs]); return r; } _MAV_MSG_RETURN_TYPE(uint16_t, 2) @@ -254,7 +254,7 @@ _MAV_MSG_RETURN_TYPE(double, 8) #elif !MAVLINK_ALIGNED_FIELDS #define _MAV_MSG_RETURN_TYPE(TYPE, SIZE) \ -static inline TYPE _MAV_RETURN_## TYPE(const mavlink_message_t *msg, uint8_t ofs) \ +static TYPE _MAV_RETURN_## TYPE(const mavlink_message_t *msg, uint8_t ofs) \ { TYPE r; byte_copy_## SIZE((char*)&r, &_MAV_PAYLOAD(msg)[ofs]); return r; } _MAV_MSG_RETURN_TYPE(uint16_t, 2) @@ -267,7 +267,7 @@ _MAV_MSG_RETURN_TYPE(float, 4) _MAV_MSG_RETURN_TYPE(double, 8) #else // nicely aligned, no swap #define _MAV_MSG_RETURN_TYPE(TYPE) \ -static inline TYPE _MAV_RETURN_## TYPE(const mavlink_message_t *msg, uint8_t ofs) \ +static TYPE _MAV_RETURN_## TYPE(const mavlink_message_t *msg, uint8_t ofs) \ { return *(const TYPE *)(&_MAV_PAYLOAD(msg)[ofs]);} _MAV_MSG_RETURN_TYPE(uint16_t) @@ -280,21 +280,21 @@ _MAV_MSG_RETURN_TYPE(float) _MAV_MSG_RETURN_TYPE(double) #endif // MAVLINK_NEED_BYTE_SWAP -static inline uint16_t _MAV_RETURN_char_array(const mavlink_message_t *msg, char *value, +static uint16_t _MAV_RETURN_char_array(const mavlink_message_t *msg, char *value, uint8_t array_length, uint8_t wire_offset) { memcpy(value, &_MAV_PAYLOAD(msg)[wire_offset], array_length); return array_length; } -static inline uint16_t _MAV_RETURN_uint8_t_array(const mavlink_message_t *msg, uint8_t *value, +static uint16_t _MAV_RETURN_uint8_t_array(const mavlink_message_t *msg, uint8_t *value, uint8_t array_length, uint8_t wire_offset) { memcpy(value, &_MAV_PAYLOAD(msg)[wire_offset], array_length); return array_length; } -static inline uint16_t _MAV_RETURN_int8_t_array(const mavlink_message_t *msg, int8_t *value, +static uint16_t _MAV_RETURN_int8_t_array(const mavlink_message_t *msg, int8_t *value, uint8_t array_length, uint8_t wire_offset) { memcpy(value, &_MAV_PAYLOAD(msg)[wire_offset], array_length); @@ -303,7 +303,7 @@ static inline uint16_t _MAV_RETURN_int8_t_array(const mavlink_message_t *msg, in #if MAVLINK_NEED_BYTE_SWAP #define _MAV_RETURN_ARRAY(TYPE, V) \ -static inline uint16_t _MAV_RETURN_## TYPE ##_array(const mavlink_message_t *msg, TYPE *value, \ +static uint16_t _MAV_RETURN_## TYPE ##_array(const mavlink_message_t *msg, TYPE *value, \ uint8_t array_length, uint8_t wire_offset) \ { \ uint16_t i; \ @@ -314,7 +314,7 @@ static inline uint16_t _MAV_RETURN_## TYPE ##_array(const mavlink_message_t *msg } #else #define _MAV_RETURN_ARRAY(TYPE, V) \ -static inline uint16_t _MAV_RETURN_## TYPE ##_array(const mavlink_message_t *msg, TYPE *value, \ +static uint16_t _MAV_RETURN_## TYPE ##_array(const mavlink_message_t *msg, TYPE *value, \ uint8_t array_length, uint8_t wire_offset) \ { \ memcpy(value, &_MAV_PAYLOAD(msg)[wire_offset], array_length*sizeof(TYPE)); \ From d5c94d9f22d575304e9695f25a1d4e37a2a981c5 Mon Sep 17 00:00:00 2001 From: Harry Phillips Date: Tue, 6 Apr 2021 12:00:46 +0100 Subject: [PATCH 120/286] Ignore unused function warnings in MAVLink library --- src/main/rx/mavlink.h | 3 +++ src/main/telemetry/mavlink.c | 3 +++ 2 files changed, 6 insertions(+) diff --git a/src/main/rx/mavlink.h b/src/main/rx/mavlink.h index 1519149237..a54e16d76d 100644 --- a/src/main/rx/mavlink.h +++ b/src/main/rx/mavlink.h @@ -17,7 +17,10 @@ #pragma once +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Wunused-function" #include "common/mavlink.h" +#pragma GCC diagnostic pop void mavlinkRxHandleMessage(const mavlink_rc_channels_override_t *msg); bool mavlinkRxInit(const struct rxConfig_s *initialRxConfig, struct rxRuntimeConfig_s *rxRuntimeConfig); diff --git a/src/main/telemetry/mavlink.c b/src/main/telemetry/mavlink.c index 0ad0cc9279..f9e7e2527b 100644 --- a/src/main/telemetry/mavlink.c +++ b/src/main/telemetry/mavlink.c @@ -87,7 +87,10 @@ #include "scheduler/scheduler.h" +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Wunused-function" #include "common/mavlink.h" +#pragma GCC diagnostic pop #define TELEMETRY_MAVLINK_PORT_MODE MODE_RXTX #define TELEMETRY_MAVLINK_MAXRATE 50 From 871d4f32211a29fa149a60a7ccda887f0ebdcbd7 Mon Sep 17 00:00:00 2001 From: Harry Phillips Date: Tue, 6 Apr 2021 12:01:13 +0100 Subject: [PATCH 121/286] Fix #6577 --- src/main/telemetry/mavlink.c | 11 ++++++++--- 1 file changed, 8 insertions(+), 3 deletions(-) diff --git a/src/main/telemetry/mavlink.c b/src/main/telemetry/mavlink.c index f9e7e2527b..6c9ded6869 100644 --- a/src/main/telemetry/mavlink.c +++ b/src/main/telemetry/mavlink.c @@ -96,6 +96,11 @@ #define TELEMETRY_MAVLINK_MAXRATE 50 #define TELEMETRY_MAVLINK_DELAY ((1000 * 1000) / TELEMETRY_MAVLINK_MAXRATE) +/** + * MAVLink requires angles to be in the range -Pi..Pi. + * This converts angles from a range of 0..Pi to -Pi..Pi + */ +#define RADIANS_TO_MAVLINK_RANGE(angle) (angle > M_PIf) ? angle - (2 * M_PIf) : angle /** @brief A mapping of plane flight modes for custom_mode field of heartbeat. */ typedef enum APM_PLANE_MODE @@ -604,11 +609,11 @@ void mavlinkSendAttitude(void) // time_boot_ms Timestamp (milliseconds since system boot) millis(), // roll Roll angle (rad) - DECIDEGREES_TO_RADIANS(attitude.values.roll), + RADIANS_TO_MAVLINK_RANGE(DECIDEGREES_TO_RADIANS(attitude.values.roll)), // pitch Pitch angle (rad) - DECIDEGREES_TO_RADIANS(-attitude.values.pitch), + RADIANS_TO_MAVLINK_RANGE(DECIDEGREES_TO_RADIANS(-attitude.values.pitch)), // yaw Yaw angle (rad) - DECIDEGREES_TO_RADIANS(attitude.values.yaw), + RADIANS_TO_MAVLINK_RANGE(DECIDEGREES_TO_RADIANS(attitude.values.yaw)), // rollspeed Roll angular speed (rad/s) gyro.gyroADCf[FD_ROLL], // pitchspeed Pitch angular speed (rad/s) From d0e604a7cad3dc41d3b45b571cb9199957e9f2b8 Mon Sep 17 00:00:00 2001 From: Alexander van Saase Date: Sun, 14 Mar 2021 20:31:52 +0100 Subject: [PATCH 122/286] =?UTF-8?q?=EF=BB=BFaltitude=20estimation=20debug?= =?UTF-8?q?=20mode?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/main/build/debug.h | 1 + src/main/fc/settings.yaml | 2 +- src/main/navigation/navigation_pos_estimator.c | 9 +++++++++ 3 files changed, 11 insertions(+), 1 deletion(-) diff --git a/src/main/build/debug.h b/src/main/build/debug.h index d8916f6d56..e8f47543eb 100644 --- a/src/main/build/debug.h +++ b/src/main/build/debug.h @@ -82,5 +82,6 @@ typedef enum { DEBUG_DYNAMIC_GYRO_LPF, DEBUG_FW_D, DEBUG_IMU2, + DEBUG_ALTITUDE, DEBUG_COUNT } debugType_e; diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 5be6038db3..1521c3cfcb 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -87,7 +87,7 @@ tables: "FLOW", "SBUS", "FPORT", "ALWAYS", "SAG_COMP_VOLTAGE", "VIBE", "CRUISE", "REM_FLIGHT_TIME", "SMARTAUDIO", "ACC", "ERPM", "RPM_FILTER", "RPM_FREQ", "NAV_YAW", "DYNAMIC_FILTER", "DYNAMIC_FILTER_FREQUENCY", - "IRLOCK", "CD", "KALMAN_GAIN", "PID_MEASUREMENT", "SPM_CELLS", "SPM_VS600", "SPM_VARIO", "PCF8574", "DYN_GYRO_LPF", "FW_D", "IMU2"] + "IRLOCK", "CD", "KALMAN_GAIN", "PID_MEASUREMENT", "SPM_CELLS", "SPM_VS600", "SPM_VARIO", "PCF8574", "DYN_GYRO_LPF", "FW_D", "IMU2", "ALTITUDE"] - name: async_mode values: ["NONE", "GYRO", "ALL"] - name: aux_operator diff --git a/src/main/navigation/navigation_pos_estimator.c b/src/main/navigation/navigation_pos_estimator.c index 496aeb6c1f..7f8d624507 100755 --- a/src/main/navigation/navigation_pos_estimator.c +++ b/src/main/navigation/navigation_pos_estimator.c @@ -611,6 +611,15 @@ static bool estimationCalculateCorrection_Z(estimationContext_t * ctx) return true; } + DEBUG_SET(DEBUG_ALTITUDE, 0, posEstimator.est.pos.z); // Position estimate + DEBUG_SET(DEBUG_ALTITUDE, 2, posEstimator.baro.alt); // Baro altitude + DEBUG_SET(DEBUG_ALTITUDE, 4, posEstimator.gps.pos.z); // GPS altitude + DEBUG_SET(DEBUG_ALTITUDE, 6, accGetVibrationLevel()); // Vibration level + DEBUG_SET(DEBUG_ALTITUDE, 1, posEstimator.est.vel.z); // Vertical speed estimate + DEBUG_SET(DEBUG_ALTITUDE, 3, posEstimator.gps.vel.z); // GPS vertical speed + DEBUG_SET(DEBUG_ALTITUDE, 5, posEstimator.imu.accelNEU.z); // Vertical acceleration on earth frame + DEBUG_SET(DEBUG_ALTITUDE, 7, navGetAccelerometerWeight()); // Acceleromter weight in position/speed estimation + return false; } From 431af0fbf71e4c756db270e3ed8c347a04ab9aba Mon Sep 17 00:00:00 2001 From: Alexander van Saase Date: Tue, 6 Apr 2021 17:21:33 +0200 Subject: [PATCH 123/286] acc weight scaling --- src/main/navigation/navigation_pos_estimator.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/navigation/navigation_pos_estimator.c b/src/main/navigation/navigation_pos_estimator.c index 7f8d624507..594eff636f 100755 --- a/src/main/navigation/navigation_pos_estimator.c +++ b/src/main/navigation/navigation_pos_estimator.c @@ -618,7 +618,7 @@ static bool estimationCalculateCorrection_Z(estimationContext_t * ctx) DEBUG_SET(DEBUG_ALTITUDE, 1, posEstimator.est.vel.z); // Vertical speed estimate DEBUG_SET(DEBUG_ALTITUDE, 3, posEstimator.gps.vel.z); // GPS vertical speed DEBUG_SET(DEBUG_ALTITUDE, 5, posEstimator.imu.accelNEU.z); // Vertical acceleration on earth frame - DEBUG_SET(DEBUG_ALTITUDE, 7, navGetAccelerometerWeight()); // Acceleromter weight in position/speed estimation + DEBUG_SET(DEBUG_ALTITUDE, 7, navGetAccelerometerWeight()*1000); // Acceleromter weight in position/speed estimation return false; } From 9b6a805c96dadfb20cfc12f3ab3847ee9f3133ac Mon Sep 17 00:00:00 2001 From: Alexander van Saase Date: Tue, 6 Apr 2021 17:26:54 +0200 Subject: [PATCH 124/286] debug element order --- src/main/navigation/navigation_pos_estimator.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/navigation/navigation_pos_estimator.c b/src/main/navigation/navigation_pos_estimator.c index 594eff636f..88a25c418b 100755 --- a/src/main/navigation/navigation_pos_estimator.c +++ b/src/main/navigation/navigation_pos_estimator.c @@ -616,8 +616,8 @@ static bool estimationCalculateCorrection_Z(estimationContext_t * ctx) DEBUG_SET(DEBUG_ALTITUDE, 4, posEstimator.gps.pos.z); // GPS altitude DEBUG_SET(DEBUG_ALTITUDE, 6, accGetVibrationLevel()); // Vibration level DEBUG_SET(DEBUG_ALTITUDE, 1, posEstimator.est.vel.z); // Vertical speed estimate - DEBUG_SET(DEBUG_ALTITUDE, 3, posEstimator.gps.vel.z); // GPS vertical speed - DEBUG_SET(DEBUG_ALTITUDE, 5, posEstimator.imu.accelNEU.z); // Vertical acceleration on earth frame + DEBUG_SET(DEBUG_ALTITUDE, 3, posEstimator.imu.accelNEU.z); // Vertical acceleration on earth frame + DEBUG_SET(DEBUG_ALTITUDE, 5, posEstimator.gps.vel.z); // GPS vertical speed DEBUG_SET(DEBUG_ALTITUDE, 7, navGetAccelerometerWeight()*1000); // Acceleromter weight in position/speed estimation return false; From f96f8123932347974bb29dfefbc0536f2b7b69f7 Mon Sep 17 00:00:00 2001 From: emtrax-ltd Date: Tue, 6 Apr 2021 20:06:16 +0200 Subject: [PATCH 125/286] Added rangefinder "GY-US42(v2) Ultrasonic Range Sensor" --- src/main/target/common_hardware.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/target/common_hardware.c b/src/main/target/common_hardware.c index 5227932418..bc32325ced 100755 --- a/src/main/target/common_hardware.c +++ b/src/main/target/common_hardware.c @@ -321,7 +321,7 @@ #endif #if defined(USE_RANGEFINDER_US42) - #if !defined(US42_I2C_BUS) + #if !defined(US42_I2C_BUS) && defined(RANGEFINDER_I2C_BUS) #define US42_I2C_BUS RANGEFINDER_I2C_BUS #endif #if defined(US42_I2C_BUS) From 1b8d890a672c77296dbdcb42bb08bebd7fb9d816 Mon Sep 17 00:00:00 2001 From: Alexander van Saase Date: Tue, 6 Apr 2021 21:09:57 +0200 Subject: [PATCH 126/286] acc clip count instead of acc weight --- src/main/navigation/navigation_pos_estimator.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/navigation/navigation_pos_estimator.c b/src/main/navigation/navigation_pos_estimator.c index 88a25c418b..e78ffb445a 100755 --- a/src/main/navigation/navigation_pos_estimator.c +++ b/src/main/navigation/navigation_pos_estimator.c @@ -618,7 +618,7 @@ static bool estimationCalculateCorrection_Z(estimationContext_t * ctx) DEBUG_SET(DEBUG_ALTITUDE, 1, posEstimator.est.vel.z); // Vertical speed estimate DEBUG_SET(DEBUG_ALTITUDE, 3, posEstimator.imu.accelNEU.z); // Vertical acceleration on earth frame DEBUG_SET(DEBUG_ALTITUDE, 5, posEstimator.gps.vel.z); // GPS vertical speed - DEBUG_SET(DEBUG_ALTITUDE, 7, navGetAccelerometerWeight()*1000); // Acceleromter weight in position/speed estimation + DEBUG_SET(DEBUG_ALTITUDE, 7, accGetClipCount()); // Clip count return false; } From 3aadc493c01244a238757c4a0337e6218d2525fc Mon Sep 17 00:00:00 2001 From: Michel Pastor Date: Tue, 6 Apr 2021 22:21:49 +0200 Subject: [PATCH 127/286] Rename NAV CRUISE mode to NAV COURSE HOLD (#6297) --- src/main/cms/cms_menu_osd.c | 4 +- src/main/fc/fc_msp_box.c | 6 +- src/main/fc/rc_modes.c | 2 +- src/main/fc/rc_modes.h | 2 +- src/main/fc/runtime_config.c | 2 +- src/main/fc/runtime_config.h | 30 +- src/main/io/osd.c | 28 +- src/main/io/osd.h | 4 +- src/main/navigation/navigation.c | 524 ++++++++++----------- src/main/navigation/navigation_fixedwing.c | 4 +- src/main/navigation/navigation_private.h | 30 +- src/main/programming/logic_condition.c | 2 +- src/main/telemetry/crsf.c | 4 +- src/main/telemetry/frsky.c | 2 +- src/main/telemetry/ltm.c | 2 +- 15 files changed, 323 insertions(+), 323 deletions(-) diff --git a/src/main/cms/cms_menu_osd.c b/src/main/cms/cms_menu_osd.c index 874c5fde29..5b36b61300 100755 --- a/src/main/cms/cms_menu_osd.c +++ b/src/main/cms/cms_menu_osd.c @@ -217,8 +217,8 @@ static const OSD_Entry menuOsdElemsEntries[] = #endif // GPS OSD_ELEMENT_ENTRY("HEADING", OSD_HEADING), OSD_ELEMENT_ENTRY("HEADING GR.", OSD_HEADING_GRAPH), - OSD_ELEMENT_ENTRY("CRS HEAD. ERR", OSD_CRUISE_HEADING_ERROR), - OSD_ELEMENT_ENTRY("CRS HEAD. ADJ", OSD_CRUISE_HEADING_ADJUSTMENT), + OSD_ELEMENT_ENTRY("CRS HLD ERR", OSD_COURSE_HOLD_ERROR), + OSD_ELEMENT_ENTRY("CRS HLD ADJ", OSD_COURSE_HOLD_ADJUSTMENT), #if defined(USE_BARO) || defined(USE_GPS) OSD_ELEMENT_ENTRY("VARIO", OSD_VARIO), OSD_ELEMENT_ENTRY("VARIO NUM", OSD_VARIO_NUM), diff --git a/src/main/fc/fc_msp_box.c b/src/main/fc/fc_msp_box.c index 8734305d48..f517be11f1 100644 --- a/src/main/fc/fc_msp_box.c +++ b/src/main/fc/fc_msp_box.c @@ -82,7 +82,7 @@ static const box_t boxes[CHECKBOX_ITEM_COUNT + 1] = { { BOXOSDALT1, "OSD ALT 1", 42 }, { BOXOSDALT2, "OSD ALT 2", 43 }, { BOXOSDALT3, "OSD ALT 3", 44 }, - { BOXNAVCRUISE, "NAV CRUISE", 45 }, + { BOXNAVCOURSEHOLD, "NAV COURSE HOLD", 45 }, { BOXBRAKING, "MC BRAKING", 46 }, { BOXUSER1, "USER1", BOX_PERMANENT_ID_USER1 }, { BOXUSER2, "USER2", BOX_PERMANENT_ID_USER2 }, @@ -219,7 +219,7 @@ void initActiveBoxIds(void) if (feature(FEATURE_GPS)) { activeBoxIds[activeBoxIdCount++] = BOXGCSNAV; if (STATE(AIRPLANE)) { - activeBoxIds[activeBoxIdCount++] = BOXNAVCRUISE; + activeBoxIds[activeBoxIdCount++] = BOXNAVCOURSEHOLD; } } } @@ -345,7 +345,7 @@ void packBoxModeFlags(boxBitmask_t * mspBoxModeFlags) CHECK_ACTIVE_BOX(IS_ENABLED(FLIGHT_MODE(FAILSAFE_MODE)), BOXFAILSAFE); CHECK_ACTIVE_BOX(IS_ENABLED(FLIGHT_MODE(NAV_ALTHOLD_MODE)), BOXNAVALTHOLD); CHECK_ACTIVE_BOX(IS_ENABLED(FLIGHT_MODE(NAV_POSHOLD_MODE)), BOXNAVPOSHOLD); - CHECK_ACTIVE_BOX(IS_ENABLED(FLIGHT_MODE(NAV_CRUISE_MODE)), BOXNAVCRUISE); + CHECK_ACTIVE_BOX(IS_ENABLED(FLIGHT_MODE(NAV_COURSE_HOLD_MODE)), BOXNAVCOURSEHOLD); CHECK_ACTIVE_BOX(IS_ENABLED(FLIGHT_MODE(NAV_RTH_MODE)), BOXNAVRTH); CHECK_ACTIVE_BOX(IS_ENABLED(FLIGHT_MODE(NAV_WP_MODE)), BOXNAVWP); CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXAIRMODE)), BOXAIRMODE); diff --git a/src/main/fc/rc_modes.c b/src/main/fc/rc_modes.c index 1bd12915f2..785660e384 100755 --- a/src/main/fc/rc_modes.c +++ b/src/main/fc/rc_modes.c @@ -206,7 +206,7 @@ void updateUsedModeActivationConditionFlags(void) #ifdef USE_NAV isUsingNAVModes = isModeActivationConditionPresent(BOXNAVPOSHOLD) || isModeActivationConditionPresent(BOXNAVRTH) || - isModeActivationConditionPresent(BOXNAVCRUISE) || + isModeActivationConditionPresent(BOXNAVCOURSEHOLD) || isModeActivationConditionPresent(BOXNAVWP); #endif } diff --git a/src/main/fc/rc_modes.h b/src/main/fc/rc_modes.h index 6ac89225c7..6b3f877197 100755 --- a/src/main/fc/rc_modes.h +++ b/src/main/fc/rc_modes.h @@ -61,7 +61,7 @@ typedef enum { BOXOSDALT1 = 32, BOXOSDALT2 = 33, BOXOSDALT3 = 34, - BOXNAVCRUISE = 35, + BOXNAVCOURSEHOLD = 35, BOXBRAKING = 36, BOXUSER1 = 37, BOXUSER2 = 38, diff --git a/src/main/fc/runtime_config.c b/src/main/fc/runtime_config.c index b0d014d25e..47d6267cbd 100644 --- a/src/main/fc/runtime_config.c +++ b/src/main/fc/runtime_config.c @@ -154,7 +154,7 @@ flightModeForTelemetry_e getFlightModeForTelemetry(void) if (FLIGHT_MODE(NAV_POSHOLD_MODE)) return FLM_POSITION_HOLD; - if (FLIGHT_MODE(NAV_CRUISE_MODE)) + if (FLIGHT_MODE(NAV_COURSE_HOLD_MODE)) return FLM_CRUISE; if (FLIGHT_MODE(NAV_WP_MODE)) diff --git a/src/main/fc/runtime_config.h b/src/main/fc/runtime_config.h index 7f0bdd6e38..9f6ec9efa3 100644 --- a/src/main/fc/runtime_config.h +++ b/src/main/fc/runtime_config.h @@ -82,21 +82,21 @@ extern const char *armingDisableFlagNames[]; armingFlag_e isArmingDisabledReason(void); typedef enum { - ANGLE_MODE = (1 << 0), - HORIZON_MODE = (1 << 1), - HEADING_MODE = (1 << 2), - NAV_ALTHOLD_MODE= (1 << 3), // old BARO - NAV_RTH_MODE = (1 << 4), // old GPS_HOME - NAV_POSHOLD_MODE= (1 << 5), // old GPS_HOLD - HEADFREE_MODE = (1 << 6), - NAV_LAUNCH_MODE = (1 << 7), - MANUAL_MODE = (1 << 8), - FAILSAFE_MODE = (1 << 9), - AUTO_TUNE = (1 << 10), // old G-Tune - NAV_WP_MODE = (1 << 11), - NAV_CRUISE_MODE = (1 << 12), - FLAPERON = (1 << 13), - TURN_ASSISTANT = (1 << 14), + ANGLE_MODE = (1 << 0), + HORIZON_MODE = (1 << 1), + HEADING_MODE = (1 << 2), + NAV_ALTHOLD_MODE = (1 << 3), // old BARO + NAV_RTH_MODE = (1 << 4), // old GPS_HOME + NAV_POSHOLD_MODE = (1 << 5), // old GPS_HOLD + HEADFREE_MODE = (1 << 6), + NAV_LAUNCH_MODE = (1 << 7), + MANUAL_MODE = (1 << 8), + FAILSAFE_MODE = (1 << 9), + AUTO_TUNE = (1 << 10), // old G-Tune + NAV_WP_MODE = (1 << 11), + NAV_COURSE_HOLD_MODE = (1 << 12), + FLAPERON = (1 << 13), + TURN_ASSISTANT = (1 << 14), FLIP_OVER_AFTER_CRASH = (1 << 15), } flightModeFlags_e; diff --git a/src/main/io/osd.c b/src/main/io/osd.c index 1b63cababd..6861f8a675 100644 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -1487,18 +1487,18 @@ static bool osdDrawSingleElement(uint8_t item) break; } - case OSD_CRUISE_HEADING_ERROR: + case OSD_COURSE_HOLD_ERROR: { - if (ARMING_FLAG(ARMED) && !FLIGHT_MODE(NAV_CRUISE_MODE)) { + if (ARMING_FLAG(ARMED) && !FLIGHT_MODE(NAV_COURSE_HOLD_MODE)) { displayWrite(osdDisplayPort, elemPosX, elemPosY, " "); return true; } buff[0] = SYM_HEADING; - if ((!ARMING_FLAG(ARMED)) || (FLIGHT_MODE(NAV_CRUISE_MODE) && isAdjustingPosition())) { + if ((!ARMING_FLAG(ARMED)) || (FLIGHT_MODE(NAV_COURSE_HOLD_MODE) && isAdjustingPosition())) { buff[1] = buff[2] = buff[3] = '-'; - } else if (FLIGHT_MODE(NAV_CRUISE_MODE)) { + } else if (FLIGHT_MODE(NAV_COURSE_HOLD_MODE)) { int16_t herr = lrintf(CENTIDEGREES_TO_DEGREES((float)navigationGetHeadingError())); if (ABS(herr) > 99) strcpy(buff + 1, ">99"); @@ -1511,11 +1511,11 @@ static bool osdDrawSingleElement(uint8_t item) break; } - case OSD_CRUISE_HEADING_ADJUSTMENT: + case OSD_COURSE_HOLD_ADJUSTMENT: { int16_t heading_adjust = lrintf(CENTIDEGREES_TO_DEGREES((float)getCruiseHeadingAdjustment())); - if (ARMING_FLAG(ARMED) && ((!FLIGHT_MODE(NAV_CRUISE_MODE)) || !(isAdjustingPosition() || isAdjustingHeading() || (heading_adjust != 0)))) { + if (ARMING_FLAG(ARMED) && ((!FLIGHT_MODE(NAV_COURSE_HOLD_MODE)) || !(isAdjustingPosition() || isAdjustingHeading() || (heading_adjust != 0)))) { displayWrite(osdDisplayPort, elemPosX, elemPosY, " "); return true; } @@ -1524,7 +1524,7 @@ static bool osdDrawSingleElement(uint8_t item) if (!ARMING_FLAG(ARMED)) { buff[1] = buff[2] = buff[3] = buff[4] = '-'; - } else if (FLIGHT_MODE(NAV_CRUISE_MODE)) { + } else if (FLIGHT_MODE(NAV_COURSE_HOLD_MODE)) { tfp_sprintf(buff + 1, "%4d", heading_adjust); } @@ -1675,10 +1675,10 @@ static bool osdDrawSingleElement(uint8_t item) p = "RTH "; else if (FLIGHT_MODE(NAV_POSHOLD_MODE)) p = "HOLD"; - else if (FLIGHT_MODE(NAV_CRUISE_MODE) && FLIGHT_MODE(NAV_ALTHOLD_MODE)) - p = "3CRS"; - else if (FLIGHT_MODE(NAV_CRUISE_MODE)) - p = "CRS "; + else if (FLIGHT_MODE(NAV_COURSE_HOLD_MODE) && FLIGHT_MODE(NAV_ALTHOLD_MODE)) + p = "CRUZ"; + else if (FLIGHT_MODE(NAV_COURSE_HOLD_MODE)) + p = "CRSH"; else if (FLIGHT_MODE(NAV_WP_MODE)) p = " WP "; else if (FLIGHT_MODE(NAV_ALTHOLD_MODE) && navigationRequiresAngleMode()) { @@ -2074,7 +2074,7 @@ static bool osdDrawSingleElement(uint8_t item) return true; case OSD_NAV_FW_CRUISE_THR: - osdDisplayAdjustableDecimalValue(elemPosX, elemPosY, "CRS", 0, navConfig()->fw.cruise_throttle, 4, 0, ADJUSTMENT_NAV_FW_CRUISE_THR); + osdDisplayAdjustableDecimalValue(elemPosX, elemPosY, "CRZ", 0, navConfig()->fw.cruise_throttle, 4, 0, ADJUSTMENT_NAV_FW_CRUISE_THR); return true; case OSD_NAV_FW_PITCH2THR: @@ -2744,8 +2744,8 @@ void pgResetFn_osdLayoutsConfig(osdLayoutsConfig_t *osdLayoutsConfig) osdLayoutsConfig->item_pos[0][OSD_THROTTLE_POS] = OSD_POS(1, 2) | OSD_VISIBLE_FLAG; osdLayoutsConfig->item_pos[0][OSD_THROTTLE_POS_AUTO_THR] = OSD_POS(6, 2); osdLayoutsConfig->item_pos[0][OSD_HEADING] = OSD_POS(12, 2); - osdLayoutsConfig->item_pos[0][OSD_CRUISE_HEADING_ERROR] = OSD_POS(12, 2); - osdLayoutsConfig->item_pos[0][OSD_CRUISE_HEADING_ADJUSTMENT] = OSD_POS(12, 2); + osdLayoutsConfig->item_pos[0][OSD_COURSE_HOLD_ERROR] = OSD_POS(12, 2); + osdLayoutsConfig->item_pos[0][OSD_COURSE_HOLD_ADJUSTMENT] = OSD_POS(12, 2); osdLayoutsConfig->item_pos[0][OSD_HEADING_GRAPH] = OSD_POS(18, 2); osdLayoutsConfig->item_pos[0][OSD_CURRENT_DRAW] = OSD_POS(2, 3) | OSD_VISIBLE_FLAG; osdLayoutsConfig->item_pos[0][OSD_MAH_DRAWN] = OSD_POS(1, 4) | OSD_VISIBLE_FLAG; diff --git a/src/main/io/osd.h b/src/main/io/osd.h index 9fc2cfb539..fe35e28b71 100755 --- a/src/main/io/osd.h +++ b/src/main/io/osd.h @@ -151,8 +151,8 @@ typedef enum { OSD_REMAINING_FLIGHT_TIME_BEFORE_RTH, OSD_REMAINING_DISTANCE_BEFORE_RTH, OSD_HOME_HEADING_ERROR, - OSD_CRUISE_HEADING_ERROR, - OSD_CRUISE_HEADING_ADJUSTMENT, + OSD_COURSE_HOLD_ERROR, + OSD_COURSE_HOLD_ADJUSTMENT, OSD_SAG_COMPENSATED_MAIN_BATT_VOLTAGE, OSD_MAIN_BATT_SAG_COMPENSATED_CELL_VOLTAGE, OSD_POWER_SUPPLY_IMPEDANCE, diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c index 55f1c7fb29..9cc98a7260 100755 --- a/src/main/navigation/navigation.c +++ b/src/main/navigation/navigation.c @@ -236,12 +236,12 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_ALTHOLD_INITIALIZE(navi static navigationFSMEvent_t navOnEnteringState_NAV_STATE_ALTHOLD_IN_PROGRESS(navigationFSMState_t previousState); static navigationFSMEvent_t navOnEnteringState_NAV_STATE_POSHOLD_3D_INITIALIZE(navigationFSMState_t previousState); static navigationFSMEvent_t navOnEnteringState_NAV_STATE_POSHOLD_3D_IN_PROGRESS(navigationFSMState_t previousState); -static navigationFSMEvent_t navOnEnteringState_NAV_STATE_CRUISE_2D_INITIALIZE(navigationFSMState_t previousState); -static navigationFSMEvent_t navOnEnteringState_NAV_STATE_CRUISE_2D_IN_PROGRESS(navigationFSMState_t previousState); -static navigationFSMEvent_t navOnEnteringState_NAV_STATE_CRUISE_2D_ADJUSTING(navigationFSMState_t previousState); -static navigationFSMEvent_t navOnEnteringState_NAV_STATE_CRUISE_3D_INITIALIZE(navigationFSMState_t previousState); -static navigationFSMEvent_t navOnEnteringState_NAV_STATE_CRUISE_3D_IN_PROGRESS(navigationFSMState_t previousState); -static navigationFSMEvent_t navOnEnteringState_NAV_STATE_CRUISE_3D_ADJUSTING(navigationFSMState_t previousState); +static navigationFSMEvent_t navOnEnteringState_NAV_STATE_COURSE_HOLD_INITIALIZE(navigationFSMState_t previousState); +static navigationFSMEvent_t navOnEnteringState_NAV_STATE_COURSE_HOLD_IN_PROGRESS(navigationFSMState_t previousState); +static navigationFSMEvent_t navOnEnteringState_NAV_STATE_COURSE_HOLD_ADJUSTING(navigationFSMState_t previousState); +static navigationFSMEvent_t navOnEnteringState_NAV_STATE_CRUISE_INITIALIZE(navigationFSMState_t previousState); +static navigationFSMEvent_t navOnEnteringState_NAV_STATE_CRUISE_IN_PROGRESS(navigationFSMState_t previousState); +static navigationFSMEvent_t navOnEnteringState_NAV_STATE_CRUISE_ADJUSTING(navigationFSMState_t previousState); static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_INITIALIZE(navigationFSMState_t previousState); static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_CLIMB_TO_SAFE_ALT(navigationFSMState_t previousState); static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_HEAD_HOME(navigationFSMState_t previousState); @@ -276,15 +276,15 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = { .mapToFlightModes = 0, .mwState = MW_NAV_STATE_NONE, .mwError = MW_NAV_ERROR_NONE, - .onEvent = { - [NAV_FSM_EVENT_SWITCH_TO_ALTHOLD] = NAV_STATE_ALTHOLD_INITIALIZE, - [NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D] = NAV_STATE_POSHOLD_3D_INITIALIZE, - [NAV_FSM_EVENT_SWITCH_TO_RTH] = NAV_STATE_RTH_INITIALIZE, - [NAV_FSM_EVENT_SWITCH_TO_WAYPOINT] = NAV_STATE_WAYPOINT_INITIALIZE, - [NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE, - [NAV_FSM_EVENT_SWITCH_TO_LAUNCH] = NAV_STATE_LAUNCH_INITIALIZE, - [NAV_FSM_EVENT_SWITCH_TO_CRUISE_2D] = NAV_STATE_CRUISE_2D_INITIALIZE, - [NAV_FSM_EVENT_SWITCH_TO_CRUISE_3D] = NAV_STATE_CRUISE_3D_INITIALIZE, + .onEvent = { + [NAV_FSM_EVENT_SWITCH_TO_ALTHOLD] = NAV_STATE_ALTHOLD_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D] = NAV_STATE_POSHOLD_3D_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_RTH] = NAV_STATE_RTH_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_WAYPOINT] = NAV_STATE_WAYPOINT_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_LAUNCH] = NAV_STATE_LAUNCH_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_COURSE_HOLD] = NAV_STATE_COURSE_HOLD_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_CRUISE] = NAV_STATE_CRUISE_INITIALIZE, } }, @@ -298,9 +298,9 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = { .mwState = MW_NAV_STATE_NONE, .mwError = MW_NAV_ERROR_NONE, .onEvent = { - [NAV_FSM_EVENT_SUCCESS] = NAV_STATE_ALTHOLD_IN_PROGRESS, - [NAV_FSM_EVENT_ERROR] = NAV_STATE_IDLE, - [NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE, + [NAV_FSM_EVENT_SUCCESS] = NAV_STATE_ALTHOLD_IN_PROGRESS, + [NAV_FSM_EVENT_ERROR] = NAV_STATE_IDLE, + [NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE, } }, @@ -313,14 +313,14 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = { .mwState = MW_NAV_STATE_NONE, .mwError = MW_NAV_ERROR_NONE, .onEvent = { - [NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_ALTHOLD_IN_PROGRESS, // re-process the state - [NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE, - [NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D] = NAV_STATE_POSHOLD_3D_INITIALIZE, - [NAV_FSM_EVENT_SWITCH_TO_RTH] = NAV_STATE_RTH_INITIALIZE, - [NAV_FSM_EVENT_SWITCH_TO_WAYPOINT] = NAV_STATE_WAYPOINT_INITIALIZE, - [NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE, - [NAV_FSM_EVENT_SWITCH_TO_CRUISE_2D] = NAV_STATE_CRUISE_2D_INITIALIZE, - [NAV_FSM_EVENT_SWITCH_TO_CRUISE_3D] = NAV_STATE_CRUISE_3D_INITIALIZE, + [NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_ALTHOLD_IN_PROGRESS, // re-process the state + [NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE, + [NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D] = NAV_STATE_POSHOLD_3D_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_RTH] = NAV_STATE_RTH_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_WAYPOINT] = NAV_STATE_WAYPOINT_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_COURSE_HOLD] = NAV_STATE_COURSE_HOLD_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_CRUISE] = NAV_STATE_CRUISE_INITIALIZE, } }, @@ -334,9 +334,9 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = { .mwState = MW_NAV_STATE_HOLD_INFINIT, .mwError = MW_NAV_ERROR_NONE, .onEvent = { - [NAV_FSM_EVENT_SUCCESS] = NAV_STATE_POSHOLD_3D_IN_PROGRESS, - [NAV_FSM_EVENT_ERROR] = NAV_STATE_IDLE, - [NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE, + [NAV_FSM_EVENT_SUCCESS] = NAV_STATE_POSHOLD_3D_IN_PROGRESS, + [NAV_FSM_EVENT_ERROR] = NAV_STATE_IDLE, + [NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE, } }, @@ -349,131 +349,131 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = { .mwState = MW_NAV_STATE_HOLD_INFINIT, .mwError = MW_NAV_ERROR_NONE, .onEvent = { - [NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_POSHOLD_3D_IN_PROGRESS, // re-process the state - [NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE, - [NAV_FSM_EVENT_SWITCH_TO_ALTHOLD] = NAV_STATE_ALTHOLD_INITIALIZE, - [NAV_FSM_EVENT_SWITCH_TO_RTH] = NAV_STATE_RTH_INITIALIZE, - [NAV_FSM_EVENT_SWITCH_TO_WAYPOINT] = NAV_STATE_WAYPOINT_INITIALIZE, - [NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE, - [NAV_FSM_EVENT_SWITCH_TO_CRUISE_2D] = NAV_STATE_CRUISE_2D_INITIALIZE, - [NAV_FSM_EVENT_SWITCH_TO_CRUISE_3D] = NAV_STATE_CRUISE_3D_INITIALIZE, + [NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_POSHOLD_3D_IN_PROGRESS, // re-process the state + [NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE, + [NAV_FSM_EVENT_SWITCH_TO_ALTHOLD] = NAV_STATE_ALTHOLD_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_RTH] = NAV_STATE_RTH_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_WAYPOINT] = NAV_STATE_WAYPOINT_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_COURSE_HOLD] = NAV_STATE_COURSE_HOLD_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_CRUISE] = NAV_STATE_CRUISE_INITIALIZE, } }, - /** CRUISE_2D mode ************************************************/ - [NAV_STATE_CRUISE_2D_INITIALIZE] = { - .persistentId = NAV_PERSISTENT_ID_CRUISE_2D_INITIALIZE, - .onEntry = navOnEnteringState_NAV_STATE_CRUISE_2D_INITIALIZE, + /** CRUISE_HOLD mode ************************************************/ + [NAV_STATE_COURSE_HOLD_INITIALIZE] = { + .persistentId = NAV_PERSISTENT_ID_COURSE_HOLD_INITIALIZE, + .onEntry = navOnEnteringState_NAV_STATE_COURSE_HOLD_INITIALIZE, .timeoutMs = 0, .stateFlags = NAV_REQUIRE_ANGLE, - .mapToFlightModes = NAV_CRUISE_MODE, + .mapToFlightModes = NAV_COURSE_HOLD_MODE, .mwState = MW_NAV_STATE_NONE, .mwError = MW_NAV_ERROR_NONE, .onEvent = { - [NAV_FSM_EVENT_SUCCESS] = NAV_STATE_CRUISE_2D_IN_PROGRESS, - [NAV_FSM_EVENT_ERROR] = NAV_STATE_IDLE, - [NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE, + [NAV_FSM_EVENT_SUCCESS] = NAV_STATE_COURSE_HOLD_IN_PROGRESS, + [NAV_FSM_EVENT_ERROR] = NAV_STATE_IDLE, + [NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE, } }, - [NAV_STATE_CRUISE_2D_IN_PROGRESS] = { - .persistentId = NAV_PERSISTENT_ID_CRUISE_2D_IN_PROGRESS, - .onEntry = navOnEnteringState_NAV_STATE_CRUISE_2D_IN_PROGRESS, + [NAV_STATE_COURSE_HOLD_IN_PROGRESS] = { + .persistentId = NAV_PERSISTENT_ID_COURSE_HOLD_IN_PROGRESS, + .onEntry = navOnEnteringState_NAV_STATE_COURSE_HOLD_IN_PROGRESS, .timeoutMs = 10, .stateFlags = NAV_CTL_POS | NAV_CTL_YAW | NAV_REQUIRE_ANGLE | NAV_RC_POS | NAV_RC_YAW, - .mapToFlightModes = NAV_CRUISE_MODE, + .mapToFlightModes = NAV_COURSE_HOLD_MODE, .mwState = MW_NAV_STATE_NONE, .mwError = MW_NAV_ERROR_NONE, .onEvent = { - [NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_CRUISE_2D_IN_PROGRESS, // re-process the state - [NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE, - [NAV_FSM_EVENT_SWITCH_TO_ALTHOLD] = NAV_STATE_ALTHOLD_INITIALIZE, - [NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D] = NAV_STATE_POSHOLD_3D_INITIALIZE, - [NAV_FSM_EVENT_SWITCH_TO_CRUISE_3D] = NAV_STATE_CRUISE_3D_INITIALIZE, - [NAV_FSM_EVENT_SWITCH_TO_CRUISE_ADJ] = NAV_STATE_CRUISE_2D_ADJUSTING, - [NAV_FSM_EVENT_SWITCH_TO_RTH] = NAV_STATE_RTH_INITIALIZE, - [NAV_FSM_EVENT_SWITCH_TO_WAYPOINT] = NAV_STATE_WAYPOINT_INITIALIZE, - [NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE, + [NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_COURSE_HOLD_IN_PROGRESS, // re-process the state + [NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE, + [NAV_FSM_EVENT_SWITCH_TO_ALTHOLD] = NAV_STATE_ALTHOLD_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D] = NAV_STATE_POSHOLD_3D_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_CRUISE] = NAV_STATE_CRUISE_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_COURSE_ADJ] = NAV_STATE_COURSE_HOLD_ADJUSTING, + [NAV_FSM_EVENT_SWITCH_TO_RTH] = NAV_STATE_RTH_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_WAYPOINT] = NAV_STATE_WAYPOINT_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE, } }, - [NAV_STATE_CRUISE_2D_ADJUSTING] = { - .persistentId = NAV_PERSISTENT_ID_CRUISE_2D_ADJUSTING, - .onEntry = navOnEnteringState_NAV_STATE_CRUISE_2D_ADJUSTING, + [NAV_STATE_COURSE_HOLD_ADJUSTING] = { + .persistentId = NAV_PERSISTENT_ID_COURSE_HOLD_ADJUSTING, + .onEntry = navOnEnteringState_NAV_STATE_COURSE_HOLD_ADJUSTING, .timeoutMs = 10, .stateFlags = NAV_REQUIRE_ANGLE | NAV_RC_POS, - .mapToFlightModes = NAV_CRUISE_MODE, + .mapToFlightModes = NAV_COURSE_HOLD_MODE, .mwState = MW_NAV_STATE_NONE, .mwError = MW_NAV_ERROR_NONE, .onEvent = { - [NAV_FSM_EVENT_SUCCESS] = NAV_STATE_CRUISE_2D_IN_PROGRESS, - [NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_CRUISE_2D_ADJUSTING, - [NAV_FSM_EVENT_ERROR] = NAV_STATE_IDLE, - [NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE, - [NAV_FSM_EVENT_SWITCH_TO_ALTHOLD] = NAV_STATE_ALTHOLD_INITIALIZE, - [NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D] = NAV_STATE_POSHOLD_3D_INITIALIZE, - [NAV_FSM_EVENT_SWITCH_TO_CRUISE_3D] = NAV_STATE_CRUISE_3D_INITIALIZE, - [NAV_FSM_EVENT_SWITCH_TO_RTH] = NAV_STATE_RTH_INITIALIZE, - [NAV_FSM_EVENT_SWITCH_TO_WAYPOINT] = NAV_STATE_WAYPOINT_INITIALIZE, - [NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE, + [NAV_FSM_EVENT_SUCCESS] = NAV_STATE_COURSE_HOLD_IN_PROGRESS, + [NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_COURSE_HOLD_ADJUSTING, + [NAV_FSM_EVENT_ERROR] = NAV_STATE_IDLE, + [NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE, + [NAV_FSM_EVENT_SWITCH_TO_ALTHOLD] = NAV_STATE_ALTHOLD_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D] = NAV_STATE_POSHOLD_3D_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_CRUISE] = NAV_STATE_CRUISE_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_RTH] = NAV_STATE_RTH_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_WAYPOINT] = NAV_STATE_WAYPOINT_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE, } }, /** CRUISE_3D mode ************************************************/ - [NAV_STATE_CRUISE_3D_INITIALIZE] = { - .persistentId = NAV_PERSISTENT_ID_CRUISE_3D_INITIALIZE, - .onEntry = navOnEnteringState_NAV_STATE_CRUISE_3D_INITIALIZE, + [NAV_STATE_CRUISE_INITIALIZE] = { + .persistentId = NAV_PERSISTENT_ID_CRUISE_INITIALIZE, + .onEntry = navOnEnteringState_NAV_STATE_CRUISE_INITIALIZE, .timeoutMs = 0, .stateFlags = NAV_REQUIRE_ANGLE, - .mapToFlightModes = NAV_ALTHOLD_MODE | NAV_CRUISE_MODE, + .mapToFlightModes = NAV_ALTHOLD_MODE | NAV_COURSE_HOLD_MODE, .mwState = MW_NAV_STATE_NONE, .mwError = MW_NAV_ERROR_NONE, .onEvent = { - [NAV_FSM_EVENT_SUCCESS] = NAV_STATE_CRUISE_3D_IN_PROGRESS, - [NAV_FSM_EVENT_ERROR] = NAV_STATE_IDLE, - [NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE, + [NAV_FSM_EVENT_SUCCESS] = NAV_STATE_CRUISE_IN_PROGRESS, + [NAV_FSM_EVENT_ERROR] = NAV_STATE_IDLE, + [NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE, } }, - [NAV_STATE_CRUISE_3D_IN_PROGRESS] = { - .persistentId = NAV_PERSISTENT_ID_CRUISE_3D_IN_PROGRESS, - .onEntry = navOnEnteringState_NAV_STATE_CRUISE_3D_IN_PROGRESS, + [NAV_STATE_CRUISE_IN_PROGRESS] = { + .persistentId = NAV_PERSISTENT_ID_CRUISE_IN_PROGRESS, + .onEntry = navOnEnteringState_NAV_STATE_CRUISE_IN_PROGRESS, .timeoutMs = 10, .stateFlags = NAV_CTL_POS | NAV_CTL_YAW | NAV_CTL_ALT | NAV_REQUIRE_ANGLE | NAV_RC_POS | NAV_RC_YAW | NAV_RC_ALT, - .mapToFlightModes = NAV_ALTHOLD_MODE | NAV_CRUISE_MODE, + .mapToFlightModes = NAV_ALTHOLD_MODE | NAV_COURSE_HOLD_MODE, .mwState = MW_NAV_STATE_NONE, .mwError = MW_NAV_ERROR_NONE, .onEvent = { - [NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_CRUISE_3D_IN_PROGRESS, // re-process the state - [NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE, - [NAV_FSM_EVENT_SWITCH_TO_ALTHOLD] = NAV_STATE_ALTHOLD_INITIALIZE, - [NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D] = NAV_STATE_POSHOLD_3D_INITIALIZE, - [NAV_FSM_EVENT_SWITCH_TO_CRUISE_2D] = NAV_STATE_CRUISE_2D_INITIALIZE, - [NAV_FSM_EVENT_SWITCH_TO_CRUISE_ADJ] = NAV_STATE_CRUISE_3D_ADJUSTING, - [NAV_FSM_EVENT_SWITCH_TO_RTH] = NAV_STATE_RTH_INITIALIZE, - [NAV_FSM_EVENT_SWITCH_TO_WAYPOINT] = NAV_STATE_WAYPOINT_INITIALIZE, - [NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE, + [NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_CRUISE_IN_PROGRESS, // re-process the state + [NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE, + [NAV_FSM_EVENT_SWITCH_TO_ALTHOLD] = NAV_STATE_ALTHOLD_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D] = NAV_STATE_POSHOLD_3D_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_COURSE_HOLD] = NAV_STATE_COURSE_HOLD_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_COURSE_ADJ] = NAV_STATE_CRUISE_ADJUSTING, + [NAV_FSM_EVENT_SWITCH_TO_RTH] = NAV_STATE_RTH_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_WAYPOINT] = NAV_STATE_WAYPOINT_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE, } }, - [NAV_STATE_CRUISE_3D_ADJUSTING] = { - .persistentId = NAV_PERSISTENT_ID_CRUISE_3D_ADJUSTING, - .onEntry = navOnEnteringState_NAV_STATE_CRUISE_3D_ADJUSTING, + [NAV_STATE_CRUISE_ADJUSTING] = { + .persistentId = NAV_PERSISTENT_ID_CRUISE_ADJUSTING, + .onEntry = navOnEnteringState_NAV_STATE_CRUISE_ADJUSTING, .timeoutMs = 10, .stateFlags = NAV_CTL_ALT | NAV_REQUIRE_ANGLE | NAV_RC_POS | NAV_RC_ALT, - .mapToFlightModes = NAV_ALTHOLD_MODE | NAV_CRUISE_MODE, + .mapToFlightModes = NAV_ALTHOLD_MODE | NAV_COURSE_HOLD_MODE, .mwState = MW_NAV_STATE_NONE, .mwError = MW_NAV_ERROR_NONE, .onEvent = { - [NAV_FSM_EVENT_SUCCESS] = NAV_STATE_CRUISE_3D_IN_PROGRESS, - [NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_CRUISE_3D_ADJUSTING, - [NAV_FSM_EVENT_ERROR] = NAV_STATE_IDLE, - [NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE, - [NAV_FSM_EVENT_SWITCH_TO_ALTHOLD] = NAV_STATE_ALTHOLD_INITIALIZE, - [NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D] = NAV_STATE_POSHOLD_3D_INITIALIZE, - [NAV_FSM_EVENT_SWITCH_TO_CRUISE_2D] = NAV_STATE_CRUISE_2D_INITIALIZE, - [NAV_FSM_EVENT_SWITCH_TO_RTH] = NAV_STATE_RTH_INITIALIZE, - [NAV_FSM_EVENT_SWITCH_TO_WAYPOINT] = NAV_STATE_WAYPOINT_INITIALIZE, - [NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE, + [NAV_FSM_EVENT_SUCCESS] = NAV_STATE_CRUISE_IN_PROGRESS, + [NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_CRUISE_ADJUSTING, + [NAV_FSM_EVENT_ERROR] = NAV_STATE_IDLE, + [NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE, + [NAV_FSM_EVENT_SWITCH_TO_ALTHOLD] = NAV_STATE_ALTHOLD_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D] = NAV_STATE_POSHOLD_3D_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_COURSE_HOLD] = NAV_STATE_COURSE_HOLD_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_RTH] = NAV_STATE_RTH_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_WAYPOINT] = NAV_STATE_WAYPOINT_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE, } }, @@ -487,11 +487,11 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = { .mwState = MW_NAV_STATE_RTH_START, .mwError = MW_NAV_ERROR_NONE, .onEvent = { - [NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_RTH_INITIALIZE, // re-process the state - [NAV_FSM_EVENT_SUCCESS] = NAV_STATE_RTH_CLIMB_TO_SAFE_ALT, - [NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE, - [NAV_FSM_EVENT_SWITCH_TO_RTH_LANDING] = NAV_STATE_RTH_HOVER_PRIOR_TO_LANDING, - [NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE, + [NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_RTH_INITIALIZE, // re-process the state + [NAV_FSM_EVENT_SUCCESS] = NAV_STATE_RTH_CLIMB_TO_SAFE_ALT, + [NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_RTH_LANDING] = NAV_STATE_RTH_HOVER_PRIOR_TO_LANDING, + [NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE, } }, @@ -504,14 +504,14 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = { .mwState = MW_NAV_STATE_RTH_CLIMB, .mwError = MW_NAV_ERROR_WAIT_FOR_RTH_ALT, .onEvent = { - [NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_RTH_CLIMB_TO_SAFE_ALT, // re-process the state - [NAV_FSM_EVENT_SUCCESS] = NAV_STATE_RTH_HEAD_HOME, - [NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE, - [NAV_FSM_EVENT_SWITCH_TO_ALTHOLD] = NAV_STATE_ALTHOLD_INITIALIZE, - [NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D] = NAV_STATE_POSHOLD_3D_INITIALIZE, - [NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE, - [NAV_FSM_EVENT_SWITCH_TO_CRUISE_2D] = NAV_STATE_CRUISE_2D_INITIALIZE, - [NAV_FSM_EVENT_SWITCH_TO_CRUISE_3D] = NAV_STATE_CRUISE_3D_INITIALIZE, + [NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_RTH_CLIMB_TO_SAFE_ALT, // re-process the state + [NAV_FSM_EVENT_SUCCESS] = NAV_STATE_RTH_HEAD_HOME, + [NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE, + [NAV_FSM_EVENT_SWITCH_TO_ALTHOLD] = NAV_STATE_ALTHOLD_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D] = NAV_STATE_POSHOLD_3D_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_COURSE_HOLD] = NAV_STATE_COURSE_HOLD_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_CRUISE] = NAV_STATE_CRUISE_INITIALIZE, } }, @@ -524,14 +524,14 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = { .mwState = MW_NAV_STATE_RTH_ENROUTE, .mwError = MW_NAV_ERROR_NONE, .onEvent = { - [NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_RTH_HEAD_HOME, // re-process the state - [NAV_FSM_EVENT_SUCCESS] = NAV_STATE_RTH_HOVER_PRIOR_TO_LANDING, - [NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE, - [NAV_FSM_EVENT_SWITCH_TO_ALTHOLD] = NAV_STATE_ALTHOLD_INITIALIZE, - [NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D] = NAV_STATE_POSHOLD_3D_INITIALIZE, - [NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE, - [NAV_FSM_EVENT_SWITCH_TO_CRUISE_2D] = NAV_STATE_CRUISE_2D_INITIALIZE, - [NAV_FSM_EVENT_SWITCH_TO_CRUISE_3D] = NAV_STATE_CRUISE_3D_INITIALIZE, + [NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_RTH_HEAD_HOME, // re-process the state + [NAV_FSM_EVENT_SUCCESS] = NAV_STATE_RTH_HOVER_PRIOR_TO_LANDING, + [NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE, + [NAV_FSM_EVENT_SWITCH_TO_ALTHOLD] = NAV_STATE_ALTHOLD_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D] = NAV_STATE_POSHOLD_3D_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_COURSE_HOLD] = NAV_STATE_COURSE_HOLD_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_CRUISE] = NAV_STATE_CRUISE_INITIALIZE, } }, @@ -544,15 +544,15 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = { .mwState = MW_NAV_STATE_LAND_SETTLE, .mwError = MW_NAV_ERROR_NONE, .onEvent = { - [NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_RTH_HOVER_PRIOR_TO_LANDING, - [NAV_FSM_EVENT_SUCCESS] = NAV_STATE_RTH_LANDING, + [NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_RTH_HOVER_PRIOR_TO_LANDING, + [NAV_FSM_EVENT_SUCCESS] = NAV_STATE_RTH_LANDING, [NAV_FSM_EVENT_SWITCH_TO_RTH_HOVER_ABOVE_HOME] = NAV_STATE_RTH_HOVER_ABOVE_HOME, - [NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE, - [NAV_FSM_EVENT_SWITCH_TO_ALTHOLD] = NAV_STATE_ALTHOLD_INITIALIZE, - [NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D] = NAV_STATE_POSHOLD_3D_INITIALIZE, - [NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE, - [NAV_FSM_EVENT_SWITCH_TO_CRUISE_2D] = NAV_STATE_CRUISE_2D_INITIALIZE, - [NAV_FSM_EVENT_SWITCH_TO_CRUISE_3D] = NAV_STATE_CRUISE_3D_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE, + [NAV_FSM_EVENT_SWITCH_TO_ALTHOLD] = NAV_STATE_ALTHOLD_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D] = NAV_STATE_POSHOLD_3D_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_COURSE_HOLD] = NAV_STATE_COURSE_HOLD_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_CRUISE] = NAV_STATE_CRUISE_INITIALIZE, } }, @@ -565,13 +565,13 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = { .mwState = MW_NAV_STATE_HOVER_ABOVE_HOME, .mwError = MW_NAV_ERROR_NONE, .onEvent = { - [NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_RTH_HOVER_ABOVE_HOME, - [NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE, - [NAV_FSM_EVENT_SWITCH_TO_ALTHOLD] = NAV_STATE_ALTHOLD_INITIALIZE, - [NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D] = NAV_STATE_POSHOLD_3D_INITIALIZE, - [NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE, - [NAV_FSM_EVENT_SWITCH_TO_CRUISE_2D] = NAV_STATE_CRUISE_2D_INITIALIZE, - [NAV_FSM_EVENT_SWITCH_TO_CRUISE_3D] = NAV_STATE_CRUISE_3D_INITIALIZE, + [NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_RTH_HOVER_ABOVE_HOME, + [NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE, + [NAV_FSM_EVENT_SWITCH_TO_ALTHOLD] = NAV_STATE_ALTHOLD_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D] = NAV_STATE_POSHOLD_3D_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_COURSE_HOLD] = NAV_STATE_COURSE_HOLD_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_CRUISE] = NAV_STATE_CRUISE_INITIALIZE, } }, @@ -584,12 +584,12 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = { .mwState = MW_NAV_STATE_LAND_IN_PROGRESS, .mwError = MW_NAV_ERROR_LANDING, .onEvent = { - [NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_RTH_LANDING, // re-process state - [NAV_FSM_EVENT_SUCCESS] = NAV_STATE_RTH_FINISHING, - [NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE, - [NAV_FSM_EVENT_SWITCH_TO_ALTHOLD] = NAV_STATE_ALTHOLD_INITIALIZE, - [NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D] = NAV_STATE_POSHOLD_3D_INITIALIZE, - [NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE, + [NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_RTH_LANDING, // re-process state + [NAV_FSM_EVENT_SUCCESS] = NAV_STATE_RTH_FINISHING, + [NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE, + [NAV_FSM_EVENT_SWITCH_TO_ALTHOLD] = NAV_STATE_ALTHOLD_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D] = NAV_STATE_POSHOLD_3D_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE, } }, @@ -602,8 +602,8 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = { .mwState = MW_NAV_STATE_LAND_IN_PROGRESS, .mwError = MW_NAV_ERROR_LANDING, .onEvent = { - [NAV_FSM_EVENT_SUCCESS] = NAV_STATE_RTH_FINISHED, - [NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE, + [NAV_FSM_EVENT_SUCCESS] = NAV_STATE_RTH_FINISHED, + [NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE, } }, @@ -616,11 +616,11 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = { .mwState = MW_NAV_STATE_LANDED, .mwError = MW_NAV_ERROR_NONE, .onEvent = { - [NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_RTH_FINISHED, // re-process state - [NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE, - [NAV_FSM_EVENT_SWITCH_TO_ALTHOLD] = NAV_STATE_ALTHOLD_INITIALIZE, - [NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D] = NAV_STATE_POSHOLD_3D_INITIALIZE, - [NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE, + [NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_RTH_FINISHED, // re-process state + [NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE, + [NAV_FSM_EVENT_SWITCH_TO_ALTHOLD] = NAV_STATE_ALTHOLD_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D] = NAV_STATE_POSHOLD_3D_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE, } }, @@ -634,10 +634,10 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = { .mwState = MW_NAV_STATE_PROCESS_NEXT, .mwError = MW_NAV_ERROR_NONE, .onEvent = { - [NAV_FSM_EVENT_SUCCESS] = NAV_STATE_WAYPOINT_PRE_ACTION, - [NAV_FSM_EVENT_ERROR] = NAV_STATE_IDLE, - [NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE, - [NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_FINISHED] = NAV_STATE_WAYPOINT_FINISHED, + [NAV_FSM_EVENT_SUCCESS] = NAV_STATE_WAYPOINT_PRE_ACTION, + [NAV_FSM_EVENT_ERROR] = NAV_STATE_IDLE, + [NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE, + [NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_FINISHED] = NAV_STATE_WAYPOINT_FINISHED, } }, @@ -650,11 +650,11 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = { .mwState = MW_NAV_STATE_PROCESS_NEXT, .mwError = MW_NAV_ERROR_NONE, .onEvent = { - [NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_WAYPOINT_PRE_ACTION, // re-process the state (for JUMP) - [NAV_FSM_EVENT_SUCCESS] = NAV_STATE_WAYPOINT_IN_PROGRESS, - [NAV_FSM_EVENT_ERROR] = NAV_STATE_IDLE, - [NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE, - [NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_FINISHED] = NAV_STATE_WAYPOINT_FINISHED, + [NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_WAYPOINT_PRE_ACTION, // re-process the state (for JUMP) + [NAV_FSM_EVENT_SUCCESS] = NAV_STATE_WAYPOINT_IN_PROGRESS, + [NAV_FSM_EVENT_ERROR] = NAV_STATE_IDLE, + [NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE, + [NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_FINISHED] = NAV_STATE_WAYPOINT_FINISHED, } }, @@ -667,15 +667,15 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = { .mwState = MW_NAV_STATE_WP_ENROUTE, .mwError = MW_NAV_ERROR_NONE, .onEvent = { - [NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_WAYPOINT_IN_PROGRESS, // re-process the state - [NAV_FSM_EVENT_SUCCESS] = NAV_STATE_WAYPOINT_REACHED, // successfully reached waypoint - [NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE, - [NAV_FSM_EVENT_SWITCH_TO_ALTHOLD] = NAV_STATE_ALTHOLD_INITIALIZE, - [NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D] = NAV_STATE_POSHOLD_3D_INITIALIZE, - [NAV_FSM_EVENT_SWITCH_TO_RTH] = NAV_STATE_RTH_INITIALIZE, - [NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE, - [NAV_FSM_EVENT_SWITCH_TO_CRUISE_2D] = NAV_STATE_CRUISE_2D_INITIALIZE, - [NAV_FSM_EVENT_SWITCH_TO_CRUISE_3D] = NAV_STATE_CRUISE_3D_INITIALIZE, + [NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_WAYPOINT_IN_PROGRESS, // re-process the state + [NAV_FSM_EVENT_SUCCESS] = NAV_STATE_WAYPOINT_REACHED, // successfully reached waypoint + [NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE, + [NAV_FSM_EVENT_SWITCH_TO_ALTHOLD] = NAV_STATE_ALTHOLD_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D] = NAV_STATE_POSHOLD_3D_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_RTH] = NAV_STATE_RTH_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_COURSE_HOLD] = NAV_STATE_COURSE_HOLD_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_CRUISE] = NAV_STATE_CRUISE_INITIALIZE, } }, @@ -688,19 +688,19 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = { .mwState = MW_NAV_STATE_PROCESS_NEXT, .mwError = MW_NAV_ERROR_NONE, .onEvent = { - [NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_WAYPOINT_REACHED, // re-process state - [NAV_FSM_EVENT_SUCCESS] = NAV_STATE_WAYPOINT_NEXT, - [NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_HOLD_TIME] = NAV_STATE_WAYPOINT_HOLD_TIME, - [NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_FINISHED] = NAV_STATE_WAYPOINT_FINISHED, - [NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_RTH_LAND] = NAV_STATE_WAYPOINT_RTH_LAND, + [NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_WAYPOINT_REACHED, // re-process state + [NAV_FSM_EVENT_SUCCESS] = NAV_STATE_WAYPOINT_NEXT, + [NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_HOLD_TIME] = NAV_STATE_WAYPOINT_HOLD_TIME, + [NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_FINISHED] = NAV_STATE_WAYPOINT_FINISHED, + [NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_RTH_LAND] = NAV_STATE_WAYPOINT_RTH_LAND, [NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_HOVER_ABOVE_HOME] = NAV_STATE_WAYPOINT_HOVER_ABOVE_HOME, - [NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE, - [NAV_FSM_EVENT_SWITCH_TO_ALTHOLD] = NAV_STATE_ALTHOLD_INITIALIZE, - [NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D] = NAV_STATE_POSHOLD_3D_INITIALIZE, - [NAV_FSM_EVENT_SWITCH_TO_RTH] = NAV_STATE_RTH_INITIALIZE, - [NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE, - [NAV_FSM_EVENT_SWITCH_TO_CRUISE_2D] = NAV_STATE_CRUISE_2D_INITIALIZE, - [NAV_FSM_EVENT_SWITCH_TO_CRUISE_3D] = NAV_STATE_CRUISE_3D_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE, + [NAV_FSM_EVENT_SWITCH_TO_ALTHOLD] = NAV_STATE_ALTHOLD_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D] = NAV_STATE_POSHOLD_3D_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_RTH] = NAV_STATE_RTH_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_COURSE_HOLD] = NAV_STATE_COURSE_HOLD_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_CRUISE] = NAV_STATE_CRUISE_INITIALIZE, } }, @@ -713,15 +713,15 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = { .mwState = MW_NAV_STATE_HOLD_TIMED, .mwError = MW_NAV_ERROR_NONE, .onEvent = { - [NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_WAYPOINT_HOLD_TIME, // re-process the state - [NAV_FSM_EVENT_SUCCESS] = NAV_STATE_WAYPOINT_NEXT, // successfully reached waypoint - [NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE, - [NAV_FSM_EVENT_SWITCH_TO_ALTHOLD] = NAV_STATE_ALTHOLD_INITIALIZE, - [NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D] = NAV_STATE_POSHOLD_3D_INITIALIZE, - [NAV_FSM_EVENT_SWITCH_TO_RTH] = NAV_STATE_RTH_INITIALIZE, - [NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE, - [NAV_FSM_EVENT_SWITCH_TO_CRUISE_2D] = NAV_STATE_CRUISE_2D_INITIALIZE, - [NAV_FSM_EVENT_SWITCH_TO_CRUISE_3D] = NAV_STATE_CRUISE_3D_INITIALIZE, + [NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_WAYPOINT_HOLD_TIME, // re-process the state + [NAV_FSM_EVENT_SUCCESS] = NAV_STATE_WAYPOINT_NEXT, // successfully reached waypoint + [NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE, + [NAV_FSM_EVENT_SWITCH_TO_ALTHOLD] = NAV_STATE_ALTHOLD_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D] = NAV_STATE_POSHOLD_3D_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_RTH] = NAV_STATE_RTH_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_COURSE_HOLD] = NAV_STATE_COURSE_HOLD_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_CRUISE] = NAV_STATE_CRUISE_INITIALIZE, } }, @@ -734,15 +734,15 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = { .mwState = MW_NAV_STATE_LAND_IN_PROGRESS, .mwError = MW_NAV_ERROR_LANDING, .onEvent = { - [NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_WAYPOINT_RTH_LAND, // re-process state - [NAV_FSM_EVENT_SUCCESS] = NAV_STATE_WAYPOINT_FINISHED, - [NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE, - [NAV_FSM_EVENT_SWITCH_TO_ALTHOLD] = NAV_STATE_ALTHOLD_INITIALIZE, - [NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D] = NAV_STATE_POSHOLD_3D_INITIALIZE, - [NAV_FSM_EVENT_SWITCH_TO_RTH] = NAV_STATE_RTH_INITIALIZE, - [NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE, - [NAV_FSM_EVENT_SWITCH_TO_CRUISE_2D] = NAV_STATE_CRUISE_2D_INITIALIZE, - [NAV_FSM_EVENT_SWITCH_TO_CRUISE_3D] = NAV_STATE_CRUISE_3D_INITIALIZE, + [NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_WAYPOINT_RTH_LAND, // re-process state + [NAV_FSM_EVENT_SUCCESS] = NAV_STATE_WAYPOINT_FINISHED, + [NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE, + [NAV_FSM_EVENT_SWITCH_TO_ALTHOLD] = NAV_STATE_ALTHOLD_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D] = NAV_STATE_POSHOLD_3D_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_RTH] = NAV_STATE_RTH_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_COURSE_HOLD] = NAV_STATE_COURSE_HOLD_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_CRUISE] = NAV_STATE_CRUISE_INITIALIZE, } }, @@ -755,8 +755,8 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = { .mwState = MW_NAV_STATE_PROCESS_NEXT, .mwError = MW_NAV_ERROR_NONE, .onEvent = { - [NAV_FSM_EVENT_SUCCESS] = NAV_STATE_WAYPOINT_PRE_ACTION, - [NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_FINISHED] = NAV_STATE_WAYPOINT_FINISHED, + [NAV_FSM_EVENT_SUCCESS] = NAV_STATE_WAYPOINT_PRE_ACTION, + [NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_FINISHED] = NAV_STATE_WAYPOINT_FINISHED, } }, @@ -769,13 +769,13 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = { .mwState = MW_NAV_STATE_WP_ENROUTE, .mwError = MW_NAV_ERROR_FINISH, .onEvent = { - [NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE, - [NAV_FSM_EVENT_SWITCH_TO_ALTHOLD] = NAV_STATE_ALTHOLD_INITIALIZE, - [NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D] = NAV_STATE_POSHOLD_3D_INITIALIZE, - [NAV_FSM_EVENT_SWITCH_TO_RTH] = NAV_STATE_RTH_INITIALIZE, - [NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE, - [NAV_FSM_EVENT_SWITCH_TO_CRUISE_2D] = NAV_STATE_CRUISE_2D_INITIALIZE, - [NAV_FSM_EVENT_SWITCH_TO_CRUISE_3D] = NAV_STATE_CRUISE_3D_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE, + [NAV_FSM_EVENT_SWITCH_TO_ALTHOLD] = NAV_STATE_ALTHOLD_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D] = NAV_STATE_POSHOLD_3D_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_RTH] = NAV_STATE_RTH_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_COURSE_HOLD] = NAV_STATE_COURSE_HOLD_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_CRUISE] = NAV_STATE_CRUISE_INITIALIZE, } }, @@ -788,15 +788,15 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = { .mwState = MW_NAV_STATE_HOVER_ABOVE_HOME, .mwError = MW_NAV_ERROR_NONE, .onEvent = { - [NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_WAYPOINT_HOVER_ABOVE_HOME, // re-process state - [NAV_FSM_EVENT_SUCCESS] = NAV_STATE_WAYPOINT_FINISHED, - [NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE, - [NAV_FSM_EVENT_SWITCH_TO_ALTHOLD] = NAV_STATE_ALTHOLD_INITIALIZE, - [NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D] = NAV_STATE_POSHOLD_3D_INITIALIZE, - [NAV_FSM_EVENT_SWITCH_TO_RTH] = NAV_STATE_RTH_INITIALIZE, - [NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE, - [NAV_FSM_EVENT_SWITCH_TO_CRUISE_2D] = NAV_STATE_CRUISE_2D_INITIALIZE, - [NAV_FSM_EVENT_SWITCH_TO_CRUISE_3D] = NAV_STATE_CRUISE_3D_INITIALIZE, + [NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_WAYPOINT_HOVER_ABOVE_HOME, // re-process state + [NAV_FSM_EVENT_SUCCESS] = NAV_STATE_WAYPOINT_FINISHED, + [NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE, + [NAV_FSM_EVENT_SWITCH_TO_ALTHOLD] = NAV_STATE_ALTHOLD_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_POSHOLD_3D] = NAV_STATE_POSHOLD_3D_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_RTH] = NAV_STATE_RTH_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_EMERGENCY_LANDING] = NAV_STATE_EMERGENCY_LANDING_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_COURSE_HOLD] = NAV_STATE_COURSE_HOLD_INITIALIZE, + [NAV_FSM_EVENT_SWITCH_TO_CRUISE] = NAV_STATE_CRUISE_INITIALIZE, } }, @@ -810,10 +810,10 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = { .mwState = MW_NAV_STATE_EMERGENCY_LANDING, .mwError = MW_NAV_ERROR_LANDING, .onEvent = { - [NAV_FSM_EVENT_SUCCESS] = NAV_STATE_EMERGENCY_LANDING_IN_PROGRESS, - [NAV_FSM_EVENT_ERROR] = NAV_STATE_IDLE, - [NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE, - [NAV_FSM_EVENT_SWITCH_TO_ALTHOLD] = NAV_STATE_IDLE, // ALTHOLD also bails out from emergency (to IDLE, AltHold will take over from there) + [NAV_FSM_EVENT_SUCCESS] = NAV_STATE_EMERGENCY_LANDING_IN_PROGRESS, + [NAV_FSM_EVENT_ERROR] = NAV_STATE_IDLE, + [NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE, + [NAV_FSM_EVENT_SWITCH_TO_ALTHOLD] = NAV_STATE_IDLE, // ALTHOLD also bails out from emergency (to IDLE, AltHold will take over from there) } }, @@ -826,10 +826,10 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = { .mwState = MW_NAV_STATE_EMERGENCY_LANDING, .mwError = MW_NAV_ERROR_LANDING, .onEvent = { - [NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_EMERGENCY_LANDING_IN_PROGRESS, // re-process the state - [NAV_FSM_EVENT_SUCCESS] = NAV_STATE_EMERGENCY_LANDING_FINISHED, - [NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE, - [NAV_FSM_EVENT_SWITCH_TO_ALTHOLD] = NAV_STATE_IDLE, // ALTHOLD also bails out from emergency (to IDLE, AltHold will take over from there) + [NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_EMERGENCY_LANDING_IN_PROGRESS, // re-process the state + [NAV_FSM_EVENT_SUCCESS] = NAV_STATE_EMERGENCY_LANDING_FINISHED, + [NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE, + [NAV_FSM_EVENT_SWITCH_TO_ALTHOLD] = NAV_STATE_IDLE, // ALTHOLD also bails out from emergency (to IDLE, AltHold will take over from there) } }, @@ -842,8 +842,8 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = { .mwState = MW_NAV_STATE_LANDED, .mwError = MW_NAV_ERROR_LANDING, .onEvent = { - [NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_EMERGENCY_LANDING_FINISHED, - [NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE, + [NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_EMERGENCY_LANDING_FINISHED, + [NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE, } }, @@ -856,9 +856,9 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = { .mwState = MW_NAV_STATE_NONE, .mwError = MW_NAV_ERROR_NONE, .onEvent = { - [NAV_FSM_EVENT_SUCCESS] = NAV_STATE_LAUNCH_WAIT, - [NAV_FSM_EVENT_ERROR] = NAV_STATE_IDLE, - [NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE, + [NAV_FSM_EVENT_SUCCESS] = NAV_STATE_LAUNCH_WAIT, + [NAV_FSM_EVENT_ERROR] = NAV_STATE_IDLE, + [NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE, } }, @@ -871,10 +871,10 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = { .mwState = MW_NAV_STATE_NONE, .mwError = MW_NAV_ERROR_NONE, .onEvent = { - [NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_LAUNCH_WAIT, // re-process the state - [NAV_FSM_EVENT_SUCCESS] = NAV_STATE_LAUNCH_IN_PROGRESS, - [NAV_FSM_EVENT_ERROR] = NAV_STATE_IDLE, - [NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE, + [NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_LAUNCH_WAIT, // re-process the state + [NAV_FSM_EVENT_SUCCESS] = NAV_STATE_LAUNCH_IN_PROGRESS, + [NAV_FSM_EVENT_ERROR] = NAV_STATE_IDLE, + [NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE, } }, @@ -887,10 +887,10 @@ static const navigationFSMStateDescriptor_t navFSM[NAV_STATE_COUNT] = { .mwState = MW_NAV_STATE_NONE, .mwError = MW_NAV_ERROR_NONE, .onEvent = { - [NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_LAUNCH_IN_PROGRESS, // re-process the state - [NAV_FSM_EVENT_SUCCESS] = NAV_STATE_IDLE, - [NAV_FSM_EVENT_ERROR] = NAV_STATE_IDLE, - [NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE, + [NAV_FSM_EVENT_TIMEOUT] = NAV_STATE_LAUNCH_IN_PROGRESS, // re-process the state + [NAV_FSM_EVENT_SUCCESS] = NAV_STATE_IDLE, + [NAV_FSM_EVENT_ERROR] = NAV_STATE_IDLE, + [NAV_FSM_EVENT_SWITCH_TO_IDLE] = NAV_STATE_IDLE, } }, }; @@ -1007,7 +1007,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_POSHOLD_3D_IN_PROGRESS( } ///////////////// -static navigationFSMEvent_t navOnEnteringState_NAV_STATE_CRUISE_2D_INITIALIZE(navigationFSMState_t previousState) +static navigationFSMEvent_t navOnEnteringState_NAV_STATE_COURSE_HOLD_INITIALIZE(navigationFSMState_t previousState) { const navigationFSMStateFlags_t prevFlags = navGetStateFlags(previousState); @@ -1027,7 +1027,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_CRUISE_2D_INITIALIZE(na return NAV_FSM_EVENT_SUCCESS; // Go to CRUISE_XD_IN_PROGRESS state } -static navigationFSMEvent_t navOnEnteringState_NAV_STATE_CRUISE_2D_IN_PROGRESS(navigationFSMState_t previousState) +static navigationFSMEvent_t navOnEnteringState_NAV_STATE_COURSE_HOLD_IN_PROGRESS(navigationFSMState_t previousState) { const timeMs_t currentTimeMs = millis(); @@ -1037,7 +1037,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_CRUISE_2D_IN_PROGRESS(n DEBUG_SET(DEBUG_CRUISE, 2, 0); if (posControl.flags.isAdjustingPosition) { - return NAV_FSM_EVENT_SWITCH_TO_CRUISE_ADJ; + return NAV_FSM_EVENT_SWITCH_TO_COURSE_ADJ; } // User is yawing. We record the desidered yaw and we change the desidered target in the meanwhile @@ -1056,8 +1056,8 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_CRUISE_2D_IN_PROGRESS(n uint32_t distance = gpsSol.groundSpeed * 60; // next WP to be reached in 60s [cm] - if ((previousState == NAV_STATE_CRUISE_2D_INITIALIZE) || (previousState == NAV_STATE_CRUISE_2D_ADJUSTING) - || (previousState == NAV_STATE_CRUISE_3D_INITIALIZE) || (previousState == NAV_STATE_CRUISE_3D_ADJUSTING) + if ((previousState == NAV_STATE_COURSE_HOLD_INITIALIZE) || (previousState == NAV_STATE_COURSE_HOLD_ADJUSTING) + || (previousState == NAV_STATE_CRUISE_INITIALIZE) || (previousState == NAV_STATE_CRUISE_ADJUSTING) || posControl.flags.isAdjustingHeading) { calculateFarAwayTarget(&posControl.cruise.targetPos, posControl.cruise.yaw, distance); DEBUG_SET(DEBUG_CRUISE, 2, 1); @@ -1071,7 +1071,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_CRUISE_2D_IN_PROGRESS(n return NAV_FSM_EVENT_NONE; } -static navigationFSMEvent_t navOnEnteringState_NAV_STATE_CRUISE_2D_ADJUSTING(navigationFSMState_t previousState) +static navigationFSMEvent_t navOnEnteringState_NAV_STATE_COURSE_HOLD_ADJUSTING(navigationFSMState_t previousState) { UNUSED(previousState); DEBUG_SET(DEBUG_CRUISE, 0, 3); @@ -1088,27 +1088,27 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_CRUISE_2D_ADJUSTING(nav return NAV_FSM_EVENT_SUCCESS; // go back to the CRUISE_XD_IN_PROGRESS state } -static navigationFSMEvent_t navOnEnteringState_NAV_STATE_CRUISE_3D_INITIALIZE(navigationFSMState_t previousState) +static navigationFSMEvent_t navOnEnteringState_NAV_STATE_CRUISE_INITIALIZE(navigationFSMState_t previousState) { if (!STATE(FIXED_WING_LEGACY)) { return NAV_FSM_EVENT_ERROR; } // only on FW for now navOnEnteringState_NAV_STATE_ALTHOLD_INITIALIZE(previousState); - return navOnEnteringState_NAV_STATE_CRUISE_2D_INITIALIZE(previousState); + return navOnEnteringState_NAV_STATE_COURSE_HOLD_INITIALIZE(previousState); } -static navigationFSMEvent_t navOnEnteringState_NAV_STATE_CRUISE_3D_IN_PROGRESS(navigationFSMState_t previousState) +static navigationFSMEvent_t navOnEnteringState_NAV_STATE_CRUISE_IN_PROGRESS(navigationFSMState_t previousState) { navOnEnteringState_NAV_STATE_ALTHOLD_IN_PROGRESS(previousState); - return navOnEnteringState_NAV_STATE_CRUISE_2D_IN_PROGRESS(previousState); + return navOnEnteringState_NAV_STATE_COURSE_HOLD_IN_PROGRESS(previousState); } -static navigationFSMEvent_t navOnEnteringState_NAV_STATE_CRUISE_3D_ADJUSTING(navigationFSMState_t previousState) +static navigationFSMEvent_t navOnEnteringState_NAV_STATE_CRUISE_ADJUSTING(navigationFSMState_t previousState) { navOnEnteringState_NAV_STATE_ALTHOLD_IN_PROGRESS(previousState); - return navOnEnteringState_NAV_STATE_CRUISE_2D_ADJUSTING(previousState); + return navOnEnteringState_NAV_STATE_COURSE_HOLD_ADJUSTING(previousState); } static navigationFSMEvent_t navOnEnteringState_NAV_STATE_RTH_INITIALIZE(navigationFSMState_t previousState) @@ -3065,7 +3065,7 @@ void applyWaypointNavigationAndAltitudeHold(void) void switchNavigationFlightModes(void) { const flightModeFlags_e enabledNavFlightModes = navGetMappedFlightModes(posControl.navState); - const flightModeFlags_e disabledFlightModes = (NAV_ALTHOLD_MODE | NAV_RTH_MODE | NAV_POSHOLD_MODE | NAV_WP_MODE | NAV_LAUNCH_MODE | NAV_CRUISE_MODE) & (~enabledNavFlightModes); + const flightModeFlags_e disabledFlightModes = (NAV_ALTHOLD_MODE | NAV_RTH_MODE | NAV_POSHOLD_MODE | NAV_WP_MODE | NAV_LAUNCH_MODE | NAV_COURSE_HOLD_MODE) & (~enabledNavFlightModes); DISABLE_FLIGHT_MODE(disabledFlightModes); ENABLE_FLIGHT_MODE(enabledNavFlightModes); } @@ -3182,13 +3182,13 @@ static navigationFSMEvent_t selectNavEventFromBoxModeInput(void) // PH has priority over CRUISE // CRUISE has priority on AH - if (IS_RC_MODE_ACTIVE(BOXNAVCRUISE)) { + if (IS_RC_MODE_ACTIVE(BOXNAVCOURSEHOLD)) { - if (IS_RC_MODE_ACTIVE(BOXNAVALTHOLD) && ((FLIGHT_MODE(NAV_CRUISE_MODE) && FLIGHT_MODE(NAV_ALTHOLD_MODE)) || (canActivatePosHold && canActivateAltHold))) - return NAV_FSM_EVENT_SWITCH_TO_CRUISE_3D; + if (IS_RC_MODE_ACTIVE(BOXNAVALTHOLD) && ((FLIGHT_MODE(NAV_COURSE_HOLD_MODE) && FLIGHT_MODE(NAV_ALTHOLD_MODE)) || (canActivatePosHold && canActivateAltHold))) + return NAV_FSM_EVENT_SWITCH_TO_CRUISE; - if (FLIGHT_MODE(NAV_CRUISE_MODE) || (canActivatePosHold)) - return NAV_FSM_EVENT_SWITCH_TO_CRUISE_2D; + if (FLIGHT_MODE(NAV_COURSE_HOLD_MODE) || (canActivatePosHold)) + return NAV_FSM_EVENT_SWITCH_TO_COURSE_HOLD; } @@ -3270,8 +3270,8 @@ bool navigationTerrainFollowingEnabled(void) navArmingBlocker_e navigationIsBlockingArming(bool *usedBypass) { - const bool navBoxModesEnabled = IS_RC_MODE_ACTIVE(BOXNAVRTH) || IS_RC_MODE_ACTIVE(BOXNAVWP) || IS_RC_MODE_ACTIVE(BOXNAVPOSHOLD) || (STATE(FIXED_WING_LEGACY) && IS_RC_MODE_ACTIVE(BOXNAVALTHOLD)) || (STATE(FIXED_WING_LEGACY) && IS_RC_MODE_ACTIVE(BOXNAVCRUISE)); - const bool navLaunchComboModesEnabled = isNavLaunchEnabled() && (IS_RC_MODE_ACTIVE(BOXNAVRTH) || IS_RC_MODE_ACTIVE(BOXNAVWP) || IS_RC_MODE_ACTIVE(BOXNAVALTHOLD) || IS_RC_MODE_ACTIVE(BOXNAVCRUISE)); + const bool navBoxModesEnabled = IS_RC_MODE_ACTIVE(BOXNAVRTH) || IS_RC_MODE_ACTIVE(BOXNAVWP) || IS_RC_MODE_ACTIVE(BOXNAVPOSHOLD) || (STATE(FIXED_WING_LEGACY) && IS_RC_MODE_ACTIVE(BOXNAVALTHOLD)) || (STATE(FIXED_WING_LEGACY) && IS_RC_MODE_ACTIVE(BOXNAVCOURSEHOLD)); + const bool navLaunchComboModesEnabled = isNavLaunchEnabled() && (IS_RC_MODE_ACTIVE(BOXNAVRTH) || IS_RC_MODE_ACTIVE(BOXNAVWP) || IS_RC_MODE_ACTIVE(BOXNAVALTHOLD) || IS_RC_MODE_ACTIVE(BOXNAVCOURSEHOLD)); if (usedBypass) { *usedBypass = false; diff --git a/src/main/navigation/navigation_fixedwing.c b/src/main/navigation/navigation_fixedwing.c index 51c25d77cc..40f2f49b0b 100755 --- a/src/main/navigation/navigation_fixedwing.c +++ b/src/main/navigation/navigation_fixedwing.c @@ -259,7 +259,7 @@ static void calculateVirtualPositionTarget_FW(float trackingPeriod) bool needToCalculateCircularLoiter = (isApproachingLastWaypoint() || isWaypointWait()) && (distanceToActualTarget <= (navConfig()->fw.loiter_radius / TAN_15DEG)) && (distanceToActualTarget > 50.0f) - && !FLIGHT_MODE(NAV_CRUISE_MODE); + && !FLIGHT_MODE(NAV_COURSE_HOLD_MODE); // Calculate virtual position for straight movement if (needToCalculateCircularLoiter) { @@ -649,7 +649,7 @@ void applyFixedWingNavigationController(navigationFSMStateFlags_t navStateFlags, posControl.rcAdjustment[ROLL] = 0; } - if (FLIGHT_MODE(NAV_CRUISE_MODE) && posControl.flags.isAdjustingPosition) + if (FLIGHT_MODE(NAV_COURSE_HOLD_MODE) && posControl.flags.isAdjustingPosition) rcCommand[ROLL] = applyDeadband(rcCommand[ROLL], rcControlsConfig()->pos_hold_deadband); //if (navStateFlags & NAV_CTL_YAW) diff --git a/src/main/navigation/navigation_private.h b/src/main/navigation/navigation_private.h index 6e7c4d3c0c..9848dfefbe 100755 --- a/src/main/navigation/navigation_private.h +++ b/src/main/navigation/navigation_private.h @@ -140,9 +140,9 @@ typedef enum { NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_HOLD_TIME = NAV_FSM_EVENT_STATE_SPECIFIC_3, NAV_FSM_EVENT_SWITCH_TO_WAYPOINT_HOVER_ABOVE_HOME, - NAV_FSM_EVENT_SWITCH_TO_CRUISE_2D, - NAV_FSM_EVENT_SWITCH_TO_CRUISE_3D, - NAV_FSM_EVENT_SWITCH_TO_CRUISE_ADJ, + NAV_FSM_EVENT_SWITCH_TO_COURSE_HOLD, + NAV_FSM_EVENT_SWITCH_TO_CRUISE, + NAV_FSM_EVENT_SWITCH_TO_COURSE_ADJ, NAV_FSM_EVENT_COUNT, } navigationFSMEvent_t; @@ -187,13 +187,13 @@ typedef enum { NAV_PERSISTENT_ID_UNUSED_3 = 27, // was NAV_STATE_LAUNCH_MOTOR_DELAY NAV_PERSISTENT_ID_LAUNCH_IN_PROGRESS = 28, - NAV_PERSISTENT_ID_CRUISE_2D_INITIALIZE = 29, - NAV_PERSISTENT_ID_CRUISE_2D_IN_PROGRESS = 30, - NAV_PERSISTENT_ID_CRUISE_2D_ADJUSTING = 31, + NAV_PERSISTENT_ID_COURSE_HOLD_INITIALIZE = 29, + NAV_PERSISTENT_ID_COURSE_HOLD_IN_PROGRESS = 30, + NAV_PERSISTENT_ID_COURSE_HOLD_ADJUSTING = 31, - NAV_PERSISTENT_ID_CRUISE_3D_INITIALIZE = 32, - NAV_PERSISTENT_ID_CRUISE_3D_IN_PROGRESS = 33, - NAV_PERSISTENT_ID_CRUISE_3D_ADJUSTING = 34, + NAV_PERSISTENT_ID_CRUISE_INITIALIZE = 32, + NAV_PERSISTENT_ID_CRUISE_IN_PROGRESS = 33, + NAV_PERSISTENT_ID_CRUISE_ADJUSTING = 34, NAV_PERSISTENT_ID_WAYPOINT_HOLD_TIME = 35, NAV_PERSISTENT_ID_RTH_HOVER_ABOVE_HOME = 36, @@ -239,12 +239,12 @@ typedef enum { NAV_STATE_LAUNCH_WAIT, NAV_STATE_LAUNCH_IN_PROGRESS, - NAV_STATE_CRUISE_2D_INITIALIZE, - NAV_STATE_CRUISE_2D_IN_PROGRESS, - NAV_STATE_CRUISE_2D_ADJUSTING, - NAV_STATE_CRUISE_3D_INITIALIZE, - NAV_STATE_CRUISE_3D_IN_PROGRESS, - NAV_STATE_CRUISE_3D_ADJUSTING, + NAV_STATE_COURSE_HOLD_INITIALIZE, + NAV_STATE_COURSE_HOLD_IN_PROGRESS, + NAV_STATE_COURSE_HOLD_ADJUSTING, + NAV_STATE_CRUISE_INITIALIZE, + NAV_STATE_CRUISE_IN_PROGRESS, + NAV_STATE_CRUISE_ADJUSTING, NAV_STATE_COUNT, } navigationFSMState_t; diff --git a/src/main/programming/logic_condition.c b/src/main/programming/logic_condition.c index 017d585b0a..89a1fc6fe6 100644 --- a/src/main/programming/logic_condition.c +++ b/src/main/programming/logic_condition.c @@ -560,7 +560,7 @@ static int logicConditionGetFlightModeOperandValue(int operand) { break; case LOGIC_CONDITION_OPERAND_FLIGHT_MODE_CRUISE: - return (bool) FLIGHT_MODE(NAV_CRUISE_MODE); + return (bool) FLIGHT_MODE(NAV_COURSE_HOLD_MODE); break; case LOGIC_CONDITION_OPERAND_FLIGHT_MODE_ALTHOLD: diff --git a/src/main/telemetry/crsf.c b/src/main/telemetry/crsf.c index d72175cf24..439ca8f2d0 100755 --- a/src/main/telemetry/crsf.c +++ b/src/main/telemetry/crsf.c @@ -331,9 +331,9 @@ static void crsfFrameFlightMode(sbuf_t *dst) flightMode = "RTH"; } else if (FLIGHT_MODE(NAV_POSHOLD_MODE)) { flightMode = "HOLD"; - } else if (FLIGHT_MODE(NAV_CRUISE_MODE) && FLIGHT_MODE(NAV_ALTHOLD_MODE)) { + } else if (FLIGHT_MODE(NAV_COURSE_HOLD_MODE) && FLIGHT_MODE(NAV_ALTHOLD_MODE)) { flightMode = "3CRS"; - } else if (FLIGHT_MODE(NAV_CRUISE_MODE)) { + } else if (FLIGHT_MODE(NAV_COURSE_HOLD_MODE)) { flightMode = "CRS"; } else if (FLIGHT_MODE(NAV_ALTHOLD_MODE)) { flightMode = "AH"; diff --git a/src/main/telemetry/frsky.c b/src/main/telemetry/frsky.c index 373a055a87..0896f412de 100644 --- a/src/main/telemetry/frsky.c +++ b/src/main/telemetry/frsky.c @@ -47,7 +47,7 @@ uint16_t frskyGetFlightMode(void) // thousands column if (FLIGHT_MODE(NAV_RTH_MODE)) tmpi += 1000; - if (FLIGHT_MODE(NAV_CRUISE_MODE)) // intentionally out of order and 'else-ifs' to prevent column overflow + if (FLIGHT_MODE(NAV_COURSE_HOLD_MODE)) // intentionally out of order and 'else-ifs' to prevent column overflow tmpi += 8000; else if (FLIGHT_MODE(NAV_WP_MODE)) tmpi += 2000; diff --git a/src/main/telemetry/ltm.c b/src/main/telemetry/ltm.c index 8a9a5c8590..184c947ead 100644 --- a/src/main/telemetry/ltm.c +++ b/src/main/telemetry/ltm.c @@ -168,7 +168,7 @@ void ltm_sframe(sbuf_t *dst) lt_flightmode = LTM_MODE_RTH; else if (FLIGHT_MODE(NAV_POSHOLD_MODE)) lt_flightmode = LTM_MODE_GPSHOLD; - else if (FLIGHT_MODE(NAV_CRUISE_MODE)) + else if (FLIGHT_MODE(NAV_COURSE_HOLD_MODE)) lt_flightmode = LTM_MODE_CRUISE; else if (FLIGHT_MODE(NAV_LAUNCH_MODE)) lt_flightmode = LTM_MODE_LAUNCH; From b1b8b9973af7c46e188b86476a3942dfe1004c32 Mon Sep 17 00:00:00 2001 From: "Pawel Spychalski (DzikuVx)" Date: Wed, 7 Apr 2021 12:53:43 +0200 Subject: [PATCH 128/286] Serial BNO055 detection --- src/main/CMakeLists.txt | 2 + src/main/drivers/accgyro/accgyro_bno055.c | 43 ---- src/main/drivers/accgyro/accgyro_bno055.h | 41 ++++ .../drivers/accgyro/accgyro_bno055_serial.c | 188 ++++++++++++++++++ .../drivers/accgyro/accgyro_bno055_serial.h | 35 ++++ src/main/fc/fc_tasks.c | 2 +- src/main/fc/settings.yaml | 2 +- src/main/flight/secondary_imu.c | 17 +- src/main/flight/secondary_imu.h | 6 +- src/main/io/serial.h | 3 +- 10 files changed, 287 insertions(+), 52 deletions(-) create mode 100644 src/main/drivers/accgyro/accgyro_bno055_serial.c create mode 100644 src/main/drivers/accgyro/accgyro_bno055_serial.h diff --git a/src/main/CMakeLists.txt b/src/main/CMakeLists.txt index 47cb31eea8..b98d9538b7 100644 --- a/src/main/CMakeLists.txt +++ b/src/main/CMakeLists.txt @@ -105,6 +105,8 @@ main_sources(COMMON_SRC drivers/accgyro/accgyro_mpu9250.h drivers/accgyro/accgyro_bno055.c drivers/accgyro/accgyro_bno055.h + drivers/accgyro/accgyro_bno055_serial.c + drivers/accgyro/accgyro_bno055_serial.h drivers/adc.c drivers/adc.h diff --git a/src/main/drivers/accgyro/accgyro_bno055.c b/src/main/drivers/accgyro/accgyro_bno055.c index fdb73b7250..1ed096d76a 100644 --- a/src/main/drivers/accgyro/accgyro_bno055.c +++ b/src/main/drivers/accgyro/accgyro_bno055.c @@ -32,47 +32,6 @@ #ifdef USE_IMU_BNO055 -#define BNO055_ADDR_PWR_MODE 0x3E -#define BNO055_ADDR_OPR_MODE 0x3D -#define BNO055_ADDR_CALIB_STAT 0x35 - -#define BNO055_PWR_MODE_NORMAL 0x00 -#define BNO055_OPR_MODE_CONFIG 0x00 -#define BNO055_OPR_MODE_NDOF 0x0C - -#define BNO055_ADDR_EUL_YAW_LSB 0x1A -#define BNO055_ADDR_EUL_YAW_MSB 0x1B -#define BNO055_ADDR_EUL_ROLL_LSB 0x1C -#define BNO055_ADDR_EUL_ROLL_MSB 0x1D -#define BNO055_ADDR_EUL_PITCH_LSB 0x1E -#define BNO055_ADDR_EUL_PITCH_MSB 0x1F - -#define BNO055_ADDR_MAG_RADIUS_MSB 0x6A -#define BNO055_ADDR_MAG_RADIUS_LSB 0x69 -#define BNO055_ADDR_ACC_RADIUS_MSB 0x68 -#define BNO055_ADDR_ACC_RADIUS_LSB 0x67 - -#define BNO055_ADDR_GYR_OFFSET_Z_MSB 0x66 -#define BNO055_ADDR_GYR_OFFSET_Z_LSB 0x65 -#define BNO055_ADDR_GYR_OFFSET_Y_MSB 0x64 -#define BNO055_ADDR_GYR_OFFSET_Y_LSB 0x63 -#define BNO055_ADDR_GYR_OFFSET_X_MSB 0x62 -#define BNO055_ADDR_GYR_OFFSET_X_LSB 0x61 - -#define BNO055_ADDR_MAG_OFFSET_Z_MSB 0x60 -#define BNO055_ADDR_MAG_OFFSET_Z_LSB 0x5F -#define BNO055_ADDR_MAG_OFFSET_Y_MSB 0x5E -#define BNO055_ADDR_MAG_OFFSET_Y_LSB 0x5D -#define BNO055_ADDR_MAG_OFFSET_X_MSB 0x5C -#define BNO055_ADDR_MAG_OFFSET_X_LSB 0x5B - -#define BNO055_ADDR_ACC_OFFSET_Z_MSB 0x5A -#define BNO055_ADDR_ACC_OFFSET_Z_LSB 0x59 -#define BNO055_ADDR_ACC_OFFSET_Y_MSB 0x58 -#define BNO055_ADDR_ACC_OFFSET_Y_LSB 0x57 -#define BNO055_ADDR_ACC_OFFSET_X_MSB 0x56 -#define BNO055_ADDR_ACC_OFFSET_X_LSB 0x55 - static busDevice_t *busDev; static bool deviceDetect(busDevice_t *busDev) @@ -104,13 +63,11 @@ bool bno055Init(bno055CalibrationData_t calibrationData, bool setCalibration) busDev = busDeviceInit(BUSTYPE_I2C, DEVHW_BNO055, 0, 0); if (busDev == NULL) { - DEBUG_SET(DEBUG_IMU2, 2, 1); return false; } if (!deviceDetect(busDev)) { - DEBUG_SET(DEBUG_IMU2, 2, 2); busDeviceDeInit(busDev); return false; } diff --git a/src/main/drivers/accgyro/accgyro_bno055.h b/src/main/drivers/accgyro/accgyro_bno055.h index d0c24f6dc4..6e6abdbff9 100644 --- a/src/main/drivers/accgyro/accgyro_bno055.h +++ b/src/main/drivers/accgyro/accgyro_bno055.h @@ -25,6 +25,47 @@ #include "common/vector.h" +#define BNO055_ADDR_PWR_MODE 0x3E +#define BNO055_ADDR_OPR_MODE 0x3D +#define BNO055_ADDR_CALIB_STAT 0x35 + +#define BNO055_PWR_MODE_NORMAL 0x00 +#define BNO055_OPR_MODE_CONFIG 0x00 +#define BNO055_OPR_MODE_NDOF 0x0C + +#define BNO055_ADDR_EUL_YAW_LSB 0x1A +#define BNO055_ADDR_EUL_YAW_MSB 0x1B +#define BNO055_ADDR_EUL_ROLL_LSB 0x1C +#define BNO055_ADDR_EUL_ROLL_MSB 0x1D +#define BNO055_ADDR_EUL_PITCH_LSB 0x1E +#define BNO055_ADDR_EUL_PITCH_MSB 0x1F + +#define BNO055_ADDR_MAG_RADIUS_MSB 0x6A +#define BNO055_ADDR_MAG_RADIUS_LSB 0x69 +#define BNO055_ADDR_ACC_RADIUS_MSB 0x68 +#define BNO055_ADDR_ACC_RADIUS_LSB 0x67 + +#define BNO055_ADDR_GYR_OFFSET_Z_MSB 0x66 +#define BNO055_ADDR_GYR_OFFSET_Z_LSB 0x65 +#define BNO055_ADDR_GYR_OFFSET_Y_MSB 0x64 +#define BNO055_ADDR_GYR_OFFSET_Y_LSB 0x63 +#define BNO055_ADDR_GYR_OFFSET_X_MSB 0x62 +#define BNO055_ADDR_GYR_OFFSET_X_LSB 0x61 + +#define BNO055_ADDR_MAG_OFFSET_Z_MSB 0x60 +#define BNO055_ADDR_MAG_OFFSET_Z_LSB 0x5F +#define BNO055_ADDR_MAG_OFFSET_Y_MSB 0x5E +#define BNO055_ADDR_MAG_OFFSET_Y_LSB 0x5D +#define BNO055_ADDR_MAG_OFFSET_X_MSB 0x5C +#define BNO055_ADDR_MAG_OFFSET_X_LSB 0x5B + +#define BNO055_ADDR_ACC_OFFSET_Z_MSB 0x5A +#define BNO055_ADDR_ACC_OFFSET_Z_LSB 0x59 +#define BNO055_ADDR_ACC_OFFSET_Y_MSB 0x58 +#define BNO055_ADDR_ACC_OFFSET_Y_LSB 0x57 +#define BNO055_ADDR_ACC_OFFSET_X_MSB 0x56 +#define BNO055_ADDR_ACC_OFFSET_X_LSB 0x55 + typedef struct { uint8_t sys; uint8_t gyr; diff --git a/src/main/drivers/accgyro/accgyro_bno055_serial.c b/src/main/drivers/accgyro/accgyro_bno055_serial.c new file mode 100644 index 0000000000..ca1ebfb50b --- /dev/null +++ b/src/main/drivers/accgyro/accgyro_bno055_serial.c @@ -0,0 +1,188 @@ +/* + * This file is part of INAV. + * + * This Source Code Form is subject to the terms of the Mozilla Public + * License, v. 2.0. If a copy of the MPL was not distributed with this file, + * You can obtain one at http://mozilla.org/MPL/2.0/. + * + * Alternatively, the contents of this file may be used under the terms + * of the GNU General Public License Version 3, as described below: + * + * This file is free software: you may copy, redistribute and/or modify + * it under the terms of the GNU General Public License as published by the + * Free Software Foundation, either version 3 of the License, or (at your + * option) any later version. + * + * This file is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General + * Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see http://www.gnu.org/licenses/. + */ +#include "platform.h" +#ifdef USE_IMU_BNO055 + +#define BNO055_BAUD_RATE 115200 +#define BNO055_FRAME_MAX_TIME_MS 10 + +#include +#include +#include "io/serial.h" +#include "drivers/accgyro/accgyro_bno055.h" +#include "build/debug.h" +#include "drivers/time.h" + +static serialPort_t * bno055SerialPort = NULL; + +static uint8_t receiveBuffer[16]; + +typedef enum { + BNO055_RECEIVE_IDLE, + BNO055_RECEIVE_HEADER, + BNO055_RECEIVE_LENGTH, + BNO055_RECEIVE_PAYLOAD, + BNO055_RECEIVE_ACK, +} bno055ReceiveState_e; + +typedef enum { + BNO055_FRAME_ACK, + BNO055_FRAME_DATA, +} bno055FrameType_e; + +static uint8_t bno055ProtocolState = BNO055_RECEIVE_IDLE; +static uint8_t bno055FrameType; +static uint8_t bno055FrameLength; +static uint8_t bno055FrameIndex; +static timeMs_t bno055FrameStartAtMs = 0; + +static void bno055SerialWrite(uint8_t reg, uint8_t data) { + bno055ProtocolState = BNO055_RECEIVE_IDLE; + serialWrite(bno055SerialPort, 0xAA); // Start Byte + serialWrite(bno055SerialPort, 0x00); // Write command + serialWrite(bno055SerialPort, reg); + serialWrite(bno055SerialPort, 1); + serialWrite(bno055SerialPort, data); +} + +static void bno055SerialRead(uint8_t reg) { + bno055ProtocolState = BNO055_RECEIVE_IDLE; + serialWrite(bno055SerialPort, 0xAA); // Start Byte + serialWrite(bno055SerialPort, 0x01); // Read command + serialWrite(bno055SerialPort, reg); + serialWrite(bno055SerialPort, 1); +} + +void bno055SerialDataReceive(uint16_t c, void *data) { + + const uint8_t incoming = (uint8_t) c; + + //Failsafe for stuck frames + if (bno055ProtocolState != BNO055_RECEIVE_IDLE && millis() - bno055FrameStartAtMs > BNO055_FRAME_MAX_TIME_MS) { + bno055ProtocolState = BNO055_RECEIVE_IDLE; + } + + if (bno055ProtocolState == BNO055_RECEIVE_IDLE && incoming == 0xEE) { + bno055FrameStartAtMs = millis(); + bno055ProtocolState = BNO055_RECEIVE_HEADER; + bno055FrameType = BNO055_FRAME_ACK; + } else if (bno055ProtocolState == BNO055_RECEIVE_IDLE && incoming == 0xBB) { + bno055FrameStartAtMs = millis(); + bno055ProtocolState = BNO055_RECEIVE_HEADER; + bno055FrameType = BNO055_FRAME_DATA; + } else if (bno055ProtocolState == BNO055_RECEIVE_HEADER && bno055FrameType == BNO055_FRAME_ACK) { + receiveBuffer[0] = incoming; + bno055ProtocolState = BNO055_RECEIVE_IDLE; + } else if (bno055ProtocolState == BNO055_RECEIVE_HEADER && bno055FrameType == BNO055_FRAME_DATA) { + bno055FrameLength = incoming; + bno055FrameIndex = 0; + bno055ProtocolState = BNO055_RECEIVE_LENGTH; + } else if (bno055ProtocolState == BNO055_RECEIVE_LENGTH) { + receiveBuffer[bno055FrameIndex] = incoming; + bno055FrameIndex++; + + if (bno055FrameIndex == bno055FrameLength) { + bno055ProtocolState = BNO055_RECEIVE_IDLE; + } + } + +} + +bool bno055SerialInit(bno055CalibrationData_t calibrationData, bool setCalibration) { + bno055SerialPort = NULL; + + serialPortConfig_t * portConfig = findSerialPortConfig(FUNCTION_BNO055); + if (!portConfig) { + return false; + } + + // DEBUG_SET(DEBUG_IMU2, 0, 2); + + bno055SerialPort = openSerialPort( + portConfig->identifier, + FUNCTION_BNO055, + bno055SerialDataReceive, + NULL, + BNO055_BAUD_RATE, + MODE_RXTX, + SERIAL_NOT_INVERTED | SERIAL_UNIDIR | SERIAL_STOPBITS_1 | SERIAL_PARITY_NO + ); + + if (!bno055SerialPort) { + return false; + } + + bno055SerialRead(0x00); // Read ChipID + delay(5); + + DEBUG_SET(DEBUG_IMU2, 0, bno055FrameType); + DEBUG_SET(DEBUG_IMU2, 1, receiveBuffer[0]); + DEBUG_SET(DEBUG_IMU2, 2, bno055ProtocolState); + DEBUG_SET(DEBUG_IMU2, 3, bno055FrameIndex); + + //Check ident + if (bno055FrameType != BNO055_FRAME_DATA || receiveBuffer[0] != 0xA0 || bno055ProtocolState != BNO055_RECEIVE_IDLE) { + return false; // Ident does not match, leave + } + + // bno055SerialWrite(BNO055_ADDR_PWR_MODE, BNO055_PWR_MODE_NORMAL); //Set power mode NORMAL + // delay(5); + + + // DEBUG_SET(DEBUG_IMU2, 0, 3); + + // while (1) { + // // bno055SerialWrite(BNO055_ADDR_PWR_MODE, BNO055_PWR_MODE_NORMAL); //Set power mode NORMAL + // // bno055SerialRead(0x00); //Set power mode NORMAL + // bno055SerialRead(0x00); // Read ChipID + // delay(100); + // } + + + // DEBUG_SET(DEBUG_IMU2, 0, 4); + + return true; +} + +fpVector3_t bno055SerialGetEurlerAngles(void) { + +} + +void bno055SerialFetchEulerAngles(int16_t * buffer) { + // bno055SerialRead(0x00); +} + +bno055CalibStat_t bno055SerialGetCalibStat(void) { + +} + +bno055CalibrationData_t bno055SerialGetCalibrationData(void) { + +} + +void bno055SerialSetCalibrationData(bno055CalibrationData_t data) { + +} + +#endif \ No newline at end of file diff --git a/src/main/drivers/accgyro/accgyro_bno055_serial.h b/src/main/drivers/accgyro/accgyro_bno055_serial.h new file mode 100644 index 0000000000..5c0b4945e1 --- /dev/null +++ b/src/main/drivers/accgyro/accgyro_bno055_serial.h @@ -0,0 +1,35 @@ +/* + * This file is part of INAV. + * + * This Source Code Form is subject to the terms of the Mozilla Public + * License, v. 2.0. If a copy of the MPL was not distributed with this file, + * You can obtain one at http://mozilla.org/MPL/2.0/. + * + * Alternatively, the contents of this file may be used under the terms + * of the GNU General Public License Version 3, as described below: + * + * This file is free software: you may copy, redistribute and/or modify + * it under the terms of the GNU General Public License as published by the + * Free Software Foundation, either version 3 of the License, or (at your + * option) any later version. + * + * This file is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General + * Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with this program. If not, see http://www.gnu.org/licenses/. + */ + +#pragma once + +#include "common/vector.h" +#include "drivers/accgyro/accgyro_bno055.h" + +bool bno055SerialInit(bno055CalibrationData_t calibrationData, bool setCalibration); +fpVector3_t bno055SerialGetEurlerAngles(void); +void bno055SerialFetchEulerAngles(int16_t * buffer); +bno055CalibStat_t bno055SerialGetCalibStat(void); +bno055CalibrationData_t bno055SerialGetCalibrationData(void); +void bno055SerialSetCalibrationData(bno055CalibrationData_t data); \ No newline at end of file diff --git a/src/main/fc/fc_tasks.c b/src/main/fc/fc_tasks.c index 33d417a1d6..df5fc7357f 100755 --- a/src/main/fc/fc_tasks.c +++ b/src/main/fc/fc_tasks.c @@ -605,7 +605,7 @@ cfTask_t cfTasks[TASK_COUNT] = { [TASK_SECONDARY_IMU] = { .taskName = "IMU2", .taskFunc = taskSecondaryImu, - .desiredPeriod = TASK_PERIOD_HZ(10), // 10Hz @100msec + .desiredPeriod = TASK_PERIOD_HZ(1), // 10Hz @100msec .staticPriority = TASK_PRIORITY_IDLE, }, #endif diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 5be6038db3..99ad656dce 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -10,7 +10,7 @@ tables: values: ["NONE", "HCSR04", "SRF10", "INAV_I2C", "VL53L0X", "MSP", "UNUSED", "BENEWAKE", "VL53L1X"] enum: rangefinderType_e - name: secondary_imu_hardware - values: ["NONE", "BNO055"] + values: ["NONE", "BNO055", "BNO055_SERIAL"] enum: secondaryImuType_e - name: mag_hardware values: ["NONE", "AUTO", "HMC5883", "AK8975", "GPSMAG", "MAG3110", "AK8963", "IST8310", "QMC5883", "MPU9250", "IST8308", "LIS3MDL", "MSP", "RM3100", FAKE"] diff --git a/src/main/flight/secondary_imu.c b/src/main/flight/secondary_imu.c index da3a4dfde7..d47ee33712 100644 --- a/src/main/flight/secondary_imu.c +++ b/src/main/flight/secondary_imu.c @@ -33,6 +33,7 @@ #include "drivers/sensor.h" #include "drivers/accgyro/accgyro_bno055.h" +#include "drivers/accgyro/accgyro_bno055_serial.h" PG_REGISTER_WITH_RESET_FN(secondaryImuConfig_t, secondaryImuConfig, PG_SECONDARY_IMU, 1); @@ -82,6 +83,8 @@ void secondaryImuInit(void) if (secondaryImuConfig()->hardwareType == SECONDARY_IMU_BNO055) { secondaryImuState.active = bno055Init(calibrationData, (secondaryImuConfig()->calibrationRadiusAcc && secondaryImuConfig()->calibrationRadiusMag)); + } else if (secondaryImuConfig()->hardwareType == SECONDARY_IMU_BNO055_SERIAL) { + secondaryImuState.active = bno055SerialInit(calibrationData, (secondaryImuConfig()->calibrationRadiusAcc && secondaryImuConfig()->calibrationRadiusMag)); } if (!secondaryImuState.active) { @@ -101,6 +104,12 @@ void taskSecondaryImu(timeUs_t currentTimeUs) */ UNUSED(currentTimeUs); + if (secondaryImuConfig()->hardwareType == SECONDARY_IMU_BNO055) { + bno055FetchEulerAngles(secondaryImuState.eulerAngles.raw); + } else if (secondaryImuConfig()->hardwareType == SECONDARY_IMU_BNO055_SERIAL) { + bno055SerialFetchEulerAngles(secondaryImuState.eulerAngles.raw); + } + bno055FetchEulerAngles(secondaryImuState.eulerAngles.raw); //Apply mag declination @@ -144,10 +153,10 @@ void taskSecondaryImu(timeUs_t currentTimeUs) DEBUG_SET(DEBUG_IMU2, 1, secondaryImuState.eulerAngles.values.pitch); DEBUG_SET(DEBUG_IMU2, 2, secondaryImuState.eulerAngles.values.yaw); - DEBUG_SET(DEBUG_IMU2, 3, secondaryImuState.calibrationStatus.mag); - DEBUG_SET(DEBUG_IMU2, 4, secondaryImuState.calibrationStatus.gyr); - DEBUG_SET(DEBUG_IMU2, 5, secondaryImuState.calibrationStatus.acc); - DEBUG_SET(DEBUG_IMU2, 6, secondaryImuState.magDeclination); + // DEBUG_SET(DEBUG_IMU2, 3, secondaryImuState.calibrationStatus.mag); + // DEBUG_SET(DEBUG_IMU2, 4, secondaryImuState.calibrationStatus.gyr); + // DEBUG_SET(DEBUG_IMU2, 5, secondaryImuState.calibrationStatus.acc); + // DEBUG_SET(DEBUG_IMU2, 6, secondaryImuState.magDeclination); } void secondaryImuFetchCalibration(void) { diff --git a/src/main/flight/secondary_imu.h b/src/main/flight/secondary_imu.h index b674bc9c93..2c3b90ec6a 100644 --- a/src/main/flight/secondary_imu.h +++ b/src/main/flight/secondary_imu.h @@ -28,10 +28,12 @@ #include "common/time.h" #include "sensors/sensors.h" #include "drivers/accgyro/accgyro_bno055.h" +#include "drivers/accgyro/accgyro_bno055_serial.h" typedef enum { - SECONDARY_IMU_NONE = 0, - SECONDARY_IMU_BNO055 = 1, + SECONDARY_IMU_NONE = 0, + SECONDARY_IMU_BNO055 = 1, + SECONDARY_IMU_BNO055_SERIAL = 2, } secondaryImuType_e; typedef struct secondaryImuConfig_s { diff --git a/src/main/io/serial.h b/src/main/io/serial.h index 7b4f05c76b..c704c1c37b 100644 --- a/src/main/io/serial.h +++ b/src/main/io/serial.h @@ -44,7 +44,7 @@ typedef enum { FUNCTION_RCDEVICE = (1 << 10), // 1024 FUNCTION_VTX_SMARTAUDIO = (1 << 11), // 2048 FUNCTION_VTX_TRAMP = (1 << 12), // 4096 - FUNCTION_UNUSED_1 = (1 << 13), // 8192: former UAV_INTERCONNECT + FUNCTION_UNUSED_1 = (1 << 13), // 8192: former\ UAV_INTERCONNECT FUNCTION_OPTICAL_FLOW = (1 << 14), // 16384 FUNCTION_LOG = (1 << 15), // 32768 FUNCTION_RANGEFINDER = (1 << 16), // 65536 @@ -55,6 +55,7 @@ typedef enum { FUNCTION_DJI_HD_OSD = (1 << 21), // 2097152 FUNCTION_SERVO_SERIAL = (1 << 22), // 4194304 FUNCTION_TELEMETRY_SMARTPORT_MASTER = (1 << 23), // 8388608 + FUNCTION_BNO055 = (1 << 24), // 16777216 } serialPortFunction_e; typedef enum { From 9f950620bd6c70b4aa06baed55cd3b80cd357815 Mon Sep 17 00:00:00 2001 From: "Pawel Spychalski (DzikuVx)" Date: Wed, 7 Apr 2021 12:55:51 +0200 Subject: [PATCH 129/286] Increase loiter max radius to 300m --- src/main/fc/settings.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 3a446cf6b6..01f129aa22 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -2482,7 +2482,7 @@ groups: default_value: "7500" field: fw.loiter_radius min: 0 - max: 10000 + max: 30000 - name: nav_fw_cruise_speed description: "Speed for the plane/wing at cruise throttle used for remaining flight time/distance estimation in cm/s" default_value: "0" From fc0e5e2741a16e2bb00e3d5fd8baf36ba686782a Mon Sep 17 00:00:00 2001 From: Michel Pastor Date: Wed, 7 Apr 2021 17:48:09 +0200 Subject: [PATCH 130/286] Get setting default values from settings.yaml (#6595) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit * Get setting default values from settings.yaml * settings: Make the generator more robust and versatile - Add support for resolving types and values in multiple compilers - Add support for resolving types and values in clang 10 - Add support for using the host compiler for resolving the settings This allows us to run the generator for the unit tests, since they now need the settings_generated.h file to get the default setting values from it. * Fix regexps in settings.rb and add execution bit * Fix git revision issue * Fix issue with settings validation * Fix issue with targets not defining USE_MAG Co-authored-by: Alberto García Hierro --- CMakeLists.txt | 14 +- cmake/settings.cmake | 20 +- docs/Settings.md | 346 +++---- src/main/blackbox/blackbox.c | 21 +- src/main/common/log.c | 6 + src/main/common/time.c | 6 +- src/main/config/general_settings.c | 6 +- src/main/drivers/display.c | 3 +- src/main/fc/config.c | 14 +- src/main/fc/config.h | 4 + src/main/fc/controlrate_profile.c | 33 +- src/main/fc/controlrate_profile.h | 2 - src/main/fc/fc_core.c | 13 +- src/main/fc/fc_init.c | 4 + src/main/fc/fc_msp.c | 104 ++ src/main/fc/fc_msp_box.h | 4 +- src/main/fc/rc_controls.c | 23 +- src/main/fc/rc_modes.c | 5 + src/main/fc/settings.yaml | 906 ++++++++++-------- src/main/fc/stats.c | 9 +- src/main/flight/failsafe.c | 27 +- src/main/flight/imu.c | 20 +- src/main/flight/mixer.c | 39 +- src/main/flight/pid.c | 160 ++-- src/main/flight/pid.h | 8 + src/main/flight/pid_autotune.c | 13 +- src/main/flight/rpm_filter.c | 11 +- src/main/flight/secondary_imu.c | 26 +- src/main/flight/servos.c | 13 +- src/main/io/gps.c | 15 +- src/main/io/lights.c | 6 +- src/main/io/osd.c | 125 ++- src/main/io/osd.h | 5 +- src/main/io/osd_canvas.c | 6 +- src/main/io/osd_dji_hd.c | 8 +- src/main/io/piniobox.c | 5 +- src/main/io/piniobox.h | 6 + src/main/io/serial.c | 3 +- src/main/io/smartport_master.c | 6 +- src/main/io/vtx.c | 13 +- src/main/io/vtx_control.c | 6 +- src/main/io/vtx_smartaudio.c | 6 +- src/main/navigation/navigation.c | 150 +-- src/main/navigation/navigation.h | 4 + .../navigation/navigation_pos_estimator.c | 47 +- src/main/rx/eleres.c | 14 +- src/main/rx/rx.c | 41 +- src/main/rx/rx.h | 6 + src/main/sensors/acceleration.c | 25 +- src/main/sensors/barometer.c | 16 +- src/main/sensors/barometer.h | 21 +- src/main/sensors/battery.c | 41 +- src/main/sensors/battery.h | 9 +- src/main/sensors/compass.c | 30 +- src/main/sensors/compass.h | 7 +- src/main/sensors/esc_sensor.c | 3 +- src/main/sensors/gyro.c | 47 +- src/main/sensors/gyro.h | 6 +- src/main/sensors/opflow.c | 7 +- src/main/sensors/pitotmeter.c | 7 +- src/main/sensors/rangefinder.c | 6 +- src/main/target/ALIENFLIGHTF3/config.c | 3 - src/main/target/BETAFLIGHTF3/target.h | 4 +- src/main/target/CHEBUZZF3/target.h | 1 + src/main/target/KFC32F3_INAV/CMakeLists.txt | 1 - src/main/target/KFC32F3_INAV/hardware_setup.c | 53 - src/main/target/KFC32F3_INAV/target.c | 42 - src/main/target/KFC32F3_INAV/target.h | 146 --- src/main/target/OMNIBUS/target.h | 4 +- src/main/target/PIKOBLX/target.h | 4 +- src/main/target/RMDO/CMakeLists.txt | 1 - src/main/target/RMDO/target.c | 48 - src/main/target/RMDO/target.h | 125 --- src/main/target/SPRACINGF3NEO/CMakeLists.txt | 1 - src/main/target/SPRACINGF3NEO/config.c | 39 - src/main/target/SPRACINGF3NEO/target.c | 46 - src/main/target/SPRACINGF3NEO/target.h | 163 ---- src/main/target/common.h | 22 +- src/main/telemetry/sim.c | 21 +- src/main/telemetry/sim.h | 9 + src/main/telemetry/telemetry.c | 62 +- src/main/telemetry/telemetry.h | 13 +- src/test/unit/CMakeLists.txt | 8 +- src/utils/compiler.rb | 5 +- src/utils/settings.rb | 205 +++- 85 files changed, 1708 insertions(+), 1875 deletions(-) delete mode 100644 src/main/target/KFC32F3_INAV/CMakeLists.txt delete mode 100755 src/main/target/KFC32F3_INAV/hardware_setup.c delete mode 100755 src/main/target/KFC32F3_INAV/target.c delete mode 100755 src/main/target/KFC32F3_INAV/target.h delete mode 100644 src/main/target/RMDO/CMakeLists.txt delete mode 100644 src/main/target/RMDO/target.c delete mode 100644 src/main/target/RMDO/target.h delete mode 100644 src/main/target/SPRACINGF3NEO/CMakeLists.txt delete mode 100644 src/main/target/SPRACINGF3NEO/config.c delete mode 100755 src/main/target/SPRACINGF3NEO/target.c delete mode 100755 src/main/target/SPRACINGF3NEO/target.h mode change 100644 => 100755 src/utils/settings.rb diff --git a/CMakeLists.txt b/CMakeLists.txt index b73616735b..400e890b7f 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -22,6 +22,14 @@ endif() option(COMPILER_VERSION_CHECK "Ensure the compiler matches the expected version" ON) +include(GetGitRevisionDescription) +get_git_head_revision(GIT_REFSPEC GIT_SHA1) +string(SUBSTRING ${GIT_SHA1} 0 8 GIT_REV) + +# Load settings related functions, so the tests can use them +include(main) +include(settings) + if(TOOLCHAIN STREQUAL none) add_subdirectory(src/test) else() @@ -49,10 +57,6 @@ if(CMAKE_BUILD_TYPE STREQUAL "Release" OR CMAKE_BUILD_TYPE STREQUAL "RelWithDebI set(IS_RELEASE_BUILD ON) endif() -include(GetGitRevisionDescription) -get_git_head_revision(GIT_REFSPEC GIT_SHA1) -string(SUBSTRING ${GIT_SHA1} 0 8 GIT_REV) - set(FIRMWARE_VERSION ${PROJECT_VERSION}) option(WARNINGS_AS_ERRORS "Make all warnings into errors") @@ -64,10 +68,8 @@ set(COMMON_COMPILE_DEFINITIONS FC_VERSION_PATCH_LEVEL=${CMAKE_PROJECT_VERSION_PATCH} ) -include(settings) include(openocd) include(svd) -include(main) include(stm32) add_subdirectory(src) diff --git a/cmake/settings.cmake b/cmake/settings.cmake index 17d48168b4..5d4e4e3862 100644 --- a/cmake/settings.cmake +++ b/cmake/settings.cmake @@ -4,6 +4,7 @@ set(SETTINGS_GENERATED_H "${SETTINGS_GENERATED}.h") set(SETTINGS_FILE "${MAIN_SRC_DIR}/fc/settings.yaml") set(SETTINGS_GENERATOR "${MAIN_UTILS_DIR}/settings.rb") +include(CMakeParseArguments) function(enable_settings exe name) get_generated_files_dir(dir ${name}) @@ -15,11 +16,26 @@ function(enable_settings exe name) list(APPEND cflags ${options}) list(APPEND cflags ${includes}) list(APPEND cflags ${defs}) + + cmake_parse_arguments( + args + # Boolean arguments + "" + # Single value arguments + "OUTPUTS;SETTINGS_CXX" + # Multi-value arguments + "" + # Start parsing after the known arguments + ${ARGN} + ) + + set(output ${dir}/${SETTINGS_GENERATED_H} ${dir}/${SETTINGS_GENERATED_C}) add_custom_command( - OUTPUT ${dir}/${SETTINGS_GENERATED_H} ${dir}/${SETTINGS_GENERATED_C} + OUTPUT ${output} COMMAND - ${CMAKE_COMMAND} -E env CFLAGS="${cflags}" TARGET=${name} PATH=$ENV{PATH} + ${CMAKE_COMMAND} -E env CFLAGS="${cflags}" TARGET=${name} PATH=$ENV{PATH} SETTINGS_CXX=${args_SETTINGS_CXX} ${RUBY_EXECUTABLE} ${SETTINGS_GENERATOR} ${MAIN_DIR} ${SETTINGS_FILE} -o "${dir}" DEPENDS ${SETTINGS_GENERATOR} ${SETTINGS_FILE} ) + set(${args_OUTPUTS} ${output} PARENT_SCOPE) endfunction() diff --git a/docs/Settings.md b/docs/Settings.md index e5d99e738b..a2987a8270 100644 --- a/docs/Settings.md +++ b/docs/Settings.md @@ -12,8 +12,8 @@ | acc_hardware | AUTO | Selection of acc hardware. See Wiki Sensor auto detect and hardware failure detection for more info | | acc_lpf_hz | 15 | Software-based filter to remove mechanical vibrations from the accelerometer measurements. Value is cutoff frequency (Hz). For larger frames with bigger props set to lower value. | | acc_lpf_type | BIQUAD | Specifies the type of the software LPF of the acc signals. BIQUAD gives better filtering and more delay, PT1 less filtering and less delay, so use only on clean builds. | -| acc_notch_cutoff | | | -| acc_notch_hz | | | +| acc_notch_cutoff | 1 | | +| acc_notch_hz | 0 | | | accgain_x | 4096 | Calculated value after '6 position avanced calibration'. Uncalibrated value is 4096. See Wiki page. | | accgain_y | 4096 | Calculated value after '6 position avanced calibration'. Uncalibrated value is 4096. See Wiki page. | | accgain_z | 4096 | Calculated value after '6 position avanced calibration'. Uncalibrated value is 4096. See Wiki page. | @@ -22,17 +22,17 @@ | acczero_z | 0 | Calculated value after '6 position avanced calibration'. See Wiki page. | | airmode_throttle_threshold | 1300 | Defines airmode THROTTLE activation threshold when `airmode_type` **THROTTLE_THRESHOLD** is used | | airmode_type | STICK_CENTER | Defines the Airmode state handling type. Default **STICK_CENTER** is the classical approach in which Airmode is always active if enabled, but when the throttle is low and ROLL/PITCH/YAW sticks are centered, Iterms is not allowed to grow (ANTI_WINDUP). **THROTTLE_THRESHOLD** is the Airmode behavior known from Betaflight. In this mode, Airmode is active as soon THROTTLE position is above `airmode_throttle_threshold` and stays active until disarm. ANTI_WINDUP is never triggered. For small Multirotors (up to 7-inch propellers) it is suggested to switch to **THROTTLE_THRESHOLD** since it keeps full stabilization no matter what pilot does with the sticks. Fixed Wings always use **STICK_CENTER_ONCE** or **STICK_CENTER** modes. | -| airspeed_adc_channel | 0 | ADC channel to use for analog pitot tube (airspeed) sensor. If board doesn't have a dedicated connector for analog airspeed sensor will default to 0 | +| airspeed_adc_channel | :target | ADC channel to use for analog pitot tube (airspeed) sensor. If board doesn't have a dedicated connector for analog airspeed sensor will default to 0 | | align_acc | DEFAULT | When running on non-default hardware or adding support for new sensors/sensor boards, these values are used for sensor orientation. When carefully understood, these values can also be used to rotate (in 90deg steps) or flip the board. Possible values are: DEFAULT, CW0_DEG, CW90_DEG, CW180_DEG, CW270_DEG, CW0_DEG_FLIP, CW90_DEG_FLIP, CW180_DEG_FLIP, CW270_DEG_FLIP. | -| align_board_pitch | 0 | Arbitrary board rotation in deci-degrees (0.1 degree), to allow mounting it sideways / upside down / rotated etc | -| align_board_roll | 0 | Arbitrary board rotation in deci-degrees (0.1 degree), to allow mounting it sideways / upside down / rotated etc | -| align_board_yaw | 0 | Arbitrary board rotation in deci-degrees (0.1 degree), to allow mounting it sideways / upside down / rotated etc | +| align_board_pitch | :zero | Arbitrary board rotation in deci-degrees (0.1 degree), to allow mounting it sideways / upside down / rotated etc | +| align_board_roll | :zero | Arbitrary board rotation in deci-degrees (0.1 degree), to allow mounting it sideways / upside down / rotated etc | +| align_board_yaw | :zero | Arbitrary board rotation in deci-degrees (0.1 degree), to allow mounting it sideways / upside down / rotated etc | | align_gyro | DEFAULT | When running on non-default hardware or adding support for new sensors/sensor boards, these values are used for sensor orientation. When carefully understood, these values can also be used to rotate (in 90deg steps) or flip the board. Possible values are: DEFAULT, CW0_DEG, CW90_DEG, CW180_DEG, CW270_DEG, CW0_DEG_FLIP, CW90_DEG_FLIP, CW180_DEG_FLIP, CW270_DEG_FLIP. | | align_mag | DEFAULT | When running on non-default hardware or adding support for new sensors/sensor boards, these values are used for sensor orientation. When carefully understood, these values can also be used to rotate (in 90deg steps) or flip the board. Possible values are: DEFAULT, CW0_DEG, CW90_DEG, CW180_DEG, CW270_DEG, CW0_DEG_FLIP, CW90_DEG_FLIP, CW180_DEG_FLIP, CW270_DEG_FLIP. | | align_mag_pitch | 0 | Same as align_mag_roll, but for the pitch axis. | | align_mag_roll | 0 | Set the external mag alignment on the roll axis (in 0.1 degree steps). If this value is non-zero, the compass is assumed to be externally mounted and both the board and on-board compass alignent (align_mag) are ignored. See also align_mag_pitch and align_mag_yaw. | | align_mag_yaw | 0 | Same as align_mag_roll, but for the yaw axis. | -| align_opflow | 5 | Optical flow module alignment (default CW0_DEG_FLIP) | +| align_opflow | CW0FLIP | Optical flow module alignment (default CW0_DEG_FLIP) | | alt_hold_deadband | 50 | Defines the deadband of throttle during alt_hold [r/c points] | | antigravity_accelerator | 1 | | | antigravity_cutoff_lpf_hz | 15 | Antigravity cutoff frequenct for Throtte filter. Antigravity is based on the difference between actual and filtered throttle input. The bigger is the difference, the bigger Antigravity gain | @@ -40,73 +40,73 @@ | applied_defaults | 0 | Internal (configurator) hint. Should not be changed manually | | baro_cal_tolerance | 150 | Baro calibration tolerance in cm. The default should allow the noisiest baro to complete calibration [cm]. | | baro_hardware | AUTO | Selection of baro hardware. See Wiki Sensor auto detect and hardware failure detection for more info | -| baro_median_filter | ON | 3-point median filtering for barometer readouts. No reason to change this setting | +| baro_median_filter | True | 3-point median filtering for barometer readouts. No reason to change this setting | | bat_cells | 0 | Number of cells of the battery (0 = autodetect), see battery documentation. 7S, 9S and 11S batteries cannot be autodetected. | | bat_voltage_src | RAW | Chose between raw and sag compensated battery voltage to use for battery alarms and telemetry. Possible values are `RAW` and `SAG_COMP` | | battery_capacity | 0 | Set the battery capacity in mAh or mWh (see `battery_capacity_unit`). Used to calculate the remaining battery capacity. | | battery_capacity_critical | 0 | If the remaining battery capacity goes below this threshold the battery is considered empty and the beeper will emit long beeps. | | battery_capacity_unit | MAH | Unit used for `battery_capacity`, `battery_capacity_warning` and `battery_capacity_critical` [MAH/MWH] (milliAmpere hour / milliWatt hour). | | battery_capacity_warning | 0 | If the remaining battery capacity goes below this threshold the beeper will emit short beeps and the relevant OSD items will blink. | -| blackbox_device | SPIFLASH | Selection of where to write blackbox data | +| blackbox_device | :target | Selection of where to write blackbox data | | blackbox_rate_denom | 1 | Blackbox logging rate denominator. See blackbox_rate_num. | | blackbox_rate_num | 1 | Blackbox logging rate numerator. Use num/denom settings to decide if a frame should be logged, allowing control of the portion of logged loop iterations | -| cpu_underclock | OFF | This option is only available on certain architectures (F3 CPUs at the moment). It makes CPU clock lower to reduce interference to long-range RC systems working at 433MHz | +| cpu_underclock | False | This option is only available on certain architectures (F3 CPUs at the moment). It makes CPU clock lower to reduce interference to long-range RC systems working at 433MHz | | cruise_power | 0 | Power draw at cruise throttle used for remaining flight time/distance estimation in 0.01W unit | -| current_adc_channel | - | ADC channel to use for analog current sensor input. Defaults to board CURRENT sensor input (if available). 0 = disabled | -| current_meter_offset | 0 | This sets the output offset voltage of the current sensor in millivolts. | -| current_meter_scale | 400 | This sets the output voltage to current scaling for the current sensor in 0.1 mV/A steps. 400 is 40mV/A such as the ACS756 sensor outputs. 183 is the setting for the uberdistro with a 0.25mOhm shunt. | +| current_adc_channel | :target | ADC channel to use for analog current sensor input. Defaults to board CURRENT sensor input (if available). 0 = disabled | +| current_meter_offset | :target | This sets the output offset voltage of the current sensor in millivolts. | +| current_meter_scale | :target | This sets the output voltage to current scaling for the current sensor in 0.1 mV/A steps. 400 is 40mV/A such as the ACS756 sensor outputs. 183 is the setting for the uberdistro with a 0.25mOhm shunt. | | current_meter_type | ADC | ADC , VIRTUAL, NONE. The virtual current sensor, once calibrated, estimates the current value from throttle position. | -| d_boost_factor | | | -| d_boost_gyro_delta_lpf_hz | | | -| d_boost_max_at_acceleration | | | +| d_boost_factor | 1.25 | | +| d_boost_gyro_delta_lpf_hz | 80 | | +| d_boost_max_at_acceleration | 7500 | | | deadband | 5 | These are values (in us) by how much RC input can be different before it's considered valid. For transmitters with jitter on outputs, this value can be increased. Defaults are zero, but can be increased up to 10 or so if rc inputs twitch while idle. | | debug_mode | NONE | Defines debug values exposed in debug variables (developer / debugging setting) | -| disarm_kill_switch | ON | Disarms the motors independently of throttle value. Setting to OFF reverts to the old behaviour of disarming only when the throttle is low. Only applies when arming and disarming with an AUX channel. | -| display_force_sw_blink | OFF | OFF = OSD hardware blink / ON = OSD software blink. If OSD warning text/values are invisible, try setting this to ON | +| disarm_kill_switch | True | Disarms the motors independently of throttle value. Setting to OFF reverts to the old behaviour of disarming only when the throttle is low. Only applies when arming and disarming with an AUX channel. | +| display_force_sw_blink | False | OFF = OSD hardware blink / ON = OSD software blink. If OSD warning text/values are invisible, try setting this to ON | | dji_esc_temp_source | ESC | Re-purpose the ESC temperature field for IMU/BARO temperature | | dji_speed_source | GROUND | Sets the speed type displayed by the DJI OSD: GROUND, 3D, AIR | -| dji_use_name_for_messages | ON | Re-purpose the craft name field for messages. Replace craft name with :WTSED for Warnings|Throttle|Speed|Efficiency|Trip distance | -| dji_workarounds | | Enables workarounds for different versions of MSP protocol used | +| dji_use_name_for_messages | True | Re-purpose the craft name field for messages. Replace craft name with :WTSED for Warnings|Throttle|Speed|Efficiency|Trip distance | +| dji_workarounds | 1 | Enables workarounds for different versions of MSP protocol used | | dterm_lpf2_hz | 0 | Cutoff frequency for stage 2 D-term low pass filter | -| dterm_lpf2_type | `BIQUAD` | Defines the type of stage 1 D-term LPF filter. Possible values: `PT1`, `BIQUAD`. `PT1` offers faster filter response while `BIQUAD` better attenuation. | +| dterm_lpf2_type | BIQUAD | Defines the type of stage 1 D-term LPF filter. Possible values: `PT1`, `BIQUAD`. `PT1` offers faster filter response while `BIQUAD` better attenuation. | | dterm_lpf_hz | 40 | Dterm low pass filter cutoff frequency. Default setting is very conservative and small multirotors should use higher value between 80 and 100Hz. 80 seems like a gold spot for 7-inch builds while 100 should work best with 5-inch machines. If motors are getting too hot, lower the value | -| dterm_lpf_type | `BIQUAD` | Defines the type of stage 1 D-term LPF filter. Possible values: `PT1`, `BIQUAD`. `PT1` offers faster filter response while `BIQUAD` better attenuation. | -| dynamic_gyro_notch_enabled | `OFF` | Enable/disable dynamic gyro notch also known as Matrix Filter | +| dterm_lpf_type | BIQUAD | Defines the type of stage 1 D-term LPF filter. Possible values: `PT1`, `BIQUAD`. `PT1` offers faster filter response while `BIQUAD` better attenuation. | +| dynamic_gyro_notch_enabled | False | Enable/disable dynamic gyro notch also known as Matrix Filter | | dynamic_gyro_notch_min_hz | 150 | Minimum frequency for dynamic notches. Default value of `150` works best with 5" multirors. Should be lowered with increased size of propellers. Values around `100` work fine on 7" drones. 10" can go down to `60` - `70` | | dynamic_gyro_notch_q | 120 | Q factor for dynamic notches | -| dynamic_gyro_notch_range | `MEDIUM` | Range for dynamic gyro notches. `MEDIUM` for 5", `HIGH` for 3" and `MEDIUM`/`LOW` for 7" and bigger propellers | -| eleres_freq | | | -| eleres_loc_delay | | | -| eleres_loc_en | | | -| eleres_loc_power | | | -| eleres_signature | | | -| eleres_telemetry_en | | | -| eleres_telemetry_power | | | -| esc_sensor_listen_only | OFF | Enable when BLHeli32 Auto Telemetry function is used. Disable in every other case | +| dynamic_gyro_notch_range | MEDIUM | Range for dynamic gyro notches. `MEDIUM` for 5", `HIGH` for 3" and `MEDIUM`/`LOW` for 7" and bigger propellers | +| eleres_freq | 435 | | +| eleres_loc_delay | 240 | | +| eleres_loc_en | False | | +| eleres_loc_power | 7 | | +| eleres_signature | :zero | | +| eleres_telemetry_en | False | | +| eleres_telemetry_power | 7 | | +| esc_sensor_listen_only | False | Enable when BLHeli32 Auto Telemetry function is used. Disable in every other case | | failsafe_delay | 5 | Time in deciseconds to wait before activating failsafe when signal is lost. See [Failsafe documentation](Failsafe.md#failsafe_delay). | | failsafe_fw_pitch_angle | 100 | Amount of dive/climb when `SET-THR` failsafe is active on a fixed-wing machine. In 1/10 deg (deci-degrees). Negative values = climb | | failsafe_fw_roll_angle | -200 | Amount of banking when `SET-THR` failsafe is active on a fixed-wing machine. In 1/10 deg (deci-degrees). Negative values = left roll | | failsafe_fw_yaw_rate | -45 | Requested yaw rate to execute when `SET-THR` failsafe is active on a fixed-wing machine. In deg/s. Negative values = left turn | -| failsafe_lights | ON | Enable or disable the lights when the `FAILSAFE` flight mode is enabled. The target needs to be compiled with `USE_LIGHTS` [ON/OFF]. | +| failsafe_lights | True | Enable or disable the lights when the `FAILSAFE` flight mode is enabled. The target needs to be compiled with `USE_LIGHTS` [ON/OFF]. | | failsafe_lights_flash_on_time | 100 | Flash lights ON time in milliseconds when `failsafe_lights` is ON and `FAILSAFE` flight mode is enabled. [20-65535]. | | failsafe_lights_flash_period | 1000 | Time in milliseconds between two flashes when `failsafe_lights` is ON and `FAILSAFE` flight mode is enabled [40-65535]. | | failsafe_min_distance | 0 | If failsafe happens when craft is closer than this distance in centimeters from home, failsafe will not execute regular failsafe_procedure, but will execute procedure specified in failsafe_min_distance_procedure instead. 0 = Normal failsafe_procedure always taken. | | failsafe_min_distance_procedure | DROP | What failsafe procedure to initiate in Stage 2 when craft is closer to home than failsafe_min_distance. See [Failsafe documentation](Failsafe.md#failsafe_throttle). | -| failsafe_mission | ON | If set to `OFF` the failsafe procedure won't be triggered and the mission will continue if the FC is in WP (automatic mission) mode | +| failsafe_mission | True | If set to `OFF` the failsafe procedure won't be triggered and the mission will continue if the FC is in WP (automatic mission) mode | | failsafe_off_delay | 200 | Time in deciseconds to wait before turning off motors when failsafe is activated. 0 = No timeout. See [Failsafe documentation](Failsafe.md#failsafe_off_delay). | | failsafe_procedure | SET-THR | What failsafe procedure to initiate in Stage 2. See [Failsafe documentation](Failsafe.md#failsafe_throttle). | | failsafe_recovery_delay | 5 | Time in deciseconds to wait before aborting failsafe when signal is recovered. See [Failsafe documentation](Failsafe.md#failsafe_recovery_delay). | | failsafe_stick_threshold | 50 | Threshold for stick motion to consider failsafe condition resolved. If non-zero failsafe won't clear even if RC link is restored - you have to move sticks to exit failsafe. | | failsafe_throttle | 1000 | Throttle level used for landing when failsafe is enabled. See [Failsafe documentation](Failsafe.md#failsafe_throttle). | -| failsafe_throttle_low_delay | 100 | If failsafe activated when throttle is low for this much time - bypass failsafe and disarm, in 10th of seconds. 0 = No timeout | -| fixed_wing_auto_arm | OFF | Auto-arm fixed wing aircraft on throttle above min_check, and disarming with stick commands are disabled, so power cycle is required to disarm. Requires enabled motorstop and no arm switch configured. | +| failsafe_throttle_low_delay | 0 | If failsafe activated when throttle is low for this much time - bypass failsafe and disarm, in 10th of seconds. 0 = No timeout | +| fixed_wing_auto_arm | False | Auto-arm fixed wing aircraft on throttle above min_check, and disarming with stick commands are disabled, so power cycle is required to disarm. Requires enabled motorstop and no arm switch configured. | | flaperon_throw_offset | 200 | Defines throw range in us for both ailerons that will be passed to servo mixer via input source 14 (`FEATURE FLAPS`) when FLAPERON mode is activated. | | flip_over_after_crash_power_factor | 65 | flip over after crash power factor | -| fpv_mix_degrees | | | +| fpv_mix_degrees | 0 | | | frsky_coordinates_format | 0 | D-Series telemetry only: FRSKY_FORMAT_DMS (default), FRSKY_FORMAT_NMEA | -| frsky_default_latitude | 0.000 | D-Series telemetry only: OpenTX needs a valid set of coordinates to show compass value. A fake value defined in this setting is sent while no fix is acquired. | -| frsky_default_longitude | 0.000 | D-Series telemetry only: OpenTX needs a valid set of coordinates to show compass value. A fake value defined in this setting is sent while no fix is acquired. | -| frsky_pitch_roll | OFF | S.Port and D-Series telemetry: Send pitch and roll degrees*10 instead of raw accelerometer data | +| frsky_default_latitude | 0 | D-Series telemetry only: OpenTX needs a valid set of coordinates to show compass value. A fake value defined in this setting is sent while no fix is acquired. | +| frsky_default_longitude | 0 | D-Series telemetry only: OpenTX needs a valid set of coordinates to show compass value. A fake value defined in this setting is sent while no fix is acquired. | +| frsky_pitch_roll | False | S.Port and D-Series telemetry: Send pitch and roll degrees*10 instead of raw accelerometer data | | frsky_unit | METRIC | Not used? [METRIC/IMPERIAL] | | frsky_vfas_precision | 0 | D-Series telemetry only: Set to 1 to send raw VBat value in 0.1V resolution for receivers that can handle it, or 0 (default) to use the standard method | | fw_autotune_ff_to_i_tc | 600 | FF to I time (defines time for I to reach the same level of response as FF) [ms] | @@ -139,27 +139,27 @@ | fw_turn_assist_pitch_gain | 1 | Gain required to keep constant pitch angle during coordinated turns (in TURN_ASSIST mode). Value significantly different from 1.0 indicates a problem with the airspeed calibration (if present) or value of `fw_reference_airspeed` parameter | | fw_turn_assist_yaw_gain | 1 | Gain required to keep the yaw rate consistent with the turn rate for a coordinated turn (in TURN_ASSIST mode). Value significantly different from 1.0 indicates a problem with the airspeed calibration (if present) or value of `fw_reference_airspeed` parameter | | fw_yaw_iterm_freeze_bank_angle | 0 | Yaw Iterm is frozen when bank angle is above this threshold [degrees]. This solves the problem of the rudder counteracting turns by partially disabling yaw stabilization when making banked turns. Setting to 0 (the default) disables this feature. Only applies when autopilot is not active and TURN ASSIST is disabled. | -| gps_auto_baud | ON | Automatic configuration of GPS baudrate(The specified baudrate in configured in ports will be used) when used with UBLOX GPS. When used with NAZA/DJI it will automatic detect GPS baudrate and change to it, ignoring the selected baudrate set in ports | -| gps_auto_config | ON | Enable automatic configuration of UBlox GPS receivers. | +| gps_auto_baud | True | Automatic configuration of GPS baudrate(The specified baudrate in configured in ports will be used) when used with UBLOX GPS. When used with NAZA/DJI it will automatic detect GPS baudrate and change to it, ignoring the selected baudrate set in ports | +| gps_auto_config | True | Enable automatic configuration of UBlox GPS receivers. | | gps_dyn_model | AIR_1G | GPS navigation model: Pedestrian, Air_1g, Air_4g. Default is AIR_1G. Use pedestrian with caution, can cause flyaways with fast flying. | | gps_min_sats | 6 | Minimum number of GPS satellites in view to acquire GPS_FIX and consider GPS position valid. Some GPS receivers appeared to be very inaccurate with low satellite count. | | gps_provider | UBLOX | Which GPS protocol to be used, note that UBLOX is 5Hz and UBLOX7 is 10Hz (M8N). | | gps_sbas_mode | NONE | Which SBAS mode to be used | -| gps_ublox_use_galileo | OFF | Enable use of Galileo satellites. This is at the expense of other regional constellations, so benefit may also be regional. Requires M8N and Ublox firmware 3.x (or later) [OFF/ON]. | +| gps_ublox_use_galileo | False | Enable use of Galileo satellites. This is at the expense of other regional constellations, so benefit may also be regional. Requires M8N and Ublox firmware 3.x (or later) [OFF/ON]. | | gyro_dyn_lpf_curve_expo | 5 | Expo value for the throttle-to-frequency mapping for Dynamic LPF | | gyro_dyn_lpf_max_hz | 500 | Maximum frequency of the gyro Dynamic LPF | | gyro_dyn_lpf_min_hz | 200 | Minimum frequency of the gyro Dynamic LPF | | gyro_hardware_lpf | 256HZ | Hardware lowpass filter for gyro. This value should never be changed without a very strong reason! If you have to set gyro lpf below 256HZ, it means the frame is vibrating too much, and that should be fixed first. | | gyro_lpf_hz | 60 | Software-based filter to remove mechanical vibrations from the gyro signal. Value is cutoff frequency (Hz). For larger frames with bigger props set to lower value. | | gyro_lpf_type | BIQUAD | Specifies the type of the software LPF of the gyro signals. BIQUAD gives better filtering and more delay, PT1 less filtering and less delay, so use only on clean builds. | -| gyro_notch_cutoff | | | -| gyro_notch_hz | | | +| gyro_notch_cutoff | 1 | | +| gyro_notch_hz | 0 | | | gyro_stage2_lowpass_hz | 0 | Software based second stage lowpass filter for gyro. Value is cutoff frequency (Hz) | -| gyro_stage2_lowpass_type | `BIQUAD` | Defines the type of stage 2 gyro LPF filter. Possible values: `PT1`, `BIQUAD`. `PT1` offers faster filter response while `BIQUAD` better attenuation. | -| gyro_sync | OFF | This option enables gyro_sync feature. In this case the loop will be synced to gyro refresh rate. Loop will always wait for the newest gyro measurement. Maximum gyro refresh rate is determined by gyro_hardware_lpf | -| gyro_to_use | | | -| gyro_use_dyn_lpf | OFF | Use Dynamic LPF instead of static gyro stage1 LPF. Dynamic Gyro LPF updates gyro LPF based on the throttle position. | -| has_flaps | OFF | Defines is UAV is capable of having flaps. If ON and AIRPLANE `platform_type` is used, **FLAPERON** flight mode will be available for the pilot | +| gyro_stage2_lowpass_type | BIQUAD | Defines the type of stage 2 gyro LPF filter. Possible values: `PT1`, `BIQUAD`. `PT1` offers faster filter response while `BIQUAD` better attenuation. | +| gyro_sync | True | This option enables gyro_sync feature. In this case the loop will be synced to gyro refresh rate. Loop will always wait for the newest gyro measurement. Maximum gyro refresh rate is determined by gyro_hardware_lpf | +| gyro_to_use | 0 | | +| gyro_use_dyn_lpf | False | Use Dynamic LPF instead of static gyro stage1 LPF. Dynamic Gyro LPF updates gyro LPF based on the throttle position. | +| has_flaps | False | Defines is UAV is capable of having flaps. If ON and AIRPLANE `platform_type` is used, **FLAPERON** flight mode will be available for the pilot | | heading_hold_rate_limit | 90 | This setting limits yaw rotation rate that HEADING_HOLD controller can request from PID inner loop controller. It is independent from manual yaw rate and used only when HEADING_HOLD flight mode is enabled by pilot, RTH or WAYPOINT modes. | | hott_alarm_sound_interval | 5 | Battery alarm delay in seconds for Hott telemetry | | i2c_speed | 400KHZ | This setting controls the clock speed of I2C bus. 400KHZ is the default that most setups are able to use. Some noise-free setups may be overclocked to 800KHZ. Some sensor chips or setups with long wires may work unreliably at 400KHZ - user can try lowering the clock speed to 200KHZ or even 100KHZ. User need to bear in mind that lower clock speeds might require higher looptimes (lower looptime rate) | @@ -177,65 +177,65 @@ | imu2_hardware | NONE | Selection of a Secondary IMU hardware type. NONE disables this functionality | | imu2_radius_acc | 0 | Secondary IMU MAG calibration data | | imu2_radius_mag | 0 | Secondary IMU MAG calibration data | -| imu2_use_for_osd_ahi | OFF | If set to ON, Secondary IMU data will be used for Analog OSD Artificial Horizon | -| imu2_use_for_osd_heading | OFF | If set to ON, Secondary IMU data will be used for Analog OSD heading | -| imu2_use_for_stabilized | OFF | If set to ON, Secondary IMU data will be used for Angle, Horizon and all other modes that control attitude (PosHold, WP, RTH) | +| imu2_use_for_osd_ahi | False | If set to ON, Secondary IMU data will be used for Analog OSD Artificial Horizon | +| imu2_use_for_osd_heading | False | If set to ON, Secondary IMU data will be used for Analog OSD heading | +| imu2_use_for_stabilized | False | If set to ON, Secondary IMU data will be used for Angle, Horizon and all other modes that control attitude (PosHold, WP, RTH) | | imu_acc_ignore_rate | 0 | Total gyro rotation rate threshold [deg/s] to consider accelerometer trustworthy on airplanes | | imu_acc_ignore_slope | 0 | Half-width of the interval to gradually reduce accelerometer weight. Centered at `imu_acc_ignore_rate` (exactly 50% weight) | | imu_dcm_ki | 50 | Inertial Measurement Unit KI Gain for accelerometer measurements | | imu_dcm_ki_mag | 0 | Inertial Measurement Unit KI Gain for compass measurements | | imu_dcm_kp | 2500 | Inertial Measurement Unit KP Gain for accelerometer measurements | | imu_dcm_kp_mag | 10000 | Inertial Measurement Unit KP Gain for compass measurements | -| inav_allow_dead_reckoning | OFF | Defines if inav will dead-reckon over short GPS outages. May also be useful for indoors OPFLOW navigation | -| inav_auto_mag_decl | ON | Automatic setting of magnetic declination based on GPS position. When used manual magnetic declination is ignored. | -| inav_baro_epv | 100.000 | Uncertainty value for barometric sensor [cm] | +| inav_allow_dead_reckoning | False | Defines if inav will dead-reckon over short GPS outages. May also be useful for indoors OPFLOW navigation | +| inav_auto_mag_decl | True | Automatic setting of magnetic declination based on GPS position. When used manual magnetic declination is ignored. | +| inav_baro_epv | 100 | Uncertainty value for barometric sensor [cm] | | inav_gravity_cal_tolerance | 5 | Unarmed gravity calibration tolerance level. Won't finish the calibration until estimated gravity error falls below this value. | -| inav_max_eph_epv | 1000.000 | Maximum uncertainty value until estimated position is considered valid and is used for navigation [cm] | +| inav_max_eph_epv | 1000 | Maximum uncertainty value until estimated position is considered valid and is used for navigation [cm] | | inav_max_surface_altitude | 200 | Max allowed altitude for surface following mode. [cm] | | inav_reset_altitude | FIRST_ARM | Defines when relative estimated altitude is reset to zero. Variants - `NEVER` (once reference is acquired it's used regardless); `FIRST_ARM` (keep altitude at zero until firstly armed), `EACH_ARM` (altitude is reset to zero on each arming) | | inav_reset_home | FIRST_ARM | Allows to chose when the home position is reset. Can help prevent resetting home position after accidental mid-air disarm. Possible values are: NEVER, FIRST_ARM and EACH_ARM | -| inav_use_gps_no_baro | | | -| inav_use_gps_velned | ON | Defined if iNav should use velocity data provided by GPS module for doing position and speed estimation. If set to OFF iNav will fallback to calculating velocity from GPS coordinates. Using native velocity data may improve performance on some GPS modules. Some GPS modules introduce significant delay and using native velocity may actually result in much worse performance. | -| inav_w_acc_bias | 0.010 | Weight for accelerometer drift estimation | -| inav_w_xy_flow_p | | | -| inav_w_xy_flow_v | | | -| inav_w_xy_gps_p | 1.000 | Weight of GPS coordinates in estimated UAV position and speed. | -| inav_w_xy_gps_v | 2.000 | Weight of GPS velocity data in estimated UAV speed | -| inav_w_xy_res_v | 0.500 | Decay coefficient for estimated velocity when GPS reference for position is lost | -| inav_w_xyz_acc_p | | | -| inav_w_z_baro_p | 0.350 | Weight of barometer measurements in estimated altitude and climb rate | -| inav_w_z_gps_p | 0.200 | Weight of GPS altitude measurements in estimated altitude. Setting is used only of airplanes | -| inav_w_z_gps_v | 0.500 | Weight of GPS climb rate measurements in estimated climb rate. Setting is used on both airplanes and multirotors. If GPS doesn't support native climb rate reporting (i.e. NMEA GPS) you may consider setting this to zero | -| inav_w_z_res_v | 0.500 | Decay coefficient for estimated climb rate when baro/GPS reference for altitude is lost | -| inav_w_z_surface_p | | | -| inav_w_z_surface_v | | | +| inav_use_gps_no_baro | False | | +| inav_use_gps_velned | True | Defined if iNav should use velocity data provided by GPS module for doing position and speed estimation. If set to OFF iNav will fallback to calculating velocity from GPS coordinates. Using native velocity data may improve performance on some GPS modules. Some GPS modules introduce significant delay and using native velocity may actually result in much worse performance. | +| inav_w_acc_bias | 0.01 | Weight for accelerometer drift estimation | +| inav_w_xy_flow_p | 1.0 | | +| inav_w_xy_flow_v | 2.0 | | +| inav_w_xy_gps_p | 1.0 | Weight of GPS coordinates in estimated UAV position and speed. | +| inav_w_xy_gps_v | 2.0 | Weight of GPS velocity data in estimated UAV speed | +| inav_w_xy_res_v | 0.5 | Decay coefficient for estimated velocity when GPS reference for position is lost | +| inav_w_xyz_acc_p | 1.0 | | +| inav_w_z_baro_p | 0.35 | Weight of barometer measurements in estimated altitude and climb rate | +| inav_w_z_gps_p | 0.2 | Weight of GPS altitude measurements in estimated altitude. Setting is used only of airplanes | +| inav_w_z_gps_v | 0.1 | Weight of GPS climb rate measurements in estimated climb rate. Setting is used on both airplanes and multirotors. If GPS doesn't support native climb rate reporting (i.e. NMEA GPS) you may consider setting this to zero | +| inav_w_z_res_v | 0.5 | Decay coefficient for estimated climb rate when baro/GPS reference for altitude is lost | +| inav_w_z_surface_p | 3.5 | | +| inav_w_z_surface_v | 6.1 | | | iterm_windup | 50 | Used to prevent Iterm accumulation on during maneuvers. Iterm will be dampened when motors are reaching it's limit (when requested motor correction range is above percentage specified by this parameter) | -| ledstrip_visual_beeper | OFF | | -| log_level | `ERROR` | Defines serial debugging log level. See `docs/development/serial_printf_debugging.md` for usage. | +| ledstrip_visual_beeper | False | | +| log_level | ERROR | Defines serial debugging log level. See `docs/development/serial_printf_debugging.md` for usage. | | log_topics | 0 | Defines serial debugging log topic. See `docs/development/serial_printf_debugging.md` for usage. | | looptime | 1000 | This is the main loop time (in us). Changing this affects PID effect with some PID controllers (see PID section for details). A very conservative value of 3500us/285Hz should work for everyone. Setting it to zero does not limit loop time, so it will go as fast as possible. | | ltm_update_rate | NORMAL | Defines the LTM update rate (use of bandwidth [NORMAL/MEDIUM/SLOW]). See Telemetry.md, LTM section for details. | | mag_calibration_time | 30 | Adjust how long time the Calibration of mag will last. | | mag_declination | 0 | Current location magnetic declination in format. For example, -6deg 37min = -637 for Japan. Leading zero in ddd not required. Get your local magnetic declination here: http://magnetic-declination.com/ . Not in use if inav_auto_mag_decl is turned on and you acquire valid GPS fix. | | mag_hardware | AUTO | Selection of mag hardware. See Wiki Sensor auto detect and hardware failure detection for more info | -| mag_to_use | | Allow to chose between built-in and external compass sensor if they are connected to separate buses. Currently only for REVO target | +| mag_to_use | 0 | Allow to chose between built-in and external compass sensor if they are connected to separate buses. Currently only for REVO target | | maggain_x | 1024 | Magnetometer calibration X gain. If 1024, no calibration or calibration failed | | maggain_y | 1024 | Magnetometer calibration Y gain. If 1024, no calibration or calibration failed | | maggain_z | 1024 | Magnetometer calibration Z gain. If 1024, no calibration or calibration failed | -| magzero_x | 0 | Magnetometer calibration X offset. If its 0 none offset has been applied and calibration is failed. | -| magzero_y | 0 | Magnetometer calibration Y offset. If its 0 none offset has been applied and calibration is failed. | -| magzero_z | 0 | Magnetometer calibration Z offset. If its 0 none offset has been applied and calibration is failed. | +| magzero_x | :zero | Magnetometer calibration X offset. If its 0 none offset has been applied and calibration is failed. | +| magzero_y | :zero | Magnetometer calibration Y offset. If its 0 none offset has been applied and calibration is failed. | +| magzero_z | :zero | Magnetometer calibration Z offset. If its 0 none offset has been applied and calibration is failed. | | manual_pitch_rate | 100 | Servo travel multiplier for the PITCH axis in `MANUAL` flight mode [0-100]% | | manual_rc_expo | 70 | Exposition value used for the PITCH/ROLL axes by the `MANUAL` flight mode [0-100] | | manual_rc_yaw_expo | 20 | Exposition value used for the YAW axis by the `MANUAL` flight mode [0-100] | | manual_roll_rate | 100 | Servo travel multiplier for the ROLL axis in `MANUAL` flight mode [0-100]% | | manual_yaw_rate | 100 | Servo travel multiplier for the YAW axis in `MANUAL` flight mode [0-100]% | -| mavlink_ext_status_rate | | | -| mavlink_extra1_rate | | | -| mavlink_extra2_rate | | | -| mavlink_extra3_rate | | | -| mavlink_pos_rate | | | -| mavlink_rc_chan_rate | | | +| mavlink_ext_status_rate | 2 | | +| mavlink_extra1_rate | 10 | | +| mavlink_extra2_rate | 2 | | +| mavlink_extra3_rate | 1 | | +| mavlink_pos_rate | 2 | | +| mavlink_rc_chan_rate | 5 | | | max_angle_inclination_pit | 300 | Maximum inclination in level (angle) mode (PITCH axis). 100=10° | | max_angle_inclination_rll | 300 | Maximum inclination in level (angle) mode (ROLL axis). 100=10° | | max_check | 1900 | These are min/max values (in us) which, when a channel is smaller (min) or larger (max) than the value will activate various RC commands, such as arming, or stick configuration. Normally, every RC channel should be set so that min = 1000us, max = 2000us. On most transmitters this usually means 125% endpoints. Default check values are 100us above/below this value. | @@ -252,8 +252,8 @@ | mc_i_pitch | 30 | Multicopter rate stabilisation I-gain for PITCH | | mc_i_roll | 30 | Multicopter rate stabilisation I-gain for ROLL | | mc_i_yaw | 45 | Multicopter rate stabilisation I-gain for YAW | -| mc_iterm_relax | | | -| mc_iterm_relax_cutoff | | | +| mc_iterm_relax | RP | | +| mc_iterm_relax_cutoff | 15 | | | mc_p_level | 20 | Multicopter attitude stabilisation P-gain | | mc_p_pitch | 40 | Multicopter rate stabilisation P-gain for PITCH | | mc_p_roll | 40 | Multicopter rate stabilisation P-gain for ROLL | @@ -265,19 +265,19 @@ | moron_threshold | 32 | When powering up, gyro bias is calculated. If the model is shaking/moving during this initial calibration, offsets are calculated incorrectly, and could lead to poor flying performance. This threshold means how much average gyro reading could differ before re-calibration is triggered. | | motor_accel_time | 0 | Minimum time for the motor(s) to accelerate from 0 to 100% throttle (ms) [0-1000] | | motor_decel_time | 0 | Minimum time for the motor(s) to deccelerate from 100 to 0% throttle (ms) [0-1000] | -| motor_direction_inverted | OFF | Use if you need to inverse yaw motor direction. | +| motor_direction_inverted | False | Use if you need to inverse yaw motor direction. | | motor_poles | 14 | The number of motor poles. Required to compute motor RPM | | motor_pwm_protocol | ONESHOT125 | Protocol that is used to send motor updates to ESCs. Possible values - STANDARD, ONESHOT125, ONESHOT42, MULTISHOT, DSHOT150, DSHOT300, DSHOT600, DSHOT1200, BRUSHED | | motor_pwm_rate | 400 | Output frequency (in Hz) for motor pins. Default is 400Hz for motor with motor_pwm_protocol set to STANDARD. For *SHOT (e.g. ONESHOT125) values of 1000 and 2000 have been tested by the development team and are supported. It may be possible to use higher values. For BRUSHED values of 8000 and above should be used. Setting to 8000 will use brushed mode at 8kHz switching frequency. Up to 32kHz is supported for brushed. Default is 16000 for boards with brushed motors. Note, that in brushed mode, minthrottle is offset to zero. For brushed mode, set max_throttle to 2000. | | msp_override_channels | 0 | Mask of RX channels that may be overridden by MSP `SET_RAW_RC`. Note that this requires custom firmware with `USE_RX_MSP` and `USE_MSP_RC_OVERRIDE` compile options and the `MSP RC Override` flight mode. | -| name | Empty string | Craft name | +| name | | Craft name | | nav_auto_climb_rate | 500 | Maximum climb/descent rate that UAV is allowed to reach during navigation modes. [cm/s] | | nav_auto_speed | 300 | Maximum velocity firmware is allowed in full auto modes (RTH, WP) [cm/s] [Multirotor only] | -| nav_disarm_on_landing | OFF | If set to ON, iNav disarms the FC after landing | +| nav_disarm_on_landing | False | If set to ON, iNav disarms the FC after landing | | nav_emerg_landing_speed | 500 | Rate of descent UAV will try to maintain when doing emergency descent sequence [cm/s] | | nav_extra_arming_safety | ON | If set to ON drone won't arm if no GPS fix and any navigation mode like RTH or POSHOLD is configured. ALLOW_BYPASS allows the user to momentarily disable this check by holding yaw high (left stick held at the bottom right in mode 2) when switch arming is used | -| nav_fw_allow_manual_thr_increase | OFF | Enable the possibility to manually increase the throttle in auto throttle controlled modes for fixed wing | -| nav_fw_bank_angle | 20 | Max roll angle when rolling / turning in GPS assisted modes, is also restrained by global max_angle_inclination_rll | +| nav_fw_allow_manual_thr_increase | False | Enable the possibility to manually increase the throttle in auto throttle controlled modes for fixed wing | +| nav_fw_bank_angle | 35 | Max roll angle when rolling / turning in GPS assisted modes, is also restrained by global max_angle_inclination_rll | | nav_fw_climb_angle | 20 | Max pitch angle when climbing in GPS assisted modes, is also restrained by global max_angle_inclination_pit | | nav_fw_control_smoothness | 0 | How smoothly the autopilot controls the airplane to correct the navigation error | | nav_fw_cruise_speed | 0 | Speed for the plane/wing at cruise throttle used for remaining flight time/distance estimation in cm/s | @@ -289,7 +289,7 @@ | nav_fw_launch_accel | 1863 | Forward acceleration threshold for bungee launch of throw launch [cm/s/s], 1G = 981 cm/s/s | | nav_fw_launch_climb_angle | 18 | Climb angle for launch sequence (degrees), is also restrained by global max_angle_inclination_pit | | nav_fw_launch_detect_time | 40 | Time for which thresholds have to breached to consider launch happened [ms] | -| nav_fw_launch_end_time | 2000 | Time for the transition of throttle and pitch angle, between the launch state and the subsequent flight mode [ms] | +| nav_fw_launch_end_time | 3000 | Time for the transition of throttle and pitch angle, between the launch state and the subsequent flight mode [ms] | | nav_fw_launch_idle_thr | 1000 | Launch idle throttle - throttle to be set before launch sequence is initiated. If set below minimum throttle it will force motor stop or at idle throttle (depending if the MOTOR_STOP is enabled). If set above minimum throttle it will force throttle to this value (if MOTOR_STOP is enabled it will be handled according to throttle stick position) | | nav_fw_launch_max_altitude | 0 | Altitude (centimeters) at which LAUNCH mode will be turned off and regular flight mode will take over [0-60000]. | | nav_fw_launch_max_angle | 45 | Max tilt angle (pitch/roll combined) to consider launch successful. Set to 180 to disable completely [deg] | @@ -306,15 +306,15 @@ | nav_fw_pitch2thr_smoothing | 6 | How smoothly the autopilot makes pitch to throttle correction inside a deadband defined by pitch_to_throttle_thresh. | | nav_fw_pitch2thr_threshold | 50 | Threshold from average pitch where momentary pitch_to_throttle correction kicks in. [decidegrees] | | nav_fw_pos_hdg_d | 0 | D gain of heading trajectory PID controller. (Fixedwing, rovers, boats) | -| nav_fw_pos_hdg_i | 0 | I gain of heading trajectory PID controller. (Fixedwing, rovers, boats) | -| nav_fw_pos_hdg_p | 60 | P gain of heading PID controller. (Fixedwing, rovers, boats) | +| nav_fw_pos_hdg_i | 2 | I gain of heading trajectory PID controller. (Fixedwing, rovers, boats) | +| nav_fw_pos_hdg_p | 30 | P gain of heading PID controller. (Fixedwing, rovers, boats) | | nav_fw_pos_hdg_pidsum_limit | 350 | Output limit for heading trajectory PID controller. (Fixedwing, rovers, boats) | | nav_fw_pos_xy_d | 8 | D gain of 2D trajectory PID controller. Too high and there will be overshoot in trajectory. Better start tuning with zero | | nav_fw_pos_xy_i | 5 | I gain of 2D trajectory PID controller. Too high and there will be overshoot in trajectory. Better start tuning with zero | | nav_fw_pos_xy_p | 75 | P gain of 2D trajectory PID controller. Play with this to get a straight line between waypoints or a straight RTH | -| nav_fw_pos_z_d | 0 | D gain of altitude PID controller (Fixedwing) | -| nav_fw_pos_z_i | 0 | I gain of altitude PID controller (Fixedwing) | -| nav_fw_pos_z_p | 50 | P gain of altitude PID controller (Fixedwing) | +| nav_fw_pos_z_d | 10 | D gain of altitude PID controller (Fixedwing) | +| nav_fw_pos_z_i | 5 | I gain of altitude PID controller (Fixedwing) | +| nav_fw_pos_z_p | 40 | P gain of altitude PID controller (Fixedwing) | | nav_fw_yaw_deadband | 0 | Deadband for heading trajectory PID controller. When heading error is below the deadband, controller assumes that vehicle is on course | | nav_land_slowdown_maxalt | 2000 | Defines at what altitude the descent velocity should start to ramp down from 100% nav_landing_speed to 25% nav_landing_speed. [cm] | | nav_land_slowdown_minalt | 500 | Defines at what altitude the descent velocity should start to be 25% of nav_landing_speed [cm] | @@ -341,15 +341,15 @@ | nav_mc_vel_xy_d | 100 | D gain of Position-Rate (Velocity to Acceleration) PID controller. It can damp P and I. Increasing D might help when drone overshoots target. | | nav_mc_vel_xy_dterm_attenuation | 90 | Maximum D-term attenution percentage for horizontal velocity PID controller (Multirotor). It allows to smooth the PosHold CRUISE, WP and RTH when Multirotor is traveling at full speed. Dterm is not attenuated at low speeds, breaking and accelerating. | | nav_mc_vel_xy_dterm_attenuation_end | 60 | A point (in percent of both target and current horizontal velocity) where nav_mc_vel_xy_dterm_attenuation reaches maximum | -| nav_mc_vel_xy_dterm_attenuation_start | 10" | A point (in percent of both target and current horizontal velocity) where nav_mc_vel_xy_dterm_attenuation begins | -| nav_mc_vel_xy_dterm_lpf_hz | | | -| nav_mc_vel_xy_ff | | | +| nav_mc_vel_xy_dterm_attenuation_start | 10 | A point (in percent of both target and current horizontal velocity) where nav_mc_vel_xy_dterm_attenuation begins | +| nav_mc_vel_xy_dterm_lpf_hz | 2 | | +| nav_mc_vel_xy_ff | 40 | | | nav_mc_vel_xy_i | 15 | I gain of Position-Rate (Velocity to Acceleration) PID controller. Used for drift compensation (caused by wind for example). Higher I means stronger response to drift. Too much I gain might cause target overshot | | nav_mc_vel_xy_p | 40 | P gain of Position-Rate (Velocity to Acceleration) PID controller. Higher P means stronger response when position error occurs. Too much P might cause "nervous" behavior and oscillations | | nav_mc_vel_z_d | 10 | D gain of velocity PID controller | | nav_mc_vel_z_i | 50 | I gain of velocity PID controller | | nav_mc_vel_z_p | 100 | P gain of velocity PID controller | -| nav_mc_wp_slowdown | ON | When ON, NAV engine will slow down when switching to the next waypoint. This prioritizes turning over forward movement. When OFF, NAV engine will continue to the next waypoint and turn as it goes. | +| nav_mc_wp_slowdown | True | When ON, NAV engine will slow down when switching to the next waypoint. This prioritizes turning over forward movement. When OFF, NAV engine will continue to the next waypoint and turn as it goes. | | nav_min_rth_distance | 500 | Minimum distance from homepoint when RTH full procedure will be activated [cm]. Below this distance, the mode will activate at the current location and the final phase is executed (loiter / land). Above this distance, the full procedure is activated, which may include initial climb and flying directly to the homepoint before entering the loiter / land phase. | | nav_overrides_motor_stop | ALL_NAV | When set to OFF the navigation system will not take over the control of the motor if the throttle is low (motor will stop). When set to OFF_ALWAYS the navigation system will not take over the control of the motor if the throttle was low even when failsafe is triggered. When set to AUTO_ONLY the navigation system will only take over the control of the throttle in autonomous navigation modes (NAV WP and NAV RTH). When set to ALL_NAV (default) the navigation system will take over the control of the motor completely and never allow the motor to stop even when the throttle is low. This setting only has an effect on NAV modes which take control of the throttle when combined with MOTOR_STOP and is likely to cause a stall if fw_min_throttle_down_pitch isn't set correctly or the pitch estimation is wrong for fixed wing models when not set to ALL_NAV | | nav_position_timeout | 5 | If GPS fails wait for this much seconds before switching to emergency landing mode (0 - disable) | @@ -357,23 +357,23 @@ | nav_rth_allow_landing | ALWAYS | If set to ON drone will land as a last phase of RTH. | | nav_rth_alt_mode | AT_LEAST | Configure how the aircraft will manage altitude on the way home, see Navigation modes on wiki for more details | | nav_rth_altitude | 1000 | Used in EXTRA, FIXED and AT_LEAST rth alt modes [cm] (Default 1000 means 10 meters) | -| nav_rth_climb_first | ON | If set to ON drone will climb to nav_rth_altitude first and head home afterwards. If set to OFF drone will head home instantly and climb on the way. | -| nav_rth_climb_ignore_emerg | OFF | If set to ON, aircraft will execute initial climb regardless of position sensor (GPS) status. | +| nav_rth_climb_first | True | If set to ON drone will climb to nav_rth_altitude first and head home afterwards. If set to OFF drone will head home instantly and climb on the way. | +| nav_rth_climb_ignore_emerg | False | If set to ON, aircraft will execute initial climb regardless of position sensor (GPS) status. | | nav_rth_home_altitude | 0 | Aircraft will climb/descend to this altitude after reaching home if landing is not enabled. Set to 0 to stay at `nav_rth_altitude` (default) [cm] | -| nav_rth_tail_first | OFF | If set to ON drone will return tail-first. Obviously meaningless for airplanes. | -| nav_use_fw_yaw_control | OFF | Enables or Disables the use of the heading PID controller on fixed wing. Heading PID controller is always enabled for rovers and boats | -| nav_use_midthr_for_althold | OFF | If set to OFF, the FC remembers your throttle stick position when enabling ALTHOLD and treats it as a neutral midpoint for holding altitude | +| nav_rth_tail_first | False | If set to ON drone will return tail-first. Obviously meaningless for airplanes. | +| nav_use_fw_yaw_control | False | Enables or Disables the use of the heading PID controller on fixed wing. Heading PID controller is always enabled for rovers and boats | +| nav_use_midthr_for_althold | False | If set to OFF, the FC remembers your throttle stick position when enabling ALTHOLD and treats it as a neutral midpoint for holding altitude | | nav_user_control_mode | ATTI | Defines how Pitch/Roll input from RC receiver affects flight in POSHOLD mode: ATTI - pitch/roll controls attitude like in ANGLE mode; CRUISE - pitch/roll controls velocity in forward and right direction. | | nav_wp_radius | 100 | Waypoint radius [cm]. Waypoint would be considered reached if machine is within this radius | | nav_wp_safe_distance | 10000 | First waypoint in the mission should be closer than this value [cm]. A value of 0 disables this check. | -| opflow_hardware | | Selection of OPFLOW hardware. | -| opflow_scale | | | -| osd_ahi_bordered | OFF | Shows a border/corners around the AHI region (pixel OSD only) | +| opflow_hardware | NONE | Selection of OPFLOW hardware. | +| opflow_scale | 10.5 | | +| osd_ahi_bordered | False | Shows a border/corners around the AHI region (pixel OSD only) | | osd_ahi_height | 162 | AHI height in pixels (pixel OSD only) | | osd_ahi_max_pitch | 20 | Max pitch, in degrees, for OSD artificial horizon | -| osd_ahi_reverse_roll | | | +| osd_ahi_reverse_roll | False | | | osd_ahi_style | DEFAULT | Sets OSD Artificial Horizon style "DEFAULT" or "LINE" for the FrSky Graphical OSD. | -| osd_ahi_vertical_offset | 0 | AHI vertical offset from center (pixel OSD only) | +| osd_ahi_vertical_offset | -18 | AHI vertical offset from center (pixel OSD only) | | osd_ahi_width | 132 | AHI width in pixels (pixel OSD only) | | osd_alt_alarm | 100 | Value above which to make the OSD relative altitude indicator blink (meters) | | osd_baro_temp_alarm_max | 600 | Temperature above which the baro temperature OSD element will start blinking (decidegrees centigrade) | @@ -381,155 +381,155 @@ | osd_camera_fov_h | 135 | Horizontal field of view for the camera in degres | | osd_camera_fov_v | 85 | Vertical field of view for the camera in degres | | osd_camera_uptilt | 0 | Set the camera uptilt for the FPV camera in degres, positive is up, negative is down, relative to the horizontal | -| osd_coordinate_digits | | | +| osd_coordinate_digits | 9 | | | osd_crosshairs_style | DEFAULT | To set the visual type for the crosshair | | osd_crsf_lq_format | TYPE1 | To select LQ format | | osd_current_alarm | 0 | Value above which the OSD current consumption element will start blinking. Measured in full Amperes. | | osd_dist_alarm | 1000 | Value above which to make the OSD distance from home indicator blink (meters) | | osd_esc_temp_alarm_max | 900 | Temperature above which the IMU temperature OSD element will start blinking (decidegrees centigrade) | | osd_esc_temp_alarm_min | -200 | Temperature under which the IMU temperature OSD element will start blinking (decidegrees centigrade) | -| osd_estimations_wind_compensation | ON | Use wind estimation for remaining flight time/distance estimation | -| osd_failsafe_switch_layout | OFF | If enabled the OSD automatically switches to the first layout during failsafe | -| osd_force_grid | OFF | Force OSD to work in grid mode even if the OSD device supports pixel level access (mainly used for development) | +| osd_estimations_wind_compensation | True | Use wind estimation for remaining flight time/distance estimation | +| osd_failsafe_switch_layout | False | If enabled the OSD automatically switches to the first layout during failsafe | +| osd_force_grid | False | Force OSD to work in grid mode even if the OSD device supports pixel level access (mainly used for development) | | osd_gforce_alarm | 5 | Value above which the OSD g force indicator will blink (g) | | osd_gforce_axis_alarm_max | 5 | Value above which the OSD axis g force indicators will blink (g) | | osd_gforce_axis_alarm_min | -5 | Value under which the OSD axis g force indicators will blink (g) | -| osd_home_position_arm_screen | ON | Should home position coordinates be displayed on the arming screen. | +| osd_home_position_arm_screen | True | Should home position coordinates be displayed on the arming screen. | | osd_horizon_offset | 0 | To vertically adjust the whole OSD and AHI and scrolling bars | -| osd_hud_homepoint | 0 | To 3D-display the home point location in the hud | -| osd_hud_homing | 0 | To display little arrows around the crossair showing where the home point is in the hud | +| osd_hud_homepoint | False | To 3D-display the home point location in the hud | +| osd_hud_homing | False | To display little arrows around the crossair showing where the home point is in the hud | | osd_hud_margin_h | 3 | Left and right margins for the hud area | | osd_hud_margin_v | 3 | Top and bottom margins for the hud area | | osd_hud_radar_disp | 0 | Maximum count of nearby aircrafts or points of interest to display in the hud, as sent from an ESP32 LoRa module. Set to 0 to disable (show nothing). The nearby aircrafts will appear as markers A, B, C, etc | | osd_hud_radar_nearest | 0 | To display an extra bar of informations at the bottom of the hud area for the closest radar aircraft found, if closest than the set value, in meters. Shows relative altitude (meters or feet, with an up or down arrow to indicate if above or below), speed (in m/s or f/s), and absolute heading (in °, 0 is north, 90 is east, 180 is south, 270 is west). Set to 0 (zero) to disable. | -| osd_hud_radar_range_max | 4000" | In meters, radar aircrafts further away than this will not be displayed in the hud | +| osd_hud_radar_range_max | 4000 | In meters, radar aircrafts further away than this will not be displayed in the hud | | osd_hud_radar_range_min | 3 | In meters, radar aircrafts closer than this will not be displayed in the hud | | osd_hud_wp_disp | 0 | How many navigation waypoints are displayed, set to 0 (zero) to disable. As sample, if set to 2, and you just passed the 3rd waypoint of the mission, you'll see markers for the 4th waypoint (marked 1) and the 5th waypoint (marked 2) | | osd_imu_temp_alarm_max | 600 | Temperature above which the IMU temperature OSD element will start blinking (decidegrees centigrade) | | osd_imu_temp_alarm_min | -200 | Temperature under which the IMU temperature OSD element will start blinking (decidegrees centigrade) | -| osd_left_sidebar_scroll | | | +| osd_left_sidebar_scroll | NONE | | | osd_left_sidebar_scroll_step | 0 | How many units each sidebar step represents. 0 means the default value for the scroll type. | | osd_link_quality_alarm | 70 | LQ % indicator blinks below this value. For Crossfire use 70%, for Tracer use 50% | | osd_main_voltage_decimals | 1 | Number of decimals for the battery voltages displayed in the OSD [1-2]. | | osd_neg_alt_alarm | 5 | Value below which (negative altitude) to make the OSD relative altitude indicator blink (meters) | -| osd_pan_servo_index | | Index of the pan servo to adjust osd home heading direction based on camera pan. Note that this feature does not work with continiously rotating servos. | +| osd_pan_servo_index | 0 | Index of the pan servo to adjust osd home heading direction based on camera pan. Note that this feature does not work with continiously rotating servos. | | osd_pan_servo_pwm2centideg | 0 | Centidegrees of pan servo rotation us PWM signal. A servo with 180 degrees of rotation from 1000 to 2000 us PWM typically needs `18` for this setting. Change sign to inverse direction. | -| osd_plus_code_digits | 10 | Numer of plus code digits before shortening with `osd_plus_code_short`. Precision at the equator: 10=13.9x13.9m; 11=2.8x3.5m; 12=56x87cm; 13=11x22cm. | +| osd_plus_code_digits | 11 | Numer of plus code digits before shortening with `osd_plus_code_short`. Precision at the equator: 10=13.9x13.9m; 11=2.8x3.5m; 12=56x87cm; 13=11x22cm. | | osd_plus_code_short | 0 | Number of leading digits removed from plus code. Removing 2, 4 and 6 digits requires a reference location within, respectively, ~800km, ~40 km and ~2km to recover the original coordinates. | -| osd_right_sidebar_scroll | | | +| osd_right_sidebar_scroll | NONE | | | osd_right_sidebar_scroll_step | 0 | Same as left_sidebar_scroll_step, but for the right sidebar | | osd_row_shiftdown | 0 | Number of rows to shift the OSD display (increase if top rows are cut off) | | osd_rssi_alarm | 20 | Value below which to make the OSD RSSI indicator blink | | osd_sidebar_horizontal_offset | 0 | Sidebar horizontal offset from default position. Positive values move the sidebars closer to the edges. | -| osd_sidebar_scroll_arrows | | | +| osd_sidebar_scroll_arrows | False | | | osd_snr_alarm | 4 | Value below which Crossfire SNR Alarm pops-up. (dB) | | osd_stats_energy_unit | MAH | Unit used for the drawn energy in the OSD stats [MAH/WH] (milliAmpere hour/ Watt hour) | | osd_temp_label_align | LEFT | Allows to chose between left and right alignment for the OSD temperature sensor labels. Valid values are `LEFT` and `RIGHT` | | osd_time_alarm | 10 | Value above which to make the OSD flight time indicator blink (minutes) | | osd_units | METRIC | IMPERIAL, METRIC, UK | | osd_video_system | AUTO | Video system used. Possible values are `AUTO`, `PAL` and `NTSC` | -| pid_type | | Allows to set type of PID controller used in control loop. Possible values: `NONE`, `PID`, `PIFF`, `AUTO`. Change only in case of experimental platforms like VTOL, tailsitters, rovers, boats, etc. Airplanes should always use `PIFF` and multirotors `PID` | +| pid_type | AUTO | Allows to set type of PID controller used in control loop. Possible values: `NONE`, `PID`, `PIFF`, `AUTO`. Change only in case of experimental platforms like VTOL, tailsitters, rovers, boats, etc. Airplanes should always use `PIFF` and multirotors `PID` | | pidsum_limit | 500 | A limitation to overall amount of correction Flight PID can request on each axis (Roll/Pitch). If when doing a hard maneuver on one axis machine looses orientation on other axis - reducing this parameter may help | -| pidsum_limit_yaw | 400 | A limitation to overall amount of correction Flight PID can request on each axis (Yaw). If when doing a hard maneuver on one axis machine looses orientation on other axis - reducing this parameter may help | -| pinio_box1 | target specific | Mode assignment for PINIO#1 | -| pinio_box2 | target specific | Mode assignment for PINIO#1 | -| pinio_box3 | target specific | Mode assignment for PINIO#1 | -| pinio_box4 | target specific | Mode assignment for PINIO#1 | +| pidsum_limit_yaw | 350 | A limitation to overall amount of correction Flight PID can request on each axis (Yaw). If when doing a hard maneuver on one axis machine looses orientation on other axis - reducing this parameter may help | +| pinio_box1 | :BOX_PERMANENT_ID_NONE | Mode assignment for PINIO#1 | +| pinio_box2 | :BOX_PERMANENT_ID_NONE | Mode assignment for PINIO#1 | +| pinio_box3 | :BOX_PERMANENT_ID_NONE | Mode assignment for PINIO#1 | +| pinio_box4 | :BOX_PERMANENT_ID_NONE | Mode assignment for PINIO#1 | | pitch_rate | 20 | Defines rotation rate on PITCH axis that UAV will try to archive on max. stick deflection. Rates are defined in tens of degrees (deca-degrees) per second [rate = dps/10]. That means, rate 20 represents 200dps rotation speed. Default 20 (200dps) is more less equivalent of old Cleanflight/Baseflight rate 0. Max. 180 (1800dps) is what gyro can measure. | | pitot_hardware | NONE | Selection of pitot hardware. | -| pitot_lpf_milli_hz | | | -| pitot_scale | | | -| platform_type | "MULTIROTOR" | Defines UAV platform type. Allowed values: "MULTIROTOR", "AIRPLANE", "HELICOPTER", "TRICOPTER", "ROVER", "BOAT". Currently only MULTIROTOR, AIRPLANE and TRICOPTER types are implemented | -| pos_hold_deadband | 20 | Stick deadband in [r/c points], applied after r/c deadband and expo | +| pitot_lpf_milli_hz | 350 | | +| pitot_scale | 1.0 | | +| platform_type | MULTIROTOR | Defines UAV platform type. Allowed values: "MULTIROTOR", "AIRPLANE", "HELICOPTER", "TRICOPTER", "ROVER", "BOAT". Currently only MULTIROTOR, AIRPLANE and TRICOPTER types are implemented | +| pos_hold_deadband | 10 | Stick deadband in [r/c points], applied after r/c deadband and expo | | prearm_timeout | 10000 | Duration (ms) for which Prearm being activated is valid. after this, Prearm needs to be reset. 0 means Prearm does not timeout. | | rangefinder_hardware | NONE | Selection of rangefinder hardware. | -| rangefinder_median_filter | OFF | 3-point median filtering for rangefinder readouts | +| rangefinder_median_filter | False | 3-point median filtering for rangefinder readouts | | rate_accel_limit_roll_pitch | 0 | Limits acceleration of ROLL/PITCH rotation speed that can be requested by stick input. In degrees-per-second-squared. Small and powerful UAV flies great with high acceleration limit ( > 5000 dps^2 and even > 10000 dps^2). Big and heavy multirotors will benefit from low acceleration limit (~ 360 dps^2). When set correctly, it greatly improves stopping performance. Value of 0 disables limiting. | | rate_accel_limit_yaw | 10000 | Limits acceleration of YAW rotation speed that can be requested by stick input. In degrees-per-second-squared. Small and powerful UAV flies great with high acceleration limit ( > 10000 dps^2). Big and heavy multirotors will benefit from low acceleration limit (~ 180 dps^2). When set correctly, it greatly improves stopping performance and general stability during yaw turns. Value of 0 disables limiting. | | rc_expo | 70 | Exposition value used for the PITCH/ROLL axes by all the stabilized flights modes (all but `MANUAL`) | | rc_filter_frequency | 50 | RC data biquad filter cutoff frequency. Lower cutoff frequencies result in smoother response at expense of command control delay. Practical values are 20-50. Set to zero to disable entirely and use unsmoothed RC stick values | | rc_yaw_expo | 20 | Exposition value used for the YAW axis by all the stabilized flights modes (all but `MANUAL`) | | reboot_character | 82 | Special character used to trigger reboot | -| receiver_type | `TARGET dependent` | Selection of receiver (RX) type. Additional configuration of a `serialrx_provider` and a UART will be needed for `SERIAL` | -| report_cell_voltage | OFF | S.Port, D-Series, and IBUS telemetry: Send the average cell voltage if set to ON | +| receiver_type | :target | Selection of receiver (RX) type. Additional configuration of a `serialrx_provider` and a UART will be needed for `SERIAL` | +| report_cell_voltage | False | S.Port, D-Series, and IBUS telemetry: Send the average cell voltage if set to ON | | roll_rate | 20 | Defines rotation rate on ROLL axis that UAV will try to archive on max. stick deflection. Rates are defined in tens of degrees (deca-degrees) per second [rate = dps/10]. That means, rate 20 represents 200dps rotation speed. Default 20 (200dps) is more less equivalent of old Cleanflight/Baseflight rate 0. Max. 180 (1800dps) is what gyro can measure. | -| rpm_gyro_filter_enabled | `OFF` | Enables gyro RPM filtere. Set to `ON` only when ESC telemetry is working and rotation speed of the motors is correctly reported to INAV | +| rpm_gyro_filter_enabled | False | Enables gyro RPM filtere. Set to `ON` only when ESC telemetry is working and rotation speed of the motors is correctly reported to INAV | | rpm_gyro_harmonics | 1 | Number of harmonic frequences to be covered by gyro RPM filter. Default value of `1` usually works just fine | -| rpm_gyro_min_hz | 150 | The lowest frequency for gyro RPM filtere. Default `150` is fine for 5" mini-quads. On 7-inch drones you can lower even down to `60`-`70` | +| rpm_gyro_min_hz | 100 | The lowest frequency for gyro RPM filtere. Default `150` is fine for 5" mini-quads. On 7-inch drones you can lower even down to `60`-`70` | | rpm_gyro_q | 500 | Q factor for gyro RPM filter. Lower values give softer, wider attenuation. Usually there is no need to change this setting | -| rssi_adc_channel | - | ADC channel to use for analog RSSI input. Defaults to board RSSI input (if available). 0 = disabled | +| rssi_adc_channel | :target | ADC channel to use for analog RSSI input. Defaults to board RSSI input (if available). 0 = disabled | | rssi_channel | 0 | RX channel containing the RSSI signal | | rssi_max | 100 | The maximum RSSI value sent by the receiver, in %. For example, if your receiver's maximum RSSI value shows as 83% in the configurator/OSD set this parameter to 83. See also rssi_min. | | rssi_min | 0 | The minimum RSSI value sent by the receiver, in %. For example, if your receiver's minimum RSSI value shows as 42% in the configurator/OSD set this parameter to 42. See also rssi_max. Note that rssi_min can be set to a value bigger than rssi_max to invert the RSSI calculation (i.e. bigger values mean lower RSSI). | -| rssi_source | `AUTO` | Source of RSSI input. Possible values: `NONE`, `AUTO`, `ADC`, `CHANNEL`, `PROTOCOL`, `MSP` | +| rssi_source | AUTO | Source of RSSI input. Possible values: `NONE`, `AUTO`, `ADC`, `CHANNEL`, `PROTOCOL`, `MSP` | | rth_energy_margin | 5 | Energy margin wanted after getting home (percent of battery energy capacity). Use for the remaining flight time/distance calculation | | rx_max_usec | 2115 | Defines the longest pulse width value used when ensuring the channel value is valid. If the receiver gives a pulse value higher than this value then the channel will be marked as bad and will default to the value of mid_rc. | | rx_min_usec | 885 | Defines the shortest pulse width value used when ensuring the channel value is valid. If the receiver gives a pulse value lower than this value then the channel will be marked as bad and will default to the value of mid_rc. | -| rx_spi_id | | | -| rx_spi_protocol | | | -| rx_spi_rf_channel_count | | | +| rx_spi_id | :zero | | +| rx_spi_protocol | :target | | +| rx_spi_rf_channel_count | :zero | | | safehome_max_distance | 20000 | In order for a safehome to be used, it must be less than this distance (in cm) from the arming point. | -| sbus_sync_interval | | | -| sdcard_detect_inverted | `TARGET dependent` | This setting drives the way SD card is detected in card slot. On some targets (AnyFC F7 clone) different card slot was used and depending of hardware revision ON or OFF setting might be required. If card is not detected, change this value. | +| sbus_sync_interval | 3000 | | +| sdcard_detect_inverted | :target | This setting drives the way SD card is detected in card slot. On some targets (AnyFC F7 clone) different card slot was used and depending of hardware revision ON or OFF setting might be required. If card is not detected, change this value. | | serialrx_halfduplex | AUTO | Allow serial receiver to operate on UART TX pin. With some receivers will allow control and telemetry over a single wire. | -| serialrx_inverted | OFF | Reverse the serial inversion of the serial RX protocol. When this value is OFF, each protocol will use its default signal (e.g. SBUS will use an inverted signal). Some OpenLRS receivers produce a non-inverted SBUS signal. This setting supports this type of receivers (including modified FrSKY). | -| serialrx_provider | SPEK1024 | When feature SERIALRX is enabled, this allows connection to several receivers which output data via digital interface resembling serial. See RX section. | +| serialrx_inverted | False | Reverse the serial inversion of the serial RX protocol. When this value is OFF, each protocol will use its default signal (e.g. SBUS will use an inverted signal). Some OpenLRS receivers produce a non-inverted SBUS signal. This setting supports this type of receivers (including modified FrSKY). | +| serialrx_provider | :target | When feature SERIALRX is enabled, this allows connection to several receivers which output data via digital interface resembling serial. See RX section. | | servo_center_pulse | 1500 | Servo midpoint | | servo_lpf_hz | 20 | Selects the servo PWM output cutoff frequency. Value is in [Hz] | | servo_protocol | PWM | An option to chose the protocol/option that would be used to output servo data. Possible options `PWM` (FC servo outputs), `SERVO_DRIVER` (I2C PCA9685 peripheral), `SBUS` (S.Bus protocol output via a configured serial port) | | servo_pwm_rate | 50 | Output frequency (in Hz) servo pins. When using tricopters or gimbal with digital servo, this rate can be increased. Max of 498Hz (for 500Hz pwm period), and min of 50Hz. Most digital servos will support for example 330Hz. | -| setpoint_kalman_enabled | OFF | Enable Kalman filter on the PID controller setpoint | +| setpoint_kalman_enabled | False | Enable Kalman filter on the PID controller setpoint | | setpoint_kalman_q | 100 | Quality factor of the setpoint Kalman filter. Higher values means less filtering and lower phase delay. On 3-7 inch multirotors can be usually increased to 200-300 or even higher of clean builds | | setpoint_kalman_sharpness | 100 | Dynamic factor for the setpoint Kalman filter. In general, the higher the value, the more dynamic Kalman filter gets | | setpoint_kalman_w | 4 | Window size for the setpoint Kalman filter. Wider the window, more samples are used to compute variance. In general, wider window results in smoother filter response | -| sim_ground_station_number | Empty string | Number of phone that is used to communicate with SIM module. Messages / calls from other numbers are ignored. If undefined, can be set by calling or sending a message to the module. | -| sim_low_altitude | 0 | Threshold for low altitude warning messages sent by SIM module when the 'L' transmit flag is set in `sim_transmit_flags`. | -| sim_pin | Empty string | PIN code for the SIM module | -| sim_transmit_flags | F | String specifying text message transmit condition flags for the SIM module. Flags can be given in any order. Empty string means the module only sends response messages. `A`: acceleration events, `T`: continuous transmission, `F`: continuous transmission in failsafe mode, `L`: continuous transmission when altitude is below `sim_low_altitude`, `G`: continuous transmission when GPS signal quality is low | +| sim_ground_station_number | | Number of phone that is used to communicate with SIM module. Messages / calls from other numbers are ignored. If undefined, can be set by calling or sending a message to the module. | +| sim_low_altitude | -32767 | Threshold for low altitude warning messages sent by SIM module when the 'L' transmit flag is set in `sim_transmit_flags`. | +| sim_pin | 0000 | PIN code for the SIM module | +| sim_transmit_flags | :SIM_TX_FLAG_FAILSAFE | Bitmask specifying text message transmit condition flags for the SIM module. 1: continuous transmission, 2: continuous transmission in failsafe mode, 4: continuous transmission when GPS signal quality is low, 8: acceleration events, 16: continuous transmission when altitude is below `sim_low_altitude` | | sim_transmit_interval | 60 | Text message transmission interval in seconds for SIM module. Minimum value: 10 | | small_angle | 25 | If the aircraft tilt angle exceed this value the copter will refuse to arm. | | smartport_fuel_unit | MAH | S.Port telemetry only: Unit of the value sent with the `FUEL` ID (FrSky D-Series always sends percent). [PERCENT/MAH/MWH] | -| smartport_master_halfduplex | | | -| smartport_master_inverted | | | -| spektrum_sat_bind | 0 | 0 = disabled. Used to bind the spektrum satellite to RX | -| srxl2_baud_fast | | | -| srxl2_unit_id | | | -| stats | OFF | General switch of the statistics recording feature (a.k.a. odometer) | +| smartport_master_halfduplex | True | | +| smartport_master_inverted | False | | +| spektrum_sat_bind | :SPEKTRUM_SAT_BIND_DISABLED | 0 = disabled. Used to bind the spektrum satellite to RX | +| srxl2_baud_fast | True | | +| srxl2_unit_id | 1 | | +| stats | False | General switch of the statistics recording feature (a.k.a. odometer) | | stats_total_dist | 0 | Total flight distance [in meters]. The value is updated on every disarm when "stats" are enabled. | -| stats_total_energy | | | +| stats_total_energy | 0 | | | stats_total_time | 0 | Total flight time [in seconds]. The value is updated on every disarm when "stats" are enabled. | | switch_disarm_delay | 250 | Delay before disarming when requested by switch (ms) [0-1000] | -| telemetry_halfduplex | OFF | S.Port telemetry only: Turn UART into UNIDIR for usage on F1 and F4 target. See Telemetry.md for details | -| telemetry_inverted | OFF | Determines if the telemetry protocol default signal inversion is reversed. This should be OFF in most cases unless a custom or hacked RX is used. | -| telemetry_switch | OFF | Which aux channel to use to change serial output & baud rate (MSP / Telemetry). It disables automatic switching to Telemetry when armed. | -| thr_comp_weight | 0.692 | Weight used for the throttle compensation based on battery voltage. See the [battery documentation](Battery.md#automatic-throttle-compensation-based-on-battery-voltage) | +| telemetry_halfduplex | True | S.Port telemetry only: Turn UART into UNIDIR for usage on F1 and F4 target. See Telemetry.md for details | +| telemetry_inverted | False | Determines if the telemetry protocol default signal inversion is reversed. This should be OFF in most cases unless a custom or hacked RX is used. | +| telemetry_switch | False | Which aux channel to use to change serial output & baud rate (MSP / Telemetry). It disables automatic switching to Telemetry when armed. | +| thr_comp_weight | 1 | Weight used for the throttle compensation based on battery voltage. See the [battery documentation](Battery.md#automatic-throttle-compensation-based-on-battery-voltage) | | thr_expo | 0 | Throttle exposition value | | thr_mid | 50 | Throttle value when the stick is set to mid-position. Used in the throttle curve calculation. | | throttle_idle | 15 | The percentage of the throttle range (`max_throttle` - `min_command`) above `min_command` used for minimum / idle throttle. | -| throttle_scale | 1.000 | Throttle scaling factor. `1` means no throttle scaling. `0.5` means throttle scaled down by 50% | +| throttle_scale | 1.0 | Throttle scaling factor. `1` means no throttle scaling. `0.5` means throttle scaled down by 50% | | throttle_tilt_comp_str | 0 | Can be used in ANGLE and HORIZON mode and will automatically boost throttle when banking. Setting is in percentage, 0=disabled. | | tpa_breakpoint | 1500 | See tpa_rate. | | tpa_rate | 0 | Throttle PID attenuation reduces influence of P on ROLL and PITCH as throttle increases. For every 1% throttle after the TPA breakpoint, P is reduced by the TPA rate. | -| tri_unarmed_servo | ON | On tricopter mix only, if this is set to ON, servo will always be correcting regardless of armed state. to disable this, set it to OFF. | +| tri_unarmed_servo | True | On tricopter mix only, if this is set to ON, servo will always be correcting regardless of armed state. to disable this, set it to OFF. | | tz_automatic_dst | OFF | Automatically add Daylight Saving Time to the GPS time when needed or simply ignore it. Includes presets for EU and the USA - if you live outside these areas it is suggested to manage DST manually via `tz_offset`. | | tz_offset | 0 | Time zone offset from UTC, in minutes. This is applied to the GPS time for logging and time-stamping of Blackbox logs | -| vbat_adc_channel | - | ADC channel to use for battery voltage sensor. Defaults to board VBAT input (if available). 0 = disabled | +| vbat_adc_channel | :target | ADC channel to use for battery voltage sensor. Defaults to board VBAT input (if available). 0 = disabled | | vbat_cell_detect_voltage | 425 | Maximum voltage per cell, used for auto-detecting the number of cells of the battery in 0.01V units, default is 4.30V. | | vbat_max_cell_voltage | 420 | Maximum voltage per cell in 0.01V units, default is 4.20V | -| vbat_meter_type | `ADC` | Vbat voltage source. Possible values: `NONE`, `ADC`, `ESC`. `ESC` required ESC telemetry enebled and running | +| vbat_meter_type | ADC | Vbat voltage source. Possible values: `NONE`, `ADC`, `ESC`. `ESC` required ESC telemetry enebled and running | | vbat_min_cell_voltage | 330 | Minimum voltage per cell, this triggers battery out alarms, in 0.01V units, default is 330 (3.3V) | -| vbat_scale | 1100 | Battery voltage calibration value. 1100 = 11:1 voltage divider (10k:1k) x 100. Adjust this slightly if reported pack voltage is different from multimeter reading. You can get current voltage by typing "status" in cli. | +| vbat_scale | :target | Battery voltage calibration value. 1100 = 11:1 voltage divider (10k:1k) x 100. Adjust this slightly if reported pack voltage is different from multimeter reading. You can get current voltage by typing "status" in cli. | | vbat_warning_cell_voltage | 350 | Warning voltage per cell, this triggers battery-warning alarms, in 0.01V units, default is 350 (3.5V) | | vtx_band | 4 | Configure the VTX band. Set to zero to use `vtx_freq`. Bands: 1: A, 2: B, 3: E, 4: F, 5: Race. | | vtx_channel | 1 | Channel to use within the configured `vtx_band`. Valid values are [1, 8]. | -| vtx_halfduplex | ON | Use half duplex UART to communicate with the VTX, using only a TX pin in the FC. | +| vtx_halfduplex | True | Use half duplex UART to communicate with the VTX, using only a TX pin in the FC. | | vtx_low_power_disarm | OFF | When the craft is disarmed, set the VTX to its lowest power. `ON` will set the power to its minimum value on startup, increase it to `vtx_power` when arming and change it back to its lowest setting after disarming. `UNTIL_FIRST_ARM` will start with minimum power, but once the craft is armed it will increase to `vtx_power` and it will never decrease until the craft is power cycled. | | vtx_max_power_override | 0 | Some VTXes may report max power incorrectly (i.e. 200mW for a 600mW VTX). Use this to override max supported power. 0 to disable and use whatever VTX reports as its capabilities | -| vtx_pit_mode_chan | | | +| vtx_pit_mode_chan | 1 | | | vtx_power | 1 | VTX RF power level to use. The exact number of mw depends on the VTX hardware. | -| vtx_smartaudio_early_akk_workaround | ON | Enable workaround for early AKK SAudio-enabled VTX bug. | +| vtx_smartaudio_early_akk_workaround | True | Enable workaround for early AKK SAudio-enabled VTX bug. | | yaw_deadband | 5 | These are values (in us) by how much RC input can be different before it's considered valid. For transmitters with jitter on outputs, this value can be increased. Defaults are zero, but can be increased up to 10 or so if rc inputs twitch while idle. | -| yaw_lpf_hz | 30 | Yaw low pass filter cutoff frequency. Should be disabled (set to `0`) on small multirotors (7 inches and below) | +| yaw_lpf_hz | 0 | Yaw low pass filter cutoff frequency. Should be disabled (set to `0`) on small multirotors (7 inches and below) | | yaw_rate | 20 | Defines rotation rate on YAW axis that UAV will try to archive on max. stick deflection. Rates are defined in tens of degrees (deca-degrees) per second [rate = dps/10]. That means, rate 20 represents 200dps rotation speed. Default 20 (200dps) is more less equivalent of old Cleanflight/Baseflight rate 0. Max. 180 (1800dps) is what gyro can measure. | > Note: this table is autogenerated. Do not edit it manually. \ No newline at end of file diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c index 2a98978442..a39aaed510 100644 --- a/src/main/blackbox/blackbox.c +++ b/src/main/blackbox/blackbox.c @@ -52,6 +52,7 @@ #include "fc/rc_controls.h" #include "fc/rc_modes.h" #include "fc/runtime_config.h" +#include "fc/settings.h" #include "flight/failsafe.h" #include "flight/imu.h" @@ -91,18 +92,18 @@ #endif #ifdef SDCARD_DETECT_INVERTED -#define BLACKBOX_INTERVED_CARD_DETECTION 1 +#define BLACKBOX_INVERTED_CARD_DETECTION 1 #else -#define BLACKBOX_INTERVED_CARD_DETECTION 0 +#define BLACKBOX_INVERTED_CARD_DETECTION 0 #endif PG_REGISTER_WITH_RESET_TEMPLATE(blackboxConfig_t, blackboxConfig, PG_BLACKBOX_CONFIG, 1); PG_RESET_TEMPLATE(blackboxConfig_t, blackboxConfig, .device = DEFAULT_BLACKBOX_DEVICE, - .rate_num = 1, - .rate_denom = 1, - .invertedCardDetection = BLACKBOX_INTERVED_CARD_DETECTION, + .rate_num = SETTING_BLACKBOX_RATE_NUM_DEFAULT, + .rate_denom = SETTING_BLACKBOX_RATE_DENOM_DEFAULT, + .invertedCardDetection = BLACKBOX_INVERTED_CARD_DETECTION, ); #define BLACKBOX_SHUTDOWN_TIMEOUT_MILLIS 200 @@ -1656,6 +1657,7 @@ static bool blackboxWriteSysinfo(void) BLACKBOX_PRINT_HEADER_LINE("motorOutput", "%d,%d", getThrottleIdleValue(),motorConfig()->maxthrottle); BLACKBOX_PRINT_HEADER_LINE("acc_1G", "%u", acc.dev.acc_1G); +#ifdef USE_ADC BLACKBOX_PRINT_HEADER_LINE_CUSTOM( if (testBlackboxCondition(FLIGHT_LOG_FIELD_CONDITION_VBAT)) { blackboxPrintfHeaderLine("vbat_scale", "%u", batteryMetersConfig()->voltage.scale / 10); @@ -1667,6 +1669,7 @@ static bool blackboxWriteSysinfo(void) currentBatteryProfile->voltage.cellWarning / 10, currentBatteryProfile->voltage.cellMax / 10); BLACKBOX_PRINT_HEADER_LINE("vbatref", "%u", vbatReference); +#endif BLACKBOX_PRINT_HEADER_LINE_CUSTOM( //Note: Log even if this is a virtual current meter, since the virtual meter uses these parameters too: @@ -1723,17 +1726,25 @@ static bool blackboxWriteSysinfo(void) BLACKBOX_PRINT_HEADER_LINE("gyro_lpf_hz", "%d", gyroConfig()->gyro_soft_lpf_hz); BLACKBOX_PRINT_HEADER_LINE("gyro_lpf_type", "%d", gyroConfig()->gyro_soft_lpf_type); BLACKBOX_PRINT_HEADER_LINE("gyro_lpf2_hz", "%d", gyroConfig()->gyro_stage2_lowpass_hz); +#ifdef USE_DYNAMIC_FILTERS BLACKBOX_PRINT_HEADER_LINE("dynamicGyroNotchRange", "%d", gyroConfig()->dynamicGyroNotchRange); BLACKBOX_PRINT_HEADER_LINE("dynamicGyroNotchQ", "%d", gyroConfig()->dynamicGyroNotchQ); BLACKBOX_PRINT_HEADER_LINE("dynamicGyroNotchMinHz", "%d", gyroConfig()->dynamicGyroNotchMinHz); +#endif BLACKBOX_PRINT_HEADER_LINE("gyro_notch_hz", "%d,%d", gyroConfig()->gyro_notch_hz, 0); BLACKBOX_PRINT_HEADER_LINE("gyro_notch_cutoff", "%d,%d", gyroConfig()->gyro_notch_cutoff, 1); BLACKBOX_PRINT_HEADER_LINE("acc_lpf_hz", "%d", accelerometerConfig()->acc_lpf_hz); BLACKBOX_PRINT_HEADER_LINE("acc_hardware", "%d", accelerometerConfig()->acc_hardware); +#ifdef USE_BARO BLACKBOX_PRINT_HEADER_LINE("baro_hardware", "%d", barometerConfig()->baro_hardware); +#endif +#ifdef USE_MAG BLACKBOX_PRINT_HEADER_LINE("mag_hardware", "%d", compassConfig()->mag_hardware); +#else + BLACKBOX_PRINT_HEADER_LINE("mag_hardware", "%d", MAG_NONE); +#endif BLACKBOX_PRINT_HEADER_LINE("serialrx_provider", "%d", rxConfig()->serialrx_provider); BLACKBOX_PRINT_HEADER_LINE("motor_pwm_protocol", "%d", motorConfig()->motorPwmProtocol); BLACKBOX_PRINT_HEADER_LINE("motor_pwm_rate", "%d", motorConfig()->motorPwmRate); diff --git a/src/main/common/log.c b/src/main/common/log.c index 284589ca6d..24093c6595 100644 --- a/src/main/common/log.c +++ b/src/main/common/log.c @@ -39,6 +39,7 @@ #include "io/serial.h" #include "fc/config.h" +#include "fc/settings.h" #include "msp/msp.h" #include "msp/msp_serial.h" @@ -54,6 +55,11 @@ static mspPort_t * mspLogPort = NULL; PG_REGISTER(logConfig_t, logConfig, PG_LOG_CONFIG, 0); +PG_RESET_TEMPLATE(logConfig_t, logConfig, + .level = SETTING_LOG_LEVEL_DEFAULT, + .topics = SETTING_LOG_TOPICS_DEFAULT +); + void logInit(void) { const serialPortConfig_t *portConfig = findSerialPortConfig(FUNCTION_LOG); diff --git a/src/main/common/time.c b/src/main/common/time.c index 27faeeadfb..f1204c57f4 100644 --- a/src/main/common/time.c +++ b/src/main/common/time.c @@ -33,6 +33,8 @@ #include "drivers/time.h" +#include "fc/settings.h" + // For the "modulo 4" arithmetic to work, we need a leap base year #define REFERENCE_YEAR 2000 // Offset (seconds) from the UNIX epoch (1970-01-01) to 2000-01-01 @@ -55,8 +57,8 @@ static const uint16_t days[4][12] = PG_REGISTER_WITH_RESET_TEMPLATE(timeConfig_t, timeConfig, PG_TIME_CONFIG, 1); PG_RESET_TEMPLATE(timeConfig_t, timeConfig, - .tz_offset = 0, - .tz_automatic_dst = TZ_AUTO_DST_OFF, + .tz_offset = SETTING_TZ_OFFSET_DEFAULT, + .tz_automatic_dst = SETTING_TZ_AUTOMATIC_DST_DEFAULT, ); static rtcTime_t dateTimeToRtcTime(const dateTime_t *dt) diff --git a/src/main/config/general_settings.c b/src/main/config/general_settings.c index 38b8ecf3e3..47015f8ee6 100644 --- a/src/main/config/general_settings.c +++ b/src/main/config/general_settings.c @@ -28,8 +28,10 @@ #include "config/general_settings.h" +#include "fc/settings.h" + PG_REGISTER_WITH_RESET_TEMPLATE(generalSettings_t, generalSettings, PG_GENERAL_SETTINGS, 0); PG_RESET_TEMPLATE(generalSettings_t, generalSettings, - .appliedDefaults = APPLIED_DEFAULTS_NONE, -); \ No newline at end of file + .appliedDefaults = SETTING_APPLIED_DEFAULTS_DEFAULT, +); diff --git a/src/main/drivers/display.c b/src/main/drivers/display.c index 5afe475184..2bb96175a1 100644 --- a/src/main/drivers/display.c +++ b/src/main/drivers/display.c @@ -30,6 +30,7 @@ #include "drivers/display_font_metadata.h" #include "drivers/time.h" +#include "fc/settings.h" #define SW_BLINK_CYCLE_MS 200 // 200ms on / 200ms off @@ -43,7 +44,7 @@ PG_REGISTER_WITH_RESET_TEMPLATE(displayConfig_t, displayConfig, PG_DISPLAY_CONFIG, 0); PG_RESET_TEMPLATE(displayConfig_t, displayConfig, - .force_sw_blink = false, + .force_sw_blink = SETTING_DISPLAY_FORCE_SW_BLINK_DEFAULT ); static bool displayAttributesRequireEmulation(displayPort_t *instance, textAttributes_t attr) diff --git a/src/main/fc/config.c b/src/main/fc/config.c index 5fcf8a6d11..d38554bd21 100755 --- a/src/main/fc/config.c +++ b/src/main/fc/config.c @@ -112,11 +112,15 @@ PG_REGISTER_WITH_RESET_TEMPLATE(systemConfig_t, systemConfig, PG_SYSTEM_CONFIG, PG_RESET_TEMPLATE(systemConfig_t, systemConfig, .current_profile_index = 0, .current_battery_profile_index = 0, - .debug_mode = DEBUG_NONE, - .i2c_speed = I2C_SPEED_400KHZ, - .cpuUnderclock = 0, - .throttle_tilt_compensation_strength = 0, // 0-100, 0 - disabled - .name = { 0 } + .debug_mode = SETTING_DEBUG_MODE_DEFAULT, +#ifdef USE_I2C + .i2c_speed = SETTING_I2C_SPEED_DEFAULT, +#endif +#ifdef USE_UNDERCLOCK + .cpuUnderclock = SETTING_CPU_UNDERCLOCK_DEFAULT, +#endif + .throttle_tilt_compensation_strength = SETTING_THROTTLE_TILT_COMP_STR_DEFAULT, // 0-100, 0 - disabled + .name = SETTING_NAME_DEFAULT ); PG_REGISTER(beeperConfig_t, beeperConfig, PG_BEEPER_CONFIG, 0); diff --git a/src/main/fc/config.h b/src/main/fc/config.h index bab2d96eef..48dc455e71 100644 --- a/src/main/fc/config.h +++ b/src/main/fc/config.h @@ -73,8 +73,12 @@ typedef struct systemConfig_s { uint8_t current_profile_index; uint8_t current_battery_profile_index; uint8_t debug_mode; +#ifdef USE_I2C uint8_t i2c_speed; +#endif +#ifdef USE_UNDERCLOCK uint8_t cpuUnderclock; +#endif uint8_t throttle_tilt_compensation_strength; // the correction that will be applied at throttle_correction_angle. char name[MAX_NAME_LENGTH + 1]; } systemConfig_t; diff --git a/src/main/fc/controlrate_profile.c b/src/main/fc/controlrate_profile.c index 1c633ec1bf..ba464a1fe6 100644 --- a/src/main/fc/controlrate_profile.c +++ b/src/main/fc/controlrate_profile.c @@ -29,6 +29,7 @@ #include "fc/config.h" #include "fc/controlrate_profile.h" #include "fc/rc_curves.h" +#include "fc/settings.h" const controlRateConfig_t *currentControlRateProfile; @@ -40,31 +41,31 @@ void pgResetFn_controlRateProfiles(controlRateConfig_t *instance) for (int i = 0; i < MAX_CONTROL_RATE_PROFILE_COUNT; i++) { RESET_CONFIG(controlRateConfig_t, &instance[i], .throttle = { - .rcMid8 = 50, - .rcExpo8 = 0, - .dynPID = 0, - .pa_breakpoint = 1500, - .fixedWingTauMs = 0 + .rcMid8 = SETTING_THR_MID_DEFAULT, + .rcExpo8 = SETTING_THR_EXPO_DEFAULT, + .dynPID = SETTING_TPA_RATE_DEFAULT, + .pa_breakpoint = SETTING_TPA_BREAKPOINT_DEFAULT, + .fixedWingTauMs = SETTING_FW_TPA_TIME_CONSTANT_DEFAULT }, .stabilized = { - .rcExpo8 = 70, - .rcYawExpo8 = 20, - .rates[FD_ROLL] = CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_DEFAULT, - .rates[FD_PITCH] = CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_DEFAULT, - .rates[FD_YAW] = CONTROL_RATE_CONFIG_YAW_RATE_DEFAULT, + .rcExpo8 = SETTING_RC_EXPO_DEFAULT, + .rcYawExpo8 = SETTING_RC_YAW_EXPO_DEFAULT, + .rates[FD_ROLL] = SETTING_ROLL_RATE_DEFAULT, + .rates[FD_PITCH] = SETTING_PITCH_RATE_DEFAULT, + .rates[FD_YAW] = SETTING_YAW_RATE_DEFAULT, }, .manual = { - .rcExpo8 = 70, - .rcYawExpo8 = 20, - .rates[FD_ROLL] = 100, - .rates[FD_PITCH] = 100, - .rates[FD_YAW] = 100 + .rcExpo8 = SETTING_MANUAL_RC_EXPO_DEFAULT, + .rcYawExpo8 = SETTING_MANUAL_RC_YAW_EXPO_DEFAULT, + .rates[FD_ROLL] = SETTING_MANUAL_ROLL_RATE_DEFAULT, + .rates[FD_PITCH] = SETTING_MANUAL_PITCH_RATE_DEFAULT, + .rates[FD_YAW] = SETTING_MANUAL_YAW_RATE_DEFAULT }, .misc = { - .fpvCamAngleDegrees = 0, + .fpvCamAngleDegrees = SETTING_FPV_MIX_DEGREES_DEFAULT } ); } diff --git a/src/main/fc/controlrate_profile.h b/src/main/fc/controlrate_profile.h index b8428347aa..b425148581 100644 --- a/src/main/fc/controlrate_profile.h +++ b/src/main/fc/controlrate_profile.h @@ -33,10 +33,8 @@ and so on. */ #define CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_MAX 180 #define CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_MIN 6 -#define CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_DEFAULT 20 #define CONTROL_RATE_CONFIG_YAW_RATE_MAX 180 #define CONTROL_RATE_CONFIG_YAW_RATE_MIN 2 -#define CONTROL_RATE_CONFIG_YAW_RATE_DEFAULT 20 #define CONTROL_RATE_CONFIG_TPA_MAX 100 diff --git a/src/main/fc/fc_core.c b/src/main/fc/fc_core.c index d4de465bac..40c18fd31f 100755 --- a/src/main/fc/fc_core.c +++ b/src/main/fc/fc_core.c @@ -417,9 +417,11 @@ void disarm(disarmReason_t disarmReason) #endif statsOnDisarm(); logicConditionReset(); -#ifdef USE_PROGRAMMING_FRAMEWORK + +#ifdef USE_PROGRAMMING_FRAMEWORK programmingPidReset(); -#endif +#endif + beeper(BEEPER_DISARMING); // emit disarm tone prearmWasReset = false; @@ -527,10 +529,11 @@ void tryArm(void) //It is required to inform the mixer that arming was executed and it has to switch to the FORWARD direction ENABLE_STATE(SET_REVERSIBLE_MOTORS_FORWARD); logicConditionReset(); - -#ifdef USE_PROGRAMMING_FRAMEWORK + +#ifdef USE_PROGRAMMING_FRAMEWORK programmingPidReset(); -#endif +#endif + headFreeModeHold = DECIDEGREES_TO_DEGREES(attitude.values.yaw); resetHeadingHoldTarget(DECIDEGREES_TO_DEGREES(attitude.values.yaw)); diff --git a/src/main/fc/fc_init.c b/src/main/fc/fc_init.c index 38f69891fb..ce6fecb053 100644 --- a/src/main/fc/fc_init.c +++ b/src/main/fc/fc_init.c @@ -220,8 +220,12 @@ void init(void) ensureEEPROMContainsValidData(); readEEPROM(); +#ifdef USE_UNDERCLOCK // Re-initialize system clock to their final values (if necessary) systemClockSetup(systemConfig()->cpuUnderclock); +#else + systemClockSetup(false); +#endif #ifdef USE_I2C i2cSetSpeed(systemConfig()->i2c_speed); diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index 0e47385044..5fdcebc9e1 100644 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -487,7 +487,11 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF sbufWriteU16(dst, gyroRateDps(i)); } for (int i = 0; i < 3; i++) { +#ifdef USE_MAG sbufWriteU16(dst, mag.magADC[i]); +#else + sbufWriteU16(dst, 0); +#endif } } break; @@ -785,12 +789,23 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF sbufWriteU8(dst, rxConfig()->rssi_channel); sbufWriteU8(dst, 0); +#ifdef USE_MAG sbufWriteU16(dst, compassConfig()->mag_declination / 10); +#else + sbufWriteU16(dst, 0); +#endif +#ifdef USE_ADC sbufWriteU8(dst, batteryMetersConfig()->voltage.scale / 10); sbufWriteU8(dst, currentBatteryProfile->voltage.cellMin / 10); sbufWriteU8(dst, currentBatteryProfile->voltage.cellMax / 10); sbufWriteU8(dst, currentBatteryProfile->voltage.cellWarning / 10); +#else + sbufWriteU8(dst, 0); + sbufWriteU8(dst, 0); + sbufWriteU8(dst, 0); + sbufWriteU8(dst, 0); +#endif break; case MSP2_INAV_MISC: @@ -813,8 +828,13 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF #endif sbufWriteU8(dst, rxConfig()->rssi_channel); +#ifdef USE_MAG sbufWriteU16(dst, compassConfig()->mag_declination / 10); +#else + sbufWriteU16(dst, 0); +#endif +#ifdef USE_ADC sbufWriteU16(dst, batteryMetersConfig()->voltage.scale); sbufWriteU8(dst, batteryMetersConfig()->voltageSource); sbufWriteU8(dst, currentBatteryProfile->cells); @@ -822,6 +842,15 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF sbufWriteU16(dst, currentBatteryProfile->voltage.cellMin); sbufWriteU16(dst, currentBatteryProfile->voltage.cellMax); sbufWriteU16(dst, currentBatteryProfile->voltage.cellWarning); +#else + sbufWriteU16(dst, 0); + sbufWriteU8(dst, 0); + sbufWriteU8(dst, 0); + sbufWriteU16(dst, 0); + sbufWriteU16(dst, 0); + sbufWriteU16(dst, 0); + sbufWriteU16(dst, 0); +#endif sbufWriteU32(dst, currentBatteryProfile->capacity.value); sbufWriteU32(dst, currentBatteryProfile->capacity.warning); @@ -841,6 +870,7 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF break; case MSP2_INAV_BATTERY_CONFIG: +#ifdef USE_ADC sbufWriteU16(dst, batteryMetersConfig()->voltage.scale); sbufWriteU8(dst, batteryMetersConfig()->voltageSource); sbufWriteU8(dst, currentBatteryProfile->cells); @@ -848,6 +878,15 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF sbufWriteU16(dst, currentBatteryProfile->voltage.cellMin); sbufWriteU16(dst, currentBatteryProfile->voltage.cellMax); sbufWriteU16(dst, currentBatteryProfile->voltage.cellWarning); +#else + sbufWriteU16(dst, 0); + sbufWriteU8(dst, 0); + sbufWriteU8(dst, 0); + sbufWriteU16(dst, 0); + sbufWriteU16(dst, 0); + sbufWriteU16(dst, 0); + sbufWriteU16(dst, 0); +#endif sbufWriteU16(dst, batteryMetersConfig()->current.offset); sbufWriteU16(dst, batteryMetersConfig()->current.scale); @@ -947,10 +986,17 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF break; case MSP_VOLTAGE_METER_CONFIG: +#ifdef USE_ADC sbufWriteU8(dst, batteryMetersConfig()->voltage.scale / 10); sbufWriteU8(dst, currentBatteryProfile->voltage.cellMin / 10); sbufWriteU8(dst, currentBatteryProfile->voltage.cellMax / 10); sbufWriteU8(dst, currentBatteryProfile->voltage.cellWarning / 10); +#else + sbufWriteU8(dst, 0); + sbufWriteU8(dst, 0); + sbufWriteU8(dst, 0); + sbufWriteU8(dst, 0); +#endif break; case MSP_CURRENT_METER_CONFIG: @@ -969,15 +1015,25 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF sbufWriteU16(dst, rxConfig()->maxcheck); sbufWriteU16(dst, PWM_RANGE_MIDDLE); sbufWriteU16(dst, rxConfig()->mincheck); +#ifdef USE_SPEKTRUM_BIND sbufWriteU8(dst, rxConfig()->spektrum_sat_bind); +#else + sbufWriteU8(dst, 0); +#endif sbufWriteU16(dst, rxConfig()->rx_min_usec); sbufWriteU16(dst, rxConfig()->rx_max_usec); sbufWriteU8(dst, 0); // for compatibility with betaflight (rcInterpolation) sbufWriteU8(dst, 0); // for compatibility with betaflight (rcInterpolationInterval) sbufWriteU16(dst, 0); // for compatibility with betaflight (airModeActivateThreshold) +#ifdef USE_RX_SPI sbufWriteU8(dst, rxConfig()->rx_spi_protocol); sbufWriteU32(dst, rxConfig()->rx_spi_id); sbufWriteU8(dst, rxConfig()->rx_spi_rf_channel_count); +#else + sbufWriteU8(dst, 0); + sbufWriteU32(dst, 0); + sbufWriteU8(dst, 0); +#endif sbufWriteU8(dst, 0); // for compatibility with betaflight (fpvCamAngleDegrees) sbufWriteU8(dst, rxConfig()->receiverType); break; @@ -1154,7 +1210,11 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF case MSP_SENSOR_ALIGNMENT: sbufWriteU8(dst, gyroConfig()->gyro_align); sbufWriteU8(dst, accelerometerConfig()->acc_align); +#ifdef USE_MAG sbufWriteU8(dst, compassConfig()->mag_align); +#else + sbufWriteU8(dst, 0); +#endif #ifdef USE_OPFLOW sbufWriteU8(dst, opticalFlowConfig()->opflow_align); #else @@ -1831,10 +1891,17 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src) sbufReadU16(src); #endif +#ifdef USE_ADC batteryMetersConfigMutable()->voltage.scale = sbufReadU8(src) * 10; currentBatteryProfileMutable->voltage.cellMin = sbufReadU8(src) * 10; // vbatlevel_warn1 in MWC2.3 GUI currentBatteryProfileMutable->voltage.cellMax = sbufReadU8(src) * 10; // vbatlevel_warn2 in MWC2.3 GUI currentBatteryProfileMutable->voltage.cellWarning = sbufReadU8(src) * 10; // vbatlevel when buzzer starts to alert +#else + sbufReadU8(src); + sbufReadU8(src); + sbufReadU8(src); + sbufReadU8(src); +#endif } else return MSP_RESULT_ERROR; break; @@ -1869,6 +1936,7 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src) sbufReadU16(src); #endif +#ifdef USE_ADC batteryMetersConfigMutable()->voltage.scale = sbufReadU16(src); batteryMetersConfigMutable()->voltageSource = sbufReadU8(src); currentBatteryProfileMutable->cells = sbufReadU8(src); @@ -1876,6 +1944,15 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src) currentBatteryProfileMutable->voltage.cellMin = sbufReadU16(src); currentBatteryProfileMutable->voltage.cellMax = sbufReadU16(src); currentBatteryProfileMutable->voltage.cellWarning = sbufReadU16(src); +#else + sbufReadU16(src); + sbufReadU8(src); + sbufReadU8(src); + sbufReadU16(src); + sbufReadU16(src); + sbufReadU16(src); + sbufReadU16(src); +#endif currentBatteryProfileMutable->capacity.value = sbufReadU32(src); currentBatteryProfileMutable->capacity.warning = sbufReadU32(src); @@ -1895,6 +1972,7 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src) case MSP2_INAV_SET_BATTERY_CONFIG: if (dataSize == 29) { +#ifdef USE_ADC batteryMetersConfigMutable()->voltage.scale = sbufReadU16(src); batteryMetersConfigMutable()->voltageSource = sbufReadU8(src); currentBatteryProfileMutable->cells = sbufReadU8(src); @@ -1902,6 +1980,15 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src) currentBatteryProfileMutable->voltage.cellMin = sbufReadU16(src); currentBatteryProfileMutable->voltage.cellMax = sbufReadU16(src); currentBatteryProfileMutable->voltage.cellWarning = sbufReadU16(src); +#else + sbufReadU16(src); + sbufReadU8(src); + sbufReadU8(src); + sbufReadU16(src); + sbufReadU16(src); + sbufReadU16(src); + sbufReadU16(src); +#endif batteryMetersConfigMutable()->current.offset = sbufReadU16(src); batteryMetersConfigMutable()->current.scale = sbufReadU16(src); @@ -2538,10 +2625,17 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src) case MSP_SET_VOLTAGE_METER_CONFIG: if (dataSize >= 4) { +#ifdef USE_ADC batteryMetersConfigMutable()->voltage.scale = sbufReadU8(src) * 10; currentBatteryProfileMutable->voltage.cellMin = sbufReadU8(src) * 10; currentBatteryProfileMutable->voltage.cellMax = sbufReadU8(src) * 10; currentBatteryProfileMutable->voltage.cellWarning = sbufReadU8(src) * 10; +#else + sbufReadU8(src); + sbufReadU8(src); + sbufReadU8(src); + sbufReadU8(src); +#endif } else return MSP_RESULT_ERROR; break; @@ -2570,15 +2664,25 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src) rxConfigMutable()->maxcheck = sbufReadU16(src); sbufReadU16(src); // midrc rxConfigMutable()->mincheck = sbufReadU16(src); +#ifdef USE_SPEKTRUM_BIND rxConfigMutable()->spektrum_sat_bind = sbufReadU8(src); +#else + sbufReadU8(src); +#endif rxConfigMutable()->rx_min_usec = sbufReadU16(src); rxConfigMutable()->rx_max_usec = sbufReadU16(src); sbufReadU8(src); // for compatibility with betaflight (rcInterpolation) sbufReadU8(src); // for compatibility with betaflight (rcInterpolationInterval) sbufReadU16(src); // for compatibility with betaflight (airModeActivateThreshold) +#ifdef USE_RX_SPI rxConfigMutable()->rx_spi_protocol = sbufReadU8(src); rxConfigMutable()->rx_spi_id = sbufReadU32(src); rxConfigMutable()->rx_spi_rf_channel_count = sbufReadU8(src); +#else + sbufReadU8(src); + sbufReadU32(src); + sbufReadU8(src); +#endif sbufReadU8(src); // for compatibility with betaflight (fpvCamAngleDegrees) rxConfigMutable()->receiverType = sbufReadU8(src); // Won't be modified if buffer is not large enough } else diff --git a/src/main/fc/fc_msp_box.h b/src/main/fc/fc_msp_box.h index cefbeab29f..ac3604d809 100644 --- a/src/main/fc/fc_msp_box.h +++ b/src/main/fc/fc_msp_box.h @@ -19,9 +19,7 @@ #include "fc/rc_modes.h" -#define BOX_PERMANENT_ID_USER1 47 -#define BOX_PERMANENT_ID_USER2 48 -#define BOX_PERMANENT_ID_NONE 255 // A permanent ID for no box mode +#include "io/piniobox.h" typedef struct box_s { const uint8_t boxId; // see boxId_e diff --git a/src/main/fc/rc_controls.c b/src/main/fc/rc_controls.c index 51b9b2d605..563d181fb6 100644 --- a/src/main/fc/rc_controls.c +++ b/src/main/fc/rc_controls.c @@ -44,6 +44,7 @@ #include "fc/rc_curves.h" #include "fc/rc_modes.h" #include "fc/runtime_config.h" +#include "fc/settings.h" #include "flight/pid.h" #include "flight/failsafe.h" @@ -73,22 +74,22 @@ FASTRAM int16_t rcCommand[4]; // interval [1000;2000] for THROTTLE and PG_REGISTER_WITH_RESET_TEMPLATE(rcControlsConfig_t, rcControlsConfig, PG_RC_CONTROLS_CONFIG, 2); PG_RESET_TEMPLATE(rcControlsConfig_t, rcControlsConfig, - .deadband = 5, - .yaw_deadband = 5, - .pos_hold_deadband = 10, - .alt_hold_deadband = 50, - .mid_throttle_deadband = 50, - .airmodeHandlingType = STICK_CENTER, - .airmodeThrottleThreshold = AIRMODE_THROTTLE_THRESHOLD, + .deadband = SETTING_DEADBAND_DEFAULT, + .yaw_deadband = SETTING_YAW_DEADBAND_DEFAULT, + .pos_hold_deadband = SETTING_POS_HOLD_DEADBAND_DEFAULT, + .alt_hold_deadband = SETTING_ALT_HOLD_DEADBAND_DEFAULT, + .mid_throttle_deadband = SETTING_3D_DEADBAND_THROTTLE_DEFAULT, + .airmodeHandlingType = SETTING_AIRMODE_TYPE_DEFAULT, + .airmodeThrottleThreshold = SETTING_AIRMODE_THROTTLE_THRESHOLD_DEFAULT, ); PG_REGISTER_WITH_RESET_TEMPLATE(armingConfig_t, armingConfig, PG_ARMING_CONFIG, 2); PG_RESET_TEMPLATE(armingConfig_t, armingConfig, - .fixed_wing_auto_arm = 0, - .disarm_kill_switch = 1, - .switchDisarmDelayMs = DEFAULT_RC_SWITCH_DISARM_DELAY_MS, - .prearmTimeoutMs = DEFAULT_PREARM_TIMEOUT, + .fixed_wing_auto_arm = SETTING_FIXED_WING_AUTO_ARM_DEFAULT, + .disarm_kill_switch = SETTING_DISARM_KILL_SWITCH_DEFAULT, + .switchDisarmDelayMs = SETTING_SWITCH_DISARM_DELAY_DEFAULT, + .prearmTimeoutMs = SETTING_PREARM_TIMEOUT_DEFAULT, ); bool areSticksInApModePosition(uint16_t ap_mode) diff --git a/src/main/fc/rc_modes.c b/src/main/fc/rc_modes.c index 785660e384..48dface10e 100755 --- a/src/main/fc/rc_modes.c +++ b/src/main/fc/rc_modes.c @@ -34,6 +34,7 @@ #include "fc/config.h" #include "fc/rc_controls.h" #include "fc/runtime_config.h" +#include "fc/settings.h" #include "rx/rx.h" @@ -56,6 +57,10 @@ boxBitmask_t rcModeActivationMask; // one bit per mode defined in boxId_e PG_REGISTER_ARRAY(modeActivationCondition_t, MAX_MODE_ACTIVATION_CONDITION_COUNT, modeActivationConditions, PG_MODE_ACTIVATION_PROFILE, 0); PG_REGISTER(modeActivationOperatorConfig_t, modeActivationOperatorConfig, PG_MODE_ACTIVATION_OPERATOR_CONFIG, 0); +PG_RESET_TEMPLATE(modeActivationOperatorConfig_t, modeActivationOperatorConfig, + .modeActivationOperator = SETTING_MODE_RANGE_LOGIC_OPERATOR_DEFAULT +); + static void processAirmodeAirplane(void) { if (feature(FEATURE_AIRMODE) || IS_RC_MODE_ACTIVE(BOXAIRMODE)) { ENABLE_STATE(AIRMODE_ACTIVE); diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 3a446cf6b6..264a1abfcb 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -171,11 +171,11 @@ groups: members: - name: looptime description: "This is the main loop time (in us). Changing this affects PID effect with some PID controllers (see PID section for details). A very conservative value of 3500us/285Hz should work for everyone. Setting it to zero does not limit loop time, so it will go as fast as possible." - default_value: "1000" + default_value: 1000 max: 9000 - name: gyro_sync description: "This option enables gyro_sync feature. In this case the loop will be synced to gyro refresh rate. Loop will always wait for the newest gyro measurement. Maximum gyro refresh rate is determined by gyro_hardware_lpf" - default_value: "OFF" + default_value: ON field: gyroSync type: bool - name: align_gyro @@ -191,7 +191,7 @@ groups: table: gyro_lpf - name: gyro_lpf_hz description: "Software-based filter to remove mechanical vibrations from the gyro signal. Value is cutoff frequency (Hz). For larger frames with bigger props set to lower value." - default_value: "60" + default_value: 60 field: gyro_soft_lpf_hz max: 200 - name: gyro_lpf_type @@ -201,72 +201,74 @@ groups: table: filter_type - name: moron_threshold description: "When powering up, gyro bias is calculated. If the model is shaking/moving during this initial calibration, offsets are calculated incorrectly, and could lead to poor flying performance. This threshold means how much average gyro reading could differ before re-calibration is triggered." - default_value: "32" + default_value: 32 field: gyroMovementCalibrationThreshold max: 128 - name: gyro_notch_hz field: gyro_notch_hz max: 500 + default_value: 0 - name: gyro_notch_cutoff field: gyro_notch_cutoff min: 1 max: 500 + default_value: 1 - name: gyro_stage2_lowpass_hz description: "Software based second stage lowpass filter for gyro. Value is cutoff frequency (Hz)" - default_value: "0" + default_value: 0 field: gyro_stage2_lowpass_hz min: 0 max: 500 - name: gyro_stage2_lowpass_type description: "Defines the type of stage 2 gyro LPF filter. Possible values: `PT1`, `BIQUAD`. `PT1` offers faster filter response while `BIQUAD` better attenuation." - default_value: "`BIQUAD`" + default_value: BIQUAD field: gyro_stage2_lowpass_type table: filter_type - name: gyro_use_dyn_lpf description: "Use Dynamic LPF instead of static gyro stage1 LPF. Dynamic Gyro LPF updates gyro LPF based on the throttle position." - default_value: "OFF" + default_value: OFF field: useDynamicLpf type: bool - name: gyro_dyn_lpf_min_hz description: "Minimum frequency of the gyro Dynamic LPF" - default_value: "200" + default_value: 200 field: gyroDynamicLpfMinHz min: 40 max: 400 - name: gyro_dyn_lpf_max_hz description: "Maximum frequency of the gyro Dynamic LPF" - default_value: "500" + default_value: 500 field: gyroDynamicLpfMaxHz min: 40 max: 1000 - name: gyro_dyn_lpf_curve_expo description: "Expo value for the throttle-to-frequency mapping for Dynamic LPF" - default_value: "5" + default_value: 5 field: gyroDynamicLpfCurveExpo min: 1 max: 10 - name: dynamic_gyro_notch_enabled description: "Enable/disable dynamic gyro notch also known as Matrix Filter" - default_value: "`OFF`" + default_value: OFF field: dynamicGyroNotchEnabled condition: USE_DYNAMIC_FILTERS type: bool - name: dynamic_gyro_notch_range description: "Range for dynamic gyro notches. `MEDIUM` for 5\", `HIGH` for 3\" and `MEDIUM`/`LOW` for 7\" and bigger propellers" - default_value: "`MEDIUM`" + default_value: "MEDIUM" field: dynamicGyroNotchRange condition: USE_DYNAMIC_FILTERS table: dynamicFilterRangeTable - name: dynamic_gyro_notch_q description: "Q factor for dynamic notches" - default_value: "120" + default_value: 120 field: dynamicGyroNotchQ condition: USE_DYNAMIC_FILTERS min: 1 max: 1000 - name: dynamic_gyro_notch_min_hz description: "Minimum frequency for dynamic notches. Default value of `150` works best with 5\" multirors. Should be lowered with increased size of propellers. Values around `100` work fine on 7\" drones. 10\" can go down to `60` - `70`" - default_value: "150" + default_value: 150 field: dynamicGyroNotchMinHz condition: USE_DYNAMIC_FILTERS min: 30 @@ -275,6 +277,7 @@ groups: condition: USE_DUAL_GYRO min: 0 max: 1 + default_value: 0 - name: PG_ADC_CHANNEL_CONFIG type: adcChannelConfig_t @@ -283,25 +286,25 @@ groups: members: - name: vbat_adc_channel description: "ADC channel to use for battery voltage sensor. Defaults to board VBAT input (if available). 0 = disabled" - default_value: "-" + default_value: :target field: adcFunctionChannel[ADC_BATTERY] min: ADC_CHN_NONE max: ADC_CHN_MAX - name: rssi_adc_channel description: "ADC channel to use for analog RSSI input. Defaults to board RSSI input (if available). 0 = disabled" - default_value: "-" + default_value: :target field: adcFunctionChannel[ADC_RSSI] min: ADC_CHN_NONE max: ADC_CHN_MAX - name: current_adc_channel description: "ADC channel to use for analog current sensor input. Defaults to board CURRENT sensor input (if available). 0 = disabled" - default_value: "-" + default_value: :target field: adcFunctionChannel[ADC_CURRENT] min: ADC_CHN_NONE max: ADC_CHN_MAX - name: airspeed_adc_channel description: "ADC channel to use for analog pitot tube (airspeed) sensor. If board doesn't have a dedicated connector for analog airspeed sensor will default to 0" - default_value: "0" + default_value: :target field: adcFunctionChannel[ADC_AIRSPEED] min: ADC_CHN_NONE max: ADC_CHN_MAX @@ -313,9 +316,11 @@ groups: - name: acc_notch_hz min: 0 max: 255 + default_value: 0 - name: acc_notch_cutoff min: 1 max: 255 + default_value: 1 - name: align_acc description: "When running on non-default hardware or adding support for new sensors/sensor boards, these values are used for sensor orientation. When carefully understood, these values can also be used to rotate (in 90deg steps) or flip the board. Possible values are: DEFAULT, CW0_DEG, CW90_DEG, CW180_DEG, CW270_DEG, CW0_DEG_FLIP, CW90_DEG_FLIP, CW180_DEG_FLIP, CW270_DEG_FLIP." default_value: "DEFAULT" @@ -328,7 +333,7 @@ groups: table: acc_hardware - name: acc_lpf_hz description: "Software-based filter to remove mechanical vibrations from the accelerometer measurements. Value is cutoff frequency (Hz). For larger frames with bigger props set to lower value." - default_value: "15" + default_value: 15 min: 0 max: 200 - name: acc_lpf_type @@ -338,37 +343,37 @@ groups: table: filter_type - name: acczero_x description: "Calculated value after '6 position avanced calibration'. See Wiki page." - default_value: "0" + default_value: 0 field: accZero.raw[X] min: INT16_MIN max: INT16_MAX - name: acczero_y description: "Calculated value after '6 position avanced calibration'. See Wiki page." - default_value: "0" + default_value: 0 field: accZero.raw[Y] min: INT16_MIN max: INT16_MAX - name: acczero_z description: "Calculated value after '6 position avanced calibration'. See Wiki page." - default_value: "0" + default_value: 0 field: accZero.raw[Z] min: INT16_MIN max: INT16_MAX - name: accgain_x description: "Calculated value after '6 position avanced calibration'. Uncalibrated value is 4096. See Wiki page." - default_value: "4096" + default_value: 4096 field: accGain.raw[X] min: 1 max: 8192 - name: accgain_y description: "Calculated value after '6 position avanced calibration'. Uncalibrated value is 4096. See Wiki page." - default_value: "4096" + default_value: 4096 field: accGain.raw[Y] min: 1 max: 8192 - name: accgain_z description: "Calculated value after '6 position avanced calibration'. Uncalibrated value is 4096. See Wiki page." - default_value: "4096" + default_value: 4096 field: accGain.raw[Z] min: 1 max: 8192 @@ -384,7 +389,7 @@ groups: default_value: "NONE" - name: rangefinder_median_filter description: "3-point median filtering for rangefinder readouts" - default_value: "OFF" + default_value: OFF field: use_median_filtering type: bool @@ -395,14 +400,15 @@ groups: members: - name: opflow_hardware description: "Selection of OPFLOW hardware." - default: "NONE" + default_value: NONE table: opflow_hardware - name: opflow_scale min: 0 max: 10000 + default_value: 10.5 - name: align_opflow description: "Optical flow module alignment (default CW0_DEG_FLIP)" - default_value: "5" + default_value: CW0FLIP field: opflow_align type: uint8_t table: alignment @@ -419,83 +425,83 @@ groups: table: secondary_imu_hardware - name: imu2_use_for_osd_heading description: "If set to ON, Secondary IMU data will be used for Analog OSD heading" - default_value: "OFF" + default_value: OFF field: useForOsdHeading type: bool - name: imu2_use_for_osd_ahi description: "If set to ON, Secondary IMU data will be used for Analog OSD Artificial Horizon" field: useForOsdAHI - default_value: "OFF" + default_value: OFF type: bool - name: imu2_use_for_stabilized description: "If set to ON, Secondary IMU data will be used for Angle, Horizon and all other modes that control attitude (PosHold, WP, RTH)" field: useForStabilized - default_value: "OFF" + default_value: OFF type: bool - name: imu2_align_roll description: "Roll alignment for Secondary IMU. 1/10 of a degree" field: rollDeciDegrees - default_value: "0" + default_value: 0 min: -1800 max: 3600 - name: imu2_align_pitch description: "Pitch alignment for Secondary IMU. 1/10 of a degree" field: pitchDeciDegrees - default_value: "0" + default_value: 0 min: -1800 max: 3600 - name: imu2_align_yaw description: "Yaw alignment for Secondary IMU. 1/10 of a degree" field: yawDeciDegrees - default_value: "0" + default_value: 0 min: -1800 max: 3600 - name: imu2_gain_acc_x description: "Secondary IMU ACC calibration data" field: calibrationOffsetAcc[X] - default_value: "0" + default_value: 0 min: INT16_MIN max: INT16_MAX - name: imu2_gain_acc_y field: calibrationOffsetAcc[Y] description: "Secondary IMU ACC calibration data" - default_value: "0" + default_value: 0 min: INT16_MIN max: INT16_MAX - name: imu2_gain_acc_z field: calibrationOffsetAcc[Z] description: "Secondary IMU ACC calibration data" - default_value: "0" + default_value: 0 min: INT16_MIN max: INT16_MAX - name: imu2_gain_mag_x field: calibrationOffsetMag[X] description: "Secondary IMU MAG calibration data" - default_value: "0" + default_value: 0 min: INT16_MIN max: INT16_MAX - name: imu2_gain_mag_y field: calibrationOffsetMag[Y] description: "Secondary IMU MAG calibration data" - default_value: "0" + default_value: 0 min: INT16_MIN max: INT16_MAX - name: imu2_gain_mag_z field: calibrationOffsetMag[Z] description: "Secondary IMU MAG calibration data" - default_value: "0" + default_value: 0 min: INT16_MIN max: INT16_MAX - name: imu2_radius_acc field: calibrationRadiusAcc description: "Secondary IMU MAG calibration data" - default_value: "0" + default_value: 0 min: INT16_MIN max: INT16_MAX - name: imu2_radius_mag field: calibrationRadiusMag description: "Secondary IMU MAG calibration data" - default_value: "0" + default_value: 0 min: INT16_MIN max: INT16_MAX @@ -516,48 +522,48 @@ groups: table: mag_hardware - name: mag_declination description: "Current location magnetic declination in format. For example, -6deg 37min = -637 for Japan. Leading zero in ddd not required. Get your local magnetic declination here: http://magnetic-declination.com/ . Not in use if inav_auto_mag_decl is turned on and you acquire valid GPS fix." - default_value: "0" + default_value: 0 min: -18000 max: 18000 - name: magzero_x description: "Magnetometer calibration X offset. If its 0 none offset has been applied and calibration is failed." - default_value: "0" + default_value: :zero field: magZero.raw[X] min: INT16_MIN max: INT16_MAX - name: magzero_y description: "Magnetometer calibration Y offset. If its 0 none offset has been applied and calibration is failed." - default_value: "0" + default_value: :zero field: magZero.raw[Y] min: INT16_MIN max: INT16_MAX - name: magzero_z description: "Magnetometer calibration Z offset. If its 0 none offset has been applied and calibration is failed." - default_value: "0" + default_value: :zero field: magZero.raw[Z] min: INT16_MIN max: INT16_MAX - name: maggain_x description: "Magnetometer calibration X gain. If 1024, no calibration or calibration failed" - default_value: "1024" + default_value: 1024 field: magGain[X] min: INT16_MIN max: INT16_MAX - name: maggain_y description: "Magnetometer calibration Y gain. If 1024, no calibration or calibration failed" - default_value: "1024" + default_value: 1024 field: magGain[Y] min: INT16_MIN max: INT16_MAX - name: maggain_z description: "Magnetometer calibration Z gain. If 1024, no calibration or calibration failed" - default_value: "1024" + default_value: 1024 field: magGain[Z] min: INT16_MIN max: INT16_MAX - name: mag_calibration_time description: "Adjust how long time the Calibration of mag will last." - default_value: "30" + default_value: 30 field: magCalibrationTimeLimit min: 20 max: 120 @@ -566,21 +572,22 @@ groups: condition: USE_DUAL_MAG min: 0 max: 1 + default_value: 0 - name: align_mag_roll description: "Set the external mag alignment on the roll axis (in 0.1 degree steps). If this value is non-zero, the compass is assumed to be externally mounted and both the board and on-board compass alignent (align_mag) are ignored. See also align_mag_pitch and align_mag_yaw." - default_value: "0" + default_value: 0 field: rollDeciDegrees min: -1800 max: 3600 - name: align_mag_pitch description: "Same as align_mag_roll, but for the pitch axis." - default_value: "0" + default_value: 0 field: pitchDeciDegrees min: -1800 max: 3600 - name: align_mag_yaw description: "Same as align_mag_roll, but for the yaw axis." - default_value: "0" + default_value: 0 field: yawDeciDegrees min: -1800 max: 3600 @@ -596,12 +603,12 @@ groups: table: baro_hardware - name: baro_median_filter description: "3-point median filtering for barometer readouts. No reason to change this setting" - default_value: "ON" + default_value: ON field: use_median_filtering type: bool - name: baro_cal_tolerance description: "Baro calibration tolerance in cm. The default should allow the noisiest baro to complete calibration [cm]." - default_value: "150" + default_value: 150 field: baro_calibration_tolerance min: 0 max: 1000 @@ -618,9 +625,11 @@ groups: - name: pitot_lpf_milli_hz min: 0 max: 10000 + default_value: 350 - name: pitot_scale min: 0 max: 100 + default_value: 1.0 - name: PG_RX_CONFIG type: rxConfig_t @@ -628,40 +637,40 @@ groups: members: - name: receiver_type description: "Selection of receiver (RX) type. Additional configuration of a `serialrx_provider` and a UART will be needed for `SERIAL`" - default_value: "`TARGET dependent`" + default_value: :target field: receiverType table: receiver_type - name: min_check description: "These are min/max values (in us) which, when a channel is smaller (min) or larger (max) than the value will activate various RC commands, such as arming, or stick configuration. Normally, every RC channel should be set so that min = 1000us, max = 2000us. On most transmitters this usually means 125% endpoints. Default check values are 100us above/below this value." - default_value: "1100" + default_value: 1100 field: mincheck min: PWM_RANGE_MIN max: PWM_RANGE_MAX - name: max_check description: "These are min/max values (in us) which, when a channel is smaller (min) or larger (max) than the value will activate various RC commands, such as arming, or stick configuration. Normally, every RC channel should be set so that min = 1000us, max = 2000us. On most transmitters this usually means 125% endpoints. Default check values are 100us above/below this value." - default_value: "1900" + default_value: 1900 field: maxcheck min: PWM_RANGE_MIN max: PWM_RANGE_MAX - name: rssi_source description: "Source of RSSI input. Possible values: `NONE`, `AUTO`, `ADC`, `CHANNEL`, `PROTOCOL`, `MSP`" - default_value: "`AUTO`" + default_value: "AUTO" field: rssi_source table: rssi_source - name: rssi_channel description: "RX channel containing the RSSI signal" - default_value: "0" + default_value: 0 min: 0 max: MAX_SUPPORTED_RC_CHANNEL_COUNT - name: rssi_min description: "The minimum RSSI value sent by the receiver, in %. For example, if your receiver's minimum RSSI value shows as 42% in the configurator/OSD set this parameter to 42. See also rssi_max. Note that rssi_min can be set to a value bigger than rssi_max to invert the RSSI calculation (i.e. bigger values mean lower RSSI)." - default_value: "0" + default_value: 0 field: rssiMin min: RSSI_VISIBLE_VALUE_MIN max: RSSI_VISIBLE_VALUE_MAX - name: rssi_max description: "The maximum RSSI value sent by the receiver, in %. For example, if your receiver's maximum RSSI value shows as 83% in the configurator/OSD set this parameter to 83. See also rssi_min." - default_value: "100" + default_value: 100 field: rssiMax min: RSSI_VISIBLE_VALUE_MIN max: RSSI_VISIBLE_VALUE_MAX @@ -669,53 +678,60 @@ groups: field: sbusSyncInterval min: 500 max: 10000 + default_value: 3000 - name: rc_filter_frequency description: "RC data biquad filter cutoff frequency. Lower cutoff frequencies result in smoother response at expense of command control delay. Practical values are 20-50. Set to zero to disable entirely and use unsmoothed RC stick values" - default_value: "50" + default_value: 50 field: rcFilterFrequency min: 0 max: 100 - name: serialrx_provider description: "When feature SERIALRX is enabled, this allows connection to several receivers which output data via digital interface resembling serial. See RX section." - default_value: "SPEK1024" + default_value: :target condition: USE_SERIAL_RX table: serial_rx - name: serialrx_inverted description: "Reverse the serial inversion of the serial RX protocol. When this value is OFF, each protocol will use its default signal (e.g. SBUS will use an inverted signal). Some OpenLRS receivers produce a non-inverted SBUS signal. This setting supports this type of receivers (including modified FrSKY)." - default_value: "OFF" + default_value: OFF condition: USE_SERIAL_RX type: bool - name: rx_spi_protocol condition: USE_RX_SPI table: rx_spi_protocol + default_value: :target - name: rx_spi_id condition: USE_RX_SPI min: 0 max: 0 + default_value: :zero - name: rx_spi_rf_channel_count + condition: USE_RX_SPI min: 0 max: 8 + default_value: :zero - name: spektrum_sat_bind description: "0 = disabled. Used to bind the spektrum satellite to RX" - default_value: "0" condition: USE_SPEKTRUM_BIND min: SPEKTRUM_SAT_BIND_DISABLED max: SPEKTRUM_SAT_BIND_MAX + default_value: :SPEKTRUM_SAT_BIND_DISABLED - name: srxl2_unit_id condition: USE_SERIALRX_SRXL2 min: 0 max: 15 + default_value: 1 - name: srxl2_baud_fast condition: USE_SERIALRX_SRXL2 - table: off_on + type: bool + default_value: ON - name: rx_min_usec description: "Defines the shortest pulse width value used when ensuring the channel value is valid. If the receiver gives a pulse value lower than this value then the channel will be marked as bad and will default to the value of mid_rc." - default_value: "885" + default_value: 885 min: PWM_PULSE_MIN max: PWM_PULSE_MAX - name: rx_max_usec description: "Defines the longest pulse width value used when ensuring the channel value is valid. If the receiver gives a pulse value higher than this value then the channel will be marked as bad and will default to the value of mid_rc." - default_value: "2115" + default_value: 2115 min: PWM_PULSE_MIN max: PWM_PULSE_MAX - name: serialrx_halfduplex @@ -725,7 +741,7 @@ groups: table: tristate - name: msp_override_channels description: "Mask of RX channels that may be overridden by MSP `SET_RAW_RC`. Note that this requires custom firmware with `USE_RX_MSP` and `USE_MSP_RC_OVERRIDE` compile options and the `MSP RC Override` flight mode." - default_value: "0" + default_value: 0 field: mspOverrideChannels condition: USE_MSP_RC_OVERRIDE min: 0 @@ -738,24 +754,24 @@ groups: members: - name: blackbox_rate_num description: "Blackbox logging rate numerator. Use num/denom settings to decide if a frame should be logged, allowing control of the portion of logged loop iterations" - default_value: "1" + default_value: 1 field: rate_num min: 1 max: 65535 - name: blackbox_rate_denom description: "Blackbox logging rate denominator. See blackbox_rate_num." - default_value: "1" + default_value: 1 field: rate_denom min: 1 max: 65535 - name: blackbox_device description: "Selection of where to write blackbox data" - default_value: "SPIFLASH" + default_value: :target field: device table: blackbox_device - name: sdcard_detect_inverted description: "This setting drives the way SD card is detected in card slot. On some targets (AnyFC F7 clone) different card slot was used and depending of hardware revision ON or OFF setting might be required. If card is not detected, change this value." - default_value: "`TARGET dependent`" + default_value: :target field: invertedCardDetection condition: USE_SDCARD type: bool @@ -766,31 +782,31 @@ groups: members: - name: max_throttle description: "This is the maximum value (in us) sent to esc when armed. Default of 1850 are OK for everyone (legacy). For modern ESCs, higher values (c. 2000) may be more appropriate. If you have brushed motors, the value should be set to 2000." - default_value: "1850" + default_value: 1850 field: maxthrottle min: PWM_RANGE_MIN max: PWM_RANGE_MAX - name: min_command description: "This is the PWM value sent to ESCs when they are not armed. If ESCs beep slowly when powered up, try decreasing this value. It can also be used for calibrating all ESCs at once." - default_value: "1000" + default_value: 1000 field: mincommand min: 0 max: PWM_RANGE_MAX - name: motor_pwm_rate description: "Output frequency (in Hz) for motor pins. Default is 400Hz for motor with motor_pwm_protocol set to STANDARD. For *SHOT (e.g. ONESHOT125) values of 1000 and 2000 have been tested by the development team and are supported. It may be possible to use higher values. For BRUSHED values of 8000 and above should be used. Setting to 8000 will use brushed mode at 8kHz switching frequency. Up to 32kHz is supported for brushed. Default is 16000 for boards with brushed motors. Note, that in brushed mode, minthrottle is offset to zero. For brushed mode, set max_throttle to 2000." - default_value: "400" + default_value: 400 field: motorPwmRate min: 50 max: 32000 - name: motor_accel_time description: "Minimum time for the motor(s) to accelerate from 0 to 100% throttle (ms) [0-1000]" - default_value: "0" + default_value: 0 field: motorAccelTimeMs min: 0 max: 1000 - name: motor_decel_time description: "Minimum time for the motor(s) to deccelerate from 100 to 0% throttle (ms) [0-1000]" - default_value: "0" + default_value: 0 field: motorDecelTimeMs min: 0 max: 1000 @@ -801,25 +817,25 @@ groups: table: motor_pwm_protocol - name: throttle_scale description: "Throttle scaling factor. `1` means no throttle scaling. `0.5` means throttle scaled down by 50%" - default_value: "1.000" + default_value: 1.0 field: throttleScale min: 0 max: 1 - name: throttle_idle description: "The percentage of the throttle range (`max_throttle` - `min_command`) above `min_command` used for minimum / idle throttle." - default_value: "15" + default_value: 15 field: throttleIdle min: 0 max: 30 - name: motor_poles field: motorPoleCount description: "The number of motor poles. Required to compute motor RPM" - default_value: "14" min: 4 max: 255 + default_value: 14 - name: flip_over_after_crash_power_factor field: flipOverAfterPowerFactor - default_value: "65" + default_value: 65 description: "flip over after crash power factor" condition: "USE_DSHOT" min: 0 @@ -831,27 +847,27 @@ groups: members: - name: failsafe_delay description: "Time in deciseconds to wait before activating failsafe when signal is lost. See [Failsafe documentation](Failsafe.md#failsafe_delay)." - default_value: "5" + default_value: 5 min: 0 max: 200 - name: failsafe_recovery_delay description: "Time in deciseconds to wait before aborting failsafe when signal is recovered. See [Failsafe documentation](Failsafe.md#failsafe_recovery_delay)." - default_value: "5" + default_value: 5 min: 0 max: 200 - name: failsafe_off_delay description: "Time in deciseconds to wait before turning off motors when failsafe is activated. 0 = No timeout. See [Failsafe documentation](Failsafe.md#failsafe_off_delay)." - default_value: "200" + default_value: 200 min: 0 max: 200 - name: failsafe_throttle description: "Throttle level used for landing when failsafe is enabled. See [Failsafe documentation](Failsafe.md#failsafe_throttle)." - default_value: "1000" + default_value: 1000 min: PWM_RANGE_MIN max: PWM_RANGE_MAX - name: failsafe_throttle_low_delay description: "If failsafe activated when throttle is low for this much time - bypass failsafe and disarm, in 10th of seconds. 0 = No timeout" - default_value: "100" + default_value: 0 min: 0 max: 300 - name: failsafe_procedure @@ -860,28 +876,28 @@ groups: table: failsafe_procedure - name: failsafe_stick_threshold description: "Threshold for stick motion to consider failsafe condition resolved. If non-zero failsafe won't clear even if RC link is restored - you have to move sticks to exit failsafe." - default_value: "50" + default_value: 50 field: failsafe_stick_motion_threshold min: 0 max: 500 - name: failsafe_fw_roll_angle description: "Amount of banking when `SET-THR` failsafe is active on a fixed-wing machine. In 1/10 deg (deci-degrees). Negative values = left roll" - default_value: "-200" + default_value: -200 min: -800 max: 800 - name: failsafe_fw_pitch_angle description: "Amount of dive/climb when `SET-THR` failsafe is active on a fixed-wing machine. In 1/10 deg (deci-degrees). Negative values = climb" - default_value: "100" + default_value: 100 min: -800 max: 800 - name: failsafe_fw_yaw_rate description: "Requested yaw rate to execute when `SET-THR` failsafe is active on a fixed-wing machine. In deg/s. Negative values = left turn" - default_value: "-45" + default_value: -45 min: -1000 max: 1000 - name: failsafe_min_distance description: "If failsafe happens when craft is closer than this distance in centimeters from home, failsafe will not execute regular failsafe_procedure, but will execute procedure specified in failsafe_min_distance_procedure instead. 0 = Normal failsafe_procedure always taken." - default_value: "0" + default_value: 0 min: 0 max: 65000 - name: failsafe_min_distance_procedure @@ -890,7 +906,7 @@ groups: table: failsafe_procedure - name: failsafe_mission description: "If set to `OFF` the failsafe procedure won't be triggered and the mission will continue if the FC is in WP (automatic mission) mode" - default_value: "ON" + default_value: ON type: bool - name: PG_LIGHTS_CONFIG @@ -900,18 +916,18 @@ groups: members: - name: failsafe_lights description: "Enable or disable the lights when the `FAILSAFE` flight mode is enabled. The target needs to be compiled with `USE_LIGHTS` [ON/OFF]." - default_value: "ON" + default_value: ON field: failsafe.enabled type: bool - name: failsafe_lights_flash_period description: "Time in milliseconds between two flashes when `failsafe_lights` is ON and `FAILSAFE` flight mode is enabled [40-65535]." - default_value: "1000" + default_value: 1000 field: failsafe.flash_period min: 40 max: 65535 - name: failsafe_lights_flash_on_time description: "Flash lights ON time in milliseconds when `failsafe_lights` is ON and `FAILSAFE` flight mode is enabled. [20-65535]." - default_value: "100" + default_value: 100 field: failsafe.flash_on_time min: 20 max: 65535 @@ -922,19 +938,19 @@ groups: members: - name: align_board_roll description: "Arbitrary board rotation in deci-degrees (0.1 degree), to allow mounting it sideways / upside down / rotated etc" - default_value: "0" + default_value: :zero field: rollDeciDegrees min: -1800 max: 3600 - name: align_board_pitch description: "Arbitrary board rotation in deci-degrees (0.1 degree), to allow mounting it sideways / upside down / rotated etc" - default_value: "0" + default_value: :zero field: pitchDeciDegrees min: -1800 max: 3600 - name: align_board_yaw description: "Arbitrary board rotation in deci-degrees (0.1 degree), to allow mounting it sideways / upside down / rotated etc" - default_value: "0" + default_value: :zero field: yawDeciDegrees min: -1800 max: 3600 @@ -945,26 +961,27 @@ groups: members: - name: vbat_meter_type description: "Vbat voltage source. Possible values: `NONE`, `ADC`, `ESC`. `ESC` required ESC telemetry enebled and running" - default_value: "`ADC`" + condition: USE_ADC + default_value: ADC field: voltage.type table: voltage_sensor type: uint8_t - name: vbat_scale description: "Battery voltage calibration value. 1100 = 11:1 voltage divider (10k:1k) x 100. Adjust this slightly if reported pack voltage is different from multimeter reading. You can get current voltage by typing \"status\" in cli." - default_value: "1100" + default_value: :target field: voltage.scale condition: USE_ADC min: VBAT_SCALE_MIN max: VBAT_SCALE_MAX - name: current_meter_scale description: "This sets the output voltage to current scaling for the current sensor in 0.1 mV/A steps. 400 is 40mV/A such as the ACS756 sensor outputs. 183 is the setting for the uberdistro with a 0.25mOhm shunt." - default_value: "400" + default_value: :target field: current.scale min: -10000 max: 10000 - name: current_meter_offset description: "This sets the output offset voltage of the current sensor in millivolts." - default_value: "0" + default_value: :target field: current.offset min: -32768 max: 32767 @@ -982,24 +999,24 @@ groups: type: uint8_t - name: cruise_power description: "Power draw at cruise throttle used for remaining flight time/distance estimation in 0.01W unit" - default_value: "0" + default_value: 0 field: cruise_power min: 0 max: 4294967295 - name: idle_power description: "Power draw at zero throttle used for remaining flight time/distance estimation in 0.01W unit" - default_value: "0" + default_value: 0 field: idle_power min: 0 max: 65535 - name: rth_energy_margin description: "Energy margin wanted after getting home (percent of battery energy capacity). Use for the remaining flight time/distance calculation" - default_value: "5" + default_value: 5 min: 0 max: 100 - name: thr_comp_weight description: "Weight used for the throttle compensation based on battery voltage. See the [battery documentation](Battery.md#automatic-throttle-compensation-based-on-battery-voltage)" - default_value: "0.692" + default_value: 1 field: throttle_compensation_weight min: 0 max: 2 @@ -1011,54 +1028,54 @@ groups: members: - name: bat_cells description: "Number of cells of the battery (0 = autodetect), see battery documentation. 7S, 9S and 11S batteries cannot be autodetected." - default_value: "0" + default_value: 0 field: cells condition: USE_ADC min: 0 max: 12 - name: vbat_cell_detect_voltage description: "Maximum voltage per cell, used for auto-detecting the number of cells of the battery in 0.01V units, default is 4.30V." - default_value: "425" + default_value: 425 field: voltage.cellDetect condition: USE_ADC min: 100 max: 500 - name: vbat_max_cell_voltage description: "Maximum voltage per cell in 0.01V units, default is 4.20V" - default_value: "420" + default_value: 420 field: voltage.cellMax condition: USE_ADC min: 100 max: 500 - name: vbat_min_cell_voltage description: "Minimum voltage per cell, this triggers battery out alarms, in 0.01V units, default is 330 (3.3V)" - default_value: "330" + default_value: 330 field: voltage.cellMin condition: USE_ADC min: 100 max: 500 - name: vbat_warning_cell_voltage description: "Warning voltage per cell, this triggers battery-warning alarms, in 0.01V units, default is 350 (3.5V)" - default_value: "350" + default_value: 350 field: voltage.cellWarning condition: USE_ADC min: 100 max: 500 - name: battery_capacity description: "Set the battery capacity in mAh or mWh (see `battery_capacity_unit`). Used to calculate the remaining battery capacity." - default_value: "0" + default_value: 0 field: capacity.value min: 0 max: 4294967295 - name: battery_capacity_warning description: "If the remaining battery capacity goes below this threshold the beeper will emit short beeps and the relevant OSD items will blink." - default_value: "0" + default_value: 0 field: capacity.warning min: 0 max: 4294967295 - name: battery_capacity_critical description: "If the remaining battery capacity goes below this threshold the battery is considered empty and the beeper will emit long beeps." - default_value: "0" + default_value: 0 field: capacity.critical min: 0 max: 4294967295 @@ -1074,29 +1091,29 @@ groups: members: - name: motor_direction_inverted description: "Use if you need to inverse yaw motor direction." - default_value: "OFF" + default_value: OFF field: motorDirectionInverted type: bool - name: platform_type description: "Defines UAV platform type. Allowed values: \"MULTIROTOR\", \"AIRPLANE\", \"HELICOPTER\", \"TRICOPTER\", \"ROVER\", \"BOAT\". Currently only MULTIROTOR, AIRPLANE and TRICOPTER types are implemented" - default_value: "\"MULTIROTOR\"" + default_value: "MULTIROTOR" field: platformType type: uint8_t table: platform_type - name: has_flaps description: "Defines is UAV is capable of having flaps. If ON and AIRPLANE `platform_type` is used, **FLAPERON** flight mode will be available for the pilot" - default_value: "OFF" + default_value: OFF field: hasFlaps type: bool - name: model_preview_type description: "ID of mixer preset applied in a Configurator. **Do not modify manually**. Used only for backup/restore reasons." - default_value: "-1" + default_value: -1 field: appliedMixerPreset min: -1 max: INT16_MAX - name: fw_min_throttle_down_pitch description: "Automatic pitch down angle when throttle is at 0 in angle mode. Progressively applied between cruise throttle and zero throttle (decidegrees)" - default_value: "0" + default_value: 0 field: fwMinThrottleDownPitchAngle min: 0 max: 450 @@ -1106,19 +1123,19 @@ groups: members: - name: 3d_deadband_low description: "Low value of throttle deadband for 3D mode (when stick is in the 3d_deadband_throttle range, the fixed values of 3d_deadband_low / _high are used instead)" - default_value: "1406" + default_value: 1406 field: deadband_low min: PWM_RANGE_MIN max: PWM_RANGE_MAX - name: 3d_deadband_high description: "High value of throttle deadband for 3D mode (when stick is in the deadband range, the value in 3d_neutral is used instead)" - default_value: "1514" + default_value: 1514 field: deadband_high min: PWM_RANGE_MIN max: PWM_RANGE_MAX - name: 3d_neutral description: "Neutral (stop) throttle value for 3D mode" - default_value: "1460" + default_value: 1460 field: neutral min: PWM_RANGE_MIN max: PWM_RANGE_MAX @@ -1134,30 +1151,30 @@ groups: table: servo_protocol - name: servo_center_pulse description: "Servo midpoint" - default_value: "1500" + default_value: 1500 field: servoCenterPulse min: PWM_RANGE_MIN max: PWM_RANGE_MAX - name: servo_pwm_rate description: "Output frequency (in Hz) servo pins. When using tricopters or gimbal with digital servo, this rate can be increased. Max of 498Hz (for 500Hz pwm period), and min of 50Hz. Most digital servos will support for example 330Hz." - default_value: "50" + default_value: 50 field: servoPwmRate min: 50 max: 498 - name: servo_lpf_hz description: "Selects the servo PWM output cutoff frequency. Value is in [Hz]" - default_value: "20" + default_value: 20 field: servo_lowpass_freq min: 0 max: 400 - name: flaperon_throw_offset description: "Defines throw range in us for both ailerons that will be passed to servo mixer via input source 14 (`FEATURE FLAPS`) when FLAPERON mode is activated." - default_value: "200" + default_value: 200 min: FLAPERON_THROW_MIN max: FLAPERON_THROW_MAX - name: tri_unarmed_servo description: "On tricopter mix only, if this is set to ON, servo will always be correcting regardless of armed state. to disable this, set it to OFF." - default_value: "ON" + default_value: ON type: bool - name: PG_CONTROL_RATE_PROFILES @@ -1167,43 +1184,43 @@ groups: members: - name: thr_mid description: "Throttle value when the stick is set to mid-position. Used in the throttle curve calculation." - default_value: "50" + default_value: 50 field: throttle.rcMid8 min: 0 max: 100 - name: thr_expo description: "Throttle exposition value" - default_value: "0" + default_value: 0 field: throttle.rcExpo8 min: 0 max: 100 - name: tpa_rate description: "Throttle PID attenuation reduces influence of P on ROLL and PITCH as throttle increases. For every 1% throttle after the TPA breakpoint, P is reduced by the TPA rate." - default_value: "0" + default_value: 0 field: throttle.dynPID min: 0 max: CONTROL_RATE_CONFIG_TPA_MAX - name: tpa_breakpoint description: "See tpa_rate." - default_value: "1500" + default_value: 1500 field: throttle.pa_breakpoint min: PWM_RANGE_MIN max: PWM_RANGE_MAX - name: fw_tpa_time_constant description: "TPA smoothing and delay time constant to reflect non-instant speed/throttle response of the plane. Planes with low thrust/weight ratio generally need higher time constant. Default is zero for compatibility with old setups" - default_value: "0" + default_value: 0 field: throttle.fixedWingTauMs min: 0 max: 5000 - name: rc_expo description: "Exposition value used for the PITCH/ROLL axes by all the stabilized flights modes (all but `MANUAL`)" - default_value: "70" + default_value: 70 field: stabilized.rcExpo8 min: 0 max: 100 - name: rc_yaw_expo description: "Exposition value used for the YAW axis by all the stabilized flights modes (all but `MANUAL`)" - default_value: "20" + default_value: 20 field: stabilized.rcYawExpo8 min: 0 max: 100 @@ -1211,49 +1228,49 @@ groups: # Rate 180 (1800dps) is max. value gyro can measure reliably - name: roll_rate description: "Defines rotation rate on ROLL axis that UAV will try to archive on max. stick deflection. Rates are defined in tens of degrees (deca-degrees) per second [rate = dps/10]. That means, rate 20 represents 200dps rotation speed. Default 20 (200dps) is more less equivalent of old Cleanflight/Baseflight rate 0. Max. 180 (1800dps) is what gyro can measure." - default_value: "20" + default_value: 20 field: stabilized.rates[FD_ROLL] min: CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_MIN max: CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_MAX - name: pitch_rate description: "Defines rotation rate on PITCH axis that UAV will try to archive on max. stick deflection. Rates are defined in tens of degrees (deca-degrees) per second [rate = dps/10]. That means, rate 20 represents 200dps rotation speed. Default 20 (200dps) is more less equivalent of old Cleanflight/Baseflight rate 0. Max. 180 (1800dps) is what gyro can measure." - default_value: "20" + default_value: 20 field: stabilized.rates[FD_PITCH] min: CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_MIN max: CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_MAX - name: yaw_rate description: "Defines rotation rate on YAW axis that UAV will try to archive on max. stick deflection. Rates are defined in tens of degrees (deca-degrees) per second [rate = dps/10]. That means, rate 20 represents 200dps rotation speed. Default 20 (200dps) is more less equivalent of old Cleanflight/Baseflight rate 0. Max. 180 (1800dps) is what gyro can measure." - default_value: "20" + default_value: 20 field: stabilized.rates[FD_YAW] min: CONTROL_RATE_CONFIG_YAW_RATE_MIN max: CONTROL_RATE_CONFIG_YAW_RATE_MAX - name: manual_rc_expo description: "Exposition value used for the PITCH/ROLL axes by the `MANUAL` flight mode [0-100]" - default_value: "70" + default_value: 70 field: manual.rcExpo8 min: 0 max: 100 - name: manual_rc_yaw_expo description: "Exposition value used for the YAW axis by the `MANUAL` flight mode [0-100]" - default_value: "20" + default_value: 20 field: manual.rcYawExpo8 min: 0 max: 100 - name: manual_roll_rate description: "Servo travel multiplier for the ROLL axis in `MANUAL` flight mode [0-100]%" - default_value: "100" + default_value: 100 field: manual.rates[FD_ROLL] min: 0 max: 100 - name: manual_pitch_rate description: "Servo travel multiplier for the PITCH axis in `MANUAL` flight mode [0-100]%" - default_value: "100" + default_value: 100 field: manual.rates[FD_PITCH] min: 0 max: 100 - name: manual_yaw_rate description: "Servo travel multiplier for the YAW axis in `MANUAL` flight mode [0-100]%" - default_value: "100" + default_value: 100 field: manual.rates[FD_YAW] min: 0 max: 100 @@ -1261,6 +1278,7 @@ groups: field: misc.fpvCamAngleDegrees min: 0 max: 50 + default_value: 0 - name: PG_SERIAL_CONFIG type: serialConfig_t @@ -1268,7 +1286,7 @@ groups: members: - name: reboot_character description: "Special character used to trigger reboot" - default_value: "82" + default_value: 82 min: 48 max: 126 @@ -1278,38 +1296,38 @@ groups: members: - name: imu_dcm_kp description: "Inertial Measurement Unit KP Gain for accelerometer measurements" - default_value: "2500" + default_value: 2500 field: dcm_kp_acc max: UINT16_MAX - name: imu_dcm_ki description: "Inertial Measurement Unit KI Gain for accelerometer measurements" - default_value: "50" + default_value: 50 field: dcm_ki_acc max: UINT16_MAX - name: imu_dcm_kp_mag description: "Inertial Measurement Unit KP Gain for compass measurements" - default_value: "10000" + default_value: 10000 field: dcm_kp_mag max: UINT16_MAX - name: imu_dcm_ki_mag description: "Inertial Measurement Unit KI Gain for compass measurements" - default_value: "0" + default_value: 0 field: dcm_ki_mag max: UINT16_MAX - name: small_angle description: "If the aircraft tilt angle exceed this value the copter will refuse to arm." - default_value: "25" + default_value: 25 min: 0 max: 180 - name: imu_acc_ignore_rate description: "Total gyro rotation rate threshold [deg/s] to consider accelerometer trustworthy on airplanes" - default_value: "0" + default_value: 0 field: acc_ignore_rate min: 0 max: 20 - name: imu_acc_ignore_slope description: "Half-width of the interval to gradually reduce accelerometer weight. Centered at `imu_acc_ignore_rate` (exactly 50% weight)" - default_value: "0" + default_value: 0 field: acc_ignore_slope min: 0 max: 5 @@ -1319,21 +1337,21 @@ groups: members: - name: fixed_wing_auto_arm description: "Auto-arm fixed wing aircraft on throttle above min_check, and disarming with stick commands are disabled, so power cycle is required to disarm. Requires enabled motorstop and no arm switch configured." - default_value: "OFF" + default_value: OFF type: bool - name: disarm_kill_switch description: "Disarms the motors independently of throttle value. Setting to OFF reverts to the old behaviour of disarming only when the throttle is low. Only applies when arming and disarming with an AUX channel." - default_value: "ON" + default_value: ON type: bool - name: switch_disarm_delay description: "Delay before disarming when requested by switch (ms) [0-1000]" - default_value: "250" + default_value: 250 field: switchDisarmDelayMs min: 0 max: 1000 - name: prearm_timeout description: "Duration (ms) for which Prearm being activated is valid. after this, Prearm needs to be reset. 0 means Prearm does not timeout." - default_value: "10000" + default_value: 10000 field: prearmTimeoutMs min: 0 max: 10000 @@ -1344,7 +1362,7 @@ groups: members: - name: applied_defaults description: "Internal (configurator) hint. Should not be changed manually" - default_value: "0" + default_value: 0 field: appliedDefaults type: uint8_t min: 0 @@ -1357,26 +1375,26 @@ groups: members: - name: rpm_gyro_filter_enabled description: "Enables gyro RPM filtere. Set to `ON` only when ESC telemetry is working and rotation speed of the motors is correctly reported to INAV" - default_value: "`OFF`" + default_value: OFF field: gyro_filter_enabled type: bool - name: rpm_gyro_harmonics description: "Number of harmonic frequences to be covered by gyro RPM filter. Default value of `1` usually works just fine" - default_value: "1" + default_value: 1 field: gyro_harmonics type: uint8_t min: 1 max: 3 - name: rpm_gyro_min_hz description: "The lowest frequency for gyro RPM filtere. Default `150` is fine for 5\" mini-quads. On 7-inch drones you can lower even down to `60`-`70`" - default_value: "150" + default_value: 100 field: gyro_min_hz type: uint8_t min: 30 max: 200 - name: rpm_gyro_q description: "Q factor for gyro RPM filter. Lower values give softer, wider attenuation. Usually there is no need to change this setting" - default_value: "500" + default_value: 500 field: gyro_q type: uint16_t min: 1 @@ -1405,22 +1423,22 @@ groups: type: uint8_t - name: gps_auto_config description: "Enable automatic configuration of UBlox GPS receivers." - default_value: "ON" + default_value: ON field: autoConfig type: bool - name: gps_auto_baud description: "Automatic configuration of GPS baudrate(The specified baudrate in configured in ports will be used) when used with UBLOX GPS. When used with NAZA/DJI it will automatic detect GPS baudrate and change to it, ignoring the selected baudrate set in ports" - default_value: "ON" + default_value: ON field: autoBaud type: bool - name: gps_ublox_use_galileo description: "Enable use of Galileo satellites. This is at the expense of other regional constellations, so benefit may also be regional. Requires M8N and Ublox firmware 3.x (or later) [OFF/ON]." - default_value: "OFF" + default_value: OFF field: ubloxUseGalileo type: bool - name: gps_min_sats description: "Minimum number of GPS satellites in view to acquire GPS_FIX and consider GPS position valid. Some GPS receivers appeared to be very inaccurate with low satellite count." - default_value: "6" + default_value: 6 field: gpsMinSats min: 5 max: 10 @@ -1431,27 +1449,27 @@ groups: members: - name: deadband description: "These are values (in us) by how much RC input can be different before it's considered valid. For transmitters with jitter on outputs, this value can be increased. Defaults are zero, but can be increased up to 10 or so if rc inputs twitch while idle." - default_value: "5" + default_value: 5 min: 0 max: 32 - name: yaw_deadband description: "These are values (in us) by how much RC input can be different before it's considered valid. For transmitters with jitter on outputs, this value can be increased. Defaults are zero, but can be increased up to 10 or so if rc inputs twitch while idle." - default_value: "5" + default_value: 5 min: 0 max: 100 - name: pos_hold_deadband description: "Stick deadband in [r/c points], applied after r/c deadband and expo" - default_value: "20" + default_value: 10 min: 2 max: 250 - name: alt_hold_deadband description: "Defines the deadband of throttle during alt_hold [r/c points]" - default_value: "50" + default_value: 50 min: 10 max: 250 - name: 3d_deadband_throttle description: "Throttle signal will be held to a fixed value when throttle is centered with an error margin defined in this parameter." - default_value: "50" + default_value: 50 field: mid_throttle_deadband min: 0 max: 200 @@ -1462,7 +1480,7 @@ groups: table: airmodeHandlingType - name: airmode_throttle_threshold description: "Defines airmode THROTTLE activation threshold when `airmode_type` **THROTTLE_THRESHOLD** is used" - default_value: "1300" + default_value: 1300 field: airmodeThrottleThreshold min: 1000 max: 2000 @@ -1474,224 +1492,224 @@ groups: members: - name: mc_p_pitch description: "Multicopter rate stabilisation P-gain for PITCH" - default_value: "40" + default_value: 40 field: bank_mc.pid[PID_PITCH].P min: 0 max: 200 - name: mc_i_pitch description: "Multicopter rate stabilisation I-gain for PITCH" - default_value: "30" + default_value: 30 field: bank_mc.pid[PID_PITCH].I min: 0 max: 200 - name: mc_d_pitch description: "Multicopter rate stabilisation D-gain for PITCH" - default_value: "23" + default_value: 23 field: bank_mc.pid[PID_PITCH].D min: 0 max: 200 - name: mc_cd_pitch description: "Multicopter Control Derivative gain for PITCH" - default_value: "60" + default_value: 60 field: bank_mc.pid[PID_PITCH].FF min: 0 max: 200 - name: mc_p_roll description: "Multicopter rate stabilisation P-gain for ROLL" - default_value: "40" + default_value: 40 field: bank_mc.pid[PID_ROLL].P min: 0 max: 200 - name: mc_i_roll description: "Multicopter rate stabilisation I-gain for ROLL" - default_value: "30" + default_value: 30 field: bank_mc.pid[PID_ROLL].I min: 0 max: 200 - name: mc_d_roll description: "Multicopter rate stabilisation D-gain for ROLL" - default_value: "23" + default_value: 23 field: bank_mc.pid[PID_ROLL].D min: 0 max: 200 - name: mc_cd_roll description: "Multicopter Control Derivative gain for ROLL" - default_value: "60" + default_value: 60 field: bank_mc.pid[PID_ROLL].FF min: 0 max: 200 - name: mc_p_yaw description: "Multicopter rate stabilisation P-gain for YAW" - default_value: "85" + default_value: 85 field: bank_mc.pid[PID_YAW].P min: 0 max: 200 - name: mc_i_yaw description: "Multicopter rate stabilisation I-gain for YAW" - default_value: "45" + default_value: 45 field: bank_mc.pid[PID_YAW].I min: 0 max: 200 - name: mc_d_yaw description: "Multicopter rate stabilisation D-gain for YAW" - default_value: "0" + default_value: 0 field: bank_mc.pid[PID_YAW].D min: 0 max: 200 - name: mc_cd_yaw description: "Multicopter Control Derivative gain for YAW" - default_value: "60" + default_value: 60 field: bank_mc.pid[PID_YAW].FF min: 0 max: 200 - name: mc_p_level description: "Multicopter attitude stabilisation P-gain" - default_value: "20" + default_value: 20 field: bank_mc.pid[PID_LEVEL].P min: 0 max: 200 - name: mc_i_level description: "Multicopter attitude stabilisation low-pass filter cutoff" - default_value: "15" + default_value: 15 field: bank_mc.pid[PID_LEVEL].I min: 0 max: 200 - name: mc_d_level description: "Multicopter attitude stabilisation HORIZON transition point" - default_value: "75" + default_value: 75 field: bank_mc.pid[PID_LEVEL].D min: 0 max: 200 - name: fw_p_pitch description: "Fixed-wing rate stabilisation P-gain for PITCH" - default_value: "5" + default_value: 5 field: bank_fw.pid[PID_PITCH].P min: 0 max: 200 - name: fw_i_pitch description: "Fixed-wing rate stabilisation I-gain for PITCH" - default_value: "7" + default_value: 7 field: bank_fw.pid[PID_PITCH].I min: 0 max: 200 - name: fw_d_pitch description: "Fixed wing rate stabilisation D-gain for PITCH" - default_value: "0" + default_value: 0 field: bank_fw.pid[PID_PITCH].D min: 0 max: 200 - name: fw_ff_pitch description: "Fixed-wing rate stabilisation FF-gain for PITCH" - default_value: "50" + default_value: 50 field: bank_fw.pid[PID_PITCH].FF min: 0 max: 200 - name: fw_p_roll description: "Fixed-wing rate stabilisation P-gain for ROLL" - default_value: "5" + default_value: 5 field: bank_fw.pid[PID_ROLL].P min: 0 max: 200 - name: fw_i_roll description: "Fixed-wing rate stabilisation I-gain for ROLL" - default_value: "7" + default_value: 7 field: bank_fw.pid[PID_ROLL].I min: 0 max: 200 - name: fw_d_roll description: "Fixed wing rate stabilisation D-gain for ROLL" - default_value: "0" + default_value: 0 field: bank_fw.pid[PID_ROLL].D min: 0 max: 200 - name: fw_ff_roll description: "Fixed-wing rate stabilisation FF-gain for ROLL" - default_value: "50" + default_value: 50 field: bank_fw.pid[PID_ROLL].FF min: 0 max: 200 - name: fw_p_yaw description: "Fixed-wing rate stabilisation P-gain for YAW" - default_value: "6" + default_value: 6 field: bank_fw.pid[PID_YAW].P min: 0 max: 200 - name: fw_i_yaw description: "Fixed-wing rate stabilisation I-gain for YAW" - default_value: "10" + default_value: 10 field: bank_fw.pid[PID_YAW].I min: 0 max: 200 - name: fw_d_yaw description: "Fixed wing rate stabilisation D-gain for YAW" - default_value: "0" + default_value: 0 field: bank_fw.pid[PID_YAW].D min: 0 max: 200 - name: fw_ff_yaw description: "Fixed-wing rate stabilisation FF-gain for YAW" - default_value: "60" + default_value: 60 field: bank_fw.pid[PID_YAW].FF min: 0 max: 200 - name: fw_p_level description: "Fixed-wing attitude stabilisation P-gain" - default_value: "20" + default_value: 20 field: bank_fw.pid[PID_LEVEL].P min: 0 max: 200 - name: fw_i_level description: "Fixed-wing attitude stabilisation low-pass filter cutoff" - default_value: "5" + default_value: 5 field: bank_fw.pid[PID_LEVEL].I min: 0 max: 200 - name: fw_d_level description: "Fixed-wing attitude stabilisation HORIZON transition point" - default_value: "75" + default_value: 75 field: bank_fw.pid[PID_LEVEL].D min: 0 max: 200 - name: max_angle_inclination_rll description: "Maximum inclination in level (angle) mode (ROLL axis). 100=10°" - default_value: "300" + default_value: 300 field: max_angle_inclination[FD_ROLL] min: 100 max: 900 - name: max_angle_inclination_pit description: "Maximum inclination in level (angle) mode (PITCH axis). 100=10°" - default_value: "300" + default_value: 300 field: max_angle_inclination[FD_PITCH] min: 100 max: 900 - name: dterm_lpf_hz description: "Dterm low pass filter cutoff frequency. Default setting is very conservative and small multirotors should use higher value between 80 and 100Hz. 80 seems like a gold spot for 7-inch builds while 100 should work best with 5-inch machines. If motors are getting too hot, lower the value" - default_value: "40" + default_value: 40 min: 0 max: 500 - name: dterm_lpf_type description: "Defines the type of stage 1 D-term LPF filter. Possible values: `PT1`, `BIQUAD`. `PT1` offers faster filter response while `BIQUAD` better attenuation." - default_value: "`BIQUAD`" + default_value: "BIQUAD" field: dterm_lpf_type table: filter_type - name: dterm_lpf2_hz description: "Cutoff frequency for stage 2 D-term low pass filter" - default_value: "0" + default_value: 0 min: 0 max: 500 - name: dterm_lpf2_type description: "Defines the type of stage 1 D-term LPF filter. Possible values: `PT1`, `BIQUAD`. `PT1` offers faster filter response while `BIQUAD` better attenuation." - default_value: "`BIQUAD`" + default_value: "BIQUAD" field: dterm_lpf2_type table: filter_type - name: yaw_lpf_hz description: "Yaw low pass filter cutoff frequency. Should be disabled (set to `0`) on small multirotors (7 inches and below)" - default_value: "30" + default_value: 0 min: 0 max: 200 - name: fw_iterm_throw_limit description: "Limits max/min I-term value in stabilization PID controller in case of Fixed Wing. It solves the problem of servo saturation before take-off/throwing the airplane into the air. By default, error accumulated in I-term can not exceed 1/3 of servo throw (around 165us). Set 0 to disable completely." - default_value: "165" + default_value: 165 field: fixedWingItermThrowLimit min: FW_ITERM_THROW_LIMIT_MIN max: FW_ITERM_THROW_LIMIT_MAX @@ -1703,132 +1721,134 @@ groups: table: direction - name: fw_reference_airspeed description: "Reference airspeed. Set this to airspeed at which PIDs were tuned. Usually should be set to cruise airspeed. Also used for coordinated turn calculation if airspeed sensor is not present." - default_value: "1000" + default_value: 1000 field: fixedWingReferenceAirspeed min: 1 max: 5000 - name: fw_turn_assist_yaw_gain description: "Gain required to keep the yaw rate consistent with the turn rate for a coordinated turn (in TURN_ASSIST mode). Value significantly different from 1.0 indicates a problem with the airspeed calibration (if present) or value of `fw_reference_airspeed` parameter" - default_value: "1" + default_value: 1 field: fixedWingCoordinatedYawGain min: 0 max: 2 - name: fw_turn_assist_pitch_gain description: "Gain required to keep constant pitch angle during coordinated turns (in TURN_ASSIST mode). Value significantly different from 1.0 indicates a problem with the airspeed calibration (if present) or value of `fw_reference_airspeed` parameter" - default_value: "1" + default_value: 1 field: fixedWingCoordinatedPitchGain min: 0 max: 2 - name: fw_iterm_limit_stick_position description: "Iterm is not allowed to grow when stick position is above threshold. This solves the problem of bounceback or followthrough when full stick deflection is applied on poorely tuned fixed wings. In other words, stabilization is partialy disabled when pilot is actively controlling the aircraft and active when sticks are not touched. `0` mean stick is in center position, `1` means it is fully deflected to either side" - default_value: "0.5" + default_value: 0.5 field: fixedWingItermLimitOnStickPosition min: 0 max: 1 - name: fw_yaw_iterm_freeze_bank_angle description: "Yaw Iterm is frozen when bank angle is above this threshold [degrees]. This solves the problem of the rudder counteracting turns by partially disabling yaw stabilization when making banked turns. Setting to 0 (the default) disables this feature. Only applies when autopilot is not active and TURN ASSIST is disabled." - default_value: "0" + default_value: 0 field: fixedWingYawItermBankFreeze min: 0 max: 90 - name: pidsum_limit description: "A limitation to overall amount of correction Flight PID can request on each axis (Roll/Pitch). If when doing a hard maneuver on one axis machine looses orientation on other axis - reducing this parameter may help" - default_value: "500" + default_value: 500 field: pidSumLimit min: PID_SUM_LIMIT_MIN max: PID_SUM_LIMIT_MAX - name: pidsum_limit_yaw description: "A limitation to overall amount of correction Flight PID can request on each axis (Yaw). If when doing a hard maneuver on one axis machine looses orientation on other axis - reducing this parameter may help" - default_value: "400" + default_value: 350 field: pidSumLimitYaw min: PID_SUM_LIMIT_MIN max: PID_SUM_LIMIT_MAX - name: iterm_windup description: "Used to prevent Iterm accumulation on during maneuvers. Iterm will be dampened when motors are reaching it's limit (when requested motor correction range is above percentage specified by this parameter)" - default_value: "50" + default_value: 50 field: itermWindupPointPercent min: 0 max: 90 - name: rate_accel_limit_roll_pitch description: "Limits acceleration of ROLL/PITCH rotation speed that can be requested by stick input. In degrees-per-second-squared. Small and powerful UAV flies great with high acceleration limit ( > 5000 dps^2 and even > 10000 dps^2). Big and heavy multirotors will benefit from low acceleration limit (~ 360 dps^2). When set correctly, it greatly improves stopping performance. Value of 0 disables limiting." - default_value: "0" + default_value: 0 field: axisAccelerationLimitRollPitch max: 500000 - name: rate_accel_limit_yaw description: "Limits acceleration of YAW rotation speed that can be requested by stick input. In degrees-per-second-squared. Small and powerful UAV flies great with high acceleration limit ( > 10000 dps^2). Big and heavy multirotors will benefit from low acceleration limit (~ 180 dps^2). When set correctly, it greatly improves stopping performance and general stability during yaw turns. Value of 0 disables limiting." - default_value: "10000" + default_value: 10000 field: axisAccelerationLimitYaw max: 500000 - name: heading_hold_rate_limit description: "This setting limits yaw rotation rate that HEADING_HOLD controller can request from PID inner loop controller. It is independent from manual yaw rate and used only when HEADING_HOLD flight mode is enabled by pilot, RTH or WAYPOINT modes." - default_value: "90" min: HEADING_HOLD_RATE_LIMIT_MIN max: HEADING_HOLD_RATE_LIMIT_MAX + default_value: 90 - name: nav_mc_pos_z_p description: "P gain of altitude PID controller (Multirotor)" - default_value: "50" field: bank_mc.pid[PID_POS_Z].P condition: USE_NAV min: 0 max: 255 + default_value: 50 - name: nav_mc_vel_z_p description: "P gain of velocity PID controller" - default_value: "100" field: bank_mc.pid[PID_VEL_Z].P condition: USE_NAV min: 0 max: 255 + default_value: 100 - name: nav_mc_vel_z_i description: "I gain of velocity PID controller" - default_value: "50" field: bank_mc.pid[PID_VEL_Z].I condition: USE_NAV min: 0 max: 255 + default_value: 50 - name: nav_mc_vel_z_d description: "D gain of velocity PID controller" - default_value: "10" + default_value: 10 field: bank_mc.pid[PID_VEL_Z].D condition: USE_NAV min: 0 max: 255 + default_value: 10 - name: nav_mc_pos_xy_p description: "Controls how fast the drone will fly towards the target position. This is a multiplier to convert displacement to target velocity" - default_value: "65" field: bank_mc.pid[PID_POS_XY].P condition: USE_NAV min: 0 max: 255 + default_value: 65 - name: nav_mc_vel_xy_p description: "P gain of Position-Rate (Velocity to Acceleration) PID controller. Higher P means stronger response when position error occurs. Too much P might cause \"nervous\" behavior and oscillations" - default_value: "40" field: bank_mc.pid[PID_VEL_XY].P condition: USE_NAV min: 0 max: 255 + default_value: 40 - name: nav_mc_vel_xy_i description: "I gain of Position-Rate (Velocity to Acceleration) PID controller. Used for drift compensation (caused by wind for example). Higher I means stronger response to drift. Too much I gain might cause target overshot" - default_value: "15" field: bank_mc.pid[PID_VEL_XY].I condition: USE_NAV min: 0 max: 255 + default_value: 15 - name: nav_mc_vel_xy_d description: "D gain of Position-Rate (Velocity to Acceleration) PID controller. It can damp P and I. Increasing D might help when drone overshoots target." - default_value: "100" field: bank_mc.pid[PID_VEL_XY].D condition: USE_NAV min: 0 max: 255 + default_value: 100 - name: nav_mc_vel_xy_ff field: bank_mc.pid[PID_VEL_XY].FF condition: USE_NAV min: 0 max: 255 + default_value: 40 - name: nav_mc_heading_p description: "P gain of Heading Hold controller (Multirotor)" - default_value: "60" + default_value: 60 field: bank_mc.pid[PID_HEADING].P condition: USE_NAV min: 0 @@ -1837,139 +1857,145 @@ groups: field: navVelXyDTermLpfHz min: 0 max: 100 + default_value: 2 - name: nav_mc_vel_xy_dterm_attenuation description: "Maximum D-term attenution percentage for horizontal velocity PID controller (Multirotor). It allows to smooth the PosHold CRUISE, WP and RTH when Multirotor is traveling at full speed. Dterm is not attenuated at low speeds, breaking and accelerating." - default_value: "90" field: navVelXyDtermAttenuation min: 0 max: 100 + default_value: 90 - name: nav_mc_vel_xy_dterm_attenuation_start description: "A point (in percent of both target and current horizontal velocity) where nav_mc_vel_xy_dterm_attenuation begins" - default_value: 10" + default_value: 10 field: navVelXyDtermAttenuationStart min: 0 max: 100 - name: nav_mc_vel_xy_dterm_attenuation_end description: "A point (in percent of both target and current horizontal velocity) where nav_mc_vel_xy_dterm_attenuation reaches maximum" - default_value: "60" + default_value: 60 field: navVelXyDtermAttenuationEnd min: 0 max: 100 - name: nav_fw_pos_z_p description: "P gain of altitude PID controller (Fixedwing)" - default_value: "50" + default_value: 40 field: bank_fw.pid[PID_POS_Z].P condition: USE_NAV min: 0 max: 255 - name: nav_fw_pos_z_i description: "I gain of altitude PID controller (Fixedwing)" - default_value: "0" + default_value: 5 field: bank_fw.pid[PID_POS_Z].I condition: USE_NAV min: 0 max: 255 - name: nav_fw_pos_z_d description: "D gain of altitude PID controller (Fixedwing)" - default_value: "0" + default_value: 10 field: bank_fw.pid[PID_POS_Z].D condition: USE_NAV min: 0 max: 255 - name: nav_fw_pos_xy_p description: "P gain of 2D trajectory PID controller. Play with this to get a straight line between waypoints or a straight RTH" - default_value: "75" + default_value: 75 field: bank_fw.pid[PID_POS_XY].P condition: USE_NAV min: 0 max: 255 - name: nav_fw_pos_xy_i description: "I gain of 2D trajectory PID controller. Too high and there will be overshoot in trajectory. Better start tuning with zero" - default_value: "5" + default_value: 5 field: bank_fw.pid[PID_POS_XY].I condition: USE_NAV min: 0 max: 255 - name: nav_fw_pos_xy_d description: "D gain of 2D trajectory PID controller. Too high and there will be overshoot in trajectory. Better start tuning with zero" - default_value: "8" + default_value: 8 field: bank_fw.pid[PID_POS_XY].D condition: USE_NAV min: 0 max: 255 - name: nav_fw_heading_p description: "P gain of Heading Hold controller (Fixedwing)" - default_value: "60" + default_value: 60 field: bank_fw.pid[PID_HEADING].P condition: USE_NAV min: 0 max: 255 - name: nav_fw_pos_hdg_p description: "P gain of heading PID controller. (Fixedwing, rovers, boats)" - default_value: "60" + default_value: 30 field: bank_fw.pid[PID_POS_HEADING].P condition: USE_NAV min: 0 max: 255 - name: nav_fw_pos_hdg_i description: "I gain of heading trajectory PID controller. (Fixedwing, rovers, boats)" - default_value: "0" + default_value: 2 field: bank_fw.pid[PID_POS_HEADING].I condition: USE_NAV min: 0 max: 255 - name: nav_fw_pos_hdg_d description: "D gain of heading trajectory PID controller. (Fixedwing, rovers, boats)" - default_value: "0" + default_value: 0 field: bank_fw.pid[PID_POS_HEADING].D condition: USE_NAV min: 0 max: 255 - name: nav_fw_pos_hdg_pidsum_limit description: "Output limit for heading trajectory PID controller. (Fixedwing, rovers, boats)" - default_value: "350" + default_value: 350 field: navFwPosHdgPidsumLimit min: PID_SUM_LIMIT_MIN max: PID_SUM_LIMIT_MAX - name: mc_iterm_relax field: iterm_relax table: iterm_relax + default_value: RP - name: mc_iterm_relax_cutoff field: iterm_relax_cutoff min: 1 max: 100 + default_value: 15 - name: d_boost_factor field: dBoostFactor condition: USE_D_BOOST min: 1 max: 3 + default_value: 1.25 - name: d_boost_max_at_acceleration field: dBoostMaxAtAlleceleration condition: USE_D_BOOST min: 1000 max: 16000 + default_value: 7500 - name: d_boost_gyro_delta_lpf_hz field: dBoostGyroDeltaLpfHz condition: USE_D_BOOST min: 10 max: 250 + default_value: 80 - name: antigravity_gain description: "Max Antigravity gain. `1` means Antigravity is disabled, `2` means Iterm is allowed to double during rapid throttle movements" - default_value: "1" + default_value: 1 field: antigravityGain condition: USE_ANTIGRAVITY min: 1 max: 20 - name: antigravity_accelerator description: "" - default_value: "1" + default_value: 1 field: antigravityAccelerator condition: USE_ANTIGRAVITY min: 1 max: 20 - name: antigravity_cutoff_lpf_hz description: "Antigravity cutoff frequenct for Throtte filter. Antigravity is based on the difference between actual and filtered throttle input. The bigger is the difference, the bigger Antigravity gain" - default_value: "15" + default_value: 15 field: antigravityCutoff condition: USE_ANTIGRAVITY min: 1 @@ -1978,42 +2004,43 @@ groups: description: "Allows to set type of PID controller used in control loop. Possible values: `NONE`, `PID`, `PIFF`, `AUTO`. Change only in case of experimental platforms like VTOL, tailsitters, rovers, boats, etc. Airplanes should always use `PIFF` and multirotors `PID`" field: pidControllerType table: pidTypeTable + default_value: AUTO - name: mc_cd_lpf_hz description: "Cutoff frequency for Control Derivative. Lower value smoother reaction on fast stick movements. With higher values, response will be more aggressive, jerky" - default_value: "30" + default_value: 30 field: controlDerivativeLpfHz min: 0 max: 200 - name: setpoint_kalman_enabled description: "Enable Kalman filter on the PID controller setpoint" - default_value: "OFF" + default_value: OFF condition: USE_GYRO_KALMAN field: kalmanEnabled type: bool - name: setpoint_kalman_q description: "Quality factor of the setpoint Kalman filter. Higher values means less filtering and lower phase delay. On 3-7 inch multirotors can be usually increased to 200-300 or even higher of clean builds" - default_value: "100" + default_value: 100 field: kalman_q condition: USE_GYRO_KALMAN min: 1 max: 16000 - name: setpoint_kalman_w description: "Window size for the setpoint Kalman filter. Wider the window, more samples are used to compute variance. In general, wider window results in smoother filter response" - default_value: "4" + default_value: 4 field: kalman_w condition: USE_GYRO_KALMAN min: 1 max: 40 - name: setpoint_kalman_sharpness description: "Dynamic factor for the setpoint Kalman filter. In general, the higher the value, the more dynamic Kalman filter gets" - default_value: "100" + default_value: 100 field: kalman_sharpness condition: USE_GYRO_KALMAN min: 1 max: 16000 - name: fw_level_pitch_trim description: "Pitch trim for self-leveling flight modes. In degrees. +5 means airplane nose should be raised 5 deg from level" - default_value: "0" + default_value: 0 field: fixedWingLevelTrim min: -10 max: 10 @@ -2024,31 +2051,31 @@ groups: members: - name: fw_autotune_overshoot_time description: "Time [ms] to detect sustained overshoot" - default_value: "100" + default_value: 100 field: fw_overshoot_time min: 50 max: 500 - name: fw_autotune_undershoot_time description: "Time [ms] to detect sustained undershoot" - default_value: "200" + default_value: 200 field: fw_undershoot_time min: 50 max: 500 - name: fw_autotune_threshold description: "Threshold [%] of max rate to consider overshoot/undershoot detection" - default_value: "50" + default_value: 50 field: fw_max_rate_threshold min: 0 max: 100 - name: fw_autotune_ff_to_p_gain description: "FF to P gain (strength relationship) [%]" - default_value: "10" + default_value: 10 field: fw_ff_to_p_gain min: 0 max: 100 - name: fw_autotune_ff_to_i_tc description: "FF to I time (defines time for I to reach the same level of response as FF) [ms]" - default_value: "600" + default_value: 600 field: fw_ff_to_i_time_constant min: 100 max: 5000 @@ -2059,26 +2086,27 @@ groups: members: - name: inav_auto_mag_decl description: "Automatic setting of magnetic declination based on GPS position. When used manual magnetic declination is ignored." - default_value: "ON" + default_value: ON field: automatic_mag_declination type: bool - name: inav_gravity_cal_tolerance description: "Unarmed gravity calibration tolerance level. Won't finish the calibration until estimated gravity error falls below this value." - default_value: "5" + default_value: 5 field: gravity_calibration_tolerance min: 0 max: 255 - name: inav_use_gps_velned description: "Defined if iNav should use velocity data provided by GPS module for doing position and speed estimation. If set to OFF iNav will fallback to calculating velocity from GPS coordinates. Using native velocity data may improve performance on some GPS modules. Some GPS modules introduce significant delay and using native velocity may actually result in much worse performance." - default_value: "ON" + default_value: ON field: use_gps_velned type: bool - name: inav_use_gps_no_baro field: use_gps_no_baro type: bool + default_value: OFF - name: inav_allow_dead_reckoning description: "Defines if inav will dead-reckon over short GPS outages. May also be useful for indoors OPFLOW navigation" - default_value: "OFF" + default_value: OFF field: allow_dead_reckoning type: bool - name: inav_reset_altitude @@ -2093,7 +2121,7 @@ groups: table: reset_type - name: inav_max_surface_altitude description: "Max allowed altitude for surface following mode. [cm]" - default_value: "200" + default_value: 200 field: max_surface_altitude min: 0 max: 1000 @@ -2101,57 +2129,61 @@ groups: field: w_z_surface_p min: 0 max: 100 + default_value: 3.5 - name: inav_w_z_surface_v field: w_z_surface_v min: 0 max: 100 + default_value: 6.1 - name: inav_w_xy_flow_p field: w_xy_flow_p min: 0 max: 100 + default_value: 1.0 - name: inav_w_xy_flow_v field: w_xy_flow_v min: 0 max: 100 + default_value: 2.0 - name: inav_w_z_baro_p description: "Weight of barometer measurements in estimated altitude and climb rate" - default_value: "0.350" field: w_z_baro_p min: 0 max: 10 + default_value: 0.35 - name: inav_w_z_gps_p description: "Weight of GPS altitude measurements in estimated altitude. Setting is used only of airplanes" - default_value: "0.200" field: w_z_gps_p min: 0 max: 10 + default_value: 0.2 - name: inav_w_z_gps_v description: "Weight of GPS climb rate measurements in estimated climb rate. Setting is used on both airplanes and multirotors. If GPS doesn't support native climb rate reporting (i.e. NMEA GPS) you may consider setting this to zero" - default_value: "0.500" field: w_z_gps_v min: 0 max: 10 + default_value: 0.1 - name: inav_w_xy_gps_p description: "Weight of GPS coordinates in estimated UAV position and speed." - default_value: "1.000" + default_value: 1.0 field: w_xy_gps_p min: 0 max: 10 - name: inav_w_xy_gps_v description: "Weight of GPS velocity data in estimated UAV speed" - default_value: "2.000" + default_value: 2.0 field: w_xy_gps_v min: 0 max: 10 - name: inav_w_z_res_v description: "Decay coefficient for estimated climb rate when baro/GPS reference for altitude is lost" - default_value: "0.500" + default_value: 0.5 field: w_z_res_v min: 0 max: 10 - name: inav_w_xy_res_v description: "Decay coefficient for estimated velocity when GPS reference for position is lost" - default_value: "0.500" + default_value: 0.5 field: w_xy_res_v min: 0 max: 10 @@ -2159,21 +2191,22 @@ groups: field: w_xyz_acc_p min: 0 max: 1 + default_value: 1.0 - name: inav_w_acc_bias description: "Weight for accelerometer drift estimation" - default_value: "0.010" + default_value: 0.01 field: w_acc_bias min: 0 max: 1 - name: inav_max_eph_epv description: "Maximum uncertainty value until estimated position is considered valid and is used for navigation [cm]" - default_value: "1000.000" + default_value: 1000 field: max_eph_epv min: 0 max: 9999 - name: inav_baro_epv description: "Uncertainty value for barometric sensor [cm]" - default_value: "100.000" + default_value: 100 field: baro_epv min: 0 max: 9999 @@ -2185,12 +2218,12 @@ groups: members: - name: nav_disarm_on_landing description: "If set to ON, iNav disarms the FC after landing" - default_value: "OFF" + default_value: OFF field: general.flags.disarm_on_landing type: bool - name: nav_use_midthr_for_althold description: "If set to OFF, the FC remembers your throttle stick position when enabling ALTHOLD and treats it as a neutral midpoint for holding altitude" - default_value: "OFF" + default_value: OFF field: general.flags.use_thr_mid_for_althold type: bool - name: nav_extra_arming_safety @@ -2205,72 +2238,72 @@ groups: table: nav_user_control_mode - name: nav_position_timeout description: "If GPS fails wait for this much seconds before switching to emergency landing mode (0 - disable)" - default_value: "5" + default_value: 5 field: general.pos_failure_timeout min: 0 max: 10 - name: nav_wp_radius description: "Waypoint radius [cm]. Waypoint would be considered reached if machine is within this radius" - default_value: "100" + default_value: 100 field: general.waypoint_radius min: 10 max: 10000 - name: nav_wp_safe_distance description: "First waypoint in the mission should be closer than this value [cm]. A value of 0 disables this check." - default_value: "10000" + default_value: 10000 field: general.waypoint_safe_distance max: 65000 - name: nav_auto_speed description: "Maximum velocity firmware is allowed in full auto modes (RTH, WP) [cm/s] [Multirotor only]" - default_value: "300" + default_value: 300 field: general.max_auto_speed min: 10 max: 2000 - name: nav_auto_climb_rate description: "Maximum climb/descent rate that UAV is allowed to reach during navigation modes. [cm/s]" - default_value: "500" + default_value: 500 field: general.max_auto_climb_rate min: 10 max: 2000 - name: nav_manual_speed description: "Maximum velocity firmware is allowed when processing pilot input for POSHOLD/CRUISE control mode [cm/s] [Multirotor only]" - default_value: "500" + default_value: 500 field: general.max_manual_speed min: 10 max: 2000 - name: nav_manual_climb_rate description: "Maximum climb/descent rate firmware is allowed when processing pilot input for ALTHOLD control mode [cm/s]" - default_value: "200" + default_value: 200 field: general.max_manual_climb_rate min: 10 max: 2000 - name: nav_landing_speed description: "Vertical descent velocity during the RTH landing phase. [cm/s]" - default_value: "200" + default_value: 200 field: general.land_descent_rate min: 100 max: 2000 - name: nav_land_slowdown_minalt description: "Defines at what altitude the descent velocity should start to be 25% of nav_landing_speed [cm]" - default_value: "500" + default_value: 500 field: general.land_slowdown_minalt min: 50 max: 1000 - name: nav_land_slowdown_maxalt description: "Defines at what altitude the descent velocity should start to ramp down from 100% nav_landing_speed to 25% nav_landing_speed. [cm]" - default_value: "2000" + default_value: 2000 field: general.land_slowdown_maxalt min: 500 max: 4000 - name: nav_emerg_landing_speed description: "Rate of descent UAV will try to maintain when doing emergency descent sequence [cm/s]" - default_value: "500" + default_value: 500 field: general.emerg_descent_rate min: 100 max: 2000 - name: nav_min_rth_distance description: "Minimum distance from homepoint when RTH full procedure will be activated [cm]. Below this distance, the mode will activate at the current location and the final phase is executed (loiter / land). Above this distance, the full procedure is activated, which may include initial climb and flying directly to the homepoint before entering the loiter / land phase." - default_value: "500" + default_value: 500 field: general.min_rth_distance min: 0 max: 5000 @@ -2281,17 +2314,17 @@ groups: table: nav_overrides_motor_stop - name: nav_rth_climb_first description: "If set to ON drone will climb to nav_rth_altitude first and head home afterwards. If set to OFF drone will head home instantly and climb on the way." - default_value: "ON" + default_value: ON field: general.flags.rth_climb_first type: bool - name: nav_rth_climb_ignore_emerg description: "If set to ON, aircraft will execute initial climb regardless of position sensor (GPS) status." - default_value: "OFF" + default_value: OFF field: general.flags.rth_climb_ignore_emerg type: bool - name: nav_rth_tail_first description: "If set to ON drone will return tail-first. Obviously meaningless for airplanes." - default_value: "OFF" + default_value: OFF field: general.flags.rth_tail_first type: bool - name: nav_rth_allow_landing @@ -2306,7 +2339,7 @@ groups: table: nav_rth_alt_mode - name: nav_rth_abort_threshold description: "RTH sanity checking feature will notice if distance to home is increasing during RTH and once amount of increase exceeds the threshold defined by this parameter, instead of continuing RTH machine will enter emergency landing, self-level and go down safely. Default is 500m which is safe enough for both multirotor machines and airplanes. [cm]" - default_value: "50000" + default_value: 50000 field: general.rth_abort_threshold max: 65000 - name: nav_max_terrain_follow_alt @@ -2314,191 +2347,192 @@ groups: default_value: "100" description: "Max allowed above the ground altitude for terrain following mode" max: 1000 + default_value: 100 - name: nav_rth_altitude description: "Used in EXTRA, FIXED and AT_LEAST rth alt modes [cm] (Default 1000 means 10 meters)" - default_value: "1000" + default_value: 1000 field: general.rth_altitude max: 65000 - name: nav_rth_home_altitude description: "Aircraft will climb/descend to this altitude after reaching home if landing is not enabled. Set to 0 to stay at `nav_rth_altitude` (default) [cm]" - default_value: "0" + default_value: 0 field: general.rth_home_altitude max: 65000 - name: safehome_max_distance description: "In order for a safehome to be used, it must be less than this distance (in cm) from the arming point." - default_value: "20000" + default_value: 20000 field: general.safehome_max_distance min: 0 max: 65000 - name: nav_mc_bank_angle description: "Maximum banking angle (deg) that multicopter navigation is allowed to set. Machine must be able to satisfy this angle without loosing altitude" - default_value: "30" + default_value: 30 field: mc.max_bank_angle min: 15 max: 45 - name: nav_mc_hover_thr description: "Multicopter hover throttle hint for altitude controller. Should be set to approximate throttle value when drone is hovering." - default_value: "1500" + default_value: 1500 field: mc.hover_throttle min: 1000 max: 2000 - name: nav_mc_auto_disarm_delay description: "Delay before multi-rotor disarms when `nav_disarm_on_landing` is set (m/s)" - default_value: "2000" + default_value: 2000 field: mc.auto_disarm_delay min: 100 max: 10000 - name: nav_mc_braking_speed_threshold description: "min speed in cm/s above which braking can happen" - default_value: "100" + default_value: 100 field: mc.braking_speed_threshold condition: USE_MR_BRAKING_MODE min: 0 max: 1000 - name: nav_mc_braking_disengage_speed description: "braking is disabled when speed goes below this value" - default_value: "75" + default_value: 75 field: mc.braking_disengage_speed condition: USE_MR_BRAKING_MODE min: 0 max: 1000 - name: nav_mc_braking_timeout description: "timeout in ms for braking" - default_value: "2000" + default_value: 2000 field: mc.braking_timeout condition: USE_MR_BRAKING_MODE min: 100 max: 5000 - name: nav_mc_braking_boost_factor description: "acceleration factor for BOOST phase" - default_value: "100" + default_value: 100 field: mc.braking_boost_factor condition: USE_MR_BRAKING_MODE min: 0 max: 200 - name: nav_mc_braking_boost_timeout description: "how long in ms BOOST phase can happen" - default_value: "750" + default_value: 750 field: mc.braking_boost_timeout condition: USE_MR_BRAKING_MODE min: 0 max: 5000 - name: nav_mc_braking_boost_speed_threshold description: "BOOST can be enabled when speed is above this value" - default_value: "150" + default_value: 150 field: mc.braking_boost_speed_threshold condition: USE_MR_BRAKING_MODE min: 100 max: 1000 - name: nav_mc_braking_boost_disengage_speed description: "BOOST will be disabled when speed goes below this value" - default_value: "100" + default_value: 100 field: mc.braking_boost_disengage_speed condition: USE_MR_BRAKING_MODE min: 0 max: 1000 - name: nav_mc_braking_bank_angle description: "max angle that MR is allowed to bank in BOOST mode" - default_value: "40" + default_value: 40 field: mc.braking_bank_angle condition: USE_MR_BRAKING_MODE min: 15 max: 60 - name: nav_mc_pos_deceleration_time description: "Used for stoping distance calculation. Stop position is computed as _speed_ * _nav_mc_pos_deceleration_time_ from the place where sticks are released. Braking mode overrides this setting" - default_value: "120" + default_value: 120 field: mc.posDecelerationTime condition: USE_NAV min: 0 max: 255 - name: nav_mc_pos_expo description: "Expo for PosHold control" - default_value: "10" + default_value: 10 field: mc.posResponseExpo condition: USE_NAV min: 0 max: 255 - name: nav_mc_wp_slowdown description: "When ON, NAV engine will slow down when switching to the next waypoint. This prioritizes turning over forward movement. When OFF, NAV engine will continue to the next waypoint and turn as it goes." - default_value: "ON" + default_value: ON field: mc.slowDownForTurning type: bool - name: nav_fw_cruise_thr description: "Cruise throttle in GPS assisted modes, this includes RTH. Should be set high enough to avoid stalling. This values gives INAV a base for throttle when flying straight, and it will increase or decrease throttle based on pitch of airplane and the parameters below. In addition it will increase throttle if GPS speed gets below 7m/s ( hardcoded )" - default_value: "1400" + default_value: 1400 field: fw.cruise_throttle min: 1000 max: 2000 - name: nav_fw_min_thr description: "Minimum throttle for flying wing in GPS assisted modes" - default_value: "1200" + default_value: 1200 field: fw.min_throttle min: 1000 max: 2000 - name: nav_fw_max_thr description: "Maximum throttle for flying wing in GPS assisted modes" - default_value: "1700" + default_value: 1700 field: fw.max_throttle min: 1000 max: 2000 - name: nav_fw_bank_angle description: "Max roll angle when rolling / turning in GPS assisted modes, is also restrained by global max_angle_inclination_rll" - default_value: "20" + default_value: 35 field: fw.max_bank_angle min: 5 max: 80 - name: nav_fw_climb_angle description: "Max pitch angle when climbing in GPS assisted modes, is also restrained by global max_angle_inclination_pit" - default_value: "20" + default_value: 20 field: fw.max_climb_angle min: 5 max: 80 - name: nav_fw_dive_angle description: "Max negative pitch angle when diving in GPS assisted modes, is also restrained by global max_angle_inclination_pit" - default_value: "15" + default_value: 15 field: fw.max_dive_angle min: 5 max: 80 - name: nav_fw_pitch2thr description: "Amount of throttle applied related to pitch attitude in GPS assisted modes. Throttle = nav_fw_cruise_throttle - (nav_fw_pitch2thr * pitch_angle). (notice that pitch_angle is in degrees and is negative when climbing and positive when diving, and throttle value is constrained between nav_fw_min_thr and nav_fw_max_thr)" - default_value: "10" + default_value: 10 field: fw.pitch_to_throttle min: 0 max: 100 - name: nav_fw_pitch2thr_smoothing description: "How smoothly the autopilot makes pitch to throttle correction inside a deadband defined by pitch_to_throttle_thresh." - default_value: "6" + default_value: 6 field: fw.pitch_to_throttle_smooth min: 0 max: 9 - name: nav_fw_pitch2thr_threshold description: "Threshold from average pitch where momentary pitch_to_throttle correction kicks in. [decidegrees]" - default_value: "50" + default_value: 50 field: fw.pitch_to_throttle_thresh min: 0 max: 900 - name: nav_fw_loiter_radius description: "PosHold radius. 3000 to 7500 is a good value (30-75m) [cm]" - default_value: "7500" + default_value: 7500 field: fw.loiter_radius min: 0 max: 10000 - name: nav_fw_cruise_speed description: "Speed for the plane/wing at cruise throttle used for remaining flight time/distance estimation in cm/s" - default_value: "0" + default_value: 0 field: fw.cruise_speed min: 0 max: 65535 - name: nav_fw_control_smoothness description: "How smoothly the autopilot controls the airplane to correct the navigation error" - default_value: "0" + default_value: 0 field: fw.control_smoothness min: 0 max: 9 - name: nav_fw_land_dive_angle description: "Dive angle that airplane will use during final landing phase. During dive phase, motor is stopped or IDLE and roll control is locked to 0 degrees" - default_value: "2" + default_value: 2 field: fw.land_dive_angle condition: NAV_FIXED_WING_LANDING min: -20 @@ -2506,100 +2540,100 @@ groups: - name: nav_fw_launch_velocity description: "Forward velocity threshold for swing-launch detection [cm/s]" - default_value: "300" + default_value: 300 field: fw.launch_velocity_thresh min: 100 max: 10000 - name: nav_fw_launch_accel description: "Forward acceleration threshold for bungee launch of throw launch [cm/s/s], 1G = 981 cm/s/s" - default_value: "1863" + default_value: 1863 field: fw.launch_accel_thresh min: 1000 max: 20000 - name: nav_fw_launch_max_angle description: "Max tilt angle (pitch/roll combined) to consider launch successful. Set to 180 to disable completely [deg]" - default_value: "45" + default_value: 45 field: fw.launch_max_angle min: 5 max: 180 - name: nav_fw_launch_detect_time description: "Time for which thresholds have to breached to consider launch happened [ms]" - default_value: "40" + default_value: 40 field: fw.launch_time_thresh min: 10 max: 1000 - name: nav_fw_launch_thr description: "Launch throttle - throttle to be set during launch sequence (pwm units)" - default_value: "1700" + default_value: 1700 field: fw.launch_throttle min: 1000 max: 2000 - name: nav_fw_launch_idle_thr description: "Launch idle throttle - throttle to be set before launch sequence is initiated. If set below minimum throttle it will force motor stop or at idle throttle (depending if the MOTOR_STOP is enabled). If set above minimum throttle it will force throttle to this value (if MOTOR_STOP is enabled it will be handled according to throttle stick position)" - default_value: "1000" + default_value: 1000 field: fw.launch_idle_throttle min: 1000 max: 2000 - name: nav_fw_launch_motor_delay description: "Delay between detected launch and launch sequence start and throttling up (ms)" - default_value: "500" + default_value: 500 field: fw.launch_motor_timer min: 0 max: 5000 - name: nav_fw_launch_spinup_time description: "Time to bring power from minimum throttle to nav_fw_launch_thr - to avoid big stress on ESC and large torque from propeller" - default_value: "100" + default_value: 100 field: fw.launch_motor_spinup_time min: 0 max: 1000 - name: nav_fw_launch_end_time description: "Time for the transition of throttle and pitch angle, between the launch state and the subsequent flight mode [ms]" - default_value: "2000" + default_value: 3000 field: fw.launch_end_time min: 0 max: 5000 - name: nav_fw_launch_min_time description: "Allow launch mode to execute at least this time (ms) and ignore stick movements [0-60000]." - default_value: "0" + default_value: 0 field: fw.launch_min_time min: 0 max: 60000 - name: nav_fw_launch_timeout description: "Maximum time for launch sequence to be executed. After this time LAUNCH mode will be turned off and regular flight mode will take over (ms)" - default_value: "5000" + default_value: 5000 field: fw.launch_timeout max: 60000 - name: nav_fw_launch_max_altitude description: "Altitude (centimeters) at which LAUNCH mode will be turned off and regular flight mode will take over [0-60000]." - default_value: "0" + default_value: 0 field: fw.launch_max_altitude min: 0 max: 60000 - name: nav_fw_launch_climb_angle description: "Climb angle for launch sequence (degrees), is also restrained by global max_angle_inclination_pit" - default_value: "18" + default_value: 18 field: fw.launch_climb_angle min: 5 max: 45 - name: nav_fw_cruise_yaw_rate description: "Max YAW rate when NAV CRUISE mode is enabled (0=disable control via yaw stick) [dps]" - default_value: "20" + default_value: 20 field: fw.cruise_yaw_rate min: 0 max: 60 - name: nav_fw_allow_manual_thr_increase description: "Enable the possibility to manually increase the throttle in auto throttle controlled modes for fixed wing" - default_value: "OFF" + default_value: OFF field: fw.allow_manual_thr_increase type: bool - name: nav_use_fw_yaw_control description: "Enables or Disables the use of the heading PID controller on fixed wing. Heading PID controller is always enabled for rovers and boats" - default_value: "OFF" + default_value: OFF field: fw.useFwNavYawControl type: bool - name: nav_fw_yaw_deadband description: "Deadband for heading trajectory PID controller. When heading error is below the deadband, controller assumes that vehicle is on course" - default_value: "0" + default_value: 0 field: fw.yawControlDeadband min: 0 max: 90 @@ -2611,27 +2645,27 @@ groups: members: - name: telemetry_switch description: "Which aux channel to use to change serial output & baud rate (MSP / Telemetry). It disables automatic switching to Telemetry when armed." - default_value: "OFF" + default_value: OFF type: bool - name: telemetry_inverted description: "Determines if the telemetry protocol default signal inversion is reversed. This should be OFF in most cases unless a custom or hacked RX is used." - default_value: "OFF" + default_value: OFF type: bool - name: frsky_default_latitude description: "D-Series telemetry only: OpenTX needs a valid set of coordinates to show compass value. A fake value defined in this setting is sent while no fix is acquired." - default_value: "0.000" + default_value: 0 field: gpsNoFixLatitude min: -90 max: 90 - name: frsky_default_longitude description: "D-Series telemetry only: OpenTX needs a valid set of coordinates to show compass value. A fake value defined in this setting is sent while no fix is acquired." - default_value: "0.000" + default_value: 0 field: gpsNoFixLongitude min: -180 max: 180 - name: frsky_coordinates_format description: "D-Series telemetry only: FRSKY_FORMAT_DMS (default), FRSKY_FORMAT_NMEA" - default_value: "0" + default_value: 0 field: frsky_coordinate_format min: 0 max: FRSKY_FORMAT_NMEA @@ -2643,26 +2677,26 @@ groups: type: uint8_t - name: frsky_vfas_precision description: "D-Series telemetry only: Set to 1 to send raw VBat value in 0.1V resolution for receivers that can handle it, or 0 (default) to use the standard method" - default_value: "0" + default_value: 0 min: FRSKY_VFAS_PRECISION_LOW max: FRSKY_VFAS_PRECISION_HIGH - name: frsky_pitch_roll description: "S.Port and D-Series telemetry: Send pitch and roll degrees*10 instead of raw accelerometer data" - default_value: "OFF" + default_value: OFF type: bool - name: report_cell_voltage description: "S.Port, D-Series, and IBUS telemetry: Send the average cell voltage if set to ON" - default_value: "OFF" + default_value: OFF type: bool - name: hott_alarm_sound_interval description: "Battery alarm delay in seconds for Hott telemetry" - default_value: "5" + default_value: 5 field: hottAlarmSoundInterval min: 0 max: 120 - name: telemetry_halfduplex description: "S.Port telemetry only: Turn UART into UNIDIR for usage on F1 and F4 target. See Telemetry.md for details" - default_value: "OFF" + default_value: ON field: halfDuplex type: bool - name: smartport_fuel_unit @@ -2674,7 +2708,7 @@ groups: table: smartport_fuel_unit - name: ibus_telemetry_type description: "Type compatibility ibus telemetry for transmitters. See Telemetry.md label IBUS for details." - default_value: "0" + default_value: 0 field: ibusTelemetryType min: 0 max: 255 @@ -2686,36 +2720,31 @@ groups: table: ltm_rates - name: sim_ground_station_number description: "Number of phone that is used to communicate with SIM module. Messages / calls from other numbers are ignored. If undefined, can be set by calling or sending a message to the module." - default_value: "Empty string" + default_value: "" field: simGroundStationNumber condition: USE_TELEMETRY_SIM - type: string - max: 16 - name: sim_pin description: "PIN code for the SIM module" - default_value: "Empty string" + default_value: "0000" field: simPin condition: USE_TELEMETRY_SIM - type: string - max: 8 - name: sim_transmit_interval description: "Text message transmission interval in seconds for SIM module. Minimum value: 10" - default_value: "60" + default_value: 60 field: simTransmitInterval condition: USE_TELEMETRY_SIM type: uint16_t min: SIM_MIN_TRANSMIT_INTERVAL max: 65535 - name: sim_transmit_flags - description: "String specifying text message transmit condition flags for the SIM module. Flags can be given in any order. Empty string means the module only sends response messages. `A`: acceleration events, `T`: continuous transmission, `F`: continuous transmission in failsafe mode, `L`: continuous transmission when altitude is below `sim_low_altitude`, `G`: continuous transmission when GPS signal quality is low" - default_value: "F" + description: "Bitmask specifying text message transmit condition flags for the SIM module. 1: continuous transmission, 2: continuous transmission in failsafe mode, 4: continuous transmission when GPS signal quality is low, 8: acceleration events, 16: continuous transmission when altitude is below `sim_low_altitude`" + default_value: :SIM_TX_FLAG_FAILSAFE field: simTransmitFlags condition: USE_TELEMETRY_SIM - type: string - max: SIM_N_TX_FLAGS + max: 63 - name: acc_event_threshold_high description: "Acceleration threshold [cm/s/s] for impact / high g event text messages sent by SIM module. Acceleration values greater than 4 g can occur in fixed wing flight without an impact, so a setting of 4000 or greater is suggested. 0 = detection off." - default_value: "0" + default_value: 0 field: accEventThresholdHigh condition: USE_TELEMETRY_SIM type: uint16_t @@ -2723,7 +2752,7 @@ groups: max: 65535 - name: acc_event_threshold_low description: "Acceleration threshold [cm/s/s] for low-g / freefall detection text messages sent by SIM module. A setting of less than 100 is suggested. Valid values: [0-900], 0 = detection off." - default_value: "0" + default_value: 0 field: accEventThresholdLow condition: USE_TELEMETRY_SIM type: uint16_t @@ -2731,7 +2760,7 @@ groups: max: 900 - name: acc_event_threshold_neg_x description: "Acceleration threshold [cm/s/s] for backwards acceleration / fixed wing landing detection text messages sent by SIM module. Suggested value for fixed wing: 1100. 0 = detection off." - default_value: "0" + default_value: 0 field: accEventThresholdNegX condition: USE_TELEMETRY_SIM type: uint16_t @@ -2739,7 +2768,7 @@ groups: max: 65535 - name: sim_low_altitude description: "Threshold for low altitude warning messages sent by SIM module when the 'L' transmit flag is set in `sim_transmit_flags`." - default_value: "0" + default_value: -32767 field: simLowAltitude condition: USE_TELEMETRY_SIM type: int16_t @@ -2750,31 +2779,37 @@ groups: type: uint8_t min: 0 max: 255 + default_value: 2 - name: mavlink_rc_chan_rate field: mavlink.rc_channels_rate type: uint8_t min: 0 max: 255 + default_value: 5 - name: mavlink_pos_rate field: mavlink.position_rate type: uint8_t min: 0 max: 255 + default_value: 2 - name: mavlink_extra1_rate field: mavlink.extra1_rate type: uint8_t min: 0 max: 255 + default_value: 10 - name: mavlink_extra2_rate field: mavlink.extra2_rate type: uint8_t min: 0 max: 255 + default_value: 2 - name: mavlink_extra3_rate field: mavlink.extra3_rate type: uint8_t min: 0 max: 255 + default_value: 1 - name: PG_ELERES_CONFIG type: eleresConfig_t headers: ["rx/eleres.h"] @@ -2784,27 +2819,34 @@ groups: field: eleresFreq min: 415 max: 450 + default_value: 435 - name: eleres_telemetry_en field: eleresTelemetryEn type: bool + default_value: OFF - name: eleres_telemetry_power field: eleresTelemetryPower min: 0 max: 7 + default_value: 7 - name: eleres_loc_en field: eleresLocEn type: bool + default_value: OFF - name: eleres_loc_power field: eleresLocPower min: 0 max: 7 + default_value: 7 - name: eleres_loc_delay field: eleresLocDelay min: 30 max: 1800 + default_value: 240 - name: eleres_signature field: eleresSignature max: UINT32_MAX + default_value: :zero - name: PG_LED_STRIP_CONFIG type: ledStripConfig_t @@ -2813,7 +2855,7 @@ groups: members: - name: ledstrip_visual_beeper description: "" - default_value: "OFF" + default_value: OFF type: bool - name: PG_OSD_CONFIG @@ -2829,7 +2871,7 @@ groups: type: uint8_t - name: osd_row_shiftdown description: "Number of rows to shift the OSD display (increase if top rows are cut off)" - default_value: "0" + default_value: 0 field: row_shiftdown min: 0 max: 1 @@ -2848,92 +2890,92 @@ groups: - name: osd_rssi_alarm description: "Value below which to make the OSD RSSI indicator blink" - default_value: "20" + default_value: 20 field: rssi_alarm min: 0 max: 100 - name: osd_time_alarm description: "Value above which to make the OSD flight time indicator blink (minutes)" - default_value: "10" + default_value: 10 field: time_alarm min: 0 max: 600 - name: osd_alt_alarm description: "Value above which to make the OSD relative altitude indicator blink (meters)" - default_value: "100" + default_value: 100 field: alt_alarm min: 0 max: 10000 - name: osd_dist_alarm description: "Value above which to make the OSD distance from home indicator blink (meters)" - default_value: "1000" + default_value: 1000 field: dist_alarm min: 0 max: 50000 - name: osd_neg_alt_alarm description: "Value below which (negative altitude) to make the OSD relative altitude indicator blink (meters)" - default_value: "5" + default_value: 5 field: neg_alt_alarm min: 0 max: 10000 - name: osd_current_alarm description: "Value above which the OSD current consumption element will start blinking. Measured in full Amperes." - default_value: "0" + default_value: 0 field: current_alarm min: 0 max: 255 - name: osd_gforce_alarm description: "Value above which the OSD g force indicator will blink (g)" - default_value: "5" + default_value: 5 field: gforce_alarm min: 0 max: 20 - name: osd_gforce_axis_alarm_min description: "Value under which the OSD axis g force indicators will blink (g)" - default_value: "-5" + default_value: -5 field: gforce_axis_alarm_min min: -20 max: 20 - name: osd_gforce_axis_alarm_max description: "Value above which the OSD axis g force indicators will blink (g)" - default_value: "5" + default_value: 5 field: gforce_axis_alarm_max min: -20 max: 20 - name: osd_imu_temp_alarm_min description: "Temperature under which the IMU temperature OSD element will start blinking (decidegrees centigrade)" - default_value: "-200" + default_value: -200 field: imu_temp_alarm_min min: -550 max: 1250 - name: osd_imu_temp_alarm_max description: "Temperature above which the IMU temperature OSD element will start blinking (decidegrees centigrade)" - default_value: "600" + default_value: 600 field: imu_temp_alarm_max min: -550 max: 1250 - name: osd_esc_temp_alarm_max description: "Temperature above which the IMU temperature OSD element will start blinking (decidegrees centigrade)" - default_value: "900" + default_value: 900 field: esc_temp_alarm_max min: -550 max: 1500 - name: osd_esc_temp_alarm_min description: "Temperature under which the IMU temperature OSD element will start blinking (decidegrees centigrade)" - default_value: "-200" + default_value: -200 field: esc_temp_alarm_min min: -550 max: 1500 - name: osd_baro_temp_alarm_min description: "Temperature under which the baro temperature OSD element will start blinking (decidegrees centigrade)" - default_value: "-200" + default_value: -200 field: baro_temp_alarm_min condition: USE_BARO min: -550 max: 1250 - name: osd_baro_temp_alarm_max description: "Temperature above which the baro temperature OSD element will start blinking (decidegrees centigrade)" - default_value: "600" + default_value: 600 field: baro_temp_alarm_max condition: USE_BARO min: -550 @@ -2941,14 +2983,14 @@ groups: - name: osd_snr_alarm condition: USE_SERIALRX_CRSF description: "Value below which Crossfire SNR Alarm pops-up. (dB)" - default_value: "4" + default_value: 4 field: snr_alarm min: -20 max: 10 - name: osd_link_quality_alarm condition: USE_SERIALRX_CRSF description: "LQ % indicator blinks below this value. For Crossfire use 70%, for Tracer use 50%" - default_value: "70" + default_value: 70 field: link_quality_alarm min: 0 max: 100 @@ -2963,12 +3005,14 @@ groups: - name: osd_ahi_reverse_roll field: ahi_reverse_roll type: bool + default_value: OFF - name: osd_ahi_max_pitch description: "Max pitch, in degrees, for OSD artificial horizon" - default_value: "20" + default_value: 20 field: ahi_max_pitch min: 10 max: 90 + default_value: 20 - name: osd_crosshairs_style description: "To set the visual type for the crosshair" default_value: "DEFAULT" @@ -2983,77 +3027,77 @@ groups: type: uint8_t - name: osd_horizon_offset description: "To vertically adjust the whole OSD and AHI and scrolling bars" - default_value: "0" + default_value: 0 field: horizon_offset min: -2 max: 2 - name: osd_camera_uptilt description: "Set the camera uptilt for the FPV camera in degres, positive is up, negative is down, relative to the horizontal" - default_value: "0" + default_value: 0 field: camera_uptilt min: -40 max: 80 - name: osd_camera_fov_h description: "Horizontal field of view for the camera in degres" - default_value: "135" + default_value: 135 field: camera_fov_h min: 60 max: 150 - name: osd_camera_fov_v description: "Vertical field of view for the camera in degres" - default_value: "85" + default_value: 85 field: camera_fov_v min: 30 max: 120 - name: osd_hud_margin_h description: "Left and right margins for the hud area" - default_value: "3" + default_value: 3 field: hud_margin_h min: 0 max: 4 - name: osd_hud_margin_v description: "Top and bottom margins for the hud area" - default_value: "3" + default_value: 3 field: hud_margin_v min: 1 max: 3 - name: osd_hud_homing description: "To display little arrows around the crossair showing where the home point is in the hud" - default_value: "0" + default_value: OFF field: hud_homing type: bool - name: osd_hud_homepoint description: "To 3D-display the home point location in the hud" - default_value: "0" + default_value: OFF field: hud_homepoint type: bool - name: osd_hud_radar_disp description: "Maximum count of nearby aircrafts or points of interest to display in the hud, as sent from an ESP32 LoRa module. Set to 0 to disable (show nothing). The nearby aircrafts will appear as markers A, B, C, etc" - default_value: "0" + default_value: 0 field: hud_radar_disp min: 0 max: 4 - name: osd_hud_radar_range_min description: "In meters, radar aircrafts closer than this will not be displayed in the hud" - default_value: "3" + default_value: 3 field: hud_radar_range_min min: 1 max: 30 - name: osd_hud_radar_range_max description: "In meters, radar aircrafts further away than this will not be displayed in the hud" - default_value: 4000" + default_value: 4000 field: hud_radar_range_max min: 100 max: 9990 - name: osd_hud_radar_nearest description: "To display an extra bar of informations at the bottom of the hud area for the closest radar aircraft found, if closest than the set value, in meters. Shows relative altitude (meters or feet, with an up or down arrow to indicate if above or below), speed (in m/s or f/s), and absolute heading (in °, 0 is north, 90 is east, 180 is south, 270 is west). Set to 0 (zero) to disable." - default_value: "0" + default_value: 0 field: hud_radar_nearest min: 0 max: 4000 - name: osd_hud_wp_disp description: "How many navigation waypoints are displayed, set to 0 (zero) to disable. As sample, if set to 2, and you just passed the 3rd waypoint of the mission, you'll see markers for the 4th waypoint (marked 1) and the 5th waypoint (marked 2)" - default_value: "0" + default_value: 0 field: hud_wp_disp min: 0 @@ -3062,16 +3106,19 @@ groups: field: left_sidebar_scroll table: osd_sidebar_scroll type: uint8_t + default_value: NONE - name: osd_right_sidebar_scroll field: right_sidebar_scroll table: osd_sidebar_scroll type: uint8_t + default_value: NONE - name: osd_sidebar_scroll_arrows field: sidebar_scroll_arrows type: bool + default_value: OFF - name: osd_main_voltage_decimals description: "Number of decimals for the battery voltages displayed in the OSD [1-2]." - default_value: "1" + default_value: 1 field: main_voltage_decimals min: 1 max: 2 @@ -3079,23 +3126,24 @@ groups: field: coordinate_digits min: 8 max: 11 + default_value: 9 - name: osd_estimations_wind_compensation description: "Use wind estimation for remaining flight time/distance estimation" - default_value: "ON" + default_value: ON condition: USE_WIND_ESTIMATOR field: estimations_wind_compensation type: bool - name: osd_failsafe_switch_layout description: "If enabled the OSD automatically switches to the first layout during failsafe" - default_value: "OFF" + default_value: OFF type: bool - name: osd_plus_code_digits description: "Numer of plus code digits before shortening with `osd_plus_code_short`. Precision at the equator: 10=13.9x13.9m; 11=2.8x3.5m; 12=56x87cm; 13=11x22cm." field: plus_code_digits - default_value: 10 + default_value: 11 min: 10 max: 13 - name: osd_plus_code_short @@ -3103,24 +3151,25 @@ groups: field: plus_code_short default_value: "0" table: osd_plus_code_short - type: uint8_t - name: osd_ahi_style description: "Sets OSD Artificial Horizon style \"DEFAULT\" or \"LINE\" for the FrSky Graphical OSD." + field: ahi_style default_value: "DEFAULT" table: osd_ahi_style + type: uint8_t - name: osd_force_grid field: force_grid type: bool - default_value: "OFF" + default_value: OFF description: Force OSD to work in grid mode even if the OSD device supports pixel level access (mainly used for development) - name: osd_ahi_bordered field: ahi_bordered type: bool description: Shows a border/corners around the AHI region (pixel OSD only) - default_value: "OFF" + default_value: OFF - name: osd_ahi_width field: ahi_width @@ -3139,7 +3188,7 @@ groups: min: -128 max: 127 description: AHI vertical offset from center (pixel OSD only) - default_value: 0 + default_value: -18 - name: osd_sidebar_horizontal_offset field: sidebar_horizontal_offset @@ -3162,7 +3211,7 @@ groups: - name: osd_home_position_arm_screen type: bool - default_value: "ON" + default_value: ON description: Should home position coordinates be displayed on the arming screen. - name: osd_pan_servo_index @@ -3170,6 +3219,7 @@ groups: field: pan_servo_index min: 0 max: 10 + default_value: 0 - name: osd_pan_servo_pwm2centideg description: Centidegrees of pan servo rotation us PWM signal. A servo with 180 degrees of rotation from 1000 to 2000 us PWM typically needs `18` for this setting. Change sign to inverse direction. @@ -3190,7 +3240,7 @@ groups: table: i2c_speed - name: cpu_underclock description: "This option is only available on certain architectures (F3 CPUs at the moment). It makes CPU clock lower to reduce interference to long-range RC systems working at 433MHz" - default_value: "OFF" + default_value: OFF field: cpuUnderclock condition: USE_UNDERCLOCK type: bool @@ -3200,13 +3250,13 @@ groups: table: debug_modes - name: throttle_tilt_comp_str description: "Can be used in ANGLE and HORIZON mode and will automatically boost throttle when banking. Setting is in percentage, 0=disabled." - default_value: "0" + default_value: 0 field: throttle_tilt_compensation_strength min: 0 max: 100 - name: name description: "Craft name" - default_value: "Empty string" + default_value: "" - name: PG_MODE_ACTIVATION_OPERATOR_CONFIG type: modeActivationOperatorConfig_t @@ -3226,20 +3276,21 @@ groups: members: - name: stats description: "General switch of the statistics recording feature (a.k.a. odometer)" - default_value: "OFF" + default_value: OFF field: stats_enabled type: bool - name: stats_total_time description: "Total flight time [in seconds]. The value is updated on every disarm when \"stats\" are enabled." - default_value: "0" + default_value: 0 max: INT32_MAX - name: stats_total_dist description: "Total flight distance [in meters]. The value is updated on every disarm when \"stats\" are enabled." - default_value: "0" + default_value: 0 max: INT32_MAX - name: stats_total_energy max: INT32_MAX condition: USE_ADC + default_value: 0 - name: PG_TIME_CONFIG type: timeConfig_t @@ -3247,7 +3298,7 @@ groups: members: - name: tz_offset description: "Time zone offset from UTC, in minutes. This is applied to the GPS time for logging and time-stamping of Blackbox logs" - default_value: "0" + default_value: 0 min: -1440 max: 1440 - name: tz_automatic_dst @@ -3262,7 +3313,7 @@ groups: members: - name: display_force_sw_blink description: "OFF = OSD hardware blink / ON = OSD software blink. If OSD warning text/values are invisible, try setting this to ON" - default_value: "OFF" + default_value: OFF field: force_sw_blink type: bool @@ -3273,12 +3324,12 @@ groups: members: - name: vtx_halfduplex description: "Use half duplex UART to communicate with the VTX, using only a TX pin in the FC." - default_value: "ON" + default_value: ON field: halfDuplex type: bool - name: vtx_smartaudio_early_akk_workaround description: "Enable workaround for early AKK SAudio-enabled VTX bug." - default_value: "ON" + default_value: ON field: smartAudioEarlyAkkWorkaroundEnable type: bool @@ -3289,19 +3340,19 @@ groups: members: - name: vtx_band description: "Configure the VTX band. Set to zero to use `vtx_freq`. Bands: 1: A, 2: B, 3: E, 4: F, 5: Race." - default_value: "4" + default_value: 4 field: band min: VTX_SETTINGS_NO_BAND max: VTX_SETTINGS_MAX_BAND - name: vtx_channel description: "Channel to use within the configured `vtx_band`. Valid values are [1, 8]." - default_value: "1" + default_value: 1 field: channel min: VTX_SETTINGS_MIN_CHANNEL max: VTX_SETTINGS_MAX_CHANNEL - name: vtx_power description: "VTX RF power level to use. The exact number of mw depends on the VTX hardware." - default_value: "1" + default_value: 1 field: power min: VTX_SETTINGS_MIN_POWER max: VTX_SETTINGS_MAX_POWER @@ -3315,9 +3366,10 @@ groups: field: pitModeChan min: VTX_SETTINGS_MIN_CHANNEL max: VTX_SETTINGS_MAX_CHANNEL + default_value: 1 - name: vtx_max_power_override description: "Some VTXes may report max power incorrectly (i.e. 200mW for a 600mW VTX). Use this to override max supported power. 0 to disable and use whatever VTX reports as its capabilities" - default_value: "0" + default_value: 0 field: maxPowerOverride min: 0 max: 10000 @@ -3333,6 +3385,7 @@ groups: default_value: "target specific" min: 0 max: 255 + default_value: :BOX_PERMANENT_ID_NONE type: uint8_t - name: pinio_box2 field: permanentId[1] @@ -3340,6 +3393,7 @@ groups: description: "Mode assignment for PINIO#1" min: 0 max: 255 + default_value: :BOX_PERMANENT_ID_NONE type: uint8_t - name: pinio_box3 field: permanentId[2] @@ -3347,6 +3401,7 @@ groups: description: "Mode assignment for PINIO#1" min: 0 max: 255 + default_value: :BOX_PERMANENT_ID_NONE type: uint8_t - name: pinio_box4 field: permanentId[3] @@ -3354,6 +3409,7 @@ groups: description: "Mode assignment for PINIO#1" min: 0 max: 255 + default_value: :BOX_PERMANENT_ID_NONE type: uint8_t - name: PG_LOG_CONFIG @@ -3365,13 +3421,13 @@ groups: field: level table: log_level description: "Defines serial debugging log level. See `docs/development/serial_printf_debugging.md` for usage." - default_value: "`ERROR`" + default_value: "ERROR" - name: log_topics field: topics min: 0 max: UINT32_MAX description: "Defines serial debugging log topic. See `docs/development/serial_printf_debugging.md` for usage." - default_value: "0" + default_value: 0 - name: PG_ESC_SENSOR_CONFIG type: escSensorConfig_t @@ -3379,10 +3435,11 @@ groups: condition: USE_ESC_SENSOR members: - name: esc_sensor_listen_only - default_value: "OFF" + default_value: OFF description: "Enable when BLHeli32 Auto Telemetry function is used. Disable in every other case" field: listenOnly type: bool + default_value: OFF - name: PG_SMARTPORT_MASTER_CONFIG type: smartportMasterConfig_t @@ -3392,9 +3449,11 @@ groups: - name: smartport_master_halfduplex field: halfDuplex type: bool + default_value: ON - name: smartport_master_inverted field: inverted type: bool + default_value: OFF - name: PG_DJI_OSD_CONFIG type: djiOsdConfig_t @@ -3406,9 +3465,10 @@ groups: field: proto_workarounds min: 0 max: UINT8_MAX + default_value: 1 - name: dji_use_name_for_messages description: "Re-purpose the craft name field for messages. Replace craft name with :WTSED for Warnings|Throttle|Speed|Efficiency|Trip distance" - default_value: "ON" + default_value: ON field: use_name_for_messages type: bool - name: dji_esc_temp_source diff --git a/src/main/fc/stats.c b/src/main/fc/stats.c index 43534a62a0..f34e2d949f 100644 --- a/src/main/fc/stats.c +++ b/src/main/fc/stats.c @@ -3,6 +3,7 @@ #ifdef USE_STATS +#include "fc/settings.h" #include "fc/stats.h" #include "sensors/battery.h" @@ -20,11 +21,11 @@ PG_REGISTER_WITH_RESET_TEMPLATE(statsConfig_t, statsConfig, PG_STATS_CONFIG, 1); PG_RESET_TEMPLATE(statsConfig_t, statsConfig, - .stats_enabled = 0, - .stats_total_time = 0, - .stats_total_dist = 0, + .stats_enabled = SETTING_STATS_DEFAULT, + .stats_total_time = SETTING_STATS_TOTAL_TIME_DEFAULT, + .stats_total_dist = SETTING_STATS_TOTAL_DIST_DEFAULT, #ifdef USE_ADC - .stats_total_energy = 0 + .stats_total_energy = SETTING_STATS_TOTAL_ENERGY_DEFAULT #endif ); diff --git a/src/main/flight/failsafe.c b/src/main/flight/failsafe.c index 6ac2783c18..7a0c3612d7 100644 --- a/src/main/flight/failsafe.c +++ b/src/main/flight/failsafe.c @@ -40,6 +40,7 @@ #include "fc/rc_modes.h" #include "fc/runtime_config.h" #include "fc/controlrate_profile.h" +#include "fc/settings.h" #include "flight/failsafe.h" #include "flight/mixer.h" @@ -67,19 +68,19 @@ static failsafeState_t failsafeState; PG_REGISTER_WITH_RESET_TEMPLATE(failsafeConfig_t, failsafeConfig, PG_FAILSAFE_CONFIG, 1); PG_RESET_TEMPLATE(failsafeConfig_t, failsafeConfig, - .failsafe_delay = 5, // 0.5 sec - .failsafe_recovery_delay = 5, // 0.5 seconds (plus 200ms explicit delay) - .failsafe_off_delay = 200, // 20sec - .failsafe_throttle = 1000, // default throttle off. - .failsafe_throttle_low_delay = 0, // default throttle low delay for "just disarm" on failsafe condition - .failsafe_procedure = FAILSAFE_PROCEDURE_AUTO_LANDING, // default full failsafe procedure - .failsafe_fw_roll_angle = -200, // 20 deg left - .failsafe_fw_pitch_angle = 100, // 10 deg dive (yes, positive means dive) - .failsafe_fw_yaw_rate = -45, // 45 deg/s left yaw (left is negative, 8s for full turn) - .failsafe_stick_motion_threshold = 50, - .failsafe_min_distance = 0, // No minimum distance for failsafe by default - .failsafe_min_distance_procedure = FAILSAFE_PROCEDURE_DROP_IT, // default minimum distance failsafe procedure - .failsafe_mission = true, // Enable failsafe in WP mode or not + .failsafe_delay = SETTING_FAILSAFE_DELAY_DEFAULT, // 0.5 sec + .failsafe_recovery_delay = SETTING_FAILSAFE_RECOVERY_DELAY_DEFAULT, // 0.5 seconds (plus 200ms explicit delay) + .failsafe_off_delay = SETTING_FAILSAFE_OFF_DELAY_DEFAULT, // 20sec + .failsafe_throttle = SETTING_FAILSAFE_THROTTLE_DEFAULT, // default throttle off. + .failsafe_throttle_low_delay = SETTING_FAILSAFE_THROTTLE_LOW_DELAY_DEFAULT, // default throttle low delay for "just disarm" on failsafe condition + .failsafe_procedure = SETTING_FAILSAFE_PROCEDURE_DEFAULT, // default full failsafe procedure + .failsafe_fw_roll_angle = SETTING_FAILSAFE_FW_ROLL_ANGLE_DEFAULT, // 20 deg left + .failsafe_fw_pitch_angle = SETTING_FAILSAFE_FW_PITCH_ANGLE_DEFAULT, // 10 deg dive (yes, positive means dive) + .failsafe_fw_yaw_rate = SETTING_FAILSAFE_FW_YAW_RATE_DEFAULT, // 45 deg/s left yaw (left is negative, 8s for full turn) + .failsafe_stick_motion_threshold = SETTING_FAILSAFE_STICK_THRESHOLD_DEFAULT, + .failsafe_min_distance = SETTING_FAILSAFE_MIN_DISTANCE_DEFAULT, // No minimum distance for failsafe by default + .failsafe_min_distance_procedure = SETTING_FAILSAFE_MIN_DISTANCE_PROCEDURE_DEFAULT, // default minimum distance failsafe procedure + .failsafe_mission = SETTING_FAILSAFE_MISSION_DEFAULT, // Enable failsafe in WP mode or not ); typedef enum { diff --git a/src/main/flight/imu.c b/src/main/flight/imu.c index d75f416f8f..3401e3b00c 100644 --- a/src/main/flight/imu.c +++ b/src/main/flight/imu.c @@ -45,6 +45,7 @@ FILE_COMPILE_FOR_SPEED #include "fc/config.h" #include "fc/runtime_config.h" +#include "fc/settings.h" #include "flight/hil.h" #include "flight/imu.h" @@ -99,13 +100,13 @@ STATIC_FASTRAM bool gpsHeadingInitialized; PG_REGISTER_WITH_RESET_TEMPLATE(imuConfig_t, imuConfig, PG_IMU_CONFIG, 2); PG_RESET_TEMPLATE(imuConfig_t, imuConfig, - .dcm_kp_acc = 2500, // 0.25 * 10000 - .dcm_ki_acc = 50, // 0.005 * 10000 - .dcm_kp_mag = 10000, // 1.00 * 10000 - .dcm_ki_mag = 0, // 0.00 * 10000 - .small_angle = 25, - .acc_ignore_rate = 0, - .acc_ignore_slope = 0 + .dcm_kp_acc = SETTING_IMU_DCM_KP_DEFAULT, // 0.25 * 10000 + .dcm_ki_acc = SETTING_IMU_DCM_KI_DEFAULT, // 0.005 * 10000 + .dcm_kp_mag = SETTING_IMU_DCM_KP_MAG_DEFAULT, // 1.00 * 10000 + .dcm_ki_mag = SETTING_IMU_DCM_KI_MAG_DEFAULT, // 0.00 * 10000 + .small_angle = SETTING_SMALL_ANGLE_DEFAULT, + .acc_ignore_rate = SETTING_IMU_ACC_IGNORE_RATE_DEFAULT, + .acc_ignore_slope = SETTING_IMU_ACC_IGNORE_SLOPE_DEFAULT ); STATIC_UNIT_TESTED void imuComputeRotationMatrix(void) @@ -156,8 +157,13 @@ void imuInit(void) gpsHeadingInitialized = false; // Create magnetic declination matrix +#ifdef USE_MAG const int deg = compassConfig()->mag_declination / 100; const int min = compassConfig()->mag_declination % 100; +#else + const int deg = 0; + const int min = 0; +#endif imuSetMagneticDeclination(deg + min / 60.0f); quaternionInitUnit(&orientation); diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c index f5d7ba2279..af1ca40a20 100755 --- a/src/main/flight/mixer.c +++ b/src/main/flight/mixer.c @@ -43,6 +43,7 @@ FILE_COMPILE_FOR_SPEED #include "fc/rc_modes.h" #include "fc/runtime_config.h" #include "fc/controlrate_profile.h" +#include "fc/settings.h" #include "flight/failsafe.h" #include "flight/imu.h" @@ -75,19 +76,19 @@ static EXTENDED_FASTRAM int8_t motorYawMultiplier = 1; PG_REGISTER_WITH_RESET_TEMPLATE(reversibleMotorsConfig_t, reversibleMotorsConfig, PG_REVERSIBLE_MOTORS_CONFIG, 0); PG_RESET_TEMPLATE(reversibleMotorsConfig_t, reversibleMotorsConfig, - .deadband_low = 1406, - .deadband_high = 1514, - .neutral = 1460 + .deadband_low = SETTING_3D_DEADBAND_LOW_DEFAULT, + .deadband_high = SETTING_3D_DEADBAND_HIGH_DEFAULT, + .neutral = SETTING_3D_NEUTRAL_DEFAULT ); PG_REGISTER_WITH_RESET_TEMPLATE(mixerConfig_t, mixerConfig, PG_MIXER_CONFIG, 3); PG_RESET_TEMPLATE(mixerConfig_t, mixerConfig, - .motorDirectionInverted = 0, - .platformType = PLATFORM_MULTIROTOR, - .hasFlaps = false, - .appliedMixerPreset = -1, //This flag is not available in CLI and used by Configurator only - .fwMinThrottleDownPitchAngle = 0 + .motorDirectionInverted = SETTING_MOTOR_DIRECTION_INVERTED_DEFAULT, + .platformType = SETTING_PLATFORM_TYPE_DEFAULT, + .hasFlaps = SETTING_HAS_FLAPS_DEFAULT, + .appliedMixerPreset = SETTING_MODEL_PREVIEW_TYPE_DEFAULT, //This flag is not available in CLI and used by Configurator only + .fwMinThrottleDownPitchAngle = SETTING_FW_MIN_THROTTLE_DOWN_PITCH_DEFAULT ); #ifdef BRUSHED_MOTORS @@ -103,16 +104,18 @@ PG_RESET_TEMPLATE(mixerConfig_t, mixerConfig, PG_REGISTER_WITH_RESET_TEMPLATE(motorConfig_t, motorConfig, PG_MOTOR_CONFIG, 7); PG_RESET_TEMPLATE(motorConfig_t, motorConfig, - .motorPwmProtocol = DEFAULT_PWM_PROTOCOL, - .motorPwmRate = DEFAULT_PWM_RATE, - .maxthrottle = DEFAULT_MAX_THROTTLE, - .mincommand = 1000, - .motorAccelTimeMs = 0, - .motorDecelTimeMs = 0, - .throttleIdle = 15.0f, - .throttleScale = 1.0f, - .motorPoleCount = 14, // Most brushless motors that we use are 14 poles - .flipOverAfterPowerFactor = 65 + .motorPwmProtocol = SETTING_MOTOR_PWM_PROTOCOL_DEFAULT, + .motorPwmRate = SETTING_MOTOR_PWM_RATE_DEFAULT, + .maxthrottle = SETTING_MAX_THROTTLE_DEFAULT, + .mincommand = SETTING_MIN_COMMAND_DEFAULT, + .motorAccelTimeMs = SETTING_MOTOR_ACCEL_TIME_DEFAULT, + .motorDecelTimeMs = SETTING_MOTOR_DECEL_TIME_DEFAULT, + .throttleIdle = SETTING_THROTTLE_IDLE_DEFAULT, + .throttleScale = SETTING_THROTTLE_SCALE_DEFAULT, + .motorPoleCount = SETTING_MOTOR_POLES_DEFAULT, // Most brushless motors that we use are 14 poles +#ifdef USE_DSHOT + .flipOverAfterPowerFactor = SETTING_FLIP_OVER_AFTER_CRASH_POWER_FACTOR_DEFAULT, +#endif ); PG_REGISTER_ARRAY(motorMixer_t, MAX_SUPPORTED_MOTORS, primaryMotorMixer, PG_MOTOR_MIXER, 0); diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 2bb7c4a204..b8c4b94e24 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -40,6 +40,7 @@ FILE_COMPILE_FOR_SPEED #include "fc/rc_controls.h" #include "fc/rc_modes.h" #include "fc/runtime_config.h" +#include "fc/settings.h" #include "flight/pid.h" #include "flight/imu.h" @@ -163,38 +164,38 @@ PG_REGISTER_PROFILE_WITH_RESET_TEMPLATE(pidProfile_t, pidProfile, PG_PID_PROFILE PG_RESET_TEMPLATE(pidProfile_t, pidProfile, .bank_mc = { .pid = { - [PID_ROLL] = { 40, 30, 23, 60 }, - [PID_PITCH] = { 40, 30, 23, 60 }, - [PID_YAW] = { 85, 45, 0, 60 }, + [PID_ROLL] = { SETTING_MC_P_ROLL_DEFAULT, SETTING_MC_I_ROLL_DEFAULT, SETTING_MC_D_ROLL_DEFAULT, SETTING_MC_CD_ROLL_DEFAULT }, + [PID_PITCH] = { SETTING_MC_P_PITCH_DEFAULT, SETTING_MC_I_PITCH_DEFAULT, SETTING_MC_D_PITCH_DEFAULT, SETTING_MC_CD_PITCH_DEFAULT }, + [PID_YAW] = { SETTING_MC_P_YAW_DEFAULT, SETTING_MC_I_YAW_DEFAULT, SETTING_MC_D_YAW_DEFAULT, SETTING_MC_CD_YAW_DEFAULT }, [PID_LEVEL] = { - .P = 20, // Self-level strength - .I = 15, // Self-leveing low-pass frequency (0 - disabled) - .D = 75, // 75% horizon strength + .P = SETTING_MC_P_LEVEL_DEFAULT, // Self-level strength + .I = SETTING_MC_I_LEVEL_DEFAULT, // Self-leveing low-pass frequency (0 - disabled) + .D = SETTING_MC_D_LEVEL_DEFAULT, // 75% horizon strength .FF = 0, }, - [PID_HEADING] = { 60, 0, 0, 0 }, + [PID_HEADING] = { SETTING_NAV_MC_HEADING_P_DEFAULT, 0, 0, 0 }, [PID_POS_XY] = { - .P = 65, // NAV_POS_XY_P * 100 + .P = SETTING_NAV_MC_POS_XY_P_DEFAULT, // NAV_POS_XY_P * 100 .I = 0, .D = 0, .FF = 0, }, [PID_VEL_XY] = { - .P = 40, // NAV_VEL_XY_P * 20 - .I = 15, // NAV_VEL_XY_I * 100 - .D = 100, // NAV_VEL_XY_D * 100 - .FF = 40, // NAV_VEL_XY_D * 100 + .P = SETTING_NAV_MC_VEL_XY_P_DEFAULT, // NAV_VEL_XY_P * 20 + .I = SETTING_NAV_MC_VEL_XY_I_DEFAULT, // NAV_VEL_XY_I * 100 + .D = SETTING_NAV_MC_VEL_XY_D_DEFAULT, // NAV_VEL_XY_D * 100 + .FF = SETTING_NAV_MC_VEL_XY_FF_DEFAULT, // NAV_VEL_XY_D * 100 }, [PID_POS_Z] = { - .P = 50, // NAV_POS_Z_P * 100 - .I = 0, // not used - .D = 0, // not used + .P = SETTING_NAV_MC_POS_Z_P_DEFAULT, // NAV_POS_Z_P * 100 + .I = 0, // not used + .D = 0, // not used .FF = 0, }, [PID_VEL_Z] = { - .P = 100, // NAV_VEL_Z_P * 66.7 - .I = 50, // NAV_VEL_Z_I * 20 - .D = 10, // NAV_VEL_Z_D * 100 + .P = SETTING_NAV_MC_VEL_Z_P_DEFAULT, // NAV_VEL_Z_P * 66.7 + .I = SETTING_NAV_MC_VEL_Z_I_DEFAULT, // NAV_VEL_Z_I * 20 + .D = SETTING_NAV_MC_VEL_Z_D_DEFAULT, // NAV_VEL_Z_D * 100 .FF = 0, }, [PID_POS_HEADING] = { @@ -208,83 +209,94 @@ PG_RESET_TEMPLATE(pidProfile_t, pidProfile, .bank_fw = { .pid = { - [PID_ROLL] = { 5, 7, 0, 50 }, - [PID_PITCH] = { 5, 7, 0, 50 }, - [PID_YAW] = { 6, 10, 0, 60 }, + [PID_ROLL] = { SETTING_FW_P_ROLL_DEFAULT, SETTING_FW_I_ROLL_DEFAULT, 0, SETTING_FW_FF_ROLL_DEFAULT }, + [PID_PITCH] = { SETTING_FW_P_PITCH_DEFAULT, SETTING_FW_I_PITCH_DEFAULT, 0, SETTING_FW_FF_PITCH_DEFAULT }, + [PID_YAW] = { SETTING_FW_P_YAW_DEFAULT, SETTING_FW_I_YAW_DEFAULT, 0, SETTING_FW_FF_YAW_DEFAULT }, [PID_LEVEL] = { - .P = 20, // Self-level strength - .I = 5, // Self-leveing low-pass frequency (0 - disabled) - .D = 75, // 75% horizon strength + .P = SETTING_FW_P_LEVEL_DEFAULT, // Self-level strength + .I = SETTING_FW_I_LEVEL_DEFAULT, // Self-leveing low-pass frequency (0 - disabled) + .D = SETTING_FW_D_LEVEL_DEFAULT, // 75% horizon strength .FF = 0, }, - [PID_HEADING] = { 60, 0, 0, 0 }, + [PID_HEADING] = { SETTING_NAV_FW_HEADING_P_DEFAULT, 0, 0, 0 }, [PID_POS_Z] = { - .P = 40, // FW_POS_Z_P * 10 - .I = 5, // FW_POS_Z_I * 10 - .D = 10, // FW_POS_Z_D * 10 + .P = SETTING_NAV_FW_POS_Z_P_DEFAULT, // FW_POS_Z_P * 10 + .I = SETTING_NAV_FW_POS_Z_I_DEFAULT, // FW_POS_Z_I * 10 + .D = SETTING_NAV_FW_POS_Z_D_DEFAULT, // FW_POS_Z_D * 10 .FF = 0, }, [PID_POS_XY] = { - .P = 75, // FW_POS_XY_P * 100 - .I = 5, // FW_POS_XY_I * 100 - .D = 8, // FW_POS_XY_D * 100 + .P = SETTING_NAV_FW_POS_XY_P_DEFAULT, // FW_POS_XY_P * 100 + .I = SETTING_NAV_FW_POS_XY_I_DEFAULT, // FW_POS_XY_I * 100 + .D = SETTING_NAV_FW_POS_XY_D_DEFAULT, // FW_POS_XY_D * 100 .FF = 0, }, [PID_POS_HEADING] = { - .P = 30, - .I = 2, - .D = 0, + .P = SETTING_NAV_FW_POS_HDG_P_DEFAULT, + .I = SETTING_NAV_FW_POS_HDG_I_DEFAULT, + .D = SETTING_NAV_FW_POS_HDG_D_DEFAULT, .FF = 0 } } }, - .dterm_lpf_type = 1, //Default to BIQUAD - .dterm_lpf_hz = 40, - .dterm_lpf2_type = 1, //Default to BIQUAD - .dterm_lpf2_hz = 0, // Off by default - .yaw_lpf_hz = 0, + .dterm_lpf_type = SETTING_DTERM_LPF_TYPE_DEFAULT, + .dterm_lpf_hz = SETTING_DTERM_LPF_HZ_DEFAULT, + .dterm_lpf2_type = SETTING_DTERM_LPF2_TYPE_DEFAULT, + .dterm_lpf2_hz = SETTING_DTERM_LPF2_HZ_DEFAULT, + .yaw_lpf_hz = SETTING_YAW_LPF_HZ_DEFAULT, - .itermWindupPointPercent = 50, // Percent + .itermWindupPointPercent = SETTING_ITERM_WINDUP_DEFAULT, - .axisAccelerationLimitYaw = 10000, // dps/s - .axisAccelerationLimitRollPitch = 0, // dps/s + .axisAccelerationLimitYaw = SETTING_RATE_ACCEL_LIMIT_YAW_DEFAULT, + .axisAccelerationLimitRollPitch = SETTING_RATE_ACCEL_LIMIT_ROLL_PITCH_DEFAULT, - .heading_hold_rate_limit = HEADING_HOLD_RATE_LIMIT_DEFAULT, + .heading_hold_rate_limit = SETTING_HEADING_HOLD_RATE_LIMIT_DEFAULT, - .max_angle_inclination[FD_ROLL] = 300, // 30 degrees - .max_angle_inclination[FD_PITCH] = 300, // 30 degrees - .pidSumLimit = PID_SUM_LIMIT_DEFAULT, - .pidSumLimitYaw = PID_SUM_LIMIT_YAW_DEFAULT, + .max_angle_inclination[FD_ROLL] = SETTING_MAX_ANGLE_INCLINATION_RLL_DEFAULT, + .max_angle_inclination[FD_PITCH] = SETTING_MAX_ANGLE_INCLINATION_PIT_DEFAULT, + .pidSumLimit = SETTING_PIDSUM_LIMIT_DEFAULT, + .pidSumLimitYaw = SETTING_PIDSUM_LIMIT_YAW_DEFAULT, - .fixedWingItermThrowLimit = FW_ITERM_THROW_LIMIT_DEFAULT, - .fixedWingReferenceAirspeed = 1000, - .fixedWingCoordinatedYawGain = 1.0f, - .fixedWingCoordinatedPitchGain = 1.0f, - .fixedWingItermLimitOnStickPosition = 0.5f, - .fixedWingYawItermBankFreeze = 0, + .fixedWingItermThrowLimit = SETTING_FW_ITERM_THROW_LIMIT_DEFAULT, + .fixedWingReferenceAirspeed = SETTING_FW_REFERENCE_AIRSPEED_DEFAULT, + .fixedWingCoordinatedYawGain = SETTING_FW_TURN_ASSIST_YAW_GAIN_DEFAULT, + .fixedWingCoordinatedPitchGain = SETTING_FW_TURN_ASSIST_PITCH_GAIN_DEFAULT, + .fixedWingItermLimitOnStickPosition = SETTING_FW_ITERM_LIMIT_STICK_POSITION_DEFAULT, + .fixedWingYawItermBankFreeze = SETTING_FW_YAW_ITERM_FREEZE_BANK_ANGLE_DEFAULT, - .loiter_direction = NAV_LOITER_RIGHT, - .navVelXyDTermLpfHz = NAV_ACCEL_CUTOFF_FREQUENCY_HZ, - .navVelXyDtermAttenuation = 90, - .navVelXyDtermAttenuationStart = 10, - .navVelXyDtermAttenuationEnd = 60, - .iterm_relax_cutoff = MC_ITERM_RELAX_CUTOFF_DEFAULT, - .iterm_relax = ITERM_RELAX_RP, - .dBoostFactor = 1.25f, - .dBoostMaxAtAlleceleration = 7500.0f, - .dBoostGyroDeltaLpfHz = D_BOOST_GYRO_LPF_HZ, - .antigravityGain = 1.0f, - .antigravityAccelerator = 1.0f, - .antigravityCutoff = ANTI_GRAVITY_THROTTLE_FILTER_CUTOFF, - .pidControllerType = PID_TYPE_AUTO, - .navFwPosHdgPidsumLimit = PID_SUM_LIMIT_YAW_DEFAULT, - .controlDerivativeLpfHz = 30, - .kalman_q = 100, - .kalman_w = 4, - .kalman_sharpness = 100, - .kalmanEnabled = 0, - .fixedWingLevelTrim = 0, + .loiter_direction = SETTING_FW_LOITER_DIRECTION_DEFAULT, + .navVelXyDTermLpfHz = SETTING_NAV_MC_VEL_XY_DTERM_LPF_HZ_DEFAULT, + .navVelXyDtermAttenuation = SETTING_NAV_MC_VEL_XY_DTERM_ATTENUATION_DEFAULT, + .navVelXyDtermAttenuationStart = SETTING_NAV_MC_VEL_XY_DTERM_ATTENUATION_START_DEFAULT, + .navVelXyDtermAttenuationEnd = SETTING_NAV_MC_VEL_XY_DTERM_ATTENUATION_END_DEFAULT, + .iterm_relax_cutoff = SETTING_MC_ITERM_RELAX_CUTOFF_DEFAULT, + .iterm_relax = SETTING_MC_ITERM_RELAX_DEFAULT, + +#ifdef USE_D_BOOST + .dBoostFactor = SETTING_D_BOOST_FACTOR_DEFAULT, + .dBoostMaxAtAlleceleration = SETTING_D_BOOST_MAX_AT_ACCELERATION_DEFAULT, + .dBoostGyroDeltaLpfHz = SETTING_D_BOOST_GYRO_DELTA_LPF_HZ_DEFAULT, +#endif + +#ifdef USE_ANTIGRAVITY + .antigravityGain = SETTING_ANTIGRAVITY_GAIN_DEFAULT, + .antigravityAccelerator = SETTING_ANTIGRAVITY_ACCELERATOR_DEFAULT, + .antigravityCutoff = SETTING_ANTIGRAVITY_CUTOFF_LPF_HZ_DEFAULT, +#endif + + .pidControllerType = SETTING_PID_TYPE_DEFAULT, + .navFwPosHdgPidsumLimit = SETTING_NAV_FW_POS_HDG_PIDSUM_LIMIT_DEFAULT, + .controlDerivativeLpfHz = SETTING_MC_CD_LPF_HZ_DEFAULT, + +#ifdef USE_GYRO_KALMAN + .kalman_q = SETTING_SETPOINT_KALMAN_Q_DEFAULT, + .kalman_w = SETTING_SETPOINT_KALMAN_W_DEFAULT, + .kalman_sharpness = SETTING_SETPOINT_KALMAN_SHARPNESS_DEFAULT, + .kalmanEnabled = SETTING_SETPOINT_KALMAN_ENABLED_DEFAULT, +#endif + + .fixedWingLevelTrim = SETTING_FW_LEVEL_PITCH_TRIM_DEFAULT, ); bool pidInitFilters(void) diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h index 900e54575e..3f3d5a3bcc 100644 --- a/src/main/flight/pid.h +++ b/src/main/flight/pid.h @@ -138,19 +138,27 @@ typedef struct pidProfile_s { uint8_t iterm_relax_cutoff; // This cutoff frequency specifies a low pass filter which predicts average response of the quad to setpoint uint8_t iterm_relax; // Enable iterm suppression during stick input +#ifdef USE_D_BOOST float dBoostFactor; float dBoostMaxAtAlleceleration; uint8_t dBoostGyroDeltaLpfHz; +#endif + +#ifdef USE_ANTIGRAVITY float antigravityGain; float antigravityAccelerator; uint8_t antigravityCutoff; +#endif uint16_t navFwPosHdgPidsumLimit; uint8_t controlDerivativeLpfHz; + +#ifdef USE_GYRO_KALMAN uint16_t kalman_q; uint16_t kalman_w; uint16_t kalman_sharpness; uint8_t kalmanEnabled; +#endif float fixedWingLevelTrim; } pidProfile_t; diff --git a/src/main/flight/pid_autotune.c b/src/main/flight/pid_autotune.c index 245571c2f4..684f42d1b7 100755 --- a/src/main/flight/pid_autotune.c +++ b/src/main/flight/pid_autotune.c @@ -40,11 +40,10 @@ #include "fc/rc_controls.h" #include "fc/rc_adjustments.h" #include "fc/runtime_config.h" +#include "fc/settings.h" #include "flight/pid.h" -#define AUTOTUNE_FIXED_WING_OVERSHOOT_TIME 100 -#define AUTOTUNE_FIXED_WING_UNDERSHOOT_TIME 200 #define AUTOTUNE_FIXED_WING_INTEGRATOR_TC 600 #define AUTOTUNE_FIXED_WING_DECREASE_STEP 8 // 8% #define AUTOTUNE_FIXED_WING_INCREASE_STEP 5 // 5% @@ -54,11 +53,11 @@ PG_REGISTER_WITH_RESET_TEMPLATE(pidAutotuneConfig_t, pidAutotuneConfig, PG_PID_AUTOTUNE_CONFIG, 0); PG_RESET_TEMPLATE(pidAutotuneConfig_t, pidAutotuneConfig, - .fw_overshoot_time = AUTOTUNE_FIXED_WING_OVERSHOOT_TIME, - .fw_undershoot_time = AUTOTUNE_FIXED_WING_UNDERSHOOT_TIME, - .fw_max_rate_threshold = 50, - .fw_ff_to_p_gain = 10, - .fw_ff_to_i_time_constant = AUTOTUNE_FIXED_WING_INTEGRATOR_TC, + .fw_overshoot_time = SETTING_FW_AUTOTUNE_OVERSHOOT_TIME_DEFAULT, + .fw_undershoot_time = SETTING_FW_AUTOTUNE_UNDERSHOOT_TIME_DEFAULT, + .fw_max_rate_threshold = SETTING_FW_AUTOTUNE_THRESHOLD_DEFAULT, + .fw_ff_to_p_gain = SETTING_FW_AUTOTUNE_FF_TO_P_GAIN_DEFAULT, + .fw_ff_to_i_time_constant = SETTING_FW_AUTOTUNE_FF_TO_I_TC_DEFAULT, ); typedef enum { diff --git a/src/main/flight/rpm_filter.c b/src/main/flight/rpm_filter.c index 6a9604651e..183220461e 100644 --- a/src/main/flight/rpm_filter.c +++ b/src/main/flight/rpm_filter.c @@ -38,6 +38,7 @@ #include "flight/mixer.h" #include "sensors/esc_sensor.h" #include "fc/config.h" +#include "fc/settings.h" #ifdef USE_RPM_FILTER @@ -48,10 +49,10 @@ PG_REGISTER_WITH_RESET_TEMPLATE(rpmFilterConfig_t, rpmFilterConfig, PG_RPM_FILTER_CONFIG, 1); PG_RESET_TEMPLATE(rpmFilterConfig_t, rpmFilterConfig, - .gyro_filter_enabled = 0, - .gyro_harmonics = 1, - .gyro_min_hz = 100, - .gyro_q = 500, ); + .gyro_filter_enabled = SETTING_RPM_GYRO_FILTER_ENABLED_DEFAULT, + .gyro_harmonics = SETTING_RPM_GYRO_HARMONICS_DEFAULT, + .gyro_min_hz = SETTING_RPM_GYRO_MIN_HZ_DEFAULT, + .gyro_q = SETTING_RPM_GYRO_Q_DEFAULT, ); typedef struct { @@ -203,4 +204,4 @@ float rpmFilterGyroApply(uint8_t axis, float input) return rpmGyroApplyFn(&gyroRpmFilters, axis, input); } -#endif \ No newline at end of file +#endif diff --git a/src/main/flight/secondary_imu.c b/src/main/flight/secondary_imu.c index da3a4dfde7..93cd11b9d1 100644 --- a/src/main/flight/secondary_imu.c +++ b/src/main/flight/secondary_imu.c @@ -21,19 +21,26 @@ * You should have received a copy of the GNU General Public License * along with this program. If not, see http://www.gnu.org/licenses/. */ + #include "stdint.h" -#include "common/utils.h" -#include "common/axis.h" -#include "flight/secondary_imu.h" -#include "config/parameter_group_ids.h" -#include "sensors/boardalignment.h" -#include "sensors/compass.h" #include "build/debug.h" +#include "common/utils.h" +#include "common/axis.h" + +#include "config/parameter_group_ids.h" + #include "drivers/sensor.h" #include "drivers/accgyro/accgyro_bno055.h" +#include "fc/settings.h" + +#include "flight/secondary_imu.h" + +#include "sensors/boardalignment.h" +#include "sensors/compass.h" + PG_REGISTER_WITH_RESET_FN(secondaryImuConfig_t, secondaryImuConfig, PG_SECONDARY_IMU, 1); EXTENDED_FASTRAM secondaryImuState_t secondaryImuState; @@ -62,8 +69,13 @@ void secondaryImuInit(void) { secondaryImuState.active = false; // Create magnetic declination matrix +#ifdef USE_MAG const int deg = compassConfig()->mag_declination / 100; const int min = compassConfig()->mag_declination % 100; +#else + const int deg = 0; + const int min = 0; +#endif secondaryImuSetMagneticDeclination(deg + min / 60.0f); @@ -171,4 +183,4 @@ void secondaryImuFetchCalibration(void) { void secondaryImuSetMagneticDeclination(float declination) { //Incoming units are degrees secondaryImuState.magDeclination = declination * 10.0f; //Internally declination is stored in decidegrees -} \ No newline at end of file +} diff --git a/src/main/flight/servos.c b/src/main/flight/servos.c index 918a20ce99..df58e6f649 100755 --- a/src/main/flight/servos.c +++ b/src/main/flight/servos.c @@ -44,6 +44,7 @@ #include "fc/rc_modes.h" #include "fc/runtime_config.h" #include "fc/controlrate_profile.h" +#include "fc/settings.h" #include "flight/imu.h" #include "flight/mixer.h" @@ -57,12 +58,12 @@ PG_REGISTER_WITH_RESET_TEMPLATE(servoConfig_t, servoConfig, PG_SERVO_CONFIG, 1); PG_RESET_TEMPLATE(servoConfig_t, servoConfig, - .servoCenterPulse = 1500, - .servoPwmRate = 50, // Default for analog servos - .servo_lowpass_freq = 20, // Default servo update rate is 50Hz, everything above Nyquist frequency (25Hz) is going to fold and cause distortions - .servo_protocol = SERVO_TYPE_PWM, - .flaperon_throw_offset = FLAPERON_THROW_DEFAULT, - .tri_unarmed_servo = 1 + .servoCenterPulse = SETTING_SERVO_CENTER_PULSE_DEFAULT, + .servoPwmRate = SETTING_SERVO_PWM_RATE_DEFAULT, // Default for analog servos + .servo_lowpass_freq = SETTING_SERVO_LPF_HZ_DEFAULT, // Default servo update rate is 50Hz, everything above Nyquist frequency (25Hz) is going to fold and cause distortions + .servo_protocol = SETTING_SERVO_PROTOCOL_DEFAULT, + .flaperon_throw_offset = SETTING_FLAPERON_THROW_OFFSET_DEFAULT, + .tri_unarmed_servo = SETTING_TRI_UNARMED_SERVO_DEFAULT ); PG_REGISTER_ARRAY_WITH_RESET_FN(servoMixer_t, MAX_SERVO_RULES, customServoMixers, PG_SERVO_MIXER, 1); diff --git a/src/main/io/gps.c b/src/main/io/gps.c index 2dbfd77766..3d693deba8 100755 --- a/src/main/io/gps.c +++ b/src/main/io/gps.c @@ -58,6 +58,7 @@ #include "fc/config.h" #include "fc/runtime_config.h" +#include "fc/settings.h" typedef struct { bool isDriverBased; @@ -125,13 +126,13 @@ static gpsProviderDescriptor_t gpsProviders[GPS_PROVIDER_COUNT] = { PG_REGISTER_WITH_RESET_TEMPLATE(gpsConfig_t, gpsConfig, PG_GPS_CONFIG, 0); PG_RESET_TEMPLATE(gpsConfig_t, gpsConfig, - .provider = GPS_UBLOX, - .sbasMode = SBAS_NONE, - .autoConfig = GPS_AUTOCONFIG_ON, - .autoBaud = GPS_AUTOBAUD_ON, - .dynModel = GPS_DYNMODEL_AIR_1G, - .gpsMinSats = 6, - .ubloxUseGalileo = false + .provider = SETTING_GPS_PROVIDER_DEFAULT, + .sbasMode = SETTING_GPS_SBAS_MODE_DEFAULT, + .autoConfig = SETTING_GPS_AUTO_CONFIG_DEFAULT, + .autoBaud = SETTING_GPS_AUTO_BAUD_DEFAULT, + .dynModel = SETTING_GPS_DYN_MODEL_DEFAULT, + .gpsMinSats = SETTING_GPS_MIN_SATS_DEFAULT, + .ubloxUseGalileo = SETTING_GPS_UBLOX_USE_GALILEO_DEFAULT ); void gpsSetState(gpsState_e state) diff --git a/src/main/io/lights.c b/src/main/io/lights.c index fa6bd2fe4a..2329aa8bf0 100644 --- a/src/main/io/lights.c +++ b/src/main/io/lights.c @@ -31,9 +31,9 @@ PG_REGISTER_WITH_RESET_TEMPLATE(lightsConfig_t, lightsConfig, PG_LIGHTS_CONFIG, PG_RESET_TEMPLATE(lightsConfig_t, lightsConfig, .failsafe = { - .enabled = true, - .flash_period = 1000, - .flash_on_time = 100 + .enabled = SETTING_FAILSAFE_LIGHTS_DEFAULT, + .flash_period = SETTING_FAILSAFE_LIGHTS_FLASH_PERIOD_DEFAULT, + .flash_on_time = SETTING_FAILSAFE_LIGHTS_FLASH_ON_TIME_DEFAULT } ); diff --git a/src/main/io/osd.c b/src/main/io/osd.c index 6861f8a675..b6943791f7 100644 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -1615,7 +1615,11 @@ static bool osdDrawSingleElement(uint8_t item) timeUs_t currentTimeUs = micros(); static int32_t timeSeconds = -1; if (cmpTimeUs(currentTimeUs, updatedTimestamp) >= 1000000) { +#ifdef USE_WIND_ESTIMATOR timeSeconds = calculateRemainingFlightTimeBeforeRTH(osdConfig()->estimations_wind_compensation); +#else + timeSeconds = calculateRemainingFlightTimeBeforeRTH(false); +#endif updatedTimestamp = currentTimeUs; } if ((!ARMING_FLAG(ARMED)) || (timeSeconds == -1)) { @@ -1642,7 +1646,11 @@ static bool osdDrawSingleElement(uint8_t item) timeUs_t currentTimeUs = micros(); static int32_t distanceMeters = -1; if (cmpTimeUs(currentTimeUs, updatedTimestamp) >= 1000000) { +#ifdef USE_WIND_ESTIMATOR distanceMeters = calculateRemainingDistanceBeforeRTH(osdConfig()->estimations_wind_compensation); +#else + distanceMeters = calculateRemainingDistanceBeforeRTH(false); +#endif updatedTimestamp = currentTimeUs; } buff[0] = SYM_TRIP_DIST; @@ -2659,71 +2667,84 @@ void osdDrawNextElement(void) } PG_RESET_TEMPLATE(osdConfig_t, osdConfig, - .rssi_alarm = 20, - .time_alarm = 10, - .alt_alarm = 100, - .dist_alarm = 1000, - .neg_alt_alarm = 5, - .current_alarm = 0, - .imu_temp_alarm_min = -200, - .imu_temp_alarm_max = 600, - .esc_temp_alarm_min = -200, - .esc_temp_alarm_max = 900, - .gforce_alarm = 5, - .gforce_axis_alarm_min = -5, - .gforce_axis_alarm_max = 5, + .rssi_alarm = SETTING_OSD_RSSI_ALARM_DEFAULT, + .time_alarm = SETTING_OSD_TIME_ALARM_DEFAULT, + .alt_alarm = SETTING_OSD_ALT_ALARM_DEFAULT, + .dist_alarm = SETTING_OSD_DIST_ALARM_DEFAULT, + .neg_alt_alarm = SETTING_OSD_NEG_ALT_ALARM_DEFAULT, + .current_alarm = SETTING_OSD_CURRENT_ALARM_DEFAULT, + .imu_temp_alarm_min = SETTING_OSD_IMU_TEMP_ALARM_MIN_DEFAULT, + .imu_temp_alarm_max = SETTING_OSD_IMU_TEMP_ALARM_MAX_DEFAULT, + .esc_temp_alarm_min = SETTING_OSD_ESC_TEMP_ALARM_MIN_DEFAULT, + .esc_temp_alarm_max = SETTING_OSD_ESC_TEMP_ALARM_MAX_DEFAULT, + .gforce_alarm = SETTING_OSD_GFORCE_ALARM_DEFAULT, + .gforce_axis_alarm_min = SETTING_OSD_GFORCE_AXIS_ALARM_MIN_DEFAULT, + .gforce_axis_alarm_max = SETTING_OSD_GFORCE_AXIS_ALARM_MAX_DEFAULT, #ifdef USE_BARO - .baro_temp_alarm_min = -200, - .baro_temp_alarm_max = 600, + .baro_temp_alarm_min = SETTING_OSD_BARO_TEMP_ALARM_MIN_DEFAULT, + .baro_temp_alarm_max = SETTING_OSD_BARO_TEMP_ALARM_MAX_DEFAULT, #endif #ifdef USE_SERIALRX_CRSF - .snr_alarm = 4, - .crsf_lq_format = OSD_CRSF_LQ_TYPE1, - .link_quality_alarm = 70, + .snr_alarm = SETTING_OSD_SNR_ALARM_DEFAULT, + .crsf_lq_format = SETTING_OSD_CRSF_LQ_FORMAT_DEFAULT, + .link_quality_alarm = SETTING_OSD_LINK_QUALITY_ALARM_DEFAULT, #endif #ifdef USE_TEMPERATURE_SENSOR - .temp_label_align = OSD_ALIGN_LEFT, + .temp_label_align = SETTING_OSD_TEMP_LABEL_ALIGN_DEFAULT, #endif - .video_system = VIDEO_SYSTEM_AUTO, + .video_system = SETTING_OSD_VIDEO_SYSTEM_DEFAULT, + .row_shiftdown = SETTING_OSD_ROW_SHIFTDOWN_DEFAULT, - .ahi_reverse_roll = 0, - .ahi_max_pitch = AH_MAX_PITCH_DEFAULT, - .crosshairs_style = OSD_CROSSHAIRS_STYLE_DEFAULT, - .horizon_offset = 0, - .camera_uptilt = 0, - .camera_fov_h = 135, - .camera_fov_v = 85, - .hud_margin_h = 3, - .hud_margin_v = 3, - .hud_homing = 0, - .hud_homepoint = 0, - .hud_radar_disp = 0, - .hud_radar_range_min = 3, - .hud_radar_range_max = 4000, - .hud_radar_nearest = 0, - .hud_wp_disp = 0, - .left_sidebar_scroll = OSD_SIDEBAR_SCROLL_NONE, - .right_sidebar_scroll = OSD_SIDEBAR_SCROLL_NONE, - .sidebar_scroll_arrows = 0, - .osd_home_position_arm_screen = true, - .pan_servo_index = 0, - .pan_servo_pwm2centideg = 0, + .ahi_reverse_roll = SETTING_OSD_AHI_REVERSE_ROLL_DEFAULT, + .ahi_max_pitch = SETTING_OSD_AHI_MAX_PITCH_DEFAULT, + .crosshairs_style = SETTING_OSD_CROSSHAIRS_STYLE_DEFAULT, + .horizon_offset = SETTING_OSD_HORIZON_OFFSET_DEFAULT, + .camera_uptilt = SETTING_OSD_CAMERA_UPTILT_DEFAULT, + .camera_fov_h = SETTING_OSD_CAMERA_FOV_H_DEFAULT, + .camera_fov_v = SETTING_OSD_CAMERA_FOV_V_DEFAULT, + .hud_margin_h = SETTING_OSD_HUD_MARGIN_H_DEFAULT, + .hud_margin_v = SETTING_OSD_HUD_MARGIN_V_DEFAULT, + .hud_homing = SETTING_OSD_HUD_HOMING_DEFAULT, + .hud_homepoint = SETTING_OSD_HUD_HOMEPOINT_DEFAULT, + .hud_radar_disp = SETTING_OSD_HUD_RADAR_DISP_DEFAULT, + .hud_radar_range_min = SETTING_OSD_HUD_RADAR_RANGE_MIN_DEFAULT, + .hud_radar_range_max = SETTING_OSD_HUD_RADAR_RANGE_MAX_DEFAULT, + .hud_radar_nearest = SETTING_OSD_HUD_RADAR_NEAREST_DEFAULT, + .hud_wp_disp = SETTING_OSD_HUD_WP_DISP_DEFAULT, + .left_sidebar_scroll = SETTING_OSD_LEFT_SIDEBAR_SCROLL_DEFAULT, + .right_sidebar_scroll = SETTING_OSD_RIGHT_SIDEBAR_SCROLL_DEFAULT, + .sidebar_scroll_arrows = SETTING_OSD_SIDEBAR_SCROLL_ARROWS_DEFAULT, + .sidebar_horizontal_offset = SETTING_OSD_SIDEBAR_HORIZONTAL_OFFSET_DEFAULT, + .left_sidebar_scroll_step = SETTING_OSD_LEFT_SIDEBAR_SCROLL_STEP_DEFAULT, + .right_sidebar_scroll_step = SETTING_OSD_RIGHT_SIDEBAR_SCROLL_STEP_DEFAULT, + .osd_home_position_arm_screen = SETTING_OSD_HOME_POSITION_ARM_SCREEN_DEFAULT, + .pan_servo_index = SETTING_OSD_PAN_SERVO_INDEX_DEFAULT, + .pan_servo_pwm2centideg = SETTING_OSD_PAN_SERVO_PWM2CENTIDEG_DEFAULT, - .units = OSD_UNIT_METRIC, - .main_voltage_decimals = 1, + .units = SETTING_OSD_UNITS_DEFAULT, + .main_voltage_decimals = SETTING_OSD_MAIN_VOLTAGE_DECIMALS_DEFAULT, - .estimations_wind_compensation = true, - .coordinate_digits = 9, +#ifdef USE_WIND_ESTIMATOR + .estimations_wind_compensation = SETTING_OSD_ESTIMATIONS_WIND_COMPENSATION_DEFAULT, +#endif - .osd_failsafe_switch_layout = false, + .coordinate_digits = SETTING_OSD_COORDINATE_DIGITS_DEFAULT, - .plus_code_digits = 11, - .plus_code_short = 0, + .osd_failsafe_switch_layout = SETTING_OSD_FAILSAFE_SWITCH_LAYOUT_DEFAULT, - .ahi_width = OSD_AHI_WIDTH * OSD_CHAR_WIDTH, - .ahi_height = OSD_AHI_HEIGHT * OSD_CHAR_HEIGHT, - .ahi_vertical_offset = -OSD_CHAR_HEIGHT, + .plus_code_digits = SETTING_OSD_PLUS_CODE_DIGITS_DEFAULT, + .plus_code_short = SETTING_OSD_PLUS_CODE_SHORT_DEFAULT, + + .ahi_width = SETTING_OSD_AHI_WIDTH_DEFAULT, + .ahi_height = SETTING_OSD_AHI_HEIGHT_DEFAULT, + .ahi_vertical_offset = SETTING_OSD_AHI_VERTICAL_OFFSET_DEFAULT, + .ahi_bordered = SETTING_OSD_AHI_BORDERED_DEFAULT, + .ahi_style = SETTING_OSD_AHI_STYLE_DEFAULT, + + .force_grid = SETTING_OSD_FORCE_GRID_DEFAULT, + + .stats_energy_unit = SETTING_OSD_STATS_ENERGY_UNIT_DEFAULT ); void pgResetFn_osdLayoutsConfig(osdLayoutsConfig_t *osdLayoutsConfig) diff --git a/src/main/io/osd.h b/src/main/io/osd.h index fe35e28b71..1dc185b0d1 100755 --- a/src/main/io/osd.h +++ b/src/main/io/osd.h @@ -333,13 +333,16 @@ typedef struct osdConfig_s { uint8_t units; // from osd_unit_e uint8_t stats_energy_unit; // from osd_stats_energy_unit_e +#ifdef USE_WIND_ESTIMATOR bool estimations_wind_compensation; // use wind compensation for estimated remaining flight/distance +#endif + uint8_t coordinate_digits; bool osd_failsafe_switch_layout; uint8_t plus_code_digits; // Number of digits to use in OSD_PLUS_CODE uint8_t plus_code_short; - uint8_t osd_ahi_style; + uint8_t ahi_style; uint8_t force_grid; // Force a pixel based OSD to use grid mode. uint8_t ahi_bordered; // Only used by the AHI widget uint8_t ahi_width; // In pixels, only used by the AHI widget diff --git a/src/main/io/osd_canvas.c b/src/main/io/osd_canvas.c index cab358f09c..5044e93c26 100644 --- a/src/main/io/osd_canvas.c +++ b/src/main/io/osd_canvas.c @@ -354,7 +354,7 @@ static bool osdCanvasDrawArtificialHorizonWidget(displayPort_t *display, display } if (!configured) { widgetAHIStyle_e ahiStyle = 0; - switch ((osd_ahi_style_e)osdConfig()->osd_ahi_style) { + switch ((osd_ahi_style_e)osdConfig()->ahi_style) { case OSD_AHI_STYLE_DEFAULT: ahiStyle = DISPLAY_WIDGET_AHI_STYLE_STAIRCASE; break; @@ -381,7 +381,7 @@ static bool osdCanvasDrawArtificialHorizonWidget(displayPort_t *display, display // so that's 135degs each direction. Map that to the configured limit. const float halfRange = 135.0f; const float limit = halfRange / 180.0f * M_PIf; - float multiplier = osdConfig()->osd_ahi_style == OSD_AHI_STYLE_DEFAULT ? 1.0f : halfRange / osdConfig()->ahi_max_pitch; + float multiplier = osdConfig()->ahi_style == OSD_AHI_STYLE_DEFAULT ? 1.0f : halfRange / osdConfig()->ahi_max_pitch; widgetAHIData_t data = { .pitch = constrainf(pitchAngle * multiplier, -limit, limit), .roll = rollAngle, @@ -413,7 +413,7 @@ void osdCanvasDrawArtificialHorizon(displayPort_t *display, displayCanvas_t *can if ((now > nextDrawMinMs && totalError > 0.05f)|| now > nextDrawMaxMs) { if (!osdCanvasDrawArtificialHorizonWidget(display, canvas, p, pitchAngle, rollAngle)) { - switch ((osd_ahi_style_e)osdConfig()->osd_ahi_style) { + switch ((osd_ahi_style_e)osdConfig()->ahi_style) { case OSD_AHI_STYLE_DEFAULT: { int x, y, w, h; diff --git a/src/main/io/osd_dji_hd.c b/src/main/io/osd_dji_hd.c index 023fb47037..ed72a973d3 100644 --- a/src/main/io/osd_dji_hd.c +++ b/src/main/io/osd_dji_hd.c @@ -122,10 +122,10 @@ PG_REGISTER_WITH_RESET_TEMPLATE(djiOsdConfig_t, djiOsdConfig, PG_DJI_OSD_CONFIG, 2); PG_RESET_TEMPLATE(djiOsdConfig_t, djiOsdConfig, - .use_name_for_messages = true, - .esc_temperature_source = DJI_OSD_TEMP_ESC, - .proto_workarounds = DJI_OSD_USE_NON_STANDARD_MSP_ESC_SENSOR_DATA, - .speedSource = DJI_OSD_SPEED_GROUND, + .use_name_for_messages = SETTING_DJI_USE_NAME_FOR_MESSAGES_DEFAULT, + .esc_temperature_source = SETTING_DJI_ESC_TEMP_SOURCE_DEFAULT, + .proto_workarounds = SETTING_DJI_WORKAROUNDS_DEFAULT, + .speedSource = SETTING_DJI_SPEED_SOURCE_DEFAULT, ); // External dependency on looptime diff --git a/src/main/io/piniobox.c b/src/main/io/piniobox.c index b8b01ad329..4fbcc657e4 100644 --- a/src/main/io/piniobox.c +++ b/src/main/io/piniobox.c @@ -33,6 +33,7 @@ #include "fc/fc_msp.h" #include "fc/fc_msp_box.h" +#include "fc/settings.h" #include "io/piniobox.h" @@ -40,7 +41,7 @@ PG_REGISTER_WITH_RESET_TEMPLATE(pinioBoxConfig_t, pinioBoxConfig, PG_PINIOBOX_CONFIG, 1); PG_RESET_TEMPLATE(pinioBoxConfig_t, pinioBoxConfig, - { BOX_PERMANENT_ID_NONE, BOX_PERMANENT_ID_NONE, BOX_PERMANENT_ID_NONE, BOX_PERMANENT_ID_NONE } + { SETTING_PINIO_BOX1_DEFAULT, SETTING_PINIO_BOX2_DEFAULT, SETTING_PINIO_BOX3_DEFAULT, SETTING_PINIO_BOX4_DEFAULT } ); typedef struct pinioBoxRuntimeConfig_s { @@ -68,4 +69,4 @@ void pinioBoxUpdate(void) } } -#endif \ No newline at end of file +#endif diff --git a/src/main/io/piniobox.h b/src/main/io/piniobox.h index 74ba398a32..638fd04b4e 100644 --- a/src/main/io/piniobox.h +++ b/src/main/io/piniobox.h @@ -26,6 +26,12 @@ #include "common/time.h" #include "drivers/pinio.h" + +#define BOX_PERMANENT_ID_USER1 47 +#define BOX_PERMANENT_ID_USER2 48 +#define BOX_PERMANENT_ID_NONE 255 // A permanent ID for no box mode + + typedef struct pinioBoxConfig_s { uint8_t permanentId[PINIO_COUNT]; } pinioBoxConfig_t; diff --git a/src/main/io/serial.c b/src/main/io/serial.c index d8dc546c6e..7c6360c43c 100644 --- a/src/main/io/serial.c +++ b/src/main/io/serial.c @@ -49,6 +49,7 @@ #include "io/serial.h" #include "fc/cli.h" +#include "fc/settings.h" #include "msp/msp_serial.h" @@ -147,7 +148,7 @@ void pgResetFn_serialConfig(serialConfig_t *serialConfig) } #endif - serialConfig->reboot_character = 'R'; + serialConfig->reboot_character = SETTING_REBOOT_CHARACTER_DEFAULT; } baudRate_e lookupBaudRateIndex(uint32_t baudRate) diff --git a/src/main/io/smartport_master.c b/src/main/io/smartport_master.c index 95e1c74b22..ade2b00acc 100644 --- a/src/main/io/smartport_master.c +++ b/src/main/io/smartport_master.c @@ -34,6 +34,8 @@ FILE_COMPILE_FOR_SPEED #include "drivers/serial.h" #include "drivers/time.h" +#include "fc/settings.h" + #include "io/serial.h" #include "io/smartport_master.h" @@ -137,8 +139,8 @@ typedef struct { PG_REGISTER_WITH_RESET_TEMPLATE(smartportMasterConfig_t, smartportMasterConfig, PG_SMARTPORT_MASTER_CONFIG, 0); PG_RESET_TEMPLATE(smartportMasterConfig_t, smartportMasterConfig, - .halfDuplex = true, - .inverted = false + .halfDuplex = SETTING_SMARTPORT_MASTER_HALFDUPLEX_DEFAULT, + .inverted = SETTING_SMARTPORT_MASTER_INVERTED_DEFAULT ); static serialPort_t *smartportMasterSerialPort = NULL; diff --git a/src/main/io/vtx.c b/src/main/io/vtx.c index f4b999a534..eabe0d1f00 100644 --- a/src/main/io/vtx.c +++ b/src/main/io/vtx.c @@ -36,6 +36,7 @@ #include "fc/config.h" #include "fc/rc_modes.h" #include "fc/runtime_config.h" +#include "fc/settings.h" #include "flight/failsafe.h" @@ -46,12 +47,12 @@ PG_REGISTER_WITH_RESET_TEMPLATE(vtxSettingsConfig_t, vtxSettingsConfig, PG_VTX_SETTINGS_CONFIG, 2); PG_RESET_TEMPLATE(vtxSettingsConfig_t, vtxSettingsConfig, - .band = VTX_SETTINGS_DEFAULT_BAND, - .channel = VTX_SETTINGS_DEFAULT_CHANNEL, - .power = VTX_SETTINGS_DEFAULT_POWER, - .pitModeChan = VTX_SETTINGS_DEFAULT_PITMODE_CHANNEL, - .lowPowerDisarm = VTX_LOW_POWER_DISARM_OFF, - .maxPowerOverride = 0, + .band = SETTING_VTX_BAND_DEFAULT, + .channel = SETTING_VTX_CHANNEL_DEFAULT, + .power = SETTING_VTX_POWER_DEFAULT, + .pitModeChan = SETTING_VTX_PIT_MODE_CHAN_DEFAULT, + .lowPowerDisarm = SETTING_VTX_LOW_POWER_DISARM_DEFAULT, + .maxPowerOverride = SETTING_VTX_MAX_POWER_OVERRIDE_DEFAULT, ); typedef enum { diff --git a/src/main/io/vtx_control.c b/src/main/io/vtx_control.c index f80b992c29..9a3c2ef633 100644 --- a/src/main/io/vtx_control.c +++ b/src/main/io/vtx_control.c @@ -31,6 +31,7 @@ #include "fc/config.h" #include "fc/runtime_config.h" +#include "fc/settings.h" #include "io/beeper.h" #include "io/osd.h" @@ -42,8 +43,8 @@ PG_REGISTER_WITH_RESET_TEMPLATE(vtxConfig_t, vtxConfig, PG_VTX_CONFIG, 3); PG_RESET_TEMPLATE(vtxConfig_t, vtxConfig, - .halfDuplex = true, - .smartAudioEarlyAkkWorkaroundEnable = true, + .halfDuplex = SETTING_VTX_HALFDUPLEX_DEFAULT, + .smartAudioEarlyAkkWorkaroundEnable = SETTING_VTX_SMARTAUDIO_EARLY_AKK_WORKAROUND_DEFAULT, ); static uint8_t locked = 0; @@ -182,4 +183,3 @@ void vtxCyclePower(const uint8_t powerStep) } #endif - diff --git a/src/main/io/vtx_smartaudio.c b/src/main/io/vtx_smartaudio.c index cb057f0fc5..31030573d5 100644 --- a/src/main/io/vtx_smartaudio.c +++ b/src/main/io/vtx_smartaudio.c @@ -43,6 +43,8 @@ #include "drivers/time.h" #include "drivers/vtx_common.h" +#include "fc/settings.h" + #include "io/serial.h" #include "io/vtx.h" #include "io/vtx_control.h" @@ -129,8 +131,8 @@ saPowerTable_t saPowerTable[VTX_SMARTAUDIO_MAX_POWER_COUNT] = { smartAudioDevice_t saDevice = { .version = SA_UNKNOWN, - .channel = -1, - .power = -1, + .channel = SETTING_VTX_CHANNEL_DEFAULT, + .power = SETTING_VTX_POWER_DEFAULT, .mode = 0, .freq = 0, .orfreq = 0, diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c index 9cc98a7260..c953686269 100755 --- a/src/main/navigation/navigation.c +++ b/src/main/navigation/navigation.c @@ -39,6 +39,7 @@ #include "fc/rc_controls.h" #include "fc/rc_modes.h" #include "fc/runtime_config.h" +#include "fc/settings.h" #include "flight/imu.h" #include "flight/mixer.h" @@ -97,92 +98,96 @@ PG_RESET_TEMPLATE(navConfig_t, navConfig, .general = { .flags = { - .use_thr_mid_for_althold = 0, - .extra_arming_safety = NAV_EXTRA_ARMING_SAFETY_ON, - .user_control_mode = NAV_GPS_ATTI, - .rth_alt_control_mode = NAV_RTH_AT_LEAST_ALT, - .rth_climb_first = 1, // Climb first, turn after reaching safe altitude - .rth_climb_ignore_emerg = 0, // Ignore GPS loss on initial climb - .rth_tail_first = 0, - .disarm_on_landing = 0, - .rth_allow_landing = NAV_RTH_ALLOW_LANDING_ALWAYS, - .nav_overrides_motor_stop = NOMS_ALL_NAV, + .use_thr_mid_for_althold = SETTING_NAV_USE_MIDTHR_FOR_ALTHOLD_DEFAULT, + .extra_arming_safety = SETTING_NAV_EXTRA_ARMING_SAFETY_DEFAULT, + .user_control_mode = SETTING_NAV_USER_CONTROL_MODE_DEFAULT, + .rth_alt_control_mode = SETTING_NAV_RTH_ALT_MODE_DEFAULT, + .rth_climb_first = SETTING_NAV_RTH_CLIMB_FIRST_DEFAULT, // Climb first, turn after reaching safe altitude + .rth_climb_ignore_emerg = SETTING_NAV_RTH_CLIMB_IGNORE_EMERG_DEFAULT, // Ignore GPS loss on initial climb + .rth_tail_first = SETTING_NAV_RTH_TAIL_FIRST_DEFAULT, + .disarm_on_landing = SETTING_NAV_DISARM_ON_LANDING_DEFAULT, + .rth_allow_landing = SETTING_NAV_RTH_ALLOW_LANDING_DEFAULT, + .nav_overrides_motor_stop = SETTING_NAV_OVERRIDES_MOTOR_STOP_DEFAULT, }, // General navigation parameters - .pos_failure_timeout = 5, // 5 sec - .waypoint_radius = 100, // 2m diameter - .waypoint_safe_distance = 10000, // centimeters - first waypoint should be closer than this - .max_auto_speed = 300, // 3 m/s = 10.8 km/h - .max_auto_climb_rate = 500, // 5 m/s - .max_manual_speed = 500, - .max_manual_climb_rate = 200, - .land_descent_rate = 200, // centimeters/s - .land_slowdown_minalt = 500, // altitude in centimeters - .land_slowdown_maxalt = 2000, // altitude in meters - .emerg_descent_rate = 500, // centimeters/s - .min_rth_distance = 500, // centimeters, if closer than this land immediately - .rth_altitude = 1000, // altitude in centimeters - .rth_home_altitude = 0, // altitude in centimeters - .rth_abort_threshold = 50000, // centimeters - 500m should be safe for all aircraft - .max_terrain_follow_altitude = 100, // max altitude in centimeters in terrain following mode - .safehome_max_distance = 20000, // Max distance that a safehome is from the arming point + .pos_failure_timeout = SETTING_NAV_POSITION_TIMEOUT_DEFAULT, // 5 sec + .waypoint_radius = SETTING_NAV_WP_RADIUS_DEFAULT, // 2m diameter + .waypoint_safe_distance = SETTING_NAV_WP_SAFE_DISTANCE_DEFAULT, // centimeters - first waypoint should be closer than this + .max_auto_speed = SETTING_NAV_AUTO_SPEED_DEFAULT, // 3 m/s = 10.8 km/h + .max_auto_climb_rate = SETTING_NAV_AUTO_CLIMB_RATE_DEFAULT, // 5 m/s + .max_manual_speed = SETTING_NAV_MANUAL_SPEED_DEFAULT, + .max_manual_climb_rate = SETTING_NAV_MANUAL_CLIMB_RATE_DEFAULT, + .land_descent_rate = SETTING_NAV_LANDING_SPEED_DEFAULT, // centimeters/s + .land_slowdown_minalt = SETTING_NAV_LAND_SLOWDOWN_MINALT_DEFAULT, // altitude in centimeters + .land_slowdown_maxalt = SETTING_NAV_LAND_SLOWDOWN_MAXALT_DEFAULT, // altitude in meters + .emerg_descent_rate = SETTING_NAV_EMERG_LANDING_SPEED_DEFAULT, // centimeters/s + .min_rth_distance = SETTING_NAV_MIN_RTH_DISTANCE_DEFAULT, // centimeters, if closer than this land immediately + .rth_altitude = SETTING_NAV_RTH_ALTITUDE_DEFAULT, // altitude in centimeters + .rth_home_altitude = SETTING_NAV_RTH_HOME_ALTITUDE_DEFAULT, // altitude in centimeters + .rth_abort_threshold = SETTING_NAV_RTH_ABORT_THRESHOLD_DEFAULT, // centimeters - 500m should be safe for all aircraft + .max_terrain_follow_altitude = SETTING_NAV_MAX_TERRAIN_FOLLOW_ALT_DEFAULT, // max altitude in centimeters in terrain following mode + .safehome_max_distance = SETTING_SAFEHOME_MAX_DISTANCE_DEFAULT, // Max distance that a safehome is from the arming point }, // MC-specific .mc = { - .max_bank_angle = 30, // degrees - .hover_throttle = 1500, - .auto_disarm_delay = 2000, // milliseconds - time before disarming when auto disarm is enabled and landing is confirmed - .braking_speed_threshold = 100, // Braking can become active above 1m/s - .braking_disengage_speed = 75, // Stop when speed goes below 0.75m/s - .braking_timeout = 2000, // Timeout barking after 2s - .braking_boost_factor = 100, // A 100% boost by default - .braking_boost_timeout = 750, // Timout boost after 750ms - .braking_boost_speed_threshold = 150, // Boost can happen only above 1.5m/s - .braking_boost_disengage_speed = 100, // Disable boost at 1m/s - .braking_bank_angle = 40, // Max braking angle - .posDecelerationTime = 120, // posDecelerationTime * 100 - .posResponseExpo = 10, // posResponseExpo * 100 - .slowDownForTurning = true, + .max_bank_angle = SETTING_NAV_MC_BANK_ANGLE_DEFAULT, // degrees + .hover_throttle = SETTING_NAV_MC_HOVER_THR_DEFAULT, + .auto_disarm_delay = SETTING_NAV_MC_AUTO_DISARM_DELAY_DEFAULT, // milliseconds - time before disarming when auto disarm is enabled and landing is confirmed + +#ifdef USE_MR_BRAKING_MODE + .braking_speed_threshold = SETTING_NAV_MC_BRAKING_SPEED_THRESHOLD_DEFAULT, // Braking can become active above 1m/s + .braking_disengage_speed = SETTING_NAV_MC_BRAKING_DISENGAGE_SPEED_DEFAULT, // Stop when speed goes below 0.75m/s + .braking_timeout = SETTING_NAV_MC_BRAKING_TIMEOUT_DEFAULT, // Timeout barking after 2s + .braking_boost_factor = SETTING_NAV_MC_BRAKING_BOOST_FACTOR_DEFAULT, // A 100% boost by default + .braking_boost_timeout = SETTING_NAV_MC_BRAKING_BOOST_TIMEOUT_DEFAULT, // Timout boost after 750ms + .braking_boost_speed_threshold = SETTING_NAV_MC_BRAKING_BOOST_SPEED_THRESHOLD_DEFAULT, // Boost can happen only above 1.5m/s + .braking_boost_disengage_speed = SETTING_NAV_MC_BRAKING_BOOST_DISENGAGE_SPEED_DEFAULT, // Disable boost at 1m/s + .braking_bank_angle = SETTING_NAV_MC_BRAKING_BANK_ANGLE_DEFAULT, // Max braking angle +#endif + + .posDecelerationTime = SETTING_NAV_MC_POS_DECELERATION_TIME_DEFAULT, // posDecelerationTime * 100 + .posResponseExpo = SETTING_NAV_MC_POS_EXPO_DEFAULT, // posResponseExpo * 100 + .slowDownForTurning = SETTING_NAV_MC_WP_SLOWDOWN_DEFAULT, }, // Fixed wing .fw = { - .max_bank_angle = 35, // degrees - .max_climb_angle = 20, // degrees - .max_dive_angle = 15, // degrees - .cruise_throttle = 1400, - .cruise_speed = 0, // cm/s - .control_smoothness = 0, - .max_throttle = 1700, - .min_throttle = 1200, - .pitch_to_throttle = 10, // pwm units per degree of pitch (10pwm units ~ 1% throttle) - .pitch_to_throttle_smooth = 6, - .pitch_to_throttle_thresh = 50, - .loiter_radius = 7500, // 75m + .max_bank_angle = SETTING_NAV_FW_BANK_ANGLE_DEFAULT, // degrees + .max_climb_angle = SETTING_NAV_FW_CLIMB_ANGLE_DEFAULT, // degrees + .max_dive_angle = SETTING_NAV_FW_DIVE_ANGLE_DEFAULT, // degrees + .cruise_throttle = SETTING_NAV_FW_CRUISE_THR_DEFAULT, + .cruise_speed = SETTING_NAV_FW_CRUISE_SPEED_DEFAULT, // cm/s + .control_smoothness = SETTING_NAV_FW_CONTROL_SMOOTHNESS_DEFAULT, + .max_throttle = SETTING_NAV_FW_MAX_THR_DEFAULT, + .min_throttle = SETTING_NAV_FW_MIN_THR_DEFAULT, + .pitch_to_throttle = SETTING_NAV_FW_PITCH2THR_DEFAULT, // pwm units per degree of pitch (10pwm units ~ 1% throttle) + .pitch_to_throttle_smooth = SETTING_NAV_FW_PITCH2THR_SMOOTHING_DEFAULT, + .pitch_to_throttle_thresh = SETTING_NAV_FW_PITCH2THR_THRESHOLD_DEFAULT, + .loiter_radius = SETTING_NAV_FW_LOITER_RADIUS_DEFAULT, // 75m //Fixed wing landing - .land_dive_angle = 2, // 2 degrees dive by default + .land_dive_angle = SETTING_NAV_FW_LAND_DIVE_ANGLE_DEFAULT, // 2 degrees dive by default // Fixed wing launch - .launch_velocity_thresh = 300, // 3 m/s - .launch_accel_thresh = 1.9f * 981, // cm/s/s (1.9*G) - .launch_time_thresh = 40, // 40ms - .launch_throttle = 1700, - .launch_idle_throttle = 1000, // Motor idle or MOTOR_STOP - .launch_motor_timer = 500, // ms - .launch_motor_spinup_time = 100, // ms, time to gredually increase throttle from idle to launch - .launch_end_time = 3000, // ms, time to gradually decrease/increase throttle and decrease pitch angle from launch to the current flight mode - .launch_min_time = 0, // ms, min time in launch mode - .launch_timeout = 5000, // ms, timeout for launch procedure - .launch_max_altitude = 0, // cm, altitude where to consider launch ended - .launch_climb_angle = 18, // 18 degrees - .launch_max_angle = 45, // 45 deg - .cruise_yaw_rate = 20, // 20dps - .allow_manual_thr_increase = false, - .useFwNavYawControl = 0, - .yawControlDeadband = 0, + .launch_velocity_thresh = SETTING_NAV_FW_LAUNCH_VELOCITY_DEFAULT, // 3 m/s + .launch_accel_thresh = SETTING_NAV_FW_LAUNCH_ACCEL_DEFAULT, // cm/s/s (1.9*G) + .launch_time_thresh = SETTING_NAV_FW_LAUNCH_DETECT_TIME_DEFAULT, // 40ms + .launch_throttle = SETTING_NAV_FW_LAUNCH_THR_DEFAULT, + .launch_idle_throttle = SETTING_NAV_FW_LAUNCH_IDLE_THR_DEFAULT, // Motor idle or MOTOR_STOP + .launch_motor_timer = SETTING_NAV_FW_LAUNCH_MOTOR_DELAY_DEFAULT, // ms + .launch_motor_spinup_time = SETTING_NAV_FW_LAUNCH_SPINUP_TIME_DEFAULT, // ms, time to gredually increase throttle from idle to launch + .launch_end_time = SETTING_NAV_FW_LAUNCH_END_TIME_DEFAULT, // ms, time to gradually decrease/increase throttle and decrease pitch angle from launch to the current flight mode + .launch_min_time = SETTING_NAV_FW_LAUNCH_MIN_TIME_DEFAULT, // ms, min time in launch mode + .launch_timeout = SETTING_NAV_FW_LAUNCH_TIMEOUT_DEFAULT, // ms, timeout for launch procedure + .launch_max_altitude = SETTING_NAV_FW_LAUNCH_MAX_ALTITUDE_DEFAULT, // cm, altitude where to consider launch ended + .launch_climb_angle = SETTING_NAV_FW_LAUNCH_CLIMB_ANGLE_DEFAULT, // 18 degrees + .launch_max_angle = SETTING_NAV_FW_LAUNCH_MAX_ANGLE_DEFAULT, // 45 deg + .cruise_yaw_rate = SETTING_NAV_FW_CRUISE_YAW_RATE_DEFAULT, // 20dps + .allow_manual_thr_increase = SETTING_NAV_FW_ALLOW_MANUAL_THR_INCREASE_DEFAULT, + .useFwNavYawControl = SETTING_NAV_USE_FW_YAW_CONTROL_DEFAULT, + .yawControlDeadband = SETTING_NAV_FW_YAW_DEADBAND_DEFAULT, } ); @@ -3432,7 +3437,10 @@ void navigationUsePIDs(void) multicopterPosXyCoefficients.dTermAttenuation = pidProfile()->navVelXyDtermAttenuation / 100.0f; multicopterPosXyCoefficients.dTermAttenuationStart = pidProfile()->navVelXyDtermAttenuationStart / 100.0f; multicopterPosXyCoefficients.dTermAttenuationEnd = pidProfile()->navVelXyDtermAttenuationEnd / 100.0f; + +#ifdef USE_MR_BRAKING_MODE multicopterPosXyCoefficients.breakingBoostFactor = (float) navConfig()->mc.braking_boost_factor / 100.0f; +#endif // Initialize altitude hold PID-controllers (pos_z, vel_z, acc_z navPidInit( diff --git a/src/main/navigation/navigation.h b/src/main/navigation/navigation.h index 1bd5e858fb..9fd15582dd 100755 --- a/src/main/navigation/navigation.h +++ b/src/main/navigation/navigation.h @@ -209,6 +209,8 @@ typedef struct navConfig_s { uint8_t max_bank_angle; // multicopter max banking angle (deg) uint16_t hover_throttle; // multicopter hover throttle uint16_t auto_disarm_delay; // multicopter safety delay for landing detector + +#ifdef USE_MR_BRAKING_MODE uint16_t braking_speed_threshold; // above this speed braking routine might kick in uint16_t braking_disengage_speed; // below this speed braking will be disengaged uint16_t braking_timeout; // Timeout for braking mode @@ -217,6 +219,8 @@ typedef struct navConfig_s { uint16_t braking_boost_speed_threshold; // Above this speed braking boost mode can engage uint16_t braking_boost_disengage_speed; // Below this speed braking boost will disengage uint8_t braking_bank_angle; // Max angle [deg] that MR is allowed duing braking boost phase +#endif + uint8_t posDecelerationTime; // Brake time parameter uint8_t posResponseExpo; // Position controller expo (taret vel expo for MC) bool slowDownForTurning; // Slow down during WP missions when changing heading on next waypoint diff --git a/src/main/navigation/navigation_pos_estimator.c b/src/main/navigation/navigation_pos_estimator.c index 496aeb6c1f..f62de049ce 100755 --- a/src/main/navigation/navigation_pos_estimator.c +++ b/src/main/navigation/navigation_pos_estimator.c @@ -37,6 +37,7 @@ #include "drivers/time.h" #include "fc/config.h" +#include "fc/settings.h" #include "flight/imu.h" #include "flight/secondary_imu.h" @@ -59,39 +60,39 @@ PG_REGISTER_WITH_RESET_TEMPLATE(positionEstimationConfig_t, positionEstimationCo PG_RESET_TEMPLATE(positionEstimationConfig_t, positionEstimationConfig, // Inertial position estimator parameters - .automatic_mag_declination = 1, - .reset_altitude_type = NAV_RESET_ON_FIRST_ARM, - .reset_home_type = NAV_RESET_ON_FIRST_ARM, - .gravity_calibration_tolerance = 5, // 5 cm/s/s calibration error accepted (0.5% of gravity) - .use_gps_velned = 1, // "Disabled" is mandatory with gps_dyn_model = Pedestrian - .use_gps_no_baro = 0, // Use GPS altitude if no baro is available on all aircrafts - .allow_dead_reckoning = 0, + .automatic_mag_declination = SETTING_INAV_AUTO_MAG_DECL_DEFAULT, + .reset_altitude_type = SETTING_INAV_RESET_ALTITUDE_DEFAULT, + .reset_home_type = SETTING_INAV_RESET_HOME_DEFAULT, + .gravity_calibration_tolerance = SETTING_INAV_GRAVITY_CAL_TOLERANCE_DEFAULT, // 5 cm/s/s calibration error accepted (0.5% of gravity) + .use_gps_velned = SETTING_INAV_USE_GPS_VELNED_DEFAULT, // "Disabled" is mandatory with gps_dyn_model = Pedestrian + .use_gps_no_baro = SETTING_INAV_USE_GPS_NO_BARO_DEFAULT, // Use GPS altitude if no baro is available on all aircrafts + .allow_dead_reckoning = SETTING_INAV_ALLOW_DEAD_RECKONING_DEFAULT, - .max_surface_altitude = 200, + .max_surface_altitude = SETTING_INAV_MAX_SURFACE_ALTITUDE_DEFAULT, - .w_xyz_acc_p = 1.0f, + .w_xyz_acc_p = SETTING_INAV_W_XYZ_ACC_P_DEFAULT, - .w_z_baro_p = 0.35f, + .w_z_baro_p = SETTING_INAV_W_Z_BARO_P_DEFAULT, - .w_z_surface_p = 3.500f, - .w_z_surface_v = 6.100f, + .w_z_surface_p = SETTING_INAV_W_Z_SURFACE_P_DEFAULT, + .w_z_surface_v = SETTING_INAV_W_Z_SURFACE_V_DEFAULT, - .w_z_gps_p = 0.2f, - .w_z_gps_v = 0.1f, + .w_z_gps_p = SETTING_INAV_W_Z_GPS_P_DEFAULT, + .w_z_gps_v = SETTING_INAV_W_Z_GPS_V_DEFAULT, - .w_xy_gps_p = 1.0f, - .w_xy_gps_v = 2.0f, + .w_xy_gps_p = SETTING_INAV_W_XY_GPS_P_DEFAULT, + .w_xy_gps_v = SETTING_INAV_W_XY_GPS_V_DEFAULT, - .w_xy_flow_p = 1.0f, - .w_xy_flow_v = 2.0f, + .w_xy_flow_p = SETTING_INAV_W_XY_FLOW_P_DEFAULT, + .w_xy_flow_v = SETTING_INAV_W_XY_FLOW_V_DEFAULT, - .w_z_res_v = 0.5f, - .w_xy_res_v = 0.5f, + .w_z_res_v = SETTING_INAV_W_Z_RES_V_DEFAULT, + .w_xy_res_v = SETTING_INAV_W_XY_RES_V_DEFAULT, - .w_acc_bias = 0.01f, + .w_acc_bias = SETTING_INAV_W_ACC_BIAS_DEFAULT, - .max_eph_epv = 1000.0f, - .baro_epv = 100.0f + .max_eph_epv = SETTING_INAV_MAX_EPH_EPV_DEFAULT, + .baro_epv = SETTING_INAV_BARO_EPV_DEFAULT ); #define resetTimer(tim, currentTimeUs) { (tim)->deltaTime = 0; (tim)->lastTriggeredTime = currentTimeUs; } diff --git a/src/main/rx/eleres.c b/src/main/rx/eleres.c index 638818c531..705c4b7e15 100755 --- a/src/main/rx/eleres.c +++ b/src/main/rx/eleres.c @@ -72,12 +72,12 @@ PG_REGISTER_WITH_RESET_TEMPLATE(eleresConfig_t, eleresConfig, PG_ELERES_CONFIG, 0); PG_RESET_TEMPLATE(eleresConfig_t, eleresConfig, - .eleresFreq = 435, - .eleresTelemetryEn = 0, - .eleresTelemetryPower = 7, - .eleresLocEn = 0, - .eleresLocPower = 7, - .eleresLocDelay = 240 + .eleresFreq = SETTING_ELERES_FREQ_DEFAULT, + .eleresTelemetryEn = SETTING_ELERES_TELEMETRY_EN_DEFAULT, + .eleresTelemetryPower = SETTING_ELERES_TELEMETRY_POWER_DEFAULT, + .eleresLocEn = SETTING_ELERES_LOC_EN_DEFAULT, + .eleresLocPower = SETTING_ELERES_LOC_POWER_DEFAULT, + .eleresLocDelay = SETTING_ELERES_LOC_DELAY_DEFAULT ); static uint8_t hoppingChannel = 1; @@ -784,8 +784,6 @@ void eleresInit(const rxConfig_t *rxConfig, rxRuntimeConfig_t *rxRuntimeConfig) channelHopping(1); rfMode = RECEIVE; localizerTime = millis() + (1000L * eleresConfig()->eleresLocDelay); - - return true; } uint8_t eleresBind(void) diff --git a/src/main/rx/rx.c b/src/main/rx/rx.c index f028e73330..c8e1184594 100755 --- a/src/main/rx/rx.c +++ b/src/main/rx/rx.c @@ -45,6 +45,7 @@ #include "fc/config.h" #include "fc/rc_controls.h" #include "fc/rc_modes.h" +#include "fc/settings.h" #include "flight/failsafe.h" @@ -125,26 +126,32 @@ PG_REGISTER_WITH_RESET_TEMPLATE(rxConfig_t, rxConfig, PG_RX_CONFIG, 9); PG_RESET_TEMPLATE(rxConfig_t, rxConfig, .receiverType = DEFAULT_RX_TYPE, .rcmap = {0, 1, 3, 2}, // Default to AETR map - .halfDuplex = TRISTATE_AUTO, + .halfDuplex = SETTING_SERIALRX_HALFDUPLEX_DEFAULT, .serialrx_provider = SERIALRX_PROVIDER, +#ifdef USE_RX_SPI .rx_spi_protocol = RX_SPI_DEFAULT_PROTOCOL, - .spektrum_sat_bind = 0, - .serialrx_inverted = 0, - .mincheck = 1100, - .maxcheck = 1900, - .rx_min_usec = RX_MIN_USEX, // any of first 4 channels below this value will trigger rx loss detection - .rx_max_usec = 2115, // any of first 4 channels above this value will trigger rx loss detection - .rssi_channel = 0, - .rssiMin = RSSI_VISIBLE_VALUE_MIN, - .rssiMax = RSSI_VISIBLE_VALUE_MAX, - .sbusSyncInterval = SBUS_DEFAULT_INTERFRAME_DELAY_US, - .rcFilterFrequency = 50, -#if defined(USE_RX_MSP) && defined(USE_MSP_RC_OVERRIDE) - .mspOverrideChannels = 15, #endif - .rssi_source = RSSI_SOURCE_AUTO, - .srxl2_unit_id = 1, - .srxl2_baud_fast = 1, +#ifdef USE_SPEKTRUM_BIND + .spektrum_sat_bind = SETTING_SPEKTRUM_SAT_BIND_DEFAULT, +#endif + .serialrx_inverted = SETTING_SERIALRX_INVERTED_DEFAULT, + .mincheck = SETTING_MIN_CHECK_DEFAULT, + .maxcheck = SETTING_MAX_CHECK_DEFAULT, + .rx_min_usec = SETTING_RX_MIN_USEC_DEFAULT, // any of first 4 channels below this value will trigger rx loss detection + .rx_max_usec = SETTING_RX_MAX_USEC_DEFAULT, // any of first 4 channels above this value will trigger rx loss detection + .rssi_channel = SETTING_RSSI_CHANNEL_DEFAULT, + .rssiMin = SETTING_RSSI_MIN_DEFAULT, + .rssiMax = SETTING_RSSI_MAX_DEFAULT, + .sbusSyncInterval = SETTING_SBUS_SYNC_INTERVAL_DEFAULT, + .rcFilterFrequency = SETTING_RC_FILTER_FREQUENCY_DEFAULT, +#if defined(USE_RX_MSP) && defined(USE_MSP_RC_OVERRIDE) + .mspOverrideChannels = SETTING_MSP_OVERRIDE_CHANNELS_DEFAULT, +#endif + .rssi_source = SETTING_RSSI_SOURCE_DEFAULT, +#ifdef USE_SERIALRX_SRXL2 + .srxl2_unit_id = SETTING_SRXL2_UNIT_ID_DEFAULT, + .srxl2_baud_fast = SETTING_SRXL2_BAUD_FAST_DEFAULT, +#endif ); void resetAllRxChannelRangeConfigurations(void) diff --git a/src/main/rx/rx.h b/src/main/rx/rx.h index 0aee1e8353..dcc1804f5d 100644 --- a/src/main/rx/rx.h +++ b/src/main/rx/rx.h @@ -114,11 +114,15 @@ typedef struct rxConfig_s { uint8_t serialrx_provider; // Type of UART-based receiver (rxSerialReceiverType_e enum). Only used if receiverType is RX_TYPE_SERIAL uint8_t serialrx_inverted; // Flip the default inversion of the protocol - e.g. sbus (Futaba, FrSKY) is inverted if this is false, uninverted if it's true. Support for uninverted OpenLRS (and modified FrSKY) receivers. uint8_t halfDuplex; // allow rx to operate in half duplex mode. From tristate_e. +#ifdef USE_RX_SPI uint8_t rx_spi_protocol; // type of SPI receiver protocol (rx_spi_protocol_e enum). Only used if receiverType is RX_TYPE_SPI uint32_t rx_spi_id; uint8_t rx_spi_rf_channel_count; +#endif +#ifdef USE_SPEKTRUM_BIND uint8_t spektrum_sat_bind; // number of bind pulses for Spektrum satellite receivers uint8_t spektrum_sat_bind_autoreset; // whenever we will reset (exit) binding mode after hard reboot +#endif uint8_t rssi_channel; uint8_t rssiMin; // minimum RSSI sent by the RX - [RSSI_VISIBLE_VALUE_MIN, RSSI_VISIBLE_VALUE_MAX] uint8_t rssiMax; // maximum RSSI sent by the RX - [RSSI_VISIBLE_VALUE_MIN, RSSI_VISIBLE_VALUE_MAX] @@ -130,8 +134,10 @@ typedef struct rxConfig_s { uint8_t rcFilterFrequency; // RC filter cutoff frequency (smoothness vs response sharpness) uint16_t mspOverrideChannels; // Channels to override with MSP RC when BOXMSPRCOVERRIDE is active uint8_t rssi_source; +#ifdef USE_SERIALRX_SRXL2 uint8_t srxl2_unit_id; uint8_t srxl2_baud_fast; +#endif } rxConfig_t; PG_DECLARE(rxConfig_t, rxConfig); diff --git a/src/main/sensors/acceleration.c b/src/main/sensors/acceleration.c index 241f1c417a..aeb6af2965 100644 --- a/src/main/sensors/acceleration.c +++ b/src/main/sensors/acceleration.c @@ -58,6 +58,7 @@ FILE_COMPILE_FOR_SPEED #include "fc/config.h" #include "fc/runtime_config.h" +#include "fc/settings.h" #include "io/beeper.h" @@ -93,22 +94,22 @@ PG_REGISTER_WITH_RESET_FN(accelerometerConfig_t, accelerometerConfig, PG_ACCELER void pgResetFn_accelerometerConfig(accelerometerConfig_t *instance) { RESET_CONFIG_2(accelerometerConfig_t, instance, - .acc_align = ALIGN_DEFAULT, - .acc_hardware = ACC_AUTODETECT, - .acc_lpf_hz = 15, - .acc_notch_hz = 0, - .acc_notch_cutoff = 1, - .acc_soft_lpf_type = FILTER_BIQUAD + .acc_align = SETTING_ALIGN_ACC_DEFAULT, + .acc_hardware = SETTING_ACC_HARDWARE_DEFAULT, + .acc_lpf_hz = SETTING_ACC_LPF_HZ_DEFAULT, + .acc_notch_hz = SETTING_ACC_NOTCH_HZ_DEFAULT, + .acc_notch_cutoff = SETTING_ACC_NOTCH_CUTOFF_DEFAULT, + .acc_soft_lpf_type = SETTING_ACC_LPF_TYPE_DEFAULT ); RESET_CONFIG_2(flightDynamicsTrims_t, &instance->accZero, - .raw[X] = 0, - .raw[Y] = 0, - .raw[Z] = 0 + .raw[X] = SETTING_ACCZERO_X_DEFAULT, + .raw[Y] = SETTING_ACCZERO_Y_DEFAULT, + .raw[Z] = SETTING_ACCZERO_Z_DEFAULT ); RESET_CONFIG_2(flightDynamicsTrims_t, &instance->accGain, - .raw[X] = 4096, - .raw[Y] = 4096, - .raw[Z] = 4096 + .raw[X] = SETTING_ACCGAIN_X_DEFAULT, + .raw[Y] = SETTING_ACCGAIN_Y_DEFAULT, + .raw[Z] = SETTING_ACCGAIN_Z_DEFAULT ); } diff --git a/src/main/sensors/barometer.c b/src/main/sensors/barometer.c index bb1325aeeb..63532a3986 100644 --- a/src/main/sensors/barometer.c +++ b/src/main/sensors/barometer.c @@ -44,6 +44,7 @@ #include "drivers/time.h" #include "fc/runtime_config.h" +#include "fc/settings.h" #include "sensors/barometer.h" #include "sensors/sensors.h" @@ -56,21 +57,16 @@ baro_t baro; // barometer access functions +#ifdef USE_BARO + PG_REGISTER_WITH_RESET_TEMPLATE(barometerConfig_t, barometerConfig, PG_BAROMETER_CONFIG, 3); -#ifdef USE_BARO -#define BARO_HARDWARE_DEFAULT BARO_AUTODETECT -#else -#define BARO_HARDWARE_DEFAULT BARO_NONE -#endif PG_RESET_TEMPLATE(barometerConfig_t, barometerConfig, - .baro_hardware = BARO_HARDWARE_DEFAULT, - .use_median_filtering = 1, - .baro_calibration_tolerance = 150 + .baro_hardware = SETTING_BARO_HARDWARE_DEFAULT, + .use_median_filtering = SETTING_BARO_MEDIAN_FILTER_DEFAULT, + .baro_calibration_tolerance = SETTING_BARO_CAL_TOLERANCE_DEFAULT ); -#ifdef USE_BARO - static zeroCalibrationScalar_t zeroCalibration; static float baroGroundAltitude = 0; static float baroGroundPressure = 101325.0f; // 101325 pascal, 1 standard atmosphere diff --git a/src/main/sensors/barometer.h b/src/main/sensors/barometer.h index 2e365fb115..3c04c86e46 100644 --- a/src/main/sensors/barometer.h +++ b/src/main/sensors/barometer.h @@ -37,14 +37,6 @@ typedef enum { BARO_MAX = BARO_FAKE } baroSensor_e; -typedef struct barometerConfig_s { - uint8_t baro_hardware; // Barometer hardware to use - uint8_t use_median_filtering; // Use 3-point median filtering - uint16_t baro_calibration_tolerance; // Baro calibration tolerance (cm at sea level) -} barometerConfig_t; - -PG_DECLARE(barometerConfig_t, barometerConfig); - typedef struct baro_s { baroDev_t dev; int32_t BaroAlt; @@ -54,6 +46,17 @@ typedef struct baro_s { extern baro_t baro; +#ifdef USE_BARO + +typedef struct barometerConfig_s { + uint8_t baro_hardware; // Barometer hardware to use + uint8_t use_median_filtering; // Use 3-point median filtering + uint16_t baro_calibration_tolerance; // Baro calibration tolerance (cm at sea level) +} barometerConfig_t; + +PG_DECLARE(barometerConfig_t, barometerConfig); + + bool baroInit(void); bool baroIsCalibrationComplete(void); void baroStartCalibration(void); @@ -62,3 +65,5 @@ int32_t baroCalculateAltitude(void); int32_t baroGetLatestAltitude(void); int16_t baroGetTemperature(void); bool baroIsHealthy(void); + +#endif diff --git a/src/main/sensors/battery.c b/src/main/sensors/battery.c index 420a9790e9..1b62c01e9a 100644 --- a/src/main/sensors/battery.c +++ b/src/main/sensors/battery.c @@ -39,6 +39,7 @@ #include "fc/fc_core.h" #include "fc/runtime_config.h" #include "fc/stats.h" +#include "fc/settings.h" #include "flight/imu.h" #include "flight/mixer.h" @@ -97,20 +98,22 @@ void pgResetFn_batteryProfiles(batteryProfile_t *instance) { for (int i = 0; i < MAX_BATTERY_PROFILE_COUNT; i++) { RESET_CONFIG(batteryProfile_t, &instance[i], - .cells = 0, +#ifdef USE_ADC + .cells = SETTING_BAT_CELLS_DEFAULT, .voltage = { - .cellDetect = 425, - .cellMax = 420, - .cellMin = 330, - .cellWarning = 350 + .cellDetect = SETTING_VBAT_CELL_DETECT_VOLTAGE_DEFAULT, + .cellMax = SETTING_VBAT_MAX_CELL_VOLTAGE_DEFAULT, + .cellMin = SETTING_VBAT_MIN_CELL_VOLTAGE_DEFAULT, + .cellWarning = SETTING_VBAT_WARNING_CELL_VOLTAGE_DEFAULT }, +#endif .capacity = { - .value = 0, - .warning = 0, - .critical = 0, - .unit = BAT_CAPACITY_UNIT_MAH, + .value = SETTING_BATTERY_CAPACITY_DEFAULT, + .warning = SETTING_BATTERY_CAPACITY_WARNING_DEFAULT, + .critical = SETTING_BATTERY_CAPACITY_CRITICAL_DEFAULT, + .unit = SETTING_BATTERY_CAPACITY_UNIT_DEFAULT, } ); } @@ -120,24 +123,26 @@ PG_REGISTER_WITH_RESET_TEMPLATE(batteryMetersConfig_t, batteryMetersConfig, PG_B PG_RESET_TEMPLATE(batteryMetersConfig_t, batteryMetersConfig, +#ifdef USE_ADC .voltage = { - .type = VOLTAGE_SENSOR_ADC, + .type = SETTING_VBAT_METER_TYPE_DEFAULT, .scale = VBAT_SCALE_DEFAULT, }, +#endif .current = { - .type = CURRENT_SENSOR_ADC, + .type = SETTING_CURRENT_METER_TYPE_DEFAULT, .scale = CURRENT_METER_SCALE, .offset = CURRENT_METER_OFFSET }, - .voltageSource = BAT_VOLTAGE_RAW, + .voltageSource = SETTING_BAT_VOLTAGE_SRC_DEFAULT, - .cruise_power = 0, - .idle_power = 0, - .rth_energy_margin = 5, + .cruise_power = SETTING_CRUISE_POWER_DEFAULT, + .idle_power = SETTING_IDLE_POWER_DEFAULT, + .rth_energy_margin = SETTING_RTH_ENERGY_MARGIN_DEFAULT, - .throttle_compensation_weight = 1.0f + .throttle_compensation_weight = SETTING_THR_COMP_WEIGHT_DEFAULT ); @@ -150,6 +155,7 @@ void batteryInit(void) batteryCriticalVoltage = 0; } +#ifdef USE_ADC // profileDetect() profile sorting compare function static int profile_compare(profile_comp_t *a, profile_comp_t *b) { if (a->max_voltage < b->max_voltage) @@ -182,6 +188,7 @@ static int8_t profileDetect(void) { // No matching profile found return -1; } +#endif void setBatteryProfile(uint8_t profileIndex) { @@ -202,6 +209,7 @@ void activateBatteryProfile(void) } } +#ifdef USE_ADC static void updateBatteryVoltage(timeUs_t timeDelta, bool justConnected) { static pt1Filter_t vbatFilterState; @@ -342,6 +350,7 @@ void batteryUpdate(timeUs_t timeDelta) } } } +#endif batteryState_e getBatteryState(void) { diff --git a/src/main/sensors/battery.h b/src/main/sensors/battery.h index 353f537d01..94201fb28c 100644 --- a/src/main/sensors/battery.h +++ b/src/main/sensors/battery.h @@ -69,10 +69,12 @@ typedef enum { typedef struct batteryMetersConfig_s { +#ifdef USE_ADC struct { uint16_t scale; voltageSensor_e type; } voltage; +#endif struct { int16_t scale; // scale the current sensor output voltage to milliamps. Value in 1/10th mV/A @@ -92,6 +94,7 @@ typedef struct batteryMetersConfig_s { typedef struct batteryProfile_s { +#ifdef USE_ADC uint8_t cells; struct { @@ -100,6 +103,7 @@ typedef struct batteryProfile_s { uint16_t cellMin; // minimum voltage per cell, this triggers battery critical alarm, in 0.01V units, default is 330 (3.3V) uint16_t cellWarning; // warning voltage per cell, this triggers battery warning alarm, in 0.01V units, default is 350 (3.5V) } voltage; +#endif struct { uint32_t value; // mAh or mWh (see capacity.unit) @@ -151,10 +155,13 @@ int32_t getPower(void); int32_t getMAhDrawn(void); int32_t getMWhDrawn(void); +#ifdef USE_ADC void batteryUpdate(timeUs_t timeDelta); -void currentMeterUpdate(timeUs_t timeDelta); void sagCompensatedVBatUpdate(timeUs_t currentTime, timeUs_t timeDelta); void powerMeterUpdate(timeUs_t timeDelta); +#endif + +void currentMeterUpdate(timeUs_t timeDelta); uint8_t calculateBatteryPercentage(void); float calculateThrottleCompensationFactor(void); diff --git a/src/main/sensors/compass.c b/src/main/sensors/compass.c index 287d3d1da4..8e1da170b9 100644 --- a/src/main/sensors/compass.c +++ b/src/main/sensors/compass.c @@ -48,6 +48,7 @@ #include "fc/config.h" #include "fc/runtime_config.h" +#include "fc/settings.h" #include "io/gps.h" #include "io/beeper.h" @@ -59,27 +60,24 @@ mag_t mag; // mag access functions +#ifdef USE_MAG + PG_REGISTER_WITH_RESET_TEMPLATE(compassConfig_t, compassConfig, PG_COMPASS_CONFIG, 4); -#ifdef USE_MAG -#define MAG_HARDWARE_DEFAULT MAG_AUTODETECT -#else -#define MAG_HARDWARE_DEFAULT MAG_NONE -#endif PG_RESET_TEMPLATE(compassConfig_t, compassConfig, - .mag_align = ALIGN_DEFAULT, - .mag_hardware = MAG_HARDWARE_DEFAULT, - .mag_declination = 0, - .mag_to_use = 0, - .magCalibrationTimeLimit = 30, - .rollDeciDegrees = 0, - .pitchDeciDegrees = 0, - .yawDeciDegrees = 0, - .magGain = {1024, 1024, 1024}, + .mag_align = SETTING_ALIGN_MAG_DEFAULT, + .mag_hardware = SETTING_MAG_HARDWARE_DEFAULT, + .mag_declination = SETTING_MAG_DECLINATION_DEFAULT, +#ifdef USE_DUAL_MAG + .mag_to_use = SETTING_MAG_TO_USE_DEFAULT, +#endif + .magCalibrationTimeLimit = SETTING_MAG_CALIBRATION_TIME_DEFAULT, + .rollDeciDegrees = SETTING_ALIGN_MAG_ROLL_DEFAULT, + .pitchDeciDegrees = SETTING_ALIGN_MAG_PITCH_DEFAULT, + .yawDeciDegrees = SETTING_ALIGN_MAG_YAW_DEFAULT, + .magGain = {SETTING_MAGGAIN_X_DEFAULT, SETTING_MAGGAIN_Y_DEFAULT, SETTING_MAGGAIN_Z_DEFAULT}, ); -#ifdef USE_MAG - static uint8_t magUpdatedAtLeastOnce = 0; bool compassDetect(magDev_t *dev, magSensor_e magHardwareToUse) diff --git a/src/main/sensors/compass.h b/src/main/sensors/compass.h index 1695311bd2..baba7bf6a1 100644 --- a/src/main/sensors/compass.h +++ b/src/main/sensors/compass.h @@ -26,7 +26,6 @@ #include "sensors/sensors.h" - // Type of magnetometer used/detected typedef enum { MAG_NONE = 0, @@ -54,6 +53,8 @@ typedef struct mag_s { extern mag_t mag; +#ifdef USE_MAG + typedef struct compassConfig_s { int16_t mag_declination; // Get your magnetic declination from here : http://magnetic-declination.com/ // For example, -6deg 37min, = -637 Japan, format is [sign]dddmm (degreesminutes) default is zero. @@ -61,7 +62,9 @@ typedef struct compassConfig_s { uint8_t mag_hardware; // Which mag hardware to use on boards with more than one device flightDynamicsTrims_t magZero; int16_t magGain[XYZ_AXIS_COUNT]; +#ifdef USE_DUAL_MAG uint8_t mag_to_use; +#endif uint8_t magCalibrationTimeLimit; // Time for compass calibration (seconds) int16_t rollDeciDegrees; // Alignment for external mag on the roll (X) axis (0.1deg) int16_t pitchDeciDegrees; // Alignment for external mag on the pitch (Y) axis (0.1deg) @@ -75,3 +78,5 @@ bool compassInit(void); void compassUpdate(timeUs_t currentTimeUs); bool compassIsReady(void); bool compassIsHealthy(void); + +#endif diff --git a/src/main/sensors/esc_sensor.c b/src/main/sensors/esc_sensor.c index bd6e9886ec..672428b615 100644 --- a/src/main/sensors/esc_sensor.c +++ b/src/main/sensors/esc_sensor.c @@ -46,6 +46,7 @@ #include "io/serial.h" #include "fc/config.h" #include "fc/runtime_config.h" +#include "fc/settings.h" #if defined(USE_ESC_SENSOR) @@ -80,7 +81,7 @@ static bool escSensorDataNeedsUpdate; PG_REGISTER_WITH_RESET_TEMPLATE(escSensorConfig_t, escSensorConfig, PG_ESC_SENSOR_CONFIG, 1); PG_RESET_TEMPLATE(escSensorConfig_t, escSensorConfig, .currentOffset = 0, // UNUSED - .listenOnly = 0, + .listenOnly = SETTING_ESC_SENSOR_LISTEN_ONLY_DEFAULT, ); static int getTelemetryMotorCount(void) diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c index 0f72aa9f7b..6a8710bff2 100644 --- a/src/main/sensors/gyro.c +++ b/src/main/sensors/gyro.c @@ -61,6 +61,7 @@ FILE_COMPILE_FOR_SPEED #include "fc/config.h" #include "fc/runtime_config.h" #include "fc/rc_controls.h" +#include "fc/settings.h" #include "io/beeper.h" #include "io/statusindicator.h" @@ -106,26 +107,30 @@ EXTENDED_FASTRAM dynamicGyroNotchState_t dynamicGyroNotchState; PG_REGISTER_WITH_RESET_TEMPLATE(gyroConfig_t, gyroConfig, PG_GYRO_CONFIG, 11); PG_RESET_TEMPLATE(gyroConfig_t, gyroConfig, - .gyro_lpf = GYRO_LPF_256HZ, - .gyro_soft_lpf_hz = 60, - .gyro_soft_lpf_type = FILTER_BIQUAD, - .gyro_align = ALIGN_DEFAULT, - .gyroMovementCalibrationThreshold = 32, - .looptime = 1000, - .gyroSync = 1, - .gyro_to_use = 0, - .gyro_notch_hz = 0, - .gyro_notch_cutoff = 1, - .gyro_stage2_lowpass_hz = 0, - .gyro_stage2_lowpass_type = FILTER_BIQUAD, - .useDynamicLpf = 0, - .gyroDynamicLpfMinHz = 200, - .gyroDynamicLpfMaxHz = 500, - .gyroDynamicLpfCurveExpo = 5, - .dynamicGyroNotchRange = DYN_NOTCH_RANGE_MEDIUM, - .dynamicGyroNotchQ = 120, - .dynamicGyroNotchMinHz = 150, - .dynamicGyroNotchEnabled = 0, + .gyro_lpf = SETTING_GYRO_HARDWARE_LPF_DEFAULT, + .gyro_soft_lpf_hz = SETTING_GYRO_LPF_HZ_DEFAULT, + .gyro_soft_lpf_type = SETTING_GYRO_LPF_TYPE_DEFAULT, + .gyro_align = SETTING_ALIGN_GYRO_DEFAULT, + .gyroMovementCalibrationThreshold = SETTING_MORON_THRESHOLD_DEFAULT, + .looptime = SETTING_LOOPTIME_DEFAULT, + .gyroSync = SETTING_GYRO_SYNC_DEFAULT, +#ifdef USE_DUAL_GYRO + .gyro_to_use = SETTING_GYRO_TO_USE_DEFAULT, +#endif + .gyro_notch_hz = SETTING_GYRO_NOTCH_HZ_DEFAULT, + .gyro_notch_cutoff = SETTING_GYRO_NOTCH_CUTOFF_DEFAULT, + .gyro_stage2_lowpass_hz = SETTING_GYRO_STAGE2_LOWPASS_HZ_DEFAULT, + .gyro_stage2_lowpass_type = SETTING_GYRO_STAGE2_LOWPASS_TYPE_DEFAULT, + .useDynamicLpf = SETTING_GYRO_USE_DYN_LPF_DEFAULT, + .gyroDynamicLpfMinHz = SETTING_GYRO_DYN_LPF_MIN_HZ_DEFAULT, + .gyroDynamicLpfMaxHz = SETTING_GYRO_DYN_LPF_MAX_HZ_DEFAULT, + .gyroDynamicLpfCurveExpo = SETTING_GYRO_DYN_LPF_CURVE_EXPO_DEFAULT, +#ifdef USE_DYNAMIC_FILTERS + .dynamicGyroNotchRange = SETTING_DYNAMIC_GYRO_NOTCH_RANGE_DEFAULT, + .dynamicGyroNotchQ = SETTING_DYNAMIC_GYRO_NOTCH_Q_DEFAULT, + .dynamicGyroNotchMinHz = SETTING_DYNAMIC_GYRO_NOTCH_MIN_HZ_DEFAULT, + .dynamicGyroNotchEnabled = SETTING_DYNAMIC_GYRO_NOTCH_ENABLED_DEFAULT, +#endif ); STATIC_UNIT_TESTED gyroSensor_e gyroDetect(gyroDev_t *dev, gyroSensor_e gyroHardware) @@ -536,4 +541,4 @@ void gyroUpdateDynamicLpf(float cutoffFreq) { biquadFilterUpdate(&gyroLpfState[axis].biquad, cutoffFreq, getLooptime(), BIQUAD_Q, FILTER_LPF); } } -} \ No newline at end of file +} diff --git a/src/main/sensors/gyro.h b/src/main/sensors/gyro.h index 95174bb055..6aa3c11414 100644 --- a/src/main/sensors/gyro.h +++ b/src/main/sensors/gyro.h @@ -61,12 +61,14 @@ extern gyro_t gyro; typedef struct gyroConfig_s { sensor_align_e gyro_align; // gyro alignment uint8_t gyroMovementCalibrationThreshold; // people keep forgetting that moving model while init results in wrong gyro offsets. and then they never reset gyro. so this is now on by default. - uint8_t gyroSync; // Enable interrupt based loop + bool gyroSync; // Enable interrupt based loop uint16_t looptime; // imu loop time in us uint8_t gyro_lpf; // gyro LPF setting - values are driver specific, in case of invalid number, a reasonable default ~30-40HZ is chosen. uint8_t gyro_soft_lpf_hz; uint8_t gyro_soft_lpf_type; +#ifdef USE_DUAL_GYRO uint8_t gyro_to_use; +#endif uint16_t gyro_notch_hz; uint16_t gyro_notch_cutoff; uint16_t gyro_stage2_lowpass_hz; @@ -75,10 +77,12 @@ typedef struct gyroConfig_s { uint16_t gyroDynamicLpfMinHz; uint16_t gyroDynamicLpfMaxHz; uint8_t gyroDynamicLpfCurveExpo; +#ifdef USE_DYNAMIC_FILTERS uint8_t dynamicGyroNotchRange; uint16_t dynamicGyroNotchQ; uint16_t dynamicGyroNotchMinHz; uint8_t dynamicGyroNotchEnabled; +#endif } gyroConfig_t; PG_DECLARE(gyroConfig_t, gyroConfig); diff --git a/src/main/sensors/opflow.c b/src/main/sensors/opflow.c index f0f47b556f..9c1b6f265e 100755 --- a/src/main/sensors/opflow.c +++ b/src/main/sensors/opflow.c @@ -50,6 +50,7 @@ #include "fc/config.h" #include "fc/runtime_config.h" +#include "fc/settings.h" #include "sensors/boardalignment.h" #include "sensors/gyro.h" @@ -78,9 +79,9 @@ static float opflowCalibrationFlowAcc; PG_REGISTER_WITH_RESET_TEMPLATE(opticalFlowConfig_t, opticalFlowConfig, PG_OPFLOW_CONFIG, 2); PG_RESET_TEMPLATE(opticalFlowConfig_t, opticalFlowConfig, - .opflow_hardware = OPFLOW_NONE, - .opflow_align = CW0_DEG_FLIP, - .opflow_scale = 10.5f, + .opflow_hardware = SETTING_OPFLOW_HARDWARE_DEFAULT, + .opflow_align = SETTING_ALIGN_OPFLOW_DEFAULT, + .opflow_scale = SETTING_OPFLOW_SCALE_DEFAULT, ); static bool opflowDetect(opflowDev_t * dev, uint8_t opflowHardwareToUse) diff --git a/src/main/sensors/pitotmeter.c b/src/main/sensors/pitotmeter.c index 60fbaed019..a330be79bb 100644 --- a/src/main/sensors/pitotmeter.c +++ b/src/main/sensors/pitotmeter.c @@ -38,6 +38,7 @@ #include "fc/config.h" #include "fc/runtime_config.h" +#include "fc/settings.h" #include "scheduler/protothreads.h" @@ -58,9 +59,9 @@ PG_REGISTER_WITH_RESET_TEMPLATE(pitotmeterConfig_t, pitotmeterConfig, PG_PITOTME #define PITOT_HARDWARE_DEFAULT PITOT_NONE #endif PG_RESET_TEMPLATE(pitotmeterConfig_t, pitotmeterConfig, - .pitot_hardware = PITOT_HARDWARE_DEFAULT, - .pitot_lpf_milli_hz = 350, - .pitot_scale = 1.00f + .pitot_hardware = SETTING_PITOT_HARDWARE_DEFAULT, + .pitot_lpf_milli_hz = SETTING_PITOT_LPF_MILLI_HZ_DEFAULT, + .pitot_scale = SETTING_PITOT_SCALE_DEFAULT ); bool pitotDetect(pitotDev_t *dev, uint8_t pitotHardwareToUse) diff --git a/src/main/sensors/rangefinder.c b/src/main/sensors/rangefinder.c index 5c7f10c353..0129cdfb3b 100644 --- a/src/main/sensors/rangefinder.c +++ b/src/main/sensors/rangefinder.c @@ -45,6 +45,7 @@ #include "fc/config.h" #include "fc/runtime_config.h" +#include "fc/settings.h" #include "sensors/sensors.h" #include "sensors/rangefinder.h" @@ -65,8 +66,8 @@ rangefinder_t rangefinder; PG_REGISTER_WITH_RESET_TEMPLATE(rangefinderConfig_t, rangefinderConfig, PG_RANGEFINDER_CONFIG, 1); PG_RESET_TEMPLATE(rangefinderConfig_t, rangefinderConfig, - .rangefinder_hardware = RANGEFINDER_NONE, - .use_median_filtering = 0, + .rangefinder_hardware = SETTING_RANGEFINDER_HARDWARE_DEFAULT, + .use_median_filtering = SETTING_RANGEFINDER_MEDIAN_FILTER_DEFAULT, ); const rangefinderHardwarePins_t * rangefinderGetHardwarePins(void) @@ -287,4 +288,3 @@ bool rangefinderIsHealthy(void) return (millis() - rangefinder.lastValidResponseTimeMs) < RANGEFINDER_HARDWARE_TIMEOUT_MS; } #endif - diff --git a/src/main/target/ALIENFLIGHTF3/config.c b/src/main/target/ALIENFLIGHTF3/config.c index cc1e81203f..ad553d6aad 100644 --- a/src/main/target/ALIENFLIGHTF3/config.c +++ b/src/main/target/ALIENFLIGHTF3/config.c @@ -63,9 +63,6 @@ void targetConfiguration(void) pidProfileMutable()->bank_mc.pid[PITCH].P = 36; failsafeConfigMutable()->failsafe_delay = 2; failsafeConfigMutable()->failsafe_off_delay = 0; - controlRateProfilesMutable(0)->stabilized.rates[FD_PITCH] = CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_DEFAULT; - controlRateProfilesMutable(0)->stabilized.rates[FD_ROLL] = CONTROL_RATE_CONFIG_ROLL_PITCH_RATE_DEFAULT; - controlRateProfilesMutable(0)->stabilized.rates[FD_YAW] = CONTROL_RATE_CONFIG_YAW_RATE_DEFAULT; parseRcChannels("TAER1234"); *primaryMotorMixerMutable(0) = (motorMixer_t){ 1.0f, -0.414178f, 1.0f, -1.0f }; // REAR_R diff --git a/src/main/target/BETAFLIGHTF3/target.h b/src/main/target/BETAFLIGHTF3/target.h index 2f50dc866e..1c4cc3fe68 100755 --- a/src/main/target/BETAFLIGHTF3/target.h +++ b/src/main/target/BETAFLIGHTF3/target.h @@ -110,8 +110,8 @@ //#define SBUS_TELEMETRY_UART SERIAL_PORT_USART1 // XXX #define DEFAULT_FEATURES (FEATURE_TX_PROF_SEL | FEATURE_BLACKBOX | FEATURE_CURRENT_METER | FEATURE_TELEMETRY ) // XXX -#define USE_SPEKTRUM_BIND -#define BIND_PIN UART2_RX_PIN +//#define USE_SPEKTRUM_BIND +//#define BIND_PIN UART2_RX_PIN #define USE_SERIAL_4WAY_BLHELI_INTERFACE diff --git a/src/main/target/CHEBUZZF3/target.h b/src/main/target/CHEBUZZF3/target.h index ad03b71bcb..eae3c8ebdd 100644 --- a/src/main/target/CHEBUZZF3/target.h +++ b/src/main/target/CHEBUZZF3/target.h @@ -39,6 +39,7 @@ #define L3GD20_CS_PIN PE3 #define IMU_L3GD20_ALIGN CW270_DEG +#define IMU_LSM303DLHC_ALIGN CW0_DEG #define IMU_MPU6050_ALIGN CW0_DEG #define USE_BARO diff --git a/src/main/target/KFC32F3_INAV/CMakeLists.txt b/src/main/target/KFC32F3_INAV/CMakeLists.txt deleted file mode 100644 index 1af4cecc06..0000000000 --- a/src/main/target/KFC32F3_INAV/CMakeLists.txt +++ /dev/null @@ -1 +0,0 @@ -target_stm32f303xc(KFC32F3_INAV SKIP_RELEASES) diff --git a/src/main/target/KFC32F3_INAV/hardware_setup.c b/src/main/target/KFC32F3_INAV/hardware_setup.c deleted file mode 100755 index c12e7dfde1..0000000000 --- a/src/main/target/KFC32F3_INAV/hardware_setup.c +++ /dev/null @@ -1,53 +0,0 @@ -/* - * This file is part of Cleanflight. - * - * Cleanflight is free software: you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation, either version 3 of the License, or - * (at your option) any later version. - * - * Cleanflight is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with Cleanflight. If not, see . - */ - -#include -#include -#include - -#include "platform.h" - -#include "build/build_config.h" - -#include "drivers/time.h" -#include "drivers/bus_spi.h" -#include "drivers/io.h" -#include "drivers/io_impl.h" - -void initialisePreBootHardware(void) -{ - // setup CS pins - IOInit(DEFIO_IO(PB5), OWNER_SYSTEM, RESOURCE_OUTPUT, 0); - IOConfigGPIO(DEFIO_IO(PB5), IOCFG_OUT_PP); - IOHi(DEFIO_IO(PB5)); - - IOInit(DEFIO_IO(PC15), OWNER_SYSTEM, RESOURCE_OUTPUT, 0); - IOConfigGPIO(DEFIO_IO(PC15), IOCFG_OUT_PP); - IOHi(DEFIO_IO(PC15)); - - IOInit(DEFIO_IO(PB12), OWNER_SYSTEM, RESOURCE_OUTPUT, 0); - IOConfigGPIO(DEFIO_IO(PB12), IOCFG_OUT_PP); - IOHi(DEFIO_IO(PB12)); - - IOInit(DEFIO_IO(PB2), OWNER_SYSTEM, RESOURCE_OUTPUT, 0); - IOConfigGPIO(DEFIO_IO(PB2), IOCFG_OUT_PP); - IOHi(DEFIO_IO(PB2)); - - IOInit(DEFIO_IO(PA7), OWNER_SYSTEM, RESOURCE_OUTPUT, 0); - IOConfigGPIO(DEFIO_IO(PA7), IOCFG_OUT_PP); - IOHi(DEFIO_IO(PA7)); -} \ No newline at end of file diff --git a/src/main/target/KFC32F3_INAV/target.c b/src/main/target/KFC32F3_INAV/target.c deleted file mode 100755 index a32e395332..0000000000 --- a/src/main/target/KFC32F3_INAV/target.c +++ /dev/null @@ -1,42 +0,0 @@ -/* - * This file is part of Cleanflight. - * - * Cleanflight is free software: you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation, either version 3 of the License, or - * (at your option) any later version. - * - * Cleanflight is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with Cleanflight. If not, see . - */ - -#include - -#include -#include "drivers/io.h" -#include "drivers/pwm_mapping.h" -#include "drivers/timer.h" - -const timerHardware_t timerHardware[] = -{ - DEF_TIM(TIM2, CH1, PA0, TIM_USE_PPM, 0), // PPM - - DEF_TIM(TIM8, CH3, PB9, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0), // PWM1 - DEF_TIM(TIM3, CH3, PB0, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0), // PWM2 - DEF_TIM(TIM3, CH4, PB1, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0), // PWM3 - DEF_TIM(TIM4, CH2, PB7, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0), // PWM4 - DEF_TIM(TIM4, CH3, PB8, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0), // PWM5 - DEF_TIM(TIM4, CH1, PB6, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0), // PWM6 - DEF_TIM(TIM2, CH3, PB10, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0), // PWM7 - DEF_TIM(TIM2, CH4, PB11, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0), // PWM8 - - DEF_TIM(TIM1, CH1, PA8, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0), // S1_out - DEF_TIM(TIM2, CH2, PA1, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0), // S2_out -}; - -const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]); diff --git a/src/main/target/KFC32F3_INAV/target.h b/src/main/target/KFC32F3_INAV/target.h deleted file mode 100755 index 5d3dacf3ba..0000000000 --- a/src/main/target/KFC32F3_INAV/target.h +++ /dev/null @@ -1,146 +0,0 @@ -/* - * This file is part of Cleanflight. - * - * Cleanflight is free software: you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation, either version 3 of the License, or - * (at your option) any later version. - * - * Cleanflight is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with Cleanflight. If not, see . - */ - -#pragma once - -#define TARGET_BOARD_IDENTIFIER "KFCi" -#define USE_HARDWARE_PREBOOT_SETUP - -#define USE_DJI_HD_OSD - -#define LED0 PC13 -#define LED0_INVERTED - -#define LED1 PC14 -#define LED1_INVERTED - -#define BEEPER PA13 -#define BEEPER_INVERTED - -#define USE_EXTI -#define GYRO_INT_EXTI PC5 -#define USE_MPU_DATA_READY_SIGNAL -#define ENSURE_MPU_DATA_READY_IS_LOW - -#define BUS_SPI_SPEED_MAX BUS_SPEED_SLOW - -#define USE_IMU_MPU6000 -#define IMU_MPU6000_ALIGN CW90_DEG -#define MPU6000_CS_PIN PB5 -#define MPU6000_SPI_BUS BUS_SPI2 - - -#define USE_MAG -#define MAG_I2C_BUS BUS_I2C1 -#define USE_MAG_HMC5883 -#define USE_MAG_QMC5883 -#define USE_MAG_IST8310 -#define USE_MAG_IST8308 -#define USE_MAG_MAG3110 -#define USE_MAG_LIS3MDL - -#define USE_SPI -#define USE_SPI_DEVICE_2 // PB12,13,14,15 on AF5 - -#define SPI2_NSS_PIN PB12 -#define SPI2_SCK_PIN PB13 -#define SPI2_MISO_PIN PB14 -#define SPI2_MOSI_PIN PB15 - -#define USE_FLASHFS - -#define USE_FLASH_M25P16 -#define M25P16_CS_PIN PB12 -#define M25P16_SPI_BUS BUS_SPI2 -#define ENABLE_BLACKBOX_LOGGING_ON_SPIFLASH_BY_DEFAULT - -#define USE_BARO -#define USE_BARO_MS5611 -#define MS5611_SPI_BUS BUS_SPI2 -#define MS5611_CS_PIN PB2 - -#define USE_OSD -#define USE_MAX7456 -#define MAX7456_SPI_BUS BUS_SPI2 -#define MAX7456_CS_PIN PA7 - -//#define RFM_SPI SPI2 -//#define RFM_SPI_CS_PIN PC15 -//#define RFM_IRQ_PIN PB3 -//#define RFM_SPI_CLK (SPI_CLOCK_STANDARD*2) -//#define RFM_RESTORE_CLK (SPI_CLOCK_FAST) - -#define USE_RX_SPI -#define USE_RX_ELERES -#define RX_NSS_PIN PC15 -#define RX_NSS_GPIO_CLK_PERIPHERAL RCC_APB2Periph_GPIOC -#define RX_SCK_PIN PB13 -#define RX_MOSI_PIN PB15 -#define RX_MISO_PIN PB14 -#define RX_SPI_INSTANCE SPI2 -#define RX_IRQ_PIN PB3 - -#define USE_VCP -#define USE_UART1 -#define USE_UART2 -#define SERIAL_PORT_COUNT 3 - -#define UART1_TX_PIN PA9 -#define UART1_RX_PIN PA10 - -#define UART2_TX_PIN PA2 -#define UART2_RX_PIN PA3 - -#define USE_I2C -#define USE_I2C_DEVICE_1 - -#define I2C1_SCL PA15 -#define I2C1_SDA PA14 - -#define USE_PITOT_MS4525 -#define PITOT_I2C_BUS BUS_I2C1 - -#define BOARD_HAS_VOLTAGE_DIVIDER -#define USE_ADC -#define ADC_INSTANCE ADC2 -#define ADC_CHANNEL_1_PIN PA4 -#define ADC_CHANNEL_2_PIN PA6 -#define ADC_CHANNEL_3_PIN PA5 -#define VBAT_ADC_CHANNEL ADC_CHN_1 -#define RSSI_ADC_CHANNEL ADC_CHN_2 -#define CURRENT_METER_ADC_CHANNEL ADC_CHN_3 - -//#define USE_LED_STRIP -//#define WS2811_PIN PA8 -//#define WS2811_DMA_STREAM DMA1_Channel2 -//#define WS2811_DMA_HANDLER_IDENTIFER DMA1_CH2_HANDLER - -#define DEFAULT_FEATURES (FEATURE_TX_PROF_SEL | FEATURE_BLACKBOX | FEATURE_OSD | FEATURE_VBAT) -#define DEFAULT_RX_TYPE RX_TYPE_PPM - -#define USE_SERIAL_4WAY_BLHELI_INTERFACE - -// Number of available PWM outputs -#define MAX_PWM_OUTPUT_PORTS 10 -#define TARGET_MOTOR_COUNT 10 - -// IO - stm32f303cc in 48pin package -#define TARGET_IO_PORTA 0xffff -#define TARGET_IO_PORTB 0xffff -#define TARGET_IO_PORTC 0xffff -#define TARGET_IO_PORTD 0xffff -#define TARGET_IO_PORTF (BIT(4)) diff --git a/src/main/target/OMNIBUS/target.h b/src/main/target/OMNIBUS/target.h index 3a222d8acc..3338db998e 100644 --- a/src/main/target/OMNIBUS/target.h +++ b/src/main/target/OMNIBUS/target.h @@ -119,8 +119,8 @@ #define CURRENT_METER_ADC_CHANNEL ADC_CHN_2 #define RSSI_ADC_CHANNEL ADC_CHN_3 -#define USE_LED_STRIP -#define WS2811_PIN PA8 +//#define USE_LED_STRIP +//#define WS2811_PIN PA8 //#define ENABLE_BLACKBOX_LOGGING_ON_SDCARD_BY_DEFAULT diff --git a/src/main/target/PIKOBLX/target.h b/src/main/target/PIKOBLX/target.h index c60d00116b..9a68d246b1 100644 --- a/src/main/target/PIKOBLX/target.h +++ b/src/main/target/PIKOBLX/target.h @@ -72,8 +72,8 @@ #define VBAT_ADC_CHANNEL ADC_CHN_2 #define RSSI_ADC_CHANNEL ADC_CHN_3 -#define USE_LED_STRIP -#define WS2811_PIN PB8 // TIM16_CH1 +//#define USE_LED_STRIP +//#define WS2811_PIN PB8 // TIM16_CH1 // #define USE_TRANSPONDER // #define TRANSPONDER_PIN PA8 diff --git a/src/main/target/RMDO/CMakeLists.txt b/src/main/target/RMDO/CMakeLists.txt deleted file mode 100644 index ed40102c52..0000000000 --- a/src/main/target/RMDO/CMakeLists.txt +++ /dev/null @@ -1 +0,0 @@ -target_stm32f303xc(RMDO COMPILE_DEFINITIONS "SPRACINGF3" SKIP_RELEASES) diff --git a/src/main/target/RMDO/target.c b/src/main/target/RMDO/target.c deleted file mode 100644 index 59769ba505..0000000000 --- a/src/main/target/RMDO/target.c +++ /dev/null @@ -1,48 +0,0 @@ -/* - * This file is part of Cleanflight. - * - * Cleanflight is free software: you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation, either version 3 of the License, or - * (at your option) any later version. - * - * Cleanflight is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with Cleanflight. If not, see . - */ - -#include - -#include -#include "drivers/io.h" -#include "drivers/pwm_mapping.h" -#include "drivers/timer.h" - -const timerHardware_t timerHardware[] = { - DEF_TIM(TIM16, CH1, PA6, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0), // PWM1 - PA6 - TIM3_CH1, TIM8_BKIN, TIM1_BKIN, *TIM16_CH1 - DEF_TIM(TIM17, CH1, PA7, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0), // PWM2 - PA7 - TIM3_CH2, *TIM17_CH1, TIM1_CH1N, TIM8_CH1 - DEF_TIM(TIM4, CH1, PA11, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0), // PWM3 - PA11 - DEF_TIM(TIM4, CH2, PA12, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0), // PWM4 - PA12 - DEF_TIM(TIM4, CH3, PB8, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0), // PWM5 - PB8 - DEF_TIM(TIM4, CH4, PB9, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0), // PWM6 - PB9 - DEF_TIM(TIM15, CH1, PA2, TIM_USE_MC_MOTOR | TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0), // PWM7 - PA2 - DEF_TIM(TIM15, CH2, PA3, TIM_USE_MC_MOTOR | TIM_USE_MC_SERVO | TIM_USE_FW_SERVO, 0), // PWM8 - PA3 - - DEF_TIM(TIM1, CH1, PA8, TIM_USE_LED, 0), // GPIO_TIMER / LED_STRIP - - DEF_TIM(TIM2, CH1, PA0, TIM_USE_PWM | TIM_USE_PPM, 0), // RC_CH1 - PA0 - *TIM2_CH1 - DEF_TIM(TIM2, CH2, PA1, TIM_USE_PWM, 0), // RC_CH2 - PA1 - *TIM2_CH2, TIM15_CH1N - DEF_TIM(TIM2, CH4, PB11, TIM_USE_PWM, 0), // RC_CH3 - PB11 - *TIM2_CH4, UART3_RX (AF7) - DEF_TIM(TIM2, CH3, PB10, TIM_USE_PWM, 0), // RC_CH4 - PB10 - *TIM2_CH3, UART3_TX (AF7) - DEF_TIM(TIM3, CH1, PB4, TIM_USE_PWM, 0), // RC_CH5 - PB4 - *TIM3_CH1 - DEF_TIM(TIM3, CH2, PB5, TIM_USE_PWM, 0), // RC_CH6 - PB5 - *TIM3_CH2 - DEF_TIM(TIM3, CH3, PB0, TIM_USE_PWM, 0), // RC_CH7 - PB0 - *TIM3_CH3, TIM1_CH2N, TIM8_CH2N - DEF_TIM(TIM3, CH4, PB1, TIM_USE_PWM, 0), // RC_CH8 - PB1 - *TIM3_CH4, TIM1_CH3N, TIM8_CH3N -}; - - -const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]); diff --git a/src/main/target/RMDO/target.h b/src/main/target/RMDO/target.h deleted file mode 100644 index fe467f8296..0000000000 --- a/src/main/target/RMDO/target.h +++ /dev/null @@ -1,125 +0,0 @@ -/* - * This file is part of Cleanflight. - * - * Cleanflight is free software: you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation, either version 3 of the License, or - * (at your option) any later version. - * - * Cleanflight is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with Cleanflight. If not, see . - */ - -#pragma once - -#define TARGET_BOARD_IDENTIFIER "RMDO" // Ready Made RC DoDo - -#define LED0 PB3 - -#define BEEPER PC15 -#define BEEPER_INVERTED - -#define USE_EXTI -#define USE_MPU_DATA_READY_SIGNAL -#define ENSURE_MPU_DATA_READY_IS_LOW - -#define USE_IMU_MPU6050 -#define IMU_MPU6050_ALIGN CW270_DEG -#define MPU6050_I2C_BUS BUS_I2C1 - - -#define USE_BARO -#define BARO_I2C_BUS BUS_I2C1 -#define USE_BARO_BMP280 - -#define USE_MAG -#define MAG_I2C_BUS BUS_I2C1 -#define USE_MAG_AK8975 -#define USE_MAG_HMC5883 -#define USE_MAG_QMC5883 -#define USE_MAG_IST8310 -#define USE_MAG_IST8308 -#define USE_MAG_MAG3110 -#define USE_MAG_LIS3MDL - -#define USE_FLASHFS -#define USE_FLASH_M25P16 - -#define USE_RANGEFINDER -#define USE_RANGEFINDER_HCSR04 -#define RANGEFINDER_HCSR04_TRIGGER_PIN PB0 // RC_CH7 (PB0) - only 3.3v ( add a 1K Ohms resistor ) -#define RANGEFINDER_HCSR04_ECHO_PIN PB1 // RC_CH8 (PB1) - only 3.3v ( add a 1K Ohms resistor ) - -#define USE_UART1 -#define USE_UART2 -#define USE_UART3 -#define USE_SOFTSERIAL1 -#define USE_SOFTSERIAL2 -#define SERIAL_PORT_COUNT 5 - -#define UART1_TX_PIN PA9 -#define UART1_RX_PIN PA10 - -#define UART2_TX_PIN PA14 // PA14 / SWCLK -#define UART2_RX_PIN PA15 // PA15 - -#define UART3_TX_PIN PB10 // PB10 (AF7) -#define UART3_RX_PIN PB11 // PB11 (AF7) - -#define SOFTSERIAL_1_RX_PIN PB4 -#define SOFTSERIAL_1_TX_PIN PB5 -#define SOFTSERIAL_2_RX_PIN PB0 -#define SOFTSERIAL_2_TX_PIN PB1 - -#define USE_I2C -#define USE_I2C_DEVICE_1 - -#define USE_SPI -#define USE_SPI_DEVICE_2 // PB12,13,14,15 on AF5 - -#define M25P16_CS_GPIO GPIOB -#define M25P16_CS_PIN PB12 -#define M25P16_SPI_BUS BUS_SPI2 - -#define BOARD_HAS_VOLTAGE_DIVIDER -#define USE_ADC -#define ADC_INSTANCE ADC2 -#define ADC_CHANNEL_1_PIN PA4 -#define ADC_CHANNEL_2_PIN PA5 -#define ADC_CHANNEL_3_PIN PB2 -#define VBAT_ADC_CHANNEL ADC_CHN_1 -#define CURRENT_METER_ADC_CHANNEL ADC_CHN_2 -#define RSSI_ADC_CHANNEL ADC_CHN_3 - -#define USE_LED_STRIP - -#define USE_LED_STRIP_ON_DMA1_CHANNEL2 -#define WS2811_PIN PA8 - -#undef NAV_MAX_WAYPOINTS -#define NAV_MAX_WAYPOINTS 30 - -#define USE_SPEKTRUM_BIND -// USART3, -#define BIND_PIN PB11 - -#define USE_SERIAL_4WAY_BLHELI_INTERFACE - -#undef USE_GPS_PROTO_NAZA -#undef USE_TELEMETRY_HOTT -#undef USE_TELEMETRY_SMARTPORT -#undef USE_TELEMETRY_IBUS - -// Number of available PWM outputs -#define MAX_PWM_OUTPUT_PORTS 12 - -// IO - stm32f303cc in 48pin package -#define TARGET_IO_PORTA 0xffff -#define TARGET_IO_PORTB 0xffff -#define TARGET_IO_PORTC (BIT(13)|BIT(14)|BIT(15)) -#define TARGET_IO_PORTF (BIT(0)|BIT(1)|BIT(4)) diff --git a/src/main/target/SPRACINGF3NEO/CMakeLists.txt b/src/main/target/SPRACINGF3NEO/CMakeLists.txt deleted file mode 100644 index d93726a632..0000000000 --- a/src/main/target/SPRACINGF3NEO/CMakeLists.txt +++ /dev/null @@ -1 +0,0 @@ -target_stm32f303xc(SPRACINGF3NEO SKIP_RELEASES) diff --git a/src/main/target/SPRACINGF3NEO/config.c b/src/main/target/SPRACINGF3NEO/config.c deleted file mode 100644 index 922e391a27..0000000000 --- a/src/main/target/SPRACINGF3NEO/config.c +++ /dev/null @@ -1,39 +0,0 @@ -/* - * This file is part of Cleanflight. - * - * Cleanflight is free software: you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation, either version 3 of the License, or - * (at your option) any later version. - * - * Cleanflight is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with Cleanflight. If not, see . - */ - -#include -#include - -#include "platform.h" - -#include "io/serial.h" - -#include "sensors/sensors.h" -#include "sensors/compass.h" -#include "sensors/barometer.h" - -#include "telemetry/telemetry.h" - -void targetConfiguration(void) -{ - barometerConfigMutable()->baro_hardware = BARO_AUTODETECT; - compassConfigMutable()->mag_hardware = MAG_AUTODETECT; - serialConfigMutable()->portConfigs[1].functionMask = FUNCTION_MSP; // So Bluetooth users don't have to change anything. - serialConfigMutable()->portConfigs[findSerialPortIndexByIdentifier(TELEMETRY_UART)].functionMask = FUNCTION_TELEMETRY_SMARTPORT; - serialConfigMutable()->portConfigs[findSerialPortIndexByIdentifier(GPS_UART)].functionMask = FUNCTION_GPS; - //telemetryConfigMutable()->halfDuplex = 1; -} diff --git a/src/main/target/SPRACINGF3NEO/target.c b/src/main/target/SPRACINGF3NEO/target.c deleted file mode 100755 index 6a932ead1b..0000000000 --- a/src/main/target/SPRACINGF3NEO/target.c +++ /dev/null @@ -1,46 +0,0 @@ -/* - * This file is part of Cleanflight. - * - * Cleanflight is free software: you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation, either version 3 of the License, or - * (at your option) any later version. - * - * Cleanflight is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with Cleanflight. If not, see . - */ - -#include - -#include - -#include "drivers/io.h" -#include "drivers/pwm_mapping.h" -#include "drivers/timer.h" - -const timerHardware_t timerHardware[] = { - DEF_TIM(TIM15, CH2, PA3, TIM_USE_PWM | TIM_USE_PPM, 0), // PWM1 / PPM / UART2 RX - DEF_TIM(TIM15, CH1, PA2, TIM_USE_PWM, 0), // PWM2 - - DEF_TIM(TIM3, CH4, PB1, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0), // ESC1 - DEF_TIM(TIM3, CH2, PC7, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0), // ESC2 - DEF_TIM(TIM3, CH3, PB0, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0), // ESC3 - DEF_TIM(TIM3, CH1, PC6, TIM_USE_MC_MOTOR | TIM_USE_FW_SERVO, 0), // ESC4 - - DEF_TIM(TIM8, CH3, PC8, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0), // ESC5 (FW motor) - DEF_TIM(TIM8, CH4, PC9, TIM_USE_MC_MOTOR | TIM_USE_FW_MOTOR, 0), // ESC6 (FW motor) - - - DEF_TIM(TIM2, CH3, PB10, TIM_USE_PWM, 0), // PWM3 - PB10 - *TIM2_CH3, UART3_TX (AF7) - DEF_TIM(TIM2, CH4, PB11, TIM_USE_PWM, 0), // PWM4 - PB11 - *TIM2_CH4, UART3_RX (AF7) - - DEF_TIM(TIM16, CH1, PB8, TIM_USE_ANY, 0), - DEF_TIM(TIM1, CH1, PA8, TIM_USE_LED, 0), -}; - -const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]); diff --git a/src/main/target/SPRACINGF3NEO/target.h b/src/main/target/SPRACINGF3NEO/target.h deleted file mode 100755 index 9bec2fda3c..0000000000 --- a/src/main/target/SPRACINGF3NEO/target.h +++ /dev/null @@ -1,163 +0,0 @@ -/* - * This file is part of Cleanflight. - * - * Cleanflight is free software: you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation, either version 3 of the License, or - * (at your option) any later version. - * - * Cleanflight is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with Cleanflight. If not, see . - */ - -#pragma once - -#define TARGET_BOARD_IDENTIFIER "SP3N" - -#define LED0 PB9 -#define LED1 PB2 - -#define BEEPER PC15 -#define BEEPER_INVERTED - -#define USE_EXTI -#define GYRO_INT_EXTI PC13 -#define USE_MPU_DATA_READY_SIGNAL -#define ENSURE_MPU_DATA_READY_IS_LOW - -#define USE_IMU_MPU6500 -#define IMU_MPU6500_ALIGN CW0_DEG - -#define USE_IMU_MPU9250 -#define IMU_MPU9250_ALIGN CW0_DEG - -#define MPU6500_CS_PIN SPI1_NSS_PIN -#define MPU6500_SPI_BUS BUS_SPI1 - -#define MPU9250_CS_PIN SPI1_NSS_PIN -#define MPU9250_SPI_BUS BUS_SPI1 - -#define USE_BARO -#define BARO_I2C_BUS BUS_I2C1 -#define USE_BARO_BMP280 -#define USE_BARO_MS5611 - -#define USE_MAG -#define MAG_I2C_BUS BUS_I2C1 -#define USE_MAG_MPU9250 -#define USE_MAG_HMC5883 -#define USE_MAG_QMC5883 -#define USE_MAG_IST8310 -#define USE_MAG_IST8308 -#define USE_MAG_MAG3110 -#define USE_MAG_LIS3MDL - -#define USE_VCP -#define USE_UART1 -#define USE_UART2 -#define USE_UART3 -#define USE_UART4 -#define USE_UART5 -//#define USE_SOFTSERIAL1 -//#define USE_SOFTSERIAL2 -//#define SERIAL_PORT_COUNT 8 -#define SERIAL_PORT_COUNT 6 - -#define UART1_TX_PIN PA9 -#define UART1_RX_PIN PA10 - -#define UART2_TX_PIN PA2 -#define UART2_RX_PIN PA3 - -#define UART3_TX_PIN PB10 -#define UART3_RX_PIN PB11 - -#define USE_I2C -#define USE_I2C_DEVICE_1 - -#define USE_SPI -#define USE_SPI_DEVICE_1 // MPU -#define USE_SPI_DEVICE_2 // SDCard -#define USE_SPI_DEVICE_3 // External (MAX7456 & RTC6705) - -#define SPI1_NSS_PIN PA4 -#define SPI1_SCK_PIN PA5 -#define SPI1_MISO_PIN PA6 -#define SPI1_MOSI_PIN PA7 - -#define SPI2_NSS_PIN PB12 -#define SPI2_SCK_PIN PB13 -#define SPI2_MISO_PIN PB14 -#define SPI2_MOSI_PIN PB15 - -#define SPI3_NSS_PIN PA15 -#define SPI3_SCK_PIN PB3 -#define SPI3_MISO_PIN PB4 -#define SPI3_MOSI_PIN PB5 - -#undef USE_VTX_FFPV -#undef USE_VTX_SMARTAUDIO // Disabled due to flash size -#undef USE_VTX_TRAMP // Disabled due to flash size - -#undef USE_PITOT // Disabled due to RAM size -#undef USE_PITOT_ADC // Disabled due to RAM size - -#define USE_MAX7456 -#define MAX7456_SPI_BUS BUS_SPI3 -#define MAX7456_CS_PIN PA15 - -#define USE_SDCARD -#define USE_SDCARD_SPI -#define SDCARD_DETECT_INVERTED -#define SDCARD_DETECT_PIN PC14 -#define SDCARD_SPI_BUS BUS_SPI2 -#define SDCARD_CS_PIN SPI2_NSS_PIN - -#define USE_ADC -#define ADC_INSTANCE ADC1 -#define ADC_CHANNEL_1_PIN PC1 -#define ADC_CHANNEL_2_PIN PC2 -#define ADC_CHANNEL_3_PIN PC0 -#define VBAT_ADC_CHANNEL ADC_CHN_1 -#define CURRENT_METER_ADC_CHANNEL ADC_CHN_2 -#define RSSI_ADC_CHANNEL ADC_CHN_3 - -#define USE_LED_STRIP -#define WS2811_PIN PA8 - -#define ENABLE_BLACKBOX_LOGGING_ON_SDCARD_BY_DEFAULT - -#define USE_OSD - -#define DEFAULT_RX_TYPE RX_TYPE_SERIAL -#define DEFAULT_FEATURES (FEATURE_TX_PROF_SEL | FEATURE_TRANSPONDER | FEATURE_RSSI_ADC | FEATURE_TELEMETRY | FEATURE_OSD | FEATURE_LED_STRIP) -#define SERIALRX_UART SERIAL_PORT_USART2 -#define GPS_UART SERIAL_PORT_USART3 -#define TELEMETRY_UART SERIAL_PORT_USART5 -#define SERIALRX_PROVIDER SERIALRX_SBUS - -#define BUTTONS -#define BUTTON_A_PIN PD2 - -#define SPEKTRUM_BIND_PIN UART2_RX_PIN - -#define BINDPLUG_PIN PD2 - -#define USE_SERIAL_4WAY_BLHELI_INTERFACE - -// Number of available PWM outputs -#define MAX_PWM_OUTPUT_PORTS 10 - -// IO - assuming 303 in 64pin package, TODO -#define TARGET_IO_PORTA 0xffff -#define TARGET_IO_PORTB 0xffff -#define TARGET_IO_PORTC 0xffff -#define TARGET_IO_PORTD (BIT(2)) -#define TARGET_IO_PORTF (BIT(0)|BIT(1)|BIT(4)) - -#undef USE_TELEMETRY_FRSKY diff --git a/src/main/target/common.h b/src/main/target/common.h index abe10db98a..34c1ca9110 100755 --- a/src/main/target/common.h +++ b/src/main/target/common.h @@ -124,7 +124,6 @@ #define USE_PWM_DRIVER_PCA9685 -#define USE_TELEMETRY_SIM #define USE_FRSKYOSD #define USE_DJI_HD_OSD #define USE_SMARTPORT_MASTER @@ -138,7 +137,19 @@ #define USE_I2C_IO_EXPANDER +#define USE_GPS_PROTO_NMEA +#define USE_GPS_PROTO_MTK + +#define USE_TELEMETRY_SIM +#define USE_TELEMETRY_HOTT +#define USE_TELEMETRY_MAVLINK +#define USE_MSP_OVER_TELEMETRY + #define USE_SERIALRX_SRXL2 // Spektrum SRXL2 protocol +#define USE_SERIALRX_SUMD +#define USE_SERIALRX_SUMH +#define USE_SERIALRX_XBUS +#define USE_SERIALRX_JETIEXBUS #define USE_TELEMETRY_SRXL #define USE_SPEKTRUM_CMS_TELEMETRY //#define USE_SPEKTRUM_VTX_CONTROL //Some functions from betaflight still not implemented @@ -164,23 +175,14 @@ #define USE_STATS #define USE_CMS #define CMS_MENU_OSD -#define USE_GPS_PROTO_NMEA -#define USE_GPS_PROTO_MTK #define NAV_GPS_GLITCH_DETECTION #define NAV_NON_VOLATILE_WAYPOINT_STORAGE -#define USE_TELEMETRY_HOTT #define USE_TELEMETRY_IBUS -#define USE_TELEMETRY_MAVLINK #define USE_TELEMETRY_SMARTPORT #define USE_TELEMETRY_CRSF -#define USE_MSP_OVER_TELEMETRY // These are rather exotic serial protocols #define USE_RX_MSP //#define USE_MSP_RC_OVERRIDE -#define USE_SERIALRX_SUMD -#define USE_SERIALRX_SUMH -#define USE_SERIALRX_XBUS -#define USE_SERIALRX_JETIEXBUS #define USE_SERIALRX_CRSF #define USE_PWM_SERVO_DRIVER #define USE_SERIAL_PASSTHROUGH diff --git a/src/main/telemetry/sim.c b/src/main/telemetry/sim.c index 8fe6b7c53c..1d4ef3fdd9 100644 --- a/src/main/telemetry/sim.c +++ b/src/main/telemetry/sim.c @@ -63,15 +63,6 @@ #define SIM_RESPONSE_CODE_CMT ('C' << 24 | 'M' << 16 | 'T' << 8 | ':') -typedef enum { - SIM_TX_FLAG = (1 << 0), - SIM_TX_FLAG_FAILSAFE = (1 << 1), - SIM_TX_FLAG_GPS = (1 << 2), - SIM_TX_FLAG_ACC = (1 << 3), - SIM_TX_FLAG_LOW_ALT = (1 << 4), - SIM_TX_FLAG_RESPONSE = (1 << 5) -} simTxFlags_e; - typedef enum { SIM_MODULE_NOT_DETECTED = 0, SIM_MODULE_NOT_REGISTERED, @@ -124,7 +115,6 @@ static uint8_t simResponse[SIM_RESPONSE_BUFFER_SIZE + 1]; static int atCommandStatus = SIM_AT_OK; static bool simWaitAfterResponse = false; static uint8_t readState = SIM_READSTATE_RESPONSE; -static uint8_t transmitFlags = 0; static timeMs_t t_lastMessageSent = 0; static uint8_t lastMessageTriggeredBy = 0; static uint8_t simModuleState = SIM_MODULE_NOT_DETECTED; @@ -146,7 +136,7 @@ static bool isGroundStationNumberDefined(void) { static bool checkGroundStationNumber(uint8_t* rv) { int i; - const uint8_t* gsn = telemetryConfig()->simGroundStationNumber; + const char* gsn = telemetryConfig()->simGroundStationNumber; int digitsToCheck = strlen((char*)gsn); if (gsn[0] == '+') { @@ -177,7 +167,7 @@ static bool checkGroundStationNumber(uint8_t* rv) static void readOriginatingNumber(uint8_t* rv) { int i; - uint8_t* gsn = telemetryConfigMutable()->simGroundStationNumber; + char* gsn = telemetryConfigMutable()->simGroundStationNumber; if (gsn[0] != '\0') return; for (i = 0; i < 15 && rv[i] != '\"'; i++) @@ -189,7 +179,7 @@ static void readTransmitFlags(const uint8_t* fs) { int i; - transmitFlags = 0; + uint8_t transmitFlags = 0; for (i = 0; i < SIM_N_TX_FLAGS && fs[i] != '\0'; i++) { switch (fs[i]) { case 'T': case 't': @@ -209,6 +199,8 @@ static void readTransmitFlags(const uint8_t* fs) break; } } + + telemetryConfigMutable()->simTransmitFlags = transmitFlags; } static void requestSendSMS(uint8_t trigger) @@ -337,7 +329,7 @@ static void transmit(void) if (gpsSol.fixType != GPS_NO_FIX && FLIGHT_MODE(SIM_LOW_ALT_WARNING_MODES) && getAltitudeMeters() < telemetryConfig()->simLowAltitude) triggers |= SIM_TX_FLAG_LOW_ALT; - triggers &= transmitFlags; + triggers &= telemetryConfig()->simTransmitFlags; if (!triggers) return; @@ -508,7 +500,6 @@ static void configureSimTelemetryPort(void) sim_t_stateChange = millis() + SIM_STARTUP_DELAY_MS; simTelemetryState = SIM_STATE_INIT; readState = SIM_READSTATE_RESPONSE; - readTransmitFlags(telemetryConfig()->simTransmitFlags); simEnabled = true; } diff --git a/src/main/telemetry/sim.h b/src/main/telemetry/sim.h index 081f74e036..299981e623 100644 --- a/src/main/telemetry/sim.h +++ b/src/main/telemetry/sim.h @@ -23,6 +23,15 @@ #define SIM_DEFAULT_TX_FLAGS "f" #define SIM_PIN "0000" +typedef enum { + SIM_TX_FLAG = (1 << 0), + SIM_TX_FLAG_FAILSAFE = (1 << 1), + SIM_TX_FLAG_GPS = (1 << 2), + SIM_TX_FLAG_ACC = (1 << 3), + SIM_TX_FLAG_LOW_ALT = (1 << 4), + SIM_TX_FLAG_RESPONSE = (1 << 5) +} simTxFlags_e; + void handleSimTelemetry(void); void initSimTelemetry(void); diff --git a/src/main/telemetry/telemetry.c b/src/main/telemetry/telemetry.c index 98531e65e3..6246c3f47f 100644 --- a/src/main/telemetry/telemetry.c +++ b/src/main/telemetry/telemetry.c @@ -34,6 +34,7 @@ #include "fc/rc_controls.h" #include "fc/rc_modes.h" #include "fc/runtime_config.h" +#include "fc/settings.h" #include "io/serial.h" @@ -53,38 +54,43 @@ #include "telemetry/ghst.h" -PG_REGISTER_WITH_RESET_TEMPLATE(telemetryConfig_t, telemetryConfig, PG_TELEMETRY_CONFIG, 4); +PG_REGISTER_WITH_RESET_TEMPLATE(telemetryConfig_t, telemetryConfig, PG_TELEMETRY_CONFIG, 5); PG_RESET_TEMPLATE(telemetryConfig_t, telemetryConfig, - .gpsNoFixLatitude = 0, - .gpsNoFixLongitude = 0, - .telemetry_switch = 0, - .telemetry_inverted = 0, - .frsky_coordinate_format = FRSKY_FORMAT_DMS, - .frsky_unit = FRSKY_UNIT_METRICS, - .frsky_vfas_precision = 0, - .frsky_pitch_roll = 0, - .report_cell_voltage = 0, - .hottAlarmSoundInterval = 5, - .halfDuplex = 1, - .smartportFuelUnit = SMARTPORT_FUEL_UNIT_MAH, - .ibusTelemetryType = 0, - .ltmUpdateRate = LTM_RATE_NORMAL, - .simTransmitInterval = SIM_DEFAULT_TRANSMIT_INTERVAL, - .simTransmitFlags = SIM_DEFAULT_TX_FLAGS, - .simLowAltitude = INT16_MIN, - .simPin = SIM_PIN, - .accEventThresholdHigh = 0, - .accEventThresholdLow = 0, - .accEventThresholdNegX = 0, + .gpsNoFixLatitude = SETTING_FRSKY_DEFAULT_LATITUDE_DEFAULT, + .gpsNoFixLongitude = SETTING_FRSKY_DEFAULT_LONGITUDE_DEFAULT, + .telemetry_switch = SETTING_TELEMETRY_SWITCH_DEFAULT, + .telemetry_inverted = SETTING_TELEMETRY_INVERTED_DEFAULT, + .frsky_coordinate_format = SETTING_FRSKY_COORDINATES_FORMAT_DEFAULT, + .frsky_unit = SETTING_FRSKY_UNIT_DEFAULT, + .frsky_vfas_precision = SETTING_FRSKY_VFAS_PRECISION_DEFAULT, + .frsky_pitch_roll = SETTING_FRSKY_PITCH_ROLL_DEFAULT, + .report_cell_voltage = SETTING_REPORT_CELL_VOLTAGE_DEFAULT, + .hottAlarmSoundInterval = SETTING_HOTT_ALARM_SOUND_INTERVAL_DEFAULT, + .halfDuplex = SETTING_TELEMETRY_HALFDUPLEX_DEFAULT, + .smartportFuelUnit = SETTING_SMARTPORT_FUEL_UNIT_DEFAULT, + .ibusTelemetryType = SETTING_IBUS_TELEMETRY_TYPE_DEFAULT, + .ltmUpdateRate = SETTING_LTM_UPDATE_RATE_DEFAULT, + +#ifdef USE_TELEMETRY_SIM + .simTransmitInterval = SETTING_SIM_TRANSMIT_INTERVAL_DEFAULT, + .simTransmitFlags = SETTING_SIM_TRANSMIT_FLAGS_DEFAULT, + .simLowAltitude = SETTING_SIM_LOW_ALTITUDE_DEFAULT, + .simPin = SETTING_SIM_PIN_DEFAULT, + .simGroundStationNumber = SETTING_SIM_GROUND_STATION_NUMBER_DEFAULT, + + .accEventThresholdHigh = SETTING_ACC_EVENT_THRESHOLD_HIGH_DEFAULT, + .accEventThresholdLow = SETTING_ACC_EVENT_THRESHOLD_LOW_DEFAULT, + .accEventThresholdNegX = SETTING_ACC_EVENT_THRESHOLD_NEG_X_DEFAULT, +#endif .mavlink = { - .extended_status_rate = 2, - .rc_channels_rate = 5, - .position_rate = 2, - .extra1_rate = 10, - .extra2_rate = 2, - .extra3_rate = 1 + .extended_status_rate = SETTING_MAVLINK_EXT_STATUS_RATE_DEFAULT, + .rc_channels_rate = SETTING_MAVLINK_RC_CHAN_RATE_DEFAULT, + .position_rate = SETTING_MAVLINK_POS_RATE_DEFAULT, + .extra1_rate = SETTING_MAVLINK_EXTRA1_RATE_DEFAULT, + .extra2_rate = SETTING_MAVLINK_EXTRA2_RATE_DEFAULT, + .extra3_rate = SETTING_MAVLINK_EXTRA3_RATE_DEFAULT } ); diff --git a/src/main/telemetry/telemetry.h b/src/main/telemetry/telemetry.h index 717e9fec03..242f5cc83e 100644 --- a/src/main/telemetry/telemetry.h +++ b/src/main/telemetry/telemetry.h @@ -68,14 +68,18 @@ typedef struct telemetryConfig_s { smartportFuelUnit_e smartportFuelUnit; uint8_t ibusTelemetryType; uint8_t ltmUpdateRate; + +#ifdef USE_TELEMETRY_SIM + int16_t simLowAltitude; + char simGroundStationNumber[16]; + char simPin[8]; uint16_t simTransmitInterval; - uint8_t simTransmitFlags[4]; + uint8_t simTransmitFlags; + uint16_t accEventThresholdHigh; uint16_t accEventThresholdLow; uint16_t accEventThresholdNegX; - int16_t simLowAltitude; - uint8_t simGroundStationNumber[16]; - uint8_t simPin[8]; +#endif struct { uint8_t extended_status_rate; uint8_t rc_channels_rate; @@ -98,4 +102,3 @@ void telemetryCheckState(void); void telemetryProcess(timeUs_t currentTimeUs); bool telemetryDetermineEnabledState(portSharing_e portSharing); - diff --git a/src/test/unit/CMakeLists.txt b/src/test/unit/CMakeLists.txt index d1aebcd5e0..1f6e7dcd00 100644 --- a/src/test/unit/CMakeLists.txt +++ b/src/test/unit/CMakeLists.txt @@ -43,9 +43,13 @@ function(unit_test src) endif() list(TRANSFORM deps PREPEND "${MAIN_DIR}/") add_executable(${name} ${src} ${deps}) - target_include_directories(${name} PRIVATE . ${MAIN_DIR}) + set(gen_name ${name}_gen) + get_generated_files_dir(gen ${gen_name}) + target_include_directories(${name} PRIVATE . ${MAIN_DIR} ${gen}) target_compile_definitions(${name} PRIVATE ${test_definitions}) - set_target_properties(${name} PROPERTIES COMPILE_FLAGS "-pthread -Wall -Wextra -Wno-extern-c-compat -ggdb3 -O0") + target_compile_options(${name} PRIVATE -pthread -Wall -Wextra -Wno-extern-c-compat -ggdb3 -O0) + enable_settings(${name} ${gen_name} OUTPUTS setting_files SETTINGS_CXX g++) + target_sources(${name} PRIVATE ${setting_files}) target_link_libraries(${name} gtest_main) gtest_discover_tests(${name}) add_custom_target("run-${name}" "${name}" DEPENDS ${name}) diff --git a/src/utils/compiler.rb b/src/utils/compiler.rb index 39f7d57184..dc44611426 100644 --- a/src/utils/compiler.rb +++ b/src/utils/compiler.rb @@ -36,7 +36,10 @@ class Compiler # on Windows if PATH contains spaces. #dirs = ((ENV["CPP_PATH"] || "") + File::PATH_SEPARATOR + (ENV["PATH"] || "")).split(File::PATH_SEPARATOR) dirs = ((ENV["CPP_PATH"] || "") + File::PATH_SEPARATOR + (ENV["PATH"] || "")).split(File::PATH_SEPARATOR) - bin = "arm-none-eabi-g++" + bin = ENV["SETTINGS_CXX"] + if bin.empty? + bin = "arm-none-eabi-g++" + end dirs.each do |dir| p = File.join(dir, bin) ['', '.exe'].each do |suffix| diff --git a/src/utils/settings.rb b/src/utils/settings.rb old mode 100644 new mode 100755 index 10deeab1c7..acae823773 --- a/src/utils/settings.rb +++ b/src/utils/settings.rb @@ -299,9 +299,11 @@ class Generator load_data + check_member_default_values_presence sanitize_fields initialize_name_encoder initialize_value_encoder + validate_default_values write_header_file(header_file) write_impl_file(impl_file) @@ -417,8 +419,33 @@ class Generator ii = 0 foreach_enabled_member do |group, member| name = member["name"] + type = member["type"] + default_value = member["default_value"] + + case + when %i[ zero target ].include?(default_value) + default_value = nil + + when member.has_key?("table") + table_name = member["table"] + table_values = @tables[table_name]["values"] + if table_name == 'off_on' and [false, true].include? default_value + default_value = { false => '0', true => '1' }[default_value] + else + default_value = table_values.index default_value + end + + when type == "string" + default_value = "{ #{[*default_value.bytes, 0] * ', '} }" + + when default_value.is_a?(Float) + default_value = default_value.to_s + ?f + + end + min, max = resolve_range(member) setting_name = "SETTING_#{name.upcase}" + buf << "#define #{setting_name}_DEFAULT #{default_value}\n" unless default_value.nil? buf << "#define #{setting_name} #{ii}\n" buf << "#define #{setting_name}_MIN #{min}\n" buf << "#define #{setting_name}_MAX #{max}\n" @@ -444,6 +471,12 @@ class Generator end end + # When this file is compiled in unit tests, some of the tables + # are not used and generate warnings, causing the test to fail + # with -Werror. Silence them + + buf << "#pragma GCC diagnostic ignored \"-Wunused-const-variable\"\n" + # Write PGN arrays pgn_steps = [] pgns = [] @@ -629,44 +662,47 @@ class Generator return !cond || @true_conditions.include?(cond) end - def foreach_enabled_member - @data["groups"].each do |group| - if is_condition_enabled(group["condition"]) - group["members"].each do |member| - if is_condition_enabled(member["condition"]) - yield group, member + def foreach_enabled_member &block + enum = Enumerator.new do |yielder| + groups.each do |group| + if is_condition_enabled(group["condition"]) + group["members"].each do |member| + if is_condition_enabled(member["condition"]) + yielder.yield group, member + end end end end end + block_given? ? enum.each(&block) : enum end - def foreach_enabled_group - last = nil - foreach_enabled_member do |group, member| - if last != group - last = group - yield group + def foreach_enabled_group &block + enum = Enumerator.new do |yielder| + last = nil + foreach_enabled_member do |group, member| + if last != group + last = group + yielder.yield group + end end end + block_given? ? enum.each(&block) : enum end - def foreach_member - @data["groups"].each do |group| - group["members"].each do |member| - yield group, member + def foreach_member &block + enum = Enumerator.new do |yielder| + @data["groups"].each do |group| + group["members"].each do |member| + yielder.yield group, member + end end end + block_given? ? enum.each(&block) : enum end - def foreach_group - last = nil - foreach_member do |group, member| - if last != group - last = group - yield group - end - end + def groups + @data["groups"] end def initialize_tables @@ -795,25 +831,27 @@ class Generator if !group["name"] raise "Missing group name" end + if !member["name"] raise "Missing member name in group #{group["name"]}" end + table = member["table"] if table if !@tables[table] raise "Member #{member["name"]} references non-existing table #{table}" end + @used_tables << table end + if !member["field"] member["field"] = member["name"] end + typ = member["type"] - if !typ - pending_types[member] = group - elsif typ == "bool" + if typ == "bool" has_booleans = true - member["type"] = "uint8_t" member["table"] = OFF_ON_TABLE["name"] end end @@ -825,7 +863,7 @@ class Generator @used_tables << OFF_ON_TABLE["name"] end - resolve_types pending_types unless !pending_types + resolve_all_types foreach_enabled_member do |group, member| @count += 1 @max_name_length = [@max_name_length, member["name"].length].max @@ -835,9 +873,82 @@ class Generator end end + def validate_default_values + foreach_enabled_member do |_, member| + name = member["name"] + type = member["type"] + default_value = member["default_value"] + + next if %i[ zero target ].include? default_value + + case + when type == "bool" + raise "Member #{name} has an invalid default value" unless [ false, true ].include? default_value + + when member.has_key?("table") + table_name = member["table"] + table_values = @tables[table_name]["values"] + raise "Member #{name} has an invalid default value" unless table_values.include? default_value + + when type =~ /\A(?u?)int(?8|16|32|64)_t\Z/ + unsigned = !$~[:unsigned].empty? + bitsize = $~[:bitsize].to_i + type_range = unsigned ? 0..(2**bitsize-1) : (-2**(bitsize-1)+1)..(2**(bitsize-1)-1) + raise "Numeric member #{name} doesn't have maximum value defined" unless member.has_key? 'max' + raise "Member #{name} default value has an invalid type, integer or symbol expected" unless default_value.is_a? Integer or default_value.is_a? Symbol + raise "Member #{name} default value is outside type's storage range, min #{type_range.min}, max #{type_range.max}" unless default_value.is_a? Symbol or type_range === default_value + + when type == "float" + raise "Numeric member #{name} doesn't have maximum value defined" unless member.has_key? 'max' + raise "Member #{name} default value has an invalid type, numeric or symbol expected" unless default_value.is_a? Numeric or default_value.is_a? Symbol + + when type == "string" + max = member["max"].to_i + raise "Member #{name} default value has an invalid type, string expected" unless default_value.is_a? String + raise "Member #{name} default value is too long (max #{max} chars)" if default_value.bytesize > max + + else + raise "Unexpected type for member #{name}: #{type.inspect}" + end + end + end + + def scan_types(stderr) + types = Hash.new + # gcc 6-9 + stderr.scan(/var_(\d+).*?['’], which is of non-class type ['‘](.*)['’]/).each do |m| + member_idx = m[0].to_i + type = m[1] + types[member_idx] = type + end + # clang + stderr.scan(/member reference base type '(.*?)'.*?is not a structure or union.*? var_(\d+)/m).each do |m| + member_idx = m[1].to_i + type = m[0] + types[member_idx] = type + end + return types + end + + def resolve_all_types() + loop do + pending = Hash.new + foreach_enabled_member do |group, member| + if !member["type"] + pending[member] = group + end + end + + if pending.empty? + # All types resolved + break + end + + resolve_types(pending) + end + end + def resolve_types(pending) - # TODO: Loop to avoid reaching the maximum number - # of errors printed by the compiler. prog = StringIO.new prog << "int main() {\n" ii = 0 @@ -853,9 +964,13 @@ class Generator prog << "return 0;\n" prog << "};\n" stderr = compile_test_file(prog) - stderr.scan(/var_(\d+).*?', which is of non-class type '(.*)'/).each do |m| - member = members[m[0].to_i] - case m[1] + types = scan_types(stderr) + if types.empty? + raise "No types resolved from #{stderr}" + end + types.each do |idx, type| + member = members[idx] + case type when /^int8_t/ # {aka signed char}" typ = "int8_t" when /^uint8_t/ # {aka unsigned char}" @@ -878,12 +993,6 @@ class Generator dputs "#{member["name"]} type is #{typ}" member["type"] = typ end - # Make sure all types have been resolved - foreach_enabled_member do |group, member| - if !member["type"] - raise "Could not resolve type for member #{member["name"]} in group #{group["name"]}" - end - end end def initialize_name_encoder @@ -925,6 +1034,11 @@ class Generator @value_encoder = ValueEncoder.new(values, constantValues) end + def check_member_default_values_presence + missing_default_value_names = foreach_member.inject([]) { |names, (_, member)| member.has_key?("default_value") ? names : names << member["name"] } + raise "Missing default value for #{missing_default_value_names.count} member#{"s" unless missing_default_value_names.one?}: #{missing_default_value_names * ", "}" unless missing_default_value_names.empty? + end + def resolve_constants(constants) return nil unless constants.length > 0 s = Set.new @@ -936,7 +1050,9 @@ class Generator # warnings to find these constants, the compiler # might reach the maximum number of errors and stop # compilation, so we might need multiple passes. - re = /required from 'class expr_(.*?)<(.*)>'/ + gcc_re = /required from ['‘]class expr_(.*?)<(.*?)>['’]/ # gcc 6-9 + clang_re = / template class 'expr_(.*?)<(.*?)>'/ # clang + res = [gcc_re, clang_re] values = Hash.new while s.length > 0 buf = StringIO.new @@ -956,7 +1072,12 @@ class Generator ii += 1 end stderr = compile_test_file(buf) - matches = stderr.scan(re) + matches = [] + res.each do |re| + if matches.length == 0 + matches = stderr.scan(re) + end + end if matches.length == 0 puts stderr raise "No more matches looking for constants" From 4c0ef9f2b0351cd73318e42ca7f92adb02dcf9b2 Mon Sep 17 00:00:00 2001 From: "Pawel Spychalski (DzikuVx)" Date: Wed, 7 Apr 2021 18:37:20 +0200 Subject: [PATCH 131/286] Add raw rangefinder distance to OSD --- src/main/io/osd.c | 13 +++++++++++++ src/main/io/osd.h | 1 + 2 files changed, 14 insertions(+) diff --git a/src/main/io/osd.c b/src/main/io/osd.c index 6861f8a675..e88db825e1 100644 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -1586,6 +1586,19 @@ static bool osdDrawSingleElement(uint8_t item) break; } +#ifdef USE_RANGEFINDER + case OSD_RANGEFINDER: + { + int32_t range = rangefinderGetLatestRawAltitude(); + if (range < 0) { + buff[0] = "-"; + } else { + osdFormatDistanceSymbol(buff, range); + } + } + break; +#endif + case OSD_ONTIME: { osdFormatOnTime(buff); diff --git a/src/main/io/osd.h b/src/main/io/osd.h index fe35e28b71..24161323fc 100755 --- a/src/main/io/osd.h +++ b/src/main/io/osd.h @@ -220,6 +220,7 @@ typedef enum { OSD_TPA, OSD_NAV_FW_CONTROL_SMOOTHNESS, OSD_VERSION, + OSD_RANGEFINDER, OSD_ITEM_COUNT // MUST BE LAST } osd_items_e; From c26a676bb1df029cb859b51875898c58d7279134 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Niccol=C3=B2=20Maggioni?= Date: Wed, 7 Apr 2021 18:41:23 +0200 Subject: [PATCH 132/286] Add defaults scraping to settings docs updater (#6406) * Add defaults scraping to settings docs updater * Merge #6596 into #6406 * Fix default values stringification --- docs/Settings.md | 178 +++++++++++++++++------------------ src/utils/update_cli_docs.py | 106 +++++++++++++++++++++ 2 files changed, 195 insertions(+), 89 deletions(-) diff --git a/docs/Settings.md b/docs/Settings.md index a2987a8270..96dc9efe9c 100644 --- a/docs/Settings.md +++ b/docs/Settings.md @@ -22,11 +22,11 @@ | acczero_z | 0 | Calculated value after '6 position avanced calibration'. See Wiki page. | | airmode_throttle_threshold | 1300 | Defines airmode THROTTLE activation threshold when `airmode_type` **THROTTLE_THRESHOLD** is used | | airmode_type | STICK_CENTER | Defines the Airmode state handling type. Default **STICK_CENTER** is the classical approach in which Airmode is always active if enabled, but when the throttle is low and ROLL/PITCH/YAW sticks are centered, Iterms is not allowed to grow (ANTI_WINDUP). **THROTTLE_THRESHOLD** is the Airmode behavior known from Betaflight. In this mode, Airmode is active as soon THROTTLE position is above `airmode_throttle_threshold` and stays active until disarm. ANTI_WINDUP is never triggered. For small Multirotors (up to 7-inch propellers) it is suggested to switch to **THROTTLE_THRESHOLD** since it keeps full stabilization no matter what pilot does with the sticks. Fixed Wings always use **STICK_CENTER_ONCE** or **STICK_CENTER** modes. | -| airspeed_adc_channel | :target | ADC channel to use for analog pitot tube (airspeed) sensor. If board doesn't have a dedicated connector for analog airspeed sensor will default to 0 | +| airspeed_adc_channel | _target default_ | ADC channel to use for analog pitot tube (airspeed) sensor. If board doesn't have a dedicated connector for analog airspeed sensor will default to 0 | | align_acc | DEFAULT | When running on non-default hardware or adding support for new sensors/sensor boards, these values are used for sensor orientation. When carefully understood, these values can also be used to rotate (in 90deg steps) or flip the board. Possible values are: DEFAULT, CW0_DEG, CW90_DEG, CW180_DEG, CW270_DEG, CW0_DEG_FLIP, CW90_DEG_FLIP, CW180_DEG_FLIP, CW270_DEG_FLIP. | -| align_board_pitch | :zero | Arbitrary board rotation in deci-degrees (0.1 degree), to allow mounting it sideways / upside down / rotated etc | -| align_board_roll | :zero | Arbitrary board rotation in deci-degrees (0.1 degree), to allow mounting it sideways / upside down / rotated etc | -| align_board_yaw | :zero | Arbitrary board rotation in deci-degrees (0.1 degree), to allow mounting it sideways / upside down / rotated etc | +| align_board_pitch | 0 | Arbitrary board rotation in deci-degrees (0.1 degree), to allow mounting it sideways / upside down / rotated etc | +| align_board_roll | 0 | Arbitrary board rotation in deci-degrees (0.1 degree), to allow mounting it sideways / upside down / rotated etc | +| align_board_yaw | 0 | Arbitrary board rotation in deci-degrees (0.1 degree), to allow mounting it sideways / upside down / rotated etc | | align_gyro | DEFAULT | When running on non-default hardware or adding support for new sensors/sensor boards, these values are used for sensor orientation. When carefully understood, these values can also be used to rotate (in 90deg steps) or flip the board. Possible values are: DEFAULT, CW0_DEG, CW90_DEG, CW180_DEG, CW270_DEG, CW0_DEG_FLIP, CW90_DEG_FLIP, CW180_DEG_FLIP, CW270_DEG_FLIP. | | align_mag | DEFAULT | When running on non-default hardware or adding support for new sensors/sensor boards, these values are used for sensor orientation. When carefully understood, these values can also be used to rotate (in 90deg steps) or flip the board. Possible values are: DEFAULT, CW0_DEG, CW90_DEG, CW180_DEG, CW270_DEG, CW0_DEG_FLIP, CW90_DEG_FLIP, CW180_DEG_FLIP, CW270_DEG_FLIP. | | align_mag_pitch | 0 | Same as align_mag_roll, but for the pitch axis. | @@ -40,73 +40,73 @@ | applied_defaults | 0 | Internal (configurator) hint. Should not be changed manually | | baro_cal_tolerance | 150 | Baro calibration tolerance in cm. The default should allow the noisiest baro to complete calibration [cm]. | | baro_hardware | AUTO | Selection of baro hardware. See Wiki Sensor auto detect and hardware failure detection for more info | -| baro_median_filter | True | 3-point median filtering for barometer readouts. No reason to change this setting | +| baro_median_filter | ON | 3-point median filtering for barometer readouts. No reason to change this setting | | bat_cells | 0 | Number of cells of the battery (0 = autodetect), see battery documentation. 7S, 9S and 11S batteries cannot be autodetected. | | bat_voltage_src | RAW | Chose between raw and sag compensated battery voltage to use for battery alarms and telemetry. Possible values are `RAW` and `SAG_COMP` | | battery_capacity | 0 | Set the battery capacity in mAh or mWh (see `battery_capacity_unit`). Used to calculate the remaining battery capacity. | | battery_capacity_critical | 0 | If the remaining battery capacity goes below this threshold the battery is considered empty and the beeper will emit long beeps. | | battery_capacity_unit | MAH | Unit used for `battery_capacity`, `battery_capacity_warning` and `battery_capacity_critical` [MAH/MWH] (milliAmpere hour / milliWatt hour). | | battery_capacity_warning | 0 | If the remaining battery capacity goes below this threshold the beeper will emit short beeps and the relevant OSD items will blink. | -| blackbox_device | :target | Selection of where to write blackbox data | +| blackbox_device | _target default_ | Selection of where to write blackbox data | | blackbox_rate_denom | 1 | Blackbox logging rate denominator. See blackbox_rate_num. | | blackbox_rate_num | 1 | Blackbox logging rate numerator. Use num/denom settings to decide if a frame should be logged, allowing control of the portion of logged loop iterations | -| cpu_underclock | False | This option is only available on certain architectures (F3 CPUs at the moment). It makes CPU clock lower to reduce interference to long-range RC systems working at 433MHz | +| cpu_underclock | OFF | This option is only available on certain architectures (F3 CPUs at the moment). It makes CPU clock lower to reduce interference to long-range RC systems working at 433MHz | | cruise_power | 0 | Power draw at cruise throttle used for remaining flight time/distance estimation in 0.01W unit | -| current_adc_channel | :target | ADC channel to use for analog current sensor input. Defaults to board CURRENT sensor input (if available). 0 = disabled | -| current_meter_offset | :target | This sets the output offset voltage of the current sensor in millivolts. | -| current_meter_scale | :target | This sets the output voltage to current scaling for the current sensor in 0.1 mV/A steps. 400 is 40mV/A such as the ACS756 sensor outputs. 183 is the setting for the uberdistro with a 0.25mOhm shunt. | +| current_adc_channel | _target default_ | ADC channel to use for analog current sensor input. Defaults to board CURRENT sensor input (if available). 0 = disabled | +| current_meter_offset | _target default_ | This sets the output offset voltage of the current sensor in millivolts. | +| current_meter_scale | _target default_ | This sets the output voltage to current scaling for the current sensor in 0.1 mV/A steps. 400 is 40mV/A such as the ACS756 sensor outputs. 183 is the setting for the uberdistro with a 0.25mOhm shunt. | | current_meter_type | ADC | ADC , VIRTUAL, NONE. The virtual current sensor, once calibrated, estimates the current value from throttle position. | | d_boost_factor | 1.25 | | | d_boost_gyro_delta_lpf_hz | 80 | | | d_boost_max_at_acceleration | 7500 | | | deadband | 5 | These are values (in us) by how much RC input can be different before it's considered valid. For transmitters with jitter on outputs, this value can be increased. Defaults are zero, but can be increased up to 10 or so if rc inputs twitch while idle. | | debug_mode | NONE | Defines debug values exposed in debug variables (developer / debugging setting) | -| disarm_kill_switch | True | Disarms the motors independently of throttle value. Setting to OFF reverts to the old behaviour of disarming only when the throttle is low. Only applies when arming and disarming with an AUX channel. | -| display_force_sw_blink | False | OFF = OSD hardware blink / ON = OSD software blink. If OSD warning text/values are invisible, try setting this to ON | +| disarm_kill_switch | ON | Disarms the motors independently of throttle value. Setting to OFF reverts to the old behaviour of disarming only when the throttle is low. Only applies when arming and disarming with an AUX channel. | +| display_force_sw_blink | OFF | OFF = OSD hardware blink / ON = OSD software blink. If OSD warning text/values are invisible, try setting this to ON | | dji_esc_temp_source | ESC | Re-purpose the ESC temperature field for IMU/BARO temperature | | dji_speed_source | GROUND | Sets the speed type displayed by the DJI OSD: GROUND, 3D, AIR | -| dji_use_name_for_messages | True | Re-purpose the craft name field for messages. Replace craft name with :WTSED for Warnings|Throttle|Speed|Efficiency|Trip distance | +| dji_use_name_for_messages | ON | Re-purpose the craft name field for messages. Replace craft name with :WTSED for Warnings|Throttle|Speed|Efficiency|Trip distance | | dji_workarounds | 1 | Enables workarounds for different versions of MSP protocol used | | dterm_lpf2_hz | 0 | Cutoff frequency for stage 2 D-term low pass filter | | dterm_lpf2_type | BIQUAD | Defines the type of stage 1 D-term LPF filter. Possible values: `PT1`, `BIQUAD`. `PT1` offers faster filter response while `BIQUAD` better attenuation. | | dterm_lpf_hz | 40 | Dterm low pass filter cutoff frequency. Default setting is very conservative and small multirotors should use higher value between 80 and 100Hz. 80 seems like a gold spot for 7-inch builds while 100 should work best with 5-inch machines. If motors are getting too hot, lower the value | | dterm_lpf_type | BIQUAD | Defines the type of stage 1 D-term LPF filter. Possible values: `PT1`, `BIQUAD`. `PT1` offers faster filter response while `BIQUAD` better attenuation. | -| dynamic_gyro_notch_enabled | False | Enable/disable dynamic gyro notch also known as Matrix Filter | +| dynamic_gyro_notch_enabled | OFF | Enable/disable dynamic gyro notch also known as Matrix Filter | | dynamic_gyro_notch_min_hz | 150 | Minimum frequency for dynamic notches. Default value of `150` works best with 5" multirors. Should be lowered with increased size of propellers. Values around `100` work fine on 7" drones. 10" can go down to `60` - `70` | | dynamic_gyro_notch_q | 120 | Q factor for dynamic notches | | dynamic_gyro_notch_range | MEDIUM | Range for dynamic gyro notches. `MEDIUM` for 5", `HIGH` for 3" and `MEDIUM`/`LOW` for 7" and bigger propellers | | eleres_freq | 435 | | | eleres_loc_delay | 240 | | -| eleres_loc_en | False | | +| eleres_loc_en | OFF | | | eleres_loc_power | 7 | | -| eleres_signature | :zero | | -| eleres_telemetry_en | False | | +| eleres_signature | 0 | | +| eleres_telemetry_en | OFF | | | eleres_telemetry_power | 7 | | -| esc_sensor_listen_only | False | Enable when BLHeli32 Auto Telemetry function is used. Disable in every other case | +| esc_sensor_listen_only | OFF | Enable when BLHeli32 Auto Telemetry function is used. Disable in every other case | | failsafe_delay | 5 | Time in deciseconds to wait before activating failsafe when signal is lost. See [Failsafe documentation](Failsafe.md#failsafe_delay). | | failsafe_fw_pitch_angle | 100 | Amount of dive/climb when `SET-THR` failsafe is active on a fixed-wing machine. In 1/10 deg (deci-degrees). Negative values = climb | | failsafe_fw_roll_angle | -200 | Amount of banking when `SET-THR` failsafe is active on a fixed-wing machine. In 1/10 deg (deci-degrees). Negative values = left roll | | failsafe_fw_yaw_rate | -45 | Requested yaw rate to execute when `SET-THR` failsafe is active on a fixed-wing machine. In deg/s. Negative values = left turn | -| failsafe_lights | True | Enable or disable the lights when the `FAILSAFE` flight mode is enabled. The target needs to be compiled with `USE_LIGHTS` [ON/OFF]. | +| failsafe_lights | ON | Enable or disable the lights when the `FAILSAFE` flight mode is enabled. The target needs to be compiled with `USE_LIGHTS` [ON/OFF]. | | failsafe_lights_flash_on_time | 100 | Flash lights ON time in milliseconds when `failsafe_lights` is ON and `FAILSAFE` flight mode is enabled. [20-65535]. | | failsafe_lights_flash_period | 1000 | Time in milliseconds between two flashes when `failsafe_lights` is ON and `FAILSAFE` flight mode is enabled [40-65535]. | | failsafe_min_distance | 0 | If failsafe happens when craft is closer than this distance in centimeters from home, failsafe will not execute regular failsafe_procedure, but will execute procedure specified in failsafe_min_distance_procedure instead. 0 = Normal failsafe_procedure always taken. | | failsafe_min_distance_procedure | DROP | What failsafe procedure to initiate in Stage 2 when craft is closer to home than failsafe_min_distance. See [Failsafe documentation](Failsafe.md#failsafe_throttle). | -| failsafe_mission | True | If set to `OFF` the failsafe procedure won't be triggered and the mission will continue if the FC is in WP (automatic mission) mode | +| failsafe_mission | ON | If set to `OFF` the failsafe procedure won't be triggered and the mission will continue if the FC is in WP (automatic mission) mode | | failsafe_off_delay | 200 | Time in deciseconds to wait before turning off motors when failsafe is activated. 0 = No timeout. See [Failsafe documentation](Failsafe.md#failsafe_off_delay). | | failsafe_procedure | SET-THR | What failsafe procedure to initiate in Stage 2. See [Failsafe documentation](Failsafe.md#failsafe_throttle). | | failsafe_recovery_delay | 5 | Time in deciseconds to wait before aborting failsafe when signal is recovered. See [Failsafe documentation](Failsafe.md#failsafe_recovery_delay). | | failsafe_stick_threshold | 50 | Threshold for stick motion to consider failsafe condition resolved. If non-zero failsafe won't clear even if RC link is restored - you have to move sticks to exit failsafe. | | failsafe_throttle | 1000 | Throttle level used for landing when failsafe is enabled. See [Failsafe documentation](Failsafe.md#failsafe_throttle). | | failsafe_throttle_low_delay | 0 | If failsafe activated when throttle is low for this much time - bypass failsafe and disarm, in 10th of seconds. 0 = No timeout | -| fixed_wing_auto_arm | False | Auto-arm fixed wing aircraft on throttle above min_check, and disarming with stick commands are disabled, so power cycle is required to disarm. Requires enabled motorstop and no arm switch configured. | +| fixed_wing_auto_arm | OFF | Auto-arm fixed wing aircraft on throttle above min_check, and disarming with stick commands are disabled, so power cycle is required to disarm. Requires enabled motorstop and no arm switch configured. | | flaperon_throw_offset | 200 | Defines throw range in us for both ailerons that will be passed to servo mixer via input source 14 (`FEATURE FLAPS`) when FLAPERON mode is activated. | | flip_over_after_crash_power_factor | 65 | flip over after crash power factor | | fpv_mix_degrees | 0 | | | frsky_coordinates_format | 0 | D-Series telemetry only: FRSKY_FORMAT_DMS (default), FRSKY_FORMAT_NMEA | | frsky_default_latitude | 0 | D-Series telemetry only: OpenTX needs a valid set of coordinates to show compass value. A fake value defined in this setting is sent while no fix is acquired. | | frsky_default_longitude | 0 | D-Series telemetry only: OpenTX needs a valid set of coordinates to show compass value. A fake value defined in this setting is sent while no fix is acquired. | -| frsky_pitch_roll | False | S.Port and D-Series telemetry: Send pitch and roll degrees*10 instead of raw accelerometer data | +| frsky_pitch_roll | OFF | S.Port and D-Series telemetry: Send pitch and roll degrees*10 instead of raw accelerometer data | | frsky_unit | METRIC | Not used? [METRIC/IMPERIAL] | | frsky_vfas_precision | 0 | D-Series telemetry only: Set to 1 to send raw VBat value in 0.1V resolution for receivers that can handle it, or 0 (default) to use the standard method | | fw_autotune_ff_to_i_tc | 600 | FF to I time (defines time for I to reach the same level of response as FF) [ms] | @@ -139,13 +139,13 @@ | fw_turn_assist_pitch_gain | 1 | Gain required to keep constant pitch angle during coordinated turns (in TURN_ASSIST mode). Value significantly different from 1.0 indicates a problem with the airspeed calibration (if present) or value of `fw_reference_airspeed` parameter | | fw_turn_assist_yaw_gain | 1 | Gain required to keep the yaw rate consistent with the turn rate for a coordinated turn (in TURN_ASSIST mode). Value significantly different from 1.0 indicates a problem with the airspeed calibration (if present) or value of `fw_reference_airspeed` parameter | | fw_yaw_iterm_freeze_bank_angle | 0 | Yaw Iterm is frozen when bank angle is above this threshold [degrees]. This solves the problem of the rudder counteracting turns by partially disabling yaw stabilization when making banked turns. Setting to 0 (the default) disables this feature. Only applies when autopilot is not active and TURN ASSIST is disabled. | -| gps_auto_baud | True | Automatic configuration of GPS baudrate(The specified baudrate in configured in ports will be used) when used with UBLOX GPS. When used with NAZA/DJI it will automatic detect GPS baudrate and change to it, ignoring the selected baudrate set in ports | -| gps_auto_config | True | Enable automatic configuration of UBlox GPS receivers. | +| gps_auto_baud | ON | Automatic configuration of GPS baudrate(The specified baudrate in configured in ports will be used) when used with UBLOX GPS. When used with NAZA/DJI it will automatic detect GPS baudrate and change to it, ignoring the selected baudrate set in ports | +| gps_auto_config | ON | Enable automatic configuration of UBlox GPS receivers. | | gps_dyn_model | AIR_1G | GPS navigation model: Pedestrian, Air_1g, Air_4g. Default is AIR_1G. Use pedestrian with caution, can cause flyaways with fast flying. | | gps_min_sats | 6 | Minimum number of GPS satellites in view to acquire GPS_FIX and consider GPS position valid. Some GPS receivers appeared to be very inaccurate with low satellite count. | | gps_provider | UBLOX | Which GPS protocol to be used, note that UBLOX is 5Hz and UBLOX7 is 10Hz (M8N). | | gps_sbas_mode | NONE | Which SBAS mode to be used | -| gps_ublox_use_galileo | False | Enable use of Galileo satellites. This is at the expense of other regional constellations, so benefit may also be regional. Requires M8N and Ublox firmware 3.x (or later) [OFF/ON]. | +| gps_ublox_use_galileo | OFF | Enable use of Galileo satellites. This is at the expense of other regional constellations, so benefit may also be regional. Requires M8N and Ublox firmware 3.x (or later) [OFF/ON]. | | gyro_dyn_lpf_curve_expo | 5 | Expo value for the throttle-to-frequency mapping for Dynamic LPF | | gyro_dyn_lpf_max_hz | 500 | Maximum frequency of the gyro Dynamic LPF | | gyro_dyn_lpf_min_hz | 200 | Minimum frequency of the gyro Dynamic LPF | @@ -156,10 +156,10 @@ | gyro_notch_hz | 0 | | | gyro_stage2_lowpass_hz | 0 | Software based second stage lowpass filter for gyro. Value is cutoff frequency (Hz) | | gyro_stage2_lowpass_type | BIQUAD | Defines the type of stage 2 gyro LPF filter. Possible values: `PT1`, `BIQUAD`. `PT1` offers faster filter response while `BIQUAD` better attenuation. | -| gyro_sync | True | This option enables gyro_sync feature. In this case the loop will be synced to gyro refresh rate. Loop will always wait for the newest gyro measurement. Maximum gyro refresh rate is determined by gyro_hardware_lpf | +| gyro_sync | ON | This option enables gyro_sync feature. In this case the loop will be synced to gyro refresh rate. Loop will always wait for the newest gyro measurement. Maximum gyro refresh rate is determined by gyro_hardware_lpf | | gyro_to_use | 0 | | -| gyro_use_dyn_lpf | False | Use Dynamic LPF instead of static gyro stage1 LPF. Dynamic Gyro LPF updates gyro LPF based on the throttle position. | -| has_flaps | False | Defines is UAV is capable of having flaps. If ON and AIRPLANE `platform_type` is used, **FLAPERON** flight mode will be available for the pilot | +| gyro_use_dyn_lpf | OFF | Use Dynamic LPF instead of static gyro stage1 LPF. Dynamic Gyro LPF updates gyro LPF based on the throttle position. | +| has_flaps | OFF | Defines is UAV is capable of having flaps. If ON and AIRPLANE `platform_type` is used, **FLAPERON** flight mode will be available for the pilot | | heading_hold_rate_limit | 90 | This setting limits yaw rotation rate that HEADING_HOLD controller can request from PID inner loop controller. It is independent from manual yaw rate and used only when HEADING_HOLD flight mode is enabled by pilot, RTH or WAYPOINT modes. | | hott_alarm_sound_interval | 5 | Battery alarm delay in seconds for Hott telemetry | | i2c_speed | 400KHZ | This setting controls the clock speed of I2C bus. 400KHZ is the default that most setups are able to use. Some noise-free setups may be overclocked to 800KHZ. Some sensor chips or setups with long wires may work unreliably at 400KHZ - user can try lowering the clock speed to 200KHZ or even 100KHZ. User need to bear in mind that lower clock speeds might require higher looptimes (lower looptime rate) | @@ -177,25 +177,25 @@ | imu2_hardware | NONE | Selection of a Secondary IMU hardware type. NONE disables this functionality | | imu2_radius_acc | 0 | Secondary IMU MAG calibration data | | imu2_radius_mag | 0 | Secondary IMU MAG calibration data | -| imu2_use_for_osd_ahi | False | If set to ON, Secondary IMU data will be used for Analog OSD Artificial Horizon | -| imu2_use_for_osd_heading | False | If set to ON, Secondary IMU data will be used for Analog OSD heading | -| imu2_use_for_stabilized | False | If set to ON, Secondary IMU data will be used for Angle, Horizon and all other modes that control attitude (PosHold, WP, RTH) | +| imu2_use_for_osd_ahi | OFF | If set to ON, Secondary IMU data will be used for Analog OSD Artificial Horizon | +| imu2_use_for_osd_heading | OFF | If set to ON, Secondary IMU data will be used for Analog OSD heading | +| imu2_use_for_stabilized | OFF | If set to ON, Secondary IMU data will be used for Angle, Horizon and all other modes that control attitude (PosHold, WP, RTH) | | imu_acc_ignore_rate | 0 | Total gyro rotation rate threshold [deg/s] to consider accelerometer trustworthy on airplanes | | imu_acc_ignore_slope | 0 | Half-width of the interval to gradually reduce accelerometer weight. Centered at `imu_acc_ignore_rate` (exactly 50% weight) | | imu_dcm_ki | 50 | Inertial Measurement Unit KI Gain for accelerometer measurements | | imu_dcm_ki_mag | 0 | Inertial Measurement Unit KI Gain for compass measurements | | imu_dcm_kp | 2500 | Inertial Measurement Unit KP Gain for accelerometer measurements | | imu_dcm_kp_mag | 10000 | Inertial Measurement Unit KP Gain for compass measurements | -| inav_allow_dead_reckoning | False | Defines if inav will dead-reckon over short GPS outages. May also be useful for indoors OPFLOW navigation | -| inav_auto_mag_decl | True | Automatic setting of magnetic declination based on GPS position. When used manual magnetic declination is ignored. | +| inav_allow_dead_reckoning | OFF | Defines if inav will dead-reckon over short GPS outages. May also be useful for indoors OPFLOW navigation | +| inav_auto_mag_decl | ON | Automatic setting of magnetic declination based on GPS position. When used manual magnetic declination is ignored. | | inav_baro_epv | 100 | Uncertainty value for barometric sensor [cm] | | inav_gravity_cal_tolerance | 5 | Unarmed gravity calibration tolerance level. Won't finish the calibration until estimated gravity error falls below this value. | | inav_max_eph_epv | 1000 | Maximum uncertainty value until estimated position is considered valid and is used for navigation [cm] | | inav_max_surface_altitude | 200 | Max allowed altitude for surface following mode. [cm] | | inav_reset_altitude | FIRST_ARM | Defines when relative estimated altitude is reset to zero. Variants - `NEVER` (once reference is acquired it's used regardless); `FIRST_ARM` (keep altitude at zero until firstly armed), `EACH_ARM` (altitude is reset to zero on each arming) | | inav_reset_home | FIRST_ARM | Allows to chose when the home position is reset. Can help prevent resetting home position after accidental mid-air disarm. Possible values are: NEVER, FIRST_ARM and EACH_ARM | -| inav_use_gps_no_baro | False | | -| inav_use_gps_velned | True | Defined if iNav should use velocity data provided by GPS module for doing position and speed estimation. If set to OFF iNav will fallback to calculating velocity from GPS coordinates. Using native velocity data may improve performance on some GPS modules. Some GPS modules introduce significant delay and using native velocity may actually result in much worse performance. | +| inav_use_gps_no_baro | OFF | | +| inav_use_gps_velned | ON | Defined if iNav should use velocity data provided by GPS module for doing position and speed estimation. If set to OFF iNav will fallback to calculating velocity from GPS coordinates. Using native velocity data may improve performance on some GPS modules. Some GPS modules introduce significant delay and using native velocity may actually result in much worse performance. | | inav_w_acc_bias | 0.01 | Weight for accelerometer drift estimation | | inav_w_xy_flow_p | 1.0 | | | inav_w_xy_flow_v | 2.0 | | @@ -210,7 +210,7 @@ | inav_w_z_surface_p | 3.5 | | | inav_w_z_surface_v | 6.1 | | | iterm_windup | 50 | Used to prevent Iterm accumulation on during maneuvers. Iterm will be dampened when motors are reaching it's limit (when requested motor correction range is above percentage specified by this parameter) | -| ledstrip_visual_beeper | False | | +| ledstrip_visual_beeper | OFF | | | log_level | ERROR | Defines serial debugging log level. See `docs/development/serial_printf_debugging.md` for usage. | | log_topics | 0 | Defines serial debugging log topic. See `docs/development/serial_printf_debugging.md` for usage. | | looptime | 1000 | This is the main loop time (in us). Changing this affects PID effect with some PID controllers (see PID section for details). A very conservative value of 3500us/285Hz should work for everyone. Setting it to zero does not limit loop time, so it will go as fast as possible. | @@ -222,9 +222,9 @@ | maggain_x | 1024 | Magnetometer calibration X gain. If 1024, no calibration or calibration failed | | maggain_y | 1024 | Magnetometer calibration Y gain. If 1024, no calibration or calibration failed | | maggain_z | 1024 | Magnetometer calibration Z gain. If 1024, no calibration or calibration failed | -| magzero_x | :zero | Magnetometer calibration X offset. If its 0 none offset has been applied and calibration is failed. | -| magzero_y | :zero | Magnetometer calibration Y offset. If its 0 none offset has been applied and calibration is failed. | -| magzero_z | :zero | Magnetometer calibration Z offset. If its 0 none offset has been applied and calibration is failed. | +| magzero_x | 0 | Magnetometer calibration X offset. If its 0 none offset has been applied and calibration is failed. | +| magzero_y | 0 | Magnetometer calibration Y offset. If its 0 none offset has been applied and calibration is failed. | +| magzero_z | 0 | Magnetometer calibration Z offset. If its 0 none offset has been applied and calibration is failed. | | manual_pitch_rate | 100 | Servo travel multiplier for the PITCH axis in `MANUAL` flight mode [0-100]% | | manual_rc_expo | 70 | Exposition value used for the PITCH/ROLL axes by the `MANUAL` flight mode [0-100] | | manual_rc_yaw_expo | 20 | Exposition value used for the YAW axis by the `MANUAL` flight mode [0-100] | @@ -265,18 +265,18 @@ | moron_threshold | 32 | When powering up, gyro bias is calculated. If the model is shaking/moving during this initial calibration, offsets are calculated incorrectly, and could lead to poor flying performance. This threshold means how much average gyro reading could differ before re-calibration is triggered. | | motor_accel_time | 0 | Minimum time for the motor(s) to accelerate from 0 to 100% throttle (ms) [0-1000] | | motor_decel_time | 0 | Minimum time for the motor(s) to deccelerate from 100 to 0% throttle (ms) [0-1000] | -| motor_direction_inverted | False | Use if you need to inverse yaw motor direction. | +| motor_direction_inverted | OFF | Use if you need to inverse yaw motor direction. | | motor_poles | 14 | The number of motor poles. Required to compute motor RPM | | motor_pwm_protocol | ONESHOT125 | Protocol that is used to send motor updates to ESCs. Possible values - STANDARD, ONESHOT125, ONESHOT42, MULTISHOT, DSHOT150, DSHOT300, DSHOT600, DSHOT1200, BRUSHED | | motor_pwm_rate | 400 | Output frequency (in Hz) for motor pins. Default is 400Hz for motor with motor_pwm_protocol set to STANDARD. For *SHOT (e.g. ONESHOT125) values of 1000 and 2000 have been tested by the development team and are supported. It may be possible to use higher values. For BRUSHED values of 8000 and above should be used. Setting to 8000 will use brushed mode at 8kHz switching frequency. Up to 32kHz is supported for brushed. Default is 16000 for boards with brushed motors. Note, that in brushed mode, minthrottle is offset to zero. For brushed mode, set max_throttle to 2000. | | msp_override_channels | 0 | Mask of RX channels that may be overridden by MSP `SET_RAW_RC`. Note that this requires custom firmware with `USE_RX_MSP` and `USE_MSP_RC_OVERRIDE` compile options and the `MSP RC Override` flight mode. | -| name | | Craft name | +| name | _empty_ | Craft name | | nav_auto_climb_rate | 500 | Maximum climb/descent rate that UAV is allowed to reach during navigation modes. [cm/s] | | nav_auto_speed | 300 | Maximum velocity firmware is allowed in full auto modes (RTH, WP) [cm/s] [Multirotor only] | -| nav_disarm_on_landing | False | If set to ON, iNav disarms the FC after landing | +| nav_disarm_on_landing | OFF | If set to ON, iNav disarms the FC after landing | | nav_emerg_landing_speed | 500 | Rate of descent UAV will try to maintain when doing emergency descent sequence [cm/s] | | nav_extra_arming_safety | ON | If set to ON drone won't arm if no GPS fix and any navigation mode like RTH or POSHOLD is configured. ALLOW_BYPASS allows the user to momentarily disable this check by holding yaw high (left stick held at the bottom right in mode 2) when switch arming is used | -| nav_fw_allow_manual_thr_increase | False | Enable the possibility to manually increase the throttle in auto throttle controlled modes for fixed wing | +| nav_fw_allow_manual_thr_increase | OFF | Enable the possibility to manually increase the throttle in auto throttle controlled modes for fixed wing | | nav_fw_bank_angle | 35 | Max roll angle when rolling / turning in GPS assisted modes, is also restrained by global max_angle_inclination_rll | | nav_fw_climb_angle | 20 | Max pitch angle when climbing in GPS assisted modes, is also restrained by global max_angle_inclination_pit | | nav_fw_control_smoothness | 0 | How smoothly the autopilot controls the airplane to correct the navigation error | @@ -349,7 +349,7 @@ | nav_mc_vel_z_d | 10 | D gain of velocity PID controller | | nav_mc_vel_z_i | 50 | I gain of velocity PID controller | | nav_mc_vel_z_p | 100 | P gain of velocity PID controller | -| nav_mc_wp_slowdown | True | When ON, NAV engine will slow down when switching to the next waypoint. This prioritizes turning over forward movement. When OFF, NAV engine will continue to the next waypoint and turn as it goes. | +| nav_mc_wp_slowdown | ON | When ON, NAV engine will slow down when switching to the next waypoint. This prioritizes turning over forward movement. When OFF, NAV engine will continue to the next waypoint and turn as it goes. | | nav_min_rth_distance | 500 | Minimum distance from homepoint when RTH full procedure will be activated [cm]. Below this distance, the mode will activate at the current location and the final phase is executed (loiter / land). Above this distance, the full procedure is activated, which may include initial climb and flying directly to the homepoint before entering the loiter / land phase. | | nav_overrides_motor_stop | ALL_NAV | When set to OFF the navigation system will not take over the control of the motor if the throttle is low (motor will stop). When set to OFF_ALWAYS the navigation system will not take over the control of the motor if the throttle was low even when failsafe is triggered. When set to AUTO_ONLY the navigation system will only take over the control of the throttle in autonomous navigation modes (NAV WP and NAV RTH). When set to ALL_NAV (default) the navigation system will take over the control of the motor completely and never allow the motor to stop even when the throttle is low. This setting only has an effect on NAV modes which take control of the throttle when combined with MOTOR_STOP and is likely to cause a stall if fw_min_throttle_down_pitch isn't set correctly or the pitch estimation is wrong for fixed wing models when not set to ALL_NAV | | nav_position_timeout | 5 | If GPS fails wait for this much seconds before switching to emergency landing mode (0 - disable) | @@ -357,21 +357,21 @@ | nav_rth_allow_landing | ALWAYS | If set to ON drone will land as a last phase of RTH. | | nav_rth_alt_mode | AT_LEAST | Configure how the aircraft will manage altitude on the way home, see Navigation modes on wiki for more details | | nav_rth_altitude | 1000 | Used in EXTRA, FIXED and AT_LEAST rth alt modes [cm] (Default 1000 means 10 meters) | -| nav_rth_climb_first | True | If set to ON drone will climb to nav_rth_altitude first and head home afterwards. If set to OFF drone will head home instantly and climb on the way. | -| nav_rth_climb_ignore_emerg | False | If set to ON, aircraft will execute initial climb regardless of position sensor (GPS) status. | +| nav_rth_climb_first | ON | If set to ON drone will climb to nav_rth_altitude first and head home afterwards. If set to OFF drone will head home instantly and climb on the way. | +| nav_rth_climb_ignore_emerg | OFF | If set to ON, aircraft will execute initial climb regardless of position sensor (GPS) status. | | nav_rth_home_altitude | 0 | Aircraft will climb/descend to this altitude after reaching home if landing is not enabled. Set to 0 to stay at `nav_rth_altitude` (default) [cm] | -| nav_rth_tail_first | False | If set to ON drone will return tail-first. Obviously meaningless for airplanes. | -| nav_use_fw_yaw_control | False | Enables or Disables the use of the heading PID controller on fixed wing. Heading PID controller is always enabled for rovers and boats | -| nav_use_midthr_for_althold | False | If set to OFF, the FC remembers your throttle stick position when enabling ALTHOLD and treats it as a neutral midpoint for holding altitude | +| nav_rth_tail_first | OFF | If set to ON drone will return tail-first. Obviously meaningless for airplanes. | +| nav_use_fw_yaw_control | OFF | Enables or Disables the use of the heading PID controller on fixed wing. Heading PID controller is always enabled for rovers and boats | +| nav_use_midthr_for_althold | OFF | If set to OFF, the FC remembers your throttle stick position when enabling ALTHOLD and treats it as a neutral midpoint for holding altitude | | nav_user_control_mode | ATTI | Defines how Pitch/Roll input from RC receiver affects flight in POSHOLD mode: ATTI - pitch/roll controls attitude like in ANGLE mode; CRUISE - pitch/roll controls velocity in forward and right direction. | | nav_wp_radius | 100 | Waypoint radius [cm]. Waypoint would be considered reached if machine is within this radius | | nav_wp_safe_distance | 10000 | First waypoint in the mission should be closer than this value [cm]. A value of 0 disables this check. | | opflow_hardware | NONE | Selection of OPFLOW hardware. | | opflow_scale | 10.5 | | -| osd_ahi_bordered | False | Shows a border/corners around the AHI region (pixel OSD only) | +| osd_ahi_bordered | OFF | Shows a border/corners around the AHI region (pixel OSD only) | | osd_ahi_height | 162 | AHI height in pixels (pixel OSD only) | | osd_ahi_max_pitch | 20 | Max pitch, in degrees, for OSD artificial horizon | -| osd_ahi_reverse_roll | False | | +| osd_ahi_reverse_roll | OFF | | | osd_ahi_style | DEFAULT | Sets OSD Artificial Horizon style "DEFAULT" or "LINE" for the FrSky Graphical OSD. | | osd_ahi_vertical_offset | -18 | AHI vertical offset from center (pixel OSD only) | | osd_ahi_width | 132 | AHI width in pixels (pixel OSD only) | @@ -388,16 +388,16 @@ | osd_dist_alarm | 1000 | Value above which to make the OSD distance from home indicator blink (meters) | | osd_esc_temp_alarm_max | 900 | Temperature above which the IMU temperature OSD element will start blinking (decidegrees centigrade) | | osd_esc_temp_alarm_min | -200 | Temperature under which the IMU temperature OSD element will start blinking (decidegrees centigrade) | -| osd_estimations_wind_compensation | True | Use wind estimation for remaining flight time/distance estimation | -| osd_failsafe_switch_layout | False | If enabled the OSD automatically switches to the first layout during failsafe | -| osd_force_grid | False | Force OSD to work in grid mode even if the OSD device supports pixel level access (mainly used for development) | +| osd_estimations_wind_compensation | ON | Use wind estimation for remaining flight time/distance estimation | +| osd_failsafe_switch_layout | OFF | If enabled the OSD automatically switches to the first layout during failsafe | +| osd_force_grid | OFF | Force OSD to work in grid mode even if the OSD device supports pixel level access (mainly used for development) | | osd_gforce_alarm | 5 | Value above which the OSD g force indicator will blink (g) | | osd_gforce_axis_alarm_max | 5 | Value above which the OSD axis g force indicators will blink (g) | | osd_gforce_axis_alarm_min | -5 | Value under which the OSD axis g force indicators will blink (g) | -| osd_home_position_arm_screen | True | Should home position coordinates be displayed on the arming screen. | +| osd_home_position_arm_screen | ON | Should home position coordinates be displayed on the arming screen. | | osd_horizon_offset | 0 | To vertically adjust the whole OSD and AHI and scrolling bars | -| osd_hud_homepoint | False | To 3D-display the home point location in the hud | -| osd_hud_homing | False | To display little arrows around the crossair showing where the home point is in the hud | +| osd_hud_homepoint | OFF | To 3D-display the home point location in the hud | +| osd_hud_homing | OFF | To display little arrows around the crossair showing where the home point is in the hud | | osd_hud_margin_h | 3 | Left and right margins for the hud area | | osd_hud_margin_v | 3 | Top and bottom margins for the hud area | | osd_hud_radar_disp | 0 | Maximum count of nearby aircrafts or points of interest to display in the hud, as sent from an ESP32 LoRa module. Set to 0 to disable (show nothing). The nearby aircrafts will appear as markers A, B, C, etc | @@ -421,7 +421,7 @@ | osd_row_shiftdown | 0 | Number of rows to shift the OSD display (increase if top rows are cut off) | | osd_rssi_alarm | 20 | Value below which to make the OSD RSSI indicator blink | | osd_sidebar_horizontal_offset | 0 | Sidebar horizontal offset from default position. Positive values move the sidebars closer to the edges. | -| osd_sidebar_scroll_arrows | False | | +| osd_sidebar_scroll_arrows | OFF | | | osd_snr_alarm | 4 | Value below which Crossfire SNR Alarm pops-up. (dB) | | osd_stats_energy_unit | MAH | Unit used for the drawn energy in the OSD stats [MAH/WH] (milliAmpere hour/ Watt hour) | | osd_temp_label_align | LEFT | Allows to chose between left and right alignment for the OSD temperature sensor labels. Valid values are `LEFT` and `RIGHT` | @@ -431,10 +431,10 @@ | pid_type | AUTO | Allows to set type of PID controller used in control loop. Possible values: `NONE`, `PID`, `PIFF`, `AUTO`. Change only in case of experimental platforms like VTOL, tailsitters, rovers, boats, etc. Airplanes should always use `PIFF` and multirotors `PID` | | pidsum_limit | 500 | A limitation to overall amount of correction Flight PID can request on each axis (Roll/Pitch). If when doing a hard maneuver on one axis machine looses orientation on other axis - reducing this parameter may help | | pidsum_limit_yaw | 350 | A limitation to overall amount of correction Flight PID can request on each axis (Yaw). If when doing a hard maneuver on one axis machine looses orientation on other axis - reducing this parameter may help | -| pinio_box1 | :BOX_PERMANENT_ID_NONE | Mode assignment for PINIO#1 | -| pinio_box2 | :BOX_PERMANENT_ID_NONE | Mode assignment for PINIO#1 | -| pinio_box3 | :BOX_PERMANENT_ID_NONE | Mode assignment for PINIO#1 | -| pinio_box4 | :BOX_PERMANENT_ID_NONE | Mode assignment for PINIO#1 | +| pinio_box1 | `BOX_PERMANENT_ID_NONE` | Mode assignment for PINIO#1 | +| pinio_box2 | `BOX_PERMANENT_ID_NONE` | Mode assignment for PINIO#1 | +| pinio_box3 | `BOX_PERMANENT_ID_NONE` | Mode assignment for PINIO#1 | +| pinio_box4 | `BOX_PERMANENT_ID_NONE` | Mode assignment for PINIO#1 | | pitch_rate | 20 | Defines rotation rate on PITCH axis that UAV will try to archive on max. stick deflection. Rates are defined in tens of degrees (deca-degrees) per second [rate = dps/10]. That means, rate 20 represents 200dps rotation speed. Default 20 (200dps) is more less equivalent of old Cleanflight/Baseflight rate 0. Max. 180 (1800dps) is what gyro can measure. | | pitot_hardware | NONE | Selection of pitot hardware. | | pitot_lpf_milli_hz | 350 | | @@ -443,21 +443,21 @@ | pos_hold_deadband | 10 | Stick deadband in [r/c points], applied after r/c deadband and expo | | prearm_timeout | 10000 | Duration (ms) for which Prearm being activated is valid. after this, Prearm needs to be reset. 0 means Prearm does not timeout. | | rangefinder_hardware | NONE | Selection of rangefinder hardware. | -| rangefinder_median_filter | False | 3-point median filtering for rangefinder readouts | +| rangefinder_median_filter | OFF | 3-point median filtering for rangefinder readouts | | rate_accel_limit_roll_pitch | 0 | Limits acceleration of ROLL/PITCH rotation speed that can be requested by stick input. In degrees-per-second-squared. Small and powerful UAV flies great with high acceleration limit ( > 5000 dps^2 and even > 10000 dps^2). Big and heavy multirotors will benefit from low acceleration limit (~ 360 dps^2). When set correctly, it greatly improves stopping performance. Value of 0 disables limiting. | | rate_accel_limit_yaw | 10000 | Limits acceleration of YAW rotation speed that can be requested by stick input. In degrees-per-second-squared. Small and powerful UAV flies great with high acceleration limit ( > 10000 dps^2). Big and heavy multirotors will benefit from low acceleration limit (~ 180 dps^2). When set correctly, it greatly improves stopping performance and general stability during yaw turns. Value of 0 disables limiting. | | rc_expo | 70 | Exposition value used for the PITCH/ROLL axes by all the stabilized flights modes (all but `MANUAL`) | | rc_filter_frequency | 50 | RC data biquad filter cutoff frequency. Lower cutoff frequencies result in smoother response at expense of command control delay. Practical values are 20-50. Set to zero to disable entirely and use unsmoothed RC stick values | | rc_yaw_expo | 20 | Exposition value used for the YAW axis by all the stabilized flights modes (all but `MANUAL`) | | reboot_character | 82 | Special character used to trigger reboot | -| receiver_type | :target | Selection of receiver (RX) type. Additional configuration of a `serialrx_provider` and a UART will be needed for `SERIAL` | -| report_cell_voltage | False | S.Port, D-Series, and IBUS telemetry: Send the average cell voltage if set to ON | +| receiver_type | _target default_ | Selection of receiver (RX) type. Additional configuration of a `serialrx_provider` and a UART will be needed for `SERIAL` | +| report_cell_voltage | OFF | S.Port, D-Series, and IBUS telemetry: Send the average cell voltage if set to ON | | roll_rate | 20 | Defines rotation rate on ROLL axis that UAV will try to archive on max. stick deflection. Rates are defined in tens of degrees (deca-degrees) per second [rate = dps/10]. That means, rate 20 represents 200dps rotation speed. Default 20 (200dps) is more less equivalent of old Cleanflight/Baseflight rate 0. Max. 180 (1800dps) is what gyro can measure. | -| rpm_gyro_filter_enabled | False | Enables gyro RPM filtere. Set to `ON` only when ESC telemetry is working and rotation speed of the motors is correctly reported to INAV | +| rpm_gyro_filter_enabled | OFF | Enables gyro RPM filtere. Set to `ON` only when ESC telemetry is working and rotation speed of the motors is correctly reported to INAV | | rpm_gyro_harmonics | 1 | Number of harmonic frequences to be covered by gyro RPM filter. Default value of `1` usually works just fine | | rpm_gyro_min_hz | 100 | The lowest frequency for gyro RPM filtere. Default `150` is fine for 5" mini-quads. On 7-inch drones you can lower even down to `60`-`70` | | rpm_gyro_q | 500 | Q factor for gyro RPM filter. Lower values give softer, wider attenuation. Usually there is no need to change this setting | -| rssi_adc_channel | :target | ADC channel to use for analog RSSI input. Defaults to board RSSI input (if available). 0 = disabled | +| rssi_adc_channel | _target default_ | ADC channel to use for analog RSSI input. Defaults to board RSSI input (if available). 0 = disabled | | rssi_channel | 0 | RX channel containing the RSSI signal | | rssi_max | 100 | The maximum RSSI value sent by the receiver, in %. For example, if your receiver's maximum RSSI value shows as 83% in the configurator/OSD set this parameter to 83. See also rssi_min. | | rssi_min | 0 | The minimum RSSI value sent by the receiver, in %. For example, if your receiver's minimum RSSI value shows as 42% in the configurator/OSD set this parameter to 42. See also rssi_max. Note that rssi_min can be set to a value bigger than rssi_max to invert the RSSI calculation (i.e. bigger values mean lower RSSI). | @@ -465,43 +465,43 @@ | rth_energy_margin | 5 | Energy margin wanted after getting home (percent of battery energy capacity). Use for the remaining flight time/distance calculation | | rx_max_usec | 2115 | Defines the longest pulse width value used when ensuring the channel value is valid. If the receiver gives a pulse value higher than this value then the channel will be marked as bad and will default to the value of mid_rc. | | rx_min_usec | 885 | Defines the shortest pulse width value used when ensuring the channel value is valid. If the receiver gives a pulse value lower than this value then the channel will be marked as bad and will default to the value of mid_rc. | -| rx_spi_id | :zero | | -| rx_spi_protocol | :target | | -| rx_spi_rf_channel_count | :zero | | +| rx_spi_id | 0 | | +| rx_spi_protocol | _target default_ | | +| rx_spi_rf_channel_count | 0 | | | safehome_max_distance | 20000 | In order for a safehome to be used, it must be less than this distance (in cm) from the arming point. | | sbus_sync_interval | 3000 | | -| sdcard_detect_inverted | :target | This setting drives the way SD card is detected in card slot. On some targets (AnyFC F7 clone) different card slot was used and depending of hardware revision ON or OFF setting might be required. If card is not detected, change this value. | +| sdcard_detect_inverted | _target default_ | This setting drives the way SD card is detected in card slot. On some targets (AnyFC F7 clone) different card slot was used and depending of hardware revision ON or OFF setting might be required. If card is not detected, change this value. | | serialrx_halfduplex | AUTO | Allow serial receiver to operate on UART TX pin. With some receivers will allow control and telemetry over a single wire. | -| serialrx_inverted | False | Reverse the serial inversion of the serial RX protocol. When this value is OFF, each protocol will use its default signal (e.g. SBUS will use an inverted signal). Some OpenLRS receivers produce a non-inverted SBUS signal. This setting supports this type of receivers (including modified FrSKY). | -| serialrx_provider | :target | When feature SERIALRX is enabled, this allows connection to several receivers which output data via digital interface resembling serial. See RX section. | +| serialrx_inverted | OFF | Reverse the serial inversion of the serial RX protocol. When this value is OFF, each protocol will use its default signal (e.g. SBUS will use an inverted signal). Some OpenLRS receivers produce a non-inverted SBUS signal. This setting supports this type of receivers (including modified FrSKY). | +| serialrx_provider | _target default_ | When feature SERIALRX is enabled, this allows connection to several receivers which output data via digital interface resembling serial. See RX section. | | servo_center_pulse | 1500 | Servo midpoint | | servo_lpf_hz | 20 | Selects the servo PWM output cutoff frequency. Value is in [Hz] | | servo_protocol | PWM | An option to chose the protocol/option that would be used to output servo data. Possible options `PWM` (FC servo outputs), `SERVO_DRIVER` (I2C PCA9685 peripheral), `SBUS` (S.Bus protocol output via a configured serial port) | | servo_pwm_rate | 50 | Output frequency (in Hz) servo pins. When using tricopters or gimbal with digital servo, this rate can be increased. Max of 498Hz (for 500Hz pwm period), and min of 50Hz. Most digital servos will support for example 330Hz. | -| setpoint_kalman_enabled | False | Enable Kalman filter on the PID controller setpoint | +| setpoint_kalman_enabled | OFF | Enable Kalman filter on the PID controller setpoint | | setpoint_kalman_q | 100 | Quality factor of the setpoint Kalman filter. Higher values means less filtering and lower phase delay. On 3-7 inch multirotors can be usually increased to 200-300 or even higher of clean builds | | setpoint_kalman_sharpness | 100 | Dynamic factor for the setpoint Kalman filter. In general, the higher the value, the more dynamic Kalman filter gets | | setpoint_kalman_w | 4 | Window size for the setpoint Kalman filter. Wider the window, more samples are used to compute variance. In general, wider window results in smoother filter response | -| sim_ground_station_number | | Number of phone that is used to communicate with SIM module. Messages / calls from other numbers are ignored. If undefined, can be set by calling or sending a message to the module. | +| sim_ground_station_number | _empty_ | Number of phone that is used to communicate with SIM module. Messages / calls from other numbers are ignored. If undefined, can be set by calling or sending a message to the module. | | sim_low_altitude | -32767 | Threshold for low altitude warning messages sent by SIM module when the 'L' transmit flag is set in `sim_transmit_flags`. | | sim_pin | 0000 | PIN code for the SIM module | -| sim_transmit_flags | :SIM_TX_FLAG_FAILSAFE | Bitmask specifying text message transmit condition flags for the SIM module. 1: continuous transmission, 2: continuous transmission in failsafe mode, 4: continuous transmission when GPS signal quality is low, 8: acceleration events, 16: continuous transmission when altitude is below `sim_low_altitude` | +| sim_transmit_flags | `SIM_TX_FLAG_FAILSAFE` | Bitmask specifying text message transmit condition flags for the SIM module. 1: continuous transmission, 2: continuous transmission in failsafe mode, 4: continuous transmission when GPS signal quality is low, 8: acceleration events, 16: continuous transmission when altitude is below `sim_low_altitude` | | sim_transmit_interval | 60 | Text message transmission interval in seconds for SIM module. Minimum value: 10 | | small_angle | 25 | If the aircraft tilt angle exceed this value the copter will refuse to arm. | | smartport_fuel_unit | MAH | S.Port telemetry only: Unit of the value sent with the `FUEL` ID (FrSky D-Series always sends percent). [PERCENT/MAH/MWH] | -| smartport_master_halfduplex | True | | -| smartport_master_inverted | False | | -| spektrum_sat_bind | :SPEKTRUM_SAT_BIND_DISABLED | 0 = disabled. Used to bind the spektrum satellite to RX | -| srxl2_baud_fast | True | | +| smartport_master_halfduplex | ON | | +| smartport_master_inverted | OFF | | +| spektrum_sat_bind | `SPEKTRUM_SAT_BIND_DISABLED` | 0 = disabled. Used to bind the spektrum satellite to RX | +| srxl2_baud_fast | ON | | | srxl2_unit_id | 1 | | -| stats | False | General switch of the statistics recording feature (a.k.a. odometer) | +| stats | OFF | General switch of the statistics recording feature (a.k.a. odometer) | | stats_total_dist | 0 | Total flight distance [in meters]. The value is updated on every disarm when "stats" are enabled. | | stats_total_energy | 0 | | | stats_total_time | 0 | Total flight time [in seconds]. The value is updated on every disarm when "stats" are enabled. | | switch_disarm_delay | 250 | Delay before disarming when requested by switch (ms) [0-1000] | -| telemetry_halfduplex | True | S.Port telemetry only: Turn UART into UNIDIR for usage on F1 and F4 target. See Telemetry.md for details | -| telemetry_inverted | False | Determines if the telemetry protocol default signal inversion is reversed. This should be OFF in most cases unless a custom or hacked RX is used. | -| telemetry_switch | False | Which aux channel to use to change serial output & baud rate (MSP / Telemetry). It disables automatic switching to Telemetry when armed. | +| telemetry_halfduplex | ON | S.Port telemetry only: Turn UART into UNIDIR for usage on F1 and F4 target. See Telemetry.md for details | +| telemetry_inverted | OFF | Determines if the telemetry protocol default signal inversion is reversed. This should be OFF in most cases unless a custom or hacked RX is used. | +| telemetry_switch | OFF | Which aux channel to use to change serial output & baud rate (MSP / Telemetry). It disables automatic switching to Telemetry when armed. | | thr_comp_weight | 1 | Weight used for the throttle compensation based on battery voltage. See the [battery documentation](Battery.md#automatic-throttle-compensation-based-on-battery-voltage) | | thr_expo | 0 | Throttle exposition value | | thr_mid | 50 | Throttle value when the stick is set to mid-position. Used in the throttle curve calculation. | @@ -510,24 +510,24 @@ | throttle_tilt_comp_str | 0 | Can be used in ANGLE and HORIZON mode and will automatically boost throttle when banking. Setting is in percentage, 0=disabled. | | tpa_breakpoint | 1500 | See tpa_rate. | | tpa_rate | 0 | Throttle PID attenuation reduces influence of P on ROLL and PITCH as throttle increases. For every 1% throttle after the TPA breakpoint, P is reduced by the TPA rate. | -| tri_unarmed_servo | True | On tricopter mix only, if this is set to ON, servo will always be correcting regardless of armed state. to disable this, set it to OFF. | +| tri_unarmed_servo | ON | On tricopter mix only, if this is set to ON, servo will always be correcting regardless of armed state. to disable this, set it to OFF. | | tz_automatic_dst | OFF | Automatically add Daylight Saving Time to the GPS time when needed or simply ignore it. Includes presets for EU and the USA - if you live outside these areas it is suggested to manage DST manually via `tz_offset`. | | tz_offset | 0 | Time zone offset from UTC, in minutes. This is applied to the GPS time for logging and time-stamping of Blackbox logs | -| vbat_adc_channel | :target | ADC channel to use for battery voltage sensor. Defaults to board VBAT input (if available). 0 = disabled | +| vbat_adc_channel | _target default_ | ADC channel to use for battery voltage sensor. Defaults to board VBAT input (if available). 0 = disabled | | vbat_cell_detect_voltage | 425 | Maximum voltage per cell, used for auto-detecting the number of cells of the battery in 0.01V units, default is 4.30V. | | vbat_max_cell_voltage | 420 | Maximum voltage per cell in 0.01V units, default is 4.20V | | vbat_meter_type | ADC | Vbat voltage source. Possible values: `NONE`, `ADC`, `ESC`. `ESC` required ESC telemetry enebled and running | | vbat_min_cell_voltage | 330 | Minimum voltage per cell, this triggers battery out alarms, in 0.01V units, default is 330 (3.3V) | -| vbat_scale | :target | Battery voltage calibration value. 1100 = 11:1 voltage divider (10k:1k) x 100. Adjust this slightly if reported pack voltage is different from multimeter reading. You can get current voltage by typing "status" in cli. | +| vbat_scale | _target default_ | Battery voltage calibration value. 1100 = 11:1 voltage divider (10k:1k) x 100. Adjust this slightly if reported pack voltage is different from multimeter reading. You can get current voltage by typing "status" in cli. | | vbat_warning_cell_voltage | 350 | Warning voltage per cell, this triggers battery-warning alarms, in 0.01V units, default is 350 (3.5V) | | vtx_band | 4 | Configure the VTX band. Set to zero to use `vtx_freq`. Bands: 1: A, 2: B, 3: E, 4: F, 5: Race. | | vtx_channel | 1 | Channel to use within the configured `vtx_band`. Valid values are [1, 8]. | -| vtx_halfduplex | True | Use half duplex UART to communicate with the VTX, using only a TX pin in the FC. | +| vtx_halfduplex | ON | Use half duplex UART to communicate with the VTX, using only a TX pin in the FC. | | vtx_low_power_disarm | OFF | When the craft is disarmed, set the VTX to its lowest power. `ON` will set the power to its minimum value on startup, increase it to `vtx_power` when arming and change it back to its lowest setting after disarming. `UNTIL_FIRST_ARM` will start with minimum power, but once the craft is armed it will increase to `vtx_power` and it will never decrease until the craft is power cycled. | | vtx_max_power_override | 0 | Some VTXes may report max power incorrectly (i.e. 200mW for a 600mW VTX). Use this to override max supported power. 0 to disable and use whatever VTX reports as its capabilities | | vtx_pit_mode_chan | 1 | | | vtx_power | 1 | VTX RF power level to use. The exact number of mw depends on the VTX hardware. | -| vtx_smartaudio_early_akk_workaround | True | Enable workaround for early AKK SAudio-enabled VTX bug. | +| vtx_smartaudio_early_akk_workaround | ON | Enable workaround for early AKK SAudio-enabled VTX bug. | | yaw_deadband | 5 | These are values (in us) by how much RC input can be different before it's considered valid. For transmitters with jitter on outputs, this value can be increased. Defaults are zero, but can be increased up to 10 or so if rc inputs twitch while idle. | | yaw_lpf_hz | 0 | Yaw low pass filter cutoff frequency. Should be disabled (set to `0`) on small multirotors (7 inches and below) | | yaw_rate | 20 | Defines rotation rate on YAW axis that UAV will try to archive on max. stick deflection. Rates are defined in tens of degrees (deca-degrees) per second [rate = dps/10]. That means, rate 20 represents 200dps rotation speed. Default 20 (200dps) is more less equivalent of old Cleanflight/Baseflight rate 0. Max. 180 (1800dps) is what gyro can measure. | diff --git a/src/utils/update_cli_docs.py b/src/utils/update_cli_docs.py index 7ca8bdf711..f7f7d8a24b 100755 --- a/src/utils/update_cli_docs.py +++ b/src/utils/update_cli_docs.py @@ -1,10 +1,26 @@ #!/usr/bin/env python3 import optparse +import os +import re import yaml # pyyaml / python-yaml SETTINGS_MD_PATH = "docs/Settings.md" SETTINGS_YAML_PATH = "src/main/fc/settings.yaml" +CODE_DEFAULTS_PATH = "src/main" + +DEFAULTS_BLACKLIST = [ + 'baro_hardware', + 'dterm_lpf_type', + 'dterm_lpf2_type', + 'failsafe_procedure', + 'flaperon_throw_offset', + 'heading_hold_rate_limit', + 'mag_hardware', + 'pitot_hardware', + 'rx_min_usec', + 'serialrx_provider', +] def parse_settings_yaml(): """Parse the YAML settings specs""" @@ -21,6 +37,23 @@ def generate_md_table_from_yaml(settings_yaml): for member in group['members']: if not any(key in member for key in ["description", "default_value"]) and not options.quiet: print("Setting \"{}\" has no description or default value specified".format(member['name'])) + # Handle edge cases of YAML autogeneration + if "default_value" in member: + # Replace booleans with "ON"/"OFF" + if type(member["default_value"]) == bool: + member["default_value"] = "ON" if member["default_value"] else "OFF" + # Replace zero placeholder with actual zero + elif member["default_value"] == ":zero": + member["default_value"] = 0 + # Replace target-default placeholder with extended definition + elif member["default_value"] == ":target": + member["default_value"] = "_target default_" + # Replace empty strings with more evident marker + elif member["default_value"] == "": + member["default_value"] = "_empty_" + # Reformat direct code references + elif str(member["default_value"])[0] == ":": + member["default_value"] = f'`{member["default_value"][1:]}`' params[member['name']] = { "description": member["description"] if "description" in member else "", "default": member["default_value"] if "default_value" in member else "" @@ -45,13 +78,86 @@ def write_settings_md(lines): with open(SETTINGS_MD_PATH, "w") as settings_md: settings_md.writelines(lines) +# Return all matches of a compiled regex in a list of files +def regex_search(regex, files): + for f in files: + with open(f, 'r') as _f: + for _, line in enumerate(_f.readlines()): + matches = regex.search(line) + if matches: + yield matches + +# Return plausible default values for a given setting found by scraping the relative source files +def find_default(setting_name, headers): + regex = re.compile(rf'^\s*\.{setting_name}\s=\s([A-Za-z0-9_\-]+)(?:,)?(?:\s+//.+$)?') + files_to_search_in = [] + for header in headers: + header = f'{CODE_DEFAULTS_PATH}/{header}' + files_to_search_in.append(header) + if header.endswith('.h'): + header_c = re.sub(r'\.h$', '.c', header) + if os.path.isfile(header_c): + files_to_search_in.append(header_c) + defaults = [] + for matches in regex_search(regex, files_to_search_in): + defaults.append(matches.group(1)) + return defaults + +# Try to map default values in the YAML spec back to the actual C code and check for mismatches (defaults updated in the code but not in the YAML) +# Done by scraping the source files, prone to false negatives. Settings in `DEFAULTS_BLACKLIST` are ignored for this +# reason (values that refer to other source files are too complex to parse this way). +def check_defaults(settings_yaml): + retval = True + for group in settings_yaml['groups']: + if 'headers' in group: + headers = group['headers'] + for member in group['members']: + # Ignore blacklisted settings + if member['name'] in DEFAULTS_BLACKLIST: + continue + + default_from_code = find_default(member['name'], headers) + if len(default_from_code) == 0: # No default value found (no direct code mapping) + continue + elif len(default_from_code) > 1: # Duplicate default values found (regexes are a quick but poor solution) + if not options.quiet: + print(f"Duplicate default values found for {member['name']}: {default_from_code}, consider adding to blacklist") + continue + + # Extract the only matched value, guarded by the previous checks + default_from_code = default_from_code[0] + # Map C values to their equivalents used in the YAML spec + code_values_map = { 'true': 'ON', 'false': 'OFF' } + if default_from_code in code_values_map: + default_from_code = code_values_map[default_from_code] + + default_from_yaml = member["default_value"] if "default_value" in member else "" + # Remove eventual Markdown formatting + default_from_yaml = default_from_yaml.replace('`', '').replace('*', '').replace('__', '') + # Allow specific C-YAML matches that coudln't be replaced in the previous steps + extra_allowed_matches = { '1': 'ON', '0': 'OFF' } + + if default_from_yaml not in default_from_code: # Equal or substring + if default_from_code in extra_allowed_matches and default_from_yaml in extra_allowed_matches[default_from_code]: + continue + if not options.quiet: + print(f"{member['name']} has mismatched default values. Code reports '{default_from_code}' and YAML reports '{default_from_yaml}'") + retval = False + return retval + if __name__ == "__main__": global options, args parser = optparse.OptionParser() parser.add_option('-q', '--quiet', action="store_true", default=False, help="do not write anything to stdout") + parser.add_option('-d', '--defaults', action="store_true", default=False, help="check for mismatched default values") options, args = parser.parse_args() settings_yaml = parse_settings_yaml() + + if options.defaults: + defaults_match = check_defaults(settings_yaml) + quit(0 if defaults_match else 1) + md_table_lines = generate_md_table_from_yaml(settings_yaml) settings_md_lines = \ ["# CLI Variable Reference\n", "\n" ] + \ From b529229a1ca00c01a50aa48962bc7129b81e5730 Mon Sep 17 00:00:00 2001 From: "Pawel Spychalski (DzikuVx)" Date: Wed, 7 Apr 2021 20:10:30 +0200 Subject: [PATCH 133/286] Fetch Euler angles over BNO055 Serial protocol --- .../drivers/accgyro/accgyro_bno055_serial.c | 62 ++++++++-------- .../drivers/accgyro/accgyro_bno055_serial.h | 3 +- src/main/fc/fc_tasks.c | 2 +- src/main/flight/secondary_imu.c | 71 +++++++++++-------- src/main/flight/secondary_imu.h | 1 + 5 files changed, 77 insertions(+), 62 deletions(-) diff --git a/src/main/drivers/accgyro/accgyro_bno055_serial.c b/src/main/drivers/accgyro/accgyro_bno055_serial.c index ca1ebfb50b..7955c9d7e8 100644 --- a/src/main/drivers/accgyro/accgyro_bno055_serial.c +++ b/src/main/drivers/accgyro/accgyro_bno055_serial.c @@ -33,6 +33,7 @@ #include "drivers/accgyro/accgyro_bno055.h" #include "build/debug.h" #include "drivers/time.h" +#include "flight/secondary_imu.h" static serialPort_t * bno055SerialPort = NULL; @@ -51,11 +52,17 @@ typedef enum { BNO055_FRAME_DATA, } bno055FrameType_e; +typedef enum { + BNO055_DATA_TYPE_NONE, + BNO055_DATA_TYPE_EULER, +} bno055DataType_e; + static uint8_t bno055ProtocolState = BNO055_RECEIVE_IDLE; static uint8_t bno055FrameType; static uint8_t bno055FrameLength; static uint8_t bno055FrameIndex; static timeMs_t bno055FrameStartAtMs = 0; +static uint8_t bno055DataType = BNO055_DATA_TYPE_NONE; static void bno055SerialWrite(uint8_t reg, uint8_t data) { bno055ProtocolState = BNO055_RECEIVE_IDLE; @@ -66,12 +73,12 @@ static void bno055SerialWrite(uint8_t reg, uint8_t data) { serialWrite(bno055SerialPort, data); } -static void bno055SerialRead(uint8_t reg) { +static void bno055SerialRead(const uint8_t reg, const uint8_t len) { bno055ProtocolState = BNO055_RECEIVE_IDLE; serialWrite(bno055SerialPort, 0xAA); // Start Byte serialWrite(bno055SerialPort, 0x01); // Read command serialWrite(bno055SerialPort, reg); - serialWrite(bno055SerialPort, 1); + serialWrite(bno055SerialPort, len); } void bno055SerialDataReceive(uint16_t c, void *data) { @@ -104,6 +111,15 @@ void bno055SerialDataReceive(uint16_t c, void *data) { if (bno055FrameIndex == bno055FrameLength) { bno055ProtocolState = BNO055_RECEIVE_IDLE; + + if (bno055DataType == BNO055_DATA_TYPE_EULER) { + secondaryImuState.eulerAngles.raw[0] = ((int16_t)((receiveBuffer[3] << 8) | receiveBuffer[2])) / 1.6f; + secondaryImuState.eulerAngles.raw[1] = ((int16_t)((receiveBuffer[5] << 8) | receiveBuffer[4])) / -1.6f; //Pitch has to be reversed to match INAV notation + secondaryImuState.eulerAngles.raw[2] = ((int16_t)((receiveBuffer[1] << 8) | receiveBuffer[0])) / 1.6f; + secondaryImuProcess(); + } + + bno055DataType = BNO055_DATA_TYPE_NONE; } } @@ -117,8 +133,6 @@ bool bno055SerialInit(bno055CalibrationData_t calibrationData, bool setCalibrati return false; } - // DEBUG_SET(DEBUG_IMU2, 0, 2); - bno055SerialPort = openSerialPort( portConfig->identifier, FUNCTION_BNO055, @@ -133,44 +147,34 @@ bool bno055SerialInit(bno055CalibrationData_t calibrationData, bool setCalibrati return false; } - bno055SerialRead(0x00); // Read ChipID + bno055SerialRead(0x00, 1); // Read ChipID delay(5); - DEBUG_SET(DEBUG_IMU2, 0, bno055FrameType); - DEBUG_SET(DEBUG_IMU2, 1, receiveBuffer[0]); - DEBUG_SET(DEBUG_IMU2, 2, bno055ProtocolState); - DEBUG_SET(DEBUG_IMU2, 3, bno055FrameIndex); - //Check ident if (bno055FrameType != BNO055_FRAME_DATA || receiveBuffer[0] != 0xA0 || bno055ProtocolState != BNO055_RECEIVE_IDLE) { return false; // Ident does not match, leave } - // bno055SerialWrite(BNO055_ADDR_PWR_MODE, BNO055_PWR_MODE_NORMAL); //Set power mode NORMAL - // delay(5); + bno055SerialWrite(BNO055_ADDR_PWR_MODE, BNO055_PWR_MODE_NORMAL); //Set power mode NORMAL + delay(25); + + //TODO Here set calibration data + + bno055SerialWrite(BNO055_ADDR_OPR_MODE, BNO055_OPR_MODE_NDOF); //Set power mode NORMAL + delay(25); - // DEBUG_SET(DEBUG_IMU2, 0, 3); - - // while (1) { - // // bno055SerialWrite(BNO055_ADDR_PWR_MODE, BNO055_PWR_MODE_NORMAL); //Set power mode NORMAL - // // bno055SerialRead(0x00); //Set power mode NORMAL - // bno055SerialRead(0x00); // Read ChipID - // delay(100); - // } - - - // DEBUG_SET(DEBUG_IMU2, 0, 4); + // DEBUG_SET(DEBUG_IMU2, 0, bno055FrameType); + // DEBUG_SET(DEBUG_IMU2, 1, receiveBuffer[0]); + // DEBUG_SET(DEBUG_IMU2, 2, bno055ProtocolState); + // DEBUG_SET(DEBUG_IMU2, 3, bno055FrameIndex); return true; } -fpVector3_t bno055SerialGetEurlerAngles(void) { - -} - -void bno055SerialFetchEulerAngles(int16_t * buffer) { - // bno055SerialRead(0x00); +void bno055SerialFetchEulerAngles() { + bno055DataType = BNO055_DATA_TYPE_EULER; + bno055SerialRead(BNO055_ADDR_EUL_YAW_LSB, 6); } bno055CalibStat_t bno055SerialGetCalibStat(void) { diff --git a/src/main/drivers/accgyro/accgyro_bno055_serial.h b/src/main/drivers/accgyro/accgyro_bno055_serial.h index 5c0b4945e1..2db1e78f5f 100644 --- a/src/main/drivers/accgyro/accgyro_bno055_serial.h +++ b/src/main/drivers/accgyro/accgyro_bno055_serial.h @@ -28,8 +28,7 @@ #include "drivers/accgyro/accgyro_bno055.h" bool bno055SerialInit(bno055CalibrationData_t calibrationData, bool setCalibration); -fpVector3_t bno055SerialGetEurlerAngles(void); -void bno055SerialFetchEulerAngles(int16_t * buffer); +void bno055SerialFetchEulerAngles(); bno055CalibStat_t bno055SerialGetCalibStat(void); bno055CalibrationData_t bno055SerialGetCalibrationData(void); void bno055SerialSetCalibrationData(bno055CalibrationData_t data); \ No newline at end of file diff --git a/src/main/fc/fc_tasks.c b/src/main/fc/fc_tasks.c index df5fc7357f..33d417a1d6 100755 --- a/src/main/fc/fc_tasks.c +++ b/src/main/fc/fc_tasks.c @@ -605,7 +605,7 @@ cfTask_t cfTasks[TASK_COUNT] = { [TASK_SECONDARY_IMU] = { .taskName = "IMU2", .taskFunc = taskSecondaryImu, - .desiredPeriod = TASK_PERIOD_HZ(1), // 10Hz @100msec + .desiredPeriod = TASK_PERIOD_HZ(10), // 10Hz @100msec .staticPriority = TASK_PRIORITY_IDLE, }, #endif diff --git a/src/main/flight/secondary_imu.c b/src/main/flight/secondary_imu.c index d47ee33712..ecdee8b33e 100644 --- a/src/main/flight/secondary_imu.c +++ b/src/main/flight/secondary_imu.c @@ -30,6 +30,7 @@ #include "sensors/compass.h" #include "build/debug.h" +#include "scheduler/scheduler.h" #include "drivers/sensor.h" #include "drivers/accgyro/accgyro_bno055.h" @@ -83,8 +84,17 @@ void secondaryImuInit(void) if (secondaryImuConfig()->hardwareType == SECONDARY_IMU_BNO055) { secondaryImuState.active = bno055Init(calibrationData, (secondaryImuConfig()->calibrationRadiusAcc && secondaryImuConfig()->calibrationRadiusMag)); + + if (secondaryImuState.active) { + rescheduleTask(TASK_SECONDARY_IMU, TASK_PERIOD_HZ(10)); + } + } else if (secondaryImuConfig()->hardwareType == SECONDARY_IMU_BNO055_SERIAL) { secondaryImuState.active = bno055SerialInit(calibrationData, (secondaryImuConfig()->calibrationRadiusAcc && secondaryImuConfig()->calibrationRadiusMag)); + + if (secondaryImuState.active) { + rescheduleTask(TASK_SECONDARY_IMU, TASK_PERIOD_HZ(50)); + } } if (!secondaryImuState.active) { @@ -93,25 +103,7 @@ void secondaryImuInit(void) } -void taskSecondaryImu(timeUs_t currentTimeUs) -{ - if (!secondaryImuState.active) - { - return; - } - /* - * Secondary IMU works in decidegrees - */ - UNUSED(currentTimeUs); - - if (secondaryImuConfig()->hardwareType == SECONDARY_IMU_BNO055) { - bno055FetchEulerAngles(secondaryImuState.eulerAngles.raw); - } else if (secondaryImuConfig()->hardwareType == SECONDARY_IMU_BNO055_SERIAL) { - bno055SerialFetchEulerAngles(secondaryImuState.eulerAngles.raw); - } - - bno055FetchEulerAngles(secondaryImuState.eulerAngles.raw); - +void secondaryImuProcess(void) { //Apply mag declination secondaryImuState.eulerAngles.raw[2] += secondaryImuState.magDeclination; @@ -138,17 +130,6 @@ void taskSecondaryImu(timeUs_t currentTimeUs) secondaryImuState.eulerAngles.values.pitch = rotated.y; secondaryImuState.eulerAngles.values.yaw = rotated.z; - /* - * Every 10 cycles fetch current calibration state - */ - static uint8_t tick = 0; - tick++; - if (tick == 10) - { - secondaryImuState.calibrationStatus = bno055GetCalibStat(); - tick = 0; - } - DEBUG_SET(DEBUG_IMU2, 0, secondaryImuState.eulerAngles.values.roll); DEBUG_SET(DEBUG_IMU2, 1, secondaryImuState.eulerAngles.values.pitch); DEBUG_SET(DEBUG_IMU2, 2, secondaryImuState.eulerAngles.values.yaw); @@ -159,6 +140,36 @@ void taskSecondaryImu(timeUs_t currentTimeUs) // DEBUG_SET(DEBUG_IMU2, 6, secondaryImuState.magDeclination); } +void taskSecondaryImu(timeUs_t currentTimeUs) +{ + if (!secondaryImuState.active) + { + return; + } + /* + * Secondary IMU works in decidegrees + */ + UNUSED(currentTimeUs); + + if (secondaryImuConfig()->hardwareType == SECONDARY_IMU_BNO055) { + bno055FetchEulerAngles(secondaryImuState.eulerAngles.raw); + secondaryImuProcess(); + + /* + * Every 10 cycles fetch current calibration state + */ + static uint8_t tick = 0; + tick++; + if (tick == 10) + { + secondaryImuState.calibrationStatus = bno055GetCalibStat(); + tick = 0; + } + } else if (secondaryImuConfig()->hardwareType == SECONDARY_IMU_BNO055_SERIAL) { + bno055SerialFetchEulerAngles(); + } +} + void secondaryImuFetchCalibration(void) { bno055CalibrationData_t calibrationData = bno055GetCalibrationData(); diff --git a/src/main/flight/secondary_imu.h b/src/main/flight/secondary_imu.h index 2c3b90ec6a..e7414a48c7 100644 --- a/src/main/flight/secondary_imu.h +++ b/src/main/flight/secondary_imu.h @@ -62,6 +62,7 @@ extern secondaryImuState_t secondaryImuState; PG_DECLARE(secondaryImuConfig_t, secondaryImuConfig); +void secondaryImuProcess(void); void secondaryImuInit(void); void taskSecondaryImu(timeUs_t currentTimeUs); void secondaryImuFetchCalibration(void); From 8df415d5445f6e914d9a15a9a3596a1fa028929a Mon Sep 17 00:00:00 2001 From: pieniacy Date: Tue, 6 Apr 2021 21:04:35 +0200 Subject: [PATCH 134/286] Fix int32_t microseconds overflow in navigation. --- src/main/common/time.h | 14 ++++- src/main/io/ledstrip.c | 2 +- src/main/io/osd.c | 16 +++--- src/main/io/osd_dji_hd.c | 14 ++--- src/main/io/smartport_master.c | 2 +- src/main/navigation/navigation_fixedwing.c | 16 +++--- src/main/navigation/navigation_multicopter.c | 56 ++++++++++---------- src/main/navigation/navigation_private.h | 5 +- src/main/navigation/navigation_rover_boat.c | 10 ++-- src/main/rx/fport2.c | 4 +- src/main/rx/ghst.c | 10 ++-- src/main/rx/sumd.c | 2 +- src/main/rx/sumh.c | 2 +- src/main/sensors/battery.c | 6 +-- src/main/sensors/rangefinder.c | 2 +- 15 files changed, 87 insertions(+), 74 deletions(-) diff --git a/src/main/common/time.h b/src/main/common/time.h index bef2f6b6a2..ca8f4b6107 100644 --- a/src/main/common/time.h +++ b/src/main/common/time.h @@ -24,10 +24,19 @@ #include "config/parameter_group.h" -// time difference, 32 bits always sufficient +// time difference, signed 32 bits of microseconds overflows at ~35 minutes +// this is worth leaving as int32_t for performance reasons, use timeDeltaLarge_t for deltas that can be big typedef int32_t timeDelta_t; +#define TIMEDELTA_MAX INT32_MAX + +// time difference large, signed 64 bits of microseconds overflows at ~300000 years +typedef int64_t timeDeltaLarge_t; +#define TIMEDELTALARGE_MAX INT64_MAX + // millisecond time -typedef uint32_t timeMs_t ; +typedef uint32_t timeMs_t; +#define TIMEMS_MAX UINT32_MAX + // microsecond time #ifdef USE_64BIT_TIME typedef uint64_t timeUs_t; @@ -48,6 +57,7 @@ typedef uint32_t timeUs_t; #define MS2S(ms) ((ms) * 1e-3f) #define HZ2S(hz) US2S(HZ2US(hz)) +// Use this function only to get small deltas (difference overflows at ~35 minutes) static inline timeDelta_t cmpTimeUs(timeUs_t a, timeUs_t b) { return (timeDelta_t)(a - b); } typedef enum { diff --git a/src/main/io/ledstrip.c b/src/main/io/ledstrip.c index 8fd74fc8b6..022d85fa36 100644 --- a/src/main/io/ledstrip.c +++ b/src/main/io/ledstrip.c @@ -935,7 +935,7 @@ void ledStripUpdate(timeUs_t currentTimeUs) for (timId_e timId = 0; timId < timTimerCount; timId++) { // sanitize timer value, so that it can be safely incremented. Handles inital timerVal value. // max delay is limited to 5s - int32_t delta = cmpTimeUs(currentTimeUs, timerVal[timId]); + timeDelta_t delta = cmpTimeUs(currentTimeUs, timerVal[timId]); if (delta < 0 && delta > -LED_STRIP_MS(5000)) continue; // not ready yet timActive |= 1 << timId; diff --git a/src/main/io/osd.c b/src/main/io/osd.c index b6943791f7..2f2d9f265f 100644 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -967,7 +967,7 @@ static bool osdIsHeadingValid(void) } else { return isImuHeadingValid(); } -#else +#else return isImuHeadingValid(); #endif } @@ -980,7 +980,7 @@ int16_t osdGetHeading(void) } else { return attitude.values.yaw; } -#else +#else return attitude.values.yaw; #endif } @@ -1614,7 +1614,7 @@ static bool osdDrawSingleElement(uint8_t item) /*static int32_t updatedTimeSeconds = 0;*/ timeUs_t currentTimeUs = micros(); static int32_t timeSeconds = -1; - if (cmpTimeUs(currentTimeUs, updatedTimestamp) >= 1000000) { + if (cmpTimeUs(currentTimeUs, updatedTimestamp) >= MS2US(1000)) { #ifdef USE_WIND_ESTIMATOR timeSeconds = calculateRemainingFlightTimeBeforeRTH(osdConfig()->estimations_wind_compensation); #else @@ -1645,7 +1645,7 @@ static bool osdDrawSingleElement(uint8_t item) static timeUs_t updatedTimestamp = 0; timeUs_t currentTimeUs = micros(); static int32_t distanceMeters = -1; - if (cmpTimeUs(currentTimeUs, updatedTimestamp) >= 1000000) { + if (cmpTimeUs(currentTimeUs, updatedTimestamp) >= MS2US(1000)) { #ifdef USE_WIND_ESTIMATOR distanceMeters = calculateRemainingDistanceBeforeRTH(osdConfig()->estimations_wind_compensation); #else @@ -1911,7 +1911,7 @@ static bool osdDrawSingleElement(uint8_t item) pitchAngle = DECIDEGREES_TO_RADIANS(secondaryImuState.eulerAngles.values.pitch); } else { rollAngle = DECIDEGREES_TO_RADIANS(attitude.values.roll); - pitchAngle = DECIDEGREES_TO_RADIANS(attitude.values.pitch); + pitchAngle = DECIDEGREES_TO_RADIANS(attitude.values.pitch); } #else rollAngle = DECIDEGREES_TO_RADIANS(attitude.values.roll); @@ -2232,7 +2232,7 @@ static bool osdDrawSingleElement(uint8_t item) if (STATE(GPS_FIX) && gpsSol.groundSpeed > 0) { if (efficiencyTimeDelta >= EFFICIENCY_UPDATE_INTERVAL) { value = pt1FilterApply4(&eFilterState, ((float)getAmperage() / gpsSol.groundSpeed) / 0.0036f, - 1, efficiencyTimeDelta * 1e-6f); + 1, US2S(efficiencyTimeDelta)); efficiencyUpdated = currentTimeUs; } else { @@ -2262,7 +2262,7 @@ static bool osdDrawSingleElement(uint8_t item) if (STATE(GPS_FIX) && gpsSol.groundSpeed > 0) { if (efficiencyTimeDelta >= EFFICIENCY_UPDATE_INTERVAL) { value = pt1FilterApply4(&eFilterState, ((float)getPower() / gpsSol.groundSpeed) / 0.0036f, - 1, efficiencyTimeDelta * 1e-6f); + 1, US2S(efficiencyTimeDelta)); efficiencyUpdated = currentTimeUs; } else { @@ -3296,7 +3296,7 @@ static void osdShowArmed(void) static void osdFilterData(timeUs_t currentTimeUs) { static timeUs_t lastRefresh = 0; - float refresh_dT = cmpTimeUs(currentTimeUs, lastRefresh) * 1e-6; + float refresh_dT = US2S(cmpTimeUs(currentTimeUs, lastRefresh)); GForce = sqrtf(vectorNormSquared(&imuMeasuredAccelBF)) / GRAVITY_MSS; for (uint8_t axis = 0; axis < XYZ_AXIS_COUNT; ++axis) GForceAxis[axis] = imuMeasuredAccelBF.v[axis] / GRAVITY_MSS; diff --git a/src/main/io/osd_dji_hd.c b/src/main/io/osd_dji_hd.c index ed72a973d3..65d76b6e44 100644 --- a/src/main/io/osd_dji_hd.c +++ b/src/main/io/osd_dji_hd.c @@ -112,7 +112,7 @@ }) -/* +/* * DJI HD goggles use MSPv1 compatible with Betaflight 4.1.0 * DJI uses a subset of messages and assume fixed bit positions for flight modes * @@ -685,7 +685,7 @@ static void osdDJIFormatDistanceStr(char *buff, int32_t dist) if (abs(centifeet) < FEET_PER_MILE * 100 / 2) { // Show feet when dist < 0.5mi tfp_sprintf(buff, "%d%s", (int)(centifeet / 100), "FT"); - } + } else { // Show miles when dist >= 0.5mi tfp_sprintf(buff, "%d.%02d%s", (int)(centifeet / (100*FEET_PER_MILE)), @@ -722,7 +722,7 @@ static void osdDJIEfficiencyMahPerKM(char *buff) if (STATE(GPS_FIX) && gpsSol.groundSpeed > 0) { if (efficiencyTimeDelta >= EFFICIENCY_UPDATE_INTERVAL) { value = pt1FilterApply4(&eFilterState, ((float)getAmperage() / gpsSol.groundSpeed) / 0.0036f, - 1, efficiencyTimeDelta * 1e-6f); + 1, US2S(efficiencyTimeDelta)); efficiencyUpdated = currentTimeUs; } @@ -755,7 +755,7 @@ static void djiSerializeCraftNameOverride(sbuf_t *dst, const char * name) if (enabledElements[0] == 'W') { enabledElements += 1; } - + int elemLen = strlen(enabledElements); if (elemLen > 0) { @@ -825,14 +825,14 @@ static void djiSerializeCraftNameOverride(sbuf_t *dst, const char * name) // during a lost aircraft recovery and blinking // will cause it to be missing from some frames. } - } + } else { if (FLIGHT_MODE(NAV_RTH_MODE) || FLIGHT_MODE(NAV_WP_MODE) || navigationIsExecutingAnEmergencyLanding()) { const char *navStateMessage = navigationStateMessage(); if (navStateMessage) { messages[messageCount++] = navStateMessage; } - } + } else if (STATE(FIXED_WING_LEGACY) && (navGetCurrentStateFlags() & NAV_CTL_LAUNCH)) { messages[messageCount++] = "AUTOLAUNCH"; } @@ -994,7 +994,7 @@ static mspResult_e djiProcessMspCommand(mspPacket_t *cmd, mspPacket_t *reply, ms for (int i = 0; i < STICK_CHANNEL_COUNT; i++) { sbufWriteU16(dst, rxGetChannelValue(i)); } - break; + break; case DJI_MSP_RAW_GPS: sbufWriteU8(dst, gpsSol.fixType); diff --git a/src/main/io/smartport_master.c b/src/main/io/smartport_master.c index ade2b00acc..60b9a93e26 100644 --- a/src/main/io/smartport_master.c +++ b/src/main/io/smartport_master.c @@ -550,7 +550,7 @@ void smartportMasterHandle(timeUs_t currentTimeUs) return; } - if (!pollTimestamp || (cmpTimeUs(currentTimeUs, pollTimestamp) > SMARTPORT_POLLING_INTERVAL * 1000)) { + if (!pollTimestamp || (cmpTimeUs(currentTimeUs, pollTimestamp) > MS2US(SMARTPORT_POLLING_INTERVAL))) { if (forwardRequestCount() && (forcedPolledPhyID == -1)) { // forward next payload if there is one in queue and we are not waiting from the response of the previous one smartportMasterForwardNextPayload(); } else { diff --git a/src/main/navigation/navigation_fixedwing.c b/src/main/navigation/navigation_fixedwing.c index 40f2f49b0b..4ec12bc690 100755 --- a/src/main/navigation/navigation_fixedwing.c +++ b/src/main/navigation/navigation_fixedwing.c @@ -171,11 +171,11 @@ void applyFixedWingAltitudeAndThrottleController(timeUs_t currentTimeUs) if ((posControl.flags.estAltStatus >= EST_USABLE)) { if (posControl.flags.verticalPositionDataNew) { - const timeDelta_t deltaMicrosPositionUpdate = currentTimeUs - previousTimePositionUpdate; + const timeDeltaLarge_t deltaMicrosPositionUpdate = currentTimeUs - previousTimePositionUpdate; previousTimePositionUpdate = currentTimeUs; - // Check if last correction was too log ago - ignore this update - if (deltaMicrosPositionUpdate < HZ2US(MIN_POSITION_UPDATE_RATE_HZ)) { + // Check if last correction was not too long ago + if (deltaMicrosPositionUpdate < MAX_POSITION_UPDATE_INTERVAL_US) { updateAltitudeVelocityAndPitchController_FW(deltaMicrosPositionUpdate); } else { @@ -401,10 +401,10 @@ void applyFixedWingPositionController(timeUs_t currentTimeUs) if ((posControl.flags.estPosStatus >= EST_USABLE)) { // If we have new position - update velocity and acceleration controllers if (posControl.flags.horizontalPositionDataNew) { - const timeDelta_t deltaMicrosPositionUpdate = currentTimeUs - previousTimePositionUpdate; + const timeDeltaLarge_t deltaMicrosPositionUpdate = currentTimeUs - previousTimePositionUpdate; previousTimePositionUpdate = currentTimeUs; - if (deltaMicrosPositionUpdate < HZ2US(MIN_POSITION_UPDATE_RATE_HZ)) { + if (deltaMicrosPositionUpdate < MAX_POSITION_UPDATE_INTERVAL_US) { // Calculate virtual position target at a distance of forwardVelocity * HZ2S(POSITION_TARGET_UPDATE_RATE_HZ) // Account for pilot's roll input (move position target left/right at max of max_manual_speed) // POSITION_TARGET_UPDATE_RATE_HZ should be chosen keeping in mind that position target shouldn't be reached until next pos update occurs @@ -440,10 +440,10 @@ int16_t applyFixedWingMinSpeedController(timeUs_t currentTimeUs) if ((posControl.flags.estPosStatus >= EST_USABLE)) { // If we have new position - update velocity and acceleration controllers if (posControl.flags.horizontalPositionDataNew) { - const timeDelta_t deltaMicrosPositionUpdate = currentTimeUs - previousTimePositionUpdate; + const timeDeltaLarge_t deltaMicrosPositionUpdate = currentTimeUs - previousTimePositionUpdate; previousTimePositionUpdate = currentTimeUs; - if (deltaMicrosPositionUpdate < HZ2US(MIN_POSITION_UPDATE_RATE_HZ)) { + if (deltaMicrosPositionUpdate < MAX_POSITION_UPDATE_INTERVAL_US) { float velThrottleBoost = (NAV_FW_MIN_VEL_SPEED_BOOST - posControl.actualState.velXY) * NAV_FW_THROTTLE_SPEED_BOOST_GAIN * US2S(deltaMicrosPositionUpdate); // If we are in the deadband of 50cm/s - don't update speed boost @@ -473,7 +473,7 @@ int16_t applyFixedWingMinSpeedController(timeUs_t currentTimeUs) int16_t fixedWingPitchToThrottleCorrection(int16_t pitch, timeUs_t currentTimeUs) { static timeUs_t previousTimePitchToThrCorr = 0; - const timeDelta_t deltaMicrosPitchToThrCorr = currentTimeUs - previousTimePitchToThrCorr; + const timeDeltaLarge_t deltaMicrosPitchToThrCorr = currentTimeUs - previousTimePitchToThrCorr; previousTimePitchToThrCorr = currentTimeUs; static pt1Filter_t pitchToThrFilterState; diff --git a/src/main/navigation/navigation_multicopter.c b/src/main/navigation/navigation_multicopter.c index a0beb4149c..debec2916c 100755 --- a/src/main/navigation/navigation_multicopter.c +++ b/src/main/navigation/navigation_multicopter.c @@ -209,11 +209,11 @@ static void applyMulticopterAltitudeController(timeUs_t currentTimeUs) // If we have an update on vertical position data - update velocity and accel targets if (posControl.flags.verticalPositionDataNew) { - const timeDelta_t deltaMicrosPositionUpdate = currentTimeUs - previousTimePositionUpdate; + const timeDeltaLarge_t deltaMicrosPositionUpdate = currentTimeUs - previousTimePositionUpdate; previousTimePositionUpdate = currentTimeUs; - // Check if last correction was too log ago - ignore this update - if (deltaMicrosPositionUpdate < HZ2US(MIN_POSITION_UPDATE_RATE_HZ)) { + // Check if last correction was not too long ago + if (deltaMicrosPositionUpdate < MAX_POSITION_UPDATE_INTERVAL_US) { // If we are preparing for takeoff - start with lowset possible climb rate, adjust alt target and make sure throttle doesn't jump if (prepareForTakeoffOnReset) { const navEstimatedPosVel_t * posToUse = navGetCurrentActualPositionAndVelocity(); @@ -477,8 +477,8 @@ static float computeNormalizedVelocity(const float value, const float maxValue) } static float computeVelocityScale( - const float value, - const float maxValue, + const float value, + const float maxValue, const float attenuationFactor, const float attenuationStart, const float attenuationEnd @@ -491,7 +491,7 @@ static float computeVelocityScale( } static void updatePositionAccelController_MC(timeDelta_t deltaMicros, float maxAccelLimit, const float maxSpeed) -{ +{ const float measurementX = navGetCurrentActualPositionAndVelocity()->vel.x; const float measurementY = navGetCurrentActualPositionAndVelocity()->vel.y; @@ -539,15 +539,15 @@ static void updatePositionAccelController_MC(timeDelta_t deltaMicros, float maxA * Scale down dTerm with 2D speed */ const float setpointScale = computeVelocityScale( - setpointXY, - maxSpeed, + setpointXY, + maxSpeed, multicopterPosXyCoefficients.dTermAttenuation, multicopterPosXyCoefficients.dTermAttenuationStart, multicopterPosXyCoefficients.dTermAttenuationEnd ); const float measurementScale = computeVelocityScale( - posControl.actualState.velXY, - maxSpeed, + posControl.actualState.velXY, + maxSpeed, multicopterPosXyCoefficients.dTermAttenuation, multicopterPosXyCoefficients.dTermAttenuationStart, multicopterPosXyCoefficients.dTermAttenuationEnd @@ -560,23 +560,23 @@ static void updatePositionAccelController_MC(timeDelta_t deltaMicros, float maxA // Pre-calculated accelLimit and the logic of navPidApply2 function guarantee that our newAccel won't exceed maxAccelLimit // Thus we don't need to do anything else with calculated acceleration float newAccelX = navPidApply3( - &posControl.pids.vel[X], - setpointX, - measurementX, - US2S(deltaMicros), - accelLimitXMin, - accelLimitXMax, + &posControl.pids.vel[X], + setpointX, + measurementX, + US2S(deltaMicros), + accelLimitXMin, + accelLimitXMax, 0, // Flags 1.0f, // Total gain scale dtermScale // Additional dTerm scale ); float newAccelY = navPidApply3( - &posControl.pids.vel[Y], - setpointY, - measurementY, - US2S(deltaMicros), - accelLimitYMin, - accelLimitYMax, + &posControl.pids.vel[Y], + setpointY, + measurementY, + US2S(deltaMicros), + accelLimitYMin, + accelLimitYMax, 0, // Flags 1.0f, // Total gain scale dtermScale // Additional dTerm scale @@ -638,14 +638,14 @@ static void applyMulticopterPositionController(timeUs_t currentTimeUs) if ((posControl.flags.estPosStatus >= EST_USABLE)) { // If we have new position - update velocity and acceleration controllers if (posControl.flags.horizontalPositionDataNew) { - const timeDelta_t deltaMicrosPositionUpdate = currentTimeUs - previousTimePositionUpdate; + const timeDeltaLarge_t deltaMicrosPositionUpdate = currentTimeUs - previousTimePositionUpdate; previousTimePositionUpdate = currentTimeUs; if (!bypassPositionController) { // Update position controller - if (deltaMicrosPositionUpdate < HZ2US(MIN_POSITION_UPDATE_RATE_HZ)) { + if (deltaMicrosPositionUpdate < MAX_POSITION_UPDATE_INTERVAL_US) { // Get max speed from generic NAV (waypoint specific), don't allow to move slower than 0.5 m/s - const float maxSpeed = getActiveWaypointSpeed(); + const float maxSpeed = getActiveWaypointSpeed(); updatePositionVelocityController_MC(maxSpeed); updatePositionAccelController_MC(deltaMicrosPositionUpdate, NAV_ACCELERATION_XY_MAX, maxSpeed); } @@ -761,11 +761,11 @@ static void applyMulticopterEmergencyLandingController(timeUs_t currentTimeUs) // Normal sensor data if (posControl.flags.verticalPositionDataNew) { - const timeDelta_t deltaMicrosPositionUpdate = currentTimeUs - previousTimePositionUpdate; + const timeDeltaLarge_t deltaMicrosPositionUpdate = currentTimeUs - previousTimePositionUpdate; previousTimePositionUpdate = currentTimeUs; - // Check if last correction was too log ago - ignore this update - if (deltaMicrosPositionUpdate < HZ2US(MIN_POSITION_UPDATE_RATE_HZ)) { + // Check if last correction was not too long ago + if (deltaMicrosPositionUpdate < MAX_POSITION_UPDATE_INTERVAL_US) { updateClimbRateToAltitudeController(-1.0f * navConfig()->general.emerg_descent_rate, ROC_TO_ALT_NORMAL); updateAltitudeVelocityController_MC(deltaMicrosPositionUpdate); updateAltitudeThrottleController_MC(deltaMicrosPositionUpdate); diff --git a/src/main/navigation/navigation_private.h b/src/main/navigation/navigation_private.h index 9848dfefbe..d20bd76d85 100755 --- a/src/main/navigation/navigation_private.h +++ b/src/main/navigation/navigation_private.h @@ -37,6 +37,9 @@ #define INAV_SURFACE_MAX_DISTANCE 40 +#define MAX_POSITION_UPDATE_INTERVAL_US HZ2US(MIN_POSITION_UPDATE_RATE_HZ) // convenience macro +_Static_assert(MAX_POSITION_UPDATE_INTERVAL_US <= TIMEDELTA_MAX, "deltaMicros can overflow!"); + typedef enum { NAV_POS_UPDATE_NONE = 0, NAV_POS_UPDATE_Z = 1 << 1, @@ -197,7 +200,7 @@ typedef enum { NAV_PERSISTENT_ID_WAYPOINT_HOLD_TIME = 35, NAV_PERSISTENT_ID_RTH_HOVER_ABOVE_HOME = 36, - NAV_PERSISTENT_ID_WAYPOINT_HOVER_ABOVE_HOME = 37, + NAV_PERSISTENT_ID_WAYPOINT_HOVER_ABOVE_HOME = 37, } navigationPersistentId_e; diff --git a/src/main/navigation/navigation_rover_boat.c b/src/main/navigation/navigation_rover_boat.c index 1f0a01669a..40c075107b 100644 --- a/src/main/navigation/navigation_rover_boat.c +++ b/src/main/navigation/navigation_rover_boat.c @@ -71,11 +71,11 @@ void applyRoverBoatPositionController(timeUs_t currentTimeUs) static timeUs_t previousTimePositionUpdate; // Occurs @ GPS update rate static timeUs_t previousTimeUpdate; // Occurs @ looptime rate - const timeDelta_t deltaMicros = currentTimeUs - previousTimeUpdate; + const timeDeltaLarge_t deltaMicros = currentTimeUs - previousTimeUpdate; previousTimeUpdate = currentTimeUs; // If last position update was too long in the past - ignore it (likely restarting altitude controller) - if (deltaMicros > HZ2US(MIN_POSITION_UPDATE_RATE_HZ)) { + if (deltaMicros > MAX_POSITION_UPDATE_INTERVAL_US) { previousTimeUpdate = currentTimeUs; previousTimePositionUpdate = currentTimeUs; resetFixedWingPositionController(); @@ -86,10 +86,10 @@ void applyRoverBoatPositionController(timeUs_t currentTimeUs) if ((posControl.flags.estPosStatus >= EST_USABLE)) { // If we have new position - update velocity and acceleration controllers if (posControl.flags.horizontalPositionDataNew) { - const timeDelta_t deltaMicrosPositionUpdate = currentTimeUs - previousTimePositionUpdate; + const timeDeltaLarge_t deltaMicrosPositionUpdate = currentTimeUs - previousTimePositionUpdate; previousTimePositionUpdate = currentTimeUs; - if (deltaMicrosPositionUpdate < HZ2US(MIN_POSITION_UPDATE_RATE_HZ)) { + if (deltaMicrosPositionUpdate < MAX_POSITION_UPDATE_INTERVAL_US) { update2DPositionHeadingController(currentTimeUs, deltaMicrosPositionUpdate); } else { resetFixedWingPositionController(); @@ -143,4 +143,4 @@ void applyRoverBoatNavigationController(navigationFSMStateFlags_t navStateFlags, } } -#endif \ No newline at end of file +#endif diff --git a/src/main/rx/fport2.c b/src/main/rx/fport2.c index de2b7d7062..55d4f4349e 100644 --- a/src/main/rx/fport2.c +++ b/src/main/rx/fport2.c @@ -531,7 +531,7 @@ static uint8_t frameStatus(rxRuntimeConfig_t *rxRuntimeConfig) } #endif - if (frameReceivedTimestamp && (cmpTimeUs(currentTimeUs, frameReceivedTimestamp) > FPORT2_MAX_TELEMETRY_AGE_MS * 1000)) { + if (frameReceivedTimestamp && (cmpTimeUs(currentTimeUs, frameReceivedTimestamp) > MS2US(FPORT2_MAX_TELEMETRY_AGE_MS))) { lqTrackerSet(rxRuntimeConfig->lqTracker, 0); frameReceivedTimestamp = 0; } @@ -593,7 +593,7 @@ static bool processFrame(const rxRuntimeConfig_t *rxRuntimeConfig) } - timeUs_t otaResponseTime = cmpTimeUs(micros(), otaFrameEndTimestamp); + timeDelta_t otaResponseTime = cmpTimeUs(micros(), otaFrameEndTimestamp); if (!firmwareUpdateError && (otaResponseTime <= otaMaxResponseTime)) { // We can answer in time (firmwareUpdateStore can take time because it might need to erase flash) writeUplinkFramePhyID(downlinkPhyID, otaResponsePayload); DEBUG_SET(DEBUG_FPORT, DEBUG_FPORT2_OTA_FRAME_RESPONSE_TIME, otaResponseTime); diff --git a/src/main/rx/ghst.c b/src/main/rx/ghst.c index 54d3b6f0f5..4c620de3e5 100644 --- a/src/main/rx/ghst.c +++ b/src/main/rx/ghst.c @@ -119,7 +119,7 @@ void ghstRxWriteTelemetryData(const void *data, int len) } void ghstRxSendTelemetryData(void) -{ +{ // if there is telemetry data to write if (telemetryBufLen > 0) { serialWriteBuf(serialPort, telemetryBuf, telemetryBufLen); @@ -178,7 +178,7 @@ STATIC_UNIT_TESTED void ghstDataReceive(uint16_t c, void *data) static bool shouldSendTelemetryFrame(void) { const timeUs_t now = micros(); - const timeUs_t timeSinceRxFrameEndUs = cmpTimeUs(now, ghstRxFrameEndAtUs); + const timeDelta_t timeSinceRxFrameEndUs = cmpTimeUs(now, ghstRxFrameEndAtUs); return telemetryBufLen > 0 && timeSinceRxFrameEndUs > GHST_RX_TO_TELEMETRY_MIN_US && timeSinceRxFrameEndUs < GHST_RX_TO_TELEMETRY_MAX_US; } @@ -191,7 +191,7 @@ static void ghstIdle(void) static void ghstUpdateFailsafe(unsigned pktIdx) { - // pktIdx is an offset of RC channel packet, + // pktIdx is an offset of RC channel packet, // We'll track arrival time of each of the frame types we ever saw arriving from this receiver if (pktIdx < GHST_UL_RC_CHANS_FRAME_COUNT) { if (ghstFsTracker[pktIdx].onTimePacketCounter < GHST_RC_FRAME_COUNT_THRESHOLD) { @@ -282,7 +282,7 @@ static bool ghstProcessFrame(const rxRuntimeConfig_t *rxRuntimeConfig) int startIdx = 0; if ( - ghstValidatedFrame.frame.type >= GHST_UL_RC_CHANS_HS4_FIRST && + ghstValidatedFrame.frame.type >= GHST_UL_RC_CHANS_HS4_FIRST && ghstValidatedFrame.frame.type <= GHST_UL_RC_CHANS_HS4_LAST ) { const ghstPayloadPulses_t* const rcChannels = (ghstPayloadPulses_t*)&ghstValidatedFrame.frame.payload; @@ -300,7 +300,7 @@ static bool ghstProcessFrame(const rxRuntimeConfig_t *rxRuntimeConfig) case GHST_UL_RC_CHANS_HS4_RSSI: { const ghstPayloadPulsesRSSI_t* const rssiFrame = (ghstPayloadPulsesRSSI_t*)&ghstValidatedFrame.frame.payload; lqTrackerSet(rxRuntimeConfig->lqTracker, scaleRange(constrain(rssiFrame->lq, 0, 100), 0, 100, 0, RSSI_MAX_VALUE)); - + break; } diff --git a/src/main/rx/sumd.c b/src/main/rx/sumd.c index 1e89705e37..2c27a1e95d 100644 --- a/src/main/rx/sumd.c +++ b/src/main/rx/sumd.c @@ -79,7 +79,7 @@ static void sumdDataReceive(uint16_t c, void *rxCallbackData) static uint8_t sumdIndex; sumdTime = micros(); - if (cmpTimeUs(sumdTime, sumdTimeLast) > 4000) + if (cmpTimeUs(sumdTime, sumdTimeLast) > MS2US(4)) sumdIndex = 0; sumdTimeLast = sumdTime; diff --git a/src/main/rx/sumh.c b/src/main/rx/sumh.c index 6f9af2ec2b..33ffa0e07d 100644 --- a/src/main/rx/sumh.c +++ b/src/main/rx/sumh.c @@ -69,7 +69,7 @@ static void sumhDataReceive(uint16_t c, void *rxCallbackData) sumhTime = micros(); sumhTimeInterval = cmpTimeUs(sumhTime, sumhTimeLast); sumhTimeLast = sumhTime; - if (sumhTimeInterval > 5000) { + if (sumhTimeInterval > MS2US(5)) { sumhFramePosition = 0; } diff --git a/src/main/sensors/battery.c b/src/main/sensors/battery.c index 1b62c01e9a..b0f6fe060d 100644 --- a/src/main/sensors/battery.c +++ b/src/main/sensors/battery.c @@ -558,7 +558,7 @@ void sagCompensatedVBatUpdate(timeUs_t currentTime, timeUs_t timeDelta) } else { - if (cmpTimeUs(currentTime, recordTimestamp) > 500000) + if (cmpTimeUs(currentTime, recordTimestamp) > MS2US(500)) recordTimestamp = 0; if (!recordTimestamp) { @@ -575,7 +575,7 @@ void sagCompensatedVBatUpdate(timeUs_t currentTime, timeUs_t timeDelta) if (impedanceFilterState.state) { pt1FilterSetTimeConstant(&impedanceFilterState, impedanceSampleCount > IMPEDANCE_STABLE_SAMPLE_COUNT_THRESH ? 1.2 : 0.5); - pt1FilterApply3(&impedanceFilterState, impedanceSample, timeDelta * 1e-6f); + pt1FilterApply3(&impedanceFilterState, impedanceSample, US2S(timeDelta)); } else { pt1FilterReset(&impedanceFilterState, impedanceSample); } @@ -589,7 +589,7 @@ void sagCompensatedVBatUpdate(timeUs_t currentTime, timeUs_t timeDelta) uint16_t sagCompensatedVBatSample = MIN(batteryFullVoltage, vbat + (int32_t)powerSupplyImpedance * amperage / 1000); pt1FilterSetTimeConstant(&sagCompVBatFilterState, sagCompensatedVBatSample < pt1FilterGetLastOutput(&sagCompVBatFilterState) ? 40 : 500); - sagCompensatedVBat = lrintf(pt1FilterApply3(&sagCompVBatFilterState, sagCompensatedVBatSample, timeDelta * 1e-6f)); + sagCompensatedVBat = lrintf(pt1FilterApply3(&sagCompVBatFilterState, sagCompensatedVBatSample, US2S(timeDelta))); } DEBUG_SET(DEBUG_SAG_COMP_VOLTAGE, 0, powerSupplyImpedance); diff --git a/src/main/sensors/rangefinder.c b/src/main/sensors/rangefinder.c index 0129cdfb3b..b251635b12 100644 --- a/src/main/sensors/rangefinder.c +++ b/src/main/sensors/rangefinder.c @@ -217,7 +217,7 @@ timeDelta_t rangefinderUpdate(void) rangefinder.dev.update(&rangefinder.dev); } - return rangefinder.dev.delayMs * 1000; // to microseconds + return MS2US(rangefinder.dev.delayMs); } /** From 51f9c4080e6ef568ff73afcb488d9501a68a65e6 Mon Sep 17 00:00:00 2001 From: "Pawel Spychalski (DzikuVx)" Date: Thu, 8 Apr 2021 14:41:03 +0200 Subject: [PATCH 135/286] Fetch calibartion systus --- .../drivers/accgyro/accgyro_bno055_serial.c | 17 ++++++----- src/main/flight/secondary_imu.c | 28 +++++++++++++------ 2 files changed, 29 insertions(+), 16 deletions(-) diff --git a/src/main/drivers/accgyro/accgyro_bno055_serial.c b/src/main/drivers/accgyro/accgyro_bno055_serial.c index 7955c9d7e8..f69ab322ce 100644 --- a/src/main/drivers/accgyro/accgyro_bno055_serial.c +++ b/src/main/drivers/accgyro/accgyro_bno055_serial.c @@ -55,6 +55,7 @@ typedef enum { typedef enum { BNO055_DATA_TYPE_NONE, BNO055_DATA_TYPE_EULER, + BNO055_DATA_TYPE_CALIBRATION_STATS, } bno055DataType_e; static uint8_t bno055ProtocolState = BNO055_RECEIVE_IDLE; @@ -83,6 +84,8 @@ static void bno055SerialRead(const uint8_t reg, const uint8_t len) { void bno055SerialDataReceive(uint16_t c, void *data) { + UNUSED(data); + const uint8_t incoming = (uint8_t) c; //Failsafe for stuck frames @@ -117,6 +120,11 @@ void bno055SerialDataReceive(uint16_t c, void *data) { secondaryImuState.eulerAngles.raw[1] = ((int16_t)((receiveBuffer[5] << 8) | receiveBuffer[4])) / -1.6f; //Pitch has to be reversed to match INAV notation secondaryImuState.eulerAngles.raw[2] = ((int16_t)((receiveBuffer[1] << 8) | receiveBuffer[0])) / 1.6f; secondaryImuProcess(); + } else if (bno055DataType == BNO055_DATA_TYPE_CALIBRATION_STATS) { + secondaryImuState.calibrationStatus.mag = receiveBuffer[0] & 0b00000011; + secondaryImuState.calibrationStatus.acc = (receiveBuffer[0] >> 2) & 0b00000011; + secondaryImuState.calibrationStatus.gyr = (receiveBuffer[0] >> 4) & 0b00000011; + secondaryImuState.calibrationStatus.sys = (receiveBuffer[0] >> 6) & 0b00000011; } bno055DataType = BNO055_DATA_TYPE_NONE; @@ -163,12 +171,6 @@ bool bno055SerialInit(bno055CalibrationData_t calibrationData, bool setCalibrati bno055SerialWrite(BNO055_ADDR_OPR_MODE, BNO055_OPR_MODE_NDOF); //Set power mode NORMAL delay(25); - - // DEBUG_SET(DEBUG_IMU2, 0, bno055FrameType); - // DEBUG_SET(DEBUG_IMU2, 1, receiveBuffer[0]); - // DEBUG_SET(DEBUG_IMU2, 2, bno055ProtocolState); - // DEBUG_SET(DEBUG_IMU2, 3, bno055FrameIndex); - return true; } @@ -178,7 +180,8 @@ void bno055SerialFetchEulerAngles() { } bno055CalibStat_t bno055SerialGetCalibStat(void) { - + bno055DataType = BNO055_DATA_TYPE_CALIBRATION_STATS; + bno055SerialRead(BNO055_ADDR_CALIB_STAT, 1); } bno055CalibrationData_t bno055SerialGetCalibrationData(void) { diff --git a/src/main/flight/secondary_imu.c b/src/main/flight/secondary_imu.c index 35b087c2a0..cd835cd0db 100644 --- a/src/main/flight/secondary_imu.c +++ b/src/main/flight/secondary_imu.c @@ -147,14 +147,17 @@ void secondaryImuProcess(void) { DEBUG_SET(DEBUG_IMU2, 1, secondaryImuState.eulerAngles.values.pitch); DEBUG_SET(DEBUG_IMU2, 2, secondaryImuState.eulerAngles.values.yaw); - // DEBUG_SET(DEBUG_IMU2, 3, secondaryImuState.calibrationStatus.mag); - // DEBUG_SET(DEBUG_IMU2, 4, secondaryImuState.calibrationStatus.gyr); - // DEBUG_SET(DEBUG_IMU2, 5, secondaryImuState.calibrationStatus.acc); - // DEBUG_SET(DEBUG_IMU2, 6, secondaryImuState.magDeclination); + DEBUG_SET(DEBUG_IMU2, 3, secondaryImuState.calibrationStatus.mag); + DEBUG_SET(DEBUG_IMU2, 4, secondaryImuState.calibrationStatus.gyr); + DEBUG_SET(DEBUG_IMU2, 5, secondaryImuState.calibrationStatus.acc); + DEBUG_SET(DEBUG_IMU2, 6, secondaryImuState.magDeclination); } void taskSecondaryImu(timeUs_t currentTimeUs) { + static uint8_t tick = 0; + tick++; + if (!secondaryImuState.active) { return; @@ -169,17 +172,24 @@ void taskSecondaryImu(timeUs_t currentTimeUs) secondaryImuProcess(); /* - * Every 10 cycles fetch current calibration state + * Every 2 seconds fetch current calibration state */ - static uint8_t tick = 0; - tick++; - if (tick == 10) + if (tick == 20) { secondaryImuState.calibrationStatus = bno055GetCalibStat(); tick = 0; } } else if (secondaryImuConfig()->hardwareType == SECONDARY_IMU_BNO055_SERIAL) { - bno055SerialFetchEulerAngles(); + /* + * Every 2 seconds fetch current calibration state + */ + if (tick == 100) + { + bno055SerialGetCalibStat(); + tick = 0; + } else { + bno055SerialFetchEulerAngles(); + } } } From 95e1fe61d44a79f10d4f654962a0099abe58b473 Mon Sep 17 00:00:00 2001 From: Alexander van Saase Date: Thu, 8 Apr 2021 14:49:09 +0200 Subject: [PATCH 136/286] Cell detection fix --- docs/Settings.md | 2 +- src/main/fc/settings.yaml | 2 +- src/main/sensors/battery.c | 6 ++++-- 3 files changed, 6 insertions(+), 4 deletions(-) diff --git a/docs/Settings.md b/docs/Settings.md index 96dc9efe9c..f6afd27afa 100644 --- a/docs/Settings.md +++ b/docs/Settings.md @@ -514,7 +514,7 @@ | tz_automatic_dst | OFF | Automatically add Daylight Saving Time to the GPS time when needed or simply ignore it. Includes presets for EU and the USA - if you live outside these areas it is suggested to manage DST manually via `tz_offset`. | | tz_offset | 0 | Time zone offset from UTC, in minutes. This is applied to the GPS time for logging and time-stamping of Blackbox logs | | vbat_adc_channel | _target default_ | ADC channel to use for battery voltage sensor. Defaults to board VBAT input (if available). 0 = disabled | -| vbat_cell_detect_voltage | 425 | Maximum voltage per cell, used for auto-detecting the number of cells of the battery in 0.01V units, default is 4.30V. | +| vbat_cell_detect_voltage | 425 | Maximum voltage per cell, used for auto-detecting the number of cells of the battery in 0.01V units. | | vbat_max_cell_voltage | 420 | Maximum voltage per cell in 0.01V units, default is 4.20V | | vbat_meter_type | ADC | Vbat voltage source. Possible values: `NONE`, `ADC`, `ESC`. `ESC` required ESC telemetry enebled and running | | vbat_min_cell_voltage | 330 | Minimum voltage per cell, this triggers battery out alarms, in 0.01V units, default is 330 (3.3V) | diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 264a1abfcb..f5b2dc3b5a 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -1034,7 +1034,7 @@ groups: min: 0 max: 12 - name: vbat_cell_detect_voltage - description: "Maximum voltage per cell, used for auto-detecting the number of cells of the battery in 0.01V units, default is 4.30V." + description: "Maximum voltage per cell, used for auto-detecting the number of cells of the battery in 0.01V units." default_value: 425 field: voltage.cellDetect condition: USE_ADC diff --git a/src/main/sensors/battery.c b/src/main/sensors/battery.c index 1b62c01e9a..fe54342561 100644 --- a/src/main/sensors/battery.c +++ b/src/main/sensors/battery.c @@ -274,8 +274,10 @@ void batteryUpdate(timeUs_t timeDelta) batteryCellCount = currentBatteryProfile->cells; else { batteryCellCount = (vbat / currentBatteryProfile->voltage.cellDetect) + 1; - // Assume there are no 7S, 9S and 11S batteries so round up to 8S, 10S and 12S - batteryCellCount = ((batteryCellCount > 6) && (batteryCellCount & 2) == 0) ? batteryCellCount : batteryCellCount + 1; + // Assume there are no 7S, 9S and 11S batteries so round up to 8S, 10S and 12S respectively + if (batteryCellCount == 7 || batteryCellCount == 9 || batteryCellCount == 11) { + batteryCellCount += 1; + } batteryCellCount = MIN(batteryCellCount, 12); } From 651ff1a9d04915b4ffa96607c596795ed58a3712 Mon Sep 17 00:00:00 2001 From: Alexander van Saase Date: Thu, 8 Apr 2021 15:09:23 +0200 Subject: [PATCH 137/286] minor docs change to trigger GH checks --- src/main/fc/settings.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index f5b2dc3b5a..c389ab422d 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -1027,7 +1027,7 @@ groups: value_type: BATTERY_CONFIG_VALUE members: - name: bat_cells - description: "Number of cells of the battery (0 = autodetect), see battery documentation. 7S, 9S and 11S batteries cannot be autodetected." + description: "Number of cells of the battery (0 = auto-detect), see battery documentation. 7S, 9S and 11S batteries cannot be auto-detected." default_value: 0 field: cells condition: USE_ADC From d878f199706f508428132930e6eeb28843548998 Mon Sep 17 00:00:00 2001 From: Alexander van Saase Date: Thu, 8 Apr 2021 15:21:47 +0200 Subject: [PATCH 138/286] docs --- docs/Settings.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/docs/Settings.md b/docs/Settings.md index f6afd27afa..a9b79f5abf 100644 --- a/docs/Settings.md +++ b/docs/Settings.md @@ -41,7 +41,7 @@ | baro_cal_tolerance | 150 | Baro calibration tolerance in cm. The default should allow the noisiest baro to complete calibration [cm]. | | baro_hardware | AUTO | Selection of baro hardware. See Wiki Sensor auto detect and hardware failure detection for more info | | baro_median_filter | ON | 3-point median filtering for barometer readouts. No reason to change this setting | -| bat_cells | 0 | Number of cells of the battery (0 = autodetect), see battery documentation. 7S, 9S and 11S batteries cannot be autodetected. | +| bat_cells | 0 | Number of cells of the battery (0 = auto-detect), see battery documentation. 7S, 9S and 11S batteries cannot be auto-detected. | | bat_voltage_src | RAW | Chose between raw and sag compensated battery voltage to use for battery alarms and telemetry. Possible values are `RAW` and `SAG_COMP` | | battery_capacity | 0 | Set the battery capacity in mAh or mWh (see `battery_capacity_unit`). Used to calculate the remaining battery capacity. | | battery_capacity_critical | 0 | If the remaining battery capacity goes below this threshold the battery is considered empty and the beeper will emit long beeps. | From d19837d32092d44f43635dea7d8a33e0c34ec67e Mon Sep 17 00:00:00 2001 From: "Pawel Spychalski (DzikuVx)" Date: Thu, 8 Apr 2021 15:38:57 +0200 Subject: [PATCH 139/286] Read calibration data from BNO055 Serial --- .../drivers/accgyro/accgyro_bno055_serial.c | 39 +++++++++++++++++-- .../drivers/accgyro/accgyro_bno055_serial.h | 2 +- src/main/flight/secondary_imu.c | 10 ++++- 3 files changed, 46 insertions(+), 5 deletions(-) diff --git a/src/main/drivers/accgyro/accgyro_bno055_serial.c b/src/main/drivers/accgyro/accgyro_bno055_serial.c index f69ab322ce..337f1198ab 100644 --- a/src/main/drivers/accgyro/accgyro_bno055_serial.c +++ b/src/main/drivers/accgyro/accgyro_bno055_serial.c @@ -36,8 +36,7 @@ #include "flight/secondary_imu.h" static serialPort_t * bno055SerialPort = NULL; - -static uint8_t receiveBuffer[16]; +static uint8_t receiveBuffer[22]; typedef enum { BNO055_RECEIVE_IDLE, @@ -174,18 +173,52 @@ bool bno055SerialInit(bno055CalibrationData_t calibrationData, bool setCalibrati return true; } +/* + * This function is non-blocking and response will be processed by bno055SerialDataReceive + */ void bno055SerialFetchEulerAngles() { bno055DataType = BNO055_DATA_TYPE_EULER; bno055SerialRead(BNO055_ADDR_EUL_YAW_LSB, 6); } -bno055CalibStat_t bno055SerialGetCalibStat(void) { +/* + * This function is non-blocking and response will be processed by bno055SerialDataReceive + */ +void bno055SerialGetCalibStat(void) { bno055DataType = BNO055_DATA_TYPE_CALIBRATION_STATS; bno055SerialRead(BNO055_ADDR_CALIB_STAT, 1); } +/* + * This function is blocking and should not be used during flight conditions! + */ bno055CalibrationData_t bno055SerialGetCalibrationData(void) { + bno055CalibrationData_t data; + + bno055SerialWrite(BNO055_ADDR_OPR_MODE, BNO055_OPR_MODE_CONFIG); + delay(25); + + bno055SerialRead(BNO055_ADDR_ACC_OFFSET_X_LSB, 22); + delay(50); + + uint8_t bufferBit = 0; + for (uint8_t sensorIndex = 0; sensorIndex < 3; sensorIndex++) + { + for (uint8_t axisIndex = 0; axisIndex < 3; axisIndex++) + { + data.offset[sensorIndex][axisIndex] = (int16_t)((receiveBuffer[bufferBit + 1] << 8) | receiveBuffer[bufferBit]); + bufferBit += 2; + } + } + + data.radius[ACC] = (int16_t)((receiveBuffer[19] << 8) | receiveBuffer[18]); + data.radius[MAG] = (int16_t)((receiveBuffer[21] << 8) | receiveBuffer[20]); + + bno055SerialWrite(BNO055_ADDR_OPR_MODE, BNO055_OPR_MODE_NDOF); + delay(25); + + return data; } void bno055SerialSetCalibrationData(bno055CalibrationData_t data) { diff --git a/src/main/drivers/accgyro/accgyro_bno055_serial.h b/src/main/drivers/accgyro/accgyro_bno055_serial.h index b73a3534d3..22686f0b26 100644 --- a/src/main/drivers/accgyro/accgyro_bno055_serial.h +++ b/src/main/drivers/accgyro/accgyro_bno055_serial.h @@ -29,6 +29,6 @@ bool bno055SerialInit(bno055CalibrationData_t calibrationData, bool setCalibration); void bno055SerialFetchEulerAngles(void); -bno055CalibStat_t bno055SerialGetCalibStat(void); +void bno055SerialGetCalibStat(void); bno055CalibrationData_t bno055SerialGetCalibrationData(void); void bno055SerialSetCalibrationData(bno055CalibrationData_t data); \ No newline at end of file diff --git a/src/main/flight/secondary_imu.c b/src/main/flight/secondary_imu.c index cd835cd0db..b8e5a20160 100644 --- a/src/main/flight/secondary_imu.c +++ b/src/main/flight/secondary_imu.c @@ -194,7 +194,15 @@ void taskSecondaryImu(timeUs_t currentTimeUs) } void secondaryImuFetchCalibration(void) { - bno055CalibrationData_t calibrationData = bno055GetCalibrationData(); + bno055CalibrationData_t calibrationData; + + if (secondaryImuConfig()->hardwareType == SECONDARY_IMU_BNO055) { + calibrationData = bno055GetCalibrationData(); + } else if (secondaryImuConfig()->hardwareType == SECONDARY_IMU_BNO055_SERIAL) { + calibrationData = bno055SerialGetCalibrationData(); + } else { + return; + } secondaryImuConfigMutable()->calibrationOffsetAcc[X] = calibrationData.offset[ACC][X]; secondaryImuConfigMutable()->calibrationOffsetAcc[Y] = calibrationData.offset[ACC][Y]; From fb67fb1b883bd4136edf809302daad6950e23634 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Niccol=C3=B2=20Maggioni?= Date: Thu, 8 Apr 2021 16:11:22 +0200 Subject: [PATCH 140/286] ZEEZF7V2 target --- src/main/target/ZEEZF7V2/CMakeLists.txt | 1 + src/main/target/ZEEZF7V2/target.c | 40 ++++++ src/main/target/ZEEZF7V2/target.h | 156 ++++++++++++++++++++++++ 3 files changed, 197 insertions(+) create mode 100644 src/main/target/ZEEZF7V2/CMakeLists.txt create mode 100755 src/main/target/ZEEZF7V2/target.c create mode 100755 src/main/target/ZEEZF7V2/target.h diff --git a/src/main/target/ZEEZF7V2/CMakeLists.txt b/src/main/target/ZEEZF7V2/CMakeLists.txt new file mode 100644 index 0000000000..fa01c1d7a7 --- /dev/null +++ b/src/main/target/ZEEZF7V2/CMakeLists.txt @@ -0,0 +1 @@ +target_stm32f722xe(ZEEZF7V2) diff --git a/src/main/target/ZEEZF7V2/target.c b/src/main/target/ZEEZF7V2/target.c new file mode 100755 index 0000000000..4178c3fa6c --- /dev/null +++ b/src/main/target/ZEEZF7V2/target.c @@ -0,0 +1,40 @@ +/* +* This file is part of Cleanflight. +* +* Cleanflight is free software: you can redistribute it and/or modify +* it under the terms of the GNU General Public License as published by +* the Free Software Foundation, either version 3 of the License, or +* (at your option) any later version. +* +* Cleanflight is distributed in the hope that it will be useful, +* but WITHOUT ANY WARRANTY; without even the implied warranty of +* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +* GNU General Public License for more details. +* +* You should have received a copy of the GNU General Public License +* along with Cleanflight. If not, see . +*/ + +#include + +#include "platform.h" + +#include "drivers/bus.h" +#include "drivers/io.h" +#include "drivers/pwm_mapping.h" +#include "drivers/timer.h" + +const timerHardware_t timerHardware[] = { + DEF_TIM(TIM4, CH2, PB7, TIM_USE_MC_MOTOR, 0, 0), // S1 + DEF_TIM(TIM4, CH1, PB6, TIM_USE_MC_MOTOR, 0, 0), // S2 + DEF_TIM(TIM3, CH1, PB4, TIM_USE_MC_MOTOR, 0, 0), // S3 + DEF_TIM(TIM2, CH2, PB3, TIM_USE_MC_MOTOR, 0, 0), // S4 + DEF_TIM(TIM3, CH3, PC8, TIM_USE_MC_MOTOR, 0, 0), // S5 + DEF_TIM(TIM3, CH2, PC7, TIM_USE_MC_MOTOR, 0, 0), // S6 + DEF_TIM(TIM8, CH1, PC6, TIM_USE_MC_MOTOR, 0, 0), // S7 + DEF_TIM(TIM3, CH4, PB1, TIM_USE_MC_MOTOR, 0, 0), // S8 + + DEF_TIM(TIM1, CH2N, PB0, TIM_USE_LED , 0, 0), // LED STRIP +}; + +const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]); diff --git a/src/main/target/ZEEZF7V2/target.h b/src/main/target/ZEEZF7V2/target.h new file mode 100755 index 0000000000..4fede7e11c --- /dev/null +++ b/src/main/target/ZEEZF7V2/target.h @@ -0,0 +1,156 @@ +/* + * This file is part of Cleanflight. + * + * Cleanflight is free software: you can redistribute it and/or modify + * it under the terms of the GNU General Public License as published by + * the Free Software Foundation, either version 3 of the License, or + * (at your option) any later version. + * + * Cleanflight is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + * + * You should have received a copy of the GNU General Public License + * along with Cleanflight. If not, see . + */ + +#pragma once + +#define TARGET_BOARD_IDENTIFIER "ZEF7V2" + +#define USBD_PRODUCT_STRING "ZEEZF7V2" + +#define LED0 PC14 +#define LED1 PC15 + +#define BEEPER PB2 +#define BEEPER_INVERTED + +// *************** Gyro & ACC ********************** +#define USE_SPI + +#define USE_SPI_DEVICE_1 + +#define SPI1_SCK_PIN PA5 +#define SPI1_MISO_PIN PA6 +#define SPI1_MOSI_PIN PA7 + +#define MPU6000_CS_PIN PA4 +#define MPU6000_SPI_BUS BUS_SPI1 + +#define USE_EXTI +#define GYRO_INT_EXTI PC4 +#define USE_MPU_DATA_READY_SIGNAL + +#define USE_IMU_MPU6000 +#define IMU_MPU6000_ALIGN CW0_DEG + +// *************** I2C/Baro/Mag ********************* + +// Onboard BMP280 Baro --- I2C1 +#define USE_I2C +#define USE_BARO +#define USE_BARO_BMP280 +#define BARO_I2C_BUS BUS_I2C1 + +#define USE_I2C_DEVICE_1 +#define I2C1_SCL PB8 +#define I2C1_SDA PB9 + +// External I2C Pads -- I2C2 +#define USE_I2C_DEVICE_2 +#define I2C2_SCL PA8 +#define I2C2_SDA PC9 + +#define USE_MAG +#define MAG_I2C_BUS BUS_I2C2 +#define USE_MAG_HMC5883 +#define USE_MAG_QMC5883 +#define USE_MAG_IST8310 +#define USE_MAG_IST8308 +#define USE_MAG_MAG3110 +#define USE_MAG_LIS3MDL +#define USE_MAG_AK8975 + +// *************** Flash **************************** + +#define USE_SPI_DEVICE_2 + +#define SPI2_SCK_PIN PB13 +#define SPI2_MISO_PIN PC2 +#define SPI2_MOSI_PIN PC3 + +#define USE_FLASHFS +#define USE_FLASH_M25P16 +#define M25P16_SPI_BUS BUS_SPI2 +#define M25P16_CS_PIN PB12 +#define ENABLE_BLACKBOX_LOGGING_ON_SPIFLASH_BY_DEFAULT + +// *************** OSD ***************************** + +#define USE_SPI_DEVICE_3 +#define SPI3_SCK_PIN PC10 +#define SPI3_MISO_PIN PC11 +#define SPI3_MOSI_PIN PB5 + +#define USE_OSD +#define USE_MAX7456 +#define MAX7456_SPI_BUS BUS_SPI3 +#define MAX7456_CS_PIN PA15 + +// *************** UART ***************************** +#define USE_VCP + +#define USE_UART1 +#define UART1_RX_PIN PA10 +#define UART1_TX_PIN PA9 + +#define USE_UART2 +#define UART2_RX_PIN PA3 +#define UART2_TX_PIN PA2 + +#define USE_UART3 +#define UART3_RX_PIN PB11 +#define UART3_TX_PIN PB10 + +#define USE_UART4 +#define UART4_RX_PIN PA1 +#define UART4_TX_PIN PA0 + +#define USE_UART5 +#define UART5_RX_PIN PD2 +#define UART5_TX_PIN PC12 + +#define SERIAL_PORT_COUNT 6 + +#define DEFAULT_RX_TYPE RX_TYPE_SERIAL +#define SERIALRX_PROVIDER SERIALRX_SBUS +#define SERIALRX_UART SERIAL_PORT_USART4 + +// *************** ADC ***************************** +#define USE_ADC +#define ADC_INSTANCE ADC1 +#define ADC1_DMA_STREAM DMA2_Stream0 +#define ADC_CHANNEL_1_PIN PC0 +#define ADC_CHANNEL_2_PIN PC1 +#define VBAT_ADC_CHANNEL ADC_CHN_1 +#define CURRENT_METER_ADC_CHANNEL ADC_CHN_2 + +#define DEFAULT_FEATURES (FEATURE_TX_PROF_SEL | FEATURE_CURRENT_METER | FEATURE_TELEMETRY | FEATURE_VBAT | FEATURE_OSD | FEATURE_LED_STRIP) +#define CURRENT_METER_SCALE 250 + +#define USE_LED_STRIP +#define WS2811_PIN PB0 + +#define USE_SERIAL_4WAY_BLHELI_INTERFACE + +#define TARGET_IO_PORTA 0xffff +#define TARGET_IO_PORTB 0xffff +#define TARGET_IO_PORTC 0xffff +#define TARGET_IO_PORTD (BIT(2)) + +#define MAX_PWM_OUTPUT_PORTS 8 +#define USE_DSHOT +#define USE_ESC_SENSOR +#define USE_SERIALSHOT From c7b9637a0b8909799bf512c4f0056c100bc32fb1 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Niccol=C3=B2=20Maggioni?= Date: Thu, 8 Apr 2021 17:23:01 +0200 Subject: [PATCH 141/286] Merge ZEEZF7 and ZEEZF7V2 targets --- src/main/target/ZEEZF7/CMakeLists.txt | 1 + src/main/target/ZEEZF7/config.c | 2 + src/main/target/ZEEZF7/target.c | 11 ++ src/main/target/ZEEZF7/target.h | 115 ++++++++++++++--- src/main/target/ZEEZF7V2/CMakeLists.txt | 1 - src/main/target/ZEEZF7V2/target.c | 40 ------ src/main/target/ZEEZF7V2/target.h | 156 ------------------------ 7 files changed, 115 insertions(+), 211 deletions(-) delete mode 100644 src/main/target/ZEEZF7V2/CMakeLists.txt delete mode 100755 src/main/target/ZEEZF7V2/target.c delete mode 100755 src/main/target/ZEEZF7V2/target.h diff --git a/src/main/target/ZEEZF7/CMakeLists.txt b/src/main/target/ZEEZF7/CMakeLists.txt index 513e023060..eb14cb8802 100644 --- a/src/main/target/ZEEZF7/CMakeLists.txt +++ b/src/main/target/ZEEZF7/CMakeLists.txt @@ -1 +1,2 @@ target_stm32f722xe(ZEEZF7) +target_stm32f722xe(ZEEZF7V2) diff --git a/src/main/target/ZEEZF7/config.c b/src/main/target/ZEEZF7/config.c index ffbe3639f0..bde6f06f21 100644 --- a/src/main/target/ZEEZF7/config.c +++ b/src/main/target/ZEEZF7/config.c @@ -25,6 +25,8 @@ void targetConfiguration(void) { +#ifndef ZEEZF7V2 pinioBoxConfigMutable()->permanentId[0] = BOX_PERMANENT_ID_USER1; // VTX power switcher //pinioBoxConfigMutable()->permanentId[1] = BOX_PERMANENT_ID_USER2; +#endif } diff --git a/src/main/target/ZEEZF7/target.c b/src/main/target/ZEEZF7/target.c index 86573bcc1c..b0ede6a513 100755 --- a/src/main/target/ZEEZF7/target.c +++ b/src/main/target/ZEEZF7/target.c @@ -25,10 +25,21 @@ #include "drivers/timer.h" const timerHardware_t timerHardware[] = { +#ifdef ZEEZF7V2 + DEF_TIM(TIM4, CH2, PB7, TIM_USE_MC_MOTOR, 0, 0), // S1 + DEF_TIM(TIM4, CH1, PB6, TIM_USE_MC_MOTOR, 0, 0), // S2 + DEF_TIM(TIM3, CH1, PB4, TIM_USE_MC_MOTOR, 0, 0), // S3 + DEF_TIM(TIM2, CH2, PB3, TIM_USE_MC_MOTOR, 0, 0), // S4 + DEF_TIM(TIM3, CH3, PC8, TIM_USE_MC_MOTOR, 0, 0), // S5 + DEF_TIM(TIM3, CH2, PC7, TIM_USE_MC_MOTOR, 0, 0), // S6 + DEF_TIM(TIM8, CH1, PC6, TIM_USE_MC_MOTOR, 0, 0), // S7 + DEF_TIM(TIM3, CH4, PB1, TIM_USE_MC_MOTOR, 0, 0), // S8 +#else DEF_TIM(TIM4, CH3, PB8, TIM_USE_MC_MOTOR, 0, 0), // S1 DEF_TIM(TIM4, CH2, PB7, TIM_USE_MC_MOTOR, 0, 0), // S2 DEF_TIM(TIM4, CH1, PB6, TIM_USE_MC_MOTOR, 0, 0), // S3 DEF_TIM(TIM3, CH1, PB4, TIM_USE_MC_MOTOR, 0, 0), // S4 +#endif DEF_TIM(TIM1, CH2N, PB0, TIM_USE_LED , 0, 0), // LED STRIP }; diff --git a/src/main/target/ZEEZF7/target.h b/src/main/target/ZEEZF7/target.h index 2d6e4ab51f..9de6c3359e 100755 --- a/src/main/target/ZEEZF7/target.h +++ b/src/main/target/ZEEZF7/target.h @@ -17,9 +17,13 @@ #pragma once +#ifdef ZEEZF7V2 +#define TARGET_BOARD_IDENTIFIER "ZEF7V2" +#define USBD_PRODUCT_STRING "ZEEZF7V2" +#else #define TARGET_BOARD_IDENTIFIER "ZEF7" - #define USBD_PRODUCT_STRING "ZEEZF7" +#endif #define LED0 PC14 #define LED1 PC15 @@ -29,6 +33,21 @@ // *************** Gyro & ACC ********************** #define USE_SPI + +#ifdef ZEEZF7V2 + +#define USE_SPI_DEVICE_1 +#define SPI1_SCK_PIN PA5 +#define SPI1_MISO_PIN PA6 +#define SPI1_MOSI_PIN PA7 + +#define MPU6000_CS_PIN PA4 +#define MPU6000_SPI_BUS BUS_SPI1 +#define GYRO_INT_EXTI PC4 +#define IMU_MPU6000_ALIGN CW0_DEG + +#else + #define USE_SPI_DEVICE_2 #define SPI2_SCK_PIN PB13 #define SPI2_MISO_PIN PB14 @@ -36,15 +55,44 @@ #define MPU6000_CS_PIN PB12 #define MPU6000_SPI_BUS BUS_SPI2 +#define GYRO_INT_EXTI PC9 -#define USE_EXTI -#define GYRO_INT_EXTI PC9 -#define USE_MPU_DATA_READY_SIGNAL - -#define USE_IMU_MPU6000 #define IMU_MPU6000_ALIGN CW180_DEG +#endif + +#define USE_EXTI +#define USE_MPU_DATA_READY_SIGNAL +#define USE_IMU_MPU6000 + // *************** I2C/Baro/Mag ********************* +#ifdef ZEEZF7V2 + +#define USE_I2C +#define USE_BARO +#define USE_BARO_BMP280 +#define BARO_I2C_BUS BUS_I2C1 + +#define USE_I2C_DEVICE_1 +#define I2C1_SCL PB8 +#define I2C1_SDA PB9 + +// External I2C Pads -- I2C2 +#define USE_I2C_DEVICE_2 +#define I2C2_SCL PA8 +#define I2C2_SDA PC9 + +#define USE_MAG +#define MAG_I2C_BUS BUS_I2C2 +#define USE_MAG_HMC5883 +#define USE_MAG_QMC5883 +#define USE_MAG_IST8310 +#define USE_MAG_IST8308 +#define USE_MAG_MAG3110 +#define USE_MAG_LIS3MDL +#define USE_MAG_AK8975 + +#else // Target has no I2C but can use MSP devices, such as those provided on Mateksys MQ8-CAN/MSP // Which contains: GPS SAM-M8Q, Compass QMC5883L, Barometer DPS310 // See: http://www.mateksys.com/?portfolio=m8q-can @@ -55,18 +103,34 @@ #define USE_MAG #define USE_MAG_QMC5883 +#endif + // *************** Flash **************************** -#define USE_SPI_DEVICE_1 +#ifdef ZEEZF7V2 +#define USE_SPI_DEVICE_2 +#define SPI2_SCK_PIN PB13 +#define SPI2_MISO_PIN PC2 +#define SPI2_MOSI_PIN PC3 + +#define M25P16_SPI_BUS BUS_SPI2 +#define M25P16_CS_PIN PB12 + +#else + +#define USE_SPI_DEVICE_1 #define SPI1_SCK_PIN PA5 #define SPI1_MISO_PIN PA6 #define SPI1_MOSI_PIN PA7 -#define USE_FLASHFS -#define USE_FLASH_M25P16 #define M25P16_SPI_BUS BUS_SPI1 #define M25P16_CS_PIN PA4 + +#endif + +#define USE_FLASHFS +#define USE_FLASH_M25P16 #define ENABLE_BLACKBOX_LOGGING_ON_SPIFLASH_BY_DEFAULT // *************** OSD ***************************** @@ -82,14 +146,16 @@ #define MAX7456_CS_PIN PA15 // *************** PINIO *************************** +#ifdef ZEEZF7 + #define USE_PINIO #define USE_PINIOBOX #define PINIO1_PIN PB11 // VTX power switcher +#endif + // *************** UART ***************************** #define USE_VCP -//#define VBUS_SENSING_PIN PB12 -//#define VBUS_SENSING_ENABLED #define USE_UART1 #define UART1_RX_PIN PA10 @@ -111,12 +177,20 @@ #define UART5_RX_PIN PD2 #define UART5_TX_PIN PC12 +#ifdef ZEEZF7V2 + +#define SERIAL_PORT_COUNT 6 + +#else + #define USE_UART6 #define UART6_RX_PIN PC7 #define UART6_TX_PIN PC6 #define SERIAL_PORT_COUNT 7 +#endif + #define DEFAULT_RX_TYPE RX_TYPE_SERIAL #define SERIALRX_PROVIDER SERIALRX_SBUS #define SERIALRX_UART SERIAL_PORT_USART4 @@ -125,15 +199,24 @@ #define USE_ADC #define ADC_INSTANCE ADC1 #define ADC1_DMA_STREAM DMA2_Stream0 +#ifdef ZEEZF7V2 +#define ADC_CHANNEL_1_PIN PC0 +#define ADC_CHANNEL_2_PIN PC1 +#else #define ADC_CHANNEL_1_PIN PC2 #define ADC_CHANNEL_2_PIN PC3 +#endif #define VBAT_ADC_CHANNEL ADC_CHN_1 #define CURRENT_METER_ADC_CHANNEL ADC_CHN_2 -#define DEFAULT_FEATURES (FEATURE_TX_PROF_SEL | FEATURE_CURRENT_METER | FEATURE_TELEMETRY| FEATURE_VBAT | FEATURE_OSD ) +#define DEFAULT_FEATURES (FEATURE_TX_PROF_SEL | FEATURE_CURRENT_METER | FEATURE_TELEMETRY | FEATURE_VBAT | FEATURE_OSD | FEATURE_LED_STRIP) + +#ifdef ZEEZF7V2 +#define CURRENT_METER_SCALE 250 +#endif #define USE_LED_STRIP -#define WS2811_PIN PB0 +#define WS2811_PIN PB0 #define USE_SERIAL_4WAY_BLHELI_INTERFACE @@ -142,7 +225,11 @@ #define TARGET_IO_PORTC 0xffff #define TARGET_IO_PORTD (BIT(2)) -#define MAX_PWM_OUTPUT_PORTS 4 +#ifdef ZEEZF7V2 +#define MAX_PWM_OUTPUT_PORTS 8 +#else +#define MAX_PWM_OUTPUT_PORTS 4 +#endif #define USE_DSHOT #define USE_ESC_SENSOR #define USE_SERIALSHOT diff --git a/src/main/target/ZEEZF7V2/CMakeLists.txt b/src/main/target/ZEEZF7V2/CMakeLists.txt deleted file mode 100644 index fa01c1d7a7..0000000000 --- a/src/main/target/ZEEZF7V2/CMakeLists.txt +++ /dev/null @@ -1 +0,0 @@ -target_stm32f722xe(ZEEZF7V2) diff --git a/src/main/target/ZEEZF7V2/target.c b/src/main/target/ZEEZF7V2/target.c deleted file mode 100755 index 4178c3fa6c..0000000000 --- a/src/main/target/ZEEZF7V2/target.c +++ /dev/null @@ -1,40 +0,0 @@ -/* -* This file is part of Cleanflight. -* -* Cleanflight is free software: you can redistribute it and/or modify -* it under the terms of the GNU General Public License as published by -* the Free Software Foundation, either version 3 of the License, or -* (at your option) any later version. -* -* Cleanflight is distributed in the hope that it will be useful, -* but WITHOUT ANY WARRANTY; without even the implied warranty of -* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the -* GNU General Public License for more details. -* -* You should have received a copy of the GNU General Public License -* along with Cleanflight. If not, see . -*/ - -#include - -#include "platform.h" - -#include "drivers/bus.h" -#include "drivers/io.h" -#include "drivers/pwm_mapping.h" -#include "drivers/timer.h" - -const timerHardware_t timerHardware[] = { - DEF_TIM(TIM4, CH2, PB7, TIM_USE_MC_MOTOR, 0, 0), // S1 - DEF_TIM(TIM4, CH1, PB6, TIM_USE_MC_MOTOR, 0, 0), // S2 - DEF_TIM(TIM3, CH1, PB4, TIM_USE_MC_MOTOR, 0, 0), // S3 - DEF_TIM(TIM2, CH2, PB3, TIM_USE_MC_MOTOR, 0, 0), // S4 - DEF_TIM(TIM3, CH3, PC8, TIM_USE_MC_MOTOR, 0, 0), // S5 - DEF_TIM(TIM3, CH2, PC7, TIM_USE_MC_MOTOR, 0, 0), // S6 - DEF_TIM(TIM8, CH1, PC6, TIM_USE_MC_MOTOR, 0, 0), // S7 - DEF_TIM(TIM3, CH4, PB1, TIM_USE_MC_MOTOR, 0, 0), // S8 - - DEF_TIM(TIM1, CH2N, PB0, TIM_USE_LED , 0, 0), // LED STRIP -}; - -const int timerHardwareCount = sizeof(timerHardware) / sizeof(timerHardware[0]); diff --git a/src/main/target/ZEEZF7V2/target.h b/src/main/target/ZEEZF7V2/target.h deleted file mode 100755 index 4fede7e11c..0000000000 --- a/src/main/target/ZEEZF7V2/target.h +++ /dev/null @@ -1,156 +0,0 @@ -/* - * This file is part of Cleanflight. - * - * Cleanflight is free software: you can redistribute it and/or modify - * it under the terms of the GNU General Public License as published by - * the Free Software Foundation, either version 3 of the License, or - * (at your option) any later version. - * - * Cleanflight is distributed in the hope that it will be useful, - * but WITHOUT ANY WARRANTY; without even the implied warranty of - * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the - * GNU General Public License for more details. - * - * You should have received a copy of the GNU General Public License - * along with Cleanflight. If not, see . - */ - -#pragma once - -#define TARGET_BOARD_IDENTIFIER "ZEF7V2" - -#define USBD_PRODUCT_STRING "ZEEZF7V2" - -#define LED0 PC14 -#define LED1 PC15 - -#define BEEPER PB2 -#define BEEPER_INVERTED - -// *************** Gyro & ACC ********************** -#define USE_SPI - -#define USE_SPI_DEVICE_1 - -#define SPI1_SCK_PIN PA5 -#define SPI1_MISO_PIN PA6 -#define SPI1_MOSI_PIN PA7 - -#define MPU6000_CS_PIN PA4 -#define MPU6000_SPI_BUS BUS_SPI1 - -#define USE_EXTI -#define GYRO_INT_EXTI PC4 -#define USE_MPU_DATA_READY_SIGNAL - -#define USE_IMU_MPU6000 -#define IMU_MPU6000_ALIGN CW0_DEG - -// *************** I2C/Baro/Mag ********************* - -// Onboard BMP280 Baro --- I2C1 -#define USE_I2C -#define USE_BARO -#define USE_BARO_BMP280 -#define BARO_I2C_BUS BUS_I2C1 - -#define USE_I2C_DEVICE_1 -#define I2C1_SCL PB8 -#define I2C1_SDA PB9 - -// External I2C Pads -- I2C2 -#define USE_I2C_DEVICE_2 -#define I2C2_SCL PA8 -#define I2C2_SDA PC9 - -#define USE_MAG -#define MAG_I2C_BUS BUS_I2C2 -#define USE_MAG_HMC5883 -#define USE_MAG_QMC5883 -#define USE_MAG_IST8310 -#define USE_MAG_IST8308 -#define USE_MAG_MAG3110 -#define USE_MAG_LIS3MDL -#define USE_MAG_AK8975 - -// *************** Flash **************************** - -#define USE_SPI_DEVICE_2 - -#define SPI2_SCK_PIN PB13 -#define SPI2_MISO_PIN PC2 -#define SPI2_MOSI_PIN PC3 - -#define USE_FLASHFS -#define USE_FLASH_M25P16 -#define M25P16_SPI_BUS BUS_SPI2 -#define M25P16_CS_PIN PB12 -#define ENABLE_BLACKBOX_LOGGING_ON_SPIFLASH_BY_DEFAULT - -// *************** OSD ***************************** - -#define USE_SPI_DEVICE_3 -#define SPI3_SCK_PIN PC10 -#define SPI3_MISO_PIN PC11 -#define SPI3_MOSI_PIN PB5 - -#define USE_OSD -#define USE_MAX7456 -#define MAX7456_SPI_BUS BUS_SPI3 -#define MAX7456_CS_PIN PA15 - -// *************** UART ***************************** -#define USE_VCP - -#define USE_UART1 -#define UART1_RX_PIN PA10 -#define UART1_TX_PIN PA9 - -#define USE_UART2 -#define UART2_RX_PIN PA3 -#define UART2_TX_PIN PA2 - -#define USE_UART3 -#define UART3_RX_PIN PB11 -#define UART3_TX_PIN PB10 - -#define USE_UART4 -#define UART4_RX_PIN PA1 -#define UART4_TX_PIN PA0 - -#define USE_UART5 -#define UART5_RX_PIN PD2 -#define UART5_TX_PIN PC12 - -#define SERIAL_PORT_COUNT 6 - -#define DEFAULT_RX_TYPE RX_TYPE_SERIAL -#define SERIALRX_PROVIDER SERIALRX_SBUS -#define SERIALRX_UART SERIAL_PORT_USART4 - -// *************** ADC ***************************** -#define USE_ADC -#define ADC_INSTANCE ADC1 -#define ADC1_DMA_STREAM DMA2_Stream0 -#define ADC_CHANNEL_1_PIN PC0 -#define ADC_CHANNEL_2_PIN PC1 -#define VBAT_ADC_CHANNEL ADC_CHN_1 -#define CURRENT_METER_ADC_CHANNEL ADC_CHN_2 - -#define DEFAULT_FEATURES (FEATURE_TX_PROF_SEL | FEATURE_CURRENT_METER | FEATURE_TELEMETRY | FEATURE_VBAT | FEATURE_OSD | FEATURE_LED_STRIP) -#define CURRENT_METER_SCALE 250 - -#define USE_LED_STRIP -#define WS2811_PIN PB0 - -#define USE_SERIAL_4WAY_BLHELI_INTERFACE - -#define TARGET_IO_PORTA 0xffff -#define TARGET_IO_PORTB 0xffff -#define TARGET_IO_PORTC 0xffff -#define TARGET_IO_PORTD (BIT(2)) - -#define MAX_PWM_OUTPUT_PORTS 8 -#define USE_DSHOT -#define USE_ESC_SENSOR -#define USE_SERIALSHOT From 33f340fbbbb5fce11666875800e587226532abfc Mon Sep 17 00:00:00 2001 From: Alexander van Saase Date: Wed, 17 Feb 2021 08:33:03 +0100 Subject: [PATCH 142/286] =?UTF-8?q?=EF=BB=BFadd=20uptilt=20to=20pitch=20an?= =?UTF-8?q?gle?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- src/main/io/osd.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/io/osd.c b/src/main/io/osd.c index b6943791f7..7c8825e3a1 100644 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -1917,7 +1917,7 @@ static bool osdDrawSingleElement(uint8_t item) rollAngle = DECIDEGREES_TO_RADIANS(attitude.values.roll); pitchAngle = DECIDEGREES_TO_RADIANS(attitude.values.pitch); #endif - + pitchAngle -= DEGREES_TO_RADIANS(osdConfig()->camera_uptilt); if (osdConfig()->ahi_reverse_roll) { rollAngle = -rollAngle; } From 1e2df731eabc984977877238298eb367a9df68b7 Mon Sep 17 00:00:00 2001 From: "Pawel Spychalski (DzikuVx)" Date: Fri, 9 Apr 2021 09:43:48 +0200 Subject: [PATCH 143/286] buffer write for BNO055 Serial interface --- .../drivers/accgyro/accgyro_bno055_serial.c | 17 ++++++++++++++--- 1 file changed, 14 insertions(+), 3 deletions(-) diff --git a/src/main/drivers/accgyro/accgyro_bno055_serial.c b/src/main/drivers/accgyro/accgyro_bno055_serial.c index 337f1198ab..967c316d60 100644 --- a/src/main/drivers/accgyro/accgyro_bno055_serial.c +++ b/src/main/drivers/accgyro/accgyro_bno055_serial.c @@ -64,13 +64,24 @@ static uint8_t bno055FrameIndex; static timeMs_t bno055FrameStartAtMs = 0; static uint8_t bno055DataType = BNO055_DATA_TYPE_NONE; -static void bno055SerialWrite(uint8_t reg, uint8_t data) { + +static void bno055SerialWriteBuffer(const uint8_t reg, const uint8_t *data, const uint8_t count) { + bno055ProtocolState = BNO055_RECEIVE_IDLE; serialWrite(bno055SerialPort, 0xAA); // Start Byte serialWrite(bno055SerialPort, 0x00); // Write command serialWrite(bno055SerialPort, reg); - serialWrite(bno055SerialPort, 1); - serialWrite(bno055SerialPort, data); + serialWrite(bno055SerialPort, count); + for (uint8_t i = 0; i < count; i++) { + serialWrite(bno055SerialPort, data[i]); + } +} + +static void bno055SerialWrite(const uint8_t reg, const uint8_t data) { + uint8_t buff[1]; + buff[0] = data; + + bno055SerialWriteBuffer(reg, buff, 1); } static void bno055SerialRead(const uint8_t reg, const uint8_t len) { From 235b02081fa8b917758d8ff8dfe809e5731f8f50 Mon Sep 17 00:00:00 2001 From: Alexander van Saase Date: Thu, 11 Mar 2021 20:48:38 +0100 Subject: [PATCH 144/286] =?UTF-8?q?=EF=BB=BFdocs=20(+25=20squashed=20commi?= =?UTF-8?q?t)?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Squashed commit: [51a36e00b] faster updates [7fed94eae] some cleanup [ea00282b2] make into feature and add constraint [a2bdfab5a] require sticks centered [19c5c8773] defaults [191ebe04f] fix bug [a50a11de7] fix a dumb bug [92d6760b7] use same servo selection logic for regular autotrim [d4a389f78] minor cleanup [999b968b2] build error [a1c25b474] don't update trims when gps heading is not valid to avoid changing trims when on the ground [317bca1f1] add check on ratetarget for when flying in other modes than acro [8abd5682e] use time since last arm instead of total arm time since boot [cf3179325] setting name [78b00bb0f] comment to force new GH check [e5527a9f4] build error [6d0e09938] change setting name [3824ce3f7] more docs [9e04b5b25] docs [591cade68] transfer I-term to servo midpoint [ad97f692d] fix mode [2d7438b4c] merge conflicts [290288203] change rotation threshold [6fc3676cf] always the docs... [48ba7b7ae] Automatic servo trim --- docs/Settings.md | 2 + src/main/build/debug.h | 1 + src/main/fc/cli.c | 2 +- src/main/fc/config.h | 1 + src/main/fc/fc_core.c | 12 +++ src/main/fc/fc_core.h | 1 + src/main/fc/fc_msp_box.c | 9 +- src/main/fc/rc_modes.h | 1 + src/main/fc/settings.yaml | 12 ++- src/main/flight/mixer.h | 1 + src/main/flight/pid.c | 16 ++++ src/main/flight/pid.h | 3 + src/main/flight/servos.c | 172 ++++++++++++++++++++++++++++++++++---- src/main/flight/servos.h | 2 + src/main/io/osd.c | 2 +- 15 files changed, 217 insertions(+), 20 deletions(-) diff --git a/docs/Settings.md b/docs/Settings.md index a9b79f5abf..414b0db15f 100644 --- a/docs/Settings.md +++ b/docs/Settings.md @@ -474,6 +474,8 @@ | serialrx_halfduplex | AUTO | Allow serial receiver to operate on UART TX pin. With some receivers will allow control and telemetry over a single wire. | | serialrx_inverted | OFF | Reverse the serial inversion of the serial RX protocol. When this value is OFF, each protocol will use its default signal (e.g. SBUS will use an inverted signal). Some OpenLRS receivers produce a non-inverted SBUS signal. This setting supports this type of receivers (including modified FrSKY). | | serialrx_provider | _target default_ | When feature SERIALRX is enabled, this allows connection to several receivers which output data via digital interface resembling serial. See RX section. | +| servo_autotrim_iterm_threshold | 5 | How much of the Iterm is carried over to the servo midpoints on each update. Only applies when using `feature FW_AUTOTRIM`. | +| servo_autotrim_rotation_limit | 10 | Servo midpoints are only updated when total aircraft rotation is less than this threshold [deg/s]. Only applies when using `feature FW_AUTOTRIM`. | | servo_center_pulse | 1500 | Servo midpoint | | servo_lpf_hz | 20 | Selects the servo PWM output cutoff frequency. Value is in [Hz] | | servo_protocol | PWM | An option to chose the protocol/option that would be used to output servo data. Possible options `PWM` (FC servo outputs), `SERVO_DRIVER` (I2C PCA9685 peripheral), `SBUS` (S.Bus protocol output via a configured serial port) | diff --git a/src/main/build/debug.h b/src/main/build/debug.h index e8f47543eb..921f4ff734 100644 --- a/src/main/build/debug.h +++ b/src/main/build/debug.h @@ -83,5 +83,6 @@ typedef enum { DEBUG_FW_D, DEBUG_IMU2, DEBUG_ALTITUDE, + DEBUG_AUTOTRIM, DEBUG_COUNT } debugType_e; diff --git a/src/main/fc/cli.c b/src/main/fc/cli.c index 68f951f687..148b74810f 100755 --- a/src/main/fc/cli.c +++ b/src/main/fc/cli.c @@ -156,7 +156,7 @@ static const char * const featureNames[] = { "", "RSSI_ADC", "LED_STRIP", "DASHBOARD", "", "BLACKBOX", "", "TRANSPONDER", "AIRMODE", "SUPEREXPO", "VTX", "", "", "", "PWM_OUTPUT_ENABLE", - "OSD", "FW_LAUNCH", NULL + "OSD", "FW_LAUNCH", "FW_AUTOTRIM", NULL }; /* Sensor names (used in lookup tables for *_hardware settings and in status command output) */ diff --git a/src/main/fc/config.h b/src/main/fc/config.h index 48dc455e71..40fd41dfda 100644 --- a/src/main/fc/config.h +++ b/src/main/fc/config.h @@ -67,6 +67,7 @@ typedef enum { FEATURE_PWM_OUTPUT_ENABLE = 1 << 28, FEATURE_OSD = 1 << 29, FEATURE_FW_LAUNCH = 1 << 30, + FEATURE_FW_AUTOTRIM = 1 << 31, } features_e; typedef struct systemConfig_s { diff --git a/src/main/fc/fc_core.c b/src/main/fc/fc_core.c index 40c18fd31f..d248c38917 100755 --- a/src/main/fc/fc_core.c +++ b/src/main/fc/fc_core.c @@ -121,6 +121,7 @@ typedef struct emergencyArmingState_s { timeDelta_t cycleTime = 0; // this is the number in micro second to achieve a full loop, it can differ a little and is taken into account in the PID loop static timeUs_t flightTime = 0; +static timeUs_t armTime = 0; EXTENDED_FASTRAM float dT; @@ -842,8 +843,12 @@ void taskMainPidLoop(timeUs_t currentTimeUs) if (ARMING_FLAG(ARMED) && (!STATE(FIXED_WING_LEGACY) || !isNavLaunchEnabled() || (isNavLaunchEnabled() && (isFixedWingLaunchDetected() || isFixedWingLaunchFinishedOrAborted())))) { flightTime += cycleTime; + armTime += cycleTime; updateAccExtremes(); } + if (!ARMING_FLAG(ARMED)) { + armTime = 0; + } taskGyro(currentTimeUs); imuUpdateAccelerometer(); @@ -905,6 +910,8 @@ void taskMainPidLoop(timeUs_t currentTimeUs) if (isMixerUsingServos()) { servoMixer(dT); processServoAutotrim(); + processContinuousServoAutotrim(dT); + } //Servos should be filtered or written only when mixer is using servos or special feaures are enabled @@ -961,6 +968,11 @@ float getFlightTime() return (float)(flightTime / 1000) / 1000; } +float getArmTime() +{ + return (float)(armTime / 1000) / 1000; +} + void fcReboot(bool bootLoader) { // stop motor/servo outputs diff --git a/src/main/fc/fc_core.h b/src/main/fc/fc_core.h index 34eb6fe68b..b321c2162e 100644 --- a/src/main/fc/fc_core.h +++ b/src/main/fc/fc_core.h @@ -44,5 +44,6 @@ void emergencyArmingUpdate(bool armingSwitchIsOn); bool isCalibrating(void); float getFlightTime(void); +float getArmTime(void); void fcReboot(bool bootLoader); \ No newline at end of file diff --git a/src/main/fc/fc_msp_box.c b/src/main/fc/fc_msp_box.c index f517be11f1..09bbd59a5f 100644 --- a/src/main/fc/fc_msp_box.c +++ b/src/main/fc/fc_msp_box.c @@ -90,6 +90,7 @@ static const box_t boxes[CHECKBOX_ITEM_COUNT + 1] = { { BOXMSPRCOVERRIDE, "MSP RC OVERRIDE", 50 }, { BOXPREARM, "PREARM", 51 }, { BOXFLIPOVERAFTERCRASH, "TURTLE", 52 }, + { BOXCONTAUTOTRIM, "CONTINUOUS AUTOTRIM", 53 }, { CHECKBOX_ITEM_COUNT, NULL, 0xFF } }; @@ -240,7 +241,12 @@ void initActiveBoxIds(void) if (!feature(FEATURE_FW_LAUNCH)) { activeBoxIds[activeBoxIdCount++] = BOXNAVLAUNCH; } - activeBoxIds[activeBoxIdCount++] = BOXAUTOTRIM; + + if (!feature(FEATURE_FW_AUTOTRIM)) { + activeBoxIds[activeBoxIdCount++] = BOXAUTOTRIM; + activeBoxIds[activeBoxIdCount++] = BOXCONTAUTOTRIM; + } + #if defined(USE_AUTOTUNE_FIXED_WING) activeBoxIds[activeBoxIdCount++] = BOXAUTOTUNE; #endif @@ -373,6 +379,7 @@ void packBoxModeFlags(boxBitmask_t * mspBoxModeFlags) #if defined(USE_RX_MSP) && defined(USE_MSP_RC_OVERRIDE) CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXMSPRCOVERRIDE)), BOXMSPRCOVERRIDE); #endif + CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXCONTAUTOTRIM)), BOXCONTAUTOTRIM); memset(mspBoxModeFlags, 0, sizeof(boxBitmask_t)); for (uint32_t i = 0; i < activeBoxIdCount; i++) { diff --git a/src/main/fc/rc_modes.h b/src/main/fc/rc_modes.h index 6b3f877197..bf4840164e 100755 --- a/src/main/fc/rc_modes.h +++ b/src/main/fc/rc_modes.h @@ -70,6 +70,7 @@ typedef enum { BOXMSPRCOVERRIDE = 41, BOXPREARM = 42, BOXFLIPOVERAFTERCRASH = 43, + BOXCONTAUTOTRIM = 44, CHECKBOX_ITEM_COUNT } boxId_e; diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 8b4be4c17c..f616f1ae04 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -87,7 +87,7 @@ tables: "FLOW", "SBUS", "FPORT", "ALWAYS", "SAG_COMP_VOLTAGE", "VIBE", "CRUISE", "REM_FLIGHT_TIME", "SMARTAUDIO", "ACC", "ERPM", "RPM_FILTER", "RPM_FREQ", "NAV_YAW", "DYNAMIC_FILTER", "DYNAMIC_FILTER_FREQUENCY", - "IRLOCK", "CD", "KALMAN_GAIN", "PID_MEASUREMENT", "SPM_CELLS", "SPM_VS600", "SPM_VARIO", "PCF8574", "DYN_GYRO_LPF", "FW_D", "IMU2", "ALTITUDE"] + "IRLOCK", "CD", "KALMAN_GAIN", "PID_MEASUREMENT", "SPM_CELLS", "SPM_VS600", "SPM_VARIO", "PCF8574", "DYN_GYRO_LPF", "FW_D", "IMU2", "ALTITUDE", "AUTOTRIM"] - name: async_mode values: ["NONE", "GYRO", "ALL"] - name: aux_operator @@ -1176,6 +1176,16 @@ groups: description: "On tricopter mix only, if this is set to ON, servo will always be correcting regardless of armed state. to disable this, set it to OFF." default_value: ON type: bool + - name: servo_autotrim_rotation_limit + description: "Servo midpoints are only updated when total aircraft rotation is less than this threshold [deg/s]. Only applies when using `feature FW_AUTOTRIM`." + default_value: 10 + min: 1 + max: 60 + - name: servo_autotrim_iterm_threshold + description: "How much of the Iterm is carried over to the servo midpoints on each update. Only applies when using `feature FW_AUTOTRIM`." + default_value: 5 + min: 1 + max: 25 - name: PG_CONTROL_RATE_PROFILES type: controlRateConfig_t diff --git a/src/main/flight/mixer.h b/src/main/flight/mixer.h index 7f5088fafc..7e653e9679 100644 --- a/src/main/flight/mixer.h +++ b/src/main/flight/mixer.h @@ -127,6 +127,7 @@ void mixerResetDisarmedMotors(void); void mixTable(const float dT); void writeMotors(void); void processServoAutotrim(void); +void processContinuousServoAutotrim(const float dT); void stopMotors(void); void stopPwmAllMotors(void); diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index b8c4b94e24..89458e30ab 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -371,6 +371,22 @@ void pidResetErrorAccumulators(void) } } +void pidReduceErrorAccumulators(int8_t delta, uint8_t axis) +{ + pidState[axis].errorGyroIf -= delta; + pidState[axis].errorGyroIfLimit -= delta; +} + +float getTotalRateTarget(void) +{ + return sqrtf(sq(pidState[FD_ROLL].rateTarget) + sq(pidState[FD_PITCH].rateTarget) + sq(pidState[FD_YAW].rateTarget)); +} + +float getAxisIterm(uint8_t axis) +{ + return pidState[axis].errorGyroIf; +} + static float pidRcCommandToAngle(int16_t stick, int16_t maxInclination) { stick = constrain(stick, -500, 500); diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h index 3f3d5a3bcc..cb8dc55780 100644 --- a/src/main/flight/pid.h +++ b/src/main/flight/pid.h @@ -183,6 +183,9 @@ extern int32_t axisPID_P[], axisPID_I[], axisPID_D[], axisPID_Setpoint[]; void pidInit(void); bool pidInitFilters(void); void pidResetErrorAccumulators(void); +void pidReduceErrorAccumulators(int8_t delta, uint8_t axis); +float getAxisIterm(uint8_t axis); +float getTotalRateTarget(void); void pidResetTPAFilter(void); struct controlRateConfig_s; diff --git a/src/main/flight/servos.c b/src/main/flight/servos.c index df58e6f649..4efd2db680 100755 --- a/src/main/flight/servos.c +++ b/src/main/flight/servos.c @@ -40,6 +40,7 @@ #include "drivers/time.h" #include "fc/config.h" +#include "fc/fc_core.h" #include "fc/rc_controls.h" #include "fc/rc_modes.h" #include "fc/runtime_config.h" @@ -51,11 +52,13 @@ #include "flight/pid.h" #include "flight/servos.h" +#include "io/gps.h" + #include "rx/rx.h" #include "sensors/gyro.h" -PG_REGISTER_WITH_RESET_TEMPLATE(servoConfig_t, servoConfig, PG_SERVO_CONFIG, 1); +PG_REGISTER_WITH_RESET_TEMPLATE(servoConfig_t, servoConfig, PG_SERVO_CONFIG, 2); PG_RESET_TEMPLATE(servoConfig_t, servoConfig, .servoCenterPulse = SETTING_SERVO_CENTER_PULSE_DEFAULT, @@ -63,7 +66,9 @@ PG_RESET_TEMPLATE(servoConfig_t, servoConfig, .servo_lowpass_freq = SETTING_SERVO_LPF_HZ_DEFAULT, // Default servo update rate is 50Hz, everything above Nyquist frequency (25Hz) is going to fold and cause distortions .servo_protocol = SETTING_SERVO_PROTOCOL_DEFAULT, .flaperon_throw_offset = SETTING_FLAPERON_THROW_OFFSET_DEFAULT, - .tri_unarmed_servo = SETTING_TRI_UNARMED_SERVO_DEFAULT + .tri_unarmed_servo = SETTING_TRI_UNARMED_SERVO_DEFAULT, + .servo_autotrim_rotation_limit = SETTING_SERVO_AUTOTRIM_ROTATION_LIMIT_DEFAULT, + .servo_autotrim_iterm_threshold = SETTING_SERVO_AUTOTRIM_ITERM_THRESHOLD ); PG_REGISTER_ARRAY_WITH_RESET_FN(servoMixer_t, MAX_SERVO_RULES, customServoMixers, PG_SERVO_MIXER, 1); @@ -83,7 +88,7 @@ void pgResetFn_customServoMixers(servoMixer_t *instance) } } -PG_REGISTER_ARRAY_WITH_RESET_FN(servoParam_t, MAX_SUPPORTED_SERVOS, servoParams, PG_SERVO_PARAMS, 2); +PG_REGISTER_ARRAY_WITH_RESET_FN(servoParam_t, MAX_SUPPORTED_SERVOS, servoParams, PG_SERVO_PARAMS, 3); void pgResetFn_servoParams(servoParam_t *instance) { @@ -93,7 +98,7 @@ void pgResetFn_servoParams(servoParam_t *instance) .max = DEFAULT_SERVO_MAX, .middle = DEFAULT_SERVO_MIDDLE, .rate = 100 - ); + ); } } @@ -113,6 +118,8 @@ static bool servoFilterIsSet; static servoMetadata_t servoMetadata[MAX_SUPPORTED_SERVOS]; static rateLimitFilter_t servoSpeedLimitFilter[MAX_SERVO_RULES]; +STATIC_FASTRAM pt1Filter_t rotRateFilter; + int16_t getFlaperonDirection(uint8_t servoPin) { if (servoPin == SERVO_FLAPPERON_2) { @@ -399,11 +406,15 @@ void processServoAutotrim(void) case AUTOTRIM_IDLE: if (ARMING_FLAG(ARMED)) { // We are activating servo trim - backup current middles and prepare to average the data - for (int servoIndex = SERVO_ELEVATOR; servoIndex <= MIN(SERVO_RUDDER, MAX_SUPPORTED_SERVOS); servoIndex++) { - servoMiddleBackup[servoIndex] = servoParams(servoIndex)->middle; - servoMiddleAccum[servoIndex] = 0; + for (int axis = FD_ROLL; axis <= FD_YAW; axis++) { + for (int i = 0; i < servoRuleCount; i++) { + const uint8_t target = currentServoMixer[i].targetChannel; + const uint8_t from = currentServoMixer[i].inputSource; + if (from == axis) { + servoMiddleBackup[target] = servoParams(target)->middle; + } + } } - trimStartedAt = millis(); servoMiddleAccumCount = 0; trimState = AUTOTRIM_COLLECTING; @@ -416,14 +427,26 @@ void processServoAutotrim(void) case AUTOTRIM_COLLECTING: if (ARMING_FLAG(ARMED)) { servoMiddleAccumCount++; - - for (int servoIndex = SERVO_ELEVATOR; servoIndex <= MIN(SERVO_RUDDER, MAX_SUPPORTED_SERVOS); servoIndex++) { - servoMiddleAccum[servoIndex] += servo[servoIndex]; + for (int axis = FD_ROLL; axis <= FD_YAW; axis++) { + for (int i = 0; i < servoRuleCount; i++) { + const uint8_t target = currentServoMixer[i].targetChannel; + const uint8_t from = currentServoMixer[i].inputSource; + if (from == axis) { + servoMiddleAccum[target] += servo[target]; + } + } } if ((millis() - trimStartedAt) > SERVO_AUTOTRIM_TIMER_MS) { - for (int servoIndex = SERVO_ELEVATOR; servoIndex <= MIN(SERVO_RUDDER, MAX_SUPPORTED_SERVOS); servoIndex++) { - servoParamsMutable(servoIndex)->middle = servoMiddleAccum[servoIndex] / servoMiddleAccumCount; + for (int axis = FD_ROLL; axis <= FD_YAW; axis++) { + for (int i = 0; i < servoRuleCount; i++) { + const uint8_t target = currentServoMixer[i].targetChannel; + const uint8_t from = currentServoMixer[i].inputSource; + if (from == axis) { + servoParamsMutable(target)->middle = servoMiddleAccum[target] / servoMiddleAccumCount; + + } + } } trimState = AUTOTRIM_SAVE_PENDING; pidResetErrorAccumulators(); //Reset Iterm since new midpoints override previously acumulated errors @@ -449,11 +472,16 @@ void processServoAutotrim(void) else { // We are deactivating servo trim - restore servo midpoints if (trimState == AUTOTRIM_SAVE_PENDING) { - for (int servoIndex = SERVO_ELEVATOR; servoIndex <= MIN(SERVO_RUDDER, MAX_SUPPORTED_SERVOS); servoIndex++) { - servoParamsMutable(servoIndex)->middle = servoMiddleBackup[servoIndex]; + for (int axis = FD_ROLL; axis <= FD_YAW; axis++) { + for (int i = 0; i < servoRuleCount; i++) { + const uint8_t target = currentServoMixer[i].targetChannel; + const uint8_t from = currentServoMixer[i].inputSource; + if (from == axis) { + servoParamsMutable(target)->middle = servoMiddleBackup[target]; + } + } } } - trimState = AUTOTRIM_IDLE; } } @@ -472,3 +500,115 @@ bool isMixerUsingServos(void) { return mixerUsesServos; } + +#define SERVO_AUTOTRIM_FILTER_CUTOFF 1 // LPF cutoff frequency +#define SERVO_AUTOTRIM_CENTER_MIN 1300 +#define SERVO_AUTOTRIM_CENTER_MAX 1700 + +void processContinuousServoAutotrim(const float dT) +{ + static timeMs_t lastUpdateTimeMs; + static servoAutotrimState_e trimState = AUTOTRIM_IDLE; + + static int16_t servoMiddleBackup[MAX_SUPPORTED_SERVOS]; + static int32_t servoMiddleUpdateCount; + + const uint8_t ItermThreshold = servoConfig()->servo_autotrim_iterm_threshold; + + const float rotRateMagnitude = sqrtf(vectorNormSquared(&imuMeasuredRotationBF)); + const float rotRateMagnitudeFiltered = pt1FilterApply4(&rotRateFilter, rotRateMagnitude, SERVO_AUTOTRIM_FILTER_CUTOFF, dT); + + if (IS_RC_MODE_ACTIVE(BOXCONTAUTOTRIM) || feature(FEATURE_FW_AUTOTRIM)) { + switch (trimState) { + case AUTOTRIM_IDLE: + if (ARMING_FLAG(ARMED)) { + // FIXME: use proper flying detection + // We are activating servo trim - backup current middles and prepare to update the servo midpoints + for (int axis = FD_ROLL; axis <= FD_YAW; axis++) { + for (int i = 0; i < servoRuleCount; i++) { + const uint8_t target = currentServoMixer[i].targetChannel; + const uint8_t from = currentServoMixer[i].inputSource; + if (from == axis) { + servoMiddleBackup[target] = servoParams(target)->middle; + } + } + } + servoMiddleUpdateCount = 0; + lastUpdateTimeMs = millis(); + trimState = AUTOTRIM_COLLECTING; + } + else { + break; + } + // Fallthru + + case AUTOTRIM_COLLECTING: + if (ARMING_FLAG(ARMED)) { + // Every half second update servo midpoints + if ((millis() - lastUpdateTimeMs) > 500 && isGPSHeadingValid()) { + const bool planeFlyingStraight = rotRateMagnitudeFiltered <= DEGREES_TO_RADIANS(servoConfig()->servo_autotrim_rotation_limit); + const bool zeroRotationCommanded = getTotalRateTarget() <= servoConfig()->servo_autotrim_rotation_limit; + if (planeFlyingStraight && zeroRotationCommanded && !areSticksDeflectedMoreThanPosHoldDeadband() &&!FLIGHT_MODE(MANUAL_MODE)) { + // Plane is flying straight and sticks are centered + for (int axis = FD_ROLL; axis <= FD_YAW; axis++) { + // For each stabilized axis, add x units of I-term to all associated servo midpoints + const float axisIterm = getAxisIterm(axis); + if (fabsf(axisIterm) > ItermThreshold) { + const int8_t ItermUpdate = axisIterm > 0.0f ? ItermThreshold : -ItermThreshold; + for (int i = 0; i < servoRuleCount; i++) { + const uint8_t target = currentServoMixer[i].targetChannel; + const uint8_t from = currentServoMixer[i].inputSource; + if (from == axis) { + // Convert axis I-term to servo PWM and add to midpoint + const float mixerRate = currentServoMixer[i].rate / 100.0f; + const float servoRate = servoParams(target)->rate / 100.0f; + servoParamsMutable(target)->middle += ItermUpdate * mixerRate * servoRate; + servoParamsMutable(target)->middle = constrain(servoParamsMutable(target)->middle, SERVO_AUTOTRIM_CENTER_MIN, SERVO_AUTOTRIM_CENTER_MAX); + } + } + pidReduceErrorAccumulators(ItermUpdate, axis); + } + } + servoMiddleUpdateCount++; + } + // Reset timer + lastUpdateTimeMs = millis(); + } + break; + } else { + // We have disarmed, save to EEPROM + saveConfigAndNotify(); + trimState = AUTOTRIM_IDLE; + break; + } + case AUTOTRIM_SAVE_PENDING: + case AUTOTRIM_DONE: + break; + } + } + else { + // We are deactivating servo trim - restore servo midpoints + if (trimState == AUTOTRIM_COLLECTING) { + for (int axis = FD_ROLL; axis <= FD_YAW; axis++) { + for (int i = 0; i < servoRuleCount; i++) { + const uint8_t target = currentServoMixer[i].targetChannel; + const uint8_t from = currentServoMixer[i].inputSource; + if (from == axis) { + servoParamsMutable(target)->middle = servoMiddleBackup[target]; + } + } + } + } + trimState = AUTOTRIM_IDLE; + } + + // Debug + DEBUG_SET(DEBUG_AUTOTRIM, 0, servoParams(2)->middle); + DEBUG_SET(DEBUG_AUTOTRIM, 2, servoParams(3)->middle); + DEBUG_SET(DEBUG_AUTOTRIM, 4, servoParams(4)->middle); + DEBUG_SET(DEBUG_AUTOTRIM, 6, servoParams(5)->middle); + DEBUG_SET(DEBUG_AUTOTRIM, 1, servoMiddleUpdateCount); + DEBUG_SET(DEBUG_AUTOTRIM, 3, RADIANS_TO_DEGREES(rotRateMagnitudeFiltered)); + DEBUG_SET(DEBUG_AUTOTRIM, 5, axisPID_I[FD_ROLL]); + DEBUG_SET(DEBUG_AUTOTRIM, 7, axisPID_I[FD_PITCH]); +} \ No newline at end of file diff --git a/src/main/flight/servos.h b/src/main/flight/servos.h index 3b536ad3d6..841415ea23 100644 --- a/src/main/flight/servos.h +++ b/src/main/flight/servos.h @@ -138,6 +138,8 @@ typedef struct servoConfig_s { uint16_t flaperon_throw_offset; uint8_t servo_protocol; // See servoProtocolType_e uint8_t tri_unarmed_servo; // send tail servo correction pulses even when unarmed + uint8_t servo_autotrim_rotation_limit; // Max rotation for servo midpoints to be updated + uint8_t servo_autotrim_iterm_threshold; // How much of the Iterm is carried over to the servo midpoints on each update } servoConfig_t; PG_DECLARE(servoConfig_t, servoConfig); diff --git a/src/main/io/osd.c b/src/main/io/osd.c index 7c8825e3a1..ce17eb9e0d 100644 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -3598,7 +3598,7 @@ textAttributes_t osdGetSystemMessage(char *buff, size_t buff_size, bool isCenter // by OSD_FLYMODE. messages[messageCount++] = OSD_MESSAGE_STR(OSD_MSG_ALTITUDE_HOLD); } - if (IS_RC_MODE_ACTIVE(BOXAUTOTRIM)) { + if (IS_RC_MODE_ACTIVE(BOXAUTOTRIM) || IS_RC_MODE_ACTIVE(BOXCONTAUTOTRIM)) { messages[messageCount++] = OSD_MESSAGE_STR(OSD_MSG_AUTOTRIM); } if (IS_RC_MODE_ACTIVE(BOXAUTOTUNE)) { From 06ade636fb560ba5b217eaea8fa181b90a498ba9 Mon Sep 17 00:00:00 2001 From: Alexander van Saase Date: Mon, 15 Feb 2021 23:23:59 +0100 Subject: [PATCH 145/286] FW autotune improvements --- docs/Settings.md | 9 +- src/main/build/debug.h | 1 + src/main/fc/settings.yaml | 51 +++++--- src/main/flight/pid.c | 8 +- src/main/flight/pid.h | 17 ++- src/main/flight/pid_autotune.c | 223 ++++++++++++++++++++++----------- 6 files changed, 210 insertions(+), 99 deletions(-) diff --git a/docs/Settings.md b/docs/Settings.md index a9b79f5abf..0f01acfc89 100644 --- a/docs/Settings.md +++ b/docs/Settings.md @@ -109,11 +109,14 @@ | frsky_pitch_roll | OFF | S.Port and D-Series telemetry: Send pitch and roll degrees*10 instead of raw accelerometer data | | frsky_unit | METRIC | Not used? [METRIC/IMPERIAL] | | frsky_vfas_precision | 0 | D-Series telemetry only: Set to 1 to send raw VBat value in 0.1V resolution for receivers that can handle it, or 0 (default) to use the standard method | +| fw_autotune_convergence_rate | 10 | How much autotune adjusts the gains on each update. Setting this too high can cause erratically changing gains during autotune. | +| fw_autotune_detect_time | 250 | Time [ms] to detect sustained undershoot or overshoot | | fw_autotune_ff_to_i_tc | 600 | FF to I time (defines time for I to reach the same level of response as FF) [ms] | | fw_autotune_ff_to_p_gain | 10 | FF to P gain (strength relationship) [%] | -| fw_autotune_overshoot_time | 100 | Time [ms] to detect sustained overshoot | -| fw_autotune_threshold | 50 | Threshold [%] of max rate to consider overshoot/undershoot detection | -| fw_autotune_undershoot_time | 200 | Time [ms] to detect sustained undershoot | +| fw_autotune_max_rate_deflection | 80 | The target percentage of maximum mixer output used for determining the rates in `AUTO` and `LIMIT`. | +| fw_autotune_min_stick | 80 | Minimum stick input [%] to consider overshoot/undershoot detection | +| fw_autotune_p_to_d_gain | 0 | P to D gain (strength relationship) [%] | +| fw_autotune_rate_adjustment | AUTO | `AUTO` and `LIMIT` adjust the rates to match the capabilities of the airplane, with `LIMIT` they are never increased above the starting rates setting. `FIXED` does not adjust the rates. Rates are not changed when tuning in `ANGLE` mode. | | fw_d_level | 75 | Fixed-wing attitude stabilisation HORIZON transition point | | fw_d_pitch | 0 | Fixed wing rate stabilisation D-gain for PITCH | | fw_d_roll | 0 | Fixed wing rate stabilisation D-gain for ROLL | diff --git a/src/main/build/debug.h b/src/main/build/debug.h index e8f47543eb..7c47bc5b8c 100644 --- a/src/main/build/debug.h +++ b/src/main/build/debug.h @@ -83,5 +83,6 @@ typedef enum { DEBUG_FW_D, DEBUG_IMU2, DEBUG_ALTITUDE, + DEBUG_AUTOTUNE, DEBUG_COUNT } debugType_e; diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 8b4be4c17c..7ec65be4ec 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -87,7 +87,7 @@ tables: "FLOW", "SBUS", "FPORT", "ALWAYS", "SAG_COMP_VOLTAGE", "VIBE", "CRUISE", "REM_FLIGHT_TIME", "SMARTAUDIO", "ACC", "ERPM", "RPM_FILTER", "RPM_FREQ", "NAV_YAW", "DYNAMIC_FILTER", "DYNAMIC_FILTER_FREQUENCY", - "IRLOCK", "CD", "KALMAN_GAIN", "PID_MEASUREMENT", "SPM_CELLS", "SPM_VS600", "SPM_VARIO", "PCF8574", "DYN_GYRO_LPF", "FW_D", "IMU2", "ALTITUDE"] + "IRLOCK", "CD", "KALMAN_GAIN", "PID_MEASUREMENT", "SPM_CELLS", "SPM_VS600", "SPM_VARIO", "PCF8574", "DYN_GYRO_LPF", "FW_D", "IMU2", "ALTITUDE", "AUTOTUNE"] - name: async_mode values: ["NONE", "GYRO", "ALL"] - name: aux_operator @@ -163,6 +163,9 @@ tables: values: ["OFF_ALWAYS", "OFF", "AUTO_ONLY", "ALL_NAV"] - name: osd_plus_code_short values: ["0", "2", "4", "6"] + - name: autotune_rate_adjustment + enum: autotune_rate_adjustment_e + values: ["FIXED", "LIMIT", "AUTO"] groups: - name: PG_GYRO_CONFIG @@ -2049,22 +2052,16 @@ groups: type: pidAutotuneConfig_t condition: USE_AUTOTUNE_FIXED_WING members: - - name: fw_autotune_overshoot_time - description: "Time [ms] to detect sustained overshoot" - default_value: 100 - field: fw_overshoot_time + - name: fw_autotune_detect_time + description: "Time [ms] to detect sustained undershoot or overshoot" + default_value: 250 + field: fw_detect_time min: 50 max: 500 - - name: fw_autotune_undershoot_time - description: "Time [ms] to detect sustained undershoot" - default_value: 200 - field: fw_undershoot_time - min: 50 - max: 500 - - name: fw_autotune_threshold - description: "Threshold [%] of max rate to consider overshoot/undershoot detection" - default_value: 50 - field: fw_max_rate_threshold + - name: fw_autotune_min_stick + description: "Minimum stick input [%] to consider overshoot/undershoot detection" + default_value: 80 + field: fw_min_stick min: 0 max: 100 - name: fw_autotune_ff_to_p_gain @@ -2073,12 +2070,36 @@ groups: field: fw_ff_to_p_gain min: 0 max: 100 + - name: fw_autotune_p_to_d_gain + description: "P to D gain (strength relationship) [%]" + default_value: 0 + field: fw_p_to_d_gain + min: 0 + max: 200 - name: fw_autotune_ff_to_i_tc description: "FF to I time (defines time for I to reach the same level of response as FF) [ms]" default_value: 600 field: fw_ff_to_i_time_constant min: 100 max: 5000 + - name: fw_autotune_rate_adjustment + description: "`AUTO` and `LIMIT` adjust the rates to match the capabilities of the airplane, with `LIMIT` they are never increased above the starting rates setting. `FIXED` does not adjust the rates. Rates are not changed when tuning in `ANGLE` mode." + default_value: "AUTO" + field: fw_rate_adjustment + table: autotune_rate_adjustment + type: uint8_t + - name: fw_autotune_convergence_rate + description: "How much autotune adjusts the gains on each update. Setting this too high can cause erratically changing gains during autotune." + default_value: 10 + field: fw_convergence_rate + min: 1 + max: 50 + - name: fw_autotune_max_rate_deflection + description: "The target percentage of maximum mixer output used for determining the rates in `AUTO` and `LIMIT`." + default_value: 80 + field: fw_max_rate_deflection + min: 50 + max: 100 - name: PG_POSITION_ESTIMATION_CONFIG type: positionEstimationConfig_t diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index b8c4b94e24..ff3522c700 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -406,7 +406,7 @@ static float calculateFixedWingTPAFactor(uint16_t throttle) // tpa_rate is amount of curve TPA applied to PIDs // tpa_breakpoint for fixed wing is cruise throttle value (value at which PIDs were tuned) - if (currentControlRateProfile->throttle.dynPID != 0 && currentControlRateProfile->throttle.pa_breakpoint > getThrottleIdleValue()) { + if (currentControlRateProfile->throttle.dynPID != 0 && currentControlRateProfile->throttle.pa_breakpoint > getThrottleIdleValue() && !FLIGHT_MODE(AUTO_TUNE) && ARMING_FLAG(ARMED)) { if (throttle > getThrottleIdleValue()) { // Calculate TPA according to throttle tpaFactor = 0.5f + ((float)(currentControlRateProfile->throttle.pa_breakpoint - getThrottleIdleValue()) / (throttle - getThrottleIdleValue()) / 2.0f); @@ -717,14 +717,14 @@ static void NOINLINE pidApplyFixedWingRateController(pidState_t *pidState, fligh pidState->errorGyroIf = constrainf(pidState->errorGyroIf, -pidProfile()->fixedWingItermThrowLimit, pidProfile()->fixedWingItermThrowLimit); } + axisPID[axis] = constrainf(newPTerm + newFFTerm + pidState->errorGyroIf, -pidState->pidSumLimit, +pidState->pidSumLimit); + #ifdef USE_AUTOTUNE_FIXED_WING if (FLIGHT_MODE(AUTO_TUNE) && !FLIGHT_MODE(MANUAL_MODE)) { - autotuneFixedWingUpdate(axis, pidState->rateTarget, pidState->gyroRate, newPTerm + newFFTerm); + autotuneFixedWingUpdate(axis, pidState->rateTarget, pidState->gyroRate, constrainf(newFFTerm, -pidState->pidSumLimit, +pidState->pidSumLimit)); } #endif - axisPID[axis] = constrainf(newPTerm + newFFTerm + pidState->errorGyroIf + newDTerm, -pidState->pidSumLimit, +pidState->pidSumLimit); - #ifdef USE_BLACKBOX axisPID_P[axis] = newPTerm; axisPID_I[axis] = pidState->errorGyroIf; diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h index 3f3d5a3bcc..9405786049 100644 --- a/src/main/flight/pid.h +++ b/src/main/flight/pid.h @@ -24,7 +24,7 @@ #define PID_SUM_LIMIT_MIN 100 #define PID_SUM_LIMIT_MAX 1000 #define PID_SUM_LIMIT_DEFAULT 500 -#define PID_SUM_LIMIT_YAW_DEFAULT 350 +#define PID_SUM_LIMIT_YAW_DEFAULT 400 #define HEADING_HOLD_RATE_LIMIT_MIN 10 #define HEADING_HOLD_RATE_LIMIT_MAX 250 @@ -164,13 +164,22 @@ typedef struct pidProfile_s { } pidProfile_t; typedef struct pidAutotuneConfig_s { - uint16_t fw_overshoot_time; // Time [ms] to detect sustained overshoot - uint16_t fw_undershoot_time; // Time [ms] to detect sustained undershoot - uint8_t fw_max_rate_threshold; // Threshold [%] of max rate to consider autotune detection + uint16_t fw_detect_time; // Time [ms] to detect sustained undershoot or overshoot + uint8_t fw_min_stick; // Minimum stick input required to update rates and gains uint8_t fw_ff_to_p_gain; // FF to P gain (strength relationship) [%] + uint8_t fw_p_to_d_gain; // P to D gain (strength relationship) [%] uint16_t fw_ff_to_i_time_constant; // FF to I time (defines time for I to reach the same level of response as FF) [ms] + uint8_t fw_rate_adjustment; // Adjust rate settings during autotune? + uint8_t fw_convergence_rate; // Convergence rate + uint8_t fw_max_rate_deflection; // Percentage of max mixer output used for calculating the rates } pidAutotuneConfig_t; +typedef enum { + FIXED, + LIMIT, + AUTO, +} fw_autotune_rate_adjustment_e; + PG_DECLARE_PROFILE(pidProfile_t, pidProfile); PG_DECLARE(pidAutotuneConfig_t, pidAutotuneConfig); diff --git a/src/main/flight/pid_autotune.c b/src/main/flight/pid_autotune.c index 684f42d1b7..e2b54c5dc3 100755 --- a/src/main/flight/pid_autotune.c +++ b/src/main/flight/pid_autotune.c @@ -25,6 +25,8 @@ #include "blackbox/blackbox.h" #include "blackbox/blackbox_fielddefs.h" +#include "build/debug.h" + #include "common/axis.h" #include "common/maths.h" #include "common/utils.h" @@ -44,20 +46,23 @@ #include "flight/pid.h" -#define AUTOTUNE_FIXED_WING_INTEGRATOR_TC 600 -#define AUTOTUNE_FIXED_WING_DECREASE_STEP 8 // 8% -#define AUTOTUNE_FIXED_WING_INCREASE_STEP 5 // 5% #define AUTOTUNE_FIXED_WING_MIN_FF 10 #define AUTOTUNE_FIXED_WING_MAX_FF 200 +#define AUTOTUNE_FIXED_WING_MIN_ROLL_PITCH_RATE 40 +#define AUTOTUNE_FIXED_WING_MIN_YAW_RATE 10 +#define AUTOTUNE_FIXED_WING_MAX_RATE 720 -PG_REGISTER_WITH_RESET_TEMPLATE(pidAutotuneConfig_t, pidAutotuneConfig, PG_PID_AUTOTUNE_CONFIG, 0); +PG_REGISTER_WITH_RESET_TEMPLATE(pidAutotuneConfig_t, pidAutotuneConfig, PG_PID_AUTOTUNE_CONFIG, 2); PG_RESET_TEMPLATE(pidAutotuneConfig_t, pidAutotuneConfig, - .fw_overshoot_time = SETTING_FW_AUTOTUNE_OVERSHOOT_TIME_DEFAULT, - .fw_undershoot_time = SETTING_FW_AUTOTUNE_UNDERSHOOT_TIME_DEFAULT, - .fw_max_rate_threshold = SETTING_FW_AUTOTUNE_THRESHOLD_DEFAULT, + .fw_detect_time = SETTING_FW_AUTOTUNE_DETECT_TIME_DEFAULT, + .fw_min_stick = SETTING_FW_AUTOTUNE_MIN_STICK_DEFAULT, .fw_ff_to_p_gain = SETTING_FW_AUTOTUNE_FF_TO_P_GAIN_DEFAULT, .fw_ff_to_i_time_constant = SETTING_FW_AUTOTUNE_FF_TO_I_TC_DEFAULT, + .fw_p_to_d_gain = SETTING_FW_AUTOTUNE_P_TO_D_GAIN_DEFAULT, + .fw_rate_adjustment = SETTING_FW_AUTOTUNE_RATE_ADJUSTMENT_DEFAULT, + .fw_convergence_rate = SETTING_FW_AUTOTUNE_CONVERGENCE_RATE_DEFAULT, + .fw_max_rate_deflection = SETTING_FW_AUTOTUNE_MAX_RATE_DEFLECTION_DEFAULT, ); typedef enum { @@ -69,11 +74,15 @@ typedef enum { typedef struct { pidAutotuneState_e state; timeMs_t stateEnterTime; - - bool pidSaturated; float gainP; float gainI; + float gainD; float gainFF; + float rate; + float initialRate; + float maxAbsDesiredRateDps; + float maxAbsReachedRateDps; + float maxAbsPidOutput; } pidAutotuneData_t; #define AUTOTUNE_SAVE_PERIOD 5000 // Save interval is 5 seconds - when we turn off autotune we'll restore values from previous update at most 5 sec ago @@ -89,8 +98,9 @@ void autotuneUpdateGains(pidAutotuneData_t * data) for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { pidBankMutable()->pid[axis].P = lrintf(data[axis].gainP); pidBankMutable()->pid[axis].I = lrintf(data[axis].gainI); - pidBankMutable()->pid[axis].D = 0; + pidBankMutable()->pid[axis].D = lrintf(data[axis].gainD); pidBankMutable()->pid[axis].FF = lrintf(data[axis].gainFF); + ((controlRateConfig_t *)currentControlRateProfile)->stabilized.rates[axis] = lrintf(data[axis].rate/10.0f); } schedulePidGainsUpdate(); } @@ -114,10 +124,15 @@ void autotuneStart(void) for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { tuneCurrent[axis].gainP = pidBank()->pid[axis].P; tuneCurrent[axis].gainI = pidBank()->pid[axis].I; + tuneCurrent[axis].gainD = pidBank()->pid[axis].D; tuneCurrent[axis].gainFF = pidBank()->pid[axis].FF; - tuneCurrent[axis].pidSaturated = false; + tuneCurrent[axis].rate = currentControlRateProfile->stabilized.rates[axis] * 10.0f; tuneCurrent[axis].stateEnterTime = millis(); tuneCurrent[axis].state = DEMAND_TOO_LOW; + tuneCurrent[axis].initialRate = currentControlRateProfile->stabilized.rates[axis] * 10.0f; + tuneCurrent[axis].maxAbsDesiredRateDps = 0; + tuneCurrent[axis].maxAbsReachedRateDps = 0; + tuneCurrent[axis].maxAbsPidOutput = 0; } memcpy(tuneSaved, tuneCurrent, sizeof(pidAutotuneData_t) * XYZ_AXIS_COUNT); @@ -164,27 +179,35 @@ static void blackboxLogAutotuneEvent(adjustmentFunction_e adjustmentFunction, in void autotuneFixedWingUpdate(const flight_dynamics_index_t axis, float desiredRateDps, float reachedRateDps, float pidOutput) { const timeMs_t currentTimeMs = millis(); + const float convergenceRate = pidAutotuneConfig()->fw_convergence_rate / 100.0f; + float maxDesiredRateDps = tuneCurrent[axis].rate; + float gainFF = tuneCurrent[axis].gainFF; + const float absDesiredRateDps = fabsf(desiredRateDps); - float maxDesiredRate = currentControlRateProfile->stabilized.rates[axis] * 10.0f; + const float absReachedRateDps = fabsf(reachedRateDps); + const float absPidOutput = fabsf(pidOutput); + + const bool correctDirection = (desiredRateDps>0) == (reachedRateDps>0); + const float stickInput = absDesiredRateDps / maxDesiredRateDps; + + float pidSumTarget; + float pidSumLimit; + float rateFullStick; + float pidOutputRequired; pidAutotuneState_e newState; // Use different max desired rate in ANGLE for pitch and roll // Maximum reasonable error in ANGLE mode is 200% of angle inclination (control dublet), but we are conservative and tune for control singlet. if (FLIGHT_MODE(ANGLE_MODE) && (axis == FD_PITCH || axis == FD_ROLL)) { float maxDesiredRateInAngleMode = DECIDEGREES_TO_DEGREES(pidProfile()->max_angle_inclination[axis] * 1.0f) * pidBank()->pid[PID_LEVEL].P / FP_PID_LEVEL_P_MULTIPLIER; - maxDesiredRate = MIN(maxDesiredRate, maxDesiredRateInAngleMode); + maxDesiredRateDps = MIN(maxDesiredRateDps, maxDesiredRateInAngleMode); } - if (fabsf(pidOutput) >= pidProfile()->pidSumLimit) { - // If we have saturated the pid output by P+FF don't increase the gain - tuneCurrent[axis].pidSaturated = true; - } - - if (absDesiredRateDps < (pidAutotuneConfig()->fw_max_rate_threshold / 100.0f) * maxDesiredRate) { - // We can make decisions only when we are demanding at least 50% of max configured rate + if (stickInput < (pidAutotuneConfig()->fw_min_stick / 100.0f) || !correctDirection) { + // We can make decisions only when we are giving at least 80% stick input and the airplane is rotating in the requested direction newState = DEMAND_TOO_LOW; } - else if (fabsf(reachedRateDps) > absDesiredRateDps) { + else if (absReachedRateDps > absDesiredRateDps) { newState = DEMAND_OVERSHOOT; } else { @@ -192,60 +215,114 @@ void autotuneFixedWingUpdate(const flight_dynamics_index_t axis, float desiredRa } if (newState != tuneCurrent[axis].state) { - const timeDelta_t stateTimeMs = currentTimeMs - tuneCurrent[axis].stateEnterTime; - bool gainsUpdated = false; - - switch (tuneCurrent[axis].state) { - case DEMAND_TOO_LOW: - break; - case DEMAND_OVERSHOOT: - if (stateTimeMs >= pidAutotuneConfig()->fw_overshoot_time) { - tuneCurrent[axis].gainFF = tuneCurrent[axis].gainFF * (100 - AUTOTUNE_FIXED_WING_DECREASE_STEP) / 100.0f; - if (tuneCurrent[axis].gainFF < AUTOTUNE_FIXED_WING_MIN_FF) { - tuneCurrent[axis].gainFF = AUTOTUNE_FIXED_WING_MIN_FF; - } - gainsUpdated = true; - } - break; - case DEMAND_UNDERSHOOT: - if (stateTimeMs >= pidAutotuneConfig()->fw_undershoot_time && !tuneCurrent[axis].pidSaturated) { - tuneCurrent[axis].gainFF = tuneCurrent[axis].gainFF * (100 + AUTOTUNE_FIXED_WING_INCREASE_STEP) / 100.0f; - if (tuneCurrent[axis].gainFF > AUTOTUNE_FIXED_WING_MAX_FF) { - tuneCurrent[axis].gainFF = AUTOTUNE_FIXED_WING_MAX_FF; - } - gainsUpdated = true; - } - break; - } - - if (gainsUpdated) { - // Set P-gain to 10% of FF gain (quite agressive - FIXME) - tuneCurrent[axis].gainP = tuneCurrent[axis].gainFF * (pidAutotuneConfig()->fw_ff_to_p_gain / 100.0f); - - // Set integrator gain to reach the same response as FF gain in 0.667 second - tuneCurrent[axis].gainI = (tuneCurrent[axis].gainFF / FP_PID_RATE_FF_MULTIPLIER) * (1000.0f / pidAutotuneConfig()->fw_ff_to_i_time_constant) * FP_PID_RATE_I_MULTIPLIER; - tuneCurrent[axis].gainI = constrainf(tuneCurrent[axis].gainI, 2.0f, 50.0f); - autotuneUpdateGains(tuneCurrent); - - switch (axis) { - case FD_ROLL: - blackboxLogAutotuneEvent(ADJUSTMENT_ROLL_FF, tuneCurrent[axis].gainFF); - break; - - case FD_PITCH: - blackboxLogAutotuneEvent(ADJUSTMENT_PITCH_FF, tuneCurrent[axis].gainFF); - break; - - case FD_YAW: - blackboxLogAutotuneEvent(ADJUSTMENT_YAW_FF, tuneCurrent[axis].gainFF); - break; - } - } - - // Change state and reset saturation flag tuneCurrent[axis].state = newState; tuneCurrent[axis].stateEnterTime = currentTimeMs; - tuneCurrent[axis].pidSaturated = false; + tuneCurrent[axis].maxAbsDesiredRateDps = 0; + tuneCurrent[axis].maxAbsReachedRateDps = 0; + tuneCurrent[axis].maxAbsPidOutput = 0; + } + + switch (tuneCurrent[axis].state){ + case DEMAND_TOO_LOW: + break; + case DEMAND_UNDERSHOOT: + case DEMAND_OVERSHOOT: + { + const timeDelta_t stateTimeMs = currentTimeMs - tuneCurrent[axis].stateEnterTime; + bool gainsUpdated = false; + bool ratesUpdated = false; + + // Record max values + tuneCurrent[axis].maxAbsDesiredRateDps = MAX(tuneCurrent[axis].maxAbsDesiredRateDps, absDesiredRateDps); + tuneCurrent[axis].maxAbsReachedRateDps = MAX(tuneCurrent[axis].maxAbsReachedRateDps, absReachedRateDps); + tuneCurrent[axis].maxAbsPidOutput = MAX(tuneCurrent[axis].maxAbsPidOutput, absPidOutput); + + if (stateTimeMs >= pidAutotuneConfig()->fw_detect_time) { + if (pidAutotuneConfig()->fw_rate_adjustment != FIXED && !FLIGHT_MODE(ANGLE_MODE)) { + // Tuning the rates is not compatible with ANGLE mode + + // Target 80% control surface deflection to leave some room for P and I to work + pidSumLimit = (axis == FD_YAW) ? pidProfile()->pidSumLimitYaw : pidProfile()->pidSumLimit; + pidSumTarget = (pidAutotuneConfig()->fw_max_rate_deflection / 100.0f) * pidSumLimit; + + // Theoretically achievable rate with target deflection + rateFullStick = (pidSumTarget / tuneCurrent[axis].maxAbsPidOutput) * tuneCurrent[axis].maxAbsReachedRateDps; + + // Rate update + if (rateFullStick > (maxDesiredRateDps + 10.0f)) { + maxDesiredRateDps += 10.0f; + } else if (rateFullStick < (maxDesiredRateDps - 10.0f)) { + maxDesiredRateDps -= 10.0f; + } + + // Constrain to safe values + uint16_t minRate = (axis == FD_YAW) ? AUTOTUNE_FIXED_WING_MIN_YAW_RATE : AUTOTUNE_FIXED_WING_MIN_ROLL_PITCH_RATE; + uint16_t maxRate = (pidAutotuneConfig()->fw_rate_adjustment == AUTO) ? AUTOTUNE_FIXED_WING_MAX_RATE : MAX(tuneCurrent[axis].initialRate, minRate); + tuneCurrent[axis].rate = constrainf(maxDesiredRateDps, minRate, maxRate); + ratesUpdated = true; + } + + // Update FF towards value needed to achieve current rate target + pidOutputRequired = MIN(tuneCurrent[axis].maxAbsPidOutput * (tuneCurrent[axis].maxAbsDesiredRateDps / tuneCurrent[axis].maxAbsReachedRateDps), pidSumLimit); + gainFF += (pidOutputRequired / tuneCurrent[axis].maxAbsDesiredRateDps * FP_PID_RATE_FF_MULTIPLIER - gainFF) * convergenceRate * stickInput; + tuneCurrent[axis].gainFF = constrainf(gainFF, AUTOTUNE_FIXED_WING_MIN_FF, AUTOTUNE_FIXED_WING_MAX_FF); + gainsUpdated = true; + + // Reset state flag + tuneCurrent[axis].state = DEMAND_TOO_LOW; + } + + if (gainsUpdated) { + // Set P-gain to 10% of FF gain (quite agressive - FIXME) + tuneCurrent[axis].gainP = tuneCurrent[axis].gainFF * (pidAutotuneConfig()->fw_ff_to_p_gain / 100.0f); + + // Set D-gain relative to P-gain (0 for now) + tuneCurrent[axis].gainD = tuneCurrent[axis].gainP * (pidAutotuneConfig()->fw_p_to_d_gain / 100.0f); + + // Set integrator gain to reach the same response as FF gain in 0.667 second + tuneCurrent[axis].gainI = (tuneCurrent[axis].gainFF / FP_PID_RATE_FF_MULTIPLIER) * (1000.0f / pidAutotuneConfig()->fw_ff_to_i_time_constant) * FP_PID_RATE_I_MULTIPLIER; + tuneCurrent[axis].gainI = constrainf(tuneCurrent[axis].gainI, 2.0f, 50.0f); + autotuneUpdateGains(tuneCurrent); + + switch (axis) { + case FD_ROLL: + blackboxLogAutotuneEvent(ADJUSTMENT_ROLL_FF, tuneCurrent[axis].gainFF); + break; + + case FD_PITCH: + blackboxLogAutotuneEvent(ADJUSTMENT_PITCH_FF, tuneCurrent[axis].gainFF); + break; + + case FD_YAW: + blackboxLogAutotuneEvent(ADJUSTMENT_YAW_FF, tuneCurrent[axis].gainFF); + break; + } + + // Debug + DEBUG_SET(DEBUG_AUTOTUNE, axis + 3, pidOutputRequired); + } + + if (ratesUpdated) { + autotuneUpdateGains(tuneCurrent); + + switch (axis) { + case FD_ROLL: + blackboxLogAutotuneEvent(ADJUSTMENT_ROLL_RATE, tuneCurrent[axis].rate); + break; + + case FD_PITCH: + blackboxLogAutotuneEvent(ADJUSTMENT_PITCH_RATE, tuneCurrent[axis].rate); + break; + + case FD_YAW: + blackboxLogAutotuneEvent(ADJUSTMENT_YAW_RATE, tuneCurrent[axis].rate); + break; + } + + // Debug + DEBUG_SET(DEBUG_AUTOTUNE, axis, rateFullStick); + } + } } } #endif From 7d404863f997618138e70d79918d1606a37a8393 Mon Sep 17 00:00:00 2001 From: Harry Phillips Date: Sat, 10 Apr 2021 00:10:37 +0100 Subject: [PATCH 146/286] Settings fix --- src/main/fc/settings.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 63239d51d6..b658aaca71 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -2813,10 +2813,10 @@ groups: - name: mavlink_version field: mavlink.version description: "Version of MAVLink to use" - default_value: "2" type: uint8_t min: 1 max: 2 + default_value: 2 - name: PG_ELERES_CONFIG type: eleresConfig_t headers: ["rx/eleres.h"] From 96cd26461c53a0328536812fb063ae56483b2773 Mon Sep 17 00:00:00 2001 From: breadoven <56191411+breadoven@users.noreply.github.com> Date: Sat, 10 Apr 2021 13:14:26 +0100 Subject: [PATCH 147/286] Update settings.yaml --- src/main/fc/settings.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index af53ec3850..cbde7b7ca9 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -2317,7 +2317,7 @@ groups: table: nav_overrides_motor_stop - name: nav_rth_climb_first description: "If set to ON or ON_FW_SPIRAL aircraft will climb to nav_rth_altitude first before turning to head home. If set to OFF aircraft will turn and head home immediately climbing on the way. For a fixed wing ON will use a linear climb, ON_FW_SPIRAL will use a loiter turning climb with climb rate set by nav_auto_climb_rate, turn rate set by nav_fw_loiter_radius (ON_FW_SPIRAL is a fixed wing setting and behaves the same as ON for a multirotor)" - default_value: ON + default_value: "ON" field: general.flags.rth_climb_first table: nav_rth_climb_first - name: nav_rth_climb_ignore_emerg From c0e98e4fc58cae6ca97b9e068a41abe1edbb6b73 Mon Sep 17 00:00:00 2001 From: breadoven <56191411+breadoven@users.noreply.github.com> Date: Sat, 10 Apr 2021 13:46:08 +0100 Subject: [PATCH 148/286] Update settings.yaml --- src/main/fc/settings.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 6e4e29da99..875ca28165 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -2339,7 +2339,7 @@ groups: table: nav_rth_alt_mode - name: nav_rth_alt_control_override description: "If set to ON RTH altitude and CLIMB FIRST settings can be overridden during the RTH climb phase using full pitch or roll stick held for > 2 seconds. RTH altitude is reset to the current altitude using pitch down stick. RTH CLIMB FIRST is overridden using right roll stick so craft turns and heads directly to home." - default_value: "OFF" + default_value: OFF field: general.flags.rth_alt_control_override type: bool - name: nav_rth_abort_threshold From 9138229d792c1fe30d345c7e80762341a2741b04 Mon Sep 17 00:00:00 2001 From: Harry Phillips Date: Sat, 10 Apr 2021 21:36:52 +0100 Subject: [PATCH 149/286] DShot Beeper initial implementation --- src/main/drivers/pwm_output.h | 2 +- src/main/fc/cli.c | 6 ++--- src/main/fc/config.h | 2 ++ src/main/fc/fc_core.c | 17 +++++++++++++++ src/main/fc/fc_core.h | 1 + src/main/fc/fc_tasks.c | 4 ++-- src/main/fc/runtime_config.c | 5 +++-- src/main/fc/runtime_config.h | 3 ++- src/main/fc/settings.yaml | 17 +++++++++++++++ src/main/flight/mixer.c | 18 ++++++++++++++- src/main/flight/mixer.h | 3 ++- src/main/io/beeper.c | 40 ++++++++++++++++++++++++++++++++++ src/main/io/beeper.h | 4 ++++ src/main/io/osd.c | 2 ++ src/main/io/osd.h | 1 + src/main/io/osd_dji_hd.c | 2 ++ src/main/io/serial_4way.c | 4 ++-- src/main/scheduler/scheduler.h | 2 +- 18 files changed, 119 insertions(+), 14 deletions(-) diff --git a/src/main/drivers/pwm_output.h b/src/main/drivers/pwm_output.h index f1f2c1f646..b78d5b5e17 100644 --- a/src/main/drivers/pwm_output.h +++ b/src/main/drivers/pwm_output.h @@ -54,5 +54,5 @@ bool pwmServoConfig(const struct timerHardware_s *timerHardware, uint8_t servoIn void pwmWriteBeeper(bool onoffBeep); void beeperPwmInit(ioTag_t tag, uint16_t frequency); -void sendDShotCommand(dshotCommands_e directionSpin); +void sendDShotCommand(dshotCommands_e cmd); void initDShotCommands(void); \ No newline at end of file diff --git a/src/main/fc/cli.c b/src/main/fc/cli.c index 68f951f687..3ef1070a48 100755 --- a/src/main/fc/cli.c +++ b/src/main/fc/cli.c @@ -2501,7 +2501,7 @@ static void cliFeature(char *cmdline) } } -#ifdef BEEPER +#if defined(BEEPER) || defined(USE_DSHOT) static void printBeeper(uint8_t dumpMask, const beeperConfig_t *beeperConfig, const beeperConfig_t *beeperConfigDefault) { const uint8_t beeperCount = beeperTableEntryCount(); @@ -3484,7 +3484,7 @@ static void printConfig(const char *cmdline, bool doDiff) cliPrintHashLine("feature"); printFeature(dumpMask, &featureConfig_Copy, featureConfig()); -#ifdef BEEPER +#if defined(BEEPER) || defined(USE_DSHOT) cliPrintHashLine("beeper"); printBeeper(dumpMask, &beeperConfig_Copy, beeperConfig()); #endif @@ -3655,7 +3655,7 @@ const clicmd_t cmdTable[] = { #ifdef USE_CLI_BATCH CLI_COMMAND_DEF("batch", "start or end a batch of commands", "start | end", cliBatch), #endif -#ifdef BEEPER +#if defined(BEEPER) || defined(USE_DSHOT) CLI_COMMAND_DEF("beeper", "turn on/off beeper", "list\r\n" "\t<+|->[name]", cliBeeper), #endif diff --git a/src/main/fc/config.h b/src/main/fc/config.h index 48dc455e71..15aa89edfc 100644 --- a/src/main/fc/config.h +++ b/src/main/fc/config.h @@ -88,6 +88,8 @@ PG_DECLARE(systemConfig_t, systemConfig); typedef struct beeperConfig_s { uint32_t beeper_off_flags; uint32_t preferred_beeper_off_flags; + bool dshot_beeper_enabled; + uint8_t dshot_beeper_tone; } beeperConfig_t; PG_DECLARE(beeperConfig_t, beeperConfig); diff --git a/src/main/fc/fc_core.c b/src/main/fc/fc_core.c index 40c18fd31f..c43a9d86f3 100755 --- a/src/main/fc/fc_core.c +++ b/src/main/fc/fc_core.c @@ -131,6 +131,7 @@ uint8_t motorControlEnable = false; static bool isRXDataNew; static uint32_t gyroSyncFailureCount; static disarmReason_t lastDisarmReason = DISARM_NONE; +timeUs_t lastDisarmTimeUs = 0; static emergencyArmingState_t emergencyArming; static bool prearmWasReset = false; // Prearm must be reset (RC Mode not active) before arming is possible @@ -182,6 +183,8 @@ static void updateArmingStatus(void) if (ARMING_FLAG(ARMED)) { LED0_ON; } else { + const timeUs_t currentTimeUs = micros(); + /* CHECK: Run-time calibration */ static bool calibratingFinishedBeep = false; if (isCalibrating()) { @@ -300,6 +303,15 @@ static void updateArmingStatus(void) DISABLE_ARMING_FLAG(ARMING_DISABLED_SERVO_AUTOTRIM); } +#ifdef USE_DSHOT + /* CHECK: Don't arm if the DShot beeper was used recently, as there is a minimum delay before sending the next DShot command */ + if (currentTimeUs - getLastDshotBeeperCommandTimeUs() < getDShotBeaconGuardDelayUs()) { + ENABLE_ARMING_FLAG(ARMING_DISABLED_DSHOT_BEEPER); + } else { + DISABLE_ARMING_FLAG(ARMING_DISABLED_DSHOT_BEEPER); + } +#endif + if (isModeActivationConditionPresent(BOXPREARM)) { if (IS_RC_MODE_ACTIVE(BOXPREARM)) { if (prearmWasReset && (armingConfig()->prearmTimeoutMs == 0 || millis() - prearmActivationTime < armingConfig()->prearmTimeoutMs)) { @@ -402,6 +414,7 @@ void disarm(disarmReason_t disarmReason) { if (ARMING_FLAG(ARMED)) { lastDisarmReason = disarmReason; + lastDisarmTimeUs = micros(); DISABLE_ARMING_FLAG(ARMED); #ifdef USE_BLACKBOX @@ -428,6 +441,10 @@ void disarm(disarmReason_t disarmReason) } } +timeUs_t getLastDisarmTimeUs(void) { + return lastDisarmTimeUs; +} + disarmReason_t getDisarmReason(void) { return lastDisarmReason; diff --git a/src/main/fc/fc_core.h b/src/main/fc/fc_core.h index 34eb6fe68b..40331dfdbd 100644 --- a/src/main/fc/fc_core.h +++ b/src/main/fc/fc_core.h @@ -37,6 +37,7 @@ typedef enum disarmReason_e { void handleInflightCalibrationStickPosition(void); void disarm(disarmReason_t disarmReason); +timeUs_t getLastDisarmTimeUs(void); void tryArm(void); disarmReason_t getDisarmReason(void); diff --git a/src/main/fc/fc_tasks.c b/src/main/fc/fc_tasks.c index 33d417a1d6..9bb54d7671 100755 --- a/src/main/fc/fc_tasks.c +++ b/src/main/fc/fc_tasks.c @@ -306,7 +306,7 @@ void fcTasksInit(void) setTaskEnabled(TASK_AUX, true); setTaskEnabled(TASK_SERIAL, true); -#ifdef BEEPER +#if defined(BEEPER) || defined(USE_DSHOT) setTaskEnabled(TASK_BEEPER, true); #endif #ifdef USE_LIGHTS @@ -401,7 +401,7 @@ cfTask_t cfTasks[TASK_COUNT] = { .staticPriority = TASK_PRIORITY_LOW, }, -#ifdef BEEPER +#if defined(BEEPER) || defined(USE_DSHOT) [TASK_BEEPER] = { .taskName = "BEEPER", .taskFunc = beeperUpdate, diff --git a/src/main/fc/runtime_config.c b/src/main/fc/runtime_config.c index 47d6267cbd..6f7241244d 100644 --- a/src/main/fc/runtime_config.c +++ b/src/main/fc/runtime_config.c @@ -36,7 +36,7 @@ const char *armingDisableFlagNames[]= { "FS", "ANGLE", "CAL", "OVRLD", "NAV", "COMPASS", "ACC", "ARMSW", "HWFAIL", "BOXFS", "KILLSW", "RX", "THR", "CLI", "CMS", "OSD", "ROLL/PITCH", "AUTOTRIM", "OOM", - "SETTINGFAIL", "PWMOUT", "NOPREARM" + "SETTINGFAIL", "PWMOUT", "NOPREARM", "DSHOTBEEPER" }; #endif @@ -58,7 +58,8 @@ const armingFlag_e armDisableReasonsChecklist[] = { ARMING_DISABLED_ROLLPITCH_NOT_CENTERED, ARMING_DISABLED_SERVO_AUTOTRIM, ARMING_DISABLED_OOM, - ARMING_DISABLED_NO_PREARM + ARMING_DISABLED_NO_PREARM, + ARMING_DISABLED_DSHOT_BEEPER }; armingFlag_e isArmingDisabledReason(void) diff --git a/src/main/fc/runtime_config.h b/src/main/fc/runtime_config.h index 9f6ec9efa3..84fe113c59 100644 --- a/src/main/fc/runtime_config.h +++ b/src/main/fc/runtime_config.h @@ -44,6 +44,7 @@ typedef enum { ARMING_DISABLED_INVALID_SETTING = (1 << 26), ARMING_DISABLED_PWM_OUTPUT_ERROR = (1 << 27), ARMING_DISABLED_NO_PREARM = (1 << 28), + ARMING_DISABLED_DSHOT_BEEPER = (1 << 29), ARMING_DISABLED_ALL_FLAGS = (ARMING_DISABLED_FAILSAFE_SYSTEM | ARMING_DISABLED_NOT_LEVEL | ARMING_DISABLED_SENSORS_CALIBRATING | ARMING_DISABLED_SYSTEM_OVERLOADED | ARMING_DISABLED_NAVIGATION_UNSAFE | @@ -52,7 +53,7 @@ typedef enum { ARMING_DISABLED_BOXKILLSWITCH | ARMING_DISABLED_RC_LINK | ARMING_DISABLED_THROTTLE | ARMING_DISABLED_CLI | ARMING_DISABLED_CMS_MENU | ARMING_DISABLED_OSD_MENU | ARMING_DISABLED_ROLLPITCH_NOT_CENTERED | ARMING_DISABLED_SERVO_AUTOTRIM | ARMING_DISABLED_OOM | ARMING_DISABLED_INVALID_SETTING | - ARMING_DISABLED_PWM_OUTPUT_ERROR | ARMING_DISABLED_NO_PREARM), + ARMING_DISABLED_PWM_OUTPUT_ERROR | ARMING_DISABLED_NO_PREARM | ARMING_DISABLED_DSHOT_BEEPER), } armingFlag_e; // Arming blockers that can be overriden by emergency arming. diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index b658aaca71..f550e2734d 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -3490,3 +3490,20 @@ groups: field: speedSource table: djiOsdSpeedSource type: uint8_t + + - name: PG_BEEPER_CONFIG + type: beeperConfig_t + headers: [ "fc/config.h" ] + members: + - name: dshot_beeper_enabled + description: "Whether using DShot motors as beepers is enabled" + default_value: ON + field: dshot_beeper_enabled + type: bool + - name: dshot_beeper_tone + description: "Sets the DShot beeper tone" + min: 1 + max: 5 + default_value: 1 + field: dshot_beeper_tone + type: uint8_t diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c index af1ca40a20..cc9f6b2bcc 100755 --- a/src/main/flight/mixer.c +++ b/src/main/flight/mixer.c @@ -73,6 +73,8 @@ static EXTENDED_FASTRAM int throttleRangeMin = 0; static EXTENDED_FASTRAM int throttleRangeMax = 0; static EXTENDED_FASTRAM int8_t motorYawMultiplier = 1; +int motorZeroCommand = 0; + PG_REGISTER_WITH_RESET_TEMPLATE(reversibleMotorsConfig_t, reversibleMotorsConfig, PG_REVERSIBLE_MOTORS_CONFIG, 0); PG_RESET_TEMPLATE(reversibleMotorsConfig_t, reversibleMotorsConfig, @@ -273,7 +275,6 @@ void mixerInit(void) void mixerResetDisarmedMotors(void) { - int motorZeroCommand; if (feature(FEATURE_REVERSIBLE_MOTORS)) { motorZeroCommand = reversibleMotorsConfig()->neutral; @@ -720,3 +721,18 @@ void loadPrimaryMotorMixer(void) { currentMixer[i] = *primaryMotorMixer(i); } } + +bool areMotorsRunning(void) +{ + if (ARMING_FLAG(ARMED)) { + return true; + } else { + for (int i = 0; i < motorCount; i++) { + if (motor_disarmed[i] != motorZeroCommand) { + return true; + } + } + } + + return false; +} diff --git a/src/main/flight/mixer.h b/src/main/flight/mixer.h index 7f5088fafc..2fcb25647b 100644 --- a/src/main/flight/mixer.h +++ b/src/main/flight/mixer.h @@ -130,4 +130,5 @@ void processServoAutotrim(void); void stopMotors(void); void stopPwmAllMotors(void); -void loadPrimaryMotorMixer(void); \ No newline at end of file +void loadPrimaryMotorMixer(void); +bool areMotorsRunning(void); \ No newline at end of file diff --git a/src/main/io/beeper.c b/src/main/io/beeper.c index e24216cda0..999cee97d2 100644 --- a/src/main/io/beeper.c +++ b/src/main/io/beeper.c @@ -45,6 +45,13 @@ #include "config/feature.h" +#ifdef USE_DSHOT +#include "drivers/pwm_output.h" +#include "fc/fc_core.h" +#include "flight/mixer.h" +static timeUs_t lastDshotBeeperCommandTimeUs; +#endif + #include "io/beeper.h" #define MAX_MULTI_BEEPS 20 //size limit for 'beep_multiBeeps[]' @@ -331,6 +338,17 @@ void beeperUpdate(timeUs_t currentTimeUs) } if (!beeperIsOn) { +#ifdef USE_DSHOT + if (!areMotorsRunning() + && beeperConfig()->dshot_beeper_enabled + && (currentBeeperEntry->mode == BEEPER_RX_SET || currentBeeperEntry->mode == BEEPER_RX_LOST) + && currentTimeUs - getLastDisarmTimeUs() > getDShotBeaconGuardDelayUs()) + { + lastDshotBeeperCommandTimeUs = currentTimeUs; + sendDShotCommand(beeperConfig()->dshot_beeper_tone); + } +#endif + beeperIsOn = 1; if (currentBeeperEntry->sequence[beeperPos] != 0) { if (!(getBeeperOffMask() & (1 << (currentBeeperEntry->mode - 1)))) @@ -407,3 +425,25 @@ int beeperTableEntryCount(void) { return (int)BEEPER_TABLE_ENTRY_COUNT; } + +#ifdef USE_DSHOT +timeUs_t getDShotBeaconGuardDelayUs(void) { + // Based on Digital_Cmd_Spec.txt - all delays have 100ms added to ensure that the minimum time has passed. + switch (beeperConfig()->dshot_beeper_tone) { + case 1: + case 2: + return 260000 + 100000; + case 3: + case 4: + return 280000 + 100000; + case 5: + default: + return 1020000 + 100000; + } +} + +timeUs_t getLastDshotBeeperCommandTimeUs(void) +{ + return lastDshotBeeperCommandTimeUs; +} +#endif diff --git a/src/main/io/beeper.h b/src/main/io/beeper.h index 0fcc8249dd..38f38b236e 100644 --- a/src/main/io/beeper.h +++ b/src/main/io/beeper.h @@ -60,3 +60,7 @@ uint32_t getArmingBeepTimeMicros(void); beeperMode_e beeperModeForTableIndex(int idx); const char *beeperNameForTableIndex(int idx); int beeperTableEntryCount(void); +#ifdef USE_DSHOT +timeUs_t getDShotBeaconGuardDelayUs(void); +timeUs_t getLastDshotBeeperCommandTimeUs(void); +#endif diff --git a/src/main/io/osd.c b/src/main/io/osd.c index 7c8825e3a1..c0e1c0e103 100644 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -736,6 +736,8 @@ static const char * osdArmingDisabledReasonMessage(void) return OSD_MESSAGE_STR(OSD_MSG_PWM_INIT_ERROR); case ARMING_DISABLED_NO_PREARM: return OSD_MESSAGE_STR(OSD_MSG_NO_PREARM); + case ARMING_DISABLED_DSHOT_BEEPER: + return OSD_MESSAGE_STR(OSD_MSG_DSHOT_BEEPER); // Cases without message case ARMING_DISABLED_CMS_MENU: FALLTHROUGH; diff --git a/src/main/io/osd.h b/src/main/io/osd.h index 1dc185b0d1..8a1b0d06e5 100755 --- a/src/main/io/osd.h +++ b/src/main/io/osd.h @@ -77,6 +77,7 @@ #define OSD_MSG_CLI_ACTIVE "CLI IS ACTIVE" #define OSD_MSG_PWM_INIT_ERROR "PWM INIT ERROR" #define OSD_MSG_NO_PREARM "NO PREARM" +#define OSD_MSG_DSHOT_BEEPER "DSHOT BEEPER" #define OSD_MSG_RTH_FS "(RTH)" #define OSD_MSG_EMERG_LANDING_FS "(EMERGENCY LANDING)" #define OSD_MSG_MOVE_EXIT_FS "!MOVE STICKS TO EXIT FS!" diff --git a/src/main/io/osd_dji_hd.c b/src/main/io/osd_dji_hd.c index ed72a973d3..6033801889 100644 --- a/src/main/io/osd_dji_hd.c +++ b/src/main/io/osd_dji_hd.c @@ -505,6 +505,8 @@ static const char * osdArmingDisabledReasonMessage(void) return OSD_MESSAGE_STR("PWM ERR"); case ARMING_DISABLED_NO_PREARM: return OSD_MESSAGE_STR("NO PREARM"); + case ARMING_DISABLED_DSHOT_BEEPER: + return OSD_MESSAGE_STR("DSHOT BEEPER"); // Cases without message case ARMING_DISABLED_CMS_MENU: FALLTHROUGH; diff --git a/src/main/io/serial_4way.c b/src/main/io/serial_4way.c index a45f41c8bc..0119e61e75 100644 --- a/src/main/io/serial_4way.c +++ b/src/main/io/serial_4way.c @@ -427,11 +427,11 @@ void esc4wayProcess(serialPort_t *mspPort) port = mspPort; // Start here with UART Main loop - #ifdef BEEPER +#if defined(BEEPER) || defined(USE_DSHOT) // fix for buzzer often starts beeping continuously when the ESCs are read // switch beeper silent here beeperSilence(); - #endif +#endif bool isExitScheduled = false; while (1) { diff --git a/src/main/scheduler/scheduler.h b/src/main/scheduler/scheduler.h index 0ad456967c..1a98f37b38 100755 --- a/src/main/scheduler/scheduler.h +++ b/src/main/scheduler/scheduler.h @@ -56,7 +56,7 @@ typedef enum { TASK_SERIAL, TASK_BATTERY, TASK_TEMPERATURE, -#ifdef BEEPER +#if defined(BEEPER) || defined(USE_DSHOT) TASK_BEEPER, #endif #ifdef USE_LIGHTS From 18c5fb5d8f42d2f4386e176ddca0d386a2300e23 Mon Sep 17 00:00:00 2001 From: Harry Phillips Date: Sat, 10 Apr 2021 22:28:47 +0100 Subject: [PATCH 150/286] Bugfixes --- docs/Settings.md | 2 ++ src/main/fc/config.c | 9 ++++++++- src/main/fc/config.h | 1 + src/main/fc/settings.yaml | 30 +++++++++++++++--------------- src/main/io/osd.h | 2 +- src/main/io/osd_dji_hd.c | 2 +- 6 files changed, 28 insertions(+), 18 deletions(-) diff --git a/docs/Settings.md b/docs/Settings.md index 9c67d1f059..4627cc6d7c 100644 --- a/docs/Settings.md +++ b/docs/Settings.md @@ -67,6 +67,8 @@ | dji_speed_source | GROUND | Sets the speed type displayed by the DJI OSD: GROUND, 3D, AIR | | dji_use_name_for_messages | ON | Re-purpose the craft name field for messages. Replace craft name with :WTSED for Warnings|Throttle|Speed|Efficiency|Trip distance | | dji_workarounds | 1 | Enables workarounds for different versions of MSP protocol used | +| dshot_beeper_enabled | ON | Whether using DShot motors as beepers is enabled | +| dshot_beeper_tone | 1 | Sets the DShot beeper tone | | dterm_lpf2_hz | 0 | Cutoff frequency for stage 2 D-term low pass filter | | dterm_lpf2_type | BIQUAD | Defines the type of stage 1 D-term LPF filter. Possible values: `PT1`, `BIQUAD`. `PT1` offers faster filter response while `BIQUAD` better attenuation. | | dterm_lpf_hz | 40 | Dterm low pass filter cutoff frequency. Default setting is very conservative and small multirotors should use higher value between 80 and 100Hz. 80 seems like a gold spot for 7-inch builds while 100 should work best with 5-inch machines. If motors are getting too hot, lower the value | diff --git a/src/main/fc/config.c b/src/main/fc/config.c index d38554bd21..46ddb52ecd 100755 --- a/src/main/fc/config.c +++ b/src/main/fc/config.c @@ -123,7 +123,14 @@ PG_RESET_TEMPLATE(systemConfig_t, systemConfig, .name = SETTING_NAME_DEFAULT ); -PG_REGISTER(beeperConfig_t, beeperConfig, PG_BEEPER_CONFIG, 0); +PG_REGISTER(beeperConfig_t, beeperConfig, PG_BEEPER_CONFIG, 1); + +PG_RESET_TEMPLATE(beeperConfig_t, beeperConfig, + .beeper_off_flags = 0, + .preferred_beeper_off_flags = 0, + .dshot_beeper_enabled = SETTING_DSHOT_BEEPER_ENABLED_DEFAULT, + .dshot_beeper_tone = SETTING_DSHOT_BEEPER_TONE_DEFAULT, +); PG_REGISTER_WITH_RESET_TEMPLATE(adcChannelConfig_t, adcChannelConfig, PG_ADC_CHANNEL_CONFIG, 0); diff --git a/src/main/fc/config.h b/src/main/fc/config.h index 15aa89edfc..b293396dfc 100644 --- a/src/main/fc/config.h +++ b/src/main/fc/config.h @@ -24,6 +24,7 @@ #include "drivers/adc.h" #include "drivers/rx_pwm.h" #include "fc/stats.h" +#include "fc/settings.h" #define MAX_PROFILE_COUNT 3 #define ONESHOT_FEATURE_CHANGED_DELAY_ON_BOOT_MS 1500 diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index f550e2734d..fdee714fd8 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -3492,18 +3492,18 @@ groups: type: uint8_t - name: PG_BEEPER_CONFIG - type: beeperConfig_t - headers: [ "fc/config.h" ] - members: - - name: dshot_beeper_enabled - description: "Whether using DShot motors as beepers is enabled" - default_value: ON - field: dshot_beeper_enabled - type: bool - - name: dshot_beeper_tone - description: "Sets the DShot beeper tone" - min: 1 - max: 5 - default_value: 1 - field: dshot_beeper_tone - type: uint8_t + type: beeperConfig_t + headers: [ "fc/config.h" ] + members: + - name: dshot_beeper_enabled + description: "Whether using DShot motors as beepers is enabled" + default_value: ON + field: dshot_beeper_enabled + type: bool + - name: dshot_beeper_tone + description: "Sets the DShot beeper tone" + min: 1 + max: 5 + default_value: 1 + field: dshot_beeper_tone + type: uint8_t diff --git a/src/main/io/osd.h b/src/main/io/osd.h index 8a1b0d06e5..75ff0e1c4b 100755 --- a/src/main/io/osd.h +++ b/src/main/io/osd.h @@ -77,7 +77,7 @@ #define OSD_MSG_CLI_ACTIVE "CLI IS ACTIVE" #define OSD_MSG_PWM_INIT_ERROR "PWM INIT ERROR" #define OSD_MSG_NO_PREARM "NO PREARM" -#define OSD_MSG_DSHOT_BEEPER "DSHOT BEEPER" +#define OSD_MSG_DSHOT_BEEPER "MOTOR BEEPER ACTIVE" #define OSD_MSG_RTH_FS "(RTH)" #define OSD_MSG_EMERG_LANDING_FS "(EMERGENCY LANDING)" #define OSD_MSG_MOVE_EXIT_FS "!MOVE STICKS TO EXIT FS!" diff --git a/src/main/io/osd_dji_hd.c b/src/main/io/osd_dji_hd.c index 6033801889..9f6880b27b 100644 --- a/src/main/io/osd_dji_hd.c +++ b/src/main/io/osd_dji_hd.c @@ -506,7 +506,7 @@ static const char * osdArmingDisabledReasonMessage(void) case ARMING_DISABLED_NO_PREARM: return OSD_MESSAGE_STR("NO PREARM"); case ARMING_DISABLED_DSHOT_BEEPER: - return OSD_MESSAGE_STR("DSHOT BEEPER"); + return OSD_MESSAGE_STR("MOTOR BEEPER ACTIVE"); // Cases without message case ARMING_DISABLED_CMS_MENU: FALLTHROUGH; From 4f58b89735f92bc16887978d6ecf7126db7ee0d4 Mon Sep 17 00:00:00 2001 From: Harry Phillips Date: Sat, 10 Apr 2021 23:11:08 +0100 Subject: [PATCH 151/286] Remove import --- src/main/fc/config.h | 1 - 1 file changed, 1 deletion(-) diff --git a/src/main/fc/config.h b/src/main/fc/config.h index b293396dfc..15aa89edfc 100644 --- a/src/main/fc/config.h +++ b/src/main/fc/config.h @@ -24,7 +24,6 @@ #include "drivers/adc.h" #include "drivers/rx_pwm.h" #include "fc/stats.h" -#include "fc/settings.h" #define MAX_PROFILE_COUNT 3 #define ONESHOT_FEATURE_CHANGED_DELAY_ON_BOOT_MS 1500 From e535c70e9fcf28a86ed41a981c02c6469716f298 Mon Sep 17 00:00:00 2001 From: Harry Phillips Date: Sat, 10 Apr 2021 23:17:53 +0100 Subject: [PATCH 152/286] Fix unused variable when DShot not enabled --- src/main/fc/fc_core.c | 4 +--- 1 file changed, 1 insertion(+), 3 deletions(-) diff --git a/src/main/fc/fc_core.c b/src/main/fc/fc_core.c index c43a9d86f3..28cec05d75 100755 --- a/src/main/fc/fc_core.c +++ b/src/main/fc/fc_core.c @@ -183,8 +183,6 @@ static void updateArmingStatus(void) if (ARMING_FLAG(ARMED)) { LED0_ON; } else { - const timeUs_t currentTimeUs = micros(); - /* CHECK: Run-time calibration */ static bool calibratingFinishedBeep = false; if (isCalibrating()) { @@ -305,7 +303,7 @@ static void updateArmingStatus(void) #ifdef USE_DSHOT /* CHECK: Don't arm if the DShot beeper was used recently, as there is a minimum delay before sending the next DShot command */ - if (currentTimeUs - getLastDshotBeeperCommandTimeUs() < getDShotBeaconGuardDelayUs()) { + if (micros() - getLastDshotBeeperCommandTimeUs() < getDShotBeaconGuardDelayUs()) { ENABLE_ARMING_FLAG(ARMING_DISABLED_DSHOT_BEEPER); } else { DISABLE_ARMING_FLAG(ARMING_DISABLED_DSHOT_BEEPER); From 94b184033cc1959be864333e9cff85e6ed795413 Mon Sep 17 00:00:00 2001 From: Harry Phillips Date: Sat, 10 Apr 2021 23:22:24 +0100 Subject: [PATCH 153/286] Handle edge case --- src/main/fc/fc_core.c | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/main/fc/fc_core.c b/src/main/fc/fc_core.c index 28cec05d75..ad3ba5988f 100755 --- a/src/main/fc/fc_core.c +++ b/src/main/fc/fc_core.c @@ -308,6 +308,8 @@ static void updateArmingStatus(void) } else { DISABLE_ARMING_FLAG(ARMING_DISABLED_DSHOT_BEEPER); } +#else + DISABLE_ARMING_FLAG(ARMING_DISABLED_DSHOT_BEEPER); #endif if (isModeActivationConditionPresent(BOXPREARM)) { From bb3ae698b8291d71e7dfcd290789aaf88158f4c2 Mon Sep 17 00:00:00 2001 From: Alexander van Saase Date: Sun, 11 Apr 2021 14:03:01 +0200 Subject: [PATCH 154/286] Ignore mixer rules disabled by LC --- src/main/flight/servos.c | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) diff --git a/src/main/flight/servos.c b/src/main/flight/servos.c index 4efd2db680..1a71aea5cd 100755 --- a/src/main/flight/servos.c +++ b/src/main/flight/servos.c @@ -68,7 +68,7 @@ PG_RESET_TEMPLATE(servoConfig_t, servoConfig, .flaperon_throw_offset = SETTING_FLAPERON_THROW_OFFSET_DEFAULT, .tri_unarmed_servo = SETTING_TRI_UNARMED_SERVO_DEFAULT, .servo_autotrim_rotation_limit = SETTING_SERVO_AUTOTRIM_ROTATION_LIMIT_DEFAULT, - .servo_autotrim_iterm_threshold = SETTING_SERVO_AUTOTRIM_ITERM_THRESHOLD + .servo_autotrim_iterm_threshold = SETTING_SERVO_AUTOTRIM_ITERM_THRESHOLD_DEFAULT ); PG_REGISTER_ARRAY_WITH_RESET_FN(servoMixer_t, MAX_SERVO_RULES, customServoMixers, PG_SERVO_MIXER, 1); @@ -444,7 +444,6 @@ void processServoAutotrim(void) const uint8_t from = currentServoMixer[i].inputSource; if (from == axis) { servoParamsMutable(target)->middle = servoMiddleAccum[target] / servoMiddleAccumCount; - } } } @@ -556,6 +555,11 @@ void processContinuousServoAutotrim(const float dT) if (fabsf(axisIterm) > ItermThreshold) { const int8_t ItermUpdate = axisIterm > 0.0f ? ItermThreshold : -ItermThreshold; for (int i = 0; i < servoRuleCount; i++) { +#ifdef USE_PROGRAMMING_FRAMEWORK + if (!logicConditionGetValue(currentServoMixer[i].conditionId)) { + continue; + } +#endif const uint8_t target = currentServoMixer[i].targetChannel; const uint8_t from = currentServoMixer[i].inputSource; if (from == axis) { From 8cc66e5a03925acc8cc84da512bb04cb2f7bb348 Mon Sep 17 00:00:00 2001 From: "Pawel Spychalski (DzikuVx)" Date: Sun, 11 Apr 2021 17:08:16 +0200 Subject: [PATCH 155/286] Set calibration data --- src/main/drivers/accgyro/accgyro_bno055.c | 58 +++++++++---------- src/main/drivers/accgyro/accgyro_bno055.h | 3 +- .../drivers/accgyro/accgyro_bno055_serial.c | 44 ++++++++++++-- .../drivers/accgyro/accgyro_bno055_serial.h | 3 +- 4 files changed, 69 insertions(+), 39 deletions(-) diff --git a/src/main/drivers/accgyro/accgyro_bno055.c b/src/main/drivers/accgyro/accgyro_bno055.c index 1ed096d76a..90dc96c02e 100644 --- a/src/main/drivers/accgyro/accgyro_bno055.c +++ b/src/main/drivers/accgyro/accgyro_bno055.c @@ -58,6 +58,35 @@ static void bno055SetMode(uint8_t mode) delay(25); } +static void bno055SetCalibrationData(bno055CalibrationData_t data) +{ + uint8_t buf[12]; + + //Prepare gains + //We do not restore gyro offsets, they are quickly calibrated at startup + uint8_t bufferBit = 0; + for (uint8_t sensorIndex = 0; sensorIndex < 2; sensorIndex++) + { + for (uint8_t axisIndex = 0; axisIndex < 3; axisIndex++) + { + buf[bufferBit] = (uint8_t)(data.offset[sensorIndex][axisIndex] & 0xff); + buf[bufferBit + 1] = (uint8_t)((data.offset[sensorIndex][axisIndex] >> 8 ) & 0xff); + bufferBit += 2; + } + } + + busWriteBuf(busDev, BNO055_ADDR_ACC_OFFSET_X_LSB, buf, 12); + + //Prepare radius + buf[0] = (uint8_t)(data.radius[ACC] & 0xff); + buf[1] = (uint8_t)((data.radius[ACC] >> 8 ) & 0xff); + buf[2] = (uint8_t)(data.radius[MAG] & 0xff); + buf[3] = (uint8_t)((data.radius[MAG] >> 8 ) & 0xff); + + //Write to the device + busWriteBuf(busDev, BNO055_ADDR_ACC_RADIUS_LSB, buf, 4); +} + bool bno055Init(bno055CalibrationData_t calibrationData, bool setCalibration) { busDev = busDeviceInit(BUSTYPE_I2C, DEVHW_BNO055, 0, 0); @@ -149,33 +178,4 @@ bno055CalibrationData_t bno055GetCalibrationData(void) return data; } -void bno055SetCalibrationData(bno055CalibrationData_t data) -{ - uint8_t buf[12]; - - //Prepare gains - //We do not restore gyro offsets, they are quickly calibrated at startup - uint8_t bufferBit = 0; - for (uint8_t sensorIndex = 0; sensorIndex < 2; sensorIndex++) - { - for (uint8_t axisIndex = 0; axisIndex < 3; axisIndex++) - { - buf[bufferBit] = (uint8_t)(data.offset[sensorIndex][axisIndex] & 0xff); - buf[bufferBit + 1] = (uint8_t)((data.offset[sensorIndex][axisIndex] >> 8 ) & 0xff); - bufferBit += 2; - } - } - - busWriteBuf(busDev, BNO055_ADDR_ACC_OFFSET_X_LSB, buf, 12); - - //Prepare radius - buf[0] = (uint8_t)(data.radius[ACC] & 0xff); - buf[1] = (uint8_t)((data.radius[ACC] >> 8 ) & 0xff); - buf[2] = (uint8_t)(data.radius[MAG] & 0xff); - buf[3] = (uint8_t)((data.radius[MAG] >> 8 ) & 0xff); - - //Write to the device - busWriteBuf(busDev, BNO055_ADDR_ACC_RADIUS_LSB, buf, 4); -} - #endif \ No newline at end of file diff --git a/src/main/drivers/accgyro/accgyro_bno055.h b/src/main/drivers/accgyro/accgyro_bno055.h index 6e6abdbff9..de0d751d7a 100644 --- a/src/main/drivers/accgyro/accgyro_bno055.h +++ b/src/main/drivers/accgyro/accgyro_bno055.h @@ -88,5 +88,4 @@ bool bno055Init(bno055CalibrationData_t calibrationData, bool setCalibration); fpVector3_t bno055GetEurlerAngles(void); void bno055FetchEulerAngles(int16_t * buffer); bno055CalibStat_t bno055GetCalibStat(void); -bno055CalibrationData_t bno055GetCalibrationData(void); -void bno055SetCalibrationData(bno055CalibrationData_t data); \ No newline at end of file +bno055CalibrationData_t bno055GetCalibrationData(void); \ No newline at end of file diff --git a/src/main/drivers/accgyro/accgyro_bno055_serial.c b/src/main/drivers/accgyro/accgyro_bno055_serial.c index 967c316d60..8fc5764515 100644 --- a/src/main/drivers/accgyro/accgyro_bno055_serial.c +++ b/src/main/drivers/accgyro/accgyro_bno055_serial.c @@ -143,6 +143,37 @@ void bno055SerialDataReceive(uint16_t c, void *data) { } +static void bno055SerialSetCalibrationData(bno055CalibrationData_t data) +{ + uint8_t buf[12]; + + //Prepare gains + //We do not restore gyro offsets, they are quickly calibrated at startup + uint8_t bufferBit = 0; + for (uint8_t sensorIndex = 0; sensorIndex < 2; sensorIndex++) + { + for (uint8_t axisIndex = 0; axisIndex < 3; axisIndex++) + { + buf[bufferBit] = (uint8_t)(data.offset[sensorIndex][axisIndex] & 0xff); + buf[bufferBit + 1] = (uint8_t)((data.offset[sensorIndex][axisIndex] >> 8 ) & 0xff); + bufferBit += 2; + } + } + + bno055SerialWriteBuffer(BNO055_ADDR_ACC_OFFSET_X_LSB, buf, 12); + delay(25); + + //Prepare radius + buf[0] = (uint8_t)(data.radius[ACC] & 0xff); + buf[1] = (uint8_t)((data.radius[ACC] >> 8 ) & 0xff); + buf[2] = (uint8_t)(data.radius[MAG] & 0xff); + buf[3] = (uint8_t)((data.radius[MAG] >> 8 ) & 0xff); + + //Write to the device + bno055SerialWriteBuffer(BNO055_ADDR_ACC_RADIUS_LSB, buf, 4); + delay(25); +} + bool bno055SerialInit(bno055CalibrationData_t calibrationData, bool setCalibration) { bno055SerialPort = NULL; @@ -176,9 +207,14 @@ bool bno055SerialInit(bno055CalibrationData_t calibrationData, bool setCalibrati bno055SerialWrite(BNO055_ADDR_PWR_MODE, BNO055_PWR_MODE_NORMAL); //Set power mode NORMAL delay(25); - //TODO Here set calibration data + if (setCalibration) { + bno055SerialWrite(BNO055_ADDR_OPR_MODE, BNO055_OPR_MODE_CONFIG); + delay(25); - bno055SerialWrite(BNO055_ADDR_OPR_MODE, BNO055_OPR_MODE_NDOF); //Set power mode NORMAL + bno055SerialSetCalibrationData(calibrationData); + } + + bno055SerialWrite(BNO055_ADDR_OPR_MODE, BNO055_OPR_MODE_NDOF); delay(25); return true; @@ -232,8 +268,4 @@ bno055CalibrationData_t bno055SerialGetCalibrationData(void) { return data; } -void bno055SerialSetCalibrationData(bno055CalibrationData_t data) { - -} - #endif \ No newline at end of file diff --git a/src/main/drivers/accgyro/accgyro_bno055_serial.h b/src/main/drivers/accgyro/accgyro_bno055_serial.h index 22686f0b26..1ffd55c646 100644 --- a/src/main/drivers/accgyro/accgyro_bno055_serial.h +++ b/src/main/drivers/accgyro/accgyro_bno055_serial.h @@ -30,5 +30,4 @@ bool bno055SerialInit(bno055CalibrationData_t calibrationData, bool setCalibration); void bno055SerialFetchEulerAngles(void); void bno055SerialGetCalibStat(void); -bno055CalibrationData_t bno055SerialGetCalibrationData(void); -void bno055SerialSetCalibrationData(bno055CalibrationData_t data); \ No newline at end of file +bno055CalibrationData_t bno055SerialGetCalibrationData(void); \ No newline at end of file From 611e5a22eaf4bc148fef4bc990129885bfa2300a Mon Sep 17 00:00:00 2001 From: "Pawel Spychalski (DzikuVx)" Date: Sun, 11 Apr 2021 17:10:38 +0200 Subject: [PATCH 156/286] Rename serial port function --- src/main/drivers/accgyro/accgyro_bno055_serial.c | 4 ++-- src/main/io/serial.h | 2 +- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/src/main/drivers/accgyro/accgyro_bno055_serial.c b/src/main/drivers/accgyro/accgyro_bno055_serial.c index 8fc5764515..9bf2a0725a 100644 --- a/src/main/drivers/accgyro/accgyro_bno055_serial.c +++ b/src/main/drivers/accgyro/accgyro_bno055_serial.c @@ -177,14 +177,14 @@ static void bno055SerialSetCalibrationData(bno055CalibrationData_t data) bool bno055SerialInit(bno055CalibrationData_t calibrationData, bool setCalibration) { bno055SerialPort = NULL; - serialPortConfig_t * portConfig = findSerialPortConfig(FUNCTION_BNO055); + serialPortConfig_t * portConfig = findSerialPortConfig(FUNCTION_IMU2); if (!portConfig) { return false; } bno055SerialPort = openSerialPort( portConfig->identifier, - FUNCTION_BNO055, + FUNCTION_IMU2, bno055SerialDataReceive, NULL, BNO055_BAUD_RATE, diff --git a/src/main/io/serial.h b/src/main/io/serial.h index c704c1c37b..3c76e160b3 100644 --- a/src/main/io/serial.h +++ b/src/main/io/serial.h @@ -55,7 +55,7 @@ typedef enum { FUNCTION_DJI_HD_OSD = (1 << 21), // 2097152 FUNCTION_SERVO_SERIAL = (1 << 22), // 4194304 FUNCTION_TELEMETRY_SMARTPORT_MASTER = (1 << 23), // 8388608 - FUNCTION_BNO055 = (1 << 24), // 16777216 + FUNCTION_IMU2 = (1 << 24), // 16777216 } serialPortFunction_e; typedef enum { From 4670433405c63362ea09c4c187f432170d35112d Mon Sep 17 00:00:00 2001 From: Alexander van Saase Date: Sun, 11 Apr 2021 22:59:02 +0200 Subject: [PATCH 157/286] revert existing autotrim mode to original logic --- src/main/flight/servos.c | 56 +++++++++++++--------------------------- 1 file changed, 18 insertions(+), 38 deletions(-) diff --git a/src/main/flight/servos.c b/src/main/flight/servos.c index 1a71aea5cd..c66489051d 100755 --- a/src/main/flight/servos.c +++ b/src/main/flight/servos.c @@ -406,15 +406,11 @@ void processServoAutotrim(void) case AUTOTRIM_IDLE: if (ARMING_FLAG(ARMED)) { // We are activating servo trim - backup current middles and prepare to average the data - for (int axis = FD_ROLL; axis <= FD_YAW; axis++) { - for (int i = 0; i < servoRuleCount; i++) { - const uint8_t target = currentServoMixer[i].targetChannel; - const uint8_t from = currentServoMixer[i].inputSource; - if (from == axis) { - servoMiddleBackup[target] = servoParams(target)->middle; - } - } + for (int servoIndex = SERVO_ELEVATOR; servoIndex <= MIN(SERVO_RUDDER, MAX_SUPPORTED_SERVOS); servoIndex++) { + servoMiddleBackup[servoIndex] = servoParams(servoIndex)->middle; + servoMiddleAccum[servoIndex] = 0; } + trimStartedAt = millis(); servoMiddleAccumCount = 0; trimState = AUTOTRIM_COLLECTING; @@ -427,25 +423,14 @@ void processServoAutotrim(void) case AUTOTRIM_COLLECTING: if (ARMING_FLAG(ARMED)) { servoMiddleAccumCount++; - for (int axis = FD_ROLL; axis <= FD_YAW; axis++) { - for (int i = 0; i < servoRuleCount; i++) { - const uint8_t target = currentServoMixer[i].targetChannel; - const uint8_t from = currentServoMixer[i].inputSource; - if (from == axis) { - servoMiddleAccum[target] += servo[target]; - } - } + + for (int servoIndex = SERVO_ELEVATOR; servoIndex <= MIN(SERVO_RUDDER, MAX_SUPPORTED_SERVOS); servoIndex++) { + servoMiddleAccum[servoIndex] += servo[servoIndex]; } if ((millis() - trimStartedAt) > SERVO_AUTOTRIM_TIMER_MS) { - for (int axis = FD_ROLL; axis <= FD_YAW; axis++) { - for (int i = 0; i < servoRuleCount; i++) { - const uint8_t target = currentServoMixer[i].targetChannel; - const uint8_t from = currentServoMixer[i].inputSource; - if (from == axis) { - servoParamsMutable(target)->middle = servoMiddleAccum[target] / servoMiddleAccumCount; - } - } + for (int servoIndex = SERVO_ELEVATOR; servoIndex <= MIN(SERVO_RUDDER, MAX_SUPPORTED_SERVOS); servoIndex++) { + servoParamsMutable(servoIndex)->middle = servoMiddleAccum[servoIndex] / servoMiddleAccumCount; } trimState = AUTOTRIM_SAVE_PENDING; pidResetErrorAccumulators(); //Reset Iterm since new midpoints override previously acumulated errors @@ -471,16 +456,11 @@ void processServoAutotrim(void) else { // We are deactivating servo trim - restore servo midpoints if (trimState == AUTOTRIM_SAVE_PENDING) { - for (int axis = FD_ROLL; axis <= FD_YAW; axis++) { - for (int i = 0; i < servoRuleCount; i++) { - const uint8_t target = currentServoMixer[i].targetChannel; - const uint8_t from = currentServoMixer[i].inputSource; - if (from == axis) { - servoParamsMutable(target)->middle = servoMiddleBackup[target]; - } - } + for (int servoIndex = SERVO_ELEVATOR; servoIndex <= MIN(SERVO_RUDDER, MAX_SUPPORTED_SERVOS); servoIndex++) { + servoParamsMutable(servoIndex)->middle = servoMiddleBackup[servoIndex]; } } + trimState = AUTOTRIM_IDLE; } } @@ -526,8 +506,8 @@ void processContinuousServoAutotrim(const float dT) for (int axis = FD_ROLL; axis <= FD_YAW; axis++) { for (int i = 0; i < servoRuleCount; i++) { const uint8_t target = currentServoMixer[i].targetChannel; - const uint8_t from = currentServoMixer[i].inputSource; - if (from == axis) { + const uint8_t source = currentServoMixer[i].inputSource; + if (source == axis) { servoMiddleBackup[target] = servoParams(target)->middle; } } @@ -561,8 +541,8 @@ void processContinuousServoAutotrim(const float dT) } #endif const uint8_t target = currentServoMixer[i].targetChannel; - const uint8_t from = currentServoMixer[i].inputSource; - if (from == axis) { + const uint8_t source = currentServoMixer[i].inputSource; + if (source == axis) { // Convert axis I-term to servo PWM and add to midpoint const float mixerRate = currentServoMixer[i].rate / 100.0f; const float servoRate = servoParams(target)->rate / 100.0f; @@ -596,8 +576,8 @@ void processContinuousServoAutotrim(const float dT) for (int axis = FD_ROLL; axis <= FD_YAW; axis++) { for (int i = 0; i < servoRuleCount; i++) { const uint8_t target = currentServoMixer[i].targetChannel; - const uint8_t from = currentServoMixer[i].inputSource; - if (from == axis) { + const uint8_t source = currentServoMixer[i].inputSource; + if (source == axis) { servoParamsMutable(target)->middle = servoMiddleBackup[target]; } } From 668124ed80d071157e8e3bffdd7b61c15f92bb7f Mon Sep 17 00:00:00 2001 From: Alexander van Saase Date: Mon, 12 Apr 2021 19:23:23 +0200 Subject: [PATCH 158/286] do not scale FF update with stickinput --- src/main/flight/pid_autotune.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/flight/pid_autotune.c b/src/main/flight/pid_autotune.c index e2b54c5dc3..1b2f42881d 100755 --- a/src/main/flight/pid_autotune.c +++ b/src/main/flight/pid_autotune.c @@ -264,7 +264,7 @@ void autotuneFixedWingUpdate(const flight_dynamics_index_t axis, float desiredRa // Update FF towards value needed to achieve current rate target pidOutputRequired = MIN(tuneCurrent[axis].maxAbsPidOutput * (tuneCurrent[axis].maxAbsDesiredRateDps / tuneCurrent[axis].maxAbsReachedRateDps), pidSumLimit); - gainFF += (pidOutputRequired / tuneCurrent[axis].maxAbsDesiredRateDps * FP_PID_RATE_FF_MULTIPLIER - gainFF) * convergenceRate * stickInput; + gainFF += (pidOutputRequired / tuneCurrent[axis].maxAbsDesiredRateDps * FP_PID_RATE_FF_MULTIPLIER - gainFF) * convergenceRate; tuneCurrent[axis].gainFF = constrainf(gainFF, AUTOTUNE_FIXED_WING_MIN_FF, AUTOTUNE_FIXED_WING_MAX_FF); gainsUpdated = true; From c08c00090582f4a3288af13e5752f14ad403f5d7 Mon Sep 17 00:00:00 2001 From: Darren Lines Date: Mon, 12 Apr 2021 18:45:31 +0100 Subject: [PATCH 159/286] Renamed OSD SW and LEDLOW to OSD/LEDS OFF --- src/main/fc/fc_msp_box.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/fc/fc_msp_box.c b/src/main/fc/fc_msp_box.c index f517be11f1..65bf1de1cc 100644 --- a/src/main/fc/fc_msp_box.c +++ b/src/main/fc/fc_msp_box.c @@ -58,9 +58,9 @@ static const box_t boxes[CHECKBOX_ITEM_COUNT + 1] = { { BOXNAVPOSHOLD, "NAV POSHOLD", 11 }, // old GPS HOLD { BOXMANUAL, "MANUAL", 12 }, { BOXBEEPERON, "BEEPER", 13 }, - { BOXLEDLOW, "LEDLOW", 15 }, + { BOXLEDLOW, "LEDS OFF", 15 }, { BOXLIGHTS, "LIGHTS", 16 }, - { BOXOSD, "OSD SW", 19 }, + { BOXOSD, "OSD OFF", 19 }, { BOXTELEMETRY, "TELEMETRY", 20 }, { BOXAUTOTUNE, "AUTO TUNE", 21 }, { BOXBLACKBOX, "BLACKBOX", 26 }, From 266317cce84a6b8bcc05c412631db301d6e5e5eb Mon Sep 17 00:00:00 2001 From: Alexander van Saase Date: Mon, 12 Apr 2021 21:30:30 +0200 Subject: [PATCH 160/286] disable autotune in angle mode --- src/main/flight/pid.c | 10 +++++----- src/main/flight/pid_autotune.c | 10 +--------- 2 files changed, 6 insertions(+), 14 deletions(-) diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index ff3522c700..db136670df 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -406,7 +406,7 @@ static float calculateFixedWingTPAFactor(uint16_t throttle) // tpa_rate is amount of curve TPA applied to PIDs // tpa_breakpoint for fixed wing is cruise throttle value (value at which PIDs were tuned) - if (currentControlRateProfile->throttle.dynPID != 0 && currentControlRateProfile->throttle.pa_breakpoint > getThrottleIdleValue() && !FLIGHT_MODE(AUTO_TUNE) && ARMING_FLAG(ARMED)) { + if (currentControlRateProfile->throttle.dynPID != 0 && currentControlRateProfile->throttle.pa_breakpoint > getThrottleIdleValue()) { if (throttle > getThrottleIdleValue()) { // Calculate TPA according to throttle tpaFactor = 0.5f + ((float)(currentControlRateProfile->throttle.pa_breakpoint - getThrottleIdleValue()) / (throttle - getThrottleIdleValue()) / 2.0f); @@ -717,14 +717,14 @@ static void NOINLINE pidApplyFixedWingRateController(pidState_t *pidState, fligh pidState->errorGyroIf = constrainf(pidState->errorGyroIf, -pidProfile()->fixedWingItermThrowLimit, pidProfile()->fixedWingItermThrowLimit); } - axisPID[axis] = constrainf(newPTerm + newFFTerm + pidState->errorGyroIf, -pidState->pidSumLimit, +pidState->pidSumLimit); - #ifdef USE_AUTOTUNE_FIXED_WING - if (FLIGHT_MODE(AUTO_TUNE) && !FLIGHT_MODE(MANUAL_MODE)) { - autotuneFixedWingUpdate(axis, pidState->rateTarget, pidState->gyroRate, constrainf(newFFTerm, -pidState->pidSumLimit, +pidState->pidSumLimit)); + if (FLIGHT_MODE(AUTO_TUNE) && !(FLIGHT_MODE(MANUAL_MODE) || FLIGHT_MODE(ANGLE_MODE))) { + autotuneFixedWingUpdate(axis, pidState->rateTarget, pidState->gyroRate, newPTerm + newFFTerm); } #endif + axisPID[axis] = constrainf(newPTerm + newFFTerm + pidState->errorGyroIf + newDTerm, -pidState->pidSumLimit, +pidState->pidSumLimit); + #ifdef USE_BLACKBOX axisPID_P[axis] = newPTerm; axisPID_I[axis] = pidState->errorGyroIf; diff --git a/src/main/flight/pid_autotune.c b/src/main/flight/pid_autotune.c index 1b2f42881d..d90e70e7bc 100755 --- a/src/main/flight/pid_autotune.c +++ b/src/main/flight/pid_autotune.c @@ -196,13 +196,6 @@ void autotuneFixedWingUpdate(const flight_dynamics_index_t axis, float desiredRa float pidOutputRequired; pidAutotuneState_e newState; - // Use different max desired rate in ANGLE for pitch and roll - // Maximum reasonable error in ANGLE mode is 200% of angle inclination (control dublet), but we are conservative and tune for control singlet. - if (FLIGHT_MODE(ANGLE_MODE) && (axis == FD_PITCH || axis == FD_ROLL)) { - float maxDesiredRateInAngleMode = DECIDEGREES_TO_DEGREES(pidProfile()->max_angle_inclination[axis] * 1.0f) * pidBank()->pid[PID_LEVEL].P / FP_PID_LEVEL_P_MULTIPLIER; - maxDesiredRateDps = MIN(maxDesiredRateDps, maxDesiredRateInAngleMode); - } - if (stickInput < (pidAutotuneConfig()->fw_min_stick / 100.0f) || !correctDirection) { // We can make decisions only when we are giving at least 80% stick input and the airplane is rotating in the requested direction newState = DEMAND_TOO_LOW; @@ -238,8 +231,7 @@ void autotuneFixedWingUpdate(const flight_dynamics_index_t axis, float desiredRa tuneCurrent[axis].maxAbsPidOutput = MAX(tuneCurrent[axis].maxAbsPidOutput, absPidOutput); if (stateTimeMs >= pidAutotuneConfig()->fw_detect_time) { - if (pidAutotuneConfig()->fw_rate_adjustment != FIXED && !FLIGHT_MODE(ANGLE_MODE)) { - // Tuning the rates is not compatible with ANGLE mode + if (pidAutotuneConfig()->fw_rate_adjustment != FIXED) { // Target 80% control surface deflection to leave some room for P and I to work pidSumLimit = (axis == FD_YAW) ? pidProfile()->pidSumLimitYaw : pidProfile()->pidSumLimit; From d19436096f6520d352946540e838221d4a4aac06 Mon Sep 17 00:00:00 2001 From: Alexander van Saase Date: Mon, 12 Apr 2021 22:00:15 +0200 Subject: [PATCH 161/286] pidinput and disable TPA --- src/main/flight/pid.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index db136670df..8af99fc136 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -406,7 +406,7 @@ static float calculateFixedWingTPAFactor(uint16_t throttle) // tpa_rate is amount of curve TPA applied to PIDs // tpa_breakpoint for fixed wing is cruise throttle value (value at which PIDs were tuned) - if (currentControlRateProfile->throttle.dynPID != 0 && currentControlRateProfile->throttle.pa_breakpoint > getThrottleIdleValue()) { + if (currentControlRateProfile->throttle.dynPID != 0 && currentControlRateProfile->throttle.pa_breakpoint > getThrottleIdleValue() && !FLIGHT_MODE(AUTO_TUNE) && ARMING_FLAG(ARMED)) { if (throttle > getThrottleIdleValue()) { // Calculate TPA according to throttle tpaFactor = 0.5f + ((float)(currentControlRateProfile->throttle.pa_breakpoint - getThrottleIdleValue()) / (throttle - getThrottleIdleValue()) / 2.0f); @@ -719,7 +719,7 @@ static void NOINLINE pidApplyFixedWingRateController(pidState_t *pidState, fligh #ifdef USE_AUTOTUNE_FIXED_WING if (FLIGHT_MODE(AUTO_TUNE) && !(FLIGHT_MODE(MANUAL_MODE) || FLIGHT_MODE(ANGLE_MODE))) { - autotuneFixedWingUpdate(axis, pidState->rateTarget, pidState->gyroRate, newPTerm + newFFTerm); + autotuneFixedWingUpdate(axis, pidState->rateTarget, pidState->gyroRate, constrainf(newFFTerm, -pidState->pidSumLimit, +pidState->pidSumLimit)); } #endif From 2a01175e0eff56e02e9b37ed43e0aab8090bba0c Mon Sep 17 00:00:00 2001 From: breadoven <56191411+breadoven@users.noreply.github.com> Date: Tue, 13 Apr 2021 00:15:09 +0100 Subject: [PATCH 162/286] Update navigation.c --- src/main/navigation/navigation.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c index 2e7588cc7c..ac5161098a 100644 --- a/src/main/navigation/navigation.c +++ b/src/main/navigation/navigation.c @@ -2470,7 +2470,7 @@ static bool rthAltControlStickOverrideCheck(unsigned axis) if (!rthOverrideStickHoldStartTime[axis]) { rthOverrideStickHoldStartTime[axis] = millis(); - } else if (ABS(2500 - holdTime) < 500) { + } else if (ABS(2500 - holdTime) < 500) { // 2s delay to activate, activation duration limited to 1 sec if (axis == PITCH) { // PITCH down to override preset altitude reset to current altitude posControl.rthState.rthInitialAltitude = posControl.actualState.abs.pos.z; posControl.rthState.rthFinalAltitude = posControl.rthState.rthInitialAltitude; From 32ef596278f9561dffd9ebecb8c346cedbede2c5 Mon Sep 17 00:00:00 2001 From: Alexander van Saase Date: Tue, 13 Apr 2021 09:59:03 +0200 Subject: [PATCH 163/286] re-enable FF tuning in angle mode --- src/main/flight/pid.c | 6 +++--- src/main/flight/pid_autotune.c | 9 ++++++++- 2 files changed, 11 insertions(+), 4 deletions(-) diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index 8af99fc136..ff3522c700 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -717,14 +717,14 @@ static void NOINLINE pidApplyFixedWingRateController(pidState_t *pidState, fligh pidState->errorGyroIf = constrainf(pidState->errorGyroIf, -pidProfile()->fixedWingItermThrowLimit, pidProfile()->fixedWingItermThrowLimit); } + axisPID[axis] = constrainf(newPTerm + newFFTerm + pidState->errorGyroIf, -pidState->pidSumLimit, +pidState->pidSumLimit); + #ifdef USE_AUTOTUNE_FIXED_WING - if (FLIGHT_MODE(AUTO_TUNE) && !(FLIGHT_MODE(MANUAL_MODE) || FLIGHT_MODE(ANGLE_MODE))) { + if (FLIGHT_MODE(AUTO_TUNE) && !FLIGHT_MODE(MANUAL_MODE)) { autotuneFixedWingUpdate(axis, pidState->rateTarget, pidState->gyroRate, constrainf(newFFTerm, -pidState->pidSumLimit, +pidState->pidSumLimit)); } #endif - axisPID[axis] = constrainf(newPTerm + newFFTerm + pidState->errorGyroIf + newDTerm, -pidState->pidSumLimit, +pidState->pidSumLimit); - #ifdef USE_BLACKBOX axisPID_P[axis] = newPTerm; axisPID_I[axis] = pidState->errorGyroIf; diff --git a/src/main/flight/pid_autotune.c b/src/main/flight/pid_autotune.c index d90e70e7bc..5a0e050ef6 100755 --- a/src/main/flight/pid_autotune.c +++ b/src/main/flight/pid_autotune.c @@ -183,6 +183,12 @@ void autotuneFixedWingUpdate(const flight_dynamics_index_t axis, float desiredRa float maxDesiredRateDps = tuneCurrent[axis].rate; float gainFF = tuneCurrent[axis].gainFF; + // Use different max desired rate in ANGLE for pitch and roll + if (FLIGHT_MODE(ANGLE_MODE) && (axis == FD_PITCH || axis == FD_ROLL)) { + float maxDesiredRateInAngleMode = DECIDEGREES_TO_DEGREES(pidProfile()->max_angle_inclination[axis] * 1.0f) * pidBank()->pid[PID_LEVEL].P / FP_PID_LEVEL_P_MULTIPLIER; + maxDesiredRateDps = MIN(maxDesiredRateDps, maxDesiredRateInAngleMode); + } + const float absDesiredRateDps = fabsf(desiredRateDps); const float absReachedRateDps = fabsf(reachedRateDps); const float absPidOutput = fabsf(pidOutput); @@ -231,7 +237,8 @@ void autotuneFixedWingUpdate(const flight_dynamics_index_t axis, float desiredRa tuneCurrent[axis].maxAbsPidOutput = MAX(tuneCurrent[axis].maxAbsPidOutput, absPidOutput); if (stateTimeMs >= pidAutotuneConfig()->fw_detect_time) { - if (pidAutotuneConfig()->fw_rate_adjustment != FIXED) { + if (pidAutotuneConfig()->fw_rate_adjustment != FIXED && !FLIGHT_MODE(ANGLE_MODE)) { + // Tuning the rates is not compatible with ANGLE mode // Target 80% control surface deflection to leave some room for P and I to work pidSumLimit = (axis == FD_YAW) ? pidProfile()->pidSumLimitYaw : pidProfile()->pidSumLimit; From 4424c6606f34b9b19e571e80e478fce7c41c968f Mon Sep 17 00:00:00 2001 From: Alexander van Saase Date: Tue, 13 Apr 2021 10:11:15 +0200 Subject: [PATCH 164/286] some cleanup --- src/main/flight/pid_autotune.c | 14 ++++---------- 1 file changed, 4 insertions(+), 10 deletions(-) diff --git a/src/main/flight/pid_autotune.c b/src/main/flight/pid_autotune.c index 5a0e050ef6..314b9b9663 100755 --- a/src/main/flight/pid_autotune.c +++ b/src/main/flight/pid_autotune.c @@ -192,14 +192,8 @@ void autotuneFixedWingUpdate(const flight_dynamics_index_t axis, float desiredRa const float absDesiredRateDps = fabsf(desiredRateDps); const float absReachedRateDps = fabsf(reachedRateDps); const float absPidOutput = fabsf(pidOutput); - const bool correctDirection = (desiredRateDps>0) == (reachedRateDps>0); const float stickInput = absDesiredRateDps / maxDesiredRateDps; - - float pidSumTarget; - float pidSumLimit; - float rateFullStick; - float pidOutputRequired; pidAutotuneState_e newState; if (stickInput < (pidAutotuneConfig()->fw_min_stick / 100.0f) || !correctDirection) { @@ -241,11 +235,11 @@ void autotuneFixedWingUpdate(const flight_dynamics_index_t axis, float desiredRa // Tuning the rates is not compatible with ANGLE mode // Target 80% control surface deflection to leave some room for P and I to work - pidSumLimit = (axis == FD_YAW) ? pidProfile()->pidSumLimitYaw : pidProfile()->pidSumLimit; - pidSumTarget = (pidAutotuneConfig()->fw_max_rate_deflection / 100.0f) * pidSumLimit; + float pidSumLimit = (axis == FD_YAW) ? pidProfile()->pidSumLimitYaw : pidProfile()->pidSumLimit; + float pidSumTarget = (pidAutotuneConfig()->fw_max_rate_deflection / 100.0f) * pidSumLimit; // Theoretically achievable rate with target deflection - rateFullStick = (pidSumTarget / tuneCurrent[axis].maxAbsPidOutput) * tuneCurrent[axis].maxAbsReachedRateDps; + float rateFullStick = (pidSumTarget / tuneCurrent[axis].maxAbsPidOutput) * tuneCurrent[axis].maxAbsReachedRateDps; // Rate update if (rateFullStick > (maxDesiredRateDps + 10.0f)) { @@ -262,7 +256,7 @@ void autotuneFixedWingUpdate(const flight_dynamics_index_t axis, float desiredRa } // Update FF towards value needed to achieve current rate target - pidOutputRequired = MIN(tuneCurrent[axis].maxAbsPidOutput * (tuneCurrent[axis].maxAbsDesiredRateDps / tuneCurrent[axis].maxAbsReachedRateDps), pidSumLimit); + float pidOutputRequired = MIN(tuneCurrent[axis].maxAbsPidOutput * (tuneCurrent[axis].maxAbsDesiredRateDps / tuneCurrent[axis].maxAbsReachedRateDps), pidSumLimit); gainFF += (pidOutputRequired / tuneCurrent[axis].maxAbsDesiredRateDps * FP_PID_RATE_FF_MULTIPLIER - gainFF) * convergenceRate; tuneCurrent[axis].gainFF = constrainf(gainFF, AUTOTUNE_FIXED_WING_MIN_FF, AUTOTUNE_FIXED_WING_MAX_FF); gainsUpdated = true; From 8f13205e7f5c7e2368da0f63aab2a9abc1920e74 Mon Sep 17 00:00:00 2001 From: Alexander van Saase Date: Tue, 13 Apr 2021 10:24:02 +0200 Subject: [PATCH 165/286] build errors --- src/main/flight/pid_autotune.c | 9 ++++++--- 1 file changed, 6 insertions(+), 3 deletions(-) diff --git a/src/main/flight/pid_autotune.c b/src/main/flight/pid_autotune.c index 314b9b9663..0a83caec25 100755 --- a/src/main/flight/pid_autotune.c +++ b/src/main/flight/pid_autotune.c @@ -182,6 +182,7 @@ void autotuneFixedWingUpdate(const flight_dynamics_index_t axis, float desiredRa const float convergenceRate = pidAutotuneConfig()->fw_convergence_rate / 100.0f; float maxDesiredRateDps = tuneCurrent[axis].rate; float gainFF = tuneCurrent[axis].gainFF; + float pidSumLimit = (axis == FD_YAW) ? pidProfile()->pidSumLimitYaw : pidProfile()->pidSumLimit; // Use different max desired rate in ANGLE for pitch and roll if (FLIGHT_MODE(ANGLE_MODE) && (axis == FD_PITCH || axis == FD_ROLL)) { @@ -196,6 +197,9 @@ void autotuneFixedWingUpdate(const flight_dynamics_index_t axis, float desiredRa const float stickInput = absDesiredRateDps / maxDesiredRateDps; pidAutotuneState_e newState; + float rateFullStick; + float pidOutputRequired; + if (stickInput < (pidAutotuneConfig()->fw_min_stick / 100.0f) || !correctDirection) { // We can make decisions only when we are giving at least 80% stick input and the airplane is rotating in the requested direction newState = DEMAND_TOO_LOW; @@ -235,11 +239,10 @@ void autotuneFixedWingUpdate(const flight_dynamics_index_t axis, float desiredRa // Tuning the rates is not compatible with ANGLE mode // Target 80% control surface deflection to leave some room for P and I to work - float pidSumLimit = (axis == FD_YAW) ? pidProfile()->pidSumLimitYaw : pidProfile()->pidSumLimit; float pidSumTarget = (pidAutotuneConfig()->fw_max_rate_deflection / 100.0f) * pidSumLimit; // Theoretically achievable rate with target deflection - float rateFullStick = (pidSumTarget / tuneCurrent[axis].maxAbsPidOutput) * tuneCurrent[axis].maxAbsReachedRateDps; + rateFullStick = (pidSumTarget / tuneCurrent[axis].maxAbsPidOutput) * tuneCurrent[axis].maxAbsReachedRateDps; // Rate update if (rateFullStick > (maxDesiredRateDps + 10.0f)) { @@ -256,7 +259,7 @@ void autotuneFixedWingUpdate(const flight_dynamics_index_t axis, float desiredRa } // Update FF towards value needed to achieve current rate target - float pidOutputRequired = MIN(tuneCurrent[axis].maxAbsPidOutput * (tuneCurrent[axis].maxAbsDesiredRateDps / tuneCurrent[axis].maxAbsReachedRateDps), pidSumLimit); + pidOutputRequired = MIN(tuneCurrent[axis].maxAbsPidOutput * (tuneCurrent[axis].maxAbsDesiredRateDps / tuneCurrent[axis].maxAbsReachedRateDps), pidSumLimit); gainFF += (pidOutputRequired / tuneCurrent[axis].maxAbsDesiredRateDps * FP_PID_RATE_FF_MULTIPLIER - gainFF) * convergenceRate; tuneCurrent[axis].gainFF = constrainf(gainFF, AUTOTUNE_FIXED_WING_MIN_FF, AUTOTUNE_FIXED_WING_MAX_FF); gainsUpdated = true; From 2a3532215d0c98eb8dad1327edb82028c736fa83 Mon Sep 17 00:00:00 2001 From: Alexander van Saase Date: Tue, 13 Apr 2021 12:09:20 +0200 Subject: [PATCH 166/286] autotrim only stabilized servos --- src/main/flight/servos.c | 51 ++++++++++++++++++++++++++++------------ 1 file changed, 36 insertions(+), 15 deletions(-) diff --git a/src/main/flight/servos.c b/src/main/flight/servos.c index c66489051d..eed1432649 100755 --- a/src/main/flight/servos.c +++ b/src/main/flight/servos.c @@ -399,20 +399,24 @@ void processServoAutotrim(void) static int16_t servoMiddleBackup[MAX_SUPPORTED_SERVOS]; static int32_t servoMiddleAccum[MAX_SUPPORTED_SERVOS]; - static int32_t servoMiddleAccumCount; + static int32_t servoMiddleAccumCount[MAX_SUPPORTED_SERVOS]; if (IS_RC_MODE_ACTIVE(BOXAUTOTRIM)) { switch (trimState) { case AUTOTRIM_IDLE: if (ARMING_FLAG(ARMED)) { - // We are activating servo trim - backup current middles and prepare to average the data - for (int servoIndex = SERVO_ELEVATOR; servoIndex <= MIN(SERVO_RUDDER, MAX_SUPPORTED_SERVOS); servoIndex++) { - servoMiddleBackup[servoIndex] = servoParams(servoIndex)->middle; - servoMiddleAccum[servoIndex] = 0; + for (int axis = FD_ROLL; axis <= FD_YAW; axis++) { + for (int i = 0; i < servoRuleCount; i++) { + const uint8_t target = currentServoMixer[i].targetChannel; + const uint8_t source = currentServoMixer[i].inputSource; + if (source == axis) { + servoMiddleBackup[target] = servoParams(target)->middle; + servoMiddleAccum[target] = 0; + servoMiddleAccumCount[target] = 0; + } + } } - trimStartedAt = millis(); - servoMiddleAccumCount = 0; trimState = AUTOTRIM_COLLECTING; } else { @@ -422,15 +426,26 @@ void processServoAutotrim(void) case AUTOTRIM_COLLECTING: if (ARMING_FLAG(ARMED)) { - servoMiddleAccumCount++; - - for (int servoIndex = SERVO_ELEVATOR; servoIndex <= MIN(SERVO_RUDDER, MAX_SUPPORTED_SERVOS); servoIndex++) { - servoMiddleAccum[servoIndex] += servo[servoIndex]; + for (int axis = FD_ROLL; axis <= FD_YAW; axis++) { + for (int i = 0; i < servoRuleCount; i++) { + const uint8_t target = currentServoMixer[i].targetChannel; + const uint8_t source = currentServoMixer[i].inputSource; + if (source == axis) { + servoMiddleAccum[target] += servo[target]; + servoMiddleAccumCount[target]++; + } + } } if ((millis() - trimStartedAt) > SERVO_AUTOTRIM_TIMER_MS) { - for (int servoIndex = SERVO_ELEVATOR; servoIndex <= MIN(SERVO_RUDDER, MAX_SUPPORTED_SERVOS); servoIndex++) { - servoParamsMutable(servoIndex)->middle = servoMiddleAccum[servoIndex] / servoMiddleAccumCount; + for (int axis = FD_ROLL; axis <= FD_YAW; axis++) { + for (int i = 0; i < servoRuleCount; i++) { + const uint8_t target = currentServoMixer[i].targetChannel; + const uint8_t source = currentServoMixer[i].inputSource; + if (source == axis) { + servoParamsMutable(target)->middle = servoMiddleAccum[target] / servoMiddleAccumCount[target]; + } + } } trimState = AUTOTRIM_SAVE_PENDING; pidResetErrorAccumulators(); //Reset Iterm since new midpoints override previously acumulated errors @@ -456,8 +471,14 @@ void processServoAutotrim(void) else { // We are deactivating servo trim - restore servo midpoints if (trimState == AUTOTRIM_SAVE_PENDING) { - for (int servoIndex = SERVO_ELEVATOR; servoIndex <= MIN(SERVO_RUDDER, MAX_SUPPORTED_SERVOS); servoIndex++) { - servoParamsMutable(servoIndex)->middle = servoMiddleBackup[servoIndex]; + for (int axis = FD_ROLL; axis <= FD_YAW; axis++) { + for (int i = 0; i < servoRuleCount; i++) { + const uint8_t target = currentServoMixer[i].targetChannel; + const uint8_t source = currentServoMixer[i].inputSource; + if (source == axis) { + servoParamsMutable(target)->middle = servoMiddleBackup[target]; + } + } } } From 36fc9356f99271e7cf8fbebad7196ef4d91743ff Mon Sep 17 00:00:00 2001 From: Alexander van Saase Date: Tue, 13 Apr 2021 15:22:01 +0200 Subject: [PATCH 167/286] increase max rotation for trimming midpoints --- src/main/fc/settings.yaml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index f616f1ae04..39cce57212 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -1178,7 +1178,7 @@ groups: type: bool - name: servo_autotrim_rotation_limit description: "Servo midpoints are only updated when total aircraft rotation is less than this threshold [deg/s]. Only applies when using `feature FW_AUTOTRIM`." - default_value: 10 + default_value: 15 min: 1 max: 60 - name: servo_autotrim_iterm_threshold From 0e7d3812520aa3f11441bc52f77716c32ac7a13b Mon Sep 17 00:00:00 2001 From: "Pawel Spychalski (DzikuVx)" Date: Tue, 13 Apr 2021 19:15:43 +0200 Subject: [PATCH 168/286] Fix compilation --- src/main/io/osd.c | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/main/io/osd.c b/src/main/io/osd.c index e88db825e1..17ca15df68 100644 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -104,6 +104,7 @@ FILE_COMPILE_FOR_SPEED #include "sensors/pitotmeter.h" #include "sensors/temperature.h" #include "sensors/esc_sensor.h" +#include "sensors/rangefinder.h" #include "programming/logic_condition.h" #include "programming/global_variables.h" @@ -1591,7 +1592,7 @@ static bool osdDrawSingleElement(uint8_t item) { int32_t range = rangefinderGetLatestRawAltitude(); if (range < 0) { - buff[0] = "-"; + buff[0] = '-'; } else { osdFormatDistanceSymbol(buff, range); } From 291fe75e144650e82c82fac0cdcacd767344ff65 Mon Sep 17 00:00:00 2001 From: "Pawel Spychalski (DzikuVx)" Date: Tue, 13 Apr 2021 20:11:25 +0200 Subject: [PATCH 169/286] Scale rangefinder distance to one decimal point --- src/main/io/osd.c | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/src/main/io/osd.c b/src/main/io/osd.c index 17ca15df68..ac06190109 100644 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -313,11 +313,11 @@ static void osdLeftAlignString(char *buff) * prefixed by a a symbol to indicate the unit used. * @param dist Distance in centimeters */ -static void osdFormatDistanceSymbol(char *buff, int32_t dist) +static void osdFormatDistanceSymbol(char *buff, int32_t dist, uint8_t decimals) { switch ((osd_unit_e)osdConfig()->units) { case OSD_UNIT_IMPERIAL: - if (osdFormatCentiNumber(buff, CENTIMETERS_TO_CENTIFEET(dist), FEET_PER_MILE, 0, 3, 3)) { + if (osdFormatCentiNumber(buff, CENTIMETERS_TO_CENTIFEET(dist), FEET_PER_MILE, decimals, 3, 3)) { buff[3] = SYM_DIST_MI; } else { buff[3] = SYM_DIST_FT; @@ -327,7 +327,7 @@ static void osdFormatDistanceSymbol(char *buff, int32_t dist) case OSD_UNIT_UK: FALLTHROUGH; case OSD_UNIT_METRIC: - if (osdFormatCentiNumber(buff, dist, METERS_PER_KILOMETER, 0, 3, 3)) { + if (osdFormatCentiNumber(buff, dist, METERS_PER_KILOMETER, decimals, 3, 3)) { buff[3] = SYM_DIST_KM; } else { buff[3] = SYM_DIST_M; @@ -1459,7 +1459,7 @@ static bool osdDrawSingleElement(uint8_t item) case OSD_HOME_DIST: { buff[0] = SYM_HOME; - osdFormatDistanceSymbol(&buff[1], GPS_distanceToHome * 100); + osdFormatDistanceSymbol(&buff[1], GPS_distanceToHome * 100, 0); uint16_t dist_alarm = osdConfig()->dist_alarm; if (dist_alarm > 0 && GPS_distanceToHome > dist_alarm) { TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); @@ -1469,7 +1469,7 @@ static bool osdDrawSingleElement(uint8_t item) case OSD_TRIP_DIST: buff[0] = SYM_TOTAL; - osdFormatDistanceSymbol(buff + 1, getTotalTravelDistance()); + osdFormatDistanceSymbol(buff + 1, getTotalTravelDistance(), 0); break; case OSD_HEADING: @@ -1594,7 +1594,7 @@ static bool osdDrawSingleElement(uint8_t item) if (range < 0) { buff[0] = '-'; } else { - osdFormatDistanceSymbol(buff, range); + osdFormatDistanceSymbol(buff, range, 1); } } break; @@ -1671,7 +1671,7 @@ static bool osdDrawSingleElement(uint8_t item) buff[5] = '\0'; TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); } else { - osdFormatDistanceSymbol(buff + 1, distanceMeters * 100); + osdFormatDistanceSymbol(buff + 1, distanceMeters * 100, 0); if (distanceMeters == 0) TEXT_ATTRIBUTES_ADD_BLINK(elemAttr); } From e8dc880b5cb1588964eb30563ed7a4a15edfe4a9 Mon Sep 17 00:00:00 2001 From: breadoven <56191411+breadoven@users.noreply.github.com> Date: Tue, 13 Apr 2021 23:16:03 +0100 Subject: [PATCH 170/286] Update navigation.c Reduced stick hold delay time to 1s and limited climb first override to fixed wing only --- src/main/navigation/navigation.c | 11 +++++------ 1 file changed, 5 insertions(+), 6 deletions(-) diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c index ac5161098a..01f522cb54 100644 --- a/src/main/navigation/navigation.c +++ b/src/main/navigation/navigation.c @@ -2454,24 +2454,23 @@ void updateHomePosition(void) /* ----------------------------------------------------------- * Override RTH preset altitude and Climb First option - * using Pitch/Roll stick held for > 2 seconds + * using Pitch/Roll stick held for > 1 seconds + * Climb First override limited to Fixed Wing only *-----------------------------------------------------------*/ static bool rthAltControlStickOverrideCheck(unsigned axis) { - if (!navConfig()->general.flags.rth_alt_control_override || posControl.flags.forcedRTHActivated) { + if (!navConfig()->general.flags.rth_alt_control_override || posControl.flags.forcedRTHActivated || (axis == ROLL && STATE(MULTIROTOR))) { return false; } - static timeMs_t rthOverrideStickHoldStartTime[2]; if (rxGetChannelValue(axis) > rxConfig()->maxcheck) { - timeDelta_t holdTime = millis() - rthOverrideStickHoldStartTime[axis]; if (!rthOverrideStickHoldStartTime[axis]) { rthOverrideStickHoldStartTime[axis] = millis(); - } else if (ABS(2500 - holdTime) < 500) { // 2s delay to activate, activation duration limited to 1 sec - if (axis == PITCH) { // PITCH down to override preset altitude reset to current altitude + } else if (ABS(1500 - holdTime) < 500) { // 1s delay to activate, activation duration limited to 1 sec + if (axis == PITCH) { // PITCH down to override preset altitude reset to current altitude posControl.rthState.rthInitialAltitude = posControl.actualState.abs.pos.z; posControl.rthState.rthFinalAltitude = posControl.rthState.rthInitialAltitude; return true; From f55edbea27bd739ab4f3f591faa91ef4c856b1ef Mon Sep 17 00:00:00 2001 From: breadoven <56191411+breadoven@users.noreply.github.com> Date: Tue, 13 Apr 2021 23:57:23 +0100 Subject: [PATCH 171/286] Update Settings --- docs/Settings.md | 2 +- src/main/fc/settings.yaml | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/docs/Settings.md b/docs/Settings.md index 0b531d23e0..b24760adcc 100644 --- a/docs/Settings.md +++ b/docs/Settings.md @@ -356,7 +356,7 @@ | nav_position_timeout | 5 | If GPS fails wait for this much seconds before switching to emergency landing mode (0 - disable) | | nav_rth_abort_threshold | 50000 | RTH sanity checking feature will notice if distance to home is increasing during RTH and once amount of increase exceeds the threshold defined by this parameter, instead of continuing RTH machine will enter emergency landing, self-level and go down safely. Default is 500m which is safe enough for both multirotor machines and airplanes. [cm] | | nav_rth_allow_landing | ALWAYS | If set to ON drone will land as a last phase of RTH. | -| nav_rth_alt_control_override | OFF | If set to ON RTH altitude and CLIMB FIRST settings can be overridden during the RTH climb phase using full pitch or roll stick held for > 2 seconds. RTH altitude is reset to the current altitude using pitch down stick. RTH CLIMB FIRST is overridden using right roll stick so craft turns and heads directly to home. | +| nav_rth_alt_control_override | OFF | If set to ON RTH altitude and CLIMB FIRST settings can be overridden during the RTH climb phase using full pitch or roll stick held for > 1 second. RTH altitude is reset to the current altitude using pitch down stick. RTH CLIMB FIRST is overridden using right roll stick so craft turns and heads directly to home (CLIMB FIRST override only works for fixed wing) | | nav_rth_alt_mode | AT_LEAST | Configure how the aircraft will manage altitude on the way home, see Navigation modes on wiki for more details | | nav_rth_altitude | 1000 | Used in EXTRA, FIXED and AT_LEAST rth alt modes [cm] (Default 1000 means 10 meters) | | nav_rth_climb_first | ON | If set to ON or ON_FW_SPIRAL aircraft will climb to nav_rth_altitude first before turning to head home. If set to OFF aircraft will turn and head home immediately climbing on the way. For a fixed wing ON will use a linear climb, ON_FW_SPIRAL will use a loiter turning climb with climb rate set by nav_auto_climb_rate, turn rate set by nav_fw_loiter_radius (ON_FW_SPIRAL is a fixed wing setting and behaves the same as ON for a multirotor) | diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 884ac0a2aa..c475c5d72d 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -2341,7 +2341,7 @@ groups: field: general.flags.rth_alt_control_mode table: nav_rth_alt_mode - name: nav_rth_alt_control_override - description: "If set to ON RTH altitude and CLIMB FIRST settings can be overridden during the RTH climb phase using full pitch or roll stick held for > 2 seconds. RTH altitude is reset to the current altitude using pitch down stick. RTH CLIMB FIRST is overridden using right roll stick so craft turns and heads directly to home." + description: "If set to ON RTH altitude and CLIMB FIRST settings can be overridden during the RTH climb phase using full pitch or roll stick held for > 1 second. RTH altitude is reset to the current altitude using pitch down stick. RTH CLIMB FIRST is overridden using right roll stick so craft turns and heads directly to home (CLIMB FIRST override only works for fixed wing)" default_value: OFF field: general.flags.rth_alt_control_override type: bool From 3f180038952cb9811588d4aa03706f5940420354 Mon Sep 17 00:00:00 2001 From: "Pawel Spychalski (DzikuVx)" Date: Wed, 14 Apr 2021 09:32:55 +0200 Subject: [PATCH 172/286] Move Lagacy Boards from docs to Wiki --- docs/Board - ChebuzzF3.md | 88 ------------ docs/Board - ColibriRace.md | 126 ---------------- docs/Board - MotoLab.md | 101 ------------- docs/Board - OMNIBUS.md | 87 ----------- docs/Board - Paris Air Hero 32 F3.md | 46 ------ docs/Board - Paris Air Hero 32.md | 46 ------ docs/Board - RMDO.md | 12 -- docs/Board - SPRacingF3.md | 97 ------------- docs/Board - SPRacingF3EVO.md | 160 --------------------- docs/Board - SPRacingF3EVO_1SS.md | 159 -------------------- docs/Board - Sparky.md | 207 --------------------------- 11 files changed, 1129 deletions(-) delete mode 100644 docs/Board - ChebuzzF3.md delete mode 100755 docs/Board - ColibriRace.md delete mode 100644 docs/Board - MotoLab.md delete mode 100644 docs/Board - OMNIBUS.md delete mode 100755 docs/Board - Paris Air Hero 32 F3.md delete mode 100644 docs/Board - Paris Air Hero 32.md delete mode 100644 docs/Board - RMDO.md delete mode 100644 docs/Board - SPRacingF3.md delete mode 100644 docs/Board - SPRacingF3EVO.md delete mode 100644 docs/Board - SPRacingF3EVO_1SS.md delete mode 100644 docs/Board - Sparky.md diff --git a/docs/Board - ChebuzzF3.md b/docs/Board - ChebuzzF3.md deleted file mode 100644 index 7df5604125..0000000000 --- a/docs/Board - ChebuzzF3.md +++ /dev/null @@ -1,88 +0,0 @@ - -## Connections - -Board orientation. - -These notes assume that when the board is placed with the header pins facing up, the bottom right of the board is next to the 8 sets of INPUT pin headers. -Inner means between the two rows of header sockets, outer means between the left/right board edges and the header sockets. - - -### SPI2 / External SPI - -sclk GPIOB 13 -miso GPIOB 14 -mosi GPIOB 15 - - -There are 4 pins, labelled CS1-4 next to a label that reads Ext SPI. The 3rd pin is connected to the flash chip on -the bottom right inner of the board. The other pins on the flash chip are wired up to PB3/4/5 - -### SPI3 / SPI - -sclk GPIOB 3 -miso GPIOB 4 -mosi GPIOB 5 - -ssel 1 GPIOB 10 / Ext SPI CS1 -ssel 2 GPIOB 11 / Ext SPI CS2 -ssel 3 GPIOB 12 / Ext SPI CS3 - wired up to Slave Select of M25P16 15MBitFlash chip -ssel 4 GPIOB 13 / Ext SPI CS4 - not usable since it is used for SPI2 sclk - -### RC Input - -INPUT -PA8 / CH1 - TIM1_CH1 -PB8 / CH2 - TIM16_CH1 -PB9 / CH3 - TIM17_CH1 -PC6 / CH4 - TIM8_CH1 -PC7 / CH5 - TIM8_CH2 -PC8 / CH6 - TIM8_CH3 -PF9 / CH7 - TIM15_CH1 -PF10 / CH8 - TIM15_CH2 - -### PWM Outputs - -OUTPUT -PD12 / CH1 - TIM4_CH1 -PD13 / CH2 - TIM4_CH2 -PD14 / CH3 - TIM4_CH3 -PD15 / CH4 - TIM4_CH4 -PA1 / CH5 - TIM2_CH2 -PA2 / CH6 - TIM2_CH3 -PA3 / CH7 - TIM2_CH4 -PB0 / CH8 - TIM3_CH3 -PB1 / CH9 - TIM3_CH4 -PA4 / CH10 - TIM3_CH2 - -### Other ports - -There is space for a MS5611 pressure sensor at the top left inner of the board. - -There is an I2C socket on the left outer of the board which connects to a PCA9306 I2C level shifter directly opposite (inner). -The PCA9306 is not populated on some boards and thus the I2C socket is unusable. - -There is a CAN socket on the top right outer of the board which connects to a MAX3015 CAN Tranceiver. -The MAX3015 is not populated on some boards and thus the CAN socket is unusable. - -There are some solder pads labelled Ext 1-4 at the top right inner of the board. - -GPIOE 6 / PE6 / Ext 1 -GPIOD 3 / PD3 / Ext 2 -GPIOD 4 / PD4 / Ext 3 -GPIOB 3 / PB3 / Ext 4 - -There are some solder pads labelled ADC0-3 & Diff Press at the top left inner of the board -They are connected to the ADC socket at the top left outer of the board - -PC3 / Diff Press - ADC12_IN9 (Differential Pressure) -PC2 / ADC2 - ADC12_IN8 -PC1 / ADC1 - ADC12_IN7 -PC0 / ADC0 - ADC12_IN6 - -There is space for a MPXV5004/MPVZ5004 differential pressure sensor, if populated it's analog pin connects to PC3. - -There are sockets for 5 UARTs labelled USART1-5. - -There is a socket labelled RX_IN. - -GPIOD 2 / PD2 / RX_IN diff --git a/docs/Board - ColibriRace.md b/docs/Board - ColibriRace.md deleted file mode 100755 index 89a3de4c58..0000000000 --- a/docs/Board - ColibriRace.md +++ /dev/null @@ -1,126 +0,0 @@ -# Board - Colibri RACE - -The Colibri RACE is a STM32F3 based flight control designed specifically to work with the TBS POWERCUBE multi rotor stack. - -## Hardware Features: -* STM32F303 based chipset for ultimate performance -* PPM, SBUS, DSM, DSMX input (5V and 3.3V provided over internal BUS). No inverters or hacks needed. -* 6 PWM ESC output channels (autoconnect, internal BUS) - * RGB LED strip support incl. power management - * Extension port for GPS / external compass / pressure sensor - * UART port for peripherals (Blackbox, FrSky telemetry etc.) - * Choose between plug & play sockets or solder pads for R/C and buzzer - * 5V buzzer output - * MPU6500 new generation accelerometer/gyro - * 3x status LED (DCDC pwr/ 3.3V pwr/ status) - * Battery monitoring for 12V, 5V and VBat supply - * Size: 36mmx36mm (30.5mm standard raster) - * Weight: 4.4g - - For more details please visit: - http://www.team-blacksheep.com/powercube - -## Serial Ports - - | Value | Identifier | Board Markings | Notes | - | ----- | ------------ | -------------- | ------------------------------------------| - | 1 | VCP | USB PORT | Main Port For MSP | - | 2 | USART1 | FREE PORT | PC4 and PC5(Tx and Rx respectively) | - | 3 | USART2 | PPM Serial | PA15 | - | 4 | USART3 | GPS PORT | PB10 and PB11(Tx and Rx respectively) | - -## Pinouts - - Full pinout details are available in the manual, here: - - http://www.team-blacksheep.com/colibri_race - - -### SWD - ICSP - - | Pin | Function | Notes | - | --- | -------------- | -------------------------------------------- | - | 1 | VCC_IN | 3.3 Volt | - | 2 | SWDIO | | - | 3 | nRESET | | - | 4 | SWCLK | | - | 5 | Ground | | - | 6 | SWO/TDO | | - -### Internal Bus - - | Pin | Function | Notes | - | --- | -------------- | -------------------------------------------- | - | 1 | PWM1 | MOTOR 1 | - | 2 | PWM2 | MOTOR 2 | - | 3 | PWM3 | MOTOR 3 | - | 4 | PWM4 | MOTOR 4 | - | 5 | PWM5 | MOTOR 5 (For Y6 or Hex X) | - | 6 | PWM6 | MOTOR 6 (For Y6 or Hex X) | - | 7 | BST SDA | Use For TBS CorePro Control Device | - | 8 | BST SCL | Use For TBS CorePro Control Device | - | 9 | PWM7 | Can be a normal GPIO (PA1) or PWM | - | 10 | PWM8 | Can be a normal GPIO (PA2) or PWM | - | 11 | 12.2V DCDC | If 12v is detected, the Blue LED will turn on| - | 12 | 5.1V DCDC | Voltage for MCU | - -### Servo - - | Pin | Function | Notes | - | --- | -------------- | -------------------------------------------- | - | 1 | Ground | | - | 2 | VCC_OUT | 5.1 Volt output to LCD Strip | - | 3 | PWM Servo | PB14 - PWM10 | - -### IO_1 - LED Strip - - | Pin | Function | Notes | - | --- | ----------------- | -------------------------------------------- | - | 1 | LED_STRIP | Enable `feature LED_STRIP` | - | 2 | VCC_OUT | 5.1 Volt output to LCD Strip | - | 3 | Ground | | - -### IO_2 - Sensor Interface - - | Pin | Function | Notes | - | --- | ----------------- | -------------------------------------------- | - | 1 | VCC_OUT | 4.7 Volt output to the device | - | 2 | Ground | | - | 3 | UART3 TX | GPS | - | 4 | UART3 RX | GPS | - | 5 | SDA | mag, pressure, or other i2c device | - | 6 | SCL | mag, pressure, or other i2c device | - -### IO_3 - RC input - - IO_3 is used for RX_PPM/RX_SERIAL. Under the `PORT` tab, set RX_SERIAL to UART2 when using RX_SERIAL. - - | Pin | Function | Notes | - | --- | ----------------- | -------------------------------------------- | - | 1 | PPM/Serial | Can PPM or Serial input | - | 2 | VCC_OUT | 3.3 Volt output to the device | - | 3 | Ground | | - | 4 | VCC_OUT | 5.1 Volt output to the device | - -### IO_4 - Buzzer - - | Pin | Function | Notes | - | --- | ----------------- | -------------------------------------------- | - | 1 | BUZZER | Normal high (5.1v) | - | 2 | VCC_OUT | 5.1 Volt output to the device | - -### IO_5 - Free UART - - | Pin | Function | Notes | - | --- | ----------------- | -------------------------------------------- | - | 1 | UART1 TX | Free UART | - | 2 | UART1 RX | Free UART | - | 3 | Ground | | - | 4 | VCC_OUT | 4.7 Volt output to the device | - -### IO_6 - IR TX (extension) - - | Pin | Function | Notes | - | --- | ----------------- | -------------------------------------------- | - | 1 | IR TX | | - | 2 | Ground | | diff --git a/docs/Board - MotoLab.md b/docs/Board - MotoLab.md deleted file mode 100644 index 8a5f0f90ff..0000000000 --- a/docs/Board - MotoLab.md +++ /dev/null @@ -1,101 +0,0 @@ -# Board - MotoLab - -The MOTOLAB build target supports the STM32F3-based boards provided by MotoLab. - -At present this includes the TornadoFC and MotoF3. The TornadoFC is described here: - -http://www.rcgroups.com/forums/showthread.php?t=2473157 - -The MotoF3 documentation will be provided when the board is available. - -Both boards use the STM32F303 microcontroller and have the following features: - -* 256K bytes of flash memory -* Floating point math coprocessor -* Three hardware serial port UARTs -* USB using the built-in USB phy that does not interfere with any hadware UART -* Stable voltage regulation -* High-current buzzer/LED output -* Serial LED interface -* Low-pass filtered VBAT input with 1/10 divider ratio -* 8 short-circuit protected PWM outputs, with 5V buffering on the TornadoFC -* On-board 6S-compatible switching regulator (MotoF3) -* Direct mounting option for a Pololu switching regulator for up to 6S lipo operation (TornadoFC) - - -# Flashing - -The MotoLab boards use the internal DFU USB interface on the STM32F3 microcontroller which is not compatible with the INAV configurator flashing tool. - -Instead, on Windows you can use the Impulse Flashing Utility from ImpulseRC, available here: - -http://www.warpquad.com/ImpulseFlash.zip - -Download and unzip the program. Start the program, plug in the USB on the target board, and drag and drop the intended binary file onto the program icon. The program will put the STM32F3 into bootloader mode automatically and load the binary file to the flash. - -For programming on Linux, use the dfu-util program which is installed by default on Ubuntu-based systems. Connect the boot pins on the board and plug in the USB. - -Verify that the system identifies the DFU device with this command: -``` -dfu-util -l -``` - -The output should list a "Found DFU" device, something like this: -``` -dfu-util 0.5 - -(C) 2005-2008 by Weston Schmidt, Harald Welte and OpenMoko Inc. -(C) 2010-2011 Tormod Volden (DfuSe support) -This program is Free Software and has ABSOLUTELY NO WARRANTY - -dfu-util does currently only support DFU version 1.0 - -Found DFU: [0483:df11] devnum=0, cfg=1, intf=0, alt=0, name="@Internal Flash /0x08000000/128*0002Kg" -Found DFU: [0483:df11] devnum=0, cfg=1, intf=0, alt=1, name="@Option Bytes /0x1FFFF800/01*016 e" -``` - -Use this command to load the binary file to the flash memory on the board: -``` -dfu-util --alt 0 -s 0x08000000 -D -``` - -The output should look something like this: -``` -dfu-util 0.5 - -(C) 2005-2008 by Weston Schmidt, Harald Welte and OpenMoko Inc. -(C) 2010-2011 Tormod Volden (DfuSe support) -This program is Free Software and has ABSOLUTELY NO WARRANTY - -dfu-util does currently only support DFU version 1.0 - -Opening DFU USB device... ID 0483:df11 -Run-time device DFU version 011a -Found DFU: [0483:df11] devnum=0, cfg=1, intf=0, alt=0, name="@Internal Flash /0x08000000/128*0002Kg" -Claiming USB DFU Interface... -Setting Alternate Setting #0 ... -Determining device status: state = dfuDNLOAD-IDLE, status = 0 -aborting previous incomplete transfer -Determining device status: state = dfuIDLE, status = 0 -dfuIDLE, continuing -DFU mode device DFU version 011a -Device returned transfer size 2048 -No valid DFU suffix signature -Warning: File has no DFU suffix -DfuSe interface name: "Internal Flash " -``` - -A binary file is required for the Impulse flashing Utility and dfu-util. The binary file can be built as follows: -``` -make TARGET=MOTOLAB clean -make TARGET=MOTOLAB binary -``` - -To completely erase the flash, create an all-zero file with this command on linux: -``` -dd if=/dev/zero of=zero.bin bs=1 count=262144 -``` - -## Todo - -Pinout documentation diff --git a/docs/Board - OMNIBUS.md b/docs/Board - OMNIBUS.md deleted file mode 100644 index 7918e8a5bf..0000000000 --- a/docs/Board - OMNIBUS.md +++ /dev/null @@ -1,87 +0,0 @@ -# Board - OMNIBUS F3 - -> This board is not supported in recent INAV releases - -## Hardware Features - -Refer to the product web page: -[OMNIBUS AIO F3 Flight Control](http://shop.myairbot.com/index.php/flight-control/cleanflight-baseflight/omnibusv11.html) - -### Hardware Notes - -There are few things to note on how things are connected on the board. - -1. VBAT (J4) -This is a battery input to the board, and is also a input to voltage sensor. - -2. J11 Power distribution -The RAM is user defined power rail, and all RAM through holes (J6, J7 and J11) are connected together. By connecting 5V or VBAT to RAM at J11, the RAM becomes 5V or VBAT power rail respectively. The VBAT on J11 can also be used to power the Board if necessary. - -3. RSSI (J4) -The pin is labelled as RSSI, but it will not be used for RSSI input for a hardware configuration limitation. In this document, the "RSSI" is used to indicate the pin location, not the function. - -4. UART1 in boot-loader/DFU mode -The UART1 is scanned during boot-loader/DFU mode, together with USB for possible interaction with a host PC. It is observed that devices that autonomously transmits some data, such as GPS, will prevent the MCU to talk to the USB. It is advised not to connect or disconnect such devices to/from UART1. UART2 is safe from this catch. - -## iNav Specific Target Configuration - -The first support for the OMNIBUS F3 appeared in BetaFlight. -The OMNIBUS target in iNav has different configuration from the BetaFlight support, to maximize the hardware resource utilization for navigation oriented use cases. - - [PIN CONFIGURATION PIC HERE] - -### PWM Outputs - -Six PWM outputs (PWM1~PWM6) are supported, but PWM5 and PWM6 is not available when UART3 is in use. -PWM7 and PWM8 are dedicated for I2C; in this document, they are used to indicate the pin location, not the function. - -If servos are used on a multirotor mixer (i.e. Tricopter) PWM1 is remapped to servo and motor 1 is moved to PWM2 etc. - -Note: Tested only for QUAD-X configuration. - -### Hardware UART Ports - -PPM/SBUS jumper for J8 is assumed to be configured for PPM (SBUS=R18 removed). With newer boards (the 1.1 Version) you don't have to swap an smd resistor to use SBUS anymore. It just works out of the box. - -| UART | Location | Note | -|-------|----------|-------------------| -| UART1 |J13 | | -| UART2 |J12 | | -| UART3 |J22 | PWM5=TX3,PWM6=RX3 | - -All UARTs are Serial RX capable. - -### I2C - -I2C is available on J22 PWM7 and PWM8 - -|signal | Location | Alt. Location | -|-------|------------|---------------| -|SCL | J22 (PWM8) | J3 (SCL) | -|SDA | J22 (PWM7) | J3 (SDA) | - -### RANGEFINDER - -HC-SR04 rangefinder is supported when NOT using PPM. - -|signal | Location | -|-------|------------| -|TRIG | J8 (PPM) | -|ECHO | J4 (RSSI) | - -5V rangefinder can be connected directly without inline resistors. - -### OSD - -Integrated OSD is supported. - -### RSSI Sensor Input - -The RSSI sensor adc is not supported due to the hardware configuration limitation. - -## Usage in a Fixed Wing -Due to the way INAV handles PWM outputs the first 2 PWM outputs are reserved for the motor outputs. When using SBUS on UART3 as recommended this leaves only 2 additional outputs for the servos, as output 5 and 6 are blocked by UART3 serial for SBUS and 7 and 8 are used for I2C. - -You can free PWM outputs 5 and 6 by simply connecting SBUS up to UART1. For FrSky there is no hardware inverter needed as the F3 chip UARTs can handle this without additional hardware. Just make sure that `sbus_inversion = ON` is set. However, you will not be able to use UART3, e.G. for telemetry. - -This allows to control a standard airplane with rudder, ailerons and elevator. If you use flaps or a servo gimbal, you can bypass the FC by connecting it up to the receiver directly. diff --git a/docs/Board - Paris Air Hero 32 F3.md b/docs/Board - Paris Air Hero 32 F3.md deleted file mode 100755 index e1f0d58811..0000000000 --- a/docs/Board - Paris Air Hero 32 F3.md +++ /dev/null @@ -1,46 +0,0 @@ -# Board - Paris Air Hero 32 - -This is the AIR3 PARIS Sirius AirHERO 32 F3 board from MultiWiiCopter -Source: http://www.multiwiicopter.com/products/inav-air3-fixed-wing - -## Sensors - -MPU6500 via SPI interface. -BMP280 via SPI interface - -## Ports - -6 x 3pin ESC / Servo outputs -1 x 8pin JST connector (PPM/PWM/UART2) -1 x 4pin JST connector (UART3) - -## I2C bus - -I2C bus is made available with a special target - AIRHEROF3_QUAD. This target limits motor outputs to 4 and adds I2C bus at M5/M6 connectors. - -## Pinouts - -The 10 pin RC I/O connector has the following pinouts when used in RX_PPM/RX_SERIAL mode. - -From right to left when looking at the socket from the edge of the board. - -| Pin | Function | Notes | -| --- | -------------- | -------------------------------- | -| 1 | Ground | | -| 2 | +5V | | -| 3 | RX_PPM | Enable `feature RX_PPM` | -| 4 | AIRSPEED | Airspeed sensor (3.3V max) | -| 5 | USART2 TX | | -| 6 | USART2 RX | | -| 7 | SS1 RX | Enable `feature SOFT_SERIAL` | -| 8 | SS1 TX | | - - -## Serial Ports - -| Value | Identifier | RX | TX | Notes | -| ----- | ------------ | ---------- | ------------------ | ------------------------------------------------------------------------------------------- | -| 1 | USART1 | RX / PA10 | TX / PA9 | Internally connected to USB port via CP2102 IC | -| 2 | USART2 | RC4 / PA3 | RC3 / PA2 | | -| 3 | USART3 | F3 / PB11 | F2 / PB10 | | - diff --git a/docs/Board - Paris Air Hero 32.md b/docs/Board - Paris Air Hero 32.md deleted file mode 100644 index d07e9a309c..0000000000 --- a/docs/Board - Paris Air Hero 32.md +++ /dev/null @@ -1,46 +0,0 @@ -# Board - Paris Air Hero 32 - -## Sensors - -MPU6500 via SPI interface. - -## Ports - -6 x 3pin ESC / Servo outputs -1 x 8pin JST connector (PPM/PWM/UART2) -1 x 4pin JST connector (UART3/I2C) - -## Pinouts - -The 10 pin RC I/O connector has the following pinouts when used in RX_PPM/RX_SERIAL mode. - -From right to left when looking at the socket from the edge of the board. - -| Pin | Function | Notes | -| --- | -------------- | -------------------------------- | -| 1 | Ground | | -| 2 | +5V | | -| 3 | RX_PPM | Enable `feature RX_PPM` | -| 4 | RSSI_ADC | Enable `feature RSSI_ADC`. Connect to the output of a PWM-RSSI conditioner, 0v-3.3v input | -| 5 | USART2 TX | | -| 6 | USART2 RX | Built-in inverter | -| 7 | LED_STRIP | Enable `feature LED_STRIP` | -| 8 | unused | | - -When SOFTSERIAL is enabled, LED_STRIP and CURRENT_METER are unavailable, but one SoftSerial port is made available to use instead. - -| Pin | Function | Notes | -| --- | -------------- | -------------------------------- | -| 7 | SOFTSERIAL1 RX | Enable `feature SOFTSERIAL` | -| 8 | SOFTSERIAL1 TX | | - - -## Serial Ports - -| Value | Identifier | RX | TX | Notes | -| ----- | ------------ | ---------- | ------------------ | ------------------------------------------------------------------------------------------- | -| 1 | USART1 | RX / PA10 | TX / PA9 / TELEM | TELEM output is always inverted (for FrSky). Internally connected to USB port via CP2102 IC | -| 2 | USART2 | RC4 / PA3 | RC3 / PA2 | | -| 3 | USART3 | F3 / PB11 | F2 / PB10 | Flex port is configured as UART3 when port is configured | -| 4 | SOFTSERIAL1 | RC5 / PA6 | RC6 / PA7 | | - diff --git a/docs/Board - RMDO.md b/docs/Board - RMDO.md deleted file mode 100644 index c785930f46..0000000000 --- a/docs/Board - RMDO.md +++ /dev/null @@ -1,12 +0,0 @@ -# Board - RMDO - -The DoDo board is a clone of the SPRacingF3 board in terms of CPU pin mappings. See the SPRacingF3 documentation. - -Hardware differences compared to SPRacingF3 are as follows: - -* Rev 1 and Rev 2: the CPU is the cheaper version of the F3 with only 128KB FLASH. Rev 3: the CPU is a F3 version with 256KB FLASH. -* 2MBit flash size -* The barometer is the cheaper BMP280. -* It does not have any compass sensor. -* Onboard BEC. -* Different physical connectors/pins/pads/ports. diff --git a/docs/Board - SPRacingF3.md b/docs/Board - SPRacingF3.md deleted file mode 100644 index b54354f69b..0000000000 --- a/docs/Board - SPRacingF3.md +++ /dev/null @@ -1,97 +0,0 @@ -# Board - SPRacingF3 - -The Seriously Pro Racing MOF3 board (SPRacingF3) is the first board designed specifically for INAV. - -Full details available on the website, here: - -http://seriouslypro.com/spracingf3 - -## Hardware Features - -* No compromise I/O. Use all the features all the time; e.g. Connect your OSD + SmartPort + SBus + GPS + LED Strip + Battery Monitoring + HC-SR04 + 8 motors - all at the same time! -* On-board high-capacity black box flight log recorder - optimize your tuning and see the results of your setup without guesswork. (Acro and Deluxe) -* Next-generation STM32 F3 processor with hardware floating point unit for efficient flight calculations and faster ARM-Cortex M4 core. -* Stackable design - perfect for integrating with OSDs and power distribution boards. -* 16 PWM I/O lines for ESCs, Servos and legacy receivers. 8 available on standard pin headers. 8 via side mounted connectors. -* Supports SBus, SumH, SumD, Spektrum1024/2048, XBus, PPM, PWM receivers. No external inverters required (built-in). -* Dedicated output for programmable LEDs - great for orientation, racing and night flying. -* Dedicated I2C port for connection of OLED display without needing flight battery. -* Battery monitoring ports for voltage and current. -* Buzzer port for audible warnings and notifications. -* Solder pads in addition to connectors for HC-SR04, PPM, RSSI, Current, GPIO, LED Strip, 3.3v, -* Developer friendly debugging port (SWD) and boot mode selection, unbrickable bootloader. -* Symmetrical design for a super tidy wiring. -* Wire up using using pin headers, JST-SH sockets or solder pads. Use either right-angled or straight pin-headers. -* Barometer mounted on the bottom of the board for easy wind isolation. - -## Serial Ports - -| Value | Identifier | RX | TX | 5v Tolerant | Notes | -| ----- | ------------ | ------------ | ------------ | ----------- | ------------------------------------------------------------------------------------------- | -| 1 | USART1 | PA10 | PA9 | YES | Internally connected to USB port via CP2102 IC. Also available on a USART1 JST connector and on through hole pins. | -| 2 | USART2 | PA15 | PA14 | YES | Available on USART2 JST port only. | -| 3 | USART3 | PB11 / IO2_3 | PB10 / IO2_4 | NO | Available on IO_2, USART3 JST port and through hole pins. | - -* You cannot use SWD and USART2 at the same time. -* You may encounter flashing problems if you have something connected to the USART1 RX/TX pins. Power other devices of and/or disconnect them. - -## Pinouts - -Full pinout details are available in the manual, here: - -http://seriouslypro.com/spracingf3#manual - -### IO_1 - -| Pin | Function | Notes | -| --- | -------------- | -------------------------------------------- | -| 1 | Ground | | -| 2 | VCC_IN | Voltage as-supplied by BEC. | -| 3 | RX_PPM | Enable `feature RX_PPM` | -| 4 | GPIO | | -| 5 | SoftSerial1_RX | | -| 6 | SoftSerial1_TX | | -| 7 | LED_STRIP | Enable `feature LED_STRIP` | -| 8 | VCC | 3.3v output for LOW CURRENT application only | - -### IO_2 - -| Pin | Function | Notes | -| --- | ------------------------- | -------------------------------------------- | -| 1 | Ground | | -| 2 | VCC_IN | Voltage as-supplied by BEC. | -| 3 | RX_SERIAL | UART3 RX | -| 4 | | UART3_TX | -| 5 | HC-SR04_TRIG/SoftSerial2_RX | Enable `feature SOFTSERIAL` or HC-SR04 rangefinder | -| 6 | HC-SR04_ECHO/SoftSerial2_TX | Enable `feature SOFTSERIAL` or HC-SR04 rangefinder | -| 7 | ADC_1 | Current Sensor | -| 8 | ADC_2 | RSSI | - -### UART1/2/3 - -| Pin | Function | Notes | -| --- | -------------- | -------------------------------------------- | -| 1 | Ground | | -| 2 | VCC_IN | Voltage as-supplied by BEC. | -| 3 | TXD | | -| 4 | RXD | | - -### I2C - -| Pin | Function | Notes | -| --- | -------------- | -------------------------------------------- | -| 1 | Ground | | -| 2 | 5.0v | Voltage as-supplied by BEC OR USB, always on | -| 3 | SCL | | -| 4 | SDA | | - -### SWD - -The port cannot be used at the same time as UART2. - -| Pin | Function | Notes | -| --- | -------------- | -------------------------------------------- | -| 1 | Ground | | -| 2 | NRST | Voltage as-supplied by BEC OR USB, always on | -| 3 | SWDIO | | -| 4 | SWDCLK | | diff --git a/docs/Board - SPRacingF3EVO.md b/docs/Board - SPRacingF3EVO.md deleted file mode 100644 index 874f3e38f6..0000000000 --- a/docs/Board - SPRacingF3EVO.md +++ /dev/null @@ -1,160 +0,0 @@ -# Board - Seriously Pro SP Racing F3 EVO - -The Seriously Pro Racing F3 Evo board (SPRacingF3EVO) is the evolution of the first board designed specifically for Cleanflight. - -Purchasing boards directly from SeriouslyPro / SP Racing and official retailers helps fund Cleanflight development, it's the reason the Seriously Pro boards exist! Official retailers are always listed on the SeriouslyPro.com website. - -Full details available on the website, here: - -http://seriouslypro.com/spracingf3evo - -## Hardware Features - -* Next-generation STM32 F3 processor with hardware floating point unit for efficient flight calculations and faster ARM-Cortex M4 core. -* MicroSD-Card socket for black box flight log recorder - optimize your tuning and see the results of your setup without guesswork. -* Race transponder built in - just turn up at a race and have your lap times recorded. -* Features the latest Accelerometer, Gyro and Mag/Compass and Baro/Altitude sensor technology. -* Wire up using using pin headers for all major connections for excellent crash-durability. Use either right-angled or straight pin-headers. -* No compromise I/O. Use all the features all the time; e.g. Connect your USB + OSD + SmartPort + SBus + GPS + LED Strip + Battery Monitoring + 8 motors - all at the same time! -* 8 PWM output lines for ESCs and Servos. Arranged for easy wiring on standard pin headers. -* Supports direct connection of SBus, SumH, SumD, Spektrum1024/2048, XBus receivers. No external inverters required (built-in). -* Supports direct connection of 3.3v Spektrum Satellite receivers via 3 pin through-hole JST-ZH connector. -* Dedicated PPM receiver input. -* 3 Serial Ports - NOT shared with the USB socket. -* Telemetry port -* Micro USB socket. -* Dedicated output for programmable LEDs - great for orientation, racing and night flying. (Currently mutually exclusive with the Transponder). -* Dedicated I2C port for connection of OLED display without needing flight battery. -* Battery monitoring for voltage and current. -* RSSI monitoring (analogue or PWM). -* Buzzer port for audible warnings and notifications. -* Developer friendly debugging port (SWD) and boot mode selection, unbrickable bootloader. -* Symmetrical design for a super tidy wiring. -* JST-SH sockets only for I2C, UART2 and SWD. UART2 also on through-hole pins. -* Flashing via USB or serial port. -* Stackable design - perfect for integrating with OSDs and power distribution boards. -* Standard mounting - 36x36mm with standard 30.5mm mounting holes. -* LEDs for 3v, 5v and Status for easy diagnostics. -* Copper-etched Cleanflight logo. - -## Serial Ports - -| Value | Identifier | RX | TX | 5v Tolerant | Notes | -| ----- | ------------ | ------------ | ------------ | ----------- | ------------------------------------------------------------------------------------------- | -| 1 | USART1 | PA10 | PA9 | YES | 2 through-hole pins. Use for connecting to OSD/GPS/BlueTooth. | -| 2 | USART2 | PA15 | PA14 / SWCLK | YES | JST socket and PPM header. Use to connect to RX. | -| 3 | USART3 | PB11 / AF7 | PB10 / AF7 | NO | Available on 4 through-hole pins. 3.3V signals only ! Use for GPS, Spektrum Satellite RX, SmartPort Telemetry, HoTT telemetry, etc. | - -* You cannot use SWD and USART2 at the same time. -* When using a Serial RX receiver the TXD (T2) pin cannot be used for telemetry. Use UART3 TXD instead. -* Two SoftSerial is supported. Enabling SoftSerial disables Servo output 3,4,5,6 -* Windows DFU Flashing requires Zadig (see configurator) - -### SoftSerial - -| Pin | Function | Notes | -| --- | -------------- | -------------------------------- | -| 5 | SOFTSERIAL1 RX | SoftSerial replaces Servo 3,4,5,6| -| 6 | SOFTSERIAL1 TX | | -| 7 | SOFTSERIAL2 RX | | -| 8 | SOFTSERIAL2 TX | | - - -## Pinouts - -Full pinout details are available in the manual, here: - -http://seriouslypro.com/files/SPRacingF3EVO-Manual-latest.pdf - -### IO_1 - -The 6 pin IO_1 connector has the following pinouts when used in RX_SERIAL mode. - -| Pin | Function | Notes | -| --- | -------------- | -------------------------------------------- | -| 1 | Ground | | -| 2 | VCC_IN | Voltage as-supplied by BEC. | -| 3 | RX_SERIAL | Enable `feature RX_SERIAL` | -| 4 | | | -| 5 | +V BATTERY | Voltage as-supplied by Battery. | -| 6 | -V BATTERY | Voltage as-supplied by Battery. | - -When RX_PPM is used the IO_1 pinout is as follows. - -| Pin | Function | Notes | -| --- | -------------- | -------------------------------------------- | -| 1 | Ground | | -| 2 | VCC_IN | Voltage as-supplied by BEC. | -| 3 | RX_PPM | Enable `feature RX_PPM` | -| 4 | TELEMETRY | Enable `feature TELEMETRY` | -| 5 | +V BATTERY | Voltage as-supplied by Battery. | -| 6 | -V BATTERY | Voltage as-supplied by Battery. | - -### IO_2 - -When TRANSPONDER is used and the IR solder pads are shorted, the 6 pin IO_2 pinout is as follows. - -| Pin | Function | Notes | -| --- | ------------------------- | -------------------------------------------- | -| 1 | IR- | Short leg of the IR LED | -| 2 | IR+ | Long leg of the IR LED | -| 3 | CURRENT | Current Sensor | -| 4 | RSSI | RSSI (PWM or Analog - select by solder pads) | -| 5 | BUZZER+ | 5V Source | -| 6 | BUZZER- | Buzzer signal | - -When LEDSTRIP is used and the LED solder pads are shorted, the 6 pin IO_2 pinout is as follows. - -| Pin | Function | Notes | -| --- | ------------------------- | -------------------------------------------- | -| 1 | | | -| 2 | LEDSTRIP | WS2812 Ledstrip data | -| 3 | CURRENT | Current Sensor | -| 4 | RSSI | RSSI (PWM or Analog - select by solder pads) | -| 5 | BUZZER+ | 5V Source | -| 6 | BUZZER- | Buzzer signal | - -### UART1 - -| Pin | Function | Notes | -| --- | -------------- | -------------------------------------------- | -| 3 | TXD | | -| 4 | RXD | | - -### UART2/3 - -| Pin | Function | Notes | -| --- | -------------- | -------------------------------------------- | -| 1 | Ground | | -| 2 | VCC_IN | Voltage as-supplied by BEC. | -| 3 | TXD | | -| 4 | RXD | | - -### Spektrum Satellite - -| Pin | Function | Notes | -| --- | -------------- | -------------------------------------------- | -| 3 | 3.3V | | -| 2 | Ground | | -| 1 | RXD | | - -### I2C - -| Pin | Function | Notes | -| --- | -------------- | -------------------------------------------- | -| 1 | Ground | | -| 2 | 5.0v | Voltage as-supplied by BEC OR USB, always on | -| 3 | SCL | 3.3V signals only | -| 4 | SDA | 3.3V signals only | - -### SWD - -The port cannot be used at the same time as UART2. - -| Pin | Function | Notes | -| --- | -------------- | -------------------------------------------- | -| 1 | Ground | | -| 2 | NRST | | -| 3 | SWDIO | | -| 4 | SWDCLK | | - diff --git a/docs/Board - SPRacingF3EVO_1SS.md b/docs/Board - SPRacingF3EVO_1SS.md deleted file mode 100644 index eeaf4233ee..0000000000 --- a/docs/Board - SPRacingF3EVO_1SS.md +++ /dev/null @@ -1,159 +0,0 @@ -# Board - Seriously Pro SP Racing F3 EVO - -The Seriously Pro Racing F3 Evo board (SPRacingF3EVO) is the evolution of the first board designed specifically for Cleanflight. - -Purchasing boards directly from SeriouslyPro / SP Racing and official retailers helps fund Cleanflight development, it's the reason the Seriously Pro boards exist! Official retailers are always listed on the SeriouslyPro.com website. - -Full details available on the website, here: - -http://seriouslypro.com/spracingf3evo - -## Hardware Features - -* Next-generation STM32 F3 processor with hardware floating point unit for efficient flight calculations and faster ARM-Cortex M4 core. -* MicroSD-Card socket for black box flight log recorder - optimize your tuning and see the results of your setup without guesswork. -* Race transponder built in - just turn up at a race and have your lap times recorded. -* Features the latest Accelerometer, Gyro and Mag/Compass and Baro/Altitude sensor technology. -* Wire up using using pin headers for all major connections for excellent crash-durability. Use either right-angled or straight pin-headers. -* No compromise I/O. Use all the features all the time; e.g. Connect your USB + OSD + SmartPort + SBus + GPS + LED Strip + Battery Monitoring + 8 motors - all at the same time! -* 8 PWM output lines for ESCs and Servos. Arranged for easy wiring on standard pin headers. -* Supports direct connection of SBus, SumH, SumD, Spektrum1024/2048, XBus receivers. No external inverters required (built-in). -* Supports direct connection of 3.3v Spektrum Satellite receivers via 3 pin through-hole JST-ZH connector. -* Dedicated PPM receiver input. -* 3 Serial Ports - NOT shared with the USB socket. -* Telemetry port -* Micro USB socket. -* Dedicated output for programmable LEDs - great for orientation, racing and night flying. (Currently mutually exclusive with the Transponder). -* Dedicated I2C port for connection of OLED display without needing flight battery. -* Battery monitoring for voltage and current. -* RSSI monitoring (analogue or PWM). -* Buzzer port for audible warnings and notifications. -* Developer friendly debugging port (SWD) and boot mode selection, unbrickable bootloader. -* Symmetrical design for a super tidy wiring. -* JST-SH sockets only for I2C, UART2 and SWD. UART2 also on through-hole pins. -* Flashing via USB or serial port. -* Stackable design - perfect for integrating with OSDs and power distribution boards. -* Standard mounting - 36x36mm with standard 30.5mm mounting holes. -* LEDs for 3v, 5v and Status for easy diagnostics. -* Copper-etched Cleanflight logo. - -## Serial Ports - -| Value | Identifier | RX | TX | 5v Tolerant | Notes | -| ----- | ------------ | ------------ | ------------ | ----------- | ------------------------------------------------------------------------------------------- | -| 1 | USART1 | PA10 | PA9 | YES | 2 through-hole pins. Use for connecting to OSD/GPS/BlueTooth. | -| 2 | USART2 | PA15 | PA14 / SWCLK | YES | JST socket and PPM header. Use to connect to RX. | -| 3 | USART3 | PB11 / AF7 | PB10 / AF7 | NO | Available on 4 through-hole pins. 3.3V signals only ! Use for GPS, Spektrum Satellite RX, SmartPort Telemetry, HoTT telemetry, etc. | - -* You cannot use SWD and USART2 at the same time. -* When using a Serial RX receiver the TXD (T2) pin cannot be used for telemetry. Use UART3 TXD instead. -* One Software serial is supported in th SPRacingF3EVO_1SS version, see table below -* Windows DFU Flashing requires Zadig (see configurator) - -### SoftSerial - -| Pin | Function | Notes | -| --- | -------------- | -------------------------------- | -| 7 | SOFTSERIAL1 RX | SoftSerial disables Servo 5,6 | -| 8 | SOFTSERIAL1 TX | | - -## Pinouts - -Full pinout details are available in the manual, here: - -http://seriouslypro.com/files/SPRacingF3EVO-Manual-latest.pdf - -### IO_1 - -The 6 pin IO_1 connector has the following pinouts when used in RX_SERIAL mode. - -| Pin | Function | Notes | -| --- | -------------- | -------------------------------------------- | -| 1 | Ground | | -| 2 | VCC_IN | Voltage as-supplied by BEC. | -| 3 | RX_SERIAL | Enable `feature RX_SERIAL` | -| 4 | | | -| 5 | +V BATTERY | Voltage as-supplied by Battery. | -| 6 | -V BATTERY | Voltage as-supplied by Battery. | - -When RX_PPM is used the IO_1 pinout is as follows. - -| Pin | Function | Notes | -| --- | -------------- | -------------------------------------------- | -| 1 | Ground | | -| 2 | VCC_IN | Voltage as-supplied by BEC. | -| 3 | RX_PPM | Enable `feature RX_PPM` | -| 4 | TELEMETRY | Enable `feature TELEMETRY` | -| 5 | +V BATTERY | Voltage as-supplied by Battery. | -| 6 | -V BATTERY | Voltage as-supplied by Battery. | - -### IO_2 - -When TRANSPONDER is used and the IR solder pads are shorted, the 6 pin IO_2 pinout is as follows. - -| Pin | Function | Notes | -| --- | ------------------------- | -------------------------------------------- | -| 1 | IR- | Short leg of the IR LED | -| 2 | IR+ | Long leg of the IR LED | -| 3 | CURRENT | Current Sensor | -| 4 | RSSI | RSSI (PWM or Analog - select by solder pads) | -| 5 | BUZZER+ | 5V Source | -| 6 | BUZZER- | Buzzer signal | - -When LEDSTRIP is used and the LED solder pads are shorted, the 6 pin IO_2 pinout is as follows. - -| Pin | Function | Notes | -| --- | ------------------------- | -------------------------------------------- | -| 1 | | | -| 2 | LEDSTRIP | WS2812 Ledstrip data | -| 3 | CURRENT | Current Sensor | -| 4 | RSSI | RSSI (PWM or Analog - select by solder pads) | -| 5 | BUZZER+ | 5V Source | -| 6 | BUZZER- | Buzzer signal | - -### UART1 - -| Pin | Function | Notes | -| --- | -------------- | -------------------------------------------- | -| 3 | TXD | | -| 4 | RXD | | - -### UART2/3 - -| Pin | Function | Notes | -| --- | -------------- | -------------------------------------------- | -| 1 | Ground | | -| 2 | VCC_IN | Voltage as-supplied by BEC. | -| 3 | TXD | | -| 4 | RXD | | - -### Spektrum Satellite - -| Pin | Function | Notes | -| --- | -------------- | -------------------------------------------- | -| 3 | 3.3V | | -| 2 | Ground | | -| 1 | RXD | | - -### I2C - -| Pin | Function | Notes | -| --- | -------------- | -------------------------------------------- | -| 1 | Ground | | -| 2 | 5.0v | Voltage as-supplied by BEC OR USB, always on | -| 3 | SCL | 3.3V signals only | -| 4 | SDA | 3.3V signals only | - -### SWD - -The port cannot be used at the same time as UART2. - -| Pin | Function | Notes | -| --- | -------------- | -------------------------------------------- | -| 1 | Ground | | -| 2 | NRST | | -| 3 | SWDIO | | -| 4 | SWDCLK | | - - - diff --git a/docs/Board - Sparky.md b/docs/Board - Sparky.md deleted file mode 100644 index bc4612c574..0000000000 --- a/docs/Board - Sparky.md +++ /dev/null @@ -1,207 +0,0 @@ -# Board - Sparky - -The Sparky is a very low cost and very powerful board. - -* 3 hardware serial ports. -* Built-in serial port inverters which allows S.BUS receivers to be used without external inverters. -* USB (can be used at the same time as the serial ports). -* 10 PWM outputs. -* Dedicated PPM/SerialRX input pin. -* MPU9150 I2C Acc/Gyro/Mag -* Baro - -Tested with revision 1 & 2 boards. - -## TODO - -* Rangefinder -* Display (via Flex port) -* SoftSerial - though having 3 hardware serial ports makes it a little redundant. -* Airplane PWM mappings. - -# Voltage and current monitoring (ADC support) - -Voltage monitoring is possible when enabled via PWM9 pin and current can be monitored via PWM8 pin. The voltage divider and current sensor need to be connected externally. The vbatscale cli parameter need to be adjusted to fit the sensor specification. For more details regarding the sensor hardware you can check here: https://github.com/TauLabs/TauLabs/wiki/User-Guide:-Battery-Configuration - -# Flashing - -## Via Device Firmware Upload (DFU, USB) - Windows - -These instructions are for flashing the Sparky board under Windows using DfuSE. -Credits go to Thomas Shue (Full video of the below steps can be found here: https://www.youtube.com/watch?v=I4yHiRVRY94) - -Required Software: -DfuSE Version 3.0.2 (latest version 3.0.4 causes errors): http://code.google.com/p/multipilot32/downloads/detail?name=DfuSe.rar -STM VCP Driver 1.4.0: http://www.st.com/web/en/catalog/tools/PF257938 - -A binary file is required for DFU, not a .hex file. If one is not included in the release then build one as follows. - -``` -Unpack DfuSE and the STM VCP Drivers into a folder on your Hardrive -Download the latest Sparky release (inav_SPARKY.hex) from: -https://github.com/iNavFlight/inav/releases and store it on your Hardrive - -In your DfuSE folder go to BIN and start DfuFileMgr.exe -Select: "I want to GENERATE a DFUfile from S19,HEX or BIN files" press OK -Press: "S19 or Hex.." -Go to the folder where you saved the inav_SPARKY.hex file, select it and press open -(you might need to change the filetype in the DfuSE explorer window to "hex Files (*.hex)" to be able to see the file) -Press: "Generate" and select the .dfu output file and location -If all worked well you should see " Success for 'Image for lternate Setting 00 (ST..)'!" - -``` - -Put the device into DFU mode by powering on the sparky with the bootloader pins temporarily bridged. The only light that should come on is the blue PWR led. - -Check the windows device manager to make sure the board is recognized correctly. -It should show up as "STM Device in DFU mode" under Universal Serial Bus Controllers - -If it shows up as "STMicroelectronics Virtual COM" under Ports (COM & LPT) instead then the board is not in DFU mode. Disconnect the board, short the bootloader pins again while connecting the board. - -If the board shows up as "STM 32 Bootloader" device in the device manager, the drivers need to be updated manually. -Select the device in the device manager, press "update drivers", select "manual update drivers" and choose the location where you extracted the STM VCP Drivers, select "let me choose which driver to install". You shoud now be able to select either the STM32 Bootloader driver or the STM in DFU mode driver. Select the later and install. - - -Then flash the binary as below. - -``` -In your DfuSE folder go to BIN and start DfuSeDemo.exe -Select the Sparky Board (STM in DFU Mode) from the Available DFU and compatible HID Devices drop down list -Press "Choose.." at the bootom of the window and select the .dfu file created in the previous step -"File correctly loaded" should appear in the status bar -Press "Upgrade" and confirm with "Yes" -The status bar will show the upload progress and confirm that the upload is complete at the end - -``` - -Disconnect and reconnect the board from USB and continue to configure it via the INAV configurator as per normal - - -## Via Device Firmware Upload (DFU, USB) - Mac OS X / Linux - -These instructions are for dfu-util, tested using dfu-util 0.7 for OSX from the OpenTX project. - -http://www.open-tx.org/2013/07/15/dfu-util-07-for-mac-taranis-flashing-utility/ - -A binary file is required for DFU, not a .hex file. If one is not included in the release then build one as follows. - -``` -make TARGET=SPARKY clean -make TARGET=SPARKY binary -``` - -Put the device into DFU mode by powering on the sparky with the bootloader pins temporarily bridged. The only light that should come on is the blue PWR led. - -Run 'dfu-util -l' to make sure the device is listed, as below. - -``` -$ dfu-util -l -dfu-util 0.7 - -Copyright 2005-2008 Weston Schmidt, Harald Welte and OpenMoko Inc. -Copyright 2010-2012 Tormod Volden and Stefan Schmidt -This program is Free Software and has ABSOLUTELY NO WARRANTY -Please report bugs to dfu-util@lists.gnumonks.org - -Found DFU: [0483:df11] devnum=0, cfg=1, intf=0, alt=0, name="@Internal Flash /0x08000000/128*0002Kg" -Found DFU: [0483:df11] devnum=0, cfg=1, intf=0, alt=1, name="@Option Bytes /0x1FFFF800/01*016 e" -``` - -Then flash the binary as below. - -``` -dfu-util -D obj/inav_SPARKY.bin --alt 0 -R -s 0x08000000 -``` - -The output should be similar to this: - -``` -dfu-util 0.7 - -Copyright 2005-2008 Weston Schmidt, Harald Welte and OpenMoko Inc. -Copyright 2010-2012 Tormod Volden and Stefan Schmidt -This program is Free Software and has ABSOLUTELY NO WARRANTY -Please report bugs to dfu-util@lists.gnumonks.org - -Opening DFU capable USB device... ID 0483:df11 -Run-time device DFU version 011a -Found DFU: [0483:df11] devnum=0, cfg=1, intf=0, alt=0, name="@Internal Flash /0x08000000/128*0002Kg" -Claiming USB DFU Interface... -Setting Alternate Setting #0 ... -Determining device status: state = dfuERROR, status = 10 -dfuERROR, clearing status -Determining device status: state = dfuIDLE, status = 0 -dfuIDLE, continuing -DFU mode device DFU version 011a -Device returned transfer size 2048 -No valid DFU suffix signature -Warning: File has no DFU suffix -DfuSe interface name: "Internal Flash " -Downloading to address = 0x08000000, size = 76764 -...................................... -File downloaded successfully -can't detach -Resetting USB to switch back to runtime mode - -``` -On Linux you might want to take care that the modemmanager isn't trying to use your sparky as modem getting it into bootloader mode while doing so. In doubt you probably want to uninstall it. It could also be good idea to get udev fixed. It looks like teensy did just that -> http://www.pjrc.com/teensy/49-teensy.rules (untested) - -To make a full chip erase you can use a file created by -``` -dd if=/dev/zero of=zero.bin bs=1 count=262144 -``` -This can be used by dfu-util. - -## Via SWD - -On the bottom of the board there is an SWD header socket onto switch a JST-SH connector can be soldered. -Once you have SWD connected you can use the st-link or j-link tools to flash a binary. - -See Sparky schematic for CONN2 pinouts. - -## TauLabs bootloader - -Flashing INAV will erase the TauLabs bootloader, this is not a problem and can easily be restored using the st flashloader tool. - -# Serial Ports - -| Value | Identifier | RX | TX | Notes | -| ----- | ------------ | --------- | ---------- | -------------------------------------------------------------- | -| 1 | USB VCP | RX (USB) | TX (USB) | | -| 2 | USART1 | RX / PB7 | TX / PB6 | Conn1 / Flexi Port. | -| 3 | USART2 | RX / PA3 | PWM6 / PA2 | On RX is on INPUT header. Best port for Serial RX input | -| 4 | USART3 | RX / PB11 | TX / PB10 | RX/TX is on one end of the 6-pin header about the PWM outputs. | - -USB VCP *can* be used at the same time as other serial port. - -All USART ports all support automatic hardware inversion which allows direct connection of serial rx receivers like the FrSky X4RSB - no external inverter needed. - - -# Battery Monitoring Connections - -| Pin | Signal | Function | -| ---- | ------ | --------------- | -| PWM9 | PA4 | Battery Voltage | -| PWM8 | PA7 | Current Meter | - -## Voltage Monitoring - -The Sparky has no battery divider cricuit, PWM9 has an inline 10k resistor which has to be factored into the resistor calculations. -The divider circuit should eventally create a voltage between 0v and 3.3v (MAX) at the MCU input pin. - -WARNING: Double check the output of your voltage divider using a voltmeter *before* connecting to the FC. - -### Example Circuit - -For a 3Cell battery divider the following circuit works: - -`Battery (+) ---< R1 >--- PWM9 ---< R2 >--- Battery (-)` - -* R1 = 8k2 (Grey Red Red) -* R2 = 2k0 (Red Black Red) - -This gives a 2.2k for an 11.2v battery. The `vbat_scale` for this divider should be set around `52`. - -## Current Monitoring - -Connect a current sensor to PWM8/PA7 that gives a range between 0v and 3.3v out (MAX). From 753829ffdaa5d705df0126684590d34191e77a9c Mon Sep 17 00:00:00 2001 From: "Pawel Spychalski (DzikuVx)" Date: Wed, 14 Apr 2021 11:37:08 +0200 Subject: [PATCH 173/286] Minor F7 optimization --- src/main/fc/fc_core.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/fc/fc_core.c b/src/main/fc/fc_core.c index 40c18fd31f..ec0cb59365 100755 --- a/src/main/fc/fc_core.c +++ b/src/main/fc/fc_core.c @@ -790,7 +790,7 @@ void processRx(timeUs_t currentTimeUs) } // Function for loop trigger -void FAST_CODE NOINLINE taskGyro(timeUs_t currentTimeUs) { +void FAST_CODE taskGyro(timeUs_t currentTimeUs) { // getTaskDeltaTime() returns delta time frozen at the moment of entering the scheduler. currentTime is frozen at the very same point. // To make busy-waiting timeout work we need to account for time spent within busy-waiting loop const timeDelta_t currentDeltaTime = getTaskDeltaTime(TASK_SELF); From 740a51812d406874573158ac0d8f77130bd2c909 Mon Sep 17 00:00:00 2001 From: "Pawel Spychalski (DzikuVx)" Date: Wed, 14 Apr 2021 12:46:16 +0200 Subject: [PATCH 174/286] Always process gyro at 8kHz --- src/main/blackbox/blackbox_io.c | 3 +- src/main/fc/cli.c | 2 +- src/main/fc/config.c | 4 ++ src/main/fc/config.h | 10 +--- src/main/fc/fc_core.c | 1 - src/main/fc/fc_tasks.c | 18 ++++-- src/main/flight/dynamic_gyro_notch.c | 2 +- src/main/flight/servos.c | 2 +- src/main/scheduler/scheduler.h | 3 +- src/main/sensors/gyro.c | 16 +++--- src/test/unit/scheduler_unittest.cc.txt | 74 ++++++++++++------------- 11 files changed, 73 insertions(+), 62 deletions(-) diff --git a/src/main/blackbox/blackbox_io.c b/src/main/blackbox/blackbox_io.c index 1cd0d19f47..fd3b4daaf6 100644 --- a/src/main/blackbox/blackbox_io.c +++ b/src/main/blackbox/blackbox_io.c @@ -42,6 +42,7 @@ #include "msp/msp_serial.h" #include "sensors/gyro.h" +#include "fc/config.h" #define BLACKBOX_SERIAL_PORT_MODE MODE_TX @@ -239,7 +240,7 @@ bool blackboxDeviceOpen(void) * = floor((looptime_ns * 3) / 500.0) * = (looptime_ns * 3) / 500 */ - blackboxMaxHeaderBytesPerIteration = constrain((gyro.targetLooptime * 3) / 500, 1, BLACKBOX_TARGET_HEADER_BUDGET_PER_ITERATION); + blackboxMaxHeaderBytesPerIteration = constrain((getLooptime() * 3) / 500, 1, BLACKBOX_TARGET_HEADER_BUDGET_PER_ITERATION); return blackboxPort != NULL; } diff --git a/src/main/fc/cli.c b/src/main/fc/cli.c index 68f951f687..9e0ec89cd7 100755 --- a/src/main/fc/cli.c +++ b/src/main/fc/cli.c @@ -3234,7 +3234,7 @@ static void cliStatus(char *cmdline) #endif cliPrintf("System load: %d", averageSystemLoadPercent); - const timeDelta_t pidTaskDeltaTime = getTaskDeltaTime(TASK_GYROPID); + const timeDelta_t pidTaskDeltaTime = getTaskDeltaTime(TASK_PID); const int pidRate = pidTaskDeltaTime == 0 ? 0 : (int)(1000000.0f / ((float)pidTaskDeltaTime)); const int rxRate = getTaskDeltaTime(TASK_RX) == 0 ? 0 : (int)(1000000.0f / ((float)getTaskDeltaTime(TASK_RX))); const int systemRate = getTaskDeltaTime(TASK_SYSTEM) == 0 ? 0 : (int)(1000000.0f / ((float)getTaskDeltaTime(TASK_SYSTEM))); diff --git a/src/main/fc/config.c b/src/main/fc/config.c index d38554bd21..feeaf744d3 100755 --- a/src/main/fc/config.c +++ b/src/main/fc/config.c @@ -166,6 +166,10 @@ __attribute__((weak)) void targetConfiguration(void) #endif uint32_t getLooptime(void) { + return gyroConfig()->looptime; +} + +uint32_t getGyroLooptime(void) { return gyro.targetLooptime; } diff --git a/src/main/fc/config.h b/src/main/fc/config.h index 48dc455e71..cc28ea9b3e 100644 --- a/src/main/fc/config.h +++ b/src/main/fc/config.h @@ -29,13 +29,8 @@ #define ONESHOT_FEATURE_CHANGED_DELAY_ON_BOOT_MS 1500 #define MAX_NAME_LENGTH 16 -typedef enum { - ASYNC_MODE_NONE, - ASYNC_MODE_GYRO, - ASYNC_MODE_ALL -} asyncMode_e; - -typedef enum { +#define TASK_GYRO_LOOPTIME 125 // Task gyro always runs at 8kHz + typedef enum { FEATURE_THR_VBAT_COMP = 1 << 0, FEATURE_VBAT = 1 << 1, FEATURE_TX_PROF_SEL = 1 << 2, // Profile selection by TX stick command @@ -140,3 +135,4 @@ void resetConfigs(void); void targetConfiguration(void); uint32_t getLooptime(void); +uint32_t getGyroLooptime(void); diff --git a/src/main/fc/fc_core.c b/src/main/fc/fc_core.c index 40c18fd31f..9523d6ad63 100755 --- a/src/main/fc/fc_core.c +++ b/src/main/fc/fc_core.c @@ -845,7 +845,6 @@ void taskMainPidLoop(timeUs_t currentTimeUs) updateAccExtremes(); } - taskGyro(currentTimeUs); imuUpdateAccelerometer(); imuUpdateAttitude(currentTimeUs); diff --git a/src/main/fc/fc_tasks.c b/src/main/fc/fc_tasks.c index 33d417a1d6..5c71e7c9de 100755 --- a/src/main/fc/fc_tasks.c +++ b/src/main/fc/fc_tasks.c @@ -301,8 +301,12 @@ void fcTasksInit(void) { schedulerInit(); - rescheduleTask(TASK_GYROPID, getLooptime()); - setTaskEnabled(TASK_GYROPID, true); + rescheduleTask(TASK_PID, getLooptime()); + setTaskEnabled(TASK_PID, true); + + rescheduleTask(TASK_GYRO, getGyroLooptime()); + setTaskEnabled(TASK_GYRO, true); + setTaskEnabled(TASK_AUX, true); setTaskEnabled(TASK_SERIAL, true); @@ -388,12 +392,18 @@ cfTask_t cfTasks[TASK_COUNT] = { .desiredPeriod = TASK_PERIOD_HZ(10), // run every 100 ms, 10Hz .staticPriority = TASK_PRIORITY_HIGH, }, - [TASK_GYROPID] = { - .taskName = "GYRO/PID", + [TASK_PID] = { + .taskName = "PID", .taskFunc = taskMainPidLoop, .desiredPeriod = TASK_PERIOD_US(1000), .staticPriority = TASK_PRIORITY_REALTIME, }, + [TASK_GYRO] = { + .taskName = "GYRO", + .taskFunc = taskGyro, + .desiredPeriod = TASK_PERIOD_US(TASK_GYRO_LOOPTIME), + .staticPriority = TASK_PRIORITY_REALTIME, + }, [TASK_SERIAL] = { .taskName = "SERIAL", .taskFunc = taskHandleSerial, diff --git a/src/main/flight/dynamic_gyro_notch.c b/src/main/flight/dynamic_gyro_notch.c index 7e6c12afbc..46d7dd696f 100644 --- a/src/main/flight/dynamic_gyro_notch.c +++ b/src/main/flight/dynamic_gyro_notch.c @@ -37,7 +37,7 @@ void dynamicGyroNotchFiltersInit(dynamicGyroNotchState_t *state) { state->dynNotchQ = gyroConfig()->dynamicGyroNotchQ / 100.0f; state->enabled = gyroConfig()->dynamicGyroNotchEnabled; - state->looptime = getLooptime(); + state->looptime = getGyroLooptime(); if (state->enabled) { /* diff --git a/src/main/flight/servos.c b/src/main/flight/servos.c index df58e6f649..f2033b80ff 100755 --- a/src/main/flight/servos.c +++ b/src/main/flight/servos.c @@ -196,7 +196,7 @@ static void filterServos(void) // Initialize servo lowpass filter (servos are calculated at looptime rate) if (!servoFilterIsSet) { for (int i = 0; i < MAX_SUPPORTED_SERVOS; i++) { - biquadFilterInitLPF(&servoFilter[i], servoConfig()->servo_lowpass_freq, gyro.targetLooptime); + biquadFilterInitLPF(&servoFilter[i], servoConfig()->servo_lowpass_freq, getLooptime()); biquadFilterReset(&servoFilter[i], servo[i]); } servoFilterIsSet = true; diff --git a/src/main/scheduler/scheduler.h b/src/main/scheduler/scheduler.h index 0ad456967c..ff4c429299 100755 --- a/src/main/scheduler/scheduler.h +++ b/src/main/scheduler/scheduler.h @@ -51,7 +51,8 @@ typedef struct { typedef enum { /* Actual tasks */ TASK_SYSTEM = 0, - TASK_GYROPID, + TASK_PID, + TASK_GYRO, TASK_RX, TASK_SERIAL, TASK_BATTERY, diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c index 6a8710bff2..6af0ba20e6 100644 --- a/src/main/sensors/gyro.c +++ b/src/main/sensors/gyro.c @@ -257,13 +257,13 @@ static void initGyroFilter(filterApplyFnPtr *applyFn, filter_t state[], uint8_t case FILTER_PT1: *applyFn = (filterApplyFnPtr)pt1FilterApply; for (int axis = 0; axis < 3; axis++) { - pt1FilterInit(&state[axis].pt1, cutoff, getLooptime()* 1e-6f); + pt1FilterInit(&state[axis].pt1, cutoff, getGyroLooptime()* 1e-6f); } break; case FILTER_BIQUAD: *applyFn = (filterApplyFnPtr)biquadFilterApply; for (int axis = 0; axis < 3; axis++) { - biquadFilterInitLPF(&state[axis].biquad, cutoff, getLooptime()); + biquadFilterInitLPF(&state[axis].biquad, cutoff, getGyroLooptime()); } break; } @@ -282,7 +282,7 @@ static void gyroInitFilters(void) notchFilter1ApplyFn = (filterApplyFnPtr)biquadFilterApply; for (int axis = 0; axis < 3; axis++) { notchFilter1[axis] = &gyroFilterNotch_1[axis]; - biquadFilterInitNotch(notchFilter1[axis], getLooptime(), gyroConfig()->gyro_notch_hz, gyroConfig()->gyro_notch_cutoff); + biquadFilterInitNotch(notchFilter1[axis], getGyroLooptime(), gyroConfig()->gyro_notch_hz, gyroConfig()->gyro_notch_cutoff); } } } @@ -313,12 +313,12 @@ bool gyroInit(void) // Driver initialisation gyroDev[0].lpf = gyroConfig()->gyro_lpf; - gyroDev[0].requestedSampleIntervalUs = gyroConfig()->looptime; - gyroDev[0].sampleRateIntervalUs = gyroConfig()->looptime; + gyroDev[0].requestedSampleIntervalUs = TASK_GYRO_LOOPTIME; + gyroDev[0].sampleRateIntervalUs = TASK_GYRO_LOOPTIME; gyroDev[0].initFn(&gyroDev[0]); // initFn will initialize sampleRateIntervalUs to actual gyro sampling rate (if driver supports it). Calculate target looptime using that value - gyro.targetLooptime = gyroConfig()->gyroSync ? gyroDev[0].sampleRateIntervalUs : gyroConfig()->looptime; + gyro.targetLooptime = gyroConfig()->gyroSync ? gyroDev[0].sampleRateIntervalUs : TASK_GYRO_LOOPTIME; // At this poinrt gyroDev[0].gyroAlign was set up by the driver from the busDev record // If configuration says different - override @@ -333,7 +333,7 @@ bool gyroInit(void) &gyroAnalyseState, gyroConfig()->dynamicGyroNotchMinHz, gyroConfig()->dynamicGyroNotchRange, - getLooptime() + getGyroLooptime() ); #endif return true; @@ -538,7 +538,7 @@ void gyroUpdateDynamicLpf(float cutoffFreq) { } } else if (gyroConfig()->gyro_soft_lpf_type == FILTER_BIQUAD) { for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { - biquadFilterUpdate(&gyroLpfState[axis].biquad, cutoffFreq, getLooptime(), BIQUAD_Q, FILTER_LPF); + biquadFilterUpdate(&gyroLpfState[axis].biquad, cutoffFreq, getGyroLooptime(), BIQUAD_Q, FILTER_LPF); } } } diff --git a/src/test/unit/scheduler_unittest.cc.txt b/src/test/unit/scheduler_unittest.cc.txt index 96191469dd..ff8c4cdb0e 100644 --- a/src/test/unit/scheduler_unittest.cc.txt +++ b/src/test/unit/scheduler_unittest.cc.txt @@ -88,7 +88,7 @@ TEST(SchedulerUnittest, TestPriorites) EXPECT_EQ(14, TASK_COUNT); // if any of these fail then task priorities have changed and ordering in TestQueue needs to be re-checked EXPECT_EQ(TASK_PRIORITY_HIGH, cfTasks[TASK_SYSTEM].staticPriority); - EXPECT_EQ(TASK_PRIORITY_REALTIME, cfTasks[TASK_GYROPID].staticPriority); + EXPECT_EQ(TASK_PRIORITY_REALTIME, cfTasks[TASK_PID].staticPriority); EXPECT_EQ(TASK_PRIORITY_MEDIUM, cfTasks[TASK_ACCEL].staticPriority); EXPECT_EQ(TASK_PRIORITY_LOW, cfTasks[TASK_SERIAL].staticPriority); EXPECT_EQ(TASK_PRIORITY_MEDIUM, cfTasks[TASK_BATTERY].staticPriority); @@ -117,16 +117,16 @@ TEST(SchedulerUnittest, TestQueue) EXPECT_EQ(&cfTasks[TASK_SYSTEM], queueFirst()); EXPECT_EQ(deadBeefPtr, taskQueueArray[TASK_COUNT + 1]); - queueAdd(&cfTasks[TASK_GYROPID]); // TASK_PRIORITY_REALTIME + queueAdd(&cfTasks[TASK_PID]); // TASK_PRIORITY_REALTIME EXPECT_EQ(2, queueSize()); - EXPECT_EQ(&cfTasks[TASK_GYROPID], queueFirst()); + EXPECT_EQ(&cfTasks[TASK_PID], queueFirst()); EXPECT_EQ(&cfTasks[TASK_SYSTEM], queueNext()); EXPECT_EQ(NULL, queueNext()); EXPECT_EQ(deadBeefPtr, taskQueueArray[TASK_COUNT + 1]); queueAdd(&cfTasks[TASK_SERIAL]); // TASK_PRIORITY_LOW EXPECT_EQ(3, queueSize()); - EXPECT_EQ(&cfTasks[TASK_GYROPID], queueFirst()); + EXPECT_EQ(&cfTasks[TASK_PID], queueFirst()); EXPECT_EQ(&cfTasks[TASK_SYSTEM], queueNext()); EXPECT_EQ(&cfTasks[TASK_SERIAL], queueNext()); EXPECT_EQ(NULL, queueNext()); @@ -134,7 +134,7 @@ TEST(SchedulerUnittest, TestQueue) queueAdd(&cfTasks[TASK_BEEPER]); // TASK_PRIORITY_MEDIUM EXPECT_EQ(4, queueSize()); - EXPECT_EQ(&cfTasks[TASK_GYROPID], queueFirst()); + EXPECT_EQ(&cfTasks[TASK_PID], queueFirst()); EXPECT_EQ(&cfTasks[TASK_SYSTEM], queueNext()); EXPECT_EQ(&cfTasks[TASK_BEEPER], queueNext()); EXPECT_EQ(&cfTasks[TASK_SERIAL], queueNext()); @@ -143,7 +143,7 @@ TEST(SchedulerUnittest, TestQueue) queueAdd(&cfTasks[TASK_RX]); // TASK_PRIORITY_HIGH EXPECT_EQ(5, queueSize()); - EXPECT_EQ(&cfTasks[TASK_GYROPID], queueFirst()); + EXPECT_EQ(&cfTasks[TASK_PID], queueFirst()); EXPECT_EQ(&cfTasks[TASK_SYSTEM], queueNext()); EXPECT_EQ(&cfTasks[TASK_RX], queueNext()); EXPECT_EQ(&cfTasks[TASK_BEEPER], queueNext()); @@ -153,7 +153,7 @@ TEST(SchedulerUnittest, TestQueue) queueRemove(&cfTasks[TASK_SYSTEM]); // TASK_PRIORITY_HIGH EXPECT_EQ(4, queueSize()); - EXPECT_EQ(&cfTasks[TASK_GYROPID], queueFirst()); + EXPECT_EQ(&cfTasks[TASK_PID], queueFirst()); EXPECT_EQ(&cfTasks[TASK_RX], queueNext()); EXPECT_EQ(&cfTasks[TASK_BEEPER], queueNext()); EXPECT_EQ(&cfTasks[TASK_SERIAL], queueNext()); @@ -275,38 +275,38 @@ TEST(SchedulerUnittest, TestScheduleEmptyQueue) TEST(SchedulerUnittest, TestSingleTask) { schedulerInit(); - // disable all tasks except TASK_GYROPID + // disable all tasks except TASK_PID for (int taskId=0; taskId < TASK_COUNT; ++taskId) { setTaskEnabled(static_cast(taskId), false); } - setTaskEnabled(TASK_GYROPID, true); - cfTasks[TASK_GYROPID].lastExecutedAt = 1000; + setTaskEnabled(TASK_PID, true); + cfTasks[TASK_PID].lastExecutedAt = 1000; simulatedTime = 4000; // run the scheduler and check the task has executed scheduler(); EXPECT_NE(static_cast(0), unittest_scheduler_selectedTask); - EXPECT_EQ(&cfTasks[TASK_GYROPID], unittest_scheduler_selectedTask); - EXPECT_EQ(3000, cfTasks[TASK_GYROPID].taskLatestDeltaTime); - EXPECT_EQ(4000, cfTasks[TASK_GYROPID].lastExecutedAt); - EXPECT_EQ(pidLoopCheckerTime, cfTasks[TASK_GYROPID].totalExecutionTime); + EXPECT_EQ(&cfTasks[TASK_PID], unittest_scheduler_selectedTask); + EXPECT_EQ(3000, cfTasks[TASK_PID].taskLatestDeltaTime); + EXPECT_EQ(4000, cfTasks[TASK_PID].lastExecutedAt); + EXPECT_EQ(pidLoopCheckerTime, cfTasks[TASK_PID].totalExecutionTime); // task has run, so its dynamic priority should have been set to zero - EXPECT_EQ(0, cfTasks[TASK_GYROPID].dynamicPriority); + EXPECT_EQ(0, cfTasks[TASK_PID].dynamicPriority); } TEST(SchedulerUnittest, TestTwoTasks) { - // disable all tasks except TASK_GYROPID and TASK_ACCEL + // disable all tasks except TASK_PID and TASK_ACCEL for (int taskId=0; taskId < TASK_COUNT; ++taskId) { setTaskEnabled(static_cast(taskId), false); } setTaskEnabled(TASK_ACCEL, true); - setTaskEnabled(TASK_GYROPID, true); + setTaskEnabled(TASK_PID, true); - // set it up so that TASK_ACCEL ran just before TASK_GYROPID + // set it up so that TASK_ACCEL ran just before TASK_PID static const uint32_t startTime = 4000; simulatedTime = startTime; - cfTasks[TASK_GYROPID].lastExecutedAt = simulatedTime; - cfTasks[TASK_ACCEL].lastExecutedAt = cfTasks[TASK_GYROPID].lastExecutedAt - updateAccelerometerTime; + cfTasks[TASK_PID].lastExecutedAt = simulatedTime; + cfTasks[TASK_ACCEL].lastExecutedAt = cfTasks[TASK_PID].lastExecutedAt - updateAccelerometerTime; EXPECT_EQ(0, cfTasks[TASK_ACCEL].taskAgeCycles); // run the scheduler scheduler(); @@ -314,7 +314,7 @@ TEST(SchedulerUnittest, TestTwoTasks) EXPECT_EQ(static_cast(0), unittest_scheduler_selectedTask); // NOTE: - // TASK_GYROPID desiredPeriod is 1000 microseconds + // TASK_PID desiredPeriod is 1000 microseconds // TASK_ACCEL desiredPeriod is 10000 microseconds // 500 microseconds later simulatedTime += 500; @@ -323,27 +323,27 @@ TEST(SchedulerUnittest, TestTwoTasks) EXPECT_EQ(static_cast(0), unittest_scheduler_selectedTask); EXPECT_EQ(0, unittest_scheduler_waitingTasks); - // 500 microseconds later, TASK_GYROPID desiredPeriod has elapsed + // 500 microseconds later, TASK_PID desiredPeriod has elapsed simulatedTime += 500; - // TASK_GYROPID should now run + // TASK_PID should now run scheduler(); - EXPECT_EQ(&cfTasks[TASK_GYROPID], unittest_scheduler_selectedTask); + EXPECT_EQ(&cfTasks[TASK_PID], unittest_scheduler_selectedTask); EXPECT_EQ(1, unittest_scheduler_waitingTasks); EXPECT_EQ(5000 + pidLoopCheckerTime, simulatedTime); simulatedTime += 1000 - pidLoopCheckerTime; scheduler(); - // TASK_GYROPID should run again - EXPECT_EQ(&cfTasks[TASK_GYROPID], unittest_scheduler_selectedTask); + // TASK_PID should run again + EXPECT_EQ(&cfTasks[TASK_PID], unittest_scheduler_selectedTask); scheduler(); EXPECT_EQ(static_cast(0), unittest_scheduler_selectedTask); EXPECT_EQ(0, unittest_scheduler_waitingTasks); - simulatedTime = startTime + 10500; // TASK_GYROPID and TASK_ACCEL desiredPeriods have elapsed - // of the two TASK_GYROPID should run first + simulatedTime = startTime + 10500; // TASK_PID and TASK_ACCEL desiredPeriods have elapsed + // of the two TASK_PID should run first scheduler(); - EXPECT_EQ(&cfTasks[TASK_GYROPID], unittest_scheduler_selectedTask); + EXPECT_EQ(&cfTasks[TASK_PID], unittest_scheduler_selectedTask); // and finally TASK_ACCEL should now run scheduler(); EXPECT_EQ(&cfTasks[TASK_ACCEL], unittest_scheduler_selectedTask); @@ -351,12 +351,12 @@ TEST(SchedulerUnittest, TestTwoTasks) TEST(SchedulerUnittest, TestRealTimeGuardInNoTaskRun) { - // disable all tasks except TASK_GYROPID and TASK_SYSTEM + // disable all tasks except TASK_PID and TASK_SYSTEM for (int taskId=0; taskId < TASK_COUNT; ++taskId) { setTaskEnabled(static_cast(taskId), false); } - setTaskEnabled(TASK_GYROPID, true); - cfTasks[TASK_GYROPID].lastExecutedAt = 200000; + setTaskEnabled(TASK_PID, true); + cfTasks[TASK_PID].lastExecutedAt = 200000; simulatedTime = 200700; setTaskEnabled(TASK_SYSTEM, true); @@ -371,17 +371,17 @@ TEST(SchedulerUnittest, TestRealTimeGuardInNoTaskRun) EXPECT_EQ(NULL, unittest_scheduler_selectedTask); EXPECT_EQ(100000, cfTasks[TASK_SYSTEM].lastExecutedAt); - EXPECT_EQ(200000, cfTasks[TASK_GYROPID].lastExecutedAt); + EXPECT_EQ(200000, cfTasks[TASK_PID].lastExecutedAt); } TEST(SchedulerUnittest, TestRealTimeGuardOutTaskRun) { - // disable all tasks except TASK_GYROPID and TASK_SYSTEM + // disable all tasks except TASK_PID and TASK_SYSTEM for (int taskId=0; taskId < TASK_COUNT; ++taskId) { setTaskEnabled(static_cast(taskId), false); } - setTaskEnabled(TASK_GYROPID, true); - cfTasks[TASK_GYROPID].lastExecutedAt = 200000; + setTaskEnabled(TASK_PID, true); + cfTasks[TASK_PID].lastExecutedAt = 200000; simulatedTime = 200699; setTaskEnabled(TASK_SYSTEM, true); @@ -396,7 +396,7 @@ TEST(SchedulerUnittest, TestRealTimeGuardOutTaskRun) EXPECT_EQ(&cfTasks[TASK_SYSTEM], unittest_scheduler_selectedTask); EXPECT_EQ(200699, cfTasks[TASK_SYSTEM].lastExecutedAt); - EXPECT_EQ(200000, cfTasks[TASK_GYROPID].lastExecutedAt); + EXPECT_EQ(200000, cfTasks[TASK_PID].lastExecutedAt); } // STUBS From c8992101e6811a09f8c4f122c718efa14cc3e6d8 Mon Sep 17 00:00:00 2001 From: "Pawel Spychalski (DzikuVx)" Date: Wed, 14 Apr 2021 13:07:24 +0200 Subject: [PATCH 175/286] Fix unit tests --- src/test/unit/sensor_gyro_unittest.cc | 1 + 1 file changed, 1 insertion(+) diff --git a/src/test/unit/sensor_gyro_unittest.cc b/src/test/unit/sensor_gyro_unittest.cc index 0433b88939..f80ecf7db3 100644 --- a/src/test/unit/sensor_gyro_unittest.cc +++ b/src/test/unit/sensor_gyro_unittest.cc @@ -143,6 +143,7 @@ uint32_t micros(void) {return 0;} void beeper(beeperMode_e) {} uint8_t detectedSensors[] = { GYRO_NONE, ACC_NONE }; timeDelta_t getLooptime(void) {return gyro.targetLooptime;} +timeDelta_t getGyroLooptime(void) {return gyro.targetLooptime;} void sensorsSet(uint32_t) {} void schedulerResetTaskStatistics(cfTaskId_e) {} } From 34c91848fc984dbc7486a27f76eaf46c60ba86b9 Mon Sep 17 00:00:00 2001 From: Michel Pastor Date: Wed, 14 Apr 2021 16:47:20 +0200 Subject: [PATCH 176/286] Fix error handling in settings.rb:Generator#resolve_types (#6830) --- src/utils/settings.rb | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/utils/settings.rb b/src/utils/settings.rb index acae823773..2f91a49f44 100755 --- a/src/utils/settings.rb +++ b/src/utils/settings.rb @@ -988,7 +988,7 @@ class Generator member["max"] = $1.to_i - 1; typ = "string" else - raise "Unknown type #{m[1]} when resolving type for setting #{member["name"]}" + raise "Unknown type #{type} when resolving type for setting #{member["name"]}" end dputs "#{member["name"]} type is #{typ}" member["type"] = typ From 7b50b059f9ec880c6f4ac3624627315ff1992f5d Mon Sep 17 00:00:00 2001 From: Michel Pastor Date: Wed, 14 Apr 2021 19:49:30 +0200 Subject: [PATCH 177/286] Settings generator: add support for the native C type: bool (#6831) --- src/utils/settings.rb | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/utils/settings.rb b/src/utils/settings.rb index 2f91a49f44..9b4e48336c 100755 --- a/src/utils/settings.rb +++ b/src/utils/settings.rb @@ -971,6 +971,8 @@ class Generator types.each do |idx, type| member = members[idx] case type + when /^bool/ + typ = "bool" when /^int8_t/ # {aka signed char}" typ = "int8_t" when /^uint8_t/ # {aka unsigned char}" From 5afa12307f995599ba3c3501c8404804e64151c0 Mon Sep 17 00:00:00 2001 From: OptimusTi Date: Wed, 14 Apr 2021 18:51:12 -0400 Subject: [PATCH 178/286] Fix for stats page no.1 Adding check for serial provider. This fixes issue with crsf stats and opens room for other protocol stats. --- src/main/io/osd.c | 39 +++++++++++++++++++++------------------ 1 file changed, 21 insertions(+), 18 deletions(-) diff --git a/src/main/io/osd.c b/src/main/io/osd.c index ab7c286d3f..4a6afae64c 100644 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -968,7 +968,7 @@ static bool osdIsHeadingValid(void) } else { return isImuHeadingValid(); } -#else +#else return isImuHeadingValid(); #endif } @@ -981,7 +981,7 @@ int16_t osdGetHeading(void) } else { return attitude.values.yaw; } -#else +#else return attitude.values.yaw; #endif } @@ -1925,7 +1925,7 @@ static bool osdDrawSingleElement(uint8_t item) pitchAngle = DECIDEGREES_TO_RADIANS(secondaryImuState.eulerAngles.values.pitch); } else { rollAngle = DECIDEGREES_TO_RADIANS(attitude.values.roll); - pitchAngle = DECIDEGREES_TO_RADIANS(attitude.values.pitch); + pitchAngle = DECIDEGREES_TO_RADIANS(attitude.values.pitch); } #else rollAngle = DECIDEGREES_TO_RADIANS(attitude.values.roll); @@ -3123,22 +3123,25 @@ static void osdShowStatsPage1(void) osdFormatAltitudeStr(buff, stats.max_altitude); displayWrite(osdDisplayPort, statValuesX, top++, buff); -#if defined(USE_SERIALRX_CRSF) - displayWrite(osdDisplayPort, statNameX, top, "MIN LQ :"); - itoa(stats.min_lq, buff, 10); - strcat(buff, "%"); - displayWrite(osdDisplayPort, statValuesX, top++, buff); +switch (rxConfig()->serialrx_provider) { + case SERIALRX_CRSF: + displayWrite(osdDisplayPort, statNameX, top, "MIN LQ :"); + itoa(stats.min_lq, buff, 10); + strcat(buff, "%"); + displayWrite(osdDisplayPort, statValuesX, top++, buff); - displayWrite(osdDisplayPort, statNameX, top, "MIN RSSI :"); - itoa(stats.min_rssi_dbm, buff, 10); - tfp_sprintf(buff, "%s%c", buff, SYM_DBM); - displayWrite(osdDisplayPort, statValuesX, top++, buff); -#else - displayWrite(osdDisplayPort, statNameX, top, "MIN RSSI :"); - itoa(stats.min_rssi, buff, 10); - strcat(buff, "%"); - displayWrite(osdDisplayPort, statValuesX, top++, buff); -#endif + displayWrite(osdDisplayPort, statNameX, top, "MIN RSSI :"); + itoa(stats.min_rssi_dbm, buff, 10); + tfp_sprintf(buff, "%s%c", buff, SYM_DBM); + displayWrite(osdDisplayPort, statValuesX, top++, buff); + break; + default: + displayWrite(osdDisplayPort, statNameX, top, "MIN RSSI :"); + itoa(stats.min_rssi, buff, 10); + strcat(buff, "%"); + displayWrite(osdDisplayPort, statValuesX, top++, buff); + break; +} displayWrite(osdDisplayPort, statNameX, top, "FLY TIME :"); uint16_t flySeconds = getFlightTime(); From 0356952c310e4498f038c568056a2753f78b2479 Mon Sep 17 00:00:00 2001 From: Michel Pastor Date: Thu, 15 Apr 2021 15:13:58 +0200 Subject: [PATCH 179/286] Revert c853269e7 as it is causing issues with the CLI (#6837) * Settings generator: add support for the native C type: bool * Revert "Settings generator: add support for the native C type: bool" This reverts commit c853269e735aff0ab55c584f691b1160f4b3ce05. From ee71cdf967d08a2b60566a35adbd12c830298539 Mon Sep 17 00:00:00 2001 From: OptimusTi Date: Thu, 15 Apr 2021 14:48:57 -0400 Subject: [PATCH 180/286] formatting and indicator arrows to let user know to use roll right or left --- src/main/io/osd.c | 39 +++++++++++++++++++-------------------- 1 file changed, 19 insertions(+), 20 deletions(-) diff --git a/src/main/io/osd.c b/src/main/io/osd.c index 4a6afae64c..7130cf03bc 100644 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -3102,7 +3102,7 @@ static void osdShowStatsPage1(void) displayBeginTransaction(osdDisplayPort, DISPLAY_TRANSACTION_OPT_RESET_DRAWING); displayClearScreen(osdDisplayPort); - displayWrite(osdDisplayPort, statNameX, top++, "--- STATS --- 1/2"); + displayWrite(osdDisplayPort, statNameX, top++, "--- STATS --- 1/2 ->"); if (feature(FEATURE_GPS)) { displayWrite(osdDisplayPort, statNameX, top, "MAX SPEED :"); @@ -3123,25 +3123,24 @@ static void osdShowStatsPage1(void) osdFormatAltitudeStr(buff, stats.max_altitude); displayWrite(osdDisplayPort, statValuesX, top++, buff); -switch (rxConfig()->serialrx_provider) { - case SERIALRX_CRSF: - displayWrite(osdDisplayPort, statNameX, top, "MIN LQ :"); - itoa(stats.min_lq, buff, 10); - strcat(buff, "%"); - displayWrite(osdDisplayPort, statValuesX, top++, buff); + switch (rxConfig()->serialrx_provider) { + case SERIALRX_CRSF: + displayWrite(osdDisplayPort, statNameX, top, "MIN LQ :"); + itoa(stats.min_lq, buff, 10); + strcat(buff, "%"); + displayWrite(osdDisplayPort, statValuesX, top++, buff); - displayWrite(osdDisplayPort, statNameX, top, "MIN RSSI :"); - itoa(stats.min_rssi_dbm, buff, 10); - tfp_sprintf(buff, "%s%c", buff, SYM_DBM); - displayWrite(osdDisplayPort, statValuesX, top++, buff); - break; - default: - displayWrite(osdDisplayPort, statNameX, top, "MIN RSSI :"); - itoa(stats.min_rssi, buff, 10); - strcat(buff, "%"); - displayWrite(osdDisplayPort, statValuesX, top++, buff); - break; -} + displayWrite(osdDisplayPort, statNameX, top, "MIN RSSI :"); + itoa(stats.min_rssi_dbm, buff, 10); + tfp_sprintf(buff, "%s%c", buff, SYM_DBM); + displayWrite(osdDisplayPort, statValuesX, top++, buff); + break; + default: + displayWrite(osdDisplayPort, statNameX, top, "MIN RSSI :"); + itoa(stats.min_rssi, buff, 10); + strcat(buff, "%"); + displayWrite(osdDisplayPort, statValuesX, top++, buff); + } displayWrite(osdDisplayPort, statNameX, top, "FLY TIME :"); uint16_t flySeconds = getFlightTime(); @@ -3168,7 +3167,7 @@ static void osdShowStatsPage2(void) displayBeginTransaction(osdDisplayPort, DISPLAY_TRANSACTION_OPT_RESET_DRAWING); displayClearScreen(osdDisplayPort); - displayWrite(osdDisplayPort, statNameX, top++, "--- STATS --- 2/2"); + displayWrite(osdDisplayPort, statNameX, top++, "--- STATS --- <- 2/2"); displayWrite(osdDisplayPort, statNameX, top, "MIN BATTERY VOLT :"); osdFormatCentiNumber(buff, stats.min_voltage, 0, osdConfig()->main_voltage_decimals, 0, osdConfig()->main_voltage_decimals + 2); From 0006471e074685fa5721ac85ed3bb3358c29cd5e Mon Sep 17 00:00:00 2001 From: "Pawel Spychalski (DzikuVx)" Date: Fri, 16 Apr 2021 18:56:09 +0200 Subject: [PATCH 181/286] Fix DSHOT beeper reset template --- src/main/fc/config.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/fc/config.c b/src/main/fc/config.c index 46ddb52ecd..46ed815100 100755 --- a/src/main/fc/config.c +++ b/src/main/fc/config.c @@ -123,7 +123,7 @@ PG_RESET_TEMPLATE(systemConfig_t, systemConfig, .name = SETTING_NAME_DEFAULT ); -PG_REGISTER(beeperConfig_t, beeperConfig, PG_BEEPER_CONFIG, 1); +PG_REGISTER_WITH_RESET_TEMPLATE(beeperConfig_t, beeperConfig, PG_BEEPER_CONFIG, 1); PG_RESET_TEMPLATE(beeperConfig_t, beeperConfig, .beeper_off_flags = 0, From f88de74dcff17aaa1b0887db71ba0938d1b58d61 Mon Sep 17 00:00:00 2001 From: breadoven <56191411+breadoven@users.noreply.github.com> Date: Sat, 17 Apr 2021 09:29:52 +0100 Subject: [PATCH 182/286] Update osd.c --- src/main/io/osd.c | 10 ++++++---- 1 file changed, 6 insertions(+), 4 deletions(-) diff --git a/src/main/io/osd.c b/src/main/io/osd.c index 3cc6898cc0..088de50727 100644 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -970,7 +970,7 @@ static bool osdIsHeadingValid(void) } else { return isImuHeadingValid(); } -#else +#else return isImuHeadingValid(); #endif } @@ -983,7 +983,7 @@ int16_t osdGetHeading(void) } else { return attitude.values.yaw; } -#else +#else return attitude.values.yaw; #endif } @@ -1927,7 +1927,7 @@ static bool osdDrawSingleElement(uint8_t item) pitchAngle = DECIDEGREES_TO_RADIANS(secondaryImuState.eulerAngles.values.pitch); } else { rollAngle = DECIDEGREES_TO_RADIANS(attitude.values.roll); - pitchAngle = DECIDEGREES_TO_RADIANS(attitude.values.pitch); + pitchAngle = DECIDEGREES_TO_RADIANS(attitude.values.pitch); } #else rollAngle = DECIDEGREES_TO_RADIANS(attitude.values.roll); @@ -3584,7 +3584,9 @@ textAttributes_t osdGetSystemMessage(char *buff, size_t buff_size, bool isCenter if (FLIGHT_MODE(NAV_RTH_MODE) || FLIGHT_MODE(NAV_WP_MODE) || navigationIsExecutingAnEmergencyLanding()) { if (NAV_Status.state == MW_NAV_STATE_WP_ENROUTE) { // Countdown display for remaining Waypoints - tfp_sprintf(messageBuf, "TO WP %u/%u", posControl.activeWaypointIndex + 1, posControl.waypointCount); + char buf[6]; + osdFormatDistanceSymbol(buf, posControl.wpDistance, 0); + tfp_sprintf(messageBuf, "TO WP %u/%u (%s)", posControl.activeWaypointIndex + 1, posControl.waypointCount, buf); messages[messageCount++] = messageBuf; } else if (NAV_Status.state == MW_NAV_STATE_HOLD_TIMED) { // WP hold time countdown in seconds From fa9184bc138aa47ac1fc51a6b399d202052cdbfb Mon Sep 17 00:00:00 2001 From: luca Date: Sat, 17 Apr 2021 17:54:52 +0200 Subject: [PATCH 183/286] Turtle mode bugfix --- src/main/fc/fc_core.c | 29 +++++++++++++++-------------- 1 file changed, 15 insertions(+), 14 deletions(-) diff --git a/src/main/fc/fc_core.c b/src/main/fc/fc_core.c index 0d08114801..f4b1479143 100755 --- a/src/main/fc/fc_core.c +++ b/src/main/fc/fc_core.c @@ -497,20 +497,6 @@ void tryArm(void) { updateArmingStatus(); -#ifdef USE_DSHOT - if ( - STATE(MULTIROTOR) && - IS_RC_MODE_ACTIVE(BOXFLIPOVERAFTERCRASH) && - emergencyArmingCanOverrideArmingDisabled() && - isMotorProtocolDshot() && - !FLIGHT_MODE(FLIP_OVER_AFTER_CRASH) - ) { - sendDShotCommand(DSHOT_CMD_SPIN_DIRECTION_REVERSED); - ENABLE_ARMING_FLAG(ARMED); - enableFlightMode(FLIP_OVER_AFTER_CRASH); - return; - } -#endif #ifdef USE_PROGRAMMING_FRAMEWORK if ( !isArmingDisabled() || @@ -527,6 +513,21 @@ void tryArm(void) return; } +#ifdef USE_DSHOT + if ( + STATE(MULTIROTOR) && + IS_RC_MODE_ACTIVE(BOXFLIPOVERAFTERCRASH) && + emergencyArmingCanOverrideArmingDisabled() && + isMotorProtocolDshot() && + !FLIGHT_MODE(FLIP_OVER_AFTER_CRASH) + ) { + sendDShotCommand(DSHOT_CMD_SPIN_DIRECTION_REVERSED); + ENABLE_ARMING_FLAG(ARMED); + enableFlightMode(FLIP_OVER_AFTER_CRASH); + return; + } +#endif + #if defined(USE_NAV) // If nav_extra_arming_safety was bypassed we always // allow bypassing it even without the sticks set From d50ae85e99499565ed328838410e3cf8668c5636 Mon Sep 17 00:00:00 2001 From: "Pawel Spychalski (DzikuVx)" Date: Sun, 18 Apr 2021 13:10:44 +0200 Subject: [PATCH 184/286] Run most filters with PID not gyro --- src/main/fc/fc_core.c | 2 + src/main/flight/dynamic_gyro_notch.c | 2 +- src/main/sensors/gyro.c | 59 +++++++++++++++++++--------- src/main/sensors/gyro.h | 1 + 4 files changed, 45 insertions(+), 19 deletions(-) diff --git a/src/main/fc/fc_core.c b/src/main/fc/fc_core.c index 9523d6ad63..eef3351e1f 100755 --- a/src/main/fc/fc_core.c +++ b/src/main/fc/fc_core.c @@ -845,6 +845,8 @@ void taskMainPidLoop(timeUs_t currentTimeUs) updateAccExtremes(); } + gyroFilter(); + imuUpdateAccelerometer(); imuUpdateAttitude(currentTimeUs); diff --git a/src/main/flight/dynamic_gyro_notch.c b/src/main/flight/dynamic_gyro_notch.c index 46d7dd696f..7e6c12afbc 100644 --- a/src/main/flight/dynamic_gyro_notch.c +++ b/src/main/flight/dynamic_gyro_notch.c @@ -37,7 +37,7 @@ void dynamicGyroNotchFiltersInit(dynamicGyroNotchState_t *state) { state->dynNotchQ = gyroConfig()->dynamicGyroNotchQ / 100.0f; state->enabled = gyroConfig()->dynamicGyroNotchEnabled; - state->looptime = getGyroLooptime(); + state->looptime = getLooptime(); if (state->enabled) { /* diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c index 6af0ba20e6..c2dc1c99bb 100644 --- a/src/main/sensors/gyro.c +++ b/src/main/sensors/gyro.c @@ -248,7 +248,7 @@ STATIC_UNIT_TESTED gyroSensor_e gyroDetect(gyroDev_t *dev, gyroSensor_e gyroHard return gyroHardware; } -static void initGyroFilter(filterApplyFnPtr *applyFn, filter_t state[], uint8_t type, uint16_t cutoff) +static void initGyroFilter(filterApplyFnPtr *applyFn, filter_t state[], uint8_t type, uint16_t cutoff, uint32_t looptime) { *applyFn = nullFilterApply; if (cutoff > 0) { @@ -257,13 +257,13 @@ static void initGyroFilter(filterApplyFnPtr *applyFn, filter_t state[], uint8_t case FILTER_PT1: *applyFn = (filterApplyFnPtr)pt1FilterApply; for (int axis = 0; axis < 3; axis++) { - pt1FilterInit(&state[axis].pt1, cutoff, getGyroLooptime()* 1e-6f); + pt1FilterInit(&state[axis].pt1, cutoff, looptime * 1e-6f); } break; case FILTER_BIQUAD: *applyFn = (filterApplyFnPtr)biquadFilterApply; for (int axis = 0; axis < 3; axis++) { - biquadFilterInitLPF(&state[axis].biquad, cutoff, getGyroLooptime()); + biquadFilterInitLPF(&state[axis].biquad, cutoff, looptime); } break; } @@ -275,14 +275,18 @@ static void gyroInitFilters(void) STATIC_FASTRAM biquadFilter_t gyroFilterNotch_1[XYZ_AXIS_COUNT]; notchFilter1ApplyFn = nullFilterApply; - initGyroFilter(&gyroLpf2ApplyFn, gyroLpf2State, gyroConfig()->gyro_stage2_lowpass_type, gyroConfig()->gyro_stage2_lowpass_hz); - initGyroFilter(&gyroLpfApplyFn, gyroLpfState, gyroConfig()->gyro_soft_lpf_type, gyroConfig()->gyro_soft_lpf_hz); + //First gyro LPF running at full gyro frequency 8kHz + initGyroFilter(&gyroLpfApplyFn, gyroLpfState, gyroConfig()->gyro_soft_lpf_type, gyroConfig()->gyro_soft_lpf_hz, getGyroLooptime()); + + //Second gyro LPF runnig and PID frequency - this filter is dynamic when gyro_use_dyn_lpf = ON + initGyroFilter(&gyroLpf2ApplyFn, gyroLpf2State, gyroConfig()->gyro_stage2_lowpass_type, gyroConfig()->gyro_stage2_lowpass_hz, getLooptime()); + //Static Gyro notch running and PID frequency if (gyroConfig()->gyro_notch_hz) { notchFilter1ApplyFn = (filterApplyFnPtr)biquadFilterApply; for (int axis = 0; axis < 3; axis++) { notchFilter1[axis] = &gyroFilterNotch_1[axis]; - biquadFilterInitNotch(notchFilter1[axis], getGyroLooptime(), gyroConfig()->gyro_notch_hz, gyroConfig()->gyro_notch_cutoff); + biquadFilterInitNotch(notchFilter1[axis], getLooptime(), gyroConfig()->gyro_notch_hz, gyroConfig()->gyro_notch_cutoff); } } } @@ -327,13 +331,15 @@ bool gyroInit(void) } gyroInitFilters(); + #ifdef USE_DYNAMIC_FILTERS + // Dynamic notch running at PID frequency dynamicGyroNotchFiltersInit(&dynamicGyroNotchState); gyroDataAnalyseStateInit( &gyroAnalyseState, gyroConfig()->dynamicGyroNotchMinHz, gyroConfig()->dynamicGyroNotchRange, - getGyroLooptime() + getLooptime() ); #endif return true; @@ -433,22 +439,15 @@ static bool FAST_CODE NOINLINE gyroUpdateAndCalibrate(gyroDev_t * gyroDev, zeroC } } -void FAST_CODE NOINLINE gyroUpdate() +void FAST_CODE NOINLINE gyroFilter() { if (!gyro.initialized) { return; } - if (!gyroUpdateAndCalibrate(&gyroDev[0], &gyroCalibration[0], gyro.gyroADCf)) { - return; - } - for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { - // At this point gyro.gyroADCf contains unfiltered gyro value [deg/s] float gyroADCf = gyro.gyroADCf[axis]; - DEBUG_SET(DEBUG_GYRO, axis, lrintf(gyroADCf)); - #ifdef USE_RPM_FILTER DEBUG_SET(DEBUG_RPM_FILTER, axis, gyroADCf); gyroADCf = rpmFilterGyroApply(axis, gyroADCf); @@ -456,7 +455,6 @@ void FAST_CODE NOINLINE gyroUpdate() #endif gyroADCf = gyroLpf2ApplyFn((filter_t *) &gyroLpf2State[axis], gyroADCf); - gyroADCf = gyroLpfApplyFn((filter_t *) &gyroLpfState[axis], gyroADCf); gyroADCf = notchFilter1ApplyFn(notchFilter1[axis], gyroADCf); #ifdef USE_DYNAMIC_FILTERS @@ -467,6 +465,7 @@ void FAST_CODE NOINLINE gyroUpdate() DEBUG_SET(DEBUG_DYNAMIC_FILTER, axis + 3, gyroADCf); } #endif + gyro.gyroADCf[axis] = gyroADCf; } @@ -483,7 +482,31 @@ void FAST_CODE NOINLINE gyroUpdate() } } #endif +} +void FAST_CODE NOINLINE gyroUpdate() +{ + if (!gyro.initialized) { + return; + } + + if (!gyroUpdateAndCalibrate(&gyroDev[0], &gyroCalibration[0], gyro.gyroADCf)) { + return; + } + + for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { + // At this point gyro.gyroADCf contains unfiltered gyro value [deg/s] + float gyroADCf = gyro.gyroADCf[axis]; + + DEBUG_SET(DEBUG_GYRO, axis, lrintf(gyroADCf)); + + /* + * First gyro LPF is the only filter applied with the full gyro sampling speed + */ + gyroADCf = gyroLpfApplyFn((filter_t *) &gyroLpfState[axis], gyroADCf); + + gyro.gyroADCf[axis] = gyroADCf; + } } bool gyroReadTemperature(void) @@ -534,11 +557,11 @@ bool gyroSyncCheckUpdate(void) void gyroUpdateDynamicLpf(float cutoffFreq) { if (gyroConfig()->gyro_soft_lpf_type == FILTER_PT1) { for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { - pt1FilterUpdateCutoff(&gyroLpfState[axis].pt1, cutoffFreq); + pt1FilterUpdateCutoff(&gyroLpf2State[axis].pt1, cutoffFreq); } } else if (gyroConfig()->gyro_soft_lpf_type == FILTER_BIQUAD) { for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { - biquadFilterUpdate(&gyroLpfState[axis].biquad, cutoffFreq, getGyroLooptime(), BIQUAD_Q, FILTER_LPF); + biquadFilterUpdate(&gyroLpf2State[axis].biquad, cutoffFreq, getLooptime(), BIQUAD_Q, FILTER_LPF); } } } diff --git a/src/main/sensors/gyro.h b/src/main/sensors/gyro.h index 6aa3c11414..0e37e91c26 100644 --- a/src/main/sensors/gyro.h +++ b/src/main/sensors/gyro.h @@ -90,6 +90,7 @@ PG_DECLARE(gyroConfig_t, gyroConfig); bool gyroInit(void); void gyroGetMeasuredRotationRate(fpVector3_t *imuMeasuredRotationBF); void gyroUpdate(void); +void gyroFilter(void); void gyroStartCalibration(void); bool gyroIsCalibrationComplete(void); bool gyroReadTemperature(void); From 8e22378f1afb7031b7009aff4a2c045155c781b4 Mon Sep 17 00:00:00 2001 From: "Pawel Spychalski (DzikuVx)" Date: Sun, 18 Apr 2021 17:03:45 +0200 Subject: [PATCH 185/286] Restructure scheduler to give highest priority to RT tasks --- src/main/fc/config.h | 2 +- src/main/fc/fc_core.c | 1 + src/main/scheduler/scheduler.c | 34 ++++++++++++---------------------- src/main/scheduler/scheduler.h | 2 +- 4 files changed, 15 insertions(+), 24 deletions(-) diff --git a/src/main/fc/config.h b/src/main/fc/config.h index cc28ea9b3e..16324b5310 100644 --- a/src/main/fc/config.h +++ b/src/main/fc/config.h @@ -29,7 +29,7 @@ #define ONESHOT_FEATURE_CHANGED_DELAY_ON_BOOT_MS 1500 #define MAX_NAME_LENGTH 16 -#define TASK_GYRO_LOOPTIME 125 // Task gyro always runs at 8kHz +#define TASK_GYRO_LOOPTIME 250 // Task gyro always runs at 4kHz typedef enum { FEATURE_THR_VBAT_COMP = 1 << 0, FEATURE_VBAT = 1 << 1, diff --git a/src/main/fc/fc_core.c b/src/main/fc/fc_core.c index eef3351e1f..17c6ab40e7 100755 --- a/src/main/fc/fc_core.c +++ b/src/main/fc/fc_core.c @@ -794,6 +794,7 @@ void FAST_CODE NOINLINE taskGyro(timeUs_t currentTimeUs) { // getTaskDeltaTime() returns delta time frozen at the moment of entering the scheduler. currentTime is frozen at the very same point. // To make busy-waiting timeout work we need to account for time spent within busy-waiting loop const timeDelta_t currentDeltaTime = getTaskDeltaTime(TASK_SELF); + DEBUG_SET(DEBUG_ALWAYS, 0, currentDeltaTime); timeUs_t gyroUpdateUs = currentTimeUs; if (gyroConfig()->gyroSync) { diff --git a/src/main/scheduler/scheduler.c b/src/main/scheduler/scheduler.c index f3de93c7b7..0dc3ed48f7 100755 --- a/src/main/scheduler/scheduler.c +++ b/src/main/scheduler/scheduler.c @@ -219,22 +219,10 @@ void FAST_CODE NOINLINE scheduler(void) // Cache currentTime const timeUs_t currentTimeUs = micros(); - // Check for realtime tasks - timeUs_t timeToNextRealtimeTask = TIMEUS_MAX; - for (const cfTask_t *task = queueFirst(); task != NULL && task->staticPriority >= TASK_PRIORITY_REALTIME; task = queueNext()) { - const timeUs_t nextExecuteAt = task->lastExecutedAt + task->desiredPeriod; - if ((int32_t)(currentTimeUs - nextExecuteAt) >= 0) { - timeToNextRealtimeTask = 0; - } else { - const timeUs_t newTimeInterval = nextExecuteAt - currentTimeUs; - timeToNextRealtimeTask = MIN(timeToNextRealtimeTask, newTimeInterval); - } - } - const bool outsideRealtimeGuardInterval = (timeToNextRealtimeTask > 0); - // The task to be invoked cfTask_t *selectedTask = NULL; uint16_t selectedTaskDynamicPriority = 0; + bool forcedRealTimeTask = false; // Update task dynamic priorities uint16_t waitingTasks = 0; @@ -263,6 +251,14 @@ void FAST_CODE NOINLINE scheduler(void) } else { task->taskAgeCycles = 0; } + } else if (task->staticPriority == TASK_PRIORITY_REALTIME) { + //realtime tasks take absolute priority. Any RT tasks that is overdue, should be execute immediately + if (((timeDelta_t)(currentTimeUs - task->lastExecutedAt)) > task->desiredPeriod) { + selectedTaskDynamicPriority = task->dynamicPriority; + selectedTask = task; + waitingTasks++; + forcedRealTimeTask = true; + } } else { // Task is time-driven, dynamicPriority is last execution age (measured in desiredPeriods) // Task age is calculated from last execution @@ -273,15 +269,9 @@ void FAST_CODE NOINLINE scheduler(void) } } - if (task->dynamicPriority > selectedTaskDynamicPriority) { - const bool taskCanBeChosenForScheduling = - (outsideRealtimeGuardInterval) || - (task->taskAgeCycles > 1) || - (task->staticPriority == TASK_PRIORITY_REALTIME); - if (taskCanBeChosenForScheduling) { - selectedTaskDynamicPriority = task->dynamicPriority; - selectedTask = task; - } + if (!forcedRealTimeTask && task->dynamicPriority > selectedTaskDynamicPriority) { + selectedTaskDynamicPriority = task->dynamicPriority; + selectedTask = task;e } } diff --git a/src/main/scheduler/scheduler.h b/src/main/scheduler/scheduler.h index ff4c429299..6024714a8d 100755 --- a/src/main/scheduler/scheduler.h +++ b/src/main/scheduler/scheduler.h @@ -27,7 +27,7 @@ typedef enum { TASK_PRIORITY_MEDIUM = 3, TASK_PRIORITY_MEDIUM_HIGH = 4, TASK_PRIORITY_HIGH = 5, - TASK_PRIORITY_REALTIME = 6, + TASK_PRIORITY_REALTIME = 18, TASK_PRIORITY_MAX = 255 } cfTaskPriority_e; From 3b3dc05820f7ff59d934f23f2cfaa5886d25a636 Mon Sep 17 00:00:00 2001 From: "Pawel Spychalski (DzikuVx)" Date: Sun, 18 Apr 2021 17:29:12 +0200 Subject: [PATCH 186/286] Fix compilation --- src/main/scheduler/scheduler.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/main/scheduler/scheduler.c b/src/main/scheduler/scheduler.c index 0dc3ed48f7..b03276aacb 100755 --- a/src/main/scheduler/scheduler.c +++ b/src/main/scheduler/scheduler.c @@ -271,7 +271,7 @@ void FAST_CODE NOINLINE scheduler(void) if (!forcedRealTimeTask && task->dynamicPriority > selectedTaskDynamicPriority) { selectedTaskDynamicPriority = task->dynamicPriority; - selectedTask = task;e + selectedTask = task; } } From 5d3a01cfbee7448efa7a5a970233879f08bd1a43 Mon Sep 17 00:00:00 2001 From: "Pawel Spychalski (DzikuVx)" Date: Sun, 18 Apr 2021 18:03:12 +0200 Subject: [PATCH 187/286] Drop gyro sync --- src/main/fc/config.c | 4 ---- src/main/fc/fc_core.c | 26 +------------------------- src/main/fc/fc_msp.c | 4 ++-- src/main/fc/settings.yaml | 5 ----- src/main/sensors/gyro.c | 18 ++---------------- src/main/sensors/gyro.h | 2 -- src/main/target/FALCORE/config.c | 1 - 7 files changed, 5 insertions(+), 55 deletions(-) diff --git a/src/main/fc/config.c b/src/main/fc/config.c index 46ed815100..25ecb435af 100755 --- a/src/main/fc/config.c +++ b/src/main/fc/config.c @@ -294,10 +294,6 @@ void validateAndFixConfig(void) } #endif -#if !defined(USE_MPU_DATA_READY_SIGNAL) - gyroConfigMutable()->gyroSync = false; -#endif - // Call target-specific validation function validateAndFixTargetConfig(); diff --git a/src/main/fc/fc_core.c b/src/main/fc/fc_core.c index f4b1479143..4d035c33a3 100755 --- a/src/main/fc/fc_core.c +++ b/src/main/fc/fc_core.c @@ -101,9 +101,6 @@ enum { ALIGN_MAG = 2 }; -#define GYRO_WATCHDOG_DELAY 100 // Watchdog for boards without interrupt for gyro -#define GYRO_SYNC_MAX_CONSECUTIVE_FAILURES 100 // After this many consecutive missed interrupts disable gyro sync and fall back to scheduled updates - #define EMERGENCY_ARMING_TIME_WINDOW_MS 10000 #define EMERGENCY_ARMING_COUNTER_STEP_MS 100 @@ -129,7 +126,6 @@ int16_t headFreeModeHold; uint8_t motorControlEnable = false; static bool isRXDataNew; -static uint32_t gyroSyncFailureCount; static disarmReason_t lastDisarmReason = DISARM_NONE; timeUs_t lastDisarmTimeUs = 0; static emergencyArmingState_t emergencyArming; @@ -812,33 +808,13 @@ void FAST_CODE taskGyro(timeUs_t currentTimeUs) { // getTaskDeltaTime() returns delta time frozen at the moment of entering the scheduler. currentTime is frozen at the very same point. // To make busy-waiting timeout work we need to account for time spent within busy-waiting loop const timeDelta_t currentDeltaTime = getTaskDeltaTime(TASK_SELF); - timeUs_t gyroUpdateUs = currentTimeUs; - - if (gyroConfig()->gyroSync) { - while (true) { - gyroUpdateUs = micros(); - if (gyroSyncCheckUpdate()) { - gyroSyncFailureCount = 0; - break; - } - else if ((currentDeltaTime + cmpTimeUs(gyroUpdateUs, currentTimeUs)) >= (timeDelta_t)(getLooptime() + GYRO_WATCHDOG_DELAY)) { - gyroSyncFailureCount++; - break; - } - } - - // If we detect gyro sync failure - disable gyro sync - if (gyroSyncFailureCount > GYRO_SYNC_MAX_CONSECUTIVE_FAILURES) { - gyroConfigMutable()->gyroSync = false; - } - } /* Update actual hardware readings */ gyroUpdate(); #ifdef USE_OPFLOW if (sensors(SENSOR_OPFLOW)) { - opflowGyroUpdateCallback((timeUs_t)currentDeltaTime + (gyroUpdateUs - currentTimeUs)); + opflowGyroUpdateCallback(currentDeltaTime); } #endif } diff --git a/src/main/fc/fc_msp.c b/src/main/fc/fc_msp.c index 5fdcebc9e1..8f968440b5 100644 --- a/src/main/fc/fc_msp.c +++ b/src/main/fc/fc_msp.c @@ -1229,7 +1229,7 @@ static bool mspFcProcessOutCommand(uint16_t cmdMSP, sbuf_t *dst, mspPostProcessF sbufWriteU8(dst, motorConfig()->motorPwmProtocol); sbufWriteU16(dst, motorConfig()->motorPwmRate); sbufWriteU16(dst, servoConfig()->servoPwmRate); - sbufWriteU8(dst, gyroConfig()->gyroSync); + sbufWriteU8(dst, 0); break; case MSP_FILTER_CONFIG : @@ -2163,7 +2163,7 @@ static mspResult_e mspFcProcessInCommand(uint16_t cmdMSP, sbuf_t *src) motorConfigMutable()->motorPwmProtocol = sbufReadU8(src); motorConfigMutable()->motorPwmRate = sbufReadU16(src); servoConfigMutable()->servoPwmRate = sbufReadU16(src); - gyroConfigMutable()->gyroSync = sbufReadU8(src); + sbufReadU8(src); //Was gyroSync } else return MSP_RESULT_ERROR; break; diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 95b3df1bfc..c6dff3e7ad 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -179,11 +179,6 @@ groups: description: "This is the main loop time (in us). Changing this affects PID effect with some PID controllers (see PID section for details). A very conservative value of 3500us/285Hz should work for everyone. Setting it to zero does not limit loop time, so it will go as fast as possible." default_value: 1000 max: 9000 - - name: gyro_sync - description: "This option enables gyro_sync feature. In this case the loop will be synced to gyro refresh rate. Loop will always wait for the newest gyro measurement. Maximum gyro refresh rate is determined by gyro_hardware_lpf" - default_value: ON - field: gyroSync - type: bool - name: align_gyro description: "When running on non-default hardware or adding support for new sensors/sensor boards, these values are used for sensor orientation. When carefully understood, these values can also be used to rotate (in 90deg steps) or flip the board. Possible values are: DEFAULT, CW0_DEG, CW90_DEG, CW180_DEG, CW270_DEG, CW0_DEG_FLIP, CW90_DEG_FLIP, CW180_DEG_FLIP, CW270_DEG_FLIP." default_value: "DEFAULT" diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c index 6a8710bff2..0d32b49bfd 100644 --- a/src/main/sensors/gyro.c +++ b/src/main/sensors/gyro.c @@ -104,7 +104,7 @@ EXTENDED_FASTRAM dynamicGyroNotchState_t dynamicGyroNotchState; #endif -PG_REGISTER_WITH_RESET_TEMPLATE(gyroConfig_t, gyroConfig, PG_GYRO_CONFIG, 11); +PG_REGISTER_WITH_RESET_TEMPLATE(gyroConfig_t, gyroConfig, PG_GYRO_CONFIG, 12); PG_RESET_TEMPLATE(gyroConfig_t, gyroConfig, .gyro_lpf = SETTING_GYRO_HARDWARE_LPF_DEFAULT, @@ -113,7 +113,6 @@ PG_RESET_TEMPLATE(gyroConfig_t, gyroConfig, .gyro_align = SETTING_ALIGN_GYRO_DEFAULT, .gyroMovementCalibrationThreshold = SETTING_MORON_THRESHOLD_DEFAULT, .looptime = SETTING_LOOPTIME_DEFAULT, - .gyroSync = SETTING_GYRO_SYNC_DEFAULT, #ifdef USE_DUAL_GYRO .gyro_to_use = SETTING_GYRO_TO_USE_DEFAULT, #endif @@ -318,7 +317,7 @@ bool gyroInit(void) gyroDev[0].initFn(&gyroDev[0]); // initFn will initialize sampleRateIntervalUs to actual gyro sampling rate (if driver supports it). Calculate target looptime using that value - gyro.targetLooptime = gyroConfig()->gyroSync ? gyroDev[0].sampleRateIntervalUs : gyroConfig()->looptime; + gyro.targetLooptime = gyroDev[0].sampleRateIntervalUs; // At this poinrt gyroDev[0].gyroAlign was set up by the driver from the busDev record // If configuration says different - override @@ -518,19 +517,6 @@ int16_t gyroRateDps(int axis) return lrintf(gyro.gyroADCf[axis]); } -bool gyroSyncCheckUpdate(void) -{ - if (!gyro.initialized) { - return false; - } - - if (!gyroDev[0].intStatusFn) { - return false; - } - - return gyroDev[0].intStatusFn(&gyroDev[0]); -} - void gyroUpdateDynamicLpf(float cutoffFreq) { if (gyroConfig()->gyro_soft_lpf_type == FILTER_PT1) { for (int axis = 0; axis < XYZ_AXIS_COUNT; axis++) { diff --git a/src/main/sensors/gyro.h b/src/main/sensors/gyro.h index 6aa3c11414..b84477d686 100644 --- a/src/main/sensors/gyro.h +++ b/src/main/sensors/gyro.h @@ -61,7 +61,6 @@ extern gyro_t gyro; typedef struct gyroConfig_s { sensor_align_e gyro_align; // gyro alignment uint8_t gyroMovementCalibrationThreshold; // people keep forgetting that moving model while init results in wrong gyro offsets. and then they never reset gyro. so this is now on by default. - bool gyroSync; // Enable interrupt based loop uint16_t looptime; // imu loop time in us uint8_t gyro_lpf; // gyro LPF setting - values are driver specific, in case of invalid number, a reasonable default ~30-40HZ is chosen. uint8_t gyro_soft_lpf_hz; @@ -95,5 +94,4 @@ bool gyroIsCalibrationComplete(void); bool gyroReadTemperature(void); int16_t gyroGetTemperature(void); int16_t gyroRateDps(int axis); -bool gyroSyncCheckUpdate(void); void gyroUpdateDynamicLpf(float cutoffFreq); diff --git a/src/main/target/FALCORE/config.c b/src/main/target/FALCORE/config.c index d862359d49..0428b0a266 100755 --- a/src/main/target/FALCORE/config.c +++ b/src/main/target/FALCORE/config.c @@ -59,7 +59,6 @@ void targetConfiguration(void) gyroConfigMutable()->looptime = 1000; - gyroConfigMutable()->gyroSync = 1; gyroConfigMutable()->gyro_lpf = 0; // 256 Hz gyroConfigMutable()->gyro_soft_lpf_hz = 90; gyroConfigMutable()->gyro_notch_hz = 150; From c7d9415761ae04bfd0cde669cffd286afd82e20e Mon Sep 17 00:00:00 2001 From: "Pawel Spychalski (DzikuVx)" Date: Sun, 18 Apr 2021 18:06:37 +0200 Subject: [PATCH 188/286] Docs update --- docs/Settings.md | 1 - 1 file changed, 1 deletion(-) diff --git a/docs/Settings.md b/docs/Settings.md index 537f8239e3..47c551e696 100644 --- a/docs/Settings.md +++ b/docs/Settings.md @@ -158,7 +158,6 @@ | gyro_notch_hz | 0 | | | gyro_stage2_lowpass_hz | 0 | Software based second stage lowpass filter for gyro. Value is cutoff frequency (Hz) | | gyro_stage2_lowpass_type | BIQUAD | Defines the type of stage 2 gyro LPF filter. Possible values: `PT1`, `BIQUAD`. `PT1` offers faster filter response while `BIQUAD` better attenuation. | -| gyro_sync | ON | This option enables gyro_sync feature. In this case the loop will be synced to gyro refresh rate. Loop will always wait for the newest gyro measurement. Maximum gyro refresh rate is determined by gyro_hardware_lpf | | gyro_to_use | 0 | | | gyro_use_dyn_lpf | OFF | Use Dynamic LPF instead of static gyro stage1 LPF. Dynamic Gyro LPF updates gyro LPF based on the throttle position. | | has_flaps | OFF | Defines is UAV is capable of having flaps. If ON and AIRPLANE `platform_type` is used, **FLAPERON** flight mode will be available for the pilot | From dc7a002b8847ee7f9186e2cc72e18c3acf73c829 Mon Sep 17 00:00:00 2001 From: "Pawel Spychalski (DzikuVx)" Date: Sun, 18 Apr 2021 20:32:54 +0200 Subject: [PATCH 189/286] Fix build --- src/main/fc/fc_core.c | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/main/fc/fc_core.c b/src/main/fc/fc_core.c index 4d035c33a3..e154490715 100755 --- a/src/main/fc/fc_core.c +++ b/src/main/fc/fc_core.c @@ -816,6 +816,8 @@ void FAST_CODE taskGyro(timeUs_t currentTimeUs) { if (sensors(SENSOR_OPFLOW)) { opflowGyroUpdateCallback(currentDeltaTime); } +#else + UNUSED(currentTimeUs); #endif } From ab2836123d1281fffc9b0fbebc7cf476f4ee4935 Mon Sep 17 00:00:00 2001 From: "Pawel Spychalski (DzikuVx)" Date: Sun, 18 Apr 2021 21:44:55 +0200 Subject: [PATCH 190/286] Fix compilation (again) --- src/main/fc/fc_core.c | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/src/main/fc/fc_core.c b/src/main/fc/fc_core.c index e154490715..d6a6019fc6 100755 --- a/src/main/fc/fc_core.c +++ b/src/main/fc/fc_core.c @@ -805,6 +805,7 @@ void processRx(timeUs_t currentTimeUs) // Function for loop trigger void FAST_CODE taskGyro(timeUs_t currentTimeUs) { + UNUSED(currentTimeUs); // getTaskDeltaTime() returns delta time frozen at the moment of entering the scheduler. currentTime is frozen at the very same point. // To make busy-waiting timeout work we need to account for time spent within busy-waiting loop const timeDelta_t currentDeltaTime = getTaskDeltaTime(TASK_SELF); @@ -816,8 +817,6 @@ void FAST_CODE taskGyro(timeUs_t currentTimeUs) { if (sensors(SENSOR_OPFLOW)) { opflowGyroUpdateCallback(currentDeltaTime); } -#else - UNUSED(currentTimeUs); #endif } From e4f684fda6fa640ddd67a923482023cc8d7527db Mon Sep 17 00:00:00 2001 From: Alexander van Saase Date: Sun, 18 Apr 2021 22:24:55 +0200 Subject: [PATCH 191/286] remove setting --- docs/Settings.md | 3 +-- src/main/fc/settings.yaml | 5 ----- src/main/flight/servos.c | 18 ++++++++++-------- 3 files changed, 11 insertions(+), 15 deletions(-) diff --git a/docs/Settings.md b/docs/Settings.md index 414b0db15f..b67551d1a3 100644 --- a/docs/Settings.md +++ b/docs/Settings.md @@ -474,8 +474,7 @@ | serialrx_halfduplex | AUTO | Allow serial receiver to operate on UART TX pin. With some receivers will allow control and telemetry over a single wire. | | serialrx_inverted | OFF | Reverse the serial inversion of the serial RX protocol. When this value is OFF, each protocol will use its default signal (e.g. SBUS will use an inverted signal). Some OpenLRS receivers produce a non-inverted SBUS signal. This setting supports this type of receivers (including modified FrSKY). | | serialrx_provider | _target default_ | When feature SERIALRX is enabled, this allows connection to several receivers which output data via digital interface resembling serial. See RX section. | -| servo_autotrim_iterm_threshold | 5 | How much of the Iterm is carried over to the servo midpoints on each update. Only applies when using `feature FW_AUTOTRIM`. | -| servo_autotrim_rotation_limit | 10 | Servo midpoints are only updated when total aircraft rotation is less than this threshold [deg/s]. Only applies when using `feature FW_AUTOTRIM`. | +| servo_autotrim_rotation_limit | 15 | Servo midpoints are only updated when total aircraft rotation is less than this threshold [deg/s]. Only applies when using `feature FW_AUTOTRIM`. | | servo_center_pulse | 1500 | Servo midpoint | | servo_lpf_hz | 20 | Selects the servo PWM output cutoff frequency. Value is in [Hz] | | servo_protocol | PWM | An option to chose the protocol/option that would be used to output servo data. Possible options `PWM` (FC servo outputs), `SERVO_DRIVER` (I2C PCA9685 peripheral), `SBUS` (S.Bus protocol output via a configured serial port) | diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 39cce57212..f7b0b4f8c3 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -1181,11 +1181,6 @@ groups: default_value: 15 min: 1 max: 60 - - name: servo_autotrim_iterm_threshold - description: "How much of the Iterm is carried over to the servo midpoints on each update. Only applies when using `feature FW_AUTOTRIM`." - default_value: 5 - min: 1 - max: 25 - name: PG_CONTROL_RATE_PROFILES type: controlRateConfig_t diff --git a/src/main/flight/servos.c b/src/main/flight/servos.c index eed1432649..2d1835b029 100755 --- a/src/main/flight/servos.c +++ b/src/main/flight/servos.c @@ -67,8 +67,7 @@ PG_RESET_TEMPLATE(servoConfig_t, servoConfig, .servo_protocol = SETTING_SERVO_PROTOCOL_DEFAULT, .flaperon_throw_offset = SETTING_FLAPERON_THROW_OFFSET_DEFAULT, .tri_unarmed_servo = SETTING_TRI_UNARMED_SERVO_DEFAULT, - .servo_autotrim_rotation_limit = SETTING_SERVO_AUTOTRIM_ROTATION_LIMIT_DEFAULT, - .servo_autotrim_iterm_threshold = SETTING_SERVO_AUTOTRIM_ITERM_THRESHOLD_DEFAULT + .servo_autotrim_rotation_limit = SETTING_SERVO_AUTOTRIM_ROTATION_LIMIT_DEFAULT ); PG_REGISTER_ARRAY_WITH_RESET_FN(servoMixer_t, MAX_SERVO_RULES, customServoMixers, PG_SERVO_MIXER, 1); @@ -504,6 +503,7 @@ bool isMixerUsingServos(void) #define SERVO_AUTOTRIM_FILTER_CUTOFF 1 // LPF cutoff frequency #define SERVO_AUTOTRIM_CENTER_MIN 1300 #define SERVO_AUTOTRIM_CENTER_MAX 1700 +#define SERVO_AUTOTRIM_UPDATE_SIZE 5 void processContinuousServoAutotrim(const float dT) { @@ -513,8 +513,6 @@ void processContinuousServoAutotrim(const float dT) static int16_t servoMiddleBackup[MAX_SUPPORTED_SERVOS]; static int32_t servoMiddleUpdateCount; - const uint8_t ItermThreshold = servoConfig()->servo_autotrim_iterm_threshold; - const float rotRateMagnitude = sqrtf(vectorNormSquared(&imuMeasuredRotationBF)); const float rotRateMagnitudeFiltered = pt1FilterApply4(&rotRateFilter, rotRateMagnitude, SERVO_AUTOTRIM_FILTER_CUTOFF, dT); @@ -545,16 +543,20 @@ void processContinuousServoAutotrim(const float dT) case AUTOTRIM_COLLECTING: if (ARMING_FLAG(ARMED)) { // Every half second update servo midpoints - if ((millis() - lastUpdateTimeMs) > 500 && isGPSHeadingValid()) { + if ((millis() - lastUpdateTimeMs) > 500) { const bool planeFlyingStraight = rotRateMagnitudeFiltered <= DEGREES_TO_RADIANS(servoConfig()->servo_autotrim_rotation_limit); const bool zeroRotationCommanded = getTotalRateTarget() <= servoConfig()->servo_autotrim_rotation_limit; - if (planeFlyingStraight && zeroRotationCommanded && !areSticksDeflectedMoreThanPosHoldDeadband() &&!FLIGHT_MODE(MANUAL_MODE)) { + if (planeFlyingStraight && + zeroRotationCommanded && + !areSticksDeflectedMoreThanPosHoldDeadband() && + isGPSHeadingValid() && + !FLIGHT_MODE(MANUAL_MODE)) { // Plane is flying straight and sticks are centered for (int axis = FD_ROLL; axis <= FD_YAW; axis++) { // For each stabilized axis, add x units of I-term to all associated servo midpoints const float axisIterm = getAxisIterm(axis); - if (fabsf(axisIterm) > ItermThreshold) { - const int8_t ItermUpdate = axisIterm > 0.0f ? ItermThreshold : -ItermThreshold; + if (fabsf(axisIterm) > SERVO_AUTOTRIM_UPDATE_SIZE) { + const int8_t ItermUpdate = axisIterm > 0.0f ? SERVO_AUTOTRIM_UPDATE_SIZE : -SERVO_AUTOTRIM_UPDATE_SIZE; for (int i = 0; i < servoRuleCount; i++) { #ifdef USE_PROGRAMMING_FRAMEWORK if (!logicConditionGetValue(currentServoMixer[i].conditionId)) { From 80bf6e398e9b104ed8b8dab6f1267d627d4cbd54 Mon Sep 17 00:00:00 2001 From: Alexander van Saase Date: Sun, 18 Apr 2021 23:18:08 +0200 Subject: [PATCH 192/286] remove mode, feature only --- src/main/fc/fc_core.c | 4 +- src/main/fc/fc_msp_box.c | 3 - src/main/fc/rc_modes.h | 1 - src/main/flight/mixer.h | 3 +- src/main/flight/servos.c | 197 +++++++++++++++------------------------ src/main/io/osd.c | 2 +- 6 files changed, 80 insertions(+), 130 deletions(-) diff --git a/src/main/fc/fc_core.c b/src/main/fc/fc_core.c index d248c38917..3b810641b5 100755 --- a/src/main/fc/fc_core.c +++ b/src/main/fc/fc_core.c @@ -909,9 +909,7 @@ void taskMainPidLoop(timeUs_t currentTimeUs) if (isMixerUsingServos()) { servoMixer(dT); - processServoAutotrim(); - processContinuousServoAutotrim(dT); - + processServoAutotrim(dT); } //Servos should be filtered or written only when mixer is using servos or special feaures are enabled diff --git a/src/main/fc/fc_msp_box.c b/src/main/fc/fc_msp_box.c index 09bbd59a5f..7544614810 100644 --- a/src/main/fc/fc_msp_box.c +++ b/src/main/fc/fc_msp_box.c @@ -90,7 +90,6 @@ static const box_t boxes[CHECKBOX_ITEM_COUNT + 1] = { { BOXMSPRCOVERRIDE, "MSP RC OVERRIDE", 50 }, { BOXPREARM, "PREARM", 51 }, { BOXFLIPOVERAFTERCRASH, "TURTLE", 52 }, - { BOXCONTAUTOTRIM, "CONTINUOUS AUTOTRIM", 53 }, { CHECKBOX_ITEM_COUNT, NULL, 0xFF } }; @@ -244,7 +243,6 @@ void initActiveBoxIds(void) if (!feature(FEATURE_FW_AUTOTRIM)) { activeBoxIds[activeBoxIdCount++] = BOXAUTOTRIM; - activeBoxIds[activeBoxIdCount++] = BOXCONTAUTOTRIM; } #if defined(USE_AUTOTUNE_FIXED_WING) @@ -379,7 +377,6 @@ void packBoxModeFlags(boxBitmask_t * mspBoxModeFlags) #if defined(USE_RX_MSP) && defined(USE_MSP_RC_OVERRIDE) CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXMSPRCOVERRIDE)), BOXMSPRCOVERRIDE); #endif - CHECK_ACTIVE_BOX(IS_ENABLED(IS_RC_MODE_ACTIVE(BOXCONTAUTOTRIM)), BOXCONTAUTOTRIM); memset(mspBoxModeFlags, 0, sizeof(boxBitmask_t)); for (uint32_t i = 0; i < activeBoxIdCount; i++) { diff --git a/src/main/fc/rc_modes.h b/src/main/fc/rc_modes.h index bf4840164e..6b3f877197 100755 --- a/src/main/fc/rc_modes.h +++ b/src/main/fc/rc_modes.h @@ -70,7 +70,6 @@ typedef enum { BOXMSPRCOVERRIDE = 41, BOXPREARM = 42, BOXFLIPOVERAFTERCRASH = 43, - BOXCONTAUTOTRIM = 44, CHECKBOX_ITEM_COUNT } boxId_e; diff --git a/src/main/flight/mixer.h b/src/main/flight/mixer.h index 7e653e9679..60107e28e3 100644 --- a/src/main/flight/mixer.h +++ b/src/main/flight/mixer.h @@ -126,7 +126,8 @@ void mixerUpdateStateFlags(void); void mixerResetDisarmedMotors(void); void mixTable(const float dT); void writeMotors(void); -void processServoAutotrim(void); +void processServoAutotrim(const float dT); +void processServoAutotrimMode(void); void processContinuousServoAutotrim(const float dT); void stopMotors(void); void stopPwmAllMotors(void); diff --git a/src/main/flight/servos.c b/src/main/flight/servos.c index 2d1835b029..85e43ae00d 100755 --- a/src/main/flight/servos.c +++ b/src/main/flight/servos.c @@ -391,7 +391,7 @@ typedef enum { AUTOTRIM_DONE, } servoAutotrimState_e; -void processServoAutotrim(void) +void processServoAutotrimMode(void) { static servoAutotrimState_e trimState = AUTOTRIM_IDLE; static timeMs_t trimStartedAt; @@ -485,6 +485,81 @@ void processServoAutotrim(void) } } +#define SERVO_AUTOTRIM_FILTER_CUTOFF 1 // LPF cutoff frequency +#define SERVO_AUTOTRIM_CENTER_MIN 1300 +#define SERVO_AUTOTRIM_CENTER_MAX 1700 +#define SERVO_AUTOTRIM_UPDATE_SIZE 5 + +void processContinuousServoAutotrim(const float dT) +{ + static timeMs_t lastUpdateTimeMs; + static servoAutotrimState_e trimState = AUTOTRIM_IDLE; + static uint32_t servoMiddleUpdateCount; + + const float rotRateMagnitude = sqrtf(vectorNormSquared(&imuMeasuredRotationBF)); + const float rotRateMagnitudeFiltered = pt1FilterApply4(&rotRateFilter, rotRateMagnitude, SERVO_AUTOTRIM_FILTER_CUTOFF, dT); + + if (ARMING_FLAG(ARMED)) { + trimState = AUTOTRIM_COLLECTING; + if ((millis() - lastUpdateTimeMs) > 500) { + const bool planeFlyingStraight = rotRateMagnitudeFiltered <= DEGREES_TO_RADIANS(servoConfig()->servo_autotrim_rotation_limit); + const bool zeroRotationCommanded = getTotalRateTarget() <= servoConfig()->servo_autotrim_rotation_limit; + if (planeFlyingStraight && zeroRotationCommanded && !areSticksDeflectedMoreThanPosHoldDeadband() && !FLIGHT_MODE(MANUAL_MODE) && isGPSHeadingValid()) { + // Plane is flying straight and sticks are centered + for (int axis = FD_ROLL; axis <= FD_YAW; axis++) { + // For each stabilized axis, add 5 units of I-term to all associated servo midpoints + const float axisIterm = getAxisIterm(axis); + if (fabsf(axisIterm) > SERVO_AUTOTRIM_UPDATE_SIZE) { + const int8_t ItermUpdate = axisIterm > 0.0f ? SERVO_AUTOTRIM_UPDATE_SIZE : -SERVO_AUTOTRIM_UPDATE_SIZE; + for (int i = 0; i < servoRuleCount; i++) { +#ifdef USE_PROGRAMMING_FRAMEWORK + if (!logicConditionGetValue(currentServoMixer[i].conditionId)) { + continue; + } +#endif + const uint8_t target = currentServoMixer[i].targetChannel; + const uint8_t source = currentServoMixer[i].inputSource; + if (source == axis) { + // Convert axis I-term to servo PWM and add to midpoint + const float mixerRate = currentServoMixer[i].rate / 100.0f; + const float servoRate = servoParams(target)->rate / 100.0f; + servoParamsMutable(target)->middle += ItermUpdate * mixerRate * servoRate; + servoParamsMutable(target)->middle = constrain(servoParamsMutable(target)->middle, SERVO_AUTOTRIM_CENTER_MIN, SERVO_AUTOTRIM_CENTER_MAX); + } + } + pidReduceErrorAccumulators(ItermUpdate, axis); + } + } + servoMiddleUpdateCount++; + } + // Reset timer + lastUpdateTimeMs = millis(); + } + } else if (trimState == AUTOTRIM_COLLECTING) { + // We have disarmed, save midpoints to EEPROM + writeEEPROM(); + trimState = AUTOTRIM_IDLE; + } + + // Debug + DEBUG_SET(DEBUG_AUTOTRIM, 0, servoParams(2)->middle); + DEBUG_SET(DEBUG_AUTOTRIM, 2, servoParams(3)->middle); + DEBUG_SET(DEBUG_AUTOTRIM, 4, servoParams(4)->middle); + DEBUG_SET(DEBUG_AUTOTRIM, 6, servoParams(5)->middle); + DEBUG_SET(DEBUG_AUTOTRIM, 1, servoMiddleUpdateCount); + DEBUG_SET(DEBUG_AUTOTRIM, 3, MAX(RADIANS_TO_DEGREES(rotRateMagnitudeFiltered), getTotalRateTarget())); + DEBUG_SET(DEBUG_AUTOTRIM, 5, axisPID_I[FD_ROLL]); + DEBUG_SET(DEBUG_AUTOTRIM, 7, axisPID_I[FD_PITCH]); +} + +void processServoAutotrim(const float dT) { + if (feature(FEATURE_FW_AUTOTRIM)) { + processContinuousServoAutotrim(dT); + } else { + processServoAutotrimMode(); + } +} + bool isServoOutputEnabled(void) { return servoOutputEnabled; @@ -499,123 +574,3 @@ bool isMixerUsingServos(void) { return mixerUsesServos; } - -#define SERVO_AUTOTRIM_FILTER_CUTOFF 1 // LPF cutoff frequency -#define SERVO_AUTOTRIM_CENTER_MIN 1300 -#define SERVO_AUTOTRIM_CENTER_MAX 1700 -#define SERVO_AUTOTRIM_UPDATE_SIZE 5 - -void processContinuousServoAutotrim(const float dT) -{ - static timeMs_t lastUpdateTimeMs; - static servoAutotrimState_e trimState = AUTOTRIM_IDLE; - - static int16_t servoMiddleBackup[MAX_SUPPORTED_SERVOS]; - static int32_t servoMiddleUpdateCount; - - const float rotRateMagnitude = sqrtf(vectorNormSquared(&imuMeasuredRotationBF)); - const float rotRateMagnitudeFiltered = pt1FilterApply4(&rotRateFilter, rotRateMagnitude, SERVO_AUTOTRIM_FILTER_CUTOFF, dT); - - if (IS_RC_MODE_ACTIVE(BOXCONTAUTOTRIM) || feature(FEATURE_FW_AUTOTRIM)) { - switch (trimState) { - case AUTOTRIM_IDLE: - if (ARMING_FLAG(ARMED)) { - // FIXME: use proper flying detection - // We are activating servo trim - backup current middles and prepare to update the servo midpoints - for (int axis = FD_ROLL; axis <= FD_YAW; axis++) { - for (int i = 0; i < servoRuleCount; i++) { - const uint8_t target = currentServoMixer[i].targetChannel; - const uint8_t source = currentServoMixer[i].inputSource; - if (source == axis) { - servoMiddleBackup[target] = servoParams(target)->middle; - } - } - } - servoMiddleUpdateCount = 0; - lastUpdateTimeMs = millis(); - trimState = AUTOTRIM_COLLECTING; - } - else { - break; - } - // Fallthru - - case AUTOTRIM_COLLECTING: - if (ARMING_FLAG(ARMED)) { - // Every half second update servo midpoints - if ((millis() - lastUpdateTimeMs) > 500) { - const bool planeFlyingStraight = rotRateMagnitudeFiltered <= DEGREES_TO_RADIANS(servoConfig()->servo_autotrim_rotation_limit); - const bool zeroRotationCommanded = getTotalRateTarget() <= servoConfig()->servo_autotrim_rotation_limit; - if (planeFlyingStraight && - zeroRotationCommanded && - !areSticksDeflectedMoreThanPosHoldDeadband() && - isGPSHeadingValid() && - !FLIGHT_MODE(MANUAL_MODE)) { - // Plane is flying straight and sticks are centered - for (int axis = FD_ROLL; axis <= FD_YAW; axis++) { - // For each stabilized axis, add x units of I-term to all associated servo midpoints - const float axisIterm = getAxisIterm(axis); - if (fabsf(axisIterm) > SERVO_AUTOTRIM_UPDATE_SIZE) { - const int8_t ItermUpdate = axisIterm > 0.0f ? SERVO_AUTOTRIM_UPDATE_SIZE : -SERVO_AUTOTRIM_UPDATE_SIZE; - for (int i = 0; i < servoRuleCount; i++) { -#ifdef USE_PROGRAMMING_FRAMEWORK - if (!logicConditionGetValue(currentServoMixer[i].conditionId)) { - continue; - } -#endif - const uint8_t target = currentServoMixer[i].targetChannel; - const uint8_t source = currentServoMixer[i].inputSource; - if (source == axis) { - // Convert axis I-term to servo PWM and add to midpoint - const float mixerRate = currentServoMixer[i].rate / 100.0f; - const float servoRate = servoParams(target)->rate / 100.0f; - servoParamsMutable(target)->middle += ItermUpdate * mixerRate * servoRate; - servoParamsMutable(target)->middle = constrain(servoParamsMutable(target)->middle, SERVO_AUTOTRIM_CENTER_MIN, SERVO_AUTOTRIM_CENTER_MAX); - } - } - pidReduceErrorAccumulators(ItermUpdate, axis); - } - } - servoMiddleUpdateCount++; - } - // Reset timer - lastUpdateTimeMs = millis(); - } - break; - } else { - // We have disarmed, save to EEPROM - saveConfigAndNotify(); - trimState = AUTOTRIM_IDLE; - break; - } - case AUTOTRIM_SAVE_PENDING: - case AUTOTRIM_DONE: - break; - } - } - else { - // We are deactivating servo trim - restore servo midpoints - if (trimState == AUTOTRIM_COLLECTING) { - for (int axis = FD_ROLL; axis <= FD_YAW; axis++) { - for (int i = 0; i < servoRuleCount; i++) { - const uint8_t target = currentServoMixer[i].targetChannel; - const uint8_t source = currentServoMixer[i].inputSource; - if (source == axis) { - servoParamsMutable(target)->middle = servoMiddleBackup[target]; - } - } - } - } - trimState = AUTOTRIM_IDLE; - } - - // Debug - DEBUG_SET(DEBUG_AUTOTRIM, 0, servoParams(2)->middle); - DEBUG_SET(DEBUG_AUTOTRIM, 2, servoParams(3)->middle); - DEBUG_SET(DEBUG_AUTOTRIM, 4, servoParams(4)->middle); - DEBUG_SET(DEBUG_AUTOTRIM, 6, servoParams(5)->middle); - DEBUG_SET(DEBUG_AUTOTRIM, 1, servoMiddleUpdateCount); - DEBUG_SET(DEBUG_AUTOTRIM, 3, RADIANS_TO_DEGREES(rotRateMagnitudeFiltered)); - DEBUG_SET(DEBUG_AUTOTRIM, 5, axisPID_I[FD_ROLL]); - DEBUG_SET(DEBUG_AUTOTRIM, 7, axisPID_I[FD_PITCH]); -} \ No newline at end of file diff --git a/src/main/io/osd.c b/src/main/io/osd.c index ce17eb9e0d..7c8825e3a1 100644 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -3598,7 +3598,7 @@ textAttributes_t osdGetSystemMessage(char *buff, size_t buff_size, bool isCenter // by OSD_FLYMODE. messages[messageCount++] = OSD_MESSAGE_STR(OSD_MSG_ALTITUDE_HOLD); } - if (IS_RC_MODE_ACTIVE(BOXAUTOTRIM) || IS_RC_MODE_ACTIVE(BOXCONTAUTOTRIM)) { + if (IS_RC_MODE_ACTIVE(BOXAUTOTRIM)) { messages[messageCount++] = OSD_MESSAGE_STR(OSD_MSG_AUTOTRIM); } if (IS_RC_MODE_ACTIVE(BOXAUTOTUNE)) { From b44e991d53f019d523305ac2395dabc2c25b80f2 Mon Sep 17 00:00:00 2001 From: Alexander van Saase Date: Sun, 18 Apr 2021 23:24:34 +0200 Subject: [PATCH 193/286] change default --- src/main/fc/settings.yaml | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 95b3df1bfc..8bc90163bf 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -1727,10 +1727,10 @@ groups: table: direction - name: fw_reference_airspeed description: "Reference airspeed. Set this to airspeed at which PIDs were tuned. Usually should be set to cruise airspeed. Also used for coordinated turn calculation if airspeed sensor is not present." - default_value: 1000 + default_value: 1500 field: fixedWingReferenceAirspeed - min: 1 - max: 5000 + min: 300 + max: 6000 - name: fw_turn_assist_yaw_gain description: "Gain required to keep the yaw rate consistent with the turn rate for a coordinated turn (in TURN_ASSIST mode). Value significantly different from 1.0 indicates a problem with the airspeed calibration (if present) or value of `fw_reference_airspeed` parameter" default_value: 1 From 5b297d6fa358384050c61a522ba7ab98b21df4b7 Mon Sep 17 00:00:00 2001 From: Alexander van Saase Date: Sun, 18 Apr 2021 23:30:07 +0200 Subject: [PATCH 194/286] docs... --- docs/Settings.md | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/docs/Settings.md b/docs/Settings.md index 537f8239e3..357489e069 100644 --- a/docs/Settings.md +++ b/docs/Settings.md @@ -136,7 +136,7 @@ | fw_p_pitch | 5 | Fixed-wing rate stabilisation P-gain for PITCH | | fw_p_roll | 5 | Fixed-wing rate stabilisation P-gain for ROLL | | fw_p_yaw | 6 | Fixed-wing rate stabilisation P-gain for YAW | -| fw_reference_airspeed | 1000 | Reference airspeed. Set this to airspeed at which PIDs were tuned. Usually should be set to cruise airspeed. Also used for coordinated turn calculation if airspeed sensor is not present. | +| fw_reference_airspeed | 1500 | Reference airspeed. Set this to airspeed at which PIDs were tuned. Usually should be set to cruise airspeed. Also used for coordinated turn calculation if airspeed sensor is not present. | | fw_tpa_time_constant | 0 | TPA smoothing and delay time constant to reflect non-instant speed/throttle response of the plane. Planes with low thrust/weight ratio generally need higher time constant. Default is zero for compatibility with old setups | | fw_turn_assist_pitch_gain | 1 | Gain required to keep constant pitch angle during coordinated turns (in TURN_ASSIST mode). Value significantly different from 1.0 indicates a problem with the airspeed calibration (if present) or value of `fw_reference_airspeed` parameter | | fw_turn_assist_yaw_gain | 1 | Gain required to keep the yaw rate consistent with the turn rate for a coordinated turn (in TURN_ASSIST mode). Value significantly different from 1.0 indicates a problem with the airspeed calibration (if present) or value of `fw_reference_airspeed` parameter | From 65f344358ae01aae1c74ad68016c2bba62eeb141 Mon Sep 17 00:00:00 2001 From: breadoven <56191411+breadoven@users.noreply.github.com> Date: Mon, 19 Apr 2021 15:17:32 +0100 Subject: [PATCH 195/286] Update navigation.c (#6852) --- src/main/navigation/navigation.c | 1 + 1 file changed, 1 insertion(+) diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c index 9858471eb6..82a50595a9 100755 --- a/src/main/navigation/navigation.c +++ b/src/main/navigation/navigation.c @@ -1480,6 +1480,7 @@ static navigationFSMEvent_t navOnEnteringState_NAV_STATE_WAYPOINT_INITIALIZE(nav */ setupJumpCounters(); posControl.activeWaypointIndex = 0; + wpHeadingControl.mode = NAV_WP_HEAD_MODE_NONE; return NAV_FSM_EVENT_SUCCESS; // will switch to NAV_STATE_WAYPOINT_PRE_ACTION } } From d7233b3d84a25062e37989de9eef102ca4ef170a Mon Sep 17 00:00:00 2001 From: Alexander van Saase Date: Mon, 19 Apr 2021 21:13:06 +0200 Subject: [PATCH 196/286] use filtered rate target as threshold --- src/main/flight/servos.c | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/src/main/flight/servos.c b/src/main/flight/servos.c index 85e43ae00d..007c9760e8 100755 --- a/src/main/flight/servos.c +++ b/src/main/flight/servos.c @@ -118,6 +118,7 @@ static servoMetadata_t servoMetadata[MAX_SUPPORTED_SERVOS]; static rateLimitFilter_t servoSpeedLimitFilter[MAX_SERVO_RULES]; STATIC_FASTRAM pt1Filter_t rotRateFilter; +STATIC_FASTRAM pt1Filter_t targetRateFilter; int16_t getFlaperonDirection(uint8_t servoPin) { @@ -498,12 +499,14 @@ void processContinuousServoAutotrim(const float dT) const float rotRateMagnitude = sqrtf(vectorNormSquared(&imuMeasuredRotationBF)); const float rotRateMagnitudeFiltered = pt1FilterApply4(&rotRateFilter, rotRateMagnitude, SERVO_AUTOTRIM_FILTER_CUTOFF, dT); + const float targetRateMagnitude = getTotalRateTarget(); + const float targetRateMagnitudeFiltered = pt1FilterApply4(&targetRateFilter, targetRateMagnitude, SERVO_AUTOTRIM_FILTER_CUTOFF, dT); if (ARMING_FLAG(ARMED)) { trimState = AUTOTRIM_COLLECTING; if ((millis() - lastUpdateTimeMs) > 500) { const bool planeFlyingStraight = rotRateMagnitudeFiltered <= DEGREES_TO_RADIANS(servoConfig()->servo_autotrim_rotation_limit); - const bool zeroRotationCommanded = getTotalRateTarget() <= servoConfig()->servo_autotrim_rotation_limit; + const bool zeroRotationCommanded = targetRateMagnitudeFiltered <= servoConfig()->servo_autotrim_rotation_limit; if (planeFlyingStraight && zeroRotationCommanded && !areSticksDeflectedMoreThanPosHoldDeadband() && !FLIGHT_MODE(MANUAL_MODE) && isGPSHeadingValid()) { // Plane is flying straight and sticks are centered for (int axis = FD_ROLL; axis <= FD_YAW; axis++) { @@ -547,7 +550,7 @@ void processContinuousServoAutotrim(const float dT) DEBUG_SET(DEBUG_AUTOTRIM, 4, servoParams(4)->middle); DEBUG_SET(DEBUG_AUTOTRIM, 6, servoParams(5)->middle); DEBUG_SET(DEBUG_AUTOTRIM, 1, servoMiddleUpdateCount); - DEBUG_SET(DEBUG_AUTOTRIM, 3, MAX(RADIANS_TO_DEGREES(rotRateMagnitudeFiltered), getTotalRateTarget())); + DEBUG_SET(DEBUG_AUTOTRIM, 3, MAX(RADIANS_TO_DEGREES(rotRateMagnitudeFiltered), targetRateMagnitudeFiltered)); DEBUG_SET(DEBUG_AUTOTRIM, 5, axisPID_I[FD_ROLL]); DEBUG_SET(DEBUG_AUTOTRIM, 7, axisPID_I[FD_PITCH]); } From f89e97cf8de71d574770bc6f9793d301dc391e65 Mon Sep 17 00:00:00 2001 From: Alexander van Saase Date: Mon, 19 Apr 2021 21:33:31 +0200 Subject: [PATCH 197/286] add level flight logic --- src/main/flight/servos.c | 15 +++++++++++---- 1 file changed, 11 insertions(+), 4 deletions(-) diff --git a/src/main/flight/servos.c b/src/main/flight/servos.c index 007c9760e8..b31c3a038b 100755 --- a/src/main/flight/servos.c +++ b/src/main/flight/servos.c @@ -505,10 +505,17 @@ void processContinuousServoAutotrim(const float dT) if (ARMING_FLAG(ARMED)) { trimState = AUTOTRIM_COLLECTING; if ((millis() - lastUpdateTimeMs) > 500) { - const bool planeFlyingStraight = rotRateMagnitudeFiltered <= DEGREES_TO_RADIANS(servoConfig()->servo_autotrim_rotation_limit); - const bool zeroRotationCommanded = targetRateMagnitudeFiltered <= servoConfig()->servo_autotrim_rotation_limit; - if (planeFlyingStraight && zeroRotationCommanded && !areSticksDeflectedMoreThanPosHoldDeadband() && !FLIGHT_MODE(MANUAL_MODE) && isGPSHeadingValid()) { - // Plane is flying straight and sticks are centered + const bool planeIsFlyingStraight = rotRateMagnitudeFiltered <= DEGREES_TO_RADIANS(servoConfig()->servo_autotrim_rotation_limit); + const bool noRotationCommanded = targetRateMagnitudeFiltered <= servoConfig()->servo_autotrim_rotation_limit; + const bool planeIsFlyingLevel = calculateCosTiltAngle() >= 0.878153032f; + if ( + planeIsFlyingStraight && + noRotationCommanded && + planeIsFlyingLevel && + !FLIGHT_MODE(MANUAL_MODE) && + isGPSHeadingValid() // TODO: proper flying detection + ) { + // Plane is flying straight and level: trim servos for (int axis = FD_ROLL; axis <= FD_YAW; axis++) { // For each stabilized axis, add 5 units of I-term to all associated servo midpoints const float axisIterm = getAxisIterm(axis); From 92d8fa6e358345770e1061d981365eb2057d2e1b Mon Sep 17 00:00:00 2001 From: "Pawel Spychalski (DzikuVx)" Date: Mon, 19 Apr 2021 21:41:43 +0200 Subject: [PATCH 198/286] Fix merge --- src/main/sensors/gyro.c | 4 ---- 1 file changed, 4 deletions(-) diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c index 059a200a31..911f9546a3 100644 --- a/src/main/sensors/gyro.c +++ b/src/main/sensors/gyro.c @@ -321,11 +321,7 @@ bool gyroInit(void) gyroDev[0].initFn(&gyroDev[0]); // initFn will initialize sampleRateIntervalUs to actual gyro sampling rate (if driver supports it). Calculate target looptime using that value -<<<<<<< HEAD - gyro.targetLooptime = gyroConfig()->gyroSync ? gyroDev[0].sampleRateIntervalUs : TASK_GYRO_LOOPTIME; -======= gyro.targetLooptime = gyroDev[0].sampleRateIntervalUs; ->>>>>>> origin/master // At this poinrt gyroDev[0].gyroAlign was set up by the driver from the busDev record // If configuration says different - override From 036e86ac106d2a626f373f3a582c4689c2901094 Mon Sep 17 00:00:00 2001 From: Michel Pastor Date: Mon, 19 Apr 2021 23:42:20 +0200 Subject: [PATCH 199/286] Add white background to StickPositions.png so that it is readable with GH dark mode (#6854) --- docs/assets/images/StickPositions.png | Bin 712327 -> 1036130 bytes docs/assets/images/StickPositions_trans.png | Bin 0 -> 712327 bytes 2 files changed, 0 insertions(+), 0 deletions(-) create mode 100644 docs/assets/images/StickPositions_trans.png diff --git a/docs/assets/images/StickPositions.png b/docs/assets/images/StickPositions.png index 2d45981c2a0998a9661d3eaef4ed1849870ea264..98281caeef3d56ef02ed38b58bf38097ba0a676d 100644 GIT binary patch literal 1036130 zcmeFZby!tz(=Wd1?(R*ObZ@%5J0v$qH-eOOcSs0GcS?7Ngn%>((jka|fKq~Z*7jRI z&w0;#e(!bt&VSG1lD*a)bI;5@^O>2O<&M?TP{cwfM+bpGSjtLrIv^0GG6;n2fr<>Y zxWtyIgFp{>{Pm2yb*y~B?w)RT4$ihMEtwM5k)f-?@t=_5m*+JX}Zr>#ai3pSP0leq6k>7`!EY$XWMJyJo*% z>^vr&tLx{T&_@5@{#C=z9;Ty)FZbs`@4_U%)Qq*`znI@g=?#{M{_^E%@7AqXcA$Q6 zm)3@bq*4F9qHBaY6R;>Q-`o zz@p-*>yMrXfxXvPKc^Fyn}>pQ5||@*Z-mOP$)x5sHs^NZC;Mr!dQ)(M)~?%2F9qOgzhv-F&Cj_;gXiU)Uqj{_rWp;;ESk!MRosdQxE=)F@0!?O`oNXP1dt#T;InT! zDKRc=nkH}{er#%%NXNLy_B4AS&B91tk|0v{X8TlX_4!rHsX2w}T_DAzHLXfDgU+K+ zCZ|j|!R?AY4a})D70$ASITfyw$y~?!vZa;k0k^gVPxiXDC4UN+%LOm4-pi%#&n44% zitv;Zl_iN1BiZTCHR{ElTJ92^l6{tR>0!6DQj=z4Tu+38YDWj%r1orh7ctr1o+#71oBn{OHfv@ywM2x&|ieEPs64@YqMQd6;wK=}; z_3W>GK5_H2)%7f3FYnn&f0L(%>J<+A)s@fftLmKhcDK9h=&V=*RT8PeJ@zASX@oyb zzZkE^_uo_5TODpZfBRO(*rVX#ye*c&+-_tuMac`_hG5rEBc*8){*G?!$(@(gHr*jGM8N2ZS zM!-Jp_4ZOwf}#YYqaYK+?n9!OT;FW>O4M{~==bKRlMKd!`*ZB@!5sHF^tPt$i5|^+0treqHOtw3mo_3NkR9Xg(m;z=Ax7&AK~p_T(+$kQ+)S23)-v9cYCT#cp52?C6vNOIR@}Ez&K3JoiKrSs zJS_79X(E~@Z<S z>LbYnHxr$=f+|gj5#Ptt;t)@9Hp!TW+i#`$4wCNOW76S>D}m(CxBFsIE-{KHP~#(O zWBH0{R_HK4I%j5o#(X3A>ZMrQP)G8!&wJXWR&mQA^+@$tTVMMS^;ELPFZocAX@*n9 zmyO99Q#cELW=)6ul&V4;m|`9hg$wzCk4L{^wk$TTc8;k@nU#GbxKc%7ITW7Q{Me7i z1@g^ojd5?Sd%KHhGbs>};v?to6Jt>V5=6rHs1MB2#?sTsC@+RbvD+$`<%hr@&}J~? zo&`#e5y>;fQ}Ue@j5Vx)LVJs~I`hTjX>i}|!83@e$}wq-`iC4j;K6}*tioj3Ra1_o zE3-k#NFz<2s!x&@EgwIbZsg=&6M98qM5U<2?JW%DK@+fdH1d~uk|xZJH;X*;@{}cb z!UaD!^ZLC_XmG_y8qVpP$j44uk?`HfXK))g#M#mFoAJtaJh}`47Et%v!A+t9#H}{` z81;p+u9~w_>{{X`Oa(;rhuldAxlZ=g6RcS=T+cX5(HX@+;WA=ux8hbtoDwUBxR7M>R~ zZkVyI`|$N{n0q?+yM$QNO@NlqDVl$1wnR*tK31MYbJJZdOIW^dT6SBvfF}W_at_GZ ziYca_7CUE9if$B!NT+L>31C0^` z-+7bOFU|F98YxZK9sF>%}LOmznOrb4*S{m-!S)n{_q%If9AZiX1_cf|;SGGOf>NnY#nFNisaUa7*VxzG4w3@9?|AqaQ#nSJSX)TyI_WL% z*GQBaaOZl{zwW1eIDZYQr19-EHxhIAk)hrTkam14h*uB+e@&e;nP5V}kh4M7kDwFE z+6%W9^`_v0L2-uahR;c#tt>W ze0s>#4ydik6plzsH3Iuv#36N#1023Ky%M)&k6cPFbJ!pkkBQU`2N8yBNIt4;B&KpK zwIC|4Iej6*P?$WDLAG;^+VsVN#>j9f+Q1t z%#XoX2Xfa@w1S^#Cs{w>j2P5ojuwrMjVsrW$Afmj%o5<$BdS=M6mz${W^C3Of>J#O zWkPbbhNeP89p2OdzA?SR>>d{xlE6{h(rtt3;2|GT1ZJ@SM%`(Ua1?LDYpodS{l1G<3rVs}=z*KbGD(-b^mt zYpCVa{uDp<14Q;2v8?(7#!q;Sso4hx_*sRDp3af=U}B9eD-3e9v$_SQm+VIo@ODX&_6pn0t*9oZPv_^qu z<|iqFUQ%8KsBN}q{hyo!& zGQQ~>W~c&hg;O@iikwMU-&z_cu{?`BZg5A5On%3YuMmR}t{l3C99{fU$sxhwr8Is- zSw?Do${igPE*TW;l^DM$y|*hM$|m;2@9suYssJyQSE)BfBJR@y+o$N`6KlK1EH!uu z%VF<#cq_3TJmI>02zd1A1Gr-zpU&N<1yIh%W;1;vC#X#nAMX!#7k>**T@8;)hVJ(8 zx?u=;vlqqb9b%^DJQVG!9hNc(Cy!C(QW3wUzUJnarK;Wraqp#|FD51Yzma3zQ!L76pCWgkS+r?%ouLm;L#j3+Tt zmL_M1q@xp4nGqc(ldB=OAe18uK4&3!?_xcUyty|a3y&EiA$)x`waRk%U0cN!=O!O@PrWQ7yUE5}bIM?b0gGWs@EIU7&b9}d!T@a!&7X6cI`QHXTedxi(# z?C;_{f{IF8#Iam%GOw|eN>q~6WHLvP#E?m=w+Ye_Pw?|iD@t_IGy)n+`Whe(J# z)Uw=UI2|4ryAAtjKW0>7&0Y=mOc2w5a+CYWSG-E(TFSU1BkNN&@9mseh88@iSLTzt zk3Sn-hH{dfFfdcN6cGC}^o-!Dg;PrA=O#tU3o3W1%OMX$qVs4%Oro-+B)Wae`s&iW zQY=YO^NI{jke4E{45IXV~5--xD@uo>EMt_=@dORcO>(mx-2Ahr+SOxR>!t z<%xpeLs4Yvw|cOj*pW&w#B$u5C(hW-1!MJyH11BE$;lOE(f1RP6%UVmpSWR`j%$m^ z5K^&UdK8~L!-hiD!-hc!L45p{h`3Vkkf0$5JtTjT7+}xt`Y{Ns!H2 z5|tpiRa(Fipo%wyG$Wkr6wGQu6>FSW?&&(jS;!= zvPa{2^L1Fl&)%3V3I}Dta+||DNroC*3q3Kw@CpL|T{I-{jbh+s}~TYzzBpGN{?50ld7VF<@b=(je;?TW;M;WsaRDG#LZL zg39Xa(|P7MDO6VjPGu2@;8(sZ*j!PTbmpOWy+N+Bn>@a3#O6AMYlv?B{s>m`BdQB@ zLRzh~%$ym}OcGw)ykq=J4yiMD^7%&|lcYkABlt10El0FcjiB%nijl3BZ@~yMp_2;V zn;!U7KYQcs&Kfqw%~8r8LQbl|oQBgjTX-_-M|G~=P10dp`a&kzzIFu#Z5O@r8@lDS zXP}}}M+8$=SxR(e6N|y+BDQiFu|>SxvW)olKH&g z#A+ezaKvN>@zsY3HRQ4Tqt*LM5!8vIhoe9F8sUVc+FAT}o5#>e59uTb5x6yTrFeAg zdfICuim;PQf=Km^x1zQhdEDVu{_{@E&X4th#x5K3 zRuI8?p05W@7fz)dK2j#|042tKT&aRVE#tc_1jUQ4gUT9`;316BCMWEGNW#_)h4hz&1-@pv69wTnAsiYWe ze*DfA{(6jk2zM(MQ~G(RFfB*R3q=0;;Bo_v%o?;`N3QgY zDwl%UM^HtB+crca5QL>dcb_csD0?s1$~GCpjp1Vy7hp8rA9L_8DJ(X!@X{Efjp&sm4%o+8^6`-j9W-hstLkkRNik;gd@B(}KS#2g zq&A7)=%Uh^;+4QkNkKNIV5Yb@pVJxe_L{J?)q&i&{iN)jk${%A0OBmp)lls!h@Bv+ zR^!z*onB5hyL~tZsJ3?KEXkdU6-kysTDtcwSao=J&RidFjEIS) zsgR>OYo52gg#@i3tLhq z7(g^{u(KBTcsk#x3Wt5oWph!yo!@$`1bQOt=jdI^fFVl=j~=m|hACx)w_49kD>G=C zeeO1etM|s7Ex`LiIK{u3;@mFB`PjxwQ)~Pb@quJVj331)Ri{s%Cj1pv?YTctuB`TV zwPmQxEtRAAlhz&v%4d4xGdU4Zb2-{mTSi1)q)oYfzWGkZ1;If5CTrR%40X93#x$-u z{}QW{Vewx2qUpKYbKyN)Ws?@`;e9$cePuppmG%(&TG#N6>Nie-8k0xTloGy@ zJ*jxgj6V)r#z$Mw1#mufi06QhqI2dx(Wsko;(40C?vcXQy@kl$j^~ixWo+RdNg2;Q z`&7%~2HwQJ5IH-M;4P)#wIf)>NUZF8T^HjhBTS^Zz?ZxDj)9c&?!wzqK?z=|dKYZ1 zoU0369xz@7uV8f|xmKCAcwy@%8m+Qc-^I&V-4wl9ht8tBXhe_&`OD$r&0?kSDBQ>l zp%T+F&#tBABw$%t-ncBP@$VX(=HYSOc+TtPjB>ZAb28T)M9_E03dAP#T|Gi)w=`0^ zIhzMi)(?RVac6b5+VfB*pFp zRo1K>WV%^x$XV14N`o5mhnikyC}$JtsOKZHcX> zQE75KVb|Ph3W!;rj3z>wD7tEftwBS{t%{O*BpEkIhO-#MNNytBL3%YIMko$lF8Sq$_`maE(+tKlc`@^>r-28UeeM&p~OXySqV$ zp)C}irj;HQjAoaalFCqB)FUI4-rA}Ed}kPq6t$9m*jK-wo-5n|gJE;r4#-63Z5zhrdmGQ|DbPT98k8h zYDl1uUqKu}K|7FH0|`qh{LI-z(1%vaOoLsZxCU)R9cslLMGz;zvl8~|pPI0?yWzdQ z>ow>CKfZ~YS&e&H%flak5E_2 z3A;a;*eXymooR5Fgl@ub#UIvH9-$*T(<*30GRzT&sEF%#;~M^;9hTM^x5qr{TB$rl zIQ7)}ysLr9CTF)yQX2!g6f*w2gSnC=ZNp>7=USwa9f!8k=7XV)W@Z&V13tqZrd0N8 z@s;+QI7vtB@m;~<9R-wSk}=WMMCujA)Cy67LTw9v1w50 zy4&tFgMSFk)8gFCnS0H=MeMVweDvtC{=ALbT+n+ixf4l6H;<~oLX zRFvxyiRy<|?2nD2N`pjj>m9zrca~l9QTDKXa5q+?p;cQQY?gkFD}$0-8*wg&%D|c! zdgV}3DFsJs=)(o)k))P*jYq22hspj!P@`8xFhT0=TO1c8I(lg+LimeQtWN@ z5>vTZUYmndyyEMZUTojG*QCEw;p-%SF6L0yv4lqpMabuMzkXKaf}oyV-O=c-HyT;p z_VW3$073UgS6sLWQJL)`p&<9ZvFJdj0ivb6(e`J*kVLLW_|4xl&gYEdcS!GQ*jBD6pUv_|2S+Or*@%`?+jb|E|3-P8lcAJ@dpQY{&8 z({gPH{QIC0;c;e_6~*-uebUue6gj=&EZS88?57mintMh{B@gx&KfIP1pHk?-!7i_k z&U=3Gnr>5b;XWc*Iy1!h7qRlgnJR#(jMb zWCOV8&o#hPW)Cu-yw%qRuWPJ5H&YMuY#cZ^}f3 z*MigiAjrgpwlB&Y{6SEm8qw~dMSice!*G-jevt^?XTb~9F|239DY)&b(R?7SG;`4f zDjlcu&_;0Jc-o+>Bq;i#4D)AuRbq)n80xH7@Sy{mDl2+in2~3@zzrf z7rU%MP$N=?IF4CG6fvgQ3k^>MvW_EK+K_{D_544C=H#zzVxD5ep=G&y=&>x#X*c-2 zBwjNdRn?qO(SA;?%z!Inp9y|}EqV5^UZ;%Mhc141?jyT`C^q418FlP0?ie$nGt^o# zKZ*v3pT=M%^nceZEYVxgq+^@cVE9gII)}@RRhTdljj2@&TUyEyo z{F1>N_glHTld}10RaXhlCnac&gVBQ!9w?qSwN=EfX$Y;V@6>}2+Ja&0 z!>=B(b=5j1q6#K(EcfqXep6Ttw_#n_WxZiY7BsGT=U3wa@6FiPOox+5!CLHD5cR_2 zco-6k4+g_e4x+5U=><%!X`a2JUV%pKeluYz>667ZVHQ3EwG0z|U9PX*ujX6}d5es0 zSz0Jg&W{IAN3V$@g2kEvZwP&6FuVvz4Z|b!?G*isezP`q1Lsr8ev;^g>($&xTS6dzC?^pULbQGO0YL`lB*;gz$E# zK3@XG#B{v|xnGp{7GmUoWojuKs!Yt{#!pc${VoWeAg2^V$Y z(dp=hjeNfmD?_~f7(z?T_p;l?HFDuK)mW`tilE4B8_0#KcNBDbND?=f2_stTKE7vU^utJD@!T|=c53~QRt4=Yh7JS1EadTSv0K?oQK z;isqfyKk|dkTx|Jb{*0wd0&hbzLPk1gM5a!sb>G`Lo6lvMQjAla2-xui7ZNfPFU?N z*c_`)x-#&)-t6s|hrmsZDAK1pPv?|rcnD=Ajy!R6=WCqmjD@c7FQ>2Ct2QXLQK=M< zvuhrziR7U=Xt5qzdZstX_3a?zaUGsnnj_p(xR9@PLR#Lwq7=}U&ws49f0rU|z{r4p z+4jAE;h-$MUG`C2j@G$FKKQb2CZ3QYE9xFEV@&^;5i`b?o(%&Y9sXj3 zD$=c{c;jbx(Dcrfa?42085P03gx82S#$AqN@?8kU)=71%fem`s!NGfyg!_*%V%j9! z2D6N<(L2dj7r8!>y><*Gx1toni}A!YRa{?vd?;hTRP|YbRlmCyS&i2o*Pc_$)oK*? zQqD!`TPpCl7(f^KAX9~1c=}BIEtIo$UXWV8nNN$OF=Bnz186fORd64PbzI?(DBK+w zy0i5czj53UZt8y1%$U!0OqvmKZx}%k3d!36jYM17+&mrzg(I_fC#Cc5ZAbz=>B<0sSKC0_w=7txWi|m1lV&k&Q>=}e6^xC;lb7C4wo>C{AKdPlx&3pdgDxq80`a< zW7NY>x;LQ#S)?uL_ebss8cDgU$6i`T?<-6L!rFznkfM&tR;o*ed;Q-J1T(10Cy))y zE+SPOE9!UQzR<1!xuQWQUp?jAT68M4kO(zD<)pwsrmiLAQW6M%Q?#4>T}zOOOp#;j zp*NJadg1wEUkxXX_YZa^>P)h(oip}Y|3<$R$f);}_M+jqdQp%I_2}u!Q6V-J!mt3! z#Z(QWIm%62n`C-+ThWi5rP&OO9(@ij>dCP>uKi?aNgM(O!eZ=SFdj7)fQU z4B(GNY!@%8F3W|rHZU?A(2arv!-wq9$SvjQH=RN6*W1J4B0r zx&TYJXWj$uSTdKnD*Uq0DJt{5=$HDrLhONqs1#{@pe23^ValNN7lS&u5& z|HS5Jj&q-SuG)GnINXwoLQ5XL#XMjj-2hkcs#hIKpEGb+Jwhc=e0a%>e}E#a>PVQ* zEsgLX|J;%a@m95>HyV4t2KRH)<&O6U7C6)u*)%zF6%#6#-BOLsnuA)xA>w`}+A@bq z2sgWG;j!WTRv!EhCwjQqlD88PvvZ+7F2rQT>r0~H{(2o?OJmyOM+3SnA@)+3^~|3{l0Na<`UhmdixEIs_m8ZMGuW+~;2x|%>8}p7e`V>=Dowp{fr~a?nt{;1UZF$?3Kkxo&DKAfU0Z4q*U8^MzRs$9JeUEX$EV;pMp@tZ@QlNT@3vLXL*wRtNnqW*#seluO{0wax zB2>-lB8i1Kdk=G}vc=9A-)qHaO7(nKXkJQ6&rILR(EZ`$DnFc$m6W?=W^kRQjII=Xc=W+R1jHmsyR?k3}P<7=ZK6Qbf6Kz*M zLbjXw`M@k066f}$4Kvq-8utK#;;83ozBbBJxQg)y@q&vhAA$s27^sKRomOY}a2i{M zV?VD;(=F)f{QR_}{G)K)C#}z|L*hWH6VYhE?Z7WEvn2zqQ&E&rmulnt6r$@;C3Vt3 zDvw5{zmblX+G1nfT2*^d%!v$F%?;fcw2AsKxd)N->%h|kdNFOiiBWQ$Nyz(l$wUgv zH*;2*#Xmw3>mqzzza6|6HMiMX8;4gm1MAEsM`ylVv@)F`BVb~LgbUs*d_KB$5}$vA zY8!a-!)vKQr3yqbkoAc2phoUqP(M~SP{$}MG)=M$U2OCniFCB2W%Y$dxQv2sYXCp= zh-x$1TK+IUJGKFiRtcrQoMP_z`*%ET40uIvo{g#;`eqzlA)$Ghx|jj`UbO0mCEUbUY<3W;U`yWs`fgMc5NxW|Nc&dN?26>#3C zRl{u~``Qm$s_VBKdx-JmhpP`U2S?U98;z|-SAYVHr_3vK>PhQl^8lzO_{r?P&Xu__ z%@r5z)+$*izoHQc1Q+2TE32g}EBn{W!@w)Uc|ob-O0T3SUt7$p$g|N+fwxnN;=+-% z@f7Q1@Yo_uiu6&w&T`q$rIdCEqt_#$ND|jzbH43@bfAZAqQpFZo?aZEaot}!LpKt> z6Wr5%)e(T=o)}kbT;w4&@4}a_hPu33jTNVvjxwP&%TwraIZ6e%uFlAe?gF(<1M|dSUy4B&%@a@*`&5US-Kzl z#JcGHpeG7Dqb_{`T~bx9#*NvMP39S~O`VRe$rY3A*ok);Hl z!o$Qz=Rv81zMm+?}(6n!_d3FWUVN*A?WV0wzMn0>4`$>$IGDWSq4_G*8;6iA3XPz z`bKw0KCj? zq^<_BadY9cg1TATa{9Tr1221nKw^@9?p8KVw%%ZC+eZ$r;&flS`{}?AP;oj#0d;P5 zcUfC|2PJ<`TU~z*JsW=~8xbg-qy)N{9|R!aV(V=M_H%J|^@8|`)BU0g0q$YVTy)@H zL%f~D>5SC1z_M-YIU zue$o*vt7OZ)FPlyEvF&dcZsNTspR1Za$tiw(^f{ zUA^glU)0^%$LseseY|X8O~3edhT3rfO#NE<_c@Bn>RNx#fvNG4gNysG8L-j6kA&L% zJKqJJ|iRLH{BTtml7Y1X%8G`u{TeFMa*u<(FO| za&9(0utk;S#OYxCLZEIo4p7LiTLCL!VJl%C9u7MjVOtJ5xuyLx$Bx!TymNCCt-9RNDQc2EI65vU*sx2+Wq2fr}XhQrFvPM8B~D=5sz zZ)GK7#cTT;2~AH2z!R*Tf9DED3JQ=C;ueP53Rwwo*zgJqbMV_ixj96jcGes=)`9>x zJR(r2u<$QZuzi3?YblG<@p5wi)1u{U&K@o2Q=5H&ZOUlV@~wuQp{ z?N5)Bv3kVy>rssBzZ?9&QPQ<{^L73IM(3ZQe^Zh6^!9c0bkg+Hw05+$@&51W{8xkj zrlbR4otL-g6XpMlQ2#bg><@EQ0_M7TKKX5ZU0aVop8fzNXNO;^0)u~n0L04X5A}Ol zJ+_7Z+5$k2e{|W{Te&{61<>|SSNp5o;lD6r8$n(hs1>gOhphmw5C=aml%K;|$cB%@ zP5`(SfeQ23@cdybe^d8zv-9?~^0bwH1lRzu&wy+F+GjA!FY9CdS6+PWZ2=o$=jMfQ zbJP8?yr)YT!sD?kheNf2e2<>~skyE{ABdj6|o{xX#R!?@q(|7n!} zx9PtR`(w7OoBI>MY3;o=eO>?C<^PL;e^5|!u(5UZa{KRC|9!|GYWXeh06hQE2E;QU zJaYXVAOAE9nCJXIy#6$||A#98r2n(X|46_8QP+Re^*_?U|7h@ktm{AO`X6cFe>C_% z*7g6Jy3qfrux(v|M9&wfpGCLPY=GJu*;-Xm4)hcDQ`lRP4z!@TD;ax%Kp3&Ge{e$M z6Lml%inp@5Ja7m|K!_*87Pg)N0)auwa?*N!D@SWjyfYV*RAQn+T}B&^aVl#_!lfOx`C ztx$E(f1LHbbgWsT-DhBU*Gl{}4|9Kg+U?8NT@i%adL7`x2blTe(lbhD)=Y}$>Jw&d zT@U;^b@0BaWCjlCyIxsobt0K~#-XZGx4pexsZ$pBtNpKuATXSc+}{uX#Z3mXIzEih z|KSSJDN?5UKlH(xwb=NT5&xTh75Qq^f4V~&Y`(1bY8x4miatJyarw~py3OT-_2;EJ zSkvv*mt-o@$8yTb%FZ9!IOyowoR`mYHY9gbWa$_g86Dr%Q&m+}r9OK`?BL*_U1LyZ zKT{~ZP-iN1xKu~sbGQ)u?b|mc^^2xQV2{h=4%W<&*Xw_wq^ukp6VuYt0#hINzi9*fF9J%8F4dXZeEm3ewiQh? z^8Mno%~Y;HligIV-%g^C_0~|NGL>pFwRo}3Xi{o^K2=Umj>q*@wBpW4Vu^XLe{LF_ zg+0K-+1V(KqLr0Z;Q2Un6QG9cgOX%xfM{w~7DB3h zWq#-P+~oZBxWJmp5TC{ z-JOk=G&Y_ViWRJ`u8vJj{g`|geA)a${_13{(>A*0YO$=k<$)(x5Z@EZiu}hWEl%$^U%q@<>@Z)Rnwp9i zP3ajE39@(gypdDDR|V=1&AhgB8S_IcR_1)MA(P)tlD28jY- zfJlS;hacMACbM~DasOr>xIwctRQdB+Wy)(>oTRg}(_=f98GQ%b?6EWAxdz4qpwwE_ z_awo0{RuNMhY2A}!IQZHsw@_kyJM+5p1WfQfUn(clk~?FhzBT?C?p~x?8yN>{J^Az zrCj|vvG1BsmQ-Jqrp@5QDwZF7z-Q_UIypq~@~k@&DM_@Y283=+-+n8IGtlO^f3q z8?f_C|IQQs?>wbzN|5z`Uu-A_;9*Eu$$9xDqe2oTD}d)L`+)Vlfn!2fvv<0?A_kT4FwZ)r+~kCQnBfUkyuKp~#? zfM@VLy}u)cc-!;4t_pwu_NheLI&2jPlpedOiX$BDtN0W`u7s!zW6UoC@nLuX$R?s8 z`NJCve_|kwQFS}lsXjJc1HuY}EJKSRAHam@m+Bu#1kbu(5OC7z z2TY`+vjzh1Z2!}r!Y3U&#en~0@+ON1T*`TPc<4Bmm6ge7vgtpNf$_D;_xQ^?6%f3Y ztM+{U@<4zOeuuO_vjQoZ)!^G>kJm(oa?b@_BD6J*mz$YS`fgpNK_DrziRtO&{TELq zT>EZ58uj|OO1()nZnk?}Yuvn@Bzh79bFjJj`AB#RnBKNulUO8dy^2v@c@cArWi@Xka-rsZhIhb zCQykAu0Pq&2i%w#^UsC;`go!G7X*Xw?S^CUfSiL|IztwgG@;N0Y5{Pk6$MlFt7sgO zVQFh%J7)op%9aS~8nHY@dVKUA4*-437MBl#fcvqepjS3Faz|lPkuNVVd(6s{$ZKe5 zIInf-bN{v8pPzwIFdYANF)Z*+i_u4T9@Mq3#{qt%1A|zLd6E4*0s#9Ak5*b$Nl&?m zxy&g^%-p3h$#~^RGhxxB*!Ro&EP#-tLat_kKr$rrmplJS7fkneh6NrBzWuNPORJEy z--?|L6TqzKItozX#_&v$tPQYNLxpp&R3@6*pCw7;V|KQH(=#cyqOh>Aj_-#xFiRc! zlk8s~zmrD;*kFqA+y(X`5e8Z?2{wjc*qdeU?@(*eZ%(RbLLulJV{w0;#p8EsY4sPC zzdljy|F+zrhk)4HT3X;3&+VK?1$gj)#j*kl_c!yNl|LIqp+OB-+26~uNOE24dL4mA z5bynF1*rCZ#a9p!0h$CR@wU z&=3Gd6x-($TqLkBqx$_ijr?a{SkKST>6n=nacvKFk|mSy=oL@_a$q>~;F~>CUZ_$a zkf;q6%B-|F=$ZvxES`-@-fNToP7Jso$eR0yx6V)H&#gbSxrDLM1AlLXX%X2^w^3d8L>$gXZm-K*zEs}o^g}4{A9kUA+D_@fV>_P9PPgt{1t0p z)=5Z0f)S3s1yw z1qqUPU#VU4^SToT^uWr5goMLDA_J?u&aQWoM-LZkaB^4CF)(&~`)?nLUB0(L$*mh4 z+%8F$h-0tsfF+?ghdT!w{ZC;mz}5ibm&bdnX!&qtthDp&)g~K7=Z!vbp!UQ^LPBb? zdKG2VY^UA>gu%0;=4lh?ze{HSn`E#u3JHT)4k!R4Qek!&f3`D<0hrG9n}hd z2C7HJ`EvCVfG$vorhW4$t*B50YC2d#_EvZ&q2r|Mv-R;x>)ByV!w3Kx*7tWedlD0{ zEE}ktcVJN^^0l-LK>Mu5;s=|jrzdt-lZDrmz-*mqyZu_155L>G7`Di63NM8!K&IsJ z@yaZqF#8W}$B+69_4EjDZf<^6nFe)nuj5HZfm)KDi)$MGugu|3@d{=B!4Y1Mt2JLm zpc*O$EMZ8gP-r8NuUsvM4sh5_j0+%bhlN(*kJ&ccWAw=n=F6=u$^pZMA-+8zshQta zB&^H@>RFD?4{eYXjtWnmAJp4M7)|c;Du3*J_`$lkpT7lS2A*ow5HWe&*YUlZF z$HGFGXTbe;UjfI37@+P@0MP6g_Q}buKK|g+6#V16QLCeV&&|i@XF&GJ0l<-M{6D8A zzk~@1Py0KBWyF4(uNbXq=pzT>*Gq1ESkXxR^v(lFHb5_6j$ruEM?dhYdn2(V3BpD$ z05+%ql;uIcA6?DEGb^L zE?qTgvDY?xyle{;a#RrSn9mLU62PeeaUz9K{pnZO@rn9#!9-wt6@cQ@=6$186R_RC zjz7+K$9bHV9>f!aKyQiwb&UYk3F7ZjPpYiq@6#k(J7D-3kkCQ^JQTKK7yx1}2`vyq z8HJqRHwxb#HJ1RXC{Z=f_P)rs8QGocn))~(XBq*r4{`u3*$j6m#QnX+zE1K1y2ipt z_k08#pzOUIw3n0Hf1AG(Md1o7)C{h_zR#)xa0FKJ13~l`0MR}a*B zBdT1zgn&`BD6tyG#mDcQbbm?8vIzcPWL*b5)@%EJ+Do)4LZw0~6jD~%qGTl@WQ0l- z*;Gc5(ynmn1`<^Id1OD?G1B2NU2KZCzuZahsi?D9Y>eFCi6TG7# zsx+zv;pt#Q&M7KL3OJjEMX7-29aR}86Y^BjdPp$#?R{;PyU)Cw z2uA~>+uLR;t{4G&ELrY4nsR za^-b;O#Jk!NNtUdnL3xjXnDys-gVCL=Z}=8NBNu=G%5go6LJ zm`H50cz5yBC;f%8X^-sNDu#+|+%=EBD!r%Z@7Q0yx9JtTO>EgE!|bTCva*7A@BG5X z`yO1rd6RwPvmp~gum9WDd2anvM_lg$L?~R}V6edOYkWqBF9f2!4fmUezlQk(QF+yR zfXmQ{n!g!``7U%x$8UGft(2CQc1LZIcW`ish!{H5^;H}s`rr$%S+?KrU%z&tTlL1~ z*AePRp{R*0*`Tkx;gzuKP?zJE5DSu9_S{!le6@EyV8^;$;w^!+lmLKRscP)ox9{{n zTl05u;O1-hLf@gU99EUxyZzhZGdA!gWqwl&#oTl^H6Jwnv_Y_&4|9&n1{C|M@KW($?J~W9= zc^w^{7j4AYQBAB)dF-TK3Mj$TKE6pLgm%2G=3u2#+$%go?*tTsO z+IVBtL4T1I%GV%oqc_<}Kr+B)jM=yHpEdjai~GC&;eq2ZAY_|BfEOBC40U}*TVLB{ zbLjc$X#@eiDz>!6KB3d1EbQ3TpbLEb2}K%;>CRJuS0GsD0TSs&9whvgIauei=8P~( zX>p+C*an02m-Q3lgE$kSOU)hvdfI-n&CgrqFR>sYvquGmL5BB#UR2=he@uj-gk_?K z*>J{osl-7t*ws*6=rhfaBcKnma z#F*JuZ#1TIIrTL?y}i_6%ygsyjnSyP50ys;z2;p@02M+n2;%OSfTQ&5|9yT9`%C^| z8wnAR$dsdwWaT?3 z$(zBVHO>1cMk=!vU=jFme>ZgP&J&K4HY*GyO-D^m9C}^yx9TSrZ*NO(11MSmH((*% z^ZVyl{rP_j$7<+cj-Q;{;*_IA4!&YKb#Yed^LJ$BYCF z(l`I&!%)5W19%>y)nDWgfBd#}cbJqUmPT!=axyYUr^{g`PKngFtqJ;RA9w<^Y_i83ATMRBTCt#&SSGCB zv-O7Pk!2~h4*P-eH>RKXzViRQ7U4f$i(bmn<&}mmM_#lMu7}Jsb$qd8f0^d2tO@KE z1yohq(D!5Udb*pqCbl>ab}R>5VpUp777q0CzpOqHj|E}y z#~$<(k9>Q35R|iWYQSa^ekR5SuPgsh`y->kBrS@m0 z;ZhjL2jlF}0wrG@;Eg)5x|jRXu>mg}eC_nIe5k9=FTP(}kgG)cA9 z4?;e8tI=fsfLs=72@)+?B)UBRw|@@lI|%)??hV#$ARc|4_0pg1uL81tF6H}%$od;Z|JrO*d(4j7y^Y6M}fo79+;#J-Ojz|YS_!( zVrj1L-@i}(pDx(2>5m28sit-*Y-04Qa#TOm2_au!U$1c+DF}qAkRs3uEu??$i-?Sz zjaK+@5y|$#Q5m*P3ZJhYgG_Njk<*FiyM02|KutkGf#9UYTF^Mdf#H4`G9aDzVEFxS z3Hn2MYG7~01MF)pU-#MmOK}(%5Z0BPi6Xq2l^Zv1G#>N-63vGxxRD<52UI!jkg}rU zVpgeHDd0H5Q8zDE`tiD4S|;L)?h2Y7|G$fSe&)Z&$QwQrCkgjgG};JB0$9!*n`r-( zw&~TyrB~lC%k`dnAt`CI@e~FlNCE9}B|6Y|`^vceYhC%*RmHhz z(v;~hsj9jD&yjfOr6K>&P0c z^QvGIJ*-<6yRUTe*Zu4G_JV+B=S{jZ*rNz1x zz>;JFsq&XKnSe9Rh`)Zay{enb-)DEiW$Uts^NDO=Z)fQIEe-No{ClAG57!MoV+Hmk z`sY;D-_IM!b-Ng z8>rq2op}AFP>y(m6gY@5#T#V&;`!(^{#bf>00wmbRd^u}LYVGTTHtb67#CU4Ah)_H zM-2oFGq-?_4#ahcfi0yw{8(0WQ_LSkCV##n$9%OLn|&@`ym+W8@$MR59&N1hFV`K` zZvor|6lp*LcM=*OId(S&Z##K3s_Z*}$cff6KE0=hmnw4D%8{8WEG*oZn9Foq5TX3V7(2JN7mMqbpm>Z6_wdR!F&#Iw8>lX6mDRW@s8W#$& z#Uj*J0fxkYbbL@-Wx~c2QU|dKA0b0PFBfKn2#IiEu06_xw&_UIalBFKKJu*OL6McT z8`rl|g=ETt;KyzO3kXNTf__a)h{%B)TA@hi--K7AM_S;osy3uU0vVf-iJZRO>(#Ro zM2uf}ku?N0NUJdR0?u0FOCR*T+__?M&d%v)i&%m6{7oRP{R%xm9g!I8D+S!X{{F^= zCF_0hj@>#20d5u$Y$m(@a!}CX^?FI?DfqzqObdICmJLhVMBWJ$&hh{_W|}+GT%6EWoqY2fb39 zh=!d6b~$>sSe8H;^eb8eoWshCXEr9$7EHBmI`u^kmZA^oj(y)tce>TON8@GL7Vp%pCimmJW2(Lq!O-@V?Q`TqGfR{m zAvlA@u0Q%pcl$%;5~EG5xZGX~v8qbb$!3vw@@5(*u`OP#y<6N?xC1=Ei$x}+28AG2 zFPR^Zb3UB_yl@5BRv+@aI};{cqj3UkEB0BC@JO2QMG~waud8z4gHOEB3>y)&IZx&| znfMg03Gw?{A9jx5(e$~?7Of3EwgZm0Rj{rPUiQX{gace8OHlP7l;JVn8+KL7FEcZ9 zqpLJz!eXiSGLNunD0xI-px09hv)1o_`ZA;Q89Ti=3K9_`wi9`V{0Jzg_^}Ku0qnho zEntvMINN?`A6D<>chGi&kjn+Vf4GtFebKfX+KLBd#CVmn<1zQ@=uc9{cg!8Xj^l)A~ z<1aZZC#53MI5+jj(~pg$ZoIIT>?v4fiyVqp;iWCNp@w_aP2lnAna|zMdjJl{!S5gL z3*i`~0vO%9bEo0XvI#hni^=VhlM8jIu_RgP)vUFl^8j8WynTXHVcw8tv_wF6-oiDi zysYe@qhdR+3HTsr!;vb9x3=YL=y3`Fh{iM@KZ3YN9f;}>O7?t&N-SR7)LHuiZ2RCq zd-aT?EbO8Jlo=rZ3fnovlsgJWRbfO2Hs9IZF9Nl%zJ!N0lgl##HD<5CfoBqRAY+c> z!(XLOUajtFu0Jrt_dJ`Rkqj!q+GoADI#I-~ea(Xm`R)iXfgxPPU-B$CPK!h6M9re_ zrQSZ^w^p02E8g4em%ZM=V0(-D-43?ol0FIOxNlFN#Z2Pus zmHT2z=f3llCLyBo zDULEvRtLno?1B1|L(veMm*F_?cCN))%>($inyi#gSQ)D60sg^$q`X!i>3v(eTUrvF zVO?`*=&={G!m1CY2@n+6kp3+^qHHUqa1%gj&Wp;AA!1Upjl5N%qn}b|52kVW&OK;aetR1 zNF9`+H0ycxZm(OB3FMdib_Wu&$DKbI_C72~&_uvPk^pGr!76gvfnJoO>=A`~1%b28 zE0~dSveS>cYyJ2@3Mm$6C0;?ycy(v{nMRnS{&Y_JrVh1i^jHc}(on-n66KRz-iR_f zq|INxyn>d#7`0)`!>`{U16@S@mp3<$oVI^g5J(3A%t6SD-Ds_^Pzcubbw@sUkl*n9 z%smJtzfdS`Yjm6?e`(K*7Q=tyzr1rvnt&Lb-}O0np`kUxPquAwDLT=vVNPE{P}|@l z_6D)@XgnhSl}IwlrNBIK*kS&?G=p}Cx#KKd`o!dsB6BAT`)`-(0&2WfRAQsBi#FUpsIZ$Vg)0Ku=MQ2NQv zqrD>BcMGFe#1uVHjsO`9R5mD1=3NKCECc{^=IE!zPNnZ)5k_moUBD@J;$HLF<|9&I z%Qt3r?Ldm>=?f8s(Y-9vqg|)Wm!KT^T-cD#3!5vI9NG|r3=OBOz7IRG7XHFq`4BG> zwy!+)erH9pCCqNGvGwawEg_4nJa2OdhNtrWPW<3m36DTo@YY!y@nB*FJIgO|N^Vu~ z=UguRv%0u(t@N;uxzFMTp}`vKggl5VgkcvCZ0W^jgUG-2*iRHOKa?!Re81)Ql&`L7 zF`w&l>+z^KPE?^Q6L^C1e%5@B;};hmoE$%m`Z6%vC4_EgyT!t7^TpqgS5Q!(Y(_Vv zLTRLD1x{;dsjI8sjSreLdv+svAKN-JpS+Mbw+t~nSiPHn?fV(b7u(wp&q)ad06dt9 zj81o%=5Bg{>xyhyK}n#ZA8fCBgg7?YuZB)$Cw|n3U^lW-!3HAN<^HQA25ze>`iqC& zDI0B=A+4lxi*{3C(7JVM@z?}Pj=jn^SLsLrO8UOOx;CFWpT%t_^RqSqLrz~WL$O=4 zA*1~<5*zLzB6IJafV{YdthGaM07skGN=F6ET`*A#m>RC=&uNQ5OFp75ocP9x^Y|Yk zNv@lI!tx@U&`LD?CDJ1${`_iZUV4bQ476{7g&U4z`zwF|0npTzF$>{xy{J|VaU>_Q zRa-ky_l6jO(Ny_>SJ<(oE`q`5+gBRwz01xsPIx45^>zIPz|AG_A04}%&uF3?@ssR+ ztqq7z3b0<9b==O56eoClO3{F-!3cJesk!0VP~+@j!`h+lCK91+OVz}*eFs0TLp!+5+mx>`di2NtcyevMBv;PW#rC&gs%0rT6DZHurMutie>Y>S}R9nLCloX+*OPGzP zOYul-^gPrVc0vq|jV2dPPeF)i-eA3lN7B*StsAhTXIGoLlM{o2%O+x_b@)?iNyPQb zSuTUGUYwoNL>rLwL5d5glyi4qcvq70$!=$;eZ@Y4{eT|3;DGVL#_s5aboA>haNR#X zk-F92>ad2@7)n^w=CpSRY2F?5K-fnPsDFKfN+Yx+TJkkf=$q z;UZsGMXniWfkLam!oG;?N`MmaFS?@@RNjAQjStTUoUu0%@oIw%-u2*jey9qPDIitP+3(yFF;SZGZs(kMOgYU&pD9wN<>#GF!{uq^sG|ll8r*GJczqZQ( z1fYW&yJ?f<3FUJv^gvv%uw8PfkSL0f#kYoa=%H#NF2`BD>WW9*juhkUbU!)dX+yM)>6M#z+Rn#{Z=R?Ol} zp1o7w$y&XO3{lk5*byqIs{D4v>T^3*%?MRrI5lrl%UF}-gW{#pl@jgA19hX%JZ^9O z`K+>E!sStQ?Cp;ynEo75F^z$C$);g}V*$2vrS|JE0uKCJwp4|SAb+0os%R3YgyS8g zX$n7n+>dhoX8yUsvBjT1f9@F=(1Ad+h>L4Cx6TGhNdq9Ipp1+RETICohnVq;sZP$$ z{DOk&ihdl%-yY1}<~EWvotfhyKb zzP<7K;;my-8HrrbKuU3z!2%-@vzu4Ib>7!k1I9ZV#>Oa||C%QjEN6rQj_kTE5e05? zrJhJCAjPzPggq`NHcP6Skk3-jdq3d*@C~#D&U1{0%Z4FV8hLW9&+B0X(a8Dht&@--SD*Fr`qupVYT?_rJZy}K1Sq11z%}^y#&gu~ z9QQ-;JR0WyY-Z+n^NVep4vayby&-Jo%dhn?U{zSH>aJZn5N@GKaeYT^K0I?yy31fZ z9g+hF4nY1?nm>QOR}U+PxSbqorZxa$AI3et+j#tBcR_IE?=Pce-ha%8Fdl0|a4$oK zPS)y3ZF+e}@24jg{_^LlFR=;j0OJy7ukGDM_v-?zI6&mJ!bo15l7mJUc54J%3wAYo;I z#XK&q1gJ1k@S$mj

8lA*BEK)fGO85ip!@XkZ>C!;6LCxZ(Q70~Pf2Iy&OfGUpc@ zNVIEF$pS;`>FRoqX_Y?Yi|5V0{SgNjn)Rj&yL*qL)aIEl-;F_j)=NEh*AVfkX)|Vg z9g;v1jt5%&o^xu-VdgoXF5vyeV4qV11BzZ1IPn4Ucp0*CX{o6+hPCxrgJ(y5_U=Dz ziCn?I4On`n0dX809AywRWv%9NO6a0oJozX95&r>D&AucB{JuKLp&bwzahh<9xT(p@ z$;~S`An4q8s65_aKh6tN@RJ2le98dNn}7U>3l3(R>@*!}uqI@Va==$+-$!;VoD%N= z?X=_cL@q7gc1`AXF1mFy9tO@_8wMbzPHspO;+a3dyZg38WLZmqB_rp6{nvNP-Qd5# z+q=%@H*^41{{viF-o1PG&gIymj-&Sh`(oDg;&wbJ*shuy8dsv+7AT9=i9G}c`-o_Xh&I}df;UAb~)ADEIQ5;;?Q zYz9e;vcv|Kun1Lq7b<=)Cr5@H1{dG6Xm&EtcQjGKlAU_ZYtxROQ~&#H;h#(L1LXew zt<2LH!>shJ4(*4O;=s{99`UPp?Y$(=vLDCBAGXB{?_^mh{l1qjl|xmD@maWbLBVkq zRaGr;hs}1)NGNl8hy zmOfxPW%>l=r}alleFbtA4+;vDu($oyyB+{dJwQ+67ZNHdlM26gucfW+&1t;B3$F_c zKj8J2Wx1Tpg2>M#H?k1tpd4p(*zJW8tbW?~C~dLeO$(JoRd(&F@SHS-svxp2N^5t8 zzDIU?*7Sl?7|e=AS7^l%t3*Ya*(^?M7B*7Z=x}e)?c2M!9eVZ+!|H(YbCn*Nm)F!C z!&4?rnsj5UeKhQYnN8w&3W}&pWF96VzdudY-N&}x{r9mj1iV-L zoxaDv^*nqX_5>1_l>vI#kudC1+Bf6^tXhH^Es2{yZyxuI88bvTgOyZYAKCD4EtFIp42p1MxNqy9`4?XlUeiV;JMnClS5s`7+A?SNvMY}2Rr)KB`b>! z`l;OL@+imi6-0L`DFyiZ`@?5mDCyYcDZ_MZxW^3V^1_+jM%Ux@)4~x5RDwLi%```O z*RE^$qj1Bb^8L9M&h)f!KxN9c)_JIYs=;`VQ%|$d{S8wtOk#L^0lo3Pc(DwsOcXSt z`R7mhaY@JH8GT3Q>OBUABC#pv&*mAcuVHnrbr5QHt~5r8naH4`S1m12z%CG={8|TIiNNa5^2X)}k;Z!BM_Y@vSl; zjAQxo>W2=d@U^1hjl(wD!M3C<+rxv7@axyFy^OclyZ?-Hb#+aGqyNIwpe~^ervdin zeD6u9p!a7jTytz_5f&U(C)XN;>H>B<6g1)cNAZ>*N9_Z&>U94C%%g*nH*~v5&d0|G z)p=X?CfO?|@JQ{ye~<_66H4|+Um6}7O611hScwyMzoEg1VwkgL&6?U1N_`c`m~Yjp zGlGw+u}I~x2+?vEdi&s2heKOC&1lzx;sVhduz%xnY4=bW0iN~i)nF$9*UhY#t~rmo zdI%zz6>K7GUlgMC`l;$zj|ys2O}n9$C<4Rp0qC0ri~sL0PY%!j?&l~h6-0Ca!?kEV zps|l`ej-c>B%<5i4-y@X*7j5CDee226M}&8;PUGZ^|^sWZNvxB`wX(JQfxf znpRbn5XEeEeK`x*PZMADY|5;>bAn*3BL|#Qj#o>{W8yY{++XS#uA10P3Wddzro2x+>kP4_o2Y;>VPkcuaryf&mU0x}v+cJ+!AcW9qIX$$oHp^u%UX+CqD?_&m z4*lj6-_(EY+2^#KLH1i4^DcN9oBh4pf$?d#U%3Rt7>Pf>;AQqOHc>FIizyIfr}pN)@L-z;9YV#Ri(0}`v+(D3$Rqa9Y=c5GtY4RQ2c z*zjwauJWu|Q~2i1n++Y3<}QFpJ7i_carR$$))J1V;~fw%?}cDB$kJw`90nmFLB?qO z#fwbHu-0{@zkB!2i{*j~!hvd08SZ1wgxjVuJUKdm7jK9kk3 z)*&_d&`q@9$S;AtY?^^R8w2xlnY6ovv91!Vz7yyRryX~J4Cp{|r}zLi&>Ub39T^$V zUnO&mkFW0sSWO9G?jt^ev54EP?SsbLEB^8{vCyv99t%J@Ag&yE^ytx_lVdW>xql)z zPk9*4T2O2SBXmIf05cA6O~AR>*VliJg)4sYwAqP5NA%%h;|$!Rjhs07*|TSnTq}>f z!Jpl1->OA88T$-f>kd*^eDDSEa`5DkAmF|q8M$D(=ftt5S93L6A>iNWGPd2YVMAmY z%pyAcAd8Rly@ZbV;RrO1yYgdk&`9ZD!5%3iR%$N}4~hVA81n&+lXVlb*((RF|Y zZX$UvvV6-iPZ@Da}6fB+ET4grKyU}G3|eHCt~?Q4)cyn z5*D(vTbG`@Y4FT-AAIg7Rc$WL&aKcojAd(50Y->if+((HTGvAq6>}pi@lt790K35L zR%J`vhT{|z78Yh=Vlp$gFm`{+Eq}w)m(}u#t(7fqJ^1yKSP}gW4_4mR#xwt&xMvd8j|G@)mJU%NJa>i`UINdjd z4tn2~aN5CE%quFYgXih%@4uFbk$49-=|e~j$hNIyT6Z}h;11kn|FBbQ(!jvisNIMF zy;|tK9YEnea1xCcIyfCq{yKa!^GAH2;_Y#|@&JvN zYD!+cI%TMXOBI}Mh}m$SOP)OB|1+h8s-XCmE%UHcJI>CIEN5nBt^h9IHrc%Bs?&QvE)SW7d0SCn&?64LLa5UH{vN?#&-_556h$Zl*03ynQMe`@e)j%9dAyC z@h*?!uZ_F_J9hNnU!EKn|I=AzhlYojYFJ8UWEn2X^8Wl}9KqoHl)W zG-Qa@+Kd$RxCA`bg52EPF9|k)1mp^rP7dHf|FVP(;9VJ~pCEC&WQNIiB+kZIoX<#8yyAw8;7$BV(&F>9B_ zVPBLzJ`#hlOAP32;vehBDEMr*@Q>f;%*5+0MMYS@b!HbAMj?1@Y;0QI9ogow1Tgyn zY-WLh5r~-@&}OU8i&9+y-@1&IFlDW5>gd=YCMFiN!9WktxEzd!nmxQrRXli64>tr~ zuxw#OYQKz4K|`#B9W`sR!&jtobf6{#1q2xNXQij6KEwnz;(R5 zN;ufRxA~8{to<+u%(oL1vPMN%5i9^w1eZcROv-I$(@8l61q#*B19G4)$szvX==8Of z5BJD|YF|UR2{KF|o*Z6z1{=d^bjY^|#!gQ^jWvxniRsNYGO z)uCM%-8*sU?_H07{ufX-!Hswqu)+u4ZwZWkOH`XAbT&6fp#MIebfgMO zGXaR3-&4Twdiwjzk?Dva{)Gag1#qbSP<;iM-o@O7?E8%S79-93`&++z8 zjE_1YpCI4e-L2y>h6juPb`#Qw7KD7!u52H08$_RH;;F((A`u1J$ibj7RMZ&6*hq?_ zA1Z8CNIK9&f7=p%EhTy;2r-VgcyOVc5bT0{-wJMzZ*d*`$TX}66ZG(wZc>2TBJ``kfoCd$ zW0~N|Gum}gwg@Xxa$9g*833}DNvW@=2O2RJ)6j7{2H`bNO`fI|p>(-^+zEsNr{)CO zIN;bWFfPmxdV^55qpjiXE=Fh+aIaHk0_TvEZ~Q$`Qeg0t-5(jj@^nQQi#2mHmpuFtZh>7w5j78Ix} zD0pMkg${FVrjsU$PAUA=J0Ov`aiaTAOxb+sKkSk1RZ)=|DvlQ5QJ(dBgLEwa@1-1GS?xT2eejAyUGkrPwGTa;b^Y<1xz>9B{Ts>O?=9xSrc!mrgJddqDJK zc1lVLi8C1S+lNsS+#hijyx<;`UX~@a@E-C?j ziH3-631@gKya}jeBG|7nulb~;3|FjJL0I?m(n*P&P&|b*BLQll^2-kizO_!sGPfE7 zsEk57Vm1?#DYiB+*7KR2dyQ#o=;36Kv;)1n|s|YzaT$ zF{Ukc^piST8EF*={GXn3%=iXCIN^BUd%?IF(@F4e7QlKzz2e^*G&43A+8Tuf62$k7kkwiQM} zIdEoBXz1af5W;rYwOYrH2^RmzAs(OuSd-FbEde|uHPkRA=Ep{uKL91_XkYkl;z7K7 z8aGP!b~H3}edPWKC{8`ZW2h7UlNNOhP}-p2;D;c-Xd3Ily2Snpt5S*O&Toj~reFO2 z^1O>&vsFRBOH@BAh`-T$?s;o}?S^z$6tXElTm&TL^{Xd)$o)ZuihV4$d-v|SnAC!9 ziiDI)$KCTDgfO-5?wE!GyB$i*2Uw8svJxQssQ^@8mB zr`lRFq|n3;0jqCy?t6%QP_`vfi#pdStBv4KBtYt;kmK`Ep2?nsrPgB;7IZ5#G;req zDL`^+l-WP`4WkEB<;MapW@nGV0r>byhcl4@jNsRf z9^IR38Kwx%q7|vSl==(w$aGR+A#I@ZsFCRj3Y=v2W(iuLo#x-$9{;Rbzw5vsG8S85 zZmujj2S{PAH?+o4i-vWf&2nqYiEja5VSt8U2tWO&gc!5KkxC$D%E$LQdSscPkte*n zSQPXqRN2UZD+q#=$L`Z42_MxHHv+9V>+P*qzI{Fl^21PXq)2yr4u04J{Nf-tS zPH6~13FnRIfymATK$6tLhKMPSR}B#!X*k1D20hRX6KYTzMaPa+;;DQGxFBn)>XQs} zFY*(?J!u(u4_+*?i#HnTPnp5I*^Kv_H0G(UVtOSCpz!<)g8W4O< z*9hSlB)^-#e~&_WqEnjvqni**S_jLb718dyfO!o#IXScEmYsF8 zwYS%X_t_5|{}68me>oJc-Hr7LWB$+IK;#+$n@e)|p%>#`AuJ)GPYRzwd@oWoiOc0q5zo0; zsH-M_{Hu>(y%9YvZ`d{3z>WZ83kE9T9XvFi5q=xiTFPC`iZz=yX#(Tv=;{XNS%pe_ z7~)A*kEcVeeM0W8Y%fWN+L?~s9NkjIP*MR|M1aO)zVm@HBU5`B)|PoeL+V$1;7=kO zIO1aM#{~Chvo8G%$5<1q5%*XO{hoA4>dD$HK}7`x?@=+r(O!v(yhkD>_Gu+LDcv0- zZeoLSx*u&`G?|Dx@mn}o@1CBy_x9LehryA$%zZG)A_wTcCCIR)U8j~Z-irKIrYgG9 zA)tLgt~zIOjym%4>NwnLow?Q3#HTpyX2*y@(#!P@2QkcVW&W1576sDVEu{(qAXIU| zeA5iD81&3{{4VUmrsv^?Jj6TXM;u(I`XH3q4ThNyfm^L<3;FsAXU?$Bn1J~wk7_Lx zQGr6xf(muA^9`!|IvyUm=g*)2-tiRoa$?b0At0cNJu4Pl2Nt<_*)o-`Az66Pf`?_C zQ7Mw_+jMqJY{b-6QDXKsX{Vl-(GyE1B;XW30NJVP=j?`?j67aH{P!4aFX|~P>^o7l zuy(w{!Www9PYMqMFPK(sI$GhFz_s~M0J?^`eU|vPZM35)u>mw=m7Kj0l8ibjW#9sc z?;RYPmjGAe(CARN&eeR}5OxP=mrY|GVk;mzjk2C+dDgCF7Y#fC)j;Rxrj;3;X((@% z09bL6570`Zz?1Ck?UP&EshM?k9h-|A5umi`lxPAw!0%oS9F|d^0iyc>Y~aVpvX97I zAOTE|oIrxHtYazQdQY?Rk@{z~R@wJRjOCM!UjH3pv5ru{7GNdHf57!r<5K^bNg_#S z_JkpHL+|mTZrIG5+Oxa=Pz9O=($4>J7L1Z8R|8(ZIidoNmwB?dD`q#AEDvB?w8-@G zB%j1g=-aQXrt>_tuG|Irdj!%Bxri~+fsc%ypN5!rpP6F|tqLXrUh|K%BB)0aVzc`= z?$U_0lQF@AS!HS5R13XkyIBQGhp4#y^7xm|q{WE*Qgpf*6`v@{|RuUq64o zeKEflSstEJ0q;&?53LWP=nAPHgvj4=Q z_^I!0q6D}_f_KxVt3^ld;ilg>JQtd?fS^T;8!h)}m8YCCWX8nItSr(J$piym)e&<< z(*Hw8g*!GxKCUx!*j^#gU~8jQ#z z{P`>8eIH^i(UY!&1b-iU`SRrt z^ar5VG9&^>-jE(2x(BylB|cW@X!!JwAD~}+>(>_rj^OQJj^G^Kbs4m7@4NWNNDOC^ z$GqM!vnq*`w0@l5MCpBeLHRVh7P#F;i?c9v7zOzSh*wnFWm3*` zfc-a#ox@?}AO$DByj_YtNvdM>ImuB@_)2LR8E@+A4`8-nuc|6jeQ(po*Kglawgd3n zRF4G?3b?ef%t{6$aVKDD1vXO*s41DE*qVvH_1g$JldOd}M;S0moH^qyfT!wDm*^0H zHWGwlzUM;-z{aY;Oks0pX9A>z8vRI82C%q`!6xv?zacDr{lSB^VyhWMVJAcDN*9pS&SC;LsclYW<6r9r^)20Ww274#5bGc>MwUSUL_6uj4^K zZsDRKI22bdT$m&F?{$$3Pc>xw{;gR#i^wB~(J(ivA6K*K*7k?t??K>sRMj?!TXXOz z&F@tQ<9u6zj}oFu_D5}PgPD#cyN=>xR2rfKiU52G-?&Y+(+pSBZgm|x=2ugScB7%C z6{y|UZ4D$E zIY^iEVSr8v6!^?6rj;qqGq*H+W^ojvX{qjS^7I z@3Ma1;|TH_(Jo#W7Z>-rgC?nY=tyK>;>Qvh?8q`u@F&RDM&i`(|55j5gP)(DGmOW6 z3QHngMv4?h08;%*(;Y?@YJggLl>`)tawvPQ6PDawVwi#2WR^@Gf zFJ1)2jJ#Y;7i7dw;}#sCXG^iPFA!D15lgVk)wuYP-4=ws_vQSUD|$3kivoVV4j6RT%hWfAO=0=@oAOTn9Y2Yk8bKp|t@K*=XF)j!VxHAcr3U^CQh}VdDV_w-GIj%)bYT zQ*C>WvkY2b`l}eR-@bhtEu<6!Q4atx=(vlPx`0z8gO&k^*7q6W(#7L%Z##j+(>H$k zQm)j+*1fA@& zE9cIgLn4LltSWUr4-o^7aJ##-v}0FJ->I-G%^-cKd$fd+%#E*^CwiqPD13NJ%1!St zl%7^%eg^19)7xX~wry=_Lsqrv$#x+6_kgtDTy}Z0Q><_IA(A?+jIK~B0 zV)f@-El8~IVNAt7PC=L=3%88vJ#$UN6U;N2_DpOLUrG8?u8#oqXh@4l?=4uiJR> zprGm2{=1_O4K!WM;+ zX4BfKz_{3pfbT<~qsW0obQ~bXk1bghfM{@Ow>sh?dqf=D=yqpnn7RAsLvH~-Hh?^u z3~|8nN%1lgte~Wo`z~Im`eW7R&01hIqS+e(#w;9*phUoDC$FNgyK^VRI?{x2Fz6zD zXkKrN{;yn%`mv;4DxlPLPHz;a{kQ3I2g4&6%YH4 zPa*6_MCqn5KMrw|Lu$axU`e|X-Vhf(iQiC3_2z2Wgd&9+^tgM{Hv570eMJ5OYaqv% zPBKcz)PU|08_K}K=+rkbNIv#?$Lcj}9vd-?R-+LS1ccC}`LnCLy9D*FrKxGvj2Y)R zQkYSTK49D+0x~i&q{DAH1lFzF38QZ>vKNr|K%Br~nUv65L=zC3cM$4o34_A*4MWNA z(aO|*G~qNjWP6Yk2PS)?E+0{tKHOYin3uOYKkM9b4h}6`I{EATxeg`5O)S1NH&K&R z6@2J`YD_2CJuzW4kw-scB_$^rg{Dc?CLAPqNfsort$G@7gObKfvD7`T4F4jS7CMm`~XDj+<%;E(5? zWA&flwNW&dvY@=YJV5Un=dK|EbV(85#tT{Ql!3=A%d5vitQ!qC_a`Pn#zE?6O+E*& zAtoz}ZcFZ8rEpO&?i?KZZxGoXLOLN?Q?^Fa1$RuOA=4iX!g&15_NU?iV&Y=iP3zZy&Z+TmTB&}XOpuZK&ZTddvvu2gJG$j znlhsh3KYRe{4((N?RSlnJbf_v7zfTATqA2_bO$XB9Q{5=HYdU;%OstX0A4IwPu>zN z#^9qE4sTJJWGd`_m@StuQOpp1L&#Dmjza70<_&M8j~_dxjsi$~s?@m(DF%biyC6%D z|M-Eg%cO|qfcG!npua^i`@!*+!uzm-tWcP#+LU9E?dS>5TjF3kczPBpM}Sa%;5r&b z$$R^8&6-b0!Gz@?j z7iriDxFO)oTLC>XR>G+#@zLS8AS$Xc%Q-nW95_(Vaovf^0td(8nq!4n;BU||96|uN zt;zM!8$(T}Y%tdOlYYPva$+&ouvD)UB5au3KnP|&awd<~V({vPzOHm<#o~sUKVn&zk9tU1#b{*A-NBYx9n&_ zDatN-s1^vMYE-uA>jen1RUYl7>=KEvRxAiTUIsx!Qz~Nr*$K~W73o^2P2?KE=$|<) z@mEq03anbx541dBo$o<=4PV=)VRp-!#x#%#=Ij3nK?migce)_Pa!Uh|yMbg=)n8mf zry6yEg0g)5IAn)QJq7{!M3b-#y;^v%b$5z~j<=!ms3Y%Y{w`FRn@aUxUz*)s>;C>V z(xfK{l>$e|pPQkgp{I8P$OR)THih<)k&#w-@k?CX@ba@T(teniy%+w~GAYM4F6hxK zq@}YtZn^BxIO#7n7Bi-dfEl!tJfJ3_P~2&RCyoxeHfu{Pu+ z2(PBPlF)nbc{Ivocu&}1i$)^T%_jPZ(g$dYO`Db&DI)tDjfN4T@v*pgH_*Sp@$+Xi?wL}MEm(*}=gk}>GGtu#VIa|nQqxosW;)fRwHOg59J z^mWwkCoP!t;)C(ks6KGGuv0f@I^e+}YG-uS$->n18?sj-v2~cA$EOh4*F|{j!Iq*( zJ-k#)fH7CjKYZ=r|Hjcv0eCbvnZJEc1E<}35GH9f1%GL7_B-fm`}&RrkX!(!gxzo@ zD9EhF^BQuH03>goKVPAkupR6viOvMJ1LQI93T76TN)+5OqzW<$dO#9JFy^fqgP?&6 zvaDw(`4l*y8+&sdUW)b)OK0AAXTjCA{`d?5nhF9)c^z;Awcb=|rIF&Zv4Q=NU*8AT zpiLFtx|yG1Y=DT)3Ye04`S~C9J0ZDE2s^&SHUTu#1cqvXB}=&lz+jWhlepPOF;qsW zKFfaSTG1$to4C7|E)vjH2T$J%wSsmsM4mwPc2%%mOmzP4yU+va!b*YZT{D0@!m*X+ zd7XMY5>* zdGq-WSa2d9uq;&=0wV17s?rF zkMwh+vh)9~1q?gR@e3aR{@zntuvMlK?l%heo31u|_K-T}Kk^LgOG~fH>K=X%*tq#* zhkkC`bR^#39uwG`&qLBtl5r43=29pCKUU_$?xm=$;QgmyUEnZx0HGh>z5D}MooRV^ zI?68mva1afauBoTE-y?+`$Tjyq+!ywfZMmnH%=+P*!=nPUDSrhGZVK0QI~_#{%F(# zJd8vip)oE1c8pP0K+#yt$r+DA{|!ybd8Ci;0yD#75A=9kErG6s^`w*noHfRUZr0TF zbSo4u=<`?bY3rw|d_de=ag)DT+%v>!pcVWYDmnZCaZvl_g85L^Bovw5hR@+!zk!Gq zSp0b43v?7zp7WOdVn=Es@E>}=jL(5shcTfTd^anc^Qn`of&qYFKQ1#o31mib%Q;v@dQTdlmC5KjIPfAbnV31pt2Nqn;S)}9dVYW&Tf!hmMl-C&=o*P@FmlOZ?_1% zLeqs)uY)P#eV!f~z@R9|ue;JgP&S#Mv`v3Ig7KikYN@Yd>{R=J}G>ss`VIc9}RcQ3Z5`fK{!*UVZ&gEZzes z7BL`BOuMFfekFyFVh4dWcmXuLLz%55jpEHnQ((CKHVNw}B@519`k3vJjS6n6=Z0yz zAD_}RQI3~kgoSZf1vWw-yeMl_Wqw2~{fh75BT#5m>a%@i?7n-Tjv0mIsi9OvmW@MM zrm|nxP#lF21qP~vN#=#=IDe+m6b$Sd6`l;l;nzY4uDPKhXXWTsDD#+*Q1-681?>hO zQ^C4~o1L~G;V6*!miG40re2z|R$UsAi6KoCmf|lx!glkVAws0FTJ(0F#i^jT55;-b zAIS_O+yCT?bT0wTFV*-8nF~Z;v^||1W2lxGqCnhJRg#Uhm&9VkjcAt9Ef)$meOpdt$29{{S>29gvN6rg+g5U&!B&_NRvF)f%UzIE$@ z(M?o&?@r;wTtN&J)p-&|xzto@jMxB0Y1$N>7jr~un<2`B9n_2^kJmNGL2y8uv_?dO zQVB5ZOs<|&pX|As_6@e+?|lR1d+Fv|b3bk;?01MztvpD4qY56tS>%Ag{Ef?;{iAHDtko5ek`8M~_RNN^XPP#ep?&X-67$lxzaT2Xj zpD27yz6lx_Xq5|ND24_=0jh6kkix}}J47x7s8Ns%ZwW+L6Ndi$Q&j=j)G;{!k>w}h zZOh3`oq_=jPZ|&@0omP*BpYG{p)%uY1Q#JK=e8N9+2tJ-W@2FxYq4QQo(ShT5Z^Xe z;oTY#2FipZkFUVy8ALKJ)o;gIWMTDTJPYJ-@ggz{{AcD^3pDZ_{6!$g<9ThZ0Wyf7 zv%JN(C;YJgaoOY^?@EMVqSabXAmJw~&2pVj)4RxW1>Q+1>Uz-S%Xjf}=Rvml`zvSV zf2$}z>aI}4E}Yka&U%~ft;EMs1d#_p)F0T7k3Ij@DCnCvbKOY3;dj`5b}fyzm!&n4 z)S_b6m5!OG$JK8s$%J0N)G!5}Jcd?h++4C`A@aVZ9%dC$&kcb@L5#S zak`Ioz^f@ot4B_5hc2VN0#ci5REE*>$@mzW{Q{oT$dATN4j7DHXQibp3ViVN2GD9wMwQa+9fMK(Fz`f4vCzN;?&I|Td_i8_?~6G&U#WR9BA^#)t^=xB91xkK$$mwP5yc1sOM5N>Zu13L(u@B{~p#XU}3 zpwQx-K_2qRWM8tljj-+At%!jNUMq)24~TV4Fbev0YOdVN@bg$g2jUxC_@xbk>!-Jo3 zxBD3ofAh`9ao$LElpP=uwJ8K?ryKZWEmt=;@x(;m%uI=cxzbHdP30IeR`HO5070qx zi>A29Z2OW6XFp7uaUAa-{!703FpdAp$S~L^oohhqigM4WQ~YM5Ecgr_y+or^5Njf^ zIEY$5L?j3*VF^-*anN1bz|7N;v;!03WAc>EIQIE8K2zaHQyQQGvP}$jBYB!NhT>28 zQ%YyRsS01T?L^$TXslFQxfgjzR#tB%n7$md*-~v*2RFV3hbv+4=U<(QZYZ z!k%IGQLW3hmWOBc0SaI;S|oI%;2n%@#J`(Dkl@-nC9UJ?dZ{;RN=on1KKUlZ^wLj6 zVN^!)_Nr$rC|P?>oJcBp3j`~!rHElD(QAlE6yE)#m*zbGukpGU?&n2=8_sMgfOrt9tl z%c2M?E)obRUfup4iPk>cSjfL>RqmQ$+$jR^a}{=XkvTT(xktF-fo58;rz0-@Z20`y zcyv<^epJkzJ2`hykMDlKKI+{>3d5(?O>+!090T$uAvzjU;{T_ta`$NLi&O|Zk}zq!DDRcUb_#(ecrIgbUqW9ayz8R)+)_aBzd zv19*1oK?#qp+i@Et5gAydjbwd%xE11?v&OVNL_`-wlbKO5WT^QriE;=@5Cj9O8Rm8 z(y}56^~ zbRo_7X3K8C6>L$XRSk)!&-@=>X9Ct^-nRcc&D0pgn1+xrGE-zNDN2?RvPG7{P(o42 zURlZ*8Y)W?g_2}nT87D5m?D*AX(2=^$smY2D(t~SCDuQ)87o;7|;q z^sZ#S+t18t#&NH<vV0nPmJ4dtobr; zkwFVzAli9Gi-Ac$`c7HC^QcGVM|M!GJo8cdj0(G5o1hQJs`v z(!V-W@AID%x(Pjm3dk}>nqIQ;cO|bp5Ow3Qc5ki=_568oT2noZANur}B>hcBD&_$_ ztY4~t2(@b0qYB@_D-Q3g5KiHeH`6E$*}jNvQ8{^ycbua zGK@~#vMJ3CJ6-+%^r@g=m+9H3|0BW`*!={)#mPUn8S+%Y) z^`K+FQogt(v#X=s$79&L{`YgWe$@DxTy%?w={HmA9YWEB=;}~}LebAdc-(0n77mcX z+T6KEGDg}Aj@Z4G=_A0>Tq8p$LeMcZGB%%zzwxPgx0kjt-}An;bo39U*scE46<#mCxY|$t04x z45Ba-%B6iNx)J}37_J;EWVl|NZ$^S8@+L#yJdP3KHOzuyYEt9m&A{m|IT{^D@3Yi83~Z&N&M8WR@vTt`ZNMXpAbnzb_Ol3 z`%DPb8#{=(no&scFxA9SC$>9P{P#E&#cgH%7;qdfR4XEtae#=$zvCz6)lxK#v}B2M zUE20vY0}c(^blFOP1-ITb%3FMDs5V|^xC6Gk8G#GLK2AX+^Yhh5HZ}zNTpwgNbRX@ zWqX%1Bj@&Lkss&(+5n45PvHOa&s!&RX0+F-F2n5En(LOy|MR@l>#>;cq}s!a;hOVoPON@)2O+aFZtfjY1*Io@Rodgk;(xgS-2p|D z46E*`e@lz(qI(cQVc@(kLGgPAEXEJ`y4s67Tq9A3?<)B4;e!iYO$3KC4fB@3YpS4( zf*`l1*NXu_#)81`v_%R?)&a5G757z(gu$>7Z*ju>ih9j#eL~K^5O<=}_gCA@oVg$J ziU0Js>`PHTe8YUls5?bRhZTKSygkO?Zmv-*^IT*Y-F!5?M@Vf8XUNqkmMEZEvqjox zW<6SIzb7vCb8Abx-!o+Sr}s;JiVu(%l41nlfe)vlTjzjk)EB{?%cSgC=i}2RS!S9B zEm8)5(t(a7=iu1sBnEMvd-V!f``CMQSpVa6%8nkCv9h-!vE^Ela+|a%RFb5Pzs_z0 zi*1ueYZVOYbIsw`2~<`t;M^0_=P*`5VK(ORL~AHG9ZeZ#cbcWT%$bNPY|c?xQQ+G$ z15ES2jUPn@YgG1u5pe~=2EDk(8G)d$E{F)GyfTg*{PVts39E%PO()nZ#1v~3bDZA; zo91kq=+C(>+CZBi%%)lz*FuV(>=2zI;}^~8jp_Nr@xV;7 zon~dVkw-o6k5#z<{C(<9@DaAweYO3uMJ#K;WA6B0!?ABmQtBm?zA(&wYtAX z)H+J@V(!QtUN0#L$1GA(t%a5%W0H*Xw62+t`^MNBh}}#^sx(1Ehb|tV5_e3<@oByr zHiVbYc!PTf5e+{h6oPs>6UU-gjPRfU*SPHuXq|DM625-zWM4mSUf|cmWqdS@?H5ON z-qq|13p)utC~K}u3AsRy*cvx$nOTQi1x>vQQ~@l}E2IhqPWHTZ2fBAHudJK|7H9ff z)L9_3Tf;`9MAo~%ar(*or(B#Wc4KxrYxeAMCiXwVQV_rnO?vA*)e+xCo@G5(Zw7outDA=sf8@dAWv1W8f>iD)R*Z!WNnO~T-!cU z8B1LNEFeKrfeakUKhR1Ufyczyv0Dc(U4oia>ZUYpdP9rFN?r^jxJXIzX)Ipf`)yv7X`pg63+_3K4e-fYcN@5Oqq3s$YM~zsZn7o? zDX>uI0x_dNRA&8K+xv|AgzhgT6IBGXn!TDzO(*mb@OKPDsu(b(Jb19@&EsBZQxD@j zn|Q6rI$jrEH^1Id=wr%iQAJsn(Q&su5=OUk!@UrL(d0{Ctc>$gB0{3kobE?`d6+KP(F1(RdfBqm4=Ruo?FCEu z(|I-&UG-)Ymls_B?x;rJ>am2w3H!d-{Fj~3wEdq>J-cK>j7l2}z{!oVG6LLn!VyBb zCl+`)&Eh_W?dbpU^Rh=%IXAM*7MRZoJC7i`c7C z3rV4PPuGIZl!ur9N$gvzM*x)h6$-2#{$8kZeud&6#j($~Z{TK!prTI*MrvKGy3ZyM z^a*i#65Ktmn@##Y(|aol78P7?mCh&CSFhKKCe9o2IVLJT&6vgDE~S;+y5F*62u;0i z*N9Vj28rxQK+ryV@yZ}Gx9ddO*+LK8T3TjV$Aw-22Cw$1q`kgu@+J?db0BlZC z`SmVrq}IuEm;=w6Vn_7jX3*A z^X}0wP4yFRh;my%0_lDs+*yC6NT7Ds8Qd<BzG0Ak#d2-hx#6}|I;90HqU z-=toV__yms2ZS?3OdBXW+0F8PypA!ijCzF%&VW$=PZe=zlNwPtDg=(PU*+wi!;|`D zHo)7q@TP7JX#(UW3%tDTV>@+S+P^(%e3^vqz0C9e+`3*jl(bD6NP2Wd)%&P$N$`zu zHs}tQ?VVyY39N1Ql41LF_n0nT|HR=tfZOwvs=uUm{C)UK*j?{}jXysaMdP>l;dS^_ zZdycH`Q=-W3F8(ltg<2u_bp5ZW?Z{?Uz!+zGtt;+=|`QKu1_JqB(ySPtS7ECLMFD9B+SZ6SstS>o&&E zj;- z`=LzzK(NI0&^UQPZd^1SWlGr3e9zZMM|v$Q8Wv^_Tl1c?6g%TtE~?Q<*&!AL5gvq6 zYTXF;E{7Oj+IXd}FVUNDL=!A_pm49QT=6er{$V5Z2pA(KyX!nTlVqa?E6ysOH#lNvTpLEM5vdvFh4J)Sr4#>aY=?wZhtP#e z?M^|Fz20Ys^dh3gLbADpwiZo%UYlt8mpuy`cm+#wRzAsy%{gFkkYn2|a>{UijejAVD<%bM9PMFlojdkn z7p4I+!Z%J1MP5%&6c`qku7cs&zds2B>#_CntZ7GOGS<-!d@74_V0A^<8Orl%UZ0j8 zJ?_BZg~_dbe%qbJzAN~vdJnAV*Tvn^(F2Vo2zk`5EH8Q=iDW4*2$JX}Ox>f-;xpPN znMKGy%xW{^dGm9-cWYl)Yb?!xFlo!ef0kJ%`I8IUhtZL!n!{x<4ubNWeeaRk(ZD%J zm>UUn_R@MsrmDN!4;E!x_cI%x#{1B0dT{%W9b)D>hbzm0qc-P`rg{Iw7tL+CUbi}i zl|;2AjUfwgOt&ag?;-g0D-_ejTgUgKCkNK+6@;p$FS6BfX6`3lZ+<(*%pvv#v*WRqXqu`t%ZzXksat>iNV6gH6z ze(bHnbaE^lDTXzqfWqOZp^UpR(}ec04~xg=ZrzLgmFGkXH zOMxcRGKn=| zzz*1z7%rUi$ZUS&N>a+91R zJd3{{F><-XI;@g0R*ed$A$U$5bKon*`wFz;@L#99QA0&|fDG~-_PHzFx)32SeoT6U zi&0ADfG#V*a{LSF5VjXNU0N|~Ee))=1w!G-acx3ey+xxgM%ahRYDww|b!6<`A8dZ9 zlVg-kNpu=a<_!b3Q^C^ke0oxI8Ui`pgKgJy{Mq&NTDt$6oCCiC(N#$qxL^oqyJgwP zraye_4SBeCNK3{mYu1XH%os==r=^BAn;zDr*HY8(5Cs_4gtEWV?S3+o5OkC&=a$47;ms0~Wy)|N!I+#5 zED-K)vgh*UL6~khvzCi%Pn#}#0&1`Q$Jqpiz}Jna%=`<5WKYki*)(C+&+)4tI6vfa z4=kR@U+!JoKkr|n*$;w()w^~8Ilb|oJ7=5Xt)j|Cri1kCjO{k3)jgIs>PO8jLNa*~ z^O2=eBe2$}&N<0BlEn@aQ-E88xZSCi{o**E!1ZlXj&d?0r`QSqcF-Wd0%3eED?Qt& zQ6qniABC&QUaM>J80nOWsQ?W(_o~clBRay4rtG*3Y+_sWC>lbv3p{?~>9iA5WD(Q> zMeQB#iIAT~N&Bd#db!BL#35h6@$1piw1)V0&jEI4SiHYk!iK3AJnuS-5~JX+zebh1 zb*AtW9yBcRdD}jUQw!6t#_dTK396;9U-x*g{cD7Y^Z{m}RVsPUM|zEJrL8@(cw!4J zEoY-wgs#9%Gp-IOot{N*J!h9feIZW|_SIZI;`QWYhqZO$Wz@Iz8aWeP9zLkX^jq=;Ft7Ja;w79w0^+kRxP% zKuvy^fGO>gfPmz{qVxpgUW5Yf_>Rg!-rx4D3f`;>K4IN=_w|2TrEJ$jWrB`lsu5T# zc6HYyWCXr$Cy0#ENj!!EH4-8zcaxo`Sz9;b)e~;~3&n5q*4P*1Pfe8D-_#7I*w#eA zImtz6qVfgJ*$?(UWqS2KGB|#@G^UtAB&*%C~No*-HqC7L9L`RSdgrx~f zNh-IyF*2!?eAi@I1MG|Ygj`)WXEsBi`_2!sGHMTpX4l2p(hQ6CO{2A69BF^+ZsteM zoHNb&*AP6<)AhMDzc|17$jB#pn}`zN7|@nt4yChoun~Bw1zOl@H*7R zlx?7>hM@t9Bw z$r6UMlRFFT6`je{ruvV6q~ezeP(*O!k_H?{^DAW{0kZm|7orwv^{@iy5%33cTX2QL zn7#yVo-u*34AF7ypEd6)SF+h#aJmP_yY}u~@hI%UALLCI===guvX5B&M!Q*^^SZu% z9*P34=nYwgwjues9Ybp|b%TenBridQfvsKF5|(v2F5ZJ3+ImU#J@W~}y+bNFb=fPG zLgspu9!$U2s&(XW&(w9|RtEm{s|>1C^k&mC-=1Fmrlk<(3SEL3F$$7FE;Uw*vf*Lz z&25;`P#Z>)l!uelG9eLQ0)9dy>=0sz z+UgPDc;jTJT99UAn=qf6Tnc5s!c+4fOn&+DB{?Q?6)U)sVrY<_=I0MCRs%wM;5=>O z#1@ee*<8th=BtMgTYJm2(C(Eqs(c=CY2Ix1=C^5{#hnm4O~dJW598SEy0-bp>RaCV zOoU=7vi4z_Vrz%(y-398w)*!rmgoxRYS0B5BkKPHOH?VwsTa;IODjBCMIB;U2D>l?S4QFcnOT=4#FCr@^7qiWKSD9y1Q!q=UM$x3gC z`Z7BP8@t=*(aH$MFrqURzA!zO{);~E*>!dZdiK~a^7hixiKfG#JE5R-JiMU=OL^uPo#cE;@PscghiH=YCX{`wR(l%ZKz--xpU zgn;LE)X%5xy)vhs2S(Z4Wa;;4LHh&={{mO7Uv4#dU0K;DWO>E&3A^G8VWQl#oyf$4 zGj8G>b@rEE8USC7`$ETd_3Hkel*9xm1)>0CHjq09vF1H5m#?Vi6?oY-!B&gs^2O{n3yUj=4YfqC+^LXK6 zGdL>7(b4e~6R`XpDHMANRXyJo6rpXMZNx+W^_EJU(YNW<)#H)x={;z6+hRrh!hAJO zBne{d<(kJtsJo&j08{Vx(Z!Cj35V+Ks~VvE6GfpbMpL7S*AavF@3ge88GiWuER~OH zACHgw{AcegjT$$;Lq#Apw9o3cOh*g5r}o>*>?un3$gyd)Zh>b;6=-q7q$QVtd!S{+{d;0{O3GP z#7{*x2RA3y9{VxW{TKfw-!r)C8FIBp%fV2r{B`Us%;KZ@@^eTFfFJ3MC6)npIbZqNA z0-0OSkV%9jt3F68(w!d?%k|rypLBXFkHc9gwEq4}-)Dv3$HxxwuulTVBg5a%ze74@ zakuAaYQ@x1n0ePDS-Lf+;gB8`OTCgUCGChaW@hhtUNr1!t{aWmDRBO8l!mJ-&pnc0 zVfjWVG`lWNyS?_Q*1Lu_Q|CE1QL?{X(R#}>`cpvPqtv*rjlI>IBMtZVR_F6ef~@(Kxri4Ju-kvPrJi=PjVV&YFZ!7}PU8D9xquE~;K zK$n(fgeJWQ;#TP>#R~QR?>R%d9Oxe2nGK=pjV&P{`8HP))cz*69B00LFVRzU6TF(( zp2|kBl^ygdny6mUz#U?AYX#>H{R+&P15zQx~0^DhNt<+s{{}>EcKZI%D zL8|PtE8B6cJt4oWe(OTa1O(lIo>R&>F7gEMRGE$|b)L)ai48l~eJ;Lycf#w9$C>7I zL(Cmx*NiSC#K;G54k{jTm`#PQZzqqMJ}7ZkRgZNHN9;o{G#Msqp5#15_f-5|S5$~a zshILmQ4}*|?2Gjx3yj0Xyar9%=+eG6Ha4Q|H`aD*cEF zNVV2yQZaU;(T(t6`YQSD0T&r~zIj?H_8z2?=<}et{nN8y~!Ea4u-hzg&QN2IMxyJifulA@bdu-SWldvm=`h zfBHlxwzfE6x_$q}slvj-)@fZ>Z!2d5h7V_>J*5vFRZg1sp9JiqFiBh+E4%0mt*P|c z({WUu39HN|!lD5>7e5}K)$?S{5dC+~%mkI47gy#o`o8tJ2i6{k7#!T!Jx6IEa}AMk z%5_oJgT09a)^-}iVmw=hmnu_3ii>WOEc73=-!{1U$*E@sJ@#(P3A}iE!K2aNREk%k zC`G8f1x03CwT&E2n$N)P+lLIS^FVy+Q-W+Zz@EXyrtRztbXNtJzx#2P=dgV{cC?Ia zL(S%Bl)inv?jpPXBZZ=|vn=2HrxmT{hV9;c>|ogH5Puc>Ns!v)_|X6iZ@2RGniJD5 z93eoO-YJ@DAzW4C@7xq%a?la418Qc6L<}JOrJA-|_j9jmt-_V4a1vsK2USqy5~W*u zxuKbv%bd)Po6TkzlrS}lbW0d5G7UiMh~Ly%kF0{5^0+bP6@fY)Prsozm?5m!X72<7)#IyUa@Sj>?Lm$?6UE< zo7#8kGz3cL>0}&}j_{%wxTMs+UE_eTumwgZ$(|hfg5cHb8E@~o)dWSBagZ?VDV(g- zU;G3iPoCgL_hDov41fHDjsiaToEw)^lUuh3zQQVu&#yS4n56*&(69aeVJV`BvG9>y zcWy(?&HU}l{Mo@ePn}Xy4CJ&MTmfRxwo;TXA0Sq8g#Mag9_CejW_8VnN5nhr(Qa;0 z0zs4Rim?e6i^8@u9sokwmnr>rpk_4MoW9EbmFOQBd+PyAzyw9-!`j%B;DXT|_)y+W zcCPL(I45XV

o^M|yr~CVg{pB61O2KU5m}C&5^y*Zl_LcSX zUtih_(UJXpvL{p=L6d{NHC(l(XUoc+v)&I{K4sc(^WHb_+%3hYc7sicj194lz-=HG z6KS|)my#4WXBx=4%wwdMLgjP;ee<#l54q2+Y79GhKl(c;zROjA`$CL=ASG~r>GBzv z7bEHoN}S=~D3AQgNQ0#)Co0Q)>rJ5#!kg=OPG=B1lEzchZ1P_;qw(-1^MUa!%V_I}&wym6DQ;E= z(v6k407}ERklMRt#x5=zEoZx@vfEO?90vBn#C?x$MmDpZo9YCIiDvZA^(xB@ST*Xb z6ZRot+A0j&0QR>5I{r0#6bs^Sfo^8YWlHFAJKZUeK22U-=$OUxd1RwMUC>VW1QfSM z91IvNUe~jfoPczOIJFegs0@WUMnV-Ww(7_}`y%^0xo`@vhyMA^WJ7 zmxMrv5uK@;sp;(<2No{eYXL+trk?|dvi4&;{2 zij_|@cx2Ow67zO?tv~ov!xOo^lJWgJ9KS>L-8|SXB5~bgvTzU$QpA=SK_DdLN$}TY z_{23^_usd{(!9YK*E;WNx2(d#$INkg2}k7HUHJ_tjReTqYEwDyz1gSn4i4cN-0I7k zOdJQ4!Z!|;`^3oQ{@;aDT%3uqn)e9icSqzO(DI#|oW;Z`TRUMiIkt_|joG`{{!>D1 zSr&hTFp^!-ekh+PA$X_Gv6-H3n#$`_JJR(>l>xnmIHfpsRmqJf@`^B9v{%UQ_4n_j zz0rsDz{+XVt1aq&A=iyRp04x#Mjr>{`MPP-KXz4&3^lU@p>uSc>Rs%+MCyiOwO}1> z=M>*ylPSEf?zXCmlh*>_xT9dd(=D@%?Kv=|yi@*Vu>oJ*OOx8{YLc={cHeNJ3@<%2 zxn6k)kXfjzm#*-;y;Tl`rC&==KUkaHnwj@MXe3$!c{%QJCu=wa;}$K7p`bp1GTcgk zw%7aHVZw<6aQEv~=d&AM`PRJ79}oVEdd$zfjS#>%aY)>`b8XTL&3{k78~pNqxyO5b zB4BKBuCBSEsCQokNra^WGZd@(HBT&QaSE9eH%0VBsPsoG`RwFH5$#$O}9~y7|vuw@* zzFfj5I)r1PUez8n?t17*;RNjsoDr~Rk2rGAWf1?d)fb=NjlK4m^3Q)#c28qsFY>#x zrR9I!-;w6l;XwCM`M)fTB?lX=3SE7@d{!0hjdl+$hc~~RLNzeXL^%kKd=xWAXXctT z{g@QX^CU0GR~iiIwF1xEdn?=GHR@of^b;FOqs=K-*ZBMa9Z_6Dfwm9Uex0w03{RYr zmG=0h$s4tbCF14%K|}8+T_+{V^XWD3cQDf*qM!Jb)zY_FO&Yjs*N8gZxZ>!(PyO>Z z%4<$@?sWU3u$4ikTz_rU({$yCCr@;4Pn|RDd#6@6u0Q?K;f}j<*ra!r2sVqAL9h$t8 ztFG5yoYJ7YEWw=Z@2|GC*h$xYOjBhX{2af+QqxE~*1{~6S|ar;5CH^3Vqy(yFN;R0 zM>-~&?UigR>#ovI*Qo~m^4<>l{HnZDF!1ypwFtNaagm;JLLv$M76~PM)roGksikZ8 zm!;+95G^~6?~y3H^cF3X6}J`wX6_xhR_3NTE4u=E-Q#6Ib;#CI;+j&c-=$BV7Zi_C z5x9=8zw*M57I)?PhMFf)T|1L4k}OWD@i}$}vt}XVJg?A7J?{-P9lt_h7MvX@NK(*u z-+a>nK;KGz_fD!o8Qe0}mJ0@#_l~HOvC)Z0|F!R0WDu+%TT^tpPIj7Dzm-Y(3YSox zsbA4WiaC6l?Nz7x6_2<*a+k8v2SO%$Kyk1u~I zzKlc`?ix6+_&LFvrUrHk`-jl5>%?Q)ZPzdCokmV?k>a#-KTa(l7aY^INywirQctyAsxCkLK_ZX)CtS!~QGrwdI2jg5$FTZOo{ z6SWl-9P2u-pMCIhZ8sRBpN!=CYOg)SWPOcwz@Z zVw?$(;#W^M5>Br>hat7)qoRHR3`L6RJvHmR)2R9jr>9y`G7Eg_=)rRBm|{1>>`k=< zV@w{_+-f4Krs!j&&HTZAysnpT`Kep@FZw@pw@slPbW=3s>8AJpa7;`z%T9qQPXI5R z!0HHYXH$tjj2dI?s=?=SzDT$s`=V-btX*PSK#s&n;B?hwp(!y|N|_N&VNH4%EOy%r zf43=l&q-Rf+B2K89Y~~>WuosWR;trj^DKiuE}iUlgWxrpIGw@c5tWFj5Cf{iNOh%U zEBQD6^W%ND{xE&qKL@X)os1daa_Bu65`58qx60H)7$-vbvz^8>VQawb*en;%ydQVe zTna>r*e0d#%6C21sk#mOhi64$PvfGZswUoQ@Kc+~w||Ul*&lXqf&X%pgd1Oc@r70z zW24=}IhqiLj?37a@gjo1Q&l0LUkAlN%T)39LhG;(-irbm&$5iUj2?iep}B4;Z7;B6 zv6G5NsFH9};2R!5IY@R1O#GYNzC+x#XDF?*pemh^4w?paN%DL&8ArL>JYBcC6Q0CP z^*W**WkNWwYV-om&eqMFo31;jcYAPiEQj?TY$0FPVkM!+yJs1%YWN+GK@*5XT)%R2 zrxJ9mMLz;wm!0?_6;NL`8B(gyzU9@*Mh35M96kIk^9>%vx*Au5;f&P+H4e(IaZo3H zQdWQ_&_54AtHz*Whu9DDUT17mlz{yVQ>+wk_1F0;R@u0ZZs2{i3t{E&VAto*@-2Td zpBdZN`w{%4p*%2Zu61J6LkQ>$g+;79g`9-ft?Y;Dbk;rJzmJ%4EUu%9Q~vi!uvzN_ zro)qacO=SXLPnQS4!50m!%pikUYJQ=7DNOG{eFaG+L0F+VqMVDWk|Os!_k1{5=f(_&im)951YTH%|^SppSs2UI5*_i zb7!11=FSf3a%Pvy%Z*cJY&sWw*xA{4#N2KLgZ5b-Sm&^H-;5{g9e-KB@wcS6=iO%) z8s7f>u-mkjx$&>gITg0rRM!8-wiRdME>!F>8_dg z;EzxhM$b|5@}xUysJvECm-rV_MOpV%d}hDz+sW%>kE`YF^Kgr??(6Nk;<#A8iy1O` zs07-;_3dlFWnJe%XwuP-KR;J*gZi5o2g^oj!dz%qi~9i1G$i|Gid{rf3=pgTBkQEG zGAnhS#*6H_>O9?g63aoZ*?-Z;R@J*E2ry9ihRm5y+ot0pAzK>sSRdu8q0EeiOPm{! zi!s-Ab?uL_5+%f5q4e5T#;W^-%ZS?y`^AK=yXy5N^`MiGI)~Hv+@X)pHdEZ}&sR3^ zR(JGFX#R+J`wRz4rlNBUUI{L^Wbo?u#Xm-P5T_!RD3+k{<|d^?mrSUNqT4FS4&NYz zXDRum68`7sTCaMC$^<0DLWZO%UN2$d$7QJL4(HzYu2oveoZWxL#+KdZ7w5xh+$*~H z0&!#EOg-J(;XZj`3~*=fpylXPt<>$OP(etKC4=)R$DF8)TI0ITt_kpzYigiYVx3!> z%Z%N+x?Nsuk7L$Y_AqyBm3DsFU#$~6UWG^R$K|blvs9J<%7*pRD<1$Qu{ifA)7xu5 zH1J*vt=hN~WTYOS%>C)L#`SewTM`&GDAdg34*iy+hv0L2<2vAr4_V-M=dpaQ!#E8b zAq)y^-v9lxj=axNf;P|^MUw0af4eF+semTC)9RESGh5Z?5l_T15ig`{gPJpB<%p@n z0(4fq=o#U`oG5U90;6@mLc3M@I#|?^v2WT=gT_wNV(Q#uAHO;&2Il(k$BJulyP9`C zvT>6zcyER|pVk_pN5PBKtlcW@`&ksB{%f7?zj5OewfkP)^X>1J4J7yZ%@*$=&CS|G zZha}}VGCw_c>lojuTxpBJ%8)Zh>^M|Q=<4t#ce?2qw3!3M9TR_3xGU;C3F(6w1^ws z@TmKW6>ZW!(8%jQ@TP>g2>GOyMvo|l3h);v-)I+ z2I966Et5o9L48Xl`#MH%!WQL!vAfs;I}eIa!zDUOV}07rPG^`91=jR?cV0P)dOq3t zA;(U?t!a`_DB+eDq=(g9tocJsIN=ToyD6pJP&I9^HZ08?ThuOEBdhpi?0*-t1`5?-z3oXD-h88P}8dGzN>jVet zHj^$fEIUGAJw4e`+SxDveIAN(t$F0);Nbz*!lGd|dLR_U{m zDvxKTAAKGqp)>mjcqH4_KGhY~=@qq(Bb%;2|65mnUp4cNS-ojPumaV?B+jeL^zKz)!5y zw{&DF))4?TG3vzH;SL#V$}tywEr|X9{j?tuXI-bjPsm`6feW(+HkGeD6n4eB>Bq_3 z!3lrN{}}>W!6O>8nj0*V-`TUu{L^o=|#%!~XBd0;nM zY(59k&$3L^C#<9F2a2(Hx>=bY4jCsI=A~pdXX*+f9IgNP(Ruqk z8CmB>viABQ0z{^e4|K1MrMyV3B|ba&UY+OZo*`>_z|j}xZTSnoHQA5u=z;EJ{GdS% z5>zkT%CaE z*l@7;v_^SXkTwzXVF5R}+Wisn8MSlSdsRh)5#hS#N}h^Y^}GC9jj1SqHu@BxS=Cnw z5_h@Ml0d;F-?;`b7=)4ACe6SuMmJIQ+62hd$7ozdL>tq4xA}@7|Ic4vF*B-{xs{m_ zwRJkCQv1qX1AzZGTU2K|ANYB_Zi_AxQ=O-Aj!?LoF6pt4vd^-Nf!^q1#ZS6xQ9Q7| zI=w|nFt6mmn6PeG2pX#^21{5Pl^;wx4u5BnKoIT(;Wfg8E+YtF~B_OV}}2Aw$eknW}vr%#CVKNaB3{|D@=B3|gep;nro@f!N+M|$?lQVuClJZ>N z3Ry!5;qC03!n71(Zz6S$zf(}>2^*i(4Dj^q+bN+JD4k(Q%fDCTSeqFX5j+qokK>i~ z37LXW9vjBnE!v*PR3Fj1x{r#|_&V=fhA9ma)_MTDNuLrC44*=?X$lbZM9~9fn5kLJ z3KJ1gnhOAar0TU(6sIS6FszG9bAL;z@~&SU5nWz*J-)8ezKt-U8RO9|VIBZzxiMYs zz=GV{lrU4wtBEgd()ulZuIZy;8Q677rt?F>UaK!u+TTY^EjGJ0m?N^Z#nHF7m}rB# z_gVd|yOOZr9KxK^k)ZP%Yqn)ANa!rn`O=Z+&W~B=$mW;Ac)xSX#0RNOE$N6`C%EL-BL|r?i z)ay-a4ckchoazjWJIB#Nx!Xu+o}am^TlJe|Ezm%?RISN))?zW#>GDtRiw zI&s1ofnRU)&$mU2C_O^-d2D#t`M32+tSx!J^i{Pv>QBw4?XyvZi@Q=SX_po9?&^9UafJ5SnW4! z;hyFbDL%y^Vun!}__AhLqKGTOm>oT!Y6we<&{siXID|Z+b^5Hjw1+&0|2gZspf}Yz zSm^iBN}mDkM2ta$Cc)|#GtMKbAV{5xVP|Uh*Lo~S{{lV{PH*AviXH8ii@2LYxtJHN zLr4(MZM?Yc7b^*~nFt>zhiIjDCA@7ak!8V2F__h)JpMJZ7T<;gR|4X-*WJX7UZ~>L zqv>=oF)O=*Ts#MiN3@=jqzo|bGH(BG(Z{40G*Qg59*2uASV<)gPplnrK$K%+0^M#{_kOc%07`tnDmA?FYDko zrVo;B7a6kgEg3z`#I~Bh6g|q_PmMhV}+Bf;}bn5ZDFr(~+y!&TB{H~|7 zsq_&=W8gLp7-TG+m%wMvgUfmxG?t% zD;Du}h4xOhaaB#lIGVLQcWJAs6f@CYjOwsuX4;Y24Gh$IXFyh>A=s zp^3*sGR`wJ6NS=~*aWI(|3cwuNITT^+#xm*m!@)-0|tN95rytNv&C2k!MfYKQmO3v zcixaygk3Z9DUU^3A-*BI_f&cIvKIoVaKdC^wFq&3Qysy6A?|^HQ4nSE(6`&^6KXGi zTWQ9U48e*IVXU+6%^I)Hl*+85+eiB_`C<^|y$c@aR-;GHc2wH?)&dW{n>WKXa327R zxYq5fD5Uzt*DunZZuyC3PJ@|@lhFdOJl4pVC0gtU^91Wia%SO^dBKw>gQ^s5tG{QE z)pe9VucP7`)gdH9Y9Rkg6X`$uA_@}0SU>t~VUIR~yf$eoNmBFf52fj;J?v%RSa5MF zP5iDcV}69o!>)`}cy+S8C9rAUMZWk@LgYBEK-Wy0_|>c0;#~sx;;OMlbqRZ5X_am> zr`EeN@YCxnO_K~&8#EYCkEorqBYD?mc8>a}ew~73_O|68+@>c}C2p zW$%ckVVFl@tBvY;ZUBKYcrn=QjrT@7`BFP1`jIQsj}0RL+?g?PxhX^|)8wW6E$rw!}TlSV@?^2sH%rhmjC`>C$Kj zWJB|a`QsYqUBdOn1~l(3k6r)DrI~CqV93)W)FpA$)7a}kz;g{sxT)IXY)=rR;3@4c z&83PJ*r^FT0$}F~Oir#bFMM+<75OgrWdcStOo;5>T};_8rWt*SJhYpw)BRAVNHx(e zK$jk%mO-tlkwR7+MzHP^Vv24#|IxRp^Q6c#N(V@mO3hu-xXNZgKD|+p)0+i_pP!a@ z?uL3Z9%Vh3*a1G&_&K<_y58 zNe~+BwtTtxkopxNBd|t8F*Q@VO zEtD7g>C}^A>ZPjxbBy-mCL)zXMk+XA3Gw&9NA=gx)mpXdl>_ysAbH!>%nP#L*1Zt! zMqI--e(p~;{3v?Y{4+ok2N;c{@}z!diccH79Z}M}=unylY5S8F6l%0Dk+24qEC8zh z6&1Rh8v%xe#l^vn{S2!nFJ~vWhQ8}#2(hPUw!bf$TD8gs@t2rtf!;P)jq2XDo;vx#bcXLq*?R3d*XA^!g0Uv9;WA}7iTCrFakUty!dZ1P zi{+}K9&-rRZa#&E|Lgx>;(Ee9m>tts=Qz*Cs0bO$$f`>H-n}PqaE(HzK!CizyzI|! z$JDyCEIw1(ZY1QOKXfbEcXv~!zErhhMZ{qk2mT}e=f1sH0Ypl}FRh)L%9%f9TLtaW zs_LpD-u}TYQ=h|UkttSbuGCJ+$@*8{&R*n}{I_5?)nQ%1Mos;F5(d+M{$u_g`m6*s zB|e-ANJv&)GjIK|V7mMc*RPTF&|kGlBVK2L-B7&f45Ac~J%MJ+@zf-j*sE&qPA&<= z&-ON%W>9fd;u#?uXQVPl*EciU{pQte!LVS|wN5)m-+*A#k^nxD2orrw@LZG67w&(H z!UWj?*(O~Zvq?ZC9$W_|ih$zb8L6U11N+l%Ly5bNNDDFigL)!F3YdtFXh; z%~`!9HVJU@qZ%r@!+C`wUZJgUjn<(VWJW!V=(lKDVV7$I=8Jnb9KdJt>@BKGn|_@< zi%q|psHTT6qo!Q6WH}IuWf=_e>uc6^h4FnoAeZmOb)Vv@yl#WC^dt#9X9oL5mdGeQ zN0ZnMWELcRfK-72p1iVw_;c&lExiWb`)lGy%VO62GvXW2u%QDF5)>liJoS;4PuJhU z4%`c=b7UM&!G}PU8Kpq*r&h>T1`~V`R;RhF*0_ZwB+v0q3zs=i@1VH>h;XqNJ>&+YUFHN9r1YH><8HtF${>E0dc-+8BgxSgf% z#cpAZ2Cm@{$%I#(k@~km#JK9|lE#{7^+<4^FD!<3QP()^vUo&V#i(Jlx8iAJa8k6% zb>Pnb?csa8VDxMhY6>H0{_iy{1%QCyKu3$-VT4-K{)faOBwl5^E`G6-dDPmsK9pk@ z=S-iZxv-sZ_au@B& zNhuU>2OweD|L#xT?Kg=HPL0~URwsh*>*<)Jl}@evJN@}iM^70eiSbZT5^f9N-P(8J zpK)!4i7&^PM(XP`O*>fy-el@(ki5y)_cq>L+js7~W|{pbG!4{ui+MJq1TJVcQv|KD z^J^@Mgsz!}Tx{WiQ{&BCtf-3~OZ?^Z^nP!Gaev7DxzT++YEI5YlJbC$!)#IGY)QbT7Ly?l1AK;CJ;7{Qz;_roX+aj7V{DD@WK{ z!spbSag(p;KTE_Cfb9SOJF|7DP6jq%enpaFvHEpTT|E?O62<;;W-0kO!bV@X3a5{R zfL!s_Vke7sw#7om*^E+bWlzj3v%O^HDM1(Tjis3!jtbMEqe1z}fe~0xM?F2Ivgt=Y zFrm^8Fzj>cE6;ec^M>MzZudHoqA)-~Z3yqWb-CFUnDc3|=ZWyn$lG6UDD z6_d&X9j+k;^cUywvGo#fePk-30mp9~v<3fKrlJt5+q`Z=us*2Ljk_%iaRDEG8MLS&9fH^a zk^aGaaC4ODJpDi|dz5ea`_r;dygyLS=45RP8E$RKqJQh^OZJ2!<@pUmsZVTlCv0aY$^knp^tdxMTHQ#K7*iVO9 zSnN`=Q;9egsfOvY>TLq}{JNR3=n>nb&>A#fC%ndgest<@?Ntn9I|QY_ZGbt0c0lQ< zhBc3Y486KetC&K`1dWJy+IBNYGbQ^m@tLIla&6{avyc*8a3`Lp z?5~v?C$ScYWFRS4UqQzmh*$7<7Mv(9-|%B-5Ayg!Gj!nFt~8#4Z(LyH+8F>MkwO7} z(qSs}4*=(~@2C>BnF{C{7*xOuyjEnANjo;9qt0d7v+zg!r%@QCr}T6I^20n+h`gqz z!B-kgfh2TZv?XSh1O(APG78N8wMG1H9NAZC zGsb+o<2l0iMf!S!WF0*2MJA;Z#NR)`DLhQ^&Q7e(>t=={MIR)ddyy_XS(3#(?1tME z)%cowmHA~VQpcr)7jR#1i7%0C7_$kA53p34#Bfq;wk0p{2ee#7|v z%6_zJlpEpa~g;K8iLMH)0Rmh3d`g#Ti23J>A8|oVyPsA(j>CFUVF{~T@k^pcD z)tPZ#7{!27&T?&d&7EaJs1kLCP>3dGgfQLtZu+{u;s#<_1_G+r_VZhMeY0NgtJr@7 zIK$H0`vGD(CoHx_@63k{AD%&GlC>`*-h3!8-WKse&~xZ04tA?qHnxO(<|}3omEb6N3b*YpZJXW~mQ&+bi}&6iJg` zrHR`W*)HwU`mTYUCs@gWnN=k;E`k5|^t(Dc_o~ZfTvkNrH^kC%+;6ISm@j9ZoZg+& z%DZC%CM&5V)k0J4eh8Amtgr`Eq1rKJVx-64JiwO2Ql+Xfp>7VxFN z#m$!(Q8h$aw1u(OQ6Pi&81|xex9nioo;I6lc+a3dJn{9n>maEyU(`oZ4RBZLhhIu! zzzgxLqbSt4zX~a$1a+xD0zSeR7ZRuG#jCZYM%!<{jo|l*$*0Eh)i&eC3EoA`GloJU zKiR0wcipE=C0qQyvMm()tP$NW?8mcW>!{@K1%U?v8IK)!%Cts#J9*Xz#Z&lAhlw@4 zuwbhFZEcP=xMnVN6<#PrHS2%IV3Aq@!ND+4dzgc`3Zo^`r4>*%pJ7Ky-)`1>y>Xj! zF@keEPC!9*?b8{DZGIf#a}vsuoEVLM*NRR8!)8+Oh|L8{*~12Dk7TBF3f58n+ErdD zAoaDYU(YSy?62Z&F_z3d;-M467rKy};h{%Sb?@UzFt&1$dbz*S^tk<{UqV<(MzjHN zfEeih>eG*6RzaQ!}NYslsGr(Qo#R$Q#l_)-KcOi_zkhw_dCz zxB#6W@1Iy*3w~pZtP4tR7;{D`BuR(MpB3FawunUYS9bIj+lNTG#uR_Pr~kwAQj|vD z>5uz~W`y5C6SDEQ-`wIJ@^s2+Ksxhp4_FC}ZRx9r=988bqs-w0+XmHuAa$mPqC3-A zxa{_=TjjKLhd6t0Dl45#ye}192T`%RxQUk_BpGp%-}R;$nsy_c0-@Mt+{&^yIZ$r| z^ANiemMshcJ8;$THdql_!WDymBQA=fcoaXvZKD{^*Y5pv$xLk#)&%b9_fAEf?teG+ zUoJpyRr+`t-8n(=2wD>n)daij6rYy@VDNvkz9VN#ZEb;XWTd#}^0>#38`nJ2QRDO&JNa6cU3-JC1Clgi#kZI9Pm22a;o2Nd$ zx+VxWu+mZyU_6~9g|hrT?2_J;w6+t!@FYW4#O#AetD)%{j+I&nGV4w`GEv0vh9LLU z?ZUzR`<>}cpjce!RK+hm{j{p-fK0m!o~Hx>1>kRlYD@r0ABQN2ha(5=;NfBzb=X zY}tz0H8U0|u#8g0MPrw0`zhj9497ys-BU>3=I`5`_-Y>8X1`}z3j%Pjg$r0zret=P zewAp-LR%43#G!(kU)wW#5Jj=Uh4uQMYI;1xe$?&frxrgA9V$Br-qg;#wbl)w*%5QF z8NR3dC~0KPk`yQuM5-j{pjq(*OyslHCtg%g`U=o2j%h6K=sB$QfYUP3mQ<9I70sF8 ziKKK;U;ZhGN^lN^AWDECCss5$gejGZx~d)4-Ejg&Uk>{q+ZkrVwQHj4tzSZ zVg0$E?J~}Gwsgv|!vql-+MjrN0RtT5;D(|7NQBU67r%CB^ySx@H%SWy(}#kK3KGO5o7FsN z4wd<2a>2JaNfc4NH1w)d8g)Ovm&-Th*epKf`@FSHwca;Uy!Jw24l1Oso%{&I^F(nO z-%8%O_`4Zn`QNs^lF(CN0Evhz+Y%9Ms0Ww4$nNR$z?ZiocEmzJxq~+&`fc4Tb4$MP zh(%XfDFTrwWV(CyWqbu>UDMxPS>A#U~L6BDK9Udno~-+kfoEO z!umhBwKSD3l6ZRN$Q0-cDL%SZx1eBQJ3op|dL6Dl(zO6Gr@WFScMRtDg4UP*G)EO+ zZ3m+Kd6}`8VVi#{zbz#_@jOxOlMflw4)9?thzFuVRW(bc<_?&;_{Z`$0#y>?sKo_b z0O6JHRHTl3%sg&|t51EfnUa_HbrdZPyw#Yy_r!h(qIKc(YWP}*pdO5f0X01aWKudp znR1A|_VZ4REeBp;j(?jXwC&ucAfK`khX!<0$&hzH3#d)llndosah`k3Tg0?ciNAsu z8I4g)dD(mXE)H?$yA7>$Cvmbxt=8?0S^=d<(&Gm9so0|aU1@*Qwry8La3CO76l@E3 zRvQ``R^V_bE1n4^X?MdcWMOD+E_guIFX$t$dk`-`$V4e`a4QpKF9u5%S<6;J0jzkI zF&IGmVq}RHu*dE_XNI)iA`9^55S~Q0%hV(i(x{%H;kFs8EBY_zv5Ku)MlOd^ETmlj zi|Qn}l-Et#*ZH&3#%z|R7IT)*9C;)T-be|9nkLf&oX2}JXR60~ zZuE%`gV$$+yfbK|45w$@k?)yGtBsE9B%R?=(67>@(f#vLeSS&1Mv5)tVl z`rS$E=GB!`!`NH4D>AG9kuWVBu(gdo+EBp%?r=p-{MXc!6zbt4fZoD_B1bR>=qy9u z_Y|^vMU#XMBrlS+O#}xj!+_(N!WSdlq;+_^kE0m$Gzt0*^zpl}- zEh-2z$HY8RdMAjArbo`fpcCDSXJN1tj$|hZWZ z>fI?G;u0wsQ$Afa*qFHkxQS%V39E}8&Tz724|dRr4h$5eL7}E!#FkjekaWgUs|opJ zNmmMUF)}lG?X9|#ohA|wxC^wDH^bvqtGZ*Jfe4o2wP_T zPv7+|K|Buj!ThSe6q<4}+p@mu>T*wNIPq+wmzIDR>B4ArBa-J1PDQy@+uIT%gmi6O z;!$$^5Ahz7d%zaGs8wDvJAetw>lGoOYzs2ezsFzM{yP=%{o`FIrKzsi8g%^k6_xX8 zTCyD8KAj$K!9$~B2|)Qh=iK9g-giYS%RN5I%4tHyGFd6lHyj1HtNOCY+#)$i;2 zBh|od+D5_Fal@WWkULfIVHdCVIToRdcCI=+<&P7ya)9zA7B2y&)V^!S%!mT&Pw%T! z#r+v_Y7EKfc}h%EXq+@v&nP}kkHpF%=JW1j!2};C zcW#N%?567K%J?Z4H-SELji&)Mj@?3(6CA$ye2ix5x9t4+Pl^(?84hY~+0b;DFI5b|TpTng zfJV?grY-y#s`KU$O!)Cp`x`7i!rbS~`;VSx0RpToSA}Z34piyRgLhKJ(>Dqt=3!ii$P0I zIknlmW5-&?0j7Pz!l4s1)U~8ZP6U5PAZIWImtDQNc6#VfY0<}ViN>EO(ri-clN6?K2M6byG^S7>6nMo27r z7T`EhbKwxPltd&wOgJML z501iu1vsOqVV9N-zxwJX*{iG9f;i}>ei93#)3+*P41;B0N(;y}8$-EzB-lCAjl; z3$N80rg?jjC<;@0xTG}Tok>)3|Bt;lkLr2v{=YL%$2=wTJXT0iD4arw&?K1)4W<$z zmFW->5>kc;Ng@fEB0~d;sLYWPm7y{e4Zp|k;G8el<$JGn|JMD-y;$qIPKG|c->=sm zpL_4;?wX>b%EUnysZPqq)_=-SL97yjz3p)1wtr2c9IL!A_J4?OT}pS{(@3I~2tHPM zQUa$euqa^TAxw?Eqht8e)dDh|z~%5S<27t$7L77ddYu`kX@M0)6c=R|b^xX#!Re^a zNSGum2bjJv>uD(^u+Pk13%|Pz=5QKSM>%U6GQB56ze_(oUML$Alv5Oyl!7=S{Y1`O zma^jEl!w#~6);@*wzwr>*Jn>|X7}>0mpG7Fns*z1dW#*9g~L_*Y{C86BC?xHTn9{` zSVe5!A7tS$>x?zubO?)F0Eh}%loXp_h&D7cGuvQxYW3>Xj0CnP`(54FOGu#iUoH_q zi0(kSD}Jan*4W)14tgUn1VPHg-OhRLWGK<~%~UdJ#*k8oZ2zH*IV=1mnX5I$H<^i) zNCPp;Tr=iC`!?4sv{4k zvN}f47N+C`{bG&V9_(?TS?JVjDP%nU3@E)4}?;*S`HBoYz3|M1c`8olt>KQv;jouu9d+ji32l%8VQ z6n7npzSi+`WUhiF=={5=8Yjn(C$PnBYvJZeqp#9NN9Sh@zB89a_eP4@wBp zmB$@lxuS}KTLtNdL5@qab+U9Ot#SFNzDITsu|GP}{JtGy&|VN|RCmz*$(zjIT)bvAF@xJNFQruamxS`&{o zcH*Nz7)KEIXw7{}lFS!n>ke`Jhk@@!knhl1afnp8Qsv4IkjYcCi$A$N=J(>UxV*9S zWqNa9cCjPivxJO+eMCV()t%S^cDY=uAygv%k^x?!tK*JP6evyG=}V?V!qSN}gqG!d z8nz>b50}(O2>cl|84Cvg)Ufz3wB_E^=HjVvW$GYg0(ADt$q8Ly+>D_zzlTpFT}0FjC=SJIe6YZHJYXF1-gpX!-ENzxub`ru|UO zsY&`ffSb4jEe#e{fJ?Sn5Mc@NC3TMjywrHDBH-mE(3K?OO(e*A_C=xx+S zXsM;Up9@E}?}BQ~DSY>bWlO?Aks-K7>^9uXbbY+$^`wMR5`b_{0!D*r4Z}#Bn!i*^ ze{wM&bH>rgGeWXFR8?~(bo0_*Uz(7)f$%GqD32r~VAwX5BlMpg9=fRwn8ueopFWQJ zps5Z$j!tP&7)kAXTFuFm#|ULJ9?QWwlM0{-KeXA$8`8Tjuwu-n(kuZ=Tqo^$!a zvMJlk{q(-^Ru2%xn)&emsHg(?T=c(G2sP;3)1N>kqZL-Kehv{21D zF~EbKvE{8e-*~!^99WxT>e~aaqC$~sIN+kgy(z0Rs^}C9q`4#|Yr8g33|x1bP^ZKi zn@%u({UVf1K=W0AS8~iJrvW!LaEZ+v?~e9%)qb|Jej-7k-SOBC!pnpEGn^G&f_t1! z&f_FdFJ~eoaezAm$KONy#L@LM>QbhliUW*c!3np9t`!j)3|G#$EJ zZ%XOVkCdt_HSgb*(Zl(73>;V> zU=`hbVeshZ6S>WJ%}hT(hj>kIou)l4hn{@A;Gy{Rrgp~$X6#Vy)-Cw>^ANMpHt<}> z73hRzNMW@ami&s*RG}KuB)6)hx>7g?AqXi4oHkeK+SD!1ZR-66(dce zJ3AvXpZbb(#^Pd}PVCvY7sii!aZ+WA^YO(wC+>`lcy+)xkF6@l2cPodv@s%+$^B7oIagMOniBfJp`|vu007X{1Y@UMQe`J4M z88w*DlWz*w+Pb>M;!dO|i$Wq^?C7EzuMl`havradTAg9hpB>&J6IG22pL+`O-=?!q zPmyE8P8_5|LFfiRJ@V*x%0wTIz;>Z<_)qw*H|UfUh$1vmw=oKDk&R_^V2O%pNd)z` zCfz3wILRYWKyM8#&QKDYpme7_bjS9i>1&ESI1TQ-NjDOdPd(a<`i|>Wx@uBUTH~a> z?Q*YQaB+1>nXvHY$iA<)@Do%8#o!|nsaXK{nLIgYcWcij)9PC}m>s}~{DH;kxl^b< z?F21V!;N}=h|&=p$8X-XCQj(&;l~aZm~cxrHN>`yr47pmSrY{9CIMHe_mcOAuah_R zBqtrQqn8}X8}rDP?+u`_w!hlY^z@L5MW*psc|X^ZLjBG3d*noxmfYur?~v7Rp<9G( z+IE&5!RAeRgQS?#ue2yXq9AQu7%+*AcPjd;29EV@Xu~aOu>QB+hym)xB)O?2A5Rh7 zj_fdEyP#v*56#A7rb+Ig+lc+g2A8Z6^jhQmugM@AC59^GOfyC@@G z?pZPyqoIk2({o&M~|;*U9OeaXI3c<;)tId`xpk6cTO2`xsg*Y?CA7T+<@ zf<@~1)kfnjFW2lnf~Z?Ox)3X)=fd$$*4`yGyL5DLo~OMx@0lUui_0y0OYTuayRt&^ zjyYWiXFi^@QF>TeSKVrH>EO}`{0u8O(Nf0G_@2sK{QsigBMTt@a8-rCQh*pRy zf&j%LJ!K9T{4#cTX;Egs_qYCRG`L|F;xJM(V~ZHMqy--2laq*kwY{z?-KGJqh|3y^ zk2w6=q4Ty03&yFO%{ksKwpQPqjkuf_EEROoxHox)@GoFh3rD72?nZqn@BBjgbevxL zueL|+Q`Quo#oI~?ag8PgvixzIh|;9OapYzT9#|i=JC}Q{GxJZ>K<=PEy{NlmkWve` zU~n&;;#Z@9+jdc>kuE3DxuHbGU5AGQY3>qk2C{l5 znq}6>06l#KvJ%G+!{a|V{$b~Xew!=Q>a!<#+Yssos?p6HMyOmUn5sHG*-57`c2rN7 zqJo^04`*jTis&ylX7hZcfNK7j*CYsI}ix&MWAQ_+|~QZqP85?RPjPenscy- z6ZrOSlP8B4p3H2y5c4h}uuQ{pgiMX!5tWfm<$8MFrH}ZTJK8Z+qeq|r7Gblvw7gPT z>_ebriHlP2=}YIoCt=Nby|4Wu7j5?cbICipvTAA8smEK1-@8pbcY5&prGN(!6liRd zoSQFtx=f+~=EI83BNx=DUfn*f^TBkmlmtM%bKA#det7+`H(R)7&?l-2;Zt%o35e*l zZrIMnfEUhVZHyK&QNME01OWzd9W!1f?iVu_cJDsV^EWR*#q#O{c%ASF_OM=Ue2`GB z?5DB|)!xl~4i%Utn)a45xHlzOuR`TNVlV~UaKY6o6dav(>{8{3+-pt49?m{1`d;3D z*eqfxO|~~ZUbjs36>%hrdWDQmZ;B`wf9 z)8)GIDa(hmPK8(fssm=Pv(|nKu=ad@xH$WW1Lmmu-W^R54)GPzX{P8ZBI5zcO�O zn4rW9>NBwc36yOrl~Wk7J{j{Q#```YQiFCbD=?V{0s?gFylamDHqo#%+t4p>=#&99 zZy%i$X|eFJW75=aGj=+fsLA%l!mqpF5hvOWfkWX9Ro zC+qXl@SM^k65i_Ra2cBcE9y-Y9ML`K(6kMj9S-{052O8N6>mPlKDNE)yNeaZo(fU^ zwjSC{&H2xoUys6Y1>sRgUUGMq*ykz>jP2~k((&QT;ps$9E8x2gp@=z)IGJF?Iqo{W{- zHpIC%Tl~^rGdnme=<1?ci1(uf4`+3#9QYxNo&@Yke|9ZJnf-}s?Z$*_02Dj$f^F;U ziR17(6Oilc>8IA+P~a8nwkx>haNj2yn$_-CzHK-5*EZ#G-_Xr3k^`dAB#ZhyF` z+rbwn(m`0!f6CW)K6b6scStL6iVIgiJ?)D7^1u!t^;=4dQ%l9AtjblE`O(X@`gX(I zoavtIRzu}$&eznq*NT~_BdwC%aH}0% zi=Mai9#h^n{d_SY(@ikul>@!4rK^Ur7^h2VqM3b6>o=oK+VYHx`>Lh=)GI2*|jtNBB(aO+;4E;RZvCe32Xnv3R%s ztljXM&3v9MbNjRnq&}hDgO&l?icG|}-)7O~4~5C=AEMv(&sH_=GWYp-`cwJ>Ro%p< z`|^@C(sk}eIAlJ;fGl@&m5#G2&F34oSoE}#e@WeP1tEf62!NY zdXA}cSRZ)uf2hH+!Gee*=}z%l+1;iXT8SHDNMqo|6rnf*35hl6{kWY+xE*6{nk1zC z?^Hh4NbHN`q3oINguO2NmE5Pj``0fO)bC1QLLY25*eL5g((WbQ*Z|{q(Zyd z3-Vo4Oct59{CZ4#Dq|MLzrjNOq1b(ZqU|m96br9ABl{`?8#cJ+>MstA)SzK|6MvCi zTA-E?+N(`5nnn}b4g7+`mAXT!R!}h8Cs8msb$=vl5?wNdesiO$4LX#u0Ix5}g(h-v zhmzqk=OJP+$&Gi79jU3O_^u|O7bbSbrOLpX5etOV=CEGqE5htMk*=lWM|W@==FG>C zT%Gs>ZgNHAJ>V@(bX=TO&b-`vVovJBRu|33!f`I?n2(mD&4PPVsw{iAQ!rQY7TB?w zQ!eT-_rD2!(xTI(@iiq$SD|j|PP#gYCe4m@+|;3v>L-HEsdxJ>P+k5dP>;yHb_-O5 z4LjN0B#gYOAHkaD%RMW`7#g2H9DGQoCQ z`L-+WG3na5b1eD2A9#$JV(KOT)h%|A?f0Mn_hG53;Ve6l?FdqJu^CnODbZUtX}!v3 z(u#O0W%f8upAiKe<>va~hQt-E3f)0on2skM4u-m_T=qunZdL!v_h?@hR8Btdo2Q<9 zWbEc|amle-op_Q%2X~1dd^p)zT~cvLd*4&^RWv?5n@ya%>b4zR^?Br6!h;ML#SKuW zhQ4>ZQ&_u?+^9L_rTfiK)GvSN#m7|F(h+`rdf;fU(`l$$M&CKT)T})dq^(}AHQ|kMvhgj!i5aATxhM;h*m}DTG?I_b3%yR zmNqnd`9SD<{ojq6b>jA5aT-|Mu;l2TBo{egC76VTqS&&zwXwzAWr_Pr2ci99G%RQy zAi-%^)$p~{OCniupf)elS5*(H`Hw7Tufe|_ z5DP?00R?WF|2#bH)45%HAre#CWPA#Kubr$Q)q$Nh;o|~dV6rw4AwNTCrJPkU$>k}Y^a z`Eo%=e!kRokv8~PLU8yUFNGF`npSAnz^0IY3A1Aj2e**_f1$>%1W#Gw6^UF*(AZXF42KS)563Ag*GAR#sp)i_^En$Rjm9 zoeSPs%)hM{RnUlcum#wPAQ86c(c5sJ1PPHPAC9WwKmK66u+`y#fG+|N3aVFlbB>D_ zc#30Qi8an*1NeVYuBU6SSMO?`r0G)3RGjtUi$q&Jfvj_gW_H=GFyD*U)l6H{whWe` zY;Y(ruuh!0ATe`7KUYW^<`+9LxMb1#+?T;1X6vMSZnBj@sECho(i z>ha36ICLlCel?mVra0e}k_6|JPIfQ~B%|W-T4Wo;sI2Epc1sN-u3pL>N=wt}$`#{b z4+waWGox>BdAU6%hXG^-0Hu>I9chZC&2(daeBV+218Vk|e9^5RIieV$1z1QS_UvT) z%CQ%kCFml@czD3m@p5`nUl75bRGej6{!n=qQbZspJ?T3Xp}tQ@H)}-v!n}Pgi#w70 z9&R)(0-(AqYJ}lTt;c{PCd7Bl)M(Y#E|@GK=0;UuYXiwjL2v=FPLAt1{HWW5DRkl{ z&~K}T-Iv|Bt&Ar#yebe-Pt0kv`L_mAAh@jM8ZQ-qlP9D6^h^> zVzygeB-JDAxQS25hlyf3K+2P|re2`IJyR?k3?|Z|E4h?p#>dgw8Z26I#{*g(K5{ay zT1UT)Qt^=jj+kXyuBsl~p_y~+a!-7hgwpo`9k;YKsV}t+0hoaWOcmd$ZrQdile)%D z0lJW*Wkrp}!%5KL)6dDCuU#ue8y%ugK9c}W?u%ewYC_;{cSWI4>qP2R{fL?4?w7vd zxh2R2*}VvgQ=?*Fu8eaaM>A-LCZIa^<#_3SespizkYhyAAzO|X=B*>wh2t{f`G&rH zdiNUTFFbp&m>?x#!otViP3N5Jfs_s^>@A?7crP+IU!3Ez0m`OW2)l@ktpW|esePVm zdIz+x?hTu{F|Aiy0&-|R_fV52tYu&C%kW6&?^&E#v}|ak!AuAZ9gwMqieH-kq13CB z#Kxp5n~xvwxhz-@c(6&oMRUQ@BGYNO4)Jax7-Ct00&SZCP&s<`m>S}V5%L*qct0cu zD7f#cyyi5^wk73uWEO9iq^6S0NK#JerCJq*5(8}3{i*Ae@yn~r6c0h%4DL6u?qzc( zfPBT7mlf8K8N2fzk|i-4p)Q@J^+)z;Ezm6Bbj9+id$F)(zyr{S6`BLYI(B8=9OozJ zQNTRn${FN;QpTj)tCOP1^=yaTOFv3{W}$q|LHyV98Y(eVs10fvW;;@HVj|5!@Jw^x zO9DTVT`EhiDAmf*3fX>`CUyn&_9XI^ObfeGJ!6tYV)m?S%cN60Ucw)&CD8EMQ}?ZT zB8jg1kzv0gW7jKRs!Ir&PkpB+Wehp%HXG;6C8~WvYwXR~j=TLx8fsK5kOk$wg8gEr zW9ensfz>p5wy*_2vKMs^)u%mJ6?{x@^LRSn>LC^-4n|hT+H9 z-!bTI1$t$?)p)&B*kq$@KAEc($`RZ!(*mRx27YzQGtTr_9;F0(V^ClQ^aFS3Q}iHf z7Xe1wvWmax+@gj6qSZzINT5wAQia{r7|!ysxzysibm|n-!PjoH_)*4c5)kK2;>O7w zJmx05(fXK4`jo&xraeO$2Y?8qF>-QHs5o~VTRe;;+of97H|N+2!>8i~Y$wG?pd~LK zU^SiSWyH8~K9fsADDY7atqt|F9BX7&Q-% zq$K-@-4IeEwSVlGlF?D8P#;6vdb_Gr`Ed4U1*|E&v%4f9xax(0g3sL@JtTu_bsW{s zwi_e+QW_ASYccmyrB}2g4{&om1WwJDwh{)*yGXn($K7{#>42|E?u{MlU5QoS2lxG| z+3Fkqb@x($(tPSxA&-i!JaAyRZ(d83%;`-;rahKr#=)HnhOxZs>gbx-fb?jcl6kPj zhsu*?=UnTA=Mr}(E#^f3iC!mp{IFODDDq)$$WNEmg%BVn0N zQPnz%`INBPzR?~Nq)?Uyzme)6eD~!l!n}7u9dluzBa>uU{(I9Fm4QLPsg{ ziF!zu#hlzTt)=k*hnjCrKhJyKvh?2cy6Pj4F2VCCOk|R>#*uFNlGxoX$Y-!u<&twW9p73P{(NlLWImFRP%9VRo)QT%DN+<>8=|?tx za;U-{C5xjl$MqLIp19<-H_cIfn3`FP!QV=jD&^L?iIIm$pQN&K@_~)?XSJ_Xsbo&8 z?8NK<{=Do>a4F0_S&^Y^roWyeJOo-!5P^5^pX)ph}BH zt;oc4wU)$loUtMca*$X=;|;^ZrDu-()QZ|j69578mI=};L9ZI{&Bb(u^itLM&$m5Y zwv-m`E{zp)M^^U&!4ApIG{!5*w^EidU=)67N{x35F<=t~6d;74FsUg~#ccfbqlKwU zvvV{}jf^bC4@ohXCiehxfJRXZ3yRO6Rp~QyzKoOSCGvX#YCfrByo9`m&y5;qV)8<} zeQM$Pk7&gW&dP@aQ;}wx+4yD0&wf;`%MuT%H_?VLr>=rA+r;Sp?nvqTTCW2=1eqfXy=Yuy|N?WD2wNWHH!O9O7^5={~WP8`=ZS}K{eb#<6u z;z`F;CK%cHebG6{jX>y;JEVKIEYs$hk=*G!HIFjJp-w?_xZEcpWQ^|73Xn_Wi&NyN z=ZpR;sec6Y5d`)=v7WFB0#MEgZ<#OFl6HmDgX50^A~EZc(Gux00IpFU_-;}*Hg=3vay8y8PU_Y)M2Ovy zbWtL00?BF}OI^zeq04UhBwxGZ`iv6pdGFuIiHu52wcz;bLGNcVgLZ%R+1JNu);Pdf zTBn3e5Js#c*G^N;8$nn2`eujWK=GY@=1`^cmFZV(1!s%7oW8hMX+T)7y_-Zmon{Sd zrssCiU6yp_i_h2tBVy^sO^FFT`DC%J>Ygr(if``ffA)dLL5&O33sPRh@ARd$!*zZ0 zqa>=Y0NkG*GLcFuKznDY3Wk94DfKP^Lbsv_6)(sNSXOXYN{8{Y!jrTX)}G9pjyrA{ ztE9Pg#;^;%yt9>5aPKF#mi7*=HlKNOGVfi|l;ll1vgg{aQP>6+B<-Lw|6)n%mx*$^ z>UqI>vl!?(^MUE;ObP-s}`VN9r0+fPZqNpD9LBQdeX%ogcO<`*6}stUXoH`%0S0D7pX4*L@d} z59ZSV6|!mCtTV3;NKyX7+oN>txnR1zblAUFmLXEYMk}iPp}0}-Q!q6Hi3_wE|7{=L z9RG2g3MHNhycK0lCniwxBqB#ouRbXyH~h1NAH-@mMD}O( zzNE^`V(Zn-HcR>VzFpW&yU#I7J?I^h-UhL57~EA7pi6yxUS=qc#tKj*21@^8%;f$P zC+>EzQ#iIkDzsFF_7#0N>iWHV1RcpPn!Z~9IPmBJB6jY4d|X@{qkmOWwA$%|=y(aR zEA~Yy*tnJ@M?9@b3SZ%$gee}ig6mm@{US7%jQ|FGv&=Rik$Re&L z<7atehxUV&j+=A-1JoeHKXo2wD*`TvqD1{P2#$UIc8JSk5p7~f)2$xKAD3(F`Rv2s zLB=QTQT=jt1-VdKs*P=!Ot#^TYFX`DuQ80St`XlBw`TYH2XH7FF8y9ChuI)1hf%_f?p;0CK}HJ1fN z!4!Y#J@dg;AE)7=tTT+*FQ16sN}Lg?r1$LUsf6^BLzd|&KUkX9pbW6a9+;n+{n|lU zJ(*fTzO;KuGmFIUk`763z>4=x>RKC+EddJ&TAaitP%+SE2;jq61qHLNRKC1{+&eO^ zFlZONmyc-rDApX6-}*%Stc)^BXPUGy16jfqb5Ymu2z!jm`u-Sl+ackuI82l0#PqKLWmE0K zlT4&~D_yjRm`|TQ*Jxo;?Q+zrl%`P>OVdYVmbo|;T%@V*ysg`|5e#aJ@@hPqds*)1 z{^S)ksg+FqR#^`;-twLM`CZHLG(1ZeoAlLs`{s}G>k~Sx_qorK!;?ZzDpQfilGLjWgTUxACB9 z?u8r|?T%q6C4gafCP(z;8W7Veh0J(%E&W6TaLK;X1^59e+6NHqlnD);j=a%9QV!{K zlx&LgG%0DQ@Kh(_uR2P}jTG?O>65z21X3@+6;N~O0|h3wQyF=r>|f)S86${cRPi`VkDN&;#EM9ljI_5& zHqQ#)lykU4puW*@7?dLS=ofato+##=7ezaQXj_`SgS zAO2dwsM_yu|4--g`{;g)&3}=L|1VYMU1{jT=&}~m6-7rnhKA{uef>WktkHKc^MCyD z*WXNT{k@3spZ@yyH~)AlzYpxk6H)jrBtIU>Zz1{dM1Bj&PbczQNPaq!-Q@PFX*pZ`+f*v9Xrr~mM`zrXpXL-~DRKOM<$3H$L#ehbNuC-Q&G zlC->dhmrfgc>#X<8T`TTlmF>deyit?NAg=pems%?Tk84K=kf=?Py44+`K^*a9?5SZ z`SC=4tK?57@>@uLI+EX(ZbXCnJw-}UWFw=KVJoeNm`%g&ot?f>ad{`Qgo{%>dhU-tbC z{a+gMhW*1a_3uyp_Vw>q|8^2T{`dE-|9B$be*atGzn#QS|NVcf>zkB+ZruC+L)`v4 zc=DtM+mGwBd2mfoP*7k{&|0Qb^o)&-b6BReAm?7LyJy1n>+5Kn*L86z36-TWFPRog z&&bf5KK+29e&1hq@zxwLYt}3lW~;lDeo2j{C%q}tW}$TOZ*hLZ%#I@ST99L?$x=Ef zST?`~!50B5650hI_o#> z+&Qbsug}kI@dDc8&6xaQ60kqhAWnndUT40)ImBwFGB8umePxDW$6s3OEPY2uM`0k} zAUkXs%}L(#W;{H3!dGAwfnsrFdve!dWAQB=?wy6 zq1=7WMCLk}<^8-rMKZaT%OHF&h;4gbPgfiLt8Uzlm01TivgmPY@dP^VGlF-^gyKBL z_RYE+{AF`_tE(o#W_6i#S&PsOR=dMY%X8aFUv^~V9s2dt-eLx=rRfAmctsc&^RIXQXlFMisd*(gq3hBP*f=+Lw{7;0&EM!n3nqGNyg zjE|LJY}IejZm{l+-8!jJn*JUS9y~Db@t@x3>!0~uU>frpbS%@@dSNjA1zatIVR%&D zJu2qcKM9o#*Kf#Af+*!un&!WzD}VlJZdwn{IXQUwIx!vJC}G|&@8|Aqcs6!Cx*>%6yux)$ycQbyiuz{Vo zwmWHk=i2}J;x=pByK8D{{;zL6nmJ|OdRRTpy8p|2G#FlwDY#j=&4&C^@S3wr^LXsd zM-A4s)>QxBKhihthTo1~#e{BJvM`ki5cNZ{{OW)I`K+263^SAO-5U(xhc4PsYF_6T z0zcsK(%f~E?0SZ6_4d|dY(;tJuPKVe`z-@Z>jcl%Iuf=1?0Oo2iWI zFLxWveA)C(+qQLF)9e1RDSxtftZLdH_aa?gy?oSJ7?~V0!S>aRq0q~>QBw;W_@wtQ zj;&WGwr+Vsnoza8QYgd$jI}qdxTyn6c!x0_Kfcb4iwv6WxDm_%mOG@8!*%{-K(Ibb zXy$DEqi$ya z0(9GKn%Og~i+;N0um9bEv0Z8zZH|fkrTz?E#w^wl?$Gy2u)A(llxFK+KV4#jxC)&9 zKDK*py;;p_-S?CD@e8-G(M&k4RH%6iI)=Rm4h*hS(*lw*W;%z}u@>Up0*0v{2hPZ0 zvKD$-vjs}r@D-jTA*;zR-fOPOnbMLkZM$|IUgy?DB&36o`&Qe#tSNLzLjAvd=nB~A zdVk)0#Wp0O1Eg$I?ptph(P0|%g)=_<;gkHIMem|d9uv4M(j0vL98>+{pDGO4cZ^BY z#a~)vF^{>%vS;V{3T_FlRaKAPPGIxb%-eqdP|*JH=J9J}jJqVaWohWd_$++8g-c<^ zpehPVt90Acg^wh78>BWn8uFFLjvY#uDkwM)X5;6a_i77X-nz6`v-5vD|GQ@yzq|_T zf>Nwy&(||p?X1H{eSO@{a{2I_oSf54=ZeqzVE=@W?KxCR4f-iuoN>3ouzJLMzk24Z zB9KAg%A-+HK zj2qXlhs_+>W&FSQYyEvJ>qi+F800)WrMfxR656qjOcXbUw0{ko(R6$IcrZ_{$3MWs z%60+m+=ZM(%er-0uO+;EL)+Ec)yVGm}v~ODiiqU0oiWZIW#nXl%lwh6bH@ z%!^GvGSwFiO^OmbWDLHH;{!cez%|6h<-*ehSv#xs(PYAe=IZL|b?c7i3&PQ8?GPpk z>(;%^)L%?Y%o?s#Z^42E>FMc^x;%l(peZBwCc>i{RYjqofq{R! z+@q1URXe6lTJeMIDJaYc<>y>w_IDc2GnAq6d#s|9;oFHw;bKi!=rL|H@C*T|usL?> z*3ARcJgsrSUkPbYY=*-1Q~+<}RQYD)U-AQyI*Az}eOkkb1*8Y_jW40C5p}!Gylf2l z0+Y^qtnN#H@nSH?wc+u{l&G>{!ZDI?Fy`4vD1+{}B*U=Q03v{u>%w~*!g9KSfC^q(>$U(a3F($dl_-a3vug=uOC;*z$*Hbg&r99k!FB*UJbFugsipx}ogUc>)3 zr}S0|I$&5a_SHWWuix)wZlgn-A+K`)TAO zcT!efk8kSKsnguiW80)9G}cZf4zqi5PLpJ;fsh}knMQs#4rU?i#Q0RiZib3QGf3v( zFKv6_-6~aGtScEnXXZfLvKNmHtje;z_GvE}?3VQeCw8kj7tF2XUNZ(&OJm=%2lhf0 zQE5y*(gO;N#hOc?_m@HLQN`O~J=KN3^f@eSYoORtg|VU;{IqXr5*&+Xz1_D3Pxik3Cp)`pqdrfAep>0FTc`LtwacpHKaWm!jgrK=CyQ@r~xJTH`4EE8AIJHt*CYu19DuPN#1ko)%R@)kCxgzS$ZF zcBe(y@pES+EuH=3Tw!R>8y+4WOoCqx)wv!YzahtNTki7tfgvFqV4e00&F9zBr72#d z&DW62gnNZJjnXR)P>p?<29a_Pc)ht`SRJ?bb2`5Kidq1QcXCc@3wBkm!M5n3O)Fey zEg`UBah%7fM<(XxN~uLJdhcTkXD*aelU^TbLQ*U zW4_Hl>i^MHqnnU%etdhD|J3EhJl?u|I?F1`){7L}>dU$A+c}Ft{@Y0Y@D~9^9~r^^ z``6qD@-oETa?9jBjRq-kyVvtO^|BYPb3l)cjnLJ#!p2u;Muad*K?7KJx$JnKkjsGl zwax8^Pld69EG^-)&x?fJEk&2ttdYIA;yEGOofq4F+i5rJw(rFsK&6x>Mh{x9)?|ZC zlbj*q5n2Y9w3F~_C~7+|64x#^ax6VpQ+tDgx*W+Aj!0PmFc_b8D|<8$1*x7q_B1?S^u*fQ zzA!hbuqF44`$N$r5*Vi#C zAn>hf&z?QgAX)Lu%E~$|^v#QtA!&Qg(jRvw)T^~ncv=zZQ+jQOM4Xtc?fr8c?N)iu znKN3#y8*+v&YQj0*BgLY3FlMY+)Vq>x2~mMdd$4F=QOKH+Vp*BH80*o#1?)TvorIp z+2FeWcEh&2;>aq~;A`ps(S_Yu!gnfm_a%ne`B{xghHF!Xw)cu%1euohr_^i$H(@L; zGkZW)cuB#*(681I&h%>;)g^F$sG<857p6g-XCqu}qLJ)( zsnFJdf2Jt9h60UHw061R0&_>Dd0X#=%@`#-B)KpyUV)h2_fZkV6l*|snk^~i&pe2V zu90*F_3F_Ee=rT=Bne-H`OPmm>_YDPIaf;A+dP;g%6Uq8g(6pIpScBW%t>w`)#WEB zvCoPptfpE7QF|Kn=4gc=rqeYqwA!)gLlYzdb$=(!?yS8A?#AqV6n39zYitKg+*5O6m)#o zOKb7_nZXmzYOsI0p0EfBm=rr5J1`9r*yxE>&cWfG23wCiQSh}@jVImABBBKe4c{nM9AmIVh?H z6N!zz50U59Qx`rQ8QQ_idg(cWnlgI>F$?_oeQOU^a4W?=MBa{Hz={>K?z{g3NHw5; zU;Ff-!f^r7SW`Hh8pljJ$C{s9IKc)Z%+BDgo=07av*)p1q56SkpW*Y_OJIb;HywY{ zu5L_cBV*&MLTEJYZ7S55J>U$U)_#$YiPwt;duYGQ<@2jikG2kNH&ukh#H3N<3G+vj zm+9hIS5~rq2aoPxz)9YuTHBhiwjEgnIV}rvZmkp=iVzr=Pwd*ncLOfZgORanKLd&M zp{H0U<`bG9<&E`4=-E=#r%fBErWy%5p4iW|ea8mEYAbgmux|InJ$NpNxhf6pRTd(` zNmL-7cM&|974w?Fb~yxJrmG9OQFf@p(=Xbh1qf$pAA{?J=T21{)9~Eo=1-lHc#A!Orw%}$I-^d z4N2-2!XO+?+|&#{mHNXf9ET*P0OG+0$n88xoVpI}XCUCTtV_VsG{QUz=cE~qQNWAU z^@nxv%Ef40Wn*kh*(HYIS7b3?H^@LV?Cw1ctU`3@G-K?OD0|RhQ~NwSLDsiEB`Hra@@30V~>hXpslmbK}x?wM*;B6Tu~W6~0s@ z*iGw=s4jHb^WHrjI56On_%%oa8|qeYz{{5@vnU&PP|5wEHCL8<{_Rp#L8IZSmXDFx z0r63HFB~6k)6kQBN8-qjWD02QQh0sR;)~@Jidy21ZBK-$zbHR+%%4lO{yEUHudKre z@$NyIAE)yciq-n)MZN9{GuDFx4CZ0sm{%hL9kI;07O3MoC?v1K{@OrzM~O!DI?aed znh0B(^blV705gtOn5RjmQ%^!N)2 zl)?Ai_SBMu1pe8zH*SoW5^xLF#pVDY^(!h2s6P+sFNhuGQcoC#`U^iKT;mGM4&IY! zS9X@cv}>;D*bBC&?b=HhbAtt!YP-}CI%UG0&%HCP7o!)`2%~euk1jP(>R58%+2~rl=uCVt-~-uo!IYO_z)pjRpa8g=P3K1LTZUJA7~X? zixjRgTu!-OyNlWpN~vsIJpbv%Ws>E~)`@{r`-?(9Kj1rit;9BXaN)?4&B>SM4wJZt z7Z?IN&0Z*d39RaHJ|a%fE?RGA{YWr`OLaOMjUV3>4zhEt6cl!B#7l1PK4p-3!Xh{e zr@~$oHPW9DXJ>YbsBbD3`q;q@JG3uPlBCagspcZB)ktLd7;ridBpd#Oq@)>*aMy>$;gOp%v*I94(TC~ z>r6!;)+d|c2Dzvck za2e6~Vedw}R}jDOLVYA>ou6N`XBRv7V@;6Q zQe8O6R&FX^3QE_yA~rVd>7^C9M@v5+#ngMBRFeoTL;XPqK_w$1D@8v|5bN%cC zvZMpZsN1$t8OBZjK1cHGBScMm0h*S(Res5)NTbb*8Ub~@?^nI1k(Jf{uOao*J`#H& z+l!zaUpczAcnoaKI-#otEcJiaRqqLSgdz*V*S*s)tkGD-gSCWWqKj^(7OfvpV9}_X zSse*7;XsZS&5|%vV%y5vSHY~(;CimB+q{@H6do{G8nH&g;EKp5$f5O@A{0Lty&XJz zcF3{pnRtv`=<1`PiBt8cJQ*_S9xRSLjsHwT>LvQ?(zKufz=XSm%)nMHr`#P~h6?Eq z^lMG1B`mgAU_k8bQCZkaLt(L#wX8}>7LZa-6Lk29&ch45TUumEc4QkJozx1|6lNw0 zRpsE`W6^Tutz-W~9dW>8aIS8M*04(MGcFQszE)&!U+|>JiHeSf#LrN{3ej`tp(_W^ z{*aXl({sGUWbA&LiCkCTnE0gX>UQxfkjk1|;Gi>C{y?^-!1RAwy zUu9T(b;q`Q-HJ*U?YeXH;8^!FZ*R7Eax^CJ>$2->n`%goO&R$q2+BW0_@q@;wyic^YKy?i;77L2-ADx7xA z-YOtajGeaYN6gY*5r?-WMv>URi`d?uUQS5C1a(m03-%z|KW<-FQ>bVFjX6Vsw= zMJen700e{Y)T@^fd0pJio9!Dsb)9wZ*s?k;E@Jv*Va0Mjcdh~}Kh8r&IL)m1>C?$I zn`;?`goLatRscun<(Op@KI@@L2QT*llTAT!gj{%6s}>gt7WV=F)n8eOVx|rBi&=^L zYfQNqBuhRqacaRX_V&jcsaOmhJDxi{2I!mfW=BqFZrdB4zj(R1yFb27JxV8mHDKt< zDf>mb@QvBH$yOebq4S;(M#0!^wZ6MTQA)PO(4lJpi4S4g>sD=LjlXd2-_H%Ky0f~v zcPrj3#{ybLV2tK}MsM%CFl0iD4s)sz-j4-xQ(ur{7UlMsgt1}kM+b3NkOaE5>h_Wa zgi3wpw)!;9OjhM~Zqp{}*k^*|v4aMc9|#Nh>hx#NrYuak5~h`2#o!xd>G!+&{>4*m z+DuGQHKWCgwWgRwT)CnPMri?9-^N47ef@f4V#{jfFX}8NBK-hj*I7fu1hA@N`PI{V z`kZjepT7KbZG2`Td+Dp9+ZW4;IYf`tgIz|9@PJ0P+Hx0#&Ye5kq=f8DK00ZX;xSuf zI|YcoYMM52*-&uER#j{C@EO6nkL~aVk5oKn4h3rpHXKZU#yv$eo-HIwQ{YV9obIismR(lDsa-5iAG4TH`nGO%snf}&SR5-$%NS~z{rgXV?J|+*Y<^z4zQ(GZUtsH0pMFDe zdCY2d=XBB1+R>_8yS8orY&+QF7Lbjd&6ARnk|5KgySf+!w&GZkjN+2w?e*FZ8@d^r zEyhfL3^)nPOhTw<7{H=?7(k(;Z=p+@l1QG&2GdZzW=-_~mqCM;@7=erVUs5M_Yk9J zkZlgAR;}8CdCH{9AdXGftzEMB7rhecmswde{ln^cfa?~xX(KX8BUoAsKYuQ98G0z< z@#%Sg0%s+T(kTuQ`gb&Ct*7UyIExV-ymn~J3Iewb#5&eG@A;gCG33tXuZxziSdp|> zuZHoVF?uAoYldx8QRYq}&Yrz~^=gK{O=d<$XEn94*zVr5AKk(LJ93>n`!6v;+}ENx z0O`H^J{6PvC&um;y-gp8YU)>QHfHhSGgGHclTDXVr&s6X*dd#qe_H_pATW_xX7w+) zxhbq~-q&PR^@eS7zHHpKZASfsu+p||+6-#lx^>XFumi)qyR@G(XAWVHkK^NfxUM;C zmZuoMdiClv%N@4t*bxLAZUe8bol@T(*bt&^J?}2YZ9JB+6XJH_{^8Z4$^-t1c~NHR zm`&zAHh3f*8E@+1;?ia80T1~BnI$Eg|Y>UaOxwJ<7;#xrLI+9U<($zA#z9U60Wc1U~na*P}~vRjv5I*qxtsj%!% zYSF3x)GJllk8M=0T)8fjE}Xkst7VtbdshzI(#fCCeFYeIi~VKuONyNK2L#MS#dlyf zldO^`5WK!0HUzTO1u;wgwr>yIxUtUR9b-W+8lnyzcP$zu>fq_=Ic3Sc*jRmPlgQ~< z^tCkmt%JS2i%zRzQoBS}TQsa+f8X7^Mzfb+EJadM*k0+6D>{tU*M5Ai!7)7|sk>y8 z?M96n6;)7$qGd86K{c^~@mP_BTL(r_UM#tu*{5I$mxL1z2Q#({cgSI@5X%6A*|Zoq zX8$mf!=BC~I(F!$$nA^+C&sQ6+b|q3w9LgGm-!5f=PfG;0vFgOT zh54C|pWj=*#{Bo_(Zg<6`4>4k!Qiu$150{T31S3mlY%E_dLj z$b?lO+W+mBt14j=`gc4?^TVqqsYxm6#%PqS+Gp+5bs&q;S-1DCPP$19GliC`RV`eL=HYY9 zk{%q zt{+r=Sm@NNRm&mlm7DcnY=liR>CqSmu)d|vM1OyO;__2FuGyFNO68JQ{q>g(M({J} z7IeJ6%`lb`}U)?#z)MPvOc|aZP3MN zD7S6GZ8xk}Z!eJMD-uU7m!i|aRUy>F=IuLCUIG}<*BZ_(Xc0NH^vg-|U_n6$u?)UB z+uSt038&`{)G2zmLYl-HM~vFueIQVt^i&uH8FkiZ(~Qu4Yy%~wF1>sAjw-0paipK& z{Q14gE;=GX`^yMCZ|{Hw0|vNJyqTkxurc^5YbTEc4HQ^T_8;|U_k%`K#XVGOgGTTB z3+Te%bfS|U#j}vjUS8oY#fz249bdVAdIe!0Cly`Ms^l6ue$`9m2{sOr5}Y59c`J0< z5V8ydB-o2IkcVxNxCcr@U4FC8RBwuz#3$vjh4PSIkw-OxSITY;c;m_Z{7* zEN7L3p0C}Y3@ikK8$`aqkMz1wkXB>8Vvj)`ybfJ_VcFB??*8G8X%p1XxW4UF_EJ9> z^avfs`fl`I6d4^^3TAsFMD1$THd378|rH)7xiFG8FU67hZ~EMw~# z!>VnduzJ&$Edi7d18I3#ncH~k)Tw1s8!vvEKkgOGvV_PvUI~M0B*F&n5B_C#)q>Fa zAc$+1?%fSZNlzBq;XV%2!H^|xhiaeq#*KSOpsrrKc04zj($p48xCu+sRV~gf8jVNP z7--fibGhB!`{bDCS-w&Ik3Y6RNPaW(RYt~smJi6vFwszw+x_A|F~1K&r&VbhU;fK0 zRKt)!^S&(}AsZy_HcJxpZlPzb7cE)x@MieEv6qJ^UiXoE`p>_fmVTxWOY%w_V}qCd z$ZP@$>e-ZcU!_*QvT2B)yd53l15r4YTM_M`o~Obsi~c6O&0x#O0d_}yeE04aY4LkL zsSAmPic-50Zz2ZCAG!anAr=vPC13(vf25o1xwb*)y>O3bv);u7B~-VJsk4+-7d?GO zh?cTA$U&rputH;Ae;UpVIZbarzZ+>${t_F3z*o%eNbgZHYF*|(H#|PS#FQF*@8A;J zZ<0<}k6nJDHU?OXaCPSv^oAvo0n;f{9;Hi_hx6IJd-p`AjD{k8;)wy&?zx!|u~_pw z6>}TMTwO#ldk4_yGH1^o{rXwRKSDv?vt{m{-ij+a4vvmVAkYWo*2!>mN(%N7jT+-P zGip!JpW$)v>v3eH>Au#Lal50_yI3R%okb?Qy1xKPTUp}(A4id$%}L6})w`|UM%;u3HXZXYAz>8x*Mj?ZV>`vh#@YZt zEQ|L?m4*!+dTV*58t(z^;`oa<4htO}jcEhw1x8)W>In1WmeqDo3eE{jWf6t!N{K37 zK~`uDmghXZ_dSmJk69gx7TQvZ*Afc-AQlPw?QFiU(kc{%gd&6jjdWsZ*{82z9HTR? zO=GVQ*F6cT)d((OW930Il6t-1DOjSnwn3=x?)F4dQ~$f`5^ZtQF^hCL&*zP6HLXg; ziZiisJJe=uAQ-XyG*142Vy=1Ebc1Eh&9vBHu@u03fzKR*6LS4Ip}EdW*=%Y$Nj7H$ znT78CGj#|)`5<;#HowY~vd<^xX2VZMNG5~E>|e{Fhflw$L;y3AT@b67Zkg?!ukJIV zdbMi)quT^;t@|mBnSw0Bkd1{pVN*#1B3K+$nC9ED1a#ti z!;(iB7$nhHSM^hEo|{ERy~2`wds)}>FNwv+vL|=Ix((d?C;;VG^gE3vd2O!oh14aR z?MY(k;&=5wEJzYzu-!1VdG_{WxFvQZH2*V8)%?Vox3c|VbzMcpnfWQK&~a$kdCbbI zx)*N!VgjJV6#S)4n>N)gb10n3UM|GU$AY3=hkVJ*$tJX%3y$FeOLNascsq*DMeGWDo325b+_caM2G!ZpP|%uK``b#0B_T|RV%m2b5s5N{EYkuRSLJh zGq~l__nYKZa8=_U`+w;*Y$a`9!9KD=DR~El@!K>FR{K=jY5Q1i!#L2e_20Ka86yev zS@Tfyx^?|!d&I{4&ZNTtt3;b!Hs&t*khP!EWf0E{(4bhOO>A`Z5Y!@(j|;cY7)CZ) zZfuWus7D1dL2l&cvCl+&CJPp52m*BW6CIlmiqxfu`hN&J z6R@81@9lr>jIxeM_6Z?_RLYipND(OsDNCh7AwrvFvhP_cWQ{gOWnUr^YC?OY!jM9i zHc>sV)0pM=T-S3wUDyBrpZ^Ti_w#wb&$-Wi-{%}n_0QY?AE{q1MwhpIH&Tb!(*9Vy%j9iF1;d77=jvP0yUgpJ&~0zkD}-%eHN)WQl2~4BISLuZhjb zn9fm8myhaNWDbjT$mC-zkKv3E_Pu&dAbM-PYVoyEY2dic4bo25sTe|XO-HIrvRcV< zDdLDrg}c|c_k)Ov*-oXh$1OH?gzm50+cx(T6H{5AtsH7VS=i>+SqG1P$p?q8Mt_ST zqt;dRlB;{k`Pl3Zg-hyFwz|I0HoLd8a|S`Gv3ek%;?!`D%7ZbsoHa@ zLHi4<39_b$FHOpt{}E^q!4aI{bWM^Q0k}8IRPt+-+qP}npkYG?q>8~FmC@Aa#v09o zx%2Fg3+Nh?!9Jae3b#>2&K1ST9Ou3|ek?Dy>$-c%{{uwJ1JC6k&B z?td(AElf-=Jv|E)+SJXhduo%CLk}LfKYyy5+a+Mf-EOx+>cda6@HI?_ z8#mFE4!)Hi-~g{=6|%c`=2BPJ1u4|B7Q3ls)ylpvCvdMOr#JaJ{JGao)q(LZUcz=x zm)Z^X&+W+J=IpI8$Mv)iQh*cCI$@aJkRe4l8V0S&VgHP*+G}=r(TWupgg3vinyMZu zwpT#e=+&cE*`Sqg6%ed}L$;*RgUVrq+gnSmL+*F~k z9xDWy)R9x4#>I`sM9}q>``~4-x3TVY?k5*mpJ)i>BR0dlMLXk)r}N{WAdR}zbOEDM zVE1=Sg5&NpeE9QY%Z!bTQfWU-G+)AE?#~}TTE|u2;->wqs=9FZIS#*wNv$gD39EwR z-4zPO*k60|h)yFY*@Z^mFX8$X5B>>~y2K8_ zoXblK%}$*@-G_ka=jVshWxo_*FXNSQWk?+S_19mQy0}c)+^W0!1XA{H>Q=~h&zd<@ zTO8N>eDY46I)Q$=A*TG#mqjX9LC)~j9ja`>!8IoS?gkZV6lGy6K0FJSDhpdobY?AB zqL@P4Sg`l9Ft@lv2Dl%R5Y)T3A&=3elB{P;0DSH0P% z-D_i_Hv9=|w`@b(s;b^!qr?a46UEz$y0NkP1V2B7{Jh~%b5O4wFt~@lgFkF)`HQS` z@;^STzMY+&*b^WqgfV*V_ELG&*QSUZr1wCnTGOwfdrsj$o1Glra^u4iAtTPu-<(zW z_3PJO+SmRu)^u-D~{QPfxI9{`vZKd=H&p zpyOPNpU>tRr(nL-SySdT>Q=pFD^{43hNG9ioeG6AK?q9D+1kVfNX>E&a2-=-{&Y@H z!89Uk#G`VM{#`X-RQ-Ies`w!9A?&XR(tOvd@^xf~x}Rbo2o>(=Do0aFc8YHlUjGcr^o$bJ6t3Tas+vHgy2Qb?HaW|AR zd-^vTly%5Suc)jnlno!%?Kyq=^p7(em0{g{dnX(H|D-5y z^_}Mz}0RhF!?RQ3;GA&D50T?Kyh1OM^y@gm5xN9C}sw;Nq1|%MWuLT%$5P zum~;uzvWJ+CpT-VmTc9HYpSfAM)7RF%dN8@dt~iVEO6aJtn91xn)3D(DIt^tsk_e% z`Wu4Dp+Zk66ZrO|*V>ynO(6DQB3dCq#vJpcq_-Yu(Tm5INL1grtD**F3rsHkoO3=@NVFyb%!+QwR|f4OxI%z|U0JTWAYO z3oAZcV~-ge>aXW3_X5TaFJETYFOiI{-Iiu@=d%@vG)i04F4jkJ6ps zdlFi;4{0^O>yfuq^p`x~o(nq&E>LIM8t$|);G6l~W;$mbkWg(A=Ar6#O5Zgbm%{#VFbH-F^ zN5SrZ+Mnh(!;k-nu9k;$V|&A8II`N&HbSj+>z5|_uV24*0D=)RTeLU|^{1;LnC~)_ zMEuUqOUA!PLq5X>L&7HIkiXV~Er>RE;|~qAYo^>v*_=T51MN+>uxPjT`z`442-1&# z%xf}}kV6|EeBvfbs?I7j_R|^*)oHW7`aXo)qKbC#_|t^Ud}{st`SbRWXRTVbO7v`g z{x@E63R0YMum^nG`)8N;?vlqZ#9q=L!}RnHHvDkuzdWh)2p}4Z4g*W1QX_sM#P|Q+ zMH0Y&zhblTj}Ee3RQP!^0Zxj7eJo4O>--3^Cz!i&d+y7=v#(6cb^o#*S#I;|d8jzA zkM?Q$xvVT5E4M&vsUjM8vXp>^0E21V6)KuyzLX% z(GK*lp3dvh0eVMefNjnpnRM)NkK~&eR?0HPhp)fym7Sh+{R~>Z4I!cc$4gJArXh1~ z-ng(h$HAiBh&AkV9F4>FdE|93gsNJw98AR39a0W9Kj;0wPqKg8UuQjnICxcD9KlA= ziD>#J|9yu1bDs0!+#BI26c$d`pi574V!l| zT0f@g7Z^3jte9nQCXuOIZaT>^|N8A)K+J161gU5@CKS_L^q6M5B=aStPu-k5aT*3y zC413)lsdkIcj#VF*94jxk!=QUz8>4WOsGqVe5t##!5i7v*9__^Rp0!F23~jVMnCK4 z|L=%sy-nM;DPiR}Lz05et-JRBdYZAv-qY_)-Fq&<0*^u(H)wK!?PpBnmhVhH5{)tw z^8==zoi*Uv+WEkXOz#E>xw&)TEu|GfX{a_8l2p}F4#19&y=Qf0c`&^R3Eeo2-#$%TV?LiM#Bjod0Vh{bMEv>U#i)d@k#Ixs5Hse7 zZQ9frL2nYEv1>)$yjQQ};oZG=ug}n-&+@-~EGbDPMp8tgRWUgu>f84l-#<@`N-uhJ ztlpfHUisV_pY+bW5QL{uj~d*8fo8?5zZ3>!>DCFkXArQ3oEiWAN2RiXOZ#q8@YGXI zD?Ui9sN*u{KR_r?K|5Y5kZ|M%f~Z@+}X(PQ!vr`_k+ zFMkH^tJM`%5*SpRjYHP?Q)A-;pxBXO!-pRL+qbv8Nht{1T~{sAn~<^)zl(tBy7lTc zsoy-D>J5K?M5;8&?RW~qT!lujsrcy`t&$-ar2VdcfEHg|o`s?4kFZj!)o;7&s{dU= z69tunhr#j;D@`>eoC?to!O>UCoSdXX8-rw-yrve==%w%gs%1L%6Gy06M|)y|Xq@#Et0pH)?G5N|{IcJrOPA~+n0-=r z?A$rs-F;~7A$|Gs<-Dk6{aBLObknyNhFql|UuAuU44DZPGX=4}!{CklOrU=9qXB~Q zB{OtX=pChhA*#xxbXoGSp3|p$9^%md`_(_2s2)x&q&9pKfKrF<2%!reB_*b$m`dfu zrB~|Qr)$@d=zJ*@b=GNqCxe1`=txCQL;K{D$0!e4)lU?&#Fia9(rI0RNSn8NfN_t# zyu~nz!B2xX*KG>}L}g8qL5VHljTB1l|1gp3s?A~Ncq2$ZU=62jZJZFh)7-hoyT;IE zp-cj9xpX+)CfRf65$sbJ8^mRHqY6^)Jbc#VVWqyK2TkZR`%3K^RKnlGFxL|&P6(2{ zdcEzU+#9oz_-#Yq6ue3Qewg!y#CYq`0HWyFN9M;(j*Z&zu{lRYhnEbrOnw*hCMn=F zW~o13@Q_xV8nd}o$n2{ZkfRnY{y@Uh?DC;$@hgz5eD#T1^i)?^PhAgXq?-fq5bD9= zj9Ip9ue#{dJj3pWzPtGM&z?P_OEw5EmmnPZmr+pz0~CtHSKKMtD-3L&*p31B8kB=v zUfqe|a9p1K|M8{D&+DHFyC5HI>$Xz3kSFi%j?i&P73D#5sa`RbqEr;&(8aIzQDxaz zA>OYuZj?Cyu8U7LnW9J}@v-sScI+^>usA6Ezlx$kAqaKgD@C!8lq_&w;gn}cCz?ly4sx+(9PA087LJNam5bX_+i4vI zrP$TR@4Z71)zH_U0siWWfM7knIFP3_cbRdkdUfkgLzY!(QU1GWsXP_F`YfYtMk~D2 zH6n*VE0zyn2PT`N6jf(v5R;;1=+L2=9Td);sUb^g2oS5s1`x_EqaLlh{`GC-f9v-6 zs~+dKHNj&>n5jw5MgO0L65o63$xX9n&qj|Hf?ZB!C+kR#6r5xMMKnIVv>%43KVQ9) zF3dgEZ~8ytdOWy$cP!@b)$i`)) zH>Hkt6gh+4J{~1M&X1Xa|1059rS|vw@8(DnUio3x1gY_Ws-cugm{63Ode?nPyeGZ} zR4Ix<(1oab=0xUqy(8eZ?7{E0p@YD+W?>e)x7)wrr0(LD69|u!+!P-h zq0u~ofL0F9L4>LM$LYz#zuxKgk|oGf$RzF8&OArvABP^14sJ$w)Zcj36R!eFyV;3x z+qE2XXuz7&-oW!(&Yv-qtWmmI1!Qzxu z=ud5Wi-I8qSAIPcYQ3%-wQ8k10Mos1^_yyN904c`-1d%{AfyIm!z+jhfmwlW+0dJ9x1vd~7owD41KA z8$30r@@;Po(ZKkDL|Mec8|+%>d!wd$jrfjG*qWCNegxR%v6*4Fs`QAU5ZZIJj^0V! zLuykm@-wMid(?Ro2iTBI5qIws*btCn#!Bc>7nee9el9I_%`Dg^>Z%SRU(R2H1rpcL@puj``rxvB!_J{>c7RH$&K zBY1aCi~qQgnc1?zzs6Rl+dvEY>6sSx?#-LicoIenD<(xBCHqomg<`zu>7wl8(QgT7 zqq3BzHR|8^5;lhGg@kn)yRFryPoG$9y*sO!lL0Uf`|sB|HTXMT3(VncM~EHq!-o%= z^*{C`v_9bplu$Gj(O#UA9aFKvl=h{+b_6)qd@j1*0ikl=!GjeCyfW}Piufn&(#@MU zg^YZmXjXu9pCQ+>vt9M~ug~l>&rk)hg#HLH%f3Jlcq$ZIr#0^egg(q83;3irZ`|1W zM4e`LU?cGlq|280i^UO13scWbabOgdB+&|u{*(xXG!xATP&a%+6~fRd$Eyop?BBin z7+eSf9WodxD!yU#6hda@*EfNsC?vfJ?z|k~a+Fm2;N;{qd3{|ewLaClJW%0o#eBAP zYtyFlH%j>;mxdQFECz<NlP zRwd)xn1<%}bGvpLL|M;jx+~_s5JN;6e7M)23YEMw9y!3=V%5K6q@ep+`Z9Y8WonbyV69#q{S+offxM z<&NJAOU{Ltza1Yn0Iz9kd9GIk%7sRc9`W6hmKQG|3ui~##1(rrYu3!1hdk}3r-#G> zuHW2zCks-r$OQ;Y1q9n^mX?-l-}l33uiUQx2~QYC7DO?DWKdssTFq$ z+`WC8#223=7nFJ$_LLUxsh}gffGz`xXtjc&Nsy6hKtR+mch7Nk+$VKG|Ec{dvnKf7 zta&nnx0l9Gk%I{nuHHe>tSe}pwwD2~HpaoA<&sZRu$3)2_EAhMd zbPPGTe}51bj<!Mt4kvqRn~MGvLbPp zKJ4ps(%bpq4so|{m*7RudVl>6j#SF(N-PI4_(4Jgmp~lj+~__5)Byn_Z0esB8<7!> zNYCc8W*y2Zx=~$OBpyVwS@Q^8;$;C5LW@t76D(~g0tEm~_%Ca!ZkCpm_(nxV9Xfm% zO`$vNG9}U|KD~HI4fksbaBAO}aao?xiJ*k%+AtDFu#3une*~_EG@Zl0Vt!hV7xp27yWrW><@?CIgj(`*4E7LmfsaXjcJxyd0 zoeL#%Dgg$5tJpcB(Sd$*bYD4MXZlTp6~YsZ_4c?O(gVA07uUNF4d|t}`-?*jr~~#EB88PJUHYW6;N;q8EMHxU#3Zx;>`_qyfp`|HsVB z;BC=Qg$5|fo;j6jmi*j(f*cRJ21@8)*MIiyDQHXebE3FnY-?sM!9h!@!fJ1H)Y=yi;Fxd&NA#h%TrVuxO7~aL3w`ZU{t+f*g<v9)w5dDL4!6Hb zv+|^W+;*9LKz+>42wD2)17Y~^*feh7JzEMIAnb2My`R1Dwf+YkY32cqDxu`tKo4BawBjeC z`!ob|Gm}nsHn?~$V;f{8s+ghptNpYVfMI?ii^RTDZgr|=;R>xK&xcA<0f3}%wP)1t zb7P4(t$cQ-0H*x!w>e6RaHM|n*-7$iAG*c^g-I*g6Z43glCufbA<1#qSpPVrX)Ag+ z`W`zm0`y{p*9GIsym%!r=|kIPAeI3|124nSVUakWAC`*7Sc0dr2>Qs3i{o@*ex!6co`0`f?Elb}&ola9q^o-_~(Ym7TVwK0Uz8q`5 zO6~(X^WUaSHZbEAiPtU!xgE1t$6I+TiO}GT^~dII*tk)gmp$IqO@}R^spKBLj%Q~r zbS-|nZSmak^;BceQJ*`?%vx1P><1mHl|$VOTDE9mo$k+rgp#|KWV@8#Bz*SObf;3R z&HbQrbpzUXUO0l+Q=Ix3I;fkn{&ZbTMq4Ttln)LP$c8H~$ z^d~CEY6jy}TsHh34p3}2e4H@cg@uNh4HA=*jJeR-?uYQ*#)q7JT5uf_dx37-ygcy| zh}`_q1=T+k!Mdg1PrhROhj=X$55Qw8R;OQO$ejdOqV)L%6Df~&Znv#9laS-(C&SM# zJy^tBk0JEN9mn7@#koW7Ul5d{_pm2A;DvZ{93HSHv<*2UGDsOffNO(X@#oa>5To zr5KHWeD#mT%%JV|(He)O3I62BhmK%=Gk|=|+`B))d_@*St`?|IMJz348>>B61rGNR z2EL>I(hbH_Vx^Y5bV*OzPjMHCOy9UcjRweph%|8dHVA@81$Vb-lfqF^eRiB9Glf$-jamzQ*wN)H)NmuKN~ zbLGft5E)g#Y=`I9F0U^C@IF!>OXP2`NW_JD$&P)yP?1WhgpiD2@$Pf`)~#Ai<2GRw z+Z?5&$lM0gCkJ>}Hd&-w-ZwYp0oiDOC7JVfw$yd9gxkZr0IcJXFxu_><)L&f%)eks z>14ZP#KL5NtXoy3>nI=NFXiQeZj-DAQTFC~ly|Jdjk;=b!D&W-Uua*Q{BZZE%!;(rL+`G5Ygx`fcD5Hs0Is;~|6lTYS$_0?RiG03)>(+;()2@Ne#rO~Y z(BS7Fd5J>?@^gm0!aFVd^0L$7saW^-TZM`);4D=VHO1Dt@$zdd#RYf>#TP2Rt{&cI z%zru~zei@K#qC|tmZ#-P&%9v75jWr6ZEWMa>);J#{A1D~{glOOZ*R8))tVD9kIiE^ z%Z7|)eWoJFNt+&rWuVhGzK`%oRACpkIQ|NSc&xN}2fd?wwg*{Re##B?BI%4Qpi^kT z_oF2cPkpc?!H;d&QTOHh_u&Yq2d}$4!*oQax%%|!PxTv$8_hP9|9Adw+t5oFIrk1s z8WBTqlhM2r=|~F0cp&zAo>PyX2VQl~UkAhsLI~1N)VW9$lTg81bt=5kJa#~pLD`34 zAt8f{Usg0~pj^LAn=uVcule|Fn%aBrhL$G^c1;@WHZk?u{8hdBZ65dhv!~j_)RBuG zJuteL`qSa56HK?MPdV3O!u_VUFJ>!^ygbxPJI3EGW&FLCtpmxnUzi!vKAnvdDFvv3lua)6AxRgvUgyHYNvCPC-IXOA{ zHkW!1b>ZL1*U`iPQW(|xaiGSQ{+u%;tC`UgUeMcyb?Z3Vy7Y8Ni}0WM`7XIYr@$Ls zig_8F1Q5Yw2#2YZ>^$@nU~OrB#!PdFnZIcXw3AJnCIeai{%VLzaVl-BklizbXi9KG zKKCE)afL*0p%mHsM+A{UrR(qH%HS*q{CWItt!yj`R^A1pNrxh?!1n4d&UVyujx8A= zWWc%$T9mRuJDI%LHB{d!DnLCbD5#L=si~=0&9Dx!^;PN-wz@d^Uc&F(&I;{dyKbpR zI`zatvc6B6nt|ugq`MO>TwKlxJrCQ=FAzyJ112+O=*J{u>M?+zYusy^zp&D!e7wTYkVnl8zK@87KzJ>^ zwnQ-90n?GgCbj#}*>=dpnCTggopO}OHXndoBRs_`p0rHr2=$? z-Dj@N0^N-mjTT~!d61lJHzl?=CCE7PaT?Z;CNG<7&Nx3tf~>7=51WI3hZl0K@2*`V zx?FOEG_|DVAb6fq2R#$few_NGfF$^*`>6viIAzV%xmN`0t!#(yYJp9iUF+Ac2VB}! z>TzucIIUlqXSE88KkjIsjM;dup#c_kThG&a6lqCYmv-BJHqv`c$PixUgWdPFz)Y}< zkI(gnVi?11mP$(3s+xW1fYs^=q@yUm3{S;^$&l}iRdOR|!@s+>1+f{2-y{XMY1_87wv#29 zOKcRT4xNX(?ea#sNP;$r+MH~%9vc6}i`z}&h8_E-f&055GD9h&fRGUm^jBC&(>wP09BR1FFQp1|`~K+mKy!a6w1CrLNduBa)Yxp2e@C(${iKmrHio zGlK@5FTzMRa^^8En@!1ScG zOd65SICNp(5$aNPFcG7Eo!H)Dt!3)t$15}#>_2_FXPw+*Ak+ZMT(meUrk?h`K=o;l z#U&px#H-r<&o^&oR(&eYj+w|Q_DS2iX_NJoobFKphVl3A9eC^WMPt~g(W8xnuW}=9 zgUC*eqKPHka|hj{hcuoI{Kq^JTQd+(u=LkLK7+ zO7{pOo8&PfLjL{r>@-dl2g0~SD*D9@yWG@5tn;YI-&lWbE1BIWLta3S4-ymAcU|2D z#ZKUbC~o0XhX@b(e}?8><@VZ}MmXo9tQ&QX%gqfu8a|ERq3`E4vF-H+4<<$>qz(`l zVEa3YUFzfqzvr9G_q^S{W9^VB5mv*lW0&Cez(AzcSmjWd;QUkymL6P~Rh4N?^f9Or zox%wBt>b4`7R{*%A+jy&=-yWP0Z61WwIfLT+4%+HHfG1l#1@LMKWP52G0dS=+~2wQ z5fQ_wz7$u!rO}xYH|zg$^;{TNm4yW!2F1(l z5)ax~T&JPp@vc)F+_4{^vY9$K^tO3+f`0UZ&NUT{wzn#d6BE>l^v(WlM~AM~Xr^Dtq4P?YMgKl-N92q0`hUL! z#mz;4KA9=E9K6C?(2;>KL=sbn-+ue82vz97`t|jo)sF`1dT+z}f*@K-6IF{J(1e%a z+?g|8O6~ptt&tP`%dQ$5{GW_h&>9)rjp(P%=tGe-`+qPc01G0PZlU;LGQa__bPB_m zUw`X^P;O$L4%+dF$I~*p4PjRX#~62%jzWy?(OX_(5;?|aG1_)YgpMq%x?=bzs-aMplQ_a_i1&>Q^)UhTC=T-Qh)nLT^41X|GkUn zZ$v6)zIur5oSE?>sz)&1gkyaA*|S+Zj5eY2HVmE9N5vo{e-94xM*;-W8j|GvDrB$&?g2;kh#4mNSGk>?)xUSTN6 z23jVZj!4^kvJ71P-*DBL!S*MlJSXzBYN!bP2ww1;ItU&cm%g~F;Ej(4-pCw8F^qxh zZgj)De_ss6h#4-J6Q$b*Cd0jY#YXDY6GXOF^))|#(FYUYz4Pm@v}fPGQ=Go23Hh@t z;NZo40X=L^IBdH%)`Yt+qh5mNRebx_I(py0Zymb#=uwDNd2!3UMNdv{emhUfH0ri_ zQun;#CPIXJy{aa&mf6uFw=U?!9UGsdfw!LOyH=2(j= zJa=xlk>3quR0KQ~_isYGmG)@9(l6GyuD2O`QEv8frDxjZ_Vv+FysH@^%DKQTzB+Gh zqsD5d$RwvxO&7T32xAZ#`KWM5Im0Hhd5AWpm1XZAcDwQ$P8)#7K#xN}iB`IXcO7F? z%bNio!l2b^$+Lwdo++rDX33Ai2E;1c$@Twws3Wotzn(NVEbh zbYI-^T94QaHWOv0R}6umE5{CZsP96MQzZ;-46PH!hxB%j_~#`l>UE&P?|1FvI>ycR zy?tpSBJO!g?2{D82m4!Z`TpJgdRew(?L3gAzSI|tz>>Zb!qkE#OCm7lPT0fU@(m1( z;H=WmHX8^mvtUBL_JmB3p$u3uz5E7wK7|P>Aa6#nzD&v{2tmDnJ=X$lgFY5SYeV!jW}4DGd^z;2K^APnrf+C zkGbZe*GPUccMX#c(PP{5>{IPlF*$y%nN#^jBQm#<~~Y%w?2s^M<^njC#cV9-9LABdL= z$$V1mN)2|WaFv60Jcigv`i-dyQBlE3hCb_7zZL3zP^oR6ANoH!W zyfZjLBHO{i@x0gSd4TmeZ_dlBOvV09Rxc{Vwk#;LdCpv1dWCJ%TOT;F_Ndrk7LVW z>;Fh?$+eM<%kyOn8-Wpz_}JUt$As#UQ47=q-3RXkto*)lV<X8psk3Cglen$v!7S5$NFxo zSBEfHQtTQK+!|<`QjnnqtSs#mLcJx&j`O%&CYEpsF@2^Yd`x_I^)2U86p`m1<+J%K zrxGJB?zwT({vlW^+U?U^n9Kv;HYyAgc@)N3ZXW;U1V6up4j#q$^GHvsDQL-mgMS-ub%D*fA|+UoY3ad-l{? zb)AmrUPP)qRb6L1Lm8xrDRqEm%(2FbylQH)84zBFe@Obi1wNbpjZ+_gqoqDV zoM9C38|uV?!W`)-Ko2zBysd_n4`*ls<{UY%(wwMyaz)zXAEzKW z2|N7yz?;2&udnjtw-Z`8OVN}3Sg?!^y^XAPf#S8E-BO{~t4i^bL zX(B9RO1gFX@?_C?62uoo2mTa%?2S9>61ykz)8!Z|hkqlnI)cD(SJv8;17rY1=;4{D z-+Cke)1qMVLZUHLDvfM2p(~^j8d|^?NsD5>3Qb9R+RYMUI5yd2U?ispo9kK2#gt(T z`L5VC@X?tTev!Eo7>q5(D@=Tz(Hi>q1!L?Sc-jy#ScpwP&e(qw;0V>0?c0Y?xI+yj z;8o>%a?SQY9Y}Bj4E~6XBE`wCL~!Y?qm$;a!v>dBAuC4Mqf%#*hSDecR0_bU7b}uq zP&h{r%+k(GiJSZK3e8opT@JZ7uFE*7AH)~qiJ~(*$iF}%nOSlvJWFVEk{s!LZGY=4 zkdt(ILu;TLj(DVdoXROEJr^9oO$wqk^{qIEx)$(Ueaap=)A+F*8BDC%|LC?pm{}=D z7CG#_!y6LlY`1=%uU?}@jU<15?6H}sjpg<_%nu9iSSm`b>PZWk63 z;H1TJBMcg(S{WosG~Rpo@ZquXuFIEC&#a%?o5H3hsaitmP_m znh5Vq3hy&$(8ZbxrC2VcxYQ)i0t>}Z3Rf>FMw+z{dP4Pmh)I3Abn*5HP#E3;jW2XF zmRJV9buKo6Zljhr?zrCen#M#4DYP&xBq;l&Gp8*^3}5HUhu(Zc!7ePJO(}{-e3foJ zdZQlVPXkQ^?bNZ3Rd~=-w3-nBO*-k`|KpEI!m24R*>jK^hn?NhbzWD@{WYgf8uY3D z#mdU+P?u+I|Eb+04LKW_3HL=rL~y`Q^OVFt&g1O+coo8iJ&8~bB!dz3rYJOf{DlB~ zGTuKv(P+-0x(bWyAjc`#6U;&T+}>#x7H1Hg;G@M1EM7^mPKr@N8sqVlhX+0W&xH4f zpGr&30JGn}f0rDEr|rv;fYezGsgr3hj5Lw@9vkugOczw@iIA{;J|$fkHgxERRqI2z zlm%>A>F++t>)X@Zu+^hCSsDx&Fog!AVQR&t0tXW}@15y~% z!Fo`v9B!=-Ulrnt_VQ4A7Y@x=s0fUrtZ83coRybU&N7!yX#~MRuG`3_Ewmpf4H+^- z{6`;)uJVE$P@H6P>gGA9B zA|`yle5Hsy$dUJ-ngIvoTt{KvuY32gm>*=B&);7)r_cuE8WZuml7RH}^x93utr#Nu zER9=f*Vn8>TPQ*}Nu^$kVaUeT&Tkm0k$P-&qpYU~o@KvQ%1f3sZQ9fro7`!G zs?Em3if;?)jiO6K63mfgDTk*Q!XF&yx5P}}x8EEqlpJZiL_m*VENHLp-S4IjIL#Oy z89KhOM)*O~qhSqy|L(aOjNQ)6aGo`E-+5qe&JL2o}!6?o7Y@={q}P7!(zx zy|{9iSM`^`QVG{=SLvOiymsyRv-rduo?SW@ock5<-_E#|O`8rKJL(XQQ5tD&V%XrPA9_i*cT<=ZhR99ZI;%!J0f@8mtO(BYU1tpCGy4+`F$4#3tlo-D z<$6u}#^U20Sd*wZX0NQ7p)w|-m~pX`FaNd}L6g14>O*;Q}{h z2|9nZpeqvCcdFef6~-V2MwHr6|#qJ=H%<<$Gvu2wW=50{4x#gm}@yCj5LbW63{ZG2Lxf` znw-6Q%^LYFB|IL9hVGOvYtv^rIccB~vE5KCbl2Uyk~(pr%ym7)9rp7_ez*xIe1igENA5@f>KoZW9NA zcGx(aHu=g5TvmoB2jI_GAf>;>bq@985m}>{@V;oP`s5 zlD@^hHy*Dho0l?V#{sM%r@f$|o%C&NRg^oC(4@o zH~a=q0wo?(oBpC&NCAKw^PGd`#}GQ2Nld@QQ}!pB{znS)iD03>hsi+nwiu@lob}c%HI;=Q)NUR;{7c7PW;Wg zZ}Rf;j?!Evlg5!{VhS=jfD2kGBEM!FCcDx=wB5P*HeN2rTH()bl!F-{_AgarFVuMf%F;M)_U4_phHdTkQq7n#t@2j`g^4)CV3i6UglB zp+$>lWBHnigq)SCJZm~uW?$c*RsI|XZ!v|gIGI6+dBkFD)P9^7C?8f5Uw9@AfROg* zZ)J;A>MORg$kmKjo$tyhBBM(yWfo%O+wwy17--wt5tMO7lG*qbRSb@xtkAsFe(FOA zg1H%&7Hn7Du3vly!b?&-`6bz5`02XU2dIz(rs5ht3}Em!4rNJZS`r;q;|{fZHt1iR zysAy_X(k|5Anc#T#Y-Nalcq^tg-a{N!9o(8#OrHkbvzsY^zVs5^>UK2rs6ZAYK^gf ze7pCceB6>Zs;kRyV)T1ba*n!eIoohl8h$W@sA4~kbzwVV6Y1lnbT)liCB-#AM<-mC zCv5aA4Q1Ri$tOg#ZWy$&l)c&rI1!Tc}LbAY;og30*_P>L7=QlpTdc;Dt80ic>b>~q2| znk|}IAh%JqF0ia(Qlkv{rg}L4Rc0QaWPi#CYUZHrpM*D$jNJA6ssj-#rC%5ZySCx; z&^(l~QZ_TuH_rSX zBRfY?JZUtcU%;YE4>ka6DJtb&CG){&bh*^0=KC81%w)PL zg{%vm!IH&GCxVt;koBlRi^mVKxn19Xtg1Z}nHZ1ph!2sO7W9ZR>nSoJbH|PyLPnD5 z#V;&C#K2$OZHJ0Y-)0iBEp;U@Gdo z%-s|*5NVdB3u!pjyqMMIpfG zt_~4B^@c672A?OmYV-_AN8|)o{>}3qJs)NeI|;A*GJW*FCqAWmr=i6_r-Q6)6Wl z#oEXWXNVTj*8lmcA@u$V4tTY*LA9*u)4VjN3Da@-XlMB^c;W692K-VDd6uS4Gk%J0 zW_1=_OjJy9b8mG-fI1nRlr2ILTt{YVa^Ybn-CPw2XZRk*TjnBZKKdr5A#q>&=uy0+ zVUaJh#Bki8+V#%0vK=&`|r6mSR|Hnci!wid^lW8=?LzRWUw~G zFdTkVyR}n8s0f16lM@oU%(rL*Ei4n`Kh}<-BCd_yVS`#F0}xJ{vtogkvKj4qNGlK* z+9=e3dPA(GoXbX!+@hegIgHR&`&6?Qfq>@u8&9BC7MB{FnjpHzIoqVd>7#s*8ZM75EF$^lw)6ekxH)9-{X_E zhWl+6<6x%ZvsnmJKGK;xZ)71C=tB~K=tBa7;4>z*2-~6>*i*en4}Z_?ye{ht0Lht2 zvqU1NFms;joO@|vlfPc#(bP|$KR@})`QW(wlMHgk=(etrwLkbr)hde8z5f5y3ZZ$N zJ3*q^=ND`hww@`D0Yi5{#}qL?OUL67B!h}_^=a$^I7t$tEBQWVZfwJsDm@ieW22#1;^}76aSu&ku z(sB_x2xm|Gmlm;DgJcw)sl!c%%t%KwA;EBwC&4EuXArpaW%c_dOh3%HSzX#sCa4RI zL7;Q`tS6pc`22%|#{qg_-5(yWV|_(N$b9tYn*Wk{)%Wn>MGhXa^kSSJeO_&Z_V_5~ z(d$TE-AP%_Ju&0zjk;Aczbt+ucfQ+vHJPMpS`^5nl#m@6@Q>2aV|HY# z48ehQoeupZG$s*eB31t*=1<7l2bDh7ClaeW`oZ+Wjjr=C3ud5+3>|eKa8_h=Sg#lwSMQFh+9wm^FjOJ` zO7FK*wVO;csC3Mc_Ke$gtfe18PLfYXOi<{7IgttXMLYhneI8(ii4GXvgkdyJG9!?u@HQ1{@m&%N`1X$M_X1RxIfD5I9*B ze(|~1sWR^3jIf%~JS?FSrIhFQKY#IJ&!3;2gETQMnod*hu0_*;O2*+{_|ij%*wU?< z;@?xPd*8msGIa`Lwy11~O46u9n`$Md(B;&nP|fGP>!>j@ZgFL?+l_mT9K~7Vmc+q! z1at=_eI-DxrV*I32|E?@WTV;{U7%a_6q8J>&_1Sv|1<}_iDbWoP)faGWKNsEXVK|# z+W2`M0Y44X%4c-!*wNT=TIIkK{uy;+i}k@)`8po%?uKc0H})@ODh8G3nJMdct@WzW zOgu8|>BlR)=@jT0qkwY8C(SFcOT!GS*|6!ksHha2hC%?OeVN59V)|E@t$LaAWN8cwzP!n z(^^kqdhncIJ+-L+Ud*E>*+)5DxF4Api?Kd8<8amY@6sOXXYR$r*WM7@S*Co-@I<#9 z;x(PRiV!kqw10gMvR56PRX8~mvGHFe8CF)uf(C#8Nh&m~QYvA2wcNu?3(NI^wRdcqtW*GHEJip_>x`(XW#c#7cj!-ns#-?jNmj8f13kuFKp*ZxC zIkV$?^sMi5^luBO_td0Yw?;~Q8yla^?5ef(`j2HtdA<%aZ5pfm_Z3$3?ks7Zmrp|@??|N*eQRUo&AP@0V5(J#8ztfXy$&*cy zEih6>^9oFXIx-IkR(OsS<+ALILi~K@+EyjRwgAgO+n15!I3=rDbc*{7=+V7rPc^HH z=;+*apR@oL#RlKVwd>1_v=H!HT+vww09m8!mncizgHo&bJ`d*~!IH`0-gU&IENg1j zjmrQz(LENHn$qwBcDE`~9PBY{Sk6a(?A!I%G^cfRu0s`$q*lCDoW`=>f=Z2+y(;Pp zpNwHBSfa06To?P|Sd+Rk>oPm4;r`D>@ta|arf^=o7G{2C?x8)gTS|2%e2g$1{H!&( z(lq@&Nu@E98Zx7)@b*=7fc@-+p=?q^rIG(w6i-=635N~WZ}=Zl;nNaVw~_TFz*d~& z=e585_w{NnqzO?92=d{>JD)BvS^%0aD2?9K;u+H_jNyQUN#lY*>CoG-KWo?Lgg3|4l&9exFUOEHg ziedADlV5&7VrTuOcOt(vlF1RzE)Q%PJ6{}GwN~7x6vfbkp^2|8F{4T&$;T_yLD)Se z`907g7Kz&j&UQ+V9U1SE2g@OD+F6luQoM6f0JZvS4)9}7;pdx#Qg52-_ zr;aP+nNLO)RxhQ{Qub{oJbfC_z@#qE`!voa?m<{;$EW;g8G?nCZw6f$bY&f&aa}Ai zWJ($OCZla=-?M03F&eQ@#PWPsOlVDNUoh$JKt+cK5A23VK3X%s+mF{~qB4|5yP%!B z5tm6cEp!!ekN+8qeml~PRD z8UV%u+niL)0K|xa;nf&~R^`{LwW3K=<;N=svm4eNNW8Hzlc2n>&%N}Qu@N#w;jda1 z`@Zt^WcLrx^npRoBUcmOUcWkUl(@VQZhgV=DGQ)H5#COtdx#|$&$R3lxw5iY*5AlE z#jE-l6Eb4!ofolQvo5xKFM9|r1oX-fq!-KVGb6k9tm|V_&W^*{uLbL%ljRtI@cai) zf}|7KK2M<}?bvQfkVUOEfBezf|6??W=}c^MI&IYHn+ylBe2*LH{H2d`q&0>-fzgw6 z3OD88mrTdcBi}1L&2T{fj2;7(n>2Zh*N<-Z5Wg&E{s*cC1tlkNnPLgPyD`hFN` zz8bnmR;$3}9igEwobwwp_wE+WgIZ@m2R<1?VOwrl?uJHs%K$(4{tOP7DDM;YgocI^ z+whtn$JdDO%21pj+$ULe00;pYe#xeK*~PSVHf73g2}rkZkJ^XmNp1F~ zwEZbVZjH<`4fRXrKrt^bZu7tT$%hd$o*#`*d_6?kh!_>oi|?(X>Pd`hhQLPdj(+WZ zA)a2?h-;s>Sgm=lGr9J76*0wP+Rx!D+SfbLKPj zl=gBl&QMd&Xt9S0=Qyp(JA285zMg^ZQ~A~Qq6l!Ey02Mt!Lja#4aB$LQiczXNr<$B zua_nuyhHdL&Wip==(tgADIA<~0CPX1?7JHLM!;46eX8D+aUqz!ZnmK|!D6n#BIhAN0vkQdaJsxki@wId}kyM>X_ZPY{;T z@NQ8>a)mWC3T{N~q~Jf!4QQ_$!A1W#YxBOwsBGlu5I)78{?%1S-qhSIc3HV{I<-&< zvXhK^L^k?$`@x5sNLs2^U)UyA-?4#=G=UzMfjOMU@>%z=m%}aiV)Ychg8UFFC&k5O zcU07ueqPAGC*{5mRUDm0fOnd(byW5u(L94&#%d>=8jioFvuqvrD)vJvfadt@{Q4$s^$F{;WSXqZd*Z&}CC zho=kV3nmN+O)g49(9LZ>J*DEAhc=$+@Otl&Ba{9?DSW6JQx1}B^W;{mS`EQb!0!uW zXgzZDlgUX3_iN6W>z0FcB1NfSI!FWO4bwbrO97IJPLtkp?*#s#iHnZE^{1fzYnCn4 zeHraxyCiS^d*fDKe^|qPb0;e!UVB&E2fb9ExMfCvUK%tiy=qe>loN>GZqHN9Tu`>g zY@q*Y#W-RB_R)3GcFCPa7sXRq;DIkUdx?d7IufGIGJ1s@^?X)W#sj$}p;*qA#y2!4N52}b`|&B%42w3q z#099+6eOmIX(lF*Jkqkbn#CVFx?JR7Gq~;Hgp1v%LqO>l#XfwyLYY%t<62zQ&+F?} z%)c_Mii&+Q6vkYG(5Yv5AHqPRXxQkie3y9)YODR-{kwRrAXqy_k0g5OjJe(xDHZ1T z@`;*sOl;t#>EKT0LFroQP{a6!swIAVSgx?f6 z6q$RG+G8`UJPn7t8L+iB&f#Yj72G`&#hwX)7{kLP7nj{|cz!=<_K_n;Y9$ronoJs$ z#xL-4e3Lrfxx#@mbVQ_mDI2*k%X6DM_vqd|C#}ep^zOAPVW)D!e^TQm0|6zFBv?f> zWEChSj^`%|*4$6FCM(5G+x^Nd9~~?doAY%p;%C7Pk&u&fJ@rjnb{WX)1Af^KxHS!C zc7EK@r_=d7rbW5eR;Bb<>Wn>*F&!u@C#g@*_pjavwE*JyYEZrSubEl<83;q>?n2MW zy1Mf{_uk=mZ>1HMU9b7jqg+kZ=jtx>1W~9acw8SO_+L3-GKYx265C`@7P(IcF*>eD zD#8ul`cT%S(95Q@#CPfU3KxI%{hu9=uo2}gO>?@}R!{~bqd$nN22n8UY3|btH3n?dlFg=>?x+$EjFbe=-26A zb|vs{2a#`gN+?X!g&yzQ^FtTcRWE9i#pc&KL@BhA#t2FljIg0 z4{rY)=#a!ut$aRI_++AMp^`t_dKwH5-1-O-Md{Iq_L z_qf?rTRRo2rff4~)I$FLxoF+GrzRXPG2Ppt{j56uvk9Wg5pN-@Bg! z4htnX!fNjEd#+3vitDuWn-0Cp!%7~FHg|2db;}m<3qVHB#!5QX%IXO4itP8TbIoiB z)%i=7^g}0uXP7`eNShR$9utGB>N@cUY0Ww*gdcJtLG9cXeKLUzYTxrSTQC8|w0T&8 zg8P)tF&d85kHXPwa!vZS<~?NO036oL1?D_sd1elV?-|nM|2gV0*HQP{t&TU%SrTc1 zpXv#B)bQ1<^+R?uW^v;B*-&%JY@UUXH|1`u(rEYBN3H$1S4}WOr#Q@fIdHCqDoIvG zcOBDVn}_k59S_sRcnKN;szysO8?tN-tcU%s##;xF5s#dR@~|~14+sogV!8_YOV)vY z{`6@I)8{k%%4s|)GH5?p&NO&I0vfMj)CdjJ#Ko_1@P?B&{IA4?fMxqoE;G$kEBb>- z#f*#L-sP6wq^a+!O;VY><v4|b#*At4ZizigYrc+G&ZRo1Qm$X?lp9Qk z9$w*P@~1?KE6;|K>tFUFAo)fJ9PoRfBw zZrS}*wKr9DcQS9vxj69wjSJIc;RkZK3<#@x`8Qb~My6MvfXXcnei^aI{3_3ePTN$I zGZEBg&QBis{T0_#fG2~Gu0ga$)Wp@?1Qd!H86!!;!-qu&#%&>L?PadA@y~S>K4-Az zh>4eoIAi%Uocg>2m%7#(&yW2`g&L-^aI_PP^GH)c+a-u??VLfeD-%6bt;*4!H6Qm2 zA{sikH1und$z>Ej|W$Xa9k<=;PBkEBd= zD5EHKhKn%@ZI7(wDa9nm9*#3_=WO`3m-t&^*VYTS9WMQ{X|{+V!O-K$zS%N9nWt>$ zmIFlyZ|^1j^1^w`y3T;r;rl%+wPY6#aVl>GEh2?7z}@o4b(1rg*NmyJDbd;R9=}+R z-y>ZY@MfbOWlRm!%Z6`iiJ2qbJBd>LKUbLN zJ^mfXd%Vv)kI8-ie&6r4oY#4t7iXduO|;Lf9W!_-CZ|laU#m2G!@oa)_WogRV*aam zQ>L58Y}&q0_O%iV2I?|_1lhJe1_Jxrmqh{~n(TzE&M(-sZLTUo{TRI}m9pQI`>~nXAaK1dUajJD}`U*9IEUMn{^sCy!@_nvrVIi|2%EmSuhdX^PJ5iK?A?F z67@Y#kmNTBp9U(xqfB)yuYWyUM>wbRUJfdfO{daSiA}>^^D-b%S&|NjDnXyGWHPCi z<0l49cp9Gs(3uT~Qi*OdR?o{WpAUN6}~HbRq2y-;D@=_dtEN_#_UVW zKD1S_-1?auG-Voo>F3odbVtlxdC9xXgTidLr^<$1t;UFo*rNO65v?un=D4#<wgj1t z#M5=UtvZc`^v|rqKF1d#64S#H#mHSmcLAzsDRV)#4!B#or5)e3$ zT1%JAAh(@KJXYY*(XP&N20^9hDH~r&zQ@IzUvD6s09z_&2@%oJ(bdhu+&-=tuNZhw z_#c5~PZAt=72EuhCtxQ?s6iO!*xBVeHC*hazq;!T@Vx zUiPAqYNP4`%7{h$xoX2(+jW;IwlkF(B8nXF#t1wjcj^RxEs7c<7Yqk^LffrixBsO) zg^wi`oo9Z8n-i!m;ckZ`Ws2lTUf5 zkBdWUC?DWXF(sR*#N1gb@8-QrFFUJo{4Do7UK3`L9$J?GWlWD@i4^*pVPlf9mP2cJvvFAYS~|U>EPy-So8xp3(2E zTSrOugS&*~#|#xkr^aH-E!Gu`s}p2DK-|~e9f^Fr`J{@s2@YfG{O@HeeOY~ObpC@` z(H+aJqxP8{1ckM?$B#6VGv7YHsF*0k-#&R-u18$s?v4%b>>a+hRdk(CBjz`F+j(t* zxZUx=yKpRtY+YMym|iBYo;vN+Ee*fzlAtkq-@aLI6O+EO|Kv@@Tz zD<3clvmg=29GNfVtHk9C>qV*(2E^`Q?@?pt88ezRY&a-BcX|D}=NKM0h^&iXV$ILP zsR)7ix39!lXlT)UqA{2(eccpXWyMNS)+4d*GETk@TX5Yq$7|e*697(Ta`a*N9Tz_ zImwy2Y55b$PQpkQUqnWp4#l3TxW9_7kp^WHM!HKQLRUdI61L;95xWo35I0p-94Ek? z60!r1T68;J38Cit`m#td6S-F@4DmB<{`aaCUrojEVBAi<^}#h*$;UlfgDzg8*O@Zy z6JWerT|0lh_3^EG^oTM{Qg3;a?20}H4fU!cz-mLRtj_v%b%}dm3`tU|Kaoe%7i-k+ zu8&FoGyCFDAxSL1jk;V6R!u)AAQg$S%UR0IQftbBU^?_Ue1VXh7QNgWT#srpE&275 zzLzf=U~cD}I#g$?hH?(wU`TGSB;O5PjQa0dU$#oQF`Vj8hIY#f=yeM+JWqqA+6)ir zdgjP1h-Qojm4gxWwFBlsk3l3oI&ZF}9L@?;`sJRcmjc@YEx8 zEVPRPUP`;OZNZZ0RuMnhv3?IUfuig~wTJmk7J>P@0)sY9Nggbp$SAs3iWu z#Dsj4K*2zHbFgB%oAy-o>?~0lv`N0!)vH@YKhINt38wO_!-)U$9;?DBG^}2p-J;#@ z1%3LNL{};l(9e4%m3oP>k&%&R&N^!7X5JpRa6oiagg}uyn|N&fyW#n@pPlpj z^c*_YT$8;eBCK6j;QI-?gDE-l8K_wOJA2Fu;LzjnUuw+pDKIs!%n(W(QdE%-+SwusH5^ z>$cIL>RW=~nW9)0^BLmwh)#-O6TxnU_>JplgKNGz6e_&yUhsvf2-Xk$?=AWnx&lz4 z9%?_x%y0e3oZ^)fwXR3H>$YP$Xe}$LVqHN_fEhK*`m9;h`sBr5Tmb0fQOlvJ@R}d7 z?lZQEf>z@~5Mv$#`=SRA7RPpFJ^ zNs38x&j}wN=oqEX;OpZama-dNzGyZ+dCJNvg>nuF^K9%iG|_0~21busF>Q*So%Ic+ zHe8WRk=-(>%geeb^n2M3MHyK6vS^lziyLKe$mg^7=0e)5Tjx$~4wB5kR-gvug`Jpw z$&ym1O^Xe}Wq^g1O*<0FNLO8F=OG@T{j^UYVTrSh+mVn^5vx7C0)`^?)0Njlxn1E@ zofqGne=vbU1_y&KYz}=_pithF|4f*LTG*=)3j0>2lbR)4NNRrK_tOLGoJtSAQvFW4 z9{z$-sGE(hwe~E1IT3KxmMO1Z(DXC3!7_7Yb~Xuzyw!?Mzl%3{E1$1U{MVZee4c&l zgMlMTgvTA`{C5t*f@Xt|5`%{}@Ec-bVG(q<8@^o*raLiPsY%5#*a>y-pLeq}xx&z^ zpNd`T=cn_I)p>on-<`D`@|{!%5kD(mjP@0sn0V#Bq0adTGe^RylLq+(K7i4&Z(%#$WwJTFWZRL{vO^g4mwxQ4#4-$V9C7S_6?=-lspRAb`>xjwivQJRP$ObCUSJZYvK1E zg&HjadjE01`Mfduck4w6{G5S7@l6v}eme^nVQ_<)W6+~e1XVvyN4F(J{{mcenGT%> zq69HY$*@y+wq*`2;*<}1>lv!YZz*%}Yxm*bUwGC7qD4>pYF?{H=z^c+6*DTsHXk~i z-D|d>t!kqvXGy#`YE!*iHB+=eggy|gcRV#;{Gd{G_GeCnRVV1@|1s246rUfVHD0yd z3#?&7a_2ILzFepPXi9TG&}Z@Aj`T}~4);I%_1|?pc(A)~&I6Bxk%A@oe0^9)n38~W z&RZt8;rdcnK?E^Q@7rl1ZQ{l<$N0YgG$*EmSmk}Z$kSp0%4VO^cM{gq%H>`F!0^^Y z?tt%M*HiaUhOl$5@!Q3_;_LKh)B|V9|5XYME=`qzyFJku41dK1Xgo)AS1Yqee=S_7 z+yUM&p5aqm;#(GhP`rLK_w^yyhh}yA z0jl#VbE~x)taZ+tYm(+Z?b6EKj@o(fNNLnHQ-&PD-Wkc;cq~PUzsL9uolz?Z8UX<- z0_EOw(0{esildR7scYW+`YmBjTK~I|E${sCw{7^itMg*IG2G^@F!?;rVTsAB3L~ZA z@g%b2HbJ_wzy&~dQ{7k!(|GSC`Mcm98yPCJzt+N#! z$HX;BDjiI2nJ;?Y;Bu{|PS$NtcQiLo9uYNYmhde}!14|5kQv3vY7Vg+Hw2f=9ZI5A zFWEuacj+dBF|KBh2DDzf>hzF%OXR>C>4&c8yBw@aXhy);%{+Y}drEIxe4Fz3cb;?z#?XRIt6^lEQl!3g?$#ENh;nb6Z6#i)~Sdb_!$`+PclZ)q7Qh zMZf?5|8~3r0s=11JWUQjLAy=f0;X8e8C79SKO_d@%~kUo%X~oy zfd`g9)k%rhB>zC}%b@og`G3GzMV#}BwZr41C<1SO+ynbs&07^ban|zN_$W5$>e#UH z+7Z<)bGz+ab!@Yq<=LZ{MX+VUo~$LSSA_Nsfpa|J+DWC*pWn8*_lN7F7z;%-Ucr};e|J5l!>;n^_KxL!pTGdKQ1^J`;7FkoB4(jaeYF~izU^p* z?dLZScCM|%rTnvPTMMb(xewb3|HKqo1B2SSn@Z&%U{4V(hYkDl6tonJ+bqFsF*AK-$5ZTYC5B z74YnPY`$y#-+uFo=m&rw3r}Gq`$VfS(Bp7i1JuCk~g$+;BLBIvd73 zJ}o_cG(?u<@BMWT{`_2T1Ji3e4Wn``G5*9DVfSvE_*|B-J1-x3I<V+0+s11uY* zU82K2TnYdUd>Q;25=y$mpsEjgLrv=_Pr=iBa zJue20p>JSEdmYSGemYE_5_Oe*S-Njk43&8sm;m6N?sTvrch6C$fVtNB_cHS<*X1LT z-VgZn=hd=LS`$3y<>zWCr>5V@pErNU+4lz?7r|!T9lUz4fvhOR+N`F?q6ZODW4qau znIUg_%B?H#O6%6b?E7YcJ8{+qYJVm|jSY z9;MBO<82n)O!k;Gwjy{#XPE9VOg8x_^%9B-n8NYKo>ty%k9?In=Zcn;rg{ z35%DEm~V}Y=DEq_129{Kql#NdTFONmNy&JE>OQb@q{kDcDecyU6Iy;V88M50eNBPW z)sg0qFGoc3mf_`UUSHU)oA|Lx3n>et+qW+{Hs=Zpmmtpzx1J%WI96b2G7dsAXFB+{ z>v*syN=But$B(z~x1o4Nhh4!~H_*IJo)*Wo^`A2A^^ydB0kZNT%?4`jE_B$VxBm2p zKK3EGj*};`wv1BI8vK47e&wG5+W+y06Ny#L&MMj7PV#2oRENmwvy8|2FILHsxc9aB zsBS%aq`Ot}v>F=z6G`PUo8C2~z6}WPaTM_pg1D`%KR|Z4wO@=(KZ>E5XK{QWV+3VYM~zZ`%T}0NW$OWe|q6~s23wRQ!oxT25AkdtysF~ z!Mr%>LLr=np>Zqi4dVAt$@ynT*eq%CEH15U+QB#7fU&}br2s7hd>RQc|euL{m z?G!e-EW68Vr!jE7c*Co#SBJ>y28dKcHxRR3ejl|41+!oSD=8NkH~ZBd zQUAo_-n()$v!<;*eeJ=6DI9?@90Addz$zEMRiA}VgM_UteqxYHaw+B`Ik*X{rtuL( z-7XU@Hvep|t#;gHh4l=kjG8(#MN}{Id|82&brI+x6IQ@!*k1OwF=Vn9(c?UkPg{8Q zl&oBw^0gjzik=Yk(%kRQHR09Y3Duhh62!$Xe)#Ypn&Qa=+Vw;kFM9|$|4UxyIoaHn zefL1vEqdjDeihFhAhpYSG6}!ZPCzLkpKRy;%QOKiNEqv$6^RFyJ<^2iJBTPC5v%Tv zFmMLLcA4wtKoD-#R=*-#XESYTZv&A!D786u z8~vP`{IAs74{5jgsdq0^eS}_x{lG+u{h2*?420VxLQ6>d$5*aI<}Wq3Y+|F7I6;!! z+Ag8sw0I+-Q6tEW`j*Gp6|W`yU;Njp7axO@Co=v~J=>h2@Dqua5U&tG@f;<~Vn&sS z+u4(nlG4_uh8`_ zF*D_35N$-QSntggoOgkzxJ+|+Jju%+i)&?*mMu?B@whOACq!@hZD#NcR*^9CxHm%Q z$64z%bERp$#tfQAYji(B=`XJCD%*o!+&(IV;R`_`2Hrhw?OP?f1)BV^G-{&f<@gLA z-S2}x|6-KnJAAEhB$m&6j-g4K$&T(_1WjPX;|4mx;di{Oe6g$5l`%X6v|zr;eRv;f zf$czFWuMB~xs5*{n4AXK^~A5Yn3|Wks~*MY*qu|?Y^z3CSA(9n4(P3K?_6FH#(0o& zV;oI{L}zg&3CUT-WSY&cQ`273IqY(p7mXEUDCNjutQSrYTg7ZiHerdXfs#T9(r_2p zL3>EXm~R3)4nX59t~(HL|DDm3r9=5yD=3ciC)5=$M(MU!n_uyWlRbP}tLD#7MW*$# zW_Xny(UFK&tCpX6%`mIPm|4KcXoF3Cf3?B!`{7eJJN51#(^ZJu=`trv$5S_4O%@;U zfXuFr{K^DKj2xw3pt-0%yZ+K`*Mzt0f;kn*S_R^Bj^tV@4lzGETD4G`0RA0Fsrq&6 zDMuzvYR0rlY)v{>(&k4J0HzB(4!N!Rz2YgcKK#d?%kjK@2y|6xMWxR(Wn)zqcYP5} zlGMH;K9VteVD&!eOd@l`_9p!hHtK$NM5`cb>p9Qrn9lggxOi8!mkL}c&wP~Mw6!Z| zD!lQ+6a5=(LeKF(xyFtSEE_r1vTQVUJkn1+MN@b8c*%&!l?Nk>oB^d?Sm zIIl;SST+JY>{`*nQTBJ|$M>vLw_fj}aG@L8o+@wc?!lBrBCM>OMazXkbp&loF6GFp ztklNd7}`nfVzsE#f@ZX(GiLX{Q#2`F_fAnvYN@RIL0_`9otQ&W$_qmY#fznx!gDS3 z#t5+1DNFus&P7GG(5UOJ(CNRx8O73t4D%0{T3T98HH?$RNCFE7IlC)6kqgzh}9 zWji|0>4>Vv#1$;r3mY`dk&m(c$qSl5~hQU842%V+%TZ}`$9^zRVO3uHe} zy;X&+s6<5$%F?K&tqCpr{^i6jf~a@l$}=Y(&X;#ptH8?yTxAZ(I7$pTB%r|j7X>ZE zX6zzre#X?A8?4nh?@F3+&vp%?t)SEd<%}LaT>pvTTZ&l|dvna$f?M8ljk)O>Q<-iY z4yQD@z#|vqYq1o7I`k5E>A(`z@L_3!{4fE<_J7L|#qk+&4>sn^vTo~8(qGK9Bzh3u zp)M9r+dR$--)#%eL0UG8=EGBG$~9ea(3#;h zwC=+c&%zL5{v$vYbov(MM$bqilsNRVF<>dfEAuKUDk8unO-rjWid5dnILkxmTQVf- zbf39R|BYghO3b)R0Be!6kga7BZ)M(<*5hvCSP&{npg#}$D<&D)qD03BC-5_}*weIq z%kNlvOI6mG#SNv1UoKwpyP~@#B;-qi?gcnf;Xj8L6QGjH0vTDJE#;GKpr{KBh2Xn@qW+2Tr688@U_pNJ44v@pug$M)tP6pUfXHhu zOAD2igmAKc8N14TX5QtMcP_@(oANJ~v?eX3YQ5tNFp)Z3df}bg=|`s%-i`z6+fs3D z*h{2M&lEcvnRqN62^cHY(n_i&X3%cSM+QvVK7#5mvz$7VPvN}HyST%*uV3A;jT+Uv z-3RQfg_~k#?K!Nlu&{FSjq_1TC8F~6kdQX=ML|YG@*mSTjr znH8(sC6)aPai4RPytK8UXAJd%tZHWhbj_%tg!DL}i#A&dXv&*`^XSZ(zt#Z9Lvt#ak#_;qx zo*vzIoRcydtbUYqbaV{oR(|thLh+ANC67OXFf$5^p_Sp&U5dDLFs-YPRTT-9^+NP~ z^0>p@df)fTPGqc07g$G;RmA;*Ng?hRo_n--%_rU#Bt$^rS@CTDQt_vx%4z=Qf=59g z@>uG_s4aYW<@^i49E_mGKdQTC>P?O($?V_hl?n6=viF1DU;@fsF;}7cl)Y^R2Ei(S z|NPpHH{ecbsz48*UYQwlFKB9Y2u3RusA;Y-4VUO(i_!jYDf*^VKdp|>m>c7eB)@~+ zVrjwRoiY-K98-x;spyWxDJTgI(#%KCY*@oBzREmTUw5wzlS6mBV?&} z`lw4)Qp3RmV z_|;F@GGljIlp$nfYRst}qL^#;Y?IlRtS-I+mflt0(tUFnS)uce9@o0?1x*8dge0W? z<#0%{k<|s3(omd%p?#&T+WQ1Na&@JJy;d<8gFm{{44x18tUyTVCnD6aia_8V#>#%NM7~l4B#FACR+%a!5LhkbS?Ts3OCq)$BJUy@-Y7 zg)>?|Pes>Y@E8rndtJ;UWhi*Kzc5fUW^RAsoJ$(B=)M*zDcz>Fyd0X&EaY*fQezTJ zrJNEYy5m0IL7Qc_o>{QxZ!`d68j;xw_dqr6otePb$Gt~zvuD2d__L31RX;uc>a^X> zw%$)vLbyNk}q&Nlu8kvBvzKne^u*f%oO z!?)z=-Oauj0*X?2p#3dGBOsCMRx6BgzNy_w{9Y*at0e|uMKbL6~ zWd9?xWy?Tqt=0n^$ZM{8wZ2ZIoW4Wy+gP@o#UV2MKgs=sGWuuZd+a+D8|7=EG0Y~R zx^!-;&tn)?fM%f=LdP}H);P@3{~ZHtnIJS@YACw7ZlYG=gT;xSNUSNuOj95@esMS% zp!nx&ELM-!Y@qml34G!};D?A2^Zj4E);<5|8tj+JGzZy2kAMXrb8yI`*^mwQfBZms z?m!@KVYXegouDkb5kZ!ID;W^{(KDQt5X6h2U@$yV)uENSIzY|(@gxAW@b)=M(BqP}r zbKE--W3Y=dov*j=$L;JN(9ZrD1)9GiaJkHE*?gh@?5v`=vm6Wt!!LPrc)5+GsM51G zd%JJ*e^cLW(&M;QlIAVW4`l8t)`k=a6S1%sSc>KAkUZG>(gpNSIDZ6#tyS;VyasX< zTUf>w9Lz)*{b*3bwsHdpYUnA6?}-@JfSC;lvaL<`z? zq1tEq5Cg=Xh1tSHSM_ZwoV@oeNqX{T>X%E0N--OOh;&GLH_5{h z-zM~m6wc<4;=Q+Hr-_)|n%IX{lokICW_TEc!|bZF4&@o)pm(REoy^TcYfb_~=&n&W zgRg8w3V1BD9FqZmt?ih?h6xjk`fNwJ^K7Ct7wGz+)ekUP5itlAYLC)Ft zbkAAg**6ffv`W^kw`f7_Lgn zLAvN4w?R42g*KFu(8@dd=H=T^|D)bss_>9y!~!cpPb@Lb0(^WS-uw7R2&#m-cN*p- z20lhBV6jtAmISweF>(qJetWozgRD!EcuPwSSt`>_z}W}@*p~$frpb#GK4;h{C4NlT zUJk`MQBUu3dy^T@u{n|&g*>hM>I15BA%}LobjNMfFD`(vLbm@c12bBbj(?2{;j}@A zsQ&f|2H8t!O%rG+c)>@xGZ6r4`2+v^hWpKZ4>V8|FQpPqb*luvx9EOYB=oU9TwINN z+7r0=R1?`#mlhlqJ7l5zq`qzWtv@7SN9(r33gFfS<;-WUsh3(J8enkAMd8UIpK+Jq z$vyVlepXhIfGtP2uxzw{JaEXy9GRx+sWyGiAOmKDK!BwSMuZNSDsoCcWj8K7)Aw

dt-g8eJ<@i6P#VP0V6Ci2^5RcDHba|)@O;Yij44?;20>1 zCsF8%GKS1otetGS38QkHIAQvZ_VesCTC$(~6nB^4ak!MKAM?^t8K%zxXL5 zg$X_-X0zhh*E+b94PyzRdvR&Rs#36FSxJb*ymw1%aHPHPYA6ONExmfy3^Gww?hIF1 zU)_j79HkXvWT$K1SWd~T>wB$Q!R^G08g}Wmm)9{>C3SnQ=TCbH^+?*YGv(6M8=f}Ym7`V7#L(|shbLbj0|{Dipp;&KFd;e2jAmp6)#k-+%qe0i3@=)vUcDR~ zu7;QP#5hsR)mvx{9LM#6U3ZeKGMmbm!Gd@AfCI$e&1+}qH4Yu!6b^7??Gakj>YVOK zXNZGrI}JWNEp-+jX}()jDoIov+UW1~&CEDlKWnS_bwp$KhuwACO#OtbSYWj8P-3~% zW`LZs6HO$_qK-WIhdFMIOoztGj0b4r&5{K9R^MmkA*!GsntH59+xmQXyckdL8g<_w zpgc|S{xMBs;8qME?1hpZP9N&dIXQt=tozO>$na%tif#lt)fF|bti@Z|M1n?N<519< z2XDmUO~8CcA>A>E-!!!J9Gx(TzvT_#83xL{sYVYTh=8p3VUy7{( zwe0XWF^?&`N%RW`cI|iC*4g^d!UN+Kri>=G5li}j;BLp;9EaBLPm^C1B*YD!pjyN_0CmE_-tpO=#vc*`laG($qe|c zH3y}5%rVo;{?0YVCV}-QL7EYd_H`6j*c(VCFW3*D#7SzSu%B;Y`(%0Uqepj?b9f9y zmSyhP*0g(WC~NHKSFOAa+BGDg=L#E%a}Da!JF%LQIqyi5qffa2QqP_d&%bNaAFs}O zL`^Sg!M+;w0ujSb)rV@ouF{Q)k%cj4p~PyPcMDjCy+*z3m>AcQf<{eP5f~6~62z{B zTCu5E=W>vxClGqgj(?oLl8en6H0{|lBjaUR%D(Z6O#-250HHnGBx4I90s(@(-)YYH zKLH3X7IPvdf;+>;%=zETDWRMZlj&>100@e%9xfv^7!EHnrI!!~`Jvdv+?$41ztu!mOx;imTJRD6xFf@4|_o$!`We}zRlcyRop00ucL9f zRJl*ib=}{2&h-|eCnF>KwQ+z9k;_dNjDtzIt%O_|TgWCy(QW}N2j%zsMPWW>@L64P zx(7%Z?VOc26w8K-besjng1T10AY>?tA+Hi4!;7`nIOeP^@AqZwMAcN%CTv2BzDsVo zRYD|;*6iV6k3@O+%rP^l72YO(8dcA48-gToE`=k%;-AU(J@ob0j_KwYa}U1Btk~=H z>mxG!h9Dl9)Ol4X#09$Dg$ln{C-}rNawg`RC6Z!ZB*tVA${P{`nQ)5`_{#(6REq#ng@V$iH$l zr9Rm=LG0=1ub{1D@d;Ugkj_}S;e%pWasob zbQD%|=0n}#$772#1F4W6(>;!S;o@&}h!F6mxa9O+S)5JN zr_uUoTQ+sLRnq8M1PuT5`SV={QY{P1P&ddrpWcd5YtmQIHfWNlLULrg-`KGYFWU>7 z#Ojrp8wIOufKV@_sfEjmTJ>MiV1VaZiCz5imyb7_FL~ZzmmD8Pw$BuH%9ly5b4_$D z8+A(w9=tt(?^Cei)hRUesGOhke zP9_8WnjhutU5P#nA9o0|^U1hJoPG_zbdR0Ha5&{+Fb_xRr|nJ6Yrq zl`Q4hrOZ}|0gvL;)YazzNWe?-m*H??5qU(o&kT$j3~2EbRNm@zcJ)r`%$VIh^Vh%# z<5oV4)>{uUIF|dYS-5`J{ke&xp^L9hCul&QGNo0Sx%@UKPe1jKf{iaKyUXAaVz-ft zcC+KOL3W^uU5X)ThFw_e@??oOZ^WElJNJY zO&zHI9Q-@}a+&v>7`p1qZZS1VOegD26yCt?yny;c=R#gS*cG1(72U!#2ZWPzpY+d_nE`2{saTi?36}`w zr)I5OK|=(#qCU$3eK{^T1@caO=lp+y|6vn$HUB=d)YVxrX3vQE-IH8zH<$h=EbT8Q zrH^wr8?|egEWS4|I=$?YK`AW)Eq7m1pT*WKtAG`Xe@}3ZWPw!Sb=Mf#i>mh41w3Hm zT~xW9F?=h&He%ms@xYC&4NcT3@wzN_CNpr7fbk3hKkvcN1xU*B`9RG@ET0C*_OgzG zOCO-@2D4@U?EXs&bu;E@+4$>2quk<@$w_bazmsl5Q;dB?kR|n5c3L;1^Y`Y;qC#c@ zf>G=kQZ^a_1C#&*TVFiI1Q6ca4{n+~7U!Dchq*(vr;6zZfN6IIe<8V2YHgg?XHruf zSKUBXLvqhOc!M9iL0$Z>6jmP&G&a>oXMkwso1#clHb;rzKx|8@$;s%{3IwpnPTIYul#Dw=Fww_?~Bpn>b|=JM4qBP zj7{3(g*3}03o4UGLL#(!-ri+#fR;_{CY%r$RgY?5&>7H!(naW|TRu@9$aBGSS$_Od)oRz)2~P`{r*N_GNonEb23-swd{AvQ(C$O7ew9u{HBNjoq^hkIA5XB7-GfjkG1`X2STJBZ0UE|( z%Eyb~kuyJlGbm5+DH_!3>I4mq1tQe+uP9t|q)O}b%8Wui-A=GZJ*)3Qv$Sk)1IX+1H?>hUwCHewrU3=NXA$|<) zTN}5I>@;&e(%o_aL&o_GM?HW)7&P|pzd!wN#fE|f*IP<@I8%-?xr2++g1<22wyRNL zAY6@ErHhBd*7l#H_ob!3CebI0CeRq`)rpaIvis9>tk$nOq}Y&tx0G61Y;o$nFr~8* z`-OQQH^StF$q>O&x(Q#nc3Br`VVq@rb&4rY-0d$ifoTU4(=H7XK~s)gLkM(c+rp zR6GvDoS>ZPl$hd}m@%q5_P5*x!~3asOW%6pR=3!&P~X^iN7zFH---uCK{@YoPU-5U zb}Lw%JC$!`G%kU0ncC|kKVm9MVtYewmUjl`OB)&$z1vb>mC^#^fCKYYl9hvs_Z z3sbbku-6(IIv#{$#$!)g*7|Mxt``{641Wc3y?abrUWILZAa$iIUzPP+oNK;mHwzV% zdCbm#oD09<@m?i=OUcd~&v)v7JxJ%V$u}sSwxZpKD zPC<^8zCRO`UkM=7#=9k-7Aj63Bd`!oq1dXuc`~B@h7AzQ?+o~^NAMrdfMDa7yo-w? zsPHs&ud!*}j>}|XTjUz^f}XjfTfC5-Z)02g@giq9t6N;Yg>qEqHf@yF1z@ucy89U( zu9^F8K$bXp+9wFNYVHk(t~?*lVh!A<)tIffAQ%3eK^<- z!+4HB`1EgRxQAto@(?KEReh%Ww54hrb)Pc1l+gZe8=y#@!CR81slAB6SrXHBQ0hDx zr+(Z6Z@7(JN7t9Vd$0!DQ)Ro3r~}oMGiV<|auYeim+g<>SSkeR%vL}Im?|lpRGXT| z4G;}Qj%>yz>1h685LWZH^WH33)BPZC$j*NAyFVg%jwiug8mUzU3yTWw^>&(KrVOx5 z`szPBL9n~<-{~E=dkG9g;dP3&L+T1E=73Dvm$K6cimLc^#8hd2Wd3Kw3oQ0gh6D~P5uvXB~4xG!rK&wzhmUBK8@NYj2Q+cg=-`77)Y z6sDtEwTYF*$sbx#7PfO!&Va-@37mUoDV1m1fTn(WCVNMM85!Sy%$ZZ0T8l!koC>0w z`P=ACJ9qw3@$lhADu&OujMCk>PeNLfwK?>2!a1d6^wV|zmBRcCU(iR&oKglWP{VH? zOp>R{C5yYUyKX;^eA@escQ^j^R|bX7C}p)Qif}%M7tKJWUtMXdSf}gs?UgebP_t}O zKHTn*TsB_D$o-XCE2OUsG?fRL&x)pZ&Cf^cs5w6OEJ~(0|GLVOU6)$*W%k7>QhRem z)Z_>~V(Y98@HB!dH#>2tkI#TXsV8^}x~n$L%RGc>n#$QP2QH#VN=r>$J7)i`n-we4 z*YZKNe5T)$L$pWAn&Die-SVv!T zTXm<%=Hf*nYudYHEe(w}b{k~hV}tkRXas#Ex{BkNPx*_EVu^E+!M?0*P9f@T{SRCU z3&O}pqF%3M?;WuQxbd)(yB9&6^=k9JHN&%5Dpvh-tyetPBRVzwXkI`UeQnOW{0rOs zRQ`|;2zBCi)*eJ$W`8(V1`xdDpvT3!0(irU5%X3>2pbsm)yyXFU!6&B$lotN@u2|S z`ipU-Ura?(OTBoUM^dTnGEfZn(d89LV83!fe8o#q`ax0lx8xZ4n4yGoN`VI@J|ns)payW?P?sm+1?= zn(jO2(p6;&Sr=<(_}&Lr3u#Pmj4}*17}W5sc7^~y=My_vHadm-xIPF=L;t<65Uq*x zz8m#4oWCs2(m@^{cJWrPchh?L{Qe*KyenruMQvkb9TM#;15o_?mf8m`g6tM z0o~D{{25sGDygg@=|QR2hd}c$%MuIAau>c!svnj6X{@d7F-|JR!=2dRC?)ULo)oR3 zyvE7tseiG#q^^K$uy0kB>P^fHWh+;B5~Cc;?%QjPxg^rjm|U&;^*iPzXAsX~emH&B zQaGw`%gcGAG;-aUS5Agube;k5Q7R|H5%re55VEUc!``0G0hk?YQu4`Ij0BYJ=HtoqrG>#^L?s1ZmNta(7T`NgOPS|BuU$a9!qYHj_9pSBZ$|-} zz*y@wXo7(-BV;}(0uP>oka&cc41O|+KCW#Wh2ni&{J5XT*>9o7*Q#2}5wm=`XGDknW(!yl*q3d29z1_7t#*0I#fWFV z?7_^u?q!~TFueO0LLq@d)dM##p<)vTUNZlbW`jAT@I_>Zf{w4tXRFj`b1j>}%TXHE z(yMtF3We*qEy)R4wr>b*BzvP8_4Vf&Pkib3K~iHFVOZvFMj z!*{d0zTM(Y&R-YO33aWk=QpbY=~@EA}cK z8!DS2AaeDcuklG>q=)^Xo^D6&pXaw*)-lDEev)kih$u808SKJ&^I&NYm?<~n^wW;nSmtUhw;JYn=WzKuMl&{f2_-OuQm*KZokU`&L4cv$4^1nz9~SF-goBt;iR- z%yn$?;%8db_j_D1yV*>hRY3jJ=ZvHWsroJ(<;!|Qo_?q}(IQ@NGtd7*oo1B}P;T2D zG#{<%*tdog@24U;BxeOw`nD5{aN`_9^nADq9Qic-g<5rP0f(;u_$%yIIhT%BmIGO+ zbqxc{2*_Ew$mR9*HwL>$xJKPHFm;ZF4MFhH)99OlSrw?`XjyL~fHh<`se%L-777&p z6L;S~V+=l~r{}q!alp?D7xNnK1#N+7-bz`t)!ge$$|+fM1%`2JeM+@MvXCi`n5^wx z?LY_mJDzgb)WqZtfFcFdps5qx7@dM?h5O$yeAZOzGQ8^LeVa2Ww$QFC>Q~5SGOX#C z1|=VD&ZQlPo*SX%sIche_=ppoSZzI#`s#ntiBtJo8&h>WTY6QROIbo zbRd33LS}X9FH2vEboMT7^sB_(fK0FTrYAIq0PTn8VgFvuD0k3R(H9#A>gk@b*E0@o z)At|tiwj^WWYQG%sxYpo{6ZS5`7fWfXxXxxpwOY%2Jij-1%&O5NxN zhm&(*Bk4bRASSvuWWa~d{~gVAYXH< zpUAZ$2w#p!5K3Xwkk^G~3Sy20VGa3!xzELtk>9!<^sTNEEDg%}KR2(0O6lfwuClUH z^Xq8*^*XbBE?959tcammk9SjzZjAq?e){baJa)m|it2oW+f25mr0T;`+Q9 zQOm74p`fmwW%q_D3V+lu+i3R){!a9bjhz&(E^l)cFdDNF`QZU>oML>KdUMK=bXeO> znm7N)r;neGs0(;wYASX;55L>+w9noLpK#ntdu@DS=N&UX2%O@*BD+rgN3BksSJ3Rm zn@ybgfZ#_jetGv{k}k-7QJM2ENNhq2hRAn_-nZ;y@S;H3x}}{M6H}AmMT6zT@kL+X zJ6Q(onmmm1`5d+fmg<#CpMCw7XGPz~qN$Ebb$w=20e1^n2Th_bdH8V94ZoJsfh_ar zUI)y?!233ntcb|SAieVnMZWkPixLbXRL?ZeE4~ltZz<=r3;b$F`}1%b*Xqhjyur}O z^eB4dM;Syp)cNz{`Ed80&ebPs1B5lzQ6zX#h3XVKb3MH z%}9oM4#_{{lXsw4MhX274i=2E2q;`PSTuqs7>X=KUu@G@FY-k=7O%vs>O(?A_i+U_D*K?d^^?LdO^j^T1 zjZW_lor24|wirtg0>=`+Acc=yQ%5mDgZ|q%c7S(Q{sD$(F*)uys~l`D_SUkD)4`#I zQ!^Ax4%k*FgDYjzWa7F%lvbD35YPG=1I`#N%l?^36uu2`#wPuU=)-??vTU)^9T&&! z$G_qa>Vo-pZ=e%2GH|Q|+$DwsF*#JT&r+gw#qusDx7Y3wEM8I;b?Vx60>nOE@DXU0 zsp{>jDoUF=zH*p4Ra2$uLtZ_Xpo^K=E`uM3T45CN+_DjS;N@^x%eoP-By~lC_yIR7 zbOv)2fZl26HsRf)tTRa*%{|rBjeEneJRlpZU6-}qyGG(*A&QG-p@3dI+rO161BHLD zx7f(gP!_$`Qg>`>RnI1Imq2$Vz)p#lUr# z;JLX~GQ@1~v72b`r~q%v;i- zs*ER1nzSx3P>bF&%kD(mjn)ZnXT^@J!7)ma1suxTVCqXPiF72@_ z9T2WjzqtoM%=MI>GAl_=#Uk=QLuw#mEXwO53t+>mQ?DJrUm zUeO_9Bh5n5Gy4GM8H^1EHZWBpk(WV**bIpe`*fdgM};OPzRf(NyW~%?FX)dk2%0nT zUGHYtuO(5lQT*T9KhK)a9r~b}Rdq1MKc2FCllT&Z1N!c~!V#wO*tab@LkqAfR z-1EXF0NU{YkZ{|xMFreU)-R;2;hlVK{NH3{%)?VK1t7q%8bVJizlnZeJHxq;-pO<& z_##wI0^cOTg><5wtnw$sVFb3O+fW%UbED?VW-$g2!-0^dUHJTt6Kfiv^bp4yz4Zng zz@5e4SvE2<3)bLB$j?{R!vXE4QIVv~a(@!=Ei1}$tg!Q&_EaQ{BeN!bxTY&w?Srd? zTtm}f`1z&y&GC-TlZ!(7bH5CCkKlVfscNRX02!nul;TO7gBMH@#t4tp z{zxuHP)w&RpDW(i;d~ptHVPH`PGI1t`wf-zL_48Zwvei(L3c+#K5I-pn|uoY5goQk zj>^5omdnqWo=kSJKGaTz7S#6AuCDB|1Qd;yj78@oN#pGsgzXcQpBIro-`(=dyCO0q z7b4<&y-Zt#D?MDJUc7lTGPWvv1&LDaNpZ1KbM;gWAP}|u5TpgqA?`_LIF0j-VVj>* zp|7o*ZFP&JS>bd7>JP+WYev>riF~_M%4gTq~Z+f;K0PW>yNG6lu7YEF`9x zR2~oK$8G$ITbNcvOEv?7x>{W8Rsj|b`QB;EH1-Hm-u`h=RT--B+i!-EOQ;qAvov&g zH<*a(EvC-5jQ3L!GFnV-A~o9O=0|woO)I(zAT06+1}_?g!=}c24ie!j?^u?C)l!L6 za(~2X6!ZR*v9a3+MmoS^dYT?O->#AlqVdOh*r*wJj|?!MasbExgbw1S+m5w&E8yz$ zKtz%*hIzPC$R=+Qcnda1%-c>(g5n1wG8_t>+%u#Mf;#yPCZ|782J`=Fetyrw3TZp? zaZ1sK>K?nt>ek6eh~l;AiT?Lw=O=A$f+g2dpKE7hvz|^@BjsOmth_Oc>X!MqrV8Z` zf6W~gAJ}Qwe_xY4ZBC1YawM4lKDig7w83zBhQ06 zu|&q=hT`=os%=n%UK>8IU|c8r<19?Ac_c&B+YL*uS0B;Eo{7)SfKdv+G~9&k2)fZ4Ju`*J1LUtx)&;Wr z*1v8u8SfCtnW;vrhffIKDpZ>@K7eZ3=yV3Zw|$Q6OxRqIIc%Ew{Vr-^plICXOG1?*UbkR{cMJ1|kx7p!DqT-!s2&BADwR+WG0 z#e@0D*)(z@N&$-zNEIn>29;ViYbF;z&lp6JyOX+t>s}&vf&gV;{a?-+vt^mL_bdF< zu?;Ag5Z;I^wPE)jy+c-~>9 z;%jR$Ac3vxMvoV8cRIid(?uTY&=hsgw4m~QGa6TpoYr0STS5tRjap7~%%ffHshtI@ zO)S$^FvsbZ6-927)hghCHeYRLR996>QH+LPLp6E^^H#Aa787audB1^?8QfCsh?evr zGF$lAt(@J$Grz1_1F|zRx+GE_&4vwkL?aB>XbhC3q;Qp2q4>JKpt(;xNq;%RqDtylcxp({EE!9^C4rB>M5dqU+2xNH0<#mn2} zN$?daEt;KF0ktpJ+p&Fn1G%L#%3*TDrJXrZH4M|=57SM5Q=E^)x`Xf4#1=S0 zV6DYqRIXVdjp0&@3#F7FPhBBOSr-WAdAc1Xfw*V}XxrRk%7GA3c=Sg3b^WGFB>-_$ zZf>f#qz(@3B<9VUg+VgA*`RfC`w?*yr>`*Cwvp>Dq9}=9*jqw`a=dKU@UZE*`UHBH z{Ovz%ET@67owrK|-(x1(c!1*jMz90RS0cfH{CGY*_o51WUfwxg-$`Neh>>6p`9qeu_y1bF zR)C)-T^8VuaC=wJ)Mz1ugs|pdP|+dU$vsIO-nBoqmLGJXzcYn{MK8fYOb-uT6F4DY-ERbqGu0mXYwek#VsAf}kVp2Wpz;Y|<@3@mH>$9SC$E$ofMi}u|0CY_k z>sJ{POS#O+-rGyKBiP8Q{7lh)I?WcJ$~CY4HaewpB#6ECscQUxJ3)g9Q^{*Nyx!XK z z`P)es0P437`8ICaU}O{}i`1}oJG15)h^c|z*bE5q22-rPQ!zuJH9 z%y*S}?iuAGjpCn4uWvA?KKO&#IRc+STkvfvx2{N$K?$p0NJ>=Xnf%A(i=INp$AKdg z95fs4NX?$zTgL#|NTtR{&L<@FRVh+%8;Bp5gYS3Pww~aM#(Fpfyt0GvOX%eoDDHq> z@4War{<&sc?Y|n=PPl{35q1fU2G{-sv8P6_A)FxbVrCQzQ`43XO7s8&f1D7%-E0R+ z4ZaAri1YLSmEl6lU@zi)x6N{$!ovKbRq}R#?1-~r(-$+Ttyg~aM7A4HJbyeul3+C( zN*Vs`af+JN)rp(JbI#$3U9i2hkH$)1(2Q0g&0GCe;YrCZ(^GlDLn@emZXbDArC1%W zXPp5zU@jaKTP{sqWj82DAx`Jm?y5;s0J-WJ3!Qr9OBK^qp`3Y8Wm}W ztflA@i{>1#UP7poSZc}YdNBdvC8~|vBzET9qGq<{$OA|VZJs}PPmjGTsm3o^mhDEj z{jUpfT*^y14&MP7Eu3G?EVDzCT~iTJI-Q$JHT+dw$9Ugk$~FJGc?Ld#5Z$`f_=z!r z6WLhY{oBtmvc0i>&D4|Qj{Z$SrIE5}@r$e>d>o;2$jEf}hz_75BRJu?P^>9iOf7yQ zPFk){#vGI>!~6Ntvsu3C*uMR2xV&?V>nPt!uA{AP`a-;TK&1X8=@!q=M6+(pa8kU! zj!b876k)Ps!VQlnH?|OdHj_TCTXgOyfJD8+xH!1HZ{cB0sU<=iYHkwb%|l5P1+vFv zR4qlmKUJEJPtgnbQBoX{-0lRm=yztu)xUr1yKlq00ggw|{{Td&?%c?7KqE1*JJerR z)6zj2b;`6LKCyvAU|3<08M9Rq!?`EWA=2<4~Io(^%4$3V?KTS6nY;$}0Q%WXm9gI9X00 zWNEE}AYc;tQHDl9K21zY(hgq*@1R!INK9CeH!+ZbT3GqGb#BIZU|{c0 zU%uo(qUf4KA6O_7Zf=Z5bq8^pA`@XYN5&C=h&kGsy;?%?C-9NWMuVU`kDV-w16kB( zX6Zcwc_3pr{kh+dQY6lXcEsAdKoEJ{SteP`g$=)6Q>IwGt z!-`!!5xolDt+x8N%NX}C9}T$6TuMk6ohKaNtjIzF{x9v(8Yghj+taosdxw0$HdrWP zhx+qrBDntsoiZJfINW7=la2abN;?h5S%6g8IC;Lwzi=m`f5iJDh6>_gVd=f-ZT{}C z`Kdg}+tXrvtE=)YCCl6C{4eg!rsR;Eu?R(zbjuPeaaPe#4*>V?8ENAF^_tG&@x6e) zAJ}3OT@&t*;CAlQ#~umA*>Y#0qOsi5Y5tuh)?;=nXZ$noguVFj0h{aQ15*8-z>dMH?7?&DZGQZMQ&60vq`P$f=HJt(7rud;bfcV?B za4GU^F+ra;tyRa%lvV$HCiHb8vq3x4Z=l>H+UeDGT1hjon2NR589ySFEELRWMPWTr z(69=V7T8a{Hb$G^vj!HJBJzJ8ym|W&l+0n01gg7B6dXLN?oC!-1+3Gx-K)3W^j{E? z;EU3X)y<3k(Tppi%geTdA<(m(D#Wcg!n}~tusH5Du9NV`@4pYxumx)wP4LZz=^@dM zo)%}g;pH4q!FB}EyogK41RC4keY|Efla(Jw3d}h(tqVZRUy5qA5p zNjeS#tBckET)E+sh}EBglfw20Eu8>zL%dl6DgKF|86|4pH$wIiZVL%)M*mf8b|CMO zr}O5WhJ{~a_LS`Fy9z?sT$lO5(rysZ#nU=AoronQv0B%C|LoFbJgtQoo6E2q+qV+0 z7Mz*!ltsPufHhy+KO*6Qgd~mzOcYr>hX_`fyzE90ae4u6N4$p-9 z-%w-Bph&j5iz5qgK)UfC>U~&V27wnJidH&0HhFQP%5{ypN0k5h()(Wi@thUWD`c|E zyOD{=D;~9s)kNJ70(!6X_S0w&?odFstM#Leq6XAk%xYJQct7nfUAwlUwRAkk{NOm? zfmD319*#1B-M4>#@8!|nkf8TqvN348K4?F6xiB!q*ZMOh^h+>a5qlKE1QpnU^YIZI z0OE+I){hSs-4m3|nS=%H;5|hsBTwxD0-brh7EFY9nDqy>U+X8YSVZyvqwhBSGyPXZ+3IQ)|nOfJ{W zx|Uom3$d`m(0*s^s8sOckyT6=zZr%X(5DK!#68}H)$Pm_PewO<%!|ISWs|tTO527pnAA!&S->5Grenn4=kO;cA|AcnVkr&qVy3kVYK?Ug z$8h_EC8wAfX{HXuO;mCo0QO07ZXrYnADCIsS9qp1Khe&-LZiqpQbtqtP+v5R6=od+ zw-G3*d&UYXz+gx01Awg>exGiJVY963L4ANfKo?U8h_bbbXG0uEdp=q?si6C^({)tJ zI~Nz1PUIfSf^%*~Knq0qQz<1a*|dWmL=auEG$D`*Xkw8#giuEW+F=#c%xPH^X^>Ll zZQDf-OX>8)V_1BhF0@W##al1NGKEnpemeQ^hC~J>x@EUe$WNj*6Q6yCy&6?Lc8%yD zn|ic{4qo>RhSpc_0|0T&%llg{{|c!^P8jT+n7RJEM?fk0PUl3K@|?Br79?Qi@yInbG<_&MOr6eH_0(;P<(XTU!TJ*ScxD z)sOuU1w>1w7bo9_Y7x1O(CuQFl3n9zzSCEgzCwRy@FN*twaYbk0qwEdeCtaFEvW*< z`7!f@8K_`6Wp=Mon|H`+ul9h zjpKlH(u=1f1JOq7M8_SQI6#v1tl}Dx9&CHjysD9F0)R7u@lv-4zNJTUY2Gp~FKy}G z68(1eL`hAZT`USWvxj&=*D7gTO9n-Cq4aUo{i`VlLm*5UJy)f|)qZ^U`lwk%(hHyd zU3GqiNp*1Yca4M2?jbV0loH;r!3HAS*0cWUD>Cs*Mzu>Oj+J9c;Kta4+&&a?WDqiI zl#ZRV!A%Ily!3SMo@;9k;(lpSb(}ARbKRD+!}g%iFrAOX!JVzG4j#15*#3ydbgqEP z?g2fjZWDnQP8jH3`XZI1;72$pP3L96aAgCo{oP+Rl?=Oj9rjq@dgK>4huagf$H1o} z*Zjv|&xKX6t`T}0*Jk^rVP`}U0AGy!-5OCjrRoC_xDNs$lE~@**ryNpwsAfN36>W! z=5JVPHx-{G#FdOW@6t+f_5rsG;%hdUdW7`RI_H?wbRsYo-ck3q$wlo);W0$J37^^c z(buFJeImZU43x%EP*me_xfq<`uo*77UCX)68IX%{sc0amr1barc3IYXcG|#1V2F{9 zu0=&TMu0bVaFz70kk$ebtJSnyA~YxgZ^<{CeE6Q%0|V5L5{4Ib?1rV3>!LUTj<}VL zGH!klWn_4$FD3Jkv8QQ^Lr?4hh$4`qOqz6P)YHKFQl*~molfC>AF2)4Vt z$)(FWjo2`b)}vwV4Q(N2H-8p-Q2-dunpW`NyB0Bp>k~41jXHEVJ#y`6h`vIn7nu5O zmn=P^k3evJTVk~aWffOrWU8f>#spuHmSi|B`=#Rba%XMxpaqMcBn@`|;%6&s)TS?+RiQ+lIQ@*t5M4MB>-esjlP*^to?Bqxd^7|o~oj9Ab$&;?- z*Pb@hMT^#wG`Rw1u`U@G|A&k-~yQV|Bq-P`M^;4eBG}%*CISj2c-3 z>-cwDp+S7c!A%C9aXUOm*#!1GAPoZMT_Uo^PZkkl5<6|?EK6o;uE#5_s=js?X)K-pAhRh4J|{Ij1aM`Z(r*vQ>L8WSwlH-ZCuOAAQWDEkv@GXL=7BkxyOAj zZDLy5Ad$yp5?|x{vuJT6#B1o>D*JHX-o1%LShGs%NaXKd-2_Tx6MyTNQ|scZdCgDd zyUDS-b4I@tbB0~iZcdqX1XNC2=GaQEv3+q_l9kj3wX7$BI9S7!1~M2vZTs}$t9ka= zJ(>6=?U*v4S1OU|>5Uj6YTEFJ$+EYh`PuFMx>c)glghc(oK>}E%$;k))qu<#AQBpS)|F}>Ikb>wWN6jfw{Pd} zoClsush20eWoAb=O6U^2;-IpNWLKbYBB~h%y`{}(A%eQTzP_b98ov0tR?keNCn4wE z+_QLdlM(ybWyjhZuY~kVp?)wJki_Zt&b@ujOY$x*e>^tiE90y-+;<`p`?zE|6jnpBgu6R42IxC8j7IQ^ltC7hwd9864XI%*O@(rUM_v+gLHh z>xOB0(0>ujuC%&zEYc9vWN%Ye-AJYM*gDKzZIO?pc<0NkZX`>^*-&#B$iaL5@}&{> zwtxG4YofTLTh{J)E*B~j9_Skx0imaafxFZl39!;vHQW{aDyuo%+8_{B zEO#TiKZ}(}Pzd})Z-VHh&GA?&TT(+s^~IdH(q?P-L4($F(Ul3a@pmEZ?L$T5%3@SA zVeE<#oYIAKg^_$+Pgj^K86kX5QLz&QkEidJ_;q?eJoYD&Q>l8Pli#?P;?}EQzwq4L zPtwIIDF*-@f$DG_qZPzpIjDw#OB!$P@=S{935kirJSPOJ`y-&i`65-P?kNiH`U@sj zlB=`_xwbju@4r$t{TAy`F5ucfRa8@3ITY3R+cgUpwwRC?JlQeqRYCo zL;>I@ZlAZ9xt=35I6z2b5%l?(4%ye;L&J_WmME2q;wK?4@r;EQLqpf3c3Z1dsbYk` zk_KSpBr-1*e?5{~1&^Gm&@Zqwkl;zm{}O^6-c3Ty)1-2`EDB0p2(_>l7t{Kwv%wJDnI=yncyd#nOo}o_#*D z6CrXNwi^Urd|RU&Dt)3pN~AR9rBhk-zXdQS*=rx5*w-N_6zoTa?7Dv;VD=Y|ppaHc ze@A=-$vG*+*5#3ki1qP%FH~dogR60mS1nwR!2qYK%`B*v9Z;aky=tNfnKHq7{ zCBe(8)KT0QfH%wJo!(x;14Dd9u5}Qs)vDngU&vivkmc^~&TW#by z=V6p>+bl{X@5(n-l{+T=c-qR0G9G6qy91*g@%4@L3;(%K3VTO< zA)BW|7xf26Y5}OHcC;#k0YCmtqg~{UBj1F%8ezMD;K`cl3B-saZcGelu%WkE&ZiCF zq3SD`{p|t9&ri)Cql53jgF5I+(PQagNHHLa9I_2)ursIl49v1CNi8Tp4FD?Q?>Mc<@&@XDID`wWJ>q>DUSoois>B}u)o&td1Ytm|)>CgqB-I&Wwycd$3fOx`ZZnsWy#kr&b zEP1)^Vjz~iJWCN;MC!lG=}z})GvR&HL}O7fMamnm_}$}~p51!CM6_-{GsnroHukAP z#CTVxY9jy-@#ZBYy^!yYpTW|ljAoaglBT|h>b<}*WuCe z0MVKxPI(xWN-OF%!U3s?p*(EdrcG$VI(3SQ_|f{DRd9g2gELPEk{KstrmtcrM?@US z2GW95dKZ07sp@U*yZ_ZPEzp*r3Xe`q6v<_x{)zE(vK3XteN?U~-3(-kkC0*SxmiUs(&SJPG|%v8{8 z+8as>O47^dcM-k!p*b%~No`8TAbt8Y8;Yq+zHZ!Fv4ONZL|`}r8SEgD70{I*zu`}T zKF=v)Pn&Br6>;pun?S(uOIX*Cp+e&KZ#bVEUq&vyWhvH>?dyG5R;Hm6lL%ILsis#lT^Ge>?herE2+Enw_HBrmgOE{PZje?tsg zh0dQm<%ekad4=u`rf37vEN0pz$BtVF$_u1`OpNmVz&l$Up$sgR)L33d9<7Xt;q_m`6WQ_9(RSAfcASwtKd@O62Dkwwasr_F+@pp8rbA+zQ&mlt35be5^)IbMk7lc2YYCHRTm_J zrp0&8%#-F>eDr7duIVAsweNza1d>N>&sp${EviV@Dr-KuS0dX_NX@Ei zT_l&n^%8uK((@kQPkQvdyy~Vu&`<-#05<}aqa(y*qz{jqhGPimx{)D*DDLiICtH&S zj~%2D+?zHTX)VmnBLER8TtT#W#^PlI7sO6$xm5-O(y74UcF8kA5Zp{|AYKD%~9 zJ6B~s)7RR#WM|GyG_0Koi85?5Hz>$PStYm#9beL!N&vJ^1`c437=lU-*lRD2LzBB3@(xlwy+9rQ8z_Utz1ailUBUd|79@di(Leggu0~)tG;_(tZAGp>&N7WvYC9TW*8) z3ea~v;aH2BSW(956giCC_{eGZ+u@1B8(=&8xBU4&!7E|Z8NwFHFaKaNCv(+aE{Wrg zA=oW}6CSxfY+H&&?BxE!_u)KP)9neC!>oyJ@DHfFOLRGV@v##C^TaSW_8p2JJpm_SqaETSuO%k_l@Ep60Gd8eN%>$_ zB92MV2va#e$Rpb{YS+$|5b`dm^c+G}8PeBwR;_HgTB7Qio&7g{OLa?&s9LRB0NyN+ zn%s|qj)Y_cDZRWXx^eo0(e2r)UM}JHDFzNDQ2-Ao)3^sfMOQ=%bdw*)h1U>y zHQ-@u(*G)Dt|k&^OGX1}u2OMj=z3y+S44~VseydXdwAw!@S&I4d8t9$Yd!7paL^2m zau6ho4Vo2GtoWMBG4&6(E@eEk-2;Q_7sKzwT_0%`Z8aicaO)b;Ewu;7G->T(*>;cS zwE9O*=j3MgA9>fK-{hL7rq4G$;&^0hmqo5O7wjC+?!}I*9kE$3vO>4Kk**o={|3DU z1{6{i+-@mxV!-go+weW_PG{VJ7$_Bjbm$q(jW9gj1Ao8fQrn2)M$0&Jo3a*F#gLpz zHBkt(586eXlR0*2Y5o|EtI$y7Btwvvo*4GaEUiJ4Y3&^Xo^&4ySD(WC#x8ZUW=~$xkqH9>ikV0yWmdNPrA{* zeS7t@?Q_miG$#zwdHnFHwh7@1KJ893*HP5gA7Dd6=e-*h=xA~dTfEJ*M)Et?yfby? z5I?Ac7gaI8-gSQ-u&;)YjXcB7&i@>(`_N#0)liLB6RbBxu_w}~SJW?a5N~o79V>Gp zVk&c8{(gLtVxP1wr|AFC%VM7FVCLir7YBlBlao`CI0wY)zwde;9XWT|w^A5(cb<>| z(-hWqWUahISSQ*QHWF(w7^`NjT79++e{yc|h&y?m?dE5sbl+*Zc(@F`;|b1hTgg0t zin%DQnmOi_K{?Cr<;TZkiqD>BLHu`K~S8l?#t z>WSv`w(zR`?xFsmKp)zRiGeLH(Msj?In>p;3bE=U&y# zwWpdwxyB*5xbGC*hf(iI$oI%z*mw)S^NDk0>)$;uHxU}e`03MQm;9VVp^O*3cw+y& z8x!U?hZVL(G!r3@=;-JSyD?WZizPT!iWtuO^5qL5qA&h94$YR*Ap>;6dq=WQb!ybO z_U!K=z5Dkkxw)(EpEo~^vGSI*f?k{pic;nUTHN^Umtc`L_a@0CEMn(&CA9p_g}siH z!tLvogNsPS6ek<4pLP=!n2IFQQtHM-Fx)uu-B1cP!q8gUJd%K=+=3o5a)8DSL$& z`bcm&?ahSq;D=7ZKi|xJD^e3O+_Ti4ngSpqr5zwM-HR%3mmEHPSlFz%6WuQfb#u?s z&C2}J6~$Ktn@|l9rcF8RK}V>19LLpzgRLsQO0n;bKUXWPGmpBt;#~ujNRmALw_PZ9 zk^*+uGHgYM!9rr1KH+b%eG*L4r5k;dG^4`2;s__&McNAY${1MIkb>`GV zTSL^Df@ie;DQKu9*RJ+$&o^v*smyuLUc7)d*4^IN<+9%H&kWHYsS+k2b%h{aFUesDqqWl_3F`R#suJw%WWdv4c zEf2!6S*{WIK@{eJqHv?jm&|MhCn6{3ovSV6nsJVfee@Ok@qoW=i0Rei;uUYG&X3-A zn5~{-6_N91x)3W$79B5nIzk5_V@o_8?n%eBD97{Ymydpa%C@MoPc{&`G96=Y>ZZYB zA_Fo0945n3ZG&oSh$*YGw!zO1HzlE7z3m1v{owsOnXg-CPHIXDZMk%?ZY+aH;@-C; zaA=09EbYnAeoX6JS~}c^n9YhTynQn7r+)b+#6Nr}?Frb6<%8A&KdDCv(Q_-SB7v^w!kmTsFSS|ceVJG*ho=jU_E7) z7s%>Pwn>>0BSuh9wBm>?&*;eEKaTRAWdA_=noC=Xn3cs#hE#1=)4b!CG9#*w6#CGz z>Y{5i-^Fa~<+5T$sR416_KkxsD^~}|U-F{4bT-$S!_c4Dc@Er~O2@a57XfC!7W@cr zrJ3CkWTafcV>L)+q#}dRwVFqqAGSEFEz3Nxi^x|<{{n^8!|d|X;XypNA(JKu1Fc>CMTL=%x-`|4pR?2S&!LKE8mcR9l@aq#=tvPTwK}xdg z*UOl$3+AUGSqsP@lLv&5UsAEw!Y_#$1#%Mubd{^!S)5b0A3BNxKPwIU>&j)f@kpJW>qE$1YdaPNYad*yw>HX9KTY<4>Wq= zo6rX(_v{!FVAWm18j9!A5>Z4$wJ4b!5y*Nnkd1WiE%6S)2@%8kqc}iCAkaFiHU%9B zr=uuUP`hos;r!LJ1f!HDO8PGSiNO2<-X;=-LEQJ0W+LMrM0m+5rHkTUeeKm`!pacI5@trrAAN|3@BRf)Zl?E>}r&tB7cP_7(j$;D}_5o+I*DMQzmRgA)@Of9Aq*!sN2 zz@{>>g-oboR0Q+ek=t4<@@ga!UG;-YvIK5%J0WvYI4v39bpe{v%*IiVtE0wPBt$_h z^A^-k*_;3@G^*me3{3^tY z_U^rM{`?Z0qoO#Ta%=E2{lQ(k7FnXZmvBJfh}QOz%C^$k?mKtx1TNbHzelFe z%d`YeZ>h^mg`L6U5;1&n*qY-Tz&#R$uF<1+Q=j$2byq5_SYLSM4eyOiaWH<|Kx-Ky zd$-4x%&dUF6)6>n9QQ1JE{P_fv;a~UA2%j)j|^F5mV)Oa(WKTG(VP+(k&sLS9}e9K z^IURYO;Is~8YZ7lFC$Uc07I2d#-~r#_<5-wN;!Z|z~y5`11SX1EEQlE;o^K?sGEpP z=7w}tURq?k^z#HL#}%_WM?NFv}+^lXLYIAPL0L*QT`>QeZeq-2_DqK?D;NacaH z3QGPx44+RyH~V{5Ql9kEUAcL4t-1XvGIAo_J~D7l6uT%_-GN?f)PV(s7Bva)LX=-+ zi8o9u15sn(2W_!xr8F}u%$++|>^*V+8q3pp!s@4~9y8wf*u(3~RozkGg6>Gx6no~O zjV}ZzY{6z>g%HeZ_o`MBxSKRc@U4CHty0jU56oUM{s+@ZQ++t~Sb@kyb(=V8X`>e( z-(x)-U}a(VxS$a12$TIz#l4~tKj~Jex~Ayew7@vMZ%AlxG5ArEOI8z&r1KQggok0_ z@D&|AeE#yX)>;&*TI`($nlCzwJZ9s$G=Yot6D3sRdoD=nZmgPKaAqVDMY~m;9w7y| z6>GBzpG<`D)pp4o z-ND2}`-)!j>(__Kq@;EvXn-&zU>mQqzj{kX?clZ2PrukDqXtWAx)JSwol4n&r?{6W zN183*&LA~rIEa=y+R%t?PH6-^;92#5^CUtc%59l=vNEol$H=8bQd zW-*+APAn6HgBW)xym&8Ks7=^b=D>Zh_PbnqaLXc)g za!pQq)JD1b^; zY`HyDeb(xZT>o>;-#5(6%oKjB=OdY+qmcyEutY?&0uqV1uza307+rJ_Mmjl~O6U8& z^o5vqo{90jbFpk}O4}2Dh|Y721Czm#=#?|9SMOr*#7{HkO_@4XYJL*z5c(oZak%93 z3JlsnY~`cKe-TOdXQP4oRXwk4+(Py!@uc)S(RZRbbF*|DFpjHpUiL?jqK2M}o{?yg zy-7QwiA(k=>}h9Ob+kRY0JrR?x09+V1&NjB_FM49(g+cU$Cv}0Dtr;J?5*G$rAjF8 z%><4Sj(`kAf!$x^+pgK)G11;CP1m6 z#4TEc+B*>UPQv6JUpo2(GGgF9VT_cAMXAZMQH!m8g`6)I znX)@$=hcdh((rJh(ccR?;$0~%Il}mJGkxH3)tCb*Qs4xX-abB(jTzOi-<|M*WZFnW zLwPF~&M9k6IwsXr%C*a9mVoghEnZSGgQiklyQr&(E`|bA5FG?wcr${eU7El6$00$Z zb}lOyD2?|PFWHEL^WNpg8QYU zTtteLA<@WqS7116b8}_4?v=}zd$4Z8hsSl!#vciP zfrfzwJI;6#5OV@v3Aq)q*a3qFcvnC6@DROO{iXCF6A0wJ2#D^1p$X!R%8SpX&l<|U zxufFY8HjHcV@y^I!e%_D0bEH5#Nltrup7yW(5{Xt;_ZbiQTH>`4ljRhH< zDD3doJ&rY1@-88bKzbz!mC%y#b;9+)_SNN1VV)EEQ!eO1A>u`ca%G%=vBPt}zjBLP zyKbF??@Hv=_@7GCisITIDn+@#9B9=DXS;dkJtr)cSgTGb1cztOpLf4vUTjRut#OcP z`G~8Y-=#r8>Z5&$Xv>_0-XdfB7^8I1J1Y-6DGhSJKmz5^Hmn7yh4cnVb1d}(Td=+J zLzP8~7IjBQiK=0}9v%a6-cozU2Mx(wTmhLD9=f2rQosWAU_{r~>kxnl9fXvM__j=W zb6`_%{IFn!49kMSBx7QSMIEn6ueb#e(?CSm#ny_I7gm#W>>yu}mL&4e1dO7>OKxhq ztI6fgAHmN7Q$&DCejCwj%3cWjt^-z)h_e?6uEf#hV0;n1NiV~v^Ck3z2(xia5;A>@OV+~fpxdlP*{sI0X)xIoK8UkLqV00<1+imkzi(qI%zUp>TmmF4y> zP==*JyunWxT=8#CO+qFBiXGLemLO=&O$ z5bTN8anQgu-lDreu?kXX59ymlf-G-L5yK(Gu_7lz*Z@sgEUasGs-8rELnuj({)Zcer-o|5hfB1&T`Vm+A@EmUCPK@;Q3C|yUy`E(}e z;obsB;|)0je3RV$pU9%mec|OvtD#hIsgOGoDOZWO!6CgXBbh;M-d6SNI{D7tRW#p9 zU)AP?Z-|LsK^U$3tn>wVy)e;`a=Hz#flsYNq>)asNCwv#y>{7tV*>mwK^#R239B}K zVzkve#c~D7A7yK)n1XN!k5I_y)V*aSCO;Nv^^Z1yB-@^YZb>cXxl33v`Xu8heU=N@ z>FpgABACjdjNzLBRP$e%v#46oWj`f;SqC09Mc2Sj9dPd>$Q@fBa-kn>>vi*Dm8mf> z0_x(ZJwsuJNbk-qQ0OhNeO_&}9d(;o$wpI)Q^IE4e5Ym)e{3Y3p&WJTU8uNxJomntb0`KTeB+a!r`uWS3BgeOfg@qj?9~54m2yRS_tu0wQ0d2_2HzHQgR*?A( z0AJF`d*Z~2jsIjyIE~MJAWA>0*>*O6!ocR)6ynVB8MNa3bPv`x97^vQQpp!v}b!w1uD)vC_hE)>G50tluvab)m?>E)s`$%Sc2Yt#LmHF*k zHLt1W=H~mt-x>O`G06TW{IjZqhuIR0vl#y^b63xq8GW6hOi0XZwO6c4-!-=>?yg5g>VvH$a z9Wez~H~~TstAKU$XvMP3nKy(aKC@~}X=w$R8@u`I*t~VJM6GnexJ33qN&-0_ee&d1 zc{cF9K9oM06yfZntD`gIj_3WAv!;8Wld^PAG8$*D)m&2qQzxIvs)Y!x5A{}0bq5ZxDwfww)JjQ1a{PE z_H_7$`h7Nud=Cy!{qik>8iuvKy?z(pII?7aR!4KRf_XXGX62eS{bIV#5dLL;UCoc> zeMNZlfXupVtq9OIX${~C(4x~PK4v$C3`}yZ=|>1Hlj_Av0L)72F3}Gdi^SHj7$#JF zTpjuvp}mYL73EA8)@c0QBLr?A`qsDK{h|`_CT}i7bXdaxuL%Z1<3g;V`*c^S%VP8q zL9zyyl`KD^nQ=*Q_n&^5-a9ejJZmXpa-zQDhG#=96_`?(LibwtDUT(Gfg)6GJ`~G3 zT$FEBo0np(50YYQY9!ACxtn2DtUl;cuaZ@yadXtDQPSgp#b(6x&^x{&^zt2E3~8V3 z+sEltd8n2iB=&__xdd}3BooW8H;*7>;}e*D^Qei$-C$J^%|%8|5@ImbW}DqsQ7%}B zygV$dS>SKiphMltfET*O)^Hv_wUI!9)vyem28|*&W1Xu%sPgiWOZc&kogUnwv?~^s zsP;RSMVy(SwJ4;WxN>>+5rdZ;rUS z1x_i+msDki@PS++2paI$XN|mbl z`$}|X@oK=OD>moJF?d2qwGG6;v?lN&Ano69z^*vhD+j2t^@sBl zC+DVVzoJx3pb`gw%u)0aULRg;;}MPC<0m*eiclBt9RNmM-z;Lm$NiEai_b%-4-da+&q8 zlP(mwS(>I|IwUvyn%Z_+!cd1)l!}b=UHB0BCD|GTymaP^GN);6`{z=ZM)a0BYgYKq zAy`IGH8tm&jNUCF4^(9c=L(ufB_=uqQdK`|vDz{kTTQK&_M(9S4|c)2G?|^IkC|F} z#DE07&&{gX`1-;Z{7xI)5KiaJC-cSb(4b)FniDFcL(|$KNMf|OZkqr_(N}tR>tYR< zh@Y{-2UNy_F+q2L*?pvNr2V#ivoXJpMOxmrBoh3k!83%P1=EYLbV0QGrU)~H3XKu(Bb*B`2${E$ zNEC(WLep<&2wH?l0}z2Zq6r-4jS(X5IJ~PAA0joZ+jHWGO?kT6_gcLWiZ~Fqnhpe( zjY#NOa0fp~Y~I=LWSk0@$RW$}87sImmQvCnUJ}+meOd<=8INF7>xy6AzlV@nsYJ1P zz*feT-r+*g-k!GB^#zGg7*9j^0L1IB=*ptq+iTjBvjKFX3EivPazSYb<{P7ZLQaYuWFlb1^KEfl2q#N_bRzs}ZhxHfkc*OS? zuiw0Ro|b0Vo5yHPxFTpDaUCvZ7`KllZhfctz(zE2F)eWbL1M(WdED)8VrXyYcC#3+ z;uU+}?%cA|*bqL(sJEK!WEau2H-*MVryI<3a{6{}PF6FYGtf3bjGuRmvP_24(sD{p zD1#!0z>rw3CdD**8}$Y|0 z&&iw8Yi7fU#?!u+0b!@x`a-v@;B@uf{{aCOdWLy1NEQv@Jl@PwvKv7|CDQwzIPZNB z8bvfe%?c`dst;bYI*Myl21mXWO}2aI7B3PKlW)fM7w%%c*DB`KoS$sFF=6YC2^S)L zjzY;MF4wi%{?8*WNHynJonNQXvL`7dezsg-1V}qa?aZC$3-L?k^h^`-RS^CB>T&zY zl;@{T$^i@(bVw>hQxxUuEl4na$Ve_PzACRJwpYfE>B~+3ctv*pwy(vTW_i0O=YPY$ zBw5nw2O0@}LPquslPK)?NY4w*?`UtafaC2wVqtep#)AM zVb;W6Z%Al`2&33PpTua~{Fiwn$-#Dio_J&1KB<&%>jd56ivQrDG&!=Eq98&b;X zl?bpewKb{49*UTg`d59 zEQ;*gdzZ1UOFIB6X>FZ%&@W27qLtZMa2H`Q)*C2wr7Zvud5oI0(**)M%u{-u*e4%?J zN7U>ze<1w4+cf=`Oil72P!+MEpBwDwNAjm{9EVc`Mcv|>5~6_es4um&s4bIuwA=dy z=fQ{~^o!-tnism>VVo#H_o+j3AYn?w8SYo7?M128Li2H3+k%w$;p4}xMlZ44kJAx) z+|yK4ZPK9e+?F-TsnDMFm99qtei#XAt|$?*ho$2Rm>zsV^TTw=Apz)lT)BULlkC7h zU%GpUrCyTv*9nD39sq?v3l_CX=k(&N6XQ^pQhD+Q%uU+;&H9#wy$ zdL>8-A=(`LXz9&%6u?E&B25=YP#m3aSH=}Hs#Bvzjn3#ejBj~gU~Ys8Xj1V^cD*P9 zh;&l}+pB3iMU>2DqMHQTD%{TtrW$YqHYfEVBVeBO4LxDHf}LHY;Tbco?j(N2cWI$Sq zDBU*8%IY0pi!d(SUu&!IqJGE&>`q1K^d%7NIIkibM}ji~s$3l>orDqt3)%S7Y2qM9 zOtauH#NyJ^*r1-0yjIx6%v_h!-g2iBLiV5*-@m{+6~iy7RNQ5(^Tp_Ntd(#{CETJ2}3R+UgP`-w54RwwP-7^I3PLf{m#}` z^g50XCBHfZ-)fgQoyKw4pp6?gP;@?-=m1|)pm&iUc^`r_n6y7pmxoSoVq_ zboJ`_^}W1<(pz?R_7+dYWgn+s*jlGOaxRoRmZ22z%_+-6>c zMp$NJS&8f`T$Qf6c@|){pUD_15sCl#Qq2^_O~?t=trmfKJvzIfm3nJ=O7u$yed|NH zAh_V|y>IcW=Eb@d+F<`cmY^u?{ct{@=U}oYnZZX;KVoEUA-1FRh;)TNH1?L&X2l1O z2CW+1vdAev=?I>FHlxRBkH2Ug2Dq0JkyP=}6`3~Vp)^NBCi=U_y4p=(Qwa-X{Qc}I zjNYqBCmp(}#L=W%5+=1<_PZJig*-MXd* zNhy?$Yj~_xC6m`u5|Y}5x3~4Q#{(hO!v_GIma4PQ;xAiqedE?I;HacaFEHnB2;~P7 zuGnIc)rYxEq=G79m{V%;E-Z!j zG2F76KYPf9(PFYd)&F&u=|EP{(z(mG)F%NxMFLOec(DAtuYVP$-~%Bbh_VBi=Xldx zh#086U(?ilZ$^6KNDN0Ux5?M3qDK&SV(JKzECCQiKS7G^5G*%d`?7;{9=!OjFn$?i zEii=yk|@+{ZLg>neE{r~W5kZAHO2`f`_WQG-Jk$Thw+jeXEP&t+D-`C zY**J6igo#KMb=2wc=imkcUA6qCWY-}P7j=Dc_+%g6Szjtwt(teR z;%MFh_J5%83*pAfSYMHy+Yn=Ilq#fHb{Zm8>!pnl@Q|T7-8rxJGkp_;Mgk?G%CXM$ z7`Hw&7e*Y?MUBUp*->}koz|9GX*GvwWTHVqj&I;$=8H2$uzu@(5&z0rVX(s8A8w*ey!p3*2Su@n`kBa?FV85! zTtY_%BD=LY`CkFOnQ?~>!-U2BEb%qH*Xos@aNUz4cf>>*n|XPLS$S3eZ%`lp7g^!BlVGcFIJfGnzGO_dE#(mz?oo=djJ_TseM2vM%R?^Uqs(40 zSDXF%kyj&Sr^w5D8Cyeo-`zStTO0(|Ekh&%Z=S$Si})1f;jNRDx5K1?X6mE8OdYYN zz$#rUKA!XvQ|YV}_16}(?SR?}Aqx5LE&p%)1l;rx#c&6(5x2N^cMq(S8LpuAmD;k4 z&kJ~h8AkW*Zg2~<69g^0AZfLcMvb~Z-^SN@HdQEAv*AiBQ4}CErfY3`b0vE+$Rb|u z7`2W*#99f<*n=Vn(z@5#KDbOaOBFjTH7wF8ppf4%J6kb@l{DA%=d#r)7@F$vB)5#7@-g}3nkEox|rucW^R39Q0h87$A z=TozUuq3tk-D9OK98V-qO=UHkXieByM2D@WcnJGMD9`dheZ2}X<*G-3i0R?=)!kh7klZ?g!S|8TE)q;=DyqC0y!GLcr>%fK8hV_O3WKm~{ws)# zJlOetWKm)2>>g`SzrLD?r@p$QQy#Vq|MJMu?Op|9981g9eH&B~+oNnOTacgx>kAx; zf(+nnSKWxQ1aFX3HZ!GSv|>0|cy$tuQIXMhJD=%MLV5K8zRgR(7EPzkLOdW#2d71q z_1~|d;lgP!HGLBNo%~cbJy>v8*;uZk9!7LS#*Br8L(we%x^Cr7eD`E*;E`A}mDz!+ zkGeWep1j>=OY*SN$4cM0TJocn|G320Hm{anKmBmwR@I@oEjM@A8P+~-m4?T*Rnz=u zjk+ASuSK`+wQ7WQ(;eJ!PrYt?w1%x1>DK;m)#bO7%qBd{UZ9a(JMH;r%k1cFrarM* z-?SH|q%SbeZ|qWyYOu3^G--V;$h;LVuh8wha5l<2bun?Z*>Kt=XY{H$H-0p8*|KIPT-){gU7tPs z_xI3$mzDc9LXR3xx0>kATu>JN&6BtUSwW9|yoRq!Jsa#F80h|Z+Lj1^n-&nl=6}ATK%Z* z!KG-~)-NQag^7tt-Hn!|XiP7-&OQVpm*D@Upm8^LYzkV51WTTrA85A1iwYIE2blJ-=4NhYwGIT#b>BbeYeCNhK5WfH2kAZx-^#VVwCY z?G>f0y=6Xj>bqIy`I6VQWt3Gl$|ptN=9+zr8IS@G&Vwdbxjq{+&h=0N6n zs=4FFFUXSV9lu_;%&0Y89>4joDs9@dskZaujp;6}D9#zqK#VPzc0Ac22{SI1tG092 z_&T$AWz1yj{z9h}D3U4O|#L7TqGc=Gb&|ABcb2c*Heui!Z?) zNFg}an^A4#jPu1ZDBN5kcu3teV%n#@ZZ6DG7dwRnG5>AE|xD;8lbdw z_<_y0tKrJm>E)#Lyq%cXgk@g$p6&_LM~@!0G^xk&Z}VVeB`}I2#B<&T>NPgE1AnPW zon+ymk*8bIE4#iuGhwo_cLG0GB8FPCefqf$44}~)HY~SY@mprpH!hLJr^+rR4T}bt zNL`eaH7eq)BkMa7`Ffd6GM`4Xl*=`yd&usGLGiSUk`{epdQ#Kni)TmInioZBwApVE zntf!ieO68c;XNH49ecdUW~|m)$0Mg3$OfCOs45Jl=Ii|2*e)h>2YcL@j_5iw6h*&o z-I@7Vad;nq(UC6So>%+!gse_yh8fASuSchLPsZKJg3D;o-z0tSq$yK8s@gfX$bG-U zEQ+G0A2fpjJMvJLS##{Bi814IPZ>6AcJkJ`=2U4tva9K>?C#yO)|@z1Y)kfGy=o}& zC+OW@oINZkAfVBYex{-sUqbxeuUgK?1 zwJ@&L7VTXAz|(o5Pz|-doCh}8kztqev-+xjq-kpaoN5vyt|90!qBYScq=WnoEtvMO zzGZO2dqt_3frjBF9M!Ij4kw>1bgb!`ogoi3*CL<3{;4H3mN7f#r>t6PAHg)2=pnX$ z)nBR67(eXb;DCK?Kp#Lw&Y8ijXu z(fednDYg;chwPYfp;Wb_$IHvW80IaA2VEO~`v5RfhEJcKqbYo+CzV zk7No^-qXP+)o*s#vq|&41~sbn{SWXEdyXAD_WG{Q9cg&_91m$=%9FDluw=RvODqrBMVP*g$jkNc znF!hT_MJQ0q)+a2l3WJaVU$l@P_F$6rBoFbm%9+17ryXrzjl{sP5ryjoj!KxIFaqF z<^o{5Cy@<0C(M03wFR6fkDFWDINFr0-KqWX5hLp1kZLQso-|6v01J|*E) z3s|q0(q6yrl-vP}`0~{&zpyY9)2xTF>gz}R6(hTchWNV5m%&CGSHl@HY}9C#-&&KG z5Z+N}NPGKsEm8^fWNqfdLiK}X6n5r}A3DQNoIl=3OHW@V|BNb5C3<})bn8Y_c~6jB zDV0*^efI2GH^K%12wwPrJ>o1^)2!bUN^o~2cmQ5=`TG{KMSC5%yR`7;P*$#F&R(ng zNB*ioti1U03*IT}jJD#Q)e zF&Aznn)wV8C+H^Y)1sC;AmHgl(~s4bC^N2zMsP{J(XdHcEch_)cZYtq8T8Gj;h*k^ zWxL#G(K*aWPnKba^M{zFC6wZ9#JN#al~b5Lcr)`5;kp0e!#XCY_wD{+xs{3)p+Qyx zXO~s^(r3t!jasHrI7Z!xJLhcD|81pAebG*&J+5-Y9W%O4p~2Vhn1m5P`-A`!&KFg3 zp5Wjlz95BM;t?zA9?lGut12J3{%F39m3OS^PtrNEq~5Jtx3o6@HFsE9;BT_SLNC2N z>>iVj9V=Wt1lZ8Q*ogsjy4K`m6ktDZ^r;5(3TEz@`)D%rNk4eRS{}IT1dFUE z)4Y-~Km+#v;!Y}=)}~)i$Cj#7cUoBb{w)l*(f|4W>Oa2_zu3&4JzE}l`KxEC{Le?Y zp%@=gCNVKlEDC6!iQ&h$h@a`w&L(fa*sI>J3r)V;9zDm@+dcKxkS0%$oN$YEh3>J= z+UWU`-##h60_ai3F`t~qJpb}nJ6$qW%w5;-+*u3OPJ6>+c$qQ4Y+^^gZ1wu}*pmk% zGw2Q})&$tSv;I4>Iz6(8RWJf7{Ca5eIguJDDy_l2u-RWTGoy%^#rZ5bpm^t{s!{s` z1N)R*97#$%7Wct7CP4LBoLl}!_{1Bi222HUDtgo)gmRushPz!i)RtHLb_@I^SxW-K z<;o0I@st1U z<|NUw+k!a3z@6Lw?T*{$|7_v^XX`i8O9rr^t*eB4`rE5l zug20^JZRuR`>j7etl-b!3WXk8wGEsOV)qHoSh6>>CI%re4YJV&@>r9CKFwdh{AGQ? zvC$W@w>p-CykL8-`g|ltTthj%%)fVvs;FkE~pVS#li zf10P#9->65qg~c;f}UE(PtTb%2b*J1=GSlh`a-q**~(vkNh@a+V2vt39XwbHInz2i z-rLd=x;ba{n*9Yh~J@@@y zP!FPLqUAqd`dtbw<){GceE-g$<0eg-v{JHIrU_KSFZsM&gep#VB4gpwME?067_Pz( zWYl45eAR2z7z-m&oxL!gLYM41Dd<8JykrT^>9Xtzd^7MruE(O#JuU9a{-D^}fU{Jo zVnr%dD?vTuA;M)d0(Uj}kTFlQmTW0>^Cg^=+Apf zP!#-}eyM_0FZk}SuTgPkm43zlefY1%DE!6$mc8(5 z6#iQKe;cmwauj_1zoS;L7QcPU-+cRIlQ4<6+ItKq4v1drVP zuOMWxSK-(oegLsk2pB2ynSXU+&Sb`wdh4P9bhWc`sj>_r`wtBbZ9(5QKww26qTgS) z8vYSaDT>c)h+I9hpEq+|1$U4V&5JJEAK$)R@xYFmmL81l99{SKw@{Q@frSzq(4i3e z=H`O}jzPZP36N3m!$-+FKrsYwvI_7nM%0+Cw`gANopTnvsj#Tw@(xQ3%)6~RRdR3_ zjS^Y|1KQU**x6{(zMHG(_|_fX-FRVkqwaxoPOKa=NbAy))s-u+Pf-=M8dP~?e2@Lb z&o{4Uu~g;xH0MRlq(_fdZ+qH%*Q?>zLJgK=o%=d5$=Kj&%)H-Is#2Fce_7?bvpnlEc7Th9tev-@3&!3_S5|HgAWclSSaby z9|J(=bg^8!xKkRdDpsW1aW#)ETK)9jCZ1_i>Ca#HZ`%k2_4$~B@9S@svY@2Bl(r6M zgeJ_s+s`R=O}if8J#Bug4a)EM&-^utoWFU-38G$gw2(d@c8_F8I4@9DVCIPd#<3;c!nCwQ{8tiKNWaZNY-m1)>Z;e}0J3zJv{cjz3?$ ztR$TRWx4<8`-IqCwW4dz^62sVBj!hkTrT{Sz3#x(JJ~+XcbUVSHPT=r7&n~SHvd9% z{IvrLZEyXDQjdYp>acjHtS|+sK^OnbcvMCktmxQlTBt1P6+@4eAFJAXkR$)(3_yrN z!|s1!PUidi=oEUpn19Hkdv65Yn`+?~N6TlQjKOip1&Me*G=8YR`=t*$+j~~BN z=%AXnoJ^hP&bps1`utsd z4R(K^l>Qmm>lC_>t7^u{=*d%F<>6T|h-k>(tD z-18!SXp9z)9_;*ea`DIoV1>*3f3CJatVhAc8Ei)ycJINHkOcp|eUqp*>j=@p_KGUX z^#nlteAJUtX{T95gY*5DHSdnBU+`95{eT~>?rrDbq!)ZQ|Eu?UQpn5Aj5fP0Wd1om zQpKc^(GSn#^g1NE2T#HAuT23*3Yozegi9y%9x@$%_5MHYMPvlCcPqm*oK5X0(DDl< zWOo4Rb}&1i4hM&RrU~F2Sip z-wQQpQ~~H8rrTmYQ`O*Xx$^;6u3z`Ke*Jnc$~aK&dlHAVp8DuhVAAhf=>1fn5^;C# z*lXLz)c{Dx(sCPuhP`h48TNY*9C(m9vRt9l$1 z)YY|gT{J;fKXWj*~4Rl=oq2u(D zQ*^Btcg(esRa?tWK7QfB4T=|t)0WXy^fe&t8F5IBX5XNqbQyX=pQrbm!>3-m&h)$g zg$JGC*Ta~rQY7E<>W_9tRh#PFv(k1aA*|enY^PDwZo{^1)geDr)im&h4;RHdX$~sN z6oo-04c3S;q|l_D(wLgfY9=_>Zwien9^@|fc@8`}cjBshD+@2gxsxoIsIg;b9HpB8@D+eHLWaNt5O(80Ie&rvFqrkio&5#qYb_q z`ySf;Wk?&}*1YSO>d|VC?B44wqN6Bvf8Y7w32R)INu3vBJI#GP)Iz&X4B^57E7Nlc z6h6gL8~6*mWxbD&-EK#QXN9;K+&8CWwDH5|$XJ&zt)AwGj-CS{bBt<~*Vd-mvZ_nA z1+nZ-%rq=rL|ZiJQXw`}?pCfj^8O5Y)X@xm0#!#FzO|))E9Na5?(e%*n|9mzi1K}L zd`E|WPzyVT{r!qWoTtNp9(0`nq*?>rQHQ+Z8D;qix>6JcnmQ{Tf}tqxJ@{29K&-rj za~lCyWn(#ngS2L13&zq|>ckBD?lkya*oEvzj6A|E%|~DODyO3GE3UjaVWf}iKYJ*Y z7l}yw?&{QmIBjITk8KKtWvaa!V70dn-M?>Ncg%L{TD0JtB>qN2utJKI-h?HpU--Qh z^EJVckaUp>=<3tL%*+e|cvYzxas&G=kKgffTj5jdHM!Rqj@I7jc_H?=GBp=~33P3;bopV})wN3*m@7C=#q=@_?HK)Ww3<(mhB77tk z1o)c{&g#%c*3eoiBv(aoSOXFqv^YY#4kiB(K{~NcQp@L7LJ(8Qqyiv;Owy_T^tJBc zLKs0#4SCr5O`EC^f!K*gAw5_X<&7CK){SjJY7ip-pb8Ro?JVtgfiswyevvDqT3e&DLIq& zC7I*Hw)?y@!uESV{A=FMyz8YJ?$Ba**IIG2uoqZFQ$ELi5>P%j=}=w0Hr64+Ihux! zTlINwF@l$<9gN9*|Fq3N*GDxe*sA7M5NO-U$@n6C&jjRjTcETuTFb(%>?&;6kVoI4 zV67qKp`XS-6t6HAPai^#sy>_WKr$c<8N^ll`0*pxnrHU}#m`ETu8^nHW96!{)z-pR zn7^{!`|!S7Th_nX>Em_FMEU~|5Nin`I=iQ^jL`3}OiV=^+nxGY|9E9!Ddga)iPaU) zO_%Q&3Is`xo}|_4zcE{tm0SQUlk^;oQ<`!BPEOTQbs4D0M7V91hQWvY=tp zQ^`viMWe-ea~>dtOzBgjT_zjx91o6W+!Yg{T*5=nd+_8wGV?;e^P_uFq>h9Jzgry+ zo9}Bx_&}H4C8$9)sfgJKQ;FVi$8k*vpYk>@St}gAnpkY7D|J?MgXk_)vAg-VwUl|y z2C|6N?V{-<(nI8EHAJ8nrCe=(3;r!}#k~g|sI$tvyLeB~h0-amA{XLSGH!UQ3D;NV z<-W_SLKf-V6MG@!5hZkE7+D8Au&txxPHqgd{VveX2f7ygi-}QnR@BsHvfE0I;wdfJ zWswz*;*y5KwK)5KT)hcg&UyF$e-%=M7E+WJmp#c6+1iA{SW;sPE%rUxMWIqci+!&! z_9aBNHYJR8Ci~KcOqN0_QvILjGWYMkzyBVOd+vFdT-WFGUe0-)*Lj_D&?-nm?>aT2 zGiRTWbXvQSe*}DcKk;XN(+8&}rU=}WJ)!}M=P#>P(Smx=uSuxwML>`kzn7GjQXnLi z0WRP0W#Iu5{oeOGe-O2k6w*>y^Edp@={{x zsMTV_2y~$mvImJY^H(xiQDQQSpjN(y@|2Az>nz%JUGkqJRYY%q@5K0v7TjMe zEqTq;qewc%$HtmbZ`~YhG41xA-aXp+8}^vvNw&<&!h*zqz9eWxRe>~J545^yg#1ll z@`o1}yPiy`{vo2;6G!P|Fh+uhX%v8^?|txJ&=~6mKF~)VjKUyl)mh*m$k+d>3$AJyqmZ(G>Yn=ee)07}~E4K=SBn#>A{xGg0s8 z>LPUTyJPd%nX1bD1m`2TEcBA)C$)#LQ*~P`F_UBSeukveM3+gKbz{n{w3ZI|zo!yY z{@c1;%i)iP%K8bxs)mU{!IJ`3F0-r3iOMN)rSpL-Pef&rwux}AM8!owP(z_DHgWJFO@+}A(2=l z@FU^TqYGkYDXo-CsZL?pPO+GeieT=t@Xx9r4(^Pp% zEk(%*73VU3LG;=N>JA-C3+-!qA{q3^d;f~(mT?AZo4SdMm)l8G0T5M!L;D-xE+0bY z&fE*bUJIt}Uix;2-xKe({b}A9Qj6JDGC{Hc&E3(Gh5L|f5IHopRz&G+wHn_nvTxbw z7nEtIA9fJf4Tu>yV8DQ%Z+oPK>ksSGnZf3UoPl(;idCi)i-{)cO)N*48wSEu4a$hs zlcI&YJqvdqIQ$L2941C~BPDLZy?6az`#i}oX32fWy3;A?$vH|7ViAaKXB?lkBG%v; zUQ|e?^v6{%i&wVsj7dlS<_rGo98ggr=Gvu|400(Dfn2{veY;sPda>f>u$6k01?q7m zT&HsMwtPtpfL@xX7Bu4m{&u^qpAQOQPs1(JS@Pt?w>y*G?wpx_yX(F6*j#($l7|m0 zsp{r1`3bn~V898x`LA76aN2z}{_Z-8&`m|(awz#;X^c)*TAc*<8g=yjf8nY_7mQfM zzUtCLiI=X!0nC&)D}r|lqgGdS#2WDnkZrHR^oQD-Ycw>T=HSyrqfi{j}Eo;jy;jOh69_1uA`$`CVQbv1k$*k{H7xDcT9uqn#lJK z9F^kw&vd|T_2T0H@(X6u7Xoa5f6OTJ!N1WO#Qquh&2!;HkJVg_z|#bRN$A(}MVAC_ z(>l=XCGt@5b8@~5gc72nBu>81p@AFRid08(QS7afD|}YD?-yW-dZ8t%R@&$#x3#qV z&(qzdVQ3TL!sTv{>;kKQ!b-ay?t1WIjC|CuALTI+Rtk@Hd`wr>qc!U~WeDQUQSXh0 z(i2P60YW{5v5);hj1mb`cD$b!ak{!A#*S@9yZH40flNkY#k0GjfH0+*_7xr+5m786 zqMu&$b)GD2xyab-w@m9pLAHuIl_C?lwZfc4Z+#nWB##tzL*@2S>%Ca`czn8xwLo<) zSf_{4?fvOvcQG zDCM)0M+0&u(sbMpT4W#vk?}aW2h_cqo_j@CPaQh53Q=-hGE7LdbaOS@lR z8qx@~kbvd?m9H>CBsv-w zp!4f&mY{JFkeN@F_j(EQeo0=lWul@uMo}~WHB|~9Bsxgn-R91>|5EK{V`$*{xafy! z}U1Qnn;#$!Q_(9ke~~CAC5- zKtjI}Buoy(Ga08=e(TWay=v(yRzdv~e0w}G61{92j=9+{4;g!_58%y@N*}F!(*S9^ zAGBQ&`>H-n z1%|d&dFde2SmNZ?L$>Q|^PJCsDESF$FB4)i$4uSRDlG5}@ndk$i2vgzPOM+Q{stpw zwlqrrbU$v51}<^KD-7MBalH{})2YO$uE~O^ST%)dmKJNePWoHZr zlGB(v*(^B?Xtf`&XMXp8rVQh_n61X(+XB-pr1K-+&yd04Ua@WxG>bHO(3nb!@HP^c zxZip<%G0kuNbmPsIjs~nuIxEVw79<>1#)v6xYSmddD}N_0qpanBMT8CYwo(B_*Gxp z5T;Bcwv>+~UX6!vWHr5fZX@{D zu=k=_8*Jyo`U%2#Ks@q9_%T9Lkt|ElPwWp`$eH<4suCcTig*DqUNQy%n5PKt2z z7+;=wLSCg%3;fXfNlp;dAUfOrLXS)37+v;U(QvlJ8@>5e#W(ROh5zD8rPQd^2cyc; zfAp!sjszCkcI|Uo<$)9{ zgP_ZjaI&g$!s~%vE*7d|r22<4i12#sm{C-&bz`<&V8Xn-^g(0F*LS@erdCTs(@+r< z%(wzQDJ|iEwtDeY@F*nbB2^53es*(0lVfFFZD=PUP#U&0lIBBpCzJBmNz^RXhw5lU zsqeHLeL6K$>{O6a8UYwmLg!?+WJyVGno1$vkSQHcNNii!*d&g(T&7YY~?dpoB)KjH9ct@P70#x{goOe>+49G6bGUA?N^3 zf}Y(q>95EI*$?^a_m^v%Ox~}6Qi|{t^5>l?#TtES!I57drl`42FkxtqGPX`aQvy!C zeB=LnlqquhouhpFcDAQNk^EN72GO5;#UYfhJ7|HBKa41bOyg&{1O{5kx91cYy6#w7x~(UXV7g^VODjqPsr57}oKH@D%A4d|<( z{m&BHpC#7{&irW=AzYO36^4xE9{ek1SmOob<@ca}Q!SrPHuTIkzniyZ_x~KV>fe!!X_Q3+MJ;8WSPIVp|#n`p zIlR0F0AZ-iMwXbQaL?ii98@#OUQs%IuD)i#F}c)`kkqWZU*_v7GCQp_*HkcP3MEF5 zmkUPrVf7xa+?sc7ZI+bfVB3!`?Ae2drO|;A>CE+HoUg-~`f%4f)2wBi%1;IzIM2B< z$+{aWl*|f|ymHdFRZN;X(j`^+GYgi_GQL#cD`MFFAO6C#TKm zn?=o^s!W+4x~=+u;{N(!hpxM}+0$6qcMxSS3g}igRq5%24vYl*r_@vxOHL>K$zUu= z1Z0jfW6Xbojoh5}e7>+X6wdk(bAZ8hBxOWF;;1Jrh(aUi8?AE9f_5Qbg*)lS64N8F z@9mV;Bxo~9r*{Z=iP;*+Wvc^%aM-Z9ZKn?>TqyaJhD6h>6pD#o0f_DuJ4r>&KJjyR zGeywO_uT%OQd{(JStwjwpP}|urL zCCoi?zABQ~pyK1)z~=H**nu|xe33pt5+L}!i(9Lzn8t<8N-X=8R6P1jkPrIf#u9~8Q%!F#FNXUXWYpN# zp$7#zqObZavJ=R@i5)BL;bJ?J#gZ^*%Fj*YKy9I^ARR7GWtJv0Y9pc1 ze*g0oUAw0B!Eikw%HL3-L1}#WxH%VFsVcvvet^d4P5%w5?@CgZ_Jpe|HcDeB{Mm2Y zzaE&O;tv%L^O-ZhftZ(a?@5wL5js`*BFLF93_9xeT;nnb%M;$ZVyARcs@N&+5a2pW zu5U0>`YeKCk2jTNP=vx8VZipPh380=mwMIps}LD*9hRQX-`Hv~;-~&1pSSnmb1x(ZxX^ z`&#>-=42e*-Ymyf_mUDip!buHjR^=pR=7XNBve?_b_@o{@cc2^>*~nxe)_o}Y!4oS5XRaFM=}!s3r$Db`E3 zA*hxD0MJIXB6G)|0!lu>Tuj6AoMSFKQ-0zxxY0Xy7?F8@z%l+eF;mY}Vdj{9e(e$F z4bN1aRsd4HiD%jC)pZoH&*=7QN7Zx;D{=n$oCqnqBKSCaTw%c~G?n%V>|mA%+BhI# zVw~RDk)3{fp?WViI)iC0&5+q_qzBS}>^0F&-wo=D!y4p@4|{x@MC&irs}sG|o7k1f zHK$2aN-nH|TW5+v@VHb=!!s39IM*$5Rz)1m*>%JUx7=uJ@rf3IQZuQ#sj)jH6ccW) z{mZVP42kd1*d5St4+6SmFx}4BzUrT01z^4B$M*TF7A?qGW$0*rQTuaW=|v(WLvZhH zqh6@~vm$#wM&6Hx2ZHD}bzdWj069>&}2QJrZ@bRO{}`$RgQdf@ri$0UA80^2vKnY9*3#wX|e2G+-L~G zEgyGm+tyT49tc5ZIMjhI2nS4LaYpkw{1H^o{!2zFudrL!;TR3^4|>-ZMPDSdg zh-{S$4TS6B_$IlPOFIdC|KTZ=|A$Kw4BuS1DXHE7HlCdSe4-r?WmccMm1q$Zk~zXJT5W66GL z0kWZUawJheew~~^t#}!t{BI5y-$_#a(qOfczG8C4&%hEi)quDqNb5HXSB%;#rcegT8xS zAhDY+#z!)cc$VRlEBo@i8p_|JVc0rJ0l7fN0+@{^Dx$;vj5G=%ia9nbg?8vm0O8s$ z$1TM6N@o@6n*oV`$U)wn;90T)=tbOgavbTSP;mV0-F1Yb!FHr2Uw0m_bMI0Lb}vP z!f?jjDytMTB&l@R)zKw1SM1`+8oU~^4#rja+!{nE}zF^Mo2azxgH(U`>d>d zo-=?JWCiE`9;&ZeE`^q_@S}MlP3z_NewULHD9--@)IhNP%%h7`rzm_DdICS`Bf+H0 zeDo)hEzf|L(!*|Spc#=FPPMfX&Ye`NMEKe+wPSyO3O6YM>1U??_2(=1lcg7QbD&vN z#XZT6*b+Ib8oNk|@$~6OBGl=}sv<**rZI?c3LR5zFl1ZK%#l1#eYPTCC8>w<-=D6e zEq0_dZJ2R1tE>B3`gdlC>A;NYtD;`_)q+r=mL^QDl#a5Zqe>gKhA=Wi-e7KE|^eqeQB;Ear*49#CYC#cypuD1Z0{&;nZu3fvHyz=E1 zo%F^Eu}Dt`D>Jj{a@B~u^U*YN6>KIzVUhi?m9jzF49^p`O2tQ<_F6TsIt}+^DB#U= z(bLHZw6a`Kd4Vyop$UAN`|tJ{bM zq+-Tq*a=BTka6#dsR6LnD}%QbHhRkk8M>HBCe^ZZ#v>#kOQ6$y5m~5yk6R@?x;S7W zSH0^KXet&U0t-n8HS?eHpO-I|#1uS?Gm3)z_y{YN&Wr(i7KUd3sNm2;k5O9cLCt2f zQJ)XlSG7|~9`H}nA}3+NKW=FO5xDUFkQY7>HiQJf^SS}UV48=WI4sJ6t^zEHQ3-zV zG0{7Vvi>GxG*(DW(LH;d8HIA<=cmOf5zxeGZGzyPY~LS0>%+V^$r|!$T-7>~=BJZX z{Gc*bD=~`7fg2VTwdru!M3PZM=q>k)OLvc;heuyD@4Xy9guuxzL06;dE5ipjTe!$R zL(y4|_MU-#=CzsVt$OWS6)U}&2IIwK^7#^|(D>!@pfVX0upbJ< zX_&Q`E+Jhyd!;J8$+!9WpY(@(%h=s z>uvT_eGBN*XNu*DllKZ2jxXGOZ~mV5cdV2MJ%9a-kzw4VQ`IqsX|!eZnmoA<84Zno z&RyE{?Q7DaMGNiKVPOZ{7^04{CR`HxBZg61a-Uj)<6{%+K|Bs0HEL>G=B7KFx-yKS zC^GHE3uDTyy{GEwMtgSX)M+4<&L0Dt+sp`th;ZoNKO`^e#ZkDD^4atAoGkwF7&&qc z4Q4iP*%Aj1Ku@iqU^n}rxmJt5;o`}h+YLRF1B?(a`I>#3XGKa< z1M>$B|K)6ubN={yFPEll2z&QYKG4G>1UWnPH4d(>T7w2zK6Q{qaaOiRAHgN)3>Y)I zUKfvPK+~A}fB(JdnQv~B9*kZ2^>b5Dl?0zqOWlavL!g%CIqB)%Twhc)W7f+{O-SEp zuaUi2vc$H&vffQjkglm|BWAX2-?_7k4zCy=d7s?o5qk~NqgFnAeV0>b&gcDz z!(56WOU!%szFg+trDsng9t5_-0n&-lDX%KON<$^AwYIi)?1>W%I(FH_&c}>EMkpf` z)AaE0kdwr5{f-8PpqOwuD*z~TJK(W6BZSL*7nSjR>_dmprZ zy*8|TE>jGs2}om%s~-@3arN=UL>nYuOsgLZ%BTl-&o-{1?;3wNDgUBX^l6V#qaN>V zgH3s~x8;>;9w3AW)7`^Etzxd@mv!svp)2_LW^CoR8Yn;=UTxLRont?qdR(Y!S?1^* zf2kToF%S*}jZ)N(9c151pZRYf2Z{nJ54>C4H|zeRM?w+Do;)cf0FUiDJmLv9ZfpqR zX(;!0XmOvcH=cE3Y=FtSuD$6YefsQK-(0;=d_{dr1|3 z*|>d|0Vp+B@4e)nd(WOdzVb!Yx9!VUSbcfhwU&j&^prWvzZH3WT(RbHxh|smP!^xW z2NJCMxWeKW1CR#Mx=u}w4Q;0YwD%uA457s!0!}sBThVa9dRENE$3q<*)d*aOh$2WW z;~~jxm3F$P4+VOksp^oMn(Y1X5hGUd1*~}7$&(FvAT*{2%9g4Z?$G0wJ-D2mot+C^ zo(dn!T?9~R0EoB!C=9>y^*7aRm4D1H0I1qB4T8Ryk_Vf&y1VYpV01jbfKf4rVXtKP zY!U;%8@A{Z2H8JPZJ=DgMsk!kZTfq8)7P690C=Yk#01_xa{RdSj2Z1)^jQ)_DC`UB zLJ@u9@AEW2kKVoe=iL-)9(q9shr^ns-M+1jf0jeH+j(ky=_gP`zv090Po3SV zSFaye>m0s)=MGH`be=qUlFDyizkc0vXTQJ^Y{%bftwdTwa2>5W1nCnT~^* zPPWy%?XwIMeB_;P+5PC;3^==g~f@PI>B9tuA&sDJ-D5R6f~cdw_b^G|SLamS#F z&dkh+ii#?E73fQY3!%0mYa=CQtHc{SMgidXTsi(Bew z>NIKc?QY>GP;l;DgWf8 z*fZfp;qUJ+=8l#r1iX{hMrCy9*pUY68e%EQI3n0EO+r(M7$xMdSInRF#y{)p>z|wT zR1az-2yh`YaMahS&g~~`#CH2YyEPR-taRGTmnPf}w?IqF7?=MZ?y2YsXq3^Lp-&8Y zOzX5xCpw!U=R%w+rvwt5-G_|kD)#g6_||Enq9b;dzCO@XQPI(>o;(>H-SKumyjpQ> zb+sR5A0x;>USp7p{&i-ihS{akYvyQ#vYC-lfUNBtiv&x*sfO&9`&8dI#X(I^zbxchoako&40}tHGn%Rc)yJ6*OR{$LrrK59GLY?{Tafn zPza9RTU>^o{H^%K#Wf6OGHcsbQ+<_#^5Xz!XBtrEn?6?K(5?v$)#x2MiIFG2eO#JO z3|sQyY1ckWN)$w3C;YvGe=HI&a`)`K#ft~RK;J((zW2E~FPm|L!48wN^d5|3z)vpA z6&4;Yb8c^}yPAxf)FS4t`g7l%9bM~AT=A_5QDr@7D}(Pbe!Gu3e0fdtMT_ZE0r~KS zMzz|6mqYd{mC9VQ8uUWzMC#Hxxe>z#p0L3atEnz>6-nf5Kcg|DwRgQb0UAQdb&a&2 z%ue^bvSftvYmxFYc!W*{7Ena)o&Z?M<~U7tA3(i&^f&| zQTQ4jc|U~3Pai!XFU6bdCVy^@#UHZI&V-;PnC`vUkJy6;>r9^fCi2w)5054sVly30 zrp9k%NAf`Ljj-AoUk>W>e@kijIT|k7mj(LSMag$PST0fpx2rW=G_N*1fP%TQri_31 zmOQ>zty%~$$F!~V#Kprhux`4HDbV>dep}#$W#2@h)J&0ETpWq|pjqqIt&P2u24E;# zDoXaO?B2KU)mP*!S1uU`YHvb7Lp)VD9G)kqtEx6J-g~=eLDsQ33UH6rrWvO6%`e|sy?u)A!mj?c<=zs7~=kB znvah<_AEK*AeOKJ6mOf-MGNR$#CVyz?OMkP2Ka^RqGDoJvthZM7>jZ5MjJAwbrl?1 zcJ+ku<4uUW9OT3^9Ne&Z^Zl^T9|D#g!%=%)U6%Oj!m&e#)H`LZRiB<;=mKD~qZI#E9_+V{))l(F`i*o7PfS*U`~o3|ZzxM%ruu4FQuQO)56t zn{wBqB7p0hz7^O$rKK|kIEc}zN9-Q%Mysmk=KCAc-fL# z-*hSzhL0V)PJ*_rTjLKLs7*vf+*bYP)iHc0eFh%Ade6aIi}M+heBm*Wi+2IMUuRH( z^2@~;K0X$-Q1+~>M8zmE@ViGRngh%#)PA$r0vzSt^5Ia(4Q(;H1d<_J<>=2*U1);6b&@o|!elbS_YpN?qu1%g(-TZ$|h9@I*F(kD(`_{><02or42#Pn~SO*KX~9Rb|a|9s*f$K zK1ajF#pPdy_Ul^57o|`C z%~HRnjT`?wI4=RRhnSWbx^;m&Usl$u>d!p6;dlKQOg%=Ku$R{FdAyOPmR9tW$|P(Y zyh6#BFX`-xH9RzgjiXiNOrWzRK1k>TR#{+==q9dx_?v<*H~Y6RaQ*pyc`gc)61pFI z!4>F@Sn%EV>X@Gkk<-pwxUfIdrY2p=eSaw5Gvr6s#d=C@_EP6&C1}F`dka^+b_CVp z3byAbnaW5L;D~lmjol$Qlg7zEzYN|xGdYe%iERrlaHc2AEy5u-k|38Q`d|wO&-#OT zGbNutr}1|?d#BeHEG)>c_W&ZiR-Z0b@;wKwvJk$7A9u+|ZZFMBIgPZ}eX>sZC_CE> zs)sok#3lU^bP?6h$SHV#D>JhWsHPw8M%Hos`0>+b%y42I^XoaSTu)q!Jg64R2Yra4 zb{>_?Zq}d{K#kYGhC**G3|-ojC)-W4mEB+u!y+TII2-G8bG?rA2_-Q{yqq@G0t4rP z1APf!tUGu1Hgi^fmRWnz0ctnlpak%NvXMWvE-iz}k3Ds&5#h?{apOeF!g4u+Hd@#P zgrOG9#sB`)GsWpo<=8plF{IvnneJ%$ChzIYS$8i^u^hUjwS~pgRyvxheP(aJjn#I2 z!e3pBp28a*igR=6hQVa$rte?jIdi?lW~z9K&+B!sdJq+XI4dyLv&lMT?CkB_)ABzk`aQ z3O(k2q#QjLJ1(9EO4N@r*Kx*?=NPBx$S`z;*UyU2B+In%NCf0}n<}E$Pw2{_s!zyI zhH7oL={;~NY0+g#7cV}1RWm31ux{_M_2!*azj+xlt#+L{T31aNd%k_gj?VL93?Dvx zDA5q-zU}7pC-DZ28wbNkGN-gTVf9ROruEB5FI24RgeRw#;W4iQ-H&%$Z za%-{q ztb>ETMj9aFk`86EU6@T0LB>T2KW$OtrC5_;E!{z~ySFhKVn-c5>}m*oN3%a&hNOba zc@j>n^?AaTC99}0X@T6dj)hzQ59bNo}tq-d{cIi?xs;Dwxc)Wdm zTk6-*%W2H9=kc!Hxzk|ONl(YglUwy3@8ow<{Xt+A@wvsAVcr~b?OjUFn+5ksa$h%` zS7r1VPgKCOg)+5b=svqUg|T{;&|VZOwIu5mOoQUV9{d(c=O*X^UsrXHj}-ojgyXfx zkDEdR^#@#v`-^Mrvbhc{vN$VlQXXT^{bvSI(%1}if)unJRB=YbL-&@=4Tt+7#Y0@( zZW9|e)J#VX<-7$8f?*0UY4ezm2ASyRAZNo>~OY@o|=K{f71g$ zA^>L3I=SH^taU76TySK1Mcb}oi~YX7{f#=$M#5I*aOzT`rba zr`1&N=&&&5@35LAr_^a-jJ|eoYl924;gEtSN}@fHcO+~I4|{i(e@V;9F_l^M|snzmCgx%o8e!Ms6uyrMuuFkQjteI%|NJcKP)3n?YBOwvmw(QcS~X;F;nm267%TSBn`5Va~w#v&EG}HkxQ?7 zLMHz_Z>_S|H}xg6KxJtVlRhLr&^2o-0U3iB7kwEx$i*dS*RF9N7ypEqG^!W9IVEK| z28Q9e1kc<4@!ha)-D=vYQ;iVBADsQ#O0%%NP8IX;4h6ru4#D6xYu4@4t{))g^9l-T z0k>;wXxu7IfDiBi6@wcxxuti%xhY*BU}$IBp#G7S-&X!d3sAp4MWjRD-Mi|X_pRzY zo-+@+`opC7l%50ja4>sp-e4 z)w+rmf6Z)#eF8o)j#i3b-*k^)E8xf74PSd5`vWX5?t0O$x_yB6;^Xi9y%kl^6dhYz<*%;qb4 z9XWhBl^Uh{H2p@R>tMAJ%wa^K+A8*qClNM1DoUrJh8B6@jVNbJiyqcTIIu0e&i@1CcU$4f2u}fL&>(4(#``uAN)cbnxaat4}z8`Gf2Pyya@h zN~YqyE_%|)*fxk5QH0?r)AGD@THf_A`*8?*!<*hVPIG0dg!Zls(q`I3j{Py23CJ8S z_A28YpgvlZl$9a==v8-(AsMq=C^WIr5NFz}9L)TMejLI4t(M)pZ#5YjTw_sAeX4c8x;UEHte zlj@!RLx(oR3}6X^F`xx7p^(rzSFT?5o;tNnbXI#n8`XtzbiXrE*(eQ$%p7?!_tj%_ z3k%ngu;);JU=VK>8W_CgpMnh=H-wQ7K6%|tlGGb802g~#Lm($z zx+Gdv&)oymCAR=ULcxk9&Ce{qMzn9ERL0_CY#|ovchPk{jJai=V*SJh$(_!F8mFb*z)hJvBQT435$d`g;kqU81ebXR}31GEj3F^ z%hMt_b}C-L%sFZPLAQ*S5gV}V7#v?D_dR?N8rZ<Px=^>wi5k)4~MvU6*hLEiEK+&Pz<3|4V(%%VSG#fU& zPC{xG08Zme3vw43cmq7{8mMb>jl1GhHYP~e09x62>ja)9?>j3e*+fG_W4{0GK_3?V z*TO8`1t-XHkQo~gLbxiu%Zvckoc$b}IsaSi`miuf1xyK27YCy2YYab@1Q&@cWP&BG z&q zXhk1@#jv#vFH9;4ul<+>6GRLF`M)Jj)2OgLNc?KfFOLQ8H0&1Mf8jzKFk*#vbp!Tv z-l9d<$wPMztPYSwU1Re<@7^82$8+!Ae0o9)NxIR5Qf*RlvG>glTp%B~=FyiIb-Y$l z8|H%dZh-wWyj~;uZGv*=Y17(bgN!uQ6xBb!4#ScwD5lpg&{wu@&4kBVWRxY90=zb= zhmVNf4g?jsiFW>6M76nZ-?m;n)6>8I-2a}8Mv^P$uNu|X4Id9n6R=`M$eE8s!jg;9 z-T~*l7oiTU$h?owY@m4ZIloNi))M}I@ZiBP2U>6HkEQ{U1E%=_CF61n$eT#K!w%=Z zey!Af+?U9!P=O(__+bjb!K@H0FQWZH_;|1)n_j1R^X3=FEhzboFKu!&eLg6B4~I-_ zxVA~J%@JK=o{%{%AY>qYxXIJNCE(8Ae@k*BFUTy$nuTe+tU172d#6jftOS!i>NYeM zUbB|DUJTO{rO6gfcGL6pZzv{HDc*(UNK-RchjdLx{Ngod&Rz9z_o;+?r_Y}!X;GU2 zM@E{_wQrL6q0C|~-?$c;niqokCEvfax~8KCA=&A!M{M~`DvEx*mttC4OL;v}&PR|X zz<_;M$ zWVG35eP&*Ii9G^?Lp#iyKi|p4MU&LKVVLo;lP9m!A89qtIKd%*s*YyufQ=4ZH|5)} z&roto8eF>|5PwU$sL|rZi!XMd55yp2b-9wNpD!)Db!!Y_Bg()^9pa#t@4iR*3$`t@u4!Cf`aYax2IxF5S(WK);2d0mA;2x`Fr@>TLV7$|Wp`)eWY%ogSr%G0Br;&C8%sz`;;NSU(KeELSn;D9fWY!X zrfv?EsjI+l5Xs|a?9(5731I>=M_bS>%LLqFr1`KuEDuxL!B0j+BoHe;Wte!{@YEpy z@G&eV)ixm`E2{wDB-)W$M+WXbRx%3LQar!>)A+ax$Y8s*ZRfXRoZg9TohJG)W>Bevwg9D z{GmY;hWd=ExMgi8XO%u*_NVbxV(V7j*g@+6dFrQUCW}x8EbYr@N zsP|NamDWp2yc(ME2r@VuN3WGL36Z z>zH{Q;#2@*0<^7g;5za5=vA@`?#kTEEXezcdHeSDL>`z|t5RFe-kQpk^blZZR&EYXQ0^5Gy!M(& z*WVXPoo!xxOMiEHk zXTTO-g}TwB;Yl7O92M*hq?0}mr1yk3(JyH@DrK^geo+}JsRsuK zH`Vw}S%)BU1KA}ZOT2YfgjQ56VR%|Mvbm37YJ}u0y+yduL}eU!E2p%umB9`x?Uwbq_QITefOGS@EKsuw&njn zK0}Kl=pYO@JEDt2za}=DRlsVHa321laxo@1pr)!Ex8XfiH`-+~Ngqqgf+tioJb_~F+_|BjTM*u1L7ERAGv=o3bvA*$_uRSn zI(?6Ma`4b0=}`kojTuHc7s=?t*mLJLbJr=)ZzoiIc6W%L`v+Y0Lw{4Y63&HQaD?2} z+VR8O$BYR_roA5nVY#_lO|SxT$I)iFT{)JTB`R0H5=>IQ<$aKU6T61o?_E^?Tb54U0--HTk*w za4q6B>Y%)#@wE&3O8N+GHM#b#+A(80O}LVZ@v>(4^WDm$Yq)J>VHyf)VYsj{E}l!- zC!iwA`^IS^K4GT`s3sfAeNlcZE6(Y(*=FRy4L~?F3jzn)wmsd)EY$*@^WPt39@S96 z9J%yA|EwSR70$W`9JL+=5iqTTdM{b>ri-4w9mz|R$74Qy5XWvy`;|j@bky0>=0|LX&s%k^lNK`^w)emzIm}j zft|WmZjZKgY>bu<16=9CPoGFz2`2g>T8fu~{_7*_wyG&fN$m#>dxJ$a|{PV=1ERvf>IV zeTjv!Y3j$PLLiJ_6gK5rc>o>?SurOfj7}YwDCoB zUiGb`pOAme1<8qch?g_G(MI0je#AN52hWB6b$Xc$l>)hJ*e$y8&fbfyPENRb%Ph?9 zF9ZvxNo;6*4i}yU5l`o^VU$Wq_YZHlQk?e`ikp)bzBgd=&!BwLHnxp&%-#<#+>$mQ zjsc3o_SuM&briUpdJ8LmIk0AHTE`6@G9)-6qJDklqy3n_TuP9g!MMOHasu#U`#EXi zW6kz0xmpc{sp06>65g3Rl?91qJN3Xu9I0pm$N+8i&fbK6MW!a{-FntfX~&V0g>!Op zGNkX3SXH!ulzmS8?|IEkPc&%ZR#ER>zphQ7OZe+BcyN85MnaA` zbF83}5gW}z`xS+zOr_|RjHeZ+7JgVr8hA5W*3+l0w*KS2UOqgbE z_3#(=jOjHrG>pY;%GUw;MYxReRTOndIv~ULI=?!#!`fRN0pL1hiT<%yui@EF1MW0F z@A!W6`QkOMKVBrSXfFB$pc)pLXh76;KBw&2g$HC;pfA)>Av*$BV4Q0~?OG5UQSc%` zVEq35t>)wu|II3#w@Y2vSM7gxoBl^kFZJ<5sE~B3c65CW!!}e$P#EP zq75K44(4f;7f=zKdsH4fa^&XFmVAVI4HM;s1zJ#)9qx@)K23id5j^N!w)KUP6$uE& zc|QtJ(F>s?p1Chma>sb=g9i^HdlvxhN00+KblTtbQ^oi1dDM-CLNjwDqH$jmRUMu! zr3uqwmICZW6^DV4`i4LL2qjG+We##i#*8ugB~^lbQ0F@&36JOBRN>40mWI-z(`wbq zYL}C9I%3}1Oxy&g{Q2!gW}`h9LPA4@jGS!f_6LQ)!rai}U@+yCIs$0VoSFY|sS|iW zB++aUqxd*C7Jd7UZ*O47guej}4vOE8k5=PKEb2a@FN#GOyxuB4HI}IC#mkqJzBH74 z|IC^DIe>(K-*fLVp40+~6^$F{1)i}lusWPtQS&~1JU?VnU>ZttA(jZmc%FN)?I_3O zgS7g(+5mW?@tf0@O`F!>Ni8~eCTRAVJUK-4wFFCCuw#9ja;_`+bPo^nPMsR@=|uce z7;tx=eWVB$s**g%kC&XDltZMBxLRLZJD952h@u!_!!v-=*0e3a@d!~1KJc(8n+%Wy zZia@3YhkMhWkQgitZ+|z1U^KQ#X$M`$Xb$?ms?4i1o)TGLeyLWN+2s&W?!u9dXhgS z-0lCNGA}=WwdDS&9WApP$6bo!D0p~0TwLvm-@}j!!$lzf?CW!ml2ljpnDZiP-Nm|c z8{F-!CMK_k8vOI)=iWVgn%mkM(g-!zYe}QKWm1#!>*vU0bZtM0Wi_?6Ga$PAKU)M< z!kE=NL%9K>{+cxD!0=c0(Cf-Alpx@2^Kx^8I7*#$6k;nO)F8(FaLREP7FYdPD6GX2 zK09WdgqLSek?iU-7wN)9-#K$O;)12f0|zN592_&V>4!Ua?`{nXvCNT(fl9{?a$7el zEn*zWPRos>;792bNV+38KVKbM49ny|ezjS%W($89x3{u7Io`Hc%a*konc8LGpYg<* z8xJ*_@%SV!`%Z!8LJkr&_7uV&?|A!PvlsrN999E0##v4M=rBqG>RruMk7y8OX>DVp z%F6x{3qF3;^1=Q4LDWg?X_^V`BmJU27JhwSr@=(xbhe2Mv5UpJc*%~7?w9EXwuWH` za;uJVdL(Tnl@WwMQZYbeLV>l|wmC17)JTo^RklDvfK8JY3?FW!+`Mr^lKZHs(yW!G z&92bp0R`2X=;#p$(y>r zip{1uoSsM+t*sA#^_Maj(eJQr&}bnb<~|Jg!CAJD z%-8ADLCMK>eDBPCm$y*{#4V*#By8Qfn6S!tRSsnDAnyi5_4SP%${gJrP>D|goJf%Lc(YJ z>N{vaR2uAn3LS1`Dp7#)i*bm!AG={tXCm3V*tZ&>Ddo2JDf0HJP^@ge_Fo4DIqXCU zW(h6znv1FlxvcY}PlQGi+6nyJ#`98;LJx?1a(v9M(Ji z-o0VU{(9vfp4RHo<3g`aW&(tjAEmB_R0lby@SIUhg;>PEg`1Y#EGi|{WzV{I7ghrE zWp~tJ9waglSPmP3y$H%-;nGQ)WiA_{rluwWDd6Xmv{F~@t>hgZ{Ud=TnY{7Em6PVo zseTmFQMn3iShqpAC(vFh-1e>f;d;EX^VGY>0d)Kj67O7M;``Z`Pepu`(k2Cs`yC{` z4;CODmPyd)2a;wf_F&#{*F~;U2%F?W>~1GK%Al07tX->Svu9 z$1@bu%2qN7Nu5&L@y9Dk*tr+_uY$PIoUFT?I|6QUd1zR*eWsS)DTw6y3oG&RyL%6G z%Od0wB_EFlYwR^`nkGuVILl>6ATO}zalq)r!WDDtVKYItpQ%t0vXE%SXWZ%KY?rif z+Ed?2Z$#C;<_(*7*CsjTD3w#kj@2j86S-`IxEvfBc?evNBo|4*h60^&IdU+tOxVeN zdw$xZ!bnF{L9~6(S{Z}^7HtZB``~_KrCtdx;ns^`++=7-V>x`$(GTpa?J)LK8`Hx~ z4J7rM3W$i=X$8n75D1L@P#%uB&%JQwcfwh7x|+Qzm$-AG)jJC-&ky6Ta4!%Db=n#~ z7b})Y9rb-$$;1U3khBz0st8*a#yjTxM+?9Sz@n{!?1QZsav;mt#H0bLDcP=jmonD- zH*whtgQ&Hzk}?6`8%lRpiISqI8_yHZoLLP5|0N<;W?P&+bxOKsS<}m1$}5oP=zaJ9 zi@;RQg6Q!vf_3*9?x19abc}(4f#{<-s8XzjQ;h7j_8t{%#A*l+Yv$We+)d$%E)rrW z#dTH9&YpZ+M0;_ki*2TG_)*FXmYq*%+dyJ%Jn*{Ly`P`4A}0n5sXO>NsB|^VCC?*l zMqJ{4lD)(fq98};OUnB?T$Ut2m91N^E8TvTT(7sc8iYYZyLMz)*QBO8#NGa9r71x; zeZ3TvJvDvW_t4|aw95V-SO6|Fwgo8!(W$@i5`T+58 zTCkRWMOAR*)ss_dbA5mPD21~Www16_dj3xOSPk<}caK)RgTjIb81~nLAFOy&8>?W8 z5HEu^4fm5*EQj7f!z`X2C?4|cF2pb?uC&ZO7#){6Hq-7aWIW9X<%5ijU=s+1E-EzR z=O>jFVme{Jdd+6OycP}|izN~Q=rd`)U%Pf~$v^WsL_D#hH*zJep>p(|`ga?`voxsi zqSCX-Whs%Gezo{-yi4bmKbI_O_@HKY@5K!v`t%1s=U^d;)}1wLmh;NFH4An3a3|8C zeW)W8hKn$wAxZ^QsPpIDuq9RzDdYG{3*;mBq`W?H`0(hsSl+&W|IBCU&e&N}J}RP}0D_C?P2$C8Tw6r!P14fR zBs3qT-(Kou3 zzmZD8`ETmM3})^OKXz>9o~G?5BH6-4PAslOS$4Nr|3RDZb?ef}ra3?R1IJ7h7kVoB zN4@s#+gsRA!MQ^W=!hM_ZOU`HIG!k&BEGc8>(3R^q?7+nl9FhPIZEli(lw|Fj%IjN`1Bk_kZ%L5x%(9}VSvJ;F8I4&Z=NwE7mjk?Y~`!4T8 zi3uz_T)qNB?M(LfyM;~y8ETn^A#A$gz9Zmi$dM+?{63}Z!jMTgPRcYqprHc|iM(da zsK;HFbH+81%q(Tp{m(bTGb9Dp*s&KqH4ol^^fEt_Sj7FHPtRCd>NW-V`q(dm%)n0@ z0C=RgcsN&~8i;kU2y3g;rYEuOT_G(7c!dmr}D-Uq|2Ph|wm3;Lz zu(!p<)*TBEkx-a473w3~-_I`%y^@I0vD)dB>S3NveT`pJ{5K`sB65^nk!%ZY;7h^y z#2HPXoS%HgASgR_Y=F3-@zTqrTx9J<%qte?HB!X^;2&f(8Ca}&6lqe^qA40nHlye8 zNnt2tsc>0Gpkmx}er?iwj{L_WPqWV}NU4*tT1)UqlxPB4wU~I@Ua71N7W2x|4^O3` zV92E^wztcq7+dl|Sp?-G0lb1}8LKN<_75LE*d2Sfy)7lTwHr2UC@Bd^;!}wbajptK zsWo|TN8PK>kqdmLo0O_jfPZJmgT0}V(ZkB+TL^=x&XMRGOs`1`;JQRQne%URIx_jn zr{5LzmFv>56(SCoJKv*6Q+!ee>29x?GwX|Guo{ivV-;tzfGS@H(pQoM_^XT%glyGq zN9R%_2y5+O!-hpC9y)d`0!6-3F3a61<2U1@nUUeS3zzfSEz`kyrrbv=6 zQHmYcbOurh@B7NeR4Is`n!by#l$y{1>kZAfs)x^YL;OLx1A6ul@F&Uo36o}@BCjO; zzgc57FpsfafFbE}q!?=t9cn|WC3wEo3tSv&vUHT6d@xi5j<=jkFBj~?k{C^zUU;p_ zSf+;*m9Qx~u?Ho<_;hp9U-%QBiMoLnuF7ggWV*d_^ZHI_K8&$CwSwj%i1Zt)ArYkGdaLtmzgL$~V$_Xc@Y_+$~($#W~KW%)D9)VQulvNqM zC_?!+aK*K`(%{C?#U<>g5l2G*Ncn6kKsyU(qe-0KAB~gLx`L22zC;6k zNy)Y>T_j{`cdpn?o9P$V{zZ0tkpb7JBQ(d0tIL~$1*L6L9J#^2B!!hn=tR|Cc!1w4 zEPO)+H3B2Bcf=iLJ$sIsG#f#Ka7|R$83WH^k@iGCg;b2kMoB{jQQpAUiOxjGAzQZ@ z=<0;|zPqkEIL|(1jxUOjIb4B&=gSLMy zaBv*%aNNrw+%Ut*-4bZqV#IM}=N`>$n$?fh`=GHrt%XgqZVbkp@o17v#1()gf(s+C~x>wYn zZv=RViNH7U#-xKa5azE&esO*8Hn>)TP0GjDa~QU)-@(6-z43R`)iOZ9Ln~GT96kG@ zNs#C9>>R@v8nN+N5VHtH8_>kr@r@DQL<3>KZfzyL$%HOWL4+gH_JB^E;FDmW7hy%W zvMWL`lC{p~wi3V{8gzX>`9_OLnnU!8bCB93W}d_Uz7dt}WI> zwiUJ8s$19h*!o!jEPEQ&`A#=M(K1u4D_U8+yT?D}pJ)CQgi>#SAIBv}oUc z&^trXb`XS0(6?diarYnyt-T&Q6W%|Gdm;_d1CA4*2#}psQw)>Yz2b9c?e`F z2|e9eP#4;gEy-TOvK|-;Kz?B<^UBkz-Kg#(h&&JYART5UPX!b zPz#$jn{GOyiDjNL4M`-%fG^qelFt+(giEpa{rge+8(eNmGMn2O z3)K%{GBUz<2AmZ)GXzxT!?(H&9z61964P-UA^AD$Jv+M(vDzl9D!Bzp{zUXYYu0*l zfUy9zl$lSyQnAp>6^p7$NxQ&*Wax0J7E7$5ykHL|MuC=|xH*LW&!<@5tt9uo%>Lu4O>;SmQ`ft1KFPh9W9! zPCmZ<(g|m%?_me_vwpjEl&(+}vPdY@@jxrfkN^GTNyUYliG&tJpVG|+x|9M>3swk6 zLf5%|tPo!a8QY<>@7EOgrgbFEw{**tNE)bOI|pE#xO4qkWnDwVFdT-IQhEAP%;J1ticqwikU?() zCU-GH>m-F3{YH)2rk`u`d6%o)Jk`%%V{K++JCOBfEJR%j5$l`0=cp#FsO-+nh4kET zmw%isHEp_f^*zm}Hap1@$RM(iIePlsc!Y^({%MFSa^1>M>u09E+@r*0Ehk%8{AC4O zE-6+zjCoP9xG3w;sZ)_C>&-iN)+Kz@e>JU*l~qIPMVz^Rl(~gmU6|Z%IL{I4mxL zW=?5G!V$ziEOP#9Gd*R?mi?wxVfE)!4{gs>iM7Oa@j`XJNJeBW@rEey;xb;5^pHu^pxfm9z^RUd23CD+Z3}!YDVe5 zf#5~g*jQU0rs(0|ccYOdXMr&*#%n+5K&{TTiq8chku5jflzg@ndHSAt0HZ{@dmba3 z2z!Pv@sH=(8}p%}5^D3ddG6SdoPx1+rt4O{bmc1zyR;imvJGY1@t6d$k_0kNSelk+ z;4~far@EB6LIjYm(KKOd>SSo4rZn@u5&k{2{3?O$0IId2;`VR-T7*ivKM^jbb1f;+ zxB?i(JbWIJOy4$jr}w30fi!O-nCMS>dIY05i7neNTu(IuOn@0+lP#T@e?_uO%#-4km8EcJdp%!uDT*__dTOC6@r+>rXzh-#JWm}V^M8h&!3#ouYLmYiMTnz9xQ)BH?p7g~ku>Wy zXkdFoCCI@2ZghS0*0p(EFWlM<_6~36XVD?oT0O0#f9!Qq3x+JdFf^v{=T{Ubp4Q3h zP%@k^2{|MB>)USF%!IEnA1O&mik5q02HQQ7zx2)lCFx|E zK4OWm7sLcn$nPXlzdncVW1{5mUkCFOLFfT2-5?#V`1w@}R>>9)NfaYHv)e}9x~U2^ z2ee=C7SiO%xS=mMvRq^!-AKdsZ|OOnH))mI6$!k=Lx1Cbb*mnY?+WyPu(i(lYanJ+ zwd=QR+441+8_(eAO7^eTsrE09ElgayM0lfS#g7UKdQMaS_G{9Tt;V~pgrUXEZ;sWv zOB$yA`@7M`Ayhf=H5t0M_#BPCnSTguY{OulS#pmqO+Fa44Vg^CU|eUeAi-e(7t zBpNpr1f{NQNC~&$A2H7>Vpt@V-PbQ+d6%U&h5c@9F<#yHzZ7VR`Eb{mFpT?YLXj|B zIL~_8hwzpk>uEi&&=75z#dWcD;44MCG0X7#dye0~dnfz-+*fY`PCmy!7yUq>85**w zIGr=pxwq@p4^%zW5q?`v@C=y`EskocHv0(ch(t4&b+x3dOu_*^z3l=lTT>%;l#GJ9D!Te6|(c)4dT?@XwyT2y&WXhW6PP{uj*$2JtE z!KQe>%>4K`jkn3zz|7lwkY0n?+c{Y;;XliK3-kQ7>>g||d&AG;aCKej>v2TTvUXhl z@!=X+xsh+1!u#Z0#;v~iJLBNNgX^dV;)@nn0(c1Lz5NKI@eU4wv?~o_KY{na+T~nEa zWPO-9eL6#i?L(WTM-kj!5dyN1-bCg`^be+5fTJVpz1q*=75-WMJ%9;UfCLPHc;Oty z$HzZd@U)muMX;V#I^(H!8HL3|m0X2*)4=KNH7jbT4YE4&Uc+|7IK5w z!(@gL{+j`;+IDWW6Sr*H>xu5|ObOFNFRuZx zGuF_s%q~sCOz|ZPLd__4{5G@tCM#%|dGs0(^&kq(hV3xlQU=uYBlRllxdMHNuLmWT zY%p)_Gl;oH%kw2;VM6OQ4i)Mu-cpp`oQhU`#u~NmIkh(FK|7%ebf8tQ+)-gSUYmG4 zS0M0VvY|rj7CiVc$3ZkUS$k_KntJC%-HI5@G;D6zuNEeJF?S{buSF)wvO3V6Q8O9dQH?q1IBi4E~L~uJwaau z7G*Ju@0zWK0Js!5?eBoxW92sIvMCcg*@j_uH4yBkNK5?yL}sRwX4R+mV68AK%Dt-p zKnxOG%Ya?~4X`Rz7Z{$o>P6((pgE39=YulxteOwHEuuZCJz{niVUQ|=3MqsAV zUw~D3Xj7P8HS7^MsAC8De`|hXXXPK=_qC_UYWmFHMixbX1PA^3>FOSU&EWgs(&D88 zJxrhooV#fD{qZgX33p}o0T~u_Tx=mJ(vH~0x183im4=P}MCzbn9l}zD*wwtbyiABK zEHq>{Yftf_1&cxu_pE9nZP*mNoG^@|S%J%3oAwg5%lboyUN6WvM)YshEp!Ji4eE|A zrK4(e5g%?}xiVtjmU)@JN{%N&Q7+CtMGxS{u>&p0zy7@#U=Mj8)@~i5tH_4ps8lss zPy22MxrZcPY8!eU3^@Z`w>HI?MhHZdemRnx)>gX}w14 zYIm~j+}x~~?dTKd7-#Fdg7Tj6%!mp*=MCZ`k@Ogw5%usOL}nq?L0s^nXCy{Ke~$g4)t_> z_!^^i?+RLenVz_lPm_~$kdbAiF4G;0F4F*PPe}}PC`rXn#bv$JG&(In`;p*2D8vV)BC`BSJjY~#i~hnHK2quI(Nb{}Vtbu2G995lV+8$@~FM`?6u^Q#vx zl=#}kjvhuPYOg;ZQW?;=g@-9G5GYMLO8l`iW~%@_9kp6P)365AO7ru=AYZDl1$ zM1w~w*vVE0eh2XIYd`*|0uB_5VhlK;SdM}>tl@-8ea_yb+L#O^4*Zw<2GTylVXL`9 zCU<*v)b)42yOm9caWf?7zAqlrNxDH)UKF*@bNc5-hz$T-X`K@*7sII(eGNozSGSMs zy0ccV9swFXsIqii;?tXZwvB;x$2l=G-ZUD@GC9#E!+W)gb*Y}Pu^H>Smc3qN3aSq? zp;6-Pv7llQEM<1m!zs1HG?6gNyq7t1Sj1_7!#An+Lw~<6Dyo-gu=7T%Ui!Bvg#8~t zic7Qi8C>!5Z3(?2D7A}4B{nl1I&@Hgj)(pU-`9I)Ue-G_e>!|GIxAUqn940HSCTj9IgIi{jIi-GJ^AeqRlYRo7w_H;)0kA} zp$?tg|NZE^-naaznxjjkO$YiXwkfzh9RG>FBmhGXC$KX_KuwSDz7}C=Wo15f>dP-x zyTLHov=v0(9|g@2$K$PkUIL*iYSi89TA}af;fPkS{WR&t!ng?&{9) z3WBksZ%B9KcYaWYy{_$Y%%{1nEg$Ig?9|C$OQ4aW6t?kp->XA?5uyP_EY6yWOnO>I z6r)~x4Ud>PV}@?ue{{6(Hbpk5_QUEQ1%XM8y{JoDL@(iAf-)ywH;f(P%icbM@)=py zyH_t5m03Z zoH)^yASvl7q(bzDu;5VkbuIXHJ*C0*>fhH=8Wa|z1UD;Qd~b^F4v1&M>UXFF280k| ztv(TH*HkPavi9h7p?)UTYO)J!+S_<5UGe%5skEinm#1{5M0hG}tumIkiF8he*fzz?gIsfL( zO>M^C9R-SUffrJO7LZZX2cOnA><=@W2hD$Xy7~!lpRfUiw+7y?lQOh%XpGvgFliW( z$D5e3WU!uL|$ZDCC% zF_mom^)GWON9$_T6{&~=S2Y$#uX0Us}QYu2$CGsXLEIK;+o2{WY zIS4^7{9N8gT2QkOBi|R)ljCK#g;LlxM>BpawGEW*M$FHgr?q(Xd8wStnjal~-tl@k z>j>Zcf=h69-X4+9Lx68_xb%$Jl##?!oKMjIQXIbG&Oxv@=Pl2FdM=3e`Xz!ODdww| zCGY%;1jGJT@QzJPOfoSWBGFTuQ`F`E41Ue%#shT%%m7yk4mex(zug8=h%iP`Rj@`* zhn*^$83|2ta2v4nD+`+4m@m069smM0;ZcGx0U+%*jFnzMT*uxor7$syitT3i<;w@* z&M@qN>EGPbAj%5hhh{=q4CaIQkMK6G0CndxSh@jvGn(CBG^eh9`Pwr&YkQi^>1c=Lc|EZ(W>kvCc#{}pp>h9SD0 zw?*2v$6=R7mZhv*tsScC{^U$b$Qjxg)t&F^P-cV;Tn-6jQ~SZrv$kG>b9aDpDlB5A z$VQ;ZgZkHlv{m&yw+$-`TH2QLT=|0rgSM=v9f+?+u{P>+K{P`VHUxEaE!UDEA3+a@ zOgq*o&3?_C7Vq+aNJhtcb5oZYpI;8_pmZuaZeOs~k&TGl%O!98AIZ$Tq6 z!glv&E-_&KIy~`;-i+ff_0M~-t^AW^KT3HSXt7jk%a`%h7o>FlG*fyiC1n+h9^-&} zM9V^t+QQa{nm7E0`;hTl&GnDgk!-4+R|s2)f9T|gaOr|%O2Yj-mzn1qfBpXwDcGnj zAevB!mTVHxsxkoEyM=hgaBJ!_i-1i~2gpEpiTF^+EMGiK=xVALx}GwN%*0$yl=)si z3TBFr2^a|5rWyhrH=zuXDU}Q_XljD5$DTXaR#?dyRb>VyC>?O+?buqUDwlw82LM;b z06YT@c2N^0wb{YwWSGcI`S>t-7LI>cHn)pUHkhlUt1EGZp6rT994QXb-;`)Ki>;q) z&~ouB`O;ziIQJ?2bwOD{jlRFcCjWZDqer6_-5aVvK_8n`C@h;Isr8uhx3qr*IzmfM zxxR!;xbtI-g?8WiTM)oNsn*jBRS$Z2qp!F1H5%UD`2!G*r6{0nWQ0o)3?QqklgIUjbD3W4Nz?2Q9B61Xo z57Tsh;S!V7Dp#Vz?qrsL0Nl@Kw5q?Z&m&i?mWeKX2Uf4h+2im~Ss*aMv>4ev=wnNwhh> zhgtjIGjI^kC#`1sq3?8q*g!Synz{QW5DLhgBagLV_vDBx=A~^28!y@{3!2|A>bBJI zut!{TZdsn;6LgHu{spkvsz+`F$e3?ZA#r6}X}?lR-3IaLGj6p;x#dx7RPFpOcZ#Ll zq^QKMLS zqckkW{Xh2M#52`bV+MHt(S)_GzRX4c!}3kKs4+5yH=IW+{kHbe8JteD4?DJR?}_*T zNhG*kE%f<^9)xc}s0rwhi$B4;_wP$;P3ZXFh*WlRN=ggI!MI*%Wke$kkg+0{|H!z* zjqz^?<}n%W$|#}mcM}E#(|vl2lph0dE_9Pc8xUOIM9t8?o0>6?ffhaUKp}mSb7q(Q zI_i|B&OQ1UqOZ6T)*muH7?qoU-i{luou0Eez&uQdcU1Fnc|BQv7OnvVYH^vNbHcsw-^Q$I0I9v+}+GbJeIF!;Yz$$tK$j#T!XTNkw zpASvu&eA^S(JPUIG(=qQTE+%}PCH{6V)Gn&GLp3HdtN)a{4;%vo#zvxYI6PNx$PMp z3Z9Tv*?9mQEOZ9{*XsRYG{5>%X<{XCIe)gW5 z+S^IHA{eL(4X8naH=&E-MQ7_<2-79ZWZ=Z^+q%)uoofF}v%vAsLH)DbVYRhl?M~S3 z2`HurmKyfe&^*E~sb{4$D2;z2L7^eYSUN;Z}0O-g{)3d-NmV?_aT29Yl3$C@2h!&5?C5%+edK4;E2w+X1*4Y zYQuQE$})YqRQE?*%eb4LKa==)g}&2bvhiT;sKMV#O4RK8_v_ahqn@zrMOel$WeFdz z6HetIuZ7az+2ND$f~P0|?Jx&!YP*UffE3H^nmV{yX`FerchhtiWy@hxr$!|eVl=_V zG)OsX;jn0>08Fyd=SE1UC`dI~?i;=O?symBy~WhT#4yl@Rlc$Zv|j8$2>6iIdrec& z)HJjI4H~GL+8ShM_T%)LF3&Qb9gl}f%J)EO!fN_o>9Y?WytpxL#Qy*bSK%wossy+S z_*0iJB+`E@y$zWdu6h2_3I#TiQ4(8)OaYAKH2@T5> z-i2zB2FeNntzbIhkJ4arhWBKLdAzWiTjE!3oM@v_Q|Q*FWQ)qsJ3h8&-``r=wl3N( z-}b?EAN)$K{xmnYjtR?<%qZ*HYUl1v28Ic(=!sv5?ao_U0^e-`dg_*7s;0n8#HH87 z4c>Yi-%X(UIya_+dIC0*LZ`A_Mg46SabtGj+x;@hi`@rG4OkNJH+se49X@L{^XCAo z1w?ywl28lVv-~%lp`OE34zPUzj$OTYgKzsr*cxz*vfRI~S{~QK>%p%@`v?1MW;tc!ef^eZLD7fodd0^1E(t^cSz!8Jn>2Ve znLw;e5MJc8Rnv9)zGFEdGLus{ zOq>`=$ai1Zjn{Fm10OVxBEZREUT&kIfr0PJC^Ty#-3Ruo?$>HopE0g(!ZH#vFC~ie z5?NMOTB_NL)+SFUIU!-w3-#PD(gst&W_@^eZsk_3X0xxYt_56JnZd)U_uKn8>;u)G zbz}seRLC&NHS_On008hK9+-QMLxW8J)Xa7jgoHbJub17(Al4m9Dr9y(JyTul+p6V+ zO4|k6<#$v_aV;g0a&0&uEMU_+W5z!LaHAoEMR9MNwqAHm$GYdGH#oyWuqXG_t`>5}{+; zQE>ZK@}L9bEMWxr=e<#>GB^LA&wyDN&$f@SW?^JWNNVj_ZSUxKUE)z`h5FwlKf^q+ z_-*f&+7-Q7#JD4=aBjUJ(Wd3~1=XrHKmYwb+l_r)M3~GdSu2GMk?q@QExnqZoxOd7 zM1#0kBg?BC1^>MHsDYKe*X-NZjv<8q{imlFM0|}&i2et8A*^2i`)|rWn}1k6M&JF3 z6Kqm6F0H;bgsjpr;fFK^9!9Y)8H8Cn@)>`RdUI5aBF!H0&ni+P`>fk||C^}vKLO`o zXyvn36XY!43WY|-(li&a`(OI4ssj!s-b%7)z~Ah`kg5yPEMYpw%A$>~O+&9kDiRMR z>Z|FR2D6a^3nhdQb;B^Qw%U^;Pq6qBWKykCX1(qU+d}7mewoozoG!!bEceB)hH7h< zEx|6#HHFd44F->ETVW}|Juq`$d}4(5Yc2|NA6Agi{+Od?AJ!qNn%=ux!Vd;@KY#gJ zWK&=H-JibIJgQpnL;5ZvHWZbeN*TDc)`K#^*`;<88P7F!#`=W%zTLU$p5p`nwLOhO z(=0-1)`z$8HKCq??M7h4fNn}cal8|`E~L=w=!od^_#X?UG z*k`}egl3_)hGhJF+3@pOh|cEb3liX*^PO8Se~bi3whGW>OC9BG(I`{*zifX@qcqA! zmDM>4JFo3v@3vx1N~K_$>&#R>#}L0tG4*ggCng?0D=J=!t>C&qzh{FXr!+&h-t@fI zUgJX81cF@QnB7xVTZs?q|KJ!CkzsP#Due}?_fibGSN4>n9RwW;Tg@KP#(m#((zYD~ z*{X&8S(qHEyDaJ;ki77_uXZB>obI3R;w5A`0Sr#WE;YCy-XGTqRTe^Fs$CPHA3b~K z;&5=B^_x$hXc?fUXeQP6NUW6}$2W>B3*NLzvsd~lCWrJ<^<V~68xSTvX+orXQP z>XE90HkA%<@8S&YvZY9MTvNcmvwf>Gq4_^NKMr%zw=*-`wU1kBD+FiYG^+05djvX9 zf`0%Ob7bt*0g#fs2(CikU4E~%U)Nv%pW(2`+~8p{&Qne?&>qpL3yIbAg0rkbp>`BA zg@BUQK>g2)i#IY8ng5T4dDiE4KxDzJoAz@(w+5LK)SM>bC+40PcX>$Qws!!>P~aXu ztcf2{E=Ptf7A_lZ{@jtTrTNH%pboubU?aGAqj zVLo_q9+olJN(<2__&?c_^x|&BiPNXoTj{Pdi?s3l1bazbN3=%y9+~Na7Djcah4quv zg~Kc+Lr{Rh)BE{g7}Q$P^XU|+{kqL<)S}WTclR;uutdOyW-2F19{;=TP)Stf4lF*$ zbejr97`^8QP5L~$wR2Aiyfr^z;Q%v{Wz?)XTc=apBe+|~XNR~Py0zFyHeJE{r$3*j zxnM}7&2vUB7Z}F~`mSPTK5`s8*2uEjIy!!?oi9L2MdT&cJ`AIAo$Zw7_3L<2A^$p^ zkLxwLm>IODL+Hr;=k_T*o*IOO_t>&#DFs{lEf}x@9U{hhYc;A3xPt2`k57)rt(l@x zH~I&zvOo8=3f&+b{nE6F9Yeo~A3~3vI2Mh1s7Zcf9pvPUqB zCm3*#9jiude!!;l|0&NIExZDumCD|4Zl<+)X%~7r|2%JY0V?ZS!d3nAWVP&O^Im?; z21ih)A`JtP!c?=c2A>Vj9z)Es#d zF#yk898-SY*kNufHZhEu>ctNxBq38-Cp=2E^aE5Qd1r=3K%Fyy3ozEGS>CkuZLXnF zi6Af4kAH&sf{InNHtH9%4}*NF!e6NCk0r4A8(SvC#ZA3t?js{#*o#z%X0pPwK?CtI zfS@Q&EZw^|;>ge)hu?`nMUzV`_0MZrFn7a|er$rOS|)Y^P5jMD9<9y-fDdJH8=#j1aGZnyDl2JGMB$12;R znLgI}4Bz;DLSteI^{t(G{iaKg<$QS(bpIYFBMI9AtrQTUR$pD37F_yDj1i{rF#DdA z(82(RkI=l|XvhS~m|uv0Z~1h&0i|~%eiH+YUOJqHe++M=#CgKSWXl?FLLMa%f3&mn z{yP;Z?s<1BRLLU&avkwxZ}!1R8fVDq-0mrwBcp9POMgeu>@(-VrZr7-=im?lbmHe| zWbqODx`^#z)RT&Zxhob7!t!865B1Wv9UGR#d;EtButNYLe2WB_5(kNzow7^VwXrUw!7EK}dn#!-Ejhj3(H1MpyIYSU9Te<>=uw2?{5_*>>i1$fz(eaCFU zHHF7;(>G{5GVZlnO-p9B;I69f)?wSJsu)P&C{CX$4HW`!vD^`J7=W~eh*edFn}@Nq zjFh1rtHj+d=6x-v4eRRJj57E{$QhFIE8PO0VfR~8RILH@zJJUmLTipGs9?Yl9W8c1 zfC^u}?5|WUdo)qPDS=Cm^ZbOzfr^v0U~zpdYx^=9cm-(3sILc`g0DTViMtE1Eq+b} zgH#OQc$GVL=j7~h&W7FpxA35`G%L=B$~)%-=G>Y5RRwn}>dW?QM5S@J7y#X?%l96^ zCmjp$d;jRGYWP@dAdc?HpTsLgQ&I9HEf$h(Qlp;l8hOA(p+QN^z@ zk8_(g?NZagKEAxII~COAg-0PH(b~ui3yYP{Pt!Nb-ihj}O4ap&?=+}PMUWQqESO_q zPFkYE)a(c!8SIrFM-^#~~2etj%A z1qEFI7Wy@Cgpti_??y7^fvX@k!i>Vi)8d?><~k2%$|~Sn%AhNh8m?5Vf4r?WCjHNa zwzuDLr=sx8&=iIxSjNdRlhILqOELR!BZlUAs^Uf~iW9lAfM>L!=?iZ`K@|?ESWrWb zz@c+37_z%*#-&TEFV}}ju589NBAc;|>p*Ew z_Xi0cCJSr#=rQ`WTN6#gAwRDe@$02I<^wXYrz%6(W~8;0K}usGuUlQBfc0DhjE*qo zap!d>B@y$e<-WrGRAwG1(BnESZO{6Y+iG`pO7NIq((Z@U@w+4 zRGd72O=El9C@MA-NH=#*-QEGl5$`;&*D~fnU$KWg2%3k%xot(m9-f|_I6SS#VvUku z6T$xlFX~~2&)^*fE=Rw8f5OA#?Lx;^S-sl3lbN}J9-0bz^DgZc|8;3#+sxkUJ}!Aj z#wrWM4$LNuW)aNc-XV4q*S|dl(!xkKGyC7t*2D~1pW!N=@YjO7NNr!`$3k7!_DK9)u5NAm`K9ql+W!uU-6rT%* z>U59tKApit=+EkJO=!~npKeUHoLcrrY!4Ey@6oBE{%Itr2g7LLUa_aO6`;c~j{St4*ZN6+b9#D&rU}Ov!ajTc=~E*nl;W4pX%z0co#$s4wr#=EF=Pdg zdqx#!nhk!1b1KgQ?TRtgHg79JsC$}6utD+IrQ$sbt^COG5-N(G;FFx>_{+cc;yA>4 zA`~ghRw1BD*S+V@&%y|c4s9ss!L)vmbFK`MAABLo-fc5Sin!SuY3J>4q+%?i8UbA2 z=<8Wk7PGTW>0d;oA%zkU=#(zq$_wnX?p@hwuD9y$KA*iy8NkbONpYC>+K1F@eBj(& z6wQ}>EjzKjU?F5?agKq+BkaFnT2VW9?AzBp;S|uYa#b5nyWFUdfK%G74Z^4?v-a@|F~*<1Yi)oH6>?^7Sm#E6p6;`7bYAWg?vpjJQ(XD}fLpiY$M96+ z=6xiKTkSP&qs>PjkO@fRg>lgkw*6jZrNPjoCF{_*NzK3~J&FsDlrL zjW~r+K-Muxp^$Qu<%t?!&V6r8a~%kkNtEkZSHq#lh#5=OXl>8Szi*%}c6_HPEBNln zS@gM3P%!be+nKa9_O0xA!Rw4wn*U>%xlSX#N0z(D#inRK%}~pg6?yd&U2JW$LM%VM zX2}5pRar8~tG_ZNFtB#hv@{kQX({Wv%Bw?NiKAyzz+=q)OAF1@4L(2;IH$GKxM|tu zS7dk1qqVBaTjLLk5RX6KzG*I`;qsj@Mst`DOk;N~hd?Yk`8 zZSj*?oZtOZr%%t!tQO<+SD!Dxi^!u+g%5s-x+lKOM22zrPbjJFHb2-{{DmkzeGsrM%p>&+thXDuT9y_ zJF<=PZJ1;DbFBE1UVcCB#Wob+LQ&wK#~S|M>90m}jEGg?0w zz7$+XLaLwfYCS%!k9DL?y?CZR^XQSxykg9vTLE=+I_V8aGE6sRJg&JYO~ta8=?5E` zr>_GhUt?};C74=f^ms5CN+Q{c(ye=U`6$K~I#GRP7lYPP!gs6BFupvR+V+LpNR(1_ zxUmp`NZuKoJJ+&%cke<{xqjgNc67oGV;|8SRqgkx523P|ZC@&Kx-$Mq$*Ser@5$25 zPc1tN+OPVv)92SLD(>^ac@vwmiYs&@ieKoxO@R(;Pq5q zN%V9cXVVrf4z;}cjn-S6`jVS6ZJl}0^LxYpa|^Y@wh_JRCn9596_8-g9TpA=K9RCY z@qV0LL#o4!NdqR)m$Q-rOkODQ1Yw~IiytT>jTDnD_{|Dazmpp`K&w|Tb(jt7`Kt<@ z9|2RyI$Ke}^47b1odfC=|LR@%M))KJzVne~q2Q=ySEXI&TTNN8;Ml@nedZN5mpDy9 z?>GOv(W)PFDSTScT7_>jHyFCajV(rp8?0_6;F!fdqD>_B0w$||BwW9OoruvoJp^9O z9)D#1*;+^!c}FLn&{fubS=mM{_UD(5ds~K{k%2K!DjC^7w=QH&Q?=N@YgcZbd)8rk zc215gpOZm}pzTeyZR2Eq5PL(nuED?KFTUX`vksu-cirw+XFe(ILX#`|@Bl4c-T3uL z3P9Dp&Xyxa1caQC@Lt|Y!a#hYO=rJB%e9_vY#RC@&^@G{?<&i4I-(g|+Mb!bNBmfv zm6F)#DHf;Qw-^vA$UX33NulD=Bs5L2k|h9|ebJXgZUO9i(%ohKONPpzK;T&QiSOq+ zt7jeF_UGl{%#gYQW;eJXR-7&G6Njs4A-&n^nxu2f4Lljkd^&_*Fa&SitXGplBC^ZAhv zG@7a9ehG5Q>H2MAEz6656}T@3i|;4&;WbS|?=p+fW>ji1&)9T@X|LT@4)cKgzqCKu zHe%_$_(jW>wS20ROMyD{Ue~l$>&xN(O!$BmDTYS5yK z=D?iV3WaYOXFbUx-zC)}k`c${=kqi!AKs|=`{Q(l6;=o61`f^{pXj(fYQaUcHF<=E zJZ#Y*ai&xZPe$s^o;o`q2!0{)S*4G(c}p4WRdU2ImJIsJD_*XZQ%S(Nef599e zI*@PPU61-*eA`iv1oYvXqa{dCS7MJi29N+BGO6`VX^BFQ$J}G`L!{bg`;! z*S6y>UPEPLnYPShE?VNHDYNfgarFtHpm7JZ<8?y+Nrcr%YKR@Vy-OFD^XCwsnLOvM zEt^4=H2kyrUT7NfW5cavlj4kE@#h{x&QZ}L>?m^Oav600&`|;zq z5f)mgt503B57BQIEv+>!=8CXrIe!H5E8hnHDr&}XFDG_>MQ7)8sry%bHXZomq1%XW zI9#M4N8TTvX-$P?pEIyLJl9A%Bi$B!WPgwR$;)FI(Hc*T2jQISQCs2LZsFp^2Wu-l zlo;F?g$Y6b=_@6LN4uC~7Y5SsbUJ7OJ+~_+)*Rg=I=Gv@=8Dc!nL^Qt?+*RQXsf}M zdD%B^+#@9A0gIM>EZC|Pq}bHk^yc+v&mz_~ynr{RgYmi`r&mrbQ`-^Odhc1VXhq}j zaWiJbB!D!V(% zz%(!CBr$y8ucECRHgC2kt-mfVjwdsBrJtuL&!e)P`f-mFU$^;Fy0HcK%4jB1P%NY) zoKm%@JxpEc@7qfaU^bCnRZ%=5f5q1eb@eNBIpZRBnaMBFS*vGk^WAvXyT4|V4l%`4oNC7&?!3P|FnB4miR zJrs(3SI=Epd`taS#yj`#KMF&tnC@%>GjV@~@4h3BDRH4r#>+2%LO7WUv*;L~S)ZAQ znc|^eY)*Qo6Bnz)gR)(lGLny+qR3!u)au|b;P$9#4{xPe{Jmh=1NBo4G!^0Nv-6kC zeAqs_rPcmNy9VeDdy#tftXMdW&NzG#hHB}hW@Fy1sr!!&i0*idB?p$>ALn>xNi2Qc zAu7(V)A;O3k}EzVxiP3n;PK(&E|@*TKNd(Vu(SSlieUf#W|<ejNmQ>qO+YQ{2?ZqdAXG;khewp~A5i1JJM5}Gh1J8ir?)oUMMY%$=(UDUYaze08t?qM|?wDo!|$D2~5j+v;E(mpbd~i_?rP&xCaI#&)oenc3P?^PjkP z8;nU4)07ER9Q%%*1+VWu`ON)yt@#w`0`!y6C@i;9aosMa$NE${0=o@w;#QgDw%TseF`GNL>ymp1T_I<035U;tD#q{gjddeSGc>EJH zsMxry@3mID2b_NKUHSLeSldpO-y^CqTG(}CneZ>;IM;Dw4+c=U9rpS2s{Key%drf5 z#x&)*;*cNZm%F;BoLf$#JG?1=tBU`tF`8lZYM#6zGnR&Rj>Pl)l^>I@7Ja_`@6oV6 z(8MZH_DC*(hx;n59i$w0G|VINQItkKoPe;{K_zwR5{rfmLEx|{{bbmuY$=ip_@KBv}zOGR8 zC++p@m=-f=l<#5&i-Jm&A+#B%;;ZepsF_!QPRq^Nt2S#|)qcLks!8N8^oZLcBF5x) z?xSM-s4h{GKdR!cq?ns?|Mv~`P9trOwhNc&bMaF=PZ;&t0b2SqUK;i5*Z+WJsM)Y# zmd)gT3qTDXF&A3bl$-U;#jiu zHU$VMo-M>yz17qX$&l*JtF z5sM0rwCKC&-5o8d8{Val*c`)^iE~W(ur#OH$Zee?NqBSMbv3W!usd5TNOyg;X*14_ ze?UMNQ`3SEgs%S|yYxDBde11~%KDyJD|fZMb}`N3#qKjr_N=+3cs{@?J?3{sg>B8- z`{Py5$9mSRBO)vbZr9(Q()nOhO8|4I(c>8G>5dLRLh~9wEjz3^QP^WWb31mkXva@m ziYZpkfbW~wVsLx^^v=x#bZY&5gC35csQ5Sdl`KWO6}h2B*E37<@l>&ejhz}|UY%y* zgJk>4xBg4s9njmqBCFGxbHzu`mO|-$zrg4B-q1e)536y#_yCq@u*#bDDK-+Zrk|8<8_F&hmz$4QetpH|Y#kDie9kScjbr|1opZrcE>fDb3Z@ zqk2uhPBgRt^!`>}J^_K3ybvzIwfy`EM6(G}D}Vm%ea~vlm{S9kZ2DNuySY6ybZFYo ztxV*KBiH}aYmxPubd;914h|2iZ#NscqgyNxQJvbgoyrGXc=YJ^j<=Mpiknj`qE@4R zHvYDJP5x+wPxGct4*;gN8|3xyVQbM7S^7;5-}rT+8?SQ)J}6rkDP>v${+ix@)?wzf zZ;!7OJ$L`J6fv(SzEliXdhh+X`)y~*`+TD=Iy&aG750?N!Hp_arnKh_E@hlRl(uZS zaX8A7E)$RIWc*f4>BJcD{HrgG_IrJ^pQHZi4*8sEGsy@yn(^5Zdr1GlH&(l{tR zGrbbAH#;*o2NLZaHnojEF~fTQl4=%IMZ>AN_3B5 zIc!iBPd1i+FU|(&ecd`foqin^m8u1bsAIeWxDC*OuTsPFWHHdc*`UN}X6wQ6*vmyu0F#gtX5mfbvcbLLrj5Ay5(iqe^=7LT&0 z^+3;)eDl%z`RB?%f?#7|Z-j3T*05{K6~4`|4T!>M?f|IXY!Y}OEa8Z%z4U^!;c;%* zv}qeW?L$hLE>aPuS6BTe$o`sFc@`=@6vxqElW~*Jo`QAUx!LL9`2Y07Cj02!c=%Sh z?Z%X(NUo#=KQk={9CJS`jtg~W`9M5g&}|i%@fn66ljFwVixjNcGwWf4;Nrp&mKM{MaoSE8~{RRjo>6B{EmaPf;?u!Sh?Ff zXMmg6K1d7ty?TX7)%|%0ZAS39w{tORdriza`}O{~F|+yCn=e$Jt7v4R2eYwb+FV;z zMP~4J1DB_>s)!Os_*1&A3w`bhBR-O`+@nh0OTa~!uUv@(T4bqL)b$LS(u~w+|R19?$mKF zv&g=^!*GzbyI$w39?f^pJrP_1*ps$>HvDn&Q_}Z;`{~m>opL#m4Hcua?$0?Bn=$nE zwW9Jyx;fcX+EHiUV@QyKw2hL$&+I5>G)L$YX0tKf^Udy=n_Onr&I|`<{Dgj9Y@fgZ zv^HK|bg$YQGy)IM-Oejtw;+)3_})N~sx0|YOlly=&f%i;{quz}d>?2uz#=~5_?6(* zSLK&k0oT5QRp|9k7L!YuO z)JgJ-2|A!2{V+1tSxCD;(Q4A>w({utV6la1ix1oNzL=GDA3gZ>%3}o4_?hQHR6O~J zrsck)95cMroxMti?+t%NKHXb$cSC?Nam$&Krxk>Luft`j8HT9Quv5{`7^6@;cHkTX zIvJ3q=LSQ43D9}DnJr!*8514PE=!y|U_bh{IN)>0($0zHqjCWPygO3*{yz~MtR7kcnO17 zgHP;%wRGsughh*v|Ks&_XnM(&HmwJ&FjV$Z^k0sZX`)MpGq=fZ(V}ndrc1>y(q?4G zmuqFVJKtA~j@gnkq*YS=u=>yU9*ghUcz7gHI|E#I4ruo zmNq?{ZUb19A!=*ma#(^Ji@yD-^BgQG1QVByvxZA%hdtg%2@t;8m*f>a+<*G?Y5v+7 zOM+#kXVk*0H16>*h+4jP6Z;ePM;sGu?bn;V?$6Gr$4xkPTkxo?@;9o=zBQE^#bB4a zS7ov4yvM0ynOm@&@~Av9;MCw-$CNZzXJk%^%IeP~qvoEc?uU-h#-$^Ob=26-y3e-m zYO7N;SDLic7UR}kfz+y>-}v^tpWzRClJ`b|*5xA>E2XUOrTMWn-}62_ka&zMYB+sY(|Gw`9-FjzWaRTxON57Oicb>-0?i7lLq0K4An)~%C1 z_j-B&982hdv&$Z%g9uvm!WJ~`7*#~})MQPU;^4kdZBWSTlOHojN|}f#Gq=k`;>C`$ z{dfMVxiMpVr4-yFFm={Y`0hn4lbrj9|J5s1X?YQykny0K-)A_l`ef8ePp=sI>{upE zc1N8|8#hoqj^TcEOar5ccG4)vonls|uhQ`2r!q1!@G3X}@@mJ}|IWH5gN_hH7TGBN zZ05rz&sAyG!WpxhP^X%v{HX1rk&@I$mG9CA!cdTmg<)3b<6m?>-V#s&W$t@g|7}VSgTSQT!AzO}wp<2u<*?f4 z4xeOu7|4#{$Nm4;S7b)wDf5o6lRK=*z-jlEQpFalYy46DXMh8TbsXT*%HvPg#u7`_2{G%A-jylkIc7UifP?*VME~^N|%k@iYH1@$jAc zq8ExpDXBP@xj!AT=D15)GUPpn-~It{r~hqC>T(B`KOVMQZm7*An(^QwDAD1IloY8! z6zT_mF|V2%=9ke;Pw!vY7bkz65eFthx)dHW_pcnm34Jw1k=YJTVXv?oDVGNvbN2o< zeebb0G#QjnV}vXy35?6U&`|L>Bzsl!R#xLpbk|$%sc=^Ibz6+r74HpLV9TJ2zzpTi z-xjN>7;88)X{)&`zTXYYs=jeyw#}*g*7&3A>$bIO)w<8QV+8gdDvslc8Nr~TFYoSy z`E16-%FRaG<`Ve~E^a?cNi%N6=?afa&)R~zfa`b|s#=nk<>z@4PLX|hmn5HuF~x8L z;u|z+vOH*Wn?AD+`1&gL&-G7Y&!t-dnC{>@#e+2ppxNAX7&I(Z@wN;V;lzO_Kth_Y zYNu-)`v^Jz<*ZJ4gv|n^qKVvJl0sQkhS%(kC;M)U;GauKH1luZczjxRyXlZxCSs#< z&`Lf==czHqs|wF9H{i~CG^4q`k}q7P;jat?vfy&hyk~TEQ>%kg2VV=(VpK4ou96}l zliQVDR$hMCNdt_mU*g0QS7PRjW?uVjb@@KZp2Jk1jcfT%`c_)nXU8q#Pft_xO0+7}It&y|*n7>c}Ii|R8 z-`sxiQNA$b3(FqA()0K?%D~H7w^42U{E8rvJS(K>)_@QKi(afs;2rvC6;_M;`n;ta4xFtHy;|9hYml`M~%AR+pHg<^G!~~ zg1noQ|FJ-a+g7#XQ`$3mwCfFHeD24Xu9^G2NYv7(TR))tReQUOe^o_35JB(vsOn{q zd3=67tGRs%UGcE9_)H(N^e_DRT&0VB!Vy}BgL80f2#Q+xh$sv$-e+&(1k9sWGxPGK zhqp_fI=nIIWGo%FoX#^x@htv;pT+3{Po`28gy=q!o4QWetgJY2$EtDeSvC^Y(fF2r zNMBXAmPbbrl|JWXBsU~CwbNGRm6~g9O)?rK8eX_Rq&snDx2H8D`?EYu{nWsmd75GG z&e6b(k2I@u`O>9XfO9A#?lG%PfkPRL`v!NhNA5*pe?0iG9cb|YeMKA7WgmAje>nhL zJcd%rcw+B9eYU;)-;-~MWlS6_i+DC0d|=f?x0V}kwHH@Zsl^(rc!ReLM}K7;CbaVg z=SMI@$wRQvu>i@TmPTKt(X)7S_BkIhK$Q-wydVF4pFvLi+n7U##`Xv=q$iOnBXu`Z zeuyAr`}wxHo|Cf^e3arAG^?j{>9iZbFsG${B+F%d^3iD9Hi7^D#>I>4)N3%<#@o8s zU3XZ9A$}cYg@-biW*q7zfOr<&(OZo(o_~WW(Dq%sW^GLgXxvV|Q`dg|_Izoi$h=O> zEWOqDnPzt51`VR7Xq54u+MGT1ZVMgBVa|m#U~>wU{v0ZQ9OHD}YqucghL$SLlu|?K zXWF>95rUfsPwHiw^ov;A+^HT8Pgj1-)^|Q(-ZTD`=3;rHBeYh3q4sO48 zbqSMHaGHNs7O8RWMuCjxam<*(^^CV3G(o>O=C?e@8_Zn_M%kQ&BwEWv1fp6-+6rawQkeX zbJ?4k_oseM2tBL|wd^Lx!TAY&a z-1|&&9mUl}u+3Kgdc9Myc$q};N3C}mzBqHtl8Wgb#-XJ=BzI=`+v@(E|H=G@j?};6 z%V1c|DRT?LUdrds+{}tQ%^Rpp2FR^zCY}HJE5HtRHa0^FI&UKg48@wl`220&8-d#7 z*Uz6e{S=y07b(wB$xn;nEj#ls_C9|b*-RhmV$6j$-BtQCJtjciQdPH00tdd$jTsi& zzI!Pz_^aVYcW&MHHES&ZXWL}I>Hm{#m9$$cNZ?)X2lKuy2<_c->HK>T=2xy*VPmLM z`L^mR5GIb@%gjYw(5Vm1HG3Gdqbes*YwcF$?R`$3E$}E@Ql+_UXF;N z$ll;TZvVf7et$fH`0=3igGuZ&)L2<7Lwy&8OTr$rm4-~ek>PyW)?Q-ls>v9^Pb?Z2IzX#`Z zn|R!utpMAC%_d?SR$?^h&(~qZxQ)g46q$xw*9B2nb$(bU(JAfj&(m+-SE0X7KeKtqo;YB7lJb#6>!RF~<1*x%Aj2+WxfVNGlr(1ViNm@OH zNt3O|-h=;z6W`lpgJr6`ei#R+`ss-1d&3NG)1)vfL0HoW~68FtGo?ubIltYY?kt zFokm*PGuKc6aH16K7H!>GDvUJvG3HHV~yw2PR(Mb_jf9XigI*;Vu~`#=sr6TrJ7&p zK^*Pv$hZX_NiiL`H`d;I4axLF~3v(7X6=gXDJ>U6PG&Jj2?Zv`uv}q>(>P@(|J#-MI9YR zUBDO~3c0DgNv;ezj$&cquG_rP!}$JvExORC-C*y>4$CCN{8agzUhdp6jZWzv@2fjI zSVyXa4}l$h?AGwiXV0E}*!{R|{h48V_r^1-okQhhJaGY^Qcy8D)9pRR67%oM8vrhv z*nKgy2xPt6K+~)*C~Pc%^z1>!jUdp1)?B~0b<~QQa^&gm$5tj7ysxD{@~@f4w`Mvi zC5h%NzrFo92D!!02z~Jq;^);>JPeFg8qM`oRrVN)sn5aOKZo~wjwC-v&vLM1aN8+0 zmBy%5Ha7oyuUlr3V%&93bAp_L(bFrm#$UgQ!}RY{(IT*9y#!Q63pnaUS8*h?b~>v?2oZY$>|qCG6!PvxGg+9 zfz-hqb0SU;$s4bpAzBV34nY`UEyQVgBNh1>Rfhyt&U^LpgdW$e*|QlnfkmO{QN+OE zu-_w*Tol8}I8g8LHlw;*oV(&-C}gSUdrRG}QyXZmRWS+Tc*W1OKVq|H>VgXlmMB+8 zju~^>@eLyui2sKO9ODezI&u!!)zpVPvNu5kW-AQN-ip>9V~JIa?d}0M1Q$gNT(yIA z@KqzZRn5IO31CidL-`(3s6diVYu$G6w9-*0W6=l{R&YdEj-I?uE-XGSJR z7N4GfIK`K>Jj&^v%B_=ohbb;RaZf;(<>8|T0|X*?04RJ()o%z*;Q4pv6r20uHB8_T z?(uWoeDJ$7;gE7*f56jABi=g^cDmUIFwz^%Xj((9+Aw{=s&nr?f0j-KepSn7B(h$t zsq#PK)hOQxR68R6q|yp8PV9j&=j-AVgV-r1vIcw?m|%a6`W#a@t}ZFbqf;|ILZ0nKSM)g*Ex8mP{eq z$1nA~eFb$Dlfw_9Gncyf$VQ1d>>eZ@SWi`gFFHk7)D)^uu@K**$2ox7)3PHHt~ zEM19LF$e``7Efd)9yvgpjM9!l$A<${fn$A1tz11(yzLz3=nmX=7Ujep5CtG_j8{lj-`#H zNo5=z^9tS21+ikPrvx${&PFENop4MHVz2*&~q+`G(XPtUTqKAeeJ;x z4knu(9UR$yu$ofy`192i=oYbVkO4LRFl8U`mhcLmiUdonhu!ZmB#`9gsV0S2-BSH* zh`;v7505@o>s>};ZJ&GN2r1dH$Unrz zTgYI&e*bPhPH()9GVA|+*mv2@3jyu95M{p`ND1uyoPEy_VDkv)=ai2QhXmy zuGQfPHJsk$wv10f_jdWZj-@+6HYM8oL(3mmcfhD@8kytp2b&41wa<=SwVv@Tz!-=) zC9O-P6RVq7iWcd{_t|rR%DBUCp443)!U3wKV&B&sYb_I|gu>n9eb{zsMyIvE-UcXys{)a`Ux!tsfRAsoj%V2f*YfO32H(y9A6 z%d@X_PNarCJnb3pxJ!#03?&*jYUFj|)5FCTUHfr(8#hP|XZ26GqtfGDy=!6GSnu~= zNw$=^Rmv;Y7CO+X$13;y6JLVCK_g_$|Lw(o>H79moMoab<&Ur*UR3RXeC1Vqs%Tv+ zrOG8ns~&A@(Np!icR-ar1Aq>?x$s3t21j;wJN>87xHPp;oN4`b{%pfuy|yyw7gC#6c`i7N>=9hJ49)b<0tKYI1f_Fi-MF^R$*&GJ&6I9HgivXj^E=f|6p za=(5<_u#=wwcl@GGuA%7UB1(<-@tdHBlGRmY9+}W?ujh8o_NK)Sw&I z)R?Btn1_gI2VKrL_tQQAyES3;alQwwX<@p_jgg9m+S{idQBb*CEiP}<)>cO(3}p_F zzI!h&L%d^{J*&ph zX{(zY0}{}vSI;bc7`2@2u4&}G-J+{+=m^i=JxEol@rs)7=WEPj?*p~5q%(qghUL_l z2Moc?3TmI-yvmCZZT zhdy)z9H$pm@!V&<{8{WAG3<_>GIi?9b6*~s#kwIPz>(+W=UPgjaRL$^@_3|gs0iSo_HL$7#w8l0JW5{_(`g!?MLoXN;t(^{av1>9h9(< zu8P`9s1w4JaO*1+ngI8Io^p4V(4wXUQle6z?52BispXphXlXINJ(PZtP6@S*od@jh zbvJO-G1slZ${D%h4kA3Z?Mtj(uY;aPO&NP+TuiV>^*w$EFEVP${v&!Xe}3%Do9>$g^W-f6YwBNyAZDRjdqIb8-PY>3Znqr;F-aYE>&WZs`78W zmoWvcJXzro%!e(M0c}#fq*@B#Jq$?;#cJcY`Aa#%XpVL2 zn4v?Xq;Q}BB{1Bg9ydn=^%u2sNx^*?)DgK>hy2rNZ5{#Z%&VBhdx!$nDPJiHX$`a3 z?He}^OiPXc(UEDCkq^|Q0f@uypQqb$dE*u>hGr_cd$EUZ65AYBACTie0;1N-T+GM;hArDH)%!V{b7YF5>tH z&BGt5YiPKj{t)GjyiL%OsHChXKiBNL)>(Op_sb<;=$S5sx^@4mKj-k0cP;aYqvi|V zxV9}O&;MEcwZC6`s+vz%%&zNR9zJwvqIYT>YC6(|!l5m|)2GFocf;d)jv(VeR#J-l zFVg->8bTkM>cvlD291)wYQ<yQvW zUhDHVc}A;}#Q(Z2kjVc!ECc>N(mTnIE2VOHVg2}_^=Bvr`HaS&B~t<{xIp(C>&#9GD zM|9*|lsDP}otX5;)>JMiV-{`_+pI(xS?Tdb0x^O894$C`sn zM$#Ul5!j(hpU?9RYId~ucW`bY%4=?3+-*v&M`yAoaiy+~`^>NmLq)PA(jwh0UiG4& z3Vw_idP3BcxA*_LUcR7j&^aN}31|gZ;sslqT3@P&e@B+QNn2H0T`6e2`@i90zXglJ zE&DEXrg9qvaiycmnDyLs$dmf{#!xAN(@~`qBwr!Qpq%k!%1Y;2O52Zr3d-hjRmcQ@ z-k@V^zX3A&7&*M(UV8n`>{K0VHIdVg=>K(_--uAqkazFj-|$-7k6w>Lt5b&#yQVAU zPQE_Q%0ZJR~)C4?3@~u(&hTC%Q#17r{v8{TNO?96g2i z<5Fo(1e)pV{6fkh!IHu*qr=@JcDqnslZQZ^M4V_Tw~o!NDz#&LjJ$UENmC)^JUSUDyxIan9b|gdv)m} zNw!}8t$_<&uc-92n60AZKIBxZb-Jxu9TOl_#-0s&Zk06`oF>>3TuUtR-MrQxdv2ze zx^?RPKGDW`Po@?MFh-r!K`&^;J_{dj2SeYVeeu~!y?UoP5Ezr(VekH6dN*=0%Pc}? zX#16;mfNaN^M%Smv&a}(4#(h75H&j%srv6!n#ds;Dvdx2x=3T?F2vUOP!Q)H=g<;z zs*l56n$BRDliaJo>q`?k_h1eiG*{_Jr0zo}9Q^new1!V0^6>QS?*ZUs zRkx6pwr~BYZn`JMm<-402$UY!#n#Oq`?^sHHHd`pLdgCzBZpiU*T^18dqMn{`>=D#Q z=uY>2kHXsa0WI1vdzCb_%p3oPq`dh1>%ZnB%TnfpL*q<1iT1~lhq_X*x8@Q6A#z|T zKX|$v+|@#h`BFdV9JanIQeJ_ibZFmxRct$sX6j>Ku-Oheg<^_=iC3^989YlDuQ(3oV5KiVLM0+(j2tX->BPhv-uuJ z%-FfI_?^*$YL^AuDV;(K{0P!XMZVUaEWK3YpTtgg)O+RjOXj43>r=m5qhfo^t^XaW zkCcPIR;LK!V?s5W2y4yjD+d9>TK9pqO~S6~KkgzJDz(w}qhCJXpCdt*)_%jBLC}tm zF&(4CyItP<&6{VZ%1ZPtm6CUGTsxVX7CM|d8fKYQz2)qj=npDCa&N7(d$aYz$+wG` z{&;i=gk-Xz%M3aFRyD0D8Q2Q$sF%_5Aar9I2=5c0WX8v*TvPg5#(3D_CcyvPoeZ%9Qt4jg10rFpSqB`LWesuVY&? z8-ZHYtjAl<#_#Y6Dmha9wx)VGm*xia^A^?9wz44&ZM)Y2+8Vw8y9N%31Qp_py9a!w z9G>`C7(-tNG_oLvP3y9~O+@IlnGT-l@II}j^$G}VgHlF^i47by)zu5nV=}>+isbnR zVDl%TlP_`Nd;OKR0|wke&s$6Xcg%5}zz2_d*3IqohO1+hbjv7%6kmi5Rd0Lb>ea_z zEEp>rsDw_(cYn~Lj>Y`EVA+?y_tX59RY=mkoDNZf!{ckvzyD-;!!fU@a3Jlji*?&@ z@bUb(_9YYcS*%F^MOhCNAl$2%waeo`ug`D%eD?I|4En&__xJytk+ohoX!J)mPWDyQ z$Jbm!{lY#npqGgt_`gHUkw+LCcohpG0*~Wz>oQWQ-sR=L7otwx@$0%TeZlY{L-x29 zb)`qbgY|BknlZgr6AHSHOU@X#oK|?DVb){RO!Y5YoSdxZOc|S&AWb|7Dd0~)Rs4Rk zA6UcV42s8sB{qADhp!r@6C|CG!fOE?8-V1lN&}_Ha+7F(YV3nDSqpS^k8YCA;VpcQRFMrQ3srDzz8fho#)nUw#tPrck^&k`phmhIQ5k^Q-|LE zB=Y>XBswFbUUn19=>SL44Ku0U$9zUOP{Npmf6fwJ-u4a?j8-WS zYH;yuv-R|rOwSZ&&bvmxgf+&q*dZ3uBelw(37TPQG*#{))$3 z>XW;S`F&6A%<+`w?bGFM=be{pODhLoM;eUn$10vaozmqydI~MZDVcQPwlM`V-PF2K zBinrAD^GIc=z|(-Is#w?#kdYi`$PK_{ssqSv1wd2lNh(pC-zXcdu45FHv{HCC%4l* zN^`P3MwH?lb6e_cAnfroY8?_&ncI!((O>H&yevigMvuMK|_zK5g1j$N> znxfV)Z_Xpg{0)cD3h11=oRL# zuV-@Q*3{_OdC;mEO)OfxSaP+6n;BIs{6&n*@_H#*kb#rv`3^9#9C&=UtY}3~38Z#t z;jU!ziYcg&O`^Pc?Gss$+~wVD>ifPP{5Rg_voKjF&M#l)@7Gu6SShchE+dp17gumJ zKguBc^4jR=r}_0V4qe*ZZO*HHx1!I&c&66Z{}OVqK$r}q?R{sH4-jq)D&3= zo}B)m-P-w3#0%e!K<}*cf(57k4688xdhSf;2J=IIexO$;!bLxo@`iQUKk{d@Zg`BB z+_o6++?f&s+#6+xIEGg^aj971%cDb=ztY+gd{8uA@t7k!yVU_ahCe zw}`JUn-F_^8bjj&XQY_e1OUkt#fZ4gLqu0_2+J?73Z;yha$KG~11J9XVjh9c%?RV* z`m3{(@%Ot+&TR}<_%*1}#fukFX^lez{)OM#et3Wbx0M+D^Db*-TaPO6_YNDxAZHTr zEreY5ze?V<%Ual%oYNz`sLI4zzDSGb6>*^Y|4byG4Gy~sjcy`!f1yB5r=BtD64u5~ zX~1cMdev2v@eBH5H@P^j-fMT;Md&b>#m48%k)1MLgPJ`KHiGf8xNeivudHtwqzx<0 zz9tOgjr&FCH2pMxZ}eFne^)MWCp4-(A-cVW`)LxP?E}zi5bE8-%VmLl+kKS8k~Zz% z=fQ|$-(jP%+r25hujc0$LQ{x(I>^pW2sW%bjr$vZ#t|u)WWnACIKnMfqGjrjUdc1y z2};)Zl;1wS=yn+q;ZL@B-qjSH`H-xCv!+4GTZ?sEIXzLH~icn0{zu%$pz{I+a zj1bB#U)`k~vF*a|)TW>Y z%AYK@liK%1akb_(|xPVJ_TU5&OUyQBL1i=r5`L*tv4%%G^~y!o8cfMtABZ z=7y@+Z>5+!^e{6MlgDC%7>8j{l00ekx)&R0D^ca`-n%y^;$PvlZy^qf3uH>wBr_nT zA6)2w&5zJBZ-`zwsA!6fVT{VSi4*H}e?u>%YwSpP@p7l;yR(fhSsZ}zz{^)}Xw7GW z`6_DZ5f&I@vF&DK`Tgu4ueV*eHWv#jkXI8ne2o_y>SVJasd8V}1+!Ix$<&J)!UiBc;ba=dEzg zTcDHP%y+5i_)nki_fBWT$(y%si5+pyB+I^iTM!%;;uG24MycDRv*6N(1n@TV>+96W z`npZt+Q>-TDO2`dxbl_RS2zWPhD9}?QsYC)7KM-K=Z3BGNBT<7*zqJ>lDgj>xMCy# zTy(%>5rWEw78YX)^vS$WFKkR^!#*=VU!93%Dk2-Tzba9%H1l@#@9$+d%8jBtnb|T9 zkAsbyD*(*385x$?p%$0lYqY)fzY0xuA0MyujjeilyE88rev69UScMMWQF=zV8Z1N5 zt>DapIciAQ|SsXDuZ4ZM_aEHVrD00mJ9jtT`GR8miN#95pD*&CYh`@D|*-;dxOZ zByE|66e3fWa>r@-@Vp@9+Uqbf>6Ts<+-QUo)f(3JZ8z<^_4Y8SCOCQ-rTgeY?&v$IdL|y#)=kB zsIR{o%gpeTvw4FevWtjL^_paTH~ebxq(Q}P!^^TBSbH;daF0*n zX3DdsavAjI`42Qq>S}5>{xg^2^=qkN=v4GXFQ)z$b{|9-*X`~odmQSE*LE{7V9V*v zsPKM)0D!8qX1+o(zWm(mZzj}71OZ#5`{Q=(*nRBSHRtLqIuXN*sT8qOQWbM~ej<0* zWn8kg@%7;f9S;`Gg5SJw;Been5%^kEzW({K3SMVb{5g!Gy+zBGY+rUb@ueeAR}{V>Mzv+*I&zi61zdvo{IteZ6x<$| ztFLo3MDf!UK1zzE>*f4>8%|>~TXrdyGd5TqGq+iU9g?im3RJ9%qUWL6TnCXf?s zzvun+!Cwzt4FAFR`S&yu-L?c6QYe$=y^D)|$`2L$rngsP1~D z$JKtu5FA~@3z(`Nn=2lh5bq79NAnkK933Mf$6QK1bEXVJqqwxOeXo15kGMPpNBK;j ze$mfypM-SoW$^NA>;L3d`uFd~+f&_rrS3lmME+qFx6VoD^5x5V9FljXp$296NW}PZ zGxr&k{WhdiO$n@7PH>Mr`jDF}PRXbj#@1%<&z=frO1e!c%aDbP2#0l`l=}5kcpf|E z1LIzz^l<^5SpL;b2bWGdyKn`)ji~Oi_-4>tq2s9D=ItO5?p4a`voV^=%FC-mO7l!| z8#gYTI=Zc~@tW&RmnHK4J)yLtzr1WZ@CdOpO*#RJyF&Tq zWVyx#P8TpZ7S%UQCep5o^ElL{W`BmIz8T&y##2DblDE^|F7EdF?=zN3qpe&>TXbz< zbRhXdL$dqhZMTc=JUALQ`C6Lxi=V8t7>ca~?%ZV#6Zvx3ki<%Oxn`NUX#DPX4V-FN zydUp2@q3z3wtY@|Oxm=kR@r=KF*jjEU2hCV&nwP1*yIVB8Jp*pA~ell-gMnxsY|{X z^WR)vm#wxlwD4@Zb;P7eYv|%7e;jMBIGvj6&MteLcoC>({+FqWon7k{PC9!-oSk&c zd-TvXj?rv%$T7kBZ7(}TKWu}3-9#(97dV(QVo#uqi&G+M;FHQ#+`~!-f_s}@%r))N zVx39wNaM+RS&n(i9i|hdh!eN-Ih1#VSBOP^3>uyFP6!E z#+^G&sn1-UYZ9nJgQHt&YiBy8zUYQDqbX0`#z#d>&65Kju=Nqxr)Zt>e^OP=ym&D@ za$|V5cwUb??j2z_^ZE1Vn4jzLZ8ui6&f-Z&Tmc&U& zl(O5jmbjk(GY!W4kmO|-xc3g<{#yZEyKqp$_Z}uQ9u*AR_5&z0IO;H|$1lQri*tki z?|yD|NFP0Flsi}=zJ*OEUZr6a=V>H!TeS~z2ooVcUCY@mV2eH}UCMMw+c_j zNd&3L`B{zm20eTC=EchLR#=ev8L&|s>>y<0#wL8}uW8R4K-abb`!?{xHe@_ zr`@7OmoLw^ty>oaTfpxIWkln;*r5Fr(D}zQl{VWIO`A0{EBV4ys+~=4lS7&M+7aSe zp`!PND}oQsFtAGvz}JR9R;kXRUl#vg(HX3kc zDhflxMTOv$;7;~O%;iPUmw3@e^ql)lb5i=!jxlqS$f$rPDGcF@ii>v>^l38GnyJQY z7&&@$4y~k0^Zq@0H17ZNW1|qGh=BR?Lm9GWzkL4msocda<~~jNm3iumY)6dH2B2TV zvGU{(bm-D$!&a}i2!!k^Te5g@De9E*53YQo6ir4WSX9U9d0l_lDGsnBD^^}~{RO=Z zW9okX{8=-u+JlxZcuoHqHxy*sOHPwuOl2{X#4vdxyN!Oz=jtHiD`Kt|%Rzi-M6T?5 zVUF1HmH7UXD|_39Y1&X^xO#(|_QR88Oyxd1DT2X2QXL{3j!*=ZUDs5z+|g^Lyq$>v?? zAQkKJ8#|otDhv<5UFKpf&WxE7=iy$dxIK zM@YS1Oo_UG>Jr+Jt$LO350fb?TzwTMrE%eDk$5zyZ2|(*5%Lvtx%NAWG@1Wjq;VcO z(v~x+s(0qVfpF#XtHiigYV(x&^XKcCNHQvX!KKwRj%h(jqRIh5wYdGMZG5?L$?nQ_ zhS~%j`zAOaLE;q`3fG?{-cSgF&EtN#=gEnR9V9}v_--uuo6|Oqq)dE<-dUTsE*Hwi z2hV$5)*G^UswU93prmP{st1<&cz{7{n*sekeBKX{Vz$droxR77tpSnYmpFXv$?q;e z!9ncBDA&f8_e!IOK+Q{u{tw&beD6k@yC3yg@u>OIhv~~E?t6Y3qejrbOzI{R1S3f3 zADkk>RX$-K57omHkTdu$OUCsv4AJZT)0WwZYRmJ9i7Nlx=%LR=TpCTvr9VI275{-M zr=9&*lELr<6&2815978c7`qLh9aH>_shv@$B4^<)jh;zn-Je=O(tvoAL z7)CHiRfjF!Zg~s-FS*?Pn5|nQa199Ev6lZ7%NDBo0sOy)y1E?}JfA*ehVWlAE0}sT z;9P8cafDHCU1pq^GOJIzjI|wsZ|O+IH(OW~sx4)4Lwe~Dql0@79a=3hYEpZYt(n^9 z052QeT-35xfC`a9=j=7qAlzrqzNXqK3s5VHT-gToZi4T~)DQg^ype>WvSgXqmb-c@ zHu6^*wraIT#KNxhs#B+qt()79^~2GucK>fbrSS5$vZNLqb=k=E?VyX%A+RVf2T3M# z%3J>9-Jq}`ifOpCt>)$82r<^zJR~?67(>O$$w|VLs1^Yzn9O8(#*md-1G+0~2?elB^0{az!%@Gh)nfGJ^JNT95*5?XgbM-PL4|fYYq75nEAL!M$ zC|SAfpfv*RK!vJ97Ej!_FW`NI_b3oK)t3Ex_0pq3Z~dv-c9ff@rlwTd5tiLyE(DQV zAYTRnYo1;D)y3l(piwP}b0qc#m<6S#rR@eAoO)f}GEbYmaU*cg|QvWm)A`@cU zR^Kjoo&#`_wMO)&)4V2 zk>@?|Z`<_t^5!n%YhlS`Jj^gwmt^L9|CCc{9ujcDEDN|(@!HXsD#>Cles+H$}K4n)X5^}+ zHWi#Vnmu*wg^o-*r%yRLI{d>mbiR?tP;DMa!Q$$w*ti^K-me1ZdUMD7#1~XA^7BP= z+WJTK;Qs<_V#NRTV(RfK_nnHqOFj=x{!g^KNge9_fJtTOy(3W$5Bn{xYKWMr4I3Rr zGmWwNBKtUx9s69RVg4@S)~mnIG=ngc%S7kt%LnY)KBka#{5R}>0&5KHdBm(8nSL*ZA>y}4l1f?fWj069kY@xKkIxHd92TyZTk)8@$e%*C6THSQup;M-F=I-p z=!&qN1X3zswwcl+d%YUvY&O$_Y!@G{4b4m@*U8%`p8s6UTxsv#c07tIBZrTckST&K z`ytSlNyVaP;m9OEm%hWHvNu&FoplE;i^8m!D!`L;tcxZ=(Xp83<}sH@2>01Xex_V%-`Z_fprxrz@_7|(rj1S(rRj*kS#~l!{cG1Ta(THA%`ygFZdHKa( zr`Or{znsbKKE@D_02wz~SjH$Q17Cv2xq~+9M5r`BlbX6kxl#TK{2x`Bu3i8x%o#Hm zmR1#utH8mK+%Rr!K*GORl~ph`fCT$TKVEs^{3mBr$`??d zk|$4Aq9u|5YCx**56mR3nD~7`#TbN$jC=E|4P)kkc84OCR5G93Czi3%So?qS)MQ)i(k9!FPwHC|KWx*S~5IGbRWpI>8l40Lv-$of{5NsQ>6jf@lhh(x+`|N@rMb>(&!Pymejv{{y~qrhz}RE31az zT2b9}C@SItDDDf&j$xVFP(>McZY3+%ZM$}L@-?U7Yrqq*>g%M~Lw1d#CNb#QbDZ(@ zIm~`Nzd!EzpS~&PER(RNocLUWa0jDtU?_WsF)L4jKov1(7%i9?7@1+up1Sw$-ILnW z(Aj{#!ok5|-65|__!^+@=rMgdQ>6wh4DTYzzqGLYX^BZl>fGmA5@iLn1m{maetZBa z!!>EI+^v1>tM(Rr4ou ziK;3+D1moFZ~w9L$TsPK*}w|Ei)YUOmh1dKmXC-?I?6oH5Nw|M_5c)WDuc9!ij0g5 zB{4vscn)^<1PoK~4 z(;0HBgI`Mx4^a$TtHhx{wf@AxkRF<|TZe+qefsop_7;S=x%=*R+LLkT7d$*ZrQW?$ zvmzN}u`AllWwYc1O={gJHcNcXAQ-y_8!iCr)I+DQH*ya1<|+1$cKF z5G*Og_bIh*T_qU>q%0rPs?Q%?$V5S4c!S*_QJ%GiF(w`t5G(i&w1Ky7-%itQl7Z&8 z{7=LOiP0hGz~-&foT{aJL}6keniZh=#+#oXNJ?6Z)~AL4ze9jSYT|_j z0C|D4{ZfzS5LrpjgXkuuY% z)vZ@A1Y*LimYDmtLHn>Cg?0JMmxg$yaF0?M8_=F7KI=I-eQ5*~rV@6nAc_rSx2=G2 zaum^(RcKf5IaDRJ@pbNcYBA4F9PV&fhstZ*BzikCj#SQJ#4$s*EPUsG|5UMxrooyi zM7@~u$>Q~W?bG{m4rY5;C&XB2BSp^^8gX7) z9f4FPI>k7w?(<{rxj~Jtce@f5G$b9^1mp zZAr`S)VbsU?IGvDWIXxYm6eqlbb)lpYp9pT_(W3#ml2s$Au}R-PO*MDV&7evNzo^f z69hn-xppoWxhQ>EpP)zay>Jm9geii&XOXWufM!L>cJ0lZHkASsgM4K(S`3Dp@ao~Y z=G+8VhpKb=ub8~$93S!M{+%!fMKH>|i0?&kD>4bvhYsC7GlkEnE(HQ>MtU*@^7dA= z-|3(%bsMm=((=Gf%K;zDZyjtzZ!rG#-B~qHiM&1O=PAKM>Ai+@Sq~kAcGS%6Wd@8Eh3r zJB1U0AIk5Dw~h0iPou{OGaUFdGbcwCe7jM_+nbG00R`CBO-1AALfTHas%&(hyOTd4nlRx zV)lcJ&sFx(cN%)M^&V> zmSS-oZYn7^;^>MO6j#YCQ_=arWl$D3_KzPw9y)1{Wjb6jG4SPJ5B~IMQLOQ|Z{Nn9 z`J*dy@eX%R(rPKrTDT|gRO^)M6t)R;BQ|^@7zs5=_ zh<3%*&8;wP?le%rvELV*Ze76}kdyiO?W462b>YMH9Qv|@sAB)f44ch zHKBgQm@$HA(hW)xvk;4Dfww5L5|Wb}0&z(5I%!Vapc;%${XOTzRy{jiS6$tPGK2^* zT#t4#Dmq#i9=8sUZ7^ZN1kS2GMSo37Qz`4wpnA;MkJW>A@@rY`DUk^D$7_Amb0cW^ z{bM51LSc(R?)q#q z-Tuq_LSe|tv2JZgV1aBYpHaqsS9bRM{wWX;U+M~ci1gT!2`3N^m^iUH(;G9=f*JJ5@?OxF;AF~4o*%R$33gKn>2{_JGCm@;Sl3hcj#O^?ObXU zMpq|JoTzzsW*%>p$Ox!kSC$#YNBXbXZU+t?6nGy3hx3RLU-OKr8KJ??88m);@#OTm zFlbDLtHR)@j19XSX31ED=jXhSpwN_IW?a7|O{D`pDjSUAfk{ZJ{PJZGdDfli?clYF zMAo=z(^5W3(b+{O?DV_-gxWJpdaY)y3zw8XJlBW18^`zdcdpmtThvj~)HP88Eu4P) zP!qPU8pJrLv~1ZDsCYM-V$RQ2f6X!TN%i?HdgpE0?XvC zh;mi3o{(M$`L3GW{p|l8k`JN9OO8&UV#^77B)1@Vmc;{tI<-KZ-*6t#BAYa-nCj_v zkI|I7$9!}09If_R5VtbGfCRo0Sef)r$*Um%NVV%gLv5Z?C{f3+R4|b8J)B<0dA1pA z+ubsH%e!%H%!m27?eyv1SnD{C8ufAU$nM`3AOQ)Lr8`+fPc25539=#5+lJe3DobNs zeL|1b%_e`IJgKZCSQ({Y`!!Y(v9Usrt7KnTco*Fl{GcQ9{M&uw!()is=-hws;VlH@ z?DQA|?T$M^@j&!{K79n0WRm5Z+c$46`MYw$kRcWGvkVD7_~$$(#LKZGrvTyc!B&DE zJw`(}7vy_ge0;aj&+lUmLNTm<<5kd;cmng7W3FEp+s=0Uwj^t0Yy(=uP*@u8Jd&&cVsf&OseV?o+DzIo@XM*sW`i4JpS*?l%< zBZ)d<0IWh5#Do+>OYYFOf|m8tlYWg29odhtVF>^fPvKHSqUq^ZlF_H8|E5iwND0bH zqE7Bv|8ECV=~K`=*YQ#L^sHYr#1TlQTEHLEu`84zZ~gkw4fK<^Kmzn&vkTFGH7&*w z1hGq#+A<5o{?c~r*v*k^9(L^1NdwF=sb)!Kdgp*g|KS3ZWD;KMh|eN4Cu`aa5XynT zRm=_EXO@i}Jz5MuI~W`1f`N)1y({cCS_Pf+r`!O9Cbw1iBP>!6^i^z9L$3-OL%4L! zYI6&f-Phaw8ioA&kA$`RXwsR3AHlHeUf#8f7lT%a8$$wDc4rBR_LgIZgFRSX>PV+1CRJIC5b@`WV|mT{Z2FiR>F3^qD=*Eq)|52A1@Bj{BICl?F3s#VW*9^Uxo(Pg}6o z$lj(%Z!oFt<-e?!eRC(KLwSg^Ticd?jUZor`TW*x+Y<22gObY?R^ukhhP9+@dL0>P z@NePcx*Djx1e(5x_1v1$LQgM(7RNtDJ7(yJv112WTPqP3R5*NU1A=EfO7AC>SjLkP ztp;)KnD^9d4rC@1Ob1LrHqeRKwRf)s5^G!A+EQmo_U|9KhQVeDeKQU4x9j_+#+Fo6 zMAEJ;_4_cxaNsl_pL!gkgR`s_qOdbeCqkG`Qr=N~yHiu^^xH&#%3Pqj?9)B!J2a4# zUcP)O1qxq_LcH?*(;;UUzN;mpZa`Rn+~s9sp;LTE!CELL(n`uhz&aYxh^$&`8b2%Vn_^w0{Eu-^N+^ls?n9s~N_HBXg}fbEZ$0LFk94nlt29!Ez9Ytsy9j*J>ET*7NyEvJW}4p=Fp7mbQE+a_g0RdAl}xM4 zl=U94pEW}i!dZ~0z`Fb1m#IW=xI=Ak7GoPB{5is?>vTKCT%N4p3eA;<*-bM&&)Gk}E-2o7U+bZ=`GyTFkH*VkN+bCF1$TtaBn(pJ{g`^FCG3M6K zsg|Ih&)&XU&faJFy_21r>5o24p+;#m()b#&y;D!hhYV@}Q_a)JJ*j=K~JTy zHBDSYoKB0Dw_ZJyVXCf)yHbe3w`NR?Luk#pjk;=%GGpdm6d7orx9sv7E9PdZ1nQrv z|I6R0#ib_#Q@Pi!Zbq~R+L$w^!K_($S}W=RB7g(8zgUDUh0qFZjbaB#Z!dgm7Rgd> zlf63f<*;!0HPGouJcFU_;pG*=0SN8&7IZHIH`{vu-baTh1O-Cr13Z`pXza+MNfN7R z4{1WqB_%*79_a8N6`3)TdhnzRcO+Cs<+1R!A9Ck z)ZUdLil_~+(`ly@;fJIo3lS?koKFTUSil6RPDoXF#bK5UwIP7Wk*@u<85jb+Mxzje z2;{BQ*rzQ{KCx}#;>Cs#|H02RL&Bd70HaCNfeB`e8Ma2Mwe-pH@$;uzf*6fi)>V#9*;4wXE14GbT5Unh7svOwXP@P3k5DK7RXl zEo$!%CmKYg@$9))5ey_dEp19AyQRkr9iDuuiA1k%|Jw;SA{S0hP7zLU(4$h&FeDgs zykAj_N;;K=+M=RT-wb=Znxm8oLan@a^60Y} zpd(~hduLNcOZe~NcDk_?P`%Uu-L7miug9f|TlYmQ>l8*t2`E|cj1|MULVN%LhPXoe zVb!3BPK{*}^#ygW5E;rtuJky|+@g+E0|%YGyLYc(ox~9NVvnW4)T|o5=ONM2^|Pu}&iwH@c>VfD+nuz3Mxw0dP8Y`jewwaAn@a!8qk}W4OPu*uhpOX05r_Y((z|zW8Oj=X; zL`0}t%y}3kd_x%K1<$h{Zibt}sw3eoM}4sjfCRo}=th@1)xb{SZ*Om3g)%}h63(L$ zuE#gM35Ba>SxcBMq5vy%S$%zd%_se4OrP%l_5D-V$%+w#WIO5_I)Qu>s{!B+LR;zD zwQHD-U4SFgd?v2hYzsVo`ZUKnaRZ?bCK@e>CT!6JGHv&hxi~+{jT-i3kW7V%nBFt5 zMR>0u!R{NnmFdGnsV*7lW)v51^;o)+bnOY4?Yqw5V>kY)h}lH+iyyrDC;$N|pd98l zvR8U_gP2)9Jj0ty2+Y~>?LUu|e_F*HJ<487Q#xWsu9{VI5J+dz4XP!tPm-z-RJ~sT z4M&APWZ4d27NP)@yj)p(6VITxMpem}ve^On^-%)g4Ul*Y$ z2euL{3X#)Jr?yer^~848a*$!HDUuc|8OuvCv+ns@c;>Cg{L71V)Z_OEt+{{y*1XmA z+dptrWZ^womL|rJx51pC({ZDOFNpZ|lW%wBN|eXC3;2SYL&1~wYW9P!Hng`r=f7+$ zO0pl{KAL>UGHo_E(jTviGQSj9>gwwovV|bS>zg;dxP?uyLtaZK{qAj_h|!X_fPIgh zu>>(Jfy^hhhGeIU?M>M3qY2PcaohmC0!{Z|MwXRkg-ezW`^F_UWs~o~p+nsp8oKBL zD(bS60a_xm|7wOd)DKk{8le z&9xWiokdrQ$%F--|I=v>r- z1B;&IZvRgU(${RMO{Tx4^rI==7V|Lpq$I*1&%UkLIv@5r7Z=SWV z9!)v;XYivl+A#xIX~~ihz-|I$HM54Mrko8Tm;aYHZZcRT!x%S@5c)3J?*gpMo-gLG zs1?CzNK>!73JtL4N(XlaKD}gwLqAOhwPeUso0BWUS$9v*3)T-kaVnM2KtGyo^2C}j zBDBZl)d%e%v9Zehw;|UqAXeG&=#OGgql5JLFvc=|Frq?k9dc% zW1CSv2S@GLu_GGYxUe6qK%CVrPBDF`Z4_(E^gB4(&BY~HOW2DmRK4Mhu0uwc`My;vJmgFC?zn;|eB zd_>KsZGSF0-wmQZ@(f-E3K1Y>#37+V-*5c3T5nrvx^!bpv%~6@2FN4 z?5_i`1G5jyc=)iDVNCgYq53v{GH(97d8w)4R$>&TO~$A22($UPic{i+;I`n=NM7kV zm~W+@knsuqpC{AkfE54vpqk129RiNS?q}WEtfbkKxwY(v=Qt@;c@g+&E~X8W5sAju z6|7!yB;R4t{O(*t;gPVt6>7HvB@~9{FBh)Td-&i%oRpHm~3FD9PAO})jf9XSkJ{DJ1oa&MQ~;AS4`16unbSCBuVnDfJ9Np%VG(r z+U+sW30&8CC9P3gW4HPc_|ukNbB$ybgAkxL;A0(0OloSS9AO!kU6@l;WeNvU3J1)< z44@To^=~hBYg4~Rj~=W^tO8ZN{8N8xzaZFqZM`^$)!c3Y358w@300C9 znV@raTBMz!qJ4WWP#5pO>b^Scaf>Gn*GsswuxJWK z01$PI-d@^j=DkUN@jDo5ATYZc#|{6iC58z#!cmu-pOf<#pn!>^M~Bv#@OuT$<3pIB z9l^lV<#FKf8_XV$xTfjI&IvykcGM6^g-j;1->aR%6H7j1O7aqEiLr{smruH%U;Yz- zrDLeC(M%to$lvrEftczISaEEmo2#q*e@f>3~L8N1eI!$ z9;wGnqW*Gy&eYy|$dE{nhT7VNOCB4K`>zyqd)IGHQd8d_T-HwW(kXui2R_6VrmNm@K>tS1?KO1dNhdsqlZGI|=+wmvwuRP{qll?6q}9BST1`k_GsZ1DR!x zI6S7S&9%`7j3OQ^4Jp3~sY#e9vOdKONw;ocb11wf2r$O=4{SvvE6{`OqE)1F4_aS{ zwrY&fwfgkwlQw4p7QPZQ%McC_2Ihku=s$G>X=FKG1B~1_z_8;jdX4k0ceifeE(ZeY zNZqRbpWpO7cBy+!P1Scltv;&;HXLs|!e)f&h{5d|x3shih;+RXcV*)hm-gnH{T#WAnW|+w8hB$p=!zV33P}XF(loWlWV?h7CjF* zY)zGw8b5B)3knZKz->a}Nl0koW90tE9XRS6(s=Vh$HWe1=8eRNoHtJ=bly{V0Q;6@ z9}5SGDj`&d%Rje>zn-`j!LvG2XP2g_uI9{pe;$-AVB9yhS_jq8bMFBFr~o7r4Y9P= zbHJ17Bt&Giocd~Nx7y90K6#SJ1@s}4^q%`H>S18bvQOoOA?^x*li{?EV*WA;zI82| zgT-tY#QHQ_9jp4lEr(voBE6s`Y}D^kkIa5oXFW>N5V_%6zJC{Q^qZCJT> zDfq7V2IbsC$iymAw?iK7R}UWPxvJ90rWIa*o1{-KvnIQ?h@Xn+T~lz3^jwyklLIms zy~=3HX zQF=Df9_tb@jU~hf6*-v-TO(HgY*0E_vH;`=vH`6R=uB^;b5j z_2ALiwXoSQzwSa+>q!eAtKw|kNJUAhgU%o2{NvZpt}To}Cmwk%o|O#Z7zJ;?`g9vw zvSTML%JF=?R^MEt<-54=ibF;8=)gz?MUuDZeB*}HGw zAIik)R<==m@xu=Ql6E=%sVsk7ed>Y5UI_A|ZQr-lk zMy>a#t{!P;w-R70NnA?_OvkvY(?;jjU|~7oXGOb9rx-u;be+%~C=~w$y42~wAQ*N= zI7Rhmy_wH5mc)XH>B8#Lu^ea?xL2-G6C6m(BbN1W@BoCk=A6VjKaQ4kz?R)sR4XrqtDbSJ+{ zd+KfZG^o@Mj9>abg89IV+p3=YJEGonfSd2ySve^5+7xPfk+o8;tya#NlKttEl-{$7 zkLMV52YPa$=^O>ig{i+2R@AVeH9IUy4u4nM>3rh z@AiK0FSd=l{&@J^K$Mz!m>&u1aBzlD`W5m&WmhO6pw~0%UO^W`bWyL-26YnNU{vp5 zba?z|KslrjmHEpvWhLtv|Ih!NQ6+2BcVE8TrJw6D|K+I2Kl>wWMCx#%fV9|H!3EkF z6XTKcxzD`vj8au!sBkL*gye-setL(iZ#i;?W1x)IZIL>h1bq`F<1q=_&(fOPvqShK zOEsv8IQ@uaH>|HlVTu#QN1!fT%*hEgQ93s5{=WZk0kDGi=csJ6TGrd<2b5m7t$mGI z_lUeQl62|2{94zIU2`V5JJJm{9sgJ0sbZ@n<)>fFGY}Rt?3G-6^0oLkrTnGWs@3e# zdPVo|fB5D(r5Kj*-$oyL=tqZc*8x783=4#fox>ktOl>|`2C>qGL-PrbghP|h>$m;=ZbZT0F|JEX8g{a=8yKTo4tJecw(1n(lf+RP?MWo556qEoSM3R z?$Oes%MB)d%N}Dq$*KLpd9Pk6p+R4V9t)H^7}8|82&l7hH$M9V*!%#VL4=Xbbl04^ zP_!Apj_{+l=gAr?m(OmWFum)3fE$tJBUL#-sX)|EO*SO!A_GXL$bJ%H7G+ca%W5F9 z?O&IS-+v{od(WOCY%lZKm`@P`@p%gQv)iF=W0x#B1&zUxC=UD5Mo%v!A<7gFIez`o zWX~J3IBx7UXad4`Q%-M@aiN9H$3a33DyFXDfE{BQydY>u(j%(GchEMo-!#)kx$e|Xvw z7T`e8uMzG=&|gPLjTMqE2GHnj+!lKyzf;aNYS!FvaP_Z86Q5k!U3xMYQt1p4pyL8> zfT5toJnI<~QritnhHDc6y$Guw_3DhLt#eI&?2i7lEBEj3YGL&h(>2UJOl{mDU|f zXZ3{g(;L*3ew0*(SqURBN-^N${uHL5A)OUvSjWd~7gjdIQXxT2L?&uTqBB)Qe zCkH>ffBNJlZKeq_?fzsQlQAO+lN0}X%*O0I{Z_|w)$J^qPI7r?@BQ?WiNrqEU?aVo z4*E+AKYS45zL=0}c(`4I>NRRaBA1P1#KaBVgKnp9mJm>8j1j`@5lvDykm0+YbEAl- z>wIr{aNOw=WHk>nGh33e80DGMJNlg7V7$W8-5kWA#eD);FBv{N44;aV=;J;r{-;Mz zo@7Cc`oP@lGE=p+N*ob6gE+$kB#b#>oDbLb5FJ(4P#j0XnTTjU1chYEBxTVcUC0;_ z+cq_$^3crs6KFjT)$Qk~?i)wHNyT1?5FDP_k>obD9v7Km_(!Bx?Zon zfvzu{9^v!{>>jjgx+BCo72NwT#%2-Aoi_74B~c6|3G&)qfL}7WRYxQ7IT+FC*x3)7 zdL^Z%4#j;H1*fN+4*G<#u;2(aQ9&J+y z=hlHA=Bg&tU$kfM-X9KFh8oCFLbo%P`l^g;$Zw%N`oobeSCgB29iZwe#>8xEY*SAY zNYlIc_-UkjQz_c?*PL;lv_ytTebcnZ5vxK%(1X}*j{x1nnoK;qN!WJ-(CJ*kMnKSg zJcVlI0c8ACv^RI%x&u+J&Dko33FAASPfvFS5#>~Gd8P}OTd$tnX;Rb8LojGz^uVKT9 z5h68jd$cztqH(S6CmC>VwSWRV!~DGC}qzEOxWOdr_uZ;vitKW^#EeS<1ZV>O~^3tRKX4ghmZ&ErfXZK zwu0K!u2n1G*s<@Q+;W3D=2xEk3aOgG=_YB<@87@w zIyd*hnVu=VR-RRQ+{a$@1l^G#xM|pfw=~^N1DB-Jv_o_zN!U<=FM{>q5EpTV?n9ay zqThDtRriDNCL})~?v-1v9o$pKz@I64J@6p1^a*QaJSQ$`I9DpJ!e!Qxj zh*0xnN(L{+x;LaKy$1^?`CROzF{b6CXcS6+^7hWmiSTP+8?+k)Y$_%c1)uh5qp~6M z;ysQz6+%P6TvqqfKHP#}m56(HV)IyL_eS!FyFd+V=F&l?)`_$+Qx#1o+}Z*OZyJ zby)Q5H%X;uT6xmH01^XNc?dg`?x104@B?5xL_%=DfB{?Wjkl2^=wOuq4>*;bsIku9sR zasU4Ph(Xdmoqf5+zVHMlvx|wA(eN^~M@&8wN<%QW zPXY394VxLfGIqF92Cf@}093D5O*y@8{8(@)cBzt6Bj^bFOi!U02-YMq z6DaEr^QEIn-LK)4Id-ESniDvRH1Y7Lagw_YXW|ye<55=Dz_Yb113`GeptKq`l!DhY zY(vT)e_W^3w2>1li95!o|I9RkXnr-W`;dJMqJYFkZ&&|UUByqJ-;7}CP&)c%one59 z$jLp!BAtNHN%z@7*sAA$G`qlmw{IVRp$+BPGG38hCEdeSpw| zP{M>tnnvvR7H4&Dt_4n2#!<)+=YW8Kb7^N8XGF8laz==?^)h)MuDa}%zPI2}&H)Bo zK94GW{NZ9gs>lZjh_p*eX$A`eN7ud|^9tk%XbN2F(X(f*V{W&jG$bSQugq#LEE+!{+xf$>-d#ywxOqI;km+BmWwhT zJfMrom|9o_o560-pc`b$;|`k@V3k<_o@xHwAzXxxK9|+h#3bt6v=HZ=-5=zfJ8Z;{b5f%vj?Nqc82=P7?%@(mh2n0*%LWy$ z+;}2*IW?9)iF4XtEny#T+_Y&A{_V`&i5?*8q>O)rF16okw)(pnRlCakPuA|i(%uP6 zzQ@`y(yb>|UhkTxviqWiA&_ftX4`0cmRjAOkKyi(vCYq09`D)R8QoGziMJ@??I?hZ z5X=QGh@DQU8$CnQ$Ebh2p!i74*;5(h^KL%Cl1SY?wTdchoiM)2O4d#rR$GOdjzR* zow$;L?k}B}l|{2aH0LJW_Ub;Q<5v6CPiisfVrH?!yr#yNrWtVZF<}urt+@B&fZYWA z;#6ssw6d^>d!?I_ni~D8Nz#Q2wzT%L$pZcS>a4D(7H|F00#6d>G9?VZOdD0@zyp@{DGw)*hNVOs_RJJ0=9RHPLS9CI` z*r8fBSjdTG#U~`vBB)O}Gq$Mv_egNO-RxSyb5^-G z1a31S1X{t9$zQ#HTE&j>-FkcaP2&7KU=*hV>qIXKd<_%-s6!ll5y@&LEPUF;(7@oZ zlUn14&!0!40JQ1+w0P#eVaR2;>4nLiHA_mRai%p`E?C;C2`h(+G}|EX71sLcG&g(_6e6j#R($zHA5 ze2Jyb!U-gUzW)9O40)WL{=&QB=2aP+nf^gC;`a{mg1vJ_8Xy~C8Fl*IFGv!MPhGZW zNN@)MecOx=lhBN4WwVVlnaJWt8%NHg&2&Yn2^s3psZ%2X`TH%mY7D2oD~%`{CofNc zM3_arIowizyrGk5c=A!$U#wN?E6mN$83 z;`P2xbZADH`24;ZJw(z&0))h&Bbsm4vvHZH7gbLEYa~t+V&#B^G z%Kwf}l59z=HaM+%W(Nol5?b(7R}ugFR`0LLUuCbu0XVV!(CoBdW|G7JOWx!;9Walg zEU?)o_Z+ENRN@xQ*ka`Y<3KiD*-N!L_QZ)zHfo2s_$;#RYxwtr zW_!_EZ`-@~SR%rs9?rCRF$$r#UwswkqQAnKhSAlSpDmDNGOmZ99LQZ7N#- z;8cVq`z8f1-IO>fQxbAK_9iDU@E?g`aGmPS4{P$1BM5kL1=c#HLTzf$xbeX9nR|+q z)%a)$ArtRQ5AuVG!&q@+U7<=(qYX)hW7z8S7i9@>JxzjA;}Kf6{8jdfUjdJ4H`c@H zR4`LaC71gEzrk!;ZqchXCeR`m1nTemI^hd{W}`<&uH0^8!oZrl1S+M13fKZBO# zfvkcwR&2kZI)J8E366b_Uit`**6&bpN9BKycNNijrfWSVrr?pawDwox+|^kCt)9LH z73(m*4_t5=olfN3{{F~`6Zc3uLeTjfQ|xBG4U}sN#RT169<#YWs1_v0F~({|)^I54 zN3uJW{+)S&CI1V7%^pEreABjGfM0thyawEq111d``u!rFrbPuqoIgadyMB%F)k!sv z0#S9D@s}e#imd`lTQIAa`IZE^hIPyD;ZYJc$?xiHSd==bRye@Y^U2Ex0G)z~BFwVd`bQ=oAOp~^v)b*) z#2&W#fHL1Mq7b)^I!*zKWxtQ7<0_;N_)nXSCA1GmBK|mNOUFK}OMj;=L zVR>nK3}*UDBn-n}W0h0IV2OVrI@hZkUz@(V;YfL2Wl&*%9WP;k5Ye`%I6RM%0E~`| zKfA(Z)-ke@RpkTrDlT7fD}be)3Ovj(wHE&R9k*Tjg=z9c{P+ZgQ#H3O6-#5R#-_P} zF%ghtlZ_YYt7NkWWlR$$%WaPh1+Sm^q$-!TcG_!INX>Q+1zjg^MG3k9+mMpTgKawH3lsd0bOJu9^N+$SzK++6n_L2rXt%{ZRiSF z1@JwMYQD4s&DDkf4gzPb-e$wxuZUSUo1m2nuo+HSOH#(eI+c4^S;gLH128}s3KkBu)U{i0P6PD~rkxDhYSy39 zoxm!r)xae$2%shPuqb^$q|BTaIum!(OMYIz+Ux;j8PvZ`^3wb`HZtv*T<$8!z+2{G z3iKa7?r9fD+hDEN%YimI`b1#ungrP*qNTS=^CoytU(59}=LZ#+RI;O5nM3HLZOy2{ z&(CjTqPSmrE}R*CtT70JN~Ne$;~)1vU7F2~jCL5x)F2$lmb{CN9LHOVNMq8x@)7K2@49nF~88AY6um{E21 z$YxBgJS|huVJZwPb^=Q2{Nwq6b6)wd2}t7|2!*xk)zcG*>GthWyK`Wh;2CWR4pN!% z6Gi><>A_?7uVT}j3=}GtMZvd00lARAP_5AT+0DftKXF2{{1#Pn9>KHt6T9no`W{f% zXzQ@`s*3$vu?me3yIs_N7ZqC*l9`g3KXq$Voxu0}LO;#RQW);zq{u`=CM~CF<`CWg zfOi)ho$*9-ML!!{U&44tIJAW`iFBO7)GAOZd2|>alcNWqv2?+((G(y3N^zI6+g~!O z<~i`?)*eD^ngUZ`bty7YBLIL&b9uY|QiGw*f=S1;+-+d{(V|s4`^^;|*4VgZV___e zM9Wt!uotRTf=>y0%@Y-9rd*`^jyQC>)whK%zb6 zB#M94DlB5f^4wAZXk<|rH1gJ`T0V@TKbb=n>w3T2G7c<^d3wKIUps+} z0FAg|58@b`Co`-*mX|C+B?Ma_bLBRKTOv-I*?B0sDan**I*tTQAqX7SWAWR4DBMcj zX`_!nwJf}@5vh)L6O}%Yfz+HyOfJq-+lv*vKflHA)X6|(2U50lU7@B3f6PzNF25bu zQ7f~<>DMW3p-8p-WCbsiJ_u8#q321;){ca=>R9qGUWe5X5V#3ctRCt;&Z~GnCt5?i zwMM144q4S`jrBLpdN~nz-lU2&+Gy<*7#O2zDmZ?jV8_DyLx}|ZzKQRRh}ST9mGj7> zdNVX?gvCxiwPS=r5in<0AyZQtIG7`@_ZQZ^C>7#B23OjAIY3y^ObDy5z44%dT=j2lD{B^=(_V8i46E3UE1sE>U7aLcMCL zLYL0EUDz42%~-ycY>VtvL2z)Zk~nfrKsNZ!&R?D@p3q`Ix%M}gO`qPbMFpf*aampU z_$cI<^t|X@^ebW{9GahG`@%6-<vo@?CpXT#fG148tCiVIM#58y(W|ADyP)XvHTTpd9G zF-leT8*?!coR^VTYA8(yhtC~V(@k%OLRM1*@>r*zmDyi1f zr#9s~U)4Yi7d`j2OPe-m5YA(1ZDeFkL&~qmjvb5WB^6Mp9Y~{%)qXjNFbjDESG96? zUvYtouAw0zs(RVdTyPzs4tYx9KMh%0aR*9kbd09ZoebBc`-HozdCb$WTv|5Ol%as1 zQs@^?587kE!B{3jK7Y}~-{k8{(G{0aaj9Js$o~^P9wAu&E+|KCZ zTYUO6K8Z$M&2y#WyKc){=T+1X^J9QxR6e)=WG^glGmF$vSX^Y)!j2=mpLJ~!-FG9Q zf2LObu2+19Lb=Dv+Ip(^c-$2Y@cXgl_3SjZT$(6jXQj3UD*EHBSwoB!9nv6gCT0t(3c_4FLv$x!IN{VM2|{$hQ@?6r5A- z?>^nm$1ldOgr}nZz{?2|9C2!Y)-A+-{Ljqt`1nfcxZ%QXdUB)~ik9b}=%Y>Vu#z<7 z()RAda96u$YpcvI-oHPNc`H)1J4!c(ly!Kbt+w<;>Q-`Nq4yzcP(@AAb^-N7f;S&M z-}vcWrvY7q(CJr%7jZF2#bOV3ze;CK91-f{(an&GJft`&!sKS2Ac0+0@Svtl6{iwU z;dM}in+&t0rgqq=;;JkDZv3ze2R{G=Oc@jwOlx!Voq8Cot$iwu`~geK4AKHa$zI5a zvcG1y+$H3=S${~c7KELUZ}-#A7)l$9OuQBn6xA+RQ++y?rb4Cc{qC0Q;-|ZQ*3!|j z(Nib~Z|}7zu%0i1M8hflwW{!L3k_Sg%v_k?6!93T+p*fMJ9Z?%8AnzlfE%?q_}SUV zS)uJ2t+SLrcw$egTE zb2tGR)}P(#sj1V>wf$Yol zIA>IP4Dcx7Q}ai~i{;Nd9r*Msg79jA1j_!`QPzLPy;W(d=3eTe;gwte1OQE zm@6Lg$F9meV+7d4I1JSY3YU418J{9uaD$S$U83Zc3nBKmvXtg># zUKb~IeTEu~Uxu!V-0iw9s*}r3AGx0Qt=)Ik(}7Zk)LJ-!*ywd$o?*rLjjaiqF&v{L zAl4TzODS$`>k!xt&C%ZAIqe9uloLPY$;PgP4Xrya*YC8|ojI=|MqVR#|RJOd8+u)y|Heh|Ffsdq)Exs>M6hF zJQ8AfFs)5!k^()Jz3#50CLKw&&P=DS1fpNOLX=$E!|d!sq=~og-tv6=bk~!eo`ZQv z|G0I9YEDXGVt@3)C=fW(IV#YPP!zaIK&)$ophsa%x|p1308fV|_#5~!?B*~aQz0(# zRE?@DvR6=dmWp7rX5$Kua$V24JH})DN;m5P;b(CS|6~u(M+3Il;382ZswEuN__v}gXX#B-jHuAfq}Bm5vQLe88E~wK?}Zk#W#houn#Bw0l;g9%PHJD34lw6 znnE@FaLX^xTt2f%yz7-%jvoCB9R{Bcq^}@sF>CXsFpi4u}R!Yze=D zcpKRqL{Qmh7Kj7e}FL*)vE5V#oxg}JsdGq@9Re)s1 zJPr7mq|p10B(5uM3BSM(Z%bOn(tP=97fQ4-l>vdn*F6Z|JkEh&Zi_vM_l?SmRoO56 zeMV@?1c6<@&jzSyL!(Cpl4HrgN>==qgqZf>fy|7#+BWJ`C|rWSerzVpW}yg@p$Q`p zz`IRiWfER~F+tv>G|&0`UsGI5vz$vK0pIB(GFvY@W~=uXmmFVAvOJ8s3e}%Uom|nH zi79z^EbhXO8K&*Zf3P$dmo2n2s$y7e$&sNnZ>Qz@5DeNWxJHp#KHj8|2Y%zk{Ao81 z{xA~08Gz$IUg77+T8}^3d5ua>XWB3Kch4q29<-y!aQIaAoZZ8VUtF$OhqB)daY7Te zthWq-*OX$Xmiqc=?6<=%cFHY(^K5X5!NMZU&mW0ry^aoUdq&d=`v{yXuQ)$_8tvMG zi9q~y8Sd&?l$(5ma8-1EL76sg-rP_u!WXX)d!+hq5=3mIrH98J^ggr8wo4z9ElOJ8 zqm9U>g6dQn{IXwFwu48(Iz*6;Z7PiJK>rJV*S^Qc_f>gjfV&ZN?4xEX)DGocR?UsX_Uq2AFsVE@kyl}rq03kPx2+3|l0 z^0;RmqXYVZLAzWf={f#c(o=RdiAI#90Z#y>UzNJ%jC0+8D5vIHuglM#<-H9WcSKcd zm~xk{Tg$(-CbSu-Y+~BLWOsCI{u@&Z4~KL&&txzZ z8GPNov0RRgV~-tck!wQd%@0pFYiUlz-krWY(5L8DEirG-c&TW7-7SLcCAoOmE}N<;f|^!lZF)nwFi=ii0d_7v9~W2j$ix-oqvXj0 zd9V0YL&uJ7dgI288WjeSe{>9zIpQymrj=x6P1^2meRsi~8PU`h0=qrw-*Bwfq#g4| zKo3d0x=)8HvFR#x7ZTs~cHwoa*!=;oq}3mzD&3+g0c^yC-)+FSEI0RHa;F4*cq3++ zY^s2hKi(5D!a2^C=DmCOO5b?ip57HTDGk_5E+~f2zIz#j!bV&tur63z3{W!D*K#$^W^gHBHC{XTH?NF86s@x z%=D*m+D)Nw8~6zwQyBZA&g=wXmHBE5`=$$~xt8}GZCr7^<$qgll}*=p)+y5ti1i?G z+$iSBo@>u&S51tquB)a5Af=y1`q|q+YugGj44jY;t=pC8ut9>E#@u!=3-}5vtmq zC`tk}xjlUI{uY(WCGb>`57?Pg+@0Q(Xv)t{#K?{#M>cM!P!9d9j6{UGfu$;wZ= zeqBb5Gj6Ll&aA=S?Q`r{nMMW*<@(P*J1?wxvGKU2c{d8)U-4?j);65~8=OD$iC1>c zu>HpeQxx2_tA!+)nv-PG5hGxXwQ8@w@S}(fuFz9M;_xS>72DPDO|$C0u2B08-M_6o z=pp++UmKw!`5w;ps2C$SJJadTHn_Yfx#>cyT}p3CsMk2MC~S3Hr9PgHoBM;5q4+eW*cDukLrORFIu;Za+QQhK(a^!J zKAU%nx|nv2pzV|(lcDN=^JVDGEvwX9R36%Rj|&rHQW5SXvsq(r)TzUyE#^xXA8?A+ zcZdjD{m%u$Tp9a(qJJnd+ibK;1WOZh=6=<-)tYB9JQ7#3KWy5JPyC6>cL`8@wp<2B ze)}!yXxg~Oq%-T#r3tk#*>)*Owkx!|SN}8{lpb0%YWeA{?E1Ia=u6U=ymivfMs(Ut zJjJ0yne1#ra3?yC5o7ngd2>X+nxB55;tN%^&u+kQ?{B|#<$-I`KkbkGHlZ257^!2M zhcT1bRGY^x7}dFJSKWV}1>VcR{xdr@ian9LA-Ko@5|S3>PZ8A;g(s0~O)rf3>&~4_ zpdWus$I36lqOxmJ>1%wV3MuTy;YDHBCKKelqM0Ff8{}iu+jsAh;!2Bi&OdomnwJBo zdrOb`LQrpsL|_K=#LhMq_gDG1v03F}k57QF*9_?**OI${yQM$GnbQvUf3WiLm8AABM^|u#Hyj>aLX^_Mcx_x2@b||M^{o zrDo+Z{g-d6th_7t*}r_>r(Wfv{mZvCKRYo0@@<2@l~3T-bm4 zww}g6Qj;%##!~&|Jcx(Rpti&u{{1ilMPFY;adi$m~=e}{=J6EJ?MY<^FUOc&d~PtoeN1&?#~uQErgPM z{mIw`x7|`vwznwBU6&a&Y_JbkKTx4EHL46)E3WR--v*DN*`*?;1%tsG;gDmQ3)Uj- zoH*$Fds+?0%$HyIvML(%!@RcD?J&PWHALr9EJGLd6Li+{^P5rMde3KZkfaK~8 z$0)nzAAhvzhm4(`**?r-xL93PZTFi0xW!NZ&`rCvAlV|NO@NLD14mDYJOdMDM|D&f z=nW@BO8WGJ|M5kWOW+Z%@Rf6q#T6fHmy`cTrz;7Rf7F?6z(V##F2M*Fqz|jzbTnv;=NHUmG$7St`{7YHrbcS zs9?LaH2CV1fTxy`-Hsn=+3IUQ*LL&8;dggnMgR9jcHPF>cW^qaXSUV0c=1u2=V!q3V&*Ays(*Et0J$`npN{#tf0YgX z6@OzH(N?QAQ0C-%nIY(HhGuWO_(<|nZHsRK3w|`aI{JQCyp`9w#T!1sqyId`%GcQX z#?lkq$A4wtUG+#KQB{248=I2@K^Yan{fX(9z>DP)wv=pG9H6<^tNvmK9*xja@)<@!}aH{8`V>|_;1 zvej3%TDLZ=@ZU~fJ2C@?w|)?BZ&Y7)q-K}c8v73M8&Hdx6Gj}+F zu5?Z@o81M?a)!0&pg-hJ4Xhh|>j;D*8fmoQ{o`QvYZ%V-*W1&7jJYkKHX{LT!X0YV zwm06W`Q3pa9Rr2dhX1}$MHT|DM|l;u#nEpNDBP7kX4o*Z5^$MY&OdoF=QuMbrwE8p z77KKpFNxc$lvvKfhbtC$heQd zEe*$cOz69Pc;&O!iQYVJaqIhwo-|#lGFt%1k8mOq!!|HfG%3WH)iQQvFpN6_HM*Yw zQMm<6^&SWSpjOl6#L1JclxkC9<2XF=%Svgs8;LnTA`-j zYW4T6ZD*=n({4hm^O{wPO189)n>l#+`F8e|H>Yx*7<7NIeQupXd8f=h6+R6`){OwR_{)nTNjC+3;VmM!4x? z>GczUI3lgv%Olr9fjH0A;NFd^!c54ZN8F!Q5+@ynIz*h)W~bwzjrONs)w*iDyIJL4 z`fRc0G6bR#`$w}{@oQHo$EH?O*!fe6Zx7zPIp{Abf$ZM9w@#yiF%Ta7;majard+KSFdo8PhiipBsY`Te zq|l5Hu&Vt0imyWcfVJyG17GbGnVWX(n1K;^wOZOCOp(}kIsT63V;4vW>jMKrH55U$ zN^_f+Y9NO>&tWQ)zEcCN{+_!Oo`3Gj=0$qi?VR-Lstkmuzx94PiNk>liU37}fB8`P z@Z`NPrQ{I1oGhW@;R>gX@`~m0cam{Xr=Du5Z>b3fD8@b~3X07ybrtH$%H3~iCOzjc z@bzjLFNe~s0kK!pNC3s%jl8AD~Snl+XdNa;vEsD#PuXSv%>Xj&>=3 zsPQg($A$7#@mjlx^vQ4OH_%HakKt09W!2{HV#rN&dd!;)Nv2j+y$k$K6DlD-fX|yR z6ifYogPNXN8}EuY`SR~GCn)tNN)5@yc>pXrnNrt}EjUu*wpe2Dx;E9iEhp_5IWZs7 zX8Uir_I>{a`7C%Y`wx%LD%D-#;PV-Gh*J;2)!zhN8*C^@9?>Mh!xv=Y!P^GZ4FI zkkC+C8ds&AdK&6i*LKyMNV=%p7R-Wlp(_u&&u5GI**d(s!K~2UQk>O{*(nW!(3MFV zl#gei6fFYTwNx%j2u)_kqhO_5ZEQ2Vv(A6n`@Cy`FJ$Tw88_cjPMQLp#cg!?{lm6i zrKYTl+-=IkGc4)el&7myDh`D0E|AD0s{Y%11c zs%&8j=7?R!mb8n`!fcL^L6u>gm0?m>gPq>9McRi4-+AdE>*wuU{Cs3?X-RsiA57Za z-?kVsXwVu;j032uW!AJP=+g@W8cjnDMd1G80$|vp9NOqxHKo4e0VmVThaV2?z9f7o;?_p&t68CSe6F(RY3v zzu}L%gB4Wn47TTd8j>E-Gu(czr+5_2y(|9R6aRftEYH9q-R7PzqKN|oCdxJxKnVWh z+1=wZk#Cm3r#sEDqhD0jRI@w5ZJQn23;T9*JB5L_r{kxs_4yaC441sSJ;}ED<-l4z zYl*DBsY-`H0V%Hz3~{^B#i)xIFb1YMEECPH@eP$IqMIEJMp}+B8`qq^vVjMdm4Y8V z+UR>!v|E1XIn`hGW?Q}N(FmaXwH>KWYtEdA zQCUH)hq<6)^$Wd@5x?Ozwsf4(*I_D}lWTIh^jqWGsZ3h8Y#C<%Qd-4-sjX1|@xSfy zKBUuYgbs53!1ri(Nl3VcC2$;gGv^2!(-d8weF*6UsGZp@qJj~{B2YL?O>cSKO2_s2 zimFT|GmXkN;to1vyALjcXQf+{i+uNZ+bpB-?3U|#X~M15>=-{%dGjZO4~szW-Tv*; zl#j=nkEFXB~*S8=_FTGS5JlZf85Ym(=2{7cXAyFx3M5 zjgu6$9Nv;0Rb+1oNFU5cTde_Iv|4;<&@aE}LJ&9ZR2VTbYUiFRmQ{xTtNl@4rFkHU zOk~62HFQnZ)>V~WlQ~Zx-ab>0^1SV~S;m`WZ2DoQ4^kbi++GUtp-zWb%lc{x#_Qa%r8=&=?xw z5Wdgxg2liu%;W5$!9AWCv_Kj{n6wW02p6tJiH_kkBd`8s0Q;m9#NdYXKZp}wpfy!$N!h+G*F7niA)9?J&o*qBBd@O%` z)E(Bshgq_#2Ly=)A~gwXVz*=14?m)n4#Y%L>&n0}F;p!@lDAP}+Y&b{@@ zyFSb|;vPw`w>P65mrCma?GxKxO=F5(+hrfF1zY2>JJ+W>)Q$4Edeow4;hU|oX>#Qp zVn?HQj5BJ)YW}HI*yvSwje?qVtN78o*^xJEEG2qKgS}eJr$asN&nHKxTr*<-YS3lv z%O2`Z>vbqLpE=+6_MvfCqv_8y95d+A@?6$}r+>DPtEgSkgZl9%RAyOpka)8O2B)&y zAG@oGd-L=5CJT)%`Hk;Kwws0#>Hv@wC#{A(uFdoPl*42KQ)^?x|8S` z4V;X5QIEQPW~t}dDh4)e7T3Sym*?U>H$GpANchN3YIuh<7#rMA2}(X{6Rf662;gO?p3v@s><^`M2R{<3@0q7~fJ(+uMDtj+#>C0)UNb>Ge?pk5=MKq$X&n@hUCp1|AY|zaJs_+HZ39jX6Gt=xL~{n(qZ5@J#IQ8YGMNjd{Yz)3uUg2QFU2 z_B1^6W=Br9c?!b>iLp7TOdR^)M6=&1dT0ghafm6wx+bz)*PgxyZC0oAWTkY^C0TJLZCf8Ly%BJZ+E%L>&JbnSa*(@Vf5?L_w3c9AqQ z4$S*-a^ACyE=v`9>L0ziO7k&8l-F`RHeNWr!!!GMD%0X*OI%0JQtUkT*P9+5`k}me zEKNMqoquXq<<$+5w0CiBU4 z(U8}O#NWqmW3Qqwwwc~2L=fr``VaA-U#>8z_1^8Ty_O9o-*P2z&;7ICcZP1WDm?B6 z4tg{q4Wdu{V7@e^jZsyLbdFmVF|M@xlkH(Xihye449jd*w+ZydH9-xaWOHyD(Q3Dx zQ*SZ|KG%qE7=2{21_BmC>Hd>J)dukmUlCJU;Tv&=Lv$2Dqu8Uldo79uHQTYp!fP3W z#$v|2=t;8!+VD*^N;~;4!;$B{oaNP%C2#tL(Na~rh`gKKy|#y9^$~I_FtM0L`?hW4 zz;0y9Pl4zh(IN=rR@Q@eZV5U;4pAR-1$TR|V^k?+!&EDUO_h&cPo>$4#~)~#-bqtdeCc`3HZ1Ui?}(f zSBaUwIlNa%J;f1n^l$a~BI2-7fhiSVON+NzMHIhe*Oe?<13+e4J$j$X##{^PWFd6$ z+r(TpG-BCCZ2RSxHu4NGmJ#E;MvT~GRK-%$@gs7j91Td2z&nI-bVT)Edf6p4ALse< z=&fJf{8S2k7vjen233@gg?%NwU;^VIG7|+deiQLZ$D0Qfq>;3kPE8fo8MKEDJ-S>Iv(6yI0A3(}me%b{SaGpu`ZX-r2F4Yj4p6 zYx%_`o(Z8@LB@lh%ev1*b&3B>^14tQGX)9Q{gTN-t98G{RR!Uy2qxqT$sQe!XA9s- z4^|D?z#sDjQqrVyyS@X?WQJ?kI12m zi_Jfi-#hv5m0(_gmA+FaO`MnnklsW@0f-0&;OoQKRpTtTn@K~Fse4Aks>E*1XNYM<<4Kf24J(s>;B9U76W3 z9?9-)kY1dqV^LF~)l?&ve#=yTH) zuYQcPBvF<4oK9DI)fYr3Wqx@OVE%3aPpyp15_7pnTX~|Gz4m#d7)G7M6=w+ZpL{im zL$+bTLBd)O7!Vn|es0#|-5ExTbo}Ytrn>Z9tYvw0x7chg>Lz2miwH;NxGOJm1M<1XqE`I8~H zNJb&yBcbd1ojaR}ij226Qr$^BSkdNmC!Y~>ARb>B-GUWM^Aw{F8lu{>1|7&j)m8N$ z3LQsM_U~top+Yg}^QT=}7D{7n)T7XUO(Y95$t^T#*Uq1xM-#utZ`pBmEhEq`!6q&- znH0;_8Kb4{aSt+;%e}PC?gSIJfUD1Cjfix7*7m!c4U6@b?uvgOuT)5qpctmk!LKLnlI@lKm;nVqkFymvtQ{G#r_|+kTj}c~ z+N23Gy%tkH02Hi9Z=^Rt-JEP@%p;=O}Er0ByM;qMOXgws?5y2am& z!*dCRJh99k^v$~>FD)ZxFV#>PEc$zSO&45+ghY!1TU3S%@fI>ink^5`M}Juoh^51e zLYXKhxi9#I7UF_sc>sfZ!Wb|7#-i@Y_meIIf)xCfc1Bxl8>6qRXwlT~)Tt9gi<;tn z2S0bc5llWM_TXIBGF%e1(G3pn=2)n*?|a&6xoBtF2!vk$ z(23`f-rfT8GS;%20C^dOCBjr7W4C))XU!FWKa_CF(kYO<#D#H%_rz{AHxBB7IUbS{ zkU;RBKzCGLW~4j9TFFAjBYYoBb)49Qn2U|Jg-|q+gs-gfOBlTQ=~01pa)CFRc^0CF zC8cF+ijsAA6lmu66M#^1@Lo*OO6*bhz4NHki|5~^cP#mSHm+_An=%#qDQU^}c~@<9 zYin&Il}Nx;INg&Gt+WoQk$sb#X$=)J|8j_Np7II1|!{3m>udUcr`85895$xu| zAxx6E9rL46TbXeh1h3T#9~Dv18ev>W@JV^XKBWs8nuN;&I5zDu&zlQLYP#^QhA=54 z&KL6=SNw8T=E%4Y8y{D-tU_mR7Zls-S(LuNQaT~Tv~o8o+%AHgYU64BgaDcgb+UQ> zY&6?n(9K(NcO8MAxs+OBkxQ1iKv(%KQbawGZFyAY(vWetibCNdni=x9q@{%+;ky%d z+?EIcq#}b+Kpk>NSr=W2-#xIyaRW(WEsCEFaV5F@$zW*YsrQe!*+%rF|21>jAivrT z+9cqAHgq*`$9AVOaZ;kHmC8Tl+NH+`nn(*a-sq|MrT|c`RhJxM!pPI~IIr#QNsY*~ z!Y_EW0vQ$uD&Qh*ahp5OYiDmyElvCv^@4B&*J#b|J}g z0YzsGs0rFNDj%0+qZlcP(Wd9$talHfy9^43>m+HPvT|d|Ul_KGCl;lPh+35`Q?{~s zxAgX0z<}ZI9ZtW!>R@_3$K()gx*~ckO=gA}$F(I~W3-eRDRLtKjJCr^SzAezWw?2a z=E$-0hf9;-%qBs#K}~%jLUwL_^h%Gc1FNn-0~Eaa&Vue$=DeR?sq}|l+>COEFTH{i z4sLhot(Ti4+UTZ8g5&3H>ozy<1}itJ1EC>E$+c}>Rn5}T3_|hOG1W&AK-c2^ zg&{7&YxvgE2ThjYkvBchR&QtpEhI_JGEnCu&G6MoDUb^pWt~Ndla8ei>Z-pwo$_#Z zh?3|{nxaWMni)>jn;?&g^FMUg3a`giR8}Xb(n@i@Lds8`jb$SxU@Iz3Gt2sCiMOmN zKe(WWw4%i`iHR|5QSyF*LJIdb;;!(N!$6YVvko3h0vqvl`$nDwjgmdUERX1Po$drK z67)RAlmK3|1f?8`;eVRc*XK7-5~3nok9!j3@!+-wjF##$*tb#pm54ai+@*Q5Zgs|RtAVg~(K+p>{Yk_Nin0efgNX&4$!gxT9 z@0vBCWYXZXjMTFAQC#yD9L!tZ7S*2S$L`J<7}5NI^)rGh-`IzN16^>(^0is?UWrpw zYE@mZ@U33_B4<)M^QP?Gp>09Ctvfx!SxCCrBSkN_P?2P3kQ4 zhl+>sz{g)48;1~6lYT$d_2I+lC%vm7aaw4oetKP%$w4wYu{z{Iz9Mq|{(-mGEU-ig zni9D{UF^UDJ%zC;&S_-i%{#~@4k8(e{Kx`Pc^%<>!Z<%^ch4K`Rg~F06KAm&5?zFS@K(gb`-6ut%)^k0Y&q6eKED!) zNV|i>)Zj&rx2hkM@ht?=LsI?}OOYga2EK5%aUi1d#`5ua)b^dK{ZTLUt$GTsTl92*g|7s`ix;05PXUs1A>V=^ z*eYfIxANTPUg`b4)MDreo(uMT=K+G_?6S79Bgu{0G{_=nzS>z~qhDSLG#|k22uWjE z;hl7eQL0r5szokleRb-UEvCP#tQ09nx#|Rod&H;nwH1sfR<|V>kOuL5uj~&bm(ipf z1a)(nj*}?n9=TkTp+m-vA&Bk;P1OBj`(xcu0sHhayvqV-qP*=x{FXMp>EKxMjRK;S z#sq-AP7L4j=az~{Gm3UzPmkGv;c4tnquGKVI1!>)J1G} z>-1~F0+lLg{n4GcQo)(2ijQ>omCo<=EqrW`a7vgj?qI*I>mZ$ASD-#q@V;2Qex`5R zL*CNHLK3HJlRt;OCfx!JEk2~KOG(Tb>C!eD6ij}*ftXSiwE1v2qa z$1AMDn^9F7c)^))l3qAjo+(5F0{ z7F9J{>Z2#3C52asDo_-5;XccHN=GsvN=sfPvCuYiw}PA3w65B z`SHcwsYKn4uelvDphw0NC8jOEfdph;Pf4Sm9tuvEBywO}+-RNBa{%RK zY3dmAM4|F5S18*NA&qWx^ETKziOf$wi9-(mL2lc405ew`qarsmdTbmUMM7?@l&@5j zr8LiG%f+iFu>>2;3fc}OXUW{NZKf}i0nTk`t8&3K{IdJ{4$gYSqK@i{C8Gqtq>9i% zur-8A+okp^#j}KRd+tK##>2=?m9HCKJe(jn7JBB**e`QFCt}*E2x1GONosV}gqK5RTwB8Qd1nq7ei9*ysxKp|NdRA?=CYYn@rTHomR z5cL9rPB2z;9c+-@*AMZNWqKQevtJMHG-+)MANAMN)e^d-jj7L5sb}5&Fo(h8;ranm zmN}>BHdxN90JG3#+xVM2K#Rzoqt|KX>AVrYPgFD3W+;qtgDf`mBy8zq9fbjgP*ylz zeHfG(pf^aFN5C{bY+H8^tRR|zzJMq`Q+OO4Wl5rZMwb#?g%O{+Qw=Np**?Sxj%=&& zZ0Y=Z9J8N~mqM)VY2lY@h$Np?kus^pcn98D!|~yR7`iE>r+s zIU)jFtsXj<*xOK?DHP`;9TjUumC%*o*%VN2!jkF|&}V=Z0i$|EHDVbzGjXoXO&Lx* zRa;@O_2cSV`UC_9Y66R(b*MpMV~upS$Y~dRRp|H<@N)(e&s>%eb1-b`A%_%$V=;h< z;GElEm2C>{Wb^wX$*Z={z3d*KNQrG;{B(f(1;fn=n8s$ngKjQEB-X|;_P$IaRCe6P zo_x1ztS2Fcgq_%+{l9-E5}~Y1>?B=Az7+I^vOJh?3{k@jBx5v^(wPkBhfnB)9Qy^E zL4%4n2ban*+hXeK0$b`_VH2K!H}2u|m|i&>11DjWBqLarHUr~vgycM8+AOjIAu~uR zG3Y~C+1cdOpDo_krs(VnjOPCPH{aI^+g=8*M(#gk_t#5}kY4Q0A^sG!#JKrq&IWuM zK=nOyZO?3PoLQ}Es``AVhhU*7>>DoLA#B)o3Y%YRLnGp4Cyeam@2M#1bed?t1bx$7 z296{ljPAR1tXwgW;Iw+P`-(=o?V^wYnH;M z_(=Et|R9==Q2~^KV0E)uB%ba9UI7;f2uB!Tv30MYsTp}zd*WEE&&3L02eDpn* zND|IKVIAkfztZ##4a4nojuT9kMbV&^LvD4!RX(65vZ3l{5-GD+2tvJc@$(2NGyTD2 z5IrqF0jgn5Gy}?r>R7UxcS*!3o9DkP63keH&G(r+m5MvAwbqLIOxo=yA7dE40Kw3~ z=bK?%e>sA_-<7q%6EzU2Cax2#_JM1|Zu`r^DmVsyio3fP(RroiVxg z?7jw>XJ1K%3YJ}~p~@i~pX>f)$3kl`b#T?AcVEU~%}p1*eh5K8d12n0>p8IB{6DtN1Fq-&ZR5WXSs|fe7mBQCm|3N$G>kYQ z3aO-w$f!7uSw=ELMpQB?qGV)Lloh4yjI1)UN8@?l4#(d)kJsyYp7Z}dNBw@^@8@%m z>$>jip6d>L>pUadyfdIkWKJNPx*j5o@O3&VIZW=!_bRyXTwukgz=1T>yV!H3feZkU zLqxGj=SoNG630nbU7`Z6oW!24bn%?y4!&FFb2kBzw@Lu z2*yS506iy$fL}j=`MCd3&wYOneL^Q-eM?6ogAix7JIMk9m9f`AiXSFqbyzWVe$gNG z>c!KdB|X{fDO!s&UuuZTLg?%9v-B$EgNzH})hOfdD!IXO(^kImK9QzU0dfJSVac4RTK+U7XbHbYF|(UqMX>=W9! z`hK9miZVMVm}?0?J58z)>iV>V3_%yp_Do?ZN^rFkCxy2CTfzS;h75+S5Oj~a zwvib+!6DD;n+rKCoD*s?&nl~j3NR;HdW4hopARb8J>(^~rIkMUnf6+n<67y=$pK`Je8#2~>RkJ}uVx%JGUv5d#+LCX8&kMo+C}znHpzJkx7s-rh0G z$#ZJ^_JcfM?m9B6jb*OG{{2SoZ#9Ow4vo>mt*&3<_*I7SX&qXO&w9P;OUSO5*OgCP z>GA1pK$9hpq9ac}ojL#VyJH(3pS;}he=Z>ZSe52=GED=!#_En!_!WyIExMP(c-Nzd zUEHU9JDj~N#*@BkTxv9i>?{t*u$bp0)#^0O!Pjt7c=tWvxv>mrv>ki$I5^2}5>zJc*6c+%_2o61hFI#+BF5n} z>Gr8%XP}gq?N7MKcD7Mh(wO!1auXZuZqald`I=90>j26pJC%FENmyvLl1n5^ zvZE$xR=TGA`n(qeCvwbsD*Ro%i|2F=-2=;y%hnj4Qp0E61ogvzew4H&PjButugMOk z0Xzn3rO>FTsO1gYe{DYExtWb6T~QR#`KK>EdmLqNEj*?Z)w$l)#g?>wU+KwnfQ38M z)kvK6dW??EWcg^Y2~HoJ3BB(#+a1*KyoO2S@XfmA%MowN!tTvrNeh}~agTEl77Rla ziDToc+|u)DX@#e!`%ukEZm)Jf>&lfpzPET=)6xuqt(uj&XBiiY{fdW%g^wx=#X6N< zZwTa}9nq@Uw*Y74Z^X6>*RMPBGtIu-qnMw`;ssWn*>MA}AwrzIStWHRFXlt}x7VK3 zy2G`vGpH&q*aT5=$JPm{7r27Omoo5$xN|SbP8LK$P{^L3*kCh+bcycS{bU(|QYA&O zxUS;14aYd;?Pm^ThOUyrMY$_H)Av6Go#ga_<7BcOhRSB;u=JkO9xsJH>*9St;gZ7_ zXg{V)^X5T|9Tkdjki?Dzr$M}sMI}8@sP7OuiQjMB{W84U2 zm;qpLknyH=7%Ho$$66>*zh1a6Tl8d0J4+#(&3m(ep5N`mySC9)m%&Rq5`G&r zX|jLy>bf{Zmp|Cd^SFi~#8i23lsh}1u021z<8iD<4bcvZ<_=mKhsK?)sxvcXW>IVG zH)11(@PLDjtV+Qj_L8+mVw`NSCE^QR+75zBqx{cezt5zGI=_d@tkpy;TV*MGty)7d z-{m|hW|`q~)57fO}K~Ih2f3N?bRyx(^P-Wo=&i z>-_@1c)+QxayDVl83jT^vwI+j?woVKPa5aGX{Q>nyFM*F{g&6-bA}T#{|+8DKSxVU z!)RP4(<`tCV+1P}hNW-8Tbci?1Hjr^5mb*Cb7=M@>@3E3dzbE2QMmkp9lViidM$Vp z2ONQ0uU;~*jz}Ph-|5dx*^#H~^(%1v{ZDbLDKtEgvQ<%vx<)r=B|^^C|A*um3HVwbP|h56V0DeJ+)%32rpLqf+vL-V-@A+ z9)4Lf^;Fq#TF3UQfF!Mb^Bn045r*EFciQDxy;fBCuyc0YhPi@0iY@A=2pWHeQ* zWP6n>OLJp~r4yBwQ&vzKHV?mehP%pa-xlDngK`1WwFgdgb91LJlx6p6_lJ~gapu_) zc6!gWj>GnbU21au?3_F4V<=NBRz{)k@(XXJI0Q;L$T!Cjy(D1ucRd>K#8iv7 z%cz4D=)lp;*~G;>7!H-DL7kbwO`Ls+D1wW9>clEls<@MrzS7}|%vwvg_fEu87EC0h z-~aUQx2!PGY1eK=-GZ*nq#cw83Wg}VOw`rX#vfm^7ETGMWe3+spOQUw^N3AvWC)b= zCC1Pawy$2bdsMqU!8?*9$~IkTQ4zN%^}t?-n{S7NU0a%mP1SGK3SelYv; zxPHUStDIR&Sh;GgxUWeNH#vl(2HZai=tfR|o{O07>kA52D4588<`S8WxXxp5;&EC2 zG7Nd{Ue~I8Q_j~{^mI8WgDOo~{dqb&zxHCW4n7e}-14B=##X#@9aTkB;^9hWU%g8w z=@Km2FS?ndFzHZjwZMxkJCJQ97}1yPap*GXa@UWWH55S=RI6KTnvUSx7Cv}z8sm8v zW$yt$o?vP{wxoM-=Wem0N*uE19VZOOB8jU~Cv_?8_aDb6E>mlz(oo^jg?;cm&wh1R z#qg4;qE>@(Px2C+dH~4ynsDuAM*qXe{QV2&HX?KpZuOcsmyLg{(U#?EVBs^UQ8)oZ zLS^Z*l>;09WN{(P^m72jK~`2R29&nuwy^m*2K`xEnxq9ymT(_Xj_l{ZV5QH!1KLJ5 ztOkL%u;cO7Jghl<=+H1W3Gc_U+x^`qY!FY~gVf?5oce zS&CA>wRIMwl$#k-4k<1aQow46z$|p04NOMe~x~cTPA0Ne=Qm&LbiJu zPrbJfs-2~!*S(*X5(Qr5$L|`J(U`{qB(@(w_13)Jm{IU~x1vAT0eaxhpb`7#I}b5c z1mwzoEm~xaCa$vZl;YjSXybe*=~F2N-{0~)sNbT+>3ma4?qLuz`2%s0x9(d$@AFZr zrwtca_cmyDM)kj+aq76g7A+7wsU;%9IL!*}^XTw=nm$#|B z06E+q7M+Wa`FBr)-!4)Q`j40~C+6t0-GT%v$%%Jge)Y+xS+DNcbM9m*qZC$h|21p{!`+O;`_JZ(#52+;~V&MF7Z$xvVVD?yOe7zO3IlkI}aGbWUL~PgjpZ z%eD4NHaZ!(ov#~Pe7&l|$5u&6$=KhS9B7T3{P2f^ezM1nN0Q&=46ci(G#OZ+2u`9x zP4oQ9k`4ofLRI%~Gj@v%FhS$KzYTowZi1RT`M%+{-9-V6q!fglV$c^|E59vQDIiTl~@>)Ba0ouWNx}uj^aJZ zHuM#=yB>}FRkgZW-VdxeA)ncisIp}ey72cDjM~jeGxkJzXScX$sAsL19&iKv=LXM` zC5QlPrN0OSlsgt!ankW1u=eF=vV`E)Y>wl8$ju8S9~I~Y4%C~xH zP%*nfku+mYdEf0WWP>=)d-7+EMMu-ZEJ*R^khK6icgJshW## z>&?2|ui3yO7nqTSCm!zoPpSA%m`eQ!=r@{G0Dt+tzQ)IOYn)Zve$tl$fboZUU-=Jt7;mao4t*$`|-{{&GQ^d zn`<)6|M2e_ymV=&TS3kB^)F1$yMiKIy-}k{>z>|Z&g0gD%{|MUh_rI?xnAxRzsCN- zXasT@klAl|1MJXf?nkdBQ#y@_wrfH38X^yj`#9OMl4%Uu0L{4<<Z-Yy{tklIQ*VpL`KHSV4d0f_=*BaZsUpHTF#;_e+iSzVzLd zSO(GjW|h(xZ0_v3_)_C5R4Oyy6!e|>{F*t~!{aYC(b8xARc*x=THoLt5^priio6~+ z#veTwS%>QfRx&*c`xMTKB1gI?!``-*mP=_kqNtWl%@44Z{3`wbd%#O}h~C+=QtS-b zx0zhxD{Ek6=UUrQhojG4nMJ*x%aRU;96V6@_0(6geN0_7D$Zp={MU$puQ6oRmsd5Z zzlYI}lfsD*-2+NZYc^20EPCF{x9KoKPT|9cXU>r}Tm5QB)mb=i1i$m>5z^eHaY{~G zmbG5SL8($x5%g#06FbrYHxKq5>P*pwht!K5`T5kKw=?#*PDmR2OnX)hD zX+-(^HsNzG8n;5A4_;<_G=Lx&4d|!QM2Bh#lS?}{w@AkcMNgjWVTmO@nY8T2zSiBk zb(0P0CBAU7J75|2TCQnnYxIxTBzq*!R5n_&?tE<>iytViI zQIf0a{@z1~wxg*gkLOYi&0s<2=p>)M1T3R#sdda|etI=}Mcxcw-xf}(wgRSLu9#Pt zwL!oXsv42dv;4fmY*n7D^%f)xw21q;;*Fx3^tZb{Y<*d^$G_i4Tr1k8Tr&E8;yum& z@+V8SCky6!lnSK;A2S=BB_ zX?J8KI{LkRR#ASm|Bh8y(8U}1J{;iB&Zxq9DM?APGF+C&?BFxL)m1i)V`ka|pf>&9 zABj_L-G(T#mcfL0J(&%Hm>S7JHvd(%k6g55Ni5-n z*0ZZ|I$9NJ|2MBXJ&_`}2%tw`uglwdN$>E2nA9E6MGhB4Z^mA2iafw0_VY@G#%eeQ zkycauviiyDjpEHx?|a8%n4c)3$dk48(B33Zee}0~keF7DK1y$}oW10fWj}z5R!iKZbr9QSQS5oV1rq;vOlAeS5#fkmCNWo_V4rjZ~nCrKY1zh~4bF4jQWi_YCG zxNcNEZPD^ZMNDVS*$Jf3@>QujSo?Xpe!~J6UfV7{lGAZ-fRzuF=`pYXw(BN2Ic*eh z1!Y9;&Y7zkciTxMY0J(j^P22CCNaGLhfi4MT3uCjVm}rgwcPyENy$fxojxA7kL=HG zy$pO=_S+2#)9YIhS6xy#z5UG3Z^q3><4c@gOX#0i;){B2-L`GzEAAKZ5Xf&s8Eq3J zk#57fmZH>}*;1xu_EIsnWv??moQa_=3nTdxP%;;C+@6Y*+G8J6O8Taz8`G}@ZRP5% z%>KG3yi=urzqkMog4m1ur%utzt5jANaiLbPIfn4-?ndc4Lg4;!^5wYB$1 zEMk;IbyHMIN(#zjb5|R+A*`GxkNJ*!7{AplaE!07uZ72L57!hjpjPZw#M5|1agXvJ z_LrV%2$08TJ3F8vkYYu) zZXpKWfvfIdD)|=bseSLzUNS7u)nP6%&-QxUm!|-PQ%hPe3l%U^-av3PNRCb8f#?}T zy1Ga%*{^Id6n5O|k?U36ddlq?c1D45gZH}|z-`pa^nGJlTN@fN@3V}|(4SLY>@~04 zL19}2b}t+2!9)`S%I5HK52UUV-eD9x-k+zAPPsi?Dn*K_VLSRMlMYYw|!^>|KyDg7@3Z7RNftH8Iz2nK3F5V|b&U|o2nW+6-SddGXhIxA*kTwsT zyq%d@#p#No!opUxX$|xp2Jl?pe*CyTA|e9dW*BN~Rw?$#xpf(YodYYIpidwnArBqB z>00J{{T$dobdR@W68=JyuO&|z3(X+~HY>W16Y8ka*{JgRlY%a_J_MHTNO2IHL!o0w zYn@gk?Gq}!!etFlJ*Q-$DoG;29N@pw=hW_5HH+siySXDP?l|4wHLwn6--(Pk$xulf z!=m=CiS)J?L;iCPT$BS*m`R}yc~iG)jREAkM7C-Ui*ZQPzidJiNdpQUWVn73F%{DE zmAe1pzy6|=zjpq_WjLmSN@=bsG-g=AO^);fihlg?;rK&O%V};kJVQ+x7Z&K98bUS2 zPqy-s|23Pq5}e!O|Iz5hRADw{mNBBZ-S|Ps+KDE?xl1R#NPXLT1a08P1IIf~EuA+;sna z4D^TZ+Xyo5R$Tc`=nP(WY(yTnsVmUiiacUTzyc$y)FZauCMqVrumg(m+ zBEbGnpP%_P39*Okp^?-qH=Wj=n|kt#1*)@z*>@?SHnka9x|ilfYBO%QVE0+mS#osy z&-A_Mkz@-tyA6%xl2@Pw+7BhaT3TPtsc^evF^1!I9Z3GJrl^08Yvlv2$RP1UlA`X zilQPTqmk0_N)Z5VT8dZx7tZEX#h_jzLi@I`@1!%J2T%dyNWA_8TwU@@kV$A(nMK^4SqHV7}(!=L) ztB+8M8vFk&}F~=>>uhMVVZZO*Tt6aB4rqAFm&gDDNmhKv4 zKJ%v*Adedq_%;zHu0aRq{0<4S2VAxV(|8`F);jVn`3g|L(t0{gOP}976h$O#O~>4! zmcp-y5cHLylo-bQG?I55+RpICXf)O%$Rb@`QWex1`Ip|cDR^ePt>@%@Voq26cZ(xp zZ!Jv{oyE$=`hkIgXB0L15VAvZ{nF$+Tj`U)V1 zL*x#CZ1LitbFzc1D$5Var|zA4xE!gW_qr#6v!!#3o4qS0!fYbb!>hI{nTBvJoQ_YF z^FnoUW*wJvC!=XjrOBRXPhMp2s>YEqIKZn_zNZ_{#j&nZd<%Z?JEl*jWKFmuDIUfF z`M%CN<}0|#kaHV|di1_>w&7YXfU6M=>y+QVetm5xC8yeENxN5&S3hmRuaA`=0(G6W+-i`!YI?cyE zo^3c>yls==fsAy)?G&_3Y++?(Wl>Z|tLxHl@jP&;!C_k)9_)b)SHR-8I#hK;D3;!n zRc_Af;-d&ELpV8T!AgnJ1l>;c;s(>Ne8myjh(cp*X@G;nHJu9QUEEbzx{5#3m!Y+< zv$<{g=Q@f~`{OJAd9;+tAvaEOU|N_h0=*%?*s$lCSEjT2zWB0HDoT+x>Ztct$1U$* zZH=Zsu};g#h^4FI9ZqVYh?@+t`~5fr*|A0W`sB-V`M#rwu+&z&ap{TXOc+$%uDC;! zB`dGvzminixc%>=m4A-g{_L4M)6R1(j1N_N<34CZhbjB~Y;9HjN7?!L8P{yMO2%Ws z=Hh3MCa9$NHlqbKsF> zaaFP9g;73%sr{%!G$@qMt?x9`x8J#lerz|N>5s#&=qQBCjZ3s z=Kr8ewowC?saX_7m<4WI9e9>sCqLMeo3W^5HMc5~m3i*qZ?^vBAdv!x2u&x6m{Q!J zhQhcF&7lYy;4K~>)|r<0UkVvJu=0%=56@M1HHoIg8ci4a|2*l{;9-;D zCA^I5z6N@vGZw88yYmd2{sav!`w}erg?JZgtvO(D}K$5K@Wg0mZp|UN@+yfRg%^=n$qIsnTbDm^01{FborwXDmyFL2sAplEKs$RElH!aJk^-PDZJx5s6ioFZ( zsdFj#&1Y0go+T5_#b9hCR6aGTQmvY{{}dAx*U3pq zbvr3icT&iXY5s@;dJLCrQYDv0vM-mBw|tvF&n*!jL-OnM5vQ7gx6x$p%OyQnT)F7^(`{cr?IOjeQNd{(49C@?YxfZ^e4d- zNKwQp6iS$0Wdr!^rusU;1RGFH-5rX!uhZ{PF$&~)=5uZGiu4Ksa{|PuskmIlz9A7H zS|<_MFOsIK-%nWW*U44gPbFpX0`2B?{dWfNN^6cW4Qw#HXD9*y?U)|8J^+*C97upj z$V0U5w$JJzDS1Cb*Mz)`7B?=FjwxIyP}1Y%5Tx{gDT{?G^< z(K7DT8I}jrMqY;qG8oF4{qax z)4g`0DEsg%`;kdtw^&_aW+JAO#D|}|}9<dJU?RNsKeWk z&9rRUQstuwR$1qE+Nia94zD60fs-?lAeZ)BTEV@Kh_372jB6z9D?zF5xsIZeCWE|> zJ-DO@^dc^<6Z9iIcu>{lLA8Gs#ZFExF59nf`^EuJ^m)*a=(z2#HyX58Lh~UOsh{3o ziyH#Hx{K=T0@gQu6dIxntKHc2>ceI={HlVaUL)q@Agn2ruvv4AzTS3IRLr*k)cnMr{`ff@U2`lxK3)_qqbI6qY3=1_cv5Om&Hi!p za*NHnP0a%Cqh8r+ID{%mXC{^%7kGNfKilh%fv-0l9!N3q;@*jr_}0rr`6i+O zLP5$HGy2qY4{pge&Kz=k5AwW)N3|+do-z8U>!z?R1PX3=a3l$MVObM$AXOe4{0~$ycuts_!=z62GeKNfPljYFo)$5P0=+#9D3gjlmfxscrAjpA5E`pC zrl||_FZ@_`9&|lA&z1KS!+yr|JI&-E*!czH_;KUe1T>}Jt`JMOLX zRFs0!@DmX^1_W&DV6$fwX1Ce$fJ-nSjsF#P+#dC=-c@> zqflp83lw}AkmDJRpWrea#LNvHr+S^$`=2Mw-+TP%k!$2D{^AzHA&wFLDZY=Yu4quF z5TO-;p_4-j#!Y@(5zx#GzVTfdfJP|gvW)JhdyHUp3=h|B;zX^KRmbTWkOH>k$)*iB z^Z-!@kG1v3UMW5gDxDSx&?$cBu#CC5bV3;QdlNVL)Q$6XCf-jez4omUZx>L;hSp#x zaQDJL|7@V4p@*|!)4@R*(8NDJu#frefN6Cezlel&`OF-I_fvNE-T9Yp z>(+xpg-8|AtBPWUY`%f%wy4`M2P+m-Sp9)sTGlG4lf=ja#n0q@<;WUZ%$hZ8p(Ku` z+2Dm?6fd+wg^wTW_2}^jZPpD4iCvtFB8bQCz7H31>toQG58T@eJK{2SKjXw21{ZCe z|J!4A>5B-Zi^C^w!{f<)VUkEWVGQ5u%$!SoP87K{(|AH=ST;40L?fLKxU}~t(OPht z{We|UbsU~}NlPd3?Afyo)YQ%>d$@`=;|HkW)19+&c(%{{;vK2Swj zS>M?BhDYrOf|5Rqn@n2Ig~3-jW5I-(CpUnTD|`TF7`yctL8c*y<>tS6)4XfJEHK!= zzXr!PpzN3&+x)mzG)3Y=z`*{+g@5j~%bJ{2%pP<9^Y3~0`9uZed6QeWOlb_8h|ze8!&15iIu8D!Dn!8s8~ zAK~N&UFDOcUKz7w(xgf4co!s6nPr$kB`b4Bbh;6OM3ThNdzVE7ddB3vLdBDdb44y5 z!}#9WbDrGB|Mpr^`&r-B*%lsN6y`t2-(NFkt`NcP0`j0aK5%k0W2BCvD9WF2(ee13 zg~SwLqCjk4er$RTp@m+%c3GBUaGZ7b?$#AK>GK$pN%ug=TfdVc3Py0(_s(k~d{Xd< zQw6U%)~u`9@!2IiL0Q5! zZHi$u*ON+uUvgyGqr2z`nY$L9`3t6=p0xg<&HC|{;}o=ZIx{h=9JnoL-R`KUiiZ<( z*Vk>*XoV%4298y>E|1@ z$zS5Kv)(%}dU~wab#2ZlDUM{kztXzA2_%uY!+s`Q! zh1d`3M}!l_)fG_|{}ac^j}}GMs#WXZ<;TN%0lfs*G}1CKmA(n2>R_e&c0Iaw9V!zA z)C6G9t=8$;_XARzHgo3u$nGMRKH!|ZlV2n%J`u>*I9#$`S44c2yT~y*HA|@c<~%=? z{U<4vG<$UKK8iMBI6qLmY10na`j|N%qF!(75V@3V8pCIQF(mNoT1xdOd9qN>qOZj? zq;IMvCCB%QYPHGzs#4E=ghCWhh>zD9 zjJ(#u5TGWH(be6s;94=KK<0&%L}3=6;`JS3JaWQ#G~8u3&}LXW<6792fp+;u5k2-gdJIx&x#O%`cd z+6%D&CuSHrd_=dyPR{e!N;(ts<`2{$-~ZS?MSI0-ty0C*d7=!h(7J>eyZSby8W!PU zuC{&I2WsuWM`EOT?>kRky*dySq>%Cf=thKm4<9}hee`z`4Snu{Eky`o_YyW@ zxV8p86?NKU4=X}(GV@+I%UvMwv3E;H0w2rH8zSWm=#+iDK4xzQ5$_M(@)wdG zvUVR_rqjIC*GYi-@``1%RlWw_(;mEH9FTeHE0D=v?m@GkUuz}Y4~?xret?R*5n?Rq z^$B3-8JTVQs#P0St*W!)cQtC$`Zj9qW1(vo1yy3!c-QC8vvm^D2KX<@Y)f(i+vh_G zkpOh=w;_u`r;uFGqOOf%Lb^ex$KMh|2BpeA-CJ+2{jT(jkM+x8(@>b5K}GSPuyD6% z;fahE84pfqdfnwyR6kwaZNh}fWFfOJqkx@r@XwGbAtBvs)zZ~7H3+C#^Y2%b`kX~K zciqd>S`9KCItIs<97y{Q41~wcF5gaQDuP2BHh%oh+>EA3PjQHjp%Oe#4*oow%jZFF zBQzX}h|x5oo0;S1hV7qoPjClv0#uxR!8=fp3G=b-T=iipDQoiMcuzz&!#&eK>g8W- zqMUXs#EYLNx&<)*$kcEHq&!ZNlDV5*fk>q+MxC3OnJ>af)UO7#*9}{|X(tz6v}^9~ zo`bbVgX}D-&9@2{EfWd0tc^`w1QFj=%v9R9*wWA&@ekSB;#@rS`)7-h!*WB7j zPp1`IhPo64A!<64O)K(BnbE?yqWzh&CL|gY2aXzdsSj| zEK1$$4-jiLHMORtCj`Nynub-C>6j(AZ);o*`PJ@bk`n^o4p!7(-l*fNPW~d1px;7& zZMY(%Qg1#Uu>Os`t{xzYB~B?fIN4Ub&NtBqz) zv_Ar~3*iy?l8qjF!Fz@<9vr%2Ma>oUK5s(LG8r+I=m_FFgO=<8HuibfjdxXJXh6Wp zpP})`&=gd|ZZcdE^MQFpM3v_z22OE($e6iAcK<+m#lQZnTJWu{yfMP3#S2vA4y4lQ z(ffM#>Se^y0jA2aEMxr71CJzHxx{d53i9APc^$2oz3&Q&Ed5#hEwDy<$mhkkk6w9b zWOHKp>9f)$r$z!XY45*Js$SYR0_IJ{EMQVTf|x7>?L}<1rj_k?iwKppuOPniLcj#qv0a z3d8YJ+D4l-X`L^jdmoI8Cy&reRkdJ*>_U^PR^lrYGa(wC`%z;ZecqGE$fU<^Fo0*9 zCT){KI%K5@jg>$edq<}j^C~K+mG;%?{kacs$FRGt=!KB)h%DwRAt*AC>k%yNa~HDNj?v% z1qQqVIypA$^)|v_6#C(|v@x`TBJtx4&qTzBwp9;`TpFjZ-N0C_(9s;ubV6Q1Z@u5E zJiP@xs}u|J>SX2yeS>DRcPx*%&cpiByjxHNMhLkgY759e1~?>A5e`VPaTLDVwi z=21f;ZitSZoI@bD>DtSnIr&d;L-LNw?;LwmB}pnDU&3;ve_V?bQ)+9t zGO>{MOoK39rEKYV#?EN>=nDqZhccM@R3{G@cmtv;U5q;h19yJStw7zv?iS~(#MK$^ zpV@={y2H|>Uw%yyUngLJn*)No@=t*kBPh-iW539x76qI-AC}jl*`ysDm(=U@L79J5 zD_9_ogv*vKgZg-NXCyZW)+g@UKACcp-Pp8VYoTv~gX<9|DZ#TK!6uyT27DzVAt2_f zG5}}%3a2$Ah63olI!gmI+g-pPnPcTlZSCprAS2Ggc}-cmFvA23tX>tR@1z*|UGFm6 zl5v=AaZ@$#1~hK+QwxAp^gHfDl2P3{8VOtWS9+AMcqMr?8Yy!wPA@s?zB_JhCuC~G z{DD8j8Rdc*?xD{%irT-^C741SU~+_n&pw6@s=0GM2k_hdML}j>@OyPWj@%|Ap6vgl z?NF=brP^cg4>g>R7Gl3Hqs-9r)n3xffj}k_0x3zjSt2UQP5;PHOZ_1|cVJA^k5@jF zQb$q>Qj=cphuy9C`HmJAi?gT0hstK}NJ7u;!Z~l=zt0JudNPIrNA_@9WJo>E>!)Hn z{g+A~=;jx9Q;x=erFsxs06N>EPMgSZlQ|o6$Uh?QFZn8+fC#Kg7kA&j9UYr!AG*)` z94C+v&%Pae@!pI$4&{eecf%(}@?JjM2mmAELYW+=HAZ>JP;{%)`OAn%R2Rd!kgfmy zQc&3~>a#7*6_BT^!%<2B(aAh3h8~t`rd8|KG(KK84T+lR9X3a8PMY=c6xxGiyD8St z3drF?awb-2DlR{x`C3#E#67OAq;PSj1!zBNDuRKw)Y_-d?4fU1Uf0DQle)$CWb-A|>SYCr34``1T?UN>CRYJl&#<%b8D zMrv&}y^uYi-L&pTXGC?{l5J`E=Na$6`86Im@LAcNdu@&u6;5;Rkg?l0@Z^|J zPd#66oqcLdo$#kkyU(!1=E-DEF`wZ&qbNmhOoq?)_n(YxvaP;;w(F{16KCwFfuj^T z-@<#R!i8KNPp3M?AP~SrrfWekwj4;I<84O(mPG#cDabXL@0;$mmy8%XG*Yy=pc*)TmmE^NyO)>SMX<6CISFc_%3K|jLVwv1?;)L{rH|IA(K5evqG#bX44_%MU0TOVM8q}(j&@~7cQ){ZvFbbNl8wSZ4Mk|j3fn)C79$F+iW;8XS67Z zNzA9K^3;=(`1hqbCHJuudOLX99-LucM z>fUw!>oP#)CgUx~Q<|^2!xQiJVSdKSz0uLsm1O5!-?RhT+=VR@teR$7< zQQYh0%a-jJnty_#!Ld>>t#F>`CtwS2?%t+c>)W}1HUryU@knfotl*iJoK=b}>zfr% z$l#7>K4j`@Lyu2H(q~wvbUv1jO>-R@!*eln-o1T$ZBh05BK4O;oZp?4J1TzsUuefZ z4^Vc#N(B*8+m*Z+i*9aLYHFXT*S?I&NaJ?{71X<8I(-(3=$QaNU^B4Eam$Ld@!518 zAF#kZpO?4c5;j&oeRFZH@;Hf zX`pnO#PH-fvlp1h*8<{OtV)EGmihSy4<3k+9sF0ME>(4pZKQQ2F|2L*`}GgHo|%<( z9e~D57lG8h4!IL(PC_QoG8d?83J@!- z{bVx19E{+HMcFpl@ah4GAJemI;!8o|r3SXWI}WM6kgP-pjX#)%yk|!~21!2Mrei@r zp1Sx0=-gy1$S3*W0#ubET*jJ8Km5r=ZY9~)o^+AyzMv=vB|@+A&yUa)cf0?T=a3YH zFD^Ch5P8MoQL*dPhDENo;6%~k4E{kt{Cr-i`txZ_xYNd=O@MJ7&ISeRz`vz zJ9?H6AeBxs($9jmxvC8sHPX)d`2KPHeTDNCZiqgmY$-_jjH~Fa)@|7`uYaKEpw@v} znV)&J>1z}9Hv>gBOfxSYl07%nDqMJf{m*h_6ppNtDv0mdGx4N|1)NWsk;k!>F*F&R zDnH%d0r)=NYem{le#ZGLSFSvt>&0lO+(|rDuUVFph1a1b2SI(iuzJ#J=`gX8NbrBf zVC@Xtw36MTh|h^%Ru8Zmbz+WDf3*cgS9xNRV@WfQp9pZ!W>(2V^Ox9j!h4~|yunM- zR}ORs1^|NXX#VqA{rJC8__9`Yy@}g`x>tW{O<~F~N-6%Rfg;phH?P6AIuDkj#0j1- zJ)(_b#P>+$vLa%)nAB-&g|h2lz};zC?ZZ@-xV)XEmf8&F`C8n#vpH-r&g`>kdeo1{ zqqzJ6smpadOM(xgmv7EE$A*{w5EDX22;){&caYpkzWGFhK$uNYZw#LC9MLE~F~U7WCz*Pcrx93!7|D~0SB8*U zTW{QsqU6Hetw)162BzVt&IMx?(G26I0$A}=Gd&U=R1aj)9u4@NH&Flw-AP`n*7zbI z(95r%@%(P2lZ=so*U2WwrWJC)S(fNyPQM|U&}%2@50i=)MrAjth6Eo&FsNQ_iM@kE zzv_C8dd_W7_*a|9+&%f@)1BFe>;3EW8rs@Z{3t+dW+bm13krVX+p84vdA&w=Dt5)i z*-@c(Szm!rKI!oXKP<=gB5Fy0OG|$yt!7dXvTX_aO5aUn#@V zA!)Xfm1x1ajxsz1-XeAjJoH{uFH5L);7Ez|Q(P!`QqNHVxOHk-9!hy(_wz*m{ecxe zZFle9&AencbJLCBLo(UrH+LRq)MDD*yZ%)U_c}J`!`^r=I>Q=O3#JhZCCubADz-#k z0|RPhfO)L!m~!{nFe?8YT}{;kMl5>E!QtsdHD7oYndHGV<3IoG2Gaix z{xBCK&u1bGhAd&^{EG6Ysrbwv1UA>XN#e`FlC3cG1e5Kee@=f(3AK;c-N7aGLfMiOrW1PA{6n8ZN#djOuQKL`>@_R)-nO`Ce_exjE3I^G1d(Qy5xvTVid z(bN{7J?Tn}5LodrV;J5!_05T~Z3^s2PJo`FVQ4~u56GpyQejJG3)Sp;AN*%acIwC< z0vGnVv|sxHa{wf-PYla?d{zCt(}4FXo9LmSqD?5LvXlPoI;gpzPa zi3C!#27(_q9*)6~Zaz-6tOoeAVQCj*>iz2dc9d#K2g00ljva_f{UG$7)`cTZ1|k3*{E6|M-Js4|+6#}(|K`UXx7~+rA)U`r|5qTY`k(Hf z93zmcOkzy-wy>z$U+uwL@xrwdY(E!QE?kHBLd$n7lY>-%IC%~>>W3M~$_(lZk_!9R z(yagY)Bfv!MfpJD%M`NugYDF+2k}$0r#3#6;(Ex*z+#$asC$HU@BTD2ixw&+ZJ}_} zxKJBa9JQ`K!vl?kF(b;ypzI7&IytMVcMydw^Uk#7)I1Rf3x6(KPt6uH%&w2OD8Qpa>L`kv$778@6_Be@Wn~ z?i51|Q#YxsX5adA@{h>)^NE*Iy3vKnTv8ge(INh ze%AtZI4$eys@|Aawbj#$OL=5yq}$odEW+k!e`ep)6ILu=uBQ={Rr2m#m(Z-Tck*40 zk;x+8QZ`RbBHf7T2qo<9T4C*eeO*hIoy%-*E}nOY`8GVig`;^e0qACx(kI+jiTrDd zvF4zpvwwR~thEeoKi)oAQHWqrD%*s~6TQ6lfeGnehU??xbFbT4)r|0;eh4hyTz=s&HRW&zF;^=r_4dpk>dC%e= zl%q@ewn(Sb6EuUXr#ASzD^KzRG}u!`!wAevG6#Vvs#aK!U*Ay=7}AdP1}EwEZo$Aj z+4W71)7M^bUkVyz5iYuno|U8J+5UJes@buWLly~$DlAV2)z3VC-VSKYrpu#8k3vp# zyPZukf6l^(!~Px4-V5?P>YAZSuQG%VMk;y8rJZD42O?ZhlacrVK!^|KThcD>B%A*4 zpMp2Apo~Y^)U$P=WAdA2z2{Xk+aW_n(CdYJPa?#JOrSGdz+G-kXQ-1ruQu5ssEd#^ ztx}gVaxeA+XguXO*UeY{^@UZQUWv#9B%Hxql0orte zliufw2Tz|yF*2=rVGKqGKnHr7K86Tt;{7Lb()B+ECKay#!)u#A%K7EW-l+hnBE1(` zKBLP!>MAPM=6dA9uf}pAMsT{t_@pcD6TJ1obfY{ymuGDi`KhoW()ld!v+2=dpZ|Rf zREo6-W<#pmwxrpARrP{6cR77T^OQfM$0IcJ2H%h-sn<#^+^A)|B|Loa_iNmp6t%rd zehwQ$cF2~DTP&fKOUlc!uTi|XO@kl5B5ni+_b1Llp@$Et zt9gN8ACZmdeZJ4YYS?fOU5s6E#JsYEL}*u$8HUxo zvmLAqdB$X_Vpf+e8Jg%ZY!D7GBHlRp<%#x`{J6d_G?@|QU)Z{$*P=z=hAaMht0<_t z?&<5~2e)V@^!^>w4vRPmQeORmF8vcngx+kDNrpIu8T9Dban&Tve9g7}J15Ul1hvB# z(l9i9-8%EXpNYxc8TDJGwwCz>s{Or;NaAO;qi%kySwAZ$mm#ElvCM!_5E+jL1CmT2 zIlh`&j!RgKIEPk!V1}VcJ5kDiq`NIFDjIyDLOPH1ggT8)`*0(=&3d&(#x$>A1S!sW zR=eZn=3TqyJlS+drAmG}(2ERF$QLJ)s#i*i<~oQlX3m$w4Gkb3lai7=wN~qu-Cuo9 zz-WTLdUb`#(${D0w%z}o5}}MppmrPNW5|y<68orgw{urL5)&TyrDG&U2Vjw)`Aj*r z!6vQH+0-nVayhP7uTy7)yvIr=ku*AB#=8+A?a3Q6*M|Q%p-;8{YKWM^k<8M-aKd%W*_5ZYt7_2+jg9f1LaJz-!?F>RT{o4|V5hQUs0U$~bt ztD|ydTP%vNKiS0AotkOo%eZ>Cdal)8ptybE^5x-B3|;+-oq8WtG$Ib}!=_~vys86D zC;cPA%il;cT_hWn+0`wmm<3@c~74SJBw*%<)2;Gg@xIPEPcyza1wB)KJ}6&N;xdw z865ou6#57VsiCYHUB1Be_!Ik|M^X$GNaSn8UxIoLGkYt67(x6vMeD-DV>zlttYa9; z7>mdWQp}<#E$#VcINE7z2Uk#*JEkYF4@CcS9rmEx!_QG5fgP{gxKWQHk4%|L8RfNY zBxuU=vic2Iq&?Y0q*r3}R;0&%)4O;x1x^7dqi(X!ao$QLTb#F8 zzxDDOVU`EhHhf|;8eq^*_}td!l~ZR@v0q0wtr5D^FvG|7_a|6z9vH~Bx~(`lh+*Q2 zYV(#XTBMOxj$MDgR}Z(h<Uhtq=T_@a zvPY5VdJ}_9H_Mgp-D|=F`ATmyH~Hf6E_Wy9Y&HkKDmkCC5$Ag z21UG|HvUUQ;1NoANCVm44yU?9E-I&t{MLShE01Kmj5uJC-bt^XkD54;i8B;wO6!kD zJUPmkAZ;0LmH9Fv(9T+6n*f*pbBVbF#iSaLfAj=@W17 z0A$t5VuX9v>@%x`Qb655L8qMea7WJo%!b+0nnme3U`&Sh%Z$qAN4tKev zR%awE{mI#M8Bd5W{M>!(h7AMbeYB)*roN+S5EkD?rndmR**n%-s1l%dy?3rKpkkmj z)YqAe#$TrH|ke)uprt=3Zk

p`Op0KK!nJw7(a@;dPEL?RFC6}q!%1=IXrk@V-(6bu=C ze6~H*qeX&F%a(ed1K*UC*n>ETJMrzF-x#V^uVuP`7b+d+^y~(2KxvjZB&4b%_k( z&{I*W%;+dDn0p;2+lnaA9cu)W^#4;zuBJKg(lucMRTN9gV|R&q2Mu+n&|WNHudUIk z4jB0b%=prH%uW$F^b`A?H7qaGCkoF-hVvH%g`)dy0dr!TD|6URj%v@6QWSu z5$_nV^5CbnO?Qy>ifFTkiZdY=VH~xslbW2J@DD?#2mlf=fEG>faIb6a3?F6|U2jSt zD=^EZFwp6*cKzBly+%Hj5K? zZ~`8(Ywg)aS62|IIqaLtS4maETiJuqFSrKD=bho`@V4{!*cdM}!-vXbdAkVe>+e#&~#HJ_PxZQ+=0R4m>d$#7^tW*p5I~ zd3h*)#tm1P50A}#aub!XF-uf^}YC3NN-Zh`4A%( zt~dFw_7s?ndttLitj5s)7e2pWwo9=RR>#%&3J#xj)7I-O);%*!6dEGw5J9FSW`S2q zjJSULCAM)ETq`Sj_#rSBB$C>_!-fz2`}e? z+&B9XOn&^Ip#E5sj36bhDTc4KdovRU%fGTqVTV#QLT6vMG11H_7w1?g_ztamV6h~I zgZhF<-sA*`K33QZd4!-`@x^?lWi6HkHIeZbvbE2jVAj1C>vms_epLi-)CBgTvTWM) z@|Cu!5kCK*>IV!^*@N@L1vjgsl0Gx*(j}wyO{n2d2g+^}i)s6Z#{#tJY54f?d&3Ic z0INqdq9BFv+He6DL0nTId-|3HH4(iZI|iH3WJKSos=E>-hib~Jsv>_i!G>*7du9Gt zI8k1up0e?D8ZC!^gQ1qdutuTU9Ag2hPO6ey9mI>J@Dxy@UuUPZoawc6pM!#r0H|oR zc5CNuTi6!&C+%Jsnq=3TxGgu}1{R|t{(f>qUd4&kJ9GJ5TH0icl*O5@{L7U0 zyoQKyRtVxo%R7TlaJp|`P^M?h`&CPJlV%`WRN ztsanCLi@Z*ZA&f6WU*tY)9d|e5!T!f3MVBZv!8ahmCHaW2m$M(X`1D!?Hlfy+$4*A z1s4`8f?5#QL`6dBC+_YiD$2hdV}X`l)j%r}oglvd`^R(($jM|z;4Bi@t=$Uw+n|%G z`~-1yik}yoyO$j>)6A~$K#(SDU7$VA6O?~raqxvte-Mkq<#91ZhJ9F?jZ{rivwLgPDI z`r#MWJv~8o9!Ta4s>Q!4(H*3W7o2}6(MFA;&@9-$ ztt~4a&}NJ?5I+MnD+7`vqArYE4N>x_UqTl?_y`cKHO0Vs@Q4xe0J+VsH&Gr&xumWv z@d8*a-wb%yu6OTkEz1`o$alvDQOHJertDo1Ss9Y3IUES#GueFM@~mVRK}Y6#hdgsAe)3k<#q|+7V&8wkq}UFX5ojYA~C&4=_g_ z`K{yrhg#ciaex1f}dioNbzHw$xf z&8#tKTX+p=f4L0=M*P6PsGU^-q)R0`xbv5p?@@42Qtx&v=;$+jx+8NPYl?9=k_I(! z0kh1s*WxCb`MDVjmi7XgJllWt-Zi8_UvD=*C!;Z9E)U?`6%hLF%)HU!t488(OT)ap za%R1XmOomC)EFKI6V`tKAr2=8|CKtC(dtCddYzj>y3MPN*Csl)9URc`NfB(SU8qzo z9r4M5NKW`1Xf#;^;_7J)^EMw(I*DD4^(qTG%yW`yS&l?XcVh@_sF^q`g-aWzM@dTu zh*k2z=CCL8J*{;Sb)xt_F)g(Z@!IlfzKqXA{TmDP0l%JTkU|VnFKx6j!19|I_i}}^ z&Ys3dDPEOIVk6IU~RF4u=%`XJ@MQgfk{>PE7 zUu2#ENv_?2r_aDWi5}WnV=-apXe4hewgi_6rpTV;$)OIbmAd;5pk{i)+R)JNStWI> z%Zq>#x(lGD)T&TPJ9z`$UVfpq^nl%fRb4-PpVJO1uAwpE|s5yQ{2-X`vJ(28;Ytk#OUiain&9+*%jIv`B@E7f#uf3;q{zI-T4H2hF!3(>c+ zww%(P9DG4?(I}BX$wN3QGw^=%w_wL_$B#izK;YaZr3GXv zvl+Q!MvB;36a;7(uaN@uv#dbA){tWV_fOE-n=cS6%(I!4%;vpA78i-7H=P*1tsBm=5#Mr ze$%7p1kJxcchFfLLfX_n>q*rj&|Tk61ewm%jORR`G77Kz>DunWVKq%VQ4RQrN3J7C z!C`D&biS~z;3{mNys*rwUhk(D65|Dyb2V9X`&$1xz0U1XVi6v}rtR>3(R8PKVbz2; z7B3a@mmcM<=&}iAObrbhsrm#aSthOY{#9|ChJi3#Vr{hLM`CWWNv4HC!xiF@DT4qO zZ~4ZtSP7kUUNQ>EV;JQy)3(djlr|bOyD$IY7+X7&winALy{0eB%&xXWG-XIN+^}ckXhSsGE^jvnUJ}nBr|2oEJFhhLgrAR zBn|IpMT6s9=Y6m1{rA@&r>#?aKhOQ#_Zq(IyS_`Qz#-AJnL_*RP$V@F)|oxDnX=CJ zPAY}PiYa=SrZ6dEh*k~NVK1F7Cz>{E2038H{11IA_c~26{AH;szU}+6#w4OZJ@*)p z>g?u6yTJjltF@JLx)a%p)>`=H%t;>@usIO3aNk5RrMgT^pwWWAiHBpsc<3&|AtiAW zh8qkJ2|A_nNyEJUjsEOG|KB!kuK@$r$GaE@h~!YkqdN#CgT8@}yzHoF6Yvq0CPz#4 zMiY7V7&`+p8pu)ui_Sf`-5tneGUn@Axu@!+WaHcB6EF{B!T+dTn-!gLfDy;5ACcpV zfmOhZ5>rT(cJJA|Y*#7mUA=mhj=P)-QKD^4IYvZ^q*ohg6{5o)@&M79B~=sd4H|&W zyu7@SInHrN=-2+S9*AZgL*_v<;3htn~xGDxN;)`HDe; z&XAYc>9w?pNZYZfA&~M)nGm(n{$%7DfBqGL*)V!aBw6yyWEbdj?dvQ3{C3;dISG|Y zW z!JFLLvGO~UhK6DPA6O`&z0w%3hTB9V2)O9;-(GeE7(^6%Wwrz03<%2xVF1Yp7|35W zkwLcI00hzWpO&K_3#GE&<1m5dM90MzqyB5ctD0Uyk!;={a zBb(MeUWFssW2#s7+a|2~cHdcZ=jMAJGFRL;-PZD_3d(VRyl!(uQf(K|AO(dPu0|6= z-JcWkFinD9(-8*oVO)Ws(P7Jrp~=b$>oCOxhgP_a2cki&Ld3vN1!rzg3lU>+eS^IfWUlXc`zFrkGhE=3s_WhnsI zw?*n4-u;H>ZZCxI%B}ytz!<37*I6f{vUP65a|rrI!K-N^Ya(HGkfpkKE)?vnX;~AV(H3K=)r0_NHu?1c@;T`Z6Y0fmElNd;@{B&EUGSZl z1WnkRB82OGL6LYa?&}~7kyJ5ed5da-GU?f|xj(mTB`xLz(0Zg_Doka~04HTvwl;k2&__$>UuttK8_MUS5FRFb`Gr+NTC(!XBmw@7x-h!F>*b=m6Z zrLi_(vP~Z-sh$0#f1g^NLpkg%b72R`Ui(lN0W=D?dm*Jix?dZ=87G2(bWpTBVD7(e z-5$YGc}U08=SE`kCQr9|PP`XMUm=ZOfquEAN%5r#PQqjn&=bA;JwRN4#w449QDh<#mODe%Sn=YE6dHgY1|TL1-y6^41E=>uLL<^cF;(P7|%` zwCU`5>MD994Nst;A~6H@rr`<47+)LuW%Q44VI}?4m99c4FS1rB(@C%HigI}u39y0P z8c$@bV3pZ9e3J0;9#|R{_mEq zYG&>J`VFFHltCZ8@xMrQPC63q2r>-k`_GL0Ecl#F-~TYRR6jJ@!C}YhQN(5|Z^rpw#Bob*1*xLJqq(VPV!ZX} z)cG!)v-agmdu_+i({-6ZXts-tb!n$nkQ|WXKfFVsIF;2Q>Zd*MnisNbS2xOOKliqj z$3)LxTpV^+qt%%A>h-7v^rg%`gbcjaKUjZLtfXsg-F&#*Hep>@xVD=D$&mp_)~2LI zLmb2I4a7RdBy6C6h3sk%`){gE?mHyDP|ZH$s0gq^>aH4-;EHcUB{ZWGaI2GQV8;6 zq@FoR?OYL6oB}IGnS1L!TuPy};R(oPvVjZuIU-p%N>L?<=(5M>X~Lw*Y9&L%=q7s1 zDp#4`aDtRL$qmD0C10a1L&PG<*s;f+o1sMQ0^@$ifxahGqj-B#(dG>sTH1N#Ms{Wz zTMN6hqoAcrK?vmL!pz&q^+5I$@w-vg3b*Z#@6ip2hh(Z?kHezvM#iu)pfa^Xnb_d_ zaxw!^Ved3eoTjFxCSZ#1q0ds&Bg&rs&Gi6Jwf_7st7Xb_oBIA)tpMN*^A>#O2|2Ux z9$jeJRDdONAu7s%%qIKsQR}aEWaYPa&U#w9VpI>TcN|4Yrb*sZcB=)6ClnQ&+E*74 zBGWq6{i)_BjW$zN%1>~y8VeojWvMCBd)ni3Y@m5; zHI1i95SXm6nj&^vFK-k>>~&GSYWIiYY0apH`$nBO%%R9x>eblX+z$4#=&%2*gR`B* z{6aCfi-8y)IpV{o9D%?@$|LJ*{;!LuXe_M*kTH8424cM4N!=R_d+A{jUE6SD>6{`4 zU7Ird2{3^yE~WKwV@NyMXjl_Y?uY2ua(1WOPuE*P$G^+gyOv=>SjdTP+bJ!&bdSVd z^nasSDEc|G=bU^JbiNM|6$E!_VnBQPY1u!fj0J*BbUk!tk8rf?0C~rVIy+Hik904{ zN8RieeA`V~rUQic#a&0mwK(=@C;c6eGAS|c1ZaS>z*nylM!~zl3Qtf?jAWv7K#@`C zHfZs1mFOixy1hvNqt>W&RI`>X6NG5Xku__3&)coasR2qM{;-2oUNw){B5}%D!NV;LHWv_svv z-g4zz*2D&4Vt9jt`E5AxkjlD1(eS4e@A~_1;z8)xA3{X_vU|-b)dBeKh*k_$fn-+^ z#=$<{Q>v66HC6RP{#3|CGFgX93R0^z4E<}z2Qq(B5+1DLz_xN9MHskZN<$Hx9Zw{> z8rX&{v3iuYK^2pBL~RXE@ED$2Nl^0&yR#`krI+vx#3s=}xYr~QmZlVN$Sd4hVsa?@ zS<}A2@59aU%g>rc$u0v(u{ryi1}bQMbVGfAoR8M=Vj9I!$6Y{2);I4}jj)_DOXuU6 zuVaw3isc!vtFZK@XJO`!*d2&3VuIlWoZyovb^85K*Z;TGBd zpHc`qh-z{bX&Q&*`|VJwgJ(O)f_Jd_5=xxvD3Uz=AbnD8Xni&TTC+%d7c^=b9zyd; z=8(}W{OnSVDXodxWQ+>g^oYhOm9^S*lb}enOnHm-srjVvb118!p>gGsYOjG%Dt$8dOHXq`vR?pQ4m8#xkyS%|HRV$U^F>?oy@d$ zdQ-=&9o0j7)>9RUD`W!bqosP~%9WEzKyg&nCe90`nkp`&0G6hRzW4M5N!me+B@sFH zx8rG8YEpE;9cbFgTwdwEm98=X2h+z35JN}-wv7Yjk3?mEOk0&&Gaw@r*fL_ajUnwZ zNrJHcS!;|nn{4#q&wzpyX@|Eu^F_Uu&Vtu_HoLdUR5R`+&IhE)>(n91P?RZSOFRJq zeoUM0{5_G}peb4>KOFtr_f;&U zQ{mQ4nKI?XHYYeuq6~C~+&Lyux((e=yws3k-~^}v)Nu!mY7SO%k5YFARi}r!;d2n> zqcdcD8)6AcNe4jqrGh{x?6D%ZB@`>+xe^OCjCEFdYD)$yQbcV?qTc2^rrt_+J$o-d z-#ntv_&)|t^z<&|0;o4=uyRG)W(nKZt*8=j6(2CdwRw%j>mD{ew>G?0xVOuP6+>${ z2Ja4S9k;aNM9a}2U8xVh12>A4G&g}Nn%jc{2j!I3~-Ag_Y?J@WMG zO)I3mqvF_)XHkPWWlEDqjfP+B(68|tnP!o&4d8AN)#Q+=9#60ECoF6>iE9mP+h4 zsdKy4FR^qGBlnYE3VtIc7(sHq)vD_0>Yh*2CQ?oy$Ff~3Gn4PPcG4}pp{IAUzJISX za3(~oYq7EGBaHI3@>gk|*l>AYSy#o4AO;e7cmAq*V!E1Q#Xu&vss(SQ&m%gl4!LIm zIf|OzW-~iLIkK|jzlz(*M?|{`vk!{m1xOxtf&M;Sh%R1`lCg~}-?f5<2DZXJwGI!A zq(uFW`VTs7?nk%fkJ-|meUb6@UQf#0Qwh25U!Db<7gd~4cG}3tJEQVn;i#n31+XP# zK5}S;!FnQwN!WYjP3#G({6dsX67TQ_MUYADykvQZ)31RqJ@ zJxq2)py)=xq&h+&U@y@iog(+&eZ0ftlc`xUZyEpU;GAxSbnDGen~p;C5p3m$y6V@l z=3n8a{1p(e6GH?bd6j~zgRFzv#beEXmN}V+M#MJW>M@Zm;;S-dVM$JT388H#<=s`c ze~bYIT_v*d)jc#nIi@li&(+!}f+@?IYYzR+hAg9YYIagZJuk>F|@K6Lv*|z=?Sc3Dt!f z2>cs=YUX6>4gFTGT&dBzG~OWrroA+YPGozwGfv_}GJ@^8^gc&q-lX*c3oX}=+KxVA zj8sK5R$m2hok2f>MCY*YP|MU9u*C2bRl%-?7P#9g^*g&cuOf>0Sx0YiblO>(ri9%f zWrs89K?yq9-C)Z1PS9K>S(S|dxuwlMeRY7X!^1VaqVO6Jn-U3O(0X=HFnSW@WhX2f zv>Z_oY0P_ndF_<|5RRo+C=53=EMgm^Iinq_7CisML*r6;^@#}Vtw#R9E>p9<~ed?9xR=<-#jTM4?=_JEP+vjpmGjJMlo?|Cg`m5VB(@ z+Yc0;uAMu#(BG#Qv1#Q{67yz0-{y@37xBr6=tN92+$Y~JvS7x$m z=#`qKC$ub@N5btc6Z^*>_iz;3L}^mmN_Patds%bDWjWDc7JYnuc=k|6?Yro%7|auU zFh^LklAl9%%0OU%2sbHg3?2SM=FB-Mx&=@Iz;c>)?3i7v5)*wCiVaJRN-VhVsDN2` zE|I)p`-`BZl}H?3zjJ4`Ql&X*$`DIRx0h-$$WE5LB9Mi5e3gn7b?xtm5S9v_C>U+O zKqHcW2~cyZ{<7lo55wS}Gg-Xj(amYupjHs!`o_MEiJNK<^dcydrY0gqgXgC~suPHU zNZk&NJ>~tQMWj~%E=F`x_6dH%(vl{uIFcx-2uW#4wgt0FrPzoNbeY3j@Laa&dQ;f=M;!i>NEi!n!9J+hd7iB>Frh7J zscRmX{`M|qvBLl@u1B}A72>GtWdNV;2jh|I4f6-Kb9~9TKkP{1m=2JEn(!6Tqy6?d zd1<3ttfO+gebK}v#i}bwNl9T30Sxq>nomKJQDp19<5tS-o#=9dX$A5>DY7mgw=dq` zs2^D>Y#u4Fafm0gZ&Wqgy~TA&sgT;>gwIZ+QDWGU>iV1IQY^SGgYmG)mM&Gb+AN9f zQ`3lIkg&-qjqnf zc2WjmQhY>21cHgka)0vKJx9<&sJSvXo>1r=Csc}1Qj7p5mv)+u>y1S&UeGXj`r)mf zP)$q3)Te(wL}4+MKe#@wji}yActpwoF}NkeBiX9CW7DmFHeDc6P7QO6$fUB#)-s<^ zKuyTSfVs5>xGB_LK7G0!cyBZsdb$hN<%uk5nII|?F;VFXfNw(- z%MIvau{(6^cr89Yqt*e$BxTom>mpgNeq z61C`^5}isffJ4UodGl1Y+Bpe(4n-a5?fC3ek2(P0VNAcD!c0Fts^Gs9L^b7t3k^D( zlr1a(*ei#cLzkA)9vHrc#~7#}mFssN1S zE-0d53nTVDLPV;*bA?dI<339iwxIE~3iy~vcmr4)WOk&1YCE`v+3l$7(^`alh`hAuSM8=|3 z60IKx`q8&%d1t<#1BzK5QT3T}NTl zvAw%r(a?cHE|lkx1sGl!EbcZ=o=o@v&K9kt;8TL98CcCL!?TuF;LuMGhd(SEt}R8~)uhSg+u2kLm#A&_2fZhnmW0)F+AE1mIVX?i6~1rLcY9Aj z1YJuH6%!hc#p++X1>fNK2m8W=k*uXNV zykAK7^I>nv?tFaQSDe&GzhDKG{hZ#uO$)2c`o+pyLSf!Mg})%z?}%?2y(*D zJGoTyP73x=6qWT-d6i|`a7fhl*F)Z*XV1K>GGTuZrMBPYj#l#7?03yem9xJihS4%* zee+=}{T(7P5|V-dUJc@T9o{^xJe4B)U#5C`PAJ;Mh0CT;J^&CBk;b;WJVCT%V9A3; zW(-1yDcmk(0isn*gmqc%^c9&p@Msyc+FI3fCrt6|7i$L0(k`Xceg5LbF6?xyV7@X6 zSVy^$;-o8&vy5!*?N;e6q6$CHtQ>GK!1~|*4@E-|v#k0pY72^tgnQm`3ve+cZVdpS zyyKrlI8`MgaHf>!olsR(RcD;-Zem<$g&s01^6}RBbl5htJ8NcH_~ymmS6pJH)q-WB zK_?Uzmbi5xeM5)5mE~NwN5}7sjZLNFo~+gxa+2EOt^3BDIVqw!Bm&jTDQd1JKoN2v zOj=#F=)TCsYh76Px5V{f0}Np8{P=p>+N47VD9`=9<7hQJ#F^`PCI2OEm-vMdCHBz> zb1%^xIu{#{Noq7sq4Ac&BD{#dc67ptsMa1T^4fAMz^fBxI@Ko|*@mceLDX7&EApzQ z4>bo6x`$!e2Difd%JV;x$2k?Xv+&n5Jw0iiIdcR`D?v}W^Hms{Fp(R>{nm9*SX8fo z(f}e@h;0tgRBD-+nCLTtj}YO9c4GIER$vd2lIBoTW8=S+`IZ!>M0d+JmqS#&jlHG7 zDnyl*F2cjZl5>W1>58Z3@b_(P;{P(0gXh#wr|nJjCLS_Z?D5tpR#X&XSbfdk5eRF8 z{(+rCAzjuPh!uE9(@&Fel?KB_CfB%mTbrWCk*vDH>4zy=ds|t9#dt;0BZ1r}kEuGv zNihwSM5_BNRWV{Bf#`;cq~Lj3wY2-Tkkx*CMLU8Vu5gH?|9u9rOxL{ilcREn!WC}6 zD-$@GRJGR?rQf%Efq5KmR72%aaevQcj@O7N8Z*$TZ{%!?R+*^dOOGqhH1fKdhJo#N z9)!_9DrkJ@_WSRXq6nbK*k6C{uR%J{MLhG7%XE@JTC`6e&d@3u35En(*~RIqgb90v zph!9X=!%pXQ>PB32!}CmI^)B`zv^VEQcK!RPSqUJp`pSmfMn$-?ZlaMq|8_)1uMbA zIn52!uD~*(9!0}t69>8fS*O2#DYBH&$rUPA6!yE?{(`(otq%A!K3{kO;Kwjd|HryxvW z*@Q%m$5o=^cC=+mDsxNVd<)rnRKh#SWaqy5QoRDGLo;~r5R_|n5pJYsObO$=hSA9I0o-hJp@&vGyo*QwR*2~#3*fb> zYK3)y86t{nAq!AdXJIPTR$elpBar<&-gy;l=uO)l(t-M@u!Ta9y6kSW?8h8%!oO$> zc^JQZDaU?BgibdJXf1=QMg$`&ID#UaW1L>$vN^jHqhf_t(8heSCzD-Y##`Q z4Mm_=>~P=4HFlWof**Ybtrkgaq!&-pwCmS>?9-Y9q;U*4(T~x;fD5JlVZK>$j+aH7 zkgM$nO8L!OwTk^{lt)=j<|ft|Xxgk3$zS?tA3Z#VVcx9 zj8B)ZufDJmJ#29#&C<69&*k9^4R}Ip_wH?C`CD+LSRwq3$_2yqg2!x|&`LTCopTZJ zw|zXb&GC~b!GVTmzmJGL!j=VO|2ub!8JkIX|Dtrj3}86_R|!xj+#1Hj13^QiQb*2x zy~g5QjP{d0W?3+3DU?Y1=_|BXuutJ>3V&3PA-vO)Pv(93LmBK1N+JOmb%lpJ`o)JSeCuw_P;c?1B~=~sl%ZCbN*w* z8(PtEbr-FFTQTL>^$6e#Z0RC;#+>#gZe`)x8&j&ZoJ+s=y#q$gMxQ()9WQj+_9h}p zm=@c~>7LOYYNsNpO4)1^-5SC#yaqPtu3ZC2O_bt5&!K4_1lIif%k0IA79oQUp`)#c z|6{d<#xh`~V#OAmoGA0*2V}k^qff#fPe_O3=0a>M`9GSk1xJ%Qe1mh2mEZ{g%?K9B z`B4{tr_5ho9N5=f-MHT-q7c%*-b`ox+l`co!%7r;Wn7!qgvQgLUQh3}Q}0aXLsv9G zblen|UIBRs=<;knXzqafVJ3v`G9+Y2YMqcjDZ1q$vwZ-yb+zAJy1pmnWB_x9e$=g7 zSM@>fBW@4w-p&?I0)_tg*NoZ>rKI0WlpRpw*-sk@ETwymCx}y*`nmMPbAZ8${u)U1ygq<`HScvIR* zg>5F{!h2%t(qJL(JtDUAAT9`g!cH9Peh5u!WmY_js@0jfqeH2i+OSL&qyZp0b!ubD zh6}z{2oi?QSP+R_oq=9Ri--yjNW+I_iNc5O-1$jZHo?leprD4O)$Sh$R}xS{V%rQ& z!aMjUB`VlrmL(Y^<%fOx8aqav zm?|SKz}GVHknf~X0pg985Oxz`*Ov>_V#5arlul`;N7BEUnm=tM`S9>sjEk*FWs(s zcGjFZ(ygo3%Dy~Y%z{36_JwiCW$cg8|Cpz-?qzUyohD6s@-zU%nmOFxfDx5Mn?zQI zh574-ImK!-Q}%vZXJ*kn{Z?5cnV??aad=G?lI`%PAmDu#{n!gd|CIqZc!$dSWpc>< zA?v6#z-(*!>16-@{ng$%iU39&*@*5T*M5OBfaTkb>%4XAhtICf`mEJImwaZK*6JpK zlfqkKpW2P9l84y%QvXotDG+Kk1)|ys7qB7|B9N{E+&&;J~7L+GBFTN|%>2bi14F$l)%lQsYf zO{iW#h&% zY1*Qt2)0g8VEJ^_GhqS}-hbiAh&L-UcJbF^q^?qAkmgcI*QavR=GmB_H!0N1bz?-` zeTwZ8nsY=X4t!I`*NalJaMiA@e_8a*f4u0GKY8_zGcAnTu2{Udbfpa&run>l^XAID zPqU6Wzo-B}xNhasTI@#EXRSN1c{$|=AV5iR54j^5AwhrrBFTV+;*TGbE|Z)QWZ=?Z z&r_iww2vi4<$m0bN!4W#hBVhiEtr2MFFOVAvQ7d6kmTkF>C^ugE+ObTS7ON^jQ4g} z+xr+8)MjJfp^@Y2n&p?yzlu1I40-frB3*utlZZpe7yzn{Qc@HdSI9NZ*SQ*P>?`|) z3z1x`Z29uRypLz@mX!DJZ_Mr6e9o;y!wzl374M}cA&T(@x53aCQc7p+35ijQfH90h z5RGQx|JSU!i{>AFLqlsB%a@-1Y(`UgzD-l#E-PG6<<4Z9zG$6+ZF12+Tl%+oV+zXp z9VpCAUngKS_Pz3lbLvLXXV)@bTACIRv{L(Uk9m`Vg>z?1^85i5j=-Y;JsMLm)30l| z18opu6Q!s^S6ML0?2=FZB;`R}{)%r<$OWk0um{~N_tM5dQV0Mg+!1u2S$5sX0Yrw_ zA!jK;_BA}NPT>`o_-;x~bSHFPSt)p3(wQ$ln&grYa^%U`pZ6TJi#jMx1v2rE*k?K+ zkW3K~!ZuVDGA%C~BSK}NC#$Kg)qN+r$ftI?GclXM( z;{|*rWl8Cp>6 za({Gxf{}r~zAzIAlE&Ju0c)v za?94OUSpiIwqqj2Oay2(AHqyQ!PTh}Rn5u&_I7XJ3kHSEQlyn<6p-{2n43qH$1X8z z1^h?Abl7xj^?sBCh7gcLB561{ZA3=(F~RbQVeY756@KChTNWCZMGnbCT>)t(7*PS- z3){iP8M5h|D-8}yq!QZajFf zvFvhBZ@3`VU%1)9puE%}nbp!$o-ZBHkPvMB3;s52WX}4cyHb;;LCci-rk;JFUJnDF zygB|WgvQ!ubH)K55CfE3T=Bm2t`c}9rFF*H6VTZ7?p<+5zBgt?oEJWS5G*r>F{+l8 z5;M+QPn++1(%l=$#Ipu zx1X==J+jJ~7XDn#8WP8a^|Rm;_JBx#mrP|+<71?_(x>U&$_u2(+-HHpRD&}Q?!0lg z%$XDm%X$+6sGrSCT67Frn)oD$e}e}ORNwJ*7&Q$UwkyNHh@~8dKaXP1$Slft_x7%O zFefu5aR9WtC9B4IFil40rQj56IOKRE8G(t8H3@GSdvxd+#8px88%I9K;OQ;53)tp3CSLws^{i!q;FC@7XBLu z{wB=hO`YcjGEoH3q2LlSmqhB|k0*G1&zuCRcP^w7O#W70-zr_-?chmtA%G&XT9v)L z>_a(waa6MZ8Hp8MuzXi`e?Ibk;SUV0=vx+z?b=VAkXM#YrXj7K(6Ba`pg_o(mfr~= zUD!Y{Q0y&KOkV>JLs{As`5G8lm=U7scJOdZeSK>b)dY0{=^24ykb@%qHgrWGvsQn# zvKu3L4!?jGdjBd%gy|5%FlHoVMCR7E69Z(@gW`M$+q*4Q0_ZX;3>WldC*nV5*%i`8 z3a`23cb^jF0X2fj^3?F7(>fFD2n%8cD;9iRpPh|Bo_8zsoZz+NjEpuY^m@@PmVlqR z`EVK$1|%Kf0EW&v@nNydN)&PhSnYmNAi`YxH`aQRxKeJQfwn(`3{GL?CuMsa0eR(lX0VOn$_??GKul1CR1P2z0#qi02U7+_1QhayUv4w>oU7@lEA@+_e$&PX)y zBwQKtVjYEaiABZK&=W`zBZOULXsth#Ql>Bp_zN9!0O2JQkHiD&weYMyu}hbQ?Bw0# znus^|1^^Z~am9x@1fa%fJQ9E#SsPx@fgjRerYylsT9~sC?ux|HQ>VW;H1|KYZ1kt} z(BXy4u%Qc>eI$d0BQi&kfXMsPc{Q3rA-1u0r0^tR!ntsHa{2@Y`ewo@fGtA8@|LPk ztBMicWIEG5e9Q&n_b&*;3*|&IOt?!l3p&|!V3+sHXZY4$YVpmWyvX@QC(YKatdH@` zkE2BFoRA)o-2K8E5s^p&Whi~Ej;3UTxfTlhh!{4PmO&3>vqp_}-!~eGjtfzxrnV-- zatCnA;u!qYRWpAvc+P|y8ye8&alY^t|K+=K$dqCjAr9*3P6=me(T03nP>E)E zhj7SfB%APd{^wWy1Vy9(KZ3C6fl>R7eMNMR_)^P4;YIk-h4Yt4-rJT2v$1E^L6j3Y zfB-0nMeY&_y3yok_O|wsRelujbDqzT{t-#{ zg?mHVOQ^LbV%nYjAuA*p$24H_GKUEwMuF;;V9u+3ywHASl=%Hm zWTTPtTo{6TF+N^;-%rl_cKDLPBpOxT@Ew6D;bsgz5k34uj7ZSD$X&AVsG#SD>W= zG4#iwz(ai)7G;M#h=dp<3qSK8#(5k9M|~rNCiV=R5D^vexiO37N7uLXg>p|N$(;ofJ|JJsksEy4$+1ZULBpZ88o!3W>u&jc9fRuQ24a| z<$LtI;s>Z4YfVQePA)~7UJ)+1G}?}O6ZS8(&EHaHXZ{Xsp|Q6WW{1h8r0NOcb{nUq zT1MGVyoBTuaIiKR+Hd&zlx|}HRg)WpfX6movs~w2uAuNo_J7qa;;|^7+YpH2@A^?I zTw>FW4w_v*rms{lr@TBA_bx@!A2aIOHNjJmDi5=;P_PKN7nx*NNxlAEUo9g+7ymQ9 zj2~NE*trBZc_1x~6@1ivqWv3K$`o(_wHoD=y2%7MfZQ^RXNgMm1Rg>U5vu^_QZ4N* zITx6AFc(oH#!|*0$grvM<;(x4DgNb?Logr`6i~r)FxyEn%9UP%X2TEbIVkj}N`pHs z)Sa{6G(4}c&mhi}P>x-+jdg~FDvi20A`J(#B}8>}RBZnGk6zotH7@ubHm4YnjV=Z6 ze>6gt)*vQ4C#gnx<)+xZ{P>X?p;|J~A++Cm<_kvAkyUr0BCNNRD<#BYV(fXPgGh+r z+uqTgtLM9vIr9~X`TjG+3ehe$xnb2P!ev91mVpiDXIY4Ph|K)s8B)`Ym__V?w(>5T z;(QDhwto>0I0KXztn=Q%Hv9r!0*y7$Q2VZF>ggb&dG$h0$1k`0^B-=dN_?)4RA%Lo zLHtmUsV}WrfJ#C+=+N=g<$8)6*GXY9;-GwiWGfXeTA*fLs)1q^(}9Kpz_>g+Ht1*P6N zIeE_;fWFB@LTqC5|Fi=WitT`+l9YKtbl;LT;SZ`V%(+etM6i?SXbZMrbxlwVwL&l5& zS7kuQYNMtw*aU%rvfUv@(!Aj*z^H4uHmMqjvWRQZMADmaOo4e{nxI2b&39`hRn-Ck zs5u!fY@q&fj*z;m@JfZ#C^@<8W1a%zT6^dl24ga+pSP@SzTh-2x99K0sp2gh)~kf6 z=#82%wXrcmN&rZAx#wiOv9A>SM5JIJoI#&5obNQRfr^b34q<8(j3avI#>1C;kd6}BG%zLVREc&0 zuhyrWfl<7Rc@x40{GE9BTS*`U;vj+GWS}9e*zM#&SMRJg-xIUgW9rlZieQkS1f&q* z3@NY(*RH9){*{9f%Zc6ij2YlKyxS0h6*aXL@qi8$o~cl~`H?oRF)UvG-(J~AY$@`Q ziWITVgpHJwQUX4<>}Tk@lL(n@lblgaWcqTB67>3&y}; zUD@FeZfMa3m6!9{JXlD~@AmHB+Jnx(vNZLF-m#v_#E&3P2H~{Qqo_NG`T3#g6;k?jE zacolry;c%A3NNarMrMBa%tPTb62~hp2Tq&w@ZR;4M;+Jxb?~usV(6)pO~>4|SyG|@ z-F~H2y7$v@+tfj&RjUnSmyHW}Q%C*JDK{6bzIeT@&i2z?x~@GQv(WOYzn{;?QjhC? z=y-DI$pp{f$AeMpC-SI%y3zKqc7^Xdp}zmISH^MNX8kIR%w;gCWO^w62$<0I%7h5-H0`tLev>c0HqW2E__sw+=(L*k z`iDK)u%ub@=E;jHwOm%I<#C;}+3|;B;MXiLQ~$zPiAxPvnYH=oq~EI5Dq4eEcFsCi zH3p*)v|~pDfl4|kngDxfW@Kd8*xA)b0nH(VmKHkN^WMCF?+Zn}8->NXX0r_O1GOM3 zEXVZ4@VZ8)3tqa*$m@O4B;6Mr>@)3FhbFg}MXBGsd4yhLU$=I3&`27y1x{Kxr|USd zM-cp~lP6C;M;T}t#qvu@d6~y%(bp7fYnGmy>g9DLGV;>S?mlB_a)5!(rH(GD~)p2>P)Z>zp>W>~h`p$y~T3fen zy%Qh*2ebhnh%vK3*^^g!oOJlou^Kj zvOZ%Wj>~mJ?!NQnN#3?tBZxtAyvXqPg@*b8KwSoctOtm=>c9c!?Cx7e?!NE9ffds` zPHNq^BKKgpq{fC0(-n%k}c2XryU<@WiXy|9WJW7xnQf zf`)SPcpl}_C&Lh$==SJ%YgSn8cU)X~TCa%LtMDDs?5`Cv0oT(31`$Pn+} zorlE}^&;xFB4!*~0^ZfCN9DY16?Edn1D#)2LH>2cRkz4vMsFavXhP*|Q=-vvI?|FI z>wn)+IEc37pMKg_+;wl*`0ezLGzf1x_BHC6cb&16##i^=J+F1i-~aL2|7*7*@L1XU z`|YKjYvR>YKR$l=ur?`->$r&Fc14R?{O8q*FVLbCTUwp+{qG&+?; z=s196i+UyB|HtbmJ&J=Ts~bRXK|6Pj9hl?+W)zN7wc0&U$sdashP8XMFjhdA^JX__o#Fd*xI*ykhvm7DJ~$iZ_sYVnTw3m@rc8 zUeop>=w*nVGN@H_89!d=EY1BKnD0w(Ws;5}Muv%bD}0yVXjb@Z{>QP)!$x^@2lMQ^ z;=4#6dC+k_(xC7cioc&O|ID7rFc1+pOocqBKdDzAx$UnKP7(_{(y_`r$goe^dBsOQ{ln`)GWd zjak=*omEfD{W!D9#o1nYp96bZS}wly&qoMDuW5;c-QR~k&*KP&K1Yo9%G+sq@14wj z)~(a1^e<+_=89&~yG=PsuVsN#gcJr=vu4`sQ&6Fk4+I}PtY3JAKff#MuI;^h=dN8B zb?nvQEw?RmC@5NQR=pCtBPL!v6IgV1VC?))YXW`ka-P=`e$JbeljFQoh%rAPaZR+O^yKdfb)w_MJT0QO4m_Qv?kjKK$*BnkFqu zKHB&vPqaUL_>h`4H!$mBljp;tvWLLI+IGi?l-8KC-B@;>M!6ST$oemlj{Y9QDJwK6 zVYfFPp7QLxEoA*IOdoZ6ulB&LUFWYSVo|C7^Ur@Q2LFuKEMdi+m@#%wY@NrQ?zOsi z>2hx3{EztFtS@`b>){E*3QGFFzK4~v*l13Pfi85hd#%wmL2-Suv$OAIo+kBJ5fjs+ z(yxCd|57d~VVF7uu|@|^eLQ*m-GShp2M)N53QP&;({ee8jaJ9s|5i`mv1pMkTK3vK zFr0gKc6VP%%+=ti+XHoKV}0MJSE^Jgc~D^XU;`%BhZSv?qPNp=C}D*AGyZWI^^BPB zgqQb$Duj5RPRxB7T&`y>ssGJzSYU((ZS0HQq3DHX`I2wdXZOH6_wQ@^`T4arG^_^G z*o9$r&4tQw3BW1obLxPNZ5m08j?wqow5fLK-%Y*JqlBOAs|kw`tOohhB1~(h8 zI;N+}p`fDQ{P|+a-QrF9&;Q52mECsv{mYMweq^<%cw-d(u=s_)YO%p6{%O%Go$AHw zS@gr=7Z$qzmuoArwTl0$=+*LS#m=DUhs7_{8x%XT;-411((7969gBWg{K9I<|8Bf^ zHHu&T?}-<$MaftH+v+7(qxkjz&Uo=!lzgS^_CMoRay^P(|9@I_-Qtj<=q(FhL~l$0 z1If+JZEa>&w{+>!t=qJrbwh?XTU!gzW;0?$?Tx-))DfMwGQG>er%FN76kJ;M}&Hvykkdf{2DGoQbbuzvC_QAzWfXfC{)eA5Y>blX@}!(s?bAai&u{L@tcNvDr^zCd zE7@grZQHi3t){~Fzb?T(j=!nSZ}daaE^w`#(xKDW=i-QDe_3{FO(FnmvE}J4NbvXJ>s;r0rG9%6HlE=PR0zg%q() zO;N$>*I@CtKTUJjreE$jJi8_za_M2zqCzzVmc{UP3g0dd|0PlQ`3}mN)5y$^BOKKv zqh26eY!*tS26YrMR{yc4vxXtDJqvWdZ}waU05*6s?{mLO<&~}fD~#*E@7S^S?c5)2 zs5SEg;4AEohj&%%)&A?T*N|b#RNxv;{`j3{$B`WszW=pHr*wsw6dU}#GorJ1?%i`t zR~YvGkEk`ef^cqU&xD@p?o(cA^Pd~R8QhXzl^NU`RTV)ca)IBT#FpEX|DQbm25>6B zy0&!@Bw&Ap;*b_dd3)6j7iqoIkY3Vd9-k7U@{Az@xmGh=E(1nyb$DHO@vm=d-!(Wi z^yVp3lr=_}DSQKpPpY8{0|Z96JwBP?yKM&;YsZcq3ovLH8|ZcWK|t@`O{T6$GBRVv zjJU+a%RmPW*Q7fRn$m33rcK?{FW;dO>qzEPPGybl{P|~!-}}ck)OdpI>F1BP0IKDF zUOYZqcx}oGBC-@tEqoT*dCw69U;gCDlQns#u)yIkSF^74?5vKgf_H_&zbO3Pq(S|p zU`uvbkDtX3`5zmqk)n>JP?Szl;M>~s%ko8HhB0=vVLgljCA)}jrzW*j@87c5RP>#6 zQ$Ip}cl-6JTl5z@OsVrCs&@4Ov6q{@;_;Zl{||=`nsvjea}BPxT;;+mEBxNXaQwBk zr%y$&8oniPNdGk`mOf{m#C$WjZp$xKPjZJ`l^$KX>P1S{J3~Tl-2SJ#d0OwqNWE|> z5&*CU+KL`ci?IQVgR^0;CX;8c5cg0?FRv%P8E#KbccAW|rd@a~NCw%xe>{8oUoYd$ zKW<&v)PLb@|7+WOl<8j9rsn?V-9kTi`@^YQch^Ygks2DolV%SI8Cq#sT(c_2Yj){= z)}{J>)gje4saI0fcOJY;S+k3(UYSzga?>+kCY~GNKdtN3TQfpmju>|`b*@+Dnk|mr zP6hdB^e~}#jEJ9mrNL6?j*9mxj1KyFHOktKY}?ij=a+0~J8>ZB_5O#Mxpas>=iw|n zaG+Vg;;a7gO6j#4%HhzAkC0|JRZ+~1mRT}CzP9(!Z`yP@W#t$^y{KWCcF=ZDb0}*U zepulzD)L3TDfFE&TPCyb(IRuE9QTeLIQ`A-XRb-F!4-X(|Jj;$+#|zcVb?t}fg&2j zW3JtE#D-3DJvjrFc6$GfjCHcIbd_pl;w*?5!s$EdQuH|mFZ@5k zZ>>;R4E#Xp$b+_Iua|#IRSFWq3c#tzm@QugC4$s%RHfq^otq6`GHR5`?<1t2)`RJ{ z6-&8n#~|M>zV#oUc(<$6whxFVw97x1`HXC7aao$}+-&_iCbGSr3jzbbqZ?<$SlV?; zer@;80vt((G@*WROc$&Z zxw&<a)~oOOjw(7b5vB0|2JE5sh00J3%zRUuSje)^Wl64G>wWW| zV3Vch=Fua3cf#0F)#P6RC z_WQcfk&(Qt;mT^=)Mcc6_pK5kkGAX;fD(|^*cKiatM_C&nla7x{X>~S(Y)xX`cHu@ z??-XJ(Qa2WdFH4(Z~Et6wHUDv894N08oo>sYO$N*zGbm~x}rjR3eNYT?)OeS&ePXy z>ufFb%nOAFvGBoIRa#ol)Am~82B`dZ6k$tCJTrA>6}6@PXRAi!tqrXkO58{#JaJax zpB8=}_=HEf`IGT0pf?{NxFFlGM@^z9ecowfvzODC=TKS^Cz6>~Z?hrM)}bNhu6$g{bz@v$T`bPT;orZW{d#5t z9RPKw-|o_~=;L%64H-H#28&weoc8q_H+;FeE^*cHSw4)v)tj-3V4xc^>q{y}w;xKH zNX(ejd~Tf^r7ijmw6Up<1ZdMi1BMPAN(YD2>+7_YF>PBS;{4)bnn z6F9ek&}=&!}?H)e$Mt36#U~~xooC-Tzj0V!exo@J<@4U zG2QLzhBK%;^-6<+ybL0nI-RJinW6%F^A2)+&Ubh9q8`=_)fw7@x>GT1$m9U2{KW4= z>0oz?h{cRkSSdG{Bi@Ajec)U!l3msUJi|EKdGbTXJ+G}V#?MURjp_J`O|A^X!M|j*$|q) zJv&=Lt6tO5cCf;KxvjGU`FY5w6K&*?+hY4}Y%*?6Z_?T$xw75&ZkgI!R87xjgih{G zrDi8oBQ{HfGbHqJPT%hg)rDUXgGynIv+msBmZ{g8mMW)gdyEE}204%E=}v~*w{lJY zj=eMs4=&%+D$gg%Y&|b4?aUl+`k6XQ4U6`t#A=Ft-FZ&c5w7%l#Qrn2BPmb3G;!}% z4ca<=cmkD(+|TYnaO@5Lkg-eS=EL%em;Ev`;Kxp|-z(Odt~mNmw9?_QH0DqTPfCR{ zc@f>mv!5nWCzGvVRK@gtn6xHsAntR80`+^2~T8))XGR@*mrfe&k%JdyBM@8l`>6+^ctzj$&H}c@y)Y-F7NaSaH zgPlLDL5>fbPD-I;QO@UF)mT_!fur4JSF(kV_SiC>J#{C_D}Nsf6EO;l6`JkcNBP!i z-%?kXIi=dKw@m9V5138+XK9OTBbXI=KJf;Q5BpiE)?I6$ih)OuVh1aiQvlg(;z~75L!E=v!c179os>5hsp#w;JY-0B972t=A7yWT66 z*N$=X`t5Hjy_T$>&Y+*R>+N{9Njh7u2eGm|$8sY%z+zHs{9o-E+XU9xR7%m*9VtTGbn5Y^iz+>bd9s{bVh2yp>o&BhwIMPMm{S}C zf>JbvWY=;j!Pd`9Tlxlm#-k}0DszH$cCi2cOfx=+1`j-;J*-vHmwhccKVULLgKkYn z?>GB<1%*`&)DM`=Eh;M?`HTqi&osMvsdJM)U-mEhIG#Zz^w$GE-xkLdVaH z)hVz%lfAzG4H^|;L5^2n2Gcc4A)^U;Rmx(~IG-PHq%(s)oQ7lOeu$KiiCJ)aCRLFt zZt%gA&Gye#tJkqD@<|_dmbS8lt0QUml8_ls7nbiq9B#coGO*ITGmxJy!b{VnOLIED zK()yA@Bw*?A*d%@nbwaAC{=!w%B7TTjht_9pM}!Ob4g4YwWdNaYPC$44YS=xN+U`g+wmcDG6^aM+ALxrtp<(`utP;;7qx8-@RbF-ZIiQZY6sE8PQ) z;2xewN1iUN@P8sb@`n%71L)jRp|>n`yNdC7#*ozFj%r-=5@))1tLF$gw-EXQoeB2Z zx+WON1O+6+9(Hgm0QBtn?E>~ESfPW1!XiLMaHv+x|86wN^Ck*>HOkkUZavE5(jVtb zT&>cpwO3w_EuA!eoszb6nD#s*;?`Y#h1DP-ZJqx5WUdFZVfMVY#+={(LrCy7`kOSM z!*KP$87L}ro2H`Eg?MmZcecDEhABV&gYXi+e0*#Q4=I@w=nHwN{o#oD`^)pG%IOjG z8VdI(neN6~N{=m z=rL}>1Q!j(uKfsoT%_luZdJ213lwZd=qMDsLN8P?HF)W*2h?Zl$o1v4t*rB>1j*?a z>gO4r{r)&LM)HE5^=1r~Q;<$@ahdG2Ou?p{XpcqlfW=q`xq^7KLcAtd(&#=uc zPrW~!g=|Avl@gcc^_A%{7cm#ZgJQF2?(NyD*YXJ}7MnJkc#IXkMLIVMT}62$9+9K9 z!L&Bp$wjefRu0tgThB^+YC@_g?064Coy+y%ibL|OQ9!5KwVjP2X0k8Bn3@V}KX!;- znPtm}Ev4V9On4nU>GLM$-+BSr3@_<&t4S>)^7$49pRSvrqG(O`x?mB>j3cN?8ZvRe zsB#S(gyKTXkHh5OLs+RMACD*R#B-&eO3G~s-P8yJ2<)&zq8*_3sO3ww=U8zeHzBvS zER%JLw;QE;O4Q=gNrd3;-)|r~d4*qhM_>`S_8mjF%pm>bYsW-|1#bNEaHp+*km#`$ zbw0>m-uydV@Fi88i$KAc$ci0DjWAeGB+zsvl#9B+@wLk!i@2aL%71@d=Tc-y+ z+mRG4%o?xkdx1OYJpbjAThJ_YYUtLlRlTwna{9>gxtK+Ju{kx73KB}i5%tSbp4bJDTLja3!*D?yEY z4)~;ACdbFJYNxj1=)rYr#JJP?<0njLc>sj|%lV+9FRSKYvFTSDu8KQgl!Seg0!5E8 zW44tm<*$D5^NAc>G*C7fYu(xW+tXQC}h=lPb9?vhD;Lwt(Liu5t zYcbNfX{&f|y-oL26w3uu$$wERDyzG1J=1pDg!~dW2k|7j%8^ypevkF^1HVGiC36wN z85e1xZK0Y~aqqS{E>dOTG%X3QgEE#Dk>3Ed`kl=VN94K`(EzPKGbvq{%e#BeE*m?05cU#`Wv3y@rN|ARJ24OY$QC3+eX(0C_V!ebvQa_t17~12RNq)&I zP5YNtv8up8wB;zR9go@KM42TVBOnR+Jd9m9S8Yp+tZ>{p?i$z%<2IKAbKg68+;>>o z=)?D3Jd!oi$pf}74F5*&CZdb9r<6sh6KgaS`9y2R9#3Y!zNAzs{d|2?&Uoon{P7@5 z&(C_c)C2eWF=LuoY67?#@Xl8f0`{bP3WwGN)pg-}Z%lJHV(lQW{Ji3=>9Xa~qemv> zl&m0>{3eq=KndMFeoveME72=k5%=Eh*qPE#Q_g@8_8R{6$i4nPEcdS57Pna?XCAL! zuL%<-NFGT#$=1vae3EIN|JhvFcv)0vlF{E>1$1^9jlm&~w>nRG8ZBJ|;Ij?Je<$_Z zyvxhH!C#+CE39_E{s2NW>Y7?FL6BwHzD=sG$P1!T=919;2-SVCMimOYg3AV`y6$@T z^~TMce#8;o(Px$;M|>ZAB{nucCIz+rMvTv>6Nut-4($wnvbmThsKF+)|y&6BkZ1IBBjV=)rs-4M)uebDpOV?b;_yp8a z{F-xUn@n~R^P_3%=_XIT(Omz)!1_JwdMxp&u2AaJ__T3^h<5By_*-n$RXk0ReYUbw z=6wO)0ctFD0*&u|0N=aR=%SLgEq~4&9M z<#DcTEmg-_$T+r>?$s{xF+acj56d~Cuf^$n;s=P9$N{~J3cFw@WC?Kz~oJJ4Us{4dJ^_!?z6woJS=~|;O>=3Q(%zc~Xq+K9fvv+vE zlZgV-^QwsWi1VZCwU}>Xwvim0xl${7}p*R(a`4qX>ilyX=GZ%>~!Fa4WlCiXM z9St=4d@=MD>L~KuX?&BowMj;6&AZYB`-n%>dd?STF4y(oiN>AtbEf^W&I84T;}82? zdq}I)`=~Q=^7@QuAy5W;&AFK(&YtXI1uZx+!q;$ld zqmo(vA^So$}tc0N>-jA zyMGJs!6P5rvxWgnk)m60uHQM zY)&WB2*Qz{_ult&bAn5h_1q~4WKJE4_J2__Zd)MH$BJMm{5Qv z87{JnFF;vV>SqAzY$4IDrwCX;JVy;D@j^r&gJ3-WD*+=L{6NqXb_KX7;-@o<)_z5= zW(}-4Qycbs_r)Y`HWD?s`>T(@)ZU#PuN;Y&2Tnl;m1VE>Ru}%6-c}SCGHj4*VON z5oVhno?8mL0`7}9(AL*y7J)?~09Kqi|-q0WH!OgeUwKcDlgaTff(m33CrrcdDJ znrZ7SCHxCv7!$jtK-~RK45WZk7WoA?NC_ah7hxcT62uVRGIv(l#z!m96;X1i!lv(O zWqz$s1CB(C?@Wt6@xj4;3L(+z4tdvCg6hrns8!mvH!D85y_4 z4<9~=t|u4XZKycJg1q~x%;qgMk%x)&vj3fR=N=EMU>!vn( zt*c1ke1{`BISYb zsQ}e!Nfcp>T3bO}E#W+YQEO5Wydt zYdBSjE?&W;M2&hbv*^DMKssG^e%9A~zELk9w4~T=e&3MA^kCmFTh$9m$8mP8L=ujS zKbOJpPaX3%xs|G<%&>m46q42B`rvcf74Sj2$GCJ^^^G)iof1#OHB3UousQg%a2bdm zXA(rttXp3w7z+ieY5nqXu8c&qp2J3|PnV?=i}#|w3D&Ze3!{eNtCk3Mhe@WPK!Rdk zcrOvVs+mL)$$sQQ4t^)Az^)lB%?%|&W2SmnEcRO?ta6C2BmX-^+NO&p4ayMue4o6m zo$^Zl3d_FlUiu}GxN|vjfdJfZ8w4g6$#n}n-ddtz5<*ng&qh+`U6cVyPQ9HK>DUqK zucj(ky%;%Z!3nQvdxq5$KB)2c2NZuvQEtYb(0B0@!9Xyw<+oo= zainE$>n}%yut($~ezXQvTWm3k^## zd3syN9*Srrsb|MIZF;F(AkKw}BC0_VHLharDpFgZ9Kkzg#|~vOhVPAo#>Sh!zv&a> z*IE!$n``{HOyTxBS;v#+{(U*4B^eP;F|pP3_JXehA3*% z%M~lx#;~T1M2Z7L8dO;TZTFZ(++th9md34&H512Elj&~%^}4-J2yTQr9A9M-xlI%! znH-q=3=S3hkQjoBv^`8{j-<;7WHVxG?z85tOevX&@W*cxwFae|%hCe^pB_JcTzCXT z^-km!GA6G_YU12_&Z7&TokPWc7K-|l=4|fMKV(DIQg+ryG3!IrtiyXtxW$TDVdQzy zkGW?Z{3Q*b!>?at2K1!(9>rkA4sHxl3C{rEED4iy;ijG*RGL~ zi+Sht29Q-^-0UIVJgu~G-uND+bMJ@b+ z4(B`2@L~oKp+X12e&90yOJYcG*4M|Rj5|gWR*D>xyCyskX8_ba18omcSP3S%wJlol zO-r+eW-JjxjZy-VHgTq#NCmx(M&bACDN>E}*NuYo69<8mr}ZB`+!sIl!kvDkeeUh134A7(MOV(I=3GkX#R#|B>zo(ag5HtqBcP+(ASMzB33b#n|F+NPRo2fGTLpyG;=zMeDxC}q;5>Gh{}6~7m&=`u zMdpj*#k`_$(PhcIQ4`b@?1Iks^Nf^w?&m$;S2Q`3HG8-3xkd;)V<5JM2cnOJ3)!#R z5XbJKmoh@#c>E%Fzk5H3gO9S`B!vr8;T=u~V*h@3g~UB#fBeh?UW}x+InOux##_lEN60pjfn0}o`Pq4=`^qd#Un)zI$M345H7 z!$q!c_g8EXIcU#pc79&u0oMS2it)art*qn}cV|4%tzE$+Eg@a8t79-X(01S%N0f+0 zma0b)UW*w5mG095FE{1@d-Fm<#C%5KdKBi(V)Ed!Wr|r(s# ztZCodbDu9Y#9Sd7t5l%)WVVX8;fBRz;V0J7c0YB~^dXNJmgwJAF z8HHYB)_?Kbxg-}%>Pb22!UwOpZQjL_IF|MC61A}3<^?`f$E^*-0%nU*PwOPoOyPdl zv-BC3z&8CBdCUD~TO$9ARsO>naIQdpm|P!I`0%IkKJjX7)x^k^D)+N)_a-S;2iyJQ z1;_yZ1PXSJCGbVr#9GFfpCh8r6QK*`S0pVFfdvvL>N1KgoVXYga+PxgU3;~(G?WcYu*T8!QUC3YvcH~@ zgC1AJ1_Z_2PbmIMLXbF-Gv3LwDo8vU3LKEpVS%ycuj9gCcD*PNi>UGqHS>fD$}twF zzNT8eWz^JBRe}Bc_fM(uVcE3%Mc}nQUh-^Y8Mhlbf?EN!G3uB3@Osdz+XO>=82-7p zP09|&?nNda_rk6-amkWbBb)2QUM0f{$N259`za_u`;XAh-1d-!E_4axh5e!@2uDat z@qSTUMG7a<>+2&QuQ7+o>4)3n3wQ8I@A(aH#XO}N=3x~Rd4FEhdoh30hn5mA_GUg9 zCb(4!`3Oe$Or$E-F*96F<#@LiFX6P^7lj3xP;72iaRU-q;Hn|Tr#AH-hHfp0R2Qa@ z8-DU`8P%~)iKMz5PgPPHO#VdT8M&IjgdW7&S~2Rssi3)n8_csTc*0q1-F zl?*@n`U`20Q6vjkhrYfeTq^=MUug0mJy~7b#Ev<2k+V)*uMx-s3F4RbHYwxiu>w9n zxe-PlTGm@aJ5Fw5X0@M`G&IBG_WQ>ppTtmMf zzII;D>=6)n*%EyzqPxB(DR183E8{m-K$8onl|F`I)x9rUcIz!NL*nftXe{AR5@qbC zAC@OhlU3_(uJ(=Jo<-=nI&MLAD{=IZ%o;}Mn!quhFJqWbVdqR9(Z>bVeb^G46Bib> z2yTL>G4`Mq6UGE`SAeR?>vP`en`aA3@MzxL#bFm7iVrAs?fpZ$PZXakf&WdF?sxQZK-m zBRAgqiJ)eX+?FhAEo~%0V12!BV#+GO-DD@=TWc#Cm1Gnl<@d7cuz@E-z(+ZH|94O{ zIuQSb6I}F3h&HYy*l^J}P?A-#K)gTeH~xhoAz;2DeCsZPEq(IYccd8HI>pLd&?bTp zLsh7b7)_`d5yEgR-A87kuLDTl)_SZsfj8bP-@(G^DBaRzM`T7L@!Q@>y#QD}KjX==UOc$F9&G5$bg4 z!I(t|->;!sTp~aE4!m=+-|9BM5scEEse|sopJsnJeckQjw*=zkPyxZ~Zf(p}q8__j z(OJ_6`^)g$cEH{FLbyPXh!(Cu_eTFhKM-!V@U1;RX56$Kt!mP+HgSn4606n4QO z|ACKIF(OBRDw0@Y(Hbg*{4bQ1g;r@|ZWwcJpWhf(79l+BLNoaDyec-0-Xe1fF~Dm> z%rtI|qjlf23+h3KIsK?9hY)I# z?-?@(f9w07alrjz(uX`0Sd<8KqG@&e@iUD0VdSkm9TsBlO$nN9!p^o%X=%;IKR+2I z7JqM1Z#iQ>cV9QVNoKztm4Be+t{N7T>1sVG_V`vk7weE^F^gkjKa8EDb2EAP9h+f& zykriCPMz~`CF!xJu-LL@p3OCWdkqInxm=N_YYW%f=AC(4qN>5>s~r} z^Px*xYN~#w_;<+}V7_32KmiM1Q@_dg=n)D?U1uuKY|4NA@(H+m*AD#H3CBxtYN{@w zK9T{wI@^+;LkqAv*Aik-L0P$PZ8hT_uMkl^*pr)jcAmX`Aa#bgPQ3K(jw%xy`BvM` zoEZk@OVbN7u+j&(EF%2e>pe?bvEbpu^ADek4Wt=8_l?;g8?A2q$~h*9NYvBVw$Q6P(LbM!I&cI1{=Cb0&@ zIYj24|MNe8O`62iSyLOEjuYrlX4Iyz;n~PqPr)#Vk{J#!@{E#R*6=KsgB&2uQkqNl z4<}EZTFi)aCe22$6(-1WD52pBkV*NdNgLe*JTS|cZ1`{g++5$RqG9^_TcV?*7jDX4 z`%+eAYuAfjwVihjbt+~y^)r@L=>>VDiB%}ByY(y zE314y#~nsxV*gewEiI*rst(x<_u zCMxotvp4@3pt3b$zEhXpM%{(S>HhN_^jmbI=P@QO?y@z9za5`Ksd4l9F--eUh-V0m zwuX4Rb_TSFv-wf}_fG%wu!$4*ddH8PtirTtqYGv(TynZW+tfR};0&(=S?j{o8s-dN2-sj|A&V<-!1Qzp;ZyX7xGl#37esV>S zRHnz%Ms|AN4)a~@=g;47eKe1Pppb$x2%x6P*kavvF?;rek*@NuT-hpT*6#6zja{g7 zaNMf1_DR|IBHLi#FwWRT>c z`R9zLO~4CyulEcGhaI7PH8pb%F;9}DdCkU;#GBz#(s^pf*q(2nTo}ot=L2-Km^g9Z zlR8=^6?*pU=;!BGp5_oP7eFNY*RMBP&~&jeG5$LS&eAk6i0jZN)vNxyE?IvId11iK zgnk?zvLJ_P=A&V&i3jHE*AJ%_-3mo8;GQZv))ms(8W!=cdS_!$jYmeNOP5nWvq|28 zM~*DcDiOY83rz*~38atvjWtQDDsv;r;vS3a&&0(k!fh+kWgKv0zotTX(VC4LgRQj9 zOiU!uU83-bl)l{X2U%WTRrQ*6Qx}?^OAwayS#w=iUoSk4x68|2okIp_;K;mP=jQgf z_km`=yvDC`Y-HPUc9dr6P8!TuGu9r+F!New?Ynz&Oadp8wpVGZaHk&}(pHUwa|6Na4(DLoL%jphuBBpIwI+&Xg)4}B^{iN@748jV}# z&6x46NoOa#{RA`vTh?7Hb9XmqLg(DMbNPsYM~~{{NoZ*vv70>`PlwIL%|B#`fCX?m z8r{MHVu7&nNr{P~-vh&>^wV6Qa7Y03$u8nYg0>S69csf^&1G(Gml;7t0)!JDPpC~i zb*c+PJeMtBu78J+r!IaL>v@o=`wSbVG}I|Kkjy~JV;SKVmU$2Y+p$j)hvcJ%in)xu zprUBMdGltyVZ+P`dED7R*jZC;MUt5$TrKwRZk(?#^*`LjXGWK#BsD>m;$Oz>+7(O+ zs>1j~o5B$IY8l4-iSIpaS{Kb&8HSOze#lOp1ew)cRrT`i+c6`HiR4?Ef_u@oNtGYx z`R!FFai@uda1rukjnBsKJ%7AYhQgLtRt_Rnn=y1$M@L6Q7TTYgG`eWnGVv$z6O%yq zw<{`w4;<*t7=eUkahpJ;f?QmD)VTI@~aIkA>liAHpz{FLINk`g(^jTFNHA6iIP{*1g9tXsf$c5#PU zDyGkYVqn%nD0ox9$Bvl2d*%53;(RA2nymWy2Q|PI_@8JolAYUdY}C3}9jThO0$3Iy z^V!HumoD33_PqI$lmzifMow;PaB%y0Q-dmMyxD9^g09~_rVINOCfM6|MWwiAX=-6_ z*{xf*wRfhGQt`R$`s?h^oTRB~XU1y_D1N#0!TX9X;JbdlzpoX6MdrOs{`N@)22&1p`iRM-c;e z)Hiid(#dCDBanAnPR?kXDN}xpA!$#Vs2Qul!DTXZwok(dmWpxn=T_yB_)>0v=5zq{ z+KAD`PcM!JTKww0EhSD|7V>c~a7{51Ne#9D`}#vk_*}I6^_x{!OO)uuqz@R2c2o8Y z-{cgtFC=9ANe&x0+LJepku8`B*gYxf?s^RH^4++8eIZ+(D-WKc1*QMUE@wzg4m-DO zX(K5qxp4XNP`0@Ycv3gc$Vh;Rgx$M6^#6abT^mUBD>%8T-oGE{S<^Ntn&2kWv11|c zWXG5;OsEu3s%NiW>ECM=L#05hB2ls@`|hc91i%9BI0KJ^2^(F8r|I)r?=w+eBeMZ2 zdUhUD1*HeZk;&aWy}Y<=(Q?VE>@2>_1Yf?A%{E0YZZV8j2LFf7){x}qT#A5VmCb$& z<9sMqWMpLW`L78b(>$Q{nOQT`+uJ*#W8*2?Pkikf+_VKKIvx6Grb%+ka^4(cbVhmk z?DL@^Ay!u2NBU~o702i0ul}7qk-?%CWpT0BsC9IN=N;lp!* zfq{O5OzxgtIS-#k&z(DW;+fRN*S65v`Ert$g?2oQClOieajuV=z(XKG_hj{|sw&cm zU&|Rf3pI3fUNx051_2(T zPts)1EqQq}FIC#(?hBS1A(d~qLphrYxB&RpI zoUB*_s|6BiPsX@^W@(o~=gjg3&cYTlXkKF;22~ek{*Xf2@@*26k}h}9($ow(bn;{r zXFCx}Ui6?G2`jjMfuktk8r94o=V=0&{_n3zQ#50rQ8m!XSL$@YNjvT+%1QpCM;D4~ z`^G8LHf%L%k{R{#z#kiVeNZ6-+4z1#+gC$dTMGKspLreeriQObi~$#Kv0|_bDM;~n zCEceP85t&^wycs{H*Z>!dZB-=OMmS^N|W!_tsPU@bgo?24j7hqnZ$ zfTC`p7Ps%*SroTp_Jj%T;739^llndEt7)U#ifidPL%3~hm200Xyo!9qv zCcrlM)vt7S52rN0!h~H~cWvvsxC}EeNNeo-0>V=2?F_<6yjCPFAv^gPCH_09{~Xz9U};Xd0n^9oH1i7(@UAe zedx#$>A{0f3{c*0HDyW>CsA5$8o7rPa7*s(fX$6{`lyOpRv$=^f|<2B-LnCcWBRzG z+qSi%DwTTs8C0@{Emt3&UQ#0}8t%Ek>|2Wo6C@=l!eD#5q6w%m?t0RN1FSEU+GGq= zoj-f_?5i(+Eb{ejZm4){?6ewTzj^JrQ)y}S%|B*>IGDzg;*h@c^2x1VzpgdH80>TC z1nYKozhnK+01CwV=B2D^8#Q|LA?~Q?FTAYIYZ!0IaoN%Q6F>L8rw4g%&nXNIb`R|1 zR3@%7NZc*@5nB#ZQftyjrrpn}=S|i9dVNp~NSQ!N zb;0&P1KgPSnvvAjmK?Q?ojMKt-hhI68BhaMD0Mm&Etbg|y5+)!3yEdx z-Igz3M7B&2J$yCO4j7ech91-yIZ}l!El8o`;ty%Fs6*H@l9}<{!$RATdM4o0#!<^| zYXm)Q8{GVa;=Jn9r(3nB?W>6i9vqNSEUxR(qS2>M85Ybys@9M9h&@^&W>hjw( zbx!R@gv!sJF;F2^k@2JQ!*i3mEr)0@CC?ui?-N!MtzM0ntq1~@WS^el-gn55&dkU7 z?AM#1#m@^Slj^z_6!L8nv5p$t>o;-Y#GLC~oaWi70gE}O8JU@}srq2@YxAF)o0;hW z_=Aj8Si@3{b+ZoXT0u!kYnD%Crb~O;dD=+O?xGL(BPYZc-PKlB*4Cz&mD0uu|GxY0 zR?gUsaOjC3&SR(|Z)M|V;IYj(;>o$7FR}{~eFqq%?mc=O!hs|JAA?G9wLS*9%5ENl zY1IjbLB9o7vuae?zBwai?^(pM0C8X!FI5$Pt#ICpL$)%PNX#j;;2|MqQps@Sb;<#_xwaC?0GR&KzDCl}7mEo)=; zDA#8!gQz=8AekECLP@$^aYH1Y?Yc8uj~a{hIu9HpdkkxbGf`3dCtYLxu9{CH+z`)N ze$DNrt{zSCSw$JX_FgS<@zUor1Yisb0iYan<{>Lhwy;=RMh0hYMgWu5*srr=$zzN> z8E4N?Ebw)4{l}OC=W@HrcFC*2VdXu2vSm^xY{-rG*lj&re5F-w#hFm>%#as z$NoFm*dk0>FfjW{Z51hig&Xshg?wyzhf5VJFe~kL?JGSp{~S2nEDt%VY>Rr%d<9!_nS#dY#qBca-?@?b}vR2I{2@qi%c8wy-}T?iy@n z&#}LC960m1ZHWGf1(#F-8pU*jKVujZ4W%ZNCx^!m4i@P2SwY)IPW`C$Z{)#@X@h^` zY{bRK8`rPqJ2jN3`7x9K*|TT<;VMIA4PNn%&saGtKgsqm&uE`asW>MvSjvG2&ncmK zOM%rWyTZpGJ9_l;&6}M#Tq01gBqt0RZH>W8+f}QE2N#YO!w{23eQ=jdr%p{3 zSwp8uA4J|>Q}f)CjU!ryh7KoR`#{i&3y4GaOY@%HMdw1S6hzi#HDEs1wNJP3m%c{i zD1L$^e6F6?&lMu@Q9F@FnwvSWsP;0Hh(K|?2$5_oak~Hem+UF3 zNkpI4mP$}#psQx#oc&ze?aKwYtNrS#aP@Xuvye*5i}S!y6lkZpDwSiTzh=dpKvVy)J& z5r@vLouwK{sR}WruQi*7}HJe z#hm=I@cI^l*>mhbIJphzbBV^ZBp$`Rxf(SyQc_b39zB|uQHOk9L^)j$WGl@yLj~=V zKvat6BB^){zz&&6nrQv~bEVTNS#4i_ZYP37QO2SxF)=X`p%#}zcf@Dy;%|+Ojo-X` zcNw%pXy8wn9!t@_2+|rehBS)Mi(HfE!EAe9^cO7B0>^-pssoi`9R;+YS+6q>wyv(O zzr~iqF6^DZQd-)9(r6T(BbIEAe#=d)1tv!VCi~yiY0Cl2UQ;F6@4){XeY!5-W`eTk zXvDgebHs$NG}FXc8W{HGu%-0@qfG%o+6)utzH}t2QD-gDv`XVd^|N}6@?r*1`qIpr zM&_cKXVA602+8-gS!$xOoNT5^(%Kj+?NOdzG~~1ur$=sXs?SQSnWH`Z7hksE$&>vD zvAJ z@f5A^C;#yRycSy*@8_)Rr!rZZ<-`jK3Zj>H&Y;+k@ug@B`gEwGo|@|3x-fx9~HWF(>c@hkv>h7f2nZwUcGKwPPYbyTp>YAhh9Rc zc=PtHeyRNl7_b`7^A%1UtfQH|{r1Pvy3m?}S%wzcF z*(_xOtH<;@?1)K72q8GOylg=xoAw4a5n3c6aezQ}+KL^NOzmQErr%#zJz~TNJ2eXH zaZ=$vEC@B){5>T6h$MNx>fO6*O{=$G#d7_OzqOF(o&Wr>JF$kiPHu@Kn^0k#c{Z$6 z3X&uF=ura&2WkBvzl{yg3z z{&+I{HrvzMa?J$o0zk2mRdBF^s^Z)05mt`$Yyny@k=2y8zz&oT+fJSwqGH);AQ;Ts z+bCe`)|c;!Jz>AF{dNT-1Qd3WmA#DXIq}GmB1=0{vu+7$tn*Q_xi^8z4CBicsxz8a zI|TI2yz==^l^}|v2PTsa$(PB+p7FU(pO04!_a6%guX$6JS!Le3S#0h*d%Dk4~ z*@I@X#J0#;s$DxxMwz;~UEF~w<=?-$V!s#x z1=|a4ex&p7#3jNk9|Tc*2o9T?aSH?ghZ`CmI(oESr%s&&W15^SMMIaUq{y43#xHd> zF4{Ue&pG1zUgSY47utq@_)An!( z4wh2Vv*G991IB()+@vZ3Lbg}!kbb+ZrfT=?+d=hUS@pnZ#5I=OY0v=qs(u6;9q-@2 zpAWu7+Vi*44xk2OdsX6z6P+afW^*~MRo18v z(oB*y02#e)-tgx3mUu3gw)Eo%VFV@uT8D1)v=zn351Z3*l)$8ADQuPz{EN){Dhf@) z%;%Wv&FbtMCW7jD?Lux5F&X6Kk1po1rQ9E8w6LT^tABqPz}fceEK3?C({1eN(LLCv zDDES%4cE#d>`?)Q2gYt3a_U0rLn91W( zKEAj~i8m}v_z5wx*GfwZh~Q!_(tr)Uu|`DGhPDu}1U0Ot(oUgtJN4`pa3e`{(!p?+ z776K)-)}(@It1AvZ3fD<#tMmF!h}mDo=`}LOi(X(D=QhM#zkPuTl1XP`>$VCeT=+* zc0?7+r-Izw(QY%>7jIpEco!rMxn8|`2})X3RrRynh#fn33Q`yryAa7MC{*W z{6=V5V@x718{|32Y6bfZQ==^T9_aK+$E}`|LvUM2m7koySMIg?g7zpKT1sW>?>~Gf zy6SndW;~=t&d!pCS<@=-p5rXyofDBAAHfzv%{50;tL5j{z6>jm{^p11vlRwAiTebA z>OjRbe)JLqIwr=|e0@3jah{(#U8PFA`ye7;?=KbxugDq!mzvx9@891m++8=8?P6C8 zz2;JR+3XvofUjwj)0;UHC%%ra{)DPQ4&1_5;;r$ANOVk~>SPlMY*}lCiPK+*VJUpsg6%`!(ap95r z&?gq3_Tvk@)CP-Lw+Yu)RM6dBUT$CD?6ql=>AZP+t#u^)(bc9NFbZWe78!O+hdxPp zFg^Ia+LBXCT)j0Itj$4UA()I1SP2>Wx^}h@G;Djv*f;fPZsYMVXq$pR{t#!ht9CPj z+Cq2t>wBLJq|P?Y2p*3y;)>KC{jQglmC(%i?9-={>s>akU3(c+SM~CxG+ND&cDk`v z+LQR_XSgd*(B1H-oev)ax~+BKxKTX?9q9_y6aJ zpPZkwRvJ_n@ac@X_H`J@K!9Yyh#gWQd5DNs{fA~GKfRQeO!@(RTpl+pl>6EtU<`YBeB56XW65(gQ0;J zE?pYT=e3_ay)f?&bSt2(plX4jM6HWM=k`=m+M39`H{HZqyo{`<^ueJTToe@c@l%_H8ZGW{;azhbI;n+ zQu^e{lL0gH(Ff(g|zLvkVY^&pce2R{&)L=g!f+diT_pvAMPUig_s z!8}BE%O%emNj#3Cp4=TVN!)KN*m^ABgFg__-N~;>8GghZDKSA_Gfe^LcA1CwE8~ve zzEkQ(o{$R?tmQli?XQ{BS1~k$85zRp@HgyW?^}^GH?^~v%Jt%q&wtdi|NFb;;=04D z+_H$VTh3{s|00Z(nMmlkeeYh=iv-#jZvXMeRv3{%g9kI7#H#MsuPVIKm&r)jWKuJJ zbq%#hU>p*L5}~u8QOuk?o{N14?SO8IZZtBL(D&yd7}JMYli!u~+dq0;J5tHB0`q{- z=x7z~PmFR&f7x@^94^|Ini^9ps}PNvq4UodYKE$3!WLuiJwM?gpK3Uy%%YVm2V3bpfh3b-pM|B7AseN ztgVnCZCXIA{-sofVJtY%F~sevD4Zk9d1joPGLJ8bw+_+vKk3w)G?LZ@j$V;E)B~YRsLKqZ7u(BMT?$K7y&z?ZCx^r%| zljo*QRrC{Qd;cJAZNwaCA5c}1W_Upuv=JYMZS|5#uYBwW`Q`qe8sM(kCmSJ4)BtY(aI|c)JrLywi=NVE9)=^PdLc7h@$c%&VmY0{$mke#G;`(KrIb-hZ ztQl`=Vb>Rmf!byEym@CRQ3aEXJFtKNp3qT{CA5Kc*IOt-=B;Ayb&LzJXPEkd|Fcp2 z-*MHch3Z5W??@7&NybTX#+r!-7(F}%WL1*jh!KS zu%rwCRvv35H>OUXBgr}IBN`!THCE=}!|WB3?6PNf z4TPV&bzodLfm%4rM2IFFF07b^z62*32m)C3^kOLAA|DYW&84%ftl9GAy8RA@q)5^{ zq7&cr4o43=u11@3?(Fh45e6o`U~g84xVS&*gO|E=DE!4iehy-Tt*vdotGwW*ctxTNyfqZ*43*r z+&CLOhgJpv<0HTHdZ-KPS1?6X6gr1ht+f@YNG_vX(1$5~YVm-#=#2FIk03b|yV@h6 z4LHC4Z2I4aATxBWw2y*bfnWgK)1GqV z@#{Z8N<5A$f5UuJxhcL8tQ^C*>96ivXPWk_5piadUaW>tZ(wuXqTLrS4k{bpaJ;y9 zF@f*?drv+%?HnVA9BAoBvGerlQ@fRaP#;luSkN|vcG}s{&~W#SNNxi{wFjMb+S=L+ zIr*~suOYq)pn{Vh^2~>M)P95_zX_n-nbwm+T;0+9F!AkiAC=$%I|22(*!M z6FV~yq#Xx%6i!7xmAk9oSMr45Y5!J&lK)(Vjqg)sUKzBgeH$zs+e>|E^|8$e$mQUD zpVcZ$S_BS=k!k!qxm!|;V$YrtB)@JNee4XNS=OvucLkfs?uY@x4Qe!Q9E?Y)t9KHo zjX2Xolq$fruA7e|0w@nU3ml;+&Pi-GCD3zlr9y=gV(#R0(E8{k-JwHS4=W{HlZYGo zebm|6BJ17o@f-x%QiC+p-+mhbBT%Vw;_$UixPF99?=KI?aK@ZL!`95!(KLPe;>AKN z;DP~;pDxW;@2>AG)qA}6Rabd9AEi(W$nz^~TkX2)OcE}{z~JC@asMwS4H=Ms?V2pp zUhkhk4}5*%rug=PWbQV1{Y85x%TmMs>Vrien2Q=%Z|Og!VNX8Sgc z!Coeq5J9YX5bMoWgJj2lDaDpHF8XIO4?s9`zFC zCEgh~ql0`FyejdNyigxsnOX*r#7=8?EWMG?X3CVCs^cc`z=zKE{?xw4Sh!kbWK7ns zJzf6L@%Lp4Cl_1_gWtKn!UK>BJHS0qP*e<{et{Ud_HiHmGIB~fHZgO(@TRoZ*Q*}O zP(kIC#*=NCJ{S#!ixa@tpO`kSyM<&}WTf)&W@7Q|5X%CAr10$7xvwVWTH)}7BS8!Zx6)2CAI^}e%oVe zk=!5Gzeao$jsMF1f~hPfl_yAQYQAK-bO;GnR?&-=2U=b_#oH*K#=8N(xCpGccHJ-; z1&8?rA;DK-3)87qqGzxjfmb+eh2J)2(tnwR5XlA>C8!Nw=1*#M;mrgTR>F!RixpOU zjh6o!?$*A2d!2*TJmu5VOX0$f$uko+4<$hoOrf}DNK^qa zwd(cje)rlh2^;|xOCLr|Z5w$96D|(>VUY?ViT2BuEfW@h!6f}HvZtnY7(93|^|;S{{tX~ST%ayVD|hct z9n2$-o$Ce9zm;D%F7>wG&as;N`?4*xg$nVU{>EO7O-(P4wqP{G5)xYea4)h6f#PL- z)-(iusO*V|x6mQs^b&aZjZAXe39ZWCcX&8~w)uo*(y+{91zP7L+bu6+$Cdb+pp2cL^gm3 zNs!T$24jyKHD=7$c6;2`5=dx46U$g^CiKNyC+yzjREIs1p?->A+XOK1P zt%FXz@W7Ix7D1Zo85o$6Z5FOx9Zgj!TGs^P6lNoQ;({vG8#!_!44vS!C|{6*)G^px zrq&h-aNjtuZh)vFVO4_)^FSS@g*=YDfmB-Z()OfgZkE zhNSGlxb_^35>Rvy%?+-B(@O?16ayvfmJg&;fIJ~PU8BZafKvjeT-ddQm>7BC%Oa=l zcpw?7{_fGap_m+{R`;XU=fymi#JiUNz5zGe;*(B1vllOL=vjxV&M>h^uA{KX0CP~1 z>>WwZ*?LaWU3vuC5w@{2?l0A_Gr=35Ptn0ZsmreRhZ$0pUsxzLdFLQg2$qBxKSWM( z`Cvq3HlGO{L^A1xU&n(fR;JGn5}3hmubmt6iFeWiJB|f&5^O)fQ?Z;O=I()(@j>`A zhd5?SxVX6J91h^iw}+`GqHilrNx~cqLe&^Q{y=Gp4rxdP!u0fk6g~%S26c0R$s6U@ zY!p8|GU@PPae;+P2E=3|gIX{Sj*o3{YG8;WNL1zL>$oX;5xvK$(r-ArU&5Of0Z5P< zavQ%+2Jrgc{jp~K`tN?t3;14%hYz=nwF-n75v&ybBf?ipbpvOa$T@}$ncDf^=rvXX zV~^g_TRp#je54{;W>RyB#ZzM>&tzoeLyifig;Rgs@}Psgy)fA!B)4Ur&90p4#oSR5 zMjsoM@aiPHbm{Uq*Js{s?*-@AcOY3*nNGYnA;ozqB%1Av^j{}tP_@z~*i+0}jH$z9 zbp=6n`&y?yp%wumEZDhz`z)A6$OhdHI&~6ieIW82Oh3;^)-o@3m&}SkzEIFi=E6Rto zlzOntNn3Fd_tkd#^hI&0lc?YzK02UqqSI_aM3aHViV2l$-86+$2G}Pj3^%Zca0VvK z<4c6A;8lPxif`z@?r=c5Fo3N16biY(MUW!XZ*S|2o;>vPd(SlQ6 z9U~(9RbuU4A3|RN!^p#XV>i)ba-DBt%N8PV2gP%Qea2X5DcL9&YZf`|g6>2FIPR2p zKsvO=9B3#mG2w4mkAoUx$Era{WqZ~n-Kf2H>sDu$TlF6>z-;EsKl>?5#%x17Ho+1e z6&2;U%))*#9}qqzj;}dk!i4U|zD698_S}WP{oLeN9?zDeM_)CRjcIO-V7QSCg9fhN zyjg_f;tP#+giOI&Q-eT$pS+|0u`*9gq!cru4yC1ClLm9k9)jDeqr9m4{8{;5+tLwiU_h`TQ=U1gss8sz`7tG;!5I+Qix z<`~ft85%Ff{~Lp(CeJU$)Jf0rc_bb3Wjp-WMs;uE7cW_&si$WfbDyZXYq!VGw|`8`xKE>O z0eZ%5RuUYUXH3EiP&pzg0VPEkFt4h;M0ZF~R+b@YC=gBHGIoU}3--22g!Jdp5w}JE z1cAG&Z=TJ#apSTtTsVyLPw3F}#{If%VJ~g=@kv9!fddEbK7aDiyNy`(a3jh-?JTDq zH=H7JVb$dzOiVl9yQiXQ^%*gu8&yQNzI`vVqwS1nFTWZpw|Rz@H9RzO2fiRh}}LNJLbAMQYO(d_P@^ybBjK)T1I`I3%nEbNz~*s!4H z)L*AsKD~Oi4Q~FT_b)|!j;1SC3_=H)t7WM~ z^%enpTn!ujI!SnxMgJK1<}h~zM^1oabYBuD&vhFkUi$6a{*bqzx9$~!lZTm3g9=?3 zv^e@T?dP^124xD02(?a38x@zr$w?iVi99vEZi@Q9FGfgnI5ifzFBsCbv3i(;g<0mA zGgm0tUo>yZCO!+vkr$bM$Cs;fG4I%{10z#bBb5mW2cx~LqA^}gz^XZBI#2n#{r*;} zQ5|*4VuU2v>{fmjt21 zY=+#}H#B*=HoV1?Ndj63vW5plA7976ObK75!%_lz8~b*|B5nGJEvtP*d&vsvA9y(c zth2xTM*fDI)b3BppL5Q1~1=v5z zLLLS`(Q~Z=Y3O)6xD#6uZ_{(e4EK(l!eY_UCM*pt%}w}ej@g%deEm8YqWjB|;4Xzl zMT-~_cv1g-TVf&gNeAd{QUX5Cg`i2<8tI?MAj{%3g!qJ~;bxJ|+M7@hWagmU-0_9Z z`e<@}XV?7sPKcun1@FKa-j|=ZiFSLIP(?zrky1=FV;54^W4!Z;?7esV`0*kYVGXba z-c2guQI8;CbRve~hL)(S+?vni0l|qv&0xUlyCc}Hxs*X1U$D$35vm+LE$qdh{x7w) z6X_$ge`IK2aG5zQLfAr~Gza@-oQ0NHNGw^mZXMODm_lD7*z5Z5tzF@1)dxnbR250} z=?DCsvXl?YsWDxERQt@p(+$NnZ&Q!^zknn$Je8^2KIIl2y*&&p;1 z*|tPRc0wT0{PB8Q=!@x`P{^PG%hcdTQ2ks_E8pijuYoBveKZQ&CxmCh3dM-%=q>_` zqNq@8vAZaoJ}bG+z3Pe6pZ1zrd{<{$YkMTw-4FZ@BVz39N?D=X+W0Fix~}*Dea4T^ zedtol1A=XQJ!QXxD`gocU+7LkbGNav5j}Da4)@$bPk}V~x-`)x0jboRdI_$WJ~U{* z><#a`QE3LFF;&c-D!Q=mQH(?0--g*y9qUH~yn=O+ zl3K`V9-NjzvolNvtUPG!*PEsU*tqr;P9FNAqAZYYbL|4pwL`bDSjQRGsfO@#sC+cN z8Oq1&M3-Zl{qJE`dyv0G_byZFCCNg1`ue6!#}T~-ojOHE=T-;|kAxzc<|r2Ex_*X7 zrPcVVq~z{xL&;B&yG(=?U5HZ+nrjzcqfe9;W6k^K_SdvArg5DN@BYLyBep5fRkI)? z*ac3i)x`sOBTPM_rsOqi4j*p+PI`nen*nb4GUnQf9yJQngBE zkr`Q8Co+Ri0n~*LT(}R&yV>jCDhf_4bM_$5N5Uk_M}uA6^bw!1Yzbij@~@cmP(z)2 z{o^RgZBZf%^Ov1R*dS3>s>(MeQnk4&ZZxB^5kLNa@GHC2su7AI*rjgCAE-qWKFEFyi{E)0B zoNgC%Ycznf8Z~~uUaBzvHKM(?PaL0DcBb~7;^GOfiU+3CQ7V?b4G z6)G3^GdHdqFK?31J_Z9ULcj^3O;!}c6pN~FOE^1JqcIZW@ctmmC+jzKt#is(#b>|RHEV{E1h32cHR`Z#%tis7nRQm8GABLR(PF|@^Y$3nJ16Koe$=E+I)+N%uvj~D3q38I|`v7ub zpv-ejwa67%OE?up#%DrN8eUOGW~R3)c?No!B6Hhf#fGe81&a5@LlHi>_wR z$X7>Tdf=9i->no@6--YlKgS27ETW zw1rXVKEX5)TOApUlZ6{4fi7>bPATqhs%&UpqavjnGYRD|SPk;L0J|d^#)%;5zn{ti z)D}^1iLU4co{eq)vdPjdWf4;cV4Yd78A@(xsEFKZTqNx8mNo{*U~QO!a<`#&G;WI> zzEfs`q@$ubx6ZuC#gkE8x5GlQLuUBko$X8~EmNJRoO)#E{jGPmO&FZ@hoWWb!_?_v zNnr-|gB_+A9=0$KA91$Pen+OI!aT)*K$$*y#!HT#nmTddy>CIJAbI*hHFx= zdh>5~(TQ||KhB-Pqd0mr2T~Yz=?ygsmzP+$QMTKl6r8~{zgThGW`H_wJ_&}ydc)Ab z>73uo8UEBQMR2*i$ z-{Wv5f>IogOxlsu@kJOG&3Q!bSqwe?__a9;*?eNKHvH{Wu?ZlotFH4dpFI~}y#fGN zFxsYsA7{Dmf@q2CE%pY= z%E&wmtzS(r{gY7d05rM8WZBW1KY*OwaBWVA7LkYrwDZ7b)dhd%{oKgg6cgTt!$C5V z?SP$sW$PFljJt!US{kp9MZ0_FdkBs@SM{zI5V$p>8Yi{ds;-q1FRThPFhrz4GuUf9C(c%ALq=#D{~jsYu$_c{L2}(wlT2uvG02seJZn^(N?-=)tUAh7hpkB z!3}E0QM-2S!j6+d9dVssZVmmXCk2Aa4nuUh;T)t^NxxIwsfU3Dt9qWl(3kL^zt+QJ zIs(FX%;RF%RXxvt<)`tKUDIi1m_c0dG^vuO5K05VPPZ6=S-SaC&=$VGSf7?Q>(og` zIHqzR2h2I0%;++=b824yeM|DsXE800?P_-zeb^UfU)D70+t;sm8O)mm@AKe??Rnfn z*DES!!QvM;IA*!+AQ8A!T+tjb^LPr9s8)q5AjqC|apxYjiPys~<~0{RBK8B&Fa2T} z0eJJ@Z%yB2Tf5FZQ_O;7A2dD0cocQVMYMn%vhM;`MXuYN${)z74aqb;mbc+qew&p~ zFFtsq4>Cvw9)Htl*>>vGDaNU487{kRLNRn${#hm3Q+ckBS9QtmF&omJv@F=@3iiJX zH0L&Qq#VPgwHQ)y2>Ntvq5pEe9)0>x?#C1oX2fo_57kl<(F+HAsbKzka6i|V&3IM& zQdxvyj*iGgv|jvwH||eppw2#mENAi{kD;PoCzz?sb>Q0V8cqcq6d0IdcVyCtVH!0R zOi1b4y?Y85rvHc$GwDMgz4kFNN3GIT&Hv^6i=NY|7Do8)13)c@+jM(y`p6XTEw4U& zI8MgZ)zO(i&N_X^WhFZdU_h7ZKkp*luAQn+FO^=sK4v`OIhWtJzE1CiqhpZcNG207 zHNza`=`K99!{G^?T-qx7T`w)QhR*zx$y(pp9y^13cC)4-Z)sU?&U<8#MEIOM-%E_E z*{fCZwWj6{_jtHZ{rdFoF?H@VoQu1GJ?kV@{OkK3_!(2^|ICzcV`M*f?jB%uM3*I> z{yec4?kE}7IK4N&3lvlVITzVw>UjFAd4*WdCGHzSTnz@~GDaaY=TR=ES|0{{8HKEo z!J{lfljZD`>vSnBc`J5Q?(@4?ZP$h^hFmXuqc8G3)kO+g{BPqlrpJNH%fUP@ zzo5p9Ol$7rEISGtKF6p6964>=kL~xKeg^ufUY|N;3jJRDo}Ahb*}0s$yZCEUW8?Sw z_ovQOHK}{D8u~kX6_!x|7uPD%jdz!u!ntGh3o)l#mk*!6eE(j~0NNjP%fDp{&LR0x z7vTZYv2hhg{{cC}>591g@jjZ@dN>`IUr;JDcMNdouiEV%QxK6Iti*XR?`YK+4QF%s zmv^qDMD)y~Ha*g_dwFwMO>R{`QkBT?pfq7nV>s`ptGfE?52M6!1rzpftRx|@jn0cx zl*T;Y-!?r@XDbE6aMQ8h0G(_YZ+E`N<<{Pj(;%qNl~%?1YmxB}(1qaqy*8R?#rr(M zP2Owt;5b1<#5PQEhIpS{N_HDh)>d4VneM zv$Y_VjRb=t9O?E;qM>W1@&J2VlK;!MZ&UHw48Rwix6jp&X<2CSjbAT}WVR3D)@@GB zj(z)%Qz^bjAR*e#IQeuJ-Ys+{qlr<53-cu|vDR?rI?epgx(hC5^Q@Anx8GxupXuzJ zbf=n09`9KsB9`f9CrIbFfWcF1D!QX?+LLSfAuCQEBnU2NiDDW#`%dN3mLB`;812}t zFAHR`?xymsnX-0lIHh0DrhQiw^U}r4c1_E$FD92iRjMa6cJA7usHT<<8`kp6i<_3k z#xL&SE&?RHpFPWdl?Tr)|1?s%b&&qwx5mE*@u>b$Wpi=Xg{vEQcsrOhb>*>Bd|3`F zF(*UatWO!&ncEb(*eTb0_aQaU6*!3|tc48RZtUlqeyI{5br+&C3$j7$7b7k0N#Gwg zf5b5Pa9J~6WC!-KA4rb2O}2{dnKShT-KQcIWHu5W-Uq2Hc-H1-JBsN^{^r-YEYwu@ zfT}%5KgBk{Iav^PPIzbMUi8eoQ%MiuhQrHdKV}@{DWJ%x7w(mese5q$%O_sT87El@ zBQt2`dq^KE={B6TIv7V-7v}Aaj+pP?CVzrx+7xI50xPo1y$9elnxZ~^*unJ|nev^ry7K9pOfcGEu6rs3lu>CLHQ_nMY1`v^ZEbA@QNf-G z%6mVR(!>T;GV_4Zrtgg3QypDQd7XuZS#5l**PJzLGMKMtdGGj~89XsN8aoGXzH7aC z^LeoIdW?Le+4rQVdNz)TLG$5H+s_b#pVZXeuQ}BNdxartpw*3{HQ&K_{{pmF8>&-W_Fc-Lj1j(um{!)dFI-@bjD0f=(Ii$b^7)klvW zC8?Zze93DT3A%#E&B)MXSgZFKvqkL0ks9$-Y*<(hgE;zbH3$9&@$J!#4yRFqM1B(d zD50LBt;e{hJkNW~#AAEW_I|zmTQ)m2J#;_SW88%G>wm|-x8#p;Ioa7M6pL#&Y`8ah z=q1+=qnO za_18`>|oWH{-tkp8n6nz?hsZI$5LA=3boR+LGD||Zg}Uq!scMY_}$|_ z;^jAT`Na|ZgJPg8zC0|Ux3B1)SMG1?pVX%D5TxDUIj1c+CO2cNeVAyH#+vP-N10@L zQK=Mvg#fsasM(+`ZE6B%;M?tJn207TX?5N_<&E1$+-@ z&-U2ZL!8;GPoB)DQ0?~MQ4j-W7n%cIjv>>yK|}9hJOKwKvUeG93<{xwyz(()KfT-i zjf@eGGs_%A<0wi51|d;kAyt$li(ea=2>QQw~D^SOuXy6)@#o|T<_2zLBE z*7Otw0shTRdiC1XYMrVMDyNkxb-Y7}*>tkUh(TT{+o{dT8{e3Xc2Q3SYd-`I5>?}< zhCEQN$w=JX9f?i6SRIStHEY%ozkGX|6|q*uffWj8ov&4ycyV4o9&SaF`q%Z1JDwuD z&7k44{v=RFD$$cRz~ZiMMx@J{Efa9_KiI$hiZx?NB+^IGQ;z#JyNH65Qv2=a&!dW%1#^ zm*zb>@w85|?2JUc@aTuVYO|(#MvP%5>_*X8BK%TsZxb`Kl-C(BH?G%SM>eHgpp|Tt zjJaY^Y;0_5W(8a$xVpiSWvEDX zGh6(o`WOQ+F9m={T*2I;PcOzZ6RbTqhmBkgE*bpynV`Lf>WxNAy_qrQ)P()cO2TTo z{hYi4>NT*2f(Ojt!7QFNKEh`u#wKvUePtSa2cUaQrcP8YO zqu=*Bg?xgB+S<}fHfgrjbrwcrM>!S0^G3RO`Lkqh5Vv*IiYgjLO(R}sj$k2o=$5VS zhQ}z1$C00*Cq9OECvsfD7@6l31h_k+pbQ-yeRocJgpv2#4DkS}@xk+Q?l-%)$0E%}7DUW$uqw~F1;Wjv(&SJdnJBg}eheU?xv!|AG_@u~H{^b>5_$wS{b%!@?b@;9D3$0) z0PNQ*R=w!ySALlT^80V*ZIH7w7gkZU7(m34j;07>?mI*$F5GAmv~jJ!RqS#7K&yY3 zC;wtka^3V@)Q$2RJdK#L>ed;6$Y!TTu$6?_fQ$sY?b3EQ7dQdjc^E7P8ueVf_`=W9 z$t=kY&@uZRzk2aERv+plwE(-of@{gjaa(wK`s$-94#K6}*}CTA zHnrZ*3A1N0Tdvc*cY6BEb+M_W2NKM^Hj|RG3MO7zHVXE(d$1Mkns2O{W>ANTPo2c; z##l#*x;E0=)an#xj%fCk^X>5j4h$NkJ-XBPz(u{5eA$OUk;i^=GXBZ;LL`u|(!_8u zdAxMUZL|nin7-&ZSQiH|X@hySzmC=J^Hxzc4cxX(c%Z=*KRwF{O3S`vt$>DJ#IPRU zoZCAJPBK-l3dDgAk|#zkk_w*n0?*^AxYxraB7rRBWNAuy?yQ&yZMJDD8C$K zDmr7mh>;|DDv~q)uH7l4=`_yAs4+G{Eli!A$1*!>%@&BAMxct>f|jUO(TF@YS0Us+ z$YLvN#!W6fD;;}sTsMBhVcgYjfw4TuHBAC_8aJNip;|h;_HuQ>NPt2r-q&q#a+n+j zHsl;5)5LF*ijg+?i}&}A!hj;eIR=a#<8!6B6BB<@!KApDjys$1u9`vlAOtP)=c$rr z!Rf|t{K|f+c7#lf?O!F<=n(;;8BWf$j=FN?%DE{wG-F=aq3jzdqzvrQSwN!=Ih&ev zvF1#SruG_pR8iL0b8rN7tEr{+1W}NG?DjH~m)rIwE&8t(VA+TE%(dA6S+gN0d?L>A zCWh(awKs!X&GD8@2rxQ|2QccWT|a)QQ9v(7|2W?F8F8~F;l6I#JV+GlC1Zf#M;9)(zB&$-7+}!{D$$UrRW!K;r zw2!P$+8d~*4zSr82XHyEC&+P4|3`?DMPOio{(`b}I03#WlZCl#DsC;G=uI@TJ3Me(eH0pQx)Oy`4Fbq|Lq`a z>vk<$#+|Z25%-w>r--3M$-l3Eh#4?_(Cw)jN)K&@ku|DSo5kz+$?^$7eve2rYn#Q&L(#1Ad%laDmh({pY=Jh-X?NX>ds!;0NmJ z)!o3%-291?7gbkA?EO234jq~fI}l$88kAeF)LJd!DzyI$s)%J3zlzV151U$*(*RSw zC9A(uq*Hg>M2zNN9CnRUpk}!FI!*2hqE}N><$-1ODE@Lc1_UPb^d*5VVU>FHJL=qu zZi<*XjHMx=WcYqgxFS+;|rp?WW zqT0@2Tkv>EFM2&(ba7o`r7riDwi`9b;00og{;Ca(I&t6L( z%47J35KXT%zh5AWIqG1!N0+N9-4b9MM~5-kdkI=A0Ol4rf=rDB%MxWY})~yiP*&fP?t{ zSO8`!Zo<;Af{`RV8GP|?hYAn-%rOTB)XoE` zZ%RZn0d1`+evG&kA3u|_p%>i8splKPdK^KDa@6K47^B61-@o+`L33nh=gr5aoR{Z9 zI=}c`SBctByA$pC95i|xi$Pg8=sc9_6FJ!_GL7cfVewg(YDh$)Xf*WPQ99~(>h z3nWW8@XKDDao@DAGS7ock;+98giF)l^$7Hy8_}oC&c8L3(F#NL(eHpQJ+<&R1_$v-6RmRpTj=BkaWoQBLf(d^x13Z1|LB{1vG!daeGsvR*!uohUuh3>!`Lg zdGHOr9QRTjJMidDL-mNulzRBUDp~>{Bh!7rFB(v1ak&YA$eW0eq_{pT7FZmlNJGN% z414~r9D8KSv-qzqLd#ZgXkr40FH9K2kmVX6dHPe<-qeyG$Vq*rm&(^fv7yW-}V_m^ROFFdAXuHKh+s zJ!;>dg6u%ce?+4tf6zEINxFacZV>a1{fhvNVt_T*RcMXK=sw}2PlgjM zP=HOeF65AhgP_MaS+uD(2L5y;P*HY`nFG0n$0_jjlBiz#7^-_20Ri*11ybj)`A3wBQiPOh#Zwztl5(Wqu+!5goxd+xlB=I?oa%G#}QJ(*FN{!gkW_>7+ z;`&LuOTV_idYK`cMX6rRqkfzA`|q-A)Fd!8^b|1o_}*!mnUm3k+lBVt_~XeTDuu%0 z;tM5--ZG~;d0vGrc5z+tu4MMeK0bC^$H|if^?&J8JW>A#(z;_Wv<8>4t;yeKT_Kox zWREFkv=rmcxCihO4N2diq%so9u=6P<2z4#uL5Rp~^*DxUlsKa<%$c}GuXa%+7;)5&?Iyo4J8~AZzYS2o+xkXf_?d9j2mjH}GcD7+X|56DF-%x$-e~ z`V`rB4)7!HL!1A~#!mA!wbEtYn-8f^h|h+<{WhiH1>uU>X=Qh8<#s6I=GfO5I~tYPj3C12+FSIT3DQYYZM9c^k3LD(V9*qtsshu0s8_v6xR) z0-og$y9~-E)rRntWj_VBip&SXE2w&|vz!6};9R@UcsPLe|A1@UX8NJ`dn?M0q`356 z@})0IAAUZ8SNcKT^6c>A&PA8HJUG`e;{>Vc}M8Ey%gM^B_n~Pmj|AfD2v1 z26d;di%Af0=DTjpjeq~~xO8U|d&QI^R+wTxXU;JtVSt=;8gVUWKegj1%gsJrjk$OcCGZsMH3o$3aWppwIlw1 z7W1-Hx2jF4c#1Nh=}cuNkM$c{y@o+YbPcp(1Q4QV6o9Ddq&T(ReSF4IwVYY<30|Nh zRKl8%sdRpTZ&4+iZ%XpGeY-OBOkz`ZLw3gTO@yOBvtvL^fA@1XgB_+As3sm9x>K=M`;gL0}R%L`B0EPeGzOpihPP&k@Hsrqdk|ht?dcJ^U^3U&YFqjKz z{%i<8+}{ls$UG4BnNQCi8+LjR4h#Bei)|~OPBUuRQq}h8HnZp)ku9Y=)a*2!&;Jy+ zfq}iRlY<{Ka!(~j#cTF`*^-3Kt=h5O%TRqag?*ExcV8PS30F~Xyv;4(X?~ou*Hwu- zh<&Tce<~VADMV}f3$x_){{D_wA83qqf`)TMbwVdp;8z~frASe^2x})<3!E<;Mohvh zRXRXRPbPTm>qWKi)aii@@~dgajU(!FMjx=+&9&e;N$x130jR= z`)~vXcwXmcpO20`6PA*b#XtFY>I}hTFnXfSfiI||8yT90al*8Ewk)t}=Xi74Bt1=?0K81*xrz75Ia>;&i8SNRd(ORb2C-8-85iyVg#q6yo%P_IJzM1<;Bul^EwgrZ-X}|zsZO4xdmqmK35l_^uk%-0M(Y|SLxiKF z`8v+8^OL%K;jf>IZ2u0mDvFsw%0pIk&vMRdT8N10n9Y{f=FgJYC$}HW$3@7lvp>!) z;E}mxh*C9K-2}hkX{OJD*MwJz@Zrn@%DD7BuxkCLP2Gbh@&amVdMa``dtqS03xwe? z2F6=oO|e+?_1!QU3R{^`03~9RQCE=B;hgbyqzxcv##k*x=0&1urCz_zF2s!eGxgXm zrL?Tne+Y0WVH_}4IGhfXF58$bF>qMbL;%t|8YgaoLW*`~Lyi{9cV8_Ev<))a4|eI) zJ1x7$)F*e@IV9`>jl3hBL};8}+`W6lfV0el<-WbR>htVWuTm(Afb7+4)*J^>HMNaf z30gn3s~?lxTA2l`d_0JZYl4umwLW#n@!>d%2OaN|k7xnb_`K-Nw{iIa0_0LqMN3S} z*>-%vG}MAUu?O46J$+bu7wxT6@5THHQ?6=rppO0>FMnCd=4;_RC&b7?cp-^-g^#2J zv{enHhZ)2#9BzMbwJb1V9`m1XFzdlkts)+2=#8THpFYj;cr)F+V2~H8{`VAW-2z`w zMumKe&&U`{6%}9D7*Pp?Z2H1MP&MYkbW*cJ_l<5N)kwE8(qDdijh1OhSs@5)kpyVF z^*9O%qnqU3u~*A!*(m68KR7FWzdI6ap$d9>T?W0e>!qZx;1lH0k^N+H{2S&9ANb*; zX!A!!IQJWMYQk87NhwBzG@MB!&ek(Urb~+yCD@V6hip(M$&XTh zTVUYp%`bjp9f}A8y{v?$4If(#EiVs2Yh$~;1Bw(v*yzf8ic4*!QbeU@aRF`a;jplA zUAutUQ8<{sEzk1_J1h+=>FnSHMe!X3zAeXII*OTynod1m&G6yQYE{flD8Lwz*;n1k z5j`MI^vC&?@}y%%w}1F=L0M7e*&wa2S-W=Ez>h~m?x%&1O0qe1@T2|8+h6@Eiv4VD zY$BGN0vRXM&iMKZs5@=Ca3I5q7qA=AB_m8T;dOyjc5JTc6!;?6&+p)>9co#{I8_W1 zrXCT8@r@OtcC+^Mfl>!g;YO<|)A%R1dTjPo7gl^d2dcO~r_r}z!wR2J2q?B=@drdgPrQ(KeN5 z@~@Vc7su))fr%XMUp!tA1>_F#g>{?uh-; z{pUT`1O)mT~U3M&`2@E#%6RPt>uPX%LgyV~^aW zz~EfxHpN%N$DM1(%QHg*{y+{7htfeaX!mXpk9zYUp^bDB`bjfbRf(uVPn@*njo8)Q zzwhr}MVU1pB4$tTr6aj_yAX#Gj1Q6B?(ouB9Wxp_7JaVqop_<@D~hK1Hc(f}gGetX zJ$g8y9-Zbewe0%iYB9A3^O7iK+J`pL)wPYR%Tg?lg1guYr2EVjwF{`_wzeahH%f5< z?Rk{cG1kM>4^^0XW(w%TImR~iGrli8VD3D-@ZG)RDCwIdWwMav#=@R}Nde6)MpN)k zn__vLtm$eR12VxVsb|SXlTqP^Xh!rBAqUT4v~>4(&3rk%rlCD$_t0+dwe(?Qdw*pw zn|3WFZZ8Ts4UWkW;8+=S@^8`6(sIwkDQ94xjMaLw$zqbAH=H%2zpEtZ#pf?xe57iv z*UQ>u2a|Ag6HkKRZ=Ah4v;hPWv$Jlde#B?{qY))li%@q|%^m*0mbW#yvOnz8$37 z6+Tk9I^gI#d#z%j2EK7TXR#CoG`E3a#(o~m2YRO1vYkjCRJWsm329c}v$EOwMh~4* z=VG2uP_66(7)s(T{p`R_e#1^t_E#-C&5sJxuU*e*B9zh35UugL`i4$_sQM8ax(16cGpuM*t7YGCDj@e6PPD){5_!Rlgx_p#Uj%F zwfm3co>P+Wxf)y}!Co}UjHnL$+chC|=?FMpwbs;J1JC~diKhlJeih~DX3mA2;`X25 z+UX=sj&NRqw~NbyD-$j)Oms3=|MSEnRP3`1E^t=b+3iy_)H}6l*|Kj%$!)7|#jzC= z`S_fv1KMTRv%B>pP91V?SxxH36RWu3Y_0eBrUSkbKJsu)-i_8J-(Q!Q%h?BFg|=-1 z(Zhkd^f9;6#rw*QUI3BB+bfdqKT%Y%hxl-LFR#Q5-`%5M6Ro?C*Da9Cl7>vQeV^(V z_?NpqN-c&iH@l~(xsKt{LI%t=urM>TXG$zGW6toseNIY+m_dTN=bfEamJM=t#a}LpmFA- zumE}K(xUK7jf?g))aV%@uLW$qK3#;;h=E0{cv@h`-5c?H{1U!uC=O4HEaxqJ6);v7L>^rI<$0mgKB1*YirMa~lJ2dm60 z08Mr?jX{{KU1kE;Jcc4Mz!d|O2}m}BYNt18(6~%wUsEeYdfh&BDe%>L4JE51lSy_M zmk6y+i#^UmzXp25A3uMN?BvlX$FOC0as3Bg{Ksg964&ZtOpHSnpX>1-UtW1;GY4{W zV(6yk_3Al)^F`2?`mR?raD8OHC}_wIXjI~(klq^Us0Jcsoyl<^fsA521-xv4amhr= z&rX5Q*sAiD_05E%o0|tH5zR&Ki5fU^c?0h$YN~@th)~Ir>@v14Vo)Ac-zbsoFrJMH z!@iy`WQ#Iil)LpJ$t7Zl=_d_g_#-(PO3o-Gy{I!Wec^J<*` zfX7s);9m$QMt+$K(-UtqhtijDH0YXY&kvZ^_Mv0Z+N}Sj-ljw6H#uDtC0JN{ip3d_ zTEZC8+yC5Nh)wwYKlet=*u7o#oCTB8A^q#L*Zb?VBeLsHWFnhAf^7|OUGwLk8E;?e z(SIVE*%4VrJz-*Kcq^=z*&?5o5^^XT$N>y|tW zHeSNsK^oht?BDkMuNI&@0Hj1WsgG}24-IuM153+^*t$eccNb^{BJ9pDb?C|SOWyIn zjT$LREiU+JAPt0my3#O+oC-Zrg(Gt#Hzzlbs-A6re@Nds(S8+u4F3^+=2g${Hg&AW z>rK18(1y{$CE0ipfb-8^$O-knKfRo)Vs^M#ZtEnvRW$mr@J&35E1m+qhu=D6p}r-r zs^N4edY!Fn)>J#(^4ZXjgB{Sd-qG;f!MYiOPQw`w_O00RoNH~2rdPYRvdNf`LeDFv zJ2IWb7|TTHoXC>0n&}rRmLIvr%~9P#x-^E&}UT(qvXr%|GCxrnIsldu_ZI__5iy$hZf7ftk{xIk-K!+vZyRo(~&$h@csuj-=T?x z@wZ(q+#C{g{R}F*16gZ~9K!rLBmu4=ml8Dle!_C{#H}@ZyGy)ju^-5ad&Q+x+_9Gc zQtQhjsl3UI?jhe9hWSry>0D795Way{whbHY{MG43LVGUe!K(jwT4iD)h<=5-R!=X7&T0=b-N%Dm2om1<>%Fp2S+ zeQ#W)xNq01*HfGGta;Q>@98gE&4~~yry&o=_uhWxx{_7E^0FbIrlUBEqmGR<6fGTO zw)U#q^$F%2>bLuADykZAr}nUw0%{0TwRRiv`Z4!nArDG^3J(-Y;*wA4d6FmP>>T6v z?S~N&MXMo?QRH;oW6?3dB-yT0?dfc$E$Fp&X>|(bs3l>CtM!Y!3bbiNLk??ZT}VJX z#_#xSKx3=&l7p2RtB{U^Rl9K!JjATy%k)H_~RK$dYWy5Tg%uQQygiod^0 zehF~FW7y9K+*O#9JYq^jEsx88Jn!U{@1&nu=bu$;H<4~>wNJct>(Ba|UD#Fe?Y_ll zokXoVbG`P26d)f&v!rWP9NB}ur=OVuHQ zzn~Q!F$;1wMtYp;QpGdJl67R4SaLB|G*M2IaANcRQ!Q?<2(>BaX2HhrXgmEG*IGDN zf+`(Sgda`M?%8{9llyCn74XxOXs)G_bmmQ}pxI>Di}9S1n)$nk zG=?n)Aueau*!Q1lU0Gr}Vf?Dy-zotZiN2?xI_NGwUX6uO(YaSaHcO3OFzxtfSvwjZ z1<-Q0L2Av3SC?lvnN#LC*)sM9NhzSuNX;li;JC_vt|*7dmkHzx(3%LFm&Z?EZ3i*6 zUPI-1oNuDg;!{6Nrr|rgZc2|QpSU745-HSS@^Y9;HbR{uQUZW?p@HgrQ_Jc> zbO`5Gj_Y~`$s+ap;pc-G`#HPh9e}q{BfmRfS0*@H>bJ0#)=AV^zV$y(Uc~CBVWGV< zY(!-<*0pbkqjvjOM&=_$I8GHCl%F;9$~)M;(U8_gC5w6SiMb0x&~ShBMjR+sGaKAN z=8*S?c(!0e&P1wukmn0d?|ADA5e{~6<{GI67L>Q>IU6ZJyUAC3|FZ>Al;amD+4Q4{ zr3=@-@{++;%10@V^nZxLiO+S?_e?RmM3;=+b?59ES|`4C_3F-6LzG7H3I$QhEjNs4 z(PoXouJl#)6(#NoM$_+c_;^fbT(-(SL!`awHPKZg zR!F#U*xqRBDb^QWaSsu{5_mY;Vt!i4I*X}UE2}MvCWUISD1ro9I zKU=M<9B|>#wS!D*)MRrlV9nX4rSziOwPoTVQKn1w9xhX(rgh1n%CzH?7&VEQPk3~o zr5M;Hq}!~p-b+i?ce9XMB&F55Kvt8-lSEO~u}Yz_VawaNpsuSm^B-wQQ4Rb50Fn#S zQ?9q_wb09JH1%xcl{d_CEG*0(jMOkS+*`75q{sbg7ly;Gng@SI(Y+n68cVvM2D zVt-2*HDi~SzxCaC)jp!@)>Z5B*RAV#6#d+`(9q%0B@O@>Zl>NeI*nQcP8m!#+r{+f zmV7{tJH&Mj&FqzAKAogK>@3!>xX6(c8pp;<>gtlZ+VFTB;VyxcDKlzn=5xgl-0IR~ z-!32iJ(xF_PpUAb(I}uwTh)BH7#S$zKYg40^P$JJ!Rsu|<`h#>C}v)DvcziV_VjY; zsEeBIXljHdO8nN;nskg!65Bf-t@pM{#6>b!-nq0%4yb?Jvg(qLb(8L+8j-;&8^#Z0 z$h*SRny%Bhfl@B+OkL3$Y8te@jdkAo5mrjveNK$6i~%vk%Z94`6fEz-YZJCTD||Sl zyruC4;E<}`hg(|8jdp;RTEyndlx2y#SjgRlKU+F)Rc!y+;eGr zDHtus$`<=63+bcIEjhI=OtHw_`A4d)Ygs~ z9=6(G*^(u89X9uu=)j(#SwLF17iFXfv99j)M@K*(&HR$Ms z=%_8v!4cqKDUH?zs_D5;-7%l7Nf$NSy55qm$$yNv%M5L zptIy;n}471FcCq}Nx+;o?Rfx$jI{kfsnT~v=0p7Cv7XzJe|hqIdC27sn_I1W@}84o z?Rj#kj*;_R>Td)$2Pa&q1BwKF@_@d~GkXbe>}FaxkoSa1`dQuTFm>9Y-MF(yamqVJ zxl#V!Pp@fHnNG87O{|g8GSo}^s%Mv6dh-ZDU0=@YL*jWVl(5w5kUI&HE~m$xvxPBO zexm#K^9B^Stp9$@;Sv;u&$r>vYH{xQNEcu4+~_s+z_}?Zr8&E?Hp^&Y?F#v5?1?9e%(P-KGxjQ8C?P{ngBz;!^j%%f9`F*0z(wVGRARiF2hCqI0%iIPPp(GrOl zCFs4)E^+Z7OHREP@K>}$Hk9~F-Pxvh@pF)^KL(P@W>EPFFcX=4=wIi3=rc4Fk@<9D z4iSI)pmOJLj2dif+c_}lYIBuK0n%TDfD1`Xw{G6dPB0_spI}mif?#-WB_SoPE2DL^ zV#*g2V%HO(TC~ZRt%^Q#ODw-@RaFgI3xY+!8Q5}=3?FV;akhx<(<+Hi@Y+W^L`I~AOT+v3vW5(C-yTU?LDVGC;vh})CId63# z2mSb^WITq=0>ych-g)&vjqE5sW0NF;vblx@`>#(mNh~TTuxDV_xa2q6VAC0<7VX;y ztW?umjEAXhL>B-+@=Xnrz>%tDh5$=K#_X4}7fbfqHvjCq6!Qt~$W>FPc+xMpyz_Mq$lY-`__z4vZuc~{)~wZV`@9<( z4F~qSnc3p&o~xPe4m^;qwpJp1Sqp@1qV`1I==9p^4963UYh-f{I%nCwis>h$#_}8n<-B^eA5cyP7DNOx z@Ch_n3MKl-Fj=8A3u(F_K;e$Z4shd{Y&a9Gq#vT;7`d|9RF(4S@yE+L^J2uQfFbp< z_o6X&YuBgGOPlX5n=IqqtDNX7=elw#cV)A5a7zDdG<2)$x&^5c`vsOQubO_Im0-9A ztZ1EIJ6WdW zbBy{!)YGErM z?Lj^PA^-6q5%8o;R+E2S^wlDkQh)8MYT;TKurgwTum0=^DagDWD&vqwq7FJVSBv|F39{ym~!ake1r9{hcHB4AJK(<&Ov;3 z)yOeGZIv0M9E-`_GLayva|oyZ0FJwN?>3UO__we0KWX0_R@V864(H_e^r2eX?K*UL z_j-I2S`>g#-NdWFBpT{kOuvr)nPXKkBeICr>M@kFOL5SO*#b8zs<$G(MuJLoD|s<_ zE@wz6L~%Ir^m6}JUkSQtS^T_zjfjf~Qag!IAQqvB^qDAX#=Bl2t%u|vyLizFg{31o zgd*}RB!_ujCFa`Z0i`!tb)e1zF}agHm(~-G9b6QVdza z>tBC;o{8vtl9F$G`~k zKOqaL(HLp5f%!cO^PcD|h4A0LoEOV8=LC=ZqmGOZ0OXq;?`@7m!6k#zbHZ{HM<$o` zqcQsqs?{TcmpKz4u+jLr^u!{^6|KvU%>wT#s;JkDX}%0*{W_qxDI}PxP|gX@8!#vl0#sX&ynH}&Y}++D>Mz|_q}gh5hIZ{w|6g4 zBF6JW!H;57n5JRhu|s8Ej}`bB?saIg8NeWShBL=qn`EkXVl9`>wPSg01U=(hH|N7mNnqzKY4Mk@-EqfY+(`7 z+5-cgXLrh)cZj{sO_a{~qyres79njn-b2~~GXDtj`Wv#2@viq|5jekf3YtvlxoZRG z&8rOAMi$WDoW)?;w{!5l z%>!-&*-y{^a}TJ!-Mkurc{<)&8`Ng0W?NuaU5SkvMS=?cxq9`;KUU3P8dRntvDjc! z)sRfz(vyM-Q*IrjMgE@zqvb~B5afIVd!6aq)V*{9bC(FBmEq%)*TRE|o}*yVOW)Gj zZ*y|SFKP_@pC;Ncbl*{C47Ss^J5*6sJj80$rMu=!z8`y}!4mHH=s zg>F+!S=u-PHReV}zfI4-Lw+wt1a}tCqcVw)Vi(qcYn9)1!@BkB9|}HSrBQ>XP4CL% zxJL%8G_p#=PL%C@yGDij&7U7BUSz_*afCgpD_SemswloA$XeoM;PS)m$Gg1*Nxejh zz7z6bc@%3CyYD2-2dr!slvsDzj;8rD=5k!vj}Yf)Y-F^rvVOSmcK@2@2dY}sXWR{r zVHO8mMy(KS3SCHt{+^m?6dcd)Jxgn~|J#G$zp4ztC|1?a7o+BrpLPHRVhx&oV0K@2S5j> zxbX*PF&X9bVkt94GB-L#)B)SK-?mn=I!OgzL-8F${0MNv!|@pdEk|!98(UdT=Eira z6lIv#adsQ}GM~ZxjY-*?2cQkC*`XDa29;%yhh7U0dA0Lm(666Fh-pW-ggWU)bBRaT zBsqT)zwj`Fth7N1Hiq-SXueTR`Pl#eSJq+*npy839@d^xjo0J0sCaYdo_4wLY7-Wk zWE1WU7W27-YGsX~ph<0LciS^WlW6T*p014pFe7Izd+ z7boaPR5z(&!1qV zbz1IS%&~t}K`KkZ4Op2y{5LN)yY`ofy;04!@)IdMvN$*npL19h^?-`kSSRkZfZ;tC zymRuna`|#^OSmsaC40QJXvsSSQ(^L(1Iy$vUv_Osg)(M46SWBJbXYwdq$Q?fz=X=S zpk&UR?mK+=Mo~$?*>3w0P1YMNp`3QjqgYHkPSvbiS4K}5Y<5KbArfx@Kjs{^zye*q zd>M6G^yG^9Ugf`j+(Cf&u47H-wi261i+~PYuU?`r+;l_yXF2;Db`Rmcf&ff408G5* zx#+ENKvP5Y z%?2_}MKY25@^bmtVH-}+$P5=hG)7}O|6Ee-gD9p!VS<-^g35*Ym&fal>(*mJuW(_k zH+%plBZhG6`yH!Mv!;|+S(~WuWc6UY=4+dg_=)eVJD3HtCxjCqSEc8l6WtxG;|Y$n z$TazfVxa(~l+CUR1P3r#cOE_Z8m3-Xt%o@W|Kc`o(=oLF;x2t`+BA3fJh9sVG;QzM z0T&zV)ds6quMX!Acr=QuSUcst`0bFC%>-@O!Alh`f`dBApv2~{*PumnTHbC zj$0s!SDoGVf zNj|9O#o2Xxe^D8TbjPkUhp5}GSgLsuB${k(<;P*BfJIgmm|FCbUxq(0jFQxWONiHH zL!?szC=II-xAw2PWIT%BE0Ov12mvjuCXt7`FViA5J(_j;ihF)(ccl^rijT4+eWdr> zxqNGQ;dyC~(?e*(fy=|DV*_gBLP#!-d(lYuC8E48VvK#?3Bv+`Fyd>(?&9 zK7b4_&<6+A&WdIBJQpn(R>dcQ)TeFfhGmeOxbE>vCn(i^LpD;0*|W6efa_n+_82cm z&KuR@d)miG^ycU0yy%7D*hno~`o=awhbcxP?kB9Zv$TAXt_!zh2ds80xo*x`%+*>d zg9r3;oQ)7zCSpc?YYJ)0BGPd%)+7029Gj(YG?Wd1z+qLD3k+}`>i%*`|I=50hHohY z54_Gq3OTjO(gu$})f{On&1*OxrV4T0yz7nUqHe|7-uCvtX>7Oa*Kfs^35;Lg(eTij zyjLDsS(E0?n+smRY}s)jqLFLMi$a6RqZqkPpotvCp7>Su{`GJ@oF+{QMFT?enqFS^ z12$+3hkI1h5#;OuOiFQ<6vSwuQHwc~Y0v}QT39X%TB-KjkxWyR*mkcGZ1&VyeOQ-- zJZ!zp$Y{({D!D@@7H?DenQtl$<7cK{K!yY{imA{zMn)e~0-r_kEq+K6=v$VZ@=Mku zt1J5kK+hJ@6YDh#EP)boT3)#~Th4m$j0-dz;)Ef76->A!GIUd?+P=FxbgLAiD?KJM zq}u7?uC1dN*aM>rA!EGHydl&HzOJ8v1&oJQ-1?o%--vm(hK14A{`H4xeLveBNEu?x zu7mRp*rRC%i@6sBQN2ndG_95#PkL;+x9!~1rmuHnb#p)hIsd6yqsEU#)ern9>O(Ze z%Srm%S40EoLpGxCuWoUU&8x8a&)RLgmHkTtBVl?Q6Bkd_W2Bu_&T}MIh>w{7^5@+t zu+AYT=A3z|}t<>hFx2VsKx15`W{l`+zC z(E)7;lri^q{0fzdunX6|ZG9_rXzmXt)~xx-3wIU)IUGSdGJM#su2c4DD#z(2W*cm8 z+)gPiy&A#i97o%6Y#MF8xEbcXyb`m;rGh}u%<{;*Xu66iI;PHVhidJ1Nx$-@^U{xh zJyGA6^lt|y#I4@B?0)KF=h5;^o)|Ns!xU~LzRde5Kc9Vz#tdZ`)j;^9E0@UTCsZm0 zVU0Z}2=ehV>~(g2{@sE9f==eu1LWUd)k1dITK>#Eh(CexR~OhVv&sM)CU1>pP^k* z)esrX1H9`}RrhE^vCX$0FD*AwS6URA$AR*uF5KRSv2YO$2C3&gI+9%RsX?Bi1O+9w zePfuUHTW{33pOR6hNIe0T!Q6V9#_Ya^(`GE)FXO@tyX=3wwX*l^6hSyWF5(4Wb)Mh zbuA6Xq&@U|itwSK^~R+Pdet7ReqwjxiUpu3fGv|7%$}e8l(PBs)5pn!XwM#R9^kME zL6V}J7{g~18EXJz!W`bwOa0MsNO3?hw*Mb7x3s85gVQncm;Uw@XD+`RDE_|c7TCo0 zqIe2No9yOmN;hvF>;t<&WZcDLmQNk0UUxbJ_=iLH;6?^YcAQVFT_-Dyt1Mm^5Un;M zlStUbT@bZcqmGkKaB}DUmM{4{G9Put42sI2+Hqm?CpYI~*aX6@kaQ&-(m}kZn-_h( zjJ3$1JCXb_h9gFI)L1f6mR6BGajv2mx)&xcc|Un>=ZGMf$y{7y&Q14|o)=vlXl!eE zV{>igxS&9WO<)&B&wT4P z+%1l6fkGDDAVp?Lf1s*mRPK0W^%FDP*0X%SXG|lsr!grPJ#F0{3|Q=#l|73y-pJ(P z7;4dk`M;>Et(dN;B#uff(aFikl$;ehjme^%LbkS$5=Jf#RP0ZLhi9S9?nLS1@JkB) znd#FP|QR}Gs#otV! z?(2$kQl)ypsHoLCk*#cTL6si`GaeVFoOsR;W&%IH^KCazo7d7_Tog+ecZ2P(s;Fi! zSiHD>jkZBV)gaO&wWUKwt76+(Pt^5?mIFm&!8vmskIfx}Jq1h0qVI*7vAQvR0RL4{j^VRYaar-wU6WH(Gc2LsxQ^Tq_l!hu@XB|~HkRCWUs6_3%( zWk`{;?{Nu%DaMQaqYdqJGfkckB|jVML`>lnb`Olf&-ISR zzaEd3Ia>;>A;2ucdfT`TRm0Tx>O+A4!J?OC;Y~ifgJ5q4QzI#oR}1 zY!Hh*fj&D&j+6O7^x}^(!FS8|-L}z`PD?}`z%u0@(&Rf`Y|rho{)$n^9MhPiM{UH% zFy&1PI4M9oLn=!1T1tq_Q|m|bNgbrOQ}+@xSNf|i!Li^`m=eDk;l`1TPl%N9Zc0Bn zj%AfIru>~D(OMmhj1p|-j2riKZ@F%6b%Lb7TPdy`P?h93e>e68g^K*;f4B2ZfBMxkXQDd}lfs+B5?Kai zF^hR-sv75mO2k;oX&`EGJtC^fz(W)7EenW4cxz-etN1~doQ}`UVc!WPYT%P*jax+g z28s9+7jmW=Adwr-d>VqhX3<=ls8#p`-@`A#4Ow9~^96@M1bbO}tNmL;&%gb1auIYxmln2nrO z9HT_*)y~wkY?10U7qo~6DzPJv4NRxkytoOFemFNbH>A9m3<_Pi_3hq|iEKx7`@o0w_(J6RiJQ%oc>E+@+ROoscuOS|cM7{_8F_y7>J?MO zBZU#7pUssmK(xpbJ>;S@e)NbdEA{D2uB8ZNII$Sz&Kl#>yQ1u?IMrsqf4?*-Q}Cm> z**&3GQkJ7c(vKF8KQT;fwiuPg?ma)E)<4WixbZXhNf7d|zg6ROL_Jw=3xf~?YCUQBRAhJyQ)|6fB zxU|nQ^#iuLlbk%Q%UDrR=q24xOti1Ghr8&)-+w@eH@);umJ=~9aOZ+LBIEcd)}x6* zJc%g?IUSt+WqHskl*&ddKmr&|!^P*ggj4tr0ska^K7T#|R6CE+Uooy>4CfrA79+6Z z8NnS%pq~A10@YbpPJ8`d%dd59tMpwb;u)&P_(E=%IF9I3wq_uW=@R%W-DT7JQ!$^W zu8OO^+Tfx0Sbh?-LC{%ej;LA45)ypL9##f83{r}o{p=!hrT$Ha&|!+%)xc^V2}mPw zj>LUc3V7#gIY`dvwsjNxEX=K^k@>r?@6|pPWefB(0D_3wD?r-x5+`CMMTQw9x#NGn zLS+AzO9r)t_5|C%JVr>EZp&cAhJ(LsiY&uTMSG)v58_G_<`w4yf7h&ZloQUmRU4p0^JK@jn)9P z(MRw=p-sg5QveB?V9~KO>oKpd$dF*%=?o39DV@j$Lo=@y*?~}AY5c8j#FYA(Aqevo zRsAmAT)$-U52u-yR$9+-J4{djnOFCB-ZvrR;)_-jzK4M`{xx<1VSMJAV&Ro+S*+vj z@EVTi8?`Z9#v|; z#P0J^4u_ORQ+SDJ-03^BIsbRnRfI{+U#pV1*q7*NjiZCQ&Ux-c{;Z4yW8bO29a>j= zm&M`^%>Av!>;kZVs;QsM8L}I9p!!7@5yZhViIym35vwT|;U(EYRR?H(SIVno1_vH+ z9-3ZyLr)czqO1Z$@i0jHW|=RyZcr8?42eAGM_^@kGL(-=t<{yX!Xy!ddwqU+Qp^mY zllh#F=-i~y`5+8G$6dR5^Hclw?W@n^>M;;#xdfQeh<)LlS;M!QY;UO8$&Ki;i%NI4 zO9o>JoeiqBF{^#hqsPyAwp-P!YBleDlptZ=#Q6x6DLymP1yoKHOHe47A>X{wVEZ?{ zQLE$SmV~Gd=Fa_PZ4=n0UZQz7okr(xKb~YmB%sBWR?fD(8L>Y;sy}O1bRyK->|G^t z2tGR6`v3iMPL)}7?yi0YhNL5pWjd#BK^i72pQJ3FpAdJr}U|pd^y=_53 zt_FTYZn1p%6kFkI9<*&bJX%Vb5DmOM-8uUm9DH z2S)VcD#cb56emd!+ zpoohXUarI~(!t~c-eEoP!s-06kawIh6TsSgYA#KEoLXGj9sq2wmxKxX=|xo7QHhF$ zdtsde;7iLuzAC2a57DStQSSnWF*ystaCUP%`k%IlkN zhNKfUE6Ej?xmi`5#{x*?;TOE6?p#!8j)Z0K=Ka(E_jM_%R59<2th{majJsGq{|td) z`l!`9iAvaK;W!*9b&|LV!^C?SnfwDnVxo&cK&lv$p?q?JhGDSfF?%y`Ss3ejkE8(8 zHJlpE^}+D~0sK?SWXu4~#pBJb`Zo=GgltFZV3`BgtKI4D7CjQ8*&r+awAzKXR__hymuoN#Ws&jX8m=T)?1mh_0MKj_W0wgM(VLSl#W4mnoWId z4j>u`^($YhpFtE#L&coos%ln?b$W!YfVXqgBVN_@O9qtmubt@o@m!iC1|}UkJG&6; zrWkPDE>#+J^bgb)Siqu7m#r)37A|;3^Xl(JwKzH7Zk-%L#%MaRiF?m9+R*p(s&WgoKKoBVOHN^q#8=ikM0yY4$oNm<}Wy%wTzoYECu` zNBU>aVJTK+NuZaLm)8ymapay`0xb7kCzI*ZuO_ajLJsI0I3BNBb4`CN6M7jw@ZO23 z{<$sBL3_A!XE>y3*sQo@08NhU*Y8^U7B@1Ha_i3mVY)G4LR|cWXwQKNqs;>?=Vir> z^f}Pze0!WnI5^f(%(xD?7OUU3?I74Q^S~F#8<=siAqRqG@jL&Y?cV4Q^vYe$9p9qd z+%AK@+dG41&{ICn)eZZUG-f1|?+-AK@;_A|6a|Qh$IMYa89OH zVm_K7mw|yg3rKO8hhpByxfMgX)}D+VF2v!XrY1-Ci(cjZPoUl?!mHv%uCB1+UiqSy zh{FE!?mLzJ1>Gut`du-aex3K!^y)JNL3&BU2M?a|wTf2>1^Hf7D}YDGjO zJUt(g--hn48~fm1_*MLZpYPOXhxp5|TF>lKyjX!#JujF)m($uG;Q8U&-}Hk3o?68X z>(R5PY_>#^5s`ZdM}mjU&GVT15qKTA1p!v8H#M_(@=3G0sKQY$f`vV1;5TEa>d#79 zhxww>Do>hRegN<;a)p|O-}2`0vXN!jh`#uiHv06W*{=6M{B`v1*NWRML(xg6#AWGC z_o2RPZ)f-IJDy!x9;PAJ5>!6N7-@;@9(3f;pT+mG% z@x>i}Lzy4Za0ahhoUb|M##N2fD1HNQ$|C3uZ*hFZ7W37gjr~t=Wfu|> zja>9X=9w8BIhh|{l>6Y?>`$>hpj;tvC3>;Gy2x_hMett?`FS5-dg41n>JGN8Pq zjEuY64+EAov6$nqs=%2>(#d>i9kJaEys`L0PR_J%x1Inet*lc!JsJ@w6@P-wcgfm7 zClx-4DL!GcF!l6$)>4B#DX`>_;2 zF|$OM@FSgFhU3q)h|64TM??<~D_tRr*oo${e66FG*Lj&SEGt{y;o>250J6u=+GUS4Ss11Qqit~dv@$dxUxrz$1+WUH~eH5j5KPOtfdEbV+2|12({S}mb}*Z^73ni zSD?4;Pr2sbsySFH)k-i`)$A+lyBHguY3j#$s3^)DT4%F(ql&Lvhjz{`q`NQ*Cw*RN zaWIQ`4BvF8yM(lyDK0i!_Bxbr60>C|FKj$ZO6#Pz+L~+y!mL{w`Y#1;<#|_Gi0?nM zGyt|n=@ICr(x#VFXpM4obPUT$KWeYfi%cl#1t85fym`65q;!%!v3XRNJ72`aq2Fnp zRw^<{#M_QePPq|jIH@}QXVB*x$U>p2=JWl$coDo)zN2YIBQ3#$@+w*}Kg8(SGLpAx z)DuDl&$uIOYnuDdL@GX_YU&m5#g;HWPjMm*(Tu2%bh=Am$eukXdFXbPLgdMU9e-IP zpa~7w!C7LU=X4$|q}#7|T26IW8wA&JSY-S!lHC>wcTnFsD~Gt1zRE} z(17uSRt|CkTYX~Z{`fJ+| zL^oaJen|s*t|o&~GadQAg9;LnL%8h*xT4s412m~=sMm;ofr3>>+V|1!?(RE74{q47 z0mYxk@;%Iah#3QOSqm@v@OY2%fR0nL?&`U~eP@g0iHlzxrlr)K+?MrZ+_HHI-UG`E zlR8SNPS3)S%B@w#U)s{UWWFwLU_-SBtG5XBn?~q812?xmjX#596u$35JVfPrKM93C z65D7PDX=;r8YAo#69;MvmkjX=?_alrC?md%mlXMYB$F9PC0!Q=h@$RFzjObOz4!j- zx_|%wpW2;G4Gk)lq@pC0&Pp1Jw6xHml!*48v{N)SC{aoS5$#0-4JwjoO1m_aq^$4t zc&gXg+3E8Kd@q;x^M_NMJfDxp{c*n^<96JR+hNfqX_b2iBdzwPrU5giGVCO_FT1_% z3riteh&;WnJ0Wq^edV%7@gxC+G?mB{plW`2@oom5Q6}8>qcbN)+DA|A@bLaF`jj$o z;MxbmbXfa34WdJ^0CADqb#3>86|Vl62kNkZc@W;?E0!7?sJb@_@-dEJoCKbb2)2|Xb#9AEi_g_U(Spm#EARQCnrE`3}my;34vEIHPXB(-jC|{{F>!02a}gd zl9XhG+)}7u&4d6%eoraHks@8*>nf5oZ+RkkgfORY8kMP|j)q<$6eo7oft;LK#+4lQ zgQhy_=2PjohaJ$7c-2Eo$@T6|!9yY`GKe3#gZa~=1Y5=sumoxz3X!lR~w>6@T^A} zVUTHf%jI1_<4%#`U2jNomk$nLX5$=~` z$ln)O7gz8tHQRSQ>ty@lA0r@5Jb3^00%~DmCi^s=--`rg#F#OSYwY$=p=@_NPjPO= zigM?1;E0k`s25{C>-B9jrQh`v!E=5N_c#bu4)Vo|nXg6<8Wf0~orI8<@L1rR?xX+D zxST^-Y_9i93TVC`2a3(_t0eEL;TsN!9-4i8j=DZV+1?WZI?rC~UB+TFHSkRjDSci) zUmmy9{4}4&IPpLS3)(n0Bo&BEKehZi)zXfF)5+Xb^GH)vzzQJ|3D=vBp-0xY+!?jW zxSpjdM$p7$niQFX`+r6)yIs}c0D*k<&BFI=DZWoS0F;pVDZA}_AO4P29l_s_V z7>S_{7xIssa7zc3`tQrb0GqL&0Hfix#*k(JMXau=S(Bd#0&#=ksK?eWT}tz7GA>as zayV)*!iytMJo}&>TU(;N*$^6Bsdfxs9EyfaT*b%&-QP#U3#0XyhQK&&8Sh~nxC_m}xS=*aK zx^_&b_6%a6WdWRGJ_9p13nb{T3zdI0KAg}ycP-vpLQz9^Qm!8ywpI-z9JQa^{sOGi zrfJhXDKp7V2-ss|%epaFOMWT+9m0Pn=@!9d0Xc9E(in~Xv6F8bFL>P);#gk*ACZPZ)f8_Ko|a~Fo_Pzj zMP>FNnPmWAGyC}Z2mfM4l${ESq8r#x=`wuw4z8?Z{c-n@0v$o1(zr$!|$17P_ztWMr$xNh^)riD4RUleT+kl%gNS6DYs%U6k64+7d7(xt&>|Lxlk zD0!J=Vf&~ipPolOhCya@>N)B{9=j>iB6~lpA#&v;xcb{nAzRYD{^aQbQM~giO8CSL zN6c5L2(PPuwI`)4Jx(1+l9lP?VAAokDl1n_btNtAAn#*03g?JOKc9vmX;%$CDOA~r0gJOTfx zdxc&&03-my(}s^CccF#Ju!dLn1|c+0(yKi-$%vGT2xDM+!oAjgq!xmtL`b(dR37vY zPJ})wqo}07PEhVjg-m*#(eXXK(PO#PUTD(wIs*G7NW#2@7cvJhq^$>N6Bv?;y7)hR zkrnfJl1E}Jb*D|onRTT}kIlMJx`jJ9rXvA6?R8g>rN)AxS1+C$&jfRB+Gz86K#<0} zrtB%vdBQE7E)D{PQv1G;O3k1Ox3(?g*|^XP4DeokLd@Zutn@p<>rVnRjxvl{HR|k~ z{!`CouS)7TTwDsVrjK{6_FbnTQ!t<2<~cjM!7rLJ>tH(jS|$w}x6RN*8F}g^BS7}P z>F&w6Izu$zyM9UN@TXXa@}ahHm|=%+z z9tH;xHEJ2eJ^W-v3Ny##W`T$Kr!~1)=#-eN{`8~zfw1ac_#mf~x~z?Jdk zn3C~zgXUW7p8LV@$XkQr;=KHb<(h#Wk*8Fq91N_#s@YndhE1oGo08YlB5|>+;hl#@ zMt$^*TGfqBF}ybYR)XP>_UkU~f2h-{+V+SU$#qUXcXxCu7|~&Ci&xXCpW8im(ZMc9 zZp}U){!ZiOkHY%LAo|@s6OG z(6pU#wFpfK7D#fvjhBFUv$rcXbbmYF67!xE7XYh?^3G`O$a4$79Z~9we=U24Xw9I0 zRgF=BUX?0W7B=5tSK259NjP^RrzVMe_<`LEEQNxxWJ$NB%8m2y|HL#og})VMs?SkM z@*vVZuC~}mBtYcT{(hdYTk$xF1t;LQ#qBGETjf@gbwG{F?hoIMe6_F zYjV@G2dGtBN`Mq-ww&(mJ>(yHW6tgHmNe07jW((<7Ri7(?7j(+9i3j$$DbV6K^(HW zF!fMfp5)uP9i+kA$cu>apVGxt(#;^RoxZ*iIlS?F{#s%~rDN!WdvCerZUCAajnwAW1Ch+K2p&ay)vMKM#k19VJG(hnM8KUz0Pe~$5*(;<{xqm-y<#&w-Vak(^H19|O@4`&`WdyTQ%L_{F1ji#i^dvq2 zGdtSzZZxM~$h3eYgw~ef=X3p_*zV^+2c@1z9!0VWioYWyD1vLk486UQFHi*8pSx(J zt*r|zeR9V>o{3|nB1u1uxOJ{ocuheO3+)JUD@POn_gmi-JPZhtS zoMQEOUNFI!x%yd(MFIfA0Jbcgf0~0R+m^GS1FWEaI~-PZqO;@22PX%De6A=k1sh9u zcBHlOFfjuMwVYln{2>n{{l zQt~rf)Ag^ZuX8LeJE^E;N_EP;ts0U+?C48z0l5%@bU?pFj`MmRW*7`6m=23}>N}Ia zTf4*WEBaVmbnBpz%;+@f)M=W-QiGSmnZ)>eJzmicG@Xl}Vts_dSf!#GqZmdmhgb?+ zmrB__9;VX7Y_9%y=gxi}{cMoRU@o)sv22og9JPb-lhGSc7RG?ngk!V!%hl`Gtr54< zLVvP1neH(5FU^8F6o>)Si}zR9E8&PtC=rT+7@MuQb1BbqClfv|;z_%5qM%=hprhri zwh|KnH@C2$|4GDH;AYBTX&N?RqrD&aWWbUiZ(1$}Aw9F`dnEl@2jSq3l85Hxq&;_v z$I7UaC+~4kr3G^m`$IeAZ``?PG2$}amie}!)kfM=2xSM*39GrWVomxkL^1#^mSN;x zOBLD;9W#>IQtiK=v3dhhiTdF-Qp;1>1r^BTgS@*s##Mz%ZcDIRuCNUV zA%+bas!W-~miD>5Svpb}Awr;6|9mR7t?`bIL6AA0xYsTdJ?q8wx?#+l!eTeCYLilU zmH#hUaFo9z4EpgCCd9H8fm8;H>S2<4I840JP<~rZD{x^miOxuppO$4Z(hDg_e=y8s z+nJXiL<%3mhfUD=ZOtu+_6ZO(Bg#zJ=?4mJO#;}TXO*oE2Tc0D zB!a@*mn#h|b0@RkosSj8S^Ihi2)#% z+!?zIH&vy9m;UeSD4a>rk3t2F& z<+O$R0TunO5Ecc3hDO%@P2oZsbRasU5S$ucOn}nmzI*PP_c?yuRumP%k%QB%d7?`L$;j)H2R!9%FGUwZC3 zXV$EtU=JdO5ai`f{v~8yA~L24zrdf+k{trz=Y4-ipy8+7T;uy`9LD?Tv0z)xZ%1cB z+azfc0ZW}{B5>|rod2ZX=*yd;TQm0d;QZo;GIj@fb7rWR5&Etuo(l2rHa^*iMnq%= zYF5%6K$M;^X9}ZJU;<6tqNp+rp0c!$6q-!te@U&sOuT(6#a!9L#fqv8q z_iLZT3(4C{?K7+c0(ba?nS+)Zv^F#xD%3>~KZhJUa!=5#+w0n0z(Si|Dn5z04?`1r zO3ZHmp_KjPvDzWQsSQee&Iu;t*|hOyZxd~T}XStAhBWQo$+{XEQ7 zGRlqpZby&Pub%=HJ;>V>422-hE3h~*DQU34bhhUSzYalHBN7|sd#ea+Q+5XdLKp+g zZfmNl-GJrDf*IU6Eos2|_3PbgIlPn>AtF0MksGfi=ZYyT(|9fCN_(>W?yC)H$Xm}Xx zC6ZbIJlH?Ck%K~~t(5A(X|3#{@b_Xg;JRF?VBx(Xx6~`8qaRwlS+ONh$Kdz?NVvjZyIm|XfXqnN zR!Y*+PUK7@1;zC-zBlqIB&WcH6fq|%vu40PnKdtEqQ-Pb_tjmr7mO?Jzl04%8o~Z; z>gb}jI%-XKdSZCsZzniEdj*-3T9MPq4`d21Xyyo{usYDoIeDKa&E9R~m@6CxH+CvC*;(#vQ&!(lKLIPQ4T=UI(8IB=BPkH-Bwkndw_^^H@oV)}04^pNg9j7;(YO z>j5_s6KPcs6PDDv^BW4I+KxIE2rTecVmImf_xDeH+Vc=ygN~8|-Q6YHFqBcEOmyl> zQwxo)&LUMtbx1;@b@q2~(9*bpK~2~CDhxj4GHh+LKeuYU`}pzWEX{A$!D3PtR5&<}PtqoA~#IbFGVa{wY@`jgoGb9TJA&lH^laztUyXsj@3 zhFur15h_!qvL3Pe^{Yy!a{n2HJ^`$rzuY?;EUL0v45PF_E~F$s#HmB2Dpl@~AO?t8| zZ(R%z2X?al0)GnKG5~`n#p64ozDTTw;YN}JUEk%W+IL;y;h_k+*OzWmBvvZtPXyMx zOR^u7S|8GnP&Y0WbtC}B?O%O9n@C_j@|B|ylNxYnQSBlW5|T-pnC{*Sg5zN^z0)(x zRjj^$J>#h*P8vSbVAwOGn3U*_>ePL(>~Fb%RCb2$lxpG|NR9JW1C>2y! zUE3cF{aRyFRFKDr&N}&9>~l=c6KBkE+*xZtu2rrXA z7J({on5}0<0qu#Xm{6OTh2kekI&H?&GFZ+(UxD?1%7&Y(Ggl&bg`1P`(>fOcM}5T_ z?0j**k%opLkS3K$pXGzhnVqAY{fv}hWu9}y@#9EfKcjZuaf=KqKO*hY)ZLC&Rtuy+ z&tS(h%;0kP{BE?*F~Yictc1Y2RVysN_U?|BYM!vkWqMGFK{g&$zP>{DHt8J#5P?9M zGFu@#EU1Ve%u^I3J`EAH!fw0I}R~2o6OLjdNYfCG3c&zYiNhl(j7uz zCO91sbU<$-_wRagf(Q`7s*4R$J}0mvKO{;~`cXNaa22ICH?;%ovz zfjmNAEy8q5#fdXCh{UGk#TNk^^8(98!e==|V6f z1AnMBSxjonj6nj6SKZ6xtz;925$5)qL=YrX9Df#@bi3Uk91}q~88t+XDnqQt%q@-f z)!bxB!KuioC~t!~sLDUNVbqhn7#T>TN!1+v4`n)#a1hgp$p3^Rdj{D`bEzQl#5`JK z6%qB}nM#`8OxvHkzZ3?r?t85*LUxUn8e+nWGIOjO`@)@uGO1- zs)#id(X*We-$X^qBY7RD&!9_!4M7!ri_w;m_Po_TVteUdQ%)SD)T@J_uE>x`?HuQI zq3QO$eiuHCneSxgrH?;lJ<;2JwjwmZ@+>bpe$DN3=mlF>AK7#kN z4{+oy#(yI{yi&+RRcDG<>_IYN^Kb#Ki4>iQ#9Lf=Oqb7FC`^|9`V9dbv<&rhz~@@L zrJKVJ9cwWA-qfUK^cmQJDIdl)>?0EP5C zn0*0_+husQ+8-I)Bn3C*2?%BHayUy54IW z<>s}3oQHkV55(Bews{KJs0_Z*Fjo{0AsR89|M_{XL;kD<&rt3QqWn&*@wmb1j|lZ- zz5vwWPzbJ~?!odr8&pcz;IWByzv1jC#m0BjcQj=ONH%dij(`=?(D+mn=u^2x-#QHb zK|bihA+$Tcz({r(3IR}v^yd$G!j1dI&bG|XKvhXd{)`UaJM>69JdbFCQd0(v8kG6v z>5p+S%Sl3#z#N@?&fC4Awu(pc2kQNsSFi5lnQ{S8zLeS;;GWzduYml zlf0*pRc}>0+YbB3gRV(-#nM%U}EYF=8MIJX&;KO-X2r(f#rX*LdW|iAF7~vCR?((+VhR zt>lK<{{7(UNYG1x*#+m@;zybR6ce6`M$P*D!NJEMRU|BAt*>;R-0sS^Pm?E4z8f`hAlxT=9*FUzEosSaVA)b!sZ>UP z6Kf^`ID8{xmq#sk&+EZX7={l`FIVMhMn<3<9)eJg!5u_vbeHs}fG4U7a!)ke8GqSp z(d<(OrtJW!odBsj*wyBm;+Bhke3{-%G3dTjtyc^xBb9XKBJY^6Tt+uYE{(_O;PMDX zK4%(#NXth6^+?kQ@$1#t*!^j)b?erB!&0N?u$%Odr1_P`b1!x)P$k>9?@ z(oV!r-cc49P9QdnY<`^koflmA51JNzaD0^Y-yun2boS*GA}SPFv|Tpq6o#l~VNpad zRn#XgUc3l^rkmUO$>MMRJop^wRx(DQ!;LC8M^HOu#}09JcJ{p4oV8Srj*YFg8|n`3T?+Ag0;BCyyq@u}~`-6E2oR@KLaq?0P} znc{DVfFFWHih##<_7g^uo}p2pw?;@`6F7y`IRNE5tTX>WzfPb_YpZ5x4@j37hQwB) zc4-%s@|J9NEYQw^et(I3$56v>POyLYYR7e)6HE=G>Y5%;hSBsa7?5=Q!#=QdYfN%` z2OM3{GE|QagFON{9F(fti!$PTX)? zqJVXN3sDLs;0lyZXLVz*#6{hO?G9)KU{~aXE&Cl$s!{A-L#T<~ypS3-Ur-NhB^7Xpt$wEl<0_m|&up`p23zLopJgtfq zR~MVafjP(wEeTBVj6bLh3Rtxm?g0?;T{f!w;IhH@=)c%vWZ@&R0p`bMvz!FnYY2No z>IO)%Xc8R*I~6~DA4xmA*danEBpqA$|D211ozTm)lHi_lf;D0T*Cq`h^#gd>3_M8^ zihf0N80c5;+!;c!gUnMwS)(vSR#P|vLs>fjV#$U9qV?CUGOMwd!koZxu#6_EeMzUO z$#}{`9Se?!H8F(LUpV+O6b`gEwrJs7%1Q!%!LSn*0BM3&`yOoIV40el%J7q^;7V?H zf~%>k6kK=rGb<4R1hGCM z{boG49e4c#n0-vpu_bY}MW;@koQZU#Vb~(zY~N%Gn#ZYQv5iH~>FBVrKTUsx}L00KV9PO(qRe(L@c8)NOR>-s4^lt8S}#oq`_BxnDtQl*V_FyZNj zW_eF7{*w0|`vD&*TxglDI6AKKl}BK&l9{!m_G^tkJtNlHu|@-f=S#V?u z+>vjn6RdFDd@!_1RG}j)tL56XW9FC)b0=5Eg*kzn+{}6RsyGr*A9;TT*Yj$0%PvM#Qd?quwp zn)?Urs!&gRvK@><=2Ih<%@mz?l1(9#$0XlmYwA)lCyu)YwvGz{x0g*yek=(4mzVu5 ziYL{Y#>A;Y!Y3IdFX!7IO<3&c@g1wSd7MTscRNRrlVdezAp|m1cE;BBsEEKp5T?TV z^a-q%a4*yfpH8-EOad(cIZNMm(+|_Br-X|sD%vayE@A|dC&i!Z4U>*_@@Zic3Nm<{ zP^#AVM#VFQQ_jQlPOVg{`x{X?qzoiXN~ew|!6OSYKaLqQhT^DP5Aw}I)~%KUI6@JmJ8C)ejzjT$Z%7?l+`rm5FDSktvld&`c4a2m z>GTAUy|x5qVl0BDN0YPO5M?-CkCoVx!777BQ zUs62#(VW9&)pxzc*f`!!jNMNLz{Bjmv0p622P{MFYfAAnmRV~gzUz)gy#V}0af*ymDJ`VLO3v>Dqu1g>pvZA%e zFXn;*tX+Ho1NqWH#eFbw=`s4B6MZ|p>KGoa?AeH3Nt)*|?kORjICZ^M z(nvmIV)-4{o?S(_NXjK0=D&xvYpl?DMjw|UdfIq{+7+zYVgX1nhx4}mW;MpE(eZS4 zN%q;(JPVmgk6n9Cs~qK_^!xX(1GIybgKnn$EDvI)dQ)#9uH2CD!DJ$TZ%CO=Dz_Pm z0vC{3(NMx8Pf7ivtD`fBMt}9vlJ5yuDXcpmI~p+mWdjOHH6wjP$mW0Qp*IKGQ>(WJhZb{g9N9Gb}02@j379FabMe zwWZT=7))wlzB)+C9;>hmysB+(`^AeEb+U@-c@S6bl(1tD)RR_hIHIKHb!11^SRs#m zwHothL7#iGjns^$LZ}duA$?9dJ1Ygjl)oTk23o^5>061cj6^Qfz$*ont6#N(L`0ht zE(~s__%gVb_X#F%>3!~bO0jkzG<*nWoJx!OI_(LEkbiLTG^|pjCm|X!oL;vF(2|5u zK+9l8pi15i04|AINQJZaSp7P0zkT(vmEy#hh3}1myobka&ByT+cXUpgDQd)% zeG!q}1_c$Yj9#AggJdBJnu;fCd|jy3K!5B`o96{PAq^3H%Iq|^m-aHVis0-JW@JTn z8xE`JHL%s|4eoeUnDXCW?v&P>u|Buck!d*~K|nL_TMC=q#K_8F;v~=gheu&LCcN#r zOR+>GO<*%oK(946@Jf@vzX9L!{wOfkUs`7i&c%i8D1EtO74w{-7Tk|^kA@LPy3$Yc zw`ce3pB}X&q6#u9qChnmz`k;N4;aO12bJ{+92wcu$uuuuNYPFP6J#84Z@7js5vuW- z=^Ib`mMDGHMSdkRuJ=nsQ7s6P2M-y@Zsm zlXI4Zs4*vr&L9b-job1o#X?#^6ykbtk%a_3)Yk;lMwrmI`;wkmf1lmE z2h`MlNhOZ`QM*@VRPDSOrY`i%OEgXq9`_$k{lg9)JW^4EUNLjf3?J$>ir^!M1gHISzM4=GY9F^2HiVT>vX&Cq|Rv`1~IHXi%eiE zN&=$!NzMU6c#yzH7?e;vl+7a~$HLx3FBDO$guP~awMN5)OpBj2bLO*Yir7%zREQ%o zqFdzPO64)RaO3S5+y#9dc z)@*1pN!J`Z%*G4vzJXpcH-?PBWn-l%M_1PnBB*N@FPZ`8wA>i2L$fb%la&0%Qa#0A zrw|M0Je+w;fT42#fnz3kK|M}tc*Q5yyqA%N%>iYR=bABh(*tpVPe;HVkBkg`z*={2r*YN#x3lf~lX_pvCaZs9E!lpA~FFnm? z_lU+vT>`dp|o3V6^^=3y&{f4oV0JXB$T9nPV;i=|mk`;(uaV zOn`j}7eXf+ZJzH^cR-2GQ(}8kM$xG4azTO!B{9;YA_|oUXj8L*TSAE6ZkX$rPoHk% z>?HfqY1wjhVo@Xf$}sYN(8|isRInQ|wC*+~7-hPlodc&^LQ=AemTV4$fC$)R5RoXO zusH&-IpW8*7+QFFdN!RFSXIi5yI~m{e4jAbrUJG|l5X@1rOa3P`9LZZGUi-Vz^U&V z^6VC5$JT?x*46L+pBtllgJ&fSm1R2GfQ165SGtsN>h$U9H@}8!@7C!i%>ua9%6oUD zl!zE>uQ50Rn7_g!p8*kEZTFh)n+UtXeu&kiq>08?Yp{I^es`%!%6KojMUMA+naV&! z_NkTZbxLGBG?&c5RW%wll4P8RAyq;dqDo`CI1D)cy#!E2Oz8VLT)D6=#LIDDr4f<8 zOV=+5#+eXOPNDgK6i&ZNg1!Nr!$5|AduF&f3|@cW`30hdgez0GUZ zo<3tnGUX5@#g3@5ob@tV&R8W~St*Rf%Q=b+cj;~>Iq+K0q&=-T>`7JQL2fm+x1BoR6vcav40*#5!gTAPa4hkWcw}bRiFOZ0 zifnb<5QSo+*BrebM+TPLLBq5Xx1>PjCTpU=0y zbre`Kh2p;8S}J0bTq|X?}gJVN{S*v7Pl%@#wB=|WNzxL$e1ev08+Ta!}!`v zCFuDoeN6%svqw!7#kur1wgS!MEmvgM6d)NR5A5$p(2Sl9xn&Z>fB|JwHZS=+R#}KU zh}NETkcnH+Q7qYmF%fqv4J^kR!VfHGR(8S~ffutkxF3{Lrn2rcLRyaYPj~*}0=xk} z7F8H(g_WPrW%KqFfFF>6TE7MC2Fcrm0u7E(f@S>YN$=l?FCbGHhQ3Pik7(g}fszUT zjnxW~Z^0%+&_$Y+=}EC2Ia11cY}jI36KFN~W_XpFJ>iry6qrT~_J z_+(GY>u00mg%KziB2%I*;E65gls?bM&|H2|vrnHsq=4jEGpLZPsH%WoPfB%kgRhTI zuiBGhsy24b|M7#(^=+>E5gY#cR(Q%Ex92`!71T>?OIq|i6S{nAll6~)*4Q85cM}J zplcB)Ik|G2@wANYVhaeJw5!_eV|moWEXI+u-VHP>+oQtuHFTRYF>fY@qQcmiW=xB4 z<|BmdLxN$Y8BNN&mjewk>qx@+Ipt$yxVi;o+jFA1NU|q9Kg;$qW`ko)mxiTslFy$H zE+#AO{{6!oiE@L@NyAt@q#%50px57vkE)x$L((_K$mX1SaPi_&P5#`&dPSRxWfe=t z60|4}F6A>C#ZM)S8Q%?Qf6S{yy1rej@b|+Y{Z^~xt%>r9ltZcORld9e6Mt+;;R0g1 z+N&DC_fgIvBij;Dq;j7(VwQpG}csXdssRjCq?&K|YNX3ag zA^<%IQq4A;&}}IrjUA_pfgQ}apFblDz_qLFY!Hj-9cEU=VuYm7!9+zw0Ws73)cii9Lk${BcQw+of?^7G{i z8Of|wwYF&BF)ri?$I9o>DzleISZ}GzQEN2fY?b-mh`>XpbnU0R$rW=b;hu6Y?T=%J zdK-x5&W*xu*EK|Xh$9ae!ryP%zJ0`@(XQYQGR+nV>_K=T$1j6hOT|>=;UM$TXb@&_ zvIxWb1|uLUe}oPuH5Ca$3lWm-V&qaess>97*0XJ0@c%!cuYCm^msSMk`nD|fG_pt@ z1~4QC@i{uV)5*82PAM18r>2sYZoqxXj3^oOB1koZYzRNErlxM2wq!LNGLbWakb-Xv zO9=}H!o82c3-@{euz>EDh&S3)VfF!Bi#R)`NULV;$45{?6&4E&vt`{AUR?9sx|OV4 zBzPr&gyGY9Pr*p~dmb~*YB7`t8Sx_`nNOdZ>Fdmv&H=Gf<_*sB)XI4oNC%NlF~7>sDnsXfMZa1n?!~QEnDVET~sbmDgW`APEixo`|Fj)~>z<{-(fZgNA7MVK0VdU2O3#b*n8yQ8UoBA-U*m~A1 z!-;!WNLHgX888_cDdN@LYTnu15T&}&rB6dltu?2+SR9SCCZ7~Gi$oOkKo7FAvhuyr z6i|XUN~T0g9@?2Q+d=3|m<)|tK6V5&Q;xdpm~Cd`29YDx(!bt?-erDCoUy1C8xg;+ z8{LnRzSKd0Wre&!x-fD1;NYPICG?o5z`pqkIe*)Aqc}29!b9N_U{dLdNS~)rJ4BYG zd{_b9!o*#15_M)axWcazw@ccD>`4$%JVE#B3inD`W95%deZ(5*<+i8n7i(0q#5!hO z+Ei3jxd%rYF^}%l<+h{QMtIJ)L8Gf<+s0GNQQCqXHAISx+`Z_@WTxtK`UOhkWbIPz z(8ZoWKq^)^#{<#>Liy|XL8B|oRlsAZ8BJtp2I0F47b}!1A;{56w9c+4G!9U;elOnd zWjDgMwx-)j6l$bR02B(ir`2p%_zcM&DrHk3F>`$bA@IFiuR7s6W>%o&ATNJ{hM1UJ zojP|u%wdrHe>YIB&?KmwiqJs%t;KNnWC#sVfs0qAzEm-ih;jwZJOAb8-_i&~XgoH_ zRpJIwQx)Y2Nabz@9@dBn^ylstWF3t+ZOow zN*+j&^!UQ_Rm(p`Q^$%b^3(2>;UOrZ2GI_Kc1d)?k!Xo}iK@t|cJ6!-{ULC&n-qYg zs~NP3lF&8|B*u{+p3Zl_pEhm8m|BuGNRUDUfKWdHL4yG!qx`2@TCSDBHIi{)n{{)u z;ShSwP@gkxn$DJ2H)u2>Z)LOjOZo$mLXZz4e*Lqj;lT`0v`~_4({x;=(Taty*AnoPF{i0)Z1$)ztr_&R#)Ck>y?*IEmwY`bLm^hZmU2M#HG zx!43DWeGVMFChwMM{l+94wDA9)85--<0gn`oaMG{v*uK*uzauqqqQn@Os#meTgfOE zF+bvQ<^70urNcJ%NE_lOY4nl4PPnrV-NVc341noB;YZ<>&6nOWaHiCkV?MXwJ!5ea zr~H@cyE=$MWiMQImtCS%O4(YbUJ!gIjvF8lFZM`)rTGgNMp>wRpn&}gJQ4f=!=oM6 zgnk{`ZkP}$DcV-Tbds`ug|VB2p2q%=92@Aa{ro6RLK0~MCa`NK``%!$fw zGaRI3$ikE+?%4T$6!7dp+?2d8$8iMOhH2fqCljH`qv24h<)>lv39&VrUe<*P(_nx5 zVc;~3lP@0RIO^QI!PIP|uMhaoF**1O`w25HgL}A2Js|FBHXhL#RFIXE89@Rmw8Uq< ztOsmFpsnNxxZc}F*U8QbfeB|J8Ohf2@3zLrxsIQkIwHvSz$2yvGR?+V#`P+KF)Oy7PE5q zaNV&gPC7(2gjI!)0z`g^gJ4k-F#YSf#uP^|fREYh_1^9HS%$)nSFW$H+%xsUck}Rb zpRa!`@}deGN8!uHqe16Ic@emV&u2}MSl)||Q{A>ngdC}6=UooisXw;NJ>Rkkx4~9? ze0{&4HDx@s9vR8`E3YP9p2Gm;PfRP6LJ^e|7rc;i&JOKhR2z4?RTr8orW?j}+42=C zTqkMpZq*wDBD+WEv(mRAa|{s7F_S?$;$jK%MM#1zRd~aQ0jDrdCJRXf(<>v_rSyuI zW|Uh03d{E-yqE$&iJcXqu1K+tng61$yc=}YI|6jUWTINdT4Ra+Y2R#y01)-rN4KX95x~H5DSW6Tz}&Ov{n&FOM5jL z`IeITq?U^v2WN!K)*0MT>jwZH#D#KI3(7x!{FosWLV1Q|bUgW?9Of%bAUQbXxAUr&t5cTvQ#3TFKqRfGT${^2fERd!T!(w!*y@g; zrSw*BZan|F>aRoRU2W=MMa(U+-qOK^+Ag+2lpDdJ2aDd0u&z)IfE}2*M}1sz15Sf* zjH%WRJw9m|H2_-*0GmTX+Wfb48B^@|v1obd;dk9U+_#PX5NZ0MXxZ$0-B;GVSFZf# zi<7J`d#tgH8rwQ#&!FYgk8PedtxkMF-Gf8Uw3xJP!dvHOKE??*ZN>VE=czw5Uvbg_c`Z+EKJN(>lS^-@9i|Q)v8QdHGL@8?)^#&mkVM zdb9!wnsc3=d3D%(qGU8CJps!Xw*9S&9ND4YiRlp zsNti1lgIMvXVDcQ!^)r z+h^VCqIM{;B(1ykspl7Lf6uYyI+|^{){BuiSxiR2GB&{`uEA(B9DxnK{&iNCmz8#H z+PIR^((2=2ytpyBFrTekm6v(vjZW*Qq{fn5OgKAVnE` z^K(5i_@PSW`Nb0-krfxTdyt=>A9VhF*ZU711kp*=xpQa62%7SxS+l)SQ5~>-6~@%S z=JsyAb?wmBb*U_msd7>C@eM2g&PKF;G{mB;ftc8p3XA_d3Qf=a=RWqt^T$DrYZ%Sc z&l(HuzkdBX{iW~3I9Ov15mKo?FJNDAa9oorWi&b zh-n#xaiDynz(fw*_rM)Q9G#l}%bfDfRX>;n#*~7OUwI`hw32L1Tj6p^#Y5Nfz@7Fm zqjz+fKVz4B(RWqVq(_e$Apn(>l2T{%=+R~EX=f=rLFHq;G^(?G0QVVF&6Kg}K0ZD= zs!vT@w`%3Nd2=nZs*&hO$WwtFI`;YJ#9O9S6kO;E6_fFJv!vU%H*QT2$ba~%Tvgj` z+qZZ8)G)=hX??qI`o%|V&V`4!Gw`)KcKmoXpEYx5&#qP{AouInSc}~>MN?7A%#11_;B+H6ZVU=;=f-Wp$phl zj#yvMTcc07*__4z`ILZ!z3ypszs-c4h1F z{Pk58OGGyn9%lFM-)~4hI&-*_{hag(?GX%bT8d^?_v)kvo&>d5s3k=jxWl`lD6HlD z`#YTk`JRU|Wvzgn0}ec6szjYquV*8>)dpkxaL$9twh!U~sb_SO$hN2s3xVRs$rv+dzaL zJTDGtc=y(H%F?B-QJ(}rOmzUV>4`@7uHdnPPM`jM@TMI3B?Zr##KYfBGHNue0hYTtP<$O= zm|Jg`93f0O7#==1iO15t`e1gSLQ&O^+FbJwr=G13O(;q|d7-XyJM%yOcxfImG&H;d z@j7!w?E!0=6n$EqO`$6m)6)nkOpR@?9wa5HQW^8^aI;f$9OkCbk5eg=ciT{tH=1F! zR6fh&Vk7}YXadAer$t$gO-fxUlaT%UTTD6gCU|Ll!1siSnXdNvcDrcfo1SdZ5^~V@ zjoqRF@#En8Hvy#af+s$1Q11L2dMB??y&Wxc;Wj$@z_Cy$mS?i&>q3s4Y279YO!IT! z{nkB7p$j=&oriJEgbdwa-nn_%nLD~JoMW}SiwO>*)A()^!!gd(dsgP%4PR21YGQHV z`;$1iwr5^9t5=Ipd9K+4K@~+zpbg3)!(Rs9dDDq%c=^(H)W#D;Ha5V~y?UkGyBFUv z`g>w0MDL&N+cID++ou~oWyVXJg$ovp18zP_F*fhoz$LXLxD0G^xzXH*s(0_+ZL7hD zT{SD&9-*$-WI{Nn3Ar1V6`; zwa+-J4%ld8o%lNs9;hQ2;Rzto2EeF3`g9KGm~zucH{c_k?(J!Wt|w9r&vlgd+&vYl zS~t4B{jj7+9F%&~5vj?^wM+}&3@pBv3qRtD?L_~&Y~?k?1AWNou~~h@E#G;=D3oCkqPOm z3QR)BobP51CMlC?Vfy15J*+y!?cPh=6DK0-G=5_?*73({eKa8mvSaJ`3NDAt@ z5`yQusgz=sWAsl>dwS^+`+A9U(eG8_93sboX-&r%v-u{Ue58i-fNE)LT-6cY z*>sEtedsI2emPVsu{`5r!DW1pr`s1~)+=r2C@!bb^`LH=pMI;3oadODLp9}CYr6Pa zX`?`~X={4D(X$5cnw4x(+OhT+JBbqCbi}&N&6Zk%W!16GWz`j0hX_*%>9?})^RBcU2UoQ8OL)1tUt3RV z?(y_1)-Ab-5hXWa0WWF2?7lWp-PZFVM7wcV|MpPXl)FuTpK`bLO+c2~?|3=~8THryed7QA;ZO&rr>C+!hkB>e5YPb5m(Q`+igN%0+>RuXJ0T11a3y*>%1ZgXM&P}lS9i3-SA}T%A ziL1-d*{fWGPHu#<)2VcEZcyVHM|xk{hdJ~`*MB|quz2`Zj>vLNztg{CEZkJtxY?jf z@g!08@E}uHC4NuE4APaX5%44Bz_{dn7K3eV-!$VhJ%W6cW+Ee^@cS+I^#Ij;sMt$z zh0;H?-+t@CBSzGlJ$rV096{dWZx`y3RM=Td%q9!Fub{q)Q$ zODBtKXP+{IWop)=vmI%-Xe6+_+k+E&s55=1z4vlp<7dbi$ojP>eeaZgrM4sC4trKc zrKm~Rk$3Xv$4`(mbuYTes7(~34OJ=ElpHn<1lo=r3<;@SuBsy)X=**x{AXNmB_80f zGOOB0f048F>yF(XS@87>Kp0lJ^)jEx~m1K*80}dND44QT_Y(Pf1CsR3@Y6;Uh=ZljE5Ex>DuWPfPb(A0i-F zd-yQKPF^M!%INpg9vR;$<2lAp$nAI`+dJ=dA5&$#{;xk*e$y?2eD4%;wB=Py4xT?h zx>C<8Z0hFau9}eu_k3xyRT*YAxnFXqpM1YLSG;#=8;k8<_8opMF3#Lcp;6&hP|VY0 zM%XJ9i*)GlStC((o2Ua;zO|Y#Hmp>eBEQ_z>*tDmzucr~0RiM1^KgA|Hd?R)%wX8!!O?zKt| zNy=|Hs1opON{0ER5p+&qihq=jJ2+XDV7L>A*s6B9dFxg^o&UYv&wuD^l|%+5-+I*8Ls>*< zqgNPf-&k@*=632(V4ayZQ{p7}bsHeMU1@b-S1mzPhPG}o?a#M#=zEoz>tns&()xm8 zVC{1`$^DMP>N*8cA_%bUU4CeI_y(vz@g}c!)utl1p0?B~5`zBg^<4LN_xhrZPyT|G)g?-aT@GkFo6qA*| z-F3-_t?%@&>mMl5l;Bd?k3KHx2#D2r{QCSV_IW)X&bXmBov|Kv%Bbt-g(|;ftSL!E zfBsB0>G9*nb05tfF>3yctvz!!8ZXQ`nwPx&LLJCYYRcxk-~YH%GxIh7eD4u7nv9{% zkyGRP*3&ZYl|T&m;oO&-uiw8ve(;~)D*0E8>R=^i@bl;EoxnfZf@hv$Hp=SQ@4DbV zRL;i1rW`yd(XLU1b#i&lPW+DMh@aN82}4SC_LPHj_y>M&vxoyo7e0^u${PKNX&g+Z0 zT?4+oo84*ow42|g4AFfZGg%$wsf}?j{5lU_-?NJqn|P5&YM;)9!}M%p&+4A2b9McZ z`(B;OUzv*5H(@o5lY+>QA+cf#pO-pjkfD{PC3)+#j8c=tz<&89U-v9Z?D@~=v&kjtWv;mPoVwt*S0lVvbFZW zF6{?&db%Qwirro+L2DJ8Q@J3{x3;vc?9dbE)`(@Z+eJo!~0zOhgO~+TeD3jboKREzHXByO|r|;6$Eh? znfXz`N?2ScW_axTL~nZ>u%$ZX3rAg>h``>rW>5o-rNr!rJx-!w_eDhie}BFt(d`%H zygugp4Yb6IYwwf&+x_#;?mbXGYPJLbDvlz`v*B}3P;rW_J7Sb7dC*iC>8+|bZw2_P zD^^k@hwl2nr$Vy81SK+i$SzvEn525 zF)=Z*EN7P@wH+~#vOTR1&t#U&`|0qQ`yhW&>-#dR-(2Fx0dRo==5&Y#p5{UhF8b{r z`DfrYH^yH=7;Y@9n5@^=YmIOPGG$9b5o%T2SruEJO($R+YY{N3O0DM2ZkiSsbYc{- zfY#TaS21Y#=>0#7>R}j(t^w$V<=N8=L9GK{@9RJRx8%d;q`?->MAFQwNkK+M3)35W zRyS@a)|$WUeYAU&3x=g)<9V7io@~vDs@l*{hp8SH(gy|YDp@1-0> zsA=ji{;-pOyFvIB$jZlNV|~?Zx@qEN1*J!B#!{!5d=0*`?8=%o0!e^jI4R zd4XjjV_28vU97RBCd{(o`l@69b@YQ_BfE5*Ny|kYk?i{Il@;OV=U2bmqIs4#jj6cj z0bMlUNvy3<%L!{ENaz0NZuI=kD#)5>x(>FtPQ!ju)BXAC+m4;WJI|kgI`f~O%K!9e z6bVg7m<5)%I?{H>r5j=80h51S z?BBSN0U`6NRH^dkF?2gg+;Pk%tF9{N*lV8hpc)>M+BtpOSSxvWfSm_(3@846r7i~| zBMrh{>Y+r?`PY(^e2sV{VCfFy$#(Y>@+p(HYUFGU=r|o#%)Z2PZ*q*5yWdn|O+WuE zc>pX1yTk|d#O?oMgFSc;X}NF6e9+GiY3JrgWiHkJ0^fQ6-E$P_To)B&$FCjexJ?Pf zmAu);ImweP#xaN}`Hx5Pe)#Co{=>ht#qo%K_q@Ps`yNE5uUP}^GcemF;osMDM$ZB< zo}vG_FF>F>Q(kT`Ewah(zQrOwHUw_Te=Y!S@tsMJ(PjG2&FD4SzEDV`Yb*LOpTzat z`pv^5|2b-I>)30CnAaAS`i|vHdnYGbn;8ay+uNXAt7#X5wM_nS`UH(du_rxXcU(@k;INtvvs?Ftd;T9#~Rpf zPuf|8F@hYQIcjOi8Lszd-y7|S0OK-ebGY!63K(x)vsaaQgcRJNRP993Ax~`@_tXde zp0YOg&tB+mEmOYE`s#fv<7Z9Gaz8O^OuaXW|15Rj-FzJJGZ^4&5iR>0i>1bb+W+_h zCD;+7$l9q(zU(~uV9HrxtFNdO^GZ~P6GJJOc3bqvM#93A04F+I-KHKNm zxPNx*?bb$A;E0lNm72m8BCaa1i=M+VKZ-%zVdOfzY6Al;gpTXeMzpNFX0v9{T#}tq zk+fra*sA|*rfutQoQENJfNdn#^eADQR&d9E&YQ<@CFV{?T`VIDo>3P_fBH_IJTU#h zUX&N40F%UiZ(4Ta@P4)KWcUC4&$7wCy5F&kzoj>Fb0x$5v(DUGd2evD-+Z~9r)(64 z6*MW%k1kecZ6f;prbwKw!A~kB4=$fv3VXf3Kl`p;E3m3XIUXu4zs9}!q zTN6L;z<#fnZ6z~I%PaQ%%YrHY=tTcBdf$Xzo2(KZ^tS^gu48V-i7rikzNf-jm9e2~ zE7#V(f^$#G+E9okjira$5kIw7(f41bLismtJ7?uToIz!6YjwjzBI11a9ENiE0>2w- z|1%)BeOSff{iYY-1Iko>j=Ct9G<1Ou*_3mnyGF~$-@g&Zw{EX8 z!i1zsIAqC9j?|JD_a>gW1$#zs^lyO*7on*qTj=gN%-EPdbFD}lw(frU+vE02q3U1M z&K=3F{?j4#p3GZzB|4uPKuzj8>y}sV+;{rEb5aCt51rpkYY*zWrq!V?m}%)epzax5 zupfUUws3o!}3m&cS9kbmyhW9U&RILew% zQV;mj>Oczv*4d)v?Pt6$!EUG68)f~gnVF3A3=h(CW@EZ^c=vJ1x3f0q7aUI>*&S`f zOeiapvS*Gz{J+iIcOPRt^a)yb5us*(RMWMoxqZ|O3kVu*s%m=gyS(`d4ai;SINd5a z&~LA-2#oGAUq(i5s6MZeQ5(sqFsSTR#wczrrZ=CRAG1mhD^n z&8Iy+Qx`o`U;FR#^TLtJ}}TZLqcaOLT29Hq*+tDP#7mze)XiAuLvbS$S zP%|ZT)M@KIidIL6VD`U4z!_j?UBSA%YwOn5|9Mw<-FLu+@?^YOiDuM8)53xbf`kZNm3uEs|vK9BJ6v z3VqFJ&}#Mf0|7jmy!|6g^0R_QAe!c_OeeN>uinVnmS&6T_@M&@yC4T#kNYP^afp}f zZTEt=hkOFQ2>B1Bu#JXhEayQ$9Lt-jV4vmxh(`KrGkhYK4ALjy&gNd4n;?ENXW=NF z-$}ojv8)|(*D?>oCjiw0xEO-yCOh&NP zX8d{3QNEA&31INHeNy*J(pymAwN0y*>L$JaSxtw9hQ`KmOl;Y>Eh=E)xjtT3esKXl zoK8A0FrX7l(^XYr-5uV6<;`KEMumwg=kbcZGYJ@A%0reB6G;d&E&%O9i?~7XZOdQPYQxJOKg)NI2hvw z{`FKB<5)JPehD9S+CVl--utQUJ3mkDT(0lzLq9rC`RO?dT#;$_LhNyP&8!Has`gMQ zPNXxDX&eL*z1+NbjhR+ixbC%zz+q6X!=eK$tofOja7Gl#qYgir7=+EWtWc}<-H#QM zXrydWTlH6p72z-Y3ZIw_L={IuB4DR*C1EZJ@7^3|kGc`MWSn5mK=YsW*^?yS`N@Uz zHB}UwmB<7!zh%A#qkS7NO7fN?ve5yB4eEP5-?DaUp_xN`x5y$>lge^l2u>BQhy}v)~TCWwqpoK*%3D#nPP3zIn=Pj5#A~Kmc&kdkG zZmW+xpsg&8=EA+)DtsYP_Vtpy2mUYcs5vrd@EMUVT~*6(5WWd)q+qDosgkh5Nt#!%e%MGv@9GR)1w_p|2_INUo zdhF@HN{)=f*`M4R+>$|yS2G71X(pitJPt`Ep&|L~LW*tFZ}(R4|8aHR0X^^U`!7Vp zs8B{UyhTLAib4xTRzgTx%Is8Dkrq*+X%rz9$0mCgDHTx&*{O^pBt=E)_ju-<^EuyN ze;gco_j*0YJ+AAz?^~S=LV%%xjn#Y0It12Ph;j(csA-6QK;|0n?|qOF*klAsnnEYP zdl>ikI2OUdzzigxd%z)2p(2X+!PK759(}Nb)Zi1nCv2B}t)A%jHsKy^{*2xyty?c0 zy`HYRid2w=Q!uta--cNacDjGRlWG4`NFxt|EZ-X>bg1+JcVVc`zpn%kn;orbQA%7f zKCd2i(^eoGs$OkZTDvSzp&V&p$7#J$Qv zLpb}P9|EFI)PY10c_AkgUnl3%QDK_T@m^6T_cKzY?`Tc0R1l8=W-|SM2%+h0K&53S z0%FYTy+bIRO{J{nU=p?n;VT}b3;htng!K>tVrCwR`}isSGlxC>`-$P7&OC78%x>N@ za?M!ez~WL(b#y!~iG5qJ!~dd?c++Om1>s-E)~2ymXGcfS{I~zVhoiGSYR`rrk25@{ zd+aoD9johj#fU&b^=}mA+ASE!QPmm#m=QBuZ+rX`HCuTLkst*(*^EBf zbFORA!-v^d{Hm3Ibh-Tb(qfvA-RRgpsdGTPgU60-;cg4iK^zuPgIE3TuH>nOn63I^ zWq0sTwG~BE?_n}%+&A-9dT@C8d8#cnm#m|@ex#6xzbE15beXbSj+i{AnbM1r->(+*Y5U>59-k?k^bw9M37q<(KM@t)=FEbrJSV6s%m-+1)#QvO z^+1z8ecqjJD!6+$^5YV{=H%mAJ@3EwJ)lQvKKfARL+^+6e-e3;C;$BT%wvzAd)w|qbx}L-+}o%>O0=IbpG^TsVDjU-gP5vzd3gM zT-|c;4(gtA3ksq*`dfF3;TE^v3>xYseJoSRR&lo>b!}*RA4SYHmaCb<`J~E$DlvUd zCz5grc2WF^I=5uPlixJts6X+kbWv8O(5_Q!DV<|v1OiYMr9Vup-xCI7DcS$_GUlmF z0Q_NL0u;v=t56y%0>L*Al0mQ5pWGri2T^*oxrg!4Q>U-aRY)4rzi;0#(J6?^p8Ew~ zxBTI6vG4D_lsa||!sT&1)hS0z=75!5VODh7W5+c*qUHfm`@}I=|9dCqWe|m19}*o) zwkZr2JzcsOx>=c!vFQw%iOAijzNuiDlGK?=B3_(3_v*yawHSy#D`kB3hD!ADPXJh(x&5Q)4WR8;etMCM`!WqPxIG;Hs(-1rUp{weWTs5EWwhZ z<^8@sX^g09q^$w+dRf=*qTo`HhSbC)>r2f}%g@iRySpmABip>{egQLQSB3s-6kE_~ z%k$y8U5w8B$C@nJffo@Wfvu;t3;qBC;{h+bbxy3kt!=XXs5qS#LFL16h8GYJlpRco z+(bhrQM|EPZMfm*jHX7hKQ34ybMaGx2B#5#Tp-KVwQ8lYe+gA)+jVCt3xUA2NyB?H zIY|xNXyOr>@TP<8FZ+1qJ;h56?*bb=APS4(*1PWjMu-I|94<)C z0qoRlMy#d^8jnVdV<^T)uFBbcOrjoC>^`hW5yA1Pm&}A;9#^56qV$PDC_dwJYVM7&LnfBk_?~k5( z{rgYT{{7rirV4W|csWDho%t|GNVvr$k>4rFwUuH7%B?$KA%%$)s9lYWy^r0qYAH-! zU{h^i6kQmyp#-|4NFS#=`%GW>_R!`WOvepdAe;pcTm*(w7C*>il7mlP?|W1x8&T*b zsue=&<(sP1o0L#2j1#p^qM5$p&TJG6n(I#-x^!s-b?g~Q6mL)E`OMoGA+w`Vil{rL0 zTE@?Iv?^Bm^>=YbwmW--{BBQ`s zNH3Ug`{quW$aw!rkQbNFM!j ze_E)4;0n4q`)k+kX;Sx#DMND=I(D?sLd)X9yF@XG;`+6f@y4q>6JecsEfBz**ety& zQskWTMAF(BpNa6Vcr%MRDD@O~UlftKg6d^TY5XO$0;1M;rh|8={?1$)P`l7QN3f2f zeC$_u4Y?J?&KsL+@yJ4yZa)43HmVO|eO}~`q0)u7$p0k2+9I-e`NuyNpfIzs=Fib% z(5T-}TqWuYVEc4_^V2~mdOzI0<`a69@iIH7lL?H!Lp&Y0|LgDsEh#5DavHehMt3oA zu{438^>h@~Oj6dV*lfxd=VfznL#0h^F99>@TG>R44HsmtJx$s5t>T3qitd~m=WYn5 z9^4{CiamIJr);B|kCmJI>Ii-DX?&SZptIg0+uf_EXZURVdv~rq-oEJeRieQ@`&-;O zjN9PU(X_#~K5>bX)R$F*-1EakVO#GV4$2hGEEG&{v(T#W)>X>X4czm)kZLv+rCP_8 zQU$Ta`16HcFdlssl}j#-GCo_Y7I%_08LSYzo`k@GRrkoFgM1Km_!-T$#Q` zM4h+{(s78Lu?XU|g`hz^9y>P4PpWXU0B_fPzUa}=Sep?=YYqAJ+B;%mltg@qOA-92 z%-10{yM_)BbkS(a#1C@9ILGT&duuhqYL*iq;<)JUx?joa#t|Wrbxuo%BLg-TBY^x_MVq01#R`tH^>q7 zih8o*W35gC+GTOeUAYE229x2RZG+wEPo?zg$D{MM?$$1NVcU8&@~gT(84TQ2Jo(;+ z@;=_6P;|&in<73XX`3H~9Uik8wR#*wuV^*qAYTM znv0!i6tLe)>>5bUtbU);U}zmWd2P2efx)VwXP;1x2( zzvhMUcT@#E(~Vjos)V@$-Qil-_`frL70xd8m>pd=Uxc5872d*(ib`7FntS8QC~Mgy zYVU+F4{NvntF%)a`UUnAq=8wqzVMyRH-x}Wp=|Mm_sJ1rZLw!EBUXNlFM6jpV8C=O z@eF*KDhSy3$V@k-Q=v0alikC?VMAAqkdKbOskTJiY)~|xRq~9 zvN2^;4sh%_YeG^r?<^%^$^p#?_yT7^kB9>2*HOz*DUrS{6?J=Q4<;F(Exy9ZNP~gQ zsm*Bm6>m6yJW%6U3J6V;HOk+QWoaYThK-a<1KJyl+guG(oXs zN-c2CyW(~-IXScY&ELmPN@{kl&@2Tuz6;#haEXXih>{A@ucA-TUa~>dayExwzrD=K zJFscfTtw(Q4kXDu*nLNS?ZvurLMaC2(r;@4phVEp8NfD#^lqY>?)Rpt)KJbij@QXN zPA6h@y4`F7H5$hWYSms!XAxCKVi9DOMOQ4(tKS}7z`Td1ZSkZw4b{a?4XN4oh8cbh zB1UCAb?@I#BPDevC{#gpEXxQ}Ac~&t?dzVLzr6e}WuTM*b9a$s(VwPo$L1XgTDra8 z?W>|^jZM-R0ib57aph)e&vOESn>T@nOI-1YQps$2=ifVA%=kf@or86pAFCUAISEN; z`;j$;x%oAdt)-%GeW<8#Ejl<=Zf<&85RWQurU`nLzMGd0rX*LvMtZhAmq#eSto^>z zPEn^BjO2rpLQU$+;3cyEH4OI#5-=f3F zqJHm=QlJJDev|eF{hPjz%Orr&7EB>o^&FuCz{u%sJ>eygBvDFc_(zL2R%6?!d70+# z8m9Nx-B0bxeqRwneOFP_3wJWOe#K92%H>bzo~fn>G9_|+aWlTeWw2DLM%|hP; zS_1)?_(Ak}*d})-j7Q8CQn1O$nrsX1G3#pnR~ank$+-eUY13o*6wz(q_c3%A9oTz+ zdPc3k(zCOs8yC*+-nDDQf3N=a2cR3`u?^zQ1fT@$>PiDO zZuzD$vNuHzHV%nU9|Csj!HdaUT@hdt+=(EO&{n3c>8%kg)4zE#`CzBlZn_I=!#?DoGFGm9P86gZaV1v`4=na0BQ9S zy+@+;sN&E4pJyHLio9lWUJi$Rf}<;r51@s7m!PeN zWJpHXHdj=Vkphz8>&VE(gdb|l@(sM-@7s8jviUNh5=ExyJ~~Il?OUf!3I~q%Z4u*z zn-p!SSnP4aowcY}7X1LkW4yF#SD26MiTj+728=c`ssAW{g51AnD(G>5s92ul=FwC! zC!0QbqXOz~sNW=Qh>gw>HMRr(mMuH&5(1V(1%ZSE<8Ymwn%(;Aaz4>cEK9bvTVYc&HyL3#IX)) zMYl@#xOu0sx2RT1wiX;m!Uoi^r!WU(tyUtlz!{`u3$K|YGJZORE_fKB)RX|D)Mdo- z1*nonQD~offetl0f3+R<+{;3^5j0I@a$_>*dwAr)RQ2R$dLtIhp;t`k*U9;RJ9#+F z7du|Qmkl~Fow>EkJx1YO)*kzPAf%)gxDx~jp0juPQ$~6HxV1Mfw?O8_e}d8E-htIVu8J4S&a?2~atCE?SjM>&sohA-4w6RO{ zx$C!Q;g5WXvKQBZr%FJ*RSTjzD5DRzRhgg)8xXe zr%LogyNj^z_m?u`vo2|P(@QJCK}##vntt$gfF{v<_C!+KaXl|j4nys8HWF87ZfDnN zY^2(g)Y|}P{kd1ea}Qdh7tkC$xD#g6)o{;^$B$Lf7rSVMh3%XY)pMlV;j8pq_&E5V zANKdJ$=X5^Ag8Zfxj!~ux-lT+gc(e((l7A$jycG++| zdI*F5D=ee7IJCQ`Dt?1PuS&5jfd&3hd#T3p(VliJ|G5_yB zuCe8GGDCa&lkeTTc1`OTesC6j>#yIqu`X}Zu>60t06(Y38tcYdSxP*>dG+b3A<@>B z|M8{tv_nR~?jEN9e4r${yxc83M{SUMTU=N3uFBC#+{nAMRX}6Chp(TX86ESSA6*5P zElyu+%oyBax7(oSB}pmnsP}>qcJwkbiqG@i0FLjBVv*Jeu7+%B5Mv%~&d!u5r#%S! z|Gst>GT$d0AU`~Zv3tYYwrkhDp&pt`TFECY2Hs zyNWYwN=LZ)hp+lqZTRoA^814ecboXxMqd!kCsIb8AU`FLl|Q$K`GYP0zZWBy9Pj?@ z_@95~w3j5idw6=@{naSp+h5IZR>grJn8v6T7f?%pGlZ}+4 z7Q4FM5GS-qIoz@i%)kqaSyxwQN)NWz%V@Y40ZV`F$&-uL+gH_po!{Pa(F}%F7_I#= z7`7zSt>JdOfot^6owqC%mE`i71nmBlMx8oxC9MmU3*<1Co}L4FTbQ_D zP#LY>z4td()0A0XUq6V#NS-C>#EA|}#Ou<%`wdbYR-v%4a7wNN#*)?x>U@ZAL!Cj- zk*n8(@N&{L`uA7he>Ua*nUK(xjI0F)j27G7NR>8VUid%_<#6$t9RDs|x|pIGTDPs- zk>M2@-Xyp3J$mdO+qC=8Asujx@Sq?mv!`OH?q>SK_t4A9c?`UR!Q7Xe+yY#LGc<0+ ziXk{#&?!ujSv*z1OoY)SoYU=FSn|AK(2K)yhDGxVA zHekVDu_ny++RTzZs=a#n@S?SQ&o8(a5-!mSox*o8{Lv(!p$oB2ZeAW(XU{z*ej#8q zKbn}7dy{=^^6s(7-Muw!gy;-u0a%8Y4(3b+2TRw~)Hu1htp{GL`oUpuHEsIzP%8US zLJ=h$ZJg)lCz3L0DV~LHk##w*nCzb1IAXH3wHzWFx*x5h#0-=7q3JBAu2Q3;bvQ4E zEX7P$r57uWRRWf^m^xJnN~iJN&yJHP7yS5iflO83z#v#Wz_)LrXy<)J#p74C@7Ahk zPnkP+3mrpD*8b=OrOW~x$z`5))=6UHs&UGuc`fra6^>0^V|ovHaKl$svY} z^|hg`*qKESH|6!#(mEst3W)LzFD92+6zDOzYXd21C-t>8F-f;+zI`2^;k;l$ZtAqt z_X`WJLBBHuJqX@lqg;IyciY$3*9jMC(^@IU6m+8S#Jtf7ePglfYgTrl=j{K!@&r92 z>C>l-+Yz>Dlev?VQ%YmH=9qlQQIYfSLj`#9?AcHpw!_;~SR$6yYW(<2ZY2yu_}I05 z_HaF*26qG;Cy{{blqG|4TDr6yR*8JVM?da)J#||$;Xj#jp8oY!`|d++PHKgZi&P7a zit3<{l2_-g!KEWCFrP6)rKgE+5Ppho$ZyW>jq#ylD|g!2)3g1cL8pI|W|0FF78SXD z-DS3P=>X2z7m^-E^dY2b$uv@{Nt4D}S>-OyQ6KIjAy%9B>J=HT{?MTtpb%?2T1p<| z++u*RiGQ6UqiQU1A8bLl9)B8SpTYk_TPPYC8{fEjv$cwfO5x+j9v+HHQ`d>p5T!o) zW?4>DRe4@VuCXcSYrueZGfy~g&*SqlA{kz&F?jHqse$U9J3l=5sUhRlzJ^*ysml7>2cXqV{Jypl>%cfih9yPO zS+$JdB5d>Kt;If;T1s^3cXoAMhflngnc0G+_?9Evx%2jMnz5@+&3=CS>C>IN$64B% z&8So&f{NKat|0q}l$MrE70|vjMUO_y-fm91Z9QRv>#6_w7Mn1waA@%$ZcZE%3v7u2 z+JFXU(?O|xrHiID4URK0*RLxpUnS*jaLfVYnk9)z4dLG-%#MzZ4uaV0HFW59>+AF4 zP7^w`16=YTDrlwEzk|!JojXsR{=Y`n6FgXJ)To|J%D4tWEHm#V|E<`;Am4SMZ<)7m zE6^dya9;6WAbJ=4sMWv9(W%WSyxe^D>~5@# zr0N1b&&$ip_f5tsZ*Qd~yEz-)l`lt5TGF={v7m-Zgm+WpTJ)YRv8b73O)_&Zu+Nt% zn730OrG^&lxZc<$E-sm_!BT{>G26Fqpo4e`K_h{ZWy+@ILuPn+eHj$^2d<4?!bZb~ zVJYA`@3i2F<8vY>P`SX>}KC zBXI#=xR}g4XvbLYlV{GXha#y|RnAMm6j*Q$;5`i=dg9+!_EfT#0nxm0>5}}9xqB6j z-pO2*jXKpiWC!IhHYKMR-;@hfog!i8p- zZoJ4uN4Mh_E_A_gZS!>?LQJS{uOvliun}Qvc+MwM!nN}z*y3@RCcT-QRGNXEcebC$ zNr2Q#Gp#v@MhtLB;bK?uEZY*&!S%DjrM#FVwB+TLi%*_a*CvMp-cm%R@}su01Fo>A zm6f|_2p)X_=sM)mr62v9uI|~>ld_q0$Yo{kr!zAQAyC*hJ~jxVPougGA=)AF}(AE{b=udV&#Yo=pC?AER8d6-{itH{4BP%q#c zq?jCtX6Iye*uhy$pwbvRR0UPp#Mur9_O)o)QmIQ9`@rW7CF?qreE!^?zM43{(=GSD zM#?}4la^KX^vM&E9vpsj_W1GkOlU-)_Sc}wd!!=|)fYFdiTa#T`wm~6l$u&I`!*fa zLFbQUJ)U4;QFd1T=Is#Xsv_qpKU>CpQVSYKc~(n`MowJ#e9vA6!?y;iLBwyqXi-0A zl0|?ny@oO~e6p8me6n(K^*NQ~B4Lp9*XXFg{W`Hi-Et4z_REMbl%pz`Ft*~c8wHfU zKWfBKuMbg+*?8OfU(=S&{WSLL>ZFfeecW)VWMD)K3kc8YI(7t!tV{&{# z0_V@2!_FPe!mTNF>Qp2dqtyLbIw4l8R;`Mf?!J7P-WVX|J}d-A`B z_(!0*O`=vohxCq0F-y`IfT!#j!_*~9t(c^aUDU&da0jW+qPQ| zX@xUqQwRY2Rxv*EqVj;)__(-rA}@p~W*xU4R@3g?+ZmQlDu}u4Lh7Y&Xc+r+6f(A( zFiJvff9Up`K+-4WK9l#XrrnyU&=rRDQB5;q9w-!!sq_EES@5g5Ebu8tm zgmEetEa*%2*{9>uQOpdd4GTTj1`WCaYtPsQY2vTAnK-Q`G<*1pNRM{+OEL12Pg zCSA$7ic4!oUP6#yc1{x;&1GnPcAZ2R`=eqVxw$J9;v)I6voo687>M#? zGEKL`v&!Cam1wWj_`n@FcO_g5E> z9Y1~(8-{n%EM7ax8p5&*KdZHruwf7g(#!)Ck!ts49rrALerbgQubmBJ*J^j5xh&3( z%Ez_83JAUEaM=%_KbUXMeEvK!M^>wA`TO^|TH%(KC$2wwG~?}4-8%(#Q>)9mj#P?i zK}+Lyc6?>*8^0Dlp3Hvy@OLx$}=KQ5^}0SEpPS@G}I$_ ze(VU$Izmb%gm)!Or2JA3@upy6Vxmu<#P;pQ9SPM6CyaS#+2`L6<~KF7ajDUSvY4t~ z+S;uNzIMDXPas(=7;sX!WKrk)v6T#iuOkQrU2!C~!>#Kx1~^{nLY3T2USEGnvHpTZ zi^flU#$a@;$a?0d+_q_AewHhQ0R#1N@GP(Y`YSKlUNVSsTrY5>@SGkdzWPg+EO9iB zOiAg5fP!bxtQxAVtz8P5AJVDUftxyg`(ER0v}bBdB5=cj$8P|&jhF}}b&YU|126`9 zzwr26cHRjHkh`RkB-k(Af2D|QqMyyOF}|8%Th*`AuB5z{eI*nCh|xm5|iqt zA!ok4gptwAz4T{ls>Yx}9kI_oPtFU6YHwBz@ZTEc-1aA#^Yoyq`&tEO5#)`}R=Wt` zOoE-oKEx11ay2Z+jq6_%phqANjMMtd65WyX%WCrEaV{=eO!1Rq zDZ(8e+|14%dEVE{%}qkha*u>t$SY+CA=abAC(5?d{`vgXD|I}!Vz?!R90SBZI+qDJ!9)eQ@A*5*6>0!QrCCH z?AVc`6)xFET3rQ#Q;JL-RHn6yi%X{w>Zscqvqtuj z{Od3HP5o95DnWtEg2a@=2S zK!dpk?WCQ1js?po#>R1Z)L8Bf1b)MmV^^cZ-lgG9WZpMMOLg|_*%%Nx)W8xeGZvB3Ryh;-wU#wcfsm<@kRg;Z`~2CD5z|iar1;9~*k}VTQ~J-|W#BJL zJ$h^*2vkODX8Yn1wnSdVm|?J<5fTuW@imF{zgV9OZc?&RV|&fmj!+BEOR20vQK)?OUb_OGUgMMYvD5!&}b zEOxoe?kpDriWn72PPm*EZwkbil$?30OKkB?5wMh$I31mHev;;+;Uh*|YP@up!$Y2D z2||jf(gU=>dy1m94smqCqDo{*`wkzz6-l%pRDlVL{$QbIrGzO_41eJeCRsijb zB`Qa#tqFN^t)QR-=#X05iMnJr6a?ziIGtU1bIPEpwmAIasuS*h@%96fpJlz?KqK^9(GXvJy? zl=0z91S`-kR)^pJ^_NJ{+@FdzF)O=WRcy}ByFk&hURH(kPg-gHV*p~@7g;I3y+%bx_fnlPnA3cJJN-7PJq)Xyo@b)#11+yusM0o?GauCwp!)8M?#O)tw<> zgp_-#gM;qlGBgJU#l-hOz87IQ>S17I!Ye8&`rq><0^!czQF_w(hbV0bmGh{n<$}g z4J5LGC2Nrn-5uS-iqP*E6Y>QS2|T?1P^Z`}HP%H$eme8L{UArFA zcU^#-|4VhX1zS6;Z)`WNPROSRmsV8{tcnMcQ_)qGmX=Peb2Pm6lPV0P;P8tU82L>uZ@i|`KQx26Ced|+BAM`BQ7*yuvT)DU@8Ew@C#EM z95$2p+s&NGC`dIyVjMpFBS%$4c~eLOU{VD0bKt-R=$L(+xWb1I$FP10c0g>cX(2z> z*;x}(P<#xT%a1Z|fWKew7$dX~JP4-%v}9^w(ULkZfqg?Hb+j&Oh0kNE<$6pjrY(sG zqS61Npms8(lc*UKks?1ASH)ko;Pc2u%{gI|mR=bxDLlI+mEAT-vouUSe*AMnt&t$9 z_!f9nKCLLD@EDvl(Wxm&-hkO>OnKUU`}c52OvEJm3f_8Q`9#v4 zxKz}5XTEwhZ{H(h0|O=C3Z@gTx7;Md86Du;^!y6G*=Y+oa> z3CWs!c2Q^Or`|o!b9{YKI4Sn*Ine5|-rP&Am~GR(t=!%mKFkS%1%K#n-faOu9#bSb zQl1#a8F*N5L!!XxN%Jln`*G5hwJ61Z&j@&SKB~tPQ+i1WP68j}&HQMUtD3f6=(B{t z#ha)_yKEkzrDEsKp*Xjm6m$(eacHL*R3B5E>6p3*ggy7+rLJ`7{F^k$a(#8hZ(3nU z<-|`!+997(#GFv7x|9Lid}59eJjpAcCvI8co%`O6IzyuIJC=$=P7B`OW=`my%NxVqPbX)a z{+@MM)+ZvvcPrI1{n6Mo2do=kyrnsU}FU`SWFfd+cY; z+J|Pw#^_%E!-oz9+v|>ns$i4@9l1h4@~msd%p^DB7$!rtEqHvq2a%?;ySuoFX)VLT zx*bePl0t$avR}T@^W~LrqSjf-al0U^QiwKBo;!EE$%AiT-A}g+bBm_#nDL`ByISe= z>9f&VJ0TU}G8A(rBeIm*8a^$!-rBu1u^5*&v(>`49< zw97gZVJnb07(RGOkI8mmjMIy&wXG9Z!D$;PS;~I|EB^BB+kEHOsv>qoxw(!T;!K$v zRJVui&Rnt=R2l7oImQiqJQ=klE;OW*wF!QKphI)mMH*mvFzFJ2pKWt6GszPph#AIa<87H&P zWY#%G2h~xkOOgJx9qmEw9VLi2ke|};QEl| z1%v2Z-$7^CP^gcl`d`$Wy#~?;+a&6!4oT$Lmf9yGH z3AzdMwH}mvP*-qHoN8-MiXgz^?wJx&nUU~tRokG9(Y6$=TgbU%k7xxJJYd1iN;iat zYU=5Az>^y8O?moswm&TvLI(Ci#qTMfB{y~XGE#vlQ(xCqe5KTG8cQa{l+Hq8FKtV~ zG#mNX>61*~a=VfYkSxj}IiSvl??VAwz92n!A}&T1kk@Wj!sS~pUhLjI#?sbWqMT=G zOl;nkJGHXQRmIBtN=iCOcv`bE3|N1MNdVQ&RQvq-uW-~co+Ab|~4ZKy=+Zq{>w?{tI`*-)<)fsM4srR{(z zqN~p2GpHxDV!Jcx@q%xa7PQo^RH=s31jJhU`ij@)jqwM7UymjsFaOROt$m<08W1`- zJ3t=$xK2+Og@Gfk4-I|TbY;mCL%|t%EnmJhqD@hFShp0c7BMUwKn~WWik<+#w3#<< zDD~{=*i_Ah{h(mCD?Z-T+B&?4m1O_=D;*Lr&&=?GLWIDE8G7sGg68PiHL}}f#hGYW5*kAQ%;LzLsii@x>}|0 zDIQfwLLj10wTxNUc;L*LPW0x=q(?#gH4gz`i*3EMVzKtRo!R>*>}VB!*|*AN0sPY= zPks=+3VHxht4bOnKxRp*h6pBwIpOBs-Ue~|qzKhc(zE>I%d5NcG_+RBaFb|lrgxHZ z6z!B4TyG>oWGv#w%I^viE~Yq^G^{c|dV+d$r!@6Z{(V);1|xFdbvS%|`^xqjyE(ev z`n`g@d}CF>$iy&9dZMm; zO#(=!S=uw}e9;m?u;r?ztY|?l&tblr@ND)sETgneVK%`1I zphJ zvEQi&Mj{Z7d+M+S!1&eL+T9vyJD8I_`n%*WG$cN!M{B-@#xA8+!iWNf(egRtPq%E- zzUWm;^^vOw14wR0^0e$qM{CV}87ZWf1cI@C&?G`R34KUaYLb4%FRDPxs7zT8mfuEN z>E2>M-soE^$(y$Cw)js_lbW<=)Y?si>^eou6eV-%TU!N+dOas+Bl6ZeyN2ChrZ6WZ z;4hE3xVS2UeyzcSw}!8YWcm?zs=JV3!w7v2$vE@P{m4eeI&{%ke-|3W{|ue`lFI#Mr&JJfBF>&BDZLg z{ls?DV4aJK?9fk_lDpL*jzRi)uOwr#jg2J?&ALhtWOdgV-6rVtiIKxhw47V=`|wxRGB*)(ZDA6j5$YFV>s^Oi09k56PJ zNDkW>Z~Ra2VPh)%?fL*C|vHtR%d}Hp(VmMym%mBu&2j7<^ju*%s zH7R%9`i5a;C{B{wvU71rSXkq{>C-z4@Qs#CgSwv3p(KERO_rpHRf&&&yd2a9nAX-g zLSv1epPa3v3LKd(8HqOU4gf4bNKQO;lnqPJi|uQ9y%WJje0;puxcwRgyL5O^FQH)I zP1UC%*>J2BSiu{(7&=;@1E3+yQtc4O!>3LuW2{_)Ewy6j;8aAaUGnZ-pNF5y&$d*u zJ`LVwwKm35cd|TR9{qM-al#<`mR}%LfBw9k9MczA}z_UmZOd%+@^v_+Sp-( zNK=7xDTr*x-7SPEq7BWtdos%1`vU|aExN}3@BJynSPHWjlrc_V98gZ1>-{}z&0pW> zAW3a#@K-}MfY4S91+?j8)o>Z+Jd>fFOs6loeckW?5-3$Y$1Dzgd&~tD2~Myr#WqyshciSrl9uC+zJ1W)|`=-KuzQs{bqV>$K`<&7m^~%#5p&2ewjDRFq-^ zI588+fP86F;`?f^<6+DK!G_3mH}F@pb{42A+7zt^6@y6kDJHUyZlh`K1AiXP z^3oSw9_udJpQjO(qZ-g3UJyh6DatV560dj*6}{0;DBlC3SVm=@^qwX>K-! z7|l1GI;?AWz&ESc?m#2LGQ2)=BHbM1r;XIqLBHJ*DM*AQ)?v}B^pPT)g@Mq?`f=Rx z>dv?$ib_{ogL<}AALVfPsiY`X`9HycrDQifF~o)gYQ^Ryxxab+TJSjfO6|@NLN)n~ z8$?1zh`t#8MiUxIv#t+6RWR2T@!5^YmQ=9u}hRRSdP_b;iah`>ULAu(!gZRzW!h;s?f$QX^t8A%pN)J+zc z%c5b`fNDgvTmYMXOOInf54jmmo~X!d^sO6aS8+r{wr-~qvbM*)d-o8@-{3PusTlg3 zQ#bYt_5_FRRi3M4)^oMZZHP&O#7D+*Z2D4xxYStzc72TY+sdP*x9? z@xYXdxd7ehkzUiPMd*i~zj)#I%c`iY$V(z#Fy>Y}zI;148~!|k48n!deV@RqXL0)t zv?lPs5RHMeem3DN!r+fWWd84$i~w~6BY+D9T`->X`I#>_aJaA}K=P%;_z)cD@~>p{ z+afq|%p0X8(Rmuzf5C`_T+#bjR`wvZAi!wEh|K)_w&0Z^YN{OZO;`i3YgbIMIf8h` z-A-8AU4WNw-?m2AD&Tnx?}!m2q#9TEIRgbC3LJ>|+2=`nN8PD@pvu4)=?xv!BSrML zWH0J<&LYAuSrR${%=6vc=x@(f0-%Z5nP^HpEO?M}j`Gwv(HKQkWp`d$_2tVoa21s5 zEkRU884LE8Dv2pm{k|8a^J$_og&#}Sgcw`Y)(}*KupE5ySVu!cib?MF7kzrqKC|Gu z^kb=+#~twU{?}UMHtnOn376yoC#pWXqA3=Jh%o6t|eHlL0QKez`}0{&f4`^fL_pn5y}Y#6eQyTh&8W&Tzd zp`-gO9^b30ZxGtB zW4F!BEN%B=HH0B3y0YZ40|Yw7;`6XZtr0M-NA}#N&D4&{Uqj^((YA_nmc{r((9) zfnY3r^k@T>V=1NEwr(vU)&ExOvX?S3&<8}t&7~EqYYy5SrCdXF4u~QvA9+67;Mb5g zMVMrRRi7QSEzHY2ft2p7oK&leNhRLx1XbRFu$`(uGawWWuqbTXwCUF9?=4wtDr^~N zBI?>InndmgzJc8EXhsJOOYb>jzSjIT4M9=ztnJ5smz^YIKi(Io^+IddeT9iA5?D7% zwM&;UL?a@L7Fn%#>H7T6FM4a`8K7hS@cqftr>EkOU4)Z(T525{*}-FEw?%zl?$^|^ z)d#-}$^Kqd6-=+DI2)lUu($8>{qu_m`o)>fnX}7UGdWO1Ht>mpc|7EAaNuHo;_!~p zw?c9T{gV)_y;Ugs0vo%MlamB*%0IpljS%(CN~)@TtQz>6gfYt9-*bAgz)F(upi}_O zHzf}iPnb06CeBdCuh4F`sE`z4ciXm>Y|L=ko#6RhMMVt~I1MJ*$Sq6)AM>Xhw@~)R zNjvf8LR~;!P=Zc(J*2_#!`q-UdORhu`cb>L}RG|k=2NGszySXU}TB9Q1 z*om0dA$TN@YAvNtOpAnEse}5cEG)QCG9up7u?tk~+}WAlzXb1|k5-C!Kw!6YG8Ce~ z@J{ZucHT%`J1PF__wU09;00*(O{QZZ)@Bj_74N)`5B9A2k)d-V)a^FUBagPN9?tl2;U$#f$*s6A@*%??@@MyU-C znLMh#6$@^-BW`q4!|A*oUDedu640WT%XLVHeAKCm2g`}c3yC?g?CP-P@2B7iMQ>KY ziv|aqr9^0?Z-yT|bLPyRIpx|obgmwCc{jmGL;=40t_ONxl>jJX7`bpEC18VyZaFUC zx>9kP(%fBvB_Ms-b#(w0@n)j@&%>jQf39%CL z5H;B?USCSg*qvd28M2ZJzrA!aVt^36$ru9?9EzMQIUoY?rDRA{w_NbAH~e1ts^DT+ zcqcZaJu&e9l>q}edJ80Hi)^Qz>NM7aH>)(BFzt~5(i`R-9TtqI5|2#78^s!vLx)fbDdSIW zA~b0go&jDZC@-pHUzhlWlk!m{MAY_tSm9!-fv!;lx*nvVU-^$d63I0{N>PRpB;C?T zW2f*#%ER~H>Hm)wKw0@DM^+_jQL50sIbIb&rHf%BM|Y3_EFRAb+`>RJ(=nm*Rwz!b z#$sj(aRfbm4Z0SQJBN&l|=OQT%dkPyjSFk?+Fs(GCE+SC&0h7M9X*$T>T6E zQp9|yiBC7+T+0Y(>qaxNbYRZMCiS`_6UUFaH#b+M-B|y zgYHyoxBlHGY)`9Pchp(eKy$)@u7hp5-dwPx*SDW#)dr(q)jP%hd|pveUtqFfW9{(A zKNBm`PgGnwvFs(!pSb>yJZgDB|7S_nwS?rB!WDjd*ORao^nkjGNo) zIbZ?B?9u+ijf@V~_`Q4gZX+)0#-m5yE0;}Cdi0erzrHfW9?vlrS1Ib<$h(~}q=uCR zcs9Y6?AJZl?>+#Ic-vma#=G*OV=-g7^CL58F((DGokFMsy`uDT57&6ng`q!n!u^r&|)~0@_zt!uU|h_)YaFgp?pT1Vu}PwFib+Gtr^mQSr=R`l4rYQ)6(vu zv9Zc!a2KRv<>mEa8bYSy47~Jq8%I-(hue65yuzWS!CB`=7w}EO^i7h#E-*Y=U?tJj zO*gtO2BXvt=vKw5A&7r11ZIMU1#L21h2aEVJ89}NypEx{tQ)P7RpZPgb)&{f!Ywl; zakK9g6|E-{r+GBaQ#M^L`)ckn9v}Y1^NS zNRXy1yl{H}GBXaEmTn|LrxZv{oLw?)(ZK=(YuCHFtD<5M5pF3*U9bSWQa?M5DhuwI zMvRr9w;5Yo0{;gTx&1HifwRwqRqDj)!&ety@J@*?rsg%~w!!qnc5)DviYkHN>b}2z z4Czxaha|Pn`Llfpt}akc%ry_iIp-cVExjz#LXZVIFd2w)Yfj(cC1!UBkVUsO!#^H# zc&K{G0c!*@-U!)yE0yn}kAWx=N3L>1r~r5YT!GKr7E0Tm3Aq93bHdqoi~uLZ5bfwh zm#*g#HV;qCYpSgDo~1IGdv1mv92z^@x=sc1RHeRNl~hdhB@WHHOGm^@{#2+>Ik4CH zk7s8V#@+uktM6lIiO zjUl5EVF2*ID5!_iNO-A=ZEeGuD{J#@G#Sb+8Rh*Ve55%}hQm|ZHW-SEH?Bu(k_w*} zdqHv(+qZ8o>iLD50bLb8=itX~pEdzC-GDure}4wAAz&@Ci`!RO89P=V5C@RAS!Tz# z2n9BXWMW^-ez%Y^X#VUSW0l(I*g`2rmh{!GINkc^T8Zu4W|it#NK+AGX{N39_n$%q znRdcU9r7c>Ce^PkB}zIXF%Uv346AzF)JX!q0VtqDe0*CS#0>)ADr8y#A&vE$$O6C- zWa)}3qkH!bUG{Zen19+mdi2U2koT+q+M9sB4eZjkhL1-MLe`b$#`LmTFGUWRO~Vi; z508!99|KKw8eg*>zZE%Ux zfAd+T7~422i8z_UZlPA;t|X7&*3WR^oCy;Y*cWCabY)*s4R;;qlJ~%Xno&M0@ER1_ z31^NRIwVIGucUYH*BmRu+waJv1oE`UMv}!jKZ(pwGi}bg8uJY?xqm1w+iK_y6{AX1_siY4pSy3Cln6C$ri^ zg!&!6fDS2B4kn88AdV2(r8ryA+?jXoY*AOVtZ^hh4PkzG{?lE?s|GS%U7or_E>?fL zf&8e=5I!HDU!3MHvtK}91ee}g_s1<;xl%>B7JoB-M+BAFW}Zt4)1@gP8NaG>z*3mv ztxpCSOhxl31ng>~1JsZuV;g$4ZreqQK;7=x?CCmDhcNAQMe8eE48=9MIfMFW|#rlqMe3_qs>{pNi$W6`hZ=>D9>^3I{XxIPvf!=jvOMulA z2n^+)oWNmP(9*Z1<(uR?!j(i-zbjCPfXwt-u;|qHfmi2|B9Hp{aS+Bpie;H^9#%w7 zFDFW8;S;q}LSChjuUmR~=@Wm)M^8?SAW9lXG3ouQxz%VpKOhS0tC?2&>Dfk(tY`<5 zXjl2=>zfH26uIz`BrAiL6h~Vhh+2tzz^cp6+5G26uut#rA7>Hg8+NtzZ$zmoSRU<^ z7LWU*S`ez~_{~u#@np}T3(0+{Q*bd4vpPuTMB}fSZ}oJJgn)RM1U9OHC6z0*vur{C zhRF;3k-)8RrvnULJ(;+v4iU-p&m%M_zI^3h-Hy0#c(JAB8#kumDE^vKr26FF(mK*M ztBXYA*FkXw&_e0~V1nLs%0-{~c64+mCiDIFp>t5v(fzCO0{q&$3C`mS=tj!HgR<eNMkccuvB9&6KwEoYx^Skf+{Lka}c--fAPAA{*=ktEQ#`U_c>(vC_B1j$|3kuv; zzPy|sJ%sA9@ZIfUqGK1p3Dj_tfESpOuyD?*^i+EoQGy;I*c3F!k*osww1&GrFI?uE zbPIXgMEb6ntCq8vhov_jbm470TuO3>4qx$npF*2Spi8XS?R%q&PT%MB>Gl+W zZi6a*Yp+AsHhT0TC|jZ#f}0q0(`oW}$KP$BT;}HktSok(*uxLT> zKIXlJk+{Cbza^Yq=v~r zQRcR`2PjHDZ3-EWmfO|eUol`oZvdO5Cr_rYsFLJ>JFz2z5=94Vmz_vWM8aT;{kg5* z@K);1BN%Ygn@P@Mm~a47QR1u#=4?=weh;f_>mu-WHOD@J55I1#UOAuYo1{alN+Q5urnj<A%U!i zJZ^e=v>ZErECQBLn<$9b3iQ`;xSYip4gC%JhPmqg|KcGWu>NbZz%hj)a-WP0UFR8s z{5kdHh)+Ui!VnDqO;KHd^vc7YSXS}!2(a1b{QNA2$5d$Q)_J0sr-XrDEL40ZelIx5 zLhUX)OXd6YiO@15?bGFh*5C*sG}=4$0xn*L@W8qUMfp11Fa3(IuUNTqF+z`IPc;h1 zHk_Q9b(<MTYwUp&=58{PuCwQ)KG^OC>p`(7oXG zT3neqn3E;D<_h&apt?}=7Y$SW6G@W3Kv;3`@e zrO*RAyPTq*fRDUz#nT-6|0T=@H~tXvK0)XXejQRom^<$Hm; z9<@66rUR7^zN#_^4OxNO6Q=tHnoy=~Z79oCx{c_}Cn9Vwrhkx~)u+4zqe6j2z>t?c zn<+;30%QeaPzpJi`U1!l~X5jpPDfuD-dd570+H1bW-& zia9+oee_Tny}c*KY#M+(-4pJc0KoWK%U@mX&e=M$Y%@nnpX&QB@dhmm{ro*pbq|o} zzqD{7jhi4pUtl7pzp;|)LGwxMg4-JnOP$3;ANSs_!D%^EeftzMV))*L`7zWHLIsNv zikR&Sp*sudPBr&fcg5yP7r4W2@a%$qP-+E1N?bTm_rzPuyHuLUa{4>c);XpSSgje- zG6c`*Q~o5{$IVR!o{i{!55k#bn3N16xEnA^%%%D5T{d22>M4~D)VBlZRM#eagzX$h)FOc(T>U4(pa#|}_+-ytdbaVYQg>)C}FV^v+2 z+suT#4p7`hejFq*GCv74gXbuxps<%ZWoY-_f^H&SsOX&SckYAPB)lW3|8jwY6)c-E z)Aqd6N z^7Bo)n7Sfa5C##d}D#N#0?Y8 zksw5`%nt2)s}U?yum)+o_IrDGXf1W?0EJPY_q+_t)gR|xn==9oL9--oGLY5-XkSAi zWl@QnfBLmz_3DF^KJ7A>p$Rb?>3f+DtBr?pj~=ql&d#@cFC_5{r9H4K@1!#N+^b*9 z0G^o9dYQg?6Yn%iggu(ZFzb#aNii7y$J@jxsP2#83=^~mtm&XKx-(tzx$5Z7xu6ULmN8?3AK$;< zg5PytnCFV3p1SpjRBdCi*-=d!A%@zvt=cT%A(SD-=c)7`R~q8Kq&|B3bf)Ld25NuH zrqk3byXmx&dkWd3u+^fv!^O5IzsQigY8`XpWh%w*L(27id9Bqmb_5;7P1~Dc?`N_( zJWf@x=0%uWPBrbGlgC^v^u2H+AREyGL?`Pe8kN zR=2><@ulxUd0}?1e!*u&a-++?Ko=4FUQ48;f2&+zhay_ug$WJeQB$c8K~u zf3uHlc}0JXv&=M!kgZATJ9B4TTpURBqvPVrYnYzJ{Pfe0A06=FVwjSYlq5m$z%8wJ z+Em5~%ohbN9C?DB2nX*HoRtOn0YaMMS6lHVNv!v<`1AX*Fx=(g!R~v>g`sOY;?pD= zGDvJ~Jiw9;ldIS7t-mEiBgqr*g9oIM!5o$@jXJVIravQttSldy0U6(^_k}mlU*fT~ zIkhl)w9Xhn+eq)`@(U4&%m+17>b53{P`% z+^G(Uz!lZi86Y3R6;xDrU}5)pFX95Cm6~mLNQej+!Jv2lX5a6R0fNLXciO}XVVrK3 z9{d!HYrBigY-)PYM6ny+%RmgVgt3(l)n!_oG)Q;#X5)WDjh=n+iUS4`ej+E=`fB{& zIIgs`)a2IdZQx&#Xoc`K=Gm*sw`GYELDcaaJ3CE|W|F$vWqtz(8j4Ko4LbTwWntEe0VN4iHRciL31qg-`Hq_GsRV;NdBm_<-wNG~NI?umr z>k+pqM_Y*Swa=RM&gT{yU>&Rf+kp>R11(8u*VB5;8!Qiph1P9GQP@{U=Sc7UWp*o9 zdLqXSq6JTu{f&2)eo$cbwh|HNp&serRBW`ee zaLCJR3h6XkQw&~2*b@{a5PZSVONJvmChLIzB!;X?m-R$tG28ykfkJ1RU;cmi4uJNb zlFVx(3Idv|s=lnk3vu86{jwfCIo2RtI_cJSaHYf?i9!@yO{ttcOk!Nl0PV=!-db8( zooQ#0YutbSyrOJRv$GZX-w7_|Lh`|&Shz4Ci4w%?dv9`CqiK!DVU%OPB2*XlG30YKPI zPNx(a*8S`vf=04zk6HT+R3~$C1&RpZa*xhe+;jYc7)y{~e*URkRnH8A{TMQ2GB?tu zP|wt$C0^fHzr$^|{RzetRP4a5Kb`y!XkG+_q{HtvZ0W&yb#ipi9NG)m5rodYrA@7; zw_@*krerAAgZQ>W00t9FnD*Str=98zICyRSkOiGJPEj?q^2(p-7xRYX5+LwV?g_0Bdb5&0Q5I! zdA&fn<`;*D67e;zp1c2+S;3_WVC+}SR>XuF^3Y)Rhg{WZxZ_+mlCjLbTME2$%E{2p z3_!Yf>(+$#^x_k#&awXo6y3^PaI*tMUC?|)nIA4)Ehix@b{y$P}S$aD$VxcsbV7?u6xQTqD zd%LUpVfvF5@(^i#792VD&&xU z(Ii$#UVpu#O7He@wJMOXS8RSgYeX+6>DUP_AF zug*zS7MM5mQQJ!2I6A4#-y!WIm$yu~of!Av#oS zg13hb9E0Dpch?t)5J|oxi1}b~2KhbhM~#>fqr-wAbt9>@obyd;+qizSTHiu1x|9#dpL0I zyoY2qpJec85>pz83&MLIZLh3c7I`cCY-X8E#+nRFj^DE&8T z_g^kuHvlD`@PIEel+Sx?XgI_dlhO@tMP^&K#^`r7)?z?KffL-r3JXnohkA+SV&RF) z>;jm*e`T&X&ra3a)$t82X^Hp)%!{{tqa-|@zTgVrmJlipJR|!$Zd=5WSY8m2m1SN1 z$rC4pYZ$LTg@$&H-&@4?SdeXBS9KLW@)>~Lz?bf_v6-I+00w`UV>5$cHu$kpMG-H?Vi(ujG8Ne7yeB?dzw&lG&$ZOt{uSb9=FBh%?(v z=Hxct|LMNFNIs|2E-FG)hwm;N4n!>|oIhT^bR%YI&I+g0?I7?+zV!C0lJ24m2G{z8 zK!oMx9U)~z=%lj7w1-f{FrQb+y+wjRd^x9YmqQwHtY{XtHfw)OH8c$3zoWi8{%2F0 z7A;x;f87UO6(fBbL5Lb|JS3Wfp1OLkAQ%bl3sm>dn_8=C-V){(H1xMk-(s|3h>Z{Y z3yWYArGS_<^i&*lv2RHru!mqhk@6pv^gjRvm>G000qG3vZ6O2KOckR!+npFI@tSqA z3<_Y6xRPiVab}6KR&VmVAx?ygnE50bwG)+67wY4|$5lPZu%9LWUw_zyO97@HEqQLW zrNh(3HgX+WH!+xUI26V->SuZRJ?a>T4}{X;EG!Wf$IRO2Wc$ zU_WPH;kz-YN9ciHFETQw<(7xzkR~n<6Nv1^rC`zN7Z-h?uzVR_^F?p=oH@dbi{jfo zA|m45AJL<={rU=FS_MJ8#etw|y zrLh4-ZQk_y+hxFt(3#hI3|u`1xVoz#bW!%ctmrKcyo&Kg7cA4pHeQXc~e5aSY`2eLva#H38z_K967Rg%JH^;b2W2|V;+rds1=0W924RF%>K^< z;K1nh&zK^Cc6#FE$@9E17X2u5C*83K?=x`V)U`70z|jR22^#Rc@O+%GO?>_c18aJwh(;dtf z3R}y*6$^oX+1K8l3R<{ai9eRw@zW3d0_^ZnTG`NiHnvs-I5%L-47G2MLK*LgXayLy z?|~i@CE$SIS7Q>$P9Ssx)T)TtvFZVeuL$>9!PB}TM-Kus=B2(F_#}rafFRLZ^q`c- z?ETTM&R_N7l6UgFb!l(uRmB2h*@ba0nyTw-H_?@So=^|cy>e}`M<1v;g+uWkFX8|tg!w~Q%rbp}O3 z@Qfts$m2K4urr14EPW?{ZF)n^s_f8aw7cThQkw}J#WdlD;9v30D&Ba7Hpd*t?O}Cg zrOkoNfvOG`+$0e9WLiuC{y+Nm3LLQpS2i6~xgFKzvJyK-@4lb7B0E&h@4pLWM62m? z1JgA`Pr|iz+j?Vd5mx!~gMJ%TDpCBn(c1-dF}{hlE5tW=u9r8iYKJkgSS4U*C=Z`f z)xNBm8f+h<+r&(+BxUFm0?A?ZjMGUkR^e?LJVvUdB@+N<#J`-Nf znrBmq-hJ@3VbG%>pGbqiU0UgrVFpLr10-7b#{#vjJk{Ew><*%T+n|VM9Mb_u>onnf zhAADMK5g24K@^-5y^_u0!n>>T1cJkQ9m{4=6y;);ec)-7gSZb(FR)*??l6=4*tobU zb5A+67ZM<>gxaiGf(}6-j9;vV^3?S|ckHGkq5O2%(?n}5;Vv67Mic&<94`2RFl;?H}CyCh~{C}EEFc)v;ORI@Mj3AGi)-iHDNC}j_<-&YS00y zUm`tAWVYaz9i6xkbt>Kq#j*`=sSDbD_lu6+;#PO@5C_s-kl`<0qd}uf6#~Y-n=GPN z+R#R|ga<4b8zRNuj2C1n*e|3b%eS?o93CllS@owY&sr=1Vf_V2$wT;zn4esX8S^2* zIqpa-&s8iD7&ug;J(_O80u}!xJJcuJ>-WqsOBpCXQ-GiZ`v6oz1^a`nd z-|`pFpFd;b7@aE>+%f(9xVX4}iF@2JN10%NHnsS zW_gM-UdY>-RtP^yUi<(zY5EWBhHO^@>WDoRFnGCjV0pp`i6E#T@50bdWDkm^#~yWo z^@3%L^l9Z;FTRyD`dOYbk3W^y5}c6ZeN$R|M-B2GjO=0jcm0lT2mH3qJ>MH!!6S~Q z9Q<8i#|N;)>Ec-7zs%6r zPDUPkB%QVIM!UU>oc)2jgEYY%*4qU-Mj?OY*MKEs?|Ess%n0}>xC8BloA?n!Vtqiq z$8Tm#pYGr@NVWfO2q@wy!4%hj%WeP59%GpAHuy+QW+Pu%L1fSmGi_~8{H!0OyK9^O zVWwh{qKpUZ@PG5QiZ3Eou^h>z#LdqLy4YE~2qMd8U_L=Vg4sW~e)3I*8KS4;27jW* z6j~OaB)|PbUipc^j9|sfHr;)X41r%MKIzY8x)?OTn&>mI zU)IzbyehB`)4VBez<`sXdwPsqeSkJGpv!^dm#|7c$GVFa#svMkUVXj=-j_RI*zl94V#D^$Jtu>_plf^-#~9s&j?#AjNKOiJ-} zdE!ymi85=nQ z7*H3zx|X3}?iOBaI3YxUX$tktXw_4xTLh##(S3UcH^?nQ354-$p7=Q zz=6U%^+#Lckkho@d%)jUpeVoZzj$G_qNwc1=Vwc04;=8%12Yg_IiNQ4ZIu-Um?r(7 zoNZszEQtN0t)(0cqPG;2?67>ijwjOOw}Qzd%xICd%6(Ys=s5NbG+?hes{Nyo%`kGB zvs_s?K}~eNll*Sy@#CUdfRe?`0H0k^`Pd5oRbd!pUlBF3VF*()(OE&75QVnb$gq6* z0rW|37yW6VAgKxyG}6A*Kj?8q~`Qn2i_>V}OQ^{E^LKn%js&~T=X zT2J(K62YWuArU%IL?oZQ9#4D)RLTHhfL{!mMz{#>WKrs~SFA5?#?==S%+r8fq_1Si z`ulF(ruh?tLs1tP8yYl;DP(eKzz06?q@6p*i^RXVb^}D5pegR6MiWd5+i{FM_VHO6 zHY6JxTXz)xM?Lfi((P;yOhi)Ni`C<;Xw}3kpw2OQQBA6&lN1I!Unj}S#k-SnfN!xJ z)wnTIX~%usrvCSruQ$aD`fwFI0!>?`KLLq~n3=c`car(J<8E?_$nDo7y%UuzC~k$` zU2Us{h0h!%{l4GFQo|jIr4O|!Y#uuOQ>_V2W9ooJ41= z-~rN83ISo?a)Q5DSG}R4F^6a-059t+GriNdr_NIp_9}=HC^h4uA^>mcjd{w=#vil) z%U@a}Mkw(ltz(^@Q`}5znEjSZZL?O-cFW9=89iC&(e`!T4}?{dI6whkwt<%#5z4C_ zx4TnV$POOQZEOc{JYbID``eqQO6M_%9B`bAWT3j6Ybhi`IlG3heRpE-WCN4*2M<~b zLZmQls>53*{Rlkf5}DX*=U$OTp(9)Acn%oyi?DnSb#XX*(?~Er~8? zpOV#Ftn2}f1wnX*>2AWlFpYqZ*ODym#_m1f7>b+ftm}|Ke=-Lek5993ki#BxKkNYk zz_2n*jCX?@RYPvwx)r&nHzpF3*RBnZ?3%6k5-|T zN)6|U*UAu|N-5=lTt9-}V85*t9b#!jf`Qr;f{ZZ4E28M4*q#h>#g*&bGX%a05$8Vj z;!g~@A%=Grtqa?0h1(^`Nc5dcQ=_?Y0g5JD!CC+D0`gyccNL%{7XX>`BFvGK-xlX3 z>WPF*1Lfy^ppkTXfF7;w8p<3MU@(*Tz7~|2g2E(-yCf?8H-f+4vG>G-;H0M&{0bM2 zXV;%#RlnH}wM}8VT7N1o{6v%mO_yaU!QZ!Gk&TNY2Q6i?nyObGOVYtus3_R)${4 zZQKVYv(x&|pt6zFhnTyA5&kX3V&O1S$eq$JD4{#B%+sr$-Y6YGmx^9!YTCpJfYf`~ zr{h=OMd*Caj60rbqxW}Iw0N4DJYB;+4_|N zua9c*OZ;JOcQw9BqG`QnV&mAqPBq$C4m~8e31dUS$=vQF2ql1c{&z;d3_%v2ECBhE zlB+R#kGZu?^Fx<0P|$xnmYuKIgKO;O{`b?=G+C}Js3n5=AwQ(~OHvM~Yzp@VEKuX3 zHd4_Q6 zItfEFT+GGp&FwWmgN_tuGV5A|YfbW|?qPIuRki|)3v?xWi7a`PsQJ70>6GYk}b9u0yx^OsQvBrVE^ zx9xjey55??^v$$Z!yr;YkIi^`?v6M<_9NHaj8kSnh>v^pK|lRwIT{ z!j+abRyUra2||eMKgZ_*0IK>tp9ezSSTwZhw!5-MqD!fQ+FvN=uaog|Kq zj>M|Y6RTlFuvs=);jHxDkKF5m*P_s+iG^R_GZr3@y7F!^YMPf@ozA2~sDNh7&_FmY zq$TboZQ&0~o9Pa%#x%l%b7d}aYiukVTO^z+P=Jz<36QiN@EhGK$LQ)>7Cx-^6pl^Y zJtcW3`ox{zJ?$TYr*T$=%aK4?GJRzrq%j z{Ci-MlU8V0A&8ic^pSv3399(xz*Re8=BhRBVzSi?JiE(Qm)~Y&>rm@3dB1-zA-=d& z(B-%=*GG5sQ3*JCQUEQ|)fT7nHFeRq*cnbkEHuV0OdsrCRc$ScA1qhv=0(~~0T68G zg)Qx_qo+sF?vwvJcgcgSPb*f?_VOgKt^LFUS+CwG(rAoyvkWd)AaEO`RmDuloasRH z(}=XY zDUm(%W6$wL#+O?k3_ZUA4Y>Wrrd714#51;vxb)pP1XIg_ z$O`<7XKpxO^$m`O9*emx=Tv-Hw+&in=)Bvyt@j-8pUHny@TQRpj!64pHzm|xV-P-~ zqR=dH&RD(icPkZhz;3Xasb-#0cD7d;ZC*Ida-EPEb2^08AjNq0S}o!8BDNW#m2<`C zW6sx)&Kyx;QI4%cn^pNh1%i-pVzsJF6%5@)2)C0zyTB*5ebE`0HeebIkY8z^oA5&K2I_h zlsx^n3{`}wrOoHy`>;jQzSQOry36TP0{snEmcj}Zx4DZ8nprXDOJDYwc1oahbVBE25W{JrH(;9G6+6STw41+~r>&Xdb!45P+X-}ViJts#W0pm%S zQwQ99i86Hnp349IOX|(ix6d?T{AsRqy9)YhQyI_WP^2JVM=n|!U}cfGyQ`w&`sC+R zonR=-D=4@-$jpV3DUReO;7kVXtH=`4jVnBD;P38igu=S>eyUPBUl1*)8?#x zXP-yocGbtelMRRD*dFklQS!^YPRYUw1b)Wd?d5+Xws2-Z+)8DuZjgWlt^haSEJU9f zHczd>j@}G=q>(79wXS-T(;mgkj**Mc1Nw4ncos!4Jeqh9iy_9$gGl6RmAS$|6)vaJ zt|<{P%Iu%>G>holNE4%0IIx4il( z_+P;nxU9RQ7}+?Cqdu$4Wh7`|h2!rY#?ofysv~09V#q(TmX^?m0(V^g?YxxhOkb;Q zpS5V~){$amPZvpFM|a_vfHwN?*>{6b>Nj2O1f4DGG7gEIVEDpN_gLg6fG6VlZc%VH zNa5P9eHb1WZ6JRhEsC4>yRMD~$Li#HpMovyu(7*-4KqUwI0DbfegP5_I|w6d zXfL!5PBK^-g!qcCNlxC0nE(W)@HgLVso6*Y0IYi>i%hCc!%!{5 zAjPQ4_m8vN`4YKu*i+!iU;XkSIy7c(8b~F`vl181@_RAi9TiuwPF{=WW5Y)Y`o7-` zneXh+^~g~5NTMgsB1O!I+DJ_*_%TOvnK+M?t@LHh&J3d~B=D zrS-g%b96J*e$DSNd&2=*Tdj%%`b@O+X%`sQ3qNKqg>*s{vpTV=d&aAcg3O9-?90+1 z6J|d9QYKQfYcIIlh6{W2=_hUpWajpLuMadmA(nIoo>LQ|4T1?1q##SfBA8}^xi$~K zBN`M+emHO}QyLvGkxHi%hgNXdqBRfIRBg-WvF%H_6&X3{A6kO-#!^9Z0<%nh16DN_ z(N^sGBhu*;_=g-BdHHcPi~{WNTJ{6e5n5ca&?~r?6X$@Z^1Gv;(!=_5i@}jDEw7!2 zy;RjlZ0h-mJqy(aK~^dJFaV!08k#-n=z+osC@fKh?51wATZ1G;mKbe1l0);KzkGUp za(Zgu84TQ^|5+s7!M|2O5>ZbWEa`=2AG3l~~;6cBF>x z#*&q#0{_3e;|39yeG`@E_T1i(zdv$YEnEeg@B<71>!2FXw=ID}t8=gECC{fDReNl# zZCd4xJ0%KLU!6`)TtFWx_}1_btuAjsE#ywEqw&DGNf>jGb>N&ao-KB;K^IlTUa(EFNW;Uc2M9{&tPh1{mTJuTV4uR@vHGMCdf<9a z!!SDN(Y+Th6of+wW27t|NHm@VBnu2O(8=(~L zZo)U}n=%MeCj4gYl3*vp`nw%FbO5b&%U+U-dMd73)p91pX?D7XJ{yTM(kE<&>=n{K zjf;ktgxizB-+f>ksDoA1RnJ$$R8FNdCcj^p+G)Va|7L9QH_}6bsQ>pk;qZ6DOv91P zJW%T>!^VdF0&KfZy&k%`R_Ma^r7*Wh0cy5cT8fUf6Hcw2yPRG1R4J2U!!0EcE*mtS z0DDzSu@+US%iCA4j#PtT4U41>F;wpNVZfZ+vL_%Jbkw`kxsA zwi~Q(W}tTRZeYHkEVH0eoR&=b9_=5{UAl6qR3w)>|78g-frd5}Dh|QF##hmKINTaL z<;@rpS=sG!9dN#>j51bKYuWjC=vjaW)_AtQn+13VMmKFjDPd|oOq3b9+i;T6#~1^` zX-me5(Hf6xwP9o@FCK`8GJmM%sW?8Z*4!9VX$jAfL>_om%?3WPct6ZOALUh#Soc8% z&5qm~BW^Uar6iBaW*krI%L)G{43E}2RP&I5fiStVD`ykoGoaI>PlG^w#Y%BzY<6oP zo|VhYR2^Xrszv?SrnZ+U(rltS)exf)VYVY*$2~h8Y0yO7F^HdUyKWizA5*|3$sS)G zaCrG?u`ZHHmeS>QRQoARkVZ=l49YN3958$rHNpw|e`QPlc8q*9pTpy2Ts_J5XFn# zb;HtiM&jrsv_m42`9;CUB$TZG%y~{;Dy|9XA+k$+3{+J|MB^N%H9M*$IN=iA&}8B7 zUeet5sMjDl4R%)QUS$2I#~GH7Gs6sZSb#EBU9FU2Y0Qcy!Tb}dW+d?!U!|PVx29P?wmDxWus9Y@`yjg8pHt_CCO)0^9c@s;wJf>wREeSH_jrfZoTA2dD6vbm6>fzvcTi)JHE_`L46UlTRCB?we`in;1y%+SLQr= ztl_1in<`D$G2W2e?6jeI=TEiyALhJYJK#}c&!<0kUE7jV^nG{chi{vzUZ2^NTUR}J z)(o==mHs{@3{GWRm81cbh^0dro8Wrbj98uf%)#l}%xfnDT^e^}Z2I=(R0z`9j`NIZ znR7UpBVaq!*40_1xquL@pay+d5ir+#4-$qfhf47(X z_b<<0QyQ+?oHuCu$G~UY#xDmL>@HVB&Hu3D+i4toZ-)qW*n)6&bSZ&`4mpbtHGfBk zj!u=J63Q99hA!Th)ui3rb;8gw%O5`IGjb{hiOWj@ZwjjNo()z^d7xb%FSyOa8&Xl( z7b4v1iq}Jj56^yH_sG*R>7qwpp=bS;vVU%JgO7SBPBRZ}N9HxeeyH2%M;K3kGh3oE zlVrUn;R(o$<#dyB`GF^r@f3im#C=dDf?Yt9t}@V&5vJ(97lE^BJ<71W|g<3AbJ4`k3N zm`?p(^8@1bz1Xr9(2EgXx^ycP*e|6|bB0G6G4OlI&Di1O6j!p$pKPq0fg{o=$6Q`3-rd58Y9iiS(&RB>}%lWy(=MyyS3Q*@csMwT>O{t z0>c0$6l)u`=EWW&X6wy~Y@ahy0+&*DvQOHhN6($g;+Lt#Q8?ZC z^>m!ZYqt~i;WoaPW}M+j%(NITaknO2omxAD`jo_=*ePiRLx&fa-U6DQA>^s|y5;?{ zx>-`n5A0(^i$bGoeSNWE2b>dlwARp}3eHWUz;;kO{;8#F!)OM#Xz7;hD5W&8xn$b? zc-bvjlA_=)xJ}Qt<@F5s;9)FmHKaJu);sq(!hi7GnM3aW+;hF2lQju-(Tta`scF}! zjbad;vc^thR_|EkI$=O$-QVvIJJq`HFUU6lc|*9>b!ej7_j$AKhzr z91vtYKAw+jwhBmgaf+wK=tUubyu%4KtpzbZNArJKj*3pPKDz zV0?#7p4)f737~ovwZ_8=vd4^c$-J*SSFU`2taY#LNd-BtV|^$Cx@BlT>bqsV3hiCT z>cIM?WcDCnfv#giaICHP{rm3sOa>&gW6Efgau6DLULAME{qQXqxLpH!K6>)xC0v9p ztf;`kw}>eC2hL11z5>oz{ZjTGPBc?sMxn#EZ?+KD9 zs*qdQ2oTKZrr&FtvTv>_(=qNc<8kQtt|`im38q8nmH~@mCOZYDRR6+Vq)CR=q%09ukVZ`_2p@~JPTjLnKQOK|_I}zoFX%a4 zUN13`N+Z=m$Y3&mZeyEZXk;`erfFs;O|4MS;$r@U=;1eLk?y9gRs*pTj1V)k9LUT! zLE6^5i?ulGTonLoIfQ;;ImQ<&-`osi)eUOC5Kuy!@OiVDwjp=JdG4ZMXLEvzg4}yO zhqgH!r=;l&T%lt;2g&i=EYbYw1hakkvH2)MWVa3lkS%(BBc%KX=&S*e=gT@ZiHZq! zmj|z=JNIKM6|z|}BiKIO7tYs-Cowsp7X3`725q-&Ci!RtKom|ZIY?Di-)v&-MZ)V9 z?AEN>H+sQIr2M_W2xZzP_7WZS@ww-=bCO)#u7*4Cz838$wKAI!ECRt#mv;oxW6KzA z%#FBP;Ypv9FW+1@FfhRpLTmfqk77vszBA2M*^;Jh^QzPCd)15*U9+6^BZ zSTf@H?aVf=GPf<@GwtQOH1kfgRD9JNoQ4)6ciFf;NV=2wVYG27gm@U-?qwk~2c8EG z=v4LK{dxP0=(0K`JN}BVHN@Tp=eB<3);){@w=}-zYPT-F@)&v#GdUUijU9H=C31{m zXPJHI;LkEEY)FVFUIni?4E9e{-ud&&STJxI5!AFzB)ltqr|efuUor=us-R(%Z6wJ# z=gwAER9K?aC66vmZ^X6Wj&wmB&B3vVyp-C!9$cp!9{3hKxXyqQ-g2491Q&bqaC-P+ z5DJIU+Vf0!%-2`@O}T29h~_(#A6L$s0UkF3PS*r-%vHA*Np@5UGhrxFM)G_oPLZrP zp^tk(=35hWjYSf&Q-dOXM@T+i;uM4lcSkKPQ<3SwO;bOPt?z2(WwkKg@#vBc>Wk^@ zo^kHAo~%X@fX9k@^)}6!S3_ypW^XLdDC;?95AG`iuh2Cd0a$!Vm|GIJLO&q`YOdU?n zoVLjYzmENC$-xX!Rx(H8ybVpb1`BfTws#v^2e;@LN{Q}Ray>O05@Y=D=hNnZTThP> zW>@n81!2IBZ!7sZkP?Fntu~?jfr8spadop&mj3U&Np9`GXY=awA4A2X*r7E1I@>hd z_dR`NNMYLBw{xKg4;`si?$&j|Gv@w1D|-DV9f1uzl2$PTE@1rbmHqUDimxpdj%=&; z?b^4u1}rtqZRWT4sO_f0huE+|%^28kihLsW>#|0>PuLDz!UY}cR3$RfvKOho$BtbR z#o^e2lt*B4GL1V1<&-$(dyo44cvxoc-8c@++{W7Uyu7nra|bhe^JQQcM&d1ILNXQx zA)B3lvm%m|a%PWf-fJLvAMrXKWo91UGvAy6*qWGYumEhRwq7FbzDyJRC^gl6pu}~4 zyH1^MrEf)~`*@6;tJP(o{pm5k9w?aU3~S}~b%u<@b&m_$y#M9rZX+P#&$^cSW3EI-%5|X zL1h<47WMBqI+jvj1f`M6^|U`vmkpG}dO(a!eAb;$c}&~>Uq?gzu6%zJ^F3~{W~~ez zAarp6ua6%^K1LC;{$wD_N)CQ4dlQ|}*JO! zhf`+qxqw7Q$|L&S1~ao@DkPJC2`ScEwQ3HLVa1N;vH(g6!Dl;l=n(xoVd#}!sn?j< zMGdE5E6q=-6stlm^P4n*U=a-t!QA2KE%fbIu>aF-XCt}k_ZN95Q9X+I0 zW(gD3ltvGVRog&D0!E&0f1%F-GfpqxIS#OnG3jYMa5Q?ZZ_cF><(LpK5@HAG=se4f zA^p@W=p|XJ%4Xw;_^LTUx(jxe8$P}Dzn8ygnP=A_Gun2+gzr*IMGqS0WNE3N*+dOM zm5}cmYH9|+?7GJiF^6_9DIgv{j4HcRtWD-w&eJ#A$aTA>>+ndlV_1PMQadhFx-OhL z$!H^NmdzATvJa;pCHc^z9Q#)K_EzTC!zrFyLcu2yFm9ceNDmgwOfjQZDwl4}hchp$ zJ#)gW+`HzKK^+-{x*GDL5P=os=U*U~U*Rb_{?3|ONv&-qZ$61k^e}3AhmQLjepOYy z0{B^-@SZw5lpD43byP4oLMRk5Liw+KlE^_bT|V6Q;a*sdl!SaKEnWL1?v`Fu%A~om zN#!Plfwz^^4WY2D9NEO?f9A~ezS=kmEjHTN-M5Y6HSmuWWT<0HvAD=U8lh6q=s|3o z8+d!givga7yE#qIZL9e0{wGcpM!v`VaKVuH-@ZO2^!jP6P-)RwC+*AeLStiNqjgL7 z=Cg+QCHT#9wK}YFM{fPKyWbtGyS*Jwscde0u8`T7#gN4MsNevozIszK*Re#?oG*5B z_$Cw}gdh2W-M>EtD)!K0 zdN`0$-8}x}IP|vpF5>$RpEm)VfbZ0^BYXu<|90Gs_G*$G_@5Dc+HUDfpFMse)3Rl^ z4C(?EnG2J6$;)rDqAA=(ib^pXl8H1_spo^ z5G$&vlvG4qAu25A6`UNqoWQh- zUW6Q`-?{|8e+`ob<~_?6sY>Dm@A*P4X?)n9hBgw#5q`xtOrG!UG?8`YeLRhV%5yeI zCI#U>WDW`C875d7_YIYXEO;j^1p-&^s;*Ci5?>IY^=11K&ZC2?#M2be?RUwI%pMs{ zIf;!0k?(i%p(yEZjlU(zw}S`2d~DJFS9Ns`>D9ZSH+&@D@>AS*^}c=YNvm3P8b9e? zqJoObRXh&+y#}TF_IYqpTaf$Nf$X}^c=k&fIfZa^9Gp4g_7TaVP8b1%@|{ktE`|y- za-&sWNgcbX=(JjWr{>5abqwx#EUo&a#9W7|VB8Ox&h+6fbG>@~?|n-iFP5r0CznGY zyDt?qC6I;u&CUS{OMfDt-xNE;^uybv=J8djUmSb#wJ$A_X!k=002pNy#ZK)RvIzp_ zQ_e_O|EW1|mdm+p$5*l)mA~DMR4BB|jc0&`tt+ku`5xSGFb31XQ(Mqw44w81GW-Al zpyN9S6J1O&NHC|wq=3o3FK>VPKU@IfI{9J-75M4pl6DK19jZxaUloCH{G^VZzE4>y zl#1-zo0g%p5h?E^tt-W0x|5T{kREvsA%EzxJc<&NRv)wZ1WEvQT21swz93K0MXJ!Y^#tzZ5!E4hBWJ`lHdDF5*WiCx| z*GpzC*kQ&5+L277)$g^ybl7<}=BGqiXUxSNa6=Uj=AG!4RC$GgfYr}3=xCRr zL>WaT+& zpQncxC7p0~dvaE9A^nlt)75O?OKCo-Su`X{;`Zmk4NsSyb$YELOLWU~<$%LdZ)+duR9qgmcL zyK1}X)uG$_Q4d89&-`+}sj863DbV3wP9;O%Z%FOw_;Gh@v7Ts#mTO-qBqMy^GS36C zvxxL62DzR8Ba^^7oYhkT%d`U6%*LDmT+xY}?gtC}Dr+PHhf79&B2Cc7bjwHtS=8w6 zYW3^)?`&H4#afAwK>gcK(@~R?ED9jm(~*O<+4wh`#@MLg6o$Cz#gjCP4y9F?NVW>@bM^Gs0+zUN(|Mn}d%fU==C zlWp9a$cKFxbuA&O$D}MaZ@x%utQ<0JSU*+OgYOUKYni?aU9+-pvvZ*i_Vy?#EgpU_ zYs?ZTe$49Go~`GZQX*s{`UB&bws2>;8!><5+%e1w%~{pz#%tfPeS0BUa8^@eL%ZL< zFq64MJT%zw@0ZWQ5RKgvKPACLJP(n$NBSWTj}33&C=@T%!Z@pvvUh9fL^1pCcbmHE zI5%U6^|(z?WtP0A>$%94zyIagYtiBVoQ`&uP3v?7AYtm=WImUbMNnli+0mozU2LaAfEAS zyQl?mZI~~XtyYiR75<{`BRXd@<74-t2St-L6CF!$e-mSsj$@OI1WSti(C;;c z&kIlojqZ0g7I-pa;Vj_94vKc}i=J?xyI#Wil5KT5jc=mWd;BYA`pokzOBbqJT{>70 zK+U4qE(DC^!tn);#vp7_=~ZG`$%y68Tt7GK`W1`^wqW7Hra#BUtDz5`260B_&wFxB z7?;DpjpSA+@Qfq7I@dZ5`$HW3ox9|4`uj5(j?Mo2`jZw6&CZ-dQRpu@CE9+UcO)Wq z+-9$ELZI%GRWF7dc6Z>6vs}&DL9OoTxUw&XE8R`QbC8fty(gLXkTTNS;kdfRo;~f7 zvQH`xoN@ZxJjGn+>h}bC@PO4(Wx&wUYWOqVF;DCba~XboMz0(io7EL#RuXHD6)@l( z(~w+`9mK0u5%HG@-rs$u_OieQZ(!%7{h-9F)oyVO{o5HoM-HIaty^K|x|8%zIG=|S zD?L3uEzQl3h*JTSwgUGS3W(#dpKbT{a_uit&i8L;7BYD`v$0w1K5&BfE=>)`zA@ZC zK0;?v_nBoaCEbKqFD7#<@z}k)VZ$V%JGOqW827l;%w7FWD;EBIabQeKas|?T)Pk1A$0TvWzw8NJl{p>~KuU5LF-uH}L?tvmW)vb}{v7Ym z&7{6`;)LD~FGDS+AhCQ#C_|xg5xBjWB3ew5*vqcet5-Ofi<1hlE_W%D=(Oh4i|+VA(#$~SWGk;y=EJ=#tUk} zYt$gGrHznXUnaV^)YjDGI(OyUJ}m9`Wui@;{l@r?`_-DS?@!dwIrikpgFWot)$u*$ z>#KiTT}PpM8RYnd++$1Fuim9`O84Jm5F_%5zJoB#)7E|(_2~#>kKmJ|)~&r6hqw+G?d!KQd@HTBV0Dd zqARI_k9}oJaJ+xJ43dlM3oj23AAC&a|FBxHAdpz~?f3gmVvESMW7bsNwU(FL2-F6) z(xbGrXJUc?KYu>uzKEgNYr52Zqs%RYe)fmP!2je)lMSw3H!9lQTS%cp&^pnr&w9*1 zX*iB*&!2-b>luozgI|WsxhbVf?pXfeCxge4zHd=Awz@1a6dpqlqcTsAx&LI@GoFXH z1MEespBv}`N z&f2IC#7q5etN_mS{j>|d#IXr60$j&7$*A?Vo)dTGKEEpx_KfN`vHY}<9mw@FQt#Jq zub+(hK6WqCn%OQ>Jah%rPyoG^D5=M$6nRZFArVgnD*}n57r1!`O9JcHbw_o(y zZ@0AOlsaPC;cefk$8ae?qsnquZ_1u0wp&^BOH`hmuKl#gYom*jqT)qjyT#8ACmerJ z&w$d2S@BKgOnSC1`bU1Rd#hfUlrlfFpYBLMtY2x z?1*{Z-vO#bvS=W8^^%wxwEW*8lDK+ffToj45=PB~D z)$=M(Pjr4GT}uJgy+|f0_(YH4Dhdky%*>8u4?e)tR8F4`>IcY^V{~B>AV|1qoM#Ag0j`>m^=U-al-r|?gDLm~OZ_b$psO8&>TE;RGUzK(g@Ll5@Ua}oF` z3pV*mN|LzO-VVRsciNSSsSo_in@rY!>)n?*lfqQ+CW*Z1&$hvHZkp4g+TiLzn*1If zSL%zPpmj^vJSq-D@gFVNRqHUV0&>8lFv&auoj|=1+^!L=aR7r;{|Zkl*z3WoB56Ej%DYbJweb3(xq!bSi7~X?D}_NR!4vX zcsnhglvqtXZ5Y;q=L8N}IvtsB7;ycu_lthjeFIjBQf%)YgD9I^^nU5vI>vzq6W0B2 zM*PpoD4C6!!vd!tBl^8&nhf3H)QfF}#l=?!25uL7QFMLv1UO!myJV|NViF(wphI0< z-LJ*bFMheVg-(8coz>1-}cENK_Vj$oJpOVCypR{!UY4-3TOD4IE&>ESXsQ zMWK?InO2QgPlEO@x3@opj{-`%1kn1y^Y2)5CSJVXmUoS=PlFf=LM$*#_OqX7QaiMd zoT~-W{+r`p%+}V_dGg>0{0oEmw>P?Nv_Sc`w&;D_t?TumB*m22TkC3m{rWY6H8=fU z^IQEQB8If?Zr;*#K@A2K`uBET%Atbuw$DL6xMy9(n~v?<5BGJh`DsVFETnw}0X%__ z+~$|fgZ2~zP%|jDbB_UE6Ul*B55-l`*F9VRD;n^(t{HFQoo-zpu?|>q-lbGA&V90N zU-V{H+VYxfYJM(`p2GH^k-iyw$|Uo=!=ev&8*a6pTH#?74s;sbWM(oM%^0TFShHkF z>L&u%Z;D~XvCAkKk9Ab=_VkR>Dkd@vkLi_58}*y6i;SeKJctpVyGu_5sD%`@c9T>W zxtk_LXv~gM`+sD;30Thg+x~wSYLLuWD?1H_$WnsMD ze)Qzw@HE z6mA9mAv`SK+m=#u!P2gUbULh_b)2jc0p^PT^sk~+4KX)2Z?Hcd1aM4wcYF<~Gy;pN@E$h(ZUoPC zY$efP@wdzb0`;Sdi|l*Myc*n8nKyfS-mFhm8m(<%2t@x)$d2`BNq1_FHfajNqBsQD zu>J*~RSMQpg_qHiHScE6ATO7>t%I_gH4M@pUrP5~F;JY|JRwO@pv>^X6v$Qpk} zR1lf$?AaL{?2=w3qbXwZN*KRf_<1|z87-yYy>KO`8Dj5msFZb#s^0bhW=Hi&ahn&r zJ|_*DF0Qz)zR-foxvX%6bdG4~^xr4lRl!8`Q|%rQ@xt$hI$ey6i)slHWCgA1#4zA|dh z8x1UGf871s^EM(hYBt~q?d{1)M>ZU*6s-r8!r!TyJrSd+iv!D-7bGZ+`P_QtiYb)k zQr%JEF6sDf9cA14KmR;Ije+zioOzZWFdraK^8w)~B#&!nspVH4Jn`tv%$c0R46m_?Wn!GNfT5J)zumcs=Mnv}ANR#g)83^_6#D(id0%s1O!EJ^rdK${5P( zlk#$>o(@Le(TSH!tQyT6KsW2?qfmDpIB2}MX|zCDGYN+k=8mVxsVd3M7+nVwj~c+(}__^VY4&e>NR-CLux+rMRHJJg_>|tQ!1x z?($(x`XS?lz)x9%&!`vO`?hrry3Xcn_X*s;YuE2Qg3<1iuvh38I2~DGgPzwi-O)M< zFu|)ve*4SshMMRR9-ziP$;>sbBOp{_3(YUtTCeH#*JzAz7zmQZ|+5LUxjfg%`x4%=|>&?C)9zoWDdx?9= zm5E%(PR5K_bYtqA;`nUgz&LdMW6H}Y8*CW*a9Al4x?^c)A_M|K2&`r3cJTVuxj+Px z>-Z9Hw;H{l$VjaPI4T;ul>Gde^xiv}q4)6c_~z2k_ttWgu1*ejc0NE+)*Sa=DyH6D zzb@w$h10HghqooST1wqp&uuX<=p#BQfRRZB`Jhr;+OB%l;Z7aJC~Ei&x|$=5aVvX| zG{IDnI6kTEmPN;v6>bZby?OM2imQwn-W->Gast?c_ilXiNdeWK?#>+d||;1_4aC*>PgHgN&k3N zm=F5t6;8{Ob^)@DR|tZiNicusUV}}gpTiQ}OI1*n`ho9=1b~908K`-g-f`~YuG^Ap z=3eVB;5|Cv^~d&tjt4gWE&6%heQ-Eh%@nh3ppSt)ZE64*tuJ|lj!ngY-C-rs>B6wK zrSedQshQykEaM^q{zqV>B+Q!N`8c-EM~Wcao<0*NWo#Wo9%TtE)Qs>7<I}{$`@oCf=uk(Z&@@^C7f@@vDs1$|p3DQBJK?BPO8F#+oT0>p-E00mnpH9C4 z&vcCE-<88kh)_{&rtmFRdpY-qZcpv&?@pp3Lto{j80s%t8he-&n{3;Jxszo z3s9Mp#q>i<(H2^*D(E94P54<<2>9l%KIV#OBvir(N^r4%Uz9ZWg}Q z`qOeyy8tMWWxhs_<130=-Aqb)lIuk=Bnw8rGfyC0r;vISrK`Zw$(trI^V3a1(54W9 zT+R>bZKqpXORfIh@HheBP3+49lZOsn+t%ybtOgb9M2Jjn$Zc=wpcwytjcVKbBxD;+^gUM&u*EeWRrwksfSfAK+kY*i)c2#BL zyC^-ys-=007IQsTQ#CLGz5n{xU7)^Xk>TicEI^6geODf%xY%4xY0-FY&yl)M#SjPn z_r6v2KG&;ZkV}_Wgv){Te+q_1NC{b2KeA3|)v)PTJ@&4hZsp{3(*LP&&kL1ycAa$1 z>MLzRR}D8i%e+68D)B!UQ$KPwus?GW=|rdn(Qz0OiIAmy+fO$!!5hzyaSSMi{-UbK ztOyr9yXY$7{#Ge-Z||u3h*HnS2fa{7ZOY~GW+y6?C~iGMC}#`30UuSwOH z9S+taJOa-7yUaZz@wB?=;C5R|-9yMzZO*$2a3RrWy42Sv?0?WrO4s;qt~b(>6P(hw zlr2v%8SgxH&p}Fh36hd(7;|w%(@VwwRxlDRJI4?@Im#k9sO9z($jvTv~QYwCzhHCMn#O(hjAQxk7npCt-8q zt179Hkm!tCMF%AkpiOHl(sHLL6yM^YxE#sULhfFTK!ua{ti{PxD2#sj9!DTePEevr z4SrCmU&F?m{O#a@ zN?EqDSQr`W$CL#P_y5+SMJQ@=&3CK_9>dUnV38+?T%ZG0L27h3|BBHgS~&kh==05~ z&qrKpM_|iDHZ2qH5)0`WcR^jT5^U}cXu4t}m((DTsgEZZ}K9*iAQy~sI3kzp4MRw~%3p{!i6 z4F^2G>JDFyY#Y?=Q_&aCkpI&H+;S|sX&v7W^TZ9!6z(?)+v_hJv}8JkgAG<6{w1P7 za#O-!g_+!^W?xa=%k2YEKUh~`;P6&yBdD+hC%=5{pP&PUVG#P)(QbAG+w&<^vk!S> zPChiW-v1S+5oK;H6}Z!*{SN2Zy<8ezI6{FemG7~=7%&f4oKGJ= z_Re#dGG(U&b06c*&WX(Dlbn;GpRZK^tJIfy#4|Y(IH=lIdIXBaH`EByre{EY5*;_~ zP@g!sv#$xjVYu)MXagLXi5P%2g(#U2hivlK!;B7D#Od3<&FVxpqIgwVr8Kxh+L6vC zHHv=Mwhvb%gvg8SQSsvGJ=|er3dB8k1f$CW8gLdwtJu1=B3P?VUB!>R(iip(G-g(s zX@C>A+uD-UM1P}QiBjTufl`SX*1KEirXMWZ>+wGJ_PGM7*ON37q82^qsIcv}NogY! z^`e^V+U}=Y@NGXr)41yIZ<{?!kPVguH8MqQC@U5g=*^%B@2A(52B5zEt8mXNnRdMw zJUz`NwmPs9PnV(daP2Xx)f0^8VJ>)R50VdY+#^|d6HZ;j{YzNhGNzq9eP!$7i~KJa zB&2+PMa&y@Ytp7fV(8&1gST?`eGkNL6eKyTO@?A zQ2KGLzQ>#RGU96%)(NNSz*3e##>g6_B_BAt6slHVMMji0V(%5rgh%(C^?_LL8n4UWbkLv5Ty}QJlF`U+99B}ftxshH z|GbMgCvJ|*ir5U1VoFO<{`#pp&x6hW*0E|Cfs`oI1^o20hD!DDww=EYbyKm^Qu~%ZDpKoW^JxdM91}{GU z=2PR3Ua!7lOLtBr--jgg+y7KX-m^i)b{5nP)UiIybkOA|U3QfJkPO}>SADNmuf?zK zcM!;7Y;HqVn=ZQ3;~)pvK0Ex(x*Q8Y@07k5a4tBJ52Bw=5#1oJ9g^kUv=#R*Z4rdf z8T-2R)gIBTW=;8eoG+!mWGqQ9J2Zck8te)5uK=avw)IQcyVqmp^#p?~+*K|>M1~>c zk|#%Q+bOeH)W=h05NK~(Zel1p@SOKoXR7cJr?mkHRCtGhbR?<%?!|v{2}B@wU@>1t z)NM4+^lH!BU)+dHfiP`ew?JE>swvd+!CGY*AA&T!?_RrfN_;8!JC$yu+`TrWPL}45 zVNJ%V-&yC`x59Hnvi;-iW+HeK6N@5c^(_tmRNBL2d?;l=_R_)687hmQn)dw}?(s2M zgyeJF{sFr>1}6X;9+a7hj6r)_2dseQJh0SgO7xsjonpxW7R;E|tK+5_s?u*k?xkIa zTGm_2RE@94wz@Bi|526%RnRUsciR6KKSXHO^?BQPAWzPvz+I<2uEVHwCfC@tvi$7X zvoY*QNNH47J97s1g+2pH~{0w$HZ_iU#l>8!(yG7f;=gOVqfafki7Nq@U}7d%A@+wMb~K?AzjGGu1|R^Tw(N!!~KvyWDk0 zT3qtx;^2>-o9a)S_KTqb4NI@rNOT#Z3=xUaj(37%L99`a9rTc|@~=MyDgZP?>W`lT zed-=#Lb&J`=t;?Nz|Dv$qj}o>5ZYI^RJynm&{fMQBzlHrVzANjRI`Pk&sHs!)-Ogr zYJFfOvq?GCxp+DZbv4n}zEQlciZLa&)X%fdBXTXQT+WK_(=t+;x6SCzuL)G+#NE7a z?$j1#^tC5ZkOX@x)gdmxml-QchA@ZJ3>5-KCyvZ$z#%_G<=+gBgn_0opv>`QD-ipH zLRuYI!PiL`|6xH3=0g5C;tRNNSq4byM{;Fj%-YQZ>_Z%TYREd3{x?61v_@1gsG9L# z5xkFjLkdi`-JgV;{09;`1P!H8)u*k`3!EU1)>ZTql*c|>GCr2Fhs%BO9yaSPp`jS2VO#Y(Dye+MV=*#;(ZO;_3|`UM7K zXWaR@9$6?3M!?#kzjW!tj0wr--op1WpF!ONJ=sIR>uL7uuXZ{{efy@r>RnIq^~R&^ z!(ZoLp9_>VB)OklzfQ5pg?4Tp+PnAI4`i!JJcxyF`xYVAY#3LCpoFoATgB5&(b@)< zCAD&O#3zU^Uf84Ny{JucwxORhywN|;GJho0urteKfK^qxG)~x_)4w$LPNii+9(2_S z5(rgg5F+D2wR9l3w9HS+U4^(1SswYfc>6Y8YCf4uiqan`*zi*=OKTOy)18Dq?8}{& znmw0EMbc^WZ2P-uX_*9#f{As+y;mVcrA_#K^v+%b>elsHG8-SW zcoxPV6e3@y%0I>9e73LAC{Xt!BoCc5&*vMvARoJ%sC-dFWRPx?DC<|PSV6TYl}NJw z#~DgK@1`+2)I?z=xVW@v=;K_X<7TRA-oUuPkAXnxoLEKpL2U0uz^&n}Mo|#d+kS(834>g2 z+ZFD23g?ZVT<3{h4Viw6!{^wm1+pyQ~$ngcj6+a2ddbEGf(jYQ10AwE7nssvPNlADq&nncw9tzq{( zdxQ;^uINmku#EQc!Ve=E9;fefsI~F*HQb8A z%R$csO+HZX9EiU<9INQm?ya-WtZVAsCW!eZbCGrOL8L(u*q@3h=UsK$p58~yGUMdZ ze{cy~{hVbox=T~c?5x-+#A&>G?Ki^pPqX&A<*@3*xckc zwC39_6Et)VEi0eDf6ug`jb`$U*u}@9Dhu5z0of0<@2^n@yQVa3@jzP)fSW6rmu zfW7?wQc8!d&j4|5I93DjS%686Z{gF3*wmHT7c;Rd>#8xnsC-@1ky|tlp^vo@eI6&} z3(B2t0|q=<_;neXUmoqshkQYwj>V&777WwI3?x;0^hEYKSm{|%dMx;U5lR$MUO~sw zevBSc`AwxMv4A!f(r2CeVE+%FKlcr|jFMsfh7IO!zs0`TYSdLJa1pY7yc*8&zjvyT zp(aAz7WMstWcxoI+0OIPE3Z0TTT^;z>b<)((Dl*N5lxzzDF#dp^aymvo)pAk|cW;W}re>Q*$b^gj? zhHINQX<}XwGGWY^bX4;V;&KtuHlM$nIb_Bg&durN1PJE!{dE`Ch&B7RT~XaAte zlCz>Lmy~V4*}YCeM6YE-tl@V>M1bNzT%J%I3FW%do!w9P>3fc8IWl(>lNqmMY`-WL z;6QD(Qb};~nk8zZmQGh2b*?dKoem;^v_AJus4r0TlP{}X6eFC$5qSwaI4A&ooHGf> z4X}Rxx$-*bgSqV=i5nwk#64NKLG<-Et=w72Fs#YDvDB-64$RtZmK(z@rAEJYd-ZvG zEb|ZJM9b|6oJ5kf09LAFekhAS_$s?Zx$9pEs8Ul>UwLKK?zo`}?pa zzt5K#c20fIuDAYl1p{t;bYAr;o5WG{{(b63<;X*}n$fGfhY#piXX`brJW!Qb0N-aI zAe3?ybFupj@rFkz_pRP37YSOP&E8BbX4~3~MX`AHK zY5G~s{yVu5jva@lK3{xtO#kNHjtBSbINa>=ouci*fg?61_hN^_@y&ksw#J*BKJ7Jq zdjF!z>bC95b0TsgtkN=He*S)?JiELy_vPo6{eJ@=Y1D4eQ>$uD;T8vpjArk#hn{lI zaZ(FayIag|h*aRgvfkPc1`(Gul6y?M*y~=s)_cN2(n3|vDo2{&IhEOT!oXQEN*kHF zAPF?DLpSiBKR&BjL3WxtX1A$f6!+f?KyJABdG2zRTvhz-eC(9O!$No~!b7HX7UJ zf0*AWt~yufQ$ZmxffWlE-=4WpsE7oE?0@6CF-o5Y<=3_i`rfrm{e(MXjaM5D8dQBC zHO!~4L?P5;Xn_s_9l?lPH3y}QxY{PL^_@n1m4dvL_mve1xl;c=_(51X^==HeSxVKi zw&NkW8810ZFu%T5*>3mPMoQ1FO-rbtIBb=Iqufbh29f7)E)Jzk!8_CRdX!*-L zET#t8r-fT1iDDVE5^V7Nx34L2X}_#H;|Emh5`Z_j*7zr~yix)>{yb-OoRM7;O>qC|8GNoTCoa^B5U|K@5xV}JJWr*~=5X8Zn8+g`n zF7eDiQ$~v|@FKQu=Cgkcwm^aR+UsXJEH>*n?Be@8nBg{Ws(w?v?P?Wy(EllnaZp3U zC;=0!&ze$r#@O7WhOhNqX&3m@nO7K|0=DQBE75{v9_#&~XFXL_DNaPE8z4pG2_~bt zU}s-@1zmZ+nlT1pc-WHuYAt!F0N+^51BygR&`$!5UjsN36zk3!pCIla-SfpDTX^BQ0M zaIK+f$jjM(;H-hy>R$g6kgXHQ+k#swT3y^1(#HESV=OXi6QuUzQ_wL^lTF;J7{N&S zIa+5bm;@Eyjc(>gf2r-zB9%`6N%x|U^~#Y71k0i#`B;Vd_!f@8W6V`g4h}y-pWt9v zcXG>D=#SjSqsswYWxrbP#YNX=gT$_s=@?8=#|tQmC2L{ppre|A{CVNpW_qHI&Bo z6^+Gg2g=yKf`0uW6{XA75V#e_KmYPC0ql5%q(`T-k^SNmnM{~YSSJPA^|?4Vj9=&a zj8-ve!=|ZhY*D#B8%azAqVCOeMV7I=Y+0a&_5da}FKeC0vv^@tIM1eGzEeCPWI zl^R_gNn|w$s=3!MKg*+CF0`0n1{G>6qfL++^bU-J1whUBaE_v9B8miBGt+2%^+xiv z8H!?tS-r`Iw-Tp;opgdK=+K^ibQTmA{IBw=v$(~b{;mn>1i8L>4r=c1y?cM;nKN_! z;N9}NwWAc%r36;<@e}0E>MO8`W3LaY!qCAkNEe}A)wVw0A>kbw9)(-EiQdy(nN&NX zFHChMm%UEEWmOHKvkNVrf4GE! z91P06o9kF@zo{_v@veG1({wQgh!z*!W@(jN6S}K#2CZOm-g<71MvB2oxP#AQ8}+UR zZYK@Q!S+$e&a(HpIZpb}_@}5FPqBJhkmLXSDVT5@jDX|mHwT+SxzBnPZHYS=DJ58O zF+T6&+nixo|xEyW07k1m9AD}!0LuF za~N1fC6#VxHHAAe;xhx4n~ertxZb4P520{Q?qDOnI#sxpjw^-Uau4r?VTVk4M6d}v zhlOX;S!$;A#Js9Yy#&+0Czf5bR}+b^EUdRsHj)6d20YMW5GqvWA$WIjwD8^^2bbIgg{ z1ICWG6uBZ!WRM`L!VAaC2UcI@eC*nFb9Qtmz#DTelV4FO<0YNJday1cUte1tX8{9m-@aIse>^laZ_)PW zx#HpQG`G!*MX%9)>IX7HvuJz+A3qU~Bm6LpX3vY5a|NkAEv`Jn`#nh=5T(VtJM!-r zFt#T8#+U6kput<+!EB||0_J)$5AoSg4_l!q@4%#!)%o9jM_t4a_?IIUff}OVnkTp!$} zT435mxDZ78>N4-03aVl`w{(0kA?y?iX3~jaPM4g-E=tuAVmNf5hNa1EB@PhwS z$|!KHbi0I6VuNJ$l_tY;X3ILc`1~SUyXwCHVo_7Io!hTqFQOOlcgd?G10)DjU9yCl z(ehBGyjpy*e54{=8fB}m@eJ4!$uA=E83h&a5SoSq4NCj{?aZ1xexrVR#tEJ>30P~w zAj7=k=`gN(hm3g=0qrYj@Y4zEE|<26V;1tMs2@m>q6sPV%YF{Jj8;hUqQvZqjxVYN zzlH#nnpC!vIqK{?^t{*2hiOrla<`X>rHQ_N+0{>kL_c+(xOV?kEmnCD2@5Nh2e$rZ zfe)as^%*qk?ib73DO_qi*#W;$OYP|p$zmli)SyZC=kWOxRD@H%*abq~^mD^zUF~#o z3U%N9E=oA`M(UgEwje4`X!`21Me`mukE{$}d0)zO4vJo6*`OKIracX*uJNW^$(dnS zHZSheo$d%bl;uH-LS@^wegTkA^R-HzkG;CVVA+uHzWr;l03r;JHnQW0O?WTeH9McI z6rs4wXzxTg5ClG*Pi0J_wxP;8%0isQKRs!MPbIDxD`u9rt95=VdY^hEyE+_{Gz@)V z(kF^Lp%3?!zGQ}GfzE1RnM>@3+nl_0%87|Xm<38|=-ubq|7ig}G23kvB=BlCi#u77 zo8@Q`ZS*SoE$r9&y;tvqhM`DIt{+}0l@C#_`a`MC$FV1=44#xN`9r7w{iR5!?%cim z<9d80b-0fC5mZ3Z`dkmsv@Lt##k`_hAB9UeGLWQb69#i%q6TBfB}}LDph4N3Ip3hNaSrIy$X=yoW8$lZow2vhxv;o=H0uf6MP2}tN zH2OPndNCP&q=GfkdiXi)I|cZuk@FtU_uFvP^^`Y56CAH^P;h5h6Lf9B(w#bGgCfsf zq%^*Snxmh4-&?X#3`N(+12yrKrmqm#UikJ^RNhE8pB=2DUPw1cPrj?$h;`Gaq0IGj z2r=tms0YEb~tLVdcV z{O>oxKSyqd6N4wsFmp%n&8Uy#C(n~sQHSv+^(%rhn?YoYem#hVVch^Tt*U_vZ}gp4 zau;Ww`8unq&Ish~#;FuE8LNJD2AG72iT6joLU(U-L#-r_6kB=R% z;^Wu6>%_runFfLJ7+jK4*CeOSz!isYlBFW-=<*)0R@jmD4s0jRySwwKH_~Be{T5tE z-keqNWZFurcAgj7Om0~p*9yOz^$ep(w9+jmeWEm2omP`ZySK$9#Yd^mMXRQ6y~HJF zG3M!h>bKX~uKmFzZ6a>FJvzjV()H^ae zDc*+wFRn2`9MC7ROO8ViCj8vNDs)~E`zNAF^)Epz_t37P>=c7t@p~-b`M7dXT*039 z4KxvbjPa_SH|LBLG*qvKdDu`aMTg@0vL$z)317Uj?W$rmgwSOqIn9(ajC?fIG?l9T zu*fTFKF>qCRleT{nXvJPggWl|X(#|UADHKDqVWONsYMF=x(4Pe0=QW7mF-jk_Z@Ur zBWFo3fa+<=!ZFK&E#785PMfXZ~ z)KJZBHl<`zbiT*uabnGPI(`t{7W&!fZ|-=fcI()2?Yc&Wdzg>7Q|nnFqeh%`9K*_x zxN0OR4=>wi_bD~@=0dxdOAIyW3K`QGLX~MFX^wYck$B25|Vmn zDpP7dQ<{p%NSXxuZ0f#ai7lF-#MGBSmeWpm@gnS_HIN`Rb6blt(0A zELYKjI_fEozeIxz{M;%7Z`)^+U0Q=@KiU@w47 zLF#E<^y754O6h48*}jp4Q5T;M*o$6vr>Hto(Yf6q*H0~t0U+!bC1`BWmfZlS$CSp0 zDJciwwDd;{je;8@X2q@o!(61NrQe;Qi;gkayxH|S>cd+9RD*)S(%pd(Y9&)(wP>_K z8(R8?*?S@}R6kI#C>I-P*+uusMw!}vxEv55!+5lZ^Rj_z;<=btQXcWO%5VdNtA6<8fbRt$O` zZwzQ|`dMb-B**@Z@-ey$8d?0Xs_IjUuAc*j?OD$@>`CmMKjvFyrLB%GCku0g>azG9 z0RceoZZr4N;L0@4urKLsFJ@SxMM@3~=b4%VyLTTaC);RWph#E0V7a;3AG5WWQ$tXS z6_Wt6Fl#vsC1zhi5%HR%-1!HA9UZrK;Vh zoEW^W!zdZVh$_jS;o?{0jta;=n~Qxo(WfbN*`BB&=jmFHCi=9I*eHgXWF4!BxwMn| z_+iZ$`Jac*UpAEF#pPJJ6N4Nh>q*K-&ur=}uATGI)I>|G8>N+f z1sJ`Ea^`d3TefcP@<7Qkeiw+C=qY>V{a`kCo|oCrp53fjvl~NH zT94&&MwOs4d1z-AKqlPc(2jOjCI1@QR=+}n`44X#>S?Cx=8S=>@Sgr9fLU*(-qVa6 zhR%5+Rh}3_T16BC?91RLS9{yfqIiXdSdvkiXPKQRmEayWZbuItx+lkK%2k)@ADc+q zM=A)955e(o&5mvg>z6K)_j5_$GmIjQJF8@<|Nqko5QIxBJ~`|zc_)2tVBlPBBQRpB zkU2`%yXo|QG~wZywD3F0$qxu5rAE3GLWll^-e_z8`0kR){j(`CUI{Qita@PIBGk;q z!xe_l(vIX;B2}H=$E{(F<{g*$+$1^pM4>}=8bitH@v$K}|S^MNBworpyiQftmhNSu)wVHI`F%$?Rv^YDh5 z@NRdkzJh-`erU&jsdT=Y-yE8)?O+5q3Wqh2C0|6Kz0{E>ASJ}&*yf&45zJYbKfAX6 ze)ghfKVsB@_dO~t<0w{xWs_ul!;Udeven+WwR5r%e9x*38(=E zgvMCk=D0Yzf5reJl!LTnaH(ixsBSITQTrxJV_hHSM`G$V0t*E>sj;LB^#E zHqwdyChh9G^>0YSGUj{p?S}R1G4k2DuA$)`lmeQqg0F4|9ySajxKkwy{#?oLXb5Q(Jz*)u9UM(dU1bKHIO9~)c9ZS zbU*Wo?(6x!H*J}Q(@JiH9Z)Q<^dJ0gs%?nXG8;iPJA#iSom~BN_^jCoumNfoZL*T z*ZMAjXr9i#4o=_Vpe3lg#*a4`@az3a6#pq!?j$HGu;KDtYOAtp)QL-By@J%TtVF{- zcJJYy(=S=Yyhq6+*1WNomt@!Dga7-FojI~_v5x#ZcsV^fn7m<+*Q+mdtA2xQUoHI8 z*S9;pzM~&1GHYtwSwpH4V`uTEWwI&b`wJB6K=~RG{=vcFJlGE|fhJ;26Nf*h*1#3L zxS_$9FJBh>*vET76hWD%9}Yo{@K}C9QDgQVY%~6<(XgeZ{3OnUwccms(=9Xy<#E`2 zOXdsllXsyc_wrPcQ1TGw_+J5THJmd6s&v+uxc5Bg>7ObA3aiBIQ`AV~H$*lJRgK-M zr9YYq=E#_`FC;Y)?8DN$>Qi|I$*UM7BT}}&`6J(IdE%q{m5io85E)d0=?a^6T-4T> z!8m$JEFe2}%0@P0H&MNs0gZICo-MmS2`xQ;Rq>=Px7d_r9K8eDv}@E^t)HOZOuu!? zhE$jJ2jf(N|6(!?#kPY^4Lr%wWS>U3EsPM3kGvCZDOUNg9R}aCf3CEBYIg70a{|d$ zr)j~KeY1IyLStAtyw_re^QoO9sWLoCP?O2<;cXj zV?nc}u|kC3=p3%CsRo)jEh|M)w(y)C^y<1bkqKPE^^4MQCkyeej!iHBi0NdK>UhD; zb*!xRA4_XYaLS;qdvDYQ)}=!wnM`U+H5H#3qN;utl7EJ(Npz#Frz9k{ z?!S0AAHGk3l*p$h9vZ5rS#u18D=C^<`|jJ9`G%MaAOqeqr~K7}b)~Ke_36LUtiCex zFBU)C+rhj9DKD8rpu9wy!err`FU7Y1zOwSqHSff4B(Unyc`k`p6%H)r`?ON_GqURDO_{@%zQ zx|A|}N!9@X`y30C-(SO>EzX+f!6VaX^Q4-=z1SP~^St@@}>Bb~*a^Y}%j{rFyOJ!whY4AfAV)yzt(sWBoAeW*|L00{g?s@vnXVpo%|8 zhfvEZlpvWvdAzrC9ZPp?253O1;dcxg_i&8vYw2|D?Arf)RTYr|0s+--^8q>pQ6IXa z(JAY*p)vwh#J2GqQHcXWAb1s8ZpJ*=&{g=QQzpF*aDGj>_!x`8snnczU-de1nT13V z2;*NYV@9TVyLP1kw~V@U8A+)%37vsH9Q625pB;US9S!N#aH4p?vkN}GTy%|W|B)nc zj?C*hg-q3=d-!Qyuc*3CEpU2zF=|-3-roWU_n2{C&l`IemtFcpmS=qTXuev^&INnv z+V%Ep)f`5rY)?*Vy%09l< z6{rnj>M#0DTIcjl8)&bS^W|mVtsT)ZWsSQRFolLGNX<~JP+v2hkNvTXWJ6gvCXALj z4KgdEbBWM6C|e=XXQE3k;7k1XkL4Fxgi?#kFkrB+X+`Y-Y_5^ejbIdl9n zD?1A@4hDGoysSS`&z+o})}Bpw7t;r|{LEmeh}w`uC)IE4uFKFhI6FX0-<_^ zTvzzXI1dUkYIB~*DSg|n`fELA`;4MKw)zTqS%!@1;hcm>fCDjskcT{G%vqaUni70+ zCSCb??o;7nM~9UZjbrV|brITq`aE>3hKv=OJl*pPvQlc12y&4q8u1$*89Pd*jBGdprignQHR_L`%7YlmEJJ8aRmCL4|<<{GjttnQ7 zgE=y5P0c8?uBaa@@L7sn;RS!+*89gI^^OI06bLeJW?#X})6={lwvP7w&VkR*C&%~9 z&Ho1zvho5=wz}cd6nDR^4}OsHWbK5WRxfSr^l(C-1RNi3Q%*591-KrCp5gzrNxQ^D z4bLQe&@pVGW;oEOy%g0TeQI~tn(O#$O9nQP0x@w}OFqXrH74BGQ`A!JALv(o$?P{ZH9 zzH>g8M5lh52-^S0lgrGOiBFDP7*B&{cB64Mw3x9Dmy7--2Za|9Nr@O)taPpEf(OiD{AHR~U5gEfQsUw+0izR~@P5{cUDIP@qO<9#Rr;PiNbWM7EEKzsZNYS1w?vI5lUyL1#KqtO>wD_Ro!wbR^DB%!x=%`K0PA zxN+vma-(xtCkPv!{?s`Lnv2RF@N_&{x^z^8hO?A=g5xvj(>v)ctEoqW%`LsXrjsP~ z%kB}Fp>1)Er!QqN@p{~{$XLdjP7o51rl$7Md&Fny_A=+PXODMkDCFi}F{QVUVa0@R zNt&E8#@A&!8>!e7D(f!&47J4)0WEE}7+mi}QiNpE=x%w1@Q-|2*~~x=nv?j{7e_F$ ze4t2|nTeEvttzNT-#j?t8});P|G8R%KxD0WyLzIzd4SkaUrD9$-O~6#@BZ2!f#7vo zn0+s=*QT=vqbe}9M-wkhq#m`8i}$GL)97)$w8;x%01v#vjVyv!NthzZFqBrASN3#0)|ibfZ465cm<&P>{YFX$jNQc9VdLzs9| zSta->RiLw;H#rhrp^=}84yeHhuy%=Z;g)AnHi}mw?x(^A|L2d7ALDb*G|5Sy#^tGQ zF!@TEu6a+U`5>+k;c;~%-{^mk5lHYhW;omR|59)T)Pc%-L<1%+P~o(mdl?1UmnY6= z{-CaM(g)_0{YCx1oIQh}vW$Y_1o>C)9y>czMm2F;1k7g|()^>XXcp)WP68PX80#&a zB~E-X3v-`zE0Zxeg8QEJ^)KHiLhW9}anZsa-7JPvnsN zvNpx@GUW}^mi;kcfDIqJ=uZ{ja<=EZqP&kD9RZ??#gsGRD=7QW>0e1mtz_m!>H&J+ zh?(&RM$CSue;o5fA37UgKv6-G^-U%uGXp-8t^FpIT^PgJsz>xlCY|E z#=$SKFXCZfJA;5Y)zU?d|EC3D&c^qu;je*{LGA3bF}^&+QK5#lZ#g6c=Sh{C!=#KP zXIx*c;2Do(`-r?FNJ<~=)tq-xTHRd2T*w(o{_9i?w`27ox*4mh5(Qd)C}=GR%ioGO zR^Khd3}E-$w-l1+7&x`BAT8Q|RPK3@2rDbPY&v!BY$`6%**^7J6IQrK7vA{2mqi|f z;8R4A{6CVmgbfg~F}2VD=gWfrdVcgSV8-NEpS!bTTGp;+-@GbaHPP?9(>*C-!S2-i z8R~K*DQwLFkS{GSJk`W6+*zw+fQY9pt{?0ZG%ama zuVBxa=J+JL?y?cq@SjzXG8z3o1qEIPL>#t}>D|g?v0$zDQ~k@7CXM#F4g)9;K6#v8 zlL51{uMLJH#-n8z{~!&kt@#u9 z6q8cJIojg&gGK(?&4Yw+6iWdq}$#9wRw*kUgjo-icwf>T- z@5^Lvv?zW#oCY-D@4qive94bUpqX4HEa-b(da^Q@1&xT`TD3X_|In>pzug=1S!n4Y z%RFfd_3F4E8N5bWg50nLeOo#!Z=J^sMG$7XwNSB$8TGW5ALC^vMK^8I?Zsg;}{t(z8e8vS-JKs}gRv*STy>NiwD)ua828wm?vhVDcP1Rf|7&#Z?sVqlBF# zg*S4V?4rs&mzPn4+}qQ87#BxeaiRub?zaWP%X%PZ&8JN*z&{loqWA;)sqgw0A#mn` zeC&z0I9&eZ8!hHn1R(JPyZobk8-s9B9W$kf?T==RqFmbM%LGPpLRPV3u9a<;_Z@?b7%zTSHq{>$d@ zhdSPfE10VanHCyCpi8Hb0&*n`=H=z-OiN%$Yq*TbIr=eGC00@fv5{HUvRfH2o-j)h zch>Q*hO(rHVV34h;bo|{{!vxxoe2Utg`mdrG}aw z@0Hv0YYdlAc$pg((31^aI;DYp10QmCZr--iUKM{LzcJz3au& zEbP1WvC+*@O{F=N$wE3NJtQkaRfhqj+U=+kqj8$AQ1l8A>*h}RSL3Lb3>PJa2@y=v zHV89b@I;T{bPEhu?(ZlY3ImaN)o`Jezoo(Hp(;>%Gu zw@R78^x^brzc`NLGYJw-7K>R`=5%$9*Q_@kU&#OyU8~-=7P6eBQ!HFd2HH8t54uvF zQm?i;!>4FPwePo~ramcmlDGyTLkUv4{3W4IW7%b*9u+}Rx_J4^PE8Xgp`#nYE`nY* zFCs4F$sX0AXBWu;i#{j7z5-TDO8BLpi^m-=Euvj{#IBVi`r=A)xZbpqxMTp*Et#6Y z5rI9YpCsZ2d2JKr1a#gTqUdqWo}KB@#EqKBGCSiEUv(n29o$Re{}ksX349SqK*Zp< zd#*xk;u!5$S49P^$ntL$q8OxP=ko>u9A_DvG9fyPA+HqNJd-jAN9(c~yTxaH5KHRj z-n}ndP3d({@oJwij5PVQD*%ivrm<{OQf6DfetnTCj}Fuc6Ek^M9|D3~o(f_22UrQc z-W!x0ld*2*nRTKX_1n?TXSF%|MtWFB-#DpH^YaBUPFLcY?1LSrl8Vf8vcLsm1TJ-~ zP1Nl57H=C`Yv~j8ye^jUxc!?<{br~xrx^?Mxai_QlQ+S@UuHs>Zfv6#T-XjGZ zK%%H-K(3#Jq~67^0cawI=P1|}!#R^sO|$APRCY5WQEa`!NWe(_g-@*r0_Jqn62V~# z{?OG;TX-J$Y9^fVeGpj-c1=>b+VBi5S8l_9{Z2QpifEgtCeKedx}EKwx{ zfK%iey^59`S?QlY4&ZBxXbb{&LVD>?oUIH{94pmP>Ds^l-VF(-`(I0oMxwHB#+7;n zc}I^Qe+&JPiOg@yLD_K(mUww|+o8gcBJ_wtkM!xsE={8GHmngIB#TNU7zc z!y_89)&WF4*-_w0*|a8WO)X3sFQ3A=5P;8aLBf&VJ^Zz$h`Obu;@m;Jb7+N)@la9Y zsr>s-)?U66JQN6$V`QV)%a>2_+od6l(vo5ADNOxe&YDR#Cku6CCYtlR;Cver;6tK| z_9)Jw1-Wkul^%9&vKD;t^5yZRTKEA!0_t7H5_)2nnE(vmeY&rnNJNx@G2lIe+^ux4 zPS0;_{8UzEK}pBmG3NWdW+U~EZQIg3U-IWIzfM{JaF9@5p)+@jl&afhfT9H*P(!Kj z#BSB%EhtQR=8!eKtI6nt#DYgG8^Y+Pk5lu=xUE>bL=Abiya_Iz1g4bh>qa)aK8W*q zZ*I)~vbIzHDIN;1q~j&A3k?7DDpJQdG4`7~?Aq~h`@TBA<1tDcnaXLLf*Yz>OaRY< zJ9V3O@k6@Gl5x zEMb&R5QQysGjFwBeb=!%zovRQrpH!=YfV-&F>^#rlG!9N>zB+P1AlzD4rQfdb&%$* zXZwmeqP~ts*c&d52Wx8dY|QyKvi-v8?w;OB&iV_U$PVO(%*YOT;rfC|Ak)8m`aVHQ zz7xUQ$d(h@d3;!tyYL0AZEfw)qCf#iMGP>!x8Eq!aX*I!nd|lqFAh+RjD_tGD;>WK zd^R`Bx*C@MEv-V)n@Lpg%PZ;jI&%zE1sZRRv&!>5PvbCV--?ygrpWVeV6$IX(R$K^X8suV+Xz1dDA7o=k zqNV%i@a z;NaFEv|Xuw#RkazT@WbrQCVw_Lph0vMBDLn z#+Kv^uQq`NSY0F65`GiV2c%>Qg_LARGSn;NJHWJi#Mc(A&@EC)V=|qw6u_v)VzSfr z$Vw$JL#ci#t}Hlo7AzL$<1=Jb8gl0@!x8){@oJU>Cq)hosZ;t4)w*>h6-?o}TCa$- zg~Z=OgR~2USjHus;AC&l{_|(301d(_S?+6ClpvG++`5UfNcGKU&l)qBApAg9=bQo7 zdvm7Ty5_5KN1w~+*pA>6aEeso9y6W7BW812h}~-XD{NUR&tz5!8b!4teIVD_oM<$a z4^is8Vm1{WQWnuRPp6EJ?o=jv|5LMEp zpRti$w}iCF@6|0hfA}2=eC<~#+_wBiNUZ&fMn(XK}75XUdN(wt*Zqfz59jBb>sL3o9eJbb1PDLt*vrhzxpf? zxIzqe!S*wp*!}}cU)G}Ef}g8=caco@ayNO*J;zpd+O3v z`z8OL6`Xq}yqYY9zdWEVnr`4iq zg~f@9YwD?9E`Ikef(}(;g=K`SnLGa8`R`dLDFrhw!JsaEa)A7832J7E9POQ#EyH8G z_UyS`U*%$BX&a!i%HVPzsAa`L^)kFw(YWR`PnZ@gfC)VbYQ%O)9DF*t?i@$>oeq%Q z8)LcUQr=U9IFidO+s*Op0!jS{4SDJiVTDW7COoUFXt@I>l{x#Nvh)sYEaUBj{R1pG z;TMvx_Co}j#f-B+(6jCB#ffady;ZRQ^zt`g!t7kttzX6OMyMB&-4|jsGYlMm*YV?B z?p42QXw$f{>?e?Q!TzFMI|U2GJXqa(kK=U>LU8u6*ia8vBn;s1J3T@MzKM^oR(mEs zC7?=Y6OrnGLdn`UH4Rx<1bgVaR57efpci4cJ(5-CMmshedW)$rX z>qE|@l-KYfq)a7Zq}*%T(}k&c@m>%A-ak{EDLD7+FyHq-lFuwz;i;Lzd+6Qhd{g=r zeOA1ayVy+?1_o3zH)P$~wdW9?8?z|IVCDO!;im+`K?f(v3G)#xWra7z__1@KjFRT? zvT#2;9cC)(b%E)at6xux%D<6%2RujL=(Am^=#g<5shyfO!&e*XBrmLz!8K>F@T6B~ zU6r!7alxI4w7^2@_k6A|4h^ve5F)DYH$1^P@M*L7cs)gd7Kt_#oW>mF#2oO(W$~eD zF!PTDvn^|nM%=NQpQ8sC?J#ei(T`QqqjT;vsGZ`NdRH^MG6pnQtaJT}bZyU%t@-iN zWd4_SE7)1(=Wv`Lh6xn=vW`E97m*_2D8iC)g&86(Ha0T5o;jSK_@!&DhzzJFPx*{$jeR z>*-b{@ zch?FPElgg1XY-G^#;QOc|M!pk8%hINhUZ^`un593eU8y2fQTbPygeYLyjvK5iZ#IU zSjo~l$}PoS6{JC2tM%$A-VY-nz?pYPZy7Xe2nQ@30$@z1f?kkDE*Yz6Uni1rgRh=> z+;&hl$`u)|3tRn$$ijw#k;siaUQ~}FT2CT=qQ#%DQZj=__5}mOFxW1=7i|d^<47?F ze~a9Mz!p`-B7#`^d-63(SXZT#u#s>oq+^hY_f=VL3=J;Z(G0Q!0WJMBdJpBL8oV7z zG@4R7vn@W&gmk>RGiPPqBZEkZBQwAcXs9YoFO3Y0PUQgV zeQzleAk6H-ZIB69$)E=hskj`b4zj19es~NsdffyePuB=g1a))pf(gDlzJJ=^^M`jy?#gKy4%Ia^ zU5x(n9XHth!!IX%x@v4Z9M-i#$0Nf#hwa(4=i=_7KpUr?MLTWIcb&U1EF@Yl{jQGh zgsp!(baXnsH*DC-RF%n@9FhO>fhRMTje{0ey)q`Prt45nvuD>DEKV^ z9kS;8j8aWlG`u_e44YG~?TIj1mw2$T$qxamP%+y7_wezzh7Na@wGpNu_R^&$(d$2a z{P+NZckB~-6eFF*`P9cJDMzL<2{g8D_kvfiPK<23ayUC4+N&wPH6@120A+*UE&VCC z%(tuPkB7mwZkSI!YdhjJQ#|l$dbZwliI)D(XoZlFCazhnDof*J8wN+~ceO_vWS&(> zu9|6Tj?#!vhlLU1o3)I;s+=5-fKT2kij7}|e6u^$mD#PBu_H#T-`dvrB!Z~!Y<7mP zlP$Fr<0qG0Mu|21@%m8jjr2b>*>gX6L8G0NwI+ap`cChKnG+SmKO$MTUb%-DQr>ZX zrn&iHpc(V7@A45B?EBGpDDd55*o69(HOyTMO*5&vVqK}zv!^8wMG^<=M3=`HZwA5|n`MVEa9v!tqFtg50P71RwHa4HU^04{E*ZMU|xf7Ero?couhVyC( zuYdPpKFUQQb8pmbKdOP68cq4;yHDoLpC2Z9fKRd?1Ea@t;fs0*U@6LC%H0gE8^z(4 zQL1Ob&@y<@kD}~HUi8-+1>{^YK#oz?g~6v9}+labi*V6xqpc>rMZBcGI*r zty}Nhx6k=}HFDeVxv2~u%1-Exs->kYV>@>3+EDuHK>b-0JBd|4|4H4Ct(q_(k6H~y z(-MxtGXpkfB>~^d zda0%7i$4aYjxvDzNB9Xd$TB4A73yf7A|_osy6*!LFiRGI1N{?y;D9$5Ms@j1w8 zhik-zpQ&80gTGy$*+rBZ1F0=bJrc~<*6Dql)~yB3rt9J$#YgdZ7M&6tUBBG0Z!>um zA0>gCzrT%sn>JQJH$?J>$bTils#CD~sOx-6crr+d)3meGq;(I!df}ZY?M-sJ5j@2Drr-N3U>2N_LH%V#3hmIrTgn_?yTa2$wM9teyU;kh$jxUfd`^uFZFK`R3z=tsp3B2$V zR*O2dGIAa}BTqp~da_U*v}mrx81|7b+WN?j+=-`|)}-k{T!b?YjL z!4mN&jZX;8`6L?phi9cl`>qxS?<7Bm8 z>?1EhV3E=nM1}3uZT7Q+1t`T%RwRhBq$+$;OxdJKHDYrncG052MCK4(HukDhzy7qX z%Y^CSaz;;2k0%DwZ2`>qRk=oj##+x4Avy@{rg{wKKQ8;@Eag_VOo2Z0-(T3(-|;e!1{-JJ$v@7LIp_Z zLBF^X_zxvJOavPj5Czcg*Lm}fo+?tY*X2$b2Qd|tW_J^d=!ULOw;YhJ34vGfE)txd zsM{JEQk+mpXc3j(4>rZsq@-W7 zF9DrqbGqe`$vgqj*Et!htdEC>(-3ylwAQV_qjTz6juGGSWzr@11Tu9;rj!z#1-<$5 zhG@-@$;xys?IV5M@$rUdZIi*wac+fa!pUT z#veb$6qY7sqJF0D<|V8mnM~aU-chM9OsT0a{7g@?X5;;1KLD!{eOz0=58(V!(627D zXIrw&#LbiLEhD3yvgJb(A1V`^3BU}{dan9Z6glL>$AdrYi3Co~o9EQx%nlNicB(g>4;caa&#gSWaBTibT;)akh0`jta=b}<+6&T>bRNTr0%X0H0$ z?-taGM2Z1Cbz>}yUeP_V1QhFkBl3KS!9Uujc=Q;;O@gT#cDUj)IE=k_7@VNY z`=8`CB>Z{0Xk*l#V^|NuEc5lcbl8ukx6)?Y4D6*v+5ox>6K{{HTemLTcn~nSfKS%i zaphL_C8o5RJo(`6J$rhD?Mo7A9kD4;N{Q+0CY6&SINAV61%}LPjBNf`sV{)xo@Dn` z_0;dc6s$ozI++3ume~6a?0v=R44y_E@o*g`|Ag@qM8U4o+! z17C}a8$d>l?JK(Sh6ce=bm=4j7Yo%o3leM+QC>Jgawlq!iBiSK#+s4(O>VU*KY-II zlOBFHHy=tYE)zn58*aqKz3_g%Sy!M)T*U8UjYmG30T6x`dloa<4{5xlVjTQFELTf? z{htN*A*e!9SBF*Z(Y^blp}WwzOd>5uou+;xqh+I|?oQ3Q?b*P+US3`ztRr;-Y+~ZY z-VmhjQl`}fQZ<*JZ*?6JE;r6mPi6c8dm_?T5=k0D!=;bNYUGkiQStQLjF55~?Z4qO zu3#79j1|=jSm~5K@5L`mIx%I4%+n&cv)eq~EIG14-pR&dwH0jWCe`;U<6a zk0*f;22?h6@`4R>JkLxdo}w-~zi>(AsIzm45hb==S*?%lo0Q|cn#F&cU z8NP5(QXFsNWa49D4g+d0amW!t=>SE@A0}=pTnF1Lh~mEV!Aze%-SJUg%7X`i0E>$# z0SPeJ`~B5sm9s(z{`~Vjj%<+3uW?ykRxz3~YgQ;kVS6AlExHYaXw}b$miGg=PC4$8 zXZ0#r)`Wnx>tqpO`PZk_f>63i#iWV7w3VN%S20Rh-(FOA5hOUN^mYYZAU& zkkMzZaj~)bl=6vj;57yA9*%GJVYg!$q(Xsp`b7o;GRi9yY(9&42HI_Zuhz$>bx8m53+EMD z>3@Hu9 zZqQbp@Pl^uiq)_ISz>m_)3c|b6P2Pl~etbxRJOR1D6P-#3uEZY0?qdEF5TaNR0b%5C>b*r~$ zv)lFP*|TN^H`Su0(q;nWKDO=SC#Q93QM$v^WEjBJXyXuxukYoN!_VAu#;|+$mLR4R zO*LV<+`O<`mk3`9vJ?Orj62S2RQ89#EL+x&DSLO6v)rRoB#a@K&B>BO_M@~>5#4rf3mG-4*`IOy4`$1}_11sgG<*7q=n?Cf7 zdg8yZ!dqQ6PM*Gx-LMNW)pWfE^fs3`MsIlA^w+PCh&=pjQ%zMWOxcj5ZX!5xYpibW zWAyo^Lj17lmP5oAAkAZZV(I>^0s0lAtoRxFnx8JVu5RED|mx3q>p;7Cz71HB> zqCg}%8RQY{Cs3D{^J@<%`!1Wb)5)T5wEb~F9e_jk<*P67GuKEFn&e78TigU*lLV~Ydi{nB z1Pf*C_Ga_)>D9tcoR9*nP+WVXW>M$d9;4nHR76x0Il55a2QsH~i&5n4Co3+p){d_8 zh1vV3uPCe-ht_e^DH(N((X4TNwG*ZqY-^B>Xa7wcLxjxlYi(H{92`uCOE9I`3(Q~> zo#&86Pd>7lgV83CY(8Yn)JXDNi9iwhA1BO&{TG9$&BPT-yh4522s4VZkBS? z1Io8#jQAE|{fB?OxQv2}jQ4o<>eaQVD~>quA#iJth+;Q}Kk@qKp0cL}rs|?q|DFB# zDN_z1oNvl}7NKy#5+dC@$)mgcr7Y35z)?h|3t}a?chu15KS9v~d{XgXQ-sA%KS7j- zHi8TkEmvLdE_5(o*9vu2qMW(tbyytim4=rC%dtZjK`V8k5{*eZY4*1Npmy)eu!4&| zbz1CWf;(W(*X%RynV9+8Wdpksb%}eDP`QS^74p6zTldEtJP9Q$)#d&ei>}K2C3|IV zl{JQ#8B6=k-j+74{#OJ-**X&lSkLL)5WhaPl7ciEHfkgf#P?|YabQt)a^lr5=`1h3 zTl}DICW5tftsX*^>N2=!LwU*0sN2(}sgP zxd1)iNKM^SQ`fDhCNqZw-3C*=3(Idz_I+Q;prOHj+!k3VXVpTnU5y>`t!4fimbdi_ zEl5X{Hfz_dJ7Qr95zXX`;$0M`mkCSyTgDx>Cj0<=k*RcsW!>4g5&^;O zZFs=OZ7&UPrlb$)sO2SCBk-}4)$(XO*>KV##Vhl3o&!sOo05 zMl z!w{mXRtS5P@=$`F_DW_ybLE4j)=cK*nA+#-*RPI5;{u;Zovf1X<)AJzk`r{_#Ky&q zaycs5Lazn0x14SOSPr#M?``|pgkB_zpO6+3A#j6bu0YF@bxUkc_Eu|PnPq+1WCEnX zw_|Mdso1x11g(LDLL!F(Ij?zHO;s5@j5uNlr(Y555uHnctl`ohKifu{{$!dveS_9? zS(TN^0&aF|S`1L48r`^dZ78uFCctunk;(InjQ!A-l3v!mlK9GJKx3)$3AS@NzrFY# zaFLp;c1VK__N2^EFMu>D_+Vf(d9F_Z*d>4nk5A5jVy#3zhU~DwdwI;eygbRBTP2TN zT?auh?9~dza#DP2u{Ah2GwIg-^OqG7kB)En9zQ>krs;RIDbW_8xJV+0>$HYbDBUFj z_!4TfT?*~O<`GK!emosnGzeHwQf4#|p7Keb;?@x80dI?>uAs3Z3<5#Sv2Ypc*4d(P z#wE+RUkZV{F85F@E)&Z^QAq-`5LOF(qGPc+!(kE$WJvmsV;5QY3I5|95n-uh!3IFnk*{ZMjE5gZOCTT&dYu6i=UC34B2?0yQniK5`k{#YvXS?Z*?q z3z0`i?}XIAnNBxPRRaPS#6fzjSVTn2yfs4lv3;Zl1i&E!s2f40q3nD((aE>DuCBnb za)#-%3B_EzZNP%MC4cQ@QHDl$nI=z*%?X<&8*ictE=3`>!mX&{!DPuqaU)<5P2@U6 z1#H!O`v=L%C=3r3poKKQLbRt)B_zp^;nU--YD%uXyF0sPyhx8$EE5%ambRUV9qI`$ z%J;Jn?)o&TW*a8W>a3(*1>0IYMM31)&}5c1mlsx)ayy;59)Gr^{J??IbdtT(cI-s|(bcY#BDz#4e0+ru_{|7q%z zde?qWOEcW&MqgQH^S$2Q-n+{ROi95)%_AelCsCF5`Lq#VQULbO9T+FSE6kI=d6AU@%!* zpbu~cVS@}FJh*aJ`_U~H4wy0~rxxhQZ&DV&o(yeH&(@r(t*~|Aa@b6o0iS&9IM?!{ zMvW3eGf>+I;7~bnR2S@C=K+Y*P&5Mkaya)s$;oNdv!z^r5QpRElq2R&@kCtjmql*v zT(2@2$Tkr@LB!MvF1Xlx)7vq^pX6rwOzX2phXZ3;bc%Pnl02MC+>#@6zJ*{tjrj4a z4C;c39c~u7^h2h>0rFVzqx8M9DUWFij8fQCP6IpyA@2*s&Dw&Qz5+oq~SjH{0D{kyRl{l`0^i_xy1Em~^5|v`Orj^AVuc}?& zXj!kO+h0D+090w=?)Vgf0r?eFk)Hwe?qa8;3uL|Rm5x;TaW^9%sbIG!&6<_`+Z%gO z?clvXaSE*|Q1@tg#0|U;o^b*-cBvl85yV0TU#O43M|UC@r!^fCb4=|!#)OLDAN zFGQ)9)#bJ&Ou}=9glo4u$I}akm&GIKoUV$YoVaDzuGSzKm-AOjM2skiY^_z}pU=53 zSh~(F z=z64?66T@gR`Kb4@^IlUtKF(H?pGeX`gT03QcwpbT%jvz2| zX@VdmbRjenUZh^@V;doA<3P}qwFEBV>>=-T8Z%w5iBe~2@pMY6qOC$uOn&&1OhH6c z_CW*ReqrkVOXKc<`U)q42?kfEb_pVNsa#93ZG}nVZ(8cZnK@JBabsC%?e9~%^!AM;yHJGykPngCzWi2A5)Vw!z3bU_DEA|19)N|y9F1<} zV)~tlh#F9?Q=Pc1j7ysH2{4s1UMMp(IfAP!Wt0E; zYn+etJi?WcQY~hAixFeKWahNYzXd>g1&tV5#C^d?fj(Xmn1~RN^kix;%kPVaI)3`} zHnLf;Hz9b_(zCf~1uhxJnRP;*c3b71Hsq_L7FM}b7p@-B)j^DAK;-<3X zE(nz1D)a@EJ$k;YR4|G1t;Uak;-|kGfs0TqiU=Km@BtHA!j2-)sHh#NS8nCL_YvCk zhDkL@P92R32_Q5_VV#(O2Fy{!IHOC~xCS_brKAMuFkg5?86+K=pzcK`nA_o>5pHHCOtl$@OQ+S-1q z1p}VGdDC{~r6@Of==r8rShvCv1R<7$RJ1aLO!k{GeR`L1QA!U(!z>6I6Ez1)lQ3IQ z0rBMyig_KUD5yB6s%Zub}Pg*RQ8eU=_J+>E&ak$ho(e zwhfs!sG|~5lYEwVx+o?mOl3PRkIav~EhRU(Km0@C>(Atnc%t?c7_K`VWnXhVTC=(O zF(o#4=~7F}JFZ6#&EqN=@FB_EFi2~nI}Ny$O4;UKWYTehRA08cU$3>-zH}+`>VO2FQ07C^N-i&nvdj=6az)P== z>oD8MvAKZUnVHR?%&6I2mPPR`O?bGi$nAx8Ms9CY&{wDe*sviK6|-ntNxZ!pqWG=7 zy+^_MYgo@(Dh^^s)~;R4w2VPbCObGe1p_|G(FNshR{bXc%L#(}(yEa#VMFhO{C4&0 zeo~{A!U^sfKc!PW!B#AmFa}BO^=B<6jpocZo0zZ&fg5SMB4bRPe1{ZQx&u*so6^re z_IOyB5EsJd=Zqc1FgMU=y?upDKwb{P14Q1Wo)TouRu(=X9#iVOP(WI2HWCC**d@G{ zhSYWq8Z_wlH>^f>LZY|BFh%)=bHM*9luEx?GC5}b#9zuNiuupfu7wRHO-5vCCzDHg z7%}a3iWGnGD>kQdq-<5QCx-*LaH2)}Nvg&Jg5R_%-xT?-cLETnirBck>QZ&wjF^~JFH0!gR$%mv2wntk2R8-(1<6EI#SSvDHfbOpGQU<;sg>2lZdX>!y3X>yUBgGpmH&N@kq6kA;aGk!Chju5-!6D!7Ox~|1+ zzH}{q;VK0dVzGf(=1tHF{|+boxL9sl-oWGICV>xp6x}>+=5Go+mEfUO*RD1g>{TD^ z%akc21JG!DH4RCP<77zdsT90mBh;JQS+a0QMgqTI?ht^FX-lVwjrM(mp;QfJ0I@U< zLRqP$nct1)EoN1S0|Y#yi9NTVmwCzGM*1pv>QgqIg8N#ro(;L7b?-$V@1R{H^Kt^E zFKMPW4(bSN{fb)qmb+6Y-WeJ!J*~u(q5=&395OjloBnBeG9({3?AA3k++sd}xDbh< zOxN@RNVv_=V_!G8LqKdDdVW7dSx1tOvs9$Ww=Mn-sQtKFN_YiRO%4C8H)yz(7ZZI6 z)q>+-Md=*DEr|_Eecgj~RWns_sa!^=L4hCM9MckT6lHOdJvp7W zDA>j*o__kx3lA{WtpH3PdnrEI^?W4*jAP$!+gi)~Aq0K&L z(!yl}g@H7xm99fy&?Y)mr~GvN`t@OCJEkt<5+%^YV2%JLrI-!z;e*An=@EvWj29uv zw?PtX`ivR%rz-|yIiL)|JtE@;NihYdly;}XSO=S&`6MQSGoimLm8`|x{>y6ePpvQi zyjdCdg!QbABBh&~g5@O86xl5aQ0LFzMzfF98PO!MeSUU9ati9ABY_#@VTz!_G`fT8 zM5bc+h+q9+xX~HjzTCZj`~^2+PFKu5f^1>T3$H}vWgg7%UbjxPj3qHaIpNNoJ6b!I zLOd$CzBaU&2S}9HF}8 zSXeCZEoBy@XtzL=*pD!;bEj3-f()3i)8*ZS)=;K$;R(>?8$$^1*8DZ zXcC*yu_0W3!a66>WBCq!ArwlztgI*t_mjnlTRG4GRQCf^apaw|-?CvfsL!`4t zEG!PHLXwbxv%y9F!r&S;$%Kf&%BiCj9vtvk6jpJ*fJ;P0LFd=l3iJt4xfKcvqJSh7 z0U-g)$Kf3;er*@zNFXL>15P!ER){wP2E^GJ&0`Z8T+6r2Kb>a_`e2^CG{d6>;t_qL zqC$_IK0Sg%LW@))y-$)}@K}Ba(mRRH3|Y3-jiPukUzwLCsr{NY*OQXAUi#W7-tu5X z+F3e5W>!fjk|gvX_jIIc^A$>JIK?mqa@l_Ta37;ePhC%BAV{=!EHA8$_5YomZ~CS) z6J`)5hR|B5KkKyKt?}i{m-ijrd4XS1-0`ouMfM~U)ExuJmcSGSidAK!wppev%f^*T zGR3S}V3tSS(&uAM4%f)ZeHZ8Vw@DR$+9vrma45EAKr{Lodp!fqgQ@t6>djqf;Z|@i zC2%5Co&0z!)>>-j_p%WMo)6D_~Swr4C&$ zuoyhp2InI~naI#uQF#}#(fjwC_UzeX{j~UZhb44CCQvXIp@F5enMlA#tup9*JafKoBxUFE`iS3ECV@H7kHe=^tZsqc_1c(;NsFw^uXT_>ZF2pJsSE@Cf+ z6Hq`ZoopYG)`3kF^1S<3!7K289i4O6a|<7Nhn)xBu<1*cD^_~;Ja;|Y341S1jxtJK z;2Ch#-(-{V#1bgc2cLXQ$S>C?d!8mhE`_A1-bSOVfIK+JCKX`c4Pgn9$VrtDMaFBh zX@T=6ZPAW&wW1^`s>tjI33?i4yBeb#+|KfV%e3zbL&)bB;E zJA5`Gg110Re=zTE!@~ee6$c|Q%D4`qvbU-Tx$^Epa>A-f{Y2M%&hwknS%Ack<-*-C zBeMv9zf&5h|2}s1!+!}!2xJ1M)gJB<;rTUyJ{p+Bnf0IZ2(^gaR#{}-D4px!8!>Ue}6t%gagF+9jGZgkf;_1iB zg1@uL01bGqEzF@a&78Eai}0{`W<(Z1Vr&*+?|tKE;ks&cEy+ef>ZCS7@swl$H51J> zWeMuhBH4h{TXA-dm6cWDYgggV@NB+Q&e$h4yTcuejXn6@9>-E?T`B8y@E`Eio#x3n z^KKfY^%kXl35)9LwlgLzZLHu@l`4tV(|tu6CC^f}AbF)=N?Xc4acxtny}5`vJSz%g z*dfeTq0OXnSbwpApaVokE!`?9)=R2AXJZZ-60iE&6LWD7Y(Z!k43f^Al4FQT z)K78dcpSLyx@Y%p?yuj~Kd^Av}AdRkj+DU+f-V9nOir;J9=)Pa!RAK2H6&|NW1kig-`?6}N{%1J|3@ z4FDdX&QttVgoi+}pV^49mqelvsf zE4sku$!LoQ@xy@s%cfSpkyYn)-}=E%JfLPqel~v7sdFybW>J6ooPYkoR{8#7-*W)u z6`0#wygL5f&j@&5Tkoox4!Cf=Qd-SMTs`d4R%>!>WHqqQyZ+j&@ zy|Q&4RH?sewPr<~vgVUkSF>tap?T}(RoWihF>LNnx6eHvWZ+}4F}z1-mkHiy-kfhd z=3Qj5<*Rvx?;qtY>TOWXHSUF5#TplW*zQ+LgNi`%c}`xU$-qiyy(vsK$%J}0ZsI@R zlk&$)=y}_H?F{}@Q&hJm$|`kkp-Yt{epQhxaUkCzZH^`VQnbB_Yr z>)au-H~W_@YrMCH+PZAYe)-ZJ47Qu^i57!))&fuK(&>jc{_)$VO?^!Gxyc#CELt%z zVAHjSCy{+AsY|v+_}^9$7@u7ui8+ufC)qjd@9Q8lR zJPul{-L0;NU8Md&I^Y?!=49HP)x-<3WZt@z*?rsxnTc%y){eLF&~;o2JzvJ z-4`hA)MHM7&);rBy#z{=cn|eHO-)KmYfMIU>vbVyhsXb51Ju8Nr*z4-PGwx+z6UgR z>rIJdl5$7DwST>+YE9JsZEgzO`0WoL5>$5lH;nIN_0@XvIcGD0I&zWpVPT0RCXhp# z$%b(2H5BZQekp@8A06Oto8@mCL(y~DsebK=ikmc3U*ivd+&T2^a^HhnO`exWhx>jC z2;bhz`If=`XiDH!G&Txt+5vHjH9x$#RgVNj)n&&?+-|UBm zhKBHAE>~AlkIi{8xS5iAuVm|u=+doQoLH@%Dkwx9{%sR|>`-E|wk-k1WNb#R#qF^d zmn?iV`$`q{D*Ww!S>(oMYC9C+0Q)cuw$aVoGB(@m-^^C;jp}1GJBE6;HhPUC^`^w{ z=-fhPcKn+meYw2E^tRq~1XN5Y;_UmEed=Gn#6%puN*byKpYq1j*Slt( zZ3T6};a@*x-?vYZW}2Jo`-aa`|KE(`qQHJNWjVG5pE%K(qEI6!3AdjXVotPn^a)^A zC}v`7;UkEEnPP$d`gNx@%)AcBlT5AfG6v+&YvVo?dh&Zzy6^}p)`yIqm3}=l`u3c^ zzU;@JQxnzC_Q${a@q6P+>Sy`mU;X&KvF88&^N;KC?fd^-mVa0jeAoZ(mVa88|HqeQ z@`{|v1V@UwNnfjfy1kOW`+xi<|M$lJx&r_EK>xZT>Yx8VUY3$L`5*6EIl2ZMxjRiO z%#>&fcw7C}ty|7u#8YR?@Pd5q?v5b0H^@D$lOsWO&0qwKUi>CpGGAlmYXAQdMHoLf z?AbHm=+SnLPEMKd&Ah2?#ZZIUPu1s_7Zkl)6OU;UpL>B6_n3J;H@0Qlwgz34(UZ!a z8QpG*gz*3T)g(MADXBXh`A8SVLPE%t$;lt%Mb_SXo<-*)q%a`~v5jdwO=G!?|Je=FJvEx%_?d~#d*b&<>9cUwUP>X0> zMd_sT4ehw>qogK3bmDUBQ zp@|S-@3$$iBr@o-T!#>p30?cW^b}p)|MJo@GX(`AYxN z#@#-3{`U`jHzdtfRW!E2Ku`Ath_(k@yqG30DM^U``J-I3DMg1d`gH8lI7qB9Jm%hA zmAStE<)OSg4F+tgGpPd*PKq95ppg|@) zdS}>6Vt>euY2Efjv$$TTi35>M>qOD9m)7WgKsmjIef0N_OGrw}JJXXY<>EizTaz4v zu3gPsTwHc{chb+*xsQj<2S2QUGG$a)BbQDbB7`z;dgG% z_RFeK=RU zap{FqAOlb$n?}mVNbXDZ3>-H%{o!-?_T^o8eQ7V=KB{|Vqq1?GDzonQf8RJlY%`$d z1Vo6NBqt}IT{@OR6my@u!T-`?dfdRuu;Z>8r^&+6ep0X0qOiQYd4EAU?qnj>bTU45 zBuV?(!RpLi{Rvlpf*YE9tJ~vf$akGQDjIa@5)CV+E;3qE1ugsAM5cDX(}wo-(H`Mf zWq;lA{W6t^og}Ue8Eg$eR+AA#{q>eds~l&}^agW}p+%_5-{wJXd$rT)=&L#NA(!U8 zQ|_4lXS+vPf52_T(kzXoX>|AAz4xJoI|dCJ6h=EMclqg$5B>hl?D|v(xhUoUsrRZn zqgVfk;8}A6zE1I`U38@0EIIdKHT>&k5_SPopfKv14=^P z%Re1#V<9Gfv@f>%<(FU1Gg)Uu?~VEP(9}G+|33JF=TBaKlE+L{&p@!mAHQbDe6`bc zru(vSBjxNqzdyRa-CVDLgx8=$db)U&(2JtT;!LwmwZFcD;<}IP9g;diHpQVkfBds? zl9rZM#^FPI*Fl_n@$56zzxMRx)4#pu+UW~v<$zCX#? za{Vu5&4OJ*5*=%woG`}U^f#TPs-0?mF25zia-B|h+cuj`o0&wpRexCR7q>Q%Mw73H z+G(%fRC?st(Cq7Fd-V9yCZ}MR?}=xNzk}4eiS&MFZu+I2hxCR%FsM_vZXB$VOt~@m z+NPx)p>^$^nsO?niGMl64EJWMeq3IX-wStWXIK<2b1+TIyRGy$|HDLV>_Qu-`;B(z zj+k{DShx#5M%Sbfm9A}R2q<0t48abDfh zY_mT*!*hw_S=KJ0PXF+Fo&9vx{q3b6pf$vOj#L#HhHWEGii4Bez{8~9_`f>&z|UE~ zWJVE)b?4#NYN%9Qp(oWO^_Out^|C3wSbCSB!|Cv>3+nY=G#8WD`fsrGC%dK7Ha%dg z41_u`Tj^fm$3@6M-_V;nY2ulsusjU+F36}@_OB7H|JdO6gEZ+Vs?|WLI&9}3cFAoe zy%n`6@rRr!I7f6BKrqp9T$G|76#M-`{l|WxF(rV~cyb&C>ZMewDNy@2h7RZf1=qSD z0;2YMh}#wFrB@H+bFy#r(P*;chuu{5=aNt;;C9XjT1UO5nAeES%;MVZJo zdkN4D#Rn08W(m;Q=OKpbO>hP}z$x@C(gFm2h&ubE~W5b<==FJ*knV7a_# zb4&WN)LY9y|Kf)|IV~4xwz0q13(8?Lce`=)GinX(Uj-4^>oSqaF~;t~9~)3f9QGM_I$CKvYquIS4jMKmoi}w=E7b4^Djqmx}XmjP$&#Z1%VPkpV zT1wST|Jig=+lCQSaD~C!7DvK>&SdkA#GGFU8Q}-_qWyFedJ_P8t5~4FbD~)W(a?GPC}UGJE5_KVGjasV_{h3-RgeS|4?X z-qQ{Zo+obK-ah@9xvpj|1Ml=)yhE2fYke0!*7)zw!MOJ}r0EWQ9szeApKm=?=|1p> zgJ3V5{PVE4KlNEFt4zqzP*Onn!`;7~sKDB2)HKHX)hbi|4SZK`xQu-h6fr`GlC(cO zM=~7Va<(Ns3-xF*HEc7dZmnAKWNT}eFnRK1TK{dwsaJmOQ2usLnmqz4 zwdyND%YWD~b@L$IS9p@cfLnbCVp@mx%-i`N1J!2|dgdJ|`d{g)qUkYntx@54T2Wh- zQxYs$we<)YHoD^ipQA9J-P)K0$s2Nn%(`vH5El264<9(j^$pU7N$V;riN@czS}_hK z=mCO-+;1x?=?xJ+^MraBjV@0u+Ihp!N0;er}2Yn=z=EI8y)PLP-AqN4+S zxjvnhK700DP>JA&&yF4SA(G2~cwE&D@tY%t4bxdQqz7g=Gm;*S0L;ndZ@Ur4(UYQf zpY($Ru5BEs>mX(Atiy-&xj7A~>Z2vNB;z=p8F6&L?`O&H;!CGOy7O1R!6v3xpBS9e z9wv&9hQHR-Zy9ft(E|jCWdL$6D;sUkJnaSa)JvMK>Nf$oX!6{W^XtWsIjfdXm|kXaxBqC@X|-zbxf6RkD82A0$Z?Zl51;5AJWzIh{y^G zcHosT!&>}tNZnL2QnGQLL9c?>C)iTPjk=D%ylQ)g@%>CpWULa@(fZ`d?y`;Sb`GDV zww6|h*xNDmPB;*LyZq`3b4ngpRO|4P#WIOjb(uR7oa8)KB^Q4&z})=W_>uf+-V45j z-><+O3IZ`UJ)ZSl+$3ifdhf+)dEZNAyq(i%={#Kum5%(W-22eW`pI6+Dd3 zc&sXV96(@D>JR6dbpiHubffu?oFClp$b1Po0s*prN+~X*;oAy|RHMe-NLQ6)V7r&R zzjl{ts87@vYUN98dGvNybGqzoSg&$bzL_RVS8HqQ82afakh9;#BfO*wOX`zrL;7YI zb*5Y#@#a(tHPC*!ic&RzQeMR;b7)WPN~x{oe;lg2?Xhuvy++=kTwJ@1lC=Zj7j>W@ zDu|Dke7F_HC{n7jwba%jsxM^0rF&9$ao#L@e|MOHYZ(C300N~KTWbL4E0bwY-X=Ot zVPI6fN*1;1-H_2PiPjyRlG^;?WRy>746Am$X}7E(<*^xUSo;;@-UgFzZtwm!BGUnN zzg)8AbN1>rm|dN7F>=c1t&BX0QY@={x6YQX7`GVIKx=XuhV=SyvvVm8H&+J|>@coI z%Is_@g1`atYcT9SyB5vzLnVKJiOJ7DUoTc2I(X2GFij$+yIf3>Hj$t-d1(xH>QN(l z`I+<;KRF-z%#Q!z&eyyGEmN1FOQM-2ut;^S4D+( zCKnz$_Cl??N|48QQ|mGsLhgA;49=Iz)-}=b3EWoyb;{}Xh(~Tt=t1kY4ob8dkY6!W|D`KH$t0O>m=9S}n>yBN}6z$p7t`X;99^a;oOkfTw#%AQX&1i+uI6M?a zzTT?RUC>jOfYXzh5xtg{f0zgVw3c*pOGFjnf@*!&c4uGiSBI|^<-QusjA*)dY2L#H zs7hp@4PQ&W^`kTbgI`2XyMJ^~Mk!+hSpxa;ulft#{7(OBF*8o5+Jw#lg!KXzIdD6d znK?12`!t%X7+mGz9Dr=mq`$rZd^N*J^}%QYYy^X4sTf~x!fV%z<5a(6c|j&%q!+|^ zQ@fp~m&{(kikaHk3F+piOyvkIIh5aH5x?D_6=i)%{v zjjl2-63}1(=Pz?akV!7tcocE&5-F<2L@`yBYh5? zH2T=o+R}d6K?$rY`7a;34wo4NQk6*`%F)h?8}ytvU960b&S1k=2O`)X0kpD+V%X{z z#|S*U*dW5nS?%n$?A=Oj6Iyil^9%oRVOgs&LH8eo7d@%wD#1s-1;lxmHjsX3acG1Y zFR`onu2*l~gtzWFP*>B%(Vm2S#Aqc@5BJcpU*eb8R66RTQ7%-^9aX!daHqQBj_67! z*Ia%~jI;mVQ8WftZe-YnK(%FMCF=+V-j?o!HY1Pg3N9M?<<>0J1rFTKS&SX%IREV< zcXu(-@{+;bpRAgibY#cM75#$<+jiq{iH9!1^_V!g+!ca;3$A{5}veQO&Bg=IJB zJlUe~-C8400=bw{O7SIHck4*vEME045|_)>FI%!kQm{8Cf6pLAJUqNWI=H{xUBWPW z5;u@utFAml>ttQ$+ufUiX4ItP&|U5mwU?4?>eE%YHVqyZy`3+c^RnW98{YZ`LaZ35 z$rJFgxrn=59Q&fT~x_HJcruaHcmW zp;a5j%D0@35;#iI1e!gj>~@Z=e;4_k?q8nzPJmSuHty02Vvo~q@;UzAqafgb?3t`I zQL;AEuD5`0qKs~lUj{Myp$V-44XLVxe6FIYN&||_JpbYLdS+(z*}Ev~F!cf)h}?) zV##oVHed!_9tp<^xalsW!W3bRH;%5flhTs>Rf)ZhCd?xeDIpI$8AY7OH($1wd1 zo^2W701b>>;k0Zf2|!kSX^4H7tX_S<(Pi1dp|7<1fFYY;KhgP${1r3LzB(wcEhAf3 z{gq&j+KPA2w-Eucp&uBL=5hoPvrCFwyn%TCXIn=(;KWN#R{mx((&K~Qw2*U6mQ|<3 zl7G5-e9{f^wkW!7$WQxxv96E;Uo)q&gJl)k4C?Zn5{p4d zJBYUb%1mLdZhyIK-O<&h@4Csi+fv*NUx>7}fs-^ctk6(Y2}sFg(EI2`_E>VT3ikkd z-#=WmdtUN1A0~SoQ!ED&j|3hPV1_MhDSJY`e}LqXWqOldY^l5WH%AqtU~9a6vAx@@ zvBQ3KrRAaIZcw|e5t$#;K4X65<_>X)!k2IX3ie>>!bulu(5LzHk{3N6doZ}NW8($s z8vCqP(ycjwmX}_1j{<+TTfVUtV5&#x(l<^#E=>=U!j#COQKX)xe^614_jH1?ZFeFG zcPqPOzuer0WgnZp&{H3%e8Jxg!~LMR>FX<2&47Qh&S3WAdc4?OQ;s_!d^8SdmHM~C zZ=$+ZN7|&CaFtS&!6wzrpleJF#80{vkDhtHGvf8}8xB;YE;ng47-W?wF_Z&NjfR9t!qtWuUeL@{@qa7u6Eb(PRKg~J@%ZfA zsb4E7v0{te)u|_k(Ji{2-ELgna%U zxx@Ctb&U)eMX-KODhgpBT#~y=uqmsi%GFj44Jh`;eTRC|iQE}E+c)FXk234R$B!So zqZW~UbD@mFS77(*r@ z#fEx4ew`?3Sre;ZxRx=J3&$~Dp^?l(W)+!4S6@6O>1TzK*xl{GjjLA!w-wApVQ^UI zTeok|o9+AnfFXv>xXEyB>`?fVOivwHP_yi^qgrbg6{g*&D6{0U=Q#T+6Gi;wm6AzB!S;1`!8Ra~$RL`I>baFYc z0=d8Y6?XxPZoRSeG4^-_VL6JuE#q{|_9zYK!RH)&J%W)rlVxmsyYufZ zALwH4>5eS$8p#9u18$gN93;hrk((}WB~BriK`FgZty^N7^wHO^Q1>EpYI~0CDrd;p zgt5ejZ@)&laX??71_MN0RW~6jWRUc2Ks&AEkvY0!`dYlHR$vqYH=*OC>zngQ)HB5q zs6MzB4?+6RFvblt*2igg9L*Rs;F-hKujDIKbr(Z1}H?LKr zMvL9E``t@&#dF2d#8q2Uaa_)S<36)vfKIz*fOV}$c&gER1G3QVsP;I>tt-*0Ub9Bk zoj7r#35e+mvJc`3x6+-+d?Qe(cA#ZgZIIorOefUOjVfJDcj;b|knLS(*xmFqGGeTn z7TuX^=lWsUyaY(WRA$1UUoTw-80;Ei6bM`ZIsJFSLJ(n>Atu(=TRH^JBMD{TD2^3XWhqfENo>r;>Nx`=c)ll zsxR?EnP~3fjZ5n)1d{x>^CKHj1~G7W42tF{KpD_uGXsOfAhqXF)2xGfD0CRXhI5$7EgZh*B z!%b@zICt&|zoH|_ac$eQDO~Gox$IN0y%$N&f!7n@poh(SV^_AUn;qI1%?I~*wt2gO zlMP{eAo+U}cAGIq_LxR`j$mqo8Mj7^+Fz7Djvqh1pSV@*&A=f;HlQdY2{(TWF67CM z;OwtQe#{Ic%VLjVcx+BbpMlZ9%o(fej&+BQ(#p_WfW`$#7G%o#Mkc?~(>cgW^kH(= zoA8C-7r;MuF=Tt+PH!;;@_~L4ZP4&J3hXZ?#kbQ7!tKm+kBg4(=fNf70oCuw?&czV zK7J7Z?#1po-s$p8+!nQ^348GC01}>c6_rE_VA_kDDNnYMc=;u=nBSIx0X$>x#Wp5u z-Ay2+rCZ7}bPwv~x$KqB0v`(+i=z?{7DojLn}08J@ZMfM=r?SPDP0Hf5<@$I19*xc zI0=D8jDc>AlEf=w`-qjx4=_@|gX7VjYF%A++6Ci2aU}e8=w-jlWT^4qZ(cD-f&t{) zX(oK5J|EsQ&!|O+c?KS+K1zr4NY?e+C<((zGB)sYSBh7gKc10NBFo^73ezREkNaeU z=McU7js&817d8+hx7JgvPLO4#-#Ghxr<0D)zzU`0Bdhx0X}5QGb{H00yv_B?wg?Id z)q=j%R1&&!EImmbd&+o7+v5|>31FL#Wqu#Yt&BY@mcU@!{haUfC|h!1-pnxW-$gVIf7iStXZIv};>3_ukx8N^mk6Q&9_4$1X9jN}9Ka6L{K`oj7X{!W2D_6B)z0;}l zQkw3_<`s3+O~jTtoZ{f5oTezHHr4Ro`J0Cln2muw3IvkabXTajO@xwEAAtB{k+vua z4FM-mkOpaz?j-4Pahh>TB1-nPLA!IWJC)axD{NRwn_BdFWQWY_7f2BhSfpZoH^W&E zj*WL_m&c8`_=-dotdXTPw z;mBrLTz^Ck=plJTJ0-z^*wY!zUi7Bx5q=+@gMM}&1ftr{m3O1s`cu3wa>p%o6)R$s z<%T)fVlTp~#0Q_i?@S2OBqn3aNJc5qmxL74Q%<$TmZB+QATE}0XUOiw?HuMcsDYOxXcNoncz5IP~ z6h}`wqzQyuZ0a-*M{4vr`!Uh*rc?d1C$>PG%OL&)&U_fr?ch&x+|o=wbkBYH;{ka3DuD z%X{ai`r_w|N^L7Wc_-b6)U-Br!krbQ8v2bfGc%K_o%tTAT~{6ErZtiD+ZoyX0U)e8 zEm|B(3W)^B5Z+?6WPOo9ywYN|c1aZkDvdwBN$Ga)-uVlwp7IpN6y}|d^ukZKUdA}f zN>4i%5-B@%9OCufu`aefNnXNnX+m83ZJ$R z=spf>mWhmVuj$!mrEYuqr}m!25v?@UOJdHB51w^Ivl$#KH|W|%(<8d+t^x_MP^*kV-Z&>cV*@Q+?Vpvx}F~4J6MhPE*j^A!Y@#hTPb zyH9u8fzp`TrWtFZ`eH(DDSX4?*!oi{)OEAV5%VX-d}0UfDSKRO_DtKTRvB}aT*8kr z+%F$Ts+#Uu3{+ROL)F8Cbg^P41&APetnhTW|SQ^ANuv816C!FS^r*@lsY&)Q2Fc%3*}#r!zpq`YnY>mW998Yt? zv+G-OS%^;Tr$(fAE{o!&mI`7LH1~NrDc3f zLM|LQkxg2mP93u@5O5|Xhcg~FEQPIw)WbI7b$}EPFQD$FdAZ~Nj?Ho{Na;vBaR084u^0izj}Ln%cVKyos;Zpa zBYDm==j)AD99I((#?WM|ZIpkhnhulRo7A|=iU#?%?KqDA1=3m zbz_c0v%_<~Rl}=GrPc1tTk#4EX);y@uIN%jNthvqRIoXaTfdKI2zqNH+r9z+_LG|V z79{f*0?hw z(d49KQQSf8D(LD6swZ&s?z#8OzdJm)Mf)MMNl+^EB9LV>9UpfI!H58KbR2+qz?-xr z|8sZe4$R1wyHJjmIvvt0RC=1Bx3|4U7$K1w2;%|En-ME;DRh6DnnV+UHEY|pEdo6a zCG`~HOA1GlAmC|r0PEyk+TNPBB+!U5iUSG0dHuxhn$Owp)`*g@!&KpT;A*>Ym$7M>n40Lo9A*^N;WBd|&1g6C~qA3O(eI`<4 z66J4zycac3|4x4R$nQeRte_9Y#y}{&=a(|x^YL_nm8+{?_AFH>Sqc=yoXEG2muVk7 za9~Nsul|!C`!RcO&Ju>tNZZ2$G>POUzr{J=yw--L&brs^bDXyPfyVm%Gk>>N6nex=Xcs9il$;RW|B2Hc%2uLm8SWWqD`x zT$(REimfZ*^!e{>uIb^Z-I?9RIa&Rau@}*we+0aq%vCi9zk7Zu-iO~EG1}R*kkCPh zDa~Dul5I@eG|+1{73nw$Bgy&aOC7P4hdTjrmtxZDMIqn0mRW&hWwGcavGKYC#55EF zEPRbd3EPQCb3k*+Esq^sEoMrRxWKNzqFcckD0PAq3+D=1D-TYa znwbqAPPTuqeNn_|n&oAIica5`#F6|_B<3L`3bhHeNn+3a2{q+zUA5C^#XS;YH<1}H z@s_=KWX!p)a{Nd=z8Wo2aX@r9d1wahXB*rl-DIHGvliZ=jJNO#z~y5roB0fs8O6}ej}#t!;jlRf=BW!zfdSs@gqTBVANCXDiryB-&tZIX>()1xPUvnT zfj*UDDRF~eSIa0; ztmk|Ibv2<6V%gW<{bI#HCoCiqZ9d)6lVVE*=W{;}OVXqJ1-+K`AuXfI(cV{ z2Hj;I$h8CUW7vjKp(A#?7JpHfUV&ER6~s)9s7&pgaH%gkhwd`b47fp!%!6o9;ZWqZ^r3ttvX9!(pm>=I_+g({w2cgR^O=bJ4nrr@*DP28`wm+^xqE_@M z`(svOYn)y!v6pghLc3vqQ&SnxvU);am!(gFEpLh7?VAY%>SK=mAF9p-uIIdK{~=PLMP*n0Buc1^ol1&oLK1~+$(AkK2rZ;k zLW?Zf5{0s549b>v*%|v<+4r)wX!*a7ndiR$x7YJ}?paKJzwh_+Ip;dpb)9pRZnxyW zl)6EDw0w=>29sn&-CjZgexDtG^~DTtI@2dk>}*mH5r@jU*L~@ZsrV0PfJ$d|>~#uN=a?bTx=ra-p@;5wrD#!vheM1 zS8i|Rc8bUTD~Um-i#;P9v|jSp-@_w0#ltt6`J5of|HC_dx_9*1pny8Q>&ClAW%wi3 z()4z}aQWA%jjJtJkh16n32ci@c@x=>B(#LgX>+sL zm)gJOgyB0a!+@60C=@clx3ojkfFgCi+NH?odDsQ6_Mz~U$J^R?aMLvYH)(yqEyqrG zF27n%c6#)z@d9cWu^#w=4rb>)q?#0STTSjX7!3S!C9>HGdIFumSdFSsm$$(dp7@ zpGUJBOd7Ie;L>MkVZ^%cJw_HRI>dQt%l3(%4@Ml#VIDgXgKeTcq`QkE^3s$EmkB~s843T> zI#Z+vE`!iiMC6f&TJJe@XiiXvCm>Fvk4tuT7cc2CYNQdKnDDfOL}2gChxxniGtkfa zr;Cc%S4DaONT-f|Z^eQET@9t!-2TS(-o*%)8L)xS+4vWGmimA?P z#0ba{puV^NYvxVGxKnZc6QQU7@dC_iCtUEBEiJgO7;@upZ>)ME^2$Gi{S!a|HsyP3 zpNn%uTr7x)yp1Su*$$}&(oiX zD{CR7x`>SHhke)Q_CqlEVgi*mJ=>D^cYojNN>z~h_^mE(l-D@xd{Vw{0c#uV`SU=(Q{t-$~j!w#yY$<#T zx2i?cJ{m4W3SyQ*6!xtHmMvUAuUXmKA&&BA&kuM%{wA^@To~6nS526trM4h?X~UU;o_zCTzu2s0=9Or#Tf@2ts4 z$FuUhMP`NliKsJ7!)*PiG)dMz0boD{PoCX)CuSUM1N}xn0`>=${Fn#7zJQ0FC(^y- z*}s0MsGhk}`P;9yB6|Ar;F_t?`G1+Ex{Umccnv zCkl!Zw>u|tuYf%9!C$@{30Um+_u^B0qFN-LT=AQdQBsb*ul`x!;yz)?XwjH%0=36| zJ-u+kp8}tw&h?KvXi-zAQhV$E9y5Ep&0cGxJGy!qH;wh)ZZh65&*Nd-6OTQ|=I2Cf z<@ZXmYHH}{?dYu-rPWnoYx`(qyOe96qpmNlx>~Wd)#Z#4GnNh1k6PxR`E|>Pp?|d1a*k2HTYq7bFXjQz5D8gwn=Ar zjLfIp@4vllYEcDMBF_{0?uULiQTB9#h zZ26m)w^(H|==uKS>NfFgIRE(|Bkcg+9pxs?Cet~LXYInpi;YS3FIPM;O-)a4 z2`Q1qtBQ_`Gdfk{?BH;Let{`oUcvMjd$FC7Wjq@^zgt*a>oZxJn6lpU^%o5`Ej4b| zET24$S+e9Z(tv7A8^{8bNboL?%vqy0aHe`c<0RdINMH*d-U^$6V$BxAi zAe`!tLwBAUeTof1xw*MY6`yz6un%Tsa&ku;ZcXSc9R6ke#It^@`1h#S8ZvRW?;5Pa zlQ}p_D`7t;!+x0e>?w-8b!Ej+yU(wQzrlHT*o%Pk$^7G0@F%HqYc5PtQi`bkT92{IKEOz!;P@lQY2dc{Dsp8n- zsfN?&-ol@g8TnyJfUZqT-i@%qx11^+pINpHTt zzvmhyWM1d@;Yx&y1$tWeh7D^nGc#>^m2Uj~_v)x9 zjYEeIPnk1k9d9q?gFg;ydCP#at&Hw;UEj5H=f;FSB_(E-GekG*%9SQW#Mdf29Sq1n z!4rG`#bDRsMT^TnO?Z56MT^XuA8neo?PKufr^#B%@hNzVNETLRUAvM8=3x3;nYX?n zyI@zb6qixDSsrnkyB(d#A>DSp<^i*syo_=`z<|oyyS8-d^?T=6(B& z;rh5yhMWB8&Cxn^6r#ZWVX)3EGY?EW#?)C3CZ9W51y&1Wca7@L^gb@z-iP_4$BkRd z*mm?n6^7II@hdgZ2$>(TVS|K7rODt|-$jcCQ)7(v7`BRrF|M4VTeoJ6Z8&sVL!r*# z^~Oz`+Jj_tQyum=7};{{0Y^#EnJdl>+zf64i#BAx>>8GWT)A>(_mLylG{WLAk#{gO ztY=+U-2;*%@%Gf3ujkwdv;c8c2rF|-OI!Gp^p@c-wlZbi_WM2V5vTBVE;mv&Wa%6MqF8V8oXBXp^xCc+JJylfr6h22Hz=F%+MS+>Fs8M@J%xhmu{Lhi_gBi zW}CoF>6Jl!bT|Zk#KvBG)@OiDBD!Z*Ys0hVkc_wLm@v`0fYl;JuU@U;=5(2`D)-dK zr{~%*qL+J>Ps98Iri`C2NY0gDPJf2sfB~MRCx7eNa{&thQ*>Dq)Sy9wtg3@o%HLv$ zGQQ)NSyy|;_0?PE&jjZl{-l^irJI(Pb^!=K)}U-4W}$ZQF&6+f3z+*%E48JG+8-Zb zOB*tgnmg1Ujoz|r^nq}U3abG{&nZf?0a&`VOOKvnMz<2Y!03A;U?(=#s*;M?Vo=QJ zQuvH60jqMCPKX*2Z`qKfX^%f4jga#=GbkbRn08@`cnsWvGAl~dhhvJ0S?OUE6Yh}5 z$^)m;UKM-$aoT?Uidn3BY?TAhINpQP*70xf@Sx{km)f}Mu)!*q$4_Y5{b%3O(o&ni zK&uuF{_ns0^}jdkmeo}CxdKu~#0S4`@TMBxz-lsN%ewB~!45 zg-uo7cc(+ZFwMUivLYP`dZm{o(W-jjGp%#1gMK$|Eed_ei4#59J8|`WUlV%u z9GsnVd6iQb4?&Th>e-CO&t?o3_WypzU6S0qdEHv9(Js7vIY-{`w>KlR9#!qN*+U$< z{d~EfU*lUdre!RxI*AD5RO6?|a`Lzg7HR1j&7;m-rsQfPUAU&3{XRb(Xl!>4(8_+8 zIV%$r4dDkuKwuUopACEV)Rm`q_;6R2v20p?hy@sUERE#)B1T#xA(zyzxG*~zL;Pg8 zfeK`L?}b(=jc5B16wJ-dvm7nE}q5q5T_`>4Xr zd-c-$tLeNAPN2bGzl>Nr;8CfoYY;lPR;+P{;bwi86Opbvy;w~ht+ZQ4YxT&YCZ z@7Cx)5ljBUb(Yt$mPqg{;3&ZB7s|{EOB16Wc(*WQGypfPLr^z)Qo$aCDKln-A!apx zI>yOKjjeAM{uRE--J)M|O>qh_{2@-?Ahv(^N#}Fff~OoY*T={7j^F0Zn}c6Y;ckVHZ5d;q4LDg0n~z?E+)A9!!%lyfOO~ zEHg5hC=@|WzK5zX_yiEl%;N(fJ9&E@GoaP9-NQ>F@4UsD!y}4Y?#a5o_91a0j2@Dz zjMXH@gNGlHt<&r1hHguBG1b<}24n}iT!+$mpt^O*>&gV&K?Y`KO*l6p>|JgCr9JW0 zoOxs*OfB+%=Pq5Qx`wBsJ#Mqz8HIAaW8)AzJEIpF^+ws)B`sgPWJ$yI-D4cKdMgyM zv*o)vI;tXb0LvnIMI9Qr zPq3zlu7~hLr^+>Nt-@7n*Q$l*d7*e2cJHpi3-GHbHAJOAk73(oed3gw*RieT!QOV^ zTM{d7wTmfXNXuYw1bVG8X+Etv^K{J4-5?aW@B3-|7| z;0KrRGmKc37U{8Glsz3Ts_KKf|^9jNs4Xkf31QC$}G`5CXv#@)RIGl*8(T6L)?ShkS` zBfaf{GyvZ8gLD`4BIo4KKVuyYtCU)FpQvMYl9u!4&OwzMFz*~K!>5)A5vf|)B1k8E z!Q#asbeG%K=Ar&f@$pduVdUoJSs1F7P@mIT(;g+EgWq^wdLI_H6g?XTp&JSaH8{oq z?@d0n3D!Xy;6~K7$D>Q!c}u%#aPG%j$?6(*PDVyX%3Qu|+Bwso4eQzd9{h4sBpkiD zKW(%llxf#Mx@;(ZXw9mj@Uf*|zvke^@H_l1=H4rc8GBye#zAgZxgfgxjzZ7B4B`2! z9x(oO-hTV-H|6z{4vy(+T-kugyPGPX;swH=bLV)+nm85=i$MP7pp{j9pRKgg!|cfE zLRnXv>Vfyuh6P40{O9n^N(=;IahMJKU25$thTgHr<$7-Om|H9?MaF8si|;5$m_{5w zLPL8i|8QPESfY6KRAyFXQ>BVZ&-j-cNsi5D&YG17M!s?Dme(zycWn}_4lr?f(99B2 z(dddHI;au?KDW%z&j;@eA|0Uwc6syeo$b(}DtuFx=bcSeZ{!8plu3U_n+2+pwV-{-@n|f|8nR1?;|b$q7dSa#;)N zksJ`X+!o00Vr0DVh=|Ift^DTz&y`fdcxN|YCK&VNIfABY)IB?=mG;^ZwG-*-^u#N?;S4NK+Q$l&Z)heJLg07pT z3ZOaNx41rA#mXEPFot{V_p_@_X~EHDW>wMKw}mqXq&ytxU!jSTziGKAxLF#2qOH`& zYVzmQ#{dLT(V#_G+<>6kwlrwuz@=|p7ghA}qqOEu;a{rVoW%9S2Qvk!z;wmaZzk;g z(rVv+LrIC3kV&eB%QdjhMlWimWd7T4cCf5ly{m*D%mb{^l3=|X%Fg-XczDbOL<`8C zRSm7Jvg!7Tj*pLkRp`-b;SmlS&5_c*r9A(4W@K@}1y>f$Wry zzpbYbPj8l9rP!$ZQ$Iz~{9VILN?`>

6;@xp5-(zEsm=1`{dgGDYFE;JJ5V;o3PFLKz4FMBd85nfC-%PHQ4xC*qv~T& zC4POOB>>OKwYvw~*~yE3S~-IkC_#@)nRxW520g}%%3I_G5i$)epPu7(T))xX#{{I3 zcHlCquh-3kFo0A}r0x98 zk2w!GoGp9JX~|yZ`b3St*acG#>HHL6r$aG6l>abx<3@Q$1T4oB>v7~{u19=swqnH! zqBQtOyhi44}?!hbR_YRK_!?`sDGSE}` zA0Yeh|4)hoG6+g%G{frcsiRKN`c%LLmS%uW*MzC0^3rP6Dgv<~&}|)i%KVlucdBXS zWRIk4_Sc*Tt|`6Nbjp(*+k;A$FI%R1QpJf#VLmm#35DK-=n6u#cZ{8hBUxwr_BP=4 z_$Im%v+#s080@ErO<8*GRJX2#+{ZK4)Q*`svok(18F;pJD`H;VesUQMeHPY`_$IDC zk0d4trX_**&gfGrw&`TtS{A3!-%fmjB6=Qbq+a{@H)*YxAp9+l%b9L1XlL} zClq=g2**onHt}pr$|!HkaEE=@b`9*}F^36^##?Jgy1Hs3S!lLurAmz1biII5OyVr8 zVz*|lK3>N>rsqWywSC!Jv~}CION-apJ~@cGCCCi1SSPR@(#ceeG{Gmi9w|EZ+Ie0U z=;U3$r%wv3?5N@3G!KWQW@Mh+L?UHDh{ISgU;X;^)7YnK{Jg_Czq0yRUF>{)%_aI# zER!pzD8e05-ms_A&YHJ%v5>Q--BOB-l-<%P^n5m6G`sfd>gr|R-i|`VBXTsL`nBxZ zOc5t$rk5(&C__{5g$UgZ2~Q_+^W>d>^8|{xL;ByJO+3BWG1>||GKlAp_~kyCU*mMj zSBUDi)`8V~9|B67dpF@YW0^Z``|=zWl~dhdf2zSV!Ycix(bT1QYk}KLm7xX!-3lGY{gQVQt{{^YDX7-_%3<-+d-H zwP3g>R5vteBdL{0;IP)uS3f*QTMH1jK+VBmdn8@8uP&RGy161Daq~2 z@iZ534-DNCc7n7e?+VW9+BMX}OvWbPsjGV(rZ@XeYA6P@L4yZ35HNN7wjE&tH(-Ng z&n|qo^o1|g2oK@V;29sLQlQ?+i2_oVmX)odX$!igcHxnvbw@lif_V@Aw}ZCt*kSg) zw(Y>>I_zhQzUtPlaDb1dRbI>oD-KK7{(wbKC1P3ND)9qW0s;K?Rb=uPlJRT)upGG2EI5lcsxz*QlUi=OGza zG-)w9lI@3fLx=u8@Cb3aX!|g=u?C8c9UIa{WZt94b46uinmz{E*eKy9+uB4IAzGD% z+Hj+*3JnzT?Cs6PIHMea0&CW?WlQtIZ%3(k;MP_WiEJlLnl$ZeDGY#Yh6(=IW{6Su ziJ5`x5p~pl)O?-6&J==+4SC+%>KYcQL~tLmHhTF7h=AnIv7K|+h`9MG1qf6Qn(Ic76OXZ&6^`H)pvo~;RD$MLnL$8RxiuwcBA7k zmhT2tjudatuXMlU{f{ksnqMjcZ9}zhJZJ^79Z~$n5Br$L@>U`nF5FU%YGLB}6x+zq zWV2WgD0^&?ZLR!UQm3M!{i|%5TUcbl&6rQVphUzt@VIj5ym?0q625;Llt zTz4NIlS_Fi!2;e43dmvj@O!^m6$Bsg~I(SzR!R5g|!Wv>hNt8dUz*HkP%$_`% zaafIZ!-WaP;h3xeyk~R0iti4G(jp{;P712g1+KauI3H=JfcnY0eq94d1%k}wzr8`S zO0M1k9dYSS&$IzYw;+r){!#Q47mjkq5B(m~n~a$EX8xuvTb@G-_gVg}GaouRdMa2) z1&N|wyLRn_8^XHDgwd{@wm4j)c~MR8(XCsts>aEG<4dS!bb*9emMz8wo`w9&W`+)P zv4VzM{ZBP*@)uWLRFTH&>Ql*j;dTn9uBnn`r&-p@gdx-b)!DP(XV&zmXykMX-fH+j zX?*++L_Oa3)}FKVHV(2}%5fT0LTjn+37(@|0Y#Y{dtZvL%^x(*| z^%abj73^T=wv+tb>HhaMk&)`noj-TwaVg+pU{7i_esrdgJ+# zJB6OMep}Csm$mz;R`bc>P1pu${Jc4qfnzp`bEo9PIv6REWsN&0<2lc5;gjpANWOmx z_nP0o7u>7EMHOXiWXU@AooUyJ*|aH?SOX9?G&hgd7*n0VXPOKHNr@AJ&1KG0uz|(}x3HunBTRJdmC^!hmNQ{BxL`!XxP*i#{HzMd?Jy1fA#>&& zGWZBIslNWO{QGyA_x-H0ylImr|ADZzG?cYrc)h9n9Ou>qafJiY@aO^^MdARzQYDB9 zdf-;se*eiAf%Q~kk!~~Aq(U4nwA6KmB?8cv5y)iW6=kt*(oQZ18Xs-p<@b(G+Dl=l z7IP3?ov&9+Gg&rdt#06_=OrcZd@4*%uK4*=Mz_dKu-36lx*>oa4wIg?rp3j@P8Rz> zIq7H4+wUAR#DMX1xVl24yD zh-yT=u)`my@!wyreW=Yjva9#*?XYpsomY@F!mE-~sz+)2p5!sg65otkRUhknefil z`JPi%$C`IXnua=k=~ZEaj)pILi@~wOhYsCpdztztt4@N37;oj2LMo!mLPVg?*Mwr6aNJp)Ni z6vPW}MK~0*U_voqKwIXDTwuI-+&-Ixs#ozxNsC>RO7u3b(PZ5K903<&a3unhMQf7VU6{wXkaFevfZ|8jMzU!Ze$-`16{&jZ_1A)drjR=^Mnk!ifw% zmArTFY95so{CBM80aOQ->`P8IWawt%yix#P|G%Lj@y)U0trbFlMXTO8!L8xWn_MAgn=R%xYs}9Q$j?9Dvc~x z{yAvBN&$Gwr$yW%(-p@mt%wF0E><_N|D>+%XysSBQq^#ehOyqSnqJ%kEDBMp^S$(Q z8{Pxny6=mNS}@Jsl>2^#Zxc+xdt1poe`cvD?$t18JrpoExopI)0fqKutL8mB-_>J5 z_YosT+&(g4yQ6Um%+|;|88zJ7P18Na?z4Jz1Afz0$IXMODR_Aou)I(4_71_0NkP0Z zqEZFOAxZ4Y)vL(dN#CA-rv`_2(`?(8B>}G8loYWmIJD`0WeVcBAX@@qN^Uw|vI*r) zHKdhVjds(RJ?PXezAd#e?A=%q+XCxn7QA8Fvai#V+wV_E(C;?s{1w$s|BYb$RVemY zU6Cd4hmphiZf;cz)`W#krK48u&I~G0>ih*Uw^ULN9DoK; z@(&1@w(#0=0@dR8cZYS$KXUM(I+tgVSC^`Q`-JREf1SfN5_jDupdmDO#8BHP{n0Apc?yoKf*L ze+;v;OSOo~4*j|G^_sb?CgCc3eZqD=>!~^=JNHoBG^+G56-X&0DsFkrt=0 zGv~2E$v|3Oi!gaGK56Zx^#fs)se{&F5JcMU)?4F4bFFh-q5d#O767P5_fmQK^eHM~ zwkW2vW)<2GGMb8Cj|t*hMn5prNzH8>?-qo+2q5s%t;*+5<9vYXI6hv7qN^|Z8Ef+)6d$qhlHV~EPQ>Y+9;iE^WqkYX5d`; z=C>XpW()tsHU!b(@u_$Dp-+qY;WEtV68(nk4yP&}%^Npwt~z!YD2QwgFwLP_6hj#@8>gOj^X3_ql?edy3)oVQRCy!#YjqfM;lc$aN|vFV zfyCtGhJcFOsiW2LkJMgnlrDrm(jZ=aY-MX1@Fc<%*a1$UfB`vL3=b+ajvYnyO>NA> zVl0t46tVHxOE~k`%B{|Yr;7J z1#2;2#g8VC@)Z~b8=9Q`mckR4a|~(hV)FVMM0Lx4iw}5ypM_f-L zrd1FEi#ggS6N8-w{^8}Nz&ob9Z~}I1*)3~1_bWujn|ntSY+oVO{r=7ZKjhfkRflJL z@wZq`)KFIVindtkc5`&8_V<8yacT~~vt$8}@lM~n95=%l2Oi|OB>~lZQ+bvY;zBkc zHoWjT3%bkWA6P7Ui#oaZ2yp4THTNJt6G<+qZ+L3t0d8pi@Gd z7J7QL%jk=eNxgey;yGD2vFKKWn5Ey;zeqgwT{M^ z=ZeV^9!!w#2gwzpd9yzd2v1WxAsIH-GeB8`l9d|<$y58q#kepZ50qq&8?#+6ZuyG< zhHW>5I%eUqv#*zxQ${IAP_-`vl3EmgD->~qpon)|lUQbI@rrJ>60U+kSZJd3#;IA9 zj>4_`(@5uxmq(jjn?$Qz1=`Zlmk|OAbs=ZJA3f)&fV} zzklDST@DYy#nW>gm66*DE2&@beW5Pt%5Ywh{=ST5TqshajUvXzOpo?uZC*WwSNqW; zlgKDzeooJ#a3b)#-ye3}ae=G=3eH`!bSd_-BhCf4Fb6hL$uU2do(Bri3f2s548gEAxp7Hm3M~Ge>1C9>zeGqT(5G5&aWJCY zU5EqX)B+?t?rYiL5k@K~;ziG&ceps?7SNA@$98D6Ti-+_!KMSmVRHBI(};(Ic#AhU zUL(5YvnA^6vQeW;e<1G)Il@VStQtEv11f(IZ6>s#>|XqJM_gQp;ovoHE39Dx8)C&o zp|F`WNgpao|68CNe@yL{W%KDq2J=^KS@c8{0g?u0xc-z)W9`N^Ji0WnwsHm?ArneZ zzBp!6qR;p2=J1$YlYRN}`_KSkU>iVjg_&?Wrsp8Pu`7tOxsm ze?8$u#-}Jt0J(iz=dUN$fm;y%Rof5nTgiKrnbLQ*sgr0JK&o6-RaKq6JPiH^Iw5$4 znv)D&8!%wNi#^>T9Mevmu;;haDBeb7-RW0Hbhj^~cMN8`10HFO_5&`!muhNhWi2X- z65q@fxT;qlt12okT)WoH*w|RQLvKGP_a+h}WNo&qO`8v{k<8j1zFfLg-id<=XSVi%qq6RNTE*e^3Loidm>eBZEf&+ ziDJ-5)3mOa`$C~)c|CgYz#{8gA@4Yh!8wzeSzy}x4~)iujVZLoi96r3e?{@|w zOkmJtdu@6aXUHJT8`)NC-|-4?0u4WLVm?_K7X|Ed<66%Z1eVEj<}~4r@;lXd;^}m8 zA{3;}ebg)CNyX@Gus6joUyeRJb;4Oc3r0`WCwZb%k4XE&=k|eB0FgvGnbKH)&Tg7A zrok9>>hzrMg~p8=7u8#NpfyF*WE@p8eQfgU;y=6nIP``-Q?W&gFP*MvTfl(}hFqV% z`qj_VpOD+`KYBD3S4VnKot3~>{5}T8#zS7!Y{STeRZV292OF1-cyNer2 zhV200i7c69wHhABa`tts&Ls03p;JkXKCKqX+xGTW2Bg6PN^vh)6UJJrWy^~_2Pf5M z6tvWD88wFwo!$@yd^KG<{DO|AR?23ujJn>VOPAKF?yLLdjAe@<^x$;MaI8wA&xK@U z^qu5Vqr#n9P3^@ni>*!SZ6e=-1qj%q*WwS9F=dmF?cK8no$_Md*Y*XcY3CmleV_Ew z{6m(FtN2M+?AVyFdL$ORvj`i|)drn9g+F(@ZLzY)BPbEB9G5@COtfrufk$ZXSN9jCGHJ?HXe zbyRM*OWBz`e6jU(+V(&B{Y5^NNicj!SxpaD6Ip}lUtunMck||}kpXgu;v|s-PvsF< zr0^@)5R@!XMG!@?3D#$!p40a09FWm-55&jCDH8$3a?V1|1UCb%in@j|Ty}FnAmm30 z#(v+$i{sw5gj|Y*<@}N}ICxn_X`;dB1h_HTq;bd6*f3I=mt(-pc=gv^LB9p?B@?>N z9kHX27NkQC;s7#?OV{glyju-XcWG<&#lj#Q9gNf}ES;{`%3(3m;5kAY9&x0$PQGy4za67*&0nGE=NPGJG1m;*uSXKoN1!>5zZ-C$h@7~(J}h~_oj z#m7$I#UhbNKMeYnSas=~)(0}Dj)T0~34G0=`w9Im2OuD+g#c3cmq?8CGmY6+*&SMu zr*QCP0N#$lZTl^53@=9qM>ol~U%xZHDq+xw5gI&IsYvCWq9T*eHmIm{WHf;$R(#-} z+SAQvTVQR+x|Vfq1$>|Gynyh$5}z*H0vX*IdRfe#29=h-{pNOh>IhDY@XJy%aPN5X zIcTloyBGY$7?G6CoZkHuHN@{6{6fNKqu+mfe{aOT0FU$jeGz|hx*s`3`(kgi=*};n z-=}yuIBil_o~qnw`H9$K&6&BDCC|WGEewl@KGsR#S^+09jd1>HRBdlcVef@ky^&9V zhR=~-SpE>~sjEZ{-`;uHu;NdjCZ3oM;XHb7roL9p9Nd^==lvwU_83l*k1;!97++tZ zxFhN;(Li1M5oOPj?v^Rb2}@0&Aq(JOw>DAe{-7FxeSA(=#Wc&OgJ}Yhkx%;f8XEjl z)3f-XHD|=h{ZI<1KKQ&#VTpd-Csf_wbK##qd@yszqU4o9fELOP7SP5>JH_8>_>MlS zI3i#B*%$Tf&G8raskj4dtu6qOBuH``d$g)EpgV#~MD^PnpdYWPh>gemu1q~1aLU6d?=Lx39Zv1c` zVm39Qcv(=^J@-`}+MsYt>i0`Pc-(iSjJ~cs&Nlv#RlE+F&Fs|kHKmfs9nOykzjl)+8wj*PSif|~ z_#`nxo{4DF=%OM#AxjVFqp4a%3C}X#mN@@;^M(#>FyH5ZEw_GHD%NzEHMv>HeK6Nr zI5KP@?AUwPAhgv`lCDr1jPk38(hJr&edXaFOP3a4z|#%vO64h46WB3s)|nuk+6u;_ z*gi%IYm^hzi0-(QedzlcOE<8q5WLa#sPPj1;``nt!Hw$~k-4n3ts z69Bp`Li|da3{+1rNuk@%p1Am_j}n>_fQ=gT{B_?JyuaIZP@r=Cl{8S7{%N6pu$yj}!1Hiy^ zc|UzY2WHV!gbp@pNL<26bZ+)wB#|lqzCi! z>T`qAGcpXV(=ylv_|IGb28@>e0eZ7o_QgB&+7n!A=Jfu?pX|$gBL3995ZeoPZfEvV z6RKHT(6ID`;Zd5*gq^cn4(ukq6e0Ed^(XL&%ZhuEq`6EhHF~PIcT)Dp2@8>ceP)k0 z(l+edw-ucfY8#JYbafo~3PgYKqgB~{@?|dXU%Z*%_d#zz)ubLgIF+v;iBVjKa5!0y z9&-t)QE!|NSP>Xes0tGxLI!zs)#lB5Xdz8ViwCGVv7Tc)s;>ksV{4|@0p*u~v@j!T z7BZWsr>FPBSB#%9CzD8v*QeK~0T=Z1tT}ef0*NaG_)-GrOQqrR;Jbd?wq|d7?xWHB z$LD89U0&A@8pGicm<7kW21-KAI|Kx7ZE2BXlJbis-6O%~3l;+OyqD~}C?IzO*-=ez z1r{l1^O}SA?|b~ZNdR;PIAw=urij2G3oI;`6^&?xA!RDR6E9pa0#EZVF_4;K8G-K| z+_AmpZ@qhm@nM)#)-WOnz`@L_81?t<P0(r8jAn?XBr3^C6Vayngxq{biEq{E_bN1(a$6$(*DLLyH)gBSB%biBL zJfQ#YSKScY!@zUtD3zTtI3Mc^Uvlp4oPWGvrLYZWvD<*&gnRw;EHesaSv*3H?h6QJ z7g(nub!^7lQIsO`=ZL2l6?xE9SLKt_4Kc_CraPp5 zdU`JM%^NIXY(_rxV`=i$D_4{wcgDsB;|k#2j(>UY1{HnUw99(&f%>eFZ$YE8+NS+* zqeMp?%p+U-E;6PxMhw>L{Mf&Ru5Q+&N8_h|G;tkt<VYHEVZ=DR1x?RK zSOwcD29d9t>06mq^eu;-E&lMK2~60g>p!KB8y}6(M%>M}1>>3SL9zOrvF15E(uUR( zkB&Xwh;TOM{?a6(qZ%PL{=^LW__0D~VV5hmN}xKp0BK&z2F4;p8F~r^(}AV&Nv#Ba zu%(4pNYO{UR7F?~7B0E|KA_4>XbiNVL1Jin^)hu3lbyZcqL3 z=H}qU4PA+U!Gt4f2^lf~A#GYy_Ln$`PcJqR#tYwZ)}#K5UNwm{8q=J=fpruufJY{7 zQ$nRJo5`FBA)u~qTkAe% z50^>$JIDjY@83sKP-mfVB>yYDj?^YBTJG5K-_$SRQ?X2+iIkA^4x zTkSm}bgCD1jq=;&KyvMG7VBV_q>!dnaHG2R3RESv^4*TY= z$X&?KUlUPb$4MlBONot&nH%E28x=z|$YAToW+P~NW!z4E60v=B1+Ll<)X&I>b2(IT z4Y!Yi**D&5by19@c*%H?maXa(=H{9_BFvIEhYqx{r%Pi9Gy>5HTfa`5HtL)NYEWf@ z(GU^oj!vw(+2(Iet(u*%1yT+K>l%*gh!3exQ0;)D3&MXD_B2oh=bk)uDvZ9&jn3bC zLpZL*GbgLQSP2FDHG{bag8O>E0oCK!s7kY}I_Wss6L#@#&(Jxek%oK$9YJm;L=5$REag5tvt2&~>3 zs^=z?sEf%O_J>2-$w5m5oaSpb2*KXJD zsv^Q@iLLH;`t3qapI&}q!(rGlp|h~fDo5z!v;A^?GF5&Sj_m0lrAxWFv9n9Xv%kN8 z|89Z9Q^Z4#Rq#hGBk2`Yb@li@0Sqo?bCc>dvIx_jt#EPOzR6BlH_^=u&H*Cdu*^E4m18q>Sn$>OxPSYU>}L26Ws1 zt)@Z6e%H0TVjCCgjcsk4WY^($=b4w<|LNcJ4~3)7v}cwNVmm%i8Kr&cxBMop*3B_| zAM$5h$B_|EuB`LUzkVotk@fr6HOsHQ@45ZG)$4xc%XTfgw&>IQThUIB8O|r<7N|kS zeL%d|etU*{V!Z?hsugkKE~xx6Ev2?uwY${(G@f|Jjsk|EX;#e7wC>V@GYD$dr)Uca z3saFUFyi`&KD{CTLj&%j|4qT+il({w(#v7v$G0IUbBR~eSVG&v8fFu2(?4nFRAoj< z*O2x_wG$IyXkTSf-o^+ z4sI!~p^p138E~Ej!@^W?14ecBFq{U*sZ4l=8{AS@VI$*eDB@TMQt0iU-Sc-oSbSp> zQoFSKAta1s51esGc#<$Zz^bqtE#BX`x@A%zoW<}95Nw48RmwOq5YM#rtO-7pfX=_& z3?fVBlYpkopMO+4GeXK0>}qEdh!3&BRL_!wiQRscUpf-4!^9p(m5r!PpN<`HN_tpW z>tLDp*U6Ll)QP2eEhb3;>pGs6od5fk)C{XLwjw1s$F1A959Uj~dbFE!4?T=cLg1z6 ziv7mY%M4p3psYCIBlPQJh6N%O#N5c&NXRCoayO|S&Sm$`!CIIFhKr7gF z;hGp4=y*=d_uOAwum1h|ndVxt)^oha)MTbHfRskg-QJizHB%Ayv)?^B`#UopU}VMl zozhmOB}s3SRPlOx#v6?%CzL=~m45jWTj)WdD}#C5e9oO z+R-txJbSjy%0wN`!El&1ZK%z-_{kTu2vaZ5H{}hc`&a(y>c5`=!9ZX%_NsY2RU_@HW*J-sqX|tbJ(h=y=NbFsaKNC<-}? zQrmg)A=DVGyrYbNzNA%x9GSzpe)(!WHCpPJ!>M|&c}l7j9}2TTKfiRRUJm|S(C`U1 zX>%S}3M&K;Jma{XqT_Ak#L(HX7ia9|@cfx$OuxO4^v@t7YH9a3Gjo2a9~{#Xz&r#o z-NLuX#(1?u!@T@_({GAlY}ycy0BBxhyihgWX!>smC&ImPV$~kFb+@q4k~TFmRmH-b z72<^f)G~=7_2H}Ccxahwq<69<^3&Xk>aW+w`h9JOSf_LMH_}w7>ORZJf5xFSFVlj8 z4$ivSp>`~4&$FljEJUQTzP+#(8!kQC&J}$q9;Gw_7&yHe zN!JCJ%N|b;RiQa2In=X-i#^zTRChr_sa1>s1qSVU!uO{Sx>p-^S~ zl-lf>XE?zX6&1U^^42(roBB5=tIlkm}{GT8;^Oo_rlQe*9jiO{N3P zxsI0Eave>j3lwV6w*9Vtpeo2eXUl-UXd+Amk~!6^9-vk8gyE_ykv3dEJz@Y$V~&e3 zDw5)poi@3oxOSuzp+)8WxuEkj2@{;;6~J4v1#yii)!<88z4ADEgmfsm8U+Q_S$+)b z2SAlkOo)iL8LrZ(iRN9S1(cBgikf}i24;SGnl@#1^nKi5;y2&uY}5f3N+_!~+L>$v z?ggU~QdBe<&pG|XvffSYf!}3h929Scf2;(#5;S&mA(c9tnb|Q!2_aQ_mvajWIt&~* zA@O7oQ^v5C3svFw#Mw^&BXoWR?VB%a`_7#Gggd2)f_ciqD0 zHU%bNSb*UVMK4I5b5s96?Dn3hm-xY`=Msr7{%f2pdxK!nbsE9#jJovk==f9JaP10H z7<#`w5)%VD5-Fvxyv{_gX|&xa_IBF=el8A=kDjk`nsR<4tDZzDfyZtfp|7i(y!qFj zIPWi?cF>KF{JK|fi4)Zlcq0l9R74Ca3iksa`}ubDk-TCGCJ|7j_Qv( z(WRmffHQ~kzGD2TwdsF++!mc4bbIf}p#Xg`ZwZvefM)=xqrY{>96Kl9X9pbE5`4R4 zS##u@Jw@%hX~p0NjubCYNG`{H4RdognsFf85q|5TV!0jhYy|<+dD8jngutTZRE07@ zg0dVuwfDe*Hy5yS8R8+GmD8y=~CcDIl7|8>@z>D0vZbT{G zfpu+qHv?TSe6|^%&TiN)aioAt)^dFZD`Mg*uV<1h<}NBj3lX0yGRnfBSr?x@)2AB9 zCn)zV4u7-y3A5V>JJFB6j>bnRnM9b!h={^YJ^k+hI7kaF$xgaTHhd7-izQToj34Bv zN}VZ|Qp;!8rGg;jV3*&q*iPSoIXEQm?2@q1&_t9>5cyCEMfg~;Bv@d3B~qC_u_9rM z`G=R*U5KPtNql z-xbocmr%So3YTf$>U8_j3)yWN&Esj%6i19SdA7w+lNv?`Jt{~fPVl3xyzX$+8F!sm#E16CO`orP@*Qe ze!E$I@mrt_3X{$ZHXs&t@KBLnn zqWmUqpLQ^kQUbxkvEsy5hB8tlL}Icu3Ajd+K9hSJL;1PCJNEg>85K&E(5H2q%+Z-uscq2u}rA%QHHuTkV-dFaW zJEB;xBDq#6jb>C&nSKN99CnE5_<4gF2Wbu$GjQ@nJu-g3rApO$ltn2PjOBGkr+v z71H*`x9MUwBRou*Hm&2vEiSORoYJPSLJdIDdE$)#l(7gw4{E{k(PwgOF$Z-hP^?d7 z_$A+A=Y#436R%I=5;iLCZ3nWAy0&@utcJKz&)K5=WyyYkcYn zFd?vs5uW0P@gV7oUN@tF>3C0ZUO|;#jAvN{Nav^9|2uZm)edz?m}?d*{w$omD8foK z0P}f|`w;!dcKk6E_XD|?olTjZ`S8sBXa->kU?Z`4J1jV#b+#mGX{y3F!cC#~<> z-=Q^%m=^lb9gdSaflkuV4%KK{CwyWncu7)pSM;GcbbsG7h~tw*<;4WcwKQ>FTrwUB zE97X?c1&nPNxc1ILSw22>;odY;Y+5ORyp%Pc#^XsW-@FUm+1Klcem--p11Z{uqVwp zLQ?Yf?b}^ncu}xr7c_3t1j1#@!(L&_PK`a8kdWYfFCg^{A6U}LoahQIyPhw9cXS;2 z#yEn}qtXjyJNIw$jP7a25>RIv^ju2*VtlRWK-Rf&9!Pa*G^tKGF^iEuq@50-Yu4-u zs;fOP?nLUi*TtxVFyJXSECBj)$S6I1OH!ByR3G7GP|A^Wao=kT5oE}==+3%hf;NR) ztp{SlGwSZ?USH||s;WZG?b617fD&X6-`Y0NvmITi{7_hgEW4oHVy(wiiB<`XrN^ZR zJ#QeKAvM;U_wVZtN8BawQJSq{d--!ZKc%MyjQk?Dg-l6hKL026(QpkR+{wV(2^fk8 zkVTWv4-r|aLk;uI@K3)d4%0Gt#Sx`|rg(P(OY~d^VHDvq=?M>r96`V)t&<|+O zzKEsN#<5ksP_IqAEq3!@OLKx>usu!_D#&uK_ZfsrDZZGr6#aftIi*t;k}da6{KHT# zX-nRZiC_2e^XKccyYDHNK`>OWTnghkO?X|>h)N%XHeZ$xt_@a*8=rMy1WQbu2qR-Y z2Ce2c=P{|DwI6M1f_o0VBSai<_aC;o^PvNWEU`PS!K6sGNIsdj^1uB3K;|JxG$LgC z*M+~xMvtdIIT%Lm(&2NTZ>Ma$QozwQM9f9~8j7f%>Sl#>Ed!&;Xoh9P=`aM+(xjxQ zF^~~-HjQzZ!`l!2x$cllVtvt032~|sOp{)PD=p>f=TqGL7Uw6dd! zC2B1hPHn}Suaeqxb8~mY0GV#eq*79kIQV9mHw{^lWRSpxZ9&B;cgn`oS1BDsKhD1c za=J#n{qW(6hJ~)dmp*(&U8tgIvDR{;SU{p9I~+bPk{m>PVB%EL*<=P2`y21`YHuBA z_Tu56xa{N-H?MAiPe7LMBEUAI`_eeXpu`oU?A0Yu4zlE?R^||cHTE< z30>D04R4Xxr%<%dx$WABCm|DwAml{Wt^Pkum#AnLmGspA69!4ujMFl$JG^1om$p z`B6{bE+}h zAjCJI(fNQ&Vz6ga=JDfShh{SCZ+n*)JuGOpg>4$pH=^{>*Y9|eWMG(OtNF0lG!({V zpIbcch-Y7(w?IX@k2)h>h4enQ0jnOuhRi4#Znq~$8e&BngEiCcMJJ#Nziap2y*VMx z9b;(K66TQ+Q?%t0<(y`n90qgA>BGMo{qB7+wv*(zO+M9rlfdV zQRkZnR{J3-s755j#AF#qd~+08orA3w@dPj$`gjJYYoNd zrC3hI+#j)f1fA;Lk{rEgHIi>D;`F!L&ks-hKGPo`kE9@=>y9~@f8V2)%PU&I4tQY_GfbWT` z6mV`ke~kg`C+an24P!SQ9V{48*@eixO2fHMq%DSvuVm7yjJp$iD_@dk#1!AA+I4@7 zEM1Wk%rz=PB)We-pg%T0?_RZdDSYc{1BNRU9g)#{(KIlwq7OXz5DW{fragK?L#vD1 zft5({#oTseS{@ zG0W9ThhH0W7Nl} zAjqpWMozduM@aip{Cc)JinmqTv?!Nc!(04n8(>N#=H;d+UU3i7VNG)>h5D*Sys-vj zDpQ1LTQROUz?a@8sql(h8=XJsnX&NA?Ud#+g%-+se1#vFzIdS3P3r;wkF56&$a(+& z|2xNO7!5*ZmrE(xGoz>rSIHLK9`Bz#nN| zx$N;UHCYxx%G3b0J4jt%-!t30PLau8Yu)a!usw!z3u$U>9zM71*|Q~2fKjuLV-<2n z4aV(DV74t!e(xxOzcSE|Z^-raNu#P7R|Gf>DJmO4!zT7U&jdxsiBB~n8Sl-X zpj4kRqlALAzj+Tp)Y zgP*&s!f1Exkwkviz@#bud3hevllj!9N1(ad%U9@7)*JAH`+5V=!AnpJBCS^vpMrDp z#JvuWj2bt-3}4W&y?Z|ok3Bc9g@%N@h$ZV6G9_($^3S`ETKDYP2o>CjZBJ!Bq_?Lj zG3a$0fa%ex;Ack|9C4ur$CuG6jd=2O7rJ5oQyZ@f=g<3=M7Z;>)diwti}NMWV@^=Z z&##JLa7B{~{5PAnHD=w=Dmzdh^8YOxtIZ|92{a_Sn?HbMBXLC{MCgZUu5b(*s|Tr6 zEsjAV2snlUM|y7hol68?NG2ncp?H+g_=(&E@aig$BEWE{b3+;g8)jdF{`}j2|1qBe zorF|lHE@ZJOSroTh+*IG&N{7|I0SB}fl)5AQ(^Y*3~X0&i!NQh3Mlqd_5B-1(nTZq zF;2i$Jvi{MSqS=L-sQ6_I;JG`uUD|hSku!^RGE)De$(LVc)H;xq95|ob;a4Z#o_q0NIJ8|f+Ev?c5dtxv2EL0Iz;UgHGmfa!6Kq&gp~#Hf7c0F zf-X4lX)Z%n+`R+Qef&Cqf!(65ikmTpYlU>txY?FVuv@e8C)Q{#4GWOs^IF^1B3E4* zHb0xcM^sn_KRfUi1aO>1!ZB20VJM?(SSXMoGHyFS8{UnnYRZ(nQxt~A!8L7fhhHuclWtl z-mmC&%xS+i^S;PTm^86Mj(xpIKw&hOj&MUmyDC}>Q$nrRX?6u&2Dhp$jZ{?UL84mk zy}&{YVl>}cO#~@qm^n1^n33s$+SIE(6kfr0#llTZ*vpnaL^cD^1IUQLGyJ`5hNtVC zIh{0~bMPdFh{0cgR89zbjD~zOq)uJsuNDum@sp!CumOa;c`0r3vZK4y;&SNETYP`2 zSbnu{yx~+Wsu<$?$B|JTWIYb!fXmhzP83#F59)y6(#;!*a&w}$cxYgSrTsSzs*P>$ z<&6%hD)=kuTtN%#G%$Bw^kdnPefvItTb@j2Kd+X?3dsTDO(D$v=O3jXgr$UAK&Ak0 zjAoqD*a<#yxQRDdi3gY$@O`pPi$Y{I5<)^oTMY#8x&{WVmO3z9GARvuHDr8 zlWUvL+MxRy08rL|22)1NCu~664gIi+62F(Hm}WE*7mox!%g-`3iQ5myvcqbb5hjNu z2gA5;Z}01H3wp0sDH=~><^Xp<2E7*JjoHs{a3Hcvrk!?geZ9LGOP~}mZ}Cf3Lya%D zG&0kHq=LyZy_Suir21QZ5IPiYdJa zM3?48s;|)X5?_WCSxnFDqnEV z%g=1lB4NlxBS0foOM=hYbkOT|i#qz;Q+rA*8@P06Z)T>2Rd&SL3Kn~>a?ayn7k_XM zS1XRBT;Vw5uYw)1NO@2v9c+~X9=pZ!J{bBZo89|v^Zf8+M;HPq5gKO16VkU*2| zp_S`+uG*p2!j#8-9hg%}R;2Nq@}^P2k>#UjA@F!~e*O{nGXfs>VfIvYz9xjP{cC^` z2^UCVzXZlNon|m-Lq&p4b9C^5Wf9Rpm8GdwG1Oaf3T{8SHt20X8A)dGVDyQ<|6OUM^)B|Oy z&6hWje{h#@Nua2&s|`EA*v8zfDgp%)sR@m|BwUrIpZZ|n2%Pn&>;mBf8{@N8 z|3oh}Zo+mYc%WF(Z8AJoH@#YdhQbR(G%YV1whYA8Yj22prj3maWC9Q=!kH9qXIaDH z`S7nzKkoy^sh?7q%-&m(z&`Hvp9i_ZlTIvDCr0hsC2&VflF6@iTTpn1#;2x?&QGGB zk?<-+cgp)`<(meTsQ=N*(I~qbS_QkWEk_h&+>3Ka8k4>xy2!yfVql{3D&AP2T3n$d2b!v8VWV z3ZDH9>FEA?pRyu&pY{UjNFMOowE`~4Wc#9yK6AXl3`xs8FL8r1al-Q&0gy|=qWGd` z=yd4;BEe^GP}!=G#$ZvJf!Ru~0@=-!mIq4P3{|IdoYyj*PQQ8cPTko-o_N@%5%zM) zlXD|LkOasu(A6U`NPrsG)vD)bY7QeAB7ih~7`+6Rcl{(RU;0cy8!NK~6hL&>ZKpr6 zwXu=y8zVm}t*pkW-I)sb9@m&EGS{OGhj|F3MNOmJzi2p>dsfyFV=d~cM>J!S)R&Cm-%UlKUSyI}5n2q9h4M~u|SJ=QXm&iNDKj*l@_q3F{ zBdb@}kj9G3-q$z69w~b|*M9;~tVrL0YA<1@n(&|{K8te6QQN*>H!~U)bgy}@S5P}- zX;#dc`2bVy7PhL`HB5WqY+8fmU^ejLjgkc-ut*Z`BMIEtM$8Q)_vCPrmp&4%2 ziNZv}DAIapg*O1vW$C%w^>vp1=;50I%HH%bFpGz>i3uGn`FrBNmK3Yl-v@bkxbu({@`2>x%pEv0~Lpif+pTlJLi{M;PYG z40x$!m&QMOLE7UWS{~x11G?`wkZcTLYmh;LuF+>&PKizD`ZacqR6FD3idMsSrv;8iQ*^E}qH2CCDAwpf(w@W{FNo`X7S`^(_qZ zvU)eo$P#`{!l~(*w`CO^NAe>P3h`xhaW4zI#;xbO5VIt==%aIUO+n|XGlElp>x;H| z>C$9}Gn2R;zyx2Otfi>og$sbjQWOWCMek&L9`Ajzhn`+sJSgOIK2pym-z*5f4rM)9 z3^T5@zp#(%;Qu@j1EsB75jKzRVq@U!|B4f)pw)cAFCgMYqwxw$o|=K-4~p!~gY}Eg z7=Les<FXJz;(T7r zR=DU9D34*yIDs!#6V;343Hy9Rza?1>a#PUDgf0GwVNi+_^`KByp@HaumPDcxh7;u) z^=uSRkTf?>-cg&pBI2hwecdG^8qlNHZ}v?p+Ef`I5T2918QkyN*Mwu zRuHp;yFR2k^Ws2?^FQ1>4?q(Jdb6KFy8p}k`~VK<%ahqwFMl&}>L~O%HqiIyy1)i{ z#Gbco)-w-Y^RzsPW~XfR>EGZwKTSGIi{)TJ(LM^N$zsq2>_l}dftXBKM4PV;F$Qf{ zzwN}npfvzLfnzrMQ|3GqTI+{ojqS22Z!u(pSTBGfwVGuhACi+K!@*`h!~35^GsA+C z)oJew7v?TAjmo|Rj8O3=x3Q5?8vHm#Ix1^HbMQf6gpt8JB=Sml=&3g`-F&_>q(;c$ zKQ3J>Ozmp`z0kX8N|uzBv|UZAt-w^j>1a*!kw@DoXJSRzG+1SS1U8JA3l+1T`?X0Q znn7`tdu&oEocy5S9c&uIHr9lw#Z~Iv`L#6xXj;oMxnJF87+L6F33TsNgNC&CmRT57Nk zQMl*B{-9yrCnxJK&m}XvYt-V9)4b0zWqtZl1(&2WG1-eiVTZcKWV*Pr+^Wi2V9QBQ zO2Mc+Xe?|pNSeCLTBPH?4{r)a0{*SbQnW9lJEFy){Pl4Rxi_4Mw$1Iwnk!Eu=S#bC z#W88BOVsTc^!5DA)qqKPj9E?_L4YrrgeZHk-^6@=#d$hC(XlXu_BpH4zKz&YLOV%V zAz`xMn^UQEG}M6vT$McRNpPcV5u~$N{6v?~*o$(L8|*W|ixNVX*{XF9|69G|q%EwY zA5uT~kWKoE#ofOels6wm>IW@N7_W<$48mS*pN!rDN~<%bhY{}_a6t@1zIm60j3feO zdKNQg-7s*z9K3Y*-5WOsyH;y}6BUJMN7MzvlOrq9qigkXy0-(tO4wPEd;;<`m^$9u z%PZjOxKoq$gj~@YkOqug8>K@(!kq8Sxp%R@LIbGo%9Sg&Z*P5K^K7S?PkRO~>hT@K z2l}NVzbAkUU`F|q)S{gI>+Wq+9w%OrmQnz&+rd>tq6efe`}#KO z&%y^-1gAMC^*wxIN%Yojx?i7oFmM50Ui;Hc6&p#>ln942i;bA#3j0fMlhI9s zolYtVk3je@RjIo=wzFV_vcS<|IZfLphuA$Jx@;da|9Td@T(+EuP)xCr!g7^u(q-Ix!nfI{sGfkb z66AnHhw31U%S@dcmw*4B2g!(*BW84Wb@dgfFEv9`NwGrYaoFdhNJ)fvqWgU$uJucF zDvJ{J<0Q~SYg+nwbj(R^&!++yv)E2E89=R`K1?I0$sq?uu5A3y&#r8grxP*;7gDm2LZ^$#*2SCth3QpJ9A?=HyAiSeU? zllhGXA1)^R){#qc`SrM#jwq!+e~xxH?-CxGR_@@iEckE&^QxPe!7ejQ${f074%e3> zD%0i~lr~E#ZHjJZ(45E;A&H&Gr4X%a-i1g76lCMo@437K%YWMzT$w993^m9&0W2&S zb@L$?tG}K;1?*FNJdKqu_~F&l7W`a!S3HXwTDY#D zCyu)9$&49x7O33@Vr|jhhDK<`Yme9wG0 z9?Y%Xrft6W(!|vCWOTc`gA!RM#&$rdNbpNtj8*no?6Q&QLGWyWUUZ)bwP{$4r*AmK z!$3l8NjCnSNm%$8!K#D>rTCbrZ|$`9-(|-O`kz00H;#n3=Q7j^Id=SN@PLccz9TIf z>S})S?_H9E)$h5J+Wyt6I}}Itnl-i8r6MMyaX51Gr=DBWWExA0y2m|cK1((``sg%! zRk2)TDGkD%K*UsB&|)1j4oYV#C7O7*n(uHRz_n6H3tO(Qjym%<%RWw$H?E$i1U(BV6BeZac}rBOSKomN{eH z-OI+HFbLlfZ>P|sATgy=uREgFqNH{){^BSski|FXf~_8k8U}cLVv8U1Fg(4z_bn(V zuQgCqKaZ^0_H6ooyl^98c_E*=OuwJl@aB!ykReuw zjo$!#%3TOG`Rjhm9Z$FCS!ZYNe1Q6)$Nlj^tO3K4pg-Pmryng=(B-%7x||*xb%=}A zMqW3AoJH^I!MyRx*Bs`d=r&g{{yK_@H@E(hD~+`x4M1Ybj1%z3WNQun@|$R*$>I_; zoZbK`VS-bzb3Z5@Nf1Lk1nI9j{zSh)gFt4s5~R6iE4cPWfKKw9bgIKIE}kz0Cg{;1 zY{UjC{}M7mx@i5|->Fnz?4S@kD39;$wk#})_Y&9`N5jyO<|icwOPB|*GvaNZTY|Mx zbKSDncZ|trJ(T{`kFJZWA@HhXOAZ@eV=hiv&x8!_d?>;$nX1@(*x$uX8aU;VDiQT> zYxzqaK?9zOe}n>mlILRmx3tA1?n)qk;0ABX7`uW_p1$9kvJ{wpae2l0r=_xUnjVF_ zky#cZsqA4D+cMyqKGWej4j-|yF zKOG(@y2Yz?wX|Lo8tIuG<71*}9Qx|zXg9Y`<$M{wX<0uou}o#K{;3ybIzh~FbZt%>pFm47z#f?~?!$)*scMpkMzR252=ZKG0&CfI1JMQn-%c+4zkeuKup`EE?kB~K2S*7=XYo;< z&;HK6{YqlP&O)WxGJr*OHl>}gd3XYA9f#?h-{%AI5}3Cd{zMKFG4!4ZNJ_J-8-!;q z9odV89EeLDYT6AT;4E$WV46;r!6+@3GgA0nvfC;o2L*t6P`B4mPJcY5ihDN5&OZp# zANRuk=A)b|4%U-@{RwJCP5FeB&Zx20t6 z+5E$L(I<}=>m2{c$?17_`J@>$LJ_8j@B(d>!}TI2J>y?HN^90`v}a^_SB%*Fwt-hm zOG^ha9h$Ze-v)QSH>}2YYSz@Mu|0X@#&ZT3-X*dV#z>eEJ|W8})^PkzdRm4X3A(~0 zu$HTy1d{&kpS1M#Y^bMKF&2k7!zSyL13_v9W##>{@$yn^vTc!18|3i6i(Z#??f!Ms zafGW{AxDl6F@1AI9MiPCHeM1f$7E|Cy@h2kB-0Ld{mJ~ech8=FZwj-Sq64oQp8FPU zb(>-*7nO0s4A!s6CN8$f4dnVqKOy*?$!g~;pu8|1yYd^-uHl#)OritWFE|Jn1YWbY z^L8M(@Ps5lwcP>j!2afQse^zSQSX-9?Q`4*=*3B``drnCH7{q*oVj;;j8D7B#IOYy zn{%Hp->4g|D_+_jxl3CqPs1QKNj}{8ZmtVV&(V-UD*GFM{yF+p=^^}W-gyiz`LRu9 zvucVU#5C0xD<+}HBpQ?NX>E_G&J*P|$y>YFY)!D$Q<$E3x~;g3vFCgXmW;nMTo!vW zP1(-63?}v@wx%^~^2_Nr z?xGDYO@M8viD0N{y7-siFk*8Nft;}V#1(UpMwBN9AFWY{b~j{@4fBY2<+#T6du^bc z3a?34{W`mc}%9GTYsCM+KMh+9u&W9Cm%82csG7P!SX zlW5t8u}0b8wgp)Ur_Lv~F*jelWarn6@gPS27aovAxMaz(uk=_Z8}joc^4{KFU;hb> z9(Fs2M59hHhRbJkMoLLc+s>rt!Rj~H&+j@2w!y#{y@;>W^2Me;3dQ2K2#A2yG*%$r z^FS33AL{t-mC={woY_F$h;id~IloLiYZ9I(kuXDd^)b7@6(o*I;m}?KEw3PO^yf~Y zOE9Ix=*+1fg?fp&CyVz5ZCv)o#H>U`rrq}N+oGb4SUI&BDP2OA3U17!wehM|qek5P z5hE0OJ9h32e0uPwtb!C3*Pq>$ogj9Q8j=T9tMbl|dXlXxK#;VPtEr=hCX6GmDHv0- zXJ{ZOhy>=Lgq)*q5Esu}AUhqX!F>Mqd&(-TxOXl)&59r`c$_S8Y1}mKA~w;=iGx)B zd)d3nAKG|BJXJt^n=;QzRF8=7Dr{BC>)gB?vlZ;)0H_VnGLAck^oUV5RJ zBdp(a=K1TN*|TTGrl_oZkaU~8p@dewc^KK90Bwc~wSz-_eQTJmxb4b`EJzaqb2>ie zD7Q^V?WK>mSb(>IKKC+6S^biox!~rWb+$JUGo%E>OA}(?xPie1n5QEFWT5}Esvx(; zX)f;E*aHW&FVu8i0Lx>zxMGMh@9FLyvvxUedsNY|yV_#=-?q})0LVNq)UM~c*>T_x zJHLI5@9>;M)o!|&dsAQ22tMZn3JmS;TgtK=`)=bn){7!C5szg)xwb~U)i-X~4jp>? z$-5(edj?vyZL1?YZV?sSv#u4alD&?GW5(^ith9DH?7R{pP_|)CmyNDMk27L;4jHU) zGRLX$PmP(-xp@)yY;gX%`-f>xT7cV046Vy1(GJjhB7@;uzx2z8}qL5%3qYh7MGi0sWba!CT9~1m`ikJvaw4$9(YZ&BZCxJfC34 z;lIDMxNs#V%K1i;UgCQhD=C1HylsJ6f29V&JPuWH7&09IeD z96fG{-oBCbMq9)tPtp{AS>wd?Yf+u}kbf8-3gOW7Wc{6gKAK zpcBgpHB)vvAXxyBQ*a(QXPo$SP_PCIa(TlgH^2L(utfUijSKo2X=KKUSNQ zgW6__$-`RV<7jLDxlS;BY`IczE>Y_L*!F3Z%0Qx8H-2gtdmIl{;Dp{id&YY-+jru` zMoe0o%_1~46mx_uUvAJgzMYA8tOx zXkcKn*TG}ftZc8>sYHU=lo*F-3?DxHl5vR2d^5UvZneSN%(mQ+5R1(}8Uh*0rcseA zxgD^H={I`xw%hLMk`=Ot)e|E3)B0+7c+`mv#IR8KwKB)UccKL&ll^G%*q!io>W}F1 z07hdT9z z&upPHJ$!u9eS0>f(wY=;_tsOK)ND>>K-m}`hec{4tDwX~XbVGE;2?XbF?w=-M z%GTE7AN_vqe}?GTzf^Tbnzd_pKhp~9hM!Y-mOJhoj&(#~R)Mc5zS{Oe6u9ZmPcDgl zv`a>hL$4c$G-y`0f7S^2GJrju%#UnJ;$ONF5h6AY2v@)Is?-moNAT9rdkSMyU;7uY z45+N>m-_iMFOhg&7{TW1J{`kiyZ*p5l6=MkLv&1y&jbJHNkc|HqgzA-QD`V{^lTC< zDk|d2dR}`v_sQl({F->5qcP`6a z$j+xk;}F<^bTEBKro8v&$Q3E-(EF;I1=Be#GRCS(Jjz)B6bP_pSpp-h*qoj{^^ChN_p?tX&t`y z&B@;hjXMO%{SuePRp|^tJrZ>n$6*Yx@g-ist$p~~kg{rjF*ek@@K=LRc-DIA>GfP!3h za7;QMndu{6BrJWVvF0pMk?>m^KjwF)5xQHx0sTzCgd&FbNev&j%&9?GfeC2T2?-Nu_-_DHR+x36vCzrLT?god2F-H$>8D)tGChRH76&sUH zQw9uzg_7Mr@jt;|rcLjmIPq@5ym|Qs&bP#lE!SK3hc#O&;xJdHp<4#Y5)etmE-!g$ z=p1eEL4X6gocGrb{QVz2bc>_@L7e@X^d8R>}QQ-Ws7>XU^< z8{kD0ELy9kA5ux8g!<9Kvv4;{Kjb)Ojobc8e9?R!QIB$Vi+^jWNHXynvVPQ?$0tp1 zm?#uZo$8_k*?XotPFn%g25o41nYYGO6Z58nx_x#B#r@vc#qy(3hIk~L5czm!7fRu= zuNUdVg&^*;^01i4XdlkSFCCKN_HGzYmuq@UpEG=25R}!>5cP*!5hIpoIXB~j==P!w z$J_t;_aoYpyAl5$^eo;(gP~7y)FPUJEkWaLc8J~Ieq3@JKgqL=IgxFBrt+^PMHfx| zCITg_YrQy-7S@67Zcr&I-aT1x2g4qdoV=qz_f1crBJskfR(xynPmZqJ0t{hp9reOh zTr+oxIAAr{$=XiZ?N6ce{r684s{HHv%Ji7wvF2jS03QnUN&S#l`PPB$GqM4Fa|?nU zKoO_<^pHL?V%nrhwE%dn?Cv`Z9JnT42CjGyg+6mAx^aAbs+Eq3ovgf~!NS0P z`pySQycZe(9M`#a5Qy6%}9w0otZC=K` zZiDcuXQCLt(b_47qlW%<@HR_P=73Taq}b5&w$83Ue_zPxWrqqnYTIFO9q;;$+ITRb z$Fz#_5ad||AJRwb*3#mMq@cX(2w4&}9gU#@ zmyc|&6g2}4cgacAHqGu%bH%7YyHwMH{T=G7J%?Z!I;@0&m&{K-TL8z*aPK}G2PVob z>&6+x(Gl&)k7f!k7=FRu!`r)6xyDK$uacxl^K|Usj4yhd{h{_kKfL13VS{31*mQcZ zT}xG~?jIX7>UE$o zv|?Uw@I3CbtG$boujCxrgZeyT5|YShD3 zuNTm~#j|6st~#iSItSxw@)*y>^RE-N)Kt<*j#&-M9M& z@yLo%&kC_26F)yh?89@G)mQwcs+c*45J#v6od=$n5hN~td&Og15y1H?_`dfdYmta- zIaVCT(6K!tgS&2VZ=Zp4HlSc`vq9WCUuC{iDWj1}$U;wXuHR@W_hx)`MFZBArh!Sf zzV-f(gY}S%x_+ES6zAxYn-CcBu7`Eui9ECK(gow*vwPXgj#Zk!5bq^`gMHovN5*n% zrX>!j?XI}h01J?ERQoOaAUG__W{CrB!r)~qyXnQDbT3q9&-3;5N;pAQUmHL2RgBiP zKD>&V-o+CQ#hL}R1+u`F~XH{)_v9Hr*kEpVWJbNTaYfpo^NTo&AkHF0WF{}J2y zB(D&VlAOM^9onc?wKg@?$^1YMTlH)pG#~ElGove0+{j7cEQyR!;zt-!M1DiujT0-c zf0oa+TA@-#o;YzL%$*-~tYS%b#;*e}vRpVSaeTwFA3%~bZ?sh8cp#tb5BU%q{SV!6 zV_6-&hAI>(CpjXB$po=>t>hvYK62y%$DNGVBh@pJFn@Zsq^LNym6LwBe`GR&M>-}N z{phdgKm&Y+ZR@IqK+vLmmY}^8OqEL7#C9b-2CjzzIX5}km!Ce}QIIl_+q^ZN_?tcn zD*Zki2X1U$hii-pU3%O7x_bXCZ{<@+1e?U`QlKTxgs6opX#Ys1_Hv|zu*BO-D^`Eo zx6;x7o(p>Z7Vf8_mPJftAN)X=jnJW(OO)9 zNY?`gzMsaJM{^#4i2V||$G+#9GFc78+sTv>i~;v96`5>;AE*8Zx$4O%u!`&Y zo<=f`@EV?J3-_6_?1)_Z{y&>Tz?8!p*27V@l6%&TIHqHsPz1J3;IWy zogb!JEFU*5apC(%D~Y!aeIvm(e0{nRx2z*{%|`JJ3-N6Hm?{sD=#d%l{{8gyYuB_h z7tEhO;qnQlemMZfkADtbr@jC>t={z*7{TyIO~o2@$*VewHr4(<%B^j`weZ9R*&Ti5HY3>(rL~3b{S=uPmvw;|S%{>=p6}Q=X*Yuqfj)APOn< znVZyu2^~_oE@0qx?dXyvt~KhL)?aH#Q?RkM6@`{z`vfLxyIp=%oBg$0RI&(2nK=cI zaMbc=B#I%rk7)K#NYI5_z*V{Y?Kk&;)Bv}cTrf8_Ik_nrU@|F(?O3;`lL=sGi%$y?wqk?~@Pkt`gVQmY^DyGYJ3Q<&^w2DmS8Y2dhD zx@usMh{6SL?}ZfTqlnXzSy^EWTE#UQ1V^%Z!l7t{yAWwFXx=(>*hYsnaM7Zkk|9lB zD!XgtC6P2R&$Q}diuoG=TgiNd21c^p!~?+mnvQP4)gpYe-keSa7m8ADc;)s>4{Vsc zHS*aSenT<;7wiUI^gb{9oxlP*#J;5Vl=!rA>MZxwW5ukG%3H;lKs_JxGWuLTAyF}O z-^Py_c}HXgep1CY1&EC3^T*x0;a5{u1{;SkB3f)#doj3MN)8%^oXz3@k5$qBNcOF3 zb+cx2>i_-YIDt1cq53yFBMTOYt3RUm@t8E};Qhan09Idj0=#zeK2P(dyLracsad9n zw`FE$H{48s>(x~u>UQ&so;3qycg#B7efpz4b4nA$3r?FvuFz#Nld}v4*5Z3)$R);G z=fIf-Ow2DHdX!Q}0MlNsQ1w?G-6>4Fj7WbK@vXmqdhTSFrI@DCZ5-M02iAkk4ej{K ziCWP8b39IlSU>g^#*DPG=7~+r-W5u1YNb>LbZDe9cXD;TUV2HngBDNBd~e?#F3yO+ zd?YJM8G}S+{>-ni8B9fOPJRZhp)T8{7*vlJ3{W+AaB|WxyU#q8q{^bld}rIz!y4#@ zpGIEY=x>Drji(GR8kfLQ*kE5$QbiLgLQp{PPjHreQe1t%);ZFNAr(LV?XR}=wPk}W zpKl;7m|th@k&v&zC3(;X6oo~8w}mWd)vB7fGU@08#$Fn#`cpLYG%k@{Pjbp-ch-$% z)%0xjDV;aG;?lS%WM{S{r>QHkFv`MryY>(C3ud$mA$`|3LMYLMBy1A9yWN_XhM=856zvVzKIY1uQ zj#`D}aG>gPDa!>y-Fx=4@fZD-_`EqDlwAQ#0_Jxsrui26JqV?edo`YekFBPPCiP3Yfk7@v5hUC*{e z*U*S0By2dXrX074S-~Qe_==%+cJgvX&0YNEwejP^$#SC=o<^7;8@?Vtc60UTb&Plw z@^q97-kEf&NhhjYidr{rrBI-c6%QJ>zU6XZ!3^0C2k-arytgd3X4BQz@l`vq3L-BT zM2m$SY>#iw@6bmt_;u9og`k?^xwk7BxW3V4C%MWrd8xx6%tg(Q*dyufR+#i+49mja zb0h`v0s#_h3;4S@iKm`Cfq7P(&cY*HFYw(J7Uj&WKu9W4Y5&lk_;|%fhWJF{9oy8` zHXNXekO(y<_E-TAC6--|W{=sVKA&`5+!oWmfP(}^wa&hsot?pZz!sN%51I*120&qx zcriJ7QG{MxKfc|7321&+)G#31IUpbVM`RDbWD4WzJB8i(G*eP&CNpP6&Y3^|esGO8 z&bN@La;0wivLpoYLVZKSl7+3z0zpOZTpdXbC@q;~MI{rtB3hx9z+O>JXDwfD#iNn! zwFG8tT#X=u1cp684U$$v19q#kddec(ZvPfT{+IBN=U7Cpj}`J3j#q zLeG{u-wWHpxhoDiyk_m%wE+`8f3Kl%TF5sGs?(!#Qh)zAWGW*+1SwzT=L?!Aww^^^ zpDI?w`gCqg{abvI!SLXn8Mq!iPzp2+J5>GIvu6>xKbDam%APT;il^-Z8JpGLqRnA+ z<)X@GsiEWR=a*3x+I|0Fo8PNd)&lkn@b=UK$H-e(EqohW5gO-K+mr*3UYFaEb%47c zw3r5(3Zzr@E|sZWm$dni(q~KRfn4U5UQ<=q=t#~w<^yHph}Y`CnYC7_ zjMQXX($X^Eigf5M*%P&mx@z){XYf{kdVRMQU5q?Uv@lz;N_&td2z!}ENiBbV5m-a( zANiE*d=P>RQ*jNnz8o^x?iZ`;SxI!eM8H|F5EIe%3&O-BB7^%|D{2``F|f6LQ}}3i zuld^KPFid+ze!XSa>le0l-P&96~-QldC7~rh6Jx+ofQojG&jV*2URw7z-Fc&@dWg4 z^ADq<2|9~cCA?ak+G)1gD*YU#CNt8;d)67a<}8cG1k;QtX4Dik@LxRwnfdb0=4 z)y?gVDhMt`ygYvs80ksd_pVW{O|}j;AC`addGhj=!S{AznPYorT>zJuCvGWA&yCJ) zi-If&7|XZc-nvVQ^8(|i<4`hmXn(%8Ny>MJoB%6|_}WOQ-w1(QTtTdt>L%ezJFNYiBhKj4DR`bcr%zcvlQt@jv*Z=S zw=*@smN)DaNmmVk)&hx?md%T2m;LBYW^mgtd!O`>mF)P{&mMX^UWzbeI*r~V(kwtk zSJT5wm17V{gE5uWKz3E}gH+CX?75HX^9R`D)sZU>br@!OXzR0)*ct;S>}J7`_yeAJ zXWzcfMyRl`_fR*V^A%2d40D_} zH4LV#gn-QKfWo|prxxLnG09_KQ~$!*q$U%sZSsJVRIOLOb% zn;2Hf^&CVilv#oWKDIb}B{zZi&IOs%`90i7zkH`F2g|i%MLqOU;6!>=-yf#VOIauS zC+v}dV2+|F5z6?`p2aMFPiGyg~JbaHS`{R|v8z6EH# zMEe0_^1B~Td|@F>ICsXp@5PW!8L*DB2a5mH7C43umeeOr$q3^r+I5O#Z_K2d`<#@4 z;PmgRI(GHTYmLeo$^b~QY_Pcs`1J6HgsLC8RRfnVPsu;Wm9q*N*Rb}A2Iyt_(WVCa z96WF!(WBXxsHoN!0(MxG)nV{7Rh9zmTWnG5U?<@PyWP7e{*g94`IbJLa_5A#56|?O z&8}7e8(I1CH=Oc~1nJqJ)K;2*9|*Pw(_bpU;#w^GqNii*neS^!OY8Q(ZXCVvx_a|x z5eg>3@-ZXEt{oRp#TPa;_cFjmV7~91*~${lmooF5~N0a zb(+DR-!X;OCR{0{#q%G(>3{J%4JBy*_yK)kJSz+tr4tsl4$JhBy)F6Y2zIymR*K^! zB41t6cP$eO;BxC52461*+?#fUbUufD+2XYm{Tuh!(oTmiO&h>^NM)0n_RfxqhbQUW zJT8CPxO*e0@+v^hg!NUBdp7)b0O2bkIrY%{|EA9y&%`@&uNiLUCBapeKDzs={Q)eg~(05dC`M$$4d}{tF~QMMn;hT z2?ovo{>d>eC@4rXyfvx|7GClz#OxwD>X18$mOo|b?ynC^NBb`ypO2IJ zUT!UwqE_JNz8x=apzW0GcCI+teJLs4 zGi)aLZNqvgavJgIB&%)Roz99`HZ4EMDn)uEb%ajRxB|A)*>tFRVm*$|q^wIA{Q8?nYJebqmc88A^w|(zuUI7vh@@$O^okb;48nuo+G34T z67T;HUZje(6?&K}%rM3no!n};TN3_`6jXBDHOGWKd-Q0l=?bH~RiCIo8F)Y^r@&1z zq_|Wz-@|n(12mB0sk}LJ=g%Ji-|&g6buT4UjvQ$dG`v8a9ypCSb;(VaA3AX&ds9UN zh%sm0^t!aY=8c0X3vI6Zbe5%U^#VMkyYJJ5ZW1~^vg>EYC0YajsJTo#fnQ$#gBX5p zZEmj3ScO(2VtN&0&Kwk^nI#mFQs3FT4j{FVM_*q%aAcpLC12kLN>{pBG0hO=+7j%<8?_UYI0QUMWy z%wOSe+X#K$iev;J*?ZfXnb>(y&B*umz!?{px08sO!*rq(!}Z7{rzOi?Jj|`5K5uE%46F&OoNE1%=!n z)|`c?QcR(e)Dfi!3hjF-J7L`d$_lqQL62|bmjWg}?cNGiw8I)S4woRJDPFcTr!BYK zfSvm-+MyA?b*JrJo0ybzmGi;KP!IWP$*#0ikswu7R=l7h!!E^8pSc)s zFoJ2*w4V61a0`y1LHtD5rh0eViYHaZ#mY&|?-JS~3%XL4M>x%Hfaq%#QVbc>k;S-7 zdE(A9;pR))n@Uef;+zDU(vB1|MdeIZG`weW|AoeHhmV%#`b&RhlgP&wA5oI!11|Po zInSrbY#U~x=+?3z@TmVNK(kfzT}O`G9Olj(LM}?e^B%gE>7OVy7F*S;DF-e4^w}|B za^vuZQ3g}=xjPa0ynHy`zrnY^Ztcbwx(co$ex6)x4G6Ue!WJ)*NMt-eX}x4$qFZej zd(bbp|5bkJg>R6E2x2GJ-C?`*(aVgN_kD9A@dGBjdpB3B>wKl}kd{`jRIyXYS_*uH z`fTj}6Q!cR;#+DTF+&jwgm#=+O9u|@R|p$c z2rBCpst{LO(=heujkfxBW1+4k)s_Y2uBWFr)+yf(($7ZL27DJUjIva{J}oXQ*fKt) zIOz^bKq}5^TGzp?6yGIy!ISG5cS_mr-$FDbxRe3*1rKC`o3g1~Y6^`N%wQ=M-^K_= zX!eBetO-~^xAP~7nD<77GaENP8Se;U7U534cH_nkZ>`bl`~g9+e^CV(##&kU(;hST zj#}rXG4_KrMZFs zTT9eIIPY3^?i?{-@>Zyfq_JmI`M;MX*BjTjG3$SC7+S6%kv0G#S^JhXyRq)9Oh0jw zB1$y)uo>sM=en>HIF$D8DQk%sIVEmdAXIK;$&9fnYHLz(vBmKFmc!l-X;xEN8AsuC zALSGeD5c&Ny>)9Fw~^4Xv}Yp=c1R-rhr7tgkrzcA6@wez^u%QVcM%phcz$e6HJZPn zp`oD{N=eDS*}hEDn;238sg*6kTSbmXXT-wUbu^PP+?h>Rse28}|KXfc>p+&ky)q6% zDUqcW9QgAVIL&j|wer~$6;2({<(Iubnf-|#XCEy~)#p@6D3Q>6#(2GpHo!0Fiq^2E zCgoc$>W&!N7NHkhjkI8gaL`8lJ$nXomA9R+ggSfwx=ur6b&~LP@7S1zkY}%Ko~UA7 zdRABUF5bA;xdGCVv_vtZ1lq)ucw4k-bLnsD9BiXcNbtRY>rI+N2WG9BzcsR9d1VxX z;0>4Jx!YH-2=4lXpXR#CFmGW~Rn-wxE(G+2-I)P=q(EXb!GN0~PK&==M2a*NgIKYM zUnB&*=jS4)FB=8jyvnfnKY!+_VPXc~8IJ8>_>Btv@*ah>tyZ6=iaf-%do6GM@B?w9 zPBhQ=Js1Cz3M9clZ!#KiMhL=)Dt$*O7C{)jD?jrRU+f)TPAuZ+&R3Z3(dJQqs?d0I zlxsy0FCkNGby1;p7<*pn_5dr_-}lFzf9{W#ZsYLvkDgekSuf{($AcioMg5(?qMC<3 ziS_=Y1&Asd49m~37zF<(QP{LM0f4(qgXK1=FWQRn)6R~p=`w)#vKZI$bAGwB2RH9I zOF;pl(QDi|vvcsNt@OOm-~vuT>d4|P1Z({{m4xq{o!Cn6u5}p!7V)fq(60I+`=8pi z3%weCHuud@mReL+dd|YwR`xKpvh~t2V1#L0lHCq$hj5VdSb1)*I2oqwO5J)2Y1Ez(Iq=QBva0hkViSu%6>pmlNa0zAhY)GlWgjB0mh9K5qEv zPDtL*h+D0^Oh|c`oZc+e9JZYEA`Jyu9Y!M-2Z;=jTv5qi6Imrjdx12PT|5m z(HP*d_UvC>L+=0o2Vm1fo?1qq=^C(RbfQnl=x5pDbE3UAe$15lyxNI1>qA2WCwyM+ zpvdXUuOhqL@6714v}|}>MkGxcijKIWBhI;RK!HzF^@0yAs6He!Le%|^^9w43*4Hsf z+Cs`Blh$(v0t;IWns%Bz`)%h)>SjBkUBrC6Vdj%WrTQy>VgJE{2YuWiio)`{JOQ0V zm}~xcdD4_W2JX9EB)MwP1{mS!OJ&spB#EbVZht1Vl?kO`^S#<2Xn9LKoTkAv3QYkB zIU7qcI=cA!8Vn0%&sW!zw1v(6V++vDt)b3|JRGnch}P4L2K_k$4ywOLV=5eQ&J&@( zk=kphw)xayC9iyHYSS-YJF)av8i=yfh$xTi?SydlfcSXLqB4ZDei6Fi5At+%6^oulogOP^f}EH)iw&+Po`dP09OY{{7vwltzH zYiw4~@2va3(5fCu_|&1u=t)$lQ>?h+s5#rZxWo=-Y_Q z(-l8|k~|a$F@W<|HY6-fFkU=Z@vyc<+qT}9m5u2uWVwqlYy{Hr!XI=@?J(fYlQSJa zO4p&b*`<_e?hnZ&C!g59VmcZJltbD2cACGW-J?2VfQEq%@V}fSc^J*FOAI_%9k7p~ zX5G54Li(#l7tx|ytPHLkMAP*A#S3F)Z}LcMpInXlZ%2%1Tu>;9| z2%JD$b<(3*>ynzaMHuGZpXAo4>`D{*7FpHQ>#~u{j!w zTItm9DG{uole3KxWkO^9X@49D&5wr6Iqb`~y+ip!B!~Nvt$VISx0PA49HP;D5|Co% zHxc#6CneqA`gt+ZTA51TcK<=77@(%aTFY*~JGn1Ze-6!G=+(6T(7je?y3Kkfa+6=t z>@?X-ZCYp$dOgYfXCuUrJiQtU*z> zbH)|YbpxoaV*duyy`PP4_|WG=s;j=3i#P)aFa9S^y{L-fN|1FMU*bXLV%ytfS})uP zNK<$7t#zN~Z@HeatKrU(I}X)Iu~deuPZsyCtH>F^b7ZXDcV|Y^Uh{%~y^T}i`PO8Q zsgyuz7WoQd_wP4mzJ^5>bBlwCx8vya8#mVdKVWt+gU3?bz7{rDSo5W5*KWv%Z#S4z z)iY`CTFmR_LmzDMLr&k;;I11OH@7iM#zekE;OzMQ$5lkY^)bt(lN7LM8Q^tt`jWGD zf8Ykx<+8y)KaT$X{ykEvqYs_NS18%unLnd2sD z`og4fI(yw8K)$T9@lVM{hz)_>V}nGZ`2>0u`;ulj;^(NTG8Z>f>OTcWpanU+AY&PO zK$iY2&ftON+4WQP3Tzl@aM!w=Y8^0k_EIW1tZ3Vct(5bQ3qQIWKL+&ZGbQ-m`s+I7 zs9D_NH|YBnM_*s}>J$qQw5-YA@V7S5tH=O#l%)k0{xq)Fv$DMI%BwP3R=n~M#rh@O z0~fgPCpcSp`*`E8ahILz7`|p(uleRIF}SYypQb4|cwRcmQ?9Evm*I}NKXW(8Oh7&% z#KpgjvQ-q#Sb|GY$3zk8O%h|*@ou*IED)Uo`?G(#fOb@=Yaqtx7_&F2tA#X;aK&o3 z%I+mXzdG=Kc}CUsnSG;{?)(G7FU9`d3QD?t=i!e?0Ert5HLqT9%b%vLzV?9q{wYU~ zI^=wvFeYj0e!arEetwp?9gm;-AW3sz`PVqqj)cIZ?9sJL|JBgt+J_|EZXHZe43^7r8ag>zh1QyE?B@Ewsp08 z-CcYQ3~5Q0uh9Ec1IBjw^J<^BfJ1fA^~wz_Ei{y_7ekQ5w@LE`15SG43m(qWMBOP` zmBs6WD?^0zuyAc@QN?d=Z++m`Hj$4AU(Dw<`c2QJE^=PYrV)pltN!x2Vx6ToSYm!z}O`@H}i4w2UFf_Li#+i*#5MX&RcCeU^=pE2c= zH%1k~E246?1O?UNZmGx16;oQa=@mr69iNCSKE+yQ2eTPVWah>%LkzFZ<_^Dcg@f0P z%DE}+^`cLEa;*I^c-ej12gYIlAqROEbZta|OF{D-$^@am_NP05dE^9tw(QhN59QSG ztkXE@C7ye3Xcx4>$4k@auUAJ&V?!yIlta-z62T}Nt|haX%1brcGW~htd6?-8Q2ujP zEjjgXB2nXr$0ULVe;uNnMCXWab9=OHso2}&4Kp9N^&OolT>Z~qy*g0heG$wEK}F2) zHm5$$nMVpSpe0hLT5!OmtB5cR1qksJtN(w1d4rTo!Tv{0bDIBr*;wx)E2!!rJZh(0 z!V(L4SPUCZ8}{QEW^~t!AoO?kE(_IEHME3l_YSM>j8)9KR4~N4!6?a@IY$Qzv9wyM z$(uwrS`43DDGR_+NhVB|=h@5)DiL6NF9&_{9|!7ac(Arb=HrPIO+MzP3MffzsYo~k z_MT#H$dV~|xA_0fli;U>M?|CQ<_51$CPI^ir5OYPv4@QdmF*BtcVpczv+fG2HliRDOQ5NLz#>4&|4^Am@MS(U3^PkQa2hrQw#i5cEMU358%%6BdT|dhgwgb8v>dDzbiZ=^c=MwJGWo22O{>Wcq%*RW7d= z-;VX(J;3!hDtZV|i8RbemLXOg9M)$ADf!AJw_O6ZuJs+rUni+U3@W%&Rs(vAXSZ|o z`vP56`1sQX;oqK1-@Fr~*#nj&V)^5*qa-`LW*wv0NbhnYtQg_U{APmT1iCLBq;fGW z89bl2$nfYsRo(0PHR2j%FqnTamm+okCLd3P78KmT{Gc^!yqFX~)O|#drs{Q@w+jqO zY?y#zziCKxC@!UcEVKe{@I_DsbxTSu7bnq5d@gDz|KY!Xdga3}TJ*ocKR9$hOhq5+ z`53-|8a0tyl54?7+AUQ*JLhYc<?J%pJ(=3dX1SQmSdT(ZS@8*K>EBdcqGWemrUS zWwaR@=zCVf|02vDX7*$^IcFe9#HMGzn}6gGy3mM8q+C}NBz>6G%G$a`?y}OcQD=yM z-xtxIj5J}O8>cJvyq+UI}v*=zfp&)I9Oy;t7% z_j{guxUTEI?n8Z?o*Tj&qiEjV$e38MAeuhRGH?}HO#ni(n7i#?$4v_G#&{FHThFAD z300X&NH4&JTsGn^9IU$(ZfS|D>lS8fWkZ6aV=Xouaqc%6bkeZgIBbYlwkx}2jDP{> z4AbQ8*5}60*|ei#A^n2cGJ#ifTFEcwJywqxUno;G5jF0;@ysq#3F&4RpSs_A;jT8{|UQ3mMQ+ixA@2Scx`%*puvm?bAGUL?r)>Hi@mZPm8 zfcG0aZXaK4PnQgnbq_U1Y0$(C+%v>@lBO&I*3kK3`Bi$Zwheb}g1@OgFZ8&7w|~dw zG5R?^vY{n<`;9~hWN?0cc7{*CrpPKRob@#`_Gk@FZ#*?RU=-F(_$YmIWv4M360=B8 zV2U?|LzOK)f7^65^h9{~m6lIHoq}uhWj|hoIBy>~P~ecFP_!NE>UMNgpd(rV#F^_p z=0yTiiLxbq{nhON63xLH9@Rdv9SW~Qa({e~sD%nz|HhT=6 zD7IXFr+)=$rk)LOZVN19`o7f;!dYM}4!p8Gs=Kywr0z2Jc^;49PZ+2$yxS?77sG(#dDXJoqre3!Ado{~Bn}Mx)c=(fTO75GmX&eAM62#(cF# zC(A_E)iI8Jhc0$YmM4a>BKmq_PwR4N*PmP)(uKdga?LeQ4*|Z`e{Ky++hWb5g)Ob` zzA<+nq)>cpY#LXBd4Lc-td-=oT&x*V&p+v-*bTxcZOJ-{$sKi@%v5@L>CM4Pp0b7i zGcY(=S(XVwp~A=jd{Ne}qk)nw547fDuuo*wE@_KWS*mATE*=@nAMyWxR@_IoL}$R(6l-_9uK|^u{;QL*&1yg z6teJha-PK-h^H=luBV>;<-ucP%6v~F8PHpB4l%v0g)GpMQZhI9fqz);(HpkZ4wE*c zG#A^2KP+$3aaI~G>yUx`SvbwTKRWMD6qrOtxv6Ehg@tt*+e(SI2m8>il1KKT{b-pr!g zHb1Qey}(f0U86JV96NN2*C_?}2v(tsv&DPzz=g%H`^C(w>Ji?}>^&p`b@d{}m~a`5`te(jX0%eHHp?FqNA z_KPq%8NY1M(J8&O6AUkU#XCRRYP9_Fg+qah{zx)9Isa0kUf(SfCtMq~?Z$Oa&oBLV z{w{W{^YhoBm?M?zVtcN2KcDCB{#Rbix4fA1kEgYN^#aCLCT%!r18lm>lF+XQsF~T^ zX;_#UyROujRlu{>OkWg!aSCSPvI`J6cVQwH9&3a3y~+C%Ud33PUwUGU{N<<#4^AFn z3B5RKYt>Szn=VLIhFZRdAWV4gpK^*dL#!I!!683Jbt+qHWa5$E?0qxlfgRj;Hu~U~ zZW8pB8GCq5G*#Xoafy$#RD^BmG>Tm1_>sxOz@q^HCsKY}vL~B{G-~`PULEqnRybKP zf3*dEwqUHFvO&3Q&AF`>v3tAEnyELXY{O(EeEu<$Rfi8JD+JS3O-89B z+p)o}MHDy9C3*||DI?SkNPYvsQFh@^3uku-uNtwlr4Es`d3K<}OoFHteTWt?Ez)an&Uu>6!!Q4!UXAzy%4v3de8KFz=7}fF(S|y1@U{rI2tPQ1 z{wm@N`My0?WQLyn`8`ZO`k&Z3a@p(KjDczIf1n4nMHSz9!lt;`N$02Q$m$a`2&R#K z6bd5uXWpyP%TZKXLWevHEDgzvcYA#bMh49Zl4T^Gkf=9WIaDd-(|KTW3Dt{CNqaIASB#B zr12@dybPLZQ3G%d#>I?9=xVFf$u&K)-C}r=UY7s?pe*M>@EtP== zYc}md6jR?h^kMSHZV?wP?b4fSYj4k*%<7H!4ypFz!{=03j8&E?%dSN8>a>8L3{80F z8aamC+Lsv_>@X@0^E9APM_z|@=um8b_@*LPWqA25qhYs@GArBGTRn-zH@6vV*aAUT zxLyv<|Nqem23k?c4B|}|w;C?dk;UA~iLKuWN?x z_L1IpM`j}O9xyU4pwF3;C4bZ zihP(gVU?GuIYrr1QC>I}6FlX*X)w|$INc)lx|^8jUHe-tR|+nAkO|Y?0MZvQ&U`x~ zqwJceW#X_L5~rYq_6%O^}@!%cq-6?NU`ba3YayWBgPK>SDjBt7);H?6kK8r88lHJ zQN~>EcufW-fHV^9o}O_xm|d4~`LpEpZh=Sr{X-($DXnIa7uEfqcgHU1TgE&2kB+!O;aTa5;q| zKzrcjvYg-`*B2PyJI14diuj+=6U#T-6Ti;VmTV-2OlkA@_3If13vK^BPl#jgffm|t zFlJcPum<8>KkQ<2%5Qe<)JdGA%_H%;7XjpvR$iBafPw7Nse|ozD;45=D|2-7j=FDM z^=3_NKzaQ2gG#Q-A_2e|^A~<%#*%u8St3TLN3J_W3EayvbjsWZnvFHAZ7nlLw`ke& zjOsTdpBgd_>Gv!-GqWvYKO&Dw*Hw9~I5JUdmo(J-?%zT;CPprHrJzeA*nmB?Cm*9r z(E|y!flDlQtjGHz~#JJ%xZ8cPLElee;&s*wk;bKvF z8To9&-|68tG}18qQ)Q2?$SnX{=@hC)%|$TJSma&M1S5>m*^WL3OKUQqLBEw>>hLbF zUcD+$L?}v{t+YrhQPgkJreE^X&7kMGT#=m!k_aStF18 zo1dtZm9lU=G0R9BeOa1|LXk+=BE14h{>@*1-FW}4_11)x6zi||vWa*dpJw)FT(-+j zQ;csw6-TXi`g-+$v;gyRb3T7QdHF2UX#opbUm#YUT7K$8jM z)ys5^FFd{lZDqH?yF!o_{+$F-2*cwR6i_xKn@S~r|EM+~E^gt2 zXW;+-IAl%i+!Rz$;s5p9x9%AY9yh*r{W{ylbSRV*r81m^Zn?(tQ`l(2sT;S(mhFk0*u={vayb7+7?$j=hZHM5g~%(m2P z-glWE(-6-B@T<4PE9~0=&kW-HFe2}px z@@%-R`>q2AIt4J{nXFYq6=2~p_y&teaT9x_(E2?J!dvdq~nkmu)E38ynC%8@PdN(WI?CbqodU4-mwhfS}RVLIK(u@Q5xfiM`;1i5+CZVa`|#neI+~|{`>CmR)#Gc z-TlAv8@%V#L_he1PA;MxW=i`&o8>E4CO~rP73*B0v}Y4T%F7CfkoV%}1K!qXu?{CY z%+!TfKx|G~{&*NcnHLPvvf^|AI#F{A3$Kjt55^yZfC+OnM1KMVksE&~nK|sF=lfbT z))AN22-i>sJ0Dcn8n{K2#bb}|AXlXNe2;N51fcgbyy|MWSZ_n#mJZQZ#o}YHKFW`C{s3g7uiq-D3<|J z`HJn+ey^gf4j(=tBE|SfpmE!``|{a{;Pa{NpmjERzii+HF~RWpxO}PI z>!%n|M0}xSJT^$*QP_yN|3=BpX-avaYpi|=o-aJK_FjvXvqt=PI zuN*52Xeh5ntnj4?(iZ%t0S-9ou-L&71(5&oAy2!^A>Aw7+7Z73=X)$&8Zo~@63NVC zVmI)&1^%HP%FtD;OYZF7vTfVP?=^K!@hj&5Mk~?@lxY0demsOQ!e>B!Z>GTspmOz38DlPKdQr z{i@LXZ+C{j44p$n7Ffb!r6`&OVU88Kr5M{Ada|ZUY$YcDOd(BCTWcgCh3RPEze6>k zEt;-(w7Z7J8aX%V!ej=~Fq7Hn?g#ZoF6xFlC8k9}B#JddQm z<>bDqZO%nzgl6+XVE_2Ywyb++EYxRlB@ECLFoQ8o6sgn358mDu)DaHFu!pLev5}GN zKZyH2<*ZG?gJ!Xkc_J5qd^p-WD+Vs3#;oddv|rrk+j?gmfyAcgHq2knh@O`tRkX_E z49%KNueR6QE8NFN*qKMj28Ii_{_oqU`nwhZ`}ccMfvCPztD&BMEpeX%f)=#yQw#^g z2(ZNFNtp+p>)RdaC0h|^C8aTtz;U)KMMCVT^0xTQFZcfRlSwU9rHXV;x81Acc}x68 z@Pw5uO_@Aand|$>m!gOkn9R?wFVH6@tQ+%24A{n$hiqeb{t#+q;Ezmd!Pbbb!Ut@< z-m?E`AE4SHT<#00$Z+LN$EZdAChqUfM{I8 zdi4-#@TP)SF0l(%u-IJ>qt3|uqSU`*mlvOKeoR_&KeZ)jZDZ)x`i;yf^x5M zBE4F#`p#LT8Rar^RQl#K4d7e52XrmH^@m0xYr-~0M@L(%q{HQcxczXq*q-%m@qc+Q zMGlaEJ~o$O)$5nn15`8!j3ptS`*tj=`a4R45&-GGsug4f@9d3f;$U?*hncA24|Y(u}P=n@wK4+~0Oxel>A@bFF1s9&?qd zt>fMY1{F8vnf~r_1`S^jv8CU%v97(MGpQOUeEt?5TX8zTh{+&vD2YB0KE5GKXwI!M z)=iD1@7nJ8?e&d)M{9c>IR^KD5tVk*V`?`Q3HZJ)Bd4=I*vtB;|9uxl?;rbHX+CBz z*!8b6;`m@+_s5oI3**~%zjSxMOGtpTKW$z@qPatmY+%86F`^uONX7BWX<kQ8%#AIAaL}P+HIL-gP%EUiB&9S- z!eU_FhX2zB{GC)xI%0Z2zpt4{U-;?b`0!IOL+si*XyZZA`TS^a#0CM`gW4d4(z3iK zj>m@mDX4fm5~t204T*R>({uUKr5Z_K&e}1_2HNYP7KmY2Ma-7}hr=Ku-Esw<~TK^k>|6nH(r{3;iERH3X{LDncI5XFVLVpuK2Gj}?$vA;)$`L__Y8-J$(?BA zw-H0V9Y9=`FVoPuz(MhLe0;EXBOvoN?<)TSzT+`APCtSCJwHe*M@8I@a2`}Lipu0! z(lw`!R2ZGBM(uek)J1OR=@k^$2syK*)F0qE z`rZjQ2kkcUcm7mYjlg4Qyg)V&GE8E~?B#fS`(joj*Z!l`?1G~qWPBtfQ=C4b*RA;c zm^v;j;F|&#IW@;^DrWg;ToYY4EDQTKSAFYBk^1U0c%aw`AIjc_4~JXgO&%;;-}QNWHCb0Wf~_w zmJ-JHqO_e6i`bP5+^TU?XLNtx=)TAC3b*Ly>D%7enG*dDg8^~hXMNiGTta)xN22h% zS)AcCAY18{g-}&-4XqkYm;DC6Ls@I@B$?Fhb@|&vN?Gm1;-Vsl4c-lK3xcPh(m*R3n>@)?7e#&pdorGaIorD0w3EMMrQ zT|2RpXErl=O4#FJME(IfjdRu$%COc0vBh-biMoKLz-Zl9408gy!U?*4WLFUUUX6OGj(7v(B{po(0Pq#yGz*+x=J~uF zK+z&@J>Io@cl+Y9fU=p`2TkJss{RC%NpbqfRp2X0h_(c?g@3sgXDWSYu-o z9e9RvUo$Z|G4Y-EdW+f?=La^?(FuF5e^I&BBG+A84H}!^E$AR_45=9}QL7A4D;i&w z-5(g@ZfX)+h1U4DK8_&#LE=z5aJ>Ggpncsc^I*sPnOG2y=4BfRTSB4$rzz5*w7aL( zL@h8%_`8fN_9gl6h{2z3>!-G7-?PYGEiU8vUVpUOdTa`PvWDZiclGN2S{~ayGY6b& zl?|nA z&u^SO3lLz>$PV?brcS_(`0TVRW6J+5?~T-p35@C2mTifzY~~tn37Y3~>C&ZpmFqq~ zYE3^GvurUPV^PQv1m+mZT2Ffe;9r?J_+DJxsSY6CF+EJ5g*&Dm5A4l=N4?H_w;R+@ zbUiaGHfDYWfyl-oWS;OtHkY%0XiAW_LTf4>no+$fpj}os&OR=?3pMtplkGYMW+Fcz z=9(As`up~^XmC(3@~;ft^dWEgCWmG#)gS)(XDbAmoKzgA3X}V5Hq_h@8>pB2DtV$Z zp&K+KCcTPut3Z=E8|tdbqAS&(+y?AwkMbI5nu5RuBswJqn;yzr#e!L29}c_Xp| z05SS7!@(#Kt*xygGjbk&KJOA!s!rMo`G7dIu81L8ALF6hrKjels89;7P-e!s$A3Q- z5|WPx%Y`SQKh{*hT;G24W@h^jtW}xK#iRVv_o-|Iym*gW_$+V@xXiuEN?;ZDk2lFF zPR>V8ZNir)BNA|=Vs*m{qCPb_RC7ICQts>Ndl&le-?NALs@uD(sRC@LO_Q?1t`v%7 zwKQZ_MNOJ^p4d2PuC$4Y)K!%?pC4~1!=pr}Mm3{*oX;H#FBl*=<}BRzBEG;)Y|nCI zN;B%k%nw<*MvjYWn&&!xv62NfbxHfGFQ{;qaEbchCdc@bw*%X%FON>%B|9oHjgQ*- z4L1ieL%yz7j!vTfLdqz^9@VNC>VR`!+Xk&j2fS4-gO-}(b|o}a&L{<-oidpML_A6p00)7Hn-*`ll);K>a_($#kA zYgv}mURQTW)hk0i*4kkAA2~Q0&ac377-OmjW?HJ}zu5~|l6`Dv+`ToQ53h>5x69do zb>8!?W3=1b<9a|jqM{R4Iw8RaTYi{lWTpx4D~I7IlNR_B0&Z|c zB_YP+<4u|SWh&k!HwP!V%Z&QnznA%%1aj{3pA^iwuLne7)0Xt;;lnHmc8oIG8saBY zmzYFzb;K-e;&#VTGrJ41m%5qJzxl4^DEoYVc2(Ss(+8EBIG1`$BRQN>)aG(pax{(e zhOkbVV=Q+>^v-*#ZU1cu&JG(mJU$NLIHdOe&{*dxb?f`5V+S`)%8d1_TweKz?`EC= zV(g#oXQ|Ya%}aV$z>Msy=2%uD+?;0ki}zxfuhnty$cSyf@LH;}`#o@f2#cX6>-9D= zQhf%~T$396^%(^lX!t*VQrD9#y&N&QRMpbvzBk^rHsJDEIIJGb?xz?*Rt9i&fs1)pf^*`=OwKE8`00L?c3;@7@v_z#84`EfHtH*7hDK zY8W}bVJ+scm$D&D<&1IZmcMcle)rqx=Zwr02>4Y7a=Wwr+LmA zH*P&6I^Mp&!K3ouYUUh}S&E9%&x&-wo~>b>x`(IEqO5uM<%^+z>(;IP1U4(3C1p$T zu8Tztw<|+!38ETFgRmu}5$$JHHr24@z5hd~{>`i=GOr_}(&mu}EuY{|p=~<^Xw+Q3 ztnba61J36B6L8SP&V7TAyn~nK-Q8iQv|+z7Y^5l;UAwK<4*tM)mxu2P z79t6`yBc^1gOy&j+KoSk7H$p@=D3bc-rHLpFs?<%`u+osepZHS5y1h`o96aij(-Ln zWag>jYsC14iz0vM2`zbk`n5Z|Q+v`bv@w`gdIezPF7~{^r>w;4wR?42HY^ z0Swy5qr4CDnbl@`=cvQ~&eY^@S%G@&oC~8PtnqSs@@qA#oG=g(^Ja$AKl$X=iv4LJ zbBI2p$Te=~z??EYE2E!ZX^(k$?M*YAPbtucXi0qZNc+ZuGMDj#zq3@Y{lI}U!Y>~v z${)6O-10qi5ZY7fyMFe&)z~S&x*A_@^T^}xYw8=%28#lC)lM8UcI*ge=Pwf~e-l$1 zH*0pXcl0XT?8NPF0@LU%fi7A;-CXG^knk1hFPtcG08x9#AW=MIRF z3EZ^gL0`C)XGvQ<1QSfkK)i{EQ1In%dAMdhNm|pn)b}gj)OTg;RIY(I%!7>d8{BBu z7)VtTi)=msbZb7zYVXJnGAV+*e*Ty1Cwg^iQV(YbJ6az$?zNdU(QyTJm}+`r6f$DK zR3>g4f*?%I+(Ep^pZ%o1khqG@S~-g!!Xup;e|)>e<*c3ksgU1i>8g3YZhXR}@I_-9 zf1j*RA3y52FFJha5KFDP`-@AF0%J0*{rA^1kvtnx)UK4wJ(NwBmF3q)9=}7?piD@M zv%0Pf^`OITob<`>k0KX%CA;(tkJ+{_h_>La-X3igio0x--h>pw+u zdSyOzu(!W!_FqCrU6!)%vbhW?Sl{pz;9f{6#s1T>K`Ytmy3QADDZliz=QW;z*e+AB zFIX|1m?~>~S4fM4#iUq()h>&b@W>S_DJhZJed5JMAU#~9N40OLS#9>aJ6ldoJ{Oo! zJT@}t3DMndMd8PzR%?A-e}|G^?I6eAA6*V{yMq{bB(&lT6Balb43o__T3UBE&IdxP zD>gu&vQEwqQA*9h$EJKh1Cg37OlA ziO#uCUyBvpE?9=F%sq1Opi@!QxpBeOkw|t4h-LNqnAwUNFIYN?v4gh*bDXuQPRfoB zjzusAlySVcKvskC^z(zrAV~S-jyi1Hym@}nJFV14{(e0@u?s==`PZB1F2y}QQ{~KA zvW5O=6Kk2+0o$`Hvqxi{NpytJD|rrJ2sQbr{M$ zS`Qd1V45uJqA5lEMO@D}fk%5$7L&V?H^!V{9aa(|u(>w*ayv@6b-v=m3&}FT`)&9< zN72^uVFcjSRW4k~BpzGNEuG)}HgK=-pYHV45<{8J0RySqXc98#e;S4U;$ZRH@Dob| z=clz|^^v#)C~Jv=k>5H39_W_{#Zo!^Cm;w@y;{k%tA=5d8X zfWO?PN%lB=+CWXvCzlub5r6ZyHWjppexH8K#O#;B*bt5tdg=EE8|CZ)%WZFBGI513 z7K*xq2E7T&J2gH`7Fet{VsuV{dOL(l6ga_&&TV-wpu<(od451`TFBtB@{yVpwv zLa3sw{-c1mcu9vCr`@zA{;|^2I}DY+-z7wg?DgX}Z~jzC^Lsnn-jiiov~R}_j|J7; z&umC}dHF6>N>N{ztXL7p{Bqnq9hfxY%A!m6rjf*P9z*iQZ+lyJuSK%>Dx9ntI2av&|{D(Gj5Wb&d!45!w3>R zu2X)pZc=9ikr-*bj9%MY3ff(sr9^q<`3+1t)B^Z|D+R;~&q)p3M(6&iT5z*FO)KO) zFlpMk!hR?_sQ(o@NJ<8Es_k;}FJVAX`=?H5o9jJ3wfsn9g zwsvu4pgGG8uxXl}H63H0#7&l=^JGnjaB3f~%um^9e!{?Guxee_Og+Hwo)8U!3~1i{71L?k!F|h~5 zAR+dQi?(C_j`~!6!WFUF=(9wPh}duWj0yYbnFJS=^?tZ7M0p&ZHHD!{u z$7$MSgE%{ITXlM-FjTsxxO6K>Fk+u@`-_N-Qs|yl$b!+qV z48Z5tT?_q7K%BIj;Z)sK1~LK$OC-IyX7?6KPiF8N|F$mUd+)6DWLO*I@!6n+J)I_u zgK}&$S}|}rJCpeM-tODZu_oPT^w=~MY7t)uh6Vj^5(~GrGviAvly4VX?F)#x; z-}xam(rn~9!>29BFXeE000}2plFqEEq?6Ic-E(pjg;Pi=kJ9w&x46!G>k)bHf*qL; zKB*bDn`$5ZO8h;UVV25wlZ|~Y5Ev+*SZa)!@gmKf=FJTFPf{IS+r8{O4VDR#VZ9ti zRZJen6A!HmYoulC`P-3Ow4?TxnRTjIiJs=ehbwyZn!Lgn#&i3peO;!EF??mSIPmyq zI8Mz($BQxZb}yXj3c;%Vv&ux+?GKQNPLy)C^A$Dp*G$O7|5#RtV*(;301A8Zws&on z#p5PTn~JfCT`8UHTvbk(B=F}B-;h$v*tuU!0u-2k*_VbNe zU0^59#eDysFW2lwjZC_d6+lnS9BF3v zbPt-xKB(Ia&zw|6km50v_hBZcx${7xH>}!bF#Gz{QXgtQog)}LFbQGSv=?K=12wd? z?DzawoEqx=$T?|!wHgiHPu^NKXk!Sqj;cS%E`ZpKDR6%RDXj=N_H2U!*!m z9DQZPOD90V16qvbcok(JwQ^m!mO5h_QqeZeJNJAOHe9J3yO)hFz{uNoK4nxfWgQa3 z2yxsV)1o@glDIwVmYHoF7@1lpa1sidT@p3?OSLL8*Ja7{G7R5(DqMQd#(4>K94HKz0 z?mOzwKHlC9Y{-_%gI7hEL8Ie9Fv=7AyaID0_L_ni{tb})`~R=oxog*^i%i5jS=)9K z(`x30v5W}{itIiZKB*zs*w42q6Maama2QAo`ZN-+3lP$plxa*d+Mc`;#dq94*fS;} zK^)g*q?mebQ@>A;-u*!_xA9u$fFuu?qZwvtZLPsDQDbdwb+lcweNY<5^Z>vBOdLixKuF&ze7i5^4z4An$)M%!Odr!qY@WE&Ec2YIi?d#;~C-TIrrfc??pLG zuf2Kk6g;1~+a|5sOM#u>cb7|Dba@Px)G}zxXkGzTP@&)EqT=$fFsC_SaGhiu2e@{0 z*jPq;dbzyurE#8(CCs57;j_z23S=pmxCQb`nkRd7**kI_Y?mmSEh?l=1cCsOMz%JS z4kWNBi_ZGGLrj`Siyj6oGvUz)w z2^)W#Dv^e7tcUOJ3RTYTv4dpOu#c5Pkf4_AE3G;>5ySO}7w@a-rS?RN6LNG0voRvo z(hfWl27F+bj-NmR>1^($sNZlOyi|so5^J8`A66r9 zb)O^uER62&7jX>#(`JcnD4D;ogTtXA_OBY$!*Snt0%>vuIcaBL6CqH|>_3Qe8;m6H zyfz~=MQ%F;SV9OOPz5UV)4lx;;QQ<=OP-u>;?*L zrb|SUEeII4q$PH4L&=HT$UvNNH1#LO9Ntb$&vktb$ji840T1dfhDT57)U4{fIQ*lQ$L395Q5mLZF`L#aQ8W3vtMb>-zK-vQ11$P{N|x;>HhrIV&1Xy>=A0~$;3YHS6Sudu;FM3RFS~liO+u?3`(;s)4#;6k(JbD zK-Y5UWaVBJ>bML&zelmN%A`zN1~y zb{{l;BfPhakv;#h#-A;c!Q4i%%cZOMae5|YjJVR__HyXZA-wW$Yw4woBD9(lph?_5 z;z!I-ID?lP$x{H2;XOY;1ux7~4(q_=iR^=IZy)`;_>~RAAWWUCaQg%M(YK)IeAb$x zrFtIpXY+}SZc*7i%ihlm-UzWPrdd=Y(fvJ=l{p039;T*;I^2ZOjEZT9u7al2#LKN85`UKJt`io}{hn{Nsli5CgR0RAzn>)8+tkB^iCYvjD^5%GaToj@H`ksrOd7si zl%`a5H3pUE4`YZ+ozh-C%9HyEmMX2XZm$G{qC%)8O6B8b6*f^pnf>{49)V^+pNVs; z#3q&PnLj|2@&1GCg#)Ep*XnQU4KxUNWOnVEoc-CiXyc8tAS4`i>GY@{ymNFyJ?2B> zXWIT*xiB@fF3CW@UjoK_BKFXpW`%nygV{E9QgvWnS-TcEb5S8xHtx*{n?{6=z#*$Q z_0(j;!G8}6&JNDG#vRzkMv<|GGX2|U`6u6$XFm?Q#Aqbby$m$yuF?=$JPAOrUjpwf zg1U@Utv$V=xLW;AAHr)53JMzXt&Q0;M`o>4@7IVYY1=Jd@ZZiY9Td9<_S$$*@7g$Y zmFt!~3;(|{ChbL>NSRfe(B`+}*QyUU343|*1GDAkpvg(0HjxcoC{Nt_Ux81FpuhFo z+28qP4>5Q&7<(kG^j9jjb+YoE(asyC!O|y408mDsRL5Cs0T1&f7xWJO^x6J)%t+HL-j!3AN4*z~;HsN3|p+7R_Xj_85zG`?NOS zNz{qg3QCw8l9gU#$B#{6$Fb^9a+BE98}_IuD{zPBhmPKq-3KQAF2u@;_gkB|uVi48 zM-fbmDtxg`<5V5=ux=O60TIoAb-8P!7OFOh4<4}g$t$}#VDKgoW*;j@#qTGnfK}qX zxxHzZ7RmGe1mW}g_!wV1K4?~42V+ed!~K1hhGaKK<2Z=j8u`$B7v6@8nJ3L-Jwzqq z*m_Y{UAD8LxEhLbU&=oUFpoDLwJ>>n|M_xkL15i~uS);^Yt>l*>O(SZvKCdq-vfhP z2~{FXh`@+YC3MYU+k+t_ak(awTP%yA8YrR36*-Y;t+>luA3fXv)d;O41X~>0qQOFs z+BIv=X8cX6*5UVFM>#bUms+|nZUVbzw3zSdXN{mmlV~T@flZ`CUGa)k==u5h`8`|s z;Q&#WO-vQBgS&L>NcSu;8zvEprCs~>ZG-w^BwW$AuXr)K&H9DtT8fN*QC3Wf+$E?R zVZrNn5%4PuP2UntQ>5i^NrG|`9L=lp|2R2-W2XaoALJB zx1F3EC&zMN{M*CtD1&M&_YtQ5Oy0ZBP3nma zs}Pmkf|3(M)E|QJ?}s8NEOJC(Fnw2)%db}Vr>lO z26@CDFtS&t7<>Z2Bse6JR-4+(R_kdfFF#H?2nIUWG3 zV({qm)2{qM5Kgt3VC~-inch-JpAuZjWpYnjXFY<*5-SCr23x;-1h#ZZ6E;*p2##Sj zHS{D7))prjQ&H&Y)ueWQUb+ZMw{z!7r{gva4>b@c%lRpal!IGYOL%5)ZDUJMb|z?1 zik1{T9UFh%+?jDZ-lqDjQCrN~y(y=xd~@iMa(F03r0fAj>}AmJ5l%8oSt-e^*M4)) zx`Hr^e#76tI&n)oZoN4swbaP8H%gWPjp=F@(j9z`Yqpg>v><{kTAQj@`|v*tbPM61 z6alXp;?cGl#W;`b;fRn?QlG~;BX^5V0W0llgoLLhFL?%hQcVBuP%(+hPUafz;$NQAmZ()kgDZIliHyu=*nb%Wo2=yc_Ig$up{^6OAzfYsPK|3SET1!9^ z&d!XU+U4lCFlX+=@2fclc-6{pfY3NR{mQW8|ZiV1cLl!yo+W!`>sgz7u9<}7|RDy+zk3)JPg zo`#wXxaH(5c#_$4Qs<_yf*r~RI*|V6zq#4hz)+>q8MN-|>fLd?ZLzA6Aw;o-4B3~0 zKagxy@~fCR^!gl}#^tNx8?DLugoZe_&a-|{vStBAo#c|?2`#Bu{_jks)TT=1|MG&A zjMFa;Nn$gMvJeo{hY~91;qtP7^=FqdTJ?bXpkSryhtky&XDcipf)QvebXgWWK1}=} z1igZy8A1WhVALFw!dL9C5nB{u5d7Hry^8<}bZHIis5S4iL>I{)|Vt@(8Hucrk z*n`H#3OSXo6h7g$5stE>YT}a@VV9N zemmlHflts-+CD1Mt`oy&{yzSqBKdF2KtwM~kl)ax)*__Jf`YN-4rzw0zi83o^j>}M z%A&_n=VZ+!cqT4prZ7~?NCwj zem+cn>b8&+Xmeq9we_*j({iz1WYOkE^4)@SVV6f9H)CZ$x9L~c^F3!1k6N^@y!d#~ zFuVc!^d%DC*X6^R&%4qU!*VOlu+di|duymvGI&gamrDXiN92J5n!jQ&5(#A?jn$dG zEj>-Twr^hp+++mdVb*}Lx~2AckVz%CZcq4o@SmV#E+ezW3B#-kc|NSMy5RJpTgp>&A*G@kvX7fbc-&JUcWrLz$b%nm># z#9oSHwm%#8itYg@g#M(BFhVUqAZCn1dvA>cz^1cvc-&wlo%} zr{QhnO*W}sa%Mp}?@D46he%6iEm(p{xmvk1`qz%r?=v#hE1u*9kFp-uuU@_FCPQml zD>r&=@2IVe*kq%-afWq74Q;#H^}Xs@-n1#0XLi#_TWO@ZX{4f;;cUarr{<4)Gx)ED zF8*h{hd+9^=kq4RK7RkB1-Qui6@91TBlEuXoj3Q~E zSS&gZM4UWc56|Ow<4Y?poH(H`m|)wsN9TUMM|o8_&wUPM6fxE0O%3ZQQ?{{ckg-I% z^!3-T_ZzUbxPhsu>FTngPMEhflfoBF>ch7FDbsAGPL+v#niad0qjbIZ%cCvJDxuse zmz>k2_Cn5e2c#Oe(I-!zZW^LH|NF|k+}v@KCT$^x1e40)dT%j2a`3C$E~naJ+Gf(% zHFwX;;;&q!B}Ef; z-t*N!Uq1v8V*Ipmp8*4^R(-I;49~IxwKAVI`iA@o=9EH|Ja>aCLlKCWy(;F{FJ%aE zZe@h+^p0J+e2@i8ZYrf3#siyp+E6%HuEphNzNH1g5unOWW9v}OAAOB+V~FVH-@HYO z2OAXrG6xDrr{UHg7R9Lk#V3Om1_s-}F6%{E*s-RKrd=}asne%NPVx}z69&<@WF(Lc z;G^y&j?_Cgd`GKJ{>^+Tmm9Zfvx9~WBF3nsx2&}#2T5kzI#g{v!~%G*SV-YfzKsE* z`<_$$Ysx}KX}wiZ&04lhTB`d`C`B`pe=~T|4O+qmHVrV;8*<8A6UE3T&ItBB-tiUn zYE}nQcuzZK7j>Z`vulgLK7Op#y7l1%R|kpu2mn5S^2}*>B9`I$_Pw^KJY2{-E(41( zk58c#&d$v>n=nC(xOL~OePYL$_u?npzkAVFgY6}-GEP->do-kn(9c-ONU(Ni- z@SH4Qgb@Il7r${P&oCb$17Kk@+L~jVi4g4fuy!k~n-5x7GE3Wf?+SEE$*Jd|)6(Wr zN=|{|{{3;)nB~$l)^DR(_cjia2ztCLJB=Qxx0q&@HumS>%zI$6><}6b-V?;WiKb1P zGQKxt(xj$rD`FA&M$ie5-#@0P4RLoLn46oM&p2Mf`|Ho1J(Jz15T_AT8WAruIR|zW zXsuTBV*Fb~MMd>l@^<~p;%|JCR(fs*({JzavQU4GS0p18aU3-BH!DR@gqWkO`v7xu z04t06jasBVe{P4g7B{~GX}@=^p(;5$6i0wZL6yoG zGg<@4v$na(gW{*Dsc+GN-04&K5rIFEAJL*!_9EuyWrKu(?R=Pf`IUK;%yggqrT^|> zq3~Y*E*+BLl1t{1O-j8_En2h~hmKhs_p%qoCnSV$r5YTrh(I}I&n==~J^m!aV$`Tj z!NKkL5PuN(!|s1Y9@Cr|$!8P=0mommKlHUpo*F3(>m zL)cH-T9#|#d*AtPQ11*JPWcVn80DM;5P~J|hgCm4!@V(L*eNRcv&qc#Ywy$lZ^!=h z>CVAjgBCkEy)txie^R+@n3a|Iq?_>{)VA&Xdo@EsyuRX6 z8V|>pH)h8@(UkmwR+BJFD<4O7^yJAC-GT!HJ<6K{nalisgqq@ zO{rqV2)V4x?ZBgGqzusA;QNjnlx_x8Dk5u#O&1im91`64pgH&A`S}?>GK+BOHRi7u zhjjQa;M*?Q=so|{4))6MT!v}aC@U*N$#!DgD|-4pFTY+v6^5kTytu(&)9$#SHNXLv zyyn%FD(J)>4gKxNu=t*|l7(Y*LH)-cML$5%wqzqZ%mckG0>` zo*M0D3Y4r<(>t0oBP+Tayl9v2;DBY5x?@m`@07#i~V zA*`e-#l^y%$GE2kjuxqt(>DryR0 zp0gwxPKpcTy+|#>3!o|5Qy9~MYQ4I%O!;lpo<`rIyC5)L?dG;@iAB{sB8^BKi*`a6 z#*!5-AoozsI4K($=|vBXOVi6V;qeg=@I}<=tTB`a zjsU0Wf`c&aL(rE3tr;v|lZh`G-6UGlVQgnH(2c%=c0;#gM_$YyVg*bI)^5VDSrjk~ zcdD8-^Q+3i{SCyE{y%csC%aKhh%#FBUwJl7d!go!pLi71M2Q;eaim}0zC(2zd!3CYf+i}@)5{DeV-BuU zDLWslcJ+ar1fM~Eh2F99 z6ymvac>Z&F+5TG(oNXAt)0&N!n{wI$@*f^|Q_8HA827beT(Nsc=ytjfoT$4^b{{!< z^k(Yd+S@|SY;AR9!za^8y3C<=u9x4WY14N6L1GlO;4ufAdHPQdA^KW6I(0dV!B?~x z&H*ktcK-aB_r{n%3rfYC-27&;n_EAz+%xj7qxOU%gDq|rp^L<9pZyXbgZ|m?FMoT_ zXrMWOIdD4ANIUfZmU+fy(y7m=|I+Sx z-{Rt60NiEIqP4i(y0@Ns{l@re(A)+7Y3kPluZgi zDw?9QfZwUh*HtxXziTf4Hp!#u!j{Cdc&cHY;&1a8ED(LtsYT$8Em#y3PsiK5kJ_Y3 zlSC^}eZ!E@uVU!J6o#u|)HOXxmq47FX9j^c`P3LT z{v=%xkz+on1M^kV0_}$s%5RQh3R_F0@B)9d>D;38A9d zil5b8U4^%)XT`c!eV5f_kY{|PVf4B6O?F~XJgX5%nkTFsKW^L*wnw6re&E{5 zLLZw;JAy|KhRSRYa-dU@d~CqHAByz|AR9km6NCo(H6~N#W3Z_MwLnfJ9Qz!P|RC6mk}m?VNWR( ztqjYa(o6vl*NZ-!kujA|Y>)Svm@L^mIteHu(7Z5NfUZ~$+}(7x?x{0p>i?dzLM-&S0)^% zI8m|$O4ujO%HJN4*MQsNKFIOJ-0x%U&XxlqLsWp1>O|i+vKDLEjVv?-d6CV> zV7c2}+Z}=8ngbjR89oA!0E<@@GOl%@MqODa-?(MVF8-Hc^J>F`h=|nA{cI&v6GmUA z%p+{1BUD|w20xXZoxQ|NGo>wUr5#*!a%!=V`h;>B<90Z@<<9t7vvvYh-~}Z6Re893 zy=r%{=r?ilWIMnCS(XIQC7zu^0r4?9AI!JV5NxP_|6$aipEcCZq~nT2H+>AfMRME39&1VWz2-v@cpgbD36e$=B; z>tj=N#Rl8B&^O?$U_vjPD~axJDVF^z2T+aw{FZr-L$tK;lhwqD6N7LV7ZJP!VJgBF zvged@6oJfu99W0Efdx-X{(#x6xB-t8Fi=%hRV9)3smholC*g%(6dFDqfUs7YFTs15m^7%2AGY#F@3UtvT0}GE~jC{5ku=>f_!Y>4Oo~Ib9&cV=us(NnD2)rF(D?{cc$*OWV zVnML!_49L|b@47aBx>fRrQaQ;NkXp9ija6AfUAu(HXk;u1}#eypBS2TuNr2?9|*`q z#Pw8pFs>q*^s%}fw{@$!c;CWAG1#glW`r~A^rQ0xZO&4*3m!LdqLM3f|F7MER=4ln zt4&uwY|^B?HjXKLFNO=*$2>i>(GS#A4fy?se3eDR;aX*jX5hKsFnwa7AR=1ois zU}Es8Qx*=jbWW|2?jMv`HWP64BIdcch-Q{5_%=%Y(>{Gz7zY?#O>)Dn%0ylfAl(8P z5BdIm`t_(z;1O-AKrcocxG?wl0(9$IwrjywfZ=YFLyzYMnl*7+Nm5Hp`!g*iVZ z^P4dUJVt9LP#fH(8D9PELqnNML5Ozu&p!{%*_B4Wd5e}vqgk_iJu12YiPK++;J&%K z=>ZPEd}h0-zXtOCRXr>oxOu#u}JKQ%`*mr&t9|h48*9qs{IYwKahhM=*e2_Ve4$Q>Qxe`e)@Dh2gqb?56wW>OOX#DUTkK2kdrG+t4PKOO+%(kxnkQ>vL|Be%>cSS zmtJ$egb>$nyI((KRWooyQI-&dlb>%BMLte2q*pTrG`Y>Qxw@0Pk9+>?nRqny@vIm? z;ala<(nfO{$B`qppuCWyrO~{39WDmTtLjzX5R_i_oY%rc^D%;&?2rK$i`Tz$^X7n; zFJCUH{vRh_{?_|evsvrfU0O5(osvVt>TaHNrc9^~Wuw%S3}G+%FmZ$G!%EYeHK4UK zJo2+qNm-eK5QAt;Gi4TeAE)VCoMNyPaXzSBFff@F=X4%h!5;4FYE}mgB_B&)l97>N zd@+-@R=iG$sXZu_4jipb?Jgy>+;Xs@HqEU@v(sf%fZ+y;K}a40BcB02yYcY~20LzG zR{aOsI1wP?g=o2!YbqTwCS2^YK3CIPdRYzj3*IUYGa#oFlK-tLO9ad_3;scE8tHKC1NN$pz zVe40Tc#QCz85SDam^x$s0Rt+CJEO%GTkLL~gVNnrqjlP4_4y0-lM)Ss2!Dq&w2q8n zXpV|K9^lyV<4EXLlQWj|z1=TTcOef~;wt+xjSjCp)bC!a8vUBg;yO&a4+b`@OUEZr!l_%Ij#Vy#Z zz=13=sz3*zN{aL-Z^C6QxSDSDDTDqFhG0ctk>=v*dGx-gPkYo{qdkHqN$|h=CB4AJ zBqIa$=})*N$kzTgwLar;QTkAzs2`3^+v?-fZpVOuP@e809$+F3a({iP9tjADgqynh z+%itu8l9*gKoqVLW(Gceb&{=M7=-x33xz9O=Sk-_5jP2dq^IP<<;y(s0e;`w(%*%! ztrId}%p5cXTUGiphs;HI8WK{#!z|ZCtt`NasLxYb(;eOLbFQ32st=-|m6j3g?n!+g z|K&QPhO#giF^R$!k~tmZw^d)wp>k$HRfUXg6}dpUty4(YDZ{n~Gi~aTL*anIaqcjy zgeR)z<3n_3dR~KBa-JE(-ThqlZ``P;NOoQ3-D^ttGA3$qks;%vf_8dne z8>4$moJm4gQD9Tr_ES|-SbAUL8fr!0V>{De6%zj@$HPqo& z@FB7${)g|`kNpQ8rJ$AySh47HN(Dj_cN#iV*c+4;r(2e}45D{h({GoohrXu%tXTok zk9$%2PhD{f^h90PN}1-B^5lzBWCFUQ46$)_3V~<+Di$&d>+9FA^7Iie*+B9kHEnSl z5;@{}-J_q;UC5(z?iRW#;LMpXHS~IHq(_W&os+O~(*A@?wU#X87DO#h>VSZ~>jwCcaerGIO1+`TGn>S z_)NG=)p4R;I-4F$1gH;!-cl9iL&wni?g2l!0O@htk90n7&BcEyd@CuuWveO5gG|s}`3ozogapSBVIh-5m*YUbNi5&wb zS$q1{2)!qiCq+@&Gy6G~3nX;~curE$ixCqh6hu`h!o$*MSp{ORdV>ZqcN>7ln!;ni z@?s+t3YrZoDCCd`kD)NHgESyZzj7&$q48ZM83(4XuOIAPg+yR;4QI8G!*Yz;%*z`V zFfOPpgM(6Dco74;(y z5pCrMxhh0CiG2)WEAZ$DdF1K= zJ;I69t(TvK_iI7SNvWwn$*R9yGJvD_y7$jR8W6g1Sfa?Ate3|K*SksoHH^Ng$MqJ5 zYuj1Cz(-+2`go9%SRD6dP5f)%NdXh6fiZpAZPcegeay1*(c&cqrZFGs}O%_*5YjrnZL5tz4K<2hv~GdDzn^x0lXF3o=jEP zM5aH5W=qV))?3R!MI%HQoM5g_P(c+W8QIB^V9sRoQo5OnfvEbrowVFh+H^(ExD;&h znnyh3{x*YR4Czut-vj4U+BGsTNYI8Mx&>nahVK40n1|+f;6U=YPV0}fqMst{(K_v0 zy0{21kl?wKI*LXiYr-JCHVam(B_E{t3FzFgo1YD5Y!#lW%iygMw4d86+^n9`EH2tg zWPk0es&%L9N@$G0ynUI%5S-&rVTA|goY#|7gYsE9wCe99BnZQZJ5(s-?Bu+!CzJU= z0n%Q-US}OeU0A9-<|a-3oDY&)BwciDfi90$hjxkUGtxV3?SyiV%KB?}G;T?M6arXS zM5XP77k_yf8L%bLach3&!{rMX+FMnEs?aLo15e3P5({dd7iLPAJq#Lrj?l|+N2_CE zH*<&5#?QlY-MxGFs87eaH1cj3Dl38Sr6*l?tj^ts59_d@$T7hHcNjh8E`R8btsoh# zvMEGJ`cnapQTXqT+(VP?1u~~d8WSQSR+Vd8u~X;Hs&s5%z9skphS_$}!kzGeBya1j zl%y|VWC$JT#_zQ_t2~adrtuygf{Jo^;n7CaX+L7rDCq}UHA1_CuI`9K@v0D(R-gMo zc#`c~a=%w~#^B+9w*uwM^u%X%h#^G6>)ixy#s{l|j_dT-%fB#C4i)4hYn zRr{~Ps*3tR9pjm$@DuKn1NukpDywR4%_P;UCMA&yelnMW)V;lvR=qC7#uUD*fGk#B zxqBnTcfu^%hng)|=J{oX9`5HlyHhHV?gI@<2%PFka!7{OW$@I6T3W*qbsY#bfNl@< zivMK3xYbIto)B`ZA)M~NLqV$$Fi>B{os)qx=F_0=MCvw;B;7&6N9voqI@)eBN;a-@ zmbPwbm@3s^JbE;yQB8O}@>!a)*yyM1QwL%J4Ugq629=cflpr~w?saXo+7LNlN9>n~ zmr*!8_x*u^@l%SY?%-4i+zI9D;OpL(Y0*J3`17**9sCa*SP7!mgf7SNBA-sC9E`c#;FQko$=H@Np2u*TQle>LE-`y@xG9|KxG`I?!zB|knH z6F5ake-}&omK?{}V3PWxivy!Vt9EWrpeB&4wG`ToW_B2(HFfyt(UEiniR=oOMdQVA zzg%0{OU!G9GT8k>$rf-9`c~Hy*;+)nsltf6E<<1FZt$AadKqsOJBtVbK~r~Qww2ML zBDy%#9wYA4W8&4k$h(MGpU0&jCM8x+a>umGB%#7K8MFx zhCHic6ALW5;9@JM4Wx95IdhHC%j|3LY2etgbvbLbrtiTm*<>2%>c92KfO)cQ1ZCg?RvQ{6eER5OXa%ka@AwSXw%V4BY*1A8UCB33AHQ5Z5@~)P3Hz+Yhuqud-77hc~pDc%yifzUvbDWVUZEiKE4RRX`@tfmS#5hS&IS=WU3 z=AZ?Ta6ZFF)&xOiNv&KBWFY5i8UiRwxSxo{+B5~)Bx_Rh@(Ku;=pi`jwxPm;-g32dQ(avwnpex*9pUs8rC80HH6Sl4* zbE~Bweq>95q+VI_bzM2bIsVbgZfgk?bm*qw^r!fVchDo`i+5RQ_E+HFQw9iG>qrvtn@!6mv1Cu$wdK{zs96|HZB;af}l zYxHA+ZHqSsx__|QsNSUCw$L7f_LDmo^X22SJyV^0DR)a97<>yJ#O@7Ls->L8aKaN4 zFM7DDY#o)EnHdgsiIMTwC6)f`%ix5cpA5L__vjqCE}fO?r_0+fTv+#Aj>PqpTdp%v z?2oK_4<1NksMvPe2VTgyG)X>ri0+ST5OU%O!@;fKu|q^mxtdv#pcg~5l8{dx8gOq4 zY(#Xi>(YMIc6fmr4Mt>ie7P#wK*NcCVF*?U1x~$Qy}qDpObh|tSwY4@z=HrZ`pV6q8{+yYag zA8?xIEJtSEn2t)*eRl1_LbSDRuPkgt9Zir&Nz{03!Uzfa6pVEeNQC;+b-MESc|#+= z+-^6(ms%tFDs4kEAD^j2!mj$Hm^LLX9yEFoD=5?`nMO;A``x?BI74t|RcW=OOd?Zr z2`7vr`#gT~#1aPD8@>xGc{^i1yVbAj1;_MF248`Q8=__FN~W%~`MFb@7-jbCGLqX$ zl83Rp3GYXOTDCP06*$!1CHX25fNJB$aY_--x5Atgg$z3TJ5*J*gAD*SCTFbuZs|1^ zbt-gw>tGJ8G!4FtA&w<*?q_3j=+2XMn>M{JH{C#&4_WV`Tmav0TxDfteYhnM57eXd zISv~}-bw;E({s37v+1$SE`qb7fO>7vxv0S4^_({r?X99}D#MU2utJtwV|d5@bLYmK_|`y8 zZH43#1ph$JuWUPp+{HwJoM?_L&}r8$dC^CRJ4qLER-QiXVCmYN_@T}?RZHXz!wKB) zggHA)Z5q9kjY+kq3noe)aCd)yC3Q*#0gVI<_rCGDXX*z?$IRob2;eB&g>OM4N(*p= zsZmvfGCqMV-o3nVj#R5i3y28S^eU2D&~t5<^&#tNo?gaT60cO$ZR@Ve4 z3-56Xn>J~(|K|DQ#2lj?BVh4EffFeP0(H~j!(EZ_6>=}l6+3q=t4F7pERc``o!6@1 z*^O6HGc=m$gAh-6!OSmXPAKae7<`%u2O27*0r%%WJDDI71)~ix(%oP6uby&^F%fY0 zN{i8pM2la#r;!TZM6t8Iit0BuhbTRlma=WMRg~K={5Vk-mh9YuGx6}#ZPlg&l|zjD zpk>lCSg+gGs<-GWnKo@&>g^5&DMu*eN%xCD9K{yFXQb_xuE93J>o=)voklHaDtjIK zd7V(ibn@glQ#YfOh#NO73B;YuN@uVCj~%z#0lwLPnifeixyck2?{o`esu$UwH!deZ zNtv>*e4|E<+72A*93H>A#^Hc~YVan&mxW3tLdvkc?2Eq5FD*}ytpn24IMsb+V^C?_ALjYW7J!2hn?fD{q(7@2H}LJO(`>rUtqG0NYgDiF%JC^qths~9aRXT}&An;<2AraSIOq|eEm`@_4849B2k8_<1a zECeJJEgF`@y;Ei{t*C@7sV!0f^sGdGL&e)w)Xf+bCy?*iTF3x;yK&b+vcW-l>)DXyCR zp)S67^G2DtN1Cjp9|&@jPzpwk9lQR7h5*B)Ktl3hRSrlu$DGa2_Y%KCPMcz3QI;%u z$gcKfgh<6&&1J>eaRU40y$_Dx9TLdWNj$%Py)fU`lBi@Z>>%bm_X`^T#uI&xA3JvN z^bXa|)#}J_ov>FZ>xh^b05cBP<8e`~(gd&eK0W76U?$ZIeI(q#Z)q*fGkgpw#}Z6S z>a+x7^=Niuq!OM3J5>kvmb#JSc_9|K)>VW;&@ow>Z{$~^a&wm*m!zOjEQ`a6CENppEd&brZ#X}N)G7uTP3B~gT zv4cRwz)5Q$mxpEdoCIhh`apn+0{sGzQh~cl!2#fa9-mkGZd=I4L3XCjo?V?173F_= z4g&0BH1w8l;2VG7;8#VUlQLZjZ3q~qf+rs)j30ju4FW9YM#~ci4wUQPzyFUIg%GEq zvX+OjBxc`39YXOn@I7!!Wo3uJX|!jRo6m};F2_0u4G2c2$eFCLD6cRbIdX=M38lG( zvNLcc*MMmS-k1B8FkO&A*fcn~vfTcKC0|Kc|0rHiAS2EfELg)H$ICT{FA=Poh>rsI zDScg4)y~~r^%ab*g{N!*xlZtFig;Wlyod*Bk@qn=FL4tTnJKS^Q>O#UHAxcQ{R9iSd_7wm^s$jy-bE;1i^t#Ezt}k z;fnZLK)BSf*OJzyd;Pp_=gv|}lK|0GU$bTbPhq8S3aH;mJ8_FL#Us6&kU?bM;-J9#+~jv|(5 zDpF%Zi?b9(&XP(f8)!s>I+6CTCLAe++_>fce0h;(DG}$gaf?2b6i{<4q`4ikm?7N> z3VoQt?6{33%|AWoN7QG%R; z-E<+S+cVNusenk+0p{R3_zMl{H3hR}c@)0T_9)b|h4ewJ2}C9OhQ$FUy#MIYo4#-N z!bBJ0J22n}$;DxW2l%pZpm&9|6kvnw3cISR4tK1nTwY|AK==a%Lkg|K;2Jhc_nbL*6aE-upA16;Ug^1Op_}Cf(eIMiAslZuxl& z84IZ9N8QXQ?eQAQ!7pCioe*RsVqG;v$PAVQ8N!_h54K+{N=!_=LFj)+u&RRxqrXfX zhkPYkQ|&yn^~ZssW&Y$Xpf6HRRh2uI`X!OMPJ1l@`J{_SO40P}SPXyMJPnwHq!Fdr z>q$wwT(=ldMX&kz91akZUS-V?ZSrAVn)T#5X$ce-zKlSjsvv?16|-6p^NM611_;!u za|5y<$(M-I*xjD_1viJ@1es)8tc8Rcn0jy#lR<6hU*9>t#)75|Lb|xVe_!A*mHG{f1|Go$`+BmT?6idg%`xx{(6SRs#v$h+XiI z>J_(Eon%>0T|GQJ+-2$=rP!tv80y?aOGHb}5}&y(dO4%K9Wvj1NYvWk;^wyeK0z@w zZ*?MbWWFv8sT)O08JZENW7^t%K;@EDj|ggI1+~>;;2^@nD6u8;R#6t%R%hHPm#J=* zcw+CLUaU#v2##W{sWAoQ9@cLZ1R7U}{B?}9-EHtCXgckmxfEcx>MkoJd6b2!Ce`l( z4Z2F%AJ~^5eJ$f&tSI6@h$huysjH@i@og(GX=7?gzmgwc8*$!_a zO>wV*Y}2h9I(F_nV9=n-Z6{xG!)%qWSh1g`N_%w$jz!wrh+SHA^x_Cf6AN7v(%h!4 z;)nAV!E_j0*6LdHWv(G2Pen&4-FELTjy~loMQC116~U0eI;*~FRIO2uS~yh$sPI4} zG?kXcxK!+~7Rci8$fjp~_*PJGfPxxJx`HKyKrN6myYKcW`;TZsNxPefDzzwUydCbJ zNMO-rz2$8~@N|3Fz(`%z17r85y=e^SAO36&_BsU_mdvkTty_Mw4>`9qceb={W4Bk} z)kvk)H*CPj=tHLwxuS_gj}*-0$+y?g2MZ(*pz%xvSuC?@+x{mW`Tc(xm8G0K%%Y$e zJ6c^cM0GvG_HJsz3K4RK%=DJz8UsvGsvkJw%fkHiBPKT%b|}R}8 z6UkUba>Xfc?vHa@bIrjxLK0LP{@9lNlAenWQ#{OwxCDG7Djq&&%o( zJ_w{pvs`WNb#aH;`Pq#`mjjb?G&vcj#{sY#lys)5j?(gIi5HWEiTaO_PPwStPR;R! z<1Q~~gYB66_Fgj~evuOM>%7~4QKB$Ngcd;yl|6swt>@gBF?|{u&J@5dwMlnmXsn1^!}+P=?p6tB4EfiOYPj^WNrSJc!Mur#61Yl}lki-5 zKK)f|LiS*TiBA#)kC-CL@8oG|f|FnC=m;UO+4>4wVx)!H^c+RKiI8%1r+6$TWf%J) z^aDz!q8cWQChD|W_64Vb=2kJ=5RX)Du&yyBOUh@0O)~d8h!P7$W24w&I731ghZ*id zyrAUmro9_WMW@4yj~_RYCa6<3(OO*!=PNIn!-JD3oxY5L zn;Vn!&3=IrNF+G{<%nkYA9d^5vniBIn5!<-AzO%U!GC>iT?LnAXY^S=XPISGT`NSj z^(+-tV@)a-&_R8X*=XOjt09$dR07J2+kN}Cud%UvlS=%`+9Z0?%TMtHgK0b@nW{v4 zcCb**CQR6~veg=55^;_|adilvxtN-8?}oG5RkTB(fn=9w$X*E*ou_0)kj zvrPk(+m4(O@W9LN#^d2A> zn1UY3C0gQbJD{d* zZ1=hA6@T8`!MV85WNEBnLwf`7CGU>f-|4F~fE8`l&j8_r8?H)?)e+NNhpX=0wWnEkaT# zSZgh=SZ`C0cSn+t%z+mjtyEF*x7R!%lYsTmb=xj2f`}Vr(8n{`sM5U3N{!~3K1I6H ztfw;_fJ=j>(xlG;Fknt2M!V%7f2^ZqH4u06q(qNt2}-eDbpUCg(T643`Yf{mk+Q$h zXki5NF`l&w{OZ7dTl!ab2d|J?q#y43K<@dBlb4&HK1_S+#SS-B@ETTxJ_~m zcG=3c-TRglOg^lRplGv%p1by71$7akT-Xa(FgfpQ?5++?H}%SjPU2^l=btUe{+d}A zVz;4|&m3YRou?*kL1;+?G~hyR4vy>5`gPpi2F?Xfk}pLpbD*47m{K>)jqV4iw~LVk z8vmwbZo+WGNH*K9==2pcv!8uh-j#UmbdTFIG;VTJiZZWmerv{CqtLaIc}4b*ryq_) z*Dq!7!sqT>WslicK1a3f8jlrBl_!Mrx&iXlc-glvK~@XgCCogUD5ot0gn;$wQjs?{s3Z2ug zP9t3_w-n_(>4VUaGY&m;VcsL;`5fRGSN1Y#wb!JDL*VkK_BWP&xOZS)59wLiL~SfW zZlGOgp2br*eD1a*NZaWoSO?x8g=)ig=kP?Q#)0F4w4N_Y-MHx8(Zea+V@9XMG1SRh z?438_J!Yf>o!uMSRaMIS1Mg=+B_)XV{Y4o8=TmrSn@CJe9G6jd?||rfw-+wuEPif# zF8tt#R<{q_e^%u|V&Zu{pVE!_Z2nj>55K_^ODeVSRxxb6*lOF(ora`DM3!cM?7anB zC9(ZN!F-XA9vxv*|8qmka{)1k9NP8Smy4TQ>F8`AZKzVE$9{QQ#Yo96|7a=4 zfXl8srXSH)1D?f)x_v&54g0n|)U#3A>bjHNR#xwEtXB!^LvlnxBu zvk`Aq&`mfW`Za)vg$mZXZrL#%Q&FMY1ROyXe=&8cKk(x61Vpkj3c0WX|2Xeniyf-g z)72ihL0y&Lzzt|?AdOmn5Yd7-AOoQ89DqxD-0>4|GtRUN)3h4Mqn)_y>jU~68HX-K!|nd3V3{I(P_RAetDpnKtyVuDHV++l2;?kWMWW+ z)Jh6-&WCLTFgQY}MMG{BqFR1vB(XtK{C7^41!&#i=EDM6QqBm%whg3?Z^?wx zO~lLVSCqWu&4UhV+EWk+;t5nzKi41QPq={Z+PEy|Q45nk`Dqt8i+&UnnKIIdg1FoV z-Axo@YeG5M=v-2eY<_IWC2pW%^@i#M;Ixyvf#}=Wsg+#f@39zw>=QRM z=-M_kH{??!sb{5+-J zoS5@7AE^Zd1O)MimK;jzvXleX(*@n+qg%UqSy_G{o?1xxqOS!lNHm=#acGG=^;=(~6{pIMm_oLkZA8cxSfD$v-$G z!?WzD_+>61;7rxz6rt*%MjgEZokA82IiPu)XA z{F-V#E`dwBRMwd50QEgyw6tO94y~G0PH%uXTY|9?K;13Q*)#jn(GZnTTDdXea&jnV ziJsRczA>_O;FZ(QD%?-bk6UPgY+?sA1;jmpG)6G?yNGi>372hT_g$8+CFFiRa}z)Gj$zWJxghw z%*GgiWFW3^sCrLR3OwN1mHj*i`;WK_A?6J>S_HW?Gpj8XkF$Sm<9C^k%q>VIEUwf3 z{fqb@pQp~tN9a_YHyMEFL*!u&yU!A5ihU3nj)qLk@H~%vnnTh=CR^53XNn@p zL_)V$DGG%FOQQUr?yB_L-?@CE#6?|6<-RLp7g?WS)#b;dk4P>l&NYZ5NO5Cm+=TGoe0JxS>CN0F?mT#Z`g;Gp9tONm)1FxV z^KJ8D-L1j#y>p+E&8g}B^XKn0OGr$#P3Q$e<3^o!oX&^uzutZ6`=A?YJw@{UcWZZP z16k#UJx7Cr?pjPgT)ON=qribC+DF#rti0Mjb|CfR-|g472MAeGZ5XbyH}alI=|kUg z1b5t)Fh;F=0Yr;B?SFI0{^C9P=;Z|ge|{9M^o|QTk7v~EUU1OUWSuC6(G>AduQwyu z8awSf2ABm${dwInIF1xjQ=_!^lI%D*IL-LYMt{=-X0v+Wm8?Ngu?#l)MR#)qlErg<@VS z68hWG7g;BroeRF7G=rzkB}MLgdm}Vi<~P?p$>h)uZ2~$^{QsCL&9$_$WTzarSbCnCzBMWO)fDsi>nn3R%tu{ITS~x_P(O(F_-+E{ogm`q!i)^=Pmq(QEm|0U zKZg2ayvPtJ-HlcG^?T&Mx0I>VcH*v}V6v$R?NwFxkCq?4X%Csx{=?Zs&NK8Ji{xdw zwSRl4{O`Y&{39w3j)d(^gH@%+(zjg5w|B=sQ!H=ckW-=VL>2s^%5T4-GmsuWj;|Ow z((=!*SV=?@>BE4`h`18cfHqcgX@7tD-nN@LGv-acSfu~?tW0UL$GdX}fzCx4N!B|j zt@_F6TE2a93w{bWJ%O zne*E);J59?G!G1FPLicGZkHNV7ZZqdn!p<6%hWp|M*eU2Vq^ry8Wj)FsV=z1d!6s5 zChu`$?0Q68pVSQ<5!x90ebZJV-n;=fsjc$gzQ^SQYR>OA_yq0W-`7}cYg5@kFEYll zT_%DcxO?|@NQVNmPIZw_^A6Xh`>~(&#e{JRO<|BF< zp*+lQ&A;4{OZ9}Lq?YC1tdd5HRc(cEMs%R5CPP-ZOp?vai%A(rN;lq=g3^t5tx7vQ zv1~l@*WYYB^T$}n)OW`N>y!^qoEzIYQMpMOiS$XkyQZdJNm?%vQCr6YY6cJGXC-q`~rc zTi)uYSebD2%Z1eHS%~H8Rv#Hm?_HtXYQ)Ftvtj8!g!7Z~Hmp$XJKpepQbU zz+krY3Pom*$FC&h2L|2KIlD{e&iUhRJA4nifAeXh(lnHvKI3%&!QPi2rrW^Qu)^`ui%T>tBLnuRT2 zHYTR?cTE|c(zS5_eVqj z{e9cCJ!X9m=YI25+6SpYW!Nyu@Ia((hksupGt!fXu~(*Eued(mqI7rv=1Uc7MDjlK zJGM5Cnf>qYV+zoY6MN7Q4yfZ3`Cl#+%8prANAzg^>xI^9KSlpx>{BEf{_BOFzR`H8 z)q^X7TcKm|U*E0^k^@mBZujD0-Cu13JqiA=PgPQS`c_)ZcufP!vH!YfdQeITwuqDJ z0R0ozFU`J3#=lu<{S^Wmo`32vI4V9r+oz-#9$@u<+vfE?HPP6{imR$0s6hp{*moO2hco)(E;W}0QC|H5(-Q2(t1?|KWo-0X2 z?n+W(P519{?e8ALJhz5|;QaR0h&b}Me@WicyUzYZNm+rTV^Z(ec z3oo(!^!&h;wp9vU!)z7xrffML?6$?ewcGmrQ;#3iTJa`jjLq)4DuW%HJ!^Bty5Yoj z7up`JGW*c%$MvV3Y^*$V(trh3m9O0?TS;|$`8pN0ACI2fpyHZjjmwMeoOk$jjQQ}j zhi%-;o-t1Q(zBQE^U4m1NiXsaUQpRR4_U__Wx;m^0TD{K25BB_1mC0a7id5emiC$+Q6u+X39ra1J>-KAF1zOS0h~G zJ6u`%^HIkwH}~#xqoLylP2W0~AejY2PVAfD;8XB4-2s%ka#_3m(86u$VPEmrFLKcv z@&D>t8(986WHNn%#G^IbEj9~7V;1Dc*!F4aW_OxCBB3+{Yz<--kSFD7Yd{m&v6H9? zdsF1rHbxp{JOB5mvlb0qM|9Y>^{>%8VZ~KL+ANM!G7XGM|7ElOZBfS0fmiO;dBzS+ z-^M*t+?sCF9&>;TXmls}0J5sL$U`MshAyK;Jyqk|(kEwU>}}A@JIBh{qLcRwhu(iCxI3bdgo{p-L_-L>x=38ZX>t^{ra17op&n#mS%_F zI1Q@-P`WD(*T+sNv*WgHGgPcBezxYRKG2?H)Lw?{pc^dLHFg$elMUxl3tkBGTegu`LU1TV^v%>&J z0m%pJQv4X-Ii4mw8?wfWuXaqh#~k-6h9QtRPWKq;F>cz$fBg2+AG(yxU_w)Oh+El7 zmYW~k$}h@Xs~7X(eDXeeCC2L<8St+gz&Qnd>3E2+UXXak_l}1|^ZKr##@Q*R>wmFy z-+z082`Bmyos2Y)?1Z(dZ45*p|IJjmgVf-RJ=g_@0QilOAJXpj%O$_CTudQC*Dm1* zS)Z>-OfGnC48)V2z~YZn`nK`?>rt!DsD~WzqBa7n$+HmXP%txogPabj{=g_J8D}Zr z##^W-)qB1_MQ?T=NGfq`3h$LQ`pCqhR&CpwfH=Vv+|y~s!x+C`E=pnS0hjvKp5#Is z2;?IVzm}SS&~|(Z(^|CAN@)Mrqiebm-4sY?yRX1XH49lXU&x4Z%$HmiY*#R^R5^)=!_V+Gj4Jb`YSnZJLno5o1xA>howe6udk-? zJ%$Xf6<7P9$3Unjf<0GD*d;9)GzbSg>>veH=vPzADfG()!)qx_)ho8#hpFh%H5Zy$ zI<%$R!<@g}#~nSk=vOUk5KqB$&WC50yP}c4WV^g4hn!0v$hC0NldeARV zxz?9+bjZ1+_(6}T_3c7;iREmdP}JWd=NT567{}oBOeFnZGw3`9`l|yBZL?8Fnv8D< z`m%(6%Smr(sNSxTyRP-g=vn8VHGk3LH7v?Fci)}HW+lE|5uvxM)ASKS z=`OskSJWvGD_q_a=YWXyOR z09`|XkhgqykA)vkbo$k|zS~0O_BEWz(IMS^J~^fLe9N*kS7x^T{)!h)|D{@#bDd7w zY*{ZbshjYo%4danJ)L)dD8C3ARGeN(VME854sYo~p##3Dwmh&OuOt5KaWNz!8T|F# z6%!y$VL!ttJvd-ZxxYcxPA<|x3%0Wu{(~cjymDGODcHj!dA0OBOobF-QTi0Ux4lmz zK_|NeBcq?cihr99mg6H^duMvi*Ffjq!+L)m#Yt{orJB+fuqqe5({Q^1hOF^vkAf@> z*{WWg1{)5Q7$Pc(BPLHSE?H&{KNx?v1u{FoqovT@x$Pa$(vZj1r60$Lb30<(xRz3q zaT$MvGX(qY5!;n>`Stk?WS4`nIHeYiFD(xH)CR-hp|^PU8{d`H2iey=5XQxz5`_(g8m&`HzKBK z&CDK|H(UzcwfpN`miK`9xTp?%IkGLpt$lHejdvX)kmNNirG!3Lu+B7gEXT10s;f>VGq2y5z@eJcGqfg)tv+j4=d7i|9^ENcgrCO& zE@xB7J!7)(MnP;rV9B>YN7@Pep>cew-D1p*)ysd}Xm|;88VA!6>>)e~bDm$GgvZ3G+ajAMd4ng~Rsh&HF$FtG#I7BAh zrOc?Nga`V2DQF)O`zXtT{k}gXZYjU@;>$G+o?l*uCwbl4Jb#e%i68*p5ess@JKyWF zWI9{a?9#mH$ieKTgQ0VD-Mb|(h`VA5h`v4lAiIRRumd8SdodpVkX^g{^j`1Ra{Fn_ zQi_LhyLxz)SRxiqPz)OprmVW_FBh+0d-2DM>GjhV)1K1@A<&!UYV_K|_=88vrb8tv zoG;BMVgrO?;?;iY3Vjcl8cq2cBM(%PTS)<33yz*I~&aF zkYcX8I4g~2-TaUz#3Zpu5IMq~lpSCC5xL{A7u&~9zS%i(q9@@0kY8Dq`YVH&r~s|| z*wuX{GzM)&Ezt^Dh3QYnZErA2NzY87ACOBYQu@xYLhFwgQa5oD%r%{(I5fJn3u)h^ zRPV)JNv~p_*AJ!Ck7eYLnjR+S>>~SV(!yuk`304%^qz6RE)u$VYLA^Xf3yijy@vJ? zsuG#Z{jq2-HcK*gc<+MogdVGY_2Qjc3Nf+1Q$@s|jTkEAP@JFsM8&k)nO6$M*@qYz zu}Q-Hzm_x%Uv!I*FjiZH-m}H6weX(Xug8M-EeTh!KgZCVq%kG?PX8L#^zoEEszod| zV|K{bhuSx(9A$NN&hGj)IV|j&!H>(Te}=vV*0($DGC{xc`w$@PY4Hxy|+e3R+4X=Q~7&(6YZtZ$d>rIdy5&K3|(^^@fe;uXF6LgD= z&9@i#`?Bo&wLIc)K#M*strS{J5wiiQ-Uix4o<*KKoak`OcJp$syyFovk3j zuRl2D2_}g_q#=zZYvUCt&DoMe18EF2So6>w<7 z0fEG-*jJr;r;mHz+vA+0v_Y5&tw9^m%`A8FbqAN3o`JMms+9H&a zetCMOcFwz72hotLIBIK@mO^1S2zBRui(h+9T>NUg*}0is`gX{Nm`TrtaGZ!Ohfr1C zF3#bU>CP^Er|DM?|8|YS7fKh&Jh2_i=;(+?!o3JGxMu00=<)U-s&8ZdyrRW;Gn2L< zU+(poum#zc7vf?`tEK~Tc^m}1Y1Oi&5q@}S-Q+oBXeeASfO>qEmfGlRza$ijPcs;( z)o+nT*XN5YX*Bh6jO%WwZdyT%Q0EjbU>QXJE%SrTZNCsUWbV_ZI94%DE1DksM z!B)L2%i`pO3fOF>nC>P~7^glze9rRhJ>K&lnjzzLbAIBaowQA)Q#7sn(^}4#&6o9I zj&o0;2M6p;$;lHZhA2DptrNEZ7aii{ zTyxocwdby~3Z)oW>lE;(RRG$+eeI^R)rNjn0pRqxG?yYFen>ON_d`%a{!(75 z2NTGjHu+~ikAcjArH3|Phux|^7WIu1L>*l2#=t8~4R#oTAQ#m`8l^y}C1^{rQhO(7V)SeSJ?c-E6HQ^RlE z8CPDRKcChNbMeR6Qfm;EvPUiDc%z7FMZ3o}`FNjgKs2u<$0pWTH9THSGM!f@1Y{yX zts?K?Wfvd?ZaiVYU766^tL~(E!@-(%Q8qtllJ<}QhnAAow9VX-{8*aDm|Yx0Cw5j| z>gII#jOTuZdwi>7NNQbM+a9$tvD(Q6&vWk`8Q#=2+)!YXBV*vRchn%oWLZD;EPafME6yGa5Mhsy0cSjDeEC-f&eWIIX zcmEo_J00G)#O@@MZ?Q%#aSgYj7u#l+?&_~2s9ccT*XT<=(-%x8hWq$K*>`fro&775 z=hF;fIAY?&6WX8qOVAfgejrR~^;15T2arsyEkPK6)0Idj8|Y&>a9iiANV}kQJ0BK!`IoT#z&s zOK1#mcAv;pQen0N#`NaWl9RNIgBf^g<3esCsU|=?!Z;n-_VHASf>CQM#kJ^HBiy%e zl*{-U*UmZrwpc9zEcWgxV1e%)9_B+QQxlr2aE+r8xbTv_!-M5li97J1<);vhC-(@o&>A^HGlhld2uD_6*sw)2F4$ zw?P^9a-o&V_tBQfefu{8v%TE3KPvBI;vMN9&cC-abQ{LjANMS)i22RUkm0@sU%#>= z=in!{u^rO{F=eA?fmYk%GrEvmDNNIW^Io&_Zm}=yiYDrPJv!{w4D*7=OXDld2SLJ% z4<=s9DHvB!(D0td>Bmful7a^@vNe?*GdcrFq{MPP8uH^#Oe@eL??{7U<2xOI1~hDG zOX&_Ap%Y$W$9ga_&8N7iK*ml>C?4B2`TZR2Ulzf)PO0&i#&E)?no(;zS>M{`loRX| z3-0hi`l8DBDPOu0UU1hZBdDK#~Y zsstSqYrqzmPGEqb;%=sLr5!gfvXi6?jpaT9Przs2XS;84b2`X_uKu{S7fsm}btOP3 zrz_ko;e|8R#57Z5%YAU-ow8xQy3U!r9Jv6&)>i!3Qu1+^`Z;TJ4JV@G-#$k~H)SgW zPZ_9ZHj$d-u?T#i!#(2`4~_{K(JHy<<3dL&aED;T$->)BnhlOXhBympxjhn=)SWoD zue6n4!8OY&iaLc2`Ck{Yx~4N|3rQn$iAOeJQpW2A0ID{)5;3vhxRWx1XQF=~@(~TO z)nXb1czBA4f!SHR(UbA`66!9-kj(1w zHrgq=0-F$icuR;(;(l^_1$Sq`R2;8WYLO;^HC4DQ%D>jw=QNR8w7^Odv8$hphYLNso+qce>QC8>5oqP5B(WTeM%+hi;*U% z52=!b0uxtO)AJ+ONW(qGON2qGLSX-X|M6(E+=n@7f#KDTSUw(O#iREmc&l8lj|)KJ z3(I^26?SsmAFR75tb0kmn-AOP4d2=y0ou6qC%EN?%f8%|1W67izhgtg?)Rd~^*JN! zfauJLbDIH7tAGR7mp<~R3!conSo|$xMx+y&%O3i$)Afo|^&Ck04}qC|)y+lHil*Eq z_0UleY^N9D%UPRJbeuKwJ%<$6yE66CZS@MwI2f%mHy~P4@snwJYM6z6%4iN;5C{ zYAz$_>-*fs>iG!hb!49u()!%bH8T|FNWUB$^pYMk9*TuW%+|{cEML)?B zNTo=!OKgnYNO9K>{P0bB-|lX9K#%0Xn_i*Wdp}v_XT7>e?|ZMh9)emh;cu=SYbg?|w+BFh+wu_W`?n^{D_5g!m)H)Q;c3$lx;4@}k2Bs-PV_jSCF4@LbEnx)U-`##rc6z#V>0(@1hm91wJZR#d#pkPDtTm?Ij*9 zFI&RNO7v|)v$h8&L?;RSO&qTaomsGVi}!~)PTuqG8Owr>0Ds|ACM~~Tj48pEZ8H7Z!L7}- ztU9?5I-@e+SH+$E#mn>aa60`}3|FmX$v_#$`Cgh0rt;mp=617AA0)5150x zA#K+ci&SOA4vJc$0r=12q*_|XnxlpBV>U;jGlP|r4lM$?@^>AR7%}bU_Iwc2-ajM%P{+_>Z<=#6RfpjoX#4Chxy)qk*%ZQL!S|fKAD*>`{SZrUH?&KQ&n}e zClFH;w3<=pW*$6hL**FnWus!?Z0RWjE0(lIep0!5^^*^+!N%(_l5;I%XcmSmp*P$g zaS}xSHz7yHoQdzRB>^v9Oj)#e@#qX)dfHSQG+0kC*KhQ8bMs36c*Svy(7?>Q0#5z; zksqJ*n1M8>0n+bUUW(_)pxUzP4?%%iki{72~P6SiS2b2&~ z=#SpM`6Y4_|Mc4GFoRSCQDg)}Dz5olhX`N;u2olAOVL@q44()wVZQa#XU~q$_mYbP z@)&-6Oq-92GL(l^nb4QS>{SuU{<-JNh+yt1@I5Y<8DXFjn?NHRl}2@hwhk!J5ZuBA zy*u)U>!$y?UyM##SzA{cA4YIo)r)aA3X33h{zn`nBF~WJF|>%Pik2f9zX!-^MwbL0 zI#iJ_u4pJHpmG4xf}|}pubk{LtfC8`mM`y;$y#(_L&A_r$Q2K1Q|Lr?e)3nZqX>5L zU{uyej<%#D_rSYsPu_@VOTC6$YZ+^+;oyAHXXh;_NHH&^OlD%oC3GaqH^OhTOFX%; ze&=o>EmT{ry>)94O_>Kc?eD&R)`&Mx4;X#C4#E%WNe$d0Q;y~CA#!qlh0Jgzx?K?$ z_64HkYtjm;G2VK`%g&nLl7Om_HeN*lcFbN`!Hk*{mzoD4h7801d01cWkM~EA0#0Re zCG+w^LuRy);(!96V+8Z7He5{K+u!lf__JrGZ6P{y?MB%oJQLSAxAp^peLrm8R zIE1Nb)<3Pypg5)h@WBV?u28lv%b>VvY>De7ZbU^ZPz#t!e+ycK{SkHSFN&?dye{%< z%Jt3YI^*GnCZ123`n%FdyB{NfxmOv;hUf~(j-Riug7g6~mQ~|)Dk@08e9^9amyC3c zFG$L0y~LvWb7A0pwYK( z^<$_l5joN`+=>XS-hGDwz7rRh_Y(#<=G=l zQ!tb(UBCW#+|nwQD{HE`qd!!3t5NwEforN!9x6G8G_=_R@0UI{)kM!|BlIp)Zy$<~Q-oD#DLNY}`i`Lr}mJ-x7F9Y$hD zq#AY$i`1ORsPd$sJJGBNP`OoDFd9)?0b3psuKFGYNtQnKeTkUi!??2lVR4xaJ4x z@n9n%iQeJ#@}t@kPO0(Q4E---_J#G?%e)Kp>t28vizX^IH~G>g9Alr7LPwcaPINo~ zEbTK4$INe?II|uy;EUgppgWB@YkQQXR(@QLUQkb)htMR|IfwuIqCRL|1_G0PMrNGdQ z8EfRNyxYwvZu^cM^rhFuN1wDC;mI!}-%5cbp`ffDivT%zpS7t=rxDht*o*hFnWGC~$Wbi@YoSScUQNAbx zIb$yK#<7N)1Z9u==vBj~P=a)&UlytDI|q$xcG=s)zfWVthh@AT_^kzWl@S;BF?npo zmMvR$?i)!1u^%Ud`DK>(4YWMWa55NX85hxZ;sxO>Tp1ahw9}Z0)KjLsa9%!V=FF;$ zN0cCv{C4z(O4pjH$*{P&1v#@saF)v^FaGx868n?nrCm`lmF@i;5|Lps_HJBAwSBs)Eg@?+r#(p;lragn&~}U8l4?` zUae-$&(sDgE!S9yPn9i+w7eg7;Z9;AQ)e6VtZwY>r%}`~T`$pvfoh3%Ar(1zt0;k8 zqeV=VTlm_rc+(Ns&e6FJ<&~70HgEoV<&1%bh74-|rWbae(?D{3hg5=zfcyFCr)5~< zLB2qO-42{TXEEhAF4cgH6z`9Ea6bwHNs*2RsA z2ndo21`;BQ3D^MwqN0P{P1s`~CQ3*O7NE{3Vxg#G>p19GqyYwDVNxOp24xTe-)}o- z&UfDL^N;t;JEQVE_kCS^uf5jVdmn@5RIX{7U%7PIvb=Zini3JhYMqKPtk4&Z)G>Cm(`20mIagOT!<7i^ z{Qc@0;D&dx!*YWB9i-Y!TsmyRP0q*&uk5I)b<5Rqc~0E+y?gaJ&nnrtG>hVZJUCEnlg#O zbM$WAIlvz}&^7iCT6gYc*j>GJbG_~}6sc}tr2W)&LLJT0Ty}9apnreB7Crq|vzI*r zIIJes4Q6|!056oVVK6HNR=X@6@``T3=c1RRQiPHP>!SK2sf0{!5(y2ZLm{!P9-;^41ah& z!?3GZQ5>;z=Y&rmR(^RuUZCHQ_;oB?VJlxIWH_cTZ3L$Prh>0)r*d4dQ=4()#$}AS zsigbQWj%EWfFA}p$2Q#9%*l1dyqq%M&9Cz+jlem9cHg|avaT_?gNe1hvg1;t4QKR6?NKmD^CYev}Fb3o^< z?}J@QZTghS*W=3Xg`|jIzJX~nKY!g`%a>u#{SJxuJA>>vLPi4z zw!yKxiJ{d0zYt9f>5#Z2iW@du-Nyfyr)>VzmVNEOnHe+MNNfY7ExCSTgd@D$EppLm zpUdpelRa5{?!9~Wb}lH$VZgP&P1O8_3pax&$`U-s$|4hL9>tT^&It-+;aM)1b~&2< zy{@#ql&PCGDNwtme!RBD!34L7DhQ^r0(K8Qj=4QKs9>$@vQzDW@*uR`8{TlYWjw9w zfj7)9J?7z)jV3HVcfwgnRZHK%h69UK&Sc?v(C{>WlOJqbhhK1Clnl4MW~_tTidd~V5_ zA=L#sNR4Bcd;Gn+SX=Jf$t;7x>n+IVO{W(zrk4TbZu~jJ*pas@8Ks|BTr9gh)~{dh z4Bx4`YFRPd5Q}_L2Q^%$Ah3JZ$5eJCJv9xaXfk;4tdccEy%1zp z$D53)>j7bMfum&itadh&=VVMc`}){w+Y%#^X}|QCqQXL1GAJYP-Q6!Zota3;L$y^` zkZ6f4o59q^V?N7N*l-J=JT_;YOxR*vcIRavF{ z?PP&s|oCbdsUfBQVX=x9GI^c4#0A6hbMb0mwVQa<)M49S)UU9V7oO{q6qceov8Z8Cm6 z*MTOH86|5NT;akyO&M4ONHjB3;^qGqS(!-!cE*RTunE{Io5C#m^a+xshpjc8pd|-X=J2;$~mvhDgh)uVn@3~ zxTN4@&B!z&ClZ`o_OJ< z{pefRTX|iRA8KMX+ky0LY-2OW{Ia!`RmI%xCe+QAi!zMJsOAp}0@%F5w4`&+-(Rtq z!lNSb`=sg9LqUIUNh1@^FLQB;zEDf4tITpG2f)(Pf;AOjp=PXXwP5WvvXl!w-XN%o z!-pH#^zOL#u6$ca-V#l%$c}kv^Mk83H83Ym`o;*zBWI?(KLy(%6M@v`W_U>%E44X z8f_YZ=<{%3K1W7!>bGTmBq9G)(S!X54#+wfSf|cwF743+QmR&5lg(hMlXudcEv=%rqSY4I+^b}L-{fFow`55dL7o68lE;i5SB9EuC;qck zOQC+>MA>CWTxs_5%8D@R!OjkEdoWwl)YQ~5Rv4kuiR)88|lgrBiegBVm|{` z=q4DbAbVK$vLBQgqbxPPZBTm!M$#_rLvm75%uCaYWS@R3vTwncUxb{DOnbsAFo--^ zZHd)Lm6@k~e}Vg)U<=@q{b&NP+1A0WqZCS}vtkP@hNaqsEUKy2p^0R4p&?a{U!^;?1T1GTes{4iU`iiG19FJeP3Q_(`7j^AM z3G8|#fvHALWZ#X^_MOXTl=YBocYZYtnwHZevO5=%%an4;xh)imn9>r zdU|>wLA&E^P2(0(OtJtYk)og+vh}l*x#y26%f^ix%aaz>_cK3m5drr7;}aTEd0>iR zI@zW{kp;s6rR-00S+PQ+ZQHhy%fqXV;I(+mJdGeIJH}IA?G@@+Lis5BX=QB=4KY@4 zo|GE)=+WanE7V#0GJejS9=$&N#@cS^NzX+W+b@-Y%64 z8Eg~l%}J|4T;~oJ7Mi~G)hC>87KJZGuFbV7`|<@EYk_{uq<{GI z`eEH#vEY*<-^a6J9>be=MBg`pd6478?#G4y67{Mjq?cQ~lhgC3Q&Y)a@1c7pl5*LL zc#Gbk|L?B#RK@5{9j>n`YBAL8B;%8%CX~&N%(^zQun3R7;o#*p7`uu;kXG7^Jcg(| zgN#r-ZJ?ub^vU)dALSt?UKK7?xWY)I%6*mG=-<**#uK%Dxy|(Q^DH_g(lK`V*69TjlTrw+J+BB>C zT>IkUVi%q@wAsS*I9zBAOA2)-3LjG^lrlF>x zs9{@(V;d|w&=+Nx`v-6QrUnFpmfP%TdTN6vSV9dgO*nESA2`$K=;%fii@bIP8MVz3 zki|u|p)u|Upm{7%+7L0xZ5-9P$%)k5tcXdF=$N`&QQ)$L_`xR&AUNmwc_iwa2 z&41+PY-ZmAV|VmH+G;@|>*G4N>Hyy;U*`UOsO(eM*3N>7L^c;18|(oOJUPo%XkdPn zXb`}~pTuSL5o*`~+4K1CuMN$42L8|VLLCU?^;y=w4y6Ixt+U@SqG&sbQ|5}_BB^J> zS6zLjq@$yQ=|Y=3rW;dFQWTOPv0}&Fd^o$NCvP$=83A|F=U!dd-$n>v53uK3T77g2#(6?Q9 zboriX-KL!SuD)1yT(dFz43pFOX=u?SdKArK70zMjSRH7J>WcaNyUk4s4U}K#zRu2` zQ(XBQB_=CZMLR+$4SI2opuq+hHFAOM0wh=72`DzmZUTL~opalD>8_%p+qZ7X{#o`L z!~Ha++3!J7^vDHd;(V$Ew76({gD}eZ_w;RIer*QApL5CR$agHlffO}v6vvb=9`V*x zP6@CA@`I_@&d*36K_nCAP7JQ9eTG9~2(=2ecoa5!@S^uaU}etO_X-u1hCA)RYp{#c5sfomsF>Hp%)Y5$I=$UhF-M;NYhEUif%81-s7#S{kik?!( z4y|NQUg!!|nXzeIHU1`x?)9k^Sr43tVoP0D)+SsnoQH%IOV9jP$k;9q)`mKmF#EoZ zFu|KQH-n&ma9}j|)|nDhCJ(~vVE}BYR?rkaLkb@@2qqgHOE42kW707wQ+V>Tzt8`~ zWO;VaEflyzuq=jhppN4G zVOK#Tp&3t~-b(VBGCJg}<6JMDh}B%fc?%bIfaGUStuUf2*zOV=729d_zCgT_!Y0Zz ze2T8B84iK*Z4B!le=&jIFBxpix>2YMSz`-8V2#;Bjkv`75Lp~bc5Tji>gmtuZuZ@0 z3Q1q7*ms}F&T~)-Cf3#l_=pMZcBHfx?fM&ijjDtF;3;iLU?)8<%MMtSNzfj1eE8GP z=mpPjc&>`=_5n+70J7H5sj}ZsxYht8lVkLAY~~<}lEOFeKhP6nmo_Y*Py_5y%jAzr z{?X9*U;X*!;o<6ehGE@1ch2>;b}-DOb`gZbH%&@QJ2zv;#>K2YNT%th>90Hg244Ps z?V3Hq|C+q(KAZjN0!*K@$DlE5VTTAuxhi;XwDB8^8Z?~JW@T!v*Wy`m-Lb8UVPT}e|wZ1S`f-{NuX58 z#5{R*hA&bjg_ASRH2)_#?1<&kdNvfyvJHxO0?(iGRsKfH8>r}5F zXz1WF|r9+!Pk3i|6iYVqi`@F)Oys#NkJj%IiKxy zYEN0e&H+(Scuv;azkk0hA5}W4$PUJGK4fA}&Ro6;)&Vjm+b0I_O)_!!vS%FN@9Pjv?X+FAHsQ_VSwbQEj)WtEpMcOVM0i{HQ6&Qbas1YX8E_QJ6noYnQn)Z- zIU_X0@iTc?)UlATun|G&TnOl>op~ySn zEWR)ww6INQu$K2cAc#Ag{BUbuSYPf66+B`2Gn@yX5l4rvU3=N+Yx zr|7}fLW#HQ)vSXy-l{`}3}FZO?f}JxA(`?<$$7UIF510&+}*z*A6N&h`#J6y1g zxd5UJraj}>FM2_#C&}Vmgdm%Uz^H$2kc_rK={XuKb93HI= z+n`2rZn5)<+-sK9H=nSP?$lAQ&P*`X&XZlgwEyZ}ZhdcSX(GTTNfvCAzmE)8A2as5 z817gc=M0@X>X77Wk)S}|hsnfCm;CbdGgZs;IPdHB{VsTyzkUxOktOLs!)6LvH?cCd z0<20k8h{elIz~j+&jeA2K!hTWwmTw zTJ-92_aQo64c^Fp zbjVl1-yq?W7{W7Gr>(9jsZ#CT3L<1#Wo0sAp=aD^)v$HZoSN~!!2&Fqp2okdPG~xL zD=exg$b5Bj#NXbveW6JU3WIk6yLk;wzyWZWM3m-8Azt5JER--0KD&CUIF^@)%cDn~ z#_m2#%JP&NiBf57+=v=H@XD1d&N>}#j9k6C_mQJV8!3<^^=22s1t#FR7&ydBBOu-`>;Ev6F z{|UXP`mF30_o4d8jT<%$gPU!DcSO~C3VJ%S>gjzZ#Ne67pOsdx@`Vn-tkH~@E{QC? zdSnSlR?36KJ=jqDk4YL*!PVLip4P+6?+-{=sQ{(Dbb>FKk_9}V2Q zJ41;;z+MucHEaeXvV`_AeJiykiP475g2}Xvy>^eIm~VR8yC}^dXvxlLQ>K_bw@zjI zqUuvq%MKleh8Hz~9rn!V63&@1COd(b1NmsP&a9%}Un*h9J-d?4n>Sw?nLiVh#jf-) zqu8@_43&JXnr&sh?sGe^oze5E-{<3uU~{n&C%b3A?K4lyrgT0-k7=u>dFrxO6%UFF zWxgB${A{Z0}I%%iDjAjd^;QhRFvRbsh}UyI5jOTn6jO`e#fG|6(638F&WYh z8g!K9`8Rh>!5`l~PM7M|eC{D^YwomQu&&bGg1cpI)T>u7MBIG%(V+PFAx)}Zq0z~@ z6M^V-1`z~w40&!VgK9thKIP5m$_*)E`zej%M$)EdX*+U)2!U(W! zOOI_*_lojH`2Y!Nr1xPzCLYRx<0aGPQ2(?uFd%iF`K$E0>1g(ZSY}No!--H)LGTuIfVeJ}JUf%B*ZJR)mj9j51vIbyS3P{H z%d%w;4~NbRB->&f$Ym=zb|#SN(84TF8jt<^j8xOmxO>RAK9s$3+XCLIqeXU|_9@JJ zMM^Ct@g0tfh1(sq7CW*Sw9s6lo!i|Oc2R{9A5UU)0yioy_F08&#gpplH08>QoK_!O zXLA8vXu$TU_PTSQP2H*!_B?#6n@$xk4k*gM!;q*}@o|91y}cIha86Hx?`TIQC7`6) z-CyywsC;|ygp^&7e`!x|YwsFe%BkD*H0|CntNZM0y6iC%$*q(1-MQOSp}_AQ*E)}x zvGJ^U9|PjmuBhShriWzb_vg>`=Lf9YJ(`0kJFEGz<&_Frb@z!a7Z3rs;ho=HuC1*d zO3=q@nDOjc{oxs~!2P{zWY?!3`uj11u{`F#-SZkl?579(5^8E@Mz+QesHHYkh1oAI z*ja^&Pd3^h>z75K;lfyL+rwr#t#imtuqjeip2R$0NF-OGu4qK_VxZNe5ycW%MC_d4F! z5GD8T4PRw%`iZ~#{U*jE@kKXMkoDi3?@Zhx*-OWmE)+8iN@2G-H}p*+91F9ul9QXT zbXX8HdQ~tTG47r2p*C3Y&hjbj`>!fgV+B@4c5MCPKdJ_@TmjZ^GwYd*)P3;ch2zD7 zqL6LdV6~wLxQ{+fjd1*O{!VLvvQVc3SHC}Op%JQca}3gn6zupEQ->Xm$}+f5Tg5HV z17Ac966`3xlck%68m?YS38oy^j$OjyZ18)$apKgeccBOsspw!(sHYB-Y1C@10c^@s zu0vX=RW*S+oie-M8*EU(gq-WopfOgGJ*U=Rg*w3IBD|;cnx6CMVs4%YY+`!XTIAIG zpX%qZ{hLgvgkMSwm+xr|JFiZu@$avQM=+_}B-k7ah-29*z0{<&)xxxPJR?`QIjfv* zuqBFfy~>+PQS4Y(R_5M!q*nk>aVe8&!e0_OGz)yz8)F2JO|k5eF3MfsJR%~3m!Hg) z0Bz7BB8i0xP4d+d+uiGG*5HmQuhbbi8#>S4-oD`T)t1IX zdeUR424_A323B7=VkaJks=DtvmYjPouKBVSbZm~GPQHj6{`9b zUd4-%?c%;;Fpy6BMWc*(z7$=*ZS_olp9~l423TgZjtZ3SmB&z|IrGH@yhKSvy!cuy&`C%(TnQfZ4X2tgHh zn!c)e48pb!S11sAoBH|PHM{(+{0Jp{!~F*i%x4-yik#HjVjW%Grv0*I?c z*-Rr^5kWdkt3#~`{?Q&nL>URlZprzXS6X0`c1$f*AUR(#@9fJsgn zH7L_k7zdc`w1eDBq7o|qQamwo&REJ%#NCk>YH(CXRIAxsM$Be&{G9NY6@Bf%rzWBg zD}|Qg@m2wV`}Ym#@ZvFV*{%dl$_OxOY^A;RHf!oNSGTs$X_zt_{LX^#9$&AaIRx(8 z*G6Mn)4sT05Jr_1!XHl&mfV<1E9oV7S$Ya1p z=mCwsHitA35|@Z?{^s(YJ$u{;dP~Erb`)Lb(@4TdHad9~GY@%#kJ)oTHoC+8;lmw9 zAp>hr(96Ql-MpwF9GcCDRa)Ai1qT{Fkuq}Mzfa*$+{h9Y9>bm5P(Kumb~ax6*dO0{ z2JOT~AmT?`Q*G-J-CDU7r5)f0x4?%aS|J9Soi2`v2q-{=P|j&Z&6noX*+Do&mGGm- z9EMC64n(peBFO?H1xDlsFb7e|Zp+5ky&3};uKCmrgluU#JD7+`ro4BdIDl>pu~ks} zx#_P6-?_6f=1RoNMjX|pzdM=+aB`|phbopcYC=>_t{|iGQw_Qu<-wDDFN(JO>19h$ zBFb2(eebpcJ=wJz1}s@MBHqs4zVUGEHVAI*AeAUivov*MCb z`ktE2|7m-QpC&%d@I-{{*l`CgUWhvhM${x40iI$!$2XLmnyP{oe9~T(zvfVO%#Ga+ z`B`{Nbn}0I#biLQDbBqk4LiLfwvpsC2Di4YKK1m`;$6$IK`9P zj;&xTC)`Tw%T2@;8gz56{UThbEPTI9rKKOd6D$00&gb83u?l_5!)f}feQg*FR+n4e z2k{5nA(TD&u(sq$XtUGW?4E5$M~zwK2lIl#!e0LES!vcRx$A!GjZ-d|DZ!{r*`ijO z6CZ}T)rEZ1Zrpi6I?Sh>eDa`SPrU!AeROc9HYz)K=uiton+F!}0}c#WrNkj7H!alf zDIP$a07s7;@psWQk4C@}mSGx+GI@O(B}h*3zRB?K3@8hNXR55J{-s@icLn@KYW8*% z;|a7WAObAZAH{)a0;q#5nWyjIOU|PGtgXPK;PUol<(1GJO684&?!s)JN3}Cwzh}gO z1KY9j;YB~>BDK&32s$IgI2=mz^%dxdMY>&Xp*+FypBA0|9t9z}_orR;=vb0V#!xzJuL52k^_a5{oH2_%zL8?mEauT1_Pn; zX|5t7MTCbZIPadtZdSQRazH7rC87>BzPfwYt^gu=BC8!FM4wuw(W1q0ZX~b5Z~y*w zEtEDKMOAH{-R=d;W7n=-s}>bAJSDWHtDD<^$ajMPAbw$Cl)#;w_A20G4F1uIdlbU8 z6zi>ABM{<)aoOVEr+yLtHwwu)-B;ka?er}q`N!MADoQ#@^`ub+~2a;h+l_??=b{FI;XPv=d_uoh4)4u}zozaBi= z&A~(qWkl>p!O@EJ#R%xJTXOgA-L4M9(SciewFYcDIyvpGJwu{ZJXy;uvkK?Q+9YXM z(VfL>SW@3|wS8T9xMqvMJ)L{@gmF1!6f2B|?)SBahWN_5_e5Bz0f)1lA-G2M%Jbwf ziRfI{?YtV(;&yQRY)v98)HW=nrHJSgfO!TUrgh4%e_y|Tf7rxj2ofnNj(82bJY3~b zO7SzD%G>8{&ULyhq!>ue%5g@55q)gK?fQbik(IzF`~LN_Xjxed>H07^4?USFxc z#^>Ow>$^<;ajACP!}%q0HIHitq6{AWMeEp!6K@$nkigWSa2D&@hS>N+mPsofw;>s6 zF{fm~K+jm3^$-MxmnjYYvVgBG$eYWeMC`zXW~Eyb2GAz6yYgjmM$b1g?FW;Dg?y^m z-8ngvOS;i5IFmU^3P6tBgu_#JLbhC4aVYmbGv$nh$4(fz1Rt#E(upO_Sjb$k&K=sG zQbl#bMymU9liy`!otM@Cp+1QoQf8m8y!oyupHu;W$Y%r1)O%`cOS89ADL6wXQ6Cmc z`SRaiL5~1KiF-s!VeW;YT9jk3=f;h=dS@PQ+q4A>eJ{ME#>N(0XuCd?$F4f%IvlG< zL?Unj6TtmMqmD40RXGjyou(!C+S#4d;r|3U@yF|aS6DroAOyzz>0Gv5wHH@o>pz*b z+TDE<&U%eW9oa3diOqlFf~>KlN2}vc(K%C(kArrY+kR#KM2%c7HUbct1eIL3?{$i& zs@mG4d;qUjH3m}2qm%BlU*X0*Cq*{{6@7L$uM(Ho@vCMdzhC-}2fz~DBe?u`RE2VS zs+8>JLy%Q9-df2gYm8^Mcbm%-BXGl zWeOl5T@UADBpcPtl%IEgZWtDxUTWt0)*E`aXg@f`D0>2(IB^a28=t>?X>Y1+8GeP0 zflJ6xy_9+0O-~#<7DS!%7PTuQGjn`NH=?a5lYKjM3QC@C23ffKH3zZj+&eH<(dQ*N z&a-73Hr}u@7C?L0FFmpZK{r^0Q64iuZQPb+Et)r%=FFxCCT#-w{-sOm3kEAvn`pLb zb!XiB?aVg~m5AKGzx4a51SIYn=E zYXlpyF(sRwoIpE{D0HVw*;PGKJ$q;P!zoaR<&ZhO3Z_C`ZQ^E$fs3}~Eq*tS;pP<) z4IrY{B!M?54#d|tH|~+_@b9mPBr(K3cjJ|JQ8lwA#s7LRW`K1pFmoDAb zP*>{f|8xi_tyyAoF0woW&OL4$dd_~URs$%uz^@WHs~;3EUq%_K;I@b!?s@hI7`k)4 zzqXVb8G0?5`)ihE4Q&fFx1Teoy+-e*S@;C)N`~X#t~32cAB)Zz^DF`6X5Z8Q#>eS+ zvk>w{_nK$xx;>7j{!vj;PVo(btLe}2f4b*gWnI{?KSrMD-mROMY=Y9#;1}TW3yn5t zFu3t9`clhps|u)7pBs(8H+)CClf5guhObULfL%*mvccs$indV6{AeHGmo};gts!RDwMT2Y+)bBt%kGQQg zngFhk04~%5Pu=)v*JANZ2tQU1FW#l~F4mORPbZLsAOLZ|w9Z36|XToJz0?C=_np!9Z>4ybvCj1zI0NmS2&&J}~_`Muu(M zw!zoz29YE~{Bb5xJHf~_A^&9kn+&z3N#quR;Q}t>U^~O5BvEPMLGYA*N(;@MDhMGU zvbX9#R(PH}1Sv(+3dq1p(|-Q^jy#@DRUXj3$rnay(iWUVw}S=I+OFjM?CY%FEFjeA`SI?hYKHtBT6#ZStY$p9BFXc7=9 zz{EU52MITA-r8DPV?@OzcO5}+wz}fes#Pn|=HWGsUiko(zA*Z)N!<#rRjZhD7+Nv( z&EN0}5Rc)%cKq+lNHcI<$-sUw%(m&hsLbvwsvkCu=9sG$Y(Y$x9tDn|*bY=i$TEET zTO&Nz-sNrZnhGyjsh;5{ONGbK+iqW9s;H=V3>xr0-}|jo+K8=^MQtkVZ)>dxKl7RX~!Aa*nNDs(ka|Rd)tp=#~uF#MO8DG3484>mDc0 zG#ECVzLYWYM5yY>&Q`~CM;4qJ`DoJK38MnrjEb1#FszTt05kK*NxknFry55ra=14x z@JZ**`>htc9{*D77FSqwF#YwaL4Te5b#B&IADi~|S4;2S$eL4n^IP27jpt+OWvZ;} zPm!HlQliSnUFmm_p5zG=YW~RoC`uNNfpR>J00BMt630=aurxQBTU+@4eF#Wv!w%!X zXetmS>BC^--D>vAjl!d%w(`#|#8>IYMzRk-aA0(3wF8z%gtVcU3px(9MU-)de{{;8 z>Z8`L-<9~d4vU!qfcQY4{Et6Muj4;iv~=k$`SFleqI7ZbDKdvaGamTZaDuiFY$0CO zN(d~BeO8y=ElRk<7{X91g$;qoGHh1_L6=Mf{pri3RbKR1QS7L^nPrfXYG(Km%0i^h zoUoxIMl{1BkI8Hpf0IqIK?XER{k0z14~q$lE>@l1xj~@6<@f&;-`&NPOCe;*O*hj( zO?!tx(hI_J2nE;HrO^dY9A_wau$BkHAa_szgM}#SbD{tSLy* z67wx>?^M%A0yX27ELtS4sEB?0Mx|ES!oOUIjU9&4OZ$d{iq=YmuKrLckNZO2H@plC z(QHM`OYXK7Nd^?1vU*|*yR7HWLpYkfLJeBCR^f?{s@VAbBZ1$Q8fy2pFa$MbnkWOb z##!0eIy)P&ulOQYnYD@&?3Imq5dZx@CKn`E?^?@10r?&BG^k^DuTL6DP`+^G%4W=O zl)Ys#kAv$%t{jSxJ#gOksjF7?;YIjK|mjfWq7 z>{wGA1Ja%d>A{s3%7lcJa>-{S-ZZA3#i+LrdX8ogdCQvEN159pd;lEQTz8`Q!OM}v zWKxYSwxZmMH2~2TVZ|jSH7|xOY8r|08Ere`#8^YA8L_=Gq{HvV4X@2Vlx}v_rdI0O zyKmnk7cXk@fYCV;@W<=ZWqN zZ1n5tcIw=@7F^N0NWO=e3)`=8(nw@utU}!+9CJr$zLL9$ugbS&me1FcmIqO(F{r+! zlX3sRdO`>dygPQ44p%Yue+bdTR~!dP#J?ApOV6I{8SmS>Uq5Py`UGYQ2UA1l(MwzS zq;4e@MR-I+BBi1X9td@_q9+W$l0<-}O@rb+klttVM~}wFwxsM?UtQ7Br@ngO!T7DU z{X_$>bW>tDlbv3$!YIBamt^jp8&|IMBh`>bUKxLw-qF;Q@1%yq1P1H-kotI_n#`6F zm6#5bEilF_3u}sw0i$4(H}zb$al-~z^kjKdmoKl~>@^clYA-CkKYTwA`41N_XMyKU zpEk($anrH&naCk73m4wK@{Pl2xgvY;-l(XdGiP2On%xLoYe#*JTRvgxT<;(zt{rB! z3*FNryu1eIWH;hZYM}oArJ*(AD)vjxy%@)D9tCI(=L*r&Usmz%7wMc~?dvV-{;E07 z)uqV&0y+jpa92Y@5Ek!s96tQRWZ=G zb7r79CQyVj0oG3Wr7vTf3)bPabgVVhZP^lok>ALrKFiG#IoD>6m((g{oDRdiy{(a387Zcf;gep}V)a|?T_Qt&E!o`Q6^+TmYam0wnwUd)V z!ZL+=pK;s78KvD-?MA-AjBQBuCA~j*j!DwvXUy0_{CC5e zYVRBO>eVYG-n#{LU*fGBx?@rxW=OkQ`pcGjdLu|?nT#oy5eW!C&+%c?+cKV74Klv` z<16#L%j9Lz&b>UF{e@0PYIUJt9PHif3$te>W{ODx&1r1y{zHetz_Nq|i_%`z>%^^z z6Ne#}DqOukPMz|3QtE*UEc7agUfi^u5iekt685aPQz}+bXxN6LLG1-v1n!|yP(^b` zl~U~JtszT#=auy$$;tI2$$dG~Gf9Y^-u2MW8Mq)u^>#_i%#4^Gaf_A%ih#s~giYFY z{mz~}i`%%_uIHTPFW-$f0kK_hzJZI6Hu|n2E+#C5;x6C4ZL9!|8ymi=q6+~Ltvml8 zcmB$Gj)W@@Ij$6$+-ppWa4`hC$~jtTe!&={tMz8 z>IZQj$B!GAuSb1zX}S9N!K&F3Xy%|a#$NF67=ZGiS* zu4*$qt+!3-Iuu|f>NjazUZV6O$)WS`uo@fIv2$l-X(s`L7o{Dg&l*{o^m2}@Ogf_sXPSaDioUcZQo;#> zB9>Gx`!pL(USu*X*(wEBxno1YWDr80FF(^$4`0=YS~Tv9^Xh) zBC4Ryj0{bx3uO(B5ztPeF>+9&(@yZbCaDvu8i_I`ZM~m2Uwt%FhjDP8eD4_&m)Dt(UJ} zK~AX94=~a1md1@NaXr(@eSDtR5rRV^%j;E5wGZNx_TRd-dC7+l=B)!4((uQj9o~k+)&KMP}yrJfcdF5?&B3ZDYq9}`nix!0-Fj;O*ckR%|rHqXZ0=%7EDRFFP z&00NV`jyL<^EVs%B*0Fcw9W4XBG%%R@;488Okag~#<0B|+G+QHQIg+0+VLKLqqL`q zbDLgIgYJE9{_6#pGp<5K>NhdnNPMZO*&tB_$Srh>kXF^|EjWfQOk#i%&7;qhhdIkZ5VrWQ&*~=!N1b{ufjY9 zTjZ@ARnz*q>6GvKa1{)$k*kda|LgC`|jMc=Pp|n z#RLzNl8}}*=6ywLI!1V4%zbo&9f6dnk*_lvIt~Q29hVH2oMR+OJ^JU*|k;Z@%^9-_0EX=1n- zJrAZ`TE*xL0`p=`E3;0WHot$rNSr)i)gY-1ot*YQe7qASk3aHzkJ(#Lr)ULlOHDWU z(<%&Ol(wTf9TJ+yj!I0DwTcL-^?`9nk_JaMwUxey;Uj>F}){zIN~#UrFR0DwZ?oE5f$}I>U#zFBbxhfp(qh^{)gES=s|-pF2};fQK%U1Yu66v zQ6Erv zsapmS9bf&{FKbrg*k6#fFy5!G|BE7a9VQV37q{h9R+!iHIfb`5D8y63mw%Flk{IKq+ra=2ph%$cw9EWN$F*3VeK?{;Y_fjS&? z%E=P@iaY)4I7My0f0!L{654jZjrYEV8@pi%@L2o#5?wu)o6BgaUUik3gfi$&q7v1`!PvI#+BwyZwsHl} z%1i@D5HUT2M3~61euEmHNTL#XI`Q$!r?7_n*GwZLPOQUjyc{gk*a6mvc-7=Fc)rud zt=dVdEl2+3Y=wM%T!okP{i9>eJMPnLv>WOg|LS8?U3(kK2HlQ%I?Rc5LF8S1zS@?= z;|H*6nQr8@CnCZRcl|gM<%!&q!8sQ`=KN>q7GvxH^U35L^1~)*$mp1ug7dyyT4{M1 zL+dwR+7*)%{W5Qhla4iXgzW_^im`X`;ZWRv+Q-We?BBmWZqvSy%e3Fwjvl>zy4RGb z7?o2J1?hfqa&4xhBqk=4J&(VATT5q?C?SBH<96RdrIeD!O&TYT%v*gJr_6tuoP zziG#S|8s|p8xNZ?qYcj10?+H9r(4M8Krk6SH&Cl!*ZK_`WEP(2u?);q0UnFtg#%=t zm4&ilgproOTR3tfU(Ya3Mo4$3*;N!YRyBE9wgK&dbd;S{*e}b2&S}lUUxn z_jb+=&t5Hed5>l4@}=i6nOax9E~APOZ6s@+I(>TaChsF+mn1Rm-*9}TyF1=kMKLOI z<&W~XrH}(h@uFp}5trxawQFs}gaCvQz6Lnt#fDpQThXn;&I-tI+hLKFpA7H#f77|K zLe;=hrq|7vE=Z`uvLDr}2DUEn^;olJ7;i>wlLS(6MnKfwODIV4ok;OwXT1h#>gIB)_HH7hB){kCjj)qOo*~qra9Pw6|sG+ha6* zSiMo!uUe&JW8wu!$it&;x^~&Jvk3)x*a9A89dGE#g=L865Jj_v;30$dB+$7=HRp~W z4~7xqwMrty77KnJ%554}7$<`$KyQW1mNC8Z((T5miIKN)o#Gn`N~tqDz{`S4c5)>g zJr1jHR)2F14gyMkisJBHO^rxyGX5QfeAmKdg+)cZW^c^MRBhhe^UnpZDALK|JgM-6 zg5ks&;L!*d7vr&8T0#R!HM4q^u>r8Nv2efZ*Z1#~(*o$ELXWBH*#njfBmjw z9QY~cy|nPs?j#j8<1ySzs!$&yXlU4#9HbZ- zOn_l}+{~F`1Kaq(;#W25D`b9ZB? zc84!z`5m)2HEPm1)8P+954>Kggf3Cau!Mj#5PvjyK0GvAARn2>x2_*5meQv9zPp%N z)@R1dnVAr_;QY7$nE)UI9LQ(zw_7H9e=9FfgqC8UrG1l4nn@3U#xe_*88s8F>W`xd zNk1xnAoJGk7V`Kw*q9F53d=#-7E=oH6W#$n0rP!_k|`B3V5qrVKklyL8r&aZ(@6pb z$8mqx!++ASFCF02<-HXgH#705#oW)H02qLJ0-Mq-ptE2V)~s&}sbP=rFB1SK^KIw} z`{V4{VMHougp0H!9sK4=Xpxo}C5`q@O~$75>D}An%@1V-45btbMdIk9YPJlW&Bp}R zfIue|RK6w#K?#{%#BPELVmzVRtE$Wx4_Em9{Z4x23m2_eF@kW9B3`wj9kVq>preXA zeDvsJW8bzc4{s_f1mJ|w8pL=L_jLu?S6Z|f17KuqY}X|0|AQUukZg0jzZ>x1s~-7) zzGdUD%@t2)&)OGmLk7z1h60v)OH5SPJF?Ww0yIA>v zS(@3nNv1w{$hi_`9&by4AWr$`rd{;kRe4-EB3<2E-F)y?^40oZS^TX?z6g)m+08B4 zG!AS={Sm3>*YQR_%+zM9;!dABrFi`PY#wh;7@GRh-AKdHJdnb|UOrf7i-e0=vRfJA1np$qP}83U3KhWp8r z-HQiF2NHg)Vf9g+_B?&|%oVe}lWWwzeew^?3yjJr91DlZ=hh$KVR~-XRezdJjr;WZ z?eSeNh^qw`83#ek?))Tc8)O{Qk8&Z?`3lrdRtN6eKBqaYCAx9smA9WhZHGxPtDRi# z&mqO4yM@Cc4G2HK<_LRx|8Xs6yhDP}!!GQG9glgrML){hv~EqqkRA=+qPRW2>{CLTH>6#@3_GFLu^U zvXs6x&K}Mg`O_47vOY=71@Pnuf7Mg9F5`u70(F%r?h}C*xJAJ+B1?9v!J8X|j^>72 zu6OY#YZkGvfMS?b906`w2T>1Y3hlCGpW+Ydu)pcvUvI$qA|N3H*u(gt$H;;NO2W8z zNAOq)2SV49y4oXSB!mZ*8l!kyv;qcExTbg77rJ$8!{pwXq--j3v6JA|_FStbmd*tW zj*nY5YiNDXXIZ<690}dpnp$nKqyqdkU$+5np`te0*y% z8cm^_P`Rysb>jxge=FM3n{t5T@Ea-&e^B;U$J~MYXhM44d+VTuOXH%NSAH7xNe~N` z5e+-^?b`}{)C*ea_jwv$9Gm(2^)^I<1?kzM zE?pY3ANLeueGpjCmGmg{9p8*o%n;z5-OO)D{4446&hgz5OJ70L#F#1SZ1zI*<^N|luUxuORe#bw_5P18!xAsULGfd-@vgb)=A4fi%oVOFo zQ(FD7lSJJkEig3JMfqsUlL| z!eF5JtI`c}&3%yVnl*jqs>Y_X5h};0w!2LIALz;1H6YFg;Y5;lf2bO14ITXHI{Ga9 zkFix&H+ggauUMgY2#i5O9MG$i*?F zQ)uLqhTj+8=Rlx{pYCyAiU3UmgYIQbwR19`JaGX2ig%n!^arM#nL)|S>^j{;@5qKm7GCz$1U4ta`# zpOKM~VET=F+_b``>Z;gtm|`XdL9omI>M{29S4d9`_uf=px)hEwLC?|a1hSemqT(b- z`I3!%J&^KaYk~DUx6&aT`ppLp9Iy?$D3OSYmrml|9 z?z!>Sp{OwQS*S7zTiQ8bIfkQ2!6GO;Kd|FsS%22NBR&fAjWg)!+rdXI77|o7!dxyF+$n6u?#(y z9aOJZ(pM&>EWE}o(e`Nbk_JLq>3~h;K!1}*l#DJ^8{KS9`*%BZ8j1q~ZCywM9wUs&N8U$g!rfk_<54`-(c zP5J88_2jqDo<7aPzO=v8m&~(p2kU6{#lY&7MtC1M*sP_c&c}_8fW%8x9N~8#2ggGi z7d*5*->V-NY9*K&m?n%WM5xjvr4P% zyYJRLl3Sdo0lNJsVi}T~?cYr0?f}!n#thWb?>#hBZm=cL{6Qxrx^i}DkM5t$Rt=F_ zsFcY)@TI}B*a(WRaqP5-a6z<)$izs9i2IN27nHs<4ZP3~p_M%^x_A&}Fo&3Q`QsKV zrs88~(lQ&BZOy2i4%TO zoBhva^gA3AL#d!R#?TE`lv+{-o586K<)=!yLh*z{(kVzMG=&d1j56U;)vO*C?AQA7-RC7DL&A)h3#MIGS)AOkAai?Hxz_hYp7IS^@ZGwb2$_;e+W> zgQO3>bg38fP!g(LI~$IoZG%B3i3}fP2};VYz7)x$0=8-f!*oib73Vyu8TV4*tBbfk z{~xN(1T5!vjrQiE&(I)4K2&cZnIn-YFN#bl${Z0DKC`4U6eTnoODUPBLbcOxRU0Dq&>g99Xr4(Uz+nxYRLyit)X>O*F7FfK%^A{tR<>HDA$ zL9N!_T*}t6O$@3eBh!?b&o4!hyXcIE9j${b-qC1U2Xupc-7xJ<6TQdw3;Ncp1@=!~3$ZNx&J|Z}+<1LVp?4NqGbx0{Wmh`|8~ih#x%cl|!1JQi2w3s$8D}|;I^FOz zUp5=hmy3gI3(!r%mEDFu=K}f8T)BBS}I`Ml&$_Pl0D5azbf(O}2w+%z?+a}fIf86+tADUrPrd7@PzY&0%sC!pq)KB$ zH#q-6J>6@+{=SFbgt#X88?HHkL;)Wfwp~S7RYJ00!s!3}_gx}KuV-PxGBx?9RV!vL zm}@kmOlGhnEEmeZD044b0)b5q9yF8>0vX{dv{HeTP?G?ne~lFfRE}n$)f8sc!{B`v z^)ann1VR=}1gl-Qfmm#2KAUoRRgW=|<_$Sm7hnDV**Ddl@){cfsn@wixMEI@4+4Sl%m8hVt`EL3WtzQAaFW^^teb^ToTsuhYdw zi%B29nrscHDj^=+UAXGW`1-x28v?sqZh4%ZNTxApWnMYmg^<bq&`bX|_*A=JrcgAPrq0#yKwhEeJq;9d7C)xNjOU;K zCfG`UvQs=X~VaB;l6Zc{vFI$eh z|H!W(D>Z`50Vdu2gBK;8>|G(bJ8INW6GBrKD>&J&*T0quvpz;0h(!Kcpu&b1l06e#ht4V}nwh^7RpX4BRbb{)ae#UyF9#%rZ zdBxcIHf4C&vd1i+aRYNxYWlw~DpKJ^8GZxx3@j|1`|FM#cmJlhLFdkD{J#8%KtqIl z&5HvWAGSl??#sE|`%mipvmgGgtBtiMUe1E<6Jot;E`7F?r=Z%WDf%JpS}PhUlEqv^ zSct&JyU#sP@;H z@eF1}{Dw1)Z_awiov3?I+hmRH8l+m8IAjE2TS(T-6ea$O7YmRxO?ng5iLa>5c+%-` z5SyW2IvaPN_qCWk62c24s-hKbp4jv;l_b>4b@)9!SOKHqA1Iv{uiPAw!M~*U*_F0( z>C$Ul1E0C;&PYGau{W-!(Q`j%x%TTAI<^NtnTi1s?DkM}rl6oFKWF*&3`0sj|7?x^ zSo2YF?HlrJ9gJI%t_sdUCtx2pvoh);7Id6$>axkzt0rLhO?CTF7Hd9?d z7|5qC`;WLn_}Y-WqpaEy4sW?dm;a_z5`N^mDSmns=~(~G&HeNn!&Wm6F|0to? zovJK2EKISiYUG$P8=U}wV-AzoKG{SRk z1pO_34VjIOPzb12Rqh1JHZ01;v=>C|H93Ef_clDQn*GVAby* zU0yu*6aLP8?p*ae8Gb|1^v9s6YO7R zS`j58#R;C!cbqO}&#|<;>3kwfTOm%AG9*YFwCZd76O#;URZqz-Sp5OP+R)DeKW2IrDX@4Wi`fVG2;d~7B*s*S_2%| z8d3s?Lyvd?ax$@SNgDc$sgu~iaFsR^_pdT!bTZstX$l%4+KUHohFpoN|3$T89J{DNq>I~sRatVf_zi>XWIn=_N^ zrP@Iw8H$N)=wl`gCNul51zv)?vXSCyCK*Mf0elbli?zm5n~=IKdKpNPSzYlA3k^D;dQse9hW0QLr-Tsh9bgnY2fO><&M`BiqE01v4eT#Y zbV!JOsWg{AKK^Rx_DjbAbL;^8p+ zHdvN_0QdsT!g|)3>5FbI=b#8>5BN)Y@{82ds;V=e-oSJRE>)_3HSV{1ER=JolrY8e z(byaJh~v9XHZ|j(4e0LIo#C}e3Iy=TQ(G5BJl~ymlKw)#sDIS;6ylu(>c089M;*Yx z!UaebTVag97P$Ie(Sz&PeH`2#d9)4+EC(&HorKuE%ZJ|smgF!f*?qnFIZqG2e{cgE zN=0|(C+)xuNGeLBL*UP{2}65O|Nc{)(p|CH6(4zBcz6$UU1 zgN`?*<5H1oiipeK6FSh;q}AIS9Bx*DdDQM3&?#g2JgX+P*|ME#mIU2GFEk5cUw8jv z7)hjp`)jLY>4@YT{$F>t00;!)8ATkCDMI0w_QVnQ$xb2liEHAv#(m3t_7tfh*u(o6 zVdwt8YvFh0AE<9whg<#_fRQ|RTI9g8@RU(LRlm65bGSqbB~&=Xd#yYA^s-;D`u6ub zasN6TOr07rPJ2u3>gTIWQTklV|G>I0a!|yFVtUekvE!cP3GfR}ujl=RfyLaOdCByK z3qM*MWCm*eCO)$K_Yo3%zu3N_4qMaLq2OsgP<9x;C!A4!!S{tajDn)NZY| zQ$S!!#|ZaGrd5vGd914_xDkk)L=7H4H|*KynC1C0DQ zPlK#5*tMbGn*r5>)DNV-Rer^1QkIxzz9HUCDOh5}4Ddv{xPNL}|ICMOK*`T%m) z&-SG*_xT#g@gOJcK1XLm$5fy%Ok|Ku*bL1#=UGyaLbUR6?@xaUPxnymGJjkYh3p8; zbS8Z&1MisFW;9NYApeq(pJw#02qw}`A0eSAJ zI+7AfODpiB7#Gvf83kH|!|S_Lt>TQUD6yY0BM>yNr$37nras+Qq8B;--=zx+9 zn_C;rY&pL7SI%zbP6$EN;>=Zl_=(i0yrXMttf#ZSQE;o>6oP zPGA%NUby?!8cboxN{E+^>DZNJCPtioytZ^&wcl4Dfw>yxB(I3iD1niV^E4mA&?)pWC*S1!lr%5fb^*f66g3jWOGp;UBj zpmr_`UUE~gC5O@1Hc-h%_+ zJ0GX=ciBT%q@FYt#(B1%(9#H>)52 z#__uI>o!5$%Ena*&?#tF%q7qN|dl0>2NS~DI z^VmafEY7j|bk;R@jQtNLp7t~KY)0x8y48LK!HxdT=F;kmiHQv}Mq?Bv7k}m)rERG4 z^PrA1Vy171xuFsKP#QLJx?=@6$hc=u!9m1|Jhf!Q`t^NPVuvfM3p0wf7)GwPqi&Zf zK$uzE%?^SuGtBnyu(P0IK&S!sUpDPx!WI4c_djz}nPAPcWoeeij&{g^^fU*g189Cj zz$IfZ_$w0=^B*uQRcW_oEpbTAGlIfgS6_ca-4FF;#~WTPH|TV~-OyPSXQnrRl|$Mu zy=y!jD@aeXgWvBK_fDD()r@MKReaqxD<<(SQi%w@PElq{H*T=2PwT37$NN)4uDw&p zUemla6YN(Yk)le9z5l&)r%vjRd{(l7!0q@i4X7=$$%JuDGm14AhYbL1NMV%r$%*?jr z#HVKLv|PJ!FC9_F>7JcCJN%x#C_BdY-oeRw ztw-f6(x`R5~~$|77bf&WmG$MUNW?i0Bc2E7@zbM z2ZBCQC@&t;D0>3>MiHR|Xm_!xFdU6dQ|e3Swkz4|(d2lMjQyabzJ|z7n22v*HkF4j z3sQnULUUTUS~>PRO}NaLEmv4t`rT{LeMv0HEu4@>8M3I*`<_khM!pvFl4+y^tm7}= z-Q92b(KzvFntq^oCF(``f7J}WW??PQ+SJ9@IZts;85+1v9bVoAK83=>o8I0iZ|!}6 zd(8PYezg=`{EQ3#6BJ_+{T!*wM2aHibjxm?;4jGpm|_F(3pJ)Qv*|?Prwj)|P*aAu zyttUaVL7$5clYjBkaTx6t^wjO>ODqzqT8zC6X%^)9;en0apuOg1dLg8&0WqevZ*fa zpNZQJQ=99%Ed4rpx-<-h=taM^gOib+48&cO`o=5C%V4hziLal5X zlf*5S3|p|an*rcS^k|Kp#tt!Bcl4r)h!TEVwrGXiNqAA#cf5{G&53j2g9BFoU3=yO z_e!(eesgYV{Rsm30bvg}Lrx;g_&No+v>c_h)%>zGhQxn5^7x@!8c*8$xz>GH9Kj&% z%!ODjrb|9^;8-2?^n{@0d$+c~ebXT&@^SzL8fDP9OuRk(9Zykl0vZ*xMK4i+g;m>w zW-7|cm&8m;`?ERJB{nW@+`MF3SPqXSgLw5Fo>*~f?|L`mrPuETvTa)|!cBVTD$E=fZqPniW6~_*x&yph3t&$ z|KCZfUbt3EQ?oAf677QKX)KxIZV6jiY^tGuY+H;geF|2O=_JRYD{ksc)l zR!`dGSKath)a1WhfZydPs3@m+)p)ur)UpvnfXA0so(ghoVE9DE+zi%&_g#6vdiY#NJPp4y1%2tJ&dYT#d z%+`fI*je$L656M;0CFc>43}f=S;FzF?H}Ac!PpcK(Z$B>9};sz+7xE8Phs6 zTW!EPSoIb9N8WvD6MI>>E*ZV&>`0iyR znYsBE7!3_oQu13znmbso0K@}{ww<+wB7C^zXNh8Nl}Sf|3D;Ct6ot%{)wMW}7KGo? zG-1U_NhXL-e4Np6_$BuZy}j9#6msXFlCA?b{fQNKrz}ePe*ODoo0ktXAi;r3IXp!2 zVOL_UqN2hn8wYOaTd*T6tYg}@;`i@mUFx6ecKe&BHS-0!qn6TrGWFT$vG?&VaeN{L zqr=FSf%=?TxRK(U+A!ngrIp=e5aDFgSx|r_0Le}drbha`n{%qS za(wzhFTHRDe8Ho_1$do7mG(+URPnpXA9(Pd*X?Ai!deR5k5YFAPGLjEIeKE_zb#jz7eVQ8 zJm@2DL3>xV%;;oojCr}3XQ7aoocjn(XVkc6tjqEmqr(9V?!2>RMH0rNSe5$^7AHTkNb#C1U`@f31ILJ@ZlD=S}w;kJhMt z<@W7S_AA&78{~R;kKTvKNIqA7Lhkf@wV>+#%ZneS#xwui0yHw&>?CQo@>m|D0kM)mpyF0pBa8Hej-wS9Np619R9TJ>H{1e4^ucFCe;@f^_=N!v|>UPg- zwib*jXcNT9RjqQD>C06^?fVq1eK>+g=e~6;sbJ(a2y|C!ZYq1~xh(z;dTKvaZ*?r-kVPp!=J6lB&z(O%0=|{3@Z!2P=@+6T z1Nauqoipe3g|c?sjnIPKSRxMNI66$Nh1@a3nJIC?qlJSF=KDp~bzy%DrIJBO;RpCI zf~g$%upR0P*r8fRBnz2%a%LTC=5Si58}igT`|=1%g^Nd0#}7$8 zfMEORZ26y2MxLmMv>P^`z;?S%iM8R6%UPGt=Wd_KMI<8}x1vQnvVv@ri#R?Ye zkY+66Q9T?fEA|8_*$fLWW2rOGhSu-a=T%Y$xz8p0{~TVbCHJ6@+pdB{be)ci?X2k@ zeG8_uK>1PN2`nf1TSr&ir8tA=T=lSK#;xyDVX@%6c>Md*D)^2eg-I2K;cHGG=?;M< zqu2KNrIW~(7)K7amr>yMB~v?UH99^yVWi~`<|K(Lc1MDF4_}4Ln(y1(iJI?lUDZuQ zECCQwbj5wk%?8Jhu4vjcyVTE3>&o2W%GfPDKZboqir;*-%PT@fyr6P?%fnMMX4tQQ zWJt(zaN6Ai9n}t+$A!~Jg&Zfvvc02CGk-G%+bgTdh8988}(!{dfS|-f5Kijhr zs9Qy$f(sDu)}T`dy*-x97{h(-L^5X4+NByl&IB(vSbaeSXJ$c*$-3O)R$AJy`&k>G zCVxpl%!R@xWbS6SzRIIV&YrbB68Qz-!Dr4X`_pyc!jD|p#QXOrQ$>dkqavA)2I8>v zZJ5Q_oIeSQ$kp4+ypeAmC|{bAK{l~$`=5K}H&~lToYxHb8}@X-+{en^X^d9v`^*@n zj6Dvy%eUaDYu)Aq+eZ@``@K|q1I$*C;F}5b(&)A%weX$6Zd5;xd|8Y5^sPE1Chz%= z<8+B4x{ChL!W%od?@e{J;c`ENa`V*;;_15=DsdKPx@?^WGuAD<)Bm zjrK@#S*qwt_^=XpB#4Xbw=ZrP9;h?X&>#Lu&kawoLs+_}W8ssRcV{yLm2D5UPau{x5;{GjfeERj=GV`nIaY=htEz<7gM;H`juIed@cR zv-E9A=LZJRxRWjv9xoF-^IS1YFW3$9Fkfw}?PPcY*+_vLEBH16HaYtlxAAXVgj8jZ zXxDI2(BNA&tF;dF*`qLI-{c--cA&zM7pgkR-YAy%cVXv(y>ZOMGu7q;M_c8z@3`x-v68Z$<_PaiXt zGAAl@tq?+70Dt-P6_+av3rIx+LA5PSaLgeZG=pEO!CJSDtU@)zTi|A>Wx}@3kJ+R0 z(C^)7m&C+g!IwqRo|xU0N|zCjad-6r_c39QR@+Y$%Q+eex+5sL$Co4(EcTjo*RTLR zWURl#968O_v;*EhS&3ti6@eG2&e^KlxNmiJp9^TDcI>*cTjj9{hnkRyYPR)+i|ts! zn96AUA!cMLsXyYZ*bI(A}e$u2b%U89NE$5VGY8kS>5}MM(i^+H&?dresWZaq~ zeTM&UEfS&zz$7*)_%*0DZ0NplLyOgB+(uM3Lzu?0M&)Yb%@1t}j{tReavOT{pTH~NAfXw=CM1gXy3klG+O`CjIydcINc!w%3*a-ct4u_N!&=SN|8J3 zogLCBXA9i`2xA3WWKc^hnO7->(wlg;=HsvDVA$kZ*?I~S{ELaVZ04X=F$?@rM|J@ldl0USyRQ`_<90RJh1dpY z@8;9~_m}M$AfG?hjOKTv`(urZbaA z#Bvn7$L{Wtw8h8LoJ0Hlx|Fz*A4+H$lPCX*a-B{^fu}f7FN%{E;$Xsv2@{+~XR#UQcC&+{}1>Ljb)7u($-oBI}^ z$!O$yfB#1+F`w!CFJ8KI=4Pxmyyp(a;f)vgSvvG~dpyo^p}TwH>JN%3 z+Qs4R?p3);=oT`1y6Wn{`x9Ij8h9g*EsSwvNMAx0T}q)6QFnEt;H7uK zP2Vy03^?8ZxsbY!PG|S7e?s74!`@8arz1&k76BkNV z8@z|^q*-!MB-K?xIuX>eYq&xqI|cP;!`mo^N5D)$3SgJKP2&H-y0){G&pUFq*5OFk zCrhuE_e*Qz1X-}k*6}pY)M9t+wts#+lcl(m1U?n6Fw(tSTfHg+F$)kS{%P7v?!dii z#Tc^V0#9+{hJ~)0(H7Q;%BvYur%vs=>Bsu0X>5Gi8uXFs5--x=VK<^o|3;M!K6@ZzC`sbu6R1rEuCn+g^Va}eIQ2PZg_~OM!;jO zMrnigw5fhkW&jM%a}>+9poH~hhXk)+`;6sQlx^GK-eN>o(O@DHub@p@o1+DRlO&Ols zDoSc{D4L#gYP@Wwa7;IWvp7=uggDPgT;dCeg>R-XAa!-#lLq6jC}YDOKOAfJA!DOS z$pg;k>p)JwFK6W8MScW|+W?x#r`ij0D!MMbr21^ubvScOX8G7N9(^fY^ zAO{$zPB{^3{u@=Laoiq8zS<$T6Pr@J9o_mRUkHA{H01t37$y#b^1x$WF?I7rkukPy zPHV4chP0EK^o&~7E!})96oWuLG}H&&W%NnOrnMsq%XvLbD{Ez_a(ah*`B0HNxgUf% zsrFk7fMch5FNR0Hb`~*z29Hk^!C~SR!N8`2u|Vb{lLnW-3b$B-k>6M7#7k@4A0=Lg zF#LR+_@)sl(<+O58;wN0sAl~?XVGXZ94rF7Y`ojV@J;}&U0Cm`=|f(2V(r0f@K-o0 zvxd~`%;=B)w^_S(ftnr0Fql^rpK-KMFV~d**;~Xc0Z7(@-@V7aZ~3>v8~v9?zlHOD zW=PCTqK@oi6WkIZ?FrlO;-uzN0L9~0$*QGGmMCV7eVw0wZMjyr!GN+QnWS9piNpNu z^Qy7Xet7HP8|t2)pD%qmy7vKf9aX`jTfbq!{Im7>*qTM$CyOOZ;+@tr;w-9i|L^RaA@OHW-Bz(zW!U9Yv z=v_q`Dl5g~y-(7GziGICa@sUEDJOm2WlU!|9!sYU%jzo$7)Z? z5J-b6EWmoCEM2t!0YDGsYG0UXBK9S-35FK&)Va5Y9|e8tldG9+IeEALIDf z)*!$-)`FD0Z(K(Vk7`pFJ1({Qf2Z}XwKu8+ZdRa0nxJAb*s zTnYZzQfyopv;yPh&ie#vs<;yln!|DXS;wO`FYAkPRlqqVr5y5Ia_4&3K_(gq3RnM*a za)J*xFs9?9T^dSq26k0*(;BQS8ylhTIN*JbbcHqL-ZkRJ^yp^3XIb`$zN^bR@(ef8 zwUrJ$%f_m_OEuMzkAzmqpNgN$%`DU&D1gN?v- zGEnR!t-rHU_EEw8<}WqtSVe9_3Krw8sD~ReRlJ`GlaJ}-hO`q`zr2NU>g2dR=$%=6 z$PU`O!!}|L!|ciTE#zKhr{X0H8+CP37e1xsDXl zFr!JshC^sQICW?U1pf%F?)n1ynhsb0kq4q5y1FvXHlLZbQIKIJKq3j2{F|Y z>kTnjgcb;=c|YBP4A^CvFaaxmNK6QC^(yQcUneB4uUP#uZgW48Vbghp6pdlo6X@yo zw>_MloGc^f<3!yeN3B`5LjC8N+8RgNGEC5Qbqx&(ajCRNx?_;Bc=>YABPomMC~k#>Wdllw=myk?vthFJ{LOCnu5sF?nCP%vO1s&vVMhQo|=~CTi<;%Ej5JK z=5kI^ejotNj5)jyjAu3v!XkcbMBjD5;J1<+ARJB#dd`oW@nhGp%aL2RYh9(}j*PXu*A~5o|u+K@4;4{H-?pE~$p^3qzF#-unQ-?P7k^LtY zD;V5yCAG|$8kn^5AhKy1A1a?~RHzPG&2y4HJuY0l*l7N;J;0OLX$i}0M|xM6*_RPU z9kBm0wnVpNI`pl|*8R5uE$Jqr^}amgjhV7fu&Aua?hZ~i;a_$um1Q+piJVqF-XST5 zPuVDZk^2r97y=tmS;}GwQR{)b^idwI%?H0m=ya`I%t9%#2$z~ijCWu+-n+1Pc4RY+ zkq_GhQK~Ugc6jC7%uFSW)elff2%3wVibH_Sa`>hKXupP7%%!$pQ64DPT_}rN#Cvzi zE{!`=OM0JXFAiVFhH;pj*`;{+V1T6Av*O^mjv=9N<40(MAmb|#>{H2h@~3H*fJnXAEh*83BgJ_E*%jAX``(ZaS1i?nlckho^_VlRy!_|m=` zt*a3D4^86Y-3+$ z0|i8wEp*r;Blm`ePS~HPJB;WU_3`Db+ubK+wbtX(^h)w*@M2d-ts+m z(jf?25~;y-5=ws3-%)w1)B@R+Epf01hOkLsAA6ZK6@z)&KAz{j)xp?7=xW!ZgWZoa z!@L(4d!r(7{aD__rX7|W+99%l#B2IM@dS@6--E@0wteF`z9;ECfX+#va4}28p-FM9 z_yyNS^i&=EUD~89)|8YlY?zA6Ljo+1)000>@opzAV>Pp5F$NRIQoKDls^dCp@gH(ZEvo3bUd${p0Lhfx_l1YfVcgk@p=V{4Lqri)C7X_dQ7!Ofn^2{J{_4g5@TI9-4-mOl)l`-H z_O_x+&___e&m?LB6qOA@LR@+Hc%wz^WNJ5`InSOxjlVLMzSqFGKZqrVWOlH>(?`m1 zv5_deyLZZo2qcOyrW(W8nmgX#FH+ycq-=L%jdm%0#EKK0LpGsAd)vQImZfj2e;uR% zKQ__)z4R=fCL?}$CKi;jVsA|7$4+YOyD#%^SwxS+AVr5{$;|k~D0{gC1^SIr+e!ujS z*BABY_2P$j^7P!3HZNw}NAx%8*GdM;a(_NXo`tWH^LWn<%2umVKI)+zIvY@GvxcgP z(0EXvrkQH@HYnLs1{x(YqtMXM|d__RV*WU5`$qt5qDhN`Woe( z4||u_H=luWtf8`|`?TrP@9JqL`Jf#^%L4!^(-Sy^aw=srGCDB@Fz|-LGw`G)BnlBr-C1b40yL_Vd*QK#JN7L+ ze&U2d7TwPA(|3AXwvO?3yNqK2r?_aV>C3ew9<4^c1y6XP;;4(2=$sjw(1#gMIcIHo zuFb+7?cXlL(?BJ6gr-NL)nR>fv$V9ui!`MO3p%B(scdT1p zhFL1h{u19twh>JK`K7Z~BkR5Wnv35Ic3f(f|JJ~F88orHiti4;+`=!rhyvc#NheVGv zcUiNGJV%~KCi5B$4l9D$r~NDq{GYG#G3^&(G_?ct#Mi(e9A1lm-T&H7`(p?EAG37_ zV;dJFo*~X9>7gxVySNy+7xF&r?TNijm=NxRFYxqfrN4ie93pY#gR-Jqfz!@j8xt!u zeP@HV_M|vxD_Q2c;ir@j4YjDh@||;fK?$|GX&A!sOQ(0X_t0x)#YSBYEnmc^AP@62 z8OTRsBKa3syZ3wV77(96=q}$+G2qF_uN>xbtd^bo{z;S7o1w7Ir#z4?x6p6Q#voY_$NclC<`LuRUEj zzEP)6b(4L*KYy-r7?=Y2yQZ?iQ>v4{{_dwm0m)Ec1ZNl19=$c)8rxMYBs{FfOX}7S zv+7Ip-{j?=npkjYbX+)JzZJuivG$?YX|-hy0gBh)Pv7z=WB^aKLaJVRd|9k+loY}T!XiX9 zgS8X6QuZeVPlO&$;A3eQxgjT>1i)VqWtK&|4jeQRzVlU%=pTY0d3?tEysq|fwV2*+ zL|LcyUlgn+nzE?1Z3p?F*?g5NoFkQdaIi4&za(#icj)04T|7ZMX4k;)pm zF9fanCdEE_pEmExl-wN6`wI1196f{EhW;_1AAA6?Jf5{2({`21jaEY%4cM33Z^g%s z?sK6sqNndznV6oIW)+fOSUB;7_kVx`A?1b?Ek(lpI%)y72RlDj)LDzS8}Zxj$8sAA zI{_})9&x!vc;r!T6@s-iKtZ2^5lW@(c1iGx5C-YF1>r#-*&wiFfoW1tG&Q=3Oo&m< zA&%GjRT;2(v(KCWug`6UW@%$#&(rM_sGl%S*bGt_#ON$I6B*ltlPrX|%Mgl2mPYV- zPp_#c@vbz*ITO8XQ?iIJdS!mH>^R4vk*_+udvK^JcH&!j$VPXT4*ciU74O1&HD79~ zn?eDy0DKcpA@QKpL$~(Nr6;YzsOQgI={6Yfi)T#Dx$oGq+j_^kS8p6bzWZ?Z;X^Ih zPa9DFQ@R+sZOtr=80guW$-tY2(XG?|{tx!rxCF$CUek&?S7HlcP|TyrH>fI3cSlU% zi1>lB;tjVx#JPqXW!ybuGa3RaK!79}=Hij!e%>2L<`{N9v#N8R5tZc%mz@(z79+uZO%D0=8m=V`stX^6GEZ@TRAtUHvJb{ zP(QLUJ-h=Nn&QCf-_ROmzXGfa{Q(31X0DVj%p?rJ518Mrsrus0+)@YR!mjW+6&A9G z&r0^As`MNX3e?2!(<17-{}A{PhfaqDCNx?#QFq23_zIKZ8aT|OqKnvp*PUK}0QB== z;~Zhr2AP)<_&`53lQT+bh>z{X%Biu3$8VP+0@27v%epHR^@P|0m^|?kY(GN!s2!$Q zlFV~G_Oj1lxSKz@Ct}IhXGClfmJO-EHogmLorm`dJbw6*>4X%5M9}CDFpBT7;I@fk z^&H`=?wB@lqT%K2SLC#?_&!IKPWs~6-{)HUgeyX>uc?Z(B!bRBLnuzk0`|-g)a}== zHE3DQ&qXg^zGT^{+;*ZWvGHov{90i1;@3AaNAb-{q|<=?1Am<%)`4T7K2-rwZNr@! zh7jqo+o;C=ZXxq#&b(45rB?9L!7xxpYuH5?Rkt0<=`1sv?p?LKo{GwBn*CK@lFAj+ zHvP~8@&1<4A!_j0v4I^nxcs$pEF@|h&6p0gDcPy5uUdrO$6o$VL$N4t8+d6nbq5dt+jvB5~K-^PteKCS{F5>;09&hp+XMxMuaw2P05d2b!)`bNlPOj9=+du5W%fHLA z2fz1l3D*M$??=+*7G&A2ay8CV7lS@Bc$IxT6Vr8|#5yg#hPBYm>ZwI<-!8tQ*{Kyn zSR}h6BQcOSs;nwxv~+C6U6|mCCo^BbbBmwN6=^5f%A;=RAMcxwFE7YBNK%=}@PScM zXC1|TwPK_RGOu?UF>IJ_pFZav1%BfcSS)G54x!<-Y`f^`)CQtz-6mVgP*QY>T zyIoBctJU>%lIXsJf8BrdXgJp^A|k@}!v$a$p#=Ylj8D&st0>`Z6M0SKK}=RPpIVQ; z`}EubaX8-I&k!mC9H%xAoHjr`AW%3$v*QTh>f4e7Oe8Cayx_8S>EML15NCr)&|u3_F8%|E-p{c9*O%1O!_i*Snq?Ba+H!lJF-S*F*T94?>jRRtRm|y+kKhU z1pKbUI&!0eO9MMa5sw=!*ql1-YV2_`nku73kyS+QrOt;c9|P!2aRw?#$<`=?zWnu- zm`OXfZ#OWW;-op)PWBdKff-@$Zd2lxtrMW0?frr^ZUUmA$aAecZ8m9?vTO#~itZ*s z;Q+xkWX_+H#eWSrNewdZ(eYL?)ZzBL_OAKePC7XxdEnTxc^D9D=cm#@s9V5)e&_SN zrp(W}ny1qWf6TDDA=5YeHJIw%f7!brw><|3uKC$azI+jtPzxHI_(56l*<;txwf0~K zBNyFrj|Vn8Ryr!xQ^2ehW}NAe0x-DLtgNOY(p1Pw6Ur9#i79;hQG0SJ-nP4s27A#X zF7kTwb?POZFZA;SV=sKp%qS8v4&nf_0j6Z3ry{H-W>hP_09BH8m|M#w;HJ@ z4jK4mDM}8e;gQrQui#1^&v6glgDp>XO=Xio;J8eLVJb949S6WSK49rU?k9xjwqQmR zp3Kg?dGlx1z((|Q_$k%n!U-;d!gT0}5t|m(G4lEMV6i2>)xQI5mvrd$GIZbplING; z9dKcI+w-I@xRK_yZD&7Dqj9&~%9l`E5V8vH-k1-eHlRw_grswB<4__;Z`G%_A5Zl3 znX@x2?4SGv8b=`_a_+E1N|mu7b3U6-BJC^zKGUhCQ9@FP!1UwjC`;B$OPFX8AEU1N}RkMq6)9p}6J6MKZ z|3mLp_KpR{5+EDI+I3EoW6Ut66CYboJv&7jRAdjq3Rf5{yOh3R{G3L+#F{W5k!on(oQqr;QPc5yuCE($>N=ZcKmb!2 zb{b?&C&&qHbxqY1Sw@fw*GiGK7q@>P$&{ozx$o3FbpSKT07};P?XwX`DvG@N_HBLU z$QhBLr%s)6%8e3BRO%`4%ElS4ldza%_-(bL=+mbz0a-IUvG{4$-3T-A#n(D@lj6Aw z4R60>wGLA|Ac85K(|>SgR5SL%PRwmFE+Oa0gc0^DsAYs=66Yp_#-J9!m8Zgrb_!T# zIgPj>BvU0V#={R6%0E5ZM9nhbS1Cw=+yyvsS0Gp5S7!8kfHzqb!>XNcFE$+?5*m7% zx!nlwa|pI};n9$BLF1$Dl@GS71p+{{+w zAHq)aU%S8_8OL`Vy<;sujwg^*!#mFT^+L`NzT_I2o{gY z*0hP~Q*3R^0*6)Fv}W3%qGdN~;=~<6&w;?=E_54g8~T7&Q+wy(23V)h26k!L)5-G0 z9mlPB-xJ~AX0+E^ZH+AzTwH@YTm1ZHI{x{netw4~iGpd1#7Sn6g42L`%gTdz_ibRv z1mt}-Io}CCLh{m4*d#P&)sFp-Qz_L(0kbjSjxE>?{eqJ|Zu8R2gmC4pBg`?;JRtjf zeDQ_gCNol!pPFG(6Vhkjaz^FGofQh@Oqf%}*E;(@d<@h;z#n=DH8Bxs2>gwdG zA5#)e4#E0wW7Bp`R8&;NdJ(R@(WNX~rgWzhDKwvik#xAEQ_O~ZuqIo1G)?C2p28k_oyFH zhs6+G;Wk;hWEp7m8enDI9oC^PkPSEjpl|fEv^r(RxK+(MgqeJkWQrWwUBHo(S~$Js z_}(ao;#G|{2K3t=N<_L>Ne_McX6#u?0ojh>SV8{Up7U)u+*C6&hmxr~1&Sg%sfZyk&_@qQYYyh80tf!God4)FnRF&YZD@Nc9K-N4XU@dsxnPVv zxj3Z)5!ov$olx_YB$JV6c8?JCMZaK;p1pggu}Msk#h9u^$(n96l8rM4m`JJ`*zI4I znbi&m4Q9IELw{XnOxgMS&r=AKICX-}zPvU%#2x7_?Ek5WHujoNoz1YxgX*NnVr9d` zU*+l{D#77ba<8u(9jsH}>ayfm^3%3M|CNR*6?;*Hz2T=duD4d!VzlnvhzdH5j|V~Q zz>>xnA-2h;YT(T69oop=>Q^{9LVK2#+%ks>h;e$XIQ^s3Z*v)Ys0a51oES(w4$WmP zrIPL)jbY^{bbpziyey6Z`p?@(J+$Ij324G_`f6@&3-BMO@qT_Teze7LZhhkPJK--g zlMZ6u{n&=s9Qrf9)b_R1BikCU-~774$6HoqO}9nV)th5VTyuOPS#SFENaJm1;^S=| z7j6c{M=#PXv6@LoreFcfx*CchEApnUm zrC>|1hta18caz@jI$f(65@X(bWwxmR1dfUag$he1Gr6^npcoV2Ll1}>UHF@V};cm!Q5tetx zwe)_%km%yF_v8APwG#pL5%%s_oGG`EAC1 z(sbxx=W08O&#JVY#^kdl&TfQoA22=3N51>$B<1@*Ea)YL#`AUB}sU z)N-3>rHCeYiPs(8$3b_NPR1_u>zBWk%3FohWGB#s4Ard<% z!mqIkP3Pxued~^nKbn1`^y5Ku_QHGg$H?h$K}$ltkf$Bz%&8B@$YkEVS`Z9@gOo&U zA$#KP2uo(@*s)&@Jlb{W;K7n%KicT!E)ThtQ1Uuw56>E#M+ z2CZ!Ct)xiG8=*yDT5tu1kIbv_faU23D;2GmkcD__9?S0h?s-ZjxP0bOv8=U@8YufJ zElO5?d|aDmLP!lazlr5E#M@K>L$SW@(&MRJDaK>X$~r6y~m`XP#fs@wm^DrZ)jM!y%aLZl^B~l z-ce2sDU1GyD18gDn2ft=>ng)m&~fI*L!-yVwdd|yB6zazlb8SAP9y?ktTcploXM-` zly2%B-i0_#dq_k7$hsFLrdp32%_0e4iO3*B|5|bL9~hPau0L9|LQ2`7=kT8zc}MBy zbSU|q1p2`_gq-Cw2Onvez8XT225ta*9iT8Yy!9dR`}+r*I`|RgkaPsPbkXzPA(A(_ zO~tv+(f=uRkX0{OaGLFJlG!F4v_A69>=W@@o)g7(=(Gi4i9=#QyE&vPIy&LrCL4|r z=y}O1OctYcBC|^!?CocQv`W)g@Pun{g<{fWj;H1D;TsjBFI6u<#4$2KS3}ql-PCgx z^licgTefVO;at1|x?# zi-`#S#G)%fskp@ZR65;|bj?Cr5$FB03!Y6hNd8^^Ax90^=x0Ib*tq4z6C>ZH%LJB{ zfmh+jiRtglGhHXd6skhFadB!gs`)=nI7lu-VO_)P6Kh$%Y$|t(XFhpp0ofZu)d4@T%JDEgOi{L*Wc;{e~GH z$fb0547iWxNWERV8OPFhGP#wi$8rTsbhEXGpHfZv%*j1ADX_j*ZTQ?vA|ns~>e;JT z4Mw z58qa&M4c*M{yq6+?(eGAlJO%HdNCT#RpgMB?VbLU*Y7*_eRsya_^#`cLfDL48s4If`H5m!+?5(trQ}AvB0-%AR=|_mzoGs3`}o1@!3c^o$84`{lmHnWc%PPKMs`Pi;A{B3#gb{-_9~qC zu0d}f77y3!c3oesP%MFVEO2o-dMmF(L#!pt`!=$Q4_^DQ@((PS-7e~hO!NfO`&hBe zlZv*V3{%Jht15pk_J4ecG%<^xL8zS6DP|eonk`$Z!leZC-;;K42wOk(64@*4v{=nQ zS31N6B)MTQZDccI81mh@nhkQ1a(o?g-a z?|#d!aBszXUpwe*se6bRQuS;D-WO8=0o1tXkq=L1eIeV)Q&O`~#_EsUZd9Kj5bSm& zT=Q^2;__l`?$JhWV)1i}5~YD16-2<`SpC>jwaJ$Orp-fMcAk-Z`LTuOpZI&?bChlV z8)y^R!?xc1$I>g=nW^__;Fa5q`R1<=f%Q97!Gc|T z+4XWEEI>%JGdD#kfm*=a(z4sYfWz5}@gJTm1`OD%GD6pC`lulD@;{(i%pe#9GttHL zUH(A}p#0kU%F)!Wa<)ZX=U0WqYY5fQX`b0}HgN#`MHCbN(@pz=zYWxonz|QJS21?nut3g{kJyYov1dkva+Td-y@ zfz-wnN)TW{q!y)AZ6w0JW*jp2s-^2se=y3MYc0D4K^;od3w4qBO3LgrLZNqmJQHrc z#HmhEjq>^AGFl7Q*Eq68Exy_NChuJbGb0!J%ga^yjkUweM40<6ucmsE!h<$g7-Ik> zwt1)E7GNYcL-CuYF+a6?Y-!Opx94_!L)5}cel;GSEr@)txtUi$U6-%WLC18D=3tDr zdUhWC`O(K6s(LZnx{fYfrmX7)faX{5m@{#4S70BFzqo9^EMnT;gUR+5m~s0V4^Gaj zVHm~q+P1@I$K@+mrZYPd=EcLq03bLrQ~I|4~IuH1_}?6&wywe*e>I87p(4 zZOqFzG#z;FQsVa2Npu@w0pTvekTL>hikoZmEQkvx=tNFfKBA1#nqR_1z+mzSP>6c< z>dj>7%I1Q!>(@K37+b`h*Ls%4Ex7jd>Eu(}z6p!sKXUWg-10%O{3fnK=D}9Jqk=h;A3aIS1sxjZ`A-(CecVCvU zbS+RqCx^Y#!$R5M1ddQ*Yo<1&pa05ozH6(XC2w!}7qa6-_PV~gHd$fRty@#dbUhoZ)?!Knf1vCoyB7UvF*gs` z#rE@t@G^4Biz)f^)NZ%~NW(o4K2T-4JJoSO3N~f}3n1LOPB8T2nDk7D9WxdQl`A+S&*$q|&0$uG8L1DP=T_Xs4t>Ln@WZDwLd(PJ^6MXeUHP!tZw1 zd4GOif4s*TdA**`$KyV(`*mIS`S025gcTobnSWmN1(kd~w7Oi#|28*QFP_-Cx!>wO zOI}T{sn8<&TKya4vLp8Zw)UF=QeHh+2%Xk^IYy0%74Ba4HYY5a5CaX?H%owbCk2)H z*YW}US5$9!ZMJ78buXU!tGrwv!y*RJjO8{)tq_w?zDx2CmbL}2U*(1&rzSY*(@%Yc z*z5CXMulN?B^4&f_VLUS2dF|vV&j(^Lv~rZX3hIrBYXnf zhS=<~xEi4L3eI~}t0kR2D?Ojb)SRd%vllID%pMzQSMcQ%(55G@W*F|abX$xLHUnZ3 zjH9+hNu!?WeV_)lms7<(5(|F;$i#xtsj;k=-N{T%b{dGo^zbWVzO^0>AM}t?+arIK znPVM=B6Kcq`4JkR=f93HT0B9q_kr!)!aob!TIPL7!jhJ+O2dS+7Ro z=k56#n>(qT0m2OHsn7Q$If$V2ahmD+(gw@~oVO;8kcli3H{O;*W4jh@jyM0fyVvad z3F+zMSMC(}{!!RZ3P!WY-Wwr$FaxvS7$2ss(u(30X2+d6J62Zx>pxZ_+Nq0Ud?$lj zFzjUwE9`>D<%LtgTC}DZ`gxsEa>w==(Hv{R;z^(&e>A)?67{ z_PBXo_Jqd^Yc*rUgcn+~|IaNP4Xr_^;TuPoG_?CReK}DHUZT-zs_iuwS(Tl9HSOus ziA#1Dz%&s_88ejU_sfuGn4x);aVakudEb`#|6Y%BDEW*M<}k*1i?YRHBA^26;G$Q5 zX{?Lfr^reuYV}d|o7K%#c6lvps?$(+%Cr{5)2-^AEnd@JiE9xvfhjFjpfriAruoqE z3%6RFNM!%h9x2*|L;$*!eBr^zKuFhO1uR>9nr-SHc*`PT;jkBxnvu2C)ibz$odD`L z7UYz$y9JQ!VQnhAj~z7yufMkxfLH8>m`A9~zOGi}Xv4nxU-1+ZDi%iVGi@pgI3f~h zDW%TXhily5XWR@cT&8KE1TE$+n=#6xV4F2(&X4!igQvY72wMdl`&})pTU`Rp?EWDz zi(@gOzQ@OYWJA`a5w!?1m}nE_Y`(YKTZn z+kRMBOE?&hyVKYtFpf(82H@!dIs78$j+*n$*RMv8=P3}}04bVLB-v+&P|!FNZG7G@ zFll|O;_FS*nU=;6+hr-EmP$P+lTTe;{i6@ewY;llz|o_e*-qyGd3T)gRbY`UMuTti z)zvE>@btB&SYGWRz@^ydfxZ};Ni|OYto6tUWb$mLgMzJq-U=!*;;pn>k;8M-ievK%wU@-u~FoyS9XxeCR6 z;_IF3%kq4FS4gW&04_J}OWm^j4;{141ZYkfztQ>}$8N~bMBf0O7hmW$RnYfMPA09J zkV)%+Vd58moBhdJ2KJjzo_rg!t64~&p;xeTd^=}}LebE!pL)@8w_i8m$IiUx*GWK& zSB1W?O<*hfz{$}Bj;J}~LEY-_vMAY8QWrAh(Jp=u{oOY2@^W+n(lRgki6uYqUYjw4 z`C~+29-IA@FqnJ(+N)f(TrI9bjSItA{~H(WfT-c3$#1LDdYrF8kYlLwX7uOf$=x$71iij{W;ODUXZcU=~j`}Xb0Pq*U% zC44H6pzuxR&v!Vtlj(W4t4jc3)~_(yM@x!2F=F5xIsNdXO%`SPEitI8BG97)x9V~+ zL9oTmRs}ic78X8>3OS;-$498PZxME#@-u2tYVzk0|LMbyq>oz=KW=)*5Xq#LYg)Ey zbzDM^-N{Bns>)p4KwD%x9lz<~ybJ}?n%4G%WugUUBa1+izMdc+NnBu}$!h|)twnCM zfWF7Eoo7=bd!qjp!m$hdpp|-IaMXbiY)-nVUbGEfefiKs9TzNI)?T5z^i2!J%UpD? z2jlS~HeFk!@-}T>Kd<$Sb3YZ7YgFC@diR=*4ss@ydW2yEv<4%^@I-lNtiNSQO~5M( z)4rEVue!|JQ9R5^6?dtvzc08+AO65R+RMu^mlqSUTQrX~7qKQE#WF|o;A9tX7`%tr zh{%CBJ?{I)F}N6DPHx{O5@C#F2$8+FpIP_yo((_iH)*YWtXxK|CVwMha-`AKO1Sar zQ~H-w{>*uvJ)lqT-kPZo?%&T<+is~#0P|%2N4wP`jY4S@*oS|}pwtHsl3AkU8{3ZR zY~reAcCZ$cFCmfJ!mY&>_^4X%$sumv8n*HH$H_k2 zPL+(Dgh!;=H(4kF=$rfvG^#khx^lehG~eu^pbxX5vzaZY@<<-?-gNx8wz_;UAOus( zs${rU-nnE&8a!l3T+(ytJ~)^@jx&CCkC2+^ zkJd_N9?RH32oNi4A;Ep09*R!zk)E*Sz?EIaj}n?a8Geg@-3KHiU?o0_ zQ0hZ72T(!C|YFK zMbz@Q)^6o*_X)Z1V810zfucE>cQBM^2!C&;WuHNQ3*sz)b%9^aRrn|CP$(7H4rIqN zzM<2;4pstp$Ok8foh^ITW=d)<2n2Chd;csyF`X8ZtK1}`zSv9uLYzeiKQ8;(apLeg_JD#ucZqN(s;DEM*4Rx@S-MiC$< zJ|!=RcN6d0-_-A1lWVzx8fZjSnCccU!_VMMP-QmDHl_hl(@^dzR%WnvC_(bMadX^8##fOP;FaX1Q1tYk_m&cb#vSQZ!^~V`ldCg8g zGDwF_2DXNUkR_AXr>{EzbhJrN*%MB%PU`pKV%ynAnxtCn3{Z>TrwmvX@ax18KHyqZ)E0(POz4a9_6)^;vWZAh zg&hh!u5oi*BDAGqJco+RBUNwJ3)@Khs8KlxD&qEn(;=Jelymt$vK~zhMr7E?%;TQa zfeDg`-gVK{UAD!|?r+Y4I#qM;>^xlJh&XvbCgNWrvh4p*ALxd%{hKjy|7B)oejC=l zdh%p%vaU)3z92%VJCb#j0oH4hL;@}H()SPBiFK`XLZdot+{TB!{qW)L_YLiHqdN8X z|10Y9Wz7zqjf{4Cp3*phu*x}$a@HJw^bbG431%^fRb+vHIgecO_-y)5TyhPUb{f34 zXqa+evkuKGCptSTgtf< zkaX8?jK^~Aq~(Q9M)$45#?lC1%{`DjJ~E(k530SRqN|pMZL=e zusSp8{6WXIX<+ZvLoGU-MnV0bd^2a*hm+zBgLKT>^=?W^F6q(76?K&i#TD7*S#v{* zX&`t!?fn0RP!(~hlF;FtRdblEk93P|P&m6r$MJYP&zI@CNvr#q}1@4mVBi z(iQ-gQ1C2WXQ#Z}ToIS#zkAoZ^e|)I6V$+>`9C#2WH+88;uj+3s|-CPzm6JpS0gGC z9PKs?^|BF@55P8Y=u02EJ3qQtYB|69z`=P?D4PFAVl<<0&V9$ zxaVEE<;jMXRFEqp^dpYX+S;b_WX8Nl0C-}^zGx>+m)p10E{1)B8b}2S=`uibRix4>_Lp0_o_FNWs-zW`iUFu5fGI{&OPK3y^e11- zI>T{#sL9VnHUE%PkQ3hlKtyVXy1D!7GxG=_PTkU1>#s9H^Rax)fnFyqbFS$sm0hp40|frZx8fYj&5OvhzI#e2 zvP4qh>|Q54AF37Z6U{|@8&FNS2*vmz*>U`9k}N%+ty=|d0z*aq&TuYCN8d!yM%LQ!>jcgfAOXdUWWI1zW)2|A$ z#ho1U8~8;_L#y`yDUlU~C=2^1(7mr?)k%ZWRDq9I7CY`^Z;$>!LUQ|}>;y;}#U;BB2ZkH}6X#c0S zVXsmXFwQ%J3U7%m`nU!eE0j8ePz%oj@)yg`P|WPRPumXK0!n!>Sub`j^%BenFPM5i z)~^G;ndPh~`F`%n>Sk^F?hLtOS@@^>1cyq}9_kus7CJQU^Fb^#WWE#oY7dd+1epF| z8%5|owq4$ulrq-R@{eFm>aoXzm;szBYq_AGm0{Pe3A{Y>yTmxkaL*k(>NivTnoON4 zni5Jot#nouYRwP0KI=}s?J5X8G*ooP8jku!tg`SH;p( ziHB5jdwKR#!%yNnPHwr#NWuIY1eslQ+P8ib;tOwZb-XmHzIkN?dFP*JhWvvulmH>q zOM@Go=c(|K6uuK{{Om^Uu{b|9+9#-U^yX@y=UoE8iTTU?!m81SuwGx?6f z@E+{XPTD10Mjy)vD@BLZCX2v%_xD?J)IoE{Y1SFzp`WNN|e>5Kc!t?AV{DF#hAFC2e&p8Vg_>koM`2*+XF$VLQ^XmGtk;Ue_RE`NWI&>d}up&n09zfY;&YkS6 zXzN92((PajvLd*(@jnW|?jkZ>7#-o9F=gTR(vp%1h6^c8BP(U~mbh89$lgvB@i}>! z8~b82eoF~W+2J?pV!u7TZW=&M1~ zzt6CQ&M2X_rq*!oQ_@|LSgMWH-Og?igm;~TJ7{a`1$6uFmqbjRxo}}xovo#x{#BpJ zl=A+A2U=-H6HZ|wRF9QMYE00~WC#X=8v9CzZ0`ntxks;Fxk(!8u}J`>7jb$&zcZFb z)3J9=9l>#meS-Ka<2OTwi9bh0EKBIk;At4$#@g497S4Zm?tsH;Zgl86Y_M$H+VG!V zoXiiErol9ehJ(6y-(}oFp(P%Q#eZ`M z7ZC7mYJA|qgAtj|ZVdFKuX@-ao&%LTHj&BYG#R%q(JpiVdM2Uz&w=kI^K zZ1sbqgWE7=&d(3r5Ax_U`|P?e*4_Om>=QUF8-m^ zE~_Jau)w>6HdMa8@L%U%T|qw)nqJ)6XYMY|LniYjuFqst$Ku_-{6bmwfrRQ)(rTQO zUsD+$?xEFZ!4pg6zUP}8jrw#nob+?~IyKKC3J66ER>v9(izJ6e?ifqJ@M_HR`oOZ5 zkDL9nOJ&=-FZb~5z;}#LWwWvjR3Wn;uB%YUZX;2ID6;_&Rr}h|a3|Bq*tc7Og|IO5 z*{Y-hOKDu~jCj8VE8)Z@wi)W}bA~pNb06v=;`Z;V@4S>7ixTTQXF>jRD==q|>e1f} zGJO^;Vbrj8m|sf-f*I+1N(=GE|NixIEq|lYbdNetZd+(<_IXuKp|6s;h_^(Wq$IQR zyV!+mbfZVRKZTFFe~XV6Jr=l6zK+T=Y7yl4x-4|Kch9SS)7W5~=IiilTckSv*}mK& z_5Q<$DPY-JIVsF%_jx@ZOQ=u!TAAlD-s}_8u;APS3_>=Y4@lI9AD_2J;B7>2s^XIP?0U)ij*JZ-_AW=d|K&yWXhU}pUs+~TW-;8n>hC9%m;PU* z@Iw<}8IP=8wqN=#nhb+=K(W&L3F88$30PQ8LrlUU-o&aksXnZB$V! zBnF`YX$UL?odKQ{;b<<8MYs;zo3@L1THu3Ph4(jLcq1%9Z^JRF#rlum?}t_=^8G~Z zBMx;;Po90cx{>dM94}899tMP@h#~UKT2^f?t6Iz7>i91Ly^``~80O{9IOaN+M{d~D zfQOv=Ux<;kU2Nfv^>uID6Q(R$-(6xf1F(s$(y%WQ(f4GhjmgF%y}Vjr60gam$;)-x zlqpdO^*%8b#5Ui4wjpRr+rjHw2nZ=dO1IDv0dAHEhP{1K_?leFP#2cAcOp3@pVAPS z3d>@h%dL(5S|aB9b}^PA(1Pa|9@Ozjvh16&;dfb2wdoD$ch%GAeocroC(w;ZtfO`k z)u~3*_!&1=4krNjk3spD%d4N-&gszD*8h>uynODbq*$0ZJunv%uT&gSzj>S|MtC+d zop;7^$gdBb{r{z+Prk4V>W;ytbfn*ErlzLWNo^0Z+L#hlk##F)kL3(z31TbQ;(^=C zfL_pLT&@Dd_2H=aUR${+~2BNYxZ*#ipCEfZM$9j@GsC%Ah;_t=0&(cV^NZ4GhADfr#`#!N zssM~GKkwmQj2n4WAH8*qjPEU-0r{r1z3S0J)JPxe{i$__QNiRkYbwiYf&b@5g+G$| zUl!c;nnWTN4=j;Ip0qCByWh)len7KzNPsRU)awe`gZmC=0gle1mp8P;)2vr7=VEWi zlFO&|A_Udmf5rTGf-i6zA`khWAy@g# z9{tkZ>9OUIW3yPiQ#)|@y?gf(u#G5!i|F0GX;>3LwNcD;`X_P-oQaAYg6#k25Zp#~ zZ^FlMir#321RXAt<<-DIO_A%j?_uvs_dAl5V|IxO_PFKlq|lK7d<=`sy$_CRh+W`o z-))w&>Fg$&7%P?e(O)BTcneW?zrd5T-e+|F`_vmZZ*~ighWYh;URq$SUPBT;~)qIDbz1p0$k#?>l>R z-&%$Z8UAz*E`W_75ksm*JOa$>k-4nr{d1}bY$+chCBR>b*c}g8-FMmqf6jsMX_%4I zU%PPCaj%%Ts!iyL69d!@!a)ZdevphEUQhPBRWJX;hkLa()mxq6sC$PT8eP={HA3j^ zH}`|(*L>nLYj=Q_GXww<+{ik$1Apd{a?VzMB1bqu#vtY`LDm&Wu3%$ z!u&@(@Rw^*bUy@CSvP|%0*a4O^S4!@ues37^^~q=?x%z+87o>h4=gw zLnqWpsWkKN30Jfh(Ut>1uh%`e`_o4U8vA;Bs#@%9*(tah`PHpw>B0|?YvMNO&mbYS zVHaCmFnkX8zI6}ds*}IUJ@z71Et$Da3|zcte@-jcRR8!RXCT3&*K}Tm^q3U9TVby} zy|4~qb}kR^uu6BahG26P^+wV6=T2w)ekFnnC8eRw7T4Z`9qX*`BNKha$+!9WGno_R z=AjKdnhdY=aj!oY-v(GC*9Gdp;eHKlkJE5_?GXxu5T0T08TMEA@0e=-=2%28c#3lzh}cABKgcwgf9G@C&B5R8O-pEld9Tp+bv79G>(}qeg&EW4e){ju$jobvgD2fG#~HMfl#yzbjJbD;(^ zLdWH`4W=|w^BcGlMOcz@&JVwMD;=~k^C@FS4XYWdVJL^zpP5Bm+k4^!QWY8`vmWr z5fe0i0xr4A!aF;jDd7nM-r;$Q&=J=+Ro)o4Xor!>!v8g->)SnkFq6YKF?%0wDgM)e zQ~D1YR5J6p^X2I1WFSuSOKhL4PqC}>aZSm$Zx?C5ldpXY96v;V*@~`NnW7=xVylos6Sy_35UgrlgQoZ2%wcMAxmNtf8j zG}F~Jsb76NGw_9;5K5wcGa%9V^!}*nIu9xi-$f&-)B9&A$EC)vazDL#PX!CV=Bcu71ILajt{jAw=rJayOaXD=xK3Hh z(dwStuUaDgeRj0Spf^v;6k4R2<;jboimF(V@aL_I}VwJ$&@XdXp(?*gXHRo4X!y0EU464+lW(r{Plv_V3@Z zW5;1pw}{tWzxDQHu-HAe{~Wb{OfQ|0Vzr;D-4mwphQZkB{5xV}@L=HCd$Dz$Hh$Ma z`Y<3>`BivuF?=Nk{{f#W@;-m=7I2fRV05&?XT^@&eSF}N#UMBLdG;nY@n}!e1!B@koxysLp(Af40eKPtnT;%));5VUW)Y{-c=2%&w8=xh9ZwlJ5d1wP*fz-}#?z?S^ieZX)f?)R=V# zz3$mqRTeCPu;#mPbJO;wT@MaLe87Bmx2ImRjKfQ-CQHEF=eto;YY!!}K^|x5Vy1m4 zPS(Q>H;|Sn(7HY@M9?6uh9Vo4ahGwX;VEqFQ?7wEiAH$tmg;0In9P>9!I(AI&N~3l zD+XqmHw0&v%m8CzK%T(JM{}H+y(N;Kfl=*f-QNBGQQqIt>=ng*P^D~#r06iaG}`$@ zzsE;m3m&JRQe-jR(tliNWfI%8KTi>ZFL#kmL z53D%=pq76QKq3c#s?1Y(6#ZF6V0d-+nJJg`pn>TROd%vrnHMAp$?(wg&5ls^Y#5kf zBgZADa}d+C=we`rJE4^H^VMrKvaNdH4L;ALb@8(6!-m9KlUSccIvEEKrBGxELKO6I z1H0T#FLc8BExrq9(yxoEF6uWoR8$X!Cf0ZKPkkL*^&p6a2+*}U zu(+&e%>vSKx?Su_XiY*wdMx;|I4rh%Qh5um2(jNZr-fy4$t#%@8oKi}IXIG)^t_3#1GCNqTOa*z9kY`Cmm|AX zD6{O18B5lGpe7l5_T|K0>iXiuEn{pcV=K2Z5|L#&15+qh9Okuq46hSP>F2j|U4M0R ztYa7As%ZnsSz3FQ%IsKU>%gfCchD|2`VS1+&%ZB)=a7e_LNpyI9{;GFq*|nSa4siD zjS9CjEzniijmDbD@^aH*KepF>JeD%Z3=@8bDxm@^0vn9nWEn~ZR>bgynbW>WG`DXo zqpl7#w{<8}X%CZ-=UMPflD|zp=U^N5?mkKqMYgODa2(ZnqE#QOx|?a-qY|pW^{e2M zD6*z+^RV{e*@y@myr64F1($Dp?9|PByexI-Tt5GORaS5bH#zKorQIt@{|;BVp9c14 zj@{p6*>}Sgh;&;<_DDn(5)ZPvlG{|@Ml7FxJtKHEI zzXu9|jNI0w1bEuG@*1dy$j_78pMd@`aLV{Ubn&jiYd}r+Rb|r&v}kjhj(YuyzpcB! zc*(Orvgnr8uXIfJzr;O(2iK`*&*GESOb(5D{YB#m9UOzr^UYGzO(N}}5l6}HuHs|m zhoDnPgQCnvJxkb|T%R9jtGS#N8OexF>MA?^|rnLd>7B7DumTkC~etHnH{) z!xe{lT9y~ApiE5CE?Yt?ei5OJ{~~Vm1>0d}uE#+5k9}H8DZCazmYP^(>fX`>J-U|= zi)ts=g|O|++cE4L>z6%l-E-BOWxfXKves{z0FkhBTr;_xuQblp2zNF z3$<+H4R&Yzu07>0HK*_SPPK+DYBJ>4S@{5zAj%D7enE+Se;;P37F#-X-Lj3cv5HUc zdMvE!>hJ6v6RNt$P5-oVOF=d!yiIvbVDuf_eE6@i%WDb6Lf}(#$~jxP;#XlIs4bc) z#YEH>WEB?^l<#J%HEHR=2<{Fjm$G8jAxrfPrzumXI^!c3ak$xEFRpm2p4Q;Q&m`nK zmlL2joDbQz_wZ7?p#1m0#T=u|smgKmvZYIBZn0W|dsuVfUGQ%-$?bH7 z+x6*b?glfn8v@)7j2;Zo^!~rQ?mygsIV7!D%_e{k-gu(RN^8R$2-49T{Y}3j16X&_ zFZac@CB^|adEO(8TQUplBcOCfG5vYQvEf@a2Ct!0)}Ho6y)19gNQK>a01eJPKYM`j@&6ETdHuE03X5B`aM`J@VnSto5$Xs1DAh$>RN&MBdgQf;D=w+QcUm` z4MEfwM#p)e&7Qxq0Ph67aUG;wQl+~N)hctSpJtbYt#Bq}5H2aXF3s4bG8;NVamjP`NJM=> zi`cPePk*j`kUjk^pp(xWLiqO%)dg?v?C|+_VJ0J|BNt8s%!Tf9r5dfcJ@o>y+23vB z&G8c=)-^cf5f}#j1-Ato-I%o%v7B_)q$I z9$lw%7E`}&&5l+FQpiGG_Ilgx<-EgQr6U1Z%$H}KVO5-$V=@YfFXfrAMK2``JOgva zISW0V;YDklWsQSl!Bn-&s_9f+z_)?L-FLvC`t`>A=dCka778hMezTBZyyFy6JK9$N z#r#o%YvM;^YEJXzH<1l9nyGo+gWSq4T3&FSpz(Mtg+wNM!)vs($1~i~M(-ln_Go;{1!(q~V>=g$k9mvmX>^tCsm^EV(jy1>>@G$xyrKx{Mqy@cGBe1FyI z)z5sln+UadcI>?4+3-R>JgA6y9Hw2c2}NE`W6Pc7;@rZ^>1qudmZ!Z>q|@{IcVrJ* zU*MT#Bdn(5;30-FHM8?$bQn`##!ro2Ac7a=2Fh$nT+I|JG$GhRwwXU!>li}b$e(;GTTj@PIbfIgN*^SybcZOivH{Zw#Hi;Pb zc{@ANn|Zl@&fa>=-i;?BzDlV-ii(KIdyn^1U9oz#c3SX)OXFO-f)r%HxtN%8&~#ld za$|Dn1aEQq@S1Hr{d$p8hfBf^;cl3vFemTbW&f2^KxfWb)ju==MW#?N9w~_REOwj4s)M zW>@F>Q(IV44mVeycc?;L*$_R*(;s(_!)1eAz#h(eSPD6j|S1idkFA<+u_utp(GPDMXV3boi&}n=P1(<^G%h;uj z|HlQ`N@ZC6HhxCTa&!MhyDU0i9TvG)COG+6!}%?#tGrM3{>3qci~gFw>|V7bT=n>% zRiBc)l@sA1&M>@5@C$uDz7gV#wzTmr(#Vhb!?z9nIwRmQ%`lX6mKE2t{IuSE(j;x) zn&~ZAeAj@q^u9O3$#jMz8nQi3@5XLW^$iZLDu92^S!jKG=5+D)YP?tD=d~73MQhU# zfbVhbnsl#V3HUL7ZN$z#G`x;=j|~Nha4_VSAFMeC%IcKoVD_r-)_FKnBR#Agcm@Zo zt*@#%eBQH1_gT@YmlEyy?#u^S?mFyU{MGM>nt`skk3vh20D@nHC*x zqH0^ZYSr7ifzKdCn;TZZzH-^(Gz<-CWLn@5Q9_G#itD}RTVE71zdOc1U-R}YCm;I6 zW|5`K|Eu&=Fri=|T&xP7JEd_uP594{d%fRYo0l=-X{G;ymUUF53#c_xft_&?8(q9z zcWmDt_bo@awI1{AjAQR2DfL3jY1hmVt~qrzGr)9alF1TIp8NbSKgq9dc-JI9gddyX zyZ-<1ocNryb-zmIn=h~U{`vG8-|nLqFTuGcr-9{O2%HSf^n&9lw^b4-W@QIXa*2dk z+!>ZDtfT{$-IT$F1qJ6dEA+oNZlEGtH;7LrAM!qsF4<19*C7t_l+!ujH32^MrQaZi zXT*m?zqY(!abni8RespjJFI4M%OTI*xm9}+zlI6?n0lXCQ*Jsb_V90(naQXusL0MnFdpE~s#d}0s>-Qv+ zv6tdn8E1*CQ+R2IzN7OiNUt6WYn~x$@_n}9Jw97+2GpBLhM&t*UP;tpt-2!S*W;Dn z*4g=vyo+A2XG8D6MLvDObs==?04%Ae7Bc-SEYO<>uESEPk;*}MGZEsRdRGFW4gvVX zYt+SM8DDgoMC5&QR_d0NcfxNr6UgaH+Ilf8$q zaWfE=zZiuW4)h*!&w26U2Tc0HyD|nnS8E)dO!>@xwE}MFd%SO0Y8rm%DR@tl8y7&Q z;IwXasKUN$ZIOo7KlEQ`UO33){=t5)Cr@szTLFaR|3z5*`uZPv^iZnGoR_J*{_`mRhiW)0=5h9P(2ee6AR<#u-A=?>Ni)43j!CXfqtjBge=x;_C!J#^g&* z&*ts6Y)H9>A~)9Z2aup+S&r`4^2V4=EuIwNi&Dm7Py6lzQ<$8yrtYCmBmawl8gC%2 zPBJN>9GwbuZz*r|xcHbdJBD<1=WbK0SRZFx?U?qiF6f?=Wfu z+2!e6F0?%ok@>^tvA1sZ-=dT(4GmN~jk9|5;g_?h)ZsT91y?hX>)fqdTE{&n5WHZ0 zHqo#e3I51%yXo!y@7ZK!W_sm+`!$GbRe6YYivH94TA7?H$DiGHF=r#$y}H7whQE-9 zs=nToY!EV^3lm;KL%CpVB^ZEH+_|U4K(n(iy63=vxZ=I07RX6PNLc&jZvN;#i#pZQ zSls@OaeZ&EC#z_d5b624ZdkkaVuI;-o4xtDCSgw4c7Oj`_Ed+s-y-d3@3~C{cCNGk zOiQFH;)3-wr46dv$U3&(EUfQ*qh$i=iN|~bo)o;Qvb8vaX%Fh4Sl^BSshu_i!#K5? zUbR!NUUtgtoScx5YgVTPbU<47I6QgHlNK!ZKppuF6C8u>(7;$lWZUIh0S)0{&b0i5 z>a^_lS6j+0PZBBpdx&a=2POF|vVEuvP}m)YNKY#R7PTf9^laZ>Df{MS_)@d>3*@Bg zTh8(oD~f5yx(x|NDmVP0$XU!vM0W(~K$}zO7S+7rgkO#oKy0;Dn2pF1ime+$RuBm!s;)n`ZYcbE$JiUM+MR~QNXgGW7xKm7{8&5qzc%N*Kb4>}Nmebfn*N`) zN|YPb1v>#UXAT(+mDcy=q4S?%m{yxE2`G9vsIW+Nv}*$QrDFg=Wy+sm@R4?1fnsud z^3&k~pGVT2MkN@)G-l2Oz~ITiskQ5jJGXD+jMh0geIcB_?V_}w+HNQ%Ae~mO881D2 zXH9DOFdpBJyBzq5tv-Xg$xhF*V==qaP(g9Qm$El1%1s}n>F_XxLJQKRwQ}-_jAN8( zh8MRS-BDz^B@Fi^ulzZ)XSZ@&eB{UxM8X#4=EW*{2B%O)o$dRKTPC?NBfijVK{;F` zuHlmg{qZbef}-AP{Q5U0p|^SZnrZmHt8f(9RTQ;o#sprhOJ*eQ2vyDX2Y4Tv)|{Rvx|eLCms55 z01G9XT} zL*=+nTL$&Ds*5_bh+(qNTS3^fva(FQynT+-&z7NTo!Djxp}*|o=@5stk2e4tVHOZ? zxtd~a+~NMk`vlG#kgVfdXEWwj-?qRWtBp96Mw zkRlte{)(9ab_yMem!WzOb8zV~ay516mA_uL7VA&n;ngW06}EE0@QexLb|&5ZZ_1J>v>dr8(MRjQ_X746{|t>e$|bYJ3$ zVp_nM^_OD-+FuWLRFWi{B53q>h_g5`oM5=}ulTWyNtV7I7WJFi$)}_daSLG6Co~)S zyW@+NcB%C;&B8(`zRi|HLtIMcVBFwkeT$Oo&51ojZquN7W4fT7-$!<4DSwIJ`vm%{d$ztA8rr7ddxguKTXT-i{o7&S`yoF!y1%M6jV!+9 z`26R>g8l{MW>|%g3&rK7W1}2eQGlO&o1#_D>8>lU+Tu z_NZ2#}`iU+Yb?pfA3P z;P;JMkGf}WXt;lbner&SXW8cHaYblab>gsHw*%H8V?c@>#DI?GP8y&q494ITY{x zeHI3HT>L&i-#+!7Q^af7snWuS6@VbIh3LcK#~KRoYru- znY}6mr*0psn7esx{jb$7HO^l?fDa)mbKyVy+j876WZ zy$L>NQ*{?dFUqphUtwTxt5FkH(_&Sz2o}QzaY;jx_{P(wE(342731|1q6_~__R z_uZtEMy)$zmrp4hU|I$8*YL$Po~*11T+nkL2)`F|n})ysW);D~F5UePYJNLdNpa8{ z@?coszU+D7UwK0X=RW1;ICo>pWxqAwNs*7Rl)Y6ATt~z=hqEVzQLT+?h0R@QN1rr_rIeXCZ0xa~lD7WX*G6u$SdnKJ$2u%GbPTGqYS5e>3vh zFQ`;au!FRMLQ|M?7jdzW^7BV0b@0vLFBq>qN0Z+0SzkGv-tVbMJ+5>MOchm{p2OIu zboKF?mP@ggDF|kmqx+OXW95`$;moP9Rq8cID??y*$h% z;KqhOzB&}q{=9_?&wkIGzi{Clq_;)=F)!Ic(@RgY?)I&%1O)Mi5Zl+l;ni%EiLlOA zYtUerwe|h_(XvEoUZ;7-G1$Ix|3yT^!t+c_MW?&B{M_WoqsQ7bbjJ{2!K#Nt@vC8q z%ks{ARU1*+n9rPfEWhI8n>WXQBqvxoF!!S9PoXwf-IYi8nbj8KRN2MtkpfLHfudpJ zdLu|(vK7T6W&hv}SC3UZYVQB--#rahx!*t)0jH&RsvJT@$C4i0811b6@uijKw`N>H z)%k1a%tGVxFn;$$LZzXpIi+ohlgOhyu2itDsb@?hd=Pgq@o3aqm^TLTIZw&`s!kyg zQ>IHd{^#`JMWazk*J)F}W)LUP?#_&h{J#(GLPvViERL>KT|?uxbDu_&rcCh$Q#nqt zjmL0v-BW(ad&+tp^|EY>WwJEM9Tyv8JV|fv67Y1dUxyy1?p22y>Zz~}?|8=o9E*C! zlr1ivNd-N#po>z8z3Fyuy?53FY^=~{N~jTxeUoV zwwF$f1opeJDpT(U&-@Qvus}yP*+35;O6S2{VbAFs< zn^wd<^RPIp)Arh`e@>seexTqAVB9?UzTng;58gz`t{-uNhNh6{!sy@n_}v98W^%{| zBwaAUL=RPqR8J(}b+x@QHdF(i^q4V?Wi3m27Y0ji@@U$csxVg4Jegnz!w8m95!`sa zN~ad=XcKE>!~pU*w>_FRpYIoRb+VRf1avWRr8@awKCx06G+u=>W&fhE55?#9njBC? zH|ie}68Y`@dQ3?{e#PA$D)s@Lp{n>n$LCjk$jhtKy?ghj+6`B?g9YA>PPOyA!${g? z|JdUgVwBdEO5jpS$g@LwvSp(a*9*kMBUzT&G4vZhUEy)+l-}VZM^xtz($!T3{FxQt z?PXaYW}-Z*yNu&)CrwJ(_Ip<~iUW@;*&NA?J-Rd*RH$N$a`wx*YZ2=J&=s9KpS?BX zz?ki*uzDmm60y>}ivJc4-XBs2U$MKuGz>x&4vkscx~Y1&S=c+w&%0Pv8Yzi>#TPAQ z99G$#^4vC0^-m#--R#V7TF2N_N(H=w15Ybw=G)D+_ypsVnXWey-u5d0ZU|Nf_GR4o;r<1yg3XG23+jDIBWtT+&}#6}zbr zc|D@}QVRsa=I~B@brl3qXIG%I;f$FTMB4?i;RP^6>MoR_uZ%L1!j>XVmaVflRu>rD zPf4@8h8hyD;-wg;P4p-Hs7<;T7Xgi7#W60I{Ji=HB3j1P#nV_3+KK1QT7xn7H;7JN zc74OpN_2t0*N{xpXz(kl$c8UA;iOMSyKt%GBu@d!7r@3~;J9Q>%UUtJ_bsMPYXc^(t^16jO;4N^H?^s320YOCk09CWhEd?&kP8=# zxMT+5L$b27yR90xYuB!xn|`mg`fimK{}Qi}%QL0mcuntUu<K2RMte6C?WmgOgD42|5if zrMyk2&k{4WqqofJs$kF5q9)fY-P0S7+0g{|r7Kq^QF_A$Z=#n!p0_Xhjo~j?Ct;Vl zj6;Iaga%`qwZ*xr^?!{Q4VL9i39YwwoW2Io-5GR4t+fjmqwLe!NPD9-yf8#6NlY*5 z?z-%nssVfIP+$=GI0*ZEUUI<4bXvIk!x^w?sQ;+~a~7~sMm&KR{4Ic&fr0h;eg>1a z2{nm%!G1ut4^doNJ%?OdIQO{1R^l4N z(ms8n?`K0jf4$?oatd`9XBR&H@Q3=dWt5zFVY13=R)&lnyJ`IhA2uIuV-ClpN_~0E z^nwz+teO&H$2C@j8pMqcya9c@g|_x`$pG>*B!Rs9hylfz{XJ??S9N7+uidK=>=U4e zsmBn8rlVo|bj*X9M>iV!l~Gv37klXS#AtkS>VOg`7BeA>rQ-geKfk!oic#G4MuUGEN9}RNx9T$vYvzOL0)J&dh3@u3Mb+~ z99>p}shdX_GLQ_icgOH%@4%1|KgEdwMojSiBiR7yqFj_kN)S;>oV(PryS~FMkY6)m z)TkX>%_d&p3Nbp?IGON^6JQg+Y2(RzX^7(LI*otx>5~s%A^eHkuWxC_LSh;2LicO^ z>1R)MeMeAf)rDg0Tm}*BfGdY{8d#0gAyJzu*JEwatCI?L>iYL?E`(5G0?YjPUA=v2 zJt^(Q&r%+uvGL@J;)gi@z^PE3e-ojd>2alOs@)_7m>)Nq2dh1i zq|T@SA$tz*_o7j=W|PyB2Z29(Z0h*p#HeQD9PQ3P<6tGK+KP97&+4Ue&$So5gP=`g z>Z#jmt^ZvXWmg*5rUeLt{7kIbFOSzXky+M8Qp_FL+YgD`9kiU{eU{<8-P1Gi(IXp; zyM_7r%6Sz;D1+}0K67%4!3shdBK>|hUO|Qn3JpQ;DcjNNe;H5pDqVeIztz7-QicLN zH1E(M^4aGh9B(l_W|0c!GaBT9!#hidKw}@;)yY4`U!Aq%VT74a|cNDxM_OW$5Q z2o<3wG4o*|9$T1KEn6_?`OhY~S&tdB4ZYCoWoYGXBuplll}=^6QFrlB6O+9gKz99K z;J7ymB!3x>j^LewA0PhIAX(ZZkex9uUn|{R`9>?$(e?7m@ z&e%wUg#>kEM_y5Xo)9I@1n2<09VF?Pmd3eW@F02_S+rDGs zd@<}6&;ZS79j%~*er4u79UUr^K2F3D-nl*M$5GK?`&sP=%3POo0Q3dl0Gm({Ui zNNno@E2%jGaEncw;-x{uSs(goI{!nBYGFG?A$AN8)D914e07b@ ztd|JH_B*G#GUTz7d(|{%*4TPPv8mBE_dHzVkBz!BMmPo!*ds*-J~-(52LyyZDf!4) zQOqJYLs0)Nj6?3d7wZd!M z*2Nk-e8nn&btsJ^i~0k8*&J9CK=iDVdlFuS(?Q$iF^B7nBzJ3Qi%DRp`x9Ol723#S z<68fZ3*g;eUR~Lj%jq`M!^6*vpppOf;5r%V)FCaYEl*wZ^gu) zxx_d(MZ|J21CPHh##3LmY}v9$#)1oQCa^Ay?6^4X?Q*dl?OrmK>IveONGSLB>i8kr zV#fQ-oSrOP$9TeE?P038JkXIbW%bMOj~6;KEpy4q*n>=|THLyI%WTh~;NW_s9}#nH z9cF!!X$fu}6AxDHZaW}gt3^PUCmUaPAQL~C<3EX(T8Q8C8Z%)WH`UIIiVUvF33w?& zJQ=a+ePLlU_>+1bUY07n7CWC!!g<(=5=?%Sel(b3fXKDI%f(y9a>(fHu1(7dT5Uw; zM@88PtOL5KPwSyDED)~yjTwSFn9xuk@qQV}Q zm3UE()_6V(R{L>NKHfS$9cW^bCct5n1z8z(`v3Ea1j^Q9FK}M5r z+_)ioOI|NydL&LX)Nd!#Y|YJ8`EyiXs=Z66_^(e6b=>du!B{d76pi67Vqh~=g=3e4 z)6*<&otPt$F2YKGrY<=_#Uh>xD_36WSRlq?QnZAt4u-VjjjLlm9XZY)D?t^e**$5s^tpMdYdXxD1NolFjK!)L2me)wY7<>^MU z5d8J)*PEfutjN{04eF+^pNM=8%+JtaCs1q8jJhfwMt;B8wD;5~NvxqPgHhgl@L=}s99vE;pBR*>+pm<_%I;8flK{Ls2Bx-H zK2$nWF=;1pwQX$I8fP~9djm=?pn_$=OmeNxsS7FNC6X;1iX9N`i@|ONixq_OSURNb5Vw}HIM?2hfYAdFyTm*Z=#pvW zQtx$+6UYD??L^e{J%Qq_&d&<1eu@Wym3ze{lo;$DT6XM}u+JQ@I}ktQU55@I-U(XS zi9tAHLW6zJ=}dS^wGqW{f!;XtP@YDu_}Ie9SLj}(&N2x6hySz+?O<0?@GDMMG~=R_ zm}lsCt0h8wN~;Mzx1Ayf|87lLpJx9UAuPeF5R5!p3eKy#2w$v++6>`)Nm+s z3KnO{*jV?K0N6;Y9d~+tSkiEYo1@V_7e<0&XdgJ1sC^TaFZv*{V9$)*&`&DRAk&A; zmjtn(T=PhlrqeFp87l=UHBBPPHW~1;_sl-EXSGC59&6&!BXFx%Jct(HN{xmwG->(D zl^p%LKEA#|ihdY5%T7uu-fVLhW8?K`;|^ev0egdkS+^ADH$C1#>Cc5R{qMru&&+Je z+--v87hG=`P397)+j{-9XL~IXUDV1HZ2{A(x4BJ``B(Idx94mI=d-O$GvACw4q}?DXl= zE#jl#)1`!3g_+1;VqOJK7zXhT=Q4hM?W-Y{s7wQv-3~ZI1>O@lg}SwOt7=_d8)(az zy6XNaj&W|@#PX9L7INt8v*}QNEX{y`#Ho|IN%A9vYaJMhm#r7K?6NPdW=T4%T_J09 z&DmUe1l-=4?-=m6zk}y4{4)hmmRN@{@>0oLrV=gblt=(F*K+K-cQdUey{cYT6#|bH zepTjpdeJl-_sf(DiBe8>l*87U9A3gx^}=Jl7DpVBN($pn845PE@L2frM)d6pn3|Dx z1N!iHz)`eb>|m(k?U*pBE2%3%SRLNc)QZoO6T2B0C=cu*hXE|yNlMz5(?kC_0Mh7F z#vI7E*IsI1yRsDs{5btK{_Uf1df;3 zTi?!k2_w}6uTisrig?HZ%a-!~&2B4L$8A^<8 zgF0UC)U>9*RrmK`U!Ba#SvoTeKFr0n#m=(Io=`4@d4$Wl@1L|bRh7?$&wremmDG$Q zLed9e`-jI^z{@cY2|bViLX%6rf0vPfn5nV))3bv+g$HTclT4T(vgUe?C0?(!wdrM7 z`C~S~gDe-X|Dij@SHm0rpX0@ZM+~aR53Ov$SFk-e_bIvb`s#w##PBUJJvbBE)FS!Y z6t+y8>q7c!DtBh0o-s!W{!PY>yWU>y0Cp5VU z-h8`vH#HszfFjefll{xZf|~L{2%?R;XF5u#aR05&Q>te@IWX-ThNr;Z<+qrW_W()~A33}wS;^#=_SSlx zAsUyv(7@}u>-C3T{`GCgSsgZ(s!Jz4^%m5gPzEk#pisKSGtxt1*A~~t^Gf`%ghiQ= zyB!hfkE3+oVPhjIeox0WVC(j zysdslGo$o~yMo!M5u>Gv%MX)fmVJDz%T;j3NBeM@neq%m#ZaIsMpf9(SsR@@`ExGO zeO?8vVhY9o#9nKFI?B@BtLmVdW1pK>;KvT)j(hQHWo4!K%jlJ_;ZB*c)1qq>zE-c< zw?ui^ySQ55Y#H;yNqznP{oQSka)|f0LFvWT-1B=UMhxiUDAQ+WOb9UL+0yNdzUoeU z{OIY^x5NA&KYH>+>WsS8gL;GH1XZ+|0L&?auecr}3=nJ5F5SHOowc!V*$f|uN_oFu zb#6hyE*kG1>SkuO@295PM#Oy^(CELrn49``ISfU)E5is6B7H$cTbmSy=5Qdkipr-s zmoD}C7wVdPbb6_opy1T%uW=HvExXK)VNG$MB_#&5UjZs+n}^$@D0(D>@Xu{SS>%1A z;cNyvf91sOB2Pzw-;T*x>`1N9I+M~+s8Ly4l-}S@^0I%QPtGK8x%Bl`0>gH zEaT(5P&F@u>1OIyX0!ZIJ`}sQ3_v?$1=EPx3Vn|@=}4S2yJ+Z}aTH~}1$z%*5Lj!U z`UrqEB9GJHc=FqP?k~l?t;+?c`1b!FTW22EbN;>kPm3jMlBMiPlRZmWCZ%L4SxS+$ zhz8Bbu8^_|Nh-<`Dx{EIOte@+QYcvq*+VI*wB65R`OeJacl+aBzC(RJ@AvC<&ULQq zI_L14uaIM#oK!nEsoPKnoj@t90phcyc;x9~1KpNvkhff3)Jk1Fq5l4$>-2@L!;l(A zzn5fq?z}gHux#o!l^Y6-gJ`#4gqqV|fBAxn*1nYG$iX!1-PBZR-FxO%{j2AyWWo{S z(3(@gxkax>1O^3JUdX`!b(#N`4kIax zJ__8qUCbQ~vf5k*?4#MKA)R^73>g=*+*X#Cy)zkSXD99f+3bgv6M(Ji*N=pT=3J!H zZYpLPnawcsGqi)a-327)tSCRK8>%GhN@Pial<2+}NeUGB3pDVTD4tO|Z~Ksi?R|ei zd=_c){i@C?B+a)+wVF3~f^)9(@iOgDn_=?i-g(%9yHN}Ilgr)pI3V)C+XcYz9^c7g zxTWyO$YBSreWT!$X_7)cAZU(Up`($}rp2tNNi#fYi}W=JNqma&W>4-Ji=c|FjQvL8qWSIOnYXVTha`389H#PY%ySL@2&AjBn^-iq-> z$_?2l#^#$O5=)2T<@DVSDR8TZqAO5KjDUW-wrx{JHtS8=!(J%=4$uOUa6B-Ns34oW zsrB)Ew}w5CoPpDK3*}6q?l`5LJ9kP0T*NnKrL947oI%x$Bu_jHiwkNG?B>f5YkviiHVT` z^dfVb32u9vb!sxr)s-7nckS2xUe#4CYj;>g%7A6r4_ZvWb0-l+@C1Dkf>eZm#4~FW z>pqRJ20wtWT6iphyKs%GD2)ZE0z=ofis#)hd)x-$J4kB`uu`Tj6J5S6I|wr)co*|s zOyM8>s`c14Mcht6lM`v?<;&?dxoN>YRA7kXBGvCqoCS0ftIdQs00;S1v^e41K-;Jd zHZwlT5J)^}iCl9TEP6RTU0pN;5@7xS>XO5>+9z?=M7-p3GP>B8;01v`EX?aC?BVpu z_#Rn@85Y-!_3()xO8Yr+fbH$w9c2>)RGt%>qb!mWe^rQ1h|Evqt%-<=d5jrY7J;!| zw|ZZd|<}1x(eBIj-s`RFw^;^Rys}?^%gDa0(Ib-HdNJ{iP{j>Yz9Q+EVgy* zm>y3^DF8S&Koaq9Uh>wOg*%Nd)}=Q?HXCxv#yYft;ai0mBryXU!5SXYBfk^~Em^<-y9%P6uqK2y}h##ow%{Ixq&2qM-yxQR7vvTj=Hh96hTUqGFlGvpJTjjWb_asdk zYwlGOKASC63Bp*wwvZ|FTP%z7qiIN}41ihg6^~jnryXOkQ(aZE{&TiXaLao+9B$i4&IUKEA&7kq7n9>ztN;A8L@k10xm+qXs+&D647zf#t%$ zz(Abawr&rwFsuWEZ#elm3e)I>s$+VeU(sMmKv{Lk)OIobD(e`S)|kL@9EIBc@Ha+8 zwFg_QQ$dT6Kf3eizvW4l?e$u+c=5U)<{()oupfjSIp$CU8M+6LIB3k6#w(=DJXXztAAYRU-am)Wzb1Be}Ky-pQ&!#HxBb-Zhj7M14r zV>XvS=hHLqC`Z$c68 zzXl5P+ABL!UOAlo^P%JGd|I*7F+Kne>j~F~@j#v#j<(8Imkvip)`t8fI+@|Tfq?1%x^*>s8yZT$2@DL}7ZIU4|9neKT4JdJ^a_Tu{M3%GD-A;A{OBirwYk(B zHsK`RoKZ?4%VOjL=4_sO~s0defk0pq!jt~?Hj3B9s~9QAHHY&iY4I(4y@g_O@m`2`WK~w zd`huX!|&wMgOh+=zi%Ywu=!w~>cVr6sK2Fpr!Jie4h5ZX{>G%6*fCt035>*hu}e>B z_Lt^k**A#AB(fryDHx657E}u!|GotrEb}dCyB4O;VtmYGN*`{6&VT`}pnb1V*;%Bm zW&(3kXs^3}-^bFiZ#vuV;}dj!VWUm3Yr6EyuS5@VO)i^P_#~>5WdH| z65h9O#A$ZDuawingIRTL{F&KJ#6b_K1r`|9F<*@ikmI;m5}oPr2H-Z>y<0by!{ST| z>GGvYW&^5_rbNY2m6ATHD*NvxeyfLkk7*VWSLkPA^=q2(mE>>|msRE)*it7VC!cYf zg_kf1INypQ!p`wR2FSK<0`YbWFD7uMnR$#tis(Dl6Bpv!NXP76uQuQr z)OozNc#G^Yg&96Da!_&ox_j6F$G92 z|H>2n(tui1y-AbeZ}GHU$leK=jS8rhW9Bq-W^MWL1nx(V+|C&;#ZAKsXGsXS0^0Ey zM2W)1C~B5jN&TfQxTAI{#Vepq!1M1MtNxX&_RU_}CY{I; z2ep#6*K{ajoxnlXnmwY~r~FILVFQ1q~y@3HV$4aW7oFKymp0 z(&F>1f$|3P5I_rxJqnH9YzW+|bYpr?p>45J?}j4(vLk*qn0njC9O{3O4jO2JOA@~G z;aq3LJK9Tp2_i4(Y8x}tk*c0%%$mGY*3Gs7A9r%>@)>`u_#?;+VLf+UhO|}};*(#j z>e~y>olu%0mqV8bFdNH%tbSrE`E(A!* zLpW;6;97n)j)}_*JMQp)RT@Z28ZaK5U+^0|rp?8ri-K^e8XDCyffK+v_hYS& zG~xVa`Nf$7MkaXUz1*APy*--_v6Ug@U$Xr&{$XfxWBU%2|Ml9KO!~5@9a+czU7R*G zGSP}UU(;|72a3zd=^`IC!I^lw@s7X=WR++6`F^|+o7@Yrv5^b?``J#0h_%^d{#6J` zP>?tise<(p3L2CK30w}HFQ zH0NJS9BVEo_M$NZ#?$jQ=jS+f97v) z$R190B=}gwHmFKFcQ4svi0*p%_U#5l+I6fx0*l$}yLx!w?S!4$!oKNegp|e~`xsvwPZCfk z2yn-Sg_)A1ye{f{4;%mP-MgtMC1HPFUqozUqMS3(2(C7uVp;@6##GGL)R)aVWb?YD zhiasihBbk>K^qhC;uvUEC-7CE+o*7-a+WM$erlbcbj;4vgp!SV=zMG_FHZghZh;z* zf5%Vi{ILg~gStpgF8)_Kr8DgeO5dQ7j_krF>8X=H(pQL)Gp%p*K#99V4YA`;5n0#v zzp@B#dxm?@kb1BJW6I+Ap_9Jo_(WamcoGl~0$R&$7FcI#z8?Xc-OK)f*MMnd*#eJ#2pEa7#-wE31unUze~j>h06N`;6+O zv(!yCAkASXSVYp53BPq-!{DE3bcsvvbuHTwpXPBDlwflkL~Og8o}R!z^xv^#JtoS+ zipXhTY*~xY_l`6RaBv6Stz_|OzH|lu$OFAYl!-D+9ic!8mxuIsHI6?@>FXQH zpv3CRoa*OvrC5dJNa|y@ju7pcQBCMBwmUmer{Hl?bC_tGX_$(X2d%4@$ViDzu&$P` zq!xi01d#p?p17*)V`250Ry;hy7I0#+C|f2D#l=nP_-zG-VRSoL2p865oIeazojT^! z9sNAswkiJfv<0e8*p)hPdB9I*km@d}LOsgmyD~Il2+X5H+Y@!Pw6$fu>#Af{tqkU5 zBC>Y~>9>dkEl`e*$gpG_p$u4kuz#zw9~(zejfH5PbYY3zqvAM(yvRO}(7j_1^vFWhD zt$-h^ur|`9&;I3FYwTW!jy_U9NY*{$Njx&~+{lwx=OSaD@Ty)R4w%zx9TB+Er=CyO zO@wfp##QXHwRC&DC-FR_wvLX!>>YdeZ8v-=#c6}qKgjS#9fLR+FYqGRyjS+|I)3tG z()aQs{{}Kpb%|GXEiW%Gy+KM;SHeNA{hEj&7|+3TCp&+5<7a4cvb!z7Om`mqcG$J@ zmjR=2YS@{?&|<7%n9bXsC(Dp{hC;9@>s-2npzz<#Kp zrAs2cnbrXad&lTPO>eka8F)f|G^h0lB&fOggV0XYIbZ{)sZ*68Nu=9Jc~Ha^;gntP zcY#oX!cpN0UZ&m$M|V8Df)4Z-B+61A?)QG-x}Aw4b)`GO;7bgN>^R)tofmtc>F^Mv z+;+#qvWCO8wrWTPwISn6HY58d(b2%Tt=*c+(lgTGw!8^Yg=`vYmmI?FC-yqQuG0P{ zO&EljBgd0`vDnIj*Q9F$`uEr7Pz~bg$T}i+dJnr>bQ)_S2R?}^N~lJ1*1~dKt74FkJ~ks8zB{I;Md&75Y$v+yID*x_9v-uc_wgVGMl)uZI~n< zpR13lry|G2wB`~>8;i8vVo~4NbOG0v1E1I@XZdP)m>U&M<0uQo*YN&eAP+}Kd`{4B z?bTl3suM!BX}3w&r&nxZ3nlEeXhS*-P#Sx41&aaa75oVKEI0sveedUi1F(M`%yy2O zz8TqreoR_t6vE~DHJU_{XIOP$>i|RXW)QY>6`QqUyMTxz%l6W;&=?!P!qaBcZLt2R zu7$H{d4!?d*mTt7jd$O_ee1;QL*IXXqr3q#OXrd)7-m-wvcP)7DlDNRGK#I?k=8*Q za39)t>QskBXq!@AL@nHwu8WL|qwGAp`L-Ug)2D4%r#p;WTTcJjkLw_N)kLH5Gx@gR z#^}ae6WoDo!RrD0l^F%J1zq8Dd1$9X9PXFz-3!PmS8sHuMI|!iw*2su$2ziT&sOir zRVms{9>{t{7@=vPKm39iTR5WR<1gRhX={>;A(kM}`C8<^=k5+rwn>wve{C&_3>T59TGEE1PWU(^Ltk7?(Lk)naTF*S3y zREh!eExiyTetx<_4#%$75W#IBvw0!V)xSJBiZKCZR#-KuKDcyYRYtad&~wk*H6J;0 z%QKNPDfiaWiZ0GOpmVbiDzTKd7*3kOoDD{?KI@j#?Qk5$yh`V>M+V}UrJihZUoc)S z=R>jLu~RZl87S%CPX8oRB1GCySv7#(PbiE3^qYTJg^m6RP(WDInM=o!1d>oV6$(DR zG*D7Aig>9ed-(97JHtOkToghCCVcl?E3xy9IcRP6=buN+%b$Pva4WHG z(6pH|H$aIdFg^J+TSI(A{D7hph7C?o3}#_%u*CI~y)m-EgMm331F9JG5gXD(?KK&z z;PYqym*s;GYf`?Z|%PG&eLeoXc8x4M^+p;@`hJZb_->%lp< zHuOR(NIpURjc8w!jskHhVXC~DSk;VhPv1f_IEI+{3(U0My?;+TkvbN#IzIXIR-360 zq?|rZrzDOZr_V300--EuCmtP`)8HqD+9epOB{4nh%52O%k6y|YH0eKv%yTciRD<{2 zW%7l<76*r0ld&z9ofR*w-tNF_GGmOed!}lNJaLp(3rg!)o2pvknncpM?dIqcc@yWA z6V#=U{?KqC>cAb_=GX)!%M)9y*X@*zdGPB=U+d{km5QHB5Xn5{oNzC1vDRF^ygH4{ z^41qKzD(b^QBow#Saw`LaZL>yxcsDyGqz9Sp+SXvvOZE8&a2hRYgA(DQU@OQAjQVBd)y5ypQpk`EDX0aSrL+*zxv6^(5oO zq$Fj?%^?s3qGJnx=;+u6%ai+?#f}%3u7v{BfnxD%V>*PRD|8UDPJ_Pw+Wp-N+A{RA zwnDt6gin!pp^o<9$j@K2$D;-4#~n|poI?0{8JoJ_gS>f_K}@P$hYgQP24-W&Tq$F% z<%2L;vktl%E+1JN@q;U=Ea?=;>#Yko(WLP+kq^2xB*cgAENyA~_Uif!Rly?XWP9rr9>jMa0rMv0}! z5ad!Ng|s8F)AJ6lO&O;KZe6wFxWlALo2k;C5iG#XLR6V)Etf!<5qRJGq2GI;1pxezcJ+M8H z0um`Jn`n3;YfZ8<>vWjsh;ghtPg!^N6Yjq3ZKblgj7lL?lZ{cQrA#KcR9&@9&LJ^z z+_2ky5U@A_3kv<)@~LZ~q|TXA*%Vzsc#;Qh_TcnW6vPKUplKM@V)DBDu?r=+U~E|p zM3HT$g?SEdV}MX=sXQ+uk{iv^9KT1zVo^+hl8DRnv#@UZ(iF6Q(PKU2NLLK>^8z zJ8O@cM=hd(Fg^cy zR`>GxtZ^sQQ~7k#ZT9y4dz6Iz;1tn}gybkJoDbQhP@s8y#$?3&XCXD2BASN5Gq7LogV7bETvL&n)`YsA-_s0I1omZpd_f*zWI0D@8j?IiOU{LNev29o% ztC1sz%%5MFSH1nl^zRU4zRUbJj^4!U1b<;Jd3Cn}3has;tWiCe2BGS*r*EG>S7LO6DDZ+W!DTRaWjmBT zY3Fmt{Kn-FBN0O3^X^^~11-VvpsD&)sFq!=%EH^;ZX>G{X=&te^`4zkd+E}pf(fwz z><pv2IezuAk@syFcHm2XHX|0T=K1flS4s@zr;5db>$lI&b)Zj%8Tfdx&piy@XKqG|F=0!q zP4vN-9zBK{A)nt_&vk=V00>GJt@`sm##Dkw-G zFL7*>rdL>slZo$7-(9d*UiLQ31oG{Hl;CRJ=GYaO3MXB5v=Y*zHX9e1Y-~q24cK3k z{}V@{5Am>8<8jwaPFQYX5y`oq&D4)=?zl0P>a{yKHEnVq^)4b1Js=Iiupe|fEk zaf99q4xAs6GN$Rle!b509QwNd$dkRt__hDAVcE;Rvm4Csym8-)DHT-SEOnznMitKLPxXwm5Ua#`XFji6&vOsYQzV5)t%#0QZZhjiApm zaJ)1@_oOK^V7D{7$~lxJJG|1;((;$&FF^~sMG z6Z@FPHrm1X3^BCn_1tS$uO4Pvlcu6+3Jacb-Jhiz8@sPXaVpb6Ao#DBmk{@5&S59} zsKAZQx*yXSyzx4Jl_~)H{jJAWKJzX;Ml2$h9YI~CY@J|Q`6Xa`x5-g-RR>%@Pb%gq zoV_{XZ>LFIiuCgQ_!z7qN$wWFgR>V>)szzKo?P;rh&#`B{OwA}Ax(7!f1K>G8QL-1 zgk?we7-BQH!-g)mny;wt(}O$#DcsyJ@z2=I`O|PQaMs zx7_PtfI;(mV?7+lAp!el>muL1AG_=Wr`-x-p-l!6D2f8)BIy;+n~|H8eDcnEGqitM zsqlN1@9KmMxu;~x#rdzSv~`o{>96Gq6j@r7+DA>LlojJ!88-I$R-1FYFb3wS*YF;^ zV9Am!JaR{{QU-+FgVN7n2m-&rns44|;@Kmzg-XU$u&|d=HQRu`%RN0`<>$xXdX&S^ z{ZeU|*rICe zd9TLWx;t0C30?LnWX{Yil=SnlvFa7Q7@rXaZQ2SyF|J|NZGO!EIE~UfWBT-hM}EKc zU$ObHTh67zf`a20CUJshe@VuIcj~=!pB_DIAVJT4t2vQTQz8vz@;r9>hWI$PSkodU zFD8D@{1$Ii;%UszLNNQni22Vui)%;O9S)QGI`C8D9<^Z5_?{$qaqLD z7F7tXPt#xwDV^OYD+d;u(fCzWw6{7EIY`eV^bjbq|B8xOHilgl@ zMzOm9JvaozmSuAZy6~znl>c0+>Vswvv4P>DkbPz?=l*}L;quto3SBPSbdi3QZ=pltSNKpyee|r zsD7>mTh|){-75Wo{3lGNV_*MYn%{bbKll+lwPr zyHj&bVi*~*j~JBh{-RlYc3o&aM>)>5z5k#Id#MDXH&Ydg()zL)vmmLiJWP>*NeYc_ zj@fNWwXpI=PD)F4^?m&HcX$kULfVu}G2IWnB|CuLKRk7aN=xm^Ms|6)qE}i$C$>!Q zPT$BG`jCs6J+NS)!aI2}>d&bVR|fmkP*Egoh&8XFP&Ch9z3%8yIus8Q@E3%Kg)IAY z{!M6^GfMm{Dwqm`FKg(QISeshFvw65Aamc*pL|-(*y@Hat2f57Rqp0E`_pM1vO8$4 zmE^twIx3pDp=#qz%8lfqZ%G}i=;cf+5&tC*vf66cu(s-TxihAOnxM8-Oh}Bt%o+MH zw>~Nli$hc!Mx$j%@W}M_^*0z4zWe(t<00&iP3o64@Df$_KFYsv6g7=&&NyDq63j-Z zD@J>>=*8)NQQFO$VH@OZDJsG+oo@8=%K|Je!qP-JHJmcOZS(d>L-(0Yer85WQxQyk zPpr6{nmU3xqM?%IqQ@`5m@ny-Bp=Ph@s_GC2Z8WuFXwSpZ@4+v;2ciDNtL-I$MX=M z(&4WK(ZAhc9R1Nr=WX#9QP~Fb@LxT9HqgL*8a&64zdnpUF>P>k>RE)rAuY=Dy{dmt ztITQX4DWz2yDjs`d9HoT(T?n+7|6 zU$=+py2CKG(i+e%7`(CaYjw_J{q zEq{cw-<1ew7kIC(c=1qfYflaK$3(6*Mlk>|1xbHfBpmv`Ijy48s1B zHD#Hvfua`<*Mm&6n8nV9HB*V$%X`z1a^TP*SGp?yoK#X9@J+k>-ao#&lL~eAUw$9R zTJ;>yl2*Hl1{~mH9-aL6SbnKqy_eA#kvM0AFR=V^s_{CyM>-lAZEvgC=e=hD``#Fg z5@Oki@5&AL;$2|xi2%-ZHZ-({V7eRUQIvsWFX=`tw3TmYhJnlq=1JXjGEl$S24~%9 z{?=)HQn>TRf9St@)hc$;EtlR)qU5WproHUBbZDiCZ~g8u88ykM5lsWL?}V+zrW;A8 zi;ek92} zve4_2xr&6wlr=j^^v*^_h9%2VloSSwCLL8)C_>dJ1nFU6PpLh;qK_90q8cJ^P?X|0EC#dM+21)04^kvsZP5l7--=;TOjx_Xuu1S zhhOy&$5x23%rb8RvVC^kik}L2kCE$`t{ zC9^NU#M^0V+;0p$RQo_ggb4Ia&ve6dA-5cfcr*d&)u&G*PoxF|->E+S$VkRFGkws= z#?uX&%gmC$9vnAA{UvH0&HMulJt#G#*HF1Qq^t~pcnv0RKJK&h6wewGiK0m9-{{3#`1+PsSGe|zZNKCUI8s#{GfFn8sUna<{_ zvXeY7ZIk7Fry5(3%#GZ#kH9FTWM&2pY(0j8rqR!CUoV*p= z7G-xN#q*k#%Y%rWL}T?U&a8J!uhnLGTe!(2g`)9s=i~;bLaT`*`haNu_+3ZgL4PJL zrEk{iwXmwfgUx@l3H}?mY|-0in$B=G4lJ|Y7HH%9k)0rAAAe)j@?m%pSNL2mdC$Bv z5G(pT2F@eqN{y@#hpm(>wc5|44{< zWbxt6WDk-YaE71HU^PynQh3k%XN>2kwB4>TCtHE`)-Vv{AeKNxnH54; ziIj@Z&I(QD)cc&V5O&3^GQ!;NDFrFj_-&7m%WW?$_u!q5AV`lPMWxREB&5)U2|K0S z<+a^wc}H2RUd{T=4H9PNW*wclV{586efu2aOtkKg4^9jskY-UBbU8LPEYT^Q%~}UI zMtOh`Dij-jtbiBosn0_ohwLty#;>cVl~jIudZ24pEYcpnrFFSNf@LJv(;W^b3Qm23 zWwSSlQ4X^dUF6AG+g;6Y;2(r*U*4t<#9o(O@1K|a#9Gvc%#O6AOuC0x^;K@J^WCCP zj9nuBe~16BStAE;)Z8DOhO2D{<3f+}jc4)bfy+VTV-7)^QW@XNa`ZY%9e?BD!w?-6 zgBEB}0SMKbdgqup z9cb~;Quj_=TtT|zHJ7985FscUo!u#h`DnOralXSy3Ba&7T;C=dhz{D+2jLxku8QQ>b)gl?VE{0HBGmdaCt@&Bx?CfBs31 zT0}f6oK*|OTs@j|F>B4HHZ%bjTh~H8bA`b;WFw<;LuCG-AGc_0L+tJL zYzPSIHhBVHI)nR1G2C$vvMIfx_qpEm@pRcV*$amcZA8(K$esK44PMr4^YSAghQov* zLD1s9;FP86zIjA~?Vzix-@pH(nOPxZe@^@0GLvK2?`l16>|XY2hduakd7CW$b=0o3 zot?@1xU@SWHGn?up^u+IrZ~!mKG{9=@aQ2P|3+V5djfP5#7tECjhmI>!Z9<9H(W1rK}c84_*P2@O%(uZ???gOm8B-U#4mSCpWXB3B8UN*Gq zSrom^XSE8vl~f=T^Ov_`?qYz-$3_F(3le6Q_&`eS%Inp2b9i_-(F7LwC_cIOM9N1O z7DWpcOeEDTtq=nP2Y@eegLt(nM8Rh?#K}!=khjCFEJi7Wz@4mTT0)?gfDJU=j>OsV zbsL22o;N)@FIZMZt6^x-k|DPH&1;-YeUtwnJ~TyW5J4Vpm&%67Sl$sXTjZw_xtcKi zgtHpdW&F0Z0GDHsogcE zlXqaRakFPjWq6QZy{qC0bY={#^CBnjR#{q`7Z|>L4zXgp{7v>C z3d$$KpMF%2GjNdTTj?LV34M7ih@sH40UQfl^oFZ5ma%Ywn`q5TK6Tys4MEmrNun)% ze0Xn;Ar2HO2RB45-;aI{pLi-nE7a%7lS!fj3ge7E+8A_DE>gsnnhq+Znk%2_xK818h=ag#lvZ~2yoYmqbi#Q(Aa`MD!PJ1IF$`7mft>G0gSS4j1!}Hd znWQvO2_fo&5Ue~wS0<^{=)Z$_uHUYm8R^~T{QOs&?>BFB(3v9=#UjH?ey#@1rE+Z2 zZBp>oq8o&$ChF>2^K(Z6IWVNGi0bhvCq4$j3p?8+M96fGLT}8z;eS9ExRMt->*$y# zCMNd&X4IHI{Y)&CXRxjCDy=!`h9T$0LXJ>l3;d-hX0O=LM1?~fF0(vhn|vP#+h|0Q z=9N91r%j_raRA!-VHWmvvI$$m5XZ8vH5E={X^$&rg70_Y5+ndC9>i+6`9&Y@9@qqn zaTF3<@&UA^X1*Hr!cYd6-^IMhCWdZA=4-n754UXIYzKq|stDj=!rPsN+BIIIwfAQ@ ztRNRehOwk(yPIt!YT9I_ph?Hr^fAcE0gqxna(BG)KO-3lUW(tXM#+;w^$ zjrN?NhMI|CQt*_Gw!1k&P=K0!oclb0rT0MVfea4)0#_mR1jsoKXOo$}pKu>iGf|n1 zT+r2c0e~B`$QV)x9ng^|2@4wC>6IQfxbB%*_l7`c?M8^I{Zi|K`{yv5YppQn#XL_K zWEW*;$x+D!i5VjPMyKVn5qu0)Kx|0eA;idNYWPMJlfJ&0VAsK`EffYlyfS-dOp@_3 zdOmUi6~-#KBo|XCE=Fkx_ilcJg)TabXk|Nbr!YjDrbFry&6^4e~S zXL^u0^c}gwxZ&aJgR%dd|E;0IdILO@9B+QVD@`zu2zPR|;kckKGb8<;(zPtO5d$*c zemfrO66f(D$SPVho}fKz>rQ9{vJB!_b+8H?4Y`5FjuTEtij0&~uV3H3kvM)jIbMpRLt!B; zw55;gD$eflyeG)4or(F`}Pf2%Sh6@?c&8}+1j~l zNrOBtt;d_=m{O}~s*1E3#Fu~yAC_Gfi4Y(8QzE6^7u!);HXPtG`3nFw0Xa~)c{1e& zpMmU|Cq;<@hQza%^81VRC&SHte&hJ|McTRi?wfeP@^6<#jNWhVk#)KE%=Cez*s*vP z2r`S{AVk_{j#4I}q0PJ9$@l1S0cui|-sotm$MF=P7WHI&T zAoOd-?E5(oUMvi;L)C3r&!D5=Wb9GJ(1f$`$~V1uA0+16yob3+%=K!jTV# z3K8-R2f|j=t5C!8M{F6`J+5GtN$E2ciZO>* z(3yo3a?=CUpU5K1^1Qb-kdlvNuUXqqCq#Ow|`_0!@mlaVY>i_oJ zP`En^_V4?r=~f@Zhmp_N+Sq*`f8i}y)T6~N+7z$j^I*0O#gjpP66zD_e7_|Y7!v}-LZMo_k@iFA2 zqU;^9wNfs~o;^B;hiL6`{hNsyk>XKccsN4*-RcUDIdhIdZ*{fA%C|*69^-E0Ly1pj z?%b^x2TV*FuL8MTR$3YfW(Ku{i&#Ntz-z)d5pN9J&mQf1MvexydU>8a-fz{DYViewOMXv}$?8iV z*md4_&DCaz*z@Pj^C$SCK8eMQbr-5&!sK=KFvAa!P*cj=#H%E=tgK!zkSk(Ktdg9-$-y`A7B;3o zJJF#6lau?`f}A`?+4^RW3dz7B+jNs=$0S!>#l8gKQ;UoKXL~R*XZs5!g_G{2Mli|N z+i-NF6da-8u4KpI&K}O|E|tA*v8t>tObeo^d?JP3K_csoDQVM+y~6NiqL%UH=C)1H z1qF{~T>c%D`3yAmO%wOB!V)|CQ5@?bdNb?RaI7#O8rcwoQdz_ zm~e_;mVM5p&(a1byf_@F6c${rZ@rznf|b3PUJsFVgb2;{s`gyvp)A9MKwS1zhxA?op`Q z&iEj*a7Sa~-NN7P>9^n-fdFM(f2dh22CEG zzg0!=2*Q4R`4T`a)rlp#jV+|Z8H6+xk@T?p#wdmO&2EsfRTO$@nVI&U-@e#D;|p1h zc$4uVOQc+CwJ0Rt-ZtHw08o-4p;yBpW?TtsYo;mFy{XA;Sw3VjtQ^g-_c3@bBE?0g zz^9l4Fzrd90G*q@rwlstls5YK*ieazpO7MGHax#bh|ep0rd(Wln6i{mxXmh1za6Xu zSyJjA@P=>C#i%TYdlfVn-C8zYADuW(2Ccyw|LGrZ*t}j%(USP|WBhdn>l=q^?bJ01 zh(F-#&Zx>`~*AE1LS0>lBvr<%lyCk zD8kE%;~IWXgo-ZB-gqk%+g}o|UHav7;%$ua!d($BoOeI+j%2PcIC=6I+I|j%OOE_1AXU?3YOs9&4}K39DYy z$6HI^J}P5Sbr*UmAm&F?_*0|W;V(NiCGYqAxbHs|cDDDhH4nvuNAbqyyq;Xp;8)rr zfN}3;ad}7;?dxldSOa@a`x3@xW6}4iP}WQ{7XcataW;QVm|*mbE-O-rO?QYghatpW zdGhb?gs-Q_zA{8%;7VOImP$%!tUP`nl=2>ghfb>#or)haef;g}h9>DOkCc_~;yzgV z`D!zJoJU6E4BDYK--W0z^n8ME4~ndjL4m6HRZHTlC-YnGF?ZnrQjo)y3p^q*$zfQC zrUjz+uq(ZG5HM={F)PRt3rOJ6t~>rx@;-bru!RHZc<|4c`u$y%US(Hl46TbV%VU4Z z49$PMMfpE<#kJ7ojd%?qFg7VGE1XonTJRrk`9eqWbRfA7@I1bA0`Xq)(pw<_t?BvK z6#F#vxF9{AEY4Q*{^bRr${Ez&d%buWz}@H^;Uq<|DShL6{dISNQS*BEwvG$;L@vho zy3$Q=5VAzr0 z#E~VhkD&8Vo$t87#r{H&F*PA=Kk8lcIz@Q=lL%)s9y8U;HdKxsA(n!$h@jimNy<{8MtglFsK3a2oEozNf- zS0><)vo`ylM&#Wp6k8bp+m%p|%`ZcuB;Krn2K@DB8)L}koLzu}%GO7tgGi-ioA+cY zDZIybwu{^fEI5Q?u_Ined9;|A4@zN558zH_JU%qMNSllpfPS*A!kMO|Y#P$cFA-=1 z-)&D*=4n!n6oDLC!;mroY6w~$zSfjhzF!>xO83Z^oPP?h;HxKCoc?EE@ve!DH-c%y zUow*qeVE?L(BEgoOkcy#NrG#LD1!=VGmP*qvZD8}+Yp)(d$cRQPT z+`2TJy7^5I9ngu|H1O^qy*FXOjYvM6Qbeg9A!TIr!_;6xI&;f>A3f7)*Dg^%`2Llr+AnsCcod~RYnU^U(bDZdVuYbj*^r?_qk+m&>+^crb6q$w^xtfm_F9FaQRoNimiq|( z`$V(`TwsB_(`)Z=ilYp?9QH>XYyi&`R4ep^Z9KBH*JAR$QH!cq=La@8nEB+Mlflcs zF696o#ZqiqRT?hfwJX1G;OmTdFE6oUVkH<&7WQcDp=um+YZpO~d~}GGzdh{W5rGr0 z+$c|&vb5#hfMnw)P1-G|gI=QKIZ59lh~Gyp?B9Qa&X!jf@0TF5K;x#v*1|!X&f>b@ zFBy5sWQzQ=amS7jcA{BLEOZe1HR_K|lTLOUR#hm;R7{8(Diy^3EH3*ZI320{Oqm-` zMm;1Da=?I#179}-Z_v!7-@CUR4u?9mYcr9vO|fcgaYJ^3N8w~2BreXvZ!Cow4U9*j z3g2>+C!CnJmd&%S+lt)SS1W%H3GpVvhj2raVhu*ryY++*=-cxXom($IejJfdR#BO7 z_3GNTZQ)jgku@=yLit8>^h&hv43kNBoF<`7)mH6_K^Yb+7 zZ@li?z#&)Y)f|6o!rgoK;8-ua{VQ-OqQBDZ=*b`z*Dpc~IKFfw!ij+mW5MBr) zi1E_HZA@WTc=Dqi9D=R3cRBpCBrN>;bvrc%@ASd-QY~o>IBa_W7O|sP>qU(wfF2W; zmDGuE(Tw*hJPDo+Y6_%42S^d>v2b-o)m8{42&yQaaul@)Jv4GCe({7XyS|9ZzBk5C z3mp-skLW`lzB}CatU7a&%9Hh~P}-SvIg5ZJM?f6sGszYNu^U;*kPdsmmv%5TJo+1H zX`?6|G?VIF+7U?6#8I*5yfKdG7{Bv9=lR0?R~xzH!uNww-=iJ~eL<6f*lcfNdw}GG zija!;B?Tsz58)rj;7z>ZkpC{4Wp;-g9UCHGbhZ=_%nY~PdP zGoYNMACkprIc4=O9_$T%1`@mt$n<4yZhOW(UFxnpD)Gh*OR^Oo3h zXYwzH@oo#ZIR6r@0t5~@v|liS=o=au=CtWUa24%<_b(J8uUI(iuU{u+E8QClr(#~( zm<`64DCB7BxW%&6gM*L$_WSR69!~XpA&($D#Mncr*0E2Yaobp(G2kf+9JZ}E=W`LK zWy_W=*@z*=SW08{vp&?`He_(SFbg$7FBB5@DVF5z@=9j7xPY>UU%Q=t8e)Ym45J7L4-_xB!7{eF27+twLmM8aMEJGa>J=?twCc(6A^Ws z^Ro+VxxJ*{bKW%rkZpNHFn$M!vz?JNzF+2rZ(qMY$aUb{6nUBaripxXuxaVjCAIkJ z7S5lqGijf9cAi(|r`9c7 zR;>4WgF#)WFQIDqjd_Gsxd!s&<;Tdapj^qXGL_Hqdz29?-J3eXS%{4 zrI3(3m-KF4&m-QRFXjM$gWYcH`t`L^Pfp#wv1wPU+6|@uh<`Z>C_+i^BJ-!3o{m!tOoU&K^HLb3(FI{plx=H%Z z%t*39Yg$t;_`#APx4Wuv4;!|tScya?!#8qTC?#y({ri&JVfO=qf(G0CE4+`*qb&Xg zfkY(>pq%0CJm~t*(9Zu=u!%4oTwKARC$UoVannH(KmT|yFXYyWX{O9%j)lbWo<7f43X1S6fSxL20RVBzu@zW!5i!R`j@1K2~s56T=U(sWvtk({#6I9$b~p? z8Bh)FM+l#&siE**J|w{6pMQJ379WRcs{xyPex)dc&oSpC2UxUEs5Zo+t=&?^TT4X7 z!AFyyxPgEE_ve`bsgs43mh}ZPYK`7WU4*&bz3U7NU zNTpLE@WlDEm;rwMsmj9Hn7X3AfV8&N|M{(4ZR^TW6`s1C$RD_k%Jo7b=$u|+f z3dPG_f*61ONf;xY5wvwq3p~-7fXmx01Y$WAYl+-QA<4Z%FLhdnA${6BR~l+Iaf2=A z{rh~)0BJp9qm|7yMtAJ^_lvIDOL_K|@aPhcPFruInqr_yxL}80e~b^{qlXXU+JV3t z87LB73JlH3$%*R4pp;{%ZC4W&3KtCBk9CCYwciM9_3O`ZZ5F*}A*#;_JQ1*M8)csy zlV5)ds*w^O>fE0PnEU_y=C#xa?`Q()aq|H&eEZR^rg>x=c5DP@9`od6FQqB3Uu{Lg zX2DG_FYx`%|MRV#sfmEZ{x;V}T3Nk#H0Rf^r58bk28)v$7Kul2nzCYRTI`E|-HXs9 zvL}&)>jXX-lAutWvVoD67J;W1VBL=(Zq0?-2}!wr9bZ4(@R&*e{A}+`wkYlAR)L&Z zPO9_|OiO4(KY7^TBkd7uq)AYdOJ(uzOXY4(WV{2q_3il;kzULXJgrmW(-h((bEgO# z9MqKT>`;E}ttXTXUFVCYsM+kF58%`U1ZlYR%S<{O7%Y73*`;$5Tf;t#=tk9BU} ze)Wrgj;rEG60mu*(PFk7{rkHQVrgg6wEOc>hU?VfkbQ6H`}LuXSE3EfWIXQ>t#a}D zz#gh;DV!F|^f!zN(GH?g;*S`3M=3!Wd@!35x9Rz_=#92czkUWU>^V12sq&97qt~#HXnHEUNn)BX26C1ga0{R3Wp=q!octplHsQ(uJsru653p=j|>4Wm8ai`OlC)e#-{P3V3_co3B5B< zDItjwHWR;mlgsY^&zDm)eaFj{)=Qgf4H`Cl^PkIJmp2YZV7#Y|okCIAXaqf`zy3Vy zu^Z71e@3sH58N&mZx@-Jlh*@9m|Yb!zUDFU^&VV+9knRAv#EYj^@G1l9z@kG@_N?r z(1F>{_tiUIC-eNsb#~o0*#x%R@;sy8*$(wL-ksd|+}c|q^(uyXiL;-U;sa z$BU%4&8_=~J7=#RbYrf&^XB1;ip|dl-L)Or`9Rl?C3|0$KX!BMexS1D%(OSf-$tEI zD>F`OvB~Gnm!}UL(#tyve;|MMjlRezkP{TKakucMKMp^AI^)-)FfrdBq&w%q@pcTn zP_3&_vqAp;{%3AW()f;Z564Yj6r`dudi2Nv#Tk*uj%}d=YVdvlqnuk2q%RFrs$<81 z4zv8MCr`Re|M4#sb+GJ47aEn{>>RVu>Bs-(?PoJ{Z{{(!3Ne`@QF)+w;ID-B#di4c z;T45D;%}vVz=x7dY0*l{dlhEW;L^{2UM+d3s|(8rb6R-uHFXCnJ;p4}?|bk-|9`*l zHlnsx$&&@6rnuxR%9=b6e)adC_c$T>0Xl!;M`45h`+M)=QfgCNT@@nAGmp=o|`eC|ZkXx7Tr(l5cEmG)}8W$VG;7)v#7XB+b|{xv5uWo@4u6sSQfs zRM1Y-ih{A3_P~{{OkkSSZCv{ARI+2zfBtyQz(b)+JGoWIdoc;a#?J2LuYBAl9AXe{ zW9-~zLPNzWm^kpm!ok^^Nk}TH$eXKLJqU?@0L3`IQLesw3b`bcBH!G z?qA<}BV#$IDn$%resN+xQ<1*C{q(7+(a+yrV?VPr#J#%X;oh=af4n%E(rv-d_wGHV zFCziXKAnMO{?C_QzM0DJ8o{`zXZe=jKW+HaDDS!=;wH>@OKhNRwwhYMXV=sM1}N20 zYulkt@#;R+hZ*uKYUR|Q;d!+Bl;xkVGDUvk6qoLJsj)tO`t`jp&Xv^`Poghfn!$|zyNYQvSIKsJ&y~ro zZ2$AKez9eu3Xg%No&P?DHFSLR`SI+|JzYF8aK{c_LX8-ijpOQST97|^{*4YNpgtm_ zqklh~)|V+%>{0*B5Xd_9n$OK%QU1{#n!lA!Lp13!yANgP!Lrw|1~z#4ld$hA7yL@D zpDFJKUVRIvw*SxDF|a!);&|JVZz+yd8Wbo9Av*0;2Aa;;{o~&%o?RaB`ATDtT?0^M zYSnC8%=2xXQ}McwGb_V>`817SRlOgPiYt?1&p zpMd;X>4%*7__ifCU(Gk!TVJ8}d5qY1puh zbybNh2L0{J8RkiAVX*RJSdN(@VzjGk^Z=zFKf_5qlDM@j|6-r+r=p{xUeH^-@YfN{ z9Syuc{`Z;Fr%6E?`+gN4&juM2j_#zVZ{6__wQhlCvt~T{iuLR$h<$f(*NaE4F|wF4 zE`AD=Zg@4Y}#*n%g^7znH>AAx$;!e#H0Y!4PWKDq94szLp& z(-?5-Q~9uG`v1PYlfg3P`4t(-^(~n4^H6!`2By3UqcPK|#FfV2o(kQ8zv_+zuYMRK zj7y)b_SofBoH>q%{l!)u?en^h8#*70)xnFWMU%vkjLgizfBf-gUK;S&?$%Dj&QM&M zjMSy#hSC;+wXB?hefo?DV==4X^*l-kCq- zn1=2DNzo!(i$at{WGQ=DS_s(_5h4|1U&>CK7Rr(=*#;$B_F_yLOC)7qW^CC?p@vYF zN=4t#*~Az#@B94+-hP-G_1w?>T=#XI%W)p(acYe}HnLCU-Q@;xsS|WKzwhLaMs|25 ze0Nh7k;ba0Gsv|`+|?bw1{nEEpTBiyhOx^dO2+-CCl_{tE%D2?4FI!6dlld{aoF7Q9@Hew)2YbWrZPD$agJ}!u zLnpji14T*WY?aE_mfz4G+-*%ldcq^`{+bQDR76D8L|Sq0lnmBSJ8+^U;I;umd;*}uby?x&O-rk{IdQf!Gc{5oMyZi1`4(>eywmV=bL)@$24k5N zXBEHSzP*0GFSVtRN@r4eJzFyb_ftu#42)}W$_$uNR5dBb@Js$OqPW#(-y2Pj`e=By z(ZXkI&}t6tc~;SW({i84uc72V@U-~i$8Y^MCVaYc=Z&AIDR5S#~i9F3=v28xRz%u6HkTU9xTpV{O9~ANr?OI-P)D7SpaR#pQ*YyW`|# zYJzMtO$+hy`wOQ}o?NHPwap5Z$;Gegw_`G(*yR*Nnx2s}n{moQ5y*`xiWI6qvUCoaj zOP_{7(jOArSKEYBxb55gIf;AV56`TmY3rt8lhd^o@AlpQ`UyLrd$)phcrjrfa<81R z-~YbK>r-^3p8KzRVOvpjtT2>#eyySSusmwc-{6Doe$57*TFvcx16R;)^UC~6mst2P=!e^B z>tRpTu!*?F+j&Tk&iP>hEONKadobDSX^38zl)kgz+!Zr=Yx5T08`h=MlI}Hk*t^wx zdB(%D>TA6IzGKDjyC^*$M%;1O>u$){{cuTzFXF-*_WS<4WlL~P#piDkiLB4F-)@>? z(*m-0{Nrgtj_h5QH_@hLZbL(#{BzCC&Go)!1>asqg#xz#Utnz3vSoFTC@N@e z8!gCOVu`^H{toz*%a|B%ys~>nPGRA^f8EPRT0!+!+k9C7|8%0`TZWe=pD6!Q>WMWM zU83*5v}Ti^+&%Qnv=1;&XtIQTc(=CAw=cxs$3wO#E=)*j2xVCJJ3Lsf7;yr{ z{QtV8GNuw}(-pTGwCDn;LCmRBKDO35%oNGmJaKy%Je5?&DOCNzfIYhg_T-dsjOU%( z_4|#QuOtT++fNVX%oNZynFXetl}L|{9vn_r<{EHtFPMw%jjQ~qmEQtRb$NC0m{Y>+ z$&>fVGhDvyyWd+sGPN!UJMreXS6MCHKJ|kN*>wBD7n`9j^?K%=s9mTD!zs+LEK^l7 z+)WnlJz#yQ|FG38-5CReA*AwBwU)xS1A|5OrhNy-MHe9@e(KYUp9=_?Q>kIckN_0i zD}GXRMvpx#z)NqQ-&XI-EoxTM2*2LpRvrJ6KcZrQB0821CvYc>-??j-eX-)E>i17B zI%?FYQ88!ESVLN9Kv{V?dZOfhE2yXSnLhpXGQY>Y^v3AOM;XlZa07KD4I`~fg{})} z<+l4j&0~O)J3xfjxvF}*e##*Dy#U!zg^#xDyJM_)7Yp{m8^s(r@XFs`rg*^i;_gzG ze7*a>z6U5BIdGt*JSg&uefNt%7Q_3uN&MIL`{%0^0joa%p`>-Z=Q=ch@%7E^O{Ywm zBBLjJs0LVHw*2S)e*e<^Aq48W8O5ocF-oGG<)9Ma=kzr;{r)d2fA-l$|I7){v+9e| z+{&`_Wa5xJDedZoJaa(A#O5exzKfFl(_cSwfJpgG6&MMc^NwEeaa4rKvTZt@%x}IWf z$=j;mIoKaxzvuvN{FQUq5P?=CSu40?~y-ob#r3&A9DBUbTx;LA)mvGlMoui{ZcFp)Sh(TJG z%%e58R9w{l-xe&1T+E#Mjy82qgIPC*J3H%w#7#xiuQi)3q^e@!Kepr#Z|`uGKHaSJ ziBYR4Udn93K`)QBqTas$z=7;zQJ)Wb+bb;X{usmYJr^KiR&@_-R<_hdr=Yh z1k{5vZw&%h7bsa*MX`0<_s8*v+Xy%{h0AbKP{p2l>fPm*BJr8hPg@`dqQjNcQ7&K$ z1v{HFOJN!Izul4PFsK|II(16LPI}P3aqIqleX_84)E*}xRvQ`$^C}Iq4-(vg@=aaL zWEf?D6nIK8RK{Tvco2jkH|LaeT>imDM z-ux?!{jZnGKWKd)GyMB69D@Jv&HueB{LB9>`+qKrZ0G;maR0qD|98~>dtDU&XJr0| zh57#)868oz{Oil&KFDYRUu|DN+ga4j zdwFl(gp-1%prq3fDb*RLfFlLW^3Swc{Bqyk)P*_Q^dujntGngEVC&+$31fPyQ-9{gr1`l4{MCW4Bkh4WYzFqpSABqhN-*o$J ze@RJ${6T8L3%B~Xd~a_0{Ri1Wmp&3Y4PgUGl|}jJ3)v2|HLlbi zWNG&UjwM%#Hqie&@;8_xNDs|(ZK0$3oxQgm7=l>=mRzaBcQDM?cR#ax{NIc-)&5}T zWay^bnMV}V&Zw=i=x>i*@HC_t9m5V^q35r!<@=eYJ-=3o6#+pKs|oiyjkvK9y$q_S zH_$2G2@Z6}AF@fg#F>!4WWy^ir~4;`J0XAiKAkEX4-u7A)ONJ@K+c%Y?1Zk86%9$4 zF=o}yQ41)LdqQ6cbeKgTxEdUMNbQJ#u~5B0a)XR2uJ_%4GTpvgo?)?&iQ|P{=2oPX5vR1kRN@d`xgHDRdI$+_TT<>*e)7YPLQ5u z@7ue5|FF3e8Q@C(sV%6T4AE-x&8Ahpq~=kk@!f|_XB4e*fbJR@#aml<7l(@nTU9B; z5o0&)!7nZNza~(Knd4`Sq!`pdJt)=7NgsOC3RH8-DkM)t`YvdDACQa1$ET`VReZVHAq}C&;Ryzqy|Rv0)9{XPa>_GP(!Y<~Zaz*+h61I(t;u3GSD z_6-@%l=Sge)Z+dq200Lb$SW(%G*#VnYi&TNL+|P}>$>vmq6NJB6(b_TVh3(pkKq`{ zGhzPPzjtG73|#OmbN^5M6Ez*mMnKPQOPN65%6t6P?cKZ5gM464_;wQh7H^ za3g7MUF0_E-&QFzQ-scBBY!30uMKA4^)z=zgGO`Y)FMQ3p5 zo-kRR7a1a$c0s?>G#6ew;QNX8?st*K}aQnwjMs}1r zHO?*6(Yj3Qd=?tv14BpA`ttZbjlr&bY>ejPe!h)5{*7ui*OS5mff)VE=xxqNJ=XrhN2_#HKWKznl4S(3TGSoW{S>{N{&zroyq6db0D$&s1>3 zQKag`Ry6pRl~pJrQ)mR=NyU;;)Z1nq$F!IoPJFTDK20&ZuicCIX5TVL(}OfJ@5$+t zqu!LlWvMU3Dr~~NC0{?@-`^uL<*;^svcOXS6Zff5Ha%PyK97tCa_YA^{%PgD~zMhdCjhogY&);w8TZ+qB8fb#Gvv{Z=?ZNR~c1-P-e^%MANM8|M1$JT@^^1`^rfTY27!9&|uH&w( zdJWI*2_~ldTk62hk9DZv?}}r~$7&%mQ3s20=$DWFehs537O&a5b!+~|`V~G+SA?Mo zpr@=aotL(%fgUXsTCQLzF6{Y*Gm%?508Z2WK#Xt|PdHE51}MQfq%v4UYKr+VDOAZ&_OQkdD|z(Q;` zy!vDB>gXjw`=g_ylW)w$9oOl=3!IYyb+aehl#b-1j(GW5tm224cUoQtZx23lr{cNB z+S0ib4ohrf>Ae1wS4Lj~KzF_vKZ~+!bzv$81eu|&Kkx06)}iM0tbG>X4bnh}+f;B2 z0{ZPD;ye7?%l#VZ^N7T2`~xuxP;SL1!37;=c;}$V`%w^Fy4JKQ%g=>p*W=#iyqn)_ z@0oq{7B?=_8{z8J>niHh|FIWT?GT=qU5pDIGof%F02-nx2HgIj+%Dg}@tJr+3$>>$ z%RcP2Sw*{>+P^nwreg7*yEkT`)pf+X^f-QI6TZ90y-iv9%8G!L;Vv%v?6G(hRq?>f zL(4ly`a(}<P8S;Y zoB#Wh1SpR~wYXqiRN1b}Z!%~f61_w5*1-pFr0B3a9LT*qX`_$FE8eH6oQ^4b^lYT) zV+}V=h6Q`j3XX04wCY|i9p56gf?v}{?jsa%y%)uUo!Eg133$~5e-%2;b{gMj5PWo8#0c<;lu-qj*xM7- zJz9{x>iL<+u(cjlTHAmrv7D?o$h>W`_FZ&5J%`UA1IV>z>;TQ%qE0cJT+ zENz-AYc=_j@o|%|a#6VJP$eMvE|EAsI-G;&o);vgNk$3j6Fzm+{H}~wd4thgD@u6~ zmQLYDX)(~oWco01sOf-#5(%F5;}NaG;aJN32|Wj8ona4KNmYt=0<)9Mn7DZ&Gq8FV zk--XBYL*hz2ADmCFVrpe)!=Pi#ByqlrQG$)jK^JI=9#bbCHvTwOMLri-oxoQhF1S^ z>SsQsJwA+>dicZ7H!fWYlWhodOlwcGs5lN`={O0K?~a~%!Fj|OUG_v-fE~E2z6vmu(dL3$8>;2CB!G!es1&SlmyFh|3%4O>X#ZQ-I zZ=HQT?3Z7DVa}$#L>&|}Tgu6ccG_&?@7LqDL_?FSDb$Z(&xW9#WS6)1N}H3dh>zR+ z3(ipl%oIsKDS=RgK2kW6U_j=mYwg*tv&@u{I498us4(!VIr-j0|FMA63gxc%;4(wC0V!cH;k z`gh2p!)bcXhMdMGTF_I!v@x0AVJK`8i%ZW8LlA!ax?f8U7VBHx*jP%qG){&yojo|# zbYy?(^5qYqVQCs#cf+~OQr5bX&bXNv^dgvJd*#IY>s_;>%F8D97#N|d2vB}a(z}{n zwZ1fp!mYYqw1Myf*x(y#A0M#QvaMA8S}06T4IO0Fl#Zyzp`SlUMV+WPXRh{-_oWE< zoR1eviySewMW}xP%yp9it!0zj+T%1*fEViYX*hS-BcJ!zs_YvFA~*8`WA59W{_wka zJYtrfa`0?>Z0^2zUXMHPxztn*g}OOsrOnB_qm1F)Zc+FLIxzZte7)}is>=H~Ywdz! zgruT@#cN5zTe(kZOROF+dt>i4V1`wi?VI9bY8p8OP??V3+kS?SJx`R3E}#1 zkj77#pq|}dNVj|j^&u-{OAi|j^J_W1SZRVmk{{??v|G3KlQxc@Ml8eqa@scU$@h!auvS&_9Y0u3KSDn z4`d*ci_Je_V4K=kJQ2s&v`?FE_bd5RY#!cT!{i*GUPH2%pIq=vidi>>vDEn0A|kJj zssa6WvK7S!V5d)86rQKTpC!!GvN9j{@7}TUN1scQr-2~_7Hd=(IlFu|Lkbi$&$9ta z$quukYV{YhDE?xIlasM5dvr2tSgE0PnrI#URT4t?v6HF=_zuBI=e%UFyDq9NmI70UsmK)@+T~C0v&WE~vtj zn>}LgV^my_8tu>7qB3NnFpGpGnu{fP+^3rs;DFLi}e}1U{q)a zp4sE}niyRk@?84A3vTwTb>WEr=cyGdlpiUb1i^>j3ZVf}s>wQv$bP6UZRHuXo%!UH zyq)Zq35KrVZ*8$2lMo}Y`1_M)8ck;JYS!-%+k+ACJC{}j+;ETxxDPNhfq>hrW5?qJ z+~|M^y(u$Va05AP@rV-Psn)Mg(H}fX;tuCgj23sD|9EZ%c|qPHKeEzrtB=XDH_kFG94wAVLy#lcQ15-64uA)@7tVJdn?Z0 z>&51Y8dXRV&{3Yyt?<5Qpkn_op5a!hi4Z*l%#MJ02}1_~4V_eL#AwH|a0g}KoGIO} z1m0@Kh?Jl^Vd9t($7SfOqj=p9cQ@_FsS6UqO-f zgHm7J!%${7$#AjS$9GKv4{#ocLzBw77db zd@%4?>(VD+eG-53jWOFR2(J}@8rWF~?*SoLy9{pfJ`RAgY#3q6FYe6dr zXUfRe<4bSt`V^E<8cK??JGN+XM(J2rT!t4wOCJnO%;Z1zNRzb;u1Gv25R=?BvNtLC zINADWmeoO2$A>LvHA&vi*aM}^wlwPX8}dr%1$fU@to9TGMdWb)J{vd>ip_`Or?vc> zM;!423QNO;>_iu&;uQVk&K3Jl5tOAO<)jiHC0$S!#|1gIjRdzbz%~xdHEn^`*9x9w z*KkH+f=Kri^)Ob-Y|b@SqZeK^hJ3>-FFtL9@8|hRrDG)Dz?n*DxS9`l0r|Fu4!o=F z@ehP4&ifZ(#;3BmBSq~Iv7@dS=h~7s9Avm$^YUKo_(#A)1@}Ey%Q$2ed0PMc8a9$c z7be|ab=$Z$U-_9?Pzk~NnvU#>N7oQLLR_P6WmG^0xdQTCDl1t*P9x+`2{YZUPrehI zuN3CCo7hc))zlVyw`!qkvXbtL7N~@HLXKZMZo15u>8H~+c33nZBv?l!vk-@TCC;- z9=e@)7uh2kl$$`4mY6|`QGym1WCqphjVz6Ft^1~ON=QvcF)#)&@HJ_!0^LTeFj;H z{BaYd*me|Abd%sPX2tFdUq1V~RUe~rJ2rXQjq<$`-yb3<{bt6q=`Jy5~cby|$-2HaiMEo!!rbErC{ zu?{b%vkE@oDUHou*g7bYpDY$P@w~Zu$ zI4x|+Y?6XHsT)DlI=@LO|AH(12k!9K{W|WWML(6c70O?O^-E)MTV6n8Ky2N$&J3>z z=!OZ2egW3CX7O*5dP%D=lh^74t=$eivS$1CMu_XJq-8Lb3Ud?L|3**0y5#W>*%nzz zzOQM!JP_ww&&JOOH#*6Tc<8IoFdu6GZ9wN@(gHTCCn(l`J@ZwJ4wS4D%%(qgiG!}N zL4>Miy-ru`%E~ zM+A-zC_T@N=vgxiJ%bbpJjm)RQsbIigy+#@bvLKR53Nx;E`OEe`*v{#&$oW}MRTZE zjF_zy+exOO3-wl_zT{#zvDDIxJ7m--vwsI(MvWyEL7YYNC-`^P*MF;AY3PmGvsFizHuv<;(BY9lX5-u`7_`CjoLB*?^hRE=*+@KYl!Lymb6d zx_dC)|8o(+lmwn3c`_TL<@hLt(Hcf$_7U4kT6BwU^(FQdjSK&S8x59j3I=3`R^SG1 z6=P~J$o5>INsAWyoZj?Y-U+#ci-6?~=abthjd@R$5-L5IYu8=VIno>&#_EhOg$Z6l zh#^)pZrQTGxV*DU?Wu-pq)W-9G_rgHM6eR`IzDTZ(*G`o- zNQ}J{yXC$pFWa;GcS>c0_oqR66<6(r$-ew$G_yDxk7x`BUM_G{M6iYclGmvtKTwP= zEP`r%w8)u~a7z-b%U{N_qmSAkd3g92!=q;t5_pwxYo*jKxp{hex>TI+lP!@TU!dge z0aAU76>;d>w?)^ST~O#TbUP)a2ELN5Ds%3EL+Wr}bKfXsHh66M`IV>4$~c;4!=+j; z&T}0$Oov1am>z4(TO)7Ibz-TC>NU!~jjCmwZHP^uhkxPeDZHmKdiGc$V^h<;dGT}Y zB~Fjw@>1xRApZ7&5$2>Y`r_oECimtJcQUKu7MV@d>LFG09woL%QBn)}t%~I&QoeGy zfXbv~DbI3p`fwz%t-V>m7`vcqQp%hJp0R7sp5C&|0?*$_q_(Ifiw{ppL%%S);5j%D z_hZU*C<9p}fCnkU5&JbZHf9=69jQCWm9Ue1r1nVB8p!?e=6$GX#^4Pm*sMN0U*UX? z$Loppks5*sQSvT*xUqDr_QoD8Es)CRSk(ON?OESu5`h$!43#W~=t^|VkLEnTGlc9K z6ls0;7o}yN&hi$Xf~Rv!p`k~o=Ar$uoJH&n1iusI07-q0mi{c138_ASU5q`p!zs#5 zs+7|3fZLN1*?bdK2%LaD)g1W$wMQCiR^A#Quc9xmK%Q|t4x3s6BieE zFoU9uq%(Y*aL}@btudLccKG}Fg3Hdf@!?7_FF+QG zY$5;sIqOelB_Fu)GqZe4-e)MKgH`j;Qdq7l;Yo4`NJnLM1L9>rADwE5esY+AbK6cT zDaQFrh*DwVeO3YFrV28~n;g1b`c$J4W@3$waa=+JZc=yzz|m&=^@N=r2~kUASdd zC`ic|(294}mGgZRZg(8vATZ1+RC=1&LgL8Uf{1&&?Il~4oIee3*C5Sc_5i5_F{1m` zBW1^dg8BH`=5x3_2&+H?8?xsotX{!5>#>ceN|TPfio|~)uT$&>0VtkGUpxp*CUfDFdHYXWh@hu{STQ8iLX5#eoXK1g zJX}6_=NT)nUcCw;(HCFSwXI)SvG+qi0#8Y%ypq*T?NNBywR^YopJ~kisiaR`9-OpT zu(Rp{zqGn1fP^kB=I%(U?#?S1O99#7s>w=F{xoXY*`)0&1P>SE3B4r@d5Hwqv1#2P zP-boN3=addx2Is3E$&(lC0k`DncmXjhU21YWiZCtcVrSztbqWb5HS5;y`GOWxxh@8 zW%=hTUUt4_*MoWWXJ8vn_a4~e)4Pt*64J3=EvKE9v_w>Ba*5K@Jp(b6Q|vn?V0J4? zk=$|oxO0KtN!E`A?}n9;6|6ldltO`JUflT@Y$k26nJpRNkW&81ro2yWw`}l(RS##J zJk+mS!uyArwBH@X@Lu2{)VjY2kZgqP`=TC&O7kSC2RzRaAUS*gfx}vAY;ngOAPrQ< zPb26M17ze@>I=TGoZQ_=^o`$il*&HaFzc5;2K_Nfv41zmI)#g8qoWZH+0+t?vVVGA z_S7uF0xP;eE#&MdqSs62PnOZ=$h6PZGy z#IUU<(jzGxo)ACB?3_wm(5-kB`$oWZiC$$7QEnPFTd{u=81q-Jz$hVoEa55u>jez& ztC{PW=^wdfO+jSE@sZRhymP|k@*EJR7av{TG_9tC0x*n)E5H|+hE)L7aLv?&fUaqbBCvA+822a+Z&Z8B{||` zVJsD3y_}1!WI}wKlaF!f6|x(0^|wAbT@Obpsei=Nt+ol5+?679qQ=5nc{x0hFR$mu zx_y2*;uaNsOTx;QpA+M$@e2Y~adJ1c&CV!1ZY(F5w|O5jRZl=-;WLDHc^Ml!^4Ojr z<_lVn#Oz5fAbeT|EdCUdbp3<#k&*M*@3fddl3Fllu@|Rh%$1fC#e2gGydXC(rBtDC zyvjdhjyjo~>GG1dHpS;^#Q>MO=^MQA>a6w+{Nre_af|@0Fvboy^5qQAB~L@lNjH+2ROK@ zKoDra+4-y@Ht2w=y8`!`qCcd>c)GsN& zTE@#g(39dBV1sRMV(?cv-LdbqC#S4LS0q(}R{sV%^WvBTY$S(oUg?OW66XqLGUHhK zfLC-lVt_ptp29)5Bx`wUEr%ZrB_MImRn;1?9x?nBL?zDTA14^PAc`B1W5daD?K!#~ zx-SVoEssISx3%CskMKK@yJRhZDK2wj`s@DYr#rMk!N>AxuyzbJc7lanUh97*1C`== z;>-k0DrwzGvu%$#g-vX1%16)KE3q2OW%lz>kPuXUV1zxyp%D3&u0i8FTO4oj{C6C`J~gU*CA45`kyZ9Rxvb$lm*f72ne=tH7$cq~^;D71>K?l~ z;Y_Rc?T_Up-pCPiEJat68_SomJF_@wMO&WxNew}49!ESI=*-gS{J0`XcFSK|zc#h` z)K`ipx43&5a~yfbZ*-k|st6VDWSiCea-?{(3zWR%vC5)ag9?b5v9Wk+N{73r+PZmy zfXB1Xc-ECcukP?77T*Khxxx~Ok(jjF7N!)Prg5~^{7qFOvA;EuZ%ZdoUYYY%W36=)Y|p> zuiyR4{oZZorQd7StA|d#OAgKV^j$k%You|H-pya{Xf&i->)#7YJ^lW;qTgP7`{*0* z7AB4TqPA`EjUt*M7QUH%{Znd+k%dKV{8;4X_kQz_jbc^;Ehw5~mXB;-H-RK-+`hdw zNdg>@RK%>q(W5i!(q+_)S9wpL4zXU=r&q74K(-;+3z^J%`0ajU4DE?UAI-2YDp1h& zS>KzlJ(J6crcLhSlL_M*1h4Zz)J=gNwaM7Sw0k$Q_6a5TxHRDr1wM;~@5YBs8->Vs z5VZEo(+xREk25-DZwN*b(&2gg4mLK1aytVD_VV%R#x`wpZ|XAU375B9${f37QlXpV zB#Hg=8tCY#kY(CkzI*$23MWS@+18t^?HC)>p+g6i7K?F1n{533cHiw|qGdO9=+La^ zOk6W0Z(HZG5R|JIs-&oK7|-pi6kzj03nfJ@>es<21i)8P_x1H{Hb%?wr=OYu3k0)s zE?-RzXB?ecty-#z0h=t809}|WphOmXv6^ohr?hIKInGL*@NnH`OJ=;Bg>{$pJT)z? zf46#fAvM(2)eRwlPDbc_|Jz$s;sQ}&TJ#~e@%y4lsEsfvqTjVoTao&?ysoFua`Nma z%O*JX>!-ntv8H1Wqu!7CHI09V#i`Sv!3t`PBT?BbqH|luMS)*P&}z%e-^*=4t?E8z*fh_X-UQI|ply6l~GHl{r!B z+5~47OoPP>N4*c?0--whv4wHu-PN)T`By{44W&f+?C=cN2|euja@u?F;DCs0FUv1& z@4onin$vO&q5)fFU7~s|`8{7IpYeCi91;o6ies;>VC#l(QH|j@$VTGg!?;aq-+Hs= z%n>D3-A>h>JbB`MI?;fSQ2Di{Rmp@L(agQh*v6MZ?#uk;KzU+I~ucPlu+XC z=2I*@jJ#HnMjT-1I@&y)aj4CHRM*Oo3`DyF75f z0Ak+X9;LQsjEl$5jS<99*}Z$Wch^E~kDAK}mTn(h!vJNNtAS?RqH&Y1ZeGxh^9|+HnY`X#e$?h=Ivk zl^I?g3MFK$&WBr^Ixpn*83V6N-YCqB=N?J696gHAnJ_N;005F6*Mjz=%xAfH?V59L zCx5&dV+v}ZkTnVY29knyr3;J3F!wd?`0)_V6Ud$gMH+w)bLdlRAHHqj;LS9ecm^1E zfGGwsXyay{{ris}FQBh+3rKIlqDB24FIe$qjGv!1gA7+d|B%ew>1U&YF-$>*x~vbF z9*Wlb^%{(RFm|$CZD_M}_wJ_31+`kWYL)rwRm+X)T1w6On#gZkFo!9KMRdL#kD~jI zLx-+K_pgVz0|d0%X{!wSQe+|8Rpa2C#bn;E_t(oS{=3WG-MukW6fB6tHS4ugVJeCD(9WnH>g(&Dy?RxX zG~oG9!{73^XNe@EckixBYrp&<4LhmiK>AzD86gmaIHzpGdhLJ8gn*sbv&yCG*HhU< zx0qWxAj-O-@fwF0uRu@Qq5gJ0HMJT7*U3OG_iui<#raTa*_A=mGqUHse=#sDQddVO z7}{py%Axp-6p#3nD(6HLS%O3 z!njrHMkD@AY9$zVT%lblRg01S&Xq*j>JPv)|qHw ziPsFHXY&GhlG$??pqHTd__jf9b^{nH5e#%&^g)e-3T;k3sE55t6Qar>22CMnr2{k@ zx_WgVOewR$)^#9<`#91|U^)(9;{Rhn}cL8k(ymRNqjavNSv^mDcG38{N z^`RI95=ES6hGt{WxLHqoobvm;W!sX!Pf)A|!CDqs6^)6S(6dxp4OUK7Sk`P6Y;mcjB#30#cHl018l<6rjM5 z$DJP3UwmEs0`i7d>-TIJ?|LnFXiY$D`FOuqh-osgrfzZC9@j zAn&Y>*dlDpJ+3Bs+)zytuC9-mS65?zeN_LtS;ogcIt(7Xj4dD;cGIxM z7=8)J@asY*MRGUpYSF5I00pDbYH)fk0A4JNzp1LLtNWCKmMpnujT%oGt39i2?G+bb z=Qk8P3&IP2(C`FWy^*HVbuUwQf0~sQNXmTf!GlqalQL@7teHw}RE&lMji9fPJL=9< z(-2i?nkTyws>Z=|PCR@7h?dSp4aOp1MB)o}JB%IMh#E}vkt3_|EKS0~kCRwITFuSOGzii(Pbz9IH~1#wZfu!0MgC-MGrC8Dm^74`c8gFM zvZLd%X3J83Kg_Lbj4lcctU@g%i!_c8MgfcBN+Xcn#&uT|lzg0e%D1#GsO|Hl_iyYU zlc?D`J0CC0A$HtBSVWtW1i)&n5*3|VOm6GR?zshFmy(hidMkMg>_0@|I}6G_&6Xq7 zv7;_^1$~^0D}WIHrRlmDMZaJww54slVLM_ufTwkk3KpdbMOJW1^!K#H!@R zJ!Z^kgXg@``AF&e?lS&dauk&O8^R}(QVuJ1{+*W_D2={ri_ee1G^S#UPc8bWL1Suf zo&ztY2Uv85siW{7(LPEs#PyhFWn#bEpw4_uM`!2OhB=*quX%^kU5o3kIW>vu3^V7X zN}rgq@`W!kc7A^TSuF0S>kMT0`W34ITzBry9bIxT=h#Jy7cM+^nwc5E{%K+FOZZm1Zry6=rK3o!$p_C<`4C0N zRTyA+6UMJ*O^E8YP^)^~RuLx?yHaCQ)6`r)X)~i(Ejo6LJn146j3T_PMhxrpd2_2F zJcLG#8tGg6RCHAjoeQLB`T?cThrvbY_JWbO@7lkrbT)3-W$Wfkg+z>+@5mB3Jh8mEI#>cWP-4?c^uXZ_j#|}cLt?T+Z zw;|``Xr@9fe`ee^t5k&X_U=_Bt6ABw>xQXzVYxfyh(IL7Z*DlSDDk)7jN7%-lH7u$ z)$;i!HuWqouXCgg&yF5}NKqZrP935tWBC>oI3{9Hm}I3+o}ii9HEIPcq*@9Kl3BtS zTK$ZqbNaNpPHj=3XU8BwD{tOiN&0Yw6{qxXB4#0o`1K4QQu128@|MHIKPHk-NB{syY`wt(s zul@E_#?HXNYFaw)Lk$vFmq|)fJIwyMx+9fn%;(2j zeN#<#gKkP#(4k{TIQjh{PiRmLlo`{-ySuHaLN?d4ckgPrQm>rr4U0=lvym{@YBAbG z>nUXe|I$(-C*k}d_a1~~X8jJ6&Mq|yfk*9RJ%~XtDVS86!UBO+sqiYJHSFv?mV9O@ z+!$=xy}mxHvMG;5Z{2bCws zN;LIr1u1varcJgZMkM&SW)QVlm-pQP7f!*W>kS|x;b+MHQbH{EapM%q%F6Rp#FYQ_ z71Phj)paGV(#6H4`G?+tn5;l%EV+&uBNVS0+j|>MXItif{1}2Rz@k&9pZY{=yt;E>qtSE1EJ`KHtm4K43)JI(ZH#~BsFK@7{A|f_>(_0w ziGo1aJ9|+*Q>`!ywuc1Uf_3ckb7NYz>@Dbef$NjlXatkEiO7b#t3dJ!CVQ*yRLBr2 zaQj_x@xEonZ3!c)ju(97Wr5aomW#;Rb`-i z^J$vgXr21?HTZvqju$j^)Fl)?4;ML;iCRT-#~h!c3oNyoQuhvEoM}Gth^^kyNkZ|k zmS)Q0tJkhEBEI?j+5O6ERK~Tp!RnR*iDjn6$Hko`0oI?jf0#!+34c9;oIWJc%sg8` zp4+%hn?WzHcQiJx!m0KzU5u_M2CXhzKV-v(x-8@}nx~I>g*|hn)XD~+WTg&{GyG*d zM*T}WuPK-dvBeeT9U=YAag<@1pE9%HYTnlP_7ldhqUs7$e_V zTBm>}2y8;cI&knMHE6K>*(jj+aULFl3}Y6<&d81-n|?DVK?d@-!-uB}ZuRv`Wy9vp zGXKDxg0N<_FAg!|CrwfVSdiV@y^3##H4ZA;+S;HE?i3BgNe8_%q$M++d=(}7mFJB1 zN3%0DhR^fxh;f}hY3T6bO9A?@N(_5)ZQ55S%s%ky;CdY<)ixhx70(gjMVjI-VPH(T zyM9&c9m1q@NoOzcz)`~Q(AA+Txu^Q@Ut^w3g^!a7;$k)XMe|{&ao4xNQL)G8&z=p? zyVq54MFRs(HnQE_fQvDbo01pYPn>Of@37-;!i+%D?;a*`zV}E$8f%5cO}J9GSwE8) zf9ZM>*n{en#*hr~p)8UHi}vk%!eTKQJ_8U2cN#|$nsn&&`ib~`I$6&W(N1W0t3lsX zP8Kx%IiybQ^mW&7+N6fRAdt%>k7##1ZMRFSRFq!gA>?&>)+e8?Fyaw;MuPr8&Kc+U z%wh867TAb!kih5LmM730qN1wm$@<^Cb7z9BnH?k40FL4q6B4q&h}NKp4I7rSQ(`EI z<*5nU=S{C%Z8EU6m%C{?Pv(irIIm>ioHUcL3NWx^@K&Gl!?2yDyr5~Rj%26~0~5tD zUFn>Vw4lJ4A)FJ=%wXK2|Ia7dLHzfj%jo9ayS<4G$N%(H*V!7l4*Toy3ma%^7A4N{ zv#K(Vm6l!yhET#NR(6^_mH;=6(@2G-v&2y{lCFp8^b^$mAJO& z>->sHHUqc*L6YzFG`Z^L<(?3MWH%+Ao~Wm*+qhGwx?o(h5WWIr92RwC-MXQlvNlq# zbcbSNVPRp5g|~7v-aNN3Z=~I2+-}~)&)nxnYNQ@8H(Vm#God5n#7) zqugLJ_fjIkJOnP+FUQCYvEt$#8#ZhRTDE>w@fv8!UPW)llu_4lO1#(sfD%mXxy{d; zHjM* zOWa=Ey=9p9`ZY!58kRtboODe!-wyORGD%ULK3(CG*NgC{FS;rNDtf)lX{FV-0SaK6 zjk@Vmdeky6r82+qfYl#^w|A@4O8pi45l?^O-ErfiZJIXi72!>yyYSZKI`J+mDUM{Z z%y9_`8^d;GVfo1G&*HbMw#c}4tp({TJMLCXGGpkoMqt2c6Nb7a-{7kRlWTFooSiwQKJS@g-?LeYRH8r(>RjX=% zgY`%qNBlt5zZn#WP%+>`YY#~z1Pl%daR(c97%*U1>~Dj&TGk<<4+8$Nzr6TMnU&}m z+|Ow>S+8#0r38xSA3xsKH%SMc&&*lO-NXd8(~oj!RToM|&!4V$1}K4@vwp?rItTjM z+A8RBtEXnQmShL-+^vFHM z8H4D00oa5*NEt^m&+Akb<3Q^KmeSPJEDq87e2u4Ja>f#H`RBe`P4h@z@g3*L8f$B7 zH#weD4;Yod=s~TFrRq-(9;j_A~>)6IML z`cO%FcKC1)*A-FcFI?#LA}g2ixf}2G0A@g+i6A4KK!5W=^8c;^krSaG9C=J|HG{Y7 z287SXP_``ab{;m2?NFNjoc_Abfp>g?Fo9K?k-prov&$9}-pIyprPOP#T^qRPN#mx? znt9;Bp4&VGHsnkfQ-r+<2Q@K0wBPSw2VTrE1(3M-NdGl<+;ZVNlK`8w<3TDnnA+uZ z4|#SC>h%(W&7BlMWPWmJmm{N&Yh)xIc0}*tFD`&@QNzwF1?Q{$(z3xgw6LcFU9uz6+Ukxa($aDqRRs-0X~v7j#Yu^dPTF*7zoopx1q9l&p2fDRoNwE!UQij?Tr*2ttYCvBtN@Jl8W|F4?!{=waj2hMM{)drc zPc2vi>?_4cq7rb3agJZiO-xkq5sCmPL)@1p$*UdUg1&)qi=ubW`){@Otxphci~qQ~ zHGv?2PYLm(nrwdfFj@9~^@ffbwHzXl#BnlCGP-Y1vMuoJ@^|x@O&QF17yHZRQfy7tgDqxc8H zBUaC#%N&O`FQ5JSr#d@P;XiFgdyEz+tgxM@%~A*)!d{}1v1Zez=6aq#W6P9^o7m+6 z(&f`7N>OTA)cL+M1nHq1ATjm9Lx<|<>6sSI{WT$>4~A0`=9;!*;r{wEX$%B2YxeB( zWL)|yH3apfXgFwlH)*1aTM~lZqzU`H?JkZ+v$3p)|jXrS0Bh# zd1gus7Dx;oYHJ%1{XFB;!l5(PnxJ6maB2V{;`NNRo9k;kh-17M4ur~Jd1 z_`vo4WD?E#mg#ZEWF}D677wY#fO9!*a(VLG-0EbkJr*pm?0UEBnes1-fnCHPX^bqt zf1i}rrMb`At;->QX(LV;$1wKTef!!PnbaZ7SJTiC(8O$loAJn}d3l<+)>bZY0dJkd zbRMzdQVOQEJ~P8-Z0Mr=tSkxwpiIQO#5z5>!7f~|^XciE-v*dG<9R`0ZJRMTsY!

k9 z%_XUm9_zBii@TwfpX1x^@j5N?7DmwzA{Up*;ysc;O|kpPZgvj}#|_ z!XRlM#P6%o5s!oc!fM>4hlc{7;|yxnGr{|bWK&7x=%r6xYP!$#_ReWdd;NIg$+nRbdFvlYYWD| z?ZBuC8sTc!q%OtEiYb@N4Qt5>U54bFx6;M#3&B%eS3sk*D1Mu>1f$=`;QMz>2%k|CauN1(m2dhK?NMAOFe!4e2>W4REFrUuL20QtEAi1+p$Y3>i=iR zxvw5xd{7hEuopAojV6qyu~> ztNZ-@)=Yy3SXImq8Blw>ELFiJVLEvlh3X8WNFy$K?_k4b4nCCOtq6V!ha{0EtFn)p zIg|xpHJyW__Y8HKsWu^Nr z0YB;2cdg}5SM{s?Y+XIc;L`%Cw_@y|an53rZEA!=KN6g?H*O4mv98G(bI>DEd_Otg z9E#cfn@)Rn?NVVb+Bxb-JNEC_V1gB{=aTd1+-&ln^``wPb^Yc#ZLIC`S439i4IxXH zRwE4GVexS+_zE?F8dOEB7x$e)?xKD)et;C)$&xz1d2-W%{Orx7$ydvY%bp zy7j;aZ=&?8E1Z?=`}C0#HUYMi?{+z^pk3phSC3S2Y-C{L=NjR?n9433?MuF8#J2Gs z7c@W?;@f{-MpV=U=*Ru#Vn&@)U$<>l_gI0A9Tpe#)2Oq0@;*S%RM8rsO-L`Oz_2K% zxOhReK!;*IxRG`j3U_n(OZxP9XK{v6q z+_c1qMP1vNqaX}ReYLXq5ha2d9z1w;+}FrLBY4@e>VBH^@W9#Gdal`iV}dvIG;DZ9 zYKo@4b1bPjS)4`Nwo3@@ADQ&=0a`s~!Ay&83*QB=YEzZ_^MdX{&!H~Zk)%`gbV{ld zIZ0uZ40Nl(z;+8)cl9ml07qqop?}#B_iRkE5{DPODQa8buIp63Raz90#IMiQyDmKh z^x~?lIM04buV?mWqNIK+_1jj#c1zteIW_BsdOD6T8Lu;or;#`y_Gd66=!DYkK;zbd z9!36qVKfXFLbL?MR~R4*hZ=1j4y?+ohZ-PmAq?Ddy*C#&?W2@sU$Lgp(A@S^B|PL` z500+#;`%m8o!SmD_H75@1k|U6>*@FJ-o^QR@;;H67_VKsZ-nFQ5&-e$dIkFqmcHf| z##;~XF-`L!v}!Q)EqeO{)v8wy+P1ABhI&__0ZBm|z|^_R^UMyR973 zm&e(ShMvdy*YT@4s~3bvPasj6p*EEMHul6UC9LcCo&I}3I94|39RB|DpF4$Vh+W__ z+S%KyD8Dl#T%!V%%p`rVSF*K&8 zfu&Kg7hXnxnsE!vr3wfdfy>ecdbOR8Qk_7&REIMPXdL(G-aX=o4Bw({uC11Ca3*d! zfg~+x#SUt@IM24tKO8{_TGZ8y7*4IdtM534z9U8H-0)vtTnkN;{&Ag*(h5NK2AQVfUw95|zb+c|fj&U-3;& znRe(fEbb^_eXD!%rD!}!JE@&#^VKA{QX4>3@zV5$Gl9v9J}0#U$nvCDY{9!{>Opq8 z@2IH~fn~wbnrLC&1I=#XL15~x!>G@cn&N}wE!SNf9>BI2cYDsCL{dng+I##}fW#i1 zTDWdWgf{U#C4!!CLV|V}4ccm{#FW2kfw>Jk&wR4JczfgJt(rBfMwNKLnbArtbTO(p zH%~8~Pb}KbNY|vUND|eAd4o~p%P9}DA&Ew^f2X*kN9CS=oB#Iupn^t!(XwL9bP4c_ zuYBU6+72DM?p(9AT47;f^k}OHPEK%HE!bfkQNHD(56$WKIxtJS11^Z+O^+2}i1J+qRuH z?W<#j6tfzQF`0NwsJi)FL>z4@O1OPbquh0^IcX4ayxneaHl)rePOQN;2L;@JF}W_>Y}(;|LHQB;hmTo*P>G zxV%`mr=Jr@>ej7W;Sck`iJI5FDh6fsdi}uE8#6d=T(s-m*H|0$K1~{oOcscWYGV_| z44#WF??CW?#oD^0SOpsOv-*0*RtcEjaA)g42a{y@N#%uBB+*r*RLD~g`mlyB*TCfD z9@ToJ(&pq&+=>PEw9sLfs0U1Z69wknnSuvZvrbEPH%UAKQUT11gN!@8ZF1QYmKFn`GSL8L<;o zzw?xOVK2MehN=h`mD;#eXEhowBlAzbcTc7ZOx#=Bg+eaexVWFL)hEDN5fr2hE~F_# zdGNYUix;y{v~R7evnT4riKcWFlyPUq zN0A&~93;AKz937A`?TNxzq9jLz4bK}^G?i2nQCx5nv=M9|Nf&%3_QekBSgy@$B!S! z!rKg?#U{Y-Llnt6H5SJw!L4(*z;POZ$(SJyr(|pfLKlbUS%kK3&r6yNvW6YxhS^E8dM;>b7P`41g3~KK zT~t_j2flyLwz2c)&yVg{LQ@WVm1up{;P>A(0UErw)*dTF7lEI8cAd)<_@!vO_nf?e z&4MTO56QYR$jxOc?A5E6_YO}U2imN|96cZJ|CoK{9rlP3*E{~<(z3pFEwxycy0;^? zx~_25tWdPL^>!BX2H0^VS|aSw`uNtwm>9{Ebj&_hi!7FHOuMXEYAG0+R{@@mO+rfi zqws0n%keVWkJwAYK;PM@r_a?>f+CmcMtUiJE9p8n%)xLoO*J_FR({Oyoy{(d*4RJ; zB|s1$A;KB&bLaXmT)42!?(wfmN&-2B;5Loh^!W%?LXn9n`%1X{Muu0a*B!Ld9^^M+ zWY@u|9KbKy8CO*Mby+T@hSAxF;Y@BAFEQMqoC*{%X#(}uS8ZTwpv4+%aCR?UzChcz60SDXzzetIfL#|vc(;AJ(o z)|f|$F}UViv*O$}!&-SZUQXXf^K8GyQ^;&iUwY>)B^w5uYMtrds>hNyH(~VdY)Jpr z0rx>S=Z&&4;;1FILCQ zP>=bU!@)cEF;X^Y_})veuz7QDADhSxB5;JVx^nyWxIg=k)Ook;<74B@tSk}yz}7Zx zVxPs?dc;#O3dTI_@WAD+{njZ`iK#Av775#JYnUb_kr)SF@4N=RwZo4jUKjm ztws1-0*E(}{MUMd$7ZIYJp&)nzF~4}%WSmpe6HxvI|tqTz)CYT;em)<1@3gWKL;cS z+PAkP_kZuEufYq@aq(4$9lV0Ih@`&Z$OGyE5SsFT2IcF#Znx(LHmcLt_2EaOuj?kA za`Xd39fBmD0b)~ZtZUeVr9idk*H;e`=x*HiwjxdC;t-JAjMsBMu5rNa3COYT$Ia{n zlu_}hLLm=x!4lHk+HkyF%gIScjPa!~OKpf(jW*o6v`8MFp3z70@sD+ZtJ{LS;o97b zjT$tVMVz7SigAeFu0Y3AIk%LJ)ka#;SX5fa?bV<{O&J2NSGN3;J*1qbK|FGo7L)d) z+qG*$ffHuU>TKkE+!mFqhQiVrmYn4lJwlBU775K@@b~U+#Ur zIHN?BGJ}a!|57Gg(`nP%b6jpl1;aPHg9c9|c@%QV2dgn2VGW*w)8KmcKstxjJpUB~ z-onzXhPVOdSOO%@NN8oAg4bJ+Rz`*h8fM!^y<|}0y;GO!T|wByTy~E!hkJjzE0Ymk zlVT6n>HuM+8IXFJ44McEsxaYKj;KiI!+iuFq?8H|=@& z+_`Cu7zTIm+qZ0NKlRd@AI$(+2}skx^RN+RNm0cfc{1YCYTEIeip)E20_=+Yc(JRf zwjVz}gz!^j{z#9zrsh}YX3}K&acKO~eRzAfOiBsx_m}>X*Tg=jly@-@wd^6_?_a{9 z{)-InuBzfntVZTfGp!D&z`y|`wd}b`bq}rt!DF2b431yF?L>8XZRw2Wr)y2K?m9Yk zX?>aWriG={$DHd8+CkSm<6!yD0jaxoU_$DwQ24mM6f`}`nwDCikaz&WG!wX?>_3{f zx;*|DHElN+?SOjQw)(}!(pu50^?qz-YuKw-GqQv*yENb#1(NhULYWiN2c-b48hwR& zn>J187#ofB7KaDV2r4G(k6*y{?VACM!P)jy?LThEPXMvWHjD41PkSo@Y|#4GD-Y|3 ze6H!18C8w%^bp6r_knYqXtU;G6%!TmrH&>OL9&>a>d1sYa)d&mn8m5xq*=2+d;2!2 zBS$R$)Ivh|LoU7*96_jKHg=wgkZ=6&pPCW0tzljNXovEabseZ+n{)X_Y!~w!jY3CS zXi@VD8B6Pc(uPUugzbkOS^+F8wMN!35(UzM5|iZwdvLs)^F#`h&AViZg4HARQtms= zI@fPh5RiX|BHz4bCOpZ%2N>_C54w;ry*-an=;8wDzN&r6bMz+kVVpe(3$g@ZWeAa~ zB%-O+kpakPA>|7>_-F*ofLfT>)}&92rmGI<{JJCXAjfp=$92o?sUtI@JN?Co$a7ke z{Wh@gx-Ty_X5K(RXMaA^R8LP&JU|i1QaSd;1kcg-(nN3g313=~9oZ7mS!V-4?fNgZ z{KrC>NQPWAlRTLeOFj3cXtJ>*nnJy08v_&8& zcNM({$p?*5&Pk$vkCfYB;@+r}}SuW9S_~09{dp!*_>c*;^0S%N|6ttpVt2& z(5}ZvN}x9~l``Xnl%I-fW8NXKCHu8rvbh~c#qsiss=p+gc)D@hb_2~hQdPN+R}P%| z-QrS1n4G|QXiW~9cx{JE;sKoe^ZSYte^3i9zd(>>y1~Yb!1kw`EuugEfkr_jo!xu% zP+Mz=xJpM^@$xuYSX@RXfMEA>K=^fF7@4u-zaOtNr{h-ezXRr_s+e0-q2u!!&~<5+c#5=IAZtY6Z5}` zVHHX|_t)Q zp~WNJIkn4DfAt_|1B@kIqvL@OXnbDO4->6WX)M_|Q+Uxj)zM{C)p#qqwT zU4P%#cR|P@%`*P29v<0rt;5{PgO(a5SLgzeFS%eh2uPpMf;l$}FZS;4dwgp~@gsre zCZ_&AXnuTZQ z->rvq5(~RwubHV$!5o0w?VgtMR>iOR`lm?EKWjXk~9kkq9Qud@vMoR-g-??w!9sL_<+zkId*)q42hMyT4O`gCGb9(8~ z>K_X!g3%kJ7pj0Wv?-hm06y_l+(l$;LM&4Z8WWkif!BYyxp}lzZVW=O*Zb5y6c^8` z`cWz#N-Fuww{N$A%uV_^5QE;8?ChgMB1Sj4_{V=*fW*F}=GURCX;6`;(C}NrtV5MD zrGoL2YeeNy1tU1iwd+_6r#m1d$gcd;v(}>cBd%9z68C_OGc=q&(Bf3k20#x`gLUd~ ztWcu1A_fFy64#bYaHrRDRB6KG@XtoyOtNQ8$AU65Cfjb}L_wwn(h%c8*xH4Z9bn^; za1A^IU%q|23ZHNAw>GPN0}H%rr^oxeGOIP`f8~OT4ToCcgCfgrwe^pAk(FGAmsrFN zYtPYBh-60^F*wH!H14SB4qLr{3y==9AW}s8c5ivF*Was7Jb(U&Q9{@bnrRN=rpVh7uDJRbZd+<|uRB?xE}Y zk)Rj|NFae(ojT<5k*eKhyK!iv91?$&jLmc@(T{Fn)8+U-URDxST*MoBOA(T5k-WCwMX^2^1Ks{%(}$j%9Pv!N9=#HW z#N2KnFSm_-1$Dha&w~5+%Z-d4_*q;)p93cI0&Wiqd9(p~adg>0uDd%Y)I+KtgsFA7 z$7X;`G221dh3SDk^Uzy1nU(dasy3MH=Tz`%LEkJ(=GsLeZ{>VJR@ry6Fu$4(q z6R38jT-fd&omi2OK@WirY7_IG_Ark&$dZbg^609g-G6@y-YDFgsEL@7Hc_LTz^S=m zw9)2HP7!%|TiAXjdf<22V&vy%Nn-&vgg7JadcVJAt{2NQeFD5(7n)}T2I9x+G)b|P zZ%ZxS+IhWlB;6`3Dso!1XhuwS9!CfVePdVG4_4!Yexy=%oM|Ibk1dVvX0@Le7lLMW z(IKnD^iPmiG}vGV^{7I!utuq=m}JJcBHeFdQ|$pQfPkcc@hcJ-SRt@ARFVU15y zZt3MS0g3APVd2NCCZ-HJ=il{ZcD$$(coP@pU-E%#q&ymX+}<;-T}(;D#1-Y zAO+}^uDMxv=+I#)C$ZoNYzBp@Z#6E+K~#YwX>m$_%tUJ~@&gFcLJ$V=0?@!XF%7SBTaV7nYD#;zxpS2W z_G#%m;24R$*TApSJ-lDS-;t&!*fp^ZM0RDG-B*=Ki!Sr{{n?+f3L>QwNFiE&4}t)} zVPrC?D0@vUr|1!Ebw+d2y~pb*ndA(NAPrBZM|u>jVJ9|lq@!eI~(vM zz~U;_5J^oTXeuR$>*mIGo4)-Tf`XY;50GqeCNBMdQ(C^@j%*i+ytkdm zvPH1c+TcSdM~~dkWPyMRsvd-rG52rrIPBZZNvnLn3{3+TF){{d$}2 z?YsTc{L_D4OKlo_U!i|w;aE+*>w`=uu59#R#{~aT*LDW=ygFx&uSU-_lhk={`p?_G zXLf~S_~+cWU)K)n_;zW>d3(OEm|0SJp(NL{VEUuKKQrkCoySKu>4Kvp94OF0;=PqT zMz}Fz?J6#KRwm`mMi-F*JoYXwmRlk=0rcCKx#urGd)Wt*EC7mHeYcmKs5 z5eL`h4Ob>!L>ry6s7@sz=1n|P`EQX=YkGAcL)qaY!sIVoYI~L};DAqO*yzmwEI1bz zT6MuiS_7V`ZrCZLu0lC>zf=o@kj?cE`+R9A&=zrIs_T-LLc8KPB;?ItVal~+>Q;IAaxjEZ@BC+lmhBn5suzj_@`<;T#w1o(r`&zCIYNp^DgiPs<+s6| zJJ(^td{w7&WSX!HGBcMA^EBMixpTl9g*Jh7z@_OmcE_+Xg?~8BBxTbFzQRu-}?C6%Cu1)1+-7;{SS8v%3Yc z$Bqq$&do@zi{Y=3?qavNr@rZ}`OCFGx-UF1s+EnFQ6 z7Um2DYTf9;UTz&sWaNbldviKF!EDH3O@M+$*$>n)BB*7ZXL~?oQWjOfnl|DrLnSFi zi5nV~(JyGNrg;2IOPvy0X^gYe8Eo#>rgLbHgh?0PBt1J# zpVGpm+{+#6Sza7`XP|i;m0P+H!M$mtLUMyCi104PT!tz#%Eg>Q=lr9~sFD+W3e!U1 z7=!0OlX(&!xKsknTZP3T3e6e%hC%%h)7SDx!Pa^s854j2aV>fiRpCs(B(i1;^+x7w z2WU%No%s;+P3whzVhCWyH_$1!d1v& zf{bE<`c+p~KUj3d+O{dN2VlH~v=(xwhgE$US^VzZgQun9JH(sHP7NWeT0Q*C*fFq; zZ*iKyYTy%c97t5%!}qHSL@vO<4^27osRsvH2U^8U4rya@6{hy|xlbZ8%Tk+(1=93s z0AU{BxNB_mH-ZYhth5#3I8`4Sx9lBpe9IYTfN~ZqM}k)_^hu z9X!|!?@V-clIy~STiIgN1`Ds)?@}w?v=8eZWEqzPu}=4)|M5FbmpY?B$gM5hP(~Rg zcT9g_>Q$*ONWzx%zCMMUBw*kyZC)HF`UN&f@ft;n0m`;cVaH)3S_X=XJ1Ft8@0v(o6Tl|}> zHE~Gw7>Iq)7+EWG_$*Y+0$I@D2yILQE%46yWpA4?!)@Vr>tN)GPd|zcL3F*uLW@JML~k&6?3J^;>FD4 z1D3qlaD7`G;Gd*qOBE)Pqc?*F=Ze6A(Q!Q(8AU$BL!g!aP((fxCLAjNu4CRR5Xde| zv^_Ly(LHy}#_a>(FD8XwPeIL3*F}r07N2AsHtqNOts6J4@a1;sHHS-=6xfp;uP|cn zaQW!lea3fm;yuRJg8V~dumIYSCc8BTl+4kcJfm7FR~3jOlEn4d=z_Y>`Me$j-kgF~ z3{_nW*K-C;y}jl3ty@_L>y()t;dZ^sXKsRj1R4kwaWPux`AiE@+aVyD!|q?^4I<9o zi*uV#PBSoK!R;auC}W&eo?QWz0eDrPR>>;3J!H06>(Q;LF35#yr){{D9F+I@E~jJ$ z%0WDPJ(F))vi@j`SYr_PsxDYekE-3$tb8!+x&W}imD{&(57Bp-%N4@5u58sSq%|lO z=Urpy>COB0?VEG(Z_F&BkXC+F6Y)Gvz}G_CdN53;MJ)R+oLT#`H4!;>x-%IbMuG&! zulO(t)3=3lL2!SW_GGhWMj?|`TOMB#*jw#i4D zh&suGDW;*IoH=}1gmm&{c{=$L4EVywHjdU76|C4fIN|RSFgD-C>S(4fA{_XdR{c&P z^BHhT4h)wZPyoquN_-b*t#<`OHNlJ8RQfITUv!xN`@e6d?0>yVAidollf9bNdyrw0 zdRZT;uqg^1(FS5O=Tb<-5R9PRq)CxzQh8+3YAc!bY-Y^ZPW5UiZzIDf46@?M`t5Ch zZ^Y?6jFj~B&~auNQW)uFBo3j^jF-$ZVnU|Hy*j763Zn!;%qrqY2Qo#TUkCh=&)Ap_ z6~9h4zoWKj;7b}B$rl1a6x2uRAITJ;>mi@sVt1ogYlz0{;PQ}I9%*Nwvc!8yXaiy4 zHw$7zPtVWtI=Y%4%9Z6`bUi){CO|sPN#zrFELl3Escf7+$G@aPQeI9^uPY$|hq@#W z2AK&uk0gF1<*(Jxefhk-XbDyQyntiD5;xR;v$(42Yc8PViHv@`)=lrTrztIy8PKDQ zmAm)s8KD;nde|81n9s2V1`G6WBcL@7#Co9FSZvyg37NDOUML&}f!!*`6S7_xA#YBH zT_{H+?S;X%kS&}e0j&g25;Pr2E-f;O|8$;Di46hvlh}dP=$&bXH^OvCp!JY+GgX2# zhW@Kb#Q?iMD(y<`0ZeKF=7IZ#OjV4OO8w8DsQG+0pYZVTFDn^!j4{7NKk5A`5RHNO zdw5&b3Ov?H>=)&lsba@k$ANUuX%(Uq05J85v=wc zg;$>qx}mz@#`EVLxRs+AckG?FRwpID!>u6BC=iyiE!*wkpr3k3Y;k70H%$X~^h(4Ko!qzoS_5TnRV$gXyQ{gPV%4;{%tl)|#`sYx~tQl?# z7s^|XuTRbn9k)?v=Xm?Y_rHA6@z~f_r!CJ>=D>pSpdVXaG@UpszN}=@N(JW{z+f|F zL*5o`+noPyty=TuqJ2NmoJ3Bb=$C++v0Bs8cZ{+=;k;~SAK5$i0qhpuf& zWwg)9Tj1&{@or+1k$IEG2q@j6<~+fXK3U(&%J7_=x4k`Yb%(U-l%H%2O$Vq;V5}9u zaa3d`Usy1nm!H5NoauyQwbi}X7Ch!Mw8(d60}0=G-n=5x1Y#419W;>2em%9a=YDG|>&f6LcbP6^T z;hXLGvNCr-rz!K>)5uu&)&GNr&Ex00qx48Mf`9SaML>s9!bZ@7y}mb!Nmv9(sK*b1 zOoB&CGaxMv@7z?QdNR%jWLxLCBP zmmm|#!@${Oy#e@2kxKO@K(a2(U_W%#)-EI1fK8Cy37Ul_QlXf0rNr-#fFgKI`v(sm z)c(DHN#!>;$4jf#X$)IVihs8PJFyeXV34Nns>-mm?Z_yBIw~k5?k)WIWsE3rNE5G( znkl6q2`fcaxeK zZuEwyieFA9-KRhJV4&-v=NyE*^NKsTho(L!_{IB<_r)33o_$kc!x zILJ3ruw74%mb>FFE0h5c{lxo{sb{nPXQ(4Zy380GOD@@z<>C?QaoAM7HOWPODWDK}x>#yyb=U6e8;%{Ic9LT-C zkMOeX|`o8ALgb6qkX#uu@Z9#d8=5L2cNLZB#Lv`gWN|34dr*yl%l_gCRp?ji0@L zfi~@-#B}k`$qvLj1x{cuVLf*rQH5_-qPx;bnC4^5lLmGvh6vI+XHL{@{>P1S9a{R7KA*XHyAT0G1J zNL##eF_K1)^ox8o($1h>e^h!sVzRCyYE{O@Ega`)B&2jFhU^37*-DxSji+{ZGmYM^ z4-H{uKbiQSYuBdDC7Z3F%Q48P7_Vpn4)d!p>pcsZ*zZ(AwZrvHRdv-iKof zb(63~GX7JgvZ>6; z&1FXd@wPqP|CKks$E6nU!?*FC0Eu2{+{t4DZQu2wsU$3t1_MX1oySoC45Shnl=Gz` z-pb4*pm0n7I=XfD_H*;+-UtWEnPq!@m>loJh8^5jE~$wB{0W|Voopop7s@1Tp-sg^ z9HXYFxk|_lf1!J?URoE@S8>xNC<{oehy8lqzS{xsjnJcBCUp{IIg`0qc{Uj7C&Yio z)vKdo+(UQn)bqIFQAeI}Or*#$Z`8FcSB>mhkd_tJm zuBzS1ON%yxjwUBto{nQsnq*PhbM6Dwtt`q^=i;Qs_d|?mkfZBYN8Sdf1lHTa_Ec7` z?PlN2`QVT*Udvu5Z?1fRXh;%Oqw~+1!n^{Oqg=?qN91#E*Nz>Ece;L@MygZPNaR`+ z-7DzFXnG7dgDVW`%A_;M-iD38(|(Wf>_&XnQ)fR!a=+rSmAowQ09TG^Su);CA=|b{ zjS0LrH2XayHa(;G%Fx3omO0X9$ybL|*@80d;M$i4L#4+q!;Uw6>dgpQSH69`_~$KWSrpT=7`b?p z{BdSEsde(ZkAt|Qn5D@j=>)N=`sA|RLAtmj}r$Vrr?82%gm#T|Hl6#7IWe-w#27RBe}*t#$N$j(UPXAW&U=k zaabO^NQWN5-60PM{2YZ{!?EZ{bace|^A2UMQ=~GII$>Gpz;d1qc;nM~gYFH~3^tnk zY-@Op2?&SaP+*s)g^Qn@rk^wndbK#2t^pH-Ps%j6g4F3f{S`cls?YAPV?s0BQQZy4 zCwm9lV!1iBW8e7NJzM}rLV&N|z^yIkPm{c1I!|F8a~!n?ueAR0GaF9{*KbPnDL=li zNLKNSwRkuRD_<&;F(TMFy#}bh@J>-Rg;?N49hivRvGQ6(RyoaHUsovZ)6G zcLfcBGKn=GwE2z0uJ7BjrLoA^p`%n>yBHcaC0b^}p2CpGgD**DlXEfJsA?YYD^E6ze{g{?>Y% z($Y+X2jRRBc^RJ^c}aU_@EwKCCxEj{Y60k95Dhflfx(GS28l#IsfH&d*2B?Fj3t+5#h@Yc){A4<`tKQa^j<$@!b@hKV{9DfhY=nGEbc}J%|?R$Hz0dK3RdZ ziOB96=XTOYy24Rnv-%>Zh6EjQ%(6Iam9n*kM=LNv`8Yy_36Lfd8FYyTD^h9Oo8cB^ zR_T7}6BY0!H>oyU>o*&W-o z@r9=>`xTS?Uqe9VV|NKzGE4zoCcy zFf;{*9y6|p%uZYTW<5W%+D+1@H*Kq{_{JfwC}1VYLdy<;cKr_23@+<+Or@&eO;yyT zVb$dmo%Z<=6-5HR#c9vh$sO0A8L(Q~$rDG+l#Fll2-AGo@aNH0w~)F^*)*`}T-slf z0Myh*@(3%F*cQ1JRNtacV}LtA@m5c-A{Z6xe3*yUK~NeR0iPs>o=xzC`M$-Ay}p0{ z^sF(&T3GVv=I#1g6sJ_2Gp%bD^-~5MNX`L+2lsH>m{~xVe(_j*+bY>F*y+$fV(HgcD&wS4Q+w`^ouS0L_c*-QY zo&Ztva6W(%iIw_QStKQt7C;6!F+nn8D&4+C&*yzR`DN_DnSbZ6VMY=nl5;8KW&kEi zVatFLUvRTzx_}ebh^Y^!f=83SdvuJ}6?P*E63{K^|Bd-q0eh8NF7-FSwoClZ++~lE zx4c}YLjQa-{kS)gol3V2+MPPZ^>v*n##E6OW-RNVuAaW8u320Rn4f8(=qk%{&AE9y z?i=G^oLu((3q>&b>V6cfR$7lHb%RJS^?D}Z$WKzAJ$r_k&)&hqKKv1)B_#-@ieCqk z?s-4H)@yhNRzgDR=>rjK+vW`0yKmoYibKXT%n$5~2z&KYvRfoyz@ZBK+Zmm#!Mbe- zCP{Z_&KRHZlv68{(P;I^$l5-1Zz6Tzv9wQK+y>}SbB5hp`dMu=GlQE{BGTQ_{g0w8 zwT~Q6M@+3+d-5zY^KCpcqS(QAH+j$7^K$wGq;X~DHBvqH5Io*UNA>a9x#pbu!)aUC1e~*v%m!%*p==QrLE>@uM9O5Ow{|xDziguN zNn$ZuU#{WhoE%I2W#Ffc0rpWRPMbfNp$mon%R^DC+_@Y&OR3k>VwE50sFnG#=*+ zkkI+uGOx1%DXjjd{gM4a6gKjg)B-xGs|#D*%%K$xO#nKHCVrmp?sbJPoUC6a&f1qn zmF!ZRv}fBHzn1CDy%u?xlSkaEPURY*iYXYleTZ6A7DPKhLddNre5_X;&rt0yHL)Kq zY4oJIDpoF9bZSoJ$-{>?VTo!IkVd9Bkj(Lq=aUgi0Jp<3^4J*N^h!It_*v3#JWvxW zQf1#OAJg<1$trQL@*gm4$tG>5Ql>P??u5+>>u5q>nc=^v>J1Qe(OAElE4=p!=yXpJhXIM~?~`S5B49Bl1QcAi*;A#D;HfST29-Wr0TD1ivcYIkm`yaZX z1j0YZ`DeG+v)=uU#fyjY16@jgqj4C8*e&c%KLoh9>sBt7XMyDy%tnXZb0g9g00!PM zEfyk_7iQpYsjLl9fo;0uU7nc3{KmU@73Gfow>^$l4vJr*l9ul|Dh0sMLYCW7Jo$a~ zw@RvphK7*cVmgPh)n|x`=x1~m?D>l5Ge<}4Drd5r6C80tcmUygxbH?sa7E_nBxwc-&2$k48mVTqx01b8M-lCCkI-X5W%YDYHYLIlfN| zrlAp9T3&1;QYnsGm%mEr=+YlPoHW0^=)V{5PHebp zd3x7_#S{fmL4-X&H%>29rKgqHV`l8u_@~XJH@Nw#M#(T*_%+vLG%Q4`{G?-^6WOSm zqgmOSGg1Pc&{FX>{;s33l$*wjFwvok?m)DmF2OhGVrPMzWE4W?ikrrz)-eqQ9nZ(H zkISk%{noaRS#ik=4wa03of#Q|I|FZL(wy3zUW{~%15j2SuZe8sWByMJV1$|15SZ=W zShRP~*SJnEAkYlfX~`{aN?y7^vjJmOXJqkaG2~})aQY;j0c{(y?%US-LB$GN}<8n(+=2f!xH^)%9iWA_!YC z`r;832QiX1wvxAp>AH+`>s9_iNN9l-sPz(G3}qpUX^$r5szCBcG?0wXzxr@3qZyZ8 zC`ZNhWWFSgUtOz~B#n(ca8^ApQ|Lyc6^Z|Wym|kUQHy)h3|i!gX$?_IV!lam!-M1| zcTKSg(q%J=8Vu*;$H_OZU-yoUjkRid`{A8C8yL4l&x8U{t!J_Rc&6j497L_|J>=wO zM@DqZl7hI(>GHi{4v9WLkdgnFpWPmiEDFz-Um-yoKyl#fa`R=9`n1uk(D7q z=WU-iIEy|4Zf(um0rYV40*FB9V4GYjq4l`EbrR_8L=%yiqBWXG9zdx`US*eapW^^msPHYGUWEj(nLfSe zo?#_V6UR7w)ET+?GN!eYS7nW&G!j|k$BZeeziF>plO~q|UPQc^_-+|kqfgc_dWpAQ znlUW^O@-20Roe!GqNn6Or0b9^AB=zm39j=W%%XWc?ZJ`cA)RPYEjydd$FK z9p6BfQdl^y&~G&QxG2~&oSV1leQ1*Zp@&CcjFEbFhEGgqSdbnd$zsCf4rjU#Ae&U5ffN~4%+ zc`IE~>bweMrem-+To}>qi&wxJECZM4Xr@G?_=E+yf~`@%+kRvUg?xS01w8U?o!Pa zG`TfF=T4{JB@V$;PkGEJ%+Aad(~3cI8)s@dgSh6mOSRSC3zluT*FH8T1{l&i=rK@6 z$m1`ExQ}1He$~{PdA)r@zex-sy!NC;*sY!>EI(ECE;7Ne8;3=5#axAePlu6lzEkch zGOo;#Mvc^Qa|0vhof0(2`cu7P}a+XvoA{MCO2nI z#{b4Ii|DT1x~TFe~nz3wF>MEo~ftd zdSJQ6t%FsGF7HJvwHGd>)_eO}zFV&*wdekHAU1)ckB^61k!KnP>H1TR#0?jh%kvm)T1{ZAX%X}9eoH+k zQTTG!;Gql8S=|szI^rFaMJldE1VC8#z1yDT5e>K%AdW5cu{~DIB$|hI^LS9ws|4R^ z2e1puahAF@SON)>6{7%&T zknYFzyun*&6I`{G6C<>R}>82|AHrka$X$KV}V+<7Tfi8=FVpI@-LOYx24aLq=m zFZ;ZkNVU2cxcT(DsvikK=ig6iv397GY8WCF+)Q-^ala9z z$rhoWml=gu(Fa{?~woDxAT zEuuCHTm{hcz$N65>cxm^TTS(`INp9_&{+{tI3xsfCQ8YB-TEb$l+9=pEI#58+0P2^ z`eFIkKn#5{5w(l{Nby%L?>-^Bb`Dh_tBLIgLk-YPtLKkH_ad_({ax(XMY0tY6NunE z1T%&Vlucg}%9Qa8$Ri-?=gB67OiQGaY1x>lW`dr>+0^6V-U6ukYP-?Uq0<-b>-;~q zSlx20ETi)G_sQ&tO&QYfSW@z*zm5jR-})LF60(HIp=PsM{RN1w<7G5>O<7b_WeOn& zP6_zKjAvc0)2vtIstIT^BoqJhQr`Woc=Ig(PP}lT9SDWj+W~;vX^Rb_7>BvlH$Jri ztL>I-><%ZBoZZrXz;2iEsA-g$)!-hA=OQ|lXj=ylkno(hFUIa4<~jB8)EEY@GE)UW z?iJ2^{Q|cclYz7DRNMk5w!7iHs1Bo;q+UCf(kR{x*m3_hRy-aE3~iYWG=uNmg(-%{ zQf8*E`FiB((Ch`sHhP5Y#^b^(=zhoQ8pOOZGld|h&>l9D->>nVW6ipkz7pKUgN+5h zw6Rm%((1Nt*G?=`4pqpw-Rr~#Q;k&u4ups2HjomS^Mp!fi!!rSblnuX-Rp-aA z>^b{Cgo$6Iwr#PqbB03m!D)J*Ci_~3p;DwtGR`rpC7i7Z`^;XvdD93SE$l-9WEH}R zu4lJlqFMB^tfG_J4vD+{N6L)`Mt+qm=WmHQf;9n76Udi3j< z3C@NYa7O-Q>O96ty{~>C0}SgTMp%60NEmZG4yg_Sh3Z-+AiBuBIF`Un*~_GR_BU!C z;sZJJUDj+%!gqv5-~CDV57WbT?IPLJVfA3rRVBQRDdhfVk4GqB#l9txul@UUv7Jnp z{^M2XcnJbvKgJmM>~6|+ga504oES}XYq!Ix%%AUayyWyERv{sUZxUd1e07rt9qA%< zmaS&(1FGF#Il~BdR_obEw(a15m4}W99HZoHL5lJk{Uz8H-=<*TZAL-T^1fGk;R}q- z+ir{~Pn5UUpP|5f-7Xgc9g_Ul%A?N6EtHv_q;tHg{7kJnySfTU)b7vyzIzY9E-5jJ zYBNwj9It^B)M7^AVr(xwS|=V112vYW%v9ROI$}uuV0<6RqO^#9Df92@e%2r1auhl< z!#UPgXMwJ*5q-63;Tslg@c~?RjR&7}N(dji?rPm0ID3Sz*@m+rw zqOjq8kmwtiWTXA}<1Kzg{OTjV`3;GK&e`8s&!D&W*v>IdU=v!{`0R0kAlcvT+go$D`40&fX z9lOq$orb~d9MLsWUz_an>1t2KSEA*}E8477z9ilH&4%blsi({rj$>o4KgDj$sWdSx zXszS;kY}c8@stDqX#t*YJ;1A}OE~2G_B^0sA4m$`)+Z%x4O}4dY&wv{mD=KpqpB#A zBxL}x%du*+Hq3`7*f_x0{Hl4YU~LJ`pvlRme^mAwwSJM~&JX|5zdq$v$Y@D%huFEZ|?5 zoe3<)YmK8|Jhi3ayt%J`Ixa`JWBGb){@1A(zgWO#*+2sTaN?c<>vFqj-i-`9Z}<&48|!;dJ}k;c2qntD_#c?!UYc9E zWI;PPe59!~PB)9%feGCwYf^T`Ox>>)Hccs^8N!6z?y-kYe~M#1>Ly zNBmHB3ylHpCv{){VE^uz=AwwLy{ydFj1xTO~ zt@)4yjda~#DMeAeIix_YG_(q+%b)yhTUgqIWjVRJ_E3C-N7y|pa4A3;$UBtU)tW*9J;a|^>$G+Y8o;K}S?cUKx|2ruizLQM7=MoOOSp-bG zZ$ol%Enr+jP9RmkX)|V=YFS>D^% zT4<%QqH9tm@9MB$qXGK3%(@LOv+$#M=mCoZfH9!*K;fLpdb@phgwWN-KYs6UfBc+b z=GEK6@{s{MGTT8b|R{Tx?qUTeNM% z3H#{LWQ&E19EX>aOMc{Cj%&uG}D;;34ZAVO+V>v2HWgl^5$o)k>=Eq3GHe;v{jZm^ugKtuC65S5Cqx zGYaE)!l}vzCM&PsxKT<=qUtuA1YOyeWOUyoIA;#CFP5_7rfN;`)u{HYXAqhbkFK2F z?5m>Ik|#{|-C;KUy{(!1EcCR(#e$C)L$jXjG#-E#EO52+|C?jy;`yKCuu$LQ*)D%h zFtq?rx_3Q?EZ?VhzH(xOeE2};`pP!wqdiBRJd@43>kc&T-TN})(N`T;!Rv%1n+T@S zt9^&D2L}Bewy)`mPfzU^WnbW9Y);1bM5nGaQx~1h=%!f&*n$(eny3rjw zNw?GQ(HiU!;;A;bNCQyFq`WD!a<5-Mo|sBrt=hWmy$qj#90N9d@ly7~b<|6-5)DoN ze%%tgyssQfbm&`8&0t%N`zHwuW#Yc)@%-p)JD;@wjIVB9l{ow1HKb`ScypH!hCG!cL}rlGIK2f=IS!~g&` zIQ?YD8+`0i!J(o=g7eRxU@LLdfGhiCu7sHZrUV@>jj&ReBG&znP zMrx44MlUp-p~&JR2idaAR=%*P@sB}NoV~lQ^$c)F8Kt*dF*fsHm$Zu5v}G|}7$8sv z4u8CC3Tn8}6OtqFW9xW|w!#UVGx(|iGy38rd|j1T?m`k>83=@Bj$E$qJ`g3m4v0 z3(TJaP_E1z${NbhF}1IGUBOAi*5v_J(1$pD`uYqyTPL?#w_9Y0OADK-mUZxm3b$vO z=6Vr6HXuACB=TO-5r!w6ovoi<1twPZN}bpWD*K?RRwJ4EXqGYxFS6kU)@MjW?Ir=* z!~?hBo=;Ba5KX#cLDR%>Coa@a1I|}8{qIpxSC8=*2vg6G>dKn-5WizM-p z0e#oA(vAa`4A)q5BPS==^%9{hdlTBdDL zSFbigF~s(wH=Z`*fHr)FRz8}vbotspf=g-<5S(B4y;rEAF|B%rbGNQtjnfqi%4(k7 z>{QzRj+aY8YD)a$szy=6%(T9}KG&T49=U26MM`wi`hlK5zPuF334$iGVx>m5{U37d zNA;#1vH~2@hGH_HA}q8 zi^wSlOV=uCLPd#V%G$7Q|8MQQo$31Zsqv4IGdO$Kn!PG`7;Wq5|7zs@yw|{=R>#A! zpP(-UyAd^A{-uHPB>b`9p2RY_2d<+oPAz!`fb`mZyewV@(XJ`;0@Y{i8b~(gL$$7x z5d$zPcx;b)bOgfr`|j&!4~~Lh)YP+Xq2&c&VP)U9tiOQv^t{T<&x9!$4f%QF1D=VO z8HEL4C@(#apE!|8{%j9L5+UVk+iNTVJ7iITVkK8t)7k}&$-?0yDDSV<<(T))_fv;Q zQ%!CjZP#LoN0T}d!mJpx6?;bfW7sUZRp$`P5L|c(V1_yz*9Uu+cNjY*yo=eVM5>jJ z-tJue5^I-bzpm2`OFUnb;%1!Ok=?ORRB%gW{BcbyT7h!a58Vv2nX!=%Jk4 z2I@kR;b*?{wVHhPK6lORJ_zbZ3>r#1qEl@n+zL|G?dtX`D*T@g#Ljf(%A}z4e0D42 z)+6qc6xYM>8-;q$te@dDDvA|DK;<)>1Ka(fip+7yJ2UqqcsXITuMPdoiR0o~^ARu= z?9r>0c~WAc5-v1v`^6+R>->wWnlpC0Z$qJVBVb#;@9A4Afi(O z8i_r#bZxB|NlXimyV}SU_GaR5G?1##zW-K7>@%RZdQ(k-*%f_jfbbxmZkdI?$|FD@ zzRlE`GTJ-cE0Qh1KpZN5D4czH=lbNhl37_}(LLsOH-$lb3EE;zjSLtK>YH78&75$Mj>}*YY;^K$-O>RXt9xtUd7z>Y67SkqqDE zW83h5*u4C4_V%r4(GJ^XYm*8E;X~Z|*9Hb{KQ9?4M}umY3l@<~&NQILgdLd{x;wX2 zubx8ZrOfI@AOZ--9k`3cYp*$uVf?lNvMikyJb}ZBzk6D>+6GJ^Z#KL^2aYuB&z*RD zC7uNx`yR(4F#Og?+^u_8Z(*zv=ccJM50Qa+GvWOK^bMO+-$@fX7+>)Ise4V;8g?}` z2`G8@E=SGI+04yZwGK~EZTP;`>go@)5O;d~{a~_0`N*~j`ejwuSVC;92mG%?&`&`* zLZ6}H_na|K?}t%&@!k6H);rhbNtOk0nk>F@?-`519YTYnxu!CWVM)C8Aiog&O}Wxn z5D}N6ViT{X*t?WdQIox>+iqz z?Et>%x|05pc^`{Y*xtsbv{UXt4rqy4VrJd>4`|I^H&%Bw*1{a^KLG((?us4 ztUsem;E;@fZJ%?9eQ{5AEIX3s<@n4XOqKW4W$-}(Hj;T!+Fcy~*Gi-~Ep{FV4ZV`p zGiO#jG4Xdz{MrFICdOja^?8LvuFUyCpU9GkGq~*&E$#Xj1_#d`4nJDtkW?41&s8K7vNJ5_%)oE5W}vd!U(+5>{JQUXv2S>`K@5gha4&tCe@NPtO2f42G3*AJT5_-^wS zvf-DsuS+wg4<^n{adB!zBmSPK z$FcgndUiLgVdnoToA2kYCMGi-utgr2^1@3(d7Yj>r#1S$Wb$)nyOO@uG5;m|*d(e-Q_Th~m~gw=-yk*`n*MIcMDd7SAkB^awix{#BA!B$7_X zFT5bKbO4xjhgDZKJ@sVA?KH(s9QvkZRM8dA+yCNrkb>1n=ZR5Dxkib~yHrY}=n2GC z95Rn4Hv(K3GotOYQlU&NK4Zdga&ft?_NEt89wjzVsOmESG`(5%rnRT3H|=)(M}|_Z z-M-61j#XNWbbCLKp+~XH)nk6A6&~ZO;f`KXl~o!4rdDpTgv@4V}V^3}r!4-(Ex zmx*u1=3sTUFi2^r)#?$8kv49vRhM!R9xK2+vXh$GH zahRifPX&FW9N-eLif_9W)A7#bb=&poWi4OopFclEH|~%W5TK@3Wn{KKS2Dp$j5`Kd zMSI)Em`$1TWM;cgJGuAt@HANGm%r0swsA1*u(tVo1}-&gMG*Pt;yPIqn#t@aCh{)X ziY)IV?#bHtg2w(lhn7Vmu;z@_@cQ|#+qS-zLWEQ;R_t>d^A>1*)n``&1A{|$24YQQ zP7&Q3GAh9(rcP}a5MB45mSAtZo@8KVFUuV&E89+uFYw|w5@Pc)Xp;q^KYo9 zdm!`3`g^DgFCA-&k#5V>q%(lXl8MO6Uv)Fe31M=Jrwa8Siw|c)h0VezBDr|`W`~S| zhjIe+G(Fu)wIP=f=&3g(7r(0r&EHekxP)ts4skXZ*5lv`-uVNqisT}n%pVO+M6>2h zprqL8#_!!en?fbe(?0HhM8&Ts3PMEr>NK_V`L?*YxcHA;;Y;QB#u)ZdFVVEvBCk)` zO;lb9@wJfv%ceCosb6moUN~gbD!iRmsk`rVT3(tiYoAnIL(T)KqL7aLq|;`Fqxct0 zPXqV+^lCDq`CHmHjh#bjxVD>X2H#GIkH3r+)p-7$&XVbgFP($?*6mgUJ94`H_a~|s zbV93;!Ps1#)Xu#ZSO{oLjaDUO-n_YUUnXRCoE==k4k!&qGnCi@G$VEvkuI5O$P>;C zZ-+Cer{OaebT`z~ur~Ys9S|H1@#Bye7eEV} zb0*BHt5^`(q37rtI5XPT3E0T=bivvWJrn;p(&c$KD!^RKK`OUuE0cCuMF%bHXPkPD zT^mhuP}>?2V29@AmTu?>$>#{tgJig1b${rFh<@U|FEpRqS>oIl=MRzO>>o+AtcNCa z7t`I#cSpnDJU(*JQ}5!wbNOr4OsdN@i59wp#qI3EoY$W$t6{h$??*9h=EKJ=qQNjs z-491b@`gOQ|D?{Bb!zgEd^%(s^99h?0~uiGhP5HbL+j??1Bxu%*6Dm+E*GsW@3*ZiJ|sFQe*@K876 z;73P%cFrC)XmvL-dRs8_0w$Vq7N1cZ#)Lkag2w}rM6s}JCG35^^^0ME`tgE}r03=B zH=58h($3(Y%A~6FHs|G<>*TbNerYxk9I10n`NTbVj)vGhWLj2(xz!5uEv_q@&=`D< zojN58X(_;-Q=Fkq>v1*SRADS7|L)p>x`{P+JqA*1?IC|ezejO=73vz$ebI#{I zUa#kRZ4}!DA9(+8(|qh@77G?VijH$nd`qPM6{STnD^Af8oyYqF@ zw&I4F?)zL^2sq!Z=*`~?@L7`r)<=5-x9GV`(U$jgd<~>roqw&_elstQBUCYx(WATk z4G-usrd7BhYHo`wE45EeJ|vNjA@P=P>+$oH=Zq{7Q!u0(!EzIBMWmq-^0f2a@bTSp zd>5W}s~#5Ih$GT096paT$$9yg(g-9}sMJ+bThCKMER=&VLuF#UTfg=>pM zfQFNQ-Pc@s(?mb?#Ld%c$gy?m)(K=`y;;%XEl4-y&%TS=xXMTm|pIQAu2ww2fkxv6*^Y`?)?$Iv+&E8 zvz5#G>^B1sY({1wWlUrA5Y^{Ak-Q&#i>H3(@3oM_jVb)ltP4Xn=($0Ma19m-(Rvw* zZ?>a%igw=HsDsjQxed6d4(eXCOkxpLlQ^4+-jx#&K<+Ehb@D6&c5I7Ol^ z+CVkf-}=&ZD@gRYvhtlGrC#+yJ+-XHyp5IuOhJKG_x|Pw=91DOQ#_3W?(StlmrZlNez45}#JHZ0s5w+erjDV7JX z4@kWnPB8O1&wp)(=d=rhOBHp<@t`!=@D8%uttf771Z zN$YqhX{}bp&!%fCU>2Qmp>{ zd#Upa6j}EuIR!i?vcVVt(r#q8?c57fO_oE9h2*^1nL0@8La9Uh+y?WDNjbd;D&AAG zelv332lC={S{BuO2rEr>La>7y}oV*y+uV32NBj2Vc5j^ z(G_HQ2Q@H3y7W4r9(xdM-hS~SBt;_Gpa9P<`U;}AzgHLQFMsKc1#@MVtGIl{Yv|c? z=h_B+qbN~)GZB!<)9%X`ix9JuMnv0Spz=8#xMd-1r-X80q>P+j_{?k#f=-B4WCr{e6^fHQlKY~m5xzhpi4nA#~h_air zL$xe!=KT4>SiXpFyriT=_@YXH*Tg&?_Y;F!!v?eox`CL39(C!fK{(APr*5jDT@$O^ zdS(Tg@RJ8J=|wy5b4@~@&m-QFOSw-oZ4;)dMUdemJyg{kJ9dgySF3Fo^2^kBijL}3 zK*PILg#I?lG$>a=b-A1CNg(NhZDLG<+cxRP&!2bE<}7s|BZ4Z$NVvQ_p;Z!-xkdj< zAZ@5LU@}T%e=A}?G^z|Kdk1myYUg!sJXnR3_D}J=A@X8c}t|JvW#DP+&+z?=N~7KrsMfW@zF}{5)z*6arFA@w_!*XeKB^ z!_WJY8RWE=-GZyG6bsw4b8~RfK4fM-s~e@xcmr!mjD%ltrgw*>QcKdA?DO(BOy3Vib7P&3$#pxV^#ekD!M z*omAsBqQ2+z!6JwE_CsoEK>rMXbw;UGHE}^+q#MYUJ*4c4v^pua-*14h2|osG{LPx=I9t1?JS>S;!_mG zo#$0xkJH2PmfJ9PTgF4I?prTkt{Pu^fjuC`cmXPy?kz+LHDT=7Epwd?$mQCOuUW0C zpMN~TEkaM*nJEim97KG?pifEwy<9P38i-MuU*{3qXwIB6N}uKAh)fcV0Bq*&{RdBO zbrSVpgKhAtrKP3Qs2vDoTcmv>C~ts&PjRzz^y#esjyTrRYH)k%EFOGT*h=%bOIL1P zdH0DBL{amIfQ-JCI)etOq5fD`R}We_5C<^P3&*)ypt<|e+qvZjFXHSM&e}`%9(0r< z*y8QYH{Eg4o}ijr2?0&hE2g@mH47cSVd3#-{{HD7beFdg5m~_bK?VmFkKF;IyU$pQ z9sO8~f{;eeF(vWmuU|GuImGCBVolb&swhmX5wdfWO~b;r=^O^{j82t#tRjI9=I|H{ zDF8+U+z<~-j3xqm6jde260DdYd2V}~6mkR-22nMjO@4S8J-*S&87?;MKJ7u4II5}z zIuCG+b>G{FReRvTPD-y+)99Ud6{^ zY{w$2@JN&&ZlP#rPJ9pp!H18W7Y16?Je|!rWWmw7+=3G=DxvhNr}Yq(_h^40hQ%Bm z)H@<~l(@z%NY|pb{JltJPFk_1$M@_}RX%sSqmRMm&&RnvY49*H2@}`h^vIpUhiNxz z#E6(%GN+o2#7j2xLyuo#_l(msW!};$sj_Wm*qED^<>wpPyHkjuMK&u$Ptuolpv0!- zUE{ODFQ9@+WK z`>4RJiFOeQS;uhI7(MH!T?@gy1udC4)SNm_Uo1?xHZJR@nu#xS8&B9YarT!xBKZr& z_=tZNE(vR9p(w7B8Jbyb%sY1KWNABixR^WEx9@nW8Pc5OtOQV8>_uZL#X`H zlk~TGH!GolzMIr>IP0B{H`}C=A6tl$sC;NyNy#lvUC+@EgemXHvH&;MIJNW?leGkG z!3R}Ko(`7OFXt2N1XO{q?m5b%&Hzzw<+Pi4Ulm2FlkW3|kIbjGEn+=;7W8S`l6p1z zR@EXtm+tk11S^Lx;>!}L8nV{HF3;M!>F8RtY2_P)u1T0dq-hBE>qM zmFfXw0@FVngos8$sET|RP94v9Bg|?BSL zc?L3!A^sRg5qmw~=j14$2+uu@T~s0*WqZ2cO-VVHm>62MWy9W8p9Q+CH|fc$cnV~^ zI*LU4g{m}8p1M)2x$wl0skayCH=I`Clv>xk5KVNa0i(1rnL70zUvORN2SX%n2&I!^ zA#+E_y)Ky}52V8pQye&jpy}Ijq>G4Ri_U{hB*9XAKcQaB6x9MBiE2*c*)86us>osY z7aHy_#(P9=6xWKiMPM?g%lkdUUJJ+nnhW}nut`0ypb^;G>d}&8=tUO^)4Kguak563 z0)dN>gFEYLxq5_sni6C2An|wN)uc^aucN6~&;3fqZOwfuvh54Lx;OFPuR&%|(h8o2<6<#d8X84&=Xd98Sc!8&jaPZ<7TeeV$XfaWvcoM}GF zI7x!3jf+c6c;>o?sKZxiZn(gCL=?O?74etkq?h3%SX#nADrP$r6yxMXZLkw}+XTNM zXn@fYwG9{zE@*91@knMkY9pSGcT?11rabLf*5!&>Z-<5hfzF!M!@1FB$+9GYCwZit z+I&vO6hOCwXbouX)+~R+@+%=ky#9kjOD{zVJz?SkE6IWZ1Ik|N{$S!fF`pE$n)MZH z{+-kNH(u8|7SJJwP!UY}j+&ggXaX=yy4S&EqtDziif`Lb``4#yc!6IY8Tb8=h@BoKTtPgWmidmLshG8auor ze3+`_X$Pz#rZ4FZcAU)k9^i@ZFLf|ytI+q@E%L6DVskDN(h`mM7P84klNemdOpb2X z1};QMr5P}aRE#UvuYZ|xM8s!&)B%(grRbakTyTr>SY98LXDYK&%uOzkDvXuUVxc%T zodO{Cga^uL_T5PXZg^sZy$#Ubw1@ahUh2JI_)PlWT;M#C&!-K(hm&6v)fZ(vxpt`AOrO`BCrA=Lp3=H?{2MI!q>wnTeMs}x!|{7zLyzah^h<^d z7-<5TJ`B>!#DY<5-HBWtVO(dxYJbbdlOKDCguEc(<;t<%IOTnLDuM`IDc4=~hM^Nf zv!@nFO88$SiHt%ci}*zS!9l^3d$tKW4+bcet0;z@kV<~_)i+o?+O#8`92;OP5B5jC z8`}4}L9dJ8P=xmv=#aRe?UXq%a4MCDF{pRN)iII$Ve8JBkVDa|ftxkXaK=J5N>BZc zQk_3fdhlTP0Jp}|EG>KMhD=cpnNZOl2%s!?TjL83E&ZL*I z_O+exaVDXTHa6S1)}yafktot#dYKcgRaJ)wL5y%C7{wyE{|DXMjmZ6IbwW3OziL11 z-Opj1BX`?*bz_-vYU^G3Q0K`(o;$6Lf20@%jQcHjsH}YOB zXZ~3f63JGP$Wbtoisi}_=EX>)bME(?C3*VnnP{OXIC>WL?j&%E7HPz~jrN@sZVx>! z{v7E|KgaJV$vyQ=Cr_FbSWxXr7dk0+3~tnEz1jv0eZp0k@aXP#NTz^~$aAJdS!;4z z3-@UJ@>lnVwb-CV6p{cp8XSLPMC|Uzj3>FJuM*b(n;QeAy+^b4P~X%aW4UQOi#^t? zi8FsTA&lUn(7ZXA2* zlI_-zu>}@~Qp+2H`D}|QPDCg{+_Dv-;-=kr_w8FoejA0q4D3+#+LNtV6BksQ!`8pU zStoi!wwPsGiNuHb3oX+xL_{pyYI=p|E2_$Qq1^?ijwe z*)h{m^Ap`WKo=XuZrZfz$C=rJArjfkJ$!kfpc1$)S-ZLB3czWF;h|&E&AV8A~Hbzt>Bn6F8lDYy$U`j9e3nT6dx=GF{X8M zr9LpVCjATJT08C_^t|AZnEWGz$luCY%+mFlpI+a%F8xhN?>0gKOrJ?h4=mWM_FP&j zlB4aeUb!Ol>;tTo+2W!H4&D6AF>%nN@f$x!p7tE7uA;1bhXQJ7<$QNstRg&X{c@@+ zs0+8!X%`tYt(<-#DoQi49+pcZXxXAgx0SAaAyGHiM3D7|})QM}$i+u~^ouR}%r`{${Iz8KFxQt!N_blSLh>%c%T1J(6 zahxQp@fn=mNa&Q0ISk*LwUR=iMH&Zwf9{*p>we7W)z+vSvTefhknBdT#`^6Z4Lh!JkQ@2&^To@Z zon_?iao^-=W!>^|3P_!BUKT8RsI(!dhkKtpHjRrc@n^oTePKe`{l4_;LC!bBo%OFm ze1mBs<~^y%DvVfo%u z@7gy23-aZ$B%lWd21dKHUPP@WI4a?w&a|j#_1{5Xs~`=C*ty?3_zTRtfMck9vPeKM z_>uiDG1T^psLEciqE&90*1vb}MO#g0(1nPQ&Wo9b$~;>!?O!ir?P>vUK=c zC02z)9R=vE;wG3DIa9a)pJ%KlS72$05hJnHV{d+3Itxq6{(ID2zI5b*fTan~Z+fdr zOFPdw+bp~TN|v!889g$XRzJ?#I@G_GjPgDh*h7vCaVuG9T9y6>gA355mFB+UaU-D* zUk_LOq&nvo;PXJ@XUGheuV|j_3<+-$y>M%EjR+?1p4nT}%v3)64d(O=6f-RoRz~E> z^i5~pTWVgQ`1$T#t20r(Wrhy7o0c^kN#To_HAtAtLd+dX3w+@o7vujBA(eE`t?3SE z|8IQ#AGXndJSZ-gBbHd$xo|2kx5styRO)@ z2@*s;rMGv{>!=&(aZN+XobPYz;enmj-T}|i>f+b)%YH|Q!#t>pE!sRXEPIs|rRJF@ zIUXe6@{&3xMEW6^^Wvi|MlOsCqKo10uR~HlxN3Yf6lpMrCij$XgbFe-HJ$!@P#5eQ zCg0h&S)5hYdf`vQddM~_{Su4W%Wl;#Q!gb#qdN7@)U5el`u~}i8U$|I6x3RR{zt&)OYdN;Cp`$?N>jFPWYJx z;VxjqZ$%bjLm_8%de3*?Auq+EGg&5Ew<0q$(?M?97nR;ZN*|HATj0A;U2XakqT*Vn zW!%4S;}U7x%|GO>H!wrS?@|Nh_^DvG#bGDCqZk^3U;( zW@fLhL|heIB?{4s*+IA>?Fu9jQfkVh8~VC$e2cpDQc-j;bF*-oc?mdGi+t zsPpIB?X7-x4evo&e{Bpn!Fqcx76B0`30fV$>e^-C4}22no6K z@@35hOW})r_UfWjCxUMiYAatPb#$obmSqp_e7^TIv3WcX5L3Ly)&r$2n580xsUR&zWUk!E+Zwz8A&q*fv!5#jx9z#|J4 z$yT13MU&CtGa@?$*M)0M5Q5KJ~87Ct!+K)?D6AZ@_I{B z6xX8U7H<%z;mIe3P6$?kc^LxG7`ljY^a`Cig^ZfA6@`fyCPLmf6Uj^SpS|7K`d*-q z-aX<+${vUrw15Dy;l<}Uak#{mtu+Vhj~)qhg~5I3s7aAC^JxMD^sRa3!iv))9nvTG zVdA=p#>R_CSQuG*?FTXJ6ro~7nOGzAZAe&#LJTxT9d2Q1dHeBWbp*5tnK@hjDt3?o zh$|b`+i66Q2q+?_w(KRN;s?@;y z%XYZ85-aOHH?zgH)xB{++&Nw|73Hu1_=Su{fY?~#`RuFfu22S+qQ*}mV3S1PcRqE2 z;;w;r;Pz@nKJ9>lQQaQb4N}xMb?zvCv$Wy*sN3_0(h}n~5!>YE=8o-uK-7zbuG?Zu z2sY_|U@<6atD*e`SeHPCZ~NgZwTQN}oV=kVGf=l-(M9?ed4r}RGX%nTWR_MIasWij6G6|22vKx!D7>lHB7e>K zQN`ojpBcX8nExMph3Fj@Yxv;;cScO6QZkP#J%!Zs-_@W=%Wm+r9aH{bi1Q5dfa#GlN7jxw86Q78x6sb1&xv$3XcFy z#N@F-w*90|ySMBa6!H+PyYS%vHMLLif}kMV5uJG6n7R;-%A`AYN^=+c#Y~gQrc|Je z$fxGGM=Ejg?-iREoxo@{o>qGnxyG;cTPnK!r#=ufa~Sx=rI9wL&;E#rh%JPt`A%_K z1%53O*ohfi2yJ$W^tv`d89jZnXTmaxYZLm%QbjUUD1l$W@Ru+zy=9v9m6gZFO*X2k zten7V!EmW7t>=B@`1JGvW*=F-etTDII5Yp3f&)zvu+j0~TwdzIVmjI1ua7?rIGEZ~ zT@uo=D1;Esf9)Lh{ig*uVsLMY3-~xTNii#I?woZXX(G)R+)*4qEDmL%Eq6{k*)03y zg$w2u`i0lpIPU$cr*sUIDIm4=KW9p~;t6TS#swR{4P}EhKYMcD6jk<=FXAUN{i*?= zXO9;@O)*|a#a^1+X6lj;i9+5A3&A8p6V?wQ_@fyY(z~cb z+|X&Op!fa6@LnVBG!D$>>H4UJ(l_pCE(v)V_%V|Q$gB1Mo^cU0Dac(g9Jep$FI(&z zL#@+_Q)N46ac2O&c7~zh|A_8__O0ZP;H53SXFf*>KiM7qiC~eiYFM=HCvocE{N9#J zjql;{DJrI0i6M~!Lv-YwC9E2cYv zpMb3ZXrOB{c>Cc)QzS(X2k4}gnDJyJlDN$!Et)rPeQo$c2Hb%_%#E1YMsm%o2*wp&fdnd&vF9HqY{6a36f*Wxa>#p zyP#;I-|;FyE7|~IQwv_Y_K!p}01@Z6nbW6hOLLw+-T!CRY((E`WNRD#Xc>0e#@Gek z-}#?IUtPUA+ECN{$=LYnYngX>^R3`56~4(p_Qn)3Q_@upm)4&xHYe zM*;mtuGs@x`5pzyv*1EIadgmXpP(xdcp(+k8}EwAj0E5t^#X2ByNEGN@I|Q2rWOry zM>t~SN}_gzrgB+eW$O{m7IX<4XFSvf+O;4hOD4Iu9vQ(OuthSI6YnmvK_4`x9YvxA zWYFE?HEGHN61KoGi^Lz!vXLGJw7a(u<`FgMh5lQ}-#FsAg~DiDA@tsH(FZY})~{tO5(G&SfZ**UFp=q8$l?yt z*p|JcyO1Bs!e_G+jmjsPnOWGXf2WHT6AEZoXwebP?NZM1Kh-@OE|<0PhkpsPkFQ*aAh>;~GcQ-s*WCQx9R1nu|t z*Rp&@5?18DzI}3@JUU&o&6(D6MiB7(M8oF zjQmk8Yhh$kQj%0OiOiOH+tesP{pyvgBR{_NeU3)PcF2atS4C(dd%6A#bpC5pv_j%O zc(6JhsqX!HzHYI7ueevozU~}&=uihS(g(z-J2tV$rRz6|hFKipKtz_)bdNrWw-cMl+YhQF8kc3?rnm z53nclr4tsRjECIVz=JE$kZt-gOPu^sCqWx+Et%yfm#)eHswx2`ofOYVlOK;Gzldxa z(ZCOluLaf+8hV~xKau3mp(u*lJ@*s@#%EE<@$8u~uIUz3Zpvw!3XiON`}XZE-|f;K zt_I_~tST|?fAnFdWpGrW_UJ#+GvQOjgl=Y>iX#`qoI!6beHGsV)U{`t8MWWpIrkXB zs06X*4rVAgAZmeJt#W!}qmu8}i;tf=wIk)xz`czeyNaD^(nZYQ0MH&?QhWpW49Ty3 zBl^xut>>(0kT1sx>+_=;^!^u9rIQ)XB??xgI@>R7kBS;Vc}A(_fQpFZTMe?<#N1JW zjOh=s7SJg-Iys{SBQUHDWZ`Qi-45oV!w1KPzYtUg)ntn__|FtrFV!=l5=|}20VC=~h?BU= zpI<(IM(=ACzPIaMiehHS_&amUqATN7H;a5k0SCl6e?eN`eu%w_Z-Xa~eIb=|B>t&u z%tHH>jas^?`x#RkF4>{32EbMcKw5T#2%mLhd{7vU#c@j*1{n zB~$YPw;dxjh7~GS2jBY(TvZ%E86pzgMXWRvT|iyMDZ^-^Wzj>^04+J~g{qTxcVnbJ zA{<=jw}_X$efzeFx%plCt!<>{jZN*WJiipK=>$3&o~_`#J8j>wQpKcP_RO@Pn!4DN86kip+vLly=;T1RIVMsOUJZ{2)lVa`8zNm1p3`c`rloV%ohX8AU8OmdOz5FW}po8 za!=<>7}}DDqlzQei2QntWp}8~Thd8c&m6f8Sxdz{0b^z&r!xk43fZrnvdIsQ6v{Yz z#($SYKu@1r9-2u@mj@>E7Si^v;FwMdyBbus!Z+gM>Z(}#jE+dA?!0~-xC#6$W`W)ymzyU3{6CgSf_cfFEgi>Y!TWsid&K2nZ?fS5fE4vIPHZp|Xs?cM^)71d_J9GR?%0)xu15IrAXU&rV*#Ogk0EiE?coB+}~GamPu_hTehsQsuY>B76PAaI5bJ{|mkQ^|Mp+ zUOf}vGBVLrqar4AG&i6rBTE{kb(=Wv<3*XBDpp=OC>%m+j`vX%JiT^<8qR*e=7xJl zd~fw(4MHaH7L7S~Fqp*7_#J~7dohr+(<^(%zlN^=*nvBu#0C|qW+hov8p@Ny7m4Au z4n4`2;X`HI$FJ&CxtT7nwXomK>(_1L*FkBqB7K^fReE0x-ytJ9;OxN@I6EY#LaZ$c z#52n_3OfixreL6c?l4+sP?p+Vpa#@4(oA!91c_uQ=zCe7GCP@sl(XMdT_P zW%2^w4d=;q?tX>u#CC88U-xbZJKGXvu$!@{s|)KkP!VK29PU=473xv9UIUKX#8=o? zc}4C=A78S}>gMJnQ`Aid10=aI871!8xcqKyWMXu*(06f<{cXiP4fUqTY$I~@_SOjz zIRfJ)YqrZcfOkW@^EsF^_2S5HPuvrwhFdFUFzMEJZ1@Qr3_Ga_G>mrgNtDF6Y&7n1 zuJw30qW``RM>Bjjb1xqV2xt(7hy?*{rv791B)m?(FJ!nsj9iDtEP2JF5ENycbNZV% z-NYbT;G6YopL)s5r{+fNHz^058u(X(7yAIe1z@P5Tal6n99V$fRr=?HbC>3U8~%Sc z##kz=uAi$qb?jJzD&BOhGj4}$pD$N&MT%nNd3+VdCT%W;?o*;nuY(jUe4Vf!37X<9 zz^bh%e$zs`z=_`xRaW(8T-iX6CEK*L(TRH%(2_*hna8Itn>lCBS-bOX=M$KDGO(}# zm+=l4_cJ6CIa``s49G2HD5&Xdpsfd_wU3J@s;j*KDR>GU9uywnDotYgHS50p=nZ

U_ilmf9B5_BA-%$8kgbHsghyG%q4H;d3&cu3xq!4 z@c%;*PZuRhlm8A6P)k~BC2pJ6@q0j_(3oG$&2|Pl5W;=jQ?tM2K;sS~DUZKm7*0Dz zzF8d(V+xc^KlSa#{*CJA8x(+>XI)Ek6INj{=n#ZJXu&va_l!qHq&bjsrS_E-P;Q(73ktTOZTVi0V>EHBaj7 z0ZQt}!^34J7Y-aaFt-vNW{Y%FJ)~>j1Z*`R)BbB7#s~zuCvz<8_^|5Xv1o39)bbbI>S|<@#ymffr+A zr7!#t26lx!8R^om_{HU^{i;6j$+uGSGp;lgtg{58w}4)a%35_Eu4NjSjFdXz_xI~Y zmBh^XYbv(**TSOL*G<-4WDif{md|hcxCzqhq}Z!tkumZ~zLOxf1vSfG&lgtseB|j9 z@D7T#%?*-#@18)sM{e8=1U^n6ARj;4mlVFBXkqsfw>g$NYLH95Or-ho@H`w3i-VQ4ex=}6kcyi68Yb&t+AtNeEczvEqM3df}%}z4bhR zaNOv`Jo|f^|0Z0xu_DdbFA`hHvo(1N?dM$8;=?;`@L7D1q+dHGr{^6T`U7FoLUfb0Njyrd1of`%~*Y0sWLV&tm4!EO(0t{%|O zsej(h5xD0|1kfp=??!hl>;@v_vS8PS`D$6)T>zB;C&Qz-o~Ghy40U_^rf^!bGy7jV_B>HI$5e z;L<(YgKdfZND7vUk%-L09838eQhR7xvLp+zF)=zbMyB97R61=AN2hXxuhq}_VM3sh zwmiEuS7&di7 z_8e3tUbCwR9h5J_&nj?cn#J3UdVTx;{T*;DkHt%uo?YcJ$6MtH)fskM(o&b{Sc{BHyQ?LYVABwmOpaaq6>_Iy&(VQbY?_%v2l3G4P!4~)(kj{NcG=;?_g>| zeirSFN-D0)y}yYM$6i1Zx3C7L);Gx>0v&7QI%4nS@Gj6xQjf~FxEeM`R8Qt;4RF4R z1EXW)Nm|12IN_M<_PJ_r=Mj=TL&3D`TzHRkQ~AwZLK|*1ZSiRoC zMeGf_oMGs;-W?Duzm{ z$s1LlaLMmBr42mgJn%i7@**W!2=9RxFRiVoRdc~a5SrT4iMkXNan)(Ze299{sK9Z- z_lY4$f>7gJyj0mWv6m`ud=;$dt@M;0XWHn#BIR1f_WbjOmRtlGW22~6R z5OM{1&|4itaq7+(wZ=V-ESU8g!CiL&8ql?X_GMq2k3NQau{%NrLE-Zr?gWmLWvjq@ zCB^1RvsN;*N5pBc1800$%NgkdDDVzeiRN;x8ilCPKI8x z-+Z@z)%#?KXf!iT#cVGT9s(JO5afK`8*xAKgbiGrY#2uiAJRPSAzFy7A#YhDMy@oS z4?ul8Fu`(fi?mt_Y@}it;(T?yo|V57@|ZisnQF-`OU>t3&)c>aYI zLxIIy>7z%rj``w(>=1wtt!0`ZkMV33g&plBKxiVKV(;g>RaElRCY-1rw*yP*~S#V?Uy9=$KbYiC%ikH(nss z;w4y*@Htji57^^4YNDNyKCs6rU_3uY;b~^Ed~AARr%b;SaMHr9K9|PAeo8YFkpG~0 zN3;vpb1v@%Q_QYR_^+2RjfHLL<`$EW05EN@9cEY5MbCbHxr|02WLW)}%I^jSr!Lku z{yFUI(6w2u=gr;zNXyA_TY%a5W5?R|(>`vqVB7J^GjYegPe$C3PbfWe+);Pjlb$oi z&6qx}iK&SVy1GNQn>`Yic`REnEB&Rz!w2qTa-M$KAg}s;cvYB#Ip<^f%f88F)EvU+ z7}~4os@~FPDl7|09-r7`@?`h#_4UwAviPM4vZCWZ&R8U!SY~+8Y=58L@!dM^nINq) zeyAKI`1fne~?2LttG*0>De<-t-tF(k@S|cmqJJ zx?%pIEvPn5P(Z`_j?(CF<<0pE&nrlHo1B(mDNGP@^KUzoh-Udq7!ZxB1w}tLbRN0d zL>iLAveBJ0eYzT=S>xES!mes;)_~ii!o=-t;=+N)jCZvz@8J#xKHuhPdyRHkV8ea& zLr%mi-RDf7jPPWaIdj$Cub;-aQx z)y!C1Q@NG?9ZA6N+H1>Sjnqg|5m6RvTKPWD%k#0j=omWEw4e3;oehIVi(DAXyW&zz z+}wUl%TC(fkSf3hXR8pfr$X9EA=npeDaIMaOG@Q?M51ldLm8>UxZsCrHm9VW;>=!6 zohr*bg?WJ1p^&UJbO%i55}dGPCP*FklGr^PZj;OT=r9J z>Z}>3qQ|^YpW{^<0@nAPe!K~lg-%dC7t5HOdSV`zqN0UkuYTwD`L!Y3s?0B zkZV7ZRv+#)6?$eneT)1nJNe=*S%nb;iY%M(RSzN2q$x5OF$A5$UHF8e z$y#DA@0%qrq)%nL>(0u2)R*K&YYSQf4Rs5!x^YZM;D*>8JDS_9@cHSMAg=0Zub@nE zKv=xhNli;rTiR1ZWpg@MMsQL=e@@k{2Svj6t|o8!xB2hjb*$Uh4Tr(RG=f9ZXtkn& zqdAo5fKmYxean)(yY7^M3jeynB7Bvy8bNTMl`Wk&Em zC0EhO>h_m!K?_bOz%H+Wjk!W@gFDTNn-ZX0`11NXzh?8j%ntM?lp3!4?};%HQQq`> z!|Vemh^s=pR?WCzH6q4-1DuiF)_P+zvt6Q7(`472Of_Uur6w*En`#ENxPMIEZK_|M z@CTI=Vh-WUR0*!;36UyQ66sM#$veQ{BeO8P!9F|P2&HmWN}qexwFgtde`b&IAY*m= zo^Q;!P%oczCQVIYcb8Ebq^!L)-DaIiOVK+Bi)l@v)9209#qw`0NQ&QRy`yN|j0_j_ z|9fmwcCnF3=hJKut_}I+;erDv#yRqCGnwrOLS2$^$E#Vk7tff0FsQ;LzUbC%+lK$t z7ly4v7Yd)0UqoHT2*(M#+i&Gz2wf~&_h0u>Si>{6A8Q@CbnM*o8Vxm6*w`}bCTqvO zees_bz`C(S5^ME%>2KZn*D4NuSJUR(>+rV$m~c$C{TR3X#YYvB_c^)it@~~zJgP=s zEpx)6l=m2toHQ!DD;(mS zryNC#u6U!TiG>qpG$9{($KwY;x zY1Sgv^=VqCiM(ENH6UX~9m6S>+T@8DSaYJf>g?(e=zgIH!69vhS^EuYuL<*^3VR>9 zxe@{z^4$yRAM&UJaW?{vkF#eh1G;h*A%a@#rc>n>&AnL8i(p)9Q7O6AaX%857Z? z!GdrV_0_MW;CEAKD_O7Vy;Nm%hbRufN`H`+FB%A`q)U3^q@aiUS(F4lgK3gap2wcu|qD_6EoXRwTZt1B&M zMb0qR{S78V?gp>gcbs+9{DVI0Gg3{NJISCu2XDhmNh?n4HN5t$AEU257yD79?xxL- zjX7Fy!%oOC+yO&+D2ak0FIV%_!V^++O2)>6f`d!@;~i}?zQzbe+?HYdBxwM*yDZe69vH7j+C zzp!Rg-5vEhiYYN;YDyYI*0k$T&W)y3vofzaFizpb@2W zfJJ;vSciW{u{RamRQWwd@RYb4tlS&U(-8h7^`|Y63md=--NS2-`c0)S%%4xTdGfE% z-n|;#1j3FOWxH@W^Ee|Idh&IvZkkswlN+Q*^)VDf0BjQyluq}l#$moHy7sid7en)AidR??21XB#-# zxFN9rVV?^_<6FE9WNzv7ir42HKD>LkDEb`d-uqwJPPqLv+h;>qAoI&Vk2bAQ|*l^_E+|WX=KWci|w)`dsLf>lx#G1YSY!&CS zd5{iq(Zrl0^$2XZAOUK8n?QtmOCN$R`Ymc4KfXr!HE zLSqGTv~{m3_L+UI*R!wtRQH8WcFLHIRd`>o>D#ao$ByL)`PcR^Z7?|*F>m>;6O!9q z7T;d#5{6UsG`vfNB@YQnw|R2&`PEgYm(&roB1W2ti!oOwOXB8Dj>UudKyltdJbj{$ zS-t!1+l2x<($$wYG8t;Rd)Kafn{hIwO8CLp=JrLr-8mWYCV08b%<^x~KE|!&Ye!Q- zUBIhC*wi9yMhU0%c9BFWUbYJKyf%5l*S)n~T&F3qxDn3eZssJimR<%U;}PY{Via(M zNv;kWH0TOD8kR;8))_GR^td66(WsBA(jUXLL-^M9?_$J^t1}zKQ93#V9AP&k=Z^&* z5B;lDo}HbODgF2j0xTsqM9(p)P2&!`S-^^o8>f_AUiPALW@dmm0O^Q_{QSup9?yTp z^UKb^$O*h_^Mv}^1g)o5t9c~7bae|n*Y8FLwisKYBu@w@5ZVs-Dh?SzO4SFR=>1?Vw%aZ8f^Gpt zySYET?XaE3-m2Fk1x=chm)C&^*UC-jmJagtGqgHVYs05)TD)p#6L_khQTU}VgnNilSq#w`Oo_NKo zE(;&4^Z_qB5<&S_?1H18t}7Q}oYbHE{KrB?brQ>kdS{m{SZHhF7XEb)L#82f#R^4o zu&R0RJ;tR6TU7RqN=Q;{lcjv!qu#J#OV;1{W2-s7m>50`-ytcpeP})e6splhh=p}8 z841aW{3QY{>&CyD1>|hO*mp;isYjXa6{D2kR}LK;UBnkQA^G}IHX=K*&&-G|i|*|3 z@w>RWb^`{G*tZpa_)%r$4v05It@4Nn;q?OwQ2r*=io_vh8P*PIKT(HY8OiNCs2Xxa5W@-{rwAtsfhiW3^cof}t*L`(Ld zB+sfQ!_`DT-OSt^@N1?~e!^yEOzPBaT)nzN*>BM18n>C}ET*sdQBhJd(BhfEfesv~ z@~hum^?g1i;1jLIKZ@UU1GDZq$bs{(Z_s($@k?V>*z97IaB^99i6r!Av0xl6EndMj zK)Mw)>gmFvpMq?|ICfr~lij{eXXamzx6m){JEyMv^u>!q#O~X@r{Q`C%VyGILT}|T z(>bj;NmV-OtI&@+t9%;T?^8XU=p^0Wpf9>5i>(M7*8S5d{_Et_2OOzwC%-SD;%3wQ zE1;de26WARH{0qkWBpQ2dF(?O7Bm`XsG+BkEIgVXP5zS0?fysmI?%#U)be8n@8x3{sTBWtlwK?!zwDxVc(9aNre%4$>RD*;W4@BuMEGeTKZD#wQl-WK@kYcn z6x{CN5|BC7Z1npdgp^9Vj@f)T@Z%scgC^!Hzf3fSNtfpKE4~c%cHATTqQlhT!-so+ zfV2%Uo-LbMDy6 z{_6#vhMTrAzMB`fRMU5g-^rXWrCdk5K8OKs&z{v38Oy@M(PPSA0@B4^(U@@h$vZUg z>gmHOB+X?2I z2U$$(hC2_Folo1{$%` zqvkUAXM5+v3=#-_;J)7}AeZ2AZtGIdu}X!Y`{Eq2rRgTb`&UeTC4*uSiW=EkMLI0@ zuA3Z{gWT6j`hx8=)G2%5{mrw^&z14w2*Ou4Y}kkfJa1vH z@if(UtXhL!;&u}+rIP!7qrC0dLLp7-0{>P+x;Jsh?D{ro+_z?FJybUNl_Tq;xV~|(E=2qLJCYi8xf{cuZ`k1G)?Rlh+zj$;kFPgtad#D`lK>p#sk?BI9eM0bGQo^V|*9~T|6g|-5%O@jHeC>SG8P^!4wX@xKrYV;zd z1O4+XcdNM31Zc}_P5(+ucRwUKPVT}#c5jD9#TXPzDY20LOD=^}`FQI-72F3|IP$Mt z*|E1YdZNbH{f$V%i-y5>vPAcW>G5pI91&zuTQk-cT%u((*Q5L^(l&*Tdz1(;eD5)} zbOG2d`Fgr*&kbAl+>?iLBJ*lP*+IRRb0&A=i%Iq)Ao zvv|oDi8aeGZs{mJy|%!Qx@9ZSEb2Z+2L-Cuv;lcDE=z30-Dj?$EV){A=2-AXwJhI_ zBuA-+uIGK!SC2YkzLORpVdYwC*qQUE?wPQi!?lC7Z|~lpVlwuM_s&#{?Ypx8Q3E&Z zCV?QHlvKIFRy}i*q`bqOo>Osy+w|W?hAy55x?yt zs#^Fh_TG{GQHl#+_d14g(1fM|8@lcmQB2-NrZhjvu|%mKq1Dg{nm=vYEycbCaXHp) z(mxJ8dUK@v&uELal&YYx66Muc0qD|u8!mTuH}EB##8&#>VVZN!atKPSU49qw767%f z5jD|39A8D>w{L%NaG#+0Cnl&;NP=LD*S$_ci0(k2n2Mf09Fc9(3AOF4(>_c2C-rWM z#?>##MU-n)=;XWr+bBS&LM+_1{r^r?0PqR8!IH~FD7HrRFi{#?RL)y*<781HUB|u$J%2vSz z2#b2L_rIr9u;VR%^Z@rkede3_32s?hzVZ4TuTOvc?|3D^y}q!Ru2D5!D=c#Kc*&3J z+ozbC%)kLVXzf>vm>Tv#&4*0ps^6w=P&G7XJLk3C-PVPAMu>MMd9(+|!&ZgvID7W2 zMWv{BXsk?|h-6lg2|H?s`kuQ!Cbj&9!;G+1;u3d%yo#e0xFz-oUN*V&o(XrBj%Bm;88ITS*Lz{$9eHGLGr;ZC=Kt-s3p8iwi+4#D z_gj#7xK+z13ka?DwWD4lC7Lr6mtdcO7m@$!P?gD>#t-B#+ND!|*>5!^;Qv02oVWMe zCfzqQ5+i4*G(8<2VzlWS?7=j%6U8Usd4D-+__Vy-$_@+6~x|}wO}er+wFFn$Qr4CGVnd^l2UpUL*-2~W<^$wpb2~Lvt{yU zgWnER`Ao<5)5L znAK%25p-6&^naQ+-@+O0JK1(CLI9a7kuf}`x+!!?GS~G{TSo5hH*dCX$kHQyPJ@dI zL#!+odS12cM#u=Embo-43;~7218yNwGIaJg-KMwb?s6`I63-Lt3N(q|vv zzu(UG>U(WWfbtKTZvO#h$}=Ko^vH7f0zydHuzv}2F~^m2_E#PFU4f29%vnraxt!y1 z^6^lPZvfySCbL0HxR&b=+#HhqTZ3+pGbcFPYQK~7{?O2eZ5sKwu3nvteMPq}@iQPP zUev_Y_2(&{4E&}2nM_U<*d~3B(>MRq)m?P$i0IU)S6RSFI1v^Uv?6s!uFQ6>z(y*6 zBUt2g48QO`sQ>)nlv&zoabc)pEEYC3-5eE1^n~wCCBzU7wUX|eCr<(c5AyWdA)BBP zozSTsl*FJpCBTGRf7MAX`3S6v8Mfy@E6VQ<{Wad+{*=jbtbLa7!Cth>tw1o;)0SsP zs3|*d1?}h=Fz@E(O&Sb)($JXeu8KXnU(hCIJedzZUg-XfbJ)k0fQ&;;H~&LNdWYPZ z{?ZXoj@K(#vwJrsaSnPK=LG&6KWzVKTpen}op zo}Sy{qC&d}`}=+u3HhYao>yV83w0iMhjHwDE2|O=*kvhZw#UxQKHn!`BR;_fnk6)X zy(76gBWK>H6s`e$(ZiL`9Tg!_y5eGIn_MpX6*_WFOK=ZK9`zA?*?XzL$f&4>e6G@k zczQPu3UV+$-3D#oEY6SHREq67EajN_ndwn?=Xe_hK~_ty(zDsR_;|MsI(V>G(0oRt zYvd~SkV?G^AjbBj^iXI$&;Jwnt;T|Rpc64)5gE9YsgK?%QURqo$T4j--cRoi0hLvQ zrujs%m`$G&%Mc6U6*HIam%IBf0)RrLU?UgJtnR+KZ|dvUzxyA`xnYo){+N!mD!$0; z2~juOv4zLf;Pva3I2D7mO&$kxq4MLTOU`-F8Przi8nsj#9iraDto^0PZVL3g@BHd( zNt(F1@-+;S>hsj;!5BMk$BHdGvGGmRX`Q|%ma=*d6SD*yA87OEGqoqF#Y`8p~5$;>%j)n80E(aAVP^^YJd}b z_0MWa)&%~V9DL|dn(%pLwJ{wUkLGI)DQfM~XQ#)hO`0C(G^>e{(ykAaUVk~h7Hpn2 zC(xn}zQk>>R1!N))DNG3a_$xU5+LIL_GL%duO-jU9g-~GM1qe^MrrCLD>zQ+Wk%Pd zWRNEbu}r`V%t3)0ZiT3T#%bfDlD%}e9&TY zg$nhn;8x^cDJM#X{8SDd&_nZyh_lCbv^0B@t|&58JK4L2jW4QZeD(re5n18hjayr0 zdkk7x@FugChx8b5t|13b{P7YV`}WhPPg~Sc1JkPXBOKw#Kd@Bo(n&}dc=*VXc5eox zd8uX|9&txA+zX_yKc}%p9ge_V%rFBN>a#HQ)MkY`x9`Vrs9^FKa5x7TG4Kri{Gi-b zX~Z^oxL4Vy7W$ELVHX&3I$(-Vu1aut9#f_hw7-Sz=`iH3R^f}Al>QyjMBHs`k`GZX z9KAvHbWdlTJWd9mQfv^4@7rf9FYm?1g*k4MzJl1@1?OZM=NdL0d$}P@Wb|~Rh)w#3 z+&cTO=!e!4&}^~L>qk-6c-@U6n~^&}H}-;#`Qcl9-sCc-XbfLZD=-sF!t{H7d-qOY zaAGSs1$okc_v8s!SCPsfS?m}$*S#!F$E~7BO*zUHm0c2t?6D`GWMwCV?aGv$Boez< zj1}uU`&{d^@8sQb>ayrkVtZ(&Xg@*x?TV0pJIwB`52ArXZLNTc=eC~EDVRdpHu8IJ^ps&Grt`2jiN zGEnLkKm(l;JHR?2wM`@2uz8ZV6yTcI4^&irytJbdw@b+_`4x~!vgyrqJ;<)3$(-@j z@bZ!@ql&8MoNEMU+DTNw@7jo&gOM3;n@@8inHF@5XFF%H( z9|voGXk)MwqS&XvY;%8zI5D6px8t6Vb=9+tIKlGTKWQEw)#$uMOj5q#RF3||NLXyD zMNSZH$cTog51W;B8RNp9W(4M#Ec)AnoGD`g?i#ICvyKHf9;K1*1*EZ-cqqq$G8*?6 zp{z)8CfUAN=IrdIz*$_uUFm(~396rmiu1J6LIQpoZr$v_nDT)12~+AZ2KET*f^So# ztYMOovyEkt-1Y8%V0WyHp`Tzu)Kk^!m;Jqvii)OX#R!^86RP zdiip5R1`9IwJ5hP6c(-LUR(oIR{SC(Nc+LPYMcLMLnzl-dnu6%(2PT=Ma_({i01`` zzaCMv&}!3!1EX4v+CTU&(?A^EwU;(~M-6}l*Cy9hxZ?|->~<(-|AY?zX#oiL;@R)Y{I;o`9>mQ$Az4G4D zyAX={!tfr<`BCYR9FE8_x4(aiPR8pIB?(hLJ8Ul3pmJAJRb34fS>|&{0S;;Fqw#$j zbeI9n!1V9=cWY0gze}F|^ASjJK^0)NLFdkS25;|RJ#Mpi_5eVG!A&i#4-75)^N>MA z_!G4#8(4Nc();2L9m%slsXw+oD-dRlZgAyS-!v9P-1-KVB-4JkKmM?Y89Vet|Mv8| zFLmuff~~(F9JPBbrs5N=yD}7N#wb8)K%+*DPR)6?XX}p8(63n=II!dDsQV`W?ScB6 zdOI^G$0oIuj2uEsqwQ`3j#+S&b#v>-5-+k(d@n0orccmV07i!-eX^l{`s#WC>9?)y zFOOwd{STD@r2s6E>tqE$jUeWSU2FJ5m~UlRX;epG0Fn9$0qgN}B@b-IkQjt-^!yaE zfQDM9r%$MB4IG86a2?Z03lX=*jVY`FpYFX_l84iNUQ5!7$_>w=|J%=*|CE83;pTy8 z=#tak`Y4MC3e5MNIC=E>F!=rIQZuP`~7+?l*;AO$!pDKivpdhYy|~-7g9X!j_}w!~tpcz0rorna*lqR&*d2dlMBUBw?E)r?|z9Z<@GJL+nsm(DtVMs_5i$vTqE z(EpOmIKDKlNn=w>fq!I@x6kQu^#JqmgAt!RAnL8PmOis(GBN5Cfvaq_x|*>M6@=F# z!iw*juG^fy4^XJye@>SgOd3v0DO2;S?}5xSp|IwJUF zx7i&6M?n=Rdh1KE(d94OQ=xSX?0m?M=xT;}M`{&RFeS!u$UicU^@rkV!_Kqnqs4zH zOH@6oOg%26*YAl(?tj|v%~Q%i0%wy~CnjIMqeoe&z`W?j}FPGXTJE&3z zQi7D2Xm{rIpa$h%4bz?|tY(q3vVe$g4+NoqS#+|pqGH|&bv3u+faNZ;#4;f*i#+e7 z^Di4{LT}KRixeJt7D1tS?sKkk`lbY*0n*KBEwD$T|v} z6N>fgTkqAKq>JDODsz-+AfMGXZG2q8bkwuPJ8H4_FD8gYk21Ma7nxCsd>_=3OB@v} z)-Oav=Sz`(%Z=zJ>wfYl)1`Z|${^00@e}bJ?0e0o@!|n(K}f=daM^g5LmdA#rTkJZ zIjv`@MaYdcUB={+x(vtmBFgFobBGr!zR9-%wRTQ z$3qiOn@l+eh#xm|%u4+3MD*pBfH?djOHde5(-ZCD+A4eGHY``^zU3J_TKuQGJnI6+ zy*9M&+Z?px?a-RNKrC(v?Kd(@g^tA4qXT$m5ZfT$>2URYp9EAx`@ei}TY?Qya=x4! zqf73aC__?w{dwxSK@2A(=>J!!n2bBH;bn`Jay1wlUG09Xq@Ke=z`Pgu4@0XO4PL@s zg4kuh9%M#4z$8yn=KAp|T32LqI_@H_K26#0(dl1-se%IkeRa`hW=gX1P=Eu_PjuY-%q^L39r|`;@9O~*B~wYau0yR~5;WB0<(DtbaV<@) zLN8d1*3Z;j4o}1Ut5?-4yJb3DX6R$pkh+C2#3LUM&OaU&yU06OspE_7RwxeNqVXlo zcO5W>y*hsP57S5;afq#tA8bxpR%WIQ!R~%D6IVI~dIlCzbY(XA`qrCs@ioh7Jyt=s zC(dUD*vX{5V-AWj4?5U2j9AYzh_b(Uzwq{_U$^#{&+xj_`}F@v(d08%@J3CT`8Z~% zVnG$!=ad@%hb3&P?aeTetOldQ{mID7Jfw@jyrHH} zgb2X#CMd7!eAj-D#se21)Ozs8dq*ZcI8=Ei;P--a=gxU>F%T~oXXFN^O_+SzZ@b+R zUi3Nl-BC2;9@wcOEMZ6nkE?U+_i!r|mrMM`_gDPLM(sI2PGS;N8{F>1-cJ^?+o&Br z&;||vRaYjB$4iNGbnx9SYV{n*>4!#+qO}*RWFFgE8s^qhTXTx77f^skfxZNd`k&5{ zk>68xvdWaGm1{7mUs59@e(}7Ln{Bms!6??ZWWK6tRcjF1f-2cjLUONaV_)8vXe!1j zIqnar^?F9kl2Z})8^V{(?!7(?#+`ejc_8>ZLp-m2oWf|1yfI0L^A`KX8%cG@Ryw*o z=d?(Y*TpaYZCCa2K-l$iw0gZ5=39LW56$9Nyli?C*MS&pPm>>&koLpgVxXb@{{4I3 zAAbh08c|l;kx->QoOV0^H_Xj^=mHsPBM;>#97-}S20{OJ zXJq%?0iy?5SU8{T`k%!10ZU9D$NeS~3hw0gZ2hr9KiSJ@KK-@=V#|`SF&)uO<> z5tG;!s$u=u4GE8zoL_Q8B!Smo){fp|u9oII|3!MeQA`sePwN|T9xYBV0)P(2#`cS% z!Hpx1R1UR1VE@l<2Rs5I`10Ca>GpXZxl}qgzWC=)ODy!0O_))iC{03-cKLpIe-UD1 z%)7$S??eoK3kYP(-LpGWzM@(ABHg}oOwV+C_LWyBBeN&ZD{W)EpRKF!T z;QrAG{2>&tGm{_hquMKrLBDMrTE`!T+MNbj?!lC1PF`DvWm|VNdBH{Cs4Efbm5z*` zdFu1#S;cEX6BVuUdipH1&LmAiV2O%j(tj6g_6&IM%<}5m6h+ zT*WGw0%md||Jej`D;3{69q`6XYOsE>`z((=cm+<&U|CXyE0tTz8=Jzy)cjR*qo0CH z;>uzx1gGGHos2tl$TB?Bt4o)NMiKud!gSp}vqFg-5u097aboM#Q*ClmxX6<>= z>Ng;qdi)vu6WcfQA;!jyd2O#3=LE{)G+7qFA%*J8({CQ~{pF|CE-af+tYW-iG+^WS6fRgxB38po?sFF` zHZe8L7WW8A^_vRW)p(KB#>tOsMF`Q?rwQYlyjr;FnOlw}0v~wbt0xl*r0pA9uZ@$X zyzi{O1>Qd}y7jvC>-S!-?k2Aibl4$fd{>m@JGTap8c)%28xY-PR>Xx17u@dMd;Yu~ zGe*e|wIORy9!g^6a)l0jRVMZD^esP(!G}N!aFUS~jg!iI<@lAndKLfAY({a`0amow z7$yEpfGbw6t~#`&*S+>5!xL*W%-gJ^N+~%1s#IWQp zS5&ez9)ZIG4$I@!GI4$p#%ijP@6QM<K)5Ug>*^;qji^r~vOBa#TcJlq=n`ZyMZA zgoIZgf0jIet|mYI3MYf}L_BS}yz{OH++I>!G1Ib6W}K?{!=)?shfg=x72qI1OpmD; z_I&D{%bSwvW{Q@V-UvtHfmTSi*4o|7%p6^wY~8*+=kz?Hj%N11EaD1qMcWyt`rvc{ zU-i)I@o<8p>RRIVO9G|#(r8(KDfKlaq1m9O+kW?4byl0Lc++cV_>7JqQUmpZ1Zuz11&` zKY=oY$q0&@hXlD&8-MNY?$>>fZ33ICPnIR?*{7NzswIdIx{LNbp<23KuaP+?KEyb`R*kG9+$(7 zvq+JWZd?k}IeUUl>UHkduP!NonT+yXC4Lm}ej>?MYNng*pS5pja1$+Yzs||^XZ9Qd z&D`bIG4U&(F^hU>+V& ztNr|c9=>AUytWq|57WC!6x$Msyk2#4Juanu&nw=-&f;YfzDYw@H(WRa=#@?``X6&XPBJb@@Y!|!$3GtR`rgCfP7rQJl<(Bq0hR=z1KbE(&Xyw6wK93kXwj_g*EhpuFTjvj= zpqS-Z9oP#yd200eYd*FV;++A$7Kr;rS(tog^n&7A#G0(5cDt8KTpq^Z5R=m}0S4-& zFESKm#VvymUf$w$3!!4#C#;YC25gO)JTpD8W5uO*wDXF<9GWX1-qE~w*lnC?CDn=P zx49#>8Lg?y^e0iCNtsVtH~rH~gi2yJgN}7gP7jSOCHs-heZRCvACQU<8TMx8>50)6 z%kLbjoIZ1A1kWsD6Cw5M3(jD)bH&%p))d}W$Pd)owL4l-!C|NyDixNSM{j;5*wGLv zjyKoq9pz$OA>v!Q>L=sKy~AcReP_|4PL$2-iQ0_OKx=_Yd=87wO;Y2f3HKeabqIxB zm+`3Z4Sg5B9VJa{^5I~oW@9*I!MxO0^!~~FTGMWeJ3l&v(I?uko5)#qE66d&x=xwe zy$!DG4I4I0J~WhpSdl|=a$;e4b&w-=R@$xAQQNn=FRC+NmvR3J*jBctB=FS!friw8 z?ZC7k(GbIWjVRc8#i`Lh&sBhiI$0MLJZ%aMX^hj z^@>qN$LdROF>InF*N}Dwkk2L;=c)nJtjp95;`02So=dIJ8M(t$1X_sA^VZ5uxWcSV zEGP-9op_x3efvI`_4Bi++W9q1i5T?>q3@xZ_wLy{3|B$`7e^+E0q*X6()D3TLyI6C z_p$BhzXxo8Q0aUq;CGqSy9VCOY3oqsX9U)K*V}qd=@g5*M6jDW(-I|;!^&5<*=$*g zHVg}#Lvt?BNiMI8{a)A9DY(_5jHf@p*q@lW0mRc}@K5sV=55>7k&@#1n-)5^i~uu` zjag2ge#qqPF`xxT%mhZ=D{GOCpYHv2(-ia~8PxGwlb)^)H64fDo}2|4{V)?NIHKP1 zhYb`CxnKHcdJQ=EZ2*pwbITVo*nt^@2U6>3=S{^dqu+6fkIo)|f=@*egS%p+MDFWA zdABH~$Ort@Q^6A-y33bg-JmoZif(}SP z*Ro|T*|-Rkq{DJ~d((%q_ORTX?k@42!Gi8D|Hl;g2Hv3emY@84&lu4oetEQS06u9m zYfTNqb>4i#xL?*)kEjsm(T4?KKn9BJ^lXp^@h?**fXP$SPfxVTU9@zm?8J4hTzS|w zK7(Jc^?Ma{{;TPC5*iDf%+-rSIhK5A4Uvs%LjtLYZm*wq>D|R8S=0CWt~IFtjr#a5 zT4YWXlxjo(2&PGF`!|C75f5|F<6QW~{;l$7U8*X(W?Q5*ZqB^&e#BwnG$Gpx&UnH> z_!fR*N7vw8edq2o@E(n@Uc5*}56weOdb{!)>IAWPrv&%nmRl^}eF*4DrVCPS_V2g8 z@@r}NtmbX{ealeq- zhSu-42MhfG<(BgCJ(&wn{@tc_8pU`nD3@0CptEc{a=ED2$>0ZrVVSUXIeWK`-QBaH zAcKSa`&`}GeeS)LH-Q^*t(Nwp4R-`p!mKmH&}f_%obd6f#z?2o!)J4$!#KTrKh-28 zB)otK&ESw)=ee;iqo8Pl#U{eRhBp1x6=WPu1sOku(Qq{Bqc|MY5Tqa-d@0-P5xEU7Qlp$0u1HTctPhQC%CmYkzK^^+z+Yc!1 z-IKG&9xe%4zg`xQFT<LuQkKoW6l(8OYFqSuZTS{x`XUYR3B3-ts9Rj+%cd_#ir|V2cS{ zX7Bi#F~St_%iyyrDl5I=+LI^eTRI`4lerDC-=q4lE!u~Cyse19)6Q71j8TS1!GTVl zyS0PdoW+cicpmSw1JTJpyz7p^bD7aw`nBEF9bK;}Vm~53310lBckIh;+TH{GL)_?9 z=DN*Ln-m=GbDdefS0~vcz(gu)Kc)FIZw~60SjgBs6K)BMMmcc2l#0l-N98`B>)hDeJ4zyzlq;i6lc@) zWb!oWLv?gQ(SjNEbccZA@k>)Ae$%p&!%@4Kt043N8j)}?-sG3=b}P&%@-nGL#CR%x zP99Olk-n;(s>X3dvUCAD>4E#FN)zjVs;u9@fn6@&U*~>oID<)$7%~IRp7*Clz2{>N zj(~jBnf{sk-6l=)ruhM6bDkBsjzJtTniq19$QJ^o-|A9xs(9*x+W5vHPWW`!XU2Hn z^w4)xe*gJnQnq_JiB|SdVRZ(V&=}j2oS$Y4Q;VbGtXZZAMLad1#j z(1){sp}it3(qy^3z8r##WslFeQ6r^zAUTtN)Hy@p8(Q&l2~Of&_U`Y5!ms~n0iIvF zU^7Lr3=19R(z3!B1yWee6qd^zF)$@uUg*?`sV-AZIVo75Gj4`<$Ux1@gbPt znEW$ykFNARe0>?L|Ipg7X3Rfk1so~Sbe|k}U&0y-3ejrHx;4WQOw=Pet&2PTDEER` z#nyC8Al_FnHgJcGUG4en88#kkk%+}0ESj~sqB}8hlfad4j$wxmSY}#27Z5HODh-Q? zX3c`(6{aW##@&7YjVZ#oz}lK`KY#iLS@+Uv+qUZtnER~yU1FA%zI@rYZ!Jh`a>d^< z*~5OoLQAeBBThA2XvDTcK|-h}kLJI38GvmS0zYB7|V5jJI-#NY@hNNuUwkR57HgA}%@J+4TwQG)q0k=X=!X)cwv{sHe zOVtT%Eb=AFFBOfbrb&*#z_ME!H)jh*Q;o#UIv@rLr&!1OOA97cGte z<586%bDv-27>)7ZD2SVxn5*&qS|1r|pnw*?Asz>P_P)Z2W9nh|OA||Pm0|_K=9Fwm zp$$KyGsxh_rDmM<(4jP^^sm4EGGlv(_!@L>Y4B%)m^REhOjH&i0Gibh;o@3MfTHFj zC6t7Pzl}15CfnvTWP*g-Rhu}JL7u3Iv++G7^KqoL0a|52A!^SIqayDi_SA^H2!WKH zF>r}e3RCDo8?4#g{pqZYOWB14hPj$q6oHB5;7iFd+;LE@B!0sc_1G0pSG zigEADBZptn2)c?tj(ybIkAXV?xFu9V?8qyOT++;_S1%31aPP;xV}OEoi5<_o7QB3? zH%oG|vv;6Is(H5Q(NBk3cU4h(SYpSTrUWH?qP|wxi`(1>xCsp z?9Q-#g$^dupi2VpIqo;|%eBM-hQK$R*+fAIhLtTp?CA5jSJ0|9l6|1soGw z0a{?U?N1(%hbR?vp(I-BrBF3OE@dEN^kCe6N8SYvI_?p|1ELyQ{!&EECl@pHv-RKN*KF2*A zQf*ROn(Bu?3k^$cAFiwDXgFQP(LPlz)oZP-%Apv8+?cg%R~Oer6gZ_?s;?9N{$u0y#AeGF#yhf~qb((!LA)x`4Ru-N% zMrRGce${#|-*nDwx}TpHwUpE}lOK<@x1V9;{0@&l-byAOvETrz5jl7)hOAkWv2eEK zguniZ%t+r!-5i+@~G~~)niF_1R znK)=O^{Uz=`1XSb>(89&^Wnn>9LXYa(qWHL?1>ZVls?0tk7@1BcM5yA^p5+22W+g9WgR%43{h^59TcSQ{KoKl#_ijxkZbdnE?v)6 zklh2g=PD`&mAH4A%p%LOvIRgZjmSdQJYUpKvT?H$_G`{o@Gf{rDTotS*ST|B zgjpMW_gceN3L~&@kY*`EFtWRLsv3Z9xAyWKr^CaWa2E|II22@|FAqp;=Y^Ys!yNU? z8OVA#&a3!VN#8;p=6ayChq-w+HIeg@w&#=Q(;|XhOZwY=#vcnmEUfetj?XL344Gzm z$Fxnwr&bPpHjGPNYk%ibvy*JgR!9I{qq481*kAJ=Jh#xRCTy^g3><3rzCnWq;zUS; za`EXOhwpm=5Pq!{taDT+IEBO;jv8$be4`}S=_YGw4f9}D^lkY%tTWW@w|8yGX3d)I*0OJ~nga0^ zNm(Kr%1IZK62bc*Dm(9Nc1!?)*Yl61j6z*~C4tyc_hSzT-ipl4UY?FS>rbJBXcnwa z^MQ=60Tlq({qmg)_P9izBX5YX`t_SP#b{#k5M964{YU{PU$Fk_cd`x!Chh2DIv zddYxNJ2b~@cbdNz$l;cps(lFdqefn!c&fN%weqUnxN#ah^Kqd|a|trkQh(0Y?gcGl z%D-N$gL2!pZKuzeAv5Kod=iY11*7{GDWg5vIVma|^kM$pMOxc2REI6z-nNLd+`}td zIq_MC;3bO6w%Hjj zR4S(~N8>7^xeEf7wr$yBaJJ|J7uc-u-aQYUJ8tZv$!^QKxoYg)8ULTs|Y5$y&-mlj>#Wy?aw8cPliY zGMn|VhjN4ZLVzWO`>|(C6I>{lV=q4S`__eOj+#8VO}oJX1nIPpNE7D_VUAY*S+qNP zsiAt<^(IrJ%G8BS$#)q#Nm{bEYSJ&&`QWaCV0%9+r97=JFPm< zMsWOY7ZiA=*OLxU)@_dJTZ$w^TWtHtl~mt{$}hblEy+>#Mi(n;mUZJg&|W$XWXYF; zqA6c;qIkA`u(~eOPG4-)8`iH!1;3uru6&=MBrZ%kD>4Bm0Kl0g zWA%vo2S%xb%&l$xxx>_LEnEFWtb<(c{ViGFHq6Os%Tw(@K~b+vT)A?PII<;v_)eVM z-S1xl$)J^7Mp3BZa+-fyFR+{HE0je1oAm#sMb$9t+l7U}XU;6iy5NoTyqt9QOt5od z9{4DoO+%T%wyOTwMLR--AKcQ!J+14nQ4p0a${Gh(*F&3EI{$dtHu);EH2<YqOl&;XL3dJ21YnZ|e7t(cB#8J_i7c9-f%ml>IJA$5(E9d*OP*0Ma)= z!b67u_Z`3bK_7e7w|kG4+Y=|(+jpFv9rZ$qE)IGws7Q8pRqLrp{R39zh#S`Vf90tTT8}w6SpJ9Zfh{9)1Y3jm^Yrs;0$lj+!w0Jg6Iv=MDREAQ zva0*T$B*?CupzO@ z$Jj|QY|j{i0Nv38rom$)0)8x0Sn&xp>pWJ0VRRS>BUIZ4THP%lT@gp zYaXhxk;&c{1nDM=2mP9cx_uPF+9=a6H(J$~L*!&j4aW)$`Cn?jeDjpkB}00UlCC`=`gPqN$ti-5?C_Az1MRqx?2jleu&@nPLjGcECox z9@Z_57*vu0xLS9y5|S*76_yHP;G@n4G4j+r;i*zb6{s8k9i`O(}>(v!xD zR7V1py;+NHovN{?;Noa*Kk?NoweAW+7GG)6q_+v58BAHdYD0P&M_=eLyZV>R=|ay7-v-`T2>1D=G*=^VWvyVcUp44M|lh3=)ae zTRz72Ha5Xdk+JlYz#YSw0`(cMKOfXYR6m_ntGhsz^8Fk-&5ooYrM5q};zzPoZXCO( zs7iC-*2P!7epsZnzN!b{t<&6T4CKj*QTB&u`F>iwc(D=m1f=%V=V#!Gu16RSJ_#a9 z*{P35fY=Ot_&tf}g3wUCgYAaZkakTClT%3=BR}iT;djxcP1`*6I!lcJDz-l^#o=f^ z63JklnLMQH`RH#m5a|% z&3Ry+kB)H|OU^U%-jk-!Eh{x3z%;_UUE5Ax$7xL02PJ-7xMIZsI;mY2@@nLVI%A%` z>@2c0KLItKg}X>np6Ew@e=q5^`(zEvYe&BoSN`kxgmR;VbcT{7ojiZpEvpsqpicae`D zIs4p8|DH8ZBx={L{AsU890gCa!^LmmA6soGFP=LCwEz-DakD+YDUE$R95GSJ ze`DF!3lIO_3k;**fs)br;ff5=f-@xz2q^{+}q98VX#AIXDz^N(5~ zH<}eAL5yxm(L2L^3!@e78A=G_pP0{ntch+M45Rgse7|nJdX4kT8-J=~@CSMkMbf#` zY_Xzt2u<~yF{3^D0osb5a|d!9K%-d6rHi0DX5RN0vYK?;u$-PeS_DX_U5u)$me99d zTRH)*fQmw7oB)6I(rW&sWahjj(>=sko0{ashY#Uo(S53fSG(CzrxT1T#qs|&@2~FX zN~oniW6KSAs;xHIE+C}Hpve0?;Day_$?`AH_S&x;O3W3tH*-~majPFhpx4R-lT&Tg z>WYWOK?+}(-JN?UZ?T$*u{`5brDFY=sID>6EXpuS!Uh&31+l&oAr@`@qd)PN_)sK$EfN z(vuC@s@0VYIb*o790+sx}?qBO;2+h-bMpBo62JW1(myF4-fo8HoRz@GVCR^N| z@UprH4+XiV59Nq}Q}X$wHM$0V0rbF3dKt)?2;teen3<4DnsEx!y-WAU`QkZw5?y+J zrASt%P0R?hx|K*R!V;DpGxm~J_~uZ{?Zh#FE^GJc>hkyFNW$$qbWr5oi{k;BXk?1l zuIP6Jj6)i7`1%(|j??BHJ3?5Zw)>QuiwnR*H(6s1alKlsqOk5JuU_r_vYXZ=gy}*RcmSf9TigmErO0g(=oyG7Lv&x|;cu5K!9@O8O;K~qv1VS|XHxD@( z3ydcZuMKQwI9k@z?Rwdk&L)pK^7Wl1@RHmVkt~tmV@O8Sx(gz5q|^nE_U3lW=#qP2 z^v48Ku4{|WcmnL}%> zC~v_xhJtJamFmg9Es7{Tuhd-``oRYh6Q!cLcK8ytO_I&zV5|15kLh~yCy)D{?@d1C z69RevAs4&r_wKC)?7lWQxM5JNzR&Wn?XNAgC`h60&O|tJn^~FDk%Eu&>J_0;#)MAj z;d@<2fOCOgl+=1l?GBS_R(B^1a=B1a9lm5Q=jW-;M9=D&YF-T3oCgf#mqtps|3n91Y=NB*YQ`7Ib$%xKRi+*}vR#Mhaa^Y;3IDHip?y zWvjGqz1P;i_%QWQ$BrF=DM7%S%a#M$Gh?5pksCIALuwj-?wsq4ti_3-RVW)aqV^qH20CyS^uz}g6nZR#8Cg}9OCx*YM~+-EZ&|uHsp$Kq4yY>guW}83%)C5nMpKTWxNs*|u#Yb5C}&YncFZYDRLf8b5wV z<}ZToUeE-OOx|Ha=)X|%RQOI7X^Eq+-Po}c!UxmJS0(E2hn|*o(v;BYFj#=n;2nJy zEneJSvq|fkuhXddzj2^g3RDd7wXCYb48{05VAvM`Cph5hqVcKoquElzIcdcBz8dA{ z_K}q&BEt9YtRiM^lp#xG{8D~cevgVM$A(yTfi|mcPCX7; zavMhj;L5Cf_cW=?VGV^Sx_x`KgON3u)IN*lYybIY0N>eM^D%3jvZ?$NFWX&Ab#ryq z@87@GUl*3Rgnl(q;8EPx3xCWZY7INg#&=tI6E*@V@#TjE+@InMNkh%ITAl!J#i(1i zTynk`FMO{*=qzE7sfLk$c@uJOeBE{fB}9kQAcx1N-D($XyhT|7X5bv2^JsvL9<0L1 z-+(ldKKEABMk+cA2w6D3>3n>utnM!TQi)7_j4agC4wTzu_!lC(dGh@-k&}$4j6mtU zyM!xGriFP7=zt+|Z~wD+rmL%KdUp2fMD0c_R(%D~Eo{9ISF+DTHrIpGE?*$GeMNKh zn=HRrO^yn|UB*@TeX-P3&kyVfd}p76v1oe;7yaQ5^8OrG%Q$TOjtm$t*o2OB`Uq{&j3Ud8l2#b8=FhOsQtj9A5n0NTW99R zqjJhMsM`gc-~=WcLQxb?z;SwZ@vl8ZS=S9#8wEWkaM1FZTtCnNovkdm-rdGv%O2o+ zT7;@MUqPHjfs>fn5y;&WpnTAvL5P|a6}Sxau#z3g&au#W7^Xy#;*2txQ}w;57f)r*$w!11C736m*y z62=>{u22=%E0q(`0CQuK^J-xfWx=|gp?a{+u^l_KrA?Fe)c8;3*jr7>BgG#+5K4yq z3pMi5Z{NL}#ZX`=Yt`alanq{GnnGs<>*254qT#aleHj9*c)fM(i;o{udaQiRLM>pN z(w+W`xB&bW-rc)*@6yxL6(~hSIPOS*5>9yhc!4U1RF`W|TxX4z?kd#%4m9odpZ(o{ zCNDgl$pw6|E{R(7KdYW;#udQoT8KP4JsOJ@oAJ6%lJX1;0hn0wR<0tas-3 zo@QnlfP$VgXI}G~e5*Z!XH4UGrH(e(VgfBE4$zn#0!jH;^1baktzdTy*CIVPHzdPB zgH}EhOFP}46FU_8vi6iLnod23VsENtO%~0Xn5qm`q&z5z3$6TJ{Ws$|J^#NWzd%GT zyz)%&9UQBtTD}WDJ?oJ=FReZGQ7aV#r60G;{qINnul80lPj0%DoYf ze)XDmv&Wi$4}kRo8LpC%2i^vPkgkeledSjo`~)jbxj{tu^7PxrbteV~&N3w}FxY5} zcTjqZVI8Pst$ckuiA0jhFL|m8O!09#8|2$j8f&-hVC^X#e!@gUGiG1t9{pNQa*J+F zxJ)^-aDt(_CytuXxmf@Qg*7KHGL^$1ww<0{3yO86J(}YY zG$Z`SNvo;St06bFcw@no6@PwxJ!U@hrVoA2n2K*%ElQDro9S2^Y9N>p^o2q$6&ZjM zUK{V0QrzX=xM4#>S{E+Ow_vM)mY_Z)LX!u5$wM-k$LO3DCGFNiWsoE3@#!9W==j-k zWa(Ok<={1MKM=Fk6wRU+Si4{K1*Rr0og*U{+YCW^D9YCbX}x&v#&A&TEVH3@_f^Gf3Z+8^p3-Y#Cddp{`KL!)*SuJ~_THDN?C{W0 z?qBxpTRK#P=d@{g9;(&r**=fY!BD@vUu9)po3*Ux%{%VADGed9#mYa-o%fNNBaK98;iK3WGUtyS~WEs+!;a>r4ACBef|zr_6QAR#fpAZsLxHJ zAq9g^>Jo17kQsDK(%DpTFk~l%rMoPm<%-ElfMC8Y4qwqqkD&n7Bb)v;%e&K z!+kAyA>lQZ^QZJmPZT=D=V|({Ils+hppJOiN+m8(hM$n(PBzIy&!7+ersLDF6v5lb z#aX%ZjxiAH#ksTn{KBb<=W8hTz7s#Z1xKx}`?g+3UN%`=d+pk_|9cD?f>1`HDtm!A zU5A;6$9djxe-$`g%5vk=rnk#C@C``C_r9!LMoIn+s+t|_s9gt&k9)5j)Pt0{O()0qdcN!>mIcZN;rTwJ7HRYif&iO=7oX2l2 z{MqZkr-UEPze5ugUS6GSun^Bj#!}&2BH*o)3m-xft)bG^*Vk9zW=E@ZCs#F>08f2d zI9QuXK@Kcq6AE-@W11UmX^Kuk{4R)UJ|GF9HR~<)1-=J4lwJ^r&xLxqE%9-Yy?&?W z7P%Y$>7pJcOY6H|uBeZOD;u(6cXmqw(0Ls)*CG6jr84R%!Gb|E?hV$4X6+YH(Suqy ze;VLrxf8P%2hCS@?b5~j|7(`sEN8lfsbh#-&mP-u6W#G^)8Cn?4B*aujjy`9130DN zW5B9ZM!oc{4VOw0%N1;;?B`4;KDkG!x^xo+KUTNziZH4T{$JtIsnE;0ya*G%R4VW} zV{FU2i{0dD?-sh|c1QYVjWd^dA;!WJRy{i!ic}<(yEu4~a|fmYDwrx*-j%~_o$waR zug%6&mu!{_jXl4Ve@_`AiQYbC~1U{D7#4KXp-gNe|7^ItY#v@!F4 zhskf|%v2zz#P`Qe9P27xK#MM(6r9lD^zDnw7hbHbsjSN}+a%D?WC~G0XqCnF3qFn{ z(1;4R)0_TO=FP><1<;fNzlkhbX9%>D;!vl8#E~rnuq;){UN{0^S0*q41$ye-iltw@ z*xW^7$BrG~Rf^oJbaIhBbi0tOr(dxap=W-R17sVgK{fFFjxH{Vo4w~Uxx{}STv4%X zOoQG9xPCLgxTR^JB%^x|nPlOB(Z-Leld1sxMy*9FA_%B#P^D-}%GuRXy|GZP+{ubR ze->n&8d5;r%#@U*4gN{;(+Z-Y7+gpQr=JaWF!H%G6?!lI!Go4$4VhpX5&0QdRunvh zGezD;r@hWjv_pvtp2AC>e*Tn=KyH3tgBT48(fBo1DmH$TCyH5<{+BU!n#}keHi;pa zk82#wmXXEs@^afDa}JLAyP|ordcnPW>sZq%d^idx^nJx2&nM-Hx9LnJvHO5l9X~3uyrviWZ%Aa+<1tvP`ZhL zKkpCvE$sf?fL7*L-u{ykT3M@G`F3xJkAAh1^C#ZS5@QU0f z!pI~wi**R)Qi(2wo8W7y=|KJ#L)qxTD|IQ`Z6F9WQ(5B#bu73d3iqO3vvy4UQcD5z|Kvax%!QB;ViOXqrxp#-ghxas({1v()kA}nh8n7y zmDa{_I7EVv?_PrXtH$i{1lF5}UK1Me@75!({th_Fe0MY^&uH~W4BA1V`Sjv)4YJib zK#Z*;O>O|&ndHqbx_oJ3-oME(a@AzILj-^eUe+|@>oU3_S=i)zJ9j8qL4$-=dGe&! z6%&1y)tlt~jRjWI0bSZt4Iw#{AzvzdA@t}jwlA@un87kB6CtcTYDpkhoCFdBmIr}( zNqeEJqT+mIKB4{vGrJtPb>u>gvDQtl<>xDt--e^YWfn4LwJ@!UoG(IRWAdX(zymAby#2ywr%+yrU*sf{zW+7;ZsZRz7Vfsg6Xv{E9O$;?nJ) zQ0EBR+sie7X4dtdhKo^+$$k@5#n(KlA_-eE80y!$_n`rNOmXAV=rqcshvtiu&(F@C z0;Q`YCA?**0^e(jU)~p_z>0^N4Jhw1@ozI!BtXJm1ss$(;qDNE-Nn=0#+V6UlSg8 zTiB{GlF!9rl;TPHj1m1qFpwywT;aG|^I$zaJkqmG6xLWOAiCvLHR3mqIGw?A=Jrw{ zKgFJgH@+S;Mq`BEP~G4M;uOG?(xH{Juuaje-o+&|Rj$%28YT_{y#2woRSOd*PkM=% z4SekBKZ~16cx3UFMv8&FbNY`FZ*<=ZlfT;K#lDrmSfa6^y%}iSs6GrC*aAH7-{&}DSri` z#;|eYwq79%Cfxz|goPI#5CB4=l+1WzYcOKdKqDjN{Q+f-IYcsQ(r@6v6rcIoY2MSP z%c4tkeevKt@|STEKoK)ylNd!yxgZ{wlz;$4-x^qUw=vR`ia->c5%B8k8GSbA|}(`NMym z=GIevKMCD;_1ZP9!OhY0WD(rtnT-kG`5MZeV3znU1tGskNO{bFqdk^)pTTEEO|7LU z_C_O$83RH!=J>%3{jynq7;z81-Q9}EB>h$T|b`2Ba%{T-y~fF|$H?_+qxei>mgg&gs!LdxyM_;7z#WA=xMfi?q|jB4Ms zk?8W_<1kR`BxALVvj;yvJ9PVunKQGPFF_Gw%OD%@*P%sUW2A&e-D``+S%jt(ZemETJpC$8g262?8OL38EB-;>kHahALT7${-G$CeV6SFD3)7EPK1yQ?Xp zy`c->l}8M@%$$T^9GH09002N>quMxnzkgq7h)tVX$n-d>SaFk(3J-S$7q0-OiDZA3 z1BxfK8e+n6c9x$x=%w)6gywblKFbjI`+fDqjZzBv@Q4VZub@5akTlz+LbQFD%LE0Q zx?9;qJo_;1Vf02U;kpji4wp2H#4Z3JJy2F(cU#haD<+T7=rA!YAWIr;Bt@Md?a3m^EQTktRfjmydW$g-@#!vbRld1ANm3 zx}YW(4V|LFH2yaP+H~=ZW?c2#;1g=NUxLl3FxD~oV&V%J=#HnoZ?U$P(qZamEvD6u zyr853#!hOJ78h4v402&XwFYY=d6X6i`@L|H7*S?L>))a+)8T{4AE>9OsF?WvEU%P4 zrcSoZ*OB4at-qt0XH-*Hug@h%m?C{e=(e>zHRslL$98lmqKcODFp!X+ONLPVkQ=>J z4adRukmETKVnHLtptNKtmgB8{_T_QFB+F)rf7aS|qGQ5J685zbg0*zadBRB7cAr;9 zwqyrLh-lUcTVg!!-1mw;}yqy8j~Gydd%RF6qD&e)S=}XC!^wdI8K@*mp^0nFjR`1u zHw8|n?74b6nYxEhzUmldWArf-DywHD#1cx8zu=FfESeY)CS(Fl^;VWgP5v?nt!HB% zhzx3RmVr7)oW2=e^A~E_-+vW^;-Y$GSs#N`*TX+cN7>m~@(ep?EqrJG%;p`i7}7)8+a zIfl=C4@n^d0D{gUYPr{EzYL^wKCNHx^P~0OZ*!*g?w6 zVIZoM6{rvp!DzS4=RR-47ad=RGGOOekJ0=WaU+ar0<5WuH$+B?z(>h?=;c6(MTn6l zUQ4ABRJLXQ1~xfx<^(qVzFz$hGfZGHiA$>Rw%I#$ya(qq-ZpZ}hXekY_>6EgRIB*h z-w~2XrcLp5VInAE>uWu}618f5^5D1+JKx;6af5{;>j|W$_#~m1^;%FHVr?Kem$+4+ zTj;4zTuWHA>i3(L+ja8zZXV!Y>;5xKHf|4!z<6*T zv!7@)ntcwNW_iH8%`3B31+QPck$7 zs7;W@N`%LcMiGG~++(NVqoo2N?a_O4DDxFk>J1unDdgjO=EXdVGlRugi<&8j)b^78 zUkW!8nfv)8RQ((;HGnakmDkcjgP9~cxke}SodV|bf)Y~ZsOT5PA6O121}?j`9|oX% zX-J)9$y1j21p!}I=ouJnKS&<~=gOUnL%AnCh!AI@_eYACO*!!my;t1vlu;PVbz}@w%5C`~3^AcdD<--w=#o&v z$E3)hZp?!D0eqw9e6jN8NOk1~5E(&Cbq!c#_~Uu#|{Mg^X&nZlXCpQ912Dl;#eKv&AzDs34E7YRs}_3T+&4rM0Dh!j5ZxVM-UGYL2tbKMTJ!oubY|# zj1r@No^z{R6ZEwsgrP<35W~EZ$&XCgJ`_bwj;@kIDk-2F?T%0fO z^mLbhzillQ+2^C)>|))uwB_z*0)fEbi0PCdJT- z=@ekpPpr!7Bp$&#Z5oFkV&GAfKNvCP`@{bkIQd~f<`Oo4+s>UQ|2IfQ`i*Kb{qZSd z480iN3x+^{0Y*=mI~1m!qDNHTXumoz2j%z}xH_25lKXSlVOhrG$GY&Nqe9w9`Ny#a zoF>Uz&X{3L#0~+j02Nc0cGb=8QJ2#Mto58VRtxCTQ>;UrDGJKohJKcY>UF6aVnZgG zK=9&`9?DJ@=_UBU6S_UdUcU*&CbFSE*SlYyM46>P1PYPN05*qZeHIg5^O(Ml9^|hr zo<8gonsPoeCPts)PR2NCuVj9lVOTd!$I&k1$BVOzQ~#f!xGry3x~(uHZt~wjqKWc` z&28T*O1&*sRq1GHX))_*O}QfcJ>>bGJ-YYq-?y4HsWohzOgWLk2szxhe}{OKTywsB zZQG{J3rvkUH|XDv#->|S1mhPtkFBCim-d;sm$2WgD2T+$7J!E03+&b2+PY$P_qGg| za~;2$>BiIjI zrX;H~ar*!bj4yax^}Uxq3#J9mwo&7Y&EXpy+E!W0QmnmKP`zwXe;!+xhpK-YxQ%Rz z9+9qG@j27ALIFbz`*^rp!lr>-qE$$~)3lf+3wZ2?3wh0@H5qu5SmF61eT~Gv*PWbt zAVA??oCk;llz$pBx=S0G6p(72>bij(ic-F;ql%Lq!j}s6YaC~DoJ5YPdA}h^UsB<$ z5j?D02M8PieFa#h)@t9o_x7QV3KY2TtHa31&GiOvKyEbn*T_d7eDF2Wf$hQ}qXo}M ziSu$&;T!TTCfA7x(+H_#XL>kGzu9uDc;XZOfu6amB<8ay5yZPj%B-#>b^!09O8`aA zq(z>Z$ME$g*e{`!_OJZeik`aWYU?p3DZoGs88s#BzV&TITuo3X-z2KP5^Et?mfa2n zjJh8!?^&}{=rUv)J+Pt@{y?|Sp2Nw>eny#0C^UU>)=FPN_xX+6_wR@NI|+aAU?WO; zir$$4a}p!Z<{`SdXMzzDUxlmX9cKhpdkGY$Khd*v+ahg+Q7}USYFlu`4KmE9b@?$ zH;%~)q!~gx)~v+z@^?0a-O7SY=O6z|LVgUP|j~hzu7z z@BXQEVbV<0epKVletF)iQM~PNXe9qxY>(`dmHy;KBr`GGEQ-F@)jDbgP06{BFk(9~17~yQiQ41r(c@*syEYt~?7%Bh8*MBj!CTKfLVa_hby- z(ZV?LH_^qRpt00fwVX6*@5in7iPcYf#Z3G9$ATfa^ZV-!s{H!sLOXixIzXpM34N2} z)m263<_GmOnXyd6<=1L`V#UXOgit^Qz)uTp@y65|p zUnkSzq?u4UTucBbOwDECP%o;!$ch8-68*i)?M zF}S4C;_KFhW9F1pXIXFSIAu|R&HRjpEv=_cO)T?P(0>JPVcg-ZonKgA1qB65Cz@ef zPtW3dX~l-SJJO5OS*ri2v}iMcsp86O&Fj5z=wf6PN)di+p?w(O(Yca+{&yDqT>Rj# zmZ%ALs3cf3Q_*H+dBLhNtrZmVr_28=ZQsj}CRdolXt{j(a_xFzyxbaQeglx&L?wk* z2MOa7+<)Pi)YU@4ZtJWr3JR-l^8nHhEzda-zZ;wd+P3(h``FTfe}m~_XW~Y zXE0*S^(wYt&bFv7zsv1wS^MW$tu9k@zjj5n2j*`H@-*t&BZ>$!#%xA@ks;o@D{97# z9i&Cve)r>;BKw!rYpZV6wjpe_s+`P7*2`sBDZb*vZ%8@%4~lFPb(hT*^I<{eBY?L- z^@}gE#nJO`_iWe;?(jYpGo(XDk8T8_=)nyt`9JKv_g~Ka|Nnp3^R%)hA=xynC}pLL zQnC_J5t)^hP}WIANmg1EErhI$3TY6DNJ7q(j6zeizSl=f_*`D!f8c%maMqdQcplI3 zc-+VBe!Ji9`-V&G9mV)?vq))zh<5U<0`V%jjh1A4C$+-ahL1d<7dEI@`SuB7m4_&(dOw}umSZG7fF{MG7ln=<+vJ76#; zK-GTx@eu1cEimZZ9l@i9 zvRhyFCFWF2wn;mlTCx}t%1BStGsF(vn4wBUwU@&yu=?9Wx6oeRL|)=7O__6QN|zoo z7ObbbJVx=d-~Hjsm;L2MPUTt;lG%FZu7?jBqVghM8sl*-`S(AsgqH}|H{$09pPh{* zec!+SERnpGT?EKLaAAtgw)_&B8AWWHD$WDWo)k_kz&E8mVR09bLC3m?i-JQ^dnPC9;QZeM+IeOXU1iLcgr+Liqf`|O@WZ_kc!beD$iBOCI|7+o@_!Np@|2>u?a=EYHb zpI`9|#ri-dD2+&ZbdPhZ$crqaI+a-8&EHeEdW{Fa`1C`H6*zvUNj>Ju6 z7kQQc@VW*)(Dt=y@~p7jLq&P&<$17<#?lr&X`y~yqE~?n9z7iWW%q9Qk#nsEyAb!J zmK9!ox2@SAt&|mJY40WhwLpzU9$=A?<-K+}G`U2ROr#+Kw9l^myPdj8Fkox4EY%l|i@ipA@Ba2p9LzGEr~hkGDVCva@#si_acEc&JI_?rXXC z`-?}jTYTDcLz+WTtnL=87w60o%gZSUwo}tlD@cR_qc3e8r?}M4gGK@J@rOi5by8g%HT>tZ zU=zT@T^TRm$9r{SV(v!w)pwdG0wfFlMwgpyZjI6t3xRbGp??%! zpd&67FuYaREEp8_bFwI;LBF*q<(645OsY_n}t+(qP|g(f)B$?dw8E6aIF zd1-37$6@Qiq3JFozFd1Zpkw7f$o9B?1iJ`TZs*RO9UcvHClirccb`kb1fvWmeNinU> zy(uqB6E^IhJSbF-hRt{E69@R<{pW8kc(wtC($kHi0VwBL@Rp%C$#+;MA=lC0sD~vE z&xCwck?O;>kEf4|+h4H*@)IVSLg`t0<2C&X_vke5Dvl416A6NTOK;u3_a8poSLNA+ z543zR@M zgOortg0O5j5dTcye3f<`I(!}6WmI0AvM~5q32FL9b8a{4Ccfq*i4;{|krr!DMhW)g zQ&Wonzy)F9jV;06=w$hnQ|E0fs$u&8&Df)p+^rv}h_f6|&WRiC#eFgbF=*4yX$RU4)?9Xd4WTqZ+}v_O%u|-3*_jR6S z3+dDW91BRoUV!Qe3m2Yf)glDv76nQvxDu8$0Y{66Oz~YjUDSBT7|fa#O~7WQk>Sv- z-SN!^^OZ376$V8{bp(3mZAA!A4RCU!7Fr?=?K*M@R=2b9&nHKE^hg6y*~gag4+`1~ z=tm)B4>thD$7FhQ`_0E%$E(6#3gvNjW&tarY+rHcZlhHpbGE1QF{$-`rs7+mRO}al zj>s_f?b`>A6;l503q&?2RHQ%J-vO+u0M)Zt!q3Q^`j4#sUCQ_9{IOo4Ib?G_4eDh|}ChcUZ^!iREYaaGpsPD0UdZkX>#$yrJO` zJv&sK^xC!Ac4t6v@}aTYgL6ql19s@_*aEPqrO}dt@{?SRUhdVLZ&i*b1CK)3pqebqNPY= zRP&NE1Z~0pEJ~b$CEiwjeg`0TwF@sR2!3i+ILF*$(e0`a*!gl=N%2C7aZFu}J$<#S zPk~BJg5lh`J`|*U7SkE-v7q{fx>Hl5s5A$I_3chG;aRSlRUXY=J@-ax>A9qMC|z=Y zpBN}`)WiIko=)STYOSt5oGZng=v&|496ub=VW45#=FaLhe6l3Tu5^~+BM-bM0qw6foD9uRGfp^@#vEsg zc8{7nCUH0%(940+<(=xAr3mY5`?ZY2bHOkE8UCsjJMnVOB?=qf+8P_!?62cIfAQkz z0qaE;KYCUje3OevMOH9;LoKB`q#Z4U$&^g?0~R;=>j*PpdmXT@c?z<9-HsoBw|~?Z z@~x|{*UhC}Hm=$`j2^?%;)>#weDb={n9YGeCd)|)^er?xz!wQ ziLl_%ReUqD=tUgy(f8nhPL&^ctGfi*a?(4F#_sw1XhVk&H_uv*rUB}{>$+DN3Fiv} ze<9AC`XD*==ku!+P9Il4gR&j;;Vw>8y?pud-$%=)Kv#5)t{=E#Ge}RVjvc zw4X?2Zq>mf{BWiB{cT`V4K(=kDbB>sX~th=lh<6;0Z$lHmppDkv1`__@>`3VW6>V? z+lbs1=(p7XiSO>;&xlBmGlEZ8b4DZBHePhu z66YQjwzcZ&w9Zi0ar6wna6vDo>Q83wzyM?h+~iqCmJT{B|r`gf$)fDW;-&ePkT}t)i zrK2ZJ)`pL(Ps~-|4Hrlt@E@>$CR$ovs6O1Lf8oY}@!!7|3aCBE=+Cp$r`{A5e~m#N zW2zUEqHuap8?;k?knCKqK9aOOv{y&>AnwSuKW~<=oU0t9(UC7xQg;4Rc~%;lV)TFs z{SPbzAUa31%qCYo#Ss!7Nhg&gRxKO9uZzIosy;Q6b$Sm%#lX7zHRuIC{J7brt5F(Kc-iOhD+?+QWM9uUAiQ2VksL-r zfBY|7>O50y9tJ?ntsgOFBSZ7j4$Yc>VfT=t>{WKhcdH=&P}p&(g=<8eVa=O-!^23l z$B!|nJm!aIP*mEg7u{}&(Zxfxqe6JG+N{*Ogb8+Z{|r3Revrgh>q@)I4~hRekB0`N z)Vt~r`E3v|-|+HLSur^X{a`hAyIvv{%>&kF$+3iH_X3Glo!?fHOU+9E&=tl`Yhi8; z9}S{iZ@P}kz3L4=dKb=man^E5Cv_Xzx1Cno81=Y(<%+zCPR&vC*MUkHQTxFXechBf zN;IZcKcpgQ=Dd|UdYlanof9E(uKU~W@bz_VB!8-To61YFk4q#pRR0mH`($0&k?@j5 zHOFwD&#)Xvj}_R7zfY#uDLKWy)>Ti@`IvaCfT3~PFr-YG^me#;k4Pgn%9n6!mo>~w8FNT9_z({DiG<)&qUlFNw{lDCfvSG zrvZ#8Z1ZG^$kx%S+&iK<%)Ybqi7YjFe~V(Jk8${xZQn;Ut^DNUWg-JoFMFqcIwOk& zA{wN!#>dDKEHBF6gv$MLO8G?L`_I?$ZdA)NH=6pfrG0e58w(2Q4sKUTB1p*Vx2!z0 zMzFvU{j#&>uDUgp_T5$0p0E+}FWui|rd(cjX+=&T31^6$4y&MvDA!; z|L{Gej(4Wj+3L3v5%?xELEz+jqK|eXfBjkf<^Un09RS+Z-9rR!Gs}4M(07@PVxGZ> zA&s|7#YBe5A9@L>55%c`ory=G+|IW?TeG-0gqAz$<@kp|wbr0Gvc2yIhW;6sq;+V2 zpdo3a!%=Gp9JDCOt!|Ue_Bz!|qIPoeFZ*I28Gu(xyvdFq+ejSX*K4C0tf2SxB`tpU zl9{lpbP{huVrcuvRXy&>3EI~?RqK@O$0s9K!FuX#@t%81YW3So?a9lRZo5?u$<@_} zuZaqIhpZmnKx()(vE18QkC1<{m(iYNBLq`T37zUTvX}YIMx7)u^$_+)8|w*UtCuE85H1Ex_2)fW926J@mAxK|8S8ef_~k;O z3q4uO2UYX$D`HlY(1V`$3}V>nT?Btr;CkSItg=RKQc+*Q5wqwdbCJy-ok zX;)M?z^Jt2zAl|LV}^j`*p`j@rc`Zv@jfjVYI*{nl^UAIuCLyo?|#~*kCTljCEf&AYyZd6SpAsD;_L9 zapI44(&#LCnqJ=Xq@-qDB^!UY-^!wH#o%8lMd3eXl9RVAuYx+OPkY1$Tqx%y%6uaw z`S$Ba{aRD92k6!9NIf*}L#I{O{`*%1)H|xekm}Fp$FSa{hJFQs-}&dskMB2V(0;q0 zes=d?{18}Rl9BaPGxOpD3#&g|`P19nWu{9JwJUM{5u-eR{j2lr-kuvwN^*~c$HgRX zDGc!vpc=OPw^#n`CzIL`Z3&&wZH?@=|CV@+9|blnXj+|=b&+zQXRZh6=>VhOjv$fk zC6>o7PUw%Zbbblxp9E;cM-sL9uY5CQFl#EaoEN`uSZh$7TuHm zppb*YO0!pe5BwYkNN-9osL0H(?!QvNiX-+-A}PIg3=0x*dwRA17XQ*s8Lnhe+4t~n z{b!!p4MVP5LXrsCqQ2Ud7Z?3Z3YP&Jyz|F8dm_MIhvPPc%734h76M=lc2ko~+^s zBDseefDFBpdx+j6rF2zWr~Y`fzQu#fbP$g|hpu=Li$fZ2{66+bonK40;`qD96Aq*; z9z^+D9Q+L6#mZ&X-;BAe7Xo(x(VVF_i_-SyMAU0J9*x_VE0<9bxBPEs2T>72QcDrg zt^Ti1Rs1Gx)l&7F2869XddgqP^(dvtZjy{>T-yn~m0gQ~Df>cb9wNZ@Q%I$yU5J(u z*k}Ca=g8Zc0(Rz@+ZCEyzOsy-z{%2!qzZO8p!4tFH{y(d_E{=Z;%~-?cgt)}I4p zV@=jykN2fi5!x~4=gOZO*>A zef$urU)KnJm94Iz8-j|ET}M{7 z)ozwiLQp0`6RW}V?mwLT?&l@^iVJ=iVp4osB!*e*$oU#_@lY}K85_Csp>_CG!<$=4!4D8Xp# zKfdyMUsBKY(MMgo&a_mNw6>g@TKwjF+AHh$ zeUwHUH*80~Tm^UCFNB zaF#y~B)bC0&6Iiw^EU&F;-S=3q)jU`VFK+F)jGkeppJ}37}!nHqF!wi|Hq*sx+6vq zMu})H)QIumSLi+{zF}6&23nB>gTemMQ8n9j?f@|rH9{C4@TsFkoxbKNquSo(tm$ke{Ahc^{;<-Zu38u945i_ z|FNXYoo?CgdlabpxZ$R8yT>Z@X|#P({d;xW2Pw5}y?18xxXXsK_FiebEU5j2Q$EcG zCR*&epuE#!mf8jBMFU#MXin^^*LQ~5e_IzuHkR4bpmn1$8vDj;v}&WFqaD^QbynEO z>FtJSv`8xW`tW#GX;!h#%;)c8AB5F%SpHJ~`-DgS@4NYbAOC9!RR6*L|88CNt@!Kb z|1F!pZb|hY{%_e---*9}{{JqUgt0Xtb3}*BkgNpHw$(S4Gt@hweXgY9OHl773bZM< z#aSV`G%a|6kNY8sf;w60$}X7@+>X%PEp_FfAji=B)VBT*6G~63(ERDq_|J3dtoF;B zM06;m)@O!N-ZuMe11RiNO&8%Y8C)H^uH1-9JPLq^;b{o5Qe=f*EixSqgX56gm+_FM za%|gn4uvopZ%SQ7_PWATL^qLQ||=M&^B zkYZ^JZ|#qle}`JWGh$FK#4RyxgO0WzccNcX^nk{2>L#R+k1$ZesNISiGxqN=bzT2o zmn}KpOr&5Wrm20S=bv9N1_r$I-4>$;PzJF=V6gw{KTbj-S#Y(s8yuAk9K7CeOq%gV zME((K*(R8eDlj1Q5#ZYN_ot6o4Ehz>zpz0T!&M~gM?6-+kvTSc^G3>5sf)-?<^TGu zD7HFpaOEc9*%3Nc4=<(-ya^*b>R4#6Uu5zjH^PWqrsSiH2+3{HVuOyU0|Zhi0PQg zgKxPqgG8Mr8ehJ#{Pp_OhxF-AHB!4u4*vf9-4;^y6URZXV+pjXS@G4!u2mg5zn4Z& zMRQM^)3QtQ>A|ojLS9UsSzuu#;9^euAoQxm6iJF-*uuA&?(#wcH7V+=p1&XTf(Eso za@Enll@ICg>(UZECd$~zft9YkC7r_TYGow72h6*kIR4jLs?TcF&RS8Oe@~L!PD7SV zyCw6ESltSeXF3?>u!Ys{uKB6qQ9FTH`xd@y;P(a&cEi{*0@~mXC|K6KyZRRmkJ?3B zW6}4e1ol9zJN&339ja=uU}E{>6xr%e)cl-gUVDLb5YNWQHv6&-Y|SpOULZQ>exT>X z#nhbTtEwe6pOi@arD{E4iI*+`c+~!`$GOGBUM#ytRb7A1oMVGGf1JGZ&CO8lzq7co zR=UbHE89MP=@Vb=PNO$%*dD&Wk<12Jo{#d0THjXrYea|Vzl@&b{JZ%!8He20KA*Gi z{YJ>%R*%Ot%9w2Y(MQN{Hf&(dhXcb(oUu)Uva(>sCte zy|16gx|b`v?W*RW8%xrAt1oSnWQgT}dH4VOnfPy_p@AN~sh`}u zAXk!@9sPgUv|Y7NPgk%bjn*YE3ZPR>Kj{{EAvY$qW@*%%o@8C^LO@Y{LBLR_3&D(? zJvH~I`9rrwzvvJ(pBm*0Prmx~)-H%4IHK0N7((LIVJN2L3fa13N~Z&Uy{+=?op(SC z-&9_l+5!86Z;XQw+h=#fw~f?`#<&e%HK?(_IlVD%P%gs8IL@f$$x7M@dn2B(yw_cD zj9PoF^2m=DAT6M}OWJC;thWqv#@ddpXBpF-yRu{B5*GeHnVxZLYK%%m^ex&DRKCBm zZg3!-oWIrIcjo--T3-=iOuOA$C{1qY3pef=3(wrF=3vz~LbCj7`yLWEl%FqB97+19 z*sN*OClpZs8>S#kGJ24oORgbpJc*kO3{&~~Cysid68&jH<+`KY!jDf*jjcK0+hzgh z&OM?zYx;~ChlJ=2?g_6n8ABWbpNo}X21bW1utAmLm>eO&6f{a*%;VKIOppZ~BtGGNZhnmoF* zH(FMsuD-I`D_gb;CnjeUteZ|B-1f-sBc}#kRDi!Ih5TF4>tB>DTyhD$n43hLIhbTm$L zFB!X_nhOw&v{GoRnrBvYlzX)ZQV>y__8YKMRebQMvyShd3Ua;}7?Ak2@1Q}C)`#VJ z(ATaE8|P7-M$*pkf3w2N%a*>remSA>oVM~0bC20`Up<`F?ujEc`hpyOoO405I%mNmD&aGyMN>- z|1}K1rU7ep)uTDOT4Mj&7yT;nJn0`&0>Uhbwkw$^+IJc%mx0=wqv+wa;j2y7h3M1J zU0CvyVPif;5#nHrh;=E*UhY>fEArSV`nsl7*nnxhYdwj_zGvrz$kV)OgQ915M_u7z zx!5_sPJU_}pLFJFn+pdxYTa_5n^C)DHuj-%|2R5KbmIxuy-L>kyH9Xj2*bWHkw{)R zAjO$Sw_GNuX&l#hgsUEY%r;&$dO?O<-hxj{uX#MCZ@+#QWhwg{ZaBThkSMiono&>nov3%;!% znoL9svjlw+{^Kd&ySk!=`{JK6tCsL~`Vr$9FKOv7He||`>?@^S0K*V;IZ}*F#T`gg zCOdez%C0zt@x~K6dJ$C`{@QL+Bd}e$$p0=TY!ChK-!5)u4a-Arz85WIqS5Gmf8!r} zo{X2+k2IJ+-;**(FS-L4i1LX1vD*W$zH*tnGIUYO=lrJ)d2GGN1L!rG6R2!IM!I!^ZWIlaO*z znkMDSV5OJNR*@}FqJ=>T%JPa}=zjOzt?u_#p+V;G1fULwm zTyzK9V7irH5>Y+UlTG<}c<##Nneq&`h=9T%D?AtEfe^L=$TlzgjX?=VC1$Wf$pXEo8Nm@*c-KYX- znwDA(;ld`DX2?7?by7QwBW*aeO!sBIk=d7?j=s_;@#KVN2P?`5YZtI?R49?wzu*C5 za+79FalyN_aG7)hE5f5|uI*Z0$GkfrNdz#x`^eNAKR{pBq zC#C~FQv3c&)VFBtJKv}x9rK!wDLHp7Z4VhoGt*IrNX9ExocMn414q^Ew!r&2$XJ%? z9oM2p(2pLFI9gt&8ydO^bJg2@^;mJQae7U;Lj5Z%U%QFot7zB7Mc3DlMkd9B4njF? z9U9#P(I8sddJ0l!ZWR8S?UrB4@)7&kquW)oojav%g%4t=ArXvmS?-ai*VitqBfQCw z1^Z6RUBJ4*4~*rmE7LGbGJi{Zsu{v3(Pb~JY3R-Dl3JumKzc~De0CE#wY}yEJcRfR zU7XG$HXuY>rk|uvQdaS>K67sA;$vm+SCvIo8n)$rL@V$A?Xoh4dd7|m^_YD%peA>< zZ@7}kNF?8)lX+oQU6Kru`ndSQWDZT+)8FcecJB-_79D(I{rZw2h7b#njat8Q<;|QZ zQSPdn+<>;ruxy(+b9YP4LDxT=6n$l2I=5CY(Py592U&jN56fj17A2#;Ojo7B3w@v^ z9^I%orB;&69!Cr&QQDyC1sE4Cqe|c6KxuWKDgv8Nl+Tg-=0>$pNuGxayMyuXL zk(*L*YhpHBER8{7MQL)er;f~bqnW5T9SPc`8n|Um5m&{K0K1_L(8P53F$m|d3f$#F zu?)FqS8eXkZWa4-HK&HlIpsDya!k_m=ZooZ4TX*IlpZyAytvH%^1h-y6FR{t^2!B+ z;ArD(dO>?{E8^N|U-4;u1RIf2hAUfsP)6!+X^PBaC%ck&0(Y8o!DQzG@eOYW3t0_D zntIEwo5nmWLI}AUS7qOD37b3=EGh49x3XztOw=st_DhFr7Oyw4>mjjd5Um(nN7DHw z5hV163=+N>&a<81NrlI>o+y1loS&{LgWHxZ+gTCMMGSiC4d=qbLWxE)QL~2n5>J6m z^~2^y^jXYPfn6s;%BsjV87D2y(|)55E1aV3L+V^lS%gNk{1n=+d(5^ysQhR|Pqgmi z_$OtgiYkrs5F74A9C5^e-SPj?4%@Ec>%}E&)C-|xDee|R+-vX+GFubJ(VG{KPnxc$ z*S)#KdkiDq;z&x4VgrZKG1Oj54c!=DcboUeux;%t!L%Dw_Wr970|~am-_ZGZ;KrBh z56(P0Ac~oyjhDB}hUzq82 zz-9YkC(M^!n+{L=kx&qz{WlYYFJ3ed6InJDUu`ch^lW|*(M1adijJfFxiP{oHmEUB zkL~HYRHD>94U6O+nlSiM><6_g)U-!ih0G0XW>wdicVH!0c(e z0Fb;-NiAC^Yz8?%<3@a|Ih#r(>g+htu;DV^URPA8UqN~d&AyVdz2tkVX#i`|6P*dw&odHVh8=7!!Kj^w#h{F4qV^M0Xeid!E|9v>L>&Ul*ZkN1v0bdGW;mohLUsVLDp|7fEs^g zyEpYl($Jn4WL!E)_C~DNcl-ULe|`{_ZqBWZm&IvImKX>mShN#wdohM({is0)BWEOg z8`m&syz3yZ@Cbs0bMWTuWn$lIU>BB(7}vP;(=6IhC-10y?3)5N&Tttaww3e<1vK8i z)j7UVDEIwHaN6QZH&r@8m4F~CHsYr4Vly#7Q`kh|;e2I^@?;)U!#_s26#Yh+M#;WA zG|%PkH7@Ca?`a>^VhO=V! zQgBkvQgnH6SH+q_@8Q*%#Bsu>iAzR_`4uH@8wd;)Pb|xscGf;ZEMUKCZVbjtTee^t z>FAb{cisHp?EIji*#q^WiaTxJFaTFZJC^Mivdo29_K?*xQA(HunU&R5^?EEJCd*tO zUOZ3Ju_K$DErB0qR!t9b66dN#m;JN@UWy_O za>FhF61}xWno2|`x`!e3#9Qw}M51#MN@w>yKyu#D_g!0l1!3%trvMo{=SqxB|5JJe} zL@Mse)m7nVn_n2jqFL2nmstgC1^1qvVvZ}u515O>%iKN{5AoZqtivvmV#TsFA2JR- zPW)M;X8}S88OCK9#!3t^u6*7%A~`Sq_giIDDKdAF-O*Hg95|^-O<>`@bpwk^Kplz4 zb4W@v_w+v(lSC^XFwww9(yKL?+O+9^ zC?m1&3ibIg$`1X&s)B}_e!}Svy7qP(&5FaxqLYRktg;+ilbPR+6{V zfrA&LAR|)jb;Yw8TIM0VF-@O3*s=B6-M<%F0G=^?%NYPGoq6@ zr(W7u2vg8tyl8I)p%*WBeG~OYTAOJlNW`{i1-IgfqCYwmAlz_VjU2=yOMT^vakF zp=JS2f+5e3g{X9d+#a=^H!&jQ=RDx2-sw?KQe?;P2heQ&@?23EF42bc6ye589Qn4{ ztZCB(%7p;SdqRx|Y<@U!U(*>f5RXR6A)ds;x)lc7Tl>YLUJgPK0yxl0J9 zpUOeS7xL&5ysd-p(9?bOUkmI)(PkQM}8C&A?g< z?fIBwLGO+9kXe&4H#HeW2})jcaQl6+sfqNal?w!}JYTom74dfKtM26jW5K5#=3WBQ zo!2vC1YoqNuZeU`{6mtlof@|@MubH7_I8-NMxt?$S#%dsy)e7#QSP?vTrAy-UQ~dG z>`|S2X~_XJAjNzZ6*>{~9vGJ?(WMs)*k^tBpW(AtOR}6nkh1eP@1;$tv7l!9OWp95 z%RwjN0jxgb;O%00S3PD@@SXySK63_*Xb|7f$wN!eXXefmx; zkwbtuM!UC%tB^txg-B8VQYCV_k+G$l{l9+R-m-v+=VisYHeBdu_aq&l?qO_iFQdVO zx1f`t0=#QY3ie)L8aT@J4@O|fE`ALw-*u~ae{fD&xq8`@NUagQG$)4g* z*5#o4!Mtnb?+(8$4Lg@vy!lb+xz9#P#WOUymwH&sL|TQX0{4MtJwdc&O>Vk24Tgsr z7ssQZ`s2F7cOTaQbMz_o%_4p( zzz2LHvBH!$SoXO3J98WO%r7VhsC~xl;Day!!}~5Jx3Hij%Jx@b5XQ0eh}m=I1h8}Z z&AoDl+DHHy#V5{mq)o1`7!9)G#rdtt!cIygQ|^e_WDKJ?_);F#?^_H}7Du{&WbmqW zN%jnoYS*!2%y#|CFON`%mK;pZJf)SL4;zpAg8t5??S2K!6>p21ws`SrT>crx0vdgO zdhgx@x->ivqh`T>ouWfu;8&DwCkmd)xk43M zCTuor@Mr(D_p(FeB;)=2)9iNyM5gC{|9*t&L{iARRJ;~kahxfagF%&_A!`Y=%utS} zqOgX}b4Zsy^cc)IXEX<(?Ql>~5VK^~=6}#*)}aLjlsGd<`6ObBXLL3B6fO*Gpd}8N zeuj|qxWU#)rZA$QGL@pGV(~H!$_!6ALGkg;2uGs`V|u4zeMuzYLfg3DYpR!QSrjKY zbnPdDPR$$MsXV>v|0=iZ+_~T2!NEqKR}mh73AkisXE!Mye&);>16=%!vxYNpJdXk2 zys>^lnr}6MOqiI-Jwa3Ry`dcmkOAvv1{{$)<@?6&Y#IQNf!f9a5&$1;jRikR`g&C4 z4}7cr9L4*Sa(e5yD{Ra;8+{q^4U;(_mX`_6b4~MYehwdYFnDtYsV{?p+K31@CSvS{#b7z9dCxOJCKrmITE2@F@ zt8*oVX3te#zoq>0jDcXi%3bVXP}aFb@wt2*2^p=yog3DW|PbJsCd6*z;oiI z*;76Y?%2GcaMlD%r!=bmt@0g`qoE9#&=XDEhr*b)-^ZY@AP!kwDo>;Y2NTM(*zM2EBE7A7C6r-lPbS&=@tKdEdaVwS{$ zS=>urUK7P#&9Bi56h5ez|8R2m@Mu!nghs*uf^We=!^O)rYSgG^c^y3?eM71jonfHS z&&aoO%_gF2TR^b2jBOk^E7)^FON*^_8yE!7R}-aPR(RXVl_#kp&&T*Sn|1%hps3wUslW77Rdt{6U*#HJ(0b*Yn*sFmOGWlX$fqnBHg|IlhVJ=?`TtX}dnt>?5q5 z>2Be8{*2t>T=!~VoU|uTo%)OxmBsm6$OL?l*l4q%MgJLruAt3l`Hq4wuaEP-%9nhv ziF@R}q+Hcyy*)bE6@RxK70#3RG-Wrtp)t(k#SAhwuU(rqC%LB9m6utR#LSlhg18Lr zRMOHUiftJP{cDsoz$pw}{$iT=Bb}9gET2}DACcr90t&JK(5j!)rQ!B5OqAY8ZcE3_ zMA_<-src!vD<>~+f$z}FzPmsn=I-5bH1pP9DMv(-Krv%&dQ59PT`Q6_1#Kj5M>4rv zub2tpGblhcPT5WN6M(J-69ROZWk`!df@hv}W2((idM4ZT zGuVA#Q*(sMEvS)W1AC!CjFYKa@=h6q_?>AyIrn|;S;L{F;GX=SLXy4AL0V>o8Ew$8 z;r;07UCT5ozbU2zD=HCw#SWM{ArnESd)VkA--qX&^R>TP#1H&ayBdInzi~ElrJs;7#?z>Zw=-h$s z!#SqBw61|fD~_5zE^3g$Q;?UnAD`^>woX9OP>7!s6^;k*>Dda8)<%E&^vjS*d==IxJ#6wrFUhOyYOw0$hUKsCz zM%SjXyQgD`s_wO_3lg^IV_RL;UB7<)idWb44)vR1BJtG6Ob^A?G-%Z5L2~jjjfzv1 zESSj*FA^gq{wx($J^=d#mN%2#_zM}VcxUd3{QNa5US5jz-^|o!A?$SC`WRIMs_SQ| ze+Ez+5-TtF6%Vy->(&bsM}vh-er)5db)W~p@>Pt+qysA}zf_{(X_MB&fhioFmCUrx zt<0hkPJ|T6DJjZUDnsj>Ih&D|c2YRrW6HZKPc-7I(Y4iMpjX^~HadD=7e6q=K!Aeg z8Z9ndx$>Ms-AM{AODMHfFaMAWOl#gu(XxK`*aZqpXj$xx_QamHeaFgHokZmiXWe;W zo8}-Eres)*o`;pCW%m!N_$Ghm+Kwd&)MGrNh$FCEvbwOPqWrUUsp$gv_xhlUC|}Rb z=AeyZP$Kw(W{sD`;KSQOKu->~~}dt^^7h%N9T z9U6!NYsD+{lfxc4rhwl^sXu9$6*{|ail+ZAU$LT-Vo%(UNJH95OP_(jC&dwYwf`c# zs#t($!;ueU{KXujdP+*@gt6c!F~=bZ(5ZSq$@gVkBQWy`=2;9Fy%+ikRE!mYdOi)M zs)MbHgGvTO;PM<_Zh`RCSjbI)gaSG`>lOP$Cm&P&-UYYI)*gsFdi1E#K<~^iU(Uf> zdB93dxZexP+ZkG!`=HErr{ahaV@v5b7V(<(*>G;=GS-e!z55fepoZrcY{i9WR)LlZ zDk7u-R2VRN7hugYp!j~sIUlUHvsEV;2}-{2Aj<8jOSkqO$q^7Eibc3aZ}t)%%FA1}!%tM6iEKP`x*+C^Y45Rh`}Xa_R=qg``DAC4>L*(HuJ&?j zYV%OSQ7|l~#ORC=#h9L{lbMu6pK@V5nV1Vb<+mjyJeUQ4Zab}f2ayBJ6Klv`uQaHKgRuDos8);a0!N-E z=n0_6=%_Elviw)vO%bl(hbk?NeS;CUg=ZkcIQQR-{PahX#kZZO$prDO+S#WrnMPebT)})x2BV=f6N`J1M7rtd4Y0w@=M+1h@m6VoR`hFkns0BqsoG%6957fg(@*D^k*I+LFERuM*xFivsqeEyQ?nD1sI>HKw zs6>Wa9WmWDE6DBGv15Q5bIz>*)cR}y&aD|!U%(~=4>O1^hcX?Y(m>00dvO2$6JpH6 z%SypK#)#NugA_xGf-y2doL=W4OIlThBeJ?@jKk+|-MY2?jdGHTXJDEJR9ob$%~a)s zp6$)FNS`alC&o4y`>%FvOw6=Rn{J9J4fyiOWMEI(Pi@<`KW%R>@yYMhx%1E9vduV| zXWnD#;VGu0@F*S#(d7*PY26#Pii!Rz5Z$NPNdbgrzY5I@XzHT6AJmPhw51=vgGtVW zB4I$I#Ejti^Kf=;RWzt@(Z+T8yqkbN9q8WS=R z-<9RtqWjNaxd0N&K!Qm&hdh114I4{x6U?Xbxa-!fYgOW=WoG8f%oV%uqqg7n#;^u~ z09&nFC$hv&jinAU^cC#k1K`KVjrsoYh06UbJSMVWW9K63I2rb^? zhK~`J>)Uvl8(JoOOGU2N<+91Wrk5ECk)U|W!Nk-@d=IPiQ*wwOiky|$oBbxi)~rCr z@(Vy31Mt~9R*q%S4MEq2NNh4ZEPVrmw$*ksNt#hnJg>_yN{y+S$=E5-qf zqYz`?X|j&abMcf2>%~f%xL;b)A-gF9hZihZP#8~xKA_XVkdP3(v~S%8O%*n5Fu@4p zrzh-b+uiIa^|S$EpNTIEwf2L#xv)(Nsr4yQEGo|;biwU@KbdH zAp*HVi^twu*};QxfG83iP)@~!lJ zq5YMWr+e9z*__LNMyhbG&av|d*$dgVpZQO954pR?O|1+dBnsCVIJ0-4^M~|wU)F)p zEm_>-*Aw73`<=>#FXtmTvBS}bk#l}jH$RcHjI=AeiO=m}Y@E6?X!hHi-UQhI~( zKp>V}TmrY%^{Z!sC({dOJ_E!v>?*5Qr1qeBk|g=o_k z%>E;A)&_}?55I+VbW^pt=v&@C$5Ub9Hi3F!dE7R)ek}q{lw$2pmfMyTS^6v|Rq}`% z6#Xn{)`NtEoD1cpa3zF_LGMqW@pziayO_p9w)*^xmDlLa*9Akp|K_vC;iHRRDD)=p z)WPNs-Fp-5zoB3?J~!V_+}HWgyX@0`exKQVF9Bb{FWX5;nJUfMXlwg`$%L#|oUuUh zaNQAg#txD>jtsRvLBeBV%;hdA*O(3@B-Dyag*L9m6JLit9R`=pE@#-y3__K-gwGvd z9!^92rXsztcd%uB(592fv)LD>$nh!3Ov80{>LfMu)FdI*)jDz&^`|~6Ds%AWNMY%N z=C~X@*h?Ewf&aZ*$#yLzy6-^Y_w3s@10T3D zHS8?BU*Q-B4boxTJO)g_I{ zHQKPD=&Q^fh4vUvO8-`lOVjXQ0xxP4Y3gqK8c>_>dN8`tWbPFa6yN1XSoS*H&Y0+c zu;rv;#FTB)j6_>!0`@k+WJMR?{RXPwS4&*aleR{vrG@N z^!g7RI2EYwfEXMUm#fSNrd3e~nv@{XO zdH9iI0MGDUo}1mjfBUH&pnpnyoD1Cc`DXhs|G5(v>lC^Zxy~tmg`w$i8TCx+i@k)N_Fs%hIk&T3J_s{QnR zcY_LU`Tkmu#nJd#-QY2=xEUg9wXz~Xxwz%KIco~5e=&U7rV+b%@&=Gi!TWNiT|1_& zqcfA>R>YxhZh?-Dj#=v6=1;$J`7)X9jKa(B#4UXOd?u^rT}WN~mNz0ywnvMS`T)u^OU=CPWU^ODP^T{1gsz>2{6B5Ky-_J{aTa@R2%)H z*``Yu5fm&%e2+kJ$nosLoSXwrPEOL&(nF|r2+6&``L}+ide(!kF*G{Z_}Yyd$`$M_ z8VKPI{Y&Rpbg9v(6dm5xwYmjTVqbU#8P_au-p5`2>UD38fDodKP>NR4~u@;3=ThFSuT zM-SogMYej~Dr1j$hs=J-!~8wA>Kx|xBA zqHuu4*DhRk4GS_QeN#!NoYguWD%P4{2#-ZEVU12fR`PVl`{@x^i7J%J$dUSHYEDcl z>^FM!=o?zOcgG$TRaHV~3}UaHyLM$3g_@Eu3*`LJVB`68gc%SfX1smt4ZQl`&71ky zF09!i`?x7AJrNs=z=y|_0Ap$`wVQd@R(GE$a|Y3-85#Ev74UciP}EA!d%rNjZ2v&( zR2Us=^WINo6z6ABZjs;!?Bwy|$9{vFzR$`!gX;}s1%W(2rpGr}6taYs;~{@-Et&F$ zdL~b1z}}Rv=!(t;6CaYYYv1piU1hL}O0s{p->Flrm8{HUL>42W1SIW5stESH?Z-c$ ztJHiayS#to5kvvOZS#K@cf}8KhndqS`)kTorydTvOMpEzmnN|`xq-2>=G?n?4-$qJ z-EImmjUM#}s7ZuQHFRCp?9dxGf}n}D^}nmGv`+CFsNC3pWADesC4w6<6?5cS1kQ}J zj{^cCR+r8-Qj7^A`lGRJ=pt?#AQv>cf#M%qUQC4V4nH)_pdil5xeIn zi>-hL`aVFl%fI)eWI}N&36zcjC5;)CJpP}~LpG$p)Ov4FUv%DTJo*{^Z2DkUCda@BTpjyaq!nWQ3ha&N zaA85`1q&g3pAfkS1wjGIBotTiq>hy4Q4%QNJ19tLk_MS+I14CO^tJIwXBJxC0SC^= z%rwtGZY$HPvYF5B$9KRTks?s?3E3;}NFP|?ipuhAE?_yUr%0MIw z2(^aTN#sGsIt-qlNaUJNSXsK#cpsD84f)=Z*I$8C{3wb(tn?LuP;6kbzoMuXC#<#aeTP0|!8 z^NaF{2s7cfJiIddB+&@qj0n!VoGJxrTSkRYRjQzVa5O669ZzKg%m;u1{pjJx(_6}s za=(3xIcQ$r`XG*;X|_Wa-ES*AW1vAmo55{MPI0S!uIO%Ec~nR;V#1aY8yJ#OhrLPS zZg3Wpxuee^a}{7c0|_0c)Rn|EfLkT}GjQ;V#I-(F) z>Dbuu3*n&HZo?1^wjXp--y}Lc_mcmk^ zAVFqf=tdArB3*l7R=Cx-w`00e@+!_`-j+YH00746Yq89?eD^QeiS^Cbv{k?9<8_ZD zVbAW}(^%9r;Ys0UGPPHR4|IFMq@I7ejx=DCD+lG3IXX6|-s1mOY3Uv6M5WOmUo(y^F{SG#(ue>dO@gDse>y zsY*`LD(kt#$RbQ4*Q{9k1qDNot*|xt?;)E$fRUGh#^GVC&U2i$0MfUXbinmg#0M1M zatjNG1?*~v;9vj-u{Rjr zGU{-{6xdcni#JhCzGPRvWhyOW@m@n8t)Lkq3DEVj-JDo)Du}Ti!86y@?v;+I7sI+hVV?KGQ=~VPQroNiWx61Y(Qe_xwo5O`wGYY zp^GB%1h&yBL1X`32o?M)fz8Pg zFY&Uv`I@yVfdBgLcWI&Nz%!jpDPTzK&QPWsogk9(pRl*R8wdlyuDR>Z4-aosUAhD? zBfV-ko>F&j>SCaLoB(+A^)02>#X;!V(FKPpnblxa^M>2ulLzr(5n*qHrJYVxMk*Fd zRMv2N_gQ=TEMByzFW|F?#&G*<-^X<#eAMMCeptpHwBsQZc zWt#(z{owKA$8jTf8bwq9t-?sd=!F|)EpRuYwcLWtw?HMmO25B+#r_geJ+9Q=TDzyV zuI?-{6ETz!F5#;)D-E3-9TU;&39%d2u{o^4Yl51`$p$i-vhA(%z3=W2u{{9r!*nPF zt@xgoSNz&`0oAj7s^>(~KLZUf2a*C=Ww@zH9#F>~y%~;q`;c%!a-raDmG4$lvJ$IU zB5S8u*tr$7QCTxtr-$w;({U`%Fbf4JBgRHWMJ>N_3sk=|Nh?m%@ktp`blz&#~Jp5deU&-@WZT}{=MTjp(a zujT@D?AA>;Mxlv7a|mn%CcpR5Z2&6}HA5PQ5Yf47cu(TIoy>4eT4WQOjT_xw{nS%I zigzlh%yX>v>>KHdg8K!Y@MGQUcj0YXwGyc?ypXwMqD%p6dr*Y+=cm~f?<*o+vRc%W z2AMdmw*iY5FaAWdh2oS8{d&E#-`tu@D&GG1L<0j?R?BIuMr?BO9Qv7_iwKb&yRt+Q z7el0x@$OyTjR_7@lkYZ2#`%dRf&jj3z|rMV!kd+q*mPCjVnl5*_COC4oRFX*5=Qkb z55vah78J~YjB2&U7*?A|VT1#r0roi|L%X(Z%`TXBA&^9a-`GWX%3JWi2@y^Smu^`% z_c9*GWAtW2QMiOA9Jj?zru~#NVD_r2s)=ljL*Tz#qoiWbnw&Vi<)G{oT`DPbjF5LW z^ieB0(1mb)2z(oD7qqm|!XGGzR03Ugey9Q9Ej%SfkN!i3B>G-0zzABcU3*Aa_L6Vo zpb|XsZCd>LmhCJdo1tcVCAqDy=)f1SGTV-^%u55&92neGstr{x@-rG;<}6nBLmfI$ za3raG3z~-Dlx=JdH6))AG$0z2g>xtJG*4)$jB!~4d0Zf#5V3|*T!$Said2gWWG@F? z$466I2O`*r3Gba6cML-)a1j?dDo6Wd)C{pI`q;}E5J>lqJ~F~GZHHhP={t|2=vIYz z!wYA8#8g-{qT;n7ShB!%S!d9U3r9;|UtF{o`07z{-->-v{hzn1I#HjPJ-cJ~dh)Ww z=~(pVbdU_O$u%QoNML*5PBr_xT*DaQr)CIJhKuUI)Y{6w%yD*43^rB|nYUNeR(WU( z_w7w>Tu~747Fi@`v)Xvn#ZI;!5wMJ;l%}NRS+VHb-_p$KCbBWab`yy>;UmFo`Q|DN z3n5>wE&S0*>laukz9VjjxRC#ktuqhk zdEefB#!9A&L@5n2g`&`wB2kff3ekwjRHmX-luD(^JeLfWp%5}ODTK&WA(}~s21C(w zUvKB0d++(3yZ>Cfz9@j`zVH}dKEp}F?!%4K<9XPn!HSx zF7rS+;KAi}K>;84fpsZFr$$WPqbz)De=E`nZ7mNb_cZf(**G2d7v*Fe{?nPz9xG<% zWRe?xGI2oEx!{H;KU-!T`{y)<9ilOr{I7Eg(pp$@JJ)C!YOft^LCw3D^M|`BnI7@~V`%i*zG)?VxxpEHOY)~RdPJbVgzw2sl5 zy21VCI4HIkg0rE`XFPP+asyQlPtS_apW}H}=O9PKknl+n{$hp%&0XSmF^Xb&ayR;l zUvLM1P(sB6%Ahln&gmM8)BNT2DPj<;%*a?(QQCoQ_Gt5pPJiQq%cja%aFWDhn zMilXjGjBu2PWkl|4)z&+X@jXo_h~PDu6}PUr$)$ic)f;}+T9?fEjuxiT5O8AN&NXD ztiB&EVVLyzu_gZS%+*iJT0-b*siduU4rGm@+rLlo-~UZB9&D9xa@N~!oInbd&K=v? zwbQbNUlAQ<^uNb(mycwP8cmhQ@Y%^jZuY?`M}alcKxLxvWW2dBFMfZY(Q8-lKV%IZ zB0etv#i4$oxeTJ7cEV^0)kCS3L_`b`Un!{>X3AvpaSQw}#M6O?8+(aRLH>L|ga-tu zcb9N$OA6^EB^AbTkdu?sUfas@D@+0XS+^&cW$9W;wBA>|ui(w!{%)`MkcnD~XlcTP zT-{Y4Q689=p>@jp>}4_??tZ_vfuSp@+yuTA9zi|>KTo!ktqBI^A21*1$XLr4_p7}w zkR6IE9*GC)!}E>=QQL^HE#~>~HeMpM8x9I{>TKiHoJ7<6bf3)o?5bDC(Y2USIVpUE zG1K9V;ntvM^fp$`)W^qcqhQ{23dc-5>XH~=f_Q$gy_+`WarVC9Pu~?5N-1fTVEhxl zsh6A`j|F@XIFpwQEE0GSL*)rYH~9!IDgt9X+o^=5`til_rR#<=WRYVXkwVF8Xy7I8 zI>18I*_5V3YoTzV3?rB7GOXd3NBDuf>r=2D@jPYPLv z6en4$rj298u{;TxJ)GWM~yEv z4xre>Aeh81t-QYNy}dqxfP}9M#!Gu^&N5Y1RZ#-a__=I$(+u#b=#Zd_jcgkzZdQ%4 z*Fw!&rU2<~{fM>f8Q1U8KneF^yTnYw(FvtI3Hk0MMfg!cH6I?iVtUHkR59a;J9_y> zR>8C-XcY_vRPYw7JZ2R!_YlEU5+dZwp_u_?- z9#2!wjObkx%Gp?asy@-J)?VO%gz&S#5n-JFdmM4YW_`h8cpZ!$j{{-ws8Ra^!|sbB z5hm&(9T;4x?9um^P>JRug8u-zTYnm02Ea|0`8fHzAbro?E@Lcj*e^S)8t3AB!|sG7mYS* zX>e%!`vjz3JTxIk=ozu1E@x8~5lxiD(5|xx(84L(aq)9l5Ygg6ICReNmBwc<@Pw{J z%RZzkXS$_+Z#O1<{G{*b99%D)=>PjyaUuaFqtdhd`ijfmP4)ts{`m1Y>7)LM$$;nJ zyv=0z&&v#V5uX$C-k`aJti&Xa@u7npjo3WdJ#s>GG> z!-qu;jBjFyje<(cXrwF=7y(m=ofei|4QF#WvDi_g!DmP-xoZs>!VXyFFrE?d!7{ zD!N=C`1*yVu7^w^)Xac-e8xf9RPnIqm3IetNg)hwz{Wp!{d%39jlG03oTFAn3pT*H z(XB|LCiGv9*~=bxZbabH5Rp*O*L0iz&$DEM-yYh1@>d6*Vf!uz&p~K7F`z)p#;RB_ zsu1cSc`1ap$dYxaBoE)deLHXsy|Igkogm&jV2o@FPj>jDpzeMCRXI|5J11SlwGU_m4a;&JTn&R`zn26~|Z;0)) z-gxL9!juxs98AOoNK-YuY+gFUxH(2R&o(!S97sVIEMeQ0h$0$YL_v;wp?%x(2&yWv zgF%$ZSssCh$ro=V8}>CV$dBR^i{JwI#BDRD;qnWn%O(~nmw4o@w-wPgmvGCpB7bL(0fO z`)SzfaM{&D#MjoAZNj)s5_weLcnfR|9?l&2v1@&GQ9dPlITjMgiN%kVsZn0hB#|AI zEk)~>R<~~5L<9wmUOCwO>&K6mgfT%%YPD&3#^1gIjzfvxE4>0?`1nZi)WE|cEf=t+$)X7ZT@)v$IZR2%?0bhTZF zbzzHGk;nHg!{d|%glc6{82oM?Kudc>J1;LULE?kmEu{?eJlsE>K5$hi_JytR{lb@s zl_&htrQ!0W(ud}x*EKY_32LeEX-y9?J(0CN-P?PcZEf(Lv7P$=6icm4>Ef_U_)q9E z<1`Uta&;M%&j6=1UsQ`qK_6^kdF4uK)L?VbG9#y0Eq-civpd)Ohsc>!wS*3VOQ?WFod^GSqIys zDPu=*hKpKNkf8SYn(;1RJM9v0qJJ5P#s<8Ka7YLLp30hDEps&i4{U8-I{E8WZlq{Q z`qWNqAh22##oF@~Re4NKW zkOyE;$N9lGTPS*|p_~y-p6;`c-Cv0`DmVz-?UTN>a*Te`4Nph*w^-wNvZeOTG5Hgz z9p^eZ4IF;)tnjG{ltzXA1<(*qWl0V?l4c5^U4kvOZMIjuEC}pYoIZ*fsR55S2ALmp2q52pq%dc?n_- zWB1{iVJk$pJq6GsHk0eL_)!LZf;^kcM|Kol3<}!n>KT>!-tV_Ju2JYCH8sd z93QuscWGNZ)Rfu=F5%(F_7aNEaBJy7PJ3Lp;r>Q)6sH^*mecP)Ex)e8k&%(l8l_}p zUVu_wVZLlDX=z0lR}U2xp|Kn(|8;VV2Tq(P zU=MfCYIt6T9=?-GPxvD<;a265N-G;^fkMTo8kk_qRksMJr$K^${V$j&JM|iNp3*%Y z`<-yLAt8y*TiyYppKuQdb(kye{Nv}zmM)-Mzg@fPZg#Lu6uuffivx*ZIxVM3DzPAZ z(H@ub{;8l~Xx1Sk4c+;-f^RLJQDjJ^U08W6g~4T--46QDU=fd?Dyrgv$J-kE;Sz;n zpGti4t(79C&7mOwL9gfRFb)4E<6qwlhn>CMs@f1g?!uHe)28%Si+LcoQZrjcJ?ep+ z$?E}woS*I0nktq3Mn`RG=UrctuZ+4Dn7#i(V&{+vi?(}ttlK_v!_ys~Hf$Pt>0_CT z@#C`NwcAfTZS;7O{AAO`4Mx?7E&T|DaVp!@f6FT?cV?yZD6iA2zJqc!2u)QgR{MHV z=9o^J)S+WnjqEdHa2%_)0CnnG zTD!6uf}At>eICqq&>1yq`GiVUZi6ZVh5~|v_rBd#3Glkh{Ye07d|Tgvo!dJ(j^E$x z3@|@=eO=MRW#3C++bCjcw|Bx4@jw2WXCNaZ6Igyo$F42-x&wRN6^@xtVXmSr(AIp~ zG`}-v&dfc2i7yvG98jwTBfr+H?PhxCk+Q688)p8*sx)%abg?}_O3r&bs^INgv0~Nf z&DBAuACmm^%FmxCE?LqXS$fC)d&e2a4JXW+B}+Sk3!FJ_(YbGLO;1crbb5JCmVD=y zFsM6@!N79F#k<^Vqu*|7e=a+sfQL6Zz=}b=vSW&Krg^pUS@dj~_pZ ztE->dclX=@jj5rhw>PJ%Wdc|T1Eb)^6v}Mee%JMw0-;b<7dq1M(>|? zM+>OYv&CP(=JrxMJG>GI`TLA9eXrRl(n@+QWcnfZ5!^3p@ogv@V%&l}IEHoNJzs;OIdk2PwGPvr2+S;aT*ABaAovgmeft4NYsCT`w(oLyIseF6= zQi7IfOk`1Y5AL+Vr`B!nm)>?uOlTfR+~2kfII2E+^n%jlJGXCl@7~=T7Bwv|@7ffl z+bf-%(h3WeKYaMWynzlh4}?-oVqok^%Y>L~o%wXChzAgd80FikZ0uA@0TF3_9Aibs z{rmo(`_9_;;PK;3bieri^>mCBfBF$;w59X2!MHn7ZH1qvx9qLd= zPtO#=bz$cY8yKyo4DaK#hnI3WS5p@B6c7|NapOk4pxWWkC0(F%4~|&Y$!2-ec*yxI zo}{d@@?-m^5MYPK*s)5qGSX`@ zc{ibp5r@@AjF|oH@cVE@0wQPbz?)w2##vlsT`WSQ%;)yQ>F&7GR9#)#t*%^}aVHha z4XKxzmE2Zn7{{jm*Eu)M;H0J_(z->ON?2WWl10vvm6OZJh@4AftQ4f8t%E~5d3kxJ z7buB?{_|7 z-MQ~SJ;WhScI_|1OyZh=J$o?R{v+JU?Pj!La6lA~Sk685c~OxnP37xdTZgb3; z&_K~>q+4+Vl9LUXxfidwne%?!*lQ1JS}|9J zsV1F3F^j*}-~aa+|MqsbX37oMXG_&z3%yPp9-1GuJ3f=-Arfpm%G8E4{>mHYe+D== zt6@A-zwSYegorS@Gj7`W@eEo?2O@I7ekWrUAvVX#-&kt0Dc@hl1{nGsqB z#5)NOrv1{)3TrIV>fK#uyl7_#tp8v%f!hID?F(;kMe)Z3=#h?+l9KR?i$2_+y=c*H zR6={;UtwaWQm|;{tN30r6HGPRpuG$nI56zDOJ!A+J#|Rs`#WQq5u5A~`1iT|zrUD| z;kX1=fT+h1Jby2ho_C9qHHzAiur(ggYD?g)?YM*~v$=VBy8;8V8#l?{C6ocHaSL@2 zp$x-d(#rwk#`T5W4!hZWCMwD&s;g;_e|rHa5T%_X4({LI5whLSYtQwoSLYTK!fPlK z|6^PWIbTFu104OD5l;&W6me74{QAWoWswwWvv{!*-f06=a2<5n%&aUmI1#t^&?(Wl z|4>}Z{Uh6wDon7N^?%OJ1)y7ic%d4P2G_9-U$J123lO$yM>JC`l(C&p9bF+;ZSnPxIC5<33dGh2Fw_6r0 zT-aO1$RhyqT;14M6-%7y_U+q~g5yt~lt2zN;pbN@x53ao?{lAAEE>Ei?*j*V^%=E# z;Fnvsfs2HtK(=r0F!P%A2?32mCBXJ7nh4HQp;S<67u>D*me%QM$fX||7;wH9DBO7#5$t$vwCm%JMrVLY9u8Upp~E!a#s zBJ$1PqSjT6<4!1`d5@3Z@NR6}Zce~dlf%st-juE{_iUr}nDY{5evvAxeTNR|l>1dx z0$-s$tuDMFOV;H7AJ2&E+@p+T*umye`l5dOoR;83X(d|Ib6WW(X$bVcD zl&6cSB=ELs6)_KRBqGn?R8^3&9kL88WjbuXAC+L#Z|F|JgQCw&vUh&gbmTkn>2TPZ zIvPb9liXW90{PD67GdSn32_iO-OSAH4_?x4HcPyk$90BUW~ff* zBCpnQXzn{XdKMl>so({UB9*9pZ~Q_*c5vj1JvG(#UiV(VhNge)efI1}v(af62GfP6 zqp$B58#@T8b|=@qls|OLmT$_)@_RrUwP}Bme{^a~%AaqltV&2+^LA8JHt^NTWwsTm z79+{NPrC5?(R@yUx3>(GmA-=9!dC1~2p%hRTUgdQjBu=#D0wI_U%J(iC|pxhGv3y= z-+y2+Q#A8jw$@o$S*2xWw#PGI0?mK& z;>DJfz>5GsPE*Z?9HrZyjS7?y=?I85Og9v17N_(?-pO1&XRJ#; zQ4j4PyokDrn#EpJPLBud4h)=t5A@`zQ+MrW?!y^0ivE+V>W*^#y4Cw~-~1VBC&w8% z@VY~HX58(;sBE5fjY8q>-Mj5rQqvnJ>`aW-EW4dJb*c=*2&pxE!2RU@#+I#hkbwh- z4$Z<*SJP1I@D~ZJElfi{@xAjy`w^{X!g(2cWB{O)b0&!|m3Y*e*`SRtn4L*>SWI@Ln z-vy%L3HoEDfBCc>2&v)EAAxcNI@!AQX08X-(8N`%1_O6~t7&}{#Wp$!8i~24jh&qu z7k3xD5kF<;w40w)3Df=5)Y>5T${rs=J|^z$3#<=>5ey3p^Twoc%B!`-WPQzsoLQqv!@R!Ktd;*#g8E9aiRrM{O`aJF4y^cW2n(yE5(u=nqcOQd;`XDP7x0&%5Pp5uw zIdI0mn`$Y?-6RrCC!ptmWP$JyWim+1)MGSRcwf9Y?8ncaQKwIL7&>$)sC&Z1iDh0Y zn(my<)~4T(kHus>w_jg+A2;&cEs*4$ZM~qi(;&`==$EOC-KOf|d|A3ubK zX+Ma#3|bPcHiUuYZnxT95>dr?yCG_B7(@gMK`nquZUwEs7BZca`1G0mdNmyi4)-12DpHwuiz4OYIzi)Y|%FFK*)CfnVAl1tl zR28=-7&15=f1`-&)BlN+qoEE{i6W7Y+C{x-w6yyc<_|D@EOpVv0~RT0+iht7l9H0L z)isJZbxK2c&(JO;_V3?MCxQY#fRks>N@DF6H=B#c1)4d3e#^U}=C9+T$fHcTEv4pR zWJL$@a&Y>Tzy#8!-3Cd-m&iRe=LWAzYQ~HiM2kn|^`|(1^A@*Y6o`UTb3*CBR<@JV zJ%44>f#Ei#RDd@&eO6#}L@?#01~PS2=nEv@eth#cPh>&wyLi^LLE75ex81bq=ine4 zpg64xsq6M0I&i=rZVZG|*M6GmD|@P3M&w35UYmO5%1FfG9RxL(75|;eXC-}l85^0o zVe+I&B@i$NN3T=KKNLNf&EI1h8X81U*BL$fz|9ZaTX*^Rv_V@JmxCN@p&U36#_)6JQ|p;=F}H?!TQPIJg(IKb++*Sau= ztT4UxC+P@wU6OmEy=yOg>@L0-l9Ant6@u%088`29r0SWuvuAJh@~nA#&%o;=p38e+ zViyRWqlWHsPz+{ty(GPI1Tm9a|D5n^2{f7nngol&0&6B7B?e@XlPK8#q~EujCk7~C zAl!lP!WOnk@F^*Os)huPGgOq5+W}{FlDgukx2u@>ka1}myG=(b9QJi@s8Pd55ZC`Y zV2+}_3&4rtuhQ<)Q|HW)$1Gt+w0wrW#{(K^%;qnOgoE{5^}tYf4q^kd*6dcTdYHW+ zgs7*>hz$l?Digbz&+NTtPgmeOb%Ute&&9@;@YAZ5R+D0yF24T5Ep)-T!|J+gi|&i9 zNvjS$@)~s=BTE}Bt5%B^waggDX#JwmgSRcn-}2J~dWCUB!u?=}Px#e8dl;lb6sy=- ztskjK@yv9Qk$Br{82XX^`03JOPC_1zZA=#Q6D7JJ8K2$830+-1sE?A8OwQ>G7rHPD=w1{Z} zX8^woE-#Z2 z0p}6KU{du_qZ9} zv#Y9dV=Uz1Z#vQ!e6Fs5fztFz?mT<;tceOQ?3kQfK6s9w18{-*v`1yiC}rqV8MZhd z{GE@V1d-Qlu}b7}wcngC!xOk8C`cy%_*_Ba$M3&QMGuL5A6yHbI+f7HoYmZ1G0 zE*+TdWo~KNJEJR+fSVl&p)Ihg!dER$i`*#$1);w{;Xs9Z<(9q$ ztk0Y`&oedmT6nlDFztP7BQ|YAw;Bn-*Mf(3RaRE6QYzz#goTHzHA${q=t8%cufM-c zWBMT@OPoi!G~S4B#8?p>U0t7>H@7COorB0W3*gQ1OT%@B5ldHZ^LREO~vD3IwT(XoyZnc;nSmYzJCAyU6cUg zBQ=g2F{um=nj!or1sK0}t>t93ir4C!Yn_}LZb_CC1#57Nl>l-@?1WI)sIBh&>gUM_ z_!BfIfS(5IYjdGq<-PthS~La;-b(}}yu8c>i2Jqt zp*qBx8V}O`w_wlUPsBEZMb(MG%yh^HCUJn}+rfIf{H&TMvKDTzFt^B=>a&$08RSw` zVmm=WF}t}7@^EL8=r{BKD4hQOL)>Okzaldslg3q}A#KFl)0Y4-k%H&7c6f{EJ zy4m#!j1CCrWoM$F?m7hwbSwapFYqpb`opMe1Z|OZOef((3B7XI9@*SV~X0Q#3jL z${Wpnf9WH-excbScG?24xTOm|A8j5Bo~x;^hrW`eM_1RY#UtuBzx5f` zMUrG>2Dd9=po+Uo;|zHo?VfYs)=aaksEj;Ir&cNJ^K42 z&A*X~?^3=3u_`osJSGCt>$MBC4|4S_Jo}QkO{SqIQ^&2w7G}3CcmO`$Fs8=TwP6I^++>z4h|YI zgRa7RisA(>PfYNVA~wK4B~S~kGMLPb4MkXR!(><3vU$sv``&HZ%+{!l|!-6b%Bh@4r2P$~0hmX%cizkHgFI}oa2Uu5vuRy}0$`(z{e$HW2TZ?!-BCcw;@T?Gs z`z`3wDqbM{Sn1epAKW^HM@#TSd_#i?Q0VlE^-)t(qu$vY`1Jy~9R*BKZc!($OVr>f zf8-dM3BA{kMrHM6o~1XzDAqc@wCH(mt~W!gX2|`8bDb=Eip1I1mB~7D+as(YM_o!FTo_;{rjQsos zaVm<>UcU4V4ebUm>W{n_Sb?2Qk(&Q;;6QVN8Ckj277|m`q}oG=_IEuo8>a|F2^s+hMm?gnw~^5@4& z8VSWr6O%rgExoDn@Nv*d(2lRYA<0leNI@a{pAg~zU0nzX7Xr1Ha#f&X1MXzqhQ_n|g`sbz(s43XRighcOq0GrXswHpQu z>WAaUaNDoWv^7ou;l1ux+l{y#&cIi_`yk!Zu3um=O^!JCW%y#rhZlU!0h()>T#%VJ zv`G=7nkj4D<=zrX(IH)bf4Kp%j7P!mj;iR(K00pO{sC?sY*(!Chml&)xEb7~uBq7` zelClgpU5%AS`{$XJiV#u`_qFA-b-lU=b-fMjqey(O|93BwhlQ3FWAR(=g*gG_E-ON zi3NlG(9@`@M!e`(+ZVzRc|w#dCf`>>X2IC)Pxd@={P?i&`ZM_#7W-3JWiJ!Lzxb;- zb()+Pjl%5ul9}dcXP3TePLj)p4T5&1$=QWA1J=_s8)Bn3u;{V=q^2RHEupnOIX*53 zUTdnR;AIiI(xoSBdQ%Z#oD7Au5E}s3Z`cr@cVsWfMx0vQihjJxzz@g&*)&)T2ARHl z6^1H5T60OBJ&{C(u3t;*M=5O|)vec=kN`g+%n!VabObTBKAP}S(-<6YpYz)&>z4nN z;gDij+iaQi#FoHgbB)sKV=3pKVZCGkcN-#zT=t;4bg77SOejLk2+kF1^_cBVWBBfH z`x;lq-BrLO}cT-`Egrx(+yaYKTu-p&_K(}*)Q;u6qoq893MdU5^Ajq-$^VPL+ zkTsR4tZ+jE!nJHt#jp)Q$PTN&!ndM*Bn;*crPUtqFM{$t=A>wCm?qo0`{vDmIM0bZ z+|0~ObIlbV9=BiG3~$0Z#z3y&z8}CA_xRxSVQ)BebgYQcSFYUUg$Og7vhj9hd{Pmw z9n=O4Fsn>{Oif7qH6uR3ggIWG%!fhRmOF#a#6(<#k zmFpUx=e+&z5&Vx|qP?Ee_xb~C@~Uf~TuCzKImXQhagYmNMini$Q0HjOPSyY-2M-!u zT@uh;QE}k;g}o(68B12>dw+g+>v}=~I4%QbvBz>1(Zz(E>S$0okN%wKh+)WhBzM72^ZK3W&DpIZ2T%cmM4LI5OJ6BG9}f z39V6{RFXSjcJXnHq zVZ2iWNzw!X`;}H8RZ<2ri@-5VnG|c{Gj9+9!uRg=g-Ul|51q*dI38&v!twAUV@d+z z0_aEKnG1?0cJ<&DPEG-|lL}yq=G*kuL@lc8wm82?egUvna&BZ z)cpB|U={HK2|2qwTfPa)IkR|&i7O6CGR5O40H2`c3lRcg5k-?~_r`n<3t}6`X(FQ` za;+KoGZ`--Q1el{AR{YxcLRapsgCUo1Qkn%S_bOQ8E0A}9YpdX>GYg`33(_AV2)T* zTE15!y1ETgfE4}=ZM}KP{C05$)QhrVwZ)!`L+64J1;ueEu#)4X*4a}Q`ccxFpRc`U zcm$gYl%#;J^WHK=LSBR$J-r^Z!bBX}N~4rAZInI`GjrdKb$T8nEmAXh$)0FovVq0~ zVVt%5wlk=-s%kf%D;>f~w3FQY%DBK$yuhxUr-sJHsb5alQnd>LZA89f%Tvlh2iS3< zqp)~VjYM_daEnh&RmP{;S<^vQL|*2TTy@T$zUs3JQVWn_l2sE^Qf#SOT{le6d;Z*n zM8Kw5XJ0=*QLV|XEBLbx(X<~kF)Q7d;mDZ1Wl8-y4|*zu&4V7Bz@tZ%h2eI<0RLW{ zOn*$E%yeA7m`)WPwnWA@Ejj)_VQoM_Whgv`(D-6K=2w%SQn#hLAh&TQyf0iBNZVv^Yakw+;+{qg^6k&>HkKW7$MqU!mw0SCHx%J2%|3cq$N95$lMgs>&M_=*<)1v0Oe?eMcL$M0z$}szpF!b_G z>>FxIW?x?eK!snEtnqS=>!M<)NO=T5vg=<|cyhZ6Q@psGcHhiB2)fK}GVz$v8?FVo z)<5RiS6*j%Rr4bC=ulv^FV#tMa*NZexSr5*y9wb^k4OvM=%qSz&J5QoT9%gKQ(E}D z~5l3OWJStK9a zAzfj*v0BWx*lAkadiXkC*1K=s^g&4wrUXjUu>JdY3FU|gFb3NeQpQd|nso28RDR4C z4#cylHe`qlAkm+BV;Oqp9%eA+G`^=}NcR26;;)J}lT|C=S)g;eD|o>}TCqC#@o4j= zuk|OmW>}zQ0&Q^gp@gote|QLFClfM!NH>+R)ePuXfs7w&ID>C4&e)j=rR!+7aNv+4 zs&zl^jP3S@XXB^=A5t4yL?5HD@~=|;k(7`i5=ZfuFWNtUeu3%)n-Ud;C}3W(b1ABc z_;khP<(hgksM7?3P;qQ6J)TZ;#(L;@p)(Cu(oA9LQfDIeQDZL^6l=L`A%SuAvcI(G zBc)qgD+}a;Gr&GGKz6;W>+FLa15=NUMH}K#dDmE@ABR^h675`8f5SQ*6~nxaAb0E_ zufnYD1Ql@j9%gQCWnm#>*yFbCum8SjYLi?j!lAx>GmYHqG-rJ46pca-Hs4pZhP!P_Y0arD!N(y?SDYDP`4n_J zd9@VyTFB%{batjQ%~QD!SSNnWJh*9h($%X%GgnJwPC(6{pU%_gtIr-YZtIUUSQ25V zNnVwEJ|@PObx2VL^cVypnI1ZK9~4MmvbLTXRkIker*lq`v;dQtAo7#F^)>88;$m?#-6m~wo$PDLr> zUcP#@Z28;Iuj2iQ)M<|%O;!KRjF5ZMbFwz9gDDm&Eo6+aaKcJlU$izDT^XKJfpcz( zCve}W(<@OBMajph5~K(*=$uVzN=B>1N!UH8DLa7gzkoMhPs8HMouZiF6Zcf`qOj&Z zc>`&zU*+&afqvZ;jMdfKf=!Q}bT!==X}WOrFtg_uE?<_T-j=V+ty)JjiajkS=g^uS z=RL$AND9U5y*Jm8=PsHuY{oa4133hv1^?xXUy}g(g~WqSut> zEw_PEQg@bb`)J(yyIZ$wGsbLT#C921UD%(5y=dgyr3`ly8v*R=Kai3K&KW)s+ld1} za*RiVju7~Pav2N}D(R!qatNNjlb5s`F9aKL;M_f}WJz-i3t72sq$;s=$fR)F)oa%# zgAozIhBv8{r6vJB?GiDGNdsAui;4iWf{Z0ttY^Vspy#jd;a{>DUHoUw(UB{<^7AB# zz~6NK`?5qB2>uo8I~=d$s#{NS<*=CnJ-B&BStjqV6!Q4ShL2s3IENLoF(N*pfaa)( zH!kQlGC_S2Mqnx)G#ASL9ws)@oL+l7`nbgjs`uQonSBNkAf3k~Q(0pktu4POf$-m* zh`izL9r`ONLr&7On(BO%Y19E6Fo8_nP}n{u?!ARrPnA5Y!iAkDwOF4`aG6F`eIjy| zdlmz5fc-x5`vZDRVO|5XY&w%Bla)`9QEBV)TN=;QM60ULGtux-XQB+agFWM1M*el% zg^!RRdQNnbgUXzL*tfR6{yv*Oia&hNnj@n)RhXEBcaYd(XCN#};Qz}7rmEQ-8&O_i zIxPA8Sq9$dkg`8Dx5x_^ou!`Yv(#zS%DA?O?`hO@F3fZ9j1`C^?mF&0)UqhLn53{g zbR_caTp@$7+VgebKvTrn#s#CpRNbxm@k5cb>0(LNGdm3xmz(Y?E$={ z?e}RIw=|r}ZGZ)t_m%vZapWR9cY6sJRgk0=nurc8D6daChZO*6{&S5@ctSWMg-kMM zS|pb}fIiXKw-{WgSZR+pm(YcZUbbDCY?w~B>R(U$nm0W-5rC_EP-22| zQa18X6txQC^onmA2Z?Q+YA!Ggk}!>a=RZO3`es)A)5|nwpkPo=oDl49vQ0aijgh}$ z2&Q;;5tbIzA?YB@LP5OKi-8mWK9Nq7dP__{Iz~+|5W;E5w7Js=+oEd$X}!~( zqbD5u^;D!VRS&Xq%;9WMV9&7zCa*DTS?_8dp^R_1P)^go3&h zg?ogj1lFjjB@!{UHkC1i?RwY^5ey!@ae6KWc$<`Bfe9b5xdWqF2CiOnPAu{ErPijD z6Y4(GRC$2o2`vI}tbTX1XY22$UNiYUBKfzMrj^u)dpEnng%xc6B&xxymrxi5VP75` zdIY>LM$pI~2q<3!=n&3dDosDdb%?noRaJwvTR`#BPFAT zuja*R3?CjU|8oX6$sRTtL>K;e^i(Y7a(My29rnE9#4?ewCr~Hs_f`a$B~E*|D7F2X zjwJ655BGtm%ziiVte`7V{QW*IyzD5=45I98%>jrhx+|V2+ll_k(`z3ivX4i{4J%N$ zDcIi=8i_D=7X;$F^uAXxLWfMC66`m)Zg7*)XvB1QWXvaR@r)n03hYt9@9yb?VekBy(F@9{(LOVXIC> zNAJM+5JA4BlPunFEgnAlXoe+~ZFY6tfgBfBz=?|ZoTS4o^gA#6hPG$?f8*os{VC0o zQ9H4c6KX~=)ALWEgPolZGSobG>y;i}VVN-anasE($HsRr%Yhu{SW$xc^R=VF{BRZ~ zd`&8SKZK$Z{vY8H9N^y%6mq`q;LV#i59+Oyc_H-3q%-;5{0pdnEvOXJL4~~~ ztc~xHBa4&DRcM!BzEc3N_CZc{XUg7;rIqcxDNt(1H_3~*iv(IyUEK|!ldS9jo1^)B z?-w>bl=((zFrQ>RUgxj^{I4AI0z?943C4G|NAHD5bC2S}7<-bd6!2?XUTZ8sqflLb^%>d7aQu zS9osCUpw_KUtjZgV{--jN*%cG-qovDGuf9Og=MnlBjxXj!J)~RaALP{E=TC-q=B{| zCN71Qp?;+^8bKG*#{*Lt7{xy&MT~yh!EBH(g~{@Qq`|9~zqs+h(iD$CSdh8yg*?$y z9N|tjHsp3L9RCU+JAElL1)ywhV-~^4QIt`F$d(T8GYqZ4p{D2s+mg zlycO{OE}NbAkbI5p%ss4JePcymv@is?3ei@J6kAtR*rwBMjsw6cK(&uKjY&vboJK5 zvU^!sC6t#(6GnVvqK<5zRLW@W5%UfS7KEYoR!z{K!nW}3a{+Z9Z%3+GnbmaF26k;k z3mL;-Mb*YBu@0FAO@h-^?$Sr@bXY`0TS&&_xU&x$uS%?EN|s8PSKYgQ%pg%)x=7HU zxX5yq@qdYyzXAJ?(mf@(TEDDRW?#<@;XU5UxR~5<`Ah360GH^*!jL+YU$XT% z(d;2~z#F1{&x;_?m3WaQw0A~h34V}ZpGIU6{ZrN{9=)UU-)a0Ag7>?ZTs?x<>~U*B zw04P%Y*YJL*4o3k`G-7Q`H~73k_=M-i=kmv*rvyzTR~e&` z8-~OI1t$)U-7-v&M)=w{w6Xu=MA+-0(8%NyZJp*=wt==m(&eG!^09}Id3`mBg6h}L4hq~0PeSajsMUYV$VNt5qs@-fbZ#KY_kpi zn=EsR)ayF-68-L)`^79yHaBY1fhEI(&-<>yv{*?fhNgaPfR#Z!t2E3#1d2|&%W-Tz zJ~DcS)1DnW+NRvlT7mUyH;#I#Ku_j+$M%Uo_ceA;NAJrUmx|6usLsK)g-D$fMIgD) z5BEdW+Vagtm}~^4@VCF%v{N*+E?(-{i9*jPf2@L2 zXX0#N81+dQB4MXtr~sH)fJ7S8dy!pCxau;KGPv5(fsORWS^T^)&C*h6!`dx`kn*(` zVMJ$}7YYLgjp1?kL{_B2nzz!DSTySHpDuz}Rn(1sSVpC`D{J0(I1gN4rMHy4L^BzV zI$uOyR7!v0>*i6@w@&AwktkafKCQ#~eBq%3!~ALZJ(1Nse8GZzVkfl>lIWEirlzhj zWNguCA65WQBU+g`XNX7&m#1fqrA5BH{C|VO{2CxnH)2^}g5E`Y^39R~dfxgJz@fEF*%_sx3#3*Ou~>)6mcmAtC>~Jq_SJQd(dP16-*cUPKjF&>SWtYEL)6?`$aS_TA zRnV0*Q`6Q>eD_sFyKr%paE=r?Z+x5NIsczyQv$}tkf=g@)xIamx>hs9P~k0@GiMGJ zxmYdQumV~>!f)297`Vnfbl~PL zt~xwn{&i}Tx$cjJz(AFxeX%X0Z6|SzF)u9)?k1`R5i~Hj38@D;lhGHUNv)xGwuM$F zx3n5>FY@VqBJdulz$rbx>ok{e;zpHL9FXQ-zlc113z8$b&Bmh zaoW%`yL6X3+*|ITbdTvQ4=?@MxZZ8t_4<;{cdV^%md_5&|LpEHuBf{HbMw~JrBCli zGbwS(Y@(l9^I&FE;>o&pbvdI<$;&1;Qp>8VFA;7QFCDF~zi)k;v~(uHKeYM^Hh5cp zkP3XFLlVvW=gAOS3St=6*^0wQjyP{@N7)tAxgd-l8;wo4KBRj!97FdID~F~VY({D* zlS#avm8F}#U7z;itjtU?gWtE!)c3b6$+G>dn?ji(hFSPw9b3?zi2b6+{#gtnM{;{m zcaxy^k~?|u_U6Yw?slPqB>usyGu>bT+ft{F`}uMX zvx*|_hQ4B6H7HkizA-8qQSrsdr-o%cx(p@SZM@%ii(>q`xIufzeJV&i^iFLO-)k0B(|?0 ztS`IvmD<^v3vd{Aj|gx#{qM}M|MSZjeG=&%#zt&q1dViAC!p*n25y4I_u?MXgGTbK z%N{^xI@P9!wnN;?#n=RLLhsiW##Y}J8(yx&CK8e+ zU%K<_aK$DN_fw@OC-5lUubX^Re2dRP{K?%%OA0iHsdT zb1zi&64F;+<_g7b(5*fC9C7F9=660`_R%ioSZSmbB$R8m`3_=~->A{=@d$`*&-bcG zY)33Q@a@uKlMgTLcOod=drd4Xb@)wmiT>6Sy%>8w8$7Ky-wRfEu(tNbs$k+ylKc1N zA<703yG#=-kqm-giiN}_rCtY}&h%i-*4wqWLe}@^!#=D#jNtHw2CI@cx)RHqZ+1GI-!X zo!R-l`#82sJ1y2};`~{p^;R%;Fy1E_arfZeyJI{5bHJImrZ~gQA?H}`A8q0~N*rzs z9%9q%PpBBty!muZN!-5SHt)@sY_sx@MSk5{9&Q%=s)%v3cOOS$0Z@t^OUnoa%IlX_ zb6CfB^yuo9IdG>##*8^Q`lvBGdZJZ4Y9>7TT=%-N5DT^*JY(4M$?gB`1rP&OeO-Do zk&gYxW0KYtEhU(Wy%jvm6_Fu3EgLIOg=$~2C@W0OLz#Z$+=9m5UPgkgGyiC&CT49A zwkU_B6I-hWE!p-p$#Sqb%hPjyf`=!ME)EXb)9#u!lN|}B`}31d`>k!W_fDstgDaWD zJZio%)3U_s;ujwpGv#N9y$?4(ojGzePEGRh$W1$oC~TyAd97#s6Z`D%*WJW+d7DA~ z=4lc5%zu2j@^sPC14`jv-jbYy^UhkRp9cur$An+hB&ScuYvA%`H65L=D4R8s(0~A2 z!?Y51qS=m|--v88{lMAmBNE+IjUM}Ma;12?3Dyo-d$&B~*g#Je6_v?Ph z-ho7PKJg7s&J}X6`_1XrFxFwidWnXy@LII;%#L0AodpwfU>TspF0#^RefR%!q5u9( zgAAVUS0pYmf-K{&x+tdo`uX}+vfN-E9C7{rvfT$)MOe+8Y1^wTbkCkq^S2QlMtxU) z@d`fs#nj<<9NpgmI^J!3F^vl*B%!6>uf1Jcmh)kqwO8}c{rQ$NW(=?z@Uf&M3UbyN z>PSo~^s%x<4PW){W|GOoi9a6>x#Yx+oSvvMaryEy*pBU2=J1<|8H+6AQf=+;1o7bh zsd%F%QTzb6N60JGxR{)fdMGBn`Hk57R+xwbyngFNt~oEaq-4TF@1b`pmn?aeykW(o z&urex-uPmk#IMtQ-N{N79<9yw(+_=SM3Ptr#U8z;{gNif2t!@>is%tHh;c?yn$>hTT#`SzNE_@vHc{Y6Y8diK-iC|f)> zZ2X=xIv)B-{5zIAnGC$)NDSeRO}KsWj-EwlDXCmBrS|1?w!K5Ee%-#ugK4spkIRZb zhzXYta~y^a8H6Q6m#>Lq`WbyJ7zJF>nWq1HEMa-J~sD5XaJ=Qfu0y;11IEkv~Z6mF)X)?J2Sj7!*55Q+=x3cQ|+T?E!n<3{S0WUl>4e|sR;czMibit#mTZ)5pjAMEw3A80x` zPgtv&{Cza0U+qf*w(rQAL@4b8UBdLAXsont6B|ZegccI_(dbX5#;reJQs=^fUAw)U z$^HioTZ{KEUYmGZ%Xx`}w*$LfBUtYh^72~J&y`**86w7d)LESBGC;h(w52yN1{PL! zwCbmtFiz|^tvWt-%bxXZnnVvv$g8DHvn2l=)p~@7>NA5v9cQQCKtOvUZ#$RK6U-i2 zzH%CmiX^&weXAxL6UC_X+uukW()rU5tu0J%TXAC%j?XEUmU^=rsWtYqt?9*zFHnjo zRf@4F_sf1vRxQ{?ece0u^ZAdxNTa&5>yf@K=bJ~cRcGbZ7K{f{uYJ(bddE5s$@isW zHu%*CpT2MBrg*q(Y;v%-pF4Br_%w-=txH$ve4_RCB@zBSs6(;nFM9mH*Kp5y8kPiP zLM4o$fb1TSB|oq_p@Rn#nZO_i0s^NxKHi&?k24cMSahtjE^K;DKp`k26;gyyl(Ks>d^Ilc;>nr|YeFNZ@!*5=5b-0{eJ%r(6wrvfb z9o_FFCnJ%nyJ^XGEMWw^1H9O*yAS^FI}m?omB}_*s&nxmU`|hQ7vy$l9pC%-F%!#SWPiDDh`x0|1b377^@8a!;+wa>LQHmB9;4```w zZJ9ypw%qSUm9)-1hq*yJI!H*)N!{+LvFYhi1BW?fg+QgmHVE^DWFvWw|#{^?Oc`#US+InO1k}l z;zv3U3xz_T3s3d?SzllW+8gtC@OFOw{_-`K$k}2ytws{^ejLP!{N;8qPa|g6)9UQ{ z(6F6|d^ryvj%Z2l)+nS*ha~D`=>ngFs|KzZGFt1vkOfbyzHWK%Zg$3N!~KepKewGT zb+D9ZX#V(FtaXeZ&1mS;F)>cCRwBE%`OhJ#wqgPeby_qDLSU&Xh^;8AfyTx*_$g=d zgvB8D+Q+9O2XcNP)HdO9VEo%Bx~B8cq^<(G#XNnYOd#kr4h`q>(w#9_=wDlXrM`Mo z@qHgoIdKasHqkz{sVrpln?g6H)Qb-Gsm`5oy-y%7Bl8&YfZYO z*xk&Aje|+X+WQ%LPD1<~<*y?44Se0Ktd$puL3fyZxefH%qNO8;3<>&QT%8GA&iVTO zpX~d-R<%ML)T1yM(R9&N@?DP@k6u0y6U-!vIB+GWm&h7v2 zMn#VqmAAokz?!#sYnr48do;1E8tBh zPW;8(01nIM&rzA4j=BU&abi@v)Whvvuahljlz3BQd`zg_gfI4dO9Lj#V2NjQVw+}< zofU5J%=iVgxU`NlTC**~FM#QN5^lTpBZ@g5+|>t=6gb@~iFZt}~$4r5t| zG?Q^?_MFeNWTr-zJBSu8>DPHJ%}7+j}ghbZO#%lYh~hOtHZIoZ{MePZ#(`F zm+wc@Sn7M0I^%xv9(wY3(dXy%8#?sym%?jID3+{A(_FX{0N}noK46E?DjTB^NLZOr z?ZVS~&tOg_09e!HUkDcIR6lp>UDX~HV5_P$NzVQhN4vhugyo$fYv*c;m8^NJv|#WXkrFBrHH6( z!d!!@KKd`$pNz^zeQ@05ME#-9)|4{YIpzD@#UrMU*^iebTV|ZqS{oxrC_+@8%6dbE`gm_Q^kG;NF!uRG70$V z`Pv9(S*`Ra5GJc-yZCn}qwv+PugFcjI_-mMrV8uGOU|`Mg zO;s@n9*#=~&lHa=*}>4X>G=+8N?*Q^Z&OJ(Sn~16UKSoaF@8bV$bPfFwdyb{*Hloo zuQ#tYpce7VWCl$A#_RnYv{1+mfhK@^PV*%u2agI&Te!OgR z@La{eTk8S?hR$lG>-JQ*{h7_dZwj*D#d2PstMEaQN3^?-T>qE(2O5OGH=F0p3FpBY z=_l6v;rSIMc&`SE+LPQX!FznNK?g=KV$rMq(LRY&4##YM8xpYcmn;t~LPMDZnpg9K zbTqO0_0nUf6H1I91N^otoI{fsNT{TZi*2hom8P(a=(_ z{=V)ivzf~0Jcy#>l1E=WZ9h&!#anCMpF0}%bTDNRh1b(y#%7Q^G*MQlzi@fJf=Y1? zWM9qagkp9nBWODdBq|Fhu70@ueA&JxiJBCpp}haA#o=+X3<+g-F*tMv6{)N_9kAf) z0egf0ylVMt-}cmRGZ=O*BeK88eonTTbyQZKICzdGJ(V<_ICKCW{n~RI)}j58WMk$P z&kXGaPK6zbUP{q>hXxIT?uJ*AEg#Z>KcSs_!P;VUPKPZw{aC`npx+x!K23*WL?IR6 zlO1urG7ObmiNK+PV6zM;;@rWW6~I451a#6+h1A#qB6|=2`mc<~jBT-ppXg z((v=`_Spz51f!#?`Q1t@k_`;Ht6o|_X!*?Y@CyWu;w2EYqGr z+ZBq&`TTsc#u<)l#J68ux7;W(!eVbXgQYI@!^6QD4@>5)=b3Ks%yI23H1ggVX%8Q2 zkP>n%6u#)(99$>QkMEV~cX(MVKS@(A@3;a+#m>$>!?%TQ8$#iD0QXI$+MUp5jKW)= zDracLXp2MRr58hkhgwGC9Q}*yiJ2DbUaU=CZo718yT~n$+rEpPoXUmTCuUPW`;(iM zl-QsG#DV`o>9wz|3a=|ZNuzB?V$Nm)Pi#eap)+M=+4YLo%~#+pj%Ml+FHtJsv-DFI zlN#jxIjZ`bnQZ={pYiyao5PfsGTs-vkN@k|KaiE&v}&*@F|r*E1(Pf`L}}RnmBkA7&&e+MORMpm7o(e2MwA^pu6|*;cT!QiKYl4wx^w?EHEmTst1(# ziOQ;LH)8j-L|OM2+X>F`F_oVuGj800J#%O!+743~_gi@V8UnQ2tC*72m$j%8nGSu+ zoq}T^#gQ^FOX|RBpo9PWDa1RzA=@H*5lE`J=Cgcd@nlGW-)Cs|YaZ%8#Y3C<)xyBrqUCrF-{q z%zpw@zj8v$@SmA$nfc94dCv*TC(;T2d^c{UXMUNi|Ke1@%B!-r<}uM%5IxzKzo5oH) z0b9sP6`;^I_3B#d(ij%lcX^>`{-*w+f-4P|gLlYTz*B(p+?V4Z_Y^(GpD5KnWJPV! z*sdtqlhR|i>(s3$`)g{(X5|g$WeGOOG&|2>E#5NS4BcizEMl=3jQ!!tAE}KzXAwXT z`1>!nOQoG_U(y?%{?BVm(P-Vs?)6x||LT@a<`ltACoj*$SjV$A`BJ8({dpI7BJa(r z2U0>dfkv*oIMWM)QfWnwuwE)y%4n2_0>NmzLf?ZA!mfd^b$P~+| zqZ^bBlI`@|@%iXh&EP7VyqO9`9l@;Ia%%QUCgpU|aE)`CMn{n$B`~R{t83tgoLF&H ziGJULZ?WQ3*IUpK4wt(=Wd>Snp@#+pjtsjxIlcO$Y>?^sOpHE_BL~YFTK?BDf%c9W ze2f_n){+h~0b_H5JhPDx2FRg))yqQ( z(6RuSbT;H_AWQd_^AhD6XMo*V_E_?c`lbKD50a}Oi35T*md^!%Xfu-MRC5e17%?;c z#Dq@W-Aj1lf0~d#dKv%g5UGNqP>pCZJ|ZQI^MZO&WK_g0 ze7W?_2Cn9PdlqtsA_Ka1?4B_*TD^|V6laK$BQ4-8ieARY$(~EOMbM#~u1YW@DYxtx zYuH-Whb;D#B!nbQM_R*>@)Zae{4!;1sBwG?L@s;UC=xeQl^cAkFx~K5^<;#1?f-6F zYP&&&K7Q9|+5RTBu)e2|8TB_Xn2+(yEM6(3QyCni)C&z7B)*reY-r(JvE7Ts>>jG~hPArLArI^WDOx9sm7N zC9$KAjYZq`-fMbmvW$c2YFI_9=@2HCUjB}Yr*w;>1e>6AXgTI4$ zvnopNY%HB=nKC+McW$Arm9^~5(A1;vC^PkH}l=2jv##!Eda>5ocj>JvU8L|-RX2#UmRRx>@FEnMdtZ1aVA02%WOh#V&@?oB5gN^< zpGk{D4cYI^hVBTC&E@FO#mEqXTH1h{VF}H z3mhf=S<0O|zY&#&T}zE>N(hVEy3H;%cVB#ay@Bwkwq)1= zx)8}Jg3y|nJfc*V{p9UhD5`ez_sVunnAtlAEq{*9^IN@!AXFMIJ^`qUp1=&BpauaX z-)E&Tp&_|=EhIR&#|_JRu#xxevF!A+I?~+Z$M7bY4Zg8a6TWy5b1(I38VggKu8zIl zhpokl{pN(5Ht9ro`=!BwU6d4E&TZs2omsV?5uAp+dQcbyzF67 zYzaPRnYfH89l4p{ol}ne>kv#rP6CkcC!2Rb+-0*Axs4H)Al|)8)Xr4-_H3!T8LE65 zEn2ZuO&FkZ{yucDM!hG{iKR#NT4IHN=N-PzEUh2k+R#SAhZGt#Y*$jERz1I*diSfX zXuKUN4+l7m*?e)V@2^?Ps2$~+Qo2ix%ZPm#d2~MH3!600{+;c^D9wezpHimviGjBc z%F1EC_1HKQ2y@Bx(bS9dh_>k?x;#BrPjTnDcu*(Q=dk9Ts`xc-2 z5CJj){#Lp!c${n7Z9-j-mC`b-e5d`G$O&}Xe|28CVbLiU0T^m`(@%E|jbra=YM{ZK zj{Ro#v}ye7$sYDcRxWN)rO~rzqWK3d@0t72+#z}CK;PdWMBe#M9px0(_|u2E^wL63 zmE3rr@DUn+3t(=*axG-ufFuLuAmK*!pTteRsaxY)?juSzgTI0*byQV755Zzt^%JqDgv1f6-|XP^OTE}zLt{CEGpz!W zq^W0r@Y)aA2fw%O!ue4+Y7xDh-+eyfu%C646Ex~bTXtd35%tTEsbX#n@js zAN||B7{}uf)J+>I!e_8OIw~6}n1e_$^=Sl(>FYcTYwUv2W*F$>+dcW+?!9vePw z9-sSW)j@RMb1kDIBllSq6n9+uA1%O#X?3_op>mt>Zx%0Sj_>8auPP2K&-xN$UVJUn zyRCL?oAUI8$uARkr{vZTe5-VT#4NrBg4j#JNMott*Nvk0F+|X$c$3$wEZ4tk82{;J zbEr`avu{0NQal4C1;a3^s^bx?$)4sRb~egLGTY_%e)0SEgd%%vZW4Anjp5~6IE zSoSyN`8Z_nI!bZhmp2g2wXIh&n+pWNtxLH&nNJ>9@qJZ%nV(cn=&Ug&QC?p_QaH`QF+*JSOI_R0+c>=ZSHPQ4z}d$SIxW>s9?Q2w^Maqn43lMwLt; zuM*uusP&d%i_oB+qSMgWccnDCO}ZG#AA&S9gC??DK_k0kI)Niy(~Vzj@3HByqClLi=Ekeo~js07e z0j2?lU0?`*$n)(eyLV^m;`|uyUK&3Kq_J7+)^ny!YtuO8weunB^!F5GeIHk2)%!9j z_ZXJbXHh`Q-dqXHsiQZL%sl%?uPe2LM}NG39*>-=CS$?67ZW1?^G4;bjuRPOkyX1K zZqCWTnrZ^|T4u?*joM%?JKjX6tmb`Z;;oZ=M}-Ly^Da90v?di1GaH!{jmZs?6Rtv% zdFz-QIE7N?xrOg-SZ(D{`_|sG!7LNEt{Y6G>)BwKDw&V@)_!`=;yO-C7 z`!<$uQFA+f_1k+!`UuR&+>4ycmzA0rAm}upfbSSZB91wuPnH&XW^idP!5VLbF5znT z+N_xK!|nI{jfZ}({!AE}m%wG+=k$zx>y=Ud{d=FhArJ2Qs4N|B;IBS!x5m!C12j~Y z8f34#j%-pYb*bt{x(@NZqx7S)5>>a8s9RPn_8<#z^45ROMSyjI#vCokhR&GysJG zy4KTt-L?betN-Wc)45#z$XM_JSq>{p067wWQ4NX@5mOMxw5EEa{U|l$*HD#57Q5dQ zRtb-*mP~ho;hN5{*pKMrEQpURl^hUJjYCWrGvY(y3kpsn8)GwUv{rzc?Wwi2{4$OmgRu^gFXr1$0tOa=u@q!}hA{Ng$M z&G0n6PrF{-cicN2l zGp-`u#0MahG+~DWWrKR=S_TFIZB!pn5;VEd%Bf|O%d#*vdHJ04=02Xa6VT4EGd2_u zWWMF|>B>uAc|3ZOR+-sr+Pv`i@_A~L+o366o*By+<3hCg$1xK~QFrjlrC}ps@pV_`m=k)R668;#v9S}3hkgaGAN7E25oi>nF?)jJmZiq? zLH+tIzDbH0!g#dpsqpRF8^00_PhxOPWo4!Ei!s0-nkOGf(cwlRoxwR%R*r6O z_MvC@$<|Ogm-tzLB08Eyp;=!9<#^R@-A5HJEP`=3_~1tH0CYM4f=e zzO-fZr?lMCMct^Hb+|_=q1XoWihH0&>qzrIlk04^@7*aL+bQb%XK$}l4lIXm}Ek-YG+q;i%xzZz4EM9Pe+?9%gU)M$?={6sEC(4C6$;#CL$9c)QA zqW2Ya5DIblUwmCXu}v^4n&PjMa`*LAImG|@@f->lFde8~haCjfG{!>Gb>CF6DBxKF zXXzbMgNp$pQ8@|%PrmeNv@@n3okMa^H8u@V1iys^){J64VRD(6$%f~1U`2W!UUvJN zBtDCQxMYVa$LcT6`oX}!3&KtE?W2!uR7J`ZRWPQu0J7V(okB4!#MJQI9KeN}(x7;b zb}IY)S^KJza-U;bQh8zHGM%F-?Q}gQ&x<49!^RM+e6p96<&P+N4q(Bt7$(3n9D!P! z#ACd$cJ-|4n=Y(>qL*^`)=>NHsQ&xpOai`Md@4L%@4?N}8xyv#6{Je(bOhkA2x#;3;b}?>L(T@qC@L)x4@=Z)lpWuG`gn??xCR1pcU3>OC-96~~S;&u> z;8`P_UT-zipW?uRWabR=2j|a)MQ7jHc@o`wL?5%5LM_YN(z=M#`IMY}60@3Oit%e% zWQB=y(THKOu;_d_9YT|?|9MC9*GTNG&Va+p+lBRCPL83U82GiCZtCEIH-~(EPtx8> zFSKOHjdt83-P!I0<|U)9Gl||c^f?nval@{2fAdPha4<&nJedodLBvG5KK0;_QDE@) zAa6@9eqe8n2Cty68T|av8HY4>%Afx!A&2=Ghc)#fuX`;ya23Sh0;B0LyK@*E+^q2Q z7DsZQt6E=n-wzzbdapW6W&EmR2dX#|TNPP~$C;jFvnE{+r9Sz=qMI(SEKdOasja1U z8WW?ivc?d?`^hE#IO!J&@w$s|HvxYd+^eTa7-B~0o2dMTzo}s18ccNU_5{8_AR>YD z+__MGN&YD&@&gMF^IJ}no`z>Xe$OgoTWB$FB(bNcd^YXtT5##Jkp2Q_(qKv9A#310 zd!hXEg@5tRs3n>_!G!*II>rC}XuSb<;&I!77hFYOQTA$kkL3uVmRugqoqhtEh$6x! z`Ur!1?`|~i@dPa5=<0_!anA*Gm_^-5IQ@mL9w;{jix!%r8Jq@LlFWj3L0CK2$^;hy z`P#c0BhGpT$4t$8+(Lq9HNGaA(S^$h)w-BT+%F#W^}*0)O`AsT&YwiHR}n#-B&kL= zb5q-#0`iyyt-AckhcsjnPuS%bd-bYckqfs{p}03sK;Ey0^3@W1{Eamy0#(goknb1w zzPiG0;SkH>fRThw73;7cjYhA|pTYWxF*t`gWi^acnop5>g6{fvg#*jX4n3;G^yM!G zO(s@v`z`AVB$L|O`t|BH`QC0;=!_)P=5~i0DjuE!J~Ar8m*sg?78bjr9cKf$&rZ+U zM_|42luUr?`^>!b_4LYHXD&zCsf!pLNNpmZ;dGGxZk6tVTmweC!bl(Nntt?Z?>g>` zY^0C>Q4r9%HXJy(2tHi;390dTEZ4~23iEf+WqR{Gs49epjw=1}zxT?kkN%1_MQ!iM z6C4Z4*8bYoMn_71L#;1TlYj>T`SC)W&t6go!#>NiLJ)6`{*_f&(}@s+^hSOA@}+Wo z{`jC!{af^pu)bhh+M1dsDB>_xyl~IPL9oVzP4REnQY^2~zw@02#B{Zy!k2MjmPFbW zJwCHnz+n!{!OBsv!sEEVX6QI3u8lk21Ci3bbRMuwhMdxy+)gR)9=)QVF|wAMn&JS# z)zeb}Hhz}6nVS1dVOzd7e+Wd{Y^IPQT*!fExO4PtRJo?Y_W#kWW0PinPWtgQ0T4p7TV-DS=A~JpLh<=nGNAO-(T!Bnvf}{b*f4=@iJlX~m zCMP3;d4wi@2k1Q^$!6S-i zFbGpO6{p9O^;7s?v}S&pmS}E{=$cqS3BJ{p;QTNn=VQ=kP?AUD0e(t4!zXIfGZv> zgbm6l9&{iQk^@Tq>h&(OY(|EmjjqU6i{F!%Xn%IxrOpsZkmLOd_j${ghY3HOsIVGu zI_j+bZhc#LB*E&YV}jVS-RB7Ms}4ii(w)DL-a*A8Trx*qj4;?UegS6_)s;udrgeB3 zb@!RtDSC85(qZwDrMe||CQbg%8=JEXto!uph2wB_U-U1z;^{;#9q#k#U@_D}5i?$t z16?3aSY+mUlsQW|X{n>mwTH6>c9(G?0VKYCO&eWIVInNhOSj?yq9(bTzsI0Mw{90m zDnc2-CmkPq!aH=v>0R7<+f(ICqTGM8_8|EdO!Lu~t8{_U=L(DFZfT^bnF|#rb<{@a zE85xt87n+^Q>U*R-9-T~{GCE^>;XHalnn~WTB=I*6BDjYCE(1GNfa6r#1h{5~W-Cpj_8sWGxqzkp>nX*;Lf@BwZ{)LMhD z`hXMd-=@uPk}iQ9AMKkJDnQmx8^w-eSdI{vqrQ@%> z{@QeMJ6_Z|QM*0qG3rVm{^99>RJ!oDf_A{D*DIXx_TGEglMIITkMv9cnmxjM4&!3k z14ijxiMg2(h8sWg*P+nHj*-2IIPXKx1kE5QW)Ohn5szSS_y)(qVw-eCGcQL&&CXcp zDfRt-e;k*}(L%)i?XKG114S+@S0R^)A=j|$`*oq_+q%c^k`2U|Ym^7G|_#24j1!zGXVKOn;W{(vik=PDAFXo+PU zZ4t#B5Y^U~3 z{9ZL4k$9PPCMp*^*hT{l(N<^RGm6uaqPXp9t3$T&pxm!JUn|5@#t2D_tdlrepo)2b z>;WdqtLDP%BYPpWNybQzWXvVXpBy04d6IXaqMIs1?Jeb3IKJx35Q#yrh z2$bh09_(jSl&(ck63Mot4r#y+99WZ4CZEEv`rBhpsk((C#+1@6rl{B(*-Iv}Nug0> zoRxj=udmdzXtyrK;!0(1$;{_|dN0N>|g9Vp0}8yvna#b~`Bft-uw?97#F zfhwvu)p}eHUoK3LYeCl+>+`)uehx8qZ*xrsq3RM=Brn!xE>LD7n_D*!jN*6txQgbY z&EQf_T;UEQ3c>NuqcrUvNf{BIj%E~=t2&czk6wM~6ndePD!?(`uweHmH;go0 zwCFg>^WkxJ`?3hw6Mp@*t}t!YYBpD#9<(11m+`&L^_wNyIS%wp-v@eyxG-m>vbvxO z06f>%2Qi1?@plJQ{QHOWbpBY3Yuc``U& z)$i{qhrbZ}pC)t5u#QXaurx_~C`fqJI`m-P-3|TgR~rZkCO3)zWl4ngh5SCw>#j=N77S&SdJ;Ilz|$)q4)tAl;s#i@j&_T=bW$$C37y zBsKKM(FdYdqj)C|&b7ozBjfZ_$9{TxPrvw+*Jtu7OFkaS^l(!*; z^2F1N%CLwDWfU*uN%GP#lh;s*M}~ny?>{^*w>Q>Vj!MHW5!m?sW|MbxFE4#e4T;it zpc(g0{J`%Q<0R1+^(o^dXT->q(0ZSYnD}{gPVZ0(awGkV?9v6O`O|dOMbCT==u>JI zkr8C)T&Lj>eElkeH#6BbLyrO~beQ7iQ`~pQ_N1m?WA>@zoopn1G&+VAD{~1dYoU$K zF)`j5`EC9Bc3Ycl|IJxF@$%u7`wt&DaEe6v&fh+~m%{-~-^s`BHd9zdnCe9hDsGfS zPvFLMWEralp4*h~9aVW&RH=+#&F|H#7eQ9GNBO3=LPc{9L7}u{+>YanHxMaYV;_m|%Ubi4h_s>`XT_+0 z_tH(<`~hnXHq4#;MrSkM(w#7&JaT;`tfluJ)TLuYJNq(=I@$SeG%K9!h*Kq8j;&N3({n-EKrp;w+h<_FA=L<-N#zQ0m`q zHXoFA`#M`d?b+B+$!icyABW4ys6s{Nz|y4tNXdO0?Nod+fQ8YWA?|Ejo0~{p5t1+< z>kub>^5MZ_Wwe2dBwD0+jQmB*zJ^I5+E=eeJ01573JebiTiTZ%)96p!SAS3L4Jjb; z8sIiy%y4D1)RNP+dL8R2vRau1?|u35<&v79T9Re)=EmJ4kDO;^uhT{}JTjmLfFxkD z)LdYCv*{(4?OV2;9#X1j#-!fyuj#vVw~ru-oEEAQCaKlX5XuXo;Kh)b>C*dSYh-|3 z%0Si%C2#spHi*>cK(cmOGh;=_sZ$^cfuHz)|NG-PWHnXyfqP#YE{WZgOHuRfuko!9 z7U?Z?d*54N+#*)Ow`_MDa!uu2JiA=-exe* z#h@BpF8pWP)m>SR)2o5OTc#@9KgRwBCOP!7H+9u}Vu^->MY_8Qr;{qk#&rb(X`|V~ zNffS6Lrc2p$Q~I=;KcX;x}gWE((2sW>frFWk0XBk{5kZM>P$z+NR(bDI8b$_8G0XT zqt=yv9-dj-<4&-_uokyg_nUpu{JhqH$A_&iZ&fW+9e!|i$*4B%?&x;;@09R0k@8NugH4!{Z8rpE35vv{W7>Xd#&L|kBteh%iIzcmVUpqF}%2P z!^?=KM&Y(!PXOvuX-t~rra=hdSoe`>nz06|DI-z*nV))LiE>!V@lbF z1vg8{cp|7|NI)mk+5@U%dHSiHkDMo&sy6E0w(W6{xwk20I-veK-+cTSP7y9q zWd8=7X+mnsB0TKaP9bxKwjIVNlzNgfx~|w?fxU+CusSmmaXWwYyXD5RSpjc7&`aom z{1GF}5N6trRpbpkT$vt&j-^9pinLDr>3`0xfk3uP^i%@(ZPfC>=Fti7bt-O*r#81c z&^+glW5;@)oZK#xu~N%;x6fVMeB2SL^6UB?H%2W_>fA-bt#|DL`<47W53X(d^scffVGER zV;Pk~F$^Xs16N|xLEV+!(=45%zXm*Pa_}g_v{6>{$NN;6u(R>K#wh4a^qbB98w%ua zvWzJSz4c8-B9)z70G492VP(SmjB|$s8f@2lVr1y2#?l0#S3@=lu$WG!W*W!VrUZNu zl74mgofU9zK?|n(rUgB2)7FA@vtU;)3;@ngOV)o?rURG|w4;xp%^1pqIG! z%P{TC+?wC0`@J;gVO2yhosi^|Dg759@%^vcczzgj@0BqfXd1lf6^^YYC;=;MuD?gHgg);;^rSCIn;VA1Urua1nJ9egiU#7DO=0{@G@kNjA zU5Ns}U65E;p7g}lnam%YTJR^z7`4_hHGzDqVsx9+*R1&ysl=o$zxFA>)PmaH4hBUu zV3g@kSj}=K;=bf}1bykf3TMMYm6bd*dwzLEoyrrSe?;|UUs|v^XrW?zK!OgtL#G1u#1W?K%@ax|4<{o{_5ujjFk z(j2VVe(>ng^Yn`lhR;upJ)sWV{>L8*&$wxnSpjbl<6lQud7jArjk*ptFvRQR-Ar@o_rMuuqImi1SLGh1svaZaqDm zT^^UnLj%ks$KXk76p*Oo8kkaGtdm&pW2<1L8$q;i$~*1DnjTOaQugx+OTdsLcIcug zxyx1l`lR%0f-fH#)^o0ZG~0sfX1~K1oa3a08TgwgsE}o{$4v`{{xA%xq zlvjV2iEt=&<>Eib!RT~^#TSx8v=a!UbkO%B2+7sVG=Mi1XCm1#)*6g3?*O}Q?0v5R zF{49Q|Nm~=4hE9I1D5Ihh6)f>omQa0KlJ&_N2URt>jLJXP~4Hab$-8jf1Z80>E{Zy zC?wS@-pt*B!rbw6prihmYhSU^GIArBOOKoJ`8QQ zj#j{d`0e=HuFiv2YLx@%_vIz}GDJloHxWJ&*MXy{$+A*cv{nt3c$K8pfP&J@h+t~s zOKkfixnJx#rQo~9&en$C_IDYJG<4=y$`pRc3+9K-#)D6vx}Mxm@Ffl=xPE{(@m zE?g#%Db=caa^0r91qzo?llrB{$o2as{PPAX9PX|;sceF;RrDI6(?qHHNRiW#q*o=^Plw`Nk(c2UpX|@H-P8GSopJ zbM!BBL8v2wFM-&`Z)@?0$Fv6JorD_zrP+j=Zf-SlIEYcMuXw0uR-={(D9F8J-+A}H zUTl_#GCs>HTfc{P6h%h)C@vR9T>G*MvD~vUF=|4p@;1D0(;_T7=v+To*s$Y(c~5#3P-uM_2%3aeX#8fSVOu$ycHlr^mho3X zhd(SfrJB-ep~&ftbe+ubcwq7ngb2JCd*qH_!pghz8`LWu9` zM_y8&0~RHBjatcUy<%`r^uw^zMvEV3JsnOvZ%0Gv_ogT(Hkv-PmxqT(v({DP37Yhr zI_-Ya{dedRGl?@bM0qq~b{wvn|B;;A&n(2tC+6~~10x0vXWZH+hA?#u8vQrEGorT9 zU3V_HXk&7}Fq(mcNnaW?YP6gH4q@yABB|0e_5>cyZm)Otc|uK?dRSXVQT}fx@f6sA zio4bG*-TxQ$#{c-!E?>b=Qwo;B`8x*`1UMdOH(?7D&N4|mcf52vk(K+R(1J9F70EA zSl1|djpDu@P_U2y7T)6_N%Ma{c@K}c%`W%g7Tvn|!y;kRi+8cB!03dVg%G$?aHE^^ z+O@MdU%W|tlxUKV+JBarE=sevmBaYsi`v-Fz3ebo%?Zy7*W(JuW)ch^Rq*no9R?C* z^gD?wmqVv6H@>Po>E>yz{Y+hZ`0yk3>fik;QZcC=^Ab0^3=N@BG_0w}*J`E6ssFfe ze{FkSAM-Bpa8x1CxRJwJeQZ6s9kzR3eX7`NdUnLeED@WKaPnQ{rV+|mcocA=zGp9< zV)Ek7{bzgCGdM274xWxqP#w9*@nh@5PPW)431Xv+?aCr?%?u$!9bU%onzO3J^{RDKXNL#qJ*JI;eoGm zK*>QVD1w#W2~)#03!!1_+pE`D@<1ja7}!VLNW(hM&{C6{_iKzs9EQMS}Za> zIsClC{`JHTN=lX8qgjk|aNq)e?=2pp(ntS<<0!^Ge(3;5VJI&lX*;;u0-l!j$pI*5+hCkv@)hc07+ z)-od^X^)}{*!tqRDDk@^58L`E^1913GnOTh=d@))nqE_;N{7ci*j#DCD9{Ym+{_^8w)fkf6y^GsF!2mvHET7AzlSTKZ7&xWJ1GJ zLJYq>0%3i+aSr&0ScY|Orr2&50!Th=mPi9!7Q!~Ztv0Um8E*2ErbHhkhhfF2Jv^{Z+_wMyL>?#YcJ(era`nuDdBe`u zQ6dR?=zh!46_KR(ZSTE%Gr@;Shd)HO9!A|+Sd=yK(7+Yf+MYW&@j%~2TIJ9h>wTz6 zL3)M+-MVK#m7A*Olt4G;A9kZj`N0+D&#ra!{OVc~b;m2I;Mz=uYYN9sEF1+kMJUS4 zfLiHdkgu#bnhLvbcg6OpTqi+SrOQK()9L=^n3^B0NpiL$-&4HaHM*HgPMH>-GuW}e zxImW1f7kl4zpNVZt8uu(?opnp1a@4m9c zIVP?xlOza6q7jTF9tYljvNAmWf3JqW%i8na`2pkL1CFp}dyh;JSbwHPdZrcAhcanq zT?(r~URpXlnI32LMhhw+r?Z8ZcX#X`!ueKUVwunwZms~hWk{!!N=w1j>I8_x%8pjs=6VCNEfbCpr0Il&8B@k}Wp+PV~|$Ny!7GcrthJ_0H~=L^a@* zBt%hGXEXiDjaXoV@aLlI;Z4R)blPs9tj`ADeGDymnuHtwXV4IyZJHJf}UPvjIE-kqT5SUySIqRq&Z`w2NbEhDm9hciy=EU3>N8z>|#2%kZ z+4G8*deZOzdOdotU%gtSbA6U=@hKEa8XC#QCG?(H9nEm)l+zx z=VIoaA$?bIzIc|ZuIxqvU8VjJsYTuO2?#D(tpSVtaxJwr+GR~B0_0*dOq`{ z%Wtb0?z4`%;DAD}!ac-aZzQCJIC402*I0E*6a(NiE z6UnyEzeSa-vNMia_hA=B-Y3&w>!oXN?Hsb?UYFhR6fJr!zT~)%p0-Z$bf~N0MdbQa z-P3IpE_U2bQAxd|ts1HQl9U;}OtJ4i^%U1Xs82G@)Euz{Wa6@K^F!OKPn(=z7?(fQ zX5?q6iZ#d^W-Ij9vg{GQKG(bo%rGT2)q5NlinHu>Tettb}o^!ASw%mt+lh#x|ILZ?F8B43n~W04ev~rJR~NF?S3LG?NG6b#b7zc79!ek zb)9adh&=iIBvtT`c0Z%;bdPrE&RnQmsHh5oTL~byw{0HZf9}OoGA}O$J`IQ1iEd~? zDVh$aIk!b~LdAUudzq+^eM^Y;r{Icxaf9mj)nj-go`!L5Wnu92Hv0M6Wv+w%_mcRv zgmpjyCqG|sE08`sd2zt<<&6vTf$XVEly@4^8!f=)GrZ;le9-ERBk2mnuJ6``<0*|S zphOEN)Jr@Lrk}d33Ter_5y6O5l?_B&{4BY+VWIWa>6$Br#xm{4R}il2Hs<%UXUu3f zp^?=bhD3B(j3hHBX-At5b;ufA*#`Bl%+;mD+N`tY+Jw!!6egq|GN?|2T3>yCC5Y0{ zPkkY@U4PGjA+Axih&MUDTf_)JUusO5KP$&n=Y>&ER1x= z_SAg^qMOKAqF;vX7_`m9laeQkCzp=S6XO!D)%3gsC(eSJ(?SSAVpnjz`ksMo3!Dz9 z&vwk9*7CTYu7@=!18tDE7;&NRuLsO$ds6WP``wIQ8BKP(R>6)d{=>|{pE zb$MB)hQZxCc3e9rEuKSU!5nv5MihWVF=@c%ZkhYLTPs> z0xKam`@~4F=R^iHiruwzxw}yY=VT?n+iuWm6MDbOHhDp;eZ(Qts_mxoql|^?8E9Dc3{Cl{%gBfY;}Q17Gc zAE>*UQ-LYbh-(g8873;2*EBz~0V0kiUU8&KglNq%E&=Uq)6$x*{NCNp(R=_ zWdB*GjN9rpKbpR!^{BI`Aj}XyTt4sgQe3-Utx)%2AjJ#xBNGP~igy>&)+HnfvF`xg0O+jcB##XbbOKjRIB_0C>N8+e8 z)!X&v)~1vaY(YF=_I=Y`dtT5ZLJ|_d%QgxE#tau1z5bg=5B~9X$o4<$dzRsTw@f{p za|};CesiE#%9cje)zzAVy`0Lw8#3V$;}ZkqF$3=ql@>9RfHM4KXc83qQ`a^t@4abx zk+^p<@yb5j#Y=|9;O@DV3)4+I<9s#}HAw4A(8R37P1x=$GdQ7nITGV&jHN;q=hF(<09F;@?ZLW2iaXLgUnGCY!L8E zeG5Kd^qP%BVoHux^gYj)uEYpJ~L`Q;+wMG@+WuA zOIXKgsSe3b*?@lM%d^k9haFtBiF*0oy%z;H>!h_z{Ug(VeLt=^?3R1T$7igQQWv`Y z-X}BFK!cCnRXAv%^%Ema+z-CyWC1&taRD#xP*|B$ft4}aTNvxa5`$1usq`#@W{)dS z3cpg>dUm%)BjCuZIc(<})Z~H2=(fuwj>B#PvuiCaS_dBeAyz59DXUcO80Uc7Jzmq^ zXt=xeU^CXlWB7@Gj>F)*?ut;;m9&2|l1z?eK7>^TM5t5)xMdVTX$&MR8`@vLNaDXrO<8z8PjubI6eN_1(>a zi|J#sM8LGbyAKj&AYw4IfBaZQ(R6k7u4N!*p57raLjdK2{gO!e?$oGni;L~m%SoK2 z6{RdY(i|~j#GJ3xR~gH0^>wF2(zTA_9Gydr^yL?&ynOIJ`8(mkI0L9KG`(kQ;XIysde9!?}c*csgy543E>cEjBDa-TL z!SCs`y8=7NEUrAo<6P4V7P0AnN1qZKkh(gWDhm5k?oR1K+u5+Y)=>#0W4$hptwCi* z{jq)f{QdTnTUVX(8%?;=<3PVz_smZ|gj8#`p}9st3k>+GgXe^TM% zqT_+Q`1|+o?@p~(H3xk88-f(gzJ2d53u3Cw%)vn1uf+t?$aw=&+9|a^;OZTw+E+U{w!6LkvJXcS)I$1#f+pTM9fF?ylvj+8Ce z4XCruN9E+YBKY!7s)4W=R(jVYqT}Rab8|m*>^m*dIES`#SMx;a<3mAOlV2kS+}B-I zp+!FGErK(82aH)B_&R@Lm>;Ce+>}8o3K3YrL$-IQP~1fFEd{vu#zJq4EZZwT_iBt* zuUSa=dcFJCs)7Cc-_P2FQPOGNUld@qvi+v{+~Z~?Hh+(*E>TAY@uN-{-d+y;CAlN)C_JMyX!&cW&LKeiiuR( zJbB4`Q%bTg-8U~m7iB);5RKXbG!7TM zy3>D=ix#oTCVSu=nQiY>H#Gi#FR|xd4taq1B;kxKsEmu=s8i-b!*4;9^pqybBE30F-A{OxJGZH*JtUufBXqay#!Glbf)kxDsv$is1UTy zcDuU5_x7Fc?xr9>(KbC%;XMWfncKE6Vi15gqs50b&9;$Yl3<*%GPlE4WtvqV>o0BI z0)o!4GTsmN<#l^)dB6=U<2!Wfv{c;%dY_fx_X`?++>Bc&A#I6CIM6e1a#DwG9@7BI z)SP5L#=d=HHm~{;=S*j2w0M5TnvM!bThr=m-_dMQxH-JH5NEME&P=6Y%udKQ;JK&f z8{Aj)%Rf&)e@5h-sW)utViSFiQq}R^`ZONG|NQSy&sFAFa|C(RxJtzDaBB}H++&$* zAKWQ{3OM;;VoX=FmPw|8nvE+eD&CRg_1gW!3hn3fglM#x7HI}-dFiM**UFAeuXKz2EmaO-_mSy zJGLqH9jM4R)m!0r3gqXNd&AFMHSsF}?5vQ)+^&|v97!e{8%EseDLDY6n}n%dLT zbEM&RN~UxCinz`a`pjnx4}B-Mt6n{tny&iG%P-pPdPDk_dn6PyfHHLO?e3*V+v2z;dU#GZR{!? z%SR8*Fy8tzWs!`bOP#7-jte&o`04pI-^mQ(;}aWng+n$adehXApSM1$5cMxL)SwP| zt>`Zol1XGF)6&%N zWrX_@Fb(?m=FRM;nF&9eL;U-#eUB|!u-uZcVX^SaU3RZ1x-~`g~#tr>)r-mwT`huO;tFPkjaZZ zC;&!Qeea_Y(ERzA5L@OHk8HRL(U$G^FzJo$xY>H{4ZbM*Zr9|}{9f;QR~jC%g-L$x ze6kIBOugJKSOhI=e|%Ze>7PVJ;*B+8=GCa>jENJQB}J<7hGfDhszLxtcHw9Rni1Q) zZ_}`{srF))&fs4V_3)%)y9-?m-*Ba>R7Ojg>>4b!mF}kI0p0SM)ZkdXjhAiKudi64((_gnQ9a$*LOC(lk}B8x zwjuVENe!)b0;{Tck7lO*r0u&bgH1v8mm()wAmUL*dQ=yWLa!^lfVsIM^0R9mQxm^uhC}}0}%a{oFaJAv}6qUKlHZ@Q@ z9pc(X?13~R+YfOK%=`Fp%I01v7S0V;R_^bMgmEV-M0UU;!* zZ{vpljFYO5sl3Di3sI-f;;fx)L$+A8uROu6yu|Lu5j#P?+v#g+?u>4@$dp$slQe2k zG-Vs^M|YxgI$Zoi&?ErCGxHK42dPR~aKkQ{Bb}RlYAsYg(}3w9#$3O_8!!94Z}i7H zJZ|P-IWoAD*zyW25RhUbTT*tqGby*rT?NmGkE}>|T`%&s$;0%yVxO@6NE$4^m{(9_CBHl3SK%JM z?#m%`)YMeaqQ(*15LQ0WA=K(YCs*mO`Gx*o@*&_#kfl?%Kh8B z<46ZRe=+Fgo7pr(rM743;0#r{qY|LjytC4(5eL}g!HI@4^+O9eAmtLEG#}JA&9h`C zZM|&Gz{d6+Qt68}fu?%t)hMORpVSJ#Oj*J36tsNZt0u`TPubtwXmRT!`X3Wz{5-MMqXfUCEy`apGd1|7Jt}Ms+5dCFZv4 z1|VY`H~*eBjd5ly;2d;*BlQmmKE&~i%Hr}_=O-SP^)tpfaI}53wLcH?2=ez2;|RW8 zJ`kfE8FQI(#f7VnDtt&MVb=ji3Wtx&8z>7zCAlzvmJ+>4&jmu02dc8Ww6O3D;LWoQ zLD&h&pz$MB0cN440`4{KFvv2;I0wg@={e8)&NxLC#e|F$?hPARSx8Gl(mgpG;_2PY zU|;6#&!LGId9O-~yA#BxkrhKSTf5yq6ZX?v_hlnR)lho%Crm|@4E|~|c{08nsbUrE z9PvlX7ms#k;P@b*p2IXDwCK^&in5N-QPs`sG4AuVM73gdOT~HNSdeQa8kc~)&U zd9h#r}b~=AE=Ix$!EN$%qrEumawzBvga3THjJ!nNH!Q=46+% zIM~#1!?))@>D3j#$#&Ve?zzxY=IZ}^$Bp&zLL}khpgEm@64F!e6AN9sY}|B)ryIs3 z*4wXG7Y*t36Z;DX)h=fAOeB2?0WA{50-BQna< zf@=RH?lxy3!)5AseCh`o-J*J&Yomx%6Z;huB;!-XMiV^tDf2w$Rw|D1j}x-cN;6?K zS1(d792MO8eSulXmPkaWN$kpL2mcmH4jCdn&Ry_u@1$_c_z?|VW`y`>It6;xE-Bjf z2oe6S{^Ab_&^x44Eykm>7^VG#mk!y9!{n;D*GaK<%xZRcIi=)9+#DGCc^7SVzKuou z7*ThZN>FJMS5n_+>NAAI0XJIw^T!XS<0f9)p-60{idBb93omxZ^>pyyzrGAVQuT4i z_u|A4!W9`?0N3xb-ws03m+>F9sOBB|j@c~yXXN*7Y}_ne`md{gbPQ@Kd3sbu&#Dav z>?wfx;KuBJq>2Lg0FB&R=l<~CQCXmrQxevJ`q_1|+wtE5i-onUpQzbBdOixY@InDJ zABQibzxlk)oNuPOK421U94@`5Nlw#S^tottquwkgrJ8rr8k~K;wMR!pI}%fChdT_C zZk&;y>}7ipM=t}d6$i>KZ+{n^2iAv|$mje!b1UW#e)jDrAGSjm&)y8L z%wlr=-9w-m7C=SG>G7mpUy2tXvR=ar58pDfR3-`}|?%aqONYlHD_~;oW}i?!8G8-R?IS=BpkCn|PsTj(7@xocDaBhwlR_(mzm?Z*!?i^nk`98x zhET6}P1R;NcIB9QAIDOkl!;Th-ylLIaqlUepXT+JVq}f#Qbgm(Y$qpEI)uZesO+xA z5VJ4(q0r19c%%&O6vxcB5c7jH6p>qU--8nj?Nl4M?xlA5^&c*1EZ$U)aC8spUJnjQ zHR+NWHuDF3Vx;oNy>8f6uKu&K3jADCy|exqET-6S-M~X1qtFhc`JC3~mMDeluDdL) zhi?pc5b^8IcFBv=SRdTAYu72UtH2ON=VDl%Wv!#D+m|8_CXONx@jbXrRdsC-jPWxA zX0;?=(M+xb)}|@};_)ivOSkr}{`d%+Xc6r(IyOf@y`ED9j>H@-3Yh=Fk13rvheKZ; zb;ltDrJJ}1ftmW!1 z4h~u*j-B-yHhU*qD1*+BBgCC&e8xZ?KPOhqWW4n&8y!uh_LK=W*}m9ncmmXY5s7#y z$EOwi{qsd0we%Siq%Y=|%$Mzssqsyy%5hG6f5bK@%E3{@T%`Kr-0|ocdiywye6=v< zas3K(UH|$3$T^oHGm-Lxq@EGhv7yUi;Mw%UO%p^F;uDilQ`tYMIzo7QZ`q4kCmqe} zu0#ChNc+8&%}1h?LR2?>gNiXb?9h;WqnpK?H6z5DF}0AWED)D;kou!8f$UH`&sMNE z&vHu@o(Aoy$bjh*&8xt(p4(VzBp8g?P%&7gaZF3I;1nb$`(I~=q9st-Imv7-jalE4 z>f=SAk*)qUW#c#mAb<@9=Kv{ANRp_-)3kfy>miQ?e!{yz_HWGu{||WZi2BvMgn{TY z4jQW&m68<(l2Rtvn3GPtbhU+R-c*}Qt2I9Sj;)8WRDnTi{rdesvd%lM=f8{l5eXTE zM2HF@RAlxOEm_%wknF9Jts$jUDA}`+kdawP(IB&sGRrK5q`mLwab5TQxUTE=*EOi` z_w#wb&v~8Kd7al8NpnB4{6M4iX;{Vy80C3SQ0dwlKY@QkbSzm|;t%Ac6d(btTn@o(bHAHKud8^;!Xs?IK$*DA=j`Naa4!Y^U z`*ilgDJk{)DP(j*)z}2j94KUt4@pqQIx!aL@3-8;?WJxcnNSpdrxfkT1yDv|utS%p ztBdC`>IGq|XB>SyukZx!8eS-k8oO>Q`uR59`iDsCzzQv2`yRM$vnok*v7iA6m(^Q9y zi4!-NJD9LwLD-A)&gd>^GcVPzVb|^n#P`9iTPkE^@a6jgSLPN_n=bn?%suuqbL3^j*w)()0g z;J+TEzP=wfdD;cD;0XT}M$)HQ6(W8=OSi7yp@k@cn7d!5?>Z~0w)+k7OogKpe}%Bo z2k!Yj&;|wcRa*;DD0>d*6)^X9)h?htE98E>j=p^oN>hv;3QV}?X^dc;F25JgUIP=K_+yQQ(@Cz zuddB%+n?k)brs`EfJ zE_o)e-AlPN!sQUG&T=(-pe%f%_(kUYr0vbv(NQYic59BZ(*<7J%XXE*rMdKW($MI% zU*pQ6LfHYxAxt>XY6;xDO&!*b^HVK!K?a4RQHpWe2|0$YY-k>JXNxwP)O%+<4t zx)j;0Fa4P(L+UDclIonRyIiOhaNRW3#R;@C;yF#_b%mY2Cik{-I)q_RU=%U^i^0QM ztgUC9C}34!5o(_t2!fWHQKN8ik^RU$pCF`N4a$F^mm0dXlvlziv^BT%+95~Glg{!B z{}Rk8-B#MX$8-ampt<_45GuhSuUWP@SV>(f=Cb;(bP~V*`nBRpIpy>t>vu#FX^u4c zk|k!5oac!?x9oFor7n0Wlky!{v2B!dHzr%6O#X zg~aw0lP?JZFWJHJ=-DyL4@NBX64XS&`gPz_yH(Um%xlg98YVf55R*!2&9nxK{8rKsSY;Rt?KPO*m8f^|ABhji9*9yG8tz`KFH1+9K=Ht!ytOSj9rW#|eS8B{VJ;OyM`^|DxGdL+KXuE&Ir z&AFyglL^F`|D(9-5Tjx;McQ(3Yr)URHz#4rpSVh!--j81``7hEqO-7bLllj2xIZGL zli^8(a~bc;h&*TDrBT~1c&tJDe!!CrtP#p3J{lc7L1+8+C55;oSywmHEBqbF_@;uFHLPlKZ<4=HGdj9t@L0QB6sLvRV_VqYLn z530_qJa%(a`DIRve^rMN@idDY=!-PZ@?1Azb_!+2<8m@l)ZV5 zntd0=g;q}`jffn3loRP}3^m_#m}$MLF>G}lN<}uA8M`O59PcyvDeh~MW<^T|^QHID zRCd0|k)qDzu+mUq1ZU06ojfVq=lk`5S%m46(a(C?#cY@i!xN2#o5Qrv58pZbnf8tM zaY1LrM``(}Z$#r}b<|VIeMZdiBmI^ioL_z~J|65fn9}aiii@Yl9yK{5ECxHNx?L z3zJc%;PhT3{E~qEOrgV&M(;2)A3LP{WMl>!owW}SE@0UT4|fBy(5o;%r!(h@uzpS9 zQ+oUl{K{fQ_PF@YD^_CXy6zMWk*GyEn;pjF9Ucecw(R+`)eJY}vNDyO4v)0I{I#-T z?1Q5<6_v)!+FnhYHS4eL##0irz84Xd+*>hM_#3O*)r?}N@>-_OQ{S$PU0$?7d|;XX z+dn4z`_G@t^-BogN7mF@SXy?&N(64iX?wZsjpPQBxYSHdex;xW4oKLX<(ngf^Ty*D37PY>Ahop}k+kctreZghVQ1adrCaVj$gFx@&ulBes4nr&G?br?ib0 z(ar0ddq)ulTOJ*6biuU3i!z%b@pk4uQeZB9)$6+HD+AO_5l+h3ShwQZg&M+Ii(5V4 zD7fph4t>f$uxi_Kwp+Ea=GQI&pM_UTq_{xy;YIlYVwh3VVRA)n;U|7aSSNGu9Dd0) zzqV|UT}sI!Y2@MFp677G3Ve1)!CXQzKyw_o)_MDtKq{t5QLE;tcTo6~v9X3?dT^Bq zR^IxIxyT;4XN{Z>Q1Z3j`yhr*)jcS_VytEWwA4U*{JkdEU{tT#3 zyFMxuZ`ut|=q_cy=cspTO@v$u03$QZIT{sxoWjZK^IA3>+FohBQO2ln){S4ZgRl6z zuA+jbtXE-b{wiX{xdV}DpC@a**>nMkQEX%eVCfaq=aD7(Cr_PPOd^sO_2T^|F>R4# zk7tz2&1ZG<7aU_P=;MrM>gU8VQsy0DmZrV@w+xL~I?_>Cl>wyYqZ-+CP%tQd9Rd;22jFfE6P&_h2RYN!vAo_Vap zE=Rh{+cJ71t4bjs72~ZXE#KSjKV)kxrA~0yq_*nnNn5H%z!?~mmmd8QdoAw$+6N!~ z24HX3QDKC|uu0#}*IJMu5y5o_aEe>`GK(+Usjz#1y+8`jIgc*%sJ1(`km(cw!eE{0 zd!2bST;?Y&iWf5~dMZ7cZbJ(5bkguS{eE|>@$d)$sPt`LZ`5wV4>=9;pIh)(H=lIV zSNiZIqrU2Hyxs~Tc(=X^qrmQOzWno?Q2PbeKr}!^!KrY|r^;!20*TJaxuzE+O&wVg zKFFw9I5mi6tWDtokJwn##?2Jgr-*Qw3Fg<`zP%v{$Y#Q$YSB3t4(Y1v+m0i@ zUrj!)qoX5h?5dGBMP&VRJJ}9plPt}FKYIJVY;-?Uv6iKbJnV1;vrMt1<8@81@G@1% zE@aS7Y)FzmF%9-;1FJc^_0=5#Roquy8aOsDtb5WXp{$>P-8%Y(rQ4?tR9)kXs z276ZR9|i(RE(IotasUtq=u?kL&HueQfyet2aUZ~K-R`;3jioDOIrGZf1>P_2xKcw1(!o0-IBES2 zQCZ@qLHNY65h3ifLHhuu$Xcw0X{Sfzx3Mv|w2xXTs@M}zqRjiUr%NFXyPN9@v>X4Y z9L)U?&~7{u5S*$kMUO7E3|59&MJ|X6M7fE{%i{98J0RbJIMg#=-le|`?b!FA4wF@~ z_7rr!Z(->uPKa3KkoyifxA`#+0P@4tpAISd(zbpsE%-8#S5xPUVE|6qREmSr^5e9<0T=y47F^`mXkqgM@_(IB74os$}-7pSKyZUmgw-*wu%x0>^+6}n6<6?82>PS zQI&^cgIumb%eL;Ffo7-qE^l>9xk0kl4$iweZ+7i-Uh}dq(RW4Afo6=`v`4IrQ5Oq; zEncmGeTFMkNl}SOVJIxbpu|mxnF9KyDJkq1LYmh$35l7FVJ2UQ=FxPSC#)l)5t)M5DDNz(hHMVU;Tl38#2=u-CB$Q6VQ zUR|6!xzV4l+)`t;8|3cDx}xN8k~fSP@9)*`+tOImQj8p>)vne zjpq3XM@JWTJeu~VGcq}SIS5+Bjk=hX42vLpmkqLAz>Jr~VMbkL`|Ni*A~tK)r^>-$ z#s`B9S@^Ygc4ILS0{A_eXuJ`D1^W+Hk8)lCkq!_~!PYyVulD8EvQr6f9GSiT_hPiB zrB%y$%mGQqSVKYelAn^^bZvZ*%R8=#_)Mcg4u)Q?7STOSW_GwQ649k% z=lS(|ws}^AFO$>Niz76i?nVhLF;cGu8avLgJ2{dRLAgM$ps#2bes zJ9gNriL`1Y*gW|Yi%q8R6hcusytATuA^@CX>O%I2tj~@6X3B=L?FE^t@CqX8YyZnC zo~q_715sAaw=sxbQ<~CVVZe+0O8H-*5t5t(I#j#;c)!s$U|Oxx?_18PO)V`#w6``N zZ$9#3*odZ`4|W+nK~+t=(ZH5_b{%f$+t&ZfE_3A;hDKIF-NGK6aa10t+xcbw&h^EI z4yQwHx@4B&01=@(e#({Sa_|KR&t&QxcPMKxezkh3h#lF|Z z&Wx=N7(94XRea2+aZ{#vbKB=IlMKI-Kr2k6k!#QW{TXx!xxI$b{e(f+LA9Y2=IFsX zbr`bgL2}JS=987bx@_LOIpoBNcz0hPAM0G?Ba_Z{Fxv1tt4>$;O8xhrA+~%BAde@F z#Zd6J&yF2Q#K(d^Uq>Bi?>uHq6Bwi|=(LaX|B1c-gGzciJ5W80vg_yV?Ts^&>W0A} z2918pG$^2TU1KIqp8S#9=}E*z!qdB;vMlvte(wnY~7MQ7+*HqUR%;id>Z$Z)0%4w`L4SD7AazDKtL0eQrrIgaXK(D7elLr8wtX;4O34_HE zbKDl%gMC-6TIJ55E53>WiY3}K1=4P5IWHKZFctT*#YNb$4{K-iIa9ibPK~dB)fL)>S(;V zmXgP-qJrNVwKo6g=qIJ*aTN-#qWnx;CCWD`HV+&p(}OwGAT+Yg^k| zT83m=4<^x=xK`PQIo-k}h{-Dhdf#ZBojX-~lB>*Eakl7Utkc(#w_U3b4y#235YxO} zJN=;R`01oT!?H&6cal<-o1+EaKPo!9plqnyC=MQ&=+gD;>NB)(G`62J#{)fahvBuy z!@`t+a>|VwiBv#UORIN_&q7|Lyk$X55_QSE1wA(rN;z};Q{EO77kgnZpSj7xLc8SK z6U~SjH=4qdnXP>E;X?<8gw%{(*)A4xr-tB;-DlT{OT(g~GyoBnU%Ax`kf4H15~4-qdEg!Rh@B4LwkJS@`$q+qWg$hqaB(-tPf}ha}L7H{h9s+`fKt z^WNsPw>O{VEsS_y-jVsP0wrCH9Kr^~(Q>e!|m3rjHh(_Y-8PoEaZ(9D47(DHM?9sKs)mNLqM{--s1iCOJ3HO+G($iB{24%4z4!dN8jqlilBl5Q`@}O^GYAS` zPA_E08E@alKYiMiCTFWrvzd|0`uVfxwP|p-S~RR$4YnwBrp&k`I2}F35{e+_ zhZsQ75F7E>`IE7+ag=j*Q$C_M!L!i(sC5#db1sm2!i!Aq#3!^>hX-gkq=;1EU>+?;)reQ z*&{|Yge{k6K^S;Yop5JkX;V5b35aP@u>p&$j;mZ*gfUe3#o3c4Z6%Z(2)|sBld3~8 zuEOWPPpn%%Pe(iC?3pun0oWE3I@iTIC?3~o(Gns6))(vH@e z+5Q6u=1{cQnP^$V)Q9&k`tdHhMT4g8>{Du)l~v&427YkI?B9!;hX7*Hs}FNX<@wun zEgLYnUUtJBi)Cc%Rk2?yU< zS8rHf@TtDs<0VVB)8zaU_gfXy%=$q2t=`@be<~hZ`1J5yu(#KrHxOInCQjTP{!9gm z9a)||hrnr?e&pG+jkr5{J$kfwm^Wkk^gnTC78VvpAJHmKNSnRF-kksTUw>Q}pitpo zNEr6;r~}Ke6@a9T;py3s^X{M@RQY3M#Sr$kq4v`akA7C z)zXEM53Y`_EjLt_^h88-37rIPG^z~aWOe8?!J4R!3{UNK|Ah+|+N3lfyt`xR%3hJg zSKBCJt}cJ8j!S6*_oyH@t%vWkpFF7nZZ#%*d(GTV7>P?tQlgKZy+!-Xt|LbRB*Z`k zrM!BT1g~(3UK*+jy49^G7A2q13IM+5>6ZL&cXG%&Zq0N69f0%gUQ)^Nuz*6)<*vCSn1tlu2!OQ))*(}HhiSLK8f}V7I=f= zKLVtk`iC&#W`XXR5R(G6OoX>ta{oD;1Y2X2uLbaHbf}xVyRNF~!yGR`+MA@zSin=A zwt@B7tP;=A_WTug4ErR(MHs?0gxolF?p$LOe|ma)tA?c>b1lV}to0I2Ze2Vh04Gag zs;bJJY08ICLn$|D@-?))-jpd*%r;lkcKbvaSE-0at|bo0f!Et~=&*x?+73R-%*ExJ z-4~f20ICodb|T7+OnPRc5rw{VSn zkgy-LCTn&rtgN=5jKNGmLenQFwmA;OB&Ns9esae_Tp?GlPU@aMa{m0zgioag4H_U) zZCHKFLw^QHl3vg@!mQn`HDgvJ%^WvQfgczWbBBo&iyeap57s1}=e)nwpFF11KmQ14 zc@Bk91|#X?X1OtGoATnt=Q>_sn0aQ*j87`=kLG+g7PkNW$K@9UqsHJh`VkQkDJ;Pl zFmRy!m;tU;18C-VcIbWa;K8;6j)-4nFzf{o-{w=Nwk9kk5=1&{-$&7breJDl_D$ga z{(;V6)^Ayw?gds7P)E`Gkvcg%BBJGpx7grQ+uWs|vR}D!C#{gXnVGY5f3-i6dF9HL z-4BMqm^(TR+2!r6L|@r*`qkoR5i@V8_v_a$=hb;l+F7fZ=k{IidW}sDa5VB6A?@5b z1@Rf7Zo2e9M5+vdsG>Eb9dO)fqrU2V!*evGDDh;9`YxkN7ET>@ z6M;Ch#N4=y3=M=-Xm46?{006IBtK#XhL>1g{DVd$fP2^|Cg$wf?sU3ZF$@?-(=(Ok zh)TNw%~=HcbzA>H{}&iO=N@ckp*y7atlL9j4lK^h-!`_lYw znwpx*Ro&(5c;@Wcdm!|jw8x}j|Lxlw5zIn(+!F%FtY1G6@t!KTky^JyvX5?2;{)f< z_k<^_H)F<(MN5~Cs@~7d^Q6(jQpTB73jLp+ik%eex*0btQT2!a{C6r=ZrIQxEG%r& zyuwe3joS@$s>kOsDvTLt$Iw{PEGscBqtazn;P?$Hjhc(s5$ zyKMP#aQ}A7>PKG|KI0BMa)O{-{34DwO1@K5U8w{5VRMq|#@S?2`NSlWD5mcdjgLbV zb)uYJF(l7r_3BFqvsKU<nOf+Z)eiVlqj9{qzOkY? z1~|Kfe^g)kjon{8Bw{+EbJCx6+g7{7^j)2KFFxD1-vdz?pakB3@Sqt-rOW*d^vz(& z8~vn1oe25vW(xgeq#fv-n%JgoO_($WWT)4Fy zgtKuQiR;AGt1WkGnI;h$S7D(+_Z*|OHfo2ajUKHchEiwtXO&m@YkUcEZy_%OvqOP0L) ze&zY($&=TW{AhF3Wc?QTi10Rd!5)#AD*1k>osVYMX!7By!-myixLL9`M=kNgF@#)c zt~*ydI(l5W(g!(EBgg^~=H)!gOMm(DIFAp*)w|>~e*~bTUY2$k5pw=~0Eo_k@2B0KmEc$uWJG|CTFz4}KvWQ1k(!M+W-0C&j(sTd?)U6lH1cD~MZI1d(OXMvgt*uyxLs5r~NX$MV&W>4T1 z7~01vcoziHW{w0oDGhVVMrgF3J&(0jd$oH*$@+qr96L2vO=cl==h27yHDO>r=#leR zuA&Z)nKEPU5#MIV?b3VaeE;st@ACO^FVnPP)23Eb0E|u;0Gol|Y2U{feJc9>(HtF2 zvkaC`KR%ivLz22bBkZEpsBaTVE~zY0^L=vS_U(2^tSpFrDSpG%Qe&C!_P{@wx0dvRB7?Yh*xwwLEMgevF} zxnAl!UhHu63T}PR>%g~A%C*XUl26e`#_Iwpum#?spx)$ZZJ^%<;28BF-I2YMG@>IJ zcNagYF568ZfLbE2qoQdq!?gX4)1rAw0;~7aT(&4$h)|9Hqz-m7WHHk$#UMja9mzQ& zrLlIM*bhI}6nwJ8CPoVrnxGuJ4kqi|;jeiR6Lf!KBNLyT?Au_dc2IYEf9|(#4OZx= zr>BgCyd+QXs_xB9fR`29tR=bDta<~!2qEx9z81z?_b0lKEDBw;vGGj%b&0gpVQwja z`g{&-$N(g4nMG!h=mGeEu{<*F{{033(>C}5>h<~j=;6bAq$RH@9aR_kaLM0stwy>&rYPLc7U2s`kLg;=%EI<%a?__LN4@i?M5qBRkyezDXVA${&%;}f@t$NY%$dNdv4$t!b!bcUO$*@oHL zX5`oOW43`mlItmHuN`wC`fIuTa4Y=QgrDeI`dVDlP$E`c7}_rIb`OXOUQTUM?1BCJ zk3*5NimcVuYuD(JD=u2Rc-Z90t%Tm1Jo$Wet<=HPtO~B1SKL4$UQ%6}bypF8JVj`b zJvTRT&fH=3li%Gw7and8zqqdKcZhXKNclJdUG+@E0VHH$4_>}R&3<(#+P+jepIn*n zbLZTC=S`V9^+afBUAp@B;js$7B)51Mv#~YtIi5Eah^3R~-3Ddw>xq}AZeXhz2}41l zu7?6_ba>DG;pfjc@ffN=sRAyO56xFy$2Vpa%@e#pxAGG|MX!5zGmL#-{^WA)Le~z`iI6dvYDFWd0aQ*Rz9F6*y(`w*$w&A;b6LqWp>&4TOjYH2`%T?yh96Pn zEEX+(bF~{i_{Kb%=ytVWiCw#Qd(xUZPP(*f?snqo+U@KGNg|C17k`?+v(HI_+X)x)xX{krZKIGI;rfD z=2)-!K>pjShSi2co}*7{om~GHEXG`t51kOje;v{vg1Y$sJMnpug4=1hY+8X2gU(BB zUxiES3m2HI`>nvpuIk24VFxEKySL;2Y5@+mm2#NSMZc{tnL34%FR*0T*s;yXkLr)c zoZ)W#E-Z{^o-%S>fy00S1BPOUboLc@rn8KeOY&ztSACkt+@Q?QUqD3T#*g;^6#ITB zA+=ZEb$W+{qnw%>|(dZl?|EG_GEKa(&_8#iHsc2NoR#wrvZ zoDeALVV_=&b#XDKoTyKB$HLp^xEFlv*iM|ZI#Mgs1~V`)*tNf%Bzl2| z{rag>vTXA7OR3xzyxPUZaOo?3zI2A-4~&Z@){y6S5VWgWk^Pv1b*g0ZFrr}5a4Mz&>;Ig5S$yl{$$I3S!$bcI zl<{AJG%)K4dK*b2T!(IZ?$H-SibRl5&1Hqa0kHJWz57+zfBW{$q1)p>3wgjP#7>)s zT`ym~N_FaOvGTHJ>(-^G>))zZw+=fv6+t0f*ASwgG(N(@xd**V4^Q@7Bh1XU@z4UxkN<$AK(5FNq_?TaOy0 z_&YD4wFt53X?Q^<4F&Rj>ViglxSqbg1*@uQYII%O`ZVhAzr}#IZ$3L!>eCZSq&Y~n z+xxniF_GL-{_3yye!$3rMV7icXoCU|A68MgcK-l5%6!zQQ4SSlV03{ywz!5p@YAU~ z6!-;liL%W(7W<%kkGV`ZX;-H@669ICP@k>Dbx2}jIH5n+t*XqBV1s>6)#?6;~Qpe|Y+XK<#;c74(_9N$Hwy&gR*>&%`o;`a;Vjm9$;d`nYb;q0Q zFog}7yIuYB&E(KVr>mw=*ntD39*ICQbYbOFoz#hwCtF%uD@)2ngEwMQrt8Kg58k}F zdEGWzceWzJ4>s^9d^lbvdjx%na1MCkX;+$P3|Q3>#N^d&V%Tw*9kuaUI=y>0<3F`3 zkDrHr&YVLN-Q3r@3nwo9gFQG~intSGb<}k|aX!`kQ*)ctN45Ik!2=n9Nhoo@0O!E) zg?=m3vv=~(g|p}mI;x$#1IphyjMMEsOf+SE$2NrCqb7wot!J<97Cr*I7T&!R#0e5= z54hUGw@)aorP2A9W9Vh)gYlj6wVp&3Xs40YV>Bu4aS`*laG@8!PzCY~qH_F_MqNlp z?fLjEk_+Tl)2T2S_ZQz_c(LbAq5A(`UTw8lWWDwXv?fgLs9NUYwh#7l)TBU*(48R{ zF6@A#bU+j0kpGB*wSk>#gQ(o8tmkl?CRgKta_)WY79Z;jxT7vE_mjs8KG2_iryVEV z5}CxL;(vPdXiU_$i_pd#NiR};MDWy-k`e<$!;c@LI6uwg zwa^-!I1cg~xM5#i1JQ)nN9|*V4b6i$`n`HxIDg?+cJ?-^!w_CCOjnCH3(9~&Q5#Ep zA#d;|$N640&{g%lk4l~m({%|ue8;)q}3EhPA}PB`O^3D%e_ zD-H1T9AZmL>f@I$GT7!D@uaG1lRr(&xQmH zJ3gfTdrzJ;Au6jSuNC#)y|lDe^eEv1hEj!kJ%0rYJ>J&V2O)(};dDTQqkcgB`|`Et zki{K--iL>lL@JPx@20M<&O1}h`&PR0!|h=VR4$*B@UM!_f2FOOCQ()2_L2e_hl<6Q z(A%fal@i<2l;#L=;_lt6E5$A;$l9+{bwOLE>4sp)^l<4=ElRtF5GvX|8!>y}12ZAT zAT_Fiv9TA58SDErJp#zc%idlequ+l9>BoVh*UsHScQ;|ckhL*y+yK~ZWpk801v|m~ z1mW6cmO%`iL+U<=btUOr14R6)KlbCtNjs*l$I$NIp2Wo|aH$gLj#<3=(Sq(FiJmu{^ZRm@fmz`mjXud?eeG1Da6XWW8L`H zCI)g-N3X9ci=0$aNi_cdIS_3_?zSd)@;FVmYjX|y_O-yQg1*$ikXo~GeSGfEtG^WG(bwNX+{Zbxn$O=1DMXqdX1D3eHZPBB~{^-23|Ve$;=^ z@&P+HRKuu-UcRhG%lRIJh4d%|l+*t(hxuR*yVK<{dZz+{g46(z7SnYLZoonA1P3T8 z==ba?V`l7QZv$_Gg=s?qd_8_@vzv9!aFS515I-$pi6l5k?L~0lenEXT?0VQ*>Yq6v z+t|t~M<=H(v|3bDRHWq!7R*M{O6_QlWA%7ixc>g-v_wKOa7xDk`SV zq;`ZkRZY#t3betK2)05gz`&{8ym?cKK;U}mu-0E0YleUGfMv)cE(w*hBJ)lFtFdWN z6nL8$jz@8T@X~UFKZ@pI^xS_YN1Qs<8GV<)DoVEyYB8s^K{Ihplp26X zVu8XT^SJJKJR5e>jbtyW7H?r+&K=0`+OczdO4Hqoii=Y@DsPq!%{P5yTC%zF?b4yd zIW`P*yI3bpmHP_)`U#WW=H)wMk&FqA3NBYhHZO9b9Lrz86GwJ@N<3JWy$o(bSw$r{ zcU!)X&k!p!vwO9N`!s>}mU4~0b1K~gsa7XWY(Yc8)7#tXXTX3ZOO}W%15W!l5&;Ah zp8z83=)UldpT4-h2C#AJ%_Y|rHK&v+ua9Yh8wq?0<&xsu&MMXt#N}~=OXd@KGY7j>TTyO@r|05Bf-Ui&X)12F;Zfa{3k>2e8XJbFcxsmj%oN1JO zBXLBHzn52icIv1QPdKf$YZFHb#VcYWxJ%I+zzs*gsquoG9hLCJ5FF#S-qSZG<{#mm zFI;E}Tvdoww<0nm0_Pk#JnF+bhK{YMqMBc|W{nl-A(eH8Ik~xJ9h(o8Q<#^&4XOZq zQm1OV0kbLf2^`lBMWQrr3on(xja=G4V+bw$OxGmegF`#U@j9nHIXGklql5+w?7eaD z`h*vS@8idhO{-VJF4PttDL@0YuA->hugut;GGuGIchn`0lE^U#6D`-j1r;UG>~t&b z&t53nfGsL2D*kfiQ>>^!psTkqbdd$r)a%_F9NwRa-ui&vQOirXHRe>ua+Ddi@XdWo z6pEz&c;j|x|NgkwuiMav@;-XBgD^wk<8}T;+L#juphm78OQS*wham2`tAcL$?G43^ zNn^syl`&&GPzrf!=h;C+zk3ri7@Q;Ia9 zmy+n@KYG+Ca}*>Bf=eW+fu0CYwj0Of$hc!84ujdOSsfU2zJ%XW7wvsi8SGv6Uu0Y4SkU z$P1>0w|Avco4tn)Ro=?j0aX^73Wyp@CyZjx?Jv;d%ZJMIlLFo@s%8RtbPdBuSqZ`4~L)rtUdt zk^!{s@-IyDeM7%fc<(#qBIo;NotN(-r0|$A0S_9lZSh}XBmPVBEy9HZiekvSJ&*nA z(lmrJNJNT#3DXywKf@zLH3C>PQZdvVvMDNKyKbX;bpb-#c8|M%bM^NvNN2Rk&>}bK zXrX(9P-5WtDZwo!+qW-6-W|-$%~Mgvl~q(YjZc}4N1>{NgM3Otf9x49Fe-w zPYFna&u0Qxbl09e^=To4`jclW<(Hnz%Zzn9^5fVaLt55R$|E8;Hq!enO+5ZV+Rs90 zcI1L*KxzHPdUFo1+UZCMu@W80?|Tn<+Tv~?N(IvB45HU)xWLr$OZT2-W{CE z>lBHGXt*LZn=-TG9O`4OCpFmq{dzCvj<&I};)f1=T{PlDro}A?DBtST!p*AfgKTnh zbNA!^WxHLs<^YK5Os`F4k(~2L4i&#>L{Y#W1X5@uziE1ZISLGy9uP-(4)e7}eR{#1 zn^5;>u%$>8iIF993HCH+m;Q|X2@G7kxFH?doV+~qi4(6}zZ63uz(bMDPIwS@g67RS zRGQe>dq7)_e7EpD%%LLxrq=*%?Kal|n35TmRGPxli}ja-+ll1Ik1YUrt=pz`@6m%* zQuy7>oIP8MAR%H7+C|4hLh5kA!q1#ZtI3a%Fo&s{+%v*)HKBy!v-px z+Xqjd(ognI@J|EhbM{)5H^~!fJ#-a5V`qM>r4V2jlLu;@Rcf5*|N3)MhW14J!ERdb zPN$$l{Xjg>Eau;m!cLv?VxlFBz|61sHm}xG>Uy|TqiFS$U9B~rz#p&2KULtx%$_N!4<9s)K*sJSz?HbPe z)3oSz-77-DAl2;Hy?Y~qYw#h*ZCkh6W5gxn+K$}-i&(fF z`_)x2Pg9Q~srtxd>uswBlzL5g=QOR<2&`Sxr|J2f?F0z&Z#Pg) zNlA(B_h++{JK2>yQA7x!Eq=L$5x&%_(n|6??sf{oL-HN_1MeDG>n&Th%zpXuC-;;b z7*v{@keE1(Pz=jHzoUhk4YwasT*Of@u}_x{)$I%ImvL)tZzL+ZVPn}~qBi5?_qjha zW3~3~-D|gq-?R)5PB8wa<(+yhmfpCma$7LOZGcB;KcAubZi z18sJK62P($Zi&gAi5Ul0qh&wyb0Z+nPgm`Kg476AN^qf?c#laWyGX8}w{~ab8|0ZK zD3X(l2F$p&#EbMp%jeSm?Ae##^N=t;zF2?p2?z>Cx8$~eSw48+K*zdU!zsvZuP#zW zOQO3ZzmE1|xG*oIY*|E3;i{zW9Cvc->C;_lfzGkFhrv5elSxDvgfq6dB@E4Rqaomc zcu>@ujGSutD6@M+gNWzN1Xp}V?E{37Z9Msp5O;A9aPv24S+R-!@zH-Yd2$XteUh|& zb$d})?H}N%U3#Y?fQK~X#b^r)1qCiJjPYEA+$xIf12d44fc8{mtJeGKX=fI{L?DKA zXv>i!?WcCye3eN~NZ;ULRgIDBVjncGxrMH(2&1X0eTvPxB<~=pQA4@&H)#6Z9#+#E z$B$+pMHMrDz+ogP5Mg~E742@R^#A8EdhUOb|@wKP}h(<%*f>4G4Gw$<&D-d*{@jOzE1cE z*H`Pzp{us|5C*9R6Cd~6Ycy$vO!?s1vxVbFhVDE`dj~z|$*?f+X=7IG%+XUDO~`{9 zla51U-I@UAZFF!H=H)HWC^}6KvU!INPh%o2MpF?&rc*u4<3bl^hZ80RKGT_=w_;P5 zE?un0jIpG$<{Ty5xuNW*8#LhXkj-tGkeK6l~WtxdHatD8+79A+3k$n+-5B=YN(+m#JaU=VKPD!Zgd3XDo{{awQhfY|GoNc51FHJtXXcy#BuU$K9b6b8Af3{dV8| zza3NEQ*=R67;j`^y%lx2G)}1G9@y*p@QaXE8bG!n#(a>LK}vFD5SCWEVyyhjY}^QV^!?~(O(S2vCpW2&N~i7q{#${dp1TY|4>@V)TH|=S zL!=bEcHX0k)fN&?7Ic=zpw)VGbu=EB(y)y~wByJ$iAo!ztsct0oG@Xg_iRp2O}#6a zTW`F|vHp~=!-o!4kSo6s1ttwRF~~BCoO+oM2gSc#TGQ}RYK2E8?JXInoji5gG+*+P z=sXGIDqmugc53Z}e=}0n)~;wzLL7b6qzycY%I8?DNl-`bAg)cCI`vWJ!RAxr`S}*m zo$y799z)~y=ygWPy^5hmyPK;U0>#uvX))>I&d(n@b?Uwre>{leB5$IJ^JnW3A6+v_ zB8CNfrg}}W9`rpY$DRh0$|oH(IZ{!(@7~>FWq4HsQk{e)KP8_7bXEw0(^UMdxAUx< zBZ!EnZdl*MdV9v3H`?@7SU|M}P;Ak<|Nj+jurI~tT`VHsdUV&+uXNQ8n!|+B{y{Eh zA6HK~{_n&3{CUvsDvyZs=N%CXP?R+VMXPMhwXnDENQ^CCb>uMB2OSMh$}gTR8KMN4 z^qF6^z>$Hzf)D=lkI``y3K~Z9l%g*A30K6U;tAP_1%u z3d$=cDtYk0>U*f460tz{sMS9!}cWK=BuiHCA%wr<_JO6$Yes$X~5Hp7C3{*Ny`L^Q>dI;qfZLA9yxLgdV#J}5|(li8}XCoOSoakB$ZhRk&~f234Z9{%Sp|kQB1-4 zSKcW7)r!++&p!0B(k|fP$hndDrr|aq8;MPaOg~z4r(wQ)$Sq0*4XD1Qv$TGFv|YfA zh}e@os8|vc>mvws=n*wsYXQ?`De!c_y5AlBdTes1_G50f4lYA; z_;{+I<$8Fmd`KB?_5zLgym|8se3~`a(h{yr6z5y39Z6rB(OV*Z=bW!HwkR+)G@3>tM-XpW4PhBm|1KripfE5lKNo1zt5~hVMcXLl$TD6l6+u82{nJQIAb^g<+m`TLa~r z4B~)nbsZB8tfR<0qRfT`G1*Y0EV6FKY1{bw@H~_;h>Ign!3`%3t@%2=2Lddea9M_thaGAD*Wg+dG?~Ey99Zo_#Has4OcHb@%2jtuU|@Q$cz37n~z-ua%rx10$cZlF_8 zO_yV=jO_q%V?lAQp{zP$E1$%HIZgdt)@BRm&hx0 z_ZlRhtlZq~{OY55dBRIzND-fy=vALGV;(yzRUy~qiyw&X#q5JWlCro54}4nGc7>sI zq^(a1{qu49*@-;%``GEPKR@0ES@K7|G5zz#e}h)^Z#H;ZyVOF2`cl5x=>89~4OFSP z5^B7uWYz${Y+z8(7A@DZIw0^Y=QeJ~kn)107_Ks9usbX^#O1lpZNfA77O&WI#QQoAKD&;v=CZCTvODlSUr`C$v@)+YeFAT zREDf$NC!g2;N0 zXaE%$zB}EuvgG`@3jOBFQJ1V2ojrF>X7(9_Zb(;ken&UyumJfDC0h=M%IeGi4>6-%KMWmyP+uOu0f3#6FLsuB^9%} z^tIa|afw5#rJ)oUyw0sj4roGDa*C=t9z#1C6VAN(T{g#tG(qBO^T+hkjT`qF<4BEl zbMppWpP<6p6&P5Dz!BZXnXbVlWSQz?YCK6l0$h1r)Y}dnC;dHr45Et(i``Tw;a;d& zMSAtG6UPo7-L&dwPo7|r*QRUGf0fJdK}j*ar(fY!a9*;2H}4*uZ)!VZ#%Fp7BAUTQ zWBNkPEswNm*7>&|td2x)*XxA`q)edYUWL;Hnw@N=9}^xYC0XSx=g6F()1@+F%Z~AU zK+Hn)B&JYa83vn9!-(NmA{QBnN+ZC6*{D%|cT;L5r9w+fT&GwK4G&l5#Az5WX&GO> z51!1CI#9Pumr&Zeh;QcTnLWpG4X___=~8pFODbQQar+{XISb&CG6)8m205efFn-< z#ATk)5#%79fdkthtup#&itpRvcXSfz0njmNZ9o_4f|V-LmIS)Ms~El?jn&$ce*Kt9 zHDv}V`gKnQT3?3;xikV@ZUeu!HYO(WSU>XV*L_!5|I@;q~_S zn@#Cr+atM<(k6bF0{oN)Hl*M=<^_|7ZjD3AjTGo_d^$ep1ie>2&L`}lw*qbzRXcI| zWSL9U`ogeDGZiZ|L?#G-BGJR9Y&MBn=?*JD8DZKBxK9oz0|{j%YNK0`EOe_c(S}G? z9y4RM7R0Uav5dWRx-k&p*cQt-dHbQ+XxWO;+&6Cq;bqssgN+EtHj~;zhi70IDe83c zs9|#;Ur>+NuU}6v^*%lJn3MMZQa@j;=?2;}D~|D~YO+M9zWTcwnEN9=w7F0+uB|s0ej9TnHjECG zh$MW%pNb+_rjCW$C~HBpkwQg!(b7cKRE5X9;r~>e*F4XNXK42H%$&qnBtAT1BU%K_SA;W z8>SFJ055kT(jY({Jb5x-uXYPxj#sl?M5puW7ze3vMPyh2g26*R?eO6`xGCOMPA07h z*dA4Ee0-<0FQB)SMUn-jvwla(p~5sqc2qi;?C)>YBJi=GZnielc^MtuF4ogi5eaiD zwz|=?8juWO3la&rkpR@_$MR~?cbF41zScxZ5iCEM{yuwawCJC$zf{#)?iz#^@<2=N z%G2;zM1Z>=K6HSLj;9H*D=6rmZ>0r~K?eE>f^^;?^p8AzI8saibMKFzKK)^4<^N;= zpD!p{)uAlx=g;?o7?V~e?ezgTHj7uiyX#zxF&J;y3Y7~VtnnaLfCxul|*;Vv-f#1{tImBXvebKRcB5rpLnj(U3tf=mnUa9o$nToq2R}%h~)Qhf!{5W<%dVkM=~ zo1$O~;hw90=DMCOOb^c1PS-Wz-R;MQb+pNBeSJFUXfYt@EQ(S=M6^ibH!IYd@fbm9 zO!f5IU8=7)(kUAPA_Z8`ZqRCFWPND(ziyAvkz0p0j=w401?hC>_P?JGX^&2?F>|re z2S))c=FOkqiKQw1Q=5<2MZ;g(A5@J_16}Fran0;0QNh$|Aw0|hPG6LnBm1)C9T8HH zIoESq$MMzzi}7q=0PG%c*TY(Y4xQ+xh{CdfxMOo=jfcZg1Kfr1MqBL#;qDwiNzIqm zJ+lgqc$LgjJZml=mvA5-7jfNzD5WApo{t1ayE~iOFw^1`+ku(bzw`>$Oki&7uz`aH zeImhePvAwp>;g7}t;h*C-nFL8gAs=qYm6#`Pt0wSp?E@B`L0h|bf<)hMKj(B8;?Zi zojd!odzhhd1WscdVEFd(b~vfq@W0R|aFA59bzQ%de}NF7VtpnRC9NyYO_vs**P%`9 zy8C}G4chu05Os6ulE~MR-VjB@Cj{07JfOtr z1XjMUb;K~H?h!GjUxgfpBzn9eshZ=xgkxNB-?SzKvHz9phdgnQo{IAF{IW`%yXTzO z8mxxLG(m7NwnT$4BV`2p6q1d>qc;I6WfQj0fB>t-92~%N>(UOPuZR|B@a5QmDPj>T zMJF?ZQh{NFm>8Xox#3(@R!~uyS$lzUZV{6rxg60ZYRmm0C7Dt=Cq6CQ3DzE6VdA#e zmDfKE40#Px&~v9kf?Wlg;>0nX-M)RTtJ+Y!{{1zXwKY=_3*GRWThA{s6b>d2W9t`y zCtmV`eHRl+X0raM0*(LAFvojhZAk}HzI@-Gb;tEU67&_*L&A`kw>L8hBd463O!~)D zVIrB7s^kJM| zpizW%)}WcQBQ*`F^XWWpmIoN0xS(~9IqWXT#ma0Fk@_0$1k5a0gGS~p_<-QKOE`tk zlJ_^SnfE(?^K!;=1OT#$WZQ-L$Oc_&D>KhgnpDkce8~`eJOw!+(n=lT&=_Q=Y1qG_ zN~`8~195rRO;Dm}VnAaMD*bR9hE;Xz6(_ND z6u2{$MHSe|g>e%XqyN+W)ExV!+oD@X8xDCB-L^d2Pi-n0IobtoFnltjIrW)rO;D8SpZ| z*EG=~GKal);~2r9-qWj@y#L99CK!&38xdu?EEQOaEL`{7LWzuHu^?qKr7OkzEzQ`} zMUyxPVuvKZKn(C@9S+8@fuCypk>BUssr=;}u33tM)e0FfG*6Ho)%J$}&(Vsdpjz05 zOh$f*3zac3?C(xz#;2%u&XQReksc!sOy-I2kaE1=m>&b@a~|Xy0+-W9MX8H_Y1D%hw-2aa_ALT`UXX zz<>yt#S? z3^KoO+inKAGtrcDTx751KkSZ`{o7E9VnK;3>5 ze)_X>3q{19!-pei-sEvUD!v_BDOdH@^o73&0oQ&-eQt^=Q};oG4!Zcb-Ys{ghqDKp zKNOKmIN#G_He6%h(~%8YZ;9{nCgd-355X%=^ilWEEg^890qTlnx$Hk9g2_^#v!#(# zUSp9mjiYvg-gCyD${LkwvC&>MMd4sv<2&y`F}Du%=(2kRLaYk~KP$@0e!J#mPwf$X z?Nrj{@WIy{V;7+P_O%RPy!#~-H|4|cf8-8u;%Gn`8Jb_>;e{359oDa4kd=HqGRgg zaOj*pY!k-e`vmwfpiLhuNTRjjWz+zDU2-Sk=jrJ;&^>@nB%_LJE3sswUK`0;0{||f z#lB*j+3B+l#;-KcfNNb~r@Pc$7L+{i`P`lVgrVvb8`Gxg?M@Si4;szi4&^wq#n%mR z+99|g$Yy{ISf~#i8>zbB$}~dA1mtWZflh$!U6vD?Yf7 zDZtQ?)F43Dw8j>=KQD7b6Bv<~?sXJpO`(q8v8-?3!Vs@fu#cdn{gQg{u=X#0;bK-) zT-#u-u8?6gd-iNR*k?mhP2w=la;tXPc5~oHy6$b64-^l#tQ6xNicp>>={t=%*ms21 z=AuCJ#Dzu&iU{84KvrvthdIxX4Isptvm9*sCK5#1d^&+I)_GtMYj=unWmem8UR?HT zetWXu?{WxU>6l2NM$# zqFrrR_Tk8Uln~0eu=`;7m>79uFBM?S^{q`-FdPS^8=_tV)&0b9VuAkOK5)MpO z2ycSjY?Js>#QTWvUpmtHz^&;t>NpyNat|li*zDk+UV{X~=rEULcfQnd!Mu6w#2SyX zY${a@t8PYkUYtJQVsua@Dd5S0KEp`?Vv8p^gYkiICO4Cg^=F9z%nCkq=UE@zhaIad zL7&9$ZTg~l!G@;=J=OgXPMjj)m$EfHR$RyVkFjD{zGV5#RDLoWWY6&gmrx}X8;15g zU%WWd_HfO$?;k&g@PXG%_@dtL7ye1ccb>ousQJw%K90tw?($Lg+6FncB22hdyf~Ko zWBMS4xJXMYEAdUo__0TFSsU&>R*`9pp%~Kv*;e*;=SZth{2!X_}UNaha>8D@mWW?dJ=r%R97J*>e$V7nyg*p zv%hyqpZPZdt^e9;+cclbhjriKpkiNJ@0PuRMljy0Jj}fFH$KQUUr=f&Hm1NRff0*C z?VizDlC5(nh4HMNPpGUrB2tw{C*F3y?o_T=&Rz_j>+_`-W4^6kufg@ku&hu+f^13GfI@W;&bwY2PwcDwE7R8Ea`t7 zv>?Am5C+NP?dBH0NMHXHKJFuMa?ItWL-B?Ebu+IQdWk%|4%3-RKLamHYt6-GpPzhC z>QPCj@qLF^&4y*b;e8Lu_7okz#iSWoWjSlzN3ZG8N#*3h!FZo}HPi)I9SBWFiRn%<FGipDi^yu$oOXXK8%N&R;5`mK{1=pf=7e{f7Y5sNOaXQ%epUd68)J zr-5mY-o0aUBNuXs#nJ%h&h$5BSW{di-#>7OWHf9v@4Jz*a`=#vqQb(@8TWiy1uKCN zUY{Lx3yV#jJbE+{j9Fa85|Cg)J%G3+u!F`%%_t>%`Xy>#c^v`IPTe-S|08;NCkMFv zs2sD0Ce{TYmY%8UY(i67<#~=Xfxv zqU1??mbGNPNlPqY6R&D|4Akqpq>jwbzsPSEHIUs@1^2eAIcRM&yDa_j5euDX>6F8g z0D>Hw0M=?#Gq)=!lywG(^<`54#_NIe>=H5ETTOF97G;1**0lFa{op6lP4vYq(R`;m zKl|r2HRlqT3WE3EiDMp8L*}tZTQUba$d=51^q4$>|63fc*}yvS;_W}%XStTozvcJQ zqd|w1>e9@M{1e&29a?t{QnK8IZr|9yK;tW%T=<6Kct}N#j_e^^qnRgLTL6A!kgfOP zf7NTy;P2k=ybHIAPMcPb5X0g3iNBFi{rv>Q{`g)~Ry-?QO2H<{f)?dDfb8q{bq@j> zxM)qMYC?rlKy+t~0WkHITTP)BsV!p45WST>TuX0d0`F|__dkU~pxmJSk14p=fL6lj z){QwbXfjQV=Kz|bdyRV!nA}0m8@NkcRXP9AZ4OTFP6ChR;bE11aR?^5|CGLSzE&=TJB^hgJd@Li?-31j~_ezpSsQjF6X>^|F<#r?U6l9 zB-yenVoHc9OM|E+G?k?!CE2MN#-26|vP4mo28m?L)`FzED}zD|EsSME((-?w=r^hU z_iLV~nx=dCem|e{Ip?~rbDgzUCT1F5&1$2#UUn{JFNT>M9M_LirywQJ=o7;n!gRR4@2M_$$8+nwz{=tA%LmZe<;KFX??@Un~ z@aB)raxGhVVVarQhqaIXee(v$Fq8w0(j?Stl;@a0xS!brc0XfiY-i`h0;Y*L(6}ta;_b~0wD1GkHcorp09z?ahd=lGYx*U0&RFL<2)mb47FvTGkJ%#l z1$R3C{PXvUfKuS5X&7AEdoCP|Ozv6sk&aCDXr}kxo1Sf>0pU;3d!#WzPR5UD-?Z+R zb8y2CDP;UF=k&f?^zQb+B$2xT5}eAnwccR)7A>eDCzB}^+k1S-9i%npoNF+r+f(Nz zeZix&p@yVB*zD;!mBhPH5v%=98*&^!o{7Q9l2qXj6}wbIa4`gJoiu9ZlM3^&y1NX6T_dfd6Efvoa`YeN_LOeoVnF` z2xlQ+py+9}QHjaLEh~BLUcmUcJfhdqsQIewx0H@cA4+yc&-`zs%6e~NYDd4=zw)%C z^_G!0|52Ui)wchk=;b<|zKX|1b?VmbiERZbvyvM5J@9)zv(T0FNIE*~m5PLSDD%-m zv$Qs}A6|D^-qBz&gctLFfkY;FWjI7$4I8tfLJN?W{FB;LOUpe}vvKR(paMp-t02B7 zbN957#ZhjVViK|&(kKAX^!&CYo0Ibx8J~CWWT4Zzzgq)v>8?(MVG*NYFh7A2$`e~0 z#!K zcYAO+b55i5-6@{wBwwLHLD!vZ3Jd;pKTfSV?YJ2Uqy;$&AjXA;3sXqSWSWb3-fOS1 zSrq|+zQf1opp@`rh{`t3@~kp14#7$!Ds;KUb7JRE;$gonGY+E}+V*#DuJM(}CnUj_ z@F+R?_I>*nw6Gg!)ja$^A+mM(`Ygk$0tUr@M#4FmxaqA@!1Bkw9a}vq_m;wHY zTANGv(LW(;@6oD%Jz-EqdE@`XLIkIX7|Y6r+Jf~qZ5DwA!P&0Nr7z^0fQL0OP>7PW0c^)NAg6BOAYxL^g$x%{w6T%m) zc}U_tb?zo&5q^;dI+tJJ0&By-7w1@ep!Yq^Qx zB`MA2JX^N?Vr+cNsM& zHr`Y7_5TqpJJD6S=f341%1Mt7Fz@FmsF`WqwIiTz&plsp6ucL2=c6A_4jb)v{lFrQ z6FC%R(jDvtvvc&z0W=0vWw@}~r4-pY-3cYdcB%*}ygS+3BZo8-I!<;J^ueF0>o*a@ z&4E>9Mw3+GT3TD{^r$r@lx;P0>uU0p6%gt9t6Z3EYKJu3g3cP*dE+%({Qj$HkG_2~ zNE*s#EK)m%@^qvPGUiAya)Rrg4BZncDU&-6Uvr^o?=w(7r7X}ci5H}z`J~0u1Z^L`)9jvz8+_IcA9U{xIXwC?kHY9(E`z4u5kWq~Gos!G$ zP2`9pi;vtr-@sgT@1iF0OZMrnwj|dm5C5cvf~U@m6)RXVwPFd7!FA~Jopb#&_ch+S zw`$$G=itHSG#Qx-Oz_3+eL-e)L7Ro=rMa<*FJt3yRqH{O5SSeQ{n2jZLMaduI~bVE zfDonqHN()fW1yzyTqF+89XD)zAQVTR3_K@e#RP(pPj=@{x){?_V+Uu&z7L&MG8ULa zMgx$v9RbL@6&JU`CJk0pg=?Ne`u=r`15~b8UG5&$4(Z|~YBKVy);cppvgPLFJc5td2os12Q#M! zj`GwVk1{;~x7*`1>g2`N;~A8yF-hcGa>7%m%U~%SS8`Fi4HfhoK#V|@8e&jgDwcM4=q)U=3=uS@t z=Yj_w)i{=6Gf^M4GdzMmAYJ5pvGYiRA73)_Yu5sc4kXx3My8&4)6p9~1P=nv4L4$czU&jm9rCUN z!AR{HL8krgM<_*RvxZ&z;Q=knfRfJo3tza#*_;%#pnw09MW3lir0QM_f0LK5N47td z5hqyFhJ#e(BPQdF_}oNq79^C@#R4;lIiQlPCn_UTYwG~acc^{41N}EN-7;R&w4gM5 zGqqxWtl7e91bZ+pmB$48ujK2NRG+4{C+XH! zC=3!YL0=TIj92T?vtrX${u*LR^Msq{!ht<|ZoZx!P03{soM?%RA|c8I4R!L@iooqE z6@kxBKO{@wK|Sm3o!^h?9RQf)5rLfY?;mmVc^Q&8prD<9IbM2h+jmBE5QTXz;=f~I zVZ{k?DC^A@{_*?k1z5sOcS~I-RVjzraED#U+YWM+gkHE%Tu=LQ%C1<*YPwC*$^bj) ztd09FNU`Qq`!mTQz?fV>DV%+Sx*b?GncF*s9tRpnWw4d3S43q)8D)l7L>S>$?Ls#) z)&#lhmnrK6Qc1A0d1CcgHE!eA#Fqdpqixwa#W_O`vU#M+V;o7dnJd{w6qo~ck57wh z0H`5Gw^ch1&Jb00U4<2w`oqC!g_A_py6n6)Y~;( zwQbGc(T*fGv?&}%od!%~B@uVdMsY&kYz{)M@*_Iu`$@k-cvp*{UCWDRG@L6@LO^QOAQ1P zO`sQOhoXC~XBUORK8AUx^RFWz>Iva2sW(E@Lo$@3d9`aVSaTulOvyOkxr1)b8)Vk^ zw`Os!U7EB|1n4sfVd~%<#l*9`G0eFRfw%eAdc6Lln&M@n3=iktj}|U!tWb0)yVsA+ z2CU$abwRhm2v0ThnV+0Hc@EmQ7Pm!Xw!w)>T+pVinyGzPukxm-_}(oShK~)r($guG z{XSrT@3ZVZB=?7@b@EBj_&mE4=Oi+_%1yhLqClN}xbR}ZPMU#74hT;rPw52OCUcTt z_jL5k+T5&D@yuVp79XHRW5edTB&sc2Rry`{kILP`pcLv*mGq&9#CJ>RO-9C&ApErU zdi&Fhb7fNeR?4Eh2_WO-fL1Clwb>4lc+rrRB^`+~_LHKi^5gL^vc1J2%LHcPAen4Y z`Ii;nPs|6dh~!M+KBD+fxRJ!BI3)<%Y6j}ChSWO`7ONJa3pObf%3f=)NaKk;^p?ic zx>#IHlJuOaXTg%KgN(ngc-pUDFS!!bH*6060)Mt_UFFy1KPsak+6DWD)}*P)+p0K- zW?z}&t&`ay-c0Z%X2_jrT(@=tuRjc=MN$nQ@m2{@waoX7b}#+(LPuA>dfM{nFJOvm zH#nW9HEdMVD?4POJ9Jp(PpXVWQS)4(F=MZlG!&0gcgl9tMgdAt@oB8^NJJ`|ii+D< z-RUZ+#%VYq|Mny}RdOe>p^F@414?7IG{HN7rQFiqQKa5~8dO6gyNF2IcDfHp9liB`e_%MJ^?0~R{ zRMR?@#N|Dl0aHFa?H&ez*;=y1BX+;NTkR5hPqDDb@phq**_>TdlImee0TV>CQMB0^ zJ@imy0)BSI@uVb1SDT0yfwZj`C$KM^<->P{19&pr$TZ$I#9`AFVxK29e{ zl9t-*y2G054H+9XasKaUkLv-V%pD)c<(fat_g)cKsft` zyYDxA=)p+LSws@Kbmv_&W-PUbkk`nrOD-02>Qstm?8s_i!XO2`c!vSus1D>*ZP~W^ zi?Z)_$!te5oj)UAVJv0Yr_brBSG-svKI_`QdFYyS6oHT2%s&8U-{%B+$y`r@=8P!k z5vINO6tGnCq zX*W<2?{)b=x%~uD2Ljcot*}1(6$pxSNZZT`MK1Bz^&vG}XR1#BYq3itIB^LBLE>k;J3zHD~gH*e@YzbvOA+^&SsR(O}~N!_<`gRCF+j&}d{3@;*Mo5HKZK zUFDa#apS5*MvqhoDw(`?E!Nz^!pNuU)sgoTWekud=pLLacn9Uhpb~b~B+7u)XBaL0 z%<(GI<{S!^L!bpRai)Lnn+Pkbd1qbq2&BR^5J#NbQUhYQ5^A)rW5>QO%DPoPYOuML z)yunYu}7R}SLIjbKROst;fkzEa2z?KfO89y+)GVq&%)?-$F&SG?w4@g@LCe6eU}Cq(!df)UoBXVYb=rqTLx2`08u}U0{7sx&l;04#?fziXM`4Gi<3%iw5Qh^?p)WBaRSwXLce{ zBR)A>kQndLTY2VEY0-RARUhxNC)wH6cW;atVzbzTPlsX2I=bq;m5Ud!?II#$2~b^) zIp{#v56Ef(Mh~T~)k*!|i3=C3fNnEj02me5*B<)EO@BQ+SI^F!e>Gk^pDJ*Xi%T+% z55}<{EsmQd?$?{ndt;41exucTOV*mt3xJdM+ZB5~er)Y;ziWK82s7j&@oB@h*RYqt z@zrlA(IpT;m)_@>5oxph$7|wLWhF32O3)@uQOWw$Awl70lzA4MBW}!gkYa?Mj-r1L zl!c+^U2$uIKz`KAB0OstP8~1`K9B52qurwjcmz0dx!0;-Eg~6&DjPZAQrymM^{U5( zU{e45yKe;_=c<#p9=TW3JHF=u=0ZM*3z!#@N}K`I9Fe9V678uOa@?qIM5X8ilvP zAZV3#o?|rYC}Qm4Ttrwy5@B*+3)g{&duvYYsOpO>l7m);Yd2M!i?8^NnqTr4X806c zQ!DKxS-`LDpXcQ!Xs~zrOg`x*j(W7<7t;~o^fI~>hZ%Q zrrw4M9pQ~Mt~7;>jZiz@l{;zD`~?wLz@~;X8^!mka%IYY%=iO%{*7~cqsFHdrH-X};M&-m2>@*qstk21o zi*oLE4&KNPDGr9l?LMTY_Yg7|2-Xs7O3o_dx@($)GJq210cM!A`Ai!z(EM!;h2pWh z&;Sx6i2eKee1H#X=XoY==iT3XG^p~lbIm4C&Ti>~-^XOfD!(fKapFQ@Cx|7zsIlw< z#vqUyGUGPtyC^sV{mso3F+cZH^;{tVbpQT;rno@&(zBiLJa%;T8?HUq{)Zf`#7;Gx zogS~2qt!l*&IKI7yRmW?D+krsP5bOuXn)0umi_^Gxgi$QMyJ)sULk(;)kg|q-EsN<4V)e|05?qg@O z$}dpyy@%>4V)R7_GB9?H+HMT;83(nuIR4(uRc}YjP;K?v3g;HY8aTNju*z4T*bG$l z6cIOwMdCJ}YW0b;1A?*ZmFdTd*HXM`r1$ArIZQ6|)2XyoejDm`tM}RN`ubT1ZB+GE zoF-xJ(x#1NV-Ac6$t7_P=h<|pSfPU#8O;Q*ZvQ>rj1vta%~OVordlxW8cI2_ zJ_Hs75$QSRUG^*+=9U*U-Ra!#S2I%IGRVF9W7|9+vK7#|Zj^kG^4|Yb`Xlid9V44= ziP|{GTnwc#-|b5=8`BpoF$9EeDlLbo93nn1BUOc~o%iq4pl=tH$1E*I>~fAsY!KLx z#CM!}9%i*CO>aCPct2OI{%#bCkmj-}xkh7M4eyEZlgJ`rSd`#XFLj!t>=L94v|Z#K zN^kPbdBiOciHQ6mqAuaG-R={cclGo)R|;8q1n8LCv1J@OTK zq!>_FRHbOEUfEn2bEN7z-#Zmh(e%dji4zU}Kko&29STt)dyCZSPL_l{;KCM5;u)O4dBYTz>QRe>vVTgk2ee!l(c zvVIChqw~gwBc?n@xILjr(OU-Y1y%c z_wAvYjoz7dWIAJT1)Ct_>_ysv=qit4dV+ovR<=Wzp+36r*a2-40^ZaGXETpT>I&%& z&$8o+bwK=y`REJN1Rf!`eNb%SUJyFV9w+uiZf=L2S(^i3RT16cv@{My`q|u*mRhZ4 zX>ngqp8v6i3%;dVJSl(w0h-9^$`yzyK?%)dWK=kc@sKYX%5-XIjK=%JE#*UPrvr%1 zU+u{0I@ZWY2oz{%VKFGlz!BSdp6>Unt;8?M_IhY*r_G7mh0X#$At2@rztvJq{ON1; zNM-ZjW!tCO*}|#A-Q4Npdb`~>i3sm)@6HHqWBfC^kFE*a4EO=}h=g(?qxe};U#qA9 zSXQ=(2~kZ1iBLXM`7C&i^Do1TPV%n>0@O&z%$y-jYv`_Em$r@xB$IuXm)FxTK?4$q zw2XhiuO`(Wau|Tc0Ztd)WH9m%M+Zl;?}e93&XB%$CoOaf!yKgu?U0iB8~*L5gLG%D z$nQ--t91I8L$%8C7}9(C`AtX9kbi=Ejjg-(*X$}Wr2N0P9)*;u)fdU9Z!M(<$7;#r ze6$-PAd)yvwY6Z*UiXsxS!sCVsCl+=o?^bI_^SSgKy*|Z-!<*asv{duuiflN>w&gp zVY`U02k}`{>)7R9>YDAm z{;H_)ntr36rNt}%4^A_fkxv5%X9~z2tcXUN@@IbNO*tOCWp}lU^5u1%=P<)vG}(-A z4F7p}$>}?tHl^w2+-;Z*=GxTq$EP z*vIicK=qU97ogR=rzg!~u|9CS-L{1xwR`~Se zZmt9H}8 z+a9cRWO!(up*{@rKgWpXH~+n^eD|g=z0U12Gc%L3vh=&n-h@j-GWPfEfHRJYT`%bA zl}1$hr;AdPOb$<`sWmz0<;$&LFUs<}#O7@A(xq`!IJVZ-j~DKP zpleCRwS|6H-5$34|J+XAQt|MP+~e;b;gAJuI~s*$m23F8uZGGdJUF*RM@JVjoxL%0 zSqZpXw{E?T)7qLGhOeHQhhG^6Pez)4I~yw81CTFa^|;WHxk6}DT61^$>*I*1@6YCHJx)nm6Y_#QatT=oCwjQ_KZ0)=+8C=zEXGfN8DhdD+Hkc7uKa%*1uL z2H8}#{b}HB-W=F*quqdZ+^}tU%wy&_DgAYCACutJHEI>tN1-^CjFc=8d(NWIpCfFt z(71>7TL1ljzvsJ7uDB#$2fEk#Zh!^bmJw`s)t|?M&95phOjhr-mYj2pv2nse_YJ<4 zLh~t0%Jan-eHQPf$lk(H`V)NvcfVc6CeYqhr;mN$8FuNK#L} zRzvm9^D2H_@zwfHk4hX>r~Ixen{c7tES+CUD|+c&ShO-+c|gSa&C z8P_!>ck%s7>G|VFdduHw06erRDJgMRo?nP{BqzsaAf(R!?UJwW`LflvN=##n`+U5T zaNW30mvHIEk4rELLJ=1j8ai%-dc~)#`1m?;#1pLS!$)L8S~OLtCMG5_aie>uikGhVYCUse zrERAu3xo|@hKP+RXM@9MT`s-#PbPFNPH`GKUwnu*a=)v=#nv+?RelzCysHf{ECG4z zD-+IKNV*R!`2eM7Lp_qr3^)XLo9^Ggk2kh*6Q)>)G)KiTkPjwKr=gcDD)$^82> zPAV`k@Ga@-va-_EVxbHv%+TbY@IDivK0Q|WR`vv+4x~AsFXF&}I^kf;YE)riU~%ot X?|X!0jsIMJ{u+-qF$^DR{oDTol}3eA literal 712327 zcmagE2|SeT_Xj*kDxwu-Yf&Ow*=eX`nF_@)c1rfWF_vL0sU%y8l(AG8%-F{+W|Sz~ zgvQv%lWb!dVk~2OZ#_@T^ZbAB``#Zu_cHf=o$EU1d(L&X$Q$}Pd-ojO0|J5eLUgYf zf!jsAP{Hd_CJf#r`DDbs^~l?XqpvA+zC z-WR=p|3>^i^UL={7jI}jJ8i3VabF_~bjO_?2@(@$&huV`@*@O`b{rDg$Ig+dd`Rok zy@%5KTtMPa_0K?#*>9SP#orfwJ37tiAFV5#@=IMP-#{&?PUZO}A-au#S^a(F)f~hv zIGFRKOsca0Pl|%uUFs;sOUZxV{(K8WZ0v>m{JQ%e7q;LtkRAWy`Oin?58|)JAf+3`@C3)vX^9;b=JnM6PXjX6vLO<+CDH4d9^)Q~FG3Z@IAjTTL7 z&8KeNL6|dg^W)O<>gpDc55F`q)1zKTZc07|m5CAPo~D zlm!P(EvuW>dbn<&7^~+xLfL!qss+{Vh|7p6+DDURW#5Tz)xU0UPPfwDlLN7SCiEe5 zk<_z-d67vbFcH~hEwU%~3(jqdlIa8-_Yv9U05W~=-%ozb!xhjwU~Dn6(@%sCF)+f4&msx7dyJM|d#0X@yum zST=Vn`*ihtT#b8vjr`b&cPFR&7m;n=WR%R@nhkby^u&~gg8>I_gh@v1Y~2`Vkz)z< z^sAvEGc=1z+p%dHssMxq{(7i4zSTaXz~%rD{QJ0=CA$CbE8D|D#$ZREN7v?hY35`K z$G9zJ-I_6&p=tS;lD+*>-LqQk2>R-Nx?$>PX3`1kWH1ixnV+q6I5O3ABc0a524|u6 zGO&T}`DpI;5;tyaoug|i{XJO#8`5LGr5-2&0-5S-n1FqJc0KRrGmPK$a}L0{Xx}eJ z_2|P2g0aY@iDT2H7NrC10Ah%zYFrrME;%Dz}`j^kqEBN0>0;^qOj5P?4j5py~&$P=(U*iJw&SvcyaRW4PJW3g!#s7z+oA<7^MxobofCMH?oJECfD{|_0&&lpd$AK(SLNI z=!}w+#54ciiDQ;;T3{OB?G=7*!!|)R;uivw+Um(7FrFOY0Vw~@=@%`=203Y!3>Dn? zBU-V=l%}42B#FulGFo>e`OVoyC;AbHP=+4Or2_P*mZAu?EpBkZs@i3TPc89u3$E->1-%?P6@ zkWpCF!_&VM`S0Ouoqt($+ov98gZ4hVq`U30=|Vq-9=JrF(TT8)5HhSSWzHNje|JhH zZ*uKh7P*YW@z%K|CuA-C1Gx!jgDX%tzwg&<|GsF^`3;00GdAVPiRIrO)|P$4l0n0C zUKzF!hSp|Rk53$H9s7)-Y^qz z&ZMbkgcA=aS2A(50E={dz*64q%|V3Ef2Z_3{FgG1&1C{id?Jz;`(vfHAFQ%-&&=QN zxMcgv!bA+VEGz^_^IwOl@Gt#*@qQVmbX4fx*5XRXHc+}7j+3dz6(T}AcKTmuG}1nH zT>96iz%)nS{4|@PD!|;GeRg4iO#|I=eEai{EMy#3Ery#_PURKy7Q=!(6psJT0HABS zzt{<{GC;IW?E6Xlb1=CDYZ1%M%ISLrARe3%{@-s~uKoVD7l-^hu}@&O22203jo9kV z*wnt6tm4bdn1Xp?J>*sYG_%_y6%G7?ylnQCxs{#IqrFS_Q_gPxq#FS(%qzH{y2}l( zCVyBIX~pp$+AFb^EB}ZgRAt5p>;Zg?D5DXgxdFx%u@qRStcCe#LXRkd;$N5CRHP;bo< zwKOK*N)3)Sj-{9s9u?Z*+u@E&g097Kmw;??)QwREs`jI<2KN-w>aE4hAxp4aVN3wrN4XrIZNQ$HshDiXL~=rAZ^I!uK-?pHhPaP@T2bs zC9I#ZXZ{c{Pfb0oM`}}1e5*a(?$#+Mu>#PFT)+j$Bdcx&#L*0}0*Z%=>LsdfF}~C8 zW`s{;y08^cdaim=nR*ZOQKfDm6y`1wQtN=Yj#%t&nlgcz%?15uD*P^q(g*%A5=5qQ z5%VQ2B$~T~Whq}+VcWtL83I~b$4tVozW0Ms1)q!@w3sp`%P4}A)@NzzWF8m?*T+&k zcYbgMvjfm3F~Olll;7uxV>QEI@)Y(NRtY8|lrZuZbFZ5M0CTn>gk=8>W{f2uwQZ8>e2@qrzVE?ARE1q<*1wdD&9otQokn zx`lXM?Q1Z30Sn`4XHollmrRvEe5&9c9;*bEDYWkRE=-W7aq8K}7S<5yR&~|Ji2i&; z3&!rJf&IylJi%XPC;F}~qX_fn3a=fBR-B;;ym4)OfR>55Tugy_;@XcSJBYfBhA0Mr6nFo?BDjQx5!`25`2W{0)!HCdI>ADGI6|lbL_fUTo_@XqJ~g z<00)7`5Mk3CI+8BWIk9^Z4FovaARYYsgB83zPN}aErMib*{WEJ;XqS?(WSvt`nQtj;!;rf7Tm_e}o2DthB#lv<%Z^`1P` zU;QA5IolZGUJ+>JsHU`!_)w*IHJ0)thRX|gt$1NKe+1Zvl~U;ilBq4QkiM1ib9`2W zl8 z+n8uTtuiJRDtG|5`QJo|+IPc%9Ker5x^2s?x{~=u$YK!wPzC*Uo-EAycEi(zfG73? zORmE&00Bu8tin~|gKD7Dn^DGd+7g)%$6YeIUusE3U^?0nx)#G-JUXM-RH~8FE|g*~ zw64Qgq`ja8VhQbA2mehO)OzI?9+Sr8$dPhw$jY0YXj#a+$+bEWJ>OF>7h2}P)gJBn)<%QblPe2q{uhc>IoU3~s)+hIqb6%Yn;VEwq-OCGD6 zqYN0m^(t85^Qs^y!EEN7@_QAsCN2Wz2q?>L_3p*K4h3wJaXP=iPq6&6&``v$AS;yq z?F&uDF`4o0tcgbP@DZ7OeX}1ce5a3{Eh*DKnqp5*HQ(?;bPo$^n9gV@v!Q3& z$-Belf-;a784=-dw>L5{f-w-SN!tGrPX8e0Cie?E(~q)^$J=Dbm&oppFYpDy7**SP zxwP`(A0iz6f;nGn0V@@BTA2K-oU*J%qQ5=%tahNCu z9v=Lr`ujb{b>$S9c|aMVghy_4)SfyPJVpH6IL7O_1oM3du)>@znoRyMoYe8 z0t`|6(jh;54N9?|i>8{+&9H%prYYwjQcA2+uTr)9zf>OmK?R5PYnOE-(Z&J!OQ#hL_it6uMmf^}bwE$Zkx}U8 z?7H*HEf}T)V~TB@iX3)R?26+`*_Z=D`Ek5!T`zPj@I5V^!|^}*W2^Oh`*KWyfl(N< z<-!7mt~_I33hgv!+I%Ud5DaLKDzG)t!yTwDpDh);4&&5#k~iMb4u?D7!Rk=JT8+}C z>wg4Ee@NW)?r%_;g_xo4JXBeO8oapGjt%^J&vWY}If{21RkoR)CLyX$ORvfMf#CvX zhZd171MRtK=WV6?>C?1a@=WqL+;lRQ1xg!pBO?3T|4#i35HfQr%ruWEm)6 z&CF|^+_t@{Kac0X6P3PX5vWb_t#%f2hQkZIi!f}ajM%?*modOTqz*`~WHtK|yzrcp zG17ey#M+9EId*f#whghImn|sEaJ1$RJ1Hgh;BJC1@1r7A$ z?F^XW2Qjh5qsfwV6)2t0o@{gq1AA@ma{)(su=cj4W04Ix)D_n4s4<~UgQgcg$ zZs<0pB1n7Akh<;cel~}a01sK)v25J(pZT`GN8~DBp7=R-Ug=hjexC5T_jyfFf<>yz z`#gK!i&4~}5Gcx{Uap4=@K>KI@<`h^9b^APxA4E7X=wz{EY0K}M zG!&2$jmy0LaK<~IrcI8)+2LG0^UuzV&7@hOFu9WR2ldwIt5db&H}6;nD$GzQ{pL;b zLWOZX<{Z}FH3r2UO|p&CvHJPyrl=g|MTVyyp9`C*iO3OII0%}E%R9ZNj>A;*- z$2T6)<}h{(#^@n|YRj%Q%{Aks+$Ab=r47?mJ)YDTl0P{v+wO)GTz`pK5Yy;d>{;&8 z8|OSr|HpN+9r&o?M+y2V^HpS+bIyL7uSnSrx_RrHuQ zDJFVIb=HwPET-N7ieyro#Z02bkj3~?BFGhvh#*7_;L5r8ZJckoR^r#CWyI}S{fM+| z^^HXMT)I`4%WR_Aprzwk`~Rv*jwLJMJcgjG)pRcSvb%(EtqJ0t*5>pyCJJAd;Y{7i zrVUnsP8VI!P7v+(>CiY8DcO7r6`0#4xam&34yu%2Q>s;mTOzJ^EsRYmCdNyC-)feT zx7xM*g$CBsdPjGWkWvW@gR_7_y$i^T){8UPens$dPwdwW&b|!KT0T_A^0&IrP!QaZ zLDHjt5+50k7c?2n%&nYy!I}5IZ*yZQXdJrL;gi-EfldSTB52i!GkLdcHY&NDtg>~X z#X>Q$PO5Jd-9C9?3CQnXLrq<132|1nnTe9Sb1zD@gj2&r-GN1J$xWiV#=L9wHQ!>D z^t$P4jQcF1D|kSgM&8uvXRKQ4Pr@aj+d^Y_^n zGJsL_#4t{(y_nodu9CAy&T%`K>ui;M@aTv4&2#Dv*wvd!dssPg;EQXy-I;Z33$)U) zt!jERaytSGL}znFlZ!$Sbeci3^!f8hjoM6vLzkA{W+0-goPIA{tleKkxKi(BZ$Jc$ z2fJCCE9<+s`8H$f8jws%FDR&*v0%3yA1FB2^loIV67H6VmUkf3tqJ;Z^GPERe%hDAsx`bm?Wqd@*ZVR_JjB;-Y9q${4urN4!Lg z@5NPXGNNg~2VeR+!~byB@vb=$w}QjxWBUY}o7%g5uCUks?M9W&uk~f+(6Mn^t56FJ=aYBjf zKKfiO8`uA4&KC%!dldIC$@ra!i*I1~GH4xS4aG#q_KmWkb_9|UlH5GM)ZZS{aJ46U z@U5ZPlKPuR`lJEy=D{K{kvE z&NS~JMmg|cImfz+D9JZILDQ5Y4TvV?2j?JlyR$ zuK%w+ZOh3`ss#PjSekygoFyT5+KHQvtqa7Mp~qff7JP*}7y0i9p-OV0s|MJ(=$Ml7 zAxh8#$0m8OM#}p{T)GodYV6}n2kB;4v~_q;+jhO87F%PomDP5pOTBPXFq-0-uLQLb zO#WBa2PnQ>t8iwrjH`6VP2VY3YE77O*W-itiAD{T7xsO+ux#p;7G*){{S-a@;ysR< zqu#%6)Aba1N6Q0l<%SLzC_zFHz9Y-UT7^c*(z?Mt+#cwIj;%{Xq1J|Wwb!|47NQ&J z;qXEkV}gZHS--{p;ZC8U7b62wKg><~Hz7XPpS=KXt-&)}?%GvXM??tbeAX>DULc_h zgVbe`Pr71nm>!?mW(4N;{f@O2%PCHBy76+s5_K@9vH_^yyQ{+X&sO_eju3d-al!5# zehFm@DPw~=R0Y!D?FUxHt4mF!R-g3l-j{95j6O%qntV2oB6SI!5j|gtS+=6g*H-gD zx5^l2)2Q+~+ja(jwAP@lYKY=9+a+aDfLiH`p(=Jipb)HSFgIZ(m%6pJ@|Uw+r>lY) zO=Jq7Hl(ul+)W!qg(Cg!jMf@-0VPy;{{2SPO~$=36t@BeX<@WlN)H?LUZ9zgonxpP z+68dzGaS`T;@(i8pW=V2k;K*KRig8O026D~Ub-g^Vn2A2`ssI0pNhoM*Q1u(0_rF2 zEz@&^ZLETq6~)2_vyfIJ%h~&<@@OQq^pc;me#*IL@eMO&28b=1R1B5+{8s8s;ADE~;iAosDjNwfA$( zkOqqLEt5}UsB{u87_rUTOwp(I8KT~0XCMRMDZ_aGAV$1mM&^bMx?Rr>F>O4EN9rLw zMwVZ9Og+JFu7@NE+19xYM&YO^1`lIP_N`M=Z z*48N9LzqO+jE)jyB&V1`jCEQBZo}7I4hZi4JS6=fUxmqP4$VuAsjcPB)}7zouL0bd z`bZ=7MMo{qv@)+V@cC5s50t@qva0}LOR_~Z7kQB{goO-X6IgHHv@tk`d1GWb-`;Xu zG4YhOD_?~S@svvGxu&v=@$HQ;-MuxqmW%JTgPb_TvMx*X=^&zl#)zRNB_U9+ksgQ| z%(AmdYJ9`!!sq8BC&mq|7KQBHffe?ifmljiJ>W;eCW#J}BgxUrx6Hb$^tM8TE7#CZ z*!u{`>x_;@x?D^3kOjD^y0O6h>mcTD0r+<3f3N=bA>RnQRxVtq673t?DM;g44 z%}gh=)DU*}UIJ?1Sxf-uo4C?C>kU1ls+mo-TJpcQw$PaJ)>Dzj%jxH+LcYhT*eZ`G z8T?wE)yFJRZ-!+tvv#bOxF}H&r(m`39edF8esXei$Me754YX9Mn}gqSCIzP{j|82H ztZ|H^>7cJFXw$FZirVwio^l$n^pAgR6ePvI7-=sq7Vdqq!CZn5s8sU*lc>c>e!Hd#`Fc0tMpQ_;i%xf;Br~?8J3+L>EkU$^cbkUswF7kM#{1}A zfUj?vvx9x{lJ7=w>7J^MaYH&L_NiTq;Eh)<)YPG`HTsaR-=$7PPUmExA7^RP@hx@w z9&+s?)(OT^khg&h%n`iI6jK-R&coxH^q>N|J#fo>gfN7wtFzp=we+8qf-Q$sAmpnx zIl`fN*{|k_Cwcu=)Zjz98>M*|m{}|smyT7ZtlQv?t+z}EZTZTTqRL5+iRkd#n z05~!P6vd6l<%2`a7>sxMGrNm$9^EfSC@cV06ua4u;%zh>V%0eK0ATzrPdjXqVsN@u z1>!1Wg7!*i%a98|s>(pkq{8=(HM)!xfSLXO!pzkJGB#kT2f9h+o0z^3n38PGj5V3# zC_yBzOEnv*#i*4k*a7`}bjxI*FxI!S;bTlmLbnUScq>Ls0ITzgRsqDnFZ{^eQP`Fl z+Rm}d!I@0c5dsEX)Sk0>Oe6iLsJl>pgq9#}{fvEMNT zjB7IS%=b8_+#=CmgA794MRe7hDfXt3N0OzXzR!R>JNXtFp%XED05FdRjAts@o@xVy zNJm6O$EDCj#K+K=)SNm+TWR!2G>~FZcajF-#!JXDN{Lqo;a<-XK7+u<(S!q{dy^eW zV;oq56EY?!(*~QSs>}u|8d}N&8W4Q98h8U^+p%k}Y3W!^9gbdE3f9_e1A$oVIpgX` z!rW_sLFw+Qym@wJQgf*@HXFeV8Z9{-pI5?$ZZ5YrwsQC4EpM_WYoZ?~he&T3SSPQU zgxFX)`#d@9>%(AT(p@VXfliCtRLtFeqvXfxaUwqx+M$TCdq3kE$#*Zwcz{QmWN*2) z=~uuy!mJ1)6|v z0j(X1YFqvr54SPt_E^NZ`cY1l0gD>#5sO@+Xo*wCQn6;T^u%5eAHoe`$XHnq>emf~ zZDmz|pExG@px;==wOW>;Yv5A7PYpyoVM&;*`e>Bge4tl>;-RWB`lRDh_vdh|A5;uj z;a>%YV4(PEduu5_&1Qq1Pcn-J0}VZeBo#H>R}7h;SgT$^8J5INR|5?PXMqpC#X2KB zmOlUL_Xz#4pqcrPLn7IU$H@WNQa{H1BV(wdyGP{Gp6hvh03DO31mjUXqYmLh2bm+*8| zz!0=dH6=@z20a4|#g-3_c37{_CT4Mg-1P(AUDkSW3i=*M>Z3tFH=rG$BqKa85)hCt zxKP#0?NDf$Zx#(9emS*k1}Mo>1cdHiD>X2RT!E1HHfoy*C>3m~j{DfeTNrOy0_EVJ zjWo8udtgj6Nzjn(qBWzV*NfplE+* zi{^>di3WO9yxK7FbwJ};NqsDK$P`7HI7aLg@a`ZOaKHHEpn_w@UBV}}uzA(h;v9`b zOlFjnIp?IBlxj&{tMw{rreKuUPLM6ws$vrShN}CT>RW?gq;wqbCMuwpZa%OAMIn5Q z%&5D8CXZLHN~ubSrjAQ1ia^Y(vp$L`xl&F_#n}KYB)bPQknV_&6UY9M6fx*mfv?Dk zl-HhitKA;g1e8KLf{Q*pUk)nE6@5zfOQh{3r)qQb+6F3F&C&UEILz2!IhzH{O`y$z z1uzhXgFwTcYAMpH>j8bc=M{_F`(mx0MXvXPh=}}h!FD-@t>{8Qk55OxP8>I@<;T5f z>m;y~S8NlE#Gk?fGBwaJPqiF$0knVV70ppQ!t_>!XP=Izs_HIca*ekUgo@av&H*bd zm<9R@wkGSzgm2y-Pk@Uj$5~&jn=(Rh8#=#wV8(uz5Vh8v81G&>fJ<|~+)f1Fs;}4@ zpe=Bw)qfST-q7q~k3K>V0gQrb!ydKF$rGy;M}&6p4(pStFreG!{+5qF&{1n14hOY3 ze;Jwb<4JCDIAR>{e-S~wgl943rnpptR)uzsgQTZ>XbO6Tq}atu+RJdDhB4)sPBUX{ z_14g3Yp^Bs5h_GRbn$awz%CY+Q=OUEo_*pc9tm|c>1aW%4=)Dnx8Z!1^Gg13H0y~o zy7UvBnGl&hmmae}xxp%W|4YD@sT`kXil|Y0HDzcmNP|))EguG_x~9UHrtzdS#t0>I z%BXoNkm!(w&UuXrJDa;z>k$t{JcBGp=9yOSKgYVVchd1y^O4;tT-RB&`DU{wRYPr9 zrxHCfSvLigV;0Ty;zc1p|H_VN9ys;u4zfRXrz#sQnw^_F=UK}VQSpdKh~iv$bX+07 zD~Q5+@S4&a%8G=?w+s7Po+`fXEAcsLxHFI~iFM?v(wnU!)yXQea-Wm&5{?V33N|}8 zO?G6UhF5SKJ#2=x+Wb@O%g z5%v_w9(6;i;AM0C(=36fnGgG)`|NF4)5O{-&z&9C%o1=ma%H7FW1Tu=cVyVj)@f3E zPam6d*rB81zgH38_vCow&lN)AGj}bs@Ntltl0`l@@=ez6jx}irt-!{PHoLM9@3GzJ zjLCsTfNW$2uf#}d?GX>On$*tZT4HH`rzeGTJK?3St`l$VTEnV9=ioh*gIj`I)c5tX zsvXXgfk@a{3?H=}?Tpby#vsCQVa9uwKJN}bI>F#9L2|j3E z;>p@H94r%`&_5M=Ep$Fhpd*BT&w9$C6pXUxsnoLu(u2AJNVd6`oqC5#q{Ic^rfQz} zLozObL-J>TFY)nR=#5nSPy-IKOO{@!pMB8!D$!3)z3#h!&YlD`00XJ+{lrTELc(qm z*@-3qC(rxy%$>D0ozynB-8pw*?#!Gn#UDFG8ap(NGxi1POAg-8Z05iGa$iLt=ND+8 z#F6-2llXYiT-4j$m4ZsglCTbXh8Mrm*aIiHbuX0L@&G@*&(6hSB2XV;{SqM9Q#RqlK%7-esOkV?hm_`#Nve3KA_j?_etpt#D?2YAFRt0ie+Kx>v=1Z%0 zu|##ECnq^c^g}O8JvU;6r+pQn`z=wzA(^-~yj(8?{KS*Vda{40pYwCkQ=W3Y*kBiY9u`KF0 zs-GSk35^OJy{2^Hx5(q0>6x zO2_SGUmjx0z4192C3~=vwQ_%Dj895h@4Y4z)^)uqiC__R=y|2ooQLgu-Y+eGHDxj? zYXKHKmN+~d$a#h~J*wCMg$L^2MNe&iISt(4ultN-*4(RMMbmK6QIN9*>R3w>jq znN6fR5^tdZ)tX z-V2-;_P)JXT?ejh3Xw}V6r^&<^tsP$jpU^Hx_Ksbui3$s!h#Ur9j%(b1(Df&{@-%B zJM3YL4>jN7P(JeF;0@QJIuI9WWhIFP(9)n$hRQJEno{u&5(|Zm^5nAhEDJB!T(N^+ zjH7OtF*s&tIg2l=?`ijFBeHDCP2wuigh=IFHaxY)0vM=jBy#C z4Z2z8-tT{rV{T~2D>mB{G-*&k-8&l}A_`Nw*`+Z^(ztx5_H8~8?5TCeQ(^j1!pT&$ z`;KYuX+714un)S!K8CMG^4`_zN{pwnyQs4bSO6BAvX>~#K2^xI&gC!TeDD&)5T9pI z%?qycIk~ZT!0X@~Xs$wdx0U> zlwWUK3CQCG_fEfTYqsm^EI~M*gKTrkUUHM0kJ8eU(kI48qD4_Av%=5(84KUoo!x>Ma`avH5I_yOb5B$)k=2Eg-cgv-p9RKGQsMDe>%8^U zXh@x`2NWuKSJ#r%3xUS(N$%t<+oy6?U0E0-ou^WIYQ+CRr> z2yzKgdc^<8j3`pQGjo}=78dtZF*YYT5ppTA|A5GK6Rl^VMmeO1b8=w<5cJK#D}B30 zKXD;UMBR5(N;{7{YYT0IplLYa=O-Urynbd-mxltCo&gnc^AV2b&&8Y|#btr#l(%B!YNN7Nj^Wwfcgp0PVvQBY$Y0 z#H799#{tbuuc~a_OWJbPo581LdHMH7pI~>2KJ|b-pFI!4R1fgE(-mGSJ|}H><91CT zkJkZ4rNM29!eEoS*hBF>d+?BDiwiw1)O;Og^xM5f2Pf4mAV+g#KqBIU4JWR1w4WYy z4S?IWan|h2gg=gp?{*q|CCs!tKPNXFo+WTI=Vj)?H$amM*5xNHtDgCg=3W$>JHSt2 z5Gcn|&-%sP?13UTu{u7P(DGNREAwP3uj-5+7#rK#4C}j31VS8G0>Hpe&42PD1=<6Uy@#O$rIzc!5EzhFTKqjSQ*VhIq<*PP@o^8UGVcl$@Iove`>Yps!n@EBy7U?x z1pTIUZ;nd7>wtFO{jC1SnK1@{R^KlXmlb^_!XWMIbai;&8S~_wsR3Io$E?)AHJbtb zzMz?F#Bm6koH#VVz!R%7+cZvu4b`U5LAwE@yHu==k^kxgfC=ehCbBo3Tpun>Uu|w#Z!!Z zSK-ltxW4zDnRA_OoJ*e}OyRu8=-b!gMMq^tD#Lx!+ubwiDI&*H`Sy1A5q&ddt7dem zBCwg@_%!YhPa5<><#Lu577xw24*LwZvof~274+%MEmx4|$p>zAsYrHVh68VYjeZqa zTu|a2BGXk|Z}GB`SFpsDPZZaLCsk~_M{oL;&4sVR>3k~tO$&SuTBWh*pJFf4<~S++NwCxJ zy%H<+bn5twt}0J3-#LQg$v^hZKm3ZQQX%Jsyhp_b7?pLuVZo&q`3Mt!y*@XI@pbCc z+-1Kzvo#gP!I&y&$G%~SuM~gpk(d0j|YjNBf7V4Wl{2^o8Ihlys@cb`o(|0 z@`J^NvgYSv8`Nuj z${}*^lL{rxP%B;ZqPosY1lF85>h^WaI!kZZ!dbj?toal*gj1Kwan*S4;BA|&<%a9m z6v_p?3!AdT$G+ToN>$_GNjA#NO#q8U@^|Uh3k0&Zlsfb>@0=Q5AkIi7>%7K@Uo+Gf}@nWasP*layjAx;xWhE3lQqE5#Oi!A%XWr z3$7xi63s`tv(2HfX+i$1G`kAt4c4y#uDiXKv8p=Eb#Tjm<6pvRP4Ozlg zb3xp=H4j*L9=sjC&OATMwuCuC8Gg>e&AxeQ6Cnr4L7N3;xP(#`QI=i4k`)Ya$=FW! zb}fIrva%j0}Rb}!bn$75A zNRZ;+95!Dw+*Iy|?2%C4!~fE+x>sn|TisR0^p4;c0R_gNOP@56twHMNA$L}RD?^-kGFT7$qR;6uzbmi12rR<>cnFUK+CR=6n zch@0swNl2>ba>~3eBHi;1?%PlM3?3Pwa~L?D(aR&XoqzB03qV>Z~dPgAtMdzMhQ?B zyEA+f7gTJ6JKqRi?OpgV)CeawIeY9XrL{b17?Tw(F4H#JWNMPm2`HyHJZ$C?bC(X1 zTkR%2X>j0YvAo>7{57akB^9dhP!#LJ1#n&OV%1w1BZaHr^O zvvD|d;9%m;k58@`#~hDj+}8a%{6$4{{_wDGucvn3&t8D5Pj(aS6KtAl?i!R{ zZhTg-m1Mo3-?_RP7n)VlUU^hIe)eW1y1U@@@yKRiWA0wvI~rHv*x70~vv1hTmc^dp zCn2l4V0jG-*Zta(36H5Ba(yc?$I)Ck5>q91^xI5=89{>ux1+VNSyh+N<-IrG46sXo zI7@$kmZfbEB;{h?nd7&mVUB6nLZ&YdRc*5IKMml1C|W*rG!s-iftXQ|1~q<5bGl{0 z)^&!^u2&&A=h<8U7jw?QJPR!j$hL5HWz}IN9jKVqN0uJ;-QD}@8qr#L^PfUzUe&1AD@E-#A3|_5lm5iDB2C4%gR1Zlo1CC zT);fFQm(9)&5AyltQ7|k>$Iy#V@@{TOh@xTV$|`-{CVDH5zjbL)cC+1krPYiR-Pk| z2A)ag#2$~73#E5O+g9I7;IXi}$S5H3G*mSd4l+uYQT4UGj~6Xl9fzk}bl?1;=zlQs zmD%w~P!mVH<5cMC;)lcEQpfpN$7}(eNYz?TUJ?}RjNbct?KYcvg?Qh^rkWB`N!~L6 zrm2~2XP4W-9Y)jCxsz^Wkst#0Cp?( z@PZfir<#xGzYHa$jkxwYzN9o~22P)#Omo{#KKexd@{ma4?_qHLzr&IfVkz)KKt7$5 zZ457pe&%PC=NMYdvaoH=d`|>}17qLgNkz!hAU`9Iwrtjuc!080B+vP&8PiIu*~sqJ zLtncIxyK{d_HUV|u}mL0SCzbSbe>c{e_Za_A@^NgBKGY@9$YWdzrd1ij-h0q%!B4m z*_KJ3Jua1eJhJVU?rm2=z18&HaiDELwY`n1x0(V_n5G(PbY?9$MF(EUjy3yp;}^Po zVB0O6wQ<3|CZ0;L7|C7DHvMwucHwa;m-VCksR0Fu^Smn(a~Iew4E_dU__9{Bkn1=o z@LT%rQ8(>sk-{(4HcvtxYkfH`>doSHv@Aikzuv^aD(lNyLE~YnhH|s+dz2 zIn3rWVXbN}B;kL8afiH&=>FlwSF4gBYd-mGEtO|);n90)fL#@#8fp<~Lz?xr+!QzI z#uDukEY?Vl>9>xAWDX?z0=Wk2nPpK|r#;@JO9z#-MK~@%rd2k~o+)j^p zNj$&**|S5mjRZB=>J8$0W?f(Ocb4%Dp@q=1VR0;H%Yz}ATLA9iz)tOd5P}X(T37Kp zSacbX$(Mb@7l&1tgaSZg4LH0`*i+*c<`!aT@Bc8EP{2Gp*!H{O3!m>M8YNgvDJ&Qi zzu3xJcU=|8B<*Gzw6JKL98~I|j%X$VUXvi;bhV%7nr6bzHnI+%1bA*{@oe_BI0Nqy zv2Mf#V?1e?3YEgQ)tQ@cq-6~hXSGuU2ehY=s0$9}F(vF?3MdPke__y!>+E3YA50~7 z5IH^)%#1j^_{H~S+(&#AxUhP_E2yyvCLGB6jFY@6HTU3h8E$y@@CL;C-Tqt=m`0Rhi^dcMQ@zvX-#dy(-m5ivHjp z7~0D^d}#Q@Fb8qdzW=6!252+ec4IYj>XhT5=}XuWMwmvD$D6i+U4oX?yCG%Y{f2K0 zOZJT;JG+Q1#19F(yZVo;*oReD|A1R&uC4vr?Lz|UwNn@@B0M9^(VDiH=Z5hUPiN)sfDKIj3Wy{Z`F&2 zz{CV1QqNm#&cRk`xW->y@_)Lcxjf}3-9H~bcXuaKYWiLD!i35gi@=UH=UW=Hc5!S% z)F|{G`bXtBtr&);1F!2DFO7NE-rbgL(XARE%zJjefV>?4Yl>wd>O9v9r+s(vLI3ze z+PERTy4c}sx z*G=)#yl*-jT<$x1R6g58^>GQkDS*gPLNav8PrtE*8u_Rp{o4BZNe}ll?qs89TGmS9 zN3N!|K$}HQB4YzGK`ZA}#H_rtZ#v9`?3b-L5PYeZvdB|HDl*dsZSlSM5Kmbey9}0Y zCOitX$`_Q+_ANW&O&S%t1bT*^vS+Rrg&;G?S->KpDd6Jy0%!?m@Yv{{sZ8@x3EkJXBXYC4Wnx4uxjHLXF=$0FF0q+F)JPmo<*HWoK)l{#7ua4Coq1tIRu{Gx&#tQlLMuUO=;DSnmh?hgAQzN)6{ z!v>i#t{RW)BaiZH9zef;(hw%VI-pZEvZ`6BvUM@$w7Uy9;zMCE;7fV$7#)gA&Ajn? zc|0vMAq+6lA`eBSk=r(AJM2ole1Ohc-l{feZW_@8pPileZ}@i4ouxHQD@0a zv1=!hEEx=V;81DcgMnpn_}kjYOe@Xi{4a3tz=m(E#^R#$ikCZ&2>jxZvTF%i9qj>Z$&Rf z0eaRP&Ijuz5Gi}#p0f=`_?(|7>4k8e8^c-;x$Xl0k?xEyzBSeVa*}v}j0WEse?& zN;3A3lN4DFVUT4?mJlQR{@l~~eoy*7&+mD?`XlFc8rOckulIf3_q?nBEV0BH@9F_s z{1^4cDq#as8D)rMcNqMiF(PE@*l7SvMZp|p&xTE7jlAbxFuia6`kvN>lZQxq}S=A!{1t*JT&~w&+{jz|` zV?BT!lX$R|@NV^0Z)I} z8F{d}z=S~hkY$~Te9fH)E(z^2aSpas-f!)TjNMvh7j8aq;k>mgI6Xxm;e^%%oJQ{I z`9J6RV&-gU;M}oFu0Mj`-}IU}Nw#pM9JU5wH(*~^_(c*b0>7>MO;QgRBXN7;M+!2O zxTH0~qB@mZD4V*!Z?-)tp)Sb$@o}4pg(*-d8jqcM>l<~OvT{N!Gz|n?I%GX-ZQ~WE zW-}FqdpEP!^GT;PLK-cHJd3I<&sF+Q;`ZFR@cv6Ia)fq!f%WNZZ%IM#q3)}g>Mjoj z;p%oQhk4{FV7)NW|Fs?%KF|`TMlFjLWjB{bho2`re5aear|^fc=fJwHy}Fog9Ic?_ zpDAcVoZS$+rV(OM_swhSQ_^x6)eWR=68Hq91P51x$-Cb2+u$QMuQufe$?;bhe|&!= z&zBpXEW6%xZ)>Xl`*+HrYnc6$Pd^?;HF=0I&e!WP(USU|E6mgo-(Hh5gTPN*Z_wHo z*L-1`{h^c8G$y)ApY-fHXNB)2#X2J$D|W0QiK=CwbpEK-9Q

Guulen4I~ePnwo3 zwUFU=l(C4`4#Fr`f%t+cBoIYrzBm@ z^`yTUU*5#NMii#yaZ|_@T;Bi7Vr&=BdEgrqSA&2ql2j^m<$sXX5Z(@AgNNgE`st z$VI^Q*uI}Z5R3?h40ik;_3}*bLa%@LxXBtxg}@`}w^L^hi=ZTU)~=49yswVYSXNhj zR93fIlqiru4FS=m0cOYzbxc4TXe70ofTYVZ<7;J2=6>k3erspc#*N5mDd5Iv`E3<` z{X=Wl8{)LEiw=<$7k6>~GvQCxc*mKHND1vJcJGc`8Xa{@31|_PBrhqoht!^UHAA5} zeVYyR|Il<|)S&J~vx;fp@7DQmOfmlw?y#DM^EuDy^G%vHGWWpG*1ee@5xmX_rO` z6BB6-J$o?G6sdi8$*PMeN}Nmo%eBWt@f}ck8QAF{FJOBB6I2_=g`F!tu$g{%awZ&J zWCxqZhd zoq6v{xuv!%ox@PTrFiN#JcFa4jCY>1ZJA4Pn|3?bPgI|UnR($4wh)*JZ*J$gXNi}P zJ1e!G-=@@xAL>o=1-Lj;@PTVl_1`18jmEVPLoC)M(b zksm@bd%j9omHR1Se~dr~tvfree9E1!Ml|sjc#fUg3@zx3X&Y_aMdXh4=BZzTPG};$ zf2&NUaB*!N+3eetpqHj4$6>|~zrUHasBy3#SS(vOeq`#eevV;#4!e<@B7Qk<*0tVh{4 zI?Dm}Ah5W6Z;p<>=lhzE>p$Ape3&#X^jry>%?)4RE?C~7E=zUiH-xS2p^jkiv-A`% zwG1V-0UW915UOiyPKb(-6@Dw9o0~}b4upO@U$f8TVWo_I$TndVrOOE(>P=`a);Q+( zC>m}k?NWSTjJ~gmS47%nRvO~H88B!$4#lKm*$`COIgB_0O4-J{KbG(@Z-u@9d}?6?ZhOk7*E*+Z*q*}2_< zODgJF$JVEab+gQ+4=k>7mGmsuyFkdL@ZJEMkISPe87LnSB%AYEl4Kw2EyW9*+avH^ z=(*Fg2o-yiQvOY&u=JLD?^(OV_#Huz{HvoIek(halk3WIlr6VJ?8V^R@<)~E$}9hP zcK~A0Q{-7`-sJn>IQcGMGXkHH^o#a27k(LuY4lda7d|?14c+M}>;RiZjNg>Ia@sPt zK#O#W{8yBP9y>J$AC+^j06Bm#bSS^8ttrr*CO{46VffSGba@IR%k5){Tn@JnZ@#!o zG5^vbf>n-1Fro!{NHEs~-HoO`Iu-@Qz1POxx7{Uhm>&LOt_^HJ)$a(z{!r3~`J)Ko zgl4(2`*T%iD2qcIo{b(--(8e(K-*nrd=4D>TS>${3_0n4RmbA)tzQ?w34Kw5ionq& z3d&Fk&mS;kTT>kxQp$|FG_fv9)uOt8N;}XiqTYB6_rSgL1+R~|%d7^y2}F=d1oA9$ zybtLjdhA(xC9F@vZsL|{5N3Yxe7i^XFy{h8fpETd1;zkj6cju2Z4 zJN6>Tv)}#rwFGU?hT-#gQzk87i8NrL87fj;`K@G)KTP)5&ZBnfGx+K6p*2KMr|W>Q z-Ke~2P;av!h-I$aT{$s;QzGdIfYpj67WF51UYzRU-DuRI^~%n#+`CSm)#adyFBuS& zi|iYoVzmBJ$KOE&-BCPdn|nzIaUWp`+$!FkD$ouRjk0i#VL3R7PzF|=hI(qXWOH`E zfhY48e6LE54UuA;BQ)#xdFQN@t5IEnLp?zFnsHccWstPo{)pf4YThR9+#rS6P1Pt_ z`Yx3*H=7Ok+HR&pOW|*k6IHM@LC3;|ES-J5a1mjb;u_F!Jrefcp`_I#{dt~i^2a2P z@(Fqw=z-e2p(hX;^F=C}!{sCut@zA?6oMGUT)C91-v#4A`Jtpx)vQN$_ui$H_re`z zs3R|+l3K0o-Q5Uk+C{6}%RP`QB>Sr%=ZJA(CsV69It*`9BLLj;%8H%#Lrw7%^c)Bz z*F!KX#PosHxX>Ad7B zG%SKY3jAo?mip3#qf)}9I7b!j%8{?b*R56L8sodK7cyuUz%iaBeKIhguIX#}ix#LF zvOO^4wNxi>@x9*W3M>gPYlMi*A@JfZvqE2>DM<55gw1ppvKrNg?&FC>h&aFAg$=^( zocrbjiCKwx&#dim}JeRV9zI z(7a!bmGrSV8c?719-b?^kdAow#EfrH!sU-x{c}%Je@XRk&}oZI*Gh+oo1XT&sve-y z$e`0&qZx1LDZ(i5p*@gbt=5xMYU|spmQTquhCotUN1!^F?T@7g??s31cytu4uJPzL zrMgE%H}=)y!B*AuTImz(&L#bsU<~*8eJFv1W-r0r2g0mBVLznF1EXU zcM=4P_CH?fhsWI)%vA%C>J`Qws#-Llix0(D9q$%Xn7u(;Nh$*@LM`G$zd8rD|yUyWh@Qu<; zmSo@4YMiUv85fv7<&W!E_f_>H^m&b-L-#hIXXz4#8lr*{$i2f5dZ32G-Qvq`OY5!r>XH)@}6Q*NZN zQqb@}VVknC-NoOIP*r3yi;M3VzRLFdXYy^1{P|A!n$9j_ zYiapiDOZ-;U>&W|`x!l)hEQXn`IK8?za-jpp}~toG>5Y!9afs5Jh11di3ahDeLwb` zxg+~SVK*k=%rQx9yf%uu&9Q<29!owfJY@>JisvIB)C-w<^0YslqTRiHsv^nvFz#Jt z_C6~!>5ija@B-9v6}uQAh7=C}3iQB^M47nw#V&gICnA7LNtE3^iqwHCwe!8EaS3&| zK{!cgC@re$gGIvWkR?~pcK_?boU4C;qRw|#6nZI}0pj!gY)wyQCCObL`H8s!+p9s% z5MO*fFiY7MXKuhsn~0Yr-6hTIlLC>zbX9uKTMmM{+OuvN)Wr+xYKR%}6x?6A@4Y5; zjmUasH6fRN5Kv%plt;5IuYNq|4c^JB)erMI^+pgYczP^0sa1@US>Y{^J1ChD>h^dA zNSFJj2DA-J1}D@GeWRz0!e%ddTj1`67P_tX?^{tdYP5jrpl~Mo0eTtowO6sBnK0?U zdSS_^hOJ{eE?q*M z_uAHZSMcj(-Pu+G|8#=c45wBhsVfl=cq^rs9KIM+a>KTDqq{j`3&+nG&`;2n@y*PQ zKM#eO)H|8+k9oa#^T-H3nj&yfkpaQ}AC0QVCi;Tdqv73f5h0h^NqRWhPBsrV!P*r8 zr|K2CRy?pSmlFDJRXg8dZ0Io6A9ZCsnz!at@)l zw%rOg3O{_`Z+~uFp1EmUD*PFu5agAmI%-KD5Jo4zxMJS*>Qe^{>Gkv!-0NZNm&^Q| zozpz#mF2&XTj$}6jE^6fy-kuFYIHRSG>f*y69d4w=?4m50H#D%|5&B!Xk#%6&Cxk< zQy@vV`q#hvZ&Y>wUTDkG!uW^;{mY{h5lC^BFM~Zop|EeNS*pq*Oytf07iaOf0o{<8%^L6!jTW0JKdvE%uCc8764-RWHT^IwdmSd4Gfe#6D| zb4sw~vr51XZC;D3G=u}3wy(R~r%urm4CT^(@>lvStxC8EvPGVcS2RieL=%BuZ6Q34 z2P*;U>{PLsRGmw+#Nx zxA7;Zv&F%x@qhnAPUZ-55E;#P%$tp%pa?r_Q=RrQTHaGLyz}!!>_l0BwW|)&8Pyem z>ng8e{c+PEF6>QBmFF!ic&}qBIsH{|-8(-`Pqreb;jYSk*X%s3k%O@6 zgB&L7JBozW-<-1@?>yEXR2YI-O}wNktL4z)G30mE_<2J;@;?7&|GrW%W|#=&sx{J$0etyX_UmcUYE zvG2t?sStT*@n;A>Da0c`N``O>)SjESklrfLwt_xCl(D#@+Ryo7j5DY|#vny$;+Mt^ z1?&zha_#u*@*{m*#bZy%j_OHxz&I$eEWV0z$A<5Kqx4IxL(qJcr8Fk+(Q$>-P!K>2Hg zBlnr{37%$QoPq~s$oqQbh^V1EH83`f{`TMw$-DMjuuY_kZ8>|+V|UmyWCcR$?q7VZ z^|Q}*i*l}^mV`E>Zgd;~r~(Q$CYa%k6dTXCyElCXpd1-0k76cT&Zc{k-=i#T_JR=P zcg;vPaXWN97d(?)X^>7 zva_?H&Nn#*R`hd!%o{NV>84g8+^=P6>Bh_LU|{k0!oLWfXM362Ztpgf(FU!M=H0&@ zvb*t8Msb^7>0zYTn7xfm+)(Ve`46u!aPOn{$|`J0mEiSVj-oxIOh6S$EA9yLpo`E+ zWGi-anocz;4Q%@=Uf+JHmE#9SF4B2)M@;yKa44FS3_v>@5pMtU>9JoQ#v8Qd+1vQxK zc__hHXY$1J4NJ&?2p-{JQUjVz%KiIT55~A|Sb{ksR}4VYjBA%B4%S@lkaCrY0uLIe zTXCE$Bxhq6MGQGySSC4ds`i35^sfVb{T#oew)`x1SsH!{R?pJ0B0vxT%qSP5;bw^+ zvHaPQ1}F*71cG-TwgoCAqIvTZ``r;X5t`AC+@^F8z|yoH{q%% zx%+-BRI#OPMUTLR4Opny(n%Uj1noo9dNU@Lm+blC$0UmjPbGioOCKfj3H)1nIH*k6 zUl(|k>}m{l%+&9n2MDu6LKECuN`0Q70gcycNwXEmW9^5klAARpAE5F@?EJa$y6QU5 z0TSnGpXM1eu|l_?2TqmWb>)&jSuuEPE`6a45{0B^jJL3R^`wD`CN55s3ks zg5#a|5Q$DOYkC?oUKW7V(E466l}CKvSh5w7NBnQ$`7Y9a$Y#4SI{)waR+kM#8o0b8 z=>3BpZW}vMk_Y1Eg``WNPTLp17;bHcAp>NvB{T{2?ki-{p`ViQ4J+>`t|1-eS6HSe<1_ujtg{6*Ywa zd*T5Qc_p`LA6>os>ESWR5hfDP1>|lT&k@diT1(cn=PjPkb`cDhuo*v@E8Jc0eo`XX zqd5#M$`6mR;MMS*mzey}fB$qizWsxuXhI zzwnIKr^BudayyauuY#By(Y4_%8sqpSxFZu@Yxw#(yGKsGCuxGgu&>nsJuS4+3=`M^H`n zKU&^jOszaVH#!VL2aD@GFyO(?07GPO*KN=_!a(PEx8J*rLQH9wl4S~bedLrGdBKGBXg~WL(UwU6mja&x35H4^s2HJxDen0 z!GTV`=NFCX^}r{&`DM_l@^^yD(_T6)CJ+d2tu~ z5F)YSP-z*oPJ!wSD(d^8n~bR~uF3`55L>E@RRBLhrRXeh)tDpx_;u@^jkWU`%(;Wf;N7#$Ce|GVyH;PB+GHQ4zbpvA-Z7Cz`=G#CHAlCY zqdc{gPaE)2k<5F3E6OY7t+uuEl;e65y7|X`jAG$&F{*Z_KpFT^OTSNJ-hw+5R_GS^ zPimD--)0F$YpeB}!{1_Or}P$CAv;&@qm@W1cc@0C!GB8!(1+uX`*R(b!#3cgNtg(3 z8$V8V)}Ri6DXh1D@7Mi&@8M4rg(+fHPSvTKiWYO;&g3L6_))m=@$$Q>*I+fwE3EZl z?9|5~44^zx+-Iae%c1(xEc(sOn55=sa3zSC+FmHnA5+a zZ?+CZU30=Ef0y1o>V8$`nhaLpp28#W3b#dqGF#S8vXH66Nk@KF6wB%DrqS`2c5sZn zQdL0g#6$47ou*lQ7!^I<(+|39Oa}Z!uV!>ajfaifMVZj-X(bZ{Q6~QL-}S*|$^qve zKgEiK8Nzs*zt?=HNDQ*@HD1?%zgYymsrnBU3_yl+4!1XI{_(1%2 zdK6>>WfB|0o&^iMrz9o@(9L5;<>&)_kvuCM~wZ5D0TLr<;bfBP24s=W0e zOAE_Oa9P+kdwwSYx3Dd@4pAHC*T=6^p0sCWe(#eptLJ^nr)>6oJS>}Y>ng$|%a}Fp z6ykQP)P7u^)wlUuJUVD0)?k)u-nCr;1xLGYzY2RoS&6S6{T3x1lxPhfv}&AEGOyHA zWJaV{u?EC|DjSk;0$4B2%o3kBA7+iT)h*le(^)0bjSdKla#jj2p&0V?F!}}hAn_jG z=)Y1(L<8F|b17)$kpBxgX z{y!z6su2?RjV@!FzX{T5&|_#fR1Rreq(v2oMkz(Wuz|8G)vbZGVeR{Vpwkds*bmXk zxP(y4{cnJaDQT^ZMzs&gb~V*wZ^EWy(85=tyuG&4idr z+C}QrytYMb&qx0w_FnqZq|8XW0NtR+?D?KH@x>!NE#4Jlv;9xWb@yXuJ6zieeX(A7 zkCQGeMJ0}o&DzsH_n4WtyHEvn5srf0moKRNc-m^|e2gOl)9F0tF@$9b9m%DeUfJ9s z%`#=x$YetVHY2ZQ-jh|_=$$A|ez~)C@fdysGQ<{#C@>NyWyJ6&>Ecj2xR6QuX6&H) zJ2cu2Uv9lN+i}1He#ebhL%1c|CpgdjhAQ^rr`L{cqXvOPQaT?UY|F4WG4_RpJ7)NM zV(0YvboC?Huwujs;yTHV*xGKin8?#CwXa>`;NaUx6^G03 znKENS(~?hC(RHpkz%}A*$#*A9tp8rgZeHEp5ecXswj~*GGgY6l&=;^J(^8oKfAx7JQw{|f}y&j&N7$7r8!_Q8%>37%w;!TM?nENRRuny=~mmCNO< zREWAGL~m96%e5MTshh9&;(pQ390DQeJeM&!b_;sBXp>Fm6+ zEFHDrp$tdgUaGwQ=jNY+ZlMb>atHpc0&CprIo|e(R7mI5wFC-J3dfpGksL#j-q07| z?x_ownYNMLl~#&hE=Jiod`6l)a}|F+&9ep*Asbx>cIwy@eLR^G+D3EDEV(HdRXuU> zj&=J)3#bEAtx}ZJR}mEm8uC6aLuM=8a{SLI?X-jxL2}iX7Nd_1_w9YrlhqpH{xG%B zP3xZc87xlgs*SdGo~7y6QevSSuMAf3AtH^V61gPvccQ`3nuYflJq=J^kJ;SNB6JI+&xJPe7yn70^s<0Esl$0WecLR_`u;2o+i}~3P%efv$Q>L} zm>2_pgOIYyaC6UtO*cyIZ&xl`ruO=A()Z)5Cr#al;vr#RY9QW;Iu8s+F{AZ_wsUGm zNzz^qHJc2jUhYucuw*$glmy#a`cJOfgg>18>W`H1u|}FSN@Yn8wn*)h$;$lrKq!qC zY#TefCv!G{fA>G;^SA|0br%|?C^QXdJ5#`-8BmRe8v>hrK7*#x++IYlR0L_A=E8Eb zLF$aE1gAs-C|$s{e7{_4?A_zj)_USl&3#Mo#U;c};**!Ul^mRiXNVf)@g~VF3`9q8 z@>a?)Wr}6Iqz|(KQYSFpUZmK@iYzS3e@r|qdECx zOK6`UH0jeT^uyZV9;A7S?DXl}A0w?&cvbGlKJIIeCryD|6rt;?7k(DM!h~hM`VK;6 zE~1P=iV$4d4Z>Da`Q?khR*9d9M#bnYjxD*LYWO>e$thDZ^u9?McY@CXjAQ7jXR}aL zwdO}xdGzS=iQ`H7_Y^E~I?k{q?joy&9aYsZiF?2vvZ*KuF3>p43%N-Lf%`fN$6D*3 zT$-iGi5}{~girh<+ST~zf%mPAJG9qnf92i)Fk^BFVDH-QbNi&z5=A4a0gJ_FGXy5H^E|ITM|y5Go2FtQ|f8Q$wP4AV;t9+70muEkV~M|+rRi% z<%^xwv2gl2o>UWP{Xz1X%~jYn_n?DTth-a!`iEdxx)lEdS*%Smtl*KHyhm>=fxE(5$Of zY9FqqFVbb)_$yPw+DtxxogJ5=e}XPRkCS3Jvs$`2mUa!7fx|#% zKL7f&-|-Kd#h*=_m1b$N$}oMI6%Xo%%JI47+tEgD?iK9!vJUuN*V0$FB=1y4yRhdc z8nLN!cMI$83gf}%=6#3cNgm`;M;*OWxXq_5qs_u;@>-o|pke4nLm;yhTYd9z1J$EA zxmjr4knUX@M$Vn(V?^Pt@ci_1^tg_=l+!2ayBlYjc;ml+>|1~a|NQ}rb9UjqY29R! z(Y;4*I6_gk^-LMFrTqu&9AX*Gsggsv-(qhfQ*x`gsbY>Q5!)IMy_1HwG%$@e#ZC`w z(Cm2Z=+K^8C6P8dGK3pd=3L_ux&Ke`RsW~>w$_LJxCM&ulfjR&@2EUW%Gur1>)^Kf zryTyE(hyh-Oz0!{w0~vni2Nf@d%ty4`s7K{@@`7jz@4+Kqa`QjmRtI$^5vMezpM{F z-1fi?n?-r*D7*x&XDsYU8ng*Uw=h4kysiv-5w{$V{3t(SIeu!+sHv?s4EL!@5Ipyy z8?g`j$1;a$>AW?7av@H~ z|CW++bl1|6dV$Tu6h9bHw~Dos2U?%@u)(7%FuY5$oefj)!* zPOy-qY0nGBFKTUQ{@84KSy(HDUO9%aC~-Kqy>1Lr#^(OT1Gz!_LE~>lO2eX=4h?#2 zNksF{I+!;lchroL>jHo=oqagc&^JH)a-SE`f#^vJd&_tzjs5BLaYXFkWFQK-^2Y)T zV-R{q)KZAK-^;DbPL1mxoULdWfJ{NT|5r|O3e!6O{)qd$hM?-GPYZn2NjYw0D4aH0 zZ^XoIdo8|r30d-W+TERQ4N?$Hq>xrb7?$HUODh$#xg1L?bac{#8lN+7C>sX{ zD_nQ$TIcpELBgaspbdZswK1Bz!7t)I&O5_uIrM9O$lz{q-lJRCEX3OqAFYDTEqzAf zB!ZKYr-qr8S`1lwB)#@^!}ur*{l%XmH^eaHe#`8Y>Iu!5Iu#-YIU8~7hJs9)1FrH_ zgtZnYc{?Nv$IdEa?UJ4$i<41TSw^t9XKP9XA@!XR+vqa*KzdpK;HI_=7C8h?SZ)YL^`G1cB+tt9z7_4~ zfD3sDTZ8NJbR!+(E~NKL>GDz8LX~~XUCxq@A*d)ntM!dATm-Yjg@_^|({U@V*G6w`^7x?o5NgAsEDprDq}$t zcWIN2>j6k|+}IjC*mW3gV7u|ON?}+pyA?hP3(2+;_CAZx1jFEOb2kr}_-uh~C2bgb z-3`sBFnPPQ$Keqv-Q0Kksq#nX%34&NZw=_4e@QniahR)5{>Uv*1VpP2I5EgB<>}<*(#V?Zn3$^@5{v4-2 zpj=6(XHpF0dM^{s>##I$ACpKDM$9JHd6;G$@FxpYIjuKkaI_RNY;Ce)z-87nc`WiJ z%xzcU9S?wvz3@t3W2Em=o~D*{C^qiLmXY06%I8im(+zVzNo6Hb2le7NS7)G*uk{aK z@aMF87+`T3rvZAw-M_Kn{F4K84KT~mPJZ+pI*>Ur>7gR4MLP?j4lG}OrfUg&z z(SP9Je?-@c7a#iV1aj}VEIsK)^}~Ux`MPP`PUItZ`qTR9WmEq!mzH$YSs|6nXG^N#>jG>A@NK5d>ew|!)auk(hGt_6y|fKEl8_fU}dKnK)57erHUHpDqh#91PHTwrtjJ7U%UsKG0@<>UfFFpEd$+Umu5Z;m=7q&Eggmr;s!|vU_+4IKqo%kvyj#RvgGy@V* zROjx{!S>LVSj}vw8RP6%h9YSIjA-;OP2vMt}@b{uBBIQ7>cHqo&g0&sT>n<01pgx z>m*ozepif!hZAGaJk#r$ve?dWWyO$-I6oc2Y=B#wYP2Jmv&C-a5nqr-QH;TKwn>)0 zuC_RoyD5Fo*J)o%@=xyC*3;y@+wR63o&vSB^U9*)J=yZU7INKqGq1AngGCO;Mdq){ zqk#pS2&5$<0#@^=cr!WMc7QvdNuT_i1v#RBW^=z8R3^C=J~&0E;T9`&bX#vlAGKCOC-IT}nk9g9v=}n!n zvy@*h>Er)pNAVXHihg(a)+-Az-1P?<1Dz{uGaedqgvtdzik>7JZMi7=KdP|bMuzJ% zf(nrJA@qsodQ+eZVP1wS@i~tXOw{$%{f|qrx5O)F*;VvQT5k^p-`cbThCN_vmsV;6 zqIup!gs_KjW_mO2hkfJI6CNJCJFzOWkvKm|?D9k z?}@ue_eh&C*AJ4TnZb=x`?iD?*V{j$E(bM*Fy>S?_)s{T$J3PM9=7cP+fANEXLiTS zm07Xcj`F&Ns^W`%(rJq=pk2^>Yr~uo&rlu~?KelzEv&&!U`!2!c#{4K1DoZv6Um>s z!KmOg#nvKzxJP1jy0)ynZ$*)T#e1P^MxvlMP$>Q}Pjo-Yk((;e%@L{yMk^c_fMcF` zKlFg$gHk7a@?L{q8i6k|xis7R`~_%$?fa@&A~|&>msE>FrgSU$9q;~}-rDxW)c*i< zyEjgO@SuE8YOa2w9U+SFInYCQ?CjS4rO4VYUA6F7ObKm}*Z_TaRH>ps8_#$I%5Ymd z^sdft%)o#b`Pn9OFa(q1D<^mEDB`K?w_r{kX@}&`{Cji&KqUCkRyx3V&O&;K!AVeFN{hcdVcw(Gf-@wgk&~)ldktStLIcNCWdw21e^0KV(+>Fco~}`V9CgrB~ot z5^eUDRnB48OpI2WWKzVkD6OpIdQ`CMZ$o$N`yZhvj^n-X0|+RE6z0oUrPyxO;XRaL zaA{VXqs2DjB!AkXs`8}-yQ;@f0K`k$-cUTJhXQNqdSKe(ORyz}#_V_32>@-PTw5ma z^W`I+?$==Q7&mEjq>f^Q`z9(<7EqvGcd>OF+R8<{BiZwoivLMR6rpCB3X&P>Y(8p)(8fi;mdu){Coa?M4vCH)gfT(WXw^{rV~5V zEmL|lp(@~gxFy=dBIpxxl6Or%BEj_;EP^?;s%&>6k4?Lf_Y%M~0FyrY!EQY)6X&b3 zeJPMIq6#MJBKF~)D+-@TCahq7N|q;S@?TKddVs@yV=Kf107qOptyIP@xX1G|au;$| z4o+hoX}%@Szi04%LQe3CFJZs}x($vqQvA8;;f>Rmf*1ltg7b>2379HBC;ZFHea?J) z&=Vz)GN_4flwxNAVa$|!?>*e53qgQu$OMm^_%gXIq3dh;FJ17-7_DYsPzWEUA}v`x zayLLV1aB0lmGY}&bc?@d#mjGUfLT@%>r|bY;JGz8JdgbWA+Wgv_Rt(u`Pi|#)3btg z^d#>Qu9mb!1JeCiO%*Ui4r0zl%gWf|0)4F%I~+B_ZW}KmQV}sNU=Z&p4C31(4a+$C zK{CIA_%AtWR?YqgoL)_D;tnVd!=?wovL}PSfGA&=>L%in6ch(}AX8|i zC-(VqZSxP68ZyQFt{`gE)h)+crxk~J!IRF$2M2L)gh{-(DO)#jZWp%vbmrjMtweJA zh%iHdqd-qp>koYfGPd(0OchG%sz+))R17>N@x$~cePeI1tzNFI>%RfQ{@>z)d;14r zM-~_&(<@&P-(Av7^%BSX5Zvb`-2D2trRfNDYv|hk2=d%NBFwi(R_pySx-R`9{=iS= zs2128v@2ji+D)RQbh)W0-MwK#o$m|S9}e;FF*uFGcOghlxENR1hD4g4z|s-IQ#+44 zHdBX99vZO~vAI1Nwr38YSHpLzdkYjXF{hT*A?(+B2*qBOt2S+$Yvn3iJ?F=(EHs_O z%Q|YsQoz=dL;JS`CtR7KrDzgxgQN;0-T zE=KPkL6b^Ay$Y&ngIaT}d&Vy|Dx(p$;K-q)W;+xEH-V3QlswpQTy`|%??Ksf!Ix6+ zFVXDNyyj0h&3vBG*K)}TiJpwPqw{s1=QGkt6T_J^q(OYd>aLrAl= zRI-e*x1>2cAZ=qcOVL6+pz@sL29u>8%ThSuDP>9r)SjXUFrc}zF2KOC?16TdWtQQ^ z48d1uo0NWLQcPKX_;HzrCbesk*yM^m;vD8U2xsI&+xmHW1#G%P0jJ!ptj zMe=M2+^5)8&ho&7sYn7)L@So52>4psYqrK}mUNI8t#6ZR+z$4NI;!dxd-8C%d<>Oy zAdlmBG9vM=-=?$v%?$GY;v2=5P0Tqdazc;5EOHg)Va&OvtsCfi@@_&He*e>6#UXue z!cKK%MQ7Y%H()@1)umF~Mlt^;;0vPi{+{mDi^>(AiAJP*{1-GEKNt)HK4};fsGbIe zXh*3f^Zf;OA;K^;(g-kh;>a4d=5zZ9rG*JH&J&(uI^TUHIjMpE_gfLM4_nDD0HTKC`d@S44d{}wgJgm2QT6``olM3a&1;4E~q9I%5zDXs6KBvD+aZv%EP4U6bYYFHI z=}L;+HMGo>h8cWCIpPv4!MfW9jdZu{$@BdYHWnIL>6}NbKm17g(ER>A%n~%(;tM9~ z#xfy#7aCo%{TROVx=ESvC=p%O_$XP=A}J04odKL-s-xT(Xwt@m=sNnrzUsihGnX{n z-->P#Y*Y{=5{zYflUyj85l3%ofR>pX1oBUi#gT`O{C3h_g+D>wJAHYXKnMKL85B z{cNeEiN1(roJI0!TY4V0hdk~k!8nXx1kqQ8n7o?Gu<$KH5x5yAE#it(zF_bTm;1a) z6?_*AzQ@rko0+5kvaFu?kvpi{)*pQHN=QZwqZ;Qq*edcq^CbOa(ivN9J9Gp+#nsX$ zB(fbyF<`OmVz39)i;@e6a8t*VYCb&=HzWQaExPgU;RpTaMGLDdM~e#^I72H=^+ipR z6CzIRe@Z!H&{$k)2{_0zZ;3BSVT&wV)!q`WxunHo+WT+mF0h?QEN>02tKwO`U3xWN zxt>&)StEtNcKw|eq*xc@FK5%3Q2y>b9x*fOxx45nw30Hi4ZkyB@LANEWc!>F1#tln zm%L{45V1Q@-Dt3=gy%~8?&40MILJ(#*29GE=s)$ zfmU+g?07jkIsU+{q!-Qt`v&HR%-|E=fLWf_tFK$KDz_lUn7`v($if+P6 za)7x#jyFpwCOxHu%?i)`%eay3A;~%^)36^Bk?x7Ra%$e+ACeC$PbInICC&pGW z^S-9CSs86~mans$_;ayHvKamF%6rm%#H=>_5E|5_z!UpN6zJ&C=JdpM(k;FVG3JUt z$VuKBjf1hR2qBA%Y~L=frts}cHPBYLhx`L|kOijQ=yR9*?h*Oa^6P3O&Xb%B0^b4FYG8^TLjklLm1;oX8qfy===4$0VFY0H5A8w^t@o;Yu1OlZxlV zYxa#<;FcxVexGhY-dNmZAkMpRJ zom(z)6`7uiPRutCQ>BMCdW@EXxxIICsjkmH@zsQnE`cZem{u<@X_WthZ1VsIc$|>d zY|~Kux|WCJ6!L%8vA9K;k?DIWQ*Yd2&uuF0^x#Za!o7DZ>@%Y;(V+;(td-%Qdy>TO z-y%)ka66u|zb!MGHV+fVS!B)H zG-%oBl(%isXf5LJlq{D+1M}y^7%#`~cc1r0l&Y`Jx|Dvc!`4;RUuAufV=;}8OIQrl zADY=)6khW29_riPGciJYZcKMnP3_zWUl)rW87tNx$S+Snj(UIH>wfB2*RL&u7N*`_ zxp~)DU905JTiKrwdSE^<|Mj%R_FJf_#H7DII;9Uy%pOY3ej68+ZUs-~aB`}sOr%bK zX*Tt~p?>+NlhRO1gk-kBu|flK+Vne2V+6t0@K9fw^Bzzo=Y_vkJ4~>=@KitQvEWS= z4~^-)*j$QuNpp=a-oip`scb@kX$!(*&v@h?Rm_SrzHbCglkRq^y;REQGQ_qlevvD$ z81onW*(e)9e4BgfjDrY!!$9R04;4H-G{iw^z{L~q0`u}bRJ7S^m}q$*y70xg`$giQvW`%J8?rVb5K7S<7d4# zF-c9hR}wD^oVi^%_Ygkr3Y%yjJ}%IO`TF5o472iApYJhiucWn=(u|oH!Ej#M7t>~ zvR%Dbp&#F(cvlQkj+3dVu+)g=&_0?$yuSb8k&PD`5NTz$o1gpWLVmD@uR^hvT?D2+skOlG=)@u?_SaLJJ9spi-jZA+#j)z=2T25D0|4o4I;t?!9y8`@aAEe&6L; zixpsm!C$^mW-gP-}9}PXkX`hhUxcxd>MZv zYTa7{qTT~soxatiP3Jj@7xI&agPc&OauT`;%6`$DCVeaLf?;_(;7$0kCV3%;96rkVWg)c+} z`d5e~F;O zeFrU%YZ$PIR>F+90>FWxr zJ$E7uGW3tWD%Ra~T)t~jhpj-lJ?ETy*1hrPU2`AgcE6^wL}8ow;PsP4BKk7v=&t-N z;MS^DoIwf9xW_HJjb3`|?KXqKOo3higiiUk4FPQ|eP{GxhYUu`1j6?3%Xw z^B(ng>X$!AUwynO!5Z}GfSCNzVgtI7oLHDxaN7L14{LJj1Ukh>>SFWAYK>y*&Z~fn zB<*tEp1IgzDsnsTM2EM;#My2|u^{8!$34^TzZF_@bX7ASUpIkO6O{2|v72}{;a1#} zrwS!}cTFx^a&>>wOq$#8F!*)r8!m7&#Ditqs(MQTZ(j%Ht$=yXuSOAXvr~Y85|Ycy;^nQC-z#Oa;AQ zX}P0e$0tfQ?)+1x>`%a2oCMvH+;x1k>Trxfmb#M4r}<|oTcQl^ho-3@8i2QUdVP8B zZRBdosxZ(iL%AiMCfn=Y>QnqfHA;YEk?v1t$;7d@ZF!MYvg|T#-^{C<)vx1^tgd6f zqj-Nsm`7dR=|FhS&pr$+wu@6hf>HVx}8NN+IbT=eidsS`wb3zS=C#r%W$^p@&A`6MAp97^62gRg|%l8sHo zC3C|b`qi1fU*o7O)bn*%)a)R@Ozi|vA@rwi*)PP6dJkykbC#7|Hefa!dy#UnfL)Zo zC1&S69-kjT<##NyN&9}+2xXWc@fXlZO2EScEyz~ZQB(% zpAVLg@1LZzObmA!|KXwk#85N6Mau)eJ9R^Zv^LTKqu;#D(6Xv^*Upym_}m)#XiM@3 z<@8^LUDFC#q8S8x?|~a;%xIsj!m#F%kNxqrTcT9n8PuyQQA84mz6{2Y0(JX?a~;NM z9w*moZoqCk9=4_}6TW@||4`?la&z#h=brr}U5;v5lJv-#6*Wb-_^v@=&tGBb?-cq| zSZ>sOFvrt5xmFh(S)edWPp$5AaJqd0ZD-(NuC;os|I$n4!pDYZ#$;TCDbornXka%> zfuTp<++{a3{%FOhEqBw-(oGYNR6;*xeD&80=VpORUUa=R)}q0<`H^ERKgAasrMqt| zQ36Lku$#}?pIi%^78|R%tgVzkpa~3R>R!pDUub?(1(wjt)91gb zzmDai$gzbz9*LdHobpLci}{kcfR%&Gwn;rS+?&Jqx+zHfg6SLyr5Ih zf1WA1WKMWWHKzLHO@Zz}aa|K2A06#)r|NQZH5! zVh3*OQLUlf!@dK0oo+-`4{C)cKWn7k7DO z&Y%F$Y*1Fz?&Jc^jWt;UCe=+KjDf|{fhA^$hQ*D|*$&!JO`_iQr&DSa8G&Q$8{eRw zTQN(&CliN>ibGfQhHbtqh@yO)okFwYYwtWusdipc;LSbhsR3lnc6>`|okmE1%*9}D zm@-@7i8*YGP|W2U(0D*&T}4Fosmc`;jLWV8y{&!x&i=Jde_;D(KKs}F!(&0SCKt6d z9?d>F)_%wTSV==(*DEURtwFlpgC;UPKd^i_aEtH&Ef^dfmGSe&&kWnkK1-zTM>54i zHORn1oc}_b%8gBoa*jH>(>7d?dt!F>QWQ)hZi$h-(->bX7xe<~`0TkXd?UVACXOD} zur1w)g{qkyTE9|M+>~gRu@Fl!)xvn*#=g~KshOYXtbBLO$sabIxBz0|@R)gz4~_M& zL2afFE9eSd+5-`t!Fau(1ZAzxXHxj-B2Gu&(u#DwkQ%eI#jbEKoZ zfOo+(iF=B8-qn);am1&ex_SMMn9wMdCmk4Vvg_D(WoBa0n)7l< z|6x?L=!mEsSo#!KNKFJwtCU1Bh@ebk3*3t+MpT?>+^b)5n)Agam)4!3d|B6&T)*9A z(Cj{LN$Iej@scrd)nySUx4Q*z^m+LnhbYZ_5IcTo*3JP#M1-2%n(OFAb*nmYpU=;N zt<8?hVns7mn)a$3zT6ZQXQqE&ZAsT@<|T8@*ep8&=~SAtp^r#&#)BG#Who}%k;a4P zmvtkY44fYKTYna$Wz1JO9KPh`qPrZNs^SKw`WEUYX8Lp9>3R|`f``4kL#cbIx6Avg zYQ!F}?oTgSe=O~2ysOs3iw6CM?^&KB=R!|BWn7|?wxuUme*EdgJI28Wly!0M6;vQi zaMawbKY!fQO{Ov*&>Iguh$nE>qs@%CXKoeys0f2z`%#ZR=q=d1 zZJ9aa{Dy(YSA@z!yXpNmFG_(WY`Ew1yF*O6eI8(HXivMwy^ChIN3PUQsS<3Y94$_7 z_>(06q){5u%){+nn|s?fUpNixcdp^-jOsE$_AJCo>Cut-{b|tN>t=gMo)2%2J;6my z_NS9%;ym7U7HsP1c+ey)qFsut59mJ8-A5h58&lYvf>?=47=H1M0jdPBDq&K@xuKpVFh4@ij$;5O;Hp`)>0)tulqH8GlgrD>D zY&L(x$Hy(GoxHU{3#b(!_h)~!iwEl+^mzYe?-!yAQEmR% z@Y-Ino(*3dO+dDhK{TJVVZYhnWb_^rRUbT6g&f39n~W=u&H4L(NGjqOEzn-wslP2u zC(lWeIJ-v3!qXy$jvtfQ@_Fe4=F;aYJ{`UHY}>6B&#JU0hD1EG+46ducEE*I&z$yO zvD$X!7BMP7^w^r`*VeCDvV6^DofhpBy)ORkzMtaDxCwiYu)E-L zXpQYmHL^D!Vj&}S?F)@_ybG_7hsp?yr$}>Xf|{l9jNm-BKCgY1rGlB>vk+rv?XDuR zW8SVDP(2zyhh}R=CXc(Nnv!4gFDLMn)lbCfJ>2p0og#S2L1caBz8S{RrMPWqEF?$&}73qd_u zSl$Jdu_kb~bAde+iR0%VRuN|FhY0u}%~tJkMyq5a#d`BZ_W*A0QZb9S2^<$zcLMxK z9PhG=Y@K(0k0!)f3Nr@gM&&j%dVk6`_{($L{#|J3l?a1QRn zbAQSS;-R%9BE@q7({qapw(*lv&c@7X#+HnGF9cSc3x6_M?4$w!*SV(bz|6f{%NUu5 zOQ06~X3{jrJBOGPkulqIvr?7^j+`vc+aH?r*frI_QI z_+%6>C(TN5kxRloO>bD6R`|996i~NhG2A24)x1{Kh8lAx;E-hU+36Znh=&sT^p!=J zPLW!EIeBc8+(uyGt#IJte%n;<7vnVZlv7P=um$JWo5t6P}G!x zXsC^fM*fJRtwQxLa(z47h!yr+Q79c5;n2iX9ND7mhq-jYIqK`Ez#F}J2KEM`eY!UO zt^1{bsW@7w85=#Gz3?O?q^~`GPHQZY^NvOr#mga=gi(lms7}B>dgIzI=jF${tajUe z=)|vyFDsYCGu!9;-lu(j0d*4631jqDu^xxV9w;6$q9Wupw%Orp^}~|UjWL=L>uF45 zbS_PEj2CHQJC)0V@Vb@;cI50rNfJ^3+N{nu2Xv=VA41kREZ1dbHmC)bCYWeMR@zyo zwwKixnoAtF1Lwr3`N$kvOSG!D5M~wTUu5KkjwJVH;(Y6omWdjXoq6=zr4xFh{}6n^ z>n24r@oyxcKJ2snc!g?r!n%3P5ZiGqUemxK&G zoUgnu%SNUXjS0pK&d9Jw z(D+iHs%=)4*R*tgS39nIEYfQ>t*^qE`=CUpbTW4!S=L~)Dt|?!PjQ$R_byxrZBC*n z23Q7amVT_qAeZDm*Cj&gay3_#E_BV!k_?Z12`TupDq6F-gi;uCrcXYYa-3}U&}3l$ zcUw|=_wJEzzef#VZ8pE;q~Z>88;0s2^;WpI?zb2uFN9_v;N{*V(S!{8v?2pxa)E0a$-uNh+_Fd)F>HkVbHe0hTNiH zPbRM!dYLPkr>I@IJn(^1o_D)DCumNyRc72u(B@5v4BR~5QwI0fVaI226CbDK=(Tgm zSBzGQB6DW;Vjsi3U99WX*g|O`OCQ_{ttKEfBJv{wFM=8wPUy`OLj%Qf?N10Q7Zkcz z;xG|Rro>JAU+A88EXPS%;Yu8})&y%!Wh}#u3~}#PUTs}iIEb681F{|IPu9wR+J1nT z!(>S$*`eqJ#RH&!ccd&$JP#C;a%nHR^7#WLI$4wIy}NN|I)t9rZhH6YH=ea$zk~mz zt~i0zwg_KDxu4n!rwqyLu|m{HbFRKk`13VZp`P5~IzL|H>EI@T951Fdonl832C!k& z?s3`+*~?@WebDo}`j?r3lN-N@FQKy#T8EExpH1g#7gVrp@+zO}RPU{2tK)j~*;+SV zN1#b?O&+(g=*8c7}#K=;=wPMYItE(JYeNbI7jSvwbCA&VsV9@?PDC zA-W~YDpjXkI?83@7Mu!&16opVglDgLFw;>70=!W}4=jZ^EpGjY>+ACLobThv+=%Ls zMRP+fAsFRC6)W+5_&DS%H=6eWk4u4@tKj$&N>HBR22Kr*+h|LKxYD2JTtnTsqwS{= zogEvY@K*R%wMFV@t7LtzJXOGZ9Sq4J=t7BOpY`nPR``SAwy!t>!OL{qmh%{g-S!Qx zwtWry+EU-CzgK>yUheR1UUF9hQ|CzqWW)J6-FsEJE}Jl4p4wdnhvI{fulAg+oExt5 zzhtn#at#8}rW|P@ID#D>HB4wUK#%8)f&W@Ca+oXMC=mTtLDl^;#c$&p;bkA*EENQL)mdWr`b6|EPooNKBp zzGa|Oc`w`-2TyyHn9JMiz3@|5y65#%kCJtQOWZ0j{(oErf?1`_-+o7+UZ-0a7*sB= z#^1+BqtlZaZ`bb6gyeNfQb+OC+LbX1=g?dh!7+&;fk1eEcmdHY4AP=%311plajx&w zc~PhpI67(=r8oT*-cp=wmO7_)%(+L4FuS5PeUrM$f9cw zb^_+;*1&9k+1KkG^(yZv45IN_sKiF^r3l1y&3>qlz)EAhjSmgwqKlKGwgjBxQejc> zfa$j{ASsPQwh1+d36va3z+dXxU=+3swDsg9fvpj-H9X>?8g=@M6y!|s#~@#oPVwP! zSER_v!B6lUf!^qSjqvp&Y>QV194!zf=Z74^PL3LWL$Xcgn(~rwzvm`kEGhc|sEyl! zAf=kT&&LQ(Iq#9XJ(UATKrCSTqC*aH6(DQSD?}6-ygOvd;K^BOA0@7ds*d-=HIOdA zaP;0v!gwxIv+|y19AV*khfm;U#QoG3i6YH{2*2t!3HMngXv_UjkZHP_!ylpYKl+SW^GBbPB3|%w;7{$i zQQvgD;v+0xIf$` z#MOm@&}_ho=#U^R3+@9T?o~mGdC4Oy?|BW~G0c9aj=v35dmt4Rev(^^>+wJ!Zhzs! zJ&*$4;6jS}t1+e-<3$Ky9;!>t!siiV>8>1mNDakG{1s)jEZuAAmNZ~lH7O_o*wuj8 zTKSTn244lJ+*rs6`HgFE(Py<)m70ZAzJvDS5HKj8bkM@n+k`9&%4aL$`N@S$KiZd* z0FAcG5hZ*kJimzFKX{_P{5a6D-!CgKVz%NZi8NDlieqw7TJx{DyPO=bjzOen_)q*+~qCP zNJwu=T>@1D;uPChrxpR;0Iz5O$osAtugEB?udB1S5qM)sa4KMZVCj_HQ+t#A=Eq$t zJ6~UiNR~>;i!8K9c0PN zX6^-cq&ps#mJ@wU17;ns`!vLfdk>7NY$YMxBXZr1q>{=A5ifrfP<8s!@U2)QA9-aw?j|b6feEVrn z>LNLXDPWK}lzhdzY=iYZjdlJB!kr^E1*s{zm4Ln&d^9Q&sNgpt{Yl03!b(HVa4TiD z6;@Q{qr5#>?J_7!kDtWIuG2gT_IXB6lke)MU9`1WlEexxoAy%qjDOl)2 zA)L&C(;0)`!WcPrqNh*2g%ly}OsYnCA_smB%MpT_y_Y7mU4r5?TiavP0rXg7ZWXMB z^S`XlE`7Z=^f32%f-`3lhFF%oRA6_{*CUrKTvOzb?I9%#M>HA41_<YfP&t=Ja3=$$7XVh^&mGPQD`=3Mcp%z47+O?*i0-FSk#S+g7j71!QsaLrs{}Z8T z;hSxvuKkmp2)DD)D-JpJN>ZznASb$+d0kWe zT?|E3$Kzoof{`T&pJpHK?QVo9z#8X95Gwq!JeViDane)CF+1YpBtBlqs?B$(>3(x8iPW(S0R#SMDuqv%>G0JL9W#Di1ty# z(#z@uCHHIRQ?-EzN3XzhafrGxtJOcS2>dg#% zz1E?N*RhC#`&E<>Lu^Z(Q(Gdt%Ku@inI3_k1e^Ff6dz-2G^+^D2W~FtZ+6cWXOMM5 zE~<65?}g&DUad*DPt_qQpgv3y9Mc%JNh6j-f;TXvQSXdBuPuJ~!0#@hTnKh{>2E{THK%<6E$2s^2%H*yD-wvE^b%@pG zl(5RB%}X5HKSCr_kX!EN^!=n%1rKyzd#o->Eva+GH?;`c4-VS>c6GMofH>HPsINzQ z_>da}r4od?z!dafg02!wfRSDvo(XvW6f)Y%d)2B<1}>)*_v9TpQUjqF3sLvr##Wc-;G-j zdEpJ-hlmv<1zM_kcTvKOvD00FrMPtj$DCOA+)f3(lH@nql#ow0%T=%JEGJ6~l(70fN%)3~)*Gb8jbC>^Q=~lM z)xxt0>q$!abh>JPs+S4fbJdIv@l7NPRR~;4t-hC=RznTW_kH`^t9_qmahId%%n$4H zq*V*p<18oQDkBt)Rv1cvtKeTiTtyh~;VspIWtvz#$eoT{Hl^So_>C(Cl{X3PvK#32 za!*UYQjqPZi#69Vf<8W>` zZjydk#=%mrFk1o(BR7ZnPe)WcDCJ6aehO*~oUGp+$U5M<*Jq_F`yQ0jWT2&XSm1%> z;k=p~_D5bOJ9@2~b678I?=5BR{yi5;TFiwacMa)zA(7b&Cao^K66UAi^%r#&J$QAy z2|*gdxxEBt@#N{Uvq4^ddAts$oIJ~sx6%uB=m~h4j9mGsreLCF-ET5HA1?`~6 zy}-g$qV0!Jn`@P5wQOULg+h1TRu$Q&tU1mw^1Da<2E3EW^5f@aK%VEc5h~KbzH7c` zRfClZ${?*E*X2tdq>@bWY0O?JS962ZF@chID5Em3D|_dUAYZN4@0=H0ghsmHhalw^ zJ=%m6(Tcfyq~s$=ZWWcIxMi@~+S)zEsyPmw5|K>d&WQR)UP9PNc0{U}mwYtw?vd#? z5MD*=Mmdccok(uZTgWW*rYIZ|UQ<>j5zs`Bg$Rm+utQtzI`PP{O}cOHL4>48&os+% zf?~^Hv?xejqouHKR~PaHW~P?tG1_(ZEE=KSg(7f zJ@?bKE%|h50>0aL?P~arkcI*vPr3T_SLd#@r^OIeeO_@<`Q3d3a_f! zeZDa?49g)i;ezsO%6MGD-6IL@m!ZQ17E%AxJgvbvn}!q%TZypJqr2I$(-`xD3#MC? z@8Lp@0eT*7Vx=d(P|b_YPcu^}8cz<~zWlgd1U-kzn5Wf*khMgg&b|jJ&6Wdr{&|o_ zyp5N7S<6r}XB_{NuUhxF{U1m9Xqzhc;=qx!0;YId5VLb+g5ys0kWBi`M z@k+oh3$<)ThJ?*^m+z_B6W(Teg?@@5ho%e+I$t#79lEp?C9pQ($ef5Q9rEm4D+=&M zt6>d9v#i&Oq@-C29R%mFpX;;+%+|%1$>y`Pt7!|Ta_J}c$tMxED&I>TdIBfn{gWc^ zFyGTUf?i^KZjM}2_P+BEsh2Pae$D=d05^5G3%0~*4tx0c+KwGT64U6-ZIJ-iH9Wk- zqArAAG9(xKEqDoBrv8x$CDn0dFUJrsbcv^jr`Xo%6#Jz`)`xf1{VP6*IY8M(=1gBC z!^2Q?lEH_wgpp^!$=9EEqTxZBYS(1d;J8V$DRAr{$n)Mw_PsoQ{8;ZfM&_EICiCG> zgX1mg@Uyz^R*Yp(d)wf(-CMpTvEO@}>y_;7o_H`AnTEw8yL~Rlw$k;S#^E1@LuN5I!1=j6@QebC{p34u_n_wq<(dg{vt0aT!I0 zCgG*Cb2BKqHI$C#F&^x${{`@Sv32G_h@_RaT0dvDp7I!L$VEfW^wO2z;LWWPxFWy( zPIb_Iwzu)Z8m5&3=?VQ@39#P}%8se!HQP;ij2+>5FHOC9o2mOxFPg9kYG9IIhu14FQs*8 z6Vrj5qZ96KRDc;v>ACFL=pp-B>ny}^WRzCWZ5Wkn0;&|QBBtNGfkI{3kk5sbDZ70o zo7WFBahi{fEQ7$jfT>-*=F7}9oa#ZTzTj@;ICd9azA_|F9iO;+sAL0>=d+MtZa2z6 zzNa5{l%-b++crp4-Jd^md!-(~rS8MoX-1x0dZo*xa6l2^I3@ydq-R31G?(6%x5CEW+_CFJGB_MLypK<=ITNHIL*3ENsbBOU3Tix? z$+Gfy+!~E$6+WTsyx>2bZnY; z!7{Hnc<6@MVFlp!opNXCy6p=PL(U^a>OxAQC+`XEz8t%ZXf+2P`1blJ%IoN{P2ZIX z?JBHuimA7Z^YrU+Paf!n84yAC@ZqJ^-i{!R+MU+5p{p$S@Pt|p6OtGSyL3apXagy4 ztVP16+y|Rgx2Mt4-@EV8LTTNe*FmllatKc(J$tN8;-no>mn4W*Xq7l~$%M=%F2gCy?B^5@uSGsnf zm3-`WQeN;A;g#l$EJ3y{)U<7-akUk(HL6|vDpL9 z_gc-P{5xm&Nh8G^$m9@bXYC{rfi6u`c5cX7t|rD&osPEv$2TZQ?Ss7cpuPttGaHRj~#k(1h-JVEa%z5&ZeO z@@D>Qb0IW`H)-~~_3+|(EUykRSBW;t6EIyRiwy`mXY=&U!%O|V=5p%F%SUWb;4KZE zI&!dzTQ5f=~BhT#PJQp56U<4d>Uq~sY{j7F)iNJ+JF(HWw;=l8`2U>D{gp-< z&EryDqvO%Jf+?)*)i-LKBLA-~^y-Klepp}1{GDYy$zsNnh9$vq7wp7YWff=pru-*5 z=-fST4p+?6;??C0#9Iv<9o<*0Gsv0r@2;)W9Ou(bEZJTIa!U|xQHKek$T0#Vs{+$! z6S8wuQ8D}!H{b9Wb-(S`kj4HIl9Bm|J{@Jo`seb1g7_Ck27wN4Ntc59nT_=Hh5HF`r>&&kIp7Pb%O4t_rdlqYnjF3(DkuzRhQU`@r^xHbBPLhA;6`9cY7 z18D}%RkAJF$Qy>aQo{yhoszv4^VjMMUeYvf9HUlO9NjE2mH?UJF00TT35R9pCdUrm zVk4(%D+w*KbNz0(gIm)b2z{*)N`beo%3bY{gd1M_(WCA2r6@}gR$HUB77T7N#Dg3& z{BSnT7kVLjMEIG$5GO`RK^Om0^JCnE*jY5-pC>w6V~KriN1pemvIPAyzqsxvs!ChQ ziob>jCNgY6g5U~wShbQ=I;ROd=%@zT<3N3>qH0KSoxf+L0p`*~M><0gR$1r2UmBT) zv5jp0^0jW3$Cjm8rxlAVQGO#;JbtP7xRPm(Y6~#l|pH4UU zt^+?le<1J!fgcF`K;Q=g|GN;F5w>3y{XHu9RY*5HWBKFyfxr(0ejxAzfgcF`K;Q=g zKM?qVzz+m|An*f$9|-(F;D0*;XQd>6UrG}-?d|c2LeA3_<_I=1b!g!1A!k1{6OIU^9Wog>&B5vb`32E+oQoTaHci2 z*2kOLKWnfrHD1itXgSp+{~=}Dz+IQ5!&hDRXHB0^>To`n@X*H9T{g}hWwL`~mi8B5s&`R!1U8qtL0tzx>;#qVMhh{yE~#f|o(tacV|Y-7{yo zFid@HulFx^{OcbD3>3cJlI0dA(_31X1D%DM)h{Xk>n;ECS5(936Zk4l&7kUKABB*d zD}4Po-ACAgjUL7q!g_jK6tYm1qOrK|8yNqk&2NAB<{WsYQ#^SzdSq`}Wpmv31?Ycy z>vFv!9m3?mjgkG5jx_P_WdQ!_*0$14%PeH|XW#XGJtH&Pf7e@SX9_HbQ&UfYwDT$! zD)emrs=aUjqaF>h@4LMUqqmR;3Ok9f`kPzJ3evNiy~AR94%7+g!+V|IdCDmK&FxOd z50u-;D4a7++oyxL?N&VhcjA8ZA{Fn04#i3CGeQ$$WfZQwDRK$;o5rKI_%>*v51xZk zjl`WMxBVT_+agFC6r7@(f}1?A#vl3bZANi|-h31hEBiEr*rf_^7WJ^RDx7m3#2dk{N%|o+stLq4(4b$vDZv-@k8X?-vl;G?YZIa!r1rJV_Om zs%nK{I6bbp(3(@!pz4zcju7x`82Srb{`s#4Cz>8lUg50wbcYA=;~J|oe-CGk>yG1H zr|9orWc)2P6g)F~sZ9ZSb~^t#O`TU=+m81?`&+fP5>L+2_EopvnHc-G#K0Cz zHS3bVPh9L?=}=AyeMn;0w!c-1k@M;g#J$^#UNC}K;Q_1GfBsw1yL`R+Q6x&qE!sWn z)fD1YH_>nltLcPC}??w(FMq(sv zo*vQx$TX`EG1C+0Xm20>W=P(y>fytOS@MBxXUAnzosW}WgiH+mR2%^3WJS1+wB{id zQk`Y=#xKz-r>!xm+&3Thm)0lzw}7JUa|FpIwI#SIC4V`>sWl&(ufD+#!FGqKu0%Mw zg*gXA?QdH}f9vx6zX&$UG@?)^z}O=dMaCL?h#(p}==jLA78`U9(}AOv#YG?zYFxMd zG(QLos_K73{EqJw3_71Pj^U3Orje;tZoQq&}s!(uG?$I8`EtpzfRL7()2_ zj{aR!jhzsFGoCS6&>92T6CxkK27YfUzz~#dSnY2!&c?Hw-F73QaNg=aOL*V#=-=1j z_ZJ?<^&APVClD;qdDFz{Jk_8yk}hhe$9OtD#>0n}Yiu(&^SorT3HE^eAT+@ob7@V1 z>&X`XU{WS~9>ClBte#U9x}r{VWZI6}DhsQecb!jP6v-qHCqTX(NQ7TheH` z;Tbuz=0%ym2nO5QRMCuQU>{WkmR!Ok?#bd(7y*bub}KU?Q>TdEn$n8dKic)ROArHB zKo65w=o727rhRrCv6I4R>iMW?S3?0#3m+jk#^vnUnYL!K+EhvAoVJ}71|8=WQk8z7uLOmsqaAY={ObVw zJkLA&?D*L+PJla#{ckjpt3QFwfF+?kd@`EuKDzP5oF&O#j*V@;{77gW{)B&|r6#4o z2wR!S9$%e&YYowyqmji8%FZv-DjkT_EYwkX7Nf>DpZ5>!g1u>LL(iort!;GP}|9hE5x^8(j=Xx^Q{@x z!}@)zlN_${();+8L>$4pkCtj0UNXf>o(;pEH?P^W&uh?ktui+4X`D`0kdxm1mabLwCT>Qo9$!)QL)shnpSFUPwAQ27B~v^iWDa zLniXs@J)E;VEH48J;4?!$o-g#x7DH?s^wsfANQKrchgOX`cS~O@%?=*`}YWXby3+F@ZT7kU3`vZR^PKqUUYc)k>(z#6Fi zw0!ISrxhONJqlB#7dbaaFi9uMz3JevdZjY0v9L79xwGb`y^(2{utGQDh2jcS3GGRV zAh&6L)WJTon~y=I%a<+e(=HfJYfMcVqs`B>X(zTdLnZXOW#Ji9ti=6AIcM&zw9QNV zV6k?7LWYQS0vo9rgksmK^$4mV)>CdAkho^RhFy7CXcKvbE1cK#ziaTfKsi-^`zOy>HVV z>8;0$6LG}^VK&p0OxAIAQgwA62ZetQCrfrSu%0Pc(n;*?62Dj_tzT3YY(dS=t5A%; zNoiRGStlUiri`^0zf!+&c!IP}t+x;KP~%~3gdR<*Y+F$Nf@?RQmY0U~6*TqYAr)EM zl(2M|AxalVwB5-EyR>tD^GPsv-vQ4y-l6x*PkV)QjoULa+)RQqI&<9O782OAC7=W$ ze!*%>KXXqv=PN=MA^ax*S~eVnXC+PQAP{m;VX6>wCm|+%uJ7DmSR$)c zi9|(|-m@vCvZ_0!76xQ32QxAy#NPaDuawx}xw+9+3$3poGDY1J^EUX|2I9R-wItni z!lunni%&&7$zS&(1qxCGqY z5_HUYB?u1}SxnOWWw_}RFL^pMHf2)h)O21%zMIcnpjWIvEi!jFLq&2VreKZM4G%3w;-w4b$9r9i9kHwuumiP4@JLGI;noF;R*+lo zV@S;+vU9d!3#O+XGQtNn1uFpOGt~A`#Pz~sA`V+1qqR~bO{%{7`&xiS`}yZvB*2Li`Hb&-OY5v+L~e+ux#^~2zgc+iB1!iIMVc`J>O4|R z8I%0w)yIgK{O4ow!#h;(3JWY&I(ID$r=Yui} z)NJ2^#N44O^we*5q-B_8#HG!$W>p{~#>E){aBFZCRGW8#Wo*(V7STZvJDoLNNtFyMU z#fPHD^YobkZKa2wOV^X}!fbsDz3kx+=T*s~N$*7#e9UJVx_EJPC2rN5L$*2jXu%0C zAA}|td?E^nPf2=pLbGMk2|KIEN=smumLQ_ab;?5C$hXdNVBnimn}*hdI>{MHitL?n zxZ<;Rg~Jil=p@BT79n3(TCpS0qoQ7(7GHN_GHn$WtE}P`)UuoL%G&4v>;rw!rjh?Sp7-e|B zFaeYcQ3qyWQc&BW{alC42ZiAwTmPRFJc$^Mhc@Z^b>7BbI&W?YJaqDF)zfdTS2hKw zqP2ceEUX^;^fD`){qbM|^&s>VDkktWe~DhOuVz=}PUogsC};svhtv%rQfRRdQInxm z`KoQ($>ArNG=CaiA1&Z^%=A)E6;BVq3#Zd>yUudk`;x~j<@de@URf*NWnmWQQmxf` z`9J0(uERM{24>H=Xwofbgwy6SAMP`_BIBy3FVag#gCBm{#VK7@4r5+^kz18%mZ4B* z*#7f$ZiJs#N}EL>51;w$Oqn)t$?Lny`ByVv)gsZLih8J%YFl=`soqO|0~pO)NvOpz z8kB>~k04e|-k}EvaplwAQPdOZ5x!r0SuS5)FwX>L}^iO3md)+ET}6`GMtD@?gf zGI=71f_w6gf`SgWfV(oFceLanwHt~$ zKUtjr`X;m2V_D1;$%oqpQ}A(G0=?0_8f)a9x~4)YFCUi1GYoHBSv}Ogzt~4l zFS}xJ=qCiCu_={s*tM!oCFy+=4VdoaH&{ZDh49{$+DqC`_^B`fvQ1h6m-PdUG|;Jy zzXT>A7T6My+FY~!nEXH{)R6q%)6tx9&NJn4uu-uP8Tkh0p1uw;KFN%B$S>n?Bengs zBl7jpKTi;xhQob9=)?dPQ@XUGBIH5|Z%E#=r5=q!F|~A-`kn>hK(xhN+_ze9yPl`w zv(U7hK!p?+Yt9+H%!)|v^Gwm_){f$qJ{NXaJMZnf=vDPR#2hf;(bJf=ZGrf)Vy$X> za?x-UyUz#=xwjh@IURkYSdg}N(LVYYJGL-C?K)X+lC5YJ9(lvVbA6}<#a^$Z^40=tbGP22 zU_bMX{jXii>huV#3n6)8Kv=PEJB~VTW#u++02#Qr=x@P^{+dQ446VngX|USA^p?h z??EkgsC$>^fjP@)6s6r_&s_BB=y6eLPW$%bc2Yt4-T+Q8-YGmBCDR<~3W{Y9$>bMW zA9%vgNEpAnfP=T54%KqRgUayE@5NM`A0|x~3j=^>pSa`h5^(eM=n80O^7}~ZQ%J8J z1$LS7wRp!oN$*l#siPGLV#j6j0dv>LgS_0TU45E`&82t#-86x)Z7B*A8CN=fuvdad z)VF4M1l(Oc_5~8=IV4{>Vil;1mLkUHG94K%p7K@~ki;>4o!Yg*z-lZqb2A}5byC*E zXGdpGQDqo+5cV;@TXr+wo?OAdh&@)qZqr`L8HS8A4tx5dJ$9tI+zGWzH$J$@Ii=$( zwlfF_!y9rOERjVx$*f!!C!-McChHF=eAh0gxluH^nBV7~SYxeKywk$SLamAQ+Qm-* zXPmx%pE`Ik8cxSkh~9g&R_w|>Y$V>+?QO>J3IOJCu=oAxIsf9XnQ$D?pQ1{P`9?ap z;cT*`Cm)Foz*?1zf6%x+><+1cBt{J&mmfeBmVqkq0&thPE}nlK&Xvjc>w_Pwrzina zW>pto>r%C~_3?k$JVaE(*bS(jFc&L%$Z{vN5t<|HT-{uBuPcM^i_QCZFzV@;b@;Oo zta;8d-~3Lrfu2sS*z}#nT*ag0L}ce$k`S+``+7E!VSIYo{qg+#ecCG%nfwK@(uF{j zd5)pOvvn@`N~(^wLiKo%K`RnD@IvQV2PWVrr;Vd$1n%4y;2@1==RWG8{Q&_{t6>tm?QjObxeDQMXtSaC z<(0t)xoI-Zl#|?~fak!>(D#BAWK{f-(s$o$HhmW@_)KJ)6DWDpvI^I32RLzSTtoNF z8S;!EC9Q|gL{jLAP!9k#S+aS_?Ow%Mp0D+RJRl?>T35i(225359|*93X!(yp^rV$> zmAXnHX~~C^9y}On7~OVW60aXmcV6G(4hrMU ze*ryo)fUR(DM>x^4Wl)XfdyJ#VX5nA9i(Rz-R7idZ+{km)K7&SlWwg&Ybx;J7A%r$ zavo0ly-%j$l{(mfo7m|Rkkk$P&lmzI>Os#O2h?Ivp7`k<#3p1AoauEu!gwA|DVHpW zD6!#Ka)5yeZk`LZMCV0R4NtU=SccNZ*OoR&0uS@>>w!q$grk$8P_bcj*{W*W@C-hx zWr|zrL5Xn3@;l~2QM!3#iV3!S>P`c@YYWK4cHfrg4n=U6OeuLkhNxZ5L0%5Vq z=k9SIHv;OGS6aqB>X~=MM>yY@QX@PB6gulM?sJ5dTV15p+)mRcyE#r&gVx=U#HNkf`CZZ&=F92S3xmU=^dg7C>@dB5>Nz{A|N0HxFXVf?;I3GFn~%4olv8p z%cZyP;Jw%T-tT*3_(#TYkdwXkDs#=b&fea=E~_9D<@bF&C{oF1djB%VeCT;1cset= zZdpN9dC(?t(8ftx4Fa6sclJ5_$~V--m=+pKE1t|V^#9S<<-l6q59YC2KgXyUV2Qy0{GuVlIAn=xIzcK>Ta`iOUty5^Jmvr{FirXjU^*ZTQ( z>jzeaYA$=+p%>&^_ipvKZXFwImc$qhdBt2D z1oJ$!tWK7p)ZNhLHW&VO@tCn+zN)0`$H=vf!9mcW5L;zO4L>%W3_=R_%1F;#X0>(6 zYo;upH25u!SB@6Rs;rid{poMrRPZJFLsiX>%|C0U(vHOk)eGxz_d+4)>zgl=^APJr zgClR$%Xj`q6u6E(vm6ia&G= z9e%w^5n%1J^ZgXtT1GB$Wj{T$Ez8Z9@?OToL&DxV+xmW!981__{SVJS(@R@?OqsDR zeWbE9+tsX5V^e5)kAyTAQ|O<=XK7cz4u<~f(Y&8XkA2IVau;!%Nf>(#9!T~1O@#Gx z37MC}he1XPGbzOK1rWNp%-Tt;1yP#Fm4y}-oFaYHjl00n|5_N?jL1|~1OzIAHn%TM zs#NJ4uNEA=`7w#3ma<-~?!9PtV8F06zJFP+)ZK(j^1ZV4WhdF?h|tQ$@#jPSUYjdP zx8acV&t^9$%{4ziyoihEY5xCwsoo!?A7m9f$T>|~D)tshv=LR<*(bV`jA4HSnu6y| zs~2e+o3SU8-epc1C+$3W6M-86&(H?d!5)0N*3jhOL&$3LQ}Rok z_@R%dLDnQ9ZKdnrp--oy)hFC_rQE$9k__loK<=F}^}oW{}46yKx?zL#-Jw1XnIbiFvt{tj`vB8t|3;#xZC1&V<4N)`KUG&|E2F4k)G(e7 zP81xg$bWWNf1i78tIkYq@S*&ANE*X3x5L{&bsyKwX^g_cZS8)(O0%=oM~wKPu+Yu< ztL#JIk;7AEF@+qajFud7XIY*;UO%i^-*h`@2q}z6&y=DxQ>&0Iz1;t>%4?7`m3~Bj z@EJ^F`c1N2gu_2ULw(WeW(7OH2qoE<_I=%}W?8SmO7}gkQ1dxM#e()=mh^IasE23r za*bt3u$yvTqwc1*Zr^bT<-*L&MBDwzca{YifQ>{n#vgUv$jQ9z{Prmm5B0X7I5;=(h>mD?Ij-#cc1tNmi*m~z zu78Gi&|12U;zNV`ReQ@!H5qg}u3Hpj$|!b(pS0lVK;m_!TsqBisZmSeywJ_& zKl%&X+(e2e+6vBWUUX^eb>+0-QhdL=YBJ#$dEzw*4kE1o(`!I9gPn2**^WaB$DLrG zyxa&CH8si3;^6ANA0UofhkIMU4z%jW)O-fdM-6}v9a{Tz=EA?mahD$n+1(S-Y4+U& zIkfgH`OBv`dhtbVrJ3dvIR(MXwqUctP-lBr$qel3Vj`~lY$^8DIXyGDR(UV8p?9NX zdx`@B8C&%lq<56ww$T0H{n?x(E!s9vEhzKNFKtVz)UKuC$Ew#zh9%foTY%fbwR_|c zgqx`dy6Mxyj%#{hQx*6{v6rZD{6;0OaRrN4Zra<(#ZTjn)vz0TZ?Lw9Hk)5SPC(Oi z)kCDlA>{DBk-=JVbM@%`U-o#KXqB18uju>`BKyAv0f^gzgE15(AveIA5Mt69)~$WR zjJT{@8oel!Cv$$R2D7&mkJ`sBX4t5-{n*_v^S1Uo9a8-RwF6IMm_J*%Mp6$u4y;VG3(AdX`S9;y(w`ry8kfo#NqL{QTtNR%1ft!8r;O)EFP7)rx zs>^@Z!)={m!Yt|2`5=?cWuMHAp8>5|_`nqX=KLP3N*JU(KhxC4>xMUvor>QLZ;3bX zLOp!sanks9YIwipVV(ZD;CpdCXqRP(05;C4$Kz5nlTLJ z49U~yQknmU9-CMs$D$Oowm@}*EWAnKBheA1>X`x!`Y~b<4L#kCZ zoL5iGcV^uu5A0fg{4tl<`SCW~s^q99@TN~qE-bo?QTJge@eF^g2v_7Yuqgdx$L$Y% ze3>AY;9|xI{_s|VmQXGt9D>@nsVG2rnSw|Adw^g$k*lZ<#0b?#b|!L#M*&~uPSPZ`{5Q+&jtQk}6-6oyMvHh}KxZmT|GF7l*7vdD_;x3&hw=4nP zY`7gfX~`&&ADi)Yd|z(41_lvpDtitt{3K2O5Ah+j_fCLK4IYbEDGGGlq*Z`4gO{a= z4~kM2w>b4*E%@vy-lvkTB}bgLAMBHEf*YiK_0oNEWY+>#%O?BT@^&sel`_SAP=u&b z@5j2l-K!!^f_)cxb8&%~mx@h6-~i~lrw<($_k9LiJ6F0Ytx!Ynlz?&I= z?R?*>e#N3eqr&$?lTKr%Mcbx+Amt6R{gp~@_l}2~7Cp5;{|uQW{wXmrOry-Di(a;tY(8p5obF=tMIsW$uSe)eP+_ z#QH#_IPr(L%o@DyVhm9cap{;hxF;EbTrT6%V$vLJfz-tM)}J-{&s&54>+_zo1R;^7 zFPL^m*)#ZqclwGA%I{7Do|>i4mU1|6<`sKv{=Hr|U=Pq{>2Z-OJC!aVLB{#y`YlpA zcnvF13HSnUHpMB?7{DXHwAzFOUL#z^{D2Wk!9Q2~v;n|HIUQ?pBYkj$_pp{ zUIS$(7Dy`HM89`Q-52vZ_!E21v98etGmn8|@Xof{{TZ~%X7b*FGcvh}%P^d!q4(~Nq@L7Bfq`Q1h%sB+Z-C)A4%M=!7mHh?aoH;hU39rC!K$ z%c}~9`DOocVk{dK%WW8qf6fYdz7Jh-5cIdXoO6zV%8u)#B8Vt6dGrV|Ws+aEduJOF z_XHgVc@xF4IVkrYy}|S#i*ifv)64(1QQnr#zx#f{P@% zDyTPzB57u$?kqU}#GO55v-V=W%QVAjLXnDa^W34q{J|F5gCrpxWls4sXAS!;7hp$> z<}Chia%FBN?lG=&+jqWecmgB#2W(&Ip$cw6O3eeDNbI@^;FD1oa1fNKz53!Hl zRw*guddh8bTAPXgpX-GG%xni50XB3`6vvk$S*o3T_}TZ9WZb+o%-fL3Qcs)ZyQdV` zhE$W|h5z}BzwBrHSU10xZq)>JLG0#Iv}lxWaG1JJ(1I(>byTM5B^|WYx*dA~voD(3 zU*Jw*O2)t^i{Y|deK0UH32nW8=H21h;XiVH#K^Ja!EbYSzdhk%3}*RmC+SU{JvTEG z{;C9JmhGCIlm(MGgG)pp4X3!Sj%@cPoS=9}-o@UnvlUb!x`%0y5kNTbrz z@ZY50V6twtUhz>QoFSCFYtS8Hl&?okY7mCQ1D5ZIq8jJeB_K+0<^!|>=#;_6n8=OF z2AY#|6(0ZWYdPIfQK9#f9rY?azzpighI3)FGL^KqBYdD)c-Z&^?SpAqC))Wn-<+&y zz6$UmN5IAEuHBrV{T>mbv{A0`i_i2-QmOy8%U>Uy(^@ds zMbc!T!`$_6vjdjD2NSGVdabC2i{1tByV|xt4~ZgH6|VD^D|&KYag=Q$LuclLBZ*X?D)-BXj zhZ~`f1sqn%a2ZKpj2PExF4|t<96J@? zuKn|N9URX;$NeIHMB^~(LA0U3skZ?52AZDF{A%h0Ghi})?1brqy%9~z zV@!i^(z}u=+ih5;5xfZR9*wJaNz}N6e+clX@hJo2%;MGc&f5`|JKj8*g9^CeobAbmn1x;r*+GgPbMbDp@bF zY8rcVU!vKAoRubSb7)*Aj8k)WL-ZL83>??A4;ZjePtaOhNS0;X%}U=jdC=NO6zm9i z6|iV~Q$2={rd$el1zQAGHfOoGfflsty_2=V7j4nM^O2!0BUUfzf^X$Zj8%eDFo(Qi zKqR8z6$ONnrCX83zv&j3^#0xH9O1F7`XSk|rjDBCGBe4d{^u}Xn0OItNHW`aBcH}` z^Mh8`ZpxhzC!=QCX2b3C1E%hfq4V7{g`;x~JZnZM1wzeJM=hfVoNHt6UU8h>mUcKJ zDcdfHmZ4p+p&z&S3tnyepc-?QJYchlm1DZ5(_iK|A>*dg%DNL(Kp^0UQFpP`&RC&5 zq6)6{TrmK%^&>E)dr^I9LrvZU_g&#Vnm@tG*o#|dV&ldH?Y;Vqs1Ui!t%6&;wofLt ze&z7b%o-zq#)CPWHf{*Sm8|~+b3VriH-mW!{OSR@2+Tx6E*cg0zw^)Gbci0d341ZxcSkHQ{Y~pE6l?XexM~fF-mk^-Az$ zpSZ1rn^;9y^?=*&Q0S3TD)*dtmfs~$Q0us^u&TmM;l8@(gEZU@p{u7zee=r#P*uZi z*uQJv999ORN;`3Y7|-6JSyW8AE&L-yAENJfn3iMx9^DYN+QSFX*~GmDbZn0Mf~K;u?M(#eyafyFRf@MEG;8=#ge_3Na@xijXE32xP01CtbRZ>H6v zCl3$3g(SJWIP=5s$8u@)#9-+)@imGy7mHWuCPFwZz) zqFy-kyS$YgtvDD)%;4mWRItdRH0nE7IRw}n=j z{5g!9UzYbkX$?|QP?@vB0TZJX(+;*1rxw#mvZ7zL-x|}X_`wW%@%GXL;09=i;F1Tp zDVWpWJ96lsAvN&bYW)g?cI2jyY>|gvNvf6Z5d&PKcaPh%-WukeB@d>lr%tU24>-ga zMLoEoec3pV7VTlQwYPQzOary#Qt1TVKkV9@UyxTBmxXpzWX?y?+oX`G(gZ(}HyukM z_Q+=ot+&jo=rfV$gi#(%Jh85+-9-;#;!YDGtmLe`s{7jmXD7o#RdlfM`j_%NyvU4u zSvlhd06%hM*-_=BBhexArLZkUCGRavmTKlHCjaD*E@IlZwlR~h)02WXvEYKv$+w|w zn=B9tZHaBf!L*NXL3p0vT?y@GwMJFGlTJbWBn#T!`yf-~_}k5$S-fFc%Yj#{sn-3n z$(Ee6<-gKLBB)I-*s?hl2Js~zeB$~4%l5|}fp?6W>CyDjJukG}hE=14%&n=+*@oXP z;x(B$_>W%<3b5|PUwD5d`@kJ~RCjis zd{S*;Lyka^y{5P9B)^87;RGl&gq?)dqY^$`kKUjS@;%(6?NC6AqCb}_+ZsBVbD1j$ z?^9~*R^%?DpDtKm;k0_0wxX3S>Aw4}LGz0(e{_?*&LB}KC;qw%fd^-Hx?_x8B1qsn)COPW>B2L^_%4No8t9GnNrh3aI z`v*tg=Yo+v-puO(L3gfi#8O6@NNcAhR0-LI%AAs~_H@kHqNHHMDJ7F^os2(PYy9~5 zvcoBB5%VT0yQBC7&wZ~j%J4f(20%UOrA z_@AqX$W!{~^j=3AK6ZqZLT(JSoI)lj>V%;N8`=1NmvokL#M-k*<89C&!MQE?5ScGo zZH_|v>1|5YubXG7v9wsagQni=ER`%RQ@q}o1lm^}NI)*3vg#l~Xx_za<1~K5ol?wn z!?MvHifSwE+yMB2I84}JLw*_;bub^_Hj2ddD#heq6RxoL$CIdUPgN3#u2-5vrI#~Vb^zFFSY#Wg$VJmPYqQygvB*d6h<)vjgg3F`g6QhYQJFt+SI#i3+~nL} zvRi$SdtU&xSlT(JQ7~Zvm^^eBCN=`p)D0=nDeX^O5Lc=tFu>?^Q@?V+F)@wYf{;9T z9@Bhu#y#$t$%Su6+@0pR->%=2Y zBbP?19r1F{>G2C_U0SE1+g39wmcnZ9@XPnRT5W&S%moX7)<>&je$W!PD7VNMgs`Gu zbCF^y#_9Uuj^-dPLF4ajS-xjGGZUqbUPQ0z({ML~QjU!H!5Ewm__wEhSGPkdz@hSYP{ z(+*cI;t>TCOVqBG;PrY)(ycLk3*>iL6{zDhT;2ba_oV5OxQQKr5IzHk8U{0jz?2It zQc*%tdu*QkYr>=Uj1k*fJZ0xwJ^XlE;|3KS$!hs&7&FON_++&Y>E>yf_+-WGvPHu@ znlp2{C{C*-R&jw{Z9Av#cUM0yHy$mscHAQW`;khMG@VLSP`Ma2_Vyv2>3<8&da=~BJ2aq9aFmf zv`m6BkNl5+Ldq$7_KSs(DqrK|B@(7#ie);rb+r_cCXz%UF?|yk#kikkf`?JCCDU4A zuinZeDNG8;*cB~#OQ|Prywy>IeB__&)k_)!8>^aUrXpOyna-tL2I=c_ul|CIIeHZ^ z&(j9U^`Sj3FLDxCdEF1d95%>t&Gq4L5(**Q=L&oUgiweIg9k-vvBTrLwy2Uc{Q1of zkUJ6Ml$b0--!~s}EzZfO2#xF;_qcqmE}_Q4mDSH6+-Y$Zi%$onIbhz1KL1RA?m}C^ zL_1>zo}M6mIh$uhZRgrN3|u75N3wMxKU5HIPwS+|xMU??r8NfkX2B`3pEZ)ycPOT| zraT}YP8FiLuUWeYELNRW=k2eB#e&F`5iyQ#Y6>RHKH@&UiQFUTo_4lAuvjX#lVwN0bor zprHn*F^z0r;3d_jGEw}|fA?LAJ)LMZC>xWU?oGoYCFa!EE7!r+<8>{+?A?DkOXEpi z!WF_LLOLJ@Qdjx>=*4KYtO4gC@)d!naJJHbizb#7^-T3-sUW}cJ;D*ja9-B7A$nMT zcu_=#->v}lkr7iTn);?7h*iEUTw%;q@{M5SGrlVA8~id;0l**&?iKCu+i-<%1Ipyq z@$pS;B^HpOteqv3?#~?|Ue2-Pm}k*_LyLn=Nm@NO&V-GV2WKzTJWTt6S%t-&BCmW; zve7drI(0pI?4%5l9&;r;@St4L1v!k*P*D>}Cxm20xPoDFKj!OWDM|>)j1_l}0?yeUzWj%QtsO0Qqa$a342k8EN}FX_v)Du zB9LXHP1DkD1CCu%lYAbDnw#VI=RAJ4DD*T6@;Cj_pN&F7nhbhcc=rk1_Y0@i!M^p? zdyEQ=lj$@qkWId!2e*|-&70()3w|a)S#ka30p9@n4wZy{kRbQP#|dG>sN|W>f?Vl0 zx(nUDVOOw>@j0fF&xd(<-zy*+mcJK)bTn0cHE76OJUR<82Sz|jeEM>HxW#O={drO* z7?~RgqV`BB6y!3wZZ{Kv)~k7{CvD-$fd_O>G4ubk%x|xZplbe zYV2}H!=j-$1^2~N{7U5%Q6dQ|;lQEK8MfzpZHoawQa6C6G;w)5({5^6bgm=D;qQzl zOw+Ie@dYbO!T@{>%2VPAGIMxz1Gs`Q5)9wrU`6Qle(S(H7J;U`5qWyPyzZL?`0SB*`Ci3pkPG-G zy%e7mbb8GaBw;OBQY8XDeg-5Goo<>0aLdc_`8my01r^Ds3RzsdeO{sS+Ni}=!>f6) zP>5i)llYTs=|B);4H965Dtbpw&kTQc4Z6R3G{dtLSm*(rhh+Rfa0L1dn6*{PWev>z z#Rn(QYZL}Jf0}XyFX;MDjr%iRfD%iIdO{AP71<7pt@poB_hqGY#igHLJA5u>IFl33 zq5LOkUo;lekHf(Ve(jw5r5PF5S#W6!Ll-jR1R+v`&8#)KP(YwdDw9`BdO*w!QikXo ze*PexaodE&L0!~TQr&Q)1)1dHu+O>nf@a~SkvHX9-lOpdbUSqeJA-hl46A$rDl^cX zn_x*ei$e9d+^_tBnL5>3GAQT5)akR8TebRO^!MHy+$mh$@P_K7Yz zA^-u~+bDN`xPRfw^?aG#cGJJpnd&`FLK%Sdk}3Utqgi3(aBoJdbQAz9d4bZY#(|jVpPB%T-c%+Xf+pkMt`w@ zk5^E{Id(%o&h&T%9sattm9Tf`5VXC5LM@X+;1JoTM z`_G0z*o)}7vnx8qHWK>Fug@$c zVZ-Fgl|8Ti%U@ZC-)(Zr_r*VAPsSR*+Ny)xbd4_nr z4s@~LMjFG}zg1gPj%UUP3H3ax<6_= z7V~(rI>==&Y0xr}BLrbm15IaYUTh|8@CfEfYdVa>l+%?j>?;g0A0vCSXm{@Zq(hd> zI?cH{D;In~&nGzDoba-~wvY|5g8Pfp9GTghf-$n;jJCOG4mi&mf`%DR!PwoNw)1mk z9H$bq00Z0=QLZ=fEItQ`gA`}QD?xX3uempk)`zpR8B3}czCQ$x`R7(NtNP4VkRzKh zVyT13%D&GKSDSV^;jt#*19BuwzZ-;Gh2KIzR)a$fXgZofmo9Q7A+>L+cC~Uzk%6((v4H;tzzKD zAfoGqz4mjKzINp5lnKsjCk!~tLXVKHOJ4?b1^@AzgdEdLN^6<~A?4#C8!3nr0k6I5 z2tT4LHr>89`Uyh710p^WIgfGIlfGxtFl~f34R(unu*K%M#j$#RH1x2zwuMYm8#F8&rfnScB1Tdgw zFsu-FGG7qBT)Znzcy!Wk+f-^fRBCySetpN?ODEJrf4tjUIa|bagF1F8ddTW&f*zR&MD8B^tD#N1h`mT~*r^-#ij_J3p zMn7BwxH<&7NU!u^Ag5%pp*YYQ3CEv$OsBgcTQS|4-Vqr-i^eyFdMKiVB%ccnq_d^g z?D6XjN@mBqTqiVus@TIU!S2&eWkGz-R*MK?n~u!ehop24bOk`@V0THZf_m#ufa+|! z2DFh}D1mq7dIRf@@yv66^jl1rKyQ5GYzo+;1wZzV<9#}tepf%6sxb+qfu-FJ(f#)p z_G`R=e&89%2zzx@WJPyOg8a^QVc*T9Pd(Si7r!Dpnbi_YL7tVqg2_|RptLY^ghBp^ zV{mfPKtm)X3;8Jng!n2I6kaQ${VZL(hRk3Nd-pW}!-bXtX_OC;8QUF0ZVGV_hus%o z%8DM+&#Y4La_bD~^3Ks=Mr(t)j<|K9R7Ii2vlQgaUJAgDd1IMnzIp>;i?)v}Q~qhP z44v4igF#URj(!zIuNn!j0@VUj)YTb*^4+GRkQb%LZC$KzakwmGvLxVQf+GF~vSIs6 zG3^6>WZ`Hh<|4WT{TSS?-+MCP&I<1E#T5+#Bk;YEExT4qrQ*f@Q<<_^-E8Y6UxF;y zsZkRib1%?cDAl!%s%IdyX#SXanwiQ`A3sdzWgWA4uMh)N0ML#|AH!sm#0?LGthaGh z%k?vUPVVPxwC?!oj~jf&az$T`YcrA7H>}Fgc1NDqxc`Jdx^T>#aF;;&*xL*~1TU4q zj#<4IYBJc>?bvrTA?sD9k<3{L)|HK>1| z`E3CY<&DAWNP&X=ojan7ln1Fh=NiK>IgQ2Fm*|Z~dpi0;eEQP5cA4+lUTR93)*{%V zKvww4sm}N54|@H)@3es$NS;$x|DSCuylsdIHBZI$sxj|GXNR`8!N~J~(=SE2rgi_G zJ96gGN8(CJ&+p1>@eyA3P$e_-oIiW!EE3XI{eXL@g%0*bs32lNd%{Q7^Ngew z$OYchL(aqkoa7rI1o>9I4Cl1&UrLGkneiE;X0Q-4)1=Wb!yLS}fp_g#*iMIWz@^Tl z4l-9Ub>fGz6|M_A>wbnbLGuPdNXF&Tk~6-vzc;44wXp4Xs+0b$uiOivm}8Ua@y8em zINO=t<&uP~-Aq`hfn=G{VN14x3uGUrmVYlZY5K-YNSF0vZ#5_#^y8V{lR9RY_>3wd z89XkA7lPEdYD~2X4xBmvEB-l$*G18W2(%14*`jP_7Ls()Kb+0+b4!fl8m_qWNRx9E z#joP`5gA^I?e788lg`<=U?hrub73`548Lj4np*ME=igl@9J)ua1FAqd)fulg1&w6F z3+m~+W~=b5X4!KZcHYSY=LO6Op!AwuyMBS-&CS=!GhzfNT1!kv}y$kEym&Xi& zzNntpf~J!ChTbuJ-Q8_idHahfNl96+$N?@7rmUhi8+pGFlni0$U4sh9_m`t4TRIja z3fIXCCA7{mT0^mVXF_7j?yYD8yEbvR0a?JdBjkdK1-(4yj^1Ob%{JTfy-?*o%0B3G z{!9+_sQoBM2rF;waTwWIV{lyr6hSBZjB!F?vO$78v>w4%XR|3dX?E9Hi?7M}@w2Gy z@6XQksyr96!`_mw%NwFJ6-o7D;`R`oR-9RsuMfBAg!;k53Q);O(=FRigIIgHZZzlZ zxlEsQN+L21QRQ9avCh!nZIW|g9)`A&TMCvARXWAyJXipNEKgdmsz(SD2cJ7ewckSF z&$Yopw}PAo^oJ-8cqFsj`Myv49Lc@cT<^;B7(CxOon}-m{(IjF6z!w3v)h>DW~MY( zwVd~$w9NRWVuoV4n2lp}%jF9dmy9RU-HsFMBP->GpsW)0$=lZS$}i&63sx#4CZe)| z!h`f-^K~|0k)<~Sn+HlJt8bUl_UH!DRHam*c*_jZQ&?I)&ng=wU4Fnt5@!)ud?>~f zmDT6RuUX4wG)(o1NWVd>ZyOsWP?fW+opJb_gs91#O0-IlS1A|qw4b3XXwri-oTsVN zQiKhIQjNYFRLoe?1CcCj=5Rk-vOMhi#v~;4TGEBs!g~uz&E(jLP4g80$r8)2OCFGv z*^lKW0f|C94X@;n!qmKQWqA~*wuMU=ijkac;x^g+%o}j9pxVx!L-^#r61J+|%={Y_ z8Foxv&s7s7YU^I+mVdERZ20Y4$B|hIOYd@lI*nt?&EQzWsg@f_tnS9`#a96j41IK^ zMghy=iDG$*oN%&TIi|j9UhXw^6SS|M3$V!-lK-BfDqv~u-G3bYnOROR>{UG~kD-b= z{lL|B3~;?X93AomIIn2f_;c(RuJT?N4rIMX)?$=|vMl5D*sy>la{GR~8IY zKXYq|kxnO2&jLCgynm2rNgqyyjpX5t6Z-wNC7s#NT1)mk72G~v~HUk zIgW9O##(P>!R?q=R@jzm_*wl{u9o*HyuMb846WxS$((QtBIFrN@LO|u2eRd=>)i&L zH5BMpqya|xW-t<5HO`19Jy2#9GMnc_RnROPWC*Q~ymDkkv^t$%qn$b2eanKg zV%~8k_g3s0)3{9#M1$Nm41X(^*GtQtUllI~;iPWH%DX&BhN;@D>mV)FgttjK%t&g>NkE4%@X>2ur z?~3eM!_OVCL@_B8P-mXk8rB+PGMDD7QvykyO=$zq++Zc9PA#bz%2w?&Llf!BMLA|5 z0z4G5bKbi>MMlBj_nqwcJk4n$|5HJa(%balR2?NNMQ@3JEfIn+acg;m%j;63z`KKO zw}*ajN*d|mjMUZpnuxo;o!2@pMHu$-_Qn-r^U$KAnF^0~nwrm~=FN;4+Sw$=D^Qt> z@|l5S3|hKkNY|OPfjhtFjs{4j6simdjgcd?%yJ(=f%qzrK;2M>xchg5@s|cvap{x%skig$$#v2)Jt>WcRRzU*?t7LaiZK(P~y z(+q|#pW8XN#xf&s#3D=Xn13%B5yKmcN%5s$-!Pj6E<4rM5&}-qL3WtmT&_rOMBh)2^#{a;=?R zgH4ED_kS^b^Jf`2?-m!8op2G&()5!EzHb$apt+VMvXy5*t<6u2gC2pCtn-JW3T&h>4M2fYVvxJrlAK`puDZRuc64Qr^5;Ly#I-xHxUCwj#%?6s1Hu4wFrRY6m+00PG zvdgpkpB<)8EzR}0Ik8FR6=o)jE%iU`ohTKZPG?)@xljyDvh*en zyjsxh4v`lIL*aLJsHy#dwg@)1*D+oLALdSneLWkIrmyHc~xyGDsuEQ zvSU#<*(i$W;HO2j{4hP2tmXF1!GM2)YKaP`>q-iyt3c|oq?6jHtl_~tJ1~Pgm!;MQ zX&hZfS;1+XM~k-B|hX7frzE%}f>?wDhZ`UIsG+K{`dPhN`H#bhK@j`f6e z5F=zTqTqz1-?&zig6aC`f&v6-Vxhq2t$ac*`)+|Va254=j}*tkFj@R_q36u`z;EB8 zh-B$LM~S^9q zoKLfhl`o*gEPs-iw~QSAqC<+^jE;zXJ>xd=#Es*Z*fQmjFh5!W;&w+7P~jsHK0N!qN<+AZ8CVB8oo!y^=!~S# znlwOL2Dtnnc!9YUy5~1PWnkgLbzVCh8p!MiJ=Zu>x*mv@^Q{@qJa4q72v)juEIA$( z=9~%l<)@*&{Pfi22^~%;T&=hyD32vn8(v#*s}H-X?~sB})j5-m%vjRs4&(U~9xH@Z z;O~bAvErJjcKq6v56h&`20jw7R4#cYf6g7L@(YqKC87yO_(AGd{cvr8zlz#Piimy^ zwRq>P>hDQuL#0OOjWFq@s4 z)5vxy`UnsN6Hp%<58te60K<>QUo!uS_lKGjw5j0vjuKav+kZoXWs<_?tEV}2VehSW~K}WMn7;daQUSZYgT`j1?cnq~f37bQPpABC* zW+6}z#(+Gu0?8^lN3~lViRC#9ijky(Mf4ccgKbEp;GCYbM|OPCqe~r5vh$Xy{){iD zktqAow(k$ld)fI9Wo@`18%juhK&3_laS@y$e^i!uOkCqc=eW`)crVS5f+-R#NISQ{ zf+Q0A=AdEu2)O-NO3jtm^GbWOT71}&-n1E@0D8Wd*wu521j*_fODnn04f?t~a^oZNsftP*k3Cm~r~>4nP7{ z;5!Z%9WiqUc?laYiE3TDy3KWSM2$u_@~r=j3T)}`bJsuc~b4?@_^Cd4aI48!~t`I@3L;S5^clV?&@gqvPA z!(JEVTkL2iMHOr8#^Siec9d%GfCB4)5BH13kH9hDYNn>b@P{_Wq7`lFF1! zB`P`B9ac^&S-SFA=lyuc#tt1pk3dh@Dm|Vgf5vP=|I0#0csCyj6jL&ykm_HHHBCgN zXIE{)Tg}@!ks0^*9_jr4zh8DUPEmt@KAqkv7ouK1xuejKt}1aebUo2>tQSzGZt#Fw z!O8^h%8i5q>C_D_5HKm!Mo@!uHFrAto<1*Y8=b;-?e+;0MfJC)Y_`11W)56Px_ort9zPl{I}8y4WJdua_5qsPK^iUTgP>rvS1;_S%Jrlm?C z+23B;hOHvzYMUAoH#GS{+szF>!d(SGmGV@O#Y@}~?OI)UKw7QRy_P%O?SNKCrkm4x z`%H%vMg>!Yu^?IgG5T$a`EtsI*gkN^u%?hL$|Z*+%TS*ORuC0`Ox9nXmE*fj;pu<9 z3IwD{{Iia^IF9n)E(d0+nlBrDc;k48;=gc1e%%*8>UbCLACX}Dzz93n?_K|(Dt zXQ@nJzP7UMG)McY-kGJFEtjwdx{Mc*t~P!A#6AYPo0(rZ-bBr$o67O51rWwG*1InN zE}-T#qVZxNuwyDBXHWSB)&rTHb`R!x?b^x`P)gkpA+$e9xHG!%Z)e1LwKu5e*(ar~ zuXmZFw_mdR-t2a3#7EECSV8(gSy879$|kVD$fsq%FZ}r_^(*#y*$h%nP8m$_gG4>~bB$+E%jz0nVT=%z(m4W1S=EI5dMZq1C~kX=3>rvd#T+u@@V;bQs5 z((2>>r*@$7uW^9JH!I|jWANp76xB-tfh6w5K)_`!CQyU_FW7GM zld7D;lc{3&VbeZ&g+geu15?%DXf1ztgx3F~>bj$v&YE@#Nu&fsM5K2RL5g&gCZdUe zVgx~IlqNk$6%Z0p5EP_IFP2b(3Q?rDuwp~X5;{_%C`gae`*+=Sci-=vc#eON@|!#N znfA;~UG&z^PMKrS5a80L+-keHe3U9ll_9u+rlJ|aq@3NpU8jY{*K@_KjrSwFd2rv= z)u!zCn(KUddgsth1TVJ*HP|rjyOXgymqhWMI|y@wy?e=fU^p_XM6UU*y*5|H_wJ)F z?z{*Rfk}OqavLdMX$#lySGQsi)}9M?Xm7UVUGg6zH*=K~72VMlonu!J&KMJvg%+hN zO3=*l{)`K>bEJ=?BGNnTr1EKKSc_Koe$Q9ANWA9A1N>-@PLaYk6; z!bYOM4_deZI1fNw#zFIh;3NfB>dXUS9gXcv}Oh_3I0DJfj7eW zuWNA7E$hFF$2vtV@_GumD|?b%q834W03(`iY~g&JAxzW6EMR@4)xS5~R@++lq=B6d z1#GDT3Za-QOADdym(Ab_obLg(_PCRSD1&(KI$x5&XZb!pOsC3ZrT$dy>1<>4`moBO zs!weaXqhpAkcWgLmcSG7#W$`(q>L}6W?VMr#M%))A|~}uhu)W%a*33*m+hGIr|sik z)R48W>-a{!cgcqEgg8Q)DqyG3AoLr%x|p$Gz9b|=*I1zE`hKdc`=Rg_bA$M&k;x^|lWuuvluI&eP&n(9$i5Ve@k(P35pf|g8L zH$k|r_BrDZ2h=D4Dcx(Dn%;f&u6PTez<+5@ldATvP=dq&O8g)$MpUgnJ=_0$hiS2#EJ}qgnDj!9t2;yQ}PMze`WaZRZ@rF|sT`sg~!@*H_ z;(qBX)@|jlJk5O+Ke9~f^X4e^b}4}Jz*1YyYmjmu>r4$cFBHPSH#*ugif;S!PlbI= zFJPB*c4r9ZI#tTia>-#w%kSHgCgsb{jxbI!;F+rZA99LD`OglWu7OhploqB!I1lRI z*MFDP;hz6J+%%b7Sp&^DC2-22RUOfP@wL{!Z6euQ0&AC;0@;X36KMq`hlH+yHh+e7 zWzNO8YHe*vu?sluZLA;+mE!QU=*vt+mdk#^#VLsi<$VJs0o?zZf1H`AoV|hJ?w&RW z)OYqa4m+de#oNTT7c@U;O<=$=rZ2v`t;Sn8IfR$#$M{UYU7U1^W%8KRi=8~FAVv$5 zHY=1Tg+5&?WL-))GIHfx$BU0bqG3qDhvlq3P|Y7g*`SkcLS?kZ#AByjd9a-?CIs#& zf10Mnkn~;Ow`#Y|de9fJZ#I-wj2gMb(X8%u{A9z(80U@~$L6Y|KTU)|tRVU5)*#Gd z@*(cmS9Wes?q_%EVr&&fEf&=3uxq7my)JEnZLwth`7uYGG!p9Av8#P^qDhu=mxstp z+?BJ(q#ryv6s9t{b2pPVQXlR(5z5$^Hn{-RWC1|&;@C6jENT|bF=yQ2jGuiJBcc;g z(Rolz;=u@>mPr&D4(ABj zAHq_duJ-=w{<(;1Y;vflm^;sAZLa=ln0!)JmjX64*Wp%Ao4doYC7C3LyN-~zP$!Jp z8&^>$Oj&=!h?kP_u_ph3yQ|R2RK@>Tin#u|*no3-S1=uTyt@#coPGp3E07qb<<1qC zeBr&m)QcnNMYn9@q3&&OeCTJJ9=k61w3niNWS8S>AJODJln=8(G#9cP>ynL0QnXBm zNRC##n)3A(M#~KG-N#g!zy`nOb?&G?$T<<5LJ*FRIh$q-wNOo>D?44=KrzOpjb1+| zoOoSjs{RzVC9y@C)Q*XI(qn3cbiL2A!T3`JyKc;Jw?;**91YaybdzTKv^N=n=!4;N|IN zrR^l%&`j-me}$3tP4whXZB=3zCVR+NYUF!uRr<^s+=XSR2CPy>cdJ%=%J9kPXtzDt zUxD)(%z6W^A!m`~d> z1B1B-@-*v9ZL3h1e8JzMogO52TBV# zk%>qXZ)kYK;)^btI$7K(9oBh(VmhtY6?HI;ts_0uQ-~lzb(|PpCfSpkmL$e$rWKGa zRJD{--h>Y?ev>%7fN<^`Y@kJwdQ~SId1ItB1}~72oAO9!F604t9{c;W9v^O_eZJ`T zUhpy}lW#d3;e6J5{__#EPT~jl^#vu=Y;KuQ0Zogp+bP&&a;U83{2M;UTLDLIY_Eq8Et2(; z%29{(=-O`scEb0gUY<#=eQimD-tKFx{j=8NP>j?+wk2EsjxZ<)X>ejKuFJy~P(iRt zc=UndN1{~2$$_fH7b@h-1(QhvkZSIexa=e7Sy(;K04W{SdC|M0f98gcXg0>9?;5AO z6O-NL%>N$G%DjXR=F87lPQ_WFuARg-$8siGD0>>pFvM51)d+TP@ zrU!8aY#Z-J+}#mrbLXI{6OhcxKPq~Y}GEpjwt_Xf))yTIvXlo^lGRY~nhoQ)#8o=s0d%H+jn-~6w20Bf^180o6Sm+?IG zQuHf6>7uCtH9rSMs9wJIVtnReZPz0$vGcwB2XYo5gBy260<_h%^s9d)Lx zi5^3J2cNc0Nk$MOfDUQx`qr8pcThBEEL|<6W9-AU>wRcZa^91V?*8Mi@`9anZu{D;wnx^1lrV=7U`>XeZf$a-Ly5MXsbgycOf7B%&|I6$2P9#^|vSF z2q(V8`F6W|P3$u=SC}g{(9EnVbz)QZHhf>0M@`3z_2WD17Iaz*YVz7~LxE^{EY4T= z+)(5zEGclIIK)gn=RY&nZVwtlIgz%S^D-mRGM1 ztkYlpaK`Rbmto z+&W`ECEK%-UOn0oraF;YP%U;xqaIGn@O-X6WcsHgPQ&u0#_)d(a$atZky7NEeefb$ z5;MsaS8rrBqvuV=1fls^0A|f_~hN)~iPorDf z!~z26nBwcwVgEqw+1yve)~cRSBPqUo_fyaIJfc!;*>>aVXPYei9;MX_p(0xFHEDwF zhsQlms|e@P{fU$s;2-1!+@{~QxGF}qnas%kK{Lu7H)N2s6HXH|d3z3FJ))l$M_|6F zhbUQRej8Q$hsHeObAWJ~Dd2O@I%FFoSk-7$tMXHUAi}h#Pdq_?-##@)BFDr9!5T3F zD*+;)`>{TK&~h3+3$sHHLuRqt(LU|&>h!2xF1Y@id`WD(a^jNmrZ?>y*^m+#@jP0M z5u{I#u2Ur)`Isj8ui3)l`ok2I!(wS(y&rv`nMEjPjW^GsnZSzvoEFVlAeL|3-4W>X z!+i0Ygv`L*(iO>u8xUJ~QF64oXmfqG%NeutZ;1E4U*3n~Tuq9onyA#B3jTO=O~AeS zD(8Jz9TfekEt~dlx)5yO%PR&G4bV()?i<3N#AE3* z#}YDDPov854ZA2X#%S`V2^}xf`}!`q@q8WhK3{18_5ec9WN6Iq$keW1si#vMw~Ca? zmo2NZUX&KX`bmJey{ih{;OQ!S400BtD2dL>KRJCpM3Jz6YI)k5*7~(cL;+jU^px${ z^+1w(FX2B|;nXT>Qi(Tb?4&0&CPjp$Mr2KoivE!uI^!WoZ!cLSd9(K?e^O>VC4aVU zf~CpZCP*Dgh0;c8BcT2t-hTedl|AZPN zxh>P|Xxvu3OFGNN)*QKt%=Uui(8n=SecPAr$P&3=b%~J}(@#ebLYUCXDO3E&%Ws&= zg_hLlg;+~IeV8laYlJ^kFPMdydCzlKsh7iuw_a6jaf{d{Q==~CKTb3ay#GE7!pTfb z%{JOs_mRj5M>(04Ey&^Y?+o{eqfbs#bU&;L=<;i6Tu3Vk6rqZ^hfKEpQK8m;ktPbJ zVnh**ThpFgzjH8I>!{d;vI?%L#+RW!d#QTv2J^Gfxbzf|aqS?LU%4Jr zkWBU=sP{IVCSV@F^v68@`_ai7aRVGGMbbAHv#%UtVj215_)Zk`twSiRLmz0>GXpGdZt?B}QVmm2@$T7eXkk7{1E;TfZw z*DRH6lUJN40(hswto${FtL4Rlw~3~d=Z{f6##{pz>c1mDn+@(pK&eo|K??tqpHu9! zoWrbJtex63qo^}cydbAi?zgJ&->{q<1_Y3?I##FV2h?mI}xCzlS;6|3v- z+C6+AvBtrmu=d>mZMK;-m^>tR1=F_ux2_(W7OCh_+cl}(&Hf~AkS`MMAWA2Y?IKKO z#eCm{imV-`qB*5wsR{%wrXyZ|IJnn450>{ahhIg?uI!Zp!D326z*TulWE+K<_OPY!?Y16~p;&+a0JhEKZZ20f*&C6=4%KNgHk?`l)u@36!jaH|;mNd*JWat5=Nh>b9BS_zxD@-!ifKX)`D z4;{xg9g9GOa}RJ@KQoU{CLc_{Sl#!nTHh4I?Lj4*!JxBlH~SogXkJtV@EOn zV@FRnmPr$Bd+3(D*eaYhRJN!sDmk{<&PwZ4b(vbe-d@B}wtm&Hoh6T>bp_ z!!TDO>E@2;5pGI6RIZW?`%-m zbFn*WQ5a_)*)@5-`{trat>~Hp^La06K%;p{|PBdwVxJGt1|l;qdIgNc7&8Gi(JR5ChHBfdZUmGawti-CWM@fsI42YEbC zyWH$?nbW6WyXl%^q$b!xdrlpis_SmvHS9H_eJukalq6>|;vz#hH8~L7^V-wuq2q_g z0w9wm!!T#OYuQcctB>adwbu{@F)OCo@yXdB`QU`$&L1x*pjpG4+~1geEOPFKCU)9q zS8DmZh(ohYqcsP-MJKzNxoBdgj#m=Gnz<=hOeZs~oNBY}-b*Zjsx1qfUv$PjA@YjGi zCfZK{EXAI^E4E(fhhb#NIm@sHtH!COMv*vzG1FNx^n@73F(I{gzu?4PL>aXX{NXnYa&mBjDO5y#$9h(MC$eJEg z1n-Nt7`$dPLm{>$K{Ic<5buNR2fK?H6+5|ha?+`rcr4`l)h}k`#W;=Gutaig#+m8y zDqE&bFvRI*zq!fs5&M589q}@+5F&WF{DSJ#Nq0&9eg`k&hdomn6dGUj7nNHZKQuRpg{%|DJ1o^cjEQHGdXa9BQ#(tHSLNfqQf$1|D ztIDrO2e=2k6h$5R2BgPzKpF!g~_fceM^dD_Yj+^*zhl4|F0oIijo*xz;5p3eK1j5 zN31+nBBJzfN!blGd|c)mp{i_zQElT93F7MJcI*S9)i;CC@%NY!j8I&;6GgFP&0f3U zraF0eXV39{iFHcU;ir0)N2o4^TT`_2nJ;@(1zDW+_Jb3+5UZWPT6?biIuMfZ67)cO z&QxN{Xl$eFM4*>@v#>~N*n2y@AjnjOc8okxP}POv;0o=BFZMZ$QKqFfyXCY!8#G}ave}p~unMXyyw)FAt z3?teC5q0K(rITRd%F#)IAZg>fK03$Q$2_0r6+%y=zH%a0!g}9vwq@+fGMevQ54{1D z=oIkOEn8^E$~M)qC#938@mhZ>`A?)|E)KbR5OzT5Nr_d2`k>#BJh0;2Cx9LOjQgca zH_XIua7FT4#|Layz^7f)0g9DET3N`CZ zdqb-uvEKGf>N)-i5-Hty-l%&SdR9#LP`J%s#(_L4fb=>Y+N8YCIe zlKZ-+HcLf+=Y--!d&nzsSGCkx2Gqo=+%KZvzj~3EdTuG=N~8lYvws$Z4LMF-XFThG zQeo61Q+b;~WdE#TvyJSltT@&2RfrXDsd1=oSJ=WkxN1~?;^Q2a@a$KU3PqjJ82C8G zj5(Go7kq$lvQS~!u0=iHJ*2&;s1W76q4#p|SA@tZFc_E;U-ST`u?dM-kQ3 zv?2k(Xyf5|jcl}+l1eFt+or@y@vcq&2Y!yRtPgE|3+UdM2`egE?jhoHtiy&D_O zhrt}BU*C$mtbw_Bi#sXN-MEU_K%Fy@^w%& z3G(KT>^46yVwCfQ>p=oHMXoHqthfDfHs`TBMu8gKYy8aZzp5(1*SH|py!GlJ)sh#| zLVcErZTEvYAABx%Z=d)T5AOY;cjU?)v+R?OMnc>B<$S&XC}#6U%eRCc)Y<1KQxoR`{MJT^Bjdt1PxliejjAD)kd^g`%ncL)K1k)sQ$Is(UC-3+$5C478 zlvS_9XM5z+6b;0q8Hp$5V!nr6)xbS#`d;?PXdhpj6eGGr#WoUe?=YAX>_p$*thBxL ziZ9p}G&fXW7GH)C8jbc@2J6eBj|!z}ugU|Kf(XB=aHaX%GXSrHI3d0o3s6i+)Sa2j zjhZvUAjvr54)XONL0toSS4mx8H`nV0wUjQT4Y{rO$3^CjJDD(B!;WS{JmK4i%St!v zd9~1h(|4&d^;EX8&O4z@mF}?3>+xdC?8y$D28`XngnVc}+znbZ9)`tE#}{GY^AQ?ci}~l1yI&$_h!jCx2iXw7O%$ihfRIX8H_)1K(_bz ztu<5{U*B2U2ij?@V7`?5WiO-9-1%r|>^PXrph3Am@y+m1XQBu(lg%_|d@%+JCw`_~ zBe&mVLgP6g}i)=;8G%-4R$GkA?SGQLc4ms=6J${0EDSX$FMSwJbaF zgVLEIf(g%1JMPy3jk_PTli8m7w`)Lt`RhbDv0jBfBUXod&6W~dKuMQZX~Xk7Fh<#& z!(G~EGsnlDCP&Mb?w06(>RY;T(7kNYmM9A%t@d_Y^SZ`4%aM3VwC#Yt|NK6hG}-OU zwd&-aYU|!@lykrC!u|CoFouVr;-{gA<-$UN=Ob z>`o5xiD7M&n{UdE#P0j1l-fTID`bPj%suY-3wd3Mj{In*L zCn=1C(s?IYzjNxnA#IwLMH8S!(>TfEb_F+XEsZTU{1J#LDO$#u8ATtki9GJE+CPSQ z&As5(9NpMBu)e5GH&3m(l>5qWR+s$wUbwIzqQE!!sR#gKi4P-fO<%Q`b7$QvEcYykf!1LJ20KRx%kN|V6uA6 zQq(cFU*o%det>=hpi1NRQsegfwwkE-nzW_Mj3v`c^sRc9VY>E& z+MNJ>S7Ew;{7ho#oo8puV`L-X)hTMGmMVGeIg^Pk^`p32Nbe{FU)7<4T$3a0Q-0)p zKG{N*4}K2v(XBNbwxx0*P3_z$B>iu!={fZjF{v4d`QnDvOu4bQC>RNy#04b+W(7a3 zfTm7kXU|kcRM+{pfwUgAWi<)u`(UVgMHs(-s>w1r)Z;os0{4^KB^$XI^ZR<0=_SmAgu_e<{dU{yrt5PH zmX=O$=!iqN`opu@!yv$Gu{J0yi)>g7AnY6^dp^=*mpGEcgXwpu3#{%t-?B!l0zQw=I2}=Z{vtBs#58Y+g1f;o z`7tjRr)!Fe;M_WNVN~=^(^+Zlf6MLi-Xb}>284*-1LEGppPmMYqy5yVE~G#=2p_Cz zW{bb3%P(kgYYZZv*ciM5?PIg^_t}8RLaP>s3=^bHS@LY(M+UQ~7=3)(otr(BuVhcO z@!ZSg!>Oj09N!~!F0@_9@pG+z7R+u~`y!%oPdF)n)BH9MHalC^;#D3v`W_y^#8J^# z1#;~Wd_{+fCJ!cRpPT5ev_0wuCr6BI3g}@fXJHOs?owluoP{cu|P?3|P zW$Z^_1kd(gaD*v9FO&hmL{76zw`K@(iCuyLj#A z=(;fr0BhRKt7GnXqF(ulUm~sy`{q z1IA=$*cPy7-jyYdr751siupi0lIwK#?vJ007EQP<)YHn=MnjV52wxx{+tlc_pEn#% zTD|?LxKzJt+UG8G3;}#3*x6s{(%wH72A3j?2q_#WjcK7Wjrb*pFHju3c5`xVCdQBWJC`YC{Ni$>RQsn37yw%rl)wW#ooF=$zfT?c+4}&S>9a6R)(`J~Yj`pva6PXis*xPwf=lHk>BAPF zddBN{q~xCVx6meN7aBj>%GN|kCK8*g6c#6BzgUS_|v znpfhl#5~5}q5TByt4XvQ2+yqi1#VTQfsMH*Pu=dg+?N<}6Uz-vg8MJV5FlerN!WCF z>Lp9booG>yk^>2C9$hKF3J@=%chY<0i)@AN_rLA2O9A$rOB!urcVIveOt*YtcWF^= z-x`pmz@b&`!qtwSmnwl_exRA`uqzyJ7`wQ#pZVHhYqJUF(`j*csdgj%KXvOt4IBxH z_XPI(%3=4!9(yqHO^A77GQ(i#S$}$Z3@4|A?wa}@>#SFqR|Y#A%M6C9ZF`A-)C{Jt zL37Ku+1CydB8_K`JbOV4!wNPeAd#rD{+IwsA2@wANvd4~q9uG#q`pIm2**Tu%}iXv z^WiR}`?8dl%Q1aQgdhKk=G!Ss1O?88(pDT9nf$4KdOQUiFSCI3fzN*zx^H2V`?o$y z1vZHnuqTogH3h7dlq2J5UT@-dhIIGmlpS@2^#>(H5d$%=c~+%IcIS~#p__?$xrt}u zwzk$G;{CgCp$nel0rm{~n$iD=@Wv>NEx)W<_{fgWZGux>7$X zf&Sf!g!Et73I0${^JVWB`|DC41eaPUc_6H-f8Ud_Cnny02dT1uRuP0{lS0+7-fH|> zP5#fI;cRPa)RP|`6AQ4N1$oxh2{M^E+GYAv%VDk)!x{n(K$LVVVm{=4n&Qdi+8ztW z4Vu}~)^Lab(~yx_{Fq!LcjNp&wWg9+R)if6MlC}<>nkUnR$B%62-rGj;qx-P#FH4V z?;|WZ&~Q`;pLmToXHHwwBTw4{^qdmF3o<^l^=(K{FC%6!B{Ax; z!fTqWA^OXriCRUz@EFvD?-`H%zig1@bFQgys}WB>d>F%o9E0?QwKnJx19jaCTS)XTow2UI;IgoqeTH-&BvpgI9ipopxa8 z*0p>7@_BB*2RHmMXV!<$Bu01->>*f?`}bUcFi*_&VYr=nb+kLJ>$YcN z?Hf4ApdY4@Xw@&jF{l2R(KKrjo4-T)?5$P|ZhJul`Lkm(`FhyifvCJ?dMc^}E@iUu#SKP!rh~I*FKw)KvbJmB>mDhI*!h1o1N^ zs*bzCHW#T0uwFE-?Xqe?2A~Je|0^yw=YN$CF)4p4691f`F2dGfwOWfD5D8IoiE$aa zXC=K1t=!`P9`QZ=1c!OPOouQ0xI z&m<`$qo{h_Lt%&A4c=84t{1ektia~kTE}j!y*&t|LVtP#&~sa-(Lfm`N%F)=47%kk zqVY^9Q+cEIA7C>UFcA0&Bv`eszV+d<3jQ`jJ#{RLMq{YYvje$o%7bF5kn&U7M%}o9 z_k;2SRA>AS3*G|XXhAn++_^ckyYHYQ;YLH-*f_8nf3&q-pMTdm-ptlFPz*A;8h{{L z^5;-N89!#k=QuNvZk`dw)=Le36qtSJ%W>7!8iMWm?)d^!MUbuUZFx@C6F8l?QQM)x zsYUAfwBU=FPKNwP{)!Q&?eISHw?TOJw0F}q!3c5DC)l(^c|)X&j}FfHKu?jd=*i4UbZvYC!rpym9<6+9{G-HUVlQ=y*d#E zN-Ftm4&!EIS9tQ!ls_%YaBvr%!=F0h?Sn{QYu!;+Ry*|T7KK*ahisqWd(?_XjUt4Q zb|xNY)`RJrwR6PgZ~xHc$o;)V0nlU%xDu2LQT92pWg~VcYlJ(eY(ragAZ;4uFlX^2 zlBZ~=ih=b ztCAp?BW7|Ongs%t(OL)A#HR`|w%(kxdo%QT2U1+Q@{i~CkR!@y>YQ)exp0DLWFvp- z4e_~8_#HSt7$4tOBMC?rvTnadq$E$c_Jfd7o&zyDstsa`Ky`xOWObFf{cp8U(^NvZ z=CdCc&L~?CD-D3+UJsk?YN*W;(NYMo$x7#o{uvJOa!S7B^$e5zUX$$+uV-=Jevt*mcEm zU2D3CTWE(RKst&E?H`HRhwaXJ#EB*&Wkl>hXDz@I$_NdGt-4ZO(Af3rBCK?HlG-7# zI;{6UwwL-PQ3eY>i4o|Eo13WcSq^5%(~3dakOYrfY&v~RH%4%nmxo9zq3LbY%NoAH@62l8xM!KiAX*N)V*SSLk000 z@dfWQ>Gnz$m9S;EY>a@$Vf7Iyh8DeCz!JD{_J4^XkR=Ju-yx$d3XvTX3T zCG05Ud4yideEaFffa2rh`i?z&97vYgUS59xx* zuQ}*Hzk|5>xAL$pSu#cdwm_L7h?#(=Dl`9J@W7c_O@Loj)5!0YRc+h zG<~26B4a`AP+;6)>+8B1TRTyXF$XiPN@9uu+rHtGy!Ci4?=$g=Hyn)OxV~EM>c@{O zK*>p_7tJ- z_ks{Nz^2L$7x>PX5eadB?QCm@&f&>uQgx>mkb~ABR2<;({UXT(s=(LI8~8W^ibxGO zc6v_bVSA#7`z1y>85J)oV}IA7;p#uW)6o0q_2i|E9m{3`-{*Mrw-+}EJM6TPX3yfC z2rfHZmyO1_Wp5CBN7=O7>1pIRkw@No@;H63V<6w(0URfG)}d;je*HTTF+0e@?}K>r zGI(h95j94NMR%E}8QvN(qh>$v-v+RD$m{sjP{n@tqf_g1os7;hDQx_ge-YE{70kWZ zhF8#Im(|XyS!ZNIg)su2nuo2QZngp6WSQ7~pr%IkZp0230>*RE&6m=F`AR5fJ@l+- zqOGPItP}Bd7ZmQUDO+pBP%j_4wQr$u$ zwrOOY6i7Yj&W?Y<0LC;Ewxt#lUq@U7b*iB+igZHv)f=Sv!`4Mp*H+Q@L{5!ptOA1( zAEZGQAzYa{0Sr&Xf9xQ&yD&buyw$7GB;M!UW;+ls%*O6BtD#smvg#ikbSh1w_Wpnw zdp<2p1}9k0I&fHQ^&PDooUtmumaf>UutEOc+Fhyn>npnW26(GM_eV(B;58tLU#|)x zdQqC zU_LHg=V1LcU~ze`3h~$~BwM3SLkmnXFadtmryR&}poCh=^2&}L*(DK$^_tybHJvJw z9Z{cVM*&19DZ)qD!uN(0d&vR!BMu$;yKJxiL5v}Ue-hVM$6o#LaYb#j6c||S(BeaX zT7I9!HQ#6>&-3YP;*{Z_Jogy$1nzo1$NTVmuddsFSb!Z(mqok%+I=LUfL1tPyGNaV zfHp>If_VN*I>Y%JtG9`Yq{?FpPI8ro!Kr7M0@Dr4*ws6d_8U=dULVBCs0aR^3W#rq22N*`Qb1(KmagvHIM6kz#e}+OFP>wbA%_x`zY`;%~y;< zbWL05A{e5)&&C@vM?c<3;P(?5cGUz?bG!;di+6w}1Vym?C3u|Lf8J4?G{&rG?BeGv z6s-KPT^V^36W?$SR8aVASq9XVchgCOfy{#xN_eU+tX%qmW@3h6WehY0G^kCxGP;Pv z!7P7u9zr~S9dDI9;n@6V4jUoBcwG*72H$cHk11r04t2wVGhq>zgkZprstGr77d^a(&1Wn2BxC zSNFb&_16w6z_lc}8%z}b)y1}tb{JH_xNBI1=QHbO0kiZ?p>FZm>m$f| z5TNyT*weU3@x)~kpeh4{PBH$EsxcpgEJgK|658cx5xk{VbW7SijU6=Zk%Zypk*sZO zZDn`mNp(>#(_xi+TGK2>QShVZkPd9`pVxtI0$+~zz>X$E(g01y#zmhYSa3J-@5k;r zbRNBEg>HR7#udV?P-QuR|TMU^(?O0%<&l>P4WJ#bL1E@BS+;N z{i6tC9$=u`#jqkJ3YYcSUrd3+6s{aKeO00-$p|bVL=zVv!Kn(M=JVj&zaS2h+ypj) zc&~4cF0#7mi@M2mXw2fWMGw58o1~9)>2FtHtdiCjSliMpIt+g9hIhiww&e`&w?aCX zA~;z8+W6_U@}vapoGkpeNlsuq_TjMQFFhxJH*|&*S5ak;Zm)_zJjVnI+4INRIfSPI zu35mNYCHBu{0-JMBM<}e#kjTUz%nvb&aE5UFG9;UG5;O;y-w$^%T*u}f($&m8A#T+ zeo#yILSG2umg%MB3B`@kwm%tLg|=+p)3w2guUqM|bzZ{?)n##Y&Eg%!hIIkIY=uog9zU!eE_VBkqzz?pw zss4)axJjcEq6z#H*V?ExaZ96{i@iRc+`9r{NTwHnX5p=<e2n|O22ZE)a0LFz_+gcMB*%`Mvw38?1Kt5;Trdy*@Eg6TyngPXf3P z&$BWVfZuR=Rz7)NFps3mBaQvv)PemHB1jf}dJ9gqBMB5j!pz$XalIcCNP|g6(3$Ug zjJ$`l=-9zS(v)Yx2xQ7!yYG_4vy;5u zKE;el?&uCG0b~dKR-?D)X|Fi6I}Kc&WRs=r5n`3X)^}%UtTZU(_^-atxR)x- z&kXfa)Xw(qVBLapU2Dn0_}G%c33S7%L<3)vqOhEV*!@Fn)`^O7Y6xQkh2lMHY#WfE zy@uzfj1vzVhc2*SPOu(fr7Vbg{*)p?KD1rh)C?MEpA@M&wF zGn!(5yHe^HS;wjno)8tv$_6$Nz){mh>f0R}5qSUiz|JYJp8~#cD;aPx1=H-t!X#54 z@iF~>PNwH3**TVE$I>XeYn%GeQ&q)$-@?==^Ew7&45DUsVe5!nrqqxl3r`{h%9mFJ_vm?!F^-j-wA{HGTs# z;XTGOEUM<`#Wb8dlFh@O-^xmE9lX^idSPv7>vObBkDb;da8YxwA{Yd~`^0N{dYRmk z&g&^DB;OpB8zJh@r5KOsW?%@#uP;K=KZwUdR-x=NJtJb^eGh`zna8P^F9M6)M&ljl z?2J@>MDdkh)ptmjjixha7xGm;Eca%vh_66hL)oguuA?al5nNs!Z3|Dq-q+rZpw#b; zNoj-#_L-B^ZLO6gTMD}y5L-M0N3Q3J(=O9uKg?iqCv0hkBWq#7 zu+EBVK5Tco@Tu9r%)Ad;jp2Ha3a>=2T%uK(XgDNP3k7R&!6s0Jo$hi>0|fH zfdwxcO84r=F?O#9=D~V^OBj1=2)HvRB`hWcKgl5^>9T^O^zI8KT(+6KJ zG<^_RdD@}MvK9fdDJN$|lhvO4(I(GmJ~}g;Z5;;Zz^3->ghJuW2wzICO~Y>R#V=WO z=7IjXfJiBQDQ?Cd1|RJsh~)$01nWeShpd9`JObCv^~0^KXsL;HRzI_dQhNM%-M0{~ zeUq6B+0*0HWbh#5j=rgEy}y4m`D{6g8`5`Xn`CI8-+5-(YZR3cGq9v(a<~5}T@b8m zzfd4}-qu$AMWuM`!otOCC-$LuqH;w{ijPo2XQX@Tj{pgAm=$a5``yzPe^d#U7FX`$ zPN)`q&U!k>YY$bR(bsZ2UID08$JG-4mf!rUz+}Aw!9xoE_?;lyRP3@e<OI#0mPiX*vvNa-%LVl7NdIQ-l{=H?l)i)vRjKWjcWY?ZAV4hE&!2(%5 z?QP=8-Z3g2T|6$tF?^wx^>5^%T-NzjZ)q8-a{{gS6E&jM*?nVV4243SQW3%#HL7BqfH~`pg$yILC!ehIbVz z{$Qh>J8V7aF0toCaw?xjkLZ9lrIOsSc=pQNI{1>@!;(^ky@ncWljiO9Nl+Rq?=#6? zdskI!cE&#}XL}jqR}S>6NvVt-qwUaI=<#Z_5Q!xw8-oK(4<7$DNa$^COT%7SI3LfV zR8AC#$2?yD>jG!>WrMpg2C&BF;E7Sl!%eou1LX$_`dV4l^UoF!S^y+p zlu)Aa7lTM-7& zKT~o9E1ip~w`|raf^36QezyQic-757AH;kqURZssKDI*m9_3;tPxY;k`1YXv+)O!a zD?65DKoXop$5EMImlQXHHY=|2I$_ofUxqXMqrGp7~B8|HcX_N_;&_ zIBf0Q)F}(Wr&4_p$6o|pI1VOMn`+22dfAm6~Uy-T@-O$`{~TpeaZ)8V1Aab z)uYiaFv^24GrP3H&<6_lJbmKDLtQ$8Jvx?g#fuvFA-xHg5`lf1%EamS*wj>>n(~f= z6fmi&?8v`GyvIkF=U6nYz&-fjmRKudQZafUHH{~&o+Vg}b_>XQOuE7?S3BhATa9Kf z4bOF7;`RY8guOywQrz%^n+NQ7-@|;3E7$nt1s~(CiTONC1jw>wd~9dkpYopCM`3AX z{xp_MxQ;QVg`*0wv{*<73MA|69dTF`@4DiW8>8>O3~1}6*Xp1Rxph@a_M+I=p4h8g zIEP6kBxV0V7K`A1Qw&nZ2y>4)&g=b`cSF2tTxzmU_jn_JFg35bJN9+^9?eGKMxYY6x`dUd={=F$%Oc0eq?6= z_xu#|3|ue~q}j7)Vc@|UqwGO}j z$SKv|zbU{7pe(nyA{AaFF*}Q=)aEnhON~)9KU0^lYciz+G)~6Y-kyJPec_D`=pASQ zfqjCUe>C2ZUu044Fw6PRJ*M*c{TiHHf!4*~0K8H0aGE5G(H z61Xrwx=(Qb$^B7>;eR@$6w=%c-LRe9zJV5wC257lr^}w9v>E3>j5lv?1^-Uk-b)D) z^(95yf+1-%v8{>X_Kq+#jlRrHDc?werBwb^`2hdv7k$#N_jt)p+ymMel~NIeu87{U zEa_yqdr9SK@tY0&(%pe<#&se2C(o!#E4Jc2&~fOTEx5>&`zK;1u03&?bY7jzfY};X z#Jao9n^#4hGt=cRr%6`HfSDkclg`CnYQb9DIK5OCW^Hyqy*Y$1%gF)iYzsP&3%48# zU+6&V0kb4P@n2kENyOuNmTjJ)m2-{cqtb?fYVz7;q3LX974sd3%f{&Nl}7>RWW0d0 zUY5=di-C+V`Z?C~qB~7gkJNtb26U1c<;*?fch@n8?*`rYQ_3IOuXzDl-Q)J9i85&$ zS$=k89&z~O&?de(7V^5!n%8k%8Y5sl=G4hGKHQWUpeOQ|e3gycb}Y~95HLSU7v=f} z0{U;**5`{8-+#kjPNZ_a zZL#bLgZMUTFJF6qr8P_fZynFOw$OX!V6t=Af1)OB4@CKNodg1>cw4uZO5 zy=Xvz-_q6SV2+_*j81(0VQX??#gUdwps~zmR<0O6vkqj1wwS0s%@D9}gc_yo|H7tr zXYC7K?e56YCQIUuS+11Q!^6v?Fx%A65b-!^M~gJFr_yp{bZ;U39^?+sVzPKy(~EQ5v(Q7P6>=y{()~0l zkB`sY8Z-{EIa?mYQXM-*J6Ku5Nn@Pad=mRZUp)E=7-@Gxn|gB)|>Y9LNxO}=u zO}C1`_NGPTW2Mcx8j;}j$vYbEmOu?8|7#bPrH`C!cAX$5YSEe^29F zet8XUJW2I8vMk9uxPE8}H6#t8%i#rza_&3)rL-jH##g3P>k8&idpL=>93TF}me!GS zQp7yDvVhSfDeQFA@v;~ZfKbw9qk(C3ncVI9q)KFEDqITk&Zf#%Y6C%Jch%tyFMd|* z@RS!3Ub7DozV+9aP^CxCc*J{O$81NJZbhzjm#cU5+J-L!L#J>*>0zOIBYK21>y91= zV{))Z%f2`pSG4R0E>dL%;!utAIbaj>gGA}|+rIhinhYZ#%^88)!F%JOMFJ0$+7H37 zjOLP9))cwnl;Ri1t3utjedAv7?5PJ*$9=+@JhZ-wq96rk{TLyg-D2MOI;B3aiq@va z=FIT-f&Nd89|(y&v~tXKz?iiuy5fH#-KZOx2^ZpACXt6~%|W`daEQaPKmo_4TD@vq z>c7XOJ8U2d(>T0;U7{-dhko(&NFTf5K3QN(zlA)Cg{z{Xn_2HH{bBaXhsxsD_e}|( z>YY9R#K_iAlz<+NPJCLL)_))!9muUKT^__1Bo$Igu&?6*j5B&0G?!PY7=*?YoHod=Pis2(`+IRFikH+&&AYE{_7o)u*+K ztKAGO%|kKC`+gY7CSP@1nfK^gD5t-xnHj}z!dLH!=|NA46kXLLtzOnGjEVA3fd!=3 zXG(z!vid={IG3!yg!X?c{YuZ2Bn37m-6t4u`|Ne=iIo|>1!UES?@oI+^Ow{zR^nFU zw~w!%{EXJC?L$6sv7ATjv7F#?NWN=NVPhHwcp|{wDnE@5L3euhq6wR20rG{`MXYhv zML8F~2}?1dV>s;h+p`RU&f*G3`s2ud3~*m|6O4v*)fvnA#s-j8y`G$ozMf9HUx;xs za4f-{lbeXba6a{UmWKj5BtpKwCPFjqsw za=Gz_Ok%VQlejUp4jRO{WYcxyn7`H4@ykijom~4io{EACE0Z9C0Z7sKUbW-*%G0vH zONa^v8-W2l^DpL0pE5}f#7ID9fppjL9m@^e%= zABXepFX&i^iQP+Qo=S$TWb(pB%)#}T^S&O#(0Y+|?)k`Ajj#HuY#mz;dj%hd(5;)-)R3H_XxlWUX<>;4*fmYcwwBnM?w6 zy&O2!RjU`@W-VK;_%Uo{)|1{`KSrd>R+;|f_Ywz}Et$usl?6mB_TZ>xIhvDyq*2*| zGgHjI3f2n7++=77M&C1GOe3*2N0QQTO8g>5HE=(O@t~>?28kf1hzsWJv~r(1N?a4r zqFE~3^$0bdL0W$r3crkpTB|FHW>`{%bGacuZ?h}|{{VHWl{9Nh_AuwX{ zwYr(gW4^1~yc3GIBOe?{DH=jn7bVb`k8vvnYL@v zWuDgW2#@1QApLY=rS%;TW#%w(o({bVE7f=p;;VG!J!4=f>0i_<`TOySlc1R!biyRu z>INqB@H17F-*=Iz!iYN4TLr=nVemw#XeN~P;DU#Kvnj)UBeSrmA55#Q9dJ%aEJF&w zY-b!{LItpaGC$>@K+^7?j`Nn05%OgV+4vVmwUqDI+>W7&$Mo>yydjR zRmoY331R2$ExH=`<1VT4`9U>2bma)uqt~Nnt83M}w-akg zkOA|GaoIKTG1B4tMJGngD2q)ZdRxsWz=UC4#(`}X1Vf8)Tx||nOiyirGf4YJzflH6 z7KVKjDSN7tIoBj+ef{uzs>bW$ead+Td%eAC5C7{}5?}KWjy%FaIw2;A931YbN}h?t zl`*7`;Z#y5|JR3yZyY~fkOKix8I8+~_3UbC^pQ3c_mVBR7W=o72dgTtN;#WJouCZjAb?6UwQ_t8My2(u_?h%r4g7;vBBCo>Bz;F8peD1d5C-t7Z zz6;n;DY^|VTOQtB6qoZo$#r+MR$Jj%;|H`Jx&Rht+w6IKg-h2_L^P$GeAU84Xq9n? zcRifZH@fYHvb9bQBn;vOU*VktQ!Qer#<)NxEgIytBA{L{mr`V<dmk26{HNAB?r{?w~pDD?J-|L(!Kt9)@F!~>uG$IJ-rzoRxD%v}AtSk{?g$7PSS zLTpKLqk7DItRu{hEV+icyG}fOrhb0Iht}1DvFi(9R^4x@SV-^OZ^W>!`BA4jW;9oC zBs0ffc*2KmD+eJ0Ux39hu$>sWS4dOcH^>=YJB6BM#)8S5;iKwu-mnuEmC zYLu1)+|+1TwVcutCK&HUvnRA@e1WvR0k!)066Zt&GWl5^-ksokce6;mY*tq4hF1+$ z5*AvnmQvPqJ|Xg`oYY#oNgjFVD_sLhk-%N7b=JGT4#)T0j=bpADE4%wZlAJp^ec#t z?8`Mj!fD(h_vSaH=?G|nWRFg)(098!AtV1Ms#Q|KI^A-K(I|2jd*aH}icQr(V*O%u z$*FU7QV&8Zeo0JbItbJqThYn`;xDGQ>$3ihjpnAmMW3qiZ2vhdN+7yA{yZ@ErbPlVO? zx_Wk--1_Ec`6=SvBqiQ0e^4q;3|-4gJM~2xzvd(&VN@A>U@7otYI-Dm78U~W635vX zz|p!1(^ap*jTw5Kho=yH_2ZrKoQX7hv+%_l%_UaR5yunXbnvY**alg7hH<#$Co(+=>x7~Fwu0bGy~O%Gd+<}+H8?mS zt1qc6j^)!!AQt2TyyX)UmkBe|p@$EV7MzjBTvOAF5zRJLa(gNY{F|Q@Mk>jhSZ+U0 zfL>u*zqJT2V9WqFmk~`#cpBYSiEL4~hpaO#Nnj3x(SNl&kU7Yl&2nQ)!s`J6?xWGY zHq9|j_H1eem;}U1TdN~znSg*GGn=(|&I(~{EL1HWalBq2@s(0k| zQ|pi|o#)Q*I?;vW55#MGy1^L0740u{hz^)1#L*F*$u*g<2?k3YIzDo*-@F693)6m3 z-=VK}WW6lW_eu`)x=Qd^JqSD$Cat(cC(zmc+Zj|I2X?ssWl5!WJ$3~D~blCA@} zS*Y^$zQ=F!l+`31RpB)zHz*|5veUAIL7ARsOti$ePWoYCT{g1S;Z_wB@GIvD!Ka~j7I+miT_HEG(YnBi!v6IS^T(Un&?6bRAx3h7`n@HHt( zA};450a9;r!Xfa8!jo8!343*#`sW)2RGnNJFUyHCeixW6v$iF2zB?gk@+NxxpnY3m zOJW=@P18zv>EjqN1o97nU`r|fypBZ!o6@pv)g#aaHJG>a(S`fR$8 z7Y73eG(|M!TTy6`+>(0bZvF^JxRkFdEiq2WS@Z6v283`$e%IC&RMoZw1g}8^DIrqq z$-ZJ`tr5`SFfC!m9$1e=Pm_;BCxV8lxICF{2Lm0UoUQy>x-u@%;(y!t5r6+1aZhWM zQY<|#y^c$KkuRP`>(o(kHaW_20mN7E?(7MPJ zKc+bF(8P*Uss?%`;UmGE{X^E!46xM?N;}i6CtB~4tWU4-jQck6&}v^&f@cNvV<< zE%ROV)K=t9IEE-rK!Ln^v!5j6Z;o^keH=0Qrp0i6!E!?CE7Mu3)Y@r3HqHEc^YT?@ zZ4<{P`{p3IzNTa;*p*Pf;L*ysvG9~i<4njqxYU6KcdIE);w4bRV0SmvN?$}&eqDaT z9muDC!A@uiR0_rmo#mm8U864!U5ay0I4p0|LyizwH@;0#{-R=p!kJ-yi}ncM0sN*_ zT4DAFHlw)$`zCJ?-&N9jo<{D;wgzUIvVqRTfr%4jq5EpWYK4IC0`=*Ge#OO+F}M5V4tyE`-+aDlXX~_E$kqlv8Nb5!uV${rT3~h1EJnFv*YngjP7wcx7qi_0%qk3=Z@#(+!^}NjUv_^_&QqWA1c{#AD-~}}C zdE$jGbHR~OCtr98To*JOa!TZSYhI&?H~?%rW^rvjwp_aq8Pi*@c5bZat#U6#iZZl- zlX8)W2^+Nq`4=*zjR9K`-o1flO;`zBv1<+`+KR$r@|j~2pox7+;rr_Wbmfrvn%_x> z*AAn~ENTbBVQK>qXoQ&;LP>`Z<00=Z&xb_%|8>%+6wAM`n!i=RqcGkbok)@hu&63y*$8=Xt6X ziO{_uSLpY*y>vHp4z$~_uk({mI}rq}5>&X-`0x@s5Vi>~;XW%AcxbNVAQR8oRs-0| zfdvoIrPEVt=M`}}h`~C=r9%ketyyepAn%;?FhAkQh*RRkS4;~$s+jVe9C)M(hIrX< zl{D)MG0E`52;?6syo6dHml|+;5xw8slSA`rk#}GXc2bSc6Q@hUNMxOfJCp=2ee#!%%&baCG=u5&hx)?ki&lT+6(EFz85?oP3}vIZ5&chYLeI zRIZ}BgP$T(H|R!T<5x=f;+d2D`X;j?IOho3f);hWX?cA*Rj1Kwm*JO&FU-2CN7CI` zfZEE9-m>tRp*e5vgKZ^vs zpO&L_fWhn&q8s-d?Vdo2OXY??xWgsDY{1>FRUBiw6DX&c*FCH;|MIh35dNRsQqNWR zk|2?xM}~cibzLS%6w#CJZ-b0P8i*osVT`^#31JCG-Csdj56ror2i*qWMDy$M_Zw%(ffdHP-w2Yk>8QZJLf7qv4nls7Ppc>v(RN-Q>u-@GZ8{wid8v$ z2`$vuc*MflHF$G3CWA><9ILg(v97hOUGs>JSUmVj3=6FooT6JE2dD?Gv*$qiUx95f ztzN(J%3j7#Wr{{vAZX{7$5G-g$7wX}Tt2>?Fik)uxWDU0pWnYGW~8NXpCxP*L+K6V z!PUMx^=-(ssoBfrfPLorb}f6A-lu>u3K%>uRon3fAHNSJ0?+>wwPXBW&bchIgZrlV zsd#13D%X8R@&>nJ^J8l}fT812?-ON}AnsK<^s%bl;#*L95+ql-%3&4%(TEj$%I&$ME|J_q$SIIq-Y5D$ioC#G^|j z_{|S>Zy;{p7xD>OS-X-G%uV4I2zR0wciR(jGk>nC^Cb5dyoo`} zWw>t5o+Kf~uy@S_R8~b76D2&xCf1?=?sFqvg6Nb~|EVud4K$R(9>q79ehL2~t^*PNok97fw#j@ta1OX9 zh_;}bgt>H878&!!7nq~+`BqgjSkkf$+5AOVR^c-YhafY|8CQ&}dQTE-$GO8G1x!2V zHoFAATQHagj8SL963#}2LBn`-*Yl;A>{H45+c9`iJ@mIUz4cEg@WxKNPHCPUIiTl* zPQsJqP=HFRpg`plS+XP1d{^m83gVmI6Eah@vCGY|XZ&468G=Jat*-=&UPJDiQbtKk zQI*{vd&KKN3K%6H2Izg}Gh}_eJ9IJLPcR4gdg_2J>!nUHJI z+FzYgZ=-LQ_0mof+M=bW!fj5>n~Tm(z2D=5;$?M&VW?to#b;T`JNJZ)aswlCkIzO4 zUm2QiUa-F(p87&97#kol)!cjPW#D>k|Grj(towvAc4!?X!qi_gIL7N|!~Iu@6j%sK zYlb1hhE!}l@)@s%_mROi305&JYq)r@F{&`hN}VeuKhdd*A-Vema=Rv`<0!30%`vG* zQ*Ig{W?7^PI^*l(HKeSZ$7e-*eC1$W=^Lq_9Q#rgrJC5ZvuQsioIIOA;ZDcBZ#Hh; zfi=H==<6SXik`E$rNAMX@@{sXuEx3C+7j?|+6(qBBj8zFs0AhxpoI4ntIS8)oJ0Q@ zW|-$s37%!PisHc?SK`Gv%qnD#y9_L)LJ3crctNTzt#GZ5V@b-b)aA1#JlFCc>>cnS zFUG^Y;CBwpJ=IwhA$-1;3GmrHC0cjha!zcwsQ{eFda@|f6LXI9*@TC$i8Try-^ach zXzXZ65J`q^_TSIr2X04ZAzBhPg)$gI*U`JEP}roHM)r6en$r3(VtRU3QNG33Xf&8LNMe!wvNv;d7I%ikaYs{% zF85Jb5S!6SiU%@e%5g2@cI1!5FORufWsE+MMC>t?Ph~w!94AKg;4*A}_b-Hr2RWVl zO;NZ1Sh|@9)0B0BIbx$Mw~OtMRIlAjtOcPC5aYCm?O97?r53bU!fSIge!6NcjvIdd zpOZ+#timTc^^W9YXpwGJqqqbp6{QeH(oHjt6j*0Ks5Kc(Iy)5oI0}`5z5~W?wvhz_ zK38yZG(ml=1+UE)aMTC|O!KWE8HAD(Zw*8mJY#7h!~iOoi!Fs5*#rwNgoRD~CAw<= ztr_aanrrTu1EjmioD_aCoGsl|_3FY!I;peM7gBK(nh5f0G36vt)PwW!ljAB&ddkl~YAdy1U14%ad=CvG;-oi#+sXc|2 z)-U=W%OzS)e4vL7;&-=e_SlNA^zrdy6K@NLq$T&Boc9u%(7~@22^g)Os=oB6AQJ58 z;dIaJq^VCM0U&1At0TJEmEOwG}p!^FPPQ1m)}VBuN9H zl5*EDUKn2n45z9`Xetjmg-O=pw?ljTKYDsIUUEQ1e7W62%K(Rf`lgX(e8ZhP*fYUu zTUKGM93yNeFV$!IxiH3O2K+mC3H9<57%*&9B=9514w3`ueFQYN&s-}09a_p*_1SI+ zu#A^*KA=Dfr*?&a+0Qlr#L!>e0V`kz@ODFXQ-;<% zlO88#rwSV*L2(|@mbP_|#a-F7G)59}wDB^$4TS5(mEVuzhwuf!$qu?S`NeKZc`syF zHveyl0#HX5i#B)dAK z^^4r+%yhH z^O zdw6|WkX&{v30Srgana~V;0Sw13@~W#bG$ceL-ACrf(N(tX#h#c&;-%tCa1jl6=Ezq z^k3$bgwZ63U{+g>b<@59i;C@W!XBKHN@r`podpMHA+ooA(8e54!a`hPa8h71D( z+>#T5L5mE3H1Dr1hjLV%3c3XF<$u4`!;{Vz2^{~f0BS8v@MhR)M&*??A=s)s5 zFZ7)W>4iKv@k$S!Azt=H7GnDj;K@7$_dD7Fx9n6JOY;z-_icG}ZjN=BkSLq={0{`u zeF{aVzG*q8xlfSoB$FUx^97K)5kYD$@giA~5)-p{f%6)DcK}`D)KoUSCY{ya#{kIo z4V{oZb4unv=2Y(pcq>Dk5@+nL^Y#oo(Wr`Rmua<+)nlA@$MQBSDHm5Zby2*a0L9nEh&DzIxr{BT-eUNFS8#9BJlcIZu%fL>d;rkx7%C@gBf zJ%^f5IEcZedG_hz&*DY#P{N{3ZRRGr8{M*$?e-``G|;@a0eF3Y{4&H-c-Db5mi+%T()eD17BgZM-G%NBtRnmE9x!@l=J^jI{koa8Zm5yi`Yem zA6(b0I=EyHa+5QjND<^w(L#O8nmh|Oo$_m&8q@-zKj3q!lag zAAG>FZ%$7vae8?kJq!4)r644oyu-QBjlJoLZ^hH`RFBI)_!c2;VxFabEL&M>&Y9LO zU?ezVv4I4S;b^veDV7XE`2!3)HUP%+QtqSl(*;F5YTqxiNc?u4_s+X8UqQS&0*KqeVs}&%}6;K807jx{{(0mbFfM?h$ElZGIA4+7#i>ifdPPix!^8T1zvZgN3N8b z&zvIhrWQ;bbO2Y#D1ezmwCB^px#lpzy>CC2%~Iwlyna}|)lWmn8?tn}aA=liv35_= z5rH4_&xN3NMSmSVS+nn>bXjO#C7jI^lXp1ccv(bNj%=t!1?V36lkG@+%XSKj_JVjO zbocWA%+JEZtNc}`gHCb0^x<^}qR=C55grIAqS67&gfwyAwr4Vr$*#qKk zPbTdWp?8`fygWDCB)nf}BtB_Fp;^6Sj-0VD1wJm@^jLOyvW>5)IBZLsv&(=@Lx+{m z;YC^eV}>pG`Z0vwbrgW%U08(d!cxRwSMERxATdmuPFNXG|B}qy1I>_H(1#U^H6SA0@VXTPW>GR=cXUJD@FfZ=eYrKrDFh?VX;}|bz#uTZwbdkifFN>%!4hu}Q z*pl8}j!WEV-+wgJ?9L;54MnahY^YI1w9_8xnwa_Wts`xb1)$zgyo{R%mI7+fNPqA<*D z4KR$>11$m*>#X~1J0=O+l;pH%EA!JxR!!jxje#t0Mn{9x&t#ez@naBZKKTI*&F%IW zzIf9VCd|CU5OuR&Dj)!9C0hF9kRVph$R{}%B!H)v=F!p#bB^PX@V+ z%1{9iTgI64xn7k`2;kcE5h&g~J7#1}yZgNAA$pchHq+3FdwPT8P(Ik^*#%%7o`goR zsoiXDU```b0Dc8OfR6$dySMR|C2}xNa}v-$-^THvNgwK-%MNqVB0bck%YJzQ;{UOu zL$W}&Pig@B+$PcN=wW>>RSy%NltlcaL~0v!)uaL>!XvP}OEPZg;i%h@$G1yIep-jqZwcRULnJ1X=&|S{xVFa3;f3yr{!tGGpdvNViLwod@Yr_NBk>ITU*g z6&K6Y2R1x=R(Qkv$X_g?3l;lCk5J^hx~T6qdX&$|;-h+JqqN5t!8n_iK$jVfb_(@M z{ON*~(*+@8auD!AK5%l}jzQfaQxESbSjhShzFbUU7l>-DqmLCPdd_|zmy!7%sfj8anHxSy?C$yJ2obt^kW}8JS!BtRw?YXeI0cz{%^QE){Mw3 zpz?CO#Px4HNy<#=ash6qw9&U+`TeX-L7oz#!f(7R-(j@m?RHaWOnddzJAs}ra(OBo zevgvb;iDZ(ibO4aO!u7QpnUH4d}1C(=e@H~rj3DZZ$N#1#0BaL^6GJ2DC@UpWHaJ( zj25N<=j*q2c99-$zRDp*v7 z!?@bb7YqGMTq~Idr7Z~^g8Ul~;+bjaN8kP?2a6F~{oC0)43exxAcN`}s%$J#gM`z? zt5mXT2?yxCxA%^u9!$Pt%ae$qzD8nUA~r{wUnO+MnwpVVljLEA>guDr5N`@BY-qe3 zLcMY9oX;i0O({mR7Cre`5J+I3mE$P zN-cRQ{%j)6yMAV+D*h}|`fU->pq!@4c1P2FNUh2zy4ZDM;JE;k&xrq!8tMHH;`m7! z(!EPYJJpl5v9k!DUGw(w5Zn0l@^Kf3O_f|}?fS{);5%S-{#H~M;+@O^1@?k{@RN6> z`45;)q`) zs~bB{loD9%$d$6|6)0O1y=0{oOrS3cRjbJ7(d;Pvi%0LyS_gLBw}CuX4J#e95t2vU z-{)VEZyg;-KC<@){9j|AkL=wVeihH% z^!TGuIjX}W?r%)(Xx(H2c`$zRbb){U9go`jX4j8Ijq-WSrY)zVgU%OuNAHWd<4d#Q zrtSJ0QguHU4@TIvFWvmMb{paI#yCu$npT9OkTWs6mqu|z-ryi7G3535$olcT*9rHo z=(ZP(E)z`z7CvcJHE#B)mo~b$pT--9*ix&(RpGRrH52f1jV^uLRN}2m>WaEf8d=?m zG-)d)-@u;cTK;f};nqQNp<}t*Jv*Do6zk3Y2V|^X4nHUor7yK+G+PW#gb5ucUY9jCD|F}Jesc~+1rcnnG#SX8VHPFCvjr>JsP)McIOuXQ9Y zjm@Cg3%&%bPqu9_oo)->X;Y89m0e-VYqM*u6>@8c6SvmWuwlIVjoh0#kc=q^P>UbO zP4Z8rd_F(ylW8NRl&ibr)qeWxt70}K=CO}eq@JsdL0J?9fpMSQl??u!1qH{j7*Zdm z(WbqH*KXHu#`d$G0CA?5CpAEASGzd4r{z*io1*1tXL&4NnF<6VFA6eMdxwa;-i%w0 z7uPOwRV*RCJljWGxgVc1`mSoj^#Jl)LzOy){`LWRydmNWb>IOR4k3-ZvU5%@Xrk^X zZeOA2-b#uj=S!@ve!qzXn+r4vEopN?b#h z(=!Kn`QtTZhCXI0R0a-;ua1|G;{t}3X0PGM`NUDou<673A*>%?(o#cwH#McQb?}48 z-Ee;cSs7$&uFZCL-w<|^FCwV@NLqoPT5C^@v;dP=)CCAs zwrkbpsLqVOqA$mjw$2~UXvR+FPOQf#_wGJd_wUn_ekYZ22%q}1$kD>}UTn5HWSqpC zF(A!9Rx(mK+44b4V60^BbJSc|Lp5E${YaCAvg69>RwAuy!TU5eabCf;I{IYiD>DI|s_5vx{{1%Ksvh`*HSwoD-1cB%)af<`i$#ZB_5zx{|_;g8pi zDpP;z%JaEbmCg7H%AT||8?#m0+7{i8yeHB&(CAS+OdL4rhiM%RG7lQ)BjM`4HBV4Z z1C3m-J)p{_poWbg_R&K9_@YA2v|k^u9KPBzJ3#EtY}jpJpA7}{Cgr_&Y2J;}w7)i9 z;oSN%;IdX}V_5r>%58yJ`nyN$B;^!uzSWp(`A0pdrz_Q8MEuQCTQDGhfij2iQ7ph# z?1TX3tD}n$g@&(I1}zq$a>$ZmUh1zVL&gJDauxIYgM_d9lm|XhsakI(RxQ3~#!|O) z!n)n(QdOgDSKR$K9A`QdKZKue_uZXA*|yIn=IG>{j@3WSM_R$m>tnV*JRrk>>QwwA z!ZY5H%6pYGO%>wD|3YyZjl807ASi9#gdzOum`zLLvhb~XGb%H?ZWaXXJ?9{FQ7fE$ zBxK?)dHIVy*B*yJp2VBU^gdQ+(CK4j&oPaiV&2;77EtLKuXIhlm80U9-DuK|ta8Gx z`I1!qtXrPC9+~clcXi59ecu+oF%35jaBrV^>N1y2%U!e{ed-#HpC3AP|LZ&ch1$Vy zJ`{ubvo%i}mwYHU;-@KJ*m1Qv%=-)KRzsGy9WO;+_uf9@+c<+Kbo(}8Pmj<&P1-`c z`*Z?BgWH4JO)}~J!Z%v5le?pVs%$Mgq3zcJM^86AbHxwt)0vnJ99fF-ppo@WbjF^_ z%tmX3b<=?=bD#3_1I-AcHO#w@gVlEYeX4=6fspF zzM%+|olxvNz!g>P-n~`i3OjwGwz++BcyFmYD zf#c;dO|I@oRjpz8x%qMV1J`%UZgwTk+Dcgn0k+xuWPF-2+l17|$X1#=I*{HHRnXRT-Yol z7TAsRJynrz_wYaL#)=P@#q~HkR4F@O@Yk`N&rGrAg+Lz1RjcrTEkGb>;&&M$9dKqL z@w)%6EOFYPVmCjY8Hs*0FQSia^QY2%-aXiaKuD|7#O1^H>%ZK=*qQ&cVu-zA4u%z5 z%Vx*`US)gZ9zPbvcb|pSTTHevz_dhDnfJsVvTn`4S?kSC4gGJ!S||h!2671;|1AaW zTb)`W3G}e?7!-lNLhj|KDJHjx04qmF`?LHRZ&PNQ*wa^2iC2K01?o)x)qo0HTC7#= zXTqhKw++N}eqrrjo7mgP-e=HZv9wj*a$d@MGu2uEg8p*4*|p%Y!q&1OYVUjhv92;m zI!TVZsSZx?Gcm5-$=_a*-Ngo&XF5AQKTiK3PJ4ayDjF_vE&WC+;eg>ChbrkvYOYUC zVoldQGolqHq?qH+f&Ko{3cef+plYQ5pWUi(lnQHE&iC3a3jEUMV)5d?p2z>B|3!U^ zpp3WDYY>#)bSM7U?_bB2Mjmoja0CXb=+buT8sD+eE3EG*vl%v1`p-fhn}n68lyhXM z|DAjNy71EPulw^eV>}0gOS0ay!!#xBZFRY&YS}Y?%X^I(4ng1cG8Fyw+)11nv2(9x ziYS*f;x62b8S*92XYFu8)!Op0D%NuOXAAN$oHoooPe{futf+U%Ld0 vD7~P9zWd zECLSA;=zpPsP7%`zn>~m|59gl%Xww?US2q)Dty68E2?m)#5Pdm*M4>X-LJ+ob#S=D z*0}NjZMW1it>TEezqOogoYb#!B6pfw)%}^GLo2l%N3S!r*xtBfQ)1mlTvBI-*uCO=B^7*V?{;?KAL zqXIgX!-zk`XzO3dBIW-23yLO@c8j=B;fCH$%woz2{ILs|>X~&BciFn2LRkBG%)O}r z_I97P3tE3oorHAZnyhy(ZFlvbo#`LOsG9zJ<$f)~{@_j#eZ;VqhRN5m^arAYWLD0b zY`=S*7>}TUtEG754+A8*J57%fYQDIa^Y%XJJvrwbUDf#Q@93V&ovUAk@lnIpTF)DO z^tVHO_}o@+*bX3v1*TMpE_NQK#?i_pO=JZW!P`VZ!BUl!gfwddxG$P zAEeE7m-aC(_xg*}v_0pwN^`9&U%_pAHpN~52s*rIU!imA|Tc ztXslSno_x-bkq5r-LBeAZM@rh3;E$;T9hH`D%F==_la#|K|pvk-MHO%ieqmfJ@frb zjmv2xq}6uKHhSv+Pl~?9RC{n6nUnTdqH=}ov-!ZMt(@Nfiy;#SO@A+e(XShgUfvF+RvxC(zmVH7ce+c**-_JE8{%fUlUM(W2XR@E%y=@z z!cH!`l-8>(m^tP=8vKl{^9PpLS+2+*e`d^kNDXrG{-$+7Wz1Dzy(}b=mx;w?H~+gB zNcqqTMj9xCFzTDV;|XdXc|pGi^i8q42Z8>7){^J4O=sJjcW!we5skWcc&!DyV3&76 z>-mtUzLrJVPLO^J`t8a);sYM)SaC#nJ9_w)HM44y`}Ea%Cu&=HuU&yJADd?RPqc4;gS!ZQsl?Jh#1tx_KyUfa-NLVd|Q9V;+SDo!8Xp) z1$M8B1tF@Txew$yau<2E2sd7dEtL)A``8rlgM__=sg7=+XdmT||Gr=|+S=&xxmo2k zs@lDl(tp(RN@f|{u{*_z8Ts~feZ*zdO4-em2~^^9fxvpL|LzDsFlfE4-_F$zrn8M< zBTP^iP|GY-C}jP0zN3~8fxhb?q~R6~WCKUnaB=>xzpUclx=f8Enn?Ck7pn((Yej+~ z^ENKHR!1j?|Hs~&$3ywGfx|O|7E9U)BSlgXCHq$2EJ?_|GpOuaW9-97g=C46tdTG? z*6jNf(PGIm_GLn7>}l-Fdq(N2p6C6&@4vs#=ea+F`!@G|&ULPHoohY!b(wq?p|kH# zEt_lYG`dKyR6&?LVP<8eSKSu@Bd&OP*Kob@%kEH%|EknoWsd(cl^zO($n~`-9Yb9r zH$3P(mmN2zmlXS_(tu~0yHA?ISv}JB8zem?L@8pKAb3CbOEnT*`c--|R$IIvUg($H zPMd2b6@|J-Eix1W@8Zy^8tmp-pJJ~Y$d2~Q==%$Gws9mf!R??G6kL2$_bUtcTN`GI zm8`$Je2M(}FV1XCSFI;gV16w0ymTJWz;d_=bP#w5jD2CC+oqPY+9*MI3o&Y$&n16i zG}EHlmbL2L_tMOjSEN+H&d$DNCS1XW0BsNwVm(|J3X(R?Ougq0kC*(IvV5C-&$bOH zYdeqZfEk_5&G#}JMLkXH>#NnaH2j;iy zufulfYXH_&wE_kT{M5#tU!PVkH#$w{C#_GmYaD4TBRwW8|D_F;aLV4O1nxmGOjt(I zG2kx1+b+H**>HDM`(QTGn7sjHh}sDGEdXOWR!Dj{zj(>_cgr(?xiSpe(oI#@qti)x zF1Z`Re^=#PX{uPo7ls749vdws1NmwbnAo4<1Z=vtbxId4=t4sk$)I?q)t4Vf;Mbs`tK zt<+(+m|+_0Q-85R#}_jf>929LIq9f#fX4B*AQEdrP*s|AF0oh@oRj)74q8FZVE&(8V$AQ1#p@`fFBD>pn zAuJXt)0KIo8OJGdC%guCP-z+{z75B>HK`4s67!{w^mN7x=^c8&#g8Ek1?NL>onkmT z9d+wII#*p>6xK>-RUNhqJu%ulxZze0bMI?Ee+1Yqf~tS&knrd6CNjto&f=T_O$Vtv zK*T(q90OWp-*6Y{Bv6mRK%Z<6G^r_LT2H39?9LGX{748Gs-AWnuoG^O1qoFI6&&)Q zm4U0CwiAvKGp$*PEpu)Kn-{F=Ilb-K#T?@$>rSkEdeAAiW6Yqhk$!@m(zJ_n4PpQS zG4@*(rB5)GvIq|WqLf{6k}~S^+oSe1Avg;!pm0&VO&1Dr*y;>M@HUjx*DbTHa(!WNcu zTPF?IjS4@#M>fs(@y$C0(RX?{ zGdtFE3u(4(1uhWB+%~qb?&7RO!YriO7GX<1MerxcG6XO)RMF_0H_2_T#i=HkzmZOt z^dkMC!$X9>N7nhJkIHS-$z@HyiW_Z1QF9Q^QEfXO?n-DmZHS*`O3HioO(85*X zSoFr!V{d!>J;ed%#$~61(z&)WpCv=VQ|EhwQ%ftlyeMzi$NC#@mk6UvnaKg&nJ;XBFCKl|w=iR28RzJrR`HmkajiCu+fj z17rbJP<8zZj^X_fUgYk_wn@U8CTZ@m3dI_9V5Aa8uRR}<2xS-gXqKoRI=AVtK^F>U8t+b zBwG+satIaFJC-A>k4i1`=n>DnF=$({sJsRT{1W{x*Rj8BD!nO}OLkCb5)kaa2fR?C z*!;#&J#b7;BH-kr2V*7mT}A+l%rdDflwFQf+R3v77WKoQn)4n^G;0U|Y&C0Xa{#tjJKWwwmJI?IDR-(FxM|6V&0e`fe)4exShx61%U^1Frt~1j zw7kI0zU1;Ze*y{{fK9j^Kh{8!!-eZ9iO}z&CdQt_$rcn~JG57|-Lt0)3Kp^*O{OFQ zq}j1KI;Zr^DtbBvb)PlAkb~^IIn`50o3!c7j;+@=`>e%6=MCp4aU-VR)k9Rm=jqDYY za99Mk4JRm&r3FxwUzBnVZ(Ep0iDnKP$`zw(a%+3Ff%|f-A8W$);H=|@SN?!bz5|xYf9<_K`knYvor5TT)^(U^I z|I!4-*Ejuv!P|^O=uT3-h_u%F6Wg};m4MARn&1|TZ_PYEzwSjY{Ye%e`N-Cjxn=el z8+JA(yH*XE%ZG7-a*+k2v0m ziZgbbE*(>YS(;Yndwuvzs*k=u38K-P)@#W!>b70%!|s%YCZx`oA*l0rkkgly`hk^g9$h$J@1AWol3aD#LE<>ayb+=2i&1Q5B)a_J$5-WY6<3yCdM)r z-_CA-vGsSe-y1j9kkL7#?e|Qk6bk&KFG!YzMB#CAA*qG1a`XqVF^@di_j>ywJVVh; z)oNmOzgw>=%renFlhX+0@-sNQeG3FJeTy*IoY4s~9%yq0QA2c7R)O!Y4@FyfE`!q; zWkm~S2TikWgM!DL101T?7l7*OJ%h1qh+@&1N%%|6zU!fj{PD<3rp(K#aQM**6w>fE z0LP7s10Fsd0ynPh_UC;Nh`#eL(}n_x(XGw(#U#nM6&_|}rN3mPKGy_0?UWBvah)-1 z!$T2(Q|E>w^%Fv^+Gh+1N>=S#jrFR*eh*3C-o0WNCiVS2clq~^XVb$jyb{z4&k=mdxhnxM313% zjkG(;NPUVSyR${ek#-CLL1E{)@TO*oL)zrZ8Ptq{pwE|mo$3kHC26X0qWxAR`Zen~ zQHK=}c$ZuIj6py;3(RXC#DOWdJNiq!fJW^Ly$9wKaiCUp8tF9Q!(Utq+@Sx;w9KY0 z4I>QU$I_|7Ao|ZcuJ1sBkN0$`m3v7IEG_K(eK{P^@BdKveu5ptvSWy!hBg+fH`|$S zPrWBC|1}3&x1&8Rez81onal5pi0gGtm+H46vF6{gCCAv-{DqCJhv13ZG80_#$I;2o z`a2Z&yJ^+4_RgHdH~_PWnhYn}WT z&)9kjp6DQj+&ZKDf`9F{443hm4LH_Uz1~}`3cFRxVI*VspWWf!)rGPPX32^m8R6RL zYB2bHsg$zr_T%WeOrv7Q{`RB3`+w=0pjEU6hX>I=!FT0bg2zy>mQuIjUejM?=;nxM{j25aW?x~FDF94hDiedSzpsJ~)ExtM z&|gh~%On0f@NV6aZPGi;xmdEOZT4L96GLvEmk+Y_zoK4a#~eSh?KE+(tdLBVM3DG9 z+xB7Ie+QR#c6bEr8r;6d^ZUF6nnLlZCKr=RjMvH}s0#rg}K%W9se8n`Kuy zgpo&L^5WEB=!ucMz+=8kHEE1cA2TWSe^>A}a{gEgU2gh@ow<~jRz3$7Ee?1uRxMWM zASXIh_dCpf4wNlG0W&uJ0~KV=xmOTm8vW~t@wQ82{QbX7NB64v1n-r$q=6|%_j2Wx zV?MKQ`pMbu38`YgZ~A?bd2t7?h$;sTfq{cbOSQgpEPi<~w=VPBs2ShIa`(;ELBHGU zzn}Vj0)N%Jy`PKnKb>CteAUI0dKpO~np$#Y{H4^Fdk1?;T!yXJr=0_(T*s3G!*qdk z{QJ=5-myqZU$?1AA;GO~&E5m~mSGZUQEHMl-GL`LT|Zxz89B~m(-brjvdF-t4Yw_S={@x%1(qkq%-k!F@X73gfqP?xmkDy!tO|CVSg9 z0VRzmihXGWgzMeJvree0DnGPHoWVV=+)#-Bg#qz<*07l6S3d(8={)K4! zEVpbY8@pF%*I6{^yIKf-bdVd%WA3BRt=tAY$B-=wEXZ%Ko)!77uZTMIw5v%?wKqx4 z^d)*vcbj@o5d<7vLW0}20-W0|Z~euS;5&G@$VRGH|DjO#UWZ~g@N?R*2KySwlxAQd zoir+Un=*6g%Keg{H3m^6-v@q#|C^3kzwYuyzQ&od+~JvidpEJTMw;E{S?@qa&qjvq zRXoRLd3vt3gZ=9XQ#B@C|Dq$LymuWbkD$2p!o523y$`RkTio4qm%N)0#1avyKrLb% z$F#l%x%lh2nMr3ab*Z^G37 zVErEk{)d78Vc>rl_#X!TKQNFRpLKMnhKj@Y*xHfr-LDrQ93^}saAO+@b9AV+L=u9K z&eBnjGs4DU*;zJzZadh~cJ9y)WgY@ia&#cql8mq_xE!3CLG8MnhKbp_3%SE~^~5y8 zj>rB!r7ueJG9-T@G?DNJvg5F&wi5+%%75Rq;~MTIsw0>=Fxbkd=#z^yA;SngHomPp zZut8YKO|A*bs6bBOvf$zdF2v$(zdZ6@$Z{tFj8OM2!dvF`l# zuXExo3Q5M)F-aIdA}5lVLsozMb;BQ*fnaBnaOg3|l9&k;;dIC{5Ae|bzaC6ej|pLQ zOd|1hD*>dEci!0lxRbFl`P9GvY{qXI%yDgh%`1H;{nWAlptc>2dO(`(*Jj_oGF;TL5@>4sW8)s>w%N}e zVgDLP5HvJVOcK5)3zsM(`N#GR%DpiRI}9RVE_Ub+UX-*5USDtd4~*gMw}Z-*2hj!y zHDu3t(8-0={~x!mjH97yxG;9uf3%wE`zHWuoG>B03nBO=H>H=I2bus%nEm^Y+l|>R zQ0hq#i}=q`>92Q$?!>@Jw=n>KtWjfl*uM=>&d`=~AN8ufH>|HN%GtTn3CYkga+Dw) z9jD!u+%t6%2t<08LH_8fqC5Utf&0wB@WAHAVp6GPeHg-hs)rmTf*so8P)R^d!@ye4$}^*>f}OEpy3mn3NYV6Wcv2zdk5$i5)BY$h2@eiuI~?Q zZ>8CRWyYFQa?2mGE53i6u3DY89I$InlyT@S$^t)yw?0LmD;Xe+*AGCcJNzQs?MmNEkaBk;=;*25x-|60v1)m=$Z2pODrs#bapOn2``TQ+I@yam z6Sh-cP&f2^#+NJp>zVr182%2hbF;Hch8`|VcIWeS#w8`qn9O+TRewKQ-5LgyiXMqo z`S1q?w`{V*a%*K}er=c71`7O*ehEm+_1$5;0?_2i>V*XNf$j;j&6Vu(>9WzWv1e;5 zZLb|&cRWDvygH6P=~h1u{-^J(i)?x;34$74+J&V(`Uy7@^s~&Cmb|gmCLXKVq)Uax zAGl`K#g+{h+f-?jXEGf>ML6$hi$W4pgVoizoT@T_9!5KHXW}KljWNH0tm1(lMB;qB zsMCOS=B>Oxu^d~N?4K`s=m6Nq`Q-};EQYXjbn2uFyg|-nyJ;UY&_9+fdt)6*7OO8= zyiXA180KUeff3Zz&CTFnJzUA`T3w6O1eLU0@ESw?G?U?4zS%UU3h{XH=87-2?B?Fp z4Slj-I^|o+!I7mByXIq`x1K{HXn}P0-J`e(B_#;ccmpZ3g_$GTU5`#t39nhQLc$gM zjBp<_c-Se&*$RBN9d;n#QP~VA-boS^O>vjQ2gYk8l-C+4z591+h#4ln%U_>0H8;!M z224T6Pg+dOy>~o_K>Gek(2umXwx;)JI!3|Ct1XUm75J{4TKJ9+^`5dP$R+>D|Apgv6O#g|l4%6;-HH(fDZJ>6>GL*8e-7%d`P^J;5vC+{) zNlB`b>eABEXF55o{cnGqwY6_jatiupTfvxtv7dGU}K_5xWlQ{0lrT*8s_FDwE%~!1Bk}=9`_O z!K+l$F5#=sTNs2VlSUPaAb3)7vX?2j85@EVu|#mTJ`>1Hfc5{2(XB~?BbPQvw_b8{ z){c@IRoMo`OTK3OK|&4P6`u3s_fF=6aIC0fpSUF0mqPgV1MK&!&TQi7`%`#1mn0gm z7q>76=2n%xmaOusAx3lW2=IWdyH)^B$AOAcFpNh{fB_=AwJTlC6NE}YT%@G>Aa0p7 z&~khWG8`U57{j7oUfH@rDv7iZLu65Q6Jw92NMGN=?C=sD`yuH z=wt>oP-SJ6J*Be?r*Ppq$Fdd#Nmg6K6Y7{FSmF>k;n+SunU^1+@6-7+1Ih2av)MUs zCVAsz2m`!y%riN%1eTdRUt9D3s@bJ4yo>NH(m;P5zDIJ$DTOjhP)%TQSSNPa9iAO? z3xSUUVNwL*H-&&0T25@mtL~DVcU6Vkq)9C1_p53Tqe24 zFrjmVYud6f`@@?7*bfgrZ`>3?mC(2^Tueu7)J_CGXHR+)$iN@ntUIq&MGMLOo`qA3 z{VpOacYO~erV2?Ij_Y$;$CA#*^&w=6-$?{6-n);?ooAeLm+fVlDfAtGQ7ot5U%dP0 zW=H%NBdyBpm7bR4O6FI{QpJ6DJ_#nd4Oo`qs3qNx(NO2a~)Nl{f+d&Ag`y1-(9~n6`qTiBd5~nz4sY=uynahX2LmTC29(6w()Ux z^;C&V&n@qpDNp}!e8A7JGRsTv0q+*=v>|+u%2D3ggT@JFSw{_Bf2d_t`I@5!(qrjW zDzd%okZtI5ThyzcRfQ|_ajnfO1I~AKKgvcfMAB+s%jv+8`o=WjVyS|MCdSA?4 zCGBt+Ebr0eF&IQhVKZ+qoYF&Gb;u@Wp+Y@yhWy zwnn7uEpjkoG4q)le-gMubPG8oVcf#PAe^5)mrdS2C1#TrT9|PyU~iDR(}i(6o!(sY zntEJ0(BCw}vqT>Og)io=2*D5Idy}#>Glk4_P-bKqfRuvVt zCh%<2oGR@JU#BQ&@TsuIqoaMK`}^$7$W5dk5{VS%Vz32iZi@8lm37ic%khwJ4o==i z;hqhak|4*XV!AXR9s5>0l>6Ei;(#2G6aB@Y)wRuJ%L1-=we|J&F@=TczuM3thrHxY zn~Jr|Uas#nMl_ONO6k4?tehP;ujCH)1it~Bq**CS^V!-ok|!Yqn*-~6%^~A!URkhm z3HkNy%IteSN=NW+N!-d8QXp1|!O_66L+9DKOtXxQk`L=0cDQE7$u06gf3-&;Rgu%g zFoj~L!K#7p(ZxU3#^kEE*rVqqJEa{O4W6qwoKgQo{&!C*NUr#X11)&-2LDC8*h_~T zOsclJH1x&eAM!(6Z?Yd{2Q$y7r1hc3GyE2~sJlCLu}3f#WFP zx#0Np*$%6ssC4}kMHHKDd^HPe^0@Jb0f3ukh2`#I)Rv+M&s>YO0MgSfLVMn;xo(I= zkRwU$c@k$kqHua*M=vY~5{mWg9vs<1xQc^nly|^P=0@sD7;nv{_yw?!QP;`-yLU~s zZh~9^n0#utC3BdoaP=H^jz){OCSXi;N>U>AEx6Q$f%z8>t^q ztMA;*qb`31V}9eVon!x9WN*mW2#JQkr01tyS>&Zs=y&rF{>*F?AzaOT@K~0w-QHCS z7H>S$+4OX+)$FSDcAxR6m-4C;PCPI9)%OoRu8Z<<#UEJeqQI?tk4!jB7zL6HuN{9w z#(wW=cGBC#T#f{Tb!~Eh#7py@s+5&?VAtp%&Qxdp-RBb!2Lwby$oU;jMvNr@z3K+=;%6ss_EH}HZ8QS7tm=M02T*f<=^#Vd3-`Kc*e zOlRyb@XVDXe!h5?=^CD$ojvdr8muyGaVuTH> zPrHXVl^m0S5{kaU%hB|Sl!opb10T-I;Ee0yy zVBVh^_m4ahPD$zdV+vQ7egasG!=2p&SN+;Xgld`ek}_lqa~Jg71$-JFke&o|*Z`?M z&%#-$%LqZgbG8M0+7`nZzxZP=rlAdtSn z2c$8aU(3kEP2C4=xFq0HN|2;zY!Jv&3IaQrop?d=9`|<yIqLSF zZ1-ICbD4uiOY><63*?drP>lYd5{}kANM{ zk=xz2oRJ<%h3>9^tUt9X5Z-C^ZC;8x{%El0`_Tx%!su2XlNN=!w(?;RfiY-evaY?E zRJqES|B`=GZeu|8^*8_hy9Itkf42MQ>F!-NCuU#rY4QXGX&G&(5zp`c?BOPxVqlRcs)ru`?_hz~i39_ATiIP4Dd^G>bp+cZ#F##hl64@_-f@BzX z#*cG-2=u!DCj`e8a0dxjq{wQwlhGu|B7X-vlg)7^#vntRgOjkN#P zk2-cIA@7CBv~~$RZ}9fFzaX@=&qsg2UjPsizqUhp2wBSbb%_LT0r(}gG2d7|xQ-nH zA)e0j6+^S+8a5_J%g#d?MZ959DTxyk;99c}AAq0lu5Y%g4=Ff2I-OmG*90lyUUyH7 zPqI=!8rQM-)laC!VWJ}ZYy0IgYtdPu*a=Ij!?3IJN)RAaaLB?V!>`>;zkP z4ybCp^-yY$;gOO9Iq^+d=p3q#zU@y5Q}SlN-_=ixeh|tWV}%V=`N-p z!ov6&?Bii^kT___Xar?qz(*6A1?~Ii5Z5`}qJO!K5u#FR)i_I)IAN{2GfkqagLLX5f2&Rgl0nvi-5i9m$!S#I9|a zxQJ}<^7hM;v!c#2pzya9u@Y>UUSgRIoP>;1+e3T~bt+_pj#U|;a0n1rxoe?t9Krqwo-f==E{?P;KYdoyO822 zv$&1FtrK$3^<2~Ha}*3U-HUJXrkd4N8w05{BfV@PEt5`h?U#F=Q}^v&yK6ll&TSKW zKaRTZ8O)063OXEGen7R0MTO;81xL*{iqyqaDyfXEO;Y&7cm=#kaK7t1ZQ_GTh(6fa_@!D}0%wX_d{{hH*?s zV*RntKGUk`;Zl$v(DK_pV>M4U9yRE>UB0_3VnnayR4}cHN*SUPz)DN--`Imt#wM=% zE8O9GH^5bKvvwz1W8bg z^rTp%bwr6Vi3_Z>5BNfSMS+IxGy}0>l9jw>h1) z+%l;-m?lHX0zbh?J!4CC4pPV4Aaq##usD0nVhuC@w@j#g|C=0jDCegzC3L-i zZ{B-9bq1>J_g^ZSp}0|J(=DVu6nf4V+bV~QV8hneg#*Ns63p=r!T5SOnkZ-`xAV#; zkEh8GS5tB9T|3{hbZdV1tPN3~7>D=0l=K^=$oe^a)t>t5!%O_LK5yx7oo4|AR>aMw z6s8ipom@F}ZHj>-K~kQOS=3{T;a*D`15 z=+(*h^LVQuRIpxp=!swC5#q+hHup*hCVrsn2h`X)7-}$1jO%v~tOKpH z6XO^qr*j!@Pswh5(KlTnF|Pl*zx#nlW+wsfqBf#hJk|6IDw{W;$O5lwe@}WaEqV+( z5AHcUj~+a@dYO@C;5z$ue&^I!Hu^P2)@aGAgv{!V6?#r-apa>-%P9G6;8N?_sC?xt@y6C{69C^luPlyB1@A*{^aSc~WZ4AN>fHo0bCsk14F0$R*@{ah}OZ*RxCQ2PB zWaoK&)1EridoD-UP-EHRwXZ@VMZ_Jx126~-#<0&lXR!$C;I#iu1-*#Dnf>=G%Yp@q}rdMH< zVpXs5uQQJMoC!owBuanbac9H3E}M*#(27L(q?gb$KGQoVoh4@tUYxf1g=A%j8BLLj z(6j-5PQFbJId}-<-?pmlnJ6i6*@lmlCkR;uY*s{7Rdo`?aJDe~gemV|v8rjQe*~1t zMUc26c}^A`%9Ry*VEwQ}abYe}gwx8}jh`{I5IrayV*O-^6=)m#XT?(9P(<=WJttL~ ziKz)t`$uyo^y8O!^)Y8PfE0|>3Ka`PfF|4NqB`2AItx>;KGK3E1?Qyhhnbw4eL$yLG5?P)B7A9 zkGdH86RGVUvHnTa)O$G)qIm;eHvX2PcT4e!F7=!VZo~VK#sIgGRJD>>!#6WOe?8W}jsPv~yaW+|Dy6 zVxejZ#%4>nE~|QAGoeNTERWP|HU^)U#Pfns#uSI}0$V#aXO3C#6uuSgZ@?M8^1<_| z>f@+6cCTUa=h+)W@VkiniT3attrtOaTOR>^;XFT&W$=c3D)1Cdg1B8~VTU2+C4a=W zkWBr)%sN}zetho_&KCZ;!^CCAFu#@#uwL7d>;7laCR%ilc}^>#{8iCYK|wVX&&JQZ z0ChoePyywGgaNLUmKjs|{N=Mj4}#s(=mG57>AcYV4AYlWVeMKqN&pZ-oyCZ7CrbF| zdNFC)wRVX01~I0Zme(Sn_Cy~8D4C4ri=^mb;B(sS6J+p=b44lsO&3?*?s@cl@L0<^ z>JF8ESrKEx?#UQQ`KMrs#Hgc4T94R7&LE$tb#E&snzmQ+EotB(jK1qA;&CE)<#V`8 zTq9Cac=wV_E{d{zzQVa$4!N+v#o#x4#P72~Iig`qP*4qIPlNJNimo5laGdRAcD&?O zhM(HbJbQeb=ivKq6#&H6x05tJsrWfZ2qWJ6hk_T&61_8b%YMYwZM18mq(Ut^iB-Fd zjfoz2mHb`s26*P+{B1GRKH!m}oKj|uM?;;|;>BI&n7I{G!(QKL+~VLAB((lRe?0aS zr-J<*zbAaTi&DfJ@OL*q01M;+q|-7$URe`mz#+b?gwbPP$&n?FPy&~W?*wz_)o>^s zRA7rd2%%m!=azha2`Q*V!eT=FPd0m-%URKXSZ4PmL&F*8>xIjQbI;FSrC@~8uVwdtAqHml6CK4mrjE)hn!F`4}01f=(2V@-^QH1CNxB7l|#6-sq)Z7FWwZIyW zY?mUp^5mITF#;$EYMb#*Qo`x;6yBO3aYR4)McxL(BvpmpXAe@BmVEL;h$f&(3cQh| z>jyP($(R>&8xWt;7kLG6Jt}r6QySwo*R{1q1YF;{NZ_VO?W(S{DjCwJwV{6sMJX+PS9CX{@@ zd4JyErEfC(>}ic7C4+vU7h|5V+dfKCUZH_njc$P^?Nn2%z`xjail(cAN;9k_@Lg{e z19Ig|-En*i=E~K`_A8C}dQj|W)xXC79RM{u0j=ckL?y6AaXK267g;i&kmR4&sp;$`+a+S|?co6TkA3tMpCKr^wJ8axsEL8Gy2&rC0o^J6}@lfMk4Oam=zvnT}*0di7 zx>52v`k+5Siw?t09X#;@=3nWryhW)xALoJgg1=5Iq}+DRo^P(<`ou&zo&$dE;|)y< z0f~5o8spd#?MJuak0)dHRXjs&py0aL8@AS0(7scqM%65Xw4)~*PGWqH(1r0^XZE6n z_pzz;FY1Am;6@{DhfE`%R3Cv0Ido>^2T!!QXTX_H0S0v@Ajt0B7y0D(-)cFoe2nux ztgYY;nK*p0BN$Ij6Z&fUIbA%h^!1C^D+Fy~N$i)T2a9)W4BnKiV##0)05-8*@>Es% zghOO|oUvNYiRc*)l% zI;OfZTF+Yfl`H?8X>{=-Iv^Kr-(9(jBSKQ%0tGUCpA0&YSd;&fJnUN_Lg%lqb@b99 z8Nc#=Ps4pY&5(iEnHZ{y=dj3j7`06~qyAz2<5Gq>)DPUgTctDK4;oCHG9-GY7}NxG z`}N*1AsL%VCY~y2I=pfKG0|bAVaU$c@UP3J?4comL81gUdw=cpgBBQ`^X5zBkEnfh=qyxMlfYk$^v zhYX&z<|p?Z%fA9M3M<)^rPV*ms?eaIc%V~zJ$NFE#>S!IhuXr*-eZX&1)LrKwA?L# z*d0JTZt+>FS{-ur9YmdTuyYV>bsJolusHYqn%eV|7;EaFq{lCVWv+c8lFd@ z@pF3QW%UAX*G_i2AWj4&*0{`=)N+f*|kNoJg z+u8cECF*bvoem=^sO}i9t;5>G0RDV(UPaTmKY}UfZBmwMh#6Pxky#O-Fk)U>;c3*k)YDcb^&2ugFOFH-(+y> z^ezz5w@wh0zS3~e^ekUM>PkMVP@=vS$3A=cOoktd6C9Qv3xjsp?p-L?q(0mbN_yA(mCdzVDWsG5ed5WJ~L6btqByq9f`1HHtCIv_C z?40(P6{=zDxR6J$=F=JyY|`der*rntN0g)4(e>WqVLXQ&kQVDN$m?Zdi@b&ck(FUQ zY~0~f#Kj8Li_h2T=JMxr#5H`vxa5!F2rf}f1912d*6 z5`R^CGqq_wJ|kI%8yPY1dRzBTRV>&$xBo{W{``_qK`IH9{sAjdYKEC9w^~iQ;D=hd z8T&Q+T_*QDcHaa!7XkC&YXm9l|M5KNQUf@1>sx7+L&DfI`^>LKlHOgwrw84&H#+d{ zOi1oEw@JEqy1sqJ&tZZ`o?Hf6r~YG)>Ym4Zwb~;xOGkS4j&LX+x`^6eLc0W;we~mA z=A?Lsr1sNX@ixk>z(qHQq0srkRHIq*V(h%v$hS^Z3SR0vyht_1A{5qcdN41?k{XV( z@Ea#}x5$}iFVHgIkCHl*xW?PE>zn^%2`t9g_@eTjEcu3a?kD#aiLqRqK7i-!o}=!` zrapI^4WmL%J*xjShC7q#QB#=CxTobkL-uPIJHKVTX}W2VIIB*S1;3MqQk1bV48evzZvg9RNCV#jbZ&}c&U?0PW zxlX%Z^`h+K%5zD4^(nP0?-siKnp3k{CUt7_Z8VQQ;)xY-Qg0B@W?r%4V{3-g+fD_mKs@B?V zPL0pAy2Rwk;Zs4p!6t-%4$U0BB7O1o?S;A6R8MR9$W+B#IgLjPTKGp>UWAX6N3s1p zE&p7IOw~OL3H<6jP#^qHo2T{h>0#doeBYx|#=owKeh3o|IsqJ_Vy9G76-9S1-DcXH zd??b;f~64`2%PWu7>YLoJ_yU<;3Pt!n~VlrAI83GO>U_2j`TZ`B>kA%DRA-w{Aqlf zd~$`^=k>el_0Sr-n5hr^f!|x6Eg3lP&)xUcx+&Mu&1YW;wfMo3u$8W3CC78!K`+sT z^sP;@@{c>tEYr1o)*-agiDS_&qX!uFlfGCz_=vl|)LrUZZYlHdtZRJWyI|Dv!PlFK zbkbCPk+5Rwne^Z^O1Ty3zH8;&t*HHYeK|$2 zIna<1=YfO%1`_ebV!B*xZj9WqWQOBFFmNZjSg*?hRf76Dbbu>%txA3|G2nggr48@I z+;ejxI)upDp34)4L&n6*LxUuN+QL;jo0P*A>hNLCjqY2vM^h7qPj@b&^wwgRs!qQ> zFW)i^@|_b_y#+sF@$wc4HeYKEj-(b|p3;z??;NC(qnlPgimmO#-C&>AxiQdjobA@g zlmsaQokMDUUE92=t~uasdX_5iW__=Ger*W4y0y#h*uK}h^Ui77ZBjLT_)@VS5x%Du zwWuDE=3Beg@i8naI8DLofNIUe7tS6@e7g6Nm7L}$uBk~i&z!&`BbSG~Dt8AyR@cgz zIWTeW*6WM8Yz}q_SAq+hlJOT)F8_l&jW4h5oTU{yy$Z5m(H0#a$d^--n86k8S0xtl z+CpV^uRSaYONbYG#?~ed7fU%mwDQC%$;LatKkM~N0uG;0J1}DuyjlrF6jn}!>X}y9 zs>?01s0R1+gZ9g|AAUPA+4?V8SgteTq4={v&(nP7ATRBq_oTG8=D;&(XX|QXm2mLr z!|un4A{kEt;2NdPN>ft!Io%ektzC(H1c^B-}WsdxY3)hiWEGvYj}e_^&j*7Q!J

;QT>6afcN@=UXb1 z$x~lHloG7LElMx`$od~cyYz@`WeYP4Ra*pHbw~zDU+KiPB-v8(b$POoJRJDGIN9Ks zg=6l?^XmHHC1#p4B5z`YtrSFeRb9&*-iVGl)u*`L@7^73eMf`W3@DIvoXI7;!3Tpy zVhp9|;&_TW1G}0xXi_{c9gRKbPKcDB7KCi>^8F|C6uXsTVJQ zo{6iGhm%t}vQ2P7t|A~SMvI1dStC=9e_p7~ zH@kvhwY=$OFFH6Tr(&$q@e!Va!AE!JonH2l(GiZZrgeeLUYJ|^xQtJIlFaiJnEyg^ z0~H`?RbejF#bI6`rTdL19wgAj;J~~qiap7|L==0Y2PyJf2Nk>;UmvM%Ihm5BcClIJ zaZFTvXs9I?1T{T|3`m&sYD}I$GxSuqlzA=-#ztTG+zU#V#IMGIql$nr5LntP~hBxcp(v zJxZXg9bpD_Q-h_{I-XeQR#S%YFzJ+?rg(}eA5V5@9|VNR_A46(h5DQ9WgSz5U(|qO zfw8~u`#EP=UNF+qLMxl;(KX;&=slni1nA_)Ahz$OGvllU#k@zpxs5-D_B+5dsz+Xq zR`p*+&ww(MR*!iI&*o}nR=v^cHyUY!=Z)_26_F3Pgj+mi^le_En$Pg=p!TJfX5_e9 z$i`Dte6EG{tB=KIaH_37ZC5hd#rNvPTaUd+W3>Zyz5ZJM3AvXol%kKvX2~e5B@nJy z9>SOks2=L{3*+-KeMK8QV$F!D+V`1>!AUybsDGoa9sCMd}-Yc zbNC9Xy2nS-_H^y@I8+j|Vja;nAe2(Fl#h2uf3X?PIA{lb#ykN_dS%FPpqHJx@8N`l zrXD?%m&$#=cbc!FamYvuR~{$-o;9?-ocLXm*wy$!Ui?%1dTr%ROginsMU6zLuQE#X zYP^~795RtsT6xk^`l$YCb6zkr$Y#Y_86D)LPm2Fd6Va>pNCPpHI0X2lR{VfAo#0HJ zi~@XJ;Jx{X5yXXdj`)oczfU3z`0!gLcg58Lw+wRRc{|kWivs-i);w95qhz@KYZ zs!6ZHKMOkX(2f$hz*ZH4+S3+y4j!=j@QI7KPd+(zpa0>W^_~vp0~lc(v^sr-=4G!P z?{t*Rb<0CRk)${NBh~5dY|givEHgH;E+hA`rGI7-4=TDHe3n<08&jE#|E66nVd1S> z8$V>867%F^`kpSv5)KaraXNiLo0J~5D;0|vyOo0zN?DBuo~g1cX$q=BIHA6KA6<(- z=@i0zzbA>8EdnDAW&zc8ndXs;l(P|Z`0Bp=^Uh+Olvio|(& zux?==Ou+Y98Vlx15GReinDj=zrL>}g#)lr%8WXGsAqY*0c<5v-CGx8I;X3VrGAt!9 zQ5A$bQfXMBYr8$rD-KHqXPjAb^}yq)*VsVt(<8!q_vhU!R0o4CTE?g*ZU%ldXLULD zO#?1QdI|rr7Jc&JJZGeMVCwy|De%SS;6iidcXkfZATn`_^isS)TSpZSm7j0T(LC13 zJXU;YmTrix!fc3tZ)5E|GZRz{npA65+MZTuFf@!G!!(okU=jrwjFm5@u_uVDv_zjw zRIhqo5}xS_&<(bZ<40*8Aq=6GR}&0&E$wk;5)yTZPGLC3)-*_M|3qBnB|ClmHRH}{ z1yl#rEHY-1^SNY@_JL#6(_Cx?$F5)WzcD6z?c!sZ(5@?;SBs8!)TzIeo%xVa%7AD?A8|; zJAeD2LMzWJk=^1Ud~D@zL;RZDJ-e5roX@v>&PWtt?j(v)b04&_xrquwJrCbML7GSn zeHqD4yT)2_IAl~=Cy!}P*tZ!(bb6!pS7qrtt3yUjX>8ue8sJ$sc@jTFYT8uN`SFFC zxO;M^Jf2p*IBI(DNi2FjXmxrJLYo>IRql&tLYom6W7#^J@cnoO{E^^%?RS@m9K<8= z&{_jc5*-I!ACrB&c*sW^38En}98U_)f6^~RRH;QI%hA=PQi6V|4bWP{=dzPdwMb$= zCg9M!`&WLEI;k%kpW|N{e1b3XNsrYX=|w1BMk3KHGWCL2lE=~lbO?HPH8`+&)OB83 zs|U5_OjkW{{m|{B>eANB8t`Dpu*c7s@!@EhpU8h~<7@A_nlY6z)c8&5jo%mL>a0dR! z4$ZQcRY|k+(aD7wp8)GvzSCv_{%yf^=R@607MN2s@Qm{F9EYvciP~)CvxQ!_?JUBl zS^Sg0^BBu3L1}46O6`SXx^SB=L*12PZFM%gr*)ij?n>%`jV}^g9DlSkyo7F3p@MD@ zd(8Pl?m-RVudh;V*lJ>Hjc?|I$r6vS_9_zW_mkDbVja-JL3IVBvO@o}{waqS(jU?&_S>IiV6FBE9f7+Bgg)T%ynyCwniXTq8HanDo!H<6KIHcw@q>Bd zFFz&=w%i*xWy`;8fuj!g_S}ej^6^$PRo#*X-~gn=BgE?#W~!UD2EZA8cy_fYB6#@jq**t7Q8_ zv+Tqp_7Pnss|P3OM6a$nq5NkcdekAjO(_K{P%CIB=xD&YZuS3R>dM2Re!KT9X2`yi zWF3^ftjUrs#u^c_O-jm=EnBvk2$MvkkTp`WWX--!nHJev?CXRmyTr&?e$Vv%zVG|H zE`M}Ym-)VE~A6d{cZOi`DdKp}})F7f%0rB@$1bb@~TNJeAw?Gm!R) zoJZobYLesY!<}yrV_Gx^r0v&Ek;5Vs4XiJ6eVBiJuO9y<5%f$-saPGj8Wc25!Cv+P zj+veje>(r3b$86;*vGvRLLb7!FCjt5RKM`7dF^o?0EtRV5`KC!$;q#>wE`v#DJld# z6X{1Uai!R|pyQViHl_e%H1o$2WAOBS$;L+wIN5vdUg!+FCxtKQBz*h{!PsKhFrbXI z);i1(0d7|BeC`iVa*`(B93sN;HT|Wc#6u({CeVX;5hbXrJT^|BK>&Gw_|UeBqH<6Q z!BJ>iri%4X7s+8EDV#^UyOr@`iWU?9j2Px3$ePPZh;l@l+0nw;vydU^d$@;WueI+5 z@s(!ZTy-OdC8ll>#x~Q<=XdPqHF5S7n;{i0^QkhQ8oy@P0VMc&Q(t}YJg<1B_zi&b z0ImB?r=ahiv090xVh`xYvowsQ|Jzg%=(C^t~j%Blthw zCEM&jy~%fIbSz6LLjA$9C$lxu!oi%t4})l_(^{Qj`}ik?Aa<1&ZBWfnt>K&=$Fn-{ zk+=E43E-W93G^^7zyu48J-BAvOROCzN*7)ui4|B{n0C8?<~7jLaCsD~g>z8Fjh?X< zG$}(H;QYybB;(_eLh5|0Tp_m6pyBRLb_C}Dr%20Yg!A-2C>V5lCboQ5Syz$QCLpTF-+NF96 zrtiUSKn6{Y(t1b>&yqR%N{@}RF0r)aY8abLcK=)IJ^a#MJ(C-px5(tJr~h8{4BYWIHFlJAE=vJc8dt5 zG;*pl4J+uzbyQa!PQ+4Yglt6F>> zzBZUe@l^aqT8}rzr1$;rH*ErF>+nGQ8w0L{o+&9SaLHLVM{dz?K_?YLmvo8ih-A>^ zvpMSvttevsY;K!JZ%Qcn`INY4f79<0FSJ~2&;^A~`pI2jukUnY{y$oPG`2s|wMqCt zSuC8V)vy`ua%ma~{X|}I#)kQy7svwuK5b5W!G_@aAf-xQgZSwNgU)`S47?q2(VD)v zAqQtvsatTj=EPb!O~+>)^M@74r(Sm83o^7T#v@m3e+W>dU$h>Yjjs4ro!zHU8^T{T zcqxXIJ9c#YXgS{|>6XEU4$xDT3xDpvbp`wa1#jeTYUq^zlpTdd$}?_6q-q|??v>8zadPGJZ=|1b7h|7m4$GT?Tm&}|yi1>t zh(gz@-5;iSzT_@myZ~FFW3@P1sHY9vRS!zIE1<`%(af5dQye-5<11nnRY^HO+_yYh zU=abNL8@avq$$!j^FY8a&|F=9EeqKG;!(z3_9-2&OCAMQZteZL(z6h^R-Z@U4{i=* zdeA}E`UHzOvVElT;a-mADX+oEGjMW8nRA<~JP>FXinYz?g25-zT{3+WAL_A>!+m`Q%NP?AE9iA6v@)$n@@bfLu?pgdT0=ON8|Rf42fsT zknK~SUlc^s{^c+xv0tHvi7iFw+uyI`zS(D=>>po}>SvK)TF z{4XSy;2vRDi&(8-(Xe;0WT2B!l|KXSv@|=F6dVUi)@$SdjLU827;FvaJ(*(vm+3&L zDeBYV0Hk`CPkvU8R3XO`sn=(?7gzw~^upec3k&lCR4uvq^|chO{#Ut|3=1!0wX)hv zsFfRXMirk}f@Y^`#L{^)2tEYL0Tqrr@^TGwJicAoebIdW3NF?vkxceFK@7!WvgGl= zBKQ&TbWn5RI`Jj(BcQ|}Q{r0!rXSx&J)|~{Qn1m`KKKFM3Vek=7b(fnCs`CK)F{(- zo8=qdyH-Z6c^AY>nCzns{-N23s6*3#noN)T%6%^;DW&wm**>fCg>i=BGZMSeY2|@= zq&*g<8c9F<1$Lfbp-;32U}A;s<0$#Jo*v$o@Mp0*8;GaOrsFq3DNb36=3M=D@VrHZ z83`MLEdtcd{DY`OFR-V8=Jm1Ua03H>riQWJM#A3JX7<2qJ@4ZK z{u-EovK>KTT*-tb`vUSh1yYB&aXi!sope0e{BCsLw`@=)jXDw^6B|h}x04o*V(iE_ zFaxl6j)-9x=I@~HCsuIIr=Y%johP6C=$P%#yC$~As2Foh zP>Uzy@!k&R34)(a7R_z$3}YHBKJk)C@V=q$Gch^jz22~UUV+-}Hpm*>5KayMXNsOQ z?kn_}J74F-lvkvWC}Y1YLCjmNmi?`*8uGaWDDa~;#iItjjW4e{FLnEYni#N{FUGEZ zR<+D9A8R_dsCfxu<=4Eo$d0kUTREW|do8nCV!ak!k|T#@I%bqwOTo26DUd!$-_`ey z&f7I`{N8q{d&LnBil5OO$Ez$0sWzb_Nyph{{44at#|p3~oC0kpFVF)pN1o5J(3}a8 z`s`F3r2Xp_-@Z3f<;fF#4lMS`*Tia_D(UqT}|)a8QMuS1dSItMgN zVvWf9rQDE5H;&Xa9vgifNYeUx$056JGe!m5gX>9Hee{r5bZ!#R7(pfFF*vRB@gjp? zg+AP$ufEK=mbYY|l26i($tNBwP&K4pwk@fm0K21W{WU9t7T6E9(hJh<0v!6^SV7?} zKX@h7?^LDj>&tn_dlEvhM};l2@OwZ`95P1}G&t(Q4T}a`A`~irHjwi zKgIIk=U&TL-G6?}3+5^{=zw}O=3T(v4>amp1){_=7)2|QAoWk#(io`=^cE6~4T~MB zbmb_4(=Q^fLKWCIVg_fz+lSKM z!`4o_$tgT~k4L_XhORuFPdZlwO}=7ODmg81`3Oe%?sdt*2Up1_3qCLE<4<1u2KZp< zQO)^{c9`bl4SrAW4xG-FisBaIZo?a50xFc>ZQsnMr@D3zu3nH37+pDJCNiPXIYLsI zO~St$deifKxsA#_y3$wxbHFiZ}vWv#^do z{?s}xYPEM7z`u-XY-@S3A6%^yDK~w8uJ_bkTG8YICl~gMa^7Mb&hZ?OM8ElOYii+b zDG6?@17Vf%8&3j*5{rz2=dEWPw2L;hbH4(y|H8|~glUulKyN)u&SZ{9gR#OWn0I1=xdM#B0;vc6#t{_KznagT?Rq z8Bd%@K6Yb(YcJupeXVNhv0aC=U#;oS=`YYb=_)@&|3nv@nHeBn{#|y;!bsBNz3V)6 zSDI*#kty(-$q+~?^2|hN8gsATkW<}f|6Cb(X@~1h%LS3l2i|A9r!IVyxIQGuB515^ zVs;&=mB+*^mM6A#9{=aeqlgp@EU)zH?FnCwp)~79-KaKSVaQ6Ki?C>3LHbwx?R+L_ z0gZdt?$sWx)T-q8$rvoG&ux>NeJuhltje=ZUuCp+VtVxVi2~8FH@~GA-;|$qV)8!Z z&BELxah)P~XqT};4HUAcVnd_6&Mb()+>YMd4}0^Jv>7IJxmMMA=|cgjCYUQB?9Hu0 znb2JoGK@T7FjK#S&2{Z!gYWb!nwEyVa0x6cd8yT(#p^b4+VE?ANCM@XEBnU1mTvJm zxn$@(Ct_SD^{DB0^&3WU5Ic^ci^b7#DjJ{)%$;qWyXDn8Z+F=X7G+V!$L*u>S$R>6 z9Q-TCNGIzkA3IArE1jhEZseCX(ykXA1GLMF9ap(1BaW-h^}R-uOKhrKA*Tx-F z+)#j^hsFs?D~~#^!SxnioFYI+iw`Vk5C|Y*6^4SM&EJ?h<;@%G$(=nx|5|k^<;~(Ro}~fO>_% znB=KEomIVe)^cXZ71(}S>!f9vKt~JHc-m1Z_HZYP6l3c>dG#)K@)2(861Gq^5KB!^ z8P7_{v(wshP>H#=uyt)~s)#G-!8?vS);ju4dc~1gVXnZ3Vfd2|JMT0IWsG_er^b%5 zarX5;dOb$!i_OAQMP%5Mv{+FqJ>1v*$z4P=BKdT?c=b~$ArXPp{!xu>%?C~MH!J<7|`?)}wlc z*Ug8?O)FYqx{VswYBCz=irRt}w#@spXiddXe8`I`-Cq_GiGRRwPtNJb!--!WsvBwX z!vI`x%6+LJ9yf@YDxg5RC891&87r%3AKq5>x6D80Dzc^HWJS(cXtji9@hT5`7s$U2 z!M$e`s8KqfpbB@gYN$mA6qB&h%^4l zb|z=cV3QaHEv;udpQ5L0?0=vM`#RwvLiInLw)}UMvJQ`BykXWKTyiM*VEsdP#Kf|b zkfO&_Y-4iQxbUIWBNHpp<9d1o=U_$&(=DH+uI`M;mHFhE-1M=G?=IytPCBbYi%~3J z{CCF#`-1k$H+FO0+YOxHQI8A#F^DBi{!xCb$xs&t)dIdUiH-V!G*%(+~K$TgCiD-qNFb zJI@H5vO$e}2qO zY@OP2+}{^tIXz$54#jW$qVw0f1fT935zM5wkc7J=R zPSQ`~dtYxD`*5QAqM!fkR!d}>LPODxueyIyF+(&;n{7p31;**2mfEmNF`T{#c&EsZ zHSY2>`F3PA&pPw@F-=x)U8iPMkt#Ok{|vJB8#t3hr^GsJwH+`D=;c%n!nVDhIhwF@ z*>alvE1xJMQSr<|2x%erBv}*>E}zRQ^5M`{oAe+Pg0j>Q=TJhLXxzYlP3-;{`;hKk zp4ds3;))xfbWdBXeFAu;tISIBLgLn`O^ zI95NOhR-lAG(tF@NFE7Q(}B#J8d6Y?bMa)~?<-`g!HbDn z#FiFas3UG~bS{rr1nMm+Y{+UUl5!f$#)UH&;vVK3Pc$~)a1o1rd5jO{#Wf`RoM*UR zZ3R{*-fPRlb3>EBFs8CrBg6{1V7#(^tP;MaOP-u{$@y(b4kQI-=HQ7|eHq|HZHJ7> z>91C6KG>2}zP6{grf5k2!kzw?2avYrt5oxoskDaf1tYYX%%q>B7jAVjpd%#npQF%w zmoiKbj_PZw6HbNx%ptOC8i0Y1(Z#=^J#GhzsjJAf@adhipF8&F<~-eo*VajE`C?0S zvUDxh=XB84xxVmHp5axfoAKm##MV+Qn&{iyo+dFFNRiOP+4wjU^AhqlvrJsNWb4@} z)%Q`zo2>Fr<}VPhl^ijVR(UAC54=A$rf11t5>*`lNd47mt%%rDhM z#xk%*H)36^$95ALj5Jdjq|0t^B)op%ie#1Qt>?MKL!Kd8GA$I@^`Oe{jO#K#7BXs? zYiGVk@3OGI+LlipxW~RpvAoXj*EGFI^3m>{7fn)1?B3%=D&{jd5(xN*@6kO1Flk)ojGRZ2)5|NLMO62S?td}hR>`B%vW!EhI zBtIgGuP1vw3=lM-d&&3BZMvqT;r7nv5S++M7+GH0t?6VPeAGY>>M` z;n*@Ujd;mGF_dG-R27~E7drkV-NulOV`%cga9m+2?B&KCp@T^`l0$~5!8S>Z_=VK? zNTT`RR^v)V_k8!f`w8-uyNBL}BGeNY=*pT>ab4K2xFn_@^*oh2lkG2>@@HKwo9;63 zmu;987n8zbYVcmEc@XUw4>O>)D4A;}&3~@@0kc4r_gKh;$z|h^$(1{9N&^un_1JDnxxjjJTq{eSV>^3-3p2Ki5+Uz{QF5yBNtLz>S~B2blcJa}BX zY8aOZkv*HwXLme{)ZngiFNTt_eH=ppix}kE(dnr74#YQLYmnHM_LRlS*PG77KR`t2 zXbQe|w@CTSltFj*;oNawMgIfx>CVVP=?tWkzSH9F>2-%+-&raACUob^3|iuQn+6+s z|KvF&i|BP%Ynf`AM(aacHNHd16C|xWIZQu7nAZjNZJ@3FI%Dw5T#I@15gAtzb(&{e zdIjktvKbyb<&!o6b$HzS(30bp7~}7gYbVIbz%y6wYNP-s`CIY2H?nN|>$Ym!pA$we zD~aV_%|b4Np_VI#%%?*G|H=OgE^d&^k8Dpo0fK~)9y$wAB+II~N<8be|bC=v6zrV;ik?K5*P*X(A0#hlnx^@ADBdJTc@DY+kNV`<91z)_}k7t&NgMRZ$ln z*H_Qi?<*S>)$>bvjmyn=ZF-`8pTFTw_PLM6q}IFb5FNTL*dX0ms3Vx5ACmlyXvk&I z6)>ek*;|xG<}P@Za{P`f^&-Ye$<5QCaJ%#Q>YYLKNWC(x@-h-C~`=w zJn(9>@Nn!c1giJs;|A3*J!tD$t0^y8pDI_ks9+zGv4d*g&eI3S_VE>i zf1WbcCBSz4Y{WZ|*5Ut4>qsMLAKxbr$Cef8<3?wVr)Og`LG})f7K>)D{+q~qu0jru zhjVOvi-{x@mNYZl3xoX#m`<;siJ*g%S>pqjwLc?_w!Xjjfi3Vhe@dn%YE{y6)U6#Ri(#zsi-*~q3|m4bzP@g1 zhqRe6uT8`<#Cm&Se=Xe?hn-Mr<+E3AjPFO~i6FJEW+Imi=-v*Fyd`O)sG0{~Nb}~K zNgh0NJs=`0+5Zfgw#=1Sgez3)2PDf+yBvh1Qm-^WZjj%(8IX79#Q~*+xgM4UKVTYn z_;qn-T914l@!Nskz6z^t{a^*PzbnhM1kX0NIOgVS4l5UbBXd&kVl;%=^u^fMNYZ9M zl|a3ciO%-WsK&a4bIfPjXkErpPy_q zdTXypR!8!n@1qDNaaPDW>oDPk#%VkQ?DxEPZyPi_&pukQ(%mhxxLAmO3-#vm!fxZn zVX-Ga+7Z7mK4y4mbzqLLK&Xmn$K>j6X`Mb+ba|>*|$im7;z@jix{A&tn4_f6S;vs=PK6WAqz$FOluyt{X7CEg^Z-k9~B$FMUO0?a;K)d0v1~tAY&o%vaV`$U~i4cY{Swpp-U7 z7Z`rQWfyyE7$X5Z&2>cy{b=7VtJYZTAl~ZKiaHznS+!G+7@yEL;edo1Ne%km{z>#F zdXl2*SBA#6=yOe1zPT6#sd8<@Iqo$m`Pnvh9k=0FJv4)7C({$C+{@>Qd4zDlG^LK~ zN2YIv2k@H6RKW&Vi?6?hh5j{graPzK zEoh|b@h95Fv{) zh3osdD410>e-zjsvrMW{peG0`y1WWcI4JiX`_|ahmSMqc;&^nJk8&lmhKlo*8jQ9B z=Wv`C+V*gKCWJ)(C8>ef)(uEbhRR4#UdZ%G|4tri%2GIus=A)9B0Yp5ugwY#LGsWm zR%xbK1xd;_mP^YAlaVZ_c(}I@2=e zKAIKkl6GIBu}#p zXQ)R%r_=9p`Onq_9&q)k>C_qo$uMa2Hj$CE*5-~qK`byhHf4;-UulnmuVZ#Xr!iYs z$P#YqqoHosI-hE_UFF@L`h<-6=z=Z$l7yJ-pBcJ?wIVW1*G5#Ww7c+w>A)|0DtGuX zN-?1?qxeG$sf#0e2TUmUCWXfRg3e7^`M9!G&8NslfpdLy0kkWTiTIk7GE*LqPME%i z5uO(Cj;rU=H;QykFWgbT-C&vi^9T{qsRc%kupL<8&)vG{DJf*O{9~JFp6Ql>%jnH# zbplyV2mYB#DA$sjJdW2ghrhXb!A!{`>ep{P=ePu)lE`JbR^keEgf~lV@#`@=YAIJ4 zo!{kAPuR`x^Nd^N(orx)^+z4?*YdU214pRMDw}J6U`0NU-z+7cbNdlF82C;frvRZx z4OEEtNi*N`;K@{=W-jV?ZVt$=!Y^2s9h|I`>S97AJK zXOt@H_xQI3PsF1-P7+Iqt>A$znh&X0QioKjAjh4@>PS+KeU$;yk3}4UaLb#{8|w7f z#x+uDQ7E{mhprd>!qFogzQ|6|qjZGPW{q=xvHqG@PrnXZLzC}S^L@D)_n+lQo~hB$ zde{-vmi=P}_QQF@`U@5ASeEAYLFAwajFUd3q3;S=horw$4^<03(5UK&L>1$C5ra}f zC!Lc+UwElZ^!WN9 zme9)|T|NkoTW!HmXqY^73;FI*%Y8FTs0T*}y%Mvy(1&*U;x<%qO&V;=c*UQC=Fzo7 zL8Qu;@g^=t&)12?v#eJgh4nd~SvX2J?gNubZ1&y+jnef)we-*|*t}GK+QJGHqc*D4 z@+XiOOngBSYKFT$eSAqXXN{f1zJK=8;6jTXHV+mr(Jy3?{kdHuzpkII1KTGxn0IIi z(ULLzl<_%xc&uXrW{{Z*F@jl24ODP}Q_KdZ*ymMVlDb5E2EVr7=gkM|*I>*|>*Qtp zDV`9?fr?Z>FRoA?wWb)PV)NirW=mDtaj24=Txgp~w*F&~;Xl@$sC01Kqv98-<{?M? zS$;Q=gyJ#AZnkBQZ74$IP5^JJxeN`b=(pK>p^+e(( z2_Cvy*r?WoDz4y_bV*I7-$%E&xS(62qGB0BdKrxA>=TCTBVII5*%D1j@N+uwV3f;O z3EL@V@@LWx6IWgk_{`C=l1s6y87Zho*QM7CF^-v9H~rFFrG{K(8o`kTYMV3L_Wfe0 zlwuvQp6`LQA-pX%=&^eH`^$2jo6l4~3Jvf11w^7-DNJH?$74MHJR3Tlsy;Q9hBZjY zA25=l*ynDyE!0YK^(B?zq4Tjjcvu`-;CcAd_%v8}z;A}4;d z_JHK>kZXHQwW6^^UR6vly*isMJI%h`}W-zIN-HZP^1GN*+2WYuXwUO>V5_1KR^^ z0_juhi{!?xlY*|pIhjtrT0#Jvq@9BGYk4Wy^vRcW;~C?ZgT=8`hk{)5h1d?sG0U37 zYMCP?H@Bx;P?S$T1G3PF6o!`3+Y94N{ERJ6sf&xm8WMcf1D^i*e#euiKchd5R9eUv zjuA+Uwmdx|tKVL@;}Dz++Gdk6+n%{=?m`=Ro_sj;IgT!oEeI#pYy2G|=}yA0dP zVbl~$cHAeQh%`_W=YUG#YFFFbj!i&ht^k-`@SotAwX~ZRZjt<)|hMU#`^} zZ(zJ&SwcN$?F>%ldfInv#iP~)I<5pIcaMjyDRspkF<1Tx9JKVuS|T?l)Ge8Z)2pw? zvGbID2z0G2wkh&N#&&M`$X>06|EC~kMNa;=Eo|XI9Jq)+m?fOT#YQ(VHB+&uZGS_t zl4s3-`pl)X5T_zJ-B{mcdQHacFMFHDj}uL{^fBqxe4jh~Z9>RzQ)lVg^wF5E-HW*3xk4uqD0`V{Q%gZ zmbolUoFi^m^I2EQlNCscUCh4d5*hSD*#u64Yti-0_)wb0PbfxzA^L2Ij{Lh6gJKK7 zTIi%Yxmr&-f!}4#aYHs7Dv=3`U{Y79dT&AehTmLt6Y%5qW9HBnW$8BrPQr7YhdhDb zY}cojn_D1QC30VPu1QXB^yd*4>>hdW1cYA5AE~Zr)CS!5vf|zG`PFFXc&dGSNt;2p za3_QFMgHL!hPWH4@3Ab`=Dnug@)KTc0j7tpjj_f0*Ts^#%d|6eUHK;&?Tnb zBMq?(l_mOTrk5rSn2`;6+mRzrz?I0_NDe8;yvx^+qW;7w9ZMPk>w3h5q}%Q>g#XU1 zu{jD@Hdc8Mt?Wvv>D%2jRsXP*)lj}VU8c0EG}*_o8c6zBG$w7J|J=v_lrP`T@;82b*F1n#HV z)GtT7JZl`u%3c%1zQ=!i?DLyG0A8w-X5U<*S(?&O!`M@29}o#ZjtjuSy?8uPE!ER* zf`vGO-=v_+oNfEQ^kNM@OeB`k$!6HQE+ocd51_1X&NZIu+!egV=ojL)Cry=Blq=pk zf@MPuTv(OMv!C!u8*c2X1wUJ2rrc! z$c09nE)G7r%crhX6*cE#+6u-Aopri^xa)YApV+y|5{(Hgk!nz@yVGBtp@hf4#Zy)6^4WzH!Jcnx6q%W z6VX#wN;xV9hPd=CP>($FNux@*Vmz%bUcd~{eE3?RmIA?xsL_M$NI9{Pn$aa)wh3bynmrerhySt1)M2V&%Jq@AF( zRMO}e#1u6+wCmoJ?Vwo<vAD_(>|eC@W7&MnReeR=OEH^EglGjN`v6N`k1OlZ)BSD zC~}Sj5iY*9VqVhh2ZCnvH}l#MX=n9`vr_gxlQ(nr#Vx)5P(Zr`<%zUjxcXu8F_8&i zEJkvD*E=nT%d7Mj&Dnn4DqSGMXCt4t2{r)N$+;3I-^h2JZ?(A;p9`x)-hb=yVqR)v z=QSE%yEuK8JXko|fz2U!mWISh3^y-Rlw_^S?Mil)v|Pg=7K*Q+h=QyzhsxOI_bB( zul`q7O;?QSLqA#`?Y#O@sto*t51?WDjAbv^-TMKq}bYMkk0Vm>;$O_$udef5D z6$DvYY&PUvqv?Z-5*xaeMVwwBxChXW^bWbAgpgn>>|1Jxn@{|nv31aLhCNuW+eX^4 z9&3zFT@a`4z65!9^GeR5+OF}JrG0}HmnY`)3GdTeA+=`*wJ&`uY_dKohbT+6p-vIy z(6x(m4&**`Z9LpjiWXKc9U~uUk%kvv3cR>POhebYij*T5p2Iv)gK9U6wN6sRkgBD! zE{jV!-REY$DptCsO~ETr1Cw`m;OG%K>S}VOe*@zn*2@|J3I!G)#uM*Zl*wHupVa4Ujan;%wnW1g;*-ZDO!{lu1Bu+3y?Sb0A_)#7G{-KuAr91t^Eut;)3rH}Y4ON|+ zd1mE*FSJVdz8G6S(-9^xEdKVL#Gs2JuuJg)*kuTKSVV&eh!pa^c}en8{zNJPKyP!I zPCCs3sa>32zS;JUo-%-2)7EFb8z>s4YwjpHVEfktRAd|5MDYSIx?2+{WY@w-nE+MnwAeRj%7POnusvq>3vw8E`nHv85k z!{1+RiA}`o;JG)h{=Y9T6mU|!F>0U{(E!_)`*TUoyoTM6)lX6A%tDeH)DC|7VJkcO zWdOr^<<>NCinD`=eb$!7r8~wfb%2480^bBgLuU^VVr0?0>d|~BX;hpRBRdL#XFlcO z4uSmiHm_kD-Z>N zNT@y`9DHu=`zO_WBE^IiSguKl7K6PO@RHsDV2oPaGlHipL$_Ipvq!W^S!y@d}Kt8mTqZ3#doXOg3QdyT@AfBURy1K9)6|( z%WhI=!?{Q`I`up{O+B4i#i2Ch+=G)M^LLzbg+>TU$=MN9nUQB$R3QQ9n; z7yp7lqZIyjyZH5zPSz{j9WUq+|g~> zi_2?hLwkXjb zq#$N%H@F$|m>2|bcSQhG6WgSZ52K5=eDN}j>JhN_2y6%yme>jhwy}1VWfip965Dyg zq<$Fm5KVy72p|5R(^TtoN}+}-2(O9r*r)5{>(OAP!zJ=18d4Zfd=*cbc(&DfE2*6+^i*;1g!_v7U`Gn+r~oO(OI$In9J&3O=!xGHhPwO(G#fQ% z5UQP`iJV0S1TW$KBy?>S85y9*X(uvwb(`63qml2y zs5@w|Vvzbt^YRq#tA@$gAhb zzY`$Kb3f2PI%?FiRN&N$Z6rV>h&OGrq zABCVoVyqn&2ua9!tR2=cAzqgkdX~LIDh-~tydTtAz^BEx3cFvVO^+&+9wUtBpA3;4 z1jkZRT6#I;``pX3= z^jol>Civ_J>_Z>!z)7j!%oajcqYEhai&DiaNoVY=mn9o;*(oG7_C97U!Z`rpwKq23 z{+``IUc4?+z0xt2q+dP%9f|3SZHh_Q`>t}R)>dxCurRo@DH!4x>9*VyF3=!se=5ID z6#xAX(;DdMF2THEOYq&7*eKvhCH3$GhdZ`g%AiTRRLxg_+YkYR^fVVMRK2L(7?moF zx(L}8LJF|-i;%g=IlzGN9=LF@%ma*X!W5vXc;f3$Qsf;3KFc8y=1X_s+dzE2-BSTJ z9X2S~;t@wLF0t9?2lkOZX7?(f61e@JyyJYGd<2rkdFjsMj_-5dA=5!S z^%9%unbQ-Un-!*i>i{G7IPNys_(iv9G=bX%h|>s#Z?3S0H;Go&6%GY}`%;X4^D~@- zQ0VDO4gd0x!NXIZX_`3ctAZyqWoByqc`%BKO8Irl=Vai%GXs%t$|@v2Iz24>(t;0_ zSL`qcbfnk@wMM=ts?OoH*Th2N(&EeBYTIZQ_@HsgvdfjpYKmAKN;MB;L+6w5%^MbF z6Le}1xCq<4ZjxW`#g1myn;w%yaZHa+x#CA0r^MzGXojnCWZ1q5u8+w&>|POdOLtx< zwfu)$E>5~&Tt3wwBkBu_VrzN2bVr$}hIKY|=};nm2ireviP0M80@c?ys@y3LSe2| zLrd;)JI}kiq`YrtcBsFJ`o%PhtxC4P%|&CDYr}r+QUftxmqtj2Y0s>J)$&zQzaxC;UYHI%zcTylCP59PjbyCpu&_E;89xz zO&;i65xsW@0QUPG{?}`H+8y8Uh8kU)tm~!YxaLJe>tX9{D_^VV_&f zZ(W+q;5PhGh^S4>Ji0P)HHx`AR!beM2`ii-s5 z?=If{Xzn#Gu|g~BDIaFpbo*#cKU23z5E@OHtWNYTMX{51>7PJHzcrVViz+@E2a`b! zw8<2g7oPfSO}c=-;`;zW-5uG7IfsRY8SQIn^w>+ShxXyjpoI~d$WDz}2CUKr>5x+d zPs}n%A%fjwa3V9tSZ}#|%FZ^PW=Jj?-SuKWK*2f}Pr|Odb9#F8`B|wn0Bn?4)S_BN zXg$sg=klsZ62-e`7S!g4zx~3s={b8V?L$t`-8N!w zV1PcwE?RJ)^UiNS?KLNBpW+)A=%Wlz=+;r-@t+RJ{oY>_up?iwzHlnnTo%(-$L0vBK0^iN#ZwPU@Fv}(P&=A2PX3~hh&B~#Vz8Bzsu3y<$@TgSwj5&gaCWw@w-$3=@Bw0+yAB<@NZ+9 zjr)5<(yWl-FO7it2)tViv_f1@x83*;O_T-oHlgFQI99oeCVoL|rP?TT;HDnvF!)IN zF;_|suoy4>binA4^@*mPt!XH|n-ZIo)Xs^in^ws~(jaQ9$C+EK_4?2V3TOfbsy6;( zsIa5^IMOJ=rEf3GhHa!nj@PnWVROij{FJCd(q2EtZ2Yj$j7?%t6=juE$G&!mj3f(@ zkMO4)yH0*5IiNE8B!os`aVw&V>qfZkW&pom+bZU+t${{#Yi znb$xnO#y)ytWXJ5p-5ESn@|7GVAL}Kd!?z1^*uMqx@5y?_8wCb%f0Jb`#BFtxUAR2 zV`5jX58Bz5$5bE$UCvb;Cvp%&aEB3_OQrH;528ER;bPACUaN^sH*&S#z>VSukWaWoWRuU_?@-wlcF=j%cZY-&}8oNTx3f+V8` z?f%eG$b-!&mNpN{AlGcRYxi#Jz^*fLH;LhPaRCSM?1XF0J^CAn8!PLZ=f25+4d~WI z-gp7xVk^Cio>4TnZ2Nk&cC z!7i-q2T6#LuQ%dk>)4P`DR>9dk4@!Dali9Gs`;&**o@`+!1f0kDqP9rcLCap8hFo3 zoKFpRLM}14q>M{_-2)=wXR`MESt;*@AvvZ?S2tNNerL}s-JJt;#kWv%6M7}zd2y$fa_T?yd||keYhE3y5OfE2HJ&Q zJ2e0_QHxq*DWqRVjl1m}pDf+^{iT59lqh#=x5fkO7^IHztP0 zFy7Jv_hGFoY-UO*D|bhjy5O*|eln`qOabORtJ$omk^g-3lB^%M|B5DcL=C;HisUEQ zjHSd(cJ68)HtUa|*)It``OrSPm^AA7{5A0lpkESyTg*{9KRx?DuD(1P3if?}7Blv} zEZM>!*>|!PN<*kgmM|t2AsJ*H%SqV`3_ z-fqN*mhS?aYxf;*cL_8fG9Pg%=+fkfPXeTlv6<&{{$(u-MdcZ60B{e}_+-3rR2Njj z1uKm8BWo{P9NI|=nx!u#vp_0a#lEtgiD~saE zJrf#qBxbi5Du2&~zWhp@A5GRaNq3Jc{I(9Te)iGBJds-aPmxQ$t`#%@I!;NJzGCi+ zLqc#Ng}pwG0LPwX+E1BY>zeRHFQ-Y7f9N?crxWh~21PAu`Ph_ygU@u7Qb^#^P3rK> zBVRxvO9qUT&$lQ&l<>sQ(HamYe2cBO-CkoZlNPAnKc z6C@wG@jRcB3x?h>LNw&a{VB^s2Tmw6$$zcbFMMPTyEwvCHYqiA#)j+|{u$nwi4b8) zJG8}Cx$rtc1F&f}LPhR#xO}`B_r}>=(9Rt#fqb!ooWmfgiRAR|F&<7%$8Tu>y&OjyiPJ ztco+RCVjw&EN%V{HCJ`SgThA{po}5@oVNSlnc*GhZc?i6I8C|u&U9vK!iv}#1@OhL z;LOu?<*;PbKpxL!ev^AcPgE1&t>}eH(TcOd{OJB_WkwqS{e+-#0TB}$%6L1>d<=QU zCRAT+RdCJ6)c!B2@^fJ39|a3)>eYA5Q{!2*NOFPKMP5e82XLE&tV9HF$x?oUvqV%# z+Fv(F<( zlN~Lc1nv?9(S9kVAt3==@e)N{i|K7|`h$42@&e9&=2rgrn-WWTP3Z&dSEmSmV3w-f zZWui6bsN6IK7dd^G4~`pI2GI=yKD9HCUipMaqlAOC*n_Wd;)2V6iTcj0XE<1TMERO zR6;bOPOaYB6_8coOvM#3ShP`POM##9KC!fuOf=5mce_@Jm*TJ3%wLQlKdT3Y16r>@0le>?nXZo#3fRZgq($c!3OuK(V0bFhXSy8Hfe z?pTJA@K*D{@7G0!&=MKxwKvL~B6cVUJEVe6kgi z(^*=-3;3~S0n-=5=kkPT<|Fdq$&4c08s;xFaX0I``ZLEosEAGJjx#+0PziBzL6>~b zdfhc8UKEjxN~5V{F2Dc7k{UtyEBZ~CLYI+XB`>GowVdb zb*merf}mbMDrs&DM@wX9yRQASRR*v2b}wV6nD234XX@D^lo%18xr^`ZO?><)ftX56 z?{vQn{@i%?zP7Wya8LQ3E@^1fXxp&g<9ybo$l5AsvQPj-^335p0!f1F$LWjHa8HQz z0fOvaO_<^S%IzfzkZB&a3+E1MpZ!s=&Mk1H#M7okqd1$-wuGRI55>%vY^XEPxG#Kz zW8$lO%yJZ=;HjZUqhBNGAZ-UPxPH=hrnF0T)okM<;q<#mPfgY)la?*(<*}Ws z)Mu1a*ZoBP?@kNt?PJc4ck-Yw|2CaJ)F|a5V~7}P`Bv@tj>9Xry9KPA0a3};<9@+q zOHw5zm?AdWy9uY%X#)xv?{7Gc9~m}5j|KYJIQ>fDWQ?)y8XZ&9gFW^O~p z{3{+Nh#a=)%JxuEgg&C|2i~LjiWygQ1<`B*j#Ynr7c8r=DYrYaNhyEjCyUk&ezf^~ z1qpOb8_ijbt@jnCg7;jg^~*dOP=455VW0NAAKS}8UX&rjN$lH#mm&wyoS^bM6$ZD5 z-vnKU*eKGFlF7%fv+tziOjRHseS!=JEzBgW+AZJh8(`~KZp7ZR2s3T&bIfGkFOy2*3dJT%OCu5xMInc28jmuvUkXRzrrYq&|=lkvR2ygh*`x<|b>F zbZ)9yX=+W|J*(=GU`i6=PvkvA4ps@cZJgedfa)n|9?S!>faPduG1nEjgQfFK&Vr}f zn&5GU$+3s-aiKUtoJFuqb;mKP_+SY;${w+8$x~OK$$@&bynH!n7L<}#75X1#IXW-@ zy_ex%B6H6Wv1(AhAS_MqkGa-+8{M6{_W5-_wK+3%rwu2E4hfd2mrV+ED19*%q=DgG zIEq3qoj>lirrqj%rnPn~%!9g_v6QsNFLNN=D7{LPVMlKVK$RW9IX*#dqr7+Kc`qY* zvU^)wc?9n@$O{&&Vx}fh#b37hBT^pt>-zzLArIXNaMN|=Dx78v{_WE1eGH8VB-x-Xb{c?^TM`A*N|7&_<&U@G72g@Nmcs9){TL1Bx z4}gLrKIK`^HkSa(f4N`XLlsSSZ5o2 zvZq3bY*VzlQd>p%f?x(i1tlueU>EuF} z4&$Q%L;suoWEcH0Rr(>Q5Rq5d1f&1H@R|GPiYIVuP2cGqkGVN==1h!4s{>d5^>K?u zyQtK^j^BTZVdK7aE3t@!9+?&`PoCL_+@F{q6bJKu0fI)Y z@U)c%w2Tc~0vd|x&Dk!-vrE+y7h}q`<6;!52fN=b`zTtx3ba{xvOvcch=Ven{1i3o zvwjsF+IVVed@3EJyG;PWhPlBfkWMcebB>84=isHB(L50i3&AM6HXC3A#;-jujvf&R zfotRq97SV5mSotnB1bd6N&8ne5&Gc@#er+ZSS+Fj*P8uCQ1;|v+r7WutsVE8mrwFc zz=jHHZ62_cVSFqYCTE{dxSrhs5+7Q?^FC50SQd^7bb`N4Km{+O5Eg@luc)#Xqu5yz zc6;L_jwM?ee(T%=sMh9nHxLAnn* zF*mvI+Lt^JGeWw&{Jn3RGdfR9zrlo|Mvn#nQkpT_1YrW6x!lakfT*LG#~sGpe@=$u zH9qmMg28EW=#`$6UrD>cbkyuCsV}h#`Vi{#p*~!!v2BVHN{iE%_npjO-{u%S-gPMl zm^W~0Pn+713;qY<&LAxl@l^Q(;S)eWSTXD&7B|#sk zxz+S^yn6qaD3B=cCaAmTcK8YkIQ2SzY4L<&)kmvtZV>xPA$}SKi1AdH4Yp`kX}Wt;*(HOLG}1oc34ir* z3M(}D4qxEqek9MLO^wMJGXfOl4xHL|JBui<@<$51aaM|GlBgq!5f7mE<&=JT<+^BG zBsB$zJ)6JiLT3UY$_BCXVCfMI;R13{?5d{bP08a219;ZHe3tk!O+>zFKCW|N-WnVt z4D;=Jmp(0m4lWI(M0f%_DG-tjT}1V0Z8px_nt8}y$L7+qVo@M%V9YpA52HW%_FYyj z*Ae7j3R?fNB_9S@NWPz_!A~S7;@)8K5<&Zqov!iq3Sj>$L6kJ1I4}Xqw1Nih6)tm! zqR+fN4n_5HA2Lrm`(?{g(3D_A6!2SuVKqL0qftEQ&YN@Sj^0Z9mn>G)qOpM)H`@WT z@u^Jp!}%qcDeX}Ur|Q|d=>V&WX?;rd4T3GEHe0KBfg2s#&u0#qQXXB3!r6IIesqKX z@_Pm{xIU$U1;wlhC6N5u*{qljW|2Zu2}Q0!?HNt)78;ss#*x0Vu_T+tyU~8i&^PH^ zQ5vqxj%VrMcRidZPVfEo1RIP-mP+wL2$+uOWLTd1(4vA@-H)@BJBSS%zg93EA*lZ| z9Z3Y+@sR$*7jFKj7ZrbS?%N}0fZd_N@z5cNOwQ#zRSgL|UlGLI$L?;~MRtkju-nwc z8)~_Exi=T?t>(ZZkySPdFW5&Rl3ZP`Mbqs!z}YU~`#&3_3&h}2+a|+d=X1OSc-| z1|75o>hKr};FxQ(D#c@E2R?jlr)6zD4vW-cAq}LgoB|cxTXK!E5 zZF(r=w~t(O1-xW(IUnmFM7sTzYdMuB?JpGHpstAZi+mt<9nZZvmQ2S z0!V(Q{QX95yhCa!TO;XX@}AP&5x7|v3VeTWT#obm62=}sJH@zUlYQV)q>%L zqaeJ1)M>wV+|!LLQ%%>2XF=a`lO3x}Et?is&)`gPPI9!%O7sK39lqM{`D{}&aoJzP zVg_SL?1hKJ@=%Lw8{gbd=RX6GEAoHM#1hv;_Pl99_Qh7TWJ%F?px^r@)!@{4@dnE5 zXx0()fRHG!d0qg^9t@14OG%ZjnFvZ3pc~clr#`Xnp9l$t60QN({6?6awsQuUn}oTD zt%@)lCSRv73TqHf%=vucpu<6lopNJ92_0aNB6iL2!~V4ip|)GB%~dei!al-Jt9!o%mROdZ0mD>%bEA(zx_{Xg-=?A5YeR0H(=<0s6 z^Q1UPD=Z8?0P{vVv72#Af(JxbwsXZTHVt^5HwzSZmZI7ANF%ziK~kcQERqef&2oeF z*Md*B7ZOZ#Ab71Mh(1WYmyo}lfuM-d&wrDub85T;%K#KSEk+b(UVlR(l2ka0>Q8s< zXrM|zNmYN+%x-8KX_!B?3FdA181Ox>+B*{t!F%Up$=Ly>bX|HZ&Ky@YY_CANVi;>EJ`a19iJ*ru5P(g?7b0&8Eo)#fv6EZ@-X-xlf&c zk>;|I4bI>S4_>t5vc-J5u%Atd&)Du6d};m98Pw4cbg8?P9~*~QQKs2qbe5qb;IZ(5 z>^EoI6Z74w`O9rFZcSWsobEiJMQYb1wz{X%VMFFuALQOI~+fNm9foLWH8N?V~*#o8T-eE^S-@SR2@1vz-Vb|M1k(-pl6Y zdSb%Ygq5xpg0P2R%fQ}xZ3dC#>~bT>Zf&~&ehkWY=HGj6 zIpa0yj>tlt#0A%5ML}y>19jS)nF(0fvQ4#a@QTA;MO} zp$bX7HM_qJrgkG_N0ize>RAE+Qg9YobY=h)HXMD}6JHY3qhO8-((yVz0;JNkD}ceg zv~Uh*Z89Q@F1&ykZss#db9C#jJ_GC~h#s29T_1g~ZjwffXPgZGc|CjV!0g5!U4{)N z06q3anSLmEygs@LJcRs3+9@?-ZqKJULTVKSR>eUGtyZ{qU?B2`IGNc#}(sc zlV;MA5K8J0AnJu&J|5EZE=6uCVDm4KE{SBbuWB)phPflTr>_B4zQw^lTGS`E+W;BFFaWRUFbi8c~u6e{zVa2!u@HIC`fdh zDdNBUvZsle#4Nxki4h4+Elg^*LoUi3=zBs*+VT|W{bn}0Q*H6$=<*1Qw^qC?OFxcV z0P~bQOLPO*0Jt2R?fDA-i+x=QmaA}{KU%cc$)<$Bt#!!(Smb-nm(p#~7u9&W`oCoF zyt&sx5_9PB8DhmN@;dP=ktsT3ATlIfR}8GDx+~@I(s=bqXc8n@iAAT|4URZTJRrcW zt+xVmG9IUsPbEkMu0NjzAJiX~sqPb?OC>G$!nesTp5#BRFD-B)=RUW?Us=Q(1IsaRKl6gF1 z3s7Ik+k=|8Uj@UpsQxn{FQ7aQrE!P70+#j5zlU0T1;q0y26F!1=JDQcu$hq#KRLCv ztVVpz_2-mvy=buyN;3{K-&M62qro`qVVAa?WorH_s{q}}r3dyvQSJyaCTaSrF?^@A z;wIDihjMjyLL$N1Z4mnwWdhA{BnW~?^M6NQV_qSFMET(|8X?Z z!4V0CC+Hn?OS)%8EQj@p&J>2ZDpjtiQ50C*^BsRQuAyMqKnV=|;)1aH>xt9vzg9PC z$H!#amX7ge*pPM>phN#(E(=gMCV>$c>#k&qvw<(bw^IXbM zbU9SH;>(h!2s2X(8hQoWdS=u*Q3GbCl{Mc+jqX6NusflPniVyhS`)Sh8_=QF=Q*+p zNR1M8vMMwei=;TbbcVbY6H@*OrY#| zrbV$J`@Q8tHzCD<@}oPw+M$jgZkpEVA9;jFs#jCKA{aAY^N!G5LKP{WFFl9|8A583KA zMe4ML8=y|J$GY$CgW><7_|A9We2RR?G0#Xfzw$Nmj2WWoo@3*~(M%U(9mr`+iy4pc z{4pJX>FMs(hi*f7RA`G>+-dq}i!D@tmIJ>y}wawFdzW4#$BAIMX63&Hb8^p zpn)Bx- zC+E`_w{K7EuMB&*mh}RwFTDz5V7%@g1=o#TEfOzZVKD#xK-4Wj8$_Yi2A}e>9{*u| z?4A#j+KGEf$p_1ij-6{MXmC&fV|7qu@#g#tc6h>{VMrmL{(c~E&*dO($p|cR9@yOn z1Em0;PKEN}y?+*XnWASn83=z^-^cZy2ko9@*oi`a2C57x&}oT!zH~rTq)O*kj{SQc z^Qiqm5av5sV^^{vgg=T20C1w&7m$JbiI#s)|G5u$0yA%?9G=@^S7Toq1N1)VvDxEt z#J{e$ld`y$F~+0ZCb^D;Gr0VM;ed>K(R$U&8k#Ox>%j}-8$Fws{hlL;dZ~Wq5ECH! znJKU)eifS4VEdymkePi+tb!k}P;ugWHP4$^>SZZ_dc6t`q%@8SuEy~OG@1YLo?#(E8c?{wPHur<3t(t6= zkUYlXf9K#ReG#aFVEXnYV@^x3S;hWXzB*Ze-z7En(e%1Ze4EAhlG6F zKs_nOc){>jk}ZHR?7meuM0LMTs-CcnKK7H>Emd?pEC}PF-2O4C+8P3Il+0}JQ^0+j z@b{AHXbhznB*@@7hnp-;!0zfbjLlQ@!AnnS>4SeQXv=GA7q0+WNn^JyMFm}L+J&lS z$=hsHr0%WmV?gO-aN5(Wp}^GCK=bbf%yvfzWpVc#S>JVV?r1POPoTrn0>Wm-jL+ZE zz%VzsEm#FIagb))ttVF*Hn8T5{zMJn>B@H87%gb<(`RxXl$||mHXCDD-3!LRtGFKM zmTIu{OB;8YRX!|sAPDoEr5+V|*&0jDhD^Dg083m=!SK%G(JF~z&gg|Q?9u^zIA-3J zc!%s)+rJ^~?+GM@Gk=eejD=@?=I<0f{e1s{YbEx3=!xWJ(#L9GnMpp>e$Q%m=l}zr z;iIQgY$(2z)rQVfqVIVqTBsoUOTO+*E<?m+kaO8LtY0&(|7!sq& zx?}2;W@@AS=N{YAX0sNbO>d9`^!z&O(R^UZC&v2#ZlsIv@;yg_2|gARpxk*esrr%1 z(<=ttcp-)g?!x!#6Um*#)4OFrXv+WAz56>ATC|raOsovee4?%o58cv$fkkr?C^y%fN#$@Nk1IonGDa(gO;B z34$~Z=<5Tj1S*2OVkXq?>Ar|PSurTIYHx9x7aGg6dAn}nP#`In39nV?m*$%TH&cK& ztj4x<5o^`_#y@oVudcjhnf$RYc8~sWdhw@ziK2S~wv?)Y!ay#FppGN^sJ&Zv2e5B- zqoh`0f`0%zvcc*!J4+nM`h22ANE;$u_PX5VP!@L%JvxKPI%4R`e!lVJkAj94NA-UW z!O$MVJHh55Zv-?@xu?Kp&^`vUhEGG29kHEho6v-Yf4;0Z+yia`7>Ih4pW{&%K|Rp$ z`ZT2tv7u(qrf?`T-*sibMQIhgcZyx9>3IHJy9*4Cr2$>i@GH#OR~c^+v^k zE2$pDbLi?odDv5w@7bJVJiQvjJ0e1)wHl2O`}VQdZ;!*ps2)ui?H`5BVZ$cZD`eUU)svV%`Ic0V(1q8@UAM#9tX#c}4qso6KQ17=Z-f9`bM^f}7#4T9gKh&*ghO)<@U$)moIbVqo$_Vtyd>K%(w znfp;my<`2!Sa=V*G*6swNJJu)!2f!U&F_hluz%}47q^mg(sWfh7ktBe|DV@8h+F@} z$Xt!Ce^wMcAvQ=-Njis{739BY&XaR^A-V8Vff#v+ryQ`#adv(N@%9QAhj|Pi1#jHY z0)|_Q+{>wHfnvb?nX>|%KOP#;-TxaUOH=_@NDcQ4ST0tZFqXhI+A#VRxCgvp_MaIK zp^$D6V%b+OeaT%9EPt^$sqm%9`QtNd*lc z$30`bg^1894wFGj0n|bCP%^2TeCWHR+=22mL93IUlL*30TmaXmluO~{zB1^7Etm&^ zA>=IZ*&7Jg_zRo1JQ@=_xpc~}q$_;X88)8f(#osNR0W=AKvnq?q`+n%^*uOmN_nvr zH!nh%%sUOv4_V{Ue}bRiUs_b?!wEP+kUh&AIu871+{br%kL!tpDRP}q0onF$JW+ip zsPQF(>wJvVkhEbn020L1u`aBfj5XyLxI4EAu#<|zrGQ!b#6AZU zJf-fDYM9ziE~f{%VnYYg!^H4$ZLu+c6Mu(mg(jh|cr&!zwQjK!+#yfz7eu#{78M7~ z^($+#feQL#%r(Wg4H*Wvj|Z-3oLsbqE;E(HxGJ{=9D$y%JTGmgOd!5r81tmUPS^gvd1Y^ zhV{MRx}4=Qaq@h1{}6Rp12vFgBM#VBhTIRH{XTo>rwRSrZpLKvFAm|@{+uEYT*4sw~kHcG9#z4E-vvJ#WGJDBr2B2 zunvEb%VAZ8XsNPx3 zo7?dzsF{7*4h_&ZD=uqo!8D3rJy_gl#`4zDC)ic%hfW* z;rr?#1{9ZZ&D0sR$@Jft;%i<2aIW4@1U;as z)0R9WSDYFKO+jJ#RKMuaVPpX#0@2;^nSbFJju5L2vEW7J-YA-opA8Q|h@ zPv85CTeIB&mI-mf^dTUNGPt&?x9v55u*rnr1Fh+6%QKXry*q3219Mscig4B~-C<@P%Y@U&D|E>j`~ z3U5@Bo#5(hf6k0@gph^eh<<6M-C@tJeD zwd)OL;m&;?fPg+Xrd6?Brb*AC& z8Miz8domVJ5oFN|3c0_4L|Oe`;|kCLTC(w)C$~optE-&UUK(`9v}C5vI z6!ma_uMoUap+CqN;41#YmJ?F$rTZ2jEAST2xG0$AJ!*R5R_MU?^8jfm7UFGR zv=vL2;0*1@c7O-jo;j54U?)$^my>4<-uY1p99b#H%QT8>rR?Xd2h~Qy_tLp=A)x*w zgjcBnXM!bgT=(SIG_{gL?^JUE%8oI5`Qe3gzb6ofOu$aRTFYvRaQ!mb*#_FzXOieq z%eTRr3?{B)6 z+WW@YUjv{w-~aWl0^rcQ=*PKxHwW*$gejP6$^wia?~4+V8^H5^xy{NF_f}OIX7u;0 z8Fxt+`%!md{>F(XF^LSP|D+H*^M~lc^hfVvyrDe`$c((aQI$@2iZf}9D6Ud|6vGo4 zi&`9bE`Oq*;NO&Cgl5?5sxy!&!0u6c73&pK-8uFR(b06buKf1}%+}MN@Gy1|Ss%Z+^G3Lh5$I6V zELnkMFs~2$J&S{P!1HY9f;jMkjKf}chrRbB$9-C4q6F~)KsmdQJ$hf={ojPD*NM!M z8u?d2x)I!hy#Z#905bct4==`Or%F$tu3c+b3%SZs`Rt)uWycsLPk7cg;NIRc8hf%0bJmJ6{DZ4GLEs4b8AQkV#{_T9{+l1>t#z0@&PoH)$hmZv zRQDXi(~WtXzw>2q69Ef^U!8;>ImhNnsLfd<2euJ{3CyM$672f zHeC}x3_kIK|RkJm*6U7zy8`7h!z%3#U4jNMmGzJ3J?I)RL^Y%85Tx zINnao6B>*Jj1bF_KhP0qEz}(1UDB9yBvQmsuD(N?68NLXwdq?fioJDydHZ$@? z3)?(*{h7rzamIzJ^=|q2vqZGCNv5c76?JbYY4IWe3{V?_vLSF58jmEq4$6 z^HGv5vDX=E&DjX=?PnE42cJj;cKsJwcC?r}SK+!_YM+bBl6^q_eR1pdm9NR?$bdD9 zKgcM)X1d5eeuU>5m~9CmjboR=Js=g#hClx=*1&5Zy^7iKw2kD~x-|csavQN>|4tLU zk7@x_VmiA1G30RuKklm6Rt!kW;6WUM7u-8Bl)V&##9nK=Q79iwZ3F;! z9`tJ9j4v??yoX(O+zfTF-77Hs;o!SJ z6T`QiPb{1s7NiWkGnH1z?Jbe=R$QUf(nEXMxIx;`Zw4g%G{8RXEttR(WDz-s$Etu; zMzLmK#D4-WX#Zr%CWQdHn*M)EkihGQ+FitCcXKECClT|=fC$Jo$~(u`b+&GYtxqujPKRLpsU5b%?*zG=+8ozf?Q*srmbua49t=UU|ftGFiBTC1m!O~<~@@ybtE^YKlfl(s8 z<6&Zz7CFrqDaYoO{LSoZ(tgO#Za|_`U=Bft^y{tBPZCYPR8z9k2S158-sWQ+1 z>6FhHeDmeuJUEG8^vCH}Ggi2eumc|_SiRQ*r9?vHd6lwrP(U)bir(3Np|7;yk?RmG z9UAYGc<;gGu|300VuCB$A;EdZq_3%j;{jU8I=Ra2-&7rk;&&-*(&z;(6}*1M$>2Op z3t|rQxHtA&4;7PgF>y9mhj`{-d11|y#HC{Ko)aZ1p|7q|T_2i5t3asXybJd%VfEV_ zogVU~{CBt0P#X8c4|HYfBE7wF>j1d}-mS&AGs!bO04K(HR0(UuUvaOP?xw7NYlX-u z_j*wZDT6=CbX+`I1OYh!g<%!+=WhjXJh5e0eKT%<*^-{`+T$a#F{*neWO@R$4 ze^BbvlzU6Cc+9M5=)6yZi|l{3zef4Ptbx_)^FL0XH?EsaV!Lj=i+Onm(!w=*bdIy} zXO~BfGtnQo-5++Z{iCpQa$}C$By;pU2g7YxX0uZIm-?%>-^|M41O?{30(AYo2*tA_ z9VFvKeB&H$SWyBQEl?+I$R6Q5_PxN6E#ff{Iwn^9rSFzoXP{kjxV*VY;=w^ZyOj`T zJb?Aw_LVv(h5J{jakwK@-5xtXT@?6o}8^5g19(1MuB z%8L*${&uh4pU6v!69rwpsv7tewt=$ddwd%bjK+Tp%Cl3aa67lQo+&N3<|0gNO_-~E z6ZA@fupR0dJDKi0HHzC|V=)QlBm1+6$jCE|q=1X|zJ3LO~_p?Rbf)B-K* zsrjQQ7w$R{m#V|%(}{QkUoU$l^Tz}f9$FC`oX68n1f;#s<}mN##+-roeAozsAeocd zbk0e$keYK;`gY)+AL-}Z=;dkkp0Rth-g=Pzj5t;=&L4$@0A)(8AoEGoPgm+F)n_V( z@{Zyc3a-Ds@2|s|g6ED}f9ly7oE6MJ#l81O=pn)P%0GEUl?L;qR`&*tq4fIEl<$aj z)lU=8?kq0x$@hGib9+1(A?H533~Nq%&OgLe$7h(T-TNvVr$bMqt|x%X`|0bOlnLMI z$o#f-Q^vF1>HWU^$r5xY{#mO-YAVMAa|;^AyvM1-Ks}$4wg@nuDtHy>?>dhErkqKW z+83f+q0BhZmP4`Y1<___rI`gHSKO9Hm$+TlU4qF<{Nv&Ww_*nQOj(yIdq~F zn8xYAClJ#6k?P?~;ve@WneXrmS!xXeH9(5EC69EkzJy>!WdI5L=!13Xvf`xC`peKv zd0!!Yg)-(9s2RM=CETd*GTX2EDCO^EXP5|S4baP2lEWV(nJqy?g)Fa);5Zmsa!pf% zl*u#BeX*i|nRcGO1sZ&&Df_Q|s9Fa1bV|PIMw%?eIgzl?C;Eda#+Zl2pnPr29Afv= z4yzp*$ALAE%>T<6RRm0u^$rC4koI>-M0hF&X> zZ5@^=4k%+izfoy~JXHMI=H~Biw8`wM3mAm9sGx3BNPeu65cK_wbPRY-teO&Oh4+$Z zZ)W$B@NzISDc(L!2?U*&c#x>rO<(fA-U1o9=m{Drkq~>O?nf>g zl>ox~eUVSjRTnh$g7w+%c8Jja%LPzp7f~NtlAJ6onC4zdqZxt6&iYr1no?BRq2aXu zBC0e@Ib235ZuK+nHUvEO=MxKz#VaJVT+Rp5tEh~k0OB2@!aE6F`C@S_V5)qW?xbw% zE_$q-`j%^ax=Ip=2nm9L+A0h+xworUxJM10KOZO%cjT}Az9M@~?${ZRQz^n+%i^3l zT$?-(3QzVH>+!7a*Mh7%%7Mp#mtGvd&Tq;iX<@uGSH?Yh_&QrU7p3kNM-o}_8O`h* z-=yly5nY4hmk&*veSQH;Dq@Ov&^ z7W zZJR!*yJ;@5df?Y+?O=3D@Zy(%qnqR71t(kEvg0}Y%yPBQbwfPi(g$d^eyM{=n$Ipb z3NS++irmuE=y`prE-OOa^VMytX~zVMd}-wJP?(U+6qx~=snDX?vE!-LkEt!7;@%00{W678}HNW(79=X}L3ztjT5i0+a`~v%}cTsIuy-frT z2>?P=ENg|Mv*K=wg4TEOZSZ<8W(ARG4q5DI@58tSST~g}uK+kDn4i)9zu>w6_0r5j zRkxvUtVi=2W^vqn?z;z=D(Pv7EwF?B%&V5e^&hfH`;M@6xO0xOZ?Z+O&u49E?s4H> zH4}^t;TNP;1$Q6a3lJ;cw%A5UxNtnh7D2Lm_sZ(S^cJ<3d1e;q)An~m=g>UU%{Mtv ze(RcF&Ln;*%7g~03^Di0N%~!G%k_H}qAa?n&zj9~0x@?u1o=!I8hzIQ)@HlyI*&LH zs{bE4@XFo|k>$8RH3#?B3EMgIT2MjfhMEc@UEaU6I{d5F7b`X=;N^+ut~k}!q6G8V zGpB?;{hLRyC&+pJM$_vis`S*6$(Grj`x|^w&$>@s|088<(oqLnmHQR_B-4~Fx)9fj zAT9wv+Rxl8QbjM42)|_x4lQ}ni>_@I)F=a#+F`N*2adAh#Vc~ODXz(-nP*^jke~0a z-ahzUp1Qo2fgteFB&j~rpDEFZjaa^j2Ap|+l|Ycj_7S?{ai+NN$E9en}O4{&v2jJL@hsH%74I!1KW6wm4x zeuCt1o17^ctZ5P)|L9z#WVOG;nPi)I&N)FS|H_Q%SK_0@FL=AZ#VSPcM!yKl2k0@n z{*WcSm*H3@Q77rkEob-yn^RF;*}fHx(LibXwAFrQ3zgFTNkk4Np&|FmcLgp|TET}l z8t0RQo7nSw-ZtzulJ8@0N_H9KrLZ3`SNiF%tFda?t(*IM<2oW~<0MnC)!CMBG#95O zcW*mbZZuLxF6Wq$4MO9OAfaWJ7|OMLUF=)4G#H;$Y#)*TDNeB_dzRe0%WT{f)E6ofDl1OxkdB1 z?dbE-td~r5CbEUc{rG0BG_=(Av(JuS%SVb5(V2V2JuxB{FV0B07Nc}g(d%YgGi6$N z!>8a+EV^6swqa_h^sk)7Oe_2k9z9XL!_Nf?BmNsZW+TodQSLwYk&{(apTSqGP4I-W zuzQcr>~mrf2miU2t6io@{e?;J+3ded_rG&iHefq}$=x8H5t=E>Itj=z`IK{%G6W$- z>83^WGn@nZQPm!Xb?}O?-ESTGr+-g*!`m_nS@--ZfHdA;nlySl&At3ElVfp6bX868 zAc2rT=(6N78c!5+Z;eO*ueiN|$pGrG83y#4b!2``|6ZbI#TzGSzEUAQU*BO4ebejW zROMD>FFlcyeik8if3#MuN8wt74bKdrYAta|X#*CgghXP8cbJc)^&9pJX0NE@;FC;J z!Gggxv782lUm@SMCw<22uf|U3E2zLmg!9Id_bhCAKZ7PX&y}wjWpQM zZGN(p6t0o&m%d%TD3s?rD7^7dg(X4V%34qKAGw&gwd4nAf`wY{evjY4jfF^=3YI8u z`LgO2ND5o5-S%RpfdsNCAp*f*UZ8TU5!FC7hFpkovZ*16e2%9}?Wqzj2A~rN2bBC|1_BvNAveiNePC#Rml|0lK z!xfRVx`f{s)V#8IYc{INHHi?AC!ed67AziQZ&wm<%uA*=X8wkwcFJl~^i!aBLt{Mt zrJRs;BLM5XhYK>N=sNLaBdL9ZJUlNQY;k7*U8Jl$c!K)nyBgPuB=4LR58^(}Yy{GZ z!ul-3{BK|(N;KdjHov4TcrZx0!s2`N-|vJe=ctF{ z&=)0P(IR4JE2bwYW?P;Put7nnClj(WvlDeqY}>4If87@=`lJQ*asBs=R;RWvx3gbr zQ96jzNfR4(gVxvcRIPbl#;*oO+%x##d&xK7I z{g|UR-@Xy~wUa>XZ%-o7C%zyopD04+5C_l+1L!Tj-74ngGYwe zI>NsQUeY*DIk6D&N%w`WcRRe~Y_K6xx6AV&cZCTxvl}~6qQ92ByHML(#ag#l#s8Mp zdK+On8B$!ck+c-oa{6(x7Vx(bJnq>Jc*TJKOlWsYG_ML5+<3bq5ckyc$4)k*tMu-W ze9GKTc2eESS@Nn*!_}R^6NqhN-AHq2?5Vox;VsVuVe_4G1v6o3Byb@3p6B)H$Q2!d zBco3eH(Xyc<^L( zbXHxsBHC_ce~=W@8CM9}5s89UjQIu%c5A0rbZwFFLU?qGDp&UMUa3Nz_(_Yw%P+;2I<#9*Z-*c28IKYCoq z8EY}ujI#Uu#p>}4|c>^SGviSix4^p(mN;! zgepaPQv_5{0*G`-^eCW$66sZ`hK}?e6-ByqkQU(p5|AoQ+FN);z31F}-uv_ZeftO5 zBx|odd)BlyGkXTz6nbHxDa`piy@G^Nl8=VuCyBWmjZE3mVKe8u?b)dE46My!*HiWi zo6z*3+odve&vlcfS(Yh5KEZU9lJ}}pCzOM0J+GTs0?IMJe@9ja$#aa?k7T*_S>tM< z@`UmdY2%~ER00jL!6}v5v#ZK|D6)*k>-KTPxXOb z{qtM9(4|-9Oh@tt_cNVpj&3RQQ);UqV?*12L~-gKW59C)|f3`UZ0Mxe8x z?UlQG`&Kt*Aa=QROS=}&RFHo#HOPP z*MpVRKwcy{=*~*F+iB9zqA@~ab)TGkJaluuV0Y%K@N(_9)aZfd(^l2{E=_y;yKp27 z*_l|sz`I^jNR_sCflD~(*o1KF3dp1)^o|cN=s7272E7v7HT*_|*O|3QrAYKly%^Q+FRQv zkd#CDZwNWISlA2RGP=a=dMazR<22f>1mU!u7X}1QOq7;^{nHjh9ZOJs?l#rS>*KRH zv9#kRIptemzm#iCW^uGL(q;G_sMVHEH)BpcSmIF{ZxGPWp>`bA22a^_g?HHa(K~Vq z!RF{Y8S@hC{-pM&lNIqIM_6j~paa8Jswe544R6fRxRUHhx<|09&$dS3S z(A6hHjlS<$KTv5l=)7vT2ByVuJkJ{g!y2?A2ZN^)`kf*&J_=KxKKsIqu?X0D#3lW(~#Q8E> zo;0dP=i2)%XF|-Kd&W%-7ceSiit`U4vyJ)jZw#bUMF6F z^}l!T=xTIoa_~qpHu4H!+Z0;9prI5;o|Gtmn1Dn!MqE#>cI0{v#b-y}$hGw) zy3B z9lA{~0;d#i^^dpP62(WpX<+ZZI9*0J;5EQi5XjKHqK=+)HYS_BV-_}Xp9FhX%ZN`B zI!>%X#L#2=?9|2KDx4qA9eX!G*8ndOou2m&N(L=4B8$|%Dv6{=8cnbo8S28vj8oEl zI_KA+xAoc59u`0fwAs!{Wh)fv6KF549CxR((avP}z#%K>^@M0s&VAGh13!X1_9Oik zj7EJ87!k8L^R`&P1p~pCZji^3Zf7Sb)X$`ogc((Rg`9?K zUwpV;bTKA~yiJP@WP=`W!1Oqjtt2eC$E z*fZ%U*phs5hKCEuX=ns)K*BNm9{-IXF|Q}g5=?`s{Mx|ZF#|C+4ta9)PcOX7m5OoA zy6Oo2@R(8`&^{8jYD8u@`jL|H5lI;By-c$#YLLMB;4Mb|&|6l`+j_1}_#veAX2qj- z$wzhDuDT+&EXfs9y9Rer$>PjbMgu#Hm2Bo`n&dc0*58oDWxTYY@FCyiSu49w(7tlru^sWz})ri<}8OAyXR}bCd4ov#f9HCyU6ILVC~kUgJg?)3xuy z?9J1vTIXSSE&LUG{Rv@A@6jY`>qPm`>)OD@f=})}t9_LYNn6ZbdB#14%b&cG$BVqL z%mOr;dD46KdgIjz-#-KnNF{oN!qp(k_(gzaSocP&5t+pU5}XVwIFs6l`{PkM%}i>p z=n+XgAApi%mDSJ`cqj;!(9Kis^}Ms=b=o6#amn(bgnqNIz=CppwhV|9(Kx9kATCg5 z$jE>NR3-U~t)lJ?h_m{4QZ}6~(L;MlFe|in>mauLrQZ}k?m)UCT5rl-ewvp1-dm*6 zF7$2A>6n-nM^StJWm-3-c=Dv^pB$B)MbA=XO)adO#BcI#6 z5`y3S1Avr5+7UQ;HrRpfzSbz0Vd+k>)Y*F~;|>!f;2yt@(abIfQhcpy;Q}Fa00l#~ zKkoiaxaEiFooA~4@E;`hRlHA**XwB4QH7m z0ws$t=o$It9;2a7#;n%H($O53oKY;y=$O-p0tvzWI!Qck`He0yG(JD5X;u=Z!Y$1d zz;`2Rsq$YVvvmXz?}rS37!ifKuE_Z#I} z2vid@CA$0Vnc}j?d?dWcvGyq>^?z)TE6kbUu9+*J7HL+)+2W#l&zi*g(a*J?e%+BO z?G2I1{LX{T^oK7HVX7CfkKTRJcRil{SYGi>acUc&sVx>QMcvqYRAFxiG2I{I_|->M zn&cSh61iEy{z3p-3#q!&J)4{)-c7SLmGo~E;!sIvj&3rz7uqYC>6F~kOa6L z>ySl629q!*i=^{UHBa|ZT=sl^Iqt{~^Jh)h(+ZL|l7d%Rk^Cv%!p(EI)3}S3{TfDR z+KB5(cg}dx_TAG&IfP!;->qRlv7zPwy^3EoiyG~}tUWNI^V9;&_2|t=y2~K1u|y;K+O#VQu`b^&;jaWkPe+g z!QHLd2D2PaH{`uhP0>`1gm}@ra9RePK;z(4!SLMb0oRj9M6Z# z4)af;$7ESlgPbP3%jdL{bcy4O=Z^De8y$OeN*ltCU{m*+QpQ{P*jBwD!JWsY;6686 zv@AHK^=fEinPl@lHsi0(o+N63M8Fd0oFg+7rHGvANrpgwQLMMYF@FYNgM@9m8B>6K zG^T`Td95SXFjlSORfv{%!0GX46(D{R3G|@o=MEqx@qwVnsOZ60eZ)z&awmB^(!PE5 z!jnvtl%1165(S+GImtSg5NU_@fz5``eTPMguN}ypmvW4sieOuwzQO^tC`=RP(4&DBS`GF1l8hE5+J{&gk+!@aZFCMF zFd`FpL7G-Hacg3)KEqY%{D{QMCnn!s3^i8a<8gNyEglfArCIOH0!}@_aBNQh9dC!B zj_s+A?ZShl#IhjSi)Y%dhm{;}-)h_o1maHT%5NBzVP=f(8Wf`CQ9at^D7y}0iVzV< z77i}2fzCL|)@Qs`V5o$d%QeY`0Pd~B9AGnx49SEPExHbv*M_|I$szM*WL_sN;L%3u zKeu+I3db#?J0at+F$9Zq@d#4i@-_@53N;4G>F8y~;;`A+uoY!Thto(UL+vV=V(E-W znR1=9Qz*A=YRvn9%jr#Q%eG}E$DbOJ@uTkHg6S8hjDdk0=+6k=ak4#FM>-djcT+b; z*!oC7h)0TAZh0+v0DGkX5RO}XMQlq~fz53~>rG?@$tI!)gy}dLhPgw9rVo3)VkyHs zNVO?aWV4d@V{Kep_Kb1)a;r+`$=6Zq3ecB!te*fiV zM4CyW1|3VZ;}rh{s2{Q(!!1o-pA;~lps4CvZ&sMtt>susx+c$qH4mk$%MH2}HHzxU&~@5aNM`gpLB@%woAh?GK`5|3BA5a5)qNGc z4^xIEL2DDG_1mxV6gYAm`3egn(M2Ti2Z!Sf(4uq$Bm@iYC5efTT6MN87$dN+^bQtx zr^bfblHP`=plzPW>Rc~`)Iee<)_N@aC`X6_QusgAFLRn$fHw0f>$Sp>dg}Z-@NPa;izOD zgbh+wVhAZ_$6uLD)US2E*j8C3oqA$8R|{`HX_6TfDD<4k`i ztdpmMD{zE(B5;-N`hH1&2`&}|D@uZk8qm%0@2ALy5CdN0S+csm@7dK2Kk)n8j5()tEp4fTxsg6$OVV!}NRE%PY^a(71&mPDRskk@ppA(w}RiuG(>m+u+( zlYPMFKOiU#1QyB?y$Asu3CznWV9Oken_o(Qcy=ZX&?HMH$z*gU$Va>eXL@nwxkOANo4o&@q#5#Lh4M}DJE zwUhx*%?FUsCDU%nw=&86_XlYPQL>#r5v~JXnf>QnRqM!>oF;@V7|*(rR!A&e05_G%RE4KSVKJ**1y=40N?Q%G`e}nXO?caJ$&F>DHYgRS#8Y0udPCe>oGET5 z%q7ynm{jnYFRt*a=;?xxiTAN#&M7p|ckm7_tPRD|c%$<@bXbYjdC8>8f_tbKrgKPGg%d~0gcc&c+09Uh8#*dF@& z`53N6Rn)$(Ws!k4QJ|euIw(ocwOo=Sj{Fg~bQ`PLmgP}kW4~QjEWR{p*f<@&+x;%` zs3R9rptx&gOucIPIgW5d;>26{Wxc*}p@t3v7QXmc)D1KT{k??TtnePkPC6FJF}1wZ z7!&Kx)6*{PjUTtH(7Wgo6fCqe)=}nIWhlQAQteXo+nz?=250a>hp9eGc54b}2R(a+ z@`Dc#fZYE_!5%!`*+JK zo-qbYIS@P@=AFcab~Y zmXU+GpZ@3_^TawH<^r5OI`4&70o!4~WI$;gE@Pjk&`>nIKufV`e_x!r;k_x3^u6X5 z+KIC|h_Wy8FGUa|PxzhXh2F?z_<6#Y%vMI192JW{y>^l75Wu?hlinn&kCW9j)FstN z92$;BH!V?Tq9-F8Evf|{a$%WNjY%i9*K(o9q@Mb?m=3L^*yI+Ibw7zN@}1o-EhcX;DQ zUzxf<+eX;H?n`sk0K0G?^DnN7Bl(MXmUr5ZMAdNKh9WtTBufDY?Dmj3Y+@!6t&B4Z zb15Jrs-vH7>A8}Is`!#Hw|#9gYK$MRp7#DzQ)4|<;8R{g!pVt`9nsNtJkd=`IGB3; z4i;y7ZBn?T3rB%fd*LJkUuUQc^O@ckPLnLKYPnz|8D6REEtS%FZPpS6jh-aKr2y7` zg9`fT4wzT_7zW>VJ=nkOr>{FFiG2<3zEOCALttNjUJbs!!v1^og zUiG*ztA?~nlTDZUaV}+@Cfs&KS;xy!I= zw7LEoA`3Fk{eZ3$q7OCIOSsycfX)u>PFDye%DB#=BfZ$=C{Gh`hp@OSUePk?c!PH+ zfP)f1D_lpKXZm(|8sg@?6s$m638{R-pCo&qf9uP|w~7{w)Hsi8Epk z{0uTq>T@l8l&692-UU~&Hv z${OSyU_0Y!u577 z9c4#{mX$LJLzMXbmaeBh=fVni$H@gqH!%qI!&r*~hb4}83CVH#YKF2Ptrd6IU(^d_vFr=^G*rW`Ha zd%2A}rNQ=Vz5iIJXuZ1h1}BR+qj6u)(Gfd2Y3>5H019&z#}|%8Pxz}AXm((W4#N6Y zr+mulB45P`2ot9iu&R)hpIR6XJT{m-F{bi4^hhJSsyE%7UhPIk+F)|!M(s@kCyBS< zvh{P2^)6ogYuicqM!yvD=-tBEC=#}$uFdnM%F9S^(`iV>6=3O4T!l-Y+d6;W^ALUES(%AbCHbKwb2%JJVV+oHkCY8teB!3+LlfR{;sO#=ZMF>60^+^+?|T5k zyG5ap2RCGkf(f)z1h1W%i(8bh7Kg7dOL6Yxdua8&)F*YQk9`$qk$@@`wO|SfuCQP_ z7X{y=9Sk!czRg+0R%ETuWEo0jA95`9?)_s^&$y9KUR=wl9Co}QhlEv3HMH>BcW9Bz|p}p7KpxLkn_=EWf zl5&Ft{s3}C{aOu0)u`Spvz^gueSPP4HvA#xk^Jua^2@e6Oe+c;$p)^b%6BTKmG5jS z?n}?=pz33N_WUFsn$rY`6df}(x+Po8 zNm6ks8x)eyt8TCL+n~aY?^%(0hIV5a&3%**(&e` zbERtLQOhOCrBwA-%QqXh*Pb`7<#AmjPT^`9Um(dkfT|O}-SX0>&cj;UT4oqJ_)yk+ zl-^vzrqn<(a&uAX$tDw1#M@B86gPT*vVte_TCZ$qMxqXuUNDgdMDZk3Cn-azt*n7R6-UCTv^FXvN&u>m*a>?!b` zjh!9Sjh*eh83LIJ=UfBiU#`r7m`vZ=g+3XI${Y_@^}J)sQT)1X4wGfG>a$axIETi$ z%D1r?PU)4pT#v7uM~gQKSrsC`Fz=sl=h7Bs=Q~^x;y zq!cb|;B(R^N|wVq?oDlr0sYd2DU^9@o!Zojpw1~ac_I(j;T%)GSn~K0Jj`Oar zi@w>)3pV&*K}|ROLtMT3P~ZBWHF9g`CM1^a?IG@`#pRl|co3~wVX|(4w<8O|yQg~A zZlekOA7(X^CjYbv8X$V~yzc6CS+OXe!e!PPSzh{kjKN$fXU87OG7KeevX*N{SXhR6 zPS2qE!dxmQJ(BE<xU`lzp+CL@-Dw(;e{#z!oYhl=bIe!+ z;zpYqXbTzZICHl&&yRL0eZgkA`94%d|GT*w``dPIKp5|b=VR(v4#ufYTwm`GW}MclrO05P(6O@##-0gT-qzr`blbA!*PSSt8=U~uB7^W?AB2x z)Gp8pm>rN#_ROHf!4_~Z{k3B>VsK4)754^6Tcf77^uX13kLM`zbdJyEsk>^_9{s=; zr{`KbsGj%M1>!XEjiRrfq%`)}-WP}cJkjE=SwGyz#?H=abu_~58BOu0jh2AYqU(;A zldlYS8qgd1^pUr5wk&6v1*xPA5_?%sAz{6eVT+ss6J!v$6A1v~W&93uR7 z;XaF3x0B=(WoP2gZp8RH=pGdMZ>ttEAv?p~S|u&@(*Y=x^=F7NHLNyYm#u&Y_CFxJ2Sq(ph*?PkmJ z`=tuWhd~H-7x%L}+T{RA}ef(Xfq}s>Y4YycTnb(2Xo?W9Qq9mc?p0zL7UasJp(H zu%hk@Iy2F~9cp|xLWMgXkdKkiIp$OCA$;8n-vj9ALF0YA5Z%m){Kj{0siN%|lnkym z^OUvZMGwyV84);hTvpMsf=x~N>aGi8*&cZsNx`Z~kK3?2_4i~qt_l?=Xaj1(WPIz= zjyhQ%3qppDr?alOcj}r+@u`*#K07;G-+C% z*oV?wD0{2riZlL5n)%?J#&5T%i;2h)olZ3?4?he$W?!E{RkT1XkJBn3pCGHuy$2ihh}ju8*Jvtv(S8xOQBuL<*mQ z88pO6vFRLshN<%uc2Jo(C}uLiAEt)-;+a(PzMT8Patc{}l}BE6-Hj{B(1o z_L@h8r#7KhDwJe8JA>NjLOTxBh5<3aAO4=B>%e5c#PsNg|L3o7!C$QPX|f=LdEayc zX)@W9S9K4(eto1mbccJYmJXgPcIL&3po*;jyVWcCEWjV_N9{QZdG!7gJL2s+1HOuN zW*FjgRF=0%?xB+z=#6**&p%OSx8_U&S}*n{&D1^GtWQi_1yOBh7W{eP9-9P;_Z!xr zTgG=7UPhfqOnjhx_oHIwA-*ssMD6YEBfc}qt+1J_4kPhwBJS@y^|Cj=BGNBcm63+j zt6xdjVbmR$3WSBpzLIscUMiTy1^+4bIAg*Est--@NGg$8lEE+Hs^vNojcD*<6M@&o zmikKFO&~5r+cUD`L}1Un+aT5C%i}e630H#S1%aeN2b`PHfn(qR$_&9-m}}~*N*GXO zKcGGY)96)uvO+A2TW#d0Hz6BZ$(dIQNN$3R0)WGlzu~aI4;RWhZ4u8uD$f&Am4jsf zyJ{o-RU3$I`?b@Ew=%4ozIS|S3fp!?o>|@5S}l@CQZJoE(iFLUl+7{jLBqntoe*ya z!ujZ*B}4lV@DV0lG2Jm5uhiEk)vOmFVz=(WCPvHqIv@EqEx0OV>4DZMV4;e z@iim6No)(H))xylEhj60@j@C%rS%gu*5=tjzz#UQ;a=~x8)xELkxD(?!o(3UUJzA3 z1oD;%sRK1Bvp7(6huHh;#OQ3@#+bi#@a~#!qO^!t)XR8kR;c9ne3k2u6CeDz9urk} z7Psh;v?zCkLr~xPhc#d&#~8N4ie1IwFsWaUO_>??C0Gm7#aV83$aj ze+QC{Ny$%AW&{bNqN2j4N9p~@JviWGLcBcc@R7VG0`RK#tx40vv_7oBKO?u~q`!Jv zPXFggS&Fq2KMLRsT1s{j0pXo`@s0pbST7oQ{{?Oo|s}+^WElo@fqb6G?^~(t7 z4s5)RQ^TtD#tWeuGO3>%ePpks*#yZnJYtZ|i@zj}qcfz{S!{v+zpyrnS1+M4*?X+{ z4he;kra-#gF@MnJ8tw>kdk(R>a)tqvPNU3=>PZ?!rH2ybHb7!wZZnb_Y1J??2m?%k zFK*u!Pas~^7L4UXr#{(RyUdN}!ZWB3eYvYXH2fm!dFVz#9covo>2~AxBNKJ2Z0ZXk z{RlSI%v+R(M{7L}z-5w!n#ai>mc+Z*C5WRDXRJV*r*a=c@dJCvf_~Yh(+g%n~WyYIrza0r!rnvkdNl z+9xaJO|Ubn+F>6gmKE}%eMV*|<_|%hSfXAMy)4mbhKf}6yzjXID<`|1%c=kox+0kh z$JXB>J5AE@=JjZVIXNqZuJ(Z}{0?HyJf(d&<-0%#GKBu`FjFgT{rR*lMM=Tyk+>g? zY{cLI;C=y#U&#(pg)Zpiu-C6^y~;fpm=v7?MskD$uW z>aMtuy4yWxC}IW`$fcJTyKcKe^PUVhyy`B4@DZtj7@vv!jY1Bg3-d(T(I^|CCcb#R z<5TM`lEAN~bHWC) z+BF1Q`lAjQ>Gu=~!rfoOY|2e}i3-mI-hsFH_?6^#A!`|UJ9iA|+~|HHJR|<}$JQ&Y zdENvFu$-hFTaJI*SPWLI=M#C@j9bRN{-rNlf{0l$U4s=+J*M!(cDRR_!>Zn=Vu_JI z$23Y}{XJ(VhvRqiLSGNI4m$`8KRgXB07jG1Y&%&5WH0G}1stWF+8Dt-4UCPS0&`$M z_qcRJ>M!0HTELxPiU_s=X$gln`+*a|(26m?+|JT#Jw{FZi883C=>7v>jdp*tBW{hs z>WKi^X58PQ-t+|`;<|`i^g-=o?2y zV8W?go1#!PkjVA7TN|M%Ig|rhdzrMIP9lA#KAm8r8oFF6j0CHCHkvNc#qzeNT6M>`!y=tgrkITa15@jF45%i%q!u#%F)%`~^hX(Tx62zEr zj@EeFJ&{*CQcJ=L6aLFq6#WeuF}^qijs}%gYIyVuiM^)1Dp)jvkq~HMK(%yrLbz=H zJU;JW_1PqX_X601Hb3>AX(5Q>Qombv2TvST6^-H!JJ+y*8bFOhwj%>0?QKnoPD93s zBU-L?*go>{;LK3;VS2iKUL|20I5xCRXg5E?=m;gdV%$)Pb9RKGh8nu(rvVt%C=H78q~pkN zcX5x=BF6U#8K7Ab$`BU}t@B9OV8WLtO&`MZ6IZl>-@f{)c-<#*A<-wb{SH_2FTMM~ zh1`&C%_@zxXe=CBPnks0W{`9q^Rbjj24od6Ks|fj31phubAlcy>`Sl-r3)AnIuQ|( z(fgFknKkrIM4Hg>pMDnYn~r!+yr4Z?>|`Z+FfQ#yX4oj&2JM1g1|{35Jd%Q;NI~}o z!@mJQy_J^WpE#|3*cqKfabm-v^?GP!k-R-lBA1#xXhXmR9NIjGG9@P?6uFc9sc zgam{8yTeI?js%bnvQ)CiJp}?CYdq#N87}4A78fb6F;qX*7=_#x4;aneNfdLwEq zM|7bJ&<>(?l779YTu~(a14kS=C8_=?ZT8jMa$>@-YX1f$hHm)w@D09=g;Ahhx%^l+ zAAc7fAHV9d9d_*mHmAKk1KwZ#xk*m@!pgc>@vHIoKr~|S^1^0lrwW`Q<7E9H&B2_P zC>S(8PtgH*HsNu2_sBxR>ptB#DWuy^TT0tO1YgwBH4RIHBZ-R(F0(Evi=ivshDMsJ zs-bg`R1hsMCn+aWrg80VK9l>!5kK)$lpQWRJhS8#DT|m#hE=b~gbm(Yz9GgM@GC+1 zpfd3Hhr=pbqEv`vpekJ2m8QV!tpG#s6Q>+xTgYD%T=K*lfsW2(;N>IwObP1B8V^O# z?FRuWR&OshG-wbgd4LEA`US8Xm3?Z zy4-|3&V$K$@qrxJ58nBy-+qI4$SKhRGgxqBlT=LPU=N*f{x{vfB?*ZId-!^aq$93T zl7?QTL^KdHhyh;*7oxA5_-i4n6_gKjj%R_lMU|P_^Dl8CIhKs3Tre5TQqCPo(ZqGqY zOhe3|sn7_{qol3orDx*#HFkuN{-zI*23LN0Cvobuc~Qz8z+Y7#qg3zeq$do9ZnX(mV&&=E`2KK)jg zyespP@DimMxerrZs$^u#W$~d}QUMW)ae}pri%SL2kQ0*jSAPL=c0 zZx3D~8w=Y0(tF0pk96}OtsGckKLg(;iamlMrNRk7m4{q7rAxe5;gJVpRXy(ZMe{g+ zRyPqPls%WV(>}icC1a|*DpmXwc~xjB-e05Z5!s%R9HgJ*HtaT$GMOvPl_*2-SpwwI z3)VNG9*r`gFEy`ONKLvtG4ZVSUL3;!?Xe?@RQnCwePVsC1@FHbJaiDNn^ioy1=WHj zPVL%kJoh$G>8OLGe+*nC?3cOMSI~^pi-y^A+nd=lq#<1>?A3DYs--v>K_J2wN(vK& zmgFivhWF)cp#6n36EB7iEE~Y)%z{Rrt<dhZu5gxIi7boP8A42(qrw}?tlMxEyNW3aDGvT^U zW)FJiyGs5B|4k)~GowC5gZAbumZ5Z3sA~dXB?xNc^?}ZhIaHK^biPhh57qsiHTD-% zWu2&GtbDQJ|Mz=Y=1s-gr39`Ig^+OshuWU-ZJrh?O6cXRLf4t5YCjyLExUXaTadV3OGfZjD~UU> z1))@`-#p!Y)N4SGEnSNg_LrmO`%gUG#MZMIXL%C*>@^YcPAiB`X-CLF+*f$?D+q^_hjfAU|Z9uD5$o$bjs zATaio(}I0|%E7aBzx6g5bAe|M?t1;8xex=>iqL)C_`02Q*p+LTNBswb9dx56DOkj|{a&cq3zGiVUvUc^J zXw$~(PQYGe!|4qo6aqg4myznAxMv0Vd7uRBfj6saD#oh2ukWIKX8fLHvr23P1R_hX zK?nUY4CNj#G z2qkw3k87>SWO73w9Z`|$Kc<7&kHiRD7|B1+DCFDnm?b~>`dKBz(;WxQZl)Sd!1V#nS+ZX00x+!>b?=`xeV`SBsOPwu(c+PFpHZM{MMYAohxn;6#UGCf8rl1c^yF zwecq#zV1OtM}a2+2k##Ym>qo#8g9l@CEC34bf@-;$yrg}tpLo!r+K<7b*vn^-=FBO zX|7_Uh0NHSm!H3C{yYWGead>8jR+#f{yRQ$O>?UVIl0=-rNb_WAerXp-KP$&#r@$S z+!P%D&W@)_8WD`4;n{Mi9bZeHu-p5ikRHa}KZb4y!+wRsuc7^@TVeFXhj zFN+3orQw;)1c%}6d-ye?K*w|F@2TeLcSF5CyNG7~!y7^%DDP!U%y`Fg>S%oivbAQb z6+SM&28DbO{td5LOsuSJc)Wc|@ZMIpK>pWz@cj#5iNO))5y3n3QI;@+v zW?V~Lq1iXQCpAN~c_@6K-neosG2CTuVr>nsqPLE#^pm~&6I`7wr7v1Ir+bn{<# z#*AyPh$2plj;zcD$TZLBXhI;8Pk+(pGpuR8Wpu2qpFjvTn_zPH(Z@Vh*?-ae2X|^7 z-vW1iO(n#}k;N|t9(hcLzn&5Dw9y4opuds~mx z-PLNLf=Vuh+w<>ZM69&4C0pwObi*9I7gvoL3F=O?vDw;eQb`f9D*v-flcOCt zvlO@PcuSdKFY`{4E$T@FsZ|Bi!ulO?#8j*7_Na@|*mV?L7&(8xmF|$l_*-np& z>jz{0v^!X{|2#ghN;r440qfFpGF-;#W9hXUH8;yF+X;QS52+-HAf*p~o6*s!Jt3Oq zX62_Yc$0QT((^)|t;>Ir_V05hxkBsiVW1wW+jSCO7_OMKp|0A zenTxbQzv#o>!ot@)lX49Yj@!++W!Pl9tMM1lsM@vR8B@%`+T}5Ql$X^)Kp%H6t<}@ zQ1h;{X9|6)_NPl*TuVz1!VdjUUz=0zSkBl+LN)OScx{WbVI6f4aJLE?qAE85aG zN8%W4eNLsPr%&#Ucy`F~$&3ehp056!#%n?Z(WU;4*KTilMC`Y9Ghj@PRy>+&hs*H! z=Kph*zDRj5jO5wWNN;Z}b^;Ah;x*Sl|FLYYR3+QC(0R5y(*j83>Y5s>3r8Vh?o5cE z@O_-sL9Fv_UB?u>7FoO^+02J=75E3CsmcWg23phy_{N9=`(=B5>Mes^TSu}&iOWF2 z{Ai5~88z!>bYZIW4NIqvr1&b&8K=FSEsG-iv(?qS6NLj1$WM+cO)=SxbicBKhhmVW-SLCvmcuHg6e>cIQ= zTAA6|&VS~WbjW{PI31T(9zz}1lG8_6=<4b!59T2GcwG}TSNTm)`VCOh&;-vm%2c?G z@4dfEqg+YAzq%X9lcp~zD_c3+Z{=3U-;Br?ySElgjgQ)yRtV%E;r(MM6ZW6L$u|dX zj8=#=-|uFFJnfrN-l*be+Aw@Z^_zanFRyeDdJBRNE3*c{@-#|cFO=UHHszd)y?_7y zOF+}DPpxewiSfxcA7%e@Ztx!{nxylKwvuxzDrDyR^IIRB5HiZ`dm!1CtRRzXR;Zt8 zB;XPFkBcF3+P{Hu{-R*CuvNJ*i?r+edknUN{Vp_=lvlodESp_exH;$bn=SBTBKw@* z1fVWC;Df5&_2H81eL1~zYcY0(0G5i;+g5)5{#HRE|GfF<<7M5yCT+NYHFmSpUzNtbK=y^DT zt-3{AhH^wFi{4A_X5;WxAB;2e;@|AE|G3(??JeT|(;cy9Elh=FfUV9-KAb~OJb$g? zmx=Wsz$nJ|Kjw+#v<(_xOj{BD+Vo(*;8h>Zezx>R8IVJ%+GJ|r2*2-S6gVx_WP&B3-+SIma=+L3>GM93mx!#Jq7=)_DIyayz2{d2OP-=NM2YkP=Ikx^NB)%VwG zlKlMPpC>!=Hi#7Mty6DIQ|A)aaQ__e^Ba_Rx$+bl<*<7>FIIXs{@)AJ{ap| zDm7F5uOlr}R^HtEqDUhH#;H2z#Qck`#alY3LsmK@g{TI5VBD48_V+(wJUn4+UFnA2 zFa<6;u(^%P`^|zs9P#thA>LU28A`v_8FKaLFXQ&-&xa>CbEg<;e*6e9$Hgpu+wL9y z{`2#lN98S5sEQ~X1oAZc*UtOr?}sP$!fsL2{Pu3Z)%E`Z;A~@L;(m4`0R9iZ*|R zufdfN9oj1?a55GvsTS(Xu@-2P=C~sk`7|od_esBonwJb zR<{S%_JZ4W{lrf_fBVOi3u;>DK9d`r>&p$Lqm6t5CC&&n`uboa?KSe^@j?>UP!bdJ z_FT#HRrvD)-4DNF&I!U#{edCw=50{v?+`r$k1rpvota+^P-dTV??_tNs_#^m*q#q` z3LIehuaAJ*Cz$q)=K7m>=iD@8v;HETy=8XmqQwNZPoS@PK(<~fy9_dYIX{Z5Ai9tq z@-E{SnGG_{NxO}KcLr&3J_jd`1WKKbU4CJ?jr9MxQ|`O)oREtuwxikA%kSs%b8pHk zL{vsRmMi<_x`rKU-7vFbm17XTyJ+g2B;P3hZ6&rL;KEt~*7MO9;1&qhTYHW56vk$o zb*9J@TU7+@eoHTR9rBp%l|wBq4-&SFLk3T3|B9EemHVzv4_s7{^P;v_6b<-9!LfXY zp%x!*JY#N}DB~7nvDS@;uREPjak>(}ximL78i=bUWQZM`HCd#S3*ZyII$y=OJnqp>gu?t1nwZ2t}< zI7gM?mk1TPAs6TPI*5ua12d;Y5sDzQ08;eP_x1l$w>a_Pnvd3jY2TK=dO_wNm1I#D zL7epgahulIWc_cw$GwFm@bp2A$)M(R*#>O(Q2j(8m-BA_Li=yPq5A9{*37sIms1Fj zHEa(IAk^Ez62EI}pbQh#T)fqmb2FJxi>yPiL-S@Pay3o>=G8jlP;}>a!PX%pNoQ zhCkFyp~0=xIIP8m_3xRl-4E+0MY*i1^Ai=akrDkytzd6`0=8w7cGkC*0)}$ z#%({Zx4owz_GQNZ4gJFDnx-o9UgR;a2M(6UwZ>WI_-%tvd4$E>Rz6D(NWtv2zUSQS zd)rZPqJb~AeZ$O!FHYF!TT$?YL2&w(Dqg=m^KYl-ME<5t`jvZU`(S+5900R-woTm( z&T`@6V(QeFN)J|5e9#fB(~kq!t?=Bhx%U*xWv!gRYaC*qJ7%;C$|ivSs|o$aA&28Y zgyOAidG)JCCh3cdak2ofx>I|7R=I(^9nhkAFgJ2YhZAal3P1~y)4#KEm<#aZdItr6 zK%^Y;^2AGZfQkcVdzl7#XR0{vMPCEok0;CjW~mCbWMN?@k6#3pSKAXf0Y%w+WA}S~T>@(j!QEjquahGQgFGy4 zJAbEjrwodFJum*ha5%+2+)_2RIGJN+aNxK;d8Lye()S8fyZqE|CdzVYp(4kQ`O@j4 zm1I*kC9lxPBxx{_IjPW;irz1D{dQ(w_EFLcmFsKob-@cHngUcp_g+*F_ntCEd-WM> z#Q6hCzLYN?NPuw5I~=eOWxv7ykocy>+&KycY!la|O`7z+!-hxq(PvXbi!?-B5EEN{ zAo~moX;no4;r?Ha`N}2(HoT?cr2_&Gds&yGQrNG@Mrc=UiqGVI2($y2j1NYb>sJ({oay>Fo;PMwm;Ft5ZHUlI zca zTdwJOGb`8ecLO;CW`+66A39Xt?K!Qz8gloN$yqQO^;%!q&Q-hp|Il^i@ldY)d&XFj zh9oI#g$PO6_oQMdNf^6PA^Xf?lNDJ$K?$cMb6Q>3 zw(8Da1L5!W;(uh?(!sank2#vo9~Mr64?tCHI*$6dSSXlJ#NeAQSvfiBJ>x4#Q(z*M zC=$oS5h^ab?j!NP-6v}_o0R~M&RYSI2tM>P9Iq8q{ZR}55}za!1hRyV&u-=-JhtY5 zTt5FloX9y7hL5<+cY|lzQuPL9KX4?G2BWGNg`HF6Gvv>L`B1`Tkf2FvE;+4f;4Kon z8SB42gdqOwF3C#ua!SF!+)+ebb!6JdCjB5udVwz)pPr_~?dIx2{r{Fj!T;K8 z56oLy2g9fS3J34@uVhP*lDjFxCqL44Sv48Q`F{Kkg`l4OFZ;RpL{RiV5ohJW0q+K# zn)2i|!Ipjl)f`%f_7a$=m~)*exyW|IA0W2><-Waq>g|0jS}Eow4)xu@0DIIG+WV4_ z0*4*sNQKHE+j@B~{_l@S{91=YJI$|E`StmB4o?`9I^{nsR4tV+KrpY~)+o7RyW8^f z4?C>z;gq9g0Eqfn8iMX*AGb2wTG`_T?RZpRG?<}O&wI*zQV??4F$tvj|MED8E#Y{g zt3L`0wlSfo0VwqR9_mUAP@)o$#l~%u&d= zwPZ>Kc}%c{34(d6atm7#a;Dt^Q}W~KZpG;Q?Q6XwAIbf=I-Djhwatp(Z05R4-$zBR z=4dbb1|+_q#Y(+sxIQ-CzM$9SKbDy*6o71qmf0W|CAq!Xe%^z=C(D}4Z$S*Y2P`7> zx++R)shP61yC|o`S0?Vd;1?!9ZT{^RLp{7kTPSo4WtN;bF5{Egy%1!+UarfncR;a% z7D#;$O>#5DZkRdlcUHT{X%~BL=%tL+}yN1U>TW z-4zllrw#gp?^W!4(SN`|YBf!at1rSu{z}#t{@PcgDg0bPbUI(zGdaB~qq1~9^*d3k z>Bjw8w2l{=ryioo+k(wov^~=QrV~)Yetp_rSAmE@j8UAgv*y>)GA%H>9A33n*&xL_}5W}9tDK;m#Tzr*Z?RtgKDBke32qiC8~0D~Am{#}$&2O5WHn@fA9|wA z%IaFs%gX0U1+<>&@^4{}o!1i_e3vVbsc+J_tVdTh~ksN%3JR%v{XHuF1h-zj z`kTr**@5kZ@WT2S!#Ui5wPk77?SFknvq~_wV#hFORB6D+k4gj6?(&Eyce8XZD-&3H zJ-Z!UuC0i?#W#CdWxw5u;=Jp^07W%=jXDnIXij5U@Va&d)`pfUHis}Jvp*?t^2}@2 zvv2FTT_eu8XiA+SP_?z{cgHXi~=YSJSKj@ zMgEQ7k6l{Kqf}Fz$es2D*6WMSlpmC*lwwNZ!?Sb}@tHWQYcq=I{ScE_K^#*{^afHs zS*9hh|GEE<=`I5zo;F1h;MODj%S}6f)?0K!YZbK?)h-~DHZ9da1#_Gror?6ZYmd?+HH_Garz_d*3_@eGVk;)#fH zh!rS*dmxW*4^krV5pvFFRd4qwuoTtc8;|Z_{p+S*MZNi#*_O&2K-xrFR>K8WQz-WZ zZ58tUUF%BJ0Duzial?WsDKcEBJS7hbMA@Naf(gTk_`_cb2WAZV9lcf79NlUOG!spm zkt%#hUJY^n4n%xV8mEri#ek4Y?)uc5L(F8(-t~o>e!e9h_3I!;q(6!xs$W#z0ad}N|KW9}E@tZ#0Hq}a|A0TF zvlXV)W{?&(aM^|%dMSHQ9TYY6eK##`QKV`?Jh8vOo4=URb0wd-4+uAp)G z`@zhh^{J#McD3Hh>f6*P;Av`Sl`-R}A+!GB( zJeQI;7AW6(*A*v>=MEohO5m3t0fc_nk*XM}r9$vleXvdvD@_mdHlU&&nfbG=!aSKl zZs|~`DQ);cd~2EvJ86@s@Jb7k>l%}9C69g}?9)IIB!9zoUQYWbUx0FQHRyj`X@u5B zf1_O5P3fI$T`b&^H-cQj?DI&|%KWgvN2<1h1 z?{YgOjj{|Pzloef7G`OLi@#eCYzd<31rEeCOMfEw)mHk|)%taW6Y>4HExaXwCu3xJ z`+|^{l{X6SH22adjAVXtMp4qB%UVUS4N{d6f+9GfBPq|rC$2Ne^u^4o1^A5xO$L)I z!!w@^-WDC$@U9D*zDP#$WN4+!Dc6*e3T&R`^4Pp?P=t&s>_+3y`tXVO5K`JMga=Q@ zBO(X^^E#-UtPWI)ER#Khj{Ho*&9|@Fn|zMH<}yevQhG>%}w-~8WCT! znr)Psul7y<%^rd?tFy19e_8m%RV|smvRS3~W?XUyzl0RhzD%$Au57~yD!VC@uh^`K z4~qKmXZMyXTZL@H^Q#A}mwo#P?K&A?9AcW63UVX?r&3|#HOq#a06f+2Jpq|N6-15+ zbw1p)zF*7CevqP3^pa8qvTcaD= zZk-G@qoCYj`)P!iMdlpZMDxgEKfcP?=XOoZ%au$%Ez72jQbQ5sPWOD!&N)qo!jTK8 z62<5K?sZG`OvVwC?lVNZpnbzlJGZLhVf)DqZk@L~d6mwg&mgFb-JspM|3^tf>;!X!8bbz1Hzl9tTV6R?@Bc1hz4Xx9bw=6MCJ-0r%%ph*@mcEed2 z1i9apP`v{s-eqoi+ca&Q+2Eqn@( zBgRC@WTuzp4+q?_Ju};%y2AZ!!)P^?52=)wwQqT+#VS9%fhgV$$L(~?d+k8soz2f0 zY2i*5eXoblSi;-b)xH}I&aEQ3uEwzr(fSK%4=KZB)ztCzpXlxqd}DM-&2WmEn}g`f z5H0+~>Ou3OEWXoN755AcyAEU-H&}QeF)Nl<$=esbC5g&)QgOL$JDAZqwj4f4rd#r| zstf9ZXR{Wj1A~yX|4gM${cOgWZ!(OCH%25Ny@1?MWU*R$L?Oa?wg<@ivE{ZrYHqsd z+39l0D*30v0~L6^p6cr*oq9hQAo+voxDdClJ$@hSP5-t9#P~%JJ|0xD8|YpDvSZM2 zuGh6en#bCQ$?R}0lff>1-y&Hq8H@Qfak%Ymk(TIq`_pZ=yOQL>)${nzyzje-^NEr2 z*;kfZBlW&-=gq9@#Iv&8jBBbDSwHPxb-kqJbOQ=B#Aq%}=4%Jop>E2vZ&(8XKtb`Z{rFKH{vBQwkHkDhY}Wqu zg(CHk!eMR(x7NWtmy)-`0YMh%o!6%{2U7ZR8_m%Z@=tL)x^=6YnYg9wNX9P%@DC5v z`}KcTx%MdZPKT2@>>A2&wfR;(^Baj|=N;pYr8=USsbBtNCvLDjV|C{`diidlb0kk$ zp)ZSOwgZXcgQA9! zHE~R3OJh1u)246g0WWTdlRk|8iYzchjYm zw$dt_x}Zr8o)x-$V=W(Y+uQ3rjZ52CsWl(Pzgy+aTB;J2O0&uxWP7TH7j^{kx;!g{ znjBgcC5_n&>eQ4)md0h~tfVS5wf2#hsfg}#kH#?laQJDR>rE8v>M8raCkYFgaQjh zqR3pc8r2>O1n_`?M$G?NNE?bGN_JW%ha4Te><0UUK26kC`zqr zXG3PbvFe)l@paUaeY!X3MV9kz+5SYMDw%_UL9erh*p0)2a9X=K-z&xIecHH1DvW!@ zOFCT;*tPXda5mX8*7r(;+`+(_Y^9uG56aAmD>6h&yojMd!9ef4Nl?ilA**ls@UiI- zi*(bio7C&%jO*cai9I&Jt2uadGLZoqBjh|c(XT;1)OVCn;3SwS>Us{$IeQ7`^{eC7 z`2!b|LeRJBn9ns_=A$3-S1DPLzrP)TzC(RM=@l#}$;-dp1(cvD?En&y3M;IsBL5`A zAt=d}+oH(z&hQ%;YbQ~&u($nhz!}4|0T6oP=N6t>UG?jHgYKC{ z&9h(Ul`}(duF4?&m&@Vvgcg8g34p<`|KJ&i0%okYbg7>J6tsv$lQ}L~jke*KkRnml ztPnz>N`*hr%lU120QPK{40oz%I#dcJG;jNctZ@XI9M3Hoywx23qSe;@mZrnE<4x~f zO^DUBTh#bdgJ?|wz}#ll(;MU7ZnxCdRNjeR(n}uAb)qdCOp$EQf)BRE$R8Y)>hi1Ei2!A~N;s+j|{j!a15lD*eeI@8-`AkZlB8 zccLhrm(tiE_^+H~Q}RhT*Q2#d)PBkfN@`JO*&Q|er1RJ3l0iQC z7~=7-P;f4S;U|o*A}aPo`ecXS%?!B0cMH%@A?ADo`YIkDw5M|M{GIxD{NtZ=n4FBx zSJU6Rt$FNlt`L*#(|s{E1Fymha^&RojxAb|1@XvesZ{)Q_k7V9ms!T$;dDSRJ?>s` zBMTV1xm0J?MtYY~u5#-fJJR=x))d}wuHcPTyM7`wL6!}-{i1U0yT*x9E$S5cE&1m4 zMS4=ByC>SC1Pt{peUh08kvje-^V;d6S;yR)B%j91N;sfmQ{NqOT{Q$WErfPU>ng1OFKGF=AZ=aiT5wy!(>J))2=WANHC1u!qC z_vas8U;2w-2A?n97arbDh|uGsf8Il+eSTsWlI3sK7z;5mZ_TqZSFNhNiVmab!Fi5b z854=L8%rm(b7`*B&*2AFcDf0QZnYFBS-{9T{hN~V6g0}b`JZK^wL341cVP)Ge0=W# z)8YVR=It5IEtUK%DQBzB#l9k19%VEy?{2;L%yptHgH5gk;GP1`k4}%O+1z`EtOV$d zeWJi9Kz7UC$;neJV)LY0BeUbWfhYfhOYf6b`g)OeQw7 z#zDFMI(rWQW)$9LxBMj}2+u$lP6-%-&aK<+~^AQ0C}G?dx#;-1eICo-(PdIPsGr zOMar9ej8nds8h=nc+>h!fu-;5@Zk92k7Ck+npCDJuc%=vz`KL|HY;Z6r0`l6{)U-k zbEBC20R?odOR+B1iM-^=H_!JrYj|Tzne4}54>b7HO1D5sQ#EN;vrxPSdU-i_FKZcSzvnnv>N^&Ond*`-N`s zA9mym@H)(z<3a=+DBEnqeFS4p>c8Z^Ztce*dH(i*)(nZFp` z%gAI@;2?T(N#Ggj;72jUX8M!ye4%XPotr;o2b32BA*KC^;dDbBxY~l6GoU$Gte~8M za}n0IC~6$H*48PjX5Am+{$cYbS4(OUITBA4X?@>AeM)(C(z~*9Fi*{r=%uVS@tjhC zk9f>}J7FV~(uw1`3>?Er6Wym>WUboXC!YSrSNNfpp--NgQu}5Jsxv$@>nb=tWAT~@ zUmVoTW~C$0yIK${hS%wO=Qi*@GEY}3fxAVnQ~>9G*|vK?nwb7)uk@!sOR zsafz{-!h<1fw1SY9i%Xm1ti@6d6Jv&=}twG+4Oe5KL!*k5s>Z~di1xhpzojugerQLWCU=GauNpemXtotzHX4R^13fOLWQ>K(Nv_NZbN=9X(u*Er`R_eiv{HCS?g0<@KVWE7cK#az+}?z2Wa5yc_m0&j4A+ z#X)p~=o1;Dw_I$z{rUAjZTPdC5`R+rmQsY_971D+s`!Rhlo!}6N!G>7zdq{Wh6DR^hZy z!S>aXsqY%eBUQ@3?*f=ymwZ#Bf_~sKKe>Amq42M+|B$^gI7kMq3vJ|{DR`ghjG7H2 zYDUo?WJZ}Z(jHK)FOoBnKDWz<>eS_)GD{Y*Ay;$stcFq|WS#y0u@=-BS3_c!QdUpM zh(xX>8&1s(Y|K$S>8c?cF?zKhn{{;NS!84pr61=%`~8@S*Gf(_pTi2XV2kAsB_?dK zUqC;g@fSec+Sml=)0z%GqHC_RA@AT_fpvtJV#BATA*AC$jv(StvbTUuT25ct&94~p z_z0x{SDTk{jrhFB?JlpDQCq&1%DWy?kIW^+h&}$gRx%6ZW+j1EM_I)!ooGFKKOVPX zEO6U{M>!ZUB6mCf!LOP$0e;e)58NZ^C{{LfE0DX)cZ<^xK`kKr6s(k10qT_Xnb23t zN+f)Y&we+iM%j0vB=Ncbw>@43Ey}x19K3;$F16TaXvt%p4eto2DaA&aaTkx6n-+q!+(7=I< zG2E5FfpkP)HCC5HKbTrb4|f8rr{JGgi*k*V;=GoRhm(KH=Jo-pAt37A!Q z-@F zx#!DrGR%l~RwhPybhcO0B)(EgkyIWy13zKU?&%GvFHh{OzuA(f@b=L%I}P|Cyjzq^ zh-n`-tII+5ppdJBs4q{>8K3!K@xp&pbl4mP z=O`FIe{*VX_!X1Df=**~@(sy*9+?z0i^)H3`Jhf!cu{@ewnBu&bjvM!>O%?(S>CQe z9=~KSQ1o_~J4ODs=$CR^^v`L97g_c8I!wlyPNEj6J?roQZS?{7fQ&)+0IT)IM~<0} zosNUyy-Qyxhl-MriKr{44~lF2y{TAcX@VSYr@|{4d*mFi%`A0YpZejW_~b4vI8~@i zT_sy)v$ner?u>`WWwRM`&5$0Sf~!ln7N~_pt-9P6HBx$=g<7u3YiY0FkU1DHS@@mO zV_`ykHM~8pN>1{2Z@pMFI!#f}U#Q_RUUgav9;{Uod-aeMnCI3dA5zAU?{|?JO^M;o zcOck?Iw8}0=gTPx@V2@PE7gi&H6M0Yq)YyG!`%)TYKCS&O?Pa|3A5P6dIZQHCxsZ> z#hmc?+dCyTqaTh>o2eEI(+gtW^2)q#K6NsU4mGBfXT!0?13<*yO=s%!@Rpx9l{|ld zNdEmewZ!vCeynZyu7RSPFx>gBPgx43;JuRl;=|v+03A%mjC#bgUMJk$dN;X)0o4^* z`=^?~-_}tE$S@w)w$xUe@fD@~tm4r=<_^1=E}8bePNSqKnGIU?>#TXZeGP^YGSfSw zwX{L9<_U|P$Kmg5kVctD{PLRcF|E;qZlcW9=G31Q5%VLq?`~H4QA2S3B4#^_lpWc? z+CdXbsYQS5f&m$2oU5T~vHxZ2ki{r(mtU>1s{b}+6#avL(=S&x05F&$&nMuV>)v4Z@?oN>k74i5-2V%^=q%_E3)y(~(-si~UWhcygIYYoS@1DQJxEKj~Dw@$a4foQJDTR-%{a>Lw4$9b6~Y z_MDwqpO8DqKzvJk70$Axo?SVb)@gdO<{GhPcq#~q{+u=uLiC@SUK^vZ!^@d^HS^ShJn_o*SRS-}l=V*=w2(K{;}ROzT!@^0?Di2-^v*PQI5{%izM~7)Q~| z)jp$rtB|IdEb)L&H9C3ijQPc95Y0jvP905aaIa3-g}^|bIxdx&oAm?!k2&K%Up(h*eA2aVTuPii|obo z=;Zh=i!_+C{d#z$_bR?dZqYyTj%VF=YN~voi<{f#`AA({r_pL|M&Av$wrp9eP?$5g ztHQ-SyYbrn#^|A)$LQ2NV=coFrFWGg#tld<;{wOZyY)^z23zLTr{PXQDmApOr=geT zH^`QUs>c(<8)VDe%CBTgXji}X;@O7xl_}-Y>ZaKlOZzq8t8O=UqJP|_AmFP;;w9i< zQ!gjDItv>-J>jo~Y4@z=hCJ8E^N@HPhs=82->FEn6cY>$k|m$dxif7SGh5~=6T zmp#(y$xxEc;s9j=*9I_Xf)d_;>XTDnWe9-@=szY+)J0Bdo zWw+v15Y$?u-dR#`Z-32H!)4F$X2pQK0-+pUX~dWq8_<7E0$Qip&|1de!E#9_i%!TY zOfZzKQ2=@64ZQq7*B+Bh!yy^jI$r6}B80GkkG)B)_9|T!R6K6)gA;biSy9U-Z@C@R z^^YVTPk5$%af)%8^J$QczAI;kHK&QZRPD3n2Sz}G>I+9dU>}~Dcm!&4GdhGc-RLEyoYNQMi?|rW&}X~9ImwY4jYi1eR`n7EcDLJ zSwZ1k;hf+@Y@dq3jB#woA`%rt+H@ryZ4W*4=SZX9Lx`~BXQGNHR&0hGh7d!hC{K~I z8*z`C5*ZSi)V{8HBzz?2adbNdgK2d~x1Ql2eFgezZlN&gwsxzSCu-pIf&UikT&!9~ z2qZj-Fu+gZ6V83<-@Vu5?9sBQP2qaY$`526Udx-MnmW{A(I zS)yV6o&AJ!t)WUNM0+Q(uvutJRV}eq)ve8jHz#oG@(>HjgG5M!A#m6NXfJ)ig7T0y z%70$z3IEZAv4Kned?(%?AR$jqq{$%+R8^k^U}EMCXQ4n}J2JoGuUYR{RCb)}(^GY> z!g+!@!TwCQfR-J?z^Ni}4bBa_yY$zP^WVGj!IwoLsdyEA28fm@lQJ<{lYw6wg;4bF z((c}Z0X_!npU;y{kjgNTq2}5bPdWyz!ct+q1}${i4y0q)MS`6aT^EZ{p3nMjlh+CCB5m>Y`$pJ#3PJwRIK?d@`sn?kV~G(T`Nf-ba3gIXd0+e z_kKz{918}*FCMwtGqgV^#J%RXU;Oz)&gknWjdpTijv5mTNynj+ar3ylyt^1do*+AK z@mczfg&TB(E|YQglX7E2%0;o24Xts-aa)NTU;L51SUafoiG=qT%?_+xp6A9TZP%x> zd4O06xY&9o*6@KjQWfJRI$<#(HxxlqlkqVfhyY?`y%t>M$Cf0PMp>|n3umkMIBRZGZ%<>}*u|kG5|hSyB+QznF;R$l=xZOW`9s~V z1LR2l9=d4A#0KR`AhhDJS8m1`7C(rqcYrWuJ z$6XDwq7S+#&Fi8#4UQm-HY-x;_A#9prCLG6=AkuUeLHRo3fhS~_{O7pE9cR${;BwI?{W)5WSV)Q+^Bq8uh z-u)87sh`F9+|;4RR|Pp==0Vc*#LmWp({>J|aX%xk9<2t)sj4G}dF*cmMrNvtn#W-y zJlW=tafD8Iu^j^nWxuE5=12z9H;dV%U(p2y-um^&;|3*Gc*;iM{!?ipF; zaxtk8d(=D9CXRd0CgC31E^a%`%IdwDk-Id3(fvRUZ=p^NeBUXXbS>=&WJevR#j4j%A*f6Kwk0V?@@W^ zg}v_AnWw;#PTt*}jOGx0!7X@-Kkf8iR{P6!X}$$?5fD5igx>B}`X#h9vjSb$KHn2_ zqUqj@4s8B<*D*IS)Xgrft$)nC!5w|%biq)dDE-KAIBs+bSj z_XAm;$nB*vFv&g*s`cf8{heVgfVaG=ZB$-}y#5`dpXED;w$-=Sm6=Ej~8oFm$GU zP#IycAps4HTQ(tz5sph8KD>8%^1|$eAtRK~yxFKfx9@&|f>*}-EpI5CRQS({&vDCx zc69bIb-IDJ-MxYh-5x4CXFhKF$eZ6?Wr0E4D}|1iDee5!3}Q#nosNLg-_8cQZ|U+X zqN_S6PknE#7RWXoFZbP@N#eSH=nl)_Pd*hIB%Usz~ZSZ6Y0 zAp2p?YCUORc-P)F$$1VHc}vHOOfX%>A2quE66uFxEp6o?ozPycv?|Et`r>Jp)&!8{ zj^VDyt?B=&b{y8z@qhTfp~yFc{OUAU`6V}$uz^OJA%iL=3UewnDij?G)tHj#ESFpV zQp>}va;_4(bqIBVM2q|0TnSV#%wy#o^0mY?AAgA{X20#48fqo1YNfuXZ?N3VB|h&v z#h+;+hOHIIga3A>Uq5VR2Ve@IF=PA}N~i~cIP9HSjk}KpXVeGtuv8##Pp@Ly?r)pA z$Kr|wb3fs^1-T*n!G_lz4SiR8%X1cp8%UJE5cc_|m}z2pLuq%0NX^rNZWTn2PY}2J z?axjjOh1j@bFVTLe7$Vd^6i)QbO+MAZc1m@qo!usgSb-WvOHCQFgLN6do@x9J$_}!UAPt zEbw4(zckO?LQylwnR!(K28}15hUzl?uwvenJz8d}vd4law(1b!!Z|Q8pKXl)`LNCN zQ>$P`DH*Za2m5Q#`)5~in&Gy z-ddmIQt5Z+u9r>L=w^l~XK%L=PhK<`D%p-!r3^ zZP%a#Q>LBX2Ry1Vj4RBaPQBm2@>mFN@|#E=UDd@_yz-mW^Uoc~TsG+!pOO)9JJa5o z5T?yDRUn*jB5@&5$Dm=EtvTbI82T?srr*x?p)#R3bClc{_NMei?4wZ0HdSpzK7$?9 zGHg) zeW-}L(p3=i>zQ*l!iceFmU>-dqro=`xTmr@U>a)TUqeW7sq&%hq(`An5?7L1M1bKj2$O9H6#sC+lE7&1~wzpQ)Sp2AM6VEOuil{pbOhAv0WgXdK==(UU@DA!~6%=0_!tT$?`XF@afH0Hlx z#_kNFVTBvHi096K2i~MPgsbA;tod%Y%-f`vRy^XM`Wds$ESK!U={mjj3IsDOuSH zVwf=SyNj#zHCL9>$X~3O7$-vFJsWpqLP}q(y~t5iz0#9;3W4^}J~s|TXJMYHUnPdF zl44j|`%~`l8=AkXh!ax1yd75`Azg49-I0xHgPGHbLnEPRIZIMPlG)8q9?eJ(SQ_cQgo$Xgip?o2aql8oQtIs#nIvg#o%|f=hlM7aQ zPb<_Wuv`?LtfN7yn0{nDF34)0Jn-l=!h+5uo{|)*3#?8gC7sRRVVpPnNPX4X#kh8C zzB^`Zb&I<*b{(z)<_dm14QBl-gMUb2NBL z0d;U*X|iI1$7^$w`cXZ^`2m*pdsMFhAMs$C8#rZkVdU*yyS8L+G-S~pbrW682Xu@M zS_l0sLo1|)?eb@^Ze{cRJ_4UoNh})w-x0J%wKpVG zovm=;PeQxH=!)=;w@{_R4=E3ZH7a~y1t5pI$hb?-S3KWL$1ot;Jj^a~?Gg$im{htY za)dxj7LlCRSY!7>s1@*xqSZI#x*que`fULWsM|#QBXM$_3Ish?n23oWSt* zCfkgcB81DMG|~9=Ho6bUiM93V^B*3*y{Ur}`>otS{@OLPY4vWs#Zi2yfn@K+)_$Rq zPAjevD(HrFWh7!UgBPk3Mh&cMm zsdZKC61^8tXKA^mX>i1Qr!TBI8|b%3O}%`4j8mjWM@%m;Y&qq#c+}LXK;mUb#NTz~ z<->RDs$om1EoP)=CedA1k72Tq4Ncq9TyY{5OHksFC=F#np2M}W4 z?veyBs$kI`TbL$Hri>)Q!yA&w+Q_fb{Q55iufNxg6?-U#a4yk&Jki-(|5T`hI+_#B z!~ZmZPwIrIa-)#JwGlOq^;Aextd4>Cy9v6Y({=BwaovyaBwoBHH#7n|Q5-HEcD7*O zCuhzA$f)*2*1=b&33Q1FMBF1E`;X|}vw5dV8b(M)*d>dp*IWdh(UjCP*om6ca{26D zn`_=jU^qeanT>@)Iq@?CVuS`Z_=}1-;Ob&Drba=Wd$ecasi-U6M zfP!ywK2F6zwt*1Bf#kgCZZWA1{vqlwb4rpDfhD0Krxl74 zilH&_I{3)qL%W{%X!R;eMe6wv^qi!hYK~xKtW~YK(&|yp68BNF0LdL!6|d7*d7+Ob zoz5Y=?Eezy!YWY?r0r<92PeQ`!EDY?!6K-91{JrKGrt-aGP|E9sLp$in}RQ%(N2v% z)U2w_LEAg|x;W^q=ryq(B~qzI_siYp^8cZiI9$C!?Tw2GdMo=HjhuSqMSUdPUG%++ zQ?)lfGBS>_Wy|UuC_6FR7ttBIakr4{@kIDx5~H220ViCLla*`IW~k+^w0X<7gVu57 zH{SWh2|=5Wrt_6sex4dIUGz85;ypj@bTH2ulB43`%JAqWVr;NYCO9yEVF{@n(>_-Y zSn8t?#03pQMBLqflp`Ab#MOPR1uybR@}Z+SbC=Rvm5!{r&K^to-bm^vC1T}yg-^bR zVvnN+Z_bpfBILuSnVK6_BEO4OLbBsCVzd-El6jy0r?M0%%fi=Q1rx(O@vZHMu@1g2 zanTJ=lp|Q|5t21V@a`fK2utt-gEqYm?M{NagswZ*0_wnu-R_Cn1ZaEA>2D2i$96kf zv3;@-I|{r7Qzcp^FRU1d zE_skTJ=u&f4VY`7fEgOGTWig+eR7s{ zWBkSFKJv8xJhc^Oc;mEDcZ_yGvrz^=$uY`F&jp}P4pMy7l*f>8pu`iUf56S}@(Lja zz*BkY(-@aQOTnRhgyleFA-y}xyeaC!u9rrSVs3h|eZ9fTWCaV(2-u(V8El7=)Xr@R zl^6wa~-W~_MwB&2^hzt zw0{d+9W5Lk_!L$6~$6gMbi(QtA7d-F7Mbdn|nL+4JvIVM%ZX8D$>w7PqA__KI{D{`Gw+%}E5$=i1tuf`MZ*=62a~AFrRq?$% zyRp>#;SSKF+U$su+vHr(|Gg4|x5^*V=ouM3F zY#Nmd!^eO!szdwDWqk46f{VtsXGszYdr3-$rBN++@0kajQOdfolG!g zx=ayO?^Q8W-ik?b@k7^}fw_s}NO=Q|5iVG#!qeq6{aqgvj*M$$i&&gHUVjX`%93~@ zD5m;c>7?q*qr$XgM+HO|Q zj)p`&CA+u#WjLPFjcZ-&eY?|XZSWBj`zllG zvC_w{B*SAL6*Nk|NO9qIhv$I964VXoGmg*(?#>8#$e0A=IJhZuKh13@6W@~0` z;6>Xvd8W__)He8L`0`=Z9IWgheXFYl46}u)dl-^0lMTnXL24yDuH=|fVcYc6+~pq? zNv}fJ!DbmeWe6Qp8!ru(BCS3@gVNP?U^9cIBfL*9X~?{ zv(-e;P+(5c3Lp=30Hs^vF*CSUat*u0GzZ<*@W1TbbSCX!%isLKL3I3z^J#{OBp39M zCn{mL?#yDN=yY$HYzPls9y&inJkY_zoYqm^!N7Wqc%<{?y^@JL2PfgRwTacUeSKFO zbURB06A}*1h~X+eJ71oK8k%cj#6=4}c`M~utRS&3y3;<&k1Mcr9a3O7slt}TFy%8| zdRm;)y#)|7Jf`QqDEjVa4%*_XR#9>o0bMIK8XCmlXoDTPCkBM=Wwdup?^7( zqw#ewwl6m}eZ`W7&y`{NOi3T06^fR0Y3KXaK?~U$SI-&d*#L86w8f7a?`aN}lQNIP zIhJ6Wqe3$oew3y}XE-f;_%n^^#L>G819D^z>H~U?^b(bYiBVi7BQ!up5>pmh#od{= z))Qy;;P1GzJJQYrePXq*_4GG0671X{*sBMbbeX68TI99Iu1~}_mV5e^d!+B<)$RKj zS20PK#-zepZPA`0Ft+@W%-#kS=Tc~LX1b>5@cR2c|2Umcro`d_S<+Y1l5n1UZ4ZVA zlZt_1I39$|eorm_?BdmaLIbhW!O+E~z-gl3o^Bs%nPcZy2vQK`CC`rJd5gCTqdHLn zNw7nohTLm;L*{h8tq8J`tT$gaa{BY_DL-0AJ z!yBtV#I@7DX&Vf8aFf(^EPTLkxee8vKJH+p2bB}GN#_F0dU}qP#z>kSxKe#c}lzGmhF(7K{xuA#>fLww+NhS0WLfPqT!1dPQ#TQ0S)lk_Bug!6v|p$R3&? z%F=XkscQAJkw|{xF6-^+&<4B8G;YnGe2UYi7o_}lMra+V%@e%V@M$)QIFA!KnkxHD z{&<-||MbDM5I6Ul=TU}3XHTvko%zC~5VL5N6vAsFpun7L(4I20AThardCp*fF*^zo zfzxg8bH1v`05m?G;&s|mJ{$%~$h@9wcKh&8&g$^*`x2q8j2S|z_K%hwZ$=^Sb^?Cf zVCxRd_$cv2h>W}BBz0*OAwM+8y2+}rx9f@FfdRoLY~F-W`zq(ZmXBR|`@O&p1$ zc_<`wDpVYinR;6U(g_jNG0`;NSGp6bIZ&p9P^YX&IcR+no zI&AS3&YWXSjG#XDw61!evxbz2l zRk$Vc@4Rx01@&%7Rs!K%`<8TtR6>d(_40j6{$74GK|<|J%wUzmB>Wu}gXU3}91NA6 z=#(0IK-%oKqZ@>@irM5HSN%V>z62iX_4|LAP}yz^iqPgN2_=Mvw2<5(3}ZJXWG9Vf z#$a-*TV1r+Wog6AFe9=J!)R01N*|29gp6f`G1l>abnDjr{{ElW9k2W9RpxV^=R9Y5 zpYxozK!og^{z&i!lFsBlOl3cIw^p)dw36U-AE1ppqEK#ZPZbE+k%%zvd9mc&FUkCz z9YOMz777uP_}y-twNe|xB1o=N|uTg9U*xj%8 zSL-&ZLgd1M$RjqTe$hroEE(ZJ+m}~fB<>az%MCxhQAETe^s}n?OSYh@R02Y85Bd&S zb!s$kY9tkO)mSy)rOM?yiu${!*Eiq2QED_H@L{9$8VNtAGJ%>kRFTe$5=w+$BAw5- z`jG7z2uitPph-(|%Pp_WROULsQEH#+0cdKiw8VEv z?WDHP1vz7G=Wimd&Duq;`Q~ncNQezVI}z%0-=B+-BpC2h?QOU6W^%= zF1}xjD%Koe>-Xs3W7k_Zc8@-gBmMcim0aN3CRfmacB0>frpqr_=DT~=Jn=a?yB%W^;r}?|>cObTP4H7K^EmCu>MezbHGbRjN05%Q z^hYelwV&F(N`tt!@?^C5e2dkK*D>-57U=gzS|&pZKR7MF3%<8w;wT6Bj>{J*L&pFd zv|~KL=jxpo)kYPqMBZvjX_*xeZAo0ssc#M`9KEw!tANC$Mc-ZjS{8}RI8N3x+bHE1 ziWztnkJS^klREzs>e(W=WHBk*N^H2?e!;jMRwn@3I(A+A$n>uDS^90xdQujYegT~x zJpZ@yX36MjqA4idlc_JGIU?>gBEF(k(h06oe$2DX-yg+$zLNl%`CzedV93Od_3w>4 z|6COBUv4a3l`)?9h|{Ajpe=OucKtL;5--rVlX2|_QTTU<@qs)mUQZAzSKm|VtpIb` z?H|8l4$fRh_R@oy_4S0Tr+pUCM((^Y@6u^eHX`&~S@2VW!ye8*T!*Y15Unv!F(04myEej9b;(Pv={0prQGzts_odL}dC-T$ax z>*7}NF@ZnCo!ov4ynv~Gy>Zl626KhER7Q+}PgD92SiSqam*}(YB>nbThkn^R zWY6CMPo0^){(`_x`L$+O>7~`64tkqBgJ2Cu7hESe zTQzy1`#V<)^8b-ZX#U_^tv!o4%tz1*LYJ5e!tew8C5iJV|I)kH%ZP+F+Z*@mO5Xtx z)CsUv_R8=@f#dpfeJijn=j1Kus7bPBX>swI&Qg;2A;26wTaHdf|PQ_pU{o-!F|MAGXP1hHO zVrCgc7ItPUCLHu3#55j9Roqp&4&)iqQd$k}znRDn_IHUKK^bg+4dKsAvw0uH7CtK+ zEqSU}s2LbOVT3aLrE`H83C~A8(;J50J#CjuRui!mh`cIv9VQ1G6&i9~<0O;fs#F{u z^A}@&@iou^s8}P=?&yRFvzFNyJ?0T885n$Y;xw~fpu&n$A=+^ll`t);3T8u?dfPMz z)WIZSyNIpCf+yQ86$AESV*l?h*@5Fs99PH(mD1|qU!>0LrmDZ(MFg#=mS&1#m{(5DlpSiJxO>J&LwD-9`8$N#Mc=q3#N~nBw>+!|=`%Uv@fUzdo zts-BK9&8inHq5FC{k>3f@*Gg9kGRz>&^p9}k$-Z#mpnici_IKHtuW_JeQe%nF*U5V z-sJo3=r<$I*WAyH>NWp8@uIP=s)nN#_7Yq|@-;#5@c!*(tZt)5y)Z$z28kHoW& z6S=2^ba#P(0b~Ih4FaBm4^3%C0Vhi&1ueOLxBhM6{r=LAE@MSz>k)r;`kllq1AOWD z2m7Yt%NHTt+n<y2&UL#5Xw|UZLKztdk=eIe3{4fpUY0og_QWi;~%$IOYEek#?5Z zw`ce&NxOeoDc@RY6Uf{$h?Zaj9`r;T7_kPTH^mfD{8a;wVfYg-2)(;)>Smcm0u_eO z(vjN{>I-g1Cp?bEM>~;NzJl8XmlP*)02*C|cCqh0GBz+b99^jz$h(q%_y3CFCo0y6 z_*3tEA85#Yo{;UttGBiOFf6wOl9k-lZ?6b|TXal?*E}HnX7t0(_DzFi1xpf+r)qps4Wn3zSpfKrziG-E~4kxA5 z^eq!JqHETiHzUU_3m#Myq^^7IXB#CPCqmzx!jkORQqExhLE1XLwp+OB4j%Ab@4jNU zW^P^JmfkMPmVJWGMEn19uvJba-kd!YfEVl&%mFRWYQMwbf*0{Z=fSl&5sz=1qPS)- z^YWmHE@(pr!Q=R{>|r@O89U)ECVKFh=P$2YX@UkZ-M`^#U}M>DVo=|$VaW|~A|;v# zvTFIc5TIkVCqZIhS|LIzG-7%fvHX~v%SGugclXCLbwCHgvpXk9iTnRh*|M)Q=uzDT zCE8tofByN}igaMEE}0M1g%T@G(+~SUdk>7}Bill)5pMg-i;@qL+HOAm+Q9iAuVS@t z3vBEjel51+9Dq=v3X#;Cd2lvfc#s^tQ*<3}4XB>joJ3Ho*K}`OMIO_hi}mwgDV1Cf z4F)>q1(#UU+kp6sP4fAfNx>=w$}C;LJl0fZuo5-70o5OfQB*y(bXXtObGax$$YpK4 z|L-+Q)W3ZlvjfhQK3ba1o@+Y3D|%G4*;hzM=sZ>G)?O{j%V_6QLY?Yx3z*>=+eXmY z0WzYo3-5VJ(!0rG4a2Ju%C9j#X?rCcR6*~XZ_Nb#-x|7m+V8yZzOfJylND^roiRE? zZCM0=|ATk$uEeO*?z-aT2YJ7d)&Gs!w2BRw>O?bQuM$;%{hZhJ(ZNfbCeyo*AG+=b z(&}xy)nR38++6#e`uqh_JvX7dI~)DQUOQ=SxAvCufz{p3R7R}XwqESD-fDU_ z)cz6RaQ+n{mVT7Nm0&EU1C%izSG%);YgkQT0rmR6Ro9>bZh?FYpi9#1nP0k( z=ShRaNu)Xh{~MJ4uZK&6erFCab@r~5G;;(7%v`LYHt~*wd+>t?6s%xRBfu&J_ngO`APMDr+(}3I`G2pkK~Iitu9Fh@Tnk- z{&jIxm}lOHZ}Rp>5PO0p56DP5k7yqfUF+4eR5BrT;Zl(ML@WW(yG}d&)$*v=D7BMU)5?~ zhYZiGhmyKBL@;s|QKzg|f6g{Yp1HjU1K6DYcg9r1rw~pQvooMR$ z`)SsEPTW7g?2*Tl^^>0c;Z`3_lEpNu){RL%b+>bbN|BgZh{$UC;@qE^J+|ZXUf0CU zK3&m`K7t1w-?#~+0$JsYbw}nuZ(_!Qx;%Bx2}s01D4gpF z41AKNz+rK_b#oh@(6Fb+KiywNBv=0<=PQl~bA5=p#E(aLe-t zj{-ZwvLUk4g*?pa-5M@WdweVTLPr#C>8ftGCf|ijg=nF$(Lwo>RxibbB2VFZ2DN~e zz^vY|Psc{pYz_Qv1pfqY>VD(Sqd3R?z{3<|{vU;5Kf)4if+g=iyCeWG-51yXmS`;9 z{RFoZVP^H@fdV@D#|CJC4cNLX+U=C5AYoG&f^+zN)KB;uQe8JD;k2eod`YnoG4I-P z4g59R|BU#KDa6r9~YnqiavgijVgE3BR*B^y{ z`FS6wq;yT;PEgd@P0|2 zUKvoc?OnTiVMoOr4OqC9`(gcx;EbXk7%=&d>Y}CeZWpVPe6cI$7Q2CEJGTm7Td#ti zziQH*59>r>cmR1_aTMr)l^gQ5i(3ww!vI7G9q?mbz>GwXJ&yJV!QpDYkXQ7v_NXMr z;mm4hT*7k^=JQ9zVUOnzNR$hgZ!8x`xfi<4i~q9#b_!uCG7%~ytt6GQU$0K<)}>qg z^|3xjllFEED>7ZeT1f_-X+)My#qApJPS7$ZFIa@$fk;A%zfM9`ivu?dN=d8LS~Xzh z2?-<2l=&qFgXlsmKH(rxE=bNLR^t$F zI~g?T{I@f}O0(z^(p;GLnW01@Ax|C*WVp6U zrsJA7QBL?EkO}Ayd=F{guefolpZn^M(pPFMa97@%9pfPN?(f>Z^Ry-T=;a6Tr#GBA zTJHv5qqxTCWLn<#wSVSZ5@W>th7-N@_Skl#ol1<%=|#7u`3>7|Sr7I4v^0)lok!Dx zuTECf6!XRo2DdT5IM&staFav^9(J|DqP65q(nrBQSNW8J-9fs90==*^vs%_)gnU(Z zxth=thEaAQmCXn+Yt!f&+9CzNHmRORmPB!URZNa}_Xc6G|ICt&j&r-=w9*y_~8C$>WbfKhFkm#4(EN)!O zz=pK8NcPdH!wpu(ucT|2Ip=y!ch@3Y?9+`U=7P6%qOwEE5?(yV6_V%y+;(VCm>T>T zjJ_MoL!-*b3I}=HefRW%fb8EVTK*D|O%0Z;)sP`gpcs)8MzWRhcBY>$$~yl&|B5&d zYZ&UVU2)jt*z*Y2743xK0y(0`dGEk@Ua294KUqr_t-M+hawsY*U%N$bgIHlh_dnM; zRqxYKWnOwC>mxIfkh)kszazVw`)L4PY$D|Q8Pc6kA|>IIQCBcUeAc@tL4$W|>|soh0t`Z8YQbP_&4c(?49nc7L>BqyAkhRG! ztH8Bqk8!_vK-Dot94F^IudJT#>hAKLIit1#atTdw^HWUUb}CH!fjSDUcncp`LL!|I zwlfF!$uV_Hj@)vm01QI!zTSg{j{2YzRBWU0Gm%+C1^oZc9a3g|4RSvkRrI^wVSSJ=@x2sTXar6;9Q@-^cW z7Gj&hi-^#DAVx@|CFDrfZ$YfpAG`)BFIevVv;k@#-nKSrm0<)t(Qj^&MQl8q!T!~| z3wr>Lg(wp(KS4V`nSxoex#BHx)n*&v^Sx1n7zKP20@nCBV_%&!dHB#Q;)^X9K;n#u z0(ZgmCv1@|qV1_(%j(Tf)tI=MkL(->POizLY5@6;ySCIs{SXcY^JxWBX@%Wu&YhXS zVeNo4K6+U=`_mguDw>$SA&rt?fOfL{Eyn6d+9r6BKoaB$It~shT`z2$$%HZE@W!V% zw%?)YEK7VXMz)VET;dLTH~g(xYbD2xKl1Hcd|qw^vpZ|v9|37F#S-Amp2M6TM=~D4SouF`im|ptTb= z3Jv89^C&1DDJ}&o+m>#zYRY~*ObNY9XC}5nQDXHjv|bx|LyeUoNJ?ZVfO31#T!%C( zo6-`u)&B4BQ6dZ0@WmQl9OZ`5E}RYlh7)G@yw{pRnAkD5SJ_sP#s~8q*%Hu8@;)#y zi2oF4W1U)g`NSN^X};Z+mSeS(9l?MM{BLdfmO@Uswsqq5E*8eprYfd?mTB_eq_)3uffp-RtV)d}+v@+zX%>nA{5!FoG?aJmJVF)$?n+Zb#t*Jm801 zFjxrlKD^lVfCe6+?;FU!-{hKq@glbZfHT?}>GIRHM3xR&%|JsDvy~8bW?};B5q5`D z+{!Az^?j|e0c3mPxqEKuHzcy05l`uaRxNkQJb#B}b1=YGjzVkE+?j9jeKh39*%mvPvIeyoCh&-qDNW=koa z+mUnmgR%)50sjte1hIt(&x^0oVy1rg4x5 z_bHnVrF&*x;SRiyxoDEKQlPx6VRz#0XPqGctih~ZsTMaJA4kd&Ofvth_Tlp2)5NTC zI#SLz%VGS$wCeEe9rjqoHCPAJoaEx%Q%2JGYK&AKj;eaX5w)-Ewam~0!vFhpjl>~` z8{#HA*-wE=_9W$q{vChmOaGGJfuin_^-!$HQ1P;PzJaEO>wTHRcU3-^*>OsY@X&MP z!ipgLOP*qaC@_qs$QqNwgbAah?iFq4yTk6#rnDQsNmA8{E^c$`i5iT;+hb3(a-yPB z%m3QF`pt#Cek7N6lmUHe!yiI#ncv9BW$GCUh$6=Ez`njZSU)LesuLJG0S$1m`lGSd}-rDe4 zJop#>^+LFUhQ?wcktJ9ArHf+#WyZ80HVp6?oA2OlO=*e?Uu2Fs4$De2Urmj$%Z*r- z=%^!mo`6;Kw6m+x=qTt1>#sN<(%5^>IY z!zU98r|E!dApg5r8D;6G*B^;|xbUg5xR5{ARo+*er2)J*%W0XD?W^oq+Zh7FT9{p7 zolfGw-!lo`?X^CzR?Yzfdqioc-;jNC!$;_Lu%rO9WhMj6zk5y@wG}~y=J9hDFaz3Z zMOEfB7oGp^b6;1QldVx&EeB!FV`XQ5yll3)^9d73>3hNLrIar z7z1Vz>pkl5k$R36!xa|NJrZ+KU1L?)G#PY!3jQymIW3Od+cQ&Qa34$`w1tcSJqDB> zX)^bg9Y9&Ru#oYYCu68qSe*3rn8P=H(sv8ycCx)7lU3dIqY+vBkA7W*8v0yxH$&vS z;LYL|5TmJfq(UoRDyO&Ed?pltspDoMgU^l2Lb=J%L&u3@?`u|#cWHwuq;DGs#;ZGO zCr~kZQh&CY$ zkntIGao~UrusUW+(H60<#gO^xYfjFcyBlU{3RZ~lGX}nEJjLpJqRyznd{3;{WjG+p zI^BlsOY3x@uaFKoz_!k|E`OWj3O|FD1NT^U`rxrsuXY5ZJT9}pgqxg=*eZ=|r zKTJOz)P0CEvko6zn@o#*h~s8QM7vKXu|Ip)7|lxtsAM66&KTr}dP>yW01_n1s4L|m zIT@9l9-dTSenCZV$z$00&$)5-7?3XM&UeSLU%zDJTr-XAl66j3+UNZB-n>nJwLP>f zOT4UQ*R?rAP!5b!Z-d#urIfoEh93>VLe4p z%*RZXy{seCY|vX0qMoz0A<#}yyzJ6be&+?*1VoV?hs_Imoq9aa@L!UZ4<$;#yEvbX zDpmteE`vt51v+({LNHb>;#4n|u6|bdi)Dc0uw?bK`v2h1LgqeRTY0}X_uf=Nj@64+ z<9x~dfbY(xWJnI`qC8JuztIBRV9_qTt%O@|Sk{x*b-GqI-|vPwF_xVUrrh<(mA_gB zTTJX^2ETtPE1w(jxnpwz0zdXGCp&+uy@KT4=% zy@;LmH7#%_hJ6+irL>4U!P_XDLy7wfN@Il23j!;$Pr({dGSX=kO7ZV*;mGShT;Ny4 zacuj%af*RX*d?U@#ksGfJ z4w>MQ>5g9tSKz_qdJgQXOK;&;AkuC`wK!*n#j+2IX2oTJ`C?dL;l8cH>W@OWgnSC2 zgEHrq~iAR+c|KPKYv$hdx5iB7*^B2~y_aJyDJkbgzJ>SYhhd zP)MI#EKIE}U2K1qwA+QYU%v4MQ zLfAt;oX{0ZFEH+{VTPUFNQ|9$9J|jnpN<5CgZQ2@oIjS?9WxePmveqy^mwop_5~2O zQb$V3rFi6%R=eGNSR(t_&251hcysQ35Rv@*bO>A%Yc%l1%o>M$OHdXv!>pz!{N|Ha z1uMYWaT|#~`7+6rkb-Kxz*rv2)k}{@NT?yZ*e_%HWk+lMAuMHAI@NtmK61qhkux%n zPl&qESvNx>`Y>_9OWECWI?k1&jYeQD8B_KL~{+6J5~$+ebD`!i!b`_;cv;$46#80Sb#|nvgN)^98h+CM4ze;N=$Sc z?NhJH6KE&4pyRXtlUNqw2WDM;O-(=T3CJ_2&dMc34YEC;Yr=y8H=mPpZDh8TzuJ@r zST;ncPudXqI2@P$Y61d7p{!d82K6t=H57%zfw3hM%$&O30|9MT6nf$?Tm?ugc8K06-x*(Y&w36&PsmY+mK1_1!#bL$@q4(m9O3vO!MDIYiP zv0ZgZRjtByDS%U+PK z`>7P@pYNBwrVVaMUWaN>@!QH@@sU>o%krAHl{Y&8=*Lygk;=WxuUMGQS#(e6qxM=d zeEVg6AbE1KtxOjFHw?7oH;`3FLoxDz{twmq6rj<5D(O|`t&5&*W$NOM{K0Qg*62K`I#`;z85-v9)=1la3SL%>&zUoqdqCMa;0lAKF9#tb>h58k*57l zvyB|*WlQWUrdD%{Axbh}tmIw*3Q_XJ{coS90!Sji*8GD^wEn=?co}!sq*HaK{wD z`C+dA=M2LN-HbDj*3>b_xE{1shj-Rbj=QB7e@2bDHcS3q)XRk3&%!Hwxl?TEV9BDp z)#zmw*iWYR;o=2$eUOV<$IL=RMa~0GjIDwH@*=TdxvwtMs^_6s@!@}uDAVE6QEaji z%hQIYkkIsb915k}6tAxd1T*P;fOWd*Mn)eh6)Va)_!`W4J`k4SUgilX72Y+WlX#&} zj2ymto)6~fccGNUojmuKyKp&B$*{tjEkhO*$^Dp&KA+BjnZnIa{cbjBGlqIV>R?yC ziQ<+%+fh)u&qgNz=PC+=eV_UW3wb*NFK%BrSs~-q@;$x+%YdEfU)qP_Y^@T`*8RUYPW^nrYRi z{s=p>S!$*OB4WbG_RR*QdpxXggD)x0QP*Hflb?=ATfa$?T_?I(lB`w@qMM%nr>*8) z%lCqt?V05(HT#h=-C@35#!0oG6UQ}HGj>V2l&1J?ZL5BjqSjqC+^k^3c(jmFJ37Wf ztqrf!wtdnl0NtgC*i<%y#My$R&{00>;EU~I+dw_(ry1Hey1=Ab>H*$%<})zSE-~lw zhj)OQ_5`=muW@5%mb*2!J%{iPofR%U#AD^~?(OPkfT(Q#AG`eX;J)6-xGB--5&%q< zx&NOfejnWIuJ12Qif|9)O9>A@cL*xTL3X#N;&0hpGzU{?hblw_qLnUsaG}J3+#?I` z;`vV40&8>K$E%;a=6a;N76$Wm{H54{hygR@%&<>G?DvpR=((g~N;|40&!BXZ?!Rgd zYxpz;6$ra$SurBdXd6z%fQY|AyuR-t-GW+ly;c#*pH_FzQk z!;H%8xt8Eh06RWl3>eEms={2WtR;o#Xzf|aHmWGJ^(FHQy{5R8({9SIjf@QK;q z8QMqa0-wscmo3xuZ5H@vn=&V#B^q*^oga)mlaO)Hl{nm<)w0Y(P(WDd+j(h$nppF5 zBx1hIwL{~rTRw^-+l4Sy`=PVY05}z=rjvIoST7^3KQ>$t0VAUM*$ZVt`8wCq(H5@r z<@uRll zeVCz7B|})?$_A(Tubht;0N>(F){&?OLtER{{%h}=N!6{Fc8o$Hndo;zxlZ{(axQW5 z38Jd6B!vfEJivnN$poqc~1+rIsNGaz+_bC(=l`Jh3d$! zm)j~$fn?4U3$+VP??QZVjYsYuXulxPmUNdaQquAMDV!2&(?V0IJop?}7SOslcFm?e zEnGUOT{5jVp!-6rJ-Y<4#=u+9O-Nv4rL7;TN-uOJ=o zd78QwQ*byOfa52$$>}JQr~LIBDGD~rfnbY&KGEU>{cP`;qLnh-C~)vyp5zws#=Z|7 zR!x3b2kVN~Bq8d23VRuXWXgxuCX-y=&M=Ck!6?iH9$Q}2iIdGfj(nL?xj(Q6DkahJ zTvpDav(e5%D4i^$7S@D>V$w73Mq+Dy)ZZtPJyCVqiN{n}vjYkIH!q=<*2j)vTpfDdkA?KyYV0ne=UDy(__Aq$0guy7PRA8xYs69GE= zYw!<)`R>^z%5kv<=|$GGHZH1u2XZlDXts@g5|AYgLCTKlFT>j}s65xBFEHAAmmJfE z48V9~fcjv$@7Op|r%e5C+8rfeC>#cKRXbO`|Km7cam?)b+>Hhgvj8vqz&GxYXEF}B#FwAs}{ z(gSngsxPhjAb>KpG639}MPwEFs&zj7`pG<4^5E;OP)wTk=+8(2@b^8NfC3~M6`kAa z!u9iU7YA;L(LSK0=W2m*8L))L)9aI@-FKUxZDStH z=1|sf8uCg)lqKS^*Gel(H1nfpg{k$r=4G-O0ByY&bJ-zC*S?+Oy(oh@N2npKzADaP z5=3%Blu8>OX~J$?k8{}f42Z|0vFxWHkFX^?z=d%y*N!e7sR1~oJ)o<(Go&847*1_R zi7m)MSPuYFoi%G+tgqXTDR4w*IAfvZWbU2A60P!Jfbfwuui$jy;ZzEGn{jf!PIMz&Xn*_mPqK`ZZ(4nBM?LYRy+v%9r772A~T!2b& zRD_|+El@F2W%~b=`w6@_ad&2oW!by0kbO)M@#Xw!?4Uq}j@&mDxbxz!h{yA8H3!Bc zdTV!#Scq+Fj-BDkPpJX<0thLGiSrN57Ut$%Y?cWFRD8roK<%)yEun^yY3W=;@B3TR zsS4Lo|22_4nX{3>v*~jgEKxfn-;H`5c9U%z+tX0)Yn~s;Ae|o;)NCpwjg;1$Y<0eP z@#v3U0Jm{x2v6am6)DYjgIsFcrKrBA3oqGh%00bB zXJARF(DQuTiMMIb_kVlI?{2y_VtC@<1w;os7|D?js9wfdlS{Bc>A#uE;Mxz1KU_{H@nf>&3GA5m$GmQ6rRMG8p|=dt797is8m=M3%b za+s3XZo9C0rf*6sl8m-Z&I*q^IXB8~{U1AcnCGoR-1<=z`BJuVxDt6%ITYLty}#eW z^ivXG4&HZ2O15eIG;|ytmv(xSVe!;g%09&!%Zy&s>0oa|7jo$RoVsBpiWw-_P~W5U zf}3(^!4H$Q2-P5}$r)1bWj$O-OnjKL$Q?i`xj1q-G?en8aZ$uWMRkQviWg_;B*pAd z54hc_GD!DmU5;daQ5Wo(Y$i=~&G64|bunuhm)%!qn|o-yY1s<<2e*msxKGi76ls^? zoH4UWcM-K{BV(@%jAHWq6jJDRJDJ+CMBC@DZtN>^dpxIs2Pgl)>N_Xe?!v)9X$wEM&AT(5m(8(50J8bz zk!zMBJ4)2|-T{T_uXOeY;yfM&Di<}o01R!#1wErIfpiDzgPM|0Srn6I#0!aw&KEIT zmka4`-0Kx0_DbLdeKHTO_7pG|K8u|H@mA}(Arybmw1@x*_8f`GVt$ICb{^TYXdZJ> zz_xWzWEgmC2FTu?thQpe}FBr#t zY{X)o%Wwk2oM#ItW)dKO-=8dZZ>6AizHqH%o`IdS*MyPu7c=PY(0|Qpp|NzAJlNcE zodtfm7dElhXY4N;x{MrmFI+-;*b9KcO&~BYG{0xpL&7z6$1sIw`q>HJfT=c_ACYFT z0Rp(VPvQF6^r7sr78_8P6_)12#9Rz5n9$t=_G{@`|I$N{mYl+))$?7P#x2>Ud^>huN;$G*ID4#QOh%3uuq|s_f@tFa=-lTZ zfEH=}t=4UO=d}a&aSm^-Bml3rF8Ogk{))N5VA}aW`D=<7nwb%$eW{QSsH>d#AzNLwB)(AWo{9PnZd1C*9uizFa9yf3e(v517O z!>3t6=@hh>%RujH71K_fL!&y%YxB1Ido)E)d{|Rs+qz=QqfBIi;b~YDx1W6xc?GH5sNWod55^X-PeMtlp7x^6{wlVN zv#r(4=fRZ#A{&5WDJE#cl<*|(Cr2c$810L5<<5e_@%3%?fSUBvH&f}8{KH#>d)frZ zr>tZXSw3$fk;+T)Q(!(1Gosgq5P;!7r!Yi@;J!510*NhEukhd;dxst6eq^r);rl}l zppEm(443#tMKNxb=+#pP`1H0WG_mKxxQ}anw&@*?Dq)n;aiCP+2wvFu8@zado{{OA zp*?y+e^>qXLtwl2gv)xT;LeT?kf6tzgAp9oI;iGSl&&W0Kd*{ zRqq&8xc{{sy;2lMF_2hQ*RjLq`pMKKq*goorsbh}^YCdLM{8wR$BJklP{Sx9C` z-ZQTW5RFlE2u-f3q4(_X0bE(HoE~INVtEytugMp2^`jfKLgXl49QC7#fh*z(-Mdw5 zT_&0k0GeHHYNJ0Yc_y-ad&>rbI?z_(h8pB1`#D#vkJOuEQvG*{WC0i$4acr(o{xn-C zU!ZH0N9NgzG$USb>~KO9F7W>+Xz2xY_7|2etf$u!O3r@fZXx@3qBP3e2n7oR0Fnzp zb7-ZFx^GqV?&HpK&5-SH!y>p>?3L5{bfG9znYT(I&V$=yuk3(uOgo*R&-dEyCO+R{55G|-&XdyEUhj)GKO z5k!igKImTS6zC&oTK81b&vPr0i&@5;MVFffeJEe-2&ing+8UH4-Lqzt;Z9f|_*4tE zrdLbChNf<_cL4bS=eP}l&`h*__Z2Dhf|sw>hB~p-2v~I_US0)#R-f5ZAbQ|6>NaX0 zu3QM?#zO>3gRN^Cb|;`-g@vSlZ@aZKF{ij#Ex*fNx~Y@#cIi)}`GyNT_6k6X-lOsa z^P~FmT+ls)epwlc3-om4#5}vX@-(&}XEByLcn+z&UNH-aqzqb6_Niksl85JG2ZKgH zQ%cj2!ujhFRSVO<#u`b0Aquq68Js;cQKa{T{yy4i*n?Ab4RC;M=XJpkn#?=6RU29; z-l~vZ+8)n?v_H*mb1r;CFDi|wq@Q!6U8`zazorZ-OQYVLyzEV5%TOxw zBdQ!qMxA0ZuH6M$9)}8y`hyX=gl}arg;%f*Sg|w94%l4K9eHI~Zu|(LVuas|%Zdu^ z4YIQ;YvxY2r*Y@H3Fd~lYTSud{*wM7srlY~FV0laq63#0EN@b3z@bb}5nnC&22Fw4 zFJJQ6(7w9CwBAZz(8kjKhi1mRnJx3jxE1UgO1yUBZ8!`j`Z@X7H$^C)WtbewIKQ-1JDP8T2Vg!waFAn|5+wbqi?VQ&eJiJ~ zDIM5)q+TKJ=cEf-3Y?$hYH^{}@z9?$TxU(~rG&Yi* zXy_U;*djw!J2UP8dYz*_@eOZ>TUp<4ypFtbqRys{8nM(?A9TJo^7ogTH+dzA1JLk7 zw^I59r=YHFaRp=n50=wrxf6`3;hq-m3Zinvz>qSsID%jPVqjRk;9))T)|1SUSV)5< z)#{+*&>5q_i&~7U{si-$g-%>QC{lILEYU4WhC}kP$G90e!>j>Ye#At3hq{gD#9XqV z*+2ptv4et&Xc%-cVfau+;LZmpwJidue+pO(gNVsZ{EJe)*sFTGA3MClD3v>NinNH3Dld{3Mi`eB_pCgQs&qf%xa5Q&TD8JYY zaH`)Iweu;cJYziqnONN_f7eq&dNId>h?*@$V<7=5r>a07vOXO*f4S}Q^hFjMzW3A- zbrZB?#xDhY@Ld?BK0@faTIYCrrHGz9Mkp2G7#VyP_w z`38r=Vq?fqpCv0;0{+IX1SS6x3+_}--P;EdN`vnkK{s}pjZ8ZJmJ-(!$?^H;Yk&QT zA^kYAL41y2Te&$m3wcG?r>7R8P8z6J`2QsdquAyb@`_mdclGz53 zXIkbp8n|YbzhMy-Pp-CPS8IySvJH;$BP>kQo0Rr)1vJ$wI( zBA%x>9`5`$eCf>+Ol+H=)cjfgu?h6JXXOsr%2}VqvmxH`_Y1Hp zqaB%TdZ}u;C%ZaIwGN3!!u5|9FIjBm{e{G&$d1^aIwIslj>@OzMONTGz0MYzd7Y!!Wdn`G$o77inO<); z_<_9%YBj#58eC)Pd_?d69uipf)?MGasbPh4k-1)DZhN<(mTkC;`#J9lJW)|yhxo;H z8++zkp>eZIG8+2+6h5+q!0d^qMpiC3aW_Gka9QbkBdEn@1?Aj%h+VoFUOuU1ViS;0 z1rXoNJ>0Xm51fNBTt!NYPOK?S?n96>*C%GegEylIxoS_eFXhjj!uEdodEw;WWeVG4 z{Jlu_)vutLTB`bvfIlP-Cy*D2O7vipamQuv)(fAXn1yjF$)Tzr2~6K`3$7fTWBACc zd-iJ1?J{LaU)I)ms*5F~j9%fjc`oJtNezZ1E~GV>{fe!LtkHHoGp?mJn%-GFg6Psu zPm6jCg4`AY6c0M+I+zn~G9$L*IdVV$uqCA^KjgkUaNsY)^hNu#JKbO)XSgzhEKB3ZFOLF?(4QN+Uq z7CXwO3>`pmNjbYOiI$5tY@fMDe~1g^Y9f!{h~Lst62-m4E`a=y92Oty&!L4QTNh32 zeB-@v{``*qIC~8=ql)f+>Ho8kV}EHq{O2%Ybfy*|Q%G|ER|dG~;U!xc?#S(;@%+WVjNSmedgswp`BlB;@M&^yC| z$bd_0DzA9jMzK=*b~Qe54UkRvj~o29z|pCge>#U@Np1Tm4=K+@x|i?GPiLiF-MB(u z2oyEk9(ICT1=>s=-omR+4siZ%*N#0dIQAEwY)y$Ti!e^N)rln&(AKR2UH_{b zz(IO$`3F*uevA#}!XbBvA0rD&XXp5hHpys35|vENFkEr+i~6gMRjilqwKPluHg1}7 zT>3F?n88@+dAEW2|JeKTc&NKC{uxV@kQS6JOQdW?ma$BWED0sXZl;8?i$T`WE}@it zdqQDmFp_Csca-`~GKzMof_&&+4J_nvd_Iq!4sx#u>N zmcmM}YR>7g_Pt$WI;3cYqj5M4dn{#O4^yl3Bv z%0;igca~Kyy?QRJV|KP07OcgQ-5fMIlRwS%?y0g{!zaRVv3RdxE2GtNdhN=}t{$8@MY7#eJpSOOMar89;PhUD`uV~vI4#-A|3}^rZCv=?1p&9JktDLC*A=@Qgv=mi-K_pd0ahM z@EGhe&&nxkKLwY53KQ>b2bk^#j3rm7nUs%drm^V*j*EkxFYNSqY}vg{f2>+zWVEz~Rlap0gC{xIFT3&({JH&6H2;F68j*Xi_N2B>fbG`&BLFwmxGk z;h>@`xqWdswHMuHtpv0C9FXO=`X@=%Zn7b%@Nj(4(Nm#pU*t(N1d?BX0e?`*q&Y`J z8ODTy24=&al{tH{9&THrjx7(s*9Kh6CyH7m3x?(?4US!ZQWgKcxuV$E!NgMgZUZ40 zZ-A8|K*GL6JU^1(3(6osT(eRx1P0LG9In*U-qpJ{ujt$^2Wm_tuU$VyYC1PJp^iUI}k@ScpdDGI2pU1=v zJIO2Xg0CcN6iw+)KV`OVEuSo-msd~XH09FBEex?-a+lDGd z=$j*Y5CXsh3P@DZY$BUwV`QybVi)5AM6r#HfW@jan)E@0yEO+W{yGzf;pL@EC52%B z+kOh$P0MzDEAI}w^j-F(>YQ9&fz9(zKgwo%r}IyLka82sjmTc}EOc>xe@@9b6Oo6X z+)HMd27VqGcIh2u=#_NAS5MM)(gkerdFyj7f_rPCI<1nC$>-Dy0Ow*Joj6R4q38n= zq;k4mRQh&XHsE3^pKHlQuUe_4H_xX6|Jzsq$kTo@(9O1WoT#!#&Bb6l@hyrmz*AidXAqp zH9B7fqH3qt%G!4U7mJ8}&}h#6*)-TV5D8YpalPwyS%?*oHwA$QGnBt5Pe9BGU)=#| zCz{DH*s#`Na5(gYYw2K&cBE_E5v21{V0X+P`xoy<|8nlI0ikVYsp-_uo((Hn zil)4HKHN6x97r7lQD+^bW44~{-Q9t-?&S#AIAKeiJT_QR;18%T%&u`H;BDTvjaYzx z$c2h+Fquk%H@ofQT#6-iK043&CI@IZTZ`gweW=;))NC8e)oZO+`Zy7?SNCyRNfyzU zi(>uUsH6ER_~Wt7smQ$fysFI6zz;NpHpjTdAxj(x!?CEuUA5X5b6X#T3taMgv;De0 z^=ZrZl|jEUi4gY>b%@_QJhpEnqaJ&wVkk;HUp(?GZq;4>66n6hZwxkgeng2s z73SUW{5e++JbSI|>cU+X+Gomyu$)27$EAuNfnb)>5G1AgavFv{%kJFpODD$f8vFW8O_E;gCU>*-c_SaQ|5sbL|x1$y}2`Gn4-slD}Y z=P6S|&3w2(tby?M8qGQ*WB*KfM~axJp|lUj$O3b*PzZ*5MO~Fn#`|#n5D3d41;F^EQS!NvF^F z4#l#I@#O_qYkG9le(e+VZ{^Gz%IX_1!ig)333ZC$OakZCOZ}{5Ja)J-S|q zbn&z9v2ei;0XRh^O^|Y~ciY>QyRKC%LsQ{@T~(O*@O=1F2#9i;isis_T=mnKFrO%3 zq>xX(#x_(7tymTrG=yFt;h76!KUQP;|0yJO0MLRsgVdfM@zJQ>%&cp)HYf- z&b=WCCP&$3hfno>b((kDcP2oykhU^B>9TLF!^p*=;W)(m?}=HKcC-&ds5dlky_0uT zP=6q!D{+f{u?bDeSD*od9{cQ4w(8v*Q5CX&?#~upOwcx}$S&EI<{1&>VU$q>+v4Lr~u8GogkGyi3$vU}3ok z{Yl2?HU3aF;3-0V%JeM>a%Ho|@`ENb14Uz5?Bw1*JtX==MbJy~?!cPhiiCm|zNE{? zgiVluBLX0`-wxu2r3kKI?hcL`1}}}=8ti8$As_rn4LtfmdHQ=e>o25yd-6HQQVC}J z(nl%$AH)2Y5{RywnlVklTNGjkB2-rAU57gE0QdK*H3FVN+#jDP-TXw{Ep??fF%uJQx&t5eBIE152_i`_JT)|AFtolT!CjajL)MshYl>F3S{TN-i;{IPiK6;sr&_wfH!m|PIFx@3y* z-%eRt$6J@rmlZU@#@bTmG9(1l%a~){gZnyL*vB7gt52hMn+XmL4b2O7|E%Em6N2jX z?lBvmtA$e|92j#n*bdM7QQLXc)=e^uyUX0u)6+qH`f*+m6<7Z4{o(SFv@l?_*5Aur zcj_)fCwn%j+?@bRZG}=26ZtwBruW+@fHtN50U;4Y5#j&FTX-Y21<=OBDgLf^`LF#= zxKlabiIZL-*JWKzAiQYs|IWmQWQytBC45N`f{Z|HrsxQ-0ygRQ7c(-5F^A`H;VF=!9Q|WdC zL`KeWpeQHpWc|&Tr>fV}deGnAN%*YH)X7o=et()!Zoa8=^sUXe+eSvfC|ME8{r5Ch zM-6E$kwGtI14s4P<`UrUr^>2m44_t0UxV6 zL4Rl9LkLV=`6^MqM8S0xxpc|Ehq$!)TfmCvk87Zom}fuV@$-b(wu#v>h3fO|>2Ekx z@$~BwIvwF4aD-~XRbLZ{~jn?hc8=>~??+EzX0*A&z-+_bQqlkXKkbZ*c zj@?v6`|(Ozk3isD3;n-|3%Mj;HfSb{4)R?(UuH{N78w@;wL15=w#JK_oEjyJ9LlK6 z-@)fE?}Usa=6U}mZNp1-nTqMxCh4M>x1LL*nQnaDgxA*vZtt0#Yc)*86-7?|`t<+4 z{f`3wqrm?t@IMOt|4D%b_;~mAmpMMx!;3V5lTmE3x0s?>b&o%kJQyO!bPg(cdyCZZ z9gl=1-%q}N@p6yl)f^d1&W~w>tTAD?Z*(F;_8){EGtj$wd{?OOtrI$st?|cCVzw(? zHgH|fH@wgL*!}M6cyGsYg^knd=e<)j+V%NT8aZ&jqj~ms_4@xCeNej2d;isYztygN zuE<#1n1%(`{RB=x&(yTd1c|RYXDu(XA3|{W5kc_VQ}iFgUQ{mCg7n8TB(QU-2`#s1 z;>&}>I76^Hswuk(zxLET^bel*pKl_(o544sbfnzW_TvEHD675IrCdG_$fQ8im*CQ_0 zACZ@x>l7f_FOb8DFifQU4o)^bJl{KulU1l<@-S>mHP$w~rnl+ZZ>P&&KTxd0W!1^C zAS=|~*=pV2?)diyOeOUpfqG%K>f*oGemldc)21m=wXyt7POH}n?Q)C% zFzNpDq3L6Rf#|CPVeG?|HY&}0TYp~o{iMXx2dB}waPp1h=SC8(Ve0w+i_((~PJBpv z<-syM%+a-&`}eo~{KdUo(4@P4LF7$#TjDVK=KpH|6;E;UAyFmw01enZ%Nl>a;`b9w zmvT`V2%)$WW|5cN+_(jj*!uZji~6_wG3*n2=`(@cBj#Xkn0OSVu}{gO@U!_a)n!d+%k?;6DY7KDI!KbX$Aou2<;{?U)9eoz)la=fz;#~IMam<+_U zzsUn%CsSk~dMbE*S#=Cw;0M;Xf7q3}Py>t{#o>%!NDjUbt|{7H?6X(1hs3e{%WmQw z@5RAbnolszCvfDH5T%ySEvdc%O!+*eRnaPwgVz=-`g7xz1z0aZHCUw;woxRxBDCR$CJMa zSiNyjHSY0mj4;Is8hoYleYnwW(t(-lB82t$K}`>_1t0Y92nEI zMDRd{z$Eq0B8cyo&!Ib$17EB5E;eZgFT4>78lUVgFP1vkIzjjOlr0V=8u|~(r!GEy zq=E{uvFHZQqN?YfSf5A^C^?r0aX0+GZbY`}MS9ay!N9`U{m5ZOFE6gMyzTSnpm7cj z?{gqZK9sG&GqXu;wxKTd`cQmc_L{pFcN`dhojxGh*4~O>7ng~uHH?>&I>@162)xQoUy3GgfMs&reGa9=H{EIW%y}(j?7rx^CGu(Gg zr?Ms$VZTY(2j0cyE-Cfy&d+U`F&nYH^oDbfexS%=2eeo^PSu&Nt2Y<^w~DC8U1#5h zB#xRvQQKt5|B4Y1h?QzA@qpt+EQq-o{5_&T?qgH3s&@mX%!DGD$q|F1mQMEaPB);2Zd6S!pfBM_8c zs5qE92}p|!9JfukJv*2|5yF93k^jcUV1mEg0mCVEpTMQl0-fRqbt^#Vg#p#)%D%B; zTvW{g_x6i-b5^H0*Qe3xo{;sAU^H?Tih(?Mo0KG?nc9Zz&CX1L!D>GRRV_9BR}%t7 zaY809cdx)Ds0JK(U!_zx^$Buv(9$Sf5rJB|I8lcv7yrQ38_staUg-hnIL2<%z>mXO zmwTguTPZRk|DX>&AF;+CI6{O+au6ViUwEM?qfIO=U0SYQU(T&wTWUGNJqiUmqh0<` zaE;790Tfh*9}Kye?)iVQ4u<4Gces5vcL(P-b>bc_7d|HDL7z!3|LX5 z65E^8l&ztt{8P~qcc+-GE{^QL-r(-!eI{6^3V{qeYWmrv( zD>fG|V;OwCzAVlB{JXa#2V8Rw>ci;jMl3+}S~H1yDvAL@94e$72<4$`SKP+3CqPI! z9v)<1f4FdKR!=AzZ_N#7Tn}h zlBlslAT7uVkj4i^)!S*hx&T25Z)jbDk~c^qO2b8DPhRN8by^5?j?)ZP+u*bz%6)2E z9xm4=c=ZJ(iV{Y8MjCEI+E`e_si0)JWPqG}yl#c}hmeH8S_jvCNk7FRsB%pAyPLG& z^VIv4+E*$NOjq%ta?%9 zf9bcxo&Hy-S+Dj+T=7mJrm{CdA>+v|B#*H%yRj*dvf~Mb6DN=BwoQXVh#$*eb`*Z$ z*e7sV1Plxm#|CxWoj7qI*2@%qvd}KX-wiieUR5=rC{&(pei0RYr-<=n@XBZ>*@00J-?&idNB1L>m9#&;D| zCBM};w*7)72xn8w1b|L2y9uI)s#U?Tl?!#VLGl<^pRcScbG#w~iL1^bv53d{>!Lp~ zd4`0$MTaq&F1ABZ7k`3F;%oJirB=!~#ZFR(34-FtE$P1x+W6uwqT1AD?=SK@=I?ju zjO1Efdfe^fayP0T$pI^rT@)S;zVmh7PG>y(wT!|>7chr5tR%v9a-juI!%E%L87!Pc zH0D~DYiYNSRNIfH=q8E^(kHP|{23^ONL9VOlJ4f4?P7 zf-9&!8!D%rKgDMC0#%+AaF`FfLJxyf0D5e(#3>ys4_+F51y)o3JW!%tS$4N%vx9=b^d{;H=;BVm z(P2sFEGGOCDAtr(0)7WQkMFG$7U0-?h!UdQl{!C2nxwp-#>Vzi2KSI++u+_~gV-qg zYl$gkJ_Qnp;IWbYe8hQ*AQ14=HM7w6k_dc{Vx~C_U!{;~PB_ORG(P!3uuPj8LD>od z2Y(S_;Ui$DFhGrv0qia;X9NcWM2h?cXd?&X_53nFXRuI`ck*=$RGgl8U`SI}gm7N1 zY-y-PIEFY$0ph#8{7RtpIS{XYj#}!`La!5seJF_3YfKYLVuUE(OxI??grkO=lG8<{ z9^W-}!`p09C*W@9t2|=FSkLw1s@%U-BGE6rJJAckEiz@q7IJ9nalK>M<(ygqK( zy#gpsX*~l7%0e~4dTbz`E)R+Oab^cg?$0toc~aXZKKD-Bz`KQd~Fd%rK_wh43tm5W}`bcw6x6?0XD(` z53Im12!MN{k8(Zz)c7+%f+9bj8!V_N@ zx|^;)2&}`A9C8lB-xUv?BE{4=N+4+r$UE=v!W;q#V9x`&PJm;~*{C^ZQXMct6T~02 zmXE)vhfS$9FL&#y;))TG0Av4Koxb@1{L)&B@D|^5RNGyRYr~E%X&rp=W1SFCz&K@c zSA%+a=XrJC8y%J)Jpmm4${E7@rIG}qElQ#p6O!xw82AoFiH=~Ut#DOb(NJezK> zNC|)&w62{;pDI|M!u841hs>e#QLMy?4vW=HkQs}Hm=faq?nxJ|k#qs3wFN^!cdi*-OTY z%{WLT1RFKUGhnjG=d1};u1=N(V~7AfD%fR`6v!Pj?&N=+I!r(F>mu3|5^9 zfI#MZ!%1&RZQy4Ky4;HZa?jlXY_pLE;+0K*AkejE(j*+qczO9!oxOzmdNAp3qI!G) zO`}N_56kr%S*q6tl5@((2om=>(wGxix{5F8jUx|h6u;qwSI{bk)zb&05@QjWCT{#d zP;VUV{A>1sP~t&tgkdsjQXlAnn>bTNRk%-&poMzb5|OC@ic$5xKz&4|mf5d2!=L)C zEtSzqu9&#Ect`ZT!R7%48QpAL4aO>vc&{DkYvITdIe<-sF9PSlio`_8HacX*gnun1 z&9ITDT1m~uoOnSSkj}vr9kguo(2P1rnZha=!!6WHVK4pMscH6+hAHQ}ON9I(#0vGI zr4BF}_BwaGcqRc~e5bxP6Szn}cPJ$I&1+$l%<4|kT zHb=iWLr~4*xdu+&>JOGpdM~WL`TIC2Fwq(ZNPy@*w1+MCV?KoF4(Q%~K^6xxmjY)2 z{hh}xEy<7nYR)aK*-O8}(cm9xNV+#TA>j$?j)?kW)mla6AXF zbz|POrp&+OG;IdssOg0!kc_>whri0IOA`7DG@2WADN(#L-Tpu%%UNY1NV}?%f+~Cp zWZjS+ve@hMRU3DAI~hcqa9=dJ&zzS9d~Xa81bsK5`Pzbn)f?L9zxyX1fI*cg-ZRii zS;q?Yj586ymno+XlVpp^w51fvVcP*9ySfXOZ~#%AKq%{}e%<7T*CB*IQ z$VJlDT@97b{pxahf-DPOXGrp&R|lgPLR?!Mpzz^Rb94C!>+=*6Bk=BGPFnzf@GRXp zHc1DFEN%XKmV%~BAY)}`;!Js$U0KiC{HmE>zUJx)Q z>16(K#Qm)&p0Hd;iZ1qB{z%Ca@5DV<<$$2ptt~6DOEI-iQq({`YD^#$f;x|QX60@Y z0g%RafXT^R@<3Sf-S0dJ7P{h{+ao0H+bd0ql=U?P9W0~y z0f(Ff?ixQxj18123Rzj!ar+}(eq-LkvO5)9#%BSHiao0!1c_-Zq6~tiaJE|-u^*7AM}GM*%W@7ntlA8op0iB)LiX z_6{SE)qU#p=F4PP9fj5KgVHf7zziZ*0UVIsdE_69-TA0&@|DR#k`DqIcSQptm8cEE zu!C~S>ieV!SZ`A&uuXtdt(N&V&;h{OolUi9Xwlu(7^sI{ov2u+P@aU&<3}S{AaQx! zV2MX{6;Zv|WZgAn8A#%^WH^lYvvlVzrY5n{zTGb$it1)lsogSXkpK$CL2?Af-(^b0Qpb*e)H1|c#eStTEJUoSWIsU&sXRXCj8Z6n}~ zLq~R11e&2Qqt~gCE?~@7=9i)VaelQkrNCOqh9=btEzS_LEy%X42L1UssCSwjhEiXjJwg5GjqM0};x*h}nZ zw6!LW_YU!4W4AZGcqCx9dZOUyyTiYt{XjHOPQlgYUs?o8vpZ(L1z%2EH9P}!zSLuD z63ux1!}v;Nwz??YzQsSclhv&tl(~ZOWOfWl#F#gBjWRuf2x`~ZlxkUr>^YlH|gNbW7iN@JM(M0N&LbrLo5Afj3I|8ntoS00a|XcZ2N7=yk9$g(%hyD`@P(7spm& z+lG(;v&c5)E_?YL-uNA-)>0NXsab&jG?depB0}w}uXDm3uu(O@=FOxs?rLO#3{8oW z#;`)xM4R68t7L|GU_YH+{xkwvzS=R%NW25^7V5gS)hcBPmb(;ce>+sPs#pl5*ri@z zoTtz9VjBDNN!{Xc%SHC56(EF16JV>ii7XEMWY4${u&fX7)WDnX?XSeyx>CROaa{~q zEh_gjp}wNf5?2$4k;{|z)wuP>U*))LWy#gtL?3%*CS95A zl1N~K%+4{AGC^_?Rh5Nqu$uVra^M9p+ihqpVPcpo_~eGB8FMD0*CH9NT&FdkK>3mm zEZUz3(Z=tcrYCL4BA^<6{tk93ha=8i10olU#0R-Rm-){LRAT}`VJ$NFTp@u=aJm@T z3jUZ$r@DH7X>%~9!MalwaUR31S30^ z>6zQk0{0uyacP#qCB4=^)Qp@CqL)gDKYKpP*8)8YJkk(UN>%&#YY1@tUA_shq28~p z6@*<+D-E``egVoHr923IVVtZ$*ZBKV^VXc9%W9XP!1W0d;sc*5T*}iM(oO(O!sp7G zf^Ia!KwCj_m+;VwbQbTzx+S3|ukLDKf?y=AkgLZF*qE&3mFu|M6kV~Iu3UE^&*6*u-3~wK7(N%2h0?ea%$;oBsl;-ImYB=AeiJ8N9$87K~~Z_ zDO7bj*hMYXjA-*uQ^|RqM_!TT#pAX@AQ${DP-nmfZhRIF4KnY;KloRxUml zmy6Zt8dtpCIMS71UEtbNWmM@Yv_|g2BAw;~-Ftx1gsk+q0aU$LPxtunQ^~yET(L)w zyb(0);YD_%6(7pDN@K_2aCV^x)E!k#k%gte-Kz(60bE)2$s%CqhmUt8QIP97Ms?^O+$ayZkKm>jvR-zdnb?7BLbzpOkpL}42n+XZ$6 ztZPH2Jg#mYk7qHBbesp7a2o|JRf;-RM=Gg{_L1pHAe9Z3)>suhm7skNGT)J@#nP@_ zpTGxEq;Fr7_+}WJinXc6R7NbXca%@bm1DoT4bUE<9%gJf^G+A2l`P{PM0o<^EZ) zTR7O&_~*Zjp;ksSp?0qJ2dt;EOq*Z5(%iLd2t{I7I~4i^`w)HNeJ!v?(!rPmH(C`L z!A9I>b@|Is+@}chO056`H)yWtJi{527S8r#E$yaW1Gd_aWW+pdMb1*+Y7QJuKA}mR z&WziuP{F{3iF<9_Cy8<{A1KtE9jz{e)bBt}6^0U121AcG2!;*?@cG^<336QagB#p( zJoA1^HiKPv94d93DL|L{QiO1fu!o?+HDoEPjydgEN10gb73*{D%PX|q&c!VyeKr7I zBtRWc|1`f^B1q7lXa?IpCi_6!7<~F^X<~a^tR@-CvhZlVs87?|b)RGmc|hIeX6&8L zOz>F#AO)rB^sWbs&Nq(Gw`~{EXN)+Y$cWTY(A$xuDp@t=5mF7bG zYv!gdw)PC~m3wAg{^k?DK>FEibupM?MfIFnI%xow8m=1OvXIbJye~X?WUub zsD$Y%pP31UXzbTtux6V{`1Xd$7FV?dwcK&>WFpilN`~Lf+m8`Pil}s0oaKq%t2oEg zM`CcomJMQD?&Z;*6F=M-M(${T;r%k6fvgvhvu?4x`sBt~)NaAUD2`J*O=NM<7OY}m*l!v*#QJEsry3%VccugW^89)I0Bkae1SCeZg7S6sUV zN;>Kj-bSQpHSikGN8LMOXQH0m6Aq2~1j00icU%mW%t`VM_=jTId!?3|z2I=eqdbk1z`#W)5|O*ddOIPK z6}3-C;hu}rdv!+NNY*QUY`%|KiPt%N1$mLj8^?BpI8(j(+^ueSTu`~o%8xG&ymu^* zs=&Ii4RCSwGwWwa)Bd0)+3eM|#Uy6*hvR+qnprg#@lNq3LL%o-4wnr3_7WF~8A+#g zyXB6yl$b(W%VuUC-&5b3P2md_4h?q<5<&S&h-A6;R17wJX6+AHkM0bHpRz()bFQ$* zC1m11TJW^9FQ{FA{SdAzWWVKHDpLR}nVo!=Nxw1ZpkaEUJPhlU8s1dKD&%k(2Se?r>t}C=)Xe7PwVE{} z?l7=?j>sn(-Xqpbl20k#Z_%9gad@^ZCn@#XTEK)BQ<*L{b#KkGrCmM#V87t;sEp^A zb<)I+#kSNiWS-bM%nO|p?KvC$&$u=g3KQd<&0(@V19(uXrs>?jkKH-BG)7B~L7Asr z)l=ZVtYQ7A;Fjkjzt&_HYWAjNFX0ejH+5oGF(f`Dw3+J*V{w~Dz&))Y$ts!V1HAIj z26bkykC@iw-(jfRp+9_K{xKyYh$yTXKD_Jc7B`2SEAuf-RqW&113@xfQrnUY0w$W7 zf;i4LSv>lr5(cq6u+QH(xmIUSR0#uDgEum5hNM!75xD10p160e?5KH_cfidOt%`Wo zvlY>-dLU{^`7+si)%0%7J=+#_W2r}CsAvn7 zGtfC_*4HHekO#6vb+9yIZ$ksIrtTi0MlaMUzO*A`37)>!YjF5YF|h{6p>xd-?sZ;@re}F95o}zp9q6vg$=%*+SqUXitj#l(gSZuvtMhu*xt@ zY={H~2`S$p>`&dU!^C@1A+AS2nY+EXX#mGRB$`kH=j1;zaChm1f;N0NFAKkBC+1NT zufZ)P4Mu(~6^&SzI}C7ZVz2%B`CSG=liT25;>dY)0DPPlZ|k|UD{(}F7Q3iLuFMbs03FV)R0jE zqBkt1gC%fcLPLiSE??JqYZ1$;C8)==9z$JrFcDdx+4XHlwOg%C+MnK?A!7+GiDQjk zh3|v!_dw_|4DX~xjoIg(j>2M{LvCKU=b#lOCJ8;Y^Fa0@`8<)|&SLz>M)lDrdYNpi zVo_&-_>e3fC7#!uy`>xPbd_m%7rWK3G&2gs>*1UIlUbj3q?Rgq#!@cspH}Z?^an!#KPne9{og-E5%`o88&#pu~{J-~b)FoO3WzkeEy4?ei9?sc`sw|D$;dYcM~{ z%1zb`!Kgx!Y(EVhwIK6Tlg4;`44je1pvrZEXd<^`L2;VjU4?L_e7#zZaH-{V_NmX1 z+$|}P`E)W(#R&Bgk3HTk z9C~5rcbR(ZJ}!j)UgP;jXI6JEu-~!xKezn&YsA5babt+w$v(NNzfd!$S5_BW=+j1M)bXhMVRIh-*)WX z=A7sru$p%}6w~7n;`Y$K+B<#UHvVm77Bxt~o{`wXv`ga{C0Rm73pI&Rk`0hj~g%Qn$=QYj<7@t^d3haDmL%b%mQH7b3_|rGq{y z{3;F%FC)?GPuf5DpPD>*wCnzrfd8Nwn8OZtwP}%QPmw;|L+`P~Wa2Z;zb`~@ z7r!AGq`5@A$t~jiMfZ}vOKQ#R;0stwLvSPcwvH!VE37l6;0AnYjLn}sD&~y>1?PH) zn5*>Wtkv#Om_No~laDoOyU2B}?gES8gG1fSJ)mhp5u*}w|e+GU= zt?GR?&x{&nE2^!baCbI~{8XMc9!eY~-Xx00JLzUKD|3`Q@Oq_wn861Ud!h2LcOB;ZC2Z|5A7LX^ zbxh^XaCNMhZBYjoZyf4)9kSzRv$gE@aq0x%7DWC0%npc^`~n zxyExOOqh5SwD;4+98H}*dE$YZEz$O^EG19c$HV)D+?<+JjU-11$ zr^M5xKoSruYhP;yOgr{p3Kg|!Wn&2yQ#>=bq z#D%oABT-_}4FSY`#A`&OLR&7>2^6$njm$E_5@4n(zH_)om*`LQtl_(D*cM^BZIwr- z@hP*}PP0d>3+6XU>}_r>EGAJqOND00m-fxbFAK`u_HCLAnrOBW4MHFpQVs;&>6IKu zit3V%@tQ%+QsVPSZ$jIK%u}7VXRQXw$*1ly6tgLp*wGx`E0W??(GUzD@U6@We(;Gs zmK33t&(@-F+^E!wMQsa^G}4~ZD)`e1j>;PUg(}C=8t)Mvn#i|39s0zxz%{KeQj*m% z&SV80KRvfP5`lKIm2T@!JL3#--$f9f4j8f!!#oPDt=!|Q`l@6A;~zrZ^?;p=!%62- zpbL|g!CcX~xVUbw|7Q6{~;h|ePfh+Qw$ZjuPACW7DLwmw!Zi4SYy9* zq|d8QPxIUZNrss}qnf1pTiT02dszC7OPyyqC_ zc*2$O^P#mae5xio{k58gcX)P%_nx)5*S2=y-Yf>|#x(7ZnrJ}lI0^#IQor*Y&MSs# z?fBeu)HTc93&#jR>I2x~&Mg<>>n)*6smQ#>^WlW7Ev8gVSNA)`CDHdZ3zW*H*zuYQ zc{a>uf}r<`#7Wp%?|3D8xGiE{L;S{0A*=xB&Wp%95ju{VjF@HyOGZnEdV8#GXlF~& z4o}$hL3c)XX*orpu*IG!$`Ygrq6BrqIjAQGlOj%FN0=6{=JXZjX+5WxkS=gs7t|7} zXP0%6tIxBL5w(x&Xb@li@!6-_j{y>n>Ra^&#Q4_c!?;dbzhu5Q`_{W)rK5nTPeg0} zJ@H^W8o+?}Vkj|>h-DS7Exl5H46$5O4%MkZL;eELuTh}pXJAlT3y zn{}SwVD9*mz69J=x9^&W?^$_i&XxPDe?7U@5w9@Z5n5w2Xq>yHNld@_GdD`8k7aAE zt2kqbo(|9J?An4o?tAhj5K;F(oSaiT`)tpG;p-XF!en)^0kN5PbogtG23DbwCneT*rEO%-!` zzp>dzT}s!Ioq6fKs$t%+rlF-SWFwP+Q?V8F=n?!V(O{=xo}XvId$ zI|=QmOK#)Co;)8hP0W+bf>V9HygIM|q&C6W%jqO!zEtlO{vbOS^8duFl&aN~v3#ARLX> zI*eo5$A)N@wE8t{CtBQR-Ik4o z)-WU&ordKxnsS8&)Y)StpG|~J4a=k@i-Hl%+?(zx9D10s`i!xj>4b&q`>*y`zNiun zuKk6oGZt3j^$=}R1WSl~Nbkoaww*=dJuCR)sfS8p`J5hs1Q@|+!t6Gy?)ZI4Vwfgz z$dfob5wzaIui?sj6{%+#!nQL4X~!6X)7>Rb;~2eh)A)g4#$rd{&80hH9Q(Q3h0ato z9ZP#!Q#h*>N+_IaO>$F^(NCDl=6lB%)qb;tWqmh6#3R?5%j`wifIiDw<9doiR}k{^ zX&pTV7U(iG?5p&%3o*hnkJu z--uA6hGvOJywurGK@g8#KW=v}xg)mvQDUU}n>Ej$)Q3zQZ8OTai5bIm0R;7n2rzmz zFEPKC*}i@lU4zvuX}ea+cJ*9f$a&4wOi9m0|J9J$&`MZBdBHvPq~>86O?`*+D4eEk zR@krMU17$z(9aFv>g+sJwqbvbZf|EAb)ucEAmZYdrgPjY$6pTRWY8KI2TTg-b*& zqJv}IzT?gKJ%hyu-F+u~SxcT=;a)OmN)t0NaziQgQ|-GO3P06%);JAaC8}sH)Gy%o zWaALbMZ!0Pl%7_#Ap$Xb9JT!;j|}gRZ6O+lEK$@ z@uKE*-%7mGXqF8zg~$`S9K8$E_5zWSyH6|0saRaG%Ig@$*V~Ul)Ul4cU~&DXq-kGk zC-ISHmTyPuqMMbf0dvp^EBV+_Ct+RB7kj3qMZB`Lhb$-;nWp9Y#QLkgI``dvKm|gM zb0B0QBt1mvBmWdz@eZ0b4k`q|;x9ruLqzH!`zRG00%7OAT1_X1g z2TS&qb@sMO00hb6oLIi&ShBiqfyVl=zPzoqE>l}BRR&_)#slTV@(Vt*tmzCWeKrl8 z+wUhhkqW?szLOb%4M*Gz_!yKf9Z&n#wYwz2*B%?uaeL(P@-kqV(Ypr+mXwrUH{ZFe15L7@1l3D1+h7VCsgYx+vOZBBa=CX zs_km0Z&jHb0Oooa6fR-|uV$HRwg`tgUhQKc))Ui-du#fqo_uLdo)LEUcsSg*;j#&LnwzF_it3EQ7>_6ikgdRg7B&u1jvc4mahP-G$7%d(U(7G%+^I?f9i= zJcAY617SthXmz?{Urv&Q#LUChss>FOK-@~jVgDttQF*Ome7}itHZccqc+jYkA&6jl2 z?kfRHE*HC#a7Spq}E-!&S1b*@OrGk;ID_a z++pB>MDA$&Abq`F*YO0vTjr?9hC&@92RsXKKr-MHOJhXl7&yT8^O0#5uPUQ5hP?O) ziXIwnTkbOO4a%JSg?<^Deo(kDL&P26pXn6#fgX^Tb+;&otL|J7omZw{TSGrcX0zfR zx%Mt(X%29!x7`5#+Pb_bYKUd1;7)86x5)mP`ck$>pXQ8M3g9um0mowAh6?W9qJiUb zzFT>s%#nAx5)jSkor0h1TTdbj-A2u(RBVmT7?MwV8TMpSU%6*;Z%xXmeKlV!xcw-4{`94BCaRUJ(tX*OJ!j z{AL5?q69<3J{(b!coNvRc8q<(eW?50(|@Uqoxa5-#3_|p8uQ|ca4t_UYXdk+wiDDT8#g zUAanJPUXK&Y*K4lO;v_I5YO55RS>+v+=Y@I?ecCa&x*I@=dHArvYo%c8-Imd7uo|o zT`J(qXr|nTgveSgUhCRRtSSwQ3-^g%EDVZh51!+gDNJFiFr53ydwh?7`2&p2Z08m8Nw3C?w81m{5=pOpVFNM%i_CDNu=g#PiIRktBV;wFm7+ujxc{@Sb1Pp z-*s^LX(of=j;B|5*{f(=9vZ%8^Oq18?~xm3yDH!nz&c0qR*pIXG{}<59Y>%`(B7$V(B59;0JhH_PZ212C8cm!BnDmDxT;3 z1?@mB1lWAaXLcaH&yGeUwiVgVYa+rM@t{CwEq;JrLS(pqB8hV$;!%=k8$JPZ8{f~o)iwB6}#_kxS72@ zR0Z$qeV*{i@tjn#RM^_uXM(Hk`-^q{(WkJipz<+`)Ugw@6c`vFCAI&!DUs2GZwc8R zrg6+cO;{?#1q$M$@RL;R2(Gs719+va>Frm)ZULRp0J7*JOWf$Brs3Z7PoPiZJ!;7U z4%cLvPjWl=Z9Hc*FL&@OiM4L#3&o5jv|}h~YUspAaC)hQwMwcAbuUaLXi|e1Re@%w z%uRxwfASz}3h9l%BHXx4ne|9tB?F^Ng)eM)%Euqx2ITV5iS zwcisla2fc)yT=*EZZJGZM~-xTe~U&K+Xs(VQFd>^7tWj)WukR|zB^DrI#`|Dx7xg!!|l0<1PpW)ut4dU1vx&H!RcrPTg$LWDhRWhy(93zmM(q{Ac!V4Dj zc$eF50%MXS9Lc!&LX>SC^Wn`3s)}dciF4z5`VVAV=p3yNYiF@g?6G)s%z)|z#8uBk zGo@9h@AJwPwX4tqaHgfwi7_40rxMn{WUo1RbPBc*+et~13tjazOzCgK4q~6j>IgD2?&Y2)s`;@mm`@*&xyYy3*El(N1cr*uF`k$cY@v1H!jfFsQN^`B zV}`M$jE3b?Y31EKSs)qI&CIu}1Mf15q1dMj?1#4H{iA|t{hb_=qrbDnj`Y4A>5FdP z9nfjtw(isD?&pk(W%~Y%wxGV{S<{}sHa;RbJc4o0^`5^-#+3aTGhA9~TYWPN;n{lt zSrX+rEqidQe1buY?FvJ$Q!McnB@?d6{g$5VWgpPQeLr$Z4KrGE-Jy&nWpqxp{lfzr z1`D-d+hrPQfQpI7A%TK;n;sT@*~idp4%@D>1g^ijVs76kLw%HMPrR+?IxkHzfd}IY zZ(k+Lg+>XLhHB%{2iz9~s)aL!R0@3y0KiIU^5jA@uZP7=k)6|7aknr41NLtuwo>It zdFi$(iH6A2jZ&&Te)H>QGW7A#+~EazkzbDLW}s-3<%ib`_f&_41^g`vM7YKnsVpdU zOw{ZmfNuDCwZdG}nBuidG|!k5$KiI-n=T7AeN2_wxeIxQ;;4RS;d>UDl)Xh4c9NSx zN3qn`Z_w{?!nLeKps}dwVkp=Fmlu1IKhA>FG@?ShRn4Gg`N3|VO>KJQFdRpm`+l)$ zDM#fI;+l&e*-FUxwJ|^Q2(2kg3l>9x=crzaM5VxPcC114IRIl5Ol6Tql8;P3i_NLoVgtkZ@py4S>i8S?Xi2Z z2bLba6`oA77YAzW8V-HnMLj}eQq6UmJ*q0V)r*q1LGarf)Kz!pZdh?z{#akUi+{+N zTWf>6rijI`B)(o8e>fWY#sT2Kx_^)g#iO4F4s8ptt!dFm@)jC|49B%&GPg18R?IEf zt=PoJ(UnNa4+3PycFNzG-F4|-1jxyxUI4x!N*R1b3&y9Oyh||zR1VqyxhewjAe*|` zLGy;MLW9shfan$ESYsve%pd?|gZ>P27M(fZ5>KQHu9`g+_eHtz;mRv5RTX@K( zmX-TemUs@I4RK6EO(by{HEo5`R#_HTfAx?NdimCP5NnMmt++5%_9XtfK&BR|+PlIf z_V?wxURb77XzvT0IhGf9NL%pknJCLm-8}g5RoUm^fmFzPI zS|qcgFhW%=UOjOKeJp*s&d&h=Cuk3EIMASK8!@N$l z>FlXS!GIV-y4vpOn$X|5nGYu-lJ_HUud#`-+FaIKnIF|$%~$T^Ed43-H)J`T#nobW z&;C+~!_`KAYvociA96cWa(RtS2(6ao-@b&Fe1v#(Lh^9+Gq}np8h)GCa^{in+g%+r zLzPgq`&RF(p};=p^V}1fiPXOwS@tgDwt`0tNYNyMVZ-KL{wHH|fuQ8NDexdm-_zz{ zaFP_dwH|*s-UqE@Q}fgA7C5kt;)gq-B5u*n)kT92Sg~32o?TYZMZ2hE{CC76*RPpcl|2B@ISAo_fO(T z0Z!C`seyNot~yo!$>9IEeAhu^MGgb$S59*Mdxuc5>Bh)l2Bg}K`nk?)$}(m?=pRZU zf+AJCXcaVi^`gnDH>nScYowJZO^oePzYpA*YXtL1bj0+W3v#=jd0nh{AKT>1N9HOH z`?!+h4S3SK63As}3|LmM;FHa#w}yi(BVc39$VDZ^aSrWJ;)CZqvUdu2p z_y54t1DDq%;dbY$JltVyQATfWS9jf|%I?{E;>zF@m+m#((Bl8k@XKGM=vcb;{gVhu8=YaG4JK?#L$c!O>b=QZLZ= z5_n0BD`Nl0=&6ZP^!0qYS69f9y0BJ*nJ0`IUX*C-l~EeCj0R`v_=otS>EdR{GR=ew zIe&YHas#u8Asdq&MyJfn>ObMD8q+U{@wtR;!(^W~OrGvC&DX2a)!mFvH&TY-t)5E`p%SIh!NSqQ^Ix2djif1#;G;~TiWr$YHKIV9 z-3w@^sbv8ujTYiUA!`c!Ui6eZNTr=YQXvo|y?$V+3$JSd_Zq+iSBSU@+^Ywi3=vv3 zcB&PGHxGIEO=R^~80syr2HwR~BYJvY#EvY$fV+lA#gd+zqf@gHrCh!u1x79=KDSHb z6!G3gj6k`kTx5hd4{#|zZmkt%rq7&2sQhU02R3fLRtKKJprqp^II(I`1YXZv0(@od1)}aQ&_4JA7yengL!C1=spgXJ-;{Cl%Pgsv%=XQNDiMq`H<4y;jf6MTMu2msPTuYy=5Gbf-D&QZPJk2 z&?49bD`Gp1N5mS-mt#bbR{H;ITURlmje$~6n5`fAq>grDu15|yGzt6@T)*Dh#6z2I z1rojCsL#-3LPdQ+d)`a2f#&%7Xxe-*ow?Fv-w2>96#yX(^=(ex+ju{9*IuTj12}SZ z4&Y^=JpP8Xxv1l%Bnt%}j8Sm+YCYIo+vF^GzBQyz+q!O(|`x0mbgfVSrt5J9( zEOqhD?PC8Js>pPY<=)V=QNO)L!_ETlXG#;PZA@v>&=w0GX4#A+@m; zpj|z3V#8I>-u{-9MU%T^LM~V8STb&D0rSut(Wxa;|4IEG^btAouD(ZPl=gWS8z@Q2 zqhbxBEA1hxTwhIs8;XaFHGl0Q>eow8G$?TGAga zwNditFQ7_PKjLV(Pd%P)!&?ZgD7gS(e!=WlrfsBj?ylyfK(7l6-Uz5f-nR=i>g&0n z&Y}EW-+!P+z5cO=aVVtD)&_76y${^A_De}4?H3;!I76UM@x}<%aECy(iVV?VLJ5BV z%aD2^nnAX_l{DMFl!t4_j67}-RhI|+R~53yBJVP%p)noh+8-k~?fzU-A7nJj&SB|8 z0kqL?z#0-esEzaWwWdPzbNW=B5h`wdpvgOPwMl~Q+Fg zM%smYjCf~`dU%8b*L(Zvt^M7Jhk@@A12H;wAT}Y$N8;)YUVC9cHjCh@m%4POo7G2Y zk{=n)sMZ|gPb^9k#deggwXFSAn!d3UP7ZWki+YxyCwNetfBlm8l+o_R)!S{8Vtm0CLXegcX1~pPSa{v> z4bdq4r^`z>4RlH+hk+*7+!v+O+phj=yrd}7N5d4;%YnBsAFhzCNus2AeANRU1_ayN z0YJ13u!$H-BPh?Za9HoTS9!$yk3ceI|LQcI0{qkv{cJI-lvjZshM zt_=bLi+4B@zeoO3<_O-q+a2pqswHi;t9uOa=(thCNp(C`J}dUnQEUhGRjvHJ-TX81 z{P`O-wA6;Qz~co&(!gs{3khlna=qLsWYYViT8FWc__k*TpP-f)OL)^4*@ zYRxq*+uo96i74X!MBg6t0c$vuOx(Z7Oa*>%UcN7avD4# zM4bu?t~)^^GZv{3y^Cxp+hV`2`l_`z-#N#@??el3DOEqdJoJXnKgo1Osrd5!bQ39f zLzL2)5O;fB#os9~=z}^XJmTw`x!Pld?fc_KT8|g~ivWljh_shM4!vclHkNwmj(?m{ zmRw4rHJmwd$IfHzaH_Zwtm>c8&vpQDD`fcX!*t!P3n&ixe2;{Y7tymJU%3qTJ|h89 z#P_JL%su*ttXga5U8`5>4AG=~h~wg~*FSjbA)PFKm)0&lV3U167Fw!ZKxPj+FioLT`S5$dpv$|l6x$*z3!o( z<4NTT?kjd5$O8e|SSj3)!mvE7fpbq0Ew&!W-jRJp)A?oMUsUMH zm8Qb5Kxrf;OCB~;Jam$?eQ812FlBZ8+sdLdm0}<2KopMkmSaCOA(Z`& zLb;GtruQdF&9i}DI}EKezGB#g?$oetOp+r3*De86=r88iPhY^F;^n%3g|=Y1{u2%y zMM3VMHi2P~iP1ayJ&k_;A{q-}KjUd_l3lEhg)LgLO10lk7zT*78MX0z2#JJ}YDljr zcqC1>IgrYAW%C`r!_V+Ca|Dm|LCT+ub-n2@t|1q^_Gu@J_G!S5I~R3iB{s}gw6}1O zHoLsvOep+fq7Wo0Zcl9z{@EX!6oIjK<6cS1kny+@b)#)(C+I{On3l4^x_3Er?FaFU7+A5LTXW53x zihAV|0EKfu%brd(S$jyd%q;*?tuOMJD%Sgj6fT6h=K^dmVnr<{^kkjY6=U5G&TGMz zdnq5iBR7gCqgXbP>q7%DLx%ZZN( z<;OAq*4d>#ZcI!2$4nCWXCPG^}8voI~AYm3&YBSMpz8x zs{2H_VrC3y${KyZ(wQF1QUDLa`^*?1+8>XO?=AreBX>XcIH8o6V#0}}mAEV{l)ujA zTKLr}XK_y#1*B9h0}7SCiYcUGSoUb&UxVq~L1)2%<+^PV^odeUe+M!c{ND~BkU z0rX&!&*d}zlpaKtHQH5W{Miq}d{9>r+__~+nDe3Zulyn22MlHa6qMc>dRM+{p(M^j zKrM5UPDszrbyO5{whPt1^X}CBGV(k8+F&f1(24KJXZpqOA_}3klH>F60|ENXo_q6m z*Itm+osLFLkt|7Xgl8S>6&urHPA!`F2>u*sD|X#rUkl3URMbRzfNY1SU^L}5agSEj z*iYBmo<@Ufe=0*8s)HjL)S&j&*su^0Hf3jPl4GKm^7PY91)wvIs)y6=p7DG7?cQz~ zDo%mJIATP{z>qhK#Pd=hC#=-XIBUdM6wov~Yo6$QE!-7k8})@IP-On`t^Qd>Q(e75 zOi)&egT?k(I{;l>Ar&z;Gu9P8vSz!seLvqT-cHD4$YpDv2GTu}2TE~s${>F3SLN;Fdy^!iXF7e5bAkK1&_~z}13#U@kAYDxo- zr$)#0I(icvw)-Nqd8w9dNLu{xN)}y8Q8sxOyPp>y3`B9O!Tvl6ZLP(%fo53x24A+O zzr!*EK~nghjFmljlweOik}wBu1>DP@#Lto_6EsZcCL4MTUAP!o!UXmWVcvZ^4&S92 zPYZ^R<^!pVFk1AjTcz7c@1Zm(p?Fj;;W8fm!GR_pV<8G3*_vnR<4+QRP{mcj;d2{L z?#<<;ApsV@WTZCbx)~_~RoatueoLd@#OLK9!@3E0D`*H%>$X2S+yc69^O}@<+Uw}) zFkqJ8*RAQ}^gJNrJ>Se66uHct2zKssinU2ugD(A+Ek@jkO~ie~$^1BO<7|(zbU;sA zlhO=X%Cce)h5r+7jMtbW@?=Ga>y3@PVF1mYVFu6%CiP5DBT6H9-$LBsQPo!4r=Aos zLOVBKjHanGR?!X7XWN7y~y(W6FSPEFbNGUkv3ig)I1Ok3-z|E3H(K<=C*^rw8-fZg0gG z?myn8ayHg_oI997dao25GZLPozr1ncTfD&zouisWDzzePKLPpZyOv4Q8 zJ_eYZ*~S;8e(!)3iaLvRtyBK6Au9pB-YNY24GtIG?Ju&=mu_nD`Wt_!=KjeZTln#O z@24Kyt~E~Av>^h?9UoC>1IDkIQ~3&gh*JTbT>aKuyJ&f$r)>N6di#eCAu}oqk&0*B zf`H0_CARQ&khlugmbMkm&{L5HA!_etWptF%fdfi?!!5CX2{0zr}{fQew4wC-8 zS0rRlxQeUa?vr+N*DrIFrfa1`29z1x0S#f7@9Z^vi3I*67_N6L@@r2@gvVj1p0;|j zdD4yPn4ZhF3)IPmDts`b8W)AzXHL@1*wczmM<>-B(pSg))f?F5UPk^^He0wML~*5d zmvUX59B-(X@_jjpn9Ubg3DuQG@y%kW;W~v~+jRzIFBoHWeneO&om6ZSHgy<^b(;~M zt6YF&@Z^7x!YBMp-2eS9AlQ4wqxWNEdk|07eVBz;Jy#*ZbscEuS}JH&xq ze%`rVs8`{UsY#Cq2(9kfr~BG4rMo&EQ$40lx?xBfJUBP6o~>rrR)A(WuC&8;;{4n5 z-mMKK;kzeNzOiT5?;L^+M~ANn+4!aYSjz%YidFQU6y^wSf^0R{0RF4Tp zarb9|ww+VBLZul=D7FwR$ENneEu0E~A8es{IrBeuabN1% z2J>>b-4B@O#~)^g^*-5d&KvA)kMe_e!1FGiT>lp7v@Kr62UIb1{Ytveo!d(u)k(}B zKO_d;h+SIyKEe?LYBk9p?3xQBE4DwXPoJ-q!I{S{T}2Ygyo9QB(ZcKAaZ$bfPA-Cz z$)RTgeOLFHA6-JgV3(;=`OiX2v4Tz=BQ0(oQ?1M4af_<3f19FKTx-zFOvlz-H~WaW zBNaqO1|LKSBu|vPi}v^-hRn9`roKd zJUU^Bj0KH@d^fd0qgB^g|k29PZ9uy}N4R6U&y}CTJ z6aA;c9LfFdK%=_SI#Kuz_B<&Wm&M#CSbFpbWJi#VY;G~d=F#MuhAo@_Wip#piY$4q zlnZQfe|2K=*8+KS_AHBMykYKZs_$~_JkFWVLX=*m2Sm69&*%lif43PmuMND86g?aG zay($^X=^o9LYf_G=+LWCGze?};FMR0V}pjwmn+6m-NFe7fxHcYUAqy0cFTs6n;`2$ zj}oe)UC(-eIqxY{Eii3Kq_xpZp{{sy&f_=%dIFlbNTCvykFKcqx_tPs)BwL;Qio-H zr`~BlJyq?m+L@D93IFxO8qk!-p1Jt7Wi&a;z_5)NCK0o~(sHb0xv{-j*?fS_PFBv`^v;cO!)otB=gZ3Y_Xn`KvaTdGKJ*N4* z*%9@YwO#({XfLIHgQh`kr!z9RV>q6=eT;ha{3zugyKfVI%<-110DQl+$w)4I<`&~P~l<(|04Nx$r z2#E-=Ew@UxW?I_VIBVQ1x@S{rMO5{)f*A_vc@Q2HTsk;82@OV|x@0kZir&Nfnn^(v z7X!3ay+tDWa+S9nVY5#z0n2S`hyjD9_cqJjKntPjA7qp+U463nONY!&WFEStK&!?U znyeJ_#+T^&{g$bH`Sqj3zsEa$zhkDfIU8!X;Wt^y<@#e=2Qp|xV$1&FPvWf%P3&{% z$O3)7v4&0-aSCKgai@=BNAQVvMVdP47jezsf#S8lRwgieo%@yFc;55GxM{4WtBIhJ zaNxMAPXswYxaasTuBfEgyg9*#Mf)mvD!!yb7WU~-CrHF7{cAdRHW^Ao=gYb=#y!3~ z@&(x!KMI0#Y*o*NCS_Y?xk*2kMt?PIn?+AVEnT`um53ortedy3zdy;0VgO zWIz5iMeMn&KrIIcbR|s(t@Lg+X%)9s_`87Pr{ky+6VJ9IeK*2Lc2Kgym zO+I3b1cdywiHO2UgCE>x1mdNkCx65C1AqtU4mFMscijhwNV6P;SzTR=e6>^w>k@hI zVQC;p=V><1F%3i?9YXs+Q-UiJKP#S!AZPzjN>3j29UH@}wT=0+rGFl9P|!iIc}2Z4 zlhl$N*Cx^mJAbJPW<8SL1+nf14LrV3NYk|jSq9~^q|23VU4kxA-m#<%ei{wKV-l%! z3^X{ouSP~Pt=}Ekdbz`CDaob*4OQNfPCcP(zBEDg@|kA=p}mjA4i#;3r9!E49_L4B zmHjRIU?stiK*Q2z{ow``p|#IK;^yn7a7ML4n;#A%GcEi3E~sjIYbR20|9LY<9eAJ&_pL;+O6Mt%Yt4MIXZ4 zEJ4xy*>a`++msBHNmQ8<{y#H^gYrE6p19S;pmuH#@R<|hc9lvw$}j8fq+rc}(akpa zZe4>+K$emT;f?rxWQ*-b>@Q*q-{ZOywfygL-{{G}QN3+;qw~RtRMX=dM(a$7gnHGXajc! zoiL}%=pSgHg-YM7v~<^=pM6I(-`}Noc0V`p(6f!+`9)F**d3z#Y+r@fg@4a+oohG6 zm*(YQBG+!(iG>Std7eX8gq4W&vT(w=P|n5VbTq+hg*(#Xh<+k1mXbdNNp_F`5oB4- zb>gmMR*H_?Pp@BXT6w<2nJ02{e1?+WKm%^vWtCtO!|UY66WGMqK?5{Y}|qF5RP#Wg>CB2~t|6AM4TErpy=ls^n2 zH?8W&eQ9G8YHVJodx3I#-4?o^Hynyz66lB}_`M9DNJW11?l{-cUh=pHc zmj#K(b~hk3BBd6h&}0zzog1gUW=kdniqa|fFcfT3PWK26cdhgptn2dyLcj~07~6Kd z3^M&Y-N7thk-nDR0n>c{#Qm3o%_fq_c=5e^i@rM&5*bWJQREl|+WJcHQ2s^9uVYFA zhfLB)pLkW|9S$+5wL+iO+gfAQUBf3VYH@+MD^FIaU$Lb}acxUBTGdUMhEMH=uIfX0 zE=hViiG@B9u0~XAmq6X2r^$bHYB-NmY(B8gvJN~32Q_<~-T;=C^lF2ApgxQfJPnVC zi(Ka-mBbGb1BX3sXw8MaKkL}#RFdk(oEOpx!v*y*l@vOHf74u3+X|9jhc@?HY*({a z*{=%PG%4TU$4m`2Q%;y`xcpTme=!3{h<{iVE2jkdE?zT?uZFIQcs;FrY~WPccebC{ z2HUgc49)>TqXkXV?Hke+I^RH8Htw zbnW}hF!>fu^A_8x_Zp)92pnH0oS&C)HDP8KZi(5N^hVxzqxspIp!Vc@ConIVeg~T| z7xv0nQisPvQr}T%{Pu-wLQyVLY@t3k+vEc*s-11RBhSF|NH@%%XB4e*|%plUpTnY>rISsR5hK&@hT*+aWeXA&sMKNueP2`n zihEk;8He}#gtAi2_2Z)D2wWxA9lIe_V&Jco$Um;0~?@7~c`YW?^fQ^My;nPpkhqcVH-0aJqoS__T%<)iUwNw`yQ$Of{c<;&mM~Yz?P-~Hj4Ooh zrY5EJL4rkG^rvCOsNme!pIff4FlQf&uLq)9iKAOmP-rdS2dmw0R*y*$Im4NVAp>2M z(?H{z9Jmc!0XYGiK}1tpryLCzFRDRCIqmZfRvi?W8sPSKZ?=6V08%bzYasoVr>N}Y zriJh*7hNYIar+JwZl;UbGuTORV=}~r?(^%W7-q~>Cw6iQc0!|}AaIkcNlb(FEPn@n z4IC7*g>gu2h_{vnP11B-+X|=H<0$>zyt;N+0FJi#ph1;cbxAk^vPZG5K0rC$v+Pa0 zRV&^`=}X^Td5>+NdgTF0`K@r&ll1S7JHk)6<$f#?^XT0vZ`+N%BU#?7(FSaZS&P=k zyovnUYyPcnVFajDpv5?*kI3jyU_%|jxlRda0fSJp35x$Uec>=+c}-Ww^s;#AdyA~u zuMap`(+?qN%PYY|Dcl#_{?)z^hH)ylDaHi)Zo5Mn1|Od67=P4TVq_*_1-9;Qweaf# zNe&RJGJ;ufV#vdws3(7eM0fd?vQk&;$`4O0!lx{7W9%?DQxj*}?wzAJL|u;)X_;fE1F`ZUDy|7!tKTSF3sS5E&jT3wj-Gd98(iU#rj{d&R2hz=r4 zbs+nPJy6b%4Qcgh+47)6BB(>1bV^|O9`{x@T20vYixufAxKYuV{Z|Wn%1`QO2oJ?y zX|)hJvIERrrDh&i39_5#BVP0sbNXEyeC6aSiBo^9kry0D+odBSZr4)^+hbiYTukV$ zO<%AhGJjNtj*LDD+_-H@=1TnIM5ZDNOFqL;a}xfaK$T&XiBOp-*-Gu3cg2mBWkr+t z^vBr0W$$iS3a%`$2gU5W5o=z~-`o89#sKy9Vc1YDz5gMfB}D~eS7v(GIet3lXS54C zn~jtp)0DRLs+L~Fiekz$vQb!|7;mAG$NO7crF1|lwu)XO(3Gf%hdG%tiy$%El-H=P zL9gA7dNq8&LGZ7*^p*k5RT?tWW`8#8yp%=iC3gJM#AR(JJ)=}U*8m??6o{Aw5Cd@d9jKC0Cs|(CGKSj1H`0mmPN80GbOBg7cW8G_2U-CC9PVPg& zNIiA|w{MGKMblO}T{&?_e#&IIw<%)#)q=zYZX5w+Q39W^Kx7de8gAoypk8#&_Pb6Dppk{wHdQ_))s;GIUo+ znljtMBA99!;Y33Fr63)D5xkS}$m*%Qc$>5VzOjvEK*yjbGTGhMXoV5v$#~Bbw{KPJ zP`3T_;URqe7`X31ev#GCJvhqe%3I=YB5n6u#8twNRfaAZWIk-dVLF^Z^s?u&Py4&y z&b4*4g)Z{$Xk1*>8<><*joj1SyKQQO-?oYJD_}{G$V0hX99r+9LF@dOH*+bA*iaYF1h~l{& z*`DhuY9li3S#P36)!MF_2|aNbzH_j0C%%FI@2(peB*R>##vh4fv`B&OBjiu&UW0)B@j1X>HXl}_a^z_ z-WeC#wpkn$VA$4eQDRn%Nl&8uE!=eqwGSTvO|Bbw8=*kW1;d}$Nge7`Rv-7guT+TO zt%4#c?R5lp9f*wkc|V0;Cl@Pw4>K|j^71RLDQ9) zJ9O^-{yVC9)qCUEZDX+$2LKc)kAtMB?$m>(=(;MJ9^+`CTa5JGnKr!MAWe$j1=P(v zDdQfZ;4Ij0l_hlsB!LYOzS0u87x{YBI;l~i9cM?F9RV7;mFFEknjJkG!jBuR|c8 zj68f}x$I0jh1h}mQp2hFD54p8Xniaf>#o*C<}n7*1EmvS;>jTQl-%UD$9M3#Z85M} zS^jjX(v?hDF!kqSu>U8Y#6O$Z?vt}(!giZu;X|ra0OJ6v00N)}-Fw;bc^D4fbWM6S zO>qY=HyL8$3t=N3VcVY+#sj{Cma;20_;HL+Vys^bESO<#JOvocbT%I+)-}-pG_;)>An9+NJP^SXh;O;{yo{r9-93O<#N-c?farHnrD@!$?+%j5alabv4gPtrhM%D;Ln+2O zfKdPm88{xf(*V!59k@Xt8ac3yCVr0z>3-Qv6EY z`E_fn)WHhihXt8Ychx0WcloVhyA`1dOXLo0_cm0ad`^kXL~fb5uT)iz%YtrQrgF=g zMHPiy+4-X1NFd)Gz^{q}5!$W?N>U-Zy30(6L2LUc+^FVMGiyiL&}%PH_H+O{D3~hvd7U-JB+vQ^x3EkWA&ENom)wrXiqf z^xetBtZI_thrZ-gAS!%;;l`w(29T}t+gg*@;cJ?dv%TP6+Ad$ET6~mjFpvh)Emg^b z;KtOo=Uij2!8zU#ZRi=%P&-EQL#qBfX3FMi7#=10Ncmb2gJN9Dv1OUNqGle_U&9tm@|-}5XPP3y23xR zLfAa}*gY$3A+S?5d3Po8xH$p(Fb+uO0XUI zGmy+9;{vT@K1cxeG8v*>t$N_YmHoOykwfOTrJ6mCWv} z?n9(yzBL|La=kw4=9n4!*BMRqi%PdV$URsqn> zCILsvAXQeymR>if@2W#w?UE|y`Q3%>ZrW3oe@gByXlLDi>D&R$g3a;M{blG*4h+1q zTHYSRBrMTl*C-}GMKJ8~eoR{ShSYNR>tfmO)#%bl!_#FX3S%)mcCg#SU!H+;mZ)x< z09vg{M21KbC>m-eAs(WF_SU~{#SlNsr)TczjRGi-|F}*mV(=`s03F+#Hdj_D$Q?z? z`LYl<*xcn7zEq*gCsAjnp{OK3KH^~2UjlwX=vrey+?|$_`Mh2zsQwS9Ul_-GH<3pAS2#1`15B zz9ih39hIAE*~@?$Us9(27-4fvR$#EQht?el=rD(r3;vFs(2zoK{!h9OS=J-n9}^U6 z5LeX$!Nc9!zy#Jm4w7*j2pe7<^VWBkXqWlYd7VVFzfzFrNJl+bIBp-F$=FqQNEUpb=4UfZG4a$M7JI=>Hl3IleQ}g-w+ig(lhmU0%}Y+xh3j<7}i5sLD!W@bQ>&u z(|II9S|TZvu9D(7-K}ozoj)szBq!%cKS}EVd&Z`9Ep>D3K_JdG)fXnpo_}5Sl;*Re zgn;#SAmh3*>f{e;S^P2_qou&BCM94ua0C>16>z{S23SXR#1GZUUy7D5vDpHT6PW|K zc~b`Fa|Jyo)!rIY2JQ0)galE3U|kU>k5GXHOuqU`cPNiq(`YquAn1!WHuqZc*$4k~ z5>`m*ZK=$>y>g!j$nm|J)AQ*Luz9NaHU4~;H#hYg`))qH!b(ry9W5*GV1>yxXe#s# zBZm%@oLD)~Gj#C+$iwq}31DM{zu*|^t`;~4dpckVMch>^=nL}T2I|qg=|d?SKZpCD z36TP_rzyc}MPwq_z5?c; zDK8#Ex2|_3nAH(Ep+f}{#>wLLjTSFYw9-|=X3H>VCNZjKCt|`1?)4y7x^T6 z5;xDV*-(Jyx})A|BRGffoh}m=4f)^3P3Ye)ja06&M|$xkU;@|Qwgo=GQ z%dT^N4C{;oCCBhfpk1mv<^Heuv!nb_!NPfzS`Z(eloq~)5&_z8#PC?P&R@GHMiC+c zgBV~b^rr~;p6Ajt{wi3Nq&%HHPbfa{=Dv1&WQpJ`Ink%<*G*U7@BgW6iITZF5!l>~ zeVd#|kV5J%ox<>`ubMDm&_O^vhIQaLZI@z}-P0bm^MK?NH1Un(qCI4 zKQ+t%h@2@RkK6D68Ho%KP-kenBWW9&NLolQN^fvys?NB*^pLeNHXzF-`BQXR-7Th2=P`kDICTFHbzw1UBwoIw&eyOrq&( zN$!w{OJDpApEhB(>nuh*w`vhT*}qYB7%PEvWah94>XR_>DvYgUVWhY{Y~HS(B&cU+ zb?z+!8K;`_H>}MjgareDGsV2Xs}29LCgpgItfE=+R;j{@Wz&mt4;c~+K;sxNzy^Sx zRU!N%{|_wTXU~=?ZH$D4GN>u<3V3m(9_}6-n@bkX+aXHBqp2d<|6IK?8AkehuLe=N zN(t7bWDuFd2z~(qulX-Ja@>T$`66z4tWBuugI(HnU{{1^6y1vkRX-z63(2yH{0{Mt zrA?o3whsIT$QTC7t>}ecC@Xzn%%g*gXq6(QZT`c|j&}Fb|M9!}L7W^5Mxjg4ARZBxAz4t_nh)@rV>7 zP1n@pH2 zy%u{aC2*OL`yNg@*ZyQA19Yt(NV{B}K34t%+|r?#qg7Hk$qkr5xJKH1ZdQ->2F*j? zK`;t^1Jq4WZ>aZXwBWSYYu!sue}s@*apIw2vI4b9C6u>i4J6^&oGGuvxeO#5Pi(H7 zq>?JJnMFrJJV5!x(4ZvlFz~^MjT7*C|R>61YBIJ#GjECIPKqA=dB5$ z66>yDb%Kq13&5%v1TYqtRNWYvU^&}WOdHCf2IDwKN2LPriOnhgve+lsHTQz2W!E4( zFbj%R$fQV=Ua%mr3`)Ri<2ejZ=&G^Mn;XS;Ys85ofgy3;l=5&*W%&qOR43`E_Xg#t zrud;50Oj5SMxr7g@fuu8JCA{ZSy(f1^EGdZx{5$;g;47yIhGV) zM>uhAu7_UoqY`<;+I799 znQ`DW?QnM$dbYI5I^;)YOLm{i0G=|SC=qS#jvQjSO2~fqs{X2>(*xFh;IZ5Vje)80 zm(!Kcux*rgLWZB}RbnSNiGcHknNdtqWYT=*<4Gy)sFD39A|7FO#n@p;YkL-w;4r<0&R~LZ`=&_1%~mztGo!`F9AIzAn(#7Fm_cGAL86`L-#gA zz?2X_*ev_|i%;iRszVs}!cFglb2K2f=n6$vMs7Ts#g^k_a9>1Tmv+otNCb>Zy_Iqq z^!E>W?UZ-j-hal}O6Hh)RAraROY*+-*^2uMR1GED)$nz0)F8qA56uyqeV)ygeqhC* zLuwp61bpd?IAZfm#C97XU8w5NPWJR6xzCw41pr1J2dTn-4_`IR zc>`482&5#?q@dTQ1bU-&TTH;>=7ayI=p`WP1{p{-`eWYTB6t66({`_(Tc*wDYq;co zUHp4wu`|ZSuT(3UrT*03;fbjvw{tadTL;j1E;Dq?dYUV3YI0lJP9^jzMS4d%i1a23Y5+lz5~&dcjuZir-V#v( zQIw8=V2LGkM5M#3MzJA^5Q>xt3QF(2{5za`zyBWZT?@}jJd(V7_UxHwo|!$<2X@cL ziUTCqF>qrkuLr~Ey$qYERre&{(a*`YiM7TRQkDEqj@f+4;&|)auyoHriSxwKeB|b> zoV2z&&&xw>Z>O)jBsPeT{{UR7{|chw%)#^4cqZB%0?_=Qh^khUSGSpM>RApvr<%$> zHyd4h#<#*&ekNsnD#M_&E;enEvBCvpA6b;45GV=qzwP}GUn`9rf09bVY0;(gY+In(vv^RT0PHpHBR^ z4Nbm9SOPf7HDPmae^Ii z@tmnEXvbU!Lzl#FGw-{de2lDN8>!29`43;9Yzt@W8xNfD&kf<=*ToKS^*mT=eZFo? z+sp})CfjSZ0k2}CY8`tlZ4jLTV`q4j`OM3KMoFV89JZ#3cRY%S!!*N`Zih%>$7_Fr zXX4~zeDc7G?V~8`+mW>4Ib0boa^)zmqr7-? zg`;8kV_>gz=%89!{$*1g=EUJhq)x)(S_d;!Lr_00_kSz<`^9);J0d2S<6h`*#PyGc zltW5|pa^D2>dY4`;h5W2%wQB&FPkmhw!o6kyT$bkZpM>+^M0_^@ zIl?p&V;Ieg$tN!xv^kj~g0_0laVS6k@c-Yq-R1HAWedJW0>f1mV9q63-Zd=>9MWy3 zXDH7V#`l@FF>>(7*|n|?v_tq2?rz&95VWitTBS*!+dhv5h0@OnJ4S4)`)4CMbeXUn z_qX%`t7HCkmmHorX%q4NluYT$t2C;-W0=job_?h>Zl5gKRS6D8$o=tscYuA1{{9sHcL|x#x zXP(Q{&k!BIc$*qX`-{eh4}AlA#QtORBw5kV!GX~`Yli>Ye^}|@k7mVWv{$i*W1?c} z{8Co#w@Ro@rmS12_R>YwBvhqL(ljq{W6$vAFejuw@-e(S)x52Ry~I?(E?c_V<@T5Z zsqPUyAXu*mf}TSxG9}gO6){W`dw!=w#>WCKW9aOtBOkH@;72e{agb5{?sJ~tJCKAYz-^usq*f9F1elO~?T!VOY` zRQ|al(*EnH`k$hQM6?uEU@Wt)_~#EJy7nyP0tedW&sQNgdp5~e#XUr7VpYz*j zm%q)E1!XM_PsWPE05HA~zU$s?qOK#)e=@v$s2bLe;77QjwcuH(=b)N7a_g~Y9ykI2 zxi9kNY=jVCBCPhpCexx4*AkVm7t0fTu0^&fzs^9-A}Zw69#Axmv3&{0+mxS52ay5s zMgIh&$&0iwunVUu0l0m3A`Z39-FU>B2GKP2Ks$fQK+Ouv(bLjoC9ny(h5STcYmJGqKKRf9Q$vSAX$eiOe<@Wtpos4tYC<`)M}zhu z0*u#T%a~ZUevXgtweI|50a%;r>+cN-%qL%Wt3Q>l1la*Sm2u4(A+VWq{pjl|5p9Qw z<=Ksoie%<e^XJP&u6F^nL)|GH^M^d)lDVaRviLEjc4N??B3;E2AiJb19 zL39C36eH@pHJ3y!%>HA0TKza!fhA-9!qbD%QC<;W$Pvu($GU5pHH{h);gFNbTAEBx&_&?qd0QO)>6# zRdVG=_%ddifvS40Z4h!J94D#+yzKLr#MJi&=YyddlSC!@LW|1wLP?~UuA8tF^i{7r z$3hnV&KA{Pc{bOR2x*2&#$ksYNsq>lG}F#-&oZmAhldo1x%qC5X%n=a;_@@k9l>^a zF(s0&=yI&8f7TXn0Mj*Q9}p(jdf7mZI82LdX=Agb^klJ#(FGJT`KPwF)7>zS-M(`# za_O#${uE@XQJ5Rz{F{YX&u~n5uI<&;1GBahBk0CIy39(GS+@w*Adl`0Nxq%-R;?FV6xwus)-KM#>XKy%RUTelpuvV}~IJ|+RX~#B_1o#Xo-H4K&{{>fpZ_5SVXhT$ea~GI(5PTpWw0{=$dpLc}Xfa%B z#5ryJ1p6{toRt$Uh!%ll^^@V7NL63dSz7e2i5EXvzy{9569iZ!K=L zwY=Kh&~ba3GwoJ?d?&?^=1Duf-M~3l{YNcJnl#fgVja{oU->4sYBDY1XvQ~sIt_O` z^>xk%fRuckcNmsY=N~u5(C?C=LlItC&mPSu>y@2m#tDN&*lx2po2ZYlPXRF{timW^ zSV>y&KFtygfsdZh@(&RLj{5`h1FRfVRwKYr}mpY$X5QI65V@5(F)!)8*M zr&0Uab$E10q@@hEvs|<0kOy&X_tQcs z3lN8+m>ArMWs<2R6RbDm8w><}($!r}&_#FctnGcX-O%RRZ$v&ziNkz7%e?4s-CDL< zo>h!%xCGf5+WU;Q18h zmooqFjE=Z;nANQyVnF>Z?@X$SYwW$HIfEH~iDrUX;(siazU&)b0u{%nCHA;`;)6Rx z)$P=>a+ql1rlx;>D7;*8>&c&T(mmP3h_zGa8QK}iF=z+aYdef~* zPefsvm;Ra<*&wY{eycGGMXY<~TXcmQ(&=@dh84mFp%mHu_GH?sna#J0R$qxigk+*G z;ax;)YUhvp%(3iA1t zZ@P!v+IkMN^YJ?wmk@^*rt{k1pgM|TmccZiNY8?YN&)HbImT85SU$efy8Vx>#im`; z;$~qT_ml3D$3_?Aq?sOj7y&H$hXs0dfVpFa7y`^>%dQ;-mu(0O7S5+440v*yi~`N@ z&C;3ooOYRM+uSi?m7`b;!5a3BUgYmCSC9tAL_x-!-~fZ6H()*@vTDrRpT;j!u3Mf> z-~okHGhU#J$0)BIi01Y=*obB$M+g$|UKfHCu^e^wa6KU`iO^p&hmawU18}Kq>VMFjG&jOvd50)Y0B}|+ytkCeqK*XmjFRK#j-W@flg+uAA#+Et zZr+?G6W~=dU(?+VY4iYZ?eiu0=j=tJuwZ-?p5?AX!IzBQE2M>xUH>C9k3bTB#xIO@ z@#@6a9$(}ib%U57bZuQeX&g9K8>f%rF1Hq%?rydthh*;bEA;&$Sb{-or!GAO+dF?Y zoYJkRdG#%Klkk|5pq=eK68eda#{xYmpGLaBiSA*pdHO^WH8IUkb`TV&8;crME;8!L>H{B}{{ z>y_Z07eFL7QKs3_S#_!XFG$F{`D;2H!~S%;^ia^M_@{jOC5#A4E3M2-S|0i#xQ9y!Jn+22-8Jd7$qR=C7FwcE?122^Ie=}50(#&Qk1Ldlb#Xt~c zVMGVwJ$Uacf(dk);D=R61|CQnn_IX-_~s-Qtc>yNS?|feX!J0kH(tIo9(oYGL)Q_T zs7B8GmP3Kt&W3~?`E#Xp+X!K^z8tHxBBde}$pN79XjgO@YMJ@H@zr4q?wj&D0c9qmc2QpKFLa7ggA3tw2`0!;&H)V`~+Q+myJo8TZfeOP~cN5)_NX zB5Rp*z)b}0Z1`hvt>@>ArGj6i)V`L|ZTK^o3*xB*CJXJ#@)pR-N&$YOf;aFy;87zZ zSbH$FSLe<1j@(-I`UcN&?Z{Q=K1BheTEPq>QDB5T=~-oItpCyswN0O~wkQKsS?!4~RnfeG{*&m_RgN;t9wsdiZ%irBGtI-S5stcoy^fl;{|l*Z(>& z`#6wNL=S>LK^a?kSm|*6=udyz9je$^Oe3-pt5>*6Jbr{-heiNZx8Q!kzfaHKFTRAL zuA)5ME}@gA)Y)ngtI;Q7Ic#)0&zY?GFR{-ms44A>W1)52^$YnpLhN+{b;3o36T|?d zF^KP!i3@}+tjvp|?8k(u_@`k2XJT)((TxY_jOf=9@w1f}>=ajqsbHv3A*%&7QR7n9 zOH5uzh}QZI7SbixDSGOdSj9yq=LZ}B47djxTD#Zc8%fX&t2 zZ7zE_MGB$!`&5#u_}SY( ziUHd`Y<6cvz^Z@d z#t5GHr%`YW|1A*mKO^LGMDed4K3GN)_<;G@oajkzQz<&8`gF70O0w|r6FQ@rsMq=h zlkE3T?(*-2-uPWbnOX{!7>pX?sv;)GcMI&zAL3g^pxBXGm@EVXvJjSnvO=7v zY>^&(lngo4wl*8LtFx}BE-ml_DjYe`|XVx)4>1SR<6J9H#!vk19Y)5R`7Lkq61dj0-ER3_JJy1gj9{yMC z&I`6IP->+ z8}MZeu!8Q-Y#-g2dJNQBlpm#S8$Xy70`XLxrdoqs#ZhokWPP1~cJJ^C^-lLeS|t8N zlCJ{Qh1fe$Z6q!4*tzmD)}#)6KB$p_-9%)=jgjn9Q=Y~d7o9h_@Zf#{)JsCVb~qOG zb)sbev57a?0X-an4>QYRJv50iYe0J;H#vSwija>DT;TXj%3X^PU>-Hu1%p|6DgT$ptYG*L+$wh&9Ls^ZF6mS@83_f zDd3mqgRWk>M7V!!M)MirfC&kO5$yN1}x8*pHH!_JXz8GG90K zxeBUY@aV_M-k;ktUG!PCMF&Jz`N2?H;Z+r`yS37(yKkT2!o{j>s>& zAh)H)1j3i3r7-kWS!%X4;T<+)417KKbj{><&(gR3eh? zTUecM**f{GR5aW4Z3V^cBTvhzt)BAn{7T^AnTX5l5t}M;{oz6jQc{y5KPL$nVvF=2 zRg#+w*)7eBnrv(a>?_sO;%uhkfWq09mUsxW;BS@wkKHC4g<6#u8dQchyo*&zkm)YG zvlmLFtZ%8Gc+%_4V2Lf+VZ*K%8eAK)D zu4!5)uZ47XoO0!|Pg3*<5EM-jw|~rxkeQkP9|q9X=u5)vQP^8}}3ps+@3Ssy2o%z6l)Um`|eXfO+Zn zZAxDuLe!#UmpS=E=WVQiu|zAAmk-P%-TU6HL1!q2qkEf=q24#;+O}w@pq9-`TJ!^T z)Fdhs^k$CJ9NmM)SdmGEYvt+V7Ctc^L}h}0V#ueW3zA?QZp;Evi>YXpe9hB=CaL%Dba0^?aTd{HeplO%5+GQIox1$L<`g#Q*lDBLp+v&#v9^P2#Lq8P z8~4wBeKZjARw5x9jmuyCxFvx ztU0ZIOe3j!%iu}aS%&e_Y+S?iQ_(#33-Mjjc4O#ymAC!OIf~@J`TjqoWax==pZcCu z{mpLKqdxe;R_|?i{_XyeqfpvA!_J6oSbz*w3?ZD0bF+!6&96e??iY}8@{ph%+ zVqRtpTlKhb)2m9ia=Ja+oI|pj*n5?HG5v3g!t?uEO#nuXxfWR~tiz=v(37m=z3Ay4 zGnAPGd%X>b>8p z0(rR}vH|*_nczg+;GUTIl2pDX`dRzlN(ks@7b5d>{gBN78M=e5wlfB!S_{ofT)bn5 z*3wWtMcn4NsIbDgK-TEQgh8Lyj zNS2)W{<7-Wl)I(Pz)iVb4{Cdd39?wk^UO%KUgUo)<*6si;M{U_ zztj(afVQeRqbYh_Y(3p?wb*E`1ivje2TR_U(H3~x9s7+XC9dn40jztzuiQ5uXc zu^Fg$LJH*D>TArNwpn_b5(JyPg>(92iS)N5-DOW1y_2UR^cm|LG;09t;0(i=*=7FBi_b)eop zdg~)>+t#Ge7%^f0`EkY+@hD*+X~|s1&f}uZfDGZLbXOMmQrlCapoLBdbrWh$kJ?lu zPe&hYA@#M(cG+#6y>jL(+UVJ}z1WbL{$L~uv?OsHihzW$)fo6t{_C9`JN%c+&DKZ< zna<1%U*CBZ{C1R$OV`&42zWkYL`j-$!>8w#B{5F!{@AV$25%~5J{wmy{|}de4R-&R zmb)x_G-&)p{*_`?HpHUCf>W!rc2Q<^Ms70Wk~afiL=p4SqoSE#?Ra_WOL7LE!v6-F z*yhJD%SM~Nws9UQY#(I3bp0z)J3Z;{z~RAF!MRuE=}Eb22TT| zt$CL2BdfF|S7JOQXTxo#J|^?Lk*3-=Sy~n8TLW*Gx>PHamLs&jz=UrD?&^IWEI$<& z7iP42=$m0$r07vsopO@FH(9Ed^vRj9Q|?oxJigss4&1&sElZTikzuJ}CTl0~2|!c+ z$Aa6E9unBnd|KencWj*kb=6cVxA#NqbbT&|q~00w$x$JB5yoCWEr~-c#%@7F4-|d{ zi8DNH96IPeMPeaJT=7mFcSlsn%pQ6j6UR6SR^3(?1j+L*p1#B=yd8*cda8^PW->Uq zv>}x?xD@E_-#wBF9%E4qY$4d|tanQRm`5^QqPJ}MCHqez4s%<~1+_1*&f?s+#N?=1 zox&+{siN_R-Oi62jYi0aD$bihtq;ayzYUtx`o z8OOtBkJ23Rm58l!=`a@!f)S((y?hrlKAa92Lemd1sXo8V+(ibNcvlShRu8>4)ErBZ z2sr+8#qlbmxpUnm*S(_{c60&5GICT!Kc#c|2;3vxCoYAtbT`pQZ^~^mro`x%-9Pe( z(ibd4Ey%z*ExF^|_8LvsZD--4oM=+OQDv^6f=te48G6kJM9D7cVZ!Nf$Y2PwF4NU4 zoKs-yv=oAjXiu&t%B1;pf1xH-Ys96by|0t4Ts}%v=AQWHsJ*{8zf&#|Yc*688;CoYMTG;eSZUp06IoXk)kv3I+ls_MMX6$i`}fl% zy#|cHk;YiNhpK@R6^>WH$_pOUZ;iYr>JtL&JIm#iEE`n~*@PKc!NN&b*=ebyz8J#H zzmE7}09evMx_(~Oy;h=fxnHxiH1Zwu#KMoZlD%m;6RJto8?X&IYBtz+i;^bZpnGrh zuOjqimezx902CTUgj$!xopT3!u1G;&-Wv%YGB0vDdYk{8YbwskM%N)th_F6F`Jaj& z(&Y#p1?s_g-Lj}O%!Q>f7Z>M>Po}KjQNQAIXki^T26HR9>rk~9u?b&w!~c9)br5yT z5E!lM#EoQ({paG|5^I$Ec0Qw1`T=hhc&uYky9C9zCqZGlST~qct@6I>0%#9BhGenr ztkS1Sjz*`1y9qj86vS$z*9A0dm(A>^2w#Am@0etTEgzen6AW~6-vZOBroxAvG`#=b z`|}Lo~6G%nYE1?f(cX1d6cN-6Gi#%lpiiXR`dd6fbs(uSsnI@ zyWd1W;O7EubYhDa^k(gge?_X9@K9pmv%t0Mvht_)}y%lNF+gZ zY4J(co`2T@oJ>2QkAYjruGDhp6yV|xxli3ospT053&j3-^WqmB2pIW-!0M?gA4{;{k+bMlD=|X!{$t9CQ z@Ey~Bak>A2{%o)@#Bu;%3UE$4@*9W%3ii7YOD`3Ge?p4oYO|pakaj z{TkUYZTn8&sw3rQ3gZqhouNU~o?mM2%CcC)0Bu?paC&BqSu5n)^7@%T+Ldcj?C4mu z!EqFq>Zzc(#3d(SYOP8Jg0(7KiMrAu18;wvv17 za#UA65bb$hv~el)sLEQZ<#>IU5|*Fm9A`oM@Xk;=Gel5Lp1kZ1%A#eVYfMW!$DIQu z$ga(1&J}llex&pjwImQXcqY6VP}CR05H)G3Ny5U%pFdUTVxwe*8xZCHa0z-NEYM4A zwHRjFyr^_1lx5c0eJd7X2QFd4hxQ)SxEwDL3ffo7FI#h9u4w@2hYQWD>=m&XPh(j3nWaSX_!W1`r+_Yls0=9_( zE`)4BmC0S=Y6lfJ>vBsO86!-1TlcL9!U)f<5i4Q>&F8RUXH{sOZwhZ8L3T^8yk*5Y z%hE*yV3TKHp}+l8(J%ss9JMU{TOtd7{}z|EmwD-=9qkfjJ?g-7)>h=#bRUcN>Ks85 zky#j`KF>t4=2?a*e@kb=P+2VF;_)Iwwpuk{T}NjH1J7N!8Bcha7WKcP=>v+dxP83` zElPK&CfgnbYtg^2iNSaj#=DXk)#`;;Pna2Mv{|G6GGva9k1TOofdyJ#*~(%#j}E=3 z`BGh5l4`6gwt5U$(a?EjE`;{;;bvpla#8K$`Wa!#U-cQTkT;PhX|{?#aqHa{xb@$a z6jHC}abjem1|G|rZ;9e)VRFW7UJAE<@7myKQ+RJJ-79ZMj=US7#nGKLF?vlXbjkfY}xKNVdyWmwaC2PQLSw6m(`+hy2{9*pia3 zM)sM~GA`GrT@ZmO<>HJNmMlv>bU*|is!9vPR_6@>(8e?2#?qJ%Ab#*$qc`e|I&8nv z`~P`a{ckTry1f_ScN@3UNa@S1n*46p)9)~G`ENst zzk&oRV$d=4N7R%*3@V~D7Gh{oyoz@x}*nR9$b0ZY6ML{mQ@ zu1ay=BGrdnYfXXpq$Gu?shE5Zz=~{!BK}AL^Xfb2I~p^;yCV$j^;>S`rUF zvV4^`5k^&qN_kiEIdnuo#eCE7;J}i)J6o%F68_yiv&X-inV_&lHL`*~vjO8I2Wv@f zSlFUm?9my?g2#o5`YFi!7xjSB z7gE6z82(!#7ZSkx*!Umj_MW;zsuz;(Dt*vRJf-+6SfRol9pgA)4p0*>Kuv>VJsrr4 zUkWWixq<})^3}VhigDZd_y=8jPm!SU&!WZj7fY&yw(V8~tq4Hf+@qL4TXYONsu^ce zXkWt#XadXMCV%VLKmJRW-T!L(svMPC;%U6B4{u6~y7-0_IG`JdCS#6%7H8(@#)CV( zxA|AK0)@Ep>+~atPweV%aQl-G{o zY3BH(Ya2yTDHxS~;QC?6OI*?EmglN-ec=cIjbg>ldV;ZFBhVd6WKi;+eEMndVWFd( z!3li|)8}U1#ay_sy-8u%*{jlCDjh=~uX6LZLU^ib8LPR6$;zNI`<#21{Pzbfit{%k@LOt%y@?Nzfu)gdcRFUq3{fh*iX&X&cX;TEkahd0g7l z|I74y`ODL)u+;#@$aPUC*)V$C6ROCPLR^btfy_&oM#~UK2TPA&ElcfH#+yoe3u?r< zq9f5k0h46PR(9!Vwkgh?iDltDZa5VZyfR9tHI7iGosg{fDT)ukZ(q{D=2q`uHk*bE zl6huW$~15sLoxwPw%Bp(m|f3A6qr(7L)hkBoB*>tONuv9vud*}&cTjAoJAS*I5=#Q zBi}M)hRyqw|E#6gv0-n*L!Jrk2O-2LqKE}Y$5~OUq6Wqa5S82CTB<$lEKHhJ>dJqH z%dFM;$TQ=j!MMs3Btd>r?PH&w$f7(!%jrM@5zZinET@C?X5BAvRA-%|pw(})!>HKl zFRbiHqg>pMKW&vR!S=JmvJd@OVBN?M00(9+!k5uU5EdCC|7?x$w?6uKde2i+g_2hK zvlkBpMghvhF=j1kj|UigU^)V|558;P^`^DUIJrrZ`sR&5n{Z;Bqq??eQJIHfcR3iDjT0B`dwO&;JxHE-fk_)mQ*1lLg(B z+CX*v%K`F|`+|_PwI8mfc#&c6g}J|t(I242hHWYz7ZvXAAMzWPfE$05#d!qenPOh~t#FYK^e|(iGV8nwUw*B-$lLDLFid>1K-M>fx^4D}W2n zx`{~8dL9J%L(0$=rG4XEee;`F{gj2ZJB4ex6i46W#gA{(m?P5}tD2CwyDyvF5ZjEC zc4w?Z+FOWii7T~h%4};J+V}dFo<0oKLa!nNaj#>>(9K_a#HnL-<#S^eH;jkG zms=ilZ;WwH+-8@cQr#{w;+zXs{i+QvWiNO&2hJwvo(pT(=$%*L??eq}g_l65< zxcT`O&5K$TXZ4GAnv0&n1DPwFf1Bjuyl_JfNv+%OD54>mYqhEt`Fk3~E05kWK<2Yf zo;E38A}RtbQOUbl{NN|SnCncMjf;WB<-6xkho_v@h3D8uGGgyMl@AnMdr{oV#h;6H z(M+k$S)@@!6=B|-XGTl;rnk>XqV`YV*dbHe3W!TJ%aZ`-&7=6MS}712G*`W`N23N7 zlsZ{M90Yv8KMeD(nIY*p<}nVwSKBL)YU^|-Od=@YB319{;Uvv7DwahD>ajv6@d!T4kmPDQL%>9$ReL5YJed3hoUptIL;qv}?77^b4lQ{AHO{9XV^JU70e>Ta++KT zI~8+qm<3AZT|tGjh3~7}J}CL5h_{VJn4>=tbb$|cbDwI4;2`9fJUuL#s$~3XYji~~ zr@wWh$q3VbhRZgzKI;INUIgP>0)!P9WKZ}xk&zTvd&8LQCX&dNuQ6@jTyXE!M z_L7?c=0FE~%uH9KM*EsSZ*J_hkq`JEds^L^VY;sR znV%*Gpeu8OK7mbPvbTf}9m`?BHp#N63b+;jZte~KwqJ<(_!8;1IzyoACcKM!sPNKDMm{b9 z$OOC3-fs9A=5^aM;(;<1hb|OW+AdV3b~ZlZ+TgWwX>3#rTKKNQyX5N?PZZ;6)nRYV zYxr-8N}sb@M21?H7#mbRub18Mo>hT*sop8L|GU6#^a0d%yTs1AeW^uxGL?oG9Xbs0Qpy)fF>dn(FxWv zhqBCd;I$J2LuiYn8=^FO79A67J*i6VZG_y2Aqu}d8H=O|P6Q3bNp0;w!9Ynh(#z&HchOoK)qREQ1YB~X~BSX(xJi_gyaC)Ac@7qn=&i!p6Uf1nu;9XFwD@}U$ z{Bq;n62CZKdDxDwB_(Emr7Pv1EGqte(OeIi-OL!JJ+~nfJ>kS*R;FvSoVd-~? zWF#lB%BpPE39a&Ry*U*ohCGkib~D@Zf;Aw&DvUp-YTfQB{6NiPG&ex}vNLHZc&-zu z5fp6x{3zjp;x8$(=rY>%N$uEq%8l?myZ)LC-o)A8Cn#M2jrO+;l|9Emk}FN< z{o}HTyv@(=&%9e|@I6o=QOS_m_evhJf-->9v*G_JzT7rlTsxCMC?-e%zrWHCkLF%= z_-(?Rd#vPC6MXjU>n1jAhSyZuo857ShHq~1V||oA@W2JKVdb!F?QK^*+jIMaE=3&r zuKNhdZ~NJsOR~cM6?5cLh8?(W6Gc>JX}1aMJrl!e67&tQ^j4xU*W=Tbnt`|rH}xVo zS0jkcfD%Bk-_x{y+0GV?T$SVMSq{OM-RHsT4x&CO8d5fdPesDyU$lbxp<{_^xj)~h zd{y2`j7f>(9cscsG)+t}puH-3nM z7_>c!30j6Ep1IvU(NMn^hY7D;6i@3!xAXWiSeMk!JTm=NFn$Kyd#Y4<#d5`){Bj@O z8z19-{F;p{|DDnfs5sZDo2S~zHJXvPab2hK9O;!K9BKm zbTXqzV+P|y-*v{HzDMFnbyl}Fd1}e`U1JD^2o7${Qts=1irH~(?(d0iuVOA0*qaG> z$I-ZHM?pP(w0MP7;NGC8W8POrpD3V~@RFqc6EC4p<48Y5K0_mu^J)Cm-xS}s7U;;s zjVmGy2Ix%`?%8MIfok#D7@EE0v+;wDQmiK9C2{bY4AJ}FP4PY;vCX`Q&=N;P9OWVY z{`T3Km=vEFNI6P8Mf6mhHon4`x_F<}Qvf~baHi!aAr97Ze{(GR_UQs0tssFe^gScY zq}xeV>=Lj!NAC@85yfEE$fF3wJ-H7W7`R$7<{BnwfVT&Lx$mB}o3VJkRsAXwv;L7iW zj(&dFM>3af!)l!O)>TFZ^80zyyFZbTx+&+oRpyPumxE%n7?1WBjIwq4@xDqLJH0#; z7C;LGS0$_^{fZP=5N^^coa@|bwJ!(7euzN}P#W)I9~15-K+E?p#m)#Y`dJ!GJHf6!@m1attNCW4_ zhsUS3kB~UHG^Wg}3nd=$@U!16t$0j|ynRSmzbQ{=+3uzIaL#%&)J9QHNj~?9rG#HP zg(a4^Cu5ZL2fdi-j~|l#^8*#J?pyY3=gy&DC2dq!H7@km%U2$?URA7+<;t%)`Tg{` z4|gCRU~1zGD!rf^kXTZ+L08C4Sayc!As;Dz?iYa=KxNfHUn!C3q6BS1InM-A#O$&x zb>M(I*aoNyz+jHH+N%kjB`PpJDc9^s-rXL2HaGTyj_V@Pq(zwjcsb?8k{9cfdcIl~ zT*Gj4F1AY! zxXRs_hL_v05AX(f6lb}<-3D(^RlIcD`(TJ_8h&x-Qt+Z=3dSEM%;9W(+0pHA9ZL&_ zrxm4r&QDA(8%XhiTcaLMxkMJP{%JTTIHMNJzS$p%(LrCKCF@|6x=OFq)QUxkYlP&lDe9AbV$b+ zTTpRQ=~iRrk#`VvzQ3!d0PYHNMaZ^4vJ%5{NtSrn(H?+NN>Bb0DLB;%-{3qGVRyQw zQiRO_wOLhZXk{?k&SRY3dzuj^jJwTfw;<<}MS^j?d&0|BdTpO9~ zoc!-wtej^z1=XIb!yhAS;9t|%*(6@RX+9o(WID6cJ9L{}*yc$MX}ykjRaPy3lo2k> z>etl+6O}=f&Pb#L_aAM?41p7SNO%MnuXChYA!zc1!Z~eP>%)Zbn;&Uo4e!1kohTYf z5)%9Gf}c+3t1?tk_KVWg?tDp3559tpX} z$Pt$I{c=a(Zj#g^x(M9;NuA@qg0uX?>h)E#N;div;#AkuS-6^~3P{2SLBm3FMfqs| zMaey@!gwLPDt_xePsk^nr0^UgtxohwcBTCnAj@1`EOReBW?{9RDL=O6fSjXTum2B)5*Xkf4@W#HJpkV9-JQ`K3HwneB1{n zE0j}4F0hq`%xtYeypS~*P`KfZevq`JWF?LYGmuL%egiUwfVSHlLqcnp1XkZSw}LD zMVcevwHarV-|Upd#}L{Qp<*vBM(f({FRV0esO}*qqHyi6E%XfzDXT%vQ#~UFH%hxg zU_)|Nva!KhZcdKI_$zN@$t5REYiG64kz%LrJ|>OM>s9E^y8kxm%(8;p!(&f`r`&`1 zZm_wx#O~ozSyh^44x0OJD*4yM9nQHI&_PL@@7f!xU!nGp^!dh!CV=I9ehUC`r#{5o zzA@M+bRNu({=@YFTAKr;zIIpu_t6~r#swPrQFiaZ^~fvI%_Fjs$1eKP@RGJqM4AB9 z%0E~l^10{@(pYMmZJVi*y^nC8Co`BQP&iOtt$J1|ZTp*myGVx9lTiQnnM5widPB2T zXEzdz8@)-eMhL-$c;Go(iUmK_hYiP$(0tsF-)9Y$>pTE(xdsPvg45S{hHX&+BsKs) zQ%_HrcJjP|55ZNG43F_QF$%+kogz7_OIQen+;V8UJ931Dw@r!GVtx#B1+X@;jD6$? zVT&b+B#$RU+r^}UB?b^3Y=Z#4bc#a>?&lPFYvmVOskdj`PcrE1`v)LnVe5Mo9+SwT z(X?;(-t8Y!0V6fw`m(#3V?$q^Sx`=phhRaf$}PX`FhRX`VTfkx;a_$Bl2oLAg;7zD zr?zNrC3J58T(FUF53)xD_vDvp*Q$14s7F*Ikc-z$EVwQ0mRQYN^pq=5a(0S2N}-jN zw)|#UNK$<66v&vGn-Z4iq3GZq@BOm|v(6q-thJ9ToAeT@^%bQyzuYoC_vtvpvIKSB z0I_Y|Oq8F>(JTt^E{gOKYu>dt$EVN?I(c$GPb4lM?)$o|m1S}Tod~8AguJxC5(p@i@phCrQ z&CKw)tj>Z#gqDoTY*y}zJ$U_L1eNVWN(c8N?ic9}S)!&k7P`r`w>Av$f!|O6FWF7` zgQ7Gi!Wj2Nv9}?6Kf2^jkB7h#J2tv}0|d@nL}j6GUJybiVf@ivz+B5f51}fv$LFkg zh#ErQm?`z_dB{RkB!7N!gDYA^E_SpVo({N(bL?B9xZ6068uJ#0f;T57LuY+FimT8( zK7wicmKwPz-f60^D45%Q(T#5boF+GG!0^OfAnFrA&u-O^L=2n0uYQR6E#X1*Pl~ru z2^`S1hU+71vPDUc^d?sX_uiK-!oqD4qB#M*-GpDS_Y#cw-M2`;P?Iz4QbpEBV1s+ z@sm{FHfergdL>5ho|pcbwr=~v29J+dJ!h@&^%kusrZE zh$39cWxIvz_^I~H2ABv8$Vi$!b1HwrrN$wipL6V!moeCt0`}ZWGT|{jn`8D6k=N@> z(#Qh&O-kTchAH3Wbc5ohw5n}q?Ug5{K?j{>PoGWJ-VZt`P;KrhixbD~$R+l6o>WKn zyYyQ+vu}Ise$2_%;$qz^K!Vq<;AB0pr#_YC5R9G4VQ1o-G-VF&ho_jjtER*oCr^UK zHn1pQ{o{BamT(Dq=G*noE9~2bJlJtp!B-+gP#|z1fd_fX+SFmk*>u=m&F^>QdDO{x8))O78)Y7B^Or}(3_r4l zX*LcX@lIrUVd#nGlPS;5urzM#_*$ueHj5HDr#To`lR6ozwdr>E79p6pR<{OT4hJ7- zX5G>_w|ASaE6cX$ZmN&mXCENYGJ-~`NKN9BK@Mx?xmZeE8jN>G^9RA;Yt^BBB8B1c zg7GAqqf^)~YO^olG*ew7$fr5gOkCiRwj_*GNx6?b1Z97Xb zP}sdBYHiCaw+!-Zzvw`K5qz;--9(((6-nw?w9nLgH0QqJpRU^Q)REOv28VtpAAPvk{N} zc706l@y!kLflEZTi%gQ+#<-NuZ1U*V?ry~x>jcX(lf9d(xM6SyTk>LN^g&-@h>Yc! zt`>XER;HMgAY2q3?A|vI11Ak{7ynzA`sy|Q>77Z7zS$Z!sjd1f`e7YarcdX6%((P9 zMjHM6YT}s6S+BwlGC6Fge@)rlRpmb{xI$m3X=g0@TT3$}{m$^_zCC@nUZzZm>y>QBEGE`%C+~dSVpRQ(|r6G%9T}&w&yn8dUT)-lU{8s*( z*`}^S|3xSU`yKEo%57{w-a4N3wz?CxB)C#0Dkb?jgricB{VF&n_nAK;2o^EzVUCSI z$XqO6L>=Q6zp|-h_5ian4Jd-r;LrR)Q}tE^eQ;)_<}Kmc6Jb>vPhT4top?2Lw+{ll zUlbge9gWQ6BY{2LKlA1JI78(24>H~XnLV4x%){QaM=P`V8D|jZ=B7gu(swN#V04&e zl*sFcLRW;0m0y%NUVuQxj;9>n!n*E!ZzBmu!Am~WDYsW?%9~$YEf3T{xrlp+1?Moo z2m6t%RaV$4ITwNbbwfU3bGL?fxIrb5;dGW$yXQ|#xQ$=AF;l`nzX_*0<=kH*OL-9; zJRo?8Kar7+@W1|8-Kk-Uh!i;&8GH7DaHYQI*2_-~yFYD~6}_5%oEfW{UJ;#&X zMw#|s)*Ma(xV$s?VPF^P?*%NH8x8j4-}ubYm$K_g%y*7HMMA-sfPz>Xv?jV8Y}SVn znO)i2SnMU0G>*kE8(&hYMZkR3H*LOzaycy}mN8dHc>TKbhKztlxn#p(U$PucS@?_#~ICj5oAwI(!>u|WLJ$G9yAORE!8s? z6$)az{IyxAks{|;hF$j_o*pB#g79RD7clQXqlJUE`iFz~wX?k%5_zu2iZHzy_2$mW zzunFBca((Ac{;dn>EP(t;a&!uJZ%9^kW-1-Jr~SRAM@T#l*b^X{DrQ>YOCg*TADw8 zxN!F5)G;J0a>w3TuPZIVf0&`w(dZVUwp**jTw60HQTKs$_O@o~`cx}18pUw-%Tz7q;>i}clMsMF&AHMfZ z>2`gAd1Z!0en5%ImTjD)+|Zrxa$ID5P)Jr4SNC=Is5{hyj@xl6IH4&{C5iuOL|?P{ z_Ou(*<3P4Bsc7W7+lmo+N*YoF$UjG5mUnhH}m_TkJA| zvJZNXZHKov;aimC5Pmn5vlX&EohZxCkK!s5x__?RI0so`c)345EF99%Y(7*gzjHEC zm8_s~PmO{}t!iG>(pgX(*H%F&XJvTXRPeEznk<=82&5{xgfV9Q5S7om`ab6oIvu@nn2Z4<^%gRiVnwm`LQgosf5-hMs86T zZlP|DrF?WKEG?qBK}oWa2UJi*)Lfl<{M-umFRo0vc6gOZMXIM2B?P*jZ(l&owma%- zq*0`|lH%!1c}Fsn-wDq?U=;5=nWJtgIb@;vrm8t3QD0bk_(p(jPE!W4FvdCVitbIn z-2B7l4-mYc9u|_~HCdJY^p)|nhooz6ZFH#j-FDcwH7P57VLUHb;l zWr@pH#CH=__kaV^U>#~#w);cx}gy1Oc8|q?j9|1ze#zo zxuT!nX{_t>TEW@L%T%YO%h1Jyq7Gp(Nw!Bdyl-IhL&={~-?j2e+Be%TMJEzCZp2S2bUq$*Q06HMIpu2C?SlM_dN3`FFB#=oMV0T9jkqBY>0;y z2S_rlR%X9*M$R>io?CXQF+%yZdhq$`qT5^_m~!I31{Xx$>u~a zrVjOCT~)IsIK=@otV@kWrI<8fi3Sx}G;5MN8k6 zb?f?6j?zixqZ5c0 zixI!y@-pdA+^6%RdOdEb6?4_Bnh%pL#qF!4$O^|ulo0Atp7a8S$nb;mEG9k%;IqKQn0u*|7b1 z=UJSRad&9|5|t#nRgsePcs;^{lOM9 zX|m+5Gu@UFm$qvnDs^x3O1o~j`$beMmlrJaj+xa_Prsi2W;d&&IVw&k4$1C3B9eb! zvFG!wf*oMPgq&}c<@XhTqaY0DD~wQfJ2NRznkoRUHC(y87bcz~48?|=F3ot*cLGYm zsuy6--#(I^U0Dz_6TgM62~$~FZQK?v;5vK{qvf zljPy;aCv!?K5q?_rp}T6#$)-*aYY^{99E*JqR=d_buC<7E>cg#v?oKM1fPws991EH z$)@|x^hXV%JSdYN#wS0g=2Z{G`Kt6=`*&OEXC0%|>cD9}q9ucHOsE3cNcyGEl z!q$_gqH=U$CN6cjbu9idDqFBb23LOo66Duuwdqqo8ENOEP4UsRNT(VuGb* zvQ}>$%g@prmac@Ge48!LopGJ%ANYL+kPo6|O^%tDCaA0dE$5Z(Ovp?POtJW#w0#)5 zd?c?>Wz4=f33!Fw^a;g~68o&m+%Dt4&&gKi3r`%iHn;F`K9LmbwXG5xANR?kImv5V zpXfqH>aEmmS_02G$L|^GT%H<)N@c(i)uj$D^bh@R)<<`!rs8dMrboX|992)E+gBt;ee(pZ&8GaD6+~;L+AT!^swos-#Z-DH&6P&a?efIL3m9kvVs!_a6hwc zj^5BoWrJOl6n#!FC4ob1nUEEoh9@uIro3v2lQ{5C0rX!Z+pN!hTT!|a{o3^E2cgNA;|W_RA=eKDiZvDBhpGM z-BkXtmklKv^lHO>?<+9FT%UY>P)jduf4>R*AK5DeugJoWy$r= z>GIbK?q?=}6~q4HcbkFH5`FSBX#xsniEttj7)>K34_{_b65g4BH4}KgluZg*?Fysr z#?luq1P;ZaZpSvff>yG9Z}pu}G4ydqs(?B;evqLFd>)(N$D{xnNgw9lc;~P0bJa6+eG021V!o@iI&bRb; z1;QA4+v9y$OYu}N*4Ym)+B!>{h%FE&h96Oz)8|yISB1Y{%eZUq$*Ph@zF7o^VF?2O zxA;R_gex9rDDOUW8z$%gW5(Qx9<~6d%LVFbXZjX-!cl1guI_r2Sb=bcCE3jcqz175 zl~dWH(R>$AWhwYwlLJ5eUK+l?Bn9%%{Ipi5JS1fT_1;aIfe?thNs0*7Ys$&M0GKB| z*{Lu$v2c!3CL-e*BGnE0VpOepbLlY|x?tgpwlxrb0M z?ybNW+rk}hV&DNI!`&kXXsQOiE#E? zPPV<^ABC;K(ug98wbYYuDrc}PF5+V3PDW?wIr}hXLeG87DUbe$9t3ZR%tVBb%UTcL zTwi^dcricR4Im#sB>!uI2NtoK@p8&mlLydUsM&5#oO)s|NHn3|P_fCeOynf>UrdYR z8BaGgswl_%4w-;1pe@#*UtdcQ8_m_QLS7>k+9}+S3q~Kx4*3`1(3GL~P#uHg7;TCt zu?(3q#-We!&Bm|19Os263H*@&nJ=0V&DkRH_Zc&<@5b}Oi_t97XX7Rw7i~OOso=@rM?G1;{|L5z=O808^`b2+rP6&@QB+SCK)(l76mr2L21gPIp&% zcD@|Xg2w(VYKIYUzls>lKCJrjp>2>F)_lmoF31QQp}Ugisg8PEJ_h;(bN)55z-QR7 zfV|%B*^vM8AsJndvdhpiYS5~c1I}@QLayh>TRaBS6LUmaC=HvB;nlU&m2bc^lsN9_ znud%bWkZ~7LsfT>e}u!)ybtAG#Z=*IJvkZT(`O+T44v%i)$u92LlZgxm1@2TQXKBz zxhhNg`_{;&K{*C}56DwBPLNsE;V^xXG$Mv*mf3-20-kVrYy+D&H}5} zSSZ$+{4c&G#N9KYJp91uNv6UYOulh zK+Fos|4-%!<37TkH3IlJ-5Y2+=AB8K|0B>bsv`HSAukc{MOpM;NaJewgJNP-gt6UN z#isEre!W0&^miVu>yLoOs7gSxH&rmc;8KP3m5cG7*Ore!ZNSOc9p)}>qPS|jGcNyD zkP&NaJy%misxFO2qxy?TlWUsqx0*1R%|}al+m0z`a5*&Jyf*yU5b>t6;(T9#BoU9o zCZ*siO=SO#ivQKb|NZd4OM&R1mBsbF0ROvB`oGryj{^Tkf&Zhx|0V?p(j8J86$(yE z=(_T2g;$=AsAf_ODV@{=F@;2wx=v3UVUF=TTTwpr3`Ip63;o_FWa*lcTl*dD<9wC! zn&brS!;BOp-u_&=W#VppF)4vd&s;c~UeWlUl#W z>(~GANjS`|tDQ}3{j_tRZ(3=or#a>GtE3ZusJQ+UZandNLVNj#pk6C2tccvC4*U9* zKYT{LzYM9zRdGy*-|?ahkw#9q{mJrPzY&+ilj)m-f3v{3m3pVrK-rD3w85I20&90O zj`dA=0=0Lyv>xWtrFUB+H*Jmmn`6_afC!dtIb6kGRrFR(`Rh>Jl_VL>LZ**lZa`Ui9oAbwv zev{rp(Vy_^g>AGA&tK~$`}ai1OxCyW`zxUwERWe3@Bd^OF{*d?9YPGt`F{LvC`87D z8+Jwf|7g`AdI-TU=UejKP=uTjZd}j1d7UnQ_$+Y-?u5K=`uCIJs>bh||Ensp%68XA zcYf2duLs;?YwBW-4-fzm0=!N0=e@%JdF zifOYXr{&NtrnZ3NnsY7ea^Ez_t%%7KL2L>}86uKu4$?=4F~=}7apSL&Fs@A{39Ocv z7>@F%XskqTjd~Q@(;VHFm@0sK0|O1>w8^7la&>kTv1p)H8fKIxHe(Gt;XoYKto z@5l6=5jI=Gk{mEbaXrp$(Rx*Fw|DI$C@B`vmPyJ`W23$3k}+6RD?aI=y>Xh{zGsAq znx4WAJR&_PTmU(Z{#EAc%`x(NWr8aWltb8Pgi)&J2bj&+Hpl@XFXt-?7H`y`q72gw z@`=5;t@_toIO^ zRFdiW)@3yo`D;w*1(zEG4s(eVr@A=rd;c0J3}!ar0qKSLWz)_?s9H-u=Ry`(!lUJU z&6GY52x~st7{nE?&=K@|M$L?f$A;NVf8Q`ej3zTXjhAYz%?6S(R5}J)q4({y046YV z*rcpEIbT(!j)8Xv$v-zdTi;sSW|5n?FT5=4L*T}l0d=kkn=V-mB&C>!48)N(+t8vPXD_RG_9~kRZTf;;4$fk?BWa9UqK4Fy;2n?2pE)!aXS4mG>m=MJoRE+wpY;Et2A zSpx22l7O@Iz{rN`yZS;>Snk+<%l=JA=^LrTB}Ii#?5ow26c%O@f1wHShKI-eeH>bm zRk)wawHnx8YX>P{BBOw;+4$4y1)dKw7_ZRqm{$`iEa-p;D2nai4-{A z^_gHHxaBuE%*;v)<%!AF!Ip)E!}vbsO{DNh+ zd?=8K*F@n2v7$`bYt99v04_w0wS_Csbxit@y^28yvL#k3f@r4fuAP-G1=+Kr^; z*|e@wXG?xFZ>KWX34Ci(mB-A`q;nE|_M^H=UZC>$O<~uLn=(gPk*2m?MRqRl8bFjN zBPCqH+GPXNGae_^8|CyFk_LwwDc6^zi4Vx;fBI)KkS6%SC?ndBL#p4@Ret_0Q9kt! z4u|uMRCz5g47)M@4#ir|fy<@p%B1Ud-6c+sKfoHEWf>KRYsJK-M5y4o1?57U{yjR_ zP?t0h-mu1oE!9$h`rM-Y4qV_L4$M3z-{Ybqx;zy~?_*c(;Fj-p{5aO*_?~-dA!)_K zGRenP()&B7B5fq4{pEJFNAXp)?ZtLoatgnZFvhPvlxG*gQ4)Flo4Uc#N2^O-tNj>z z*C^lH&8|BCmPoCn7J5-9WzANSLnKIH4&LShaQO9khpSV>R zrTRbl_b>=w#k*c&BT1bt+X&|jihK&9X5tJA!JMrZ!s+1`tz)KCJaHzGde!1gf3%5n zqiljNZPdhXiK3&(4x2GIc*HJIk77iVHxwOIBM`wZy&mablD1o}Y@>uf`c`OzaPS#3 z@)|Sh?JR6>8Zs@y=2YT~%LkA5c+WJ-oEC+RwdZm18b^&W77I6&RMHwYJ&I*&Ew-hP zL`&PcW^AB7*K};NcdC5(ckTq|rqeR2)Hl(lodT9GTEJm4cYN((Z4nj%A|>MOzd_4Y zXjq-Sp(D>kQp%^kC`Y`Da!)W`{U0#qv}{jU>LXToDb6&=M9hBx}rb5Xe zE;P9j7soAC>8XDUOQ{7S)I-DjUUnOQ!=@qu_0>-R(ew2&(@s zzFfwuz#|BRSBQRC#i5Mw>a<2&C$vV&UhAY5vj6vi^&GyX}!_eMlYE}Px%CgeNgbntT{p_^E+6g6Q}OAZVQkt$#F`Isj=uodMp;`LPS+01xJP-?B ze>Oo}sf1Qf8oLQ-g+*+3WY-1@Uh4QE1UY+BR^bOs2}7%}e$SZ5TL{#| zNYIfo_kRR`t$(;(QC5ABw8=3kMS#G92~slkE?M0w8?fA}1HR-zL8YkiyyL_qi3l?h z8jMdWAV2Xf(KZYbd_@Zwq`vhBw-d(2TZwtUjZfc35Zj6kjNm!FDaT$~d>i_M+w=(f zh|%oWZ&uZv+Xif=!X7{VX6S|#88mBwIDO<1x1a(5Q*}$Jm<=ppNRK5cs;dhnb3F?JAaD9DRZ7A5U@#)qW5>hr2>zvaCN6Ab%k`7adSFi}vr zwaxBZ-XM--nxdFGk)9F`96V2#+WYt}(p#Ea-3aEfV9|>Bz`Hy7d4mK&xG`LQ5$|{)e*203AL*-jx*wxy-yxV4{_%;j-1{=? zek6(o6A*YSAiGon;oO&6nG%A+C+vn_I5#;=0;E!3#~2ribBbb#O|7t7H;eB`pzmGk zNI0v@ZH+0f_L*kYl?b=-j%hz2@8=a=$LHUYszN6_LkN5urkR`~x->EpYDMFR zcq77L$fFV>!crHKu=cxuH#x*ZoAtcZ)f)(r49gP5P(eWgCAVB)0~@r1&{H4!93|VW z!p5aWyu64O4#Y9DR09*)vP~!NBg>wD>tOV^E+ZdbmufC!6=22Pt(Q9ED3JYk8S)?c zIbKqswM4iMMk2zSVjgOO-G2cW1Hr|d*Jpdr4d0{~5s0@>SkSqjX>Sd?6x1UuB% zXNz!Fkv^VZHi;j*XxY#ifK^SG3k3|@C{B`kdqH^lx1d32K3DSzNdO}Q;_pTB)5$;k zV74OvsD_3#0EfhtWokszuEA1BTTfLLfRAi$x_pEr3AIKpg0P1(MOa6rHW->blC;jf zNM)nh;h3BZSB9Om;!=O#`*9e4Y2Jtj#T&-wB;{-$Z z8w?F5{(uup77>1{s({DSpEu{;{4~ab>j+OL>Ik3i>!xGUzvN6HiSoHs=t*zc5FeLqpA$Ly+>tRZ~H#J6W~1 zJiR%IWh`*x1hd;{pnie`Hu3^nov)(wpH&6oohZW-82b!3#s*%svOvN>V8K%=aBi}v zD@}E2v_)X-Z7!6g7m-tWvV;{KP#h$W=rsn;$vWDjb8tSwFM8L_1RiLwN|KLFh_4U9 zJ*2W(oVNAS95|O?jndtbba+~}2yf=3PJ(WNW6;>-MAD-Jrg|oZV8^r@b|BBXS1d6w zryxVYV$0ZM_OkDmF=p7Xw=Oyg9yUp4Zv@SsJhx%1dc^2mO}UJ3^p2 z8dDnHa;NE09e#HI;#8qbjrj6?_5&kNju^~-w;;H;>y}~cQq)qbRj&NUZ!(*BO)7<2 zw7lhR%9%s0FRo9SgAqTJxT00UtJ1r*VP80IV} znZBG&;{_3@Q-=|{@`%7N9s@P|UF-ImzL}}9@QD(zMM@cJ=AK+51F~`=BRf+|NLcy+ zL!_eKeSp-|)vpG-nO<8*2tsu`LK3rVKuJHytCYj8t{b7RY(GdwI~g7wu)l>T_y zvU#jU5)N2Z8L%o#Q87MK!NcX_?w@_@K^p=s&&P#;iH571WdXTceKs=Nq`RNz^2u(_ z7*@6I2JZiXIjOH!P!~p9-lJrpB`b*uOjxE4Fn>&Z7Y0*g+EPFW1VI(bC=kr# zpYU882@E#$jRkm*;rfM%hHF2?n1w7V&FnQ3#QBQvs?h2Dh?RCZHv7~)$M<*fR&Lqnu((Zl0^}}rWHAP zoCM2^Bz*x6!wvlcs&RKGcp^J;X5H1kLwZg^U>$^o0snk%*E^&~#Z(sIEh)0t74K%! z$U(K{@bn_xz#QKkQYxv2G(^&XDoKoOSCN`*UyZ;07^?I00%G^b z>s3Yab1*f;bE>mv=5nqH8e`DcG#~BrZ5|2ga(AMnK#474Ym{inx*&{3T`2T&*@XLx z57jZQ2~2x+oXUEP04FpF2V3OWF`5BJU=vd#6o80e*W)!Y14iQD;KvGpm~wnOKpsW9WmVya1X;1Y;kM zz94wi{q6cpz+jk86@@+?+vfj31aU7fl{x0&g&qfq-NFPKumW(VN@;j1u>DGlI^7g| zt5WN}_Dg2_Rt&+-+z|2&N4GL4s()89XW1F`W8}%($|73uk)#1eR7al|Y$hzE&5?4U zU~^}l4rL5~riiS7?TjT9kBQ*1zMpdsgcHKFV)z5~EOjI5ULHkzI1)$DvMv^=rGEm~ z3VEK(BSDH&3D7je4>ad$k&5O+akM3y^5d3`zutzoJZcfMvArIsdT%Rua9fCIPU0zr zTM6HKZlnA8u~-L6lBCVgkcA7_+7DRORZevT_GG&}L#RW>RUjRBL>inb5}!XbX|(i_ z1mbUvkP(KHUGA(=G03njr5&C_H1sh-wF2ay?7jdIty@Q)$T}`>Af=}(!*Eh`bf@${ z4q@v;En#9XVfB-$6;chHbhtgH@>+tF!tF2txq4M;#%n+d$#tB@)Zqi!X6az>>Nwo2 z*-CmsV3npS@tEE$Qw6{zsYo@HHKm(Wh6xOBjLisySLrXEqx53TFe)btg5IDSPSh5F z$OywcXgM;|p9|$6BpIeb0=^VqpkM`{=aj=gBCBU7D(uK#^dJ4kZfS-xK=r;AwrPBF z9vLNJKmBNbr}~0A1AuS`!*dc1m-NBv5-&6eiNbqzdd!V?Gx{fG9;oL~hdvMEtN#wk zVB~e9PYQ6&W1?)%^k>=KmEQ9+0P@15g*Dz3up^e2?vRh1N?6|upsnQOQd7RL%m^02 zN;HNec=Y#$fL!Ij%FK8P39KMYzT1JN1AA_@Ani?n)3Ak$%6z9z<Z{isIS58KzP|1-5<6fc!EfutdFzWLn&-3uDdCurl#nSG~vk_@Ww? zCf%x9HqVS8u(iMh=+jD&S3riXOFtMJQ3LZ{UA_AuH$)BpPY;gw@5#I%q|cUV7E%%+ z(J$-tgu&!If>?)a)Ak>%d6v4(8FFEvt6HzETN~a5Ja~c_E$(1ds!txxkrDt+tAz}` z9{?MA216jTZW?U?z@B&HgHB|FKNz-nFrSqujj@5-+VEJbHmUS|ulIKga z<(d8hrDB>prK3emJbQ=MsaJ=TM=z(y3_;-}KnvLX(GaQq)BOhn{u5%|Wv)hf3Z6W2 z!GpXCoiG^tbJr^wE=G4^Al%Pr$&m6)(^qDd zaWhq^8KyD{{c`rUKfk<`gnPv=F7#hPHDraCabv*&;x4x-HKZ{tg4_NklU)by6`7Vz zwjQVn^BFf66-M}n9VuUFVy zu_4=@R@G^XPJntf7li1kcrL7|7=30lOt$q; zp0EqJyK18T6bCcZO$q_1lWLVu2&w(s(29AvwC^M%ASA=#jjNj6zE<+2eP16xrYb`P z?vtmGhA+X}t1g!bVxD2x5xgc#v?rvSkW*_AD-1)cJ6Pp5FYj@g-^R8zRa2Q-+C1@K zht^0B*_S%MSEnk4 z=FYBaZ1o23NSP1mTI{HUQsNiC{YXJXTqqr7g|!7B;9VGUj=ezlJ7$H6g4Rwv%ErMD zBn-*s5$ussU$#TRDMLV*bf2lR#9AhS6@*^}S6!K};JRfj5p3hHD;vnpoRod&EM%&j zLu)*exn}SZ`W31?fpV0+Rl+azZtsM0h|BRz|1dCrF`TtcMUOv}2kCPH&~pjp{*3<(K6tf>8ZSpmvcQYsaj<1r23r;Tc46{k8c=$Y znUm1%m#zHlb1=eQU}Tbj8(L76o~Uo4q;Naj*5>Dz

1DNVTi6QT2wwLw*(g8?}gL z;H?~kM0-d{+DY$DSB`In+O?g0)++EdCv#EpxK2e79KRBh7h$S5v0L~pjS+BVzokR* zf>eQmXH?agjMQ8%w>N}ilCUB=KFG(zOt2cT5g&v(aGzDKClt{lmu7-U2mtnYBf)U&WF}=VAR(SBx^*ZGNX(B~! z=wrPM;V)!CJ!T99++I7cvpLY%xf42u)Y+~DqNw($v21BLw~q1p+}vYWgAAs5?DPC# zAw$nOkB*Q+fd>({#^X5KL>*z(H-!!vm1Um8&)eA19{1ExZ|%adaa{G^^d_>iv2*bF zxbA@5EQLJ08*nnrM6W5AD>%81vi}GoMPSr*+j;D7I{9SOk!Ac|(SplQTKbh&0%aWw zcwNK#O{akm@K6J$zWo!r)gTFCbby{pBx&0Z(;YCrceV^;>y$tD8v7ux{E9~6^d(41 z#P>4Iz+kypimy{7gbN7Y3*xir*tbT)wmL`3{h;%^5kUkr*n5za}`wlviy|x>gaLeEUQvi@oSX3O~ z28Pu`-~!>fDfEm8+^zyrbpUU!L#ZTtsE7m{quLx_CvUpb2g}-O3r3YR>FZO()yL5@d0<{Eq%IP}TNaF=|=y1m&D&k#jBD9r8W zKoWKtGpg)dz)+W~tkj`8Gw(f@<->>~Z^C^Y@)7N6{o!LhwUc24C(XeTSt2Wr@uG_5 zo-WU#FI3_@e_XA%@5KTjSf@U#d>DI_L&kFJUu`B1P1zzcgU-fsgA}F`8?(&iOxnOf zNqUE(+%Vdwl2%9J#hB$(`kZ+mp<4wvk?HFePZGqEGgM5MZWTHKmM_ez``liO8kSUG zM%wv!x)9Em?5TpU^L#_+-_Q&&YDVb`&Z)Bdr@ukdnx-6#w?ja28v=6{c$Y}`Uyvu$ zKQA=)diBOIfng3cCz4pOwUTIi4%o zQ>PFGdQJ&C_`Y!kVobn2i)YbgU`751?1juf^14|$ZM2*^{DtH|9T-lyUQwywxrI9r z(VXLJ4lJ?NA>i~?6czfmHhQT_t$>M$qxz=aX6(-3`KGFyg8?pV8HsdOc*XOx$7;t7 zlw1MJv>(ahgtfYAJTt4UeB-H0p&ze7S4$dY_`dPHhx!oOyySo#xZ|zm?gbEgu|;gG z;~0h?I6wN*V9~}Ats=XjhpW0Ydv3C}xvY}5lKS;pYuT?Q6TbRVO=7G}&MJEo4?tll z`*_}#rm*NIG1x??9j=N{#?exkhW)xprHsy@N9SvfWR|AU9CY;gVX0OKN*rk!yi-UG zYmCK%XjF$Sd#$ieh3%u^07?3J;0Cv+sI}$xrfY$fB;5HSyOF+f%6go!q0CFSIp#}e zn)c|hNcy2473Wr|1?w&xW5kt(?l&`pj#x=5Z`l?IPHwc12IgwWFejve8J1m8y`MjO zUm$4}ZKCQEh-oV@Dga^hATD zp^+yjQU{3_s|kcOs=Az8_{AW3^+h;F2hzd&5&#zRvD!j}Ww({Kl2nT5*G z7=~R})zrt=wn-{lPC&?QFtmvpM2f%TB2kj13?0LWrn~am8$?tV(lWpThQ=)-IAq*N za*ZG`LV1_U8B*>2MS|C`uy_UdT+Yw4ffWMFFQQh#If};2NUz9 zXrkhkz|UeFhoML59ImWO!F#ssJ$m6Uj>GAD?kArash|?ik&o|xP(Y0sZctQvv0taD z(}wIk*^HVvCbr*3HTK0fRMs#!X?^$m9i^Nj{zuy-GMsc5^mPY2BcCwR%yihZ7i~Jo zS5O7{e`lTemHOBE;6C)j9icXiENKmQV1BSD)=IAO7sW4b#K6WkdFy{(o;!9|^~?B+ z&6(^<(PdAz)0l9VC-?5p*sOcRWlFZMvHQZtliy_jw(ytT5vs^i@>0+*8~gUL$%!rc z^54NxwZGC6S#xO*NxY0FvJpR-JlKD|XJAA(Sv_u6mG(p8%+u%HON2^7?`6^OF2N4i zyk-B=ueKY9o(0$V9BwIjx}WmZgz%j(QObGbm63mnzu8V)P6fyd9R`1GJjvDuTV}S6 zDvgpJWqrY!dH-4Ffg^9yg3%IZI}`?IiDHSu*m1TM-6SY^m*;%B z-QPntBG?cFL8AEq!VYi@*BC2gD(8c4Av`YdInKYYGKzDnS^b<@^C-a!*jn4Awz)I8 zN6p8nXq>e6+8P6IFgyFny6XIZDZ5f{n5rA+)L+}~#=O(-8#-@(tco>gVC4+b^rfWK z^B6_hC#U$YvZ%mR_BJu;_#@EKjw&|LEXV4(*to}RE7D4nk2ZGE;r20!vKQA&Lnd(fks^IlWn;Y2Qe~Na{Nhsk=x2p2_ z2E^R*<)_a-ZS7zD>TBckNFjaBi^ddSx!|l5Ojo&ECK+?Jo8E}m)J9akmWWAmcgXXe zsBNj9NffK5&S#RfiHEY)(zf*u!|?QD`cwq40U#ErI|cru*Tg8!Yi!CmoDWxQQi9U7BQGXSJvMkNmw8t?3_zrUZG& zn4G{Baa+#4y{XMo%#2_{Y<;U5c6ZYWs^dPS5Awj?MNGbDD<7?evB9I49h}XmdH!x! zVWHWx-IqCW4#~N-sF~)*O-<~Th+4zWF5y$m0WBEYO*n-XaX%M3qx*NgiIFpB)er9Y zo+!D^rcGr3(5qhUlWmA;+SZk}W00?L6vw>b1N~#4b#k`0UzLPKF!63(A@c69;Xjb^ zc<9h%WE*oE8yvf2h$yV$O^A;OG9R8G*i|b(pSgeFazf!@Cx>E}iQz1&dk9m=j_-Ff zDlR_Qqh!>Tm|@4eof`D?%Pwx)>s@X8-c^*{N_15fJu9RRGBNfit{m)4q-(!;Peyo9 zn4_5CtZ*^nwD^S=ibL1|-#c5A;-k^E!Co(Ft-A)!Vypa9ST^NbH^0w!BM z`LvC-22@lLd^*gHX_$$BtEMq_T&KF-i5;n#%w%sJ5WEl=|8<$p(su(K%);^J?Pcpt zg4^D*Q0By@+C>%@oL+NYe!<(pH}91^t!Gvdba6O-Yf`)R4km#NuKnlcqbB-RDlRto zRBZ<5vZ1AI^Oqm72yXxWWo+xpGRuxhhwq(5yKz2cW=U60mRAMNQ!Bv{#ect+wibK0 z)eTm(|J{R-5=-kG(QViAj^?Ut+KZk$+Sf}M+@&CS`M&n{;m48a!63&1iv{BD#w9OgJYR9KU3=XJ2(3xh&WQst+QQV_Dl$n#R;0 z+_h)5z4~A;-?o=sAw`zz*yf)rceeZF*ryy9Ul#E;JuDlS7DT2-bw6}z(JAG8@;Z*+ zzwWh9d-VO<>y1$ioL4es@#PhYbXWaHxQZ{`KXGXPE}f^ZCr`1tC&dslo4hzmwsstw z_LXDPx?Ikiajc15V{c>5Yn7buXyu<HYe93AYtG)`(a+ew}1b&~AQcb1!$^*L>M z{RVHC=E+Hmf&}fdffIdN{kx*xRqlGKe@D%~dFxs2B(eL)>yc;ox?5}X1rqjmGVjRQ z-;^u&g`5}}Y*Q&5M5ZSPR49Dm*Uv2~6gie8wp>xW!@tg9=!oXi%v5&Gw&bei zakg=@u7lfd1`h<;e8~8QIa=I!X-53J+4;K`v9o&|Um&}s1=N&`CNjpX#wjdaf2UT}fCP~{LX8}jf+{PsnDAIY zs;K;Q5&fVN)upRtW?q|0@I@wS&4~dg^X!5Ld@7d0e)c@0d>K_PTJk`iQr!?v%)If{sqH}gWJ)!ZR@*t#HrC!MrF9p&Izf$cbAjSsMZ2{Yy+nClY+ig}eKI||Q;=gk?(Vn~ zYWF_9OZ`RXr-(+uHHdb~p>yYJwu`k`C@&Yw2Niv;PTD@~@AtK6Olr}}K58$s29-6c z&RE0pxIq`s{&&Z-*(Jo58e4rF_fc=2P4eN%_{? zJecCafJz%Jx_shKq)S`U;Qk-ZZZMU`Lu~6GA(LIS?v3Lx+*!cOjjD*C{(&gIk(*edpzT+NQ$Re zu!y>>(WmwOg zL6-g9E1`FX+Q#8b;jH1GOoH~yzGQdi|Gb&ZwBsH>+}`o{?nmS1 zp;(r?Hm-_TnIPO{*`!FcD2pQV@mQPD(Br`io&#uy?ce{h$Q7H~hinDN_ZF7CjrNwH zjb3?wOb?*B3Mm2pkJf7@)-klZGX#2hIp-3lWnB_$n#0)of@>Dmxwlai1QMH(bT zN^0tapQIq26BHDXF3IQe|G$6tbHCeXZ?5e+&+{9{aeU8%_=Re*gZiYBY4LxTs3X1~ft;$OnmpqH-5~wO}Wmq!{;5Vk|rBl%qiK_YX8o$g^ zP|`8@^XMzvTcyeYnhgh|>}MnmqDsRS_fX)7)R*wsDQio{v+PtsH2m2iWYa9#x7Aa6 zOL-m}%-bm`jr7v_vnP}oWC!zdm83eOm`SM@86Ca0Alqr_ZAUno`^rRhtIE6nNO|+M z%6cV4n-F$h1EzPYE^bX1nzK5=29JeY*ot>|8z|(O&?Q=7y$J`B35AN@F9~{-aKPHl z0NaZT+I}unPR}dgt(hB|Lud~`J~v>cq^-2BzhKsY3__a?vThCg%z^j+XI11=FQ$H( z)0Zv*-00?-!TWXhLC4SDxpHkXI-8Im0Y%sQ@$y@#vgnGZdLmIfSX#`>|9^k^J9kR* zeX^0W8irxdr$e&eQw{@yey@d(UikBiI-Z)BMkAwz4ez#M1sXS>ZP7l(KlvO4u5_=~ zS*fZcbhYD;`piQfzg2%?c6vIhyb2eHPJ)8amdyIOqnHKRK=W$q+t^C=7yP##c!n} z-c7%CgoWeX@Cga@jY@om?9Rnn=Kr3R@~omvyn^!kOu!o@IEu0aIoDlAS~O7KV|QjG z56wKjGe)lz+vOE_84X-GaRUEmV-N`O{~j~3yU0xmm+DW9#IuQbn@`ryJPyxUYK2GF zBjo;?+iiTO+p?iOWX4<`y8jMZ#V|v|lh?{}Pm+zQ(O7Oj1b9F8m%6h@RfxlM-B#TS z$WXmm$bEx`!B=_mDG*3AcQC~vqW|k*xHL3zmjMH=wVw04Fcb&Dsl`FU)RP_xD5r4Q z`H-a%&P!o(`?5)F2$Am~vYfq%nLIrdXUa^e2)+K^^5al;{yP04-0<4!$8A%gAV3(M!FGa?U#a*b!> z9)NRKXyIf~y)9IUH5doM*^2rof8M3(pilz=-dy}X%99Kf`vQq^2Nblot6QX9|J4Fu zql*S?;vi0%30;htzx4SK9NRL^Q4F-At%A6(I64%^_I60`uQzC-1fk0@wkw=>)5iH` zUiVCL*NJU01f6Rf^SeA-0aQ&nAe^62k$a1x3p7ZfDKrp}-Ks-y56&1?bED{(jmETa0K6%QnXQlst@m|JWNCyW-UUCJ@GwHG!DB z%)b9~4K3i(<=6|3DwKFp4b5?{Y#T#@#Ud#g2JH>RUg1N+?T|BiQV5=(rrDP_7j}q0 zh;1$O$tuzcL(R+P^E9y=aK+Pn zVXh$QTHC@p|0%)unLD5yVGhO8_rx9`kH=aEm3dCn2XjoQ1_>Nxvf?!1gOd9+q017b zX8XvF2dY0Lk*QFU`k6}$eL40v3UZQ1e3Iq$cga$*%UD+&^-lU_x?pIqR1nnRX2VTA z;GF{AzcV@yaX=f8{I?Cr%}1-9Jp;nTf75M$NmR5=M9Z21=3j(-(qS z4ryrY2R{oDSy8oQj*4k3UkK(ZUs?r9t`^&K&uEyZyIYvUJVgAO+G=y!QmC=5?kij; z_y81kbM|Y_nzCDwEBN|U0S6b>Z*OgMFdTJJ2Ff?>XLt5%M)8Z@Pu zw^C{~`7{SQC=G?Vgrlg!DcTqe%e;NqEt`>OMrR6WQ5~|DMLNnmCnHHSCp@FM^v{d^ zodc{eAtQT(-MVI^?+HulD*I)j$4mm%S;z4C`bQr0zm6#mpB+Z#*&qLzCI#%!6AefW zZ&$8-iusa7*OiuL?6Z*V>APT~wn-nm9%~`~|JMI8?k5r#{k0d{q^HzN^xPdq& zSey}R_EU|}onh9@V9kEWJ(LD4A>{ytM%?%rU?O=Ju*OK--Fg4-Vy9S0P-0(UU&+`T zhE&n#Qi|ubK8xN14*!^iy>0B)5oI?>?@L=m;h1prVj!j4ooXHMgyK+hP-@9-z!XXm zzvm(vPyOJATwpyH1h~2yG32kGSFZvc*c7Sds#~fP8$%2P^nRBt(xQRQU#6vWqK(XJ z;z4I6fX=Q}#g2FPGus>5+l9yeEH8H16CSXf7YYS3jVeA8t__a|tUKeV7MI>o|4KAf z1;xTv|KWcBj?g8KtlI*dzL7pA4O&55@0}6BYx)Zz&F|cl7yf)I^dBdx%kN4?y_}suJDUVgz2n0Vz9q`mEr4ZnHG3JW$d$a4R5p?u#eL!L@2iuA&cjUY{AzOR}d zPmNuHubO3dgud8-KS`_6V&~cuB$w5V-_V7#Wd`TLzJ~9$@ zZX>|lWKZy{Ur@f34PkM{!N8WE%0{*$mf{=;b1`CF?s)fyy-SkF>-xhh(mIPx#cTAeyn9Vpe)uHJ z>j4dTIc1+qE9I%hb7EBx*2*797MIF;d;tsfhd^=lpyMLY$H|p1EL!O}Eqwa?bE;8X zrrc9b+6i>bXFHvq+q;oNr3RWYh+&&CKmHSju1qb;U!|;Kyw7xBm+C5`q;uyEOKPtb zRioFz>LC0&SP^tiWTs6AY+g zu4USact>4_eHpb7v5$yuOJ7(V@2i2sXKSdcKX|#JJx8^u#veEWFdn$foEtzKR$ot@ z9&wXo6YU6I)nOOPbj${gD1}(YokcHhty$iPJaIyqxU1&7Nj=Qq<<_7v%%#p-FX zMtci0rrmcmjsCgJFj8 zM8^M;=L2q#0g@v~1#>;x*Dm3*gZ=^r@#4=0q02|8mC_xg4V)+~2EpnavzQBLGufXe zGu&%gEs0{2(biV=5}Wo*wp_C?!1O-PpWO^wiS}2Qjs7|LM?bnQZY*|pJiK*?1LP}# z@<|JAWj$V0cypWBiKQf5lPRv_!H5mfYtQ<#V7Qj9d#pc!M(ip4Z68NXEsyNgasj+B zJE4L{uYf-qRy?^~Vx5w0zM^~BpBbphV9jb>yuVmMykPO_{StrSx+QjSG}2qhS0?1r zkhH(r5QklgA2&mIdM3qN37M@Xakd(7S77rHCp#(GDVg1>0AGtxPmlst7r6DNAw{?z z8zs|gZy$EWdIDd(Wi` z?_SY)b8E+!o??2Wk&kkaWl-tNSez;=xke7{zh(TP?wI9U1)tDNY}!;gF^~9|v~%Jn z13834Nowiw{OpA+~%tpS4xT*ZOjw59PEeyS^&tZQBH3;0l{_(CT1=6!~~f?z%%#!`NT zZ3s8q#mlenrEmLJ`2EyYGmnUE*1dVU3r?g;6dcQ}4C^p%`G1-GA`Yk8EXBMH~Q*zC&eTHs-PNrdqhhr+AYO3;k>ZG(Sr*REFt>2d2G%$ zfO(1(YV*e|aZ7Ihy)=w|M$CA@y}L; zJhpfOZxk|7j~x_dUX3LsR^6bYQ@b|vBI@~4R@+GOW%SPEd0=uBL7C%*xW%y=1}tN` zaEmNJRwhemI3Cb8IEBSEr#CY;^C-m~s{RI5U0S^JM9X8-DLw9yv7IYNRjw)u+`!gACh;T#?zCk30w({LuG%qCH=8a8Iirqcv;-b z;eRW<8$M976Vb;vGHLrKeJ(?8?BAz9&kUR9oQC<&GV15Hhu{OQM*s)EMDE$c2%P}~ zxd)Y5#6NW1^G|kP@+DMgN+()(dR=qzx2A38qy$)l)+gZe_gGRs2YT1w{&&Y{WpTsZ zAf-oL8yR2bJ{Hl3&r1(A>r5A8BjWi(cP(duEPkoL!~VGeU!hNR`}T@EdLjE86@&g7 z;WA)$l~*r~p>!-Y@H8}mo(l9vR?2IRjFj=xNwLt3!0U+XJ$r!DHPr>rgF5K! zukqAu^#ARm%wmxP`atoXrbu|ArU%j#ob?}l^G625Sw~@$GNOI}fV7l+;H1CB{J+7( zW{vKICR6Hf{HLd6A67D2H3!hFH0#S#$&RF>#SQ0cx7@1?re~ze^;KD3s_iMzH7mVu zuIHi+R~(u+#j0b!!V>1omOy9Mt8}dVClVpQrLApK-nm1d&zb|*wYZ4UD31vt;-e6Q zdyrOGdjZ=&4*E2*gu!7Ax~@6?_nA7Y)NA%%Isz%HB|V(Vi>x1MLYpPtH;a|xNd6Oy znn?AQDuo{IW;9XS|45?m6$7wf5JoULK7w}WXNXm{EN~bqhuLmnG=4=#A>~WI5$lLM zByf8LEwWMZ;-ShW5{j$^PV0zHFSr{N1g>E@e?t3Z4@;`fX10C=iOtR{#YG8M-F9zd zTB(*2;yE6PaW!~_(}YT0WG>_a5q`g>x2Hd0v{v>`7bT$MK(=d_dn;Q$qsz~X8Yp4YIT?`f@N9@du6c7Y^0`Bl%fj|o5@PA^FB)e)v zggtK>q*#VLo@S8-Ip83L5P#`Id+x*Qa{4t$hQIDgB1m^hQp?C4pr_9$gEfaHeyd-Y zd5$@WfG5u4fj%yuLH|DQG&t%L76q93MUs3+u%x4`0&DIm{r2{!V*u%a`1?jJE1Ww7 z%Oh(Ddm`(9K1p6Hh~9tRb41tGhTDD(GPTtvbTC=2KDT6}2WL>YyIgZIi2x~a794x6 zAHl{`E0-lvDX>Z{%q>>Ww2`(7Onik_LQh|bRmw=Xtv>hnuMl?rdD=+GeGtQEyBoju zsQ1CVq*@eOT`>ky9`VGwPgdMV8NCPEr{V=03wUGeLfxdHM;P_G!$nnMsp#_Qt`976 zHYxa9b+rw{guu{5671!iS^H~>_{#&P$EK%r&sqVGG+ePUhgi)VQGxU!h8>#h!Pmj?V9_9GiX81-%|v4m@1R3WPtA(afFRnTi66S3+M`a_eJf-S@d0)~F+kk` zYD;cFpe={N@q&&bQYkIzg?ob8&<&<=#%2JS@^nX&MHX~++JJ~#bxvUddId{}lHQbc zwk-spa)F|-eLpd%G5=_9G+>6cEb7j3--JP+!@ZGmUs&jQCC?M+xE3w^{hRF8Wa1z$ z!Xux6p~Tiu9)stp20r?~DDs;I9}3pG0QJilI35IuF?f{eGP4^H<;skPO8c?FmrA(Jj+i8e%9 z)T>|qWdlc}S5+tu<%gQ>Zg@)#SFuOtL3*$B6yV_4Il8U~0%K99{O>0c?oezpBM#qH z%?XFHwSf!~OUa$)$5E8{4ohu<|AZRKhqA`R-N&FXNq>IkabFh$S%=J781hS%)cNQ8 z)H$E-Ne*+lcN~bG)v2UND2S)lYaQV!1XhGHmKWS_YB$bRR2`5AUkp z;9Qs#5rTG~2N8}R9pD>C+Jvaom!#zo>uPZ8uPZ1PO!A;ZMFq7Imr}aiV0?D}J5m_M z{L3=gKrmCk1C{ip;CKiqDkWG!2?t>aOAR(23C;IV7iOsN-hmCqvk9(Cq*tFRU9fKc zr!%H#_{y>B?3;06^j%G3^?3EwQM$jx(vFST*5SAiuhWk)`a_;t#x0k!3aNBKR(EQ$ z;lg7_Sv!bQ0mMKJ^2p8Kmg`WV4)UrB-tX>CvuQ^rp%$f#sp1`gmQ(j@(qW|WH+2_K zkp=T9{@J??XzDQ6q&v11kIriXxRS|!TAu4OiV8=|)%i^PT20w|UX3iiXVV3Twub-M zCLm919^;*t-iSB$-euX8U0AJ^l7Zeb^Q95y3EQ5DinF-)75~`22^>Z7WLZME{5p}7 z1P=UYB3QnDds*j4~eyJ`|(_IKMP3Cj4zX?1}k0&4=hZPGj1xQF|(dh2e$Yr2!XZTpPyYkki< zK+QQGq33cA%p@*AC?cVE(psZM6~TEXF8a8RD3j&YocFWA-{JSNgZHl5BZz5u-p`<+ zxfEgvag4}Bim+x{g%0NL_r5)^;4s_`qu z70_Y+polw$#oNhCv0N`P8HvoLXw#ZGZbwm31l^(^GQW+4K4}oCg$(69FkR5VY=yGU zX-lAP&U(1<-KnzH3u79xyQ$u!)||h1JE~|js;!edwYC(iucL~p}ri1 zH}J6w40yesPAQ^m@)n9?87C|EA=iznk>JNZuXdInWf7+vydNxq#`yDDs|{v?EF>OU znVqJo&s47Mp@%6+^-FIYW5akXOaGc#_Ph7TxOOtBWn#iDyCE(vOLN4RBzkGIXALvv zVjaFTp5E1OlFpiYg0@-6U!vblZu63}R7FHhxB^BT@U(mUQ3hF8-^BtCH9Ne#8Ch62 zYTqaT9yc?Tlc?0ec@bE?`tPIsk&$A5QQ`Os^M6IN}vkr_t;uIzcc627o#h2kg5VT0oBT|^f)62t zuDodt^T$X@9Uk@vhH{akd8pmge|}vi&!bFUc;T#8rI+NUve%sE*2md}zygZp-o2x@ zoh-NM$X5}YYNgA7_M#Ubps@#Wi-?8LwYJaJ>PcQhM zsBs4?eTxLNOi}z=Pxrx0{Ih$@x$%ODzf0XFf>`5)@`qmkeR=PmT9;b*T!9Ta?H!4s z3U(3*2huBTnk=UiPj`d8JK13e0_E){;cRxtEBlE2CZB6<^ON8(s6qdCYQ#TnRi`by ze-#$}pHO6*g}iXi5Ma|;C?8%23WHT)p5vg z%eX59qJ~pdsUpXr5QgE4l)uE@CR?4zHzI1$=Xy-JdOi^Dwu@1V160Naa*f&D(hEHP zn02kIzf7nq6Tp#~O=%@V|7}9XGQQqz5R!R`A${1N3#KQtIGc@mBIWBt2+l9t z0c6JP5rOW_DhoYCqsK0B1aNtdi!}?8`8HuTX~@s^?Gh~y_1o_f!dYp^oLhH>4WQ9TOpuQ}os)FSz_T zQMvy9)R)5{xy-rR_Oz@}23eq|YJHUXN{!-Byl%Y)A(*Yz@kwYRLMx+^qcM9J zN4!pI&skY?|NbYwDHN)d@w0o^8g!haZ?8B9KB?aKilfW{Pmd_np9k*^R`o@n?Ij=d`k*i)&@K5VKg4@~TnW+6fT-D>~VnHC;fTnLXmMq+6Y6 zr9gU&$A>NMPhv%_>mAW9Li}aE-pp!RJ2c2fPl3d9O&|%uCs>+eAtiGBbzU7e40YxC zefReTg;phjoQF2LXSBcr3~bI^VmP4}F1k@JkrI580Z)>8-Y-Y?07&pwiU-m|KlM{a zg|O{(sb(X(P7*7u2O~2sF4!WQgPR{HNo!g>t?&-VlVu9=w4tzw2Xh~W5~7XBnZknrg`p+6{GrR z(4l^Z2}1)7eg}g_+kcOEA5N-wj+n^+dbWH8>W=h#^9S%#hYX5D0iUQ3%Q%0mIu1%~ zB-+TWbNycT{4Hx2V@HsF^BQ}e0_YDeZ@`xyw_~XZoBzaNMU~@H(kyH!-?~vS|2?uQ zFRQvuMS{G=k}K4k(~x_8D?}GUSLE}qFlf;c=QXveGc{POM&6=a$WFiXi~)Np1KRP zdc_AR)3UAN{G+eI!pH})duVlS9{BiPjMOk_3(&747W<%TVK$aBe+AlV5E}sUju6dg z^*Ymn!^^8AOMZ8}f_hW0<+no66))bzz~_?xuU2&BB_~mn5F%cH7&C7(`_fU$qRs-) z{`!k@4($mZT53-r+2UevF=BN>p@Imi%Cef}n+qFPifd({6xkn|QHx7+2bjXFh7UK% z;HS)q;qe}*PBskZ4=-$_@Mb_E?)|7`hcEk2t1N&w0b3SQerL z)M&Ov-GpW4N>B#HSNgg08AH$+bvRB*@i*N`&h}{5M-b__Hf>HIx?@~8bkXAdMY5ku z|FH|H?ig3aioWr(YJS`V%=aDc3N86p3-CX?U}&6>i?~<)AJM%~ zGAVH$GH*Wki!f{Tz*4N!RsOBJQ%0;yVDD9!L4(kjLL1|e;Whi`v?GxlEH9*F3S6ja zoxSav_vZ`BPI>R$`buc%)&%AE*!SKc>k*$5=Lzx`EFW@i%92a^0>U9D9L*P+GnIsj z8o4pLpkvdWzo}@NApf!Dgf^Jcnt$lfm2d`$o(CDQHFw@4O5r!>ndxVIf{Z|C7Sjqe z9pE+nfUxU;uCZ5Q*cC;-mJGVAzD2cpxA0(O87F1Bc7QM1=GNVn75{LN%&{=xU_o&^ zC>+9rrV=_!b~$>&qia8yAm!|o-Cj;=*||QIKh*GG177eA*D$ZTD*JA1`Qc^Jb_swv31mJrpGv%2 z&Ooy-tQGrwT#tB}_>uH()@EMQi1uhaa-)s3gL5P)dsA(#MZTOG)I=W*8oQrBtltq& zjReP3&eK+QDb86A4ZQ7Qei{WreDj9yhe8kdMb#l|hE$Q3quiv3sn0|%((+w#?`~T3 zXh76D*zHwS0D8EpC$#OIR1m0w=|4*HU7CG;x&4a_sW+D+VuQ(tl>jX%Mv``k9;E?v z4$a>9+FS=hM(WDDQJg=H)sNlmStnRH`N7u2H;xn~)nj<7i`gOLk9_M&KNZNKe8)^d zjyaxSp0O-hw4e)ihmA|Fyb~Zb>TfJ;@!bGW)WEO!7vBMbE&9)Py15lSr6xaHzd?#hDZ{rIA4-$|Tj?-Qu+b=)3Ah3GJBQ>&9}4HBaYI6b~yjDC@K7av|Xbp42oAp&378eXrIsL&df} z=K}OZ!Xj3OeqwN%XiPe_u(a2nadj}>HN8>(7!u}4_$gEFqGqD@sx{xb_W-gNq1(iN zN%K|Z_`ys9X}Ngr)cXiN=v4tbYPwkKxy02);q*!+qQWc&>6qu-XCVa6U>P(#r!*X} zGrsM{a_ZqD82VfMS#T7stG)}933R!Un{Z%~WIpfQnM?}t7F`I@T5a)usj}{BJjS2c zq!SVoyl?&_zU2W4X@KpdBp8YQwFVN;_gJgPTEP-tcRhqjD*&KX z3OCwNOM$;mBakA}#1LV=ya(zqFpTj?$D;6buI@rS=U<7oblo)-Y~)&$s|nRvCJ?tu zKqO!CLx6IK*C0|8{eTTwJBy{2=C5mA5)i6Q~U(9i!re6&)*i89}MMNp0? zKx(B(@-NYi*8foI4F)vld8IGN5j&1kq|&=HSGXuv_ZI+t7AU^ z{LN^Oyp#DsuYZ3}+|+gD#aj(holZsRC0qzFd&PKY@KLmCf?5Q;2kXQAoCs5)k=0P%ejKUe6dhHU6`fdq;rOd(- zf5uuC0}rlYIl!KaIy?1JI7vBc_TLxs)r?|*RvgeRgptF^8Y}1V>+*%P<4TBJ58;JB zGYHb@ATyr7ay=WRMrnBaBWkgv)?giscdrqRrZvw@l53Cb66UDSFM$t|4yf6Lj@~$G z?NA{bgRVfIDF|#ure6l{rj0#B6z#5(#>4NCuMi&-KaME!L&sM2Sh9Wk@aBiQ4VuQ{IxuWH>~V0@ zDcwTWgh5De@`w>!blLa(zxA_GX)Z2?S8w#*e?-qe8N&gDd;`uLSzPTQ0Jgg|=zuS} z!-f!|o4DE30d%O!?NAAWwlaA}sE|Pdtg^Y8R1pGo7!wCYY-ZIvM}Rv)`5>$(KK)KvDc)0RcL2WV3njK3 zcGy;Ad?S{^63=0qZXdRaD?&gM3`)_~_^*&7ic;u?uOQlAG4CNGkIHIS-*LvjiO_7X zjq)VL!>|>KQ48S~Zik#a^5{7`hz}5HUnV${`pb1G=SvClba(T(cWE%vqjX83DJ~uP zuCSzO89}`59kdGQtvav^ce3pum2IYr?OMPRHxjb?p^1`VB|{ zC&{F`y?RtU2B`gWP+&FRoY8B?~%mUERL0ym?g)nqJuEmglPW2S~aPUi~p>M>k zB;ou9dDmy2mu;{QPiuu>-OajT8WYI}SU7&Me`q4dyrQrb7;nL;{?$o1kbNzlj|m5X zQi`;~K^%D=YXzVhAuhXw*L)aWnLm+cOSZ_-aR^z=<)yt%A5v&hO|PvvoINZN|}E8aFrk^YXev&>k#O#El9eg7z5$PASWMkIKp0g?^^0KPr!$wqh9+F+)$MO!*#8xEKsg-UuVP>U=Sie_E%>z-8Q+D$$NK+e0{1gfO$d}W_n*SZSqauVQzd~cw-w|PS}I6>%N zin6GsH7T?7fVpR$!7gFhwou>~JZtqQ!4)@)Z-BMTf_P|Vp0_^Z)j!?KrL}&(Egxep z|63j4nv--7_%|<;edNw$->G7Q2$9}bI$%vkL!$g; z(*R-JGsqiLYqrBH_k`Ci`$vhdP?*3Au`qhN6P^zO*MKJPTm_iIcH4M}*!?pLfQw&l zl9Na=A$xrSM60Xg($9mhM|pd0D7g;$+t3QTF ztsLW5M+KdqQ8&9MIlcZ$YBiEG-RRGHM!Ro3?7i|$?v-e{;d}9zNJD2 zc#$x8l7Su{yl&>`inWAoxN36kIDWf?paS^%4%G2aZTqjk^8gTcAF}C6*2*qb{3?fN z_CKa__0j*M4Ga40Kok!wo)I9l;~G}fmKdgvs4B8M=OzVibu#6uzK9@D5fX&2< zSmH@5Zc4sgzXJQ)jQKJmg#J`zdz@S*(2lHhjN@y*sgp=U-ComnbO*{*02h#Co0VDi zju%>%Ww094d8Pb}6ka}=M-yH`j?mC;%lrf1bi^I!q1FCrgCA zGOQ;~n0)i|jb|#ED4vWq-ktJ}yXdriUqW}LwCHHAy26qGK#TOYV=ivisp@AWw z&A`0w$Ykrkj4nfythx0 z3*&>{ew=e>;$V++U-aK?R)jw?UGWOt$#=YE7LhGG-* zB%;($0J_rPQMjc@6`2LJ8A+`A>|9EcL(S?ki4UXZ1qlHU6_H$>_e0^=LFIWyyUiYM z=YmHqV47nY?vd^TDv*(}MXQ{LIXO5_MbRGCy&UxziMzBXykfRgyyZHr?{N zE$SR;jrsFGm()T5biJEuA$Z%WC(KZ^r%8W))sJVZ!z97gW_Wiz2R(I+6;om6U_Qx* z7^PWgzzL2ER{#-r7e)$gDSN4z-5QV;%RlL_FaTyZin`=7A^J1oLx;;SoW)8Tf%qB) zpKQfrNXxRLzER#@d1|W9GS}KA{($L$X%ZNSQQ3j1ZK_{0402CM@+=xMSd~&em!@WT zAe{Grac#I?Q&l*M_AJ$TCQS%HI$jd5Q?GBhk-JIXe_U-7Z?A5QNC~d%P=$hXrcAI= z!!z)Ij#yI1lU`)jgs|pOo}@txjK4`Upl9+j%hEsGYZkHju_q^^yyv;VyUZD?a6Jg= z%h#%0OI;gmLcjKsl0e6A*(!MwwKf~{*1>YJaqCI~aQO!Nby2ZPP?no4rnQI{(I_2J zu*>`CnvVVI7u#ki7E>zFy&F~#hqO7>Vbzz^!_x=dsXI`Y9?=~euip(d^BnF}GA+w@ z3Q`@$vi;|5G#GiVhd&u1itF4@;k&ifY)|wcfub{yxmu> z2yT zXk?6aa8VV7M&;4|o>N+^)NB+kMQ-sd+-z3}%WdYIE=e+C)u$FD~E`e8_^_p zwwxD!4=+y;VxdjR)w7?!(je1tV}rk%I@f$h4O#wcMdV7Z7SI0wwirl|JvcmHNzSt3 zqdKW$N+IJQS0_M_Z~#wQW@JsUik1E63fewjDe6T|uZW!VBV3a3-2yGy15pihTS2gu z*;zFy>^U9!kC6<1qBH#-!&Y#j32*7wT#&uqno;$ewS|c91;X33g;h2+p@iU?hfoZM z@6ywvfpi;ZrAvoYn>Q9XjJBy4;u9@Xw-2Z~bk@{Iqgrz(3?ACe!tY^K0o#jlS88X* zLWVNJjl9RfEL*%7)XO%9pVcWO1K3r|c6NMy$km>Zaif3kSNanUJP*dzlWCNn&B#gt zCC*|)fr727XjA{e(v8E#cqw$hzq)UK$%7<{!4Y*j^h#{WMR-R{0q3aRqt$#YvOM!T zD&$+kcUI&a%^Ip*FwjUoxmh$&x>mr~HBhmTe^ocT@VXY(g$5bQb=rv;d^NnjeFdpXn?+^Pe@cmi(Wq zMbIflBA+L7CYT>wFI?HCb%}DAX1?7H3T}(`H5TF>RA5JHr3v2QO=&cCX`I@)MC?IN z3Jd;7f2Q2@34gVvaXP>NFxT(Gn@b;;Hen)0*6tnVUes5o%TDm&+ z)1mBTMEUtGP@sO_Iepz&lU*D5#K+|fOcaLisE%GCMv}sy^stW)x1X+5h<{p-(;=h` z<3b5aw7Vx@8}&eefpYIci3`g{G}Yp{_s`OZ7Tk- zWX@&3XA~=po)IQ3FTTFvfAzVD)?Bo;^OkwVWS^?436wizjC$bp?URi!?SsBXSDAuS zCME*Zx46P+Fz@4|Ix?-Cd^V68^@{x!L;={wA6lGg&*xsB7G%V zQmZJ#->swd%|0fS9_f6~Iym{?z%jxp2^yn})KlGmTPb^qVhw$EHYt?JWS^O-$utpX z?LKqQEB>hR`|b03TU8mm-%WaE>$9Z%<`R`uYuePicU%I^@31c=AbK9kyPIP-V4p%J zmyxe&@8_s>Etjvil^O>qn%+G6`c4>8KEDrsg1l=2?#N1;y%Rzf52lf0?4JwYbG38rf^2bBypQ%Y$Qoz)RO}gB`HyPl zMgY3S8zZ^t)j|9;5f!ycc{6L6mOK9X^T9gu-h=gG&WP(q zqFxrXbtvr=m1$k#d(x-jzMQTm@CG;^&GX7!jV4!qarHu>Qc-^iG}fuWWTw_yHl$4+ z|H(ZohYI<;p1baw&{t(r;el}n$X{iPD^#);GtA+`ui$VZAnJ7cf@#~sE2b*J-SJV8 zjeIurEQ+lel(+aDDXj{AjgN|UKHCx5zMz>73IrM7=<%r##4mqf$bf0On~iqCYlLIL z=jB0S=RY147DYOd$B8Gb39VwAHVgI16~45lU4GnK_Q9&AxwBRu@#E9Q1i!Y?;NM=q zvn}kfA#x!bwC)P&!3B|l)UMAreVnz)CPZV__ybWFnI%mz4g<6036?|0ZH^`iJ}O>g zUg56|#4*yh8cY%zPlS^Swa{>+4doBAb=Ui|}t!|y5 zZ}!N6rtS5It?Dw2_`mf!QodKoC1Ex>c=O##P{=I2Y@&hap7JJ&iYFs&)?ys#R+F0f zI*=Q*mYX)adpa%!4;#|RPV1zsy(q0B+WDY{b}lbdGfSk;+&^4W`P^{kT%`Pz1|HT? zf0DkGmtp$&9&sESm%2SJG-l-VJZ)aI3oe)&3r%V{v%7^}NJqC&TCUXJ8H^m*!F#>G zeBsCXjh!A%h0>EhEjF4#72Qae&Vgh28z;R7@b>8a9ga}GVsi6Rly2)-r2#^iY87>MS9e09 zlKN}D)SJA{h6^0?h^JVKF_%GhrE4d- zRRHvGkt>o@iMlyX#ETbSj}@lLlu7suYnHsY(qXy?uxK{>uBm?>FNNqa!oPo;|zIKKtyRF#89N5pc`G_seg)EFCzf zTw{Aiycx9Eb_wE{5rugT$*}`1)-jh!dK@h8wdhLqqy>Kjwbi#DR}K*rb`{xXj2+nW zFy>S)QF|#|*>34@bL}`+e4vV){bfg#TZ;&(a#HK8jZRqG++;La5G>r1VMjczJX@jT z(9+9oFB;g@eusKCb_=5%LI?Imoz5{;t5>M77)eefJ}g<0E6z0>{SPdj)e3}qi}c%s z>KB_eQ{WtF`V_BXO8e?s?4Vm6J1h<{X~ejnooIc-C22yA>KMBfu!CrD)jq{GoRMDM z!gn!yBgI*x(&7UChZowRRp;R+@*v-c+X>%(^VAWJYV~)`Bx{`g&5Ps18amw>a_+Ft zh)FhbG(o~p2)-NMrkz0FeKv8>F)iBarAKaJFiNLjHozUTIE!k?nZ1HNBdwpE#ri3D zbk>(6^<0fD@U~r41K#YxoSR_e$<$EPtVe)$f{xL|8SfDeEYoz(M)47UM!OC$VO?gdKBB-g|Z1E?b zS8T@{>I@&evEaNMYIR^(!pZVW$%iva8)AKI39fZn;K+f)1OK20mOh(k2VW{b?i541 zYKFK_*UEY99(lKb@2Z{r8Rj3MWrV!?=3aN~@_L=cxlsl39Ii8i z!3GQ^*t)fqx6%u~_F+qoeGt-$WEMIP>le0rgwD(hGVB3*#{HM3-rR=tY;-Ml?=0Op z9%SI@XsY?=ls^}}SyJ2iK#nx+pP8gDoL;*y>r|3xf8(xd{vAt3ASG!ko;q}LJ=+lx8-QDgOGj}Wte33EKHQ<>GDq` z;tF?y|HzL^>l7IuqKs+YD+F2-{pJxLIUj*atDk)11v~nFJg7CW?2$F>Vbg;jhU@ZJ zc?Xjp86+||4e@e_kZ`A3wKQFmqP+cAk|%UNBmDqQ4FwMV_Sj|~dAIx+OK`6?JIo$_ z{d7dz!Q~SwHJ{{lMa)tu{9A^rUitb`AG~i_ph`361hAS|A!=;q%vT{}_z*-HoUyCa z$yW<@!p!i2)67vl(={vsL%%LKeDNuG6}51?J^w=zVwc%1;Pj5wc+_d{nL-9^eD zye4IW?;5OLVc@V|Ns5cznr$rAF^}0ec!ZorV@!`S+%=*c7&_d2)gk@ZCR1hRMb=dY zw$t5=r%YFE)KL8=cM4JvsUceG;i(7Vf+pGKon`dZ3~s!YW@U)}!xLyL>V;Rkp> z^-n5PQ*wi&r}oC}XIGBRxH+ljiu0H>I_I!d-oTV0%zu8(Y;SFvUPj)2R`zUk^|5S; zuSY<|^8n)ucu6_DrMShT%TXfakA?KEyNw@XR`R!S^idCR&MvzEM)tNW%V(yymMhAK0D=occsGj6#*0oU?UHO@@ zTlNy;rMU=hg@|(!2NSx38)#5<`CY&)22lf1gP4Q^$PWCVF!VJ26VdZRc6$5K@yf^Z z%**rzU%k$SL@*2J4L+zhtP@}Uk^b8m?DwNJ$h2G{_{^uMw7u8GZC#9Y)OTxH;_PAM zlNDh@x`q7ya>3?;#=|jmM{b^_bcB;mZSU{?(~IAnLLQ5Uq!8R;H1lx?aRSFWo6~I* z&bKME5aE3W1^0zkpy?;F?ceeKCuqApHHL}}fO_j->=KEe!>_TXcS!_0aevq=Tti7t z=**0CEozdRJ@qa}g}uYRKGhnWbcc=NIX;%vOc!755P3JblJ_De@OzX>p!|nZK^>ap zofyYY>g{uTZzh`Xqd)jJ9Y>OXT}b0_4Q7?iZZ!YdgyUsvs=Lq)`qkCDjg{nm941c{ z6bl2lqSs=-R^nwPvR}CbotFrF7krS~Ka_D0G5TeGL4UM|a*ld{uj-TLj8U9Lhx+Rj zAk3#s@@KGpnQg6A1-{54LRyT`^S9lSE5T-a4yb&hSB2>a=dWX?F<(o(;hgTdahXdx z4kC+s_?4_X@Ec#j|$Zv%lBvOC8w$Eu4aETe~2!~;qKaGJ?g-cT*wKpXc1CfZGhO9r>| z+VHx!@3O%kA`_O?$FFgt`s(?<6E=@-g=1SnUMX6az=cRKb5v$B07n5pG*{1FuP$%@ zpQG$rH5=hf{Bx2WIVGD$7`k)#Q-3}n94TSHa85yDLr2ZMht@~BN1DU0YzTKNxqWWg ze>a)y9>DCYUy(ThpL{))XEYB@19l5{SYM7E%$eTMy}0)5{`|%l&6)eSfnVEm!Q%o? z;#p?AF8FJ0co#H4hh2fha%X!vr0KDoH3HD+V*;F5aWtQR6TvI?Yc(|0PK zw9_=DeBGY|Z&qi#?6^fEeGiPI;_&T-pHEPeNu05c6!tKrygjCRFD85Caas(km{r_j zmWx6Kj|l_j$CchX$R)SpoTxytgk@$EvUt&rWL)0yEDi6XU|D~^Yv4$T4524CT~~Nr z5i|c2e4Dd&{uA>FS-+@aWZa@fXdU_$9k^+gh6auu?OIe^P!*iE4}*{1Ef;89viF5F3Agxblug z@P!0z*nsGyEL|8vq>i->ItCj(nys@z@ayhQiXEJTEa_9qhhU^v$yMcKdJ9Q!W?Cz` zt+Lkp=@#7Q?h5bf@t2;N)xSy0VAsM~@kVOxw|Q3S-1Nv(GysA`d0E3A4`b$?+?bfNYxq81?TZenT#|Q2d^|jz7&3gs0~U5F-E!5&r3P7X z_YkZye3q5h&{X~JWx<~d>=B(}6J3|Nn^oG*sqxDuTv0(H@TiXw=5E2fL%Sw^wS-_r z`*Y04(4l49i?_nlVZSJL5Z{Xc(}xJ>G1SJwLUvlKsEaqKht@m9QTM-3nAJTeF)~j! z!E96>9RQVT1WrC={)U{put6Haj_s;Uc9BcEe)mSd`=i;{uVXjL`2?Y|MxEDJHFDFN zT^(u>&W-t>+fs;M%Y-+w`&Ap73Vi!usbC6hW1G(eyXqQt=K1~(SFpBrmgtnhY@P_| z0O1^Yyk1kH@lyeO%&@cj$CEFgjKoHFc8{fBTeZogBB(FPyL+6~bL%E6j|ptXo#q=M zQQ~^L$49Eumslp#R>D?67@R zCAF}n{?^uEy(mb~>{}C;rHVu-^K!NbYJ|E#q7mnfqzk*(>&Wwg2$vGgveO3L2JSkj z@qQA9-mlwrab`06I`)a6J8X`NGMB&=)yM4WlHyy;r=Z1uHZig2Zb+cRdPdN~t1OMw z^k$@Q_cKI5{!Wyb8}9&K7v(9RS>I}}l&9C{{kPd~91UWTvmfC!(Q9O>7nKNI1ITf2 z6^dQBKa9praQLZzq@UDC@PbO?6Nh32MZFZ3m^CoK@RMBBgxD=11UI6_Pq$}Z6V0e4U-zKtIhd^mgY3hl|ncgFHf~~!f4bf@kuDU3|ux*Gu zmO%zQQiiA_C$(T&qfn%&>SyaR6+F(TMp@6c%&+r znW%d7`+1SNZvu>^jJq!uW6$-POz!RRGtB1@Y+o71<}V-BgPQ_~5S}Z-Rw`t=q3gDS zrWTQR-@S3ZG{je$se{J;)9G70fpOcI1NWhV}y zB2e8k6iR#oM$GbYeRoRp@9EoAu}_ZrvyBp@o~W8zZ3VAVqT*){x!Lc_sD?=W&Xj*+ zXRv+U&IJ^gv4e&wTR}JQg`+>tNNo6yOXnE{Fv}^)M*?!q{5<1BQRgxCVFKax{*a`6 zBa{1mWRawmYwZ)pGIZzS9V=%z8d{F-yx#c4phMtd^G+56Tu3rekz1vewDdd9mc?aF z?>C+-7S+h?S{%W0e1~`J$%BW*Jqjc>)Sz*q>FLtxw@B5WkJB? zW^1dzSV*2+2}Rt89j!MI8XH*g6;HZHzS!pdfgd)TQ;OH+(bn&(l&vt-S?mE*i+<`| zgCu{Zh7lWK8Xe6Jd-iReaH;Yl2wjx1tfsD00jk0hfv0^uFtu+@>v)bCL)g%QFUSsk zM#_Sl9LrkEI8e=;D}gQ6F z{zG@!#?~M*^)1mSx04rIdZZ~J!)Pl;^4nA7RICDDx*QOSjZd==R28$5y z+|>W*pBCa_G@3E&`fYH4tuIj7T&^uG>a2y_Q$)&LtB(`5cFRF{!h&D1=Dls^U5yve z3UEN@O8R10tS!Le1$#Q~a(^ASrmQFwQ-jpE76)XJ=|WcGmWD}ZiMXD+gviDSQ$`N; zs>AW_tpH=*n2`6r=T>`xHam$HR6PEx=*bxYj*Z z;R>oN@I37bwS98<#X|go+evUV?<}c*-e>UTRsZ)kpu$~??5!Me%K$gv{rY5uU;wIV z$xGB3@XZMQ;bjOr2aY5Q=@NRud$p8Bnn2hSr(rR*2&JyMj?jYuLyQ;yuDbjXUU#i6 zfRuiJScX#})O|)rwRt4eN3dDSbpEw1v&-5?AMA_w8L5ctu5uLu%fqc_XcA!2+J~6c zZ)K20QD(MwSOT<;bLM0o+D1whwkbk0$9iKe9LmQiNy_V_m4foaXfdpqI z4gvCPP!F2S2@X+!s!ck{G$D=9HptxxvYt+(G|-p_6^yRgxZq5@ks zFoB`!v_0mw3}%5C`d=6^K0QLm0j|DwbMbL|r0K^A6z0_ySLI;nHC(A{tdjMAFBWmz z5W%+9Exz^(lqWIcpHevd(8JXhJMmXLL4yP-(P-ex5jx1N-zkJ5oVin6hj0nK7P0CS z(JJ$iu#&^L3px~c5-Eyw3M^|1aS-*91zoGYXr@8XK2|O`wiG;Sud?Auo;gtKJ~>o$ zp-p96saUxUHe*^g>|;S~d{DLRaH-QC&Tp<#Zp|vz`D9FPV!-+EbqAN-xARXT1u+A3 zFHd@;riVK_)Mm~tk#f17tRlz5Qj>zXpAx%$MJl}MC$7@(o4ixOo7J!sF5Xz6=Zb%k zS_SS_FF(t%mvVU!BS=Kd$AFO6jS}|wBNcS}rKo_R%^#%rLtyGQVg z;;67_@iz^{i?Mf}6_LK^^4)#t88g1nn7mS=K)30ES#OP*fuN@LlA3^>=&MJ}dKIBy zU5O3omDVdm=R}vvmtZUi!07mF8exMK0L7o*7_sgTQ2mQObVVq z^WRiIo5j;1ftv5fw9b5zKoO>$Rk4kK6E-F18B`cI^)dSe0BdWawlc1t?*8z}N1GOe zsog&7-Xm^_7dQXuOC5hJCL+9%IPDI2*O3$BHd%FIPSzlmy-i<1#-gEfkU;M4oV55D zjt1P@C~@ui*Wbh~@SAOX_U77mvv|52g@TAaE<>*k2<9yjspToScS_x8x6F&A5Y!+$ zECK0UGAK*T1Xju$bB}GFX+?B5q!2|LqudNQ%HPfCq89EzX=}`F4V>zZ=PuYqft31D zE?;_Ca9nPielF+f$;qF@;p2e44e{Q%PvMM#udWr}o|G<}fB3-Z0LYrG62C{bCk3Xj z31eG{^O6fkN>d4*w@V2#cd4o5-LFWr;X2*&nES=?riJwMaLG{{MwQ&Adq0(t=T*^e zuSUcpgF3j#p={0l+8hdvKTkgDbyV>Y?IYxjFou-?v^Ay1XPGSSEokvB=pN2lidPS> zHYh@`06H15|LfLFcl?MOd@b6m2@QBX67`&^;ailp*ku0{d2%=D)0Wj}cKTiq*<;Oq zdSiE%?16I2EX~ibsJP`~w`kC5%W8Q}!g@hLQX;T~INY#xSkvun?)1EvZ>hvQhzmE_ z&dx0bJ!nf<5PRjLO#!Mzeg{>0T>57?SQ7g&HrM!UbH60X0X1kLI#s*{Osz@PdH^7*Pl+06r zquQ;))e)2X=S8Yn6`1MH>vwo$`r}^B={I3>=!el@V@zyD2)s{&E zPq2N|${E^F6(+z-NY9%xj2%2I5)P^iIK-eEfV07HI;S8;jpzOU${xugs2{OZ21fQ1 z^XB^(Q7Swd>{*h9jEf`lqO z!cCLajQV%3N6m3o_jx!*DZ29p-lSrIlNys6aUx2TOZaIQX`kEl6T7dY~eS&1n(tY{Aer)z|2l{zs+3eKlQIB*&I0oZoE1J}p;I4!%A@53;h`Ed%QWI83 zEY&1-BCvjcCvGfuis-@OIwP|xe?tazm*sP^bs7N(9XnELDm-5vCos|Qu!|4SL^iL2NN|*Es zQaGw*P#(FZI%i{)T=2azsnCZ==dnD(km!b}OPaK%X=A)WhfVzOhI{okhzt7>cQx2~ zEpD+LbM3hP^3Rq=9mmV4kz1Ug+kA;E_&yKn&G@iiMXboPU!Su6!#J?zdg5nK=Px)~9TmY4@ zOqHxK$Ps;WwoDnWhqgT7@b2Pm?jwbDPp?u&qq?OzKx;UI5Pp(ohN&eEycL&kbvJI4+yMWq#`I%^u^)Ovn zlsH<0DcweRwGhS!EYZTYTb&R&EjO<9U-v{51xv3#$!&+Kt`ft1_?zk(tMh8!eA4dcws~y@aNC#W8+T7yAof%IjB9r%9mgTy2_<@P+M- zz0f)=u>?Zy_@Ty;4^}h1ZaYMF+JcpqvbCZW20tl7J|^o;P6R|*4&W%%=!3NW_DtzUXq=d{P!d7vcdKo7db5{9t(q}t zT!OJ6)L_m$pa220mTJ~~lQhKQ`c$kgrh&DoO=NA8kn@yI9ti`0nh_Er+A(3Dj=^t)WhXb?LtA_i;NBA9Qe^Yvd>^+ zy@8l^aK^fV(Z{(37IM#GUJQOzCW_uID~DLCt}3yiMQtO3bcrvW`2w`j$yI&MplOG2 z-jwu#`EI^J6e%_?ZYH^(riXD8(V~H*vC?K2_Y-HxOP^tD5M0NZ*`RK>3oB19d zA0M}=KOLUa#%C^>P~Cyf&xq`jIT|so!SG%EwjI9L#Td#D=b=n&oPsx{sodB7udDW6 zO+}8&rj93{ZDyD5RBPpXmwIi{UB#y2xXq<~=(M(<{7x z!N9VmnRJnQwqf_$iZhfFC@}w)rN#HRE(<(v?WcQuNP&dJmmv;? za#V*$Ry&7+t$snvCacdeITevm720j8E7Ks~c`>iIE-D%l7GLM%p^NnWs18AcFu}J{ zBGx(1nhqC@8R7z5mu@>OkIga-A`fkzH=r_`}SkpwNkwnsLp|GY7PM<`_h zm);60HekV&^*UUC1?L0}AF3?%3ibtYHxQ0awiq`3`gn5TOqiO41-#nNVcqm!CjghZ zz3zekw-x|3pHtfYsrq#<4-Y&~CVNX1>zm)CWg6R)%!%^{Iu34?Jin=jjSy{Ff|6L) zwOWWQzP~#t-&B_Z{KD5-YI9j1mAy6^@j#kU^NvSRF|Jy@;ed}87FAY%Q}BmS~iVn6%XIn0{ip{t|xsqpOk-$ zxi=yRZyQkk&p-g+fb;IWAMG$2pD2g6b=&PjevAD1?j51_f%lTiI!c1Rni9GgF2p({9 z%78-NmYsLR1M^`u(bi*YK*pv>$~q3lrOb5W+_t5Ot~rZLm9?US!K7miHC-qljv(u&1Zl zRtn!cQO}UhaK(!O#b|J-Z`*%nU=PDmu{>xB-lP^2=S5ZNH&x{j5|M};fO_vZ)IJFA zBCKiV(P`unWKf2L0Jc#n(tYB5?I%f;z?J!(JDzVsCW6Oz(+*pKC@<9bBXQIDpc}ZO zG1ZN2P$1h{fbG7y);~{>MG1hy-RNFab8OF0T)zen>gFKXqkQj{i(~AnXqgBz!_cjZ zRb&|ox& zsQ`YK@~724{hN1ooO6sqiSjX_(eqi$a~05@vL`*z4fvxz%_KHOUQ529J!yM)%4Ps= zyFj#P>U_aGbR81N)9qeT$&?{=Y^>)u)1ccc7Tm$L7U`Py{d27d-?P{tokb_=ulqKr z)w`w+>zT2GvjTI-snOWW*ZfzP$n$G}J$1Xsi$A) z%s47sO2Av&;pczD|NlV0-uP6;T6CutUwSHG1Yvy49bYZjUI%giw(BpU(6hb#Y}ci8 zJ6HRjYn1-1tkp}Q7s-A@zZ%H%wW)8#+r>N65MJgg-q+Q6I?|71+X^#|XTM#I_4~k~ zR)p}?pp{1l2iy=;IsUyCi2Fj~A8=GRufMPwVV*ZSA;Av1k>*zS#peeZRCv>MPKfTG zQemlo-BN^dOSI=$N@1M6{=>XV7S$YZ#MyvDh^|{Y@yuT2|0u6xTw}77#31$v0}dYg zQ6_)XfNQu}qd+)K8$IRVyg2tVx=K=#oT>!uEoWu{B;;2r~xS@_A z-KGdk8PP2`{)OEHheDch2ch^5?MZp@E=;BD{qld2^xqXBv0yO1=~4k&;Oh?wkX3~; zvh8)IgSF3sUdBOH%gkBilmW)nSu^jcai`~tCwsY?8k{EKNha4S<%6{!p-l?zFQ4vg zVbpky?QfgRVXS4S@Oal0$?+gL$18Na$%=*J<@Bd8`&ADBzy_?yR+Hc6a`g=#!mJ@< zXF4g1@Q5p;!CrW^5B5=ySSAH@Hnds|X1#bIbvzwgTyixRp8h%bdr(42u%1rkm?hu$ z%=!%Tfvnh@!PV;MER~7o{Ja zSU@S?{8Y?ab*?&${iT*$E#c66BmK=v(Q zWV<|8-v{#{v&_|F#HB0?fJ0`!21Ei&i;`G7JQS7;3V>aq_$bhR6L*c_VQ$; zoz1b9M6n{lZ@5RX3XNY=jXKO+trX>VTEJBUgIY2o$`e~Um^FnmUuoQcN{E!BW7dF5 zZ;=+Wz~$ky#qU$dA|l@;Pa6NjjBfb>v8#3pyNutJBpQnVs-vG6osdg|ecZqXz2ne#qFYV>>HCl7xX5PN?xX5-a^` z@wpQ!Nh*)(7TO@j@E5LIAua`x5l;uWMY{I zBI`9OkM6rN;@vEx6Q}l*e+B=F)BZ9aY6!FvXQ9?mmbK-c9F}a`2sJ!l9(ubLA+<;^ zf`GvyZogXXX#J6(miO`{Z;+M0{4B7`V59;SUIVFc8cDw9D~m9V96queSH%+VQmfr1 zg&G#(B_yh!R8bYNz>EPwWb;v)KG4i~J|NHUa8?hEkCNxPK<1gfBh{w~sCs8XL#OhiHYtPLlh9EjumiI!i1bh*>k~tzS|8yOz2Lmu)Ct&FJ+uXup`mIX{PHt z)+x`)gN`qmW#+*9&L@I6f&_xx3heU`%*XQ1bX!%EEOWZuwTp)i$6_`N?BC^o&yb-D zWBksz$xRE**EPh1jzGc$H$8>+DX&Qps^5}`PyN|xeAE*mk)zv@QO3kBecK*(6+F=t z#|q?$0{&Li9j>Pq4=#hT{*_Nsu`Dhi!OZGP;Z2Vfgft{x-3u^tSa%^^iQNjX5vF}2 z&W}Mk0hnDIoroPoi@B6+T?a+i@gNb2Mnge;O1CtLV8)8(7;@czD<-`5?h2`aFo^Bv z?|}?)@<0A(YVoRL4GwK3ID!&5z}P`nL%g-oY4PUrq4|y)chm%-ak zblcEbmitUUVsX04uzy|H^)BdT@KAv=fx#uin3&M3W^*3*;@bHp4o2 z&bm}5j5`8ep;W<7LsE}Xb@TuHIZtH@v%Rh!w4i4w$E1=TB-30l-Ei*93VAR$4N@`y zpw!k=DBIH1QaFB7K>55k*huZWuXDm@qivOB6vR-*ahP&oysS-(0NOR@%Ws_a3cB-l z3TF@ARn~)emjZ{fg!2z93lT*TJz_3rfx2b- zv*g#$VXR)suAJ2)bTR?8$ilMpt9_qr%w$vEh|t*m%9JSGyjVu+{^Pdfe;6;*%V~u3 zTI;;Y2Hmv74Ttq-fbuq;R<+3ma{)tF4LeWa2}4&h63qKIEEbgL>or6IOQNmL#H?B| zx}9j48Ogpb=Wg1W%ib9LPCBN&>RVW{N?_e>QTPFv-V0;?e#G*aCC?_W{L)0!=P13R z#U~C$ObR{KG|lAgSq3Z%Rgv5<6Y@F`QU}}V*I-9&X|acK@G7T``Ky9S>^uV(1S)4r zovgWOF})J%1vwA(Dsj3pdt(%lhFD8y7C1U@H2kIm%qU%f zAdT`6198Zn6E}vOUSg9@O&{ag5u4g)4MIp*W5z7;h0=i4I7DxtX@>Hz@&i_Q{1XUK zXCJWQ;&4c*lisIkBGk73KZ{+YegHRiZ^bLK(bYf9p>_wE=?V*W7LI5|24pdz8di~0 zwI(rtMCA<+&G?!{U5#5Kn#;rOq;K{9XFtVi0e*(NrJ4{5EL;&|rZu<9;1*Xy)?^KZSkQSz4Mu8|E5~3!79=31}FlU7?_EMX&*3U#4i#l(7ynfgj z(nA1QQzHfFEjgLjv2fTsD6W=N^DBD#uvvqxZ=U;A{C$8pa=Gk z44i`%8{QZwdU)JbNXg{-J5I~Y0o`v=v8chnjaNEwEzdag>}q9aSORhVWJ;n{6{1qt zit0oyr}sPL>m%MZ*|JO>g`Jlo(EBh2(@*nV7wu%68|8`Qqy=CDsIN(ZF|DDFbJ>Jm zAFBZ<39rja(WUAG+W#Agbu!f{o^n#l){-cn)P4=hR(c_NCbDt*CHG-dBVL+z?KPLkLJh;-!xzo-5Id z^5Ko;0e|{@;!zMl<_KnTj2MhCBJU zT=yl(AJ}N>O|(^(iA%zMCJ9gP7h;2vlNu7v7p|O)jI_T>_msY$;XEvYLBsN!4bg~T zg;%k6sD0b6OAW$ujtA_}QB!WX_6lS{ERH#U#Q#{b^0Ti)7A;&!4ji$|1W&SnbxnQ< z+9T$+trK!@qJ0^_Zkj9K#&83DAlP-*9*SFg_4Pzu%ISg7KGH4~u9Zy)4+1s9elTDQ zKCX_q>pHCe0s!t)NU1_=D|o;x{t_nTeExPcIXs4b{vV!#x2w@uTeR&bdBRFfHxMo# z@Z?3uqBKNG(%MP4FIShGTh@cpYquTM^;-?AbQ3^cxxt>pRO!L01d zbTGGJ}!h2qZW&R#h3qs z^u-2G@nSX*=dhu@nz+FZX%uC+q;UW9zda!5^}jtJ)g-LW8TW((i8GDK&zF1x#%t2+ z+;LB2n4SIvL4pdgb_E}Cm)v+a=<)@YrAVAgPtMk$rum$?!=ye3QO}Z$NMFgJ852=r zbJ=F`*Xg_Ix&QUO?LLE$_;tAHXjo0Oua?z2-n~k8n(_F<1VjJ}3mu}X`wmqZ$mbx} z4JdiZ&q*z)!FK4xVKVO_l0A5O0?R4!w=&|Ue~8X4)$y~w?4DL)Lzx%85cbuke*eGm zRIRJyt=O~E>(7E+C!M)8pwfr^s?U<)Hvrj(&y<`~y0c6iKHnKusxWq@Xe84TuR>5^ zaepQYzrxe;`}H?w7f-fQOW_xUCwa-z_OR4apC-nrh1k`wlHK6sAJVH$uO4t#zZ5(R zcLnRM%V+qHgLey{J^d=>#jnv#+9yE+>M$cV7vE;NfB+NDCw)R!c!!{17~>1iJ=`VS z*w@lvr9Lxh2>u6|;~2_+MUR?7oPVx+F`!SYgT&+1)on&C1S|v`3?eBwwM2IBW_V_m z(m&)b{h!mW@&q+^4Sg)*v5oS7q+Y~c-CEQdm>+&@N&g*)Fes->S&<`T@SV z8VB>1KF1S4`3i5d(oq$9mBPi?UhHWaosX0pvr2~e#@2Bf z7gdqe4c7QRB)O3dJD!txO7hBx2<{{LnaOEUoIoL&zx`mB5iymE-tpcw$mziAI(LK} zc%U4XoMq(R=-SPC?4L>{+;+?t_FaD5mc=G;WVhh|CmNmu(XgDcBaNz$Lx;uy?z}M4Z+2+Ep&i)U?bB08& zzb2?Jg{k@6eqQRa1B)}0>pn;QOw!uim2&3IO9ut{?DM-{!^47}ycI{Cxd7DF^s*E-ro{I(oXx ziO-lW3;)p#Vb|x3zCT)W|Jae+V?yAi0&=ob_os+~(a`CA+X0)zM2#hf>1p#Qie3N2hO>q0a&P7@7hVg6>m6^r81CA10@%nw>9W=| z`db)(5Blq{uGW~$#rf5MYD)$2jfQ77HBaZnBecQusVt4Ojkihbtv9YI(lYB)%bf7v+3@AHg+A{cL6p`m5;+ z>ngvU5@+;M|31$LnPThvPDkb1ZUZk;3 zJAHLDhjmXJZcu9)e*@)|G|GV+>_W^&RM9M0W<#(Hx6l5S@6q?MwNBz=(<=e13s)?j zFo?qUm^)^#{74H3;@S5~LdH6>P#(WIMSD)=OU4;Plo@-HZj5}!9jdWpa~WklfmsBE zqzy|=eaErbNa}7CSRaK))TsS!khf32Ld>0c&LY(6i-WH?tjVH9b0Lf_t(@tv-R1<-HP6U34(g6rBqfv+ zg9IlM&EA}fLPzaqOn1?>Jf@HRN2N-}#X_W)XTyE}MS>e(Mr#*96id_@pbOp^sFH02IUhn=dhspPlpkFlCj3DBY1 zv0B8brP63~{qi+g@lX-)IWa)_4J3AjGq`0^CQgwd8L;Yb>Lf)`T_NQUbYUzi65>s0 z6hO($=R6zMhSo7UyYDZ`ghJgUO>yAC<>-}1&~`|2^a}Lpkbu`1d@wd3HLQiaauQB&vq?Hnuq9SbFnaY%RjPYZW)>PUoT=*Rca~3fIk} zK6l<_SbuH6lN0cr{@iAR1J4pfZ5{1(<=q0mhAxZ+jxTv!CknZd$g{57ReSz2O#+P#!{^@>B}qsf3)=*Zy}~ z_Q(_?RObxR0jL9DKcX_AgBUJD`nKA!r?A>H3ok3r%00Wk<^e6$lN3$`CNYb);om~x zGcQe|tRYf#Gd;icNV6HP-F_?;{1wN2z!nvQ;-`fii4hlqfB*AZc-H zj65Lo?n~eXlkBUz3XHFLIWZLwgG}00wi%`jigTABA3g=zeRRSGO6&jUh4g<}LCw($ zy94gkwR=TI&$C|!zn(&3zFfWU9CZ;C)Ml3_?)k}p%}O0R;G&C$-C&Q;WLFrzvG+f|ga$F$qAbuQ@5V;*+Y+dciEs-OrrBx$uYL6^KsTJv}erR~_RLD>Vo@@#NR@Vp} zw(;4#QdC-W)yua*r#9tj?tm7v#9k{HY=pMEW*%at+iZtigWAM0bvGLs+lpA~d3S*C zJbs|Bwq=MfmMoqGsHJFY_=pCil!p*W_c0A2s6%ll>ETM?z)0?&Nb_~DYIDydWC$Eq zon>ftGlRYm(;WQd62{^V+=!m7M`hKV%1Nq!qWCXTm1N~D{vR#CzkG%0A}=kzp-C+v z@n78PaxurUKa&wZfKVApPoC{w84*fvR{ecr^EOVgH<`w_VE98 z{cn$2(e>Hqn3Rwm7WgjlA=?UUG=+APqjgbzJm$iGDT`ZR8jM&dqb!!iq^4X_8Z#wp zq-aykxcDA8m$ZKw3k?EeR^d)Do0uUwQyrj~*-}B0eTQ}8CagjR+{k}~65C2GTGF$5 zsY>BWMNvo~!*ux(56<+l!h>UhBZCm?i;EylF~X`#Xi74uhGCxcW-IL7k=IaZZo*zU zzb`Vil>>6ZesS)Bmy-&%jIluz1pZNzBN%D_@5-IntXi zVr$JT&cYg@tsQbDtlw2W2pne3Q-`w@y?o`2Gj-6tXjjOt@Q0)NmsQ*G%n`+Zp1(wN zeV%+ad^y?=nKGwA?RqwEN8(|j@WZ4{v7^%7&>1=qRP6u{wX}6(V2DI`=7Hwm|JJ(4 z{j&yKr{EyqGOm9Zu0R=`vbRoDDABI9W&Fbm7)3trbj{OpjaHY(T$2(4UleeLvTgR` z24^#O;MK|^wmrhA*VKrwEn&xj;G(e$UVSF+Y+pzLRQW`91M+s{w_vLW4v#zaxDri> zpCuM=>6u*PNi};a*3z zD|G)eq-LT5u?NrQ>!LnKS7i;3=_8{-gxhAAR-S!In)ifN_bCM?op($4>ya5j92laN z^zGu{y~;axy-u?r09^S}CZ*zSg3a05%>el&18QS=!6<1Itd)}UUfSB-SR}R4cV!+K zw!L=!O-DH6A~PlRoVPK=G`ecw!>YgTw%TSt;6 zpUu0hpO!l>7b#p3ij8>T#Y!0!v=o=K&)E>AUBgCjwG<#F(m*)UI$_b>=TUpKGY2{> zM0nk=Br5D*0>1KLs(FzaYWhT4Zf=6)u)4V<^t^s<$s1C@Md0!Tu}$wVj1JwQRvtQY z!Noz0Vmcd;ZsbOk(77Cj{7l9}NT0(M4s&%vIq=I#%L!3WeT{XkJ2Zs22|Y||e22Zr zN>hiz9u2dujq=DIsT# z(VBzSCCj#SPRRmLlJ^1_{eS-L6eu+;AT@;+S6y(%F(Q3mIdfW55o^5$#{lV4RQOwG)l_--c2zN&9_3{_Z~ z8SG-tgv-^EN^hC$Z@MXP7+*7< z!mH}od(yJnf=JC^_0o!E4)`93qt^@p?Z0j`%4`YYeZh8BXIs)ce1*l}`PyX@PuYPZ z!bw?!D}*I}6!9d9D$BxW`RDC9e^-+-%=ZA$nD#MP=1kxpxcep#y6`B?=t_HN8Ecm(Jr|V9Fh`|3cOql@ zStrUFwn}5|F&~~dm4S!hf{`tvW}jIP)3W{Ycf;KzyV&RFIZxg4o-<$F^a`yc?fCZ7 z$=DF$&H$8kGNZ0vW8WI&b~=%)S8r>ap}huP3Ed;r(&C@vT3{5y|Yw$*epgCpZ0 z6ZkZ2k`KZ9(r7Vsk->2|RV)Tu_=@?rxqEBCJ$>j_9lZbK3r@#-D64UJY|m`#3A3cl z3oFP=R;p`9y22$uJe%=6(ld4H(6U?^02K*eo!`jP1>eH7z+5IWQ8eJj)wZ4`fYAb3 zdh%GpH0A6|i$h15@82v9Yt`17II2nO(dRY31SYg>LUVviDvf zN@`$Lf=7GIH7%1Y%M_!1-@?l29ItiV758!M5M=I3Jx<*rs9r3OlQ$OkXy@4C&iK0_ zfcIYw*;sl6I5H+BUs2zG>TJDf!RSO|c)~DOylMxf9O;?PFI#>q1eT$*ZaO3P1yG{_ zRp*MLB-#;o1odiaUtK!}ttFFaCmCyHgql7bYiCMN-Y}59Y>cF_MMR!O5+6Fni7j)O zaN(nU@d8$o(<~T>bW<) zOlsE@R`MUp*DaA~4@7JO8-<);Trx=A>N=DZbI(dy?CeZW5r)|yGSlbH|Ek5&C&%g}J}`YRJK-iqg>ljM zpJ_6ou-;+T7=k=t?Xj5GiM$cXdY?H@*uYFKtA5zJ7^+V1IBXC|!Vz{qy}EV;dA%)| zKUI`sc9Fu5G3~@k`5NQ+qb~XFPR-HJ4VQ|ZH~f*;n#2=$4Y?r^r2|`i?X+?SSsNOD z7#RMg58Pj}wOWlPVweS4yXfnAg2aJkloGqQy9UU#Pz3j<--M$e8Nz<6CM==HywJCy zoBjkfC)ittABN9s%uFbIgtY>9BB=bt6fChu`(}UCd$yA^V}B@z5TP@5tRo5_MFZ`S zWsF*>K6BQX-ZOpO|8$rq5@&5L8&htA zqDANW;Q&|)a%9+YHNm9l9sB9_IZ~v1eQn!<7h$n|8;g%o%gmqn%`WP)rNfh%yU6rQ zN}W^>^Fxh$^sv%5Sp7_3-)TdG*=OermgtxGGh`K`K_l=qz!BhN6u2DoNx}x$bmGHR zdzKzMdAXfez_0!#jKU@QlylgN5acRwRkKa3^%8OiD@MDcS7L&-1erCCzmPiHCrR+V zG+M_vi;Z~oyu$*X#?s}Y9@k@Vnl{)o>_KP`!{v|mxWQ{PmWP`lcrH0V@%6I?VZ8N~gywLk9aUL^kBW{S@;;26D%RjzoXjW~Jpz$4H)^T%bJ5qYEi#bfnR1O%w9Rc`YT8 z`^f~y!}#gJdsqi>YS5NbuVlheUL6%ZVAkMIkv>~q^dU91bUY#%p~(mthQvDM1alP$ z`CR^IE+MtR#=_c_7xP$_;}vyKIHfi6qgC71BHaFH-pdX&qvw%4pVM+eK~F?emxGb+ zAaD_GqqfW*94oSsYDbVB7Pz|@>2S??5cm5d2ogs>@dCRrDnxyzcU%`=7GwIjymg*) z(;382os;K0PGv|J`EW7LM@p&KW2;b?8suZfy{v$n7|=KzXd%b3K3o-K?XgokLu)Cp zaY-|mU6i^8WFh}dYL^=lCJC%uu^FDjpAo6Ds?ELnsbKUibP#CP$Dhz8km9MNy;}%R z>|_LXH*$2jj)|6N1ZJN2PRnXcDFY-qUjyxg%_qrJcFjs6)k(AeMAYV0aNKp14QawH z5IL6ENEL)#6oCS;ME$WKnV^m%gja3yS#5gYo)`SX`Q7s>k)V$Vd~!*d6iYC2l)Y-2 zo(sZnG(v6M^#Qz=t*4bP=&YQ5mhBg@m}pcB=HUl=>D&3he2z&ZPBk5Oq;5o>1QP9f=VvaE3tI5TFlpv zbp7Q`*bi9?9wO(6c#tmQ1MHc0*rRdT~o(pkxQQ>U{zG$uu}4YH4XjA zt8zHg%GI26r7_6g^#9Nyo`2<|#9YA~!L5KV&lU&^ZSp7kAmfL)=4IT(aB3N!F#lTTS&6&Itf*@WqamKomEh4XM>hpDA&2*#4IpPfhFv|pZ_%-&yp3awGsltu1YAQlRa-n>2~Ik ziBl<)(Q7=n1*`Rt8|wh@{%(r#Zwr2;h(ZHSOVRVDX-qJsSK5e z)W#U1We>W#fo+q^e6#E|;>IkZYoys_sqA7HS3uy{)o1>vgnPd3s+@1iKkZ!#+ zx)&t^q+r$w9J%enCS8$E)XPiq#zz32eanb*HrWXMN~62>u|XKPus?9|3`s^86Xf{o zRe`OmPC+P4JqLhA$81@n{a=m`aXTBq*%({&B)bo_|L?EkZcIM(%EO0{nOWcv?NTpo z@qTVKlqAP)@FMXne&jpM15dW!8)j(o1t|x07=B29N;L2yrDGPG(xro?lb-t>EeccW zA`=XNI4V?ECP~~9-)j83ya4*qgEDiyxHX*g!GMrTKNntJkh4Hor+1v)u=L(XwQZSI z$*owB`ae*F&jk!k8|NJU9G-xSs74Rn}(E;195cBUxUmR>_3Ws+Y7 z6Fq&PalT(#8A1c({PP+{9KvK{{lf{v;+E5~@_`J11{v;^h*6*NQ(kcbYWmyeM26-g z|Cc$K-C8%d zDv8yM1gu!;H^<+{Pq9HGUN{&LON}M}*i@qAo>k0FNBKQ;=F@F7Z?%%9%egidE74Vj z8IU|%2l5KW6EA$&>qWzaQq6vS3@?Zc- zXW&;cdB1xh5rP?!a)M#G!wh@^3X3Tq zv#n@3nGx;-)OHa{S}0iB$8f5pF7alB>j0(ueKo=>kgHHh@#L~$3sSr-aFu* z9?1^vEr|1uoQh?B02V``sM!l4{4Va zzh5QgviKQ6T5=qWPOL-Y#Wd#@Q=3m3_`HgJG7c!MV#dBR*8)vH|9A>Q&9NZapeJ7w z0e$HxdxuRYjU~-NVIkZ*^|`Da(4wu?^yhN z7q6&2@s%h#bJIW;?B)?6MAWx#<*9fJKXzeF+)2%*n%7^{eQFy1Fb)yeCFSc10woFX%VQ~n%V-9LsviB)6 z>^)k5mNZIZ)Z)(&+&X0_*{^cP*mRfAWsu(B*O)$6eLnMAp9;+gd5hHc4-FJ;gML^} zeQ;I|aD&v)llA9=I&h?2-Q7p^;U-Qr4$@KN#V1x~6{oz#y`^vwpWQNNgGtrbtBk~L zTfZ7=K{X(J3891fy%e7Tzc7qgl+^ga=yfB84VJuLqTaeIEH0ja08PO^Q;m}hg)-ki zwy=dW>;O@c-rvuApY}}lz4R5{g`oac)Jw5)9oG-i1U&cL?$HtP_c=DTPzpRA8j=Fk z@3z1xsEx7O+}pL5<=9K3h5zz}ilBGTU|<(Ckph^2CM=@1Zhq8&?7^$dWRpRGXE9ru zO0)EW0CO$}ySG5qX3I_@e1&IrWpsbGMmYbFkx)SNddziB} zs-)B>PvwdAABjooI)6c~iQU^2s4_;`n{5F`tBv!MrPE?-&iF^thY_vZmHDl9Kip(3 zoZsrr=sV}AX{W53v)-&Y#G7Ep1qMD?93$JC-OeB*ZMa>x+*?+a*m}(q*n2ty0elv) z;zLc`+D)m)e{ixoQP1*|OG6_4?x$71Z$ZCPE?)EFY+$r=-eV3bS<*Q!^gC;(FYB_T zKyejs^y&`G8Lk;U|6}buXxq`qDkoU>dIUY;#v6!v2FM!P88Baa2n(- zv^%vqt#U7`p5}3gWI)1*9ecW$gmGP9&Pj6U6KmV@0vX#j>I$AZTQK1WazCA~do3y; z@ow_mKbdm?M?NNT1A^%`aszaI61p(H)K_@s9PUG4vrTA0ZOzj;74MgcP#)GQk)9PS zHU6XzJ1ciUCG-%&3}&BebyR2+E zXgS=1-f=M!4Vz-_vCnh08UA&Pv~|5o2~L4%CArvns>5fZ22mdk?YbwL*>>dyODKe* zMRfi_X$CyiK2|X1^%AX|3?SM8EUJ^N_+xcuuxMyn^iAE<1ud z79mqFqc7q@tv9CTeONyG`;_+;lUm>{F${;%CM*3eik-ech`nAYNEZw; zbfHGE>0Hsig{_Q>l-C6ll+8WO?lYub!tOqXy;4-M+=sA?+5(ruCgwH8K7g*cf<^<& zR^d5S=)mHuYY!`ld~aY}40Dlb2=Wy63G5pE97wFv5G|33Q~6ibPW^B1Rm(?yD|wnFlHzpLDl8sW2-4UIwRw1Gu0%h$J+@ zZojUWxu$mR=|0e?@_Siz1q@JaA$;8d&!5E;vtRAJ2KKWOppioS2X6Gd9M%I`d^EPh zZVtoQOTwTVaY;9ilOn@tcO;L)DyiNHgcmVv38*=M+LhDFv4wkCOvMbdL%sJ%y8z@t zn;80EAj-a>Mj0XYEL~b+fN9Lpj9P}R9!x^b7_q(;&!H7m1G%Hp6PkD4r{sCSi3q*xHBuG zXG-4;57Qhh35c>b5aSCt|Hml<$JO6do%xc#bf=~F2<0~nuFWerf-B>nNI64Bk|NVnH(1&|3KLQNI&z&Dj6q_D2(v+Hbp``*b+f zUO$JwE$aJAqY)PBuxpHt+VF|vDWwff$1{#+wElt9Lq<6BF*Iie&p`=SzLu+aT^ktg z*ds(-v~K8LND4*S-^**D0~>9R;B4kbnePa#up(g#jYhPrM1`hnpDZS7G+ewg1(UiO zirw8zFO9#rzM2PbNp(s4`b^S>s)}8rKklv|*YHEJSw~Xei(v+(F5jz`gSgXV{wCP= zJd?^3|K8fyF0M*cSf^8716R>Inp}b2+^AobfVbp|=)z7ovZ&Z4LK77o)nuUU)dE?Q z3efqu4Rf(ie71MhSv7XI)AcXFe@OQaf+hhkVh>Q!9Z5fh#%Yf9CopjVgmILm~U@*YcpBDV1D8 zMT17dCr0%eo{xN2S4_wxWTLk41`?>+BpnneX&6KIj|2FnCVuQ-3d6Vnn|X&mb-Ncz z@andEROu#S%jYc+oWM23;C@si=bu3^o|U<$C8}Bw{Tn1g+Ru+~1q^cc!}QYR0u2U+ zX|@DUM4h+yAKu4{PW=)m4I%IZK!bHC&x+INhRvvGdx^i!WyjI#u<9g9^4j^&5q1k-itU{;-Nvkogk zF?{+qzso@f7Od=qFYK+vAly7m6Z=G#-{uY`5vT?qM+FuRTK<18KxndE=UlA{Xjc^& z@cT3I;`FhhFJz_D%h{`Z-WPC24c%5kQi;#A`(fL3eZj1o2B5fe=-Lk%wX8`=414V9 zYhq3`DN~<|Y@Dzvfab~?jg#1tti{u{me7)m44-IV*ZOf;N`Qq7!tnj`w!@x zq*i-qAp6e#W3@!t zbkG$ssW0)oCdm$Sy6Oud3B)?Xef|{nj5TP1+R1=c8uSUBgoBldlMh1YD0_q@m=?2<26q|B2NG z{JgL==B|iS^~z_gR|BONwwumqM(x2@)fc5O;b~o!!bKUs=}GR$(BBlzdbOfvFNKv0 z3t4%>h{Edg?Q{d{FH~dkPQ%#rNUd%g5P|7cV4Vd!0h{yKHwY0`n+nBK5!Fc z4Kv0CRpQoKR)tA)$ZOv>I&ISsY>|>Hrxu)tVi0fN*>=qyC4C2VCHkU;D(Sf{UQ5*MCn`mvmJ>+b5qh!(Kw(KrqZE%VDTL>MSPNPBD@h+L>&`LetE{daB^` zMq@J)&Q|v=sugjcW457ZVaZ!fCWR@!wNI3J^sEu-twTCKa6ZPVMqvTdpg&B~={1#7 z%k%#o-cM3xd~tN7RlgB;X>};t|JOl+ey94^+vXy+CN;VpnN;Xg(s|_n9B+~96Z%7P zd}#NEn8%awtE4ywf)_Yka3`j^HzV#|%%bHfm3Mm^4BqnIw|2Ct;+ir#J(gS}bR#8u zwPDbP0QsYv=e=z{FOC%K0h;*StnR=}xX6kPbPHtZitd=ZCQ zB;|$rhNk*<|0vAS8*{EJNm>aQ^U@Zwy%Nq|baC&?#(3;yHiI6-KHK z&^8UNEK1KvLG3+S@vIXDY6AXlJ05pBYR>6~lvgpkPF_&*!kRWfIsxa!MKZ8 zl5SfElITdM3Z!QyzJM?}e@RV80<31Uk~MjYuO=kAedkmn-YEC_BtLjXv$P zhkFa3%j$ZK&vfi4zwy{Dht^82%VQQx@0aVWrUSiJ_V4bgEc}Xmx@KuC>T!CybWiEo zT88Y6khpYubBPmY)_T{qg^R@SdW}X0MqNAHRR%lDGy)uxKYJIo6Xm=xrdSBq?|*5X zY#2~0o;sM~qK4Bhx_AwXjfBbTmmB+rFa@cs_T=cA8X0^q>8u*PPR}M`+mz!MIUT=t z`b4IeT7vefz=9?OLyJjLn-2yZ^!Z(OpZK;{_I)5VwfmTvrK>uYHFfcp`?u5L#XiDjS?**AL%sDLmy0D$(Gca8oi$ZAB=*)p-jb6@qM~X5? zjsu?qILsMaV7uaHTHUQ7k)pYlFB@VtR3Es2j>I1X?tpcsg$A+jH6F?&Mhs_A3Loo) z{_ay&55hrW%SPlgF__=&{55&!rbDW1;5tbGySKXZ7m5;&j*@z&&EYJ*l1t)t_3TWP zv5V9AvYmj5JyXV>bMwhyRBp6$oN%s&bde$$<&W~eWL14dpNk<(AS1`YF1Lg!=c44k z^v)gj)VpVjs;dJ#G83RpB%SQTX!W<3=?VUe6YG|NNlM}74W7`v)2WL(+`2Vo?0D7@ zu2VW=D$Rx=u;SWjL|i6j@!3iG2aye z(WapLgAt*R5i%M@d$eic6M}4X+ld>Umh_fzOY!-3-ZhVbeSXtaJj2Ab{bAX{3;o?5 z3JYVjh|Kx;&l<9Cc~Py9e5p`j)}O?P3B80D1ny2a7u=Yx?~{iM=`ALif_*@!curxMqTi;Q6}%zY8l--vwQLI70nMhH zrCte{3OjSA)zg!qPbBvbfl}5@Eij>JHNYrZ>Ipaf(nqo6E7(?hy0(2A2a{NLb z81&F0Jk_6V=y%Gpa+Cu7?e>INiQ8tpD_2N*KQ#LSIB^mS4^{r_^uSXRIUCa30M=6U z6kn)N5%=ye0jyvgfjc+gtP3@6^^Ehh#H`|D|GoB9!VuuWlE_P*NNv((Y)z!6{hXbg zsYEM0!g}L0h5;=cY4H71&#G_s)ewB6#azQ(`Tp+Zae@R&3;=x0IX1W!L&@!p%NTl` zXz!}uB6Of-=TMU*J1RvCT=}J7tq=hb{)^HxR~rr>(^3wUa~AaT>=2p5BF5*J$}9Y& zn*-v+7N`3?O@{+6P06O=N&tK-?JmKY#1S zRATOPIUK=?s&EILnQ3mr+}SX1!3B$v{0A!L-{XWT%+?`&!Y5MaH2MMra3cTgOEs$z z#(A1-_>;Uq2Ws=r_O4k}0%QNr=ga`!-2&pM%wSnMYGeOfO|*z2Bewk5;`Q$trxghj zucPc`@}u*a^P1PE7XR!{YGV5BGSE`pyL1A1WMP+^f48UAl=;NtCEI2F5x6CQg+I% zHR~rzNC}cJk`JezCV~xSkqw3N2PN)GxEtI|gLEX@sk5C6meyG?3gMSKX?bC=XC}(| zLa(V`DBq^p6IH1fT_2Tu<)kk2u5gkLo<&ZoXRhU(Hn@el@+lP6b^&wunFM@KzE64_ zN$SVMnzOxZ>HO0y?sb#{_k6Ets$AsBj3l43zqy}71;X^jX!Hl2TL`&h_I{5L-6rpw zS2x8y4-bV?)9`;d{yMpG7U>UWYcN^rTq>~GW3j$kD`EpRfu^<-*QeaBT06>OzxhVGo8##>KM;eGf^d-ynA|GQ*un1xf`XgWg1PafwvH>xU4$49SmeY>HC8Ay6d z12SQ`aeB_1;n(9aQ=?*ND?E?hh<->iP?hb^F+`g1)c6!pgJFND+bsW>(95G)SEmB7W!OkUg^OPj5JYu`kfRCgVykVsHxE{ zeO_z61TOdgZMHTWf{$p+MP3C>`r_N?xnXPUd0))ZVe0Vm)I&M50so?AXka>=%f?4{ zT{cu$5iD@GY8EupPDYJi=;KZGG0zlF4AgEjKWp%)dbsB>Khd3nw_F>G6z&wzH2u;6 zdzy-W#*@LqLLc8TTqxN$2rA^xU3Sk7CEaj1@I$6v!EkCsUXw(Jpb>w9PRXk=i&>NR zNnGGh*_LsdYaf|7Qj+d6^>Bo{K6(v0_5?_qd%{TaM_e`E!j5YNNi_iQe8NGxUZguD zkv32R6!(^!&GE*cU6CF5Y%Y~9r`?r>x$wprb!#kATCwVzqbySCmVrdOVefrmlD_E{ zr5EfdfW9t!=MxLlrl-&=b*s>92aZe*Ug$%tW?z?V?|Uh@!ah)uVVCJ7^NeVq^N!Im zizc+y{imoIa5C-f6Cew$<7W-4ezbKnjUis_#6UbmNj6Z`)uBAe=-$5Dzh=PxuHg#y z>PdZi2`e(*H%)3AJ?UJi&(APJL+R#Z-|-skIBS@K1nnbYKuda>!6^l>^w_Fa$WGGH z`^4Z@;XRGY7rFt8;)6NA2=GiNv$juhRky)1FL9T0Qg*kUKC8y%v1HQxW`x1|1EnG% zGyc${BS~SQ(Zz}rI%*wihqYv2VOh$cEvIM=21dl{6p?|`cSaTSbOb2-SKz)kCG`REag&xX&mS+aclf*cMc^;c) zb{wtZrmSjcN2R#UVslRIOx4R9>?c)Ued zZrr(seSN>d@R(m3gL&J1r0)||a8tEo#w{@~cvI5Gu|E&8j_eAYVu_OiJpauW<1s;V zL51gQ_y{im*l#sFtCsLH+fX=U9~Ia&L6{SD#7Pw02b-j~kQMJs>}6qV4BJrVx`V0j z%XN}zX8s+_n9Ax(r8LT?<0NHF@<-pF@ZCQ)Pp_z(3V&YAbPBvDPr8WIMt(Uq{AH<6 z;)Lc^YcwKdlb>|`Hy3eTEcZ;cvfLwD?4+Gg_(<;fV@y)V&tthL!fQe~iZjZfZFTNz zuK&Z;Jdnw!dmZe3Arxf}!@R|S&mTGF1Yd*23x)UF((fi#b01VE4Gli~s6X2o3R6$C zW7IFbFX_$TZ8ORYH98v#HFbjPJHBUxdO;2#GSCi~S8DYePt0)Id;M~0#+2&#FBqDq zX-aug%TRi^f){+iprDm8;%IZ03uV`xA&1F5IhYa?WLko02tB?DTX>aPdu*-_{1;ZJ zrwISdChtP~Q!H6ea%zfmQ(-}#tw=IYY;VazE0UoMO!58wT)}(n>&H=(@KrrJC%3jZ zNG(&(G}Q8mt5?dNIF7xj{-?zUYW6^n-OI5ZbdRs%!gl}a=g3^gs^E$xdN91S+-$F~ z`z2}EG@u-M1?2{O8X~GmKQx;yXIjwaVyXEZt(a=Ct+vh2H`T_xHyh*j``}24#qZnL zAM~VaDOFxY$6dr%SO>IX^+3s6GJ`3Md-K}i!Dm8uVVlq`+V{iuhp%7=*wB^!eV5(E z*pL$=96^mskNv@PU+53%BQ_^D#a2X26UTKh$;7E6dFp?RWFgEOm~od^G{lsXTQ)3u zrq$nUUK|2VSe0eFiKrt;`kKXZtO^G%Sj2m{pb?}L-lU+K~TL!RR8xQ?9bO3ydeua zO9!J()SzxkAWZ8-#<|Si~BbM!?!c7ydyLBtk;!q z)G@q|AeLM7=e^r^4Y6C#Bfm!fDIwD&t86zD>AJn1W8Q6m4)qhT))b-rT#9GrrJ?rB z?&6t{t7>mJYIg(CFT`2~BbC}ST9pDtaE3A^Q}&ag?H*wj+vP_VWs8Ru!u7w0ikG$a z7 zz|(3vvSO&&b*vBwGVlnv!|>=^b8U-o2a2vtn=0C%T|3)hoKfkax}Z(&aD5QzeQds- zxqWHv@}VlwR4YG?6y?Sq%hOO=Ho1|lk+rMvfMx@YB!l@+Hf^53y}g}rQ* zV7q@VWdA*y8AYkzT-6F4UwULWG4@E5;-A~0)6rivbrWCrYiD`6;-dMi< z#{ru(tzsu$7x`lcE0|rEie)Rx!=0K}vP%y{2%@TiDD_mJ`2z0 zQ_3zmYXza~SG9~1g`w$8$>vbK_Bf;Rh?WYYg=i;=qfDogR^7C!pCHbB z|BYG;2TRE7U~8*e^9n=5Ax4XSEl!HL6HGl}y6a8EnM%*@sUWNMk&dx(2!xrcJ&qsvK;A=|HzicX0Yx<=%R=&MY=0Qg_ z@FJ95_a3{q6mpo>4_yjRFOTp!?`k9DZ`zc%!&xkij+L&H5uny>9@y42g4MFEo+3Z| zHTSjhhwJbEc#jSrs8o4J4fBZiq&pH;p;q=wEw?_XdG^BB=F2m8>Wh_O`JN_|IF8dH zi(w0NO^aK-TuGayWTLzW?@+AUxei)op)D7S&7Vv*O_vuXM1%B1=oSY3l-u9@?&%2b z4tE;aI58eD6?A=F5RLprQ=B_({=T=k$wJ$9vMN}1VBCDqbtXfQRB7q*=$OF3NP={1 z_j>N?=T3!qhn7F<`n~wl?2ZIyqEDIUl1B514!59wZ<#aE)9l8>$E64g>O0<0s zV#tx=oxX~F&Jk7Hrt5D@TWhpvUu!#5C-N1h+;ExAWoo{^R_Ektl_%}YG|#$e+D=#z z?Uf$NQ!O+r&r5jrQ7cs&Lwwig8hD`)$VSeKs}S?Sd0t_JeRfk~H))srg*ZO%`Qy%p z*Ex;w=-_eWiJYm;(-d}R%1o5*sh>Hh0=H#uBCj*Wp|#`j=5TI9Ld^zRH;AxfJ+(Iy zL6qj2Ny?e3zrXw40cPQ8VJVFZnVMFKZ(e5^kMNnP|6@dbr#1q{(V8uxc<@UkyT8x7 zkKLj!w<3YpJmGd6Kam3+OK$N`^m#A?81)Hohat`IUe+Evt)%B6{u5Whex3y$!y2aBPAFi>;E9+_GI_%q8 z8=7lb4L=%!;W8`tnh>G)c4@JjtZNKhuk&d6b5t5lNG=6DbzV50ga*C3`>o!7g3b?v1LbOLDxZLfd^4V(E zR(Q=Md6e4WPbrrQ*lCtDZs**Pt{5CUA+x~82!RYW6v?o_kgLId0dI%&E`B$n-oJjd z9g#O!&y-%X{)W8LkUjDt-i=O7Ksit(^xF=$SW$+j{%3s%?3jaOf3`z5m$__)LleO_ z*mb6!f6m)TBx(`Uz-u*9$8mdCiYRDyU>W{yyPPaVJWFgq<31#FMbbWZ>~^hUJI!dW z4W5mm4`vutqBMOx*O`f(bDsBPZC;=6_j%tE9zT{<)iV$YlWSu1}iwWR81{64<#@v3q0qcsnz z_*Mp4(YjyDeD6N*YIxm%N_vyWOqlt_({X!?Ggy1~@Y2a}7o(Dh@(Rz?s1K8?uRCmp z19%*$<$XLu>bYN-B3o~+NBYJC8|QrpOk-o{F)<%;Q0UY4Eo3#clqyv2=zVE@zaiVw zblTmgonPqi?z%qA+Xy9F$hDV$z+T?NZ2 z_LC*s<4O;H4(b)_nGL({W=~JdF?Wp*1m${-F`WpG?$8%3+1_ zwx>OK?7AB)a_?^8=ohAByvOrEFi(l-i}qQsPtbcTF5Qc6+Yi zU~d8u^VFIhjAc+8BG(@WkE33%4UyB(cZutKyAQ(d-k|bY^t+W9_vMY=#ushZka>vF zywv1UKPAQFLLR?|!!D1e%Gb9sAR0Y-BnrbR8@-Eo6QVm4`+NOo3fbcJH0OK2?aS+& zXxUerlsvQW2NZFomo#(P+@_ctlufckaYFAT6HX@Q7Tz6Q7d*n_#cnARX~(Y?$cbCo zBDX3(theMZFN$bFhrSoY)%F!pavyX2O@_!C&7k%%`!cQb4&mm*kqY5Ir&FkPHEXHl zUua0%^t-8{4omfgTSP(S%R*EHzCg`mC_e4|Mu%o{U~$Adj#n$Gs=T+woXsX*U(x>m zy#VE|C3^w->(9vW2HE45;m!!-qB0B33N(jkkll>m@25PM8ufa{*?R6gfAOK*JU_31 z5+bi-@}m>$C-JT+vAKpLWarTvZRNangXN|UlGutyq#!tL#sty#jyCeDZZ@HS>)mxy($2;QLHWp4ZINKq+N z<*d0ajmX1yXK$>A-PQ=)8+p4O^G01J_{-IR%#Ipd#}u%$Lx zt=Mw)b+M(JX6Ut3ww1| zZ@bm!;kwq}C=hpB{3>RB!tUa~J1Y!rMg#?98b>rZ~4ICn``P5R2-`EMI zdgFknQOMXgG8|ZC8r~SPd?ze;>OtBRCs%au zOqriD?o#d`lieLHXS4pY{j%*;-TPycx8oK0TCMPfK9sH6H|$CH(GTnClU{w35VyIJ z4xgS|ysb3$Qp&`xa8Q1c-A#NWhm*?VCSyck;pkY|Y6iAgc}heZ*!=dlGKhN;Ejyy>+K(Rmi$X~gQgN>BBtsa zBLY6ggp8PmzudMFIYnI|-$IA(S%o_*z?kjuo2#=}>$=7D7F!TuU%X+qRC)PJO>in` zlHIGBvDUa+Gm7Z#v|b+Tb8#l`OTGskdg5gb`wg3-_%bb1d~ryiNZiq=UXU={b}283 zz-_-Kr}H{kx}A6);ky1yH-o5N-0x(|l`$|%yv8-7M#LBIC`3@ps~N_%Ybuw)+~auHu=uc7YLVz2@Puh` zAnMA*Mwaj6&NoyimLgiM`6o7iKeg>E&9lS<@eWm_lIwWCc?V`Sr1#F@Iz;z?Qa;j} zs*l>B@(SxfxM*`7T;GSYy?JHJ`m%9axG*h?$vU+Mb6TWIwKt7^>s~}n;1D%OOf@xx zKM}^%Y8$QB=h4A~dvop#f0uR3)8vygy<9sx#b(2{af=qi?&q)(kI7x=fTOMDKGdE| zL@RVbd{warQ5CH-+G@w&x*6R5{jGQNmV2>Q)hXGQPt*1-jSrgasEg(nF3lY~%66+# z3hnajuFXSS3U>1z=;6Hy@(=Qd2l#TM`_0Obp(@PnA&sWzg z$g!a}IY-nb5VDqWd%T^m4@w&+?@Uzt2((H(Q0SWv6nH8yWhu4siSA{Y#k_*u`vm(X zDtn3NLXqvegz-)ZhM9d2XW>}tp&TgP00!&Gt}xN)#cGncmS}CsMX>fgjZ3- zojE(D>`q1T(MY#Bt#^aPjEGi*X5BCPBgIr^qISeGvt&0!L@MS(#D;{$m&hOP;`c(bbX%;W0 zeBA5-G=g}odFMF>$n^MQbm&Oyc=(cq8{PBWiz4?oNDt_l?Z2jVH2OI!4^65SatQgF z-pyx$gnoZWeji&gsD*}<@G1;FnBb0x@NOC^8Rd>>DYTt`_*UT|)z~(=1*IA2Fs%up zx)K9;I$twNccc~v`^vU1 z)1o|f`g6l)*AY|vn~jLV>E7J{bnv*9T6t+ilMhsYkfUWc`SSeynjSF~$nVOC-Jagv_?gmJ?f-;@ zL*wZ2aDI0jaxONN{OBV@iM{hR_^gC?2Y6Y?wV4uf1F;kvS`!RE7I?d89}^N(<1uG9 zxAF#H#}}ueF~!sgvKFx$M73nAqPz)9+KV;jlcKI7gWZew z0|TnauZi*C!Rq3R^2{f3AVZs@69vNl$4|+%%g3Nfb<1cI?6f6v4> z=NOPp*2A{CJ^Dz{T^VsbI6{tS38B&jI9TNlFg+VJ1z*LJ@0kB0ys zENKeIK(hPO$Tx^y7Q?P|`pwO1&V6svCOV4+%j^I>$*G3jChGn<)cV)A|J|{(C<4s$ zb69m__{^4baC13FO3*M_`pB{R`oZEzEaj$c`F6DKPhNLX3ztXPH@-CxyROzaWCFfP z5FfGbj4r4_&lRIrevKoj84X++V~fSsi2iqd*$wG6{z2=5L@$L6H%K>ynOLqEZ@gdB z%VCLE?<=1gVS;F3wsXnRs?PtdQvZ6W5jo&K+{32b&nfNxAomezX0+0MhhMEeqdB&k z7+~e~;Ph|kS+JLS+U{AOe3{p6fo)P~7l_ZTOT95$qMOl|%lk6sTmn0y)pD|#!}Y;j z^KuWMxturfST}`@QL15-DDSWc&!w2+Y?A zL14gtRHLtWAJgcvzU#UOvUPGdF`7r1M$K*D0-5jC8JqoW;;mxNc@Jmvd9!klM|yVn zM*HwI`YK#M}p?2=c=>%;>Wy6wplj(7g8WGmwTWADr3q3XiNXR$_vH%qoG zQ6VJzmL*F_*>{6V*(Lin(_(prq9Q`F#EfN-eVY&^kuvsWj2MkQjeYsuq279zzQ4b| z|9$W0HjO*yp8K3cunntC9f9LZU@YP4{9Vu09HJzaFZNJ(Wu^RzdE1#LzHp#?8)*nTgmm|j zp>&1q5=&UHVb^Ci`wyCZdQ7|<>w{1<3m@B{6WqBeU_#1`-3iqywM;WnuBp)_&1xOm z(pnnOlEaH3!wqOoqIW{Gf`SA@m(pVcF=AzYFJ&4m>kBp}wIuMxCdEaaPs&dG*FbYC zcZUyxyz)DXFXUe+Y$y+?Tkf*=q|gb?2lAJtt=}q4!p*jwNR7xyGxL=4tanqNTW|I3 zT5v1-h})FX0b7RbRMdEq@`&?T6(1Sj6BT9|E_V)wRc&DE=4Kx}B1*@2C$%nz?GnF| zfAurbe%X+*`p!j9KneEond^CxBbdS2%RE)piOZGfZo`27-gLAI%q#|X%~j1+LsJ)uL& zn(SVz51z6E<(Ae5Z8~!G$gKdh9z)%Bo^>rRI0GphD`n5n|L`*)`iU7#uAm^31#7&g zlPIXJw!++5jK}_BT4g|phrms@8NB4XFL#I~03&Ie#pf3RKJwU?7A*{Bh7;^EBs|z@ zjSAMkxSB`nQpCn)R&0GQ^^Ce}SSAzBznWun&@8lML%sGElj;D?&5`3OFWJqD=SKoY z!;1o&FPbBrP+jI3g(@}d&K>=qY%b*6)v%ivgq1QqUX#MQTk{g+2(n?GD*ZH7Td>+V zYMIhLvy9Yd^GU*aZxs^0Wmu%XXgnj{vpO(3y`vaIJDHGupZR;Yi<61x<8wnYHh%%&Re zK~d#FHRfW@J_e4JXvnzMw;Z-jTM-!MYa?*To8SXPc$a-UDTb^UDohqE?jdg@c?)-} zC!}k9eaohl7Y07kgeaFXeS0Zo@$D8zaLPOH81UHpG|tWb6H5NiYtWb6=Y~}6i$AVy z*BJvwV=DI00@N(9)>$S_TGhNhShAtKeW;(l(~Ux}=>_YAZmiQpu#9@;d=GJVXTHeu z%;<{+mpdGrwvDqYr-7L44UmR|(b^<;-MezbfzuSd&(tYw)aC1{e``sU)-(%CJ{z*^E6Lx%L25kI(ELFhEFcw)?8>I z)19?$;zUfFwOOE|Xc91wB-(nRuh8QI&0Ot@qfJ@KwT;6l=+NO?-W~!`3mwF|*At#* z!lJExN5-R^4K}+h{QXW1hIZer4S&Xj@t-3qU`d0dI{WISr@U2bo?V_>9_b|kA$9^V zQcJ5(!}9v>RLgE$?EEls8*scPIUO$axo#EN1TQTs#Iyll(`WO=uy13QNW|>cnV=R& zF5|iWm<_b0lK1l(ZRuNCfGcKJtKRQFSnLgya|iNMjAoH%ZsChR(wr*VXo%Sg?{o_> z%lJg|QNG@r>*mb~e^Pe-4)|<_-93N`p$iwc*Bpm)muf@SqbowM3P@~y!U`YpVbeAj zVR2`KW%c*=dxqip!z;;?Vp^eut=7&q^N|>L5BCm(w0@eG%GV|1Tspi%yt~g<3b^i~mqY8dyzpwV7t7P=EIhB@$-~mH$YqrrC!L7#)^}1P zIvt@iZ(fR7dR5IiaQT8e#lzBJKto#J)4TDyH7vmS+4LjiJ8AXw73(coHW^5o-x5fzLn%9EqcmsZgX8SW@QLL$p}7MH)oiAlL0+{G zL&l;NOQq3X8s(ARE8 zi}}&{^G;(Uc*1$7qKO`?C&$mB{`C$1p6u)v#B$qiKzczPPVXF_0zorcSGmC!k&XH? zOPplsp@hdyHFa2m-2NCk-3ZK0{OiCl_xsP2y?VI8433ydws5|^n&to1l-x+~s318l znEXxvC1P;VKPTuFh3{|M^6j^6oqaH$Xj`b`Xuyw5QO$Y=t-Awm?IckaQ-KtAW**npjZKpJh6R|v<$GVA*uTJ z00f`i2OvjSdSt8Ffd%`w4A3u6Sh>H?1}DvndE_q5O(10rO@0f3#y$l3N;Nfj&GHQM z{|Q-<`^YkudlvLrv%Jzo{K@YT%l5r(fS2$4XqU$43ckPRal7x#SP_a81_FCZmfqiq z*hVWoL;lhnqe(`}J`m{h=_@Z^C|{rFfiJu)E&ZD4vx?!VC4B;}3Io$D zi)m(NW<@gojK3gi?8b_3l}VQ1AmWfv*>+3l2*#D8PqISHQFZp z4}$}oS^%H@)|)2rPtZFK|3kOk-)UG741{Ek&j*Fgi@!@KI(22hIin28*fByak>5=4 zfU#^it`wp(=gPWN04n}VHVcmJqiHkYI zoO}6#J7_v{2G|(DnT2J~BFq*EU$EHp#R3uu%zgalA@q_QRRYIM`y>(wGEQs}G<_X7=71GNY9q_S zd9?p^*E`%pqJfVrSf3~b%oRMC>o1w;nALX3+Qjeo^vI@+&72X1AV)#%Tq)g=QXvtM|-GFz(zYX_E<#t2FTQp$R# z{J_Yv(!!Y39n9yzFy8+^bpO}le+vT)xAZH2tOfYrddC0t`+petKMeeDVgUER;>fQ{ z0{`(WHw$f3XESFL`%B75g$jkt!O{)qdE|#IoGMIS1 zcl9?PJWrg>6ggFc)^s`%L}#R!>kjfaE%z=*PAa05V|f*oJ}$Rmd!@sEm+>GshHHGQ zaS5NGX6PesUGc?RHJsh+;=eyI&B3}c()ZVLd;-e@584a=cdNatIKHQ*Bcp3-&DYZ^ zi$?T*7nhUUDm_$n*}6Y|rfY{d(>II#cTD;7F$};;)Ohwze8}ZOJBgo{|Md-rQ~9_C zZnjMGv`S5)|JTnyF9e`p$c9yUJticS2M&N>PJ?F@e*XB+Z^^D>rxjqWb|&sT++fhm zqX*>7(tjVg%#ev`IEP~OhI5tx2v$FM`e(luyrW+qS3a%tUl?VJn+N3*>DcT7#;LWm z|JjufVJQnr6%azo?`8s_c{uhmPJ4|Q-Zq48rdI(WJ!h681DD)~BHP-Uxr(8|`e6Uy zmD%gNJKOF9r8(|H73HCuE1hx^_u>@itGe^iJfV3(ozvv(*{=s|{;7-t46Yr^0D_a+ z_oB7EpCwlP<_59SeKw!U`|O5dWk>r*>w|f}c8QCL`EGo9;SWTnd;hkU+PZ+L?tdJt z9~-*CP^E7M7eH|Fcfa(S)Ufti+;8%cx!>!<<~QGoRdV%q6%`cRTz)U>5Byt2ForxM zC@%!2107=mY+8>wB5;`Lk@t*_tRURd~)v2(LH2#Xp7kb1-fQt%{k zD1aA&ICup(W~TpwSdP{f-*hzs3Xa&f*&AkI-(_iLn7{0_7-mR^=(Y>-T);JX( z4h-#^NaI(L`YGvRV85uqjuh3sSp3Ib|%zf=aMwW752<6(1~ z?xE3@3YBk}j0RVZF6qrTY|fu*Si}1*j4GECe(i_+9jFS~HVp>@N*5z=c`tO`Yr#IX zGUU0Cwh-c+Wb0?SIvTv7pT1mFJ5R*NhW_ii0Ii zZ5BuOH#F{L71S5 zD>wh@8O^6uA7t-4IDKDyA_>Wj&A7(^mWxugwX)iH-OYqi1ql7s2RrdkSTOW%FS{|R z+T`x8{v>8{g%3)#-~s^0p$d~E6M{*&Y!xN$RZ-(&6P-9WH+akG-Me?5JisE8za3Jg z-C9uDK@tR$a(XYHu=lI}r#{mp*)eFe{f+a!mWh|zMs8dG?Ka?#fcvubUkthwy>OUd zZGSgS5fPF4_4l6h7z8xS_$vOcc*rs6oXX_t(;Yl=$q0&+wF|V7Ov!^5r$~us1kSfC~=YBp5WEFDbN@kfHsPmU zP1ARt4CwS70DXe}q?3iKXMV%=n=MDy#Ia@C|I+OkrGDU{|9PWSpIxv!b~S$9LZHoL zA)iN9fjrajw?k{_crzkZ)^on!DV{qGuth09Z4u01*r!X`Ht0)^sa}?K?}sng1+%tz zhQBn+_m1EXWZHG`01Q@ER@v5hAk6HqV$?F3WItNJI;yDP1ngB3WF8~E^G~AzioB9_ zPcYr9i8hM`SYbW%tA*Pws-8+IYSyC$GyRzWP3JKd+EDvBX)D?O%srrzEC|qJwQrIqUNh6;)?T5@r+2i z*ZR$E)aT^n>?+kbF%=dR)a7mzb#rqicv@On4azD&)*^v4c|&0h>}Ff0#827bdJrEv z>ogKHQH7d-hL}qUrhC1)elX*ZAU5Tfl?)8$HO)r2IYsE(>{t z+HZbZ>&2*of`V^=$9b9J`08!UxpiZ}{}A%79__&3g~n*$z=+bi81 z0>E@mJ75L=aY$&$0!kAxv!~@Me^r_)pkout34p9&f}DQ}qMY02>cYaHYcY44Bx8kHlBvD@h(gtOhm-;9 zZ}K884#rp4xG_6TgM2&oX-s*NP$dR%9oLf=W?ekrhRscQdA|*Vyd;W?A8Tpr52RtY z8P{flQ-@79(p`X}Zlm=Z+sb7OdqRq1s&%-Dc^Fz`#X)uv(|(F24sGi{;tO~l{x@6e z0@O)G#7Qio4EaB9nMOs~=aI|yT9 z|L%X{F1i&p zF)&cMQ{_W)1|-rcV{@1QDBCPv_ZvR3dfJgcfC6>HIQUu8w@Hta+D>vSD!r_mmTtX-`zCE@vbP+1Sac)!RgieF5klMHImhP!n)@U~=rEf$Yc`1t%UcQ#R9Z z>+SbA)*pbu18?k1!-Wn4@&xlvvkYA&4%lyB2MDN)t%ug`wPNi~LlIiK`Hb=D{vb5f z4;ffI7LRHu+~~u~nxsfJ8Ht_zr#82%3L+22&Spq!3lC*9hMuIz} zo7tLd8<3RO$M%I&8z0dPHh%f0j|wDJ_LpjqD$n02BC49k^8lDx_Q+QVB|Rfn**@5|We?x0+KtUkYA{F}^Ib(^ zT9;;t?$}0lK%x44&J*Hw+Z`mnMcFsj1i2+%YPsG}iKoLlqh@#(NySK1$~}>$VTFz9 zEN$Tewp76L0sJ?qAIb<=rf_Fjo0_Y|RSHnT`brHoc|o?M4WW#MJto6_5GCs1JAoJh zef$T9i8hE~4b4VsCM@4wm!BxRmZZM#8gJ)ues ziX@a^eNyNX)2x;26@spHXUgXWwG!78x=a>qmBZ~qz0b03g$t+MG_tO6BL}cq#=S8w z%ytWdu6R8DVaPykT*Y}6@o{odPAT_fcfL*^0>O&7?6*TmO#pAYYq;(~H8lnx$+$GH zj}jyW7=mOrUbh=QAJ6%llX4U_dGh>JaWmW4G*n}_x$mZ0XCc5HB!EBYsmV9@=6Du` zWhwaHqycrjSs|t(+0Lw=-AIh=EhrQ&OkZ&Qx^N)8cB5SAb~IIAa`pRES^riJ=bPK4 zzW584hMg!Cv+WK5sP6}yLq7Z_Mng^&uqK#S_0Qi1H`<$pUiI2j#2BU8%(wNPkF_ET zN;t_u!*js&N>~dkO&7<1Zv9*-y{@XQh;l_40ur}V(a7t#XJBNC!Y#MWd}o1&BtJL6 zmZfa@EKp+m%u^#--ve)km7|=seJ_#hQJmUZB}-3zdo8t?8OIFGc->EX9rytj`VGM6 zrxoygN-q>pH6FlFC6YRkDi-y18*ld29z4+3z8n?wZTh}6IrQj#L-wbY9AB6?1Fm}5 zvmj?*Q(yB$&XYSg6FFb;NS)jpG5gdIYW4C_Civ@{WX`wuIDKHP5s;zI>rV87D*aEe zHxfC&)n&bnTyzj3uS+Puo>5fQT8MEY;2A~r)Ehgt#I?6vGRgvk^ z`(7|CP7MDH$8X01q8~qX1r$T*CvspJ&BKjG%(m4?_1OG!f14%Mo7HxP-|2QDmaLz%H3AV&-4=nx7A+{1%k;^x3 z@9A9&O?z|Ohb4zWtZl}~N|```&pFt}{ECBODdE;B!zR;6A|W+|oi}&twRo7fHn(rj z^gYrvCn|#r82I-?;+P?_lY`rvrlE$^O$)l`Ym~TIsT>4Q4LC4dRxj0FJtFSL#Q{)^ zQ&odF-*sc(nRgWt=dgfjVB~sIXy6?3t;l8f4<7$ZT-MyOciXld%bgFzlbP_-w|*hs z(jf(4ym23T0TF6MHTb!;*Z7(EswFzS#9nLCc&Po#;WcuY0`;fwMKYe2h4LvMMTbnDw+)O(@ z4v_D=Y^GzX`&J0nDLSZwCy-^2UowFPvxIFdF3Jxpe`{bw#y*@h*(QQI11fMX&hsoE=x-OM z>-}kTS617UH;0bpx;-H%NjZ-e&nTD=PN}TUcXmFNCn&9ICkXnsUdC}&r`{LLkJJK- zRi|D9Wg*_do8irffxw|lPtHh7mtf7TC2vk&JXQbA$jaKJx;~sVTQvO#*XX_UM*Y4Qtr9!yiM#w!>?^Q)&g3V$ zH9j;IT5&OK)Xzl5I9K1{&3(S7fm2TwK+qej#@Z`XMUV^aBjj>y`{$9?wkQNCcFG1u z(kx%4K4oZ9`?&soEg0Ab*LVY5D~Q~-#Q|&FJhds#)TiVDb;-}wvIYqGv)J}%QXD%4 zzK3#~KuC*Nixp-KgOee{4{Mpe_lyL0L9e7pIqEzY)HS^fd|nSTqx1kZ9Ngl%_I@aC z|LNI*KuMwxC z=A}L*#5&aoyh8D)0|?t_fW3S8(AP#ii0zUXF-%%DC%Jg@-f5%}aMB<8q%H})vW&TR zhm>=-I*BG;=Sz7D>6k8Q!Koz$;Zn;fcVz8=#7VX728;)0iPl9As6}|4(yqwO|2@eQ z3NJ^9A(W#)uZP1xA!qzqe$WMKg=mYazHtQ?3QrI;eZUu;kWKmJAQ!DZQaE=0Nd1@z>Gy- zbQ7})$0@-KaIqcr7L7g#sAv0#NFfO6;EVWbL(fGu3lcR52^#`wAL3IiL%vi6YE^kC12rTs z&TE;RmxAZFlsr{%hAeIl8~g$@P=Qrel*%pb**q8SUi*Tf;l;-H8<@2_00E4AUqWOd z)Q48>d7*Vlnejr7X)nG*o3HfhRv~6$PyY1jbsq})_Fm>c?)_yr_IuaBZVIuG6)Da& z?vjg(J{VHVd(Lx#;3|qW8A&%7`{E^7I4%vc2@G|%7j&U{Q|Do83?>&SV78ej91Pi< z^icnaRp5~|*BGA}-m2*X8JdKJ(=DF86?&F}doGPEtLG<9;Pf!<7*mdwM%fPu4vjw; zz_pz@VU4S^Dp4>W$>kxeOZ?Nj#o(;oU()a5l>uhetBVgt97I47!Ym7@vQfw>${kQ$ zMm3vmLi<&19D3N`c`>~1$6A2Am!s4wlp%>U_}-@NBgqeIX@3xY4=co5fC@=d>C5QS zdC3dEu4w^?6;fOZ{vM$Lm#3WfXGdR0Y$om_Fx$%X@%@(45S|1J z7O>u0P}ldm0q!zv^)EsmyygotYT6J61AE-F%xffM2c4k{I1q3ofGyx~K=Iejz9Af}!+W-0Bi!Q>#i^Nlc-m*9t z?CSH41i=&aKJZ6~1vYf?VfbqV;XprmcQo>eMam+~;p5euUoy4Hl58}<4r~-bjJ9V| zi#DA4VSii>)g$Vev0(@L6VvYk(rV&)6)X#pWqK7=36f%-n+e5@x35$(gsW2Z|Itso zcAP0UdFTSB4MFE4``jp)vj51Mv=?m3&Sj)Hnvkz}LzY~U_UE~bW|ELm8!<;Tb`i!p zAz|Qey67<`j10z}xd*6Y_gbF5pVa~d4=ub0=*Io~&fMN$)D02!hXNbD!~$23ygG>a zL^)r~!Txc)3yOFgxMj_YHyFg=txz>;m~M>6U#k2_i$aUz<{u3-bq~lwS^k+8XN2R$ zOnwgE4nDAEe1%ipCU}z>eZDgn?t~D5SDs$Yypc-DrFmzUTQ9}I^vVZDIW~*LtuFGe zq(9Fym4HMkvjv2!?5^;8aWGv?ZaPCIeE#Tra0A2!dgj<@pQhu%i0}7EmvUgNs#B*k zfAUBVO&sLZn~@gCw_{Drk1MKDlt;^$!udPZ%&|t67A3~H(g}(B<#(W$f zs%um|LjUxj4P+R2(~&ZeTMjJ8)X%lXgLcTZYXt%O2)uh4F8x>)#0VzbKlMl5r~xQW zU_?Ul7tjTDiE&|=Q_(IN4fn%UZ{OsL9%5Q8tA9$i&nH`bHFfZ<(W_FD-l5;`K~ggk zJf204Qm40%+!55h;sluE$yJp_w=34kB`t?~4#Eu(hd-8MkEseqy!>7=_PmIx4*o$j zu{Ys`?rxx6QdNUszAKMl#Mo1{JH^;UGhAtVg}|aQY@I$dgjh zXO3zx^MbDu0A-zBDoHLJ`_Adn+_&}&VQ?@w7@{|MofiHcu+P?~SB~!A@@PbY|4i=S zJl`#|_IX;|eX=D=2&Pt@Hm^$Bj69(rM?FRcCt$@NxFS{{(D;&iWz7lU)fVnq)ki-0OF)Fu5 zc`F>IJ5}=*ycQ}B0Le}6ts-Wa7n~lD{hU46@_BlK-@xi4l>k>E5#aIc8_ASUseo-5 zDC?ePMvfp}ajbmbyi58|EFcuZmuz(tj8dUnY3xO12fUOPFZ9NJ+!R4`EE*v;pX!H% z=J3Avs)1RnE<3d-V72*LI`}j2y&}AZTtb6uWO@d&~8O}8p zG&5Rk(eN4Y#^dW6%6|5M98BoE8LlBat`pizHUDlz=itVXB`)%=)IZPa+0?xGFScv_ zmY9L)4N!6f0x*D&)SRKowI~ku9yE^0qUH+CECHvakp=+td`tb5qSR&iunU~YdE$i zSqQLwOMltE{Rg?|yo0(P%TuJ5D~s864!Wf>c#pc*z``CtRaVri-k3kojNhMz^ktm% zct%ZDQ76EFRnM=p0&M@Th*t(LIEbwZQ1Rqp=3n{-eXMtJb%R3snC|&;PfUuY1g`!` zaQ5r?cxImgqa(?HF4%t>_IA+0BO$y2r^&AIqX%kc2_|8x0ue95E8w9Asd^zE zqA*}2?*;W!t}IkUWdp~bQ;$2URSe12q0?VUoao) z=6dcG!iyo=gdYZNub;!Yvgk8tQS={Zc8LUw1x5*qWXd|*^<_?s4@M7Drg^`g1v87c zD!LAhE}ksK;9L59C?xW}F5E^u!34MT-C$ijvZ%oH@+GjbUk8Tdwn zyK=Yd1BaY+5)^`t`$I?0K)a}Q6CU0HtY{BA1E6!wL}2!=upi3P#9dD?Z{UDZ@-v{$ zI(AWE(H19H7wDxn1reTX;03m*)$`(q2wfRMok^-RN_OjVy0PQq!v}8~*RP1_8oieS4C0Yy zeg5N1M;w_n&x;%@B5zRwoYK!FXl5Qc0yc-OXJGkbC-!6Ds)zEX8 z5j+EZ0a2WE&k_2@mrmUa8toj8`7Y4pPKecfm|hG6W~xPE*JXyTz-(Y}UODIwzG9CZ&$%+VcMdKCpABI*j(eMiaezjvT!t_ZB#2nxr<-@VRmUU;CtIgA1$2^h@uHi zGphGfxeUaqy{UFBaKZ`A4~s`yX3m;ZDRvbyN(ON8n>eqphYaiEc=$yL7l6$HV==+X z{gtf1CP{o~)}?K`+w4;#EHNfw+*)^v3TDrQhOHG#V|_Icso#vRtElbd{D;@%^=V z7b@0bH`IHA(`c5m#F7W@D!!%@Wjd>2G#5Y?XmWl;H=69m$4)3XKAUZ0UT7Q7+asv1 z5s7qDZN9EU=b~{!_I!#OJwkz}>ZFf2Y5E05ST~qAst;`HI!fUOZlDDAYCfk#aty>B z=f7;We>eH_&kuC$PoK?;;#0ipujQ^#)tpjK+0?Onv7*wq{>32pnSu0sL0b!TUKl6L z9%lU~ZCvtDDF*Rq&Knnx@y0B31bvCV3S;5_;HEv;O#f#uokeZw_B|Rs1#Y?6Z{wmy z=eMJSD~;spBX`3UzNj--9)tAT>6+MMB_AgjmQhMPqPgCld6(w1WqtElJ@r;n9U4t54BViOOI&QjiD-nHfzd3sKs%o zLfy}bVc^QWjzK|Cm|q)0i8oK$X9Zk7X&-wrW=wy%XN50!uQFVwu|Xbr=YG}r1>H;uM~ z)8*!Myge=hk7L*Bv}IAgCxQ%KwD2ubG_MPTy#aeD7-i&hg||j{Yr;;*Ema(f62WVS<_aDsEwea;fcdk%o;xia8z}1Z>9o1Vld&=TW>RpNutOp=!?2Ro_! zif_Ds0Bl)8h=!$EmC!FL<5Dph@C;D=)wyfqT98eq)r9s-;w0iM6<57MOd?y7Z4m++NV3H!m{Xj+z)YKr}}kEer>- zF|Y1$UN}uI+6-JQ9A%G|!iPar+VtJ~V3Cpw_uu#E)Y7Bl75L)$Tvf2&%$DM+v&A0br z(&w9dc>qhYqZ&KO+wyiOhk4%V4*)yZJ3tz2&oUe_koNG)7>c2FIiHXna_XqNjw3JZ zjAXVUQaXv5uB=m-RYV3UgcF2GJHE;NX$|t1shDp=D)Zb#nED)Qkz!o1&k&U^(jva8VSjOZUBYQ)Haxu=jphhTo>*jdJ||dmC@*Is~xKf%cnHDN)&RCwS(7h z8|S#BRxC1pd0ZCV_a;36Ty>A;f}R7}IQ$95*a0^7;)8jv%1N0hc8hu`Cd@Z+z$PfQ z^S;D#i?EEV2OoNMf^_Y5`${;b40EnU8E1Yd9<2K%hu$dow8I=1E4Y_h=4jLCi;F{Q zH}k&<#Yy)wjFe01L(sden2OZC3k4`Q#@ZTF5vh z3^6{2joi#@ysvrN5^(IZ<$*j(w)%U%Tg=9smoO^(1k2V+z~`PLXZ%hk#ptLWclE+= z7#S*o-RPe_seRC)g+Fv^j~Dh@6AK=+z@|}A;A%FgJ(W;`AGHIc`i6QChn(Y^9mOgbu%5?BNMVwHlxWDkx5)mzC!VPoXx(pXi=u-+M3ND>cdu2WIccZym)zze;UW$t+>sJmWAp_CB7Ye#KT7|hw9$bMv5ZOZP( z-~$VaUqajs`+B%LVi_(R?eH}95VSAnAr(vWyz?Q4+oU5Bk_Y4>*;V94jTzrTgqfiv z72R#d^Xck$?sNTF1~=JUIr_e9m>xG~iX^e&?MnYhiF#Fga+G3_`c8OkNU_?vX-YTn z7E=I&9QPVc?BV1Obp0{x>ES9&ou*f_qCTWpBZ8wdmtvgfbWXmw!qV3Wgctl=hIdvA zE-10N?odJO`FoexA17<6;*R3i7XE1G6;fhdWAHATxXRB}6Sb%`S#sKoL!u5IT23jr zQ}d^^afF~pGTQHgK}%4v%V2ps$>nuWG|}!!o`+FT2a3XzrfTQ+Zyld4YwKb+EyCA30TB&*<|-|cFw;5i+92RXDh zvhx=zTJSxKjZr;BcO<=QL`SRbkVfWD#i{FXe6ATHPUY7G(>(0V=$Jg{4?hQl+hU5) zU2yM7uZ$Q_BLxpg1^VrT*H}J@l7YHY@O}M+QMtaJiN6BV@Wq3kENapDvl>o!v6C_f z)|8hNHmd+)cD=j|*HoH8vk7wI)Lk-bv&;vVd68 z7W}T-2~$_ee(%OEIVIJ)EcYccBk77m#6Av9M97&A-864Q{6>TzGTjDR03Ci>`#_%x z@&Z);RBgTfIMDrW1xZwp59x(jsZ?`%s=a=ob%^%8qc|9SKgyN5=J*%K=trAXYD zqpISrV(91Yi|VOeYLQ&PQ_i9u(eNn6eR4WA3rJ`?X%&jVc$2md|{- zrEGd{2{VTH1ZR2@+`T)@RTr)F)l&7L{FXI~1lJc$&Fvoy(!7Kq2DSe9sbz+e(b8kaL@|;j1Xp2sc zHKVxJ@%WOJ|3P#Rxgizi2__Wd>>X>aY!yMnDvIAS6IrChRno3{2R02}p-&x=zcgaY zf_;DG7+yMaut@)+aiG3HXXL}V68rcR_*}G4EnA`x?qx>>#XQ9C?TF6R(8Vf02P?Qe zQ|1>^7*EylP(&AK6+#$)8ginNF>3K+kheW~=IW#rIprpqQ}=)kf4Ll(gm60!r247# z`8_U=a9PkL+pKR&VkGeVz+4xx+OB10K@)#9rlN0*6h&z@0cUDC3esIC9gB-h;$!_> zKDRMx>H$XA5<#ai)$#^GrsA` z%66=OP_;7z#0DoiAPnG@OiA8mRSX|^pQz-|BYNUMSfzo2t!wRH^5`Z{Z@RDT)A z+QG%ilf7$N$k4|Vt7|kDEaJWG&(bHJ%8LK|jU6WGXhD+T$Drm` z#WXMKG`8=4(GK={gW`6nzd&D^L0k9HA-MeO%)6G@xGtehH^CE)VVFN3xgW#hJ3nHW zTYTIb*LVBYgANBgFI6^MCwb#8;MDM~z%RbhL%asJbFSR-@qfX>jGF zM|NTmcuX*&NtWnsY)DVtU`R1m2c$*w?L^O|IB`Tdk(QaUW}(&xt2p|Zs`z}w%ky}> z1id>vaN2#b;5blNVl9(c4_@=^&>Ca~dmw~It^poef4*+lLQ!olpVnpSrE$|BifN{c zSHCKrKj(mM-RVoVlO$8ewCVJ0d+S1pJ90@rho+9)Y`ciJSu?BVyky(YBzJeMIx#!Q zUv+V5*x+W9K1_=|6LC_?P)NtfDF|>tR+;;Xwe=2Z6bw;W#tTqcitF0co!X#^U5!%f zcOPnQIs0K~&1?F(AolhZcsyu^S+=7dRM#wDp>-z!o!+6g;-&s*fT*XmgWrnQ3Q7ek&8%7Pi0y1Gy3H{Wk@ zd7^bE;9D1v^p@n3gHku?o$Otqb=`#q$UB5$D<8CMPIN(D05fHeMK!qeiXYT6ohW%7 zwOTJi_OS7FgS>$EkSe0Xl2FB5GPRLIQSPI|kwAl{V@k22I-$<}7$lldmLE8+eTT0=4j>o(nz$tWrVGAxf$>il{RD+fM)V0Z z6iq3?=c!14jP;4T<@z!gjtZ|;M;Apf1!nQGqiHmTx%=a)z%P!AVW=Jfd)@kA+VCf zxD6B^0&BsJ8yNzT?nDEy^6Kza=A|H+Q^4}EBc18AuDq$}iuO!#NyhO|{PvBkO&Tv& zCb-a}0WaL?KWQ~mG$;hqFJc-LY9MlgY9m69MICo(UD({^9k&C{*BZs$ZyE0hmT@t( zeex{V<(($~eLoG{dPn`kZaKV#pUadAao}W&7Osytu*2bfeo6)xDj#C~|XoSFB?clS01Cxd`kQAlO6U1_@#APp1(o zFn#}$`oPw$wb7^4PxUF}rIB;+P?nx;DW=|wA{)$I1|m%3Z#7O19d$e#Dz#`A86Qp; zz!H#Os%#*-6+(4wt|^QWA~Wx>K=<|Vb6MXm#^^T91CbC$VPxUA6oeE@Pbl}A&8-?e zbK*RZp3kFM|=74v|W=w~htWSKSbk`fPB`Ucfn0^=(c zLbifK)<`RXc+v`O`?|#wP5Ph#ZuTeDN$X!Ug0JDb5Y|zg{9Q-Wo7-n$ux*c>v17;q zo!5w#=UZ0aPNKt~4{9?NyJ9O;yDMKxBt|226rp-Rp2fioM4k7`PUzA- zJT15cSZ9T2N1A!zwb8EDPC%e>XSBtUaN^ZO=(hf3KU>7e z$?%lHjiQ6t(&l;a=`E-u+h<0MK~<+=U z5LhJAI2pFtWjFL-WVp4T|>4u6mek zYPnJ^eeOE{)mUzWr)|coFYgA#0gD%1>{z@DU(IwTwCCAPnPByj$-eG8U|QZqAIj33 zt|1S776Iiv#HRn^5L=q7{4-Pv6?n)@2CfMIe^h;UR8!x!bqb+{7C?F@fQ2g3s}c}I zihvO55CNqlf*>t{A0_mnfOHk5NC_fEYNU$NL`tLvP*FP43FSN9ckg}g+y7)R!pJyd zpS4$+bFQ_uSe2sMQ{R-K#39W;y`80Vzk3()uD|dQ#=k6Wt)`Sett=>JA`ZC70g3*1 zmv8;84<~q@u7#kA=BneCM)Ydc?}v~3NI2*_!~H~eT)X^xP>y_l7;EWen0<3) z83JwsTv`;QPOj_-Us}=}k%C-meueWf&+qM?7C;IJmOB^~2YZLM0k?fX9K<#WJg%q@y_li*3{*m;tmW zHt1hb@IGsuhdC5_z~f7|6}r~JQnpuYiag|aED|5u0# zSkS1B8nm5;V_+tZXPisqyW0$q8W47|K42|tV($(l{Kj-{wlgU3S)x!RvY~_0Xwh;X zzkCV{x^!zzkr}MSwqj{UqS0Y(#`uw)om;bcIwj;-49qkgBxWc{qMHZ6{ezG^lm4>z zD*iGmp_~W8ZD&>h6jbrS4p0#d2tz82r5zo%|47#;u87jPn;4nO$BF2h^ytsssB>t05$?V!2(6Ps|eM+l^0Ckf65YU=H{=ZIZEhr2_mHJ>4=q zKa{kR&NH<@G{H*X=5Dov@%#$VDb^*Wbs_tPl^^I^?o^*8D#8GK&CFPawR4e)+SO(K z7400XRi2M@LP|o6odOGR0%Sm{E8O7P#=o|GrpN0?ftQh@cZBMJNtKPe&D_J~#<}U1wj^H{_A>5q}EU&@`NjFg+@XD(AsHKle zB>>r13MiH(D)iGmpyZ(#Gn8h!cgNFtH~`W6iOr==4@W~X;E0OHA$e3g|5v$<4ERFb z)l_@(H+fJz7n>8P3*Q#dSEp-y`tXB?%O2aO<~_^0`z9k)xve1E%%b&{JXrq?bVT(( zb)Q`?EQF8Zfza*5BtA}5Yq)MEeVxT$u)Gq0T5~gs>Oc9=kq@Sg*o*PXGLB^6N~#EF z`yO%Fst42Neeyo=>0}m-+yjprSu~$lGoKE5u7LG}tzC+?FMihj2pHpC9!8m;j#}r^ z?+0nb_P}?l>T9~ThghFcOv6Y5%F=*X!~$zQR2d`)ke`2rBl&-Gg&=Cy&S&QD<(<}E zNTxUARIfzSQkv3ePt%L5c$oO0)9{-w{iwIS8K$Z7Kn$O!6HX_Zh1ID}n%CcxUZKlW(;a#0Obk*`x3@$gYVBt>rH>_D*BVJ|JZ}>0Ut(SPNDl#^F<9Jv2q0aRH=4z`+Q;$haO>4Ymh8^Igpm3RtyYL&^BSu(dI zzo|iT^A&J(o&!MN{>>3;&cGdY-{6iTiX+I@Em0&U%H-pWK4&YezkddmugY>Dw!6D6 z#~Y*p>{Pj{$wGi?G#mB}+n6PWgr`-$)GUgVS@M37;0p2i>3uDrM8;Qu5Dcc|7^z3( zSp(VKq9wTwRi|{*qT#&Sxt4Z2`dJ5s${RV{gczh^6&tSGU+LC$OXSbwSE`)T`qa<} zSP!6K4sg1#kX>czx-m#CAZo8=0!tQ$Z0uKi7E)^(+LovTikE#}GLl?K6lH%jbKUHu zY&W$l1jqb9Nf;2L1}|136$)yKTZ1>70Azsjnnfx0ERC!D@CMMaFottoi0n zOlftutDPMCfjpr5XW6u$4Wz07h329izXvZ5a_drwj&CRR&5YgTTU|O*g zXIO)Y1X`+;{OCw#b(ay^x=IctJ2_*wMTzEJIcxg>5~3J}y-JjI6e3W`X@|oc4?k5i zY56qp`rcd@|M`arkQQqJ6!>*)7Onv=Tb!)Z!YSOg1jXs%ix%FuFG1;-?aU^OK;c=X zdu&~;Kg+AWbxj0PZ%S@LHXr_&n20+BZLke3dBw1Jq|&hMpD_07hZufhRj!!M ze!0v6Y6VXD>5WXQW?KTe_pP8n!#Z7l+EadH4`TDvh{CV)i?J-d&?qc|_#pHE?Wh94 z_(_$_z8p0i7wWzdOR!q!2-Sl*jK}bUpvcKxzI5eyWh>KSyST%vyZ2hD6tqB92Br=6 ztvY!$!K^_AdyAFzm`atYy+q><37z=DZI?|_LrxCv z>%Ft4rA-FElRawS2h+b?CUIN1RWA3%7JL{-Kr10qOzroznP zo?7>gNqO7~+ql)q0ToaL!x<#v4r%7Gjt;yC7r`EQsusJnNYZ0r21arC1rWFPW~MJLgr+Ory= zlRz2PENx^)EY#kN^%4`AmMGbco6pI(ybqT9ei$yozH)V8<=}Cw1BE5ZAeO~p0zn)h zq9@x6-p9h2g~t)&E5rU{DL@9NsXv{2ySSajqrqH5&=^1o4u3;!K@|nf2xrhiH?i*N z+I#8aIp(0U|l1Ch{WYqI46g>=Wq zQZWwvM`>_(1LzpYHM%&Z*X8l9__zn5b83EkvyFN6M$049{eIyBt1}%62;*&B0|1sXGGAFfo?`^$QHXRpjr zSW;dY#bJnyvZ(i8cErc|U(wR$O?i?th`#|m{*AQ6z=Wg5f`__?lOQ@u(|Ak%_PEHP zlcwcn_#5HwysBJW1M6sIzy`|%>nU{FhzhEO`bYHj2&&)I_E}@S5L2(>)Ix&!4D1w?}>EW>P2X6WZ(<;$2Ped z)!GbNQqE6m5WjV;#)Ytso?(NA&Sd>gj~kx03mUY{`e;)*RYz?v73=wH`E3{)uIv5m z{Hr~t|10!UC}0F|@`8fG9gAQ|V@XZ5eDvKdz#SsTvZLa7zqJ(Ywrj#~f&)RWch_It zs(>BGzw0&7eV?x^Zn*pYCbUf#Gkg5?HjP+C*v*bfcRoR}7_tmth79Av-kNhfjWYf+ zNp~6ijvLetIhDpUPP?{G-GF2oQ!l|Gux2zZD`TMI11;0Rs=|9DJz+a_hj(Hs0XmaH2oVwlm zP-@uCyA`K^DO;siIabgs>=U2Uz`E-vfB{;2#Y-@YV7<{Vv0 z4H}Deb)M1ti-_2zHelg@8y@DKa-A=@Oe(QIp+C^?IccZQ)0w6T(iqP1b&#>x3o!mM zB#NJipTSQgSo0rS8+~Rij2Il+1?)0Rd`rW?MbX#Bg#zON)%UX+MlIBH`XRClyxd1Y@B<$g5BY3b#aS>ZnpR43vY?CSSWSLa?P&m( z?IS0L22Ep(vVd~dyA*&-=-uBX&UeWTs}ha7t|b>oi;<1*!E4m7xfty^DvC@-4RIenJ$G{^>)WSh@ksOpO?fYazNs4p{h)p-cY zn;nK}^S!CcY)(5o|Mk6!LArZpXQ^1(Rz)FU?;D-RoA5QLEo0)~bZqgYET3{=qxJ%1 z?9k?E7*yqv?4;Q;s6m&`q@C|!7M!UqW&^B(`s7jo+xqdZ$iU$bK3M&iAP?j zW?-Z%NzntqoWbuvXV-1BXW51f9(7ZR41dLT1@qF`(ud}TCfr3T)N_#UIfK%``0WPe zn(Pk~@)uf-a6f>qmv}Uvq(V6-E(H>6dJOp>a?FSR-}b$~)$E7IU7+g|g=J|i;5-yV zlx@YI6*?mSj-(Jm1hpaVth|@m)w-+&wqJLevEGwL{6k>&+c>oBYHH5?eMej59gWBd z4=H)?>g%Chg140v<0^Vna}2F0K5$fP`6pU654ieWc0uK;V7;KPs!z*2O)utDK>elJ<*@l{TCWjUNBIdUMmP;wZXx$3P3}{T&kjwpS>#^j(tvaeFb3W z|Isw}6*Lo_O3NNgzbLfj)seisPxyd6*rrAdZ(dlaJNk+` z({&f`2>IlEMb4(&Ato#Wln0SCsi;+bwjk&uk6#?DX5A>!yr%BcV&*G&qx#vwEQeg| zeMN#y9d9bFui)?GkP`9o+!5J71IbfjU&BM3k@v6ttiEX~->@GSK+!@D?lOzc(nKJX z85?6KtfIu;cv(S1w&{kDkVJYgp9Qft-5(AC&b4?%B zvVegu=S-ToZlp_yPpH6sPK%$`i54cBx=qms?XOxA3ZKmW?1D88ITvxE(Cb>bF@Gg9 zXB(E3_l2x*zOi6lgwA>KCGX;%T@mji)dckf7o7d5gny&cC)_cdd*fh=zC6%G)sbXRNyJV&mf`KhJ6&P54<` z{ub=jlE0#VK_s%2mK0ZczEPYj+c6>k*YafLYExiB<(n#cY7%X{qmq-*`HSLCEUY84 z?OIjO1Z(yC)%_jz-!D?aTMIZ#ee)OP<%jH+Y#o$o50F2%tiEH&o{~cMeLS^1rmyCU z1m9v-`>7rU>kj$_wf%0Pw)WbQ5^6%!+*bS$8BYOeQRi8?40J&DCc)Q1b3Mv|R*tb; zz1wDqVKX*e>fU!Yd#TaGE~P6;ucWAFiH~_J@zLSP3r|q=R{yLus7c4va5&=>!!TTL z%41Z=)_z$`d~MTC(#H5C*!jQ4UpJlde7pTjji-5|g>yX1_fZjNVBsaBdCVM$6H-Ya zbOp;Lhn@SjB9FfPn1i=+3O5E#RHKDMG+2V3e&2y)oQKwF^v&N)LbziF&z%O<0E_OIt|vaK0u^kK;Vu?HZ%Y_KMKJ zhZTi^-3QUE$9So7j^n0$*9AP%rsBuf_deIpC5`O0XQxF&Ty(5#P7m&ipdFRnNU;keQad$_LNE&End2C3YdqVM-T zv6u5PtH}Si=>ABIOZu|c%dDR(a`^4RB3rzp06AdDrj?|$+uwb( z@xWyaS|BtVvyxB~_JkKh`MA`p9O#lnBMlqxx#r(StCp|N2j=75&ej6i5yEd2_NoWE zNcL0-H-T~?`}2bFQt6z!X#FiKQk_xSS7j5YLO*&{J7+-HQBU1^fg^m2B{0#jxtkPx zGek1=d{t3etus&W*KV}X!#vH-s!kKCwv^!2PuVG9nGcCSDafX*Y}bZ`37gWEx7aQT z4A&+40%5#;Z;%WPjIWutJw(-BJu-4mY?h@BV1o(-^I~A8KJhb;!eQBLzV^4CB}R*R z!x$3o_cP)&Fg%7AU4=PNJB)i30koTvdwhE;nd-4&)jRDGQPDXPU&2Ydrg=ReHE;-K z!vq-aUeFUcZv$Ck^AQSTJ>i|RO^qhL*OnLwo(tyLTMhj@GLir4%oz1nxNPWwCR_uI zK%}*l3Ot4Y2K1(r-(&74+UD&`0a;M>hS%V`;`o1Oq zl;nd8rlOm+3%*+QL8B{8?Gp7t+Nb64;hr*}fkDu?PORD5*(qQOFsMicFtu|+Uj$q| zOH!Gk5e_>(xBSpk-g}bczQhM6;uO#gUrh8zq^(Z!+Mw-}c2H)ke5(Sz(a$AB(&dyE z9}TV8J2shnrM=66tTOW@MaQ|$PZx1D@;1l5I}beH|F;$_A{A)(@M|Ibw~vOR6?c6i zqoEbx3YvG)`zzGiCl?2;MmEC8tmMjJwhL}l^{(VQ6?Yen-K4gr$o)JK(HEEC4xXLx!JfiMt@M#jf|?|n^7Hm=hW3WMv4<=w7{_2tJ2b{OlT zoPE#v-uY#4S6gmVc8~l!n&@xvk`Z@(%V9bXxmdID)Z2Ks*@>C{ocNxVM*eap=#lDJ z=IZ3RAXXca=%sOb%Jek14U5dvhKuzzn}lIDgBpJv?=5ZT+n)a@z`rpbsQfkIkmRna zf!1pu|9po?`mW^+vmyH8q_wR|Cd~;+xgu{pQy`r+6qz;1kQlv=zF2saLInZmQ{WyS;*n)WP2?j}@+mhduAtTZc<*(3@_=7ml0>o%urlRn( zw~PZEx|1`6!x%#0@BGRm*y#66KXqJR5+3!<9|Iq^QpnYd6lQBVMxMsli(Vrxs=KAXSk{;vg9fQ%9Yicjw z(dI5KqG@+42w&-(g?$C~S|7vPuHFxE(r65S#HW*a@I-9LNvo@E?xifvl+899hkEO~ zv4goK0xxFYvBfq8=FscJ8W_Im3OtE*qMZX{N7dR~XTh_j5BP(y@qwPl#x08$TE%xr z0p@oin&AXZib%R9TaBjTo9Kd%X6Zb$=S4))RSJ|MfLD5_s;j#1RM$8F@J^s=(5{XN z3N_8{lv~c8%9Zb~?cmr97*LuB`KBZCkX~PM@jbqr!#5Ih^3FY4%3^zyT6VIm{;mmP zk0nBuUcVx7ttOhqEI0)6(iE__W56fA!giH=z+0HEz%_s<{UoD?1qnmyEzN1C3}>+P zVaIT1%H!?qa|41RcVwwa9F3woD3J#W1Xm!A#>L(J&=C!PrWvNX*qh?EN~%$(tk$YQ zTd^THPD0YG7FZq!Bl-|0NP25l1ficH1UJqU3?sh8*+_@EYn$0hr~74kk;m zJ0@2){k8PCq~E258Emee2RH@P3@f;<@8ir|uM}7wLGKIQlGfI5GRHZWOt$7P-%wQO z_VF_w&!=xOysS9=`l=n-bv3)lx4nZu8lw7goZ7>=Rz7f0M#St~uM>{Se(C780^H`~ zoxo}g^)O4Wk51shTiOkA=>-;NKrpDB2ZpS8OVof@8X3m_8hcs=wcxeut7 zp&qAV|C{=KR$>g|vZ2Y`k2z(n)t25`tde{Yjfc`Ff_# z?Gs7rJ>s5uJZj!I^s(7M*HH%Ysss5(@6mOzmGZdBTX>DEn9p|2(xyqTtR-y%P6B&M zn#4w=^@o8O2`ABhL4qVfc+ubsJV13z5$#Adc1V+D=m0@B$?{coQcY@`f^)%VDzf9B z=D9MddIzaK+|KhvXcNX`&s{mGhJUYiGr>HJxsnhf=ocgazc*;69T@BYjdc!dUWe5) zsPSJ|vX%7gv$>vQkxu{i?n(2&>yRM~p*%M=5e*c_j<{(^hekR(C@ z=#N!{(*#IDCAmh+w-9rpei3ZnyYAAWe>q~W#nwc9=*w&mM6je=HuGSas^Yf=1wUeC z{{#dD{Ikh+#KQh;y#7v1MunS@KkmzO31PsQanc*GBl_AVNs)mmxh1s(*A$UxRi>b4 z54BP`)7p=ee2sq@IOe+OfjY6(*g~wJVMIPKJ!6i_L$*zkAWeXA7&po}Rlgbs9Xa7- zwxNTfeS1Wnu4;kDBDklZM^GTZI?&iW*-bY;c~_R?aInq%D~tpGH6LAiHAitEmY`YR z_tx$gzS!1{ES%2WqCj)N)O;q-c90>qR2Wc$aS)X}#&{K3V^ym3Lp;Gh1T0pM9Z+}g zF!l1VN1x^u(1ga%EOHH`hwy1R77ktd3YtPrvr_uKQ)U+LrGyp4nz1$a)|}h)AO^=3 zk4YU!9p}OM4BHf6#-Zj_Bpu($jHilLCU+=OS~-CeB_aZ5$1ui>f2evn(NXnkLi!}^ z^T^q$Rt&SCz#{vZgH6S)4jtleQgF-v`bSAQx<;m;)Kx{}tnWv(9@LtbcTn_K*n`-W z#dlCQlEA0Q3X?`Jm1l>H&m(WW_}W>vUW;7HK33S2BUGB7yd<#nu$CMdjIkQWrjapMtr-v7L@&u};gR>k%P>bU% zHVL~%|g54f*`tGd^2YWXm*T#dQ$V_D#cRL5h82G|xGojbv(DvBtJ6Uz0oBD4MRYc~KQ zD}p`!yhwVOl4DY`hHavP6Ngr9O%l2tWKPIU@vkY0ysUtGYzv8QT*sxvcy+qwi^aJWDCy;A$?G>m`s>V7y$=TpENS-t0~AS*B@ zHJ4gne9Zv66{{vuZhqExGlE%O9xMyvI1mXESQ%%qnLO6;rpwA zrB26?BHMYMOF$B7W{D74pef2ZA^%bnceD5~y~n3kZjN8AqVt@L0UhgVe7r#dBI(A> zjswouW*qqOGmAmz;QD#(3tDa0FUDC;Jf3k+yfPR1IA0&~#HxZ5Ou#PHvbHnl-S|`2Ae9-I0fXy5CU! z<>cNR5FhB6CrJ8#J*L4XudjwL^I0>e6_UxHpa5-1KW)`s!puVhEca;mr9f5GRl+Am zzXv<_oP%Jz4@q&D>xm3$0Y+OPBd)ThK%q6-1&f5be358njU6g8=Zn6Ixk?Yp z+u44rOnjsJCD5s?gNrgaYsGy+quzGD-CjVh9#2thJvGN0eg!Z2EyyxX3AQHbtR)k_ zG6lRGmD-{DbiuVx;cj~HKZ&jDrsfNBf-9Iqam~V3M#5~rD8KuXE?r>wNJ9Hw)yD+q z<7;0(Q`nVhVMy0L0pkF{?sdVXZ<4FrCsD6ts`sr?FXSY5s4~;v4eR}lw1yZ_X#}FI z(HtkA=gv&*ur6Ni!GNh8pmR@7!}&P$WZ%Tsy>W8Fdw@OO7MEFtRLlp|UK~&9a3>lO z5kPS%a=Tt;SdG}&Ng!u%5=?b`{DGq_OzJqPB*ZRXXW58p_pi}SGHCw5`^lZ;e5Rpe zf^>3qW|nE?sD9L=Sd-{qUMOzAvE@nDGOp8Y0iXmoam>UOdBEF7c|Gu?8tu_+wt?1l z-1Lubf?R(&AZhppr@(MXGihOUJE^8YTifn+ zLA(Uc6|R2TA!KY!eh1&*V34k%hCp*K&J+avl}lJFbz(ckkGw zrY^b=#-rt)Q4^4E@6kT#90m>dE0;O+dtyF=M^;}wF((cj?9Qpihejj4srdloITM-- zIW73`m~T3+@`U;KjpO~8@RT)|p%v@6G9n#m|5%jRBrGACeoKrrG!acwNY&Tv?;SL- z4=A7OE31n2aOT2r5ws2&V=urzuP3i*B>B&|&GJXJF8NU*1tQmee~y3tT^?CO-3Pz2 zX8cx%H5cP?{PGIj!R3V<*6IeF#O8|86kRluW7!430JZ@)yUMa^Us@uFZN+g9F6xI` zLhF6YXL`%Lc}}Pvekkynqa9^d<44>{F02V)H2A0s+TGzmAn;GGvFf7Uu3RBYhYD0B zcXo(ysErOb3E~j1s?%PzKbB6l{zrIF!Oqd~96hY}E9qheq1r%r zyz`p$i#-L7iy7h>7Y~=*Ob;lt4EJu4=j_txihV>)Z|fM9yJL!EI0kuQfkYRWy4acB zV_Seg2N9!*7l<(JPOhHH7+8r{l2qWQTCbXo*gC*EpRZFtUhua^C&K&-H%!%x>OZ*o z36R`YEJ`LtZUMtf@cExpsM{`v4V01RWO_8)r<^}@Hbc~$o#LO*#3-;=tsZ(=))5uY z*0~;waGO%8-^@-;EF-CW?>vxM&1Y`0IclOM(M*Llb8f|Y+9yR{Ydg2y`1y}Uv;!1? zS^D+XYwV7Ou|R|F?=lq&;wlR@MZlx`5+;M6};% zEQ3ujaZw#7#cP!S-~M~`MY2j>`7ScjAYuB}N!s^9J&mwO$cB%^sRA>cusb*L+PHk+1cIQ#K)S+o&rSRS+a-1S!5Z>MKbMIk!1fW_zr1O8zcXo0u#c5@I+gmj- zRDfYR#iTIf>HP!;^wnp<#1Fst1U*frf08CBsQ_rzm#$P3LMI3-h%E+GY;2*|q$i!i z5?-r~CXaX@?DJ*|5$@fkgxm@&nY^|;1?dL9YA&% z#Pol-_9)}%n^g2SzC!=cWkfZmfUgx)jm#?!+kzD(&*P9ov=tIgtPITmLiyBh>A#56 z-?QlCNU_HaK}NK&C%F4eb+xX-n-@0i-%tGMpx1d8=XLs+y%fC4$QL_UTh9lEdB z>l%GfoYkPUh2>!*?%h=*vleA-D;orYe8o)VcTlXJ4E5SmZ*Zr6r1x$Gyj}(CXo_*~ zq)0USAB6)!v!kQ9axYZ}hGWyC$Bd0krO{sfnoMfFd$d$<;(E~Np?O@7Njf7gP5bLG zTVGr)0qDia!7I!(K9kLL=5YMLGlT(8SV5b6mBRxhgM#4X2CiPa%mhP+=mUC^P{;cx z-x!+<5Qt`zZ!~bLxK3I!A9*di=S`^!=_IL*7)*>lh(#+=uTwKU6u6G84D94^s$Nv? zp;}cLxY;X*`g4!NmwwN45$u>m_*(YFl6arATMU#(dG27Z8xCGsM|EPSM(4hI@{*;= z2mkW5|F%+mpFWwP;%cKltphj~To_#tmmxHApKb$dQrAB!pJR$f z-WXd=)X3P~Y&4%K|5r>G^|W&p2$xoZF+C8#T&XWG%KXL5(Ak#K3XRVUyl@)Eeu`Ur z&mzyS%sv$xqIMdhj(WSU?b-fD|MqlbwDR>z9>O_}KTqyjsZKaB?0~`_2uK)-J5sh~ zdS)YbS@*s$uJrv1Yf}@1gTwlQ^H^q~KUu@7vx{Rt>WxMH2_$>IxFhe7IB==#zfiCkJ8iVN#F$OIs^-1}cu&R^9~5%gSuprZ3QXMr4$1V`cJsQxCj;S?rsyP8ysp z)2tqqA^J2T*!ybdk$@OmTvl-h381%sx?YkZdNPYBKx{+G2-URA z5n_7P9GS1T*fDsrb}AdxF#>2!Tr;0Xb&OZ}$2Z~!Cui+Ofl3C3N1^!W#fl^t7cfip zl{*;!S)SRZG?0B5V$-71=ad((Ohr;3CK-%y04UBY{w?aaS)hGKCkLz4M3|Z&W@)z> zv9W;ti6*4ReT=tFcF0Kh6An%O!0|;w3V4cd#C8IONw8aX7EJsgmnfPp{suGT5kuxp zRb~t^mY?-wNNWk9YZkw0BcG()k*`N8phTksFL&0+@tP- zTjz!Rek4Pa88qnr+w+Ki?@7}o!Lbt93Q;~93H%@}(Ui%$63$xr|)R)|j#CTOx zzeyL=$XiYZYx4Nq zQ5!o~R;pE#bax7Kby4&8fj z6Jy!4LOT4DPg05CYjWOKL!mNXL4EH41RumRCc5C2OWG(i(U@3y5aH^ObVhwS?aDoD zn0Qim`_(~q$48^*b&yQ$P#Wh6F?s0Fs~0&=6HkG{Ai5mx3FeELS&HCM+dpPI;NSuQ z;#B(8nCK`EFaNQbnCL2i9sKvY`=wbs#FNq8dfd^VzMm=?0t|Hw>AUo)`3}X?vv4`B z*iA$CcF?-gt~=r4p($tk$wFA(vYr!1+x)8#uo2&i#8vZS==Lcr^oP1$jucfD@$sxJ zZ)cFk){LMMHyoAqgTe|gaZjp*ai)E0=J4p!V1%DFf~G!8n{oSZ!JXZISSVd|Q<*rU zSEG6IBZPV75lMOnb-5Tm%zS3g;sVA)^LpvLO3i1IjRW4eU-x9TyxkhRyZf8-YUO2q zysP)EH_^N7i6hGFLb+nq3)_4IuBir*&x%lGIugx_4bsq)(Q^@frbMP%Sowh5nk1I3 zTa+}3h|DhZtD)11|4{%C`tLD3H{;`qZ4mY6_o!E$>aq@r!UxjpMj2%!>VKB7A4twm zD~=!}u(Dtyed$kidpA$SPvs>)6q3h_@bjSHwIlfM?A0jy*I*h)M%tl=d~Okvj;_Uv zTyMMh92o-hjv*eiDr^VP>|~Aa4yH$LE6(r-ARb5n!S%@iR^diOI|o4^kfCDf%2G7( z}r8wQT!g)pPq(a4y?$>^ZP74IsKL+sS-1e>D#Km5ZEt8sZ$8-x0A{|i)wFTQFBY^ z{7Sq)Iy6Wh=uxa}i#ze`{ql~YNLE;24m+22_YJdp@<#2~LsHB)HT|WI?}tPclIu~( z7QEtKfWpL%z6eos>Iw=^;Nto9lJmZMlJ z1?s~qH3)L!*Z`>KcF-+i@Q>SU6=@UrzZKPg7VGo(L>E%HVzlTD{V4rFvel^Ohw-q9 zfKm-+kMn9h&jZcE@M~jRq={jU6&T^;^ZL7Ur=*{$;%~X5W9z*LaA}Nt&${ zs<6^E^w}kHVDHhL%NJtg+%XRRCRs0j&YqKFK9B|i$mJqUg4^$v$BtLV%_{^qgNeZg zj~O6Ms-s2^faaN^=4~!FX9~AV_bF8yvj#qqbdGXdUH4WCt-Fnl!r}%OksnOS?y*hz zeC~SlMRChJGW#pLs9L6W9$z6}6RNrw@5H{G^#@1$-{5Koi*C}r@_>b|nJTT>_dX84 z==t#5W7Uav$ke{fx9?+0&achw5D1@k=aWDi>VoUDM-srHq$){8A<`a})A2Fo!Qc4{ zcu?01%_}VVrLQxu;^xhXxoEVGy<}I8YZADC*dgA+j$zi54&c*4y|_hnOoIl}V?lx} zxtgyxPKVByqyAuBy+ke^YflU}-re?Q+S@$+G2+nvYJnG4a{EDr0s&MqKvo9_%0nYn z!tWDYYMl|Ah`<*9L@$9qlP$xvaKmXTwQ+AoPu#}tdJ|DBdG~X(0Oe5M95ms9mi;Is znK)xEN)|s>BqZpo+9mMww)BIVz&gMa0|O}`T#MsXSS5bK0astu_(B4Y6+|{ZO zHs))C?g^jlJLrzM^AXrMw1dP>dCT572axZswxt0sUM`TamAl@%lAj~7QDAGPHY;9* zx|y@wKm5M|urE>ut3&XWL*OT_k^QeP!Vk<^?g!-gjSAkW#-%Yg{CRT5W-Jg+lGtr zapFinxjk;$@iRB&FVUU+H~(B?RZ|`f1-RIMb`hGw_<}I8k$YfYVPD?g%eUD}ZsR)n zRw8igc$a*uA^RT*ar!xo5~2DF1mYrKKHrmh(&OTauPansT@E8skrBsGRtd)>?(HT= zN6kI*ks~m@9a(`7u_IE-aqt) zn-n~uNpvSsLYQSU<7to_3s&SXvc04)L1UQ3X4K#8oX1&5dbFtk6Ah2rxXFvJ`OAa4 za1Nw>M$JGZFXU1BG`8z9ykqQ+)5Qj27oPHJoFXl_$nAl@>L~e3% zxVDN8gGhWNXkj7FX48SIo%R=KRm$>8p_v+js>gr$|lDZcLt(=HD?)++>AK@ zK1V&0-@oNukD_7x-EZZj?Nm*o59w!*cNb^sfdE0{ThrOabiD)u;inO<0XpR3GAQ0Q zbROWClbc8W?aNz8%*A7$)pI1?T);sn1)6tL?gr5=JkO%WllX1Y;d+LEH z8IRzx=qcH$E=tNNDq*-3h!4I9ygy1u!;HtupWQ3dq7+5@w3_L$P0YNF6n8aTa!LWa{1{ zQ38gH?3KHAND~gf5KkbUFU$FY9`3XzmJzj&p^;XuisQaJowV*V{8y8HlttxBeAnfb z)Sm3jARV^HHGD%0*?ANxZNZrIeyCI&{%OhX*=B%z$~6EIVBM%00^^YRHDg-1=P?7E zV9n*XW>=%TQg8#iSYx_$g~AU<^8ZvfM)=Y^))NsiWrUaQAE{?kJ%j-Hs^G{8sr8{u z){CM0eaYfzt{otJAIj7ajwEoR*dDo|C1J-$4>ztN4cfY^_=iM~Pk#Nl1OFxPS9r8g zEnU)v+;^)?h|8ff%OqH=0CNpVeh4 zVXgRA3@-<*_1b(ctBTX-o4r9ipvV7(rJnUd6cA~}_miSeqRrr?Hb<1V(KG?rNp#el z_z(Lx7$dy()6;zx4yO-RRxDxXwXMHzU47u99v#hP|4X^2>MjOCPW_J-pw_QZ-*N|H z>0hqFL4s$`TWGzDCJLi}eQsjf-3MEP*!bMt?$B*QeAPJE^IbNa!KQ2Z%Z2pQH#EKX z3S%4`uloQw*^k{1gTKFq_j6&TxWd3^A^DW&i=MX`X}lo?Fb=8$L)-W`H=Xrv;!C+i zw%9wQ;I$BKn_fRau?I;}Nfs9Adn7`5Z@zY4o6|AR`~;~?Ls37;)mgJdLt#?CGEH*` ze}-b4Nd83UBH=yHYP#{e`; zdlJO5I(Jv?IqZ^%L7lK~y!qOewj22oKpn_&!<~W{;RARxg;jL8QCC3is@Q6y;^j^C zg@>HJ?dEIs=(ui2;2&>sDi4Boxqf5FsY{eTz*mHtL^)1+jw7FR_)5Bzcs zNST9jX50PzbPM3(I=(TfMGV9%)fAR(K!60OxZtj=Y?(|4O(~*I+{AH8gnK9+~VTJ;5lzW z@hfQ(xPfg$r#_FhM91l5=6J0$U3pkhZGW}z>rBm_6-o0J0;t(&r()J7Cc-VIS5H;(Vb z_54o(d-7c^ zB+Z&>6=b3TP-T-m=&71hFW{VhdIy^Y<*1~{^OCZ@uhG$e5N)hN5Xy9BuAu_FrXyfy zn&p(YV}!6N143BWE(gTtx~+Nohf9g7_m}RZ+v;jR>OE@xqEXY5y=KwC)VA-Xi)x9c zznDiIin!+qSfs5v0KLXBP%HN4IG=C_NvBIkZNbNE8A+tF`Z#uPxTNV|=QpBk$)rJm zfNb}3Z(c`<*rq#j)FggV?s3@A(?hnPu9|SGqF0y`ZSeH=r4`F%gn7P%G<==)51IIJZ;+(K_= z0*Q|yf*|Hgw<$jE>!$3>zqeZDM0jRk7Uca2h(P~YX{)(Wl@9yE#4_*nr=WJ#vGLlg zWr@zmIWZR-MW4lm&PL>`0H8>AY);=FxERLR_4EL3JyXYd)(H(y% zMqwuJC=D5JU$^LV?Z(VMIh@(SkuB4Nx2L{pnl{F|nclPC>!1g8)iV`JB?Du}6 z&o1;%nq|-Md$iLZ&!ZhsNG5(ln2Q|%0YsvISt#8HikKzn32nR8wYOegl5w@Kwv_>& zeSW7_y;{@3;Mc8h)FjDBRY9m$CAs}N_r>;01nYlx>2J}jfH}oZ`dzH(#y_8TH&7c& z|Nf_?Cj^%L8@PtRr3JGly4mU=BeL)tvudsC{k(%)7lnFYB8&&#NdP}RZF+eKxIMR$ zS9$DQT8HO9CcYnih};0>v0xmkNV6UV8^Z_lsZ~u!LVkS;g)#(IL_kcx)`aT=ve7~4 z`d`v#hqpdxPtwtw{J!gVFxh2EZjZ`cB_&O71N#5C`s%Qz->-kRQIY}@D$VEyK|o3Y zVRW}JK|o4CL>e{>N@|2Oh!Z5FJLIhriU^9NB1}?3y7PDQd7f`Pzg@fd1H9aO=RWs| z*Lj_DY;Mm#{0{h^=VUvX$b$nf1`Jk(;Y5hy-+3jUA9Z;ZHKVB{f|PWltWs@P%n44t zdVcTYiAu8%o@o3%WK8X2t(R+4mH#6O*8q(3OP8ZB1 z_GGl~wX9T6Pc3|y_PVY`C9Uvi)D1LMP6c&Q+{q2r7#*|ZRf$zx%=)xdE@ z#C>J5w-3t1Q!O5^j@Z6P=f*LX!o_bN-qt8E1Qq)pS0ayjOZ$1mRI&W9m+!54pOcQD zu(scd`e4aJ7r;VpXiZ(G8?v$@cL)~|5Th8vBDhP;UcbR!q8l+axK%8&?UE9+L4{gg z1*t~Q2L>2AtZIxPQ)GB`l*Bhnu50TeGg<`m@UX%#|UG(D^ zW1r)ubJ9!nW7@ww_kqjrmWy@Dm05spnL(LN1~LG^7)Q#FRqb#SZgd}@u8{5}I3DXm ze89CLt%A^5`Y#_!10Er;xHaH@)MgQn$35|_pdD9#yIL1B(|HX_vy@mYQ}z8C3M_B{ zqoW?{3W5(FS~>6Sr>3uMEO_njRQ0^we z-ZfO6t+;wit5)VHpQ6dZY1}wN1xlCo)J!VkQ(dtl`V*atmv-Sco z)@3T)TV?yEQ%I0+K@re8(AK3wV^2BNXxu^&mTUDXZ=aoo{hg&#v!-R~`D^O7UNmdh zJ7hG}q1=XmzaKIvsk}=>KVA9Jbn*#e?{kcb3J4UQTX%-%bto2s%bv0kG>YT~G~ebe zKaT~K1b8l4t{StOvCpHmbK!>GMOyc-z~djjKBTDs#=FW+8U!2SF8_IPPi`vHbpz>5 z8kR*;gXUaARF<0@=ic#>9yqt>dExJ0Q;Q8(nthr`HZnyL)&;q)lbNfJt6OhFlF)O| z2O{BDMI;B<|s9{}-je2ZIY)d(a5TjLOE^f`V%zNe3c1e7GvVmIadq7|=E!F!Q@6`*2 ziCH3^gav0l0VnlWQ9qOV-EfEI8y+T3mK!=Ao)68aKOBU=(N#fbk#`|RsVy1PFB!G< z#2ln=urtZ>Eg0s_=DCX=yICH%54K&@5ONdFuQ&NSg1HE~ATnBmTbXYw?JB?>v%Bf( zqOHeAzON!E9obEQ;%N&ajnvh9+$|28St(NkUT>w&dsACAU9HsM)1VviWb{#@6CAG^ zo%$@0O2n*TEB%I#r~)3WY1=@#wi@ML`0fJXipTAQvQch=U0HN}r2c5rEQp$YN=dU9 zO%EM6*xZDseg_fA3)=MsYd9XZesbshbVG!EQDH0J-)g6u$vP+p| zAr-hBZThOIy*lnTGukn-xvjGQPpb{FB84rh&#zyV?$zL-7*Sc&7}VMML`%`hU82-! zeYBS;eC3_2zbm3RDk{MlYO+*ycm(+-c|?~;KK-xAZ6?{`b)Lnev9nX`T{;v@9RbX` zd3?CRf-mHe544aSe+^khUD*sWzR}%8WUPz8oDGP~I&}Fnf?t$QGmmCpcz(OP-sXk@ za^Adi*r^LSE^L9N_r-lbdg3 zDmN=%sTDE?2)I^*H1I_{I8l&fxh~4Ze`C3}x)>8=9i!FUnsCd$g8qAen!_bcaI9(9 z&pcMO;u~yf)m&dc!v94lakarHi`6_&`%exf}2LA_GVC}pHi zu!rQ_R@4&Tm;?b$=sqK&pQ7i3?Xoxw5J3t<@irD{&wtpzF*9QVV4y>S$+0s8Ml0%X zCwp*v3#{D(tbozq@pT+zT>!XizJ{S(8@p3`iRsD-P7T~onuK6h``ho-9oV_R>FmfLcDzfDL9try3)~L!3jbO^y7{w8f4eP^bgoTJ>QU>4 zJ`l@7y=1+(+@}&4wRX7uvuza2MswtD@kr*Y3TI0q9pQYt?BqB2tjbVM+9#kDZE&jM zMdWA78)b}l48bUwLT zis5=<$13)>`Y!uJeqAjG_hmUY(1dccf=j2b*gECJ`XPlg}_Ux93T{IKp zNBcP5gGBYpNHqA{Id^IFW|X^F*Nw3|(H2#a7BbvrR&1luM~_>_5!rFS2x&1{REpHB ze@#yX2sb=#+yBT}G2OLTcIV{&*1`eitz&XN*&Fs>9wnW$2Xvlym!?}-7TCLgxJpn{ za$oyaeDZr{{>TcMV9u4P$FFn9Z_Q#oG4iTno2R@*c>mfiz10s+G}ju*PT7iCndHYj zE`DC_Xi1Jn>OFC9L16Hy*_aUR@BRo z1h`Mu1H08CEe%#|fB%F7;ap@h*Va36Bwaq4cZOrAL6-cmIofVx5H1Ux;AP7F^K@2^ z|FVgijQP$wt6ugH|2_=GLwv|->zCw>w0ks5zxWs}$PSyumg<<8#`)JRX(>gw*Qh_y z-cNmCMM8OPjcSK@>18eRTJENtl~l?fHTtU^5ty)q#+9BcyqTCTI?KJ=8rwY0yWajH zn1h6Wj^ zC>g3w^(@DUUf5WJtwo#sz0&lv>Y>Ha4Un9-BC4qMjV{q{B{VuSQ9XR!^hyml#}XP3 zn(T9}&u(qDQn4bgP|1k{XX*+Cjw*dQC8A{QN+<92do9e3vxr2jzQm|UYc73d_z!$Z zmo%8k5Cp%t>GJ2CR(asx{SZnU`y7%->(o|q^ptwqFCWI&O+G9G3?ZV|0NhZkZ3i^U zb;>odwe!RNcqOLgYK}GgldBlX$BR(1YAm#=Reb<9Wj%qUA=*)y@AF{$0X3L?R1W=k z9qu+R>sR(>QzhE?7J*kpWaK*)+TcQ>rIi|+8`JO8ZM!r?6yLL^>cj7cj&1e@h76-B z1=gu_etEeKm5`@Zl^^C2_Hxy(hr;l+KT69X`}aNUKg$9b2Cu8O?9%;#>`#qw1!;$( z9U1z<8u8l^KW=WL0Nz3$(ylASDRvTuMB<9ditCcW3mlpYlzN?JN^YN0Mvo~8CB zn9a=@tBFw!j?6^)Dt+bOLK?3)qSGCZzq7h`&t3_U%@&0K`qj}LLU8RqVAEQ49_$Gs zX=D+tEQ#uiuUqvg%i+r-Fi6lm%yV}EbUFImZl!oK>=U2_D`t_IfsNf<#!=RmhPynY zg?JZiXq0nuP89xU0u&A19Chv#=(oD5ZmFY=T%{pBaL|(nklxKd=l;w2X}Jk@2v`TH z@TTZtzvP@HTvs2H?5*h7F#nmcoJ_Wmp4EVu_3K`F=e|hKlOJZH%ZMg%VxzqB;ReQz zRxVGb(l<5}>}W(TIVvf#cg3HEqwVvSFpnxFaIMR~4G=lBT9*b%bGzFOR6%3HVc+zf zZ^4jlWHTF^TX`>t`-?rargovVs4mh!?6=><{NS1j;_6?;mGCi%gvviuKB8n&J-!0Q zIWc_Y9Gp=%#|*SE^lK0@qnvZst$vwI;@xkO;U4Q{7CL-2^#m6D%jVoySYm{@%bS77(I5n zWuZNO#ufR`>xuDe``WoDfTw6qTh#3PY2xj3<-YB`YKxZeNN3XkuCk*<*#mtTbQ3!hEY|}uTh0vJwug2{P%qf@|K>6M_*~b zTE@bti`Do$Su07hJj2)eG%^-=NhVbbGoC6X`_v)vh^o)auq)3hCwj_mY`tqy<0*e` z?hfZi&o?Dk?ata;u3q@dabB>uFuqRn+Hl%TF!F0-r{&j8G=ZPsyz#BbTzz2HYW_eP z%XO9$M19o$^CCYg%G8XK5aH7UNhZpv;XA(b(9G*gUHJ! z-#xP2GdoU9rvG%le<`g3)P6R$NeYQ`)qDXpkbwZ0d}XWN*}p$=bAR4==6ei-Z099G zq<4q+AI&lnc+ZsIN>+Au0iKSKyakr#{g3Z2=3;4NY8*(1@p9S}=M!TxDHUfdPgV2J z+-K3XAvRd+s;P~7n&o2>mcRv2To@>-arDg5Jgqo)1_ZuY|5eGueLG-$I^fd_=d_ST z!GSWDEdeA;PuiQbFl`h^RL$tt<;(ho9YwX`Y_S%`U9 zjRR`~pSQ}`-&3p0fK}*!m9CNb+GMnJt9HS>pRxqop)#GIsm!wVmMQ=opaZRt8AW~e zfXB#TqI&<=EsDRmOTJe??Yl}6iUsnRCz{>7izkW|47bpCtZO@NG-x{ho_r3oaR?>V zKw7`aSp@fc0om>hYXFT@@}8JnFd+F(S;djrXx$Moe!SfKTY?gORbek~|5NsSQKpP! zq)U>h?_!_22YP{U7wc{qs3N;B-OgJw%6tZYXWqr5nxCVl_o*F~N!DTX_3Jc=+5v1T-B0+K!wQ6dH2gu<`ljoJHAi4@oU_&R z`U@|CaUWEy(K0ys$Umzvz%mhiFY96d$n*Rp>@Lg$zD#CI{ES-m8eNh{2}ckdXVXmj z-x1P{VpsPwi!;6mBaDNh zzlWAc0Fwz?y7#h{>mRDD_e-xs_2n=nCBI&YN|Y)v@&y2OX~`dNRuGjTRj!8OTK5v=w)CBkD#6W9>OXj(cJ@LJ7Xor-D;3fP}2~A#ju2+^PA1OhmVC zNE^T|C$y!GyiZ+qdFL;-{WfiH{JZyq>PVZDIm1n*$V@Bgi5Jekk(sz#{(!o3?txz! zN1gMY=Rq)0`Vz1@CvL23r6zw5{#Xh-^LW52BX@Ha3x6>~4~l-+C;+1`@gRZc**O{+ z8@a&?pGp;gGG2q8f=*HJ!fC2J2Cg$L*XTd3@SrRM{Hbc!pW~E9HlngA)0PAP*Q_0# zmcm%)ELGg>uz`ezQv-?-miGQQ`|QP-s0(gI3l+zb8orUkw~0p!M1rVx@6wU z8^nB90){6Zp$IERy&B6Qo3?C2c0=>J+NUUfr6&xoX?=>O$-Bemk9AWGavsW zn7mP*b9Ww=0p|0N#mFTIqulfW^hsob`K*&0J8@5keaumjnoX$GD-*$%wsZf?;GyvZzf;Spx{jS*5|HIU6JoO0dDG}o5sdD-QoEwqv=LLnB#Wa4;M8-Od zY*84b0Zm-lbuc!omE3iAe6`c=e}PME^5kuNxJ?;eUb`d#3(aLn7vaQA@BZedNHeGT zXzKfgG>j^nDFswH!~&VzH5SQOIZ{GN)^rW)@gyy=jkpd!tb0VY8Alee^CWBfE&o$w zA<~Dlm}vVg#sk$ED}ZX{<*ae#?(-4RX}%|=#A%Z`G6Wg_+S|L1UkXl)=T#_Q5*^E} z;dXUQk8hB9-B=S5x?;ibe+^a!^ZOdO7bg11 znQ;_PGM7mP!US8$X1z$tXcaG{_w?c!ge1nW23Kdf24*NRYI*p&s!Ifh57tBSKT=j z^~7H|k*TGOy0!GUpYDOz_u;z;l!gQkivQ)H^dnTi&wN8_hD}r>>xtd-l>WkA(UmW= zVd$H4y?il^Sf2auUSuwW3{1^%!NxqHbC3(X+V=;CC9lBYv;4DbWr5xa!j6?B=smih zN0v*`b1&i}{F5HIByJHepqt?IjS{E@dSp{g4K|Q|BS(AT=5=Y=cN;) z>Uon%87+(=W}yy|mr!~B;(8NNQu&(F85Z z_h#tHJOS%}h=gyW3Nn9-4g^L7V|0E@jzm82{BsNm48T|GBj$&UzB(F#Rba-FT}1Bi zpQ%c8byQxj)Bb%9*8NG>oBZLlq)rxJX7OL-B8g`?VA(geM~Z1_dO`&SEZU?6B!14@ z8V~95vw_J#R8*`k252)v1|%8ec%`_Kt6I)th{^AWY|Q~(pE-MnPzsKNYMEl3O)(z# z8rLCfjJ3OWj6If`87EUr;VSxIxsi%oK^=su$eY-?#3C*`_qOCYU-(PIpX09JV8kI% z?48sN>Fw>mI*RA|d%4{JoXWcmi0sEC=Pmzc)<&v;{4x}(R83rhXF zBGgfx!Tzd+ywSH9wp)Te<=e`PSYgL>q;?(dFnW?N)2D%QeWIUe&#Q)6Yb1vrJ^2gr*G#sXsXnCPWfWlDC#;op9C36(F1~g@tbL=94|VGD64T2MoV!2?QgS1rG;`q| zp$j^a#E!N3aZB=op)|~-Gun%p4O+~KJnE^_#me0AdDj##rT@BMq=BRBbi>8W%g5i> z$ev$gjkNwGjn9%BAy%eRG$o8`!tF)96;U|6xopmZ&_y(%7&l+woE0~p&HJw*zIbzK z1p$91Zy&>lW*X0L+*GoV*LX7bdw_u;>FZVdq(3_W9)6-J{=sB z^Dj(q4T?E4)MoEzO0F^#D6dqvAWvktW?Vzr~^tSfD`sh%od1PNYb}cm$j7GU<5R z5GfT^&qLoex0s_KkNX)Zc;`TpMr6P&X(@qjrY{V?(E?yr|kEOZ+FJP4^nBC

    YeE)%sfRGl{?DV8KaNsP*AGtRNMsIY!S5-i(!JMtxM5KtfO^SQ`ueO+X!PhG9+n zLM7Ie(Zt2X?x=y?$pQTQEg@VtB=lK#pueLZA+4ROlS;+aCKmp9>)>a>?P9-Hijca< zyhHM}Q>i`;9MWl^V)>jFPJ5ltb-i;RO2D*Hum($b(^Lw(a(Oebi1eiHDqxl>A4aNHzH-)yt-X8ZyLpnJVC(`}lHmZ{0 zOdYSOUSfR(>p81FB;o0w8l*@(EZx5>!RPDT5hnFBN&9iIuuH~m7f0+WZvN0oz}4VK zw(nnrM~>tw4PpCPpoT(<>GEJoD8c^riq9}`p)5u zk|^VTMN-hwOL}yH#qMk4DoH%IXW0+rXQJ*zSwWkYnIYe z{>JG6_~ZsQ)IJG{a%zt7hJQmok11Ug)$|LlNaVpH*Cmz`OjUWIxU58eXUIi zrpw8=tr+Q~*rY!38QA{0c?p7|ah0`|vcAyifpxU+U6K)a4d(4d=@IU~$Lp)lGG0o} zKKqg)U2zUS1&K<}vfY@#bx>nFrRU=M)6MXq#TM76SO?Yzr#D-bK8zcyhbNSNQlu>r zYNTcX3mW!Vw)feLAD`g6XS4>C3suDZ6>t$>xVDvAv&!>Y6yvuPP`Is!o}gS3zTT%? z5R6SA^cyhd_a>j>@QaPaqoe_ikmM;DiyL*e7`2}K2vt=re3n`&DfxBIa7IC^ICSfzwU~IUej(%wPFj6TRM|Q~xhvff9oYwD&cTZL`|=5`uL6~}&5W%Irdlb+FD71-ax}jj*!M)`kT zuM2G2Zl*0ExaFA@(a4=X;rD5fhH1n2qLcCd3@!F0YLiX-N9_Hgo!wB%MwZ4TFc#)| zL8jP!mDmOBm|N~uj6t&9p`FEsZOPqp151%A)9gLQ-}T@V-38%H`mlI-7@IO!UZ3|0 zt3ot6mqQXwbUE2>H@x~Is#wMftSVnzBU)o8^h9*ou*ntXR~2~Z1+}h_-b`jX(lY)P z7}1_VnyLf?G{p{fj$Bp0KD}q>dfGVlf`Lz13FMV%DY=?g!eXQmBu+Xj`$Y#kmWI^c zu9sRMy&w|{(&~m5;=S|hS4qREEmZ=jfG-do--J#<_k}xA9+#-Be=>_-XBMRAWN_o( znVIoz8sev^!2H;R6A(@Y4lwSk5XbrSH@Tbg&+$#=)P0lbqf*UJ*k5#s5B3rhOL=KkXRyX{?G~Og{*-v9Gp?RjCYCq)%OLO= zuZR06dhITc@<_Con!{$_3xmja3KGBn;LPS=ksYMvRZclr(Qgv*!@jc z>?mzw;VP6)kR*i_ktJ|}aHl*t=0S)oy(NuYO0ChiO(%Wr)Q6(?%>OeLxeoUYI1457 zp?R^5OkcL$99I(MdEAu|KWG^`!a_H(`;_;oyiLzd_btf~LW~8!8{;D$dBLs{d8J+z z{k@K{w&4IP%9TMV^p0DbUemc(LBOPMR9Bu-3Tx`|{PVkuHTTny6S{4~A9i_v9k$#U zV}P*}y#*mc*|`vMIog2QUKEOvY5x*F9ApHmAf~Kr!?ZtFd|qNj+DNe%GRBYu(6BY9 zf!u_bn0{POyC#thZ1?$98B|HFG*+}_eZ442yZP05pmex7ndI8LJK`kI>L}LU!Mi>= zk*|dgP)-=}_G{n@EIBz{?ZWv*A4K1GKtHr#Gw7Ydj$rIbKs=f85vR`lyP-S`ulJG1 z5;rPSao*q8$`n?JcPO(4S!l89JbgHgg8_P;APKzVU}#06tp1+BLeOa%1gMKJ@#k{l zW}i8F{yR{AUq;+o0W3&&J<`+KUPc;Yb$x6i%R(=W8#Q2CK}1WEVDtbr^%S~Dk}x*r zL6AbLpaTg!c%@b66gmF31i`vdYZhiBRX2wN?x0ugLLIfVvT@& zEXyv8)aj$uCCjU7`jJt_U$lx0SB@J&RLB=<;6WUcvo~#3EXb`xPe$xq>UVT8Kz!{U z!GjQYzsTfbC@Q$?^!(j3sLP@4rb0!)rQ)N*^7}@rF|k_8R6p7J+fX z)S*aX+TpokUT#9)bO{61{6LF=(9hn*5|Ir46|pfscg7ejF|Tpith1Co{l-XCP)m2j zrUiB1Y8mz3ISJrLuIZA6T^CxqNOJV{IJAT>fegP@v!RXNtmYq3HNj>{x$MB|KPPV9 zL$^_ZwExb7PRjxI+wxRoJH3fD)PbuSZG4XAwQ-nJA=hOWu!p!5VNbdu?8K{Ms|g0tUS0D$$Uk_mNf^QJk{!Dhu4woiDf$sf zAfGbu_N4V-QM~CzMWl}?Cmj|7!Z&^V0$e_0-eZaY9)T8dSI6OMRxRDY9uJ4lyu5z) zGu$Cw9Uixm`a|zjxqMewGIts$@CtJD`)IkzBx4;Pb&rycivMF&XSa#H#k?;p+}z#9 zPGdJwS6lmqJe`Gl>@m^**RqyaR~D6D!?~?cGWmt)lE@ET5&b7&XH%lfwg;xA3i=4| zeq=hnB~7>meS_3U5-M`n15~w{_}_HjYFLiG@^uf>%i2Co(~}4*ZmlF|z+97>W6S1p z{7H3@jStq~)Ehf!bW@K@dho{K$#hH+{bxBVoz~Mvu-x%R2ZG@NWt=w-<<3ok+NPh z0f4hm1P5wUI@&aaW%h254HN{Ne;%g$Q8n*&?-u02%vo3daeDrvhOZujUvgYU0zRnb zkEyY6GtJ(uHI}?{ea`{HQ5K2K!`<8YnO^JK9gm-Gog_-Y$?_Qc9yOjt>;J29D4||H zs1Qi7t^U%2b=Z)4DpHL7EYA*l3riyhaj<}%Ac_?p@-n|lL6LY_SI8}(kWe*rjh$A@ zR}{=9E;HUyzr=Blq8OgtK#R?1S@_q3;|| zC`K_4^0a0qw2e!n##cWn8D9A7L~wzyU?~hcM2l^~TJiRkQsu#w&3BOC5w)x3ifxLn zk|xD4y($v{i&;f`u3||Wa2&(Mq>+YTrP2)DJ2SUYWIl9Z#_SbN-MnDwC(z>v6GJUdm zqG6IW_(Q}AWg}HOrZd`aTVieg|6b6S0IjGlp7NC!74YBBw*}rx5ZB*?SI9h{-R|HW z7AbjQ*EfL4^|bkm*6hs^4sbCWY}gtpz*yDjD&iUUt*q9uv)k^e>~1jqv}nY+h!`BIs6XI zfrKX%vkJS4knYK{z8$7DG35+=VKcDVNBUl&p>MZ^-J%2nB z(@yH7vna1a&&aj0C%@rd?^?hvOgW4X0&yqad>e#5U)e5Zi-V89-*y(9h)GabQd+`C z+2vq+lHLf{TNx#PTP(c=|Hs6EpLH>Bq2EB|##nW?hdbIYrnC@Gd@#{4>c85U2Q)c| zS1$anw=(|lw~9R^N909xn6x7hD{|5341*@4g`L_E0xrYD;ZK?6oRhzBpN5$u7H(ai zyq+;9DQ2Q?p+~gxG2P!=T-o0dZCM^oG%f9{@gr~R)b4ROe3dKJ9@QhfvHjT?J2p_z zN^?Xfmto4^!?0PQQeN3;f9(jx(v_5c~n_A^MW7c?20bJ9}E|0i-Lw+ugE`q%j! zYUn;wrLW=64pay!UEkld&TgbW?k@;r4tm9xPM7Y9a#)IKg^l1k%44n3f0lTPCyvkMOAn<ko&R z)E);*=azy?+ef~Ds)6kg=}gi(gDq+5RtFfI+0k@$&IH~W+Y`Tg}` zOCzf1WLv%W?WEp$-Q=^ICm|R@IT@$}56F3l5O(Rk2HWE+p1!$Q$H%0K!fga6J{WK- zf+Tl{^eKB+V?JQ)5urM{ng+Tsb~3(1rgO(+0Zaj~J^#bJBuagxOe6^$$23PVP^HnA zd@CmorhF+ysp2K(HmoTbV}FLPwS7!ptU=V6c_#5ji$$$AzwQ>=*iXIC@5_p_9`l2a zp_|wDOJPg!1mfAVTuM3DBm7XUXm^`zf&_$hlMDUZ(M`%l`-iL&rT>`r|5a_F-lQ{F zBCgPyH~RD(z^ki*xbwVpl8{4Zp->_r1T#&^0_H|p5cc6+#;wfo8c{uSuVaSM*d~pe zc_e9EZSg|y4>7SAHSe~2UU9S^S;s!~+mHkpJYDa0`uHch3i8L+Q(46#TGn?CiJOI7 zP*XBc_On2vVI&(u<66$vW;snlw5bIHIAWCP5QuwZmqJ47FZfb2ELmprB!?VyXvYy^UqFJi~^ z7`81nl-qGrAO|ZRI;P(XL!andf_w~z-_N?bI3EBvVL z_>J29REOx#Q`?to8Gg4^jFYxb{?TN?QEg}i)}-l(Dv&xWunwAq)ArfZTQUT22W0P$ z5j9e=7``8++@$wbTB9ZC6YM*fNl~~8I^g0AuodThmjM@)LXL;CrDhi=YxBjE#@k%A z9Iwdzxv!Aax%5XNnX@KSu&myoBjf&&cFah_Sko**~uJ?B9Slw#pzl*|*q+gf;u_E}$ggIuMgx4~5vFVm-0d7O-lX-*>h+Vs0Te>gSXnyqbod#;{^bxR!)q`f- zY)2hDKb#HKI}v-Bs6A;<^U*xEibw+pdY7mI*F*L4S^gfb;{_y#j*jTg$s2?GpjUX~ z98F6ah#98{<_F4&R&{ zEm(|E2z#1-W2!Lb;3PgIjJGrPfIW;N;EI4vtofDNtyQOlavi2L@c^+=5cj%G+^P2Gn+QccHyPW*QH423LW4Md?W zjOAS|JhN_((-^=KKvY{zU#WpUxkGpa^+J-!FEXhQo+&7D2FDdC-SYhx7XYJMEM$jd zBlBaWe4{F)GmaR|b6Y+Pqxbe0O9tpF#RE&j6(T@nY0VF*I}gG(q~DMSAqd?LNy@(5 zqYQR2T5%YYhvFbF)ah#fb2(;I#Dm*n?_DABh-zNORgo!e`m00_`BHCcTBbIVjJpue zYWFxAq8|=Qrj&?pG$n}acx1l5fogAf6$B38Equ?^5R`(hTyA7yVN7WkT9|t?AF%<- z9e5fOt;?ppUdb^hc*pNDKPQJ5w{~SvS!T5{R-hJZ^BL5k|8)fT8*hkP8vrQX%Y;`l zViySJhNjOT9`o-EGop=uJk&tgX)699Vv`3Xnnc$~<|ZS9J+F*0bfh5bVM~Yxk~^gt zVdq%Yti@XdfZAE!!#7G5c)`RG^GsLlzCQy-{9N`}T*4&BA0X;#9eRGy5^Cezb1i*C zo4(>xI-{$_MvAf6#|BIhVw1P+yE^!|pd}JQ!A=7fKfS4jI{cv-67JXe8)DWXnFm)^ zuZFZT5Pc#bf-)ryJd%DF%0bk7og_rT$M={Gf~xFQ|lSZ_mlopdwlxZH`c! zLHNK=inj6Q!rz3~&Oz%2bx@d{cup^;>eyVQsWJdJnVd=~ z`ryhpMkwg7DQXHiWUxrR^mjw_@iDf)KFlRy@5^pdyQ&gq`cyKH_+EeFcN<4(=fAXD zt6kmmBB*Bq{m&9NPXq15iHgYPbTZpdiD$)hj@)&MV~ib@wT}a-NNEIa zgG-9%e#V{i@>#WTQuZ$dV_duffSc;UbledFA;&Om`W z;}O-lo>bG^yxMjRIU>vGT7pCo43f-{zLu>a^ghk>R#G<)Q!&WQ=9Q~QYg`K`c#BA8 zwS}I}%OM5BeDUWN3(rJ?g^mVMcOi!M5UBkvF$31$F}W^^-LV1 zhF9>$gQU*S*s-KeC2-2*y%zi23H^%fh87{Ta!qGtn@Z+2|ADv3-wxpIa63e~7`nCa zB5m1oNiW`krQY)8yk_D`ou`uBFkh*etzk<-%Z$81)rB=XLWx+2M>zU3mE(;d){0Ll zjGisfvairnA0i@33&R7h&Yx z^y}Yed7PhSvNnCY?OFiQ2%;N7%cximer5V8mIQ9RX{%+e)u^ueFq|Mq2$z~?rxMAN zXBddP)lLpHcBC3RX2h#o^7bQr+x7nmF+g-Nlm&!*jvY8yX*qlFfibd?wbup5H5$!QZRE|hn*TNttu;%Cs$+Hn;IM18dK3B5l`o19h0bjs+lD;z| zf!(UhD+SXSo6T{J@)C02(`j+TdltV`(J%9tsl505|C}*U;OQhsX{J)4tgZ&% zJ1JTGuMVth59Wfn8RcY2FZ!@Hrodtj$bP>6}81Ig9l9$hWnFNWqtoe{Ru`CG^>Z#8-1K_mH;tNBg@zY7 zJE&+CYqgm42KFoc=*}>MEZqdo^6g)AO^KvvqJ=x(QV2fEn#~y7$1R}X?Lv9U02d@& zqKdWzC7t`nEPKe}KMD=0*ki1Y^sFbTN1kA5jXabAYr<{B0$Uc=R<;G`qKy{}Y zmZ3LDYbhmg7q{fkVEE>!zXY z38UUKF??teM-oQ9McczclC3_juwNe2YS-q7t`7pmAUgDi{G%LD^D_=re~whX2&z%I zJ*r|ui!VFGt_D!lq0k}hC(MHEPU&0cq@K~RH2Ga)G*vO?6}e8*@KHQ6l*9F#9mxuD zeNVc4H6@31O6|K~uUpzbB1?q8Y<}MUw`d(i3sLy3j_x`Kl*65i+h$mB9AXSpB-RBkTxd6hINU~W#XF=)7UJPhF*88_&XN; zA~yMf-$IDcrG&a0mX|om7T0=2<{z1@Bd#=yuf4YWxT zXXQWr$p+WRUdbxq!^N=~HgHnZCL^nMQ=dVr;&(%Dia&X0DzXn={w0M`v|8)19`Zm@ z?vfYp$vUWjsOA+(@I~OxscLiya!v6GKp#r-X?+gg_xB%yk8S%}bcPqY&3_>ralPA* zJl}A)sD=YZ=5W0Le-JISXT<|pKl`ARGeKghqSi-`-&iF7Cvfv>${+s(;qyI))J&6d z|6vuiQLn9n&t5O+^C^RG0bUoL+yoT&E0+FahR;NZ-@nWTluGN{gKr5oGrkjMv_uso zbE_9YfvibhBG@9|3;h;3J3#7jNwJIPdedjJS@<|Z`Wv@sMfSBlgB&*PqooqBq@5^{ zo*$4shL@7HrmxGT&mB>3-_iZ7=M3!-y?ForXRGl5BFgsmFeh}KuH!F~)=WX+%x-IA zX+2U<*tP>z2LT(mh$5^3aYB-#mkD^RlF`Sbn+%&j-au7@s%LeBX5nU}kGJ=U&^yG38L*uzc|YuH(Lj zvc6IO8@z*h&h#J6W+OG+BCy{7R}`5}EZ%6tolR&Gc>fg&huu44ISyAFvhv0{m{eFV zok>JcB3K?h4*;SbCliNNr=06T-0_vRhGHFmr3)5;NJb+}x%saPF^If(!BA3Oq8zw6 z8x94pH&5L}ThH~fF>QFAk#L_$(xfMI@i$~|S~HEha-F{QEcD?Fw{ph6c6@UYY6r-( z-Au~o|6v`m5nVHj|-I{Uz3nN>Qzn~vjrV}%Rq01t2Mukrk>fMCZysw}}V&21QB{N<0h4i^Cbh+d_$IdL+okCDB+N4^HK z*>Rf23!pxq?sp~isI!cRdBdv(9_z&7X#4aTxdD;?o!)DAxK8VDb@`Noz+H6Fa!FoM zg+KLgAGEwn>a3$DK_(|E5q?ob0))+9s!ZNZZ9Kv_? zh~jx4N=8~6*nrU3=*>~13S)ZPO>{l}evan&)5BOqYIUFEX|D{8wt>3U?%p}eY z5r4Wt3bk6&PU^gixJfcsdgu(BMr_h@d*TI9X7!rhC!*M~5AYg(GUrljJK`0Z-Ug(R zQ#y>53M_<54~8=nl)uK}4!67KMD})X$B*Ro8!wL-`Nw@p_v3WsNZJ2XCx2oCQD;=g zocAl6^+_{*dl^yV|85%QYE({KNIa+cR}%m~rg&NOopaMGhVRqe$nB|gaLw*_y-&8F zl@X{EIOq)< zaf!pA`;q1PG_6Qp%NK zmOKI0K`?x_HcY6Qjq2C4j$#mkz#usc3eq6b^E)?30g0Xp61V?0m(mc$!q$knXk_N3LHQFZ3SN-f?-p)CcZ2a) zT_n~rNGqEc`alW<+2QkA#66xiSxKd;R`RFfV7{E+Lgz1Ebfza%f(HWKcf-t&_OAh6 zdD6NAi;+rC464FK4>{C*RiIPpS2qX;9lj(7kMqDn!XopfhUZ>l&X+yLO+V*Ta**;V zZxxVKx=&9B2HoeYHp8uU@K^&jJDEj&2{6LEhHApM!R_8;uJyEHT$!|+CxCbJo10fw zL@lb-)>FxT*>ZEsOxBiayWKEZeYaSs-Z%Ic98)l^D(H{B?>;{We|K_T;9s%laxMa6 z!J?J9KNx1budmWR&&ag+30wON;*Ed%5dTt<4!N7th1_X_=Ulyf!qKx0v!sjBg7Qw& z+ZX&yololk!E;No&_ut8?QNwG6_o6Da(?p=R{Kkb@C!H|0$yPbhI@_+EHn2}7?&tk zr#-PxVg#-{>5$yPBe2nU#NlW0sPHaW&tAeQ?QMT~+d|k+x}}|0u<8-$Nc8Vj ze9N@Mq$DH!K=rG>`Hf%^k0}fgaegY+yC@-j^G+Ip-H={>XC1R$+3Rm1^pI%!K5MT; z`@7|&UOUt}H?-B*;qO73#p*pM>W%-_Nm>iXxWp{nKNlIv^MO)#z?|XT_jAu;C;cV+ zfej}v|G2~7+T4Gg`pYE}P?IaJuR#0m|H)4%>EV+J#5xHlsFRn$LkGzg>Zw*i;oQY; z@^aDzE~*;F-1_ySd(Oe^OgO!&atWv*c`Cv|Ss82xsKnWRr6K$Qj^6>fzypsPoMhl0 zeqp~p3k*H8ykpU^c)-k<`kBeuG`T#t=6V^hN~19J5v)pGYXPOb%^z1RcJC0zjBaUT z701}vKo^Jr4ceL{n6fV?p8)>!rxkYa7PWso!!?G8`FoG%{u705q~*^(fJ7UtsTT~{ zwT@%f**Bx7UKlVQLG4Ig!NWF0=fu7uhv|mQ&(oJhls$IXx#s5zGOzN9&;l}7{5}6N z;<2c3NPsyaXTi7NpAhVpaCkt|aPM4}{7TW|dz$b$Hu$#zSr&ppxSO)iN2Un`fV~OF zw!a_Cp}R>I_Iigqs<1P5i?O9JGyST^JD5RcNs~KE96% z3!kYofBm-JB{5*S#ob^BCZ<2eqyyA_YM8o`ukx4g(y5WTfnCAJjpF{jqjKcGs-m7# z?U4Q1{V*2Z{wYQZOiOO9%(=+SHIPaQY6$o10?+>62f}~xg+!&6m^p)$FG!bbxQh`U)BCohWJGm?^84*a#1S z<6meA(gj0@tHqWEYT)S{raP*wn|})p+JbD_FZ87sokp2G>R80EOZ@~jXhUnl3+vD~ zWMNc7Jkbhni03KIhb}zLmZYX>yJFb;6b-h{;0e`}yV;%hD3*OHGJ0o=_gC(Ea{r&& zt~2!SXJ8dn@n6PeWA(H7vtNvI?X@R}LX^&x??Hu{Z9G?c*Z@a|{Z?^wE&O8%)v*}2 zwRuf?;%ORZ6aS4%yo0pf%-{sjjnsac;hr%4ojtZConwl2s3CZm=2yH(7IkOF#r`{4 zElgxRC>R)A;W`IlcB4grCg$Kh<^;aoP7DDIF$Zw;ci8r5Qo$lQNp^l_#fMc&$vh1Hsen*`t zPCLX$*FM|y*9cYM4Qqh7{sX1d=2FjAWyyB6&CkGt%JoMi|NrsSt*Us&L)dea$w$-v z58=>d_9xVN_X{{cfwblq#JEPfdhGN@O8E;&c+I0YDwhNy(NukEjnrHL=HJGd5fSgR zOIeP{ewo15DZku2(TWoPX@6mgUlU^-^NQ>LnET`q@_$Z;oorF};?RFPR%r`Z zDV!4sqKQ%VeBFX=D8&ZVBNQt_Dl9U6<=`&Eui2rcbqZBqCrJ z{XqQs3`|2+b2Y%K3Z|Lq&xT<-Q?ahRG-zpsL1^RHSM{Wz3sy(1le>|)ZfGgCJZoXs zpPIjKeai%(ED<-jbh4Nqv2_QB;w0fn^nwM$IO zI@6Jvmcm=f1b;{JD|DPLB%4^m)n%VBLOmb~yaJVGp7}-SZQ*kCUl`#7>|>a+WQ4IZ znB){g>6dbAvXnI@+S}a7LC{@b% zC06QACuAy(r{b0iF8CHoH#ns z$O}kzFYA0i#%Gm4IC(Ew(_Id`64az)whqTR;lU2X<7*`2psp2ScP{8mJvYlb+r)=C zLqGjY&DN!GCj${8!qz*|qQdwO6V%LB2GYvHEhcsJeX19n67`HjWc*!+Bhg6kv~}UJ z@QL;a&it>_2^&sI?Qd!DQ$tz|oLNYR`Jv~L=SRc=SQQ}X;VPvT0+tOt=1AC7L z}=`c9hZW6%LQ&KGr$pxtBxP_m2H z7M5_n+$Rp8igw1sefVU3Lp+Sr9Jxk!j73pVyFn{1O40%RsIM&aG<|iiQihA!@AgYI z@peT;)797l@_s&&1`Yu?NRuptHhAt;MQXj9pV2+;%z|SQ{by4L4&k%>Yw%a7&jIQAchv|6k1y+y zfP&4^r;QNM{#3<4<*#DS?q7+pE3OB~O|wlFLnyO(a&mDsN`BLV!06nVu{dnt2InHH z6Y=ZY(NlAOP`u5&c*!*UL4=q0qa-P1=v4954W*w)4xz^7h0o8Wbrmwym9z9y@YS2U zTBRn>qu2K;a(Vw$jc5Kn`mG5{Oi5PYaB ze^RwoJlUcXbFPY{WQ3>lo8=E6m0(2uWye3>8UvbFHw#!Yu|+{USTj3mw$)Upkv2-s z9ntU{@}CBen-P+{!bjI1cOGc%_<@K;0PEXA>n|6eYP_m(NB>ts$HDLU3nB0tuzd`D zcW?%Eq~-+27XM|@r8irs8Vt>L*@2DNS6TM3KB_*h>usrZM)U$KhCY(ViauPmjkY&p_#_Ry!m)ezW~@+p_IS0|TpLHWRfzrtvNMM#op z1Z*1-Im16pgQB*SBi*IytADJ5iNHL$TV$>`I(UmT$)Bu2A3C3T8)>dUJcGr8BP>?l z@F~(`3UKE1t>w@&90z$f)f7jX=s7h|mC3KsyzrdupPKg~7vCxB-7KsLRQJ-;_1)~W z$*`g0+Ox?4fzDNVCy;Wa>zjGhGW)HOY4a5AKLZ<}{gidVita#s1HMn|niNv@t*OMGyT|WB%+R zj*uU`#X1-_t(||tK2~Mi2KMv|+^k)nytANUTSS(^nr=3U`>}7rr{o=r?S1?S^hu))OW`B5)h%dl->76$vEgd7iZSzm6rvG8B?03_|q#n-%@Y{YV=u zF^6ZmH347OjGsF7Y_&t(hr=TpYvJI?Cp@>pPr_=)m+*ros1524jc|QwMfw$jhvDI0 zv+ikId`qP;;3#Zda0K*OSFHLkm9gjcaTA-KtyeMqwJF>1w=SjAw`Y~-^-rE^Lq=$J zY8^f3sy@|$ExNwkRv@_9x};XFVcZjr!mZlsZ)sD*79sR-_6zG}RJ#HWn}gAe$)G=F zer>Z9bsvxnG|my}3>DBH4V3yO>Ywa|OjT2!8>i;yu-o}_{ki|5={28fmW;bo2MM2V z#C&B;nWmMe<+9bUUH=>BvSDB(SWG)EbR+pY_??%F-lE~ilxV2r)w%tumKC%=_G2+z zCq;hNn9Su{F9{qZc ziNMXvjdGgW(DHmki=F|V089_VTazf<#eS9J=If2(DBtuP;d^l1W%LdHbB(lCi&~qzb*jtTAy}f9!8{><7Y9Htjpf&ow1B~(se;qIW_AySrfNX z_wC;z_RZ%U4Q|2q1D-d=!{zwdK(D);Q}vLsPHUA`BIq#>h)M@v(QdBM1ceK6s=4YB zr6_xEcG4JKTYrW9c<eg}p~wc+aHPoq-2?jZhu7Zq z&?AvK0;69MrPRXt>V;hT7Sar@(sTQ2&pJ0&qM zr)Fz(3Iiv>0olX|1TsO%}phPdfp z3l`Pe2_lc~Qu-@!!XNVj^8txZvm9U{a8dTomINaAO^mPAl?Gy{zpJ_enle# zZ%CN*uk99S*`)meO-p`mKL9o<#L?X}Be+ZV$yJv3?~<5vT~d&R4MZLhilx}8`a0v+ z3-i5G({go_9~}J3uk_X?d19Y*RG`=YN_bO~7!bmMP_$Fjb5m}-66!anx(+>be;Gis z6*eZWS?iW}Q9gm86*qK=G{O4;@#$&l|FYKeb_X|GM)YQqSAM(C-?`6)H%KeFwA4em(aR7PA89i6`M zlUm;~XS_b5-Ds!HQ-*kW1YMa8>#MUD)LoLIH@18Uv7lkS{q6LXv&%uNxV|g{L z;jv2P^A6`seq()8f&s7a)#d={k^O{L)~ud<9NsWoVyJZ*Gm1=}rg{Y9V@y6d(3&5+ z@x@O%L7VxEES$fNud=Y|JX|?qL(^ux;b>9DB+<*YYEO3A`t|9SlSDDMDSOW@t%XUT z0-Out=ACqm(sD*7%xC9+>yTU>$Z1Uhe=$9g(`Y%2n+u8;tozT*&bcF*w@*sXjd$v| zkjIyMU+}XcvR!tP{yxu7m`mDeLttzMEd~i40+=V*&EeR}*|x^p0R5+<;#VYoa6bg% z$}J&j1pB0#mbXur$&cVt+k(7t`7p}x6?t}IbI{h@Ow93@>R5fOAF7-b(#K5J; z3b6ZAhHp1CE>7QQgA&+=*GjB3F7A%ztIG^L$)BSmoX`X<@WwFGzc_q0VhDGKOCU-j z)~{CH9AcK2cIgbmRAarSA4-p{CG8|c%nU*z2!mk;c8HR^L719AOn}|z=YR?@M(H(m zU0D+>#H*Sdf%|0zxiK<^a!(H>!7j#MkLiShk`?!tJBZCenjjtM33y#dAr1{0)5ihq zxqhq0$dqX+^T@T6Hq-kunMx3#iET;|GTRk zVUn^pnzr@w+`i`}P5pMWyR4`oW2GgXxTocr0kG$#_Z|6}(koumAToakj4$MJ*;EH2 z4W0{Ky&OF@ocubi1nLFaJ?qx$zxwv)L9y(rP(x(TR)i3Tokh;!N|(SW#U3mDDs@ty zeG8xbh6rIh&&#yl2XMoA5rYWaD9!h6ZLv~bO~FDMLv-bbb9&oQH6dAI*CXX4Mg1UI z>IRm^%H?dW@t%52G{ZB=dgyzMDzF6JuZX3u3SJxV2al<57CX4a(Pcp5dQ9=VxYEsC z9Cl0=8XI~d=J=*4d&~6C#)of3`X%*840EwoZ{!h$hl+^%EuKR@BS!9!?tU9zH^P0w zS5~j=n-9})B5_o*7EXl59u;NM2WGADZmhapE5tkuGC{I3NP4u8lbvx-$&oZleMhaN ztZrYWvD@;>G@6T1%~}`I&Gb?$rkpYFwLoe3qW3hZY`_Q~BVlzniqZ~N_S?eRA5_GOWYj(i%;!JjRCucOr3 z;n=<9M9hsm&uCjy0y{4JS3R=+x(r^65P^G(N9B1*zzcQb9FNm&j;;v?&|7>@3|(6z z^Nu|_d-KW#{SXaah@6ric-`aQ^E|WvvTP#!>&$u>i1}Tk_SYxWpL=*T*DuW`c+Fpk zNH_WXQA_>DFI=&nz}|a@nEjf`Cls3*w$t3G*$fwHe9@Ef_dgkkg?n58HS6*KtvBiF z^}_FLQMQf}#NzjR*zMA<=xXfBY1=;ImUL$L8COC{PQ3I8*t|Co-)q_*uuX2=Q>hE& zt5W64(92K^Bp(p+QC!%hyy`EIgU13*7rLl9itet8*wO!{6?1A!f1ImsG-9&V5F(2D z%W-f9=-5kp{tcF|1bIIk)ylJCoS2w?-^r`ktG9UgCPVX;(dV$%uNE`}gVvZ^zE zN|ALCvP>Th^NM~A`+`#Y%|D!o3ErSF7aYkjEn~8pnxAZ=bIYi>e&J&I-xl1wVO91d zx*|cnF%!g!Yw}@Rx9TL(}X?%Gqaj zzUkmUpVHv$`?INu$duYaS1RVsD#cyZw!{g`R$k#(J#_KD*0<9!s->nsYR zItDf;x~BIdtJx@5Y5qbNgw3LPc=@;R;Fi*EZ1WmYYVRt2g8<^u>r1~h>`Uo%Gku;K z??AgqldepEy1=2^;S}=KCP0SRK?vo=Es79)hJCZ!uWep*w&M+ahFi2~4=a)1YcCrA zF4^LWRe5!)UU=kSl*gU)5R0FZ!{TQv-lIm_==jGB*9*aMYd{>Q8Wu!_TY9{u1=$LI zxaI;E`6=-e|*A{-&(aTUC;X6x0NHRt9}ew&)95C>08Z7nNUV1F@LzdM z?zPM>!Qix%8gfkdo*=KD$}iNn7PKK_ibQ2TxS=WH8ZL6g5SvWnV`N{QC8lk!@Y`I+4#7v*O-5c+jFrXBM>A)wxziMMB&jz4C;j&z-rlN(l( zfA0qB-qa&p@vRkSGt2gT$f|ZBpHEihLJuzLo)g(jzhzVAkrlR4k6Q(OD0pIvz+7DP_pdrHvn{>3`nz?-)ahkm)tg{(YF+m&(p|w&EzN0 zVE-}S7jfNAp^Q%QSl}0D;lgY1m8S+rFmZ{v+RR3=WXQ{0?p{}d#xv3fUf7scBV{tO zcfXF(_~M3(4Kkiq1~#zqrPJrd$VXCu3mZ?g+F`l+R@Ypmkm7-afASu>Td3~YSm32b zVfV5)852RK#E$d^9sT$;pWD&#NK7NvORuWf3oH3XLaN*9o@yBo;e3pQ2Kt18rhFTK&tb z>buB2Q^4H2N32r7?$;dOUeZYzT-Ido)k4+3N(uOrvfV9*;dv0Ow`gyq$YoQ!zgMeK zNH11Y9yB__LYUN2k64!JyOa&i1j`B-Gu*YBy7h8B9v2l_yL|sk9KWiDR~=UrvwSv% zh?vLQkBz-|qW7-)*U7N3;7S`_Q{vp6dlkkyl4tjLRp%E5#+ohFS{RN*)~(voA1Nf9 z*vW}bJ`zEB5RQ`J&fJZ1M<)PME5WUsBxsRu{YgE!w?RwmOZ9<_?zPHr`1BN7aRC6SeVq*LlfWQUTsix}VVoS?=} z0florE63tG2b82n@-Mc6i$HSKE6;W=^3wSJ8cu#G#l*e-nUjjcG%h<;c;bdk5zT5r z8;PWCY}S2 zcoOZ@To(%8*PMTuk%wg=3PnlE<~eJ=yhB`F8PaCM47SIyLlb@#2WWjcPbl-io6k$-$edua}#8QI+4ma+vV1Z&-=9I{Q=w zIq7&_BxgKQ6}N6fc(5^%1>fbkq{Da{d#P6mxARR1SN!aKUSLUw%f{g?Y{buPhIeg= z5JSYfX;DaG`H6er1UsOLlV$~*H8&ghC8L)FRo}Cfv74#%S++qzJr_IUGmLwh8u!-i zFGhu&q{`WT$3DObN&9rY{krVu_qI{$D0fin9RfjE z)DoRG@!iH*mrysHY!uIlTRdaL9RSah)mnzxqJBxU?6V=RJ^7l0uJ%~NuU;bcd~?{D z*@aVm@wOR!n%jn$KW-*isX{{+ueT(7{j13Jq6}f}|UnH<7?Lu6R8a zx(AnoFheU~x{PxGyG0QiH-wAKr#F-X$MHtAys~|q*{WtrM0(P|-E$JvNV)WN@-cC< zXp#nrU!3Em#bS+Qeh-Lj3gVi|xt#@}Yj-DR_%tlde-yTc)tqJhS&V=Z_9ffmoH|%tSf*#NFAfUkUla)#>fM7mqqc&yCiIr;ve_r?j-;e#y|9k#~Y`^$$Aev zjBB2qaJ%%2V;lb591lSm+>hsvBAF_F>qhyue)18V-n}kwG7_LHE4T}7fy^7q@dWj} z2QXy?FDst`LS+SJhOCAp{JWQ@!2vw;C+CpP!-g%lA`n>xKEx_exzn#@#MpW%lx)1{ zK95B_!;>Jxw_=*fv6x7y5q%Fj0zGcev$v;nv@#c4TTaMME%=E$BWo zqOHJCj$nh@K6aHFtBv8rX=Yb|+w=x=?&*5we>aqqBz3`b=lE!NW`tqW79SeQ4d407 zxX3g6*>b~xP;S&bGEv7jh+Eu%Swov#UuJz##NC;FC^e!;qG=}Z0rtX03O@x8rP)U< zv{^NfHopCG0^EEUaYLn_{umZoB){>7Fugjmi1fHQ5Gk`Q(!jGBwWKp5B?y`28<+$CKwINcO~!U8n6NZP-SwV> z!>gRQ(r-^NUAX?ddb8czzhrR6lNrSVviFVGIEdvo(|u&e9HF%Guw1Z=Z4L?{ZZY!- zLLxZ#X3gU*z-`UR`hTAsv3^qv0F7ktPs#A18k`z|k@>?rG>hm^Deb*ZIp2drVRgD!2Oj3Nn;m^>2kH!k8M z?2r)-X@dAi-(!K;Jg>U3mtn9Sx|X#hN8l9Kd7zYVq%@qgOt-4uQC`e9&}=KtA9X)~ zA(5Ap^k$!Z1Z27DWIxiv1gBF{j=xgF%p6I5{>f1qXT zH<;$JkUHdfm@t-N+^1}isolC2j6jr@4X~TkG#@B#I?g#RqevQ55Iq9TFI2VCA_vS` zJJIt}V_$zc5Pl`?)P1t4E|o(INsYBjyVPcUghymV*MDq_VB3fJDSK=b>0dsEz5k)O zqJwhkSTvM-o_#gF^=5Z#ARp+$SW6XtM4y?N(ChGq1pekkPQVt|pG%Fw;NF+SqRXgv zN9P;}N1(g?E_$XDf+7z3vUk_F6>Aa3y#W2!yE;7zQ^v}fMoUUS3Fs{aEf{Mg-iEaG zF~udu2YMYXrhWDybOcL6l=Jt#=74?JrUX&uB*J;?)tcVtB>2FUX@g%xOVrjR>OX_Sf`Nu=Y5KA5-uvuUCdaobq-nHn3k~tOJAKJzL;bm$U>i) zue|@DQHEj?d6#a7+Ru?o@=3WbyQORQrh>zGM{2n?LwH%t4Q|U2E)Z{x^y7$~9E%qe z0?C6(9l7gngkShQ=lzHL^M6aeMpAZ-a;t^G zD=&a7>g2iq>wG1t(F2xNaf@1!R%MskU`qYCapuqn=etWfp3bmMW@1$OGmoljN?`pb zZEU(c4>(}}?qGFFOp<}V! zIY$ZX5vDMAMmg67(NV&MBwpFj)2D%aw9GIU>5(Ne4r8O!TMC+;b-4u;q@h`%;^H^Y z2|(2_&Mr<^p;2>;ygO7Dnk{a5mO&#UD-@%&@Y$@osSWWi-0R}YNMs{4jFO$h1r1a1Kk-n0-DDa;TgG~ z?A)7Rv!p&Gjy&tPZ!W3k+uDT?yl^`M>cmA)CFk7?l0W#iSO-0%Lto;2P_+**$qq(v}k z|8y}!KF-Es*5nW}OQHi(LOq92J81lnqgsfk@q5fY46}ip`E@Rs5>z)sPTscmp5*35 zl+(Ec{bn=A&L>fuvOsuM7bnOHl+uJF<}}Ot`lEey)GX#VAPf8^A`85StNP&jD~Se% z2H;NbbAThv)+ZkeQU&lb_PjnNTyk}%^ynl@=%d`12SQJy%iBs^BZ;%!JrSzCr`~MsX`Md&>;8GS$PXSp0z(A zVeGAXmJEwLj-havXi&eGu+e)HSkod*$N;$-okppS03M4UlB?`BCae5G2C;^a*aKtJ6v;r;yow%RJ8E(8V#2%k2pCL&MJtWHPY2CzoJ)-L8$e&@6gBnJ* zv)QdtHN#qS>$py=8hqQeCW5|J{(0H|)dKk0ehkl8{#AlINj6W693!a{n5iEWTo0lZ zANl$>H%~r(veKXB@k^21Je!x15{SMB$W8NiUFEL#h*2Ay058~Ur#PnAf9c2d zKgJR3i(Kh4XVK_;VYy=uJx;cPXKI9?)MAjxAS{gkI=en>i(0m(^1&RRMhY^+eau^lN5%u1XK{#?nSb}f&TgJp^4{&p|YNSFS ze1Lz9(lNc#%`*gXG+#Q?^unskv1pVzl*2~Ozo^|~(77f8dlZc)cBY!P7upP-7pSFz z;q>D%t@Lkf!8S877&TiP1=_1&7eXrskI>xes9%` zkTGm*^v)kw`XsM>hfDTzA7F}&m#~JOadUo4{%RR5NHD}Dw-=!=N{`-iw?-hR+`gKv ztl*M=%re|Ad?7tjNWCwzl|S()*Y5_YMEW9B3HM=a9#{ezam2I{Hy+sK7J;r^(K|1H zu!7qjV37&xv*vHSIbzY&;BJ`;Y=1{Jijw!6zY@`1xHlrsGRWObm46G4g4K?sFhQe+ z{Pu=e+9yh#7pGJE_dZO z?p6fB-w1V)z2$mC%M-&o>EH9ZbU=w=U%A2Y9p@O?DILhe|J;)xK>tjet|uKN=>M4Y z=Ge}@r+aAB#JET7GVm_^BZ$ZjRq{98ENVR+h5I#D0i9Z_iF-mQ2lu)Y zh~4K)K#8H;vnvX0HWcG(7M<(3C~q!!i)&Tha6Eu4b)Z(g&?+;xZxy7lwCC3$y|r%@ zOx-y9#T6(7jV@*n8-V_5Ajc~p#W0@P+BNaiPeLG>x31FUne9=}LZC!rj@IYS9owO| zz`M{+a97}jX4bC2kys+~KtK}&1IvGIr}1CTa)JwG`JhA`VRcu`Lfm-$=4A;gsQc@C z=&XEu_b6vR!lIVVtUBR3fr3V_&sEs(-pB9&q}&NP#2v|=x#vmTZ$ivH=;oD8;nY(8 zG*hrl!XNyJHE9wqpOA}=ytsME86%i7TQq**tOAFRVAwj{3aCP+jn6rtwmxhQCo|N4 z*2D;5y9W3)I1IhVy6?Q$y$n#}9<3}Qc(7nSH>XO->rpv|v)pPWucTTK(ik0tISL78 zOoRM^SlkqaO%jFtN|le+-Ay;(Hjbp2KH-B+_w!@^6eq-np}A)-EX2L;M{=&%jQ;GzWiZVy^(bWjRN#fV~But&>yWP#U%qSt#J5iloirZs!xV{bL8<7`i3t+wQj zXEXn0RQ%0N0@$6K|D+)|`{j*6TYwGgb?xhSfEoL?nJgmMBpT)L(;UZ1ee&gvw8KZF z*M%{G1xv39LD(73Zssle)=aQNGNSwf4KCwHtdvh&2KPZb6C)SC3@2aMrSeZXwV<)r z^WbmH87s%(?Z1y9h;3IIod9vy$srnfv`s~Up(&~)Uc$8|Z}<|nx4T$g;;x*6DhPuDoEdoNElnVlS9*)t>LPLC0r z_ve2bdM6!4&2PjrK2hpLKJy5G1~3%d83c=Zl5!9cyp)wLHFnr@=e-c1qj9Gk-vQ9G z^@j3hu3aShZ(p#dsYeY3DWL|a(yoYFfS5H8tubj zO4sNTC{rR6N*19wd=&TQRY@imXp+xyOMvX@88y;rPavWGU>Wel4Td`?o5a=GYj#*$ zy?d^e3-T;pr!oXeBIXAYNYuK!*2-EVP2V8)q${KviTun|@Xo(1*k2LxDCA(qbM1UbG2;7wb&m&iIuOFd%X$w22=!et9p6JeY6b z8FRmVgqt(|T$CaMa-& zwalVEwRnk?MyT$gV0Vi=Qc});Hsmr<@$O}!``vrX{`Du~MEm9+i{PTjf0y$fUxI}e zxXZuMx5^ETd#0?QZ?@z= zTeGB&2G@d%lu2t$dz5LcQDIJ%3kMjsX+5XwgZ2Wjdk% zlQlogNGVCEhkgcSPWT?ms})Go_(K7A6ex#^-9BLN?ZF8y22}Vud3v0YrBoR0e`*FM zNtX?RWJPO3x&oN}#YKNzgN)O)##3Anzs*YQ`ZR<)*yv1-0^h+9UqDrwIIU$2uci1#E^o~8F(v$klOYmIRqqUU(j`Qj z((^L-M>(rH^Em5I8L2Nzf(@KLjXHlV8zP6L8pPjLm51q8>(lcsC;$1BJ~%Im1aO5Y zE4&}7c4Zx;`pGy<`b6s!tuek3C!v__u`83{Kq=W3K3V4IRzHb;S*SdDYaxZ=!R2q& zcXdZMsSp#vs*SQ*<^i;%b!1my>$re&(*npb*H&JDEP^=tMUZjvjxKZ%>>4;E3+|G1 zply!2=y#l|M`=tU(Lz0qx);{9`{mg7Ww#XmJ~Wb9)*1*N}t- zlKHU@ze=g~JEVtNgZ|PrK>Zju--9-{dDV}1YvZKUPr{rMiJ66`GVFyhiB1?RA`_x# z=U7m3GDr+Nd#@C*2X52QrrxzAk3?%?j=-!6>V`iVfbvPYAgVx1sMYvWn~V{*N03?4 zbuhvO%BSS^wWC0~<5iUD+caEou!ui=lF%k~gpbRt9oUJ*zeEV)-i%K|zUD zCybGYQ~eae0`~fuBs&7(7s17%4R&FXTFNw53X9Mq#50kM5mxuKv$^>uU^>sspekDk zUC+3fYJNEf5}ye|qj1Pq#_M(kFgv)COo}*$7SsIFM;+xvSTUjs&Md8KP0iI>K8OLI zN1+<$Y;E~}t|CtbnR3F|7-cXTBD(Z`!@O(Eynw=<@f} z-4itKGc8FJofs(xlty^cgefv1`6n}<#&zEpJ5Gz{o=EKWvlJD-n*y>KCs4mjAiJL$ zg05%S=&yY}upW#$`HPq2ZhU+Qo8=p8P?uiTKv^8)4djNk_aNO@KVtUYH&cOj*GZ46bf6hG1q$;| zFywbPXrIPIqG8#ByLGLYu?ddjAmVI`$fh=GEkfzxZG--66rpM3D+_vr0w4wu(NZUd z0g12oB9@cb=nFMSE)?oV&Zg?ZnIT??m9!1<$rMG9L$d~mfFrILQ~S5+n5K~&+?cn3 zJIXy8ZLVlR*M4T_-wgv7+3hCD6kOKUBT5p=!BA%WIO0M*%vXq|#p#Q8Dp+S&D{DN#`bpIObRm@Bc9t1|RIKyLRe^#p zcWAcky-q$36@&(#HzU@UnH0g1OT#5WONU42dqzAN8jx2#o~}b}dg1sipXZNe1O=@) zv?8V?*Q#2up98*laa>)B&{)!$n4$%;fe4T!wxo8yQ?tXfxskNPZv}!_>z9i^!L`vZ zun*tn`=6l2(OuvVjhGjc%aRp~gLJ&i5QVcYis!|@(+cx8b_^R&reG<`38kN=7>~E9j)2$knYv0Mh8w^N zPuBrPT`ah(Z$~rUzU4mpRS#UOE$w`Z0O9>qq8YuGMO`FyOvv`ohi+k(HGS{7UITIGw(v+a(&_etc8P?Hg;WuEQO4 z+_Kq`;zQ#G^U&c=PQ|~5%2AZ_m32gs4_^&RC+5!&f@waE4PaUdLrn^qsWfYfqRo}%lfG97uEXgKFmI`= z9dANZ-u-*VR?)<%`uPV>FAjVSfrksSg!H{Wdyr{TlcC2ZHM=pWhpdgY(_a*h4mGCvfM zbc*qJGL=8|AHW(Tp9b82XPnxCDM8Zn+4k$= z;^rbM7x?3=_F48R5g9KB3LvX`-m)FubB+`J^=O#{ER9G7*A@hPOimY^Jg{__$4Fz; zTwex+6WKDVmc±v4@_E^#F=3|efRp8O4eYy%Xcq>A{1SjbjMICSDBWCQ%;`G5W4 zXn^}f{ma2Uogxk7zG*yywtx(a#oxP2WFpQKU8)72OZFDnlATOH=#2R2)Y2ihBA>C& zKuo+irm^2TBp@rYQ4Wa|!=KC45WL`a1hvMB{$AFVmbR<7L2%*6J1zMyZAOPt_0(b| zZ%ddH4kl8O*YyTbp9;#?Qif^uqzRW$wbM7W(ETQW97ciquT<`SSX-@sM^WWEb+V>w z5_gPDo!4>L_0ywcUk7Jw--$B4GF(}wkp4AFay+t({jR?RQIJ^lYr8mb{qi&#t&i_8 zWQfRHC-GL#`~TSc>aeJ`?%|=tD`g-Z27+8rK%~n65$PJbq@;7`7z|J;9l8ZXUQb@Jjt}|pfUsyQS~<+`Z};n*eDS8EI=_p5sC{yTP;h5-hF=I zW6%rK2h!DAizEfJFTSfyZOT6Pa_;C;UAv=y1n6c`qkK6W>A5|-I;v_#{e(K{r;S@z zma~DKq--0DE6c!}H7|km3^Z7?gYOqmK=}M)o0!b~*)G(Jk2ZG< zw#4%0J7NBD1B>Yq-WzX60I*Iz9`Auj|7%-q=94=TK5D&41qU$j+6C=f-9OG z7mO`00f(JLB)pKb(mH!C^rrc_04ajH_ue$H^#!6F3gFav=qwLQC)?=MD~sWiz_$ju z^;fU`jf}9jn>SfJDx4JLl`q|LaAUUhvbG7j?;Tl zA*6xq^GYz??yC=zzP5!*ZCw!jXn=I$CgWDB9&3N_6x}ZOv?J7N-(S*p-_LQ1PMWy8 z=JwOen;okwDhqVdm?ufIS00y$-6?83i}5pk?s6OW^1=eCey1>K_5}Fh7tda2Y2MyM zX%$?~N6cVQ=#k&ef>{r`wViEAr&=_tM~M@8%j(Lj9#5hdHS|0-`xT`5H}VPBTXhOtJvFyYz9v}#WDAJ4sCU#*Xk8tF!drk8jW3ZQ1lNhfI9Sw*6KJ1b zc+41pCfiaQtU+yAb@bi-y^O$!1Znx@qjp9gZU4z~5~6$XaRXy-V~%lZ@(jCoCP0j2 z05t{Z(@L!CmuGCXCx>4@vXw)-&^un>dwmCkZT}{ihGdp|G2?~<{= zI1rD$ujt0-lFtY7b+aQMYVLo@486RZjXcQ|)Hl~XWjd~JtZqy)u8i^q z=BTtnP)8a~QVQf!-J@B`I%Fq|0U-!E&hn*2Jh|SLsx^{9>Av; zRg5`}xh~y*=-Tw|cWZ9P)2kHY2OnzQx?Mg~SaNRF4bU~8K;t&3qlpe9rF1A!fCG?2 z(Fa+l$*0OcTfli3Tezgr>o|UAvc{BiocEHeruF#|=#z7yzLIBJe=mvXLS=spU3vno zH6N10x*7W#D_^41vz9psSOWGq`%Z1ahNSV{#|IHQFP`i{NkU2v!m-2q#2~DHD3LwL zjQ~ihry34kSzEGYa%9fu&*#mjb#$3pL3?h2j0oLGMmdLrf1c={ciy(^6}S@dziF@G zx`@x{kx&3714lR$UZw%It(DjW`}>LyNVU(E*ScQ*ZJ5MXn)tRP5FK>8d1jmIjEbi^ z-sTBKF`#tHyWFC_UJ?9cbYh$Ngbtw(;nx#x#59)#pVvj5l!(@C>w}`thKLaeOS~^v zGYA14p706V7Wd4z6;s4r$S27^zj}Yd@U8wKQI68n6iV)4dCD@75%DI;iSx@cuy>85 z0PDbj6RovcO4WZeeeV5c?@R}0?Qi<0LVh2J{C)o1_?__u)WeSlA8eo#fJ0M06wt;X zo069F_t!)Iob>Spa7H{S1yTjr^pC*jd&ZwQosfD+*?%H7{3PMt8OJlc<@z=yf~U7> z)&zXB9eMbz#T`ZR3C>*o^57yT1Sh^WIC#nf?#Qz=?Q86>D@0t( zrV8pJiUj2*%{yt2ijw{u)zeI=baT0l?*66QKGBA9Pv7Q|CK3RVgmh$8&`Gbc=O_JQ z2HPH8+IRIFr@eJlxO8(w`Vy0qI&es80g&49g?xut_`Ej#_V23>#%5h%>kPjE5dso! zg<&Fz@RP42MNLlwDbGatARv~--kE2nexC2tHm#Fy*?Fg=zfbz_%%AONK|;eb@{t^P z-%Gvc_czBBd%5OBj@VV)hmB-GIjqqN8y437lW^t1zRO(J<=U{;tPiY zATKCF{qNXr57#`BP1=%NJ?&^Tf3I>}<`OaF+T=nlBR)`>Bt-7<^rX18OnQJkW8*`V z(#OC#qj!>0j7d21Ro3x)la|>@cP?IeuCkJQHq_TT%_&3FJI9IX!PE6+5+9N|ox*G@ zI>N8)gsRD0!@(No>jR{SGC@_q>pXpt5P`Ghz=k&S^MyA9 zr#7QLBq&riI?*njKb2j4?@V6q5lHF;_%hP8^umWI3}>G%Sec0SX4rdRN%sYkTQ2V z5qPzhi7-dEI6a$@P+yriWaL47XD)3((YcVlJCFGmj0qj(rvR5c{N(DMPt!X|bHdfL ztEeN>m1R>L_ktMIf2f;wQ0?vA)K8oDxzt_l?Ug*cDq^i>XBFPTUgev`+S$jqDP9{W zT%>X-d}gN;eMR?n`s?SvDLt$BR6#q!w$9!@p(1U_)EaY&CQIef{17K*F?=ELNye>? zd0&aX1cl3yf8+wVBx5$@km5@sZFysRk{FSW1AV(?hcf&7>r3xm?5^S1>SCsq)m}ax zg-vVHBNt|uI5GPzBU8Y8m;#D7g}2`BFZvq6Fl?T8VSNxA-+FyAh&yaSt(ovpV8%wQ z*k+P$OudImt#`N)8{z7O_&BUBNBlw4^Qz2 zjYM&J)B6tTnOIySRmTWS0rzCP?#fB_<_gFerl>`4SF&Xa$(J}%y<1c_NGfdPL+(|Q zkCs*94%yDh8AhwcZ^yG)W>Uk#9?PmfSe8mzXV`2jbpriVpvl)ZVedbA@W`Anj%_xC zu5AkC5}^p0@2_{U#jx+pu+_a*yZU9fp{gvC`jX%ErWmyc%r=!sy{{|Ya-&1JO>A}1 zYWf@NY?+k2^|^BPO~ZOd7-Lt<)-3IkE4NW z!Y~|XpQ?a&-{LYTA8>|SnZws4IgfS1;HYYd4@Rv0@*}&qWQ!ixPU#+Xc^>KPXu{2w* z0OrRsDUE<`xGF#3oW!Va_W1P(QvQT(kV@A>9=YLH)7xy`x}+xMSLKMIeytYE7A0Kj zr_PNXe>c|LEoC7fx zM+Kkx%4GcY?oon+5m0cOdlkfUYj1|L&xl8E0OMgeyD5^KAi9zPb8g#w(z@g{99`Jq z`p+D%mxE(2hqs^qgn*4g51>-4Fxpkvt@xHSrk%96cAUEy9D~Watr3a3|4!2r(KTof`{LO*PmyXr(|s$U`cYzikxG+ zWA-Djr)SGKapi-rr;REh<(hI~@`db8FC*_H#Ic3HjZ|L@UrBi8CEOGPJUkC*nDWTH z_t;3r0jbD!@zE5u2XemN>JPN-#Xn%!)n1%O-d z@&${Mry|J-2k!@-tBsXod2*cM66xNl~EVms-!;j6|&`Q$u$W zl$_ch(NFqc~?A&vEvd$*mi2ZL2gVqif`rm7I;X=4zlsK$kXZ56Z!rk&etUYBcNp01{IpouxDMTKj74CAJm( zUl}ql($p_?MJB+;Mnq4(Vyn2&RK(Sqr0DGQtOBL(eG@5V!sp4RY`~oyS@;IR=}m)3 zSuMEB`OH9WpsF98J6}%<;yo=^iId8Gg1S`7)6`dmbM1Y%&l=p=EUV>KXE^HXC+hNP zwMuQ1y;_#=C$K@uP0_ssUXHp8Aad3#FF>8`mq!RebXxMTYQ&dC9=3{_FSZ~$(!ENE z^1=lW=o6pzu zviG7_eg*^-7!H#LZoY}ijhY>enjZVT?ZnUH;P1@U&jKLd_`UIVcqAKsO4s%kV0{|# zzD}^X2Sq=V=D3$#-rr{)Cq!^tF+ZM21Ly++J(>vTY8y$IF8pDj?7{SZ-DK=uE}N6-*U2^FT6&-X z^tOj5%qZ*T-a4BU{3hdn9_Niif-W%5OE!)&%J-`(-=_89*?CLSjvQD^PmQ*$t+{wC~6wn7K^B^fc$K)LS?K?7eGXB*j{tn9Ydh~wUo`%G-IG!PE!9@ zIt+CoX;2ge3aRH?Cyy>U?v&e0FG2y(=H{@fKO~awgny~+Kw+IVGhvqHOmmc&-cKwX z*dR=O(*6-Mbn_dQ8y_0L%D{gsHmBb*5gS1^9P&U|^tE6|M_Bq^C9XNbQ>xP3{&&b*UYzL#AMN zHj&ydiwUq{m0wIXeZHK{9gutK;u``5q}8ukQJ$^|;y(8`*}q+K*M$&jZqBIdRG+e9n!^_Dd|3JQEp+vVS@i2yb^fjUfh_8SZSt&hKc^lY5VB_i)HXXXj|X5Reu=xKb@f6?0;Hm4BIV>9Y+F#Yu-%aVzV zsBtZJ{@WV$*Mq-Z{V!`_KK2qYsKA69{vB~eIS}ZHXnDrGE{&79d zX?MTy@=`FlO)k}zJNzF{fBXrxTyJLE>#pt0UxC=cq9l?zeti0m>+!4AB259sJc5fh zzSo-MYLYMhqr#6r$FpiKHYFGH2#fbZd|~IjC53)``j2arcnw<D!2J}}te=@>;XrODDG`$kBs95Fvu?ki#)WjJT{%_#Z zL4i;xe^X|J_^aJd&&M9Dm&vuu6`Fw>8&9r>{>wyo_t|09mzs3s+7;Hnut~+4eImph zTy3(GvsYM0v-MpW@LS}gcmG$Zp~-x~mU(3$*6c1UA|mI3^PQ~FK7YN-r zTW)#(HLw3({TBoO#lU|t@c%6a#*YT7wme!E9*-X7(c9OX(_KGx_0lATiYoE@^}u%z zGu*l&vN^7)7e!=%>mnczm`j4AbmMhAbA?&jyrawRX3;ZTji-BaOJCzwMYEzYG|u;M z`???hrIfOA2R2Hum1z*@o#m&O=asV~*_1jqW@l#&dEHJL!;|%Xwq1<} zv_*gu(0^Twt-eJ*epRED>|bp^RQ3Pc=KXFzARXo|1DWr?2-Pe0FKO$)SZ<)DNgK{g zST5s^C}kD<$sObV7O*;=In>H~4nZS(k;HZB?lChQW(f-EX2in?BtIQ;~gip`Q zX1>VtEcKg)Ytd3A{E-W=^zoeX;#_+8G69tbN<5SJo!oKI;upf<7?kMJF5X{{&4f@x~1rGz?vG$$iPnq9HIr3o?w2j^M$xL*9VAGp3zZUMl--97s zru#QJMp^9jmerzzr1mGS*L?jF*S=TL;foZo>2;lcE!*yDXx#Fz9BSV;Jgncy>hF_s z_%(AV-=zL3z2w>p<>Ccom0Ra+($E`Hj(gy+fAjdX`;h?kM{!7Z$gopmDEO6jiAjB+ z!>FtG@78?Zs^}Sm~Gm{`X9?WRSt`|+eyI|KJ-?}1w+U8B#5z%R*jLF>+o4&&j zfR14;1{C}l5a3G7Z)EWnKa%0`>Z(*+_PUQ-v%XEuHflCf2|?HS3E}C(r8^SM3^sN`lj7fh8~)9!w|nvl zA>HtbxQyc!fv(X(gC}mCX$w&+I0LNj(dnc&4zAHAsLT_klSwXa!C8>7c2$}h;V?1yiXy!Ke)@THP%G3x_m?&51GXhh^ zeQ=`6YX1j_=5<&rv~rp7gEM6x?a}odQ|x1QSLMZPcNHKHAtS93e@zDj+L-N5IdsJU zO01u6w*COV1y;`K4NoMAzVc6{kq_C-O4g7-?7p_1DR;84-oiRXs6tF3&N!ZkwF>N@ zQ`(RH>R}g)I+nMm!db-{e7x@9WYG?x%*4@`f9iDytP!4PsK#ty$8VoVVmgFfZSt@x zL5kMy&Y30h8Q%PXiN;HF^vaO_`;Gp#_V)IRS2vdY!I@ePtND0< z5XxYw=*J(23h(0ju{er^-x4(s6mio{m>)WZl1$=t@JlWvomfNhzB#VmGzdClF@#^* zxp~Dj-4UDFn^|s%UIlx1+j5Y1rrx%DjFt;p6CH{!iP*c2yq3EZL+}<-I0cp)bkVzcM3!bVtM3uh@{LZ7*CBXFEPHoM9+>5B4|aTD0rXaPK|xACy{ zEU8OWi&lF%LbOZz?(2XRW=9cTw{UM0d=Fm+QmoafC29Mnie962BXVzD(WFY8T*JUq zk|yyeD{YW~Uq+A$h;xX(9rSO#bq;|b;+Q&9wz{*y*c+sjSF7P$AX+kUN&FZLK>SPn zioNV0M+Hx~iK;X^;zEJ}Mt49cvG7wL*Wl$B16wLil#kh(=o_6nzC|{eotZr7iAfT2 z3(*nD@v_8y4fNd%;KWNJw}7`+cwU(GBO@utVPQQpMaV}paewDAGzH^iiZFdj?sfVr z%=y|9@Chd;T|tGlexSz$QyIvk`i+zzO{%<2W`VBWA@>_BMNPha8b_*0Ju30rpRldf zv41dew=mDfJo(Do;GuU=?H+woZfx<>wD2mp=>aq_j7G;1<; zn+l#d2D>2)HJJ68C7fyg00Q`{eM3on?PI~^5OZ^g0z?|p4k*#4W74tPz%4a9aw(Zb zTJ98hCBj>K>W0%k*6bpA`XCS%PoV0PI=y?E#FaEDj{3iIPO-|m4xEzNy(hq!{v+|)-kUQPWMjX4L8T{5AfwB2m1JEC5;y-*`3 zJDH!}2!06na)-3VrUIf!;Ne-)EzatK+)eV7WJjr$ReIgVrCHYjngz0Dvwg6&j@>{H z0rEM<_+2_dpw@oC96|(zF#qVu8I#8t`FR4{u@n7#G%%^$S~XvbX{LPO8}R`9!-6#P zvROpjUR_yS4QyL6GOEe+GKFCAvo;pzL2kxkOzrQ>T|&(c*6u#TYNAJ{?&(Cc2+}(| z>fc^lLHN`EiEo85B47>K`^VVaW7`cv4Pp76wt6*nqG_U*)7<%HIZS}dehcD=E5N#N z-cO7&Kmao@+O0z?PY>g>x?)3@;Ix9(b9+LZ_nC&USRHf$S_^K3aQjP$6dD6Kfj1of zDs%fA9jsy>?i5=B@((X~f>JA3>L&m}&FwJQE%;bgDXU0}b4i-lzF8vpzzMgE4dlCv zUsU9Kr{F>W5t*OreSH&{*TJgQsF9ba^w&tO;d|f|`!yOiLmdM(13d$wij{S1B(PvD zJab37C6LojADc1XAt{^tK*qxazyXoQ&0=W*!&Dxh7-h_@3?J24l5#tJ=)Z+HM0#oB z2C+*Nv>o`i!>U_ZhFTKl(@eI>%m6BqwS0?Hx!3qjPkdKmP_c1CK*bmU<|`vIO9|ON zY?i2Q(4!JoZqF4~0UxXxEUUCB8JnK+7K*$#nmIThcH&3#nI?q`S^jYGB-wxqoCoYL z;C5Pj5pS**mD=DkDH3vuc$bV0WN_<&fxAK)W6Z}C&DYM&N*+^rRo?Phr{9?i!J*us z1(!mM8jSEB%OHNb{eyS{z^_gkb9iQ=jHoa)@4GTB`6`06I0o zNgIL^|Iv;B(ZnKFB1*mf7((S8et! za2*N5b?&(5*w+z5cM*0L$$u;!_&LPBq+k@x(XR750EDA50Vyw1E$R>O=%ZXbkLGjd z2ch$Y*ibYP;xb&y z)&y{KqRAMqPx{N@UIl*nn5ngrQ!8<-KJkO&i!qD7{{hacMX+NO?ag@VTtIIAG2dQs zP^_2fl#*~ePIG)`AA`+997%rrg>R;_myLaPx!vCo$AQ*EEZl45W4)exXmSI5lbS;k za%1`XC{UdpjS~B27k$wg!06)EqSf#hpAiX5SzjZt@9-BqXsqmh=i>wP*8tAdE%8Ic zH4WTH9@b5pu(N<`Qg>NRNdrCs@(Lq?wwJKC(H-E9oK04~<8(mO&4X?;0I%CVhzXn>G-FX7Z)NAp zxDA-oUU`wQ62pkh@M!G6ZCs#(56T%g?Y_otQy-^Sa+69K9_XFI=e*$ybgiaf(51n} zKI^bjwA{-@zUabQbfW%2Uf@Cy3t+lJKK;oW$JK{QIwS$*WK!Vi0+|MsaKQjx38P?N z$KuhmS_NfO%G7Nc0W9piALSNU*xh9WO_>Bi1R7#iNxh+q`d zs&;{7HZ^*K_cXTP%gKE+c));SOyig1H5h`~&jU2wZ~V#b7vm6zb~pUey}WSp=(~;N zKPL7K7anX5<*6%)m@m==H&LSZ5cP2Rd+mh5fo<9Z(4EamYzO?jDQN|>mgbS6ulps5g?*;YPX*_^*^M$fc8W6`VNatqX(_Df398J)My?H1-{eL9yqoJ9nlDA!&+V6LKDTg zG5AVo>`VhTr>VbZlj7M6Sq&)-iOLBXhwm^Lw@74Gj)lw)svLA+as2TP1-lP$Wat%* zI`DyW$zE*0_j>q0&_TPhkxGoj%7BjLh);sFePb881yOPt1C_y>@Uq<&AF@{xzs$L~t|`wXBn3(M|x-a$;xp8=(s z9);$kNVYz10F(dF(7(-SJ{8JTd&3o*RM*Gei_jRpoHewMd$_mKYzHL zY^$RcjtiZNCe@RC79b?Jza@$EGRD<$Y~hupLVYQE56((2(%1**5TYjbbDriVd+)EA zhorQ<8oGbK#IHvhKNL`S!;BK&Gi7QXjgimlAeE3Tsx|qQ52v}mJw5(EKWDL^kws#~ zv%@B@W)qLHuOF~+fdBvO0p3X!SnkQ6-;LE=_SYLJ+eJig&8}o>bXZRv1urxnCn#{= zc7P`fPwvNP)fq};?PAWjaBH|wWW1Kr8A0rJW8tdjzAJXrqOH1wd9zDSUb0?p!n%0N zNFd0uK*O#4C2~VL$k8#OvAG>fH(~2f`sd z(u8l%9@mUzi5No2D*j zXUIzJcA3iR*iynr1py!D5njpaO**0FWn8z{)?ycU*R1N!9K&#BEG{Yw6cc3Zd;qja>jT% z`XZo-`$E29gUJm(s+G%PvMdA+&uXVPSE6p+@KBzXNh@y0Qwo$cXT zc?2thGS5)>A4DxJWyaF`Kt$-^`alRM^tLwc&$yFzMCq>@&OStvcLGxz2^zS?w!ZJJi;G{ zvkdqwAb7Ma|1yT~*ock`*Obyv5^#GZ^RN+Et*!%x)U#W7NfGgTmzv}o-JZjunsj5z zGp4Wzk&Xc3uiv>LHW{t*Mo3-@ECsGHZ`imFgwOcUw<(^*GB*5~=%csv9w0&^oDm?x zU#JJPpSv!^3^Lq*o8|=(q71Js9^2vLecT@2V1K?T5#?MKJp*B2%qJCC2gH#c;NO18 zvI2U5J~JD`hVmzGzycqCjIa>rfp`LXMU1ieEETKJALg=c!5)BbqjxY2IISWB9hcil!C;N9r57C39JU!B+dXC0a+ zDRx(*d=>!NRlxJg&_<>83Vpk~1rD%6m4-+tNQilI4&P2Zz5`&*ct=;YCU=6cMrTI} z2$~ktw>MGkKE!omg%DbT`qF<$Zt(k>OrQNcg`I6)3IyT+?#n2sHsTvmpjRIVMhtm` z?z~ITMd#mMddH|^(3PJ(=*}ou-0!Ndo22e5#yCVc=77Qaq2B{MP*=2N=ixJ$x#B-O zpx+0^fXA|GMd&eLBRdb)$z8Endgvz;^ZHTJJxzD3DbMA1n3@C{{+z`3uH+_ z@kG2}GREF(+52|=3ZuCh%4Z-ISV+dP0q7byun+L41&PttN~&$z9-Q7p3^4f?=ya|6 zIvxp1232}P$^ysS^<0uVN+>##+Ea%8YJk9DMR$d}Fdw&p4FgLGRW7B~_JH`C35?SW z4HITEFr~(Yqs%B*DXTrUF$Yvs>piKy%N;Diax+wh|>+VW-W_1t>o zVOafcBGJ1qu(5ei8{}?Rzw#17VKlV?d~WPWslIQokp93O#auAY(0Mzez@O1bb}U{Z zAkWTYWWQPvNs!c=PwBeMtkkMje79cYAe931cFSJ4Y|MYXq75*Jw)z#Pz6O?rM-)j? zfLfYD(GFUcS`O9=qqLQY)diy2CW_R@TLvh*omA`qf8t641>=;zcc9C4OBV`drf|e) z_v#62Jac&hf#h_c!WZls=Y9RFy;}ZizQwyk*o*1!zUy0Pn@brd>(PR`q@-=A9i!j_ z31DBbcu&wavCgQz<0G={P&w1Wpx(kp$nJxeMg0lNqb?O*B5vO;xZm5|3GDO0hv9bi z8%DMj%p8pp+cc8y5L`x}Vu^Cjp#h{kEZ<>}4H zz>SeS^Lhy0lwy8R6cOnX3(=bJpVNCz@j$xi)VBmGdxH$JRzh$Aq5vArrgk$*Hu+KVY#x6 zC8Ai!0+DrvQG^GF=dp~aOcz}-FDNrh2*hl<+Xz!tHc?R6RI=tts`~Cwwik8g*b0w% z>jOd24HO!H=RCy~Q+K;YG zvu+(CQO6th%aeYN6rUI9TRL6_Hclq=O0o>BYWvDAsYw{$5QHe!dkaa9AMqoDHD- z9c-^2+iKAs8>5dh}<&al8PMlfJEySrtN$1uu%w3-MG| zAWd9wJVLf#z4RfYw&?_9onaK1X65YU&aZv>f+a4v{-*6Gfoji>)DK3cO&bcdaO!Af zp-A)g#(UH5Y%LM&Q(KIidm_lx{G}8Gq9OPMe}N6I7@OU8h;iz`zKB?pMQbq#lKVN} zjQCp#9Oc29%E;s{M@lh7W8)G-GZAZ-dRm#CR#aRX_9y5hOH5^IDG=}4d-hsPXizKo?|q;t2SDU zE48}%#P?2p-~Q)@ft5ogyM5onZ27_M63K#8-sSayd!}&}E{uK9FzaD~H)3ULB6A4F zQPQ$`hF2SpGi-(eeUp;AR%^p_4zG`9r#@eMjh1KJq&vD*zuSnhj3ola)+h^!bL`)T z9037cGg4reB^mJG=oMZN_DjUX{jQ*HvW9PB?1F6kr z(N-kZXK&lxVv_CRZ%QXm4>RZ!{zm-UGI;p$6>6Smle*(tIptOI!PGI6x`|&1Kb4dd z!%ll2m3LJ0KmGjKb92x;WzWR`bLy#EuI2P%k$EmAkhD`|9Z{F&`gQC&K$#~(stEJNuqiNe-pQU;GA071urT>d!mUZr zb8gq$b!1{hcb|J2f$#EYU@YeR<_zY9oq2dAO$W4H~TyceZ(C(R{bL}qhIJzi+0;bGR$0-(k78I8)D-dJu=#eToBV~ z-sJ<748hcO;>FNpo3E5~btSW;VKOPfLg|*5IyiE(W@x1Qi1dhxt{^j1NsW`57f8qT z$8TEi^{-n}U(&JjPV;6cv3B}eRY!?cREa4~a>p0__DG{>}fk$P_R_gahO zXnWMXM-(XR^wfsw1B2Zel2lU;rI{KHvg*+$kPxwbp%>BH7yG#*lq7JDpYZ4?&}yuIX+LTm+;{Xtlv z5EkV_!Wq#t$Y(B1LwVJt(`{+m`RjvZBy6LE`jq0Q6z*$*H4knru8fq-?V0D6QRHiO z_FKO8y^m5r(GX05r}z=7vOOOiS*c^=wIC>ls6MW%!fTsv&qwtQ&uy7-tTR*di5NVx zjy}5R=r^O;fO=kvm1}jH=X-e`R?f{g7Q_V?H*_#2J_V1>Y&O+WXSSxPmRzMib-Rw= zN@l4{P?V!R*#6^w`#mvX2thBq%ju21P0grAJ^B$IgJ7R!R{L9+fjXIkRZ4=<`?_6` zxfF}?eswXSUT>zFM^VpEO(;t_t_P-9Pklv{9uPIOR*9(cPP)x`x~t-5V>_s`Zn`x8$8AD zcSn1k#l{^Moh;F>uWlkJXfp|7%x=bLHg=!_9zB@>zHs%O)4uT=KfeuVbX2t@-uXirHOD4G6B_)Li$cF$Nd|6;jvI@ zC?nM2NrRHP>i7-UR?+y$6KsNg1OjM31`k3K|LF>=67!V+R6)xsz0j2em6}5V1c+bD zDyLfKdF-3FG51;Zd(LT-3R8N}ZBu%j-R95dD>?s4IC~ithLVDosS=gb!@wHMm~)KV z!s3pLg^XuzE=#*EStyDURHeR&MtpX^HTm|$8uf&*y#nmm4+2B#q0Y`;7e}PS?N+Hg8y>RtPy%7lCuL<9_RB8adiMDWNV( zJTjImQxkBvOEkzkV2RWI*5q^#k$hS*0~IJGeNQ~$bzTQkON>m{ZpslMsR|ynwt(c7 z>Ts{q=IG$T!AvqQ7T}CbksYf0K<3Ig_c%%O0r^O4YfiMHw2e~PiF+B>u9Q{2S$K)e zCgW>?-8~DuBx(DkI8yP9+{zDvJXbnJ~{O6J?^AHVB|54I<`Bf9j%BSs`R5Vcj% zPXVOrv=0G(`JsT%jD+Oyf3h;O+SybdHy^j{Su_bNo|y()8k3Fl_4wq^Ddt}%XR2b{ zH7M*Iy5hJ3u6^*~fS^DE*eT}(b*1(_ zt`ngNUcM{MSKqE(A9tj7oj+k1AtYo?xOM$h((lgNlVVqiFEzhIC`RjPkqf(4*Yp z*;}c}D`_<$65Y|=*BL>1M5OAqQ!nE(3W!e``!dkQmq(Y*J%5~KL;#l8Z_!1m&l!Gp z21pBhOezB#6CPNHe{Vh?fg?d3i4*}_tK zDkfs4l*(6$@s!ur=R#O|l#O=wDx1grgZw%O{6Gx?dziegt-jVAUC}g>As0=glb<<^>{Z!+p1zm9`w;%5)NC=B`V14HVR&81gZ~b6oy?`-v0Y zuF1Rv{4K%P=ZYY1Uvf{GUrYNP#3Q` zctUBSV8!`uR^1_16FNu3&VF+N#(bsDex6k0*J%l7glYPjbD!j$Z%J+N?gNp&5eNrY z8D3f5Ht6MTr&qmiC}5{l@xhXjf}BC<#&FRUQFXl(Msz2Ufch9CNkr1q%@o=;5tP?$ zQ};WVY7XL1Bk*OyMoBKm#k4Hh_Y1P>NZzL>#@*+d+lES{M;o`cy{&$`>_*viq24)TG8bE^I8+ zBZi2E5D4a0OA;Xouip)b1M5|$dhgcBc1SlPxv+P|$$dv?C%kWYc0$A28dEVv=Bh!< zRe1qnWy|Jfc;ka7O+a0Uh>Hz*C4BSF!ia_b+;(0`Kicu%E+NS+X$5uefkVtr3lrmAi zFz-q2Q*+Xk0=I?L<86b<~d3w&N5VFJR#3kJRvQl)a5GMdtb)QP;F zKN`2*P+@6}yoyzJ$}VkbE7?RAdnYwlluB1p&zaKpF+-2{^&R8s=jO{!V+?pKayLbL zo!y3lr9JKiZ(04<6SXDBd=BUuTkJ&VoZPu@RS1O$OO|KlD^A@`B$%p?&u4 z8mCPpawW^b!k&}%{jtQ3Ke{0~zNeyV6Rrdg`jI^9as>8-;ECefiOI72?(CrY9JLXQ zw3p#^a|B4_AiuHS(!u=xy{|qQ z(ww_$;fPeKaYEkEKg%cDzwgY>@GOL{N&B47!Dw^rdYhkO(^j=j+O>!t_SatpAC#Rt zFV|Cfaje}kbUj7Z(sbS~e=?eEJZhqq{SESvi4OK)f53+t`@O3cn5||9ArvLxbLP_F z8a_X?1DdW#r_}7T9PbEl&C||=$%3N~xy)^Clkch6KD7ny(}Z?>S2^No))E+fVtrA# z*Zv8v{?@vr#k%PH#LU;*F^Gl-d@(jCiU{fm)C zHN4Mjs8Mci$^Ms?f;u%{#T}}4MIC=y=i|l==I#qCOr-ggOX2k&qis7?eRtAUQn`dMUvFiIyRuh6<1{=LPqf2I zoeMELE0fkLQLN0FMDg32Ll=UT@|HTc!=vroqg6ej-JT>Ik%Mi`f6O=90x=!YZ&VmruUi2jW7 z0j^03IHv0iSy`AzE*|Ts4a$Y|l-4ESPTLz%W7h6IIm6!uM;tLNZsw^RQ$>4Vf64~t z2HNA7Ur_%_T?5Z;87lQphzP={*%k)OJvgwTW5?$=VnzrT-D{4VV3^|>{rxI59zSRl{7ID9cpdwwzy~fiIEQUgtU-kkXagt zv)q4V!7k1k)TT|(&w?2l z*)ID@Anx)~79IW)jY|PPkFWL0kf&9<+i-Z^E;-%q- z><9W*ch%Vq4HsqE_}m@aYZ&CiYhjIh70U1bcNIo>J+>?jOa<)bW zdrti;mAP~iqh}FI<*z0<^hq*02h2U)vA1kBYRW{0J#>b>oG4ksp0K@_6;?tFhWCUr z`isLh1Y#le;>)!+4)v5qj$EJog&(GM9K3wI-nWpfJI3g#TW&0}FLW4I0-dpJW-%nX zY|+ZGCBgE!@H#Do2F83)Q6Z&4D$ft?g@q$``?+1*k2J?`r{kp@x>SI`i$rRz> zowmD{dbcTo>iM#>z~eBvf8`~anj~oUzT@6ksypI)% z=L03sWawprG;f)~E+d)ntFN9wwzH0La6CN<1uN}y=radK0Z)SMXmtr&IR zd8jhD3=$fdCOc*a&=o?I8Kl8rONxC1iv+VM`LVv0o7m8rj>7v#9jh8SGMofDIcVU_j9a0jpZOTjj(EJzz@GWpM0p@QW)XJ9%D6i6!5 zO5Q)qWeu7EZ+pW35Fk!gP6;Bx+d0`+1G>fkAO7kg?pPXr{mN}XD2$OP>qdI*@TPe3 zlvm+n%(QxXRcVxkXFw>X!AKg!(F=+$9jBEFPaxy03Jw&5%|7TVZSOaiVX$O#j>Ya) z|4;@ejDp$v8$4*at;{w|D!Ce$ovT-XC-CB8s(hZ^g9ekT1{ zYecy<>od8otvt3|_C}?Ye9n4d?zgMelxw{%XhGQ<-VZYkRU$$G2Btdi6q1N)eO}A< zb_(XrFfp5aN!Lsux(IwV78O^=dsa~qUxzdMM@<5w>C+!;039l$qZM>0@G0^A7 z-oUam@LgxW@#&t^Sk#hx4X)V&2+MyBb*)HVu-m5&HpdBR3CK~7pHaXIaWO8~hI~x| zra)-z3@_}ZftjK`gnHz(tlvb9=hW&40k!=Op2oX(OATn!+}#(|VXUPOuv6P+BEg$MJ5w3%E5}%B^ga`*(}(kV#@QenOOke^X4=8 zEu=hFz?WjTO~*A+9xLX1+v|4fnOm&nwrtAqwrIIi;F}$0ddV@*aC6py7jB= zY=fZ^oLowhu@g4X*R#W=Qxz+2n*jX7@?C1IhcDIB(wDM<%Ge-P_j+ada!%llGrka) z^&GtcF2MJ#d%lCG9&75)?euD_`$~NO14`ZwYWG>dWh7F22SPuCpxPJY4FtF(05WUD zUl%D*{1r9m2Tp);bmzDRj6#lib~wD4D&(v;Jf`2)36kk4W*!gzaWrBi+JbJWvWmEV zbAxqU@ZipDrG=YmI1|o!$e!IPPVRfdY=y{#^or&xqfON8!w6N($5FJ!dC+p3!?tk4jp?i$1zv}rzTO&Tz&RN1JpJtCm-0yBK z`V{n$eb?LN9wQ_fm`b;?g~-a{O@A!KY-&a88r&z^LdD@^@20sYLyizvbh@LMj?VYW zLuXq|12s{+>q#}I7i^8Y;Flv7kKTBI6jlfPe5@QWSiVo%g|xy z!Y^Ab)080EA7F7iD2Ic8%LnE@Lf$|*Hwpu?_`EQ6Mp_ezME@i9X^9uawbce?wF!j| zA^avFYmjuTaN&+L;o)7Y*N?62u^2|6t`7+5US=1kJCrg7+Wd`=SF8!AE~gxZ>tm4K z91g)PT1CJ3j+L!+XMjNQ5Y2Jopw1WJ;7!wfQ*6T{{SJUAwXT8!`BbZ~{mJ;!0k!WIE^cVmd!ugv#YnOQXdCvRx$t zO-TEheso&2tFF7)4meP>84<6qt0Yey{{pn*OoTVxY+yeY@H$u8t*n-Mq4w)N?GU5~ z8gIoYNbnM9xQL9|fnJ?t6p{iuf{@=VyP4F8ut224nMafV&G4G|i8_e&mY^q?2kaf) z>jhlWvR093V2`pac2j<@bMF6PbjyGxC+-nZ``CB??|LE@0IVQE2w-xZ@2_aUCk< zeuJ>x6f5G@>yJzD*o5UDEwOXvSVI`_^D9828mWD9;K+^0W`Y3EID+dULWPf7a|1 zZ$U)3Jbd!ZYD?hipsT27SC2FP>jmgUu=uI1vb@ZVFm(K2&YUXfpCAtV5N;+F&r_E( zb&kfMcl+}=BJOElhgz~pzF;mPnS}sn38yzCb2Ecmz#AXy;%2AV{I8jc0>I&4AWj_@Z#*j1oHT7mz!lnnc_al^~t?r zs)&i@6Z|T;=1}2#6uL6@_FB*$y_Yj)Dxi|F!eNb@Y>mxUb&dO#FZePBLRai=fPD?f zwolzi6-ftDfLY|M_+&rSwfeaQfj?tpU)>lTdgt4D2oshJOL2Z)FI4~m@XqVUgKEEz zPToCUh%2HVEcq*BynBEi1S;+H3&?}}R+^x5qSAO%K|e02?{%@4Yuvz86PoqM(%4#H zd%J4$>gc>|fd{{QlUe?&e&8uqWM#tT21@)w$%TmBNLrGCIe2HCD1%}nf{}4Dji^ST zLexu4Ox#^gDu-%{S(Y^Q)?Wd~4=n-%4HKbfocqPkk^&jx{3YAkm2SF%hC!^kd|r<6 zb>D3h1f$%4JJhaok9k!YV~%+YO>6Y$rqL~G_BPbP%#ttO9Y7y&rQ7>|8q4wNu^(wA zb-g3#(h_jTg-Tx8UG87^&~fun!knktZATFgIm@lMCDC8r2@ir78Z+HtUNY>v`{*q| z$he^k)s~n>-gK3IZodJWb&*d9mee_+Ds>HEgx+Yg2?(KxbMLrnCl=$!l<%!RME?-@ zc+*A6OmAOJEm{ziOE>Yfl0#!PownUI+z8)~nilG8l+Uq0O=2E0)0aT4N4vz5?bZaZ zN6(XM^sSz@Q;)E8>%Zz*pvzncz9tzuLcu3ew8es5M^=^|H-S@fZ*(MQj52AlpEkYW zE3?0+tUYt1Z0(j(hp5=@H){sbV}f#`IH>J4iC#Y3or`Vj={=et+C5M z*?t`ExdY&>@j6hKN{x%49&l^ZpfXM|Mu;%X$l zq$|{c&(8qhQnj2l>O$e}jjMF1aN9ok(s`X)5i#F+#8=k6mjn4J$s;EbE>|LbLEXSS z#Roe-7+EQx4(}0-rA62W<(`T95gtMk2%GrJ@?n9f9-;L@5#HW4xCVULu=9ot`5gf3 z{m-aI1tlE6)r99FX8z)YexdKHNs~@!nWAslM1e6|-oTY&Nnr)_(nd z1ZCi4O&f!GONw*aki_Cr2yo)tL-dqZtsbVqCa3m(Kpnp~B4hLpqd#H8HGhH-k2)vL zZ`4I2gYA4LX9;LkTgVuJX!m^PO~fet7wJl!+j-Qhmrtm!2hnu9Q%%Fw$*XIi-ITg) zQ4?;DX8`<_+;QEv?M!!hc%^qyRC25Q=l_I^%?OA^O7YipEbBI51wW~0Lp^+vXudVV^S zckww&s~Yf|@YCB9KN$-lo}eS%vd)r$1aO&l(C)y%eULga@@!cUA_I~DOd?ZjBT^@8 zK%tjTCynUTnJssVrylVe#^X+g6_O?sORwijh*6X2rZQPw4Uh|w4VVquk$Z&M!WLP2 z2t;$Tk7996JM0XEGC9|l2JVjz1821=b5_VUdBP?hMuKPZMZggq4+5!i?KPJihD$*? zCrZdDsavJqWP#<)?WI~%G!Yr69Jv6HclU>yl9jR+k0XCSC?ejzBcM4~g82vdnaotd z62iorK%R~#V-P8qW2Nr7M^4EyR+xf>3*9TEtihpX?8DH=aJu4BYA?kKh0Dz9wKPxz z^%NwcEiJ!>&`W`=qAK}MLYtF9F0qBlj0v$LMb^^O(7D%rC22!9MR+SgnfN>x3KK#+ z!8&35h3xd0ZIwJ(yd}_>LM4d-t@Z}+Mn#4Q^dL>GxlgL zl43=|HFmeic{p_Mv#4VZfA$dS zv}}ARL9kWCZZitL^Ap37+y>KMB^Z0ws&{P#R?dTPg&)yPipx>wtiOu@avgmHLwx}v zka>{EjYaaXvuFg_eM;)jG-uBeJ(Lrs+CDQ18O>Q5+<*y(6-qih82=@}b}>E{@u*`3 zJwm$5NjghmnRxi5hVS~bdrPgR0dtQ|n&;79&~@n3ypv29RhuPM>;mbEjBB*FWag7l z;HdA&gyB{g*-REFVte3i!N9FG(1xD!c6^#yWxV#VEK6eSLXOW=&ypR#FVzfjYHTn& z=aJf~PA{09e)0#5;gPh|Dv{?;@~w1V!`ik(DAH+25W%$)7m7}dA{@c?ZRbL9HvcpC2oEFX;V zhAq%S9Vs+b@JW~phU#xW2@fLgJMad2O3p)A%p!KJzqELiom+V_BsU0~E4*Ffz|`WD z{wQmn?9=W*<(b?S`PsvVv{*6QMl!x4@~nR#$^3q@fYb&s{WlkKg_4%w8hs0mx{Qo! zA-&Z10m(7wAEBtEu9RKzQWLv|1xoe>3w(4WJQ@$k6DGbFBjJnrfC_5N15ebg_b-U6 zBN*bcA!jMIlt*bLg? z0K&}l?|mQ4v6c*5NS3CoAZ!-(C!&oBn6!CFMh)^$e^H$+qblpG2}qV6!0!CtFtbpC zvwZREw=L6xvyPa@DH1<3b5Dh*DJ$Cqw8AG$YU-n>$Li(nyXF*_ z-P#Dx>Ih4NU4CwaH@xjSK|D4r(k-1K-AwsX7mbbptWN!31;lPGx@Koo79p0*@@!u2 zuv9RSfv%gJNW^N-`oUT+xQhaiNyFi2anRDxtArU(qBwTp+aFzBY6#I6Ct#X6}%j+wW`NJ3|QBlicSmat8ug%4)C2`viWb&a0 zrCPiSPZ2Daah{X2dC5>zMJU#=XWsDTah)YKwoB6o5=WdxbQ!H)Vxv9@wYL!0wpSe_ zu%!V`;uQF!?0MMD6|GeRsdx%3!h~(3(D1ln&6(c`A z9{!1bBB?YbQ$1lu4V4{VjIWxIr8=gb)_m$dNzPthuWuDK{yGp#RzpxQA@kP!@P$sS zbt|@FlLg*C(j%0;K3V4Xs`rYhLCK#=!cg^COU-^bhULhTQ!@0OAM^zk%b9%*x@m$E zDgs>rpplv|>1-t9n#KAc;cQ}>SD+8-|E9pFYa<2%0^-GQLR}&CIgTzKzunDFX{8PszM!Y{2UdV=4Bh_0;n*jwpu6WP2$Ke@oEtN+hO~J+J$N~byrJ0(Yy8|OwCv> z9?KwI=+6}$j|dz$S#f-_opVWOfWps*X}lDGsVS`gPP-2O_8%ZjWWKEjz^s4gF9EA+A^p4PQMW@CqZnsZ$@p9<7S zjn6mdj|A-4&5I-79H|lf{(Prr!bJTKj9kYYA2TJqtsRZ%EBbUMw=`Efk zmAi-Evo}L7CFvqww`-4HWHcIL&%|o7>j$)RF9PdP+ka9v*|zx^N{;3AgOE--3;kB# z4lSu_n1UYoQaX2It||5Q0q37q{^my>^ZSba;qc9pq?uctzaCMLG?E}-Dn%YYyfQ@8 zHUt}T4q%ck-b({b5nK#JqK?AHvxaaIzNc}kA*6GV)1V%_^eNq3y#i~*GKW^(@Et~? zq2dAepC`0)DQ&R@Quj8+iH<#&$B)mE{1wcnKg;f??QCgy-36k+_q5^(wm z0A;41CNPNVYdoS-&7Mh3rl#oJ8sJ(@`U$83lJzi+D zZHrs2oxZ~n(a9D+;dux)(THE)<_i03`J+O6S3s^ zCiIERPnyNhLngv_3(6D%a!0Y@b)0Dp?FrS(GnOeP(*(D*S$b%f=}8=vgOa5=%GEEK z_>?a#NVtkx%%!~B$ls?{>AINXZ5vciS2I%V)1|=qwk7u39^hk?+s?rsoEQmo1ThcI z5wK%w@Vk@f zBOc0(Uc$R>ozTdVZmzS%fa|L3MAs;79i@d6bB2F^Lbzk30>|)$M~KdmtRp4oFfUt< z)|f5t0YWzt%X_nvC!}jHrKsiWPi-)YA4WkD`4-+AdeT9uHc%6-^;>&%fIE{EO^@Cl z2~1obu|8^13G$5h5XieTEsLe_Ws^@nC**ywCdj~DNh?O)#Np5FpqAWKS;Vhqav2%u zE2Px~5am6Pl$;<{V${x5i5sWe~sy9o4=v>Tf2s& z9B3|}ci6oY#4Og-$$EY>9}#f1_scWUphsSEk615%w7KuG zr49H&Ofd~tJnd&UU#j)S-=#O|>%W6mdA0hbAjNP{hH%)}AT<0-$ps4hhX@+a>OA_| z=Ot*uKsL;Rg5o+}e~~54@hSHX#k52fo6EgcDVIF+-{r1C`dr33AQdOcZSCLnpp8a^ zy$nGRVM6vxBiCjf2orIUC$z*N zUUzrc>=7y`*O=wW4AZ>Ea*TQ5M00Yu#bH2TU525a7yDsz`!TybzZ77%pOz#my*B^OO#_X$wfIr?OTv|D;gK`|X|0M06R#*jarP#U zT1UPz*@&jr#{Wj*~f_$F;TRqGVuWx2+k#vCaft4w4W!6?T; z#ug|2Sf(vBl98B`G$R^Jn$0s4uQ&RG{8$hp;m4Ky88_&b4N4>)emXH5fPRhM%}vv0 z7~d<>HGR#B7zV6G)qz+N-wn$f-ojea{9d9|-sN?FMhfmqfY{yWS@&Iv3tZRh`t zq%}nv=5kio`BTfXOQAB^r`Vc-oV;kxw?F5?u+~+;n9gc%l>93AWKt(=>Vm0yK!> zVl6lqK%hmk+_va%iD3~Nj!baoED$D!8(bqHTVa~s2xLQC3HXf$ zj-KZl!3Mv%pwDqXXA*Sv07GlDH69}Kdh?-jK_VLXC6s)pE=1S;qKX`I;}8J4V_G$h%)dS|7In`m&V0t#wr zGhOD)rHmgJvjQ>hLla4L-41EXU%nviS}MPP!py5)nQGHwR5#Ai@tuhp$XMD>0rlnv zK#6sg*x9=@2gGzwv&+i;4PdAT52k0|18rFZ8mEH2TZ)FM4MhxEuOg=tA%|L&HeYl!`0xm?8Zj0HLbsC_!h=}Y-^%q#&W*r@<1 z$j-|#fxrPCMg-( zKFH%hdxo#ViUD21n$GmL*SX>h3ywu>i;y8EUB(Z6Q}qZfH?N`p?2f7!b}5ZwNu{Bx>glBMuO;zz!?+ zwoP#>cdCQa;X_tT>K$g;`NFGDhBVGLt4I!ulYRFK*iZK zj{GW-8=P<&sl+0yL*@xN%Tbol$#HE)nR2EId!5I^-Qn5b)xK%jF=E&a^D`f&ljh`# zx*1MW+3vJ59l@-*_8)79n_;cv!MMg5T+Qib44F zBxf&8)p$ym9aFqF+$tD$sLrbINPKEyT@CR;Mt*0{ZI3dMr)o{H0X)W^lBP#+rg*zW z`8~i2$X6LVSR$_t_C!Gmvr+ih1{l6$&f8ZZiaR08VK&tFhs%eVnvg!>E33p87)o5r zT(1JVKY>)MSaI?})Et4*O?reA+yc>_bL3MS5?PkhX;7Jj5gz4}x4mvS|K zVnUc+AVSa@Hc4D&V0D*#Am(dD@n>;=v0-bC)#gc*-;3G}i<v4$O)j=-QnvXy>p)otM^GgcCqPZ(WM*sy9(FOsgal4L#){;a1UF%ff#y>pN`W}ZAHHbqv5AxS zpDw0D1a8Q0UvzE}zRBGwRSY7sVp4qH^wzn79cq`wsW@DKZae={5gDum(+h>~}F$L~N=*lay4c*2`wBo~n3nKRIdcGIJ;oWQGY{?ypp1DD) zo({mLDhBGh1w5<6N2)Bp-=e~XHaJdQxLjTBb-+vJrXz2zwd(86tb1Xh9srjKi2JR3 zRNx0U!YmRTI>xf1p%;TkiueVj} zu>pkz3S^Jwm7h|Yi0Gjrh0NxwFFD*N6w*^|yFN4P8N(LUa5>ZS0*bc;3v=TsvmIm$ z9Xt(pj_n0{R@4Nun+-=RU1SoSCD|f*U#0c9#k;nmkeT55sUHn0L6Xw6-(-s4VFy<&iu{hZl^z%4x5VHtp|k~xL*tj zQwgz49zxZY>Ut^A*^u5a4pYj~df{{xa4ap8sUM*Wh1>Gp4RNmMa7a(w`$$0C7`&x@ zcM%O2x6zJo#IN`cY4$r7&K#(hLIlyGXVNG}U83=a_YoSK`nXI#HF9-^nl5O;&DuBNv=A-#eMKiN zK}&_NEWcKSh*8rug}dcr8bm#8oT>j^g zw>I5auNK9LPN1dS6I%=81wn!78V(luO>N*jCy>9ldj=ppunT0T|%F9vkwL% zM6b?9cGs+?+ijnP)*PdDlb-(?{DD?3+HJeF@rw-ka!uW3masA- z3Q13vxPK49cvks}qIF}?aqbum_!#I<@5o^fPs+oK?)&tJ1(SdQp>u^_vmu7Lxqt4Y zZ3|)vYu>Iz#;s(eiNkU?9>Y5RIHOU_12e?JU2}@zjY648uX@8`rd?1DC*mBk*$tGK z*qrXZM51^bLP~TNU|D(dYQ?3)7gl{B4Hy)nrA8dTmV~@woUuW?qO0DsTlS-A>XD@r zz8wBRf~8G+#R^om{~dcddE-qU!r|4pE&rtDGwWUysbHGh|C`$%~Yx_f>Pd|#^psi63WA&Y{ zP(>fR5?qYTa|$N1ozD3G*9$-#-UA#l>AQS!QAbm8ca8h@gqkaUZ=S@r8!paCcDB-7f9GN^(%4)YnVxvU-o+etskca02@Awn7p^} zrXSdlnk~JNOPka}Na>dCRGd&dN$joJ<3jP2rl#?Yq z5Q{kvEsN8j!+)WIT5I`J$;tqR4%Uc-@)QCcNb|e$wk=oG`W4BU@xYR8P%8xmbV6&L zWp6K|t6)>6>*(b%nNs!bKvlck!jX{bcG@5}1(Ji96M3(MJE`!AGqyKdznI2<;bfcq zv24}3Vm&h8RWSf`*aZ-pf*a_R>V>Q2Fc;XlhfH=84+EM${Z*xY7HOnAax1YZwwYPP z>rhW}s>OO|Yf`7~xd^%RTnU(aRZ;B_FKUSJ`EiTMF3AnvCr5jzc0`p!k{G;R(|E##J%Z(IcL?}O z3Le11XWuXn$;la_Fpm`sA zt(w#GHqECct8b+l{3*Ku;hFRDE{C~_glAH(yXw=xqelU=E;B-Ypg~{A z1kaV)7@)qzk*(MGZ=-6%oCxC=m{N)T`C;dW#|)XVIuY;PW6P@B$yroHO+bv4EN8)E zqv*8R$V2*%&YK&%MRMni9r%IRGXA|lkJ0PbnU{5JL$HHrxrYsut^6V+8M!hp@54;7 zY$jCoxeLAA#DtQ8#G4B83SJ364RIIkkX5njbARw0UTCkej~cCvF8xdnvQ{1JwgD(@ zg*d`P?NuKmqFk(4e>Mm$q-M{l&z@A3h~>O28^`M~s=t#2sn zCUea@yy_k+;7Mg8TQno`zZHHmMA+mgmgrEY4m9cfr+Ui$>o zphDj)uI$P7)>LT~BH<*v;8&D`y_H7^EJGGLRThex*D-VFUMICom`wTCvQ0DZ7HCFB zQy2{w87q^5TI>rrBZJjXKaY7F{_b_?*4_JNg{4wEC^5>$O{g z&e@N>6-~RZ@LuGN8fqZV=JFfjr%4ok)iOFTTr-pr0Q@i0l765MQ;L3|Q4HzkXmLR$ z+Ws-eXqBIIWsv4x@DS@k06B(zO4;kYCCYq8iD)iX?1XKKYST~9Sd|+#w=43j`bhFN zrnNO$D_)FeQ(rDE#b79+h)FFkB&xBl{`oP*6y0X7s0NUy$fK#7tfc_{T5p@*Hf;Ys zvJVs;??HT-i;6W_kdI3CXCc2fglCF&@>xmH}fRo^eFy2b?ipWF*n8`rBrZVM$ zZNz@wG#t=gGR&@8z+$6YEXbzB{1ZYDg|AlcF54}hAvP!nt}E)$#;dL7=oyy<#fj$3 zv%d@8?gXYcpA(qO(1!h(*IW4A8YUpvcPq~9@v-Ayu3(dyM)4Ac9S0QC0!Qv01FRv# z9ciWtRxgPYY7OVlXBh6ZPE2Q<&_?=*qGRnEh(S`Ce}}k=QZ{{TNnpo~pJ6&RzZLOQ zB5O2s)2GIE|JME|ANuKr=Uiktjikqdr>z&nWP9hFO`wdI^l*MexM64F37i(#hvJBI z{Y!E*46*hQosC)iB6O_SG40}vO~uOfK=_e^^I8^TSBzu$p z-?_a~1kzpxx>5lInAEbJ@bvX3%Z^p;6D1Dzh$BOZ@yL;buj9DWtvkLUFX_Gbn|gxC zr1tN&GCMt-$%)@EJ0?38uAtoU)Ap&GxI3o!ETk$(h4SlU=-*24Kf^?**Qmsgl6_>l zeGZOyCVLPJD*eTlkoEI^_7~M^3QM9EPRT>M-}NV*tz98=egcHKj1=T7M-Q-_$jTWP z0!(!2ruR?gV>?Zj^aAx;?*iFHvCm@ zJ9KVFp!xzK1Mc0XIv4Q@7A%OywbqhMUp?chI8dHw zXqWQzB3RZd0hVhNj(XQ6vIVfJpsU2zGpdy8+egZm`s%vso;iBk>>m>Z?mp3sfzr@0 zKsBj#DuWmVal6Qe42EwVA+&~SpMUWFh`n*w^M@V`QFC$W?bLp&V54d);Cryjq!bg~ zuOlJH!z{DvtlF*SP|A0bvg`b-Dui*mZEqwMWazXSr5}7(xh|Xbg57Z~c0ir0mw);8 z{D*SBwwXE5EOfRLJ-Mz!hd*~nJv^Jcho1H=^Z$)PC(8SlD!{Gay$Zgq9ZgZPfZ?XX znYir8-R|Pz7nP;ndtTjtew-_MKxgzvNvmJ{g&W5;BZEN|YKf|S>1&}G@VoqV$;+mkV{^#gA-Jkq7m#KQ5^!=Q{vtnPr zo^1+(9&>oPsUPY@X&1R93Pv4m=0y0&fL8d1UX6{Mku9HR9(xKu?Aw{(f;nG$o>b-&4|eld*edGU}734{^<>w zuAr5#x+kLx_HW0i=%Y>*dofunQO0LCy$igaMV85cjv0-HOXH6$!}Mpv(^YPT@ho1} zt~tc#&91+Hp1uV_sf}iJDb|fvS#J~L63neG#x+X(Ed$ zRV+69tu^GE(LY<#66HDG0oSQT+=QmXy?gV%|}p1hs*SW zp4{0Xil&j7%I1?6u~v2Kx#IOU4!nsnT1e=kJRygtBT|UHTYokpWHag*MmCQ4eHSP< z)}tV0)HiLMM6i%=lZI!(%%)1>SK_vU)86RDNPm@n#p_M7UNF11FO?6TU%G!gd1&Ln zxMI%e|I#j$J)W0}#;Jj{LHSm*#hkv|<@_6%dAs1w%|*;~CdHQO{JN$@iz`ARYEX%j zZ-SeNcS~x+K(ehRx#i%S=ew7fAeR(cd~bo|-HihwBi!yQDoaz(F)0{I*t?8SH_5_#PJ?h(TpEm1f3<3{DA%(wDy zl@`xK!`QVwa{3Lx!~83g-}E)-0BZ^;h<~M6P6RTMdqGLZHK7rfncB|n&7t`)NDEK# ze~1cNo&jH#`3o_>p$mFHTm8ix_=3E;b58lc&znEs^LwKs&iw#BnG-W4nsl*Cvb%g* z`Fp)ZMb5^hSy%JdyK(pxN>1O3pDxlc5r;Wgxxu3LIfDaBw`e$m>f*v0{|W9yGr`*l z7P1tR>%n27!SRt%L@dRSbeciVY~kSeg2t3o{%0!GL-b>uu5T@ZH$+eY!&cFgxEc0wYWfgpW6nTifu z>Yv}B7TvB^I1o?g79h@h4MT6xI`q#AkMwMvu*+1If}QJ{pe|#1uhJtr==uNTLEZOh zi#nQWRdwmH0@qtby;XMY7Bjj>yEuMqgU)EW;cK&!8iC9W^0$H_fGEl$6uCd3A|T%E zqJc={b-0QMf|23eTesExnD(wFEOnOu4Tg|VZd=rK@v+ib%OE=TU4@4)}o!kI$Q(w&T6F6COEsfVnd#hoob`gOEAp462>iVaUhfBpE!*enp7|P@hSRe zXGK8L7+*V&OPQ6;fvWrse4B2$3f=Ik6nlfo_?_(Gk`*_WhR1y~Oqk6Bj`iE?M?2AZ zmbW@*(U;LDlaSS$E0ZHfh(bDia{wjI4VBzO`ItH{T!gTPccnZjRap1^^zv;xkr!s; z5Zwyt5k>1S!s)#C-j9X2y)WXkc(J#HM`vD3%=gIRC41DK>>pp-7e@VVdd^Bz4zJoT%L>Z4zI(BLkrbLCulPzlQ+RCI=jI`8XO6( zUi(&(o3*V5zF7T%iryAozbFr3XEy7Q?XK}jwhGfi*Ctm5d%so7!(bE)HJP}5iDwV@ zUfy$I{&*5Yc-I;X+YhokZ9^n!0`n3$PfS(Qm)LeITMIlT4wf93FVT97?Q8EJ zzB!y4U#6~Onid=J{zI~|ov>}2OaJynBnFGQL2hNZ!*xyk`Ivzp+>6~4NezWd7 z!x&R7c4d+HMn=-TcO`Rv29PDVgUlCW$%wN}+blJYCmr{3Aq<402L`r3+K*SCCHnoL z@G`MG)x=pc5==cgd3rIrMZsL`{ydv@cCE2ctiNn(+GMi{I0|q#cAB{+Tm-BM)FbQn z`q%ZkYms&*_S2Wx$X@Dojo|HD7Snr5umihXb*JM?9hS3Y#b)SozQuA{cTAd%*?Y(l zRK!dvfhGi2FE2J*LksE%rvam1W1H5|E;XA*$ll9h5_b$g(kQ@!*j)60MH-n@3; zIdF}?hYmnEC~Gh=1c?i@(OAR$`O4)E(a477<{%VrV)^|<_2Eu~7}EP!p3%jAYXnYN zQsnr1TXYV(F)7W>lPXK!{dA3V<0{1!$41CLktp|pe71Oub8wi$w%^DnKi2=kJ|y%r z=JA_#zYD&MPruSa!>Oz^n2b*@c^hoo+CEf_C|S0jCX|GHPSfdhOOYUx>)Bbxzs62c z7R-hZouCdVoObCntcm-GBD6bdkakB_YWiwD!UapmZhNY7Ej{$NVDZdd#AC&_cLXwd z?H#2_ZcCMEb(J1v?OEfi+K->aGQo3?Eqh+axLq!9@;I?Do&cs!tR@ZXthVD>b=cb~U(Dd-HUl!ZzYNaYu zPws>zwdM{9t_Zb}_KjL~-P5V3tCtpQ>kX$lmJn+Ln=`8x>-7@+BB-p665z-a8FJZ- zvD7WwFAoGtXUG|Jum=<1l`f@jfjz@LKMpL(SK>F9)lY$XOg%7NT@-*5aiHt zv4PWI9Tl*!QRmF1m{BKx5VankzU8jD4prm?a1-$F$We-yqUI&4YAU*22Cup+F`Hc zHf*koSz^Sevyz%FiwX%e^?GP`Wy3&Iu!JQJi+akgHO-DOTO7$XzG;0tai6KOPC!Sd z9I`Vq*j;sLMCccyEKCc7l|NLdxZ!nY7$)%R2`^PiwXw+ ztgh0Qj}#+2g11nAj>xGTatEd;m?vgA5s}U4MR0B`fTvc>@C|1 z+rIsjuGB6^tml1YQ}ajnn-3cS20ZarbK40#ilFaw>%ywN`1@3U5Pp1LF<9&&7I6k zwKb$S6TShcd6(!TnIv3fYdLXRPe zPEXTR*`mCmye`8|q-$z0) zBP(y&>oo_nVI?=2UI)FKt1P&33Z2SHYT*|3G^)+;q`^}Hss?jHIE)Uf;|G!B78*~6 zEXLhbq!oKNUnLR1SxF3 zn|EVFvbiSyhn`$9X6WqSttXni-8WYm71`<&>c1U0;f-<6FUIMtJ&G{$n9dBjL1LAV z7IE9o|6xERUFF(c7jD%fu@E1Z;jV0B@|U9XHa_l&*n?`#x=V zr?wh#Teqf@dg`XqU`?j4(;Yxv5TZ-Gj_k^LvhE&v9f|3Zhnc62&5k@OCdw@grW^S4 zZ=b6~<_#%@C#1C)+SLx3oRQ9ugp#D)-ZGb8pu#WCyRB+ZJsOH=WE}&0OgGKHJTN32 z+Ze60;ewIluB|QTWCouzq;)l}@kqhTTXp>^PM^GCxnxz%%>cfn1M7!nrKPpe_cxct zE)w0yNeW|?d-{38{_0kB(={&&(ZY!zCLZ@}kJ^tj(Qg?DEA!^(-J}L}kAcAZUt_Ck zhOFM2;$t`4GS6_At$=Ov;30V^!e}LbURR))KpnZ>1AYDO8JIc0SX-j>Rj1i)SszwK zDw9G@2V(^R?rQ4m$c{(xVMoX8fpXk+`g#mYJiWryBh|-C9G^JwQ_%4if8ND>zs)_XfiS6fq4GTI58esHYh6$bQ+Y04~2S{u)fKsVyQjFO}qg}E_?yb8X?p<_u!GDDt4 zt*E^ka<8=uQuwMWK-Mj2p2hALmFajUwPu zhKoVF9-%g&Dg2u`_xrI{)ZH&)#+&{rkt*=mGP3i(H!}M!-J80=*9vAtp6gSe}* z)u(EE<-FFHSL7y;;9rn8=smity7=5AfXLLrz8s>I1HS*S0499~+34e3`?uzKo%OEki0ml&^u9QO-g3L;UD8i<2t{0A5cn_?k1Xxw2_prTDU zGHsH8dd58}KTLCr8e{spMu0@YfXCC|;~`EH6rL)D{{8$95l}O}d73+#VmL}h@ku)& zFHhC=VDj{$JDHjAIBa)FUp3BD;@~V8eWM;jD%_yYDdeIK!fjI;z6@DL+%0eu|d`DIcZ9Q79`u>ps5H48B<5}aR9eaCD*X&2+KdLI2%k= zW|tIK@}`MhZLS#xF7VkH*Ax@RUv8$Dg1)Re6t}TEv0g;}FSM(APPrG1kh3v5n@US^ zBk&-Nn^KzhSW$IZn|EIR=LH~Qg)fdytF=R3mqUIEa=e)M67*Q!f!ua9a*C5k1jt_f zp6F2qpK57HkpvKq#A%gvp7<$jDe->ia~@S1QR^6Q7NwUJaE%&}S3~bH?)~X|+q6eo>=_K&f)Ulq z75p(EJPg$e07XtEVtmLRgxmi7lr91tW$Ja$oMaGx$TW2$fZ@nTd-580b(=rYqCkT0 z4bl6|XJ>LGW&Lh9ZJQnk!78EN;d$^(cv0J8SuoMbBUQUy(j-r-335)dMXio(qGL&J zv$a>_e>^|8uclitoAU@9`S|p1ON~$C+AvizZ-a6@2W;)7DK-O;M#f61Ns~z-n(qKj z|Kpo)EzY_K38l9oG9*)dtS%n@vzz}$M`e5CMbmJ=H$65n_#3|Qc ztC%bsvj}48t99h>8b_6o4ZVz#EiD2OvU_s)`cLV4yA+#Iks!;YC!w7OpDYXKW!|rDUUcSsEzhSHo1{XBS@A#~DHK(xA&4GA1bi+Tg!s zt8nluboS~x^%-4Y^~8@4y>$36To3MddzdOxB4xb7)6>@8E?NfF zZM1tL^I0-(iDZo8smKnKn&1u>+d9$;Eme<*Rcn4He?2-tY%ex4v)A?gh=y!yT8cX!Um=>)oRKE;5s+FK4Yvaj}LL zu7}@R|Jsp%5onKL9d1Rzp++8z(=c~v*1a7SR}X{jT@foXfVQWZ44^=|-p`jnM$!R?0AnwCYdZ!(YttL+!_VuosBM)LOnR zc7e~{$OhN=6~^jfXH5NlyIsGSalMp-X8YD0qvQw{T|-3#J4Q-$nA;6aH^Y1_7Y11>59g-YihvCd1Ln*0c(ft;%D1Hr;I3p zh88T})bd|Wf)|ERku2)9KGS1(cNoNEF2x-Hm(G7B`9&5Vnuk|;rWf7Njl~|z2QiMt zjB^sFXa_Qxmv}99do`ltz`i+<$bQ6By?PTp@M2N@T0EZSGv50FvKXz zB;Y4lX z^?!i5=+A}zinZ^FXTg^%?BDUYrUs#b5t<=Z)uYH?mgThuiZ|Mk;{}uwN~w3cG89je ze-uK=-+Py~AG>peJRpNO!5y5gkg_EtoeW~9K6W`@KHQ7!uI(ICXfjdFFG}r))90ZE zNRZ>z)xulpO6E(9BySv-*z)9p$SLW6w$v1ZwNoB}sX4aCchuutwuB!jDl9uT@$SB;QSc!x9>upC4(H4goDO4r-xJ0#w z(YhdR8ab{fBQF-x((Yd9k)zvP8oGsWo`%nQ6b;XF zn{YAiCgWP@2(s%2_#`o9IH?b=5 zk|Z46*bKMpTUDs|p&{T#G|JewRTncCRy5+32kAD_ouqR_=1Z!to}3AZCNDk0Th1$f zvxF2w&$K&wtCT8qRoB<%nYURBern0H@Is!b`V$b$Y$t)43rTxqxD&^eA1Gk zN;sk1W1DF!C(BSvruu#y+yT^6Sw(@m^{N~LH_OpJDO?1f%>xd+DqavrTX1%$li329 zB6d#+&0dSnEv0<`lN(kW$@r)@CTZ`OL&^cYqt)RL_^-Vb@{#^EBu4(B(54Hp*gCa` zzm>NnAETA6Swly${@~g`r4IE}Es_)9 z3q?UQsR=M6Vap#K>s_CHLsK#kXtCoOr!+cUVf3T-e6t6zFBX@c1;#j1!Fmo*@!c4s*1zIqXr%|OuU zoR~aO-29Gpujwm|881dS8hBdRzQL~cBO4yR+Bq(HO42*5G(&7%6ikOPk?7-F=uJ+t z46x{Xzq~|Y8%kCtacNU&&YJL?y*|fry^p=I_fS17v(N5ch^d>4-G&b7D9TZI<-j7* zb)8U1$SDdY0*C+UNz54GsJMu1fb14SNXI+v`ZPGwXs=^tQ|A7*no&!0%3u6A$VzJk zL=_7lWR!xqNgFe_YgSPMa(T=Bw(;NTsww(Ojk0zju!z^B(i!>sgR~2@0KB2Mm_tj<|AoE zWUu3buIyWqmDgSLBI_6J_jMlrWs;~Kz2P#;*GkrUDO0e3r3_-qw{npFVyq6k>l-Hm z=4!v>>^8n#ZYJTrkN2fQ7cB7i>jzF!G0w%Wa49%?TzNEtrwTS^=j+* z7Qm}zva$k4@$<}@w-tnMx-WNn@os7x`5W&LU4rK7c5vKeI%$#TwlvD&MTJGHT2>2I z$#WXMJPgC#e02J{1THoG5f`6cTlptLeQ=W}20AN!ka7-D3)yoJERV8CsURa<8B^{g z&Dp)oPd%#|&=N66x0TBQv8bh~Ak2ST4D@J@4F<@H6TZNy&Qw;?1WWWW(C!kxUmDr% zRAK09W8ab*r`2Uzlf$!1=W+XQBdnH)9HSLA7a2GsjZjj5@P&uMiqlpGY*SNx(>#6! z>V8+2@cdJdo;1VQGXdY3g(CJihHTQ%#H_yWZ*Y95K0bTe5xO}g!Ou^JBHG%P7k+?> za~{q8>m>dE?4Yr$wH@wP<(ibrj55!mYW{HP(nR%ssDG*rV-JNwScDyX(+2mWJ$TCk zw}CcOP>k~!gk|ZMRQgRc;ylssqjQ@i(3aOOM>Xn35 z9QdkF=?@8t{knnnAG`14=5TG4j@M=r=8qek&f-4|^!*C`nBJ(6%#XPf;6yabn{j9_ z%d{V8< zeaj!-NF&yso1ro%{|KF?e?h&7e*DtnY42YsYa|tnYT{E4X7v_Ulk6WSGLP7{NiDbY zq83eZrjcQjeFKE9FN(U8@I_%Dov{1JZ$6w0c=I7Ux&>|kw@fX)6}mVW6A3Gl4Ji}O*KHxTAAfUD_Tc-N zvzBvv8qs`6319ZjkJ#1W3S^!nkFUhqQiRPSMK3U#lh_u1O5cNuTyyl-R4t8$_u?9?7mcY{vyma#fhh!A9Xr?gE0{}M!fV(9_C+qk8I$jdZSw{ zKK$ZHWO7Bk>k?H)>evfk;~!J$<8N>GHo%VeG#O)~v@hP6PV5B+20$ufao?XxWZJsS z>CQ!UPMZ}3E1;VjX7g~?xOC(JEV-uch!d9#dSrP_`_k5P^6?kE_Px3z0fk3knq<$m zd7;8cwRZN^upSs&BL}MrZ7ig-S8sfy+eR_mJ`QKSK`n|YSAtoZYK?rPZz;tF`hg+XaC7KF^Zq%B#dhs+&(4vxX7DX zsX08#L(b7nJt}dOqnAzE9~XXTZL6He^uYr9HAhLBdR~F1-#HKt zuJ`+qUkb5@6ZEnaWA(O+U)9m;A(W2VMw37Va92J(;KsS&rqv~a{|YxlO4e|SkN3oe zb7P4{a*oZP@N?B6Dir1R+E&~zZ2IQmMs|SLCh-pR#!blz%MTN83P|;@zDHig-9l~L z?rY&B0P*ZT)#gg%lo9inXTcBWThDeIpN*AEn*Eq6{WW!CeVWp~xgE#KxJyUP9M3OG zgW98y{Z_SSs&;^gNxjrtTzwx1ka8RTV{q%r4MK3&ne~o*WUv&{iepSp;AkJ@@GQ@~ zB$6#TL5@c)y1LZx$l4b$9OuN2#N;Yeh*76l_6Xyy4@j+DJOY z{@1ZE{maMp9)1P)9wjHpt~XRWLsMTc=Uwo9tQ*Wx*I$Jtyj>=*F}d`Dj`e-2o%Vyb zg{<6h8OLWL=EFq10^@4{d-FQ#WLI1kQ~vcZPSKC2edgek5$@QO`t~*Z#K>+|AQk@t zpFt*Gs&9Y(m>V}$|3kOOM`qIupN%|-`sXDAZ4rC*8<7)yzDS`YGbw~M{7z4ov(Iyb z5^bhGteFYIF19m}P}VU;fe6!cIZC3M4kjos%jrd7_@{P#R^s0>T~?_pKq^!7N_SGy zo7z@54|Vd7K262vXRtOh>HL!~TeT!NDBTU3Y>apMq}n;sq!_EruI>(7i8V!3KtnLq zb%#Ru8W7x+(qJO(`#-<$2FH%Y6Lvo}SVF%s_h!A?5g4TUtx9TUf5O%Uf<`r#(JRR& z)WY{RF8ABJ9s@~DMXEp@FqJ&X(y~r$??F+cEEBhnRv71*iX&d7ZD_-`LXFm;;I+}! zXF`;TG8?_Jb)%XVeS-tY1|#+$=KmFEclpumR;uQx0(!$+YUx#H_s=>bq&eyt>(K(S zj2noy99PyUvMlwXiv`6bsD}vUV5QGVO5!c$2NKzw87y`;?aoX9Z#n!b#YO$h*9dru zO09W&FJ#eVsSG6+4A20_3A^K8N#WM6p3>6AWyD)lGTIbzRhVEF4crg5A-W?UL+ z!YK4HqL)6HQ(D=iZj7IM@EK)TqG821L`=_MOzbqzW<_?!67hBhyzHhVS0nHfMINAN z`5ew3kA-sYj6!if$nm$JRM|;Z7^tp`+CFsmxjy{S6}(g#l2~qhUW#jm&xl^oY|c(Y z$0+w7HvITYXt2yd?Daw04_tYo^Uy1Fd+xz!6Zt2~FJSMXlMp44ogcW>?{|qnGEEej zhetbcw24);c{zING`d|%0s?hO4gfkDy(|Fn^OKVUmVpH2jq#S-y>d^E_kg@=SZ27I zGcSY8%5^rk?lQ+8J`VD$z5v*GVj#1nL;%euY)&ZR8SoKHX6^ju-7G&cj|8EpJ}gTKwwy+{R&^u@w0nNgv?0MWB9&(u&RG{b;Pn2*uxI* z>)U?rjJGa_)rr50nbB*Hu$cZeA zQL%i=Go(W1zoX%WGaP&#*F~}tT3l80B`<#$b%?7F>%ZKq8EwQga&7HLWVdv%61Q(> zr0xRCEZVJ}28(D!?YCk~RaS2=Ps6>H^9=1=5~E3z22@IPn3_AhZW(!4+TT!KIfqT! zFNkc&Zs@a-_ACa#kQtT9=+C6-TT1x7M~$V^df(4~m{tFK{!FRl?OQ=7 zOFQ=C_uwZujLmaMY-G538xdj#BUDz8w>J%_iCI^ETvuPfJ!Vgt&1gx%!9W}JCN_V=$ zDtD$d$ zvFVP7IfqpuDg(YaZY^K_J9tw1idgEqIi=X`+`;A7MQ5vK%R1*Z@D*2;9H)iJ_zm&N zfL|x-evxTp68olo%9b@>Qonafh(~HzJjwIg4f?qa6N;dcW`A#)V@p;bZ`LAK3i&n(rq#fp{YL-Q4DZH#u6$1qHQZEr!&i&GC2g-HNI7a7(t7={4{Ug;lsZ<|ATdBRDF zF*DJO+(G{eN&dsU0LD%8rKmd%)o6AUEwElbenwQRAHJ$|(1&2yGt3Wf@#JaSEh}1J zxxEACck8;{sQ?}>&uif6@FsXZoF;qgDaCmKwf|R>C7vU7SbVc^0BkE6PT{r+bFX`P zaSVV0VDA2)&qG^1GX(!QyIQnDse-cdw_Bxa6pz%W4D$2`0%pHnugzp6$SHCgA6O&m z#GdOh44Wv!S}D4d6O{Ni#qb4@s~>~Oy84Ogs3!#miS2S72OFUfSsCuS54^=3C~P>;u=KK&1zdFS zOHU=<*M_{w@}*dKiP*g=WXbO-k}(Y5+Wr(|dY5XS+nB;s=VXh&Kh&=ddu%|YoZlF- z<;m+=Zgy{x;97Qxv09-&lIMZvBN0lqUA;v@(!rrNpnTJK$GMlE;(=CGH4|(&X|UsD zz({?yoVX15zbiPkOEgLicT^atd6xN0I+KpRGe(oTnbDW;4dc>4rU?Pm+9$Ba zNzB2(bKP`4cTM+5%uv90KKouuw(NxB*2*i^5)LxR?(P(EC6Z&B{o6*T-%y^6tV=8N z|6r#g#NOHFOVa7^EwpM%LWWAF$=s6?GItua4-F`ZW}!51GNekja1Sq9nPHaK>AFe_ zOJVMmR(JRf{%Srhj$dIBxyQ21X_R9ufg%-x3mHR6f9~Ti<8y$jXh2CH8#iONqcUVZ z3Urwf{SNDReoT-tIQnbELdgjs@}klpKtZ6>VId7!+rZ{^ElZ|_0od?JuL~r=`YTqg z@X@M4O|U3fBE^E|A#tZopvFe5&UlM&aMoT#Aj3+B%&7c5Yh~P5Fc^O&IBi1JlCl96HXF4GK z3<7r|T&Muj0^xgWt>0@SDrdv;@&E-x3!u zAbn7`1|47pWl!E^8(<6$G8li1o+tP-i#SPF$^xfXeV-W(bsGsx`>R+ zMMXU0VX6Aejp79~HRb#rdsyFRyCk*Zh1s|KT z#yFFCiw%FQ3PD3f1twdjuiX!|^B>HFx2C5}BY2md4icKGKr~FgE}?SGl%k&tL)fUS zs1#vYnlpEOhB-+5iE>JVxh}Ghy3>T_%03Ba^&+ltPJS}*x}MZTY4n`uViiRoxny_v zSP=T!%_HEO9f&yZddO+sCarfQ+oreNpx`QGR|KR{Us2Ips$fbc!r(J}uPWoWQdZKW zG%fGKbz)%SNg;NFm_eP4!o(VM1Y29-!~M1DgDA|*EhfMy&yPpZfN=ZGBZnAL*Sb3j zS!bTKwhcvO{kKBsV?9&K1KC`oB+#RYRm$o+d!d)e{E;0k&!;pMcpaGil9HD6@xLRd zN>BkJ0@(BaPWu4|WEV2Ry&LBwsT0b@ZJZ$>!wfEV z-1`b&GP@UToH3Dt)4<)s-NzlD_lw4^>c60yeBcT{Mx9%uum-)E1F@?0KD(Y zUOZa_a0Br@E&$y}f5f(%b-a|(kcyf`IEbiNOBA+F(56Xn?_ndmg{Du#xur~Ly);xk z0DAivOtSBb&MMrwUAr~6K0h=fFbx;`>Ga;18Fs3jo?Dn7lFeTdcF9Y4lgzGfEgn@4 z%S9~8Z$M@W9HV1>NH<-LcG++rk>e5)OtNr+qf6}}Rg&h?$gBn##_CBpA!z&G9MGrT z)W9?1_JJ#!uNb0C#6i-k*Rjho*N_|KdDiW)B6>S z`pF2AfSH#j-X6&Dv!IyjFp}lNlPfvASSoG~!_vE`+Z}Hq2bc)Z4V|u2Q7=`a+P`Tq zpmL9LRb{p3n7hJXKi1cN)vdT;B>+ zuA05`2)p)(!Ar$x_a^Q)ay-o2R&`MV7ewA*A#}(mHM)R{qHyvsm8G@wydQ&zh=9NE zpOnMVKdnl++RJWZ2|Q7qh!_KUy`2naCa^YIA6!TJ=W|4_OK$dY+F@T#`f}gbU7ohG z+55}(R9?)6CCZ;cH|%fF1kV}@BIfl!DUn=z|67kiqD{x)hRj2Z1($L)L#6sq z5ANQXa-%KtW&4}J0s0=b!5GVEQ~XHXL6NK(wlBX)Vr!oKq%ge)O&RT*m`hSt=r5Z= zJ&Gy<1_*eeDXD4OnIUP>5e$+1$b7E-p4WO%Y1L{4lh?x0MmpgZe z<4AYm&--;SMFe1CQjG!XnN&2jQQ+d$v!qE8s`t+> zAI%Muu(2PYuj`;gz*P#R6#ee->tapk`P4mXwv6~3^ZA(#bg?8*Xt217zYIP++&j-+ zHGMgd64-`X{|%e16^%P_XpZxRvENc_4q(%va=?CLSpPg}Odk;~5!ej$Du=80o!-gl zs32=LktQsFXa!;KiDdexUWPlwzn18ZL5EVT<3;rKHZUK3z?fv0OTZ~*~ zELCy9)gV2FRQL>SAF?ZV=036^0R*i%ozO)o6*v}F%xcfKEUvq zy5N3+jgl==N;6+t#EWUu8}F+w(V3q~6bkk&cwIAWGd6aovZBr+3uOsxp7`O6s2V{b z==W$PL(ZFgQ3Z#X$ZT?2u>o`)s?WVH7!qM)R9Bxmrb95r^QiZHK3$)chwS?SvAx>V zZmLHz!If2bLEo zP=9v57<35oR}+CjqQ6ZkrF(KxSg281f(<6Zps-Zu4!tXL91`f&EWXb$xgR=7SFMf>q{+0<&X3W%t( zcb})PtqooaAEnM6=@z}nDy>x#1&nr4bjqJiWktDt84NEJBL8OCXZ67v%rpAj@4Rb< zT;P`598^wXKy94#CGb|X_U0!keGk?6SfYOgVx@(rIF>(W7mj$oEUC(G=wt zsEf6NfpRzzj+>3nuqNoAR=>f?dL)x_WIB)pv%!;vVA63T(K83c*?%0PjxQ zRuM5eKlZ$&X2Wp2F$dY#()xd|C|@`h5U`x)CHTUt4{S2+r4t|Ja8XZPz&q#RHkX0* z&oP`7+_K|>7dk$JDe&3wRFl8`n`lM?Z(~j}W&HOWEn*R*t_pZQQ(8OVvd|~HLE)~> z^xPtopT(ETi=%Q++KscmOMx{|SRh1MzKtxz8h=9GkZRa^-=g+DXo>@`t#T3?96pw4 zph^Kmi<$b=xy00c_nEW1d6y$eHP@08Vvr86+aTR?mh0|Q)??22$;WSnkgv__hC58X1fQjMit6}VN zFCrY>@Q$nTT!kUiM8`7T(zXJ&BKQmVs6@^v#vRcV68#eAml&I7;4Bs&C)qaRqLKas zx4HGW9(}4NR4at^+hGy4Q7zMBkWx^KemcHk=c`^gn3busv$ZjQ{6_tAie+LskAnha zj9VZiEN}n&EN0uu0b-krU1dZek>ktP(|vDI`tSC9j~Ne_Ng!QZYk$*_T+GoSq)QZT zbsnc@wY*EMV?(t>%%Ah|e^Ex^X%3_oa;jV6^5|0+?wY!9bdfOZ7!$mDKMYoXVP6TT zzgyb3-cmh4*}%wn?M3vN_Kw3;ce_JT&PnDINb>aYs*I>9z0*_Go~c0)F?L@$@6#GA zMvo!joo;FRc~R^FMKMp*m^{SD!@vo&Fj~EE$xbJfQV~bHJuR$_U;Lq4Vyc|aaW9xhAV7akD7nqETE@)Ie`G}vzy+7P zj9~nMfRho~5T}%TDf(5@c|kVy@T?zxHqEKw!rLPT%}_0sjweGi;OMT>Oj&H1GgXaT zc*d}z@-uOJvK_EfqxYdy`zD-c*6 z{GJAR4K{sTzR&2(ZwaJc>RyiQL2sBSKKmB)HM)(&!*;X0aM@E)Y~v)@=rmlt^2{u8 zTls=eR>Bd*=8H%xzd8`5Y-jsS3zaYr_pg(e^vXg`tcMo@J)cEBnNohPL3CnQ#iiK>C?q2g%oU50*g*S_QbD1YCDxbh2f+ zXT6QMnD*v782GU6G^HEB{#6{{wKEA`V#Gc$ajtMd|ll>`tw5b}5{E)uYgdPfDh04Hk^G=WI5s=xEbE}fEg8wi1Z0E21=fntO()9vI)*-Ab zT|+JZDBD5~I#!kE)<(fbS)#>MpvcM!_kY0(Mh#T~#3|s0)eMXJ;Jblv8eOf9o)ptR zp7us%SQjO-DQj6@+07BC904|Yv7;ec_H)Ks)?U6gJ1L(rK7BZO=rs`nRRG^24X89yA?eC$*>|1J zS)61kIW7oSd%oKxZlL(u5!mg*O>&{7E^>w#yLa`qq@gy^O$~voU#@pO2-&I(;KW{H zy()Y``nmQkuE+fMRDQQyxIhYpcOQN06?)bQPxmQn>1*iQtG>g0*)e?7f8Bm@YH2m> z9+rb~mTyOxL66Bg4A8rxookz3Ua>Wg3B%Spuu-FRT%dpmgccG3%0@Wkm~-UT-G3?ZAmy?tP zCn3e8^Zt-;B=bcrSEJyXG-oUDL9NerJnF5jM(;1vgtFD=3~kns_mZv)wOl|Jy)i|` zF8PkpUw&2i*LQ+6x(JqB$rzUogjl4IwOYMi_r;cP5rdn9Z%Ia8Z|jo-hj^$ce^Xtw zlJp>Y5bEO#UV|74rGK(fTf+6=QjuJy>B8ox@pRpqr8&Mk35E-|XRl{S!M7-JT_nr; zr2Zr$kX~#$EJ5l~JmeDbA}2>ulUPN#N?AzLMTZ>RYQdgdgKJ=GOT^VSeL3LY0;e{k zB?64zexgi^-;KFwd&0&d2F!#k@&>XnZP{gCJ z%DF^2zqeL?UunzXrbZ@V5y8Uz0VcQ0GTa;n{62}lyJUp^jWfooWybgI@4!7<^#hB( zwKb)y`TgW#DzrY;sL_)Ro}|018Js&wdh}2ChA%5dI>&7!2ht0 zccH80@ldl-k@o!FkQuC6p{KZ=N+AH%C>g?t%7s~8wEm`|OV)ZfnEpI}9ZG8Gez%;? zzm)nrn#4R?zC2L9`YSBQEs-Tdm$pnoaF*|Ev$7g(kg%IkG#!qd$QAcg*@fU_qNk6< zFKwQpre2_+ zeVKPYFryTlduygBa844Vp>acAC+|Nv^))(f1ln%AUadj*pxp#}V<#rV1iZ{{*G@d_ z$E^yW$EWj29c8w34ar~GyzlrIsoj072~~KD&B@TvxDG9@)j`s%1ARt>Utq z^2P$Z9w9;S`_zJiVLwJ27?sJzn~<=+*V^$h@H+j3dyc*T^5LfWZxX73fp?ewq5H-t z1NVmhzWr_E$#%$HnlUNcQZ`Bx%HZIy(Q_`Zes{QFU)oiVPKc)KT_n3(e z#&O|c`pUy6YT$YnIKf8M&AKcz{<>b-4amM<8`NcksXM4?@sW^(A!qSXudBneFR+N0 zoD?0_=trD?s!*#QO>~2@gn3lARo*lBBFEyjnsF&?nC{za-BT|gmAO_tar*{UFs+Dt z(J3)sFbOYW#mMFiy*_%zZEqq6lyr*fyfC!unPOk39PXP-FCB8hTgr>I;?C{UA0`TL zZ$sO13PP}tXI-8UbJCH#OPcD34$|cpM6@J}0nas{)o*76&uI-y#&sb1WunS4q`w~0 zxRbb(yQ{GH4E&}cS|@Hb3+BF}XwVp-On#ZPq?fgP?~dgAOJldYVaIWjq2G?i4m4(A zjmn~J#(}h27$z6bfz!A*@(fX6BZ-M1&AIa>rSWKs_PdM~=9-Jam#@HA!eRGwRwNMiL#2p{E zXDqCPIovVJ?lQgmHTQ3y-k-mM*}reAuCo3Ft9NLW{EWmg0Q|`)C?DQ@ZB^LGOrlRR zb1#_t30QP;rY)k1adnlg&{)i{gw1wY8J~AbxJyC!|KsVqeDko6|tH?Ctl|`}6(%?cwowo#(jk>$>ji zzMq=Sug~LpJlJ|^k*e9FE-%!MxknRaFBmN@+`SVcNXn_boYWOfdYOkG{@1Onq&RYu zV)lCW*ag((QZ^q8X0qw`#9hj@D{kW|=BZ*IS>rz8JMr(BW)1GHYca4uEi3MEojcsD zyDknnbSc}W_Cd&t4OQepa#_=GBCeuKW=ykOWKT*(%U>Vr&5IRLo`)`zD1b0}1(l*MT*MW8fmkn4v9<0x zDgy(oNVDb%B_Ud`l=?ME&U1ThdNBKhga1DJ=Mt2bx$GpL996SvewoWidwb!HIf)WI{oABhPra zUJHGEU79o~*B`xTM^sjoBZJE%%!_Lr3d!)!~^_Vk-HNdElQ3sy( zBrLp0n2jc^U@O1Aj3T&6jXdo27QbL=cfFXgA7?e){r>-~jwyw9q{~cpue!h`(K{sN zH-M>MV}HokKkx~Ek)+8Hy-T7&{qY1cysMIVZF9Fp8-6b9+}Vy~BxkEzxzbQPFhD$Z zqfl|6J!*)`#blF(y(_GkMH*zCD#r~@)fUPjUPTPOXb^iUeBAmhP+_5B6vyUPI(DM( zR()(GkcgnlU;(jLh{Tj5prOIT4ufjW2?O4cEL9i1;3b4 zik-V>5=~1__DtBzXY_|=lS>Lk<#me$&O#Qsy{d0dc)bSWcD=D%6-ypO$s(4^+NbnE zZ~xmHDeDow_z5Dz-;R`H9N;J<)~cVz*zIJj@LIl8Br4F59Tbt^3y?pUIu(l^@)V zKH4a3Gc-fbNB`^g8t%l&MC48U+6|#Mv@3914bVO0EcM2?Y`J(+mp&K?P>ZN4jb>ya zRi_Ut-nwej6?$b>Cp@n>CB#`jW!zOAmH1dD7P=+?uLo%M5nTEV^O<)^cI@XQdx_~q zU*c3`V>NpL<{R9{_u^0R&lP!1QYggTcspn`o7_Neq**-2PlSpz>5Kq8N|~&YIY&zm zrvtyr#*rpor}@lzU*&EI->feK2vVWv&yY|6a`&zPC-|!l-B%8-%qb+mt;t%5B4OY$ zpy}sH3S#uJSQZnMjOG z^t%xX`3glpeF9r$SsrInmhDhdR}%!#{9ziF?r zfHz)p72^i865Jx0<(>3?u6FE8LsWjEwc&lY@J^V0EiRwN%j?g?m<+eU&O*IPJJMY7>y?j~=Ssx=L z+7uHiQ%eM<(A+*C80+5`g%OQI2UNRI}i zRC(#yE%tQ-ld=d;v7LM+4|bw>Fn0z}Q;C?sn8 zn6pqp4AGLRO^1BK8Fg`Z>8>Jup}|bbmXg4l`jYU!%#aadNU#v zONcw!9`+=Y+#nv=${|!nlDedR4nV3H(k5KNEub?`mo=By%n&T~^QSjFQmmG*R;*@O zi1>Pod}MecHjZtbAU_q&m`D-`V-}j4Ih9=x?Nx91<4U^_(XbILAvzLz<@lExkcu82 zNBLS&dDH_+Xlh&{sa(Y{_;RtNWsWRh)XJkSo#(p%KV31 zEfQSS=h40@pH%S#%`uR*u{$;oF>A4Gaca;S(2uU6bVgbNn@KX6?Hm;*Ik6ZOPHR=4 z4?J)G&kJDoW<(E~C{O0qXKt1pgV9qlZiu+b~k#`cKFBVA&=Qp6rT^UrZhra z*Q1;zeUp9*V8&9$Z)-1*1HR|6N_vv9r5fo;oWO5B`tED|vf1-8QzK+FW2-IPyLT$u zfi$%0_l1Belsf9ui)F*Nm+z|yYf0p`&(f`Gti+ITnIcDfIvz=C+ldR9( zVw*vz3Uo#guAe4oA-<8+^9wUpR<(~#bS~SzP%Q~2^K9RcD>D3RirTq~88dy|BCPdv z*9l|+0(mZLX1nx)PZ(1SG}a<*cp@HO`@(~Yn1%O<72GlUJ#ukl`}yMCn?zsYXMf9Y z4+o7`iLV9aK3&NfwCw-CY#g6YIWQRP9FmF;`G)49a4P2-pg3VnyJaCT7#H(mK6NMR z!Q)7ZGhE$Uo>1HH;(9qr!q%o^EJ=%%@n#~P1|)!CEOm_~#dkJ*kJbKh&7uaQ34f&X z5p!&m`yWH7Jyowg>!{1Qf(P2eG+95i2`Igbgb>Z9Q{J_)hbp8s0nl|q&3HMG zhb@*HaVZfBD0I;?;N-tS+53E)_>NX?+Ax22%T!83z-w=95)Tva4S`jg>fBT_AU8Za zC?OJv3BLH_8f(2NRN__;oP(+Ie?f-TENb&L#r$sV zebmk)d^Bc%G0Y6@K;h)A$vz-=!JBW6d;Swime3S)S_^yi72|qrn*}c80qery{YZ}+ zA@y?R^A<@aj{tjPnp+tjOtgKxI3U{!nY1>sto`Ud%}81^%xz?{vOY9T$3g7zrf8Kz zaY?6AT)mz=2mMah+a@I1ppBCQDo+PHW~}Ly6S=7R4nsjud*H{4numtk2W0b&*i=fiU(fGf@4VsismVGIav;9PIV(#q}d4-H^DJp*gp4j4xW;)VgETGx6J@hCq=Sew)7pu|nQ(dA`Tf=5~KMGE{ca z%LJADV)->jJnG%BdJoLmo6G}&PSoJP$K^tLO4pm-?0MY|ddd;y>msy>yI>w_@FiZJ zsyF=6R+~dg%$>XcqJ~Wn`JVbZGqL!F+)RGyvx7j91v$msy*BZr)$*#0m(N;Z=B7hx zFGi}3s)36<5dViZAaKA~D@CwB=3lU=D#v6*yZ6DCc|fqZ?Mn3QkXt36CQ)SqqmIcE zV9pw>nQuaD7_Ai=yFIb7mV>a7nd$761q)2w!QcB9@}0QW_Z52OWX=xSN1@QVp1VYEkUO?F ze=)y4jkbaVkNQ*b+~wz{F0hEtpm^Q6gb3g+51tO1=Is;uCp&DTl7MU zi^OsnzzE3bnP10JsATF9(uLgLgW>}f(Y9!-sCT>XWeQBaGAwc%xDkV#>T_?*Yck4u zUp}-a*=F@pwXgp}xE`U^Yc%FiTipz=(Zql}2s9Rb~``Bi1D;>bZV zd-_VV3>jTdA9v$bYCOZ)KM5lujq7*Of~B~?K64kc^_?4a0Mheto(+F(xs~#=dlA3G zwpcP&7|Rp>tTEO)1)KGTJuTG1xN`9f|MEar;gv4~*$i89VziILAF~tr>prZ>4Dag| zE1OCs!({@=)7gH{KO>v5i6R82}JF1V$6$?mi+qH7~$$4^kk6_#@ zrfP7aX%b!MkE$POEgQetb&LKPIbCLF&lACw_Q%9&uUXJD9&Ef5fZuwtJQnX(qW;7! zR^&m-$EpN?`z$)na&oD(83u=8iC(H;%&j_UVcj$Ifjg{)@vpH3(U*9iDKQqFaskX%~}RYsf!5*zVhNJ1abZ%@QUt+wUpMnw^)%ET>v zSe;c4{dH|P(hkPTlqOl%q1Gj*zv#+>dP5t4wTmDJDv}z5;@zZz7KLg(aHDrv7fx%^ z^oI~Q6Z}K4%9l8;mCj=GqTpu|Kv$C4C;ZD&F&fY99}3ES+`zvFi>&oq1|WWLp0SiP z0ARB#f?OFm#I0KQO&`yuIi>>nOuOy}Jj~qH>8Yfbmwqb!=zNu(%2 zb`Di=h4?)*4P`v#&cxk91*YO85h@$oX+zus_jdOq0qA$9Cvl*uO!oVeFt=Qe4fQq& z!)^ZMHjemjZFBmz`9q1I(^u(#E6cl{0irUAtxyPu>zbYf z2Zinu>t1-2O7Fs)s~9q;sM5*1DV%iMDoqX7__<2PYPYUNsZobjp2;V-$5( zyy;tx4cA>+QKt$O6itlBj1|QQ+B0Szui}@B*`dpz;P+fLJXvtKXA8f49|U~C(lJBc zownI`7%=KD1AR)<=RldMfN=1RYlUq|nbLFNZeOT2Gu|o3&PM`vGA8DSqr&^1NvK%1 zvlx*w&~NOhxc-SYVCY72ztb8tf+bTLnLQ@fr8e3w1kN#2aGsBhj|Hl2{7pYxYn8fh zb*tloX>Cos%GS&`ZB*Ua88#HIZ0btEuR@P<;pnHN-#5g3u3xE?Rk2VwUi`!tU&FG$ z{rBW)wwK_#Byj}Wc`t7j<)JEqgj&4ji-&Ne+yB=|OMp~xWrnjXj6I;Ep>WpV088AQ z*T@$I`*Lbe-pRUW{NW^yo%AM))wx}q=S63Y3pY%+rOFk(842pKZObN_C+>J}v5)az z$aAf3PcO%4)dlF?8IoJ`(`+K(z`O4IvHd@Ht)TVr5(TE+K=DL!^h)%*0^Y#(z2!-$ zDs!3|x3z+3OKH7`*ZQb~-VCEqT~9f7H|+esRdSwf$17p>>5vr#1?NY!NKpXt<9^r^ zf4o?B!fZ#s%W+ddoxE7)(1U_}w^9>;$!>zl{Qor;PJXRN?59^KE1Q_&a%G*8$Z>X%Rj+!a7Ieb){^~xg z;GYtS;1{*ce*STHpIbQ-_i=Y(y4@sI5_ZI<_;?6g_wD$}Xg4ZlsB}?2t=dbA<6k|h z*GbX!Ue73j#-zO;TEUqaX^RTxbaY%S+)`ENGFfX(9BrIp(*&XX6)Z_Pj0f9D~m7S?SzV(!RAhPP9xu z%XQD&a>jTJ`|NA{(66DsMbp#JC)B+sT=%sq4}SIHmmk2K6~205#;nMk^~+v^ic=Tm z89YYTFpe4Fmy!Y;$Moc-twKnSXl0<1M4 zN-RHHQ%iWCx(32FWvHP)+P8p&o4VIBT<4aEKd3i5=_gS#!phF#119FYicbjQ-_ZaNqMQlsf(+Gf5N~DP9;EA9ebb_jXleM!7yk`# z1^d)E4&MMD4UJVidfWwiO7-zgP6D)ML?}EmJ*`L1T;tE}1oa4S)#?cHF+e=d{}&?| z5BG4h=pymn_SV8&yFv=*U|hLg24t7qDG{PRxwJb+zC&L&4-y=Lb}tAOu^`i$LFR0A zDXH1)3E4DLCeOa|^|C6fC|@pf0c48vyfUamFE>T|%E^vUI3H%ugNnK8H3k(~0;mBE z8QQXxEuA3_>icI4OsXhpmKTl*xIAp?SHX~WsOV#BTo2mcL1r@)mK(PUeFCwh?QKhN zqra+;s|(iS$4hE9=|efdN_pPrUiUAyndzK2UgWf5P#|L^R!2lkV(_Er0|iJV)yL-t z0sxc&*4sWD z5?s(Ds5uOe0I{>3eqimQ8;Z7TnI_4e!jQgq8g3m#$5R8j)DJbH{#fy9GkQ~lBUgF> zZl*W?s;t(Dr@aBD)h-_a-`hSXQy5^f!rOy8*u#nK-n?ab+mg;Cx&t?DfgQmxLO1Du z_OHQabo;oYmM}S_;QR(cENOZpFdNV)X^LW)Q$eUL+6r&i!H=4wBfrWkNbjq=uAzks1guIvyELMcI;*w}6&JPO@iu{)%sV zLk5b!;eN)I7o2Vr`H8l*OSmIx49!_Fl-(_lK=Y^bJsvn7_|Ed3>1_eb8mUX_fM`iX zH|ZK}UprF}0@2Vh{eR=Jc!TnR8TnSo}{#o?C|^q@XMb%R4MCYADLK(R7JS+oH_ zEiK0Fkm#yl!$F3Lf-j+0kXvdm7S7^LC$+g4maCM}IqX2Tz z-#x<~FEJEw>kLT<&ekiq#tn4?pHUVkrNp_+(?k4n+YPEddqITlN}!03f$@%}-`*OG zVn|hqw`O1fK5%;86*#|wEa;JTPZa2PY#Bytqb9{->8~|&2Q|KLjmMolU~{0`2cm_npYf-$+gjKm|Fy%0ayoFs{rD39y12LAA#Dj#}{Ju4c3`a zIryWGBSxmUV-?Yn!!;lW8s-CH&3Cr60aH}>!!Hgsd<>-wWtfm@pt6M3!~1r*lLYI& zy~FQCZ8u0*+ooW@1JM8sWC^7H^1Sr>CA?jhTj?}2OY=4CU-WX>`MohrdtWJs8`b7)Z=+K%l_*Tr$-o((*M2n#Zq7O366u5QM zDY6$l3`5&tOLvLG0A)@fh_PMpH#qu6WzoYg#IM8&3-TZGXU<=zj zca2v!G@Ag5^V5d+`=RXBbn9)N|+kxm1oOwPUR_Uhy9UVD*}33tsV zPzuWV89?mCA00z$it6{AyKO=tFy{&p9p(H+zR&J|=dS6fy`fw_XS^v##G>t7c~>d8 z>3i+r)^Y$CU4yE?oQ$9+oN1C4P7>>m1l~0hAP>F&gkRG?iy@Vjos=VDXXQ7r0dCGx zU?haaUTX;*9dF?JOgQ$omXZ$q2}bsh*<)Ys`%l&A^FOTVE^{3Uts#PvB#J(2D}0OH~y%~ zxE}x@d!d!TDTO%Ls08A)rz@|8wL~|bYjn>Qg~1;w+vfYvNcRf<95}sRrg3IS!I~Dv zpp_6+`TU0)AQx^czQt9llS^CxuY3H$M3Zg~cAli93|ZDJ);#~5jgNXX0tC6Gf2d=7 z>-b6CGYZRIYol#hBUIo7UCVV-*9nH*bEov0O=SvIjA@$iQ%2$swN zb`3Uns*12RJ=VF!h?5fqe)WK7C7TUEvm5@Ry%YPmwWqR8XBujQu05@I+IDUH@cf4} z*kNC^3#WyBmu{s%rr7lSqvEeW>lt|XvC`yXVEs#8Uc^-VOU?b{9D9IM(*^rI2474{ z5#PV?7|t1?>ILNez7$m-H%*u>RGtB}% zG>aJnr5~@tDG>G@bX`jVNra*9>Gpbj-_XaHi$yH_NI1}6Dz^BYZ_9bufX9`9xLxst zZzZ{rWVCVJX8aGC>8ol{X{ZKOU%_)&97kUxsmvPJy`1y->qRVII}G_&zE+JZqSox& z8#r81+Rufd4d%IH7g$Jl$wx-X_Pl(a%HNFNfaA=K&cZlGHaEm7Cc-(C-KQhTD$R;z zy_|Zpj5}rT0kP@vJ73_V3QYxs&pJPoFEpf6kXTAcVC4m*CH54WPSd9WG+RU@Vt^!{ zzVDQ^`UWb3%M;Qnf;e?197Y127z7nJ{2fg|4u?O8607j^)eW5}?OWoLOldSeh;{JS>O zZK-d+cjGfIfB%M;AhFX4`4Vn!{Pg1wT>(;1?DCdp9u*Kh^zPy=GY=2Wd%+3nK=XXW z?;wuVY(jt)7VZZn^epd*AzD$tfoN%0qx2w>;W ziBkZ$bni~Apgn6Qc;x^=kg1$um_RqJwms{=15CJQ3Du)d$88#bTM%uWJ#P_1h_A6V z43Bq_#HkLs0si`qv5Hn&Rp!igGgRP|B-D;>`DO?o*Rm90`Yk1H7sttJ{0ocjE6y}j zo@ZHc>{DGg1$|Yn(%Y9nVzz01x(HMEK+0)oR24={xfXJQm*yi@#HKhh zacV2v=NhG5gXT&IGz&ge5Dh6@90_P@(O=Lh`LzOKJ)%89_TgJEd(w#WnHtP{fvAWn zS`nNA7qj)R-ylf$uU|0+(@mBDfJ|Au!&l9RF{#%6qRtaguuMX`-(8 zp7DE$S~yJWjvGtMjmh4DMMPvOIMC=Gx2gb+#Qvm5FIq)_7Dfl}^~%O!bfVs!8n}r` z41XfU{)n3?oH~5vMb>5s+^j=<-^FlfrIO8ay8ku`{LXbvq8CxmgO5++u>PlejLwXou|c<(1kU%j7KYikfBeJginI}hR#qTNtkll=?V>`memkfHm} zDQVxU0=t#6=nO|)UJt%=s2K40x!&%2nLJmcIB!G44=fI$@N+-q#70C^VI#z{<^k#3 z-G{qbf6L*Zv5GPCRQ0WvB*@(Lw-?C!j57lCQij+)y-2|`1_wLJKK|Vy0-#CbFPppD z&NP)BjYP)F@1HSa=jpSBkTM(;VX6Qt&v91{f5BbITnUJ3HlXSSss_Wc+R-N7v_lSS z2wu(&oXei+jK1sEW`?nfewP6a%=){Q16hI%1;rdi?iMNS-$Lz>jQZ0N0feS;H5A~h z>ny_uU|sAzcNo?q2uUtT!k6LWq@bx-#ec>+bCJztioGc)C)pE;mfUAGY_5NF{Zl`D z#fRg$_AGvUJKylW`loLX`0+8277SGa^Gb)Q0T-3pwWQKEDOiu!}!Mlr94m}pFLsBWKWyQs6%t02Z=s?V{F$Z z-6PzME9aIAx-^!QF#~H4>V((d5ZQa6(i9^}g2N*eMJ$NUcm5|eTmaGhqoSqK?ZR@O z2A$3mI>})1X>z%;yqgMGty3j!coB)UFhILoF)K*=S^+zxvT85i@fPV~ojUPAh=;paS#_7n_#7BBilM~X`x^(s}i(JYK~yxf2wk%iK8CvPL&e zl8Yet53WiJv7Dwc~eI?9N5+D@iwk^_w9~TI6%@=Na5qI&*9RX6p$CKqXYh?4% zq(_+pxE@7{Jt2g0b+>R1zuY6Jy|{#tv!{r-0Hn;a_GVfn*;(Zh z2sNpO;W8k%#}v~jiuv^iU^ew&!Ov%ous<>#gxBQmfQk(jP`t1TM@FcfQs4^EI0JNy znoXZ!v+`eEJN6GrC&)&Ul&8YGyf>CTY1eI0-}(ZI#PZH%9tRR;660Qaeba+CVVj#0 zMoA_LKp+(0=O|Hs3JWx+7tkAQ8W1x2HQRhB;A47Lw}xdw`m93E`&C<>l`cUV8$y+n z0)YrZCtUyafyhK+EzjM2+BzwcliZgB%XW;pr+mtx(mzSD#rGp$)SO0sA1lSmJhZT- zRit2bxW77#UHf)jOQ0>g^!t0P8VQF>*@!F{G>AO3rz@HYHIwzBhwP@?YfD?uu}S<0 zA^rgB%IpS#hPCjc`|1z;2sjQXchL2!SH{s=ifN&3Yynv4mmDWWrQ$2}W;{;}BJb$f zmBR7z-{^~h6m48~uO9vY4;QQ%DRT!2gR3{!D1$hx8U1ww=t+*4S4a()UD4L`=9{tp z8q)7|G@Fj!O1XM>N&^|M^3^kS=Cn}pbv@(ygZXR3pG0;Q(ixgcllMME7wlSmvLRVM zbtNCNtm-~Cd?Mg22%I2j3VYQ8Yg3Icn~&yP_8KJO@rY=X=T$d#oI7SbxiircxETb6 zT7b{YNkdCH?A6YJH>e3wEuJx~>zd&G6X8-e{j}RyJ_{vwcVyl&$8Yc2%RHsGr<}8v zu3PNO_~;w&_z(_X#&zek7e^SNv0g6Z&Q$o}0|1_E%l zi!1iTxA>zbq{E1z4DD#fP|)U;uzE9s8lFjWESBtU{)MEmyRS(VT*~TL!C2Yp8{63^ zMMQFK7?0LCqeqYS{3qiHZ)mxm6>eQaZtE%r$NhKeVWsk7S&H1H%vc`ob`91gs5W7`)9e)TCa$pcn1I_%7Ojt;t0Z*ELB zEztl~5uvn763(!v?elc9jiyzku{!~;5bds)vgjGUC>M%~KC@Sq8Z7#s=LLy${u_R% zPi}_hh^)%Y7xX9P`|COFUv-b0tZhdEi!N0+WCq|`^7ZSewngc8_xp-Lvq!o}lH zm*53e`dmgk#{iEA0r>D|ye{?73U^xLEzGzn6MxQmsq<-}cE*+FkG^57pquSi`_{iU zAJV-Yv@?YFr5`wQ5w8)sfH{rv7QO@j>kRXAId22u>veI(zZL*&vu*a)lUWm@EDHDT zb7`FgW(m_N^Ii3PqktoBES?=P;weIO?2z*vNBhSt+(3PL?{4CIeSv)I+c)fwOCg>9 zgx%>s-Or}FAudVTO!fPd^F9Prk}VpEw*gy6XZ)HZWaQj~HZT_w^qF)<#&azucMf9< zl#&evaIhsZzvOKG8E6jdg9f8|g;Xwcea2g`~@VaQGx+z8{P2 zSj;2V;Cp0-_PN*BXafZ0bhnOot5DJa{MEy}gty(<9D*c;aH)+Q>lwxc+-|@0Iv2cQ z)AVN<{SMee9o^&8Q%~bcin%p>6Wd}K&!~!49;Z4yOiU|p4LW$YL;zgBX1LH~$$xQE z-uo(08~lKCeTpM#CCc$6d-c4d<9O!%$F}XtZ6h+0N})fyr2z(Tm*1_l9Tz2V z_t+w54rfdQl@@t3pj8?6U9|Jh?XT!m!ngtcXRD;J7sti%g=Ft_2!k9CGTZe4n z$#>0;0Diat{M=z+j17I#Dh6q4)BqVIpOZ#rwYdRu*Kdc9#f0h|x1~$BU~Qol+2&LdY&5KuhR%F8p63hW5k{8H zBz5UvoX%7nZ2i9?Y<8+Q(}mpE2y(PW3Rud?Pm z{ww}D7Pbz`GXZg)2w%7>%EaK1u&^O@9ujUz;|CXfDAt1sgRZ3)i<7dea9UGpPb+kyxP$xkCG-ef8mHx5i(di z^0j;@a!sG*?NemVsvZFmpDPaoa*I|-;KZkN-W9oEFm~Lvzp&gyBqYZFdqeFAa=uAL z6EJu}Lgo*UP)k1EWOMV-Zm!SQ(0WtmiDVZH=Bsx5QFm|WnKTZq-q_GmA%u*S;^u=Z z-g>+28{@(0y8fOA^WpOFk+{#*VjGKQo;|}DWdS7g8vo!g-A}k3qjMUyS4=l=h~zzZ z=6`NIAH4A(CIy*YQTks(sB`NBX`9>~yn9 z>x`8&eacRSHizc8+0HC&U737TEzhq%H@4NOTGL9?0oDI54nA$P#WV$bL)DlcnKE}* z%!k+Ym$6q1X$ENmOUYEcr3k$L@cgzLYp4TiM;$X3P$#jlK9sR=6kVLs%L>b-c>=Ha zi^F@!ea2zNJX;N|sT$LVXgf*FKOJ{*<&Vb(dO((|yaC(G_OA|mN?WJg--?}+>`89F zpS1|cRy3A^uL%xqqr1U>%kR3ueNv~B0otJ&d={ukYBVsq5YlDL^q^~=3@Y$`jOoQjvf65i8Zaod-Muz?f(as*nQrx=#klF;yu$_qcU%0|-}VFv1fjok4O6 z;sJXo#(!sXyDy+8CFb!YWI3rRkS-@_XINcN`4!7Br&Uyltlo)?D{Ii}O!E*D8UNm- zu1zG4qR>;59Zg7c$n^4gK1BsM?Zp z@+2}dY!_afGQjivkCxdZZ^qDwCZHsX;}@{u%LdgTD9mYQEgnDfp8#~ zGI|N%BSAkB8010nYMqy^Ouh;IP!IuESV!v4sf#P<474Rte7dE+PlMfkl=p{{aJX5b zF=zyJfjPOax6vHQn7w(09L$^_H1{rH7FmfrG%gNdNzPXSLV~CLqEb@$%D?mXYnFiq#gZ5?k!?}pn%jl1X~nYv z)ha%)YKMhKCGt|_5$4q=`S`n&iiLA~BEb>e@Zm&E2DtQ#uK|bfCJz&9yym*be`5Up zG&#HNy3!k(Bc{=|t*dT-e&IF#iwz9e;#Gs{kpBbPmuZr-J5K3>s1;qhPgk)q_dlm( z6OU7eB)pzpU225xX}=uz+xT`P;97FF=n1>ElvzK~{I8OFlctVo3nY{Xe3-*MvVly(Rz3Q30@S56-<9pYnN2m05(ZF!F7EJ6s zv-K^mNDleQoBj@k0ve;mB~1ouh-%^SzB%R@LR@JoSmWbjcDiMwkj5}Ugw@z=AE5vg zXjyU{V84*C>yg*|vSqHj6%Rt!s4D*&yby1U$rjj9zRkMQXlPB|m{M!<*;yvM1@H^t z)Hc{6-v~=|h#IPPDWa{*_7DQK%&^4?S^Zf~@}nZuu^gcHMg%sF*TegUz{I1JX?Q#$ zy6w;HUlmzVC}{ulSe7*){N#{qJqL8u#7UE6uvD~@yAB={Am`a~e5*n@ ztLa_$MjWwfyBrs8C6E7MCk}(&F<68}b;Lud-qXnDs4(~gzdiZ=fx~{SgNQJhh4BWu z@MMsU+}j!Vp!?Yr{+}#{?%@Ao}WX%TILTga%ee7Y#=S+z6c=;~6oP>80u0B83;!oK^FCc^(ucbBVh001n zPc+$+nr?h$f)5~oi!qQ$B-(%mK+4LO9W4}!@{YK_*m-RY!CSvZJ`u1Hbu5qs=L%3x zPkyp3h}^CV1X|hETO*)@qNlXe`3g8ww9?6R9vLA-`51rVM%dzmn)YXbpvW4M*=qiy zuJ2Ppyc*|bAaM$_F9e6FUk`z_X#J?uTMR1@l=~Lv&*2m31DE?O*RdZ;VhaN&K+v^w zQWBqjCJZ$IqcOb)d*+O6B>BK6b9Yywjp;Wg+!!g|3aRnnfmkaT7A=m~CYDCIvl@(3 zhOxQ+YBhIZNulHZKD<`4dw`uTCUKyvyu`jInWtbK%16BOlU_KCBj{3N%#f$xx&Wag z=Mtb$`t(Wce0nKAD=b4upa@RuCSW5uql*lk=xx2upC=GiU3Tm)mnpRV?`-<_*Ke`m zmJZ}zg77DM7pu8u2YHQPJ3?~P!G(WLf9POc`rvkfFZ% z@`>NS98mb3JTAjtJj;a`J(s?eK61Of6+SMP8?hk~3pckbhfe!u9%JV%PX%$W69XmJKhp;IT0aM( z93N2{`;rjQZls)2t1$1455|V#zN(Yo=r`7l1}{5)f3h6|E#RGk9`J8B!nQmKyB+j* zE5oH0^1)y+i2HL;udmSCFBk##j3|0Ms0Ce_qzc2Xvj>WJUY=@MS6+1EAawppKnL_n z70u-HbD75bOH`u({goF{L!6v2K=AyRiOG0(f<-riBph z>5%m=eJsY(l*>Tmmeu#+`v4uo-)D6x?s(WIQ@M*v5o zn;ZKAYHsr%*R5QIHHId%ai2|F0Zn2GDYAl#GFS=^`lOv}_S$Y&BY3MXAtKk6V@ULY zCyJXomtv9@SlYG5&1y-ul^+~kBGg1RA3}Z1FwKr=s9t|6j{vUt5GrnE?Z{d^c?#yF zb}oy4K22;-{lb^TN~uRFw^d9jR=B65W4;XUl42$+t*?sExpIE_sW2Bg*~LQs>msu8 zTildtRD4u?I4}8@c8ygqNAYTJy3A%@UvPV+d!@nE%q9hw!K^Ho{P`2eA*lwL7o_-H z)#nimJSRw^s3z&&9UEb;uEE-7@5|L12!Q}TJTDQ!FBrt|m2#&ByWcnZnDV|-o^`=G zr;qSo@7Y}K=N}BH62o=gJI*gi5h#+1!VETjO$&cLgG3-@e{kzm#0M!m?nVVSppXx$?{|<+8FdB483$AwdF#ed(Afe-rqB&&GZ=^V% zzIFS~N5E9| zpWLjxx&F83+>h;)A+AVrXO^JWy!g;&5zH?%coQ!;+sTY**43Qr5_gzbaDLFMbvH@S z;?qG_&0-`(8k)g47$vWNbLb8Q!*1xv`CZ;vlRm$fI$bs&2S}e;U?cDx@u8z%wT;G8 z(G5^{p$hl=Dx@Ok{Cq1R_Eul+S3ytgzq}m^-HaQ2$iF7ZI~R4F$r`agkj-E8Z=Mz$;G}g~fy0)=`$ljYWJk7vx{2h$7k|)I6@E|*SWZf8g z;nLnz@4@+mjW(kjxs`>H$=;3~Kk!0b?cWMH6U`p}=z6#VjT^B~*%(T+Rt%EZkDQyN zu*=)LWNvWEFZ^NFNu{u~2V))?b<^?3TC6FH>80Is38Nb}yn9l2UT%F*T4{H)7ZheX zdH3Si8;V?giAi9hC)kv;-I!G2L7qGvGS?7@M~=D(RyxA@{de@c_IirUPg`%LNt&{J zxjG=UTF}dU^^n!aZ;>Bs|A)77<|J$L{WV8#?-M zQVM^PW2dXgI=*=p)Y zvM2+-Y%@c?xv3k2C)4a*N1wU}2gbVw`|7p*>OJ*1@GpMmLc=mP$cpUsWvk5xRjI>r zGo#zxe%i2q7a?_|1KK@vqs9a`B%k-a)136?>1x|c{G9fSK00mCVdxSm^?b^pixXWK z0I?EQ^g6E`zqV3B*2Ar97dzY^8P`qVG4el(&~7}+c{lUm+l_NdKO z?PqqN=g$*YH5B8|><6~|SggdzmEIXLq8R@l$w#sAFG7bqt34(nOEn6N9zJSM@%&cv zCiPFirPM|qhg8bXw$aw7bxMEt&AYi7^0Z5)Zk5t}>iC7{)82FXDla(K7s=AdwI_4m zGgFs7j|&|h|0PA?clGINHZ+Vh^LaG|a`t>q&y$}KM7}$DM|;al z{y+WRJZ+@Ed1cbC;;QPav7|qYa#8o*wK0BGFi61y?U_}<;|W(A<)mgsXiF+9z3X68 z3`v*Y%ZBs2>T54Djk}7tj8sp{T2^XSLh)CL>UU`OpDiz8N!_e@g2c}cHEO)&uDd7w zO-@&9c9-l@Tfn6ciS2Nq?#MQ+Fe!WOEl99ftfQyV2BGGymGSB40xq$1+OwwHYtiYhCbar9* z%a~KF>q7ofaY71(%S2@<4RZM|v1>?Y>&qKp_#AvKB`8A~j9Lj^{{?0tzM>_Sr9K60^?Nef1@OkcjnHVbLMwq=Dax+HtHQwoo&R1M#(+$?dDBa3{V zZimjZO`3mfT{$=V3xW~j>zF=~d&3Emblb#i@BR%>^XrT>3MTbQvs?Sfx>5>m(k+H@ z|HT|<7WK`qVL$nos>=DG^&9Z_7tM!;F#(zP?7I%#=2YND9Yx^>#yL$PUkNu_r4X36 z9*yXf^CY(#Qels|px;tk>ZQsZaN;kQZanc-?D75j_H?(F)y%^DVEdVdzCxI;rs<`V zc~IAm^2;LK?n2uaq{~;aZHx-^%rn2ew)!pF<^e1~nRc&HA!M^$7zU1o}VXoj{ z{q@f8>h}ZcC3p8_VikycUYJ+iW{1tecM9d>tATQtL(X3~)8;tSbL2BQ*(y^4f)1Zs zu+YRyoa`5LKbJ?KPWzH7n2AxHig3^$xjn>mTvyev?pbeDpGMS2r1y~==xW2~mL@-4aN=jaIHG5o%6QuZFNHq^(rP_Z z9r?+_d@AT3DgF15;{thp917JUe)LN2Fde*`O}vW_=4b zJEwyw7H%{!h`(`KZ7 z8EPadG32dfhK|9iSf}hB5OHk&2#vb_wgw&6mUJp`_yUMVBi*Cka+y=fP^_l+jj<0i z%gDigAmT7^3@b%<9#-!viY-rH0!+p{9HKbHJPo5mL!SBJF7%#O?Mx_p<04utw%u8` zT)+Xjo++%N&#Ob&K|rZzh6b^H&&yDKc0)h3dETFzfs~dinY^g%yUE6+x`bU4&)MT&lfZ_8>GkYg!9_P%-#xlWn?B`U_n=Lap z-l!wknvlU7pKd;dJGj?m2wU=!gW&rdh?EBKuGL9`C45(NPUSod9HXBC>qy%|eD@nd zlVYo);_rr>T0f!TT$zzJ5y(&(ZSQYp>qJL3naTsQwP~0ORw$r9Oo%|54eD%jD4Zbt zG%usnC_YE=@KI>Am7+;G&%L+8Sx>%cc-Y6w7b>q+8$zzJP?JX{)0BjE8s)x3(B>?$ zQb*XlsCWORqSkd8N49)*p(Vdi(v|^^=EGjo7uYydNQUsOGXS$d<@J*3)+mjHlb)b1 zA?HyDEJyh21=xenRY{epf)Q)sL{RnG(q zwv^_}8Q#XBHYr*XZ)0;OS*>fc#J#wPW+9Z&J9=gxJ@N^hPt+3lo?W?vr%O`4q4y^i zV4$yp7PV$_WpsM00j8^Kc0WUN_e>-U2xhjshtff>Q@egFe}Udjvx<@9bmyZf-|(1I z5xhlVNxHhTq}k`B$h}*-erB^eJ`YK!&-l{ZDZjf;(u=F@>bh8(0gBE5gn0{QzIg~= zN~{JhBjbzo%)0>9{9vZ2edwXXX`B>)YnOBt|Lsb;RUo|!5$mNujtP_n9To4~oRHDU&xk{WqWAkfLRhuiKNNaqEFBGNk+;bsZ1oP!?yH_m}OZEk(YrIo(FqL)v`C2 zh^>Nd>*Q2*>=oG;m)*&fHi_B!3L9HD4o1RmkV-_x3rGsM$d>&OSgoE}yr;Sr`jUt> zPp+J+!z7Mih+pPl866&SCST9gRY~VQSz+|+$od-(NH5i%iYMJm@zu$!6@v5W#-0

    Vnxxgd^ zmdo;wwW!ZOKEbC|lM||+@>Qb!137X&fz-!)TwYw~9J$=g$C(GiSC}N5Eznh2rGQQx zG9Q7BxguiA_n^HxbN%(wWb`*&!)6h> zq#y^kc^6)6^S=76bqGOLhHxk;c@Uo$jehi7x_1t0QiOvfzN}kf#pbAHvuC|% zkKT9Kd%U{taVN@B&lZ8LlHlV_mLmrY!PA!|d0NHGQh#ON94Llfdj> zpk>KfZwkWRQB}4+OC%c8E~~8me&UeMSM1yB{Oo0Xi2V~$iJVsUm;GH;>5VXhstD^# zWD%4#+EZ$HPG{(dK?E`y?WLy2SEwe+vSom3-sN8jL^Vh6%tc}25CI|at>pL-oXmrr z!qw8r8&MgyGIW(Zpi6K>0ufd{JmrcbMFnk}KQvHU%q!ZoSR>VycFGfug7++_DdHmDo>=j;emRqgeja58&1rxfPjo>MpV2_)T8s($~qnRX_^2GEOO-@+B0&tSU%abRR+?4g3_Tl@qE$?(k;vEB=cy7w=-#sVsdf80I=KSOlH zTwq=Vi@crl&Aixf#*uZ%Y`VVB&SdBrPBmgSV6C@KSxI4HFd*)P_3_FdB%Fvcv)iQe zeCWb0^87k~Nm#~f8?&2TCHnL5=_!V(X=v2*!FH^V0pnG!PS`HB=Lg2Db7~Q?FUFe~ zbkEGn=!@@#&EEUmO3`k?_4U>PKaeFpeBG>LRYjP@%zHP;6y?0QfG>QiookvfBd{>n zdPMuDNJ3mSwPdfMHTlPx*&t7;6~9NzJVJ=vNF$Vs0b@7CnVs`mm>zxSEI|Y&aq2Sn z%J)=k8m=t=llQ>JgN03TgG2 z8))ACLJX1@&}>~BH?Drg;hr)=AOqhmb--2aT^u((Ff6Z&H=ux_H`k`MT;y!M3ZlQb zKX2kdE0-Bs)#%aAh~(VqfET7x32aqfLRtVj3`J=N><<57lZ+gV4wvGkYUIK^GL`Q_ z`CI|V%BAu8Jd1JT+_Za@2Psbhw0FE{M8T5pIC2b5)^TgNy_zCAr%SE%k!o80C$?Bi z7;*~DAsf8Yk7|N%ZJTojGu#2VHP(%9Wt6Zvh;pWO(t)-cziNklwv~TP@D&RRMkWJt zn#fifnNqSEVcjpFof@wFIBOj|vI*=i9*hKIX+BOp&x`nxNl2fmz~p9!0%{|pKB#s6 z;$F{#7#xsCSYB1q(CQ*OnP>6*riM%f8DCck#VQU2(?Z6Lfdj}XQTb;>wTKs&;3q%T zRLp}Mva7IrdkCl`*mfdE}|us!J;%-uTpk}SCIm@qeG2Q z(zZk6Sp?CjGg!mwP^T=qb;^u+J1gpvrz5sxm{>cVSoy=dv|M#I=&pV{G_kM&8kyD9 z7df@ys$faM9q-fjp}dcv=4y%rZH5W=vz2dB4-N=bgb9Ra+)P}!jvEbutZRZMYVJ5a6YQj2&BXN>s zucfIfz^U-U8Kx2XVPNBq6tEi+v|qyc?W$SIi^f)lINNQ%Y~FvfU&pt5fxKU6sGKkK zbJ)zPj)v@a!g`%rl3<;x(V7vl_{zc?)h{oytZvJtYzd`k3OT1B9>YG_v~Ug`+$CZy zx)?}FMf}ySWpj_%&?Y@HaYB|GBHOF7W#CR*GB@9b%Xn`_%NpQIr>3skOQ3h=PiQe$BIZ^<3Wl(=w+Tty^cAt%E0vL!$zu z6km0iu(&VPvR5J<3kaSYztbUNn|Tx(X~5t^vlc4dvMobJ>l9(!zKQZeHUcE0@+G{u zM9Y9No&Lg$y-Tz&h6)7Z2?HO`o_;0liHxG2&x+JA)iQcZ6GV6r1DK_3gDPFAR|H@# zfNnc3VRysD?0PNu#fT5S^gIyo0ac(4@RV$)sX=5B%^D>fVVsbApXyHBF$oq55u5;D zrN~aW0WHj8m)2|eMA&6P>8x93`K z@>Dh0iq>a*Lw@OvPOmp3jmMA3o(B*XTa&Yri#y`(x^hV}BC*|qSlxkg?g$_m!n{xf ze)PP&8*+FA-kO`bcFUi}!xma!KH1AWIqbq>VE9<5gWtkxaf133TKMs(IvUZ#*{(GLNdq=*r^W0N0R4gNCMl5e9Azehwyr^(NIeYdvO`(6bpdPtry6)T<5^fZrs<5vz29<5od&nB6ZB}3oc*BPUJ*fDR>bNv_BtV zHvzHL@>b$5uR`|61mU?2PACO5Q3K}vx~ipd_kjlC8-&*ebj8a0<+Yp6M70m?C;bH7 zC}^C$MQHf;w97}-32|m5z>v$hB|!7uEEb8A!cSmldmg+t2b0uVHIotmA%<_@ZsEmM z@3A5DSIZG*${fxQY%oVcKjW(xzZR*r;9Aqmb&P}yh9xl7dC{duCwPvfeVzl1wo1)&;XYZ&i)CX@ClBGy;W zFVV`OiU^d_cAPHmI0~*1Su4KmJM8VEOiq|M=xn~cE?6W5lGwO1DV1JZk{=%v{BYr! zU?q(&j2Nyd_>~45r4N)z{g$@`Nd(PwT&{C9IjZ3JF@YJG&LUJ0GGt=Qxpa@L|1^x& zq?ZWC)D@5Ois@~~-eCgg2hRbfgHuAKw9W2=fK%I}Eu^h&ckw~0;+8qt7tjyiiG!8M z-6oum3S&ro9h%CkIFVUz&%?~ru4Sh2K|xLEPG8=LiWnKbEHH$#<20E8P{q|#h(=X} zcAlTwRJ1!4_(_*F%4ohaf|Su2RB0FDUaE4{Hbc+dUg$4L4%!&_lpt>B2KXXlH8}Zr zk+tFho&u6QSW?E3Y6S)z#|LZRO+3Fyz6WhKjra-mWmt4*LoCtY+uz=Cn*kXX z!e=-{_zTrW#AEFhXuGO5#fI@!>(~{^ zt3j^HE;T$MZMx&7#Pu~)NW)Bn`#LQO9|HR4zp#|NF+`A@@}<7lr;iRrQ!v;$&d%Mn z)La=>#Lv#%#L|hyhllxr0K8yad_J6hQ3mg}V8f00Je~#Nr?v`z2&L| zK-1?;h@U}0Y;IhIbxQ@Z*gn)Ij1E|U-?H;HA$1$Y(!#c}(x@O5BHZa2+(^$zh4!U! z892CGHtcA(q#P)e9*4bBTq%6 z1M88Sp^kP-(KgcvXCu(oGErnM5fBMB9A=S zM6-aNn_~vjLN&7hz-r&)b9+w3P~??*)bx8Ykz1p+emPn>i4z|iR$sy$J{)Oa97eK} zzx*(^0jIVoi_diD?^>sqB>WJg+ENfxTs3ljTvmAkJBm#OD@U5t_>7f0ehydmdZt5g zR>lTta`HlKEWn!FG|mCU1uS=ZStgLt;hj~|1T*7aVLQ{dKPbHId!OQTf9#JgHLdQzErEp4T9`o zQzyt;6=7pA(tGi~)*Y%nf`C-PPXzTB??cOX0>0~7lh5|afNXSgF91mIf(>O0gA=?j z9J?3jzlmU3MDq(A2T@wc5mXdk?l)JEO_8l8*A|k>`~G7VAWsZu;!Rd0T}DqJBg8IS zINdr6!a-24GA}eZUl5M107jugUItS|pcx<(Hm+MTT3>CLdj;F#7Yp~9S?Dt^aX2;9 zztRJZ;(h*s9?M@WYhh7FN0utAVfxU~w&(R#Q}2P>LZ-7_mQ?!@_SJBNX5JjvJ)@%I z6DBj{g`kBbiZEwLV7#y556$<9bGP_b3@knI8r%|^MN%wV5+zGE0D}DJ(ItG~a%oTg zlZHv8|96X1Yvzh%EIP3t2)P$*2;G=z($cRdshM7?y(hCA#NjJ%rH~tUMwMz{B=7OQDj>P65^jwYd&t)^%p=x-AY3j)gh6_2F1mx2>4B zQI4lS(2` z!gg&=B@hRa^%GP+o>Od0Cf`f;HE$3RyC$dPpf)8*>&sKpH0UR69Gip|OS1`I8Mt=- z^cMGul;zzcWZWxBenZ5IMxog3X!Xq%KQ_l)O(EVO)O~-V^tLqJ7JV_j8+FObnasn+ z4_qT&awTrF={*lL^rur*uUrI5zM;uOm~@tKI(H$@``qOIv7i|mI)>+8d9dvL63V&h zamY;?8?qx7uTMC!$KKDd1##au>OHZ4Ciz;IdrFg~epz7y4TbYehHyqY1B3+1m@TZj z-aRnTQr2hQ2L5Z=Wyet{{DgL&ssc`R4E&huP9L2Y1Y#i`wZjf{B|HYH!C}E{V!C-L zeuTh#6u!gW&8c7Xv5`-&+U2tv4-tAp8!7L!~QS9q-J+6Q_L zlSS!dP%HzV04nAtK3+c>0 zrv~zzeL9=X<}bC}e#Fp)lUH%inX(U~7=*=7235JgID9UzKMybji|?wvnAFd;`H0{IMkRkdr31zp_05-zG%HzYe{9v zh_C1o*~%d8gdSd!a29T3P8m(q+S7I>0s8|HCXtD=raxZP_z{We8BR$M$(-&FjjuSA zPCMtI_QC(OOqBi8YOWPo?puDrt?50qcRo2Z6tH9Z5sHVzo)G?4p|Ne%r=L$7Hmq*g zrBZ>B7jKu;%2#uPpsS-Px#n|I4d{`Q+2~)oQk5qf-a%n16GRQ+sQilTDv=N|PE}VubUiqN5eGYnlppgX&*gH{_zXxk8RY9JpDU%Q*|VsJu@B&!X|~g=@$&!kFOm7 zCF8aEHySm9DVipc9-=9MNtaR*ChB|T z$->>RQIJL?aJ{^?u(alPX9FyeZOwzX99Otfv1~NuE!TdS5CHM5ZrROGZ}aS*VcR-g za^3)W3DpNX%rbULW`M-eTy&75UCYm7ip{#cds0;N;eH47Aigo!D4!DVZ_PSchD(f> zD^(IHX~4i-$wV5i}lKP4uGRlWKYt zZWIuM=n9p!xyHy{>Q1vr&S}X&B44-zNSgp3Ck`fz~5x)$1SHfOU5| z*H~lR&GH4X#bXSw=VgGk7+#hy1rjmpVLQ<+@Afg*dW_bokX{IAvV4b*ApM$Hb{#M%B2*b$+C|S$f_r0C67ds^A;B7Lq94_={HC3s9t(p?N40!;6{jtf~Yj`*GN%qiIwP1 zYGJR&_$1E};RW%u_@(4XcWJk4l)G%Hjfy7k^ji4)t@vn!G943C?wh2XAm^TjEs@nm z*h6nhicmkfs!nT#HgdJh@sGb`$|$jRVQq(RDX$fz042EQI<8pcFkU|gfOZhJ11qNL zit5E&d+Qv5973K&X`!xrT&OSoM zgeiQhn4l~_QJ=geZx&OYtnZglNg^M`G}F)6|IoMHhln`iY(d{(_435^2A&BH+BuzWwHg&3j&1Ff7{7=+jk8|QP0-P1Qlj57 z9yiwaXOdy!&(RW$Yepk+>R=^_m3$t5%S&-32UnKy(}H{U{ie02f|~Z%+Q@ip&f96| z7#G8xgA`wv9-j)j=!b>a@bK0#FDwPvsRWYu!i;K5uoh@Oa=^`*8-r--*5#SvWnJrh zZT&bu+}3v&-1>Mm00z&S%6>Wk!<)hrX30mV#Xl`BK2S#iNyddRR=JtfxrrOZ5@RO; zAUaaf48_IK)G;6KT?zY4AHPLxscz|+$l?90LJH57ezR*tmB$un_^wi`$!ikacfTp) z_i?^@f)VIckohizy4~W(oyd(oX9L;c=PsW=XVH0f9boo@oj}a8&OqM_G$j3vJx}7| z$VG`44Q!1DM&|rk!f9C-P(Y$`u@ls5e9GHrT|tZo3uazNCWZSgh48DO)}}kHjXAQb ztlm<*kWnvP;-hndb?}Co`|uP7jbVb=t|?Q9&}`01+a_Hco9fkloVf?P@sMdChx}PL{)l13NED6mO1J$MG2@&ALJ8V8sHr-<*2{;x$-XEtN3_O z>zu;dv0FjY`Jf5tiRY(jCtihg1}iQZ|*AnR0{! zNnuC17jm;(l2}5*%1n!;6Jw)H_BqzT-^Vs7ahcLsn$X-)&4>(f|MkF;4cEi+NS_ys zxT^ZPeG~_qtosscAV)<*Hz&f!u6*sLT(`i6EFPW~GU#!#47rt}N6Y{eOnrM2MJ=I^ zL$dDYa_&MJh$BBJ^JZ2+wj|tNtSGd~h6{^$hHrjbM%zj_)}O!ywNBv(;h8>4VtyNR zKxABvLkX;5%bE3i86E?g56|2V;?@q*U?JbQoie(JZ+ zKlW&t5JHq8>Vx{4R;O0{Oh08N-@SYHXU*Zku0h;YwNiALiRY0K(fECA4w*gX-0#ql z3~p`W!uIm8166~)bLfDdA1d0BTp1^>nv!aBCz?M28;+P)6)(yMloR;-2|*8HU7QG( zi4-*?IEDphY&vEUT%a4+ zxV7P>Lf1e-ZzpBOwYCp*9?L>$0$08!@`x_PaK(Py)5Bek3+yf>Lch$5w$p?icJOq6z!6w0Uh}>hMn)d znlOx#b2Pg^B5hx2g$Tca^U{czCp@46cBJ?H!1h+$FJ7^G^tvsJYUTSB?9$x_u6HhWPJgNg9uC5A z1ucooPWO`(9&uS?5??*B*WCH&aj<*E+6>PJkd`3MuTV)ezHn_6QavsIiL;}$6kYXG z9DTVWIvktV(oblF$(=oo&mNFMlJfbHb>FI?QEVU@p-Qq)Hl3FW^-Rs0Puz4LXGat` zxdQn~l|A{^!l;=XzvOHZ`=QyJ7_8fQw42|=!};dcJPWUW_kvQ{nl$dHKGoo{^b{Wt zb}+E6bt_|E2q6&LScAHE1fc9g$4H;X*Y6z~VT|>8lsC?BfAV(7qn=spMV5WP;3M5J zBpz|b03837sC9j7%*}??!Zbh4(RxHXB#g0 zVcZ)FJ@^hJ=f;yNQE8#|nLO&E<;%GFI)ZK+E3@j9!-6AUR11bH@ySexCwc~iAN2)7 zSB>@f%zaW}xsI_wT;4AfOIt*jr0Sp{pVz++FkN9;Wz7&HmH?4ij)dr)(`OKxE`-}8 z4l`EA=J?#R#|D9$BMB}^u!pcz9^%C^>r!L`oz8Mm_CW;36(rT{;DbueXGN2W$TIi} zxu3(tE_N(90gc;D){T1QoHduZ**RzjVZE02@blK$RMoD}9N|}Ls=sp)e6&zWH59tjKD@t|HIAJX=5c{Qt!*KN^{*BU>x-CtD24uoT0g*!QTZPKDfNa!w} zWyw0Ho*;!Eb`IdD(JWIcc5F?jy{Sz2%Iw4*j1m9Z*cOs~Yp5KXUFy;nqh^GSRuvc@ zLtVvfg?=_kBE8kt_+6EU2{9s0Arnf&Sz~V-<63k7w?4&A+4~kM-!1`d&-1*$?yW_u(Xl$EwMJZ64LERN9)xPh!34R%k5g@b-Ar z+o@CI^!QF}_K`+_QnIFl7!5@O@mhc!K~5ld(Vl^MCLl=s{=9RX%NXzbVVA-y>=uw8 zq)!sH?nAZas%FABr=_qtJ$5gZS=jLpi4 z04U+eN^mTikACWhv#KfXm)pW-W{NHTcR4pbH;jUkFE!W-sEzW+3{S7a9eQV!1lUMJ zfyBIU`>JdJ(H1zdO_a6&8R_^_QN z$q;8j5F4>BC$k-gs_EGCky6#Jt(VwBA*HPfNE(rj&kVWW^E;hn=^FA9XtfWauun;Wuv%TmeueESeGmCgzyn0kPHCcC(6 zP5$or&fHDB+jz2vL@&jw5C$2B{!pd>e<2rHN29fwP759{3VJb)Ev6L^%LqrbmC;;@ zTNwj8H~u@A2y$Y@)Gzgigr7b-jQt&BjG?3v1>(MApD;q-p#XEV{%0sA5}7`8fg}+u zLpBIU$rm8x8V1Hi=Qve_ms-keP-2GsSMtcsA(4Jz6(Dod}JarkH z2UD6pZ4Az%trvYj+G$_zC%Yi7C;hCm;Zkp+Oi1Lkl*EhP#s;N0UnO&hv&#KFcIEq2 z8&1|{?JMB85O;!gO0J~{-5-7yWi!Qk=f0o8JEjFn`*XRH*?D3I-)A0j5$ zg)Boyb?+B!>RmUk5aNc$FW+!>>h05Krb^_jl~d+;%<1n997%k~yJvda$*|i4#G=%`u;R1bLLrO*Lt0|y%w5<;nSJ96uize{ z-RRdxB!D5s4m*TaeiKE&!uxuP2p{HLbHX<>Z6dO*{8||}de4i7SCrzAjZ@LJ&03yD zdxOQ&z*5N)sH8p`>51$_g#-r|@7oQShD-%o7!BRR`vO@Uf~I~P1)1l6J>`JEJbvMu zt)>6P=Ag*0loFqYzT6G^TBJqDyL5Rtb>TAMwA2^v@oHD06~Yy!)qw*U1*#1f`Y+*K zKl0v;6=_|s92CZOs1_ULy8qn9Ic*cJ8joH|eifvUc{@O8NVh#$Dc)D9fwDiGiE)tN zh62LttTc*=!(U*Coj3(YeJ@vzFMB_dkiErWtiVyf9IC#H#q8gWb~cxnJ11LT=wAO=0ne!|2SZ9%phC#IBXpF$@Ato+)@!^fv}A*n#rXqA{eMLjt?&xjHhz zETYdz&w}Xk*vGt319l$f?g0s@5H@VVfHz?=2{)&AWV^KeW`F9~$h#1vUHX@Y2b7^~^ zZvfkN_W^e#%=XkuP|t{Vb!a=wHCQ>f%DIO$MFYhjg6n8Rsc*buY)-m7ET;|cXart8 zwB7e=3)7y6ea(|!kn?W}tmv&-?ax}9zAmBQZ+axf*+Bha8#l8aG0@jklnPg&$)3Dx znprAGgGqj9aQ)yqT=Lz$feo_p3qjY%Q`~*H1AXMijf)2usAnE_t6td{!7+6Vvqtht zw$J4i)ms8zrBhbnE8G$^5`=8Sb4=z=VMtf^nSMD=eoc$xZ4w%%dq3PXb$5TIQ98Pu zA;?Vd6L?EGjvm#gJc`Lj6gaAsk5q*=luC8ueK~1#CDDcNo=bi|`7XY^|4YAe-4qTuNLfy=7O?_*W(VXd&1!{*%028Q;2u0jl^?f1@(yR$!#XbRG-%zxP9{$!A04## zX^rKXVD5`b#oB1ncSzq($ey{hPWbUn${Jg)d0YvX_Q7tT9_Q zE;!kk+KZYkFZQB0cgLr@>2D3`);157j_Gl|r?$BBRr+P?$FqbELMxle8|NIh8Gg)( zv7iH*+P|Mv`CdFDM$C7`+4k6{>4lgDHjB@_v=A2r!V!#k(|f#mh*|pYCnsj-$Q}Imp5vs3WiRVS4MaxiA&|&Dc8y^a z;`aUnk&uJvYEp!Llyht~l5a@Di{O8C;JkN%(cr~H&zk>zw}ZnG2#yMa6d{hIG;EG= z@SiG@&C2+;qqW`A14Qq@bM(pw9rLd1_gzt$f!ZAfp7+>*K#H`Muz$Lip;}Hf3I z!=qhxJvNt+Wx&;O>nLR5ZwHNg#U}eK=HDi7oc;SUgweH!PjT(Xo}7_?I5S8ZvUOQ^ zx1Oz1Bs&O%zn&VpQ{ys6+X|9Ra#@-mVG@OMhdJmk!`7cha_C(kfl}E)QH1Or{~!{h zJZK~65TO^TKiFVnc+AqjD`nL03KScG#gg}EvFwqMTYPH(+DT*Ef{TKnC`^WVj!hcg z82j{S!TtL&hQ&oNr|Gw@Gu|K}^zYC4oaaJJu3@H1=lty0=+SgUn1620)x7c?TqeZ3 z$}WN<+OtkyxGT{3uuse+SG}HSIPr)Gih9>ok3Q*74Nz6EYfe?en!O2n_%Fb^?RA1N zziktQ`Z)YMXr&~@cHmRkZp0t4i(~GH&zOSOi8E&8d%_?-Jwq|aNyFN#;a6(DU+u(B ziDNGVsv%EPI2z|ajq5-Et9oQw{g=wt+kHF+m!bG%!14(FFN<%#=#n!_2zZ@7BTovV z$&MO1qGQ=V`?`)tphq+EqeBB3a_wJ+F*AElBn=}zitfp{@J|JQS5JH+=#>aI<5xQr zOrprTjlr{Cr_}%s7Z8;`ni<7Br+CZ%_c`Sp#ae)DnBtTF5g2oGw3tffVs{W$hRCJ+ zhn_bh39o^*KYRKu&gLUI+5}pkVb2|T zeI!3XEGK%@CNm1ZMg6G<2JhS*e#^1sjUW;?izBZ7Lr@tPUKtzc^09+R6Z6QaQQLQv+BpSFDR3r9nskrXTto%YIO2Hl;cRG4%-K_^N+OI z{z@QqM9)VqR{i6jn2f{b(UIo4n+&#)hEY$->`aTdDh@9>E}4GSKNsENTyD>Q;Re$s zTL=xQc4NK;lj_8q{ZjT)-nJ&%hmuyDwh3fS6cNwbcWdD zwr5gPZ#L@Gh2+ZQ(arCXh=UW(0-iH0U_V(h? zWg@)-GYDi5r@{T}8uq7I@~zu17g(Yf&c;wcp2#d!PIzn#*!6+4FeebB3?bKGZ*_7G z8{adwYzmmT;4fw)-1X?|U`+$lU0Blm1_ zT4eyz^;*1BUfij#u)yq9syBRl5#IS#7^*y0@Y*b`SSm{S_shFnN~4_xV(LU%vj5k0 zwV|c+75huF5Q*0Rb-h-srCMd6f^WcBfCa+rGwL^UyE|R2%;{$%RkUg19xZT@TU?8$h-e?R(V?OW(T(n(vG@&r(` zU$hC=2=Van1UUJ~YKAfS=6v&xjv&nTxlj|m7XawsxFDq*Cd{VG$COBrO-u;FUl(NpIdspP3hauU zoPsKQ&3`QzN}jAkWI6gIWYt91W&<@Q-NC_(Cq4;7 z!H296W}81ViC+=Y{L!H@&WK@7HKhD zZ}|*=Ri<^y67+iK ze?;CU$Jat~^7e8k>X|BJ_R&11R;1+?c)gWii(K@uMHLC8T_(pHMVuUeE8{N<%|=H@ zo9YV$aQ~MM+gk^TPCCq2LDJm+PN(^(97ae4WQD_=d+>(9GOJpQjfS+O`$<756rU*zswPImBo>)9fCu9k>um56h%&5(^A z(=UTLre=e-+bj+;1D5M5yq}B0UPN_a~?Jiv*O0C`c`u;$kd2$p;kFlDMhJOu=11vH(i@keKWOiMS&Ha0dIT@W%U z7h*K`wF(Ohl%rSf1M#)5ydPY?JC*#q_S?ZL7%GNO4fU2%zpf~=6y%$wJvGPNF4KZ2 zvus_j`~9T(pa^`s9nd=O_7IpJ(JTIS=3k2}QYl!ZZVkM#ZBX|Ga;EC%XD#sy7Y1i% zprYan!&RmKuQ8M8e8HG~ZA`^Dp^xo~i zLa3Zs$HRJ&mgqEs5UD7NA^p{+n>~ld`HCwWjM0JFKw`XoU((E;xI|eYV(YatG|KyT4G;D-#ooN8J6=j2S?}RKCx5>K3o(hEOb^|zulP-% zmOF!r+?8*<2_W|WY1jKECi>)UrI^Vvz#Ga=6jW4p?TM0Loz~e{9ExjClAa0YPx%PO zywqi=d||Y1S53*z-d;c}PPqLM{n}P2n5Gmdh(E#Y#Tk1ax$CTna{BPA%zqm@Lwo z{rvewPXzHJY)~OlR7~vI>gsAks+`Y~>Y}h&&;0s9w)bzkLlQ_TwbYRP8v5f-cGi3G3rj6zlQ?pP$}5*9&jfAw(0-K&uJHpe-9_aI2V}_LWl>h{1u+!pUQ|z zU3Jf!i*i69$i|Ad-l(t_f5izzyU`7kM>ahcbL%=7vEzc2a@y6f#t0Pwm_$7CKQS!+ zF&Ex&RUqJe3k4AJ5J-G|;S;#`V(ssKk(c)(mXx+)@BuY;<#w21W6sSKXu_|XU21KP zjP}?c@UcPwpokOxic=2oY345%M0f{Gk7OV6yBt`A!o2?T(ubJZI`0#?+unOSzaRxx z+xuuKaZ_S}M1KCycjCek!XohvIme>yOGV{B@A=(F<-;d%*_Yno!~n?szmLQ1Mt>5H z*>}SfF&x1n{70c&votF+Ig#I0T^Jos|A__oeQ89}3Cz~gAn)KTz@r>>2Rri*9XACQ zAB%TvbrCU|u9Ocd7(3ovgtDf9O(jcCHa0c}WdRV8HyPfj#@l~T1!uzZ$vUDnY2H-| zcJN|xt-i~!8gKL8ZT$XK?Ih4r4H~lVOi?I1D`5?a117U9K>2nNm?kFY0t=(xSWbrg zI^RvOW~*aNjuD63WVIA?f7#lf#q{?JgUW51Bf(l!;yKeXNDr~Z1Htv{R&`qOkeAu^Plx~XOh(Y`G}S^Y6C4tA>(9N}YR~BWGax_eWZ3bd&RX>V z;a{dD@#zm_{ZrY^u=+fv|G6hr8Nu@x&lqlSAUcGOaPO;Q3X}0LVrKs0! zxEP|Tx%mkkioME|3y9%wVgCBz88~}#4Zz!f3vpj0o1Eiyd+oT6lMvp&P!l|q<^<5+ zm3M~Kes}(@lBVaqG<>s0#isi1HHd&!29TG3i+z7N=%BA~izvyR+zK>2$}SLiOvu`Jf8#hf+z&$-{}bA8`bFDu9#m1U&YW1py0iKNY$)NsbvaL-Y2a8 z@e)E&|NmF)I!WflXy3mFZ|)H$fZY4HpBq;w$$6KG%rZL5dHtLQ0;zT6e8Y;xAZ{d^?0jZe@y!G^zYv?c)6UAit6t(Z2P|M|Gl?z z7%bdk!8?$;6K6-c?yUEt1Wu+|_unVL-^hCh((B7Nr1za?+me3klgE!B8E*gq1D7*6 zq-{6EWMlTMFdulSenZSZFIl0J%04ig&q}S>oNE6M-$AfiH)1CD5lfoq6Kq8~7Z5)l zz!~{Zp9=yhVE$W>j9ZuIq8Oz1_lzw@S^s;7HEaqIEZs5_?W~nRCY}^MoIIHhZw99c z{`Vo!{YyKlM=#jchTrTu;y|{ZfB4fF{~TeSdP@gmga}srKla`;tcmpdAEpS{02Ku( z0e4l<1q7vsCZZtHrGyTO5PA_pFF{m93|Q$MX$cUDKgSwuGt;J}#Vw*R4c)C*R*)>moNdd|64Y9=dmhXg2KwJ_ z?SwI>+CYF!o#odGqlZ=}jenMa%%W>rvAUObL>jB4+Lq2JBBg)k7|>N>`k+?&^>U2g zvxoJnZS4P1*PlnbY)LhIUdW+@6HXe=_y5bo{CvaT=K}BUow2+b{-Jc;2i9bB^6%^a zJz4pDL63U${d1hM*2A5Rs{h{b_Z4`?9Hm!CN^CVmqODz}QRMG0{(Ca+@Z`+)O=2A1 z25B(;8-%j`{J;17eFgqrD*4+6@Gi@RRuc8(^?wGmbC>JhSXgNX1h z8Er^uE$QBcJP*IIc9{bxUNW5c*-!rT%kQVjvT|$>k3r6wr`<7s&hBg@ZyGzu0;H`{)1n`Tr>Je-!vX3j7}h{(npX4k8^@|J6RR z`8-d3{%t^DtFfR#3xLJ7w}0W)X9fO2!`P$z{QPxi+nRO5rh%kXmB;8%Fp#tgwybjF z4O(jx39j>71b>T5Qx5ivm#|eKBOwhgYe2RpBURDgKCTVJE3R8+#u zRDs-)a^ON)Yfzl>#=r`saqZRDO4t5ranC^QX1@j5w->(2^ieoQ@%H7abASB)$C(QkA16CW zd*rmRNJV?!o=&*fmZX)f@Ing;vVR_(lazR;+{x>0sFr}?B=@sNhR0tqT*y2((VoqI zoq4oLwGp3nE#9Lbe~Dz=WHU2jWQ}j$44Vxis`yjG^cGRJRH8WFe6nlF99PEHRcIQ_ zRCRAsbu4trC&S^!zk(O~3fyvkg#7@jR0KFO_abBW&T=?yr*3N^CIPk}imorOZX_3n zPVy6GkH!UDb?qxM?_2#3!5QqJc|Ua}I+GC|CXx|y*LJ%d;#iw*q_12FPJ z{=dJ5r$s-%`h%o)7p!1-lhAQyp;>hyiUBYy^JlR1yY1&kDSg#T1NL#&&&dDn0_&3h z(*#CfQ z0H`{wX&pRG@gT)n*?3!M@2zW^17KTs&2h!MU$FW$n$QR5ej9u z-aqWf@BVkc45a@hU%N6g+=JuLvFg=vA}*ifZ^HibyO`@*zyJ7DdXob+WCl-`{nIz|3lDW|3*^UDAM29KMCByVPV@) z1umgdk5I-1JwuJt{f`8^+%)~A9AJ7cyf^3yFq$t5Ic;_!_h zwNnQ8AU%PP4#J}ZO+c4m@hQ#(qC1|`Bta+jU*SHWSpGWX#zJ)lDe@BWnNgHkabJmb z@YZx!3eABiPIz%9fnKkD`A;f9ij=-tpN|eBqhc(cV(^@%A||Sa)>7WwKPdCh(b0`G z-IW$bnFC(`yu7&9AwgK8c>vq5(tzi{(Epi$d_Spyd^#_JfLg_3dB=r{$BOVfmYIZ@@lTogFY5Q{{S(BSg^ulQRQnG)B$dagwARxM6pCe>!6 zJOKu5q(E=gLbZOHtDDO1S5E)7vk5vN`?Di;LDwoBiKl+_^v`i5gzf?WFVuHuz6q#u z(x)BNsyNmo({CkZ9y|3WjQID>SfW?+35aj|`Fwhx%4L*dm-inO{`wkoi?c{jI?|g~ zNEtZX^0$V>x5fijQ_iWYmvP`iRnCGmopXA?F0h&b*)+c)=g>u0+xheQf$i~tluSNs zbiYtOcKl~Skq1i1wQKF7`LSi6!z#cHLBoHO24m9-X;O~#4nP0T4ilg_hEWAv&O6wIk!5Q}TknQX>iJ+btl%`3>@W!&N z+rUZ;UR&fqRpkFjQwYQwW+JrxQ8O?0;)6;&0ir4oJMuSS{ywQ1_C_eYo-V^Jml88j z!Fr%?)5)#j#y`Hddz%_Kon#%hJ!RCI*sg+4K{e$6N`Q20ee9PCEOkNOG)4-8!eNAH zpAoU2ER*E|9eaV_^)A^o)LTe-bNm?KpQFdX65f*U?0wRbEWQn|oX1g~^OJs8f6Ps= z83O#TD(p$%$_FCLf87vJoCzu}+^ec3^s4z`-BJD~Q97VIn&*BLFc=s-o&hyhC~-gO zn?FiZ{}&b>zD(lB>+6Ch9OW}9tuYdf0h@)9ZfLU6KWTFA;Te_9n!?kaFDmh(s2xZ~ zhWw9h7rOBy_3>1iuVFo zC-=O`Y*Ht`r{=4M>Gm0zPYM+&2T`@Mmi^1T`gOXaR5N-8ASSrPo`LUl?rd>Kssod= zhAOErV7rN?U17E!O}ufOUPc=p8^g5?j=*&WhN71_NG{a2Cr|xR9>AK7j%_w>(ksIH zpef<=bZWu}hfsqf*l&qps|$T@HixQ{-zL%rC>^~}zw)l(LeuUVU~h7QO~S2}-T%_z zg{R6$5uDS)a<`M!_sRVA-}uAPyda|-MZ2$Q02l)SFc%awn`NX~V|Zv(<8N@OGe2)j zqaZO~T{Xgx6!fo{KcKZ}(NL4Ttnn69>d+BCwmU87zde-zOj_gnr2ibxql(VexI(GU z8Xi;56!3qOd|18Nv-rq-ja-sQ&e7-tlB+fe>Y zKaV@sOeeec>80Ib9H|P@CS!?f(G@?__|Nkqzqo?EJH#Uu8?pZNIksC5Y@J4HVMdzU zDs)A)a~LV9)rWO{ZVfhxGOgXr< z{PsPqEg1pqqH<3HzrQD68)Q!Y@;^Qiu261Cj*~jMIggiel>95)BP~B7Rxa*S4kZ8_ zQlzebsiRSsRbeleD~L281hD%(3Zh!xb$8{pv&TpyzD&EL>Oz5@rG?7P&^JSv@*cR! zcoyCj5aNtN8$kE2|Bu;Arl(yY=ftYvhi0rJ96_?VHCT)OtnH|Bv0YVGd9n6ZAto=wyX831Xv zUl6(QY`-#`rH_jW_Zn+kd(})o10Y;+@`+R}fx09IkfY3x?#D{88zt!`4Gk1Way(I? zIQHI$`HB*7UQif$k7x;);~7)`v_XfhGaG>y`W0-qdkeB?K&I6vEKds2 zwt9~?w(3#e11wwg(jgBKweg2gk$H#;$=ISWl;I!S>BK>Nv4}3+E|gBDE5P7qMRWj= z=Fx7b3Bs4uErDTAuB~_A!xMjDI;aie7k}&pz}_3G2=b`}d{LfXx5A7s84uA1O%jST zz(CKPg;AzBw4eg)DNA)#(7@-l3F0e!1|%c-(sAI+XeufcD!C+wT<)~9f~R$P;EnP) z%D&Cv(U5)}&^`u_kAlGqJ&}UznPWR+n|dIJ9zcllD0j$7zfya#=fx!XQCH$Ak{u9- zr2#vdK)@b3BhA9^^oD^f8%hnIe_R_v1ucDEDl*SoDiAEVjdQ9AF$lX zEHsKr6v?3N&dlVU@%|nf+v$xIU))}ZGJ1+Rwq!|lfZ33D@SHmQbXz*UlPU>=&j<7Y zG4yy|k!@2WRB4iT5qJ`(k*4|8&7!>^;x7=*p@}Wx17I9E9xJc6y+`-b=&;q#FW5BP zEVMl6cBSKxykeT7ag$-1WtuTa*`01f;Ybf7WuUkI)h!n^5EWr{2mJ}5!7WnhJvPCC z!?nEWrpo@m8q8-YQ=_%HwlK0Y<&Z>uDX^7EACz+w*qMdxUM#~W;n5~_I;jrO>XINA z@+$FV3yz*7u=_*5*e?zI0k&YS><#o~rj4o{~pK>de@$NY%rVrHB1)VyB=xRUvP z=rQA@Sz?e?C>d{kx9zOY#r(YQ3)I*#K^KknIr&2yl}cAD1})+-z)FG90DpGBka+|i z*u}y?fHqM#=7C7bj}7$gW+%r`age=7{XaPZ;P&qgqY`col7c-7^aFAWqJYbXA5cP4 ze92D=BfVZ!#h*<#R7P2-ekYxsh_@MRr1v)u?0Y0LJt!Y`1p?OP`&@dnA5X`ng4?sV zBZyP@zFM2H7+Tl-Mvai{R$c+a;e^=w)OtT{T2vt zXKvecefQ?@6J7~KGT!@7mt|K{T$p|l_9$hghq*6)3RLt z)VPOXC6uV+-=p->&~zCXe9`wEUBkHLYYcUEw+kc19xB z(S%LfV)45r_pUS1PX~YS3$h@);$;9vz*FW3NPASjNvnRU!%?ML3Lap_FT!{;HC$ns z2k}VEHvr4n*DY2JPZ#>Uq$`4H1{qI~6Mk&Fk5HmltYQ&b0`_;_zESCr)aTtw#1dDC zOZZXb=>(M$%RD*+XArAxHQvz$-Buhe47v(i0~j&RjVYk45;u{y834_KsYncTGlg#dj%pB_|W3jo50A@)!#_>D=CVOY9^j^P)*8?G8;e ziL8vWNA8%YT2Y-~b-YkI9TFk9R4|q!$PthTvk(Xbhwd)kD;jbblJ{{BodgerB0cqf zWm4leP=H@|A)bnvp3R9%|9b{ymgI-Ti@*#7p>pL7xs-tq=2bBBoPyOze1@qjGT4-} zSa9^5?ua|mi%tR2Sy=?>x0r7S@MCy?`a(O#(MX)Ehelo32lMH{2)rm?|NE-{CXoP3 z#@%Ns0w56Z$83e&mG^aB?$gje(3SLX3GPtZ801bed5S5cG~t<%b&#gPkP~NYGQ{!u z$OL0mo6(Y>8)TwJ767sUqLF)D`gxxKq_UJcS?v->jF#f#XUJP=Bn}We0qa_$32Bmr z{cE=i@IyFnpc;|#f5z9z1Z0LJxhlbf0IW^Ap+`ce%FQ!Md&2xrB20D z^vIN42Q={jUJ5mdf^khVad+_INznS(K#45C=}i+3Nbzkh^Ev3_(t5T;jM4PiN%DaY zip?0^-Q_Gdy~*d$nXdVj)eJ^bs2bwQKC^yt&Bhw4l&DpZ#i zUZgexe5NB~OEgp)Dq$+(QU8Pq( z=y;>FK+v4JdVij96L9rc$xyrrmt#`@8ov^9{g{AkL*#DZbH?84epz_J!6UNhioFla%+A6B!0hZHL*>KZ^vYKyK-vn zjSKZ~D;qq}3r616P*^#^1x=Czkk`zwa+7{o54;wB3{bdJJSrPyaa2MSkRrcqyp#QZzn(ewzY8ZsMHMOhh`p0BM&KLND1iKml&w z(Iv~_8Gq%6FbDKY(iLHkSk_{RYWQsxOdhS1i=6 zSH|zvJI>*Dj1ypsJ|+orHgcAICkbSF$K?pPvDyg>UI$bW*mmnhy^Wy z{l&wx#lthh7eaAM2K<33KI_D#zxEx}#Z{H@H@uJtnumFuCFxH8+( zt<74bx7$i_%T8CIwosEn=!JeaGplO!QtuXMAa`}hmM{Q>Wt5>4);o%&GQLuk)`1j9kZK@uv7u-ufng78yJw(JmHh+Zc5|d*-$jMY z;>}$*oI)nrct%1TDpM8hWmJ`(F;=S@5Y^i4X+eClP43xZki>Nt#1`8aDc_zDGaAk{ObuyjiPZZp zjM$0cirVnWppV`NEvIFR;*=grNlACHicRS2)yve@OEur>hnl?gJFWR&XbEE8jMR8b zv{kY2cPxHxIrGw%I20XFt=GPjq+?KHQRHeB9uhDqm>Vo3>}KQ9FKwd;tXr8@zup7W zho^m^wCr_#wXXB5fR&Fk^K<1d`$y8q;uVF4@d398b>Ti^pVyLFUN0~V&oAL46XNap z9-JEv)@f2Exx2ZzrRcdXXf{dUPIT6t=9>wObr}VhrkFeDl>P7jMHpF1Dv<*ay~HGr5C#{RPTNKe3#JTy&Vhw#@g7JYZ#9qE3GIZ zB?wPed72{-;jL^kC7Yuah9PfKWo2X8^xPNbLkkRVGm|F9iDTLw&j2k-tSq})@rK;~2&G2ur- zZ^Ui0P|#XAoATAA|Af^hrR5G)Q#!q}(D3HQTjD5QRaUtAwgYG&@%q}w7Th!&s!ry{ zk^FMp=et|%yHp6DLyBcWT-8Bg=K(FCb`oOGzy8Tl(Qwjq9;u!X-zpaBZZruabrTm6 zHzUMFpR&MT`Jjk`2q>a@v{m8tt5LSGz7jkrA%4dSNAPsfxl|7mnptnTZ~QnkqS((< zN1i&+(zuxBSl?*PKgGORl-{sj`2|&NigrH!NwPQ;5D&q@0Cxe*yZI*F{&)d*@?c9~ zy1(f%va#-6lHDIW#;QGA)*%5oMGo%UZof@b(Ddhi6_bV%a2aLdaDT(GKAD3@g^Z|oCTTu>8!pEQ0!{1NdBksk8*#@w zh(2Yk8fpNPobL;je}?K9n&vz&(@{rmXjPUuNba5=dRO{|Z)RP9Sv(_>zqL+v2$LRq zwZpOzWgkX-3 zl~3@V2`yn?PxGPQ2g?jYm-=Ow9Khw2!CZ7`iTBbZ8LNmJbM$q)>ASo;g0A;BnluvEkxs>kg{x#aVlqAhU|VJaQfWywpkm^bzz_(l^Qi zv7yB=IS_>w=ooI*L=lJ;O&dc(D96jx(OT1~Gw?dPu3*s!>7gy60u(g0JPu}1_a zndU^v)@gOi$qI|l061l5FurAP=LWawGGx>r0L%?#i&h@4$S7kGV|^E$M=H6+!-eel zLcFcaMvk0rqBFDtI$62$vgOXVI$i#m9f8I-yE8;ZM6jOiu>5pw&^s%V8;%$SBtFj~ z-o;<@j&n5HqeK%^fOxPac=A0GFy&?5B|d++)>cWc#_9(jv1t?=gaXBXuAQBEPepE+B4PZ;Ew{spXD=l?s3lx zY$Gh*@b6<}7zk9V3|cu@JgBRHf79;p;lpvI*))^~!P1wL+m9kL zS`Xks!rga`IGpZ;>XCC>TxXV3M)!SPu(IiOdz@I}@lwe>_3Z%34k$B?+|{vI_eW&+ z;fXLrG$vtq6nMt;Cq?OY!&v8q8G%t^nBSHuepOo=-;7r^Mmv#D?Cjkj5AY&FVK6V) z+b1oyi-Nra!nk*JQ27mZ1sCUrjzt7Qt0SRf4Dq?x)@|ln?hrskk|5zAjcC2Zyobv8|(sU1}bjspqHvVguVkRJ{jP$pdIOpTi zV)>E$Qw*xspf{cN5>f8eJsGII*Q-0#v&{kFmeldy->STK97%PGu2IG_x}bZYjnjyl ziuhq$=^O6x9TbgdoP=D7K2TG(4i2HtwZIDGbdzUW7KhHYgxTc4Jp~sBnloLMl=MKAR+#-@4&%-}M;wc2S zz_BLMEj%VH*d)$rcY*LFF=X{LK11g5rR8Dbh3oYtG6>p?YH{O8V(MnJ zIPEpUs>z;@%P+%(T0Px#YPELhW<%yP8Z#ogpkFun(KR{LL_QhM3Dm0tS02TCUi#K* zQYZGsj5O_sIB+>(n59eKaDh|y#sylHH=!HCf0mWjU>H!5eCQY&7lyCk8;&DMG?~<-@`~HZl^vBohts zsxk+mVU5kAPX>s3coGU{if-HYN-Qtf813e^yr)Sre;QyNCG(g`Wq2)qo5{`9eRBIt z1rj0`&Jx`Z!)df_v+?&In{M+!WUu@1)8a|M)~n)5JQp&~R7B>+om-lVpU<@gdrpEk zg(?GppFrJn78W4L@PbCeh6+Q2;F$wHU6FON?T5NOkL%M~xi+%^NU=PXogy zcl+O4HuRr26Lduix$dfj)O*Le8y-A{o{vSmYsASeZ6{Sz#XUwAQL0XEO?&m*;@3y| z!tkEbHx$B8Z?ZBnEIbh$bz62f_q8nvdt(BI#M5K+=^iO$UGb+{o1uWGFSggZTAEc~ zY|t%D$CN4l1yp(i<-f91EiHIF<5j+#@P0-H;xHmwGQ4_7$veJKsjMvgJ3MFTj0)vH zdvM0234hMSOq|0-=l1{)atGqTZQiiri*pOj_&Pq-ohXxFJzkgYh0lqP))06o--Bbq zS5pwD`Dooj?mD$ieA8)L&xq`ZMI2gAxIYpkgL?h}$=2nyP^j$9+d9;=x`*MT8rV;a zOl^$uKB+m);d%VGuwui$Ejc{y&B<%GMCUSza{N2;=)%}k*>3TTl9tzP6>F(FSHpZp z%BDtzJ?BS=LcofCuMH(j@ba=GPfPs>`Or7h7mp*al7x@T4u{$pO8r% zkUXy@s>(eMupPP`pAQr^EYgx@5CdVsG9|RI5UR)e*BPl@T?jAR3cYO&FJfqTc4i1#^XS`;JC%UEuYw_c~EtTDIx}ZCzrldA{ zG6)E8)CEPq7PT{2h!;ymh0R`I&&XWwK%p0ys;}mrSzk=WwjQK_z0KB7&H;Xyk zl>(p>2N?!bg5?CSsJ!-8U+vo*YLy#<3RSFcbHWyb7dgM>Sq=~{BD7c&hWq)sLKg}k z;{H$FN3vF*Dl!h5!dyX5?NZikSB3`3rs7T;b zKw~P1lbH_yy zloXJB|=x#GosY=r5+wZ~sBM=)h!_Ej@AXEoo5xSf01``qzm zp7Y1=X?q_M9C>_oKk#N=p$x?)ohx^~l~mvqHc3J0JNvGbZ{BuXs@Ykn=(q4X>Y``l z+h9;a0v)fYiFL@~_W~Zx7PMy>v+WGSx?tyKx4(os#i5PfCH`IpZl}ev(n(u&)-so{1PD9cFVM z$C3oJSJwRF;E+6jjSDQSj5KY~_16|7cP>duNmYalA2kz;3>0Q+VhQWCyWpA@p38A6 zOCe%ALS;5{YoQ?N{Q|-Ulvw)~IPL{rlFaLAiqnYJ%!zo0mBSXatH$k9QDc*AL{ezc zNv{kQ?G${yd|B+y8$Y^zE7M1+HTiJJ<74k_)U__*Ov2KPa|62C9X(#I4Ck-mq%1@_ zO}9y~s;+2a*aj`{vw8u%hIh;41TC@o6+)9bVOhUtbNBSRl;m$H+JOoIzK6%2dgSdkJLSpn|DjXy z+XYhx$G=*OVx82HGzU~(vEke8_|&e>ipqN}p^{Ek zG5Akn-iqT)<7VSKuaC+QUSWkEiM~@jjydbJaLFYqIYx7f5%as#Lbem9zr<8kH@tK zhbFx)6eP)LdXD2}_bPPa_W>_?Q|}5cFI5TZt~S;nIk=0i?>hLrjB{c|8daS35^ndA zab|Wi4n$mOrsA&21U~k#89!{tvrd&uVpB$`2n4V$&Wv5%PTw3@hcWA2WuJ3#=XBzE zqan{&9BFk(GA(u%x+4EZ!dwWaJS0JTkT}~Md!WYeZ8+2liKIK#~a`$J8#ZE z=qo>3;@V7TNXH0ATV`FcC3F)xEtY&Wx@I9v6lU!lnOo@0Z4G90wsPjfepjcskL5jV zp-qr0EQSeJ=4l*KJKEyXyrF$foxOz3O#^$~3X7HBh6rGuvse3{lMTI}kZ!Ww{#)1_ zP1V87`8moIiwkGA4*BZMZDMV`7(-nf&%&c3wBb1sWm-As<85Z=t8|H%kIg;iYfO~E zM}9ayId`v{jVfoi@RF77iGm2&mYM-CGjfhvD@QJZtT2tULz(ZYEVEhtIbXBqn``7$ zdD=^!^B*>_p57Odb9+8vH}=_8eJHzg%k!mYIlBk9onu8GhxKXwD++lIy}ZS3-0B%D zhJoIsOpO22UHKS8Q+4Tweo5}2HtAiYijEL*}db&f$iZyuvGsxPp zq<=QHxf5teSD9Y;Sl-5Ym*acB#LMLq!qIf%#*577M1JW}Vbtrg6Rc>VWJU^!8i!eP zfI>VoE*0?3vaX}*`S{}w<~QwEdU#AlwM$l7s>2KfE-2#)GEnhsA#Gkha3_UnCWp1P zpMl2E*e1_ias5yhS?#^+VEgivH?4{dJW{_a-9+QE5K+RtguSDcvv(CqPG8ADS!TVa zE@|cTD7PVB_3ynR1hZg3nBp~@qeCh4#m({M%->l9?D{371bjpOF0;rd}cJOSsSWY3--*sTFVCCF^D`&An=YOGCYC{#^(N;QnkRs$|&#e`0`p;KT_z=M?E=b62N_LBFqD@H*roKvH{` z*lFQ9lj)82oFdSUpFLbmpcv%2LPySALR6&t=sN2^Uj-MccqM

    zCAYvB9X)vM3_cBTW(ky8eEyV45?k|cQ51ED`$Pm9j#-5cdELDgW{kN=41(Mlg*WA( zyBM)?&hN{f&$I3x7+|wTyY?JA=O27zrRhUiqIu&Rb#~^s+zsO4<=8d*jXbbx|I{an zBmR#kCRddWRK8kZ<{-sM%u}yDlfX}zP|VzVpsP}Dv1^Ar41_MeviYE-U+x=}qnt^% zg6h_WQcIORQcBB=%>~RH`}FcDFvXZ`QHwZ_!%DB@x#=`=ZHQ@uxRaDH$BTD5EOG)L zx-0@uNdwwc|7~m@Zzt)Dmk%q+Y;hJL*jF5R3?H9U_4uTibs&i03G3bgE5w+qBZB=k zB}&1?);5^R!7z``e){aRSt-G^V&tiy>Xl;D+;+8WUux=ap-~S zRC2iVGG}BxjAqy65-P6GURPsh>(iLO?_l`ltA#;S-?a|LsLY42L2!3 zWwZny^__8O=;qe7e8UFbC(|zaS@5Q!9in}H-^&p{=oI?xY+hWksWUGO!_k z<5mCF@uR++VIqxrX501AOT!ZCs-#LgaXVWp$v(O(__K*!34X)Y1SMX+>)|o3I&1a0 zCfdI2whNop{q7mTC%6mJeJ3y+GAE@frd{PlE3N@xEvI6`!C6{b-fN)-PBtLLU3%p7 zT%K1h^#bZu1oRVhyCT}&`Ekr6S3X((c38bM;6DfAFdBwDtcVmyXoTn^MS`ugW>(0@ z_W*MThOq8V67L<|{Cj8pwIBD)_CJOy_rA%$t_#f;V^@OnoefI^SlkhHHo~=VG%qZ3 z=B_>bw8cdYP|%ncxVfGBM&XIgfOHpI`LQFzgd7A1Gd%b#*ALf#{OEc(YjYd?mR9%~==8Mu6m~p1Z71@W9?`n z%Pk|PdOYzdV46}RN@jj?k*3OkyP9N;*2Qznvnl&{q{wrNmSoL_&iE8wl5k0!VvTkz z=E5hUdD`{GA|IxvUms?T%gC){Y6)SHyL!H2?1PM3ag}9+1XHUJpS&NhuVziaQI)Ig z$6T6i&MkMbAItYU=W@&lRi%#jIC84Y+4M>vRP;5+ha0(v+SP>~@}H6I8E~MkSY~YOngJq8xa~g)r%t#ep|to|L;@bYd|AWuUCr)YO$X zz^yFg8~Gv6{OBTMaU%HX^8)4#waVjcdF*E)j?7D!XYAesZGozZ8F|` zy+6mYz^TMi&z^eSeJKT>#T1Flx*a*3J$=u?DC(e}^}djY9PysazB;@w%}=DU6(KgN zI!YA5@{qor?>3Y!=z!ZU>S9(p=mt=PX-G)Mbk#@GwGd%oxOGV--!qm4-s*kPt-|Gz z%k%wH3c+>9N)1*7BF}r27}=AKk}CPQCG)1Vq2n3fnocbRjIUu=A(x@GZWKS(pnV7` z#O~&&fxyovrdX{WcjE_fAa;R7@S}Yk8k@=8;cn?H24SPv58h@9V$4c$V{s)(+dfSU zdRn0SNoT)&lKl32uR-58G}fhSR}lbX()wqZVtic-%klVMPF$49K2hpMkL}eGld!V zn&XU&>WU2gy3)W&W~j@4$M4CYnSJlFgJJxpVSEsCIHN`;lja_u#WV5f3JRpmHuh0N#SK0Tq3VtGFK zxS@ttx8f!2C>oXXNkyO>_JMDuA%)<}YdMTU$C+c>!QaHwf*rlYj~ zgwl4B0yDMKe0yR4h>rWz<^J*u*ihi*Ow1QMl1@h)@*@@dI{)0E&@; z@Qk~So7lbhQ1)7SpA2?X1pxbB%6vXyHkoZryz7JE&rAe7E!3G>tImU&TeDarcCzo+ zTHskhofs~eW|fC`g%56Mvar%P%`SM06?Q#%9PxxNclzcd2BNn^(iW>>DqGdy8iTG0=nwT~Afjpc*-DF?{($y?v)k44YNqp7&b1W5f;Y zlPpx3M~vMgTqDi`!VqB}0sNhoA(Qnd6`TgZxp&TNsmvqylf?%*@r)3|?o^OeO5>to zj^{EPZeZdZE!>g&8hLgjnlpa|oYnGT5eUvrDjAWC#(hIZvXxa%=zKApNGEd2X}qXc z?d4_YIEPE!(oOBdjOk|xxUjWv`|S*fr@pUJt50pd+G3NdRFq<&qr-nA45O7z_rke- z>;uw~fi4%{;>F_PHQ7FNU*(kP5>t$X^T0_FgIul8ABCpbL8gKMgCz% zkyF=(8DzSYPuQ~M<9X!Sw~k;k*OssKv&-|G&{(}iEUdhC&3+HjcSG^(eB23$@(Pdd z))MtY*;OK=)pPKEKmz}2759>vF`PU_$UJs~2n|)}n|ScCflII>yvb}QS;dmTM=Fjz zc6p9D(u_l!#V?d7MxGDLT6!cLjbpH9ZmHZ&<}ZFhO0DxU93KEhj54=MAgRo)jim<wJWIw<-C_aLtDAlxfMH2BkA{N6T>6ghFy!ZRWM2hm z2b~AD1ixkgXV0(6Tu%5K;AEf{txaPqI5!I0wz z&+aCc5h10hBpGYPaeu52_I9-WMHKLabV%lXxP@ZoAS33u%=toX0c~;T$mebkYBZp%EYM&g z-h|+WHF2W#WE_&}=apv&V8a|U`@>ycyy~|VKCk^MPozYtpDGu{Uy6Oc=q~Rlz9Cln zhZlr+naTa&VY(CfyP3cB)xdF5ayee#uVTIMekQzl>VoDqaz_c<3PZDUVPFd53*t2EV*RR?$@ieisw zVh*K4Z*S}fwlP5Wh`WQYK>z}P&0mWTD>3gL-0}q2!e=RfIcjzuc1of8aL}?x%por` z3qwSh2V2Y1L{flPF(vYLHA5*Fe5Xea%ZN$|^;H9k`rG1**RU@yrFk3U<* zWZq^8jUb4`HW%eKGFI*5VCufYvd}(lHEuHw*z3f{E)abKIs=2mYeLe27i@T!)tt`6VXM>-X z8tNi3TX02oQ+;($YP*6EkDZm%!B6K3*e+<)I2`3>l4aSGaN5ltOu_hIJdQytE*;0P z$Y_ek$b7QVw^@o*RSQ;LmJJHsx4SO{5~zE*Kf*(2aCt6fBY+!SZ>qkHFYJmfA)uIT zg=3r7#5s4KUo(m7yDiGh#npP-@nQ(Vt--vg_Jrh8RM+Tal9PPH6!F329KeLAqx^ zB@~cu7*e`BM@3L-0O=e>7?kdY@8W*$=Xl=lI_6-0@MC87T6?W?o$FkCCyxo&&*>dj zk8aS~>m22D?hnN=hr3fA9Cn5B>A0?BNY!8($Ky6Y1M8YoZlCVoCCaf1A z9ILcCK;ht%PjU2gTNdY2abz~bFru&9+F?nEs55FwIVUsB}deD{2vU*^6OmcE_vgG109BlWWE75-AiGD&W8yqbEAnXSa^ilfDjc zeEiX13L~FmgcX!SuYUZ^kGJ~1-r#VBONajhiNw4)zzb~gKiPcb(4;6l)Ft~(cYW&2 z^VIP0u&moehD$>XHtgL$RUfdV*6b&_JadVpDr^Yw<~*`uC#`c0vp^Mx0Xm_nZ(2~R>~6D-OOg6$v4s%s_pfj~C)c6yTrlA4h~R#RFjjBy0= zz@7o3O+^@Vh7mwt?JcLOoi0~>b-^YjEd2r}ytlyQ<(Jw*cawkIz_jZQF6l5kTBTJL+KW|MJf~A z(A*i0{QMX8!q0RBYewLk2YwP93&Rb1DVwW!2!aihANVmgnVE$lvcx!}m_6!@(aqmr z#lbBiq-Xr|-r+Q|MKyjsm*$8D#Jr=GpZ3vJxsk8RvEDpK)@}I4<@No+fspLKJNvC_ zNb=_@E1S)*HP7YVkwq|qvFxWamrZ66@kTCS!O|TN;p1S1><{k5j*3UVOjuVboH#B$ zg;i>u;NF4uy*md86c%q@GDAuG*k7jsC^7fnp30L*DBmaV#rPzS5uV<8%yfA1$40>U zjBvP%RF&TWB|+4QYLs%kI-~mcEu0+eqbH2fsn0kjqbmsR7}2VhYa+J4)a9A_Og>mQ zFro~LObMz=*#&ZM$6Ie}V?-Na4M&Z}O9yNEt1#EW8I_ZGVRKV zu6uikx)S#nRYX8E@v*=yXkF1+TJf%HZG_lQ=wr9LMU&;Fskwx@@asTas+kSn`yOrF zTS&mOiLR6N1R$8M;+K!Q`~6Y6UJLt%c{Wcc-&g+>#n>=glN`2RSA2=j!}o6VVO-V} z^Kii@y!EX(IejKg0e=@25si?Dc^94|Hq#XutMg;S$@^Zage2K7W#n%cgS>FUTV$P? zdcYSv^SG3#r*@=1ZWo%LY3yxzoKxDu$Ij@B?5iI}^z+AB6|m=#@+rm)*yYr@IfS_QVm>aXw@Fv(C)u0thX_jV5Q{0PFxf3yu}L!DcTJeY#~I<>vcR zMa}(>Q=eP;Fn6;%)h1j+XVfpNz)i$wEEhpN(K{|`!fTszbyQo>g9GV;;D|RF2bn*J zjia{mXGEv*wl#861aMq3?O?*>!bOJa1f=%p|`f% z3{B}DO2!>|gUBzIpEo}G)x-{G_wpzbrhwDEtt@g`TX)QLpN7kN3VU5v2s>S7p;*n@pIx_$F{|opga!-a?L^wBe?i`aD($eB1&()yiEqvu zA`+`q38F@p1p>x<>Y)Ws5jJ)6CnQ|I%(f4e+;o5R zc2>x^Q#}*Wz?!8J*KplKRo$u=Xe*46z&Y>R|n)HK*XIwwtc-+lQF_d`V^ODXZ$e(>(7qHcY2I!;e_Cd#x_Z z59qMFATq)u;Z_vJ%u6n%_EhnE_N$0XzV`=L^R61g`?12vcP|oJV z_qUyH0j4&(C4a3>`@v_jLB!qhd5!+cr$2hZEv(Gid4)%ilVQ)ER;ah0Y-04fhqh?i zqhLXff__W0qTO>0AHwE&Un%Fs^pYSH6-v3mBTbM@RI`i3m|z=3I4tqDdyHx9xbY&= z$Tt~!Tj+Z&TPFBYpKfMa*lu@~yR_H8o#w)1p4P%lfAZ(X>(M&KRBQLUO>o&>fM73= zRR8TanIm_>2f5&48ElcC_av<7d~8L&m+RCp^8FMC_f9v#z0s&~D)@{#rTk4unaSkW)^b zR*U=^dZ4s}wGJjymY0?ldJnWkpAG6k3wgQj&&qyUBp;VV09V{XSDTh=yK1ncFd}um zetgALD}jJ{A#W4Wc8%QVmc-Nj)@$oe)_xE6hB(usE_DP%2ols8LY%<0&Z>4cf{CQF`re0_sSRMQ`G0LYVvoj4 z&Fo9xa%T#CE#;A@5m)Er@%iH?D7hn^vU&Ex{uLyAT_zi2KzC8}ca_ZhSFdm1&=JMqR2&lSv&wx4qf=p(3w^ZbMI zlrN9z3+xlQ?Icdw92w7^zV)82_`abv-t6dht34*OYh;;TgtHM0f+Su`roHW7DiY3{ zxq}X+Ev)lVWumLFhq_CTT;5VxOy6-~!x&UJX}Ec-MhE9(lf(8^jGhnYqZ^5pG>B&>?$iDe8BE}oPNM&J{iuCcklk;-S^1>gw`>c2S>fa~( zm#zU1KevC;Iy9W!O8ccMSPirC!-!-oPH^F8-{#*C6cdWX+NqR+x-&0?iYhm?R`(G8 zW*m83{C+(*c`MseS(2jrv_XVcG};2^`l`hJgGBXy8DgcUEO5bS;|hJ2qG<9*hDKxu z3iof zT8<%u_seOplG? z((KG6Om9=?28BDE+aR?(_A^ zYtk2OJ}!`B)sC`%A5zGc;sw;+d=Z1uGQkbth|oSBvcH-+>rG z;yF5H;qC#%ob%{1+VjE<^C`kUQT3eZD1_Gr!g3!MTRg+Q}x2(O`&8nbXP%Q{qRBAyCFK$P}ND&`xpDR>Ib`%Etiqkf# z`}T=~y7(LyokAm)7xcRFT=En-_iKXhqUN@);CIHmsQKoQoU2NTh7yObg9(xOAF}vg z9~??7f$9rox$rnEw@7l?rau1y6V)xK<{9OhDp<#s70%!;Ni4JOGg9EU7Dn+SvL8iO zG$bNO%M$xHX-C?Lzsd)9(0IP;_a;({6Y>Kue-GG6%YeSZ!YQ(Mjw%vPwXdd}V)e3v zdymcP;c(=IFZ3OXKUdZti29C}+&3v#LoX#LVwa_35n=7HWV7Eh2_)%m)#1qgjFQz| z&6VjO&y9(HRyjfw1XpDt%>(BV&~;WZVTe7RY5Ea;otdACSBA0jYlEplZuMNJB%pcg z20RSND^3g8oy2WLtTeThgRNXZ9=0Or4o6vQBe!Ij*;r5r_GDL}*5`m_g;OEvqVAO{ z3I6`!QZzxzWdnvM=bn3^#9uw4l~_bk%*(R3ySh6gESw&vZ(i3gzLq;>8gc~ai8n%7 zL#7Uzzc!y)MC5VEGRo+r;p^Xv+CoxT84T-x-DGSXn)K-}#(Dqm)v_CiJ;LBNWQ2gcF?SAvL#p8!mnQvz#z z26EHYKa3TpR#?OC}fu8Z05^O^$)2MSEUL4D-4~ZZOU4kZQi9s$3UY|%p zRmMMkGt@n-Asqj-Z>amA7+d#nAA9`8rw533PCeEk<&Hm@#G(Z}L7yHC)q?t1rzex$ z9$y^8Z205w>t?G6m9YaB&AY{#RCW*VC~*5Xe$r$4TS>7Gx8LuG-$mVjpL;h?#I)bb zDWXI9kk&QM8BlyTH*GO&5^9Awj=}Zt4F^{xYbNXG2GRQiuLbRX()zF;-3nz?ir1;& z94b3GlFhRfowH+xt2&V7BOOPbFX`6sd0?Z~H}urv&1u05G}Hp_YCbouoa>JrMD*uz zEXd!n6l(mcg9;XDsRk}XK%wKvOIpOjqsT#ogn@OiaD!xLuMUoPQ=f8ESWJUdb0E`2 zt3oRrkRj2vy0RDKWEhUgE{0FMJrpj|`?Sxi_u_9=6~@+wpCQ%FATL6-kG^7ckM^8Y z51XgGH|T<`R1dF7(Y$8IT8||k41s#t!@p|OLX5!6fZH%Wn!ZlX{7Uy`$B-IxRBhz? z0L9%@(*y9wBO*kiOO_N8SyxrZJ*rm7Z&$x@e9ImstgS46T4l!Hg1jfrk?Fjzy_I#EA&^Bl( zdpzW0A)WekkPbY*c?xPaewfh}K(rldd+gmz6olh+ho0ulz!H)GL0Qft_lqn#+|jO?uN;G*Fm*k(MkVi!hK_D8*Su-1aRcZPD~O)Tbc zWNa^bO4kQ^zY=0$ZAB1W3qj1C7bkmI)fJ-wsxX%t|er~CQF=H6k zOUQ*&N6B^36vB;miUE~L%*WuZw;a%uiR_RgAc{6R$ZCYjhPU)*l2|hY5BD`p`g={A zx&Bm9aXV@1%%$VVDLGd>@Ih36-=4raK(gGmUtw(Ay?(<|d*C)2w$JF11g-P11O*Kv z`2rU9)iFzJvq`OX$VHyN(q*`&k!81V!hBUr=s@}^>3KJ#W1=0)hjf<}qTu^c z;9#t{Ts7aaKMJprIM(o(448P%?JfyhBD=1Fhpe8`svUmHXBJd5ygtpO7Il5S8PjL zYoa_5JDJkElovx_Bfh8b zz6RBFNHVl_4AlA7_p=$N!Zq4(M4d2SAz_mqo9mm}rMrP&5XfTnS%ks@kMTXHZ)mCbx+dbdmVv z%A)Sqgq~QK$j+!&CU|N#A=x1wbc+B@>KDS z$RX+>Y8=opH|=;-@06Lm3^#g?C3*+SF6w_D=!fC8TY{53Ci*uG709Bk@O8geb!J{2_|x{{ z)1k4XECvHnSCZ5R`!f=Y{Cr3d4$?5?l0*2N7l-rwRoGvJENik3|B&}vSs~&x4}l(f zWjUr(M2w^C1x*DpnVqOXLU9%0k3nnyBUjPSZiq}p8}xTu9}jdr!PcRor-%>m zdiovj!}I?zU?dRyy0n1f(l}-^EGtT0ku}P(*eE!TicO9X8m_0XX zsg_?~Td+CpnSz>NkkxDKGlUXC6%kv8@9Wvtl_0w(8VqWATLd{##7=9UY>zCn{c`t| z_PWSnI$X6*z6vfEgyRoJDI;w%6W3sj-+P>%YF1u!T9HB5&`C8czMs{NzT z*yY2S!r&ce`ghZ2$b$7@xaxPz@tc)W1vGE~pq2{svOspHjP;A-M+|Xzgx~*21^edp z`v!}`6WItVicZCsk)&mx=56|@&Me7g7kYR-<8eEI0h$5!BJ`;ayfKwjstEntA3KhD zjq?}5dgLgl$J;F0%sHeiZVsKmj%k_taWS;MvYf-&JQ=b8SCtL)Nb9mkWPJICzC8{9 zUV3h%Crh~XW)z}2hb1BAc{Sa3Cy};*TEM&``;plNimUNz+GN}4w?&0#r=>*2l&S>` zi14DTT*u3#UG|v9#F67yoL}8P%)*%{t@8=a-1f{J3WKNu%{PtZx?^?-Vza1^nd+GL z5>SB$ub3U|=Yk+>anx>%m$E_e2Rws{$al!`&`*REF4aO*)j#td*6jE&R?zxrkDW~i znPJCClg&yyZpm~)*1~r2a^=9JkB42MLQrgPeRDP4|3A(16ila zPg#_yBVFWl`?N4JLk|y6iPK%kGexWncSI;={T3_%v3S=z9I&%HhFt6}Yg^_jdvNTB zxS}0MDlyEM5D|D2JboHN!*!NX(#l<{sn;+tT}Cqg_KYXSq&ZIUbodsBk0l#iIweBQ z^~&4)|7)HIO_OgYs$DRMihN%%DVz=B4C|>P zAx5lmNRX)wloyJIVMn6pag8ZxNM&?18J~vStUlTZ@iUn$y4q|XXEI7L)D_+m5=n0| zwi(Y{FBHRd+*0)kT^96=UmN$W)w)F5i`lF~qQnc30S~{t^?)zRW<66DR$4z6Nq22lRvZ z1|e3$fI3w<3bQ)`efF?JocCRjHS^JVy$MR@TJJixK_vaX0PVGFuENdVA7V4o8`l9E zCE=yWB3>XqbGCC1H3LQL_tB~n zDfsX2Jai#)(48n^)|IFpwSwc*EZgLi5SBV)Ko5RCg6+y6a^kDFIyxwUT1V`+Dr4)p z#@fVi-xm_j$BZg$9?AdKqxkDcCTco>0311XX&o{VUEx85GJ<PRgQLm8GKK zex6^Mt0Ej9 zd2Hkloa?Uj@tlp}u;OsIOnE~lblU1mBw7-CM!6}olfaltw2Es;^n&JP?FeBUc)02+ zPf0sUr=*RLj6O&8f{wnnK<4txBlpNYoN$F^%tDG69*stBQ0GU63kirfpj9eurNf;Y zL^%1tgmn?zi=X2}>KTO&<|aIJnVD?hrLZ_;Fe>gy=ZL4*mAw$>Z7mMcTPUdJh5V_M zsTHGwAmCbG*ZOw3iS9A2++kK*`N7_kAg1kRAPJE$Y#(Tj&LJ-j@%5RJpWiO$NkNdDQ?`vsC;we}Qb zM3iU4@FZduH$XU`#yN;7)XkkR?<4t|7Dmg~HbxUMjsHDU`j4+dQM0+@^DDM)lVwtx zSVEc}aMp|wy~ZP%3pz^Hzfn8MyAL{L z6@ckC!6!mXQ7_rP_^Ki$?v~--$5}<(`W7V_k7* zyFn)6*USsvB9$3%oV>rk3gG_1U!UM!GkA2ZrX7Xe%7G1p=a!f}5wD(9I$^E3)>vrD zVU7|p$RoA!-0!zMI0UAV4&j(}&&Otwh2UMB6%6SDWEW|SP0Rtq;Dh)U=`q2`z^qM- zrWNuvB?F3vsvfo7#BWlxnwVzT@g84~*?=||!C-6DK54}Q+4pAh;XyZsz2M2Q-R8b} zgCS#7tg^R`aL=G4I09Z zyGFvj-Z={GQCq8^V*2W5-xFeYK;myn2Z{MycCLvXi8&iiR+134-~s-^{jxyYaMX9; zlNMNgRQZxl}ekW}k5Va=SonTh6}|G-A!AyaYdpD4*i*70ZO_>|9AC>WRL zNZC%(OFTntfHEIN2;~<7Wb1pI_tpg~Rfxg8%G&RdY9l1iE&+B45b(**=y0sWOxTIg zx`9_B_6_NZlW4m9POlu$N`XTQWm2<;~X? zuxwqh-Q0U{*gLX#RXYJID+Bn~s`hEj_gn%pO_QRc{>R-0m6Ah!GkIL;Zr2KEYa4=;C1b{nB>c(?Vsm( zoQe(dUo}F?Av+K;C2S79QVID=`@rW3ME%uI>hYO(Rj!2P{#V#EcFhuG$a-GA*wE6O zW5noUmbhuys47eyYULE?cnt9=t;Kx`o`)do>A%X|zDR2}bxUf)n{pB`8^LiRDC%zw zY`#Nv;QpqqYf4I*_z!6Im=(*`e9#vhm899BzDbtBg8Xh++n>U`fG zPF0X{#vG(85L*jQ!hcJk@)17}n+T3889`T}iz~)ASxsFa8PoG%YIrGcXJLNs`S{P0 zHJF8NaxKUA9nZPvsTtF%-&>Rbyt(q=khYzG(ly$3YZ9@ESc&PD55+eoV1rVb z$a)HR`T^M+MvghqdSHexN*n7LI@YEwnmPNDU=vJ$6K_Qk{y6ToK{p|5tva5J^xXyr z3g6(KEpCTDcM|sasSGzWT)CHpL%rt>ek2cUXWV5iub*drlM{Bq^D8M1C=K^xwEs`e z3ml`$O^-R?l|mc8Mq^eV^Gfy720lICL)7!;9>#DJ^*$bJn`$Ddv)N-v2m50oUl@sf z0mef5bBWgseovve;tqo=vWNCJx#A7oNVUm|vV2TidT?q8>7;jEa^A%NmWFIGiuf@p zi~3!Xt*uTq;B9CvzD0bDvHAW)_98QjL`G-%m5)DY9iQQr0acIKKm0B8GF!a%X%GFa zsb7y$;Fn%dj|PKglaAg_U`mNsw_amIxtjLwv~u7hyCbH#bxX^LS1I{Jo})aW@15I} zP1rpFp`KJqFmpj!;J67*{9jJSeCwT($o|lnkZIPT$nRx8&$$qtK==2NAI>@v!>tVJ z=iR4uGnnm0@#4BLfi1eP2lZ+wE)w;p^-tTzli+>lxX=~gQa*I7WWK7`a94k z{1wpNaDKyV`cG{;U6|aw4Cx`x`{K&b;kAUr_FxB7=CL{0Z@D`vOTL>{2HCN(>1NOS zoOm%di)t6ED+8E2rHx9WAZF=Q(tkvhJrywndpM>z_;u{`Nbc8+yF942ko6ZZVQNHt zBqt$e-NOvahVtqr_ebj5^qX?pi^$d%MM^N^ARF;6da^{Pyby)3)ms8l7CwFOaw6yV zhl75I2H2`uJ0Wl`0l80azNu|dzt~2dKQBG*R^qt1d?DCx;8{GocQR~K_h1;-M}hIq z6K{;SsGUje4V~pT#~#Ogc<@meVnm|-dlg+jrWz=6Ot_|Nfl%D|Zj$srPvgM?)&{-A zO}?(+gz3J7rw!_C^Ov`p-4Ur5b}%)N&A!%GRL^UZZNj1|WJ*iC0XhE?%u>C-6n9eG8YPZo(p4KB36JQ`aveEXBBc zIYopQ9oZ&BG7(9xt=P*?7&JA_p>$hz1@com>NSSsdN!lS;}0U_lN0iLV>mNwvqm5_ zSS@@-x-`oY0F{I&2i~M3B^#(fF(mG;zy4<1p;t; zvmznl;i7^ZK2sM?7lT9?uein*#uZ`NkaZ0bdK}Grl`$BAdOUgW8tWjJq)nE~G3!mG z?vLO6fK)-#qX27c3OWD)MkA^O@KAwlolKRyeX*>k=YJ*#{>4KPDpcB+3=-m=S6xzR zvy1P(?PiwfkEoi)@iK_GH~V0n0AO0Ej$ln@)iLc&8G8(7M88`#A(Y9G1}$H<5h9xX zg%@09@O8hUgu5;zbs!}5Ri09%VFJh|vVSyKc;g*-AdgxU|Ir--@f3p(kFJyRhCn7= zD*E*(>q(xqpY;-AKikH4(oIZ(z<+=#~H884^A(G__be*w_gk@JsHyim5o1pgj|Fbalxs3;i^%xSFM6 zjBPCGc_j~buxPfGs3@medybHSt)i!d5%_(%G2bh7plHki|NjtSG#sEqK*>L-T4-8- z37-#4d+=X>XxAvDobyEay`%?G&2=K;6+%%An3w}%+?AwdF@F?N+6-;c@VE3P(NFGI zQLcbL7O=1^fQd^8Imr17FxTY9Bt!WzgK?080s$f135|OCRk&ZqvI3rsos|Pp=i`LP ze&&0_q#`Y;GZ=7>vDq0&+yYr*Jx?4_=uBZGlualDO<+y3=ZfQ9awN-jP{-gI{>!-Z zqp+uus<#Dr>#1uZQYL^%^mdVaH#5ud%jrg=6ULtXFJpUqemHR0^1qh8dy6>25yNA+ zwLQ{;Pj~6KDy2M7_=wo&jk}EK9|=zGbBl4q@#0;R|MHURQH91YDaF;6X)!}OblPC8 zKSWFvXmPDf3BykqTv^N;Nmd19NO9=4XiT6*k^)Gx_>YyRl* zp}tV|E8?}CE@qK1rfx-Zn~!X_%)zIOrOyp(Z*OHEeR#e6c$d)$%)(1i@~FE4H)57^ zIubJUDL?j_GbrAGAgu_F`-GB&P@9AjRL;`fI)?Y)gV2in^l$>}FsuHwLv8ozVVK2? zH};3c;B%Q5_Q-jcYWfZQ!Rav`hLlVEa!$SDMs?kt6u9}L$G;iIt`X(1bKet{U(=E! zkS>1C&e0myT=|I@mi)jP<_unqq{(uz`?-;m?9Vajlk+qxFf_^7G?EOOH!)99U%^#g5H9e9!WDy4Vxf# zF%LPg>?>vcSawJn>7m_Dx9nXS2vWs4o9KnU+LPG%vk-1iP2CgStJvANhq-OA5E)N% z?3uu~N7v{Y{2D{*Q*b8+1XG8SX{hRk+_*eq^NQJ1WX$4I^Itf(L zYjPCUUlT+RNjZaI6064Dt5{5mgoZZd`u?9e(~lQYUQuZNC$8-!gKeo9XuC??-Oxvg zYjXPPE?sPdOG=0(P`e9Q6Y`sAfYTazPqG$-KD|1&MJ3(K>?Zn_kvfzH%1mQzL5?W4 z`|@k#fO4oZdU>~{uucW6Vel>T$X8Q5D{R|J@eR~#;U`%Mv(MIQlXiTi#vk^DY5Rv~ zO6mO$y`_WAB~h%2{yI71(?Tx|j9|In8??kV7UMKiYSh);$$*;^T1`60h3nR&EI7W* zr}aHx7z0>VWhuy{NzUa5Rt@KPKHz*0(bsMQm}f$JVyK^=ZT>i>u|7#+EqAWOMz1jE z@H;Kb5q6QJbc;Cs_*mTDNi<7w#^4ndlX9f^i-Qskks}F-6UzjLMETU720kNwKbf8Y z){ktMVNYe$xc&p~VNAIesUzNd28>>sYne-4{8Vgr{#f7VW_)X>yQ3=$HmA(Eb)WdP z`}7rX4d|YzC6*tyJ3-G=i(Eyn(Myd#FbvJaCuBv}X-C~W4U+jG- z1~Hr#{NY=%JxA-F$w%xswtk=mEWk>B2r}ANH&`poImm$4Y}_l9epQG4B5xr1x)28|kJaC4|LddZx412sA^-eKFx6XsB+ zEKz(UX>||Nx2}e}meeKL$@mE>$EO&3;fiys8_6n3;GPD?hxD;?()L9Js51gz&po&;Ctw%UbZ@;R~~btdg1Z6vADgr!#MdW>f)bG1(?lXeh{&)I9HoEs35iU2sYQb zT9D&*vi+mi#{YrM|9l{^;u;eM3UJ~xH89}c{X^x~ODL>fqdkSG)cz@N;zu=Z{ zIp@SLr(G+no^}poPW?c5fDefaXm`JKZ7G{e&c*;dwY7nmF@hr%O4_MhGKWK}~Y zMz0i9!%DEMrn z*BFvuKu7WE5^H3{OR?RG%}>d}LL1z}oN|2wH*ZFMQfz;&hy3+pg{(8?<^y7@JAJ2(qb#R|we)~_f<-xJkY?IrDyF;S1@scUoSjZN zn9ulHvF-Wk|H(rdJ&K3B)urHktP|`!UKy&9&q>O_FExA=G`?tqc#m>Xr^`@-5kxf1 z;18x|ZJ`mRh`18Svw9>ZKg7yr7j?xRL@R+aOCxUP;<&@gc#jmY$guS9Tng`0EN#XS^bO7cy8%fI-D~VcIk&0@-88H`U zXa|RHJIz#eFZ#QFMsr4V$lTXo(LL>+b-!i37{Z$DVsg_Ch}^{H#=jW(l#aIJXK*R0Pt{9~ffmDbI({ z`vMC|eL*oi8a(ITc^A~8D~(dQ=f}@R8sxkVA!nTQ6LX^iDK&5p@MPeEZw zz~u&C*r2|YKm@f~lk!AwJ^sKp;$+HzJfvY5n%MaS8;7So>MXYDqCac+An0?|bsf`h*EKN7wNU z8G=N(K7yd36@tmLlJF(qd&-~U*qbGBY9xt-tURzEc|zF%ONNfXq=DpcUB{~Tk0mU> zS!)y;@TZeC>baO$YbgM>$i2DV4UguHHcxLNt^A>leUofp`%33$eHkMk-k=2~vBU&@ zHQpB8hhVzB8R%lEWNyag&9^sY&QVGculT`v#Dk+hU!vL(S7oQoe5#mg*jB6Ah{#Vf zj*-0>w-%p#iCFwBFR)RKuyeP4E>C<UbgfJph#vMDx<p$*MeLrGinQBu7+2rGkDif47$ERoFYo}X4VdR$_`{gwN8)+zijk_p!ZEgmo{z7 zHPbvADXbdLQmODp^CZBGDQ=YJ=fBDn7um3|pHOFYJ3aq^B z4y*mAL5W{Ob5k;ENwY{JY3o3LHuWYSsCYLz!CdY(<=uyZx>=~8?(`{dk4N)uh_2CEar7-Yk9wBSo^Q-%!ld2HKVAcKJ9OZqJQvFneA7* z4i0UqwZ70};dR2VEQo9&2g^K3ons;4`;G3W65hg;XByTwJ;h|lrIIAZT%4DEaC6ZW z0~8W=?m;-bgOvESQ|XSwbZ^kTf*VR5+0aTnnELncK4MGo4h)xM8>G+_Xviy}MOx20 zerCvXK36&fev3oop=dy2{`vIe>fD>j^m@c=_|}5DgW87gbJFK+U9x{d^qTX%nStMR zEE2n(m+Mbd_h2t-0&~2}f9GjwjXUUH%iQ-4w(fzNH-7;#H4;n>u|rds{GL@=yflME zVWY!iebit7ePQvEc&aVVN9VpqNjE-&j?`A|-{4G2?w-8=Mi)Il{J~H{@x{}%tAEyd zipf{n=0!SUiy-FFt7NGq3RDMlvGrNl^;S>jt98SO0MflDr7obYd_fmZF^=2Q^q9=U z$uM648m0e$(kjj!f|zj!<9!Ttcd8MoigV6U-(DX3Vm4kjGJ3J`Qe&-&&AA}h6z7IO zF4%UOz7!@D)`6+TrLj`_HTnbwc|?QTlc$X}Ob?ens?9~tI+)RQK_U-tqkR0{8Hkfd zbIbV=ZN!z5wnhayS?9uCr!;>DS}Z36enYtQGbOM^o`D}YD2NE*p49I+|4R&TCQ&mV z;7(Umh(_cofcoqrUa^V(knMAWg8~TZ6s*G-wLwJKauIC&v8zCEuOo1JVE!~2=HW2v z+VKvyGn`3~)C1AL?{6PgI9xr7})i^hf{sKqcwkuquGJ6>i;8RzCmu9B?kDkwzgXHL$q0RF z-7@)y1%Im_lnvAis_{vEe-MF(Xa4NT{`|UslKQHJ(A^5A>1BSV_cpj>qv_i6+b~lf zY`>$Y&B$LUMNlcuap0^~$kyNw^XYj~AFq%ZOU&nUgBx=<^*HJ|6_S=YD<*68^W=vg z{jKvH3fQ!h@>By-&})az$?v?M&a=T*L_KdsC`LhsNXh$zGs%cyxGP!?aFMt6awu;g zcdE}Mu5*X!1>L91(JO)62i!KE%p%DVmS7<74_S*R7)+fs?`3BMGryp40;&84$4Rx* z&l0yaA1O=*EpwFNld4!bfRn+mvbO|LV%ESdcpf-4BRS>k3slCB{s|Y0_TXNy9C=DF zQF-yv1^nt65tb@sJ)A?pEOG|Bi$mZx4AlxdPZD9EP9h?`BH3emrstUg0Q!HInAOmZ z@fXp~m5Z)By*Z?c=0hi0xzBn#)Rl1b3Q_1C2uz3zrQdez6Y+1dgx4F@K0+`@G_aK< z_0?kiehOcd?F%XnF8bLK0b{fd7Ylqs3tCpD3qSg~ay=*Hy=++gW!O;1yEBURn`W&v z_UuE>U=?eB)D`?{F`j5JG=9A*`MG54hn`*K9JN+y|acq^!ZDzOKS$(NJ~3-$$aLhEIrJLQ6qb9MuasdI3Ci;Qw$S zC2t}Qc{Ni0qTj&HhZ0yv{(vXC6(&D_uinT>>a(f4uQ#&c>I9r zYeanxp*v%(^F;XW5Pkzv$v?Izch-w4RpbI2tlZ!;(4=&edfMsweWb_a!Q>%$4@uiS z_nnixnIrSr0ey*VmiiO=ZclvCgF~f*DIoC*x!MZq$(8hmpA4l4j~)9^gK9tQ2KV0} z_A?uBY_RsGhcnY1Mt^kisz`nCHJP{`oj|-Q-u*Fje03c#5&HCbfndwH9b&1NtQl8r z{C!g2SC~Jgu4DT6F>kQ@j$#I$n8e<2fbqmt{unDQO)AG zmZBmpV=7#~ccZe{ZmjJO`O|H}dVfI@KDv+fPk}b|mFAxWZ-MY2 zGFzYKP)Tsnq&|UtU8uDobumwy)1nynuK*2I-;slb9va%mKDzE7(^yrz^DFX)xH?905b_#m zt*a0A+fyFdLr`LfRna>UNUyLbr>E84adEo|i7_uHV1k!>YVHg0p7YOo$BchrP@#CVaT4bg@;7`D`z}o^wD+a4rp?>(HK(59#Zv9d z{!9mDy6q#O!^axP1|BB;T3HSH>4!O_DfEBt{dz){4)(+5yEj-n?~eEe-MOvSZojY> zzyt{e1qsQ~-CbfZQV^s=lp#F{Y1l+T3}SR64y6PHDXIB_1p^cb>7m3+BS_cxOyB?a zeb=??QWSn{=Q+=P?sK1Wo@e}$t$3i3&Rr=1{hLoPr!i4i>qh2;$cY6@wIvk=H}!rz zA|A1(jIu@vzCgaNC-1z*0v#i-=qUE9fq81)Nf4vgzq=VS>7+KYHDoJXVfEG}D>(at zBd!PODoT{!rtIK597iF_a3?Ag!Lt#DmZMmOdX9Ih1&#aSV8sm~;xHz}tun`E;Zz_j z#ex|r0A$=g2u8$_V<>N@zycx!n5IPftKhg6GIe2YpkVA_w+TLLb)C5M9>rNCI0J%p zA)uSXY1LIHn3AHNf3|jhMauN3D`wbM1~pjMD{3qm>={uQ=eq-U=}1QcQvbZlJ{xzq zyIfB6LOoKVRbjMd-x0y|%2Y>QR67F_6O~0c53l!;c18d;{qMSTJ`(Hq{9$`vti?br zNB*8*+-Da;V8BNx8^jdC1dD>02%YzOjhJU|qK=#KI+dcH8=&%xU=768dfLw1Z2L?xt|^=M9i zvG93w{u||I6?dtxGj(c-ihw<0<-FyA8}=WOkr{2wYkVxRd$;0B?k$gbsJ?mk#az_a zk^`-8Ha%rKp-uQl82dP3J(QS*!VjL`To3f>DzhdJ@3^4T(5n3}~ZPngsqo*CpW z+%>25_F;j^%?VyX{h;rIevg)AZ$P%!hNX(2#>D&`Eq2k@o>J98;t^7aJmFW%$D$bI zyiEDOhl`JbKnm+Z41;aNE*2Mzsjn@@(aZtiK`e^4+73qG_PTCW7TO3k9s3#>=>l+cOL z|Mdc3NsTsn78jK5t>C5%Q`@<@QzI=(g(HXBkKvwfJYw?-E!W-l+!c|i0mW#wng0wxOrE4Njn8&t>u87aIG0#y=ymiHlxKB zh;VRVE^?&!k*%z4LOD#))L-dFMv9l_%EJ=fuV~DXBh`1P6SYmpnaY`^My!Ko4pD>UdJfIZ#aL(3 z%qA9$b)gU)iUYnVg!L2JPHn24J6G>o)-jS=bKZP$*ks<#%%>=bMJ@F9{TufJidW<` zD}`|%*KBf-4DaKKAq2UOMjN&U*mwG1mC&ny!^Ph5r3&LlVVNdLex_b0?qCs_wv;5| zsD6>|Dzd`abif)&5j_CoYO+r>EBnuO5)CZ2TfBJIQx@~&J0^opuhGdUwV#7*AzJf9 zJss#nj1DDxkmk#xD$Kaefzbbe6(oY+9pB`97&-SBX|`bqsp4&>m)+BsI_ zvXwSf#sxLbg#^vYQM&lDrxi36hzavxE?-#d*NwMq4NrcDIO*;Un{|Q^;pd1vOO|4x zglsp=3J8)V&F!C>X1yDHXeJV79=>uz6#{%NRghGP3c$4XqXkgetZ97SLY$_@?_(ds z9+TFG@D{>JELon^Xv09IM*+T7UcH{8vyu5<_H7$FoQt@989p7@>7Ylg@x>yG&}>wZbFHg8D{33*mP>v||JzTa6K z-sVD%4FV_vspCHn6aO2@gOg%CLog%Fm(r1SpGSzX`tNlsN<0z<;5C4bm&(hE1aXo{ z1xuQSest+D31VDx0D(Ax;3yb( z6W1PH9c8dLBE~Ry*pY&~d}~X1!EH9|p5N%Kl#2m#?#aJ-9@-7%;@Fy>J6BI|yS$jf zt)Fm*s2=JFNq|T3o!xg}8iO=nMc;n)OOf2%7oG8bfe_Ltfa*Q%L*DG&5ADnE&3u3P zrI0^kae322QB(k9vAhbUCS);qf=up<{#TY-g&(+_sxJJ>S#*ojIi`TK z9M2WbbV0>kd%{ly^ZTPYFQp*smaF6#jQiVvi_#zJ!&FJEG2`^r}NL&6|U6$-a`)**L}GS>Dkx z&q1l)94jkv=oJW0!N?DZfl*Il0`}*<^-1b*j$k2?sHzESiPMSdEGnwJR=HCp1!ICG zXx|kT=hp>Gv6Q9Zb6&)bDUmkx;WMpgQCZ_<0Qu#>^0*1D36h5%eEgd4iaP|$Gfw#N zY68|01yP1po?&;8chCu)R1()r)Otdtv>LJ*O52b${Fr66GV-=Z#%^4u9rYZRMr(EWW>dvx~tP|0n}G z#@H;#8^%=yle%T*FJY*5`vyZFpO_~^d_DBedWN4~@&#|L)hO35KD3{_p0mN^1egl1 z$v@#u{e7tc`v%iGFGZ6l;3I#`r%;c)!`G*I_5Or0TWPv6NS)q;B(Tr9IrbUP&gPxI zcL_l=obi9z)ZXd9hCy-SbitBR=9!G~>A))+Py{?*Fls7o!Sy|HDGXjC*z)z_dQ^8_ zqH3jfSQ4WUcSTL??!GktxZcbNQ=|4;w~@`^yyh3xbe|C?e1p;&dtXb>zW}H;53vC} zTzAk!Yw{s62v!~Y&w5ruC9pmMaKS%@B7ql&g6c(mH>-&cOM;>Gr=woDUJFc~&8-l~ z{dP6zBy@DMIaITIQ82S5f0t!|$yl+DYku((6<8D@@c0~sbYE4UCSyrW2=GwG`kPaW z+K}*j0W<`)CxUXoIZONAkPmk6Z4}{Ig}pOIxvcYtPM&5HQx!p#mSo3?qVB(W2r|Th z?%=-s-EN%nuf2$WV1fw2vu|{DW)DFMq9Djy^EozEG+`N=dat_oMCYrKGNLM|m97s7 zjk_IaifH%kbx2~`Se3u@f8G0&HQ4BEkCa(X$PPJ?DGQ2F<0p>FPuri=$m5&yT0i^To35N@EM2rx z+78{5ouOLE$O<_+xr3CcTeI%x5@xZ&H>g#~W8AME>6|~G62`V{D^R&`DGho3R|K!0 z6tOW67bn4N&Aq)-B5%DUfT|I9@S9WuxU|Sft|81NdCRbVFPt0j1mnat>Hm%!kMNAe z#Y5{5gJK%-_SgRnSq3NZHRTU$uX1A9N_n#AKyK0euPU!0^JsUNIq>;v{@$6zUQ41G zK~*eJ=08y?ogzwEB)?*8X8F$WJ>DWk1jUhsfY*FPpbZagVgnM^Oq@B*Z88W)mo$R2 z4b+f&{`I%6u2@T;mMm`K4)0Y*!HrIKN=@|=+v(fxKi6J`9YxQ!xGhqno4yxDemJ## zxMxOw4}2AmtnJe8gXXJ-bEt@1OZyLhSZgN)*JY~eGA|mTT>9OzM+=Eja2~3+pawNy zrzSFlzgQC53pu7z`kd3m@LDjgDcLRRpJ8@6h!o6`&nd1il2|YAx+xfh`Z1HaVK7Rq z!eCvrrqZurl{h$m`4wF2Q8h?xVFI#}X0PmNKXeQtX4?H4;q;Cx54M8=dCXK~D z93a+$$Oh!=5b=_|r3#0xK5kux8IU(k2_;c=U^z8DuN!L%ffZH2_ z1UDa5E**aZW;ckHD@oiPO5P_<6(I^gEYO(nojtk~EbRXDPKnzu9^P5Euj%^QPWtV? z%t19dESi4~mb1v7@oBq+)?X9+dh^^sI3jtLPgz1EtyBWm{A!NcoqJ1&><8}NQX6q7 z2oiA5+hXfjZmUR^a(7OgitmxKhHH*6ItE@P&_gm}q+vs=}|X7?CRqycovma9|h+ z4Kni*hxnchMY^4Ck>TnPePh*ecC;_b~`4=(anuCLT@d2a{^pT=y{9O1P#`@=_twx8M>)k7_-XO zky`tdN{cx~GtrahbjqRsk6yp4`z@>)4(}r?L8L6+eOwn8p0B~{1U@!L>m1Gj7iP#r zNK7Jx{Je?t)b}5K2^~%DL3llv$C;*ZU(^Ld?l5f#w?OXxj;QlIKDw_8IK*@jsqF;?4`|N9VGGM*>2f?+pg zP93c#9wRg^ie1(?V_p33LjwA-#1(VZK&>jHJ83j`89TLD{$=cxn!ip?BOYjCFTpni z%Fh7}o6=(tBekwYjt7|#fP;^v#JWs*CIRn`|CEJDfrsh^$3Rw6dL-sty!g!hF)SUD z|1shyS8Cle5ut40YOi+`hTX+>%e!rjoIRJ$H0_~OP}?4g`%w|f^U1W3{1 zH?+x>#J9jGxvN#nlQy4=j(s*|P!Fr-@3$ATV&{j~oBh7{T)Y*|CLAgE?`O z;eZtkfAg+c6BYYmCjS5^C@}zGtcz;SFnUvnJV%I~nna#|B#L@8WNe$r_6~n>rkWPR zy2iWa>bzUso@CpO@3>pKh;x0k$?a@$b&DM|37*xa+uF^lxz0s723{$wTr1P zu(3ZKE`%+WCjnnPOuFFe=$^kazxj`6?wh9qF}CNKjP1_uob1`%oIBh1>|79J2yamB zBdS(towWEv!Xvda17^>bsLsdBvYvw2;`T}TcFX)3M#^lF+dmy?LdvnKy@{Jld>LHx zG!MQ4_kkQyWSjWzze_FO29(0YX5Ut{)0 z69%IFs(W9Okd#_HmO%>Cg67%Zz<7p!YHbS;1h>C}9=Kdp6{4!qByvgH8X4<`-K@8|=>Q zbRX=1`e?3hOjXyL(`Y=ap^rFRN#mPSBPY?T`m1q%SH3nlOEe(rWv++LNT9+~El!PD zO!9-yp?f9Gk=h%`h{=uL6QiK{&}rLME^l8!x;~k4VUkbeScH;|wwBW_ok@u>zgnSY z`puB}P_?WXV5-kwO#PIxx#%0(4?bW71aUJ$1&M+}HV zxM<9~SY#?IC?inKdxi&HZ5{D6?)$JBVZe0JLlv+J_gnMOV!!{JtFtk|t&)!ui-5#j z24SL_nY(qvan43oX4J(9Jh{7Tme{E~90vP|71iAZ00dsSV(V8WnT$CbieWLQ39uNo zaMne19VPW+nw7;E)uqqh3wccvZ3m~zMGxNf;whbw!>qZfrC$7Z3Y+7-?Q za|M^rZSF|4_1JD{&;~btIj24>+L4O%F&3L{6%>gwxq^>0`Mm5%{(-d*eDMi}72Be@ z)V)cfdVz4QH=foon(qBdLC}|>3@S7Y^gF6~KUK<36OV|F#1i5!!rU|CAQtz?{U|IG zE4Sge1U%t=YKn<#zBd&!KvzNjCU z32woSMx+eOqMx@-bbo9R&l5G9e`(E?-22(roO?yh!h5&i;5k;{UbUMdqYCI(xn+*a zPSLxVym@!t{J4#ZO2uDMekl@EU^ z&+b)Y{KC&h;?gd>WAIN-rZq}( zWGtg<!;E+oEG%C{_EA(aNd+Z3>Zf)q?$cgw%SA*kO6l4e~aT4oDK|Ls(B#tYhSf z?>Pz+wKz?jDO4EBmTbTxdH<7iHS#N5pWycbOh!(de9BOK^t(2);fO{fh3};{g>u|$ zo=m9k%%H}E%r=X0!ML14PaOuQ{GOXr>C4}=#J<;$734PH*k&gN27Gw?<3%>(0k6z) zOUEni+yPyJc5Gvdk=lKePC>ubr4-_lquGOJ4!VurXLcQJPEaJa_n~PPC+bx(##68R zzxEL{588z8GKHlco$%5ZO9^?lX1#FRiF}ug1-&(#Du4M0L2Ow^Z9A;zUWemN>@lCF z@_EYvVEZXYxE75UO^$)>#?A@%^PO$jF03zH9sU@;4`Czk-C{`2_5v$!Q8c8_^iJRv zYmW3L2Q(ZCPEc|LE&QOJ!fz^7FfJ?oE3sbxG1N+0gSF(_6LFC6x7TS_xm?|nj`mxe z4e5i1nGM1wI5P4Lr*-|(kb~Fy-B$Q0tDBYn^kM$ViCl$k2?{>p3E?kj(7kLK<4Q@Z z1LA+0RS$9$qO1|}fFejIF*4Ym`A)N=vUZakS}I!pWs#q0%U{bz%+p0@R*Ai)jyjE$ z=x-A9q_Oc~_+(cTg`q&bGG!M9`?jZqLQc*fJ(G&1);3UET011n)rmt8FekVa6}N=4 zg6q1#4}&H)WNPGPbup1`HbU)BbE%uKnz9{sO^*sUt%Z$<$`$@?rz<@(ls6hW{**}7 z$G0;C0Ox$mV}+MZy7RbO>m2h>2$opspZ4E!(jbxW9P9)2KxV}&GIU_1VO)uVL8aA< z81AZm$kBK^cJ+SzW&Fq-O!wW_Xz^a*TzOu^2M`h zFe!Na$Jk7~g_Rj64`4Ih<8(wz0&RqHD{A1eLH0fGR_eM6vKqzQAR-|B+FL!15mE~+Sx}Hyaz8YMgt0Yi z5tv)WaRNA3TzwK_T{AONd2zJWANjVkGz{euUrqG+0W=GE`YqC^_RZ@(_>MBsuwFUq z$1pdu)-wkv@-1g|XF)QC>_&{B{zy{g89y!4n9zN)bbFlBo57qh zF1`TSlsZuLEZ{$;S52qnP?n1cgvR;$WCIjB<-EzOa;QCG=~^f+yCze3oJHCWJNXD7 zUGnUhH&?y!_B`&yL&>k=hX>C^bKu}dZ`>e{iMix+>Mq~oy&AqXwsoK2bOjYkcG6e%M9@c`*cfpA>+mWn`*dM*_TD7h zl}OWv_9~Un9`BJ3o589mNH<*%$PKz4aQ?5?noRvi13&A1Q=)3MT-!5+euRoc)FCgS z$#n5oA|6YietRZb3@#HcROY1dEqiiRoSqi_)xsStqu0}%lPl{f?%YVP3N&E1PxFBx zAIimYVCA-Jdr{#0&@_aWfmmja4MwB1=QD12F^Nw3IoH{gFxC06_>^nkn3NN87URNw zN05LFIxUV_;c*#s62dL5o-<;NRfvtSp1L!=1zVdW@PE?88CLPu3^5YV5_kIG zN&Y&DWUfSH37r#kko})k^#^ZyE`!P_srXN)y7|XOVU?(8a(G~!H*N!W31mN2qTsw4 zf5JZx=4RJzId)K)Hw+dqmDK12FazS>!m`8Qe)^_xSmw|wOQ_S~xO6!*yp1F?;1c;X zEz8^2IjLZ2Q-88d9W|}%VVG#3BXy$r1LhL2??H&3o;>rp^C{&}bJNGtk)k3vt8Q%%v}J>bM*ybMHk#A5zyDlG+;L!dcRol44&pi zcEIi5x|#m<&=qd<2i9p(I+Bf}iZWXgUFrw)Cl2TmB1eUkGuBHZjP7~-Mtm&~Hk36&s!z2>)tu7H*WZK$M<44SzE-G= zmZpCjYEV$@d< zBd$%)Q39en84Y0xbG!l_g*I(AUzb*?Fn^0p*55FgW8*eV5k~nh3d6*I?aj5oWh%%G z1~%CW%Jt`?)f3Ei{}&!_PeLy<{oIa%sIWJv_F@3pWc(|QdVrkrB(w~+O%06is!rz5 z+LbxF(-#%A4P{DV<hcL-TT1{rAis@#DBlxH+(wc+!w}StCRh@R$$+gKrG?Nh=XRb$lY2>=KD$0$9=yv zN3Rv%;f-}gg-OMFlo{QGHr#tt|D0CKXr##@bq))z3UluXBLuraCi!1E^>{0kd|BIh zk|}@^TsRH1CT9J{r%a?sDVZj#X?tSfw6wYR^M{pRTmJKO1(ixz`ew71u2{lorb50Z zE+A4z_?&K5(@4BR>;#$#`f{uJ18j7=SqPH-RdvqKnbP(E+cYiPv_b2kIKMbJmOz=W>)!MUzdNjO6I^1$ zlg6#DJ? z#7F6+GMN_Ff0sR~czFPnOg1J^Fq2_|2EjeiB>Uumg5NB92Zz~-FQ-?Q=-H$1@XgEg zb1imUNo17J3VC?l_H@JldI8#^BK!6;x@OpG_@7d1*-dRqQBU-KIW8}2aijswNOB^_ z?Zlza^+6aW=OQDUnyd?Ar&9SZ9zHo@o^PAForEY>ACi|zT1`ewQ+D9y_G;cxoMu!T zYl(*MXr7Zl$!urS0jGTB5%3A;}y}A;WmaW_`ydU~Ib>q?W5<3I)0}U7C!ih5BXuo}V zI+YiNo|+fL?04NR4PAS}PEc$;y~w;!$7HZz!SdM-f1-*3(*r8Ut(hNW`WKl7ZkdBA z%FOjGw|@oWvJ->a`Z}K2Bu!iiy!1sliEa&T5umBqTSAEb*aWLiM{H3=ri-rrYXoG2 z9M^;b@6n$&s0~=~BbF1l8O7MxkjYV{aMy0fCp!to7 zSOcH~iV|lurS<39%Da~Gw=Z|(oxEOK36G-k0VPYupIlc%D2%VL{G#npQoBfB&CKy9 zfJOM2&+VKPX7`UIaSOL=2W;s71QLl7(-HHg!GPm;`aaZ$JqFA$i_cHC|2NS~u|1q< z)8W0x`NW-=ua96jpbaiw!VrEq^1W#O?O}h>s_$D7q5$K|f1d04>@bb>*@m{?I(ssT zGGRu}buqK4#3d!~IzWWX9Y>k{*M5wZUmltMX}@1L5r zOhsNkDqphiC0=YbP57hwqDnt}BNk~^I(8*>ZYbr>)EaH-bhgH1WV+0s1~eKIN&AKoPgepu4lwKtd%_408ikn z{(~)oK)3ceAwl1KE|{TqG775B+8n7|@^D6iTu;14{gEGKe5P^jbFBTI(Awa=p?s8@ z%#;M#o(wJxNm>vpMgCF;r(-qZ@qZk9keZ7=ya*WD+1=6Y7lmA#(~h2fyFx{Na$)Fi zvBe|1IQko_!17cccgiHIAq=G1Wm*ca9<3L6;vBH)z&xeD&H$wHzDw=C9M!WL$P%-& zKL{V}#vInj-=cs2fS~AdQ~uUd2%r|r{hJ;Im_(m(-7OeHMElL1QDq}Tci}9XTE=d-1&6?r;}=$C$@f6T zWhS#l;kEnwxMKz{lhDfr!^Uu*rI5|61d9}v#W_%k@94(CQFA&a9Dty(ZEAH>fw{yq zUC8~#-+H$)1ZX=p!lf(WuuH;ly2p@G;*JpVSt9nM&}SFggHI`Yr{z}1CZQO zcF&V54#Dh!iDc3^l`>AvB};ljjnk8VQX~zkv<8Wh@ejr9+muCuOiWK%>3k)~#p4Sn zUJ9e$cTy~>*8ZH(*CCD1OOiSX&e^8sGR@*C-3U?7hOJ6zKBNg@Xn^kUt(|t3@2g4d z4m~=GCCo_%9yP8l1|R99^3T!cz&SF&(5pL`@gwYwa$9TiZiD(ZK;#pD+YxDxi^&{K z78r)wZzuZTzrLPz3JUEF9D;2XWF4CzOS<{l7+TI{yb8@uurRXTY+fh>ea=iilZb<| zAB%>@a)Kg3_E#$iZ&cjh_eFoHD%TamS7d`i*eCP53=SX@pQCh;v`R3RTm3uohx7ns znAxy)-paxad%wQyd>5qntB}iJ(wT;lDvzl-rt_f~%Kl-49}6lvx#@&2QcrxL%M2gP zf-#NBdDdxD2)LDWiUYf~)X#HLy%}fSd&vj+P;I)_m=QM`jIHz0Ga{^g=Wvyp^|L~n zjV65#TcY@;t-8#9D{;xDg+}9i%CZIkh3-uY)n0*;+_xZ(fIk1xocD2tP;j3Igbga4 z!fBfyFR6)*$*xiP)lKLP_XXudJM4JZt z^5h=Z+*b-alfc6K;4%+|bp9as!T;5}h5CVE)H2H{?j8F>Eh2{F z{X8*4G-*-+-hbE}p$>H7c)p@^;z&3budf~ayI#->3!-t-bJbwaXfbfEBjEP00Tm6QMc zkQf&8Yd@h=F;O+gK<&WpQMCzJv`bxP8_=E&(x^yZiyIaA#0=?f6$bczEqQP-&?>me z+7RMP-AX%Vg#kz4!-PQjs!Icr!xWDHeH4!XS3C;%RA6M{r8KdWP^Kz?8;GvXh2sf5 zidhYT8G1GX{3twZhiC#efFKES#1a0~QlCkt=-oL6E~}|cN1Av($?}Ghe-;hQg~5o~ z_HKdwz@!RU{;cCFHTi_IT^~&_3Vst+*e~%DduYIP!KV3SM&)nb*;9>~@8s~IkKQt! z<4FPBo`4>$AlEiwYMTEBEGf`i_KH1YoHZ%7%hE>UjH(luN3eBn?FNe_=ZFVg5qNXZ$*1eQ}LPR4K z-Tt!o9^Iq8tNB`IUq9!i)4z6_s>qP#RFQt_#93XwejWVw$L1hpM@pFgw!P=7e`~(% z3IzXT(tdg@tVe6VT=6Axz|RcF;VXVsufY?u?r#Co-WI{;YV=bpK9OeTI7?WJKL_ zTgEq?@2IgGdxv4*Kz{A2o=v6@UKz9PPM$*zFKADikRrvamSm0&!fvlLy4+=hMj3oc zMc4CHT{Pc1i)!4!tESx9=vZ>BvN2yV@=BC;pY~YY1$1Szo63Z5Vl;v(<9cyLwDrQS z`Wo#I`NNYZ2gXiW*Is=UIj30=TD=0rZDWxoPTm(X9Zf1luj+x)U9286B}3?9A?{Ky zEIAd#gtj|Ud*nct7&R>a*pGJl{HM3-qE^e-f2d8JaOUws3p`B$`)Vavom*;2k>y-y zdjYc?VpbLTI+yDS5EKaJa!me=1HnJm-u%H){AH=KJ0l{_j|oeTYxoX|xdx3%N0(gc zMy4XkQ-mc3oGfY>A2a7iPKuFtQtQy(o~7|{fRBCGF}aq-^?k?BIg{_8QIfMfQ&vH~ z-FA8tsh00U|Hwj_3!#ybq6yvU4u*jfe!7WwiQ9Ao{`vasL&?fZ;Htz8 zpgr7W3)$WW6BAnt7XlsQ4toF%?c|FCZc#e_1g3cR9nGL^l^Kx}?`coBL}O%HA%N;U ze=E=vd^C61+Q>AA8!QYipS-AlNNy#JS+1Q*>3>7ntfC{o8_G(M^%7>PkfIp>ELYrn{q0N+O^uvuYfMgh#;cz%N#O1EPHrhwNxRYrh7P`b;yxWQ*eZK8*3-34%t8$VWFs7}HFarG=b;7o{hSMH7$L0P-yH4L)1WWSF|{>}~yR-C6eqX)w3_!(DhfIg8XK9vi#L8yac-aH z;K<4ZBO;HJ7lQ|1=9Gug{QPw%hiw%k?pbvTRUz*}i)iF1KY`Jw^xJ#9J>Af#liL!< zluZa>?}Vl%shn$&1i0s(7RXZLYfQSX?&>5Q{^VSWh+;`T)8Z}0n!um0`*SG%<$f93 zG~n3bRqe=j8NMj!>N(}=h>$3LmI0o-_mP=sTgxU}Alc6GH-x@jQ6eW1me_uVq&3=b zW#CgHSk{8gw{lqFLV~VN=_zGfjeN5EMZia6UIJ9BBIo~7R=)bvmb^#fmo+#|{;-TI zn%&s%h39>{`bry;pqP@n+!#0PI9Kdm4{bw{CP8dDhB=Ske`^YzH2su1`$nxn!jIjf!%@cP^m4+0PLuTwMg06(F)ux*@>v*1a4%82W+yoPr)I%= zGXk{XjVProXYQ=us|DF0*I(}%iI={5lR8WFOVLEFrOTERuAOB=5Opl&ssE!58 z;Fvi?YX#9pk*tmv=J?`*VBhf7_4=Kud)>F4hejS>Egiqz`|K-;Z;r9sGoSP^?68B< z#}wH0;?109?;f$RmW@&^0s$3{W`=vpNyHdi&d%0OFZ(wkMT2~@I5HHJgOKPl2S8GM z@-rmegDa?4d{z7-jrG$DGUu>Dv8R>hEy!^()gPd~8NI7*glB4yIfaZ=$wf6cj=XO3 zc#{8EC3Fz{*Q#NU);$(%o8Yv8f2g6$R%YDrhW{OxtwrXVCa;$OKet1X1IqHEL1$z{ zMS-{&IEkBfh=r7AWxSedVpLI+jk@IQ<@g?tTih<55S6Zo{N>@N{H7#A2r6wvI_8Kp6}_U(gvv3Q zz!D^r{fZ(t{G4Yf7OVZ-G9<@Uk3_2@Uq`GIHGZ)2x23g~qr4~7c(u(kH3>~$b!Yj~ ze_`|C26N41M#i{hW|Z)pAiCGRv14hU8I;x21<{r8a(srg`@iVE18j48_uTef*azTZ z9W-)s|3%H+1IOgBPrW`e;-l55#wY|D=~~uy$k?DN8m3Uys}bodS9QgFydCdXH2f5L z*Q7I&ZYRQg>z@7PiPm+=&zwSImYlF&!_HRdYLfopY)Qw`XA0()jQT)&j7ZxXZ?`@- zZ!5*9q*sKe!xIEu+rWV~qD^IvI!n5r)~v`Y1TU0%llQfT3F{?65tO;&O|Z+r$BbAN z`_VqNiC^@BE}h6pv%fN4qg4g=&P}2j2x){y&P!e|K4AjQD@GPC5gQ0gh}*vHkU*!l z$^EvNob8NkmTa?^!nX_$Z*6xh0Rn^~tqQI}w9+%$)YoD=4xYVLk{xC6U6zM8#5ctE zRtaK4&Fg>!Q4hFHNc`-;-fESo+Vb`8Eel-z{yC72X6J~L5=7{ZRTx>glyt=7!^ z5I>cGgMRena?ON>BX&H@j~t1*#AN@2J??4x(Z&=XO}FBvI=;%`{g7$J5~swMSVQbG zbM;3^B|KjL(@Lqb5IR3SL$V|n4KYpjZ;g_}L|v!=k&ZzaQfbubC?slia@By0c78=T zD9YTQQ}!5CAiI#y(hfh5JJbSLu_E>t9;vEsJa}uDtd4X5ltr&i*?^J-s2}90fI;KO z7g-kBsXa>6(pR8*r!C28@#(`n9E1p1pGpj3Ne9hm*t;4jFF zI0fd%eMUcX8ynB=C@P)dD&;w~{j=Ipeu9Qe=WsFClexypNWmvu>~&1cD#v;Z(&y

    2y_mM}$*^b1M?@tvz;p2yZX;?wm-tPMyORBMz#q|ys zClWhy0jBq}x(VNEO^NcYR;LXPgbxcVBqjk!pv_{RRnEH|u%E0N8G$(tW|1m5%Ilo^ z^tR_0$Cb=Tdaer^OiJE1KXN+1)*J#F)pp+&GHKA6I{Uk|W624LW_oWkO^>1qhBKma z9Ii!|&CQkr=vv-96Y}Q`&(7p@EIrw`u8vnJue5k}(lVf1@KHQs zO|H?q4xIRmz}%mT@zv+^GFk+CCzPw4%yU;+qTdm`R)tsTM;>$#d$TH_EBc+>OwH<| zO`g?xC!swEZWJlK-|xCqc60|)h1ULIo8A09ubzxu%5})VZ)z^$#J{tFhfD$YJzdY zqk?G9DSN=eDQ{ZgZxoU#+JeoxoT<|Ne^F=>Vk+!vN^{q=a|OShI7Gju>vBR5l^}># zx2S15px&`^IT{6wlEL(w9&9Zjz<*v* zGVyXDa|b^&m#tA|FqN=%cRi+oJ3D1`k(|3B~8EJkKJJ2 zs2BFlZvl>&zLV+ODMBu)Io#e-kiTCcuE+cm=BALLynj?>rl5@crI8geX86g2+>B%t zGHY!I+rlKAr!75a>b5X+TL|o2wTI1fKR?E8|LcvhciHMZ*vHO9Zb%iz-HCC#-MgJc zSg+nQ4e14^x=MbEMm5^#6)4I9-G>K78f@!oM!ex)ivb(`J3J#NMf1Fh7O+jfkDXc2 zsb5`UgJ9?S{Vro&9-LH=u_K>b*N%0Of_Zj?9 zEwvj#ky(l!)P#W~oC~kQJRB8dldzAm&&kka4Zh~m%4#y}Oj0%rN-hHCTJTtu`A4*t z4YUG+uln`mIA6A4k9@!${;DBm`1DY~rac;R-4xLILw?yX1&oeME8$rf@T@7)a4b^k zPQ|Pcxs=Awz}+vnr!1qEEy}@pKE|)tN8}}ew3su%TqF2Ux6&p$kd=CZp;=h;xQdbj zFGBYPXY=*LVzljHV|f|Jv@TVC+F6ir8QEj6^}H^@+Q~yyqo)v-1` z{R`wC!cruLV-4eSWQd&uyZC)Bu{&;;vDZ7_pSdO-T4zcE!vp4FPpJnhTONkoUR=BU z$A$!{J;6r!jU&XlY#9&o55J4Yw<@s0hhM$vdZa94L+QSPRG=y8-?MO^-Xyj_Q(W z8Ot{vuWy07xuM5N8I$OJ{HSzCm=5UMsVlaRGAm3kK%&Gx{o&vq$q4Lox&aRAgA>-x zS2ZP@-{TbH^nS4mA!pJ!c0^Hh;z`>#hJ53m$}C9R44ohc-b3Hfcga)7EdqTr4Fr$E zah93{W6}!36ecV@n*h6ONa88oqYk_Fzg_^+RHL-O%dc^)6T?J#VA>BrD_SsJ#D`4= zBVU|arL=H?mxA9l+)w6>A;gvu$+wINDlR;Gd@3U;LtADWkCm|==L8cZGu!9liJQSNXSOr;L=^ujP6ZV52${c24gfLMLk?$ zSV`+I2eC8z>qJBWI91G*{_Dw`8zU#@iv{t1zQ&!@-|2#RLNqzThViJzYjJs{Vr3ng z1;65Uz28u+m2@0LK|c!58sbMh&%7QgssqxB4xSMClM>*OwcSIX)Oj^tDy|WAMq5rb zur^Q!qb?k&tWU6|?rEhVA1Ehp=R}$V+Ag_~7!B^j0c6Skr7(I`M_tKA-$91syQ1j6 z``e)>FQ3MvdM2OlJ+-HZ)6gvw($L!BPrR?zqQh+u9vi&`0Kvh z>n+m<(YQ;=o^L+;5kInQfOAt*GPJe7VVm&Pps@()8uUIYalhr;WntmDsB_kejhF3~ z=?<(w41-G%h9jnb#>EWzdhjly$(;ZgJpH8S%W&^+`{@5rWtd4x8-5cu3Q}roR+R_m zd`dNfPDk}R{DQ1)fRwG*F83U7)_nHo-IG7BlSv&1wYY7e+4`@-xFVK~1X!Lvi0_fx z$QhFkoEbm{&{KA%;o;8zVd~4{p=`sp2chgsA#2J~*&f;VN)cJgzSGFQ)7ZC3VbV|t zSrcWSA?simnMP$x8Dls08as_`4Bzc}-uL@HfB4yc?wRYl&h0pk^Sm4)eew7wgX#t- zg7_k>uj${P?X*$vSP0|54*+!QlTtt!$U|%id2SW3N4=s;+P}VTa}&3)(SYsN7*AtJ zv;QI*&;Q47>{t^=pOSYkAs=??Ej423G2dZII7xR?zyigzCa${7l&-8ZU=5cKrK>YEK%`ukT&CRow%aP z8P2$Ql(^lhE+)Ujzf7;Vw>P0QL1Rs$fp@xBq}skqn>fpe$!O#)E>z}GO`J@uZw3L! zG7TG+zSDV+rFfMwk4D6(uZSjEMc&y=W%5-H+7%3Bmwq>RRL=r(nC5M}R`9j$$jc5u zzSTyC-Ul>B^7JI~Q`Yogg(R@|@uv-%m_oj+QFdh-wtIa=uv37ysL@Sz*8Mc(m~{sI z3nq>KuE!|Quf&Lq*1BlPj8fvwzv@J*%g9HW0T25#=EPc^of&go_@7m4C&}_>)yuqj zEzj=9w{h*v7*UIz&3IP}b7FkKp*nBbjx$!08Um3)Y3l8*SfjTu?3PYESS&hzyRMp797c zU9C@W?8Hm@4p6>j0_!yeEKXw zZJ=sOpDx6=iI;ck9HmI+LQTXwAeL$d!S0Z*}e`e z!tJA1r(~ty6NS2SEtz+r8PE^RBg2z2v`1%EU)~S_X&X<2N|(ie@OwO!#8nSuhgNM` zK;VEpWzfpWZ1XkqC-j{$G3pax6*SohaLUvJKO`C(jHS_Z8h&5bAET3;F4zMw4cI6my$pAJtY|{c~M1Y>mP}c}eNQVSRo|1{HaEDSYc}^8PIMXFcS3 z>#Qp#DZ^*dplxt&0CcQ^!YN~DzA1TAoVRD{(mh^FNt|8v} zFihkd!cX^rd*9KQu7$-mok5&APMN&5=h&oK8)3Hl+yG@)`*vZ5F7nz|J-)ZY=y-Yc zU(~q?0G(L)7~5-iF9Q}Y>0b!gzL~uct)^}5^iI5aJdEClG-d@{H@|Az;y(f^#b*SI z8w@tloE8-m$xGKrrfJ*)MK`JoGpp87I@iMPfL!A z)`p_iQv}oj zPm~UluIR99s-O~wEdjhhBqL`E`RT+&p;IJr4!9qCf!`UMaLX@PanlHVT#3;?MS;_Y zh0Q%rw5A~O=l(BV<{HZ~neQ-fi3!W_$yCnbd({#VT%d63h*-YB@04^)OM!}v`;cDp zL?6su3^NcEj0=nVc0Q+KJeY>Bxp8fZg9m@>y&+s?{i1+50O0f_1OSmT`(yAcFvLij+7x2HcdNQYs%`G+iy@nvUed?;V(h zXOnM-1=3YLwTNMuZ=*@>KQ3b!(?6N^#JV*a(`jcrxN2@GG~Nm>_}N6CdikWW>ogOc z8DZq9K_`!?0-#0d)BlQcWsUPKRe}8l&?`X?z}L8w>H5oPdM)j`bpb9p9)8LtKhg*P z@K5we*3}>duuCk;o~tg%tno+b$WQnT*~Y%g?>rxrL)d%$v%Utj`U<#KV{-yX??Ziq zk5e%}zbWx7BrhB-(-s*bii_K_kGgeFj9KoV3atWGZilP~h0oaj7csm{=@>7R*IO5> ze~4xry@t}u<6j;Wi;Mrk2uz71T=hCX^n|QO1WiV%sOX3>gg@5E8KwMiD|t(%{+&@x zHk=9$1#ssjW#WtR6i*DW-~Z%Y#GruvxjjZu&46wU*`w2}Xi`NJ-yy6cWO5fLfWHt) z!aK!B{=7L_W|w82%wyYgI~uaBcB9me{lP4H8T5kmaCj)|{W6 ze{xrlN!)-GAjOeTD_h#Eo#enLm9ZBwRuoT3|CHq8K~x8aPjbB>m`y-2+XiE0;)~wl zsh1PG-0jj^7#qxj3(P)(I>jQp-4B%l4FClI054lb(S&$TL;{|}3*GSWbo>;0S)L@= zFv-d&o>i+IC?E6=wS?=k=c;cu>RV?@+PU}rMj>BA-l8J-UE<+H4ZyibV)Pn$f6$r& zOr2XmM&`ub9uT`2VH7&;ZDBQ`he-qy5zdJ>?NL&a9PqRFYJuc#9$N_zz$S^jNJ*v> ze5-v{QwOC-fQve*#96-^Vb9o+d`+2du&&luMC=caGXvsIyL*maPN>*5`v1Z8-vL+4 z@|1^gtX({8uGQnF)1G#GHxh(dq$jc9k?A`AR3>7HRt^&T|r{y$o^|8>+&??X}$me|d8?P(xH*|MF__8g4WVwdu z8Go$h5_}3Z4a@xx)J0L-nvL*k<4iK(t$5G|tgE)_5qF(@Yg3;nOhdxGdFnEzIhplR zdgkmrMjdvb9d|Am$sKCg=il}v8J>S)A0gxyWN)_KWPoz~dnPa(s?Nw|%-^ty0~JY5 z%f+^C+bIUONxF;V3NKSKQK<#y|z(}e#P@`yuDF&^KV%x4%qOVja;qUsTSVZKPQ};_;9cT8u2NQ*rBx_|Cm1xAg!hza23934@ zRuyf6tX*=@DBx5m|AGbpiZ7cpqbZ{^(b}6L4P_@?fku}T`>2JVHh6VtC#`FpGPKX0 z<}P>Vf+F0YcE*vsK}984YDHTzR4`ppWm#fbjxL`;`n7|8dZ}!Ln*2Xh&AIv4Q1a_0|?YkxM<{!Mq}$FAs{tUa~u;qogjG}s4`3Q*Jdv8PA4Dz>V3f%leOD+Vr0ZJZO^MV*gD7DyW-4Q z4n`5VV*CCG!>|15XX`E+&x&B0nTBooRWm&GJ`_(}1geslwddvXZwGaH(lxNinJHtz z&x1E!91?DtK3ICy)q%-l9{%&x0K2kz5wm@PByvtO$HA3L{_zuVZ~%X! z0WNYeHsv0)M-wB#MPcWPB$^E%+&C`XVFXR;$Eoh;WZyv!LYd!|5BOfqxU0`x^=CBZ z5e5gzRuG$qQj_-_Mc(_$q0FHoJk^8-f%rk*%EmKwX~!p?CmaLfu$>7ViH=Ug(Y#L^ zQR0wZIi}%JkSe`Rg8e~)G`>zzqB3vP z#DY}7-9bp_6gGTzaze&^F@kNXr_CpspdU6nO+1^%M5RXF_j+*Ab^6C@%JTNJHgjsy zz5ZDD?OfwG9k(XyZEv6k9%87NyWb$=t6#7T2e}EKMCdwb1#=xdm;!0#kM{m7*!cmy z_-FT3(GU<20?5tes}pzp#YV(O)Jt}6|68&AjE0M!)K|rlt-(mU#H8dzN1y~P`o)j^ zr&bsCk?J==r~Fn`@{>xCf9W$xs1__S-S5fuAr~Zec|L`cj(M6sIwv?Gh3BziEnTh7 zkZQfup4G&v_MFjAD7}v?Q#3<3d*DBH8zs1qAI#7&+6_>ld2xru_pwIan=A&a;^weh zcQmXWy4^4(7gki(sxk%6qP65(NDsg{7HiP#ghJ+3`c>v?vj`bGU|b}k%1|k$u&CQx z?&eOz`OOX}(kB_+CCRmHG68-F43Fo=R9+zI+-!IL-Kq;!q0KvAuJnTMF^X)AL$?k&c zv;EcI)|kCy%DB>CavD|&=*O%QCmgT^h6KY|N}xiF9qVGph}b*P?qhdTXq?5gI|q(d z53dC$`FlU^Fp73>@PgWv#Iti0LE5=n-rww>9WUHR$woMDT?L!M#SG-C(eV*E|JLzj z=KrLQQtn)NjVDG03GfWyK9>#J6M9XjN34$ZGV;$#*SZ6RuXG?kji5B^ywKJHwl8Ve zE+q;WRk!mPp#7u+1rO*lbdvfLlzOQP%GI{a;VO0K8hGK7Z1%C~p@NtEtALbbP@JtG zGF)Brj<3_vZKz!@-7*MSR*=WzlXs8B^blyN{qFN{r-{zW@OuYa3!wC<&%f6m`WPJx z0C5J#H%SKvlAB6%hdRIBW4tiEFp4Sl@i}5Ww(GU>>ivD5A-)@w>Frd^(4B-1Cy_+$ zvKnYNO!Uubn)?_Hl(UHjSW*0h6cUgn0@zXij1AGXk$0#hR(~2$s+cEtJ&BSS$(iB< z$M*v)JWd@+6e*#1RkThTj06Aohqi`Lq<1*^QRi7=)%#e zK<%yY6Q-87x!gqW%e8WKDrMM?MC4F^aSnCv;+*ggH!P}(%jus78>+xJGS5i^gXe)S zOn{ei>+hpCV&}#+UIx<5Y3FNsiW?X%F`ka9BBe*c;1fy#{A+YauRRwKsEM=ttU)(R z*66qx9be_2c)9d1@GJS!9MSAEvE9qe^kEsY5v9a$L$FKO%j?e!J=;8>_ipUE>Y_qqASN`FT?1}N#8J7X8KSd~$I|LFw+gA6g0Fz! z9ciAOD0X{qP+e)Bq zOawt=mJPK6z9$gAS}jyR)k-1}99^`3f_^$$^*bIbn8p#;*(gmP4Qk09W#1M8J1+!Z z>2o{}T;Spa&KaA5>AiNw(7;GKN{pm}I|npv;`vOP59^NAF8FwaC#ve&jc?yk*wTO% zKc-|vnS^O>a=j1QRl^GZD&i#A`HeAGyw(5^|LDLTiyrSyjuP|JZ2P1fBu6nJ zR%MP60@)V_kUq1H_g8^2(~5P-xyU(%T|2advgqz72ZI9|Si)ZXv1JPafR2>^^?heF zk^drjjQ*cbkvzgt>ga>rZ!dgoiukg2{N}oU>t9EFFm4hP&K82f_Z)-SHl1My8OgWU zCO|;Ezvd=Bx4bFzWTP%?SStmj&y^?iDJHeGI|pyF42lw8lDjLI8+#!ruzh9Zk}^vN zYWKGKV}?NOOriBJ2Pm+(lb_x%EE zQz3wB-B-_2wRyW(gZ(jF#^odZ^$He>-?&iTC>W#@#7k0c_`ZAl{lz8aM&29jGZIc| zhJx)8Dzz*_@BY@&lD+v%6`j;__!xkZvcpy5;ek=HBa(5O=zoK+nNHNp{XWW#;G`e@ z{f8LS_yDe=m6IILclmDT<;C3P=1x!}9Elq7VMB1y%fI3W&E4fbPgb+E2M|}h-hZ1C zfqjfpp=qXzbM<8Xopo88*(r?ZqXpjViNwC;dIJ#y2z1K#6~3d`Q4eCL2*{t|m}&C% zYEMMI=h5MrARd<>mu-wrwnRtL$QAzIkrD<>9ttbVl?~k2@%_qg_WPuLOjnLHO%x~B zmGrryx_lHF1bshi?W>3D1fJXE1;=j~V`8u?KO5*D8;k8~AaT_``H;7eg8F;iKGqb6 zdn|DXeEg0x_Q4Iv#2uF;DDu{?m5JN}DJ(eqi(d}|#>tk$W^@?+@$&_df zuCxOQdLP}@N29#q4S|j_`68Tq@u5-Q*GTPW>eI43VMq!g`6ifX&jbEmX$1QCJ z2fMlsV6Nl1x)>+eFRtdau#7DDuduvZBmyu0t(5*|u$rICArx<8cOusRcaOEby6EqPS)a{r(6;44d5 zEdK;>Zw*4O&08MqvP%~w=Q?L2Sl6e3u+WX1WtDqfl=vmV%{E&s%7eQ*|AWXE%26Sv{a!*)!nLg6~( zTv2yhxaxkJKu~9GO`!zB+EmUWz=KE&CjOp!?w$3|3(xpNdXi#0;CZk6QGv++5y1)p zu)x!B^4GRm)8`y!{vsOV}chO`}|Q* zq>PW;9a2jOY$x%_Zi*t&0l9h`pJVXb|J4Gp9qnBKpot6E__tj>N5>4_aK@1{ z%6$9d=fkhn-k}>~Ua11cnvM`Q(b=kDjb0xP?OCE@@>*t;!`XkE>4ma;-Wcp)Y&b{W zprJndT_$1h^W8TL2|!jw%;Ph>0d3r1cV;ZOdjpT%HQ9o;nbGeiIsYPN8Sq9$Obn$L5T~UIy)BTJl zvBJFjnj1!viN9og&H2bp71+dvW(mSr9I%BiZM38mGX$#=wF510w8#`o5dd zswgY#RefKC>=@R^hj^X%3!{=uC~{JI9_}{<4=;RhGO8s{8O8ci+j^kRTsl3rp=HOf zuz#?S(dU_7|0t!lWVgs~KAf|w@6D1}=Up=m1Ln)pI_JV3)?OXs6#J7p{seuZMP z%ft48Otqpx{*L*+?4R)mr}D{)Aw;rnL?710{R>7lPT}5* zkf5pJ+>00yT^DHlMvv9vHsD7v1H*XyfLDj%d(W6i4FlBqokM;6VAZyZ7)OjB_Q$L~ zthhUk=0tFR`Hp)5#p&tqh!(M}gWd1gz&EgpDv`oJcjCGz-g2d%zQATjLfBejGg~?X zdAIhpZ)_^ZvLXyrs-@dD}_E>1Jms9G8KGAh5jE?if zz<1KO{%*2QS=z7El`TXYLrc+QXD@3q&Ow7q_cLXR4!allXVxJj9?2z;uXDHEeeZ=l zMgTUOJTc<<;<_#8sP?re_0649-~I{t1vZ1p*L);)K#_h@NY_#X-So3%esP(0pkq$=55Sp7mcWX`ssMpRf;OCReW#wUb_ioF z*TNnyu=!-|xo7d@{Yg6YY`)LqJP8S-%{A?*uUnz}zUvjI^h4tSz+u#a{G5x#4746S z`HzLT5-2%j)NXA2fj}R7&-dvulYDY%FOHM7wv&F6?kIQPrHQ~GM(Y4N0$Sm$xss;C zX!I^rstKmh0a?$Pjubks4+y@ynw|NEf|`dcwTAc$tTR=~w;qXnoQ%iXL+P0DW?=;; zz7M1xd4N5aQ=gMq2|qFV&^#z|J|6qA4+Qj{creaAAe+&9v*N{EBwgy68spAIx7~J3 zmXPSAku9YNoF|Af2+ruS%uP0tye)nq796_^SdV0`+3$o($Xh7)hRV*eE=BBSIfcyEPfBwiQi`zXRlrX(Ki0 z_~*5PiwsZy8=6FJdkci^3_x7LD8v$7f6AoXBz_uC`Q4&Jqk}*v^F+!88m!%Y0u)Du z#uYUK*Bv&559d*(e=mKFqm7ewsBee@qA=mJovJA};YAt&kg`zN7CY0}r5sH8D<+`X zbYYfG`Uch@Bx7MY`jv2qK+rK&q>=Y>#jgRH)PK{MQq;Zk0lnmIhmSCPycQcX*I?Xj`m4K^F&7maz!%$I&6_Qza4$sr;kOKkc-Qrt~cA zX_mQ+^>*KDS#^B6E2MF$#id+}*^FmgQ6v@Xh%! zC|+vV`PDa!Fra^OPNqD|s6>=s)cG(;Cah*jBA|#X$4)&~Hr-E+XpXa6c26!skf(!O zyzK#amB-AQ04x_^&ck6cwQTJ~I&Uw?f~X#>O( zjXXX5uk8910!IHc+5rG;GK0Ccq9;~N#0ypKtEIJQuR_Wm>`ME~pbK6;!z=Bio@L6XEbf1IDuDEf1thT3{Z<; zVFoAT{ATMt$FuF{!r$-Sj>Jx3*i>&?q$>vaiKMIYhyN?Y9$xv69dio2wi)^UZG=D# z9nif^Px2ZfXe-jXF%L3atffY@OO4geSV9cs+Zn&QM@hHhhj(&g!A(7jMd{Db< z<03$mx}$K+3`N71_R=G}J;6Dfby(s=5qHw*K$;Vv!HZqQ2bIOukwUbaX`JrbIW84d%$DRF6i7N|N+ zI7k&9E@BpSmFRM%veVM3hX=!nrPOV368`w{=PELc=JO@)>w`IV2r$TfkN{h>TR4}h z+BJLTEUkCJ%;O5}^Jy4;DZH@hWCjY>qC>tuhic9eheahwa&L&IU0#1g&W$N27zGK< zZIC19rnf5JjL3WH5?#RI^LT7=wVRJD0N_`Mk01a$AI{qQk1IEz&Ruiyn4J0XY>>04 zc=v?{jQW%Mk95VgFxm+^5yp>}PWSVuLZj#Qt=FDE!Hi+tF_wUd%3uH;8`FqJol8yb zM_K^t3^VDnYm9kue0bvbc@JuwtZ>^qshfG^kcSOt5jJVWa<3h|t?sQ}5I#t@0shgX z4E7?*-dpaD+2Fp$=InS15LoyFfo{<0oj<(hP$8%k@H_aS%vOuGU6iO$m_V&kkTak& zi14Dky7L40wK`^eIy6`Lhq{bqODd_8>Kk@Qh;E9i?|AR$O7s&f>$CB|gAa*Vt z4H23`!UG{d0(dT5D(Kb0tvLws3WoWbO0??@AQ|8T^WjY^R2=?*i5SC1ys9ZiVeQ&8bsGuq(T5FrR~T95-@Egx{u{4(rmd z8_#>eDW&xh(DP*u)#ut#isQ|gm~=TWuh=ZZb|tZwa3gh}qMFd6|2Gb+)qOX8#RSwO zHhsnMrsjqSzytOn9DkKyYZodm5Nde>uht!$XwIp-^wUDD#g1p^YJJHG=| zgl5^N5|4iazpfH%xImxlC`g<5XPnuq z4#~aDDpHN+@Jb%IZn3OAcLeTKEq1A?XCA5aQx8%meg{;z@r>~*9Lp+#O?cTdny~Zb zJ7?VnnGLQ4DQ|+o0U@0ldB1x%ADli;32U2tit3te9GnZhbIOjFcI@e3G@us&0^{;J zt&?6{Ow&)(0eUA`(KWeh9~cd6+B8$Fe~?L$V6X9O=U?HjlSI#$wlGi4t=jy8(TzCgF)K?)e5RB6jI~7U~qpXA*Ys_c(UvDT$)I zIfHez{LTmdy~IX$LN%Qu6OmY#ZVlK*jb!qz+k+el&v0zjA)H{k&o8ipIOb&bLu;ws zxF0Wjfy)9iJbQdDV7M@Oj2)L^n@$t~Tfu12b#~NBa1GK|(Fky)O=Uvmig*<*T$786 zq=A+rpDuT_6SJ$N4WsJBk=Z zcHl5dD%)*9$Is9Iuhj#A%S*LTR6C^O`MzMZ#nj>x9QPf%Z|QU|4Jo2^VLL00yg$v@ z=_qtMRp?i2>KS}LA_f2H07RJ!McA?0d7yCT|5y*zWg1q49eYnkUk{Q{q4+MwU|7E? zjV~45nJ4bRs9uL*W!%fJ){$tAe&F8Jwmaq&BYm#rz|tlPPar5LPIvaQ%YATpB_$|v zy%}Q3uN3dBw4R*m)oK%BAu=gM3wvm%%{5?_jm zLJhC$id)WzY#@-dH!La#)K^x~82yb z%zxb8$k!xypjsX(lb@p!YBL9_gw3hllec8&lfQ%x2JA7~GCBKahMp|FyvSCyjvgK9 zuM%M9Xn*(6q5&VR@5%`~`3srpe7DsRYF8pi{^UklR-7(E4y$&eLLoI@2rEsH8KkZ3 zp6~e&CbN^IeHhMt!`LxB{7Ff}+8VbuZZYTBBF?RZDH`$y)b59MjQCXbSeq7Ve039aqV|C&*52#c&Q-vd_dnx4b501j zB{QlFY#@ii({$RmVgoqufZ}69K4djJj6ZV#fk5#GtKOLc1*3&v%?^|~{~o!+XZ=zh z@fuzff0Co|l_%tOiP+9g2%{|+=_{?d{}YF%TUlp?e;&_RW}~GLwcr#eLVjz$J$i1d zIoA(1d?RFA-F5(`RYcJeK?%7bKb$eS;49%IXc zLeGqY!1Me+W;}$D=&iq`I{1zPXg@UTLv)~RFfHb%S^8NJ=KA3s$g*BUKOtMIYdSoH z+>F-+UhzNm2?8IOk_LqyC#+T;N0FO=4IXwHyZZ283;62O@%uwO={oRk&Ew1+>*VXg zsxm*uX;^z1E!=pUf-;I&KVpY-{j)@#{j%!_Y?bbWx;RYfg~tZoCOM#z>jyj#{RMYS z>D9CSyD=!VV6WQJiU-qyTq0GxG9Sv7;{s8-~@^NfDa;;PqwwG1BAzOC>K#ZF|c2zrS^v--I;$ba=Jzl@c zxqMnH?3K)VwcfQ&5v$Ir?LM^$qh-sqawp9&4i~>@RIC1c_{#2%9!l`4E??sh74NPncU}q@Z)205QU!|eMGhja2*z*O?Ex_DcV}5JSzsHi@Q(CYY zag1v9BfmVF8oei}$W*DV5ccX~dt0k3IrGS*?&zcJI3p6R6>P|_~!+Muaj8u1jNeJdqcfOIZDBZr_>7=wOjnYd&0giT5e{zX! z!rThF`LLsphE{pz5Bxp0c_5z-)RgXsudlMV(3w(o@|Y=@xms|v)#QE)3?F2OD^{;O ztk>%jp{%dr!^$oN6#)OpiCk2b;W6+D-;NAoy?5&}D4XypQ%u-q>hzu0e$N!61wPAG z3R_Xf=Zb9o^Rx`eeK=!b4dsI%>n{k>TVwkyn!pInj=JA8zR%>W*egN@gdQ8LRUbD5 zRS&4_>f_6n*y@&!P3LVwf$efhX^Vq2!f@I(kvn%>Lk`d_UdVojs@9EX^BYxZN<3?I z$z6xx(XL(WAdt;cI=1qE&vqr?EvIc1w}#)=`_hY4u21eegbFd|hEjS+N%ox-&H?X9 zjYC&FulvBK%Fyh3#Qj_Nw|F^AogbrNHuUD%VjZWBq|y(+A|ZJ43%@KW7-ySrO}$fc zK60wPwbhtBd45;6B+5u}9cD}!?>;`b2U*dBVV0NRpZ;i%s;b0+M~I`mCuT9$@H)N|%r_pw$%nej#G8&Btm`?UR3( zpDnd&-7W_BtJMq@C7y3ZC493&aCRyxy2FB+P0BXEPUp+oX-Pep6tz)9`kWsNI*6`o zwI=7m4FaQGd9Bhs)%-IZ-HbWlklp~R zamKl(&_c)&9k?XDLd?hI7xpNv>pR}fR$OA9ZjW?wEq1oKAW~ye{d-*`2TCaP!z3b3wkuR00*7}QO<%#ytsOezPcdIZ1@>3EIzN9i^ z39f)r5J;iUR79Ti*=9|t!4KH?5Zdw~S*Z59AjkPM{fZsN#AT|o+2Us z-IPD5@`^>p`YS=x<{-mgElKrW-wR)R=2Lu}VLaE&7e?0D1S8~{f z3ke;>Jq3lx7#?Thl8#mj;wt?KS2JlwnF?*FNR%!R1!^PvCq=Wq&ZtOq^goP6zceW? zR1P~EY3;hb(YC}!YyX3l|L%M9h^=R?^#LDaO=7ECcd==8NR!IgcXc_eE3J+gQn%LLCERBv`bwit%yb5{Kj%d{_iSK>CSN-16*t%u!`$$yK3_)><>JpIeIqA0h z9e4EFQsz~30J#Ky7e_!vKe?dU&^mBtzR;RVJ~MvJwZwB8u*LJru$jRh_%FO3P~ zZB}Pc2e=5`dFuKW-d4e5Oj^>4-TQoEwa+cFS_n0!qox}_vww}oq{B|Dy44v%>a*R)b(m4TAWz7zNHfdu zofFD)5o!S&gKyeE_YR^df}6yQx${ox<0u%EknV=;oq3V1aI4_oCl zABcaUqkEavo@b`jfaNrs{ZK-c6mXQws=`bal_~2V@Z#L5aUXDx4{At#xI4!u3Sk_| z{r4+H5GHvVXF;I)mOqm;yT67)7*E3oG4IfmJdby@pQ5mbDX%NE__=B)s%g%QhY09Y zWuGPy`4d<5HCrQZ^JM1#(y!b=3R}iREJ;&VfVXnwNiKd{j~Sl+XQ-uWTe}|Fr1Zi{ zWgJ23=YHA7WKyv{r7G<3hO#+rMSV<;G2WqIm(<@yuHJDyF?Jo$>|Okcqbxe|Ow3sF z_T_q>`1cyp zjWoui>q;Vn=e3h(0w9DRhqp^C$RqH}k)ToMUMuzRDij@%Sec)uSL%^|kUSF7uAjXm zduX}Jwem{kOm&B9K>mq~{(Sc;0a%=r$T&z6>FqT*O;aLLnI|n`eDanJGq07-L+Zrj zt)}v3%VwmLYS=6B#*#=E@;6k%kLU31siN|MTs3Cj3+!(LS9y(9fj#T{*r|MDaRaZ{ zQhtcSEgZW!Z9YZs4UnR8{b%Jtpr(&fmihxiV4W#nAd>(-qU|@z*KMaN>S{;Df%DG6 zYcc38?Yr}i#Jz*Fy0?)k?TB{OsN%I~En~|a{qOVJZXkm$9NEuL@2;ru&^v!bC5Qy{L0cb@u~uPSyEX3iI72`2on8HKa{U** zx$}Ri_Ml?OSMXJKMdYidh~xG;cj6GdQN*-dwm@N6BHX!z?{6YLKvBCWDWM)^ zdyyA#jB)n(yLeOOez9)1#*(dDYwVA=XkAwJ9Uue}L}A$lolnT&^O=Qf$yUc!)!7U# zyBmKx8t{@{-3F?s2kzAa~(@Cx&NkseB7)aZ>p14!z8TT>PzoT>U>UOyQ zm6fDrwoh2u%0iW}Wb_j~ND=O`s08muo9WW{bx^IQ|M0dcSk3#t9bRqRv(ho2YB`)D z{i^>srXo@vTT}OrSW?No46h$H@sz%I5KmeIe44rdgl-*q%!0s0l)zI96?VWzuV8oyi&+Ac@2Gwl@6w7$?53@E2H6{Yu)G zckzvt=I+L4Fs-QCNIzi5SFCkxs9xmPRwXw#d-2TtD|C*?$UEd>TkigidSK5thW6hc zMbGa%lN-$Ox`J-5+Z`H$)2?|~Ef3}mFUKMyZ$rLB%uB=NtOvc@lM1zgSL5nI?t$O^3%5zdfqgz^Iwc1ow!Aap(}LQ+TvuqA!UJz{DQ-&0Wo)~v4nWwzS)O_tVgHL$rFsGz~c6VE=4gAYvu zf-oU?I>}lowY%e=t$CpvTl>*IGgCQ?%FoM;6=ioQMr7WUwQ+n5{|h}2Bm7AWiTeB9b@xArYn zSzboH*3Kzf4jUA5FY$xYDeo9j56uty-@hz7FzP_=#)GoClKV*!Nn5_@=p? zz}P4R4b>b$6C*X~?CaaUEUWCIH3`@^g_wmeTV|sHRoyeV3<3=!NCB1EiW0*|CXoGV znANX8_>fAE=1DhRUKRflt%^hH$i~Ma=1NxqZdKX0DonMT0dRIRwmglmFGCe2dX5d8 z@^T2f0FogA3?8PuToTKA^g~Uf!DLXI8Q)b(vZ9oK!u? z$H8$&{ zuea$Y>viw|vJA{=bSos}s|;j8yj+XRb$pD589+8bgJ-%8WS(>d@VLMSF?hlf?Ndi} zpVyv^l4)wQAiTXs@WR^*(6a=1qIu6x8f`fETKhbXM#~+xqstZ#6ZXRSWe3JYtqG(jj5KPz9+rAn#;|;Av(<+)#d^{)*{I1 zsg~^75Cu2r)PTQf-LxpYQBpRF)iYH#S!fN#@b{1;ghHIfEUDG1N3L^Fh&=E{VVZc> zk3ggY?Y6XRJzCunaD$~)_^bQ8Uu>@5>e{NWjPpEtY2AgufvF-sncBkYKDqsMRY3{g z=c~9q256&eTm@7}nuv$roBZjGMQVgcS~%&No)*AipF}DOf7)caaGVFt2M zh==uF`LV~mQrr|%Tl0Hn{=4WwWNcZGY{X_YEUPD~idUIwYam~ZIeQRYxniog3&aC% zct0-U;jb()VausTc^Ws{=-Mq|>N_0eFMOG${`so!d7^fG9kI9a{H)VYo18y);q&oD zy=dOHf+3ss`7t#W5GZS+GE;0?OdNK4#;RQQz^SJ8N0f*7f~pB69#@p{d$Bs>?p1};Sif8n0{{j9!@5BXxl+@#!^TH__$~-&TfjT1d={WF|q;er;Podc3r zmm%4>+Y6!y;piZ2neT#Z(+Dty^{D0a(zwYFK}D6log=@F%b@L zGl4AP#?LS+zBt3?FKra-M84J#($`Iq;@eA_pG92mVk zF0h~D?479>{T>iX7LsPnd1xQZx9;&)SvxL8S(hWMg~qN%s#k9n#@?HcIl2ztTW98X z!|^H`a(4a{2%tdJD%$AfBbSoDO8GNCK)A?y(>jXviFEe_8yt{rb0s5nPt1z-rKE2nI+dA zH_EMRr)#@@=US=v=g;4t-}5l;eZ9_jp65ExabB;(SC^#blQCDhpqSgvG@grdUT-@3 z7RuOxZ)5D3!Vk>XjF`^htAb_S+ZLwhCxaQW?Iu=IJr+VqGyKA##WJ78nxx%j7sy23 zJwVR|=Qawznz8k!tFP8wphLUcV9bpdPJU|Ycc!||!wzv4FZym6)}j9!a^c89JP8El z=W^Tp*ODM&RanVxS_bg%{ILAz_sWc{XY&y;BAjyTw!l|Kc>3TQxUk_(f(Z{U)V7s%$c2w@UvNyUi&n7x%Q8OZ((>bZdjAU;+>~37p*Rs&0$tRSo_%}WjU#-S`EAliL^ISQ1 zlJ+@Z+#xmKEwdrj7l0!MLG`Rmnt9FLt#iLLW?XdoXzrm`i^_R)!J z!G1FCbNH@Cvrn0-v56stsg5+ul1MK8I`{NO(ny6;i?7q?M5lS`J;&;v%+vXSLz3qG zy6=_=p6u8NRq2D46)~eF=?3g{Ym z-q8)I0~O)RE&an4esp}+3S-`@T{x(q#cgYede0WX=tzFswDk)e?K>9i&rASB~8=wrMZ^D|wN~d?IjO~%{C=j6ykylkS&n{VJ>Ce~ z@5B6~wXw2=-ic~IEEh`*QYa`LB{5|Oe}L0FpjYczIFzxn+(lxT6yafz$~5-~gb7Bv0#^GOtJ-r|Ox7LoSvVbowqH5rTJ^@Y z_S0;$w%$cm%J?+-?4e_{_p!fL=5WeY- zf};|vHePfH4|nsz^U*!0(Cb`G8}CO5VFhG{hP5@G4UfX+zM#!dM(9Piy+?y!Q-V~5 zr;Y1?9<6Pt@=3BdKaJW9{Mq6S&9O>f^|57N-3hg5Z>Q9yrN<%G?|@dkqt#5Na~DKt zAdsa6H{oUzqW521Ss~$A_(py}&F5Yr3F_W}5w52EAo5+g6FQ@sV&(A^PdY`f$pA8p zUj*2dTCR#-d70i1+Cltzp3cNb!T+`wKIlC;1bI(S`Dl!=D8%Lg#b&@ix31#plT4_4Z~@pl|gv7p3gQiX0Wu%fb!^Vq}QtG zGC>@jtO;W7!??%;zBOvmzvOb;WeQjL>>_#mbnyKzizoA$L&h+Y+8aD%K?ZMP7_Ui{ z+c5>4O&*Y_GhCm+W#Q^Vuoeu#bqnaGlTGaIS)qBp#dC3A-CdTYUsd&AHJ{-dHW$BP zbpL@aevDSOv5U{@*qPxx$H@}J4B3o*WG}1nP`;<$gw}Hpy<`Mfmafe5RV38+e)}zh4_G9HurHrI z@H@Wa-^+je-iAm<3=7yZnezKo*CXk*{LtJ&)pGbr98x174i$h15Kv@-{mCP?XQ@s3;j-Cs_Vpl>A z;PWl<&8d7@gi24B)Dyo%&@&=Wi*?TvJrzH@O5^(OAp=ANm+vzkzY&oyc{qH?;=^}H zlWgOjUv>18R>wwUUOO`Xq(*BXh~9dzI1&W&U*L@xn=H86K_~}!@5Ugyo`}1RW<(@9 zKOu$0enQ9FU2bS|>l!54LKnm|Sgb$*dz;d}-Oy|65^Ts$#2x12Vrt%|0x|X9a@Tat zBoQ0cy;g>(Dy-S+8{8AY@BQ2{n!v2rs4e!?aP1^o8V1N}E%!VY9YF85IYD)Wf=C@5 zetTXU-=F)!9Od!#oF^8gX`hKl#ptjmnGSy8JmIY5r^j6L#{A}-q-46E{n&;~4NxzA zi#i*%b5$<&@|TS_$**e@pt-1TvzTWmW1i>Zs5sU;K80pvY~mQOP-(a)mgVXGCF*;fXNu#?#`ea- z*2SAkC+Kx3O7zeCv=D7~Oj0w-os;nydGEILtgzN%y66^ z4--wAVMXMLndjMK$3n=wjBA&w%!kDwka(*o&b*w?x@}UR2uu9v^PDGFD}oxywyx>Q zx99^?{!3%OBm_lH%r`<9v1zz`I)jkMZS{VIKB{;lv@15KOh;ybThgOrbYfM*sjz-n zYuyFd|8Ii*>V}>@a1&OtG&WgxHg{ajRvy2Z?Q1>S)QdB|;0`-KN;6JfKvv^i*fzC> zI}<#060Iskd2(9Mr|DTGf)7Ce>SN(4?i`4HtQgnz>fYqyEsGZor3Kj_t41kF#wNe~ zd_uOGS-JCQ)0#^tZAMY8^j&YAY~PBN&$!i-O{p66-;51?iqxtIFMu1^`6EKVE_^II zp@;jWCwg?mUU&V{*xFn>+Fi7{xa_GQV|w7s{m8fVNXHv*JS)>yhCq-!y~d@m(7rvN z;8@6fYpASB1BCiQ(Njo&i{^#zjlysJY>$1b&7V7oB8V4y{xYkUAM@D=oqEt7mcY}K z9&BT8Ej40q7nuQtjkZ3 zVUU8mJc|1uu%8K?+#a6qfr*eP&-3v-WAHf^bj5x5h{m_Q8r2mNl`Dp+S>IBF^I0u< zecOd(bk^t0s;}WqkU+D%y0Khi(#3u8fkkS=w>C5btqetUa38shvB9}E>_ENdJh5%Qgp`Z7(2n*b>UH1 zlQ&;hsELi1PLACW_gJN3>ND(f651DIc0T>(yzjk zFzTga!^ArLTB2%B#$l|{>n#=ChL4}&V8zYf?{US!O@vRQ6FjTpA|t~(NT49xs&A(9 zIxa3Ry$GHP#OX86#$vTIMUv)+?sl_U)nH8uNX7sup59GznGa!#vXtagOzmCNw>)pM2hm2<-4PCD`mU+CM#G&?1{=>1JoU}zCcZER=LsgOYWo%SQrqylZLFi)7!TS_^K}fy|7r+6 zc~CzIq>1O4l09Bd(T0%CkSsK|%ir!_1*X?K#DJ(VG8B>5!~MC+IQ7*S!b@PfGNdzI z5E*gueIcv;F#+!q346KDcUDUu%#e;#3vR&ii7(AGT+Ld@lbU!zOx2vFQG_l5nmb_s}6DUcnb^s%l8Wi``MBFPVO5LHEOXQx5jGbl+b8| zKjX|79m@n-!+!3vnzXq(Z%428;~n=SZ}Xfb@6`v(SzT}4Fg#?wXhL+jIR6%Nf0A{8 zj$I8!!axO`2QXSIL&n~Hp=UH`d5MF zd4@5|kj0g$hztsdF0*KoB#OLR5kI|^*aoyfJ!295TmgdH@%L@ENCUg;QViBuF5voI zAR+pH1+F&sst3p?Zd;^sOv;UXt2Zew>|I6!JWho;H7K7v!v)ch$Xw}pztA?xcc<5A z++eH}w^noW4WS~zg)(vLGCImwCW?BEc5 zLG4N}h#x|_8(si2(o{5Vhj$>YKW1^6Ky6+__tAz>ZGl+uEV`D%v2PxVicTZ?;ieNS z&1TTvW;gm%XTLmbbDiE^0JT3Mq9|Deh!M%n68kw{MeQ%?tD! z^fhM+5}1|~W_R@jRZI`+ma4ze8X%_Lh^5y+C*RjHrEo;Q);^_hdzp6%pn(*ma|4yMQ%r}L z)tE212U58;?80P9u9L+AY@$r<{J1|9a~rFK+CTuY3f}xZymsNOSxE%49T-bnMWk;X znaXu?^(Q(ssF8f@ny&VUo{=qp@)C%Z*<9H2`L}N6RG-FDUwFZ|wdaI*O{7nNHpEK} zU-PH=J)}Bc#o1L7h`|%0J+@Q2hmJT?#C||b({Eq#KJAY1HBBi5Z^st++`E0TWikuE z{#>h%2GV=+AK*)G%V~7LQ`hJy#;vnMVDM^<1l$m_1O}Hfd6*2rW3$h8*1Lv4B!L zoug3O+5}?iM)Y-dimp3SpR)q7y1l$ZVq8OTSb*yi-0?rGOR1f$>kzd-2Dz6>WZmro zb9KA#im#Nr@on#fJ+8X8fyjoo>)al={Skl~W>Bzp!KgO5f47UW=4%fXS~qtG5wCH& z-NGYjLwp?Z34drsrl6^lAla~KZ7^=C-ROph~(yThpN*@-}G*#6?N zVy)z?BHZe>*^gSOG5|(Tqpt}*@6WKuk=o%ER;y4*@ur=$JqAj`0Dtr{t>ey+T1{XV zjmb?h;93z23ZYwC*{_pNoe1WXL;263{6`E+gRX7WgxqqC!1)4X-NXO-Gw2zlSj>at ziw7Y0UjId_GF*hFw$JWFRhu+*Vu4oCqQ5D$GxAw+r)dc71zARjNZ?;WfhIwJbq<1kJ}agoH(uXv{C@akb<+kcDpd%n5sW70$FHVr)OlGj4rRaJ8>)A z<4gSTZ?v<;Hpnol*>~tgkj>XaD|4RQWzJkH#nG>TIC;eF3Pq9*py%CkAz;2;(tdh> zyqVDg?bcOTlzZzVbJ_ALhqrUv$k1*BYgg=tH)waFy48mNMfT3BNQ%4`-yNKH+8M9+ zUNtw-m$uklMzqmHefjC^3?szmJW4^7itENoJq{@DFw|Z*fEB|n-7SfQT zqIs1Z5-(58dF&KxOQU9JPx_&wJAk|9;$DO83xj`fnF8I^DOW%O9Bg*^58xbo059@Q zC%XH7X`U^+Iw=RS#gkVymCrxz+pp0Zb%ioZj)xZV?5c}`2I@Q(;jc@o*={OgGFyP5 z&g$cjzR?GGB$LFaaTtLXhYX^(95);#9dFI6d<%~uR5TW*+@(^m^QPR zTh~pqrAt+%O?bZDh>PBsufkcbrj9w`?lz0j-LmbhbC@C7v86kD)HL~t@~ z+bgC~xt@xna5NR7qYdm>i|&VSZAsah8H~Zuei{;QGqkY1Lc_kh1>QupSn#Cio1iI#CW91 zx=gj(R8_~Ixam#s!4o&g0Py*@HAz09eX_!Cb=!{Eh8<#4uvfig{Jp3CH#C5P>=aa$xXUy|@YbG7o3|!oTzM1$esr+O zeuO~*h-vm;YsgTJ`E86A!k?I&{j2cigLXOiejP~c!@nnV8K}jNRnww>RfiOibZ3h> zBrY6>WSG9**2=^h-EA*H{WlA^wOuD?yCI4oHj9FmPYqViuLI%KFdB0()2snyJMwu&JmXHCtCbCEY>tA#YP0}!u00aGcj4Rzr z7w_~1{zvy$MUm$j@NhtExc72bPHKmCeyn5z=W;ME{9hbGl&7k8q`u(wUxVebm;gQ9 z&>*8J?L*v0y|(2v3X-I!M9-IaW+wXSCXP(KmO`kM+x zS9>p0>soYVCkeX;21%uOb`YJ%y#*FkV0R`6Wb0&h6w%h*=r6~S_4#hOAIge72cHlg z?QH|%%dU&zOXpPwdQl>Fb;uy}U#r3W$TUN;` z?gYot8)hQfYw#mvWK}=*Yn$GNSA2LFT;R3ZB<>Q!O4>5)EbBER!y-5%zL-$FxayNQ z!|Gl)-#5QrH;ovs_|UX%k?+g^^RUrGwYF=%>zcUJ&NbqHrY_&?s?<697O8|j?OKO9 znge_9LLirtt=ELn%F-LAYN1SfZ&B1{YMI!qkqNF|^i!Aiq@&nbXSOT7sw$f6(c;I~ zc>mK-(;88(EhJ>noU}JJ}&a_P#ecNPKdoDo_{~>+JVW;Kx|HZL)A^hhJ zapHH0^JRK8F24tQ*xku2TpwBxX`z{^!=;U6X)K%0sWUdxHxf$h&~oQhQvbU*SilSw zx>_MX_lk}VR~XQREm3=wp042pqDiYs729iNTUsYkQdO%s_jL+ghFif^;Sjhyk_UgQ z00>KK5V{A6PW9(~F_P>1z~zsg{(D^FH{EPPmcH2jdpfTD?977yy}x?}<13Y0g`7L{ zq&SI&ubTu4^GD-8|1OoG4aLKd3NAsHD> zyq9iaQ_%+m#kWoGzbkEjL-|UUtt8??8)Mjf^D8SnKj-|#oBy^cr~v!J%U9gSl#k3L z9YpGAIt#81{#$9!choZ`z1nQW%U`FX>rTuy;uet>ttzv1YS^9bg) zyc2UpI9VN8!(6>^RdUE+)8LOk)&Bj2x;*wA_Y9{8@>PIsA6en$1{=$f`C2;KI-1G( zd82CR7 z{C~ngOUE9)j`}4kE4$&3Rt;?@Y581cJ3w8(_Nzs8u*!8}ama1JmJv9-%YoG+2IY1` zp;U|_gHEjiO>09Se&^Z7lbPT&kIJG4FU?98b=Wx9zqYpa6OB~BLhj)@mzS4iPwu86 zc0s>4_56=T0XzPc0y%PmnKU~1@#DvJzoPP8=HAo0qqTIT(?%+1-ySrm_w~t4bkmO) zvBi~qxoO=NCoFi=lV&C_lB4~@;NT!FJssT|^W&(g1f{(pu&vR~CGD}Wbgzu8jf<}K zHpz5%=Nc?7yW7{VFYr}<3n%`9D?Jg(Yu}Oxzcb4n#~K+pc0&5Qa6@|C_xK{FLEeP< z1!&}waNqNKaq=WFz?;J4KBOGeUS4c7JT0WRJvN`HFT{saq9-2*Exye(d| z{K5SAOB=72=<~LHMd^a=VyVUL63Emr8Bb1-W%;*wT-XaqBVXOp=*#0A(^Ew)eAb|g z#0TRqr57tZZ1Fg>;0V{hts$sk?T|et$GN_qx`onq$(fon10zQ6L+J6gc&{0+kpb5p zqw+huxs5;F?DZWl2m-#CddEs|sCJ(*{^esoP?lTVAyeJqJuY~jmzQ^$@E`DD{t?Eav>kCV%OT}nx} zN!|In#oC#^@?p=3uc^hFL_`82NSD{Gpz{d>W1*7M3#-dqs@W{XU6s8SlE`|%oE0X( zb|)I+{}c++KmIXFz0%Xymyn(+hdL@&QuRuLjq>QsLax1!^}Fhk^=U3G{=B+^MX&k! z`5C0KC_DY?H%>o=uP7lPRe8ANu=f9U{!lCRk7tFVocBV{D+DaDgyxFq@H2s^-M>Y5 zuiK3peV~k=PnDAje1{Cv=0EYj>cjWA>8Mc;M?9-SfJb1r4pzH^>0gH%45q8Mw?4tk z3@?iSj>V)u6?$2v=)dsr?oF%|_=RQ@N+g_Io(Fu5#if=HI<>t1Uv9uT|2*JYRRfQOGX6p`RSC^>W$6mD@NlCEFWrq8X`16w3#I-E?KPk_6i1rG_6T{zVo9 zMd|QikJ(yqqqc74{pVA3EsyDqd(Bx_ds!AMnnHF$w7bn0tpSl=qE|PSmjf5h)X=$GnHO<3+;`&E`je$3JN|c$ozUS{5wO`wfA;F zNa(+qTeR@OFMU1*d%4p119GnK?c29iMBOzV{h0*umq@m>egwPstQ9EqYye~b7-QX7 z`d<^hxVa~Ndj>33ROm`{DsaW3_?8- zZ^(eaJ`4R1yFz&7_NW`o>*aEEi+q4CB~kD@mHC;O|D7gas3&_F%6~N*rvn^cC7@ur z25P+)|L0s%_axj{dT9~G$jZ-`qdVI~M+U#gn8--{*HNMU5@SIYU_hXR-L4%=pqm~ z$YN*f*~aAl8TAFMCe;b&krD^cdFyt?Q5 zRZh97Jc;mG{Dst`ue&_4`=`UO(j-mX_b z_##JNCpVYn^S5CSMuZW%JhR?e!FBaFb|ACET}xC;R23VV9z+8{U3Bum$6A?3%(tCJ zpjFx>dql)(q;vZ9d@NQZ%e6)l3Es-3UMqwlY@(=&$rhWj0L_eggJV{yV=IrfQ&k*w z`W=U3?J%uQ{={lGt*Fvm5oWC(x)jJTwK(q8rh+aV znjf5xi{#nlG1wk~ie1q`1EXj3lFK_P(Pw+d#s0=UM$-uK$V=7sMSYff{w~wG6S@}v zcv{bR9*t8*zenSXslfxXzIqzcNKapUBxibHrttW1b67$?y)gb9FA>a)7=`Ol_<6El&xR-Cv>o%`Zq zV`CqeGuk|3zQ(JzytFE(s%GV_3!lAu4gjLlz(~4O8k;1m&MYqUxlYeO+E-$+3|vVt z$&%3+DslM?(IpAZ8Wtb(Nl4tBzUh%YvWPKtx5B@%LIXBI5bdG?+wxq7-J}%+cE(*E zkHcx9E&YpIn^QBLyN&a0a|!qIE6pzPL16wLaX2!Sc4-r1Wc)-px7cdMh1e?KN~Y1Pu2U5D~40e&HpZmq=LE`hu-S8f4_#=F=gV(TV8tc<-Oz z8*wW?ZQ^A{q$g;I0)z4y^rHGvf$Lqcr~1*2tKzhl5iPNhzHiVCBleED{T zGPMgx?5MNv22dRw#7d>#2ad>!wQz^oIUO>V_VJbTdTK3_DRcy4D$uIcVdpfw+|0S? zCw?MPCQJcGt7Ns z&Chw>8-M^?cfAUDz+}JAnz6H!ayfZPbydd4xQ;0FhGmETb73|joVuQX&yl{SF3-PX zfc>WVCoxk{l&Dt=GJz2@y@gGVD$QQT!ajr$+(R5cavsU-??eLORR|Cf&-XNl@OtB* zs+!Qwn9;$XFQcER<0G+{wYdhEz7#v5e6&ZiBP&Tmy7psRE=$i6|7cF$&4hNEk%`(w zbPTdXi(UoWY;~IlN_wt2Un@U$PR3ZCw4R=S|pAP>B;tM|HGRk;a4T7g(!{KmNqfdE?~3g9yX= z%z!_ffZ;j!lUFl2k5B*}Af+Ng#>CBHR9ow56ycRKJIidcKI!5n!lNu@v9gp1uiTtF zMn)TKOSp{=M#kL0Runqk8_mKgWX?09ZPDYn4&*^ZKD9D&B`gr4wpM@Yv3T;6Qq2d)h zx&cpSnVF5goz{bzKGHc_qgt=jgP}|A*syAcq@<&dtmAdA_41$*2LR{A-i}2Bf zkhgLv!nvbG;mJc1U!;T``G9nFzp(#Q-m&^y>kWs>y!5s#oajK)7r7XktZ| zX`cJ%vds|^I`7<`T?29WMFdvU`LrAn6sa3rIvaIWWh?WzQf6#}3qT^NKMMCzh2JhVO;OlT-c&;YzFI|&)cb`j`=U7GBM@MgG*KeP$5^F7%WURd8w5G>u z$wmvm@^Ll8pF_tW@-^!XnT3HC1?Ab|HyjNxF+=3DnHU%l=Vap2v8*EYgxSE-AU zXE#KARXHOb{bsp-=G5U!N^B*EJtK_M4jwpB#eMD4^Su3Lm)|@*d@L%@<(S9u=hUR^ z*Qs9pW@lp#e}4EEVJ=}=Exz<7~PZ)$<5|xFie<3 z7%Xs?>^ob ziR;HFNG46HPGVtvB=^!b>w&7@Ur|>a?s-+J2O=DOj!n$jNTf3JwZEY0@-!fcf%>Qu zaip$=gx(~%$ydt{+HU@oz;0$kOHA2k*=8xE63(j9!sg*Iuo)8N^pMk=6%d2{4NRY? zblC({L!2-1lUk6>84%UtP-U&#`LKc z6&F<8Rdg9uh&l)$sVH&W>OQT=fK^P;H8ADp=TwT->1zH|`4FXl=PqFcAAwkCP?1>_ z?d%13iERPEWP2WhPVaM5V!pZqj8>VFLBmBd4{EVz%`MuHTP;(2 zjGh^FmW(6a<7GevsJ05D!dBz6!KYB)FJZebKTY%7Alyilh`RmmN&3H-NIU~$g!_P( zHUc`}4X8c#$3B+`+(8w>J4!v!;erFztfc9sQC=%!3rq{j9CaR5WjVHa2x%JW$$f@Y zhKCN_NVwL9q*&O;LN-kzvI~M$xo66ylqf>K>9-FrChIo{hfZKF%KSx&vw?i5s0uFP z!SQ{JWNT!)XI&5PRM0#GWCY>{;bWuzs3TybT{XQ=ghWPldxFPNZm4|Nn{59qG$%

    b!#%aGcaYLrt{4B^Z}qB}wXmrktD{j(Z2In**eE|0Fe}vWbwm&e%#a%xAR0 zQ=3TQz0X^I(qjGa4{~0$6FC*5Z9X7gA(Di@Ce87`Ye%#nZ!-(TIoqU zsQ<)8iUs#u_>8XJe{prI!SVNvD93EMW38%-eHO`ecsyQXQ%LrJUaACjAS9m@4~+;^*p__aS;y$Mswl`)GyuXAcrsM8nZ`W zh9kBZHF0+`^`Kgsm)M1qCltQ)IzNb`^xk@hIXFsTIqht?*!~KpULm!Ot$~||*)SJP zi@Ppwvj+Ov>xS2=_-y}U4Hl@cs7e55y-LErGA;`Rylf^j4-0@Z!X*RQg;}wmuzpHC zvT@6+FHlRJ!X&Z#4t)?K9x+t<^T6P~XNt^sWQt(}L57Q@VH$N;27@m$dQ`d$m46yR z(>^}R*py@)m1oCbv4f24(RA+c3W-^T(F@=hA0lX4nRBRQ*zIipP4)zejscIli^6nW zrW`c_M^lyUPGFlnwocp*w{VBAzIa;=Qw|&rv`-6}mpvKWJ9ulwnYnKs-+z zoRMOs$YqvO?u28zdbUX!!w6|E5&Z1i6=c5J~GC%bv!_Lns*|lO{~P#%Tg3JG&fZL zXn@VGx;U+&vWf{`Z82{<@iwEpwgNOB2p5&pPdW3*ZS<-*k35EfF^`3kxLr^9dcbVb z?ZB)tu@+-Rxyw4_hx{=df!$GN-sxw`_PoO*lcj<=e2*{j+)6f2X2z4ulFqJq_Z&nq zq7+bEs7%!A`I67{KXj_dA<87DVYgA-rc4Dw?^mj}@Or-aGK!zO+psBsS8VailCKvQ zwgQz45`Xm?D8pZ36x#^6YiW)#0L|I+t%h|5?`_JaGxydElRCGmmBZs^hKQvJ|- zLLjb#03v;nM`FLTWn7<=#*#BpdPo0Jn41&KZu33#O!3nU*Hw62GF0&B@8lTKAkybp z5-5BKRtpQdWc1b$RD3W&Zg@3rbg%+H2 zyX({zGWAb~u?J;BGRHrudex8^(hTk&q^ODKZpuWNr%x1hHsRoNsK#__sMH1G>2N?2 z@@U~-)B|)Mi3UE-7d?9I$hq6pKYn#K(Ir9kS${-vY^kAe%^P^saa4G=zh;Fh+#f!| z*?wwsl3RlUwoeX$XCGLpWf42%A%1y;XPsWG+$Ds3fqVffpH5uxBO@Z<-<_tNMP}tN zE~fX|!P^>`#e$ZqRQF{ZCZm=1m*0d2C?O9$dPS)aF}C_}Z^#Ic&XRA| z;pfVcPY|1ZHYQc_oXVV=AOKEU7<)sBd+r-g5I)x0dB+ z!of+>!I%FMz(|7j{R+-$a$XK|$*kTk*9$JDG2>#YJUUTy{#G)0c0^HKzL7)$JBa;) zvP02|FMKaq0nLTcFnt^-Bh z@Q7f~a}VAyE_Jo$lE9%}x(b%qc%c_*?BbL^lIQSFcFEuX%muMuJau|KnI{aU>dLFHeUrEsBl{IE!uzKBFy zpj1#Xu+J!WxM&3DHZD4|p?v$Im6+46v_b1=CaJx{#e%nC-I`6dIrTnrY*)`+l%1|i z`gxgs?d(s_uzpT0c~hOZHp%$%rAX*bC&4_O)s z^icd=yYlnKWD6uB19|;%afGH`Dl0M*^HcRPn zRK@vHN5-~=%kh1h5mlBgr)NW@Ff`Wxv=B`cg z`EgQNPR<|v?8d*dw33gS_#cP!=<;s8!ak}4f>BM2hpsS*F`fu;zEm8P|AszL#ez*z4`^RbYG7ZO(=dy>TNH;nt2lX>0tJ^B8rLmCCwHB4u^2N|J=29{RhCX`5K$}Vuv zvf%*_u0kj1VZ=G8=$ZB%V_m(tS&O4k(BaI5svA?KXlK+4N)6R|mEr7X3)K6D%gYqd z{LF21QRcBMNf+{PJ@zAE1P2Z>J~}1?n8tKnKKqFoC4ou} za^5x)wlUI-eZ{om@q)W?$&%U!9qtYrg9*b{X)S=It)BoyUHrGRu!9x#dRJT~%ob(@ z7d=Y6wb|H`KEwBR>khC+-z-cK&mBYezj8ZaJ_5#b4x_q{A*tANQ z-w|(eLkO?3P238%Ez>pwPWqBOQwBPr73xcALtPW~FN1nNAz`!_Y?8uUN_mxBG$&TT zGAoREx5*lG-<2Va>76)e|_Rcv1` zlZUV)-BvrraEY3wyKD{zZkascwVeYGqdWMvuWJYQz-og=$yGVFr*U=!f>HmK4E3b% zJ-T#vvjUt4<%u#6D&D3gvjBMon` z^d1vqiH#uQSUuIa;lvqbKd5iABdh)=6Sx)}`N!-=mxa)8`CUIwIsz3TCu{t;!3=3NDaGv< zcH3xEXJ_UKuaGWKsC-z`sR1zI3>LGtaY!Z!l4Ox{2WuT7C}VI8myiI(?;8MV#p0-+ zC=X21j!H=@TjIMQ0FWj?vqW9MOf((;;Eag2NgIeuTfurIN{^u||9v&RZt?F0X;Zg; zF+kNPw&9Mesu0n;g8}*?RK27gK_|T0cV#YIQ@P)OqmahIPTarbt$9OAVh_NbwiK1k ze(!_y&6`9^`@|^5kCjPg8|OA|s~pjvJ2Z!v-d$rfm%AxK6`516c<-AeO(6A7`bQ>s z3}^-IHjFV*&sIN;)>7tB{HCzGN#r+lo-4TF#M`WMU>Xw5*_c!u&bcG6=*R^+MJd`7 z+U;z?b3z5;vo}M1x>G6}7TwrqHMeKaQ!5DjD!#CIsDNSCjd;5Q&I+*xCyMWg)OjTn z%+db(#@sk5s+_FK+g!`@G79eRJUMgMjQU3#(`t@u*iq+8*bM4z^(Z`sMSbWx zbr$$LK`6mpk7t{5PQ_M@QyXW-7QmXUd1wqa%}};+^n&y|;`dFZrT%zgFm%J2`j*Kw zTkz~DKz7fY>)9V~U=CyGFjrow5{V0{@>p2B*kJmE6vtScHvLs1u1wPs8zeyz87O`P)@t~sX2s?lHYt*Gz^q-FZ zP#OGEO}ehMF)fD?#Q2OZer`e?!d%1L#iUBfDW5S1RQwFEcveN!RLs%j2gz?&&HKRltVaFpz70I5$ieAZ_fmW7Hn*uP3Ba98kfigX!h0k=QRUG)?g+O4L5nYo^Pc$qI+-k%!d~9iA0)RP~7EwgQ}Jml#-# z0p|}!51S-$qXvwfVLE|WdK!q|Iindj?(ARP5m~fF5&{HWxQC6)lxg*Rpg_}?t13@_ zHuOCYJ8W?coM5l09#E2g-OK{xrAKDj=!lvUU&y@XMIO6~ zgqsK@_d7@?Na?|8z}wQ4)~`viMtS17#Pus*;E781J8ZhetfdwXjZ^i{9an)GJw+)L z5Gq!{K=t}?9(j%%20#0#knw8b&%(&@+0%NiIeHNXUh{V_E?wqc>p6U`CC_vz6E&! zmw=)p-CTq2=4`c+Yk^0Fn+QXNIpLIZ1tUBT4U(-rENf#WVjBD~Sshcis{07iQ0eq>SJG~BH;(CpEg2#6eiBe* zbRIM)rP7^t@sTxLr}J6_N9j8UQ&!W90Mr%n1+NEPbw_zY@ZP3494cwL0@nXArT*uwZ~yInba>nSTqby(m3$l+(Od! zUn`^hX9MN8LVn$%;C;72ca^67bgfljubcyDo^0+_B`|r)CL-;*YqWxw6g5s3nXtVG1K(YwGHS zNy~ny@W~!*JZix{1$_p5K4Ba@M5(6_C5Ku@rK2)Gsz)_Np)#h{>V29S?#S3%blF9h zQA}IDtCuZvDOKojBh_Re zHetgUH6Q6t7!^gS-L+8#cu^w$f}d#hv*0z@|$nn zh>I?PDVeO5w^o*7DK_@L4~<8;i%+=E9YtYNp#t6uPh&3`9z6m1O;JS~F{*rcabz)Q zL_tu=euQF}jq^k@7hPRVjD<(`)8EjQB8=E;N3~nhD(2*Y0&jCVSBrxL-Y}VYe-hJ< zeE~W=|KrM_i2`l#Ou*5_-swv^qu3hjpB=S9$@7cw%7%xvjmc9l8+;b-rtI%)Nw=m` z*N+H#?5P(|F&SXqs0=()1_p#+Uh!%7#XCM-W;02H7w>i$zCN z9=&s`QBW0a{k%M_Ny`x-pYyAQnE7&shK^|pp|+{I%ihu^uPkNWKN#-sPV9Xy{=~yD zgl&ABoHn1qCBxDe108k5EB;&$gNp`H6@GzR%$63j2r*36GWt9qe^jdra*W^*L8^Uxk_{M%KnqH?DXUkG}*VZS-gDl3(Yk zYv*`hndy@1((1Ct?#oejd#rf2?^pW7t(1bnYv~hxk*}M7C}9*BN{47bY(OM+0XcNX zj8&bkJwpOG>IaxenXVpV=0(&sFyv`~7-;fgi^uKC3`&o~@x5Q11vU3220pA*fH2Z} zc?M@u>mpqeN!E&7YNIY{QvAO2MtV8wRp+k~d4q$O_%DC=Nz$YL-F78um7{n?&(e14 zq0%i?5fgtWdIHmxW6_e&n^(mRbeRiWe|kd1$SWi*Q*dSMv**q79S!{KGliJM4v4`$ zWISaYn9p~dx{mnIx&CWjNsYE2t_S?7!h5B@_|y=u@~-LNBzk15~^ z1)o&TPbf+)j=*}E#FEH@ZC$%08spHKf1c6QRTQ}>fM<Ys5Ye@mr?ijGny7~<)|2QRdf-#smbWz)ZH)g zJk_Tfd~?TO!SFAE(zx)nwGg&x;nfIxk~Swj#)^noS~2nt5uQw8aMU4l;C%2?1Z}~! zpDXkZbaNh-(>7@#zk-QIM|OD0xI?>b;5&Hn0;gKGvC?aP$E+pFhOY}__s@pLnmN5X z=XhdVz2Qys!FUBG!ca-s+v8_{-EZuJhRX}((4NDOpUSp~M_4s!H>~qn+aC$Wrm>&6)*5CfKX`iO zTd%uL+|$e(d_I%rW`&>5qe2=gRMEEF1*Z!GEl(E>v57%?t6Y5Ix#K)&3zxr(JTW=!sk#K&CE<$v}OUC?rUB0Dp`*UK|2K-2k!D3$%LVA-tU=pCXO)+EuNA02%0GIRgP zKA8{M{OGduAUTC5S_-vLexID{xIj6L7!tRaGh?bp`LB#9F~x4&CVbXps|`F*=;Re3 z=B?LvLhgp$jLMCb^b#Ql-JhNy!(IXNIMh|7c&C_c%0K8zl*x~;AP z7bNdyH8e)%ImO=;iDzTH?|H*kp}$F!X7oYp&4!L~Efjy?TOF(FbX|v=_oe2T>NJ{f zOx25fn z0HH&I3P=$`r~-oYju55yjx<3;=}7O1q9iK4DOC~aEg-!`iLgNdL7E^C0|L?!>Fs;m zXP~d8qxCzt ze%^e$Rzdn!v=-nrJ<1tz(l@({nNsQmut(n=3FBza;qK(ZvN5OvE_B98cabrz3*=(F zi6Z#{m!B2xveV`0=3I8vb)Ry(#bPSFB}sx3n{{@8D{x-@dDAy_O$Z!tH|1i|Tr`>2 zxishEQ(Q&=11i{25koruN?KJHw(If+R02{3nlXN^pMKAll)*50Lhrv{-sj0mqA6gx zA;9ygU2Id8gd#N(V?8fGhuGeIAS^LVt}#ZYU2Clzebkg$rE@OG=GE`hyOUNmt~{nD zbHHt%^bUipiPkfDqaZDNl0nZyQ;@d#I9#aMWpuvuO)HfA7*wTSd46K;u2K zPf^lPUe#}-v5g<&zi_awcP>Zt$tgkY&5DD|*8U)7-0ZQLrOhK@VtPFkw^d+qZyD#J zdBpmm+U)!r&+=}mnGkG0Z6~^;3rvlZ$sR&@yTztY?usH&$H zu$$sBrAz25)tX;|IkS=LP5(}N{;@dSlq_lfx!pJ*i!J#fzc>&A z$N0d(v;Mn4R~YO-2KSu&Td;kppfV8A>`X5s;e{Q1Cc%pr9``C>hn0S9b0p zCIGE!2G#YV^i##p9+!t&;gRX)LgdJgUrO7yU-zv!lULt6Y^|n@mGBQp9IA~*tZx-C zAP2^k;7zy>}b^hgnYYW^qr;SjOA8Glm|Jx)fLL=5C;d+>X zw@J&oV07cROgW?bI|MzMe%eZ7Z{Ir={CUjPH;#GE=%s`LRV6Rqs`9{Jz$YE)YvdDc z``!Q!+78DK`2>h(s`z>2uM|Gbuy@5wirh9&QciaydYVtAogQECgKVjGGR9czh<0uf zb8b~Op__^ikNzlU{`-uiRM3x}3sZk%VYrOjMg)VHK_?y55;{tCMm;uEafEkv^0oQNz zP4WPdQ%1c4afg%xp6GQD28vtU926KZ>%BqI$=6|2?B> z{Y}b89OGE%mDPBej!yV$R6F)1JADZ2K!NTH@mB7RE-N6d;I8488amQ`V$SOyJq-lZ zFSz%#LAqZ`#5+JaRBa3@R5@B)kL?q{xXPjQUlJ|KSqg8DqFg?OQEwfL+{X|f?eNlJ zztJ71+U!pZZ&Q|HTfk0`JfSD*7FrnMfh-Gs{3+A%3k3^1kEa6I>z=l)FUVe>Om=peW|CiI&zBv0Ui z=J_|zr;i^~SW{SowczeBF?}(KNa~4v7qy}>p#_u#@(F${D;Kx5*h+XU<9(;%2N*|R z4s8He@pdz6n^Z)YzA1q;Q2Ri6cbC>kLa?H9ZCuQ}OHO5ItG?Cin+cZFvF{$x6%B!=`BmW_V{;`1ze}Faw{hecv zuXLvyPDPQZ&t1qUftU)y^JLA2n@f9y$l=xTSb$vU0pp(5s{3CU$BtaHkgDa02)~eG zw#;bHWdGRxoV@$FO>5FiKrd5D2nTmkt`{}{FLUw6H~sEHCbL%^sGN_cEB~tamVO`$ zDW{%LBy6KorPsAaeVNIUM$9f=xo)`kl0cDA^a~p%4$Uci$DjC!5y8~ZM#a`+RXanV zAe<0bQT_=Ywgeff=cse1DvM47auRIj@K(HZr#Q4d4dkLi^iL z{b7sW11`8qbM3Yp2galu&aVsa4RbK(fDMGYDUT@HQv~}4Qi#XHPl=26PqV*=!(G!q zqQ`Ol7e^hDW@ZfuujS-gjG=;X&!GQ%JOMpLynMitt0q&Fjz_x@VGn!aJq>ri7axKy zzv>ku^fKo0n)OqE;#WP}ri93K?H?mA_7x9fh>pDCla%BN^I%2p3AMrAiQj_C$A?LreLh zu2tp1xJ1}K5-0`!?em!;49g>3B=7^mZc2+)tHf?93PP)dw zJ4%N>+hCQ72ape9B6Af|izf#*k_yEFg$90|T$WVw&(H(Zf-z-w10&1W4njNu_OAks znSkvGeP|bk5!i!IT!%>`wdv>x%%!<^>1K4Q*!MKXpedn&l=^6bCGn(lk}2y-7K|{6 ziGjqy617SdJz1zKJG9R6B|mv7kN>>uQ&zX>cawql3EW=$-1X!v>9oiTDN4}7v}KGd zCdOonzE~d7LES?Rs6BUsWf12}-7Pqfl5U7T!u+_p?k-J|B25tm8$B$Ua~O}$#oVbI zp;bT&)9f)8uvp1v(|ywt6HB+d%C*n$Cn6~f{Gh9#eSuAu8lXCuUIDGSvMG%BQ%($m z+J1e&9&(I*9$>a@MHx-qMwz2|T%lv8uLNN;o94XZ4$IEmrBc)7{FmzO-?7RxmQ(YdBNZ!h>(f$m4Y!Cm10%b`w9^T z=m6xDitEYsjS(eA0x+~I*Y1i!abF{qY3RO&$I*wOM{62fOB*&${a;_Kve~MDk7ei7 z3a@iq|8y-6H|K<{e)obnP6XQ?lt2>%<-*o-6!dwzL3{A02Yucmq^qP0wO5EXx*bO6 znEfMfMz6rDEe#>mfB_Vb;=sP1MN9>n`&T!S_5RE^a~RPvV5sZX2jhb;C&1C?qn_!<5G)$jLNv~9Wmmt1k0UMZqqM2XSQ z^weM9vwQ4atr=9Uyk1g?5s>zXxcHTAMLe?}+ur}Nh{?>^0pp4P>$Z4=NSs>mhXT(& zM~I)4|Ja*J-lVC1*1-RDyjg-U*m*kl4h3Mh45z0d@;5&2iCs|+Q6UiZ^vv*K^Q zQtxh1BaGE2rS{9ECd#JA!gkhA8w8?f8FaV{Zv^r=*2G*?pkaGb5@m&SOqvo8VkzZd zBrs#npnuqOGx6)+BA5YL^!Xk|Udy^loaSJv8MIU*6{vKH18(EKE4DyGoie;AKBXLk zy7tQQCEBc`g`0QDpJE^ljbm75Km>K%Z<=tqwoYJ7XP~h`;Re; zw#5L-hLJzVIeLX6=4CDYh&}3DaQ~CV@8Xlu9QDfd``$OaGp80p6KtCU`RN3z@|3VC zp*;J5W|0~^<^C=J=`?7x5en!==26}9KYwWJ;z9IB4 zZQl9vW(2MajpNumlJL6@cp{H{31aM2BIb~2Tj=pt)}NQ`2>c~uqX~Jh%rCExaiLC#IZieAOGh)bvO=dI+WumZ#_h9R|u+yo;h&Kn7t&YvfEd_Jp%y z;wv(KBjv)q(t+iD#iW1}&X@qm6qOwcALP>j!L_k`)Dz-q|?sE#!_Ks~6U8vCOhL893 z$to0sBcl0v!~=-F(7>zXC`y%VE%l!7FSPq^gpl({9S6U%=yFn&Vv&~UH<$?A>{E~2 z0pg2H#?puLw<-NQ8n!F`{MV5O3rqpul2@4@x3fQg;eRPbX+g(X(b=8!<LFhTDI6ZeJ-F=TfWAk>{I+W|BK$=0wLU~E-kY;{^p5E27YB%w2)ra}p!MOJ z^642MaMxcGK4u8L*u?LjfLNx>q2q##6vvyHw!jBV8w4)g)O%VdEqQU`feTQX#lFFU zapi#1__hkoN!+8=?HeAr9m1lmXAEft8~;Z)@Q=}!iH4oiU8T+eMasr?AnRvg03fd{ zeJJ++fROfVa^0|E^u+td8r$f(EOqU-Tmyq8)AuLSCOE&!1|>aegSJt&8RSgw^K2^O z2U}P_$4#`3v?vq@*E`pU0fDn5j1D zE1{gI`rVrxvMfgCD*B3oElChnooQX1wspvReq)Q*>OY7O)VCI`%+@aIKTD+s5U0jv zia3b69yikU1g-9h5Bu{g>?#%m3dxTkrHpR^(qYA_tPuzsMLB3g zXuu5Fg(*kZ11`yA8tsYM!7ifDmZ$t{i_&lMT!&LIQAR^mlFVaLCaV&J;Z1a#K9j=zEzF@0gwaWl zPHJ?>$IIwaj`|a%pE2NLJ}MCyQP)Z>7C?y<%o$ZNpevwD^>yuP#DhRqz442LzQU=M zw-@~+{HZY>Jz9hrK-0$L$GLVEmb_qXoU?@wBXd==?mdjt^UbH8c^S1BF3YmWUvcA0 z)GcnZW3uL)LZCS5D~bat^5%OfK9X z?%S{dXmp0@6hKl(7rBo_4;g^F-9~l~IO=aoxZ=RLY1ZE-6nM5XCDNU~o1`@e*(>2- zaFPAS>d*|&MA~u8l{NiU<#y4<=yW5;#ja<7M0NMJ4);y~dG9tIGh`gT_ekjD#5z~O z<~lvm(FYq)Acs^N=f>v01{|;iqMJ7$VEtx_g_kPk3kK-E>ivs~a9FA(c3Tc;AlVq+ zQIWFVh54^9#Su^1c$QG#7BfNAumG2b7*oQ1!c~g}t`~rlEPPceB1?On$kuPBLnV& zCtRR5rkpH=xL=J-rWO+V0~8`x+R^H)z?Isc9uv+W9Xej8!{i;_C!-t5N;;2@`1~%L zYJ`~XLZT@G`vpw#YZS+@jrykn41UxXu6S48BHpQIEbS-=>FZtq#GxC7VYs@qk7h(_ zOM2v30)LH00rJPK>m)!h)wgtw^pUv0`;Q;|SFkxeTb&Mh?qE6Ev*jyF+b%31^Rr#Q z8eyoxlEftHto`o6g9#A*gY!gd4U4pyH+rdvcKYEr{Otm7V@LK9Ww&o*lWyM`P+E&t z9}LJJN~<o{m^oi>S^|b3@>Jq8j1db{7 zYPS9zVU-|>K!tp-O1xB{_C2^2tPaYn2uDy&U7C2ktP66*?p2OcjKkeuYO`5Huxv@! zi`FP@ey`FVU2*-hyca$Cx@)pCB^Sa3p7E82LMh}yY}Trd*x|8+KIxcG#uga zEEk=PUEC_lKHn5Ov06D$!^+d0QdoO@f!%BeNhgWhw`byI%KRVmNJ(3Z>{}74g$#Ah zUuI7$@o1;~rXv&7$=dXKWw7se&`BaLoP zS&pGoVBgys(4?HaqwsKFZET2_Bt!NJ;Sdat-_s{mrF7?HEn>8AvuEwtQ{{k6$#oS% zHEveDeil_QxHKaWAHo1r2np7mD|Ki9Mca`>6JdkzicRd7Oq>iUC04`5v|}xNEr%!K zDA+(}BKK6#b?DZkecw*-Ckv8&3%Bm}jI{db$Icklt-4XRsnl)9pSKaH7G0mRaIiY)KRzF zLD}7vb_pO!5FfL*H*S~uY+JK>-|)Jl+SrrdujPfdt4`sNojN8`6S~UsbPSv2#zT(4 znP4_cRhVq|cY->0FU?SvMIqH@%FNv++h&l}DrO%sNO+=O2_8y^aFNf?Ky^FE+cXI||)+YR{i zmTA);!Cvf>Aan)k??c{J3$csA{|FWoT7?#D=s27_r`ZwiQpfnRP*es*}+%qod`TC)Eu7pS8Wm^)F55@lT69Fd>Um9!^yCiGgeel_oV2pl^3DIM(Zd`{n z2x_*)K*VxYW}Xo)Ve4Gnb_x#6)lb*l|7!M8dQKDXQFpnOCkij4$cO8JZ*ab8Xig7} ziJ^skcC?^bVJC+yP$NG1GLcW_9SIky;)ghoKl-&WAFoS>KE+Y zJ1@wxk+}hizFu+Dm(mOB1$kC-*YR&3@N&^`hXXyD($mV@++pGF%lh=tP)>eR+vVM> zlIzmCS;J5qBmJeW#jMh=iA`Tk#%|pC;n5ZJJ`Oz#XLNZ}5S847w@t?rMv3t=H!)La zYx=Si3Ud2?9FFud2i(~?gzSBYYol22R$%?0Lr1RwP z`(pmyiqczk($fBhiv0fZ&5||j{@%^(VR3ocURGp~J$Ka)L13kMkR6MO(g#N;bpDC+ zJ5`;^mp4v325(ec79X3#gLXo%@aaz9p-QL-hf^u!E1-AuMp^$CBL?M+vIM8 zB1W8rNH5?$mW!c``%phcKaNd{`Y%6Ibq$ydR2cTvYM?*Xex6CeY^_b0kmViq^CPb%kE6VzgXX1tV_r-DxbFo>?&` z?eTeqJeoG~E>F~z7p@p_Z2hxUwOG}Dn#jB*)zQFC&;{|ZPg2CekI5sx?ZIOB?hwVA zD>Y1;_X=XBz0ME(ftF^BI}Ez^>33aT{2ZP?G<(qvVuqaEx}Q4ChI{lNv*mOTEbtm%kvTjkr=Dd_W#H^tm8-HXO4hI+!psgzOmeSeu&9>IkQ%3XSJw*&7I*|p+ z#%Eri~y}1wb>&)!H~FjY)1wia8ixsWA?vU z@pX)^z&rCF7Vjn^MTVHMo|#1#0Zx~TCC_5Z9ti#a=gD1B=wsB4orpR0C4mYC(~g!# zoS5P5oD#?f6M_aO)$)D(sO>#qo(b%b`bL{B0=h*yUUo~1HET>>vCx>6@1&vZ~Ez8i;g(aet1+y+52#MYJfq)y#jh7vs!-xZ~dC^y1SG6s-&jpg@6 z@OhiIBzYA27DsUL0Us{gJUV}11rvqkifzf(`Lq}AtWIJ@Z4?*^Hx`%uLB(0iUm1LJ z=keXYIvG3P2tG&2AlvJD=5CQh4jaNe;hT2jXVF1QXZ`O9dZ^c9h8WqBh8I6>5q?^< zz7+g9-3`0twFhAa?ZIBP#>w3Hw-%t%H&E4a7Jr8o<;S+giYh^aaZwW)=%LI zeiU0_!eY=ILz#LjRRd)7-a6$v==8eGyN#7QeY6H0MxlV-8~+hh@N-9x*d1?(8O7c> zv^?POgAHvuEmx=GRtByKBpe`N!x7*wdl5EoGclcksu>8mt*FN|z2aT&7-{UC6OU>j zw)ztpJw!fwSg@6x%E9QO5Pq^7Ol=Z<<*23Kv*>ErJHZHuG8}W)O*up1Q>ZqTt7ok) z!Ic=15YfVO15soH#+%&X)z@F-ZG_u@nH$VNfPbrj zy{JB=tHddNrJAUlmlA@jJ~{^vBLlm$xRncG(bZUum1kONn^DZg3#+V|*C#yS%NZ~^k(A*w@X zKhCgqwqHARRJGv76bY1f^h#8*cdO1`$4(m7gqTgsm-w=b&=M6}#8@#KAob#A451n{ z#d6D1Ss)>R1xD?;(Yt%3`XderuF&{>`ILWX<*hlyf9G@z!eqcRC*d4H!p-|%%{BO~ z*p!sBB883Sj9@*vi`4xREWYVV>H{Ek7m|EnA` zS$g}KxlU%pgE2XZ2EnW!R&H{@xn#jQhmgbMS02|D4SpncuW`1h&0P2TU9J~m(&tL- zCT+kC`x_B*pL}!<7^^c8Ri>vlznaVsY5nXkovrB-4c``cYt=mfzntt>3IH#f%xurF z65bKxCus%Z6~&n;j$rD8>Vk6AZf8c!ygqi-ptf|CjIz{r>OfBa0Ea$FPS}EyR_|N~ z({am6GKa_2H@d`EB`dh`*b}s`sZY}h193c3w(sQI`BzbFug!71h*$VaoaNo|L=>yi1@ zLSwP(ym`gtBizv;ihc^Pnr11yNFZllr{hJV*oNrRdH*BV7#C@8+bXIA#QL6P^X{g} z_#BFyN1z6#YcyC~D$2Qcqp!X3fZCt1vPYN`NKk$wk3;X3<96PJ;TExL*u`hHTABhu z3Hhd{J325ya3{D!J9hAYYtk0=w5W~Pd<&UVYSg7FdaSg8yp;^IY}Z4S%EJ-n{^~Wbp{MsA!Mms87ak)| zA20PfJ=Vq~LYUKJ92=lGv4;-t~+M)-ZxD2xT=zzqtRfjtOxQ+384r4xk}@O8y+ZSpM6w<_5fAHeWuuhm|^B245ex zW#^dJL5A2*7wEdHvS+PTF-$vP|A3z!F0XgBP$q`C?cn436+F#m5NSJY3cCv>h!CE` zdveS|{t9%uP1SXh?V$zDzRio@iW-7YkHOyx;RV8;L#amo_rMou-Agq zPFG*n?=5T;0dRF?C@3IZAubBL_*z?!tFBs2avD>#K*^}G2Yp!A_GYBqg~@CrA_K;0me zcWHS+;g=?yjY3ZFkUBV}64RXOaVguPQT-0T{4?KkHQfG1W#WP?uMz_w*@E%@yjw?W zD=|kW4?HyCt&Bngkm&OYiA#8Y@tqA=xe64lFEpn9qA9p{N!v7i1jS6|kdCQ1@JVm%8|vwE*!4gSdT&N_%uXZ5FqGdDs^YN~@! zEAZ^~FGfg)4o{aXuiMw;iFvb(DFB)TWh!~IhcWIm`L~CA;^x*-%}!@V0qUCqg3y#{ z4aJp6W4OBUN057lq@!&f|ND{)2stY!Jw?vpd;unL2}q9V`_SnNNTw!{o;4z07L4dQ zbTChe?8wYUw_{ft)!}g?eY=Kzj9<(9P$VWqQYb&-w%<*XFxfA78Uio=ExIX9{=C5~ zvZ1=cLS_kJjZ9P!%l>JJsJQRF%#ox3`1g%>*NC1=J(y0@(cL!vI`ROYH~WRPjU9 zC`!KVqb)a8EW9`R)raz`{;Oh!;QglrVr&pE1~md=yAb{=S$N;B$D}S$cVN zPw74e4{chR zZFR#qHLcgexeV){rt#yF9-qevcDhZ{OccyZ?fAUL79B6LaHb^+%B66R%xwWXu}Xso z-M4VzpNJxQTI6<7ISI+7uN&A4MW*emoL+BVoaQ_ekPG#pP_Z0-el*-h-BcErj$M3i zumOUy$W!DS@|54*VCi&)Mxx+ z@W(Dmr!{g!7pHXpJjaTye_)9j#kgW?>dI8~DWj|OVoNgaS-Gqg$9ID>VS7q9K$ir; z@2lQ$zw!h5g5qHB2HhUL*r5F6Fl-Yg%M3Hh`Dx`6KgTQ+5bv+z2lKayzUIRHV@S_^ zM17VwZ?N{6kfxjb>t?=(gDm65`tUtBqoUlnL*$Pm;VqTnbNslS-CXpzXx{%$uh2E$ z_9Qiz2@Q3L5pApVfq`k#t*2*TOC!BE7a3J1v2-E`40_29u%fwH7{y5*CF@aG2s^=;LsRa67R~_?<*D0TQ8&mQ`rPv5x zwER)1=chU(d@u~}zDVI^{$9aMD2#ibsz!KIgjD5GysgedDJP_v+Sm5#$ov;1_xucK zKtQg<>-slz9=A z!$g^fpWo-l_qzEwTf21aWS{RZ7R!W`vy9V!%_spJ z31NYzsDY0k9@=LfKYDI%`TarEjspH5@;a${MCkM46=Ko))dzA7)Q>YIji%xSnLdc= zR-7O4_Fm^f_$|1n9kt=F_$SpecC&bzMZi6+Zi0HgoE|;jgRD>prAdx45HGsqMrGMN z{OdkX4d2#j38N$Lm}v$=Gkv2ZSgBnu_FI0h6mey<8r~!P1&I&~Hg=XGZ->7g=Fs)X zx@4h>acqLyU~GK)EI#%%QJRUR zQT>XU5DTXL@Q6J@Nb_Mh^US-5C}F(+`p|>!oMB~RQT7^kC3X-l54j5I zUS0x1#;-Pb_b;Vu?K|T4wGMa$WE4syM$h1 zmX{C*b7aE(i-=XL_v}%!=zXQz2g?5KBeCN{Gg+V<+Q{cNPHhE~KMfkau_hx7b673E zp!TYj>ND^x((zI!w9RHM5GhNzLp(%H14E$r7j7;d{rbNoPYTlTCo_=)*K!lQud=C4 zaw2e|Li;x)6-;v%=}0-m!xf>i^+b8>OQP5ja;z$q0#)PmI2GNP>ajO^Na#uRIHOh& zC=KR5$q8i~aOSmIoJd@XU(Mh$5sy7!IL6KT}UA_LAw|~}h#5FW7`lXT>)H^Zn|Ht=~ z-kDOLOwqE*A}VxjqY*zvmTJ={nc*BkTOQ(UC~Y~)gR)_ z9bLB(_Iff2T$aP3N0~B4#q5yd7c>htS$6}Enagb1V=$rZO0jo&9d8TwpE&GvXi)O6 zgN_+}LC^R)#>NE_w`ra4{$lq?{LtGj5-tx*+RrCSmHE*&S0_)g#u<$8R8f@PqXu^h zxPS35P6P%{>>(3VfBKLE{-WdFkVEh1g@W)QGn7n}R+IzOf|km;_gZ-N4MtO+Kfx1~ z`p5*Sku%j9b&*BFG?T!yAvso@mbX3x$^yNZMK(jQ*HzRv8$$xpWUOF$ls?EAcL`0( z_dCt{7vvwb--{K=DWCy%9Cg%|OjHCFK`6 z+XQJ)tI$Y#p^`GAO$JS#@9U>HQioUo_@ltsl|GYSGp~aq&QH(=ocr_2K{Q(MQR;k% znx>4Trx|4KssXPLayC<;TarwjQaJO1CWl6hD)On#bFYFaOeMNekKNPT`3JoFUTY7Q z@g_aXt(Hb><=ntlmTb!ep^J*1wRqJ2*EyW)C)$d18>h8jp3b3WYueb&B({`g>$Sak zZ#5nbY5B{`C(YD_MPDCH*ysGDa;BfeV|GNy0bx>MIX`gmvsr4ztQ20_!xf(krLl%# zK>-~8p=-{4wu!=hB81Xj*GQS>=qX_OmJydrXu)LZ@2~v?!L+&sshse*Vxynh#CMtb z@an|yMqf~-P$J&#{z=Cn%4My6scp0ACa&xE6y&TlRAR%;>essKjHdl;P|6T-@f){= zYSrXVow66B*mmV0*(`-`9Tb};9|Hcj_E4(M17#`!N)t;=M#n06^@EEAX9Hgd0t6Up zqFWDTe!sML9aLpl)_%uq8u9I#kMHY2!Lh2R?WxO-#y-C9J8W1d#9plnJG(1wbA4i* z&*9HuoltsJwfxh9lv5^TidMu)f1O1hs?Sig(W-!QbSX(*^kRCyZ<_7YYT}}AoN}zR zC?QMNi&K{M%%9p5l=V-K{#!k9Ys`Q;I{%T}-%t!q?stkmSPIw6mc0?2Qz%W@Mu z$Zjsuw-+|5$7!>8E(!Q<&(#qHCmN`c!BfopA!cj2=ohIT<&nVQ)qM<#6G+?=gdLyW z{iWa*9vkWqB^{P?2_DC?bIKzKMg&vk0c#$$BTII^$E`xOszHfSv){5mTwa-vR$b)R zd@2)kg(cfao$j0Mmi-eyajdO4zPO99!YkQN-3cn=)Y5b8U6 z6;AbEu>tiQ35I1;m!N`UQA(x3tXwG;d#<`cmd5$7n-`CY@37R!_!}K*6K1h9{IAj3 z=s9dh+zPnPLZ-*4UeI6d?Oo=S*`A_-D)8wGlMq?wyx5i$3!z4EIo#7cmt^1^=yLLd z)syXqXYWo7m3@xN<3GDN+NaUi+XXLaiWz?*KM114Foy;?c?IMXQo$(P`8T&KFn= zi@x9@ouB2)^Gn{Lg}M;2MkSVE69b1ihG>)7Et}&*&(b&^xASg3A_8?wOlV@;RsZ(s z+gHHq)uBwO*eSeWZUbe`!F!w(5A& zENEAN>AJez|6fxP9Y&J94&p713z1sbZ8t!e0Xbz-p>G4(GjUzZDYa)}w;$qqbFZ1< z`E!#H(B`Yymg|b~6XV&ptelGrCxdD(E2?TbQ4i>rOkJXfOd&_w6KF21-{h3YpJTH~CaLpz43k6~6$)Z`>9B%WcsERIE$^m?>dC(8sod1*$x3PB1S&I}v zyp|)UlyYCQx5MT=yXfEsE~$LEWw574DN7BNdwmnU|f6}A`# zF%9|6uL25z(yI|BosTuoD6ObY6BUTS@WVe={Lf=H^^9-VN8NNwrA$U1*1hFTI!AIO zua2z|6|1V@i8xzSx{daNT&HQ1V>lUfZ@5qmOr=;`R( z6WbEQH^3Rx@)1ioDB#GuTv}NLVJeWW#+)YGw61RmbY$3{fn3`E9!9Oyr{O}O@P)N^ zN5C*2=%KA-&_dUzdgvRyx#f+LpAKq)lU3o4dpa~6#dZdvPK{FZ4G zbIrLCIBQpF_s`eR`I&zR5Sc8W=h29`l8W7Xdl_V%p%UC+I`3Fqm1@&kYQ+2A^bdL4 zD+q&9I2rS}b0Ut7)+Cb^Let;)ysNy79Bt3B$sBFX)udd^rB`uyx8H>p0CHDV-~hIx zxKzGTg`$tg`E_@Gm6i#8?h;0DMe&o8J&ONV;bGRohUqW8GZ^f^2(jXS@FhX;fyoVU zBZW4FGV41dv-3VWWvx19(*<1lHW3caFNlYhwlEvjSL?|8otL7d(sDHZGVfJ&pGCSS zvl4Cr;I{eVMDCJ&i6nFrhR|6Y1SU6)qc5Yiu%0`%zIw!0r4!!JvUbH%;l|(EOuJj%egb zwKjSO?etlZerk`EkNSmG(JAM4*?q|7EoUbLQFA%oNnmVO>Dq=nr;Dr45D&$!e#sOi zKV}v=!mT0GY+VO{6TyQ!E^Au=RRok(mtAD=y(-cwCuriN;TF8+{8oT+5wQ1)#YDU# zpWKruSmDCTbM^r$em1}YuNsotVp|>~gNy3WYxjJe9w9z~y}-vTO=$|-(ej%y z02U}1HLQfGrJHOIPOz00r?PqsKq}HqtWzvIKpM6ou`8?@8yt`~Tex53Ykvc=CY#J#K@62|ijvV{2@t4!3L)87K zbWfSE!PY&`T+k#w|Hy2(${hI5B{N^($h;{q$Y_9spfdk0Ge^y0)d+ zJDF|Rr&xpx6}Fg}b3@xqcaWmLu5ND6OL-C1zSa{L;^wf~G-ffOxi7C)8W_DCc0$J> zN4!B9yIIUSn6t6|6>&xt_R7TYzGyCI!c__dv9M?T=XAVr3J5^tR^x2WW%%8hie&1^9adN=5Rh#;7C!^(CXM zH2%sjYl0T4X>5p~EeX>55T;VP{tSo|fl891rMut$goBDjk>$deHZKG&akZ0xCG-U7#*7!e!95Uea7Uj&{_m zsveN!nh?jIR^uK<=Pm$(UE-lKC;&NDUEXPOd@D(*RRF)sM=Uzo!nuz0_v7l7Pc|Qt zUy>oX9ruDsNv`u7bO`6VyYv(f;mf-p#O7rMLNAwA9lAFZwnTRd+^c;>#O#XNQ&(uT z<#U-2as9`O{QaRUI~iEZy`L3zz8n|$!x8q^W6O>&+5r4mDWjut*;H&2u@P+!|J#jt zMF@?auu96620M9QU)8@d%uQIM8uxn0hKUgvk!2~bJK?m+rFmsO%Sbs!$pl%4u7f)a zbB5NC5cC>+nJ3evj`6A{+}fx_M7L}#3Rk~qSTSv#Z%`3+#ZUJiBW}l_wEi-X`zv$# zhoiH-<}`EixV@FxcAH-8_1hejUb!}%>+4V}NWy!=UQw&=8*-9J(=*RI&Wo`vodR-h zqH?Wn$40&xCgwk0$ToL)YKXocn?I=l3f3wBR6nK*6)4?hnD^NKr$dU3Tg zPhW7%O42u; zHh_3U!o2>AYW~=1mNR_Q)mfj^{A6+X0zbN%q7lSQF$-EK3B2J^CZD=2$A^U7*Lorc zxnNM?l<^2{$hvGqd71?9g$!Myu`cU*RK5Po0X=;cADo*@S3#>{vV$7Yt4?8+l$V!Oc~zC=33V=d z;0Y{O0V&Hc#7fgp3!W`5eU1`eC#Xm~rA{S)B4;5&5&Yl_z^i zA)j9-WP>a_%>QD<&miy(Wm_g@(~nY@cdZtVUacOjEoZ+-UB1)Vm{BMwQiuD?20;_a zL0X0XKepa7DhjoGAD)02dISWdL52`1r5gm15s{G2F#ze2Ze~OgL?xv|a8#N>q-zjV zMB<3F#8@DW(*3{jJiq5TXT9%Q^MMa89QM8A+Sjgk|92YZZ%GyvNeP`3_aNJs3WI$z zeUM0)MM;2)Tly`sDy@GOOHvR@SwT^ zF&XHhopS#3Oc$0FS*-c*+!@u65bX7WdjqWG{8x?f_RHUH^F~#g%v&^t;Uv$m&p~TCV09^A=<#Q@5( zp94S7`b6EGOj+T9ZXZwsY3`&wA@M=$_iaAe)}`=jrZz3bBlM4uZ9*YfzS#3~~u0KO=n!xReLuovx4H?!=KMUhO} z4Q$?mvbj(evlU!oQVGJX}2v}ncwh9oz5=PEL4DGZt6MsjuqoSV_0k( zXoMb=AF!FYe7U?e0L1i?MfY>d=phG{%98L#7JqENh8@%%?q;xB zRZ}vhA+^Wtsyq*JjD0iiWCDYIAh@g^HgS$+*r+sI@0#ZyEqg%tuK4^Fh$NccB7 zc<4X}XxIBa8{i~u*H?5+CxVmahU_h#bRusKZzdOEpG3S-SFzr~2h#ZhoEB*TS zO}nJu>L3BDvf81jYIy+Q0Lc;2o9kC&V8_)^| z8nM+?i#cYSRCAoBsmwQnUC;F>a56Tf7zZBQ$#iRRAxIz7o^XLD7w24_XQ0+VsZs4g zPB<+3?RW}djoaj-p-ZGm1b%ctSO$2cQQrM10HdzEPn#^4;-Vue;Qko5>uTLaQF6zrpIQX ztk`HG@KmM7nmR0T@(DB-Q$(OM!b6U#l{H;y)gp3NOeZG0JSItqoLw6W7wh3BtWq7? zKa{nunH&-b8q3`Oa$&Qw{HIg^-LJr+l>Yu|Y=xO>&JZVL0j5T?cdXAY;Jz~cJl-5M zu!hTTz*?Ai=yr%kTqoG4(JHwVE-oyB+Uy zJ^ zqC3rO_iBM+?a{mWHxbB`?`QP>_E(15jG9Sn4pIiCoG>(vWY-ZYV^6VR2c@hjFnMb&W%eX>H!d>XgyRp)JjeJVC9b zDcOHZ5A*4p@=1wXf)oQ?(*66gLO2>6H_4~^iitD>)ikvQ2(227JXCuKr9>L#rs$vF zz}DV~V3rl}l@mBo92-t*MU-MmM&bAIM$pf&sEa#xa|;d>@W`@|Q1oE$D{dJ;u)ZOG zqV?Z(m%qOpc}rEM68yC{1X7EAM1Soy(SV+Z>ML)oX_UB?c+*S1)~}t{%VW`Dc_AsJ zbfmUPL^r3qXy+#6Qpq5TyJND|AF#*eBp`zhesBg*0qYJ!hI3%;tZWBEg)0L@ z9UPmotr}Tdb3?p`lLK=U(#Id>id%jjz9RzZ6CFBKc~cQw7U3K4cHs8_Y7@a5=}`)+Q^I#XY|nVt70pxlxx^WAf#BpV@S{9A$f2xk0t#2)}+8~ zO@5JH>m0CeFPnaZe&jyQbOtI);)5Uiiks=;4}yHMn~T-sukHDeT!@rmq3xYPR<{Pl z@SPh}f_-_$R~9!FD!oHwu~#Xdb?fqWyfq45bY7Kvy8M}rOaid*+#N^vxw8(e)8y4- zNdpSfpsvkQnqu`iLH9_;b7vFwxPMDA7_7?@=V}a`h&9jJ=u?$Q@RX+qd zPmVDNQd!2ny5|gk_yR{-V_`ZWN_dy#rZ{@Kqph5wLE?q1ho=sm_^vXeCk+_nsa8?)F^8mb5{QUq7Nuls(3T+LTVw?hydhexI>AL zsU%4mM^Ew?WOc3MjPZT%NPYQaFRF=AjoOZy2Ngy?@`z?4qdwEsjZ2Sps#PM-u+;t) zD7_p&)6>lwTT_h+w{<~;3Q^6U4g;>%Iu{zjR7xYk1k$wSjRnjUqd;#{F0$M7m)-L> z!*}LD_!u6h#<-<@G&g!e1?aT!jJlyefE5}QODX7no)-0M`nK6CYy2o-v_a!?-}Og) zha##cbdsL?(UY7^J-I6lmK9Dfw;<9d=kG4L--Z4#vgW-&UD5Cc4<2Yq8fo7u6 zr*8m~Su6TJb`SFjAI1+@o)#HWK>ymGUUDcQBvu7U zgXBTV3xlNm<&KZwM|7&$avj~Oc4K{&=0yi`sYhaW5Yz#m`O<~*m$nBK8Qf3ZUp73G z7{r@tg_}^YQOTcPxs%BPZk#FGqww`HVGZxzP=)l{Q*4;U0HbS*&j0~9IxSMps<+bmrBeuWh}54EuwEotoa;IJnsJQs=1!&%i<|gV z-S?fA3UkM@BM;TFnZ`L*#e%cA7H;l>u6(cjO}Uj-QN>8$Hp>j?{TDdOMS>~H*B0|{ z#vMcQzZDtqsv3=<)KDMkJt}!w$iD)`8Dvx17`xf&D;h70x}~*CbF#Z*Dt^w6X~FKb zQXXuGN`rCS^!3qhXg5#>JF`}Hi`+(bCAZ)0{Z~Ki5#G(SkN2VnO;GvrC18k3BGRo| zQ3wYm!I{%XW~ZSwPz{6!UANt$pIO1TPFf7r$i1C==%v5I;(04{{7+^M@#r+nS9P%8eD8E)_Qd)@zR6>N#Mh6y2XW`|Lcc+tTE^u{=ZH%kE%&DID|U>C(GvWS*nZu(_|a6o2f{oiR{KNV&fT z#d1!DKm2RIb1Nrd&h7D?%=ja#t7UaN@-3h~TmJGK%M^6!V(|nb9wyB?Typ*cLyK9p z2(xx9r=}vD)lH$TvymWiFZ~o_m z9Qd1#KJsnJ_VKOjTUGTj5%7|;%1zF+*OtRYxe?j49+0EzZ?TLJiI*8$b?PH4XrSSt zi23wdm%}9t!gXlb1z#86-{7b4)B4A~)EZoX+)0BsmyR8 z_UiKFHJCi~n=1ZuLGf>yX8vTObd_JI6(rx*<-hIpVrvAG&m{}xU~w@$u>ED%^^%H& zT2aRK-k&L$dk8(C3sWyT0ITRQ)w}M}KD~LoM%h9YC15NWPdALI`0N(_u90tMV+&Z> z>R(qJLL*{&Rp^^w%>B*sGWlQYwe0A7&DPOoN{$?8AkoQoPryFqyc`ledfrb?GRjwJ z)q6nib(aD?=@f~U#D43#3+|b0645#2L3V;}%y}me!R{*Kdw_S4k9jB^C7zRLW~4(9 zW}VYWG_HC$d9l#-go7+7!KiRux||?Ij!c@XdEXASQ%c`MFG4=QrdY9Nj`r09N2A9- zVmrosK~OBYs;l*88Yrvz3R&-poxYr9ddc@&ic|;CafUrwCGMcI@5+9~tf!>Wm7hz2 z=|_;?+sAVkZ9;v)m;Nylb&cs_1pS`|^8YC5QHQ{{7zep{=Ft5{|4k9Ans?WOPg+%! zg}3TKW+3y-j%hVx2_x>Ur(~XdtuJ(Vgly64?R)bgCadROzjAv|@KF=)r${y*O9v zWiX3-Yr=nW4;Pu0kl!b&M26GOf_og)VGLGzBTB-UxjZmn5O_@}z}$rz`=R`u%Bw)u zYBQoRUXa#5lJ=5*;b#SU(E+)=rx}31DwiQ@&Ek&OUGE;HT`S0gNp)IpPH{6 zkqdlAr4y&zed*5x8~%KHf^W*~I^jypN`rXm;QE#%?hK9v$0^fLuI?4#-aZEnRAoCc@IB*!5KsD_z}~> z{~=JFUEDt)cnH!0=doDVW$}RVTwJ@EIQb!pTus)at=6RRfM@3zH)<*fi{j^bC_g|A z|17#Pd_nuENGNYz_58tHTTfHS!SEO z*c+yQ6D&kPwh_Pb_N^U$F?r5y zEZR~Xx*iKs^2X@n8%xe3B7;^bDG zzB&Y(I|~fS*N7_Ibwt^HT;Jk2?3)a~H){SklDk!uR`AYWtRcu*{R^i|shLl~`gP4{ zC@2$hG|kELuOg^N~ra9+=ql2zwAP8&2a0gvTxDjB4_n%ZWvPJDDBU< zO>9qKx&fB?V&<*?m7rZOblb=FJ;7@IlSu>g6uE>Lo~PKe(wBD9KGciGL$Be$PQcq; z@?-p~WgmM=+3oPHTw|mCi;4mmx1!i*+%)X|lAEwx?mu!`)Dm2PR~G{XS#~_Si~WZ^bg?x<2>J z5^QNoc*K@R~^>j0(FEx187rJ5Gj-Qwu~Yq45IwJt*&&0!XC5xKc8OJ{_`CMYJ9+GxP+49Tp3fp;M03a?6hmrpC6&+-$uf# z<^zs3cFiIeXu{aHjxAPpTDu{o^osMepK%-K1)=oKU){h4m@5>US$}u`j(EJ|Q|Rcf z>}OgJR!gGN6$L^+;=5TCU`+6hw|#1cxU;jN_K)*}?Ir0JCby|&*n(1K;cH7L83(y7 ziH#d;R6y_azi2dkLXA9Gbmi)icSqT{(#-n4uCUEM{SUW* z9_)!EOa(?jCA7HG>!?YxBAeXes7l-LM^z_HS+M4mAc$2-91Wb~ALQctizBitE1&-e* zAi>db)Hl3t(PN$@D@_xWAKQl`z3bEtkw!QVzsVxoYL6zuWz17`SZqHGUr}!6N%_3A zIjuosM?Ve?qf@k2c+*PYPxUIervg~PeSP&QswTwv1r;TVEP2Pwmva3vkHLcPeG9I0 zDq-IY$ozN{vK6I*tVpoO3*Yw#@syZ5l9vvDz(mve_tDA!sNUR?e1FJ71=JmSI$`Lm zBjS?8$Z&VIt_t#5&KmMEr2H*3cCIlGekJPVy(>AgNKM{^ZT6Kc{J6zxDGSDM#gU>$ z!S>g4b0;CzC+%MrWu)JGwfQoojryo~uRXrrdFP4U(JQ&x?&CRby9a5t6oX=S9+{dP za%7s%L+;j{zquH3q+p;!i}(J4JNDtrMg8g@2^~9}Rt>;yZsN_(n?$XNBq@}+ZWKgg z^q9zI)>33Q$$Mz=7lnec?YDiNdNPfl8w+dp|Kecw7DMcDUc3n*F}X%<^kK5TIUv2@ zy#k)84smlHljXN7$!DHrrNiSOst`xy0ZffvjnQs=yDtC|%|3rLsUy1+WW1ERwP5~F z)6BeL1Q85Mc-8em=E6o5ehdz22&X6hl6dy_DBn|iAPMP55PTZiX z9^39%zQ@(>Bq5`15y|u$xc*RkAFKeG3EEi{Ks?Y zqBfWF38z5HJHqxqHOG|I5PC4VT`hv6Be}OlC{PW-mn28p7n=C%U{-~GLU)_!9tbYF zMr6DSED?3Qju)if@#KgUY1WHF+*qb#cs8!F)<)KV)^EaVslV%;Fl&HfienTpQG`BN zz{y2l`x2}s67mk7e0JDhzX04cmYnmQ`}|Lz&h#_>-omLoL;Mu%bgGwb(wbC70@I9V zb%1h={SQz2Y=dwVIkmWu{|wv*kkM=EPnkvZT%y(RXV>wDg5Lg#Qa$Q13SgkwPwInRp6wR z{UE0UBMd+5AByAPIC&iHidn`GcviRh>uuZokW@%6eYwy-{kZ&~4*3Gc-5;=NCWY}) z_;$L%+?*bLI2hVa?Qkc{5~1OJ$s7EB`&_c^Q$`Y>oEMuJ_vnX^psKL$%{V*(9_67z3(TeD^9@q)mPYRDN8L z!~gArIa12ZB|WS{Rxwqe`iT=1r4R#h- z!jFKN=M7296>=}RxN1A_0z41m_0a2L5*LM)xcZJg^G&NPXB~2jb;Xu4%Mf1lb|*0+ zMg%b&Y=mo(lLiAXsz;%-SYj^oY3#idB{S@3$#QijB>JMTE@NxbKxiL~6@(F#ely#; z2D3a>@uPwWX$JCLl_WFy#N~MNCe2~~E#}$$5LM3x3r<#WWVqy_qA4^El82BI^4!v7 zv#P(Lqg@WwxH!fzd4rb-7r!jXyM~jW6?iCq-~c?28Eh&sjl++x;)mfrPn8%9lAITV`;B9sc4~_4Mt#>5>i8o<(2!hSk3jsm zXll;@1${3FKB|`P1teHOL1TyXU>i(F%XiEWrex-!u?f9C<1}0BD^#-&TH^f=e2a#6 z62VR&W;>TMN|-!o1Y9S+5u2NICu+`Y!iJNiwuFoB6h@7xms-kJ!Emx9`&m8l1#`+R z3SU3$Cxxa#qelz(v92-Gj zS6>O&27s5t{~$B;5M2~%0C|zr+pGe&YDICvF)rU2T(3GN4WQlOQ_Jm#<(ee0o^nOZ z^{;}~xWUtKl|z;LYsQ*gV}wz+njiW)hZYjR>wP0sk`2i8IfwQ0!ym|}C&COf1ZU#b z79U`@%Uc ziBZ2~eeN87NQ=(+x?3Ya=R@1;sD4Vv=eU3Yl}#8o=rm`%WcHG3#j}5N=K|62u{ZnT zQD(iflNil0Eu%>~HH-Ua`3a2)R-^Nv9@kabmAJ=S*vly(gda3e)E?tlb)kgya}^i1 zFPING;%5kN;4e23kI+*Zu?OTAFrQ_rCECu1$K@B?PY!O|kLoJltHT)ncEJWxSbj5( zjza6sT#<~~-M@7T=WAT!y!@!2Q9o&bo)zB)t%t+U3K!)_!g=64TJF8#wme|e)SCV! zy2!PKoIT}j2i-b@gW*(WADr#u8}jhyO*-)&{+sP;aKB*ohcwUYkD95~bZj1cT(7Xd z$NdoM&=DmnT-_S0T|c=u_`pJx(e#nk&-7196xaSQ8%DDGDqqe|hOg`&(yfd2Jknu# z=Jty(p_s^jYxp-4W}&{b$I$_)Z(6N8krnmIq>7IHB%}T~2++PaVapL-YicVz%RDhk zh@dj#c>m4o6cvgZ0IFqXy+hpMDu1f>1*ZM_5@h;=vA24^_Tsm*1DTJb@Zt1Bc{Fx3 zdwg|Vo6L%Bx)h#%hjNk&{oNg( z5s51~t_71EUa%JK6kuL+P5?>G+%PbGDYkN+LW4Dqt5R$ij7=Kg;J~a zLn>-z96^7P{#P3!%1KJxxP(DS(#*OA$FIK5u)^P;S6%KH53e{5SOA|a07T?ppC!)U z3E)iFI=&({;V2r^**r)7awNhjxD9K1)=0j4alD0Q;X+qlTW(xf*~!*-UFB&KVGoTT8exItl&F!UNzGHZ*C2${!Qd4c_oN0ItK(V=m!TH3E z_{%-U^j{}A#!+DKAVl2F`RyJ+R|8VhU>!YU&;6YTHKqWi4q8WINAct5LFdll{NDVi zQnKDeI;j2RwfRpAPAs&j*FDO-%GWPgdpgaF$bCgWGIvRYH+u(y<6rT45BkN<0ThyR zBBf-b3W8H6na%1ooZ+m?3v>`w1ASC4Po0XYH&7IqAwgkbD->LqDzJNgf6(}Vc;NK*t&qdjr@J-k|=+ry+2%xg%HJx_eaI6C*GGqQ< z8)+`KyKTU0dW_xeyO=P0v?T0wCaeCg(q*JWx1H_aFD0D(N#9_^ex%rZpqeo-<&|V&VCZ&a*aC}qR@f-_$u@2}`znV!TG2LH&V~8TC$v?pO?V-Jl8RBJYBT86#mY){92rydxzzl>L63-=T{) zmKH1D|8{d3X89H05wNC2;wGKJonng4`iRu;yCkHLlLjJDkiRg6hLZ7e(X1D2Ev57k z4da38-aa5ZXSnyCS0VmOcTG7Rh58O|ub&-A&p<&!Z6Vid)p~wVl10<*rCwI=MLHWKg-Fi@M!e6jqldN%*+-_zK$SR@lKpbZrU9NtuLxt6a5SWI zA0HLxYp(G2!_uD?_T^K3Bjg1}cv6R@%1M0c<0XmXgj~xojb5Z9sC!qEesH3|*#2Mo z_uje#g|QP?igZ>GJvjU<_mq%>k8~=2v2iBWFSoA}I@%6EH9h<#Q~^3fa}FlB2(*g?Wn>Mr-X_Z6zowTS8i)+t0#EKop(e?`x@1#R92q4<(x$$Qjunifx< zqX*lrf9Fo3nH}3Y0hR&MP^3BCORnwsZ})vf1~O4AFqG%-%n2}xWxvo`JVSiL`~i9a zFld-eR(O<;BdW_h^e5^8)wH3ZU0i6Y0&6Kv@3l;i#KevEL|M#qovI zB_?_5b7DRugZKGI>d1XOg?qc+JD`P8!sN*kH+d7!M(6|0JR+fqYrb_XB+eDquk6~j z$SUFTA&7%<1tDB{`k1y!}I7KvVr<*bwp9%k$468OdzHXXI z@n&nv-g7SY<#tWahO@}a3dMeIJfkM2D86e>iHFwL@5R?mWqhCpQ2()ALcSi5TBUaV z2LSy+=TfX1KXE`^9n*F73Ppv50sVnnYA)^_R&4jn|7ZzxiACH2ZNKYy^IYw{y94b#f>5 zJ`*No=PlqNlYVK}Cq^C4iOf}J#2jNf2y~dOR2s9mF!y>r5XJ|)Ea2t!P`r3Pp~LpopLclKUOZDyH z!bOkRX}kROulX_x9%_HKgt;e13$^6b*9<&5S@B|qwmWrhfxRCRuMY?&qw|t2_5yz= z@ykUR({%~1lwCLDrG@M+)jWT_T~XJcZ)ON-vP0&(8JBEy1CNtLNGHJ*)H314hidbV zSn@jq#l*H%A|W;uqJxQIH4>EknQcs}7-vUHz3T&Ozl(y0(DumGmriX*Y5o5`6n;)O z-9`G$jPMxEl;3n%$IR*_hj7J{VzvA@Ev= zEcUdV95! zQdCcWPVlDbFr%QcaLUjnd2{LbWCej`qy)TFH{@X3p#OEN$~;65R+8d{ep*X#$5+5@ zA@oTRj=ab{#-OKm?$)KW+13rT;Lq#6h&r{W^=0ROVgn@Ir-`)uw`!bje$@uJgH}Q_ zA-2umj6V(DN9Dw?HIG{7oBXPVpD%hh8e_7{oQ67rAh9hQ-N6%_^%o|pDnMX0I;xyF zds9=~`Z>?hb1XS!PNjHhC5?@ICT~B>`nksUy!D-gaUF+~%s2RGR@A&dB$RsCxOolU zl36rl)t@rLOoRh2vaDJ_#?m+-HfqFtQi~Fo;ij&m^4Csn#vI1~EHx5~F;Dr%_t#{b zycz2HZ69C8{_?28VR4^dfREbTVct|d+%{pw^f*;%qXYkCHn zA^d=;%YW+*@p;T2X>A8#YnJKn_s@M9o8xp0#F*qVb~~Jsu}$*%31JcpXPT(NN!ve! zOG!-JL08v1rg=SyW1?Werjv3|UL$dOuZ$<;Kfm{-NKpfv8qV((BJalXvj}1g*_Ats zI{>rht@xWkzY!V4kMoYUF||L>NVd@svvh-jR9d_MK?Vh&;)_-SzA9b;V+^927LgXY7z~>n)k%qM`|NGU1+n z)LBiG$2k?PYvKK`|6(TFzdFPY$zqn(fshPICSP`R@bYw<|GI%7Kk7?ti%j%kDjB^$ zqTb*F+oYZ@Y3UVN{17A4bk`bLBjcWVhMWiYF*w1N(kL$DZCrykf1lj8tz4n%2$`hm za?ljBoO5g-ME~|rJO1Mg*3@{T0c0B~?V!J+uS$CC2i4=%j>2A8&U4+i>#p4t2RSw6IB(A_P2wh5wJb%XOS1EXXE( zU-3DC!ymVOo{4ncLfy3Q;%6wGi)%<-hD0NrhB1-Y#3vq_FEFOn5^U+b%^A*m?ZKO} zy%wRFYYRUgQB?Z7)sS{{Jg5a-o2E)R(&9PbI$ z%FO9}QyYf_1w7-g>`;}Pc1gufgXD$6H@UJ^M!16ypn30Gn?G?f1W|X#8g)6W(kig3 zRsiQQD>BxpOW|`?SNi3JH7!DfQhb9BLHTKLN>A&;tO%YF|LayOqV%bo?WiWEuj6F^)K9e;6b^RV#_C|;``GcaZSK5`**b!>G?yMB3Q!o|y= zj6!NT8#W<)e#a8K5keSE8gM%e9|qML1n^+a<-$4dgfN-DGo}-=^Qn*nm>q-xRLE@# z8@X#NoeLYKXCYp$7Bxr&^|TRW1NN3`C;@F)dl4J3bmYD+cSvaj!ADqwBuUW&XcL{J zRNyx=Prg%s(o8rnJ7BVVpNClMgAK6PZP5d0gXm^0DWf{mTwhIKgRQ=RAz|_0_u06< zZHtYAE>k~(NQp4+mnQW==pTC(JnT_Yx}}3<#=7*wK85OYPnBHrqIC8f0v?OHqzm`D zcMbK2qr6cvKPjHB%bxZpO;>8B0FY5VA?ip*Aqq08KE|Q{l~HViq9R|AidduS$IUwt zw0pvJ(Z^lIUE99I6DGhpr5&Q+eH6!nduAIM`Yfw3W0>#(FP+TZiK5@+`Pm%0DC(1zY&GufK3?N%-#?GYp)76`>wKk+D{^+Gsw z8qXMG%NhI!azQEG;=B9t?dHemb%z0$7xxIKv_~gm!vfaCahI?yX0MO;@AoDH7NVMv zd@9s&-+0`EXO@j(M1DtjgKx&i#c1NLJG1{>0HfJO2I=-oRA$uSqNwZsXl5TQtwyPej*q5`SC1=24C8N+Hs9;IgzBDD z{y!ld`SaN?}Cmx`I$zecdx2S^=1DK*o$k%AJo~Pczd+1a6#%T~r-jbpQY3 zphjSA+w~hwr9A237l7gYaHZUTve1m*-dSm#hcJ2El0K$->?QFttZI`9D2jMY%Tk!2 zQly?yVl?c+*0Em|X_@b?hC6LF7~EewcB_hRnp%OeE0x&7g|K7DrU#MFz-WTe&h$uF zMa!nsb}w73*y|7{pbS#uaXU{M1!Ex`H;N;V&*qoQJ?%s-UB|D&R)sOkHNbx?E`e0h z|2&f}H=`X_dWEz5Z}`RqLag>EF!bpMpuJ%1_*X<;#cj>NDpq-YA32bz)&QV2B)p{U z1^R>BKh`onS3kqNWo=wzcK!-5wWE95tuE!Q2Gb$8IS3WW?AtulP1%(jsdb6Qz=eO( zd&ZrvJ-GAl(i?itju(Fj^QN8-tk%VDgo9#2ZlZR*&d<*Y%{{LBEsO8T(l*)FY>G(d z$OjpDmEsuxW5E-&v!$eQFWRmg?h$#h|5)4~Ws;=ylJ^#2Rl-HJ0f+<4Zn(t`VipV~ zn$9=V?GgQ0oBx9Hz{U|xwK(ayXBq0`Mre?R&i+Hf#F4v+-E;DR$_UChCarmKBb{f| zV(bS=m$4hm`cWZ78p8)PJfc;&A)(HNPdARsZ-ohBe>_=GIRPtW zPg#@+#T2LZz^j&NyK_yxS3Py_^tG2T|M-x2evn^uiqe#GcJO0kGc;RoM*g<8hZ#{hm1LAtmr$4t67#xO)%q;>WOP9PO{X z)3%Etl=~ojnD&ID_LR-#|F0L|#Fd6v2OI7>_!gaPUh|9l8Ka7BXK@bC`7T2dH;SKa zR9?hx3J8U)e$dUgK4Y)n|f!~I$`Z5*As_+C$$VdUjkmFQR72Dk> z+ca-&&WJLq-K=JYW|;egV>co|xHlUc10WOWXXI;--3WL!1vyszw&TfoXJ3RrWnQZ2 z2vmfjW}6f-;!dpnoCioiQlp+HMLS}Wt~E1dTQz`Xaok%{ILXop((cc@QI!LXXzO3*FIS3gNbl_ zu&v!;E7{RD+gjd|+BuTWN)2fK#rqDjxxQMPa+~lzY9|yGNI%QmLRG~A^^Lvkfg5tO zRD>tPRyXN^x60szgeA+-1U!4)yf(;>S2a)+#4w&=2?BN&{z`b3@7uvM?vJHCfWdAj zqMVg^P~FKzsblSQ+MsQN_4Z4kH(A^x0%E&EPwdPsL8aiyLSv__z{Q@yRzLXN^Gv6w zPRrQ3-HwDSIv1}q+JLXRxUBuPyd8MJGWbz?EL&#&R4Y{!uK6?b>!*N(7M*WlEBg)ojw(F=kkFzxFV+bAvHxM=yMo@q43U{9 znV|L3oBBubXf9j|)is&t=Vf8UbIdprEu$<@d$IiF(gT7v*(K`bvxe<%2QQ!8 zbk?AMz}TIaQAD1Ct)kkY?uK$2Vv260u)js-4ckOaFGtBLp?_4X%{~~dNRAe+c}h#J ze3q3!L}XC$m^Z{KlOQYZl-eNv{FlywG!xd=4ftpHcG*GG^&Z0=YL+iwKoOw7+f&y@ z?zrHvmW%m21w+sIwT!N?z{1w6R%FT+r+0r9(9@-EPi9e`WbIz zlFw<6E|zNE(qVC=2Cm~M*zplY!5)%8Q8(Yi_72bIDPy3&4eSkSD@+4G=&-X?%C12MR~$z zZQTZ%k|x<(S}+sCtMx@oqR1rzVt0wlPURI&Ic^F2fKE1y!Rzh~P1SZU%29qD`Kfk0Iy(m-vRC}zbhzuR-kMhx zw~Vjgis>Ug{bXWVkD0-s0s{b(8~6ZBmv`Ee@l)S5L0ivx-e{DA=*@=0Hkzm?pU-co z`t=^r>=P!yTRyL&2|9D7>^lAjkoaz$Bp{7e5}zyt)SSB2F67;=4HlFL^V4_b=pH0aFidku#LJpwSS`Ka!G=X*`A62khYnO;A&kU1G2-^{TfG5iNqe~rpA^%KJh)oEnOY%^F z>W%VD4ZiG1Yuv$u0;Z<58A}roH)y&J1W0=l2fGCF0o4Bj)eQ^tHOT!6)=-DXW{Mx6 zieK;_#p9YbvT5u%(=}DSlMups)?8Z`fsAI#p4j!VWLFmshwFo4q`yD^%1!LSZ*mK0 zMa%Rw?9ag710Man26C^$k2Pt)_$huF^pR2{K;M`p+<<-Qd``&O%HJ=(XZ6>!l8m0) z6qk3tk@$8L@0M#cNY{)v%(d%_6~K&qJ80(QQ0S1lJ$7tFLB`STwrs5J4Y{YhgnbYS zLhGbtLXrJ?T85h+$qPVu!kFf&K4Z#yyzhq!dJZ!4vgK)v8z$5 zt#3<_0woS_seVZDrPy+S{y-__l+zg5`)b$Wl;hY*3YU4!#aRTAYYxH$U4jfrjT@BM zG1wgeb+@`Jgu@iCg7Y0`}#+gXniIhVnUI{cG8(gf#eSJY9EcCgnWEl5;+XfLynj2qT92 zb3ov?-PI9loGI#5$k~8uDbhLPjnm`_P~s;}a?bdmExe9k%sed*gd^JjqDX>j7=F((IK=# zgbMpIq`v9*wV^RihL9Fx&}Pb|*RZ*zey#7-fXA92d14cfNqp#6d2F`*V#kVlPrVZ% zi*N|vKs#3sj6*X%b+Z9840!=1M`({4!2+qBO2)t<3nnJa1JF{YCKDNoZ3b^o+k(2( zZRV~`QS1)8g730H)X^%X_@Tt!H8*^KRcUgjPk3oCr-ut~FA0GbEKh0d`zH$*@R8I` zeGdRC^C?(*GwgKBX+l>c^w-}KHNU3$qW2UC#Q)IJ!QpXmU&Ws+?quuQbT^jY%-0L| zN0_%YtM1DKZpIqh=ykB&PrhekH}6TYlPfeyD=)taGQJpbpUO7zFQ1eQk^NlBD&2>aqv}ylYyz_Cj5}L{1-L$AyCnm zv^eQpxLEbr5ILOO`;6PQ(mTl+rJvN*2;1_V6|pIVZ?WL$S8`uB%UKwq6eB_7aJZB$ zT-3SrMegXaI!3G0b(cFRZ(P-aOp<<&K77@A`ZYhHNPD!nrgQ#IFsy3OfhXz2{A

    XPE8cJ7cCHank1?$-HEKYn>dQ;Z_BkR3`n%cgw;S`e4d#}<2mEJ*WkR~9Y(z__V z3P^_p1XQY^NE1R)QMwcnDaQt)l!Ojaf(V2T())M3-@Nnw?tS+k44Gk=oU_+n>silw z_F6H!7jQ92%f}S#1HZcOZE*TW`=&l1o&=d4=_84dT(>A!$Ib1KOFSr+=vI(!H{S5cB zy(7B8(w{CW-}x3`0m6X(_y~{O8(6UA(XpITyCN_D=}LwUh6?*WZdvZDFMtJWA-aP# zZP5KOR|^O7?kO3N{ck=*Y`FTTG^i{O4ka4I4YywlUw<;lnUtZP7ZW$t!|useD7Y>r zWYE8&hRC;TehAruwu?M}tql6`l>R)bY!R^Za7em_T14HNOqSpAg=5w;Pcid{pE<&m ze!T_LoYF-FB#4>Y=$d4~1Z^QW_U?mlV6D$e2D*R4Kc$ce6Y{vGsxyJatOY^ZVZ(h^ zH`a&wlv3)?g-xN=l2H=0-5O+UhTN>#Q7ju^TorolcKCQ|pkxB3%9*VpJH?doJ?sJq zb~T&lyKf#BfcG1EdLs-KrMSu=aaX~N;Qv~5srNA4mn@n}%)05>d8?<}HhQ`G@0~qL z;C1gHtd!l=Z$AS1L|A`;?^mV7Z0&l~pWcsUwuD1x^EAlFJEgkks`}&QygouT;R!0i zyXr(<;)hZ%dig%}(rN2X;=wiL6*_KSyIFPOJdv|?r0T!KJbnQ8fKa6c=$cI^i^GL0r6iFaFEz6XUxVO9+l z8GnZW4IoYz3SLG5pI_vWztvdu?lHfr*w5Vh{7)of7+qB!AE?Kaa4|N1C%T~smaUd` zd4oPgIyDM;G*^Q(0yj9zcoPIvS7;3Eq($HH=$uJ7BKEKQ)4X15qiD&hodTnq=B3*e zuNM;@E6_yd$ko3s1Y&TvF2dQUJcau7%pDx zQjw}|1|>Lce?xV~6WgIgAzA<)`M*}4`w~7%x3B0gCgnGF&(n*6DDw+-a>5a+(A3eL zOxev#9w{f){$`450>N$9{1_Pu-8VGd-Hsk)83B`iwA=F+GJ|iDp6Y)Vj)@&%4S)3%vt@wNUpH-f);yLiw;!@bR5?RLbJ>+t zr~bIPAV+*))Ubrp@K^Getcjj+a0UCet>aeiV8!F!E)$Dz6VZRM19%uZF@~~3hbo}B zG^%k@43ZB~1siwBnc81p7P}fQvFS6u6>`NSDrkx9;ZFk*pKlM zuN$Ul4erZ*R9`wj-Cc1tJlkr7V#r*EeqX{vRO|J}Cw-4;Kcv(!E*3jvFuO<)6R`(u zuxhXvcvwK$?SRsVgakeD#lajv#+b~T{mEo%G;h8Jd((rUIIZi``OxD_fj#E>dl>E~ z?0ypc@V@f0;bk=nB@w9@j{c6emj_BU4o;`*4<@(I8!Y8yVuj&beaH^PeBZUfs3Ppm z7^Z~z3Exua%u*{N?k*qQ_xO5)11BddryMKV&D$t^6tkQo;J<*Gd!4aeuY7S?_osgZ2{E zUeo29PUr-cr~sBlw~L7bSZ&!MPq(~`gyPc&a`dlxRJp;})D1F3YbZDzBy(Sv`$2EW z4yiEiqiXD4h6j2E5JUUHc`{5Py77az`N(*2r+WiuSDH008oX(n#poF-GI+=I%T9qF z?7C$XN5;OwBK(GLd8T@)xIOOPjSBb#GBW-x>c9wAK;1npBlUr2g`E3uF4+*wUW=+s zq3*Vb2xSSqAQ}Pg!T$N5RQq;|=Pwe+fR&eyN%w+j17ZqZS<@(8YSO*oZ9Lvx3&_~_srRM5OtGsQu!}9IbQ>Xem%mwU$UZ$J0zd5x4C>)g&P;$(Wi{OLO z(sdV7=W}FFGyM$@l-<54F6gx6j?9L>CU`^Yy6>t?q<_P|G1Y6vEWyf3UYbabk%7Ih&44k$$>C`XYkOSOyE4I^ zAH%uB<0+jUBy~Iqbbv?j&Fe4E%^E~D#xQn!9ws4Ni%#3z7Q!cOUe6kel!GB3G7^;41=^~#^ z+uZs7g_3)JOr3Ule;?$JrV2Rjv0zQWHow;`-@zB|RO~^C>izQ0=G8A0k!KA;PzFg+E1-lA|A^1X=V)ac0daSc3Vgntx!!=`=E1NDy!xTsuGH#@A3)m zA2jlVH4fXA(me@*EkE^{J4;d}?Ydyc%oP;{T!3Z>CgrX8G zJtdPOoj!hQz4>t#*vGBVq2?M(0!rhx2)oGCZqrv);H{p=tmoSJtm?flUnF`GEfGsq zy3M<`<(jK1c}exkOA=0TpIW=Wi}v?9Jc-B1!nVri^GZ{a&3uty_2-#T&s0XK*7oB( ziBrwP7Q6m3U_}j9S6%_K%pNzc4xVdyqnsYJ`ZjD)cl=u}D8B-`hFMcLJ*l@=?R*KS zJUHZ5&uxmR0v)SzrA;%I7rfZ^zh|}CCPOZJ%>e9|MS16^2UjBO9qfnO8^zdBXGXQ) z8~&#eSV_c)sB;9-p+77d{-d!!v>i4@`;7;^yk{T!a`;2k&&y3-h7RSbZpWm1F%HT>5<;Xn> zjO5FG4TyTBCq*cH0bIzBwtG!aK4HR=unLp{$1eH{?grOs^)lCGFP3g$`Vec-d*7`> zEE1rJ=~X?xA~fYYQ_q-P0*LFtJM3wEMAwWy@!)TDH+;^5*xhe@8&nNmCN2QDPpyOB z^Nr#pgJ)`}L1mXQc09QL!lKjh?CW8gwr_F18t5(FCzV&jKPzRNbxzqQdYD)Gai4v9 z;Doz%=@B`5w%*E`Z`x|D+C8?8+7BO=NwptrO{M8;pE8nNDb+&f=1lYLMaRerNN=vS z-kT^~9v|V>?ft;(IyD-x+E8#gPm#4-?1Yu9Ku2+G_HI;in$H;HLf9vDp?8`>;#Xg9 zcixu}(s6}E-3njn*+{_Vu9063>3+kib>FFh~MPWxwMQ>^!;^&601A%29U9KC`=@qH_du%9g?qQo^ z_lK)ReC`mnSw8P^fk;M#JCWzM zk4nvOc0Y2ZVoq-kCZQSssBDCi1iHH`9C}fQi4p?*ozOy}(ZjbM-+nzIkOYM9>kvv0 z7qK0EaW61u#cYE%&W1TJltg6NAgO7an**gV`A=t@RwDl5u33J2jDAd;r@(-;dfOet z`~?a_NaEN1eUE;!CieZ<6}h`c*X@kQxuA(1+oRTUuqJtI04JOdmMA1;!7GP6rP)T_ zqq9qT@tOUTz1};>l)!2FJ5^i8Q1*T1eLbSh(yyprYU^;HYqkpW(r8cY{MoMw+$fWH zskCgJ)J2TQgBb0Nq*B3&i)(>#0?z|^sgKE5=$q3q7cpD-SfVj_mCN-*aiRHp8dP-Z zOu#}Z4;~R8sb~_qA~q)YvhzglWv9HXtgN9;zL(4Q*A$dj<-kw>T6s@90AF`HKS!cW z{d_a&BIX8O{V0lw_ehTUNj5SQ^UQ^}go3iT`)+$CYnj0*U6Z6X=OkY<#%7;M5mxv- z5ASv90*iYeA(44`tLDN|+&0SwC7$0~Im@L7%S31!HlbkplcAdg+r#nt8zA+sowGd& z)x(|pDO8!Lo_Gys1#K=bDJVX{2b*PmW-vToG-74PS%fb`+~O82S5H3JJk!3nyPs{n zYfnc_E4O<{+Byi4dv;G~bA`)}HC|^L8p)LsqF6(-xy>zI=LvQy!tP=3fN$kV?foB0 zGO~3Ec@S=zoADAt{L96GzoXD>87LZ`^b;^%DpS5r48z!(I`s=+J--#Zy2rLF`YSCy z_g`n+nk~F`D)M*)qOPrYxB%{6&fHJoFMKM&JW^`&(qfl?shcRFvK7G%v!x@Yvc6HI zu`{&UN#3kAfy^guZr078&#p~xU z*~{r@X|xiG6Y6`RJzWjp*)MkYm8MIQakDs_uvMQAsDfT};r-m*?OIvyq`pQoGJuC| z%8`&p4>vdI;5L7au!eg>g>uAB)nDABR$+r;gZ-07&cD;EaoCr*7Smok;b zX9C`VomhI$P4m->V=rN_?v9fx&Od5n$H?MasI{VQ?-qx)HqGln*;@(kL$r_3BST;U zcE8iRERk?Z5|Y>ZsQLVX$OBRFi}>XYI5Tw^_rbE^sPcTLUtDZ;X0-UL$wX<5Xj!80 zpvL!ZG2a-N2%@TpV@Y~j3eOg=pboctco^#)-)n8Nu~!hDQnD_Yw#&T+%YOu8n7$Hd z7iXr?mTAVv8dH@cT%sHUQuELHbd1L|B6!PDDOAfHh*ebvV>t*m^{Z{143#^Mvp~vWYpgtTm31aTau;}m-|1OuZ$ulaItO;F9+@h`i<*8^1lM z9t8c5-MD65NWmCmBQVouE}t{a(yu$1Lr^8BjMX?K(SQra&Ez!kK8%k2F`#4!vp0=t zsrRAsfUsK8 z@f+i8t+em>9Sg(cTb~2$5k(`x4lxNkg!s^-_}JJs2lRrUt7||6B0lQXSES4LtN%v} z@D?~F9sl8%d-ju!oyG{gCE$X22qcq^+F@!~Mg04oE}&O)`L>~Aoqhz7DoET;h!2u> zU*c1D_xE3RL8;cvDW-nAdtD={&c)ve#}NX}?2jUTd0exAsFST-p~$tUgej4We+nZ* zINsJ`$;BrREdG`}c99H%&t0DYhPr=+sQBB%X6>kr44t@tq2r#-GW#`=~SVAo4}tpkG`_SI@Gf1P@89jma_;mo-K8jHPU z9&TeOf*`OV@H1;ac6j>1Vn7dwI5M&t@E^^!*_{4sIXULoPnv5GB-9aVS*%7GHk#IU z(O;gjEDUBPYs&K!C=z8C^?3=WJ1;jJTc$CUq?vcWqaq1xGH6yd`c%}k+B_51W7=K$jLm;`N1U7cQ+w1~UQL!oe)Gq=Xm;~O{6395^YhD{V{E?na*L6qC__(73ehuC0P921#hA7yNv z_G4*emQDJYqnU=+8T=^FH2ucfh6g*3s%!LnUvvsd75Fgn!qs)xau!o^Ve05Gi!?=e zSwz)UeD}UktVDOJ;7Vq-v?uvbI0Cr6qkHRpH`Os0Vn)?KK|9(ZvC8OUn~C+r%2urZhq{kP}kJQG_S2_u~j1#97SZRmx?$58g6&7Gt00% zyMzA(&Ez13OLQUhQBb}cEqO5EO6(CvofJOBzieUKijUMLDdU*%-1znlZhVXT^(K)G zxfMpwVO9E0`6l|V{KYxeQ4RcjTvcwvjP{)?1^7@l*k?x0 z^2T2UOfVm!!)0v1uVKniN*_BrMSpSYI?C{>sE6p;x6!Fqd~Q%K5J*+ybdo#>>^&*l za9_Rk2Aw+k4alb22w&3wm{Rc+>MyqW-&Oa=lPpN{$Vgoa8!d@TPj9&3$oHYCk|G}- zq!V$sPw%4{zda<9z9~WZJxg{rF*Ks&WcD3&m61EdVjfOQ=Ad^(qXJ7pRDN4>5ejd= zT5^PDOJ|V9Xq(npiSpnhwEVUmYHose4aX1AbH^gBv6;-E=2j8Kn4ca&CA)D!ux z%!F3LhRxZGF_mTz` zk~tlvZX{?~FNMEJQka*aTo`F}BaTK4+*@+w!Ko0ux*v-raSPsMdwn8IoNj8&3Gdy| zvpp5kHBf)J6l9;ir^uVTj8`_PPiWggLpg}wHOtzVaJ(nTEc<0!h&Y1R$J+%ZQw0n z3uH*AF+H2ZL}2|eK5#Im#WXP0_>%-~)sSx>{dHk3<}i^=Z>Ov~optUEw)x{l2+yQ1 zs@Lo+$arJ=&M3@w6F4>r>fjY(n2Vh7Ysf-}-1@HS+xHvf`N4F~19wYY*fTD|(i zP|hB;`;B578ugkU%Rs~Z8Zmzk8}^-ZHP`W;zk)AHiDLtL^^%U??&sjyu(vQO|u|U|01S2 zt>G#r%NI%`(*N~dS@}BHC{w{{KQr<%F5m^=TPmpfMw&8Cg*g-w?-fO72U6HIRk^&{ zAC)~6xI;x}r1OfSzP4f&&2Xkk_d6VrO0ZB~b|g5vkn*OGT-#T{6zBW1_^ltFZe*0Z zS7qvhEiBN^ad8+nLP4K!ti??qUZ)$|))Op4p&~UjV-&IqdCwI90%^kI93@XA>P+wa!zDhsg;qn5n(gvZM#)Ulgcy6c&e#e0SCu!079k`o~Y68R0nOD?C# z!^j5_8peUd!ZOPt`a#sJ_oF(E$%=0v;-j>`pL6H>C8CSmnf(>XjFqtWAaZymxS>rF zfMug56~iB{%x4Q#P0+MUBLDVJ+L|5iJQP*l5*#GoKEVxrDTl?EbJMIKX3V==(MnkyVlSqdU4ZlKf;_vpA&ACHOc8>3-gMJusAAH{KiF!)G_KN zq_~38<46UcLOSs`DmlMl=3-|O+C9N^sl*tuf!q9P$&US44AL=b&Pp zV&a+%m3FFTk_sL4RNtxPZZ@5G_GPJ}%{ARTXis;46-#qKD1{9WqcNsB?TBnbjmmzM zeg*4<$TD)*HNL?k@l-CnuY{XJ>Mv0ycgMzK2e7jGoiF264^TGldupDV8!#zhcb&~K zj@FJ}%arc_S$pb09@4}_W6Y$MfmPEIpS!68M34|?ewu8hb{dW^nD zBf|N{Av@nDn0E$KFVokwy)21tF-Um$bY28m%$rtNG~%Lt^gtqUo>M_fV~>WAe?Nz1 zV6v>*tP>x98&5xI*j3?v4-LhNquCh0IT5G-Nf$)l>+YJ~PEzkFF~%Q>(% zf;MTpW8nMC&$lr-)erLlZ$K=?u{>DfYx2hkYG$ktDTgb8O>Xr$ojYiW=DOTzOf(R zo;byn3jLdv&Y7_I3+2Mc!6Z^RuOvQp`nnoQPTE~(u8uYruJYg*Oip`*-9NoEhPkZ9 z_MXw|CEXhERwKp;jpRo_HWRPsPe7?^&v*i!WvOg;LvwT3!GRF*|@LSI1?nH)6* z03T`fd+&RwxY>C4;Csc!`e78`>{xxMr$sxYqu~W0N%Ie+Y`jW>EfuNJ8vK{nsQWF> z#q+c0S38YP)E!+|35HNs{h8Wn!YJn5pj4>C;GT0pyLxQrj*LB_z$DXj!|wIF8-Y$(l9kZm)?MJL5Z|Hu%uOgAPEFP$ zz>frzS-MldAY*)#$q8T9>(n9aMQb_hPZ#?aVBN(RoVKUTZK+`5X6zG`Wqn`+AWyfQ_*4ipk8Ip=P}oG7jDT%^vh{gU&b5|rq*&7@(poY4=dhNG+hj0 zkltq+>^Rd4B(LRvni zk!+=}LQauO`6iMe&+|@W3*d41qXE%3)h_3Y_g)>{@5G z2Aclw-YQLzHzG6rPCP^{A~Gm*ONSU06en;AY>BO@=(vkxR}`Iw96~OEN$e-!1}SA$ zjQEPIqoOheYy%P%KzeM=Di6E&DAgG#DdUJ(PuCej-ypgV`bUo4?^NeCu^XLGgWGA# zXlW(Et6`EwU0>TMlC8N?@CP#&NZit?TSo$U_p7*Q!RRuqX&F>CqC8(a|5Cb!#b1gt zZ~Z|u(qIXhPcu=l(S#crbRdN<-%SCF+E=Z=cS~?<~hJm$*l*CShbT{`p$Ku7yIyGH*iv;Hrl{`-4fy_Jf}Ba*BW zTfLitVU>=Ws0F3HKFwBft9*I9#21+vv%x3wi%wbA2i*JWK@1%CE>l(+1ZGYu5Ng+r zrmN8r)@PbC^f9;-cc!wb*#YKNX?y0x_qk8Cncis6JtDE5a?)ohYGZw{X{Kq}a-O}@ zpW6)2!(7ZX@~%rwg$5Pa9#Xqw)!A56Tl;T?QXwo7R-R6g$MZDOSZVi1>qXR=klS_2 z2o2N?c0VeiRAP;?SZ8R;s^_9TrdkcmKd>1S>tnsLprsQtHJ#e~wWnL`tO|bb%6p_v z{kaY9rP(lkDM4@LMP^Iw&012|2*{fHJzxbMmzmA^6Fr#E5maxR)e*?8sFZl(|0G}O z88dmo4txK6?u~3j6HvxE)1%^XWKwf-ZB1}GIT(2 z>yrfRh4|9S}#Afp7_{-ZL(TKP#%~pu@(1Z9|$=bMQ(@#d!)L z>5=5RH=Iy%0jlKof!s9pf)m00JR}?OCmo`YSt_C}vlXvEeraQt7WDIavF3aP);K`3 zSlES|^3{QK5PuyWhR^`}32H)r9A);N??^FqAMg+z4Pd5(tJ0u^hPN5I5=aWVRMAI} z&h*r+N?ia-%HO`4L8VU?r5^| zJ=2!%7oC0ux^GgfrMHfs$39nI5n-EUQ~D5)Yebj-dV8O*{pHdshxWqobktN z%D?Uw0n)wVK@6_0(%Gqp+f9-`SE8sZ;=)&RS-QTLwHp%Pj1iE9#J_wG zlg4s@`^)1eXV7QG9n}V}HJ!E3uJpyRnd*Oh_C>L_S(H*{jB+%^zbCs#OquISCyk~h zF_`E>iZ;r5ALnpT=Pe$|w$hkQ%rwGuMSN zJ(x;DSnTJF3@~wJwo<{?Uc^TbPs_{>n7**73^R)3z*I-*rL^gcd%eC}91vWQf-b7k z>q~rxU)Yg^df7n4UB}f`hC8qiScgTo?IPI_Coc%Gq?6#7?XVNUnCI}NJSXuN%D4u6 z#Ka+CMj1E0_@Fmw*wDXw4uitfnMuuTxocZy%xf)R*`Pd(yj-};{=OKH8Oo57q)ZtX zKHRDX5`hsQL{#Y{O(xA6+ly$d2)N(&*qVYnHg@VQ(S>*;$Whqfr*7vusg00i#*=xA z(@)DuaGQG!A%48QxSlwZ4#kXJ#oyOdRtAmrNM``7XP@2XP`<5oguGd8Cq zL;0Ghlr9AEbL_!uD8~m}yPElwX zgG_LwR-IQ+J$gq+3Ghg~It3O*9gDu*(;ah-C^`Df`cPJg0W5#*M^MQ}nAA}c`3M2G zwp(XN6kAl`(ofrVQM!Yhn+w`BzRw^w&>tmE8IA5AKV1|Wxi91OvN&nXRMBR<`25BK zjLGcAz9dFn({HB7--3@ux3UV-o)YNkPf{9zupMD2PPwwSu&w(y2b+<^wf77{7`(}$~t6mb~m!OD-Jm$vQf;Z|%2 z{rZb8tpPdYE7;Q$4|3#ly7(KbtS9g34qHxDRHDxn8I zAR5tH=eQEY?^InhmHILcP!RU625QPL#6)Wq&V63q-scoO88cuEcyUF1gJqt zg;br7rakzD69K}R1ME4~lT>p|yvu3GAXv(XC`u|TNixfhBL7hw*u{iz=^Jr(C=+RR zSV`&?^!lLhMS+t)TycESLEv>fyF>T{Xc43lJncg3v#Vyj^F3-vT(iE;? zrWAHaJFUs^E2Tp@KzM14WEiJ;k!d;=UY#mY==?epQN(EnBd?l}7yE1=#hq?C&i(93 zjx`|#N{gJLwbzH7m}&GV;t$@8t=xom7Pq$dz(%giJ!5n&{E%bK2zDujg+Mw{pb98j z^P_vf-a@&@pNX?a)+?MP(N=7;yuWa~s*VYNy=81D*(gsBoY~s;Nu@vYhBwjXy5Pf? zxg{7MtR<$BiM={M>q>*)<-9X>3j5>ahi$_xn=R*>omDhElc(j{WT?RAvihGSQ*-!6 z1yo!WSk&dh1$-FrVTQahr;U+bd@o7kx8=N(zO@gX|p2vsa7NRq8 z#$G4GLF0{T>0Vj5-v34pC3m^uIs77m-?}AQ3oBdAG_Up+;eS1d8eR1Md2w~S!ba`0 zE7+13Ti>5@?uXLI^C;1uboX>W@_tPk6%FE-4udp-fNTd(5lZ=-?fy+lT;7mc1%*3p z$i?OaNe^t*BwKQuO|-lYSGhvcsU2b)?fh`MM6;*OUESG}qr)q9{*nc1m`p)y-G1xY zOzm~Y*Zr8@=FdokDifv(VAJ$_6SE-rU2f_Yz)1?3f{y05_gV|DQW0A)pmrfA z=d19xoln%$HTm;A6F&d7NsFL3Lik~C5!Ho~*T5-Y3H-u(b!B`klRiGeE=x7>HHqU# z&4DOvM{ixHsoyg<0xy?rUGWm^I4NOIwd?Innd6TbGyTV7E89>BR268WAC1UTwMTSl zX=LhDb7}J-$K(@NWybhCWO;TZo0I=+o4f{1-NwBKs~5n2v+LFEx45}x$*N5ILPCAw}@YDSEiQw#N;hSQL zAG!-csF;9ox;aKV)4@(XLKg6g3Kj zGT*FY_q^Up>G}3~V_&l@HU!P)?CM$@PK7vg1&^~t>E3B~Eck!>$=&&Q_SERvjd#|= z#cUj<)ea+A0JNs;_K`1Wq&6i7ALnZ=92HB2IAo{Tssmz3Ob1 z;YQHh1c@(8tL8-a`c65&XvD5|L`E*6N|?$)Q|nv_n(Y?kn68O+hem(AQ?m~;%c>>f z6h*u?eF3W(X#`-SfVbhem4;VbZFo+X3%s5Yz>n&FP%1$l;{aiVrMJ zf`8E?Yk3GZ3gb~k<$);z4uDDUjd|fHoxYO3CGBAhwb>ruqJr`i3(~pbbl$+V*=~V{ zgf&y4h{_#vPf3XIANEena1Bv_q(xmr*`O*rmTt-T^Dbptury^UMUk&O{B(|IUfGKY zONDoEH%yZTMLc}2{cBWGu8+4-3S?D+_N}Pyw_x7LNYrQce5*ftw}KW4Jwt1?SX5H( zw474MGc{1#roI@IbCSee8V_{tQl@gJT44p>Mjd+;MMd19Pri2BkyI*D0<|XOeQx|W z#>KXFH8p%$lDj#bSsA+Sp&%<8#?}PhKn~)+P%Dh**%z7l^fituARvea6oyIwqghc4 zC2Ungj*>xj9o=Fy1!yY4i{L}Q_rG|UUw3$f3{?nXxFFBnBn9H{J#Z?Y)H#11SBr=d zXawBl@88{1DOh;$`7@C75A9|Zg<)M=aDF`93M3UJeQV-(hqqb3k4!~dX-nK{{CyLq z2lWFD5qyba=YK$YwAl!dsT#1@`>&Oy7K8I|xaWS>8S(pakR+W7ln1|FN!||boAzcK z7R+j`s8B*L%^%c!VhBC61w-AdBK21#U?C(+zSRuBMG0Kc?5mj@wd;ngp%=Jl_48Y= ziRP1b@bOljDd?H?wUnn78~{IH@(F<)|d(P41#D+;qg9PdjA=w#^yHtQuw2t6^S_#;{LmD>@;InC*-vLV-%Xk!fB z{j6BHTteA|vSRytG27J@cdqd>xyECU18H7ioC)Z+kmfK@y<3DZRPYMTu5^i7P0o9h)m*0BZB%8I2jX-`U9^mg|Qj-7lWm(hWkGb3`*Jnj(_ZFFbByU zI}bC;h0?+IpyrZwL;pt$@KtfRywM1NKxZ`536`jy}Cw#8ye*2q^ z*+2Yp0sMQ$CLmrG_6J)U!RxA$`zl;Dh+la_G&dwLyEVr;x)rsed%DJw4Br)wIjd4K zhMdx@q$uuM9UPOb=!dh)*tWdE1Y#ooJ}~G%>M9b2TwA06?U-d{Gu6S1YBJ!IDlet_ z^Q9Mnp|Y-nK&%(VqDOt2QxFLaTZVym8F=?@e2+b1;x4~9vuTu2#7hx@i)ORnEit8g zvPil+VtNla+Xk#+gl{-%uGh>RryS8(Umr#k zfq5M=2(AL510BrW7fj>HG6F&TS>SvK2EZ6ZFW|d3M1Q~|g|^lh%ahbdqHCa8W(4AQ>+AIh%v`6JUdz+o+c;iJ83gN za@&z*Zep-v8AZmwG@-CD3yVZ~C-g05Dln?0f$080jx{==+3D&>>91x(7tSLEN*{9wRPoSia&H<{tU6WYXf&)BZUeoH zJGnD;gFTHOP!`o(xXaw_JxAafh3zb^@-}5vWRt5>ti-|s`}hl;V^1)_gs?VYH(1Rj zN}4>*CRVVZmy8~Hy^0Upi7~M3YuCUon6>}9gB`#??}V?syc3?ELz4n3BOvoy2Vy2r zDzYl!l&LFwufgY>pT8n^b}MFxai2auW5K}&&8U@GumH84sb9cf#EuYXx`OdNuj&)5 zClh?fM;(JvD!9Tlw!Um@P9sF>6{bAe$(m9-$i&~>TY^=j#GY21_G&G3vpQJTd(6JZ z$7fqtKAp!jLfUBU3<>5?KgiGYQ)OIp6V1K|WkRDe7u5=d9cL4*EGSJNcd~sXulGmB zU#=iAe@v3ZeXQD|1$=iyhF{}}FSOJlP>qs{zxfL%FFpE;e{e6$7_k51{lE`%wioJO ziZ5LVpjex;pk=~v5mbsZMIE&_R5=kVaPGw2%mp*yC7)WA@pd^;Z{=agp0HF0J#mPv zqjsDFt1e9)^d5v0gA>RkD zToz|PyTw_fUckY`5+D0OChUHW?~@G#%ZA}>jdM5>AJ)Fga2WcJQY+D>IQnb7YRjMZVNb2 zpW&0Nr!}!=z87MQO~jx4I1lC(=DBD_!)DyWU$fZ9Sg#AY4@Je?Gj-7HJZQY^5Eo1w zAvl@sac2o#^V(hZ7f%OXxzm!Y>TwepvJ9aRA7G4z#0}QBTapVLr$H$2wM|c>WaOWy zQX9r*_3V~!cPRBFx#ZrhA7Z)C30MV1Bx_Svdes;H)wy9hgr5UjbGr3DQf<8zS`+yO z6M7<5Z8i{sU>dD|XvZ5-1f&C3e6}qqYTQ^W=sl^X@zDaW@Si z@G#xgu!$&mb@JmWkOXL$`X#A@@keGKwl6U+&5L~bRW)H~QXiBD{wW*zO6UB8!k6Bk zi*nYAR*7X_#$6{%e!w(+yi4;E^V={Qyx?s!4q2AxZY#)o_*%zNN#-`PM!5N2_>)^_ zE3aUF@H5?y4_=%W&BBM7zXt+Wfy$tT!)gSw^ zMAMk1!jDocJLHSWye>(3(Ls=256!up* z{@?7Tq6t~NQBsK&#zdLAtUB-~U48BBZgR5ZPB(uNx*iy{gN~xy)uEZFTQaE^31J}? zDjA9UttD;N8b&dTwVp4UCfERnWs*0{!ryFbwuULdEHs_J7ZTfU4*Q#n-rj-*sShFJ zE7el=_Itk5iIRIO2Ni50Y31n_xtN*jsr^-#imb_I&GZ*$fa~DMnmFc&nCLF|ZBBcd zP|CfqYu|4&UmK%s!rn><%FV$ZszgF2Qej;oR~AOLN?K#fq02?Q7b9RtdYx5-J%!}c z2wR*mrHFUp{4&_dPzNMd9(z~MAGm~eJ2!RWo=($}4jZFL=<6%t6Q8@fyO$P_#~d;u zKT90mfHgHCS`Qinu>*J$nSGFboXE@XuNgzB?CYzjCyaIn)(Z7{e3P;`)oG1yddKIU zd-#C$ZD zUxM+vu8t9dJ$C zdwFF`1BPXe<`K-_?kbHZ$tpS7^V6VsVfXZomix43%q(~%amZ9Rcsru%g+z0ae}0y` ziSKmHoRv|5rdDB&`GS+|#K)<`>X%vtSDlkTb21B+%-rXg5FmHjXg>LBA5MO*H z&wU3+uM(hke0E_KPx}Pwf5XMa!FmJpGNm1NGdY-G zRa_-XMe-_&tYS>e8uhEXnZRopoGGzcRoYM?5%?vtO3@GD5EH0LG1Xwp0r%u_IJ-Ql zyUqZZBn8iK*}5*uknJm}ROvZOqB2kYf89OcRgydO%sRhbvUUHg=-dDp&sf_h?z8a- z6;U=ZlE_q3xr|=w#5l!lH@B0I=KYFf9qDwc#~j4~X<({$i;LzM)dWRY<;SuBn=wj?NKT8dL&C;%Ze z<&I8Xye}dZ6bVka;+C6#;EOHtTKx#FJAwG)lUmJ09LtNJqV)j_yg|M>@|pOP*+gs% zZhDU&K!fh#{T_aK_5+AQK>I;tJ&R}$I|%vv`~NQMFdAKF_B5Rs-}vF-5Xdk{xbnQL zA2rgViM(7AJYe5tNi^Uz);%%t)l)my3mc}eAyh=cTOP#nM06M}7k z*>=WyzGsb>&9co>e}^nmPP`k~7s9Tus_}D1TJ`0N>FeOn&%qUT^Ey$%sbKZ~J4ovb zCm5FPbUF_LecjN`_Uwu9XF0$Bpt%iIbRrsj(3h#LQpg zsgx_G&Jmv}PX%i8j1j!BDs<*=P>NSLygz+n>O_3%j-7JW9H7?`c49otZ)0q=%y~iZ z0~$5;2F29%0pA_VFjEJ+39mPOMi3DqxnB&!Mc3C&_|%_Z{Uveu%((W^5t-CGdiv?9~-} zoVvzl_XtJ+CspOup^AxbJ28u(E2`7L#=!lcbdaP3@mDKvFbnvOM-Cv{oVy7`5eQJk zd0z5*YU`Vij5N1WEa5R^RWIz?bn>3`A$HO2TdQ4D;}4r6lRm{yQ-Y}y=&P>YejV^J z|M%Q9NW%VmZ63<2I8<%6X1TYXr~p?r^GM%5o#u0Z%3Vv~8DW!U>9I>!9#WNoXBm2= zqgS?L>|*Lfh!CxJ*F?{@J;5tkT;_|H0Irua?yG=DH6m>osQ~-``uYx_Cc5a~AObev zQxH&ysDz?a>4YwIK?qW%qx7yMNN*}47SPap=z>VEQbhqnFVdt#Bmo2@Gzsl3|2O|n z^1Ye2Gnv`lyL0Zjr`_KoUJr_JWI=?7kxGgltQ28!rlh7tNS zk0q!LRYSs^DP8+~@GqO1DVz#E0Pl<1;>04-9wKtj6Zj#^asHm{z)j1?jD8Y zi4?|~cbXN7O|RbHIAA7xEm6-8mhNqiN(!E61{ZoVgs6qE>ih;*$qnb0{jqRo{N-`} z;Z3J+sd1{~+0^6EwUeQ?N}OlT`$I$B-5DyAOIhe;UUd7hWv~rNGJbkU19u+-(=@{` zkpwSf;KrtU`RPoD=NRV}=HSS_CyHgFNfa{d(2ws^DV-RKbINz#EJ|St8QtmL%lV%0 z!-VD-LB>M)xy4iZ#0Uk2cfr>!4zP!upA4l>b`83+lxIA@^zwGc3O9`eAH&A=-fB7^&n2DW1hG4g1B*3HOgu8~mKc?987D45#!t&A2R5I#7pL%+fyXMmxkyJX zel4|2%~71v`(ibepG_E?5X+o=jH4EJZ<1S^ot>fO)6BVwga&!}!I1Fm--))tEg>yV zY+VluTrUfq+cagp62KqIyz!E5wHC-->FYaB#pM<@wC?^oRq3ZVrL17Xi-?|()klp~ zA|L7(c>I6!P8SmV)-jXdN}xcNj`W8!hJC}a3Io&;rlE3k&od`4eCgXYMlgOKkjU@i zqraIa7v}dM=dh9`NOs%h%?U#$rF}L{eCngEd#T{t|FDc<>jh_a#&|IZ&2=X1Ka2vF zB36Nq7-L6n!edp^XtR4lTiS(Ilo_KXMB&uw!*?PHEauN|6>R0dpG>nT74p4OoU&zQA?t`yiad2J%o0uF*)@!yBIvgZRL5Wj&YW%D_uo{^^UT13Tnn zj-W;(8#}w9e~chWC&s|+=V!zTL#i#sB@SWU57)xQXio~filA$4ONp;PE`8!mV@F|A^U4(T9q<#wy>vgQOi$=U!4VJho>0(qD-(9+7G)*Mj3j=MmLW${OlQN@+Ng^e&YlpP- zQtrbrRktdIq;Z)sg ziKdT)A41NmCl0Ar6!2I>ix?iB6(#U147{l25}IQF_Zb(2C6|P(ic(9 z>gqj8X^wOY6=(f9s8epdA%F=22M-?oHwCYSYi8!4eCiIJ|7*DS@0<*L9U+yF*7%%D zT=DZi7)Z-b#$mnH#*~JJ;(0yryMZiUz02o=V2d+R3)I0|hZ-T_kT*IbBTf$chBr<* z0*tW!bDruE4)kqfU#y_-rsoFZi&i4o0b{JLjW`BXM{hyDBd_(+p= z`ZTaEg&7$G9+)Hb;LHm;W#Iw*9?x_~&0c+I2xqU4J@UY}VKjL$^*X;D=}=1nf03c@ zU^x+=!J#8LTTQ(WE|69|k%Ip{g;z>D`=$?Be7=m%J*j&tEsCLc{bBRemBD^HzTW`q zo<|O*=S59`Hkj5+uK$CW!q;>mlDe>kO;H5@@%aMQ-)aA|C1XYZMA(U`w<#8wbeAV9 zd8<_Z;HwCXL-G98Rv zqLy-(8auUI(={$TW8w3EDw%@ZFZ{lTDxhY2GnpCts;Im);RTobigLQ9oiX?qK-6cH zB`NQL3*L}oR|fa~>80X#DBkcDg#Vyuqg2CC={t&2gx&}^<9`L}LqD}A3fDdEnm?!64|NCfc#(}#I1igCp(k&<+>hazUos7aBB zXYCYYY)l1YIi!h$2mJc!h;C=6a5nctfrk{uRD;(XK2i>e@>2Htk25Gl7H8u$yiokr z4B#qw*Ab6-r^|45|C8Rd?w#80qWO3)ZAFSPzYLWK=gc?i#^RK&)xLCHQW2-Xm6hjI z72j=RXx~X^s$3gGM~cyan{`M?0;yE}e*BTiZT4}YC~U#kqEs9IckVup4HGVQeb2zB9#Fbw(P8`rR_#7rh-wm z{}e4mEeYAK1$P;HpQ8j9NzmK`UlKg-pkSv6OPBLVh^l(~HIYQQt)^zp__peF@qJXm zYwg=*8SK9~HQEm@geq#EGr7BUM@{QlnsWN--1MB-pAABVMSV@Cgf5)x?Ed~F8&suI zfp9{1+PQR7jwOyVgCpz8`_tP}O*!vVgHC9M@i}oB@F*`nnl=`_b-ye&j@m~>+7$Y_(B_^-E$_YSdAH^9Rlz|JdkiwTd|c@dL+E<_ zvnS{0lH&qmTv8;$bV8fOYAKo1A@yBOY)h0{PYsOx0)hql)~JPqU8M5FnY`if%M{Cf ztG?@md)LeyE5y$eUItFFW29h~(nqPmp!SV>LV`c0-zt-5nPgX*hP&YGU}R->F4B2t z=-ej%z7gTJaK;(Q3p5Q$3C4ADCksysoHkQq;tM9}Skjv*w34vdoEw~OG4DfuRW(iY zZJS`Ys+=+NJIMgsOBbFIMO+Ig4#B`CW};% z)zNYF5=BgJ6Mp9h_hiiRZ6uG6Dy2KM6n{S@kzc7s&E#ZGTN6`JnNMo9p&8T9rM#cCK z!)ImA_>*tH!X&Wuz)FhzRt@Ycqz0F~E*;rp|8WXl6ZU0wdjb!W_FEmVZ4@~Re&!fA zso(aZDw`Cr!g3xXr5eet%74co@UccdG9+p-GK#f$C`TXfJoQmNkYi9R#FaKasl?|h zwg|Qp30TA}Cibd0y567d*5@3p%KgNJ@_8(-8<^x7W^YTApQNZ#LW7^e)EAq~<@f_C&9?Xq_>2v_>O_+S2pfJob=z z+VMRab7vUDvq&zDaf{ERd37&d{pHlVr?7oVyP~mL5F3WR0~jQGn&gAa!xpVopANBV zcn*w*Ejmx8q>u=& zxudO{Mr5bNFphbJ6e({PFY5#aytbfBjCEp{A=!KLQJ3OLJ*~fLs6|c8YT*W_(as7f zGc%-Wfx&~rEdAj(+6V*RGM>TJbk%6KNZ;!Q#zXD;oaUAYgVtHl)LyqOTDw>1hc2pR z!2>1p50-$uZc5cxn>o4gl|#q;xq}dWyhrAt8>XOO-vvSR+PrmZP@SekoJLW&4)5o12)ZCzv4 zHLv~iDBt;v{2k9<)xv`ZVe^;3jqOzp`csmnJc9=hR@XbFHjC&rJsYH$zU0YGa~y@{kqW-k@=T&h12n}qL7(1UkSEc`@&#g|D9zENkF3?9}74D^Lf z{7d9nmm)Jmnahhp_FNpBH}oZF;MWGhgh9(dM??(!tlpxEA8QpLW6qu_k+-Kf9?U4| zJ*I1}Tw|Yx&&~OSU3upxtF3K->yN|7^0;-SvX;2#ACkiLck?P9&6#;$Gez$q170U4 z;A5THP1tv1F|=7X#m3&rww-2OjQp^42;CBocADCIT6)X-JjbY>m87AFUJA!K3DNUH zhNikPJ^M=(b&oyrn=cFpvj7I-WgcmG?wp(LQ2x#o1NMRSJ(VQZvRoacb$+AX5ZBJ# zXpZlvRr?^2OM8H_qCN%c$-7@Sq_%016V5!?`pq@`CZ5Z_M=YH|7s}E7Ej37jt%Ac~ zX14HG4VS2|IYA8w?BDxS9~C9UnvJnQOAtPpbP}*7lt6+N5X* z-0&qf?7mIcPuo<_pHhNA$|v{wDK6guu9K;&S0fDOW6LmKUv(PUA39WE9L4Hh_vis5 zETW|qX4EPh0de?9oA>g9dk4}_&N~qAKg*DI?@zZ^gGiZuk3h;#l^cMJ;7{} zqw8HEh`4jNJ6sCo;Zr9e4)RD%8i!)Hu`6V3V(RxFpaI%cq!(KHvJAI5AwBF$dzZvi zvU%;}K@Izbt6zXgG>#?|3F%F0NW=vI2U!mzB1){=BEGm+&pbEt95j%ckGIxn^HB`Q zru<7h7-Mjur@Ge(y3`Ze*@*%p4Yv=yL)f5|icS_A# zE9-ifdtezV8lMe2|Ci=gyeT-4K{T5w$lCFvmm!ujVepxcyF2KTAt3Rip}3mUeQzD& z0zGje`zb^Lot6cUW#fZpMZJzynF`pR$j;2CCr&w^oo zJONk8)v3u`y}vcK{`(7kTtr{^5w*sFZ zC;k48mT(#B1(y!`6d7^Ru^oU(;{IeU+QqfG4prU89wg&6>0`gZlCro6+m5B9fb6a{ zqWyC~4TuN}qXh%kUR`2g;eq~?iiVx|mz097#R5dD|Kk|dzqgJ~%EQ(Gx$jvu?k@dx zsNy53cy#b*Tch%OxOdbS{(<~H5zD=!4f1WbyR}>cj!d)7n~pR5FAbdpP~K6frN@7D zebl%-Qx%om_sqwObLTe0eD;@*7WMF@p^v=KOojkKth8nrB$)ZuTUn}`|NJ~G z%sEna>mDd&2OsomG$>{3-&H^%FGyI1M99qMkkLFILi9gXz)DQ3&i+gA=!7RX7B>gP ziaH)_0te{;a^Uky4L)_w>(|I#xN7cB8X%_!8fo3hLn-XAMi>F+JY3jSXkP1-jf9c; zj(%0_>27u5de9Ay9~kHF=sgwmhs5euOU}{JuR9Am2${97YuO^Gct5n6`4Mep=_o$U z(n{1@?OURv(e|l6b`KWV!jB7?W$+_sIN;^ZdYm*wv||kr(DYDVxaw+CesxVEsk(Iw z#_l@1hbnd*tArZDm1vBUoY=+n;in*M&uD?kc$;FdlM>sG*wfh}FMct;9y>S)(_QX)u5pW zrvBrLp1KLm&~q89boF02M>Y`ke0KF@UAQts7cZ1-ED-oeHLNinSA;G=co?Clv^{LF zI{c4S(eB(eb=aj}>cr1nMT#^Z2Ayg(?Fs)h#^mdqfqZJ&%!_FAIX~C>hV@3uwY$eH z2-uE0)y@B(PCk9DhT*wt1h79~UR*nwF#Bw2KZTs!oBU?)Z0?^O+UV`9!mu7{h8^%gkF)6`s=j8C5u}ZKD$lHk&TDvzCSumlXw6F zv4jn}hs#F|oq2ZN=~*}s%k3kIt&L*qe7Xu)-}4YL-M#ZGS_W6CAQKKk zSk|ZlL1~9z-$g|n`@^G#>_2MXH^&gO~ri#4#cta zQD#v?%&^HM9Eqt2pn)%!4;XH5=8ca=rsSQ;3^W6+vb@P02t1N3>g#zmo4r449H3ia z|NOV>@ab__1HezXh#l}N(g4$7TWNhNy9Tiy7Rf!T6J(ZVh(UQ&fcZEz+e=D%L+g@O500&WDh56lm>T>|VRJUd!fCi2d3%mjC)$y)-m$o*AV zkTDn+7=I@L+6WK3g{#Z6)pHTN&>7?75$CE@4l0;QQvCQmkR8l~)*T{|M4*!3-f>v? z2NfHo4Zp!_Sil3#G?#gclh6R8`t8TC8c}Rj6x&@%%}1c~bcA!K-h;Q6k&fF`aI+IY z(hPG5$vl4dXv8BO`7n1)VO{vC6x+!VaM92I!kx-QF(fr|8XL^Qhsa;aD^B8tQ-8FAaudKk_^;q^kfQd2*gB~ zH+w#4Bd(f!)Y}U=cM{Mlub5&Y*4X|kAlNV=7MB4&2_MNCi4|w9b1<4M$UI)pHQ78?ym+lqM|RzCE*(5z0V^gOzx9e{HB%B5xo~otK9gZ$D9@Hf#52y<)Mf!V(M0myY~s@xNwS7CLWgp%0(gR zM=I#+J<)Xi8Vuapoul??=I~Y!>;9d#6c9@>#3qXyPsy>_kN!yXUJ`6On(p>?ynIk# zg#5vu{#zys+<<_Vu+EV)mka`h8l@jlZw;lE3d)3S+oS)BpL|VS5ax}J@WDuCLd9+4M!4iZ}MKIff(emGukVLuJ26P6<>!~0do_0Yv2vwFLRjO zO${HbA{Xu>+x++FH9UZ0d$^4);io1R-e*R%U$RdIEs1w87iP&1JpqA#Q$zdLpzyJXBC{YTZd&Pb1DbB@>u5?wI{1NH4DLJ3 zk`MZ9QpkshGxMGa&9)ZpSOx-fL1;nHQ39uNTv+x~(I0=-=GIBWjdDlgA_v&Psg)qi z?>}#n6IXfhUx^%@IMnQ-_im#t0pgueo{dC3gsfO@TA=R&%bQ9r>_y;l3>e&?zp$_( z&C(jwTh&j~PtJP$nF))FSbNfXtayWJ0FT|UurPTYI8CpDgN9qg?}clW(=s`&9VTZD`ug(!bP>@M109Q$yFZ==}q;HPCb z148B<$kiuQwCYGQE(2y>atop}!U0UC;tshMcv*i=r+|sL&)^3O199myr27Rh%(|((XNIhG|7QxrI~&# zQx3Kma?TE^udFBQyjLJKKe9Fr-w2jJQD~{WGZ%9Q(^vzNYlfZeVGAD)eT57@&k*Kx z&>=8sy^CLD-{mtta?M986I9qWN_nvrfKzDMFK0=tDcZjbOnV86hul~IIM7_Ux(lXz zMA+dykd5OBd$5K0dW&VhK@Rt&I2*5pt7P0XAzcb1W3rzrNK|kAaHgNY=S7+$yTq2j zT=9Sl8+{>R3;$M$Zs#ez|1{-cw9QmqZw%PP6%8&f1ykU7kL8NtMaj%1+H>dpe}l8& z;ptkh)rNdj?A;0z*X8qSY$d-IKNJND?Xq#4J9q95AER^UFNMDh5NyjdXdFsR(3gPJ z8_>U#!v8#xXhQ_|HVg%%)sYrIl(|iO8uy-iGg|Jb7jD+GwrJD{XEuBIIgEyyTFwl9 zwyA-dh9>P28(3PPGy(sd*asoEzksj@5qNf{&B}NU7!gKG26D7&?X9dZsN|>pM)1FE zKp4L}gBhB;8+mkt;ErY<;?E8w5zEco^{D+uE^cqVM=+5B^8Piegn*IRBb9lup2(;4 z(XgXGTXQ|atBwQggEuzw-M{fx0YTD1M~dE`bk&My0J|i(iD=UBC>+mhj~J1427y-1 z>`Pt`uqg6#$ke?n0xE6t>0rDDiCoBz%Q5^=fij0o?`S28-f$0GPg-A#43bHbaCcky zQD)cG&N}(~Ia|=)8oznf=>KN(I?Y?8Bhow{na@)CJxU@AW{_#ASUzKP=vn6ZSW`D_ zdkn8>>HM$JGUT9vk!|y{)BMKt5XpxkcAejnCO2}%AAX?)W$Qxn4G0L3+Wl3Aob