From dadd8703b422523d88b02effe48e76152bcb2fce Mon Sep 17 00:00:00 2001 From: Simon Wilks Date: Fri, 28 Jun 2013 08:42:05 +0200 Subject: [PATCH 01/22] Initial non-tested code for reading from the ESC. --- .../hott_telemetry/hott_telemetry_main.c | 44 +++++++++++++ src/drivers/hott_telemetry/messages.c | 28 ++++++++- src/drivers/hott_telemetry/messages.h | 62 ++++++++++++++----- 3 files changed, 117 insertions(+), 17 deletions(-) diff --git a/src/drivers/hott_telemetry/hott_telemetry_main.c b/src/drivers/hott_telemetry/hott_telemetry_main.c index 1d2bdd92ee..2c954e41e5 100644 --- a/src/drivers/hott_telemetry/hott_telemetry_main.c +++ b/src/drivers/hott_telemetry/hott_telemetry_main.c @@ -149,6 +149,29 @@ recv_req_id(int uart, uint8_t *id) return OK; } +int +recv_data(int uart, uint8_t *buffer, size_t *size, uint8_t id) +{ + usleep(100000); + + static const int timeout_ms = 200; + struct pollfd fds[] = { { .fd = uart, .events = POLLIN } }; + + int i = 0; + if (poll(fds, 1, timeout_ms) > 0) { + while (true) { + read(uart, &buffer[i], sizeof(buffer[i])); + + if (&buffer[i] == STOP_BYTE) { + *size = ++i; + id = &buffer[1]; + return OK; + } + } + } + return ERROR; +} + int send_data(int uart, uint8_t *buffer, size_t size) { @@ -218,6 +241,7 @@ hott_telemetry_thread_main(int argc, char *argv[]) bool connected = true; while (!thread_should_exit) { + // Listen for and serve poll from the receiver. if (recv_req_id(uart, &id) == OK) { if (!connected) { connected = true; @@ -242,6 +266,26 @@ hott_telemetry_thread_main(int argc, char *argv[]) connected = false; warnx("syncing"); } + + // Poll the next HoTT devices. + // TODO(sjwilks): Currently there is only one but if there would be more we would round-robin + // calling one for every loop iteration. + build_esc_request(&buffer, &size); + send_data(uart, buffer, size); + + // Listen for a response. + recv_data(uart, &buffer, &size, &id); + + for (size_t i = 0; i < size; i++) { + warnx("%d", buffer[i]); + } + + // Determine which moduel sent it and process accordingly. + if (id == ESC_SENSOR_ID) { + extract_esc_message(buffer); + } else { + warnx("Unknown sensor ID received: %d", id); + } } warnx("exiting"); diff --git a/src/drivers/hott_telemetry/messages.c b/src/drivers/hott_telemetry/messages.c index d2634ef41a..149c4d3675 100644 --- a/src/drivers/hott_telemetry/messages.c +++ b/src/drivers/hott_telemetry/messages.c @@ -73,6 +73,31 @@ messages_init(void) airspeed_sub = orb_subscribe(ORB_ID(airspeed)); } +void +build_esc_request(uint8_t *buffer, size_t *size) +{ + struct esc_module_poll_msg msg; + *size = sizeof(msg); + memset(&msg, 0, *size); + + msg.mode = BINARY_MODE_REQUEST_ID; + msg.id = ESC_SENSOR_ID; + + memcpy(&msg, buffer, size); +} + +void +extract_esc_message(const uint8_t *buffer) +{ + struct esc_module_msg msg; + size_t size = sizeof(msg); + memset(&msg, 0, size); + memcpy(buffer, &msg, size); + + // Publish it. + +} + void build_eam_response(uint8_t *buffer, size_t *size) { @@ -92,7 +117,7 @@ build_eam_response(uint8_t *buffer, size_t *size) msg.start = START_BYTE; msg.eam_sensor_id = EAM_SENSOR_ID; - msg.sensor_id = EAM_SENSOR_TEXT_ID; + msg.sensor_text_id = EAM_SENSOR_TEXT_ID; msg.temperature1 = (uint8_t)(raw.baro_temp_celcius + 20); msg.temperature2 = msg.temperature1 - BOARD_TEMP_OFFSET_DEG; @@ -111,7 +136,6 @@ build_eam_response(uint8_t *buffer, size_t *size) uint16_t speed = (uint16_t)(airspeed.indicated_airspeed_m_s); msg.speed_L = (uint8_t)speed & 0xff; msg.speed_H = (uint8_t)(speed >> 8) & 0xff; - msg.stop = STOP_BYTE; memcpy(buffer, &msg, *size); diff --git a/src/drivers/hott_telemetry/messages.h b/src/drivers/hott_telemetry/messages.h index e6d5cc7239..e1a4262a1c 100644 --- a/src/drivers/hott_telemetry/messages.h +++ b/src/drivers/hott_telemetry/messages.h @@ -60,19 +60,47 @@ #define STOP_BYTE 0x7d #define TEMP_ZERO_CELSIUS 0x14 +#define ESC_SENSOR_ID 0x8e + +/* The ESC Module poll message. */ +struct esc_module_poll_msg { + uint8_t mode; + uint8_t id; +}; + +/* The Electric Air Module message. */ +struct esc_module_msg { + uint8_t start; /**< Start byte */ + uint8_t sensor_id; + uint8_t warning; + uint8_t sensor_text_id; + uint8_t alarm_inverse1; + uint8_t alarm_inverse2; + uint8_t temperature1; /**< Temperature sensor 1. 20 = 0 degrees */ + uint8_t temperature2; + uint8_t current_L; /**< Current (mAh) lower 8-bits in steps of 0.1V */ + uint8_t current_H; + uint8_t rpm_L; /**< RPM Lower 8-bits In steps of 10 U/min */ + uint8_t rpm_H; + uint8_t speed_L; /**< Airspeed in km/h in steps of 1 km/h */ + uint8_t speed_H; + uint8_t stop; /**< Stop byte */ + uint8_t checksum; /**< Lower 8-bits of all bytes summed. */ +}; + /* Electric Air Module (EAM) constants. */ #define EAM_SENSOR_ID 0x8e #define EAM_SENSOR_TEXT_ID 0xe0 /* The Electric Air Module message. */ struct eam_module_msg { - uint8_t start; /**< Start byte */ + uint8_t start; /**< Start byte */ uint8_t eam_sensor_id; /**< EAM sensor */ uint8_t warning; - uint8_t sensor_id; /**< Sensor ID, why different? */ + uint8_t sensor_text_id; uint8_t alarm_inverse1; uint8_t alarm_inverse2; - uint8_t cell1_L; /**< Lipo cell voltages. Not supported. */ + uint8_t cell1_L; /**< Lipo cell voltages. Not supported. */ uint8_t cell2_L; uint8_t cell3_L; uint8_t cell4_L; @@ -92,9 +120,9 @@ struct eam_module_msg { uint8_t batt2_voltage_H; uint8_t temperature1; /**< Temperature sensor 1. 20 = 0 degrees */ uint8_t temperature2; - uint8_t altitude_L; /**< Attitude (meters) lower 8-bits. 500 = 0 meters */ + uint8_t altitude_L; /**< Attitude (meters) lower 8-bits. 500 = 0 meters */ uint8_t altitude_H; - uint8_t current_L; /**< Current (mAh) lower 8-bits in steps of 0.1V */ + uint8_t current_L; /**< Current (mAh) lower 8-bits in steps of 0.1V */ uint8_t current_H; uint8_t main_voltage_L; /**< Main power voltage lower 8-bits in steps of 0.1V */ uint8_t main_voltage_H; @@ -103,21 +131,21 @@ struct eam_module_msg { uint8_t climbrate_L; /**< Climb rate in 0.01m/s. 0m/s = 30000 */ uint8_t climbrate_H; uint8_t climbrate_3s; /**< Climb rate in m/3sec. 0m/3sec = 120 */ - uint8_t rpm_L; /**< RPM Lower 8-bits In steps of 10 U/min */ + uint8_t rpm_L; /**< RPM Lower 8-bits In steps of 10 U/min */ uint8_t rpm_H; uint8_t electric_min; /**< Flight time in minutes. */ uint8_t electric_sec; /**< Flight time in seconds. */ - uint8_t speed_L; /**< Airspeed in km/h in steps of 1 km/h */ + uint8_t speed_L; /**< Airspeed in km/h in steps of 1 km/h */ uint8_t speed_H; - uint8_t stop; /**< Stop byte */ - uint8_t checksum; /**< Lower 8-bits of all bytes summed. */ + uint8_t stop; /**< Stop byte */ + uint8_t checksum; /**< Lower 8-bits of all bytes summed. */ }; /** * The maximum buffer size required to store a HoTT message. */ -#define MESSAGE_BUFFER_SIZE sizeof(union { \ - struct eam_module_msg eam; \ +#define EAM_MESSAGE_BUFFER_SIZE sizeof(union { \ + struct eam_module_msg eam; \ }) /* GPS sensor constants. */ @@ -129,9 +157,9 @@ struct eam_module_msg { * Struct based on: https://code.google.com/p/diy-hott-gps/downloads */ struct gps_module_msg { - uint8_t start; /**< Start byte */ - uint8_t sensor_id; /**< GPS sensor ID*/ - uint8_t warning; /**< Byte 3: 0…= warning beeps */ + uint8_t start; /**< Start byte */ + uint8_t sensor_id; /**< GPS sensor ID*/ + uint8_t warning; /**< Byte 3: 0…= warning beeps */ uint8_t sensor_text_id; /**< GPS Sensor text mode ID */ uint8_t alarm_inverse1; /**< Byte 5: 01 inverse status */ uint8_t alarm_inverse2; /**< Byte 6: 00 inverse status status 1 = no GPS Signal */ @@ -183,10 +211,14 @@ struct gps_module_msg { * The maximum buffer size required to store a HoTT message. */ #define GPS_MESSAGE_BUFFER_SIZE sizeof(union { \ - struct gps_module_msg gps; \ + struct gps_module_msg gps; \ }) +#define MESSAGE_BUFFER_SIZE GPS_MESSAGE_BUFFER_SIZE + void messages_init(void); +void build_esc_request(uint8_t *buffer, size_t *size); +void extract_esc_message(const uint8_t *buffer); void build_eam_response(uint8_t *buffer, size_t *size); void build_gps_response(uint8_t *buffer, size_t *size); float _get_distance_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next); From d3eb86d0ea000add6e2747fda58f77a88b05314c Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sat, 20 Apr 2013 09:14:26 +0400 Subject: [PATCH 02/22] Publish manual_sas_mode immediately, SAS modes switch order changed to more safe --- src/modules/commander/commander.c | 16 ++++++++++++---- 1 file changed, 12 insertions(+), 4 deletions(-) diff --git a/src/modules/commander/commander.c b/src/modules/commander/commander.c index aab8f3e04d..bb8580328f 100644 --- a/src/modules/commander/commander.c +++ b/src/modules/commander/commander.c @@ -1389,6 +1389,7 @@ int commander_thread_main(int argc, char *argv[]) uint64_t start_time = hrt_absolute_time(); uint64_t failsave_ll_start_time = 0; + enum VEHICLE_MANUAL_SAS_MODE manual_sas_mode; bool state_changed = true; bool param_init_forced = true; @@ -1828,8 +1829,9 @@ int commander_thread_main(int argc, char *argv[]) } else if (sp_man.manual_sas_switch < -STICK_ON_OFF_LIMIT) { - /* bottom stick position, set altitude hold */ - current_status.manual_sas_mode = VEHICLE_MANUAL_SAS_MODE_ALTITUDE; + /* bottom stick position, set default */ + /* this MUST be mapped to extremal position to switch easy in case of emergency */ + current_status.manual_sas_mode = VEHICLE_MANUAL_SAS_MODE_ROLL_PITCH_ABS_YAW_ABS; } else if (sp_man.manual_sas_switch > STICK_ON_OFF_LIMIT) { @@ -1837,8 +1839,14 @@ int commander_thread_main(int argc, char *argv[]) current_status.manual_sas_mode = VEHICLE_MANUAL_SAS_MODE_SIMPLE; } else { - /* center stick position, set default */ - current_status.manual_sas_mode = VEHICLE_MANUAL_SAS_MODE_ROLL_PITCH_ABS_YAW_ABS; + /* center stick position, set altitude hold */ + current_status.manual_sas_mode = VEHICLE_MANUAL_SAS_MODE_ALTITUDE; + } + + if (current_status.manual_sas_mode != manual_sas_mode) { + /* publish SAS mode changes immediately */ + manual_sas_mode = current_status.manual_sas_mode; + state_changed = true; } /* From 2f1de6621b34f76ddf3a0ff00ac8e9fcb8e60bea Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sat, 29 Jun 2013 20:49:54 +0400 Subject: [PATCH 03/22] More strict conditions for arm/disarm --- src/modules/commander/commander.c | 11 ++++++++--- 1 file changed, 8 insertions(+), 3 deletions(-) diff --git a/src/modules/commander/commander.c b/src/modules/commander/commander.c index bb8580328f..bb3aac0ff1 100644 --- a/src/modules/commander/commander.c +++ b/src/modules/commander/commander.c @@ -1857,8 +1857,10 @@ int commander_thread_main(int argc, char *argv[]) (current_status.system_type == VEHICLE_TYPE_HEXAROTOR) || (current_status.system_type == VEHICLE_TYPE_OCTOROTOR) ) && - ((sp_man.yaw < -STICK_ON_OFF_LIMIT)) && - (sp_man.throttle < STICK_THRUST_RANGE * 0.2f)) { + current_status.flag_control_manual_enabled && + current_status.manual_sas_mode == VEHICLE_MANUAL_SAS_MODE_ROLL_PITCH_ABS_YAW_ABS && + sp_man.yaw < -STICK_ON_OFF_LIMIT && + sp_man.throttle < STICK_THRUST_RANGE * 0.1f) { if (stick_off_counter > STICK_ON_OFF_COUNTER_LIMIT) { update_state_machine_disarm(stat_pub, ¤t_status, mavlink_fd); stick_on_counter = 0; @@ -1870,7 +1872,10 @@ int commander_thread_main(int argc, char *argv[]) } /* check if left stick is in lower right position --> arm */ - if (sp_man.yaw > STICK_ON_OFF_LIMIT && sp_man.throttle < STICK_THRUST_RANGE * 0.2f) { + if (current_status.flag_control_manual_enabled && + current_status.manual_sas_mode == VEHICLE_MANUAL_SAS_MODE_ROLL_PITCH_ABS_YAW_ABS && + sp_man.yaw > STICK_ON_OFF_LIMIT && + sp_man.throttle < STICK_THRUST_RANGE * 0.1f) { if (stick_on_counter > STICK_ON_OFF_COUNTER_LIMIT) { update_state_machine_arm(stat_pub, ¤t_status, mavlink_fd); stick_on_counter = 0; From 6d2f14e125062be623c710c4b739c46be44d0adf Mon Sep 17 00:00:00 2001 From: Simon Wilks Date: Thu, 4 Jul 2013 08:33:19 +0200 Subject: [PATCH 04/22] Refactoring of the hott telemetry driver and added functionality to read from a HoTT sensor. --- makefiles/config_px4fmu_default.mk | 3 +- .../hott_telemetry/hott_telemetry_main.c | 344 ------------------ src/drivers/hott_telemetry/messages.c | 268 -------------- src/drivers/hott_telemetry/messages.h | 228 ------------ src/drivers/hott_telemetry/module.mk | 41 --- 5 files changed, 2 insertions(+), 882 deletions(-) delete mode 100644 src/drivers/hott_telemetry/hott_telemetry_main.c delete mode 100644 src/drivers/hott_telemetry/messages.c delete mode 100644 src/drivers/hott_telemetry/messages.h delete mode 100644 src/drivers/hott_telemetry/module.mk diff --git a/makefiles/config_px4fmu_default.mk b/makefiles/config_px4fmu_default.mk index 43288537cb..0d9ca60c16 100644 --- a/makefiles/config_px4fmu_default.mk +++ b/makefiles/config_px4fmu_default.mk @@ -27,7 +27,8 @@ MODULES += drivers/ms5611 MODULES += drivers/mb12xx MODULES += drivers/gps MODULES += drivers/hil -MODULES += drivers/hott_telemetry +MODULES += drivers/hott/hott_telemetry +MODULES += drivers/hott/hott_sensors MODULES += drivers/blinkm MODULES += drivers/mkblctrl MODULES += drivers/md25 diff --git a/src/drivers/hott_telemetry/hott_telemetry_main.c b/src/drivers/hott_telemetry/hott_telemetry_main.c deleted file mode 100644 index 2c954e41e5..0000000000 --- a/src/drivers/hott_telemetry/hott_telemetry_main.c +++ /dev/null @@ -1,344 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. - * Author: Simon Wilks - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file hott_telemetry_main.c - * @author Simon Wilks - * - * Graupner HoTT Telemetry implementation. - * - * The HoTT receiver polls each device at a regular interval at which point - * a data packet can be returned if necessary. - * - * TODO: Add support for at least the vario and GPS sensor data. - */ - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include "messages.h" - -static int thread_should_exit = false; /**< Deamon exit flag */ -static int thread_running = false; /**< Deamon status flag */ -static int deamon_task; /**< Handle of deamon task / thread */ -static const char daemon_name[] = "hott_telemetry"; -static const char commandline_usage[] = "usage: hott_telemetry start|status|stop [-d ]"; - -/** - * Deamon management function. - */ -__EXPORT int hott_telemetry_main(int argc, char *argv[]); - -/** - * Mainloop of daemon. - */ -int hott_telemetry_thread_main(int argc, char *argv[]); - -static int recv_req_id(int uart, uint8_t *id); -static int send_data(int uart, uint8_t *buffer, size_t size); - -static int -open_uart(const char *device, struct termios *uart_config_original) -{ - /* baud rate */ - static const speed_t speed = B19200; - - /* open uart */ - const int uart = open(device, O_RDWR | O_NOCTTY); - - if (uart < 0) { - err(1, "Error opening port: %s", device); - } - - /* Back up the original uart configuration to restore it after exit */ - int termios_state; - if ((termios_state = tcgetattr(uart, uart_config_original)) < 0) { - close(uart); - err(1, "Error getting baudrate / termios config for %s: %d", device, termios_state); - } - - /* Fill the struct for the new configuration */ - struct termios uart_config; - tcgetattr(uart, &uart_config); - - /* Clear ONLCR flag (which appends a CR for every LF) */ - uart_config.c_oflag &= ~ONLCR; - - /* Set baud rate */ - if (cfsetispeed(&uart_config, speed) < 0 || cfsetospeed(&uart_config, speed) < 0) { - close(uart); - err(1, "Error setting baudrate / termios config for %s: %d (cfsetispeed, cfsetospeed)", - device, termios_state); - } - - if ((termios_state = tcsetattr(uart, TCSANOW, &uart_config)) < 0) { - close(uart); - err(1, "Error setting baudrate / termios config for %s (tcsetattr)", device); - } - - /* Activate single wire mode */ - ioctl(uart, TIOCSSINGLEWIRE, SER_SINGLEWIRE_ENABLED); - - return uart; -} - -int -recv_req_id(int uart, uint8_t *id) -{ - static const int timeout_ms = 1000; // TODO make it a define - struct pollfd fds[] = { { .fd = uart, .events = POLLIN } }; - - uint8_t mode; - - if (poll(fds, 1, timeout_ms) > 0) { - /* Get the mode: binary or text */ - read(uart, &mode, sizeof(mode)); - - /* if we have a binary mode request */ - if (mode != BINARY_MODE_REQUEST_ID) { - return ERROR; - } - - /* Read the device ID being polled */ - read(uart, id, sizeof(*id)); - } else { - warnx("UART timeout on TX/RX port"); - return ERROR; - } - - return OK; -} - -int -recv_data(int uart, uint8_t *buffer, size_t *size, uint8_t id) -{ - usleep(100000); - - static const int timeout_ms = 200; - struct pollfd fds[] = { { .fd = uart, .events = POLLIN } }; - - int i = 0; - if (poll(fds, 1, timeout_ms) > 0) { - while (true) { - read(uart, &buffer[i], sizeof(buffer[i])); - - if (&buffer[i] == STOP_BYTE) { - *size = ++i; - id = &buffer[1]; - return OK; - } - } - } - return ERROR; -} - -int -send_data(int uart, uint8_t *buffer, size_t size) -{ - usleep(POST_READ_DELAY_IN_USECS); - - uint16_t checksum = 0; - - for (size_t i = 0; i < size; i++) { - if (i == size - 1) { - /* Set the checksum: the first uint8_t is taken as the checksum. */ - buffer[i] = checksum & 0xff; - - } else { - checksum += buffer[i]; - } - - write(uart, &buffer[i], sizeof(buffer[i])); - - /* Sleep before sending the next byte. */ - usleep(POST_WRITE_DELAY_IN_USECS); - } - - /* A hack the reads out what was written so the next read from the receiver doesn't get it. */ - /* TODO: Fix this!! */ - uint8_t dummy[size]; - read(uart, &dummy, size); - - return OK; -} - -int -hott_telemetry_thread_main(int argc, char *argv[]) -{ - warnx("starting"); - - thread_running = true; - - const char *device = "/dev/ttyS1"; /**< Default telemetry port: USART2 */ - - /* read commandline arguments */ - for (int i = 0; i < argc && argv[i]; i++) { - if (strcmp(argv[i], "-d") == 0 || strcmp(argv[i], "--device") == 0) { //device set - if (argc > i + 1) { - device = argv[i + 1]; - - } else { - thread_running = false; - errx(1, "missing parameter to -d\n%s", commandline_usage); - } - } - } - - /* enable UART, writes potentially an empty buffer, but multiplexing is disabled */ - struct termios uart_config_original; - const int uart = open_uart(device, &uart_config_original); - - if (uart < 0) { - errx(1, "Failed opening HoTT UART, exiting."); - thread_running = false; - } - - messages_init(); - - uint8_t buffer[MESSAGE_BUFFER_SIZE]; - size_t size = 0; - uint8_t id = 0; - bool connected = true; - - while (!thread_should_exit) { - // Listen for and serve poll from the receiver. - if (recv_req_id(uart, &id) == OK) { - if (!connected) { - connected = true; - warnx("OK"); - } - - switch (id) { - case EAM_SENSOR_ID: - build_eam_response(buffer, &size); - break; - - case GPS_SENSOR_ID: - build_gps_response(buffer, &size); - break; - - default: - continue; // Not a module we support. - } - - send_data(uart, buffer, size); - } else { - connected = false; - warnx("syncing"); - } - - // Poll the next HoTT devices. - // TODO(sjwilks): Currently there is only one but if there would be more we would round-robin - // calling one for every loop iteration. - build_esc_request(&buffer, &size); - send_data(uart, buffer, size); - - // Listen for a response. - recv_data(uart, &buffer, &size, &id); - - for (size_t i = 0; i < size; i++) { - warnx("%d", buffer[i]); - } - - // Determine which moduel sent it and process accordingly. - if (id == ESC_SENSOR_ID) { - extract_esc_message(buffer); - } else { - warnx("Unknown sensor ID received: %d", id); - } - } - - warnx("exiting"); - - close(uart); - - thread_running = false; - - return 0; -} - -/** - * Process command line arguments and tart the daemon. - */ -int -hott_telemetry_main(int argc, char *argv[]) -{ - if (argc < 1) { - errx(1, "missing command\n%s", commandline_usage); - } - - if (!strcmp(argv[1], "start")) { - - if (thread_running) { - warnx("deamon already running"); - exit(0); - } - - thread_should_exit = false; - deamon_task = task_spawn(daemon_name, - SCHED_DEFAULT, - SCHED_PRIORITY_MAX - 40, - 2048, - hott_telemetry_thread_main, - (argv) ? (const char **)&argv[2] : (const char **)NULL); - exit(0); - } - - if (!strcmp(argv[1], "stop")) { - thread_should_exit = true; - exit(0); - } - - if (!strcmp(argv[1], "status")) { - if (thread_running) { - warnx("daemon is running"); - - } else { - warnx("daemon not started"); - } - - exit(0); - } - - errx(1, "unrecognized command\n%s", commandline_usage); -} diff --git a/src/drivers/hott_telemetry/messages.c b/src/drivers/hott_telemetry/messages.c deleted file mode 100644 index 149c4d3675..0000000000 --- a/src/drivers/hott_telemetry/messages.c +++ /dev/null @@ -1,268 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * Author: @author Simon Wilks - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file messages.c - * - */ - -#include "messages.h" - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -/* The board is very roughly 5 deg warmer than the surrounding air */ -#define BOARD_TEMP_OFFSET_DEG 5 - -static int battery_sub = -1; -static int gps_sub = -1; -static int home_sub = -1; -static int sensor_sub = -1; -static int airspeed_sub = -1; - -static bool home_position_set = false; -static double home_lat = 0.0d; -static double home_lon = 0.0d; - -void -messages_init(void) -{ - battery_sub = orb_subscribe(ORB_ID(battery_status)); - gps_sub = orb_subscribe(ORB_ID(vehicle_gps_position)); - home_sub = orb_subscribe(ORB_ID(home_position)); - sensor_sub = orb_subscribe(ORB_ID(sensor_combined)); - airspeed_sub = orb_subscribe(ORB_ID(airspeed)); -} - -void -build_esc_request(uint8_t *buffer, size_t *size) -{ - struct esc_module_poll_msg msg; - *size = sizeof(msg); - memset(&msg, 0, *size); - - msg.mode = BINARY_MODE_REQUEST_ID; - msg.id = ESC_SENSOR_ID; - - memcpy(&msg, buffer, size); -} - -void -extract_esc_message(const uint8_t *buffer) -{ - struct esc_module_msg msg; - size_t size = sizeof(msg); - memset(&msg, 0, size); - memcpy(buffer, &msg, size); - - // Publish it. - -} - -void -build_eam_response(uint8_t *buffer, size_t *size) -{ - /* get a local copy of the current sensor values */ - struct sensor_combined_s raw; - memset(&raw, 0, sizeof(raw)); - orb_copy(ORB_ID(sensor_combined), sensor_sub, &raw); - - /* get a local copy of the battery data */ - struct battery_status_s battery; - memset(&battery, 0, sizeof(battery)); - orb_copy(ORB_ID(battery_status), battery_sub, &battery); - - struct eam_module_msg msg; - *size = sizeof(msg); - memset(&msg, 0, *size); - - msg.start = START_BYTE; - msg.eam_sensor_id = EAM_SENSOR_ID; - msg.sensor_text_id = EAM_SENSOR_TEXT_ID; - - msg.temperature1 = (uint8_t)(raw.baro_temp_celcius + 20); - msg.temperature2 = msg.temperature1 - BOARD_TEMP_OFFSET_DEG; - - msg.main_voltage_L = (uint8_t)(battery.voltage_v * 10); - - uint16_t alt = (uint16_t)(raw.baro_alt_meter + 500); - msg.altitude_L = (uint8_t)alt & 0xff; - msg.altitude_H = (uint8_t)(alt >> 8) & 0xff; - - /* get a local copy of the airspeed data */ - struct airspeed_s airspeed; - memset(&airspeed, 0, sizeof(airspeed)); - orb_copy(ORB_ID(airspeed), airspeed_sub, &airspeed); - - uint16_t speed = (uint16_t)(airspeed.indicated_airspeed_m_s); - msg.speed_L = (uint8_t)speed & 0xff; - msg.speed_H = (uint8_t)(speed >> 8) & 0xff; - - msg.stop = STOP_BYTE; - memcpy(buffer, &msg, *size); -} - -void -build_gps_response(uint8_t *buffer, size_t *size) -{ - /* get a local copy of the current sensor values */ - struct sensor_combined_s raw; - memset(&raw, 0, sizeof(raw)); - orb_copy(ORB_ID(sensor_combined), sensor_sub, &raw); - - /* get a local copy of the battery data */ - struct vehicle_gps_position_s gps; - memset(&gps, 0, sizeof(gps)); - orb_copy(ORB_ID(vehicle_gps_position), gps_sub, &gps); - - struct gps_module_msg msg = { 0 }; - *size = sizeof(msg); - memset(&msg, 0, *size); - - msg.start = START_BYTE; - msg.sensor_id = GPS_SENSOR_ID; - msg.sensor_text_id = GPS_SENSOR_TEXT_ID; - - msg.gps_num_sat = gps.satellites_visible; - - /* The GPS fix type: 0 = none, 2 = 2D, 3 = 3D */ - msg.gps_fix_char = (uint8_t)(gps.fix_type + 48); - msg.gps_fix = (uint8_t)(gps.fix_type + 48); - - /* No point collecting more data if we don't have a 3D fix yet */ - if (gps.fix_type > 2) { - /* Current flight direction */ - msg.flight_direction = (uint8_t)(gps.cog_rad * M_RAD_TO_DEG_F); - - /* GPS speed */ - uint16_t speed = (uint16_t)(gps.vel_m_s * 3.6); - msg.gps_speed_L = (uint8_t)speed & 0xff; - msg.gps_speed_H = (uint8_t)(speed >> 8) & 0xff; - - /* Get latitude in degrees, minutes and seconds */ - double lat = ((double)(gps.lat))*1e-7d; - - /* Set the N or S specifier */ - msg.latitude_ns = 0; - if (lat < 0) { - msg.latitude_ns = 1; - lat = abs(lat); - } - - int deg; - int min; - int sec; - convert_to_degrees_minutes_seconds(lat, °, &min, &sec); - - uint16_t lat_min = (uint16_t)(deg * 100 + min); - msg.latitude_min_L = (uint8_t)lat_min & 0xff; - msg.latitude_min_H = (uint8_t)(lat_min >> 8) & 0xff; - uint16_t lat_sec = (uint16_t)(sec); - msg.latitude_sec_L = (uint8_t)lat_sec & 0xff; - msg.latitude_sec_H = (uint8_t)(lat_sec >> 8) & 0xff; - - /* Get longitude in degrees, minutes and seconds */ - double lon = ((double)(gps.lon))*1e-7d; - - /* Set the E or W specifier */ - msg.longitude_ew = 0; - if (lon < 0) { - msg.longitude_ew = 1; - lon = abs(lon); - } - - convert_to_degrees_minutes_seconds(lon, °, &min, &sec); - - uint16_t lon_min = (uint16_t)(deg * 100 + min); - msg.longitude_min_L = (uint8_t)lon_min & 0xff; - msg.longitude_min_H = (uint8_t)(lon_min >> 8) & 0xff; - uint16_t lon_sec = (uint16_t)(sec); - msg.longitude_sec_L = (uint8_t)lon_sec & 0xff; - msg.longitude_sec_H = (uint8_t)(lon_sec >> 8) & 0xff; - - /* Altitude */ - uint16_t alt = (uint16_t)(gps.alt*1e-3 + 500.0f); - msg.altitude_L = (uint8_t)alt & 0xff; - msg.altitude_H = (uint8_t)(alt >> 8) & 0xff; - - /* Get any (and probably only ever one) home_sub postion report */ - bool updated; - orb_check(home_sub, &updated); - if (updated) { - /* get a local copy of the home position data */ - struct home_position_s home; - memset(&home, 0, sizeof(home)); - orb_copy(ORB_ID(home_position), home_sub, &home); - - home_lat = ((double)(home.lat))*1e-7d; - home_lon = ((double)(home.lon))*1e-7d; - home_position_set = true; - } - - /* Distance from home */ - if (home_position_set) { - uint16_t dist = (uint16_t)get_distance_to_next_waypoint(home_lat, home_lon, lat, lon); - - msg.distance_L = (uint8_t)dist & 0xff; - msg.distance_H = (uint8_t)(dist >> 8) & 0xff; - - /* Direction back to home */ - uint16_t bearing = (uint16_t)(get_bearing_to_next_waypoint(home_lat, home_lon, lat, lon) * M_RAD_TO_DEG_F); - msg.home_direction = (uint8_t)bearing >> 1; - } - } - - msg.stop = STOP_BYTE; - memcpy(buffer, &msg, *size); -} - -void -convert_to_degrees_minutes_seconds(double val, int *deg, int *min, int *sec) -{ - *deg = (int)val; - - double delta = val - *deg; - const double min_d = delta * 60.0d; - *min = (int)min_d; - delta = min_d - *min; - *sec = (int)(delta * 10000.0d); -} diff --git a/src/drivers/hott_telemetry/messages.h b/src/drivers/hott_telemetry/messages.h deleted file mode 100644 index e1a4262a1c..0000000000 --- a/src/drivers/hott_telemetry/messages.h +++ /dev/null @@ -1,228 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. - * Author: Simon Wilks - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file messages.h - * @author Simon Wilks - * - * Graupner HoTT Telemetry message generation. - * - */ -#ifndef MESSAGES_H_ -#define MESSAGES_H - -#include - -/* The HoTT receiver demands a minimum 5ms period of silence after delivering its request. - * Note that the value specified here is lower than 5000 (5ms) as time is lost constucting - * the message after the read which takes some milliseconds. - */ -#define POST_READ_DELAY_IN_USECS 4000 -/* A pause of 3ms is required between each uint8_t sent back to the HoTT receiver. Much lower - * values can be used in practise though. - */ -#define POST_WRITE_DELAY_IN_USECS 2000 - -// Protocol constants. -#define BINARY_MODE_REQUEST_ID 0x80 // Binary mode request. -#define START_BYTE 0x7c -#define STOP_BYTE 0x7d -#define TEMP_ZERO_CELSIUS 0x14 - -#define ESC_SENSOR_ID 0x8e - -/* The ESC Module poll message. */ -struct esc_module_poll_msg { - uint8_t mode; - uint8_t id; -}; - -/* The Electric Air Module message. */ -struct esc_module_msg { - uint8_t start; /**< Start byte */ - uint8_t sensor_id; - uint8_t warning; - uint8_t sensor_text_id; - uint8_t alarm_inverse1; - uint8_t alarm_inverse2; - uint8_t temperature1; /**< Temperature sensor 1. 20 = 0 degrees */ - uint8_t temperature2; - uint8_t current_L; /**< Current (mAh) lower 8-bits in steps of 0.1V */ - uint8_t current_H; - uint8_t rpm_L; /**< RPM Lower 8-bits In steps of 10 U/min */ - uint8_t rpm_H; - uint8_t speed_L; /**< Airspeed in km/h in steps of 1 km/h */ - uint8_t speed_H; - uint8_t stop; /**< Stop byte */ - uint8_t checksum; /**< Lower 8-bits of all bytes summed. */ -}; - -/* Electric Air Module (EAM) constants. */ -#define EAM_SENSOR_ID 0x8e -#define EAM_SENSOR_TEXT_ID 0xe0 - -/* The Electric Air Module message. */ -struct eam_module_msg { - uint8_t start; /**< Start byte */ - uint8_t eam_sensor_id; /**< EAM sensor */ - uint8_t warning; - uint8_t sensor_text_id; - uint8_t alarm_inverse1; - uint8_t alarm_inverse2; - uint8_t cell1_L; /**< Lipo cell voltages. Not supported. */ - uint8_t cell2_L; - uint8_t cell3_L; - uint8_t cell4_L; - uint8_t cell5_L; - uint8_t cell6_L; - uint8_t cell7_L; - uint8_t cell1_H; - uint8_t cell2_H; - uint8_t cell3_H; - uint8_t cell4_H; - uint8_t cell5_H; - uint8_t cell6_H; - uint8_t cell7_H; - uint8_t batt1_voltage_L; /**< Battery 1 voltage, lower 8-bits in steps of 0.02V */ - uint8_t batt1_voltage_H; - uint8_t batt2_voltage_L; /**< Battery 2 voltage, lower 8-bits in steps of 0.02V */ - uint8_t batt2_voltage_H; - uint8_t temperature1; /**< Temperature sensor 1. 20 = 0 degrees */ - uint8_t temperature2; - uint8_t altitude_L; /**< Attitude (meters) lower 8-bits. 500 = 0 meters */ - uint8_t altitude_H; - uint8_t current_L; /**< Current (mAh) lower 8-bits in steps of 0.1V */ - uint8_t current_H; - uint8_t main_voltage_L; /**< Main power voltage lower 8-bits in steps of 0.1V */ - uint8_t main_voltage_H; - uint8_t battery_capacity_L; /**< Used battery capacity in steps of 10mAh */ - uint8_t battery_capacity_H; - uint8_t climbrate_L; /**< Climb rate in 0.01m/s. 0m/s = 30000 */ - uint8_t climbrate_H; - uint8_t climbrate_3s; /**< Climb rate in m/3sec. 0m/3sec = 120 */ - uint8_t rpm_L; /**< RPM Lower 8-bits In steps of 10 U/min */ - uint8_t rpm_H; - uint8_t electric_min; /**< Flight time in minutes. */ - uint8_t electric_sec; /**< Flight time in seconds. */ - uint8_t speed_L; /**< Airspeed in km/h in steps of 1 km/h */ - uint8_t speed_H; - uint8_t stop; /**< Stop byte */ - uint8_t checksum; /**< Lower 8-bits of all bytes summed. */ -}; - -/** - * The maximum buffer size required to store a HoTT message. - */ -#define EAM_MESSAGE_BUFFER_SIZE sizeof(union { \ - struct eam_module_msg eam; \ -}) - -/* GPS sensor constants. */ -#define GPS_SENSOR_ID 0x8A -#define GPS_SENSOR_TEXT_ID 0xA0 - -/** - * The GPS sensor message - * Struct based on: https://code.google.com/p/diy-hott-gps/downloads - */ -struct gps_module_msg { - uint8_t start; /**< Start byte */ - uint8_t sensor_id; /**< GPS sensor ID*/ - uint8_t warning; /**< Byte 3: 0…= warning beeps */ - uint8_t sensor_text_id; /**< GPS Sensor text mode ID */ - uint8_t alarm_inverse1; /**< Byte 5: 01 inverse status */ - uint8_t alarm_inverse2; /**< Byte 6: 00 inverse status status 1 = no GPS Signal */ - uint8_t flight_direction; /**< Byte 7: 119 = Flightdir./dir. 1 = 2°; 0° (North), 9 0° (East), 180° (South), 270° (West) */ - uint8_t gps_speed_L; /**< Byte 8: 8 = /GPS speed low byte 8km/h */ - uint8_t gps_speed_H; /**< Byte 9: 0 = /GPS speed high byte */ - - uint8_t latitude_ns; /**< Byte 10: 000 = N = 48°39’988 */ - uint8_t latitude_min_L; /**< Byte 11: 231 0xE7 = 0x12E7 = 4839 */ - uint8_t latitude_min_H; /**< Byte 12: 018 18 = 0x12 */ - uint8_t latitude_sec_L; /**< Byte 13: 171 220 = 0xDC = 0x03DC =0988 */ - uint8_t latitude_sec_H; /**< Byte 14: 016 3 = 0x03 */ - - uint8_t longitude_ew; /**< Byte 15: 000 = E= 9° 25’9360 */ - uint8_t longitude_min_L; /**< Byte 16: 150 157 = 0x9D = 0x039D = 0925 */ - uint8_t longitude_min_H; /**< Byte 17: 003 3 = 0x03 */ - uint8_t longitude_sec_L; /**< Byte 18: 056 144 = 0x90 0x2490 = 9360*/ - uint8_t longitude_sec_H; /**< Byte 19: 004 36 = 0x24 */ - - uint8_t distance_L; /**< Byte 20: 027 123 = /distance low byte 6 = 6 m */ - uint8_t distance_H; /**< Byte 21: 036 35 = /distance high byte */ - uint8_t altitude_L; /**< Byte 22: 243 244 = /Altitude low byte 500 = 0m */ - uint8_t altitude_H; /**< Byte 23: 001 1 = /Altitude high byte */ - uint8_t resolution_L; /**< Byte 24: 48 = Low Byte m/s resolution 0.01m 48 = 30000 = 0.00m/s (1=0.01m/s) */ - uint8_t resolution_H; /**< Byte 25: 117 = High Byte m/s resolution 0.01m */ - uint8_t unknown1; /**< Byte 26: 120 = 0m/3s */ - uint8_t gps_num_sat; /**< Byte 27: GPS.Satellites (number of satelites) (1 byte) */ - uint8_t gps_fix_char; /**< Byte 28: GPS.FixChar. (GPS fix character. display, if DGPS, 2D oder 3D) (1 byte) */ - uint8_t home_direction; /**< Byte 29: HomeDirection (direction from starting point to Model position) (1 byte) */ - uint8_t angle_x_direction; /**< Byte 30: angle x-direction (1 byte) */ - uint8_t angle_y_direction; /**< Byte 31: angle y-direction (1 byte) */ - uint8_t angle_z_direction; /**< Byte 32: angle z-direction (1 byte) */ - uint8_t gyro_x_L; /**< Byte 33: gyro x low byte (2 bytes) */ - uint8_t gyro_x_H; /**< Byte 34: gyro x high byte */ - uint8_t gyro_y_L; /**< Byte 35: gyro y low byte (2 bytes) */ - uint8_t gyro_y_H; /**< Byte 36: gyro y high byte */ - uint8_t gyro_z_L; /**< Byte 37: gyro z low byte (2 bytes) */ - uint8_t gyro_z_H; /**< Byte 38: gyro z high byte */ - uint8_t vibration; /**< Byte 39: vibration (1 bytes) */ - uint8_t ascii4; /**< Byte 40: 00 ASCII Free Character [4] */ - uint8_t ascii5; /**< Byte 41: 00 ASCII Free Character [5] */ - uint8_t gps_fix; /**< Byte 42: 00 ASCII Free Character [6], we use it for GPS FIX */ - uint8_t version; /**< Byte 43: 00 version number */ - uint8_t stop; /**< Byte 44: 0x7D Ende byte */ - uint8_t checksum; /**< Byte 45: Parity Byte */ -}; - -/** - * The maximum buffer size required to store a HoTT message. - */ -#define GPS_MESSAGE_BUFFER_SIZE sizeof(union { \ - struct gps_module_msg gps; \ -}) - -#define MESSAGE_BUFFER_SIZE GPS_MESSAGE_BUFFER_SIZE - -void messages_init(void); -void build_esc_request(uint8_t *buffer, size_t *size); -void extract_esc_message(const uint8_t *buffer); -void build_eam_response(uint8_t *buffer, size_t *size); -void build_gps_response(uint8_t *buffer, size_t *size); -float _get_distance_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next); -void convert_to_degrees_minutes_seconds(double lat, int *deg, int *min, int *sec); - - -#endif /* MESSAGES_H_ */ diff --git a/src/drivers/hott_telemetry/module.mk b/src/drivers/hott_telemetry/module.mk deleted file mode 100644 index def1d59e9b..0000000000 --- a/src/drivers/hott_telemetry/module.mk +++ /dev/null @@ -1,41 +0,0 @@ -############################################################################ -# -# Copyright (c) 2012, 2013 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 -# are met: -# -# 1. Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# 2. Redistributions in binary form must reproduce the above copyright -# notice, this list of conditions and the following disclaimer in -# the documentation and/or other materials provided with the -# distribution. -# 3. Neither the name PX4 nor the names of its contributors may be -# used to endorse or promote products derived from this software -# without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS -# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED -# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. -# -############################################################################ - -# -# Graupner HoTT Telemetry application. -# - -MODULE_COMMAND = hott_telemetry - -SRCS = hott_telemetry_main.c \ - messages.c From 49aca1ae5ba240fc9ae695e58ca392b8d61c939e Mon Sep 17 00:00:00 2001 From: Simon Wilks Date: Thu, 4 Jul 2013 08:50:34 +0200 Subject: [PATCH 05/22] Add in missing files. --- src/drivers/hott/comms.c | 106 +++++++ src/drivers/hott/comms.h | 58 ++++ src/drivers/hott/hott_sensors/hott_sensors.c | 232 ++++++++++++++ src/drivers/hott/hott_sensors/module.mk | 42 +++ .../hott/hott_telemetry/hott_telemetry.c | 287 ++++++++++++++++++ src/drivers/hott/hott_telemetry/module.mk | 42 +++ src/drivers/hott/messages.c | 271 +++++++++++++++++ src/drivers/hott/messages.h | 268 ++++++++++++++++ 8 files changed, 1306 insertions(+) create mode 100644 src/drivers/hott/comms.c create mode 100644 src/drivers/hott/comms.h create mode 100644 src/drivers/hott/hott_sensors/hott_sensors.c create mode 100644 src/drivers/hott/hott_sensors/module.mk create mode 100644 src/drivers/hott/hott_telemetry/hott_telemetry.c create mode 100644 src/drivers/hott/hott_telemetry/module.mk create mode 100644 src/drivers/hott/messages.c create mode 100644 src/drivers/hott/messages.h diff --git a/src/drivers/hott/comms.c b/src/drivers/hott/comms.c new file mode 100644 index 0000000000..4a7d3c8451 --- /dev/null +++ b/src/drivers/hott/comms.c @@ -0,0 +1,106 @@ +/**************************************************************************** + * + * Copyright (c) 2013 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 + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file comms.c + * @author Simon Wilks + * + */ + +#include "comms.h" + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +int +open_uart(const char *device, struct termios *uart_config_original) +{ + /* baud rate */ + static const speed_t speed = B19200; + + /* open uart */ + const int uart = open(device, O_RDWR | O_NOCTTY); + + if (uart < 0) { + err(1, "Error opening port: %s", device); + } + + /* Back up the original uart configuration to restore it after exit */ + int termios_state; + if ((termios_state = tcgetattr(uart, uart_config_original)) < 0) { + close(uart); + err(1, "Error getting baudrate / termios config for %s: %d", device, termios_state); + } + + /* Fill the struct for the new configuration */ + struct termios uart_config; + tcgetattr(uart, &uart_config); + + /* Clear ONLCR flag (which appends a CR for every LF) */ + uart_config.c_oflag &= ~ONLCR; + + /* Set baud rate */ + if (cfsetispeed(&uart_config, speed) < 0 || cfsetospeed(&uart_config, speed) < 0) { + close(uart); + err(1, "Error setting baudrate / termios config for %s: %d (cfsetispeed, cfsetospeed)", + device, termios_state); + } + + if ((termios_state = tcsetattr(uart, TCSANOW, &uart_config)) < 0) { + close(uart); + err(1, "Error setting baudrate / termios config for %s (tcsetattr)", device); + } + + /* Activate single wire mode */ + ioctl(uart, TIOCSSINGLEWIRE, SER_SINGLEWIRE_ENABLED); + + return uart; +} + + + + + + + + + + diff --git a/src/drivers/hott/comms.h b/src/drivers/hott/comms.h new file mode 100644 index 0000000000..a1173631dd --- /dev/null +++ b/src/drivers/hott/comms.h @@ -0,0 +1,58 @@ +/**************************************************************************** + * + * Copyright (c) 2013 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 + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file comms.h + * @author Simon Wilks + * + */ + + +#ifndef COMMS_H_ +#define COMMS_H + +#include + +int open_uart(const char *device, struct termios *uart_config_original); + +#endif /* COMMS_H_ */ + + + + + + + + + + diff --git a/src/drivers/hott/hott_sensors/hott_sensors.c b/src/drivers/hott/hott_sensors/hott_sensors.c new file mode 100644 index 0000000000..dc10685b47 --- /dev/null +++ b/src/drivers/hott/hott_sensors/hott_sensors.c @@ -0,0 +1,232 @@ +/**************************************************************************** + * + * Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. + * Author: Simon Wilks + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file hott_sensors.c + * @author Simon Wilks + * + * Graupner HoTT sensor driver implementation. + * + * Poll any sensors connected to the PX4 via the telemetry wire. + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "../comms.h" +#include "../messages.h" + +static int thread_should_exit = false; /**< Deamon exit flag */ +static int thread_running = false; /**< Deamon status flag */ +static int deamon_task; /**< Handle of deamon task / thread */ +static const char daemon_name[] = "hott_sensors"; +static const char commandline_usage[] = "usage: hott_sensor start|status|stop [-d ]"; + +/** + * Deamon management function. + */ +__EXPORT int hott_sensors_main(int argc, char *argv[]); + +/** + * Mainloop of daemon. + */ +int hott_sensors_thread_main(int argc, char *argv[]); + +static int recv_data(int uart, uint8_t *buffer, size_t *size, uint8_t *id); +static int send_poll(int uart, uint8_t *buffer, size_t size); + +int +send_poll(int uart, uint8_t *buffer, size_t size) +{ + for (size_t i = 0; i < size; i++) { + write(uart, &buffer[i], sizeof(buffer[i])); + + /* Sleep before sending the next byte. */ + usleep(POST_WRITE_DELAY_IN_USECS); + } + + /* A hack the reads out what was written so the next read from the receiver doesn't get it. */ + /* TODO: Fix this!! */ + uint8_t dummy[size]; + read(uart, &dummy, size); + + return OK; +} + +int +recv_data(int uart, uint8_t *buffer, size_t *size, uint8_t *id) +{ + usleep(5000); + + static const int timeout_ms = 1000; + struct pollfd fds[] = { { .fd = uart, .events = POLLIN } }; + + + // XXX should this poll be inside the while loop??? + if (poll(fds, 1, timeout_ms) > 0) { + int i = 0; + bool stop_byte_read = false; + while (true) { + read(uart, &buffer[i], sizeof(buffer[i])); + //printf("[%d]: %d\n", i, buffer[i]); + + if (stop_byte_read) { + // XXX process checksum + *size = ++i; + return OK; + } + // XXX can some other field not have the STOP BYTE value? + if (buffer[i] == STOP_BYTE) { + *id = buffer[1]; + stop_byte_read = true; + } + i++; + } + } + return ERROR; +} + +int +hott_sensors_thread_main(int argc, char *argv[]) +{ + warnx("starting"); + + thread_running = true; + + const char *device = "/dev/ttyS2"; /**< Default telemetry port: USART5 */ + + /* read commandline arguments */ + for (int i = 0; i < argc && argv[i]; i++) { + if (strcmp(argv[i], "-d") == 0 || strcmp(argv[i], "--device") == 0) { //device set + if (argc > i + 1) { + device = argv[i + 1]; + + } else { + thread_running = false; + errx(1, "missing parameter to -d\n%s", commandline_usage); + } + } + } + + /* enable UART, writes potentially an empty buffer, but multiplexing is disabled */ + struct termios uart_config_original; + const int uart = open_uart(device, &uart_config_original); + + if (uart < 0) { + errx(1, "Failed opening HoTT UART, exiting."); + thread_running = false; + } + + uint8_t buffer[MESSAGE_BUFFER_SIZE]; + size_t size = 0; + uint8_t id = 0; + bool connected = true; + while (!thread_should_exit) { + // Currently we only support a General Air Module sensor. + build_gam_request(&buffer, &size); + send_poll(uart, buffer, size); + recv_data(uart, &buffer, &size, &id); + + // Determine which moduel sent it and process accordingly. + if (id == GAM_SENSOR_ID) { + //warnx("extract"); + extract_gam_message(buffer); + } else { + warnx("Unknown sensor ID received: %d", id); + } + } + + warnx("exiting"); + + close(uart); + + thread_running = false; + + return 0; +} + +/** + * Process command line arguments and start the daemon. + */ +int +hott_sensors_main(int argc, char *argv[]) +{ + if (argc < 1) { + errx(1, "missing command\n%s", commandline_usage); + } + + if (!strcmp(argv[1], "start")) { + + if (thread_running) { + warnx("deamon already running"); + exit(0); + } + + thread_should_exit = false; + deamon_task = task_spawn(daemon_name, + SCHED_DEFAULT, + SCHED_PRIORITY_MAX - 40, + 2048, + hott_sensors_thread_main, + (argv) ? (const char **)&argv[2] : (const char **)NULL); + exit(0); + } + + if (!strcmp(argv[1], "stop")) { + thread_should_exit = true; + exit(0); + } + + if (!strcmp(argv[1], "status")) { + if (thread_running) { + warnx("daemon is running"); + + } else { + warnx("daemon not started"); + } + + exit(0); + } + + errx(1, "unrecognized command\n%s", commandline_usage); +} diff --git a/src/drivers/hott/hott_sensors/module.mk b/src/drivers/hott/hott_sensors/module.mk new file mode 100644 index 0000000000..ca65d3de22 --- /dev/null +++ b/src/drivers/hott/hott_sensors/module.mk @@ -0,0 +1,42 @@ +############################################################################ +# +# Copyright (c) 2012, 2013 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 +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ + +# +# Graupner HoTT Sensors application. +# + +MODULE_COMMAND = hott_sensors + +SRCS = hott_sensors.c \ + ../messages.c \ + ../comms.c diff --git a/src/drivers/hott/hott_telemetry/hott_telemetry.c b/src/drivers/hott/hott_telemetry/hott_telemetry.c new file mode 100644 index 0000000000..fc80ac4d23 --- /dev/null +++ b/src/drivers/hott/hott_telemetry/hott_telemetry.c @@ -0,0 +1,287 @@ +/**************************************************************************** + * + * Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. + * Author: Simon Wilks + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file hott_telemetry_main.c + * @author Simon Wilks + * + * Graupner HoTT Telemetry implementation. + * + * The HoTT receiver polls each device at a regular interval at which point + * a data packet can be returned if necessary. + * + */ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "../comms.h" +#include "../messages.h" + +static int thread_should_exit = false; /**< Deamon exit flag */ +static int thread_running = false; /**< Deamon status flag */ +static int deamon_task; /**< Handle of deamon task / thread */ +static const char daemon_name[] = "hott_telemetry"; +static const char commandline_usage[] = "usage: hott_telemetry start|status|stop [-d ]"; + +/** + * Deamon management function. + */ +__EXPORT int hott_telemetry_main(int argc, char *argv[]); + +/** + * Mainloop of daemon. + */ +int hott_telemetry_thread_main(int argc, char *argv[]); + +static int recv_req_id(int uart, uint8_t *id); +static int send_data(int uart, uint8_t *buffer, size_t size); + +int +recv_req_id(int uart, uint8_t *id) +{ + static const int timeout_ms = 1000; // TODO make it a define + struct pollfd fds[] = { { .fd = uart, .events = POLLIN } }; + + uint8_t mode; + + if (poll(fds, 1, timeout_ms) > 0) { + /* Get the mode: binary or text */ + read(uart, &mode, sizeof(mode)); + + /* if we have a binary mode request */ + if (mode != BINARY_MODE_REQUEST_ID) { + return ERROR; + } + + /* Read the device ID being polled */ + read(uart, id, sizeof(*id)); + } else { + warnx("UART timeout on TX/RX port"); + return ERROR; + } + + return OK; +} + +int +recv_data(int uart, uint8_t *buffer, size_t *size, uint8_t *id) +{ + usleep(5000); + + static const int timeout_ms = 1000; + struct pollfd fds[] = { { .fd = uart, .events = POLLIN } }; + + + // XXX should this poll be inside the while loop??? + if (poll(fds, 1, timeout_ms) > 0) { + int i = 0; + bool stop_byte_read = false; + while (true) { + read(uart, &buffer[i], sizeof(buffer[i])); + //printf("[%d]: %d\n", i, buffer[i]); + + if (stop_byte_read) { + // process checksum + *size = ++i; + return OK; + } + // XXX can some other field not have the STOP BYTE value? + if (buffer[i] == STOP_BYTE) { + *id = buffer[1]; + stop_byte_read = true; + } + i++; + } + } + return ERROR; +} + +int +send_data(int uart, uint8_t *buffer, size_t size) +{ + usleep(POST_READ_DELAY_IN_USECS); + + uint16_t checksum = 0; + for (size_t i = 0; i < size; i++) { + if (i == size - 1) { + /* Set the checksum: the first uint8_t is taken as the checksum. */ + buffer[i] = checksum & 0xff; + + } else { + checksum += buffer[i]; + } + + write(uart, &buffer[i], sizeof(buffer[i])); + + /* Sleep before sending the next byte. */ + usleep(POST_WRITE_DELAY_IN_USECS); + } + + /* A hack the reads out what was written so the next read from the receiver doesn't get it. */ + /* TODO: Fix this!! */ + uint8_t dummy[size]; + read(uart, &dummy, size); + + return OK; +} + +int +hott_telemetry_thread_main(int argc, char *argv[]) +{ + warnx("starting"); + + thread_running = true; + + const char *device = "/dev/ttyS1"; /**< Default telemetry port: USART2 */ + + /* read commandline arguments */ + for (int i = 0; i < argc && argv[i]; i++) { + if (strcmp(argv[i], "-d") == 0 || strcmp(argv[i], "--device") == 0) { //device set + if (argc > i + 1) { + device = argv[i + 1]; + + } else { + thread_running = false; + errx(1, "missing parameter to -d\n%s", commandline_usage); + } + } + } + + /* enable UART, writes potentially an empty buffer, but multiplexing is disabled */ + struct termios uart_config_original; + const int uart = open_uart(device, &uart_config_original); + + if (uart < 0) { + errx(1, "Failed opening HoTT UART, exiting."); + thread_running = false; + } + + messages_init(); + + uint8_t buffer[MESSAGE_BUFFER_SIZE]; + size_t size = 0; + uint8_t id = 0; + bool connected = true; + while (!thread_should_exit) { + // Listen for and serve poll from the receiver. + if (recv_req_id(uart, &id) == OK) { + if (!connected) { + connected = true; + warnx("OK"); + } + + switch (id) { + case EAM_SENSOR_ID: + build_eam_response(buffer, &size); + break; + + case GPS_SENSOR_ID: + build_gps_response(buffer, &size); + break; + + default: + continue; // Not a module we support. + } + + send_data(uart, buffer, size); + } else { + connected = false; + warnx("syncing"); + } + } + + warnx("exiting"); + + close(uart); + + thread_running = false; + + return 0; +} + +/** + * Process command line arguments and start the daemon. + */ +int +hott_telemetry_main(int argc, char *argv[]) +{ + if (argc < 1) { + errx(1, "missing command\n%s", commandline_usage); + } + + if (!strcmp(argv[1], "start")) { + + if (thread_running) { + warnx("deamon already running"); + exit(0); + } + + thread_should_exit = false; + deamon_task = task_spawn(daemon_name, + SCHED_DEFAULT, + SCHED_PRIORITY_MAX - 40, + 2048, + hott_telemetry_thread_main, + (argv) ? (const char **)&argv[2] : (const char **)NULL); + exit(0); + } + + if (!strcmp(argv[1], "stop")) { + thread_should_exit = true; + exit(0); + } + + if (!strcmp(argv[1], "status")) { + if (thread_running) { + warnx("daemon is running"); + + } else { + warnx("daemon not started"); + } + + exit(0); + } + + errx(1, "unrecognized command\n%s", commandline_usage); +} diff --git a/src/drivers/hott/hott_telemetry/module.mk b/src/drivers/hott/hott_telemetry/module.mk new file mode 100644 index 0000000000..7673d7e775 --- /dev/null +++ b/src/drivers/hott/hott_telemetry/module.mk @@ -0,0 +1,42 @@ +############################################################################ +# +# Copyright (c) 2012, 2013 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 +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ + +# +# Graupner HoTT Telemetry applications. +# + +MODULE_COMMAND = hott_telemetry + +SRCS = hott_telemetry.c \ + ../messages.c \ + ../comms.c diff --git a/src/drivers/hott/messages.c b/src/drivers/hott/messages.c new file mode 100644 index 0000000000..1a29b7e73d --- /dev/null +++ b/src/drivers/hott/messages.c @@ -0,0 +1,271 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Author: @author Simon Wilks + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file messages.c + * + */ + +#include "messages.h" + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +/* The board is very roughly 5 deg warmer than the surrounding air */ +#define BOARD_TEMP_OFFSET_DEG 5 + +static int battery_sub = -1; +static int gps_sub = -1; +static int home_sub = -1; +static int sensor_sub = -1; +static int airspeed_sub = -1; + +static bool home_position_set = false; +static double home_lat = 0.0d; +static double home_lon = 0.0d; + +void +messages_init(void) +{ + battery_sub = orb_subscribe(ORB_ID(battery_status)); + gps_sub = orb_subscribe(ORB_ID(vehicle_gps_position)); + home_sub = orb_subscribe(ORB_ID(home_position)); + sensor_sub = orb_subscribe(ORB_ID(sensor_combined)); + airspeed_sub = orb_subscribe(ORB_ID(airspeed)); +} + +void +build_gam_request(uint8_t *buffer, size_t *size) +{ + struct gam_module_poll_msg msg; + *size = sizeof(msg); + memset(&msg, 0, *size); + + msg.mode = BINARY_MODE_REQUEST_ID; + msg.id = GAM_SENSOR_ID; + + memcpy(buffer, &msg, *size); +} + +void +extract_gam_message(const uint8_t *buffer) +{ + struct gam_module_msg msg; + size_t size = sizeof(msg); + memset(&msg, 0, size); + memcpy(&msg, buffer, size); + + // Publish it. + uint16_t rpm = ((msg.rpm_H << 8) | (msg.rpm_L & 0xff)) * 10; + uint8_t temp = msg.temperature2 + 20; + float current = ((msg.current_H << 8) | (msg.current_L & 0xff)) * 0.1f; + printf("RPM: %d TEMP: %d A: %2.1f\n", rpm, temp, current); +} + +void +build_eam_response(uint8_t *buffer, size_t *size) +{ + /* get a local copy of the current sensor values */ + struct sensor_combined_s raw; + memset(&raw, 0, sizeof(raw)); + orb_copy(ORB_ID(sensor_combined), sensor_sub, &raw); + + /* get a local copy of the battery data */ + struct battery_status_s battery; + memset(&battery, 0, sizeof(battery)); + orb_copy(ORB_ID(battery_status), battery_sub, &battery); + + struct eam_module_msg msg; + *size = sizeof(msg); + memset(&msg, 0, *size); + + msg.start = START_BYTE; + msg.eam_sensor_id = EAM_SENSOR_ID; + msg.sensor_text_id = EAM_SENSOR_TEXT_ID; + + msg.temperature1 = (uint8_t)(raw.baro_temp_celcius - 20); + msg.temperature2 = msg.temperature1 - BOARD_TEMP_OFFSET_DEG; + + msg.main_voltage_L = (uint8_t)(battery.voltage_v * 10); + + uint16_t alt = (uint16_t)(raw.baro_alt_meter + 500); + msg.altitude_L = (uint8_t)alt & 0xff; + msg.altitude_H = (uint8_t)(alt >> 8) & 0xff; + + /* get a local copy of the airspeed data */ + struct airspeed_s airspeed; + memset(&airspeed, 0, sizeof(airspeed)); + orb_copy(ORB_ID(airspeed), airspeed_sub, &airspeed); + + uint16_t speed = (uint16_t)(airspeed.indicated_airspeed_m_s); + msg.speed_L = (uint8_t)speed & 0xff; + msg.speed_H = (uint8_t)(speed >> 8) & 0xff; + + msg.stop = STOP_BYTE; + memcpy(buffer, &msg, *size); +} + +void +build_gps_response(uint8_t *buffer, size_t *size) +{ + /* get a local copy of the current sensor values */ + struct sensor_combined_s raw; + memset(&raw, 0, sizeof(raw)); + orb_copy(ORB_ID(sensor_combined), sensor_sub, &raw); + + /* get a local copy of the battery data */ + struct vehicle_gps_position_s gps; + memset(&gps, 0, sizeof(gps)); + orb_copy(ORB_ID(vehicle_gps_position), gps_sub, &gps); + + struct gps_module_msg msg = { 0 }; + *size = sizeof(msg); + memset(&msg, 0, *size); + + msg.start = START_BYTE; + msg.sensor_id = GPS_SENSOR_ID; + msg.sensor_text_id = GPS_SENSOR_TEXT_ID; + + msg.gps_num_sat = gps.satellites_visible; + + /* The GPS fix type: 0 = none, 2 = 2D, 3 = 3D */ + msg.gps_fix_char = (uint8_t)(gps.fix_type + 48); + msg.gps_fix = (uint8_t)(gps.fix_type + 48); + + /* No point collecting more data if we don't have a 3D fix yet */ + if (gps.fix_type > 2) { + /* Current flight direction */ + msg.flight_direction = (uint8_t)(gps.cog_rad * M_RAD_TO_DEG_F); + + /* GPS speed */ + uint16_t speed = (uint16_t)(gps.vel_m_s * 3.6); + msg.gps_speed_L = (uint8_t)speed & 0xff; + msg.gps_speed_H = (uint8_t)(speed >> 8) & 0xff; + + /* Get latitude in degrees, minutes and seconds */ + double lat = ((double)(gps.lat))*1e-7d; + + /* Set the N or S specifier */ + msg.latitude_ns = 0; + if (lat < 0) { + msg.latitude_ns = 1; + lat = abs(lat); + } + + int deg; + int min; + int sec; + convert_to_degrees_minutes_seconds(lat, °, &min, &sec); + + uint16_t lat_min = (uint16_t)(deg * 100 + min); + msg.latitude_min_L = (uint8_t)lat_min & 0xff; + msg.latitude_min_H = (uint8_t)(lat_min >> 8) & 0xff; + uint16_t lat_sec = (uint16_t)(sec); + msg.latitude_sec_L = (uint8_t)lat_sec & 0xff; + msg.latitude_sec_H = (uint8_t)(lat_sec >> 8) & 0xff; + + /* Get longitude in degrees, minutes and seconds */ + double lon = ((double)(gps.lon))*1e-7d; + + /* Set the E or W specifier */ + msg.longitude_ew = 0; + if (lon < 0) { + msg.longitude_ew = 1; + lon = abs(lon); + } + + convert_to_degrees_minutes_seconds(lon, °, &min, &sec); + + uint16_t lon_min = (uint16_t)(deg * 100 + min); + msg.longitude_min_L = (uint8_t)lon_min & 0xff; + msg.longitude_min_H = (uint8_t)(lon_min >> 8) & 0xff; + uint16_t lon_sec = (uint16_t)(sec); + msg.longitude_sec_L = (uint8_t)lon_sec & 0xff; + msg.longitude_sec_H = (uint8_t)(lon_sec >> 8) & 0xff; + + /* Altitude */ + uint16_t alt = (uint16_t)(gps.alt*1e-3 + 500.0f); + msg.altitude_L = (uint8_t)alt & 0xff; + msg.altitude_H = (uint8_t)(alt >> 8) & 0xff; + + /* Get any (and probably only ever one) home_sub postion report */ + bool updated; + orb_check(home_sub, &updated); + if (updated) { + /* get a local copy of the home position data */ + struct home_position_s home; + memset(&home, 0, sizeof(home)); + orb_copy(ORB_ID(home_position), home_sub, &home); + + home_lat = ((double)(home.lat))*1e-7d; + home_lon = ((double)(home.lon))*1e-7d; + home_position_set = true; + } + + /* Distance from home */ + if (home_position_set) { + uint16_t dist = (uint16_t)get_distance_to_next_waypoint(home_lat, home_lon, lat, lon); + + msg.distance_L = (uint8_t)dist & 0xff; + msg.distance_H = (uint8_t)(dist >> 8) & 0xff; + + /* Direction back to home */ + uint16_t bearing = (uint16_t)(get_bearing_to_next_waypoint(home_lat, home_lon, lat, lon) * M_RAD_TO_DEG_F); + msg.home_direction = (uint8_t)bearing >> 1; + } + } + + msg.stop = STOP_BYTE; + memcpy(buffer, &msg, *size); +} + +void +convert_to_degrees_minutes_seconds(double val, int *deg, int *min, int *sec) +{ + *deg = (int)val; + + double delta = val - *deg; + const double min_d = delta * 60.0d; + *min = (int)min_d; + delta = min_d - *min; + *sec = (int)(delta * 10000.0d); +} diff --git a/src/drivers/hott/messages.h b/src/drivers/hott/messages.h new file mode 100644 index 0000000000..ecfad8569e --- /dev/null +++ b/src/drivers/hott/messages.h @@ -0,0 +1,268 @@ +/**************************************************************************** + * + * Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. + * Author: Simon Wilks + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * @file messages.h + * @author Simon Wilks + * + * Graupner HoTT Telemetry message generation. + * + */ +#ifndef MESSAGES_H_ +#define MESSAGES_H + +#include + +/* The HoTT receiver demands a minimum 5ms period of silence after delivering its request. + * Note that the value specified here is lower than 5000 (5ms) as time is lost constucting + * the message after the read which takes some milliseconds. + */ +#define POST_READ_DELAY_IN_USECS 4000 +/* A pause of 3ms is required between each uint8_t sent back to the HoTT receiver. Much lower + * values can be used in practise though. + */ +#define POST_WRITE_DELAY_IN_USECS 2000 + +// Protocol constants. +#define BINARY_MODE_REQUEST_ID 0x80 // Binary mode request. +#define START_BYTE 0x7c +#define STOP_BYTE 0x7d +#define TEMP_ZERO_CELSIUS 0x14 + +/* The GAM Module poll message. */ +struct gam_module_poll_msg { + uint8_t mode; + uint8_t id; +}; + +/* Electric Air Module (EAM) constants. */ +#define EAM_SENSOR_ID 0x8e +#define EAM_SENSOR_TEXT_ID 0xe0 + +/* The Electric Air Module message. */ +struct eam_module_msg { + uint8_t start; /**< Start byte */ + uint8_t eam_sensor_id; /**< EAM sensor */ + uint8_t warning; + uint8_t sensor_text_id; + uint8_t alarm_inverse1; + uint8_t alarm_inverse2; + uint8_t cell1_L; /**< Lipo cell voltages. Not supported. */ + uint8_t cell2_L; + uint8_t cell3_L; + uint8_t cell4_L; + uint8_t cell5_L; + uint8_t cell6_L; + uint8_t cell7_L; + uint8_t cell1_H; + uint8_t cell2_H; + uint8_t cell3_H; + uint8_t cell4_H; + uint8_t cell5_H; + uint8_t cell6_H; + uint8_t cell7_H; + uint8_t batt1_voltage_L; /**< Battery 1 voltage, lower 8-bits in steps of 0.02V */ + uint8_t batt1_voltage_H; + uint8_t batt2_voltage_L; /**< Battery 2 voltage, lower 8-bits in steps of 0.02V */ + uint8_t batt2_voltage_H; + uint8_t temperature1; /**< Temperature sensor 1. 20 = 0 degrees */ + uint8_t temperature2; + uint8_t altitude_L; /**< Attitude (meters) lower 8-bits. 500 = 0 meters */ + uint8_t altitude_H; + uint8_t current_L; /**< Current (mAh) lower 8-bits in steps of 0.1V */ + uint8_t current_H; + uint8_t main_voltage_L; /**< Main power voltage lower 8-bits in steps of 0.1V */ + uint8_t main_voltage_H; + uint8_t battery_capacity_L; /**< Used battery capacity in steps of 10mAh */ + uint8_t battery_capacity_H; + uint8_t climbrate_L; /**< Climb rate in 0.01m/s. 0m/s = 30000 */ + uint8_t climbrate_H; + uint8_t climbrate_3s; /**< Climb rate in m/3sec. 0m/3sec = 120 */ + uint8_t rpm_L; /**< RPM Lower 8-bits In steps of 10 U/min */ + uint8_t rpm_H; + uint8_t electric_min; /**< Flight time in minutes. */ + uint8_t electric_sec; /**< Flight time in seconds. */ + uint8_t speed_L; /**< Airspeed in km/h in steps of 1 km/h */ + uint8_t speed_H; + uint8_t stop; /**< Stop byte */ + uint8_t checksum; /**< Lower 8-bits of all bytes summed. */ +}; + +/** + * The maximum buffer size required to store an Electric Air Module message. + */ +#define EAM_MESSAGE_BUFFER_SIZE sizeof(union { \ + struct eam_module_msg eam; \ +}) + + +/* General Air Module (GAM) constants. */ +#define GAM_SENSOR_ID 0x8d +#define GAM_SENSOR_TEXT_ID 0xd0 + +struct gam_module_msg { + uint8_t start_byte; // start byte constant value 0x7c + uint8_t gam_sensor_id; // EAM sensort id. constat value 0x8d + uint8_t warning_beeps; // 1=A 2=B ... 0x1a=Z 0 = no alarm + uint8_t sensor_id; // constant value 0xd0 + uint8_t alarm_invers1; // alarm bitmask. Value is displayed inverted + uint8_t alarm_invers2; // alarm bitmask. Value is displayed inverted + uint8_t cell1; // cell 1 voltage lower value. 0.02V steps, 124=2.48V + uint8_t cell2; + uint8_t cell3; + uint8_t cell4; + uint8_t cell5; + uint8_t cell6; + uint8_t batt1_L; // battery 1 voltage LSB value. 0.1V steps. 50 = 5.5V + uint8_t batt1_H; + uint8_t batt2_L; // battery 2 voltage LSB value. 0.1V steps. 50 = 5.5V + uint8_t batt2_H; + uint8_t temperature1; // temperature 1. offset of 20. a value of 20 = 0°C + uint8_t temperature2; // temperature 2. offset of 20. a value of 20 = 0°C + uint8_t fuel_procent; // Fuel capacity in %. Values 0--100 + // graphical display ranges: 0-25% 50% 75% 100% + uint8_t fuel_ml_L; // Fuel in ml scale. Full = 65535! + uint8_t fuel_ml_H; // + uint8_t rpm_L; // RPM in 10 RPM steps. 300 = 3000rpm + uint8_t rpm_H; // + uint8_t altitude_L; // altitude in meters. offset of 500, 500 = 0m + uint8_t altitude_H; // + uint8_t climbrate_L; // climb rate in 0.01m/s. Value of 30000 = 0.00 m/s + uint8_t climbrate_H; // + uint8_t climbrate3s; // climb rate in m/3sec. Value of 120 = 0m/3sec + uint8_t current_L; // current in 0.1A steps + uint8_t current_H; // + uint8_t main_voltage_L; // Main power voltage using 0.1V steps + uint8_t main_voltage_H; // + uint8_t batt_cap_L; // used battery capacity in 10mAh steps + uint8_t batt_cap_H; // + uint8_t speed_L; // (air?) speed in km/h(?) we are using ground speed here per default + uint8_t speed_H; // + uint8_t min_cell_volt; // minimum cell voltage in 2mV steps. 124 = 2,48V + uint8_t min_cell_volt_num; // number of the cell with the lowest voltage + uint8_t rpm2_L; // RPM in 10 RPM steps. 300 = 3000rpm + uint8_t rpm2_H; // + uint8_t general_error_number; // Voice error == 12. TODO: more docu + uint8_t pressure; // Pressure up to 16bar. 0,1bar scale. 20 = 2bar + uint8_t version; // version number TODO: more info? + uint8_t stop; // stop byte + uint8_t checksum; // checksum +}; + +/** + * The maximum buffer size required to store a General Air Module message. + */ +#define GAM_MESSAGE_BUFFER_SIZE sizeof(union { \ + struct gam_module_msg gam; \ +}) + + +/* GPS sensor constants. */ +#define GPS_SENSOR_ID 0x8a +#define GPS_SENSOR_TEXT_ID 0xa0 + +/** + * The GPS sensor message + * Struct based on: https://code.google.com/p/diy-hott-gps/downloads + */ +struct gps_module_msg { + uint8_t start; /**< Start byte */ + uint8_t sensor_id; /**< GPS sensor ID*/ + uint8_t warning; /**< Byte 3: 0…= warning beeps */ + uint8_t sensor_text_id; /**< GPS Sensor text mode ID */ + uint8_t alarm_inverse1; /**< Byte 5: 01 inverse status */ + uint8_t alarm_inverse2; /**< Byte 6: 00 inverse status status 1 = no GPS Signal */ + uint8_t flight_direction; /**< Byte 7: 119 = Flightdir./dir. 1 = 2°; 0° (North), 9 0° (East), 180° (South), 270° (West) */ + uint8_t gps_speed_L; /**< Byte 8: 8 = /GPS speed low byte 8km/h */ + uint8_t gps_speed_H; /**< Byte 9: 0 = /GPS speed high byte */ + + uint8_t latitude_ns; /**< Byte 10: 000 = N = 48°39’988 */ + uint8_t latitude_min_L; /**< Byte 11: 231 0xE7 = 0x12E7 = 4839 */ + uint8_t latitude_min_H; /**< Byte 12: 018 18 = 0x12 */ + uint8_t latitude_sec_L; /**< Byte 13: 171 220 = 0xDC = 0x03DC =0988 */ + uint8_t latitude_sec_H; /**< Byte 14: 016 3 = 0x03 */ + + uint8_t longitude_ew; /**< Byte 15: 000 = E= 9° 25’9360 */ + uint8_t longitude_min_L; /**< Byte 16: 150 157 = 0x9D = 0x039D = 0925 */ + uint8_t longitude_min_H; /**< Byte 17: 003 3 = 0x03 */ + uint8_t longitude_sec_L; /**< Byte 18: 056 144 = 0x90 0x2490 = 9360*/ + uint8_t longitude_sec_H; /**< Byte 19: 004 36 = 0x24 */ + + uint8_t distance_L; /**< Byte 20: 027 123 = /distance low byte 6 = 6 m */ + uint8_t distance_H; /**< Byte 21: 036 35 = /distance high byte */ + uint8_t altitude_L; /**< Byte 22: 243 244 = /Altitude low byte 500 = 0m */ + uint8_t altitude_H; /**< Byte 23: 001 1 = /Altitude high byte */ + uint8_t resolution_L; /**< Byte 24: 48 = Low Byte m/s resolution 0.01m 48 = 30000 = 0.00m/s (1=0.01m/s) */ + uint8_t resolution_H; /**< Byte 25: 117 = High Byte m/s resolution 0.01m */ + uint8_t unknown1; /**< Byte 26: 120 = 0m/3s */ + uint8_t gps_num_sat; /**< Byte 27: GPS.Satellites (number of satelites) (1 byte) */ + uint8_t gps_fix_char; /**< Byte 28: GPS.FixChar. (GPS fix character. display, if DGPS, 2D oder 3D) (1 byte) */ + uint8_t home_direction; /**< Byte 29: HomeDirection (direction from starting point to Model position) (1 byte) */ + uint8_t angle_x_direction; /**< Byte 30: angle x-direction (1 byte) */ + uint8_t angle_y_direction; /**< Byte 31: angle y-direction (1 byte) */ + uint8_t angle_z_direction; /**< Byte 32: angle z-direction (1 byte) */ + uint8_t gyro_x_L; /**< Byte 33: gyro x low byte (2 bytes) */ + uint8_t gyro_x_H; /**< Byte 34: gyro x high byte */ + uint8_t gyro_y_L; /**< Byte 35: gyro y low byte (2 bytes) */ + uint8_t gyro_y_H; /**< Byte 36: gyro y high byte */ + uint8_t gyro_z_L; /**< Byte 37: gyro z low byte (2 bytes) */ + uint8_t gyro_z_H; /**< Byte 38: gyro z high byte */ + uint8_t vibration; /**< Byte 39: vibration (1 bytes) */ + uint8_t ascii4; /**< Byte 40: 00 ASCII Free Character [4] */ + uint8_t ascii5; /**< Byte 41: 00 ASCII Free Character [5] */ + uint8_t gps_fix; /**< Byte 42: 00 ASCII Free Character [6], we use it for GPS FIX */ + uint8_t version; /**< Byte 43: 00 version number */ + uint8_t stop; /**< Byte 44: 0x7D Ende byte */ + uint8_t checksum; /**< Byte 45: Parity Byte */ +}; + +/** + * The maximum buffer size required to store a HoTT message. + */ +#define GPS_MESSAGE_BUFFER_SIZE sizeof(union { \ + struct gps_module_msg gps; \ +}) + +#define MESSAGE_BUFFER_SIZE GPS_MESSAGE_BUFFER_SIZE + +void messages_init(void); +void build_gam_request(uint8_t *buffer, size_t *size); +void extract_gam_message(const uint8_t *buffer); +void build_eam_response(uint8_t *buffer, size_t *size); +void build_gps_response(uint8_t *buffer, size_t *size); +float _get_distance_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next); +void convert_to_degrees_minutes_seconds(double lat, int *deg, int *min, int *sec); + + +#endif /* MESSAGES_H_ */ From 3f9f2018e20f4be23e7d62571ec0a3678d960108 Mon Sep 17 00:00:00 2001 From: Jean Cyr Date: Fri, 5 Jul 2013 20:51:29 -0400 Subject: [PATCH 06/22] Support binding DSM2 and DSMX satellite receivers The px4io bind command allows you to put a DSM satellite receiver into bind mode. Since this feature requires that the dsm VCC line (red wire) be cut and routed through relay one, it is not enabled by default in order not to affect those not using a DSM satellite receiver or wising to use relay one for other purposes. NOTE: Binding DSM2 satellites in 11-bit mode is not supported due to potential bug in some DSM2 receiver streams when in 11-bit mode. Furthermore the px4io software folds 11 bit data down to 10 bits so there is no resolution advantage to to 11-bit mode. To enable the feature the RC_RL1_DSM_VCC parameter must be set to a non zero value from the console, or using QGroundControl: param set RC_RL1_DSM_VCC 1 From the console you can initiate DSM bind mode with: uorb start param set RC_RL1_DSM_VCC 1 px4io start px4io bind dsm2 For binding a DSMX satellite to a DSMX transmitter you would instead use: px4io bind dsmx Your receiver module should start a rapid flash and you can follow the normal binding sequence of your transmitter. Note: The value of parameter RC_RL1_DSM_VCC defaults to 0, so none of this will have any effect on an unmodified DSM receiver connection. For this feature to work, the power wire (red) must be cut and each side connected to a terminal on relay1 of the px4io board. This has been tested using Spektrum as well as Hobby King 'Orange' DSM satellite receivers. Both px4fmu and px4io images are updated. --- src/drivers/drv_pwm_output.h | 9 +++ src/drivers/px4io/px4io.cpp | 96 +++++++++++++++++++++++++-- src/modules/px4iofirmware/controls.c | 13 +++- src/modules/px4iofirmware/dsm.c | 43 +++++++++++- src/modules/px4iofirmware/protocol.h | 9 +++ src/modules/px4iofirmware/px4io.h | 1 + src/modules/px4iofirmware/registers.c | 4 ++ src/modules/sensors/sensor_params.c | 1 + src/modules/sensors/sensors.cpp | 12 ++++ 9 files changed, 178 insertions(+), 10 deletions(-) diff --git a/src/drivers/drv_pwm_output.h b/src/drivers/drv_pwm_output.h index 56af710592..52a6674031 100644 --- a/src/drivers/drv_pwm_output.h +++ b/src/drivers/drv_pwm_output.h @@ -115,6 +115,15 @@ ORB_DECLARE(output_pwm); /** clear the 'ARM ok' bit, which deactivates the safety switch */ #define PWM_SERVO_CLEAR_ARM_OK _IOC(_PWM_SERVO_BASE, 6) +/** start DSM bind */ +#define DSM_BIND_START _IOC(_PWM_SERVO_BASE, 7) + +/** stop DSM bind */ +#define DSM_BIND_STOP _IOC(_PWM_SERVO_BASE, 8) + +/** Power up DSM receiver */ +#define DSM_BIND_POWER_UP _IOC(_PWM_SERVO_BASE, 9) + /** set a single servo to a specific value */ #define PWM_SERVO_SET(_servo) _IOC(_PWM_SERVO_BASE, 0x20 + _servo) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 19163cebe3..54b9d50e47 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -89,6 +89,8 @@ #define PX4IO_SET_DEBUG _IOC(0xff00, 0) #define PX4IO_INAIR_RESTART_ENABLE _IOC(0xff00, 1) +static int dsm_vcc_ctl; + class PX4IO : public device::I2C { public: @@ -700,8 +702,6 @@ PX4IO::io_set_control_state() int PX4IO::set_failsafe_values(const uint16_t *vals, unsigned len) { - uint16_t regs[_max_actuators]; - if (len > _max_actuators) /* fail with error */ return E2BIG; @@ -1271,13 +1271,14 @@ PX4IO::print_status() printf("%u bytes free\n", io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FREEMEM)); uint16_t flags = io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FLAGS); - printf("status 0x%04x%s%s%s%s%s%s%s%s%s%s%s%s\n", + printf("status 0x%04x%s%s%s%s%s%s%s%s%s%s%s%s%s\n", flags, ((flags & PX4IO_P_STATUS_FLAGS_ARMED) ? " ARMED" : ""), ((flags & PX4IO_P_STATUS_FLAGS_OVERRIDE) ? " OVERRIDE" : ""), ((flags & PX4IO_P_STATUS_FLAGS_RC_OK) ? " RC_OK" : " RC_FAIL"), ((flags & PX4IO_P_STATUS_FLAGS_RC_PPM) ? " PPM" : ""), - ((flags & PX4IO_P_STATUS_FLAGS_RC_DSM) ? " DSM" : ""), + (((flags & PX4IO_P_STATUS_FLAGS_RC_DSM) && (!(flags & PX4IO_P_STATUS_FLAGS_RC_DSM11))) ? " DSM10" : ""), + (((flags & PX4IO_P_STATUS_FLAGS_RC_DSM) && (flags & PX4IO_P_STATUS_FLAGS_RC_DSM11)) ? " DSM11" : ""), ((flags & PX4IO_P_STATUS_FLAGS_RC_SBUS) ? " SBUS" : ""), ((flags & PX4IO_P_STATUS_FLAGS_FMU_OK) ? " FMU_OK" : " FMU_FAIL"), ((flags & PX4IO_P_STATUS_FLAGS_RAW_PWM) ? " RAW_PPM" : ""), @@ -1424,6 +1425,26 @@ PX4IO::ioctl(file *filep, int cmd, unsigned long arg) *(unsigned *)arg = _max_actuators; break; + case DSM_BIND_START: + io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_DSM, dsm_bind_power_down); + usleep(500000); + io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_DSM, dsm_bind_set_rx_out); + usleep(1000); + io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_DSM, dsm_bind_power_up); + usleep(100000); + io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_DSM, dsm_bind_send_pulses | (arg << 4)); + break; + + case DSM_BIND_STOP: + io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_DSM, dsm_bind_power_down); + io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_DSM, dsm_bind_reinit_uart); + usleep(500000); + break; + + case DSM_BIND_POWER_UP: + io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_DSM, dsm_bind_power_up); + break; + case PWM_SERVO_SET(0) ... PWM_SERVO_SET(PWM_OUTPUT_MAX_CHANNELS - 1): { /* TODO: we could go lower for e.g. TurboPWM */ @@ -1614,9 +1635,71 @@ start(int argc, char *argv[]) errx(1, "driver init failed"); } + if (param_get(param_find("RC_RL1_DSM_VCC"), &dsm_vcc_ctl) == OK) { + if (dsm_vcc_ctl) { + int fd = open("/dev/px4io", O_WRONLY); + if (fd < 0) + errx(1, "failed to open device"); + ioctl(fd, DSM_BIND_POWER_UP, 0); + close(fd); + } + } exit(0); } +void +bind(int argc, char *argv[]) +{ + int fd, pulses; + + if (g_dev == nullptr) + errx(1, "px4io must be started first"); + + if (dsm_vcc_ctl == 0) + errx(1, "DSM bind feature not enabled"); + + if (argc < 3) + errx(0, "needs argument, use dsm2 or dsmx"); + + if (!strcmp(argv[2], "dsm2")) + pulses = 3; + else if (!strcmp(argv[2], "dsmx")) + pulses = 7; + else + errx(1, "unknown parameter %s, use dsm2 or dsmx", argv[2]); + + fd = open("/dev/px4io", O_WRONLY); + + if (fd < 0) + errx(1, "failed to open device"); + + ioctl(fd, DSM_BIND_START, pulses); + + /* Open console directly to grab CTRL-C signal */ + int console = open("/dev/console", O_NONBLOCK | O_RDONLY | O_NOCTTY); + if (!console) + errx(1, "failed opening console"); + + warnx("This command will only bind DSM if satellite VCC (red wire) is controlled by relay 1."); + warnx("Press CTRL-C or 'c' when done."); + + for (;;) { + usleep(500000L); + /* Check if user wants to quit */ + char c; + if (read(console, &c, 1) == 1) { + if (c == 0x03 || c == 0x63) { + warnx("Done\n"); + ioctl(fd, DSM_BIND_STOP, 0); + ioctl(fd, DSM_BIND_POWER_UP, 0); + close(fd); + close(console); + exit(0); + } + } + } +} + void test(void) { @@ -1918,6 +2001,9 @@ px4io_main(int argc, char *argv[]) if (!strcmp(argv[1], "monitor")) monitor(); + if (!strcmp(argv[1], "bind")) + bind(argc, argv); + out: - errx(1, "need a command, try 'start', 'stop', 'status', 'test', 'monitor', 'debug', 'recovery', 'limit', 'current', 'failsafe' or 'update'"); + errx(1, "need a command, try 'start', 'stop', 'status', 'test', 'monitor', 'debug', 'recovery', 'limit', 'current', 'failsafe', 'bind', or 'update'"); } diff --git a/src/modules/px4iofirmware/controls.c b/src/modules/px4iofirmware/controls.c index 3cf9ca149b..9561c9b1e5 100644 --- a/src/modules/px4iofirmware/controls.c +++ b/src/modules/px4iofirmware/controls.c @@ -95,9 +95,16 @@ controls_tick() { */ perf_begin(c_gather_dsm); - bool dsm_updated = dsm_input(r_raw_rc_values, &r_raw_rc_count); - if (dsm_updated) + uint16_t temp_count = r_raw_rc_count; + bool dsm_updated = dsm_input(r_raw_rc_values, &temp_count); + if (dsm_updated) { r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_DSM; + r_raw_rc_count = temp_count & 0x7fff; + if (temp_count & 0x8000) + r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_DSM11; + else + r_status_flags &= ~PX4IO_P_STATUS_FLAGS_RC_DSM11; + } perf_end(c_gather_dsm); perf_begin(c_gather_sbus); @@ -138,7 +145,7 @@ controls_tick() { /* map raw inputs to mapped inputs */ /* XXX mapping should be atomic relative to protocol */ - for (unsigned i = 0; i < r_raw_rc_count; i++) { + for (unsigned i = 0; i < (r_raw_rc_count & 0x7fff); i++) { /* map the input channel */ uint16_t *conf = &r_page_rc_input_config[i * PX4IO_P_RC_CONFIG_STRIDE]; diff --git a/src/modules/px4iofirmware/dsm.c b/src/modules/px4iofirmware/dsm.c index ea35e55135..79e8925032 100644 --- a/src/modules/px4iofirmware/dsm.c +++ b/src/modules/px4iofirmware/dsm.c @@ -40,6 +40,7 @@ */ #include +#include #include #include @@ -101,6 +102,41 @@ dsm_init(const char *device) return dsm_fd; } +void +dsm_bind(uint16_t cmd, int pulses) +{ + const uint32_t usart1RxAsOutp = GPIO_OUTPUT|GPIO_CNF_OUTPP|GPIO_MODE_50MHz|GPIO_OUTPUT_SET|GPIO_PORTA|GPIO_PIN10; + + if (dsm_fd < 0) + return; + + switch (cmd) { + case dsm_bind_power_down: + // power down DSM satellite + POWER_RELAY1(0); + break; + case dsm_bind_power_up: + POWER_RELAY1(1); + dsm_guess_format(true); + break; + case dsm_bind_set_rx_out: + stm32_configgpio(usart1RxAsOutp); + break; + case dsm_bind_send_pulses: + for (int i = 0; i < pulses; i++) { + stm32_gpiowrite(usart1RxAsOutp, false); + up_udelay(50); + stm32_gpiowrite(usart1RxAsOutp, true); + up_udelay(50); + } + break; + case dsm_bind_reinit_uart: + // Restore USART rx pin + stm32_configgpio(GPIO_USART1_RX); + break; + } +} + bool dsm_input(uint16_t *values, uint16_t *num_values) { @@ -218,7 +254,7 @@ dsm_guess_format(bool reset) /* * Iterate the set of sensible sniffed channel sets and see whether - * decoding in 10 or 11-bit mode has yielded anything we recognise. + * decoding in 10 or 11-bit mode has yielded anything we recognize. * * XXX Note that due to what seem to be bugs in the DSM2 high-resolution * stream, we may want to sniff for longer in some cases when we think we @@ -303,7 +339,7 @@ dsm_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values) for (unsigned i = 0; i < DSM_FRAME_CHANNELS; i++) { uint8_t *dp = &frame[2 + (2 * i)]; - uint16_t raw = (dp[0] << 8) | dp[1]; + uint16_t raw = ((uint16_t)dp[0] << 8) | dp[1]; unsigned channel, value; if (!dsm_decode_channel(raw, channel_shift, &channel, &value)) @@ -349,6 +385,9 @@ dsm_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values) values[channel] = value; } + if (channel_shift == 11) + *num_values |= 0x8000; + /* * XXX Note that we may be in failsafe here; we need to work out how to detect that. */ diff --git a/src/modules/px4iofirmware/protocol.h b/src/modules/px4iofirmware/protocol.h index 674f9dddd6..6ee5c28342 100644 --- a/src/modules/px4iofirmware/protocol.h +++ b/src/modules/px4iofirmware/protocol.h @@ -105,6 +105,7 @@ #define PX4IO_P_STATUS_FLAGS_ARM_SYNC (1 << 9) /* the arming state between IO and FMU is in sync */ #define PX4IO_P_STATUS_FLAGS_INIT_OK (1 << 10) /* initialisation of the IO completed without error */ #define PX4IO_P_STATUS_FLAGS_FAILSAFE (1 << 11) /* failsafe is active */ +#define PX4IO_P_STATUS_FLAGS_RC_DSM11 (1 << 12) /* DSM input is 11 bit data */ #define PX4IO_P_STATUS_ALARMS 3 /* alarm flags - alarms latch, write 1 to a bit to clear it */ #define PX4IO_P_STATUS_ALARMS_VBATT_LOW (1 << 0) /* VBatt is very close to regulator dropout */ @@ -157,6 +158,14 @@ #define PX4IO_P_SETUP_PWM_ALTRATE 4 /* 'high' PWM frame output rate in Hz */ #define PX4IO_P_SETUP_RELAYS 5 /* bitmask of relay/switch outputs, 0 = off, 1 = on */ #define PX4IO_P_SETUP_VBATT_SCALE 6 /* battery voltage correction factor (float) */ +#define PX4IO_P_SETUP_DSM 7 /* DSM bind state */ +enum { /* DSM bind states */ + dsm_bind_power_down = 0, + dsm_bind_power_up, + dsm_bind_set_rx_out, + dsm_bind_send_pulses, + dsm_bind_reinit_uart +}; #define PX4IO_P_SETUP_SET_DEBUG 9 /* debug level for IO board */ /* autopilot control values, -10000..10000 */ diff --git a/src/modules/px4iofirmware/px4io.h b/src/modules/px4iofirmware/px4io.h index 272cdb7bf4..83feeb9b61 100644 --- a/src/modules/px4iofirmware/px4io.h +++ b/src/modules/px4iofirmware/px4io.h @@ -184,6 +184,7 @@ extern void controls_init(void); extern void controls_tick(void); extern int dsm_init(const char *device); extern bool dsm_input(uint16_t *values, uint16_t *num_values); +extern void dsm_bind(uint16_t cmd, int pulses); extern int sbus_init(const char *device); extern bool sbus_input(uint16_t *values, uint16_t *num_values); diff --git a/src/modules/px4iofirmware/registers.c b/src/modules/px4iofirmware/registers.c index df7d6dcd35..805eb7eccd 100644 --- a/src/modules/px4iofirmware/registers.c +++ b/src/modules/px4iofirmware/registers.c @@ -360,6 +360,10 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value) isr_debug(0, "set debug %u\n", (unsigned)r_page_setup[PX4IO_P_SETUP_SET_DEBUG]); break; + case PX4IO_P_SETUP_DSM: + dsm_bind(value & 0x0f, (value >> 4) & 7); + break; + default: return -1; } diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c index 2300601484..a11c135683 100644 --- a/src/modules/sensors/sensor_params.c +++ b/src/modules/sensors/sensor_params.c @@ -151,6 +151,7 @@ PARAM_DEFINE_FLOAT(RC14_REV, 1.0f); PARAM_DEFINE_FLOAT(RC14_DZ, 0.0f); PARAM_DEFINE_INT32(RC_TYPE, 1); /** 1 = FUTABA, 2 = Spektrum, 3 = Graupner HoTT, 4 = Turnigy 9x */ +PARAM_DEFINE_INT32(RC_RL1_DSM_VCC, 0); /* Relay 1 controls DSM VCC */ /* default is conversion factor for the PX4IO / PX4IOAR board, the factor for PX4FMU standalone is different */ PARAM_DEFINE_FLOAT(BAT_V_SCALING, (3.3f * 52.0f / 5.0f / 4095.0f)); diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 6b6aeedee5..626cbade49 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -229,6 +229,8 @@ private: float rc_scale_flaps; float battery_voltage_scaling; + + int rc_rl1_DSM_VCC_control; } _parameters; /**< local copies of interesting parameters */ struct { @@ -276,6 +278,8 @@ private: param_t rc_scale_flaps; param_t battery_voltage_scaling; + + param_t rc_rl1_DSM_VCC_control; } _parameter_handles; /**< handles for interesting parameters */ @@ -509,6 +513,9 @@ Sensors::Sensors() : _parameter_handles.battery_voltage_scaling = param_find("BAT_V_SCALING"); + /* DSM VCC relay control */ + _parameter_handles.rc_rl1_DSM_VCC_control = param_find("RC_RL1_DSM_VCC"); + /* fetch initial parameter values */ parameters_update(); } @@ -722,6 +729,11 @@ Sensors::parameters_update() warnx("Failed updating voltage scaling param"); } + /* relay 1 DSM VCC control */ + if (param_get(_parameter_handles.rc_rl1_DSM_VCC_control, &(_parameters.rc_rl1_DSM_VCC_control)) != OK) { + warnx("Failed updating relay 1 DSM VCC control"); + } + return OK; } From 86adaeb3e8f28c92005a38b7c71e12111efe8694 Mon Sep 17 00:00:00 2001 From: Simon Wilks Date: Sat, 6 Jul 2013 15:02:34 +0200 Subject: [PATCH 07/22] More cleanups --- src/drivers/hott/comms.c | 9 ++++--- src/drivers/hott/comms.h | 4 +-- src/drivers/hott/hott_sensors/hott_sensors.c | 26 ++++++++---------- .../hott/hott_telemetry/hott_telemetry.c | 7 ++--- src/drivers/hott/messages.c | 27 +++++++++++++++++-- src/drivers/hott/messages.h | 5 ++-- 6 files changed, 47 insertions(+), 31 deletions(-) diff --git a/src/drivers/hott/comms.c b/src/drivers/hott/comms.c index 4a7d3c8451..a2de87407d 100644 --- a/src/drivers/hott/comms.c +++ b/src/drivers/hott/comms.c @@ -41,17 +41,17 @@ #include #include -#include #include #include #include #include -#include #include #include +#include +#include int -open_uart(const char *device, struct termios *uart_config_original) +open_uart(const char *device) { /* baud rate */ static const speed_t speed = B19200; @@ -65,7 +65,8 @@ open_uart(const char *device, struct termios *uart_config_original) /* Back up the original uart configuration to restore it after exit */ int termios_state; - if ((termios_state = tcgetattr(uart, uart_config_original)) < 0) { + struct termios uart_config_original; + if ((termios_state = tcgetattr(uart, &uart_config_original)) < 0) { close(uart); err(1, "Error getting baudrate / termios config for %s: %d", device, termios_state); } diff --git a/src/drivers/hott/comms.h b/src/drivers/hott/comms.h index a1173631dd..4954a309e2 100644 --- a/src/drivers/hott/comms.h +++ b/src/drivers/hott/comms.h @@ -41,9 +41,7 @@ #ifndef COMMS_H_ #define COMMS_H -#include - -int open_uart(const char *device, struct termios *uart_config_original); +int open_uart(const char *device); #endif /* COMMS_H_ */ diff --git a/src/drivers/hott/hott_sensors/hott_sensors.c b/src/drivers/hott/hott_sensors/hott_sensors.c index dc10685b47..41ca0c92f8 100644 --- a/src/drivers/hott/hott_sensors/hott_sensors.c +++ b/src/drivers/hott/hott_sensors/hott_sensors.c @@ -47,7 +47,6 @@ #include #include #include -#include #include #include #include @@ -60,7 +59,7 @@ static int thread_should_exit = false; /**< Deamon exit flag */ static int thread_running = false; /**< Deamon status flag */ static int deamon_task; /**< Handle of deamon task / thread */ static const char daemon_name[] = "hott_sensors"; -static const char commandline_usage[] = "usage: hott_sensor start|status|stop [-d ]"; +static const char commandline_usage[] = "usage: hott_sensors start|status|stop [-d ]"; /** * Deamon management function. @@ -96,8 +95,6 @@ send_poll(int uart, uint8_t *buffer, size_t size) int recv_data(int uart, uint8_t *buffer, size_t *size, uint8_t *id) { - usleep(5000); - static const int timeout_ms = 1000; struct pollfd fds[] = { { .fd = uart, .events = POLLIN } }; @@ -108,7 +105,6 @@ recv_data(int uart, uint8_t *buffer, size_t *size, uint8_t *id) bool stop_byte_read = false; while (true) { read(uart, &buffer[i], sizeof(buffer[i])); - //printf("[%d]: %d\n", i, buffer[i]); if (stop_byte_read) { // XXX process checksum @@ -149,37 +145,37 @@ hott_sensors_thread_main(int argc, char *argv[]) } /* enable UART, writes potentially an empty buffer, but multiplexing is disabled */ - struct termios uart_config_original; - const int uart = open_uart(device, &uart_config_original); - + const int uart = open_uart(device); if (uart < 0) { errx(1, "Failed opening HoTT UART, exiting."); thread_running = false; } + pub_messages_init(); + uint8_t buffer[MESSAGE_BUFFER_SIZE]; size_t size = 0; uint8_t id = 0; - bool connected = true; while (!thread_should_exit) { // Currently we only support a General Air Module sensor. build_gam_request(&buffer, &size); send_poll(uart, buffer, size); + + // The sensor will need a little time before it starts sending. + usleep(5000); + recv_data(uart, &buffer, &size, &id); // Determine which moduel sent it and process accordingly. if (id == GAM_SENSOR_ID) { - //warnx("extract"); - extract_gam_message(buffer); + publish_gam_message(buffer); } else { - warnx("Unknown sensor ID received: %d", id); - } + warnx("Unknown sensor ID: %d", id); + } } warnx("exiting"); - close(uart); - thread_running = false; return 0; diff --git a/src/drivers/hott/hott_telemetry/hott_telemetry.c b/src/drivers/hott/hott_telemetry/hott_telemetry.c index fc80ac4d23..c87810b42f 100644 --- a/src/drivers/hott/hott_telemetry/hott_telemetry.c +++ b/src/drivers/hott/hott_telemetry/hott_telemetry.c @@ -49,7 +49,6 @@ #include #include #include -#include #include #include #include @@ -189,15 +188,13 @@ hott_telemetry_thread_main(int argc, char *argv[]) } /* enable UART, writes potentially an empty buffer, but multiplexing is disabled */ - struct termios uart_config_original; - const int uart = open_uart(device, &uart_config_original); - + const int uart = open_uart(device); if (uart < 0) { errx(1, "Failed opening HoTT UART, exiting."); thread_running = false; } - messages_init(); + sub_messages_init(); uint8_t buffer[MESSAGE_BUFFER_SIZE]; size_t size = 0; diff --git a/src/drivers/hott/messages.c b/src/drivers/hott/messages.c index 1a29b7e73d..ba2f6212da 100644 --- a/src/drivers/hott/messages.c +++ b/src/drivers/hott/messages.c @@ -58,19 +58,31 @@ static int gps_sub = -1; static int home_sub = -1; static int sensor_sub = -1; static int airspeed_sub = -1; +static int esc_sub = -1; + +//orb_advert_t _esc_pub; +//struct esc_s _esc; + static bool home_position_set = false; static double home_lat = 0.0d; static double home_lon = 0.0d; void -messages_init(void) +sub_messages_init(void) { battery_sub = orb_subscribe(ORB_ID(battery_status)); gps_sub = orb_subscribe(ORB_ID(vehicle_gps_position)); home_sub = orb_subscribe(ORB_ID(home_position)); sensor_sub = orb_subscribe(ORB_ID(sensor_combined)); airspeed_sub = orb_subscribe(ORB_ID(airspeed)); + //esc_sub = orb_subscribe(ORB_ID(esc)); +} + +void +pub_messages_init(void) +{ + //esc_pub = orb_subscribe(ORB_ID(esc)); } void @@ -87,17 +99,28 @@ build_gam_request(uint8_t *buffer, size_t *size) } void -extract_gam_message(const uint8_t *buffer) +publish_gam_message(const uint8_t *buffer) { struct gam_module_msg msg; size_t size = sizeof(msg); memset(&msg, 0, size); memcpy(&msg, buffer, size); + /* announce the esc if needed, just publish else */ +// if (esc_pub > 0) { +// orb_publish(ORB_ID(airspeed), _esc_pub, &_esc); +// +// } else { +// _esc_pub = orb_advertise(ORB_ID(esc), &_esc); +// } + // Publish it. uint16_t rpm = ((msg.rpm_H << 8) | (msg.rpm_L & 0xff)) * 10; + //_esc.rpm = rpm; uint8_t temp = msg.temperature2 + 20; + //_esc.temperature = temp; float current = ((msg.current_H << 8) | (msg.current_L & 0xff)) * 0.1f; + //_esc.current = current; printf("RPM: %d TEMP: %d A: %2.1f\n", rpm, temp, current); } diff --git a/src/drivers/hott/messages.h b/src/drivers/hott/messages.h index ecfad8569e..dce90f273c 100644 --- a/src/drivers/hott/messages.h +++ b/src/drivers/hott/messages.h @@ -256,9 +256,10 @@ struct gps_module_msg { #define MESSAGE_BUFFER_SIZE GPS_MESSAGE_BUFFER_SIZE -void messages_init(void); +void sub_messages_init(void); +void pub_messages_init(void); void build_gam_request(uint8_t *buffer, size_t *size); -void extract_gam_message(const uint8_t *buffer); +void publish_gam_message(const uint8_t *buffer); void build_eam_response(uint8_t *buffer, size_t *size); void build_gps_response(uint8_t *buffer, size_t *size); float _get_distance_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next); From dab652faf68931a2b1fa07609d63518237c9c8b7 Mon Sep 17 00:00:00 2001 From: Jean Cyr Date: Sun, 7 Jul 2013 19:04:30 -0400 Subject: [PATCH 08/22] Prevent RELAY1 control via IOCTL if DSM bind feature is enabled --- src/drivers/px4io/px4io.cpp | 59 +++++++++++++++++++++------- src/modules/px4iofirmware/controls.c | 2 +- src/modules/px4iofirmware/dsm.c | 2 +- 3 files changed, 47 insertions(+), 16 deletions(-) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 54b9d50e47..ad0112b9bf 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -89,8 +89,6 @@ #define PX4IO_SET_DEBUG _IOC(0xff00, 0) #define PX4IO_INAIR_RESTART_ENABLE _IOC(0xff00, 1) -static int dsm_vcc_ctl; - class PX4IO : public device::I2C { public: @@ -131,6 +129,16 @@ public: */ void print_status(); + inline void set_dsm_vcc_ctl(bool enable) + { + _dsm_vcc_ctl = enable; + }; + + inline bool get_dsm_vcc_ctl() + { + return _dsm_vcc_ctl; + }; + private: // XXX unsigned _max_actuators; @@ -174,6 +182,12 @@ private: float _battery_mamphour_total; uint64_t _battery_last_timestamp; + /** + * Relay1 is dedicated to controlling DSM receiver power + */ + + bool _dsm_vcc_ctl; + /** * Trampoline to the worker task */ @@ -315,7 +329,7 @@ PX4IO *g_dev; } PX4IO::PX4IO() : - I2C("px4io", "/dev/px4io", PX4_I2C_BUS_ONBOARD, PX4_I2C_OBDEV_PX4IO, 320000), + I2C("px4io", GPIO_DEVICE_PATH, PX4_I2C_BUS_ONBOARD, PX4_I2C_OBDEV_PX4IO, 320000), _max_actuators(0), _max_controls(0), _max_rc_input(0), @@ -340,7 +354,8 @@ PX4IO::PX4IO() : _battery_amp_per_volt(90.0f/5.0f), // this matches the 3DR current sensor _battery_amp_bias(0), _battery_mamphour_total(0), - _battery_last_timestamp(0) + _battery_last_timestamp(0), + _dsm_vcc_ctl(false) { /* we need this potentially before it could be set in task_main */ g_dev = this; @@ -1487,18 +1502,31 @@ PX4IO::ioctl(file *filep, int cmd, unsigned long arg) break; } - case GPIO_RESET: - ret = io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_RELAYS, 0); + case GPIO_RESET: { + uint32_t bits = (1 << _max_relays) - 1; + /* don't touch relay1 if it's controlling RX vcc */ + if (_dsm_vcc_ctl) + bits &= ~1; + ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_RELAYS, bits, 0); break; + } case GPIO_SET: arg &= ((1 << _max_relays) - 1); - ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_RELAYS, 0, arg); + /* don't touch relay1 if it's controlling RX vcc */ + if (_dsm_vcc_ctl & (arg & 1)) + ret = -EINVAL; + else + ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_RELAYS, 0, arg); break; case GPIO_CLEAR: arg &= ((1 << _max_relays) - 1); - ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_RELAYS, arg, 0); + /* don't touch relay1 if it's controlling RX vcc */ + if (_dsm_vcc_ctl & (arg & 1)) + ret = -EINVAL; + else + ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_RELAYS, arg, 0); break; case GPIO_GET: @@ -1635,9 +1663,12 @@ start(int argc, char *argv[]) errx(1, "driver init failed"); } + int dsm_vcc_ctl; + if (param_get(param_find("RC_RL1_DSM_VCC"), &dsm_vcc_ctl) == OK) { if (dsm_vcc_ctl) { - int fd = open("/dev/px4io", O_WRONLY); + g_dev->set_dsm_vcc_ctl(true); + int fd = open(GPIO_DEVICE_PATH, O_WRONLY); if (fd < 0) errx(1, "failed to open device"); ioctl(fd, DSM_BIND_POWER_UP, 0); @@ -1655,7 +1686,7 @@ bind(int argc, char *argv[]) if (g_dev == nullptr) errx(1, "px4io must be started first"); - if (dsm_vcc_ctl == 0) + if (!g_dev->get_dsm_vcc_ctl()) errx(1, "DSM bind feature not enabled"); if (argc < 3) @@ -1668,7 +1699,7 @@ bind(int argc, char *argv[]) else errx(1, "unknown parameter %s, use dsm2 or dsmx", argv[2]); - fd = open("/dev/px4io", O_WRONLY); + fd = open(GPIO_DEVICE_PATH, O_WRONLY); if (fd < 0) errx(1, "failed to open device"); @@ -1694,8 +1725,8 @@ bind(int argc, char *argv[]) ioctl(fd, DSM_BIND_POWER_UP, 0); close(fd); close(console); - exit(0); - } + exit(0); + } } } } @@ -1709,7 +1740,7 @@ test(void) int direction = 1; int ret; - fd = open("/dev/px4io", O_WRONLY); + fd = open(GPIO_DEVICE_PATH, O_WRONLY); if (fd < 0) err(1, "failed to open device"); diff --git a/src/modules/px4iofirmware/controls.c b/src/modules/px4iofirmware/controls.c index 9561c9b1e5..43d96fb067 100644 --- a/src/modules/px4iofirmware/controls.c +++ b/src/modules/px4iofirmware/controls.c @@ -145,7 +145,7 @@ controls_tick() { /* map raw inputs to mapped inputs */ /* XXX mapping should be atomic relative to protocol */ - for (unsigned i = 0; i < (r_raw_rc_count & 0x7fff); i++) { + for (unsigned i = 0; i < r_raw_rc_count; i++) { /* map the input channel */ uint16_t *conf = &r_page_rc_input_config[i * PX4IO_P_RC_CONFIG_STRIDE]; diff --git a/src/modules/px4iofirmware/dsm.c b/src/modules/px4iofirmware/dsm.c index 79e8925032..ab6e3fec43 100644 --- a/src/modules/px4iofirmware/dsm.c +++ b/src/modules/px4iofirmware/dsm.c @@ -339,7 +339,7 @@ dsm_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values) for (unsigned i = 0; i < DSM_FRAME_CHANNELS; i++) { uint8_t *dp = &frame[2 + (2 * i)]; - uint16_t raw = ((uint16_t)dp[0] << 8) | dp[1]; + uint16_t raw = (dp[0] << 8) | dp[1]; unsigned channel, value; if (!dsm_decode_channel(raw, channel_shift, &channel, &value)) From 20103f572fcf1451b6625209f02b7fde70dd3f04 Mon Sep 17 00:00:00 2001 From: Jean Cyr Date: Sun, 7 Jul 2013 20:19:27 -0400 Subject: [PATCH 09/22] Minor px4io optimization Since this module creates the PX4IO object and that the IOCTL function doesn't use the file descriptor parameter, there is no need to invoke IOCTL via the filesystem since we can call it directly. --- src/drivers/px4io/px4io.cpp | 27 +++++++++------------------ 1 file changed, 9 insertions(+), 18 deletions(-) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index ad0112b9bf..1adefdea5b 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -1388,7 +1388,8 @@ PX4IO::print_status() } int -PX4IO::ioctl(file *filep, int cmd, unsigned long arg) +PX4IO::ioctl(file * /*filep*/, int cmd, unsigned long arg) +/* Make it obvious that file * isn't used here */ { int ret = OK; @@ -1668,11 +1669,7 @@ start(int argc, char *argv[]) if (param_get(param_find("RC_RL1_DSM_VCC"), &dsm_vcc_ctl) == OK) { if (dsm_vcc_ctl) { g_dev->set_dsm_vcc_ctl(true); - int fd = open(GPIO_DEVICE_PATH, O_WRONLY); - if (fd < 0) - errx(1, "failed to open device"); - ioctl(fd, DSM_BIND_POWER_UP, 0); - close(fd); + g_dev->ioctl(nullptr, DSM_BIND_POWER_UP, 0); } } exit(0); @@ -1681,7 +1678,7 @@ start(int argc, char *argv[]) void bind(int argc, char *argv[]) { - int fd, pulses; + int pulses; if (g_dev == nullptr) errx(1, "px4io must be started first"); @@ -1699,12 +1696,7 @@ bind(int argc, char *argv[]) else errx(1, "unknown parameter %s, use dsm2 or dsmx", argv[2]); - fd = open(GPIO_DEVICE_PATH, O_WRONLY); - - if (fd < 0) - errx(1, "failed to open device"); - - ioctl(fd, DSM_BIND_START, pulses); + g_dev->ioctl(nullptr, DSM_BIND_START, pulses); /* Open console directly to grab CTRL-C signal */ int console = open("/dev/console", O_NONBLOCK | O_RDONLY | O_NOCTTY); @@ -1721,9 +1713,8 @@ bind(int argc, char *argv[]) if (read(console, &c, 1) == 1) { if (c == 0x03 || c == 0x63) { warnx("Done\n"); - ioctl(fd, DSM_BIND_STOP, 0); - ioctl(fd, DSM_BIND_POWER_UP, 0); - close(fd); + g_dev->ioctl(nullptr, DSM_BIND_STOP, 0); + g_dev->ioctl(nullptr, DSM_BIND_POWER_UP, 0); close(console); exit(0); } @@ -1914,7 +1905,7 @@ px4io_main(int argc, char *argv[]) * We can cheat and call the driver directly, as it * doesn't reference filp in ioctl() */ - g_dev->ioctl(NULL, PX4IO_INAIR_RESTART_ENABLE, 1); + g_dev->ioctl(nullptr, PX4IO_INAIR_RESTART_ENABLE, 1); } else { errx(1, "not loaded"); } @@ -1958,7 +1949,7 @@ px4io_main(int argc, char *argv[]) /* we can cheat and call the driver directly, as it * doesn't reference filp in ioctl() */ - int ret = g_dev->ioctl(NULL, PX4IO_SET_DEBUG, level); + int ret = g_dev->ioctl(nullptr, PX4IO_SET_DEBUG, level); if (ret != 0) { printf("SET_DEBUG failed - %d\n", ret); exit(1); From a7d5248f05f22e0e546df092ef507a4100151238 Mon Sep 17 00:00:00 2001 From: Jean Cyr Date: Mon, 8 Jul 2013 00:07:00 -0400 Subject: [PATCH 10/22] Feature documentation --- Documentation/dsm_bind.odt | Bin 0 -> 26760 bytes Documentation/dsm_bind.pdf | Bin 0 -> 30593 bytes 2 files changed, 0 insertions(+), 0 deletions(-) create mode 100644 Documentation/dsm_bind.odt create mode 100644 Documentation/dsm_bind.pdf diff --git a/Documentation/dsm_bind.odt b/Documentation/dsm_bind.odt new file mode 100644 index 0000000000000000000000000000000000000000..793980d4cedd8fa4b244f68ac98f3055b3e54b18 GIT binary patch literal 26760 zcmeFa1z1&0*D$;fhwc&ykrJf45$W!fk~$o^8;JFozvrAid(B#F*7WR|H8Xo_t3lCmfiuGZn0N8xUj`k>3md## z99-bu0q$@scXww8n3cDKn=6m6s|~lCm6wATx0^fM)y56x;{tc}=JtmBduxNj$N@~0 zR|cSlkXrT&D5MPy0M3DT4F!D}E)`8h9v3TD2V1z8H@Clwb4k36Ydbx$!d~JHtd95Z zHrUA}8q%{QTd}*iN$`83bM|*TlhR*CDVKhl1uVWG9)I+&@1%b&N|E_25o?D9BVJ=* z_+9VYIIr*A3Rd)33?$W_U5r|@>wSD^2N&_uN{JKY7lWJ^1>;q;Z}Fj%L|HlMDtf-w zB)R{_t;x8t0jqislEzMBS1y(kg!}Y$t3~O^mIv)%fW7~K5Z$K>jTp~AuW*P}VQNbS zo?D2?J8ekR=n%SB6p)$1E#Gtnao5D3&}cWD(Q9_*{lq@a& zA)6|rcvZ2FdA>*Cw}KkS;&SiwWPIw97=7S|@oDkkbn|shDMh3TPpn~`(cMTA6>Te7 ztY`@r8c+P#07B}N1w%;GodeA}y!$8C0Uek|MN%i^SQo7w!qK0UH#rGuYLA5|ucY(% z<(a)^n)A>WsJ8E6=!(^uqP=Up|Dr%w8$QB%?%k-h!fRtzD&dM+3Trzbv@d;ijol}o zw#Sm=v3Pyfoj$rmL`^-Od|67=S$wfP0Pl!CuP$Zgd8AxT2}DTy)ht#yPt z1fA@$6X_DsgdwI_(dS$8_4Yj*+8L(OHd|qHL*z}eF}@ZxLz?gSRExd`z~7#vzFVgI zwuI6c7^607h zlrgCNNI-OJ=GL+H;{_cY%b|PaSTPPnJ!F~c64ly5RHe4z5{oHIlA_^v9z(nnUaZ?w zsPgoruQjcS?V4~XXlUMp=6F-OBvVc6vax#eHh916!BsWPbjuhjOaAJRTH!#3-Wk+V zaGUw{)bi*()uE2f4CcbgdZnZ;;qE7&aFy9oAS~YaO!4 zYKqbK49)M4(nyucKNx11tGLy@bv&MWBM2Nx?lI3Ks8yB%Tz@sR*e!Qa6a>=`uxc8~=dz=DthP!NKEKLA1vV4z?C zu!hk5f*m2OKX|}?6~YIAHgFNVJR$rjm>h&3L^uFo4Ma)-a>45zfO{MQcI(7H?=m)S z)^K`ldpB=4FMBt4dO1BIZa(^J4q$)o=LPWd@(GCZihzH5egSbI5pe-w0Dxvee#b2f z?WcT*L>BZX415H`LK;j6^an54v;9s7JH}IX8<3S0t-0}0D{;A2vIJ;CIrccm2-0508M5fS8bwn4FN1kQ`|dlAn1Y`R@oov;rj9fG3azg)jnW zBoHVG1ThOH5*N_{kpg3h0YMos!bk%JAZX|qm{{02xOiaU|CI;<(4aqw!~g^x0!2ed z!@$DC#(?4rfJ71~Is+*mhK!CC8KcK-eoXR*Ps?SQE}heDvlbBayfY|=#T>8q_2V~~ zmk>pE#a$NpNBTAjuX4PF&kq^Q!?!9!+lPG?zAIi%$PIg$*fH{HJFlv9baBVf);ByU zzq)H|X;(z)nw?)nazRb^`0^eJfQAOf79ABQEKCd`R1_HaNYTLW_7`F9i$Qvf~` z6($KF1?=d@t1z=MzlkfQ)x55h77Tk~E{K>CXkU%}FPFCuL*ZC)a338?2bwz<{(6@4(dRA69EjG+kXUq&Im7#UX`w zrM{W=o~`jz_2M|JOlg%~o8)wB>UWWU$=_W&xfZ9}mvPfi^6hqI(D<4$0`NKEyzwZg z_)~dx(vDgFG~6n?tM*>ya1Gr!*G%BGpvTpx$-%2^&)+TwE0>)_6z;BRA%N;41kl60 zd$9lkB>i;e=6@EduXidP5EhFe;JH$Jn_rB$24?xHuk!f(!1w_LNFTiM(Rlo|>|PncEFt|{|2 zk>(l5HhZ^JRV_5&rPf5hWcX^G?oi9Fyy)<+d9Q}{r8q-oGWoiaK?8O196auM-Jzxr zO>jmFbE-f1l`#8UN30uKS zy(oPmlBz0qZL~4fDYE|gwW6-igLjW%p5P%v>{=icuonji6U7Krx zhK(c^`6f<2TnIqL_VfFCRd28=B_IIA;EqA&I0E>TcFKtWiml%np9G((w9LOftiDtr z0~Wrf-lbx zp_?K%<}xi-=8og`tE^9-YSNkYP+SX4`{vQJZD0L?vHxwZ$9kpR(D7pCXhzt`b9n0x zrzw?b5rs~~O=n2w^WE#a!MfE)!RlSXk|&`FZ#VR}u9k%ORlg?e!t#=FQa%ycY<_#> zJGWiUMKl#?iU8O)5kO5j0@$v9yE%M@ z2tddFbgmNt)E=}PVLe0uxw^V@FAr-@^=1)(8w>%gdbc?2xE&WXoWAcy07I)Fb{+xD z+?qSN+XK4Ug8o0_ne$2BsHSArc2x7-mUh@a-M)`sZtJR%yUmSrPAB0-0g^X~7tF4{ zO*@`$`Fv4nGjso(SMI%4BiO6RmNn}5G{qN3XxJAbUGbz;b< z>Xr-kEr_B2s$Q5@@_xJVUio!%J>EBu-KdLW>c)>H{2m_lc7Kiqv(^hNk`B(ZQzzE$ z?%sB%yOgGZ2BK;c<3{T)b?+;K`oB~KMV+_XC$boQKAQ8czFAAh=kse9gXW40RC?`w zav8S0C0=G=Wjl>xZ5XxfMF2+IEv?0w72?-UUV-(h`Y{4{sL#>a*>(6fX)9*1G&2ER74 z*f%db4A0GOP02|jtn$%vHxeBwO`^uXm_(6bk zQI3m`e(&1NlA#(4+dau0*a^?e6N1c?GSu_y#{-QM?B(aPGmn|vPJ^C0mRupsNKEZF ze}0@*%eweY9S~Wocdhg>ilFQ3rDW6R~ z$Xryg(;MQq&#!k{du&~wO4(LJzA{+VvU}ftY<_KOp)cg}Qf_=YnA3Hc2TsYSt~{sT z*cP_O4H9OGE57trREyVlW@k}fG+HNG8-ZTN1MrV&rVB0CuLT;3HcX ze0-w_oCe$xVBOnB0LL6!r#Gz|%TIgXXI!U@T_@_c!7VSY-1w060<$W5hi%riYePlz zRRP$XqQ4yTxShx)p105gxYys|5oDc~ZbMCBC14G2(d?aTIVGW*6+iCR9UkSbp<#HS zMk6m?6W4#!{&-hnHMc-9bIJHb=YV`z=xHgz1qk$02I>iqjF%o_^xwym%`(uQ{4k~K z^pTtXv+)Og*QL1woxPlP*x@eWb(5uTlc#A?N^5$@qvKrL@p?Uj7Mi}Md>KOm9wj~H ziNYWDs>0fuOT~D6s>`cQW8#J#D@Uh&5@$_05CG1#;z95D!Ekms7FEiZif}`}6Wo$O zA=COL!z0NX6Maou!QV6AS{$#{A_qEujC>GdJNn7Jg_jD}a{rHRh)Kj}a>n1&5dm&q zKvY&m1DzQ)I!4Z){$fxxaTKZsw2^e=^Z)?x+&vv!y|sM2-F>`4Cvpx1XnT3QA$o9Tq7HBY{NPUn;05G?Yaq@GMRNdcFVLk8@;LympdX|U;4FO-$N(S} zp!GWqq<^GA%N?n6^8)FlNKQF-Z`YsE0;6s1`OBee=cVz>A?N8T_uJv>{oA2#?d-?{LvYs~j2Cm+UOuEj<()wj&oo(p<9bVVV*&Bt|_IH-n{l$*!3x|2T zdCFUPTOm_`ilnxkw%5;cM#4yD@Q#c&pbob^6Z5xV^}&hozYA6m=6r_N_Jm0rBPE=b z9`rK`B>piyh5WpuY9|YdMu@UemQ=`(GABSekyXg^Yn4*FAP5lZvxssd4y0HuHP7x%ze-e z0PaYI<-aq~6(srJQ9uRf{{an^7uZPIKidMDG^j`euOIn_RC4@i(kNbPq!X$FT4(7& zIsv2#0)lK+xPSqwu{nSZ&KYcSHemDQ1MxqqFp~9``2&aY3(EMb`GesX;dg@tTz#C8 zJq1z|gK6#N<7(sexAp?_Mh1=QjgXbh+0E7NryOKu{L?M#?dI~sjf#e@o&66wDr%Ts zpdudbW$3JqtkRIbgoA;ifoNo3frWCY$p0Z5%g)oy$NeuTOgB#lI|tVvEd^ge4=G&> zg#%>}SowInDZyRgo>ty)8xR5LgNp$_m~qdTkr*UJ#l?;uy#3!!Kh($5`9~-Gqfn5U z|5uELm)*~f`iuwD%Gq1r%I+^*gfKYh4_vfR@lw{;Q2*I+VE;t_g&D`*&GUwgvxD8w zoFY0)yz);pm~t!|xUH2Bsw2bkg?oDc8+OB==)bb#S=-6EIlFoOk+~#i@@18N5kN11 zrkg7=uW`KH+`&g2xLWW>;o>@j73p8F_||UTV1@b@JRVrY_J3zbwJKI*K&aISG^wvB z`0PoAR4blUVM-Jqa=ik;`vzLbJkA0yPZ0b70HduSeiWbt6>i$vG{iCS+M^OjP zxY+(t)cvEV`+rl*_m86Pr+)7rMcqG&x_=aP|0wGICu%#y&XsFhr)Gfw}+OoAU2P{34@D9_h(lT+S121?mCyK^@@Fg8Hpe{>u)R zKltIG+7K?uh}`nR1(q1Ei2lzlFI>EW;=H_PtQuf7umSap|2<|m8wcBf|B}@PCT{EI z>0$+D5^7rzkE1)>4i%h+hPaBWm$#KG46Y(C$q1&6+rhy`T!>#mURFd@Sd?EuM1h|V ztRq6QqJkm{qB64lyfX59KmE%8*pvCwug(94-!t8hmGl2h#;;NlN9ipz+-$(C6=0&)}!&g>!aCJii7bji7HGJd%%gES-9%}$x zoWudwNx@}EE0~wNhMppF8v_6izUYyoFeLoLfYZ6%v9nduVJ>BDditM)|K^6=?ftJmk}-seP;cq#6S4{GZ0y}XY}tns4|04 z^#8{F_W2v&qXLRR=hX z8=0ZM%zw%7i}ODQ{s@m78Q!ni0loa$0xLbTR>QyrQy))yFL1sOr|0^6CH^0K{3BX_ z#Dh~8ZVUH>gVQm3L$H>CN3_7=cC~RpPC@7$T>mnM`9s40(AyspklOBa9bd>!U3oQ7VQB! zU443(kEib$3p(}X>@Nfwez>3=3BmzhP(trY>3TOfPp!UZcfPo4g7jWg* z54Z)~2Eu_T-~o^bqym}Xv6DjJIZy%A0F6K!&;|5?hfqd=Y2X8}2&@BNfnDGTJOF_O zA%KuUs33F@W(Ws_A0i653Q>TlLv$g=5KD+1#1-NLxdpikiGsvKQXx+vg^+Sc9i$EN z8Zro(gnWdoLbf1>XizkKG;%arG!`@-G*L7eG&MARwCiYgXdY-c&_dB-(NfT|(Mr&& z(b~}Z&_>Zdpsl0rfag>2pcGIBC>K-|DhJhqnm}!#p3op@Bs2+{4Sf!+hjv4Up&y_d z&;xW#bTV{$bS`uWbY*k{bQrn^dJuXvdMf%e^lJ1D^kMXm=wH!KF$gd&U~piFW2j&l zVc22#VT5BOVdP^}VRT}QVk~0pVq#)aU|z-)!BoaH#&pEIi5ZQVfmw#xiaCh6fVqQ( zg++zMfhCEhg=K~1jTMgd7^?)U1#1Xv5o;eC51S5~A6pUI1ltw+4t5fD5q2~75cV?m z5e_j9Gmbcp7LE;008SiEE>1npTbxClBU}>P%ea!b`nZm`w{cT&pW}Ao&ftE>!^305 z6T{QNbHEG1OTl}A*Ms*F?*N|^pB-Ng-wfXeKNi0LzZHKH|2qKz0SkdNfeC>(K`cQb zK|8^Ff_*|VLT*A8LL0(h!ZgBa!Xd&M3qEui8hFFh*^l`h^>f& zh|`Gch{uU{NXSU|NVG^?NTNxKNM4hykYbUtkSdVckcN`xkhYU9kU`0q$mGaiWOvDO z$vVjv$uY?!vsp*jFxf=eoY*qh-m#;x3$fd?r?J1~K;sbRaNx+`805s@6ytQ|%;p^D z!sn9V^5rV#`p8YetN{SUT5B1-f2EkK2^RueD!=={OtTN z{xtp(0RjO3r#B88#V5nG%^zSzcLB*(%vRIdQo_xmJ0oypnvR{D1zR0>tTs*0!vs&=U1tLdqws?Dpj zt9z=~YoKYUYCO`I(PY+i)vVD1v{bYnY0YY1*7neD(81Kv)=AS@(&f{=rQ5AXre~p7 zq_?Xts~@L7ZNO^aWAMt5$k5EN&~WdX{I&RNAB=d60*(5Nsf`_sYfZ3B3{CP)c1#sa z6HOP*gv`Rt#;#w!?tlHYIkma7d6NZ^g_T96C5GiS%OcBTD=n*Rs~u|<>vZd{FgaK< zY~4oMCc$PEE(wo^FWXAm#@nvgN!mTKTeX+6PqN=|kaI|N_~xkWnCZCZq~%oLgmAv* zT;_u9V(C)nO6uz5+TnK5&EIX%ox?rc{ey?NN2149Pc_ecFNoK5uUc<%a4^;H!|D^} z^U?RJZ<_DEpP^r+KZ(Dqe}4db!2N*b8;UpbZld3W-F$V6;nwY29|L6qp9VpKtb<+! zGX;kRFNP?EJiCp1+v)b3JG^(|?`+?_cDFv1HZ&x3AxtT(IGixtGkiQkA|mr1`aOqx z1NQ~)KfZq&X%pES#T%6rbsPo_cbwKsk8vB|LwZ^rBb*go*>Lu!{8n_z@8W|eXn$9)FHRCpiHX~YYwCuFH zw{EoAwJp4|cs1Q_)IQRo(=pJg*4fjg*wx-G)7{b|+0*!1{B>QgXm3rQa9?%5P=D1M zp*K|n!UNTBMc&rF6MI)bcxAA8NNT8USbn%`L}jFJRC9E2%wTL{+-&^Agw4dtr1Rvr zDc`B1>5v(WnaEk<*`)Uu-e=FT&6Rx+{?PPM{$u~V-u(0eY+?PA*Qevf&?SPUq-DD0 z!WI6N##P1DcWb6=i|g*|M;qavNj|4_GIO`|#S~@{!*$)^W-S+e!VY+UX3!1>D=Y2{8OPQi&W3fTuZ$ zp=i*v5{#Axa4LpSUw3c%l-D zh5^MudO%??(J`<9C_M!2+<9JM5n^d=eu^woOLsDI)WJsx6dfNI3lj^;3;ICEAO%Rs z7|9bb>3R8Ohvu0CUkdpMOKUUedd62IR*e$#2@HFWoC8lrT0g3^bpMzwr+0~h^pA+4 zg3LNQ(+B~Na*}|Eliz!rQRIKIu3XOA5c`R}(w(yJ!Jh>>_Z^@QA6-pQX#GKICGM6< z5f%kqSE=PU#fdy>1}x7=P+~{ueN}6X6g%lN#F;I6p%nx$FLZ?vg&F-Iv|Xf!yw?c< zfJF8X^g?~v3?+{q*#&I_3dVJUExi(?-Ss0ASH1 z@w|%8YpkumaY(=P$i0YNDP)W)8J!Gu(hRZm-)QLnj2}`W1kD0)TClu>3LP9M|4IS? zNGG>a?@!R5r4lS#`3->bt|b`YFHi0(0G;YtI*_ukN%x@^G^oKc(up*br1t$GR?&lh zDL-Tup|a_W2egs7g1r7P|H_1UsWSw!$}0T_6GTw$30i2w9~dx5D4IJzhRn)eknN;) z2`~8VUi=05-*FIu6%iYP2A)6$PD$uc4)AF5ZwK;hG5{X+kv;DEP-@a~Z)4H(y`N-d zRGO1DhfPkhu6ROpOW{h*g`1yV4NSw#n|vjo2B`{A zx&=Hmo)N4YD2|_!VPAR~8tK|_i~A!*zQx^v&WvmIp}As}rYCGwK?zeWsxB6J!ljs6 zDq}~oXpK{5!8QsO6^vz*B-b|X6~64|rT&IC7iWmjB}$*@HZfl#W4rnn&** zk?zbi7KFq#-@v@y8G-=TwPY5w9A_Z%>87RKPse5{rO_$R|G{#{z*%!?vfX;SnH}3r zCHo?IUG~h?%U)UpQRDwR|6yq-qnSU0RVvveoZritBr-`P(TVa3e&M&M|*qbL-rT^1^eyA;5-WBw~@9+3GIk6Cc`Q7hpEP`UMU9Ndqa%k=sZP&Luz`wss zqrytBCO7U_T>QyP+FsFa>K)hg3+}8Idto- z`H8Tv7nS@xrs3jee^oq&d=?v@%N1H*FCzdmu>-9STBo&MTbZqyC+^FOQny(w->ee0 z(Hs8p*Q0EYREoRvAkrW@b|ooe`$}GVC4Hs)&K?5LdPQAZEh0GO9huIimaeE}W$KO> zq|xWXn|>mpi6lQE&G$sMkvuvW55bs5837hJllKgjw3YLwbcg%Nnzb5bnd`HAVi zQw-Iz&dbXvyTxb=2#`p$)ca|*O$Rf(2-;^0T^&urO&}2O;5UiNIVf0anDAsU;jrc;KcIhoy+AHQXqBoD4sa7}f z9KC5CXP>X2=j{eVm?Af;fj2Xt?!&Jw{wVv zyz-47?=`Qf3}4r1?zE+ojg>qg|5VmaX;6XQ$8%@2P|rI55i!w3UHA(GFy+)S@40Dl zjdAg&oezq7)?y^Ze2^=1hWG0mL|!-Jbz!eIYQK_3 z=jbw>ewW@4pRLP~HRP_0-<2BrD@n8Vl}cQu0*YRM1)ASU#5g;92{=NnK~>9Y(&t$T;|8ektPy8*6^U7|3bt;(QFW#Jo;a4~6XrmhkH#-- z@WU^?vSzfly=s(lpA|gP-JIO!X=U~tnr1da&=pPF`He}5go$4~#-uj=@x14)gB z8k|Ul(D91tUG}U(%8XIwiPmwrVTQv9@rBI|GXK@_T%u4;OQ+%o^<<=wuT$27ljIAMeaR5=U|wa(S$2{ z(8ncC-F0z{Btjvw@fGKrh&O<};O*r5XfYm(VUhT^95tV>FF$lvTgOrJG}yeZB*q+B zyEE!=*LVtzUg^FGxn4o@{sTG=sv8|4GyoF)uyt?IU}~0ZWatAY{O4D* zYho>*gVlxo(L71g2-ymk%eT`gixp#7?V;zDpJ_@Ecu- zCGG678RE!O2$|Ggw~QXDa#fTMFxRD!5_!2pHXVCRba`UOZ#p4pFevn$?D{v%^0_A4 zPZOaDlX!%C|q-?=-V)Sp89N_s-h}+L~`(1z!j6y8HLv$59HoSzQGl}@tjkr_xvUg z>*Ck8k{s(iPjojf1w08ZtaYORh1eLgvJjt}69Fyi9lMg+hsH#TpC#pY7h0gNcO|dy zF4DcUcuKMLQg-+uG1Da_G)v~^kSu0k=l)ihD+Q2zv%+JXuh@ls(WpJd`o2WKd!^jBcmFRss_9aM z`vTq*@pUY7&2N63>D(ONQrz71`wy}|yOZ7>m#6 zdi`c+Y95Z0l97xX*Yv_^SWQ{^EM3R+_{3)1yvzEApVBwson48X{j?iLw1dYQO@Xm1 zr!4QAyS;=AQuqI#^I(MlPG}Fut1Q1?7*FGg$nohnZ!&v0SM2Y)apUQB#l@JZ z8}~nV2Vp(hxY6*rSIWLY*}MtsO=PEIt`&(m?)0l#sxK)s9_jzgeuas4Dy&c{IesMb zW>!?i+TM%rL67hMJ$>$VG>o>Xld0-_X7UNGueEl2q63*2h|*01N?S^`4l)|Y=Ee$+ z2i`lmt?QlIxe3+m6oQ>rP3rXF2m*LK{l?|QF}VNpQuS$-)ROOa(_oX#?-Dk=$xi0f zljyX*e0Iu4)C=C!OU{}}+yRyrSA07)S;k-&M5i3On@Rm&aoEfBDPDAe5#pz+3zzff z*qzH<93(&I#C78ic~R|JwHg(~u%1!D$pRyiX+|J7WgbSP>{+e9*mYpi$yYwQUKRU* zzdnCtM&?1ba*LtvLTgI<=;Mb#r%*gJ~zHb$X^GZY&P!EG6~Ixcw_{KrEeD#_@GOzMI>} zcV`@8z<0=$uSE>?+z@eCyIe!Ne^IM^?Mn^oN%rSiwbQG?Gg?*|r`SQATEnAXtd>p$ z$R>8=R&69{{?^I+Zss8XI;qXeFTi&*pi5a7tp2?;^Aca0aR!O-cAJ;;@w1!qiLBqI z`24*#eTQfD;`=(SQMxUnc@@Pu#zS5)F*&{V~!bGa&}dZga7 zlvj@_Z<5IfyULdKA)3`gg#GUbcD$DZw(RY0$ACDSjw)$f5L?ZDEyxyjIbc+=WVswBn3 zmThah)v6S`e@iWq?2)8dUc_tk=A*=#Mp3Cc?jV=H->aI-v5Y;)eCMBX8CyAj&9XjE z)5XMw(b5wa{Jq*1Dq)-Wo!#t2_}QonD`ohrJe%sthO(WtaeSh*oO$l&N1kz0+&zKe z=8x$}d{B>GU%3tb?VkOp;^vv0-KYvWtK@v84i(jnL_)@)BF6GO!H+^#=v*G;F=cWy<*{GA<_q&%jJ5P^nmpl)&)&kJV$cLmu zN?vF-U+TKeG$J3kTXqit6zc0veXlZn)~?V69WZB)e;}O}r%rK#N6I*jbEhTTP>LhN zg!>YmEx<8K!qhOJaHpoU?;LM2gw_AB?iRP3oubC9UUXpC;Ohm}YiT_3B23KoY+mJ>}zMn-^^0Ls=7`&+nRJ-OAmgW(8AEr}yBsHY8^__2tp>rZHbHa!}}o z;kKuH*CyF=GUD(skE11e)ebp23F6&sX4o?|Y)oM=$b*huuE)i=i$QFf4+~m7MF97i z$P!7#`L~U%!@bv+K2x-kpEfZUcsLAM2v&E`7TlOnyxORzs_xj}BGa7=^vtHlxUMWG7-$cuU3u*IarM*6#ln#n zhdb7}!)Z|uILJzDsaKQkN~aGoSv|bH)gR^PKlox=PJ=$=#VH+mCk&1M7QOC{YwS1h ziNnx>nEdNYM5T2eez}JYs^`vw=WanA56lgGtp)f(R&dXDy3w@fLM=(4mcc2c^NMqI zZ#?JZqm5^rE(YoSEQ#Mr1Qu*AYv~c=MBY;gfk^936`VwW+S^t0te066-XhG1?~A{O z`A#&j{zzJ6&b3JQ3fpp(P?U{$vwA3h;hUGnJn=13jZ}e;u<3GN+c?CSTrypJlbIgXg8=2_YQy|Yfw}44kdx)c zz*{L4Lt#%jUVIyLcJhpY;(Gzl+YjA?s;R|A9?3k@bLtF73@3UK%4H z6q>wP|H9YUaI@8M+IPmB&#bbquNX%gTAy>dbZTzf-+-TdnMK{A<TEFM~>r&30UWb?A1v`xbG95J$OwK{P!X_L!>54AfrMpk2aJhOYstDPJ z4pqc8p59*{8!{?W99G~b8@@K5nDTAi#O&jV_~YY>C?aZFrAEC{e>VRo6{eQ zAt_eBT!90ytax{{&6J0?(DE$>O<^@A#crdeMKfkx^!||b{XV=Eb61 z7+yF0p(RP3Zw0sX2hoa~{6)0#;ds;^-+sQem~iVtDLE-oN1nlbu}7l!5qlh8lOF~8 z6J_j9EBP%BWC++D#5N8U&WVk2d8{S*Q0-7a4H92u@&}qpOsC*06u+dz)ug8L+n-sy z%Acu}h4-e{cKW)f9u=us6tzu4O~AcnFWhf9F2DlTz5!nyIdWNSHcrRQWA5%tnyW%1`@UykZ{2@N44k$p!9jxy-f}F(w3Y zlD&+(BgF4w*}odi7+-j7DQY~8=hRyYv}bzfcbpL23p1xNCLPVt=I1&j!?|a@uMy(qndQnp1{g~CcnAqT<@g*G*1)Hxz@QxM#ck6r< zN`q55#9BSKWh?hdXv||AT{_-Moh-|hUTmvfTh2V4z6)nMPA8X5tRKR>0KaBaZ>e;wG)l!3s+gCCjv77Q9YBsFqG=5{UcIxcY$mCg3_&p!6NrCAL=odL+}+o z($&oJpGy_sUnr#;Ou;wrr=p9@evJPbu_aU zy(vAuP(0VN){Uny>ip0wrl4~F!A&;nXwvqji}0Ax(aiuUWMDTQZeJ1@*b5 z>%O1PmZ_`u{_Z`V_Iez~^+_#WacpU4}8c96TaD~C59+)#KOmyOZJf2bqI@x%sa#gi3gPH>%wFeionEsS}8 z+K0&-alR__GWYTd;V?G9k$(Q3EQv68}z^IpcSiViGNofRNdxMIxGqGZ|%}18D8n%G{5ex6T zrJnL2u>Rd!&mn_O&Klr=rH&T)mLBk}=iUL%aMU}`XN=X{7DP##d0rr7v|%B$cjuMj zS3B(YEcv1))%LQ=hwn-ble%~@MxiNmzdk-ZDcUE7b2#|yJrRbGrWkMrmG}*Q3A!OZ zGMOWoG4w{Y0{`jw{3Ey1UGwSAQXk@=*cPFg%T>H6vLQOoIK#(P%-wrh#p zaafZR*_CgrHu)hF7ISY^;@R1Rh9%@&bhO_n;>4V5~-N-}uM!5M4uZPm4W1`k%C zYPr<%9N`2N8heJ5~(cRfa@j27!=eCu*_G=t2zLiDkuTA2!>nGlH1kF~_0F>rfw9T;LkCwq{)%rAuc*>8MBK3cjweqVT>**b4yz z10{HdW%-gK^ocW zm;5J`1$;!T7Ach#YUv-dzlzW&#VmbDdn>#hR+Qka6`98o<9N9{Vd3J26zhbeiL%d! zz%~mbrALd}x>O>^#KJ5Whi$k>-Rk%5GiK3vS^6J)F7>qe zW$y0sQq#Kc9#QErIhCeH@Ri3VJ_}fy ze24Tv9AP}-2qR~2UvOPCw*0o7@M;66bmFxlb=b^AhW@yY0mJS7L3UHd61p%Q8|uq7 z=VCT(%8i!$oITQJZPiYjUO)eaV99JE{8lO3`8p`EU}Nr(K9%HI@&(DP(yaCE2QkTm zUbjfL_>V}G-70AmgHGTb=7p13RnyZXjmz5?`^2W)^mq#sgUB%6HulN$U=Ms8p9~$m z#$I=>d&&>3t+AxXXFvzt%}a4vOTp;IsFtK_{(LjfaetYNU!rta7dmAi@R!-bU?Rytory0plI0*`v3>{SyhZM?g)emhZFGIP;-28W^X>WQHHRJnYJ$or392G5N7#6p|Py3*P%gnH5?< zZ_jIqlTZ|&>VpyWfxqp^b?8l_>m&2VbS~urMXDhZ4BSunVpIH&?f2y}u4XvDtfD?y zJ$AfJ*UO&u4KAzBx#Sl7VN}IDB0W?^sYW5so;~}hzs!|EB#B8eHCMPmY9MR5v-0f+ zc{W})oPBExv$rcF!Q)#F85a87Gxv!^4@BMG!S9LN3p0cXnA_fqKu@Ai`y!+1!H~*U zrtG`T*H*$2>ms5cUt`lVLnx?h+Aa`4miBJ*;U(XrS66FZ+@0PSd2_{`$=Y<;RL5^b zfBR6d!q8J!6wFzsJ?^4} z{Pj76Vws~}4kL$C5W(POnlmt(dC7H9(a+9?HtBA2_kEqoSp0zD#x&V;4%es)<~*8q z@y!*Q>R*)}SZ{@VWhs$)8=}J`a-ow@ZK%`wzMva{c=uc|UK&)ZmeE5Teq%|Ra*~vX zyZ<0x`S3H{?GE1ZW-F2V?`xFc4`W_Pi{+UK2*aya5R!H)oFGtfb)EL zi0B;UOJyAHWyL_fc1l?RFPxRk=R!y1y(2vCU$a6PuDpG$Fvht^;ErRKxyAj~3wJ+G zo};T{^eeH_z$s>U3sx+B{dnkHBu0Y&UbEncRju2Mr=e3IfpmR7rzGOrY1jFXsQCx) z1|p%10XH05l>0ZgF3>qdA{Z^UNAU(KM_%0G5{f3xS;2V3@MU^-t+L`usfRUfs=xFh z=5)A!c65iyG0SlGmX!HHOK&aDTMLB5@S!7#lRBg(%#HNMvg-2UC12kS822Ri7jutD z4{L8+rPH8-eX1`tkni_kHf)mqSeFx%?*<7*gps>H;d+b4<7?nizj^aELS+3qImXbN zqCrF76^9k8WBc0cZyBGmIt^NBPB^;UCxHrn{;1gCPrgHvNNj;GnrHn|`feyUamJ)| z>uNY5b=Q50CkK*h4`$D~IQyG;qa~O#J>X&NsKK(pAa>J#ZO}|@*(g^6`S>&>>&TdS z-?Acsf|0KF#OUrV#kqo%-WKD=cd$$IuW&ef(-`Cl;&h9m$39}!2M=-<^*%4V4%u^J ze8p#ObK^eloKQ)1QJ4Gds)<1W#WmTS<0W6OmO@ua1-&%q!STA-q&hy)HLTkBWa+pF zSGFs$^wV6=znImp-*4`_XkFmTcvT&%KRx>zKUK&NK3heUBuoT^t@^9|Kt!83I&-oPBBq zg2sg!U?4ZX{Oh-Ekl5em=a+7@)sVjaSz$?@;_b%)hIXe*1oD@E7IM k-#z|)Sohl_-2Bf$p0*kWY9k>QfCZ$088G#~mIhJ@08-Jxpa1{> literal 0 HcmV?d00001 diff --git a/Documentation/dsm_bind.pdf b/Documentation/dsm_bind.pdf new file mode 100644 index 0000000000000000000000000000000000000000..daaad7b965109498124b0887502d665ab64b38ac GIT binary patch literal 30593 zcmcG#bzD_X_vkOuAtgw|k&uSNIdpe-cXu}k(jkp>w{*9lw1UzINF$*jp>#>zjeyVR z`99D6-FyG|abEAk?7e5Ny=K;|HM3{tP$`OvGqNzTp-}a0$L6E3fxsXKV`~&%US?S{ zdka@f5Ib;2nOWS**44}z#4K)WWQXFJ`9b$Qfw&Qii8`~s z{dLXrR0*)@LAG3NLL6&>gfHbY)xwXk&SHfq%B2UWA~POMU8&!y6=VwH_=g(o9Y!c& z%3iVYF_ymVsMg^_jh)h?|JHiorw}xUf#G*vtL1k!X_Tt(cOgW~dv>wAmuHuBO)tB2 zdfap5EpeEyCo|67#^35O-8if@mbR-~5v^-eJiAyGOG@fHt2>J{(^};*bi&@_VO!uRuEb!=0!F*>;waHl&%_>$^cp19i<_GrX+ zrvwU{!1Gh_HMt3mgbQHLjD4oN=KM`Tp!JGRZ0_h&f0h`1##8OIs27Ld^Ahb8^2ta^ zFV|GG-RN7tXj^>DUtIqj7ov|;-@B7^AzU#0V{K{QTn1bGNIfCMis5b50ed?(siu{s zH)m1hn!rUT{bzaB@}LrE>H_mO%SclRWt!Hios1w#Z(2Ic&z?4v;gj1iBwqzUC!RjD zpD_>h9*N{xR!{9Slp*zD4J!%eq2sg=vokg@m}BS%%C*U^4&yHucyhnpL|k~c67pN$ zqy8+VP2QsQHFR*rT>nTu4+h?mRp3K(7&T!uuIon_J1vI5&AP|>N9HrQU?4^s-aD~_})h5zCa+> z`FjtktxBVy%!!>lO_8%KKW^}p{O!y)@*PE5J07I1w)(93dR!lwYZU&6BkFsx*X|5Ua)t2l*U$1F8Q~J6P)x(6C|CAs)A{C~ zxLRBYI?M>mE~2WMhA&v#)eF0vMQm;76+>zadWbbi9rs2yoQ1d} z74HEy%&J}~ldI2bfBmNZZAZ^7*=Ns+ekw(>>zHmAFFuo7Tn6jriS#aLMFfo{QXuQn1v{!Sg9r&m9`ecD>xl#f)QpGb!Irh4;S^>+lzu~;bY zqnFXJyeRtkA3W@4obF!$+$c_~lK_;AMkb~}jLBe!E~Ixo;{ zD6;7ow5>BiC)^3bX(8s5#$ehiEffFp`OAV}yis^lp^{zDT-^uvE~KOj6udm?q_%R- zYO!AN(U%(m7~RAS=pm9Ev`a8F^{u z3lEQAe>L@-8y-;*<5e(Y>@)u_GhRAI9=tA8NbfDMgBNgcPTjx1hA(hQR8<~O9jpM4 z;R(K66k;)$Kz%ChrLpA4M(sbQqzMO#Ph*n!cwyOKRs$CYeJe4!rqcpyKz6aPvmZ=+ zMZGaeB$WB$S!G9W5n8au&of2}A=hM-kx%UPp+z2p?I%MENxL|Bo_m?5^wtG-e8T$Q zOdji4HOf4vdY&9ZB;zwhmqG*sFJ!j3V#)bYm6mpHr+z4ei5~GsOk@RygX84q4}|v(W$vV4!*18n0yTRDsD@o76i_oU0w&O%9@a7}iWkP$?n~PdA z#a=&7)SQNxK7;+sP3csa@i?^|*5t*J`|KB5N#1!|wK<+7#6GECrmJD*g>ebPLT}nf z)6B@e4RvB_c!?)}QAYn(RsG|la^q+zDUQm^LiM0JI`?}8&B=)?u}=A9L)})AWjiU} znCm3J9w}!SuXY)-i6`!d*mZl`cax>mW>fP z+dD#$ILpe>n3d;qjPJsvKk9sAXnpP-J|eE(@?V4Q35LG5fuf!&>X;fAjHvXs7$_0BoN+u`dmAmt42KB(b-PT>yHCS*iPoX7}# zPRTxHH0J*lHc2pqj*bm2B9m#QkJ-~AyE_we7auowW!y|Kr%OK}Qj{7c?!~NafVZyU z;#$AI%Mx638bx?<@bA5c#WdHb1&2-u53UpS3c8aBQqPlRRj~D)f1gP|Ls-SDO3=h8 zw5&>!MiZ-P{0vdFcsQb56$D=gLnqcN73E5l^7W_8u@ORi;oDvVKeUFYh&Xj4j1}FEyrL)h=)Yt=w>)biG2OKjo!HZL9~t~B!yOFK7Emjo zo|!#`79LyQs?W@KPhP{*#1g;P$X7Loc}-O6!tIh+YhIm#%!D zY1B{;JV%Q?cS|8?-R|ppGShs@-^mr>3Ue?%9lzSF0;f7FlvL+w(Rz-KlvSF<_94Aa zH-BKW#@1JrpkB3Y>XKkR=NIou_K;$cBb|qu)ltB6yy&kjhz@G#od%T2qBWMM<`{-o z(gA^t@8%BGvcwjl8Q(r|M?7pN2aNmGEMrtFL#6azT74A!zSKJl@$hUvXRZnd zmpDluUkByW#aH}HiflY_=l-dlu4-b!rq|Qy!cob5Y$hc_zfn5ugYlru z*!lr_W{BwO~Fb$hj<`NNk!+#gwpElbf>$?2Boi~HES>i0AH(c9;4>+~iF zeJ`YH{YT!hI5Sy4_9_3N?5J;^G5hpNg8j-y-;_O%q{nvs0{4Nwe0*0_CK|gw;SY4p z4Tm~&FMeHycOCUl=x}6OKcnzvUfPs~D;J3dXYIe2bi#=I>Z(@d_reaPo`+Bw&)%8; z8Y6rSSAl8OJOj0n=vpVHRD@CorI0qx2p%)eN-e7XBop3m@(tT>_<_4#b@^d=0 z#H?cR3q4ifQB`ZDhBoJFx+2LJaT+{hSd{&(AD=J2PJLxe#Byr!ZPN5Re&_XQw&l~| zcAY+f5s?ab#T5t6Me#6KUn{R5qTNfjN8tBSn_6CyqB?$QnMI?uC>lf+WwBseqY+2n zrdZ_2C4DW}HnFVTAq8Fe%$)oJ6;nnQn_<4ijZ)#6HEzcK1=j~S~ zEU4Y$1sSpS-B`@N@UaS=Zk)I|9hC*6LN5j1jP`8duNXF>lg~yqI9SsY5~-?mp7lhT ziq@Ive~Es@Pr#)lx7b^(!b!zS#bXzmVDEZFkGu=RCwftC8dJ|jt8Lm&D9cLv5|8y= z;WDiaIg&2k!v;Hte#wlJX9)`C-dk{?&!gGsn1ATSD0kvkJv02MkKg4el;B_?QqExO ztjh1-%hnB_^TydH9%+9k8O4-x+LeA$j_;MQeMVy6jwLnKhKA{*pac6=L!J(qX)L=S zU6s$?eRLF$3=XXe2zYDzt27HmrTfI!W3MStJu@EegpBjUyR)K{Y&G*UMLR!^+}&Lb z_s`D$LeHG9F{{>B(D%`!CfpnB_}DBuGuwbdADDHR%Ttk8LfJf9g5!8rZi8HyTlsDC z%qcFIBR0}qX4i|J%oS`Jf5wLISj!_cT!$d}OqqS}VM^g+>`(UG8wp=S-z#ua3&_q! zB8sENA~W)u)y}nKzbsxLL{nN>ke}tXhzsks#NKQ#)e!P5j(QsQS-caZ>i%A0uk69k zz>nM0+pZ3~NhzP|I7A9@+CByR=%tKXB|6XinLRdVR++UOZjW?c#w<5CN}-VXl*82@ z$s8_t@pYnXcp49{cNBX-rSl2T7j|^pwbwCx7MA;Z!UhOGlNTe$X!-l3|m_#f; z_9w}QWQf(QQU#$Ac8#3=9A#u^-l2*(r_tps&!`N13>^+#GapAUsKFk>fz6G&-J=?w z6&Adn)8oRwAfqik=+^j1bU`+ezhKsZI_ROno;RX!yWz~+&W!a=y_Xa`3-wW<2k3F@ zu5;fJ*+!MI1-*#K5f7WfgOWproA7u-+6h0wZgI3@8V+~LuT)`nXTf8G$(6d{Q$7$) zimvVP@5nla4BP0VH_^YEwVO25_9N^atK=Zwl~h9>Q><}(^1zw-R8-W=IQvC=xc!er z9b(;+@SVa@xsTx^=~ttf1dn$U%TopEAASpc=7ijih=@zbM&o@o8&t?&GSOX7s@Pl0 zx54gfMWj`eRveC}rrsNo{Xo!QqT!b$smNgfZ}02(2oY^MbVn~P_Iw9PMR_tJl>@>* zPg6tZ-Stz&GXfLWMpOoK+pq~-ZH;tdx?44UKr3hZQEgKAU408hrah~#1H9Shxms$R zCf17j5fjb|CJ7LU*+f|`2LMbEldcQL~e{xJs`q3?8oy zjXBOtekDxNCf0O)5;-i4lifG8EfQVK(1ZKSRDr;T=($4HDIWZXvT!D!RfcYI4EGo2|LbG?Bvby+r==?YQukLZOnV11-Y& zN6DsD+W`>?{xv2Ny&Sg<09b6`1M-fq`7kX7;Wi=uQ6yoHlcDaC0^> za{*>Z6`dVSRLop;n1MM|5VMpWAWr1&zZ8gB)y&fskR#$?>)@>7Xk=mrV!4@J1!RbM zx=N`0k!3f??gC<#u`+c5 z>7d*gMH%H!Ih0ZKKtNI5fU-mWA>n3Z@sA<`S&UqbY#l7_CEZy@!O_fK$i&sk!5*aZ zM<_5`ZFOS-IWt!yQ{Wa5^v@XOAChls`yZ13Wf&A@1w}a!3qSvTHQ$#)hgr(r)XdY& z6opyT%-zbwOj$x01;WOLLZb@?gZ~2M;L!aStU&r6D3HE~9Z25;2GaMi0qJ`nK>8jQ zAbkfrApb9b4EB2f+3fcKGT83{p3i;<3y}97HsJa#CMG7-)YQ6A2=Ihkq@<*Dp+KST zfdGZN2g(X4a1R@hzC}kzM;8ob{R;#r(_bKLx{zC}t*sp#91IK$07Zbu-9e)Ze6+N* zfLesyqpz$`0hb2MkENhYd*I0bvK8cn1^=Q3j^u{mb?Q328?nC7|{0) zMn*<9z^-7x&i6o}fVQ^?2?+s(03_d{uC8ujVWFU)pzD0|fdKkJSOH_*!q3kS*b}hG zE!^DPfW-jK?|=YJ?;bF4=U-R=W$pnQ6!;!MS@0bYK)pL;WMqIO@X$NJP(bs0fI0@> z0b#vS?hY_3Q1&|jbs+Zu^#HjCs0F}3Z+P+I1)w!hb$37jXS&1E($db(&dJHi!^1-t z0u=NP0RaI(alo%{;o;$tkdOe{3vlB-fLB3oVZU)g@Lwb)B>_!=tG94)aBy*P0S^b9 z`jA_srKO)ge-5+-z>9AY7Z<0bqy&lu z)WBbW{6PHq1K=N!zW|jFxdW(O$X|fk0^b1zG`xj^f&zFfaQhu#2w=xso;`a8JRk6p zTb@3B%FN6xDk`d|sHm*04243?%*+f84FP)sP2vtH;6r}_ycl{9ARc;)t*tH4V1c6D z1DG9p5AbB@J#2tY?*R$~y$1+H(7yoZf#!4r1PWC0JwSkj+`|f7y9ZDcat{y^Ah(c{ zlWS;bczJmN)c~Bl#n;yts7OHOJ%D*3x5&%O^YZcnrUTC0!otELCnslaZVsFP>h>NW zK0$8L)6)Y?1!#4PnwlDr1gw9HySqC*J-v#Gite93Pft%{V`D%&K>j^!Kq>B^)xG&Z zfHrv#U<{xRZUF;^y$1>y{4c=W&^ugpfe#ZC6Ce_3L3h~e-h7~do7@8dOmv5-sVR^V zD9$ZzZf?L8;P4J8;QDuPa&l5pQ2{c5U~&fp=)3NL0-k+`s;Vjw0DypSi>9U~ARfqh z2NW>o9ZE_{va+(k(*SMn0m3lk7Aq?&Kn4(*?|}Si4L2=<-QM0FumB+Q7FSnSpcw)M zyv4@G259Vn0`~w<1pftyvq1T8fCAC$9x&i%e*sDlz5@!xl6xS4_4!6+i-H%`Xv97Po0!3l@GiLhNAnngM z^ll`i%z^S}6#9>={}?AJv#_GD{25;T@*4c1j`a~cWS=azDY+8! zjM~kuR&-E}ZmP+YV`l#9qqI(1Mv^pDDB%_d`vro z=ZjE_;Y=^iXkEFpB(8>H|exzgCs7gT;MB>~6H(CBH@%7pnB&=_~7t><|+k)@!5#3bY^*(Y`T? zu+e(S3o(2wl4*gkrF3di@Q?Ru#N6jHuLJ^Ct?@Y(yGE-@345x>jn6JVM{-Nh3Ty=y zz;isJ9IfJ@%f%+q)&AV?b@;43M$zdr?{|;Pp zz7x|AQRWRI*b0}!LQFEbbG^^kb-}(2d;7i*E)iViTFE~Z8($&>wy|-w?j$jo8Gcp^ zvfP|R5!B`m$uTGh?K9XCkt*qqy2j@zid!V9au`OL8n)hWwkL$QF%yTu*UHSyDLatm)Bs$#= z5?@-*4?Z+QkYfhTgv@JPe(2%H5e-?d#N&Giz8=fm9!Odlqj<~T1hQYI8vZ64X7DW- z-h(-%_b063nmBoQQW0EF~S< z@SE?P`B8NwA=LD^lm{E%kYQiYmKOG?^VbvRdJcIya-LU(_~NK1xXF7yY1w4;)q_LR z#2}JafY_p$U^fNmB>Re?BasH93`Dm(2Zhl9c9QnsZ(#N5W=L~qhj71j4+>G{`NUmDj zEQG2pzgbvHznxsP5<9+BbR8RT1y3j;MoLS5*ft$-;VSq-E@OLfkfh5SP&uiyqG9VY zVJlHGaQE=d zKuK`%eS5H?=Vc8X1T+!mSU9cD>r^UWvM4XJ5$4HyYTo_ zfAXzfL)zDGvfRdIRbM9q-P=GH^JG87ZN6KFjLk8O;ik^bU!A#tdKDQwIPI6`^f7Z&N z3Nj8kyfHrTZ-gh44Pi8SQ1`N3J=lmD8^y?kcof~|4XBzF9m8$xKZvZX#u5BZQ6v)~}BSoo#jqhUnw zl!m12tYjGTYCQghn$E>0%CF!b(5_c`N%D~Ap+@y%;qIVjGF5p}2PTQ{ z%Os+HHF;pcVG}MkCpgCrQ*^7LVQcQ`zYlyl9{HBCsQS2>6rG~%R~)wNw>p}L$17%? z3So5u9urSnE2z~T;CiPwIwG{gUb>c26IY%!lM(vZhiOxXZwFgt#X5vkgUahyI`XP;%1@sDGI&Er8bP&d*mX65 zTpZS#L34#FG6HYVOQ5)0L~>k=&N{5D66=c@lXhGd>eZL&pZ4*PZI`$9fM{ER#9kc# z;+us?l=#X@ye9uxPfK?j$zW;KW$EhT>1mqoDm!|V4boiVK9^`5A6a2iwDi2=I1_w# zCF>_bVSjG_DX-RED!)8g-gb-SU&}`*uUqWjokkMv3|&=X4}t>#F}{))@qwwx2MIvk z2>)8|5&U%J{Z4IAU>ux!yn z;%Y+y1uxsi&N`J|)?!w!cSj69Nt8&MGGkBdGw(GgwW+ePuI*p#Pyg!FfS5DnDI^85 zBPq7`-iS^1QA=tR-VTCAfsL(G#S?|l&nsl^!v>PKfVY?O}Kt#Xj|{dyVB4q_L*ZTx@Hau_4+GSZ#Dr8 z4=HhK&I3v>3QD;}k>x5T_4E~@=@F_YQ&*nk;*PSCGiU=_!&xWa$1z5Dvi-0e%N^zf%epQU=NN2P%tLQssZXEjs_QTw5^+g|X-{v{j`K}( zs*|z}ndiw@9K)J1Gh`XLOfZzD=UM8j)+1MZ;X3}#9mt2~`OqVl*ugvHf{atAue##c zEJ;xSrr`Y|82ckuax?9EF0K+xV9#4SP*;{*>J!WNNu{YTEXjo;mSB^x<0=EG*hMc{ zp~x!7F5w><`Ez5!+USHCs^FK!yTK>c>E3kY{UavJM8&0I`7tsiRK0m`^V?Uy4bPaZQrb~D`c$DC|42?_t%NcVZe*YutAd^vG3p4S|8Zh>+!xfGnr zi}#B(>l?cP2g<90#98?dxtS(&*idwu@kmm*`GqaF^OSHtu_2PT) z_no8lgVD4jLu0F{&Wz+%#7r4$wedmJCFIM-jn3~-d5_|nN(N5WyMvJD zi57X_i#iDXzU$2t>?rtLH~H;9i@TV#kPD`+ETdd?aH!7(MwTQhpBX+uzF9394o~$3 zxSurRy}NSTgKu6l5lV^Uc}jr?Y3JQvC%GmZJt(IREk*88&#df;AQ{*sXjUFHYO|4C zdbs^g?i^kF3%7^!d6*=k(dFV2?2mA-)ze%a^Vj0y-P#u4@+`V$Hnu1f$9`6bId{K5 z?t_+6<>u@c4f^`4Jcc%5mk;i7IB(XFJt8)Pi!J$_i=C6DNikf}{XDfq%SBI*Go;Cz zA!N8vCRRNwO?-BBT*0o$E#Lq0z1|~r7ayXc$;mGZP%vFJSM}7=e#8hQqo9AsiYB(w zva0gCgT987Li5`G+MC(^clK$uIXQ(wMw4Gh!iOq&C{o_A(<#6($0VUud>MPQ#b{BI zlE`3Xt=r#`tWLt|Wv3P^CqP_coj$2rxABrx#{96PtnB&5kH&gw_M;Novv-@3eG&~D z(^%S`U#rsNi=baSBnpN?ktev_uBx2NZ4hlX)9KT<2q6dk1qkdKrpYN&=IpG*>$r#i+y9XmG9{2sXC z9rpBFYwoK(7lfkb{Y}K^In^p-&cVtk+vBfz`!2u>PKrB-h-qOgfTwNGBZ?9<0)& z{c1u!wU7xdEI+81_rbyEvg%qdYjU$TK6)nQzOrJ(DuzI*O;yDiqfIWNL<6Ok7do#) z5R8*S0iUvCH#^l!XACs4Cz(>+_#jz`i8 z(GmH@dC4a8Ot`BGO~P3wwX#aRS!7EDr3Wg)tjT2EHLt8nIEty41@kwO-}Hz|H)@Q- zD7-b4wk}Iw>$q@SYMM$Tc^2mMJ5fRn^NdbiW)_n(Zf3~|hK;%2co*NHuKERB!UW4n zS2bxKA*F|?=I)m7=BtUco2ruKp~jBq9)Csh<+j#rm{pY`o(u@9&J8{l`pKC0G!7 zt4jtJW}Z2gCO9YQiS!yKIlAlq*Rr=bf*ImbOB?IPib_!Dr?Sp3mnPIqKn5?tN~@KX zqx8QpMciD}cc_$qc9z6f%P*4>wGvs(Y>6o`vJrzme)2=C@nM&Eo7|6V3g>9F@awMJ z|K3D^c5zb)w^j20a0w-qQ>ZMmtz!}SJYs0VddI(WWqGdJs*mhdw;hB9D}Wk_oAmja zdv&a1nYQn?QJTJm;A)n`Cfg%@@-icEb~htWMJ=Dk%VXQU=)~}F1boh25KeV<9~`e5 zhl{V$OIW*;J!0o5OBi+h;zbW3wJ3&U_IwY{65*WPZ{+4Ui3oeqlj&}8-h_F;&sYdEuh$i=7%#tpj5HDV zO}b+qoVXi8R!gr~{$!*lnd1)qQ#d=_z|)5VA)VYbqY-UR2fJ2!A)6%9PWVI(T;96h zhoj5&ncZy%@VGLiirM*cNLe6@+{4J;5H2HL@=- zd-=x?)n=vnJ0Z&2AQWe5ohg(WfjW^95|wo0I#QlqOn%-<82CU(ah)K^|dkH?Lb+cREOGe`|B!Whw&$h}M|LK@9<79S%-iX1h>9Hq!* zZha4P9e@>{BGLT{e>G)(6ifVDx_mk2Rn24X*G%Z`PrOc##+OEEgPnLvjNYgW_h`dB zd6tjh6c;xRb*X6gRlx7htz=)a$Rimms;f+MsG0L2A$_CiwLAtLgYS8x`|q~{dVaocXInf8Wsh(^e4Wqw9oyAF#0d3$ zbog;t#c4x)0r`Ofi?Ial1-%vhW0!dyHWV4$f3zkp0$Lt~EBCzlnO{Em?Un>AXs4c`YOhM_6{X}=qT-Bq@* zk&D^Q<_%_54Mhzhb$V4*u!sW~%zSfB)Xc@i*~-z?!5MV3YeSh?)j`eP>gK0(Ajs{n z?EdWFxXF4m_3iB9Dq?Bm3}RuHHM&o7F#nl7chLc{L7^ZBuxo-1*y(ZuD?1l3kIo5V z2eX4%*&rZJRwxL{$?@kruuukKW+68_Qg{yyfL^8P*0u(B7jcd`0=ENW$LZU$_g0d~vips+Aouy8P2 zKv}tfFAi2_3kW9!NI}_wFW>+;!3Dt1$!x(2)RzT>?Iy)?lj6L|!hUlv7mxxL!cc%K ztekrP(C;4>_}{l-C>Pg1+i-$VOo1Sp*hyv}(R+5qBuPexR*tqGUo6d;1S9f4ki*59 zjy02=dqX0%cwb|f^~XY9PMtKLxV1FDBs*`sC_8c2|6*0$yk)}N$TwAOW~Vu8MSIeL zJmpN#ytezB#lEAXd3H9CTjGkC`Dj-q=1q^1N|j2Rok{8W8{f&b5J`hIt&CRrYrai+ zC8z8h87)|B<;Z-PqqaVISVHdDHiQWUQp86zt5%==vAqWg6?@BL2o(u=vsrUhd4Ux7 zb6y)kWFp*0GzIRT{o_3=2o=-$h6xqRp3Jm|n2gOCx|p4spO^vP&opk*Mc>AB3EF2k zjBnBf5;s?O(uR_rrq)X&m>{`NPkqA=T#>8$dx5Gw~4 z2v}VNv4L3t54-U)E?|oo8{l=&e}Zy<%nD%x0ZXYMpr!x0ej|$m@KjbPJBSSk<512U zZ{-95YrlVBW&Lv=<>sE7>zrJ-a#^`;rWf|Kng4a0wJZu9~Hc~xA^?9@Rl zz|e#FrW*qGInn_2dt+FjPX9#bTRYxF@!Jr6XRF%?|0iDG82*oq|Ah2E(fp4AZ=yUG zEA5|B0rA=l=*s@blJcM9yXySUhz@9Xx6J&fqY}Aa6$ZA!asDI3w`p4060bjB>z{DO zrmy=JTBSSg28(A2!%x8 zBXc}CZG1jcM6#Y%d_t%7*tFejuH-mb$T>A5%5Hp!Z~bAwp;bLzz61V| z1LdiScjZ#W<3`j9q!%U!T|u*3B97-ta@GerZhrW32hP z_tpDFo5%HU4^kaJUxRVqEPIY7i?34{7ro;hN83BRC+89|*UK`Y1ri2eVs3j*=R>ic zxK)WqFAX?tIi0_yE{84eD)mH4JnGX26Z3hFI-X*T9y~p~c<8l-|N96pmH?ehV18nv z!LP%9iel*P5#zK*`|lsw)jxe~4H9C;YPsbRT^0Mbgj;Kv@*lhA%YXjFErvH)+NwFe zN|RNjCnqrF(^`HgS#cJ>Sa{O(0=E3c_u+!-ou}nj&@;gCQVlz5>=n1MXh-4epfif}$J;q<>z>9679f2R&W!OeXT{+LsCsoOU7 zQSH-b?<+?9v;{EpMY_Fw>SOF9f_oG~yW?swv4xc-#hc5)!{vej*suJNx-;E5Mndot z9|Z|rE>?CdI#&wVhWJFmkafR&@hs&7n{~7HPm{+_ipOjY(cux0U}uKh1#QpqyvPnE zE+Q13Ce#EsvY|58fC^WnWy8f*c$@U6{NR3JY4A<)rr5H%EF_X;ZcDk#=0WjM-C_su^OaF5H}o6K_SoAB&X2AhShUheUiBDO~d&XF#jzQ!E}9~r_k zdq*5s@R_6$E5DpP=$0a#`{gzImcOV2=1HOTJ5uJNx-(gRC1vX|>(>ayW0!u{OJB8@ zA#6KYd&8Xl$rqNsG&4k7DkBjN`EzX;!drMp1BUXc>cvUYb*c2`^@XB*N(oHeRF&*R z#b21sbe=xaN?z8CDTL`!_J07^JU$Pi6oFINE2-rrTI((;FrqQkQE2W2TT@z_aYhjm zU@K8m@}sEnBjhV0Krma7HsLm(5}AWG;g5GWl3jHU8r3;P=Pm*S4Fe(j&kXTktwGY? zm(eOBpQ}Zea@F=v2_~zL5d}VqVb%6@*`j5X?R2J*4env?elLKUU>W@B#gk`mAFuV= zqP|FJk=A|B{M))JzTR{|6Fd2|#&n5CsQ9f*iOcEs%<)vMf)e*NBGoHJD;Q%2l9l!kV^BeVi4g_=2)bAWR1p=sz zvkHex+qK3ctY(_l-u`?mw-_W4N=AQ@aH4z?VFOL6O&gG@pF*8N-TA=3^cBARivCNv z2|X-5$#n0&d`$Fn+-@w=My^A8yERd}UL`F>99xRMrb->gxmbJOf}_DK7Y&|bo}!Uw zBe~WmaG|z6O-kdzM%_yry>A2cT&cH%e|B?iWPZl-3rPML>Gt*J-wBk)j&FC}B z;A(Su=0nEA_SMr@eQTW!nlVWxXQp&1!@UvGT4@ogMvr`!sB_$#x^dSB1-le~gmqvo z%F`lNpuZenb$@$dPfXI^sPTx4mt-4bLrIVISwInZ(oe088i^@JpVosu0pNs5`2AO*wrCixgTjLl&NMhIcv(I(Qiih znsK;W%rpGWkBT>Ct5hze(C^}PmsTQW0t+d>6izIDyTHyY=a(9nO&4VyRKdT1uj(S| zPdYRG7C%1_j>ny9N* z*?~B%j*^zrUqK*K_*aOAn6$MFxzDlT;1nhvyOpf6*y=WxP>f-H03GhO8NOmih=M7f zT5jNZ*Obe~DIP0?x~+~dUX)l0R{em?&`s?oSK9aV&;k^juj zQ+!J}R%DJV=_M~9JVH?#>e+Qe6Ot?b<(aNfdydgbk7Y14jHPIFny zkNm8KlgLDsPFhTgASF;)5e?}oI)$_sBkXrHM>CF-vh>HzggQDKgM4yPk+GWoa^7PN zFRT`#@C42{Ed00#MI&EEz(0YO}Qec)oK5Oz>c{o zsm2~GOO!PDdN5tg0Q;vZPt(|y6xPIuhCxvx8aa`c043zTcjh&O-Jz#BDME?zOUS$= zgYY%Fxi}@*lS>S3{;sXqY1lH#{OREq*x{kgncB(&N)`DN->gcR27`;wybYt@ZMSS^ zXVK^s$)>?HQh#A|?{wep5+69uj!w~THkinP#d|fxjwzlq$+zapb+PJ8&lVB9=DOJ( zU}>BIYh>-l)@P5q@RD*8vH!chp(Iq~2H`f`3HOaOt9 zmSBiC%JWTTpXgB6+FD7P>#lb3QSyQ_!uI8b-Y-3j4+VQ3ky~;J*`SjMe+xSe72RGo zy<`(R(BhQiKVajg@^uJPkw^z~R%M#Nxa^(bqih4cQV410rMiHm);XTv5Jg|H8qjbJh*P_yIrpB@z_^I0)q6?YRX8a4z-$uvw$M)aXF21*X zPughg*1l@`4RihU4apmRU!NCanmgquxUJY0*gyAaJ+bSXYu_Ae1k5Jf+P{!cM{ioS>6dqHG>~kW8 zEv7QB$;wjNhL4>}4&p<@@JwibhxiL10N`09Vb zuRtK$cptp-N$IKg6GBX%u@K}$l^UBkDpEA~iCPkU8GijZ{heiqmGy6rwFsf}|1wW? zGwJlt_x}Igi~E0>yZYygJ109Bm?h%)&lh*#_`|Q~>v(G+r=C0Xy z_GkXSaEIUiCkuBj!GA2=VSy1KIs%K9z+)?a#Z3OTo(Cc*zbxv13)B3$vWLZE;9(wE zVCA=%EI$y@fk$|Li}k<)JijjaVc{V7y8o{L&u{S_SfB$AmPfJR>p+6!j2@OBILmjI5!@1}Mw`oPS#p zJNVH=W0%IaRMlruZ9F1=JG!Rq1l|hY&}0ddg=c7_WD29U!8M@!LS_|W%-`T(y-HXTmNGNcIB{HRiabuPk*G}t-nVB1H zOqQj8>d~N7Ob=otBw5omjE`@i?~{_6xI^_0MVi%7&`4e=Yu$`k>w^IHK|Z6LN%7pI zgb!uXad8d7>*u9!Aqg`2Ix>&POIKdKCaE?ISNr5hv>B_Pg0CMY`MvG+_3%dArkwBP{%nQCapLogJGMjRQB_8uw6Ql;GLqGo#yD>h=jGOw+gA*kA#%& z3MLQvcjfC5ZEDU4FjUerq*gy|oaUUVIFG!?8@JG|&b0o(Ac(ld0Go`CfiG}#3}P|? z=ISow;uv-&Vxi)bvf&ppW_Ld6-x*FzQmjOY>EP;UtyFf-VP%cuPx>ITE|D~I$ApV7 zItDc)P})adZ7#2I(k&;<0f+BHnou;ga1BZ01#{1V7mAc&?5fip96CB1DT37mBJAC^ zILBp2lt@n$3N)j}A*b!hq!A->pAT16PKw@fjX_!4&%Ud;C(QSQehgh0H*0tziUyZu zI42t{TG*3mu=Z$Hur3Y89;*Q&(1(k_f)KC6!HM8E5$_%r)prhj z3GR(`FS0B;`KAH+uq;6kq>pbLQ~Va`L0m5TC+c8UT;om6^zObUq(&5V4Ll2CTRoq7 zjTev&oUJzl@+7I2?EhzI+U%FxxjFL~Cn?#08FT+ru~ZS8rDyEgQx*X3=8 z<$Ou=8)EAVyGy&bVuuCjm%3lk`X3ctZ|6gH)eUki8F{G586J#bu_(%t@#W&|+!3{0 zcb+w5&w997d8({7CQi=UBR7VUsy4WuUv;mm656GUsZAeuCg4e)A)gex1+{dx49N$v z%*dFRHzezG4TIP*hy8G(wPW9Bw#6v8_B><^Bb$sH#$#J)h@Z~v(P-yygQS1 z;A4ES2uf|8JxRXAWvCrOFNvC?yZ7!jwNk97o=OL>2`piOW#}fTf9xSJ5AOGoj000mK5#o zRnDzV_wHoP-=_L;oZD>Iw%?5_AlV{e5hwR9`JQ-+cwrFXedA&N;ldW=h3VHm|JClB zz|@z;aktSQfqq%HWKdk8@MuL40ce76FMeMYDe~A?&sw=dAwiW3wWL~5Tk4f!PvC7fvk+@gc%X*-!;v6A|Fe|;kOxjylG(?{GoM-;UiT-`c8^Di{l-Omp-+; zvsG&H_n5N*pN@U{Dc%{*ul<>WAFIln9=g*nJrNO{;asJ5)@_QrId6k}6S=_unBD|# z(9!sP6!?k9+X{6=q&jpAl@C#X{sH}a?jjE;7KUXDr$~83t7RP=d1zv%*xp^ zm|b`QxxH_x(qHO&N51nMf2-sHjaPs+e4BpxBx_&A)JWhZPP3M9Q4KoiOtm=TYj^X? z&GR;zotX1bqhgYi?0aFS>7aSub*jA%7N59l?g0JUbU)&j;nShxk@M~sEKiAkI@(Sp z*p5Z#URw~7fAbrAev1muRSaT%#Le3Gh7ys-j@a;42&ANLBddw>AXYCW!Gnx>#wND~kdk5vFueOa^_l3Js_HCHrcbVFQ>iq*^5;iZ zSlFRwhw_?Z<-e6J#0KBDRGqE(uqwe27Wk(BEmTRhQ>-efL6Uu7lj-G@sTXUQ?TU;7 zcK^+WaJi@sO+5#HsaP=~fgoEmuQB7Xnbfq)eJXxN>7bQK6}OB>M;JP$Jl&D6m3w6< zN$8mZc_&VV_YL)*SeN4nYA}qgyPvCh2+h9`679K+S(WcVEAlC%at+U@GT7Gf(tbal zgl-jpQjUia8DGShw&}tb@#-}@V^`2?JZ*fIk5*H>lCO{o4u#C| zj#*pilsYnuzkZ^L`Ghaw5t((_>p@AIB#jjIS4S)9;%mI%XT|&*Q;h`s@xqhu8b90T z4SXFsP4o_y5ab~)f6Z-$s8XcWUBjtg7@1hvKkBlgY|-A3-OQ5s!Pq{OO-nXk0z1~x zqvAE=J1&+x_pc`*q0}5zq{t-_P#w?^;#RDyNWDbmt8On*p|9O*_r zyfD~#;UCH-UYlp_mR|UJLxK7A&n<5`_wl@1$vNI1?8OOIJqsGRixm4E*D?sW+(G^t zjs)OV1O#1LQEjnyTS_MBc>K>MQc6sUY;Tm!2&A`TjZ}49Aj)aV$|YJy%HDF`5sgy7 z&R#UNOP2lqqSZcat&3!DguSk*n=y^~<|}N=y+@}V-rNgHwOt<%7hg})P=+3_8GS@6 z9t;=^t1QR(DfP9gp*KTuC}A`Fhn6MB^T}rag@f`hNA9WL}gr^HxkkQ`fXeJUZs|+PB&$eugO~zhL zqRt}9n3vi_tGV>gCmW0HPIq%R`D=DR7JkrgERRTiV!*qMaDrum)*BFDSU{T^L|W@H z)`6p*kCWqA2nyg`jLuZ19(2d4Q$zJ-vJlAjWyV%x!?_!)ji-rOdVjdfbM!Es3>OPe z`R-d%l_mE5U7yf)i+Jkm zt!{O0cgBk(lB&kvyThBeK`2^|L7IG5wJH%SAVg4I*|6Y zij9X{Nxe$A{a~(lZNjN4MKrwSWlNHGOGCY*Rg?M3*;P@}<>EI#h6}L^HPQ=-Tk922 z7k{^rV6%Yx>DBSZ%D}s6Er*B$=l5$#O-WwEE~?a75Hhj&g{;|v7XQo+A<9~Zr!L{G zU;Vab-fc#xCDGiAK4i~jnAkqp-~Dm+@w(uMKg7mOKy|)j3Vy7B(|<>h11B7Vy}h6fG2efC$TCoc zi=}~akmn^m+RRH_FsBvK1sF4h)KLGaeyP}M5IIS3G0mDH&MA(1fQmXHqMuX)$2iI1 zYGyLp0lo0HYH{42#7x}Rl=Yj{8XpP1-=>TD zxLIFmt|YWS?3+e`M82=c!|KJWl8Mjl(qoT}d7DRGgpY~APQsw}Gh;w;v;{@k8 zKYl9uU~K+Kx+Y7JQZa;(TUw4fJUM5#ip8WVC8KnsT5?&!A03~+r1`!<++?f%^@nP= zdSr!%y)ydgjOgLnd03>Hw8j2kx1^X@SLh{VP-Wk73ZRShQ8upB4@I}WFZdAmowS71ls5>t&n?@->$}gW|`mV_kx0q2;B#y4q zhWBh+znq|8DNbLn8v#Za8*whqFy%!yRkuDAjk9OTch zuMjA*k+3Gp^g_|)UV0Rsp3hZ!sms#ll93Y%$UAcq6H<`K(v8&+(dNEk-dU!jBWu^v zthDiQEiK0kOH9C0i~$j;Xx&qi8kejvLvRpe@7Og=eN@HrE`SA9vs+f6Au4mM2Roi3 zG}K9h8htqY$sobC#Ykv1o>Ox`)$J@v+UbjB{Kb$=$;&vE;QaPFjgcBluVpM>I%srD z;9_?o2^~`4taCMew!-f9M;;m4o_B0u>Hex65JQEIWA@gXeC@DI<6^?$X46aLjyEfg z!KsA9N0w2+VZmcvUGHd2A7*`gV|>`aOiExfzH=v4ST%(VHOo$JdZ+P=)q8!(!o>rb z#VFFVBD{&W%W_y()20X^h%H0(;iUEW$fGC}oCpVKxcFUkn6DQsu;g}&vz)Ti<7Yzy zVnaczv^ImCL{RTnpU-RbHA$o9LM8Fe`AtS!C>?WqOk&V{m;J_nnC!l(V;BPA`tSRQ z9U-sFEyT(n_KMoXtf>4ndvUkfbHwo{sZQgDmy%uv2lqlYx_&?e#HEFmi+iK=(=E;S z@wxOoE=_6{T%~2F5euTS`SMPYpT#ht=$N3 zN=L$mD<7>3Lg_@r!5djSRC}y$*qNg-;bJ({+Tz<0UeYa&4u0?Jr+1y)>rHD@*RB@3 z^6oypOLaHrzHva7N;FXqT8JE40V?RT^IV{tj#vaSe{`AUho!I<6_K&}f?MXF5o{p_ zI5ZHSsUPWEVZB-Yq%XKW)E|2vj;@EvP6u9&x1}M!mtB$-?-*@WY0*0CTc$`D__4K> z#6H-h<7;=8G4U+iud6w@b*Fh%bV+QUXIk5n`PRiZ<`m^3x|Zlm7&~~&bSt#F-E*nN zVO(^TC4G=ab^jjud4A!5A9OjspuS^%Up#<+MZWN5KGo8D9vhV~hLp9gQxk(O?s@U~ zRze^Brd~Eii=*n<%9Zi;Q5iROS5tV-P)^SFeBwAmByGD!F^Y`-r(WNa;O!w8Xj`)W0Jl{twJp4C_%=lIsve|SSX0DG;_Pj<x zle*J%2vHZXzE-B$S01SVK!?@5;F}>$dfI;#z}aZSxnCc`91Sq)_q0+UPmV=cN8kL_EopD0dD5m3e-)2T033GNxl0du`nmx$MIDc&dbZ zKy7jwHK|qBniaw3AyOpi%JCpQJNdH6$ODpFIG z8}}aOSu`5Gsh7g9JnC$E6HF&1$#i%}Y+z%CCu`YN-7C9?sU0eOphM%>7N*WpF83C3 z!j?EZMn_3?xaD-H1Z^jg#y}8Z!l>A{L{1p-10y4Xtn;og=liq4y5kdX)UzJuls7|{ z2Ka{e>aJq2PyHtqHS1V*XW|F~nekllmpcknjw{LYB-p#HD^ApTNTGXquZXG^<3sCp z_yxx#s}^X`QHA?V3&~uI0+dH-AB^FSdD@A@4Vbfp-C4nU7G^Fr>pvHO<8Zc!>#*PY z*^n@r%5oZ)x@K06I~}RxCd}e&B!|?X{7~+gqhxEpl9c*6VM4^_q2^nHc9xaCUdT6|76~?tt+UVl z@nU5$=1T80jVnrHwuwBERPqFExOk|Gj}sT}mzK<=+D^r6hr}l##29?z89|?R>Pn4a zOj>qH9xL@(UPIKVnhmZQY<^xxxBKnURTEd<$j(|yj^>ACjsj35r#X3s97ByK9XBC6 z4&jyl`<1E2II8eRZBNePv>z@~ixC|LiPAmmjJfO(Pyyz+LB&!8bu>w2{p6s%9i|XE zl?ml&e&LH;lUfnP-tc0kZ8JLFLfnFfMd4xz>65A@v;rvKuy3H>48M9W*EoC;U16^! zW1A&DVG;^T{YrerxR2>}_VMxywZoQ#{8FF#-KfK8=ZV?xhR^MvSGm``=zRKZFmteB zVy)2pX(Q`1ed3JXA)HYS%owf)|FXR*`7^!r@omNoc_JQGOY^6SjO+2CYzI{xsAmGN zGd(jP#MH*p$$As~wO&?4l$$^EMm(M!?d=MD`DP>N)OM}BxF)Va!YmdpOVBT>s>Ox4;1mZ0~0yVWG70uF7hfTEkf+zb+ z*j$^MiCd>W$@+_0Ril1-Zem(NLzzv&B-+54?B5O5)CVZPpaV=}zZPye~tDVNS+wjL+Wlx?*1$Po2ft`j;kX~=J<?ZhFxIS?eojh_~ z=M|!7gT^WS6#fepGLS5aItC`u)=+*L@$|h9JGXi_mltz>M!s3#!OaC7;`N7=j}7Cn z`WSCxYnN+WHCZg#$*<7t!sB<&p7{B=dVg5bCW>t$QWap2D7IdfrNVp=8T^KM82q7c zjwRHqI)$wGdT&nnWHM$-ET}5vy&DhYhChEN1hQhHW1WpSNr`=wMO;~HQ2v;|!v5sS zHzM3=CO*tn^R2iO(UYh1>)8U_#oyyswz30fPWCH|C&j61SSk{V4A|~GWTLOT&~Kc@ z39ffe9KA1jihYE`1w)Fizeec-;y!KnJ<9ejy&w5WZ!v_FBay`TuSQSb#xXa*7&!xRxW20-S`**uIL z*(~5kj5*T+o6<$;r8Sl1=wS*TLLPRGb^wAat%n`N-dV^)6a=p#1n@912Z$D~;$kZb z(o<2Vb#ZWj*jfXaV(fzKoNS!jth5$TVBfsE1Jst5lbsI)Gzm4c5Yhw?dw**XxDy3g zxwtqAac}_WkL>Q;><&;%4o(29kps-d!NtV}Xs|hZ+PfHgu-Q8^0R6!YvDW*un!PhS z+$MHY2RjaHJq}KGFvlMq{gLzqgQbOaY;5A->LLmf6y!4%7XXE5F=VAje zK*4MR#-^tHeB8$7ynKIjxgi`-rV23^5655v7?I0 z|L!R}JJ=A1=?Td=n7Y~l2^)$sq5yQAwVBX=Fp5kcFmMVAI{;L89N@V*g#ngN_`mqB z%}oD%07q9S1pf7ynR1vz%we`V1FxGCcmgw1AqxknoiQ->jsNmB2QYF$Vj@7Hi?s{H zTnzqgNxUGiSSJ zCf4?5>}C$89>2XUh_%>n47@dtzj^>TQ`pdPfd1Brmbud(y@}bFdP0Ax4ljmP|FhEn zFc{z>U`heT1;?KgmKY`g*ZW_&{(BAmHA@3Y$f6*3*c|+?bYxhj@$WwZe@rS&0P-{d zI|(2Z1Cx{#jJU!G&zyh%R?|-((34qG~t36&`zJK`u0FD2b3`jBm)n=f*fBS%&U*KPU z;RBFwf3XGlAb=e8U+ohBvSojf0qLW^*5%}Z<-7l)2OLQBZyBG!uRZ{9;Q4?}ny?Gz zXBVh3kZ}kF4osls(6aV22gW2VaM*(baNYti?`Z<(H`rS^(8AFcfSgDfS_2VIFsG!H zpp2}P3?H|ww4jW%tc)Ch?JmbBD<~($D Date: Mon, 8 Jul 2013 00:13:17 -0400 Subject: [PATCH 11/22] Fix documentation fonts --- Documentation/dsm_bind.odt | Bin 26760 -> 27043 bytes Documentation/dsm_bind.pdf | Bin 30593 -> 34300 bytes 2 files changed, 0 insertions(+), 0 deletions(-) diff --git a/Documentation/dsm_bind.odt b/Documentation/dsm_bind.odt index 793980d4cedd8fa4b244f68ac98f3055b3e54b18..66ea1f1bee22eba9943f6077e7fcec6c6ef6e182 100644 GIT binary patch delta 5297 zcmZ9Q^;6Uh+r?L4=`N*(rMnxHE|;YnTtvEEy1t+^(%rIvlyrCJigb4fA|;Xn!gJp< z&pY$ZbFN>`xz0cEnG-qnq;Td*4GbNeBa~&mm#l}*2y6x%xg;ESUd|Jhnd<+H?HjOk z`~cqZJ3o!7RZA|7Sl{gQ6>_!tmy3CqjAKIQb6cEB++mbilRpFUBdOrn~2TFzQ1w2E6TBZkD9(0(TstVS9{H*R$R z&eZ)|4fw@%&T|?klWq5K&?zra%s1+(;nIEOmEnm=N}Ag@Eg7%YxDkd9HpIUF?Iu1s zlH_!J?x09BU39Yl6G|b|0Pn$RsQOb{T)$3#hM*wTn36}CJ3PNocsgzCes#aQX_O3`zeIAS z49ar5SWpe06Xzip7^$(%pMp3W9GA`Vtg*V5s=Q7%i?z_^o*9TvT%l0tcYY-;2{EiP z`YX}_ShMtvU$--`rsR2#?P^$QlK9z7*ur(0yto9yj@g{rSE@>-aoCvOYs}gpe2gS) zOG49dge(bW)F3x(!*=g5g&1GeTqKj|R`&rG?6)@9i$`O>w$mG+KpQ-R@Tht1)`aOvR zc?qM$YG#4`cL#>TJHI>TyId+lC5}1;axktqKU^b6NN9gws{7)5v$u7xOPHvReg%78 zp3*D5D0S;bb0y%hr8P@BCZo1P-M4_VHrlICaia=&Y}ZJ&79>IGu|PRErPTDS&(gO# z#<+c1Q2Mh!9Lib}ZH&)!GK!&T<&^r1^{X@QV(!vU{2uv7Tbrz4ZUb2D`l)Kz{Bcie z2!C!pcnEp9j`3``K?uFrD%hKwEWX)z7RHoKuNY^2RRkDSb!{kNJI1pdoO3ZA7v`U2 zT2#)x?k(2)KKCDIQn&y4MxOktcfguJ!8sb;UZ_|lJIgMD+sQMFva@cvDYpr)r0UPo z(i_Z5x%OmJj^!5^fvKx|C87fp`7g3N`;d2+KdoVW+zebcILnOQ`noQm^zGR`B3|C5 zw5Kdq@jrk7hfT~zO3V!R6M?Igt-Jzp)~P!BW8J)4IKW|9;m&LQ(x1p8Cr&vZ@kI+E zwOlFZM}+6LgjVa6!ye~Tip6!J?3id5;eR@U{*?oSes-SunsBib1XO(9-j(Ji=mtI&1M>cu`inUQU%|TRwE^1_!hz0 z?fHnNo{e+6EB+WUQmTkfN*@10b&Fj1rEav00u`J475CxEN0~uegWJfy-MtzS8&Fm z__L0^te;>N!85nE#CfVUH9e+iT$!>z1|F#DD(zGcJ9jT*CGk*Kk!l2JvHAkW+NhjZ zP#OA1sBzV<_r)twv^cEI>9WwA%#Gck2!L!gUE%Zo);ig%?r^qqCSo75oHrlkdlJ}y z`k5aELb0}vBrcull?;QdllMg85E;qv*n4c2A$RD_>P|d8Ol#krlM%+NbD$O3?&8}} zB(}emN0t=CCb&+1CHZm-mxMb3t3d6KjH0sX_9N@*r4sRGXa~Jp{6O8pMu0$)Eb#XY zvu7t0%cV;E`u^*W&>h)pS--*|qB-_>uG^vfYA1RPWK?&C!+~T_shQtf&_h5mxx7ay zn4~+-FMJBT75QFdb-Rr;GZYmVHNk+3+|Xr!kj*TVt&#M94xy$h*3DARg-&eOpSON7 z3X5{u^`p%TSag`v>!176G)yd3BrQ@L}=i23mCpx5suThw5E!tFV zR6obftirUz=VC#JamcICDv=*qV!}a(FGiy=$Bx>Yt24F?$io`m+b3eH^+%#IHv%3R z&8xvetNsMaR9M3_2BJfCv2Yq4{Q78j8;3;Iua%iIFb*C`Xo8mOh!d|Dz`aG`98$#L zSUi5fQE~dMzAkfa&x|vTVm42R+S^f|TpqUcn#;z!@s&^S$A(y%d8(t7aJ3x?(RvY> z85uJ)uAWBWc2g75wRJovr((!d`w4Br1O`ss8+{U(7^F0{C;iD!nFfAzQoU5HZv%-34_r zM0T##$?e*olrvJQQM z0V|8nSOzsmQ2M+JwN#nH!M4I0Cv5B&EJIP=W(U#lFT~ib9o89Uv}0`cKI5|PVApe8 z)D#)8Tw5emVta}9(nBzX6U@0j#>r?NJn_nH>4TXfi#IblTA<=QJwoIw`DpF>c z8<`+rc6xOm!tH~dh`!aS%P#LJZZ%;=(BW88N20vWw#}OZFlzN=1l?JcNDjYk>nCx^$i3|w3D3sqWL7Zf>!GyD5L$vN3e+9H zXcjC^=h~kcoQbK)!^8EiuoH;ge<+~2>)DHxiCcYsGBwZy&^O(LX<`~P+15x=EAkh$%Z^?qjp{`9a%b60^euZlcNh&`M=!ZZC<%0E;>~%RCH`j84 z&YNz*$w==)>}mgem!^g#b|Xq&u?;L~D}h>Wg6%4xH11n|U4=5)nE4IUGbRznA38dp zvC+lYbO}Tb##EnC$tdu9@q%>rOIqeik?wLCCC#+1S#XwjWFje!7^MOe z#v7&Ij&mbIi4~l>e9s=nQ8zLefw0;zPJs@m52#RD=TIUXy_r|CxRr}Me4pC!KX`jn zCozcwN|AEpzudP^H`=53!(am_lHN=E1}`y)<+4@UHpyMS(qRQ8$c*+gQqF+%N~yfy z<^Zh;9b=Bz%E+I-lBQVf7gNX8)$;<-QOuVm1t%$KhDkrwRYZcR$9?hV90r^~y;T`K z7fbG%uJ=4YO1n~go4pC-6S!R0>1It*noQ;Zat`uvffps}svot9{mvV){NWELk$;jK z*~)+JOn)ijF zJsKiNWt~saD~2gnt6=kkTM=a6^qeOBk+{Ptl?v|+i#D^v;J3U48r@4+b9=!07!nRh zsBrXG(2>d3BkSJdL%$=QCka>z>8%yKk^dW3gQh`03vMaazwIYuYnyPXnk{<2dARPV zAh-EbS%Koa-0@gqq_935T#jz`QIPS9HlpWyg;HEiDCiVPr-aufmt{RdXcs!BJ*HUh zu_QaJO%u{$51ynLJJMnk{L*A6`{NXNJ&OrG+Q4pm{TDRjJZLCOc$(}tnc}<6?>-U7 zxOYcan9=2yYrOC-k*1nY!FG?P<`>U>H-47X^$XF%jemIU@ev)QrGbt?4#EUs|C1R( zSfy(QM*r9_IuHo=pAW*r*U1|3FVKB8)^J@>CGFy?c?^N`_nd^J?E)%8lt_Ha`ixHI z4{){*63ruNvsOt^VbiqE?x61SpkIt*mPx3^U;)OU^r~MP2PzuwjvEGw_Wcn}8^D#;Y;qR%UcS?zTZ#mSokKcGUniN@RhqZFpclZ}d zEzdK#`XcoB@TnoDH~105W*mQI0=b=C^n|27(_X`YVt9own< z_pSgrQ-5^lpLw7UaW2}IETr=L>)RHuGPb4RQX&$Y=RPN}R(LRy;J5N>Of|4u_l9UH z8wb1%hni%4b?G@58C9}b6B&Ox=<6#LjpaRR?%)>#y`)8TQ-p1lofju*eVt4YEfFD15f92>Ag zAn3`a5IL&+@O$UPp+dSWtK1ccH#=1siN5M=KJVdP|YuX{p7Az{MN@@l>J3ZSY#?N-OxLM21T(^lzvNV@U3QNd)e9t`8J~nBcPG1 z3nF>BUmUPrWSdIhy^XFZOEmjKUXe<80}>|0D>R^hrjamy{k%kiY&^w~S?U%@fzMM6 zcV!DAR|Ma>Jas5)+{)~Tg4z+OGTkZIyry34b@uvcL*GJG>pY#yZt5T@8AvT2y-IMc zr8$frHrc};YQyoFmD%Hp+MWlO!dIdLAk~jK3OzSKv6zj~8Mc|nPtSHZ1`qy;cG2Eq z*WdwESLaI+moG@09o~rZ*HHo}xs92DQYV%04&QZX!N&&f*qPG?3n|QMq^9}Se1L#4 z<2{b!q!2u1gifV)ILV_nqY&Td!?)kp8-05{WL2s;nwfbL#H>vNF?d4jh#x(PnN=wW>MTT+5S44JvEP{~pX~!QA1KQsP_5bv z7yRLqM>*)X&5mobVS-n9w9@RB*b@lWM3IAY%odC)4d4BJ3GhARRJnk=jPyWaG7!9# z9|DOy5#DNk^MohZAE5rpzbfWJHTUKMfd^0j*zyw);!w3D~i1)RKAL zPGSD`#EtS3;IBy!j1(wDRA=D(Zk%xEr+%l3o%ufLhCVH3#I1<)ooaquj7pgD^1$yw zpS4}uGjJ3xTR-fV&^x%P5;~e0m~(5DKJ>6jx^wxL>vJBfvaZzc>+D`BHa3j=>&bCQAVStcbbIg1#XpR*k!M<62p#cya<^n8k)8lRP22H`R_*Iqu zN^epeV~c1?nMX}WjgQa5?TN(3my+4zOHF)sy~lbof^9JAXxd8Wf@cVbg^kE~q?i2D zp0|%@E#?T9E~nr^68M+~#Gbqc6Q4pmD*{A%d?GTc`zK+Oe^*HYYMN*HsAb*SNx;U@ zT{Hq=0MIf?1Y8V+kr}0kDMV7!$LP^l_QyLv7PRVocQ&*|J6TW+wdxpoV8IP=tywQ4 z{qU|rL%&GP3<;Q-b$Zd<7b409AqO(npEhPN z5%Kb5;jw>ZS-zGAjmxCRQY!qV;i0+4o|W<;e;^#6Rix|SzzCWj8#)|(ogBRqZ|>X= zqM+T{gRv4y_1cJ#9UODzA_f$b$jO)$6 zFUBTKa;WwO@6DTeGXvH0Ti+#rz&d4gO^Pfh320%Y(rrzC96qH_Byq^KJvfGfnE`Z^ub-yJ zIrdsBMNLkK1EX{pZI>C_QYk2OFuQcL$O=kUpKqPgub1#?nzCsV1jT;8r-3EO+UM1X zO47vc=ju%rJhY&}ax}L2tb{er9Pw)y5iNFEo?O76#>$G`Nh&EAD(s-CyxVy6Ckaki zHK5U9OK;S}NE+M_r3Kf6vQeh$ZdJ$*4R=%g^`nO?Mz|vo!y^U0Igr>}k zO`1QZ`O_iavV6z#{MVZMHgL}TA4O!B8VLQ+`SeMcJv&)A*5r$^J?}DUu=k6bSH|!> z_^0TUj|M{51%UMJ>NXW;(Sd8q)y^Hf6pEl6{mSOqemgwm-|rL`a?!+RoBC7lfXbl~ zNf`6MTS$c&HP)mg5wpCtd^km7yAqS%e#AE0&5DW>w_G6DNR{ROVdbouB3eXD`!!4_ zh2RD*eDQmEvEMz;bD`_woqhhhT^b4SUjVa51$|I|Kv9wJhzI}xl@|a23jhEBV{dMB zWo~pXcx`N)Sle>rxDkC%RsI3RJZ!egk}TPi?5HelRh-NwmGP`I<76jKQxFM?h)I9} zKvUYM{fy-I_DgcQLFzX04xN(Al86S-IDNYNKsVoRs!}{D=e0G9WIDb~gfgkkv{^5b zzdyWx8GVxoAEe1-X^mPWTji5)m%skijm>kNs(EJ9rcx%1QfmU<1=cfuE{(1hN#o4i zO0WIg$V&NnNawazrk^w4+sc6b=e&J=7@U|TLzn1s>L3k& zYNtFn!|5?}mBTrQ$MLDt_28U})<^RzjaUs!DG%Fc_`(zQVfu^1-yq1pi0-KB? zogQ@Hp!10;R8i$dwOp%i`0gI6q+<)mWszVJxpuNHK;SsCdorpWM1~6*;m_@S<&=CJ ztrQXu#HCebv|Mx5Kmy>;ecn_WS+edfh&Yd%7`HsBOsoy!)Eph6Wbs6;*167r< zpE7-1JKLBHMXR(J3Qigyclg}pJ~E$2pbO2)X4_qIQyCImiyCBZ8I3Lg2;TA!+K?y|fBbMKybP)=bx^{oRB0+GHe@PTZnb6S?mu3S#Y02~u&k_fnJ8?l z5LJ;`BvCg-Wbid*#OQd?>oFEb(b}UBPso8nv-lAkUX5yP#Lqu}{pUli9>KWk=b!%D zgJmQ9oD@Qpy3z(5V+W1GTsj>=Sy6=52A2pA+MR=loLYga$H-rt@Rlw5gCV#bqKBx6 zT)V0TZsVcc{z0vhbFUWwm2Kt|O1nvx);&ENux3zM?IhjoE9O1TA} z)P>N22u}1+hWanVkf~fYB}R;hVWIJ0WA^&VQsz0)ArxrRJohjs&KwI7MBgtcZ*V?j z=H3Giuf;JOka0B@^yA$=4|H%KLHW)pNz?lyr{pd>k3!^sy>#2Xzl3v_C}a+z?{dQu zx%)NtzSf|%^JxM>J zyKHR!n?-V|JK+Llwfo%ajP5|!cjnsnt>q0mIA8!xPk|An2-y+S+mjonF#2lv!pqOgR;?tG8vtWmaZS5_doZhL~>; z&+{^#KG-h7nfrY)A#dN`etQ3Q`U&U#L{)^u6mG*vhQ9fFMahq%1;H}Q-kf;H^_R4PHX6Z3hoQR3 z&4AfKQsLCm6See3{lc7VtXm=1@EcaFV^o$$thOf0UpO1H{>V16=$VChkiE}*HG3H{ z2oufR$(WD(q=|tBG9Qoh74@#xHf09%Akig%sUUTV)*1B}WgWv3lcM){^CFM`X&blC zMTDzCZ0@AU$!Nl!ZSNYP64 zEl2vSgm}QlP!ZGUabNVJhLk92=m%UWsl>@6aWSQ2mk;8ApilBMOFU@?o?OZT9$(}O*K(-LiS zB8vh=$I1Nq{i^s7Wh-fQ_ntNZ5|~vieyn%V?|ynZ)Y+pnozl%ZUtG=eOg5#iRI^*> zzkmLvxXZK7il!2^Zsao!)L$DOZ&)j|bV@tXJx|L|uVVxgM zv(lnd-Kr4>*{y83(yeU5mQ~MxD=dy*@#$`{+zVr;N?_)}ZryAZ@J|C!GIzSa+DzSB z2`e_DlY`ymI^SE{u4c3LXzD9%cC)IS$y$=4y4m7tG0VN8Dsx>;J`22qbBs7m-z!cq zX}W!@r*3}g>SC*lQXg6vYEutxyVYj0sfjm*urpOvM`fK(r>oO7t7|!bpI^_QHE6eZ zRPyvD=<6Wter&_y{{C+E5k&s;@k8X7R!qI-0lX=?=6KjhGj%s%{V0ip;=E%W!pZ4}J8Xa)<`x#RsYpt4xF|m%7 zsCnkyD@pdt{C&vdTa6sP6{RdHS=QbAcg~mNGKv71f)e>U|5?;(V-zaG<8I|yVDaaa z!t4pH*vUp3h1}SkRM$~eA%V=%DqA9~AB9n58Yj0EjqYTzSm(=sY;l|2W=sCP$*%GL zCZD|y@v}n3$$pim?BDtznh~g%96g||pzHLIjhE4Ld=c_8`w>WQvl~W!{bj^nso4j$ z+uISm=ky-SIofYiFsYS(69vz_%S0~-&f(YDgwaX^=J+Csk4D zvgRniAB@qbGbC8KgmHjEiLjUCQ(mo?2vZiuNA`G-vMsEC-buTsA}Eele?RV0*7X^1 zT1t`Q=#3~Jc19nYij=2ux;3KdT47KNgWY%abGt9JQK3E-NNiS>0$##ky%oc1;l07? zHmCcYjA+?L)c4Yd@3|eIi5+C@%hQ&swelCfme+O&J9 zB7EE!DISV{jYL^NT6VIy=D6OaK;oBRjz)FCw9Xs!@+_|vm234m;+$)Adx0YZlUpM} z1)qleZIQ;K9ol14+9P);1*s8LNHmD#7@&lwAZRdP36!dmnso(>;4;CUT?2bqUDV=P zA8oNSs!D)6AugUVj+l7-X!t{&KAmi|wHp2D{Az)JAId7A@nu3xEaq>2;MGZ9hn$2- zN4G_#Xey%n%&%@CHy`KCzv?H_@}FtNU2H;LZ2M`e+Xus}Jb zTr)P0(1rM~gCIewXpz;5F`8@L)_E)!v4A+=7_>+UF6TylQ^OcSGjj>(pRP6g6w1_p zFL3angdsf7nMKwtC^wkT_v(&JB zEj5f1qv!>bA@AGW>a(&ut#M)|-w3F7IA(ig99e3P=8ar}!D~w*m8rV`M2kcn2qymxF#Z zhlLbyo-NM;FLqUNy6$^{|2OMh6a~`Fs?k<<=xOL-{Ajpc&my5HYtdm&*lKNmi4g-d z?cqkdcog*!l@MPHIHh8=K!b!X5S`g#!0Cn#8peqhp7}jXklht+Yr;GNamrvo* z_YbYmRv#4l_Q5u)@gZQBzY6Wz>r&Ux;+_ea;$)zwwK(cQJ) zS^?L*;u?5g-L4?A00MYv~jhmyHThXQw{^r>p+Sp%@$MVs+imh5qCyrKuKYL-R8#Ubv9fV&E zk2UT)A8h=cpN?-<)=vd*Gd;a0gnCz>F4jF3Zc5jfO@NM%6Sq}{DCD50oT#i6o~G!x zu?stWyPoTr>X1!_fyM`c4h%1b;i9iL@3hdLkJpDKOJUMIj`B_z(qFxNG4l`Cf5Rl@ zeckDXKXQZ52-~5f)fg{^a6Qd?vp?>FAF^&C%Wr z@By6VJAY%sd0@uxm=B}d$r5v0je-#~r$gu3>E!TTX`5WX5Y*tgsq`Dg96l+Un&S_8 z_n!#a;~d*da`Ad(-mWwq8tO_p%KQJ=%~>rj$tBwUc16*@AX>efM{aV=vu#jsoB`bY(t%ltc2Z_x?47>N3O$D!pOHSraov zGR-lGV5s0ols$x_E?S%E=RDseyWb2SGm@)C)@a7AnK2B*x-@4b$C=7w^cdy(8am|+ z8m>bmPPu8j|0O`*u=S1(u{m;Lkr7O4f$hM^D7Kw}in455JZHtly+LNS=Ol_IVFci% z)Q?fwibwFXt4LW%N2LtgTY)C1B{ROE}KsG-SSc86B-EB{o-i;4|t1)B|6X2%YW%pv?JXLk{4F^Epbjx5m z{JQ&rqKPejjju(3H`%H`nbsQsp}DPf42cC<;o*$K98_r7L1n!^0JnFpIUMD>{L}~u z7Q`CvT2C(6-U*8KPG$0`-;p&{Fxyav(^O+_iiV$Us0hvUJToJGD26_hD7kU#*xi>q zvzTund?QP0t}E`-2s0muAOaw8dP_$by12gVnY1T;HP$fj3TILBSFT<|wrb791~J3u-Autzam~iec2NC^h4N!GRUTXDVBT!));fe*v7y8D>ijXT}${ zk5N!_>9oj&!f>&ikLb_Htr;WhAu^1`K8(S90Gt737?n(xw$?=plGnb`xtyp(zU*&V zwXfH9r>9SJ%>*6t7+OGtnkF!K@ds~dFo^ZUo24Ht94O{j#~n8h_`cp)T%I3s(}pNRDT8L*lMiI`dG3PSsQtTLCR$KHE^HuRBEZ)p zf@bM>{PzXZ%ia57cs2xV-u5oSrzk1IT2&L7aSNwcA_AM~GB^PBp<}q+s`Soy-m*QACH z3m=K-rEOlBR6c;lyhpx3%mtN|+rfe{@!zvGW%-2l#d&_&vG3$df||e!<~0j!eF^;! zYK=_&{F;S1?mi;@=dgKU73JU(*6R@fjqgl703ib6%zdaTydgo zNltldJ5WrL`HmdC>~IcS5+U&&du_btN-b>O{F?m%bh)e|3Q3VvZ-cuzU}N{oWmZZE z2^?!!xO|Bz#;gPRz%M=Fk~z>#zgAC@2rbN>aTOOWI-5595&DlKxoc|uAL*7(LxPL* z9UdA^I;~j1$O%cBaXA}5vQ7_`LX3yB4$ZSY79-;D8YWfDGLR$A19>32%I^xPKdslP zoY6!@O?G|A4U$J1UYs&nHr>CazJe>bJSKg%Ih#bSUgK;SiHXOi!?eq?0m!*j=jKVP z{52rI3$2Htg!X@_36=K8G)>C{s>4MUF`JA$G_sxo2nTR(t+xW6r{={gk}E-en>Ysp z7c=bZ-Mh+BHCr$Qz_*Tce&Jtt8-y~XfqF9-jHgakjd8Yzg`qy5Rct@o+{Q!*a!VOn z!HyK!G`_}Vo1ZIj_3iWwOZ1xmKFwnp6B|f~I2W7qkSu6U)-5xFg(Dg=z-_T8FYLbp z_-ot(M(?*N$mZ;zz?(>!eNjfwVwi!R5QB1Vfa}_*J>zQFC7ceg)dB{ihA}PwB$+U_ zI9)l{1@x@cn&t9Fjk)ovJtJ<$Yn>Y@vUt}X`6iFdOZ5{BxKatLa^PwylDpl|K1n9A zbsYZ^)h2eM^^avomEZk5Hxo3#eWklpp%~)?JX)?4&2tDm-qC(6H#f^)#fIGOO`wqu zb1A(#1uKP;i11C&CEUH@aJ?EY7;u4wD03GdW*5!S91J4cbFk;AD-_^!*BGF1OBNk^ zFbU|n=?FAa;aCzaU}6t-s{i61Q`C}iZ~yZVvXJo}@$rR*E3XePw_dtPiM%4}nY7&u zSZ;mw3k3h1TS*g>HBykpS?cEug{ge&K+nc9{4mw(INadXT+htPy$j4^sU+uz{Q*ZX z#=7y7w$JWyMLPh#Zs+(xoQEkxoa^wMbWT+l$U#L_30N)*0S&UGHbk5jYzrQvO{vJ5 zRYQ*3o#viaqlsV3`z-zf$m^))J}XTCkhTv9W%$!pmJ;&dt`YWZpKo=451(SCAt!K%3%aP5U5~K%JtAI?Rnk2`p<8 zl#{NrJ8Xmh@miQM#Sw+cCX6`=L(^(o$bt?7*0m0Z$+;oq^tl1_=5Tt zNNCBS&7Vw}S->vP!w}lwb2iC6zFL!|_BzzY?Og-N)mTK{&uSSovrxL|omN`XNn^@q zV)LsQpEzRfVa6jYW=KMv8;F(SavhI1PgTj77m{H&VkXRDhBvdQv1!A4-Jt)NEpSU4 zX7(X^Zg17el=J0AHhKeNA8b(pu;dJ()g7P*UZWX5OT;S%>K}Cpr=DJYoMXxsJH=fn zICTzOY7Zi`;TtDSdr*JT#;6DS;%Xf^F&kZB22DPm`;dnrH5Se{{FHFUwR)XLY(hbJ zIMzJ1^S89DA~NOlvdz1Fs|voQOCuB5**m+Gg%n-F#0;vtXE+iUn`%A+^i?R^NHYml zDq-R0xL0KJx6<&1TGw?HqH-;*!5|2Xli}GGvjj^ zwMEI+x0jAQnj&Ht1E5U1;ll*DBd)~dQ3~P(-iem<>zRgk0%*oxOWcTkS)D>32yu7# z!e@`I)_`o!f$m4ax%*ZC*7s_jHP-leDzhjgzGL=1-Sp~6v7oTdpQ#R}#pA{fL+50L zX=_*rSEK>1JrqYZDRIG0{jcWg?^h}ucFI6c`765BKN|f+mg#wwoWgPn*w4d`5hcrU z!6c9OqKQpW{p7h1dSJmR3m9mW>Z%!pqehy(($YY>?iqtNh#Z4;J{O=yT5)#%-myT2 zzXMoBSk=opRUVT0m~0;5ZIt{eHMa#yz^dics#|DV z_=Z1K)yhQweif|~c)w>&a<&y^kVkP62uySF@3`~7Eu=r{RBFCV#UQ3uq}osYJ=m#O z?ecq*w$ky{@zq+>3(Z*#xI|1n&uR7}F}I%fE?(XcP_J~KX13l30Clq)*`}R0>Dn`> zUvqasYr~Z2y0>zjOactby|KK|Q9-Yt*iX7`LmcxvTJJsrS&CKMZ!vv1sXsD=5VaP@9R}CbK#S%Rm(!8Ee_MJF4VkZ?kjfvmHh2kzd-! z$oA`I%thh&a$wh(w|rjqf{ec|Tk||(cVQUN>!JVifjZcNA>Ak2mj9$# z-Hr9VaMA}4<6&>NA7Lx8wcVuODSax81(qL@h!edXmveHY5AnJ|=GeQSNY5UiZL8mT z?guwg+BCyADGMb6K7wd0L&l=}rV7=38*rz&NB67Ez~y<8VZ?hnT6TsJk+1i?%s7`9LC-U9X6kPY#v1b-yCjTOxoo}676d2*N-t zAI;dN6f-~6mXu}B4G3@ms2(U-K5#7 zk1--F2OQ@W9c?&7P>|UxWOj|w$fPD6k~PE1St4NoD{N_J^OW#XyPULntI0yQhRg#7 zdGwqbzg=DKAP^UyCmB=a5ewHW?_KWUc3PGi+^{@K{}X6S#L931?@7J774Hm|bzd3> zXd4l@EVXomdtSI`w*>7)12m?nkq3^Cshrm`T1Xj4*DskzlUF3nLHGzeU;$jA&z^?wE;1)CN0O} zj_ZTTz7=!vzku+M^>sV{`NLt~$-;D>%ghP{TRz?`_q?e_yx<)&74B)sSbQ7fd{D-9 zNG1=b(P^9*(FXlK4`tA=&d_0E2edZ+7)i5kJO<38Q=sN9hc(cyQ4l9*Ni z2^;+lqsG~pfxK1z?UTCG!ZM?`jFkye38EAA(ah+RK9M|Uo*zU__bl_+A%Y;_@dvGw zIzFM&5dL%}WTqgk8nU~ZtY5DZP{RGgH!ncK1vZm@j`0av~BR&5mlR#M8T!G{BD2PVVx_n zR(7yX4Tq=VH>gS{o;W8oD}Sxyung&i7<=@il=PNRGEEd!Llg}iWtu1y)9N|%=${OD zZE7N>g0~7t>I*{BaaC0APKnd%D-@2uIhtk35-n_ z@e{=(1zIj!2DhBUtu(Euvpdr5wG+scFI&}1E08_@PQ%5StnReCI@jfo!WQ2H1KugT zJm`_&mwD{{*UwUpc&MrdYNoued!NU9x5tjJ56Hm2jJ)qm;_TvNYH0gEIfa?=zjF!` z8$0L!46-Knq3b3}p_4MQep{7b7=C)Vh%37ox|k9%vHjPGhP;uLsj&+Y6Z?PZV)l05 z3P)lVIw>&cx4S(F)BiFsGyg~7=Z9es`JQNM=i*Gn#FeOo!Tvot0>c=DnJe)MgA16s zffBO^ium8E|DETQn3!Rh{^R}a{6Be+EwP;q4}^(5@syAdm?cq+=*PFG4G}gvHOK$* zh;w@UXA>?wZ#g}0I&AUA0&OKVwJ z3;Do?{n_W}WB_)Gu%)%YLc+;lswf~Qb!AeyeEtIncP=y~k(q-LSS-<)gAAB0ahv-` zVikurFneMT5B|5@;`y0q!>I+#kvJiTmsmpt17PRp|L;HABU5)#$ZmrX34FFoJ`XpI zg)|--$sy6M-a@gl%b!*`05cwZMDyY4s`I>)xhoBW&69$ zvtgIbiMDr--)0JX=K8rQPa5LMgwvPypX)pJ>#&3S70<6nZ+lQZ3+tMmDf`Qp0YKEJ zvG0@JdSvZqW3%+?Z|rK zFv~>QQ(=Bd+f$(pt@Nej&m6B(J}P-yLTmYP*op7a_t85QZ>43ie>UrxCJIyYH7ogX znu(^T!c7I7=|G_|b~v@fmRW#xZF=W#u%M;Gsoy^;$?f;VAS%W(2sQex?l7KgYSaCT zYQd(#;4Wb^p-*k*{TmP#CwhPTS7!&X0E_!#bE4Lk((KCg8qB0+1UB)~Ku&{aLb>dA z%E#eyvC_{CR^#DZqh@Lf7m$(850-S*fwa?CfkrtXzyl z%pA-_Y^4H@@@N`&(6&BFD48#6Q}O?bz*01 zZ(?a@PQ=XlpAF2}u*CxpY|UWK!o|j5&cyUja59*4a4H$jJU5`K_~j8@}~#2QJ2cPTzu^`CsQ>&~Fp-x5D~uW&bvCF>xmP^6`D2rVhR- zWS0N$W?=a~L#&BY{0v}7xNay30y+SnA)lMvr@u&>ex`}*@5^?rC&;ZOuPr_6i<6(9 zEJ%L0M)I&W-tUM6`qx`MpIIZzIXf$j`Xw7Nmut_DpgoTd*Sv4uGaq~L2Y~m4z0)~N zxUgTizwmtL91dR;2lg-#z7;MrP9sjA#hZP_fjUe$aYO=Eyid{xA1J;af50|(sNcts zap=GZA_p7W5ff4b;)da3o>?oZFX!)u0~jtRYec9h>@ueQ95 z?)960H3}otuY+?!;l$zz3ILo9-mVF*_iZ$$Ei-gJuyyk-|7*@5$st zN|Er*W(`R*!)?EOtT`}&L7)W2LJYBjcc&w6;uSd2gm#DnFg+tuJ;B9ZC3A(rDFyez z5Kvngam_?*!l9YY?{k(BgHPjX^>!)?GD!Ar67w9$XoG{}&=Te@sR7aV)C##^WJH0C zC0kR@#1N(f1`0jy=QS-l=ZM;dgvPyLH^ez(rj9Y|Rcddm!9$i0I9(zE!2N=54fseK zpGDh{AILrX%A;hohPG3}u(T3ottiSy@tm<$E02T$-h-+$Y%@Q!r*t@uASt{Pwv;e= zA;qysEsG}u6l<2OT>N2_wxN1P|4*f9(B#mf_TlM3vd_C(0)!X*J@M^;x=n z?FhWIsI|&Wd7F2#{Hp&UkWMJ|H2in5VL#*4?K)`o@6QkBG*m$Gl4Vf>?Q!Iq4V9w& zVquhnWX05Q*`xHDnuJJ-DQAkYWp6DS5m4VHD+kmVQlB!f741xD8{?S;7R>7M0v%&C zPJ|XVLV;MQ=puAPtZ;H{0A*4DoG5)DcYxPuD0aA40H06$kF8~&Ds+T7b^sAlT5fbF z^y-j5PPmdMCn(_DNKe(+O0M#bEv`uQA=Ds}8_~*!MXn_dYb_?ajb)z}-cnN_VeJrG>>-T~&_vgyhH5L(vPVByS5L~{cB$0Q(JxFgZ_o z=N*7B*bNJy%#O&4aV-hGA9%e@XgMxRSa86===UV1FguEll;h2k`l0;Wg+2(rc-ZMG zze6I)5S@O^dF+n~pE8{cmsGY>E=K8`^9{3(YBgD^QdG#A7J5a>iIeo$(HPALY66*Kc3Q-H%*If5eDF=KtncR+NgVvbcwtrJzJ+GS) z9P8f&9GxV4x?$fFPmB*nh7svEdE4Ba_L|BBe*blQ$9gYTi{2^J+LI3g?7h_8ix0bd zS3m37=DZoc!asa+Alpszb#`p$ef<(#Rm(BYfPF~3R+zrTw3N55nj<`lMWxJK zQLjjkkZ56qHi)1ku^0@?BnI_|h#N*ip-gZ*=Q-I|V~yt^1CjPcy;Ri?nKm5Ia8{|6 zL3=FKCKJ>+q}d9^fQ*@iuI}1tGHqPEDbXxoEAd$JHvjSGQwCs1#G}KiDuce!=)B$J zU(?IxA<~whH%XgfR!p-hbENcicDOWH{!XtCf!-{ve#)##BIUuKHq;P+MXZQ|oyYci zh-_`hJEa^ajEMj(`v(^+xDp6(2Lwm*BL$pQKG1(R!Yd!al{Yk|0>hPzJ0B~Upc@lz ze%PPM;E_AiEo=p&5n|+&$s%BD++aP48GV?Vz0xY9Zl>+H$sy}>j>$eJBdt!2R@K=M zs%V*_2Y$vv*J5GEX2DoL5-2J>-Aqz+k?gq*c}~>dCLHMghhhtxcxws((rM!f$pFY` zo`Nmg9B67zi`qXJCkI90U>+hP9~N37Ttw|CprJ~W4`{tv{88U3_Q_me1#$;agJ?Uo z0>Cd4n4{<;!!$RJmik#nNp+y4022w^*K<2R@B*yZb98qq>dMT|EY(a^m z=9xV5DaAnIPU?t4!3+W*RYIng5e-*Bf~al2Ox1p}o3{J>!5ez{>EMA#t9-z++ctqHsrWQzJ?xqq4=HFb_3 zd>)!Npd_dbCl~#$R;t3 zjtPTa-652ByVXA=ze<;Yq z+{5H7m}4b!lL`|+^3NgG8V<9Q0z<?naVY*^=u^UBNpZBJ_4%WTbDinR(pFrNs!8d*bW0(>Z&(3{ zmGP;3sap*IRMUwhDwf=*!S*49)n9z87HiuLw@T+vQGcCxLF(5a`cE6$M>6Td0Gswj zV)3(?a=K|u({TFtoiqB7>PFH^EFnmP4$h6Z5tg^}yOW2vv zjQnamdb>xRv#>J*{Dx%gc(}3~(rE>!We4{!AYC8;dm~);<{Ya&VC+0JEi>bspx-;h zI*s3j08&cWQU`fr`H!NSxj1@u4Co_))fFf$6IT5Jj?Rr$uhySH7Og^HbQBg-c_QI> z$UAZ2e3_LwL>mGpItO!`Vf%X?8Zn}CiX=uFFj2(u;yGZ* zD?n2KVk4Y5fdR%jL6nxqNwIqLU-5PVR|6l7u_FY$ivt`(Zc`M+-qHigA}q^4R}!4j zdj=SSLPn!mX;hO*xVbEq^CX`GDjykeF{U{1-SORn0MK6;L4@K0liO3< zRhyYTye*x~U1}N=J8A;E0$?qmDnA##atNdr3 zOP=RTcgWqaE(bwZLM3BnY_SkRu@}bPbus5(=lDgo&+giv&Z%EfKYku~H8%PI zj`i$`IJ4JzJ0B$~$f^7T!DcHtj8J|ZG!BUvqv#dr@uEOM4t$0)Tp)UTAjWb9M7g5S z_wTv4#_g|>Pof^*_RSI4@_z!JqRh3&??b%0;~0(?);!o#ROf}oMnmgIEpj`B;09M) zi-l;7e8h}MX+!75H|~Dzl7h`cVWzJG4saki2yN)ANbQ$%RaSEs#h8#vl+fgmS}6>k z*SgfcG#qFxh@;WCFQ5kffaFDIj;4>=twkn8qLwqNd8(gp-)_ur2p40W1je^3wleiI zwL5h>;@jf=DE<<}E(s>nD5IB4phbY7{7w?Mlm{~v?gO_npBIj?6j2M_#761>9Di5_ znn(~p0?*;z4`oI~1k*9$5Jq4dL)?M@&^ozSZ?#4$mQQw{{NVibxslH$GNqr<#3Bb& z&nwNBy99N_#Ln|Ie%p zef}#l{l6u$L_t|9od1Q(_Kb`S|H@s$!uB58bS#O1vbJpBWcf{>|G4yjnDoE+^dFV} z2WOf7kt#FOKk-eX|DjjT1Ubw^R5>o1fAsoo`Nyd&TrB^@KcMCM_F(;9%AA$ypJ4e8 z$jX{%CP#$(-)zhN9sIw*_#YF0`+b9PVze9~_CM(T4*~yQfd0qBTug~>{OSOv?@z$; zzp6u1-mo6HN5AbAzyLh)68S{`#z_ z`MN0)0KBR9_?@`-a8zrz%(Z$2IbODF$5?YzvQ)3N4!62pdL3vTq`MAluVAm(vsQP! znjNHT@19$|v?G76c)dJoui&oOXc%uoefeJgb!Bv+^8r*iZmq-LphMVe!1p*uIbIGg z4KG#fomT;@V=6dsR(fk=99EjnZ&tLU&+XcVS_#DZ1~_(eZti0$?7KZlHxDrmhT+d| z5pOD5dO56@cL{e}*n6)BVs7PeqtES!^;fVt`n`8xKG;{~R;3<2L}4r9X12fnOrR ztw(Vxsw+tG$3#~Fb3VisIV(Tv%!pfcr6TGarM<9PK6Spx66fWL&lFm;|ii@%aJ%U9+t<*xMHc!j-1 zwbjtR(0-z}369;k4@$GWob@I66nQ#s*(EEVqAz~P|8lXj3Hb52ccbb{_&U0TPN@4a zmq_USbnxu+J`&oHQ~GIAx<6U|QqrtTs+tXWe-O~&x%qu+UQw;>`2n9}{qf$#QQ1>> zG2GJQa9GUXSN9nOs?Q8~tz!`QN{+8#_xXt8*lGQHP+_g!Q}?uB{k77*)S|L;_w-kt z!_VU2q6*M@`JfubL9f@0{u9oKQ^4n`!4HAo*)NnTo9jys{J^lQaYx=8o8tsv zr~?-vG5xEu6!noPfdV}8IoxiJVm?7&H}yA?B5%VD0;!`AeQRWL8xRapJbHXMh_E0W zR6btZ&j>_Vn3`P58W<|}J}QYllFC6OA@<#%o#LN8r!!&G8#EwErH4b{9Z<$UJ(DRV z5@^AO;@EYL5`{BkoRF~yg3TqP4>7R-bFsT5ai8TwJ9O0GUBF8(=cQumd~5S->$df2 z8|_+EkT&opq?+=V_m}S1dj#U5w>b}F^-+bwVx5vnq}1x|a z^p~Sie@QQbS#3*(uP?_y&2 zG3-j{yqI|EAB_({sGQrL6HCq?RvnzTHWQU4l)n+BizV&}sa~o=9$6X!(x>K_!lWQJ zX8q`bOE6Y?p4lvi2NZA8e?51R`zb=`B@WbRo-&A2q+!pLdZ*!B{JH*f9i3rpHiTxg zZ{50{jiW4kpWVe%5q?id`(>T#j~H%5L^MDW9D+m`pz{m1JS7@gLQDfhKrisfgx0V0 zeUL?-fQ#wHKYyNh)Zz-{BM}W1`%WtdGql zwn2QF&qO#v3wsfR8a`xvNmH6EPnph1I8Ow^BeM~sbA$1N@t?QxcuWZyQZrA}Wh4C* zFL>^8_y!QGfasbT`9sJ@7|eWAseDn~dLs?R$Ryybx+xAh)?c>r{6wq`Ro!mtJWET) z=VyPMwD>Lw8fxHw&mDJX8N(mI-#4@HG7ZB+{!I>$$icamCQEjq#Kl2s@DG&uI)2rK zVBJcz9%n6i5+y{B`ehK(v|$#+edWl-i-0J}gOxz637{xEu9jI)oX9)^KchGWONWmB zIXnt$lu3 z%)k>x1Dpu?#Uhivax$^H^YFg2=Q3II{wbayhxE<5k1d24@uZ!Y+nE1Qc5i1jS? zp8N_C{@j5-1wK=hZYpZ`w6|5(JwUiGXs&7w>&%!VXjI=M+$OYv!&i_&mLM&H-Co2e z=@il*tYL;p6vQsvR~rfoW^AN`7F33ULSeW?2GE~LbE2vYbDa`79hRaxO*5VIOv^hP z2Oyi0B`LF%$uR#0ewmRPuz-p27PEo0U7H40PPe$UzG4|ZKNxOhvXr@yXD>W|OGiu( zmEoEwwO!}`!ylS0MnQV-+fep37E1Of>D*b&HF&|`x}@l^WdZ8z8c*M5Wzk&g@h1$qz?a56)l(9*j8MS63v9%qP* zv%;L7miPNT>JFHZUZMrheDh)4G6pEUgYJN-oo5Hr;JO1T-Z0rTLJ#9s0T|=EU z)`(3bbwS&a=?YP^B)<>Y+xRTvc>n`XaeOjk*^GP1r%JOU>gA)JcP_>rGdzK|<{}()bnJ zAMaq_Dv+XBAD<}<7a}%r(8pv+!bqHHD(`tbYRnf_$)$WH*v_wox3>azHc2y-_Yg$rEnf@8OlpFSszxQ{JZSoVKv^9 zL?67(`??>}&r~q0GOGwH@^-c{_USLBz|jgfj1Hd-qA_Gv9q5u&OdUiK-BSFI%C&68 zl}7VO{!lrPIU2vp&7pSF9AWWy$x)%qN#xujA)*o`A|RDyL?{5W{GmX){h~e@+x0>9 z52bs(D=iYXq2bOhyd>~Ad>}}HMQ-@H2Pk@Y%qba;7*8l!A%QvuN3TYzdeXJ|6;>;N z@Tn>A-ytlYV2~SOBc$*oxN8&xP&^AfjFBLJxTjmaK6r)mMP^COcQ0Zi!73L_NiM36 znQ2t%p4oztqAWms`>0+nR`F*F@&vKdxZk9ALe4>P@a+v&?R$U&dnU|NC`vsF)GZhk zgW94VHVER3pj{r=5q&8-;c+L?la&A2h`$`8U*y%3OUDKuktuJ?w(kSKJzWFjR!BGQ zT0*tdC0=#L8i()h*feH)wtJR{0L5%+WveC#|Mp1m^Br%$8nRWbL;VL8?|YKaB_S4`Y1ZvtbS;DdCju9 zc@04UUWHV520KD{MxRcBdV>A)2&TyC)>C3c7q*4ry$^`P-GRWu<7G?_!wX*wMMLP1=q{cNJk zGVf8iC$SN6bz)nhTa%ZQuTLp(CXnHY%zQMON9sI4p(Ov96`6S)K}}`1 zK6`*eMus&(<(iFVP^1?KAEiL`g`(S!XiMedSA!EME~A_Wqb#n_WLMUTV(+&lGFB#4 z#ll=ciB@jxsUV<0P~bZ#w5ul^+nB>I!`qV3{hbt0i^msTxT8l8i`bbXNFf+OQ%oD$ z2t1M>f{_xvrNVw85sY1wKXVJ?&=Q4(WxxRY_0-Sf5w$o{l(+9quU+U^CA<2HC?-sl zV6Sml5LlpX;8g6Ppr#XogEWzC%(*2|gaeJ?A=GBRae8q^^s>0AUh;_j zq5*-?u0aZgLgj`qLbSTNH_3FbpLd(@6;kK#EJv?9X#8Xoag+1ZY5GKHP!V&cU&R1* z2@BA=57fiJ=t5Jr$s{p?>C0y%zu1+d2ehy5mxajw@uKB8mQ$2Js=%?kA}$elYU(ML zoPCCEoxIpB%`kFlk?=+*ovhe6r}*0GGKST7vdpLtrS7X-5>GsTc@9!E!sc;oj-MUh zyw7A@r`Q08NFHp@eQiG1+0w8_OL%|^_pPw`E)W(tmd~o`!-4W*a|=xNmLH~ zRTYpL7}@$YD8%$%4Gz%g7*}GY-g*!T*8V~SdSpo`{z0Pnno-2$qrBPud}``|a6B0B z0VfR=|Jn!P?jNgqa)!n>!O-)>217vONkFhI7ua3axjN%e9OwH$yN#u7Z1b^vq-qo4 zQ4r(JVT2WHW}y>x=zPJ=Kdk^tY`!r;$By?N&xTgpo{R1)Uh^k#ntb1>DR2aknUD+<4n0A*wcfv7aIV0pc_M2y+;0a z3;z96^dmr=eNJ$KZ+$;WeMA=grjXH{x(XD_chE_WJinFW?>I~?6@Kiqb^v@>@^$QQ}eO&b;oY}D!Mo;<|M zNi7L1a7iKwTQyM9??V6+@ab_FoG117MFH5w4b0Dg0BLFEqqd;6Pa8rL?`uKY3b^3E zN5EV(a4-yYC8Ng2#95;$1Ai{#-F_Xp9(0 zw>R1M)Qi{FJIv|oRZIwL;p6M#ryxYFEiQ|qMIDWln>|l9PO}1L;+)^%IIyv$5T73r zpJEJD?AN+Yl;Nt)vPXW~d(>CJO$)o4v@q^NFFW(DHR0_rRR{-&icg!l^-FyhL_!21z zXnuod-A9iV%oW@>Rf<3Vz=;q&04$;LHm&NNGUxK}J-gandG$VMY*n}G^577#3mA3J zyt)e-aQrN1)}H4Pf1bfHm!W9aKjpvl0{;jWtP$6f19YuE`&=D9)ct9DjaXP)0A&yd zP`-!rN%I+VnQ|?&YO<&%dh@;-I60s4yQE_Hjr$*ejBJYYiF*}S9V{y@2pO02x6#xm z*Bo2vh-v#i13Qa16pc0Kl;^oE@YIIX$yOsQ1-n0ANplq8IvJL2O6LwGx(Ff0$0ANNZ0x9*f+*y=xtNH%;~| zA~toAR}pq+ye88CKsBi(fJTLZ>hFw1R9+}eOrv3i)QmXkO^X!9$M=(u+WPuFZMZYI z0Z>y{1{VeOZ<9maVY4ci>>pkC_uVz{Q>xvhy&=&6EQ&gm0#7#=*#la)gdzKcv2Y@{h;Za1 zO2%izl%E6hlTNo^CQOMvJGHd)ym+>oj8!(82oC4l4Kjw47{nrIo*S5$IB;s z@j-|O*~jcn-?lKsYfe?)`4_44nMr{BC*lL-1LR|lv;EO_I=}n=L&VASWEcD0`i=a) zz)6Iq-ME09yrt%=S~En|_}Ro_P4fkvLU-_})4+pTehjs41Q)pC2jsmwd^T>F&0 zk@&9a1=KgAJ~nnK`kHH#)t=AJGn9J(Ro+p&NaW1LhSiNRUYW%T#QDxZG>!@Xq)|_N z0DPifIn_mKt)DH~R2_TwQl~N%FaQf-Ilu%5av?fPB3Ngb3D!j41|B8~Q@A?d4jYUe zXnMcSTNqTY%w7{>_t?S=KPs`Av{M%Gk>E5Oj4TawD}eOIGlO`(bCkLL>>xLV4|WBS z9ASN?tJlh?!{kf-QJjsDCT|Cyt>PjMYv$Ntk@sy-4a@`9>vW35$SIPg(cf#rd}V zc>nQf-(2z5OQn-o?|swamc@AJ{1p0;M%qQ3h~;04w=33htNplBj7#s4)h5AG3F4rY=z~->BOn# zS10=u&J1iPCCutOgifEI2i6c+GKE(g7Vf_PC=&|J8%P?XVUy$+p#9@{r9S5MV5H=@ zVFdD*_{rDe=MK4%Wi?({RSBb=p!>_Ui|>Xr=IiCOy{m7>0Z*|vV7+_6CH9v8LINlz zD3xxGl*k?mGNP6aRl7zos6EUT$YW6Zh~ZFxEvLtg>}zfB-SzhUMBx)6sb@R;%SQV1 z@A13q(yRPVPETmBMx@E4RqFp~}3h-UOs7(n1$0kq&|aL7Ft_9iqORd*8U@UC)nq){i~bm}9NE*4|_P`@T65vw$7& zoPk45DzE`L2vWI0~9CWN8u@TWao_ zL@=4K?4XVw{u68Bxvq;*4JRw~du(*)oVZ zD3~%M%lJybYcpxQ5{L(fk4b>H^j6c4t}v=Wmd1{0X^Q&5kWXky5z*uNv8rYvdeW!2 zR78}R70EAOFDHuQP%eE&>B#(A^G#U?MY&hKdk1ZfwV9_cN@hl+yS~yH`5QmXW6&I@ zEnyH|)vq9C6r8kk-Mme~!T4Eon--ItkX6H%8%eG^>c;%O*yXE{&Z|ffv@_B34(Czj z)C6wKBQ;PBUfTvTO=wsuVcRyMGgRirPOXruvQF2YGq_E0pvrhhuCZ=DkUkO@4eI1Y z{&4!?qz(UYgMOI&JgVttcEA#}dwV=}esV*~D<+6kvQODa>cv-j{y>7_w08#eH%0ul zibzsUEd;u*Xej1goTNm?28SRD@;8;8YIuf2M8pL$sUyynCLV1ZJ&8aJeo10{R_MB8 z9YvoFbDas+e2dCeQ9W1+-~K|gaw~Wncj8+&+PTHs_!N?`HlC{=aC5KO>&*x83qqQG zeQ^E0F3Am>-dCTNiBrTUt$FPx8%Vbq4Kpc8qWRwOib!ex3dHyYkgQruTD4oRXO5;i zPLXnHkROd>>@s@7wa7csY-oi=ua0tMugq}Fkq(77fCv?WzB_iDj}FXMv*7H|4r@8% zCJ=arq$V;mIWGcOXax14$I)2dv(v~Xg=W9?++m-_H*XiiI>O+ueW(}Z&d1D8pfdS2 zsOdvb%qV-Ok{`$OVCJi5r`11lQr8nyZli*C$%+-&FZw^o;AG}U2WPa`sbn8Qe{Td% zPqk;wK~PP?Rx0^3aE0ep9lHTpvfIAhm)e-Xqx$yLInpL1qHmXB*M`eo*8Zz^fxXq_ zNXAxzXu|NSz~0XddBO!+v|#J4_qQxyXwMS=)?Z;4s@o>(iRWA%4%6ixdf)BK)Dsd% z9P$!!jjWO^mE9a+sFR*VhwBi2|EDn^`2x}40Tk&-_bhjIZqiOj@jy5~#W6LyKjP@Q z5PY3EX1I@}%YtChC~mQQXQYK?!!o2RH%VjcjvCn>UC`J!NphvJzJiifK_K;ghAZG= zfeIzTpfEAHD5imi$+jqFS^sd4^VWnczlakL1+jqDJ)D|au+=YS-d}=s!Ug=JeH;Df z@+bm|4Rs+>YaZLMgQphkRPp=@yN5M1z04DtyNeoQ@OaF(;kn3Mak$ zs&iT4%Udy#&H8>JrKU+X3W*1t`0GEPxtW>JY}>^LG3Uk#+w@WVk2f*l0Vj$&*00`E zKhH_Yv2s0S1N0!n5S<{iu*`06SiRStp$m$~)St6_8eSc~1WBord~a;k+!G@G(zSL{ zqcxB!PY7$w?n~X)GV3*nF|SpyL0mj9t@pUbGqLK|C4R-fvDut*Kkb9|`m#J9mV$Oa z_>Qq*RL`$)CA6rRR#ix=lY63GcsI=$E2~&v-xp%~l^%8ihIzv>zhR?5ew3{g@kl3Y}+(pX=%|VaaepU8B!?&3dSdzlGKjdh3Ey!|aVzeM7izB8= zb#ufJ#h97`!DaGs;3TF}8?Fjp@K@r@=9+`x}7YV=glwRhg5o#+6Mu$^3N!ilqH|Ngjr4wBe|#skufS zA-{BULOJj!TQ^MrRR#f&w5~r4H%5p9Y-lR(vNYGfHZF#wlv~(Vz&7Nga6! z%7u2zK3vH8qfw>PJ|P)}t)XvvRE@`C+mY-gHY7zzl@1=hc(AtyGJh%K6Z9lpP>LRi6fu6F!UKjQ5S-q`_Uv)aM0;c4tNMLq}V2RINlxn z7*WJG7nqC?zI3un5SkZyA%D5X@P$;tm!)N;VSAO%NUhg>M(Y4 za(N;4iGkA?cCOf8eVl&7*s&RryfGZ^aNcV^vY_>Gr`!IWR+Vn$zWsBkNMm{?d|M(X z{{$dcE_v6ME}Q?RbmJtoW%t?cW~8#&#RMaTfH)1A06K=^h%FZwYd|oV0-zy#9SdQ( z*_(a9*|ny6-n4x7J`P-gL_CND(21 zRO;NcF9P+G)T-T99%h!}QZGaqdB?6fk|gm!sN+~om99N1xZ5RBC-(wj<<8Z=ZnIBo z!mVbAH4t;U>h_2>vu4_2a&NZ4w5jf)&)yBK9c*=*`d@4eT)kY>!z7pFdXh*^jm$r? z08B@3)91H@W}^sMs59=6V{dk!evMZgfAF0db;6}EUN~rQXc(c0w94SA75}+1vAu>` z%jpQnZaOYA@SVge{4CRYvcyFp45w*iwV?w>hiVGN#o7{ILZRkp+a*N$?R=Q|F^|O5 z(M8Z0dzN9SvxR@b{%g)X|K9NaOjDuNm@Dhlt6!;QpmN~nQCZ#4M6tp zYh-ICPn;6>MDr;Xy_F7s#o(^&*>wL|J#mBV$X+f0% zsCTw?I|a8E=xX<1q-TY#NM_|v>F~6>k9&N@r!^5z$jV|)c=UO{+`x;j9V=XxvSW^B za6U$#TMm16d!*2zZ<2*v<~AVOyJnm$%VN1n<+P;GruV&Do_?>WJT_}e-PH=cD_(n% zaTK{RCiw;8O4HCX%r&qAao*|(z=;6-6b{hqx^s{fX zdG(di8VzT!n<{RETB*(00?#-i0lCQhlicI{eNIokfHDr-)jHccr-Iwzqy0@0sNq7Q z=K>uubDC_!CjzPCt*IN80#)-l?K!x!!nX0u)$2=p>bucygHv{=tJ(c+AEh1Uk8nJ$ z+CzOFci~w>G@IIiAb2H}hJf;9p~_*Lq~IqY{>&FS`s2P5h?{AN4C)GD!5F%b z{7Q|abS2r4b0B>6=H6A#^>|L3T``+VMJKt>Tz0&w2Sd2G2~Ry{+UA~CNvV;FzGz50udfJoT`EiLBz6+xX)l@kMU$ard9U+eONMhjS#%Q#W} zcN|%4q%g02g_&1xfG@1x0(GCc7E!P=MT<#823nnthw1#FGFH3oMz{4y86gscKt=vy zL5KoW)u~TU&ya@gS$A}$SXk6;nJ0q})QMUbPmdvrpDq_9!RI9+mIiks=jbRjJflA3q|2Q2ztM9=*m#;~6Yo#9S zN2ep6FLEzNk5ymB_Q>G;+V$`#in-&Y5WsMPuKUlxPL|B#6gn#5DC7CENN>*r=N2_6 zXvpogNHI05arq*}awzKb@*sggJFER%Pzf`VxY2Wq)c9JUZqJBR2I`c9tmgaaIO*Z*kvZPm$5NxQlJNcK6FDpCXBBVpT4#v~HVw zJ&=&kBN40XV(6HrdUYnNZXMe%Z=T=nK@D(cysy8!8(=-QT6cwt%H>sprW^^1p$!4H z<;+%qac(F14tz^kG98xiD*$MSmCEpA`(5Mj=c5mlGl;~H%&DSTP3 z#;%lPy4dJg#Z$0DILh&H*p$Sen$`X*E19EoJ0 z^(_Ydo@m@}Ff~N>S#DJh zP&2pIo3kK?b}F%V-lH71tKP(_%j?Q7x7XVB ztnH4-NtgvaS)V+qHO4leE3(`(1KdBX3Cz1SBVhEq+QYbFz56;?8F#baF0)}OX0!bp zJIC-Rf~b~AZl5`)Hy=l&Jwq!_e21Nn(vTAn=NknL_a~|^?@JSEu^y9{6K3prPlez9 z)kJZqMzMFH@MtM3LliCCZ+{qo-75;vV!{$-ipZ|y*Y8`VS*q7&+I_b<;yQBbYShAA z5-}fMD)1`>h|#m`B^ShAlrpZpD;r)?>Dgk3G^4x(45sI~)hG`Xg%8wY-j}{$Tv#s0 zIwZAcYClvic=(m+_el|nMtV%uyIavsIN4V!TZV16_Kjd{(e6E;spG){|}y;*$gTGSER5f4}GobIeZO>SiH1yJqDW8H@i->msI-e9S&Bi|_j=P6%n;Nn)MFCoN62>`23cm~`C+~@Pb36J>Z z`&P(B4Ax%Ln;A%G8d~h?4*hsiX8PLxVR8B3q#^=x{g-+TU?J@DF;?EAe?Q36OqV_} zuhQ?NYgAid=Ys!|<8tP_Rs+lC=eavy0WC-Ac0$h1RunjXsx3F+$ai15efFG!t#SI$ z*^cmND16zwBiWmmB)IV+&O=f6*%M+1*TM^!=vYXH*lE$^fCX`x;K|qIlR80dLH&pj zCMq@al+o-U2CA^tB=+RAo^>FzaE5%LPj`whqx6y#(g zTq33I>tOWZDn_iRf_;Bz*w2_cmW0jwy{vrZ=-P=$koTst+7dkJ8C$>bMP|pE=oe(1 zS^e^Ff_tXDX{i7YdVo|awX+x@4DnXMSrWhpvV+;cAK4*9ky1#IC>V@{h=TATY6rHH z0!bl}Fo;cRwoCFqs?&AVBmqODL5S2>uJHfOW?TdRQLnd~hTuQg#bi}YKXuzph8s^Ac4U97yK36 zuH3+XB~XYI3@?}XFAM~e0O9=!{=h&`SSrSg9|eW|$pM1F;D2IZs3cyS{ofp57)ML!pSj z@xdSxcq57b^y25^17G;()*!sN{7(!FmH5lW;;;D67sq3;zhN***q?(TApCXszxSdf z{{~8cpnoHj0OO_Ye+-6!rT!cO1xw)N@BgXu@_6*v8R>y{apD%g_t+na7uR!(8@jpS z{WJcOqH~LDy1LlAasSKo0w}Jj%55eKfv8J>5FjNLRY_F|I9_-Uk$|W{)l}6XAh-$) kqAvgcS6Be??iZe3k3775J&^W5DE<$F1Nr&Y^wfd>1I-?$d zvhJSgs(Ger-l8A^FTsCF0fTz4Ac978ChCm(u7Vn`RB0Bo+bo4zP&`Mzv_A!vdf^D9 zi)0~!YUwSa_^L-!Z|Z)vl3aeg&}4&y<1__$*)L8(#?togYF$CZxMh9%{kB^`DR>t;%Eak*lvkto88Kf5tq68-b6RHVjA@5}v~*~5p0*Sbi|62kTf!udo_=d{Xm|R^;<;AU%Qp;VaKq?RDk8Zkn5_g{ zj13GHs0Kj_o$`BAI6L{CeC?;O8y&l$F9sg<&ncaXmTd*>6T21$_lmjT&`un}gE8UM zcrjRRBjEOcHbc;9-4}y|kdaKI@=;7d{sZPMF8C$akmu4K>4HC$&EpqXFh%*B_c%u|*ZKk-D?zPe9# zsZ49n4|mpCpZTXA%TMkTK?q`6RLv=99DS`fa5G?&2uaYXehyLw9J0w(N9uQrw+VKy zbV(IID{Stt=;ewJZD>aZ&nA+?>(K={CW!o*27=Vw>*n2~vsEO`j5nsgk@2B@+ikrS z>|kX=Y++kcY>km^9vyPavD*Vn^a%QzTVF1vI3knQVI%?1I z82DaUZ(|aMVSZv&Dk4I{0CvP^#;?8mKyn!|jC-?e8^Mxn&A%X+gVO-M5+w+}SokGp zH(TuVGfj9kCj^&4JFy7X{s(QArwSq~hok=FObiu~H$NdY*gKd?woQ~gf~lPjZz}J8 zz9!lYeTV?Xqq!jFJaE;G-xYNpSJU%`1lW>i(J|=QWRN`afyB~6Ef!5!XsfhM1Iqur z=Z9d=K%0q{T#>BT4Z8QjCA}eF=gKB^mh)6g{+(tQs7dY&yy?d2twIjfYC|^5|Eo!f zjib9caySePUqBY%GHABUi}n1orc4FpwB={jI2U7NP3ST`3UAQ8M#Cm@3 zt~P9%vJF9!BWbVoRgCKqDDzFcRx@b691-63ptRy-o%?je7qY(O%y8B)jUO82FNREx zQ=}ax@f7-wH8OOL+T`%ciYIpx5P;ltn7KbRG>pT?vKJpTtO)9_5XLyFfTdM$U1JXc zS4y}6ON@{WOw1?ug97M$*F<&IarKD`mRW3(uuV}`!v#cAS#Qm4fRj34Rz(YnBq5DS zdgRTj!Mp}4p1oaq@ldywqygU5(%xYr@fY>UBED#bFI8ptP!Upy=EXB3IUwqmj4(aQ zRsXZdW1{PEawF*q6WjAT!;Ic0-(FD6VBhqMu63iF7o}HnEWVuIGF=KjIJBtw=B^db zh&nCp`eprOC=)&Gacoots-x55+~5RG*&C$Q=Olffx&1FT-%6fTh{4b(744h2P0@z# z1|v>@*pf+TDEZOHjJc@X0obLEvk@E<0SxG1;>~gKmb};rR6WeKdpOpPp4+w)^iCMZ zhGgiu1z*6ctV2CXZk9d`z z6{3X^{=5m4if&xZ!8`W-zE6|Mv^6H~&LJSGwWT=O9DD(OV>Y(6)frLc+BBAvYLM;4|?v zc(pPf7iN~q0ND0O3GBqq&kStahAY3dzj|T-^@|LU1w{!rQE5K>#Bz$t9~5QAZPb;SF`pWoG>b2fH2L?h3%NS{_?cka znJKg~0BsLBD^$R@ni(GQuCgC6HHhLZb&*?Ns;(qYJ9weGDCo?gINhoX|G6awsNMSMSrWq^x zoCG8u^HBcEd!BjLM4`FsyoaDt{&i2na*CE(0La6AP8p>DkB_?kTwu2*gZ4)3oSj)_N$*Qt{#1%kLm9rwvLP(s`u=}_tIe{ z%~|xnD+ZEG$s)SO@=pJtf!~^djfPXEc>Pq$5=k5{idC|%ZDl=fBBm-cB6rS?(#F@g z2XgMlV5R31hs~IEuwT;ewvWgAD>207axb$RxcrabZe=JHktPjq(BPl^a6Xxa$-VbF zkpso7?OEDvQ%dWS3Gsc$`$~5YRvR@nf?kp0UVo(Eww(|6eqC*T66)cNbOXPgUrE?& zR)bPs6HTh~v}}7tfy*pS;vD>4rd6At=tDLzh_T~OB>p=*?S(~A5akt3AN4k4)rcOJuT;vS zY2gGqKf+0Rfm4F>P_pNpD&x5F5LA``#>;6y9j^&q5VCAmztTUM953oJe zT#u$8g&5nZJ4op51=EY&t}<*(CtN|P8V}@Dwu>?YRWZD#(c|oQ_VU{ zxjR`(ztrZZ1T59Ni1PEFdofop_S{b|vRvzYw>UkCRL zeY8ya$wagHP76ml^;Qzd4cz9sbAHkoP71Hsy`z>j#2O}B^_%y3w;=^UmqOf3Z^V_Q z=%{7)DGJ7q4DA6IG05#+KaS{4VTOIb)doy=u(~kWeDNzkR&_G4NM9rUkmmZZH8A7K zCG595dcy)UP)z8J&OqWaz&l3KI&rMC@D|cz=;*HhN{1=eHisaX0leFmMyM7^gk;_f z$T*`$ZMl7~0(|We>iO|hu^n85K2akMv6PthEYcAh@jrEAOT{U45ejMJjiJ%vt-nXt zKgOX5^Z9lZasRwN3SL68-I(p+<^C|_mgw&Cl2s)p{D7rw5;`(>FmrWtF*mkH_&0Ji zu|eP>VIld?m7Ro>o$Y@K*;xKZDj>itZ|-2}W<|or3KB=A1hm&(2sqIg4Ib$qVsInB zyQTb;?xU21z_f@8lBF42UO`)rFy-I5oYmv-vnXzCXQf40N4=3RUpy7^Td4G^PZT>T zv;%IppIokwNf->f^xfi8V7E|$(^I0jqiEz`=LLJNuMcjzz zGoKkJ!H?h61DLWjJ?`l`-9=Be^!uNRminHoe*Az^O1W71Sz=zXxkOJHbYIn0sj0)W z_gk58Ns0#B1QOxq?0D{NYk7+?5#71fzex*t=mRldtH z(9X?|?DCfH7k;_#{GOYntbtQnX4z;XfyAbcu$h0Z2{d9F*cOZ5yZ$~5u}duLa!f%{ zeYPNbLxdN$s1)H3!^)R+zx!Cbn}2OEdZG5z*Y@-keeL>qWJTI^V}b8CQ)ZudtNKYKTw4$aw(KJI2zIE6$0{FY-R?zK(_0;BkhUv(W4exC7aRq9} zs>MA$9T(*;cjZk_?8ai2 z06cTzIMwotPSwH4P^ofVL!=ab!5DSmI{|I|98lt;7M5R&f|WvygJ%>luU&7=3M<~g zLsHq@P+SwRj1TX#!Z__J)fDwCjwTJCljE+Ee_>l{+1Pil!z?Qf zavq%Fju%(2a>8(otHTninxH6tCuN%y?@dCrrD>QVl+25}ARJ6)vtCoFz%$0E0Tc|@ zDWZPn-l<9P7{9Oa&&WZ?(qYmy3vv&UG&rzvWAGuK_p3)_hKC$w_qz&h$mz&V{Afg$ z*pN>Y%3pJ&4u&wi_JI}aGFol#Nk8h*4Stv{V|h) z`{gRJJXM4qV*e+VGkg~;EEXOojnBtgaG_AiLSKHV@=&ed375Y$zIIJoF%W_MU1KOR z3rxgtq2aF#k@#Jpz)-;eOk}4n-M#PIwf_W>1b=#zYGB0N3N`y5cY{=^^q|DUX|;)* zP7GW(J7eA0zBVmClHF&6=+19Ay~7(tX8n5wfj*pncw1|n7Y>WYU_sCM3oxwOf+v?6 zbY8SnS8DV{2xUax1D8{O9s!A-~p2Lt}JXkHZ$j06lAadJr_Vu-|~4F3-DOL zu4Pay>G3G_6SHAWkEPoCxLJW`-g~>jX%sYn4N356=UXg4oxhubEw{fX*X}orp1$uv z7x&(Fx3#=2Jt|qvA!xE$$AbF!omFgn=zAO|XUXV6+gS)_izRL(CZuUqmz9yr{$rZs_;=};BlrE-z)!XMQ_r{4 z;4RVr6MQ!~?fCx(QEaULgGQ`eh-_T{3!^wd0hszAcuaB@&VOoE1ZGK3H)%CDV>fdW z*8hOoKZ%x-iH*6b8wo4-|I14`I{agJKzf+u;Jp9b9m&}K4+R_he;FYm1ZMGnhUN}# zt|V+MAapFQf5tLcrV#9`ph_%WaQ6BmtZq1PPS61nF*p~&rE*25bKjr^2=0W%mr~1!}1%dTHsH+O9A;JPhav+1e zI54pOv!(xK`v0+jVB-QgaZ`Y1I9!3=27E?#f8@K7DK+O4sSS8AB~}w@+BbS@`Y*13 zbU6`NP_%xCfKSBV2SDJM5UPN&*D2Bj)Kn|0xk|h;-dKXoLIR6i2r*I%xXVO+`OF3nbrRh z*8lyG*tkK){E{FnZXIx*|J)nE&dVz#^gmxKo*DWe(b#+uB+18&Ap8L?}$k;-}!LqKb1?wwyL3hH6wE_5?pO=8G2s}O(GEKhwTtI?n1)lP6!6`iDvahRMp{BFzMy}>h7LVrt zD3#`a#zp(UtR8OHD!0jhn0)E~s4vqdlSotRr9q}}?kmgtI6=D#b)(L3CbgriP|$+u z1mZQr=?bx=sk2G{1pJq)Q^;DC06i;j;P!Tqi3Ho0``+u|O{}yEIEdPQG2VcV4!ou9e+`-h`RTqJsm)ZG;qno+wzfIbigH{Cu{{=0! z;3OzeQ4&Ne#0-Up@BRg(C!`0wncnsWubqiIy(THx-0HUPWJzyl9d5nZsIkAcPm4Sa zMhP<3TwHJY_;z}HzIYJn0{({c{+SNkO_p2BsBc5_-S_900*|lKv7b9~Kl7yxSqS(X zcwFuz`{Vbdk;4pm?08)EQ+L95u2lM?q>+aWSO^3?XPll;XKqRF-XH+)Gn|)u>^NK$ zV&Ojv3k^WG!!r3~`#s}|X4lJcR`rFSogpZ8ww6y3)=hc%OsuVjDesG0p5ojnpCtBT zX`9yk9!+MEzJlpC;+E3|1xXt4%ViH*8 zn?v9ovBBOZu@Z0rdj}>0FS>nNUcsbmIr`ZcCRGUr+MLePO;-DTACg%{mvRU%nR4+TNi+!@xnVPP&WOy<&S4 z-$}nkDv^R}LK-;{8EZ%icV*=xBzFax43+_?zv!BR%K|BOoUR**#2M$`+~xGI2ndX$ z3)0wNr8+gL7Xj;ZC;64(KUBi*@fp{PnLmaXJbP7H%zy5^UkjDOcEtp(6Ky^fU`?^y z8$mPsL_Smqnx+w`hArOqeIr``>pjyhRMZXrwa})6h`FflSzbs*)n?YF0H%2M9r)bd z(%E6-yv)3w;u%YRv+}1|#XnPpij!pPQt2(~3nc_qKump< zm0bA6OU&lFq{!OIJ6f@Y;QgurU{KBTe@H0Ap_HymY6b8Q`%3bSX^eE0ntNDmC~VAm zqVaGsRH!M05WWk+w8DT@( zkjNhFAORIoUf*L%d27d(MUpjU@q>_KIdnW-&uAItdt7MbL;9Kf280noRw1LlU#Z%^ z91htb`lhtX>J2cz*iBtO-dZS#v3x4V|OKAo>VEY~Wj@O{Ek{!)fdl!E7A z854QRgUZdsumd{HFQ%81HH$Th9R@U?AA4<_MuA!mR#RGJMdTvy1*S$h0r>7X!Irta z!Z#?{w4j(AWi1+41ZA)oA_Us-q1c6=v6c;_vwdQLqUgFn-oc-fdO=6r^%&|7#~$H8 zYLm>usnRa(`AF;4ro;A&c7@Gg;h)6xkDy1@$4FcDl-jg$x%y?qWyH%tq3tc`@(=o@ za=?@xlAds7=teO%#tW+tov4xbj^6%I!hT3aTN%@ie7LDnmvKGL!9V|gBGXlqznH&h znrb@7<`L?rU4N6xe28)1_Q_CtkiHxBdB{Z{?@7iSI>2w+!{(<}WW9d(WU|d?2sP+r zKqmv;$f2FVU@li!YSOBaIn0$MTFBel2moixI=R8OsuYqK@@U5I+J1wHgc@NBqQ+nk z{mtQu9@XVOzB5*=zhSAEJlg~__i?-M?;>Fu4LKDQ3s|zbx2g16qMF3<31ZeGtMY8| z48hJwJdM(Ha}I-Dd##mLRcenazp2eJp!Nx^h~KrnZ2X+f6&=rc?zl*NZ8-DU2Ruj~ z^q(NF< zGOoF=5P~t2>VX#64|9mos8|=H2i|6!%Rg1O`bsCp-@tHk89r3HCoeLb6rL(tLThHI ztU{_S6q)ZB@4EJ$ehnY?G-$;pnLeA*rA!S)%4(-Ysv9HwZBu8vH}zp1O^Ea=ABT6N zZz|HlR-lB<@42_XIS>$z^?Pce9t9G47A>b2pm2@hu_%7;lKfaT&rKUF)u$k2bG= zP{$l|9=ju)T>tKD+@riR13l8G8*dwbCRYdPmOYo^M!*y|Bz&hrwbc?E^9nl zdcI05OB9zjI~D*>9ruhL?PpNeeCIzqg^V*yooXr1v0){F0PreYL1!X z6);ZqNqR=K99OiI?NPcCu^&j)y<3Zy32&tQReH31afMvn$tyJ}TPe!Ct%CN2uIj}f zOS&_$sO4bS@3o*ysnevcrqVd@cxEf9!DFIG=^qy9M6aq)S`rH!a%6c_t9qi) zRqT=~IK{$KVgOzW*9xY-jEERP9UW?h{p2b5Op{>Qr>akb@N28k(}$0rm2KBYIn8^m z5Ij{Y55B27ovh?H+>{_yWhA(dm=vNR)bN)W?q*D9RoRhKP#vAEVIG-;_-xHsxxj;_ zH+n061c)ad9l(i?R5m6P3khNgXteMCQ|IC8eyg^RqGm8V(UZVE?YWW$66K8=ESYoH4Jb%#X_Uf<8B>=e;N#cg;2 z!U^aay&TLEjKyt+&H%SIj5G{6RiWPzmKYK09L0w-Zh)q78p8`)S2VqO`hZpV6Nw_1I1)s;n4{%OLNa2NnK4Z^$WlD!D$8iHxLGg~JS4D`)#vIg zDQu*=lt8&13hJ$tkwxVIk2R{duY&@5MhK`@oS#G16yE1gmJ61E)~4FccZejB#keYw z?}+QVE=Lz$(%u3N-%kq4T=40k_$~Ldr|PvjrVQOpPn-^GzS}2VV<@E*;}n&BN*{<8Z?UqeZTW!f+O=y4 z=A%#pPof8S^4gsht2Ki@0!byp>|yL2dQ1JgIwHylT;U8G{wu-68_G^zC3qmN zsJ2L`4}#YzvtP_lx7u16n$O-YsTs2TXS}YRjiIG}MhKC9WHKvWQCk#3vHkF;pAzRg zX73!LI~2j>-{6I{X8SesirFDoKX{Mu;tqu{QHT3&C?aDq(EC-jkRk+LD8gG{tN;}B z`G7avpAj$#EtQsB36)OKZm-V7e~bMD9=3Cdx@enhg;+Oq@v`uK*D|#wXOuq?`r|T4 zSQKcNmSN70egnCvs*+>GaY8Je?@ZNLQAl&N5b(-{Z-b%L=1Rv*pfj>D4AqH_N%CAf z_sMHrk=A9OlKZSS?~GsFAD8|q)dMUmfz=wTu78b?7W_B4r)Hcto)_Z>hX;p)!?%Oh zgQSzjKAn%I7w}Kg7Qz-Ge?Q+@t;_O9tTqfwjEfsuPmKB|yUX?!yqAFom7~fj-Rk7H zEBzdNVmDMi1+H?hKj#EDcyE4Y#OqP^dY^`uH1_S?>o?i$Z-xxCc+2^P_W=8&H)QYi z!r@M(v<~Mta7>B-i%xWq6G1T}5gzCY?|wI4ga_wA+SeFZ>G$!RfY+-4IiaI;_lIIm za~Wr`#G%GMSlwQK)*A|}t7b7T+=lwHW={>(*H$EaWfS8{l$3%zKsF&68sbDYLEmG? zz&B=16k1F6fFdUSCDk&s01z#+v^*QRS`&KJ!xZ+x6mmrd@+9Rs1@#Dmw@?3Vd>IZh z6-6{Im@$?ndGPZj^Jem3TGq$cA<}8sblY(5jN1wm4*ksWPkc3gFt|8;+x@e9uog2B zbH_jX=ve_%rk7TxESV>R`D>)$%^D!1`GUZt%n$l%BadH4wm%iHV$XT0^o2tFh6gTu zGnM&JUY^1(V)jX90*5^u`ze2Wu{U;^&!ABVw)xTY%4aS@OL|wYt(ZlXpi0M5u>LRf zE*qN;_Mmqj5k2({DAbIK6*oJ&)Wpr?w|0ID5*q{srPCQm-OLkuN z|B!zQj(?t(9IXE#?A)L?1rnHlE6{4n{r(PYj3x{Zz2cD?HkYx5M`{d{*5?D75c{Zf;<$?oO% zq{d?@|6iBQ*VSp@njZBdD$M=d|9m}8_JmoU^5_RH{v51*94jt2 z>96ax?;Ndr{0+!m-?`7PB3$?A^}y*kGhTk!xbnp`==W*9Tsc|09wBGJ>NtugtO_Ic zwraM0tjfHyc!~q`54(*?COYvAggfT%ubRRpvs>NIeXZt;6b#!ho3^%Yc-I-59jfa2 zYn1#B=TYB!F9khcm;bihR<&n7{?YBeuSd@Q?8EMM`%LM+SybQ}8d~GQcNFL9rCOuV z{X?~mnQk+02d_I#4c+>E@2(3DqchF=FDua7mMua{Js;Md2jQ`v!< zXxAI;hq&5L&d?GRn(bafV>&6F$_+ePszwctL{?Vpx1Kta*IzHgH?C(hZFMtr$2?*- z-tXI){VkTBFMMA9`qc?F{Wz|FIqF)^@fY&m%6;AZ>?1UU15T8>C^mto@jsd`-7iVi zAI zgoRMz3{JcT(tO$bW?$7>d5|xQD5Olv))qzl2vUH%vIdM_Mim8iNfO!`65r=4c7FcC zuGfoH>T|+6Owdk4*Nzaq>iP9PbpTp?99NWZ!5pqd>Q`l$Oz;iqe?~(f5k?@_>yZ@8 zF?wdP&%ok^E?qh8j6I7=+N&#&`JE%jCyU6NIvFrnq=j{;y23+VOGf^?UcY^rb*1Jx z`U@uj+UQlL*{9MC`P!w6j7v(+6?8ajXR^S;HdMmKJmrc-N5~~+!_8;RUR^Bz2fdRyw(kieolClDA!p0RF2NM<~?x(G^mfybUo*Uta!quA27fZz7 zfZhH=JACO4C8n3K>kN%TN@^p9v73a2eAWYioVJ~yqP?K-VGY_Rolh21rVa4?THorN zzxjgNL$Xf{uM|9z*2k!CCtr*^RlL9jLn|{~Q*}2j9B@?_hE;1AHuW?Tqi%R00!RrY zp-ue8A?&nQS6uG7hI{>B(+%A&JuZh0yq@Li*C?f$=|~BMBa*mZkUSDt4{gA{Zgs=*Dqn1rSuA zJf=RPPzF@vL)DC-(GG}qC}kBii?6m?e(Neeb_Xa&@#E6izlYN4q}js;siv^_7ne`Y zE{Lp%t&lUrIlm~#f3XpZxhA&(dGi22$d)4ybi)25bvbX@*{RT6OV;LmO){yoN}Ma~ zottqg)uUB1UF78X-C6!v$(J$G4>Lj&Gn|?lu=uwY)u--n2>&Xsa46qQmzHVHAW%_U zIlr+ZwCTaIfN4eX7{OHa-CjfjShBBHnL*|y4Gd3&sMC=~{YEn!S3L%hIGc7x zTp;kr%f8cmq3_D0o=-bbUspl2A^ZJa)pK3Nm9l;yw7+q-b@nNAT@3fCc?>%ySNeWZ zWOt^ln`=qIL4-$6HH$>|O%j(Y59JhE&~o2(RgWo~{;>8zUTIbsk6~D97V5Xs#C}oT zm%&<>K}`g8GSCx`7ha}xO2QG~V(DrbRzyv=B4J+DlB&%%Ma=|{ezw4g+&Y!MXLyXhQhl(y!7~mNC(K98y0L~~v9ES$EJR*o z&{cw_Gm+DQ*&1zw=M+Wtazww4&hgH(mF!DD9n>3V#vF zCU|Kn^dFW$CM|a4;*SCqcHvt0L7h6o?{rUrua=pPocfy{UYBo=(w_}C!=)F;sFVFq zL?;rL=#I}@SKWWRd`(UUO!g*R4?6p|zS)1NUE5n8In7@G{0%Ta+nhA^B7%yskFe>4 z@qEj#k{`fdbfbJfJk48QY^l8wfZp|E`*WV9Kbv#zGq*uN_9uYrg31>e`;8iddV%XA z@vkC8^qs$!wS2!!k|G;Sc|EVX*w1gnK~z)$zs*S$SeeFx7$}}GGo$veRLwY|zl(v; zd-OcxyJ#kE)vn?F#L96B#b~pc3+cSIiKJbvey3i=J&pVI|0P@ETwy&P%bIAbtLmWl zAluRr;9X(eC2|EcJ3t@LJ$B~;FX(L<9V{(cS|t+`(Z=;jp>v@Vl6y%5<$XNDL$R!; z>Y;RB%uRT7!clf%a+T?Di=mAe0?}#Zb0iQ3(E{^E3J5cTHGQKCz2*vJJzWxITw2bT zgLSh1DwrDZ1fz7S@s~EhQ9mh`MI8@y?tB>a5TC|9hZ_K8*!MYKA|5iR*E#nIFZ${H zK=15<+MgM~*RH9D$-C+2As;$@tVbu?r6k+g*t~ZOOuTbo*60(4CGT4ZgB&};`O?`KOu-HwP_Y=l17l3Vjxly zFyMX12DAYh(vG?&%lqZ~x5nRuD#zSK(KQzKa5>?e=14^ak|oG2l$MICHLbfM9f_ot(xLcV=SC zZj^-F6yhN}iwf?UayRfArW`}jzvM?G2(Za$f;fSN2mZf$+B()%D7-4tV;G+{uDbf$!xT1dkVxkxf`GuyA zPS<9}j{qWGH698HRYm{F>h%pmr=iRrAhg+_%F`P?rZRRkTbBspB`%3l+rHYqA; zOh0dcoebeUP8P#5?t`UvjK7Kei-qm;4*BE9lMl(hp(4B-U#otxTY)Q-stq-;YL`SO z*N)A&?Z{j7w&ZluCAS(ogfps17KtDyIC<9mq9|megT@Qz>_?JZ3@H&oppt>7SyIW` z7LBXLl&(b@hm`V?%6TdYv2DSdgTYt{I8+k^bmC59o1+}6QcenAM?SQ0P$lz7*+gJ) zgcBi~PdVR)M7Xq&ObcgN3?ej~&cGvd;J=HR1GT_N%EdPi+)NjI7j22zwwueJnKWYw z9O`x$Hxw{DxHUZy!4ObRX(l3)-_9=y7^P8~iSCH*x-h+nIP<6qDL3+(k2b_N#Jg&N z9(k+CROzD`O21kb%?$kX3}(|?aqJt-X!;l3Q=fovM&ZVMYxj(jUk5U@zaEc#r99^H z8%5VRZ<)%HtcEvKP&e`a_PhK0 z-Ch;-TSu338|)s*1a>4aP_LNecL+|S=WIWUauG_dQwen-=Vok{Jkf*)N|O?dKaB-X zjz2B35+e#?f;yTiLIvg2px4Ybkb#SegeH$zfCE_;qa`z^`Dw*_BB8Giz{PEY^N|?Y zb~ogw?2C^k@3a2G`I|che|a*aw=LkMt+jhJa59i6kc^|4_yw9X{{U043LYmFQL!!= zDKLyzSzfRBhGrkZ-o}81<1Az>H6*qcNf8+hvX*EU^Mq<`WN*Q_E=@2J=nC#i@$G79 zcCzX)-+6i~O?lls2gqN9UX*ZNL_V!|s0X=+40txBg&4M$H~U%NtR zHv|tmw2XM~8_ENUa-f1TCO9D0!aRq4y_=N^dr8KBqF4sH5Lp2or|p02ewfTltgkNB zZ=}F%UUh%|K8M;bw^lSs@P)M84A zE6B$yMMr>V!e#@MITTPLOoE^dkXm_Vco3*@YD5I8LhfpaW?rK|)IIPxQXLYuo1X*; z6_mIyt{ucV`G00<7|g|;hPhVdwJ)&Faijl+n@A{{&QPT*l`XTwWEYnricHNttD`fi zOUtY{s2AN934lZAF7Kq&1ub@Kzqi)AH$%$Mk4R`|P{09^Ir&I9sw8Cr$46o`3_D~Z z5-^g5tbFNa>?HxY9jPfdCW>Q&^^v(|iF$oAF5XSvhWA>XqiR>036heklHX?vpk{DZ z7-`<==6X8QOBqrL;be5bk{KA5F9ZtczcoyV$;fvcm(At?W9SSiMxD zfLs3{Cd1Dr>~K~tSWJ3ud9C?ah4SS>$tJ85!oR!ze5n6)^dM%+>|}ZAzd?9w z#HAj}&(9&24HN^4n3oYvCfS7yN~2LUWe&0!IscLgZ3$ndH>_c` z9_|CiovfSTC`Pu9{3Nu=gvdWoc~qG3jNaeC@N*!L7Am#E;iQ5+OCFxrYQ2>uN%C;< zFnOe1S-)c9V@s0GHeiwD#nYZ{lak{0s;So6`2DD;LWoFCLi!d*jisRWMOJK1w8jj~ zk(a6eOz*c`9bI7{9gOObBu`6B*6c8HB7Qg!?yN!tHx;Qff$?rJ9bS*-+!}(nRI>F5e(TMzvC488*}Q$GA1-4barsCkl2(yyDi@Mx`h_N z!7!OSh5pU2n1&0JZ7;Qa+WyC?T3fVa^HO3n2Ir|1ZJ}UW3h8ax6f6w9YmzJyrx_h` z1`3}Q>=G6geUKF4*M)LB`!b52I$YItBmIQ1@x&BOqfi|^0*=LT(6oS3z6QM6}n zlbJ38r@Ud4I9UH}VD8rBEWU|+k{T5V`0FQh1Gz7?ksy6NB4`u0qwr|vgV^ac?evJF z(SG19tCh*jzL5i`9T;Wj*2Tcaeo)c(NqjZ8mXXhkB9Whlax6kWBDj>{qkbO#c!Qc)cXU)V# z3ZW3G3!g-Ji+4FW0@cmSXU-nYrj5V%-ZlsG5%m!X5pyYx1G5!kv4&y8q+p9-sOMeR zg4{KPqP}v+R$8`hMRX|$%r+N)(mqBphUuaZ+xacsW*kL~WC!5*u(dYd`CiZLM@TLQ zz0UQdLsm;}NecJRbSreJJ&gj}_(|ipM@K146CE1<_D`7$hLONvXK43n=dR$E&^pJm zx)<%In}6H`)Jtq5)}II_mLt=n@cLe_tp>+A!CktH31Y>+U+|ubO2z?~?Zo2d{`J4Y zf!sUNCBa36Th$yk3K8UKdxH-qx`XWN!t33be%c))j1(3(^{cfzbNj%Igges@MPdAI zxR*CR%pk?P%JiHW_nb$O%gqtcHo8Eb=HKrfW!N$!mY%m=70uhUrnavH;Y~Rr={cNd zx=^0)F#I|jKcWaFW#GRcusvNG^8R8N`;}#)r!jDLSAs~JGt5y9F8F)2RIAes3?jy= z!o5A8jgL>ADH)Mm#E%>}iQl&}QLh@H9_Ze#k^n={FV-E7jV7e+vJ$WqC>Ll1hYp{| z@QKx9%SB7QQF@Hdpld^{8bGg&yJZz(NdXJ3VLrdOyneWYz~rl0i`;vwkCL9&Qs#kG zPF0MB#QZ6Js}@ST$04#4-BE)Qn$(sw&AG5~0<|yAi_yVoeaHl?=nS|J1VMob%E}f% z8A!xN81qv6EXZLYF(FF*qx2G?jDx!EKPUHtRo{*tXA!nMjDf!XFXDuY6#~IYb`4ut zRqVQORmr{r*5aw3hr$PGh`~3}FTIc9b#@lTMqJ%BWw)0X z#|rn-FJ6AM!rTG?mwFn_1fJ_qp0^4C#FpW%ta{dL8fM|FGnd162YIrZsaJ zR7V(pO|mwp>)k)0H-kG-rHG&9<$=_-jg+u7T?fmTr5gsXwkUB!Z+9BP2gd7MZAfV7 z@yWF4Rw|M?qz7g{}C`Do>J?D98GK~v49 znd8f@VUlJ57fGioU-ReGVjeujh_!i@72#mbv%_Kp?sM$^JmTW_b7K+!Uu5Jv9JQZ~ zW=pp*tg|TPg1?$?uRO7wh1oxXBY=Tu=+Y|X*KEvx!-g&LmC-lGt|1uV1ax3=q%J~1 zUk1r@JoW~POUW6N(#`&|?ug!Yqxim5&OWZRxD1ohEosdFWjnPCvJM%QiIV$o6ft?Z*kcwr= z`s?2fGqrqRCMFk7M-OyGzt=&Ug`h~D)C4RNl^|~o$St!A-NAEy=ys?R8l=1QWW`hB z?~-JiX_!yq;LHrcH~VeAPwQ`_l9lhx`z*_)WEm?ra5w5k!mj1AV(6 zC|0GnUYD8I`DERf;_WqW>ep;%Pb;N4I(2 z>DLlt-nXzCb=TTMMzR)DqNe67nxcz*p!Vk)BntZzD${zVm6BU_^{mIXx~rV+_~_7I zOV})YPqv4rm%%7+J$p6&JGF=`iSEsmf$y@t$1Rdyowv2vAxU|*v*u}nWw+Z!yT@e3 zl%Fw^q7JWB3ov@=c1B0+&N;e77~zke=Eo9+D&x#$3ssG4D&kJCydV_vd2QG@0HU(H z-?XcDmfAstk#apcPat*rO(G$z>kvUw z!-2ThejbH?FT06v6=2G+qLA9DK*+@@O&F=df_yAD|4W`pqX76wWEstg87XH8YB7Cj zq)<}EqGCA-57asGhs(L%vF~<+;~&8trbb-G|7SPFzG_&t%z7dAN97$?*^gbn@5tpp zcW}RdGJw1HV_)%=n;&8l@)9c^K`{vBNGdr$4UXdERTPHqJZ0?D(d=~<_`i<&$ zG^~BQ$^A#foJ%iN=DLfT=%lB{da68`(xLl)(_ywB78B-G&dm{%OLU$Qam9RVRfed{ zv+s+~SHypM``Y5pn=H%11Al$)7w>z2Y#(P~MZw#azg9Mx({^#r`!{=CdtTG?C%=oO8 z?T~AOZN_G{L-h$s$s6Ql7Km%u?l7FuaBzy#$$-DXTmLgzuQPwI$|%+O`l-F{Qh_Ul z3s0n1u1J(oQOW+6=(73UOy6m*E^VEEjq}^9Bd>O=%`kfsEbS-F$;H&+dka zJ<*m4jhW1IQsw`otq-=825F_&>q-4#&+VG>>PJ+4ZE4NU`yrx}_K5mf=yk8keBdg} zrPk9rQS+R^&Xc#aI;u9$lw9@q?JeuCSNpHowQOo%UuKf?&wSad_MC@FVVMgUugdU# znlH9-YxKHM^9^}l|I~GN72cTASsJv&zECtO_V$wn7RIYSOnLZxLDP+|?=~b}wU@2m zqP1b_%4l6yZ4ISuzan?v3GrXcZ!>5PI&iv2$w#7HLysT$xJJSl@B7nNf4Y+iFf0Z7Snd#>D zRThlOmX>D8mPRRtx~Y~ask(-SsYbd6K%i@pn4D~GW}KL6YBt%cW+GAQ{?&N08W>uc z7*F=8HG{I+Yio$ot5)Z(M}%9F)szh_t@DAT_2emaIz$=$xNZtdN`7+1=BoOMOe_Y5 zK=<)C$w3%~P4fg&Vt^jb2NB62qGIx`CW{y&E+7*M6wFLbjZGEO6rf_J=9a*X79gOI z2Ng52G%^70P(TqgFf|9Q^F|Xh1_o$evueE=Xr~04oP`CtxrQc&CcteGDC!JN%}s&J z!O_HkTy!x@b93PK4>WaVpiLqM2K7jeH!!s{LRV;DVPuAGpn-*<8KyczOLPYU^%|jz z85*0TI1p$*5SW^pVT6LQnK6c#sWCA8!8+g}4m1!m6pYO+Fha)60JNP21mJp&3=Hed z(4)rC*un_I65t92bj^k)K-ZudXb9XfiYaDhfgTD*=4NKVEj&OlIlWb!(P(mgt9+cH zfuW<5rHiYRi Date: Tue, 9 Jul 2013 20:37:00 -0400 Subject: [PATCH 12/22] General cleanup of /dev/px4io and /dev/px4fmu - Use distinct common symbols for px4io and px4fmu device files, and use instead of hardcoded filenames - Use common symbols defining px4io bits consistently between px4fmu and px4io builds. --- .../ardrone_interface/ardrone_motor_control.c | 2 +- src/drivers/drv_gpio.h | 37 ++----------------- src/drivers/px4fmu/fmu.cpp | 2 +- src/drivers/px4io/px4io.cpp | 10 ++--- src/modules/gpio_led/gpio_led.c | 13 ++----- src/modules/px4iofirmware/protocol.h | 7 ++++ src/modules/px4iofirmware/registers.c | 8 ++-- src/systemcmds/tests/test_gpio.c | 2 +- 8 files changed, 26 insertions(+), 55 deletions(-) diff --git a/src/drivers/ardrone_interface/ardrone_motor_control.c b/src/drivers/ardrone_interface/ardrone_motor_control.c index ecd31a073d..be8968a4ea 100644 --- a/src/drivers/ardrone_interface/ardrone_motor_control.c +++ b/src/drivers/ardrone_interface/ardrone_motor_control.c @@ -109,7 +109,7 @@ int ar_multiplexing_init() { int fd; - fd = open(GPIO_DEVICE_PATH, 0); + fd = open(PX4FMU_DEVICE_PATH, 0); if (fd < 0) { warn("GPIO: open fail"); diff --git a/src/drivers/drv_gpio.h b/src/drivers/drv_gpio.h index 7e3998fca1..510983d4bd 100644 --- a/src/drivers/drv_gpio.h +++ b/src/drivers/drv_gpio.h @@ -63,43 +63,12 @@ * they also export GPIO-like things. This is always the GPIOs on the * main board. */ -# define GPIO_DEVICE_PATH "/dev/px4fmu" +# define PX4FMU_DEVICE_PATH "/dev/px4fmu" +# define PX4IO_DEVICE_PATH "/dev/px4io" #endif -#ifdef CONFIG_ARCH_BOARD_PX4IO_V1 -/* - * PX4IO GPIO numbers. - * - * XXX note that these are here for reference/future use; currently - * there is no good way to wire these up without a common STM32 GPIO - * driver, which isn't implemented yet. - */ -/* outputs */ -# define GPIO_ACC1_POWER (1<<0) /**< accessory power 1 */ -# define GPIO_ACC2_POWER (1<<1) /**< accessory power 2 */ -# define GPIO_SERVO_POWER (1<<2) /**< servo power */ -# define GPIO_RELAY1 (1<<3) /**< relay 1 */ -# define GPIO_RELAY2 (1<<4) /**< relay 2 */ -# define GPIO_LED_BLUE (1<<5) /**< blue LED */ -# define GPIO_LED_AMBER (1<<6) /**< amber/red LED */ -# define GPIO_LED_SAFETY (1<<7) /**< safety LED */ - -/* inputs */ -# define GPIO_ACC_OVERCURRENT (1<<8) /**< accessory 1/2 overcurrent detect */ -# define GPIO_SERVO_OVERCURRENT (1<<9) /**< servo overcurrent detect */ -# define GPIO_SAFETY_BUTTON (1<<10) /**< safety button pressed */ - -/** - * Default GPIO device - other devices may also support this protocol if - * they also export GPIO-like things. This is always the GPIOs on the - * main board. - */ -# define GPIO_DEVICE_PATH "/dev/px4io" - -#endif - -#ifndef GPIO_DEVICE_PATH +#ifndef PX4IO_DEVICE_PATH # error No GPIO support for this board. #endif diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp index 5147ac500d..a98b527b1b 100644 --- a/src/drivers/px4fmu/fmu.cpp +++ b/src/drivers/px4fmu/fmu.cpp @@ -166,7 +166,7 @@ PX4FMU *g_fmu; } // namespace PX4FMU::PX4FMU() : - CDev("fmuservo", "/dev/px4fmu"), + CDev("fmuservo", PX4FMU_DEVICE_PATH), _mode(MODE_NONE), _pwm_default_rate(50), _pwm_alt_rate(50), diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 1adefdea5b..08aef70692 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -329,7 +329,7 @@ PX4IO *g_dev; } PX4IO::PX4IO() : - I2C("px4io", GPIO_DEVICE_PATH, PX4_I2C_BUS_ONBOARD, PX4_I2C_OBDEV_PX4IO, 320000), + I2C("px4io", PX4IO_DEVICE_PATH, PX4_I2C_BUS_ONBOARD, PX4_I2C_OBDEV_PX4IO, 320000), _max_actuators(0), _max_controls(0), _max_rc_input(0), @@ -1507,7 +1507,7 @@ PX4IO::ioctl(file * /*filep*/, int cmd, unsigned long arg) uint32_t bits = (1 << _max_relays) - 1; /* don't touch relay1 if it's controlling RX vcc */ if (_dsm_vcc_ctl) - bits &= ~1; + bits &= ~PX4IO_RELAY1; ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_RELAYS, bits, 0); break; } @@ -1515,7 +1515,7 @@ PX4IO::ioctl(file * /*filep*/, int cmd, unsigned long arg) case GPIO_SET: arg &= ((1 << _max_relays) - 1); /* don't touch relay1 if it's controlling RX vcc */ - if (_dsm_vcc_ctl & (arg & 1)) + if (_dsm_vcc_ctl & (arg & PX4IO_RELAY1)) ret = -EINVAL; else ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_RELAYS, 0, arg); @@ -1524,7 +1524,7 @@ PX4IO::ioctl(file * /*filep*/, int cmd, unsigned long arg) case GPIO_CLEAR: arg &= ((1 << _max_relays) - 1); /* don't touch relay1 if it's controlling RX vcc */ - if (_dsm_vcc_ctl & (arg & 1)) + if (_dsm_vcc_ctl & (arg & PX4IO_RELAY1)) ret = -EINVAL; else ret = io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_RELAYS, arg, 0); @@ -1731,7 +1731,7 @@ test(void) int direction = 1; int ret; - fd = open(GPIO_DEVICE_PATH, O_WRONLY); + fd = open(PX4IO_DEVICE_PATH, O_WRONLY); if (fd < 0) err(1, "failed to open device"); diff --git a/src/modules/gpio_led/gpio_led.c b/src/modules/gpio_led/gpio_led.c index 8b4c0cb308..1aef739c7b 100644 --- a/src/modules/gpio_led/gpio_led.c +++ b/src/modules/gpio_led/gpio_led.c @@ -53,11 +53,7 @@ #include #include #include - -#define PX4IO_RELAY1 (1<<0) -#define PX4IO_RELAY2 (1<<1) -#define PX4IO_ACC1 (1<<2) -#define PX4IO_ACC2 (1<<3) +#include struct gpio_led_s { struct work_s work; @@ -186,10 +182,9 @@ void gpio_led_start(FAR void *arg) char *gpio_dev; if (priv->use_io) { - gpio_dev = "/dev/px4io"; - + gpio_dev = PX4IO_DEVICE_PATH; } else { - gpio_dev = "/dev/px4fmu"; + gpio_dev = PX4FMU_DEVICE_PATH; } /* open GPIO device */ @@ -203,6 +198,7 @@ void gpio_led_start(FAR void *arg) } /* configure GPIO pin */ + /* px4fmu only, px4io doesn't support GPIO_SET_OUTPUT and will ignore */ ioctl(priv->gpio_fd, GPIO_SET_OUTPUT, priv->pin); /* subscribe to vehicle status topic */ @@ -263,7 +259,6 @@ void gpio_led_cycle(FAR void *arg) if (led_state_new) { ioctl(priv->gpio_fd, GPIO_SET, priv->pin); - } else { ioctl(priv->gpio_fd, GPIO_CLEAR, priv->pin); } diff --git a/src/modules/px4iofirmware/protocol.h b/src/modules/px4iofirmware/protocol.h index 6ee5c28342..88d8cc87c1 100644 --- a/src/modules/px4iofirmware/protocol.h +++ b/src/modules/px4iofirmware/protocol.h @@ -157,6 +157,13 @@ #define PX4IO_P_SETUP_PWM_DEFAULTRATE 3 /* 'low' PWM frame output rate in Hz */ #define PX4IO_P_SETUP_PWM_ALTRATE 4 /* 'high' PWM frame output rate in Hz */ #define PX4IO_P_SETUP_RELAYS 5 /* bitmask of relay/switch outputs, 0 = off, 1 = on */ + +/* px4io relay bit definitions */ +#define PX4IO_RELAY1 (1<<0) +#define PX4IO_RELAY2 (1<<1) +#define PX4IO_ACC1 (1<<2) +#define PX4IO_ACC2 (1<<3) + #define PX4IO_P_SETUP_VBATT_SCALE 6 /* battery voltage correction factor (float) */ #define PX4IO_P_SETUP_DSM 7 /* DSM bind state */ enum { /* DSM bind states */ diff --git a/src/modules/px4iofirmware/registers.c b/src/modules/px4iofirmware/registers.c index 805eb7eccd..a922362b69 100644 --- a/src/modules/px4iofirmware/registers.c +++ b/src/modules/px4iofirmware/registers.c @@ -349,10 +349,10 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value) case PX4IO_P_SETUP_RELAYS: value &= PX4IO_P_SETUP_RELAYS_VALID; r_setup_relays = value; - POWER_RELAY1(value & (1 << 0) ? 1 : 0); - POWER_RELAY2(value & (1 << 1) ? 1 : 0); - POWER_ACC1(value & (1 << 2) ? 1 : 0); - POWER_ACC2(value & (1 << 3) ? 1 : 0); + POWER_RELAY1(value & PX4IO_RELAY1 ? 1 : 0); + POWER_RELAY2(value & PX4IO_RELAY2 ? 1 : 0); + POWER_ACC1(value & PX4IO_ACC1 ? 1 : 0); + POWER_ACC2(value & PX4IO_ACC2 ? 1 : 0); break; case PX4IO_P_SETUP_SET_DEBUG: diff --git a/src/systemcmds/tests/test_gpio.c b/src/systemcmds/tests/test_gpio.c index ab536d9560..62a8732702 100644 --- a/src/systemcmds/tests/test_gpio.c +++ b/src/systemcmds/tests/test_gpio.c @@ -92,7 +92,7 @@ int test_gpio(int argc, char *argv[]) int fd; int ret = 0; - fd = open(GPIO_DEVICE_PATH, 0); + fd = open(PX4IO_DEVICE_PATH, 0); if (fd < 0) { printf("GPIO: open fail\n"); From 4685871c83e5175e58da08a68b49642daf79267d Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Thu, 11 Jul 2013 15:28:56 +0400 Subject: [PATCH 13/22] Major ubx driver cleanup: few pages of code removed, send update only when full navigation solution received --- src/drivers/gps/ubx.cpp | 901 ++++++++++++++++++---------------------- src/drivers/gps/ubx.h | 61 +-- 2 files changed, 422 insertions(+), 540 deletions(-) diff --git a/src/drivers/gps/ubx.cpp b/src/drivers/gps/ubx.cpp index 762c257aaa..895d164377 100644 --- a/src/drivers/gps/ubx.cpp +++ b/src/drivers/gps/ubx.cpp @@ -3,6 +3,7 @@ * Copyright (C) 2013 PX4 Development Team. All rights reserved. * Author: Thomas Gubler * Julian Oes + * Anton Babushkin * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -56,12 +57,14 @@ #include "ubx.h" #define UBX_CONFIG_TIMEOUT 100 +#define UBX_PACKET_TIMEOUT 2 UBX::UBX(const int &fd, struct vehicle_gps_position_s *gps_position) : -_fd(fd), -_gps_position(gps_position), -_waiting_for_ack(false), -_disable_cmd_counter(0) + _fd(fd), + _gps_position(gps_position), + _configured(false), + _waiting_for_ack(false), + _disable_cmd_counter(0) { decode_init(); } @@ -73,12 +76,13 @@ UBX::~UBX() int UBX::configure(unsigned &baudrate) { - _waiting_for_ack = true; - + _configured = false; /* try different baudrates */ const unsigned baudrates_to_try[] = {9600, 38400, 19200, 57600, 115200}; - for (int baud_i = 0; baud_i < 5; baud_i++) { + int baud_i; + + for (baud_i = 0; baud_i < 5; baud_i++) { baudrate = baudrates_to_try[baud_i]; set_baudrate(_fd, baudrate); @@ -89,8 +93,8 @@ UBX::configure(unsigned &baudrate) /* Set everything else of the packet to 0, otherwise the module wont accept it */ memset(&cfg_prt_packet, 0, sizeof(cfg_prt_packet)); - _clsID_needed = UBX_CLASS_CFG; - _msgID_needed = UBX_MESSAGE_CFG_PRT; + _message_class_needed = UBX_CLASS_CFG; + _message_id_needed = UBX_MESSAGE_CFG_PRT; /* Define the package contents, don't change the baudrate */ cfg_prt_packet.clsID = UBX_CLASS_CFG; @@ -102,9 +106,9 @@ UBX::configure(unsigned &baudrate) cfg_prt_packet.inProtoMask = UBX_CFG_PRT_PAYLOAD_INPROTOMASK; cfg_prt_packet.outProtoMask = UBX_CFG_PRT_PAYLOAD_OUTPROTOMASK; - send_config_packet(_fd, (uint8_t*)&cfg_prt_packet, sizeof(cfg_prt_packet)); + send_config_packet(_fd, (uint8_t *)&cfg_prt_packet, sizeof(cfg_prt_packet)); - if (receive(UBX_CONFIG_TIMEOUT) < 0) { + if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) { /* try next baudrate */ continue; } @@ -120,100 +124,125 @@ UBX::configure(unsigned &baudrate) cfg_prt_packet.inProtoMask = UBX_CFG_PRT_PAYLOAD_INPROTOMASK; cfg_prt_packet.outProtoMask = UBX_CFG_PRT_PAYLOAD_OUTPROTOMASK; - send_config_packet(_fd, (uint8_t*)&cfg_prt_packet, sizeof(cfg_prt_packet)); - + send_config_packet(_fd, (uint8_t *)&cfg_prt_packet, sizeof(cfg_prt_packet)); + /* no ACK is expected here, but read the buffer anyway in case we actually get an ACK */ - receive(UBX_CONFIG_TIMEOUT); - + wait_for_ack(UBX_CONFIG_TIMEOUT); + if (UBX_CFG_PRT_PAYLOAD_BAUDRATE != baudrate) { set_baudrate(_fd, UBX_CFG_PRT_PAYLOAD_BAUDRATE); baudrate = UBX_CFG_PRT_PAYLOAD_BAUDRATE; } - /* send a CFT-RATE message to define update rate */ - type_gps_bin_cfg_rate_packet_t cfg_rate_packet; - memset(&cfg_rate_packet, 0, sizeof(cfg_rate_packet)); - - _clsID_needed = UBX_CLASS_CFG; - _msgID_needed = UBX_MESSAGE_CFG_RATE; - - cfg_rate_packet.clsID = UBX_CLASS_CFG; - cfg_rate_packet.msgID = UBX_MESSAGE_CFG_RATE; - cfg_rate_packet.length = UBX_CFG_RATE_LENGTH; - cfg_rate_packet.measRate = UBX_CFG_RATE_PAYLOAD_MEASINTERVAL; - cfg_rate_packet.navRate = UBX_CFG_RATE_PAYLOAD_NAVRATE; - cfg_rate_packet.timeRef = UBX_CFG_RATE_PAYLOAD_TIMEREF; - - send_config_packet(_fd, (uint8_t*)&cfg_rate_packet, sizeof(cfg_rate_packet)); - if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) { - /* try next baudrate */ - continue; - } - - /* send a NAV5 message to set the options for the internal filter */ - type_gps_bin_cfg_nav5_packet_t cfg_nav5_packet; - memset(&cfg_nav5_packet, 0, sizeof(cfg_nav5_packet)); - - _clsID_needed = UBX_CLASS_CFG; - _msgID_needed = UBX_MESSAGE_CFG_NAV5; - - cfg_nav5_packet.clsID = UBX_CLASS_CFG; - cfg_nav5_packet.msgID = UBX_MESSAGE_CFG_NAV5; - cfg_nav5_packet.length = UBX_CFG_NAV5_LENGTH; - cfg_nav5_packet.mask = UBX_CFG_NAV5_PAYLOAD_MASK; - cfg_nav5_packet.dynModel = UBX_CFG_NAV5_PAYLOAD_DYNMODEL; - cfg_nav5_packet.fixMode = UBX_CFG_NAV5_PAYLOAD_FIXMODE; - - send_config_packet(_fd, (uint8_t*)&cfg_nav5_packet, sizeof(cfg_nav5_packet)); - if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) { - /* try next baudrate */ - continue; - } - - configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_POSLLH, - UBX_CFG_MSG_PAYLOAD_RATE1_5HZ); - /* insist of receiving the ACK for this packet */ - // if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) - // continue; - configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_TIMEUTC, - UBX_CFG_MSG_PAYLOAD_RATE1_1HZ); - // /* insist of receiving the ACK for this packet */ - // if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) - // continue; - configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_SOL, - UBX_CFG_MSG_PAYLOAD_RATE1_5HZ); - // /* insist of receiving the ACK for this packet */ - // if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) - // continue; - configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_VELNED, - UBX_CFG_MSG_PAYLOAD_RATE1_5HZ); - // /* insist of receiving the ACK for this packet */ - // if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) - // continue; - // configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_DOP, - // 0); - // /* insist of receiving the ACK for this packet */ - // if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) - // continue; - configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_SVINFO, - UBX_CFG_MSG_PAYLOAD_RATE1_05HZ); - // /* insist of receiving the ACK for this packet */ - // if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) - // continue; - - _waiting_for_ack = false; - return 0; + /* at this point we have correct baudrate on both ends */ + break; } - return -1; + + if (baud_i >= 5) { + return 1; + } + + /* send a CFG-RATE message to define update rate */ + type_gps_bin_cfg_rate_packet_t cfg_rate_packet; + memset(&cfg_rate_packet, 0, sizeof(cfg_rate_packet)); + + _message_class_needed = UBX_CLASS_CFG; + _message_id_needed = UBX_MESSAGE_CFG_RATE; + + cfg_rate_packet.clsID = UBX_CLASS_CFG; + cfg_rate_packet.msgID = UBX_MESSAGE_CFG_RATE; + cfg_rate_packet.length = UBX_CFG_RATE_LENGTH; + cfg_rate_packet.measRate = UBX_CFG_RATE_PAYLOAD_MEASINTERVAL; + cfg_rate_packet.navRate = UBX_CFG_RATE_PAYLOAD_NAVRATE; + cfg_rate_packet.timeRef = UBX_CFG_RATE_PAYLOAD_TIMEREF; + + send_config_packet(_fd, (uint8_t *)&cfg_rate_packet, sizeof(cfg_rate_packet)); + + if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) { + warnx("ubx: configuration failed: RATE"); + return 1; + } + + /* send a NAV5 message to set the options for the internal filter */ + type_gps_bin_cfg_nav5_packet_t cfg_nav5_packet; + memset(&cfg_nav5_packet, 0, sizeof(cfg_nav5_packet)); + + _message_class_needed = UBX_CLASS_CFG; + _message_id_needed = UBX_MESSAGE_CFG_NAV5; + + cfg_nav5_packet.clsID = UBX_CLASS_CFG; + cfg_nav5_packet.msgID = UBX_MESSAGE_CFG_NAV5; + cfg_nav5_packet.length = UBX_CFG_NAV5_LENGTH; + cfg_nav5_packet.mask = UBX_CFG_NAV5_PAYLOAD_MASK; + cfg_nav5_packet.dynModel = UBX_CFG_NAV5_PAYLOAD_DYNMODEL; + cfg_nav5_packet.fixMode = UBX_CFG_NAV5_PAYLOAD_FIXMODE; + + send_config_packet(_fd, (uint8_t *)&cfg_nav5_packet, sizeof(cfg_nav5_packet)); + + if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) { + warnx("ubx: configuration failed: NAV5"); + return 1; + } + + /* configure message rates */ + /* the last argument is divisor for measurement rate (set by CFG RATE), i.e. 1 means 5Hz */ + configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_POSLLH, 1); + + if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) { + warnx("ubx: msg rate configuration failed: NAV POSLLH\n"); + return 1; + } + + configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_TIMEUTC, 1); + + if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) { + warnx("ubx: msg rate configuration failed: NAV TIMEUTC\n"); + return 1; + } + + configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_SOL, 1); + + if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) { + warnx("ubx: msg rate configuration failed: NAV SOL\n"); + return 1; + } + + configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_VELNED, 1); + + if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) { + warnx("ubx: msg rate configuration failed: NAV VELNED\n"); + return 1; + } + + configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_SVINFO, 10); + + if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) { + warnx("ubx: msg rate configuration failed: NAV SVINFO\n"); + return 1; + } + + _configured = true; + return 0; } int UBX::wait_for_ack(unsigned timeout) { _waiting_for_ack = true; - int ret = receive(timeout); - _waiting_for_ack = false; - return ret; + uint64_t time_started = hrt_absolute_time(); + + while (hrt_absolute_time() < time_started + timeout * 1000) { + if (receive(timeout) > 0) { + if (!_waiting_for_ack) { + return 1; + } + + } else { + return -1; // timeout or error receiving, or NAK + } + } + + return -1; // timeout } int @@ -231,20 +260,20 @@ UBX::receive(unsigned timeout) ssize_t count = 0; - bool position_updated = false; + bool handled = false; while (true) { - /* poll for new data */ - int ret = ::poll(fds, sizeof(fds) / sizeof(fds[0]), timeout); + /* poll for new data, wait for only UBX_PACKET_TIMEOUT (2ms) if something already received */ + int ret = poll(fds, sizeof(fds) / sizeof(fds[0]), handled ? UBX_PACKET_TIMEOUT : timeout); if (ret < 0) { /* something went wrong when polling */ return -1; } else if (ret == 0) { - /* Timeout */ - return -1; + /* return success after short delay after receiving a packet or timeout after long delay */ + return handled ? 1 : -1; } else if (ret > 0) { /* if we have new data from GPS, go handle it */ @@ -254,25 +283,22 @@ UBX::receive(unsigned timeout) * won't block even on a blocking device. If more bytes are * available, we'll go back to poll() again... */ - count = ::read(_fd, buf, sizeof(buf)); - /* pass received bytes to the packet decoder */ + count = read(_fd, buf, sizeof(buf)); + + /* pass received bytes to the packet decoder */ for (int i = 0; i < count; i++) { - if (parse_char(buf[i])) { + if (parse_char(buf[i]) > 0) { /* return to configure during configuration or to the gps driver during normal work * if a packet has arrived */ - if (handle_message()) - position_updated = true; + if (handle_message() > 0) + handled = true; } } } } - /* return success after receiving a packet */ - if (position_updated) - return 1; - - /* abort after timeout if no packet parsed successfully */ - if (time_started + timeout*1000 < hrt_absolute_time() ) { + /* abort after timeout if no useful packets received */ + if (time_started + timeout * 1000 < hrt_absolute_time()) { return -1; } } @@ -283,406 +309,299 @@ UBX::parse_char(uint8_t b) { switch (_decode_state) { /* First, look for sync1 */ - case UBX_DECODE_UNINIT: - if (b == UBX_SYNC1) { - _decode_state = UBX_DECODE_GOT_SYNC1; - } - break; + case UBX_DECODE_UNINIT: + if (b == UBX_SYNC1) { + _decode_state = UBX_DECODE_GOT_SYNC1; + } + + break; + /* Second, look for sync2 */ - case UBX_DECODE_GOT_SYNC1: - if (b == UBX_SYNC2) { - _decode_state = UBX_DECODE_GOT_SYNC2; - } else { - /* Second start symbol was wrong, reset state machine */ - decode_init(); - } - break; + case UBX_DECODE_GOT_SYNC1: + if (b == UBX_SYNC2) { + _decode_state = UBX_DECODE_GOT_SYNC2; + + } else { + /* Second start symbol was wrong, reset state machine */ + decode_init(); + /* don't return error, it can be just false sync1 */ + } + + break; + /* Now look for class */ - case UBX_DECODE_GOT_SYNC2: - /* everything except sync1 and sync2 needs to be added to the checksum */ + case UBX_DECODE_GOT_SYNC2: + /* everything except sync1 and sync2 needs to be added to the checksum */ + add_byte_to_checksum(b); + _message_class = b; + _decode_state = UBX_DECODE_GOT_CLASS; + break; + + case UBX_DECODE_GOT_CLASS: + add_byte_to_checksum(b); + _message_id = b; + _decode_state = UBX_DECODE_GOT_MESSAGEID; + break; + + case UBX_DECODE_GOT_MESSAGEID: + add_byte_to_checksum(b); + _payload_size = b; //this is the first length byte + _decode_state = UBX_DECODE_GOT_LENGTH1; + break; + + case UBX_DECODE_GOT_LENGTH1: + add_byte_to_checksum(b); + _payload_size += b << 8; // here comes the second byte of length + _decode_state = UBX_DECODE_GOT_LENGTH2; + break; + + case UBX_DECODE_GOT_LENGTH2: + + /* Add to checksum if not yet at checksum byte */ + if (_rx_count < _payload_size) add_byte_to_checksum(b); - /* check for known class */ - switch (b) { - case UBX_CLASS_ACK: - _decode_state = UBX_DECODE_GOT_CLASS; - _message_class = ACK; - break; - case UBX_CLASS_NAV: - _decode_state = UBX_DECODE_GOT_CLASS; - _message_class = NAV; - break; + _rx_buffer[_rx_count] = b; -// case UBX_CLASS_RXM: -// _decode_state = UBX_DECODE_GOT_CLASS; -// _message_class = RXM; -// break; + /* once the payload has arrived, we can process the information */ + if (_rx_count >= _payload_size + 1) { //+1 because of 2 checksum bytes + /* compare checksum */ + if (_rx_ck_a == _rx_buffer[_rx_count - 1] && _rx_ck_b == _rx_buffer[_rx_count]) { + decode_init(); + return 1; // message received successfully - case UBX_CLASS_CFG: - _decode_state = UBX_DECODE_GOT_CLASS; - _message_class = CFG; - break; - default: //unknown class: reset state machine - decode_init(); - break; + } else { + warnx("ubx: checksum wrong"); + decode_init(); + return -1; } - break; - case UBX_DECODE_GOT_CLASS: - { - add_byte_to_checksum(b); - switch (_message_class) { - case NAV: - switch (b) { - case UBX_MESSAGE_NAV_POSLLH: - _decode_state = UBX_DECODE_GOT_MESSAGEID; - _message_id = NAV_POSLLH; - break; - case UBX_MESSAGE_NAV_SOL: - _decode_state = UBX_DECODE_GOT_MESSAGEID; - _message_id = NAV_SOL; - break; + } else if (_rx_count < RECV_BUFFER_SIZE) { + _rx_count++; - case UBX_MESSAGE_NAV_TIMEUTC: - _decode_state = UBX_DECODE_GOT_MESSAGEID; - _message_id = NAV_TIMEUTC; - break; + } else { + warnx("ubx: buffer full"); + decode_init(); + return -1; + } -// case UBX_MESSAGE_NAV_DOP: -// _decode_state = UBX_DECODE_GOT_MESSAGEID; -// _message_id = NAV_DOP; -// break; + break; - case UBX_MESSAGE_NAV_SVINFO: - _decode_state = UBX_DECODE_GOT_MESSAGEID; - _message_id = NAV_SVINFO; - break; - - case UBX_MESSAGE_NAV_VELNED: - _decode_state = UBX_DECODE_GOT_MESSAGEID; - _message_id = NAV_VELNED; - break; - - default: //unknown class: reset state machine, should not happen - decode_init(); - break; - } - break; -// case RXM: -// switch (b) { -// case UBX_MESSAGE_RXM_SVSI: -// _decode_state = UBX_DECODE_GOT_MESSAGEID; -// _message_id = RXM_SVSI; -// break; -// -// default: //unknown class: reset state machine, should not happen -// decode_init(); -// break; -// } -// break; - - case CFG: - switch (b) { - case UBX_MESSAGE_CFG_NAV5: - _decode_state = UBX_DECODE_GOT_MESSAGEID; - _message_id = CFG_NAV5; - break; - - default: //unknown class: reset state machine, should not happen - decode_init(); - break; - } - break; - - case ACK: - switch (b) { - case UBX_MESSAGE_ACK_ACK: - _decode_state = UBX_DECODE_GOT_MESSAGEID; - _message_id = ACK_ACK; - break; - case UBX_MESSAGE_ACK_NAK: - _decode_state = UBX_DECODE_GOT_MESSAGEID; - _message_id = ACK_NAK; - break; - default: //unknown class: reset state machine, should not happen - decode_init(); - break; - } - break; - default: //should not happen because we set the class - warnx("UBX Error, we set a class that we don't know"); - decode_init(); -// config_needed = true; - break; - } - // Evaluate state machine - if the state changed, - // the state machine was reset via decode_init() - // and we want to tell the module to stop sending this message - - // disable unknown message - //warnx("disabled class %d, msg %d", (int)_message_class, (int)b); - //configure_message_rate(_message_class, b, 0); - } - break; - case UBX_DECODE_GOT_MESSAGEID: - add_byte_to_checksum(b); - _payload_size = b; //this is the first length byte - _decode_state = UBX_DECODE_GOT_LENGTH1; - break; - case UBX_DECODE_GOT_LENGTH1: - add_byte_to_checksum(b); - _payload_size += b << 8; // here comes the second byte of length - _decode_state = UBX_DECODE_GOT_LENGTH2; - break; - case UBX_DECODE_GOT_LENGTH2: - /* Add to checksum if not yet at checksum byte */ - if (_rx_count < _payload_size) - add_byte_to_checksum(b); - _rx_buffer[_rx_count] = b; - /* once the payload has arrived, we can process the information */ - if (_rx_count >= _payload_size + 1) { //+1 because of 2 checksum bytes - - /* compare checksum */ - if (_rx_ck_a == _rx_buffer[_rx_count-1] && _rx_ck_b == _rx_buffer[_rx_count]) { - return 1; - } else { - decode_init(); - return -1; - warnx("ubx: Checksum wrong"); - } - - return 1; - } else if (_rx_count < RECV_BUFFER_SIZE) { - _rx_count++; - } else { - warnx("ubx: buffer full"); - decode_init(); - return -1; - } - break; - default: - break; + default: + break; } - return 0; //XXX ? + + return 0; // message decoding in progress } + int UBX::handle_message() { int ret = 0; - switch (_message_id) { //this enum is unique for all ids --> no need to check the class - case NAV_POSLLH: { -// printf("GOT NAV_POSLLH MESSAGE\n"); - if (!_waiting_for_ack) { - gps_bin_nav_posllh_packet_t *packet = (gps_bin_nav_posllh_packet_t *) _rx_buffer; + if (_configured) { + /* handle only info messages when configured */ + switch (_message_class) { + case UBX_CLASS_NAV: + switch (_message_id) { + case UBX_MESSAGE_NAV_POSLLH: { + // printf("GOT NAV_POSLLH\n"); + gps_bin_nav_posllh_packet_t *packet = (gps_bin_nav_posllh_packet_t *) _rx_buffer; - _gps_position->lat = packet->lat; - _gps_position->lon = packet->lon; - _gps_position->alt = packet->height_msl; + _gps_position->lat = packet->lat; + _gps_position->lon = packet->lon; + _gps_position->alt = packet->height_msl; + _gps_position->eph_m = (float)packet->hAcc * 1e-3f; // from mm to m + _gps_position->epv_m = (float)packet->vAcc * 1e-3f; // from mm to m + _gps_position->timestamp_position = hrt_absolute_time(); - _gps_position->eph_m = (float)packet->hAcc * 1e-3f; // from mm to m - _gps_position->epv_m = (float)packet->vAcc * 1e-3f; // from mm to m + _rate_count_lat_lon++; - _rate_count_lat_lon++; - - /* Add timestamp to finish the report */ - _gps_position->timestamp_position = hrt_absolute_time(); - /* only return 1 when new position is available */ - ret = 1; - } - break; - } - - case NAV_SOL: { -// printf("GOT NAV_SOL MESSAGE\n"); - if (!_waiting_for_ack) { - gps_bin_nav_sol_packet_t *packet = (gps_bin_nav_sol_packet_t *) _rx_buffer; - - _gps_position->fix_type = packet->gpsFix; - _gps_position->s_variance_m_s = packet->sAcc; - _gps_position->p_variance_m = packet->pAcc; - - _gps_position->timestamp_variance = hrt_absolute_time(); - } - break; - } - -// case NAV_DOP: { -//// printf("GOT NAV_DOP MESSAGE\n"); -// gps_bin_nav_dop_packet_t *packet = (gps_bin_nav_dop_packet_t *) _rx_buffer; -// -// _gps_position->eph_m = packet->hDOP; -// _gps_position->epv = packet->vDOP; -// -// _gps_position->timestamp_posdilution = hrt_absolute_time(); -// -// _new_nav_dop = true; -// -// break; -// } - - case NAV_TIMEUTC: { -// printf("GOT NAV_TIMEUTC MESSAGE\n"); - - if (!_waiting_for_ack) { - gps_bin_nav_timeutc_packet_t *packet = (gps_bin_nav_timeutc_packet_t *) _rx_buffer; - - //convert to unix timestamp - struct tm timeinfo; - timeinfo.tm_year = packet->year - 1900; - timeinfo.tm_mon = packet->month - 1; - timeinfo.tm_mday = packet->day; - timeinfo.tm_hour = packet->hour; - timeinfo.tm_min = packet->min; - timeinfo.tm_sec = packet->sec; - - time_t epoch = mktime(&timeinfo); - - _gps_position->time_gps_usec = (uint64_t)epoch * 1000000; //TODO: test this - _gps_position->time_gps_usec += (uint64_t)(packet->time_nanoseconds * 1e-3f); - - _gps_position->timestamp_time = hrt_absolute_time(); - } - break; - } - - case NAV_SVINFO: { - // printf("GOT NAV_SVINFO MESSAGE\n"); - - if (!_waiting_for_ack) { - //this is a more complicated message: the length depends on the number of satellites. This number is extracted from the first part of the message - const int length_part1 = 8; - char _rx_buffer_part1[length_part1]; - memcpy(_rx_buffer_part1, _rx_buffer, length_part1); - gps_bin_nav_svinfo_part1_packet_t *packet_part1 = (gps_bin_nav_svinfo_part1_packet_t *) _rx_buffer_part1; - - //read checksum - const int length_part3 = 2; - char _rx_buffer_part3[length_part3]; - memcpy(_rx_buffer_part3, &(_rx_buffer[_rx_count - 1]), length_part3); - - //definitions needed to read numCh elements from the buffer: - const int length_part2 = 12; - gps_bin_nav_svinfo_part2_packet_t *packet_part2; - char _rx_buffer_part2[length_part2]; //for temporal storage - - uint8_t satellites_used = 0; - int i; - // printf("Number of Channels: %d\n", packet_part1->numCh); - for (i = 0; i < packet_part1->numCh; i++) { //for each channel - - /* Get satellite information from the buffer */ - memcpy(_rx_buffer_part2, &(_rx_buffer[length_part1 + i * length_part2]), length_part2); - packet_part2 = (gps_bin_nav_svinfo_part2_packet_t *) _rx_buffer_part2; - - /* Write satellite information to global storage */ - uint8_t sv_used = packet_part2->flags & 0x01; - - if ( sv_used ) { - // Count SVs used for NAV. - satellites_used++; - } - - // Record info for all channels, whether or not the SV is used for NAV. - _gps_position->satellite_used[i] = sv_used; - _gps_position->satellite_snr[i] = packet_part2->cno; - _gps_position->satellite_elevation[i] = (uint8_t)(packet_part2->elev); - _gps_position->satellite_azimuth[i] = (uint8_t)((float)packet_part2->azim * 255.0f / 360.0f); - _gps_position->satellite_prn[i] = packet_part2->svid; - } - - for (i = packet_part1->numCh; i < 20; i++) { //these channels are unused - /* Unused channels have to be set to zero for e.g. MAVLink */ - _gps_position->satellite_prn[i] = 0; - _gps_position->satellite_used[i] = 0; - _gps_position->satellite_snr[i] = 0; - _gps_position->satellite_elevation[i] = 0; - _gps_position->satellite_azimuth[i] = 0; - } - _gps_position->satellites_visible = satellites_used; // visible ~= used but we are interested in the used ones - - /* set timestamp if any sat info is available */ - if (packet_part1->numCh > 0) { - _gps_position->satellite_info_available = true; - } else { - _gps_position->satellite_info_available = false; - } - _gps_position->timestamp_satellites = hrt_absolute_time(); - } - - break; - } - - case NAV_VELNED: { - - if (!_waiting_for_ack) { - /* 35.15 NAV-VELNED (0x01 0x12) message (page 181 / 210 of reference manual */ - gps_bin_nav_velned_packet_t *packet = (gps_bin_nav_velned_packet_t *) _rx_buffer; - - _gps_position->vel_m_s = (float)packet->speed * 1e-2f; - _gps_position->vel_n_m_s = (float)packet->velN * 1e-2f; /* NED NORTH velocity */ - _gps_position->vel_e_m_s = (float)packet->velE * 1e-2f; /* NED EAST velocity */ - _gps_position->vel_d_m_s = (float)packet->velD * 1e-2f; /* NED DOWN velocity */ - _gps_position->cog_rad = (float)packet->heading * M_DEG_TO_RAD_F * 1e-5f; - _gps_position->c_variance_rad = (float)packet->cAcc * M_DEG_TO_RAD_F * 1e-5f; - _gps_position->vel_ned_valid = true; - _gps_position->timestamp_velocity = hrt_absolute_time(); - - _rate_count_vel++; - } - - break; - } - -// case RXM_SVSI: { -// printf("GOT RXM_SVSI MESSAGE\n"); -// const int length_part1 = 7; -// char _rx_buffer_part1[length_part1]; -// memcpy(_rx_buffer_part1, _rx_buffer, length_part1); -// gps_bin_rxm_svsi_packet_t *packet = (gps_bin_rxm_svsi_packet_t *) _rx_buffer_part1; -// -// _gps_position->satellites_visible = packet->numVis; -// _gps_position->counter++; -// _last_message_timestamps[RXM_SVSI - 1] = hrt_absolute_time(); -// -// break; -// } - case ACK_ACK: { -// printf("GOT ACK_ACK\n"); - gps_bin_ack_ack_packet_t *packet = (gps_bin_ack_ack_packet_t *) _rx_buffer; - - if (_waiting_for_ack) { - if (packet->clsID == _clsID_needed && packet->msgID == _msgID_needed) { ret = 1; + break; } - } - } - break; - case ACK_NAK: { -// printf("GOT ACK_NAK\n"); - warnx("UBX: Received: Not Acknowledged"); - /* configuration obviously not successful */ - ret = -1; + case UBX_MESSAGE_NAV_SOL: { + // printf("GOT NAV_SOL\n"); + gps_bin_nav_sol_packet_t *packet = (gps_bin_nav_sol_packet_t *) _rx_buffer; + + _gps_position->fix_type = packet->gpsFix; + _gps_position->s_variance_m_s = packet->sAcc; + _gps_position->p_variance_m = packet->pAcc; + _gps_position->timestamp_variance = hrt_absolute_time(); + + ret = 1; + break; + } + + case UBX_MESSAGE_NAV_TIMEUTC: { + // printf("GOT NAV_TIMEUTC\n"); + gps_bin_nav_timeutc_packet_t *packet = (gps_bin_nav_timeutc_packet_t *) _rx_buffer; + + /* convert to unix timestamp */ + struct tm timeinfo; + timeinfo.tm_year = packet->year - 1900; + timeinfo.tm_mon = packet->month - 1; + timeinfo.tm_mday = packet->day; + timeinfo.tm_hour = packet->hour; + timeinfo.tm_min = packet->min; + timeinfo.tm_sec = packet->sec; + time_t epoch = mktime(&timeinfo); + + _gps_position->time_gps_usec = (uint64_t)epoch * 1000000; //TODO: test this + _gps_position->time_gps_usec += (uint64_t)(packet->time_nanoseconds * 1e-3f); + _gps_position->timestamp_time = hrt_absolute_time(); + + ret = 1; + break; + } + + case UBX_MESSAGE_NAV_SVINFO: { + // printf("GOT NAV_SVINFO\n"); + // TODO avoid memcpy + const int length_part1 = 8; + char _rx_buffer_part1[length_part1]; + memcpy(_rx_buffer_part1, _rx_buffer, length_part1); + gps_bin_nav_svinfo_part1_packet_t *packet_part1 = (gps_bin_nav_svinfo_part1_packet_t *) _rx_buffer_part1; + + const int length_part2 = 12; + gps_bin_nav_svinfo_part2_packet_t *packet_part2; + char _rx_buffer_part2[length_part2]; // for temporal storage + + uint8_t satellites_used = 0; + int i; + + // printf("Number of Channels: %d\n", packet_part1->numCh); + for (i = 0; i < packet_part1->numCh; i++) { //for each channel + + /* get satellite information from the buffer */ + memcpy(_rx_buffer_part2, &(_rx_buffer[length_part1 + i * length_part2]), length_part2); + packet_part2 = (gps_bin_nav_svinfo_part2_packet_t *) _rx_buffer_part2; + + /* write satellite information to global storage */ + uint8_t sv_used = packet_part2->flags & 0x01; + + if (sv_used) { + // Count SVs used for NAV. + satellites_used++; + } + + // Record info for all channels, whether or not the SV is used for NAV. + _gps_position->satellite_used[i] = sv_used; + _gps_position->satellite_snr[i] = packet_part2->cno; + _gps_position->satellite_elevation[i] = (uint8_t)(packet_part2->elev); + _gps_position->satellite_azimuth[i] = (uint8_t)((float)packet_part2->azim * 255.0f / 360.0f); + _gps_position->satellite_prn[i] = packet_part2->svid; + } + + for (i = packet_part1->numCh; i < 20; i++) { //these channels are unused + /* Unused channels have to be set to zero for e.g. MAVLink */ + _gps_position->satellite_prn[i] = 0; + _gps_position->satellite_used[i] = 0; + _gps_position->satellite_snr[i] = 0; + _gps_position->satellite_elevation[i] = 0; + _gps_position->satellite_azimuth[i] = 0; + } + + _gps_position->satellites_visible = satellites_used; // visible ~= used but we are interested in the used ones + + /* set timestamp if any sat info is available */ + if (packet_part1->numCh > 0) { + _gps_position->satellite_info_available = true; + + } else { + _gps_position->satellite_info_available = false; + } + + _gps_position->timestamp_satellites = hrt_absolute_time(); + + ret = 1; + break; + } + + case UBX_MESSAGE_NAV_VELNED: { + // printf("GOT NAV_VELNED\n"); + gps_bin_nav_velned_packet_t *packet = (gps_bin_nav_velned_packet_t *) _rx_buffer; + + _gps_position->vel_m_s = (float)packet->speed * 1e-2f; + _gps_position->vel_n_m_s = (float)packet->velN * 1e-2f; /* NED NORTH velocity */ + _gps_position->vel_e_m_s = (float)packet->velE * 1e-2f; /* NED EAST velocity */ + _gps_position->vel_d_m_s = (float)packet->velD * 1e-2f; /* NED DOWN velocity */ + _gps_position->cog_rad = (float)packet->heading * M_DEG_TO_RAD_F * 1e-5f; + _gps_position->c_variance_rad = (float)packet->cAcc * M_DEG_TO_RAD_F * 1e-5f; + _gps_position->vel_ned_valid = true; + _gps_position->timestamp_velocity = hrt_absolute_time(); + + _rate_count_vel++; + + ret = 1; + break; + } + + default: + break; + } + + break; + + case UBX_CLASS_ACK: { + /* ignore ACK when already configured */ + ret = 1; + break; + } + + default: break; } - default: //we don't know the message - warnx("UBX: Unknown message received: %d-%d\n",_message_class,_message_id); - if (_disable_cmd_counter++ == 0) { - // Don't attempt for every message to disable, some might not be disabled */ - warnx("Disabling message 0x%02x 0x%02x", (unsigned)_message_class, (unsigned)_message_id); + if (ret == 0) { + /* message not handled */ + warnx("ubx: unknown message received: 0x%02x-0x%02x\n", (unsigned)_message_class, (unsigned)_message_id); + + if ((_disable_cmd_counter = _disable_cmd_counter++ % 10) == 0) { + /* don't attempt for every message to disable, some might not be disabled */ + warnx("ubx: disabling message 0x%02x-0x%02x", (unsigned)_message_class, (unsigned)_message_id); configure_message_rate(_message_class, _message_id, 0); } - return ret; - ret = -1; - break; } - // end if _rx_count high enough + + } else { + /* handle only ACK while configuring */ + if (_message_class == UBX_CLASS_ACK) { + switch (_message_id) { + case UBX_MESSAGE_ACK_ACK: { + // printf("GOT ACK_ACK\n"); + gps_bin_ack_ack_packet_t *packet = (gps_bin_ack_ack_packet_t *) _rx_buffer; + + if (_waiting_for_ack) { + if (packet->clsID == _message_class_needed && packet->msgID == _message_id_needed) { + _waiting_for_ack = false; + ret = 1; + } + } + + break; + } + + case UBX_MESSAGE_ACK_NAK: { + // printf("GOT ACK_NAK\n"); + warnx("ubx: not acknowledged"); + /* configuration obviously not successful */ + _waiting_for_ack = false; + ret = -1; + break; + } + + default: + break; + } + } + } + decode_init(); - return ret; //XXX? + return ret; } void @@ -692,9 +611,8 @@ UBX::decode_init(void) _rx_ck_b = 0; _rx_count = 0; _decode_state = UBX_DECODE_UNINIT; - _message_class = CLASS_UNKNOWN; - _message_id = ID_UNKNOWN; _payload_size = 0; + /* don't reset _message_class, _message_id, _rx_buffer leave it for message handler */ } void @@ -705,23 +623,24 @@ UBX::add_byte_to_checksum(uint8_t b) } void -UBX::add_checksum_to_message(uint8_t* message, const unsigned length) +UBX::add_checksum_to_message(uint8_t *message, const unsigned length) { uint8_t ck_a = 0; uint8_t ck_b = 0; unsigned i; - for (i = 0; i < length-2; i++) { + for (i = 0; i < length - 2; i++) { ck_a = ck_a + message[i]; ck_b = ck_b + ck_a; } + /* The checksum is written to the last to bytes of a message */ - message[length-2] = ck_a; - message[length-1] = ck_b; + message[length - 2] = ck_a; + message[length - 1] = ck_b; } void -UBX::add_checksum(uint8_t* message, const unsigned length, uint8_t &ck_a, uint8_t &ck_b) +UBX::add_checksum(uint8_t *message, const unsigned length, uint8_t &ck_a, uint8_t &ck_b) { for (unsigned i = 0; i < length; i++) { ck_a = ck_a + message[i]; @@ -732,11 +651,11 @@ UBX::add_checksum(uint8_t* message, const unsigned length, uint8_t &ck_a, uint8_ void UBX::configure_message_rate(uint8_t msg_class, uint8_t msg_id, uint8_t rate) { - struct ubx_cfg_msg_rate msg; - msg.msg_class = msg_class; - msg.msg_id = msg_id; - msg.rate = rate; - send_message(UBX_CLASS_CFG, UBX_MESSAGE_CFG_MSG, &msg, sizeof(msg)); + struct ubx_cfg_msg_rate msg; + msg.msg_class = msg_class; + msg.msg_id = msg_id; + msg.rate = rate; + send_message(UBX_CLASS_CFG, UBX_MESSAGE_CFG_MSG, &msg, sizeof(msg)); } void @@ -761,19 +680,19 @@ void UBX::send_message(uint8_t msg_class, uint8_t msg_id, void *msg, uint8_t size) { struct ubx_header header; - uint8_t ck_a=0, ck_b=0; + uint8_t ck_a = 0, ck_b = 0; header.sync1 = UBX_SYNC1; header.sync2 = UBX_SYNC2; header.msg_class = msg_class; header.msg_id = msg_id; header.length = size; - add_checksum((uint8_t *)&header.msg_class, sizeof(header)-2, ck_a, ck_b); + add_checksum((uint8_t *)&header.msg_class, sizeof(header) - 2, ck_a, ck_b); add_checksum((uint8_t *)msg, size, ck_a, ck_b); // Configure receive check - _clsID_needed = msg_class; - _msgID_needed = msg_id; + _message_class_needed = msg_class; + _message_id_needed = msg_id; write(_fd, (const char *)&header, sizeof(header)); write(_fd, (const char *)msg, size); diff --git a/src/drivers/gps/ubx.h b/src/drivers/gps/ubx.h index 5a433642ce..4fc2769750 100644 --- a/src/drivers/gps/ubx.h +++ b/src/drivers/gps/ubx.h @@ -3,6 +3,7 @@ * Copyright (C) 2008-2013 PX4 Development Team. All rights reserved. * Author: Thomas Gubler * Julian Oes + * Anton Babushkin * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -51,23 +52,23 @@ /* MessageIDs (the ones that are used) */ #define UBX_MESSAGE_NAV_POSLLH 0x02 -#define UBX_MESSAGE_NAV_SOL 0x06 -#define UBX_MESSAGE_NAV_TIMEUTC 0x21 //#define UBX_MESSAGE_NAV_DOP 0x04 -#define UBX_MESSAGE_NAV_SVINFO 0x30 +#define UBX_MESSAGE_NAV_SOL 0x06 #define UBX_MESSAGE_NAV_VELNED 0x12 //#define UBX_MESSAGE_RXM_SVSI 0x20 -#define UBX_MESSAGE_ACK_ACK 0x01 +#define UBX_MESSAGE_NAV_TIMEUTC 0x21 +#define UBX_MESSAGE_NAV_SVINFO 0x30 #define UBX_MESSAGE_ACK_NAK 0x00 +#define UBX_MESSAGE_ACK_ACK 0x01 #define UBX_MESSAGE_CFG_PRT 0x00 -#define UBX_MESSAGE_CFG_NAV5 0x24 #define UBX_MESSAGE_CFG_MSG 0x01 #define UBX_MESSAGE_CFG_RATE 0x08 +#define UBX_MESSAGE_CFG_NAV5 0x24 #define UBX_CFG_PRT_LENGTH 20 #define UBX_CFG_PRT_PAYLOAD_PORTID 0x01 /**< UART1 */ #define UBX_CFG_PRT_PAYLOAD_MODE 0x000008D0 /**< 0b0000100011010000: 8N1 */ -#define UBX_CFG_PRT_PAYLOAD_BAUDRATE 38400 /**< always choose 38400 as GPS baudrate */ +#define UBX_CFG_PRT_PAYLOAD_BAUDRATE 38400 /**< choose 38400 as GPS baudrate */ #define UBX_CFG_PRT_PAYLOAD_INPROTOMASK 0x01 /**< UBX in */ #define UBX_CFG_PRT_PAYLOAD_OUTPROTOMASK 0x01 /**< UBX out */ @@ -298,44 +299,6 @@ struct ubx_cfg_msg_rate { // END the structures of the binary packets // ************ -typedef enum { - UBX_CONFIG_STATE_PRT = 0, - UBX_CONFIG_STATE_PRT_NEW_BAUDRATE, - UBX_CONFIG_STATE_RATE, - UBX_CONFIG_STATE_NAV5, - UBX_CONFIG_STATE_MSG_NAV_POSLLH, - UBX_CONFIG_STATE_MSG_NAV_TIMEUTC, - UBX_CONFIG_STATE_MSG_NAV_DOP, - UBX_CONFIG_STATE_MSG_NAV_SVINFO, - UBX_CONFIG_STATE_MSG_NAV_SOL, - UBX_CONFIG_STATE_MSG_NAV_VELNED, -// UBX_CONFIG_STATE_MSG_RXM_SVSI, - UBX_CONFIG_STATE_CONFIGURED -} ubx_config_state_t; - -typedef enum { - CLASS_UNKNOWN = 0, - NAV = 1, - RXM = 2, - ACK = 3, - CFG = 4 -} ubx_message_class_t; - -typedef enum { - //these numbers do NOT correspond to the message id numbers of the ubx protocol - ID_UNKNOWN = 0, - NAV_POSLLH, - NAV_SOL, - NAV_TIMEUTC, -// NAV_DOP, - NAV_SVINFO, - NAV_VELNED, -// RXM_SVSI, - CFG_NAV5, - ACK_ACK, - ACK_NAK, -} ubx_message_id_t; - typedef enum { UBX_DECODE_UNINIT = 0, UBX_DECODE_GOT_SYNC1, @@ -401,17 +364,17 @@ private: int _fd; struct vehicle_gps_position_s *_gps_position; - ubx_config_state_t _config_state; + bool _configured; bool _waiting_for_ack; - uint8_t _clsID_needed; - uint8_t _msgID_needed; + uint8_t _message_class_needed; + uint8_t _message_id_needed; ubx_decode_state_t _decode_state; uint8_t _rx_buffer[RECV_BUFFER_SIZE]; unsigned _rx_count; uint8_t _rx_ck_a; uint8_t _rx_ck_b; - ubx_message_class_t _message_class; - ubx_message_id_t _message_id; + uint8_t _message_class; + uint8_t _message_id; unsigned _payload_size; uint8_t _disable_cmd_counter; }; From 0ccf50bca367d39f75a7a4f76f98dec7352ef3d5 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Thu, 11 Jul 2013 18:17:36 +0400 Subject: [PATCH 14/22] ubx: SVINFO parsing optimized and message rate increased, CPU consumption reduced in 6 times, ~0.3% now. --- src/drivers/gps/ubx.cpp | 42 +++++++++++++++++++---------------------- 1 file changed, 19 insertions(+), 23 deletions(-) diff --git a/src/drivers/gps/ubx.cpp b/src/drivers/gps/ubx.cpp index 895d164377..b579db7150 100644 --- a/src/drivers/gps/ubx.cpp +++ b/src/drivers/gps/ubx.cpp @@ -56,8 +56,9 @@ #include "ubx.h" -#define UBX_CONFIG_TIMEOUT 100 -#define UBX_PACKET_TIMEOUT 2 +#define UBX_CONFIG_TIMEOUT 200 // ms, timeout for waiting ACK +#define UBX_PACKET_TIMEOUT 2 // ms, if now data during this delay assume that full update received +#define UBX_WAIT_BEFORE_READ 20 // ms, wait before reading to save read() calls UBX::UBX(const int &fd, struct vehicle_gps_position_s *gps_position) : _fd(fd), @@ -214,7 +215,7 @@ UBX::configure(unsigned &baudrate) return 1; } - configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_SVINFO, 10); + configure_message_rate(UBX_CLASS_NAV, UBX_MESSAGE_NAV_SVINFO, 5); if (wait_for_ack(UBX_CONFIG_TIMEOUT) < 0) { warnx("ubx: msg rate configuration failed: NAV SVINFO\n"); @@ -280,9 +281,11 @@ UBX::receive(unsigned timeout) if (fds[0].revents & POLLIN) { /* * We are here because poll says there is some data, so this - * won't block even on a blocking device. If more bytes are - * available, we'll go back to poll() again... + * won't block even on a blocking device. But don't read immediately + * by 1-2 bytes, wait for some more data to save expensive read() calls. + * If more bytes are available, we'll go back to poll() again. */ + usleep(UBX_WAIT_BEFORE_READ * 1000); count = read(_fd, buf, sizeof(buf)); /* pass received bytes to the packet decoder */ @@ -459,45 +462,39 @@ UBX::handle_message() } case UBX_MESSAGE_NAV_SVINFO: { - // printf("GOT NAV_SVINFO\n"); - // TODO avoid memcpy + //printf("GOT NAV_SVINFO\n"); const int length_part1 = 8; - char _rx_buffer_part1[length_part1]; - memcpy(_rx_buffer_part1, _rx_buffer, length_part1); - gps_bin_nav_svinfo_part1_packet_t *packet_part1 = (gps_bin_nav_svinfo_part1_packet_t *) _rx_buffer_part1; - + gps_bin_nav_svinfo_part1_packet_t *packet_part1 = (gps_bin_nav_svinfo_part1_packet_t *) _rx_buffer; const int length_part2 = 12; gps_bin_nav_svinfo_part2_packet_t *packet_part2; - char _rx_buffer_part2[length_part2]; // for temporal storage uint8_t satellites_used = 0; int i; - // printf("Number of Channels: %d\n", packet_part1->numCh); - for (i = 0; i < packet_part1->numCh; i++) { //for each channel - - /* get satellite information from the buffer */ - memcpy(_rx_buffer_part2, &(_rx_buffer[length_part1 + i * length_part2]), length_part2); - packet_part2 = (gps_bin_nav_svinfo_part2_packet_t *) _rx_buffer_part2; + //printf("Number of Channels: %d\n", packet_part1->numCh); + for (i = 0; i < packet_part1->numCh; i++) { + /* set pointer to sattelite_i information */ + packet_part2 = (gps_bin_nav_svinfo_part2_packet_t *) & (_rx_buffer[length_part1 + i * length_part2]); /* write satellite information to global storage */ uint8_t sv_used = packet_part2->flags & 0x01; if (sv_used) { - // Count SVs used for NAV. + /* count SVs used for NAV */ satellites_used++; } - // Record info for all channels, whether or not the SV is used for NAV. + /* record info for all channels, whether or not the SV is used for NAV */ _gps_position->satellite_used[i] = sv_used; _gps_position->satellite_snr[i] = packet_part2->cno; _gps_position->satellite_elevation[i] = (uint8_t)(packet_part2->elev); _gps_position->satellite_azimuth[i] = (uint8_t)((float)packet_part2->azim * 255.0f / 360.0f); _gps_position->satellite_prn[i] = packet_part2->svid; + //printf("SAT %d: %d %d %d %d\n", i, (int)sv_used, (int)packet_part2->cno, (int)(uint8_t)(packet_part2->elev), (int)packet_part2->svid); } - for (i = packet_part1->numCh; i < 20; i++) { //these channels are unused - /* Unused channels have to be set to zero for e.g. MAVLink */ + for (i = packet_part1->numCh; i < 20; i++) { + /* unused channels have to be set to zero for e.g. MAVLink */ _gps_position->satellite_prn[i] = 0; _gps_position->satellite_used[i] = 0; _gps_position->satellite_snr[i] = 0; @@ -507,7 +504,6 @@ UBX::handle_message() _gps_position->satellites_visible = satellites_used; // visible ~= used but we are interested in the used ones - /* set timestamp if any sat info is available */ if (packet_part1->numCh > 0) { _gps_position->satellite_info_available = true; From 39b3fc8d32ebaa0a9a01e73399884b007e97b378 Mon Sep 17 00:00:00 2001 From: Jean Cyr Date: Thu, 11 Jul 2013 10:36:17 -0400 Subject: [PATCH 15/22] Don't leave RX in bind mode on console open fail Don't leave RX in bind mode in the unlikely eventuality that console open fails --- src/drivers/px4io/px4io.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 08aef70692..ae56b70b36 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -1696,8 +1696,6 @@ bind(int argc, char *argv[]) else errx(1, "unknown parameter %s, use dsm2 or dsmx", argv[2]); - g_dev->ioctl(nullptr, DSM_BIND_START, pulses); - /* Open console directly to grab CTRL-C signal */ int console = open("/dev/console", O_NONBLOCK | O_RDONLY | O_NOCTTY); if (!console) @@ -1706,6 +1704,8 @@ bind(int argc, char *argv[]) warnx("This command will only bind DSM if satellite VCC (red wire) is controlled by relay 1."); warnx("Press CTRL-C or 'c' when done."); + g_dev->ioctl(nullptr, DSM_BIND_START, pulses); + for (;;) { usleep(500000L); /* Check if user wants to quit */ From 28f536dc4478e5536b568093ec62cf16e8a1687d Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 11 Jul 2013 23:25:08 +0200 Subject: [PATCH 16/22] Hotfix: fixed compile warnings --- src/modules/att_pos_estimator_ekf/KalmanNav.cpp | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/src/modules/att_pos_estimator_ekf/KalmanNav.cpp b/src/modules/att_pos_estimator_ekf/KalmanNav.cpp index 97d7fdd757..191d20f306 100644 --- a/src/modules/att_pos_estimator_ekf/KalmanNav.cpp +++ b/src/modules/att_pos_estimator_ekf/KalmanNav.cpp @@ -661,10 +661,10 @@ int KalmanNav::correctPos() Vector y(6); y(0) = _gps.vel_n_m_s - vN; y(1) = _gps.vel_e_m_s - vE; - y(2) = double(_gps.lat) - lat * 1.0e7 * M_RAD_TO_DEG; - y(3) = double(_gps.lon) - lon * 1.0e7 * M_RAD_TO_DEG; - y(4) = double(_gps.alt) / 1.0e3 - alt; - y(5) = double(_sensors.baro_alt_meter) - alt; + y(2) = double(_gps.lat) - double(lat) * 1.0e7 * M_RAD_TO_DEG; + y(3) = double(_gps.lon) - double(lon) * 1.0e7 * M_RAD_TO_DEG; + y(4) = _gps.alt / 1.0e3f - alt; + y(5) = _sensors.baro_alt_meter - alt; // compute correction // http://en.wikipedia.org/wiki/Extended_Kalman_filter @@ -698,7 +698,7 @@ int KalmanNav::correctPos() vD += xCorrect(VD); lat += double(xCorrect(LAT)); lon += double(xCorrect(LON)); - alt += double(xCorrect(ALT)); + alt += xCorrect(ALT); // update state covariance // http://en.wikipedia.org/wiki/Extended_Kalman_filter @@ -710,7 +710,7 @@ int KalmanNav::correctPos() static int counter = 0; if (beta > _faultPos.get() && (counter % 10 == 0)) { warnx("fault in gps: beta = %8.4f", (double)beta); - warnx("Y/N: vN: %8.4f, vE: %8.4f, lat: %8.4f, lon: %8.4f, alt: %8.4f", + warnx("Y/N: vN: %8.4f, vE: %8.4f, lat: %8.4f, lon: %8.4f, alt: %8.4f, baro: %8.4f", double(y(0) / sqrtf(RPos(0, 0))), double(y(1) / sqrtf(RPos(1, 1))), double(y(2) / sqrtf(RPos(2, 2))), From 1d986d6c040ed0123bdb8cccad1e444f9d0113f3 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Fri, 12 Jul 2013 10:09:38 +0400 Subject: [PATCH 17/22] sdlog2: Global Position Set Point message added, vehicle_global_position_setpoint topic fixed --- src/modules/sdlog2/sdlog2.c | 31 ++++++++++++++++- src/modules/sdlog2/sdlog2_messages.h | 34 ++++++++++++++----- .../topics/vehicle_global_position_setpoint.h | 2 +- 3 files changed, 57 insertions(+), 10 deletions(-) diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 3e6b20472c..3713e0b306 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -72,6 +72,7 @@ #include #include #include +#include #include #include #include @@ -615,7 +616,7 @@ int sdlog2_thread_main(int argc, char *argv[]) /* --- IMPORTANT: DEFINE NUMBER OF ORB STRUCTS TO WAIT FOR HERE --- */ /* number of messages */ - const ssize_t fdsc = 18; + const ssize_t fdsc = 19; /* Sanity check variable and index */ ssize_t fdsc_count = 0; /* file descriptors to wait for */ @@ -637,6 +638,7 @@ int sdlog2_thread_main(int argc, char *argv[]) struct vehicle_local_position_s local_pos; struct vehicle_local_position_setpoint_s local_pos_sp; struct vehicle_global_position_s global_pos; + struct vehicle_global_position_setpoint_s global_pos_sp; struct vehicle_gps_position_s gps_pos; struct vehicle_vicon_position_s vicon_pos; struct optical_flow_s flow; @@ -660,6 +662,7 @@ int sdlog2_thread_main(int argc, char *argv[]) int local_pos_sub; int local_pos_sp_sub; int global_pos_sub; + int global_pos_sp_sub; int gps_pos_sub; int vicon_pos_sub; int flow_sub; @@ -689,6 +692,7 @@ int sdlog2_thread_main(int argc, char *argv[]) struct log_ARSP_s log_ARSP; struct log_FLOW_s log_FLOW; struct log_GPOS_s log_GPOS; + struct log_GPSP_s log_GPSP; struct log_ESC_s log_ESC; } body; } log_msg = { @@ -775,6 +779,12 @@ int sdlog2_thread_main(int argc, char *argv[]) fds[fdsc_count].events = POLLIN; fdsc_count++; + /* --- GLOBAL POSITION SETPOINT--- */ + subs.global_pos_sp_sub = orb_subscribe(ORB_ID(vehicle_global_position_setpoint)); + fds[fdsc_count].fd = subs.global_pos_sp_sub; + fds[fdsc_count].events = POLLIN; + fdsc_count++; + /* --- VICON POSITION --- */ subs.vicon_pos_sub = orb_subscribe(ORB_ID(vehicle_vicon_position)); fds[fdsc_count].fd = subs.vicon_pos_sub; @@ -1077,6 +1087,25 @@ int sdlog2_thread_main(int argc, char *argv[]) LOGBUFFER_WRITE_AND_COUNT(GPOS); } + /* --- GLOBAL POSITION SETPOINT --- */ + if (fds[ifds++].revents & POLLIN) { + orb_copy(ORB_ID(vehicle_global_position_setpoint), subs.global_pos_sp_sub, &buf.global_pos_sp); + log_msg.msg_type = LOG_GPSP_MSG; + log_msg.body.log_GPSP.altitude_is_relative = buf.global_pos_sp.altitude_is_relative; + log_msg.body.log_GPSP.lat = buf.global_pos_sp.lat; + log_msg.body.log_GPSP.lon = buf.global_pos_sp.lon; + log_msg.body.log_GPSP.altitude = buf.global_pos_sp.altitude; + log_msg.body.log_GPSP.yaw = buf.global_pos_sp.yaw; + log_msg.body.log_GPSP.loiter_radius = buf.global_pos_sp.loiter_radius; + log_msg.body.log_GPSP.loiter_direction = buf.global_pos_sp.loiter_direction; + log_msg.body.log_GPSP.nav_cmd = buf.global_pos_sp.nav_cmd; + log_msg.body.log_GPSP.param1 = buf.global_pos_sp.param1; + log_msg.body.log_GPSP.param2 = buf.global_pos_sp.param2; + log_msg.body.log_GPSP.param3 = buf.global_pos_sp.param3; + log_msg.body.log_GPSP.param4 = buf.global_pos_sp.param4; + LOGBUFFER_WRITE_AND_COUNT(GPSP); + } + /* --- VICON POSITION --- */ if (fds[ifds++].revents & POLLIN) { orb_copy(ORB_ID(vehicle_vicon_position), subs.vicon_pos_sub, &buf.vicon_pos); diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index abc882d23c..934e4dec89 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -149,15 +149,15 @@ struct log_ATTC_s { /* --- STAT - VEHICLE STATE --- */ #define LOG_STAT_MSG 10 struct log_STAT_s { - unsigned char state; - unsigned char flight_mode; - unsigned char manual_control_mode; - unsigned char manual_sas_mode; - unsigned char armed; + uint8_t state; + uint8_t flight_mode; + uint8_t manual_control_mode; + uint8_t manual_sas_mode; + uint8_t armed; float battery_voltage; float battery_current; float battery_remaining; - unsigned char battery_warning; + uint8_t battery_warning; }; /* --- RC - RC INPUT CHANNELS --- */ @@ -210,13 +210,29 @@ struct log_GPOS_s { float vel_d; }; +/* --- GPSP - GLOBAL POSITION SETPOINT --- */ +#define LOG_GPSP_MSG 17 +struct log_GPSP_s { + uint8_t altitude_is_relative; + int32_t lat; + int32_t lon; + float altitude; + float yaw; + float loiter_radius; + int8_t loiter_direction; + uint8_t nav_cmd; + float param1; + float param2; + float param3; + float param4; +}; + /* --- ESC - ESC STATE --- */ -#define LOG_ESC_MSG 64 +#define LOG_ESC_MSG 18 struct log_ESC_s { uint16_t counter; uint8_t esc_count; uint8_t esc_connectiontype; - uint8_t esc_num; uint16_t esc_address; uint16_t esc_version; @@ -227,6 +243,7 @@ struct log_ESC_s { float esc_setpoint; uint16_t esc_setpoint_raw; }; + #pragma pack(pop) /* construct list of all message formats */ @@ -248,6 +265,7 @@ static const struct log_format_s log_formats[] = { LOG_FORMAT(ARSP, "fff", "RollRateSP,PitchRateSP,YawRateSP"), LOG_FORMAT(FLOW, "hhfffBB", "RawX,RawY,CompX,CompY,Dist,Q,SensID"), LOG_FORMAT(GPOS, "LLffff", "Lat,Lon,Alt,VelN,VelE,VelD"), + LOG_FORMAT(GPSP, "BLLfffbBffff", "AltRel,Lat,Lon,Alt,Yaw,LoiterR,LoiterDir,NavCmd,P1,P2,P3,P4"), LOG_FORMAT(ESC, "HBBBHHHHHHfH", "Counter,NumESC,Conn,No,Version,Adr,Volt,Amp,RPM,Temp,SetP,SetPRAW"), }; diff --git a/src/modules/uORB/topics/vehicle_global_position_setpoint.h b/src/modules/uORB/topics/vehicle_global_position_setpoint.h index 3ae3ff28ca..5c8ce1e4d6 100644 --- a/src/modules/uORB/topics/vehicle_global_position_setpoint.h +++ b/src/modules/uORB/topics/vehicle_global_position_setpoint.h @@ -66,7 +66,7 @@ struct vehicle_global_position_setpoint_s float altitude; /**< altitude in meters */ float yaw; /**< in radians NED -PI..+PI */ float loiter_radius; /**< loiter radius in meters, 0 for a VTOL to hover */ - uint8_t loiter_direction; /**< 1: positive / clockwise, -1, negative. */ + int8_t loiter_direction; /**< 1: positive / clockwise, -1, negative. */ enum NAV_CMD nav_cmd; /**< true if loitering is enabled */ float param1; float param2; From 3b9c306d64acdebddbf9de1d518777f11c066cbe Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 12 Jul 2013 11:11:26 +0200 Subject: [PATCH 18/22] Hotfix for relative altitude waypoints --- src/modules/mavlink/waypoints.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/mavlink/waypoints.c b/src/modules/mavlink/waypoints.c index 405046750b..eea928a170 100644 --- a/src/modules/mavlink/waypoints.c +++ b/src/modules/mavlink/waypoints.c @@ -373,7 +373,7 @@ void check_waypoints_reached(uint64_t now, const struct vehicle_global_position_ dist = mavlink_wpm_distance_to_point_global_wgs84(wpm->current_active_wp_id, (float)global_pos->lat * 1e-7f, (float)global_pos->lon * 1e-7f, global_pos->alt); } else if (coordinate_frame == (int)MAV_FRAME_GLOBAL_RELATIVE_ALT) { - dist = mavlink_wpm_distance_to_point_global_wgs84(wpm->current_active_wp_id, global_pos->lat, global_pos->lon, global_pos->relative_alt); + dist = mavlink_wpm_distance_to_point_global_wgs84(wpm->current_active_wp_id, (float)global_pos->lat * 1e-7f, (float)global_pos->lon * 1e-7f, global_pos->relative_alt); } else if (coordinate_frame == (int)MAV_FRAME_LOCAL_ENU || coordinate_frame == (int)MAV_FRAME_LOCAL_NED) { dist = mavlink_wpm_distance_to_point_local(wpm->current_active_wp_id, local_pos->x, local_pos->y, local_pos->z); From b500cce31ef4ec3c68a5c98e90e3e6dbe10d6722 Mon Sep 17 00:00:00 2001 From: Simon Wilks Date: Sat, 13 Jul 2013 01:08:06 +0200 Subject: [PATCH 19/22] Major refactor of HoTT drivers and finished sensor read implementation. --- src/drivers/hott/{comms.c => comms.cpp} | 0 .../{hott_sensors.c => hott_sensors.cpp} | 40 +++-- src/drivers/hott/hott_sensors/module.mk | 6 +- .../{hott_telemetry.c => hott_telemetry.cpp} | 33 +++-- src/drivers/hott/hott_telemetry/module.mk | 6 +- src/drivers/hott/{messages.c => messages.cpp} | 140 +++++++++++------- src/drivers/hott/messages.h | 34 +---- src/modules/uORB/topics/esc_status.h | 3 +- 8 files changed, 153 insertions(+), 109 deletions(-) rename src/drivers/hott/{comms.c => comms.cpp} (100%) rename src/drivers/hott/hott_sensors/{hott_sensors.c => hott_sensors.cpp} (88%) rename src/drivers/hott/hott_telemetry/{hott_telemetry.c => hott_telemetry.cpp} (91%) rename src/drivers/hott/{messages.c => messages.cpp} (66%) diff --git a/src/drivers/hott/comms.c b/src/drivers/hott/comms.cpp similarity index 100% rename from src/drivers/hott/comms.c rename to src/drivers/hott/comms.cpp diff --git a/src/drivers/hott/hott_sensors/hott_sensors.c b/src/drivers/hott/hott_sensors/hott_sensors.cpp similarity index 88% rename from src/drivers/hott/hott_sensors/hott_sensors.c rename to src/drivers/hott/hott_sensors/hott_sensors.cpp index 41ca0c92f8..ad7e74e62d 100644 --- a/src/drivers/hott/hott_sensors/hott_sensors.c +++ b/src/drivers/hott/hott_sensors/hott_sensors.cpp @@ -55,6 +55,14 @@ #include "../comms.h" #include "../messages.h" +#define DEFAULT_UART "/dev/ttyS2"; /**< USART5 */ + +/* Oddly, ERROR is not defined for C++ */ +#ifdef ERROR +# undef ERROR +#endif +static const int ERROR = -1; + static int thread_should_exit = false; /**< Deamon exit flag */ static int thread_running = false; /**< Deamon status flag */ static int deamon_task; /**< Handle of deamon task / thread */ @@ -64,7 +72,7 @@ static const char commandline_usage[] = "usage: hott_sensors start|status|stop [ /** * Deamon management function. */ -__EXPORT int hott_sensors_main(int argc, char *argv[]); +extern "C" __EXPORT int hott_sensors_main(int argc, char *argv[]); /** * Mainloop of daemon. @@ -96,11 +104,13 @@ int recv_data(int uart, uint8_t *buffer, size_t *size, uint8_t *id) { static const int timeout_ms = 1000; - struct pollfd fds[] = { { .fd = uart, .events = POLLIN } }; - + + struct pollfd fds; + fds.fd = uart; + fds.events = POLLIN; // XXX should this poll be inside the while loop??? - if (poll(fds, 1, timeout_ms) > 0) { + if (poll(&fds, 1, timeout_ms) > 0) { int i = 0; bool stop_byte_read = false; while (true) { @@ -129,7 +139,7 @@ hott_sensors_thread_main(int argc, char *argv[]) thread_running = true; - const char *device = "/dev/ttyS2"; /**< Default telemetry port: USART5 */ + const char *device = DEFAULT_UART; /* read commandline arguments */ for (int i = 0; i < argc && argv[i]; i++) { @@ -151,20 +161,20 @@ hott_sensors_thread_main(int argc, char *argv[]) thread_running = false; } - pub_messages_init(); + init_pub_messages(); - uint8_t buffer[MESSAGE_BUFFER_SIZE]; + uint8_t buffer[MAX_MESSAGE_BUFFER_SIZE]; size_t size = 0; uint8_t id = 0; while (!thread_should_exit) { // Currently we only support a General Air Module sensor. - build_gam_request(&buffer, &size); + build_gam_request(&buffer[0], &size); send_poll(uart, buffer, size); // The sensor will need a little time before it starts sending. usleep(5000); - recv_data(uart, &buffer, &size, &id); + recv_data(uart, &buffer[0], &size, &id); // Determine which moduel sent it and process accordingly. if (id == GAM_SENSOR_ID) { @@ -199,12 +209,12 @@ hott_sensors_main(int argc, char *argv[]) } thread_should_exit = false; - deamon_task = task_spawn(daemon_name, - SCHED_DEFAULT, - SCHED_PRIORITY_MAX - 40, - 2048, - hott_sensors_thread_main, - (argv) ? (const char **)&argv[2] : (const char **)NULL); + deamon_task = task_spawn_cmd(daemon_name, + SCHED_DEFAULT, + SCHED_PRIORITY_MAX - 40, + 2048, + hott_sensors_thread_main, + (argv) ? (const char **)&argv[2] : (const char **)NULL); exit(0); } diff --git a/src/drivers/hott/hott_sensors/module.mk b/src/drivers/hott/hott_sensors/module.mk index ca65d3de22..b5f5762ba7 100644 --- a/src/drivers/hott/hott_sensors/module.mk +++ b/src/drivers/hott/hott_sensors/module.mk @@ -37,6 +37,6 @@ MODULE_COMMAND = hott_sensors -SRCS = hott_sensors.c \ - ../messages.c \ - ../comms.c +SRCS = hott_sensors.cpp \ + ../messages.cpp \ + ../comms.cpp diff --git a/src/drivers/hott/hott_telemetry/hott_telemetry.c b/src/drivers/hott/hott_telemetry/hott_telemetry.cpp similarity index 91% rename from src/drivers/hott/hott_telemetry/hott_telemetry.c rename to src/drivers/hott/hott_telemetry/hott_telemetry.cpp index a88f357f6b..1c68e06b1e 100644 --- a/src/drivers/hott/hott_telemetry/hott_telemetry.c +++ b/src/drivers/hott/hott_telemetry/hott_telemetry.cpp @@ -57,6 +57,14 @@ #include "../comms.h" #include "../messages.h" +#define DEFAULT_UART "/dev/ttyS1"; /**< USART2 */ + +/* Oddly, ERROR is not defined for C++ */ +#ifdef ERROR +# undef ERROR +#endif +static const int ERROR = -1; + static int thread_should_exit = false; /**< Deamon exit flag */ static int thread_running = false; /**< Deamon status flag */ static int deamon_task; /**< Handle of deamon task / thread */ @@ -66,7 +74,7 @@ static const char commandline_usage[] = "usage: hott_telemetry start|status|stop /** * Deamon management function. */ -__EXPORT int hott_telemetry_main(int argc, char *argv[]); +extern "C" __EXPORT int hott_telemetry_main(int argc, char *argv[]); /** * Mainloop of daemon. @@ -80,11 +88,14 @@ int recv_req_id(int uart, uint8_t *id) { static const int timeout_ms = 1000; // TODO make it a define - struct pollfd fds[] = { { .fd = uart, .events = POLLIN } }; uint8_t mode; + + struct pollfd fds; + fds.fd = uart; + fds.events = POLLIN; - if (poll(fds, 1, timeout_ms) > 0) { + if (poll(&fds, 1, timeout_ms) > 0) { /* Get the mode: binary or text */ read(uart, &mode, sizeof(mode)); @@ -109,11 +120,13 @@ recv_data(int uart, uint8_t *buffer, size_t *size, uint8_t *id) usleep(5000); static const int timeout_ms = 1000; - struct pollfd fds[] = { { .fd = uart, .events = POLLIN } }; + struct pollfd fds; + fds.fd = uart; + fds.events = POLLIN; // XXX should this poll be inside the while loop??? - if (poll(fds, 1, timeout_ms) > 0) { + if (poll(&fds, 1, timeout_ms) > 0) { int i = 0; bool stop_byte_read = false; while (true) { @@ -172,7 +185,7 @@ hott_telemetry_thread_main(int argc, char *argv[]) thread_running = true; - const char *device = "/dev/ttyS1"; /**< Default telemetry port: USART2 */ + const char *device = DEFAULT_UART; /* read commandline arguments */ for (int i = 0; i < argc && argv[i]; i++) { @@ -194,9 +207,9 @@ hott_telemetry_thread_main(int argc, char *argv[]) thread_running = false; } - sub_messages_init(); + init_sub_messages(); - uint8_t buffer[MESSAGE_BUFFER_SIZE]; + uint8_t buffer[MAX_MESSAGE_BUFFER_SIZE]; size_t size = 0; uint8_t id = 0; bool connected = true; @@ -212,7 +225,9 @@ hott_telemetry_thread_main(int argc, char *argv[]) case EAM_SENSOR_ID: build_eam_response(buffer, &size); break; - + case GAM_SENSOR_ID: + build_gam_response(buffer, &size); + break; case GPS_SENSOR_ID: build_gps_response(buffer, &size); break; diff --git a/src/drivers/hott/hott_telemetry/module.mk b/src/drivers/hott/hott_telemetry/module.mk index 7673d7e775..b19cbd14cf 100644 --- a/src/drivers/hott/hott_telemetry/module.mk +++ b/src/drivers/hott/hott_telemetry/module.mk @@ -37,6 +37,6 @@ MODULE_COMMAND = hott_telemetry -SRCS = hott_telemetry.c \ - ../messages.c \ - ../comms.c +SRCS = hott_telemetry.cpp \ + ../messages.cpp \ + ../comms.cpp diff --git a/src/drivers/hott/messages.c b/src/drivers/hott/messages.cpp similarity index 66% rename from src/drivers/hott/messages.c rename to src/drivers/hott/messages.cpp index 36d5fe942f..004322a2d7 100644 --- a/src/drivers/hott/messages.c +++ b/src/drivers/hott/messages.cpp @@ -46,6 +46,7 @@ #include #include #include +#include #include #include #include @@ -53,36 +54,37 @@ /* The board is very roughly 5 deg warmer than the surrounding air */ #define BOARD_TEMP_OFFSET_DEG 5 -static int battery_sub = -1; -static int gps_sub = -1; -static int home_sub = -1; -static int sensor_sub = -1; -static int airspeed_sub = -1; -static int esc_sub = -1; +static int _battery_sub = -1; +static int _gps_sub = -1; +static int _home_sub = -1; +static int _sensor_sub = -1; +static int _airspeed_sub = -1; +static int _esc_sub = -1; -//orb_advert_t _esc_pub; -//struct esc_s _esc; +orb_advert_t _esc_pub; +struct esc_status_s _esc; -static bool home_position_set = false; -static double home_lat = 0.0d; -static double home_lon = 0.0d; +static bool _home_position_set = false; +static double _home_lat = 0.0d; +static double _home_lon = 0.0d; void -sub_messages_init(void) +init_sub_messages(void) { - battery_sub = orb_subscribe(ORB_ID(battery_status)); - gps_sub = orb_subscribe(ORB_ID(vehicle_gps_position)); - home_sub = orb_subscribe(ORB_ID(home_position)); - sensor_sub = orb_subscribe(ORB_ID(sensor_combined)); - airspeed_sub = orb_subscribe(ORB_ID(airspeed)); - //esc_sub = orb_subscribe(ORB_ID(esc)); + _battery_sub = orb_subscribe(ORB_ID(battery_status)); + _gps_sub = orb_subscribe(ORB_ID(vehicle_gps_position)); + _home_sub = orb_subscribe(ORB_ID(home_position)); + _sensor_sub = orb_subscribe(ORB_ID(sensor_combined)); + _airspeed_sub = orb_subscribe(ORB_ID(airspeed)); + _esc_sub = orb_subscribe(ORB_ID(esc_status)); } void -pub_messages_init(void) +init_pub_messages(void) { - //esc_pub = orb_subscribe(ORB_ID(esc)); + memset(&_esc, 0, sizeof(_esc)); + _esc_pub = orb_advertise(ORB_ID(esc_status), &_esc); } void @@ -107,21 +109,22 @@ publish_gam_message(const uint8_t *buffer) memcpy(&msg, buffer, size); /* announce the esc if needed, just publish else */ -// if (esc_pub > 0) { -// orb_publish(ORB_ID(airspeed), _esc_pub, &_esc); -// -// } else { -// _esc_pub = orb_advertise(ORB_ID(esc), &_esc); -// } + if (_esc_pub > 0) { + orb_publish(ORB_ID(esc_status), _esc_pub, &_esc); + } else { + _esc_pub = orb_advertise(ORB_ID(esc_status), &_esc); + } // Publish it. - uint16_t rpm = ((msg.rpm_H << 8) | (msg.rpm_L & 0xff)) * 10; - //_esc.rpm = rpm; - uint8_t temp = msg.temperature2 + 20; - //_esc.temperature = temp; - float current = ((msg.current_H << 8) | (msg.current_L & 0xff)) * 0.1f; - //_esc.current = current; - printf("RPM: %d TEMP: %d A: %2.1f\n", rpm, temp, current); + _esc.esc_count = 1; + _esc.esc_connectiontype = ESC_CONNECTION_TYPE_PPM; + + _esc.esc[0].esc_vendor = ESC_VENDOR_GRAUPNER_HOTT; + _esc.esc[0].esc_rpm = (uint16_t)((msg.rpm_H << 8) | (msg.rpm_L & 0xff)) * 10; + _esc.esc[0].esc_temperature = msg.temperature1 - 20; + _esc.esc[0].esc_voltage = (uint16_t)((msg.main_voltage_H << 8) | (msg.main_voltage_L & 0xff)); + _esc.esc[0].esc_current = (uint16_t)((msg.current_H << 8) | (msg.current_L & 0xff)); + //printf("T: %d\n", _esc.esc[0].esc_temperature); } void @@ -130,12 +133,12 @@ build_eam_response(uint8_t *buffer, size_t *size) /* get a local copy of the current sensor values */ struct sensor_combined_s raw; memset(&raw, 0, sizeof(raw)); - orb_copy(ORB_ID(sensor_combined), sensor_sub, &raw); + orb_copy(ORB_ID(sensor_combined), _sensor_sub, &raw); /* get a local copy of the battery data */ struct battery_status_s battery; memset(&battery, 0, sizeof(battery)); - orb_copy(ORB_ID(battery_status), battery_sub, &battery); + orb_copy(ORB_ID(battery_status), _battery_sub, &battery); struct eam_module_msg msg; *size = sizeof(msg); @@ -145,7 +148,7 @@ build_eam_response(uint8_t *buffer, size_t *size) msg.eam_sensor_id = EAM_SENSOR_ID; msg.sensor_text_id = EAM_SENSOR_TEXT_ID; - msg.temperature1 = (uint8_t)(raw.baro_temp_celcius - 20); + msg.temperature1 = (uint8_t)(raw.baro_temp_celcius + 20); msg.temperature2 = msg.temperature1 - BOARD_TEMP_OFFSET_DEG; msg.main_voltage_L = (uint8_t)(battery.voltage_v * 10); @@ -157,7 +160,7 @@ build_eam_response(uint8_t *buffer, size_t *size) /* get a local copy of the airspeed data */ struct airspeed_s airspeed; memset(&airspeed, 0, sizeof(airspeed)); - orb_copy(ORB_ID(airspeed), airspeed_sub, &airspeed); + orb_copy(ORB_ID(airspeed), _airspeed_sub, &airspeed); uint16_t speed = (uint16_t)(airspeed.indicated_airspeed_m_s * 3.6f); msg.speed_L = (uint8_t)speed & 0xff; @@ -167,20 +170,55 @@ build_eam_response(uint8_t *buffer, size_t *size) memcpy(buffer, &msg, *size); } +void +build_gam_response(uint8_t *buffer, size_t *size) +{ + /* get a local copy of the ESC Status values */ + struct esc_status_s esc; + memset(&esc, 0, sizeof(esc)); + orb_copy(ORB_ID(esc_status), _esc_sub, &esc); + + struct gam_module_msg msg; + *size = sizeof(msg); + memset(&msg, 0, *size); + + msg.start = START_BYTE; + msg.gam_sensor_id = GAM_SENSOR_ID; + msg.sensor_text_id = GAM_SENSOR_TEXT_ID; + + msg.temperature1 = (uint8_t)(esc.esc[0].esc_temperature + 20); + msg.temperature2 = 20; // 0 deg. C. + + uint16_t voltage = (uint16_t)(esc.esc[0].esc_voltage); + msg.main_voltage_L = (uint8_t)voltage & 0xff; + msg.main_voltage_H = (uint8_t)(voltage >> 8) & 0xff; + + uint16_t current = (uint16_t)(esc.esc[0].esc_current); + msg.current_L = (uint8_t)current & 0xff; + msg.current_H = (uint8_t)(current >> 8) & 0xff; + + uint16_t rpm = (uint16_t)(esc.esc[0].esc_rpm * 0.1f); + msg.rpm_L = (uint8_t)rpm & 0xff; + msg.rpm_H = (uint8_t)(rpm >> 8) & 0xff; + + msg.stop = STOP_BYTE; + memcpy(buffer, &msg, *size); +} + void build_gps_response(uint8_t *buffer, size_t *size) { /* get a local copy of the current sensor values */ struct sensor_combined_s raw; memset(&raw, 0, sizeof(raw)); - orb_copy(ORB_ID(sensor_combined), sensor_sub, &raw); + orb_copy(ORB_ID(sensor_combined), _sensor_sub, &raw); /* get a local copy of the battery data */ struct vehicle_gps_position_s gps; memset(&gps, 0, sizeof(gps)); - orb_copy(ORB_ID(vehicle_gps_position), gps_sub, &gps); + orb_copy(ORB_ID(vehicle_gps_position), _gps_sub, &gps); - struct gps_module_msg msg = { 0 }; + struct gps_module_msg msg; *size = sizeof(msg); memset(&msg, 0, *size); @@ -200,7 +238,7 @@ build_gps_response(uint8_t *buffer, size_t *size) msg.flight_direction = (uint8_t)(gps.cog_rad * M_RAD_TO_DEG_F); /* GPS speed */ - uint16_t speed = (uint16_t)(gps.vel_m_s * 3.6); + uint16_t speed = (uint16_t)(gps.vel_m_s * 3.6f); msg.gps_speed_L = (uint8_t)speed & 0xff; msg.gps_speed_H = (uint8_t)(speed >> 8) & 0xff; @@ -246,33 +284,33 @@ build_gps_response(uint8_t *buffer, size_t *size) msg.longitude_sec_H = (uint8_t)(lon_sec >> 8) & 0xff; /* Altitude */ - uint16_t alt = (uint16_t)(gps.alt*1e-3 + 500.0f); + uint16_t alt = (uint16_t)(gps.alt*1e-3f + 500.0f); msg.altitude_L = (uint8_t)alt & 0xff; msg.altitude_H = (uint8_t)(alt >> 8) & 0xff; - /* Get any (and probably only ever one) home_sub postion report */ + /* Get any (and probably only ever one) _home_sub postion report */ bool updated; - orb_check(home_sub, &updated); + orb_check(_home_sub, &updated); if (updated) { /* get a local copy of the home position data */ struct home_position_s home; memset(&home, 0, sizeof(home)); - orb_copy(ORB_ID(home_position), home_sub, &home); + orb_copy(ORB_ID(home_position), _home_sub, &home); - home_lat = ((double)(home.lat))*1e-7d; - home_lon = ((double)(home.lon))*1e-7d; - home_position_set = true; + _home_lat = ((double)(home.lat))*1e-7d; + _home_lon = ((double)(home.lon))*1e-7d; + _home_position_set = true; } /* Distance from home */ - if (home_position_set) { - uint16_t dist = (uint16_t)get_distance_to_next_waypoint(home_lat, home_lon, lat, lon); + if (_home_position_set) { + uint16_t dist = (uint16_t)get_distance_to_next_waypoint(_home_lat, _home_lon, lat, lon); msg.distance_L = (uint8_t)dist & 0xff; msg.distance_H = (uint8_t)(dist >> 8) & 0xff; /* Direction back to home */ - uint16_t bearing = (uint16_t)(get_bearing_to_next_waypoint(home_lat, home_lon, lat, lon) * M_RAD_TO_DEG_F); + uint16_t bearing = (uint16_t)(get_bearing_to_next_waypoint(_home_lat, _home_lon, lat, lon) * M_RAD_TO_DEG_F); msg.home_direction = (uint8_t)bearing >> 1; } } diff --git a/src/drivers/hott/messages.h b/src/drivers/hott/messages.h index dce90f273c..451bee91ce 100644 --- a/src/drivers/hott/messages.h +++ b/src/drivers/hott/messages.h @@ -119,23 +119,16 @@ struct eam_module_msg { uint8_t checksum; /**< Lower 8-bits of all bytes summed. */ }; -/** - * The maximum buffer size required to store an Electric Air Module message. - */ -#define EAM_MESSAGE_BUFFER_SIZE sizeof(union { \ - struct eam_module_msg eam; \ -}) - /* General Air Module (GAM) constants. */ #define GAM_SENSOR_ID 0x8d #define GAM_SENSOR_TEXT_ID 0xd0 struct gam_module_msg { - uint8_t start_byte; // start byte constant value 0x7c + uint8_t start; // start byte constant value 0x7c uint8_t gam_sensor_id; // EAM sensort id. constat value 0x8d uint8_t warning_beeps; // 1=A 2=B ... 0x1a=Z 0 = no alarm - uint8_t sensor_id; // constant value 0xd0 + uint8_t sensor_text_id; // constant value 0xd0 uint8_t alarm_invers1; // alarm bitmask. Value is displayed inverted uint8_t alarm_invers2; // alarm bitmask. Value is displayed inverted uint8_t cell1; // cell 1 voltage lower value. 0.02V steps, 124=2.48V @@ -180,14 +173,6 @@ struct gam_module_msg { uint8_t checksum; // checksum }; -/** - * The maximum buffer size required to store a General Air Module message. - */ -#define GAM_MESSAGE_BUFFER_SIZE sizeof(union { \ - struct gam_module_msg gam; \ -}) - - /* GPS sensor constants. */ #define GPS_SENSOR_ID 0x8a #define GPS_SENSOR_TEXT_ID 0xa0 @@ -247,20 +232,15 @@ struct gps_module_msg { uint8_t checksum; /**< Byte 45: Parity Byte */ }; -/** - * The maximum buffer size required to store a HoTT message. - */ -#define GPS_MESSAGE_BUFFER_SIZE sizeof(union { \ - struct gps_module_msg gps; \ -}) +// The maximum size of a message. +#define MAX_MESSAGE_BUFFER_SIZE 45 -#define MESSAGE_BUFFER_SIZE GPS_MESSAGE_BUFFER_SIZE - -void sub_messages_init(void); -void pub_messages_init(void); +void init_sub_messages(void); +void init_pub_messages(void); void build_gam_request(uint8_t *buffer, size_t *size); void publish_gam_message(const uint8_t *buffer); void build_eam_response(uint8_t *buffer, size_t *size); +void build_gam_response(uint8_t *buffer, size_t *size); void build_gps_response(uint8_t *buffer, size_t *size); float _get_distance_to_next_waypoint(double lat_now, double lon_now, double lat_next, double lon_next); void convert_to_degrees_minutes_seconds(double lat, int *deg, int *min, int *sec); diff --git a/src/modules/uORB/topics/esc_status.h b/src/modules/uORB/topics/esc_status.h index e67a39e1e8..00cf59b28f 100644 --- a/src/modules/uORB/topics/esc_status.h +++ b/src/modules/uORB/topics/esc_status.h @@ -63,7 +63,8 @@ enum ESC_VENDOR { ESC_VENDOR_GENERIC = 0, /**< generic ESC */ - ESC_VENDOR_MIKROKOPTER /**< Mikrokopter */ + ESC_VENDOR_MIKROKOPTER, /**< Mikrokopter */ + ESC_VENDOR_GRAUPNER_HOTT /**< Graupner HoTT ESC */ }; enum ESC_CONNECTION_TYPE { From fa29694f0ba85c0b140dc460be14a5205da9c093 Mon Sep 17 00:00:00 2001 From: Simon Wilks Date: Sat, 13 Jul 2013 01:12:47 +0200 Subject: [PATCH 20/22] Whitespace cleanup --- src/drivers/hott/comms.cpp | 10 ---------- src/drivers/hott/comms.h | 10 ---------- src/drivers/hott/messages.cpp | 1 - 3 files changed, 21 deletions(-) diff --git a/src/drivers/hott/comms.cpp b/src/drivers/hott/comms.cpp index a2de87407d..1da1c5c180 100644 --- a/src/drivers/hott/comms.cpp +++ b/src/drivers/hott/comms.cpp @@ -95,13 +95,3 @@ open_uart(const char *device) return uart; } - - - - - - - - - - diff --git a/src/drivers/hott/comms.h b/src/drivers/hott/comms.h index 4954a309e2..f5608122fc 100644 --- a/src/drivers/hott/comms.h +++ b/src/drivers/hott/comms.h @@ -44,13 +44,3 @@ int open_uart(const char *device); #endif /* COMMS_H_ */ - - - - - - - - - - diff --git a/src/drivers/hott/messages.cpp b/src/drivers/hott/messages.cpp index 004322a2d7..c5d73ab11d 100644 --- a/src/drivers/hott/messages.cpp +++ b/src/drivers/hott/messages.cpp @@ -124,7 +124,6 @@ publish_gam_message(const uint8_t *buffer) _esc.esc[0].esc_temperature = msg.temperature1 - 20; _esc.esc[0].esc_voltage = (uint16_t)((msg.main_voltage_H << 8) | (msg.main_voltage_L & 0xff)); _esc.esc[0].esc_current = (uint16_t)((msg.current_H << 8) | (msg.current_L & 0xff)); - //printf("T: %d\n", _esc.esc[0].esc_temperature); } void From 9aa25c5671b6966111dd75687945a8a0b3c8fa19 Mon Sep 17 00:00:00 2001 From: Simon Wilks Date: Sat, 13 Jul 2013 22:18:52 +0200 Subject: [PATCH 21/22] Remove unused code. --- src/drivers/hott/comms.cpp | 5 --- .../hott/hott_sensors/hott_sensors.cpp | 2 +- .../hott/hott_telemetry/hott_telemetry.cpp | 37 +------------------ src/drivers/hott/messages.cpp | 3 +- 4 files changed, 3 insertions(+), 44 deletions(-) diff --git a/src/drivers/hott/comms.cpp b/src/drivers/hott/comms.cpp index 1da1c5c180..cb8bbba37e 100644 --- a/src/drivers/hott/comms.cpp +++ b/src/drivers/hott/comms.cpp @@ -40,15 +40,10 @@ #include "comms.h" #include -#include -#include #include -#include #include #include -#include #include -#include int open_uart(const char *device) diff --git a/src/drivers/hott/hott_sensors/hott_sensors.cpp b/src/drivers/hott/hott_sensors/hott_sensors.cpp index ad7e74e62d..ada7f9fb77 100644 --- a/src/drivers/hott/hott_sensors/hott_sensors.cpp +++ b/src/drivers/hott/hott_sensors/hott_sensors.cpp @@ -55,7 +55,7 @@ #include "../comms.h" #include "../messages.h" -#define DEFAULT_UART "/dev/ttyS2"; /**< USART5 */ +#define DEFAULT_UART "/dev/ttyS0"; /**< USART1 */ /* Oddly, ERROR is not defined for C++ */ #ifdef ERROR diff --git a/src/drivers/hott/hott_telemetry/hott_telemetry.cpp b/src/drivers/hott/hott_telemetry/hott_telemetry.cpp index 1c68e06b1e..042d9f8162 100644 --- a/src/drivers/hott/hott_telemetry/hott_telemetry.cpp +++ b/src/drivers/hott/hott_telemetry/hott_telemetry.cpp @@ -57,7 +57,7 @@ #include "../comms.h" #include "../messages.h" -#define DEFAULT_UART "/dev/ttyS1"; /**< USART2 */ +#define DEFAULT_UART "/dev/ttyS0"; /**< USART1 */ /* Oddly, ERROR is not defined for C++ */ #ifdef ERROR @@ -114,41 +114,6 @@ recv_req_id(int uart, uint8_t *id) return OK; } -int -recv_data(int uart, uint8_t *buffer, size_t *size, uint8_t *id) -{ - usleep(5000); - - static const int timeout_ms = 1000; - - struct pollfd fds; - fds.fd = uart; - fds.events = POLLIN; - - // XXX should this poll be inside the while loop??? - if (poll(&fds, 1, timeout_ms) > 0) { - int i = 0; - bool stop_byte_read = false; - while (true) { - read(uart, &buffer[i], sizeof(buffer[i])); - //printf("[%d]: %d\n", i, buffer[i]); - - if (stop_byte_read) { - // process checksum - *size = ++i; - return OK; - } - // XXX can some other field not have the STOP BYTE value? - if (buffer[i] == STOP_BYTE) { - *id = buffer[1]; - stop_byte_read = true; - } - i++; - } - } - return ERROR; -} - int send_data(int uart, uint8_t *buffer, size_t size) { diff --git a/src/drivers/hott/messages.cpp b/src/drivers/hott/messages.cpp index c5d73ab11d..57c2563394 100644 --- a/src/drivers/hott/messages.cpp +++ b/src/drivers/hott/messages.cpp @@ -61,10 +61,9 @@ static int _sensor_sub = -1; static int _airspeed_sub = -1; static int _esc_sub = -1; -orb_advert_t _esc_pub; +static orb_advert_t _esc_pub; struct esc_status_s _esc; - static bool _home_position_set = false; static double _home_lat = 0.0d; static double _home_lon = 0.0d; From 6b631afaef65ba874373b1dd1652f02a7f6e3612 Mon Sep 17 00:00:00 2001 From: Simon Wilks Date: Sun, 14 Jul 2013 00:04:17 +0200 Subject: [PATCH 22/22] Reduce the max stack size needed. --- src/drivers/hott/hott_sensors/hott_sensors.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/drivers/hott/hott_sensors/hott_sensors.cpp b/src/drivers/hott/hott_sensors/hott_sensors.cpp index ada7f9fb77..e322c6349e 100644 --- a/src/drivers/hott/hott_sensors/hott_sensors.cpp +++ b/src/drivers/hott/hott_sensors/hott_sensors.cpp @@ -212,7 +212,7 @@ hott_sensors_main(int argc, char *argv[]) deamon_task = task_spawn_cmd(daemon_name, SCHED_DEFAULT, SCHED_PRIORITY_MAX - 40, - 2048, + 1024, hott_sensors_thread_main, (argv) ? (const char **)&argv[2] : (const char **)NULL); exit(0);