From 6606b5636434c2198ba76221fab58e943eecb253 Mon Sep 17 00:00:00 2001 From: David Sidrane Date: Thu, 29 Jan 2015 04:49:39 -1000 Subject: [PATCH 01/34] Updated NuttX submodule with memcpy fix, disabled run time stack checking and added modules back in --- NuttX | 2 +- makefiles/config_px4fmu-v2_default.mk | 10 +++++----- nuttx-configs/px4fmu-v1/nsh/defconfig | 2 +- nuttx-configs/px4fmu-v2/nsh/defconfig | 2 +- nuttx-configs/px4io-v1/nsh/defconfig | 2 +- nuttx-configs/px4io-v2/nsh/defconfig | 2 +- 6 files changed, 10 insertions(+), 10 deletions(-) diff --git a/NuttX b/NuttX index e4c914e261..e1e56f2254 160000 --- a/NuttX +++ b/NuttX @@ -1 +1 @@ -Subproject commit e4c914e261d2647e44d05222afa7aa3cc90d3c67 +Subproject commit e1e56f2254559c1e71e97d423cb282ec82feece4 diff --git a/makefiles/config_px4fmu-v2_default.mk b/makefiles/config_px4fmu-v2_default.mk index 76457216bd..3abebd82fa 100644 --- a/makefiles/config_px4fmu-v2_default.mk +++ b/makefiles/config_px4fmu-v2_default.mk @@ -37,11 +37,11 @@ MODULES += drivers/hil # MODULES += drivers/hott/hott_sensors # MODULES += drivers/blinkm MODULES += drivers/airspeed -# MODULES += drivers/ets_airspeed +MODULES += drivers/ets_airspeed MODULES += drivers/meas_airspeed -# MODULES += drivers/frsky_telemetry +MODULES += drivers/frsky_telemetry MODULES += modules/sensors -# MODULES += drivers/mkblctrl +MODULES += drivers/mkblctrl MODULES += drivers/px4flow # @@ -70,7 +70,7 @@ MODULES += modules/commander MODULES += modules/navigator MODULES += modules/mavlink MODULES += modules/gpio_led -# MODULES += modules/uavcan +MODULES += modules/uavcan MODULES += modules/land_detector # @@ -120,7 +120,7 @@ MODULES += lib/launchdetection # # OBC challenge # -# MODULES += modules/bottle_drop +MODULES += modules/bottle_drop # # Demo apps diff --git a/nuttx-configs/px4fmu-v1/nsh/defconfig b/nuttx-configs/px4fmu-v1/nsh/defconfig index a467fa605e..539634e3da 100644 --- a/nuttx-configs/px4fmu-v1/nsh/defconfig +++ b/nuttx-configs/px4fmu-v1/nsh/defconfig @@ -92,7 +92,7 @@ CONFIG_ARCH_HAVE_MPU=y # # CONFIG_ARMV7M_TOOLCHAIN_BUILDROOT is not set CONFIG_ARMV7M_TOOLCHAIN_GNU_EABI=y -CONFIG_ARMV7M_STACKCHECK=y +CONFIG_ARMV7M_STACKCHECK=n CONFIG_SERIAL_TERMIOS=y # diff --git a/nuttx-configs/px4fmu-v2/nsh/defconfig b/nuttx-configs/px4fmu-v2/nsh/defconfig index 9030a1f022..dedebdfa03 100644 --- a/nuttx-configs/px4fmu-v2/nsh/defconfig +++ b/nuttx-configs/px4fmu-v2/nsh/defconfig @@ -117,7 +117,7 @@ CONFIG_ARCH_HAVE_MPU=y # # CONFIG_ARMV7M_TOOLCHAIN_BUILDROOT is not set CONFIG_ARMV7M_TOOLCHAIN_GNU_EABI=y -CONFIG_ARMV7M_STACKCHECK=y +CONFIG_ARMV7M_STACKCHECK=n CONFIG_SERIAL_TERMIOS=y CONFIG_SDIO_DMA=y CONFIG_SDIO_DMAPRIO=0x00010000 diff --git a/nuttx-configs/px4io-v1/nsh/defconfig b/nuttx-configs/px4io-v1/nsh/defconfig index 7c76be7ec0..385b8d6a8f 100755 --- a/nuttx-configs/px4io-v1/nsh/defconfig +++ b/nuttx-configs/px4io-v1/nsh/defconfig @@ -93,7 +93,7 @@ CONFIG_ARCH_DMA=y CONFIG_ARCH_MATH_H=y CONFIG_ARMV7M_CMNVECTOR=y -# CONFIG_ARMV7M_STACKCHECK is not set +CONFIG_ARMV7M_STACKCHECK=n # # JTAG Enable settings (by default JTAG-DP and SW-DP are disabled): diff --git a/nuttx-configs/px4io-v2/nsh/defconfig b/nuttx-configs/px4io-v2/nsh/defconfig index 02b51e3d73..a9b8477948 100755 --- a/nuttx-configs/px4io-v2/nsh/defconfig +++ b/nuttx-configs/px4io-v2/nsh/defconfig @@ -89,7 +89,7 @@ CONFIG_ARCH_DMA=y CONFIG_ARCH_MATH_H=y CONFIG_ARMV7M_CMNVECTOR=y - +CONFIG_ARMV7M_STACKCHECK=n # # JTAG Enable settings (by default JTAG-DP and SW-DP are disabled): # From 2c644715002516ebe998bd952aa97baf9b80d64f Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 29 Jan 2015 16:31:21 +0100 Subject: [PATCH 02/34] Updated NuttX submodule --- NuttX | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/NuttX b/NuttX index e1e56f2254..37cbc3e8ae 160000 --- a/NuttX +++ b/NuttX @@ -1 +1 @@ -Subproject commit e1e56f2254559c1e71e97d423cb282ec82feece4 +Subproject commit 37cbc3e8ae6b75e9b7e79996d30fe8ed0beb9704 From 8de411619a0ce05cc9f34f5a9f756908dbd21db8 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 24 Jan 2015 15:50:53 +0100 Subject: [PATCH 03/34] Initial stab at supporting multiple publications on the same base name and auto-enumeration of additional publications. --- ROMFS/px4fmu_test/init.d/rcS | 7 + src/drivers/drv_orb_dev.h | 9 +- src/modules/uORB/Publication.cpp | 2 +- src/modules/uORB/Publication.hpp | 2 +- src/modules/uORB/Subscription.cpp | 2 +- src/modules/uORB/Subscription.hpp | 2 +- src/modules/uORB/module.mk | 5 +- src/modules/uORB/objects_common.cpp | 2 +- src/modules/uORB/uORB.cpp | 270 ++++++++++++++++++++-------- src/modules/uORB/uORB.h | 65 ++++++- 10 files changed, 278 insertions(+), 88 deletions(-) diff --git a/ROMFS/px4fmu_test/init.d/rcS b/ROMFS/px4fmu_test/init.d/rcS index 40b3500e0e..4b9a9b68a0 100644 --- a/ROMFS/px4fmu_test/init.d/rcS +++ b/ROMFS/px4fmu_test/init.d/rcS @@ -97,6 +97,13 @@ else set unit_test_failure_list "${unit_test_failure_list} commander_tests" fi +if uorb test +then +else + set unit_test_failure 1 + set unit_test_failure_list "${unit_test_failure_list} uorb_tests" +fi + if hmc5883 -I start then # This is an FMUv3 diff --git a/src/drivers/drv_orb_dev.h b/src/drivers/drv_orb_dev.h index 713618545d..e0b16fa5cd 100644 --- a/src/drivers/drv_orb_dev.h +++ b/src/drivers/drv_orb_dev.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Copyright (c) 2012-2015 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -35,7 +35,9 @@ #define _DRV_UORB_H /** - * @file uORB published object driver. + * @file drv_orb_dev.h + * + * uORB published object driver. */ #include @@ -84,4 +86,7 @@ /** Get the global advertiser handle for the topic */ #define ORBIOCGADVERTISER _ORBIOC(13) +/** Get the priority for the topic */ +#define ORBIOCGPRIORITY _ORBIOC(14) + #endif /* _DRV_UORB_H */ diff --git a/src/modules/uORB/Publication.cpp b/src/modules/uORB/Publication.cpp index 71757e1f42..d33f930602 100644 --- a/src/modules/uORB/Publication.cpp +++ b/src/modules/uORB/Publication.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Copyright (c) 2012-2015 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions diff --git a/src/modules/uORB/Publication.hpp b/src/modules/uORB/Publication.hpp index 8650b3df89..b64559734c 100644 --- a/src/modules/uORB/Publication.hpp +++ b/src/modules/uORB/Publication.hpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Copyright (c) 2012-2015 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions diff --git a/src/modules/uORB/Subscription.cpp b/src/modules/uORB/Subscription.cpp index 44b6debc7e..8884e5a3a1 100644 --- a/src/modules/uORB/Subscription.cpp +++ b/src/modules/uORB/Subscription.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Copyright (c) 2012-2015 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions diff --git a/src/modules/uORB/Subscription.hpp b/src/modules/uORB/Subscription.hpp index 34e9a83e03..75cf362546 100644 --- a/src/modules/uORB/Subscription.hpp +++ b/src/modules/uORB/Subscription.hpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Copyright (c) 2012-2015 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions diff --git a/src/modules/uORB/module.mk b/src/modules/uORB/module.mk index 9385ce253c..71ad09130c 100644 --- a/src/modules/uORB/module.mk +++ b/src/modules/uORB/module.mk @@ -1,6 +1,6 @@ ############################################################################ # -# Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. +# Copyright (c) 2012-2015 PX4 Development Team. All rights reserved. # # Redistribution and use in source and binary forms, with or without # modification, are permitted provided that the following conditions @@ -37,8 +37,7 @@ MODULE_COMMAND = uorb -# XXX probably excessive, 2048 should be sufficient -MODULE_STACKSIZE = 4096 +MODULE_STACKSIZE = 2048 SRCS = uORB.cpp \ objects_common.cpp \ diff --git a/src/modules/uORB/objects_common.cpp b/src/modules/uORB/objects_common.cpp index 78fdf4de75..20a9bcc43c 100644 --- a/src/modules/uORB/objects_common.cpp +++ b/src/modules/uORB/objects_common.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012-2014 PX4 Development Team. All rights reserved. + * Copyright (c) 2012-2015 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions diff --git a/src/modules/uORB/uORB.cpp b/src/modules/uORB/uORB.cpp index 3373aac837..cfea12f044 100644 --- a/src/modules/uORB/uORB.cpp +++ b/src/modules/uORB/uORB.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (Cc) 2012-2015 PX4 Development Team. All rights reserved. + * Copyright (c) 2012-2015 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -51,6 +51,7 @@ #include #include #include +#include #include #include @@ -68,6 +69,7 @@ namespace { +/* internal use only */ static const unsigned orb_maxpath = 64; /* oddly, ERROR is not defined for c++ */ @@ -81,17 +83,30 @@ enum Flavor { PARAM }; +struct orb_advertdata { + const struct orb_metadata *meta; + int *instance; + int priority; +}; + int -node_mkpath(char *buf, Flavor f, const struct orb_metadata *meta) +node_mkpath(char *buf, Flavor f, const struct orb_metadata *meta, int *instance = nullptr) { unsigned len; - len = snprintf(buf, orb_maxpath, "/%s/%s", - (f == PUBSUB) ? "obj" : "param", - meta->o_name); + unsigned index = 0; - if (len >= orb_maxpath) + if (instance != nullptr) { + index = *instance; + } + + len = snprintf(buf, orb_maxpath, "/%s/%s%d", + (f == PUBSUB) ? "obj" : "param", + meta->o_name, index); + + if (len >= orb_maxpath) { return -ENAMETOOLONG; + } return OK; } @@ -104,7 +119,7 @@ node_mkpath(char *buf, Flavor f, const struct orb_metadata *meta) class ORBDevNode : public device::CDev { public: - ORBDevNode(const struct orb_metadata *meta, const char *name, const char *path); + ORBDevNode(const struct orb_metadata *meta, const char *name, const char *path, int priority); ~ORBDevNode(); virtual int open(struct file *filp); @@ -126,6 +141,7 @@ private: struct hrt_call update_call; /**< deferred wakeup call if update_period is nonzero */ void *poll_priv; /**< saved copy of fds->f_priv while poll is active */ bool update_reported; /**< true if we have reported the update via poll/check */ + int priority; /**< priority of publisher */ }; const struct orb_metadata *_meta; /**< object metadata information */ @@ -133,6 +149,7 @@ private: hrt_abstime _last_update; /**< time the object was last updated */ volatile unsigned _generation; /**< object generation count */ pid_t _publisher; /**< if nonzero, current publisher */ + const int _priority; /**< priority of topic */ SubscriberData *filp_to_sd(struct file *filp) { SubscriberData *sd = (SubscriberData *)(filp->f_priv); @@ -160,13 +177,14 @@ private: bool appears_updated(SubscriberData *sd); }; -ORBDevNode::ORBDevNode(const struct orb_metadata *meta, const char *name, const char *path) : +ORBDevNode::ORBDevNode(const struct orb_metadata *meta, const char *name, const char *path, int priority) : CDev(name, path), _meta(meta), _data(nullptr), _last_update(0), _generation(0), - _publisher(0) + _publisher(0), + _priority(priority) { // enable debug() calls _debug_enabled = true; @@ -176,6 +194,7 @@ ORBDevNode::~ORBDevNode() { if (_data != nullptr) delete[] _data; + } int @@ -225,6 +244,9 @@ ORBDevNode::open(struct file *filp) /* default to no pending update */ sd->generation = _generation; + /* set priority */ + sd->priority = _priority; + filp->f_priv = (void *)sd; ret = CDev::open(filp); @@ -283,6 +305,9 @@ ORBDevNode::read(struct file *filp, char *buffer, size_t buflen) /* track the last generation that the file has seen */ sd->generation = _generation; + /* set priority */ + sd->priority = _priority; + /* * Clear the flag that indicates that an update has been reported, as * we have just collected it. @@ -364,6 +389,10 @@ ORBDevNode::ioctl(struct file *filp, int cmd, unsigned long arg) *(uintptr_t *)arg = (uintptr_t)this; return OK; + case ORBIOCGPRIORITY: + *(int *)arg = sd->priority; + return OK; + default: /* give it to the superclass */ return CDev::ioctl(filp, cmd, arg); @@ -556,40 +585,73 @@ ORBDevMaster::ioctl(struct file *filp, int cmd, unsigned long arg) switch (cmd) { case ORBIOCADVERTISE: { - const struct orb_metadata *meta = (const struct orb_metadata *)arg; + const struct orb_advertdata *adv = (const struct orb_advertdata *)arg; + const struct orb_metadata *meta = adv->meta; const char *objname; + const char *devpath; char nodepath[orb_maxpath]; ORBDevNode *node; /* construct a path to the node - this also checks the node name */ - ret = node_mkpath(nodepath, _flavor, meta); + ret = node_mkpath(nodepath, _flavor, meta, adv->instance); - if (ret != OK) + if (ret != OK) { return ret; + } /* driver wants a permanent copy of the node name, so make one here */ objname = strdup(meta->o_name); - if (objname == nullptr) + if (objname == nullptr) { return -ENOMEM; - - /* construct the new node */ - node = new ORBDevNode(meta, objname, nodepath); - - /* initialise the node - this may fail if e.g. a node with this name already exists */ - if (node != nullptr) - ret = node->init(); - - /* if we didn't get a device, that's bad */ - if (node == nullptr) - return -ENOMEM; - - /* if init failed, discard the node and its name */ - if (ret != OK) { - delete node; - free((void *)objname); } + /* ensure that only one advertiser runs through this critical section */ + lock(); + + ret = ERROR; + + /* try for topic groups */ + const unsigned max_group_tries = (adv->instance != nullptr) ? ORB_MULTI_MAX_INSTANCES : 1; + unsigned group_tries = 0; + do { + /* if path is modifyable change try index */ + if (adv->instance != nullptr) { + /* replace the number at the end of the string */ + nodepath[strlen(nodepath) - 1] = '0' + group_tries; + *(adv->instance) = group_tries; + } + + /* driver wants a permanent copy of the path, so make one here */ + devpath = strdup(nodepath); + + /* construct the new node */ + node = new ORBDevNode(meta, objname, devpath, adv->priority); + + /* if we didn't get a device, that's bad */ + if (node == nullptr) { + unlock(); + return -ENOMEM; + } + + /* initialise the node - this may fail if e.g. a node with this name already exists */ + ret = node->init(); + + /* if init failed, discard the node and its name */ + if (ret != OK) { + delete node; + free((void *)objname); + } + + } while (ret != OK && (group_tries++ < max_group_tries)); + + if (group_tries >= max_group_tries) { + ret = -ENOMEM; + } + + /* the file handle for the driver has been created, unlock */ + unlock(); + return ret; } @@ -614,6 +676,7 @@ struct orb_test { }; ORB_DEFINE(orb_test, struct orb_test); +ORB_DEFINE(orb_multitest, struct orb_test); int test_fail(const char *fmt, ...) @@ -643,8 +706,6 @@ test_note(const char *fmt, ...) return OK; } -ORB_DECLARE(sensor_combined); - int test() { @@ -700,48 +761,65 @@ test() orb_unsubscribe(sfd); close(pfd); -#if 0 - /* this is a hacky test that exploits the sensors app to test rate-limiting */ + /* this routine tests the multi-topic support */ + test_note("try multi-topic support"); - sfd = orb_subscribe(ORB_ID(sensor_combined)); + int instance0; + int pfd0 = orb_advertise_multi(ORB_ID(orb_multitest), &t, &instance0, ORB_PRIO_MAX); - hrt_abstime start, end; - unsigned count; + test_note("advertised"); + usleep(300000); - start = hrt_absolute_time(); - count = 0; + int instance1; + int pfd1 = orb_advertise_multi(ORB_ID(orb_multitest), &t, &instance1, ORB_PRIO_MIN); - do { - orb_check(sfd, &updated); + if (instance0 != 0) + return test_fail("mult. id0: %d", instance0); - if (updated) { - orb_copy(ORB_ID(sensor_combined), sfd, nullptr); - count++; - } - } while (count < 100); + if (instance1 != 1) + return test_fail("mult. id1: %d", instance1); - end = hrt_absolute_time(); - test_note("full-speed, 100 updates in %llu", end - start); + t.val = 103; + if (OK != orb_publish(ORB_ID(orb_multitest), pfd0, &t)) + return test_fail("mult. pub0 fail"); - orb_set_interval(sfd, 10); + test_note("published"); + usleep(300000); - start = hrt_absolute_time(); - count = 0; + t.val = 203; + if (OK != orb_publish(ORB_ID(orb_multitest), pfd1, &t)) + return test_fail("mult. pub1 fail"); - do { - orb_check(sfd, &updated); + /* subscribe to both topics and ensure valid data is received */ + int sfd0 = orb_subscribe_multi(ORB_ID(orb_multitest), 0); - if (updated) { - orb_copy(ORB_ID(sensor_combined), sfd, nullptr); - count++; - } - } while (count < 100); + if (OK != orb_copy(ORB_ID(orb_multitest), sfd0, &t)) + return test_fail("sub #0 copy failed: %d", errno); - end = hrt_absolute_time(); - test_note("100Hz, 100 updates in %llu", end - start); + if (t.val != 103) + return test_fail("sub #0 val. mismatch: %d", t.val); - orb_unsubscribe(sfd); -#endif + int sfd1 = orb_subscribe_multi(ORB_ID(orb_multitest), 1); + + if (OK != orb_copy(ORB_ID(orb_multitest), sfd1, &t)) + return test_fail("sub #1 copy failed: %d", errno); + + if (t.val != 203) + return test_fail("sub #1 val. mismatch: %d", t.val); + + /* test priorities */ + int prio; + if (OK != orb_priority(sfd0, &prio)) + return test_fail("prio #0"); + + if (prio != ORB_PRIO_MAX) + return test_fail("prio: %d", prio); + + if (OK != orb_priority(sfd1, &prio)) + return test_fail("prio #1"); + + if (prio != ORB_PRIO_MIN) + return test_fail("prio: %d", prio); return test_note("PASS"); } @@ -771,7 +849,7 @@ uorb_main(int argc, char *argv[]) if (!strcmp(argv[1], "start")) { if (g_dev != nullptr) { - fprintf(stderr, "[uorb] already loaded\n"); + warnx("already loaded"); /* user wanted to start uorb, its already running, no error */ return 0; } @@ -780,18 +858,17 @@ uorb_main(int argc, char *argv[]) g_dev = new ORBDevMaster(PUBSUB); if (g_dev == nullptr) { - fprintf(stderr, "[uorb] driver alloc failed\n"); + warnx("driver alloc failed"); return -ENOMEM; } if (OK != g_dev->init()) { - fprintf(stderr, "[uorb] driver init failed\n"); + warnx("driver init failed"); delete g_dev; g_dev = nullptr; return -EIO; } - printf("[uorb] ready\n"); return OK; } @@ -807,8 +884,7 @@ uorb_main(int argc, char *argv[]) if (!strcmp(argv[1], "status")) return info(); - fprintf(stderr, "unrecognised command, try 'start', 'test' or 'status'\n"); - return -EINVAL; + errx(-EINVAL, "unrecognized command, try 'start', 'test' or 'status'"); } /* @@ -825,11 +901,14 @@ namespace * we tried to advertise. */ int -node_advertise(const struct orb_metadata *meta) +node_advertise(const struct orb_metadata *meta, int *instance = nullptr, int priority = ORB_PRIO_DEFAULT) { int fd = -1; int ret = ERROR; + /* fill advertiser data */ + const struct orb_advertdata adv = { meta, instance, priority }; + /* open the control device */ fd = open(TOPIC_MASTER_DEVICE_PATH, 0); @@ -837,11 +916,12 @@ node_advertise(const struct orb_metadata *meta) goto out; /* advertise the object */ - ret = ioctl(fd, ORBIOCADVERTISE, (unsigned long)(uintptr_t)meta); + ret = ioctl(fd, ORBIOCADVERTISE, (unsigned long)(uintptr_t)&adv); /* it's OK if it already exists */ - if ((OK != ret) && (EEXIST == errno)) + if ((OK != ret) && (EEXIST == errno)) { ret = OK; + } out: @@ -858,7 +938,7 @@ out: * advertisers. */ int -node_open(Flavor f, const struct orb_metadata *meta, const void *data, bool advertiser) +node_open(Flavor f, const struct orb_metadata *meta, const void *data, bool advertiser, int *instance = nullptr, int priority = ORB_PRIO_DEFAULT) { char path[orb_maxpath]; int fd, ret; @@ -883,7 +963,7 @@ node_open(Flavor f, const struct orb_metadata *meta, const void *data, bool adve /* * Generate the path to the node and try to open it. */ - ret = node_mkpath(path, f, meta); + ret = node_mkpath(path, f, meta, instance); if (ret != OK) { errno = -ret; @@ -893,15 +973,34 @@ node_open(Flavor f, const struct orb_metadata *meta, const void *data, bool adve /* open the path as either the advertiser or the subscriber */ fd = open(path, (advertiser) ? O_WRONLY : O_RDONLY); + /* if we want to advertise and the node existed, we have to re-try again */ + if ((fd >= 0) && (instance != nullptr) && (advertiser)) { + /* close the fd, we want a new one */ + close(fd); + /* the node_advertise call will automatically go for the next free entry */ + fd = -1; + } + /* we may need to advertise the node... */ if (fd < 0) { /* try to create the node */ - ret = node_advertise(meta); + ret = node_advertise(meta, instance, priority); + + if (ret == OK) { + /* update the path, as it might have been updated during the node_advertise call */ + ret = node_mkpath(path, f, meta, instance); + + if (ret != OK) { + errno = -ret; + return ERROR; + } + } /* on success, try the open again */ - if (ret == OK) + if (ret == OK) { fd = open(path, (advertiser) ? O_WRONLY : O_RDONLY); + } } if (fd < 0) { @@ -917,12 +1016,18 @@ node_open(Flavor f, const struct orb_metadata *meta, const void *data, bool adve orb_advert_t orb_advertise(const struct orb_metadata *meta, const void *data) +{ + return orb_advertise_multi(meta, data, nullptr, ORB_PRIO_DEFAULT); +} + +orb_advert_t +orb_advertise_multi(const struct orb_metadata *meta, const void *data, int *instance, int priority) { int result, fd; orb_advert_t advertiser; /* open the node as an advertiser */ - fd = node_open(PUBSUB, meta, data, true); + fd = node_open(PUBSUB, meta, data, true, instance, priority); if (fd == ERROR) return ERROR; @@ -933,7 +1038,7 @@ orb_advertise(const struct orb_metadata *meta, const void *data) return ERROR; /* the advertiser must perform an initial publish to initialise the object */ - result= orb_publish(meta, advertiser, data); + result = orb_publish(meta, advertiser, data); if (result == ERROR) return ERROR; @@ -946,6 +1051,13 @@ orb_subscribe(const struct orb_metadata *meta) return node_open(PUBSUB, meta, nullptr, false); } +int +orb_subscribe_multi(const struct orb_metadata *meta, unsigned instance) +{ + int inst = instance; + return node_open(PUBSUB, meta, nullptr, false, &inst); +} + int orb_unsubscribe(int handle) { @@ -988,6 +1100,12 @@ orb_stat(int handle, uint64_t *time) return ioctl(handle, ORBIOCLASTUPDATE, (unsigned long)(uintptr_t)time); } +int +orb_priority(int handle, int *priority) +{ + return ioctl(handle, ORBIOCGPRIORITY, (unsigned long)(uintptr_t)priority); +} + int orb_set_interval(int handle, unsigned interval) { diff --git a/src/modules/uORB/uORB.h b/src/modules/uORB/uORB.h index beb23f61df..a0ad752735 100644 --- a/src/modules/uORB/uORB.h +++ b/src/modules/uORB/uORB.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Copyright (c) 2012-2015 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -56,6 +56,25 @@ struct orb_metadata { typedef const struct orb_metadata *orb_id_t; +/** + * Maximum number of multi topic instances + */ +#define ORB_MULTI_MAX_INSTANCES 4 + +/** + * Topic priority. + * Relevant for multi-topics / topic groups + */ +enum ORB_PRIO { + ORB_PRIO_MIN = 0, + ORB_PRIO_VERY_LOW = 25, + ORB_PRIO_LOW = 50, + ORB_PRIO_DEFAULT = 75, + ORB_PRIO_HIGH = 100, + ORB_PRIO_VERY_HIGH = 125, + ORB_PRIO_MAX = 255 +}; + /** * Generates a pointer to the uORB metadata structure for * a given topic. @@ -128,7 +147,7 @@ typedef const struct orb_metadata *orb_id_t; #define ORB_DEFINE(_name, _struct) \ const struct orb_metadata __orb_##_name = { \ #_name, \ - sizeof(_struct) \ + sizeof(_struct), \ }; struct hack __BEGIN_DECLS @@ -167,6 +186,34 @@ typedef intptr_t orb_advert_t; */ extern orb_advert_t orb_advertise(const struct orb_metadata *meta, const void *data) __EXPORT; +/** + * Advertise as the publisher of a topic. + * + * This performs the initial advertisement of a topic; it creates the topic + * node in /obj if required and publishes the initial data. + * + * Any number of advertisers may publish to a topic; publications are atomic + * but co-ordination between publishers is not provided by the ORB. + * + * @param meta The uORB metadata (usually from the ORB_ID() macro) + * for the topic. + * @param data A pointer to the initial data to be published. + * For topics updated by interrupt handlers, the advertisement + * must be performed from non-interrupt context. + * @param instance Pointer to an integer which will yield the instance ID (0-based) + * of the publication. + * @param priority The priority of the instance. If a subscriber subscribes multiple + * instances, the priority allows the subscriber to prioritize the best + * data source as long as its available. + * @return ERROR on error, otherwise returns a handle + * that can be used to publish to the topic. + * If the topic in question is not known (due to an + * ORB_DEFINE with no corresponding ORB_DECLARE) + * this function will return -1 and set errno to ENOENT. + */ +extern orb_advert_t orb_advertise_multi(const struct orb_metadata *meta, const void *data, int *instance, int priority) __EXPORT; + + /** * Publish new data to a topic. * @@ -210,6 +257,8 @@ extern int orb_publish(const struct orb_metadata *meta, orb_advert_t handle, con */ extern int orb_subscribe(const struct orb_metadata *meta) __EXPORT; +extern int orb_subscribe_multi(const struct orb_metadata *meta, unsigned instance) __EXPORT; + /** * Unsubscribe from a topic. * @@ -266,6 +315,18 @@ extern int orb_check(int handle, bool *updated) __EXPORT; */ extern int orb_stat(int handle, uint64_t *time) __EXPORT; +/** + * Return the priority of the topic + * + * @param handle A handle returned from orb_subscribe. + * @param priority Returns the priority of this topic. This is only relevant for + * topics which are published by multiple publishers (e.g. mag0, mag1, etc.) + * and allows a subscriber to automatically pick the topic with the highest + * priority, independent of the startup order of the associated publishers. + * @return OK on success, ERROR otherwise with errno set accordingly. + */ +extern int orb_priority(int handle, int *priority) __EXPORT; + /** * Set the minimum interval between which updates are seen for a subscription. * From 7932e2eda238b1d480fdc9de71bb1b5fcaa3e373 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 25 Jan 2015 16:16:15 +0100 Subject: [PATCH 04/34] Add top to test build --- makefiles/config_px4fmu-v2_test.mk | 1 + src/modules/uORB/uORB.cpp | 6 +++++- 2 files changed, 6 insertions(+), 1 deletion(-) diff --git a/makefiles/config_px4fmu-v2_test.mk b/makefiles/config_px4fmu-v2_test.mk index bc6723e5f5..91fc2a7512 100644 --- a/makefiles/config_px4fmu-v2_test.mk +++ b/makefiles/config_px4fmu-v2_test.mk @@ -32,6 +32,7 @@ MODULES += systemcmds/tests MODULES += systemcmds/nshterm MODULES += systemcmds/mtd MODULES += systemcmds/ver +MODULES += systemcmds/top # # Testing modules diff --git a/src/modules/uORB/uORB.cpp b/src/modules/uORB/uORB.cpp index cfea12f044..c4de996bb9 100644 --- a/src/modules/uORB/uORB.cpp +++ b/src/modules/uORB/uORB.cpp @@ -641,9 +641,13 @@ ORBDevMaster::ioctl(struct file *filp, int cmd, unsigned long arg) if (ret != OK) { delete node; free((void *)objname); + free((void *)devpath); } - } while (ret != OK && (group_tries++ < max_group_tries)); + /* try with next larger index */ + group_tries++; + + } while (ret != OK && (group_tries < max_group_tries)); if (group_tries >= max_group_tries) { ret = -ENOMEM; From 4f9a6273cb28e50fdabdf1f60c39da6fe0b4fcd6 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 25 Jan 2015 17:01:39 +0100 Subject: [PATCH 05/34] uORB: correct pub creation for multi-topics --- src/modules/uORB/uORB.cpp | 21 ++++++++++++--------- src/modules/uORB/uORB.h | 2 +- 2 files changed, 13 insertions(+), 10 deletions(-) diff --git a/src/modules/uORB/uORB.cpp b/src/modules/uORB/uORB.cpp index c4de996bb9..b3a9bedb1e 100644 --- a/src/modules/uORB/uORB.cpp +++ b/src/modules/uORB/uORB.cpp @@ -599,13 +599,6 @@ ORBDevMaster::ioctl(struct file *filp, int cmd, unsigned long arg) return ret; } - /* driver wants a permanent copy of the node name, so make one here */ - objname = strdup(meta->o_name); - - if (objname == nullptr) { - return -ENOMEM; - } - /* ensure that only one advertiser runs through this critical section */ lock(); @@ -622,9 +615,20 @@ ORBDevMaster::ioctl(struct file *filp, int cmd, unsigned long arg) *(adv->instance) = group_tries; } + /* driver wants a permanent copy of the node name, so make one here */ + objname = strdup(meta->o_name); + + if (objname == nullptr) { + return -ENOMEM; + } + /* driver wants a permanent copy of the path, so make one here */ devpath = strdup(nodepath); + if (devpath == nullptr) { + return -ENOMEM; + } + /* construct the new node */ node = new ORBDevNode(meta, objname, devpath, adv->priority); @@ -644,12 +648,11 @@ ORBDevMaster::ioctl(struct file *filp, int cmd, unsigned long arg) free((void *)devpath); } - /* try with next larger index */ group_tries++; } while (ret != OK && (group_tries < max_group_tries)); - if (group_tries >= max_group_tries) { + if (group_tries > max_group_tries) { ret = -ENOMEM; } diff --git a/src/modules/uORB/uORB.h b/src/modules/uORB/uORB.h index a0ad752735..30cd598809 100644 --- a/src/modules/uORB/uORB.h +++ b/src/modules/uORB/uORB.h @@ -59,7 +59,7 @@ typedef const struct orb_metadata *orb_id_t; /** * Maximum number of multi topic instances */ -#define ORB_MULTI_MAX_INSTANCES 4 +#define ORB_MULTI_MAX_INSTANCES 3 /** * Topic priority. From 50a58db7e605c61bb50cfb154cbc43bf9c4c67ae Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 25 Jan 2015 17:50:57 +0100 Subject: [PATCH 06/34] uORB: Ensure correct instance initialization, port complete mag API to new interface --- src/modules/uORB/objects_common.cpp | 4 +--- src/modules/uORB/uORB.cpp | 5 +++++ 2 files changed, 6 insertions(+), 3 deletions(-) diff --git a/src/modules/uORB/objects_common.cpp b/src/modules/uORB/objects_common.cpp index 20a9bcc43c..a8305ebeaf 100644 --- a/src/modules/uORB/objects_common.cpp +++ b/src/modules/uORB/objects_common.cpp @@ -46,9 +46,7 @@ #include #include -ORB_DEFINE(sensor_mag0, struct mag_report); -ORB_DEFINE(sensor_mag1, struct mag_report); -ORB_DEFINE(sensor_mag2, struct mag_report); +ORB_DEFINE(sensor_mag, struct mag_report); #include ORB_DEFINE(sensor_accel0, struct accel_report); diff --git a/src/modules/uORB/uORB.cpp b/src/modules/uORB/uORB.cpp index b3a9bedb1e..6f021459ce 100644 --- a/src/modules/uORB/uORB.cpp +++ b/src/modules/uORB/uORB.cpp @@ -592,6 +592,11 @@ ORBDevMaster::ioctl(struct file *filp, int cmd, unsigned long arg) char nodepath[orb_maxpath]; ORBDevNode *node; + /* set instance to zero - we could allow selective multi-pubs later based on value */ + if (adv->instance != nullptr) { + *(adv->instance) = 0; + } + /* construct a path to the node - this also checks the node name */ ret = node_mkpath(nodepath, _flavor, meta, adv->instance); From f30b02272beee4ad5c137a25c726ec158f0135de Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 25 Jan 2015 17:51:33 +0100 Subject: [PATCH 07/34] Move HMC5883 to new interface --- src/drivers/drv_mag.h | 6 ++---- src/drivers/hmc5883/hmc5883.cpp | 35 ++++++++------------------------- 2 files changed, 10 insertions(+), 31 deletions(-) diff --git a/src/drivers/drv_mag.h b/src/drivers/drv_mag.h index d341e89472..d8fe1ae7a3 100644 --- a/src/drivers/drv_mag.h +++ b/src/drivers/drv_mag.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Copyright (c) 2012-2015 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -81,9 +81,7 @@ struct mag_scale { /* * ObjDev tag for raw magnetometer data. */ -ORB_DECLARE(sensor_mag0); -ORB_DECLARE(sensor_mag1); -ORB_DECLARE(sensor_mag2); +ORB_DECLARE(sensor_mag); /* * mag device types, for _device_id diff --git a/src/drivers/hmc5883/hmc5883.cpp b/src/drivers/hmc5883/hmc5883.cpp index a06be72d9a..b1605a5b09 100644 --- a/src/drivers/hmc5883/hmc5883.cpp +++ b/src/drivers/hmc5883/hmc5883.cpp @@ -69,7 +69,6 @@ #include #include -#include #include #include @@ -157,10 +156,9 @@ private: float _range_ga; bool _collect_phase; int _class_instance; + int _orb_class_instance; orb_advert_t _mag_topic; - orb_advert_t _subsystem_pub; - orb_id_t _mag_orb_id; perf_counter_t _sample_perf; perf_counter_t _comms_errors; @@ -348,9 +346,8 @@ HMC5883::HMC5883(device::Device *interface, const char *path, enum Rotation rota _range_ga(1.3f), _collect_phase(false), _class_instance(-1), + _orb_class_instance(-1), _mag_topic(-1), - _subsystem_pub(-1), - _mag_orb_id(nullptr), _sample_perf(perf_alloc(PC_ELAPSED, "hmc5883_read")), _comms_errors(perf_alloc(PC_COUNT, "hmc5883_comms_errors")), _buffer_overflows(perf_alloc(PC_COUNT, "hmc5883_buffer_overflows")), @@ -419,7 +416,6 @@ HMC5883::init() reset(); _class_instance = register_class_devname(MAG_DEVICE_PATH); - _mag_orb_id = ORB_ID_TRIPLE(sensor_mag, _class_instance); ret = OK; /* sensor is ok, but not calibrated */ @@ -850,6 +846,7 @@ HMC5883::collect() perf_begin(_sample_perf); struct mag_report new_report; + bool sensor_is_onboard = false; /* this should be fairly close to the end of the measurement, so the best approximation of the time */ new_report.timestamp = hrt_absolute_time(); @@ -902,7 +899,8 @@ HMC5883::collect() // XXX revisit for SPI part, might require a bus type IOCTL unsigned dummy; - if (!_interface->ioctl(MAGIOCGEXTERNAL, dummy)) { + sensor_is_onboard = !_interface->ioctl(MAGIOCGEXTERNAL, dummy); + if (sensor_is_onboard) { // convert onboard so it matches offboard for the // scaling below report.y = -report.y; @@ -925,9 +923,10 @@ HMC5883::collect() if (_mag_topic != -1) { /* publish it */ - orb_publish(_mag_orb_id, _mag_topic, &new_report); + orb_publish(ORB_ID(sensor_mag), _mag_topic, &new_report); } else { - _mag_topic = orb_advertise(_mag_orb_id, &new_report); + _mag_topic = orb_advertise_multi(ORB_ID(sensor_mag), &new_report, + &_orb_class_instance, (sensor_is_onboard) ? ORB_PRIO_HIGH : ORB_PRIO_MAX); if (_mag_topic < 0) debug("ADVERT FAIL"); @@ -1185,24 +1184,6 @@ int HMC5883::check_calibration() warnx("mag cal status changed %s%s", (scale_valid) ? "" : "scale invalid ", (offset_valid) ? "" : "offset invalid"); _calibrated = (offset_valid && scale_valid); - - - // XXX Change advertisement - - /* notify about state change */ - struct subsystem_info_s info = { - true, - true, - _calibrated, - SUBSYSTEM_TYPE_MAG}; - - if (!(_pub_blocked)) { - if (_subsystem_pub > 0) { - orb_publish(ORB_ID(subsystem_info), _subsystem_pub, &info); - } else { - _subsystem_pub = orb_advertise(ORB_ID(subsystem_info), &info); - } - } } /* return 0 if calibrated, 1 else */ From 114465aba4b65ecdc9ffe3f4125afb2391fbdc2b Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 25 Jan 2015 17:51:50 +0100 Subject: [PATCH 08/34] Move LSM303D mag to new multi-pub interface --- src/drivers/lsm303d/lsm303d.cpp | 22 ++++------------------ 1 file changed, 4 insertions(+), 18 deletions(-) diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp index 57754e4c0d..6b65965b49 100644 --- a/src/drivers/lsm303d/lsm303d.cpp +++ b/src/drivers/lsm303d/lsm303d.cpp @@ -507,7 +507,7 @@ private: LSM303D *_parent; orb_advert_t _mag_topic; - orb_id_t _mag_orb_id; + int _mag_orb_class_instance; int _mag_class_instance; void measure(); @@ -641,21 +641,7 @@ LSM303D::init() _mag_reports->get(&mrp); /* measurement will have generated a report, publish */ - switch (_mag->_mag_class_instance) { - case CLASS_DEVICE_PRIMARY: - _mag->_mag_orb_id = ORB_ID(sensor_mag0); - break; - - case CLASS_DEVICE_SECONDARY: - _mag->_mag_orb_id = ORB_ID(sensor_mag1); - break; - - case CLASS_DEVICE_TERTIARY: - _mag->_mag_orb_id = ORB_ID(sensor_mag2); - break; - } - - _mag->_mag_topic = orb_advertise(_mag->_mag_orb_id, &mrp); + _mag->_mag_topic = orb_advertise_multi(ORB_ID(sensor_mag), &mrp, &_mag->_mag_orb_class_instance, ORB_PRIO_LOW); if (_mag->_mag_topic < 0) { warnx("ADVERT ERR"); @@ -1641,7 +1627,7 @@ LSM303D::mag_measure() if (!(_pub_blocked)) { /* publish it */ - orb_publish(_mag->_mag_orb_id, _mag->_mag_topic, &mag_report); + orb_publish(ORB_ID(sensor_mag), _mag->_mag_topic, &mag_report); } _mag_read++; @@ -1742,7 +1728,7 @@ LSM303D_mag::LSM303D_mag(LSM303D *parent) : CDev("LSM303D_mag", LSM303D_DEVICE_PATH_MAG), _parent(parent), _mag_topic(-1), - _mag_orb_id(nullptr), + _mag_orb_class_instance(-1), _mag_class_instance(-1) { } From 801e9ed4fbc66d0aa359184be9f4ed3899f10096 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 25 Jan 2015 17:52:36 +0100 Subject: [PATCH 09/34] Commander: move sensor interface for mag to new multi-sub --- src/modules/commander/mag_calibration.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/commander/mag_calibration.cpp b/src/modules/commander/mag_calibration.cpp index 53013fdb97..2afb9a4408 100644 --- a/src/modules/commander/mag_calibration.cpp +++ b/src/modules/commander/mag_calibration.cpp @@ -149,7 +149,7 @@ int do_mag_calibration(int mavlink_fd) } if (res == OK) { - int sub_mag = orb_subscribe(ORB_ID(sensor_mag0)); + int sub_mag = orb_subscribe_multi(ORB_ID(sensor_mag), 0); if (sub_mag < 0) { mavlink_log_critical(mavlink_fd, "No mag found, abort"); @@ -179,7 +179,7 @@ int do_mag_calibration(int mavlink_fd) int poll_ret = poll(fds, 1, 1000); if (poll_ret > 0) { - orb_copy(ORB_ID(sensor_mag0), sub_mag, &mag); + orb_copy(ORB_ID(sensor_mag), sub_mag, &mag); x[calibration_counter] = mag.x; y[calibration_counter] = mag.y; From f641a26feee76f4a811fec34834cc818a8e8f76a Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 25 Jan 2015 17:53:04 +0100 Subject: [PATCH 10/34] Move MAVLink to new mag interface --- src/modules/mavlink/mavlink_receiver.cpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index e5a21651d2..5aa623e955 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -1086,10 +1086,11 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg) mag.z = imu.zmag; if (_mag_pub < 0) { - _mag_pub = orb_advertise(ORB_ID(sensor_mag0), &mag); + /* publish to the first mag topic */ + _mag_pub = orb_advertise(ORB_ID(sensor_mag), &mag); } else { - orb_publish(ORB_ID(sensor_mag0), _mag_pub, &mag); + orb_publish(ORB_ID(sensor_mag), _mag_pub, &mag); } } From c3eb10560ba255fdda2454e6044bb4efac35b38f Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 25 Jan 2015 17:53:15 +0100 Subject: [PATCH 11/34] Move sensors to new mag interface --- src/modules/sensors/sensors.cpp | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 793b7c2b65..3f66d7995e 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -1249,7 +1249,7 @@ Sensors::mag_poll(struct sensor_combined_s &raw) if (mag_updated) { struct mag_report mag_report; - orb_copy(ORB_ID(sensor_mag0), _mag_sub, &mag_report); + orb_copy(ORB_ID(sensor_mag), _mag_sub, &mag_report); math::Vector<3> vect(mag_report.x, mag_report.y, mag_report.z); @@ -1278,7 +1278,7 @@ Sensors::mag_poll(struct sensor_combined_s &raw) if (mag_updated) { struct mag_report mag_report; - orb_copy(ORB_ID(sensor_mag1), _mag1_sub, &mag_report); + orb_copy(ORB_ID(sensor_mag), _mag1_sub, &mag_report); raw.magnetometer1_raw[0] = mag_report.x_raw; raw.magnetometer1_raw[1] = mag_report.y_raw; @@ -1292,7 +1292,7 @@ Sensors::mag_poll(struct sensor_combined_s &raw) if (mag_updated) { struct mag_report mag_report; - orb_copy(ORB_ID(sensor_mag2), _mag2_sub, &mag_report); + orb_copy(ORB_ID(sensor_mag), _mag2_sub, &mag_report); raw.magnetometer2_raw[0] = mag_report.x_raw; raw.magnetometer2_raw[1] = mag_report.y_raw; @@ -1945,13 +1945,13 @@ Sensors::task_main() */ _gyro_sub = orb_subscribe(ORB_ID(sensor_gyro0)); _accel_sub = orb_subscribe(ORB_ID(sensor_accel0)); - _mag_sub = orb_subscribe(ORB_ID(sensor_mag0)); + _mag_sub = orb_subscribe_multi(ORB_ID(sensor_mag), 0); _gyro1_sub = orb_subscribe(ORB_ID(sensor_gyro1)); _accel1_sub = orb_subscribe(ORB_ID(sensor_accel1)); - _mag1_sub = orb_subscribe(ORB_ID(sensor_mag1)); + _mag1_sub = orb_subscribe_multi(ORB_ID(sensor_mag), 1); _gyro2_sub = orb_subscribe(ORB_ID(sensor_gyro2)); _accel2_sub = orb_subscribe(ORB_ID(sensor_accel2)); - _mag2_sub = orb_subscribe(ORB_ID(sensor_mag2)); + _mag2_sub = orb_subscribe_multi(ORB_ID(sensor_mag), 2); _rc_sub = orb_subscribe(ORB_ID(input_rc)); _baro_sub = orb_subscribe(ORB_ID(sensor_baro0)); _baro1_sub = orb_subscribe(ORB_ID(sensor_baro1)); From cc7a00b96e658f5d36763ef90ebac7fa813b55af Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 25 Jan 2015 17:54:09 +0100 Subject: [PATCH 12/34] Disable UAVCAN build until sensors use all new-style API and UAVCAN sensors base class can be reworked to use it consistently --- makefiles/config_px4fmu-v2_default.mk | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/makefiles/config_px4fmu-v2_default.mk b/makefiles/config_px4fmu-v2_default.mk index 3abebd82fa..68a65efed1 100644 --- a/makefiles/config_px4fmu-v2_default.mk +++ b/makefiles/config_px4fmu-v2_default.mk @@ -70,7 +70,7 @@ MODULES += modules/commander MODULES += modules/navigator MODULES += modules/mavlink MODULES += modules/gpio_led -MODULES += modules/uavcan +#MODULES += modules/uavcan MODULES += modules/land_detector # From d851a630d874f639e0861dc8411405f83ee23769 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 25 Jan 2015 21:05:38 +0100 Subject: [PATCH 13/34] Move all drivers to multi pub/sub API --- src/drivers/drv_accel.h | 6 ++-- src/drivers/drv_baro.h | 6 ++-- src/drivers/drv_blinkm.h | 2 +- src/drivers/drv_gyro.h | 6 ++-- src/drivers/l3gd20/l3gd20.cpp | 40 ++++++++++----------- src/drivers/lsm303d/lsm303d.cpp | 42 +++++++++++----------- src/drivers/mpu6000/mpu6000.cpp | 63 +++++++++++++-------------------- src/drivers/ms5611/ms5611.cpp | 21 +++++++---- 8 files changed, 87 insertions(+), 99 deletions(-) diff --git a/src/drivers/drv_accel.h b/src/drivers/drv_accel.h index 1f98d966bd..1eca6155b0 100644 --- a/src/drivers/drv_accel.h +++ b/src/drivers/drv_accel.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Copyright (c) 2012-2015 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -81,9 +81,7 @@ struct accel_scale { /* * ObjDev tag for raw accelerometer data. */ -ORB_DECLARE(sensor_accel0); -ORB_DECLARE(sensor_accel1); -ORB_DECLARE(sensor_accel2); +ORB_DECLARE(sensor_accel); /* * ioctl() definitions diff --git a/src/drivers/drv_baro.h b/src/drivers/drv_baro.h index 3e28d3d3d9..3d275d6198 100644 --- a/src/drivers/drv_baro.h +++ b/src/drivers/drv_baro.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Copyright (c) 2012-2015 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -63,9 +63,7 @@ struct baro_report { /* * ObjDev tag for raw barometer data. */ -ORB_DECLARE(sensor_baro0); -ORB_DECLARE(sensor_baro1); -ORB_DECLARE(sensor_baro2); +ORB_DECLARE(sensor_baro); /* * ioctl() definitions diff --git a/src/drivers/drv_blinkm.h b/src/drivers/drv_blinkm.h index 9c278f6c50..b757da5459 100644 --- a/src/drivers/drv_blinkm.h +++ b/src/drivers/drv_blinkm.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Copyright (c) 2012-2015 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions diff --git a/src/drivers/drv_gyro.h b/src/drivers/drv_gyro.h index 41b013a443..5e0334a187 100644 --- a/src/drivers/drv_gyro.h +++ b/src/drivers/drv_gyro.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Copyright (c) 2012-2015 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -81,9 +81,7 @@ struct gyro_scale { /* * ObjDev tag for raw gyro data. */ -ORB_DECLARE(sensor_gyro0); -ORB_DECLARE(sensor_gyro1); -ORB_DECLARE(sensor_gyro2); +ORB_DECLARE(sensor_gyro); /* * ioctl() definitions diff --git a/src/drivers/l3gd20/l3gd20.cpp b/src/drivers/l3gd20/l3gd20.cpp index 08bc1fead8..bd1bd9f86d 100644 --- a/src/drivers/l3gd20/l3gd20.cpp +++ b/src/drivers/l3gd20/l3gd20.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2012-2014 PX4 Development Team. All rights reserved. + * Copyright (c) 2012-2015 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -33,7 +33,7 @@ /** * @file l3gd20.cpp - * Driver for the ST L3GD20 MEMS gyro connected via SPI. + * Driver for the ST L3GD20 MEMS and L3GD20H mems gyros connected via SPI. * * Note: With the exception of the self-test feature, the ST L3G4200D is * also supported by this driver. @@ -179,6 +179,12 @@ static const int ERROR = -1; #define L3GD20_DEFAULT_FILTER_FREQ 30 #define L3GD20_TEMP_OFFSET_CELSIUS 40 +#ifdef PX4_SPI_BUS_EXT +#define EXTERNAL_BUS PX4_SPI_BUS_EXT +#else +#define EXTERNAL_BUS 0 +#endif + #ifndef SENSOR_BOARD_ROTATION_DEFAULT #define SENSOR_BOARD_ROTATION_DEFAULT SENSOR_BOARD_ROTATION_270_DEG #endif @@ -221,7 +227,7 @@ private: float _gyro_range_scale; float _gyro_range_rad_s; orb_advert_t _gyro_topic; - orb_id_t _orb_id; + int _orb_class_instance; int _class_instance; unsigned _current_rate; @@ -273,6 +279,13 @@ private: */ void disable_i2c(); + /** + * Get the internal / external state + * + * @return true if the sensor is not on the main MCU board + */ + bool is_external() { return (_bus == EXTERNAL_BUS); } + /** * Static trampoline from the hrt_call context; because we don't have a * generic hrt wrapper yet. @@ -391,7 +404,7 @@ L3GD20::L3GD20(int bus, const char* path, spi_dev_e device, enum Rotation rotati _gyro_range_scale(0.0f), _gyro_range_rad_s(0.0f), _gyro_topic(-1), - _orb_id(nullptr), + _orb_class_instance(-1), _class_instance(-1), _current_rate(0), _orientation(SENSOR_BOARD_ROTATION_DEFAULT), @@ -456,20 +469,6 @@ L3GD20::init() _class_instance = register_class_devname(GYRO_DEVICE_PATH); - switch (_class_instance) { - case CLASS_DEVICE_PRIMARY: - _orb_id = ORB_ID(sensor_gyro0); - break; - - case CLASS_DEVICE_SECONDARY: - _orb_id = ORB_ID(sensor_gyro1); - break; - - case CLASS_DEVICE_TERTIARY: - _orb_id = ORB_ID(sensor_gyro2); - break; - } - reset(); measure(); @@ -478,7 +477,8 @@ L3GD20::init() struct gyro_report grp; _reports->get(&grp); - _gyro_topic = orb_advertise(_orb_id, &grp); + _gyro_topic = orb_advertise_multi(ORB_ID(sensor_gyro), &grp, + &_orb_class_instance, (is_external()) ? ORB_PRIO_VERY_HIGH : ORB_PRIO_DEFAULT); if (_gyro_topic < 0) { debug("failed to create sensor_gyro publication"); @@ -1050,7 +1050,7 @@ L3GD20::measure() /* publish for subscribers */ if (!(_pub_blocked)) { /* publish it */ - orb_publish(_orb_id, _gyro_topic, &report); + orb_publish(ORB_ID(sensor_gyro), _gyro_topic, &report); } _read++; diff --git a/src/drivers/lsm303d/lsm303d.cpp b/src/drivers/lsm303d/lsm303d.cpp index 6b65965b49..ff7068936a 100644 --- a/src/drivers/lsm303d/lsm303d.cpp +++ b/src/drivers/lsm303d/lsm303d.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved. + * Copyright (c) 2013-2015 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -211,6 +211,12 @@ static const int ERROR = -1; #define LSM303D_ONE_G 9.80665f +#ifdef PX4_SPI_BUS_EXT +#define EXTERNAL_BUS PX4_SPI_BUS_EXT +#else +#define EXTERNAL_BUS 0 +#endif + extern "C" { __EXPORT int lsm303d_main(int argc, char *argv[]); } @@ -275,7 +281,7 @@ private: unsigned _mag_samplerate; orb_advert_t _accel_topic; - orb_id_t _accel_orb_id; + int _accel_orb_class_instance; int _accel_class_instance; unsigned _accel_read; @@ -329,6 +335,13 @@ private: */ void disable_i2c(); + /** + * Get the internal / external state + * + * @return true if the sensor is not on the main MCU board + */ + bool is_external() { return (_bus == EXTERNAL_BUS); } + /** * Static trampoline from the hrt_call context; because we don't have a * generic hrt wrapper yet. @@ -539,7 +552,7 @@ LSM303D::LSM303D(int bus, const char* path, spi_dev_e device, enum Rotation rota _mag_range_scale(0.0f), _mag_samplerate(0), _accel_topic(-1), - _accel_orb_id(nullptr), + _accel_orb_class_instance(-1), _accel_class_instance(-1), _accel_read(0), _mag_read(0), @@ -618,7 +631,6 @@ LSM303D::init() if (_accel_reports == nullptr) goto out; - /* advertise accel topic */ _mag_reports = new RingBuffer(2, sizeof(mag_report)); if (_mag_reports == nullptr) @@ -641,7 +653,8 @@ LSM303D::init() _mag_reports->get(&mrp); /* measurement will have generated a report, publish */ - _mag->_mag_topic = orb_advertise_multi(ORB_ID(sensor_mag), &mrp, &_mag->_mag_orb_class_instance, ORB_PRIO_LOW); + _mag->_mag_topic = orb_advertise_multi(ORB_ID(sensor_mag), &mrp, + &_mag->_mag_orb_class_instance, ORB_PRIO_LOW); if (_mag->_mag_topic < 0) { warnx("ADVERT ERR"); @@ -654,21 +667,8 @@ LSM303D::init() _accel_reports->get(&arp); /* measurement will have generated a report, publish */ - switch (_accel_class_instance) { - case CLASS_DEVICE_PRIMARY: - _accel_orb_id = ORB_ID(sensor_accel0); - break; - - case CLASS_DEVICE_SECONDARY: - _accel_orb_id = ORB_ID(sensor_accel1); - break; - - case CLASS_DEVICE_TERTIARY: - _accel_orb_id = ORB_ID(sensor_accel2); - break; - } - - _accel_topic = orb_advertise(_accel_orb_id, &arp); + _accel_topic = orb_advertise_multi(ORB_ID(sensor_accel), &arp, + &_accel_orb_class_instance, (is_external()) ? ORB_PRIO_VERY_HIGH : ORB_PRIO_DEFAULT); if (_accel_topic < 0) { warnx("ADVERT ERR"); @@ -1556,7 +1556,7 @@ LSM303D::measure() if (!(_pub_blocked)) { /* publish it */ - orb_publish(_accel_orb_id, _accel_topic, &accel_report); + orb_publish(ORB_ID(sensor_accel), _accel_topic, &accel_report); } _accel_read++; diff --git a/src/drivers/mpu6000/mpu6000.cpp b/src/drivers/mpu6000/mpu6000.cpp index 2be7582442..e4e9824902 100644 --- a/src/drivers/mpu6000/mpu6000.cpp +++ b/src/drivers/mpu6000/mpu6000.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2012-2014 PX4 Development Team. All rights reserved. + * Copyright (c) 2012-2015 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -175,6 +175,12 @@ #define MPU6000_ONE_G 9.80665f +#ifdef PX4_SPI_BUS_EXT +#define EXTERNAL_BUS PX4_SPI_BUS_EXT +#else +#define EXTERNAL_BUS 0 +#endif + /* the MPU6000 can only handle high SPI bus speeds on the sensor and interrupt status registers. All other registers have a maximum 1MHz @@ -234,7 +240,7 @@ private: float _accel_range_scale; float _accel_range_m_s2; orb_advert_t _accel_topic; - orb_id_t _accel_orb_id; + int _accel_orb_class_instance; int _accel_class_instance; RingBuffer *_gyro_reports; @@ -360,6 +366,13 @@ private: */ uint16_t swap16(uint16_t val) { return (val >> 8) | (val << 8); } + /** + * Get the internal / external state + * + * @return true if the sensor is not on the main MCU board + */ + bool is_external() { return (_bus == EXTERNAL_BUS); } + /** * Measurement self test * @@ -457,7 +470,7 @@ protected: private: MPU6000 *_parent; orb_advert_t _gyro_topic; - orb_id_t _gyro_orb_id; + int _gyro_orb_class_instance; int _gyro_class_instance; /* do not allow to copy this class due to pointer data members */ @@ -479,7 +492,7 @@ MPU6000::MPU6000(int bus, const char *path_accel, const char *path_gyro, spi_dev _accel_range_scale(0.0f), _accel_range_m_s2(0.0f), _accel_topic(-1), - _accel_orb_id(nullptr), + _accel_orb_class_instance(-1), _accel_class_instance(-1), _gyro_reports(nullptr), _gyro_scale{}, @@ -613,22 +626,8 @@ MPU6000::init() _accel_reports->get(&arp); /* measurement will have generated a report, publish */ - switch (_accel_class_instance) { - case CLASS_DEVICE_PRIMARY: - _accel_orb_id = ORB_ID(sensor_accel0); - break; - - case CLASS_DEVICE_SECONDARY: - _accel_orb_id = ORB_ID(sensor_accel1); - break; - - case CLASS_DEVICE_TERTIARY: - _accel_orb_id = ORB_ID(sensor_accel2); - break; - - } - - _accel_topic = orb_advertise(_accel_orb_id, &arp); + _accel_topic = orb_advertise_multi(ORB_ID(sensor_accel), &arp, + &_accel_orb_class_instance, (is_external()) ? ORB_PRIO_MAX : ORB_PRIO_HIGH); if (_accel_topic < 0) { warnx("ADVERT FAIL"); @@ -639,22 +638,8 @@ MPU6000::init() struct gyro_report grp; _gyro_reports->get(&grp); - switch (_gyro->_gyro_class_instance) { - case CLASS_DEVICE_PRIMARY: - _gyro->_gyro_orb_id = ORB_ID(sensor_gyro0); - break; - - case CLASS_DEVICE_SECONDARY: - _gyro->_gyro_orb_id = ORB_ID(sensor_gyro1); - break; - - case CLASS_DEVICE_TERTIARY: - _gyro->_gyro_orb_id = ORB_ID(sensor_gyro2); - break; - - } - - _gyro->_gyro_topic = orb_advertise(_gyro->_gyro_orb_id, &grp); + _gyro->_gyro_topic = orb_advertise_multi(ORB_ID(sensor_gyro), &grp, + &_gyro->_gyro_orb_class_instance, (is_external()) ? ORB_PRIO_MAX : ORB_PRIO_HIGH); if (_gyro->_gyro_topic < 0) { warnx("ADVERT FAIL"); @@ -1763,12 +1748,12 @@ MPU6000::measure() perf_begin(_controller_latency_perf); perf_begin(_system_latency_perf); /* publish it */ - orb_publish(_accel_orb_id, _accel_topic, &arb); + orb_publish(ORB_ID(sensor_accel), _accel_topic, &arb); } if (!(_pub_blocked)) { /* publish it */ - orb_publish(_gyro->_gyro_orb_id, _gyro->_gyro_topic, &grb); + orb_publish(ORB_ID(sensor_gyro), _gyro->_gyro_topic, &grb); } /* stop measuring */ @@ -1818,7 +1803,7 @@ MPU6000_gyro::MPU6000_gyro(MPU6000 *parent, const char *path) : CDev("MPU6000_gyro", path), _parent(parent), _gyro_topic(-1), - _gyro_orb_id(nullptr), + _gyro_orb_class_instance(-1), _gyro_class_instance(-1) { } diff --git a/src/drivers/ms5611/ms5611.cpp b/src/drivers/ms5611/ms5611.cpp index 0a793cbc97..75b1f65fdb 100644 --- a/src/drivers/ms5611/ms5611.cpp +++ b/src/drivers/ms5611/ms5611.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2012-2014 PX4 Development Team. All rights reserved. + * Copyright (c) 2012-2015 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -57,6 +57,7 @@ #include #include +#include #include #include @@ -134,8 +135,7 @@ protected: unsigned _msl_pressure; /* in Pa */ orb_advert_t _baro_topic; - orb_id_t _orb_id; - + int _orb_class_instance; int _class_instance; perf_counter_t _sample_perf; @@ -171,6 +171,13 @@ protected: */ void cycle(); + /** + * Get the internal / external state + * + * @return true if the sensor is not on the main MCU board + */ + bool is_external() { return (_orb_class_instance == 0); /* XXX put this into the interface class */ } + /** * Static trampoline from the workq context; because we don't have a * generic workq wrapper yet. @@ -210,6 +217,7 @@ MS5611::MS5611(device::Device *interface, ms5611::prom_u &prom_buf, const char* _SENS(0), _msl_pressure(101325), _baro_topic(-1), + _orb_class_instance(-1), _class_instance(-1), _sample_perf(perf_alloc(PC_ELAPSED, "ms5611_read")), _measure_perf(perf_alloc(PC_ELAPSED, "ms5611_measure")), @@ -263,7 +271,6 @@ MS5611::init() /* register alternate interfaces if we have to */ _class_instance = register_class_devname(BARO_DEVICE_PATH); - _orb_id = ORB_ID_TRIPLE(sensor_baro, _class_instance); struct baro_report brp; /* do a first measurement cycle to populate reports with valid data */ @@ -303,7 +310,9 @@ MS5611::init() ret = OK; - _baro_topic = orb_advertise(_orb_id, &brp); + _baro_topic = orb_advertise_multi(ORB_ID(sensor_baro), &brp, + &_orb_class_instance, (is_external()) ? ORB_PRIO_HIGH : ORB_PRIO_DEFAULT); + if (_baro_topic < 0) { warnx("failed to create sensor_baro publication"); @@ -725,7 +734,7 @@ MS5611::collect() /* publish it */ if (!(_pub_blocked)) { /* publish it */ - orb_publish(_orb_id, _baro_topic, &report); + orb_publish(ORB_ID(sensor_baro), _baro_topic, &report); } if (_reports->force(&report)) { From 7f299ea0cc5aedbf1f7913930d592bdc696e0ca9 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 25 Jan 2015 21:06:03 +0100 Subject: [PATCH 14/34] Move commander to multi pub/sub API --- src/modules/commander/gyro_calibration.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/commander/gyro_calibration.cpp b/src/modules/commander/gyro_calibration.cpp index 8ab14dd52b..2be0e881ee 100644 --- a/src/modules/commander/gyro_calibration.cpp +++ b/src/modules/commander/gyro_calibration.cpp @@ -95,7 +95,7 @@ int do_gyro_calibration(int mavlink_fd) unsigned poll_errcount = 0; /* subscribe to gyro sensor topic */ - int sub_sensor_gyro = orb_subscribe(ORB_ID(sensor_gyro0)); + int sub_sensor_gyro = orb_subscribe_multi(ORB_ID(sensor_gyro), 0); struct gyro_report gyro_report; while (calibration_counter < calibration_count) { @@ -107,7 +107,7 @@ int do_gyro_calibration(int mavlink_fd) int poll_ret = poll(fds, 1, 1000); if (poll_ret > 0) { - orb_copy(ORB_ID(sensor_gyro0), sub_sensor_gyro, &gyro_report); + orb_copy(ORB_ID(sensor_gyro), sub_sensor_gyro, &gyro_report); gyro_scale.x_offset += gyro_report.x; gyro_scale.y_offset += gyro_report.y; gyro_scale.z_offset += gyro_report.z; From 777eda1a89222b9b7b91fd12c92475291ed56ce7 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 25 Jan 2015 21:06:27 +0100 Subject: [PATCH 15/34] Move MATLAB example to multi pub/sub API --- .../matlab_csv_serial/matlab_csv_serial.c | 18 +++++++++--------- 1 file changed, 9 insertions(+), 9 deletions(-) diff --git a/src/examples/matlab_csv_serial/matlab_csv_serial.c b/src/examples/matlab_csv_serial/matlab_csv_serial.c index a95f45d1ad..145cf99cc4 100644 --- a/src/examples/matlab_csv_serial/matlab_csv_serial.c +++ b/src/examples/matlab_csv_serial/matlab_csv_serial.c @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2014 PX4 Development Team. All rights reserved. + * Copyright (c) 2014-2015 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -190,10 +190,10 @@ int matlab_csv_serial_thread_main(int argc, char *argv[]) struct gyro_report gyro1; /* subscribe to parameter changes */ - int accel0_sub = orb_subscribe(ORB_ID(sensor_accel0)); - int accel1_sub = orb_subscribe(ORB_ID(sensor_accel1)); - int gyro0_sub = orb_subscribe(ORB_ID(sensor_gyro0)); - int gyro1_sub = orb_subscribe(ORB_ID(sensor_gyro1)); + int accel0_sub = orb_subscribe_multi(ORB_ID(sensor_accel), 0); + int accel1_sub = orb_subscribe_multi(ORB_ID(sensor_accel), 1); + int gyro0_sub = orb_subscribe_multi(ORB_ID(sensor_gyro), 0); + int gyro1_sub = orb_subscribe_multi(ORB_ID(sensor_gyro), 1); thread_running = true; @@ -224,10 +224,10 @@ int matlab_csv_serial_thread_main(int argc, char *argv[]) /* accel0 update available? */ if (fds[0].revents & POLLIN) { - orb_copy(ORB_ID(sensor_accel0), accel0_sub, &accel0); - orb_copy(ORB_ID(sensor_accel1), accel1_sub, &accel1); - orb_copy(ORB_ID(sensor_gyro0), gyro0_sub, &gyro0); - orb_copy(ORB_ID(sensor_gyro1), gyro1_sub, &gyro1); + orb_copy(ORB_ID(sensor_accel), accel0_sub, &accel0); + orb_copy(ORB_ID(sensor_accel), accel1_sub, &accel1); + orb_copy(ORB_ID(sensor_gyro), gyro0_sub, &gyro0); + orb_copy(ORB_ID(sensor_gyro), gyro1_sub, &gyro1); // write out on accel 0, but collect for all other sensors as they have updates dprintf(serial_fd, "%llu,%d,%d,%d,%d,%d,%d\n", accel0.timestamp, (int)accel0.x_raw, (int)accel0.y_raw, (int)accel0.z_raw, From 83a0f8e5ce2041494f57643246a612f2cd836752 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 25 Jan 2015 21:06:48 +0100 Subject: [PATCH 16/34] Move EKF to multi pub/sub API --- .../ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp index 923aa28614..d0f5fb6f8a 100644 --- a/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp @@ -719,7 +719,7 @@ FixedwingEstimator::task_main() * do subscriptions */ _distance_sub = orb_subscribe(ORB_ID(sensor_range_finder)); - _baro_sub = orb_subscribe(ORB_ID(sensor_baro0)); + _baro_sub = orb_subscribe_multi(ORB_ID(sensor_baro), 0); _airspeed_sub = orb_subscribe(ORB_ID(airspeed)); _gps_sub = orb_subscribe(ORB_ID(vehicle_gps_position)); _vstatus_sub = orb_subscribe(ORB_ID(vehicle_status)); @@ -1087,7 +1087,7 @@ FixedwingEstimator::task_main() if (baro_updated) { - orb_copy(ORB_ID(sensor_baro0), _baro_sub, &_baro); + orb_copy(ORB_ID(sensor_baro), _baro_sub, &_baro); float baro_elapsed = (_baro.timestamp - baro_last) / 1e6f; baro_last = _baro.timestamp; @@ -1216,7 +1216,7 @@ FixedwingEstimator::task_main() initVelNED[2] = _gps.vel_d_m_s; // Set up height correctly - orb_copy(ORB_ID(sensor_baro0), _baro_sub, &_baro); + orb_copy(ORB_ID(sensor_baro), _baro_sub, &_baro); _baro_ref_offset = _ekf->states[9]; // this should become zero in the local frame // init filtered gps and baro altitudes From c4cf28fa9529ed12a1c78f0e297863f34644167a Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 25 Jan 2015 21:07:06 +0100 Subject: [PATCH 17/34] Move FW att control to multi pub sub API --- src/modules/fw_att_control/fw_att_control_main.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/fw_att_control/fw_att_control_main.cpp b/src/modules/fw_att_control/fw_att_control_main.cpp index 6f225bb48f..4cb6c1aef2 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -549,7 +549,7 @@ FixedwingAttitudeControl::vehicle_accel_poll() orb_check(_accel_sub, &accel_updated); if (accel_updated) { - orb_copy(ORB_ID(sensor_accel0), _accel_sub, &_accel); + orb_copy(ORB_ID(sensor_accel), _accel_sub, &_accel); } } @@ -619,7 +619,7 @@ FixedwingAttitudeControl::task_main() */ _att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint)); _att_sub = orb_subscribe(ORB_ID(vehicle_attitude)); - _accel_sub = orb_subscribe(ORB_ID(sensor_accel0)); + _accel_sub = orb_subscribe_multi(ORB_ID(sensor_accel), 0); _airspeed_sub = orb_subscribe(ORB_ID(airspeed)); _vcontrol_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode)); _params_sub = orb_subscribe(ORB_ID(parameter_update)); From 165e5f5a62eedf1a4776b921b50fb84a3acdd3db Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 25 Jan 2015 21:07:31 +0100 Subject: [PATCH 18/34] Move MAVLink to multi pub/sub API (to first index) --- src/modules/mavlink/mavlink_receiver.cpp | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 5aa623e955..cbd24f0d4a 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -1043,10 +1043,10 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg) gyro.temperature = imu.temperature; if (_gyro_pub < 0) { - _gyro_pub = orb_advertise(ORB_ID(sensor_gyro0), &gyro); + _gyro_pub = orb_advertise(ORB_ID(sensor_gyro), &gyro); } else { - orb_publish(ORB_ID(sensor_gyro0), _gyro_pub, &gyro); + orb_publish(ORB_ID(sensor_gyro), _gyro_pub, &gyro); } } @@ -1065,10 +1065,10 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg) accel.temperature = imu.temperature; if (_accel_pub < 0) { - _accel_pub = orb_advertise(ORB_ID(sensor_accel0), &accel); + _accel_pub = orb_advertise(ORB_ID(sensor_accel), &accel); } else { - orb_publish(ORB_ID(sensor_accel0), _accel_pub, &accel); + orb_publish(ORB_ID(sensor_accel), _accel_pub, &accel); } } @@ -1105,10 +1105,10 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg) baro.temperature = imu.temperature; if (_baro_pub < 0) { - _baro_pub = orb_advertise(ORB_ID(sensor_baro0), &baro); + _baro_pub = orb_advertise(ORB_ID(sensor_baro), &baro); } else { - orb_publish(ORB_ID(sensor_baro0), _baro_pub, &baro); + orb_publish(ORB_ID(sensor_baro), _baro_pub, &baro); } } @@ -1395,10 +1395,10 @@ MavlinkReceiver::handle_message_hil_state_quaternion(mavlink_message_t *msg) accel.temperature = 25.0f; if (_accel_pub < 0) { - _accel_pub = orb_advertise(ORB_ID(sensor_accel0), &accel); + _accel_pub = orb_advertise(ORB_ID(sensor_accel), &accel); } else { - orb_publish(ORB_ID(sensor_accel0), _accel_pub, &accel); + orb_publish(ORB_ID(sensor_accel), _accel_pub, &accel); } } From 9190d597912d90b6d5fbf7a606f8e9d083797534 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 25 Jan 2015 21:08:07 +0100 Subject: [PATCH 19/34] Move sensors app to multi pub/sub --- src/modules/sensors/sensors.cpp | 32 ++++++++++++++++---------------- 1 file changed, 16 insertions(+), 16 deletions(-) diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 3f66d7995e..630c54335a 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -1113,7 +1113,7 @@ Sensors::accel_poll(struct sensor_combined_s &raw) if (accel_updated) { struct accel_report accel_report; - orb_copy(ORB_ID(sensor_accel0), _accel_sub, &accel_report); + orb_copy(ORB_ID(sensor_accel), _accel_sub, &accel_report); math::Vector<3> vect(accel_report.x, accel_report.y, accel_report.z); vect = _board_rotation * vect; @@ -1134,7 +1134,7 @@ Sensors::accel_poll(struct sensor_combined_s &raw) if (accel_updated) { struct accel_report accel_report; - orb_copy(ORB_ID(sensor_accel1), _accel_sub, &accel_report); + orb_copy(ORB_ID(sensor_accel), _accel1_sub, &accel_report); math::Vector<3> vect(accel_report.x, accel_report.y, accel_report.z); vect = _board_rotation * vect; @@ -1155,7 +1155,7 @@ Sensors::accel_poll(struct sensor_combined_s &raw) if (accel_updated) { struct accel_report accel_report; - orb_copy(ORB_ID(sensor_accel2), _accel_sub, &accel_report); + orb_copy(ORB_ID(sensor_accel), _accel2_sub, &accel_report); math::Vector<3> vect(accel_report.x, accel_report.y, accel_report.z); vect = _board_rotation * vect; @@ -1181,7 +1181,7 @@ Sensors::gyro_poll(struct sensor_combined_s &raw) if (gyro_updated) { struct gyro_report gyro_report; - orb_copy(ORB_ID(sensor_gyro0), _gyro_sub, &gyro_report); + orb_copy(ORB_ID(sensor_gyro), _gyro_sub, &gyro_report); math::Vector<3> vect(gyro_report.x, gyro_report.y, gyro_report.z); vect = _board_rotation * vect; @@ -1202,7 +1202,7 @@ Sensors::gyro_poll(struct sensor_combined_s &raw) if (gyro_updated) { struct gyro_report gyro_report; - orb_copy(ORB_ID(sensor_gyro1), _gyro1_sub, &gyro_report); + orb_copy(ORB_ID(sensor_gyro), _gyro1_sub, &gyro_report); math::Vector<3> vect(gyro_report.x, gyro_report.y, gyro_report.z); vect = _board_rotation * vect; @@ -1223,7 +1223,7 @@ Sensors::gyro_poll(struct sensor_combined_s &raw) if (gyro_updated) { struct gyro_report gyro_report; - orb_copy(ORB_ID(sensor_gyro2), _gyro_sub, &gyro_report); + orb_copy(ORB_ID(sensor_gyro), _gyro2_sub, &gyro_report); math::Vector<3> vect(gyro_report.x, gyro_report.y, gyro_report.z); vect = _board_rotation * vect; @@ -1310,7 +1310,7 @@ Sensors::baro_poll(struct sensor_combined_s &raw) if (baro_updated) { - orb_copy(ORB_ID(sensor_baro0), _baro_sub, &_barometer); + orb_copy(ORB_ID(sensor_baro), _baro_sub, &_barometer); raw.baro_pres_mbar = _barometer.pressure; // Pressure in mbar raw.baro_alt_meter = _barometer.altitude; // Altitude in meters @@ -1325,7 +1325,7 @@ Sensors::baro_poll(struct sensor_combined_s &raw) struct baro_report baro_report; - orb_copy(ORB_ID(sensor_baro1), _baro1_sub, &baro_report); + orb_copy(ORB_ID(sensor_baro), _baro1_sub, &baro_report); raw.baro1_pres_mbar = baro_report.pressure; // Pressure in mbar raw.baro1_alt_meter = baro_report.altitude; // Altitude in meters @@ -1943,18 +1943,18 @@ Sensors::task_main() /* * do subscriptions */ - _gyro_sub = orb_subscribe(ORB_ID(sensor_gyro0)); - _accel_sub = orb_subscribe(ORB_ID(sensor_accel0)); + _gyro_sub = orb_subscribe_multi(ORB_ID(sensor_gyro), 0); + _accel_sub = orb_subscribe_multi(ORB_ID(sensor_accel), 0); _mag_sub = orb_subscribe_multi(ORB_ID(sensor_mag), 0); - _gyro1_sub = orb_subscribe(ORB_ID(sensor_gyro1)); - _accel1_sub = orb_subscribe(ORB_ID(sensor_accel1)); + _gyro1_sub = orb_subscribe_multi(ORB_ID(sensor_gyro), 1); + _accel1_sub = orb_subscribe_multi(ORB_ID(sensor_accel), 1); _mag1_sub = orb_subscribe_multi(ORB_ID(sensor_mag), 1); - _gyro2_sub = orb_subscribe(ORB_ID(sensor_gyro2)); - _accel2_sub = orb_subscribe(ORB_ID(sensor_accel2)); + _gyro2_sub = orb_subscribe_multi(ORB_ID(sensor_gyro), 2); + _accel2_sub = orb_subscribe_multi(ORB_ID(sensor_accel), 2); _mag2_sub = orb_subscribe_multi(ORB_ID(sensor_mag), 2); _rc_sub = orb_subscribe(ORB_ID(input_rc)); - _baro_sub = orb_subscribe(ORB_ID(sensor_baro0)); - _baro1_sub = orb_subscribe(ORB_ID(sensor_baro1)); + _baro_sub = orb_subscribe_multi(ORB_ID(sensor_baro), 0); + _baro1_sub = orb_subscribe_multi(ORB_ID(sensor_baro), 1); _diff_pres_sub = orb_subscribe(ORB_ID(differential_pressure)); _vcontrol_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode)); _params_sub = orb_subscribe(ORB_ID(parameter_update)); From 95462c5b7c357b343d39da3c5e6a49ae2055264c Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 25 Jan 2015 21:08:22 +0100 Subject: [PATCH 20/34] uORB: Remove duplicate topics --- src/modules/uORB/objects_common.cpp | 12 +++--------- 1 file changed, 3 insertions(+), 9 deletions(-) diff --git a/src/modules/uORB/objects_common.cpp b/src/modules/uORB/objects_common.cpp index a8305ebeaf..4a41aa4655 100644 --- a/src/modules/uORB/objects_common.cpp +++ b/src/modules/uORB/objects_common.cpp @@ -49,19 +49,13 @@ ORB_DEFINE(sensor_mag, struct mag_report); #include -ORB_DEFINE(sensor_accel0, struct accel_report); -ORB_DEFINE(sensor_accel1, struct accel_report); -ORB_DEFINE(sensor_accel2, struct accel_report); +ORB_DEFINE(sensor_accel, struct accel_report); #include -ORB_DEFINE(sensor_gyro0, struct gyro_report); -ORB_DEFINE(sensor_gyro1, struct gyro_report); -ORB_DEFINE(sensor_gyro2, struct gyro_report); +ORB_DEFINE(sensor_gyro, struct gyro_report); #include -ORB_DEFINE(sensor_baro0, struct baro_report); -ORB_DEFINE(sensor_baro1, struct baro_report); -ORB_DEFINE(sensor_baro2, struct baro_report); +ORB_DEFINE(sensor_baro, struct baro_report); #include ORB_DEFINE(sensor_range_finder, struct range_finder_report); From 1cc4c808a88787b4e1156896453fa9369841bcde Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 25 Jan 2015 21:56:56 +0100 Subject: [PATCH 21/34] Upgrade UAVCAN to multi pub/sub A API --- makefiles/config_px4fmu-v2_default.mk | 2 +- src/modules/uavcan/module.mk | 2 +- src/modules/uavcan/sensors/baro.cpp | 9 ++------- src/modules/uavcan/sensors/baro.hpp | 2 +- src/modules/uavcan/sensors/gnss.cpp | 2 +- src/modules/uavcan/sensors/gnss.hpp | 2 +- src/modules/uavcan/sensors/mag.cpp | 10 ++-------- src/modules/uavcan/sensors/mag.hpp | 2 +- src/modules/uavcan/sensors/sensor_bridge.cpp | 6 ++---- src/modules/uavcan/sensors/sensor_bridge.hpp | 11 +++++------ 10 files changed, 17 insertions(+), 31 deletions(-) diff --git a/makefiles/config_px4fmu-v2_default.mk b/makefiles/config_px4fmu-v2_default.mk index 68a65efed1..3abebd82fa 100644 --- a/makefiles/config_px4fmu-v2_default.mk +++ b/makefiles/config_px4fmu-v2_default.mk @@ -70,7 +70,7 @@ MODULES += modules/commander MODULES += modules/navigator MODULES += modules/mavlink MODULES += modules/gpio_led -#MODULES += modules/uavcan +MODULES += modules/uavcan MODULES += modules/land_detector # diff --git a/src/modules/uavcan/module.mk b/src/modules/uavcan/module.mk index 600cb47f39..4f63629a0c 100644 --- a/src/modules/uavcan/module.mk +++ b/src/modules/uavcan/module.mk @@ -1,6 +1,6 @@ ############################################################################ # -# Copyright (C) 2013 PX4 Development Team. All rights reserved. +# Copyright (c) 2013-2015 PX4 Development Team. All rights reserved. # Author: Pavel Kirienko # # Redistribution and use in source and binary forms, with or without diff --git a/src/modules/uavcan/sensors/baro.cpp b/src/modules/uavcan/sensors/baro.cpp index 8741ae20dd..ad09dfcac4 100644 --- a/src/modules/uavcan/sensors/baro.cpp +++ b/src/modules/uavcan/sensors/baro.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2014 PX4 Development Team. All rights reserved. + * Copyright (c) 2014, 2015 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -38,15 +38,10 @@ #include "baro.hpp" #include -static const orb_id_t BARO_TOPICS[2] = { - ORB_ID(sensor_baro0), - ORB_ID(sensor_baro1) -}; - const char *const UavcanBarometerBridge::NAME = "baro"; UavcanBarometerBridge::UavcanBarometerBridge(uavcan::INode& node) : -UavcanCDevSensorBridgeBase("uavcan_baro", "/dev/uavcan/baro", BARO_DEVICE_PATH, BARO_TOPICS), +UavcanCDevSensorBridgeBase("uavcan_baro", "/dev/uavcan/baro", BARO_DEVICE_PATH, ORB_ID(sensor_baro)), _sub_air_data(node) { } diff --git a/src/modules/uavcan/sensors/baro.hpp b/src/modules/uavcan/sensors/baro.hpp index 9d470219ed..c7bbc5af8d 100644 --- a/src/modules/uavcan/sensors/baro.hpp +++ b/src/modules/uavcan/sensors/baro.hpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2014 PX4 Development Team. All rights reserved. + * Copyright (c) 2014, 2015 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions diff --git a/src/modules/uavcan/sensors/gnss.cpp b/src/modules/uavcan/sensors/gnss.cpp index 571a6f1cd7..3ae07367fa 100644 --- a/src/modules/uavcan/sensors/gnss.cpp +++ b/src/modules/uavcan/sensors/gnss.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2014 PX4 Development Team. All rights reserved. + * Copyright (c) 2014, 2015 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions diff --git a/src/modules/uavcan/sensors/gnss.hpp b/src/modules/uavcan/sensors/gnss.hpp index 2111ff80b6..96ff9404f5 100644 --- a/src/modules/uavcan/sensors/gnss.hpp +++ b/src/modules/uavcan/sensors/gnss.hpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2014 PX4 Development Team. All rights reserved. + * Copyright (c) 2014, 2015 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions diff --git a/src/modules/uavcan/sensors/mag.cpp b/src/modules/uavcan/sensors/mag.cpp index 35ebee5426..ee278aaf53 100644 --- a/src/modules/uavcan/sensors/mag.cpp +++ b/src/modules/uavcan/sensors/mag.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2014 PX4 Development Team. All rights reserved. + * Copyright (c) 2014, 2015 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -39,16 +39,10 @@ #include -static const orb_id_t MAG_TOPICS[3] = { - ORB_ID(sensor_mag0), - ORB_ID(sensor_mag1), - ORB_ID(sensor_mag2) -}; - const char *const UavcanMagnetometerBridge::NAME = "mag"; UavcanMagnetometerBridge::UavcanMagnetometerBridge(uavcan::INode& node) : -UavcanCDevSensorBridgeBase("uavcan_mag", "/dev/uavcan/mag", MAG_DEVICE_PATH, MAG_TOPICS), +UavcanCDevSensorBridgeBase("uavcan_mag", "/dev/uavcan/mag", MAG_DEVICE_PATH, ORB_ID(sensor_mag)), _sub_mag(node) { _device_id.devid_s.devtype = DRV_MAG_DEVTYPE_HMC5883; diff --git a/src/modules/uavcan/sensors/mag.hpp b/src/modules/uavcan/sensors/mag.hpp index 74077d883e..db38aee1d3 100644 --- a/src/modules/uavcan/sensors/mag.hpp +++ b/src/modules/uavcan/sensors/mag.hpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2014 PX4 Development Team. All rights reserved. + * Copyright (c) 2014, 2015 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions diff --git a/src/modules/uavcan/sensors/sensor_bridge.cpp b/src/modules/uavcan/sensors/sensor_bridge.cpp index 0999938fc3..b370764440 100644 --- a/src/modules/uavcan/sensors/sensor_bridge.cpp +++ b/src/modules/uavcan/sensors/sensor_bridge.cpp @@ -62,7 +62,6 @@ UavcanCDevSensorBridgeBase::~UavcanCDevSensorBridgeBase() (void)unregister_class_devname(_class_devname, _channels[i].class_instance); } } - delete [] _orb_topics; delete [] _channels; } @@ -116,11 +115,10 @@ void UavcanCDevSensorBridgeBase::publish(const int node_id, const void *report) } // Publish to the appropriate topic, abort on failure - channel->orb_id = _orb_topics[class_instance]; channel->node_id = node_id; channel->class_instance = class_instance; - channel->orb_advert = orb_advertise(channel->orb_id, report); + channel->orb_advert = orb_advertise_multi(_orb_topic, report, &channel->orb_instance, ORB_PRIO_HIGH); if (channel->orb_advert < 0) { log("ADVERTISE FAILED"); (void)unregister_class_devname(_class_devname, class_instance); @@ -132,7 +130,7 @@ void UavcanCDevSensorBridgeBase::publish(const int node_id, const void *report) } assert(channel != nullptr); - (void)orb_publish(channel->orb_id, channel->orb_advert, report); + (void)orb_publish(_orb_topic, channel->orb_advert, report); } unsigned UavcanCDevSensorBridgeBase::get_num_redundant_channels() const diff --git a/src/modules/uavcan/sensors/sensor_bridge.hpp b/src/modules/uavcan/sensors/sensor_bridge.hpp index e319605370..94e52dbe50 100644 --- a/src/modules/uavcan/sensors/sensor_bridge.hpp +++ b/src/modules/uavcan/sensors/sensor_bridge.hpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2014 PX4 Development Team. All rights reserved. + * Copyright (c) 2014, 2015 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -90,28 +90,27 @@ class UavcanCDevSensorBridgeBase : public IUavcanSensorBridge, public device::CD struct Channel { int node_id = -1; - orb_id_t orb_id = nullptr; orb_advert_t orb_advert = -1; int class_instance = -1; + int orb_instance = -1; }; const unsigned _max_channels; const char *const _class_devname; - orb_id_t *const _orb_topics; + const orb_id_t _orb_topic; Channel *const _channels; bool _out_of_channels = false; protected: template UavcanCDevSensorBridgeBase(const char *name, const char *devname, const char *class_devname, - const orb_id_t (&orb_topics)[MaxChannels]) : + const orb_id_t orb_topic_sensor) : device::CDev(name, devname), _max_channels(MaxChannels), _class_devname(class_devname), - _orb_topics(new orb_id_t[MaxChannels]), + _orb_topic(orb_topic_sensor), _channels(new Channel[MaxChannels]) { - memcpy(_orb_topics, orb_topics, sizeof(orb_id_t) * MaxChannels); _device_id.devid_s.bus_type = DeviceBusType_UAVCAN; _device_id.devid_s.bus = 0; } From 2f7a9eaf6553d7da6c5d6e9b3edf6e710b4dc292 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Mon, 26 Jan 2015 02:34:03 +0300 Subject: [PATCH 22/34] Fix for a compilation failure --- src/modules/uavcan/sensors/sensor_bridge.hpp | 13 +++++++------ 1 file changed, 7 insertions(+), 6 deletions(-) diff --git a/src/modules/uavcan/sensors/sensor_bridge.hpp b/src/modules/uavcan/sensors/sensor_bridge.hpp index 94e52dbe50..de130b0788 100644 --- a/src/modules/uavcan/sensors/sensor_bridge.hpp +++ b/src/modules/uavcan/sensors/sensor_bridge.hpp @@ -75,8 +75,7 @@ public: /** * Sensor bridge factory. - * Creates a bridge object by its ASCII name, e.g. "gnss", "mag". - * @return nullptr if such bridge can't be created. + * Creates all known sensor bridges and puts them in the linked list. */ static void make_all(uavcan::INode &node, List &list); }; @@ -102,14 +101,16 @@ class UavcanCDevSensorBridgeBase : public IUavcanSensorBridge, public device::CD bool _out_of_channels = false; protected: - template + static constexpr unsigned DEFAULT_MAX_CHANNELS = 5; // 640 KB ought to be enough for anybody + UavcanCDevSensorBridgeBase(const char *name, const char *devname, const char *class_devname, - const orb_id_t orb_topic_sensor) : + const orb_id_t orb_topic_sensor, + const unsigned max_channels = DEFAULT_MAX_CHANNELS) : device::CDev(name, devname), - _max_channels(MaxChannels), + _max_channels(max_channels), _class_devname(class_devname), _orb_topic(orb_topic_sensor), - _channels(new Channel[MaxChannels]) + _channels(new Channel[max_channels]) { _device_id.devid_s.bus_type = DeviceBusType_UAVCAN; _device_id.devid_s.bus = 0; From cbe3783d5eeeb6597b1f02f38abf16737f6d4d64 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 26 Jan 2015 09:49:30 +0100 Subject: [PATCH 23/34] Support topic groups in MAVLink subscription handling --- src/modules/mavlink/mavlink_main.cpp | 6 +++--- src/modules/mavlink/mavlink_main.h | 2 +- src/modules/mavlink/mavlink_messages.cpp | 11 ++--------- src/modules/mavlink/mavlink_orb_subscription.cpp | 11 +++++++++-- src/modules/mavlink/mavlink_orb_subscription.h | 4 +++- 5 files changed, 18 insertions(+), 16 deletions(-) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index 9e4ab00df2..d6e9982de0 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -904,20 +904,20 @@ Mavlink::send_statustext(unsigned char severity, const char *string) mavlink_logbuffer_write(&_logbuffer, &logmsg); } -MavlinkOrbSubscription *Mavlink::add_orb_subscription(const orb_id_t topic) +MavlinkOrbSubscription *Mavlink::add_orb_subscription(const orb_id_t topic, int instance) { /* check if already subscribed to this topic */ MavlinkOrbSubscription *sub; LL_FOREACH(_subscriptions, sub) { - if (sub->get_topic() == topic) { + if (sub->get_topic() == topic && sub->get_instance() == instance) { /* already subscribed */ return sub; } } /* add new subscription */ - MavlinkOrbSubscription *sub_new = new MavlinkOrbSubscription(topic); + MavlinkOrbSubscription *sub_new = new MavlinkOrbSubscription(topic, instance); LL_APPEND(_subscriptions, sub_new); diff --git a/src/modules/mavlink/mavlink_main.h b/src/modules/mavlink/mavlink_main.h index ad5e5001b9..baaa7bc139 100644 --- a/src/modules/mavlink/mavlink_main.h +++ b/src/modules/mavlink/mavlink_main.h @@ -171,7 +171,7 @@ public: void handle_message(const mavlink_message_t *msg); - MavlinkOrbSubscription *add_orb_subscription(const orb_id_t topic); + MavlinkOrbSubscription *add_orb_subscription(const orb_id_t topic, int instance=0); int get_instance_id(); diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 6765100c70..4a095a765e 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -1342,14 +1342,7 @@ protected: _act_sub(nullptr), _act_time(0) { - orb_id_t act_topics[] = { - ORB_ID(actuator_outputs_0), - ORB_ID(actuator_outputs_1), - ORB_ID(actuator_outputs_2), - ORB_ID(actuator_outputs_3) - }; - - _act_sub = _mavlink->add_orb_subscription(act_topics[N]); + _act_sub = _mavlink->add_orb_subscription(ORB_ID(actuator_outputs), N); } void send(const hrt_abstime t) @@ -1424,7 +1417,7 @@ protected: _status_time(0), _pos_sp_triplet_sub(_mavlink->add_orb_subscription(ORB_ID(position_setpoint_triplet))), _pos_sp_triplet_time(0), - _act_sub(_mavlink->add_orb_subscription(ORB_ID(actuator_outputs_0))), + _act_sub(_mavlink->add_orb_subscription(ORB_ID(actuator_outputs))), _act_time(0) {} diff --git a/src/modules/mavlink/mavlink_orb_subscription.cpp b/src/modules/mavlink/mavlink_orb_subscription.cpp index 734f0903a5..315776e297 100644 --- a/src/modules/mavlink/mavlink_orb_subscription.cpp +++ b/src/modules/mavlink/mavlink_orb_subscription.cpp @@ -46,10 +46,11 @@ #include "mavlink_orb_subscription.h" -MavlinkOrbSubscription::MavlinkOrbSubscription(const orb_id_t topic) : +MavlinkOrbSubscription::MavlinkOrbSubscription(const orb_id_t topic, int instance) : next(nullptr), _topic(topic), - _fd(orb_subscribe(_topic)), + _instance(instance), + _fd(orb_subscribe_multi(_topic, instance)), _published(false) { } @@ -65,6 +66,12 @@ MavlinkOrbSubscription::get_topic() const return _topic; } +int +MavlinkOrbSubscription::get_instance() const +{ + return _instance; +} + bool MavlinkOrbSubscription::update(uint64_t *time, void* data) { diff --git a/src/modules/mavlink/mavlink_orb_subscription.h b/src/modules/mavlink/mavlink_orb_subscription.h index 7af454df65..5394e5097b 100644 --- a/src/modules/mavlink/mavlink_orb_subscription.h +++ b/src/modules/mavlink/mavlink_orb_subscription.h @@ -50,7 +50,7 @@ class MavlinkOrbSubscription public: MavlinkOrbSubscription *next; ///< pointer to next subscription in list - MavlinkOrbSubscription(const orb_id_t topic); + MavlinkOrbSubscription(const orb_id_t topic, int instance); ~MavlinkOrbSubscription(); /** @@ -77,9 +77,11 @@ public: */ bool is_published(); orb_id_t get_topic() const; + int get_instance() const; private: const orb_id_t _topic; ///< topic metadata + const int _instance; ///< get topic instance int _fd; ///< subscription handle bool _published; ///< topic was ever published From 455f6abfcf25e48edd4d707fdf9b278e02a81772 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 26 Jan 2015 09:49:42 +0100 Subject: [PATCH 24/34] Support topic groups in sdlog2 --- src/modules/sdlog2/sdlog2.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index a3407e42c4..99632ef0bc 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -1096,7 +1096,7 @@ int sdlog2_thread_main(int argc, char *argv[]) subs.att_sub = orb_subscribe(ORB_ID(vehicle_attitude)); subs.att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint)); subs.rates_sp_sub = orb_subscribe(ORB_ID(vehicle_rates_setpoint)); - subs.act_outputs_sub = orb_subscribe(ORB_ID_VEHICLE_CONTROLS); + subs.act_outputs_sub = orb_subscribe(ORB_ID(actuator_outputs)); subs.act_controls_sub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS); subs.act_controls_1_sub = orb_subscribe(ORB_ID(actuator_controls_1)); subs.local_pos_sub = orb_subscribe(ORB_ID(vehicle_local_position)); @@ -1473,7 +1473,7 @@ int sdlog2_thread_main(int argc, char *argv[]) } /* --- ACTUATOR OUTPUTS --- */ - if (copy_if_updated(ORB_ID(actuator_outputs_0), subs.act_outputs_sub, &buf.act_outputs)) { + if (copy_if_updated(ORB_ID(actuator_outputs), subs.act_outputs_sub, &buf.act_outputs)) { log_msg.msg_type = LOG_OUT0_MSG; memcpy(log_msg.body.log_OUT0.output, buf.act_outputs.output, sizeof(log_msg.body.log_OUT0.output)); LOGBUFFER_WRITE_AND_COUNT(OUT0); From 678d5c24fb978f0c18bb87a6894ed2922fe227ca Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 26 Jan 2015 09:49:57 +0100 Subject: [PATCH 25/34] FMU driver: Move to topic groups --- src/drivers/px4fmu/fmu.cpp | 10 ++++------ 1 file changed, 4 insertions(+), 6 deletions(-) diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp index 112c015136..363d2cdcfe 100644 --- a/src/drivers/px4fmu/fmu.cpp +++ b/src/drivers/px4fmu/fmu.cpp @@ -141,7 +141,7 @@ private: int _control_subs[NUM_ACTUATOR_CONTROL_GROUPS]; actuator_controls_s _controls[NUM_ACTUATOR_CONTROL_GROUPS]; orb_id_t _control_topics[NUM_ACTUATOR_CONTROL_GROUPS]; - orb_id_t _actuator_output_topic; + int _actuator_output_topic_instance; pollfd _poll_fds[NUM_ACTUATOR_CONTROL_GROUPS]; unsigned _poll_fds_num; @@ -256,7 +256,7 @@ PX4FMU::PX4FMU() : _groups_required(0), _groups_subscribed(0), _control_subs{-1}, - _actuator_output_topic(nullptr), + _actuator_output_topic_instance(-1), _poll_fds_num(0), _pwm_limit{}, _failsafe_pwm{0}, @@ -327,8 +327,6 @@ PX4FMU::init() log("default PWM output device"); } - _actuator_output_topic = ORB_ID_DOUBLE(actuator_outputs_, _class_instance); - /* reset GPIOs */ gpio_reset(); @@ -679,10 +677,10 @@ PX4FMU::task_main() /* publish mixed control outputs */ if (_outputs_pub < 0) { - _outputs_pub = orb_advertise(_actuator_output_topic, &outputs); + _outputs_pub = orb_advertise_multi(ORB_ID(actuator_outputs), &outputs, &_actuator_output_topic_instance, ORB_PRIO_DEFAULT); } else { - orb_publish(_actuator_output_topic, _outputs_pub, &outputs); + orb_publish(ORB_ID(actuator_outputs), _outputs_pub, &outputs); } } } From 51088026c79aa6dfa237ecfd8276d7e4997bbb9e Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 26 Jan 2015 09:50:09 +0100 Subject: [PATCH 26/34] IO driver: move to topic groups --- src/drivers/px4io/px4io.cpp | 13 ++++--------- 1 file changed, 4 insertions(+), 9 deletions(-) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 556eebab6d..653d0d5d7f 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -1681,17 +1681,12 @@ PX4IO::io_publish_pwm_outputs() /* lazily advertise on first publication */ if (_to_outputs == 0) { - _to_outputs = orb_advertise((_primary_pwm_device ? - ORB_ID_VEHICLE_CONTROLS : - ORB_ID(actuator_outputs_1)), - &outputs); + int instance; + _to_outputs = orb_advertise_multi(ORB_ID(actuator_outputs), + &outputs, &instance, ORB_PRIO_MAX); } else { - orb_publish((_primary_pwm_device ? - ORB_ID_VEHICLE_CONTROLS : - ORB_ID(actuator_outputs_1)), - _to_outputs, - &outputs); + orb_publish(ORB_ID(actuator_outputs), _to_outputs, &outputs); } return OK; From d9d9a59ce440fab4bb1177bd888dc99c77a82350 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 26 Jan 2015 09:50:23 +0100 Subject: [PATCH 27/34] MKBL: Move to topic groups --- src/drivers/mkblctrl/mkblctrl.cpp | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/src/drivers/mkblctrl/mkblctrl.cpp b/src/drivers/mkblctrl/mkblctrl.cpp index 1055487cb9..15412984ba 100644 --- a/src/drivers/mkblctrl/mkblctrl.cpp +++ b/src/drivers/mkblctrl/mkblctrl.cpp @@ -456,8 +456,9 @@ MK::task_main() actuator_outputs_s outputs; memset(&outputs, 0, sizeof(outputs)); /* advertise the mixed control outputs */ - _t_outputs = orb_advertise(_primary_pwm_device ? ORB_ID_VEHICLE_CONTROLS : ORB_ID(actuator_outputs_1), - &outputs); + int dummy; + _t_outputs = orb_advertise_multi(ORB_ID(actuator_outputs), + &outputs, &dummy, ORB_PRIO_HIGH); /* advertise the blctrl status */ esc_status_s esc; From 8d37d58da732a621695d39c886ff4877151064cc Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 26 Jan 2015 09:50:36 +0100 Subject: [PATCH 28/34] HIL: Move to topic groups --- src/drivers/hil/hil.cpp | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/src/drivers/hil/hil.cpp b/src/drivers/hil/hil.cpp index 9b5c8133b4..0584256762 100644 --- a/src/drivers/hil/hil.cpp +++ b/src/drivers/hil/hil.cpp @@ -330,8 +330,9 @@ HIL::task_main() actuator_outputs_s outputs; memset(&outputs, 0, sizeof(outputs)); /* advertise the mixed control outputs */ - _t_outputs = orb_advertise(_primary_pwm_device ? ORB_ID_VEHICLE_CONTROLS : ORB_ID(actuator_outputs_1), - &outputs); + int dummy; + _t_outputs = orb_advertise_multi(ORB_ID(actuator_outputs), + &outputs, &dummy, ORB_PRIO_LOW); pollfd fds[2]; fds[0].fd = _t_actuators; @@ -423,7 +424,7 @@ HIL::task_main() } /* and publish for anyone that cares to see */ - orb_publish(ORB_ID_VEHICLE_CONTROLS, _t_outputs, &outputs); + orb_publish(ORB_ID(actuator_outputs), _t_outputs, &outputs); } } From aadbcb42a91d5158330c7e1f63103ee64f3fbd7c Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 26 Jan 2015 09:50:50 +0100 Subject: [PATCH 29/34] ARDrone driver: Move to topic groups --- src/drivers/ardrone_interface/ardrone_motor_control.c | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/src/drivers/ardrone_interface/ardrone_motor_control.c b/src/drivers/ardrone_interface/ardrone_motor_control.c index 4fa24275f1..0f45b65366 100644 --- a/src/drivers/ardrone_interface/ardrone_motor_control.c +++ b/src/drivers/ardrone_interface/ardrone_motor_control.c @@ -339,7 +339,8 @@ int ardrone_write_motor_commands(int ardrone_fd, uint16_t motor1, uint16_t motor outputs.output[3] = motor4; static orb_advert_t pub = 0; if (pub == 0) { - pub = orb_advertise(ORB_ID_VEHICLE_CONTROLS, &outputs); + /* advertise to channel 0 / primary */ + pub = orb_advertise(ORB_ID(actuator_outputs), &outputs); } if (hrt_absolute_time() - last_motor_time > min_motor_interval) { @@ -350,7 +351,7 @@ int ardrone_write_motor_commands(int ardrone_fd, uint16_t motor1, uint16_t motor fsync(ardrone_fd); /* publish just written values */ - orb_publish(ORB_ID_VEHICLE_CONTROLS, pub, &outputs); + orb_publish(ORB_ID(actuator_outputs), pub, &outputs); if (ret == sizeof(buf)) { return OK; From e532b81ac1f3356a3f2771d605b8d92d88d19d67 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 26 Jan 2015 09:51:36 +0100 Subject: [PATCH 30/34] uORB: Remove last remnants of ORB_ID_DOUBLE/TRIPLE and migrate actuator outputs groups to new style interface --- src/modules/uORB/objects_common.cpp | 5 +- src/modules/uORB/topics/actuator_outputs.h | 8 +-- src/modules/uORB/uORB.h | 58 +++++++++++----------- 3 files changed, 32 insertions(+), 39 deletions(-) diff --git a/src/modules/uORB/objects_common.cpp b/src/modules/uORB/objects_common.cpp index 4a41aa4655..ba1ac0350e 100644 --- a/src/modules/uORB/objects_common.cpp +++ b/src/modules/uORB/objects_common.cpp @@ -196,10 +196,7 @@ ORB_DEFINE(actuator_controls_virtual_fw, struct actuator_controls_s); ORB_DEFINE(actuator_armed, struct actuator_armed_s); #include "topics/actuator_outputs.h" -ORB_DEFINE(actuator_outputs_0, struct actuator_outputs_s); -ORB_DEFINE(actuator_outputs_1, struct actuator_outputs_s); -ORB_DEFINE(actuator_outputs_2, struct actuator_outputs_s); -ORB_DEFINE(actuator_outputs_3, struct actuator_outputs_s); +ORB_DEFINE(actuator_outputs, struct actuator_outputs_s); #include "topics/actuator_direct.h" ORB_DEFINE(actuator_direct, struct actuator_direct_s); diff --git a/src/modules/uORB/topics/actuator_outputs.h b/src/modules/uORB/topics/actuator_outputs.h index 4461404230..5f8f6d83ec 100644 --- a/src/modules/uORB/topics/actuator_outputs.h +++ b/src/modules/uORB/topics/actuator_outputs.h @@ -68,12 +68,6 @@ struct actuator_outputs_s { */ /* actuator output sets; this list can be expanded as more drivers emerge */ -ORB_DECLARE(actuator_outputs_0); -ORB_DECLARE(actuator_outputs_1); -ORB_DECLARE(actuator_outputs_2); -ORB_DECLARE(actuator_outputs_3); - -/* output sets with pre-defined applications */ -#define ORB_ID_VEHICLE_CONTROLS ORB_ID(actuator_outputs_0) +ORB_DECLARE(actuator_outputs); #endif \ No newline at end of file diff --git a/src/modules/uORB/uORB.h b/src/modules/uORB/uORB.h index 30cd598809..9c33c8a3ef 100644 --- a/src/modules/uORB/uORB.h +++ b/src/modules/uORB/uORB.h @@ -86,33 +86,6 @@ enum ORB_PRIO { */ #define ORB_ID(_name) &__orb_##_name -/** - * Generates a pointer to the uORB metadata structure for - * a given topic. - * - * The topic must have been declared previously in scope - * with ORB_DECLARE(). - * - * @param _name The name of the topic. - * @param _count The class ID of the topic - */ -#define ORB_ID_DOUBLE(_name, _count) ((_count == CLASS_DEVICE_PRIMARY) ? &__orb_##_name##0 : &__orb_##_name##1) - -/** - * Generates a pointer to the uORB metadata structure for - * a given topic. - * - * The topic must have been declared previously in scope - * with ORB_DECLARE(). - * - * @param _name The name of the topic. - * @param _count The class ID of the topic - */ -#define ORB_ID_TRIPLE(_name, _count) \ - ((_count == CLASS_DEVICE_PRIMARY) ? &__orb_##_name##0 : \ - ((_count == CLASS_DEVICE_SECONDARY) ? &__orb_##_name##1 : \ - (((_count == CLASS_DEVICE_TERTIARY) ? &__orb_##_name##2 : 0)))) - /** * Declare (prototype) the uORB metadata for a topic. * @@ -147,7 +120,7 @@ enum ORB_PRIO { #define ORB_DEFINE(_name, _struct) \ const struct orb_metadata __orb_##_name = { \ #_name, \ - sizeof(_struct), \ + sizeof(_struct) \ }; struct hack __BEGIN_DECLS @@ -257,6 +230,35 @@ extern int orb_publish(const struct orb_metadata *meta, orb_advert_t handle, con */ extern int orb_subscribe(const struct orb_metadata *meta) __EXPORT; +/** + * Subscribe to a multi-instance of a topic. + * + * The returned value is a file descriptor that can be passed to poll() + * in order to wait for updates to a topic, as well as topic_read, + * orb_check and orb_stat. + * + * Subscription will succeed even if the topic has not been advertised; + * in this case the topic will have a timestamp of zero, it will never + * signal a poll() event, checking will always return false and it cannot + * be copied. When the topic is subsequently advertised, poll, check, + * stat and copy calls will react to the initial publication that is + * performed as part of the advertisement. + * + * Subscription will fail if the topic is not known to the system, i.e. + * there is nothing in the system that has declared the topic and thus it + * can never be published. + * + * @param meta The uORB metadata (usually from the ORB_ID() macro) + * for the topic. + * @param instance The instance of the topic. Instance 0 matches the + * topic of the orb_subscribe() call, higher indices + * are for topics created with orb_publish_multi(). + * @return ERROR on error, otherwise returns a handle + * that can be used to read and update the topic. + * If the topic in question is not known (due to an + * ORB_DEFINE_OPTIONAL with no corresponding ORB_DECLARE) + * this function will return -1 and set errno to ENOENT. + */ extern int orb_subscribe_multi(const struct orb_metadata *meta, unsigned instance) __EXPORT; /** From a381ce37323b08597df637e901099fcdd69c6ec1 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 26 Jan 2015 09:56:15 +0100 Subject: [PATCH 31/34] Fix newline --- src/modules/uORB/topics/actuator_outputs.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/uORB/topics/actuator_outputs.h b/src/modules/uORB/topics/actuator_outputs.h index 5f8f6d83ec..c6fbaaed5a 100644 --- a/src/modules/uORB/topics/actuator_outputs.h +++ b/src/modules/uORB/topics/actuator_outputs.h @@ -70,4 +70,4 @@ struct actuator_outputs_s { /* actuator output sets; this list can be expanded as more drivers emerge */ ORB_DECLARE(actuator_outputs); -#endif \ No newline at end of file +#endif From e9bcc0a2624c77c6f71bb926347e85b8c7592e34 Mon Sep 17 00:00:00 2001 From: Simon Wilks Date: Thu, 29 Jan 2015 14:05:48 +0100 Subject: [PATCH 32/34] Add yaw modes that define multirotor heading behaviour during missions. --- src/modules/navigator/mission.cpp | 72 +++++++++++++++++++++++-- src/modules/navigator/mission.h | 16 +++++- src/modules/navigator/mission_block.cpp | 3 +- src/modules/navigator/mission_params.c | 16 ++++++ 4 files changed, 102 insertions(+), 5 deletions(-) diff --git a/src/modules/navigator/mission.cpp b/src/modules/navigator/mission.cpp index 9b0a092dad..b52b12ce7c 100644 --- a/src/modules/navigator/mission.cpp +++ b/src/modules/navigator/mission.cpp @@ -39,6 +39,7 @@ * @author Thomas Gubler * @author Anton Babushkin * @author Ban Siesta + * @author Simon Wilks */ #include @@ -68,6 +69,7 @@ Mission::Mission(Navigator *navigator, const char *name) : _param_takeoff_alt(this, "MIS_TAKEOFF_ALT", false), _param_dist_1wp(this, "MIS_DIST_1WP", false), _param_altmode(this, "MIS_ALTMODE", false), + _param_yawmode(this, "MIS_YAWMODE", false), _onboard_mission({0}), _offboard_mission({0}), _current_onboard_mission_index(-1), @@ -80,6 +82,7 @@ Mission::Mission(Navigator *navigator, const char *name) : _missionFeasiblityChecker(), _min_current_sp_distance_xy(FLT_MAX), _mission_item_previous_alt(NAN), + _on_arrival_yaw(NAN), _distance_current_previous(0.0f) { /* load initial params */ @@ -166,6 +169,13 @@ Mission::on_active() _navigator->set_can_loiter_at_sp(true); } } + + /* see if we need to update the current yaw heading for rotary wing types */ + if (_navigator->get_vstatus()->is_rotary_wing + && _param_yawmode.get() != MISSION_YAWMODE_NONE + && _mission_type != MISSION_TYPE_NONE) { + heading_sp_update(); + } } void @@ -275,7 +285,7 @@ Mission::check_dist_1wp() &mission_item, sizeof(mission_item_s)) == sizeof(mission_item_s)) { /* check only items with valid lat/lon */ - if ( mission_item.nav_cmd == NAV_CMD_WAYPOINT || + if ( mission_item.nav_cmd == NAV_CMD_WAYPOINT || mission_item.nav_cmd == NAV_CMD_LOITER_TIME_LIMIT || mission_item.nav_cmd == NAV_CMD_LOITER_TURN_COUNT || mission_item.nav_cmd == NAV_CMD_LOITER_UNLIMITED || @@ -362,7 +372,6 @@ Mission::set_mission_items() mavlink_log_critical(_navigator->get_mavlink_fd(), "offboard mission now running"); } _mission_type = MISSION_TYPE_OFFBOARD; - } else { /* no mission available or mission finished, switch to loiter */ if (_mission_type != MISSION_TYPE_NONE) { @@ -396,6 +405,10 @@ Mission::set_mission_items() return; } + if (pos_sp_triplet->current.valid) { + _on_arrival_yaw = _mission_item.yaw; + } + /* do takeoff on first waypoint for rotary wing vehicles */ if (_navigator->get_vstatus()->is_rotary_wing) { /* force takeoff if landed (additional protection) */ @@ -442,6 +455,7 @@ Mission::set_mission_items() _mission_item.nav_cmd = NAV_CMD_TAKEOFF; _mission_item.lat = _navigator->get_global_position()->lat; _mission_item.lon = _navigator->get_global_position()->lon; + _mission_item.yaw = NAN; _mission_item.altitude = takeoff_alt; _mission_item.altitude_is_relative = false; _mission_item.autocontinue = true; @@ -481,7 +495,6 @@ Mission::set_mission_items() if (read_mission_item(_mission_type == MISSION_TYPE_ONBOARD, false, &mission_item_next)) { /* got next mission item, update setpoint triplet */ mission_item_to_position_setpoint(&mission_item_next, &pos_sp_triplet->next); - } else { /* next mission item is not available */ pos_sp_triplet->next.valid = false; @@ -503,6 +516,59 @@ Mission::set_mission_items() _navigator->set_position_setpoint_triplet_updated(); } +void +Mission::heading_sp_update() +{ + if (_takeoff) { + /* we don't want to be yawing during takeoff */ + return; + } + + struct position_setpoint_triplet_s *pos_sp_triplet = _navigator->get_position_setpoint_triplet(); + + /* Don't change setpoint if last and current waypoint are not valid */ + if (!pos_sp_triplet->previous.valid || !pos_sp_triplet->current.valid || + !isfinite(_on_arrival_yaw)) { + return; + } + + /* Don't do FOH for landing and takeoff waypoints, the ground may be near + * and the FW controller has a custom landing logic */ + if (_mission_item.nav_cmd == NAV_CMD_LAND || _mission_item.nav_cmd == NAV_CMD_TAKEOFF) { + return; + } + + /* set yaw angle for the waypoint iff a loiter time has been specified */ + if (_waypoint_position_reached && _mission_item.time_inside > 0.0f) { + _mission_item.yaw = _on_arrival_yaw; + /* always keep the front of the rotary wing pointing to the next waypoint */ + } else if (_param_yawmode.get() == MISSION_YAWMODE_FRONT_TO_WAYPOINT) { + _mission_item.yaw = get_bearing_to_next_waypoint( + _navigator->get_global_position()->lat, + _navigator->get_global_position()->lon, + _mission_item.lat, + _mission_item.lon); + /* always keep the back of the rotary wing pointing towards home */ + } else if (_param_yawmode.get() == MISSION_YAWMODE_FRONT_TO_HOME) { + _mission_item.yaw = get_bearing_to_next_waypoint( + _navigator->get_global_position()->lat, + _navigator->get_global_position()->lon, + _navigator->get_home_position()->lat, + _navigator->get_home_position()->lon); + /* always keep the back of the rotary wing pointing towards home */ + } else if (_param_yawmode.get() == MISSION_YAWMODE_BACK_TO_HOME) { + _mission_item.yaw = _wrap_pi(get_bearing_to_next_waypoint( + _navigator->get_global_position()->lat, + _navigator->get_global_position()->lon, + _navigator->get_home_position()->lat, + _navigator->get_home_position()->lon) + M_PI_F); + } + + mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current); + _navigator->set_position_setpoint_triplet_updated(); +} + + void Mission::altitude_sp_foh_update() { diff --git a/src/modules/navigator/mission.h b/src/modules/navigator/mission.h index a8a644b0f5..e9f78e8fdc 100644 --- a/src/modules/navigator/mission.h +++ b/src/modules/navigator/mission.h @@ -83,6 +83,13 @@ public: MISSION_ALTMODE_FOH = 1 }; + enum mission_yaw_mode { + MISSION_YAWMODE_NONE = 0, + MISSION_YAWMODE_FRONT_TO_WAYPOINT = 1, + MISSION_YAWMODE_FRONT_TO_HOME = 2, + MISSION_YAWMODE_BACK_TO_HOME = 3 + }; + private: /** * Update onboard mission topic @@ -110,6 +117,11 @@ private: */ void set_mission_items(); + /** + * Updates the heading of the vehicle. Rotary wings only. + */ + void heading_sp_update(); + /** * Updates the altitude sp to follow a foh */ @@ -155,6 +167,7 @@ private: control::BlockParamFloat _param_takeoff_alt; control::BlockParamFloat _param_dist_1wp; control::BlockParamInt _param_altmode; + control::BlockParamInt _param_yawmode; struct mission_s _onboard_mission; struct mission_s _offboard_mission; @@ -177,7 +190,8 @@ private: float _min_current_sp_distance_xy; /**< minimum distance which was achieved to the current waypoint */ float _mission_item_previous_alt; /**< holds the altitude of the previous mission item, - can be replaced by a full copy of the previous mission item if needed*/ + can be replaced by a full copy of the previous mission item if needed */ + float _on_arrival_yaw; /**< holds the yaw value that should be applied when the current waypoint is reached */ float _distance_current_previous; /**< distance from previous to current sp in pos_sp_triplet, only use if current and previous are valid */ }; diff --git a/src/modules/navigator/mission_block.cpp b/src/modules/navigator/mission_block.cpp index 723caec7ce..e39fb32164 100644 --- a/src/modules/navigator/mission_block.cpp +++ b/src/modules/navigator/mission_block.cpp @@ -134,6 +134,7 @@ MissionBlock::is_mission_item_reached() } } + /* Check if the waypoint and the requested yaw setpoint. */ if (_waypoint_position_reached && !_waypoint_yaw_reached) { /* TODO: removed takeoff, why? */ @@ -151,7 +152,7 @@ MissionBlock::is_mission_item_reached() } } - /* check if the current waypoint was reached */ + /* Once the waypoint and yaw setpoint have been reached we can start the loiter time countdown */ if (_waypoint_position_reached && _waypoint_yaw_reached) { if (_time_first_inside_orbit == 0) { diff --git a/src/modules/navigator/mission_params.c b/src/modules/navigator/mission_params.c index 04c01fe51c..6310cf6de4 100644 --- a/src/modules/navigator/mission_params.c +++ b/src/modules/navigator/mission_params.c @@ -95,3 +95,19 @@ PARAM_DEFINE_FLOAT(MIS_DIST_1WP, 500); * @group Mission */ PARAM_DEFINE_INT32(MIS_ALTMODE, 0); + +/** + * Multirotor only. Yaw setpoint mode. + * + * 0: Set the yaw heading to the yaw value specified for the destination waypoint. + * 1: Maintain a yaw heading pointing towards the next waypoint. + * 2: Maintain a yaw heading that always points to the home location. + * 3: Maintain a yaw heading that always points away from the home location (ie: back always faces home). + * + * The values are defined in the enum mission_altitude_mode + * + * @min 0 + * @max 3 + * @group Mission + */ +PARAM_DEFINE_INT32(MIS_YAWMODE, 0); From 0b784c20c8bf69cee281f6717f055b0309d331b1 Mon Sep 17 00:00:00 2001 From: hauptmech Date: Wed, 28 Jan 2015 15:45:00 +1300 Subject: [PATCH 33/34] Save and check device id for acc and gyro calibration parameters. Fix config utility to work with all devices of each type. Accel, gyro and mag devices correctly set their device_id devtype. Combo devices (mpu6000 lsm303d) now correctly return their devtype. config util shows device id for all sensor types. Add, save during calibration and check during preflight ID parameters for accelerometer and gyro --- src/drivers/drv_mag.h | 6 -- src/drivers/drv_sensor.h | 18 ++++++ src/drivers/l3gd20/l3gd20.cpp | 16 +++--- src/drivers/lsm303d/lsm303d.cpp | 32 ++++++++--- src/drivers/mpu6000/mpu6000.cpp | 56 ++++++++++++------- .../commander/accelerometer_calibration.cpp | 8 +++ src/modules/commander/gyro_calibration.cpp | 7 +++ src/modules/sensors/sensor_params.c | 15 ++++- src/systemcmds/config/config.c | 17 ++++-- .../preflight_check/preflight_check.c | 38 ++++++++++--- 10 files changed, 156 insertions(+), 57 deletions(-) diff --git a/src/drivers/drv_mag.h b/src/drivers/drv_mag.h index d8fe1ae7a3..193c816e0e 100644 --- a/src/drivers/drv_mag.h +++ b/src/drivers/drv_mag.h @@ -41,7 +41,6 @@ #include #include -#include "drv_device.h" #include "drv_sensor.h" #include "drv_orb_dev.h" @@ -83,11 +82,6 @@ struct mag_scale { */ ORB_DECLARE(sensor_mag); -/* - * mag device types, for _device_id - */ -#define DRV_MAG_DEVTYPE_HMC5883 1 -#define DRV_MAG_DEVTYPE_LSM303D 2 /* * ioctl() definitions diff --git a/src/drivers/drv_sensor.h b/src/drivers/drv_sensor.h index 5e4598de86..467dace082 100644 --- a/src/drivers/drv_sensor.h +++ b/src/drivers/drv_sensor.h @@ -43,6 +43,24 @@ #include #include +#include "drv_device.h" + +/** + * Sensor type definitions. + * + * Used to create a unique device id for redundant and combo sensors + */ + +#define DRV_MAG_DEVTYPE_HMC5883 0x01 +#define DRV_MAG_DEVTYPE_LSM303D 0x02 +#define DRV_ACC_DEVTYPE_LSM303D 0x11 +#define DRV_ACC_DEVTYPE_BMA180 0x12 +#define DRV_ACC_DEVTYPE_MPU6000 0x13 +#define DRV_GYR_DEVTYPE_MPU6000 0x21 +#define DRV_GYR_DEVTYPE_L3GD20 0x22 +#define DRV_RNG_DEVTYPE_MB12XX 0x31 +#define DRV_RNG_DEVTYPE_LL40LS 0x32 + /* * ioctl() definitions * diff --git a/src/drivers/l3gd20/l3gd20.cpp b/src/drivers/l3gd20/l3gd20.cpp index bd1bd9f86d..f583bced4f 100644 --- a/src/drivers/l3gd20/l3gd20.cpp +++ b/src/drivers/l3gd20/l3gd20.cpp @@ -220,7 +220,7 @@ private: struct hrt_call _call; unsigned _call_interval; - + RingBuffer *_reports; struct gyro_scale _gyro_scale; @@ -424,6 +424,8 @@ L3GD20::L3GD20(int bus, const char* path, spi_dev_e device, enum Rotation rotati // enable debug() calls _debug_enabled = true; + _device_id.devid_s.devtype = DRV_GYR_DEVTYPE_L3GD20; + // default scale factors _gyro_scale.x_offset = 0; _gyro_scale.x_scale = 1.0f; @@ -639,7 +641,7 @@ L3GD20::ioctl(struct file *filp, int cmd, unsigned long arg) return -ENOMEM; } irqrestore(flags); - + return OK; } @@ -867,7 +869,7 @@ L3GD20::reset() disable_i2c(); /* set default configuration */ - write_checked_reg(ADDR_CTRL_REG1, + write_checked_reg(ADDR_CTRL_REG1, REG1_POWER_NORMAL | REG1_Z_ENABLE | REG1_Y_ENABLE | REG1_X_ENABLE); write_checked_reg(ADDR_CTRL_REG2, 0); /* disable high-pass filters */ write_checked_reg(ADDR_CTRL_REG3, 0x08); /* DRDY enable */ @@ -911,7 +913,7 @@ L3GD20::check_registers(void) if we get the wrong value then we know the SPI bus or sensor is very sick. We set _register_wait to 20 and wait until we have seen 20 good values in a row - before we consider the sensor to be OK again. + before we consider the sensor to be OK again. */ perf_count(_bad_registers); @@ -974,7 +976,7 @@ L3GD20::measure() we waited for DRDY, but did not see DRDY on all axes when we captured. That means a transfer error of some sort */ - perf_count(_errors); + perf_count(_errors); return; } #endif @@ -994,7 +996,7 @@ L3GD20::measure() */ report.timestamp = hrt_absolute_time(); report.error_count = perf_event_count(_bad_registers); - + switch (_orientation) { case SENSOR_BOARD_ROTATION_000_DEG: @@ -1072,7 +1074,7 @@ L3GD20::print_info() for (uint8_t i=0; i_device_id.devid = _device_id.devid; + _mag->_device_id.devid_s.devtype = DRV_MAG_DEVTYPE_LSM303D; + + // default scale factors _accel_scale.x_offset = 0.0f; _accel_scale.x_scale = 1.0f; @@ -660,6 +667,7 @@ LSM303D::init() warnx("ADVERT ERR"); } + _accel_class_instance = register_class_devname(ACCEL_DEVICE_PATH); /* advertise sensor topic, measure manually to initialize valid report */ @@ -698,7 +706,7 @@ LSM303D::reset() disable_i2c(); /* enable accel*/ - write_checked_reg(ADDR_CTRL_REG1, + write_checked_reg(ADDR_CTRL_REG1, REG1_X_ENABLE_A | REG1_Y_ENABLE_A | REG1_Z_ENABLE_A | REG1_BDU_UPDATE | REG1_RATE_800HZ_A); /* enable mag */ @@ -732,7 +740,7 @@ LSM303D::probe() /* verify that the device is attached and functioning */ bool success = (read_reg(ADDR_WHO_AM_I) == WHO_I_AM); - + if (success) { _checked_values[0] = WHO_I_AM; return OK; @@ -1005,7 +1013,7 @@ LSM303D::mag_ioctl(struct file *filp, int cmd, unsigned long arg) return SENSOR_POLLRATE_MANUAL; return 1000000 / _call_mag_interval; - + case SENSORIOCSQUEUEDEPTH: { /* lower bound is mandatory, upper bound is a sanity check */ if ((arg < 1) || (arg > 100)) @@ -1410,7 +1418,7 @@ LSM303D::check_registers(void) if we get the wrong value then we know the SPI bus or sensor is very sick. We set _register_wait to 20 and wait until we have seen 20 good values in a row - before we consider the sensor to be OK again. + before we consider the sensor to be OK again. */ perf_count(_bad_registers); @@ -1534,7 +1542,7 @@ LSM303D::measure() perf_count(_bad_values); _constant_accel_count = 0; } - + _last_accel[0] = x_in_new; _last_accel[1] = y_in_new; _last_accel[2] = z_in_new; @@ -1652,7 +1660,7 @@ LSM303D::print_info() for (uint8_t i=0; imag_ioctl(filp, cmd, arg); + switch (cmd) { + case DEVIOCGDEVICEID: + return (int)CDev::ioctl(filp, cmd, arg); + break; + default: + return _parent->mag_ioctl(filp, cmd, arg); + } } void diff --git a/src/drivers/mpu6000/mpu6000.cpp b/src/drivers/mpu6000/mpu6000.cpp index e4e9824902..e322e8b3a5 100644 --- a/src/drivers/mpu6000/mpu6000.cpp +++ b/src/drivers/mpu6000/mpu6000.cpp @@ -446,7 +446,7 @@ const uint8_t MPU6000::_checked_registers[MPU6000_NUM_CHECKED_REGISTERS] = { MPU MPUREG_INT_ENABLE, MPUREG_INT_PIN_CFG }; - + /** * Helper class implementing the gyro driver node. @@ -523,6 +523,12 @@ MPU6000::MPU6000(int bus, const char *path_accel, const char *path_gyro, spi_dev // disable debug() calls _debug_enabled = false; + _device_id.devid_s.devtype = DRV_ACC_DEVTYPE_MPU6000; + + /* Prime _gyro with parents devid. */ + _gyro->_device_id.devid = _device_id.devid; + _gyro->_device_id.devid_s.devtype = DRV_GYR_DEVTYPE_MPU6000; + // default accel scale factors _accel_scale.x_offset = 0; _accel_scale.x_scale = 1.0f; @@ -609,6 +615,7 @@ MPU6000::init() _gyro_scale.z_offset = 0; _gyro_scale.z_scale = 1.0f; + /* do CDev init for the gyro device node, keep it optional */ ret = _gyro->init(); /* if probe/setup failed, bail now */ @@ -668,7 +675,7 @@ int MPU6000::reset() // for it to come out of sleep write_checked_reg(MPUREG_PWR_MGMT_1, MPU_CLK_SEL_PLLGYROZ); up_udelay(1000); - + // Disable I2C bus (recommended on datasheet) write_checked_reg(MPUREG_USER_CTRL, BIT_I2C_IF_DIS); irqrestore(state); @@ -726,7 +733,7 @@ int MPU6000::reset() case MPU6000_REV_D9: case MPU6000_REV_D10: // default case to cope with new chip revisions, which - // presumably won't have the accel scaling bug + // presumably won't have the accel scaling bug default: // Accel scale 8g (4096 LSB/g) write_checked_reg(MPUREG_ACCEL_CONFIG, 2 << 3); @@ -804,7 +811,7 @@ MPU6000::_set_dlpf_filter(uint16_t frequency_hz) { uint8_t filter; - /* + /* choose next highest filter frequency available */ if (frequency_hz == 0) { @@ -906,7 +913,7 @@ MPU6000::gyro_self_test() if (self_test()) return 1; - /* + /* * Maximum deviation of 20 degrees, according to * http://www.invensense.com/mems/gyro/documents/PS-MPU-6000A-00v3.4.pdf * Section 6.1, initial ZRO tolerance @@ -967,7 +974,7 @@ MPU6000::factory_self_test() // gyro self test has to be done at 250DPS write_reg(MPUREG_GYRO_CONFIG, BITS_FS_250DPS); - struct MPUReport mpu_report; + struct MPUReport mpu_report; float accel_baseline[3]; float gyro_baseline[3]; float accel[3]; @@ -997,10 +1004,10 @@ MPU6000::factory_self_test() } #if 1 - write_reg(MPUREG_GYRO_CONFIG, - BITS_FS_250DPS | - BITS_GYRO_ST_X | - BITS_GYRO_ST_Y | + write_reg(MPUREG_GYRO_CONFIG, + BITS_FS_250DPS | + BITS_GYRO_ST_X | + BITS_GYRO_ST_Y | BITS_GYRO_ST_Z); // accel 8g, self-test enabled all axes @@ -1070,7 +1077,7 @@ MPU6000::factory_self_test() ::printf("FAIL\n"); ret = -EIO; } - } + } for (uint8_t i=0; i<3; i++) { float diff = gyro[i]-gyro_baseline[i]; float err = 100*(diff - gyro_ftrim[i]) / gyro_ftrim[i]; @@ -1085,7 +1092,7 @@ MPU6000::factory_self_test() ::printf("FAIL\n"); ret = -EIO; } - } + } write_reg(MPUREG_GYRO_CONFIG, saved_gyro_config); write_reg(MPUREG_ACCEL_CONFIG, saved_accel_config); @@ -1232,14 +1239,14 @@ MPU6000::ioctl(struct file *filp, int cmd, unsigned long arg) /* lower bound is mandatory, upper bound is a sanity check */ if ((arg < 1) || (arg > 100)) return -EINVAL; - + irqstate_t flags = irqsave(); if (!_accel_reports->resize(arg)) { irqrestore(flags); return -ENOMEM; } irqrestore(flags); - + return OK; } @@ -1521,13 +1528,13 @@ MPU6000::check_registers(void) the data registers. */ uint8_t v; - if ((v=read_reg(_checked_registers[_checked_next], MPU6000_HIGH_BUS_SPEED)) != + if ((v=read_reg(_checked_registers[_checked_next], MPU6000_HIGH_BUS_SPEED)) != _checked_values[_checked_next]) { /* if we get the wrong value then we know the SPI bus or sensor is very sick. We set _register_wait to 20 and wait until we have seen 20 good values in a row - before we consider the sensor to be OK again. + before we consider the sensor to be OK again. */ perf_count(_bad_registers); @@ -1640,7 +1647,7 @@ MPU6000::measure() _register_wait--; return; } - + /* * Swap axes and negate y @@ -1701,7 +1708,7 @@ MPU6000::measure() float x_in_new = ((report.accel_x * _accel_range_scale) - _accel_scale.x_offset) * _accel_scale.x_scale; float y_in_new = ((report.accel_y * _accel_range_scale) - _accel_scale.y_offset) * _accel_scale.y_scale; float z_in_new = ((report.accel_z * _accel_range_scale) - _accel_scale.z_offset) * _accel_scale.z_scale; - + arb.x = _accel_filter_x.apply(x_in_new); arb.y = _accel_filter_y.apply(y_in_new); arb.z = _accel_filter_z.apply(z_in_new); @@ -1722,7 +1729,7 @@ MPU6000::measure() float x_gyro_in_new = ((report.gyro_x * _gyro_range_scale) - _gyro_scale.x_offset) * _gyro_scale.x_scale; float y_gyro_in_new = ((report.gyro_y * _gyro_range_scale) - _gyro_scale.y_offset) * _gyro_scale.y_scale; float z_gyro_in_new = ((report.gyro_z * _gyro_range_scale) - _gyro_scale.z_offset) * _gyro_scale.z_scale; - + grb.x = _gyro_filter_x.apply(x_gyro_in_new); grb.y = _gyro_filter_y.apply(y_gyro_in_new); grb.z = _gyro_filter_z.apply(z_gyro_in_new); @@ -1776,7 +1783,7 @@ MPU6000::print_info() for (uint8_t i=0; igyro_ioctl(filp, cmd, arg); + + switch (cmd) { + case DEVIOCGDEVICEID: + return (int)CDev::ioctl(filp, cmd, arg); + break; + default: + return _parent->gyro_ioctl(filp, cmd, arg); + } } /** diff --git a/src/modules/commander/accelerometer_calibration.cpp b/src/modules/commander/accelerometer_calibration.cpp index d4cd97be6a..13ab966aba 100644 --- a/src/modules/commander/accelerometer_calibration.cpp +++ b/src/modules/commander/accelerometer_calibration.cpp @@ -159,6 +159,7 @@ int calculate_calibration_values(float accel_ref[6][3], float accel_T[3][3], flo int do_accel_calibration(int mavlink_fd) { int fd; + int32_t device_id; mavlink_log_info(mavlink_fd, CAL_STARTED_MSG, sensor_name); @@ -180,6 +181,9 @@ int do_accel_calibration(int mavlink_fd) /* reset all offsets to zero and all scales to one */ fd = open(ACCEL_DEVICE_PATH, 0); + + device_id = ioctl(fd, DEVIOCGDEVICEID, 0); + res = ioctl(fd, ACCELIOCSSCALE, (long unsigned int)&accel_scale); close(fd); @@ -226,6 +230,10 @@ int do_accel_calibration(int mavlink_fd) mavlink_log_critical(mavlink_fd, CAL_FAILED_SET_PARAMS_MSG); res = ERROR; } + + if (param_set(param_find("SENS_ACC_ID"), &(device_id))) { + res = ERROR; + } } if (res == OK) { diff --git a/src/modules/commander/gyro_calibration.cpp b/src/modules/commander/gyro_calibration.cpp index 2be0e881ee..8410297efc 100644 --- a/src/modules/commander/gyro_calibration.cpp +++ b/src/modules/commander/gyro_calibration.cpp @@ -62,6 +62,7 @@ static const char *sensor_name = "gyro"; int do_gyro_calibration(int mavlink_fd) { + int32_t device_id; mavlink_log_info(mavlink_fd, CAL_STARTED_MSG, sensor_name); mavlink_log_info(mavlink_fd, "HOLD STILL"); @@ -81,6 +82,9 @@ int do_gyro_calibration(int mavlink_fd) /* reset all offsets to zero and all scales to one */ int fd = open(GYRO_DEVICE_PATH, 0); + + device_id = ioctl(fd, DEVIOCGDEVICEID, 0); + res = ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gyro_scale); close(fd); @@ -277,6 +281,9 @@ int do_gyro_calibration(int mavlink_fd) mavlink_log_critical(mavlink_fd, "ERROR: failed to set scale params"); res = ERROR; } + if (param_set(param_find("SENS_GYRO_ID"), &(device_id))) { + res = ERROR; + } } if (res == OK) { diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c index bf5708e0b7..67b7feef7b 100644 --- a/src/modules/sensors/sensor_params.c +++ b/src/modules/sensors/sensor_params.c @@ -44,6 +44,13 @@ #include #include +/** + * ID of the Gyro that the calibration is for. + * + * @group Sensor Calibration + */ +PARAM_DEFINE_INT32(SENS_GYRO_ID, 0); + /** * Gyro X-axis offset * @@ -153,6 +160,12 @@ PARAM_DEFINE_FLOAT(SENS_MAG_YSCALE, 1.0f); */ PARAM_DEFINE_FLOAT(SENS_MAG_ZSCALE, 1.0f); +/** + * ID of the Accelerometer that the calibration is for. + * + * @group Sensor Calibration + */ +PARAM_DEFINE_INT32(SENS_ACC_ID, 0); /** * Accelerometer X-axis offset @@ -270,7 +283,7 @@ PARAM_DEFINE_INT32(SENS_BOARD_ROT, 0); /** * PX4Flow board rotation * - * This parameter defines the rotation of the PX4FLOW board relative to the platform. + * This parameter defines the rotation of the PX4FLOW board relative to the platform. * Zero rotation is defined as Y on flow board pointing towards front of vehicle * Possible values are: * 0 = No rotation diff --git a/src/systemcmds/config/config.c b/src/systemcmds/config/config.c index 9f13edb184..f54342f06a 100644 --- a/src/systemcmds/config/config.c +++ b/src/systemcmds/config/config.c @@ -81,7 +81,7 @@ config_main(int argc, char *argv[]) do_device(argc - 1, argv + 1); } } - + errx(1, "expected a device, try '/dev/gyro', '/dev/accel', '/dev/mag'"); } @@ -192,8 +192,12 @@ do_gyro(int argc, char *argv[]) int srate = ioctl(fd, GYROIOCGSAMPLERATE, 0); int prate = ioctl(fd, SENSORIOCGPOLLRATE, 0); int range = ioctl(fd, GYROIOCGRANGE, 0); + int id = ioctl(fd, DEVIOCGDEVICEID,0); + int32_t calibration_id = 0; - warnx("gyro: \n\tsample rate:\t%d Hz\n\tread rate:\t%d Hz\n\trange:\t%d dps", srate, prate, range); + param_get(param_find("SENS_GYRO_ID"), &(calibration_id)); + + warnx("gyro: \n\tdevice id:\t0x%X\t(calibration is for device id 0x%X)\n\tsample rate:\t%d Hz\n\tread rate:\t%d Hz\n\trange:\t%d dps", id, calibration_id, srate, prate, range); close(fd); } @@ -267,9 +271,10 @@ do_mag(int argc, char *argv[]) int range = ioctl(fd, MAGIOCGRANGE, 0); int id = ioctl(fd, DEVIOCGDEVICEID,0); int32_t calibration_id = 0; + param_get(param_find("SENS_MAG_ID"), &(calibration_id)); - warnx("mag: \n\tdevice id:\t%x\t(calibration is for device id %x)\n\tsample rate:\t%d Hz\n\tread rate:\t%d Hz\n\trange:\t%d Ga", id, calibration_id, srate, prate, range); + warnx("mag: \n\tdevice id:\t0x%X\t(calibration is for device id 0x%X)\n\tsample rate:\t%d Hz\n\tread rate:\t%d Hz\n\trange:\t%d Ga", id, calibration_id, srate, prate, range); close(fd); } @@ -341,8 +346,12 @@ do_accel(int argc, char *argv[]) int srate = ioctl(fd, ACCELIOCGSAMPLERATE, 0); int prate = ioctl(fd, SENSORIOCGPOLLRATE, 0); int range = ioctl(fd, ACCELIOCGRANGE, 0); + int id = ioctl(fd, DEVIOCGDEVICEID,0); + int32_t calibration_id = 0; - warnx("accel: \n\tsample rate:\t%d Hz\n\tread rate:\t%d Hz\n\trange:\t%d G", srate, prate, range); + param_get(param_find("SENS_ACC_ID"), &(calibration_id)); + + warnx("accel: \n\tdevice id:\t0x%X\t(calibration is for device id 0x%X)\n\tsample rate:\t%d Hz\n\tread rate:\t%d Hz\n\trange:\t%d G", id, calibration_id, srate, prate, range); close(fd); } diff --git a/src/systemcmds/preflight_check/preflight_check.c b/src/systemcmds/preflight_check/preflight_check.c index bbd90b961f..3e1f76714b 100644 --- a/src/systemcmds/preflight_check/preflight_check.c +++ b/src/systemcmds/preflight_check/preflight_check.c @@ -84,7 +84,7 @@ int preflight_check_main(int argc, char *argv[]) /* open text message output path */ int mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); int ret; - int32_t mag_devid,mag_calibration_devid; + int32_t devid, calibration_devid; /* give the system some time to sample the sensors in the background */ usleep(150000); @@ -98,9 +98,9 @@ int preflight_check_main(int argc, char *argv[]) goto system_eval; } - mag_devid = ioctl(fd, DEVIOCGDEVICEID,0); - param_get(param_find("SENS_MAG_ID"), &(mag_calibration_devid)); - if (mag_devid != mag_calibration_devid){ + devid = ioctl(fd, DEVIOCGDEVICEID,0); + param_get(param_find("SENS_MAG_ID"), &(calibration_devid)); + if (devid != calibration_devid){ warnx("magnetometer calibration is for a different device - calibrate magnetometer first"); mavlink_log_critical(mavlink_fd, "SENSOR FAIL: MAG CAL ID"); system_ok = false; @@ -108,7 +108,7 @@ int preflight_check_main(int argc, char *argv[]) } ret = ioctl(fd, MAGIOCSELFTEST, 0); - + if (ret != OK) { warnx("magnetometer calibration missing or bad - calibrate magnetometer first"); mavlink_log_critical(mavlink_fd, "SENSOR FAIL: MAG CHECK/CAL"); @@ -120,8 +120,18 @@ int preflight_check_main(int argc, char *argv[]) close(fd); fd = open(ACCEL_DEVICE_PATH, O_RDONLY); + + devid = ioctl(fd, DEVIOCGDEVICEID,0); + param_get(param_find("SENS_ACC_ID"), &(calibration_devid)); + if (devid != calibration_devid){ + warnx("accelerometer calibration is for a different device - calibrate accelerometer first"); + mavlink_log_critical(mavlink_fd, "SENSOR FAIL: ACC CAL ID"); + system_ok = false; + goto system_eval; + } + ret = ioctl(fd, ACCELIOCSELFTEST, 0); - + if (ret != OK) { warnx("accel self test failed"); mavlink_log_critical(mavlink_fd, "SENSOR FAIL: ACCEL CHECK/CAL"); @@ -156,8 +166,18 @@ int preflight_check_main(int argc, char *argv[]) close(fd); fd = open(GYRO_DEVICE_PATH, 0); + + devid = ioctl(fd, DEVIOCGDEVICEID,0); + param_get(param_find("SENS_GYRO_ID"), &(calibration_devid)); + if (devid != calibration_devid){ + warnx("gyro calibration is for a different device - calibrate gyro first"); + mavlink_log_critical(mavlink_fd, "SENSOR FAIL: GYRO CAL ID"); + system_ok = false; + goto system_eval; + } + ret = ioctl(fd, GYROIOCSELFTEST, 0); - + if (ret != OK) { warnx("gyro self test failed"); mavlink_log_critical(mavlink_fd, "SENSOR FAIL: GYRO CHECK/CAL"); @@ -183,10 +203,10 @@ int preflight_check_main(int argc, char *argv[]) system_ok &= rc_ok; - + system_eval: - + if (system_ok) { /* all good, exit silently */ exit(0); From a2a244584e36a0e9ffdb93a0dda8473baf8344d3 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 31 Jan 2015 23:09:48 +0100 Subject: [PATCH 34/34] Update block diagram --- Documentation/px4_block_diagram.odg | Bin 91712 -> 94510 bytes 1 file changed, 0 insertions(+), 0 deletions(-) diff --git a/Documentation/px4_block_diagram.odg b/Documentation/px4_block_diagram.odg index e66e1b1cdd704ebe713a2127004488aed7fb2218..84566844252805e31620a7313d29aa34d1b36029 100644 GIT binary patch delta 50391 zcmaG{Q+H-fw|!#Uwr#89j%_;~+fUSS$9B@OZQJPBw(aE0dw#&VsIhNq)UI8%$69mF zHCN9BMAa??f}#u2oWdcY5*GEW4h$6c3woU3hA$0 zP9Xrnpy0iR$?r>h@L)B^u{9G!olnmz-qd`;62c-q-c;wsJD~6>_(rC!J$dhjvmmIu zCe~p{m&)77hAMtALSZilitwn(t;=lC-MWMC?2-Pt1EIzybt21RK<3?}`QEmHoI=rK z;0EH-P|cHaa?1A`3E#{au95n=3wqkqkLyihnQg-Wusmm1)*y4@%*wkR$Edl6Gg$eI z0YMO-@yiF$04)D(=HuAj8W6%Wa8XZ1r;`y}9;` z-lb4O-C~`;x_4ec=MXo^?I~&xC-`$qNK}YD3TAb1Hq0DR0%R!vi+^7=Y5u1&TKZ*J z0FQxX?f&~{&g&Q3`=?FFr>3A<)MI%6Oo+C>_vi37Fo8T^F5u7OT3Db|b*d>nx;W&4 z0F-NB4#<}cg9kBa7dPak+>GQZiR%ebc5X~n;03H1w-yl)EDP3^;JT9LdM#u0E@hhz z=i0k@CLe)wP-L-y=mNYS!iya0)0A(9C6y+SP29l(?fD;c*qA^4)1Bv6_#Ho(e}nOi zz5&2+I!E|<2l3hs@^i@RdI-Oz8co(rm>#?jbsYujFQp}1K&Vb{0Su(_b}y%cA>p|V zgeO>m-qZl_EXZ&OlIRS;W_CY8{%p#%f2HrIR5{gJ6u?9%!ZRKp{2oltZk7r6Erj@(K!2*)n0} zW}oCv`n*wZ;X~{afE3(>$tD1!1bCuV`~e$1^vML$2l#`3x*?PQFtowdrFA4+AbB1V zcc=sS6(ad~r^2fmwj(N2Qm-~h2QW=J*?!8166Xh|f>Y8F=LHS7_uP11FXBrMYT~Rf zz*B?f=s|q$(k~|fpezFN{R8_eK+E7nAv`Wu@oEuTSx74U9LmG=e`4-kOI$JCRTAgkP;*Gy0?AL))eH=bTq&}utr-oDux^~j9a7OKpDnNwdJ zN$I!0Ec_uU5@(KM;sK=3PN?iMvIc==Vzt}IoMGUtop1ZpOYP{jyY*x?vxa*D978dg zV=5Uv3@qs^{`4YO-g;rsttgy-`)Uf}Q=5QbGYBuMFN4rvRU0p8?S7vRwJra!?FuK4 zd?{~G*@c5*iKFy>>QKbG_oM?%Y=%ZvuSrV$auEZLEUK3*k-A#BA#$oX(i}TX16fop*n-K>=8udFT$>xg zm&>`G0^0c5V60PUUz3aZQ)UUrwDCYk4Eey55mNcPfyRElAiMJ^1bIctw`p`aERviz zoZ)3g!ysntmEf|8vJzf9+t7M+JWZo=C@p`Gc5-y({3m;`puNMvg2)?VxAMt98x^Fo zN{=g!rKDsog){VxS=S-*t}4|XSGs~K63?%d8jtWRz76La{5XjH7&{=lss=!SA$%xB z?&{++`J9I74Q8-Mk!t*P8>cI~))BLr|JsvtCwmO2_HC}DV6TsEJkHM z78lmXy|?dRWn%AO$o+LbtLslk1-DnD)N(i=kx)BOK=V*YX7Z3PBIxX%VYBj49$229 zSl5dQpHEGl)!60DY^&5l4j51~2`3$pFsYCR2c(uTpl8LbZ~EC+zpuXhSSp+0B_P6O z3i+|@+Is`{(%G(~qYBJD^)N2GJ^yU&ZTLH;J8KMx1uUsz_t(xY10%I&+gDuA&52|> zI2AjbMUQtY9FJD7b9`KnM+$IMXUr-_C#~p7)v;NmOkjcvRdtkYQVhnD+v%#vH#HM? zpz<)tK6n<$0KqhX;E26$*vEgmp|a)|t{ZteOQF+p$?A*tw*>Fn;GS z2Hd6VOW2S#l%8K+f$kM>g6_Qk^a>^W#@bpkCsR-OGb*(mPJ)_;9tq@lf~_NEQ`}k9S0+(I$z@E3PkUZy{xU@C z^QNgKy3g5eiUTd4T8K;rJn%e%%A~B7&qdZxSrsDGh#bH z-?b67%-E1QhzyY+8g@BHGnF3#teT1FlyU)s_l`uMx)7vI{SRcrW?rC%`Ejwd#ll65 zozIIl?vc0ipFCZ}%iAeLvK^khZ2a~~^nLRfluRil!a&Xpc%wVV6-zaYFofGZG-tCQ zd_o#PsJaa*qSRpgq`ywBF8v-Ag(XXuhSIEq88}0v-q=+*gY7wyTSnC$z~W{A)9#U} z+ct4bzUw?Bs703~01ywTwQb973)`o0`*jYvvM_;J+qwucwK9 zR;(&_#Oc~Cz@HlT@sX`#nkKO@;X2b%Giyun<5#L3!Mi`zqpgsC20N}E;DV++$bne7 z>SX6fcP0BoD^wKC3?qZ#EiI%Tj8p{1R@KRn0*#S}&TzRfgjJSxrdm$=JXr7-?&9em z8r-H1!2dD2gMc{JJBeAvI8&dpnDYiLjwD4H4kOr~rn?(C+p{S+x3$6U-@f#OAyy?H zO(2d@L#LAsXg#90Kzx5u%~BsX9np*^k{z1-co}-v`ELeTu?5{^na)4!D%ycHHlXfu zKn+7Vs)u;FUtnPgU<)kE0X5j6U3=|5c=)~iMiua)Nc6JjdV0?(!(-Mcbd1j^^xVi4 zyF0^nDkBGf>9g=cXLk1@yM@ILgi(a&KunL?W~Lf7GH3ZdiZ!-BPi>3RT#c)9JGx&% z{hR68TGK6LSi~Yh3$G4*b%(^))B9t!ft)3Pxi0-KloD(z(nOY?D$WW6B;2a;a9wHBKLLuD_Z(fwF*T*?&^5JPP0#q#*_ z&}Q)4;Z3wknM|R{d8*}c)<VPR|8B{+Y*Gw5e@@%NeT8eTJ7 zg4%AOp1mxR1|5>=ULF#zx1V+wSR!OA2<|9HanEJj%`Q1FL2TnGh2ABc*5%%K1V$?5 zA1>@|H}Cm5UB`$tt3GB%E{!TtWW>6rF{c^=f+BucMHdx2oE0rYZj=gZP(VH&zt^Uu zcXGV`IZf5Q`S3W0!4oKUuT{X&GGEIxPMQV_3mpC0AMd zQ8wgC?FgD^cFKFM0DXs~82?c zM%oA+`Y3V`$@i6wEk!;DNRYPjme*Oya`8ZPrQxF2r%X?HWo6Ifec92!;M;MsC^uR= zJFly~5LD>HnZZOX&4}CTs=TTON`vm)h)v9R;5&c8kpi`jBlaGb1wZ0g%^9*N8$C>OU4^I16F$-Ur>pCeNut}>nDOVjIFF2mC z-PPiCMka3(X;{6vIQo*TDX5WhJGtTqs0h<6>-HDhvp?E5$MtPDP3b8WM&QDFK zQ#gPg$Te2GuCA-tYe@NX!qM(fBnD_)S71sRw5$V*^bk3_L>IJ<&nN0ZftSOMg2phw zQBQz8r(s>lu_Txl7^it=5+j7D;-1;TU3XJ8nWXzTrRQF$y(Hvfgx)gf#?j{cRvCe* zQ(xuI%4MZSSaVcY`}iEE;deh!zlAhn6x5S*Ci?Zr4NJ_)8re3IYS4o>E@tn~<>cfL_GaBGXJ*N_DZDB}YUQwWC8ae~j1fBRK;b)P5i;n4|V#mJdwG@qaDJTFs&g zkPF57{JCkcnutDP?BGf1P#iUl5FO4NzE8K_Xe;Y zQQZ`(oU{EN;E!delql6r%{#fhhCu%#bLv>EtFEn`rk}_IL>TnG`SJXwPB$K z#k&J2{&-KO=o}<8wJ^kXCHwn^&Iv7!k9|CqQoRM2MPM#`I(jTiZxqaTBJP;z9?A1* zoF&F)=?PsEVMS6%vYZMzriFppbQ&mNX9eCOLnicFP^B^|L^J=+>`kJ13a&(v%^c^? zdS#y5AaQ{~ao^ub^6eqjlXR3~DFBGtn{`7n6rmbT1gVk=CgD0BwnW%xk}i}$n6CP~ zh8~f`v#X8!d{uj5H0XFnw=m3Rb4M2U$yvqqbolFaKT!UGjC8G zkjCi>fC|*Y<4-j#4!8k*KDXvoJ0!q2W@X^Hc8QiyOK0q5aI6uOC8M2(uC|DwZm{fa z0vP6VY3D zNq8MZo=}He&v-@5s&?ZkY5UD+a`GD&>~bkd_ikaJX{kYvB+>{(&%R`j5~$(}(nI0z zc!7)Xp0c;iwHQ!}YRao13(AvFOq-%7%zQlma+c1;4Y_P>2-4%S%yq;K7HrC_5i*Ss zq@&$i0YY>FZWADsVN?zRPX`bAUL7Rv&_2dQ#2Azc3R$;3NPavk5{f8~j)_MgBNbc{ zkQWR|^g(78PIQAS&8+)d8Mu-!dC+CB_4s97lYZku0A6jkt9#bbL9W%5yP`NRSpj{6 zbcz5_)dc-|$7KY9h2dyt!{2#SaJ$;f1E z8v8sfqDit{X>}RMpB@+jCz}E^pYifaJo}M=alJs|hRiJGk<@E53jB((?+=qz%boz+ z92Lu+&KjPFE;LiV;gy&@gW2@@^kBTMMJHyYQQ3pEj{03J>u@P&X)*$(NMc}Gku?92 z-vt0bz_Bv$p@0m_Tq_3P^%|rCG%@Y9K>s4TTc9-^n#3AODfX01su@JRDEpf#V%CkL zj`fy|dKX^fVqMWF1DS^$@^4_05&E18+E71}U{% zN_Aiu1zIfGQDEGtok}BFls&O(&dQ^(l+&F%f2oAX23P6p`>gC&ai^C+=L>^R`rcBl zUxWJki+KjP=W0e5)aR;;9-PeSyDsMomk@PUtN#(%kwecj1>SVw#ODXNkfY+|h?8aw z&sFQ49=*&*jb}G1Ix-`J{&}R}(n{a)%7mvNA&(k#$*@aPgF? zaT!VqQRM>CY+7{9|1y-_=tARZq+qvPL#RDels$(T=oxcKI>p6WVmff35z@=v(N3I) z5)NL6EuH3;0TDL8w;4Nt4FNXvk-(JGBz_$42)AREs#-v(Xdiz491i0*Y$OvP96G*& zC?n+}xc}&H!c(~+M-a2c33M`LOrshwkt3#=sr4k_?}N)vfmSFFC4eVZSfJu($@Yya zBkl^(oGOdCLS+AKZXl#l6PBVgS?)?8M1y;Mi=B-HoIE71Js2tSLFBJQ9m$XObd+;o zub>ocU&78H^z#O%jW=t_em2`q6wBwB4nYkHV~6`@B&9QxFIXr@-ia~it9D1Li!*=u z?hqwU7Lrkg{@LKovsQJj*EBAG?50`#W8|DHUySzkSG52jKM+U#Zk=PyO)Xm6C~StB zNw3-l7MA`9kczQ^?R27DTCXTPq+=6r=Agxrksr;+YhxdJ6_x&L(@Ig*Uw4MG=dAxP zI#zsE^2(MBg9^QGODNvc)V)JIba?0|PXhDvGUJ7v8gpJyy}F_}MFxKZ1EZ3cUHr@kLG^q-n$Yh~jmNv~V4Cl)z8HLPgzlT3A+{CT(S9TLiPHyr#*B15& z$;?`7rPYX5wYe_S47C+z<>?hI#1SIgz_4A0)1A9C`;IiYk$IP$YW__W;(NH2v1@v! z+goswvcq_m(#*+~X8!rjz;!Y#C!M7b1$YfX7mI|5tX>{>y-+9Pn%i+Ad%D7tJE3%5 zhz>)*JuG5N*QNXRk>lA0UIu4#AAK$e9_14FxY=7DH4H=`(F!3e%W3?H{a3#w&?3@n z?X_wPAy>v3lC6K~@i-_UNr;fqsWI!%BU%paoB5hpZ%Kn690!a6=`Um7US~R|Q?c3A zs?OX_7*0^5aS>8j+2HbEE1JTHyG~IWSt(YFtb`3HPRQ>2{?beH!MYM7J~SM@irobQ zZO$vxe~XK9aRH;S-}!V)q0p2Th}MhiwI$28krhDn z2NOGamF%{3=Y7Yho+)pw=;8j?U12<#XvOFZM=2NXk{kGwViomVO0;=jm@HOief?sU z>(0?;>Y1h58t8V<%TEJ%;ND=X%i<8D4Qy_Q7Tr%1b^o&k%hnD8THn(L;C>l3S4hks z>~?i1D9WGZ8W8CS`S8Umxcq z1*Q&ZYNFQ#v%zj0IFmdEa1KJ!0*a2Y7Qc{UADOnE%W6e5C=KqYB&UbI;VZrM#e^UQ z-rVf^98OxI47~fL4;;NO+ZW3OTuWrxtu{FAwF=Zwm0rJjY61)zim8Z)c%V9{-*cK; z1D%~$%S&{A@n`B&OR^Xfqyhvf<7|IfcnyB6S;$nK>%G`P4Ra3#5C#$HXnC|4%6VOP z$5S4jQaE|$PKAQDYS>5oZ#ChLd)}=WisYOze_HestpUYNMv%JktH8Z=zzeg6HXDC} z6u$wAip2JiHp3=*851bF99mlZ=oV^OZ3o$40=o$d+f;RAlBoj$k(aq)@iQ*^V%- z(AUF%X=>`u0_FuNzt@x83eUSegLI;y#7p;G4iz)huG30CPoewD4mDsq*)lur9M`QH zRVlZxQCL;h6y9SG-sg8BlF+6s#hVu>{uR*z9^hH0hEBEuSn9;?|DLF@MRVfVbfiS$ z^AOsg-G3&o>1vemI>rAJ!F(H4`-gIY0Aldc2-;n)SJ$)Pc2XP@H{I{iFnZY8 zf!#uS#Dr#Lb}4a21k~bIzmWbP2XIvz;SQ~@Feo(ips9iR9s$>TmlZ1b3z0Jh+YW@m=MZMky=OcW41hX_`Yn zZd@93vPYPy7}bmU+`L~r>4LwJ$-C`vv2rWd6efXT1R_P*c`LMMjL74o45Se)$eYb_ zaPu^39o9{!QA_l8Big?5oTIk4PP$+R7y&^VFPo7wP=PYRKkUy?DiWFDYCd9Q*gvGr zn+Va!HA3jRE3?S(iFOpHAw^SYhT?-yh7tySk59u%1Z&L!H!ERrj)$SWDeX*<+p}kz zIRDya-4?`fPADD1ZUT6@iA-Fm1t%Og3_N6L0}B~eix5sNO7E0Hk0HWrMu{>8(6JXpG;<0oW1 z!^bSbiNf5Ef%ax9e#rz}XLLpx-GB@lB?%2|O|Z+Y9xrT~jk7E+;~MKCaI2K1VYXgD^6KJ~_;!KndsH zoPQw_C7Z;iW3c?L*aS3tCt0cCKHXa3euucy*mG0n&9-ck&i)wv+^M~1ldK~_U}?{U zGw<#uHDFj)Xd+dkz5l!jkGYrrt3`*ZG8L1wmT^#8ftRSL5xHy3%c*Z=tKR*xd;huP zB>X$uc%)vK`t=!I%O4{ANZMOV7D+{R6*G=)6-)$=b--N25uZK%JjvI)aZTH zlthgHT_i6ye8HYNLYnOvfpupq>Z=H)a zr>-KnO8wzy!25}ezZFJ=bZNApyT#lY6&sfF;cKv)Q-`H~ogNnDt}^dgSQNYvM*vHk z6;BXNeYI%L_?pO{Ol%s7jOF+B6V_G>+f?MI>p>rjP&-XCI&uuSp<0xpax#Ur-0W2d zEGIPX26mrapSH|}rY9%~;NgVLU>1HI(dy!Ui;G(mtAS%0gtt|`53dz9?EP|wS1U8K zHRqohM{=>~jz)W5_XL^Bs~p1hmYd`_I4UGU&+@5pPy&a=8i3-TfGmuQhkyy?|5v1aV>q(FFq|BlQ!_1jyX^?0^~P8q!DkR*=c#Y3T9Z<#qnTYLxp+g zn;)@qrB@k`8jXNe>~)c%?sEF4AAY_G?wrp^h#C!fWt;CGCFTy!cVyKMUN{vk>bh#! zqcXVi7+_U^_Ckg+_)}$6EOOLVbv6FM*bbuX=Gc|n{u~VHm%}+d zjCM;1DGPD=NH)}X5gfBYU?{Qk4d$iP)#Y*qiWgJmb(U|Wr8yInFEO^$D1LhhKr7D` zXK`SYr+pV!q9G{6X{4{P_Xhg8Vb_7E>Nu{}Wty*Ra6> zc-`hcWIMAkLee0&!6%JuGY<|l++36S}WoZ@!)rHlA(NYpcB|4;#(ZW<9x8>IC$V1;+;eZLWTK`P=`sjJnmx zQ|rv?OKy?YR!}Q+NH=F&E0>7=?;vvm2$mu;oJ};Y+tZQu410QTuaLt31a?SMaxC$< z-W9HD06_MU4Hill3%Y1ZLV)zlkrAg%E;{9mvUa1y?F z%F?>ridjj%|fHxF{hTdX>n3Z*tZitv4jlx{DUTp9-M=NNJ5 z-#goeXoT(*cxQkqZ88b7*4bt@in*43ZIDTuPk|QT8(}bE`u!(ke1_?;3;~Sk?p{O* z$?u~=0?S6EI^fDqDe<_N7b}WnDG?!9W8{xw^$^=SL#T;^?6{V^xFmD zF&rZ(-I{J6Ic~;FF`?W$40v=}6kQwv=e$?Y9okn7?rAYOSCFN9g{dGFj5yXT%a}hA zst;=(IJ-zIS27DM0H&<{YNo>Mp?Vv z``X2>CtC)fj$M21=>}mQoV1Cb{`OS9xAteK{8ly#%WWhI;-yM zwY@uX{y76Dq0yOIOS_^DbBm*i-!`6$yM+;Oxcm;DCWPC(`g16o#!A3B@Ej3Ct&SRfHOxJ5 z^`8Y6{WjhPbI-LEfkw7XyFRPy>)I1$w`Ndqme=VfFz)2(k-udJT`(X}cInOK)(UkV z%_&0Pu6O1MQeUX(=qm$ly4Xp8_q8%Oz$-e?~V zhMWD$ybvchIOvTIryWEt~~TL011S^{12#DWJkt)7vBe^8xVPTl(>0{Pdu!+{zIW z0c}o>f#5*b?%`F({j%fN#_8nmc*p1YJTdym!_O<_`m-Hx!TY0}uc!DZ=}IqK6qYK6 zq{$fIZu8>MyuH9vVI`lCb%Eq=ZStXmj5Bj$Z=Ra1FT;&p2DsnjTnuxNxeq@MaWbLp zCGhOb>+aYYYSRnn23yM2TyEqup3b);6~OE;KqDPA)1-jbjrrxTS;MqjWBWG-~pF9;@pb3Y(`^ zv)m{#V6Vv8ImBdCYRb%fn2GS`G~#%~GVl#om2X^GxT9&_R0Pg~3Ow-NtSD0~gLgKzS9kXdQJ$iun;+!Q(;|)x#*UBolZ0w>y5xc8=So`D^B-ME*p38!cC_d){4WDQ z3mH*&$tSGt>$kf!8y#;2XLeS8r&ZG17c?PrG*0@vky)%I`}M4JT-!e!1U&WCG{EH4 zY@8j2$~007G^Y=p#R`}2!u_*=BgQ%N#;O}MO_!&~*N=Mkk;ns>Gy$OjgHFrYiI{7` z5af>g)tF@qx7K`sG-wk{ABvh;R0V6?m9e?BDi=~y=I-?|pc!OcB<$U21-iQHla(mv z33@jq;AFLH^U?vROk8$LDhh#J1k_uz*3R5M_r7zLhRlYX`V~1Sq2EttvTm2hwr*L9 zFlYo8iwFwBuwKElZb^fLh)}l&hQ(ONMge2al7e(0_S4 zvVYGeH#BSgq&0RM9ch*@_626FFbVTSZ3z(#hqfA|ajyBFv3UHDrz?$dO;RfS?q%36l zX;%&>9+7m1WfqAn7El0l*_}lu@4oiu*JCtS4q+m=Nu3Nn3(@dRs4?0$pD~(!vA|SU z1Uc@fp(2YvDcF`bxoYn6RcOexTsDvc!Hn9g14MHJkhztWO2TJwZ-3-`mxFXQLE*=W zFuC7lC?@=ECaMC)0UqU=KvP+q*+Sd(|1;YJCWdRu1!+qGcOsx2^YGtiM03hbho<1O z1EpXg`w1$;Z$^zk8&ffv0UWn?au;XXe~%A*nT&L8?~fD*z$#UhOd0%_Ef4Z*eUAk6 zCgUOy0=*Q2rbc@s+wE6szbdgZb_vYN=LygFx$fQUw;oqYBQu(}a^$ml93K*yfhek!T}^YW>i%s44tz0n9T`cKbK=JK4aulp8tp2qA>`i>p)dDI>8aim56V)c=j z@tO&Gtogoez7MzRYN6_hBSglD;(MQ6@_F<2^?FR7FQ3GPlhBt=PKGP9=y1}O1xIk| zZ>+L2`{eO#+Muoen?G%HW98Xw_qw=#KJN4uHUDn!_t7m%V{I;}!BvTyfdi_|l__}B_q#q%Q%B+JQo!4e#;m>? zp9deYET7DV!}`?Tn>yX-cbm}Lr~yse9Bf4Lzd4;IblQ{IF13uS^*c7r=BDb-GdzV) zEYXW_p!I6g8ljxjlj-5t+Vw<56>T=oHjU0Y*?>mb%urWl*{081mV3cbvHgZu&R(pB zzUS@bn^1FS^`6hIvHNf|)#r|aPMgkt*lpgM9}S8a@NOTArdl8QqluZciXyaK=&Al| zRef^(T@oz*PwEWPzgd}4O-+U)a|7;cYxiIIK#Pm$4R68BtSs2UpjyS_(dw`1WR73- z*lll*)0>U8_D=5#Hm`(SBhOUQ572--T~UQn4GvH1`+XOM9LNh_C_lqwxcZZq&#Eq& zAd5Gy%Q5*(g7Jf^RmZ16nNYTuo!U|swVT%6~EJYTH24#&g zjJEJ=twLYp_K^L0HYIR-f894euU|~2JFkOG-QUWvZnvh3hrwzW7yaMPW4hDR1^V5` zkVTcc+S|*m^GfrA6nxf5=ZCnl|CtqC0+CY~kY-r_mCDj4OjDo|6l4Mq4vGm|-fm`l zn;)kqEa03E;cz>E}sPQF^#|z>WdG zxB+J~T+X@r7$QE{XZfh@FlQ3e}|Y?R70J5HC%mEqm?xM*3>U1@|1! ze{=IR1g4@af4<&V;Ld+=K9?p~Jevd0M}D&qPhfM}%0Wci-bw{RvddATPeuLl16s>A zl$D2^hHaGuc$=fNc=(NYgAT`B0>>w&4}L}$vD}$FPg?)wmJ-frP@YfvKQ3_oXSJx_ zXwABdD6T$M6tesef`BB+ZbmE0EVZJI$Ekh#fV+q0K7fPYK-!?-1>}?17Febb4*8G!fy;)syS@Vy- zB!pTt9z>MBBXro8Cb`R7XS1SxdHb1Hsb65RsO{Rx>CgKLyXvN>&geYUDgUKSxBr~h_*Q?8(Vu)zYUO;Y>GdiRf>Z%7OP1*?*=8Sd;oq-sP? zvSrH}>|*)6Hf15|Wv{?coLc?>NWl+Y*l7 zWK4Ca)DNvs82A|yrm;R!FLK4?DOfegyIA*aIq+9ps$pfU=6(TM>5O5OtR5MlyH)k9 z?fV8SM_yOn9TdHS04yN%VC=O%tY&d%g-#b{Uj6V|MqbhVlE$3|Fx~!^T#U8%jf}gJ zMT4n9&|15`&Q~`+<%5gu3)N&ebP68k=d^iyR%HNVc3IZ<{g8uCtzD|P>rn?3zsa%p zzf^G418?Fj&OzWBDINnOf^nz!a&rAyEXtPmrutP8Arv@#LW1G7ZB#3SL(mH^JnCj10(O z0P#oBFQuc1b_AcE?}n6;#U=F+rf(5}X8_FdgK)>5a( z%*af4qA78M#$xpOp(=nplcZZ!^F#f5Jgl}LSA@yV@F zfpDPqQOeu&{r6GE(>lz=`7)b5q_;ZXZ)F?NuYt2_5bC$IJYy(Z<25hIW7jnqHdV=sNzr7C;g~H!lzT0Kqx&;YS*Y+o z=>y3}n4S>yq7`tr;ET?#W@sF$@CMNkTxqkp1XK7qRqHHAFxM$EpuQR&YfOWAT@2A^+>e3zL5 z_sMBo&K8Jr-S4vF7w%CX+o7_1Oi}U<0@I{}H?t1YWn-#!tO7x%UDZo+Ednh+f`Ol? z-(|yVa%Of;r*Kt1+1f9a6V=HP(RFM3`fl{den0Bb>WN(|P&T%A+Uk0iQcP!ti#|_* zhL-HC4+oe|yR%r0J7_+FpH5aj12m0_-gV5>MGWI)U(ZPV`}BM3g(7w@$J&PzR$7@-L7)+$2`GnUiSFY79mQdoKhq4a=R9+ zwE)#SS)&|nnMDM<<_*>r81UAZeBplitiLq{9WR|rf9-L+4|zxbHDcu zaykn;OX{>YwrXqwC^&RJ9iMM?JMcU()qZ(lZ_nd#?O?37)>Fz>hikg4HrMd7k`^(z zw&opXEXfZ5kkHuD`d+H!$SLUmoGJ$G6%(#iK+?5|-~u!JmA9CEIMcf~jPEXD=slWT zs66{SRj0b`{DAWB6C>pZHfLbRM$Br%2cfWR0VK#)C;NT@aOe=JyUeW;gP`89#GZi7 z`C?tn)2qvKTC}H>xHg07pXk)O<>@j=r9~DG8%bjYY{S6VXu~2p!yf~1%}BeIY`)qp zj>@x*bqE%#8V~8tDiJ%@1pY`&iq0ZZu`NJN_HPA!)Ls`P`V|ci&j;hhW?~)VrymUD;~<^YT^B zn{x};zMjr}OU}V|clGIOVa=H&SGX@_Dd+6X`vD{%z=pFt-ThS?w(3O|`0GMKO1xaM zplS2Ys#8D-haaiGe3!3j`Gg)m)QVMI{%G~;OB_K12$}QlGoBSg!jR1e+}$-7D4euMJrqeug(5M0bf9 ziPgc88N7h}+w1LmPYhR`X9bc$(Al-37S~4x!?_j77drd?E}j z+z#n5P7 z8He~XtGWZO>E+G)>ZS*cZ^ElT2rxd8rIU&KE_?s#DoX$B{jisYwKwwXaZLR=(6ZnN z#E+3n>m!pkeKYrWPe(PQ72$$}-hT z^-1I=U#JROH@ke$B!b@+zRppP?e3txr|^!{ti0Ognfxf{@fVUuFiIVxP$3b610RHulLZy;@jT=x&#mD-V!qhAi^~s4-F3lV{nkHL+}V zK5crZ2`ItA&6B@kXgr>dtT$9;_4vX3{_(W#{sh0MzVWehd=5VNukj`&73Tq{-6Mhw zExGP8^s~6-=Jds_^HV4B^*etJD@_9LUdtk(@egzm#5!GwtNX#@(8q`eyq{3p=Ks3d zqU|fe3OJVzJSl6Uyb2Y6w_8%_AKPAao-s$nGNN@yq*Mbjn19WtWh?yVG`>wpwYf=M@KIy@yFx6{T6b{i9jV~! zeLL^$L=Gyxb-$WP;9wi5A?M8fjsMLi&pr`W+V)?}0GddMv*JqMTO zRwx%o?+6g4Mtx1Gwe)`~(5Ly2BmA&o%QmOSg9f*zf!DPPky4wly4?K1%abuSM2fOj z8@OXhkII-b7Bv}9%GWl;F8i3n>uP?O5dgqRy2fM4IfuxyxtWqP4PG08^>(gjzv0LR=l5^wzY$`^FnY9Xl;G{Q zKpk#P#0it%%Ng?C;Cr8hzt6Pv`JY3iXvI9u_4=ngpycJIdQ~DM6yr)TWIqXIHQQb5 zqZnL&UxI>QS~a&a$12=lm1H)a>Y*`OP>yQ7y|$f(_i+{DhTABaJ0}js*^I+M^g|pL z(p04Zny+D{pV8*!ujKCAf9DKmKUi?S0wLpM6?DrguI9%%ak47--Py~_o;Nml^zStw z(NXfHfF-gsSMR+<6w?GaY1QkE&ck%o@4|m5Ic;^Z;@1Nr$to`x>KmE_U^E)#irmJga zs{7P*Rb6w90=(w5E&bHm6sN=C*4i^+5xRU365+{u?RS66`l}Xy+Q>~ndvwHbs!d~P zAJ$M%?rF~AvlXRcw)4qCCO*;W=@H}U%JurIRK9^Iw6+h%S{k@QqRuz-y0hRq|BD5O zK8MY+j3qo{d&yBX=P+hwYxB+wn(SzJ%SqfNyY%X?^F{O!;hjqO_Zx$`*FkMX^RzK_ zhm^poX6x0F2budPg6s!CVA6rel7}`s6@EsS}P?om^4?yO7$^|x5h9-HRSyr zhn2adnPuc4nrox`_}RJF`6vIWd%~4xokB!V5J7fdYd0_+7K!#J;0;!^v8=33V`LHW zoFUosj3YH%?r^55t4cA8#A6I1OIY!cN+4Lw=2RXlQNO54m~?BPy5=qC3NADYKkG)m+@m;VKHN?YuzJIRZNE)u4m9%ca=pYYn#Z za|pe0W@G{DZCo4B6|}YrO-xqAhfdBPG%`aw!=>G`Puy<5@x#K;O{R%A6s^vxuKgL^ zd}3aiFM=42ZzNY1jUN2%BZ99{v!wXjvN{PSYV}w_%w= zQJ&njfJI{qa)Vxyvx9>_Uv_zU`Qzi`5me8cNdn`vpWO%mCPf}6!S6exck(v+@og#J zw&>m6uek-4@C84#m#?ropH=JYw+D({+=sygk7vDPmEZ1%V!Y6Dn2KB3O1;ivmXQ~y zYKxz~<_gsCy^exDQ1iD(9j)3;{SwhE{%ncXxDY>hW^$Z7VyKsm?ANNv0#<#38%R=} zhCz+C92w9JSY+D_k%i|8P?H;BZ;=QowZ4IgF=HYGq~d3W*4okk6AqA{ZK%FnZ+U!p zs=1YS-C|A4hK0qM_nl{ZYR=?jK0lwl|32$!rr@mo`OJ9`PxHa4iU95RO*NChAXObX zDr&~l_NOcI7y4h!*w7Pa(kF znlvzB2)IB7Gi!34>;U1ulyR8kk~X@ z8E5lmsI|n=^u*)2yWo-^ERtcV@_6(QhcCf3gT`Oy2 z|K49zRa2AVOBi;(Qf0sO5|Xs;a$F_->Y)tJeeai)Q#ChMjtHGmRHQ12)JOUhk{c96 zZj3Roswi={Tt40A^-TBv@@L&;8^LJ3M9muyhFty2uX4&F1-H>B1j~@U!yOL|U_L67 zb7^MI5GTNt$FyU-J)wu%0I#OWCo7*XhFH^EvPt zHTshjLC>h!WDKy~05G}Qp0o>d&vY31ndfP*-%qW-zfOOP3!7KRzV(U0+R>|lhvdrz z-)~sG+|cB3(qLn%HXfhT`D`Ww=o^sSTLZ^y8dht>U+~W#7RTT8zTlp!&N8!S9jVXq z*c^F)>>^Mx(KCbgeI(Dw%r9o_8lvF2AGV$4_Skwz9eYzzK0eIFa(5>Mu6H{W3neR^ z=Igm?nCv@lsNUJ}EoWsmA1-drZeM$n#v&xK3>xjkx;%=7L4wN&e^GP*hN`0DQUsip z>(ogKnyy>t9dgW?N6EaxAx-E7dfbvKA;qMs6_x$>3m2*E!d<1bLiM==6vLBc}DFLV_sj@VChD@FX#2c81H7 z-wYw9ny7rmxUd7$*Ne&=98-&-BT16h6k`|=zHSiS3nYUV-1OX1kw~8U2dxc^_P1GW zpZ&l7xc>-$67tU`WkJ)K^DZtViAb?F7!BXJyK_>lMbJ3IbNj>qELtza2sR;y7>qK? zFaOD>2qX`gpPa;?;Y}&$VPVbvs^;Vota^*pFC;l^M?p~)4W|N8zc?DF^NS1J>|0{a zq)EPtL&)vi;PwfntQPmTQS|;^r|%Y?oo1ie=smi~S~ucEk?HK?U%!dF+@1W$x0l0! z_oviWb$g)#hDB6n*`0n*^S7^3i!o}JC zKIF6Djy&d8V{vkHz8=-|?!bBA!>@lzQ8=}xg)4p*P$cc`)`k0-rVF*V_pZsD8rf?* zo6_$ zlzneF|MC$h22lBM9uq>J?I%Pdua-*2TCG{iErE10MysqfZLc$fYRO8tGlftY1rAC` zH)vEdnVxl_PcCBc=P>r~CO+7W_ZGN;dKcsshEC0PNU8C%ER0eN*YY((IiH@27!>Ff>P;UmJ)$n6l4Ska6fHu4Ra+S`Ex(R0B`F<dAcjb6D5b?iOQn`eoco=(GGW_iZ3R3FC z^;oq37C^m|;8s9o7Wh#V>ia?H|0G`%@c%En)BmLE|544r|67)+|F`$?^bShAi=AJk z6WQ|!@^KRtloBZC)A9XJ`Yvc_=zr38#aZy-0mD|y^vHtuudw{5N?ZInG$_^8l%ce^ zc|Y+#bNG={(5i=^i?B<*x4Ws7uV4oxk&KVDUH=+tX~@@V^}Sw7bre9wr|A?asa>qd zEf`H=i#jVt`t9jDiP~6wHQ^0o3(ia3EVKRSQWuFhW<%9_w-OA+i4?nxIfCB3&rIas z55zU7rD-1@2t(5dAGmOm|6#7OiV~4UH)+g}Asg3lx4HdE8S*PotBW)inYpU1M_uyv zWJT_{)472JUb2bi52sP)u$nu|YET3McBy?D(JAsQKW2Xmql zKRi3d^}~GxGHp@YtlcPzZ0C<$DlatV7mAs0epzF}E=W+#Cdb9rZu=X$g}8>IsX(Q_ zS2NbtM^2>Ybh?IN3kK5qvKk&Z+VCNY^ zfc#$vU+ACH0t6(^PXHTOx8Gtx_Bz*&xxC5t{2Z-4?+3Gj?4^-R>JQa;_e6dIIWwRZ z^(E+HU73-5w-ZXZKSL@l$Uskg{ob^0O|#7N`Eb_QnC|lzK`m$6F{&{f8!WD&eMe)) zvqWz8EL@(IEej#dLf3;pZX3neU-5(_<2Mep=Euv96zHvi0&=RrcJC)uc`F3)jWpA` z)5*GE3(wsI**6VmOfkqhJJ%+SOuC9O*^yzaS+hF))m3-|2oVH9g)eM)n|JA!YFLlq zV>LOY!q(%$y{wZ)-EO)u7sFz^&8=?$liE|f;Vz@TIPH_q9H-&>VyMlLaR7tSqB(1LHU0=g1B|-bzh7d)}zRDB8m4op&>Jn0V;3cd$KY%%N zJh(u}aUm}N<K#JaF`!+AZ5a8MNnu1UkUIX}-yPl{j%M&E`F@WitaKLBfO|?Bm&(AM zJ^U#i4U!))C>gNw^yK}Hpsm;2UZh}=R5PyF8@h#X7?&XB-tr*O9dyh=!&jOv^y7N` zPjg0MHeyWZi~7gBpC{CLWBiqEAt*~i<6R)af7sKJHBcu5PrR8LqJj+6E>u8D|N=*TtxZH;?PPC=x{F8*Oqjqwt#Z zKHz8CcuKHwjRwmi?I8D}(Z=0xEHS>Al_VozQ)b8qFWjbo-BEiQg>v&Yqq9#3n>XNv zM+=*Oal8jETW$1BFGltu#ix_O>H`EsL=*wps?jIFufTjfXl*v&n0WIs8mYKGh-KF7 z%M)<#!+i~xBZEFL2^A^3o6v0s7HR$7$=;4p6PX!atjRyUnXe+wHMaZJ4Bib?@Rlg} zja2$8LbZuhW8*4@NDBcgZtN!!nMFLYW4P?#>N|IK@zKvZB_R-#0OTsZn(T&C}+UDB4LIY$2Rs+3!F-ugHZqhWw)Gw_CvSS%vBZn%-Ym&G4tbuCZ{}0 z60EuOOfrfQk%9Ppf}(i6ttl2#=NC#BP0rcJ?`@v*f(*YiKfR2yUa?+3H7g*ZjiFu& zxg?HD!14EF`7WKQ%yA3IRL{4E$ugzOpX*39nOdrnIy5@lyv2N0JJ159)2krQeLn=W z2qazpJn+D6$s!mL9Pc?Lc2Eq}AyvXHS0BPbuf@BK2N63fx?G%OR2-TunK%cJ!-pR3 zSfj0=+&2Xa^|Acvaz3LdL@_j|cjPo3dd*LrPK zrzi*wvQ>>}hx`lJ-9b_G!+!>}2nXJ`6XV2SJv6^vY{fKS50PZf#)xC1Ph*7e{z77P zJn^RRDY%Xs^M1pA@g^8hJe1|GyOvpdZv35o_b9(j_fKc4)k<=J4+#SDlkES~nZ}KZ z5&^NK!ii)L+}gURT;F3)q?qP4C4cmZz(6JC8L;7C5E>ZHi^GChokf!1Ux029qP&E- zroV+Jx}Fsr-MNq@)hH0-bKBL;j#Y4M%tx)>n5wSUO+p z^=G*F zqc_vJpT!6-3cF#mzx`8}1f;_o4SunA1x|Ng*D5>WA0FDpOl$}`ys5f(U|L@6kh4GM ze`gI2yScgAI&?hEefN6YD+ZEQ?v=2RShVpGg{D=lYhy=lyyz`fU~McVfr8TrsE8IY zEp=RWv=jI`)OoD99bL{88oY!2A26L2!N0@LjP85fpnGb<#H}gj=M$3n9Csi)alAzz zm_@kX3b1}SsjV|;X6w5Zt~-Qvd>w`~x867bt5EHRahS!hm9$D)abxnr*DN@VhNXYJ ziI#wJDQ3NC@XT=MtFH^(jx&0Y3pD)kjbKN7Ip#s6>)-GR#sI7%t6?G%%D|zeTZQKR>AyNU|`;XlML)&OV z{k9I*OEbm;gxxTZMtxhz@PYAbG&dm$eyQl5s#y#Eoykck8;T8pMo9^Xh+@+^=e9M+D{V{kJ*X@QD{!cuwr%@0Em~~gb zk~I@G*^u|#d-}|`tbpUG?pLK#E{8eO3;Qbu7gj*QaTa!-OL*3!=OROm`V`p`P`BVB zEcXVfaF08FRXV%+_F&5iwBX&8_{Y_cT*X2m%rg5JJ7loY< zTO+()b0TB*cia9fV$aTb9c0zynZ2i!9N&HfNG!A_+UIh59IP4vT6G@Br+E$^_xtEO zFzuI#v|rl2ME-}8(~y}T>!2NE9w3(FDI-vwFe;!AZG#Wajk?T)KJ15KR!}|J?-9qK zoGkL#v_pG5SKBWaM82;*V-@@npNgm9&imuKi1ZZ-y4P%M;MB%Dc%+Q|(#HGXvMSkM zXASScWva0pd0~K_@kpLP=fRCT#Q3tbH5*uC>wfKZB%xtB!jwj!gZhoFQK%+CSd5Rd z-ax4NSbzqR_~_7ut#NFqiH9(fV!ic2+B5K-P_0@`S=33qYY6S2=Vc=Yb12?$lw^Oz zX-Y|WHX`MHf1-M~TCU!<^|!?><&A-tiK+8!+-S_LfgxaRg-Ekp?pDq9ZIPS^K52fR zWcdNdAPvJ%4O8nrUuIo9s7CbMh+YgCuH7$Zhg~slUztWb%++i>U6r|iRI=lITL#1w(RN6P>Q5m;W5sFK*M~#hcYe z^Xof(<|r_6t!-q6$oG#{m zOFM?b*?s(JX-P^^vckhICuXq-J-!cC9BnoBNfwB;vI<{)09b|z+|A0^jN|@Pm+nuA zk0%HvIJELjf!#>g{m#o8f0ZXb#23hbNViqjy|Sdd(!Ygm1Kl{3M0Vq*=-GVO=II^f z;r5mh`h@A`9zxX1q(FA52BN*6jkYz>w#F!Xm_5C`^#r;iBD|}1CKErt`JH{8do))? zxe2f#8LYoqyWF=aV|I>Mqkp-d=&qAcgh@K4nMJ$A*9zF{=7J^2`%Vx_Fmj;H{o+1# zPkO(t;}ir4>%RzGZfGMv7TZ9F_6i+VplI{pJX##l~M@$kScDiMF2fsl?T<(9q zznp!E?D9Nkf%${f_NH^^dMREMefB91=8k?Hqrr4&U!0Fkl)vaz#T8LJ-j`_{IXxzZ zT`4Qh1(EcH+EYqHVAk^Vm2{c8>l$a50wyoUL9v~S@X$p);cQ`w*-aF!#D*%v01vRm zS2hxyKC5H3<*ZQ^N6EZ1rJoIC#u65*AGfHGM=~#)sA?ox>WZv!AM|sfo_pN2i*9GY ztL4={hkM@TC8wLY^W1*_@rw)MLpgW&w z^uJx1kkr%#ZnB?vF*Qo{G!VJPi82D5wOU^_4fBRA(%?z5dURHD;iS}^N3kN~Ud=5u z81fOhNJ{LLBiQ9KETvgo1dR{YzuXD2nhznNzMqy-@-C>?=SB)7On-`Y6n{J$EVQOt zS5ci^HJB;i!4qBv+sUL}O5Ach>Zv;$<7sVknZ=KTa{~=qNo4wRZ!lFCPp1zMZ6Q)5 z6Uel?S3gT>wv&Xm#GKn>$D;SF0Pxd7Pb&P$N4?uS+BxcVMrMJWOWb$(nTXw0#shsD zFMLhYEN4> zjr-I)fBwJ-|3I1x>RDV6uu zosjQdOzkr4(_?-_dcv>U`n{Iy-zY8fWbibOjpFBZF<0W3mZXP%w2m&tu}U%V>SybE zh&oXOENbaAq&_0%=X>^u@ot6gnAmFBTpMtu8C}oyUDv8#| zMLmBY`0hZ^=I_7H79EcP2KT0V@j|o2^V(IlzgDEgP~(=Z3LUm!uC;~UMq>K-PD6}4 z0i2NBKXaxgrY*P8R>k=YV_20-LSydLTbaiGn}o}si@%df8=Wm1$4+Gj4PWD=k2>|5 zm^bpoaV!?n6#QP=uhd#@j!0IPYg}g0X^BQ5;X-lVc{bLgd=Y$fveU6`IW3C(o04r%0lkDkS_S`RzI{Qu; z`{3!xA{OY^!Bb6;P$0$bC)c3VJVn?T#xp zqTd&3FrrVp)pvDt%feYT8#STIP)BHZnyk0vkS`uW9p%A(~oLMke`@ zL~*ytvgP%wF|dQYSS0=DGLiBjUAB1$F_tA}&506!45?cDwWxpZ2M=xT3-XU^^^oJw zVbJJGX4(Z9!?$Wx7G@iSo*kZuHC1DsqL#R|R&T^x0O#i1&Kc3MTB+HS=A7R|J;S(* zq30MSh5?u3w_%Qyx}hI9aU3FQf=0t^U-myq#T91gDFZMTUW2>k6Q^ld=ETy8pXu5@ z3s$}O$}Hl%nd?-)h;VAU=?DgKJ#k+YUcWVZZJA)z(;`t+%t{*N5WQpYdtS4Qb)-)m z+r^JHod;6CAc-P{PIc_6lMhNhfRLmHWNowqYR+Bz2w3P%=JdUB4>g+)dq$}`eQ%U1 z^ovX_7A3Z%?pH?i;D`tXemt>5XcK5g+kZsIU{;myPhLra1+N{O$gn#f}2 zg`qHwxxtn?3sVsKN%5hG30Xt1A6RZyzo;Qhz5s*c2^3Vk`Pe}sK_K;4^KB?MrGmFM z@q6Mu$6a3z3-;yvzottQo6x5smMcrDHXFTEZpQXa= z?7ci}@&;R&|5ovalP~E;@V5%90t!9szmi{>B+|kS^pfNButRUqdXPVGGWFD^nDN5f z%fZJj=Otk8dBabeMWo38iuN3#B7yxO2b=vvgJXmmHHO7bUG&@(aei};#VWaQskZM< ztnFkfyFfH_|D*F>%qM2GT#d`x{dQM-AiAp*hLb|b_ohC$`%652$J=7t%@-%}M>o#( zRwJh2Xy0~}j7F>-Kdzs9^TUYxlKC%_zl?^5HM6Wt~UjRGkt zLPAJPxX2SVaR;+MLi*$GfaxU(JMgc)MeM+OX*T0P;*TQ#_@@~8KgDjZJA!rp<8GY3 zRut@ix33nq(4@cvU7)UskL-UM#@GlumGr%j9)9tkcBKb;>BmRtnFHR!bdz7`KmYd# zaP&;TKY#2t@b43@MuFKd4}TBE-`$>du~W0&AsOIlTmgJ|`!(^_iP(E>!ib5R7rH

!1yI#m)-qqqfC zK3$Q(?39cB5&B-z|9IXVzhL#!*WPT~(vP@GJLdf+ z>fpr9#<|vpkC@Qv`ol+RP#B$k^_xCC8|GFUzDLH5;co+tFvYp6NopbWOx$?@ zCBM?cORoI)Oe79m{Mg*<`G0L5J^Vj5!}Tn%*gvIBiF_M@y&2bCb8~6Ee77KMHSL)R zA6(q2iX&Gi0rtVBjloLFQtL+jtMx7BO2?A$d_uZ_qCU$s7(#P3kMXkQMKzZ{3tuc5 zbJ7s`YCVcAvj1MtLos{R0&+bj=lr*&cr}t&n^+~W1HnvfWqHeAHfs|dq^Nj^MXv|2 z+drOw$nAg-<(Lh~TTgVbh&CS|wxsMc6jh46XaOFoA>l3GTFA&MPtWu3AM8JJ6(B$g zvg)7wSo7@M?Xgh2v!z`_+TNTZXl!LWk^Iy)xs^CFcvIeYc;x=P{T!rwRrGeY3;Fi8 z=SzL4^a;a2K1OjM?~y9WA?b>$qNx37yW{s!jpJ+uXHzlva`6*cJ-19)pZQwj)yh@C zb{VkN(3=vGdJKX=BRi(_YF5>2A-LM`k<$v#GJy6w>p3j#Z#PLYkRf@PoCE?anOtC-K35W-?54)}t7F>C^pdkV?4Mo;SeZ zrxlqLLTun7M>W{I(sj#}vL|LKwU-xH4PIQ_la}bk&GYth7WadS%?bObd2w}g-vRVI z$>>9C({b1DI6CMRLYD;OH8^fc^sP+IeObvX<8(xKLJd)E>JGbk+Zi z!;a|AufMAc6pAT&`E%1>o+r9rl8Dk~D!UplC1M}og%kdXnr>amO4Ue+vsz8C-+alG z5Yl2RVo-{{&}-Dzg5-%b#=&zeBLrHJ&$d>GvbbraM*CXX;u1E*wn}PLe<&8hV_-d~ z(b*n&o?CgWtAA2ZoYl%KD}1=pkep@OKmMj|A7b6SL+DL>+W&j)`{{k)AXo%rRK}Mc zUORc)p6Z4fgq~f<>+Cb4{f=iT_xs09TAW*>VcT}doQ4_$Os&xFPRwKMqN|UDaoK%N z;WM4_s`Bpv`xiF1);g)kxh(oK7P&eZ9}CM>=tfC9v60b{3#>Gr>3Y37{*09ttM=sWs#`z>|+!9p&Sw$-h?$u8|eha#5%!E z6itq;)WsWTc`VK4a|%AOG30m-?;cne(}!!yuIuaGga-e8<51izEY1dz0df~^Hcq1N zm}nREP+C`PL_a+02jIvaR+J+R(x86l&CBog$V){V3L90V1MtLYN^}4tum6ykJbE_G z8LLPH*$Tx91?wy&D5oIG7-VLaHR`o)K2-3k5sh{7=SPcMK--Q~&oFJ~g)>RiMf(cQ zfjBDPdd;ZL@16|YWjU3H=n}K&_UbjRL7N;ffMz~@56Y~3ZSRdarQc6U>~hi8YtNtM z?!lm6OWN~432YZMpe}*x)79i7Tf~q5M*n;3OcSOkex~iWNb0*BWn3lGCM%Z1Z|a7L z9=+qHc@6^H?&;I_I`|UQ8h+`E>0f6 z9Gq(wJmVs5tLInF8k?Ebt`xL%o})l)lj(fE5_sPJuJ_f+`{~s-_n$5s4CU&7o}hlh<+XEM(6PCC(VfO3)u-_#Fpj1Z~9eEbHo+OxR~iw?@m4K+4HG+20xnk; z%z!;Enp6C7;cUk~pN+tooY*m+?UX%!!gY(;I3q+P3sc5eYJB!E?C+%T0}z34Q_|$+ zruTG}wNJqvUcp0$WHbJW-dFprX8mOow6xO!FE}798(>W)dtKcAqI#Bh?b@{2_c6P5 z=EWo!hkKrt)uLt@$nZ`Xeab zv`NPaN*I(G5|pPgxC}FX0z}3vS8ktfja1&_G{6V&Yzs5%WLkA(Haytj%?R$TR)d`2 zbdk2O^+v!%oD*JqJy#Z5F*YAZJPkU!q=lcORsMdxmG;}&ZqQ#LWzg~22pr|X0wa(h z-~LD@7uAheba>cD&ifdL)Q01hMLt*xG^|v`)wn!fTtQ}CW4VZK0zxDc%s^92m$nu9 zOU35=luM+^Amsb_?`+EX8qtWy2 zsGT*&flX8oVPw4_a9`Z&gJu1&u6mJG;5V7|X7@2|cthW~!SH-ka*Kr07xdH8@12^L z9SieI?VFKmE6%m}`>?i^K;;{^VyiWamx)ap?b;rba?dDh>Q;m)iMbz#f3GtUYBnO9J&-s%0Z+Cd`V38DmVHb-tUgr< z{#krFxm~Z@uV%v=TaRl6Hy-S+TFfiPTB>aO9nn{3u^Spd>Lo%TEX z{mI66IJ$9g-W#~0+VIANDcfCiQ05OevS1EsPHB&FkM}z5>P5Vym^cS>D9W0LJ;D&l zJ3eFsUj{f~1}q?g-S0cW=-GeUcYYNxMqB_$`Go9pP#IAfGm#jNlm-?$L%vD&`VSN5 z8tm|&#vmogB*@%K$j;8>mgE{zIiq?$~|8nX2KN5@+ALLrH72;xoDktF_8Mw zwSz>iK&J4jCo?;jW2XG3;~mEJ;HN$R=m(QM=qUam@$j*3qOsk)>c3!1qc^)lD4vFY8>H<}vnFntjG{4n1?A)ONxh)#qbK=-AfJOJd545}__0RW6Lm)*F(dI`H z`DWRmMQ@xMtVriiZ`?k-z1c!U$H^Fe2ViQQhD*U))>H~slzlpk4#s_;#-mZd_OqT8 z#mO7NY^3_T43X4y>6^&}$p5tl7UNo+>+Cf>tR{v(Mw@$8NX%`$S2~LAP5KlF ztS)C|z}nAQ)S3;ilgjIz@{s4L{?+S-IXPVTe$WK-<#6>`yS%-4PKgfowowJ9^wz)o zsgIr29w7{ZE=;M&LW|frhK8Ejx{i&%d6Exg-2`TQj=bLYA@IYV%cBt~W4-D^u;56- z3TadK59WT_sEDcwP2Fw$2H0T5Zqim)P->v*4@5}^<&Z{tGP7kbI?s1grRhh8iAGF; z?f-(@noKnDu%7yYdmi6UMuGcZbjVT60^9C!F4-u z%h(^%_a9-cJ>RPy*797JsM}ZhBwD@bFxq5+_EtUO{1d5}=1SN&FOI%5Fme=9Dj9{^E466eW8F+)7Vb7)&r~|#qz>f&I)lyV zl9DZ(e2jRx{b?oQgFDXt zWB)-BITn%Ycl%t+%xkJFV2WbcFdLc6x1d-Xa0mxx_}8vH#BPL+Mu!7{Mqf`xKjy}C z^;2Tm2J2Tm0~*Xe1lCet=&$$>H~!fQGZl^NUtQbM<&z=h$8uI)nC3B3*AR)SAme;Y z7=WHya>}cD!B?@Yi%=tbGt<-5kQ^_=bMlr-b44}5M8Ty)O{{w-6k{8E?~uZWg9^-O zp2qW4@~(nSotx)*=eQlHBi4g$a@J|M&tLijkS~HvG1OnERPdM26CPRSXnmL3lGjs1 z-Pjv}pJw92`Oe!>pWi-~Hg9tZKSc#Tc>vdXoNi2Kn#S%YeZ}mf#ZZoyK(fbc+52!A z%HA?jdrhJsQf|GXx{_)8(01Tnxi-TBi|JXTUZZ)5&n2VUdEV#HK=lc{qgG zMvW*6$8lgRe&0;Ia`<=@O6n*<3nb8&ecjN~#!gO|d=$5nBjA;N#m`DfRmeVY>8Z4> z7TOOxQG&@OU6m!qotxobQS(U}-!b?aye5i-L|a2#> z*B8u&7^y}T$tkUgkg+oBZbK*Eu&Zh4d1S#>pRalvy81T{ln?5RIAlHLlzNEkkQ=mJ zjBBl(^DFwkv+L*y>D`uD_P%q~dg|`}^=L)+FO?38I1aeUMU~s%N^%dPqbQJvfj%sXD{TF-T}MIv!s%Wv6jc< zBnt5FZ_}?A$DoC7e#e4zgG6PdEo!+=eNKyz@ZPZM<>Y>}0-swhznH8Hptkhxb}+XS zdXc!eyx1^!z94&W;Jiz9c~l1vl8}czOkmoQ7FyS6)rYF!jN3C48HTL6-B<_hcD$m$ z*m03Jkor)6I4SZ5N{iREt3ZdWj4aqhM(}m?!8dU>C z{eO93?T>Zc?Hklb9fCsC{0BuMn1)EG%&8{uX?xS=g`rgfS_kH@j*kd5Hu$YVuHHST z@3}YHd}<$-rhhIO+l+^f)gWO~nW2oK{wtqt81Cn=npLxujQC&fdNS%AXWA5Ney!ZY zxpyd)3HzU2n0~qHLDc<+U?E1MIN$MU&r4>re~o_)@Xv#P&6@l#c4bpKP+e#Pn7^bU z4eqlzOW2VDG)Md6)@XK}P)1NcWI~M0S-(}`7?sTNYuDtcU_n10m#k}bJf-aU{^xyV zB9EOd8vkl>8r5olIvv%y;c!KOHiG%9joX|Z$!8Qp%~$nbz+&w3uP~4WI;6paTWkNy z|Df=$9B13F_rqz3iYwS*qTvv~5c}}d*JHkhw@j#|eNWz?52pOrJRQCU+_9_kuauH!`mY?L|Fe;Ii~Xi~dB06iP>b_6fSIu7C@ z|D_&NI~$<%y!@=ahCG1!FC}jatf!OX%U3Ru5F=NVe$>B?zMkRg17}#-75bz z?+9VR9d-9D1>TLnTlKF+I(%cYFqZ;V4OxM56zucDuNMxh8|iw z&N|{#tqN#1&0m&-`V)hCkXc@?_m&qGace!`kR8y*FdwABjd$78UmbW}+(6Fh8?|n3 zUM6v&*D7P3yHX|uHBPQ(tbD5^ds(5%~QZ7ld7G7J7)rP<>zPd zg(fXcTE>9IFx4mh3G^g6TM$g}t=8#qE?cq%&94_F`q#r(GNm|3{^;U6fC@{uT%{yL znW{N&*w;Bu3X>SD+OYT`7`0u@cqEb$B6U=n`dLX1uO5AKbM8 zJT$kzbwUm%OQpu;ExZQZJHIuY+{zy99h#%s7{mJ+L79s5H^?INJURRTG24H-{Qx%~ z?HcR1`L&%VQg6Nsv#Ace5uFJhsaUdx($3Q-T?Umi_i0r|m`mPkdu0&x`3nFaJAFGX zjhE+AE*#kMx!FLLpKJ&*|hU9e(&^)kwSI(2O*P-Rd3JJ=OH7`9a(Cs z9I}+|AGH0|-r-f8{1&zFZI+=h;6y@f-9jJw(fWmEVeuju3@t{`^B2B`kLu@MLT2b} zzv?jJNO!kf8eIHwVLnDk@`^e+EHGdMFX;emu7Yh8msEt7L{QX={K+%oY!Ym4<8AA> z^Xq?b1PQxg(5r8n<vYgP%rAtQm{#N&Jr z8|^kPvLB$Vf88&FJL`|C@A{~J2RS?aZcT);bLx10?)7Cg?Q{BH9*~=iNSkR)v1z=G zk#>kq^DzD$PiL^eWyrq;UV4dN>M!nX1_uK^{+^jp;%iBYcY^n=fY0rP>L_$!Q>hV3 z)md!nID`@w26HwT5qT|>`icQpZKx`!Dwz-}EPmau&sw7^^UYZ6 z$0)p{kJ2!>Fu3E;xW@(J4!bxn#4vO6^zbkAhQK2_`twL?6{Ah&>=i{qe!C#6wUs% zrWK&v@vQq|>a&9PGxd?6W9{YUdY!gh%af?RS#CDiQ`Y67PYgayKu-lPrgL~Pf@G7r zV~8_@cslW>q}L%k~W_7I`ruR4i5`3$D#8loXlLWG;=YE9)mvIl@lQ5bikS ziSb|_c=!Xa5BCglBdn87yY5V|WvJBDp$Di9wNjAW#flj=mN#laT`)2}7(ojZtlAn9 zqY5C?CM+V#^TIE9s`8OO+QvgL`m;P>+Y+N{Z_`Ls_KlvN=)9n&t6&35XWsUrM<@1U zlzgmB+wp!w-$kR%9sUtkN3fvZV%V-WDLlx6^#d@@WF43kjpzShZxt_}<=l}1*eRfk zH^ta8vJH{U2ikFdlQcG6?gGx;i2?_yU7p>Psdg93(6DCXY$Xz5CLuobZkpuK3S;~+ zCF7oxrdtS)B9|+V&l(~*bpBO(3j{E3g_=`@C$c-SoP&muo=H&uU4r|&KaQw#~aYwwEDoH*~GR7g1J~tZ1eukzLXhoP0D8o0;>wcq?U3X8Qr>oUdrZ*u`WKiWkB?(w|B*IC{%PEzS9)6(^A?T_ZE8=Uv_G zG9+_|-AMjAip^Ydd2lq43T^u*`Tg;-#C%DA_7*r>3$Z;JqX*A&dJDe&%$)o#>UNk!C^J= zk%MMGPyduY2CXfWzCU=Nr`m98zi;Vz(<*A^lX$y0nE>j5C)#G8-{^LEbt(NZTh+RHxxzp|8k*{q`mS1d@3s;SDY7V?PPF;uqq-ijp&b~HLItzj#_Ya_byV@lVrYL64N@QLtA7m@G;5bY z=Qv=Ly{+2*?)pg*6Q^W|Y->jIzJ4@p$i|8Guj1{oWah$%_!r15K;V$?h z^_LgKitd3DN## zO~BkgNKU|ewX4J4SMS9|I7+@v?8Er4E2m%7D!8O)CQ?%s=U&k5?-0QF`FMZc*Z&M) zv$)8U=CThyqqT>*X!*Y*L!b8cwXKQ6fi}Ue2-8;Fj$th3)5O=(y0hF8R?`Y+v2KHskzt)BJQ>K4D`v!+Emx zZI&1))GX2X(EoM8Tc1OKC^Y-Ib$aY~R^z;8idOd5#72`Q!x;+MX1)#XIoaLr1C=v!E8dM&k}DcVpD$rLw2=?uFz^)3$ub z_H55^Pteup1#CC9bg`#pLtRZfd?E3}UGRcag_oDQkV?k-FKY8?HXgEq(`I7A?EIQq z9PKEI9c_|IvJYIal^<83Fk?bR0!tZA1A;B&kyk)GeM(U@KEFo5RsyrfKa6r>M;+Je# zTF1-eV_af>n3KIw5=N#rK=kc1GG0hTUJ=Zkoh@Z`Pe+A9`|E$T^%hWZEYYGigS)%C zTL|tFT!On>aQ6U%yK91b2=4Cg!QC~uyS+)?IrpCTuYbM;R99E;>fTk<4aARO-J7LZRCI=EGqGqV%NS|6W87^2EVF_G%(?yCBG+ z6Jhpf4RK3~OVM;S#>z2BL&CXdS_Kv={Wh5JSA2(F6_aeZZp;>hF>e-Aiy> z<>&jNAE^ubaRTehPO>o^%lFNqQB3RvckFKYGtIQPoyAP-{b7sYGf^^Q6|~hLV<15n zmb`KLAlZ05V2i0x3x{5xLY_Hug;b$!*WqfFv#X=QJx$wF-8^#cc7h|)<72m_p((^!!ds2S58Np$o6l0gagca*AeX`^rXAfo+-!{!>y*Sas%&senX zn6=rkdrDz=#(2MQ%CvUG#~3FuBE7rY?IfKD6%N!N{`AlRxt00^cbHBmnadaS{!X|@ zZGr2L8R-4dbin#!xdJ5faUAiZCLJ0@8qH_YunHVY=Cz=M8Tp5O7mVf)Ym}Yna$Kdh zdVk)kKMTD_gTmR74F^$TT*1l>$-@#xn!_@BAp7^@53FGM^J&%Y9?jivl`@FK%KgO~ z9Cp>^i0s#SU3a=oeKi$$se*cjk1N*8)z$`}3rL$JN-GD#8=iJm6J?$UQ=iz4^HSWE z^jYh2!lo88c&M)QoR|QTwe9g0okuP8kjVSem~a4tiw0>O(~yYSPP^SirrGCT;kkL<#u$vSO_K}v5+7cgQ1uFN4m1A zMnmS)Xv%}LsdUK9teIT29Dz;Dt-k6uzA|O{5Lu&|x-%2Yh9hnfS-#3;*(N7Y+Dwv6 zWhCz~=jCNUlAxT0{5RraDTKt+=HtS(jFN_AHibtnxmm{6lfbMx^yEr{k@?WSVlcy&5{qd5qC{feSGrtdH-&fAunOFnxmCC62 zb7I?4WdeWL|A@kw1X_d*+|$bET~=5SHpy$| zIsVJ{Pzkz!P9a-n;QgDOcL{#!pupZCX^9@~F115X{a;I03dqux1hRC6vg=1_qDEu< zgQPmzpWFH&x8n)gxqmT0@>e;RUjS+;@gIV=pb{2?7%42ow@uK+_B0dTkz8VRA&o|61i#Lgk3qTw@ky zdCI(!Xn4C0IN_%o+rxo+iU8ocLc-yVV}eWdTi)2Y8RO$rc_*z0rIQZ&sp~7OUn+y8{C36^8Z*9$E?D6{`UkK3I*rCtuo)F z{I9w4(+G+ZhR}@mL+8O7vYy5@yGj`fJI9|= zy!o`?EywY-dMsPDQf{5AZQ2R7+DTK(&dO+Oy~xCgr1(>FnaP`^wbOIDI~B=zU7lVmlfxrpeEWdSY2AW-+sX;2ij29!y-# z3M~KlgL>TLb_{1`?Oz*gc>gHea^H3)e4?he{nFXLis{?ld$BRTR0x)4>}WJ~H+KAo zOD8YRYL6d1l-L^@6@2mB|KP#HmoC+&VwA-e&b1~_R)KS}?;<8>(CgFcBDTc2cJb%? znR~ntX49CIJ~46Tm5LmEQo(E7o!ByprBK0Z($ylRrH~%{by9tHzR9JxGHEsUKX~xO zMr(p3qeq@IC6Ef#skYE!>@cOojTgNZP+bze+vXB?OG$~sdKb?HF(Y|H~dbCpk!eo+Fg& ztTdC=UfrA{aO{p+H3UxJt}7W`jrJ^j!*IGezppNSyJQYv;Dku*>w{dlPS z?MK-QAyMRk^M7gsj{Z+|_B{UoX)boof14{q_&?2sPw;PZDUklBxuT>0X|98%HX-8Z z%|_6qG5o;_JQou|lp*_DmiNoPI4!xkusR^NPIC|9%3goS!u16&<`I}MDg?*f$`1vz z+B2@nT4`+0Cc6QTao04Oyn#%DM&lQoo14MB0YM3^!!(rgFA`V@oTXhA} z5Bs<=bP<_aqN=aJ1cDGPIAEQM+2^9|esEJinC9 zWvs6ln*-^gS1kv!%Hm!o!2FZb$7nISET_mj*PYAZEX)MN*D|{$bA3f}1TZR=l8;?J(0uOm!Gd z2i%i^sk{366GGReW|Fj{yQgdjjS}XQk-k!1wS&^=x_5r%(D^G?mzlx)CEvok!4q_Q zaUNvT7%VFanzJhQOXQiz#X`0AX0Q7_=m;xzxSM6PHMdR;&=3rO5e!JdOhsU*Sf-RH%#ybO0viq z93;=daP4(VVJ7ApGjm6BF~$r@-F@$2^6hm;pj}5IOe7^xkEAJ1OEOsBCA-H+zycmS z+53{;m>waGI>I>z4vOuE{MkOY@AVVggC(T4=Y<>F8O@C4g_k$*og4qg=Qxq%`{o}W zAvc#wTrb)H{tOgfogo!STFe|ii=%Jxc2QXsSd(miy+3Q+WFl3*a9bpZPJjb^{L3{R z81Q;IDKdGz3|p_=K{Ix_ke$KLH2I!=C=a0lq405G>K1I>Qxu;K@BzeSJlJJJBAp9? zq`kD>M2iR21p7F%Tmc0$pYvswEOyWzO95C|ydec@8#)N$4{dfqGn~#d9+ROE-}#Wg(=4wgcRS~vZ$KM;n7PXUt#0mXuK>X9xPcs4>edi8r)p z5XwhKimq$!thu2(*kT9l#m`}7^{nnvV8Aguouj!oKFxfm6%T+@jKEY;7#vs@-yp9K znziOhTZ=TTMwljdenDK6<4-4qj6O86Q%+$pb+*3VUc zFKDBgZV+0w_2?R`A*_z2Tsu7l_C>*^-nCIsK62;%Q$2Ac!#&n;s}9fxqnh>m zzq%19+5? zi3tY8HlL+TC5Jt;^}^2&1PN0MHc`rW4Q&S6 z6|An0Y^m;geCtE5dL-7$kD7@<-VsdxM z`Sen6@-1rJqY7NE=k0OI2(I|=pUkHZze$JQ3u0}{`;{1qqn0FGhyQU;{G@cto%wlu zTOPLSrLXNz;=%mvWm{fsoW}5bs6+%2 z0+VG`hI9))Gp^S)kIeALQ##x1(09l(ChyE&J&wqsI8XMAwbVN=YBV{>;=s`ZvgZFmIh8g+q-+zaykF zQNL@U{9DT-9q)~{a%-rc?*+*_u>A=QC%CayQ@WLWx^Y#|+|wf}UeM7vUa%rY&_+E% zgb4I-%Gf_}4p%hyh$wt+2_7#6f}bFRpJcG(sSs{m7-yraS|o;-X0$^R2!Xo~ zf5#IeO#y{LgWh_?y+hA8{0cMLUu^R*m2;M5_R;c9mHmj}*MFe+^9!Z++ZLR%Z*4!~ z9uM^MX88ogB8>veA`LIfT|a>}eBwYHDKJu4oUy5MgX8WV5sA#yXgdA4fJ~L|Q19P9v3P>Xl42XI1#m010wR{iRM{PCapsIaUszhXj_Lu( zkj{eh%x?HitR@%N+$jMcje<=0-FUK+l%iVv!FuV;p*21tv)h@FnlW;GnKuT8U6taP znV_Gc146T9e)%hS9!73zJQ)!>5IA8-VZq~B5V!ad7?6tPf{*V*amJBj@+vYz2Exl` zbkYoJ?ywFW@#DbO%g;V&W9zaaHXv{#0X>qvjZ184dri?;6zR%A z#akd(42i}5)qSs%z+3vI0%t?Kcx$gZgw*kY>^PEL##0~tIiX2)I~@fmlfh!B33d)K z-i-$@exb;R8M^78ite84Z)`yYg@h3bZO{H$-=rxK$)ZUmN)KhEW_m zq1{6s@;m(}x;7GgNTX3%2_ye&sCgXRGDyq^{EY-ygDvLI z8;&8}JZ~s(0PtGF0T|o~EBlfgB(bxoc3~X8d6aGSsxp4%;?P}_1?f;Hj2O*LlIuKgkUoWus|c97%wEfD7@9B*T+SNPVGNO=1bJ_4ir3u ziUqIs2>FF#vVoA>eSvt_^q6ey8Vm=8X@kP;goJ74Yt0}yanQGsI7@Gm?f@65uV>0uBLYZ9?i#P( z{54UNiS9&-v!kkSH4O>*v7n1w6rB;m1~SAFUh-{laz(2@uYr$ctg#~y1yYliMbvYO zj|VX13j=lhV%v;!$)PH(l9RQijG8qVqfo~NCh)6HqkpP11{t^IXcma87dQ}o-K>QT z67?Jlx&zc<5woD(V{La_d0YE!^3y_hAfesMZ-&kg-l-%t7K7my7`OFI)Q8+)?<35% zk($BpAnePbJ?~)QXadLDQveQuhW6lHR1YPW48RU(a!)TCfeVV_UOIty=rn1e$VckH z$FT(uU)mY55HLR+iW9NV64RnCsoYWf8<@V=i2A!h)C!|gs0EWH=zfc)z?u* z@i^Q%XyPZ;Bx<;(kf%mw$lKJxw;oW(#2<6Ip1{u#PT&uMVSN3NyLx#h28j>ki~7d> z4S-(1FC1W@s$qOj5kA2m!ajH*KU`4Vp;`+Zm3oStAW+h04S@Z$B+P;hB;=5Tsq90F zQbm3ucJWYD+T@drMsLRU+OiXCGy^9?tcS<>o^@?>RV7p2bOg~FIh9DHhc)Sdby=vEjT`Q0u>fR;N*C!Vfjy=N2P$4hY9m06 z96C&?n&X2Try?N8KVVU&Fbbog=-poeFixsYqzL~j%rGOrC&MZZUJHL2SsvlHDmHJH z2nPz5eI*H`MTkkMDA{6dfJsuS2%ZZ?_&q_DMEn&1OEEaq7sF(*>kI*DL=j6e7(5y> zTo3XqTtcR3$YA1_Sdt-h)u0HFWCvC*<{X;oboukAHmgpuLazGkIf;Re+cFL)bU$-8 zFn&U5(t$QqtbQABT8ig!lHR>AOj*b|~8Fjl?3f@S&HC4fn?AK6H)D57u!1CVfecBek#*X-YNJ114bCQs@-25O^1^= zrdr!Ev!9WsD!-)^bO@iLV6Eqg1oAPVt!Jl@7u~+~)b%~0w_lLToQupxw({QqNM)kw zTZEWE5j1Z*gm;L1thR_u*8_*zVB*a({KvkcF1@7962MZCPc*}{4?25dJi)dre8I(c zUz|n8H20yPqB$d}Q(j$g;ml9d6 z1d|~?Kvg0|(J^Iz{ap?wZDB&V$arY(gT*+~_MHA5F`BJ`u*c~`kkXGph0sv+!^v#3 zYoqc27sQ9H0EkbFp`7@iat=1}P9@vjXP-=J@rw_CYeqW?0mEjDj$jbzfeyA$dRxtN z3%w7g;&wR3zY3j=kYs!MR(A2$cQZzANvHFT$_P{{YGj}Ht(`v+4*Xz zdQetf!W;<$fFq?{s|-`z4pi z4Evw~LLAc#+zV-5grhV?S7_Wje4xdLN0x9h$@*MqO{TG)hyuC$ zkY|#UfBqh^$;=0|ve6F4NK*tm77dANe<(#A%V)FHe5t<1a_zo*p2a2xfx$AmrSEhzGBrVi@ zDe$ql4idC45QdQ-mJ#)UgtNuRF;`7iO78=a9kRR0!`BYWsKLe}u?_M76|V`hN7^XL zcSpE}b`_t)Q_0Adk?9j*Su{@wE6C&qe2Dxv@iwPnCgt^t5osS{jl?Kto%K(t;u5yM zaTp~~Yw_{%v@5>}8FtlyIpZTj3k~Ct#sDqD=|C~xL92+5e2t8D@skg6@He}_SVSHD zkb>x-C;8D~YQ-ywKdbrN%u%r9uE5Fr`Z3Moe((sEVY`6{_9_s4uXlf&D3|^Xk1tt` zQt1gq%nNpB^1>mrYDH%4CPU959`8=5^EcuG|qG^uEz?#{fC^+eSK&-t2+o9`0gbtl8 z_Q{nSRxM)6Le=O#)LeWaBRC#b06qz}@@|o5P!o~acohD;E|rW|1Cuj97N)??5H}DO zjNioq_UT(uKksQVcb#qNqS_uoWHhkiBd%u7904vZRH4Bq$1+2?57Z_V!BVg1+JQtU z^0*&8*0kUGvjcx^eDKPO!B>1I6gvSIgMk0= z2GNAlwVzKzjx_PKi5(;TVF7tEM3{iowS??|oF)(n-S7cJf=(4xMd;h20Q54jp;|5& zfmq^c!cL)T;2X4!q|d}-l17vZJ_M4r@elSL*PW4^Us`6f{4S1K=amXOKC3HO#f#j( zG}q}Nh*l`_^{#NDM9;fIlncb<#*{y3Le7WEx0Q4{aD5f{^>y4|IXZLz7bZnGbJP%a z>c>>oWWNP}&d9#rms{I&{&|Rb3}U zYDb7=l4AEr$4@H77Vuc|xg!|@DRim?TXnv8%hcBb+clYS401+lbeVx_{t@uqR?zB9 z5c0^3UyL?;VI@H`dZ0PIo~jbvp$m-piG=Js_>pp}5ho$_(DgT}Q z>!}*iYHy*(-z=%n-r`c>9t15gb(}NDQKtJ$1Sz$_T7S_$T3UK;O`VP|#lS>iSLmY4 zv|vdc_6I@cu!_q~J8)Z_N(8Z3JthKX9HJ73ChBmj-lv|7)D-L3OzY4+@G9?_76d-Q zyWEA(0i|0xK!fXxweQw+9Jtube+VC$0q66P8IS+58!)b#4AcPjQaQiX&8ut7O_I#P5 zO@Jq;^F$;aS)%bE>O5jaYOuOWrhmFG===|oQRh2TuoBlE%1~z+r%Bci;XC_{j?fuA z^Nt+=Yp-zHU`6q}G*7cXp7lDv@aQ#s`Ml+(=Z>}psK#`^R4ja-KACd@a;(tByzXi} z%<<*om1+k|?DWKf{ErwR8^4G+B_}G^Q{JW|%<9NY+oVuxIMK=I#^zYn;PF+_?m*|c z=tNB0(8;U_CtXfO))?w*L@0+biQ_FqDBDqujFtF{R(!&1NZzCVBIyHeq=2Q4u!N1w zhp3VFn{P6BgUTnTDqZ9qxGEMI@ts>)!3oI#Q(l0wJh^V3Mai1!0kgz%+(z2kPF6dT zU^4AAZvL*&wmFY+>RM;AVLOjeV?lLTx%$-f=YzSHlP<3yDww?_qwJb+ij}O^6_Xhq z{*W?BCF4v@H>~2x{q7DIizs&ZTt|cch<9z$wHp)s_i?V066~5qpz=_Sf1IcKkYT-G zupdmB`sR8-FvEn@zEg*{1Q(kI->`RLAHi@dL!z;NWN!hUrkzl#*DP+4p3AMjsUBa%!YR^L?*+!)r8;x;B$ zXg&Tq3U0Cf4F3T{=fwDj1>@QY!`c(Dbe2JjtylIms>JkltzWQIy3`;9iF7O0%(Fh~ zMGjd}kn41fNA&+5w>lO6M2lgz(B_2ZY_;%uFbA^5KD|OF=?h;RZyqhU3qf*8+uw0Z z7Xb%c!{47^Uf}dQUpaoq(s01GdGbAQY_pXE`wC0v&sf44CiueqdiSlD�>zHuFsh zSAXg800;I$+A}mSC}5B1T;&8~plc5+z*o|N`bpE74?Rq6H@_nD2ogtKD|R@fd9^;j zj6{E5Do;VkUg+Dp{G2y{6f4Wf<)Bn?Xma~3W^-l&-=fm54-6IzX_dN=BZ0tUTQXM# zcJxnRF?O+nNu?us^>&k3qZ8ualprk^Sl3ptyDM25xlcmWmdc75>@?@X63MzWw%wo| zI`=%Bk|zII?JWB}fmiZq)w0Cd5#cP10#CzMPI>xs(ztV|WnuR33#I^_3o8%4EJSnn zdqCJDGSltbxxT@a-l40We5vYVu+l7$7+XT?i1kz#o%f<@Qoe7;O z3z^9c`utBZP^m-M_4~TAt^1j!&9_zO8Mw7Se1=`iwkOYtfLvimbMPE*<9t*dW}*Qv z*~b82y_{4}=$C$j)xvg1Kh$GK~S)t}=CtFD>ioEl%iCw+LOFML2&CJ!XZME#3}L6o|8 z#Jy~^M05~mE%?eyz*TlMLl2EevY#zldu2`^C^NQb{bbKXnQjti)z`aI;o0VTASL=> z*!N{?^u~wumaiBS+z_^yzic&NRnWp}uof?6f}sfoEAxbGbEsiReXj=SPWp#jy$nk_ z7VuD!prvAf$QDcD=yWQ#>U6SQ+QE}73T(cutoVvj+dTs$1}{H9ryjns<8Kpm9;6A@ zhTDeQ?&JFX;SB$oV6d`>pxl;9Ypa z+UE1G$p~%|rsgx^4TH}EGnNlt{NE{i!xXdr@`kJU_J~fcSHO9eQPpOypYCm%u@^LK0?NYF9QnD#hFN^Pz-NmG z-xq~X0)Hc$wH^%LX+5x%S+dS3zSZ;N0@&&brCV;3UZ&UWL1(H~KP##KwKm?ietB?Z z-6lm&0#Hn~vl0)pCIoYMh1Gj0DMWSybX;y-^O5*n%QCi-ldW#ES<5NzbsV$o0YS2% z2Y#De3za0Kc#kg=XD-m@!wtbhwKGA{MdDnd<_cAWH3wNKyELlP^kDhsu=z$Z4vn z5;nIFcwP@Iqf;y4SF=+uYb=tf!ecr&#mBLlU;DUc9DJ14zZ*su{+C}#bfa&rY%1Fv zj_n)@0!gGfA^lLNaBeyys zgrfZK>U}PMNU&Y+Rsk-TfFW#{!Tho+8ZGN%T(bJ1P#WyXCc2>T?2ljrhC}FNVy{w2 z*XJqUEx|Z3`l}-Sy501RX;<0J+<{~?!TBU=uITRY0B3c2H#y!4jW zymYgfmUJv-W}_(Nr{~(TUz%&Ay=k*Z%3M-w#jItr4B=|H^T$99^je*1vq15s{gB%a zEFT3V+WmoCY2B~hqxXz+t7;`mWs!O_ zV65>q>8qUOS~%o*$*z&k`5K;`&<5thP}RCR&|Q30>`wI z^r!$4QzPe(`FR%b8PihccF%i$L_JdhOh)5!PMzix&v0h<@ni4Ewl6qvB{1bqQ8?0p zKWGt{$>UFni}~vhiLaAkVD8&jGIt$6e3@Pewz;szrmYwNa@EFm<#v}!zuWooBu^p) z8e}EFP2We>V1n%Al+Gv-BA``l;h zqm1NO7TDzeyL{RIdm?E0atJT(L0N+>LD{>idbdr5+DF?X^0m3V&tGbu^0x>NY*~wd zK~B`QZoEG~7YN2bxwst7m@}3eP<8@RuEEtE^#jTIss18hz;9d6X(`Rc;ac*O zyLV1X#KTaLjm+#M$SkcZ4#b5=H|mdf^}w7}D98_xW^C~J*=!jI$wy;aAB1$^4-LyX zJwv$-Feq0}NYK$Vuv$5Ex<7*6`^kTLVpnTXo=hNK{ky^jqKHmWuB)RMRu?`Ya5Toae#$I9|ur zW9(N|hA!3psmqqSgZ}%RzqwB?1hEBLv2bY@6ghGMQW8=bBI9b$wlEAECZl35+eJ5C z#ur7y+@#Qik61EjJwA-H{HhOVFag0bkPm85^Fg|_9$^7LdU?u4F}M+gcEQgOS-D@z zRCpE7Vy~nI8!XFGmpI3PRC(V@jh(~8!nqfEnMUs%(6oGFtkRO`dpukiqhovq$vC2C z*_CHv)Z|y9q8$$whYm0bkE=gOgpeDMZqb7{`o3-TT4ImnfHl?+Pmp+!H^=G3X>9`T zm$AnwuH0w%BsCh`Ku z*J9F!@1EtNiE3ryz?@LUYwq3o5bU>5<;XL1tkC7{A12UNJth-VKaD*Jt5OtDtEm?^ z!s6tlr(h)5=zmF7k8lW=JYZ0v{e_5xsvK^S%gI+&IIo6nP>Om&MR|;YPP$KulDpr6 zCti-{vot!c@F5s1L`4Ew6@2Oh1_o*-|b+McPUf~7~8YbkF z)o^7vI%j+v(~oRNRG;osQ>PD2Ls&zgWjlj%?bj?9h(-$ErPE8{T}F(zSFv*yrs!ws zD9q_yDuBi|C}FG&`_pSu;y5fWD{3A8`Zi; zG0={#`!BlV@fi~UHmM(iJB>FVomHyzbwDHPCF!d{*E`z}g&@wsF@>`g+Qt-^R zmmOsv$A=?mHS>xjLD+e%j9_xPmauEJ6y}OAeIckV)V{L?>kaf6qN)m!1LM$d#4m7m z0c5~(CXAR~0(t@<7Go{trm0QvhY{G*jJ*NOD@sgWbrCU{+q%lTuwGb($aB1xak4A$ z44W!st(j-k`7Xlrz)Y;S$dNe;sMT&4{!^M>Q~U|)z!sO$v0rM!%-3=0&>^^ zj;4+;rJ8LLH?yzkr{rkl;-ye#DP0}u-F`rvK?(D)n&-pDmNgUin0}hj&8juH=M^b7h669zV3Z-F#)2G=Ou+|y ziIZVtE}xp%s0q>Plp0k{8O0XVg%I>Z+58Cqz>jWCZdPMCKU?e=XQa)?H)tW=l7d`{ z={gbGdiGXGUIs%|A|73KjOve{16_=VV1-`6AXG?|s`?E0`GikY^(9D2oi?AAMn-X_ z{!7r@S;W@JEG&AiK)#OiKu{E%_XaWW+ODHRCu2A;wrl==^HtZe^Sy&-cWLeZ8^KBQ zdCL`7qh7`R1f;V8Q8}hjno`PnJ0BV@_qn2#qScPFl}X;$u*|kMI+bwVpBz8Gf)>-A z#JdQy2aPJUFf4(oXhei|y&xospS*34kZVxj{y>}EM^5juBTKGY;Fy&56xI_4>Iv)V z`nM=T%?6i6spYHSwfT+2B@>noNF6`wMw2m2jEE29eE35J6CZB$BFY>!s{{v)D1#jq zTI+ALR%P>aYI&7VQbL&we-o%e zWknVqsW7+wRFk*{-EUox!RwpC29E;^4Z2$<^6xwHhJ2 z@z`gj#Hi*X97-Sp2b`|KzBkeEBoPs2dul`m(%vJfBshwdlg+XSnuVt!s%cH_%RcH) zf-OS1`LC67UXsg-2StY*@lF(M$!Y)m8K>@&IqVgpY`gq{s-8+Fr~nvmo2Ogiq=gm- zT>!9Dppm!F+hM%Z`|@nohA&C~v&mtDyZh&K2MtEtC!V9mt~pu zr!QZ=^gE6~-EX_=(pv$8tAex#%y2tPWz`7g7g5^{kwB`@E%d}GKm9x>iuj!|MZmB? zUp>yGyI;O}{R%q^_pu1ZzPVP|hHFRs=NaNyPFS{jTZOY6PWjCOULo=1g`>2%EoJmf z^WqrNWR!dC*s{^^A$Zgk|Rb0wZHB)q^60zWben%^7A6!@Hs zlP2Jtu<3YOKXRE_9g2~hXkGR#rf29(Q7fkwMhjs{4R5M*b~@PBf)TMFCGdr6_A?DC zJcdaJYWaPG;!e91>2;99B>2+p$W_n^A$g?3G(t-f6LQ56luup8ri@y&Dr9wR3K8Q+ zvFQ=-Aa}#C+oc2)mtjuYNM2han#oftR<>wMG5fH5AwxeAwI3k8n9)UFYNrf8{tBQ*61f z`8u_xiZpPAtn?c#_G26RTBUR#jC%ag&Ay-7qSS2CXa!U$ z16>k|x)O6CWEFTB7(*8?Zxywba!uC=1qJ?hHeyXC4nfUui7_lgC{4ipF_blMZPiW1 z7QL*R|Gh}J)TFwkDywwyjKu*<7fPdhZpx8EX*u1jszxfGo$Kw2BQJm)AdLzKrU_8@ zyY=c$vk(Rjjp~Uclwr?lfGUNr(kIv4IgJRkjm+y5ia>I=I&&?8Dt963 zP)6T4&046Sed5Dni;<=;F(I;SS&27iptij4YHN3JA;|n)Ny_YML1V%+O5D;Zcy5zG zeX2(Y{Qah#S}V zO3yZR;65BG?2G}Wn@4j_^+Lm=9?%-{HmbG<#=UAHqE$y{1!dCLz3(=67iSABGkb3= zZMnds;aZ$V`**k6*TOHS@10;X7C^`Xz@e<)O&9;bOOq8)hG&$YGv1L{q)-8sR6-T& z1d^|NVv3-NrqEBi(6z5?&NsU$4}e#R9v$`*JX&~%Blohe21B0QH7xDHHEhKCw!#M( z0)9>xPy*7dSG}Qp(!d_1nwkxF{MDKx7O-Wf;oG<;lcqQ-rXrN;Aj;e;>2LoP%Ux-; z_A=w@XnysFov7&B5}EMY#XJIm7OIWFi0y)r^{hbWG=6T|udJ0!{N3(9z;^+j!oIq3 zvFJXiar9Fxq|ksv%^`Axm~Vypp5XB!$AQM!4eZ)f0ByfJQ4v1G`A@YXWL5ZsNXd-AS|?pXD)R04kDfT~7 zT?7B;82GoEKN-~yMW)z)#cv+DI&(}Zy8iD4!5{%X#|O>W*Jgw*?A5e&+08&@V0gmU z=5)f|x3{3-iFBLmef?TnpQ1YtlKC@={DYZSc~ciny0DR|#SDUZHTZ7MVmB+zuQmH7 z8|81_+^=1xI*+?cEI=ef8kP)b%}VUVjk9#2!H24q^v#cUZqpuhFp};pCb2OCsV1(( z>p8{?;l5a*WkNuM7jE~ByvOI$lju#ecZjpqQ8#;ugDK@L0iF5wZcEU)`un9zS2XxesqWaJc#l()pLossrl z5lkE&-+g?2eR*4XUJ4{bp$;52Wc%igCa2(1@>Omf*$7^rxQa(W#8gIs=4vI|o<^wr z;9O_|%n%5M3kK~s2(~%rbl5Ro>YC9Z8LC`dQAZ+oDN6UY;>uW<>-aU~@X6O%iiNwQ z`OWdNJf+-ZQGxm6V8PmX`o!FseeL)Up7_o@ZP`$4Y1qddItmWxdsbPRGo%)hwI!L8 zDXO7qben4eCA>5UGmBcYA$+_MVzd!(^b`A6q6A>h)wEC_M)uK&!hNKjwUlW@MgG;3 z_`z&yBJ$G9m@Wc`#YEAe5lol`H@&X$z|qI1FCoB$E1KzQgmT*MM5 z{uIGjGb7?_aOTs6C9kX4y;qx8BQLJ>wzALA*n$0qA2hwu7$;(w*|Pg>;`$GN=g|@A zc)tOkkOCq%SsoDHVnRE^tq$Jk#PH@O+LS445Y{W*iptWJgbF7g4bo)}Oz*s#x|$7e z+0nM}v7t*XY%mN&aT}2O2vg+W>;UZbZZH+QPTS{fzwA`t;MI}>G79%Gb+T$H(K@#vCqbzH{y>N$g`BmURw5+GU z=;Z`nG(InRHKetrwVdC3Kcck||HT)wMKE4Nfl&+hu1Hf{&D7L3PDV|QOWT&-tp}H> zw%mdxTYWyf`M2aqfcoqc&~nd?+P9Yd^kKCIV_+1gay!6-(lSOm_!@npC4;@h`O3+a zOKLQY6qU_5G1<@wvrZ+6extaTq~ zD}U`A>|!XZUJF~mq|@$zu+AiQ9h7Y-;tKd)P$#4Mc@e_j+5jtP-FHRey@OBhr6?r( z?zJSxvJ>~~dLE5l@1V*5(pLMZ4!pub18df5PR>9b@I-IfH@48o>V(dlU>7 z4DwIp*;?P$=Qs#ZqS0Fppbq^vHKz>dh5wUEP1gf({F@+4z0m_;{yjp_2XOswwxuZm z5di`c6AUEtkH}OXeE{#@(=GY{)xQZe1Hi|B6J)6t1_1Q`J=!n>!2iAcpIEq!wFUst zwjF;2$r}MaB7+hVrz3@qq_SXOq_U|oMgVTuKY4%RsisaC0kFA$fX>*Oxfn5f*xD${ z0>ClBFv0#2Ch^}ufry|etN;6Hlusoz2HPl76StV{rS(QK@R}t?BZ!-=KTLtX{wJtz-;6qo)c$`nF8-7_ zu}DQS17Tm)QyCb^e-FU)EmN~i0Ze}{`(+A}lWv)MV){=%s4@d( zTv?`ym;tE&miU`NYn2-M|77S}rH+F#{?=rLB}bzMihH{VihGOo|J0OZm5N~wpn+zz I0i6c>Kb`b5*8l(j delta 47584 zcmZ^KRal%$)9v7dgy0a|gS)#+aCaxTyYu3%JGi?uxI2O1?m>eFcORHP-?={*-F;h6 zS5?p`?s4DVs@Yo;_G6=+C$(M|(gz@i?PMq`cS``C<{izkpv>+tJ=Q!PO*3dPolLQh&G5c(WNlhI4UxJMYiO|Hu zwcotJLsLF&>;q|FtGD&3O5-;+!J8y_y8l38piljM|L;Z%ZxSbl z{A^55TJv}SPYj6^MStlVeR(GmBM*Wy-S+XOyvGOqs;j^Fp{jzsGsCGF$(%+9^sfY0 za|Dx`Lqwig;IKjP*^#GcdNT&Z#_y#X5TJ1vC=ia4fsx*YN&QKoBT`ZD>&cZCy0@(V zB`|<2g<5fyWZ)0Vm0bu+g27AQ9XqE+g{evd`87foOjwvoRR)w5^t03XHna?uUdfOd zzrrn4HuC|h_KX+QxpDf1S8}|Q3O@ZZ?DKf*mn8Y_X_=-|q*$#0$l>!qa$sP&CV-6b=%u&j|h#ESACDuZE$76ZYgf$GApj5i*QvofsO9DbW2PO>R@sVGtS1sdk?gF?CfCd;`&C|Iy>8`!+8$S9g=tYpwG zop7G}ppaqOU@lmGN-}krh|gwU6aYMgy6IZb^tWGn=@~psPfRx*h&(IH9#ZwlprN~; zPhRi)w7iW7pIU+|{&m^@H z{o>T3gJ5HMfWP=^_?rqv+{^_%e3Q>CA<{S0jm3$1)hJ}|eIlVoI8=t3-4LJzhB%!Z z&mYR@J;E$2c*y=2{a1&T*)=b3Y(MqMUMGb!JUTy3(4)N>ctrXm$Sf(L-vB4>`|6fs zMs>pjZFhaY2JwT`z(=ea$$?&_*&jho_$uUQWsbbL2ZQuq?ZL!SoamY+bLIx2CraWA zupRUh8e&%13N=wnZvi~Po^U`;4jdS+jHto49lJLG$(d+J1${|O66b3WdL&tN-%!Mk zXgzYj$0wZNh10*%@<9JkcR(Y@IzUZde+;u(j?aByJ45 zLD#%OiYRmL|AH%#_c(X#V$Op}6Imq?5IP*6IzO2XF7D-zYZ^H^U5x>GcDfty@|n)1 zgPb@S_>OU*N}(=2QR`^#Jvh4{3LW^XKj?qiu;Y=@-~m1Je$HY45;4=?6pCLeMG3l8 z9X#^p$)jLd)3A{0f8Kl|Aehjs3<>YfH^Rm=_RI)*+PQDLqGZ5vyj#9E3@qAN;CTs^oDe7y!GF>27nOfw~QEctj}x4o5Ydc#PD$-1=)T7vG)t} zBZG-hH4XgyljzHUIcu1p%4J=^N9Oo3qgBlliL{0 zAOlHF@8W8!!EN}LHs|KkfoRYnL`;Wj97R)gZ^eSLjKDP?+U-C$37q$TQedAftT z?LlY40!Fu*YlknE=AV5Ok#gqB;gD1YJqIDS#6}fwKmvmiR@O@Xja0gWAKoGRf*@vC z^QocjZgZntaT3teO{aHhW-3fn&@N&#peWz~PcrKnOkLYQvh;D8t9H1uMz9X{4iN70 z;0{oT+L47S!!PuYFk>VAs60NHY${Janh%lMb2-RJa9ks(Ueuw8Aq4^tj*jR_yZgG7 zCDJD94--Kh7m{InmOU3=hfx28lS<;M>16AN;zjMQ4nPrdAj44IAguC>RVC?SUMTUH z-axORqg@$T-~G>t4<7T+>YsTh$@{2E%)$Qqv}Nn8<((IEa|nk2Phoe)9_A1!5c6lS zOqY7H7j$|0c&(_2%vgFBM{N*IhXhAoBhKMBozaogzFH^33Z?gl!dFjEPbzmMP}uDK zXqGb(Iq(Ou=N;J;!;+Duj7^p`gaHf(Lm32_7=8xpzox7sWfdJpe*ac({nl_fW$zo5 zH9hrWq?rpJ*WP@HE;TIfO!%sERRw2snzb*h0k1APf-@0aWU|~yt@W5Oz9T3?Fw=+N z+mc-9v@TjkCTYV9?GyfIu<%3`iZELv@ON|X1%p+kNN&YvQi^<{Mn z*cSDb#MqHrOq)6<1s=hqff8Ke^(RzajmcZjHAmkzgz~4Oh0h% zRYSeA7_@6c7?v1Z`aS7>sWgf*%2Zuh7+Q2(3C?d4Lsz>Vq6j~D6s$=6OKJ-wbCp`4 zL*5NxQzMh*SZ!wD3#!*iQ*3JEa6zApLilJ*M;dGmdCk+!J{o*SPW){;S%p^aq_}FL zb==&q5mDjSDwN+GU-&qTs&ui*cGN?v`E?vl_-0n?J6pe7IP0v zyu@dF;SCFHd1fm*Emf~<{`^Xn~VgZ;Of{hF?a*0-s)@?NMO8}7gz`?=gIrm4alp%nUu+5VFax)x`C zZD%hunhT|(bft7pG?_L}f(Y2wyR77j#(5`U9=%}xPPNp&v=U7ROrF``+J=Glmt7qT zKVyy3tx+?npR;A!7%HCU-YNi?jSgMm`V6CjXxFMqu1IKQW3mEu7hkA}>j7ImXN_JIxK}onc$N=S#L87;r_JB)N zRlk<&i>-g5_1l|yQ_yT`PPQ!V0eyT{#yT-mx(3wx9kDL?Kipl9f6s~YH&??}3)T(? zZLbDv)6D7BW?iO13WwR^nx6G5AwJFV=_4^toImAK`u1|B|NZ`cltqHaDozXMRb4d>%|Xf9yZ-`cj)eUozhy;*DC?4d8ypwd^Lc3lFOu2L|V zH-KzqAR`W6ouy3}!{JsSvX;tj$ifLSA{C z>0e7(o+YmPzzlgV4F?klU0KP__qAd2XfG=}eCRRWBJ_}6)XQ^z5I3$wSn01ejN1TM z84#PDT<{NiK)}EaJrwF`yp$ZexLIk}Np&J49`%-aXAzHP z#zg;vbYY6wkCDtrQpjt8_;SU!4K-0@D;tP62}NTDkZc-iO^YXEHR^Uw`ca**Z&SHM zjGr-Km-59sc!CMC!wc?>p^R(`WM-Ob>ocfD=5*2$)uRj@Bp9tH-jMiKAyz@bS3k6xS?+gk z5%=Y~K}r4F_+_*Kil7it8IAotFcc4vHwZZP zv}hRcyLbwk5$LG;AtJIiS(V`lt|)puma0duMPFx~XO(gD@XRQi$kyU$cNPWBytQUY zCUYO^(f(e0OMC3f{1(Bf$3Rb(pcdg}@t6RPL}X)bNLCc~I7Y&HMqCJzWEk zHg1fu;38;YmybSg^LhS;eM(kJ&Cq23d9%c%+S{}n3fJc}Ma-fYN4~UZX2ZZi&YrKC zn&Oss?i_+K?2$1KUi$@B8Dne8y?5C0)O(iJP4sNB!FMIh6%+jyIk#vJ%x^ zNwkK-PT%wlnTmJcHKD{f-7dV?`r&gdQ|};Sw(w9j{;k}78%n~{)qb^Teh##CBk&%q z3iY8@eVN^upI{k@@U+u7=D$(#*zGQCgTZ)0ji1)AfP4}UkQ*<4_?i6hXCGW!xq;F! z>HV0bnak?ircGm<`4kDI50U-yCl=e7_%j;Yx=GZ0Sv%G^e-Uy`9|S1^)JtnwbtXW= zCOwN5d`obE+F?Mile6PpKms^^Ei^UpxgqkAOg;J%!)xugYbcCb5r>1L*-XHet+FVI z;dBj{94BM!8N|Z*yJ{#T+vjyre_lp<69SmI$jO^uxnzlUHfPox3^w9RHo+ z%YLPtB)^T7PgSej9f+!0ijlvK)rFOGC+h1FUwXZubB?8UT^>3zjR)4k*GM5~)_;S%5E@=l+Sz)0P7jnM(rU5e zC27I6x^rjgQ?^a~`E6xv%3?+sRVlUhK&sfu?|l`E)~xDU-Jf<>t)x~qb>J6?3aUUHtlEfePOvTr209pALeFU02Pj>GpAhuG6>n*?relcgo~T3a z6{1@1J7w6kEB$hc-3W!%#$RsTFu$}gNR>$|69ds`E|8Y=Fo1Y$$rC{cI^SgAqs5h) z>#fO%wb4x_8AiUe!d^oAWdf>SYCttN&61$MG)a%1`F32Z;4M#qR3Vw~_#+ z=>fCLHa;9a;nMY_Nl&t#f~{-(Ot88z6@$AU^4)B7yPf^(Lq-)>MnXdZ^1d6Y_wDm%q5La)+*hMomrN7i*(P5fFl~10iB66xh~!bTE^w3E#Y&DdZ0KO|Ob6#IqLa z2lxy_0Xm%bJ6Hih(DRT1}wzQfDXa5+xmNQ@a|F zIIDLmIT0cic&`rTanY6RhDt2uBy0=Ig;^Edh`GczsZ$G9HYy3$zy>zHwiF>SziHoq3- zS0Yf{w;H%v%wcI$AO9Z`7bt7I*L$6=#WG!2VfnzDcwn5uZS z7^Vinskm|9K^OUXR{Oul{$z$?Y3JWCYrr(}jI$!~^NsVS5Ea$p!gjPt{`#wZTEhJ6Llp9iLHuFLwOe4Yz6hU=)*^*| zTQl%fn5*0)7~uK6!)Vt47Y;ZmcA-=mDBwf13-f;wx9l)eUR(_GX6uMpWW20>v=?qK zee%fk`kpdVFT zM6q4I>$6`Z%Z8ZR*u?omxs{v@GPgdijA@5Qsx-?PuoMcbQhn$ zC`=W>;bQ&>Zz9$r#GPnPwPUyRKijRcJb9&@tUhRhsmO3(4*>KLCI(@MicemAsjL#G z{YE_%>1eb5yYlU<#a^HNzqtOk&EH!KWd<2-$ZEd3-VG+%omuQ4o-CHhbsCnmX_mDJ z`jjssG#Sc2`w`kUa{^a-Gr;|kiOidgwE@aJg1y#EmQj^4pRzU7u)u2BE-m}}DJ_6} ziCQsfm$CB66>u5I>m@AFFkbG)Mv(uClWfYkoZYB+DUfuLu&1GM`mI2lh9T6jVek##@Bz17(t=O2ct_Un2%XvDvn3Jv2&#aUhFEF_p77zem#u<&mrZ2vYF4q#m0 zCJlJ^ZXLQZT&IZ6VCU0(8PGkQ5t#>e#ngtWGLs9P%)^9{iYwY+F&%DDtgv+_&2 zY)n-^U7hs2!@yuY#TKZRim$#L(wx|v>z?q{yBt1~<}2$+|$JH+li zQ&;np0*RJY`*IUHyfv$te6Kryp74NH|CI#VxhKgB2Z9(>G@M&%Cz-Aj4D{Ik4n8N^ z+I(YI-Z$oN3Bex5`wGiK8|7(LTOW%{bFPr{x z?CEY;7^?f_mROaGATSZuucC;?wl;UitzEsOy%1j|R#59ImL(_1@ zfH@9Nm_wBeS&69Dz1arBS@Qun|47rsAa4oYb)Ax_I2(MFG^jJ$4rkB|Q*kl%SCD&u zC#C5_e##n=M9oQ`=kMy=vKUgiQ@kO^QUm9EP2{+2_>#K?8xHIkOFn1!H?hGg5(V+2ZK z%q8Hd3xQQ&`~2;Gqn?3YFIYYoNDR#S)N|un#2X@q{hfN~8PZ4J@*}=;>h-+tbi(AnZo69VSg|P1Fr~2q;=R?_#huO1 zjw90EpG48QPYYP*GD5#aS_Z5SG>#R%_2|F;?u9P4_i;WKb1HZ3GwZ?mYVC!cNMh3n3FE<=RqHibKGn+MOtoO?F94Rfm|JjKtx+Ts|ps+M{z!ieH4cZoV?Ur)omjeG1XJAMBb$i0VTiqiFNGAHUz ziitB?(v}Wf5m@v@*=>!VpPK$@WjmRv6ro6ncf&b$a4B@W5Qna=c-!F~9cOhm57$CZ zv#O%#u}u9=vuXgk`FJPPyx?t9;%qjcIFh0;CeY0FzIc81=dF_I+tSV7yZ?k*&1fjc z#~SAIh#j*3L(v`yPj=`s4rDHumSPt)P6%rgE^;L-Fq?aIlE1uAVPVvhiY?iN;Ana1 zu!odu+e=7fMaXG3I@$S~_Ud`+EZrCp*ImW_3DL(SL?{COV{UUUF8f7#OI>Pkf6?1= zdoEBk!jj@J`ZYaQ=nXOb_<(&*bFvnCTr+iP&1asQQ+pf&=Sri0umF$9BxSty{U2DQ zf;f{J!bAjneU{wzl#fI(ocWoH@^&hN*I~iBK^ky0u(+!hZm(3s*a0Wg9H`F6p>@u> zs~I|z`?mqeuOT93Y5NPuit?wLvUGF(_m)Q+{+}1Dd&~5VOA?Q+A1}+yTux2BVU2#Y zo#ir6@g-`Q{k8`ek}P z4G&;Her}qp1uK+uRdv-r3G2UwcHdu}l)6Z)%!O6}Ty91*5&Lh5vd-WJjq#`AHmPS< zwpMHE;v_TcuZxZd#yL$WB&(R3BDYg)iG<5Fd7LvRe783-A$5J<$erO7N4h~VrUR8D~tZL`yKoPMMG=$eV5TK`z_e{8(R#vx<_ObqN zBe8k*4cK!oy}RgZ-oNVVMoXt=cZyjoKRZN2HnC#;OEYIWJLxgk7;xs73P&_}J4|dM z=J7|;Mk-`jnAeNa>dxRMhSNr`kjNNdu%79F&V;X4jc7SotkuBXj^y>N?Y^`4 zfHa>;eXs~|I(;)5XC-CbzhKJ-aa364Q|~uEbXM3au1&bH_QJp8<8t?v;V7azoHjNQ zh5hAew$*c1H=`SBEhK+F_~RI8bxQ2L{_aufD?=RkjoJv|&G}UW#D5Hm6 zzN|#~u2V{E&ROn-7{C99yjxIost^3_pXTiJ%+JtTIa+GO)^b{)#O_TNfS^k5AcMZ! zNUj%pAiWS6_<0m!u#V>;WZXUZx%9G^qxIN>8ETXrS%xU`y%G{!wK7&BGb4ghN6Cp8Y}iE7UGlk^D!*tO3H|qR+T6 zk{FhQdYnh-r=UJ=BnNnb@KOvQK!x>8$pmL0&0w&);_&pXxO)sn3JtI{sbV*D?}?8o zK<|fUc^Kd)h{S?|RWNeHo2V$Me1N zz>ARw^`)w}Hp(*DcA>QOb46E=dLrPu;S;#>pc0kOKx%2tdqVKn# zjhb*VDL}jL2HL#o{c6FoN!T)9pa(6OP|`iL&!~D0-n%yb5uz;?tGbo3qe@xXbU<Z9A)A$Ezi(RMA%*b+9|6JEMLXl=r5V|K5NEJgV?-KCG_f zvoI|?7e6`|W2gV5@mQGEDK^6BYp$|z`zeh@ZB!BBavB6a+I%g{_Pp5ou74ptAH~E^oc>vvV+&vQVsVG zTj@hRL4c>Oh_qHBQZ0{oDw4&y|9gfv=N!pe7r+37&%7DD1i*@h*&$qB>2EzJtY1=r zg@oJ&W`A@cdI7I8P}EAp+ooc)-lP2LhYb<`J-5=V_|sIoaPCk|jKohCeHtaRh`C9? z5pPen=eE})TvDY|-wg8-ZXa4 zLBIh%0Tg(UBVKuT^Y{ygk3%o5BKo6LG1ylXiI$Mmcj~LGD;{W!z9TM*TMnbih#WSn zpot|_R+=T+z+7Re|K^5B3o8n|5jX-!$S)MDH&IG=CwgCj1ul3BR?;pUoa7JX++q^6 zQ>!CNj9O{O^rX{dG778w+Ab(q)0DDj`d%1Cvf_R*$K?yS9>Hys3KgEjq`WVCy`S`s z@_hK`S7X|C-`Z2zkr=7f%lB!K@RpAJ9&dN=i75hiytKngyHwky;DKD?GH!sMPrSq8 zObe$6uB!uF7;vuow7;NCTlH%NcA~{jlV*aOGH{iM!#zdGDBgQPTL zPOi4d`q%KowD_OXs;a{qd>n>6i7fggD>ylLVxsk8qs<(2?YNZ5ji)_j;TqHP@5(b{ zZgt0T=oqGegPi=k6V;HOJxn_|BD37Ky$KR2g9pPZ?p4f z4bZ)rUXz6!nfoyH@%9=4A&@GJ-a%=W-PZ`31mJXuV*(>>J>YaR5{-!7B0W#oQ;=jX zfF}+%_3lv8voVfq-zy_cO;(cNL#U>$*e64fMyT?`vcC_J-KtqX@OPHr04Qmc()CMc zYwwW{KPiLTT%gg`i#I{V$MI^|9XJ@HV=8Q~srh0CIQ9ybNlIBc;BPeW6Cf#77>3fF z@LVCgpu(qu?gd%mGX3tJFKtG14UJqkP~h||J;O4N4YONQ<>awdN={1Ed>k!@9uG#tZiVhl;I!9QO>djp|m>zph@bPig1^*VyX!HjRQP@$v4@K@@Z*4!P zXOarvILuM>vjlwzw*IL!z@hy1rNw$bh9b;vAL2unU38KK@a}3kV8d8yJQ}Dmt;KvM zWY^t>0R;+0eANoa=d`pHacRs`92-&92=6P-X)E|Ws$p+W%(<dS!KG|rtPOvr1*DUKN&Y(acM)9bkih@6U z2N6RTen^*rCL%3@50+;RE)wXxupm=)akbLx?^J65zP`uLSb7SxSh>786~>bu4XyPb z?5+H{t~T86hG*~elhZy4t4Qc1bCQgj)x7j8mKqE6I37*JY$KjI;)V9oMJcQPVLDVN zK2(-}V?W|gUA}~v)SfBDGdpfA9i<5Q4Vz<3v*EIt%VHZBs&eQtrFWD#oorfYYOy~z zmeDH!&5g|+8(6!;Q`U~u5*EMV`gGc)(`Zoc#vhc(_5PU}^}7MZ$Y5OXC0v`;A;mAK zzcqY&A#Rb&UD)o`aQQ=Jmsm%|hXa}PE#)zg5-w%9$kj1rR-yPO^tNzK6V-Dx=sq;K z1t&eE@V%Br2&!{*_*N$j4MmYB*=l0KJ=Fm+eV${tp<6<2y>I)&xB2f@(37?DzU$E+ ziDXiHl0WH_$VH1Rqlr*g3|<``{gblE{623qIL%BK%K(cU-4k>(eV&JBDhPf?bh$Fpa=S@-`t$-2yHmM9h z;kq;+Q4j83?Ize`a$Z395jblW07z^WOW(H9Xq+hszF!zzcECWnn&^WQ7(@ zgY(hN$F98fwH77}HavN*1_az)RI(s=>jH9%IBz{pkE1`YR$rB&P5|VHTjkTJM(xDC z99Z8uBk<-RAC_mK=XSu)NeFUK2kF97&S6(H)I%_!!|SSr6dpajg8ZqkdV#m48Go5D zw?VUExuLxlz0&IH6*t>(GbK(YHBJd^ZHTnSu*l0=8$NX!Ww)w7?+k`#cm?X8o4KJ3 zercB8tG`3T+Ue=Iwx4ZcWnxOjwO4{7#)j_v_34jwxS}e+GG&-!;>UIytfM&eD8GhE z7cI83JKKg)#hgFP)_j^SodFtR)j$4t>j?-qTZX%8Y2aAr-tJ@-Szce)uG~LtbbH)- z*IJ)8%;BXAVZ*Sd!)6#Sa&{;+x1$hXet?l||LPWOUonTPEvUv$$!n(=vsshE$SP%s zV;ak-X&Aw&s>6f16tsaP>qYpp%eR~0oXC-ZQS18(>*N;^5Ako64(NbS2~pw+wO5** z@p&l!wVbbHMAZC^IQ2}tB^V+paPR=jvszr^tq56X1nMN?$7rH zu0*%7bqRdGJs^qg&^mO;7AY2#f-y0M@rh@&4ay+4nQ|?l*BdyvUPp>-HZhH%%+0KG zsdK)AE;<@qIsEAOk`!YqzdbMg)sJ$p5o7Et{9pEK-T3P}<#lhDhu#%gX>wI-X~!BE z6}0=G6`50$-zVggyOk^J+SvLMBMiR7^vLb@eR>g1QwJQB9S}e@wP(Yqk}Ak5XvyWt z;FBK6(M{-5$#U<>?X{v3{c8zka($OxN99_42ufs96sIMjt7~|mn48q8oia<{y9BdUh z3bP4P1J>?b-_WsPpLS1pTdtF}I|bH1KIh=aYsN?^KN$^`NQaxm&bv4`d>C>0Z*2c; zgUH5ZmE#z7X-alz5hWb+*Xc`_At8h!*);3oA=Bwy`9z@1adp-oc4YH$qt@gw#rAsY zXG=$e>-34R`@*F8HpffbuyN~&toetcRJK>7E*8{S%2b-DxUtW2!%Ob*j@`W->`igvoBdT~tu5-l zl)#7nsK}zq0WYDjFZ9N|)t>jJhL@e?PVax+x`gk`t87?zaAl?kz(SGGN9BKD%gz@n zGr#UjKk+Zmx#J2ZMb?*#tNU5q-ylmeF z)NhZbP+GcI{43kfZU$zy2ewpcXBU?eK#JB)W{4&={HIS&@zx+ygGsCQqz|=Tdl*6A zU_sL`Uty_09fi+H6iOfXJ908l7~^u5nAC{#*}M3w<@SwkJIT6n~OZY#yi3PlP+D&(=FN8zv4MV=_!e*+YYGdx z12o?KRghFypYDr|beiYNh}hU&Lu3jikVso~i`j<9FR{h^D42kisoxjM-NfvFx5;p! zHA1*b_W0xS*XnaaK&k(=M#Q?pM-juM(#VvJCXq40H+YT3!Adu`AeT~PU|$+WvgMd2 zTbNWNCX5i&k#1~>BAxZQVQ{(KA~5tHoVzj}W>ui2FQ#Rp&OH^LUhdS1k7_*BV>(jX zM)=EPTE6IE=!iPuF4N7OZ@!Q}DhVzw7cHW@7w@o}HG;HLm#8CrWO3A7(cqw{f$i&_ zvtSTgXhYDF$Y&N3+hVx}?&Mm2UGm^+N}V(JtyX5>MqSc6>Y5Awej11DftsuM{W)8- zhQm6mNxP?!abdpK!gtrR0(XBo3_PV0E?a=`>X-Th%Io#D)*i3Rt#!w)S`}I%Az#ky zDegD%_srh%`|-9{`e;V!PX6vApU_YVr;f+zF|{-AUYTK!wVHh36Ml;*@RqjZ$BmPs7gB&FgLrI6H@y+il@thIY~l-W3GC>4QF_)ek_ zp|-pR_=oKN_}n4W?ZVUBxQ303xsC=wXl-z4@5c`5(OQSGM;(#^LUAa}zv@+0Ra{&= zBlMV4W;E_Fkav21etsYnNn1;c z7e@BypVg>Ss+O!$TLSFD6_VXOGpcRoce9$S32UZ=3DO+?08eP%l+{MtJ0Rjg?k1UP z^tms#XEZtauqKIT>;q$NBo{qNMPo_}EFl8LlgaWE{-qbd?^iyZE7>|B8JOs($gYzwaOK&;kK_+Q-cbhu0Jii0&$voME;KviXnJNwM#_V-Sc{I2 zgbTG;uQmq7KvJ-kaf0>Jw4dg{5lRVu)j%sJCZW|SEpqig zUk2s;!6pX)f|bwnLK2tl(9zM%J*=jWkfUg+I|FVs^!cY)uCKYtNGC6PS zu0dO=Wd0nySBavhXOxV|h{AS-vO#j^yxA9>5 z!M#>*tHDXx)A4ZkiL=w4X29jyL}T)8==WkBAu{lrc*z&`9TU%qXN7IQ)F-15T<`OiVwwC+ucuzGzP)L{|49Iz z@YtOlgDVX$ofy{F&zCue&*gA8EaoJYiyQeL@mJASF|E}SXunKzW$wCc?Gg!CA*RGT zG>iGM)r+q^`mApCK(zXLUKj8|Ekoe}ItP$$TTwm+zix90&VDpGXE1)*y}M&PLbfJA ziT4!}CBa-*w*9{CfcCz;`AFltXKp@WxuHOPH@0R|(niP@j(hli4t-9aj59j?5FB_I z^Re}AqznD*oPEp1Z@qF(O;L;aZ6G$h?`ieWY(Tn9E06vyy*$ke6~9^JW{#4;7zMCo za+0spW%k+jvpl|TqBi{gxLEs9Vx_>c`>_~U#dGpI@37MKdYsA@m{FQ)9TtE-^u1ew z8@@C=tb3tJggEiLmy}youzR&yQdRSuL)!A6YpT9xakV{VUd!6w-5tYuy)85Gpd}>k zZw1br;~!EB+6imFFNnOQH{s2=DFU(!l$So2mqY(WOHMN4%IQZ&%2>&`@om)d(a12{ zH>3Y2iYZFpmMnX>$S||*wJkKk@gwn#Re)RAaQq0->zotjix3Zc2fF|}+ZtPepXSZt z$C)n}99494-d*7KIuem;!idWu>+yiVGhsOFT*qJ@;!8K%!W&3>RujaCH-)YQb&6_J&c$M~g9<6wKX+VNv} zI@nxUr0f7?Nry2+LqUiqd#ZflTs?Lz%H!nJIQNotLr{cE%`I$ms-@W8vD8vM+^4gM z*X$aGFcR(`)icHo&#s5Kbf~?*XhX%m7F~s+@a!4+A5g1Ws2_~?#{vczMI*{s^z^u{ z9+|Q&sj)pa1P}#BxFIt(s)6`E*N%;5-{G4^-ZowykKLB^XBgr7sHTFTYD|AjP1e9v zm7km@l)Z?HryVZ2$oiktxp?{1pIOjW3}mqV0m&aSy$bn%f!ISSJ-jy^^X&k{0dWU#WbJEyHo42OR0 zH>tZTkjZe+@s06u;b|P*EgCO=L=l`VR?$k`sDzVs_ypNUpnxqJ^%vSn*qJn8g<4dN)3Da^~1x z{&qZa>T1gF%*!WS&S3~JMt5|vY?N-gEK3jP0?!P*bhXU|&*Vc(w$hYie~Zqe>a`u2 zasaEBu?q2y`ek=|O55u^LU%EDQh~ZM6*(h*R+QsYs(|lSs7*FE7 z+5T`9Ls3PF6hV^h;GR_I5a0rPZs4TVc@@x^hbR&sEA@ zg@W~nGvA(meZ zE{}`}p;h}m=xdZRv`S&#OH~%q6ru{~kk~t+_g&Zyg6~*P@-izTvC<}AyTpjZ{`mx= z*{+Zj9v&`BwfnKBF6ru7edTVew^!VfExWDV=Sa_pJymH)VZt-Ye2R&&)zZn?(YXaU zDvTuAZ4}_)X{&;cb|gvzN?H}v(p1lwtI^CH1=l#A;#TSn z7fmjLsN*wR0ts#fgNiHHNu6&$%GKm*$jh-Cc6d*YA9k!M3e6C$BDE)gPg>S63hOTg z76sK6%O>y=1;Q!%;LO$!`?*=Vf{KB2N7Lxlge@WD?yTG?3QdR2;E2XJ?qcdq1>Fq( z3EkYHY%hC4A-~Sr9{r#0J~jwuDRHIk9_oy^&)REqYhFj8NO>YJ*vpg$gK~xY&X^|Q zx0%<;?&9OAG1;8j9RDp7!cYdmr%Oo{;m@dpz550Fu8xsJCl(1d< z@1eJibZ(*YcIIv04o#%W+O~S{zeErO-A>9z5iMz5>u(P!3HcA7&FtFS z(PC{t&&%rZY&2RXAChUBRjWTND~&IvyQ=%WvPH}4=yy0~^tOA&TWecdf=D+1&D>v; zbRm&ykNaA|QhqKLkJHW6IddST2;4QLRwz4fykiXuy%prJDF>>78 zYH!_%CKUmNlgTk{nv^Hc4Egt$ugR#aS@2Fq<%g%@OY;+!>_Q!3=v$5dY0?oa5 zXYB&-_1SR`&-sb9o$)l2zDC0#OJ2@cx!HOC%K}hHcQLPKG^ZXwoD7ss%CRrqr}jZN z8gA8Xz>;qvzb5)T;gdnNe}qNpx3kI!JN23zH0QebJ5g}FzTfQzpb3jkCkox3%{-C_ z4_UbsviCIj#6o*fBA&&|a#Viv^J5(g*ELpneI^ogo-i9GI3X04l1~(V-;>TcEVb7A zobHILn%I0E@NrB*ObF1+=F1cjF8}H~X|`;X2bcU=jV_I5Mty|iH6rFn`R_4|pNm=! zUjRFxkf%GY_p#~a;-?W?b-C|(Y3%B3wffQQm19qFWEwknrIEnh=*mKnjHr{$Zng3A z%>%I?yZnNtK#z@^Na5Dxphk;jrS$${^pB>`y*95qnH(eOxaq*_3nGY7x~1^Ph|y;E zk;;&@rHXyZF)93iAn_lL*24E{f+$7M(>^GwaaC7WS9NK(eDEo6qR35qh|bq+0iVMZ za7=i&spB%L02tx9fs-g52^Qw&#|ViJ3g&vLs`|tMN{;EA#!RS;<154%8fi9luuH*W zu6a^YoGN~(x&oA<1S{fH2v-O_jsk6XUz0FO!PFG*4TGBghaKp2Yw0A90_c!e-alhc^24Bebs9xC}MP68pl4+QM&av4qJEZZIc0SyYCNO-L%7+`7=2x2Lmq? zlb}8Z+*p_2&W777QCV<@ZulZbwK^snI_`1@W-j|50WY}~)MGg@GntCv7J_(bMZ-F} z^XNjcbfEbxnaf6|>SZUWW;*Z2_{vbEfXlLF*uMQA3P2ISsjaT2K54pcmDc&2`k5in zRF5;O$K;aCUdG9|Qtdcti=*|#uL`LrIPIGGL!3h;w~61@^mjF{Gre}SdgdM5Px-BA zM7;eLD~a~EiEZ#qMv6=}IdFOgI}o~bKW5Tnci`nmKIFR+XQgy44bP1I=e0>vYl7{m ze?O#3*4N8S+20?ODLrkpDYxN%pDntKSs1#AmPnrbGz{ec-tZ60C5`m1LVEuEhbyiv zGn-AFYVUy@tZai}+V^kv3($RXS|pBCCQ(r**P9^YAqplB!iELAFVHo2Imr-$S?Sg;_65K6#a0%{CaEAcF-66O;MR2z!xCD0y?ry;? zxVyXiaB_0)ckY^*JAZ~}(Xgnls=fQ&t9nb+dKC<`k!FH@L-SeYQIlK^tsEI*+3qQjDAOXFA`k*f6grc{ErC- z{;!z`{=aPi{)co2|KCpXKb(W^|9z5;7wCiaucYz#0mlgns&BA>zmiFS{p;&B%>ruQb)f_Taslee@jK-5+hD7v(!enx)xSCHfumZXBU{LXcVcz$8*a$L4S!IQr0|plv;}*UQv0Z3r960Ri zup{XO6B?#wo!y8msIb>@Ut9ikT5ub+a$o|K@{BwzlG2}|-EI8M%J@P@Q{M|XZL8f< ztle_etNFdSXr?#um3CO3t$$CiEde4MaR~$P-c<%HqwKee1cxz3&X8r-=h$gve$XQ4 zi3^?z-$)&CAz&yV%lDa9C$Y@jfa4>3ToLkMcf23j)2Km`p2?6c*9;=t_1Lz96QCP!efv zMU|OitKEk*TP}ZCn7JbNM6-0dDPy|z{t~wmgpCqQ0@%1tO@_LFFZTJIn|I+M^5jaN zC|v}7NR=r-z5;vVge;Rni<=PM3`rj`_Ht`l>+zk5k)Sz9^?!dbP;Ln6$yVCR-Q&^xwKe}@peS9ZhO)*j8DQcS)@3xopx?~FdBO%Jov$DSIgpB&W)lRxM zgzygk>0?`7D1som$^(9v7q-$)=`SgLS%zDI4Ee z5y0<|joJ|CP$kG@;5OaM_6}U6%^Njv6L)R@SUa0aizI48T-mM3;VT1uFKH-OyAwYi zZJ+f$$};A*G@qd$er<3_8v9%5k$r80RteQoG@<%#WnG9onI~`Cv{-P!4DHTygXQep z#0<4!3=qb~cbP%VX(@Hb_zR*oZsf%f8^F8uu`;T+GC1?R!C*l_f{&D()^#LUE{S8o zxF4_Zw0_&}DCO7gV+x_EvQj1^v_=R4UA8-b7hVHe2)`c{c2%x>v!osnHbw?{gtMmP z+PTHFcFPJO1`+LrBF{Ox3?WTt)@;D8_uWyJ*nq`8HqHWv0>uK*;9u06P=OQ{UPVj%5=F}S=N+VZ3r9L)^$FMlnJ z-`t+%tKpqGNPUVohZ6Y4S!*~&B8F0WDS{_FdBw`tl$pm{lq{1|b2Z?}Gl559I-UBJ zoKi+&C@znnz>-N}ihNEHJiri!+qLB4;P_e_x+n4No71{ks^y^r^Fn;nRB~V<0){pt ziM=-vfZiW2&Ale}jV-;_4o%)jbw^YhkhK`ZU5khI*ddb0lDmPc4{O0ie9rf9`msu| z{z5D9kuA}wEM?y9Ig)CHzhlJGcoG|>nfxmK#_+h<^<=I|ZE~u5ybLu1r+#m<99;|j zWll8PAkFZEC!~{11EXP2oD$e2D5AYGk@IcJS;s-5Eayc-@_Sz0R~G4H%t2VZHnQbg(Ukp!VW*lq~e4u(hDx;Sqz`FZ$7v!i6&7o?I&bW6K&)YZgRNg`uD=fMiincb_Z~|f@#r02vCe8X#ia_8ZmjCR1*f2A^ zf(h@`9h68Y6BwoqA!o-(NsI+;DAu$f-tsqJzG{BxzGM@aF zUst~~9lw89ieT`I!+j0K> z&2#WwY=bIdSgFq&llNF{6+VnLP+wG)M;lU#o-c?51Q7gojF<_+#aFaXgS8?13j|tm z!jD^wv6uXx_Vw~a5XIECgwl|A>$=)gHPsi)#%=gZV zDM)!Mk?zR-mka7I3ulJR8jDv}*ZVe4Ho4@Ja|!wz3r{_BO{V=j>XRvF^NTGlQNzHT zZfeVe$;892OOs|S|Kye@=f%tI(Mf$*p0|Ypub-ar6QbishR$ATZ35~uh6&qRnjbIs zlrAO3Hu{&yT}HI;s|Mk25@z`LWdom!Ql5>OWlsC@5_s1W!Yqhl)yvJFPUkJZiOC}q zfg}ybFHKq$i}~eZskOk%!w9P8{3>rY?`JNwHy=*4xGw#AJv_+R$PiW35 zJWhmdoOLH#&$cjAF99M|8`Y;T=VO<3r}L==_n;2LjTxEpk!1nGs08I6yorwE;t%hX z687x2_vI}ZURZI4VkNiFpXUc3TJ!0dc1!6AEn7M9!}F?^&B&AXy7Xr2F<0l4<$wh|L|hlB=2})qidmd5 zF2tYN;n1DVU0nXdQ+p~qCCAX0jUQg?5PW+sWl!bM4c%P{Zxvi2S0Z&&T zWJf5+^L@=7gYjnqyR$EuGdY~>Em~NMzTW7|*Y}dHpv$01)tA)s$x2R%sEXwk?dEg@ zcVRbFFc;b~AJjWz?{$)&iwjLS@?z;0ZB4Eji+qS4pvAk(5#W>_=PMc5L0-n&$-N$9 zzm>N8PpWT#ys!+j%fOyK`CjA}h2zk^juK?g>qS2gJ20!;y*h|bF?rGU>)iQk(t#kehXsR!Gh0MR*U6N{ z!#aD_nU2woIpFELE_rITaeM-+S;6$x-B?plZCR0w`!-ZE&8-tF7>EJ3tjxk#vH3@c zS*wg`zi@Hp%P*qEOLl~ZDK-YheB+(nyoc(t(W}+9`$9PPc*^U$gF*gA=^d-fg9N8= zUWOCC;ogGUYvGqva^;6nEo~dZyYs&7npC{4HNzA73!rFA^MoSz^}D678vAy=ZyEB5 z zFEJTAK>+sQiL*Ds7v4{4I<5lg2+vGnPBe#z&@SJv9v>zh+3TMan@@3yCv~;28NtXs z6tLqMfycvJCD#mu<72_O#v{ah+iT~?qb|K_+O3$72jszZc|SZ>O}8AfI0I>uoF=(n z>nK_2kwJbbC8-Y^Yp&-@J-|N8#aJJdS|-XYN#4_c}iXU$s*R404XbWZt$fcN#-V9@|4%Ox89JUOU^P-n+8i(iER& zmW^r~Rn-88>rf*{B`h-Sa^Y%`*D`*966x}$GcPn(+%AClQrD()e2mogkkQ%0ULoCmBF_;&)}XfyY@Wx7VK~m@2~lH|x$3b2Gxs!7Alw=C z4dc#>TOlFElR(h1{x>QL((I_3s1|=0zx&y+%ZlutiUivOy`cHrFuG)wtFsj?j-OPc zL5jhmpT~cJBXG4Eqc^8Q6bRLFA&W@X0u89n2QPE9PKWt5ovE_v1F`Aw41B7%N)yTr zbCx;vONtW=JCB9Yj!56-z=xVW!``mMlL)9}jA{C$Ekl0Yk!Hq^6nN5Je10N@F?Q6; ztV_5Rd=yefD-SqvdgdZVS~{RZ|Mtv9S8Evz!5I@dP_dXvrQ2!h*y@!mUQuSh3W&s7 z?KN#Mtf`qFziTnRJ;@BPNGd|3pZPKfb0B?-kqm!BD`9NUlUJ%KOdn zJrhKnq4)R!f}6#i#8hZ_xou*ePez*#g>}*CHkqgTo}eSJy}~v0U^td|!B1_PI{e@~ z7TkjvOIL)1?qia3l}53PMx*kvfKeRRGpA>=04i>_0KU%=_Z!qFLfj2x!Q+jTI|9F; zWo@)bB9Etz!@>0gMPfb#3iLz$)(_T`aSRE*wy}YV4;ANV@l*jOI`3IYNu5dvDbLWv zZdEt!d~27dH^_8Gs01Y6m!5H+N%SogIrhcpj7ul01!(S(Ed+1WpVK5e1LZ9t8v(-_ zp(oYyzJ=^L4V;RVUiuzs?rIhRkv+{CF#?^({zV{ao`7!DJllNvuQ z+(f+W_^fmOy!17(%QA;(0c`BKejfTv-|c&#_r008ktfb00XwM+FZ`JuGBk}Fc<|%l zXq5tS#rsf}qW^TvJ=5`93_zb%CetN zy_&A);8!2sQX1guaaP1dfqe;1Xtjr@;=#Pf-hY#0?7SW_kHL}B1_W^}KF{>-t{Xe)p22F0>M%WxFSy* ze=^pJz_G({aGmoJ(6Azax%yspC(jn9EtYDN)LmxY*iJtQMPgouc{w zmq9-ytp`8H%KzRza0cf}2UD`a7|x&Hhsjwj%2A{0VL4(Y0nS=dOtUo31FUNQJNSYa zyAi?E;5OLw_?K~AzZb%COC-GsOFqrDf{(k=g$D$ts?N(vz_q!4oY)j`z9+8YnTuj< z;-a(DW5TC1bJ*RB(6JxeNR4S#zPr!hy>4(WDg<`N${Jyc0YBwLI_l zMQo(CdJB5)@StDdXn zNbjuFt*aYG1E#L0@pMThrd1?l?d)uXxX%^`Qw>j`>>187%5@~`7!o|(e5GG(NF_qg zk2c%aofqg}`b!pvl?pbFAPL&E98bd`=}7WwyH{_&$FCH^C5sMN?@@=CXC9_O9$nV* zC71N2L0V14^KkUU9Wlg3ZS|764bnYsX2^!b~{ zjrlnGe9F|rb^K#;Ez_pPI#pryDTFQjwP^(7DsQrKxf7No%~BkTT!-Qg6@lqmOA{%@ zIbtoNou&(GVzUt{X=6%BmYSLzYFXntggUDmfEzq4ovcMVBXWZdbi$P_@{NGZie$Ks zOk@O!arnhC`#hB(LC`zc@~*VoDS8V+<-U??zgaX_txPr4Sn1$;_u+_G6-8PWm(}{2 ze!Wd#gFpEAmW{_@^b#*UiUvShO0@0nOK8hBu3tjb1JlSm#Et=M(UX7=sK@-_jP^-ck%k zbx#IBo9O88^+JMPT-=Bep|dwv`XF}IL1&Y@c?!D2Zk2hGKzQjb`H+py?){T7phMry zn?@dG+dm2OpmRNb)1v;ue;JXOAfY@3>=<9j#d-~`X1XL8ErsPeaJd-G*HH8uPWWYA z+T6uU+T-#22Aiyz)}Oc>(BD?B`I5LG2D+jF?D#9oa_A+qC%#aimLFZUxEz&~Hqb1q zW47G4;)-tI?wyv8to;o>9vOr5dQ^k83M}$=m@#2&B0Ai4Wasxk+Si9DKjOUu*s3~Q z{kz_GZ&0j(uLQQHh!hhi3*6 za6t`;C^p$KTtqEB6HdgTRr!v)MC+^3tRGFAp}=Qe4!2b6qo1imwQL71d4kwS1#L;%5apmw4Bc9@|*zZr_sm5R}|SxZYC__sNM zu4>70(m*ci!2Y8%5d`7is(?UqP>jr9M-YzjHhA|c5!)n@U5PGOUoKSNexVAj3qM+4 zqm;n`4x=*4O;m(fT#A@A$Ra37N0ekj%=*I~-|!mu;$kl%x^hbGZ0n2`_a&?3S!`2P z>n!VLUa7}5qdwp7t_cJyV!f!**IDzbv@Bh3s#DR0j zU1K7*<$?NUUlsm&5#A+}zDzM9%>9l5(9Os&#K;=(T&$n?$oS8Wb39#R2oJBj{rqzz zH~qj&*xP@C*3I~!|1LS>1(OMZ&JDmva9EaZ9*e!vC5WI(PJfvigDJ%jV-<>rQIMFy z5V|-C5eojpIR_=#T^>W|k4n-2l(9wkER;W@38?)+)`lc+^g%I(Li!&A$63*zQ*i=a z{eLrh;T4)(8%+|2W5P5xCxQFB#_-Gt!|^Yn?*ik!d65FSP6{KdFE?hyNm&h_i!)K@ zKisOB`6FO5e{RKnt-_TqV%H~krayavSdCr_29oHo^#1M1h$h5C6TU_5Xb{ z*VDf@C$c--r%XuD48dKF>My&yHebD15;mK6{R;1&+bjgF7?I#6fKOtB7ndg24Et9a zT27aYB;b7w=>Q6QWI=ewL;x16cQv5cWp*NbRf1O~d%mVx9Q!kvr6o7VDupa{I(Ec- zi;~Qo5T~o|n`4UJ<;;8Y{Kn|Zrr1QmGCqd7T#N*Ab*|2n^+7Dg>bsM85^IUT+zM-4*h)6yF!=y{1*1Rh2L94=aT7lCEwW-Ez{I%Hpp+2s}f2px(X1?xr zE|!1c+%yF#n$D~$2Y@+N5oJ`Zy_<@hb$>4QKpE^X^9ti;*BfGMGkp|*&Ou&|d@j=8 zCqH;Oe{``O*g@p{ahyiv4&oU*-SPG_J{-W73q_^VO}-~THA*w*eBckTBxtHW<#vMo zbJU@{6_hGUzh}ks3<^J-dojY^caJ!7opXK4L?>RC6`6ZQI7K4$0q$`PwvVh`8l_pY z#Y(NURS*5Qd&m520ttae!=hCTQOYyCk@XZaA!t$Tmjh&d%)UWS+3DZbtK?IlNrZtEyvkWurqAU+lk8I7>hb=8CffL$~UEeT2;HR<-iqM5dYDpY5KT zPO5Ko2~R)@HnsRWn(y6t4E5LaXb>zbB4cRe%WyR;`y>!QKd@cta(W@FzF@I~Gkr-{ zsEY7)atCBQdlVD3W2L!;xD8gSFU>=jw~*yL(IL z)T(X!Q1I|m5os{8v&CKv1@Fp}j@ zvCLLT-=!1Q6E(j~C_8ymi#Gm(73;%q2@71hKtCCVnWdsPw}lrQNk4iMi~n@% za0t^9@HKt4W9Yw=5c0tWc9a2wji15QIyzDG)U*23^Ou@T`vR_O|Ay!0=^P0|+^T~v zSF9CoT&#|K60$s(U_3!mi~Tc`zh&!VKO`#62U!LG+WM9EZ1J`kwi%0`^C3?$L^-it z`H#&MZT#+Bf|79GIqV8 zwq8hX!#~3+yxQ*q$(vIE7%1lRFUWlQ55oP}b4U(4W>6qx@+i&8FBxm=?yVkp! zBE6MgKH*Q3e~uj%ECg4gIbQx2Emib7b~S&NCMPIiS>wQV%%!ax>yiS6CbxEF&cNJ# z(xJ*DKxsCba#6X#QY=5_GT7kx<_CNtT|Z7UuB;+y(4oi&Xk$yA!WKQm+^vGj z_|poD+w{Wbk4xn7-7|fVw{k5aXM!^e#zVL;slQxqS!Ri8ejJ!izWe&!#?=h79J^1aXaR13&vKJ%UyTK@xs`W$@>pE%kwmAXe0W*3$H#w-?JZ;72ZB`9zYeQ^ zV5x_@_;G`JtWGZ=pxgI!Nk+y4LW$~Dmy&Rz%Dkl|&*1?@2mex+`sqkP>=u7svDCjK zW5<0gBeXALH~;#Si*EA~ZuA&_y6+>Y>BW-?8w(490%w4S?JE7(r_1ikDS=UWPy7{6 zb;4C=0zJsKd($?XK2o)>dq{-v*upQ~jMDd2`I423kb8sn<*_N^t&8<$p%iS(0s<~A z^L<9oO7B;oHwBjWSRh82GE z6`*~p>*Yq1T6dbpKz3Dkr`x=HbnbCzvr07vz(3nppuD_%qhL09xH_`e#NV}*?;?z> zH3n{qn!jS(+^(pfX5{;gpS-xj>w3R%)^9RBY^PiyqxJ-SwDNnQ735C9BCCEjQE$e% zkbc>Vsv}VYGH8}L@C4ZFWwC88l8V>1@np|>Sz~HC(rV8U1;-9-!isflH?+(0lJRUd zb6Y`DuXjdW9;d9~hfSQ|MA2)(ur>coO9Q-d+tp;O?VG5EM`iUa$=7yJX3b7dFMrG* zHMq9kaPehC8f`{?c~npRxo#p%g-Q1vBV42~)HeA;s_++D`0xAhFe<7)z9a=3lqNEs z>id(bk-y^8R{K_}H}y^_a7soWuDF7Fw`dw0rKC(x8T?9QUIJCK(4WHUo8C9QE5Ki0 zd8)I>46InA+gNQFBk#w4PJh=I+A2AphA?X6y$jG}-*SQHGel~e`^CXxmER0)Gj9~+ zIJTlWj@^gcjR9(Kvsb&>kh|4~^cT73Cmo)3ihg3W%wwgbWsMU-Q80}C5Yo|-(Qatb zb1*13$6ulJ9Hn-A6oND6iu5c9E}|dFF0wiidb-n&fUg-A2I{@Vz17bL(RR^yGiY{+T69&!>}|Z~ zSl(!!@1ZTCEv7>*vV%`PJvbWYT<=Wsp?@X^82>ut-S_=v(3dyLY)g=(fTOXmlaaLHudb>D}#FL$6a4Y{iB~cRU`d<`0R4Hz~$m@c5_6 zX9zk?5;)6974bAO1|{h9YtmDbQtlv zONdpi-kb=$W`LEgX>?aF=-ZOgD>cQKE}w2lvQ#5;Rj-yZMu%?*v(z_sEAGLWh`<8B z6_a+YWv{^O=J-qBAP0aKQ!zXsMr%;ENz{)ylF8)kW*HuE42QQA%X-R&@);7xER?@jgo_z#!Y^4 zyb^0tLt{n%!zGV11!)o2ro5aCB!I$={#ynoH%BDWw9T&x$XspIT+z5CRh~#VebsTS zJONqT`aSN|rGMthl@VC#j6nP!oqQ=10jPS>EO+6bnBr zl;;D}MCbRkRd1Y8O>G@~LJDqoZLp#RTz8kqJ9gIeE*I)-dFkrgG*S5FCob&O3N*co zT8{MKm?QLPD3Yrx64ZVqqFC`fd9kSHsEB z*^Gq^qr!Uw_lXvJ6%@l+NPfR?FkIa%K>UID;O@Oe%aNkMQ$@%5Gl@Y34IV?l*3*R^ z&2~T9H?jWSsi&3Z2ew--9CFYyqw5Xpo4pzOy_eUX{{7-p@-s~6x7d=5U7$WG&L{~~ ziA416=H|43(c{uyy$Z??iQ-A$$!}(-hxJ_Mn~UENrjRk+tfF0Q>>@1@Dc$Wp(!e_!=?x;3U(b~mnvBr`aKZ7FrK zr*A*6^t*D(z<^-?sB1S`TN)zJKR=kWWb|m1{LdDbE+f>!5ym@)mYog)H(Hnw*x%|z z43exJW?<1a0?pn_PlUjN7+nZ~4aey&Y90YHmiDSQM+%ZZWL|lq{qW?4s|;Nq!PvZM zN$KFcjYdFcAnG*yZ3Df1V?z-XA@AXE^hW&&U+b-nz@tQ>%x|8wiJn|RflW~S8#9L~ zx=}W!8DJQ`Goa@uu%n0k{;VSP=q5KUg!rIk$9Neb&oSU}kqxXz*2W&f&%x7!XO~hZ zbP`OU?TAm%>Er6tsWXC;=18f;zj}voZ}ek(_6GCzy`CPi>SE+vd%*FS>m+y*=3wau zY>c;j{;W*2pI^5EOxxmJsZ9}zg6QY+q=jnOO^_~{#HlzSH zI>Y2TY{E{1Fa$lCHHhJ-5fK;9T?{k2q6@#up?~)D)}ngW)OKXqF~{4R&;D4Pe%pkU z5(Qc|ik2B|)c%#c)y-{#U1i>=<7eWAc+0n&?|6@XJpF9Wdl!*hP?foHSE!Bt#E26W zJ9F}k|4~!cZ)FG9bgA@sed^`WCZZe&Bx<~$8GL<1eodlQs@s-4Z*nlDQkvKMIsI29 zJm#)Mofg|$?SPNw8Bt*kem89?JnF?*<%@N6g3gSJ$6Es!TB)G~pN8~W+!+u?x`D-_v_KC|mv36c`>KU$(` z0kowrbGs9}qv^Iz5S^NTEYL2WM>;KJxcjYDyQDzQ=;l>zer=Ecqc#z{L$-Uq>j>|b zeQ#L#pVGembR9}U-wGZld;D+L-QQJQ z5Nq60mASm)T~@QT-H?3iAd;!YK1u(k=|6uR>34_K;9vQVWqCb#xY92XRjNLWkd**r z&N_VCC8D91+>F*Np z5u|#lCw!f)ZjeAv=s0L=B?$K>Eq}8{`!}j;l6CE_L#<5mxPOSo8K*(Fd%MYq**r_3 zNTUGKuMZyJC=b3Y)(hDOSCgAucS3_BXw84|zB)~wL##)4^?Lo}LIkYV-`ebLF?pHo z>IM!DZv0FCrpr~EwVtef{39%Gwm5&ya*Rk|Bv$g&gdIoBzvdrYB}WExT^VmPKq;$j z2x2afHIOyHUk{Kqg@f(RrPI$>T?{s`M*sNmu@>Iz`nCJ{CPxIl1;!BeH>b_U-r2Ux zbZD1)7fyZJKS~a`A=xt|J}wh;fR|DnF!wLKD|3;a+|`5ecKl8yZZ>_=i(zP%M> z6UN;Pim)P+H^A|>9g`ZN`=mNE?3v(&!weV($0`21)W1i(im7jJ@2pN>iy**Hg6a$l z@P0OOq;@joP!bUZ>2{_Vao6Jo_%QabUm6RCe#(OqhpClXEnQhAPT4phneskasX?C2b=#dv(QUe}NrylEKu2nw>!`&g`a}K|5ceY^ ze#y9%l@we@8geW^g&)vu-F*z`a-t|3rJ=H zzx;Ay_fMDeecG!hD&iJYpNsp~J965!k=nE&Zs{pSHbL8!b0|X9fky`%<>ZvJ6u{J&=DZ6OGX_9;Yc!fL@ zEDyQ%H%g>6mw>h)X<7OqLew~2A+BM_637xtxRRhzbFq)ZTdW&|_G!7S8L%aa+^iim z9s4*P`^%G=Z&YOI2{e3Kr02%?1_CMm8tf{mEdlJT?M1`J!eJ7Q(FnlPHo+F1(lYZK z);E{^;EuA(=csPh!~Bo6;*j4qJ;;BM{osZDVfi-T;ie_Iq1=g~b%xYe>^uXF6^%6= znf1#!9B#_3{P3c#)U(p-UrW$RyfJ^!Sm@V$cqvSJkZ!ulc-Gnxu(}u{DqD$BichWd zzxTh-54~rTn_GZwb|wej6lj32reNzP#H^8LK!k-);Jmesv7JNE7|EX9c-eRPYm^l2 z0U=T47#1n;vZYI3uKV(?wN+&**h}5@c1sEYPc&;G7*c1%8Uxl;-7WZT0*7b75aAH@ z5&Pr*F}Xq;BLE>892^omRTC$2ueE!ng#T4SuxgnW)*JcW&ls5g8lqpKbez(z%OW#6 zM>TEh0;UV;eV6CNFt2P9p=xf9xm1umz(A5Pnm6Q3{$qG#EQt~!%%JDnHx)SPR$fH` zi6>6ED?`4FQ+Ep`D+(1nE^7{E3nV0$TxF=Y+ml_6?6>%j;D_8u-ncTctIVz!gHur% zM3nVQgMD^3_Ei9chh@W0RlIwuxowi2lc9^ZT-+h{!LLTb?H#=&yjZphsN(HOH;ioK z?w+Vz^6;aiCe`PiPgjDL`}CRNms{qhq}=Qdx<#*y@Il5xtb(uXIFkzo6?*J6LZYlp ztdb-e6uoJ}a#p-5r(ZA!f373&NV~RM%I(FRBR5tqbGYi@5>p=1rozUv{$GQ zE?2`5jj(5qPP7Qi|4v*M4&wGT1)^w8wdggAi93?+?}$_IK8eSFv!ud2!z}f>Rwp6J zU^D3EVor;r5z4nj(5BmtN1sA{MZhp+CDMD(Hs&lMhEu3kF51gU!GZodukbpvFqicu zh1D2qubxDY-iSQ|>6FA7=QCT(^iF|ImJ>vLbhVZle7Dd~iC3=8KLNEJA*L&)6%+cn zPy|B@2OP%l;YIi)^HZ{-lrZ-kB|2_`tPXDuf(G2`0+8a6xLm&a3B*Dr0j!UX?HY~3(?$<$nfXO#!EwCe^6>|}d<`QM z<0yQnzd1?Bm-0SxSLStgoyb_X#KS72g7{|K_EK=~=WE9v}H@r$JD4 z_Qt2~kDO?;{h^;2v3hYMzj`JLj0(#e64#x=e=5Rw9$yS*i;I(Y5@Iaf%wgM`M%4I@ z;XA^!^Cq9AGXZtSjk!^4+UjA^O(}zLiLCD9;Qit509a#0&N7#hmUgrI%}(3X-W70{ zu<9J#R_pK<&}5O#=C9Io*sc#%Ggmv)2K6eK!lnZ2999%*Az`XBRYtMle#L|z=k*+C zuCyvh6>TtBn0A+A{odk#&`=Njg{No}sopXN(|2WV%hPEWH^12Qo*6gzs|9kO&FVsq zIF-yF&E#+?5-xc-Z3n|2IS8RnK+m_<7^naR`fnM$6y?{xT9=oPWZ|G>1id@p5!~CGS=>Zr@2H74G{Mc1i@U|J#}a^x6of;gmkF&HXRDw{43#Xe9rc1Vf7v{WY%j%F1sRpfJcyF%(bCiHBOrj{dC%x<8%qp0+5=y$zmkPq@V5VzuI7;gV4eF+o@#dkAaTUZE_ozu^=bxd)v^i znf?Nk8qI$S>1Qd*gC9;KI9sWKqEkhCZ+h&cnx{$?N=~m~jdP_QSB6p_<@C9Mk$c)& zXMDc!(0N$SH$_rV1kh?A`B0tmXhRxcgB=`a>zp=$G-N-87w%iPakha}HGdKI)1{|@ zx32+_-JF3TTKBsah|R+l+f(`=40QqR6(Q=v@zkizAWZQEUYxNN?oR$@wKhjL=`Y84 z_KlP?|3_)a+P{dusofZ->X@VYHr*-5vzeuhF7B2s28hs%sI$!-CNQ+>nG1m_%9)sx zSGhZAcyf3Ojf4Al@JZV?sB62Zrtrzj#&K zChk)n<{8$jX@yf2Rg|G~l%dAJRr>RiGSim1$vDQu-);{X6L+TcmTPcn)u44!4pxu! zxTTpm-=k3c#t(&Jo$LY_Uc9$#daJ+Oe4$?63 zrEhMJ^x`g0JU}=d48NT)wV!wkB`#uu?9P=CEXh98Ey+uoAaU)v83@$`D6(J)MrUrUU23#u)lB4Wjzq;vB>Gq zb-?QmNx+v&UwAt^b1kR-VRXQd?|xjuXyIe}yU zm@IU9ZlHG3qjZ(^`shco9pGf?I2}gmXaqTWC%T42)K~1dIj40dCa#l8U1GNh+cOT@ zvE2@5dsN3EUV|?_-LRt*0RwiFHS>4*dy+DoSTn%pzfvv5XBj=JGv6A}XlH3L5w_n= zKX0{M52aG6J7YdOS87ejE9rl^F8TJNIY&3%viP&9StZWhR3Z82oYt)&bxAJk1{KG^ zxTdZ^U5XX8-15iNw?*$@0!5= zRXyxiD<#Ot3mANtsP;5kRr{y=zSFB35=e^Q_}@TU^Byp^_4l6t5QbDqqh)`%=f2(A zXoK)kzzF!1_zJki_*7p=NGpOAkPwtM?502fI-Pax$iMfiboXl(P$&jXl;TKCA(BKp)!Vr z;-cOmqjhLeE=g)@xYTA(MH$3{QG-LQgb4V`XBdT8$~LM2FThtTG%&5PTd{7-=ltY2 z3p{G6wIFeC&;@z>HOx~x-_QNjHSh7AgifY*r!3(b zOQfc>It6V?aTYw^?qDa{aHTZeClxKhdd017y9`a_=jSo${*ydh+WBnwsgLa6>Qxa} z52_NX6B@+BidqD@L9QO3wo$`twFQ#|9RR1d1Bi+e#UIrxYQC353JuU!@+hs$=fc;1 zJGcM3LzhcCt(3tFdG#%)Wd724QePJf(Y8@)zz5!m7Ep@3mVuAS-XcxgBV4MOo}?&X)Am3p&6qZ~kb zZb3VLr65eTT--&zYnjYdE^y;n@0#4zMPSy6dqkT(%5Z!#J}4|K`M&=EU5ve{Q@uJl zqbfFgz|1tgJ$wKT4QBXeL}p|3VaD8}q6RF2Y8oxQRMt|}FQj>PvxkD=`pm6@<2~t0 zKVFrH*tQW4E=z!enInBvgw}4^4?7@I6RMkL4RyI0EPm0@cWMR0m|k8kS@BE>v~3*O z8Wqy}jqKm6BcUO^H&n7&XGrE+Fj;ngoPnRgNVB$zo1;azNNiM4{Cfw(*X5MB*X5KT z)>f(W|17lVe$0MdPswHlLVBCB>{i+JLwf&VOo*HN{;zGXYb`l;Cc*!;0lvG7Yy6fl zSCVy^IHEZ5zm{N{Kk{mzMVu%ki);`~xH1_CvdaBIs4n_fvC^3%cJ7+J;ScIne#okf zF*z?_I)2ig;IYxy25JIFf*y>fAB0b>8!r^O`>I-_QDV%9Q(kXU0MFXHdgy0N$_aj~ z6L_2vc+|Yn%xH3s9Y3p$jVV>pxv$_zT7pJyX;NUuyM*}E7|7v)COfoWA!QR65nN4k zMEfZYkzP30fkj|lq7+Y=$CArO=z&+7BKviEo+Ew8d70fC0;smo`9PnxP;5C~4i|Kl zwHc!|%%>4DQJPwo0WWQkFFo0#{%b|h{(gPx=A=xmq-nWiO`kC-oO12SmHD>MmGuBo zuzw#hU-J3Vf%z-F4zHU?9#T|pX#MzU9}rt8oQIUe-Dk0nufjKgPO$2L2etRG+}%cB z0Cqyr?|9ED2{ft!3Y#v>fnC`b*=G`)>}2aP!~(c6@UI4Ej>RF&hUnNJq2eFS2&5*{ zeh?k)nx(@2JMfJE1U_*J&EY>Agb2kfga0&@QoG!L1@SuD{!f!gaF7WGdP4qy-_9O? zT`-H=rmk{LAK_VsTNn(_mzF%gFOoAZtOMohFi3kDRK}$ZOjuG%*XYFW zcNGw=5oP-Z_Lurn2Y-{y?$pq!JLxC|MM-j{2QBy!kO?XQ@5FZ6q6HRza zxmm_mXV3?@4a|>@SGe+nI%8-74{12EKMR|bSPVNAs9`QNG6rW}uZ>c?rYeNm4;w{J;*2-t zrY{@y@!C~Ua!nJSgAOlTkM%j;^Zu9wz(8erU+g2UQF8v~&IOxUg8=wy*t&T>3O+(F z-|?o-mIZ65cs6BdqOQ}Q&)R;=rHir~cGb|VAeYfbWd=ThdRT{Uk_hs)K31p-EMOp= z&G063>M)6=CVIGvY?4!C%An7(ifq!7eu1Bd|7tEp+kZ6|2++TR$13rsea3&4{8#L7 z$^MC5`@dpGiHP>6RT=C3D|Vo_A+Pty{MD>(byyg&@4hXH%I&bwYF7_HV?bSF6@Df! zAFle|5sOE0ToBnti6l7yF+kyhu{$0 zT^4tDUEC$OJHg!v9$bS6z(MUDeM#?L(<2 z3Y`uEvO;TOIH#V7qO8n;*(K)!A&Ub%_X>W)3uFo(JSOjj`UyR3!a^AFax8W5?P#f< zGlEeCUmoqlAG`g^c@eBXwu^RvO0)kdF^zS zwS&|3L8R@|Q0~|WyYGpT^#t{>?DS_NXsL~#w0!K$V;vn~L3@XMs8=^%x3D!~q&Cr= z$9Pq1X!1V%KlXW+zxMf+7$uf-2j1(!Z>^SAu2hoDd9sE_!V^?`TaI9-f>ihnYOq#P z9OrrYTwJ{|e=WSyU`YyHfvlpX_@b068h5eSQR~F!#;EhQAuO0IM4PN9H~wkDgQ$n_ zt*~$cT;TTOfi$hpABX~dc&`P0wt@%JU3&$fD$qAMTPvtyTu67Icoy2wFMGkSl2>RL z4F9;d1ZV%n15Pm5*YT6I)kJNC4|3FPrICCyl$^G&N*MD;P`7Fhm~<0mC!em25*y$^A0A|!{2rvzQAdAvKyxY13 zoTqTY8&OYRUjO51rF=?Ki$sZ_?f#ET78v4_1rxD}ppB@lLa1%Tq{~y@8tqe$VT!vt z=OLh*UfYtbYYkPiFv__g6T2o)!)^O_)|(EcCTIE6*-<#`#ywTNOJ8g!poVpJsM=AW z4QjUE_l`-arZUpzgXM5tS|U>gLJ=YaNmxbpv@_(%uiBT3V1KFi??iQ>l)z9ksel=) z_nlz!+N}!oU(rR$&y_FIjgLHo)wK~27pO`rwxFfL z^XE!Ovd5uYA-CBS_YBoS)mKfm{#k+C{HjI**r}n=V?5WQSZ1l*54aY-DK0D4HOTIy zBsRyenHPu6Zpd!LijPC``e1C9OubB+SGRhMGh*2sOK*kiR2~Pdh(&!e@pR9!AMTAw z{gcuR-Q(JKv87e6YU9NAJzqtkTyrf!z80Pl73_Qu<5Hmz|Ip#hx)%>x`D74O8r5I7 zf~h)EITot6=%tc<^|)W?)hOS#uqvD7>Ue9R^O_$L)CKY~b5wc2QPray#Dw)QnHzet z5H~ORO26SszYewYj9NUMT$@ex%#{NJyz(TL1}CgfxKwzv^bFMIhSeKTbgsjy-KZGB z>C^Sj>W*qldcz7Deq#yRdq|@Y>eO2+!>pe(tuE=#8BIk=r@j381k;E$yBb`2J}e<$ zXLjrfYgT%+P2XgK!?^kq%?Pm%|HWnM4_9XtRGgBKdqW4$L@`B}{bMOY@P1sC!0y;% z8iGkMhl7KInOFdw(rMe#jOB$OkCi0nV-DGuqXcP9<+kYz-#u^PO9X9c_%rQ#`*l{} zWQMoG6mB`I$R%IR$KRU73z&}g-zUjE0f-AscTx!)Nw^YQm)ghtqO@AL{;2c1PLuSQOs(r>H_*| zvWlB3y&gnhR$#nRe|*gx~8RAcaP>P ze*HPTr=)u5Q{l6C0duBREk^iNq@-EU+X?diE z@5*xl`TirW2H#uhC}Rb?JB&H>m;HX7w|mW3V6RLjTOH{~yrni_;ZeCl{w6tc9a(Z- zrjeq={z)2!xW%}ijR4W@Siq$KVm^GVKRmLPXVQQQb0zVRJ zd+SfBrtBw?fdBZ#K;95zr}y@q8O6dPpG}6^P!9D*A9vb5#@}}FT&`mtX5w*C&(h6; z=^#IJvu~%(BI)EFX7ISujtRbnbqFSb3i*3Mg(re*$Gn$G2xew9vQOn86IzQ_ta;XX z(fN0+Y+0f8>8<{0rmxrAVN+t#%sxJAJiqRsP$?};7jn3aXUo?d?VA#I}iez z9s4q(42^Gv5=1tp?m`>y5M6>vP$WbwJ=QS)R4&$7IF$kJLq8R#6g1r*lIQ-ym z#_ak+d0h=)Ybpla<4jyJ#h4=^o4+zVy2`3MVoqvq!*+B5&@KD@{A5X<~~}py_)k8h(I^guN1-BgrdP zg+XtVbVVTZVQ|GKqL0*8pJR8mxw3rnk^H7m%QnCG4LFIVKRs|RiMtDaw^FE89O9ks z({P(v2J2w_-k7ox$t6xa#2u5!2I|?hN%*8exn<5~yI-uJFZfBJwSVg&)fSez!1Lby z3g^VYr7aYH0QtG;t@Ce=Tobk(J{05D&R&pA7?n&F0zN$6Np<`GdO=z+a7TPOa6 zcgu3Hk(X0Vk(7pr7f@s1;wbcvi@K#5U|?{cnRBx|tc6oX02eFmQaW%;}mH#E_BlzSV7NKhvFpFiwHP@1~vJ%4ZH${yi6Nj1Kr z$B47Oe`f#Hi$R3)8I+W3!f-UUHnIEu&3m)LQOT3X;Dl7~M|5EvxEjt**=g5=?OI)ZJc1*j*$X%|$Q^uMYMg0ow4t%}X(V8XSUR>~{& zTr6>Hkgt+;m^I6GwCbAGkr|lpVk0m^*Vpu*+j}{dV$76Bo&Cvt!LR9IK zvgbsD+dqN#Y4-XgHgpP{Q8Du1M+Q15_xcc@U2=wkVt?S_0>U)ob=JC8-fw*h2+6@O zQ#1`2%wsSE_v}HPCv~lI0-P|uDFHfROn8DuR>i1QlIq}tU_ALh9B+$pomz8jTd)^C z5Pe3s^c%+jT$zOjo=nK;M}NnPnt#syV27Kh3%4Fo5?T#*QS6>u>B zfU;&Pi5B@)iWC-KBvLyqzERdl)&SZxhr`h|83+t9+U>a-K%WEp@Q*#L?e|QibqZW5 zpp%lJ6;u2gXb23Eqa+2IdXd0jgf* zbn>Vo9{{=8Pk0WJAYl-^t@MP#C~2$NheEEn0bxAD5gC_#hlzHs3=a6??8N;NbQ~0Z zmfDP1N!ehT*5pXEZ-6ikG7EbLAw+Gg*4JuG7Q5{p^e|u)RAncHKeaBpI|18JQzST& zXq3PP!w zz1cVWA|L`ZY*V^CA)SNL9drbgzIg3pAx;6tAg^qofEERA3O6^YKHgjf^G~o)m_FIr zrm+aZ9MYIfgao@a>=*@G80U#J)+HY(o@yVOz6cI|lXf&Hq!<57YB6o%Bg_{BEPn(q ztOLOam?XMvS^7Z~v7|88QOrz|ZM8Q0#Q>^3t!`9?-l~UMUd~?$)^!Z3fYub?+O8as z!z!YZ9>LKfXRW-hr9={JD+6CDj9yr2LO__k7m zGCOz{PfP;{MM*DS5#u^Q+-hIeD%4)&^dV13heK&1D}lwvlDRG+!UY_G$F1f-=n^%r z4K1zv7J!>sG;RP3tyA+6&qHo$)WTp+V2H!-N55!^=!M#TfG%DZ*u;YfZn32kQ z6Ol!I>aOn5nHHM;5j9R-eD_ArqxV6QD{5)F$(j&0w%I5T8duFf$@EcSD?Jm>$C&~T zMC^Xqqp+!Yid*T*iVQBd`8lEK*-Z+)78D@t#QpkOI*gdGv_D$5L8>V2ftdrjay(4coDTV zdK9#Dwf1Ukwe#qDo&Xw7YwbWZ!9IqpL0%2d@K&=ozj#3$Jn4eKkRJ)(Lzo1Ou|5<9 zpFv~Bk@DegwB+`5%uulX19`QdYbLtGG{bhRf^;|L84ny&<#D^Q z$*5p5(oFsz;MRifA8Kub#zph?Jl?;okuv)H_oEteSkW4d=OWpKap7de-XV!=g%^tT z%KK!<_>%_yS~Er~PAN`ull`n~Fr+xnc=M6d+=j;k7?hO`@(Bra{4tsthokCL%W*1W zGwm$?BLP@Y)>2f9sAU~s+Ax1@w?l>7E!tts73qqY0FH6UMypLn7mY!HeeFb3+RHTgl()bUE%}Yz zNH06KetJcKu#qXu?pqeOI)Qa8a~W>SWDyT2Fcd5UdSk`0jpxTjfN}ckN-L72ByNg7 zY%X~q0D%2FdH_Sz>J8o?=m^3*LCMS8?oEWnfk(-Xrm_1}MDx@5_&36aZ@zS}GN?Bt zNl+m;7L6AZP6yM<%z--!Z^$qLmg|!@88@jJdj8gN?x=Twz2HwKD#-AFBqiw+{g1cx z1NC6q7M2VKTW>R^WgJiN zm}O8}h`taIHDn2b1{whxMANWu`|&XnZDZD#%czSYyr7QDiE(fgX!*Sc=CGX9C4omhizGP7#cp9?%i`WfH4xcTclQ?@kbj;G2?a zeRWVmBkmaeE-B7FqEQS|Vie)c492&Hn0DJGJL<>E`nA!ylz(Xbx};w8W~fd-n1@~= zQEZp}J5<7__}lWHWHdM)+P1J2z2li>3OjRMD;OsB++KgtIrk1S{Vxc;hCZlRkp+@q zVNZS-t=KsmHPhoLZRv`<2v-DBBu#{dpKAg`x+qL$Lr9nPnUs%OIPE1#FlA^Uf)M^^ z-;+6{pYw`wo?m#1R_d$Fbkc2@c?D-cx*_t;pe!8jAk2}bP|T66_%1vu&mMbJUg1{I z^p2xF>7`Awm7-T7D=NMC@?dTx6psd2bEi|q<9&b=X7wEWv9F+&*qu8{2y!h7{qO*K zuxL>ZdTul~<9Vq*Kh=mKj~Fx*YVcBiWO!US3Y=W9s|rD>s`&H+s6_gz#mFEVX;ld2 zCjWOXD24;IGx1~IO;tU?E;rnl3`<-z1;iGfuBF1Zsp zgRa>3h{4696HP4STe(2CY*Ou*er*P?zqU?WE@G>(^``V`eTI=%CWd%&}v2--< zn?qLYqV6O^d*ngi&SywOxUj`F|K$JaDRtn_YOR1Ru&b&ZW(R6?D zRw;VbH+r>>lFY?|9OG4z^FjqKspZ-NUHt@0}Q}j%{c|Kxc z;HByj5eV-;6VFAH@ji~fjQuJ%*3>9J0(P5t4Zr+@vFOYFc?6joA9dL8P8BfUKa+Iy zByg=wH^tsK>MSZqF@!QUuTGyYK&DY9{U&$1esu3@q|~dS^RcQO(Yp7Jr&%>*)O0Dk zeyf=I)Ubs-qr3crizw5-c8w22+3|ZaxhC&@HEdM}PT89Jd$$&ft`voLdlE2UEv}h0 zQyN0P_hnzzQ`{8ZRl}vkCmE%{E`C%`k#JymXT10x z&Moz{`Vjit`B+ZKgcD#eNI^8v8?#xiw!P_A;wU#FkIzPJ)S-_}t1W)hi&MKcB;Hqh z6jU!71%Rv7-rDkrV49UZaO<;?;iEbhiU~*^Adcx~vNv-z+HD+~&PuEW$6QA3zc?SL z(%pnlV{@v^eigP&$7%ugX=%2F(AI45*fw#%Q}kG_?3(4QRDDW}qdHszcnd9e^~QKZ zA9&yJB5lRXw)_dwvinpSKJzcy)@r%dn1u$8YB-2|z+^Ki6ONa)bfH+R*bo$=v93BZ zn)EL%AL)U`@IWnQx4td<>=?jFsr86@!{NLyo-Mwf*Jqyng~7APfFNwc*TP0lD!_T`saIQHIH}AaPCKV1V>z|hFaa_P6npD(A^L56Ff&n z(Q@S8waT^n2)3>f_8jX-s^w<8pXVhd?gWBW>6#aEo?Z^_E~HCZn@85X{e+kWqy1b#bDEaz6ddQwd!W;m%n zjHE7;;i-I=+rLWO`p%Px?r#2?cZ;gu9`X})fv?Xma41b8rN3SKtbRZpmO!(2qM z+f(X}yNleK9aIt}zrn%+(S<~=G(QM+jaa~CXGbY5lZQzzPNe|G zF7&p!&*}!g=q8Mvn#kA$nB=qI6t);I9&B&h@fNu+EEe5)fnT-TODGHmi>&UTsGmvX z4+-Q~HD^dHN8wz=;U6caj!8$7kJ7xQ6qO+eLIbWm%ePhkmT$B4wkF9&qh0RrI~AFQ z`|Kax6x3Nr4GD0)I-2e+z_Fx(yggqZDMb67V$VREY3i{Sd>F3{D&XGS7}cGdo1XHU?8mHg_lyr5?*^0Ur?dNp;tIhQC{9*`So(@H@_nb4NjNZMtJB z4HE&|NKLi&{dkQNZ*v{qgU@O%5qIcbIkX6{WI$hRwBb!L;0-mR2ZPH#upS|Bww}iLH(R@vG5Nf`d-DqcW zpwsR$5(5r={YJ0;E=M6zPclcM_80QnU81>CcFJ+Kn>*IpiX!5#lEeJzEs3^|{w0RY z@8vRwDnp_T8Ut?ytxzl*D;o&1>KDKC}%@%w;J0Nyv_U9*N|4~Acm zC7$SAou61;HfO$Ut-kObE0iud8iDboWU*_bPohuy{7OUg-y-k+B^)zWex$i zlQI!nM*70FpQ???DRc~)EyFC=jz0+v)R)nU7aSZCb9lB5q>b3^u!G2Mh z)Muoj-TG}|d^zpE&5(aFYi(#ns@2AkF4n2-pRK~zZpn5Hmfzmo`k^d+6HSxgS3Tfn z<)si0{al2qGrrQ9Wo{)NAeG-n;`wD|R97bXi+eN_O)B?vI;S{(-B2^+I=q z!<9WAef?NRi^@Rp;Q71Ppx~~wIpi>tf>gwXJ88XGz9i%nU%&`OcKH491GvkcVl8*; z0M6L^ZzL{RU^y@-=&OIIDT>%KWNMwgJTnfz{J*Ex2d4i{t&_%cX?<-+%?Mzu0r9R@ zXY+a{f49r8;L`g(?~>TUL!cockUCLAdmFPvr(T$-9QY;v!$;JNA5cumV(G&PRG0m6Zr>Y$dUXP9Yin}tQ66d^r>nD zzcqIai!`lgV;|Pl<>2kxCWK(vj7)}RYW8EIYhrt|>vjk`d6SofqpSK;02AtZV-Z8?)54>iZE9V$D=N#Q zB#&zNd5IdV8 ztb5a^(St6VwuHQyFIfNz>n^Tj9F8o}1-Ih5*a?omp+dvsx|iH~sa=cHw(2>W<6^0r z0!kXk01hh!;@*>As^Siwv~`B5;vLf9S))o{5;uEE19c`abR!gwV-TZB%38El{*7Pi7Eh=0h0)ITk2#_b&zeqHz9!90V&j`?O;_3F;{LYWAQV8bl;8_XBUCt0g{DBSFL&Z)&ssz2z$kJ*^Cyfpe2W^ zQ=BTDGwxX$CKs-t4o5o}UMdUsX4V~wz0sRzG&|vcgHRlZKYy*ou8RraMTBXI?ZmKt zxW3e7^^^_k6m$=&Kax{E1`w9r6kHoTW_K~@Hn6m5RQSAqVcP*`Yg9aT@~G&=?8S$$ z1Af)EUp0;HBQiGXq=oAgg|q3|$ksT!T|-8=55jlHYd=ztkzcmOyCjSdK?7Y=sTf^k zU{`&S`jMRb0BDV|baNuztQn`oH+60tsMs6~)h>R{Yl`h)>*Pns8+?`06%!PzB*CDU6KBH30s`CHs66er&lRkGBxZ6kQ%d`Q2!1?a zgq}U+Il=b3$4s#gBaXBQPR4z1ltZpEav0olYjSy@uH$!8|2!zG4@C|^VJV;x|5HvZ z_xi1Fm?|{In_5^Ay7ov*9#Tv~v{Cr9Av&Fr09C6a)lv@h6Qnazbz) zQ8WU=L`%)~(_&CFr|HS8Z^49z%xS|5!wBNlm7mwT1hMfs%0W*nmEK|Rz@2o+eNYZJ zcmbl?hR*#J=^Nb-#+Go$)852~-VQ=W=SK8E$uXj#a9l&~jxBK*4#G<*J1M&{X}huT z@6v0wykN#e9Jn^9yhV*cbGBh%I;-LQSzRlE$P_RIs#$OmqmQ;$!4i z(^pKIG%HjK2{I}CKd=_VFNgj(r}k}nUM=!lFsDaZQaZ=VO@PMeTr$q4?`Sl~AI!1{ z>Yg@jr{z+>bfhrYmvJf9^$(@Pf7YSjw&vL;uOiVBP}VwIYGR5tFZ2&@ zR8$;@aG9KF0Doq#1zBh18x0mQpWn$~A#0)Z?wuBV@9;;Oj8F|#HJ69K)J}{s|DlS0#=gcUUZ2Kfwr>j6rs(Ez(3f!-YqjPa_g+c_9 zLg_-U{EX(YW=8uVr{e4H`5tjFf=ZjyQO_uJ2a3q93|yRIC(XYsE@s~48?G2Yx2{e4 z&BRA1Cyf3MRU^e!vW{{Izpy!+RuWfR^Hx3RJvYEB#0ca)M`?VZifLL}_3?|b-aAiPSWjZDE;(g|8ql>GlP9j;xs!o?} z{=uq+ATa-%wm6%zxQlRPJv53pj2y~UB`RrRejhHJC$=zmy_OHhR?v!b8b9JG6Hn9? z$95~VA^ckUa^1-&r4YM=HV*6d6u$A&WYyB*m-c zR1Q4?Rj zXe^i)(7#vbXk&}$rtw%Jk?9_ovm zle2ibM?de|5d9*&$_pAhp->*R(+enwrp5mEYniI3u5@1t)&No?R4u^%asZq& zSLOl5loW4cG3f(pTe=6)5DDl6GsQ1+PG(3&H2A!>qBdH z7(2fR)zg;s=0j=Y*`^(hz70sI6r-j<GKb#P*oBYs(?gr4cj=G1$a zt%ttTH(_sJCE1jr!$R6qjPv4M;k+!te(Z7A%h4>{yO*YGWj${8=#YNfWvWJ9J#|4I zfp^gO@te`c!C6*4zBoNg{50;vPX<5neyS3DDYi_EUJncN60wb#gae9gZLo0~8L? zq2;Y^_i`Q55|?N^dn~J+KgxXF`fF+ zO_ZzwOB6B+ELJO^7n-Lokz5*HrDSYa?%sIfLg*QjtA^ambI@SU`%%w#)zxb*REN%E zTAidF>t?gXAl!GVVz{aoZ#B_Vi&TX3WeUp9EMepF$IT<&&aUZ#RkY*{hw((_OIaVB zJaOn4{E6{qZQXWT&jv~K{X#EwV8@|<1PAm}kkj}HQDFau`iT2JF@o7`0JCK&l@5cj zRg~TeQ=wD~s|ePH4X5=2q{K~Z0afEla`?9+K-F%e>0&1!Z4MCGY9b6y^27e=1Gu2y zUIRHNnkR58;I-!i3S1COC(@s8b=EPoEOjnc9;}7`NhbtY`!^3`dfojW;a&^0`k1p? z(~NWjHt+GuU~R>}OucdBYpj-fGt?EU_fKgr`k)3$@cFGtZ#Gqna~?)M#-XelOg(3<60iEwl~tCt?Kepqn%}LN3T&(pkd&Yg~7w# z<1s2owjmE*m*u#0dnFeGeZ|1ufY1rV<5wYPDbGJK}T}KAeXmnxno$|ZQMDzjtQCQYy~=clDE(I zJge_&qk*w-X^pVy5_$>|8#K6>s1!tE+;a^!JJ}>~T((JSBCm?9k`JtJRy}fwx!}6cU{qwm#6!4lJ8}!EIU?UmoKVt=bwTEkNfgG$A4`%OnBAiUN>LZ zdCv;^fV1p%nA~UyoVBNJ-I`g1ltn3vB*Y?uk$_s{P}Kp#R-#FIR%ACQ24A91?S5{L z_B0deS};F-Y030dRUjqPm?nYYBzdOKYoe)X1P4ej4yyZ=i5Lc26O zqY)`sneSUHcfjmpa-pc#TM`sI_pn;GAfPAW494f@BMzhHG`tuE!6xJnXR=4uVR6Xj z>}QT#xb=J#T0tYUBpCIcJd&#T=A5LN=&=lGnpw``u@(4#)%q-zN(qO3SY%$KHy3H#p`Cq91@kh56w+bXT;Gwr;1l2x zOg~S*zbLjHTg}MlFz)xm<{ReHYf)j%J6@}unjiH{G-jjQfB1Ge436gYar=uO1zKf= zNgaA;vuxXaPvjuvHK_Jr7SPv?oAo#e=%%%4J1RdmK3~LAZCi70OSOHjM9Z_0g z9mNgi+F2TBbSdbbefxSyKmW}T*1;dD?hs$K94mr_t7LEq^}& z2%FNKdobo4?=kMVM8zA3*^fYaOIDhx-ZGnP+g-TnX z=3n8}7RdFlK$)p+2gLf%-i!+n>2LP`ux)k}+W|9ye~$|}0J+d0*CCTM@9VuL4gjo) zX9hR``L+L~t?U1XuJaF3>wh*AiT}{Hswx1XZ~!=ff2lnGn~ClZmFIspXQ@miM<9{# zpS=IOxsd;(0+9yZ0RZOCP9By{9)H+B|IPm2rU5wuiJ@G(GXougLVv5!4M~7@Rs?4H z=>S<}|5g9@Y7QNVy%LcC09r`^0P^==+qgOq;O^n;VELD1^xyOTEk@Z1QcR*zro9u8 z{%aj542`fQ-xc^l@0HE9?6X*)0gA+A{90vRkTO_Tl