From 0e2db0beb9228720a40bd19a7bd8891e5a8fdaba Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Sat, 16 Feb 2013 13:40:09 -0800 Subject: [PATCH 001/109] Checkpoint: implement new state machine, compiling, WIP --- apps/commander/commander.c | 396 +++++++------ apps/commander/state_machine_helper.c | 518 +++++++++++------- apps/commander/state_machine_helper.h | 157 +----- apps/controllib/fixedwing.cpp | 33 +- apps/drivers/blinkm/blinkm.cpp | 37 +- .../fixedwing_att_control_main.c | 163 +++--- apps/mavlink/mavlink.c | 122 ++--- apps/mavlink/orb_listener.c | 2 +- apps/mavlink_onboard/mavlink.c | 110 ++-- .../multirotor_att_control_main.c | 84 +-- apps/sensors/sensors.cpp | 108 ++-- apps/uORB/topics/manual_control_setpoint.h | 12 +- apps/uORB/topics/rc_channels.h | 22 +- apps/uORB/topics/vehicle_status.h | 92 ++-- 14 files changed, 920 insertions(+), 936 deletions(-) diff --git a/apps/commander/commander.c b/apps/commander/commander.c index 6b1bc0f9b5..8716caef72 100644 --- a/apps/commander/commander.c +++ b/apps/commander/commander.c @@ -124,19 +124,25 @@ extern struct system_load_s system_load; static int leds; static int buzzer; static int mavlink_fd; + +/* flags */ static bool commander_initialized = false; -static struct vehicle_status_s current_status; /**< Main state machine */ -static orb_advert_t stat_pub; - -// static uint16_t nofix_counter = 0; -// static uint16_t gotfix_counter = 0; - -static unsigned int failsafe_lowlevel_timeout_ms; - static bool thread_should_exit = false; /**< daemon exit flag */ static bool thread_running = false; /**< daemon status flag */ static int daemon_task; /**< Handle of daemon task / thread */ +/* Main state machine */ +static struct vehicle_status_s current_status; +static orb_advert_t stat_pub; + +/* XXX ? */ +// static uint16_t nofix_counter = 0; +// static uint16_t gotfix_counter = 0; + +/* XXX ? */ +static unsigned int failsafe_lowlevel_timeout_ms; + + /* pthread loops */ static void *orb_receive_loop(void *arg); @@ -252,18 +258,19 @@ enum AUDIO_PATTERN { int trigger_audio_alarm(uint8_t old_mode, uint8_t old_state, uint8_t new_mode, uint8_t new_state) { +#warning add alarms for state transitions /* Trigger alarm if going into any error state */ - if (((new_state == SYSTEM_STATE_GROUND_ERROR) && (old_state != SYSTEM_STATE_GROUND_ERROR)) || - ((new_state == SYSTEM_STATE_MISSION_ABORT) && (old_state != SYSTEM_STATE_MISSION_ABORT))) { - ioctl(buzzer, TONE_SET_ALARM, 0); - ioctl(buzzer, TONE_SET_ALARM, AUDIO_PATTERN_ERROR); - } +// if (((new_state == SYSTEM_STATE_GROUND_ERROR) && (old_state != SYSTEM_STATE_GROUND_ERROR)) || +// ((new_state == SYSTEM_STATE_MISSION_ABORT) && (old_state != SYSTEM_STATE_MISSION_ABORT))) { +// ioctl(buzzer, TONE_SET_ALARM, 0); +// ioctl(buzzer, TONE_SET_ALARM, AUDIO_PATTERN_ERROR); +// } /* Trigger neutral on arming / disarming */ - if (((new_state == SYSTEM_STATE_GROUND_READY) && (old_state != SYSTEM_STATE_GROUND_READY))) { - ioctl(buzzer, TONE_SET_ALARM, 0); - ioctl(buzzer, TONE_SET_ALARM, AUDIO_PATTERN_NOTIFY_NEUTRAL); - } +// if (((new_state == SYSTEM_STATE_GROUND_READY) && (old_state != SYSTEM_STATE_GROUND_READY))) { +// ioctl(buzzer, TONE_SET_ALARM, 0); +// ioctl(buzzer, TONE_SET_ALARM, AUDIO_PATTERN_NOTIFY_NEUTRAL); +// } /* Trigger Tetris on being bored */ @@ -277,6 +284,7 @@ void tune_confirm(void) void tune_error(void) { + /* XXX change alarm to 2 tones*/ ioctl(buzzer, TONE_SET_ALARM, 4); } @@ -795,19 +803,21 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta /* request to set different system mode */ switch (cmd->command) { case VEHICLE_CMD_DO_SET_MODE: { - if (OK == update_state_machine_mode_request(status_pub, current_vehicle_status, mavlink_fd, (uint8_t)cmd->param1)) { - result = VEHICLE_CMD_RESULT_ACCEPTED; + // XXX implement this - } else { +// if (OK == update_state_machine_mode_request(status_pub, current_vehicle_status, mavlink_fd, (uint8_t)cmd->param1)) { +// result = VEHICLE_CMD_RESULT_ACCEPTED; +// +// } else { result = VEHICLE_CMD_RESULT_DENIED; - } +// } } break; case VEHICLE_CMD_COMPONENT_ARM_DISARM: { /* request to arm */ if ((int)cmd->param1 == 1) { - if (OK == update_state_machine_mode_request(status_pub, current_vehicle_status, mavlink_fd, VEHICLE_MODE_FLAG_SAFETY_ARMED)) { + if (OK == do_arming_state_update(status_pub, current_vehicle_status, mavlink_fd, ARMING_STATE_ARMED)) { result = VEHICLE_CMD_RESULT_ACCEPTED; } else { @@ -815,9 +825,9 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta } /* request to disarm */ - + // XXX this arms it instad of disarming } else if ((int)cmd->param1 == 0) { - if (OK == update_state_machine_mode_request(status_pub, current_vehicle_status, mavlink_fd, VEHICLE_MODE_FLAG_SAFETY_ARMED)) { + if (OK == do_arming_state_update(status_pub, current_vehicle_status, mavlink_fd, ARMING_STATE_STANDBY)) { result = VEHICLE_CMD_RESULT_ACCEPTED; } else { @@ -830,7 +840,7 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta /* request for an autopilot reboot */ case VEHICLE_CMD_PREFLIGHT_REBOOT_SHUTDOWN: { if ((int)cmd->param1 == 1) { - if (OK == do_state_update(status_pub, current_vehicle_status, mavlink_fd, SYSTEM_STATE_REBOOT)) { + if (OK == do_navigation_state_update(status_pub, current_vehicle_status, mavlink_fd, NAVIGATION_STATE_REBOOT)) { /* SPECIAL CASE: SYSTEM WILL NEVER RETURN HERE */ result = VEHICLE_CMD_RESULT_ACCEPTED; @@ -871,16 +881,21 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta /* gyro calibration */ if ((int)(cmd->param1) == 1) { - /* transition to calibration state */ - do_state_update(status_pub, ¤t_status, mavlink_fd, SYSTEM_STATE_PREFLIGHT); - if (current_status.state_machine == SYSTEM_STATE_PREFLIGHT) { + /* transition to init state */ + if (OK == do_arming_state_update(status_pub, ¤t_status, mavlink_fd, ARMING_STATE_INIT) + && OK == do_navigation_state_update(status_pub, ¤t_status, mavlink_fd, NAVIGATION_STATE_INIT)) { + mavlink_log_info(mavlink_fd, "starting gyro cal"); tune_confirm(); do_gyro_calibration(status_pub, ¤t_status); mavlink_log_info(mavlink_fd, "finished gyro cal"); tune_confirm(); - do_state_update(status_pub, ¤t_status, mavlink_fd, SYSTEM_STATE_STANDBY); + + /* back to standby state */ + do_arming_state_update(status_pub, ¤t_status, mavlink_fd, ARMING_STATE_STANDBY); + do_navigation_state_update(status_pub, ¤t_status, mavlink_fd, NAVIGATION_STATE_STANDBY); + result = VEHICLE_CMD_RESULT_ACCEPTED; } else { @@ -893,16 +908,20 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta /* magnetometer calibration */ if ((int)(cmd->param2) == 1) { - /* transition to calibration state */ - do_state_update(status_pub, ¤t_status, mavlink_fd, SYSTEM_STATE_PREFLIGHT); - if (current_status.state_machine == SYSTEM_STATE_PREFLIGHT) { + /* transition to init state */ + if (OK == do_arming_state_update(status_pub, ¤t_status, mavlink_fd, ARMING_STATE_INIT) + && OK == do_navigation_state_update(status_pub, ¤t_status, mavlink_fd, NAVIGATION_STATE_INIT)) { mavlink_log_info(mavlink_fd, "starting mag cal"); tune_confirm(); do_mag_calibration(status_pub, ¤t_status); mavlink_log_info(mavlink_fd, "finished mag cal"); tune_confirm(); - do_state_update(status_pub, ¤t_status, mavlink_fd, SYSTEM_STATE_STANDBY); + + /* back to standby state */ + do_arming_state_update(status_pub, ¤t_status, mavlink_fd, ARMING_STATE_STANDBY); + do_navigation_state_update(status_pub, ¤t_status, mavlink_fd, NAVIGATION_STATE_STANDBY); + result = VEHICLE_CMD_RESULT_ACCEPTED; } else { @@ -916,12 +935,17 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta /* zero-altitude pressure calibration */ if ((int)(cmd->param3) == 1) { /* transition to calibration state */ - do_state_update(status_pub, ¤t_status, mavlink_fd, SYSTEM_STATE_PREFLIGHT); + if (OK == do_arming_state_update(status_pub, ¤t_status, mavlink_fd, ARMING_STATE_INIT) + && OK == do_navigation_state_update(status_pub, ¤t_status, mavlink_fd, NAVIGATION_STATE_INIT)) { - if (current_status.state_machine == SYSTEM_STATE_PREFLIGHT) { + // XXX implement this mavlink_log_info(mavlink_fd, "zero altitude cal. not implemented"); tune_confirm(); + /* back to standby state */ + do_arming_state_update(status_pub, ¤t_status, mavlink_fd, ARMING_STATE_STANDBY); + do_navigation_state_update(status_pub, ¤t_status, mavlink_fd, NAVIGATION_STATE_STANDBY); + } else { mavlink_log_critical(mavlink_fd, "REJECTING altitude calibration"); result = VEHICLE_CMD_RESULT_DENIED; @@ -933,15 +957,18 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta /* trim calibration */ if ((int)(cmd->param4) == 1) { /* transition to calibration state */ - do_state_update(status_pub, ¤t_status, mavlink_fd, SYSTEM_STATE_PREFLIGHT); - - if (current_status.state_machine == SYSTEM_STATE_PREFLIGHT) { + if (OK == do_arming_state_update(status_pub, ¤t_status, mavlink_fd, ARMING_STATE_INIT) + && OK == do_navigation_state_update(status_pub, ¤t_status, mavlink_fd, NAVIGATION_STATE_INIT)) { mavlink_log_info(mavlink_fd, "starting trim cal"); tune_confirm(); do_rc_calibration(status_pub, ¤t_status); mavlink_log_info(mavlink_fd, "finished trim cal"); tune_confirm(); - do_state_update(status_pub, ¤t_status, mavlink_fd, SYSTEM_STATE_STANDBY); + + /* back to standby state */ + do_arming_state_update(status_pub, ¤t_status, mavlink_fd, ARMING_STATE_STANDBY); + do_navigation_state_update(status_pub, ¤t_status, mavlink_fd, NAVIGATION_STATE_STANDBY); + result = VEHICLE_CMD_RESULT_ACCEPTED; } else { @@ -954,16 +981,19 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta /* accel calibration */ if ((int)(cmd->param5) == 1) { - /* transition to calibration state */ - do_state_update(status_pub, ¤t_status, mavlink_fd, SYSTEM_STATE_PREFLIGHT); + if (OK == do_arming_state_update(status_pub, ¤t_status, mavlink_fd, ARMING_STATE_INIT) + && OK == do_navigation_state_update(status_pub, ¤t_status, mavlink_fd, NAVIGATION_STATE_INIT)) { - if (current_status.state_machine == SYSTEM_STATE_PREFLIGHT) { mavlink_log_info(mavlink_fd, "CMD starting accel cal"); tune_confirm(); do_accel_calibration(status_pub, ¤t_status); tune_confirm(); mavlink_log_info(mavlink_fd, "CMD finished accel cal"); - do_state_update(status_pub, ¤t_status, mavlink_fd, SYSTEM_STATE_STANDBY); + + /* back to standby state */ + do_arming_state_update(status_pub, ¤t_status, mavlink_fd, ARMING_STATE_STANDBY); + do_navigation_state_update(status_pub, ¤t_status, mavlink_fd, NAVIGATION_STATE_STANDBY); + result = VEHICLE_CMD_RESULT_ACCEPTED; } else { @@ -1262,7 +1292,7 @@ int commander_thread_main(int argc, char *argv[]) param_t _param_sys_type = param_find("MAV_TYPE"); /* welcome user */ - warnx("I am in command now!\n"); + warnx("[commander] starting"); /* pthreads for command and subsystem info handling */ // pthread_t command_handling_thread; @@ -1270,33 +1300,39 @@ int commander_thread_main(int argc, char *argv[]) /* initialize */ if (led_init() != 0) { - warnx("ERROR: Failed to initialize leds\n"); + warnx("ERROR: Failed to initialize leds"); } if (buzzer_init() != 0) { - warnx("ERROR: Failed to initialize buzzer\n"); + warnx("ERROR: Failed to initialize buzzer"); } mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); if (mavlink_fd < 0) { - warnx("ERROR: Failed to open MAVLink log stream, start mavlink app first.\n"); + warnx("ERROR: Failed to open MAVLink log stream, start mavlink app first."); } /* make sure we are in preflight state */ memset(¤t_status, 0, sizeof(current_status)); - current_status.state_machine = SYSTEM_STATE_PREFLIGHT; + current_status.navigation_state = NAVIGATION_STATE_INIT; + current_status.arming_state = ARMING_STATE_INIT; current_status.flag_system_armed = false; + /* neither manual nor offboard control commands have been received */ current_status.offboard_control_signal_found_once = false; current_status.rc_signal_found_once = false; + /* mark all signals lost as long as they haven't been found */ current_status.rc_signal_lost = true; current_status.offboard_control_signal_lost = true; + /* allow manual override initially */ current_status.flag_external_manual_override_ok = true; + /* flag position info as bad, do not allow auto mode */ current_status.flag_vector_flight_mode_ok = false; + /* set battery warning flag */ current_status.battery_warning = VEHICLE_BATTERY_WARNING_NONE; @@ -1315,6 +1351,7 @@ int commander_thread_main(int argc, char *argv[]) exit(ERROR); } + // XXX needed? mavlink_log_info(mavlink_fd, "system is running"); /* create pthreads */ @@ -1325,7 +1362,6 @@ int commander_thread_main(int argc, char *argv[]) /* Start monitoring loop */ uint16_t counter = 0; - uint8_t flight_env; /* Initialize to 0.0V */ float battery_voltage = 0.0f; @@ -1350,11 +1386,13 @@ int commander_thread_main(int argc, char *argv[]) struct offboard_control_setpoint_s sp_offboard; memset(&sp_offboard, 0, sizeof(sp_offboard)); + /* Subscribe to global position */ int global_position_sub = orb_subscribe(ORB_ID(vehicle_global_position)); struct vehicle_global_position_s global_position; memset(&global_position, 0, sizeof(global_position)); uint64_t last_global_position_time = 0; + /* Subscribe to local position data */ int local_position_sub = orb_subscribe(ORB_ID(vehicle_local_position)); struct vehicle_local_position_s local_position; memset(&local_position, 0, sizeof(local_position)); @@ -1365,10 +1403,13 @@ int commander_thread_main(int argc, char *argv[]) * position estimator and commander. RAW GPS is more than good enough for a * non-flying vehicle. */ + + /* Subscribe to GPS topic */ int gps_sub = orb_subscribe(ORB_ID(vehicle_gps_position)); struct vehicle_gps_position_s gps_position; memset(&gps_position, 0, sizeof(gps_position)); + /* Subscribe to sensor topic */ int sensor_sub = orb_subscribe(ORB_ID(sensor_combined)); struct sensor_combined_s sensors; memset(&sensors, 0, sizeof(sensors)); @@ -1502,45 +1543,36 @@ int commander_thread_main(int argc, char *argv[]) /* Slow but important 8 Hz checks */ if (counter % ((1000000 / COMMANDER_MONITORING_INTERVAL) / 8) == 0) { /* toggle activity (blue) led at 1 Hz in standby, 10 Hz in armed mode */ - if ((current_status.state_machine == SYSTEM_STATE_GROUND_READY || - current_status.state_machine == SYSTEM_STATE_AUTO || - current_status.state_machine == SYSTEM_STATE_MANUAL)) { - /* armed */ - led_toggle(LED_BLUE); +// if ((current_status.state_machine == SYSTEM_STATE_GROUND_READY || +// current_status.state_machine == SYSTEM_STATE_AUTO || +// current_status.state_machine == SYSTEM_STATE_MANUAL)) { +// /* armed */ +// led_toggle(LED_BLUE); - } else if (counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0) { - /* not armed */ - led_toggle(LED_BLUE); - } + } else if (counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0) { + /* not armed */ + led_toggle(LED_BLUE); + } - /* toggle error led at 5 Hz in HIL mode */ - if (current_status.flag_hil_enabled) { - /* hil enabled */ - led_toggle(LED_AMBER); + /* toggle error led at 5 Hz in HIL mode */ + if (current_status.flag_hil_enabled) { + /* hil enabled */ + led_toggle(LED_AMBER); - } else if (bat_remain < 0.3f && (low_voltage_counter > LOW_VOLTAGE_BATTERY_COUNTER_LIMIT)) { - /* toggle error (red) at 5 Hz on low battery or error */ - led_toggle(LED_AMBER); + } else if (bat_remain < 0.3f && (low_voltage_counter > LOW_VOLTAGE_BATTERY_COUNTER_LIMIT)) { + /* toggle error (red) at 5 Hz on low battery or error */ + led_toggle(LED_AMBER); - } else { - // /* Constant error indication in standby mode without GPS */ - // if (!current_status.gps_valid) { - // led_on(LED_AMBER); + } - // } else { - // led_off(LED_AMBER); - // } - } + if (counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0) { + /* compute system load */ + uint64_t interval_runtime = system_load.tasks[0].total_runtime - last_idle_time; - if (counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0) { - /* compute system load */ - uint64_t interval_runtime = system_load.tasks[0].total_runtime - last_idle_time; + if (last_idle_time > 0) + current_status.load = 1000 - (interval_runtime / 1000); //system load is time spent in non-idle - if (last_idle_time > 0) - current_status.load = 1000 - (interval_runtime / 1000); //system load is time spent in non-idle - - last_idle_time = system_load.tasks[0].total_runtime; - } + last_idle_time = system_load.tasks[0].total_runtime; } // // XXX Export patterns and threshold to parameters @@ -1583,13 +1615,14 @@ int commander_thread_main(int argc, char *argv[]) low_voltage_counter++; } - /* Critical, this is rather an emergency, kill signal to sdlog and change state machine */ + /* Critical, this is rather an emergency, change state machine */ else if (battery_voltage_valid && (bat_remain < 0.1f /* XXX MAGIC NUMBER */) && (false == critical_battery_voltage_actions_done && true == low_battery_voltage_actions_done)) { if (critical_voltage_counter > CRITICAL_VOLTAGE_BATTERY_COUNTER_LIMIT) { critical_battery_voltage_actions_done = true; mavlink_log_critical(mavlink_fd, "[cmd] EMERGENCY! CRITICAL BATTERY!"); current_status.battery_warning = VEHICLE_BATTERY_WARNING_ALERT; - state_machine_emergency(stat_pub, ¤t_status, mavlink_fd); + // XXX implement this +// state_machine_emergency(stat_pub, ¤t_status, mavlink_fd); } critical_voltage_counter++; @@ -1698,7 +1731,7 @@ int commander_thread_main(int argc, char *argv[]) if (gps_position.fix_type == GPS_FIX_TYPE_3D && (hdop_m < hdop_threshold_m) - && (vdop_m < vdop_threshold_m) + && (vdop_m < vdop_threshold_m) // XXX note that vdop is 0 for mtk && !home_position_set && (hrt_absolute_time() - gps_position.timestamp_position < 2000000) && !current_status.flag_system_armed) { @@ -1734,85 +1767,112 @@ int commander_thread_main(int argc, char *argv[]) if ((hrt_absolute_time() - sp_man.timestamp) < 100000) { - // /* - // * Check if manual control modes have to be switched - // */ - // if (!isfinite(sp_man.manual_mode_switch)) { - // warnx("man mode sw not finite\n"); + /* + * Check if manual control modes have to be switched + */ + if (!isfinite(sp_man.mode_switch)) { + warnx("mode sw not finite"); - // /* this switch is not properly mapped, set default */ - // if ((current_status.system_type == VEHICLE_TYPE_QUADROTOR) || - // (current_status.system_type == VEHICLE_TYPE_HEXAROTOR) || - // (current_status.system_type == VEHICLE_TYPE_OCTOROTOR)) { + /* this switch is not properly mapped, set attitude stabilized */ + if ((current_status.system_type == VEHICLE_TYPE_QUADROTOR) || + (current_status.system_type == VEHICLE_TYPE_HEXAROTOR) || + (current_status.system_type == VEHICLE_TYPE_OCTOROTOR)) { - // /* assuming a rotary wing, fall back to SAS */ - // current_status.manual_control_mode = VEHICLE_MANUAL_CONTROL_MODE_SAS; - // current_status.flag_control_attitude_enabled = true; - // current_status.flag_control_rates_enabled = true; - // } else { + current_status.flag_control_attitude_enabled = true; + current_status.flag_control_rates_enabled = true; + } else { - // /* assuming a fixed wing, fall back to direct pass-through */ - // current_status.manual_control_mode = VEHICLE_MANUAL_CONTROL_MODE_DIRECT; - // current_status.flag_control_attitude_enabled = false; - // current_status.flag_control_rates_enabled = false; - // } + /* assuming a fixed wing, fall back to direct pass-through */ + current_status.flag_control_attitude_enabled = false; + current_status.flag_control_rates_enabled = false; + } - // } else if (sp_man.manual_mode_switch < -STICK_ON_OFF_LIMIT) { + } else if (sp_man.mode_switch < -STICK_ON_OFF_LIMIT) { - // /* bottom stick position, set direct mode for vehicles supporting it */ - // if ((current_status.system_type == VEHICLE_TYPE_QUADROTOR) || - // (current_status.system_type == VEHICLE_TYPE_HEXAROTOR) || - // (current_status.system_type == VEHICLE_TYPE_OCTOROTOR)) { + /* this switch is not properly mapped, set attitude stabilized */ + if ((current_status.system_type == VEHICLE_TYPE_QUADROTOR) || + (current_status.system_type == VEHICLE_TYPE_HEXAROTOR) || + (current_status.system_type == VEHICLE_TYPE_OCTOROTOR)) { - // /* assuming a rotary wing, fall back to SAS */ - // current_status.manual_control_mode = VEHICLE_MANUAL_CONTROL_MODE_SAS; - // current_status.flag_control_attitude_enabled = true; - // current_status.flag_control_rates_enabled = true; - // } else { + /* assuming a rotary wing, fall back to m */ + current_status.flag_control_attitude_enabled = true; + current_status.flag_control_rates_enabled = true; + } else { - // /* assuming a fixed wing, set to direct pass-through as requested */ - // current_status.manual_control_mode = VEHICLE_MANUAL_CONTROL_MODE_DIRECT; - // current_status.flag_control_attitude_enabled = false; - // current_status.flag_control_rates_enabled = false; - // } + /* assuming a fixed wing, set to direct pass-through as requested */ + current_status.flag_control_attitude_enabled = false; + current_status.flag_control_rates_enabled = false; + } - // } else if (sp_man.manual_mode_switch > STICK_ON_OFF_LIMIT) { + } else if (sp_man.mode_switch > STICK_ON_OFF_LIMIT) { - // /* top stick position, set SAS for all vehicle types */ - // current_status.manual_control_mode = VEHICLE_MANUAL_CONTROL_MODE_SAS; - // current_status.flag_control_attitude_enabled = true; - // current_status.flag_control_rates_enabled = true; + /* top stick position, set auto/mission for all vehicle types */ + current_status.flag_control_position_enabled = true; + current_status.flag_control_velocity_enabled = true; + current_status.flag_control_attitude_enabled = true; + current_status.flag_control_rates_enabled = true; - // } else { - // /* center stick position, set rate control */ - // current_status.manual_control_mode = VEHICLE_MANUAL_CONTROL_MODE_RATES; - // current_status.flag_control_attitude_enabled = false; - // current_status.flag_control_rates_enabled = true; - // } + } else { + /* center stick position, set seatbelt/simple control */ + current_status.flag_control_velocity_enabled = true; + current_status.flag_control_attitude_enabled = true; + current_status.flag_control_rates_enabled = true; + } // warnx("man ctrl mode: %d\n", (int)current_status.manual_control_mode); /* - * Check if manual stability control modes have to be switched + * Check if land/RTL is requested */ - if (!isfinite(sp_man.manual_sas_switch)) { + if (!isfinite(sp_man.return_switch)) { /* this switch is not properly mapped, set default */ - current_status.manual_sas_mode = VEHICLE_MANUAL_SAS_MODE_ROLL_PITCH_ABS_YAW_ABS; + current_status.flag_land_requested = false; // XXX default? - } else if (sp_man.manual_sas_switch < -STICK_ON_OFF_LIMIT) { + } else if (sp_man.return_switch < -STICK_ON_OFF_LIMIT) { /* bottom stick position, set altitude hold */ - current_status.manual_sas_mode = VEHICLE_MANUAL_SAS_MODE_ALTITUDE; + current_status.flag_land_requested = false; - } else if (sp_man.manual_sas_switch > STICK_ON_OFF_LIMIT) { + } else if (sp_man.return_switch > STICK_ON_OFF_LIMIT) { /* top stick position */ - current_status.manual_sas_mode = VEHICLE_MANUAL_SAS_MODE_SIMPLE; + current_status.flag_land_requested = true; } else { /* center stick position, set default */ - current_status.manual_sas_mode = VEHICLE_MANUAL_SAS_MODE_ROLL_PITCH_ABS_YAW_ABS; + current_status.flag_land_requested = false; // XXX default? + } + + /* check mission switch */ + if (!isfinite(sp_man.mission_switch)) { + + /* this switch is not properly mapped, set default */ + current_status.flag_mission_activated = false; + + } else if (sp_man.mission_switch > STICK_ON_OFF_LIMIT) { + + /* top switch position */ + current_status.flag_mission_activated = true; + + } else if (sp_man.mission_switch < -STICK_ON_OFF_LIMIT) { + + /* bottom switch position */ + current_status.flag_mission_activated = false; + + } else { + + /* center switch position, set default */ + current_status.flag_mission_activated = false; // XXX default? + } + + /* handle the case where RC signal was regained */ + if (!current_status.rc_signal_found_once) { + current_status.rc_signal_found_once = true; + mavlink_log_critical(mavlink_fd, "DETECTED RC SIGNAL FIRST TIME."); + + } else { + if (current_status.rc_signal_lost) mavlink_log_critical(mavlink_fd, "[cmd] RECOVERY - RC SIGNAL GAINED!"); } /* @@ -1826,7 +1886,7 @@ int commander_thread_main(int argc, char *argv[]) ((sp_man.yaw < -STICK_ON_OFF_LIMIT)) && (sp_man.throttle < STICK_THRUST_RANGE * 0.2f)) { if (stick_off_counter > STICK_ON_OFF_COUNTER_LIMIT) { - update_state_machine_disarm(stat_pub, ¤t_status, mavlink_fd); + current_status.flag_system_armed = false; stick_on_counter = 0; } else { @@ -1838,7 +1898,7 @@ 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 (stick_on_counter > STICK_ON_OFF_COUNTER_LIMIT) { - update_state_machine_arm(stat_pub, ¤t_status, mavlink_fd); + current_status.flag_system_armed = true; stick_on_counter = 0; } else { @@ -1847,37 +1907,6 @@ int commander_thread_main(int argc, char *argv[]) } } - /* check manual override switch - switch to manual or auto mode */ - if (sp_man.manual_override_switch > STICK_ON_OFF_LIMIT) { - /* enable manual override */ - update_state_machine_mode_manual(stat_pub, ¤t_status, mavlink_fd); - - } else if (sp_man.manual_override_switch < -STICK_ON_OFF_LIMIT) { - // /* check auto mode switch for correct mode */ - // if (sp_man.auto_mode_switch > STICK_ON_OFF_LIMIT) { - // /* enable guided mode */ - // update_state_machine_mode_guided(stat_pub, ¤t_status, mavlink_fd); - - // } else if (sp_man.auto_mode_switch < -STICK_ON_OFF_LIMIT) { - // XXX hardcode to auto for now - update_state_machine_mode_auto(stat_pub, ¤t_status, mavlink_fd); - - // } - - } else { - /* center stick position, set SAS for all vehicle types */ - update_state_machine_mode_stabilized(stat_pub, ¤t_status, mavlink_fd); - } - - /* handle the case where RC signal was regained */ - if (!current_status.rc_signal_found_once) { - current_status.rc_signal_found_once = true; - mavlink_log_critical(mavlink_fd, "DETECTED RC SIGNAL FIRST TIME."); - - } else { - if (current_status.rc_signal_lost) mavlink_log_critical(mavlink_fd, "[cmd] RECOVERY - RC SIGNAL GAINED!"); - } - current_status.rc_signal_cutting_off = false; current_status.rc_signal_lost = false; current_status.rc_signal_lost_interval = 0; @@ -1965,12 +1994,13 @@ int commander_thread_main(int argc, char *argv[]) /* arm / disarm on request */ if (sp_offboard.armed && !current_status.flag_system_armed) { - update_state_machine_arm(stat_pub, ¤t_status, mavlink_fd); +#warning fix this +// update_state_machine_arm(stat_pub, ¤t_status, mavlink_fd); /* switch to stabilized mode = takeoff */ - update_state_machine_mode_stabilized(stat_pub, ¤t_status, mavlink_fd); +// update_state_machine_mode_stabilized(stat_pub, ¤t_status, mavlink_fd); } else if (!sp_offboard.armed && current_status.flag_system_armed) { - update_state_machine_disarm(stat_pub, ¤t_status, mavlink_fd); +// update_state_machine_disarm(stat_pub, ¤t_status, mavlink_fd); } } else { @@ -2006,18 +2036,20 @@ int commander_thread_main(int argc, char *argv[]) current_status.timestamp = hrt_absolute_time(); + // XXX what is this? /* If full run came back clean, transition to standby */ - if (current_status.state_machine == SYSTEM_STATE_PREFLIGHT && - current_status.flag_preflight_gyro_calibration == false && - current_status.flag_preflight_mag_calibration == false && - current_status.flag_preflight_accel_calibration == false) { - /* All ok, no calibration going on, go to standby */ - do_state_update(stat_pub, ¤t_status, mavlink_fd, SYSTEM_STATE_STANDBY); - } +// if (current_status.state_machine == SYSTEM_STATE_PREFLIGHT && +// current_status.flag_preflight_gyro_calibration == false && +// current_status.flag_preflight_mag_calibration == false && +// current_status.flag_preflight_accel_calibration == false) { +// /* All ok, no calibration going on, go to standby */ +// do_state_update(stat_pub, ¤t_status, mavlink_fd, SYSTEM_STATE_STANDBY); +// } /* publish at least with 1 Hz */ if (counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0 || state_changed) { - publish_armed_status(¤t_status); +#warning fix this +// publish_armed_status(¤t_status); orb_publish(ORB_ID(vehicle_status), stat_pub, ¤t_status); state_changed = false; } @@ -2043,7 +2075,7 @@ int commander_thread_main(int argc, char *argv[]) close(sensor_sub); close(cmd_sub); - warnx("exiting..\n"); + warnx("exiting"); fflush(stdout); thread_running = false; diff --git a/apps/commander/state_machine_helper.c b/apps/commander/state_machine_helper.c index bea388a101..e1ec73110c 100644 --- a/apps/commander/state_machine_helper.c +++ b/apps/commander/state_machine_helper.c @@ -40,182 +40,297 @@ #include #include +#include #include #include #include #include +#include #include #include #include "state_machine_helper.h" -static const char *system_state_txt[] = { - "SYSTEM_STATE_PREFLIGHT", - "SYSTEM_STATE_STANDBY", - "SYSTEM_STATE_GROUND_READY", - "SYSTEM_STATE_MANUAL", - "SYSTEM_STATE_STABILIZED", - "SYSTEM_STATE_AUTO", - "SYSTEM_STATE_MISSION_ABORT", - "SYSTEM_STATE_EMCY_LANDING", - "SYSTEM_STATE_EMCY_CUTOFF", - "SYSTEM_STATE_GROUND_ERROR", - "SYSTEM_STATE_REBOOT", - -}; /** - * Transition from one state to another + * Transition from one navigation state to another */ -int do_state_update(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, commander_state_machine_t new_state) +int do_navigation_state_update(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, navigation_state_t new_state) { - int invalid_state = false; + bool valid_transition = false; int ret = ERROR; - commander_state_machine_t old_state = current_status->state_machine; + if (current_status->navigation_state == new_state) { + warnx("Navigation state not changed"); - switch (new_state) { - case SYSTEM_STATE_MISSION_ABORT: { - /* Indoor or outdoor */ - // if (flight_environment_parameter == PX4_FLIGHT_ENVIRONMENT_OUTDOOR) { - ret = do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_EMCY_LANDING); + } else { - // } else { - // ret = do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_EMCY_CUTOFF); - // } + switch (new_state) { + case NAVIGATION_STATE_INIT: + + if (current_status->navigation_state == NAVIGATION_STATE_STANDBY) { + + mavlink_log_critical(mavlink_fd, "SWITCHED TO INIT STATE"); + valid_transition = true; + } + break; + + case NAVIGATION_STATE_STANDBY: + + if (current_status->navigation_state == NAVIGATION_STATE_INIT + || current_status->navigation_state == NAVIGATION_STATE_MANUAL + || current_status->navigation_state == NAVIGATION_STATE_AUTO_READY) { + + mavlink_log_critical(mavlink_fd, "SWITCHED TO STANDBY STATE"); + valid_transition = true; + } + break; + + case NAVIGATION_STATE_MANUAL: + + if ( + ( current_status->navigation_state == NAVIGATION_STATE_STANDBY + || current_status->navigation_state == NAVIGATION_STATE_SEATBELT + || current_status->navigation_state == NAVIGATION_STATE_LOITER + || current_status->navigation_state == NAVIGATION_STATE_MISSION + || current_status->navigation_state == NAVIGATION_STATE_RTL + || current_status->navigation_state == NAVIGATION_STATE_LAND + || current_status->navigation_state == NAVIGATION_STATE_TAKEOFF + || current_status->navigation_state == NAVIGATION_STATE_AUTO_READY) + && current_status->arming_state == ARMING_STATE_ARMED) { + + mavlink_log_critical(mavlink_fd, "SWITCHED TO MANUAL STATE"); + valid_transition = true; + } + break; + + case NAVIGATION_STATE_SEATBELT: + + if (current_status->navigation_state == NAVIGATION_STATE_MANUAL + || current_status->navigation_state == NAVIGATION_STATE_LOITER + || current_status->navigation_state == NAVIGATION_STATE_MISSION) { + + mavlink_log_critical(mavlink_fd, "SWITCHED TO SEATBELT STATE"); + valid_transition = true; + } + break; + + case NAVIGATION_STATE_LOITER: + + if ( ((current_status->navigation_state == NAVIGATION_STATE_MANUAL || current_status->navigation_state == NAVIGATION_STATE_SEATBELT) + && current_status->flag_global_position_valid) + || current_status->navigation_state == NAVIGATION_STATE_MISSION) { + + mavlink_log_critical(mavlink_fd, "SWITCHED TO LOITER STATE"); + valid_transition = true; + } + break; + + case NAVIGATION_STATE_AUTO_READY: + + if ( + (current_status->navigation_state == NAVIGATION_STATE_STANDBY + && current_status->flag_global_position_valid + && current_status->flag_valid_launch_position) + || current_status->navigation_state == NAVIGATION_STATE_LAND) { + + mavlink_log_critical(mavlink_fd, "SWITCHED TO AUTO READY STATE"); + valid_transition = true; + } + break; + + case NAVIGATION_STATE_MISSION: + + if ( + current_status->navigation_state == NAVIGATION_STATE_TAKEOFF + || current_status->navigation_state == NAVIGATION_STATE_RTL + || ( + (current_status->navigation_state == NAVIGATION_STATE_MANUAL + || current_status->navigation_state == NAVIGATION_STATE_SEATBELT + || current_status->navigation_state == NAVIGATION_STATE_LOITER) + && current_status->flag_global_position_valid + && current_status->flag_valid_launch_position) + ) { + + mavlink_log_critical(mavlink_fd, "SWITCHED TO MISSION STATE"); + valid_transition = true; + } + break; + + case NAVIGATION_STATE_RTL: + + if ( + current_status->navigation_state == NAVIGATION_STATE_MISSION + || ( + (current_status->navigation_state == NAVIGATION_STATE_MANUAL + || current_status->navigation_state == NAVIGATION_STATE_SEATBELT + || current_status->navigation_state == NAVIGATION_STATE_LOITER) + && current_status->flag_global_position_valid + && current_status->flag_valid_launch_position) + ) { + + mavlink_log_critical(mavlink_fd, "SWITCHED TO RTL STATE"); + valid_transition = true; + } + break; + + case NAVIGATION_STATE_TAKEOFF: + + if (current_status->navigation_state == NAVIGATION_STATE_AUTO_READY) { + + mavlink_log_critical(mavlink_fd, "SWITCHED TO TAKEOFF STATE"); + valid_transition = true; + } + break; + + case NAVIGATION_STATE_LAND: + if (current_status->navigation_state == NAVIGATION_STATE_RTL + || current_status->navigation_state == NAVIGATION_STATE_LOITER) { + + mavlink_log_critical(mavlink_fd, "SWITCHED TO LAND STATE"); + valid_transition = true; + } + break; + + case NAVIGATION_STATE_REBOOT: + if (current_status->navigation_state == NAVIGATION_STATE_STANDBY + || current_status->navigation_state == NAVIGATION_STATE_INIT + || current_status->flag_hil_enabled) { + valid_transition = true; + /* set system flags according to state */ + current_status->flag_system_armed = false; + mavlink_log_critical(mavlink_fd, "REBOOTING SYSTEM"); + usleep(500000); + up_systemreset(); + /* SPECIAL CASE: NEVER RETURNS FROM THIS FUNCTION CALL */ + + } else { + mavlink_log_critical(mavlink_fd, "REFUSED to REBOOT"); + } + + break; + + default: + warnx("Unknown navigation state"); + break; } - break; - - case SYSTEM_STATE_EMCY_LANDING: - /* Tell the controller to land */ - - /* set system flags according to state */ - current_status->flag_system_armed = true; - - warnx("EMERGENCY LANDING!\n"); - mavlink_log_critical(mavlink_fd, "EMERGENCY LANDING!"); - break; - - case SYSTEM_STATE_EMCY_CUTOFF: - /* Tell the controller to cutoff the motors (thrust = 0) */ - - /* set system flags according to state */ - current_status->flag_system_armed = false; - - warnx("EMERGENCY MOTOR CUTOFF!\n"); - mavlink_log_critical(mavlink_fd, "EMERGENCY MOTOR CUTOFF!"); - break; - - case SYSTEM_STATE_GROUND_ERROR: - - /* set system flags according to state */ - - /* prevent actuators from arming */ - current_status->flag_system_armed = false; - - warnx("GROUND ERROR, locking down propulsion system\n"); - mavlink_log_critical(mavlink_fd, "GROUND ERROR, locking down system"); - break; - - case SYSTEM_STATE_PREFLIGHT: - if (current_status->state_machine == SYSTEM_STATE_STANDBY - || current_status->state_machine == SYSTEM_STATE_PREFLIGHT) { - /* set system flags according to state */ - current_status->flag_system_armed = false; - mavlink_log_critical(mavlink_fd, "Switched to PREFLIGHT state"); - - } else { - invalid_state = true; - mavlink_log_critical(mavlink_fd, "REFUSED to switch to PREFLIGHT state"); - } - - break; - - case SYSTEM_STATE_REBOOT: - if (current_status->state_machine == SYSTEM_STATE_STANDBY - || current_status->state_machine == SYSTEM_STATE_PREFLIGHT - || current_status->flag_hil_enabled) { - invalid_state = false; - /* set system flags according to state */ - current_status->flag_system_armed = false; - mavlink_log_critical(mavlink_fd, "REBOOTING SYSTEM"); - usleep(500000); - up_systemreset(); - /* SPECIAL CASE: NEVER RETURNS FROM THIS FUNCTION CALL */ - - } else { - invalid_state = true; - mavlink_log_critical(mavlink_fd, "REFUSED to REBOOT"); - } - - break; - - case SYSTEM_STATE_STANDBY: - /* set system flags according to state */ - - /* standby enforces disarmed */ - current_status->flag_system_armed = false; - - mavlink_log_critical(mavlink_fd, "Switched to STANDBY state"); - break; - - case SYSTEM_STATE_GROUND_READY: - - /* set system flags according to state */ - - /* ground ready has motors / actuators armed */ - current_status->flag_system_armed = true; - - mavlink_log_critical(mavlink_fd, "Switched to GROUND READY state"); - break; - - case SYSTEM_STATE_AUTO: - - /* set system flags according to state */ - - /* auto is airborne and in auto mode, motors armed */ - current_status->flag_system_armed = true; - - mavlink_log_critical(mavlink_fd, "Switched to FLYING / AUTO mode"); - break; - - case SYSTEM_STATE_STABILIZED: - - /* set system flags according to state */ - current_status->flag_system_armed = true; - - mavlink_log_critical(mavlink_fd, "Switched to FLYING / STABILIZED mode"); - break; - - case SYSTEM_STATE_MANUAL: - - /* set system flags according to state */ - current_status->flag_system_armed = true; - - mavlink_log_critical(mavlink_fd, "Switched to FLYING / MANUAL mode"); - break; - - default: - invalid_state = true; - break; } - if (invalid_state == false || old_state != new_state) { - current_status->state_machine = new_state; + if (valid_transition) { + current_status->navigation_state = new_state; state_machine_publish(status_pub, current_status, mavlink_fd); - publish_armed_status(current_status); +// publish_armed_status(current_status); ret = OK; - } - - if (invalid_state) { - mavlink_log_critical(mavlink_fd, "REJECTING invalid state transition"); - ret = ERROR; + } else { + mavlink_log_critical(mavlink_fd, "REJECTING invalid navigation state transition"); } return ret; } +/** + * Transition from one arming state to another + */ +int do_arming_state_update(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, arming_state_t new_state) +{ + bool valid_transition = false; + int ret = ERROR; + + if (current_status->arming_state == new_state) { + warnx("Arming state not changed"); + + } else { + + switch (new_state) { + + case ARMING_STATE_INIT: + + if (current_status->arming_state == ARMING_STATE_STANDBY) { + mavlink_log_critical(mavlink_fd, "SWITCHED TO INIT ARMING STATE"); + valid_transition = true; + } + break; + + case ARMING_STATE_STANDBY: + + // TODO check for sensors + // XXX check if coming from reboot? + if (current_status->arming_state == ARMING_STATE_INIT) { + + mavlink_log_critical(mavlink_fd, "SWITCHED TO STANDBY ARMING STATE"); + valid_transition = true; + } + break; + + case ARMING_STATE_ARMED: + + if (current_status->arming_state == ARMING_STATE_STANDBY + || current_status->arming_state == ARMING_STATE_IN_AIR_RESTORE) { + + mavlink_log_critical(mavlink_fd, "SWITCHED TO ARMED ARMING STATE"); + valid_transition = true; + } + break; + + case ARMING_STATE_MISSION_ABORT: + + if (current_status->arming_state == ARMING_STATE_ARMED) { + + mavlink_log_critical(mavlink_fd, "SWITCHED TO MISSION ABORT ARMING STATE"); + valid_transition = true; + } + break; + + case ARMING_STATE_ERROR: + + if (current_status->arming_state == ARMING_STATE_ARMED + || current_status->arming_state == ARMING_STATE_MISSION_ABORT + || current_status->arming_state == ARMING_STATE_INIT) { + + mavlink_log_critical(mavlink_fd, "SWITCHED TO ERROR ARMING STATE"); + valid_transition = true; + } + break; + + case ARMING_STATE_REBOOT: + + if (current_status->arming_state == ARMING_STATE_STANDBY + || current_status->arming_state == ARMING_STATE_ERROR) { + + mavlink_log_critical(mavlink_fd, "SWITCHED TO REBOOT ARMING STATE"); + valid_transition = true; + // XXX reboot here? + } + break; + + case ARMING_STATE_IN_AIR_RESTORE: + + if (current_status->arming_state == ARMING_STATE_INIT) { + mavlink_log_critical(mavlink_fd, "SWITCHED TO IN-AIR-RESTORE ARMING STATE"); + valid_transition = true; + } + break; + default: + warnx("Unknown arming state"); + break; + } + } + + if (valid_transition) { + current_status->arming_state = new_state; + state_machine_publish(status_pub, current_status, mavlink_fd); +// publish_armed_status(current_status); + ret = OK; + } else { + mavlink_log_critical(mavlink_fd, "REJECTING invalid arming state transition"); + } + + return ret; +} + + + void state_machine_publish(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd) { /* publish the new state */ @@ -223,70 +338,69 @@ void state_machine_publish(int status_pub, struct vehicle_status_s *current_stat current_status->timestamp = hrt_absolute_time(); /* assemble state vector based on flag values */ - if (current_status->flag_control_rates_enabled) { - current_status->onboard_control_sensors_present |= 0x400; +// if (current_status->flag_control_rates_enabled) { +// current_status->onboard_control_sensors_present |= 0x400; +// +// } else { +// current_status->onboard_control_sensors_present &= ~0x400; +// } - } else { - current_status->onboard_control_sensors_present &= ~0x400; - } - - current_status->onboard_control_sensors_present |= (current_status->flag_control_attitude_enabled) ? 0x800 : 0; - current_status->onboard_control_sensors_present |= (current_status->flag_control_attitude_enabled) ? 0x1000 : 0; - current_status->onboard_control_sensors_present |= (current_status->flag_control_velocity_enabled || current_status->flag_control_position_enabled) ? 0x2000 : 0; - current_status->onboard_control_sensors_present |= (current_status->flag_control_velocity_enabled || current_status->flag_control_position_enabled) ? 0x4000 : 0; - - current_status->onboard_control_sensors_enabled |= (current_status->flag_control_rates_enabled) ? 0x400 : 0; - current_status->onboard_control_sensors_enabled |= (current_status->flag_control_attitude_enabled) ? 0x800 : 0; - current_status->onboard_control_sensors_enabled |= (current_status->flag_control_attitude_enabled) ? 0x1000 : 0; - current_status->onboard_control_sensors_enabled |= (current_status->flag_control_velocity_enabled || current_status->flag_control_position_enabled) ? 0x2000 : 0; - current_status->onboard_control_sensors_enabled |= (current_status->flag_control_velocity_enabled || current_status->flag_control_position_enabled) ? 0x4000 : 0; +// current_status->onboard_control_sensors_present |= (current_status->flag_control_attitude_enabled) ? 0x800 : 0; +// current_status->onboard_control_sensors_present |= (current_status->flag_control_attitude_enabled) ? 0x1000 : 0; +// current_status->onboard_control_sensors_present |= (current_status->flag_control_velocity_enabled || current_status->flag_control_position_enabled) ? 0x2000 : 0; +// current_status->onboard_control_sensors_present |= (current_status->flag_control_velocity_enabled || current_status->flag_control_position_enabled) ? 0x4000 : 0; +// +// current_status->onboard_control_sensors_enabled |= (current_status->flag_control_rates_enabled) ? 0x400 : 0; +// current_status->onboard_control_sensors_enabled |= (current_status->flag_control_attitude_enabled) ? 0x800 : 0; +// current_status->onboard_control_sensors_enabled |= (current_status->flag_control_attitude_enabled) ? 0x1000 : 0; +// current_status->onboard_control_sensors_enabled |= (current_status->flag_control_velocity_enabled || current_status->flag_control_position_enabled) ? 0x2000 : 0; +// current_status->onboard_control_sensors_enabled |= (current_status->flag_control_velocity_enabled || current_status->flag_control_position_enabled) ? 0x4000 : 0; orb_publish(ORB_ID(vehicle_status), status_pub, current_status); - printf("[cmd] new state: %s\n", system_state_txt[current_status->state_machine]); } -void publish_armed_status(const struct vehicle_status_s *current_status) -{ - struct actuator_armed_s armed; - armed.armed = current_status->flag_system_armed; - /* lock down actuators if required, only in HIL */ - armed.lockdown = (current_status->flag_hil_enabled) ? true : false; - orb_advert_t armed_pub = orb_advertise(ORB_ID(actuator_armed), &armed); - orb_publish(ORB_ID(actuator_armed), armed_pub, &armed); -} +//void publish_armed_status(const struct vehicle_status_s *current_status) +//{ +// struct actuator_armed_s armed; +// armed.armed = current_status->flag_system_armed; +// /* lock down actuators if required, only in HIL */ +// armed.lockdown = (current_status->flag_hil_enabled) ? true : false; +// orb_advert_t armed_pub = orb_advertise(ORB_ID(actuator_armed), &armed); +// orb_publish(ORB_ID(actuator_armed), armed_pub, &armed); +//} /* * Private functions, update the state machine */ -void state_machine_emergency_always_critical(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd) -{ - warnx("EMERGENCY HANDLER\n"); - /* Depending on the current state go to one of the error states */ +//void state_machine_emergency_always_critical(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd) +//{ +// warnx("EMERGENCY HANDLER\n"); +// /* Depending on the current state go to one of the error states */ +// +// if (current_status->state_machine == SYSTEM_STATE_PREFLIGHT || current_status->state_machine == SYSTEM_STATE_STANDBY || current_status->state_machine == SYSTEM_STATE_GROUND_READY) { +// do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_GROUND_ERROR); +// +// } else if (current_status->state_machine == SYSTEM_STATE_AUTO || current_status->state_machine == SYSTEM_STATE_MANUAL) { +// +// // DO NOT abort mission +// //do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_MISSION_ABORT); +// +// } else { +// warnx("Unknown system state: #%d\n", current_status->state_machine); +// } +//} - if (current_status->state_machine == SYSTEM_STATE_PREFLIGHT || current_status->state_machine == SYSTEM_STATE_STANDBY || current_status->state_machine == SYSTEM_STATE_GROUND_READY) { - do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_GROUND_ERROR); - - } else if (current_status->state_machine == SYSTEM_STATE_AUTO || current_status->state_machine == SYSTEM_STATE_MANUAL) { - - // DO NOT abort mission - //do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_MISSION_ABORT); - - } else { - warnx("Unknown system state: #%d\n", current_status->state_machine); - } -} - -void state_machine_emergency(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd) //do not call state_machine_emergency_always_critical if we are in manual mode for these errors -{ - if (current_status->state_machine != SYSTEM_STATE_MANUAL) { //if we are in manual: user can react to errors themself - state_machine_emergency_always_critical(status_pub, current_status, mavlink_fd); - - } else { - //global_data_send_mavlink_statustext_message_out("[cmd] ERROR: take action immediately! (did not switch to error state because the system is in manual mode)", MAV_SEVERITY_CRITICAL); - } - -} +//void state_machine_emergency(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd) //do not call state_machine_emergency_always_critical if we are in manual mode for these errors +//{ +// if (current_status->state_machine != SYSTEM_STATE_MANUAL) { //if we are in manual: user can react to errors themself +// state_machine_emergency_always_critical(status_pub, current_status, mavlink_fd); +// +// } else { +// //global_data_send_mavlink_statustext_message_out("[cmd] ERROR: take action immediately! (did not switch to error state because the system is in manual mode)", MAV_SEVERITY_CRITICAL); +// } +// +//} @@ -488,6 +602,7 @@ void state_machine_emergency(int status_pub, struct vehicle_status_s *current_st /* END SUBSYSTEM/EMERGENCY FUNCTIONS*/ +#if 0 void update_state_machine_got_position_fix(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd) { @@ -750,3 +865,4 @@ uint8_t update_state_machine_custom_mode_request(int status_pub, struct vehicle_ return ret; } +#endif diff --git a/apps/commander/state_machine_helper.h b/apps/commander/state_machine_helper.h index 2f2ccc7298..bf9296caf0 100644 --- a/apps/commander/state_machine_helper.h +++ b/apps/commander/state_machine_helper.h @@ -47,163 +47,10 @@ #include #include -/** - * Switch to new state with no checking. - * - * do_state_update: this is the functions that all other functions have to call in order to update the state. - * the function does not question the state change, this must be done before - * The function performs actions that are connected with the new state (buzzer, reboot, ...) - * - * @param status_pub file descriptor for state update topic publication - * @param current_status pointer to the current state machine to operate on - * @param mavlink_fd file descriptor for MAVLink statustext messages - * - * @return 0 (macro OK) or 1 on error (macro ERROR) - */ -int do_state_update(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, commander_state_machine_t new_state); +int do_navigation_state_update(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, navigation_state_t new_state); -/* These functions decide if an emergency exits and then switch to SYSTEM_STATE_MISSION_ABORT or SYSTEM_STATE_GROUND_ERROR */ -// void update_state_machine_subsystem_present(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type); -// void update_state_machine_subsystem_notpresent(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type); +int do_arming_state_update(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, arming_state_t new_state); -// void update_state_machine_subsystem_enabled(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type); -// void update_state_machine_subsystem_disabled(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type); - -// void update_state_machine_subsystem_healthy(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type); -// void update_state_machine_subsystem_unhealthy(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type); - - -/** - * Handle state machine if got position fix - * - * @param status_pub file descriptor for state update topic publication - * @param current_status pointer to the current state machine to operate on - * @param mavlink_fd file descriptor for MAVLink statustext messages - */ -void update_state_machine_got_position_fix(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd); - -/** - * Handle state machine if position fix lost - * - * @param status_pub file descriptor for state update topic publication - * @param current_status pointer to the current state machine to operate on - * @param mavlink_fd file descriptor for MAVLink statustext messages - */ -void update_state_machine_no_position_fix(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd); - -/** - * Handle state machine if user wants to arm - * - * @param status_pub file descriptor for state update topic publication - * @param current_status pointer to the current state machine to operate on - * @param mavlink_fd file descriptor for MAVLink statustext messages - */ -void update_state_machine_arm(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd); - -/** - * Handle state machine if user wants to disarm - * - * @param status_pub file descriptor for state update topic publication - * @param current_status pointer to the current state machine to operate on - * @param mavlink_fd file descriptor for MAVLink statustext messages - */ -void update_state_machine_disarm(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd); - -/** - * Handle state machine if mode switch is manual - * - * @param status_pub file descriptor for state update topic publication - * @param current_status pointer to the current state machine to operate on - * @param mavlink_fd file descriptor for MAVLink statustext messages - */ -void update_state_machine_mode_manual(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd); - -/** - * Handle state machine if mode switch is stabilized - * - * @param status_pub file descriptor for state update topic publication - * @param current_status pointer to the current state machine to operate on - * @param mavlink_fd file descriptor for MAVLink statustext messages - */ -void update_state_machine_mode_stabilized(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd); - -/** - * Handle state machine if mode switch is guided - * - * @param status_pub file descriptor for state update topic publication - * @param current_status pointer to the current state machine to operate on - * @param mavlink_fd file descriptor for MAVLink statustext messages - */ -void update_state_machine_mode_guided(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd); - -/** - * Handle state machine if mode switch is auto - * - * @param status_pub file descriptor for state update topic publication - * @param current_status pointer to the current state machine to operate on - * @param mavlink_fd file descriptor for MAVLink statustext messages - */ -void update_state_machine_mode_auto(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd); - -/** - * Publish current state information - * - * @param status_pub file descriptor for state update topic publication - * @param current_status pointer to the current state machine to operate on - * @param mavlink_fd file descriptor for MAVLink statustext messages - */ void state_machine_publish(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd); - -/* - * Functions that handle incoming requests to change the state machine or a parameter (probably from the mavlink app). - * If the request is obeyed the functions return 0 - * - */ - -/** - * Handles *incoming request* to switch to a specific state, if state change is successful returns 0 - * - * @param status_pub file descriptor for state update topic publication - * @param current_status pointer to the current state machine to operate on - * @param mavlink_fd file descriptor for MAVLink statustext messages - */ -uint8_t update_state_machine_mode_request(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, uint8_t mode); - -/** - * Handles *incoming request* to switch to a specific custom state, if state change is successful returns 0 - * - * @param status_pub file descriptor for state update topic publication - * @param current_status pointer to the current state machine to operate on - * @param mavlink_fd file descriptor for MAVLink statustext messages - */ -uint8_t update_state_machine_custom_mode_request(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, uint8_t custom_mode); - -/** - * Always switches to critical mode under any circumstances. - * - * @param status_pub file descriptor for state update topic publication - * @param current_status pointer to the current state machine to operate on - * @param mavlink_fd file descriptor for MAVLink statustext messages - */ -void state_machine_emergency_always_critical(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd); - -/** - * Switches to emergency if required. - * - * @param status_pub file descriptor for state update topic publication - * @param current_status pointer to the current state machine to operate on - * @param mavlink_fd file descriptor for MAVLink statustext messages - */ -void state_machine_emergency(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd); - -/** - * Publish the armed state depending on the current system state - * - * @param current_status the current system status - */ -void publish_armed_status(const struct vehicle_status_s *current_status); - - - #endif /* STATE_MACHINE_HELPER_H_ */ diff --git a/apps/controllib/fixedwing.cpp b/apps/controllib/fixedwing.cpp index d9637b4a7a..9a69195352 100644 --- a/apps/controllib/fixedwing.cpp +++ b/apps/controllib/fixedwing.cpp @@ -39,6 +39,7 @@ #include "fixedwing.hpp" + namespace control { @@ -294,7 +295,7 @@ void BlockMultiModeBacksideAutopilot::update() _actuators.control[i] = 0.0f; // only update guidance in auto mode - if (_status.state_machine == SYSTEM_STATE_AUTO) { + if (_status.navigation_state == NAVIGATION_STATE_MISSION) { // update guidance _guide.update(_pos, _att, _posCmd, _lastPosCmd); } @@ -304,8 +305,8 @@ void BlockMultiModeBacksideAutopilot::update() // the setpoint should update to loitering around this position // handle autopilot modes - if (_status.state_machine == SYSTEM_STATE_AUTO || - _status.state_machine == SYSTEM_STATE_STABILIZED) { + if (_status.navigation_state == NAVIGATION_STATE_MISSION || + _status.navigation_state == NAVIGATION_STATE_MANUAL) { // update guidance _guide.update(_pos, _att, _posCmd, _lastPosCmd); @@ -340,25 +341,25 @@ void BlockMultiModeBacksideAutopilot::update() } - } else if (_status.state_machine == SYSTEM_STATE_MANUAL) { - - if (_status.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_DIRECT) { + } else if (_status.navigation_state == NAVIGATION_STATE_MANUAL) { +// if (_status.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_DIRECT) { +#warning fix the different manual modes _actuators.control[CH_AIL] = _manual.roll; _actuators.control[CH_ELV] = _manual.pitch; _actuators.control[CH_RDR] = _manual.yaw; _actuators.control[CH_THR] = _manual.throttle; - } else if (_status.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_SAS) { - - _stabilization.update(_manual.roll, _manual.pitch, _manual.yaw, - _att.rollspeed, _att.pitchspeed, _att.yawspeed); - - _actuators.control[CH_AIL] = _stabilization.getAileron(); - _actuators.control[CH_ELV] = - _stabilization.getElevator(); - _actuators.control[CH_RDR] = _stabilization.getRudder(); - _actuators.control[CH_THR] = _manual.throttle; - } +// } else if (_status.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_SAS) { +// +// _stabilization.update(_manual.roll, _manual.pitch, _manual.yaw, +// _att.rollspeed, _att.pitchspeed, _att.yawspeed); +// +// _actuators.control[CH_AIL] = _stabilization.getAileron(); +// _actuators.control[CH_ELV] = - _stabilization.getElevator(); +// _actuators.control[CH_RDR] = _stabilization.getRudder(); +// _actuators.control[CH_THR] = _manual.throttle; +// } } // update all publications diff --git a/apps/drivers/blinkm/blinkm.cpp b/apps/drivers/blinkm/blinkm.cpp index fc929284c4..6aff27e4ca 100644 --- a/apps/drivers/blinkm/blinkm.cpp +++ b/apps/drivers/blinkm/blinkm.cpp @@ -554,26 +554,27 @@ BlinkM::led() led_blink = LED_BLINK; /* handle 4th led - flightmode indicator */ - switch((int)vehicle_status_raw.flight_mode) { - case VEHICLE_FLIGHT_MODE_MANUAL: - led_color_4 = LED_AMBER; - break; - - case VEHICLE_FLIGHT_MODE_STAB: - led_color_4 = LED_YELLOW; - break; - - case VEHICLE_FLIGHT_MODE_HOLD: - led_color_4 = LED_BLUE; - break; - - case VEHICLE_FLIGHT_MODE_AUTO: - led_color_4 = LED_GREEN; - break; - } +#warning fix this +// switch((int)vehicle_status_raw.flight_mode) { +// case VEHICLE_FLIGHT_MODE_MANUAL: +// led_color_4 = LED_AMBER; +// break; +// +// case VEHICLE_FLIGHT_MODE_STAB: +// led_color_4 = LED_YELLOW; +// break; +// +// case VEHICLE_FLIGHT_MODE_HOLD: +// led_color_4 = LED_BLUE; +// break; +// +// case VEHICLE_FLIGHT_MODE_AUTO: +// led_color_4 = LED_GREEN; +// break; +// } if(new_data_vehicle_gps_position || no_data_vehicle_gps_position < 3) { - /* handling used sat´s */ + /* handling used sat�s */ if(num_of_used_sats >= 7) { led_color_1 = LED_OFF; led_color_2 = LED_OFF; diff --git a/apps/fixedwing_att_control/fixedwing_att_control_main.c b/apps/fixedwing_att_control/fixedwing_att_control_main.c index aa9db6d528..12f06cdd29 100644 --- a/apps/fixedwing_att_control/fixedwing_att_control_main.c +++ b/apps/fixedwing_att_control/fixedwing_att_control_main.c @@ -185,87 +185,88 @@ int fixedwing_att_control_thread_main(int argc, char *argv[]) /* control */ - if (vstatus.state_machine == SYSTEM_STATE_AUTO || - vstatus.state_machine == SYSTEM_STATE_STABILIZED) { - /* attitude control */ - fixedwing_att_control_attitude(&att_sp, &att, speed_body, &rates_sp); - - /* angular rate control */ - fixedwing_att_control_rates(&rates_sp, gyro, &actuators); - - /* pass through throttle */ - actuators.control[3] = att_sp.thrust; - - /* set flaps to zero */ - actuators.control[4] = 0.0f; - - } else if (vstatus.state_machine == SYSTEM_STATE_MANUAL) { - if (vstatus.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_SAS) { - - /* if the RC signal is lost, try to stay level and go slowly back down to ground */ - if (vstatus.rc_signal_lost) { - - /* put plane into loiter */ - att_sp.roll_body = 0.3f; - att_sp.pitch_body = 0.0f; - - /* limit throttle to 60 % of last value if sane */ - if (isfinite(manual_sp.throttle) && - (manual_sp.throttle >= 0.0f) && - (manual_sp.throttle <= 1.0f)) { - att_sp.thrust = 0.6f * manual_sp.throttle; - - } else { - att_sp.thrust = 0.0f; - } - - att_sp.yaw_body = 0; - - // XXX disable yaw control, loiter - - } else { - - att_sp.roll_body = manual_sp.roll; - att_sp.pitch_body = manual_sp.pitch; - att_sp.yaw_body = 0; - att_sp.thrust = manual_sp.throttle; - } - - att_sp.timestamp = hrt_absolute_time(); - - /* attitude control */ - fixedwing_att_control_attitude(&att_sp, &att, speed_body, &rates_sp); - - /* angular rate control */ - fixedwing_att_control_rates(&rates_sp, gyro, &actuators); - - /* pass through throttle */ - actuators.control[3] = att_sp.thrust; - - /* pass through flaps */ - if (isfinite(manual_sp.flaps)) { - actuators.control[4] = manual_sp.flaps; - - } else { - actuators.control[4] = 0.0f; - } - - } else if (vstatus.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_DIRECT) { - /* directly pass through values */ - actuators.control[0] = manual_sp.roll; - /* positive pitch means negative actuator -> pull up */ - actuators.control[1] = manual_sp.pitch; - actuators.control[2] = manual_sp.yaw; - actuators.control[3] = manual_sp.throttle; - - if (isfinite(manual_sp.flaps)) { - actuators.control[4] = manual_sp.flaps; - - } else { - actuators.control[4] = 0.0f; - } - } - } +#warning fix this +// if (vstatus.state_machine == SYSTEM_STATE_AUTO || +// vstatus.state_machine == SYSTEM_STATE_STABILIZED) { +// /* attitude control */ +// fixedwing_att_control_attitude(&att_sp, &att, speed_body, &rates_sp); +// +// /* angular rate control */ +// fixedwing_att_control_rates(&rates_sp, gyro, &actuators); +// +// /* pass through throttle */ +// actuators.control[3] = att_sp.thrust; +// +// /* set flaps to zero */ +// actuators.control[4] = 0.0f; +// +// } else if (vstatus.state_machine == SYSTEM_STATE_MANUAL) { +// if (vstatus.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_SAS) { +// +// /* if the RC signal is lost, try to stay level and go slowly back down to ground */ +// if (vstatus.rc_signal_lost) { +// +// /* put plane into loiter */ +// att_sp.roll_body = 0.3f; +// att_sp.pitch_body = 0.0f; +// +// /* limit throttle to 60 % of last value if sane */ +// if (isfinite(manual_sp.throttle) && +// (manual_sp.throttle >= 0.0f) && +// (manual_sp.throttle <= 1.0f)) { +// att_sp.thrust = 0.6f * manual_sp.throttle; +// +// } else { +// att_sp.thrust = 0.0f; +// } +// +// att_sp.yaw_body = 0; +// +// // XXX disable yaw control, loiter +// +// } else { +// +// att_sp.roll_body = manual_sp.roll; +// att_sp.pitch_body = manual_sp.pitch; +// att_sp.yaw_body = 0; +// att_sp.thrust = manual_sp.throttle; +// } +// +// att_sp.timestamp = hrt_absolute_time(); +// +// /* attitude control */ +// fixedwing_att_control_attitude(&att_sp, &att, speed_body, &rates_sp); +// +// /* angular rate control */ +// fixedwing_att_control_rates(&rates_sp, gyro, &actuators); +// +// /* pass through throttle */ +// actuators.control[3] = att_sp.thrust; +// +// /* pass through flaps */ +// if (isfinite(manual_sp.flaps)) { +// actuators.control[4] = manual_sp.flaps; +// +// } else { +// actuators.control[4] = 0.0f; +// } +// +// } else if (vstatus.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_DIRECT) { +// /* directly pass through values */ +// actuators.control[0] = manual_sp.roll; +// /* positive pitch means negative actuator -> pull up */ +// actuators.control[1] = manual_sp.pitch; +// actuators.control[2] = manual_sp.yaw; +// actuators.control[3] = manual_sp.throttle; +// +// if (isfinite(manual_sp.flaps)) { +// actuators.control[4] = manual_sp.flaps; +// +// } else { +// actuators.control[4] = 0.0f; +// } +// } +// } /* publish rates */ orb_publish(ORB_ID(vehicle_rates_setpoint), rates_pub, &rates_sp); diff --git a/apps/mavlink/mavlink.c b/apps/mavlink/mavlink.c index b958d5f96c..4636ee36ec 100644 --- a/apps/mavlink/mavlink.c +++ b/apps/mavlink/mavlink.c @@ -195,95 +195,85 @@ get_mavlink_mode_and_state(uint8_t *mavlink_state, uint8_t *mavlink_mode) /* reset MAVLink mode bitfield */ *mavlink_mode = 0; - /* set mode flags independent of system state */ + /** + * Set mode flags + **/ /* HIL */ if (v_status.flag_hil_enabled) { *mavlink_mode |= MAV_MODE_FLAG_HIL_ENABLED; } - /* manual input */ - if (v_status.flag_control_manual_enabled) { - *mavlink_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED; - } - - /* attitude or rate control */ - if (v_status.flag_control_attitude_enabled || - v_status.flag_control_rates_enabled) { - *mavlink_mode |= MAV_MODE_FLAG_STABILIZE_ENABLED; - } - - /* vector control */ - if (v_status.flag_control_velocity_enabled || - v_status.flag_control_position_enabled) { - *mavlink_mode |= MAV_MODE_FLAG_GUIDED_ENABLED; - } - /* autonomous mode */ - if (v_status.state_machine == SYSTEM_STATE_AUTO) { + if (v_status.navigation_state == NAVIGATION_STATE_MISSION + || v_status.navigation_state == NAVIGATION_STATE_TAKEOFF + || v_status.navigation_state == NAVIGATION_STATE_RTL + || v_status.navigation_state == NAVIGATION_STATE_LAND + || v_status.navigation_state == NAVIGATION_STATE_AUTO_READY) { *mavlink_mode |= MAV_MODE_FLAG_AUTO_ENABLED; } /* set arming state */ - if (armed.armed) { + if (v_status.flag_system_armed) { *mavlink_mode |= MAV_MODE_FLAG_SAFETY_ARMED; } else { *mavlink_mode &= ~MAV_MODE_FLAG_SAFETY_ARMED; } - switch (v_status.state_machine) { - case SYSTEM_STATE_PREFLIGHT: - if (v_status.flag_preflight_gyro_calibration || - v_status.flag_preflight_mag_calibration || - v_status.flag_preflight_accel_calibration) { - *mavlink_state = MAV_STATE_CALIBRATING; + if (v_status.navigation_state == NAVIGATION_STATE_MANUAL) { - } else { - *mavlink_state = MAV_STATE_UNINIT; - } + *mavlink_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED; + } - break; + if (v_status.navigation_state == NAVIGATION_STATE_SEATBELT) { + + *mavlink_mode |= MAV_MODE_FLAG_DECODE_POSITION_GUIDED; + } + + if (v_status.navigation_state == NAVIGATION_STATE_SEATBELT) { + + *mavlink_mode |= MAV_MODE_FLAG_DECODE_POSITION_GUIDED; + } + + + /** + * Set mavlink state + **/ + + /* set calibration state */ + if (v_status.flag_preflight_gyro_calibration || + v_status.flag_preflight_mag_calibration || + v_status.flag_preflight_accel_calibration) { + + *mavlink_state = MAV_STATE_CALIBRATING; + + } else if (v_status.flag_system_emergency) { + + *mavlink_state = MAV_STATE_EMERGENCY; + + } else if (v_status.navigation_state == NAVIGATION_STATE_MANUAL + || v_status.navigation_state == NAVIGATION_STATE_SEATBELT + || v_status.navigation_state == NAVIGATION_STATE_LOITER + || v_status.navigation_state == NAVIGATION_STATE_MISSION + || v_status.navigation_state == NAVIGATION_STATE_RTL + || v_status.navigation_state == NAVIGATION_STATE_LAND + || v_status.navigation_state == NAVIGATION_STATE_TAKEOFF + || v_status.navigation_state == NAVIGATION_STATE_AUTO_READY) { + + *mavlink_state = MAV_STATE_ACTIVE; + + } else if (v_status.navigation_state == NAVIGATION_STATE_STANDBY) { - case SYSTEM_STATE_STANDBY: *mavlink_state = MAV_STATE_STANDBY; - break; - case SYSTEM_STATE_GROUND_READY: - *mavlink_state = MAV_STATE_ACTIVE; - break; + } else if (v_status.navigation_state == NAVIGATION_STATE_INIT) { - case SYSTEM_STATE_MANUAL: - *mavlink_state = MAV_STATE_ACTIVE; - break; + *mavlink_state = MAV_STATE_UNINIT; + } else { - case SYSTEM_STATE_STABILIZED: - *mavlink_state = MAV_STATE_ACTIVE; - break; - - case SYSTEM_STATE_AUTO: - *mavlink_state = MAV_STATE_ACTIVE; - break; - - case SYSTEM_STATE_MISSION_ABORT: - *mavlink_state = MAV_STATE_EMERGENCY; - break; - - case SYSTEM_STATE_EMCY_LANDING: - *mavlink_state = MAV_STATE_EMERGENCY; - break; - - case SYSTEM_STATE_EMCY_CUTOFF: - *mavlink_state = MAV_STATE_EMERGENCY; - break; - - case SYSTEM_STATE_GROUND_ERROR: - *mavlink_state = MAV_STATE_EMERGENCY; - break; - - case SYSTEM_STATE_REBOOT: - *mavlink_state = MAV_STATE_POWEROFF; - break; + warnx("Unknown mavlink state"); + *mavlink_state = MAV_STATE_CRITICAL; } } @@ -688,7 +678,7 @@ int mavlink_thread_main(int argc, char *argv[]) get_mavlink_mode_and_state(&mavlink_state, &mavlink_mode); /* send heartbeat */ - mavlink_msg_heartbeat_send(chan, mavlink_system.type, MAV_AUTOPILOT_PX4, mavlink_mode, v_status.state_machine, mavlink_state); + mavlink_msg_heartbeat_send(chan, mavlink_system.type, MAV_AUTOPILOT_PX4, mavlink_mode, v_status.navigation_state, mavlink_state); /* switch HIL mode if required */ set_hil_on_off(v_status.flag_hil_enabled); diff --git a/apps/mavlink/orb_listener.c b/apps/mavlink/orb_listener.c index 9f85d5801f..15066010c6 100644 --- a/apps/mavlink/orb_listener.c +++ b/apps/mavlink/orb_listener.c @@ -275,7 +275,7 @@ l_vehicle_status(struct listener *l) mavlink_system.type, MAV_AUTOPILOT_PX4, mavlink_mode, - v_status.state_machine, + v_status.navigation_state, mavlink_state); } diff --git a/apps/mavlink_onboard/mavlink.c b/apps/mavlink_onboard/mavlink.c index 33ebe8600a..6babe14ae2 100644 --- a/apps/mavlink_onboard/mavlink.c +++ b/apps/mavlink_onboard/mavlink.c @@ -296,60 +296,60 @@ get_mavlink_mode_and_state(const struct vehicle_status_s *v_status, const struct *mavlink_mode &= ~MAV_MODE_FLAG_SAFETY_ARMED; } - switch (v_status->state_machine) { - case SYSTEM_STATE_PREFLIGHT: - if (v_status->flag_preflight_gyro_calibration || - v_status->flag_preflight_mag_calibration || - v_status->flag_preflight_accel_calibration) { - *mavlink_state = MAV_STATE_CALIBRATING; - } else { - *mavlink_state = MAV_STATE_UNINIT; - } - break; - - case SYSTEM_STATE_STANDBY: - *mavlink_state = MAV_STATE_STANDBY; - break; - - case SYSTEM_STATE_GROUND_READY: - *mavlink_state = MAV_STATE_ACTIVE; - break; - - case SYSTEM_STATE_MANUAL: - *mavlink_state = MAV_STATE_ACTIVE; - *mavlink_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED; - break; - - case SYSTEM_STATE_STABILIZED: - *mavlink_state = MAV_STATE_ACTIVE; - *mavlink_mode |= MAV_MODE_FLAG_STABILIZE_ENABLED; - break; - - case SYSTEM_STATE_AUTO: - *mavlink_state = MAV_STATE_ACTIVE; - *mavlink_mode |= MAV_MODE_FLAG_GUIDED_ENABLED; - break; - - case SYSTEM_STATE_MISSION_ABORT: - *mavlink_state = MAV_STATE_EMERGENCY; - break; - - case SYSTEM_STATE_EMCY_LANDING: - *mavlink_state = MAV_STATE_EMERGENCY; - break; - - case SYSTEM_STATE_EMCY_CUTOFF: - *mavlink_state = MAV_STATE_EMERGENCY; - break; - - case SYSTEM_STATE_GROUND_ERROR: - *mavlink_state = MAV_STATE_EMERGENCY; - break; - - case SYSTEM_STATE_REBOOT: - *mavlink_state = MAV_STATE_POWEROFF; - break; - } +// switch (v_status->state_machine) { +// case SYSTEM_STATE_PREFLIGHT: +// if (v_status->flag_preflight_gyro_calibration || +// v_status->flag_preflight_mag_calibration || +// v_status->flag_preflight_accel_calibration) { +// *mavlink_state = MAV_STATE_CALIBRATING; +// } else { +// *mavlink_state = MAV_STATE_UNINIT; +// } +// break; +// +// case SYSTEM_STATE_STANDBY: +// *mavlink_state = MAV_STATE_STANDBY; +// break; +// +// case SYSTEM_STATE_GROUND_READY: +// *mavlink_state = MAV_STATE_ACTIVE; +// break; +// +// case SYSTEM_STATE_MANUAL: +// *mavlink_state = MAV_STATE_ACTIVE; +// *mavlink_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED; +// break; +// +// case SYSTEM_STATE_STABILIZED: +// *mavlink_state = MAV_STATE_ACTIVE; +// *mavlink_mode |= MAV_MODE_FLAG_STABILIZE_ENABLED; +// break; +// +// case SYSTEM_STATE_AUTO: +// *mavlink_state = MAV_STATE_ACTIVE; +// *mavlink_mode |= MAV_MODE_FLAG_GUIDED_ENABLED; +// break; +// +// case SYSTEM_STATE_MISSION_ABORT: +// *mavlink_state = MAV_STATE_EMERGENCY; +// break; +// +// case SYSTEM_STATE_EMCY_LANDING: +// *mavlink_state = MAV_STATE_EMERGENCY; +// break; +// +// case SYSTEM_STATE_EMCY_CUTOFF: +// *mavlink_state = MAV_STATE_EMERGENCY; +// break; +// +// case SYSTEM_STATE_GROUND_ERROR: +// *mavlink_state = MAV_STATE_EMERGENCY; +// break; +// +// case SYSTEM_STATE_REBOOT: +// *mavlink_state = MAV_STATE_POWEROFF; +// break; +// } } @@ -434,7 +434,7 @@ int mavlink_thread_main(int argc, char *argv[]) get_mavlink_mode_and_state(&v_status, &armed, &mavlink_state, &mavlink_mode); /* send heartbeat */ - mavlink_msg_heartbeat_send(chan, mavlink_system.type, MAV_AUTOPILOT_PX4, mavlink_mode, v_status.state_machine, mavlink_state); + mavlink_msg_heartbeat_send(chan, mavlink_system.type, MAV_AUTOPILOT_PX4, mavlink_mode, v_status.navigation_state, mavlink_state); /* send status (values already copied in the section above) */ mavlink_msg_sys_status_send(chan, diff --git a/apps/multirotor_att_control/multirotor_att_control_main.c b/apps/multirotor_att_control/multirotor_att_control_main.c index d94c0a69c9..da7550f797 100644 --- a/apps/multirotor_att_control/multirotor_att_control_main.c +++ b/apps/multirotor_att_control/multirotor_att_control_main.c @@ -296,38 +296,39 @@ mc_thread_main(int argc, char *argv[]) } /* act if stabilization is active or if the (nonsense) direct pass through mode is set */ - if (state.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_SAS || - state.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_DIRECT) { - - if (state.manual_sas_mode == VEHICLE_MANUAL_SAS_MODE_ROLL_PITCH_ABS_YAW_RATE) { - rates_sp.yaw = manual.yaw; - control_yaw_position = false; - - } else { - /* - * This mode SHOULD be the default mode, which is: - * VEHICLE_MANUAL_SAS_MODE_ROLL_PITCH_ABS_YAW_ABS - * - * However, we fall back to this setting for all other (nonsense) - * settings as well. - */ - - /* only move setpoint if manual input is != 0 */ - if ((manual.yaw < -0.01f || 0.01f < manual.yaw) && manual.throttle > 0.3f) { - rates_sp.yaw = manual.yaw; - control_yaw_position = false; - first_time_after_yaw_speed_control = true; - - } else { - if (first_time_after_yaw_speed_control) { - att_sp.yaw_body = att.yaw; - first_time_after_yaw_speed_control = false; - } - - control_yaw_position = true; - } - } - } +#warning fix this +// if (state.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_SAS || +// state.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_DIRECT) { +// +// if (state.manual_sas_mode == VEHICLE_MANUAL_SAS_MODE_ROLL_PITCH_ABS_YAW_RATE) { +// rates_sp.yaw = manual.yaw; +// control_yaw_position = false; +// +// } else { +// /* +// * This mode SHOULD be the default mode, which is: +// * VEHICLE_MANUAL_SAS_MODE_ROLL_PITCH_ABS_YAW_ABS +// * +// * However, we fall back to this setting for all other (nonsense) +// * settings as well. +// */ +// +// /* only move setpoint if manual input is != 0 */ +// if ((manual.yaw < -0.01f || 0.01f < manual.yaw) && manual.throttle > 0.3f) { +// rates_sp.yaw = manual.yaw; +// control_yaw_position = false; +// first_time_after_yaw_speed_control = true; +// +// } else { +// if (first_time_after_yaw_speed_control) { +// att_sp.yaw_body = att.yaw; +// first_time_after_yaw_speed_control = false; +// } +// +// control_yaw_position = true; +// } +// } +// } att_sp.thrust = manual.throttle; att_sp.timestamp = hrt_absolute_time(); @@ -348,16 +349,17 @@ mc_thread_main(int argc, char *argv[]) } } else { +#warning fix this /* manual rate inputs, from RC control or joystick */ - if (state.flag_control_rates_enabled && - state.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_RATES) { - rates_sp.roll = manual.roll; - - rates_sp.pitch = manual.pitch; - rates_sp.yaw = manual.yaw; - rates_sp.thrust = manual.throttle; - rates_sp.timestamp = hrt_absolute_time(); - } +// if (state.flag_control_rates_enabled && +// state.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_RATES) { +// rates_sp.roll = manual.roll; +// +// rates_sp.pitch = manual.pitch; +// rates_sp.yaw = manual.yaw; +// rates_sp.thrust = manual.throttle; +// rates_sp.timestamp = hrt_absolute_time(); +// } } } diff --git a/apps/sensors/sensors.cpp b/apps/sensors/sensors.cpp index d8d200ea9d..28579bc703 100644 --- a/apps/sensors/sensors.cpp +++ b/apps/sensors/sensors.cpp @@ -195,13 +195,11 @@ private: int rc_map_yaw; int rc_map_throttle; - int rc_map_manual_override_sw; - int rc_map_auto_mode_sw; + int rc_map_mode_sw; + int rc_map_return_sw; + int rc_map_mission_sw; - int rc_map_manual_mode_sw; - int rc_map_sas_mode_sw; - int rc_map_rtl_sw; - int rc_map_offboard_ctrl_mode_sw; +// int rc_map_offboard_ctrl_mode_sw; int rc_map_flaps; @@ -241,13 +239,11 @@ private: param_t rc_map_yaw; param_t rc_map_throttle; - param_t rc_map_manual_override_sw; - param_t rc_map_auto_mode_sw; + param_t rc_map_mode_sw; + param_t rc_map_return_sw; + param_t rc_map_mission_sw; - param_t rc_map_manual_mode_sw; - param_t rc_map_sas_mode_sw; - param_t rc_map_rtl_sw; - param_t rc_map_offboard_ctrl_mode_sw; +// param_t rc_map_offboard_ctrl_mode_sw; param_t rc_map_flaps; @@ -436,16 +432,15 @@ Sensors::Sensors() : _parameter_handles.rc_map_throttle = param_find("RC_MAP_THROTTLE"); /* mandatory mode switches, mapped to channel 5 and 6 per default */ - _parameter_handles.rc_map_manual_override_sw = param_find("RC_MAP_OVER_SW"); - _parameter_handles.rc_map_auto_mode_sw = param_find("RC_MAP_MODE_SW"); + _parameter_handles.rc_map_mode_sw = param_find("RC_MAP_OVER_SW"); + _parameter_handles.rc_map_return_sw = param_find("RC_MAP_RETURN_SW"); _parameter_handles.rc_map_flaps = param_find("RC_MAP_FLAPS"); /* optional mode switches, not mapped per default */ - _parameter_handles.rc_map_manual_mode_sw = param_find("RC_MAP_MAN_SW"); - _parameter_handles.rc_map_sas_mode_sw = param_find("RC_MAP_SAS_SW"); - _parameter_handles.rc_map_rtl_sw = param_find("RC_MAP_RTL_SW"); - _parameter_handles.rc_map_offboard_ctrl_mode_sw = param_find("RC_MAP_OFFB_SW"); + _parameter_handles.rc_map_mission_sw = param_find("RC_MAP_MISSION_SW"); + +// _parameter_handles.rc_map_offboard_ctrl_mode_sw = param_find("RC_MAP_OFFB_SW"); _parameter_handles.rc_map_aux1 = param_find("RC_MAP_AUX1"); _parameter_handles.rc_map_aux2 = param_find("RC_MAP_AUX2"); @@ -579,33 +574,25 @@ Sensors::parameters_update() warnx("Failed getting throttle chan index"); } - if (param_get(_parameter_handles.rc_map_manual_override_sw, &(_parameters.rc_map_manual_override_sw)) != OK) { - warnx("Failed getting override sw chan index"); + if (param_get(_parameter_handles.rc_map_mode_sw, &(_parameters.rc_map_mode_sw)) != OK) { + warnx("Failed getting mode sw chan index"); } - if (param_get(_parameter_handles.rc_map_auto_mode_sw, &(_parameters.rc_map_auto_mode_sw)) != OK) { - warnx("Failed getting auto mode sw chan index"); + if (param_get(_parameter_handles.rc_map_return_sw, &(_parameters.rc_map_return_sw)) != OK) { + warnx("Failed getting return sw chan index"); + } + + if (param_get(_parameter_handles.rc_map_mission_sw, &(_parameters.rc_map_mission_sw)) != OK) { + warnx("Failed getting mission sw chan index"); } if (param_get(_parameter_handles.rc_map_flaps, &(_parameters.rc_map_flaps)) != OK) { warnx("Failed getting flaps chan index"); } - if (param_get(_parameter_handles.rc_map_manual_mode_sw, &(_parameters.rc_map_manual_mode_sw)) != OK) { - warnx("Failed getting manual mode sw chan index"); - } - - if (param_get(_parameter_handles.rc_map_rtl_sw, &(_parameters.rc_map_rtl_sw)) != OK) { - warnx("Failed getting rtl sw chan index"); - } - - if (param_get(_parameter_handles.rc_map_sas_mode_sw, &(_parameters.rc_map_sas_mode_sw)) != OK) { - warnx("Failed getting sas mode sw chan index"); - } - - if (param_get(_parameter_handles.rc_map_offboard_ctrl_mode_sw, &(_parameters.rc_map_offboard_ctrl_mode_sw)) != OK) { - warnx("Failed getting offboard control mode sw chan index"); - } +// if (param_get(_parameter_handles.rc_map_offboard_ctrl_mode_sw, &(_parameters.rc_map_offboard_ctrl_mode_sw)) != OK) { +// warnx("Failed getting offboard control mode sw chan index"); +// } if (param_get(_parameter_handles.rc_map_aux1, &(_parameters.rc_map_aux1)) != OK) { warnx("Failed getting mode aux 1 index"); @@ -649,15 +636,13 @@ Sensors::parameters_update() _rc.function[PITCH] = _parameters.rc_map_pitch - 1; _rc.function[YAW] = _parameters.rc_map_yaw - 1; - _rc.function[OVERRIDE] = _parameters.rc_map_manual_override_sw - 1; - _rc.function[AUTO_MODE] = _parameters.rc_map_auto_mode_sw - 1; + _rc.function[MODE] = _parameters.rc_map_mode_sw - 1; + _rc.function[RETURN] = _parameters.rc_map_return_sw - 1; + _rc.function[MISSION] = _parameters.rc_map_mission_sw - 1; _rc.function[FLAPS] = _parameters.rc_map_flaps - 1; - _rc.function[MANUAL_MODE] = _parameters.rc_map_manual_mode_sw - 1; - _rc.function[RTL] = _parameters.rc_map_rtl_sw - 1; - _rc.function[SAS_MODE] = _parameters.rc_map_sas_mode_sw - 1; - _rc.function[OFFBOARD_MODE] = _parameters.rc_map_offboard_ctrl_mode_sw - 1; +// _rc.function[OFFBOARD_MODE] = _parameters.rc_map_offboard_ctrl_mode_sw - 1; _rc.function[AUX_1] = _parameters.rc_map_aux1 - 1; _rc.function[AUX_2] = _parameters.rc_map_aux2 - 1; @@ -1122,10 +1107,10 @@ Sensors::ppm_poll() manual_control.yaw = NAN; manual_control.throttle = NAN; - manual_control.manual_mode_switch = NAN; - manual_control.manual_sas_switch = NAN; - manual_control.return_to_launch_switch = NAN; - manual_control.auto_offboard_input_switch = NAN; + manual_control.mode_switch = NAN; + manual_control.return_switch = NAN; + manual_control.mission_switch = NAN; +// manual_control.auto_offboard_input_switch = NAN; manual_control.flaps = NAN; manual_control.aux1 = NAN; @@ -1211,11 +1196,14 @@ Sensors::ppm_poll() manual_control.yaw *= _parameters.rc_scale_yaw; } - /* override switch input */ - manual_control.manual_override_switch = limit_minus_one_to_one(_rc.chan[_rc.function[OVERRIDE]].scaled); - /* mode switch input */ - manual_control.auto_mode_switch = limit_minus_one_to_one(_rc.chan[_rc.function[AUTO_MODE]].scaled); + manual_control.mode_switch = limit_minus_one_to_one(_rc.chan[_rc.function[MODE]].scaled); + + /* land switch input */ + manual_control.return_switch = limit_minus_one_to_one(_rc.chan[_rc.function[RETURN]].scaled); + + /* land switch input */ + manual_control.mission_switch = limit_minus_one_to_one(_rc.chan[_rc.function[MISSION]].scaled); /* flaps */ if (_rc.function[FLAPS] >= 0) { @@ -1227,21 +1215,17 @@ Sensors::ppm_poll() } } - if (_rc.function[MANUAL_MODE] >= 0) { - manual_control.manual_mode_switch = limit_minus_one_to_one(_rc.chan[_rc.function[MANUAL_MODE]].scaled); + if (_rc.function[MODE] >= 0) { + manual_control.mode_switch = limit_minus_one_to_one(_rc.chan[_rc.function[MODE]].scaled); } - if (_rc.function[SAS_MODE] >= 0) { - manual_control.manual_sas_switch = limit_minus_one_to_one(_rc.chan[_rc.function[SAS_MODE]].scaled); + if (_rc.function[MISSION] >= 0) { + manual_control.mission_switch = limit_minus_one_to_one(_rc.chan[_rc.function[MISSION]].scaled); } - if (_rc.function[RTL] >= 0) { - manual_control.return_to_launch_switch = limit_minus_one_to_one(_rc.chan[_rc.function[RTL]].scaled); - } - - if (_rc.function[OFFBOARD_MODE] >= 0) { - manual_control.auto_offboard_input_switch = limit_minus_one_to_one(_rc.chan[_rc.function[OFFBOARD_MODE]].scaled); - } +// if (_rc.function[OFFBOARD_MODE] >= 0) { +// manual_control.auto_offboard_input_switch = limit_minus_one_to_one(_rc.chan[_rc.function[OFFBOARD_MODE]].scaled); +// } /* aux functions, only assign if valid mapping is present */ if (_rc.function[AUX_1] >= 0) { diff --git a/apps/uORB/topics/manual_control_setpoint.h b/apps/uORB/topics/manual_control_setpoint.h index 261a8a4ad3..cfee81ea2b 100644 --- a/apps/uORB/topics/manual_control_setpoint.h +++ b/apps/uORB/topics/manual_control_setpoint.h @@ -56,17 +56,17 @@ struct manual_control_setpoint_s { float yaw; /**< rudder / yaw rate / yaw */ float throttle; /**< throttle / collective thrust / altitude */ - float manual_override_switch; /**< manual override mode (mandatory) */ - float auto_mode_switch; /**< auto mode switch (mandatory) */ + float mode_switch; /**< mode 3 position switch (mandatory): manual, assisted, auto */ + float return_switch; /**< land 2 position switch (mandatory): land, no effect */ + float mission_switch; /**< mission 2 position switch (optional): mission, loiter */ /** * Any of the channels below may not be available and be set to NaN * to indicate that it does not contain valid data. */ - float manual_mode_switch; /**< manual mode (man, sas, alt) switch (optional) */ - float manual_sas_switch; /**< sas mode (rates / attitude) switch (optional) */ - float return_to_launch_switch; /**< return to launch switch (0 = disabled, 1 = enabled) */ - float auto_offboard_input_switch; /**< controller setpoint source (0 = onboard, 1 = offboard) */ + + // XXX needed or parameter? + //float auto_offboard_input_switch; /**< controller setpoint source (0 = onboard, 1 = offboard) */ float flaps; /**< flap position */ diff --git a/apps/uORB/topics/rc_channels.h b/apps/uORB/topics/rc_channels.h index 9dd54df915..a0bb25af46 100644 --- a/apps/uORB/topics/rc_channels.h +++ b/apps/uORB/topics/rc_channels.h @@ -68,18 +68,16 @@ enum RC_CHANNELS_FUNCTION ROLL = 1, PITCH = 2, YAW = 3, - OVERRIDE = 4, - AUTO_MODE = 5, - MANUAL_MODE = 6, - SAS_MODE = 7, - RTL = 8, - OFFBOARD_MODE = 9, - FLAPS = 10, - AUX_1 = 11, - AUX_2 = 12, - AUX_3 = 13, - AUX_4 = 14, - AUX_5 = 15, + MODE = 4, + RETURN = 5, + MISSION = 6, + OFFBOARD_MODE = 7, + FLAPS = 8, + AUX_1 = 9, + AUX_2 = 10, + AUX_3 = 11, + AUX_4 = 12, + AUX_5 = 13, RC_CHANNELS_FUNCTION_MAX /**< indicates the number of functions. There can be more functions than RC channels. */ }; diff --git a/apps/uORB/topics/vehicle_status.h b/apps/uORB/topics/vehicle_status.h index 06b4c5ca5c..f9c4a5fff9 100644 --- a/apps/uORB/topics/vehicle_status.h +++ b/apps/uORB/topics/vehicle_status.h @@ -59,21 +59,30 @@ */ /* State Machine */ -typedef enum -{ - SYSTEM_STATE_PREFLIGHT = 0, - SYSTEM_STATE_STANDBY = 1, - SYSTEM_STATE_GROUND_READY = 2, - SYSTEM_STATE_MANUAL = 3, - SYSTEM_STATE_STABILIZED = 4, - SYSTEM_STATE_AUTO = 5, - SYSTEM_STATE_MISSION_ABORT = 6, - SYSTEM_STATE_EMCY_LANDING = 7, - SYSTEM_STATE_EMCY_CUTOFF = 8, - SYSTEM_STATE_GROUND_ERROR = 9, - SYSTEM_STATE_REBOOT= 10, +typedef enum { + NAVIGATION_STATE_INIT = 0, + NAVIGATION_STATE_STANDBY, + NAVIGATION_STATE_MANUAL, + NAVIGATION_STATE_SEATBELT, + NAVIGATION_STATE_LOITER, + NAVIGATION_STATE_AUTO_READY, + NAVIGATION_STATE_MISSION, + NAVIGATION_STATE_RTL, + NAVIGATION_STATE_TAKEOFF, + NAVIGATION_STATE_LAND, + NAVIGATION_STATE_GROUND_ERROR, + NAVIGATION_STATE_REBOOT +} navigation_state_t; -} commander_state_machine_t; +typedef enum { + ARMING_STATE_INIT = 0, + ARMING_STATE_STANDBY, + ARMING_STATE_ARMED, + ARMING_STATE_MISSION_ABORT, + ARMING_STATE_ERROR, + ARMING_STATE_REBOOT, + ARMING_STATE_IN_AIR_RESTORE +} arming_state_t; enum VEHICLE_MODE_FLAG { VEHICLE_MODE_FLAG_SAFETY_ARMED = 128, @@ -86,25 +95,25 @@ enum VEHICLE_MODE_FLAG { VEHICLE_MODE_FLAG_CUSTOM_MODE_ENABLED = 1 }; /**< Same as MAV_MODE_FLAG of MAVLink 1.0 protocol */ -enum VEHICLE_FLIGHT_MODE { - VEHICLE_FLIGHT_MODE_MANUAL = 0, /**< direct manual control, exact mode determined by VEHICLE_MANUAL_CONTROL_MODE */ - VEHICLE_FLIGHT_MODE_STAB, /**< attitude or rate stabilization plus velocity or position stabilization */ - VEHICLE_FLIGHT_MODE_HOLD, /**< hold current position (hover or loiter around position when switched) */ - VEHICLE_FLIGHT_MODE_AUTO /**< attitude or rate stabilization plus absolute position control and waypoints */ -}; - -enum VEHICLE_MANUAL_CONTROL_MODE { - VEHICLE_MANUAL_CONTROL_MODE_DIRECT = 0, /**< no attitude control, direct stick input mixing (only fixed wing) */ - VEHICLE_MANUAL_CONTROL_MODE_RATES, /**< body rates control mode */ - VEHICLE_MANUAL_CONTROL_MODE_SAS /**< stability augmented system (SAS) mode */ -}; - -enum VEHICLE_MANUAL_SAS_MODE { - VEHICLE_MANUAL_SAS_MODE_ROLL_PITCH_ABS_YAW_ABS = 0, /**< roll, pitch and yaw absolute */ - VEHICLE_MANUAL_SAS_MODE_ROLL_PITCH_ABS_YAW_RATE, /**< roll and pitch absolute, yaw rate */ - VEHICLE_MANUAL_SAS_MODE_SIMPLE, /**< simple mode (includes altitude hold) */ - VEHICLE_MANUAL_SAS_MODE_ALTITUDE /**< altitude hold */ -}; +//enum VEHICLE_FLIGHT_MODE { +// VEHICLE_FLIGHT_MODE_MANUAL = 0, /**< direct manual control, exact mode determined by VEHICLE_MANUAL_CONTROL_MODE */ +// VEHICLE_FLIGHT_MODE_STAB, /**< attitude or rate stabilization plus velocity or position stabilization */ +// VEHICLE_FLIGHT_MODE_HOLD, /**< hold current position (hover or loiter around position when switched) */ +// VEHICLE_FLIGHT_MODE_AUTO /**< attitude or rate stabilization plus absolute position control and waypoints */ +//}; +// +//enum VEHICLE_MANUAL_CONTROL_MODE { +// VEHICLE_MANUAL_CONTROL_MODE_DIRECT = 0, /**< no attitude control, direct stick input mixing (only fixed wing) */ +// VEHICLE_MANUAL_CONTROL_MODE_RATES, /**< body rates control mode */ +// VEHICLE_MANUAL_CONTROL_MODE_SAS /**< stability augmented system (SAS) mode */ +//}; +// +//enum VEHICLE_MANUAL_SAS_MODE { +// VEHICLE_MANUAL_SAS_MODE_ROLL_PITCH_ABS_YAW_ABS = 0, /**< roll, pitch and yaw absolute */ +// VEHICLE_MANUAL_SAS_MODE_ROLL_PITCH_ABS_YAW_RATE, /**< roll and pitch absolute, yaw rate */ +// VEHICLE_MANUAL_SAS_MODE_SIMPLE, /**< simple mode (includes altitude hold) */ +// VEHICLE_MANUAL_SAS_MODE_ALTITUDE /**< altitude hold */ +//}; /** * Should match 1:1 MAVLink's MAV_TYPE ENUM @@ -134,7 +143,7 @@ enum VEHICLE_TYPE { enum VEHICLE_BATTERY_WARNING { VEHICLE_BATTERY_WARNING_NONE = 0, /**< no battery low voltage warning active */ VEHICLE_BATTERY_WARNING_WARNING, /**< warning of low voltage 1. stage */ - VEHICLE_BATTERY_WARNING_ALERT /**< aleting of low voltage 2. stage */ + VEHICLE_BATTERY_WARNING_ALERT /**< alerting of low voltage 2. stage */ }; @@ -150,17 +159,17 @@ struct vehicle_status_s uint16_t counter; /**< incremented by the writing thread everytime new data is stored */ uint64_t timestamp; /**< in microseconds since system start, is set whenever the writing thread stores new data */ uint64_t failsave_lowlevel_start_time; /**< time when the lowlevel failsafe flag was set */ - //uint64_t failsave_highlevel_begin; TO BE COMPLETED +// uint64_t failsave_highlevel_begin; TO BE COMPLETED + + navigation_state_t navigation_state; /**< current navigation state */ + arming_state_t arming_state; /**< current arming state */ - commander_state_machine_t state_machine; /**< current flight state, main state machine */ - enum VEHICLE_FLIGHT_MODE flight_mode; /**< current flight mode, as defined by mode switch */ - enum VEHICLE_MANUAL_CONTROL_MODE manual_control_mode; /**< current attitude control mode, as defined by VEHICLE_ATTITUDE_MODE enum */ - enum VEHICLE_MANUAL_SAS_MODE manual_sas_mode; /**< current stabilization mode */ int32_t system_type; /**< system type, inspired by MAVLinks VEHICLE_TYPE enum */ /* system flags - these represent the state predicates */ bool flag_system_armed; /**< true is motors / actuators are armed */ + bool flag_system_emergency; bool flag_control_manual_enabled; /**< true if manual input is mixed in */ bool flag_control_offboard_enabled; /**< true if offboard control input is on */ bool flag_hil_enabled; /**< true if hardware in the loop simulation is enabled */ @@ -170,6 +179,9 @@ struct vehicle_status_s bool flag_control_velocity_enabled; /**< true if speed (implies direction) is controlled */ bool flag_control_position_enabled; /**< true if position is controlled */ + bool flag_land_requested; /**< true if land requested */ + bool flag_mission_activated; /**< true if in mission mode */ + bool flag_preflight_gyro_calibration; /**< true if gyro calibration is requested */ bool flag_preflight_mag_calibration; /**< true if mag calibration is requested */ bool flag_preflight_accel_calibration; @@ -185,7 +197,7 @@ struct vehicle_status_s uint64_t offboard_control_signal_lost_interval; /**< interval in microseconds without an offboard control message */ bool failsave_lowlevel; /**< Set to true if low-level failsafe mode is enabled */ - //bool failsave_highlevel; + bool failsave_highlevel; /* see SYS_STATUS mavlink message for the following */ uint32_t onboard_control_sensors_present; From 3bc385c789f2b39cda066551ff1d5b767ab26aec Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Sun, 17 Feb 2013 15:04:01 -0800 Subject: [PATCH 002/109] Checkpoint: Added arming state check --- apps/commander/commander.c | 6 +++++- apps/commander/state_machine_helper.c | 22 ++++++++++++++++++++++ apps/commander/state_machine_helper.h | 2 ++ apps/sensors/sensor_params.c | 10 ++++------ apps/sensors/sensors.cpp | 2 +- apps/uORB/topics/vehicle_status.h | 2 ++ 6 files changed, 36 insertions(+), 8 deletions(-) diff --git a/apps/commander/commander.c b/apps/commander/commander.c index 8716caef72..1bfdd56600 100644 --- a/apps/commander/commander.c +++ b/apps/commander/commander.c @@ -2054,9 +2054,13 @@ int commander_thread_main(int argc, char *argv[]) state_changed = false; } + /* make changes in state machine if needed */ + update_state_machine(stat_pub, ¤t_status, mavlink_fd); + + + /* Store old modes to detect and act on state transitions */ voltage_previous = current_status.voltage_battery; - fflush(stdout); counter++; usleep(COMMANDER_MONITORING_INTERVAL); diff --git a/apps/commander/state_machine_helper.c b/apps/commander/state_machine_helper.c index e1ec73110c..d89be781a8 100644 --- a/apps/commander/state_machine_helper.c +++ b/apps/commander/state_machine_helper.c @@ -53,6 +53,28 @@ #include "state_machine_helper.h" +void update_state_machine(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd) +{ + /* check arming first */ + if (current_status->flag_system_armed && current_status->flag_system_emergency) { + do_arming_state_update(status_pub, current_status, mavlink_fd, ARMING_STATE_MISSION_ABORT); + } else if(current_status->flag_system_armed && !current_status->flag_system_emergency) { + do_arming_state_update(status_pub, current_status, mavlink_fd, ARMING_STATE_ARMED); + } else if(!current_status->flag_system_armed && current_status->flag_system_emergency) { + do_arming_state_update(status_pub, current_status, mavlink_fd, ARMING_STATE_ERROR); + } else if(!current_status->flag_system_armed && !current_status->flag_system_emergency) { + do_arming_state_update(status_pub, current_status, mavlink_fd, ARMING_STATE_STANDBY); + } else if (current_status->flag_system_sensors_ok) { + do_arming_state_update(status_pub, current_status, mavlink_fd, ARMING_STATE_STANDBY); + } else if (!current_status->flag_system_sensors_ok && current_status->flag_system_armed) { + do_arming_state_update(status_pub, current_status, mavlink_fd, ARMING_STATE_MISSION_ABORT); + } else if (!current_status->flag_system_sensors_ok && !current_status->flag_system_armed) { + do_arming_state_update(status_pub, current_status, mavlink_fd, ARMING_STATE_ERROR); + } + + /* now determine the navigation state */ +} + /** * Transition from one navigation state to another */ diff --git a/apps/commander/state_machine_helper.h b/apps/commander/state_machine_helper.h index bf9296caf0..ed18bfbd24 100644 --- a/apps/commander/state_machine_helper.h +++ b/apps/commander/state_machine_helper.h @@ -47,6 +47,8 @@ #include #include +void update_state_machine(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd); + int do_navigation_state_update(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, navigation_state_t new_state); int do_arming_state_update(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, arming_state_t new_state); diff --git a/apps/sensors/sensor_params.c b/apps/sensors/sensor_params.c index 9f23ebbbab..df7f661161 100644 --- a/apps/sensors/sensor_params.c +++ b/apps/sensors/sensor_params.c @@ -158,13 +158,11 @@ PARAM_DEFINE_INT32(RC_MAP_PITCH, 2); PARAM_DEFINE_INT32(RC_MAP_THROTTLE, 3); PARAM_DEFINE_INT32(RC_MAP_YAW, 4); -PARAM_DEFINE_INT32(RC_MAP_OVER_SW, 5); -PARAM_DEFINE_INT32(RC_MAP_MODE_SW, 6); +PARAM_DEFINE_INT32(RC_MAP_MODE_SW, 5); +PARAM_DEFINE_INT32(RC_MAP_RETURN_SW, 6); +PARAM_DEFINE_INT32(RC_MAP_MISSIO_SW, 0); -PARAM_DEFINE_INT32(RC_MAP_MAN_SW, 0); -PARAM_DEFINE_INT32(RC_MAP_SAS_SW, 0); -PARAM_DEFINE_INT32(RC_MAP_RTL_SW, 0); -PARAM_DEFINE_INT32(RC_MAP_OFFB_SW, 0); +//PARAM_DEFINE_INT32(RC_MAP_OFFB_SW, 0); PARAM_DEFINE_INT32(RC_MAP_FLAPS, 0); diff --git a/apps/sensors/sensors.cpp b/apps/sensors/sensors.cpp index 28579bc703..ce404ee7e3 100644 --- a/apps/sensors/sensors.cpp +++ b/apps/sensors/sensors.cpp @@ -438,7 +438,7 @@ Sensors::Sensors() : _parameter_handles.rc_map_flaps = param_find("RC_MAP_FLAPS"); /* optional mode switches, not mapped per default */ - _parameter_handles.rc_map_mission_sw = param_find("RC_MAP_MISSION_SW"); + _parameter_handles.rc_map_mission_sw = param_find("RC_MAP_MISSIO_SW"); // _parameter_handles.rc_map_offboard_ctrl_mode_sw = param_find("RC_MAP_OFFB_SW"); diff --git a/apps/uORB/topics/vehicle_status.h b/apps/uORB/topics/vehicle_status.h index f9c4a5fff9..ba9b9793b0 100644 --- a/apps/uORB/topics/vehicle_status.h +++ b/apps/uORB/topics/vehicle_status.h @@ -170,6 +170,8 @@ struct vehicle_status_s bool flag_system_armed; /**< true is motors / actuators are armed */ bool flag_system_emergency; + bool flag_system_sensors_ok; + bool flag_control_manual_enabled; /**< true if manual input is mixed in */ bool flag_control_offboard_enabled; /**< true if offboard control input is on */ bool flag_hil_enabled; /**< true if hardware in the loop simulation is enabled */ From 47b05eeb87191fd0b380de008299f85262bc8953 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Sun, 17 Feb 2013 23:07:07 -0800 Subject: [PATCH 003/109] Checkpoint, arming/disarming still has a bug --- apps/commander/commander.c | 225 ++++++++++----------- apps/commander/state_machine_helper.c | 270 +++++++++++++++----------- apps/commander/state_machine_helper.h | 2 - apps/mavlink/mavlink.c | 15 +- apps/uORB/topics/vehicle_status.h | 34 +++- 5 files changed, 303 insertions(+), 243 deletions(-) diff --git a/apps/commander/commander.c b/apps/commander/commander.c index 1bfdd56600..4e470f1d92 100644 --- a/apps/commander/commander.c +++ b/apps/commander/commander.c @@ -817,22 +817,24 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta case VEHICLE_CMD_COMPONENT_ARM_DISARM: { /* request to arm */ if ((int)cmd->param1 == 1) { - if (OK == do_arming_state_update(status_pub, current_vehicle_status, mavlink_fd, ARMING_STATE_ARMED)) { - result = VEHICLE_CMD_RESULT_ACCEPTED; - - } else { - result = VEHICLE_CMD_RESULT_DENIED; - } + // XXX add this again +// if (OK == do_arming_state_update(status_pub, current_vehicle_status, mavlink_fd, ARMING_STATE_ARMED)) { +// result = VEHICLE_CMD_RESULT_ACCEPTED; +// +// } else { +// result = VEHICLE_CMD_RESULT_DENIED; +// } /* request to disarm */ // XXX this arms it instad of disarming } else if ((int)cmd->param1 == 0) { - if (OK == do_arming_state_update(status_pub, current_vehicle_status, mavlink_fd, ARMING_STATE_STANDBY)) { - result = VEHICLE_CMD_RESULT_ACCEPTED; - - } else { - result = VEHICLE_CMD_RESULT_DENIED; - } + // XXX add this again +// if (OK == do_arming_state_update(status_pub, current_vehicle_status, mavlink_fd, ARMING_STATE_STANDBY)) { +// result = VEHICLE_CMD_RESULT_ACCEPTED; +// +// } else { +// result = VEHICLE_CMD_RESULT_DENIED; +// } } } break; @@ -840,7 +842,7 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta /* request for an autopilot reboot */ case VEHICLE_CMD_PREFLIGHT_REBOOT_SHUTDOWN: { if ((int)cmd->param1 == 1) { - if (OK == do_navigation_state_update(status_pub, current_vehicle_status, mavlink_fd, NAVIGATION_STATE_REBOOT)) { + if (OK == do_arming_state_update(status_pub, current_vehicle_status, mavlink_fd, ARMING_STATE_REBOOT)) { /* SPECIAL CASE: SYSTEM WILL NEVER RETURN HERE */ result = VEHICLE_CMD_RESULT_ACCEPTED; @@ -883,25 +885,26 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta if ((int)(cmd->param1) == 1) { /* transition to init state */ - if (OK == do_arming_state_update(status_pub, ¤t_status, mavlink_fd, ARMING_STATE_INIT) - && OK == do_navigation_state_update(status_pub, ¤t_status, mavlink_fd, NAVIGATION_STATE_INIT)) { - - mavlink_log_info(mavlink_fd, "starting gyro cal"); - tune_confirm(); - do_gyro_calibration(status_pub, ¤t_status); - mavlink_log_info(mavlink_fd, "finished gyro cal"); - tune_confirm(); - - /* back to standby state */ - do_arming_state_update(status_pub, ¤t_status, mavlink_fd, ARMING_STATE_STANDBY); - do_navigation_state_update(status_pub, ¤t_status, mavlink_fd, NAVIGATION_STATE_STANDBY); - - result = VEHICLE_CMD_RESULT_ACCEPTED; - - } else { - mavlink_log_critical(mavlink_fd, "REJECTING gyro cal"); - result = VEHICLE_CMD_RESULT_DENIED; - } + // XXX add this again +// if (OK == do_arming_state_update(status_pub, ¤t_status, mavlink_fd, ARMING_STATE_INIT) +// && OK == do_navigation_state_update(status_pub, ¤t_status, mavlink_fd, NAVIGATION_STATE_INIT)) { +// +// mavlink_log_info(mavlink_fd, "starting gyro cal"); +// tune_confirm(); +// do_gyro_calibration(status_pub, ¤t_status); +// mavlink_log_info(mavlink_fd, "finished gyro cal"); +// tune_confirm(); +// +// /* back to standby state */ +// do_arming_state_update(status_pub, ¤t_status, mavlink_fd, ARMING_STATE_STANDBY); +// do_navigation_state_update(status_pub, ¤t_status, mavlink_fd, NAVIGATION_STATE_STANDBY); +// +// result = VEHICLE_CMD_RESULT_ACCEPTED; +// +// } else { +// mavlink_log_critical(mavlink_fd, "REJECTING gyro cal"); +// result = VEHICLE_CMD_RESULT_DENIED; +// } handled = true; } @@ -910,24 +913,25 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta if ((int)(cmd->param2) == 1) { /* transition to init state */ - if (OK == do_arming_state_update(status_pub, ¤t_status, mavlink_fd, ARMING_STATE_INIT) - && OK == do_navigation_state_update(status_pub, ¤t_status, mavlink_fd, NAVIGATION_STATE_INIT)) { - mavlink_log_info(mavlink_fd, "starting mag cal"); - tune_confirm(); - do_mag_calibration(status_pub, ¤t_status); - mavlink_log_info(mavlink_fd, "finished mag cal"); - tune_confirm(); - - /* back to standby state */ - do_arming_state_update(status_pub, ¤t_status, mavlink_fd, ARMING_STATE_STANDBY); - do_navigation_state_update(status_pub, ¤t_status, mavlink_fd, NAVIGATION_STATE_STANDBY); - - result = VEHICLE_CMD_RESULT_ACCEPTED; - - } else { - mavlink_log_critical(mavlink_fd, "REJECTING mag cal"); - result = VEHICLE_CMD_RESULT_DENIED; - } + // XXX add this again +// if (OK == do_arming_state_update(status_pub, ¤t_status, mavlink_fd, ARMING_STATE_INIT) +// && OK == do_navigation_state_update(status_pub, ¤t_status, mavlink_fd, NAVIGATION_STATE_INIT)) { +// mavlink_log_info(mavlink_fd, "starting mag cal"); +// tune_confirm(); +// do_mag_calibration(status_pub, ¤t_status); +// mavlink_log_info(mavlink_fd, "finished mag cal"); +// tune_confirm(); +// +// /* back to standby state */ +// do_arming_state_update(status_pub, ¤t_status, mavlink_fd, ARMING_STATE_STANDBY); +// do_navigation_state_update(status_pub, ¤t_status, mavlink_fd, NAVIGATION_STATE_STANDBY); +// +// result = VEHICLE_CMD_RESULT_ACCEPTED; +// +// } else { +// mavlink_log_critical(mavlink_fd, "REJECTING mag cal"); +// result = VEHICLE_CMD_RESULT_DENIED; +// } handled = true; } @@ -935,21 +939,22 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta /* zero-altitude pressure calibration */ if ((int)(cmd->param3) == 1) { /* transition to calibration state */ - if (OK == do_arming_state_update(status_pub, ¤t_status, mavlink_fd, ARMING_STATE_INIT) - && OK == do_navigation_state_update(status_pub, ¤t_status, mavlink_fd, NAVIGATION_STATE_INIT)) { - - // XXX implement this - mavlink_log_info(mavlink_fd, "zero altitude cal. not implemented"); - tune_confirm(); - - /* back to standby state */ - do_arming_state_update(status_pub, ¤t_status, mavlink_fd, ARMING_STATE_STANDBY); - do_navigation_state_update(status_pub, ¤t_status, mavlink_fd, NAVIGATION_STATE_STANDBY); - - } else { - mavlink_log_critical(mavlink_fd, "REJECTING altitude calibration"); - result = VEHICLE_CMD_RESULT_DENIED; - } + // XXX add this again +// if (OK == do_arming_state_update(status_pub, ¤t_status, mavlink_fd, ARMING_STATE_INIT) +// && OK == do_navigation_state_update(status_pub, ¤t_status, mavlink_fd, NAVIGATION_STATE_INIT)) { +// +// // XXX implement this +// mavlink_log_info(mavlink_fd, "zero altitude cal. not implemented"); +// tune_confirm(); +// +// /* back to standby state */ +// do_arming_state_update(status_pub, ¤t_status, mavlink_fd, ARMING_STATE_STANDBY); +// do_navigation_state_update(status_pub, ¤t_status, mavlink_fd, NAVIGATION_STATE_STANDBY); +// +// } else { +// mavlink_log_critical(mavlink_fd, "REJECTING altitude calibration"); +// result = VEHICLE_CMD_RESULT_DENIED; +// } handled = true; } @@ -957,49 +962,51 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta /* trim calibration */ if ((int)(cmd->param4) == 1) { /* transition to calibration state */ - if (OK == do_arming_state_update(status_pub, ¤t_status, mavlink_fd, ARMING_STATE_INIT) - && OK == do_navigation_state_update(status_pub, ¤t_status, mavlink_fd, NAVIGATION_STATE_INIT)) { - mavlink_log_info(mavlink_fd, "starting trim cal"); - tune_confirm(); - do_rc_calibration(status_pub, ¤t_status); - mavlink_log_info(mavlink_fd, "finished trim cal"); - tune_confirm(); - - /* back to standby state */ - do_arming_state_update(status_pub, ¤t_status, mavlink_fd, ARMING_STATE_STANDBY); - do_navigation_state_update(status_pub, ¤t_status, mavlink_fd, NAVIGATION_STATE_STANDBY); - - result = VEHICLE_CMD_RESULT_ACCEPTED; - - } else { - mavlink_log_critical(mavlink_fd, "REJECTING trim cal"); - result = VEHICLE_CMD_RESULT_DENIED; - } + // XXX add this again +// if (OK == do_arming_state_update(status_pub, ¤t_status, mavlink_fd, ARMING_STATE_INIT) +// && OK == do_navigation_state_update(status_pub, ¤t_status, mavlink_fd, NAVIGATION_STATE_INIT)) { +// mavlink_log_info(mavlink_fd, "starting trim cal"); +// tune_confirm(); +// do_rc_calibration(status_pub, ¤t_status); +// mavlink_log_info(mavlink_fd, "finished trim cal"); +// tune_confirm(); +// +// /* back to standby state */ +// do_arming_state_update(status_pub, ¤t_status, mavlink_fd, ARMING_STATE_STANDBY); +// do_navigation_state_update(status_pub, ¤t_status, mavlink_fd, NAVIGATION_STATE_STANDBY); +// +// result = VEHICLE_CMD_RESULT_ACCEPTED; +// +// } else { +// mavlink_log_critical(mavlink_fd, "REJECTING trim cal"); +// result = VEHICLE_CMD_RESULT_DENIED; +// } handled = true; } /* accel calibration */ if ((int)(cmd->param5) == 1) { - if (OK == do_arming_state_update(status_pub, ¤t_status, mavlink_fd, ARMING_STATE_INIT) - && OK == do_navigation_state_update(status_pub, ¤t_status, mavlink_fd, NAVIGATION_STATE_INIT)) { - - mavlink_log_info(mavlink_fd, "CMD starting accel cal"); - tune_confirm(); - do_accel_calibration(status_pub, ¤t_status); - tune_confirm(); - mavlink_log_info(mavlink_fd, "CMD finished accel cal"); - - /* back to standby state */ - do_arming_state_update(status_pub, ¤t_status, mavlink_fd, ARMING_STATE_STANDBY); - do_navigation_state_update(status_pub, ¤t_status, mavlink_fd, NAVIGATION_STATE_STANDBY); - - result = VEHICLE_CMD_RESULT_ACCEPTED; - - } else { - mavlink_log_critical(mavlink_fd, "REJECTING accel cal"); - result = VEHICLE_CMD_RESULT_DENIED; - } + // XXX add this again +// if (OK == do_arming_state_update(status_pub, ¤t_status, mavlink_fd, ARMING_STATE_INIT) +// && OK == do_navigation_state_update(status_pub, ¤t_status, mavlink_fd, NAVIGATION_STATE_INIT)) { +// +// mavlink_log_info(mavlink_fd, "CMD starting accel cal"); +// tune_confirm(); +// do_accel_calibration(status_pub, ¤t_status); +// tune_confirm(); +// mavlink_log_info(mavlink_fd, "CMD finished accel cal"); +// +// /* back to standby state */ +// do_arming_state_update(status_pub, ¤t_status, mavlink_fd, ARMING_STATE_STANDBY); +// do_navigation_state_update(status_pub, ¤t_status, mavlink_fd, NAVIGATION_STATE_STANDBY); +// +// result = VEHICLE_CMD_RESULT_ACCEPTED; +// +// } else { +// mavlink_log_critical(mavlink_fd, "REJECTING accel cal"); +// result = VEHICLE_CMD_RESULT_DENIED; +// } handled = true; } @@ -1315,7 +1322,7 @@ int commander_thread_main(int argc, char *argv[]) /* make sure we are in preflight state */ memset(¤t_status, 0, sizeof(current_status)); - current_status.navigation_state = NAVIGATION_STATE_INIT; + current_status.navigation_state = NAVIGATION_STATE_STANDBY; current_status.arming_state = ARMING_STATE_INIT; current_status.flag_system_armed = false; @@ -1336,6 +1343,9 @@ int commander_thread_main(int argc, char *argv[]) /* set battery warning flag */ current_status.battery_warning = VEHICLE_BATTERY_WARNING_NONE; + // XXX for now just set sensors as initialized + current_status.flag_system_sensors_initialized = true; + /* advertise to ORB */ stat_pub = orb_advertise(ORB_ID(vehicle_status), ¤t_status); /* publish current state machine */ @@ -1886,8 +1896,8 @@ int commander_thread_main(int argc, char *argv[]) ((sp_man.yaw < -STICK_ON_OFF_LIMIT)) && (sp_man.throttle < STICK_THRUST_RANGE * 0.2f)) { if (stick_off_counter > STICK_ON_OFF_COUNTER_LIMIT) { - current_status.flag_system_armed = false; - stick_on_counter = 0; + do_arming_state_update(stat_pub, ¤t_status, mavlink_fd, ARMING_STATE_STANDBY); + stick_off_counter = 0; } else { stick_off_counter++; @@ -1898,7 +1908,7 @@ 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 (stick_on_counter > STICK_ON_OFF_COUNTER_LIMIT) { - current_status.flag_system_armed = true; + do_arming_state_update(stat_pub, ¤t_status, mavlink_fd, ARMING_STATE_ARMED); stick_on_counter = 0; } else { @@ -2054,9 +2064,6 @@ int commander_thread_main(int argc, char *argv[]) state_changed = false; } - /* make changes in state machine if needed */ - update_state_machine(stat_pub, ¤t_status, mavlink_fd); - /* Store old modes to detect and act on state transitions */ diff --git a/apps/commander/state_machine_helper.c b/apps/commander/state_machine_helper.c index d89be781a8..a0b786aabb 100644 --- a/apps/commander/state_machine_helper.c +++ b/apps/commander/state_machine_helper.c @@ -53,33 +53,12 @@ #include "state_machine_helper.h" -void update_state_machine(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd) -{ - /* check arming first */ - if (current_status->flag_system_armed && current_status->flag_system_emergency) { - do_arming_state_update(status_pub, current_status, mavlink_fd, ARMING_STATE_MISSION_ABORT); - } else if(current_status->flag_system_armed && !current_status->flag_system_emergency) { - do_arming_state_update(status_pub, current_status, mavlink_fd, ARMING_STATE_ARMED); - } else if(!current_status->flag_system_armed && current_status->flag_system_emergency) { - do_arming_state_update(status_pub, current_status, mavlink_fd, ARMING_STATE_ERROR); - } else if(!current_status->flag_system_armed && !current_status->flag_system_emergency) { - do_arming_state_update(status_pub, current_status, mavlink_fd, ARMING_STATE_STANDBY); - } else if (current_status->flag_system_sensors_ok) { - do_arming_state_update(status_pub, current_status, mavlink_fd, ARMING_STATE_STANDBY); - } else if (!current_status->flag_system_sensors_ok && current_status->flag_system_armed) { - do_arming_state_update(status_pub, current_status, mavlink_fd, ARMING_STATE_MISSION_ABORT); - } else if (!current_status->flag_system_sensors_ok && !current_status->flag_system_armed) { - do_arming_state_update(status_pub, current_status, mavlink_fd, ARMING_STATE_ERROR); - } - - /* now determine the navigation state */ -} - /** * Transition from one navigation state to another */ int do_navigation_state_update(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, navigation_state_t new_state) { + bool valid_path = false; bool valid_transition = false; int ret = ERROR; @@ -89,41 +68,43 @@ int do_navigation_state_update(int status_pub, struct vehicle_status_s *current_ } else { switch (new_state) { - case NAVIGATION_STATE_INIT: - - if (current_status->navigation_state == NAVIGATION_STATE_STANDBY) { - - mavlink_log_critical(mavlink_fd, "SWITCHED TO INIT STATE"); - valid_transition = true; - } - break; - case NAVIGATION_STATE_STANDBY: - if (current_status->navigation_state == NAVIGATION_STATE_INIT - || current_status->navigation_state == NAVIGATION_STATE_MANUAL + if (current_status->navigation_state == NAVIGATION_STATE_MANUAL || current_status->navigation_state == NAVIGATION_STATE_AUTO_READY) { - mavlink_log_critical(mavlink_fd, "SWITCHED TO STANDBY STATE"); - valid_transition = true; + if (!current_status->flag_system_armed) { + mavlink_log_critical(mavlink_fd, "Switched to STANDBY state"); + valid_transition = true; + } else { + mavlink_log_critical(mavlink_fd, "Rejected STANDBY state: armed"); + } + valid_path = true; } break; case NAVIGATION_STATE_MANUAL: - if ( - ( current_status->navigation_state == NAVIGATION_STATE_STANDBY - || current_status->navigation_state == NAVIGATION_STATE_SEATBELT - || current_status->navigation_state == NAVIGATION_STATE_LOITER - || current_status->navigation_state == NAVIGATION_STATE_MISSION - || current_status->navigation_state == NAVIGATION_STATE_RTL - || current_status->navigation_state == NAVIGATION_STATE_LAND - || current_status->navigation_state == NAVIGATION_STATE_TAKEOFF - || current_status->navigation_state == NAVIGATION_STATE_AUTO_READY) - && current_status->arming_state == ARMING_STATE_ARMED) { + if (current_status->navigation_state == NAVIGATION_STATE_STANDBY) { - mavlink_log_critical(mavlink_fd, "SWITCHED TO MANUAL STATE"); + /* only check for armed flag when coming from STANDBY XXX does that make sense? */ + if (current_status->flag_system_armed) { + mavlink_log_critical(mavlink_fd, "Switched to MANUAL state"); + valid_transition = true; + } else { + mavlink_log_critical(mavlink_fd, "Rejected MANUAL state: disarmed"); + } + valid_path = true; + } else if (current_status->navigation_state == NAVIGATION_STATE_SEATBELT + || current_status->navigation_state == NAVIGATION_STATE_LOITER + || current_status->navigation_state == NAVIGATION_STATE_MISSION + || current_status->navigation_state == NAVIGATION_STATE_RTL + || current_status->navigation_state == NAVIGATION_STATE_LAND + || current_status->navigation_state == NAVIGATION_STATE_TAKEOFF + || current_status->navigation_state == NAVIGATION_STATE_AUTO_READY) { + mavlink_log_critical(mavlink_fd, "Switched to MANUAL state"); valid_transition = true; + valid_path = true; } break; @@ -133,67 +114,132 @@ int do_navigation_state_update(int status_pub, struct vehicle_status_s *current_ || current_status->navigation_state == NAVIGATION_STATE_LOITER || current_status->navigation_state == NAVIGATION_STATE_MISSION) { - mavlink_log_critical(mavlink_fd, "SWITCHED TO SEATBELT STATE"); + mavlink_log_critical(mavlink_fd, "Switched to SEATBELT state"); valid_transition = true; + valid_path = true; } break; case NAVIGATION_STATE_LOITER: - if ( ((current_status->navigation_state == NAVIGATION_STATE_MANUAL || current_status->navigation_state == NAVIGATION_STATE_SEATBELT) - && current_status->flag_global_position_valid) - || current_status->navigation_state == NAVIGATION_STATE_MISSION) { - - mavlink_log_critical(mavlink_fd, "SWITCHED TO LOITER STATE"); + /* Check for position lock when coming from MANUAL or SEATBELT */ + if (current_status->navigation_state == NAVIGATION_STATE_MANUAL || current_status->navigation_state == NAVIGATION_STATE_SEATBELT) { + mavlink_log_critical(mavlink_fd, "Switched to LOITER state"); valid_transition = true; - } + if (current_status->flag_global_position_valid) { + mavlink_log_critical(mavlink_fd, "Switched to LOITER state"); + valid_transition = true; + + } else { + mavlink_log_critical(mavlink_fd, "Rejected LOITER state: no pos lock"); + } + valid_path = true; + } else if (current_status->navigation_state == NAVIGATION_STATE_MISSION) { + mavlink_log_critical(mavlink_fd, "Switched to LOITER state"); + valid_transition = true; + valid_path = true; + } break; case NAVIGATION_STATE_AUTO_READY: - if ( - (current_status->navigation_state == NAVIGATION_STATE_STANDBY - && current_status->flag_global_position_valid - && current_status->flag_valid_launch_position) - || current_status->navigation_state == NAVIGATION_STATE_LAND) { + /* coming from STANDBY pos and home lock are needed */ + if (current_status->navigation_state == NAVIGATION_STATE_STANDBY) { - mavlink_log_critical(mavlink_fd, "SWITCHED TO AUTO READY STATE"); - valid_transition = true; + if (current_status->flag_global_position_valid && current_status->flag_valid_launch_position) { + mavlink_log_critical(mavlink_fd, "Switched to AUTO READY state"); + valid_transition = true; + } else if (!current_status->flag_global_position_valid) { + mavlink_log_critical(mavlink_fd, "Rejected AUTO READY state: no pos lock"); + } else if (!current_status->flag_valid_launch_position) { + mavlink_log_critical(mavlink_fd, "Rejected AUTO READY state: no home lock"); + } else { + mavlink_log_critical(mavlink_fd, "Rejected AUTO READY state: no pos and home lock"); + } + valid_path = true; + /* coming from LAND home lock is needed */ + } else if (current_status->navigation_state == NAVIGATION_STATE_LAND) { + + if (current_status->flag_valid_launch_position) { + mavlink_log_critical(mavlink_fd, "Switched to AUTO READY state"); + valid_transition = true; + } else { + mavlink_log_critical(mavlink_fd, "Rejected AUTO READY state: no home lock"); + } + valid_path = true; } break; case NAVIGATION_STATE_MISSION: - if ( - current_status->navigation_state == NAVIGATION_STATE_TAKEOFF - || current_status->navigation_state == NAVIGATION_STATE_RTL - || ( - (current_status->navigation_state == NAVIGATION_STATE_MANUAL - || current_status->navigation_state == NAVIGATION_STATE_SEATBELT - || current_status->navigation_state == NAVIGATION_STATE_LOITER) - && current_status->flag_global_position_valid - && current_status->flag_valid_launch_position) - ) { - - mavlink_log_critical(mavlink_fd, "SWITCHED TO MISSION STATE"); + /* coming from TAKEOFF or RTL is always possible */ + if (current_status->navigation_state == NAVIGATION_STATE_TAKEOFF + || current_status->navigation_state == NAVIGATION_STATE_RTL) { + mavlink_log_critical(mavlink_fd, "Switched to MISSION state"); valid_transition = true; + valid_path = true; + + /* coming from MANUAL or SEATBELT requires home and pos lock */ + } else if (current_status->navigation_state == NAVIGATION_STATE_MANUAL + || current_status->navigation_state == NAVIGATION_STATE_SEATBELT) { + + if (current_status->flag_global_position_valid && current_status->flag_valid_launch_position) { + mavlink_log_critical(mavlink_fd, "Switched to MISSION state"); + valid_transition = true; + } else if (!current_status->flag_global_position_valid) { + mavlink_log_critical(mavlink_fd, "Rejected MISSION state: no pos lock"); + } else if (!current_status->flag_valid_launch_position) { + mavlink_log_critical(mavlink_fd, "Rejected MISSION state: no home lock"); + } else { + mavlink_log_critical(mavlink_fd, "Rejected MISSION state: no pos and home lock"); + } + valid_path = true; + + /* coming from loiter a home lock is needed */ + } else if (current_status->navigation_state == NAVIGATION_STATE_LOITER) { + if (current_status->flag_valid_launch_position) { + mavlink_log_critical(mavlink_fd, "Switched to MISSION state"); + valid_transition = true; + } else { + mavlink_log_critical(mavlink_fd, "Rejected MISSION state: no home lock"); + } + valid_path = true; } break; case NAVIGATION_STATE_RTL: - if ( - current_status->navigation_state == NAVIGATION_STATE_MISSION - || ( - (current_status->navigation_state == NAVIGATION_STATE_MANUAL - || current_status->navigation_state == NAVIGATION_STATE_SEATBELT - || current_status->navigation_state == NAVIGATION_STATE_LOITER) - && current_status->flag_global_position_valid - && current_status->flag_valid_launch_position) - ) { - - mavlink_log_critical(mavlink_fd, "SWITCHED TO RTL STATE"); + /* coming from MISSION is always possible */ + if (current_status->navigation_state == NAVIGATION_STATE_MISSION) { + mavlink_log_critical(mavlink_fd, "Switched to RTL state"); valid_transition = true; + valid_path = true; + + /* coming from MANUAL or SEATBELT requires home and pos lock */ + } else if (current_status->navigation_state == NAVIGATION_STATE_MANUAL + || current_status->navigation_state == NAVIGATION_STATE_SEATBELT) { + + if (current_status->flag_global_position_valid && current_status->flag_valid_launch_position) { + mavlink_log_critical(mavlink_fd, "Switched to RTL state"); + valid_transition = true; + } else if (!current_status->flag_global_position_valid) { + mavlink_log_critical(mavlink_fd, "Rejected RTL state: no pos lock"); + } else if (!current_status->flag_valid_launch_position) { + mavlink_log_critical(mavlink_fd, "Rejected RTL state: no home lock"); + } else { + mavlink_log_critical(mavlink_fd, "Rejected RTL state: no pos and home lock"); + } + valid_path = true; + + /* coming from loiter a home lock is needed */ + } else if (current_status->navigation_state == NAVIGATION_STATE_LOITER) { + if (current_status->flag_valid_launch_position) { + mavlink_log_critical(mavlink_fd, "Switched to RTL state"); + valid_transition = true; + } else { + mavlink_log_critical(mavlink_fd, "Rejected RTL state: no home lock"); + } + valid_path = true; } break; @@ -201,8 +247,10 @@ int do_navigation_state_update(int status_pub, struct vehicle_status_s *current_ if (current_status->navigation_state == NAVIGATION_STATE_AUTO_READY) { - mavlink_log_critical(mavlink_fd, "SWITCHED TO TAKEOFF STATE"); + /* TAKEOFF is straight forward from AUTO READY */ + mavlink_log_critical(mavlink_fd, "Switched to TAKEOFF state"); valid_transition = true; + valid_path = true; } break; @@ -210,29 +258,12 @@ int do_navigation_state_update(int status_pub, struct vehicle_status_s *current_ if (current_status->navigation_state == NAVIGATION_STATE_RTL || current_status->navigation_state == NAVIGATION_STATE_LOITER) { - mavlink_log_critical(mavlink_fd, "SWITCHED TO LAND STATE"); + mavlink_log_critical(mavlink_fd, "Switched to LAND state"); valid_transition = true; + valid_path = true; } break; - case NAVIGATION_STATE_REBOOT: - if (current_status->navigation_state == NAVIGATION_STATE_STANDBY - || current_status->navigation_state == NAVIGATION_STATE_INIT - || current_status->flag_hil_enabled) { - valid_transition = true; - /* set system flags according to state */ - current_status->flag_system_armed = false; - mavlink_log_critical(mavlink_fd, "REBOOTING SYSTEM"); - usleep(500000); - up_systemreset(); - /* SPECIAL CASE: NEVER RETURNS FROM THIS FUNCTION CALL */ - - } else { - mavlink_log_critical(mavlink_fd, "REFUSED to REBOOT"); - } - - break; - default: warnx("Unknown navigation state"); break; @@ -244,7 +275,9 @@ int do_navigation_state_update(int status_pub, struct vehicle_status_s *current_ state_machine_publish(status_pub, current_status, mavlink_fd); // publish_armed_status(current_status); ret = OK; - } else { + } + + if (!valid_path){ mavlink_log_critical(mavlink_fd, "REJECTING invalid navigation state transition"); } @@ -269,7 +302,8 @@ int do_arming_state_update(int status_pub, struct vehicle_status_s *current_stat case ARMING_STATE_INIT: if (current_status->arming_state == ARMING_STATE_STANDBY) { - mavlink_log_critical(mavlink_fd, "SWITCHED TO INIT ARMING STATE"); + + mavlink_log_critical(mavlink_fd, "Switched to INIT ARMING STATE"); valid_transition = true; } break; @@ -280,8 +314,12 @@ int do_arming_state_update(int status_pub, struct vehicle_status_s *current_stat // XXX check if coming from reboot? if (current_status->arming_state == ARMING_STATE_INIT) { - mavlink_log_critical(mavlink_fd, "SWITCHED TO STANDBY ARMING STATE"); - valid_transition = true; + if (current_status->flag_system_sensors_initialized) { + mavlink_log_critical(mavlink_fd, "Switched to STANDBY arming state"); + valid_transition = true; + } else { + mavlink_log_critical(mavlink_fd, "REJ. STANDBY arming state, sensors not init."); + } } break; @@ -290,16 +328,16 @@ int do_arming_state_update(int status_pub, struct vehicle_status_s *current_stat if (current_status->arming_state == ARMING_STATE_STANDBY || current_status->arming_state == ARMING_STATE_IN_AIR_RESTORE) { - mavlink_log_critical(mavlink_fd, "SWITCHED TO ARMED ARMING STATE"); + mavlink_log_critical(mavlink_fd, "Switched to ARMED arming state"); valid_transition = true; } break; - case ARMING_STATE_MISSION_ABORT: + case ARMING_STATE_ABORT: if (current_status->arming_state == ARMING_STATE_ARMED) { - mavlink_log_critical(mavlink_fd, "SWITCHED TO MISSION ABORT ARMING STATE"); + mavlink_log_critical(mavlink_fd, "Switched to MISSION ABORT arming state"); valid_transition = true; } break; @@ -307,10 +345,10 @@ int do_arming_state_update(int status_pub, struct vehicle_status_s *current_stat case ARMING_STATE_ERROR: if (current_status->arming_state == ARMING_STATE_ARMED - || current_status->arming_state == ARMING_STATE_MISSION_ABORT + || current_status->arming_state == ARMING_STATE_ABORT || current_status->arming_state == ARMING_STATE_INIT) { - mavlink_log_critical(mavlink_fd, "SWITCHED TO ERROR ARMING STATE"); + mavlink_log_critical(mavlink_fd, "Switched to ERROR arming state"); valid_transition = true; } break; @@ -320,16 +358,18 @@ int do_arming_state_update(int status_pub, struct vehicle_status_s *current_stat if (current_status->arming_state == ARMING_STATE_STANDBY || current_status->arming_state == ARMING_STATE_ERROR) { - mavlink_log_critical(mavlink_fd, "SWITCHED TO REBOOT ARMING STATE"); valid_transition = true; - // XXX reboot here? + mavlink_log_critical(mavlink_fd, "REBOOTING SYSTEM"); + usleep(500000); + up_systemreset(); + /* SPECIAL CASE: NEVER RETURNS FROM THIS FUNCTION CALL */ } break; case ARMING_STATE_IN_AIR_RESTORE: if (current_status->arming_state == ARMING_STATE_INIT) { - mavlink_log_critical(mavlink_fd, "SWITCHED TO IN-AIR-RESTORE ARMING STATE"); + mavlink_log_critical(mavlink_fd, "Switched to IN-AIR-RESTORE arming state"); valid_transition = true; } break; diff --git a/apps/commander/state_machine_helper.h b/apps/commander/state_machine_helper.h index ed18bfbd24..bf9296caf0 100644 --- a/apps/commander/state_machine_helper.h +++ b/apps/commander/state_machine_helper.h @@ -47,8 +47,6 @@ #include #include -void update_state_machine(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd); - int do_navigation_state_update(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, navigation_state_t new_state); int do_arming_state_update(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, arming_state_t new_state); diff --git a/apps/mavlink/mavlink.c b/apps/mavlink/mavlink.c index 4636ee36ec..e25f1be27f 100644 --- a/apps/mavlink/mavlink.c +++ b/apps/mavlink/mavlink.c @@ -248,26 +248,19 @@ get_mavlink_mode_and_state(uint8_t *mavlink_state, uint8_t *mavlink_mode) *mavlink_state = MAV_STATE_CALIBRATING; - } else if (v_status.flag_system_emergency) { + } else if (v_status.arming_state == ARMING_STATE_ERROR || v_status.arming_state == ARMING_STATE_ABORT) { *mavlink_state = MAV_STATE_EMERGENCY; - } else if (v_status.navigation_state == NAVIGATION_STATE_MANUAL - || v_status.navigation_state == NAVIGATION_STATE_SEATBELT - || v_status.navigation_state == NAVIGATION_STATE_LOITER - || v_status.navigation_state == NAVIGATION_STATE_MISSION - || v_status.navigation_state == NAVIGATION_STATE_RTL - || v_status.navigation_state == NAVIGATION_STATE_LAND - || v_status.navigation_state == NAVIGATION_STATE_TAKEOFF - || v_status.navigation_state == NAVIGATION_STATE_AUTO_READY) { + } else if (v_status.arming_state == ARMING_STATE_ARMED || v_status.arming_state == ARMING_STATE_IN_AIR_RESTORE) { *mavlink_state = MAV_STATE_ACTIVE; - } else if (v_status.navigation_state == NAVIGATION_STATE_STANDBY) { + } else if (v_status.arming_state == ARMING_STATE_STANDBY) { *mavlink_state = MAV_STATE_STANDBY; - } else if (v_status.navigation_state == NAVIGATION_STATE_INIT) { + } else if (v_status.arming_state == ARMING_STATE_INIT) { *mavlink_state = MAV_STATE_UNINIT; } else { diff --git a/apps/uORB/topics/vehicle_status.h b/apps/uORB/topics/vehicle_status.h index ba9b9793b0..874cf256c3 100644 --- a/apps/uORB/topics/vehicle_status.h +++ b/apps/uORB/topics/vehicle_status.h @@ -60,8 +60,7 @@ /* State Machine */ typedef enum { - NAVIGATION_STATE_INIT = 0, - NAVIGATION_STATE_STANDBY, + NAVIGATION_STATE_STANDBY=0, NAVIGATION_STATE_MANUAL, NAVIGATION_STATE_SEATBELT, NAVIGATION_STATE_LOITER, @@ -70,15 +69,13 @@ typedef enum { NAVIGATION_STATE_RTL, NAVIGATION_STATE_TAKEOFF, NAVIGATION_STATE_LAND, - NAVIGATION_STATE_GROUND_ERROR, - NAVIGATION_STATE_REBOOT } navigation_state_t; typedef enum { ARMING_STATE_INIT = 0, ARMING_STATE_STANDBY, ARMING_STATE_ARMED, - ARMING_STATE_MISSION_ABORT, + ARMING_STATE_ABORT, ARMING_STATE_ERROR, ARMING_STATE_REBOOT, ARMING_STATE_IN_AIR_RESTORE @@ -95,6 +92,22 @@ enum VEHICLE_MODE_FLAG { VEHICLE_MODE_FLAG_CUSTOM_MODE_ENABLED = 1 }; /**< Same as MAV_MODE_FLAG of MAVLink 1.0 protocol */ +typedef enum { + MODE_SWITCH_MANUAL = 0, + MODE_SWITCH_ASSISTED, + MODE_SWITCH_AUTO +} mode_switch_pos_t; + +typedef enum { + RETURN_SWITCH_NONE = 0, + RETURN_SWITCH_RETURN +} return_switch_pos_t; + +typedef enum { + MISSION_SWITCH_NONE = 0, + MISSION_SWITCH_MISSION +} mission_switch_pos_t; + //enum VEHICLE_FLIGHT_MODE { // VEHICLE_FLIGHT_MODE_MANUAL = 0, /**< direct manual control, exact mode determined by VEHICLE_MANUAL_CONTROL_MODE */ // VEHICLE_FLIGHT_MODE_STAB, /**< attitude or rate stabilization plus velocity or position stabilization */ @@ -168,9 +181,18 @@ struct vehicle_status_s /* system flags - these represent the state predicates */ + mode_switch_pos_t mode_switch; + return_switch_pos_t return_switch; + mission_switch_pos_t mission_switch; + bool flag_system_armed; /**< true is motors / actuators are armed */ bool flag_system_emergency; - bool flag_system_sensors_ok; + bool flag_system_in_air_restore; /**< true if we can restore in mid air */ + bool flag_system_sensors_initialized; + bool flag_system_arming_requested; + bool flag_system_disarming_requested; + bool flag_system_reboot_requested; + bool flag_system_on_ground; bool flag_control_manual_enabled; /**< true if manual input is mixed in */ bool flag_control_offboard_enabled; /**< true if offboard control input is on */ From 5eac78d7645becc486bc6a43852b9631e62465b4 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Mon, 18 Feb 2013 13:52:18 -0800 Subject: [PATCH 004/109] Checkpoint: Added DESCENT state --- apps/commander/state_machine_helper.c | 51 +++++++++++++++++---------- apps/uORB/topics/vehicle_status.h | 3 +- 2 files changed, 34 insertions(+), 20 deletions(-) diff --git a/apps/commander/state_machine_helper.c b/apps/commander/state_machine_helper.c index a0b786aabb..68b4bbe30d 100644 --- a/apps/commander/state_machine_helper.c +++ b/apps/commander/state_machine_helper.c @@ -119,6 +119,16 @@ int do_navigation_state_update(int status_pub, struct vehicle_status_s *current_ valid_path = true; } break; + case NAVIGATION_STATE_DESCENT: + + if (current_status->navigation_state == NAVIGATION_STATE_MANUAL + || current_status->navigation_state == NAVIGATION_STATE_SEATBELT) { + + mavlink_log_critical(mavlink_fd, "Switched to DESCENT state"); + valid_transition = true; + valid_path = true; + } + break; case NAVIGATION_STATE_LOITER: @@ -146,25 +156,28 @@ int do_navigation_state_update(int status_pub, struct vehicle_status_s *current_ /* coming from STANDBY pos and home lock are needed */ if (current_status->navigation_state == NAVIGATION_STATE_STANDBY) { - if (current_status->flag_global_position_valid && current_status->flag_valid_launch_position) { - mavlink_log_critical(mavlink_fd, "Switched to AUTO READY state"); - valid_transition = true; + if (!current_status->flag_system_armed) { + mavlink_log_critical(mavlink_fd, "Rejected AUTO READY state: disarmed"); + } else if (!current_status->flag_global_position_valid) { mavlink_log_critical(mavlink_fd, "Rejected AUTO READY state: no pos lock"); + } else if (!current_status->flag_valid_launch_position) { mavlink_log_critical(mavlink_fd, "Rejected AUTO READY state: no home lock"); + } else { - mavlink_log_critical(mavlink_fd, "Rejected AUTO READY state: no pos and home lock"); + mavlink_log_critical(mavlink_fd, "Switched to AUTO READY state"); + valid_transition = true; } valid_path = true; /* coming from LAND home lock is needed */ } else if (current_status->navigation_state == NAVIGATION_STATE_LAND) { - if (current_status->flag_valid_launch_position) { + if (!current_status->flag_valid_launch_position) { + mavlink_log_critical(mavlink_fd, "Rejected AUTO READY state: no home lock"); + } else { mavlink_log_critical(mavlink_fd, "Switched to AUTO READY state"); valid_transition = true; - } else { - mavlink_log_critical(mavlink_fd, "Rejected AUTO READY state: no home lock"); } valid_path = true; } @@ -183,15 +196,15 @@ int do_navigation_state_update(int status_pub, struct vehicle_status_s *current_ } else if (current_status->navigation_state == NAVIGATION_STATE_MANUAL || current_status->navigation_state == NAVIGATION_STATE_SEATBELT) { - if (current_status->flag_global_position_valid && current_status->flag_valid_launch_position) { - mavlink_log_critical(mavlink_fd, "Switched to MISSION state"); - valid_transition = true; - } else if (!current_status->flag_global_position_valid) { + if (!current_status->flag_global_position_valid) { mavlink_log_critical(mavlink_fd, "Rejected MISSION state: no pos lock"); + } else if (!current_status->flag_valid_launch_position) { mavlink_log_critical(mavlink_fd, "Rejected MISSION state: no home lock"); + mavlink_log_critical(mavlink_fd, "Switched to MISSION state"); + valid_transition = true; } else { - mavlink_log_critical(mavlink_fd, "Rejected MISSION state: no pos and home lock"); + } valid_path = true; @@ -219,15 +232,13 @@ int do_navigation_state_update(int status_pub, struct vehicle_status_s *current_ } else if (current_status->navigation_state == NAVIGATION_STATE_MANUAL || current_status->navigation_state == NAVIGATION_STATE_SEATBELT) { - if (current_status->flag_global_position_valid && current_status->flag_valid_launch_position) { - mavlink_log_critical(mavlink_fd, "Switched to RTL state"); - valid_transition = true; - } else if (!current_status->flag_global_position_valid) { + if (!current_status->flag_global_position_valid) { mavlink_log_critical(mavlink_fd, "Rejected RTL state: no pos lock"); } else if (!current_status->flag_valid_launch_position) { mavlink_log_critical(mavlink_fd, "Rejected RTL state: no home lock"); } else { - mavlink_log_critical(mavlink_fd, "Rejected RTL state: no pos and home lock"); + mavlink_log_critical(mavlink_fd, "Switched to RTL state"); + valid_transition = true; } valid_path = true; @@ -256,7 +267,8 @@ int do_navigation_state_update(int status_pub, struct vehicle_status_s *current_ case NAVIGATION_STATE_LAND: if (current_status->navigation_state == NAVIGATION_STATE_RTL - || current_status->navigation_state == NAVIGATION_STATE_LOITER) { + || current_status->navigation_state == NAVIGATION_STATE_LOITER + || current_status->navigation_state == NAVIGATION_STATE_MISSION) { mavlink_log_critical(mavlink_fd, "Switched to LAND state"); valid_transition = true; @@ -315,6 +327,7 @@ int do_arming_state_update(int status_pub, struct vehicle_status_s *current_stat if (current_status->arming_state == ARMING_STATE_INIT) { if (current_status->flag_system_sensors_initialized) { + current_status->flag_system_armed = false; mavlink_log_critical(mavlink_fd, "Switched to STANDBY arming state"); valid_transition = true; } else { @@ -327,7 +340,7 @@ int do_arming_state_update(int status_pub, struct vehicle_status_s *current_stat if (current_status->arming_state == ARMING_STATE_STANDBY || current_status->arming_state == ARMING_STATE_IN_AIR_RESTORE) { - + current_status->flag_system_armed = true; mavlink_log_critical(mavlink_fd, "Switched to ARMED arming state"); valid_transition = true; } diff --git a/apps/uORB/topics/vehicle_status.h b/apps/uORB/topics/vehicle_status.h index 874cf256c3..dde978caee 100644 --- a/apps/uORB/topics/vehicle_status.h +++ b/apps/uORB/topics/vehicle_status.h @@ -63,12 +63,13 @@ typedef enum { NAVIGATION_STATE_STANDBY=0, NAVIGATION_STATE_MANUAL, NAVIGATION_STATE_SEATBELT, + NAVIGATION_STATE_DESCENT, NAVIGATION_STATE_LOITER, NAVIGATION_STATE_AUTO_READY, NAVIGATION_STATE_MISSION, NAVIGATION_STATE_RTL, - NAVIGATION_STATE_TAKEOFF, NAVIGATION_STATE_LAND, + NAVIGATION_STATE_TAKEOFF } navigation_state_t; typedef enum { From b7faaca435551064e6fdb070a9e762b4146ae4e8 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Mon, 18 Feb 2013 16:35:34 -0800 Subject: [PATCH 005/109] Checkpoint: Arming/Disarming works --- apps/commander/commander.c | 18 ++++++++++++------ apps/commander/state_machine_helper.c | 9 +++++++++ apps/mavlink_onboard/mavlink.c | 2 +- 3 files changed, 22 insertions(+), 7 deletions(-) diff --git a/apps/commander/commander.c b/apps/commander/commander.c index 27c5f19893..ac535dd9ae 100644 --- a/apps/commander/commander.c +++ b/apps/commander/commander.c @@ -1649,6 +1649,13 @@ int commander_thread_main(int argc, char *argv[]) /* End battery voltage check */ + /* If in INIT state, try to proceed to STANDBY state */ + if (current_status.arming_state == ARMING_STATE_INIT) { + do_arming_state_update(stat_pub, ¤t_status, mavlink_fd, ARMING_STATE_STANDBY); + } else { + // XXX: Add emergency stuff if sensors are lost + } + /* * Check for valid position information. @@ -1894,12 +1901,11 @@ int commander_thread_main(int argc, char *argv[]) * Check if left stick is in lower left position --> switch to standby state. * Do this only for multirotors, not for fixed wing aircraft. */ - if (((current_status.system_type == VEHICLE_TYPE_QUADROTOR) || - (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)) { +// if (((current_status.system_type == VEHICLE_TYPE_QUADROTOR) || +// (current_status.system_type == VEHICLE_TYPE_HEXAROTOR) || +// (current_status.system_type == VEHICLE_TYPE_OCTOROTOR) +// ) && + if ((sp_man.yaw < -STICK_ON_OFF_LIMIT) && (sp_man.throttle < STICK_THRUST_RANGE * 0.2f)) { if (stick_off_counter > STICK_ON_OFF_COUNTER_LIMIT) { do_arming_state_update(stat_pub, ¤t_status, mavlink_fd, ARMING_STATE_STANDBY); stick_off_counter = 0; diff --git a/apps/commander/state_machine_helper.c b/apps/commander/state_machine_helper.c index 68b4bbe30d..f1de99e4d6 100644 --- a/apps/commander/state_machine_helper.c +++ b/apps/commander/state_machine_helper.c @@ -304,8 +304,11 @@ int do_arming_state_update(int status_pub, struct vehicle_status_s *current_stat bool valid_transition = false; int ret = ERROR; + warnx("Current state: %d, requested state: %d", current_status->arming_state, new_state); + if (current_status->arming_state == new_state) { warnx("Arming state not changed"); + valid_transition = true; } else { @@ -333,6 +336,12 @@ int do_arming_state_update(int status_pub, struct vehicle_status_s *current_stat } else { mavlink_log_critical(mavlink_fd, "REJ. STANDBY arming state, sensors not init."); } + + } else if (current_status->arming_state == ARMING_STATE_ARMED) { + + current_status->flag_system_armed = false; + mavlink_log_critical(mavlink_fd, "Switched to STANDBY arming state"); + valid_transition = true; } break; diff --git a/apps/mavlink_onboard/mavlink.c b/apps/mavlink_onboard/mavlink.c index 6babe14ae2..c5a1e82a88 100644 --- a/apps/mavlink_onboard/mavlink.c +++ b/apps/mavlink_onboard/mavlink.c @@ -290,7 +290,7 @@ get_mavlink_mode_and_state(const struct vehicle_status_s *v_status, const struct } /* set arming state */ - if (armed->armed) { + if (v_status->flag_system_armed) { *mavlink_mode |= MAV_MODE_FLAG_SAFETY_ARMED; } else { *mavlink_mode &= ~MAV_MODE_FLAG_SAFETY_ARMED; From aab6214cdcc630dce1f64ba9220bc1f5b10b6af1 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Tue, 19 Feb 2013 12:32:47 -0800 Subject: [PATCH 006/109] Checkpoint: Added HIL state, arming/disarming works now, also from GQC --- apps/commander/commander.c | 560 +++++++++++++------------- apps/commander/state_machine_helper.c | 199 +++++---- apps/uORB/topics/vehicle_status.h | 6 + 3 files changed, 413 insertions(+), 352 deletions(-) diff --git a/apps/commander/commander.c b/apps/commander/commander.c index ac535dd9ae..6b026f287e 100644 --- a/apps/commander/commander.c +++ b/apps/commander/commander.c @@ -797,302 +797,297 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta /* announce command handling */ tune_confirm(); - - /* supported command handling start */ - /* request to set different system mode */ switch (cmd->command) { - case VEHICLE_CMD_DO_SET_MODE: { - // XXX implement this + case VEHICLE_CMD_DO_SET_MODE: -// if (OK == update_state_machine_mode_request(status_pub, current_vehicle_status, mavlink_fd, (uint8_t)cmd->param1)) { -// result = VEHICLE_CMD_RESULT_ACCEPTED; -// -// } else { - result = VEHICLE_CMD_RESULT_DENIED; -// } - } - break; + /* request to activate HIL */ + if ((int)cmd->param1 & VEHICLE_MODE_FLAG_HIL_ENABLED) { - case VEHICLE_CMD_COMPONENT_ARM_DISARM: { - /* request to arm */ - if ((int)cmd->param1 == 1) { - // XXX add this again -// if (OK == do_arming_state_update(status_pub, current_vehicle_status, mavlink_fd, ARMING_STATE_ARMED)) { -// result = VEHICLE_CMD_RESULT_ACCEPTED; -// -// } else { -// result = VEHICLE_CMD_RESULT_DENIED; -// } - - /* request to disarm */ - // XXX this arms it instad of disarming - } else if ((int)cmd->param1 == 0) { - // XXX add this again -// if (OK == do_arming_state_update(status_pub, current_vehicle_status, mavlink_fd, ARMING_STATE_STANDBY)) { -// result = VEHICLE_CMD_RESULT_ACCEPTED; -// -// } else { -// result = VEHICLE_CMD_RESULT_DENIED; -// } - } - } - break; - - /* request for an autopilot reboot */ - case VEHICLE_CMD_PREFLIGHT_REBOOT_SHUTDOWN: { - if ((int)cmd->param1 == 1) { - if (OK == do_arming_state_update(status_pub, current_vehicle_status, mavlink_fd, ARMING_STATE_REBOOT)) { - /* SPECIAL CASE: SYSTEM WILL NEVER RETURN HERE */ + if (OK == do_hil_state_update(status_pub, current_vehicle_status, mavlink_fd, HIL_STATE_ON)) { result = VEHICLE_CMD_RESULT_ACCEPTED; - } else { - /* system may return here */ result = VEHICLE_CMD_RESULT_DENIED; } } - } - break; - -// /* request to land */ -// case VEHICLE_CMD_NAV_LAND: -// { -// //TODO: add check if landing possible -// //TODO: add landing maneuver -// -// if (0 == update_state_machine_custom_mode_request(status_pub, current_vehicle_status, SYSTEM_STATE_ARMED)) { -// result = VEHICLE_CMD_RESULT_ACCEPTED; -// } } -// break; -// -// /* request to takeoff */ -// case VEHICLE_CMD_NAV_TAKEOFF: -// { -// //TODO: add check if takeoff possible -// //TODO: add takeoff maneuver -// -// if (0 == update_state_machine_custom_mode_request(status_pub, current_vehicle_status, SYSTEM_STATE_AUTO)) { -// result = VEHICLE_CMD_RESULT_ACCEPTED; -// } -// } -// break; -// - /* preflight calibration */ - case VEHICLE_CMD_PREFLIGHT_CALIBRATION: { - bool handled = false; - - /* gyro calibration */ - if ((int)(cmd->param1) == 1) { - - /* transition to init state */ - // XXX add this again -// if (OK == do_arming_state_update(status_pub, ¤t_status, mavlink_fd, ARMING_STATE_INIT) -// && OK == do_navigation_state_update(status_pub, ¤t_status, mavlink_fd, NAVIGATION_STATE_INIT)) { -// -// mavlink_log_info(mavlink_fd, "starting gyro cal"); -// tune_confirm(); -// do_gyro_calibration(status_pub, ¤t_status); -// mavlink_log_info(mavlink_fd, "finished gyro cal"); -// tune_confirm(); -// -// /* back to standby state */ -// do_arming_state_update(status_pub, ¤t_status, mavlink_fd, ARMING_STATE_STANDBY); -// do_navigation_state_update(status_pub, ¤t_status, mavlink_fd, NAVIGATION_STATE_STANDBY); -// -// result = VEHICLE_CMD_RESULT_ACCEPTED; -// -// } else { -// mavlink_log_critical(mavlink_fd, "REJECTING gyro cal"); -// result = VEHICLE_CMD_RESULT_DENIED; -// } - - handled = true; - } - - /* magnetometer calibration */ - if ((int)(cmd->param2) == 1) { - - /* transition to init state */ - // XXX add this again -// if (OK == do_arming_state_update(status_pub, ¤t_status, mavlink_fd, ARMING_STATE_INIT) -// && OK == do_navigation_state_update(status_pub, ¤t_status, mavlink_fd, NAVIGATION_STATE_INIT)) { -// mavlink_log_info(mavlink_fd, "starting mag cal"); -// tune_confirm(); -// do_mag_calibration(status_pub, ¤t_status); -// mavlink_log_info(mavlink_fd, "finished mag cal"); -// tune_confirm(); -// -// /* back to standby state */ -// do_arming_state_update(status_pub, ¤t_status, mavlink_fd, ARMING_STATE_STANDBY); -// do_navigation_state_update(status_pub, ¤t_status, mavlink_fd, NAVIGATION_STATE_STANDBY); -// -// result = VEHICLE_CMD_RESULT_ACCEPTED; -// -// } else { -// mavlink_log_critical(mavlink_fd, "REJECTING mag cal"); -// result = VEHICLE_CMD_RESULT_DENIED; -// } - - handled = true; - } - - /* zero-altitude pressure calibration */ - if ((int)(cmd->param3) == 1) { - /* transition to calibration state */ - // XXX add this again -// if (OK == do_arming_state_update(status_pub, ¤t_status, mavlink_fd, ARMING_STATE_INIT) -// && OK == do_navigation_state_update(status_pub, ¤t_status, mavlink_fd, NAVIGATION_STATE_INIT)) { -// -// // XXX implement this -// mavlink_log_info(mavlink_fd, "zero altitude cal. not implemented"); -// tune_confirm(); -// -// /* back to standby state */ -// do_arming_state_update(status_pub, ¤t_status, mavlink_fd, ARMING_STATE_STANDBY); -// do_navigation_state_update(status_pub, ¤t_status, mavlink_fd, NAVIGATION_STATE_STANDBY); -// -// } else { -// mavlink_log_critical(mavlink_fd, "REJECTING altitude calibration"); -// result = VEHICLE_CMD_RESULT_DENIED; -// } - - handled = true; - } - - /* trim calibration */ - if ((int)(cmd->param4) == 1) { - /* transition to calibration state */ - // XXX add this again -// if (OK == do_arming_state_update(status_pub, ¤t_status, mavlink_fd, ARMING_STATE_INIT) -// && OK == do_navigation_state_update(status_pub, ¤t_status, mavlink_fd, NAVIGATION_STATE_INIT)) { -// mavlink_log_info(mavlink_fd, "starting trim cal"); -// tune_confirm(); -// do_rc_calibration(status_pub, ¤t_status); -// mavlink_log_info(mavlink_fd, "finished trim cal"); -// tune_confirm(); -// -// /* back to standby state */ -// do_arming_state_update(status_pub, ¤t_status, mavlink_fd, ARMING_STATE_STANDBY); -// do_navigation_state_update(status_pub, ¤t_status, mavlink_fd, NAVIGATION_STATE_STANDBY); -// -// result = VEHICLE_CMD_RESULT_ACCEPTED; -// -// } else { -// mavlink_log_critical(mavlink_fd, "REJECTING trim cal"); -// result = VEHICLE_CMD_RESULT_DENIED; -// } - - handled = true; - } - - /* accel calibration */ - if ((int)(cmd->param5) == 1) { - // XXX add this again -// if (OK == do_arming_state_update(status_pub, ¤t_status, mavlink_fd, ARMING_STATE_INIT) -// && OK == do_navigation_state_update(status_pub, ¤t_status, mavlink_fd, NAVIGATION_STATE_INIT)) { -// -// mavlink_log_info(mavlink_fd, "CMD starting accel cal"); -// tune_confirm(); -// do_accel_calibration(status_pub, ¤t_status); -// tune_confirm(); -// mavlink_log_info(mavlink_fd, "CMD finished accel cal"); -// -// /* back to standby state */ -// do_arming_state_update(status_pub, ¤t_status, mavlink_fd, ARMING_STATE_STANDBY); -// do_navigation_state_update(status_pub, ¤t_status, mavlink_fd, NAVIGATION_STATE_STANDBY); -// -// result = VEHICLE_CMD_RESULT_ACCEPTED; -// -// } else { -// mavlink_log_critical(mavlink_fd, "REJECTING accel cal"); -// result = VEHICLE_CMD_RESULT_DENIED; -// } - - handled = true; - } - - /* none found */ - if (!handled) { - //warnx("refusing unsupported calibration request\n"); - mavlink_log_critical(mavlink_fd, "CMD refusing unsup. calib. request"); - result = VEHICLE_CMD_RESULT_UNSUPPORTED; - } - } - break; - - case VEHICLE_CMD_PREFLIGHT_STORAGE: { - if (current_status.flag_system_armed && - ((current_status.system_type == VEHICLE_TYPE_QUADROTOR) || - (current_status.system_type == VEHICLE_TYPE_HEXAROTOR) || - (current_status.system_type == VEHICLE_TYPE_OCTOROTOR))) { - /* do not perform expensive memory tasks on multirotors in flight */ - // XXX this is over-safe, as soon as cmd is in low prio thread this can be allowed - mavlink_log_info(mavlink_fd, "REJECTING save cmd while multicopter armed"); - - } else { - - // XXX move this to LOW PRIO THREAD of commander app - /* Read all parameters from EEPROM to RAM */ - - if (((int)(cmd->param1)) == 0) { - - /* read all parameters from EEPROM to RAM */ - int read_ret = param_load_default(); - - if (read_ret == OK) { - //warnx("[mavlink pm] Loaded EEPROM params in RAM\n"); - mavlink_log_info(mavlink_fd, "OK loading params from"); - mavlink_log_info(mavlink_fd, param_get_default_file()); - result = VEHICLE_CMD_RESULT_ACCEPTED; - - } else if (read_ret == 1) { - mavlink_log_info(mavlink_fd, "OK no changes in"); - mavlink_log_info(mavlink_fd, param_get_default_file()); - result = VEHICLE_CMD_RESULT_ACCEPTED; - - } else { - if (read_ret < -1) { - mavlink_log_info(mavlink_fd, "ERR loading params from"); - mavlink_log_info(mavlink_fd, param_get_default_file()); - - } else { - mavlink_log_info(mavlink_fd, "ERR no param file named"); - mavlink_log_info(mavlink_fd, param_get_default_file()); - } - - result = VEHICLE_CMD_RESULT_FAILED; - } - - } else if (((int)(cmd->param1)) == 1) { - - /* write all parameters from RAM to EEPROM */ - int write_ret = param_save_default(); - - if (write_ret == OK) { - mavlink_log_info(mavlink_fd, "OK saved param file"); - mavlink_log_info(mavlink_fd, param_get_default_file()); - result = VEHICLE_CMD_RESULT_ACCEPTED; - - } else { - if (write_ret < -1) { - mavlink_log_info(mavlink_fd, "ERR params file does not exit:"); - mavlink_log_info(mavlink_fd, param_get_default_file()); - - } else { - mavlink_log_info(mavlink_fd, "ERR writing params to"); - mavlink_log_info(mavlink_fd, param_get_default_file()); - } - - result = VEHICLE_CMD_RESULT_FAILED; - } + if ((int)cmd->param1 & VEHICLE_MODE_FLAG_SAFETY_ARMED) { + if (OK == do_arming_state_update(status_pub, current_vehicle_status, mavlink_fd, ARMING_STATE_ARMED)) { + result = VEHICLE_CMD_RESULT_ACCEPTED; } else { - mavlink_log_info(mavlink_fd, "[pm] refusing unsupp. STOR request"); + result = VEHICLE_CMD_RESULT_DENIED; + } + } else { + if (OK == do_arming_state_update(status_pub, current_vehicle_status, mavlink_fd, ARMING_STATE_STANDBY)) { + result = VEHICLE_CMD_RESULT_ACCEPTED; + } else { + result = VEHICLE_CMD_RESULT_DENIED; + } + } + + break; + + case VEHICLE_CMD_COMPONENT_ARM_DISARM: + + /* request to arm */ + if ((int)cmd->param1 == 1) { + if (OK == do_arming_state_update(status_pub, current_vehicle_status, mavlink_fd, ARMING_STATE_ARMED)) { + result = VEHICLE_CMD_RESULT_ACCEPTED; + } else { + result = VEHICLE_CMD_RESULT_DENIED; + } + /* request to disarm */ + } else if ((int)cmd->param1 == 0) { + if (OK == do_arming_state_update(status_pub, current_vehicle_status, mavlink_fd, ARMING_STATE_STANDBY)) { + result = VEHICLE_CMD_RESULT_ACCEPTED; + } else { + result = VEHICLE_CMD_RESULT_DENIED; + } + } + + break; + + case VEHICLE_CMD_PREFLIGHT_REBOOT_SHUTDOWN: { + + /* request for an autopilot reboot */ + if ((int)cmd->param1 == 1) { + if (OK == do_arming_state_update(status_pub, current_vehicle_status, mavlink_fd, ARMING_STATE_REBOOT)) { + /* SPECIAL CASE: SYSTEM WILL NEVER RETURN HERE */ + result = VEHICLE_CMD_RESULT_ACCEPTED; + + } else { + /* system may return here */ + result = VEHICLE_CMD_RESULT_DENIED; + } + } + } + break; + + // /* request to land */ + // case VEHICLE_CMD_NAV_LAND: + // { + // //TODO: add check if landing possible + // //TODO: add landing maneuver + // + // if (0 == update_state_machine_custom_mode_request(status_pub, current_vehicle_status, SYSTEM_STATE_ARMED)) { + // result = VEHICLE_CMD_RESULT_ACCEPTED; + // } } + // break; + // + // /* request to takeoff */ + // case VEHICLE_CMD_NAV_TAKEOFF: + // { + // //TODO: add check if takeoff possible + // //TODO: add takeoff maneuver + // + // if (0 == update_state_machine_custom_mode_request(status_pub, current_vehicle_status, SYSTEM_STATE_AUTO)) { + // result = VEHICLE_CMD_RESULT_ACCEPTED; + // } + // } + // break; + // + /* preflight calibration */ + case VEHICLE_CMD_PREFLIGHT_CALIBRATION: { + bool handled = false; + + /* gyro calibration */ + if ((int)(cmd->param1) == 1) { + + if (OK == do_arming_state_update(status_pub, current_vehicle_status, mavlink_fd, ARMING_STATE_INIT)) { + + result = VEHICLE_CMD_RESULT_ACCEPTED; + + mavlink_log_info(mavlink_fd, "starting gyro cal"); + tune_confirm(); + do_gyro_calibration(status_pub, ¤t_status); + mavlink_log_info(mavlink_fd, "finished gyro cal"); + tune_confirm(); + + do_arming_state_update(status_pub, ¤t_status, mavlink_fd, ARMING_STATE_STANDBY); + + } else { + result = VEHICLE_CMD_RESULT_DENIED; + } + + handled = true; + } + + /* magnetometer calibration */ + if ((int)(cmd->param2) == 1) { + + if (OK == do_arming_state_update(status_pub, current_vehicle_status, mavlink_fd, ARMING_STATE_INIT)) { + + result = VEHICLE_CMD_RESULT_ACCEPTED; + + mavlink_log_info(mavlink_fd, "starting mag cal"); + tune_confirm(); + do_mag_calibration(status_pub, ¤t_status); + mavlink_log_info(mavlink_fd, "finished mag cal"); + tune_confirm(); + + do_arming_state_update(status_pub, ¤t_status, mavlink_fd, ARMING_STATE_STANDBY); + + } else { + result = VEHICLE_CMD_RESULT_DENIED; + } + + handled = true; + } + + /* zero-altitude pressure calibration */ + if ((int)(cmd->param3) == 1) { + /* transition to calibration state */ + // XXX add this again + // if (OK == do_arming_state_update(status_pub, ¤t_status, mavlink_fd, ARMING_STATE_INIT) + // && OK == do_navigation_state_update(status_pub, ¤t_status, mavlink_fd, NAVIGATION_STATE_INIT)) { + // + // // XXX implement this + // mavlink_log_info(mavlink_fd, "zero altitude cal. not implemented"); + // tune_confirm(); + // + // /* back to standby state */ + // do_arming_state_update(status_pub, ¤t_status, mavlink_fd, ARMING_STATE_STANDBY); + // do_navigation_state_update(status_pub, ¤t_status, mavlink_fd, NAVIGATION_STATE_STANDBY); + // + // } else { + // mavlink_log_critical(mavlink_fd, "REJECTING altitude calibration"); + // result = VEHICLE_CMD_RESULT_DENIED; + // } + + handled = true; + } + + /* trim calibration */ + if ((int)(cmd->param4) == 1) { + /* transition to calibration state */ + // XXX add this again + // if (OK == do_arming_state_update(status_pub, ¤t_status, mavlink_fd, ARMING_STATE_INIT) + // && OK == do_navigation_state_update(status_pub, ¤t_status, mavlink_fd, NAVIGATION_STATE_INIT)) { + // mavlink_log_info(mavlink_fd, "starting trim cal"); + // tune_confirm(); + // do_rc_calibration(status_pub, ¤t_status); + // mavlink_log_info(mavlink_fd, "finished trim cal"); + // tune_confirm(); + // + // /* back to standby state */ + // do_arming_state_update(status_pub, ¤t_status, mavlink_fd, ARMING_STATE_STANDBY); + // do_navigation_state_update(status_pub, ¤t_status, mavlink_fd, NAVIGATION_STATE_STANDBY); + // + // result = VEHICLE_CMD_RESULT_ACCEPTED; + // + // } else { + // mavlink_log_critical(mavlink_fd, "REJECTING trim cal"); + // result = VEHICLE_CMD_RESULT_DENIED; + // } + + handled = true; + } + + /* accel calibration */ + if ((int)(cmd->param5) == 1) { + + if (OK == do_arming_state_update(status_pub, current_vehicle_status, mavlink_fd, ARMING_STATE_INIT)) { + + result = VEHICLE_CMD_RESULT_ACCEPTED; + + mavlink_log_info(mavlink_fd, "starting acc cal"); + tune_confirm(); + do_accel_calibration(status_pub, ¤t_status); + mavlink_log_info(mavlink_fd, "finished acc cal"); + tune_confirm(); + + do_arming_state_update(status_pub, ¤t_status, mavlink_fd, ARMING_STATE_STANDBY); + + } else { + result = VEHICLE_CMD_RESULT_DENIED; + } + + + handled = true; + } + + /* none found */ + if (!handled) { + //warnx("refusing unsupported calibration request\n"); + mavlink_log_critical(mavlink_fd, "CMD refusing unsup. calib. request"); result = VEHICLE_CMD_RESULT_UNSUPPORTED; } } - } - break; + break; + + case VEHICLE_CMD_PREFLIGHT_STORAGE: { + if (current_status.flag_system_armed && + ((current_status.system_type == VEHICLE_TYPE_QUADROTOR) || + (current_status.system_type == VEHICLE_TYPE_HEXAROTOR) || + (current_status.system_type == VEHICLE_TYPE_OCTOROTOR))) { + /* do not perform expensive memory tasks on multirotors in flight */ + // XXX this is over-safe, as soon as cmd is in low prio thread this can be allowed + mavlink_log_info(mavlink_fd, "REJECTING save cmd while multicopter armed"); + + } else { + + // XXX move this to LOW PRIO THREAD of commander app + /* Read all parameters from EEPROM to RAM */ + + if (((int)(cmd->param1)) == 0) { + + /* read all parameters from EEPROM to RAM */ + int read_ret = param_load_default(); + + if (read_ret == OK) { + //warnx("[mavlink pm] Loaded EEPROM params in RAM\n"); + mavlink_log_info(mavlink_fd, "OK loading params from"); + mavlink_log_info(mavlink_fd, param_get_default_file()); + result = VEHICLE_CMD_RESULT_ACCEPTED; + + } else if (read_ret == 1) { + mavlink_log_info(mavlink_fd, "OK no changes in"); + mavlink_log_info(mavlink_fd, param_get_default_file()); + result = VEHICLE_CMD_RESULT_ACCEPTED; + + } else { + if (read_ret < -1) { + mavlink_log_info(mavlink_fd, "ERR loading params from"); + mavlink_log_info(mavlink_fd, param_get_default_file()); + + } else { + mavlink_log_info(mavlink_fd, "ERR no param file named"); + mavlink_log_info(mavlink_fd, param_get_default_file()); + } + + result = VEHICLE_CMD_RESULT_FAILED; + } + + } else if (((int)(cmd->param1)) == 1) { + + /* write all parameters from RAM to EEPROM */ + int write_ret = param_save_default(); + + if (write_ret == OK) { + mavlink_log_info(mavlink_fd, "OK saved param file"); + mavlink_log_info(mavlink_fd, param_get_default_file()); + result = VEHICLE_CMD_RESULT_ACCEPTED; + + } else { + if (write_ret < -1) { + mavlink_log_info(mavlink_fd, "ERR params file does not exit:"); + mavlink_log_info(mavlink_fd, param_get_default_file()); + + } else { + mavlink_log_info(mavlink_fd, "ERR writing params to"); + mavlink_log_info(mavlink_fd, param_get_default_file()); + } + + result = VEHICLE_CMD_RESULT_FAILED; + } + + } else { + mavlink_log_info(mavlink_fd, "[pm] refusing unsupp. STOR request"); + result = VEHICLE_CMD_RESULT_UNSUPPORTED; + } + } + } + break; default: { mavlink_log_critical(mavlink_fd, "[cmd] refusing unsupported command"); @@ -1326,6 +1321,7 @@ int commander_thread_main(int argc, char *argv[]) memset(¤t_status, 0, sizeof(current_status)); current_status.navigation_state = NAVIGATION_STATE_STANDBY; current_status.arming_state = ARMING_STATE_INIT; + current_status.hil_state = HIL_STATE_OFF; current_status.flag_system_armed = false; /* neither manual nor offboard control commands have been received */ diff --git a/apps/commander/state_machine_helper.c b/apps/commander/state_machine_helper.c index f1de99e4d6..aae119d35a 100644 --- a/apps/commander/state_machine_helper.c +++ b/apps/commander/state_machine_helper.c @@ -413,6 +413,63 @@ int do_arming_state_update(int status_pub, struct vehicle_status_s *current_stat return ret; } +/** + * Transition from one hil state to another + */ +int do_hil_state_update(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, hil_state_t new_state) +{ + bool valid_transition = false; + int ret = ERROR; + + warnx("Current state: %d, requested state: %d", current_status->hil_state, new_state); + + if (current_status->hil_state == new_state) { + warnx("Hil state not changed"); + valid_transition = true; + + } else { + + switch (new_state) { + + case HIL_STATE_OFF: + + if (current_status->arming_state == ARMING_STATE_INIT + || current_status->arming_state == ARMING_STATE_STANDBY) { + + current_status->flag_hil_enabled = false; + mavlink_log_critical(mavlink_fd, "Switched to OFF hil state"); + valid_transition = true; + } + break; + + case HIL_STATE_ON: + + if (current_status->arming_state == ARMING_STATE_INIT + || current_status->arming_state == ARMING_STATE_STANDBY) { + + current_status->flag_hil_enabled = true; + mavlink_log_critical(mavlink_fd, "Switched to ON hil state"); + valid_transition = true; + } + break; + + default: + warnx("Unknown hil state"); + break; + } + } + + if (valid_transition) { + current_status->hil_state = new_state; + state_machine_publish(status_pub, current_status, mavlink_fd); + ret = OK; + } else { + mavlink_log_critical(mavlink_fd, "REJECTING invalid hil state transition"); + } + + return ret; +} + void state_machine_publish(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd) @@ -684,7 +741,78 @@ void state_machine_publish(int status_pub, struct vehicle_status_s *current_stat // } -/* END SUBSYSTEM/EMERGENCY FUNCTIONS*/ +///* END SUBSYSTEM/EMERGENCY FUNCTIONS*/ +// +//int update_state_machine_mode_request(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, uint8_t mode) +//{ +// int ret = 1; +// +//// /* Switch on HIL if in standby and not already in HIL mode */ +//// if ((mode & VEHICLE_MODE_FLAG_HIL_ENABLED) +//// && !current_status->flag_hil_enabled) { +//// if ((current_status->state_machine == SYSTEM_STATE_STANDBY)) { +//// /* Enable HIL on request */ +//// current_status->flag_hil_enabled = true; +//// ret = OK; +//// state_machine_publish(status_pub, current_status, mavlink_fd); +//// publish_armed_status(current_status); +//// printf("[cmd] Enabling HIL, locking down all actuators for safety.\n\t(Arming the system will not activate them while in HIL mode)\n"); +//// +//// } else if (current_status->state_machine != SYSTEM_STATE_STANDBY && +//// current_status->flag_system_armed) { +//// +//// mavlink_log_critical(mavlink_fd, "REJECTING HIL, disarm first!") +//// +//// } else { +//// +//// mavlink_log_critical(mavlink_fd, "REJECTING HIL, not in standby.") +//// } +//// } +// +// /* switch manual / auto */ +// if (mode & VEHICLE_MODE_FLAG_AUTO_ENABLED) { +// update_state_machine_mode_auto(status_pub, current_status, mavlink_fd); +// +// } else if (mode & VEHICLE_MODE_FLAG_STABILIZED_ENABLED) { +// update_state_machine_mode_stabilized(status_pub, current_status, mavlink_fd); +// +// } else if (mode & VEHICLE_MODE_FLAG_GUIDED_ENABLED) { +// update_state_machine_mode_guided(status_pub, current_status, mavlink_fd); +// +// } else if (mode & VEHICLE_MODE_FLAG_MANUAL_INPUT_ENABLED) { +// update_state_machine_mode_manual(status_pub, current_status, mavlink_fd); +// } +// +// /* vehicle is disarmed, mode requests arming */ +// if (!(current_status->flag_system_armed) && (mode & VEHICLE_MODE_FLAG_SAFETY_ARMED)) { +// /* only arm in standby state */ +// // XXX REMOVE +// if (current_status->state_machine == SYSTEM_STATE_STANDBY || current_status->state_machine == SYSTEM_STATE_PREFLIGHT) { +// do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_GROUND_READY); +// ret = OK; +// printf("[cmd] arming due to command request\n"); +// } +// } +// +// /* vehicle is armed, mode requests disarming */ +// if (current_status->flag_system_armed && !(mode & VEHICLE_MODE_FLAG_SAFETY_ARMED)) { +// /* only disarm in ground ready */ +// if (current_status->state_machine == SYSTEM_STATE_GROUND_READY) { +// do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_STANDBY); +// ret = OK; +// printf("[cmd] disarming due to command request\n"); +// } +// } +// +// /* NEVER actually switch off HIL without reboot */ +// if (current_status->flag_hil_enabled && !(mode & VEHICLE_MODE_FLAG_HIL_ENABLED)) { +// warnx("DENYING request to switch off HIL. Please power cycle (safety reasons)\n"); +// mavlink_log_critical(mavlink_fd, "Power-cycle to exit HIL"); +// ret = ERROR; +// } +// +// return ret; +//} #if 0 @@ -821,76 +949,7 @@ void update_state_machine_mode_auto(int status_pub, struct vehicle_status_s *cur } -uint8_t update_state_machine_mode_request(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, uint8_t mode) -{ - uint8_t ret = 1; - /* Switch on HIL if in standby and not already in HIL mode */ - if ((mode & VEHICLE_MODE_FLAG_HIL_ENABLED) - && !current_status->flag_hil_enabled) { - if ((current_status->state_machine == SYSTEM_STATE_STANDBY)) { - /* Enable HIL on request */ - current_status->flag_hil_enabled = true; - ret = OK; - state_machine_publish(status_pub, current_status, mavlink_fd); - publish_armed_status(current_status); - printf("[cmd] Enabling HIL, locking down all actuators for safety.\n\t(Arming the system will not activate them while in HIL mode)\n"); - - } else if (current_status->state_machine != SYSTEM_STATE_STANDBY && - current_status->flag_system_armed) { - - mavlink_log_critical(mavlink_fd, "REJECTING HIL, disarm first!") - - } else { - - mavlink_log_critical(mavlink_fd, "REJECTING HIL, not in standby.") - } - } - - /* switch manual / auto */ - if (mode & VEHICLE_MODE_FLAG_AUTO_ENABLED) { - update_state_machine_mode_auto(status_pub, current_status, mavlink_fd); - - } else if (mode & VEHICLE_MODE_FLAG_STABILIZED_ENABLED) { - update_state_machine_mode_stabilized(status_pub, current_status, mavlink_fd); - - } else if (mode & VEHICLE_MODE_FLAG_GUIDED_ENABLED) { - update_state_machine_mode_guided(status_pub, current_status, mavlink_fd); - - } else if (mode & VEHICLE_MODE_FLAG_MANUAL_INPUT_ENABLED) { - update_state_machine_mode_manual(status_pub, current_status, mavlink_fd); - } - - /* vehicle is disarmed, mode requests arming */ - if (!(current_status->flag_system_armed) && (mode & VEHICLE_MODE_FLAG_SAFETY_ARMED)) { - /* only arm in standby state */ - // XXX REMOVE - if (current_status->state_machine == SYSTEM_STATE_STANDBY || current_status->state_machine == SYSTEM_STATE_PREFLIGHT) { - do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_GROUND_READY); - ret = OK; - printf("[cmd] arming due to command request\n"); - } - } - - /* vehicle is armed, mode requests disarming */ - if (current_status->flag_system_armed && !(mode & VEHICLE_MODE_FLAG_SAFETY_ARMED)) { - /* only disarm in ground ready */ - if (current_status->state_machine == SYSTEM_STATE_GROUND_READY) { - do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_STANDBY); - ret = OK; - printf("[cmd] disarming due to command request\n"); - } - } - - /* NEVER actually switch off HIL without reboot */ - if (current_status->flag_hil_enabled && !(mode & VEHICLE_MODE_FLAG_HIL_ENABLED)) { - warnx("DENYING request to switch off HIL. Please power cycle (safety reasons)\n"); - mavlink_log_critical(mavlink_fd, "Power-cycle to exit HIL"); - ret = ERROR; - } - - return ret; -} uint8_t update_state_machine_custom_mode_request(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, uint8_t custom_mode) //TODO: add more checks to avoid state switching in critical situations { diff --git a/apps/uORB/topics/vehicle_status.h b/apps/uORB/topics/vehicle_status.h index f5ab91bad9..27a471f131 100644 --- a/apps/uORB/topics/vehicle_status.h +++ b/apps/uORB/topics/vehicle_status.h @@ -82,6 +82,11 @@ typedef enum { ARMING_STATE_IN_AIR_RESTORE } arming_state_t; +typedef enum { + HIL_STATE_OFF = 0, + HIL_STATE_ON +} hil_state_t; + enum VEHICLE_MODE_FLAG { VEHICLE_MODE_FLAG_SAFETY_ARMED = 128, VEHICLE_MODE_FLAG_MANUAL_INPUT_ENABLED = 64, @@ -177,6 +182,7 @@ struct vehicle_status_s navigation_state_t navigation_state; /**< current navigation state */ arming_state_t arming_state; /**< current arming state */ + hil_state_t hil_state; /**< current hil state */ int32_t system_type; /**< system type, inspired by MAVLink's VEHICLE_TYPE enum */ int32_t system_id; /**< system id, inspired by MAVLink's system ID field */ From 0e29f2505a599d473244b0bb7e4b309d392ebb3c Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Wed, 20 Feb 2013 10:38:06 -0800 Subject: [PATCH 007/109] Checkpoint: New hierarchical states --- Documentation/flight_mode_state_machine.odg | Bin 18105 -> 24059 bytes apps/commander/commander.c | 117 ++-- apps/commander/state_machine_helper.c | 678 +++++++++----------- apps/commander/state_machine_helper.h | 4 +- apps/uORB/topics/vehicle_status.h | 89 ++- 5 files changed, 392 insertions(+), 496 deletions(-) diff --git a/Documentation/flight_mode_state_machine.odg b/Documentation/flight_mode_state_machine.odg index b630ecb40242a9a52a796c30ff0dbc283a4de7e7..a50366bcb3e51c2c6571597de89d7b00d746b28d 100644 GIT binary patch delta 20506 zcmaHyQ*h=D@TK24nb@}NOl;f6#J2g1&53PiGO?YBZQIUF?5%J2X8%=N+gIIH=c?Y;1p#N)NK_HO5xtlSQm%Uw_dVhR2 z3rhDBePX8;3v_5ORL!E8hK8PfsYU5FnTfAj>#$^t&V280dr?`BVhsu;mb>k)?+2_8 zANAOC@C)JeYoL@85|f`a<41*HN*gnOp*LHJZ64ky;Re-{>Meo!VSz@2!;1M`o~1w) zxf<&21 zur#tH=#%s~k!X}~S`3?GC@McYQ*MTDja&DR6l3#Ul&##y;bgob6zQIn@bt87hfX4WbjFDNawU~EyM z@_04-Ip|{W6Kllu!bz^ovDsSZiH-j{-we_6Km6lBNXKA~-tsB48URcu%)WX@t4d->`IQI#mEC&Ny zh|_a54+elhwvZsu|H0z_#R38d1adWZbF+4^bp4NqEnNfGb1oGBo8O~eud{+E5HD6g zz&&B%5MAL4gEk)@6;|OZlA4KRkoI=8*qkPyvQco|d8fn#?ap$Fmux(n4__Db{wNxX zNJn?=X(&zr+iC5VYir}xS!N2gOSk2AM?qh z1T+m4+w;CjjkH+otYQWQdVs_5U%m%FQY1n8Y9)(E{n=XxtH~w zp%)au1DT2r{k_UZlQ#B~4{?bB{`0*-vl27B7lt?2eEfG_t`U)4j;i=R+7W4&NZS-L37giufCAlcW_lJ4rtvh9 zPI}LF{Ti!gQh|oqOh^2D={Ek}IkGIMQ83>*a6X#G4L4VMv8^7;u zvVGb#W)R}g<*G-ndgO8(>+(+HbtKpT)b0cjDb)5?rY0bjX=!j#*R zWE6*0zf@xi}S@vB1QKS53sn)hP03-P`P_P z;V+)gEeH)VvAV*KW10Dd9$b8gg+5cPz=VFo`n8Ymm7|S# z)O5t7ievj0#U-7kW=(HNs}VI9$XDB5q(XU=w2DvH@QBGNW+aBvD)6bV9f_Cv7!#&S zOFz)o8!jxyKjl^QZ6~sC+&<$1kd894&m?<6vihlO2ASI#jwnVrZPx4dXq$<xKv_27Z6J0JO{Q`($(yr;r*B0Z(vv0K>@w9`HY)-c2sP08O_kK5C zR)mUn)=8mC!lH+Edp|Ugh0VxzGjBKuG`;U8dpe8dHd!N)q9e_=VHr9B5Jc`{dGMb` zV>F4{VB2$wV^^hIbYHXj4Qy>+^g(YKEv`|t#!1l>+;X2G z7hxFJFjmM^yL4z``*5T&dOw|j$=A^hgwmKsUIbW(@2&Y5K3_*uNY6Y-?g8_-EO;Uq zjRNg-LgOdfEbqAq4IfQEx#5-n zmdnBD&+YDf@WN)o&I?f3>u;OmSca|`o;m-74`euhS+yD5))!w;uHsv!j~qp4-j zn576}htG#rVESrGN$9gv=-15+cH>KD=fmZhanJfuBuik((yrX~SbSIjJ@!27>Dky7B}5tEsQ*aPV7qB*pG z;pKjFy*SJN9<$|#o+D^0;Ll|+ZS!XZnRRxT-vfZRJiJq)`S&0eDr|?2eY@e(Cg|;( z{(jo~`S0#`W5Qj;+1}yMS@PYGO=os3(x@FV*2Hup=!7cNqeIsX3yS~gbZ)JHg#uNp zHT&hjr?Hy)wC9Zsv@>8ED|k(mW{nwM{H7*kcYR${K5!MOr)*gu|4%9A<)pWhv|6q6 zM+Xr8!$$)+M*~#bu$m0}qV@R8HK|FeBEoF%3JvvcR+OqcU5I2sCq_gk*-%Id>7Nar zhv=ix4H?W&Hf5imt0R56$H;n#yj~s42{*yjXW-4=&+}IYw^6z~Ti=E13k@tbeyt8s z-LwMjL-hnJmfAs8j^8fP0`3mBVQEvRAAyzt)7K`w;=^f|-e^1EU9COqIbX*+_WQ@+ z)E0SX<#t>CfZ84AHr>k6yYs7YfNOTo{owKGJaYEg!N`!^*W-a(NLdn)l#ac}?*Dyj zbLwSxkHj6KfYGU=K6F%EE`CwSFVh+7VDE0o~!By zo6oJ8U)+Gh%FUO(;D*29rC@Eo2kg-u)yrn)``&S{9cW(8lAF_cFdNT1H^D%4WBU!* z`9QBUv{)BFIJzA8DYO;P1-CQ)H<#Gq)4M+FYdU7<3(j!r=JwRn0r=tow%(_=Zocp6 zyY<8k2WZ!vd&bzL{q6qn=b{QAF)4{CUMR7V5QU>!N!e#*7P^!Dcu zPaeeQBsiWdT$yypBJN9mx_p-6T_f$!Re1}L{Nq9?4uL^=(IFxF)wTj*G6!wqb_+6b z#)_1xewYZx;kVwp`D;b$pX$r6gp;sbuGK!$c>LO0ObH?MVIl4EWrO$JT5N9Tq}`Q8 z_Hkv!iAR9deIb?)=ymDa(Z?k1{$=lkQ_`Lmno3VKe)+U+&$(%ELD`9z>7_MF6XT&u z{&c$g`n>t7{0lEgAv|Xk;_Etb=8IElFJ&NU(;@OjzxuN`z~8Ia@EXi(-9*~n9VR|T zsIkG^GrOEzx%3s8D7~fk9)aK)irbF3oc($B<->=4C&bAgfQ{NlF+mSVg&iY8ZP1EJ zJVzuIOwx){HmuC^5Fm5Q^}+L0^bMBoW|SaA&}wbaFn0To zjzN6;NWXswYSVtD{t(#KY~?oc|MObJ=ajjOe7AL6Ekq1&{&|5XSUegX%Wj}9RNeVn z=a+Soa4}@lDuFy}%^!!^vYKz$UH1^Sguhgu{N^bNfLOE<^Qqf!i7i75cRJo)VP`j) zSMvRF4dN*UmXtrV_+PWyj2h0x0&07b{PaJlHK;#+Ujkmnd|3_gjibqp+E{GedYd0k zjcptGl18P}8N_8bEWNxLMyx85Q0#Uervbs$C)wXLIAN7O&b{8h;{YH}?|MUx1gyMP zV3G)6M$L#AvS{3f#J6yl6y@&PsVE3hU!@~Omyh{3V%YQ!Z&AvkzC7;vKBJ5_;dxsW z2}>j>(3T^;=hv>!zudZt(hF<>BXde^3LiG$BawZ%%QVzbim6<~hJ~v!rS|o@my#-- z;3{h3i;G7^7ILP)gjDX{K5u&C=N}bH?YjZpDa+0WDk>W?S{t|}%*}^O>pN>hIOMzQ zBF^S_7L0-@rQx_lLeJF?`v@QO9sbD-&(FUmD9 z!zMCJz>yu|DdED+sQTTIFYEQ9=je?!^P07%?yiXMW_n!6Lxg@5VeF>X!==My|KtSX zkis>~5S)+L>Ui6~^C=+OWkRWnO^j_IIwe?&QdkU>$6uF&Uo(l_xuoI43!g+|x4YAn z$Lbv*x5nDSC;oPy(uyh>n#Vc2vU-j2ZQi(7-}oX;0isGMNy4c ztc|r~3d|9#azR8JES*uayrJvz zO>>YJuPJx4TqiW%p28dbjKL?-J5l4}w>h-dA;mPyvK^CEU==R{ix?v=)`x(oY9*3o zNJROc0t=Z6+!*6uXxh~f%h1=TkodhA*lPVK(!Sg_{kk1U+W9O#=4cM6c-?PODLpn( ztAAn*rD6@S@VYsE+#xik@4P>!3w_~z_hx>XJ@R>!+6i}C#3|#la8hCODq(3j1X!k? zgw*~Pzt{!!$Ezb)e{~Bb&uRjfd5^yh$2QQ!Z~w^j^PSl{@2|iEuVKz5XXn-(1l_+{ z&oPzCzc4bWhhwe{Yc)}QaQHx?|+{x7pM8ArTKCmLYjOu;6~Q>M5W4^B86t{nRgCI57pa&ni%8Yoo+30WWZ1;VZE+1n2_6|$Pi5#Lh6~6g*M>G zT24RfOMf%fN$9xhC65tZs>s8ht_cS^YscLTGlqS)IYeVk6OTPQGO0O*#_C-h4*R=x zpYZbf8IaOyyFEZR!)rHk=wdQwHPy#a*PLy5{U_8p;P^@2ngkiZVl_Uc(=2rz(X79B zh>iYb@+VT-VWf}IV)>Oj4|AM!v0=1%0gsW(+GXt5x{ZTG*~hwh`<+z%6i!*jSk2!4 z20oh9g1S5f_TGJ4RLMff>Ow|K9!7N7!a|4I;C#?>MP|}^6`bk1Q_Qx>a7K$87xsKa zleu@wx=0{vQ`Hm@IIs??Eq?oX`iZ)s!^y}?q^{&S!t21_h_TMrl(&bq2M5CU8HS2v zBZIr4Asr7Nc8Z{z2DhNT9Gv0s>4)mgw>|0a!P-QS5+IxzPp$Hc1{8Y&Ka5D)@N_b z1bJ$sayV%oPy37+giDCKEdku8e3MFM)Hqc1sTC z9;$`Oe>Mtu^TuekMfu)AJnFNG5_+VE;?0gNw-Hf3wB@(x=%~I+fy#}?Xk0!>N&N)5 z^+(zJt`Y^nmL_)RaLmR`CA$`WU|(@C#p89sF(lLC zLk;EqwTAlngFXz~DSg)_n=OA5M;%(%a(wJxAiRCC%+K{*?R1Xvm4qM~xu0Q`=s55y z#={g-nw<6<5!TGCdA7s6lcC|hno<=e)i9KXC zhvJQB92T?9(Y-#UbXMkDs5PxcbQ!`2I)B0F0N(F>cu5Lj1p|RKA#W3Qg&;pB#9&9( zUm7$3$mqH2>a>R}_GNS|ks|jSTd-2HHsun%Q$TjO5Ib^SDOM!Ho4o<_eDIQdM0V=pKi9sW%dvjt={Tu9wp&iNZ`)}(I z^rHBM4S%C0oAL?I+pD@L6fmz0(etf$a$BbXD07w{zNXT|^sE-`|9A-62mc{JcPx*e z(-eVz#P%N|$*f4)otqTlrxD=bbp?6(Fw-M?`8C#yP0O3BV0YBuu|Q3+gwrDaGDz^KJ0OTZHD#GbfIbob+wO|CLMvfI>)R5 zyB#Otb;<5ErWlNF2rm0;g|UCVUrU+UdDWRWsncaQktQx<+1iKiSu|lYp!j27 z8Nti?9UFQJqC{LqaT!r8%jy3pNyc(pJ>9C(gO*M-{7Q4O%>W2DOSK1KA4;0J@OddZ*Wm<(G!*X<=`zn%k*BynBIoE5> zgtZiPt;}za$wu-_#YYh1kirTGe!Px@_F#^A?cfN4Q;c`d>zUR}uWHD((sZ~LJwx59+%_b9;^X!np6_|MRL%_{GWp+E`{4zVt~vp+t1 zgrea(X}q9K1;i8QB|v6kPQ<=I!EYv7RElQ3(wAS%lU_KUkiG18*Q2{2;)LT<$@@SX zsxwKtP2EiD`K-Pn3=vu6HgbW2aSnqx?+*E0w{bS972<}rEmGOe!4_t}Bm7a*wSh42 zRbM`N$prn3^G!J9hbV4_+-6bQ;G?GFkze;R88x1$=v0D)POaH54v%xdjQ(e-&yQqk zzf$VZnG2;SvbQtCfv<0$E8-R3FQ`$H5GPmN9ts_Z9_xDaHkpsJ8Fg(5CXLCz!Fa@* zF+Grcxh2450byT2N9!&3fn)lI(pSI>%qPN#-Pg)z^J^UShd!|$0JgDUc}@DhqqIPH znyBn{{REtT_}tw#<&XjG7gP2dP&MBDz03{3CX*)cwID$7{&BFMwzg0FS@AYs@pZo-Fth$UdFsOBVgxC@^4Pp5+almqbgQfGa%*i*7X1Sr z?DTQM?h={bJ5_G;cnp3m=`v+B6g${$hmk%`l_=!f7SnBG*CEhHxScq6WqAo=>)l{S zjxfddYB_#Me>bkz<>xm>U;OiYFUMmjIM%EvFi(n2VcN~W!;*tqVv<5T`7^ucet#6@ zJtt0ESAHT-Lt$c!LryiEhAREUIUb>3Y&GagW|t-mlW)q{U3J&(R0?>2Zaz+z+Zx62 zV5Lt!j6w+<~;E(pcA1^OG2V2@chda79)5y86vt8IthNMvN*`YGX!`Wd_ zTB3$dQos5oGn`Tml}2PUIkGUGCN-aFMUje|+`kI)m;uhaWrbJ9B}c2&Jw>jI8>cQk z`ja0D7^aNScWC9zBSSZv5U$ZeF>fyIT^CHYD{Ov3)r*D(ZbX$ePe0&V?h{g%E1dl# zksSRNKBk1#i{(k0S$%4KQA}Y@N0SUT_C4q)C;V77!lRR@biFsmHAkbKiDMZAdfF1g z;MsF?1_&5(8&d8RvI*;i2PA|rnjebSy}VUQHRp)FjyV69I4rICc7^y6fQHEPE3DSYumRhiCN=$>bjGQqnlQ1}S3qnY&ZeDQXhlW0 zzPOAphdg;2P3c5`h^LS@m9gL6B4gjwV%&?s;4jc(W$WH(8&MQx|D(}qc`NwP@+Uea zqV#}biT@MQk=h0uElau7xjcQ^geaCy4g_L24gw;a9g$|RSJOqCbev~}s9c-gKiGw9 zl^oxga+fvH!ehL!_1!o)+omVZ+)={snI^giA`Jd%*2z4A1&f@=co_X1sPZPVi32Qtqe~89W~;E4D#*H!uzRhO`XMs zl!%yIU{#raaN@FdhqS>gZ(W)%2@w+$)s;`-bJ;6#z`WF~n>o5yFjhER zcZG%PV%D-^UL{)y6JJ~MLcz9$5c_TAHMfYqEp{$5cza$<_$od2GPP2jM>f|40y2fF zB{+quRL&aTz03}~h^&i`xo`RjEIAVHT-b7teCo3uC7pe<_a(<~xTo-s%kRQFZ=Iq4 zQ86#sn_g{sww$TtVJ>dhNAZV-`p@Ga&c#6YM%%NwMMoW3zqW9ki1LS3ioFRE=VK3J z3&GQ3td34fbGw!Chvqx0W#Ohe18N9?MU5W%(%|T;V^fMzS?atKX-uq^WhZ~teGH}H ztdk}rb>(N0hu0>x_%`%)Qso%ku1i*rH(4^}Y~MwOb0e?B;gsOuw*UMnaQt(jL9V~! zG^H^y3@enZO_7N!14pW&z^dUlnWRD!OxuN+fdr#W_scMK_ML^xSlvA%3Q(+$$5KB4 z&#hWTgzA_r;N!T{^G&;u4{n(q|1zPg;`IB%9x_UKNV1Tf75CegGBuKNah+W4^=rcO zS?@b(y0W9O;+N)05Zpc6dgJDl9p@tXJL&S2SN$^sIx4z9Zd4$t)+3LMa=g1t0C&7D z(oKxTzochQil@Lspk2K53AjE;88V(xi_q7=>ZCmRW*uE0f}+I}e&&im{8qeL&HlNX9_OWHPSzt>6mw(|o~JW6H`NjW4q z^O#rNf0z5{FR5C;Z3x4+hxXWwO7VOWf|+)aaQpDqFtj+klF?-AIDiJ3I{^=pfDxSH zhYu%Kz7KoR-+yQ^fya&B5=@(6&M&MPx4BtR`to=+Gyl{LjciF3Ck_G(4UMFi)ml>1 z&2@>>*N+Bd2Hq*1AFTl@wI zyh?b6v+8ITiH(u>v%H3T>Fn^* zHiBm;*dv}&T`%azb1mBY;w9=f^#F{)4U>@yk!R&29kYnx(~xA{<5U?%tNNsdwC8$# zMTFdMjZdS~C}yfy#=CrTH}$KHI(L(SUPT)vAE-F}OjM&R7S5NdBG>LSVRptZtG%34 zf^Hn~LmMtb)4=6Mj{G9B>kz{x8P)m=^euEudOO45l44Fy#CjZr8#(n! zAUBVJN+8BRQ9d27Da@xaDaGleWY@z(0v;YI#c8dBLx5wfvV@dPmPWakfQM%UwB`-I zQqLkfYk=ddTOXT~Q#@+Wf+%e5bDmC4eL~}|^=FKB;P!lU%p01zT)JFYg8Z{NmBYn_ zh;4a#{}AKkTCcQ0RZ4MMu@c1SB^6RFW5DL(;$f^_wRh14mli$JBcP}#BBvsO-0dW# zIDJDp0i1_Eua!KjnC2gU#1O$gODRs%bKbvIa-cTxYuqd6@w&Jew%7U&=H%>8`k^Q7 z)5*HHu+5B(_1Y>bCQXM&9$!--jwU5HLQq@A_+n6pp;b~;{qM6}dZlDoLz<4&Upptq zpmmeRH~KV0!2Ix#U~0Q(a=X?ljB^Y zH&E()SFMKlHadpO*)tDW;e0?J_Ta?L!&j^Qvn+3vNL5KvaoSe@w-4HPU4PyG-X~?v zC$_FHWnFySc3HeIIN{`C(fZMux-m3(;3D|#Um{dH`9^sO?$|#!$$z7dJPzeu4gjMy zYduUrF_T-qf?$bi_VCr6+hCEEO^UqpG0ejP=?e7|WPY%(B6GWt{GlY=jgKy8fnDxF z$3n5sgDb3%IqwtmcocsBbUOnp->e*!}h7dkk6 zAM~VFrXJ;Uw_XDX5W#IB}!qIfzfBk$qt7lUvi571`2Qh$hW zuub(}d5EFh;l1Wn3J+z0?FzH!5$}Q<(;l^>IKDcf3p&NXVeQA6W+YfLPLtd z%_Y#>@rcXKX&WMtteN|uI&zgfZ%!uaul?HK=tGXc?Y)N&ur#X5Tn@nnhAV6%DJg!v zU$+bEnNb@@4A<8;Woe)t3o-=gm02?&e0-aaS4*#yN2Gb zb`I)ks0r^QeE7B)ha;Og?^&BN92cSW?2@!lqO}B1Pmo|eKV*+XRTJO zsg4-k1Un(p(c&wcvCa}K_m$3Hpg=wp$BN{=`W4sZCg!yKN(!ge6s?zAyDyk4R31zS z?P!_<3*&*(o|NKn`fph_*+sNOQ8Lq2MtnSPy?B|qUTSMkY((Jv1UiFRCZSEcgM*P= zpt6ovdf=rAolwxN1jzM5-8;9+q}A=Kd8PTmRBeds)*=8Gt zglV=GLW18wY37S^P#^l@1F5KzulcaM5WZjbF>1cgl}2nBRU;!CO8rJMKpw+!BeFhx zJ{SbAXFIn%^cnu`AC%nqu16FbHO0C2iz=a@`((OL%ykX-BB1H_Q22qq&nYQaE8nHN zAmTe~XbgNGR{V<@w|+7Hr@ER_vK7mWSP!1Vu*mn8ePaK84iCS9bD48ZLuJG0;l~@h z1lB^b{C7f%YloiPDXjyKGkBaZxuwcP(Vg;iwSYVrXf9x>>bTi>9eXJq5FEhpjOsf9h9MBhScz0e~gP3eu#@; z#e(2_JC`K!&v#X-VVaSU^n=>JARGD#BD*6={#^?9$M16aIHk`Ua?W~1{s!sp41+U! zyhREK*#^A!jUB9OND^f-F$a~8wDbD~E%lO(@2TxL&apEL!@n2AbY8~GvteHxi!Za4K9P&zYik#PPzUz;}pMNfBf{M=-G~-=*>|Q82<+{Yj@w1 zqt@gPQ6klUjQ0V;@G^*pP2r6`a@?-&3RO#tLfya7Q`O(BND=)OKdFLA3kyKaoef`@ zx~8RIaIR8QAe_61wV`HWK0c}!);{PWzJ2U7-U3=hWCqrksU=D)h0yBC@C=9s={E6$ zQ^kEv>jv-qM2XL}OI<8ixfjV6!QUn)zzKNsr_gWApiH~n z8i4ba-^qV|yDt7VwHv_iX7t8~lDn#Lce^%|wT0|vEQLIoPMszcYk`_hg_1CKD@#($ z8F~4``7dg4we%3adz?$?c(}B?#w#SSunLOrI|Ig3iCPwg*^gby?@Zc3`P!QWKk{yU zq$^Yr^<7ZtW&46Z!tsOco^HgUcnINKRP2DxrazcY*|=ztr?||*JRym?i}(d%Z9&9d z7<%Y&D@^lh@WLs8+i|^;-TT(gU`HPEKwvjNW4} zCby8PuT{T21qJj>q!uHi84kpQV>vn+QH#NBW&+1JnGEC{#KQLSr~8^=uBrRV+0e6= zt$$bY8B||(jY;!8$rATq(j$fY@HW&9N(Of`4PFt%;ds`{6-1e|3hHBU<;sA) z!YOJhZ6rC1brgS8afL4TqR3yT*-A~%yu5^zG7M?7rYyHigbtgqEUlJU77&99KaS16 zrH`|LGNM6rL8fB*bZg@5b*}&$59)CELSYttSq7}^VvTj8laTRTbHk8s>_3hRbxF}5 zjCA_+VYl7FkZq6aK^JGrr%?PD;f_$AKO#2dx#N3HXJBwnSL6Lb=&oJ}I70^Ev1@+= z3mxFYTlwKs_$YA;5Q!n%M&xB@+$J!ttxXcKaKk^qJ|mAq0>grZpfP|PPKDr0@-|Fk z2IiO1{4kGA7zH>V98*^5M1%gRev_F@=oG}{Bd}JLHsi^?)w*2=4%~)9O=h)6D__yI zpT{X4kwuK*ZYWZ$t1_cis@@pjyGOj1Iq(3eH430s>_o~%wSHu%VESV?Wa)=L=WKE#Fp$jia8Ho#XGea3X zJJD&Fn`+_*0R}$Lt}(~$aBg3J%vLAS6~naEt&Y+q>i%BHDI&8Wg`Uuvt6JaB#E5oT z1k)M3n^^4Ka?DKH#_vu-%V+BGFtM41M_w#2;PNe6>c!&0_l!_Ryl3>S!i0>8vj+R) zNY_a3jaBxQTf@j2EuxR()HS__kzh_bPwUt^ureywU2AZqF&@<#p8xBNp1!<8=k7EvRk$=1otC@W@AGn{+DWo$`JX2%_wX9A=C7Bh?sSc} zyPerHoI$t74N))_!XX6HIi{*P_9DUN;2Ozzy&3#`qc|snf?t)#C_`v*Mx4N!G243= zv70CbS?*x&|6@!Kf$bhmlasQ+67$rbJR?j`4gp**+$Aj94Q<&lH2A2NFFA( z|HM70NC|SO;vGGQ5~zjSE&TtA%l;|P;?*%;p@>F#5Bzb zIzLRvof<|q88~3Z$EDPYEh1oYE<{vdn$dRaFApoCYLPo+aYfGxa70^ilSm+^bdY17 zH3PDLtD*SuDRtsN1dhvCSs(aw&w9M|Ji3A1utkLa z-a@lCtVS5(&{2on;$t15pre9&$aE{A(g4s=FB9=*3d#I}jEht^SE7-neK=`1jN`_C zGIx>g_}!38vT~lG_%}^cRFZ+qbB^G(R8@^w<+&Q0S-@)&l`Mt|mxVY)7g2l2i{1T^ zC;m_~BPVe4eoLu;TSP$!>=BRGl+F;$%E4I=uvv|*G3EHkD#_IJ%ve|=w3Krtb^z8^ z8!c0g78>N)Jr&X?w-y=*TJk&i3pmltjhZQXiGs8iz#P2H??cfYGI^3q{*?Um8~CGP zyOU!Ct<)*Fmw|d+8;$xM&X%{I_dGKbi9zBtj!>VSUVa+hZHgr}1g0yp(*nwc0V^bW zPnl+d1nEed0}<(}g(jXSd4ArFMh>_$vnl}5NlyHXT7{WF@qI_N)DC4mhGQ}MwG&m? z@4g`Blsi$7Qb1j9b{X}y#T4K#x#}Gvr}$m;vP*;#J2rjt`59!JuA5@W>L)=@El>Qb zCT4q3Yk}40flxrdUr|;fL5BnVu@b7usAZ;!;o#+fN`n#AZQ-EpcW?{&{BOWGNRS}? zYHK`Xv1+pO_xNw^TqS#w1(t&?akc5nb+WjD+}ila(Ev%jbnOeNcMoUL8W-M#xHaw6i%k#li0yD{K9wZ!)8asm=jkO%RDWsO7c{hs);Csj_DgRm4b@jI5T3;V+pZUbMaN-dVL`=(@yq_ zx!nk#O~iJy82i_qG!KULqgOUpRI$iaiA$owQQ%8{zOsn81_S~EEmTqz7k0?yoZybV z+#hmSY`J<*Dl`4c@j~Ff1g?nZ7ZLMZVR@aLqvcS2V+oJ;)w(SH$TSiH-^S^>>mq(* zyTHcJc8#&faVRG+ZGH)hE!{?6-)JUK~g-a`kiJqhIM*ktW7 zL+u)$)Flfoa#Rn zg-cqi_HTS9IpHmobVOlX?KS8oN_wW%`@b0Xu88Us(tQnTjI;F+f8Y_?b4FT@+KOVA zpzYbK4Zh|=4^RApj?qKZOo6H7McPm!ltK<#|5p>3)}Dd0o;S_aq9#L$=y-@q`PY#Z zdB02B0Pj$Dvr+$f_d11PAfvUP9zKAA{x#hcxEJdmiVSnI@Y{1*}x_p!Y4HG$R zZ#Wk-wQ=B}*tjak!2qj}&cm8?wmy{hk#omI9`C|LQS%2Sf3HEOL#^oOf)Hs?HOxj~AX(j0NvW0(5hZw0xGn@fgF zbte)QgVJ)qzggPm%+0#PwXrfpGpuAxTtB7IPL77X*UDO{Sz20dOW{ z#6{G-a;|dUTrs2q`g|>jeQKJ13d7?>tr!dcs3C{t&Y$7Ni0FTOsR$0PCL`5&p`2EF zf%ymMYh0?UI-6+OnYzlf58#drjp3Lg#U`MWU`)zM^Hb`~%XY3WT~C*2F*b*1=B{O) z<>lq&equ<(9oK(c<=wOr?_|CN0L4{R;6Wrc;vZPTB)90spkQTk1Ys1LP%v20I2Mpd z1u8TNEaw0SvQQEykYo`S0womhe~1(qaRMo10%DY*{tlD2LJopzG;)B4ZyoQP2%gZ& zDq6v{WXu1uGo@y231QnwPbS!a5?Qo%=O##sU^18?hL(yDUU@ll_S2p#? zB$2r3&%OcB>@T#ZD5f?6iMyY>zrYrUyPdBr%+;LLW$igQeY@B|f;SO-8XgAld_!|K zWX90f1&*McO$egRN{{G97BuYnZ|qLr6d-Q9w?f*cTM`$VH>7N#C=qk|>kQ04lCR9B@@n=bl<_+E7O>wppP6sFU(|l{fxoy*N+`MndXl6V(4^jR?yX zV`@}%a}lsbVJVW=s-PI~wzj~{8RGpnZ^;*78&E36ibw3zWp^uBkcM~=n*}859=gJIZI_dV)Ao(*_?!FYhv{x}7a}-Ay)EdH z4=^33gm!DHU;h02S5;51vS&?CoXc z47F;dwphsh;4cDxW4|KU-##}V<+rEDwE%SQkkR28gO`3Ha#Px8V%m;&5&x*%j=}_A zcIpPZsw7w?jdI|tYNL;_tS&Ch>8w{&g}5=OS-7wQzM-n#hc?~&3SBVllbp< zo`p5kh}SJo&bQ->U&H_#7n`m)bwTy43P8D1IPLyoNo`(n{;7xpYutGL5myOx_I-ke{!&=jY=~|ocgugJLrWQcp@}rb~wG|XKPE5tkPj* z_kO(L*1&@oCmR=sT5;Xp<#K)x3B0>bopbZ<$r?c$hQ&LqZSKCY33tAKcsKqVtJu`h z!YX5i|A|UVC&HO~*H}qH1Xeywejwo`EfEooS8p&G94Q$XMg7$Jd6Y!|_153A^7*>1 zmG5SE9`vP$69`{GcHF2o^d3twapC)TRK6hg0pv`X2Q=8v-1bqnsjRF*0!qHTt3rR> zDma%i6~16SseWD&8t_Sv=VlVEeA-h@VCq=mmno|#gzjZeP- zI}9`Yl=gk0*)A41#3O^C0kAB3zuvnsFO1yWQj1p_*^_V>DbWRzYSzGO$ay&0Ic=f9&C6!FFAKe2U{#2oPjMw^LI@Ps+Ab~TdycfkRT8-137&{e0{}y03oe=`lHifnAvUn(^JgEp8f<1jdptV%hphp$`9!< z4%2S-HFZs#8#kvTv?d12eX)y6$}f;HVoY`BpbAG4&*U%Q1E~`p;{vT-QZ|cJzw@d| zJZ3+SR+P@MSfrBdkzN{#qUD-L*r#F9mkSZQv-0t@Hdo;(;@>Fcx2M74lj-tC9rLdq zOU=8V_q8|7UKd0V;=S^PTBKu7P!TyK@Zs zXG8*B_kYF!Rx1!XI^`i6W7>F@vm*=eu08IETYXmmiiWS9F>v**lWT(23a5mtO{m16 zKq!C~p*P_8b={dJ!lYsbih^WL2&fKDyOx34z#WEuvrfm34f*IRb zAO@3O5Jw)7KJaA>B={Gs7k3qALy-bB+D`u9A#%WCWXub)69+g-73{%FeGD3emWcRJ zY9Q9yywT|!c%Pv9dme9F9ynB!7FOF}8%9e)=QS}mCujvvW}a`rHZ%294r;8D7d8X6 z;CcsqWgS^R7R00;D$b)1`4~6^2A+mf!wV^5v z@I?60y0$wJ<~PmSwAtt>D$;r9iE~W%GITva@~^9 zU)EPmshom1fe+#XF=faV$Rm*}-)!#rp}U8wM5jP@lEh#EQYw!sAWy3|c#EGG3_!nP zJJ^*GP8=x5%CIgRoCt5~lYk9H!6ww8%qw7b^QM3Ao?M$Sf_(KCn9c7iehCh2d>q`s zw%mA>sUd_c9kMtv3Zg0RjA>2E+28T3fp?X~d==dYee#?;U2@8P&gyN|}RB%_$ zmTE7$Jjvd4*!#x5;Z6oO6-~=4Sg8r>!t(Rp1DR1t;o#jKHN8I(D*lPAGcf0ui%e0SX$n%b7{*R%HvWDW(KBeY8%nzAs^guhY83Ih3|<&IjMcuZ2{!2 z@7m`f$WnWcT9)O2^5f&NMZ2={8;nK#hXw8S*G_)i*M`tq>5uDA`$+C8QJHa0}V1HgyLm;zWNg=-LDJE^=`x0ScDy4eQ*dPe+ zd6eN3a5O+>tl=5LP@Tz@j#WJM=`fr@B%NdPVIH$bmlk;*_;em9cY9sr`BJ0ABZ(pg zrk8$F5g`QS=bBpLi;d=Glch-4+M%#F-3)yYi_Zvt~V=5a24KI^c%^)!w+Qnx%1gBK%4T>}rU|u?2RvFf1 z)C@>N+|msd7YX&Bs+R58?!P`h4A+$+-mk`)^_tt?nPLm9hFpHU4k;C~a0);FY3`iNnTCN5N*E#?+4cR36o<|j|k7py;wu7tE zU0DK`!tOD%KfEiBC_Xy!MFtB_BJ zdD2w}iWpjib#;I7ap90+tYC=!i;larSiE}vg2vr@)BKNvKQEg{I>IfU^F=)r`K#&Y zJl0K$VwDKu4CGDROx0fTkfGJ^yA=%$^=Q(p9nDv@OcrGqFX-(YErl*DT(@ss-z~gy>1!23JcV+)w&KC) zm%u?2e3-4SzheBgR(5uN#$hYJj7MsPyI4gE;xd=Rh5==#U;<6wf%D>3IzNQiij$E` zw+7p(tPEw)qE^>psF1z2-(iop{UOT;6nq_ubE9o|9d<2LejZHVYZk2OE+d`4LBlmj zt_L5*{=S;qGgev|+H!=-sKjgiD6$#%tKFK`eW$J-IZx&t-bEDX9JbA(z{ZFh9%7{4 zWNOw*^C8dm&>I$C*ODu%IciA~&7-$7!rQ!5_c^3RhZmp#dUylcug`cNADJ_`pYyWT z(#nm<6Mt3GZzQ~>aQlk02xC)LnRF0uqThxy`-xi*pV__z%H159Qc719?Vi&+lEe=; zD@iLI*V;9PXfiB5lCU{J-efP>W20D9%#D!v2&@Fz__ zEeaArW2YuFLc89oc9_e)QsKGI%$q10CUb)kDC0QFgE|KVKR0-B>a{&@&AviumS|V} z_WF_DW!W==!VMs)o`4#ZuU#G0M_ZM{0uYTW`>;PQw9L=FnIBuDJKF)-RZq-(tNNob zk_2YvO*Ei!zWI7~*QffbOsD%Q=QIz?ZY4$0Zgx6L?8shaN)dX*sL@$4$8GY|MVazU zQ%7cx(W#waYP6{#RI>|fn)2-N4*jme12;6_S%E$9<4JUOF%NG_?C_YG>{^)&lBF+3 z@eEE~^b{RaeLxgfp_&*z zaw0OVHn=IVywqzzy39;*s~B~(Hh!aRYxB$WP#xwhJPq4>DAD3B5zs_kw!izj&-G{_ ze1Pl6a=rZC`~g&uf2$eQT3nYp+)L;=jEOZb>%N7al$0=7n--cakL62V>_03h6;@nX zdm##{I0}w$YYy5sG?8|w-(0pXN`u7FzPd0j#!=^&d-Sy`%*r1la;CEGVMlk-R;qU$ zUMd<1jl*g@=32hzb$%WHwoEK6D`4+sHcW|gw7y&A;hCqM;jn?0{wQ!2!y;nA$JXv* z{$YSwn6!5$a?kDR*!VRxFC!~NnP(7j_#NuJyRNNt#N>(7iB>eJ-LkH4oe5Gqod{@; z*w4Teb_echEaG9d=6gs|_s!?QFVbDbZbUfjwPPEq9_M5H@4iSd*Zg#weAN-KN%y@! z+3^}8RarptX{UNhzaD&>n}JmE`IS+>LOE}?!sne_#j4x1ybi108PSGYix^3&4Xy?f zNVYZi0BsCZsO204Hs>A`O%4N{<} zulIGF4AR^o(I6kjGqK`%@!c0A8@f@T*x4|hs;Wc~bFEn6SqQ)isG9b3*L{AMz-}jE zV@MGNvZFL=L1Oa)->#QZ1xf*dIOayI3{Kh0DT>b7Fx~==tH|8`-BVV6bo}~6nGfO~ zOiCIYPA4D!P>t$Skp?)Yl1_)R+lj2?(;7gO$;!1XW9?o=kbMQnz^Y(-AXOkbU($hC z2u|aRVm8Et1X_$)Tv+8rv07e@DM1#DKN?tuxhSEi|3YMx$u$AkVyN|jdZ`SGmAeSM zLOwBX2`KYv8@su0N&+8a$-Er|QCyITeGMFuss;|YWgJv#ftU6?@t<{h<*qZ#Z4xw^ zq|jz?WjlTTO64q#DFr0Guh~pHLkKhg+Mu)Jn!+b4Z|rsC=@GK0NKvtT=pIu7WNGm7 z8^|@8DJSrCVqJ&6@wC&;#c;FXyyTi3AS)grl9}6>kfE%p{rAY25(!br>ONiql@v?j zmOtX@q+?EE)LeFp=My!couilrHF2QDSH}U4+POt>lyQ$a`CFJzjKtV*ezr%5q0>kN zgn9I<-iAJRME#tlx1Rax2#{NoXS1|iWZ2=8!LJE6@=Mf`Z#w{clqvk$wryCKe$rmJ=bXH6^3?C zIrfh0F*@>WQT@^l*6x;$`37zFmqJ5%R4>O+M~@J16?fIR6VJuR#ltqYQ#j1lvxhTZ zC&^#Lnp7Z?TqSg6lV4*wh zNkW|{D&hU}jnDjT%|=Iv+tyYoFsb6sZxoT&7L%Pnyw8mHU=lt@O@diNh=-o&JG^p- zyLb7G5Eo@}qeMZ?D6%YXOZr)g$g9(c3n^hguT*)4$mp|r1P}DLA0TXg6j{f?7z@2# zV%K~3IM8bqA^Yjop6gG9MA>ev%zSvZ%FcHaYqZ<}PJv0Wof1SDn zZ=wK`;aFRhjzO0vRzvg4BX54v_WV%$Cc{bN$I^5i?adrALcE-Fex_~mCGq=O%iZ4f zcdc5VW%XIl3ng5`4nj`(kqXJAA8PUCE@C0a$>c>QutdwIQFpeC=3$~7E`^4oK}1U) zmM>Fp*96BTY^XL;kSyOwSR>g+`P)f+^Q<@&sNhbP2EEVvck`gK@ttL^NnRxVKmhh8 z|11WKc9!o8*H@5ITgu)3iEohuK77ZmRMWOjOlxO-n2%5)SV zaiHbPpVUza$TbBqGFzkX3#6P$kH*c_-qVDZG9?SkUIOUPW#5DB=Con?X;vqqx$zlu ztZPRP>;h4JVcWh38MzP$aTfcDa`Rx~#c(A3!UHESNpK&ytak{GrmfInRbG|Yek7f} zgF|_Lf%*_1l zjqTYoebwB|zDcWE9a|`{#o6hf4nA^qa{68`Sz=9k0;M=b#p9=#kZA$n&v0Iaqt0-t z!l`b(wjpzpgpbMfdY|=#HuO#z%EW%+Gis1lD`6a^nIvaQx>svJ{WK$)hlaZ*WN19X z-2#{q_mezAhn-bQ##XQ>M*)pZbw7;-%e7yy0qxv4m%H5LAQvwE)$#j*!MzkjLm`)b zYQ9ZTd081CCv9*2X}A%0&y)>WkN+D=njc;5`YwS);pH4kzCrpt~%`dSc< zcZB%=s|ou`?A+2_$wd$0V#jDUO{C7%B2|l^Kk6j}jLc<7H=O~YL`M4emxn-!U_9w_ zBYw$@5r~s<8-j70rI z=)}@b^#a|$Sz*60VgTSss8JO%IVQ>dH%RB09`<*;4gQlQrl$o0GXjkN;g0=xVdxmN z^LM*@nxLe{#(li}-z6ddVEuar001sh1aCEglL*t*;NV4xZH})XQ)} ze|h}(zK<1WbqMJQW&#vJ@jsM?I)n=dPT@bS{$S;C{1HO54#5_|{f~9lkZ|z(V<#N{ zLvf%(z#w`5^COPuUeYC;QUA4%7f3kRUY7us6+-^5WaacK-sjPbAC$ delta 14485 zcma*O1yCJZvo^eOcXxMpcY*{D65J&?Y+MI|yX@fZ8XST{fB*>)Jh;2N1Ooh(WB0uG zz4g~ORl90ducy18?lrS&SI_Fl4yd*`C^U5?7+72o2oVG#l;ul8dky?`D?J!J9c&B> z0{#9Wf3}Wo2|2x7&R9=CpD#&lZCUTt&@!ywYHWVn;^A> zII^g-wY9Bb+lRMjQeM)af2PZe-Kc~c~J-|-F&Sq}5Zo*Dxj#h5M?iRw% zfQyyW-#Wtoh714U=}(EbgRPUj81*}McNbv}4lge+b}t@wXRr+imynPU#~-uQe?cuR z{=vC;fF1roSz2&dIavK><;KCq&c*RJ_BTb~|EBfdtbT+4&B+C9PE&lhT`FjP$MgLew3$WE6!LV5Z zX6{zv+?-t8Y+T%IJX~6wLc%=!!dyb49RCaW8z*XMA^gV;HzyYl8z&bV_n!*f!n{JF z9Dh&#>B7p=*4@_0hRxCg{D&)XbuB>!UQQJrZq2`J|A7Gh4f#(^3m*#yD>w1Ksr+s5 zFY%vP8!IO(u$j9vSX|lG9Bifb$4kh;!_H5uVrHSHscYgb$WQsNDH9j4vz?WNJBJmw zkQp}@FB`yZZf<2Mz|F?V&&_3N#m{CYB*4clXu&3E&B@Pi`4`pyar39!zu#Ckx8E;- zo4c(Ay2Ivh z>$EMo0=3fGrw;v-njeON{&Dk@D2R$l0$q!)vm(&srV2ls4#eWxQ!rMCesH7-W=w#H zso)i14Yg?*OUj06y0S+GFI}gVV4W$GwD;iC)5cUp)&q&;DdKd1IoTaTVuM zt>yElpZ6^56v+#C~M+IYH@H1Xs$bHz^;dWzzlB-Yk84YZ}+u;3%VzGPruIqzc&oO~PL&W>hyP9%9 zJAh|MMBni=Tq(!Dsyz~)1KMCcho&{YS^ydG{OV{1qGQU1$i?^dpCXOpCro~ zwGQm}2yT||Qz8fOzR7rpxn!a29{5=SbMNkOV??q0I|-bkm{KO<*PCMv1c@Ior22CW z7c5W$V~2xWYWb*?rLlh1NOn7J$>iT{CqleK!|70rqlzgXf|NJnBbjy@M6s_H%1F7H z&`nDtE+jC^YbHWU8d23L3c(+U>SR~3$+;69`59ucSLRx4p11T957a66QoF=}Q*xL7 zcd|knoRTed)<;KBv;6KAt_Rin9<$bIRk3XPC-Ql$?yF0K4A|e*YMU_4H=2JfOHgDw zHK4jjYHjyWt5Ag4AgFJV*ou^{_ca z8=Bq&J6(mad8FLL?@5Ea2YeprvxNsu9Pt}rWJ=tSv6gBqENY6jste9EPAubP)-zcn z6@72?Te+WsRYKAn+Pk>VSZi8!WVF)KrPs}s_Ny;Y-SW56`$k4>r#PJe;iYlKN`Xq7 zUD)%Hc$6e&SfOmZ{rWrmTGUh+7yl=M>wEopaxvQ@BN?}Tm9|u$JeFi>USA}uo3+SE z7&RvJ%i(P9)&+uL-%dNeVztEl%exP$Fi~|rk78pbo}`UmAh50~xI+F$uCG#b+%5e# z4(jtJf8O7Gl0`XxjP6YXs&E5YU5^JVM+l|dKQagtX2@08F2(WgNF6B}7>d~Awi3wIix-4;HiM(J(vrVjxLL+g!B z$_nJR&p+&bG(S6ah5j=hQtn7=vIF?oQ5>Is@^fwWAmr8Ao1OHx02q6DiW4@gWcPwKF_=LBxWY}4_TR_wuouQ^lp--NY3f#gdRV6~njdecY^?+8@d2qit zDEtOHPRdp}s6KoEZfRKRE@c}dW-dt9@wNzu+r^Ft)iN3@hOW6eU4d;BB* zlN}_BNw2WIjV3C(+_U434xDi}0@)@_ECfX-mgBFK&R)-yOF?56U?B8<{wzcRc)wUR zqlz;uiH`knmbWC0dv7T=vU~7wda^%G0ZCSzTZGzxtXS#Th-s`RKSZtckH1ycGyLS?t&z zsd2PoJ(|f0w0p7CoUajEQ>9qa8EgPm8U-XZfBZ#f5*~x9ZsJ(l(exvlW5oSwWNGrr z>5~snhN!rBJjH_G(yB`255U(PTm?T~3C@Z*nfj19PXCqrbHok~H$5a^LAxQV#gQVl zIjbp9H{f;4$AXO0Qu(g-1$rUe;Jldk3#T#1!twqA-S%uod1s=WbyRL^Nc z;#dA}3=K*s)bN4j(hf^|yu2J%@ho;5yv>D5Bwj{&dUa}L@uK0kA&9}lVtD(-!!^iz zZf9>_c7`w{#!ex#*Xn{|CW6XyKb_jeDIv8}R!7D`4dewmHPb%|gRBz5zvG8}j%iJI zfT*;rXp=R)>p|?*d|NP+hW%9*9mG(D>6`g4tTQ z#jQHXR8LiWMl9=jBs6EaTfSI)Bi-MRnw>6d&snwpRNLrrdH9%WwEAhGtcSjF#iG4q zoZJ&}=SukJRIb0sLmHc;uOr5za1CJ8@9Z}3W3v?Y^X^c-_+EI$$&}HTAoZ7RphSan znD^ev{@no}{4+yN;sw1XS*SFy^kO`(Y$>ne+2?xf)!x$Re3iS=O58W&LBB`OAHv-X z=CqGI^#b3Q`#-Er*dC^+{L~+}?*GseXQKCs1OE;8?O}&ePlJ<<13#qSGgG#V*6liy$q{) zL1^c~Q*dnhU@_CG2 zG=TX0OCzt&yI$kx#XGwrGJFpHR&nw%^{)QHD=u?Gy;NJ{%Gc=R6vM;pb;{xlBX5cx zuImixJMxDI9?zeOHXDYKC%raK_Ftaw0OnuS9QEqF9uv4t*@5q~U-Ko0nNOfU{`-U#L*i3d=;s|5Tx-6_JTtp%t$b@pp31jTe!LT-z7@C?dPhzh}!k z;{ls=-%W4F(lOJ_V64{2y`{+yRI%IWZFG)PZk(g<9&>pL|{`PSCzp(J^j{VB>u z{d3H}1stB*!c4HHE)-oXqECi2jaCMJ8EiFuA^c!+S>RuIGxxNdP? z`!_e09JvcbE}}(dy2z>+a@e^71=Su5cDwH`r>7|$*R&QJZiF*V8XO_zT(#vsrR-I+ zjS(k%#kl0=Eiv_#+Y$MoxD5Ife+@78qEbmiGTbew2HO@f=gl#hgp4r>9r$1qE^lJV zC^dCMSPcb*UK%n?DMEUO68lZ)3+Z0mk|_`D-5%P6K@!d&o%k@2Iy5&>`YT^Bx!qby zJ!PBU`-4Fx9bbD%{pIqhVx6nSKs7GXnF~cvQz{U0H7I9(| zvp`0XQiun#HED{lVq&g8oyCX^_hoLX%P!1gsq_C>gd&>pVG3Zp8c{DL|1b+>X$CJq z6UkkGB~>3!gPk0CMg0X4*m?@axs}mswO9?;OrY6_tiFz^K7ao;tM>yr=K+*5wnng~ z8|*u|0v%MUIB7YbB+6>F0|j1W<0q`2=W(|@}JZiza!}<{cK?acXrGl zE=*!=9c^NppHV|m3XGVMTm?8~=Gdx=OqJH=?MjxpvUY#djjYW|E?v^TyE5b-1&?R$ z7%QiEe!$}rcYxP;3#HOg`Y44|7|RK-w^5dt6V{WJfg+I%_7hXMMjUIk z4vYL~)WmgD$3@Wm`7}(^gnuTWC3jw$hy4T8y!iM8KGZvK;*2_u9&NcD++L#qeSBam zg~jPC-UC%d9^qY`g^md1ey#!t<9@2C;E_nUaWMfOF7^pkzoP2zO32Pi#2G$~_a zt7J2l0B&U>PPpfb>}2akbih?;BCP`sBF-0+#i|v2GNrry!o{Pv ziS5D;wixG7mFy(f;-Vj?y+}_7i9ut1A>O(!dbrqR@2JnuI%;9=#Kq#XyUI^ZSsuy~019~54mel0gh2%a8;d$z9{JUo*(uMv z#P1NZz(-ZMcoFD5Vy0mqmM;riTw*#kokX=kEVS`)t51&8W%{euK;a!vs^{la#MK z2&@IxPMZ2*)^8;oIxS{aOYDE&Ygek&9D4U`WYqYAYa84W%6~d0k&P4IVa+Q#%)HRp z)zI-wEWvk2%SOz?Y^WUd)<1(_N)AJ-Pb_kA5CI7d6*spyOj%u*U8XVvl`cwpumstI zbcc$L3*{AVhOFckxzurupqvxUp1VqX8_?mwPWF)m>-O=Ue~~rd2icNkML~cRDH13h z90nTN*ZB$y8jXE%u0>}t3YHBrqIi9(3Z?!$6Y5tm&U@igBAkmf(JM(JuqMe&g#DUs zw0BZ`@>|Pv(zk9Cb-T1cv}nFpAB0aGG#q0P5wF;#-NZ{quAM`6pW9XasQtdO&jOX@ z)|dZ(3CG3T8>VZfy!U$&m`=-UdRuS&xor0lTB{+I++# znU}CA5+4f=C0Z^*7X96S^oBj=2b<8YH{IzOD8}gzWK^{`Y^zo&8_}sE<5s$ zcB)o@4?fFVjoMK#!=o1yTb0Q4^($Z)j{{rwP6|sb7bBD;+}cMCB9pjqwfnwiM$?Zu ze-JHcYeAQhZ<3I@BovuWm_8{IYAXF#CQJ90y7DroAy^#!u6iuA5Gb_gEs!}LFy-}$ zzYW~(cNO$qNjR9lJSmPyA1;MDxeIkP4ENt!SVhCy`W&p$VNGNXeeFe8q6Y*>OEsT< z6?V7eEU+L(@!BQT)L&W>iT-eu?r;62hm*V^%m6A3sB0{<4&LN_)5;!vI&dS7bY(~J zqa0cN&6;nj0fpaK+09O*s63ye@vVXExrX7A*~At1FfRTt-Egjgf~jTL%N;5QMoKs( zynaUGsYCByC6wX}jk0rCIy%6_bBgqlw&n>^AC>_g0cF;gT&NmWY$4OvW-l1bskEp# zVtr`|K5VEdb9*Ke4vKzMw?|s!ER|}JjwR7mCQ&#fgs!&*whu*C`TuGH_(`sU&A>z(kV_fA)Gmbmp?^*4rUNF5zmMx2Op(QVpKX z#?oUk`EsJCO4Ncgugw}L2+D6>vJ~hL$iWF@8Y=$O3o+;=Pd+1H_3l|e?+JP-q4{Ls zuMKVuBhRWit4u4bZtwxx6(ElB-*DjIwCWW!q1<|I<7V$ruFoD(e!T=@^+$f`5x!S* z>$wl6pj5f-x#id~VZAPRJyZ(4p!sq__3I4birdD(9siLc`b<7hS2Zae%qyfFJj3!U z0QVQa+FVayPtZWEz(MH1q;sI#gVMVvrTpg4&Xb&?8p&U$Q}nt3?r!Scyj6~OF74#C zb4ylP__MNInLZcF-ra@OZ&{7qrVC5$euaUq5A0m0F+>fN{4#x7sD+Yqpou!D74H1? zzB9few*D%*lHp}5BlT`$`0E(O#R=S3|M5(_Jx4PKx{WlKl$z;Tm~M_?h{V&IZ33FM zOwEMdM3!v-ZuWX6;6q*f3(B0Xs`lHf&0eR6>}fdx{$x=W!DQi;(VTK_{_a%?7gK?g zel81n=)DHmq$^%28#vX)r45E69wpzd88p_iPt;&`3lt%>1+dQcwQc}f63M*kBc%t2 z<@^>Ucf1!Zd|TUb#ObIG=}-AX%;KHQ1OL!)oe8`MdGlimK(w?_lrD);NR4ZjqQ8u^ z!!o>>*OvUdwB52_Q0iq~_&v>hrdo0Ul|$;JCUIO6rWv71TQE+}P;q1o?p!6pp6aY3 zIVIs$rbk733#*MN*<4p1(M>oUwuFbm6%k|{t&e_%cH%Xrf`i_RqQIn{#!Q>8!hnPN zR=vB$+bV1Vz4mhW33&u%^H%{7w z5716~%Q*jmyjoGs`T%<_3kx3oyK}9f*A}<@O6!lqOCXcRc&C9X3SD#;9{)S}mQlx<#RTAfZx5*mQn^e4IK-_1JeP1|4nz1iiHY)}$iAQPwW(g8s6)fpTD>ioY%c4E zp2l%v zdJQOdW@m2uDo>z`(%ad+A#kQ7Cq(Y~c{r8CH5VsWGVaU5Di)o)&yDXv$=FF|>y~1( z7<{R0c-MavrJAbOuGA9J8j!cS3mdL6w~~_U<>e@&_Uk+li_)Y0D?n5``#)T{MVulSa;S|@Bt=`b-!Gg$F!*q~>T~>?= zYSHa_?x-NU9~9p9;Vv4DA#=CiHV5jVyAoOZjvDvCdm9civhEbrSjvEl8{9am{~T}- z6&ocoF{}H4@PKrlX*Eso({{v~)~r~`9KFq)83x_L0Qo8+QpcjR=n9xC$@09wS3U90 zF5j@~@0VVk^x^X`A-6>#ZYIqlk*|NLYtO*w|70jK*tc1i8U5ZBR!CQ>5d#eCoIBw5gUj~%G?vTv z>xOxvw~91kiO2SG!)k?!YNdcrrT7r9&n#uXI>7{&o|=AzMSd#}LPQ50T$eDZQZ1MoEOcl0gdr_}5-urO z_6OzcX%o|O^OvwDGD&n`j3t5rTKXK98jkxr7CqcL^*Osh)R7PphSN_3Z+O){a#!p_ zMrg&Nq#4mZy}?FTGC?E+)xKbe%Q`KoAd*QeMBS%jijTYP^UCZ_ucBajIy+8lD*5fG zSf%M6LuxR@1+(o4^?=>#wTF-kHu@Vcne5O}ep~eHP>+zdwYD-~($v^i8irv5NmbhO z04aX_!gZb2A;k8|;51+>aGh54eff=4;yC=GnyU2fAXWqVs-2Jvb~pT89?vF$-UdQ3f9652izFjX)AuEac3iw#VflSvlbNd2ci}2s* zmhsEBga-A($hqBTA8s&#%jT++f$`uIZn&uert)cWcd$F z>+pcR$EKI9m26jzJXhAOkA#oOofCr~&S4o^@$pEbzBeiStLBONOCBNq&z zfdxEg%OI+$LmW}>Jn?@4fPWC`1O5H|K^^mpu@}cqLDE(UN%T*g)(PatRzbREm0GVP zVu$f*FQP;o0tFzzfZ${W?jjCEu675c1$hq;stY3GyG(g?MJ;exgdt6?k3?05g zrA+yCv}w{Ap$k#PK?SHsNI+9t&`eaXSalYL4>XRIEdeu_YR0Ez{f>1KDfK66WC9U; zx=B z%!}{|5sOdlBTG8GPM!Ds@3F z_}OndT+w};!BK`7y@m=79&0S-DEYN&{F(X_axLC4?J7&d*x-(G^A1r8AS zW1|Rt%{|JYt~))Ioxs7BGt5-wQ`zwV7K2vTBeNR?H2o*259>C*eLcE&EWW1Hbdx2_ zuWfXK!8dv9_qKc$^+cFzLkV*^`-*kxt!{Fdx5+TglX#pxSg7}|APH2|cD3g$;2MUe z!Z0vHN|N(TQu`1dBlTL+2VtM^I$bsf%24jweKV24CH}WLW=X#}HRR~B zFl>HGd@9WS$vdWKRFLTkLTWz8_A4(R;f^lWqWnAqKNpvxYnjSQ_{$nH)T#S{4PvK>iiy&btrGzfO}a@-_%NV)PhZ z?Bggd7~pV^z$h5#9D&WxFKV?YkkK@Flkutmc1s1+61VT6S4$))12=KB>QNeEh|~F zt6cWAQqL$PluxLOE9W9#bQhM4!Nj3~l$R;VA^bc13gX!H!q4~78i>QzFQMP~synmR z;NMjsO0AXWzX`C58_y6PD`7IhIaVXT-E_Vyd7^P9&MzTps;8zUjB*>Xy68X{b4pvl zhszTsO!B29VS{(MArJZnV8pi$uHw;Q_B(19VYayu_RqtBLsj9{nW9T&9z!LXp=SIy z&GJ)MQ|=Y%+sj|0H7B&YpHhDR#Sfw%9)>Ixw!Wv$PRSYOUWw=(wjq4^##|}soY9sV z=k{ISoaZe@2Ao(~{+i0GzPFp(w9Kf97EUumV?*0WMod1>s*A)lz`{KoLdPo&lKeFl z18QD@yuHQYQyxleRR;HFs8q`GI-_IHQ|7N)k%!EQxMIR{H{R>M=45&(i+!LsuuD1! zUD}QO%qg?ct(SvorrPOH(Gzu!H+FoGrRQP{430aDi89!&iE&l=D^miPrVo|IPjpK< zB1?@AG7rXgUSHN)0SW*qeq1^RYQ_`q?TKN4K_7<^#~l|WmRjW|Pwimu`Xs?G!cvu_ zIO51Fq@{Ph)Ad*GtQ~heMbkCRI`1Mp%zQaX6ThbILc5mdh;hOq7<*>Ce0JH9^Q5Fn zE4Zn^Wz94H8;mxC0Bb1by;!_nI#fgX;wLhkVXbX^ZwTHXU~$y|jJuvxOPkj6OPU?p zEqxM$?@{P*U$oxhWRZA1UyfGC*!CTq1Vv9FEqdVz8Ca}Ab}uQup#z=8`IoFkgzrhH zC6sev)a5|Ero46ZG7-2qX?OM$ik(G~Vu9vRt+m6LxXD{L*xc3|3+h?~NhvaV(XXV-jyvmVX3P#uQwJ-8nZ_s{A4q(%*=C44QsSes zA-TU_&lY5SrGE3GIR9X)n=6YrXy~B(MCe@@)Ul#GfXaaIHtDDDtTVJI90izR-)x8< zB&lHYtNVlv67M~p>i0AaO@+LtXi2(GlURijYHJS@dv<`+W1-}h9Z2XHHH%aQO-I=2 z?6=i>WoyGi<&GpHE;8v@j!-lV(v&j>i|zZ~I-ZT$O4v#vayREQF>?2^Zj!sd`1E$4 zP+!FZ#PZ9V{H|8&<3k5*F!oXdnZU>?~jR9@eQ&j`cmryjuBh~V+8&QJk!Xsu|f^xp|OZsbLhKSGB zZ;r@ElHeI8XU7`UF_3X>(;3U_e_AO0gqBECfpI&G719^w6l)L1K=!d}_;Yy;w9jDe z8)sUtw0XjdjAlr@{K?MyK*)pxT3)dh4Vf>uO>V#O=iC zrd~Sva0xU?j!@h~eAx}L0Oaz9;Nh9@ACiHh*t5Bi4vJTw#U+yG?kf|u8meYy-*68z z^9{~y2UDQDaNl!lQ)2YPuvGSBZ&x*+r5nJBJocDDyWz6P`y)*Cn>R4 zc3@ilOdWqI9kt4RXwSMtx1@dw(f}HP6wvvn*K9F#+0o9TM;t3er=O7Wx zQQ-NG!C&Fj>-r3M5=Yypqkc5&9d!A5;bXZW6!>Ez6kF0IKz$;LoTBWa-I@8R2YzZF zDp9;^4U3ec9twjaK)}|}d|Rcg8ESyRf=?VY4)aygo4j|5;~^~lJHJasH8Xk3)-14D zl}|KrUZ=cEX!<}4fFq9t2N#q)MMW`45BE;Iwoi##Si5&|I!!SxHhS(fI41;h(KZs+ zw-n)(%&j>ls)=H{zr|DZYcF!wVFnU-3aKx3q3i*V^gnJvk-k99_n$t}#b=sd82Tv& zx1A;~c+Bd$17en`;L_%?P2MMXSBiobfDsH!MBn_nStSvGipf{-jZ-++J0gT$eN=S% zan=`7!^qjQZhdx6_={{Q%7F~ALx!@tjv(Sk|9t&~m(4BVNpsL!9tyz<{Y@Tq?Uipn zK?I0YLxaZ@?>J!R1H`+n$GiE?$%kX%3#ehP>4!TH3Wn~7&(_YK*do;5*G3{eb;F*+ zSnd$3)Ht64yhNTZHa*Pz+(Ak{ls?1-f$A=bLP19o&oU$TT3(+4crpHlX-y9Cbl?yb{x3VYtEc7fbfY5z{$^v`+^6hJT z)_wOqZ}x<{#fN4_99*)5&FtSZB->+14jSf)$@K9wT;)3>+y%muz^7x>z%Ecbv z8V5O;1?hep0C9@jxGeRqX;}8q#DeiuEm2?u16Ia`-a|)stzpp%EE;*6&C+dBUGF}? z=T8Uez^e(IZ0oRc=F;j`)*8ldwRJc41gAdg&K>48DWhO9}5xQ)xra{LEm<0RtuQ zBfhb^c<;c6(V5PjWpK-_jc#OyUr776_|D5K$j>#~d$!<45oI_6I{1cCnfN&vMX>|} zhDR?Pv>>a2bV~z|m}Ha=_|=eka5I20Z2>(Dh8zC%5liXvJAO`%PG~#)sw#UR7%%;D zYg4UTeef(}4ZbcgiqP|jI({@8u-x)*i(TVc1C=!~;H~T)5eSBO((U9%tU?8iwND}7 zbN13BGJk!`x+Wg!Ok4i!QG!v<88rKTz(itBF0Fm?kf=vK3@T_bTwgmQ_ZrYGAb#B< zZHiJ=mAu)lT46kgqChcW4w4)1ddwt@LxPYb_Tf;w%u3&~Kb@0f5-ocWB#Y;N0fGAA zsdE-|sXt3P{|ZJd-nR8oo0sl*;9v@nOoIwaHWs}Q*<8R(_kL}tJ=BR!I8M(!DI-CQ zrmnD&4miFW!19U;#CWeFn*dBYfhy1oEh%GrId+jNer_KXu7JW>OYcEEy0cg9S0T^s%)s04+YrID);41A^Z3}+ATQ4VXgb_MF#tWvmqwATtsQGq zSf}dm=J25NU#r^}>!#WsCCn}s9;v)RWmSwS7b0~nP{;)+Q1;(XNAck~V@W#*DHpz4 zJP8f5!@E@MSknhs(7=G`ePJ4Npip7{|7#BalpIVQ;yC_Ym3YP&;~L#jnx4G)eGFOg zm8xuwwCVf*XcmJss|dsVZq8sMGYO{??w+#HFbo)$}`k zKA|k1^uNzkqyFm@H3&2d3j(=Wx&Q9Nar@oNrmh49jSIs4yUj%U-=(78XRiO}T0nvX z>l0D_Hvd0WBM^x2e-I!L2#pTXu2265=-+aRe-A*`{K*Dve?-A)KmuLF4}nr8hu9k+ z|07!Bhd3MJL)26e(f(xjcRqgOaReZ)s^pMI1LXg5L;4?XlmsA9syL8NRkZ(QWz~=b zI!plKY)Joy$KS&a0SJ*ADf55k;n+a diff --git a/apps/commander/commander.c b/apps/commander/commander.c index 6b026f287e..8b9e7c49cd 100644 --- a/apps/commander/commander.c +++ b/apps/commander/commander.c @@ -1785,103 +1785,90 @@ int commander_thread_main(int argc, char *argv[]) if ((hrt_absolute_time() - sp_man.timestamp) < 100000) { - /* - * Check if manual control modes have to be switched - */ - if (!isfinite(sp_man.mode_switch)) { - warnx("mode sw not finite"); + /* + * Check if manual control modes have to be switched + */ + if (!isfinite(sp_man.mode_switch)) { + warnx("mode sw not finite"); - /* this switch is not properly mapped, set attitude stabilized */ - if ((current_status.system_type == VEHICLE_TYPE_QUADROTOR) || - (current_status.system_type == VEHICLE_TYPE_HEXAROTOR) || - (current_status.system_type == VEHICLE_TYPE_OCTOROTOR)) { + /* no valid stick position, go to default */ - current_status.flag_control_attitude_enabled = true; - current_status.flag_control_rates_enabled = true; - } else { - /* assuming a fixed wing, fall back to direct pass-through */ - current_status.flag_control_attitude_enabled = false; - current_status.flag_control_rates_enabled = false; - } + } else if (sp_man.mode_switch < -STICK_ON_OFF_LIMIT) { - } else if (sp_man.mode_switch < -STICK_ON_OFF_LIMIT) { + /* bottom stick position, go to manual mode */ + current_status.mode_switch = MODE_SWITCH_MANUAL; - /* this switch is not properly mapped, set attitude stabilized */ - if ((current_status.system_type == VEHICLE_TYPE_QUADROTOR) || - (current_status.system_type == VEHICLE_TYPE_HEXAROTOR) || - (current_status.system_type == VEHICLE_TYPE_OCTOROTOR)) { + } else if (sp_man.mode_switch > STICK_ON_OFF_LIMIT) { - /* assuming a rotary wing, fall back to m */ - current_status.flag_control_attitude_enabled = true; - current_status.flag_control_rates_enabled = true; - } else { + /* top stick position, set auto/mission for all vehicle types */ + current_status.mode_switch = MODE_SWITCH_AUTO; - /* assuming a fixed wing, set to direct pass-through as requested */ - current_status.flag_control_attitude_enabled = false; - current_status.flag_control_rates_enabled = false; - } - - } else if (sp_man.mode_switch > STICK_ON_OFF_LIMIT) { - - /* top stick position, set auto/mission for all vehicle types */ - current_status.flag_control_position_enabled = true; - current_status.flag_control_velocity_enabled = true; - current_status.flag_control_attitude_enabled = true; - current_status.flag_control_rates_enabled = true; - - } else { - /* center stick position, set seatbelt/simple control */ - current_status.flag_control_velocity_enabled = true; - current_status.flag_control_attitude_enabled = true; - current_status.flag_control_rates_enabled = true; - } + } else { + /* center stick position, set seatbelt/simple control */ + current_status.mode_switch = MODE_SWITCH_SEATBELT; + } // warnx("man ctrl mode: %d\n", (int)current_status.manual_control_mode); /* - * Check if land/RTL is requested - */ + * Check if land/RTL is requested + */ if (!isfinite(sp_man.return_switch)) { /* this switch is not properly mapped, set default */ - current_status.flag_land_requested = false; // XXX default? + current_status.return_switch = RETURN_SWITCH_NONE; } else if (sp_man.return_switch < -STICK_ON_OFF_LIMIT) { /* bottom stick position, set altitude hold */ - current_status.flag_land_requested = false; + current_status.return_switch = RETURN_SWITCH_NONE; } else if (sp_man.return_switch > STICK_ON_OFF_LIMIT) { /* top stick position */ - current_status.flag_land_requested = true; + current_status.return_switch = RETURN_SWITCH_RETURN; } else { /* center stick position, set default */ - current_status.flag_land_requested = false; // XXX default? + current_status.return_switch = RETURN_SWITCH_NONE; } /* check mission switch */ if (!isfinite(sp_man.mission_switch)) { /* this switch is not properly mapped, set default */ - current_status.flag_mission_activated = false; + current_status.mission_switch = MISSION_SWITCH_NONE; } else if (sp_man.mission_switch > STICK_ON_OFF_LIMIT) { /* top switch position */ - current_status.flag_mission_activated = true; + current_status.mission_switch = MISSION_SWITCH_MISSION; } else if (sp_man.mission_switch < -STICK_ON_OFF_LIMIT) { /* bottom switch position */ - current_status.flag_mission_activated = false; + current_status.mission_switch = MISSION_SWITCH_NONE; } else { /* center switch position, set default */ - current_status.flag_mission_activated = false; // XXX default? + current_status.mission_switch = MISSION_SWITCH_NONE; // XXX default? + } + + /* Now it's time to handle the stick inputs */ + + if (current_status.arming_state == ARMING_STATE_ARMED) { + + if (current_status.mode_switch == MODE_SWITCH_MANUAL) { + do_navigation_state_update(stat_pub, ¤t_status, mavlink_fd, NAVIGATION_STATE_MANUAL ); + } else if (current_status.mode_switch == MODE_SWITCH_SEATBELT) { + do_navigation_state_update(stat_pub, ¤t_status, mavlink_fd, NAVIGATION_STATE_SEATBELT ); + } else if (current_status.mode_switch == MODE_SWITCH_AUTO) { + if (current_status.navigation_state == NAVIGATION_STATE_MANUAL) { + do_navigation_state_update(stat_pub, ¤t_status, mavlink_fd, NAVIGATION_STATE_MISSION ); + } + } } /* handle the case where RC signal was regained */ @@ -1890,17 +1877,19 @@ int commander_thread_main(int argc, char *argv[]) mavlink_log_critical(mavlink_fd, "DETECTED RC SIGNAL FIRST TIME."); } else { - if (current_status.rc_signal_lost) mavlink_log_critical(mavlink_fd, "[cmd] RECOVERY - RC SIGNAL GAINED!"); + if (current_status.rc_signal_lost) { + mavlink_log_critical(mavlink_fd, "[cmd] RECOVERY - RC SIGNAL GAINED!"); + } } /* - * Check if left stick is in lower left position --> switch to standby state. - * Do this only for multirotors, not for fixed wing aircraft. - */ -// if (((current_status.system_type == VEHICLE_TYPE_QUADROTOR) || -// (current_status.system_type == VEHICLE_TYPE_HEXAROTOR) || -// (current_status.system_type == VEHICLE_TYPE_OCTOROTOR) -// ) && + * Check if left stick is in lower left position --> switch to standby state. + * Do this only for multirotors, not for fixed wing aircraft. + */ + // if (((current_status.system_type == VEHICLE_TYPE_QUADROTOR) || + // (current_status.system_type == VEHICLE_TYPE_HEXAROTOR) || + // (current_status.system_type == VEHICLE_TYPE_OCTOROTOR) + // ) && if ((sp_man.yaw < -STICK_ON_OFF_LIMIT) && (sp_man.throttle < STICK_THRUST_RANGE * 0.2f)) { if (stick_off_counter > STICK_ON_OFF_COUNTER_LIMIT) { do_arming_state_update(stat_pub, ¤t_status, mavlink_fd, ARMING_STATE_STANDBY); @@ -1969,8 +1958,8 @@ int commander_thread_main(int argc, char *argv[]) /* decide about attitude control flag, enable in att/pos/vel */ bool attitude_ctrl_enabled = (sp_offboard.mode == OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE || - sp_offboard.mode == OFFBOARD_CONTROL_MODE_DIRECT_VELOCITY || - sp_offboard.mode == OFFBOARD_CONTROL_MODE_DIRECT_POSITION); + sp_offboard.mode == OFFBOARD_CONTROL_MODE_DIRECT_VELOCITY || + sp_offboard.mode == OFFBOARD_CONTROL_MODE_DIRECT_POSITION); /* decide about rate control flag, enable it always XXX (for now) */ bool rates_ctrl_enabled = true; diff --git a/apps/commander/state_machine_helper.c b/apps/commander/state_machine_helper.c index aae119d35a..2e49354714 100644 --- a/apps/commander/state_machine_helper.c +++ b/apps/commander/state_machine_helper.c @@ -54,421 +54,329 @@ /** - * Transition from one navigation state to another + * Transition from one sytem state to another */ -int do_navigation_state_update(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, navigation_state_t new_state) +void state_machine_update(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd) { - bool valid_path = false; - bool valid_transition = false; + int ret = ERROR; + system_state_t new_system_state; + + + /* + * Firstly evaluate mode switch position + * */ + + /* Always accept manual mode */ + if (current_status->mode_switch == MODE_SWITCH_MANUAL) { + new_system_state = SYSTEM_STATE_MANUAL; + + /* Accept SEATBELT if there is a local position estimate */ + } else if (current_status->mode_switch == MODE_SWITCH_SEATBELT) { + + if (current_status->flag_local_position_valid) { + new_system_state = SYSTEM_STATE_SEATBELT; + } else { + /* or just fall back to manual */ + mavlink_log_critical(mavlink_fd, "REJ SEATBELT: no local position"); + new_system_state = SYSTEM_STATE_MANUAL); + } + + /* Accept AUTO if there is a global position estimate */ + } else if (current_status->mode_switch == MODE_SWITCH_AUTO) { + if (current_status->flag_local_position_valid) { + new_system_state = SYSTEM_STATE_SEATBELT); + } else { + mavlink_log_critical(mavlink_fd, "REJ AUTO: no global position"); + + /* otherwise fallback to seatbelt or even manual */ + if (current_status->flag_local_position_valid) { + new_system_state = SYSTEM_STATE_SEATBELT; + } else { + mavlink_log_critical(mavlink_fd, "REJ SEATBELT: no local position"); + new_system_state = SYSTEM_STATE_MANUAL; + } + } + } + + /* + * Next up, check for + */ + + + + /* Update the system state in case it has changed */ + if (current_status->system_state != new_system_state) { + + /* Check if the transition is valid */ + if (system_state_update(current_status->system_state, new_system_state) == OK) { + current_status->system_state = new_system_state; + state_machine_publish(status_pub, current_status, mavlink_fd); + } else { + mavlink_log_critical(mavlink_fd, "System state transition not valid"); + } + } + + return; +} + +/* + * This functions does not evaluate any input flags but only checks if the transitions + * are valid. + */ +int system_state_update(system_state_t current_system_state, system_state_t new_system_state) { + int ret = ERROR; - if (current_status->navigation_state == new_state) { - warnx("Navigation state not changed"); + /* only check transition if the new state is actually different from the current one */ + if (new_system_state != current_system_state) { - } else { + switch (new_system_state) { + case SYSTEM_STATE_INIT: - switch (new_state) { - case NAVIGATION_STATE_STANDBY: + /* transitions back to INIT are possible for calibration */ + if (current_system_state == SYSTEM_STATE_MANUAL + || current_system_state == SYSTEM_STATE_SEATBELT + || current_system_state == SYSTEM_STATE_AUTO) { - if (current_status->navigation_state == NAVIGATION_STATE_MANUAL - || current_status->navigation_state == NAVIGATION_STATE_AUTO_READY) { + ret = OK; + } - if (!current_status->flag_system_armed) { - mavlink_log_critical(mavlink_fd, "Switched to STANDBY state"); - valid_transition = true; - } else { - mavlink_log_critical(mavlink_fd, "Rejected STANDBY state: armed"); - } - valid_path = true; + break; + + case SYSTEM_STATE_MANUAL: + + /* transitions from INIT or from other modes */ + if (current_system_state == SYSTEM_STATE_INIT + || current_system_state == SYSTEM_STATE_SEATBELT + || current_system_state == SYSTEM_STATE_AUTO) { + + ret = OK; + } + + break; + + case SYSTEM_STATE_SEATBELT: + + /* transitions from INIT or from other modes */ + if (current_system_state == SYSTEM_STATE_INIT + || current_system_state == SYSTEM_STATE_MANUAL + || current_system_state == SYSTEM_STATE_AUTO) { + + ret = OK; + } + + break; + + case SYSTEM_STATE_AUTO: + + /* transitions from INIT or from other modes */ + if (current_system_state == SYSTEM_STATE_INIT + || current_system_state == SYSTEM_STATE_MANUAL + || current_system_state == SYSTEM_STATE_SEATBELT) { + + ret = OK; } break; - case NAVIGATION_STATE_MANUAL: + case SYSTEM_STATE_REBOOT: - if (current_status->navigation_state == NAVIGATION_STATE_STANDBY) { + /* from INIT you can go straight to REBOOT */ + if (current_system_state == SYSTEM_STATE_INIT) { - /* only check for armed flag when coming from STANDBY XXX does that make sense? */ - if (current_status->flag_system_armed) { - mavlink_log_critical(mavlink_fd, "Switched to MANUAL state"); - valid_transition = true; - } else { - mavlink_log_critical(mavlink_fd, "Rejected MANUAL state: disarmed"); - } - valid_path = true; - } else if (current_status->navigation_state == NAVIGATION_STATE_SEATBELT - || current_status->navigation_state == NAVIGATION_STATE_LOITER - || current_status->navigation_state == NAVIGATION_STATE_MISSION - || current_status->navigation_state == NAVIGATION_STATE_RTL - || current_status->navigation_state == NAVIGATION_STATE_LAND - || current_status->navigation_state == NAVIGATION_STATE_TAKEOFF - || current_status->navigation_state == NAVIGATION_STATE_AUTO_READY) { - mavlink_log_critical(mavlink_fd, "Switched to MANUAL state"); - valid_transition = true; - valid_path = true; + ret = OK; } break; - case NAVIGATION_STATE_SEATBELT: + case SYSTEM_STATE_IN_AIR_RESTORE: - if (current_status->navigation_state == NAVIGATION_STATE_MANUAL - || current_status->navigation_state == NAVIGATION_STATE_LOITER - || current_status->navigation_state == NAVIGATION_STATE_MISSION) { + /* from INIT you can go straight to an IN AIR RESTORE */ + if (current_system_state == SYSTEM_STATE_INIT) { - mavlink_log_critical(mavlink_fd, "Switched to SEATBELT state"); - valid_transition = true; - valid_path = true; - } - break; - case NAVIGATION_STATE_DESCENT: - - if (current_status->navigation_state == NAVIGATION_STATE_MANUAL - || current_status->navigation_state == NAVIGATION_STATE_SEATBELT) { - - mavlink_log_critical(mavlink_fd, "Switched to DESCENT state"); - valid_transition = true; - valid_path = true; - } - break; - - case NAVIGATION_STATE_LOITER: - - /* Check for position lock when coming from MANUAL or SEATBELT */ - if (current_status->navigation_state == NAVIGATION_STATE_MANUAL || current_status->navigation_state == NAVIGATION_STATE_SEATBELT) { - mavlink_log_critical(mavlink_fd, "Switched to LOITER state"); - valid_transition = true; - if (current_status->flag_global_position_valid) { - mavlink_log_critical(mavlink_fd, "Switched to LOITER state"); - valid_transition = true; - - } else { - mavlink_log_critical(mavlink_fd, "Rejected LOITER state: no pos lock"); - } - valid_path = true; - } else if (current_status->navigation_state == NAVIGATION_STATE_MISSION) { - mavlink_log_critical(mavlink_fd, "Switched to LOITER state"); - valid_transition = true; - valid_path = true; - } - break; - - case NAVIGATION_STATE_AUTO_READY: - - /* coming from STANDBY pos and home lock are needed */ - if (current_status->navigation_state == NAVIGATION_STATE_STANDBY) { - - if (!current_status->flag_system_armed) { - mavlink_log_critical(mavlink_fd, "Rejected AUTO READY state: disarmed"); - - } else if (!current_status->flag_global_position_valid) { - mavlink_log_critical(mavlink_fd, "Rejected AUTO READY state: no pos lock"); - - } else if (!current_status->flag_valid_launch_position) { - mavlink_log_critical(mavlink_fd, "Rejected AUTO READY state: no home lock"); - - } else { - mavlink_log_critical(mavlink_fd, "Switched to AUTO READY state"); - valid_transition = true; - } - valid_path = true; - /* coming from LAND home lock is needed */ - } else if (current_status->navigation_state == NAVIGATION_STATE_LAND) { - - if (!current_status->flag_valid_launch_position) { - mavlink_log_critical(mavlink_fd, "Rejected AUTO READY state: no home lock"); - } else { - mavlink_log_critical(mavlink_fd, "Switched to AUTO READY state"); - valid_transition = true; - } - valid_path = true; - } - break; - - case NAVIGATION_STATE_MISSION: - - /* coming from TAKEOFF or RTL is always possible */ - if (current_status->navigation_state == NAVIGATION_STATE_TAKEOFF - || current_status->navigation_state == NAVIGATION_STATE_RTL) { - mavlink_log_critical(mavlink_fd, "Switched to MISSION state"); - valid_transition = true; - valid_path = true; - - /* coming from MANUAL or SEATBELT requires home and pos lock */ - } else if (current_status->navigation_state == NAVIGATION_STATE_MANUAL - || current_status->navigation_state == NAVIGATION_STATE_SEATBELT) { - - if (!current_status->flag_global_position_valid) { - mavlink_log_critical(mavlink_fd, "Rejected MISSION state: no pos lock"); - - } else if (!current_status->flag_valid_launch_position) { - mavlink_log_critical(mavlink_fd, "Rejected MISSION state: no home lock"); - mavlink_log_critical(mavlink_fd, "Switched to MISSION state"); - valid_transition = true; - } else { - - } - valid_path = true; - - /* coming from loiter a home lock is needed */ - } else if (current_status->navigation_state == NAVIGATION_STATE_LOITER) { - if (current_status->flag_valid_launch_position) { - mavlink_log_critical(mavlink_fd, "Switched to MISSION state"); - valid_transition = true; - } else { - mavlink_log_critical(mavlink_fd, "Rejected MISSION state: no home lock"); - } - valid_path = true; - } - break; - - case NAVIGATION_STATE_RTL: - - /* coming from MISSION is always possible */ - if (current_status->navigation_state == NAVIGATION_STATE_MISSION) { - mavlink_log_critical(mavlink_fd, "Switched to RTL state"); - valid_transition = true; - valid_path = true; - - /* coming from MANUAL or SEATBELT requires home and pos lock */ - } else if (current_status->navigation_state == NAVIGATION_STATE_MANUAL - || current_status->navigation_state == NAVIGATION_STATE_SEATBELT) { - - if (!current_status->flag_global_position_valid) { - mavlink_log_critical(mavlink_fd, "Rejected RTL state: no pos lock"); - } else if (!current_status->flag_valid_launch_position) { - mavlink_log_critical(mavlink_fd, "Rejected RTL state: no home lock"); - } else { - mavlink_log_critical(mavlink_fd, "Switched to RTL state"); - valid_transition = true; - } - valid_path = true; - - /* coming from loiter a home lock is needed */ - } else if (current_status->navigation_state == NAVIGATION_STATE_LOITER) { - if (current_status->flag_valid_launch_position) { - mavlink_log_critical(mavlink_fd, "Switched to RTL state"); - valid_transition = true; - } else { - mavlink_log_critical(mavlink_fd, "Rejected RTL state: no home lock"); - } - valid_path = true; - } - break; - - case NAVIGATION_STATE_TAKEOFF: - - if (current_status->navigation_state == NAVIGATION_STATE_AUTO_READY) { - - /* TAKEOFF is straight forward from AUTO READY */ - mavlink_log_critical(mavlink_fd, "Switched to TAKEOFF state"); - valid_transition = true; - valid_path = true; - } - break; - - case NAVIGATION_STATE_LAND: - if (current_status->navigation_state == NAVIGATION_STATE_RTL - || current_status->navigation_state == NAVIGATION_STATE_LOITER - || current_status->navigation_state == NAVIGATION_STATE_MISSION) { - - mavlink_log_critical(mavlink_fd, "Switched to LAND state"); - valid_transition = true; - valid_path = true; + ret = OK; } break; default: - warnx("Unknown navigation state"); break; } } - if (valid_transition) { - current_status->navigation_state = new_state; - state_machine_publish(status_pub, current_status, mavlink_fd); -// publish_armed_status(current_status); - ret = OK; - } - - if (!valid_path){ - mavlink_log_critical(mavlink_fd, "REJECTING invalid navigation state transition"); - } - return ret; } -/** - * Transition from one arming state to another - */ -int do_arming_state_update(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, arming_state_t new_state) -{ - bool valid_transition = false; - int ret = ERROR; +///** +// * Transition from one arming state to another +// */ +//int do_arming_state_update(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, arming_state_t new_state) +//{ +// bool valid_transition = false; +// int ret = ERROR; +// +// warnx("Current state: %d, requested state: %d", current_status->arming_state, new_state); +// +// if (current_status->arming_state == new_state) { +// warnx("Arming state not changed"); +// valid_transition = true; +// +// } else { +// +// switch (new_state) { +// +// case ARMING_STATE_INIT: +// +// if (current_status->arming_state == ARMING_STATE_STANDBY) { +// +// mavlink_log_critical(mavlink_fd, "Switched to INIT ARMING STATE"); +// valid_transition = true; +// } +// break; +// +// case ARMING_STATE_STANDBY: +// +// // TODO check for sensors +// // XXX check if coming from reboot? +// if (current_status->arming_state == ARMING_STATE_INIT) { +// +// if (current_status->flag_system_sensors_initialized) { +// current_status->flag_system_armed = false; +// mavlink_log_critical(mavlink_fd, "Switched to STANDBY arming state"); +// valid_transition = true; +// } else { +// mavlink_log_critical(mavlink_fd, "REJ. STANDBY arming state, sensors not init."); +// } +// +// } else if (current_status->arming_state == ARMING_STATE_ARMED) { +// +// current_status->flag_system_armed = false; +// mavlink_log_critical(mavlink_fd, "Switched to STANDBY arming state"); +// valid_transition = true; +// } +// break; +// +// case ARMING_STATE_ARMED: +// +// if (current_status->arming_state == ARMING_STATE_STANDBY +// || current_status->arming_state == ARMING_STATE_IN_AIR_RESTORE) { +// current_status->flag_system_armed = true; +// mavlink_log_critical(mavlink_fd, "Switched to ARMED arming state"); +// valid_transition = true; +// } +// break; +// +// case ARMING_STATE_ABORT: +// +// if (current_status->arming_state == ARMING_STATE_ARMED) { +// +// mavlink_log_critical(mavlink_fd, "Switched to MISSION ABORT arming state"); +// valid_transition = true; +// } +// break; +// +// case ARMING_STATE_ERROR: +// +// if (current_status->arming_state == ARMING_STATE_ARMED +// || current_status->arming_state == ARMING_STATE_ABORT +// || current_status->arming_state == ARMING_STATE_INIT) { +// +// mavlink_log_critical(mavlink_fd, "Switched to ERROR arming state"); +// valid_transition = true; +// } +// break; +// +// case ARMING_STATE_REBOOT: +// +// if (current_status->arming_state == ARMING_STATE_STANDBY +// || current_status->arming_state == ARMING_STATE_ERROR) { +// +// valid_transition = true; +// mavlink_log_critical(mavlink_fd, "REBOOTING SYSTEM"); +// usleep(500000); +// up_systemreset(); +// /* SPECIAL CASE: NEVER RETURNS FROM THIS FUNCTION CALL */ +// } +// break; +// +// case ARMING_STATE_IN_AIR_RESTORE: +// +// if (current_status->arming_state == ARMING_STATE_INIT) { +// mavlink_log_critical(mavlink_fd, "Switched to IN-AIR-RESTORE arming state"); +// valid_transition = true; +// } +// break; +// default: +// warnx("Unknown arming state"); +// break; +// } +// } +// +// if (valid_transition) { +// current_status->arming_state = new_state; +// state_machine_publish(status_pub, current_status, mavlink_fd); +//// publish_armed_status(current_status); +// ret = OK; +// } else { +// mavlink_log_critical(mavlink_fd, "REJECTING invalid arming state transition"); +// } +// +// return ret; +//} - warnx("Current state: %d, requested state: %d", current_status->arming_state, new_state); - - if (current_status->arming_state == new_state) { - warnx("Arming state not changed"); - valid_transition = true; - - } else { - - switch (new_state) { - - case ARMING_STATE_INIT: - - if (current_status->arming_state == ARMING_STATE_STANDBY) { - - mavlink_log_critical(mavlink_fd, "Switched to INIT ARMING STATE"); - valid_transition = true; - } - break; - - case ARMING_STATE_STANDBY: - - // TODO check for sensors - // XXX check if coming from reboot? - if (current_status->arming_state == ARMING_STATE_INIT) { - - if (current_status->flag_system_sensors_initialized) { - current_status->flag_system_armed = false; - mavlink_log_critical(mavlink_fd, "Switched to STANDBY arming state"); - valid_transition = true; - } else { - mavlink_log_critical(mavlink_fd, "REJ. STANDBY arming state, sensors not init."); - } - - } else if (current_status->arming_state == ARMING_STATE_ARMED) { - - current_status->flag_system_armed = false; - mavlink_log_critical(mavlink_fd, "Switched to STANDBY arming state"); - valid_transition = true; - } - break; - - case ARMING_STATE_ARMED: - - if (current_status->arming_state == ARMING_STATE_STANDBY - || current_status->arming_state == ARMING_STATE_IN_AIR_RESTORE) { - current_status->flag_system_armed = true; - mavlink_log_critical(mavlink_fd, "Switched to ARMED arming state"); - valid_transition = true; - } - break; - - case ARMING_STATE_ABORT: - - if (current_status->arming_state == ARMING_STATE_ARMED) { - - mavlink_log_critical(mavlink_fd, "Switched to MISSION ABORT arming state"); - valid_transition = true; - } - break; - - case ARMING_STATE_ERROR: - - if (current_status->arming_state == ARMING_STATE_ARMED - || current_status->arming_state == ARMING_STATE_ABORT - || current_status->arming_state == ARMING_STATE_INIT) { - - mavlink_log_critical(mavlink_fd, "Switched to ERROR arming state"); - valid_transition = true; - } - break; - - case ARMING_STATE_REBOOT: - - if (current_status->arming_state == ARMING_STATE_STANDBY - || current_status->arming_state == ARMING_STATE_ERROR) { - - valid_transition = true; - mavlink_log_critical(mavlink_fd, "REBOOTING SYSTEM"); - usleep(500000); - up_systemreset(); - /* SPECIAL CASE: NEVER RETURNS FROM THIS FUNCTION CALL */ - } - break; - - case ARMING_STATE_IN_AIR_RESTORE: - - if (current_status->arming_state == ARMING_STATE_INIT) { - mavlink_log_critical(mavlink_fd, "Switched to IN-AIR-RESTORE arming state"); - valid_transition = true; - } - break; - default: - warnx("Unknown arming state"); - break; - } - } - - if (valid_transition) { - current_status->arming_state = new_state; - state_machine_publish(status_pub, current_status, mavlink_fd); -// publish_armed_status(current_status); - ret = OK; - } else { - mavlink_log_critical(mavlink_fd, "REJECTING invalid arming state transition"); - } - - return ret; -} - -/** - * Transition from one hil state to another - */ -int do_hil_state_update(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, hil_state_t new_state) -{ - bool valid_transition = false; - int ret = ERROR; - - warnx("Current state: %d, requested state: %d", current_status->hil_state, new_state); - - if (current_status->hil_state == new_state) { - warnx("Hil state not changed"); - valid_transition = true; - - } else { - - switch (new_state) { - - case HIL_STATE_OFF: - - if (current_status->arming_state == ARMING_STATE_INIT - || current_status->arming_state == ARMING_STATE_STANDBY) { - - current_status->flag_hil_enabled = false; - mavlink_log_critical(mavlink_fd, "Switched to OFF hil state"); - valid_transition = true; - } - break; - - case HIL_STATE_ON: - - if (current_status->arming_state == ARMING_STATE_INIT - || current_status->arming_state == ARMING_STATE_STANDBY) { - - current_status->flag_hil_enabled = true; - mavlink_log_critical(mavlink_fd, "Switched to ON hil state"); - valid_transition = true; - } - break; - - default: - warnx("Unknown hil state"); - break; - } - } - - if (valid_transition) { - current_status->hil_state = new_state; - state_machine_publish(status_pub, current_status, mavlink_fd); - ret = OK; - } else { - mavlink_log_critical(mavlink_fd, "REJECTING invalid hil state transition"); - } - - return ret; -} +///** +// * Transition from one hil state to another +// */ +//int do_hil_state_update(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, hil_state_t new_state) +//{ +// bool valid_transition = false; +// int ret = ERROR; +// +// warnx("Current state: %d, requested state: %d", current_status->hil_state, new_state); +// +// if (current_status->hil_state == new_state) { +// warnx("Hil state not changed"); +// valid_transition = true; +// +// } else { +// +// switch (new_state) { +// +// case HIL_STATE_OFF: +// +// if (current_status->arming_state == ARMING_STATE_INIT +// || current_status->arming_state == ARMING_STATE_STANDBY) { +// +// current_status->flag_hil_enabled = false; +// mavlink_log_critical(mavlink_fd, "Switched to OFF hil state"); +// valid_transition = true; +// } +// break; +// +// case HIL_STATE_ON: +// +// if (current_status->arming_state == ARMING_STATE_INIT +// || current_status->arming_state == ARMING_STATE_STANDBY) { +// +// current_status->flag_hil_enabled = true; +// mavlink_log_critical(mavlink_fd, "Switched to ON hil state"); +// valid_transition = true; +// } +// break; +// +// default: +// warnx("Unknown hil state"); +// break; +// } +// } +// +// if (valid_transition) { +// current_status->hil_state = new_state; +// state_machine_publish(status_pub, current_status, mavlink_fd); +// ret = OK; +// } else { +// mavlink_log_critical(mavlink_fd, "REJECTING invalid hil state transition"); +// } +// +// return ret; +//} diff --git a/apps/commander/state_machine_helper.h b/apps/commander/state_machine_helper.h index bf9296caf0..57b3db8f19 100644 --- a/apps/commander/state_machine_helper.h +++ b/apps/commander/state_machine_helper.h @@ -47,9 +47,9 @@ #include #include -int do_navigation_state_update(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, navigation_state_t new_state); +int navigation_state_update(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, navigation_state_t new_state); -int do_arming_state_update(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, arming_state_t new_state); +//int do_arming_state_update(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, arming_state_t new_state); void state_machine_publish(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd); diff --git a/apps/uORB/topics/vehicle_status.h b/apps/uORB/topics/vehicle_status.h index 27a471f131..20cb25cc02 100644 --- a/apps/uORB/topics/vehicle_status.h +++ b/apps/uORB/topics/vehicle_status.h @@ -60,27 +60,48 @@ /* State Machine */ typedef enum { - NAVIGATION_STATE_STANDBY=0, - NAVIGATION_STATE_MANUAL, - NAVIGATION_STATE_SEATBELT, - NAVIGATION_STATE_DESCENT, - NAVIGATION_STATE_LOITER, - NAVIGATION_STATE_AUTO_READY, - NAVIGATION_STATE_MISSION, - NAVIGATION_STATE_RTL, - NAVIGATION_STATE_LAND, - NAVIGATION_STATE_TAKEOFF -} navigation_state_t; + SYSTEM_STATE_INIT=0, + SYSTEM_STATE_MANUAL, + SYSTEM_STATE_SEATBELT, + SYSTEM_STATE_AUTO, + SYSTEM_STATE_REBOOT, + SYSTEM_STATE_IN_AIR_RESTORE +} system_state_t; typedef enum { - ARMING_STATE_INIT = 0, - ARMING_STATE_STANDBY, - ARMING_STATE_ARMED, - ARMING_STATE_ABORT, - ARMING_STATE_ERROR, - ARMING_STATE_REBOOT, - ARMING_STATE_IN_AIR_RESTORE -} arming_state_t; + MANUAL_STANDBY = 0, + MANUAL_READY, + MANUAL_IN_AIR +} manual_state_t; + +typedef enum { + SEATBELT_STANDBY, + SEATBELT_READY, + SEATBELT, + SEATBELT_ASCENT, + SEATBELT_DESCENT, +} seatbelt_state_t; + +typedef enum { + AUTO_STANDBY, + AUTO_READY, + AUTO_LOITER, + AUTO_MISSION, + AUTO_RTL, + AUTO_TAKEOFF, + AUTO_LAND, +} auto_state_t; + +//typedef enum { +// ARMING_STATE_INIT = 0, +// ARMING_STATE_STANDBY, +// ARMING_STATE_ARMED_GROUND, +// ARMING_STATE_ARMED_AIRBORNE, +// ARMING_STATE_ERROR_GROUND, +// ARMING_STATE_ERROR_AIRBORNE, +// ARMING_STATE_REBOOT, +// ARMING_STATE_IN_AIR_RESTORE +//} arming_state_t; typedef enum { HIL_STATE_OFF = 0, @@ -100,7 +121,7 @@ enum VEHICLE_MODE_FLAG { typedef enum { MODE_SWITCH_MANUAL = 0, - MODE_SWITCH_ASSISTED, + MODE_SWITCH_SEATBELT, MODE_SWITCH_AUTO } mode_switch_pos_t; @@ -114,26 +135,6 @@ typedef enum { MISSION_SWITCH_MISSION } mission_switch_pos_t; -//enum VEHICLE_FLIGHT_MODE { -// VEHICLE_FLIGHT_MODE_MANUAL = 0, /**< direct manual control, exact mode determined by VEHICLE_MANUAL_CONTROL_MODE */ -// VEHICLE_FLIGHT_MODE_STAB, /**< attitude or rate stabilization plus velocity or position stabilization */ -// VEHICLE_FLIGHT_MODE_HOLD, /**< hold current position (hover or loiter around position when switched) */ -// VEHICLE_FLIGHT_MODE_AUTO /**< attitude or rate stabilization plus absolute position control and waypoints */ -//}; -// -//enum VEHICLE_MANUAL_CONTROL_MODE { -// VEHICLE_MANUAL_CONTROL_MODE_DIRECT = 0, /**< no attitude control, direct stick input mixing (only fixed wing) */ -// VEHICLE_MANUAL_CONTROL_MODE_RATES, /**< body rates control mode */ -// VEHICLE_MANUAL_CONTROL_MODE_SAS /**< stability augmented system (SAS) mode */ -//}; -// -//enum VEHICLE_MANUAL_SAS_MODE { -// VEHICLE_MANUAL_SAS_MODE_ROLL_PITCH_ABS_YAW_ABS = 0, /**< roll, pitch and yaw absolute */ -// VEHICLE_MANUAL_SAS_MODE_ROLL_PITCH_ABS_YAW_RATE, /**< roll and pitch absolute, yaw rate */ -// VEHICLE_MANUAL_SAS_MODE_SIMPLE, /**< simple mode (includes altitude hold) */ -// VEHICLE_MANUAL_SAS_MODE_ALTITUDE /**< altitude hold */ -//}; - /** * Should match 1:1 MAVLink's MAV_TYPE ENUM */ @@ -180,8 +181,8 @@ struct vehicle_status_s uint64_t failsave_lowlevel_start_time; /**< time when the lowlevel failsafe flag was set */ // uint64_t failsave_highlevel_begin; TO BE COMPLETED - navigation_state_t navigation_state; /**< current navigation state */ - arming_state_t arming_state; /**< current arming state */ + system_state_t system_state; /**< current system state */ +// arming_state_t arming_state; /**< current arming state */ hil_state_t hil_state; /**< current hil state */ int32_t system_type; /**< system type, inspired by MAVLink's VEHICLE_TYPE enum */ @@ -194,6 +195,7 @@ struct vehicle_status_s return_switch_pos_t return_switch; mission_switch_pos_t mission_switch; + bool flag_system_armed; /**< true is motors / actuators are armed */ bool flag_system_emergency; bool flag_system_in_air_restore; /**< true if we can restore in mid air */ @@ -212,9 +214,6 @@ struct vehicle_status_s bool flag_control_velocity_enabled; /**< true if speed (implies direction) is controlled */ bool flag_control_position_enabled; /**< true if position is controlled */ - bool flag_land_requested; /**< true if land requested */ - bool flag_mission_activated; /**< true if in mission mode */ - bool flag_preflight_gyro_calibration; /**< true if gyro calibration is requested */ bool flag_preflight_mag_calibration; /**< true if mag calibration is requested */ bool flag_preflight_accel_calibration; From ebe0285ce7964ac1a81a65bae417e978cf366466 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Thu, 21 Feb 2013 13:06:56 -0800 Subject: [PATCH 008/109] Checkpoint: navigation state machine as discussed with Lorenz --- Documentation/flight_mode_state_machine.odg | Bin 24059 -> 23463 bytes apps/commander/commander.c | 16 +- apps/commander/state_machine_helper.c | 397 +++++++++++++++----- apps/commander/state_machine_helper.h | 3 +- apps/controllib/fixedwing.cpp | 8 +- apps/mavlink/mavlink.c | 55 ++- apps/uORB/topics/vehicle_status.h | 86 ++--- 7 files changed, 393 insertions(+), 172 deletions(-) diff --git a/Documentation/flight_mode_state_machine.odg b/Documentation/flight_mode_state_machine.odg index a50366bcb3e51c2c6571597de89d7b00d746b28d..cbdad91c9c0e539eb5b02b700e4b9d8eab5efd50 100644 GIT binary patch delta 18537 zcmb4}MNpm%5TrlcgG+FC2=2k%J-EBO!;8DS6WrZ{2KNv?+=9Eiv-R)pVQZ`Quyg6T z_f$_Gp4Tpj@^%OmWjQElED#7D1Y$_kNJ3GB`cDi90EL7c7Od{aAEci3Ms~D_I1Q?( z8XESkY1y(p5M*gH?Dc+rVTjaXr=H zP=Wca(Fe6|h{x$J+3(*GCJ@xz&N^G1|JW1oR&khpSumPUu2_T@Gh1^NW2Ca#aw$yY z%!V#}2KJT_p37nTYKY)N!5ma->9qcV6^wFtMM$ZC*&XoQag}35H=XN^NM+IV8aB6U z%LF{^t!LLetZc>993~qNYFz0`XUe|YI|w(#gE=B@AA`AijvO@ZmNTmYoU&gpipp!T z-fOJ|9@0)jENZ&Aunu`c>P~CzcP)RHKW>?z0GoAfekx4h^E+y&(pghsH=Ue?B9mE0 z-Q&?Eb>m3&TV=U3JZEl>M&9Z>BUlU%$IxPUyv3 z!W~XJylE0JSYgd9Y*u$n6O`6M%m{*()vru&u2w&bp5c_`;NYEnzIe|=fI#-pAkhCZ zA08h5Kl4RF|HlBxAP~sa+|A9}!P1r4%ieBZ=b!5i7h1rjVbb+op+70?O>q#yI$AQv zR8lCc%lW;+GEzlSE3wR%?Y?@mt%)E7!msXpV1Y#O^j=5L)>Zx5&bK?9#^N7n$>fgS zt-Dp8&;tF5H`bHB#2cE--Ad;q%+73u5FBF2$?zRj(#n9WQj(MpZ^rcgZh8j%rZ{<} zoeM^gUDeMh)~oxbgM%{**)~FN)6~9}?krVj@Z;g_^Yz;v-tc@qCq9ah|F3*}q(uDa z3iU0M&>UMXYp6X;KBnlV+@4Ni^gaZ897b$8nED^p6|_KXR#)-8YSnuacbge1OxC5oO7)9Hyl2bmI!Z>B?|JKI%|@AjC)>fzk1+bk zSS6y`@Hj;#B}^_ux~V?zoQ&3sS5CtU4%kz8Qi|A~d$HEYd$EbMrMmgk@;EsF_egBZ zKeIyqPUpN^hxo%2ze*eDL;F1=20VA=DO}zx{|uZS(!}|wyvd61AnucOi*!uG+@77w zE?J>s$B<(TcH=rk(WTAyZnef)&Yv+}SN}9tsyl0h5t1|$?pVr4qb@}Ls1@$D-xd4$ zxR;pX6&g;3Y7kXI`4T9H}>Xy)lN z_g(1CBfS`FAPh)K_5gd(EAey*1Wk8Z&XJW5>ui}2}>pF=P@ zGl3#$h(Y$0U?jQB$AI10h&4@c48FI_iv-%$_L0K``O*(9;N>GY;8DD|uCmU9ZNsnl~s@)#y)x01_#{gVP z>ox4vK83cPn=-`u78Xtge@~KKrmYgMrq)!>0s@ri@I}fzdmXUx*3W5!E};1O&tPK1 zjibLQ*5kHT4jq-tE|+fRO_7qG%G#e%c;9bgUw%eTeGSR!3F;oL_7Ho3MyT?#wdPHX z!QdbzA@ER%mLByvEKxQ?p=S82z{hlK?E3}Ke?n0Cs4(oYq?WV zL^3~$<5GaXac*=al?hnMWGyZ&X2W=tEQ!z5@QTSPW+sNxDe$YWz(~{vSP-R2OW)Hq zE?V1+`bw!AIgVx7d%YnCds0&QC0+?r)ghke%U@3Yi2R1U-g3@`P$zx0vjb>0jW@PK zXlIMV32{%CW4))-Pj;&&ZpDX_m2^(Z0POoLN2ql#@pOZcY)-c2=}>FT4j&E zqb&bUR}@cU&EgvvP1;!Gj>}EPYSaHDcbO(;W2S#K2XBVqSCdFO9;cj&MeYfKCHqA5Tqw3om0`Da3Fz)JrP?o4Ay#0MvhRw<;*yISma0nUMWoahGBX zX9%#;#N(SmP(EppGq5w?HgzLXFImV+@C}caM3w5J=?VP>edLpIo;Oxy}LR% z2E1JVd$;xn`ZA~gZPZ#Sq24Yq7u5dXf8q|Q{;}q{e}u}dl(W!v@7A^4tE)hAu;$sd z(sjS<*V=cY@Abg{slU&^wy|V>IQc%ly#RDQXXr1*T5TzHGh!uPy3@6>v~9-JQCxVYXpKR^6aKvOVm*7NK%nd{mf z(9*lD?Z3GTwmAOtD`#US!}YD{oGGVa?Y$HAcBAF=eYNR5S3> zk>$9J_0&n5xgki~9X_^Hj+$3`3XXuDYEA$vSn}S|sHvcPZb(S1hb;-pdy-(@+ByP3 zBAgXxruS>5eVd(6i+$G3J5+AZgtIcA-2g%AAEx<$!FplYH|nt5@9Uuy&G#-Sr^o-WKl*ZzF9H0;uxXN86hE>W z&IsGi5!*q`t?igE|GuSPQQ9v}4(nejzU#ZjeO604nQT2O+$Nk(?>O4#-uwO1^9vk* zZZAJ~aQQxFCgR&INe+J9`$eJ$RP3^}p>KV}Sd^l|L!T&Ablg5j)Z?O1Z=leU@?bd?dv+tju>)&TC{8!_~ zs((yX+9Qt-k1Lr^L6J$s@g@m27b^Qw)YE>J^#3|n?9`tLUh@=Q-+LBC{T{&=z0KCR zCS2fR{^{sTyUo8fB!~Wk`RIcDla%~xJ{W(v^Jktn{zQ^|e@&r976AaQclJ}y0TRRq zh|Qr7UrX0@Xn|edg!D1FzV_~sgumd+ZMvDqe>WuM38&}4$L+3P+B=`V{ba^{E#b_R z(A1CHX%kr4{aceRDoX)o5BAKTO5o|Db(BqD+~b=2$$CoQV(j_JLvVw+ta_f*Vei|a zVb%5pDRFJ~5)`>36u`>o1xhM_+4&po`<)cWo>USJl@#YN@M$dx|91hvPRVF+Uio$| z-HP%h=7(=b0RNnF$Y9Z<<FFRYN%RxP9w5 zk+H8Fj}R`j{?_;T3fQ~_qarBZEo}u%ySxxf+W3lQZ2EE^0RYVF&Qd+<<UrW0$N2$6SI(svCjq ze@X@EN3&0u7D~Du34X86FFZmYqy>rw5tq`j*WX@~_U{;QRDKJcmg{#WXe+=~Lk2%B z3T94GJAC4FP+VLg9)zUb7$Pk0+PlZxE=bkzc0y%X1Me*V?0a8s8@+b=>bBXVYsPrM z68G)Ds!=D)y+1k(YCLrBKzSozE#sK?;{_yLL#-YaBK}T*1{wsFdF7pq^yw^#_fp$c zp8%SEcX?mQ(A?KLP(fMf+xfdB^Pa3ha^rd*x3_yzpq_Wv*Yvp^c5XGnH$$#8W1_u& zhc}w)fY$w?yGmN{~5hOtnUx| zXnfc4+WcVk?G`s8#>tuX!L%!y@0#Df67*7lt8CB^rOw3JFX*v)#+{NiuBb`7eCjKt zOuyWcKt~tw^Ks%$8hCc;^r~@eBn$Zb^%-9b^f~3cEI`Mj2l3acC?e+4+sF2ZHsZ>y znY1p)6?woEl|@!mPD)ItkLUnJ2am!30HV_K9N9IW*6cqXrwa1(;;Qo)RSX@cE8qig zHwN*+hJ>xnvoWnO=C&4-VeSdrg;K-)1O3IXm$fhV)VAaO%PG_`xNdwp6aT3_X94)+C|Yl@hdto0%8x z`(HJBLWN$l?`DQu~9Nj`w%d8^XbqY{)NzGc-Cn)f9mcOBCWQ*3{_TvU(Ld5hxXQRwsL zpHKYr?{bI3p7NT6uHf(4liiuL<^=KWh4I{nTeV;;nw670ti5+jsi%-yEQ5ai;A;B` zKQ>e4uU2_Ct{UwVTPC?X-|!F5&8I9_Biniq^vp6B_G?b6_y(Pjse;fD+Ol>F&oh zGgMKcbf2%jd!_|mj@Qqu#JRA85uuvCTzqQ|gS_hGfbSh{(JTb{ZQr&PzeckhP|%1a zyU~xq3o4kTsn_>E+v9&s33#PX;88!5IE7^09HOWuj9!ud z7V&gj3|Uy)Zi8OvL+aQ#danih<Pgmd%i z(1gCS?qqt)ib?u0W_%Ts{Nqd&uWJ1zXWMK$ULw;m1D?8=i49t~^;Bzi#I3bX<2+dpuuj#$$5gl$|1ubudI|4h}H{)722_c`T)IkHr=;Dp%wt%F#vwu{viR6y^mAp=pjbp*5oYg zsUYCjq4ukLllO+h`|>v1Lid!X)|8TAMGkKIJ0ZWGbx|K$phMr(NWj*3wH~m;rDQM` zZ&of&-HlfA>Y#0%dV{Bae|CXB&aQ zlzODxaN_z#rq2B&=}RZ+*HiQHpzT;v>|%N2^!2+^T&eJ_L%X3l6PJ-RwF`<`g?h4L6y%Wi@En9S6S(+imTZB9;UT)TJ zWG)n@S7;>{Y2?8e;^AghCJ+=FQ(m-Fg%j~3kE(BZA`7{p^>^-h55fdfh#J?h zyDC$~KLnXHP%9oZm1sR_s&`5v^mAYfC`qm7Ub&8MhMp|Es{~miOKbo~1-3E)riMFZ zQ^9=AHi6_Baf-^ROym*R24Brf4ai@bT%ww#zmQx=^6V|zk@9Qc%&kANpA@y}#VGg6 z%$+hwYrHHSb8>KQB{fnQBeBV$*A+Qx-Ru7FU|#a@+RJ7@vx;i6$Okk%wRRwl>q#i- zl2Fm9iDV&5_ada$XiEbI<_!^;qS4dEPUTq}7{2U8nGH1-c#5O-!_c&44B@NqDh%uT zPYf@G6}2P_Ba<=ota91#rn^nCe_vl@TzA-a7Z?-IyxRb zrdW+xGr7IrB==Z}Rd%bRdWTc!p9i{aoPJGKRGuVy?u?&{CgKD(wh6P`(!KZ&>1PlY zD(w&)*axvl*wsdMkD}WpbgOgp`LPu2pmM^RFK{S@QgUgng)uzH&$~Qn3s%!*f_T5H zTIw|F;WUly5L&Se8Cgji-8Nth73+^m*mR3w>kMopOJ#Z_?5yhwq)p-=axT%%!#c`q z75CI1i(hn%V}SvDRFmUo`JAb1h|)UNrJwMNaaKrp(&5}1k#eKDUVO$>b)tM&G%SbS z!JM#qcx9i)b%={M_*qfGasXD4E@=vTY>(9eeycKpK5u~pvILVlbl1U?}?#+*sfdJ+UDz*aAvsXUzEAcR zmoqn25u0NI!pdvi6~)NxS9I`@oiCZwZgY#`W&fT~K6fiBD;=Z9HD}_J&oN`sySj2P zQKe^7=t?upbr?Rbr?ZR7b|G#@BkLtAeMJomRpc%cFJ&-6- zD45~QpLT>9y35Nw)-Zk5rd*U1hvm&yi@pzWs1g!bED;DTUq7;=<|xTwU|@ni>&I0; z7S_eWECh?$tJWMCBNKA3oj-AE%QR%|z5ZwN6lvUNqV1+h{&ZKhOHZqkHIY;pJV5v| zsI?A){rbh@*m71WS7-Jn@x>f?2A<0zdcDBs9*rAuC6`c&gnz$1C$ihTf-#?hj*q9L z1Z)JN$gd;Uilqo^OO(b%M}jn1lSkQ1_HF}(`u6xZtm_8ZS8lv3h@UsdKH*ncz7P>; zb|WBbPsyhlDdko&I#oAv;1>XqF#*a zGQ)I-#QFb$HjS>?JOjOh=%FSluW0Ek~|W*4jrhb{B~GlYNe zN59)yYGV)A<`JD#p6c%4u2Dq8P0yQFqf%~_yPN|r&YfIf!#7%80GD4yAiBLUkEW|^ zhKrPh+*}Hp!?AH*7nk_l-1?Dm>tr<BpwiK`-Xbof+-3W|z?Dwa+z(Qb$u96N@(GE9W% z1Gu39+_||H1An&n0Ib6c-O?siU0&X>Y98Kq5fcU^IVZTboCP5oOQg-eTQ;g0vAMbW zcxg6Ie2&4TJB^MBDQdH->VL()>3v71pgCNy)XqZ_%E1z{IRkeCf1w~g$Hu)&!vloU zsDsnXqlqWfq!ecumfY(tR<6`ib3JxujQ6lCEpj5U)KsjNfvdp86mL<*k2ZK-iDI)$ zQBlPt#jr@~3+h^_WN&h}kVG{drNJyyF2~>m%<#Irgyvz+XiU*4b;yOW(UqVCwUSen zT|~;XR6$0;nYuOmR9=a@-?;-ar%Oo*t{mQm0ZW;46Vb$0<}k))U!QsECAJKb{PS@@ z98WxhHKA&LKrWj;1R6JjSwtoGn480FQ|Y-`@sO~+ks_W)5R7M>5q9IS8Db`tnG`-AT4m}-Krk% z4-DHT(XuQ~=c{QITOEAj&6K=?s1xr+dgCzC^=w4~mUQu%B&_t}w;dHXYq;mIMlz8N zfW4=TCxu9J0(#vs*+6YJ{>P5nXm!e@z_bFv3g>9)M{;?OrN?x7`Fl5I&+kUtCh{o0 zI2*7m>WUb+5%=fP`b>G(khyD4w$7h~9xf5r$aJiKeo&NgWlyP=JRjn2tuF0Hvr{3X?hGp<{qNdg`Wr3cLTn6O!8lRwBDKu2Z`{GKU(8Tzn>x!l z(Q6{Q`?{VzxD4YQhMYDcS3xI(J1CuQra1N1cd6^`Nqpcvg}s#WDrzxZt!w|~9OK5X zBS>4FBi}`UetUs049=y^G{jemz*%?1(GA2g@rpNpnX^qcXN~U|{$dHv{td@WQlAri zRWS)0uf#KzI_ew7SQ%EGW?9674dpk&U)`;9$GK>gDf~2@l;q~4&{4K)oSl^1hbG5! zsrh>*VVrX*-E`Y0R#|kivBip*ySWDRNc7%wG^TSS^Xvsb-)#A=)XvwjilkhtZCDf^A6L$!);59{^GiN#zk z%*yB@WL(A(@!M1~l}XD85N;ReuFx|(|FHCZ`6KkBnq;Ju^>zm(Qcub!KmtKo0~NYL zpkntZ2Gkmt55yuGcG7w-6c}OEuGS* zR+sZA<&j^Vs}|!9O|FXXv2L}cGWRYBh#m}k6Pk7MPbT<(+?p%}jI9SSJy`$A6bP?L z+PCC!bm=9x@s?E?M?>a0>|)f9ohp-rw<2fV`&sY~#CEf375@m%gVjbsG+WuaR7vfV z;^Ntz(2J!Vx!3dqq{0yv3R&dIy%U-^EeT=4No+H43%l40-P{8mbq@rknHuHoPrsHO z>9rosP)|4IRvcqAxb$PM??nil^89TkUTa^r-aROLHzO$fa+SVM+C0YzrOSN#}Q}E~hm?U&02q-oH{TitAWbfpBg>mCKJs#3p zJL*4*`HGglE?~t#illjaGomf>WTA9_chZs&z9k52t%XUDZQqjVx8WP4UndAo6ZbW3 z7$C?^4q#D6v|dcWF)NOmAZ)zw9HW7e2iTx3x!&D)uWe1{ab!K6G>C&8Nz^wb8P)l1iSqcf=I})dz8xcgWDn!%7}#ghU?hy)%8~~2M`g}9la~jV zOF0yKCb%3s{w_aeY)umk)o3s+cN!OV(B#cgk_{sQhxy7dHT_DA4$3Tl+5{=pD`A;m zpXp^WLu)Gug6$qJ#bJ1f5L{I3)Uqk;3K+Mz*uzVN*$o0+V98o5xP>E}pnxmdHfsEO zePLb{I9G#Qn=fCkxYCC``oWVXdYLIzd|EgV`#<@42HUpuybs!o1h)8Q!b+7rj}!$4 z>%%($#N|wkNVSFS)u{tlcd3jt^g9<2_o%~JehbX;wkt(dG5jV=c9 z`)k@aW%|751|9h%+WSmqK3ZEi&&;pg43vK*ixVgs3oU0K^di`pH^-ddO~o)ACAsC| zU+RBJe$H|gD3^;(Asm)MS@Jf!w=X8dwzsqbM4*a&Z8&N%maONX%0$lCBh`$GvSurS z)6(1yoWXBB2KmR|!ifx$M$zt~M7d}Svmj#a3)Slc5@>eW61q5fxUl*TyqH_u;ySIe zzjaqylY@UhoStTt8L#`vz(|bQdEJKX*0Lhp1~rp$Jm$9MSFM#D+hS+4Y_#e2<{^Lq zN)6_0J1Y%>v?Hj*c(GyJd1f=K(&rtSH0|zoEU`EaTJ?mfRO+g|P_>FO-h-WrvKGX` z%w|V)jn(E!<-Tk9c`i9h1%86j7N{k*A7itYE!=O&nJX0-9XsCGQgM%!Uy-L2h^A?% zwZ6#p?xXpmiz`%JTt!z|E*+d6xusG9y;{_9zx%t~I-Du`*YmYAQINTVvNGuO=qEu& z^pF+M@hHxfW|-6cCzE^Pcm#?OHg%aMt6DO|>Xk^K?^Ma1-<14zJ~ga0cFI1)N7O+`U|28!ic}H%BlJ)F52u< z+e*rR*6c(9&y>|E!MB4~>x>s1*ks%!u*HSQ9m`8dfU}O$G_52dMse~oZ2o;3082K;e)r45AGPIG5%Ltxi zc@-zhlAF@>_*6xPQOZPA!DXr^;L6lIN=Qt~Nb#dD?$MOSE~yd5`#S(PQM6t^`yWvy zNT@;>nJZB7>6Ik5FA@Ql&!HlY%%e^@)VoTGW_eRLevtAPF6{M8<+11j*AX7-Nh8A4 zt{WD7uU*Bht>Fv(0)2Qq6v5M|@1g>JXg%A*F=j&Sd0%>{){mK^+>uTH;9th#E(dV4 zn?Jx_jPm78TnDnUw-5rT2f~5l579QM(uwgSTXN-|nlooT)m=krMC(LPmgcAWWeUvQ z2{qA#ux}#JRvlyQ?%u764cUvsr_AoxRh@gG{vACx2&dnkO9i+n!sZByl~3q>TJC;$ z(!GDD^2tt(za#T`PZ^9#BD{DmdbFvQqm?;p@Plr=zL=Er>7tP$=8&;33Y(14wBkfM ztE()wZM!gC>QxdSG$#wEE#N3pwg%JYX<);p(tuE!6+>%4dHV+Bt`T$AVV~Ta1>+Dy z<-gePt5{PAnF3)kxm@%1ioq=7-UcXC~3qDpP#4QJNxSP%3~+b-M5yj(s`Ad8Mf*zu;uu70Lhl z!>=#P9_y6EJw$>1IRq%@JUTUaxKw|AQDJv09=z*!oq0dJ|D42wC+c2I-yT z2hlLw7$HXQmuilG9NOy8=Ar=0C?!2LOsmX~&!NH#VJRDd2t>dMWvo zCZfd(#^(Woo9RbE6GE_S=NDCa@_u#K8sq9JpV$LlTuF;IqkR!s1$Z;QqcU0_lhKy> ziiYoa*c!I8A)W!9Dy$v__`wjfDPv!C$W;2e#rKfyg*fdb7g@6d6y^Z=YJPcC@f8&7 zUIF-7=857Vl;1%oX}J{}Aw|38j5F-dxbpM>+0=NmFX~B3D?$t=tU<#0<}9=TmqVYU5%Fh6jSFeb9yAO+~;Irn{W{ zLlSiD>!AAI3tk0ju>{}#SMeW zXjH~VzUqeg3}Sc-2Vq_Ba76pHWtRVy7vJye?vXby>PzT5}mmaJ>V(-5aQ~MblygJlES6-W0R?L7Puf}&T zEiWf;s45HgX27*oU$oOzP{p#u#Yunh{STg$DrUpiiS*2U5gW19 zhZeF8isiU;PGgRS^%t6%lYl3AexZvRkL~?O6k8V2CsqL0IRvzMLETGJ^U|XFQ}erm z5!ZK07WM&4kj|5+DAd10M^@M;IJ<>MJo_CKpMBYY6J2*fqw?bOtM5P!<-@S#=TP|0(Z?v(&I7Zr!(m|>P_afwf# zS+Jw~kQ6vwP$9GcB z-#KDn@DbT*Me@yH|4MIjtoqLpF~WhwZE2>>ohP?-_A5>?XEuv@4D`-+I_b#$Nh}?M z{0!1m=D?Y(z?OtS{|}?nViV=cG^`1i!7xeHX%)a1x%E&ne%l`WaOSPsVC?zL+%MRC zDMYc~I`mcVwSfP7K1Tr6U@dKDKYq(f_3n8?ml zu66)%ymuGnjL`$w9<(z0k&mq$+=O5XHy&>`nZoGcKpi|I_HH(GP)Mcy(GB{R1vhoy zPm4MxDCG+M@s#V$0)976w|a#!HF|aB4%Hto_NJu{o*7h`L&<_?ZM2*cn}j5=96O$O zTY|;>&ExSs=}TJTe35{>JajzW+2WcYI+=p&*!jyx0BcU)U^`AVY6SOY{beug;OPo@ z(64oJ+tlOgGU1ctKhBU>^Vjt$BM3x$0`b4G>i^Buiza9}>1E=8K$$ox;T$3Wj-iaW zh`LwqdEU9}yk!2UF8=f~@tP@tZ{Y9&Sq>;XFl8`tV+~20nT#x_RsoxNnf}5>J)W~SFH|$z$frY@}l=RYW+nOl*ob-ui zHPwANZL#(DyO4l?0m<`15?~I16ipm^_!1f(I)EISG?GIU6f8~g|5}L2WiWCghrsen z@DPk*8wOb`RNfO5422c&{qa%IOYmcA4w{0$nJ{xg6+lTnNuil(N=K9I6_D`naoeG@ zNECRG70eg?*luG^l*svxI0=ww2LQ32%_d#GW5p-rR;L@ZdPQ$xc+SNx(W40H&1@5y zwfcxN{N^t%&u)<^Jt$QPmY1@{=+XkaCx&(QsQHf;G9z?g^Uzd-Upq}XLc>hP<%+RP zE1tk~Z#(4AWC)TqDT_>vAPX`#(>{$*U#h8z6po!xAEo)eBB0*8j-+HR zYzSh;zz^oz%n*#dKZ|xRxk%pIC7>c64=e-ejrtq4v1g0Kk?1F2k5|;Qo)}1}Uz<9iGVL>eag!L$x?I6eGKYF^65| zguWKI*itdV_5Q0Dk^@*418wyXLBlpINXTE7kWx#A>?Prj7F?Ox#t`IWeEJRy2xl}V zP&~Rb+bqump0`S!=%<@ZA+&VEA>8m}M2RFUBa_U^{z$<3b5q*_VD7*#yOjGzGEj?U zjsoV}EXP;g$unW>5dE2FYvi!HQC;dyv41m=Rfo#%3E~gQ_CX1@=h~k)+S0HN&0C+R zvp5~>qgG8u5HmF-P)d6uP|W|STLugqBq@iK7xS*F2fpbR8!hQ*9ccN-NUSNco2{7Y zJB6Pkv~(2^3Uo;Zo-!v2Z*l&2d8Tq8i;a-3V#`BIHv8(uX~p$PMI|>6$5^_FlS4zc zuK0I@Chb^E&w96eyQVMB=%(&&?csfEv+cSha;|F=Ws4rCr%!Ci6>Fd4W)nn2jlWaa zeGTM}2aU(%)^fI)@4%x5XfmIcPCA?5vLbuj9?~KJ$LQqmxqY7?%UvHy-`jzZ!P`Ug z#q|4CeHcg&gwmLp!-Rc6r5(@7x&q*3IXR4^~R7K3VGe0_V<5YEod%rZ+Y=VX$m-a{4E@q zbq#JOp5jlbIQ`7!3@qlW9>os5Ai=|4#P`RiSRw|fCi?qNFgqTeg_$=5Ue zZ_v~k-QCt~p$2ILqlPiBp5*=!Eg<#Zr||f#c$s3@(D&3mp5c=8!&<&QSOqYHA799B zkX1{xRAVI0_a51ExH;qX@(tHgtZdc71128aXnsba8jkhcdc2kwD8wtJ&O%&awZs4s zLlh%&TSnzC5mC!CIOnU~CDK0?{)QivbZa6AvC+w8L+iBiE0vaXcf_zrl0(p554H7t zUM422g3~b%nfX-d+p^ zIU)o{4P^)O_qYEz+B{NY!X}RUpI!iU_gKzi_Md%r-DJFiT&uk~mK*rBv2jC^tp5F&Y`G!6_3KgVl)%eYOhCes78Upw}HTn_ZnGWOZAO3Fe{BB7UOasUGK6(65e+>Kd-U zz!rOIFBQY4!ZJhNV-zeQEdsZfByKRIMx#5d$2=bTl}hFfUX!K(&OQ zdVD`2&P{T?VzeIWD@qQ8f9wO_JF{1IxH-^w;?i;<){7ecc^c6EY~ zZ`Dy*{^ut!FFHVo`OBdFd!RN_f7CuqBi&W?5l^SN!Qi}_0F5#rz6BHe%Dd3$Y9QER z#0?uM9W*DQ{$?;J4f;!`9L>VqQjsMXT$V9*;(^(|8$`R_-*YV)BfL(O@biH!chhHT zll-CG&=%IQ(p=N_%9|fgtQkrDkso6-m%$}X=a&14E(fS=@cqsKWArH}N}S8^ORiRm z4&vaSQxZSdFwuM|5@MHOco`@+K<*;LRl>8N$2UpG7R>$A)9x3R$?1Eyu>9)_Osh!4 z>98F6sqV}MJ+&O$&o^^Zv+WCWo_aDpt4Na0o5XN5VG+=t?GM&w`hfHma=r0c&+_;} zG8ZNgDjJ~E7fAl!>k*?5%+5m$iIMou9RXY5>#gL@6Jn4kSb5v5=97_tgOJ~G3#?RL zA3Sipbp^au+a=Q*h0`$gzv)@LT<5+12+(Wp6uRA_c3GokxIViZnlEn5hBTT|`sa7& z_qi!<$}USOe>Jqh5LK4`wvOcjsm-{T2#lSI0U_Vcymn8X9k&eWRR~^$Xkg0?|A8%T z_4cl?>|=~U*xe_P`-f!&I;+G6B+S^iWA>(#=^o;H?L94-JfAFOe6wq$sGt}s1i3P}!2r|x? z1Cx$#{wH(N0cRe))pFif4e*1b4!BL)oExRNeC(5HKfq{PX&~Z_U$e8R$)<=m6;u#( zN%H#d+qPQGI=TDDQo26wWElxONhss#>G2B*qmm7nV>FL-WS@zp-5S9%9d5j@n}v6y z1@eBrE;!Fj_V#Hp!_g#NF%+F*eV1>Kz^7N}fa~pk2vnX_yrQNrCpOwn6euxf*rq-1 z1&w{V<(R;5B|=XSKe@%_UGofjZuOj!w=(r+|GH97aX3J(+)|d?0jJDzf@zvmJmI|_}CmRu-3_H-`PhXKXeE4s~-S86fjD!W50Y%bjRXK*r%a69ft#+(H0Cw;2 z&Qq@I?n53G*)3SdKmmp_(pq`Zith*WLV%omyh4GqSM>JpYU@liNHlSLM979Cn3d41 z2<)2@L{Z5-9grxB^4}`Styx83-0TAJU(NK2gm$dq%1|aCVWa|GkYUZgR=wiN1t&2D z5Od9hlkG80UJE2ed?y?u?dl)THqWSm$XFFO3 zx%C$h^shvik#pbI28Hi#wXsq-p*_SXLDTeMO!Zx#f7O#nGKKUzoKZt40V#lmK<{n~^+72bAN=8R+$!SIkfk4aN&MFos5xQm*3~0AHg`b~Xli zLUc&_{bFu_;)nanJe72vlOxK49$Fbgd4gl1wJ{WE1r6=K-HxQL|GrzQX+d-Oc-QOV zd3UxkX<2ajuiBbkXTWSf99O0YQ#FcI9!7W>eydb-Vi@uW2E6O}_k7>0n(Ncr z;|p20`uL4x?ScZ!;vh&-fD^%QsDM>?;ndKB#K@SzaFa!QqvYXO#CKYf1JYpkkhJ6A z)X+d668oYN+QNwyN!>mp-pr_Nr)BsaD%1*EAPfm&dHbPMZP>34XthPr>CsL%eXM_j zhZ|}Xr_gc6C!X!8{QO%hXUlbQ#orj*-(Lx;>RSoZ`~vsevhN90yScu&0G^%m_*e#r z-`YP#85IA>3;OZB`9GFf)(8<6G{#8w^YdT}b6$^FJmJ=F`dRL|iK$gJwi4Cz^f;V; zWBx+yU05(f|Ixh-9RFHTp=N;;W>9o|8Fsv@=63v8`?rHIyAlO=+V(ziY3398q&$N3 zgdMmtK^4A`a}8}}W3F@>y}+n;gjB>YjvD~8m1=W}Itu!UUkDNH!~|+gul;HwEsRtL zvxQHLvw4HhO8FD=60)H$fPfsatX|430)AT`^mDQO#6qqE2%^pE4Mc=M1Bp7W1o@Uv ztBF$xiyA@ylyU=LTHs4S(6HhZTiHfP!t3r3WWR9&8v_2P2n$D|1(MFeLxe)Je({h} zh6yBwiPUw7Kub;Yl=Fbo06k$Ni0F$dYi^p8RX_?ux67I)&{k(d>w+NV;a_-r(WD`^ z7L~g(fdtl5%Fv%R0@I?2(8^0P#PA0w50I|q)j`@6cD$9Bk4s<5lfsPP`(IK({$5U} zye`TVM&}*`5x08EG1k6|7py$FvN&G2fyMR4pvHo3VN>v~03^|fu84L{FN7POF^(55 z$jRhAT#cNEyv6!Ge7BRHHfPb1XVV?011InaAm0>3!r7X7VZ9P3=1`Ui5j!32$J{Uv zvV}k+_X|Zh8K?)Hsedji-V}wn-xrLh^7EO5&{#fnr_V(UCT6s859$YoNcOwm3-K^r zTB`-ENd_81sP9&$bfY=FOs(s+b^B=Stj0ZOyk?uSI$ijAEX}0<`Q+%loPOZrI{J{Y!VM zhKLRX{cONWRbE!wA|;Ne`GhzttPE(AcN__BI9Z3$hNiDzrMuF3@8)eaUtBkb?M|(& zoF78L?@9C`uYFDE-Ok0YDD+EU`A_`-fW|ND8iQxK^23l+=0m0C+7FI~OH zK!W^4GIfyV`HUJJ-M0(!*d#TG`LoXsr)1nPW6;HC$^Tz7j)wI+gW3K_Xfpca{BvBj zw25R!rV`MSfJM~GTm%b~!^WGNWMTY|HqJdBs=SZmXC{Mnduj)R8JAKf402slE;Cw% zx$(*+J0@xu<5m&IC}x&rdWu{QDP1ttq{xm-D|hlxa%omX+Zrrl*Cp3t#5}LDJxcqW zf6njsIp6dBd|$uw&-s1N>%7&vg6Lk(Iev&b9Wu-njv&wPf=%@I8ohAFStjE zuu%ru2pTcR7Z2#Nn7PmGs+mC}CEvhPJLwu03qyL6Jz|@S%>i4}B|1jP5=e1c+TKN0 zUFoZNCSPh&ev(}Bk&gL_enfG^-hke z+!$@m2w^-HJwu0NuSWI%T;||y9Yyc+QLp`#7r%T|*jHfH-ij4GQfR$60?;Aq{Pb?RuGJT`G{ud zRY%#nBg1f3*7v2UKTp*xPLK0-ifChwQWlEUC&N&rST0s@4sUBMV2)1wP>^5G*t<4| zhPx0Q77AwV;#LQyy4C|4;Pp;!iQ?V;$)P!mz4Mv7CGQ?P(M;1l;bWYUeYG`=!1J^* z??fIRsWF<-zR-*)rz zg>iDAPJ3efAi`=U^`?AfQEl$fvF*9imc&>GC56a@XDR5LDi1=huN|N1Z3uCcS?(9( z^D$ZCJKB1)H!hh~OgwOQWhgC96@Lg0{#8Uco0gTS=NdAY(BiP3c-8TtM71>4DaNoaM()zCkOU-T z^{(IK`#RBKOh8kcVR*l2z(dS-1p<5n=Vyn5q>5OK(jOQ^ zeb`^&y-V8?%pvJ&rY%b~+aeKbNt{=*5jHH^%7D190H(D=Y4W|u(M<-LZ+QnkvD$yktwX>Cr?=pR_Qf;QQllxiq zowKF#hw;FBt=~GqJj-wF=hYi^6V(WM6bW_xtm0k%%k z19fu_SDq~GX79_FeLSv#Pc@bRD8*y`3m&om zk+rsvY8P~Fe~}M8Qp^c%GK)0-3E;8_S>sewB`KU;l8MN^gKz#^Zw9vA2*>B!$6-=l z>vc)tx@D!FW@9VU^p{Pl8=DGYf@{i^Z<2OIt7@X;8~+TfuVr#oWEUR=K@{>r6AV`0 zjOth7ToLbSI(7aFD_K;XLirn}Y<>(Lx8p_rKbpHHpZ9wg#BlL~Cf3duB0+yRJCeUG zaqvsnf`kDdlJ-lOFrWc>C+JI60JX`oALo{v`dg|3nD$Z6C4aM_<0w19CGyvsV?)aS z#^xJZG{s$VF7Kq|Y$PlXJn9bFxKi(Jt|;M=Y?4aAb#HxdiSqf+{1QOPr-KUJiYsv? zK%mNi@+RBl^Y*;_r`rGkI03T1)6kY=|J0}l0JSYK$zauYXq_(^DcW0N+zFXfqmBCa zQIXIL0tg{%fgx16O{t9lp7K!zDP&p2zia#WfnXyZLIlIfTHq>G?jJU&EjCyp2qDA4 zTC(C7S%f}F=oun-%13X*Q8K+p1l1|1{}VSz1Su4h!PX^`*g!vep|^OdkU%``@2AbA kAvHru0B|Of8S1|QhWTQp!BQF$D(wk&(a_RVFG-sJ0`4+`FaQ7m delta 19105 zcmZ6xQ*fpYur2(?$;7s8XJXqnCbrEdHYc`i+Y?NjiEVQx_Nj0G`{F;RZ@Ri`RdrQ& z^{RET%G<%-y1@{Y zXz1COT9j^+nfR);k4m=b{OPZ^7nSuW)}TOQx!dmfe!%+h(TF_{zYtEp21+R*G5JX| zaa0JVv@!b^db6F_=HYz`Zdg63(elrzK(oPN#h+cCr9c(A8s1^VG=o`$HpuFr-(kTk zRs^BPz_^OjOF3A74IxYD0tcm99*uusMHL-R6MV`~b_bkRSQ=Rp^eK9rNHj?78c(lk z+5`hVJPvlIjWw2(4VxOC3tidDW^1Kv7?c`IYu$ktUyUn}5t7=8AXCckZQtkPeEU{o*VOnrrmWw1OeX@@!+8UH-ei(?nv$EJ_pI z01VUE!SzYmW~xMs>@{?40K18?VVOYJ8;6U?fy4p8vtwhm-3t&x-I(2v%Uij?%_7Zc zZ9V65r6tq+TH3De=*2I!7uGZbwQ=@v!+Bjf&VNEE%fY}F;`Ckp0Rw?-Awi)3@AR;+ zu>YN26!iZK1W>XkI1;d>Yv6j$h2noxKkoH9Cx`;^V)X;u6BZ896|OL7^YKw(6}}>= zl}H9@Z%2#GX%Z?M1=pQ-T1?RHET?$M#(!Cqg#pDViGipz~xGJD?LIJL$B|>Prdtv=B zJ^zqm_i`Ag^7-f1LUzP`Cx7;&{H|FIL#raF;VIC zUODCHbowRoS)nDfL0_A$N0%x8ZL)u**j#+AP~XCic7SF%YwVIw4^^>P%y?=wSA$vO zYQ=El579Ta9X_B~^>G^K#;mmypGo5(%l1pBTj9>feCj9xO#=ltpI1kK%htv#tKn1D z_;Y%rr^b6{_C>UmRa*{(YkXO2R3%Ykq*~WwQTZtMvc5C&g5rTpMTh=g<)cX(`^ks6 z!~p;K-lSQH8Qu@Wn`=H%pO{j?!ipyF2FTWcVq)rSemW6|7W>|Hp(})=z=nCYd>=6WQvKu&L+;Obz125XYAY0nEN&<$s%XVGAqNE7L%_w3ZKv1%n1XqwA(#Lt)R z;P0O&`!!_y3IiB$UbY~(#i}3n7nMZ}!iV0-{oRQ;HpLCW^}?dw$m( z((#$FVO4#KJ^Gi>?#p^ip{Ltb zmlEK`de~caRj4qS75!P9FHRCEx|eu>#a%X{g*1uE-S-K9@qBJUXpo836Mh_@?Zfya zsYMZCHdRV}(b7HR?ZOo&lD;`xG+qWn#=`1yCW=mR) zsI@@8+V&z9%A=%Re5Qs+OinQ)F_cz;Pkrr3ywt~-FjZRmfws|bVKM$GuVP?3k$vO# z85hz~M)sLxFGyBDb1EHDr3hk&&xdwk=4x6==(Ag>_T~n=`K7!2 z;quJ5Z~dtG!3L(Ey~C%~Un~C8>8?emS5K)u=Bj(?-nh@dJ-b&=Xh!R;dOOUr|87U4 zuX)Sgz@u+7pTMve$OhKjdvRtdR}^Z2d+M z=g0{QTSd;;Mrkoi#6p{Xn^#0TTiXGj@817(eKU1toIpJsG)`*qzFk=?iJ5ogz?CIMGh%q*wC`@cJpIxb{`e)w5R z%u^GU^+r#U_r?<`#lbMFt1L*FdIkbP_<$iO$ILrSYv*m}LBWNq&&t))U^J@i} zb#9m61Ktuiyi=n2_aGK3Y=@40yW!Fy=Se%k-}@2RJ)a;6z{LKW)KrR#{c_;bTupu2_eKWV8M2KPye3Ms#tbih zQxmegzAh>sx{B0Owk(kUrxf#Y(%(&5t=9dcEBpuGqXC?w0jeEXErtWpdi>>@)Ff3A zVYYXLhI%(EO4Z#iM6#e0BchXRC?tjS&nC}9^zrDX4CW`Bvd_=ev4PxUWW7XQudd~U zo8am*@K*2VKUW90QMx-@--Q|r4JouS)|vI3uj3v2{bP7~i@dvXyCZ){?GAIBZsq9R`PDeUHM{SA z`1te>a`xH5*ofWNr$w_ivoa*J6`vB5>&rY^ObRu zkL_7;79&GyiTr(U+^$!D*yq(V?8`HG&j1+iYIrg23t|p!J$7PT@v`><=l7f6dmBN^ zE4lLN?10~{?6S`@<^Va&f_{*h9r;Y{EI$3v1UGZmu1%IK-qhi4J6eoSH@YJ^~nm)RrQ0-=hkd3H{h^x^JOo%;cs{;SeNesdvr(j zvYGk5cie9W`Xgt_&FMUxjpv=4V4%9O{RZrOpjR4NtP3Cx&x=(5^Z6jIl}k+xy|qMHN6|QW8A;1r@i`h`xex%m?A%?m(Qa_;*|`{MT3cl^4`CqAjZTsgDR>#K1*aw z9)$GIa=5t>d6zAFF13sMNr8-@6~PM-PVRN6}!NZNFXe9^D| z><{qw>NmUw^IA8Nws(h#j}dBaGWX0bCs!_gMJ7sb>%T`Jc!uJ(BQ9ruo_qQ5Vc!XH z@`sJuMlnGTNQM1PgxaJPm3WRwDww1dm3WOv>YVzV@p;>+Dq!y`CT&;=o1n;^+ zR}%skHoV%H?<4Jx_%L?B{-a|M-#$|P4?%6(uhbs`+nVj%M*hEEi};)}myz$bj;n=; z;mtoU@C1v;qhr|()P<_MUmN_gP7*FgY}zG|=dAhTFxyu14SO3N!j|xt8k65VML{gu zi22l=x5SpAg*#nuuds8Q%q#i+xCZf*0!zvtTKunB9Y#&(VgYqPUy`5x2ek(ENBt$> zWz3h=6yH3a+^mho)~mPq;ndu*kuPafN}WMmcEi%on_KA_i-8!Tz!)LO@k9w z>Eqn%{W}f-^7L*r)JVX}YX_!?m{Bt#Ml71QA@METB}KV=b}9-&)K}?9(dA?QjTyGQ z!&{WHs4q`=zRv>6Xp^3|MUk*Xf&v{m(tCcL`uxl7t0?`z7BDui)S>WU13nfxkh@Gn z4W*dMHELM68dGZDsCy}?(haVnCce0MRAeD%S}UY-_x5?yAOGi3q13)tciOW1fr`q8 zjMfHj33Kz|()!NY5Dxk7x`?y&odu&H%Bd!eSh zuL;UC4A>5|^8R<6$|f>PhUBox3=?oc3a5JhwH{{Dkz36#*W6iu)?diKK;=9>C z7xIu~R1{(qC{?k^-y4Wd36`Q17DMIn&B7=WU{CG^#uJ8B8-6S_K%ml4o)? zx-#HM^zhAn(Ai2_ku=|`%T#TQ)uyF#T7QIBRXh9cUi#ZP2^ctMWP;m(y)hHysg26v zlzBYuGiDGjA?~&WaG&x`Dw$aac`aT3(SC=qyt;e7qZ`~Zn3Ij!SGw0NIhcE-4krKE zDB#T-qumzedl&Jz&nimjksgXSJGR_LL^&|B<+teQsJ=^q%8kcpTs}-m{RFx7OWFIb z5(QhD;5qW^W48ajJ*9FIjOq&lil=s&f z>gNyoFl?vvU7KvS{3#rDXkE*R-~R&P?Tcl8uJ3B6bCjl4Jf6vGT%b2X)U755I)fP3q}J0yx;lok`%%Uh5~Cs-X`q|L4Hh#!H%r88Z;oI z=bo$6KC;-C@!yFQx!>4=m6Ek7m*|}WvcrYgk@HH~!eu^THh&!Z`FNDYÐksRuL) zMOqHJIylwHK~_~nx^h4s*Mq3Q(nbWW5e9f7$z$E@vkorE_e zyVsavFuEbQ?5`EZ{`Gz>WoGA9XWpbvm)(?~YBfcgyo_b*9KC1Jgw25Bk9}nXFB^1h z>Mw{AaT&*DM6oQV|D_}u%WaYOClQr+(tI>=-c>)USUqdl;^8x>UBm)l62jOT&3kl< zcXhLu6Syqg81*AVTuy##RiJUAid_znnZ)#DYVDwPI}dN{APkFn4%L&;xDLIOybacN z=OFwDjdHdaw2y#9Li$p-=rDSqU`Al__%SZiE(95tqx(Eik*vG!D1^+pUUMd_rKoFV zetS$dmS-wHh8TwwRzLvM!p&hd(K$n-51((c<>6EN3!HMntiZ(o0K?EWnkhPiZaAz& zpmGF-OIaFQ)}Myf=mdDL?=Yu`auFKR)_|qT#w} zyr4}5#1sEWfXu|4h21YPMWRW|_1qvoO4C1`Iw~gWxI#lnEj6M$4%FU!n{{~`Q#-N z^fS&k;gBDqxEX-lR#DpEqn6{b+IyLd8c$SoDnUZ0_Uspj$N5?P&r+Ws$<%(O)S5~OXNE&x-#%BwE52V)qa-0tt-3uFIuJcJ^yzIfA7?Y_+7e6|lYfKph&N+;Ao+4j zfXf2HzJRXwTkZqL^be)4fESoggfY9XmCx4KIO-35Vm$x=+gPx?CVk&gS|B`4RCc?5 z0!}}C?rvLh$T}~k?Khxmya)T4n}AIwP2g)mfZ+Y(U_WhbpZK%zL(J<=Y}cU~_?7We zJ1^R9#E^w;x-oc23i-Chblco>I6%0aIDch%31aKtU`LKH#rJAE zeo22fZq()HH%4Fl^?WbKV<`B$RZ-v%DK>>^F9Q!t4r+->3hmU-?7sW`ag_I*IBi|| z$vh2($=@7us^K(L=_k(d2!mp)L02-nG+~&0(}1zN>aN?V6z~AudYmq|HICuONG5d_ zx@s+0m`rv;bxbMlV1_^xyx6NMt{gm`D6hfrqr@y?euj__y%i zN?83^o}`)8r`8w66y|g^$zZ>~2mR!P{|!_lJUWR=H+o}Sb2RIjIF>=6r!652pFOu` zfJWR#l)Hs&!n)xB2_cNuhvIcFZ- z$dhN#luq3~n%BS$T?3FlRUTQYX9^ES#D;%!7!ou}1 zYgsX`k}ZUZuPu3@VB1270rgg1^NZ-)V&^i$x97!#uhPF?rdO)-$mUx>nL^bPoI+J9 zXU*?kW`{jQ*2Tx%H-iM090_+WY`I51joFTp&c50Ek`p)F)A+~bcj4W)&d~p;n3wEL zueLnf&Q$U+7q=Uu_(Mbe|KK3b$3XW-+q1bvM;%$ewsD+@@`qK50dIoD`Pie_Lh!U0 ztK(DB+-_z3q4~~gS-7drY6yWv%^v#F;OMKrrxl~J)OjbZT-rtLw@K!Q=Gt2IoWduQP?R(H>cQml^0Qa=FCty)Ed z>Y6Ly7h^8-;lN@gBOIV3pqm{;9@ zm;2~1soJ1z2*B{|p)+=)Qaqo8V75~v+&;WL3@y&CWIWkAjs}@K0S}UZ5uD z4|~zye`qm*$IadnOq*fOFRU52xmi&9@_03~|I`hQY)KU-4+0Dgjii^=+EUZab&1p0 zkA`I9f*`@Nu#V@5NB5yw#+@4EghB%L7lYFx7Mb5BXMyhm_aoU3?J-vl6#m3c`;h^^ zBXU$W^796e<&`W2_f-yLFruV3i3IkoDE(!z*pO?`~b=91n&S9VjSP>A8lM=BVdL`^2cWm~|KMy0-O^BX4cD&ZN; zYM@ypHU^OQw1*gB$k9iq!knI--@gxe3I2 zk9bOTy`Z1Swdfp(muT4312Bd+OvWlio|TVu%pyilLy~omQ)Lvb8k3sRo*VTQ5pusZ zKaJ0zn5kkJ@AA#xG_E%5+)V{~6>XG!pyCWNQ32yD7S5NdBG>M-VRptZtNonQf^Hn~ zBO5LwGnX4V@{7o>BMh5lRO>I$x6m=^oe)#->bTleK~gF_T+n0C>fA_06`2kLR~1>B zZb`*Xo)8JQ>>WtxF8L!PBVk&Y;A1h(l*ia9wE>v0fn)+=pNl~SBhtOPN7NrhC)7_hmxcmT%gReKjbaB0zFeFBP#B62Dc$h}TdiZeH) zljot&YbDPrruoMoF+{M>Qi?P5ocC{)9H=e)8u!Y1ye=+=oprv$IXU}Ne&|X2bh0ik zY_q?A_uDEeCe4IL9$!--jwdBILr`1B_+n6pp;b~;{ZCmgy;3r&Ax+2Xubl(r7_@KF z_(q?G2$&xp5=?LRO>Nhi$XA+6ATKtNQ=hnh+tfQeOgWq%DY|T)C-E?{$?fk&Z-wMCcJr{g@zP^n@ga% z;}Mse(>6sQSu+nnb>%8~-keM}Ui)>x(T5y?+k1~5U};p9xd4aY0;3hSk(3m*@7JBe zdM4F>*~UwxPg$ZzhjzU{5_pn5pEw0kL1nrG z4S5a~oi+sE@n9inDuxjMdRHk^@41(7;Woe)t3o-=gl~~>&e0-aaS4*#yN2Gbb`I)m zstF$;eE7B)hXcrF{_(6!8I2234|gikA{mw~AhRrsUP_a)N*YPz`esmt&RVTlQynqB z33fuHqs3P?Yn>%n?kkXYzz=MKY`9*mPzQ)?&4r17pQFD zl^%L&K_?V+E6Mdj-8;9+q}3g$d8PTmRBed&;*b!kl}2nBRU;!CO8rJML>|L&BeFhvJ{$zE zXFI<<@(F~0`v)aAvFj1VMon?<{h~@J=suP16LVd|y{PH;Q22qq&nYQaC*PyHAmTe` zXbgNGRs4$?w|+74tGb#}vK`BeSP!1Vu*mn8eRBVO9uL2XbD48ZLuJG0;l~@h1lB^b z{C7f%YlptvX{`f~>bT?>9eXJq5FEHpjOsf1C*g+hSccZV2q0veu#@;#e(2_ zCzmAg&v#X-QJS%k^n<#;ARGEgBD*6={#^?9$M16aIHk`Ua?W~1{s!sp48yZ~yhREK z*#-cweRCJ<8j?hrOw2*$BkiC4g0@D{I=lJbp|hH(2Q5d!enDI65~`z@M1PdTa7zN!goiQSH0WPM z^Au&r@}Am>;~YE7Fj~JLu6r&?ftF9Bx8MO7vk>ZZ`zZ?;`c3#AT%OzXt3>NAK}ayJ zI?`!BnIT_n%dI)aXmA|l8$^v;rq>Vtg~|ps_B1}@61517gDSksVy9Av|9n|_3n9?rbhUK# zTMp=CPnZY)VQPPIWu*Il&0YS^BT5XMYnQrMu5vGuErP#IO@b5f=1-&Fm_eELx;34z z)F=O{cU`PEwHw0kW%R~}lDn#Lce^%|wT0|uEQLIoNu41SYlE6eg_1CKD@#($8GHG~ z`7dgCwe%3acY;gkc(k;)#w#SSunLOrI|Ig3iCPwg*^gby?@Zc3`P!QWKk{w?AL$BJ zM12<&df9>Ck8u28yQdp*C>}yM7ZtnimS31o*|=ztr?||*JRym?i}(d%9YMri7<%Y& zD@=b7VO_V)T!hY_5pKECMmz=BxaQSMlm+uldyh*QZTv!G=h?$EzmU#s9Ea;5$*5R3II3#M(|K$@4ze zNu!&1Ge0JknaW2nqJHvLVlb;z)DUhOTin(qI^o0#UJF+VY_Jfq;9@(T=-DI4 z_RzAG&q zbLL8HXfvDZsokYL{F4V*FKw9okkv{XsypWpqT1~W)$90=NDtT&x;eSIF#3C>%=vp2l5@t}@|FBE3cmu0}pF4kBlItdxiw>FLF#{T2DP?r?_!APf1 zA9mX-4B7Fx9&~Z0dSM!}$% z6xipZUc^bNIMAJ8Bd-?ZgWKcL!@BQ9k9kM_S4 zM(Yo-E9n^lkx{hL!7w-m5#Os;vt1ES6tQHUbju+=RFXBzn|knql##eFH#3yMvlE?0 zxv3_95Mbc*>>6|2j^+*w#%y&HT`|m9-RdY^qVDg7oFXzCQs@btxvCBPOpNH1MKGPk zyNSi#Eyv8HZT{{gw0x!>4-=bNc;v+b11{gDrCux!1m8148S$Rcw+j<8Ce9h`Pas_* zy*F3cS8{dU3O3QX;M~HX7VMGjJnSuXS=i`e*LN?DR3HCDZxel-pswjZEJsL85vt-R^rS!U+qy)VLgAq;7`*EBbaXVOc);5l zg~i0ot6R9l(12>(v(j;Gy3(g4K5R}JPLt13q-?!FldFLV(;}BlMM^Jh*_l4RS?H#_ z2W&jsAfjxn=+`78F$F>;EBEmlC8sb+10k@z<7skIHdtbw`cr3w>B(We(RLj<+H7NG zA&wLd8_6B>0VEHT+rQ!-RHOvCRPm0ULkZNvZ5JShIZMS@wuuSwY2(1PI-AvZ-H^e@ zWGK3jgYfle4Psj61)U!z?+vAOgo_tgH|G`DZ=eMjl$lMC4XY7G01h2>$SpqBF$y{=xQ9%)5-JUJ)XPM?*+MeEAmbv{ z&6Q|mX&+A74db|ppUgd^JAOChlB}F(DE=)o6qRJ)@|2(M8lA@?v+t>foc_0+fA5@f;NYLRxf2@RRF>0G_0T>Qm4yZI3QQZ~} zI_iVl$p6$E2MH3SUu{iI;yqwS3_K)v3CUyH_pF7gL$4oVlvq9@&zC`ztd=_^SQQP6GBalTEW)AE zxowz99-PpG-G6$tn9&ec*Ym`Zl$I9t1#yL%5vx{nc{Ae1>tY{LuBD(o--+|osNOE0xdni9+eIMxCy_lO5yO8 z0p(aO`S)|Y1+dQ(NQF!5qEytkVoqFDEPrTORg$O6R82%7bWGonsT5TFCYTZX9!rR= zT8pm=*Bc9onRc>Y%#n`{@qXS~l@o>cC2&PNwM5MGh2;%$j+P^h%_ThASL?F) zV>3tyd>g0hu8a81odQeC2^}+LR}5Z+kr*{Ms|RnW8_&gd(*wc+-Z2RLnhFU6}?Q+-#5$iu(L#)Era zPEeE(gPeI{!b54AQn;kG>iouMk`vxWNkSzH zWCPxo*X5%OXqd=Rd&Bvd>5T&i#pYEx4hC3-bbyC7>1=%@?IY)oi#%jq!st@t48qhu zDmSUqwTFLVpHbUceox8QyNBLhcg_oYy1L!}a0wmFI3pIFVJMeOErzw;bilciDBo`T z)LQ$uQUQbYh+X!E`NE^^`){O`%%=BNih`9NsyromTI1GwM8CM2=W@=2mYbxwDa{c_ zJ%C;6fAtD-88?>k9gf-G@FBh6BRUh2SC7TPh*nEipwTtPS1}(|rm3=P8sXJ6&C20DU=1TxjTvY`gL{cOEfhA0Gi*5`G zRwhRfMzIM6gB6Wq0f|(gLX*I94uK#GC2;~t7GWV!Lh=4DkOCu4Acagoj55^UQPOtE zVNi`m4)E};>zxz96IxkCE4Yqq`CoRX)SN9LYzGUd7CQ8{E?Yy#5{J|d7y8*00PQcY zi>xpd&G!W6f`#^EQ;$s%iJSf$7y`}JqCG`1bqGk@{oJhuTO93mzOpb^b5@tN=iv11 zVFL-?MDS^N7{K$5%-fI|Lths-f^s$?h&C%dqMKRJu>X8xclxFPaof8U(lOJPxX`*G zWeXk8gg~m#4?{v4DeH(X+K$W*#C@fU`$aT?v1kq|ySp%L3JyV6iH?Ds=$kX48Uc!5 zOJX!{=>BesbKP)C`1#|Y2D2N#v3*>HrTx|P(_&d2`Fl>jon^zJkkv<>l*g;Qi8t%T zp;9msQa78R!8dC}SgsgTqoSLOfGrA3k-T;V#elc91#Zp|@4tCVz6jfZQYltE;(#u@ zTfu@fq`Nx78`|#}a+tNi)^mI&+WgT%T`$?zZ+If-^MQf-d<0(@{!jx0c4`&%b|F_4G=f?0jacWv&8RUJ0sv%05>z-Y+O+e~q%oZp(Np#ei=jf$J*;L2|u34&A$zwU~InFXv|3Z$(K%`g!s+ zyGkvT_v7>3>R!y8~KE|@TxG<-4UQrd|#-LW=!U~LU=WDKK z3iuaqj#D+~vF$D5_3u0jYp4;gTb`V6#}~E402>#Zt~hl;^_&Voxl%ae{$fdOUUB4P zT13+-zC^#Est5&0rPEQ5n!)AG*C0*H#<2-Uz{<6lgDe(8_JU&A&;F(zRhLT=n^8%n zd)P&a7t=3)Ena$F&i+tux;h@bzLjp&F(ztOAjXyzJTnwS#9J!mSXb4_wlHFLF@y_nKln-vY)*j zpzKgtS%n0Ye0f)e{<>9gE@diw!Fp2tydpHAtZMUY(*S45JZlvfNrvKJ@N>RnFl9#j!NZtAUWC+gP2_FeQdcS%T z8i|n>{vLUDTFEv({Q~ST%9UTJ1aCTG4B z^lN<*Ke^ig{G9kc4u(gMDPGl=Onxr}Oit1pMCaMx>)x?(dTIEQ9p%neV8)*Rj#^N}7h~|m)>RU7 z%oiAMB}Ty$=D^zNx~c!X9|3AM8wjsxXO3Z*-j!^Y%1Q}mvkk)Xx3&DXw4S142x#SS zqQnbFs|z;47Ab!oB-|V|S+@i!+DcP68htkPb=Innr!&L6u9U}8YBDHBJX)H%$B~CX z)J|#+qguRl)h68B^VE`HUPfLaDUERL8vqgnB4!|`Pl&Itm=7SNeNTUUCJZyXV}E9v znK<;zam^q0bEnG>v8m?wtsI+~2p&Fa+eg1EhJx65 zPPExhuW{KLs#5tO9mZkC&Az6gg>&QPbd1)-V0j>RaY^|FGDeK4!5mcKNaC6N1$-cN z!(&{aHA>26k?MC}wTQ>;=h2GNITnjll0DK(Ls7I{^9cJiE&6gHVs}?Qp4R0mJVpE) zr~LLbTzoQ9-mGK()n}=B_w&B)hS}?a2%?-%7cDGerc^{T>9PG> zi{(#u`iN^_!`<#YgZ>$jK+pZJ-vFx>2pygB5REZyBFov41$ft?D*#2q*UcKZ z`Zmb5z-on4!qp{IVo)FyK#R~DaMWIRr-?ABn1P}onG*u4gVQelh(^R46iH~5YQuFy z3)^Cu5sy+f)stYxHW!G&q!+}IN2CvZ83PIa1?$BT6thG^cw~9749l%Yc-3r;gY^yncSmpwY+8#2;I;m&-z_4e?G&qQ z%Z5}wd8q7IIxyG@7W%%uD&B$yr;iUJb?g2v=udSR>W8ws2%-tdR2lY6*Q96t#0aPF zURJ(@AV?AnM&zr#V4IKXVn)2OW+E-u_{rSJ1N%{IGRkim zvOQrTP=HW8Rfr1i%DGbQMVBYpn=X6b*f-p%;Fh8pc?ByqL0wpW-g_W3DoNb-+h_mr zh}LQ(-H&i;@`0)Q=fj1crW*VWxEBfA9$FC#gbf7=<3mvN}+cf7aceA^E ze&vqXpzCFT5uzsx{HTojB1@NO=b9eNg~r~rGVx~Hx8`O_3QLRC*BP(W&(Gxq4~;E9 zw-2~_e_Z+U*nPL8fjhKgiuotj#$W(9OMu%0_EoO&nqOxoB?kcW<>jq6`n31a2z@$q0V3kp3(9V-1#;_v?Jxa+mUi6oZ+8shZkGba}`JH{4+Y zvP0o};z&;Fz;#Cex$C?3c?h!9-lLXfIiURbcx=(Bto#OJ5&vO9yZyD3ANRE(R4?^A z-YsfN;EKV?q%bxuL332BSYmG5 zwo8$?Y)vgAO;y}4B#<SYhp7asc>G77bv4Km$x?@oS?b3T9hxkhB_qdlK4{ds*COHA zyB6Nm4xt>;)LSf;^>Oz<>sq1GtfIl3;7ou#VBG2<55-R*tfGnwh0Fi^lV66 z-HNHfsv;c;r&{S@tV9W#>>vgH6Zcd+?VAfuL@*3R&0&0!EHgj1o0;CXWYpRQ{wT(`#m zoNH?;9)^Rtr@*z;d`Xd4 z0~V{&=Kj_>BxQ}L^1?Im->+f(^*|-4DX0pNnREpPo9)zL4_!ew4&nVlHN6eOy7ot= zM#t95yjak}^ko!6tOze-F_8|jz7Y~C?wR|Q+t&lJAS^?~a6 z?@|&CD*?INfaz@jU9SxYVW6xTZ7#9{C*>#__c5fa&=EW6D%=DM&&QAs{R)sQY4;uP z=HEsf7$Mi|Cu4ezS341i*xO{_{fsV1mZ=(G5`jJl_dx9qqSyElK0UkY?LfeR(Se?D zflAU+&(wX&ENy6VK#>x%aBUQ0i~l2wJl2Q)V(9G@aN>ZyEdV15B1^x|Q@HWwtlUC= zdcPkD<*JZDIm=CXYU%i*N!~#nSp7KA7E?1t=Ii!AMAj$hb9VNh71YpBM#wj}HM*74 zJ?B1WR(|@r3OhTKw;uWTT+7G4(S@D_D_p1`__4Jbw4xgkZmHxim?S%bhev_2Zsu1M zDOURo2*MUz6WiRW!%flrL|FW=KNm;!`_6b{r|B>&jSE#$Bn);gH+>#PYi;iLIp<~l ziM@#kE{_=Kgfh?=E*r*w3Z6_pOw%*l+Dxs2K)eF2jK2l^uQa#D=rq%?BnaaYNs@c5 zv)J+LeK=4Wm6VJ*h4hM=E})uPw=97L;+Zr)T{)VHWqLWuB`D|6H0tH$cv^eDeq}IJ zqFHH1Sd>b7f1(&i)kuNR@CS_j7Pa3aV#@d4%S(Gr&72BIlf+s5Hlu#kKJwYqV+@tF zat4#7``y{&{m)nz@1w;txaV@N7C1){g*lif)`AJu&gPp*+27J@2t_|;nt}3QFUTCc zgjj45BTb2jXtSr82-?KlQ!UK&{w?2xJ#B#){M4bhA2sP<+faK&Vbg7*lSbUtBBb(I zWa$jyTq+o#R)VieMEl^7EcG0yMr*R3cY&4?hUuioe_zVH+(0C-)aYvBb4vj1e)rG5NhAL+R};p39uA`hpMjQeS^} zRK_3fR}a?uTnX{>f%#9CU(PH6L-4Kd&y9v#SBy5INb{+#an&--&-uODO8xY-dQOkH*Q3Gr!h)s5~pcI!f7l2d7m9km~;Z9P7q zG4wDAlRnz9aOb|}p0DHAwLoQyQ7H{iBIj)!^n9wP#@vf;LXxF!@95p8RCO+Y@m+b0 z8=baQ@=?{9#+IV#8=+OPx(NhWGQ;eW%9u-t+C=Pw(rqz0;j_1L;U>yG)h(uHB(F4G zfOpn+#DmLaMh!lNd3xEJ7SQNanoBlB!ff8#xf?xQ>0Fdp=Z^hA_BQifaBe%rRg_#ryixcyuHo^pFB zw2vS(E5T-Zl!k>)s?=~nhEMZ_R0>eGFHacpBwtw6-wia0g690^{)P}rKE9tuw5dP4 z;B+#2sMt{Kjc~nPk4YaNU^qs(I)JJn#@bI#>jkh>QdLl~IbL1e@b3mdM=-EY1a7*K zUcX>aTa(131(z(6w9~BUhu`&zK%O*e*WhAMeP4rjK6_*9vAzc=K53H)EiX~09V#}y z#scJly+=EcRqfAG6}`54*oei0yw=&)6Bq>%4<_zIBXofXrd%yUpNS9gjbDG#CdC=S zEX$r*QF#3MrQfG@g&<^CT3T0krfBzz3gT;1JwW+G+S+JEudQDTrK}-%k%#84NnW?( zMBV`+;JsjP;Kr7OLaYzxF8C%UUe1Qbig14KyhDH{%De7QEFs+KJ>NM7_cI}i|7(y{ zYE%a>iec`;1NZfb^2j3aSJA!^R{)P8QTzj@u)xbCkx(CXq6(yM-i*nCK1KlkS;BI2 z@R!-(*mg^_k%i3g6x*y$m+FYXds(SI!snQMFeXT!bsXf$uwdb^8uGVO85k`bC1f!; zkTttUtZZx{Ah86WBw@^Eo-S^u>nw>5_P+v7f`Ak9^Fo)v^t4C zfQKb&zwa*04P@E)^5YW6l!{IX^iSrKIC18(*;tm>iRp5WGz^r!iI8SR?|jdDYkc@q)( zn^FJLIamdb5)~F0rg1&4AdJwlMSNg0LHeoPol8edHRK$wp3b!mF@8jYJulP{iUrvz@$ z+$_$8yF)k&!PH}DqlNFsRc#G+8Zvu0>IdURazb6Dhr}-KA28UF5i_^DJeZ|#B_9~o z*})SUc>E}GPln6h4Y|dbXq#1{j+e3KGb@58E^BBh{PlJ4#WjH<<_N=ftPz|6*Hu`f zPNX=dl1UAM)*_ErZqz9v;Lx4qY1-K3ciHXj$Qb{hIjPnk`#$O`OPt$SXF&~>WAQn= z^3(n|Suryxq`BUQHwQ5TgsS<{bB~h~616BEk<>bvU2X!pe-4anF-9{N|%GM`y0|V4RrwN&ABd+ z?|BUjRS7{~0x~vhSblu*v{wZ2bg>|9C%!bE4_2Kd1_z-R=n^~$NK7+qe;TU z2dE-umbqtJ!ZUboN4|U!w6s_<0{Tkn=@sP-jB7`68J9at!qfW)WD_GnxX`H*ik7nR zx$pH19=px!J~unNaAtC5sMKn2e)j8rw@R`n4477KqGy5^Px<J*mAwe4-8~NxUo7IiQCsMyU|3Dt2OIsQR~XwGy?rIJJrez*8`4}V zt;MforK2Q}k6CYf;{u$4!R_a;i`1A+Ch3f_jre$=wytW9qW+L{T>;=BnSRHSq98vN zD?x8iL{ys?$40}iLmV@ug)`I#@@~#fT)AM2Z2jON(n|kloy3o3o|Rt&M{_I>@6bFv zsqqkhc*Cv}K<0QP6V2!R|7pU!i6T<3Ilbr%V%ri+<{*K*zi6*>8D1Mte0%uFw#}AWg%m04O;f{ zvD1MqL2Z8HZrFu(8um+;Kj{z>e;{uFz#a?$H20t&Z$D4k_Ry2Qhs{A?SwQwTAm`5+ zkoc;|<7_R8qRLYC|4xGdKz@~vAKRUxw%l711p&?{vHi_4>=p`i*~F(Hz(XW6~ISnRqX^ayUkv6IeNh!0e-%eOispy z8o1-v9e>qYIu?@IREI6Uy>%?va~4V~J*~ik*sTr-u(u@}OVS|zO7V*$`U9Fkkoo^}Ot3M-gBL3j54aHPK@GrMUk9bB&T{ diff --git a/apps/commander/commander.c b/apps/commander/commander.c index 8b9e7c49cd..4e2b4907ba 100644 --- a/apps/commander/commander.c +++ b/apps/commander/commander.c @@ -1319,7 +1319,7 @@ int commander_thread_main(int argc, char *argv[]) /* make sure we are in preflight state */ memset(¤t_status, 0, sizeof(current_status)); - current_status.navigation_state = NAVIGATION_STATE_STANDBY; + current_status.navigation_state = NAVIGATION_STATE_INIT; current_status.arming_state = ARMING_STATE_INIT; current_status.hil_state = HIL_STATE_OFF; current_status.flag_system_armed = false; @@ -1857,19 +1857,7 @@ int commander_thread_main(int argc, char *argv[]) } /* Now it's time to handle the stick inputs */ - - if (current_status.arming_state == ARMING_STATE_ARMED) { - - if (current_status.mode_switch == MODE_SWITCH_MANUAL) { - do_navigation_state_update(stat_pub, ¤t_status, mavlink_fd, NAVIGATION_STATE_MANUAL ); - } else if (current_status.mode_switch == MODE_SWITCH_SEATBELT) { - do_navigation_state_update(stat_pub, ¤t_status, mavlink_fd, NAVIGATION_STATE_SEATBELT ); - } else if (current_status.mode_switch == MODE_SWITCH_AUTO) { - if (current_status.navigation_state == NAVIGATION_STATE_MANUAL) { - do_navigation_state_update(stat_pub, ¤t_status, mavlink_fd, NAVIGATION_STATE_MISSION ); - } - } - } + navigation_state_update(stat_pub, ¤t_status, mavlink_fd); /* handle the case where RC signal was regained */ if (!current_status.rc_signal_found_once) { diff --git a/apps/commander/state_machine_helper.c b/apps/commander/state_machine_helper.c index 2e49354714..e6344f1a84 100644 --- a/apps/commander/state_machine_helper.c +++ b/apps/commander/state_machine_helper.c @@ -56,60 +56,195 @@ /** * Transition from one sytem state to another */ -void state_machine_update(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd) +void navigation_state_update(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd) { int ret = ERROR; - system_state_t new_system_state; + navigation_state_t new_navigation_state; + /* do the navigation state update depending on the arming state */ + switch (current_status->arming_state) { - /* - * Firstly evaluate mode switch position - * */ + /* evaluate the navigation state when disarmed */ + case ARMING_STATE_STANDBY: - /* Always accept manual mode */ - if (current_status->mode_switch == MODE_SWITCH_MANUAL) { - new_system_state = SYSTEM_STATE_MANUAL; + /* Always accept manual mode */ + if (current_status->mode_switch == MODE_SWITCH_MANUAL) { + new_navigation_state = NAVIGATION_STATE_MANUAL_STANDBY; - /* Accept SEATBELT if there is a local position estimate */ - } else if (current_status->mode_switch == MODE_SWITCH_SEATBELT) { + /* Accept SEATBELT if there is a local position estimate */ + } else if (current_status->mode_switch == MODE_SWITCH_SEATBELT) { - if (current_status->flag_local_position_valid) { - new_system_state = SYSTEM_STATE_SEATBELT; - } else { - /* or just fall back to manual */ - mavlink_log_critical(mavlink_fd, "REJ SEATBELT: no local position"); - new_system_state = SYSTEM_STATE_MANUAL); - } + if (current_status->flag_local_position_valid) { + new_navigation_state = NAVIGATION_STATE_SEATBELT_STANDBY; + } else { + /* or just fall back to manual */ + mavlink_log_critical(mavlink_fd, "Rej. SEATBELT: no local position"); + new_navigation_state = NAVIGATION_STATE_MANUAL_STANDBY; + } - /* Accept AUTO if there is a global position estimate */ - } else if (current_status->mode_switch == MODE_SWITCH_AUTO) { - if (current_status->flag_local_position_valid) { - new_system_state = SYSTEM_STATE_SEATBELT); - } else { - mavlink_log_critical(mavlink_fd, "REJ AUTO: no global position"); + /* Accept AUTO if there is a global position estimate */ + } else if (current_status->mode_switch == MODE_SWITCH_AUTO) { + if (current_status->flag_local_position_valid) { + new_navigation_state = NAVIGATION_STATE_SEATBELT_STANDBY; + } else { + mavlink_log_critical(mavlink_fd, "Rej. AUTO: no global position"); - /* otherwise fallback to seatbelt or even manual */ - if (current_status->flag_local_position_valid) { - new_system_state = SYSTEM_STATE_SEATBELT; - } else { - mavlink_log_critical(mavlink_fd, "REJ SEATBELT: no local position"); - new_system_state = SYSTEM_STATE_MANUAL; + /* otherwise fallback to seatbelt or even manual */ + if (current_status->flag_local_position_valid) { + new_navigation_state = NAVIGATION_STATE_SEATBELT_STANDBY; + } else { + mavlink_log_critical(mavlink_fd, "Rej. SEATBELT: no local position"); + new_navigation_state = NAVIGATION_STATE_MANUAL_STANDBY; + } + } } - } - } - /* - * Next up, check for - */ + break; + + /* evaluate the navigation state when armed */ + case ARMING_STATE_ARMED: + + /* Always accept manual mode */ + if (current_status->mode_switch == MODE_SWITCH_MANUAL) { + new_navigation_state = NAVIGATION_STATE_MANUAL; + + /* Accept SEATBELT if there is a local position estimate */ + } else if (current_status->mode_switch == MODE_SWITCH_SEATBELT + && current_status->return_switch == RETURN_SWITCH_NONE) { + + if (current_status->flag_local_position_valid) { + new_navigation_state = NAVIGATION_STATE_SEATBELT; + } else { + /* or just fall back to manual */ + mavlink_log_critical(mavlink_fd, "Rej. SEATBELT: no local position"); + new_navigation_state = NAVIGATION_STATE_MANUAL; + } + + /* Accept SEATBELT_DESCENT if there is a local position estimate and the return switch is on */ + } else if (current_status->mode_switch == MODE_SWITCH_SEATBELT + && current_status->return_switch == RETURN_SWITCH_RETURN) { + + if (current_status->flag_local_position_valid) { + new_navigation_state = NAVIGATION_STATE_SEATBELT_DESCENT; + } else { + /* descent not possible without baro information, fall back to manual */ + mavlink_log_critical(mavlink_fd, "Rej. SEATBELT_DESCENT: no local position"); + new_navigation_state = NAVIGATION_STATE_MANUAL; + } + + /* Accept LOITER if there is a global position estimate */ + } else if (current_status->mode_switch == MODE_SWITCH_AUTO + && current_status->return_switch == RETURN_SWITCH_NONE + && current_status->mission_switch == MISSION_SWITCH_NONE) { + + if (current_status->flag_global_position_valid) { + new_navigation_state = NAVIGATION_STATE_AUTO_LOITER; + } else { + mavlink_log_critical(mavlink_fd, "Rej. LOITER: no global position"); + + /* otherwise fallback to SEATBELT or even manual */ + if (current_status->flag_local_position_valid) { + new_navigation_state = NAVIGATION_STATE_SEATBELT; + } else { + mavlink_log_critical(mavlink_fd, "Rej. SEATBELT: no local position"); + new_navigation_state = NAVIGATION_STATE_MANUAL; + } + } + + /* Accept MISSION if there is a global position estimate and home position */ + } else if (current_status->mode_switch == MODE_SWITCH_AUTO + && current_status->return_switch == RETURN_SWITCH_NONE + && current_status->mission_switch == MISSION_SWITCH_MISSION) { + + if (current_status->flag_global_position_valid && current_status->flag_valid_home_position) { + new_navigation_state = NAVIGATION_STATE_AUTO_MISSION; + } else { + + /* spit out what exactly is unavailable */ + if (current_status->flag_global_position_valid) { + mavlink_log_critical(mavlink_fd, "Rej. MISSION: no home position"); + } else if (current_status->flag_valid_home_position) { + mavlink_log_critical(mavlink_fd, "Rej. MISSION: no global position"); + } else { + mavlink_log_critical(mavlink_fd, "Rej. MISSION: no global position and no home position"); + } + + /* otherwise fallback to SEATBELT or even manual */ + if (current_status->flag_local_position_valid) { + new_navigation_state = NAVIGATION_STATE_SEATBELT; + } else { + mavlink_log_critical(mavlink_fd, "Rej. SEATBELT: no local position"); + new_navigation_state = NAVIGATION_STATE_MANUAL; + } + } + + /* Go to RTL or land if global position estimate and home position is available */ + } else if (current_status->mode_switch == MODE_SWITCH_AUTO + && current_status->return_switch == RETURN_SWITCH_RETURN + && (current_status->mission_switch == MISSION_SWITCH_NONE || current_status->mission_switch == MISSION_SWITCH_MISSION)) { + + if (current_status->flag_global_position_valid && current_status->flag_valid_home_position) { + + /* after RTL go to LAND */ + if (current_status->flag_system_returned_to_home) { + new_navigation_state = NAVIGATION_STATE_AUTO_LAND; + } else { + new_navigation_state = NAVIGATION_STATE_AUTO_RTL; + } + + } else { + + /* spit out what exactly is unavailable */ + if (current_status->flag_global_position_valid) { + mavlink_log_critical(mavlink_fd, "Rej. RTL/LAND: no home position"); + } else if (current_status->flag_valid_home_position) { + mavlink_log_critical(mavlink_fd, "Rej. RTL/LAND: no global position"); + } else { + mavlink_log_critical(mavlink_fd, "Rej. RTL/LAND: no global position and no home position"); + } + + /* otherwise fallback to SEATBELT_DESCENT or even MANUAL */ + if (current_status->flag_local_position_valid) { + new_navigation_state = NAVIGATION_STATE_SEATBELT_DESCENT; + } else { + mavlink_log_critical(mavlink_fd, "Rej. SEATBELT: no local position"); + new_navigation_state = NAVIGATION_STATE_MANUAL; + } + } + } + break; + + case ARMING_STATE_ARMED_ERROR: + + // TODO work out fail-safe scenarios + break; + + case ARMING_STATE_STANDBY_ERROR: + + // TODO work out fail-safe scenarios + break; + + case ARMING_STATE_REBOOT: + + // XXX I don't think we should end up here + break; + + case ARMING_STATE_IN_AIR_RESTORE: + + // XXX not sure what to do here + break; + default: + break; + } /* Update the system state in case it has changed */ - if (current_status->system_state != new_system_state) { + if (current_status->navigation_state != new_navigation_state) { /* Check if the transition is valid */ - if (system_state_update(current_status->system_state, new_system_state) == OK) { - current_status->system_state = new_system_state; + if (check_navigation_state_transition(current_status->navigation_state, new_navigation_state) == OK) { + current_status->navigation_state = new_navigation_state; state_machine_publish(status_pub, current_status, mavlink_fd); } else { mavlink_log_critical(mavlink_fd, "System state transition not valid"); @@ -123,74 +258,164 @@ void state_machine_update(int status_pub, struct vehicle_status_s *current_statu * This functions does not evaluate any input flags but only checks if the transitions * are valid. */ -int system_state_update(system_state_t current_system_state, system_state_t new_system_state) { +int check_navigation_state_transition(navigation_state_t current_navigation_state, navigation_state_t new_navigation_state) { int ret = ERROR; /* only check transition if the new state is actually different from the current one */ - if (new_system_state != current_system_state) { + if (new_navigation_state != current_navigation_state) { - switch (new_system_state) { - case SYSTEM_STATE_INIT: + switch (new_navigation_state) { + case NAVIGATION_STATE_INIT: /* transitions back to INIT are possible for calibration */ - if (current_system_state == SYSTEM_STATE_MANUAL - || current_system_state == SYSTEM_STATE_SEATBELT - || current_system_state == SYSTEM_STATE_AUTO) { - - ret = OK; - } - - break; - - case SYSTEM_STATE_MANUAL: - - /* transitions from INIT or from other modes */ - if (current_system_state == SYSTEM_STATE_INIT - || current_system_state == SYSTEM_STATE_SEATBELT - || current_system_state == SYSTEM_STATE_AUTO) { - - ret = OK; - } - - break; - - case SYSTEM_STATE_SEATBELT: - - /* transitions from INIT or from other modes */ - if (current_system_state == SYSTEM_STATE_INIT - || current_system_state == SYSTEM_STATE_MANUAL - || current_system_state == SYSTEM_STATE_AUTO) { - - ret = OK; - } - - break; - - case SYSTEM_STATE_AUTO: - - /* transitions from INIT or from other modes */ - if (current_system_state == SYSTEM_STATE_INIT - || current_system_state == SYSTEM_STATE_MANUAL - || current_system_state == SYSTEM_STATE_SEATBELT) { + if (current_navigation_state == NAVIGATION_STATE_MANUAL_STANDBY + || current_navigation_state == NAVIGATION_STATE_SEATBELT_STANDBY + || current_navigation_state == NAVIGATION_STATE_AUTO_STANDBY) { ret = OK; } break; - case SYSTEM_STATE_REBOOT: + case NAVIGATION_STATE_MANUAL_STANDBY: - /* from INIT you can go straight to REBOOT */ - if (current_system_state == SYSTEM_STATE_INIT) { + /* transitions from INIT and other STANDBY states as well as MANUAL are possible */ + if (current_navigation_state == NAVIGATION_STATE_INIT + || current_navigation_state == NAVIGATION_STATE_SEATBELT_STANDBY + || current_navigation_state == NAVIGATION_STATE_AUTO_STANDBY + || current_navigation_state == NAVIGATION_STATE_MANUAL) { ret = OK; } break; - case SYSTEM_STATE_IN_AIR_RESTORE: + case NAVIGATION_STATE_MANUAL: - /* from INIT you can go straight to an IN AIR RESTORE */ - if (current_system_state == SYSTEM_STATE_INIT) { + /* all transitions possible */ + ret = OK; + break; + + case NAVIGATION_STATE_SEATBELT_STANDBY: + + /* transitions from INIT and other STANDBY states as well as SEATBELT and SEATBELT_DESCENT are possible */ + if (current_navigation_state == NAVIGATION_STATE_INIT + || current_navigation_state == NAVIGATION_STATE_MANUAL_STANDBY + || current_navigation_state == NAVIGATION_STATE_AUTO_STANDBY + || current_navigation_state == NAVIGATION_STATE_SEATBELT + || current_navigation_state == NAVIGATION_STATE_SEATBELT_DESCENT) { + + ret = OK; + } + break; + + case NAVIGATION_STATE_SEATBELT: + + /* transitions from all AUTO modes except AUTO_STANDBY and except MANUAL_STANDBY and INIT*/ + if (current_navigation_state == NAVIGATION_STATE_SEATBELT_STANDBY + || current_navigation_state == NAVIGATION_STATE_SEATBELT_DESCENT + || current_navigation_state == NAVIGATION_STATE_MANUAL + || current_navigation_state == NAVIGATION_STATE_AUTO_LAND + || current_navigation_state == NAVIGATION_STATE_AUTO_LOITER + || current_navigation_state == NAVIGATION_STATE_AUTO_MISSION + || current_navigation_state == NAVIGATION_STATE_AUTO_READY + || current_navigation_state == NAVIGATION_STATE_AUTO_RTL + || current_navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF) { + + ret = OK; + } + break; + + case NAVIGATION_STATE_SEATBELT_DESCENT: + + /* transitions from all AUTO modes except AUTO_STANDBY and except MANUAL_STANDBY and INIT and SEATBELT_STANDBY */ + if (current_navigation_state == NAVIGATION_STATE_SEATBELT + || current_navigation_state == NAVIGATION_STATE_MANUAL + || current_navigation_state == NAVIGATION_STATE_AUTO_LAND + || current_navigation_state == NAVIGATION_STATE_AUTO_LOITER + || current_navigation_state == NAVIGATION_STATE_AUTO_MISSION + || current_navigation_state == NAVIGATION_STATE_AUTO_READY + || current_navigation_state == NAVIGATION_STATE_AUTO_RTL + || current_navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF) { + + ret = OK; + } + break; + + case NAVIGATION_STATE_AUTO_STANDBY: + + /* transitions from INIT or from other STANDBY modes or from AUTO READY */ + if (current_navigation_state == NAVIGATION_STATE_INIT + || current_navigation_state == NAVIGATION_STATE_MANUAL_STANDBY + || current_navigation_state == NAVIGATION_STATE_SEATBELT_STANDBY + || current_navigation_state == NAVIGATION_STATE_AUTO_READY) { + + ret = OK; + } + break; + + case NAVIGATION_STATE_AUTO_READY: + + /* transitions from AUTO_STANDBY or AUTO_LAND */ + if (current_navigation_state == NAVIGATION_STATE_AUTO_STANDBY + || current_navigation_state == NAVIGATION_STATE_AUTO_LAND) { + + ret = OK; + } + break; + + case NAVIGATION_STATE_AUTO_TAKEOFF: + + /* only transitions from AUTO_READY */ + if (current_navigation_state == NAVIGATION_STATE_AUTO_READY) { + + ret = OK; + } + break; + + case NAVIGATION_STATE_AUTO_LOITER: + + /* from everywhere flying except AUTO_LAND and SEATBELT_DESCENT */ + if (current_navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF + || current_navigation_state == NAVIGATION_STATE_AUTO_MISSION + || current_navigation_state == NAVIGATION_STATE_AUTO_RTL + || current_navigation_state == NAVIGATION_STATE_SEATBELT + || current_navigation_state == NAVIGATION_STATE_MANUAL) { + + ret = OK; + } + break; + + case NAVIGATION_STATE_AUTO_MISSION: + + /* from everywhere flying except AUTO_LAND and SEATBELT_DESCENT */ + if (current_navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF + || current_navigation_state == NAVIGATION_STATE_AUTO_LOITER + || current_navigation_state == NAVIGATION_STATE_AUTO_RTL + || current_navigation_state == NAVIGATION_STATE_SEATBELT + || current_navigation_state == NAVIGATION_STATE_MANUAL) { + + ret = OK; + } + break; + + case NAVIGATION_STATE_AUTO_RTL: + + /* from everywhere flying except AUTO_LAND and SEATBELT_DESCENT */ + if (current_navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF + || current_navigation_state == NAVIGATION_STATE_AUTO_MISSION + || current_navigation_state == NAVIGATION_STATE_AUTO_LOITER + || current_navigation_state == NAVIGATION_STATE_SEATBELT + || current_navigation_state == NAVIGATION_STATE_MANUAL) { + + ret = OK; + } + break; + + case NAVIGATION_STATE_AUTO_LAND: + /* after AUTO_RTL or when in AUTO_LOITER or AUTO_MISSION */ + if (current_navigation_state == NAVIGATION_STATE_AUTO_RTL + || current_navigation_state == NAVIGATION_STATE_AUTO_MISSION + || current_navigation_state == NAVIGATION_STATE_AUTO_LOITER) { ret = OK; } diff --git a/apps/commander/state_machine_helper.h b/apps/commander/state_machine_helper.h index 57b3db8f19..1c0564d077 100644 --- a/apps/commander/state_machine_helper.h +++ b/apps/commander/state_machine_helper.h @@ -47,10 +47,11 @@ #include #include -int navigation_state_update(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, navigation_state_t new_state); +void navigation_state_update(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd); //int do_arming_state_update(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, arming_state_t new_state); void state_machine_publish(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd); +int check_navigation_state_transition(navigation_state_t current_navigation_state, navigation_state_t new_navigation_state); #endif /* STATE_MACHINE_HELPER_H_ */ diff --git a/apps/controllib/fixedwing.cpp b/apps/controllib/fixedwing.cpp index 9a69195352..584ca2306a 100644 --- a/apps/controllib/fixedwing.cpp +++ b/apps/controllib/fixedwing.cpp @@ -294,8 +294,9 @@ void BlockMultiModeBacksideAutopilot::update() for (unsigned i = 4; i < NUM_ACTUATOR_CONTROLS; i++) _actuators.control[i] = 0.0f; +#warning this if is incomplete, should be based on flags // only update guidance in auto mode - if (_status.navigation_state == NAVIGATION_STATE_MISSION) { + if (_status.navigation_state == NAVIGATION_STATE_AUTO_MISSION) { // update guidance _guide.update(_pos, _att, _posCmd, _lastPosCmd); } @@ -304,8 +305,9 @@ void BlockMultiModeBacksideAutopilot::update() // once the system switches from manual or auto to stabilized // the setpoint should update to loitering around this position +#warning should be base on flags // handle autopilot modes - if (_status.navigation_state == NAVIGATION_STATE_MISSION || + if (_status.navigation_state == NAVIGATION_STATE_AUTO_MISSION || _status.navigation_state == NAVIGATION_STATE_MANUAL) { // update guidance @@ -340,7 +342,7 @@ void BlockMultiModeBacksideAutopilot::update() _actuators.control[CH_THR] : _manual.throttle; } - +#warning should be base on flags } else if (_status.navigation_state == NAVIGATION_STATE_MANUAL) { // if (_status.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_DIRECT) { diff --git a/apps/mavlink/mavlink.c b/apps/mavlink/mavlink.c index e25f1be27f..34b267d56b 100644 --- a/apps/mavlink/mavlink.c +++ b/apps/mavlink/mavlink.c @@ -205,36 +205,40 @@ get_mavlink_mode_and_state(uint8_t *mavlink_state, uint8_t *mavlink_mode) } /* autonomous mode */ - if (v_status.navigation_state == NAVIGATION_STATE_MISSION - || v_status.navigation_state == NAVIGATION_STATE_TAKEOFF - || v_status.navigation_state == NAVIGATION_STATE_RTL - || v_status.navigation_state == NAVIGATION_STATE_LAND - || v_status.navigation_state == NAVIGATION_STATE_AUTO_READY) { + if (v_status.navigation_state == NAVIGATION_STATE_AUTO_LAND + || v_status.navigation_state == NAVIGATION_STATE_AUTO_LOITER + || v_status.navigation_state == NAVIGATION_STATE_AUTO_MISSION + || v_status.navigation_state == NAVIGATION_STATE_AUTO_READY + || v_status.navigation_state == NAVIGATION_STATE_AUTO_RTL + || v_status.navigation_state == NAVIGATION_STATE_AUTO_STANDBY + || v_status.navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF) { *mavlink_mode |= MAV_MODE_FLAG_AUTO_ENABLED; } /* set arming state */ - if (v_status.flag_system_armed) { + if (v_status.arming_state == ARMING_STATE_ARMED + || v_status.arming_state == ARMING_STATE_ARMED_ERROR) { *mavlink_mode |= MAV_MODE_FLAG_SAFETY_ARMED; } else { *mavlink_mode &= ~MAV_MODE_FLAG_SAFETY_ARMED; } - if (v_status.navigation_state == NAVIGATION_STATE_MANUAL) { + if (v_status.navigation_state == NAVIGATION_STATE_MANUAL + || v_status.navigation_state == NAVIGATION_STATE_MANUAL_STANDBY) { *mavlink_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED; } - if (v_status.navigation_state == NAVIGATION_STATE_SEATBELT) { - - *mavlink_mode |= MAV_MODE_FLAG_DECODE_POSITION_GUIDED; - } - - if (v_status.navigation_state == NAVIGATION_STATE_SEATBELT) { - - *mavlink_mode |= MAV_MODE_FLAG_DECODE_POSITION_GUIDED; - } +// if (v_status.system_state == NAVIGATION_STATE_SEATBELT) { +// +// *mavlink_mode |= MAV_MODE_FLAG_DECODE_POSITION_GUIDED; +// } +// +// if (v_status.system_state == NAVIGATION_STATE_SEATBELT) { +// +// *mavlink_mode |= MAV_MODE_FLAG_DECODE_POSITION_GUIDED; +// } /** @@ -248,19 +252,30 @@ get_mavlink_mode_and_state(uint8_t *mavlink_state, uint8_t *mavlink_mode) *mavlink_state = MAV_STATE_CALIBRATING; - } else if (v_status.arming_state == ARMING_STATE_ERROR || v_status.arming_state == ARMING_STATE_ABORT) { + } else if (v_status.flag_system_emergency) { *mavlink_state = MAV_STATE_EMERGENCY; - } else if (v_status.arming_state == ARMING_STATE_ARMED || v_status.arming_state == ARMING_STATE_IN_AIR_RESTORE) { + // XXX difference between active and armed? is AUTO_READY active? + } else if (v_status.navigation_state == NAVIGATION_STATE_AUTO_LAND + || v_status.navigation_state == NAVIGATION_STATE_AUTO_LOITER + || v_status.navigation_state == NAVIGATION_STATE_AUTO_MISSION + || v_status.navigation_state == NAVIGATION_STATE_AUTO_RTL + || v_status.navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF + || v_status.navigation_state == NAVIGATION_STATE_SEATBELT + || v_status.navigation_state == NAVIGATION_STATE_SEATBELT_DESCENT + || v_status.navigation_state == NAVIGATION_STATE_MANUAL) { *mavlink_state = MAV_STATE_ACTIVE; - } else if (v_status.arming_state == ARMING_STATE_STANDBY) { + } else if (v_status.navigation_state == NAVIGATION_STATE_AUTO_STANDBY + || v_status.navigation_state == NAVIGATION_STATE_AUTO_READY // XXX correct? + || v_status.navigation_state == NAVIGATION_STATE_SEATBELT_STANDBY + || v_status.navigation_state == NAVIGATION_STATE_MANUAL_STANDBY) { *mavlink_state = MAV_STATE_STANDBY; - } else if (v_status.arming_state == ARMING_STATE_INIT) { + } else if (v_status.navigation_state == NAVIGATION_STATE_INIT) { *mavlink_state = MAV_STATE_UNINIT; } else { diff --git a/apps/uORB/topics/vehicle_status.h b/apps/uORB/topics/vehicle_status.h index 20cb25cc02..29baff34bd 100644 --- a/apps/uORB/topics/vehicle_status.h +++ b/apps/uORB/topics/vehicle_status.h @@ -60,13 +60,20 @@ /* State Machine */ typedef enum { - SYSTEM_STATE_INIT=0, - SYSTEM_STATE_MANUAL, - SYSTEM_STATE_SEATBELT, - SYSTEM_STATE_AUTO, - SYSTEM_STATE_REBOOT, - SYSTEM_STATE_IN_AIR_RESTORE -} system_state_t; + NAVIGATION_STATE_INIT=0, + NAVIGATION_STATE_MANUAL_STANDBY, + NAVIGATION_STATE_MANUAL, + NAVIGATION_STATE_SEATBELT_STANDBY, + NAVIGATION_STATE_SEATBELT, + NAVIGATION_STATE_SEATBELT_DESCENT, + NAVIGATION_STATE_AUTO_STANDBY, + NAVIGATION_STATE_AUTO_READY, + NAVIGATION_STATE_AUTO_TAKEOFF, + NAVIGATION_STATE_AUTO_LOITER, + NAVIGATION_STATE_AUTO_MISSION, + NAVIGATION_STATE_AUTO_RTL, + NAVIGATION_STATE_AUTO_LAND +} navigation_state_t; typedef enum { MANUAL_STANDBY = 0, @@ -75,50 +82,20 @@ typedef enum { } manual_state_t; typedef enum { - SEATBELT_STANDBY, - SEATBELT_READY, - SEATBELT, - SEATBELT_ASCENT, - SEATBELT_DESCENT, -} seatbelt_state_t; - -typedef enum { - AUTO_STANDBY, - AUTO_READY, - AUTO_LOITER, - AUTO_MISSION, - AUTO_RTL, - AUTO_TAKEOFF, - AUTO_LAND, -} auto_state_t; - -//typedef enum { -// ARMING_STATE_INIT = 0, -// ARMING_STATE_STANDBY, -// ARMING_STATE_ARMED_GROUND, -// ARMING_STATE_ARMED_AIRBORNE, -// ARMING_STATE_ERROR_GROUND, -// ARMING_STATE_ERROR_AIRBORNE, -// ARMING_STATE_REBOOT, -// ARMING_STATE_IN_AIR_RESTORE -//} arming_state_t; + ARMING_STATE_INIT = 0, + ARMING_STATE_STANDBY, + ARMING_STATE_ARMED, + ARMING_STATE_ARMED_ERROR, + ARMING_STATE_STANDBY_ERROR, + ARMING_STATE_REBOOT, + ARMING_STATE_IN_AIR_RESTORE +} arming_state_t; typedef enum { HIL_STATE_OFF = 0, HIL_STATE_ON } hil_state_t; -enum VEHICLE_MODE_FLAG { - VEHICLE_MODE_FLAG_SAFETY_ARMED = 128, - VEHICLE_MODE_FLAG_MANUAL_INPUT_ENABLED = 64, - VEHICLE_MODE_FLAG_HIL_ENABLED = 32, - VEHICLE_MODE_FLAG_STABILIZED_ENABLED = 16, - VEHICLE_MODE_FLAG_GUIDED_ENABLED = 8, - VEHICLE_MODE_FLAG_AUTO_ENABLED = 4, - VEHICLE_MODE_FLAG_TEST_ENABLED = 2, - VEHICLE_MODE_FLAG_CUSTOM_MODE_ENABLED = 1 -}; /**< Same as MAV_MODE_FLAG of MAVLink 1.0 protocol */ - typedef enum { MODE_SWITCH_MANUAL = 0, MODE_SWITCH_SEATBELT, @@ -135,6 +112,17 @@ typedef enum { MISSION_SWITCH_MISSION } mission_switch_pos_t; +enum VEHICLE_MODE_FLAG { + VEHICLE_MODE_FLAG_SAFETY_ARMED = 128, + VEHICLE_MODE_FLAG_MANUAL_INPUT_ENABLED = 64, + VEHICLE_MODE_FLAG_HIL_ENABLED = 32, + VEHICLE_MODE_FLAG_STABILIZED_ENABLED = 16, + VEHICLE_MODE_FLAG_GUIDED_ENABLED = 8, + VEHICLE_MODE_FLAG_AUTO_ENABLED = 4, + VEHICLE_MODE_FLAG_TEST_ENABLED = 2, + VEHICLE_MODE_FLAG_CUSTOM_MODE_ENABLED = 1 +}; /**< Same as MAV_MODE_FLAG of MAVLink 1.0 protocol */ + /** * Should match 1:1 MAVLink's MAV_TYPE ENUM */ @@ -181,8 +169,8 @@ struct vehicle_status_s uint64_t failsave_lowlevel_start_time; /**< time when the lowlevel failsafe flag was set */ // uint64_t failsave_highlevel_begin; TO BE COMPLETED - system_state_t system_state; /**< current system state */ -// arming_state_t arming_state; /**< current arming state */ + navigation_state_t navigation_state; /**< current system state */ + arming_state_t arming_state; /**< current arming state */ hil_state_t hil_state; /**< current hil state */ int32_t system_type; /**< system type, inspired by MAVLink's VEHICLE_TYPE enum */ @@ -203,7 +191,9 @@ struct vehicle_status_s bool flag_system_arming_requested; bool flag_system_disarming_requested; bool flag_system_reboot_requested; - bool flag_system_on_ground; + bool flag_system_returned_to_home; + + bool flag_auto_enabled; bool flag_control_manual_enabled; /**< true if manual input is mixed in */ bool flag_control_offboard_enabled; /**< true if offboard control input is on */ From c056410f8439e760905cb50fe08c49c3a0b344e5 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Thu, 21 Feb 2013 18:10:34 -0800 Subject: [PATCH 009/109] Checkpoint: Added arming check function --- apps/commander/state_machine_helper.c | 55 +++++++++++++++++++++++++++ apps/commander/state_machine_helper.h | 2 + 2 files changed, 57 insertions(+) diff --git a/apps/commander/state_machine_helper.c b/apps/commander/state_machine_helper.c index e6344f1a84..0611bb52c2 100644 --- a/apps/commander/state_machine_helper.c +++ b/apps/commander/state_machine_helper.c @@ -254,6 +254,61 @@ void navigation_state_update(int status_pub, struct vehicle_status_s *current_st return; } +int check_arming_state_transition(arming_state_t current_arming_state, arming_state_t new_arming_state) { + + int ret = ERROR; + + /* only check transition if the new state is actually different from the current one */ + if (new_arming_state != current_arming_state) { + + switch (new_arming_state) { + case ARMING_STATE_INIT: + + /* allow going back from INIT for calibration */ + if (current_arming_state == ARMING_STATE_STANDBY) { + ret = OK; + } + break; + case ARMING_STATE_STANDBY: + + /* allow coming from INIT and disarming from ARMED */ + if (current_arming_state == ARMING_STATE_INIT + || current_arming_state == ARMING_STATE_ARMED) { + ret = OK; + } + break; + case ARMING_STATE_ARMED: + + /* allow arming from STANDBY and IN-AIR-RESTORE */ + if (current_arming_state == ARMING_STATE_STANDBY + || current_arming_state == ARMING_STATE_IN_AIR_RESTORE) { + ret = OK; + } + break; + case ARMING_STATE_ARMED_ERROR: + + /* an armed error happens when ARMED obviously */ + if (current_arming_state == ARMING_STATE_ARMED) { + ret = OK; + } + break; + case ARMING_STATE_STANDBY_ERROR: + /* a disarmed error happens when in STANDBY or in INIT or after ARMED_ERROR */ + if (current_arming_state == ARMING_STATE_STANDBY + || current_arming_state == ARMING_STATE_INIT + || current_arming_state == ARMING_STATE_ARMED_ERROR) { + ret = OK; + } + break; + default: + break; + } + } + return ret; +} + + + /* * This functions does not evaluate any input flags but only checks if the transitions * are valid. diff --git a/apps/commander/state_machine_helper.h b/apps/commander/state_machine_helper.h index 1c0564d077..cf1fa80cd5 100644 --- a/apps/commander/state_machine_helper.h +++ b/apps/commander/state_machine_helper.h @@ -54,4 +54,6 @@ void navigation_state_update(int status_pub, struct vehicle_status_s *current_st void state_machine_publish(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd); int check_navigation_state_transition(navigation_state_t current_navigation_state, navigation_state_t new_navigation_state); +int check_arming_state_transition(arming_state_t current_arming_state, arming_state_t new_arming_state); + #endif /* STATE_MACHINE_HELPER_H_ */ From be7aeb754b85016e2609508d1c059797d5068ec1 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Thu, 21 Feb 2013 20:01:54 -0800 Subject: [PATCH 010/109] Checkpoint: Added flag checks inside arming state update --- apps/commander/state_machine_helper.c | 35 ++++++++++++++++++--------- apps/commander/state_machine_helper.h | 2 +- 2 files changed, 24 insertions(+), 13 deletions(-) diff --git a/apps/commander/state_machine_helper.c b/apps/commander/state_machine_helper.c index 0611bb52c2..e5ef00e934 100644 --- a/apps/commander/state_machine_helper.c +++ b/apps/commander/state_machine_helper.c @@ -254,49 +254,60 @@ void navigation_state_update(int status_pub, struct vehicle_status_s *current_st return; } -int check_arming_state_transition(arming_state_t current_arming_state, arming_state_t new_arming_state) { +int check_arming_state_transition(struct vehicle_status_s *current_state, arming_state_t new_arming_state, const int mavlink_fd) { int ret = ERROR; /* only check transition if the new state is actually different from the current one */ - if (new_arming_state != current_arming_state) { + if (new_arming_state != current_state->arming_state) { switch (new_arming_state) { case ARMING_STATE_INIT: /* allow going back from INIT for calibration */ - if (current_arming_state == ARMING_STATE_STANDBY) { + if (current_state->arming_state == ARMING_STATE_STANDBY) { ret = OK; } break; case ARMING_STATE_STANDBY: /* allow coming from INIT and disarming from ARMED */ - if (current_arming_state == ARMING_STATE_INIT - || current_arming_state == ARMING_STATE_ARMED) { - ret = OK; + if (current_state->arming_state == ARMING_STATE_INIT + || current_state->arming_state == ARMING_STATE_ARMED) { + + /* sensors need to be initialized for STANDBY state */ + if (current_state->flag_system_sensors_initialized) { + ret = OK; + } else { + mavlink_log_critical(mavlink_fd, "Rej. STANDBY state, sensors not initialized"); + } + } break; case ARMING_STATE_ARMED: /* allow arming from STANDBY and IN-AIR-RESTORE */ - if (current_arming_state == ARMING_STATE_STANDBY - || current_arming_state == ARMING_STATE_IN_AIR_RESTORE) { + if (current_state->arming_state == ARMING_STATE_STANDBY + || current_state->arming_state == ARMING_STATE_IN_AIR_RESTORE) { + + /* XXX conditions for arming? */ ret = OK; } break; case ARMING_STATE_ARMED_ERROR: /* an armed error happens when ARMED obviously */ - if (current_arming_state == ARMING_STATE_ARMED) { + if (current_state->arming_state == ARMING_STATE_ARMED) { ret = OK; + + /* XXX conditions for an error state? */ } break; case ARMING_STATE_STANDBY_ERROR: /* a disarmed error happens when in STANDBY or in INIT or after ARMED_ERROR */ - if (current_arming_state == ARMING_STATE_STANDBY - || current_arming_state == ARMING_STATE_INIT - || current_arming_state == ARMING_STATE_ARMED_ERROR) { + if (current_state->arming_state == ARMING_STATE_STANDBY + || current_state->arming_state == ARMING_STATE_INIT + || current_state->arming_state == ARMING_STATE_ARMED_ERROR) { ret = OK; } break; diff --git a/apps/commander/state_machine_helper.h b/apps/commander/state_machine_helper.h index cf1fa80cd5..f2928db098 100644 --- a/apps/commander/state_machine_helper.h +++ b/apps/commander/state_machine_helper.h @@ -54,6 +54,6 @@ void navigation_state_update(int status_pub, struct vehicle_status_s *current_st void state_machine_publish(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd); int check_navigation_state_transition(navigation_state_t current_navigation_state, navigation_state_t new_navigation_state); -int check_arming_state_transition(arming_state_t current_arming_state, arming_state_t new_arming_state); +int check_arming_state_transition(struct vehicle_status_s current_state, arming_state_t new_arming_state); #endif /* STATE_MACHINE_HELPER_H_ */ From 36f9f8082e1ac676994cab0a6ee2c7a8344b0216 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Fri, 22 Feb 2013 09:32:49 -0800 Subject: [PATCH 011/109] Checkpoint: Added flag checks inside navigation state update --- apps/commander/commander.c | 40 +- apps/commander/state_machine_helper.c | 838 ++++++++++++-------------- apps/commander/state_machine_helper.h | 8 +- apps/uORB/topics/vehicle_status.h | 1 + 4 files changed, 426 insertions(+), 461 deletions(-) diff --git a/apps/commander/commander.c b/apps/commander/commander.c index 4e2b4907ba..953ba7a1e9 100644 --- a/apps/commander/commander.c +++ b/apps/commander/commander.c @@ -804,7 +804,7 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta /* request to activate HIL */ if ((int)cmd->param1 & VEHICLE_MODE_FLAG_HIL_ENABLED) { - if (OK == do_hil_state_update(status_pub, current_vehicle_status, mavlink_fd, HIL_STATE_ON)) { + if (OK == hil_state_transition(status_pub, current_vehicle_status, mavlink_fd, HIL_STATE_ON)) { result = VEHICLE_CMD_RESULT_ACCEPTED; } else { result = VEHICLE_CMD_RESULT_DENIED; @@ -812,13 +812,13 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta } if ((int)cmd->param1 & VEHICLE_MODE_FLAG_SAFETY_ARMED) { - if (OK == do_arming_state_update(status_pub, current_vehicle_status, mavlink_fd, ARMING_STATE_ARMED)) { + if (OK == arming_state_transition(status_pub, current_vehicle_status, mavlink_fd, ARMING_STATE_ARMED)) { result = VEHICLE_CMD_RESULT_ACCEPTED; } else { result = VEHICLE_CMD_RESULT_DENIED; } } else { - if (OK == do_arming_state_update(status_pub, current_vehicle_status, mavlink_fd, ARMING_STATE_STANDBY)) { + if (OK == arming_state_transition(status_pub, current_vehicle_status, mavlink_fd, ARMING_STATE_STANDBY)) { result = VEHICLE_CMD_RESULT_ACCEPTED; } else { result = VEHICLE_CMD_RESULT_DENIED; @@ -831,14 +831,14 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta /* request to arm */ if ((int)cmd->param1 == 1) { - if (OK == do_arming_state_update(status_pub, current_vehicle_status, mavlink_fd, ARMING_STATE_ARMED)) { + if (OK == arming_state_transition(status_pub, current_vehicle_status, mavlink_fd, ARMING_STATE_ARMED)) { result = VEHICLE_CMD_RESULT_ACCEPTED; } else { result = VEHICLE_CMD_RESULT_DENIED; } /* request to disarm */ } else if ((int)cmd->param1 == 0) { - if (OK == do_arming_state_update(status_pub, current_vehicle_status, mavlink_fd, ARMING_STATE_STANDBY)) { + if (OK == arming_state_transition(status_pub, current_vehicle_status, mavlink_fd, ARMING_STATE_STANDBY)) { result = VEHICLE_CMD_RESULT_ACCEPTED; } else { result = VEHICLE_CMD_RESULT_DENIED; @@ -851,7 +851,7 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta /* request for an autopilot reboot */ if ((int)cmd->param1 == 1) { - if (OK == do_arming_state_update(status_pub, current_vehicle_status, mavlink_fd, ARMING_STATE_REBOOT)) { + if (OK == arming_state_transition(status_pub, current_vehicle_status, mavlink_fd, ARMING_STATE_REBOOT)) { /* SPECIAL CASE: SYSTEM WILL NEVER RETURN HERE */ result = VEHICLE_CMD_RESULT_ACCEPTED; @@ -893,7 +893,7 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta /* gyro calibration */ if ((int)(cmd->param1) == 1) { - if (OK == do_arming_state_update(status_pub, current_vehicle_status, mavlink_fd, ARMING_STATE_INIT)) { + if (OK == arming_state_transition(status_pub, current_vehicle_status, mavlink_fd, ARMING_STATE_INIT)) { result = VEHICLE_CMD_RESULT_ACCEPTED; @@ -902,8 +902,8 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta do_gyro_calibration(status_pub, ¤t_status); mavlink_log_info(mavlink_fd, "finished gyro cal"); tune_confirm(); - - do_arming_state_update(status_pub, ¤t_status, mavlink_fd, ARMING_STATE_STANDBY); + // XXX if this fails, go to ERROR + arming_state_transition(status_pub, ¤t_status, mavlink_fd, ARMING_STATE_STANDBY); } else { result = VEHICLE_CMD_RESULT_DENIED; @@ -915,7 +915,7 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta /* magnetometer calibration */ if ((int)(cmd->param2) == 1) { - if (OK == do_arming_state_update(status_pub, current_vehicle_status, mavlink_fd, ARMING_STATE_INIT)) { + if (OK == arming_state_transition(status_pub, current_vehicle_status, mavlink_fd, ARMING_STATE_INIT)) { result = VEHICLE_CMD_RESULT_ACCEPTED; @@ -924,8 +924,8 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta do_mag_calibration(status_pub, ¤t_status); mavlink_log_info(mavlink_fd, "finished mag cal"); tune_confirm(); - - do_arming_state_update(status_pub, ¤t_status, mavlink_fd, ARMING_STATE_STANDBY); + // XXX if this fails, go to ERROR + arming_state_transition(status_pub, ¤t_status, mavlink_fd, ARMING_STATE_STANDBY); } else { result = VEHICLE_CMD_RESULT_DENIED; @@ -986,7 +986,7 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta /* accel calibration */ if ((int)(cmd->param5) == 1) { - if (OK == do_arming_state_update(status_pub, current_vehicle_status, mavlink_fd, ARMING_STATE_INIT)) { + if (OK == arming_state_transition(status_pub, current_vehicle_status, mavlink_fd, ARMING_STATE_INIT)) { result = VEHICLE_CMD_RESULT_ACCEPTED; @@ -995,8 +995,8 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta do_accel_calibration(status_pub, ¤t_status); mavlink_log_info(mavlink_fd, "finished acc cal"); tune_confirm(); - - do_arming_state_update(status_pub, ¤t_status, mavlink_fd, ARMING_STATE_STANDBY); + // XXX what if this fails + arming_state_transition(status_pub, ¤t_status, mavlink_fd, ARMING_STATE_STANDBY); } else { result = VEHICLE_CMD_RESULT_DENIED; @@ -1647,7 +1647,8 @@ int commander_thread_main(int argc, char *argv[]) /* If in INIT state, try to proceed to STANDBY state */ if (current_status.arming_state == ARMING_STATE_INIT) { - do_arming_state_update(stat_pub, ¤t_status, mavlink_fd, ARMING_STATE_STANDBY); + //XXX what if this fails + arming_state_transition(stat_pub, ¤t_status, mavlink_fd, ARMING_STATE_STANDBY); } else { // XXX: Add emergency stuff if sensors are lost } @@ -1857,7 +1858,8 @@ int commander_thread_main(int argc, char *argv[]) } /* Now it's time to handle the stick inputs */ - navigation_state_update(stat_pub, ¤t_status, mavlink_fd); + #warning do that + // navigation_state_update(stat_pub, ¤t_status, mavlink_fd); /* handle the case where RC signal was regained */ if (!current_status.rc_signal_found_once) { @@ -1880,7 +1882,7 @@ int commander_thread_main(int argc, char *argv[]) // ) && if ((sp_man.yaw < -STICK_ON_OFF_LIMIT) && (sp_man.throttle < STICK_THRUST_RANGE * 0.2f)) { if (stick_off_counter > STICK_ON_OFF_COUNTER_LIMIT) { - do_arming_state_update(stat_pub, ¤t_status, mavlink_fd, ARMING_STATE_STANDBY); + arming_state_transition(stat_pub, ¤t_status, mavlink_fd, ARMING_STATE_STANDBY); stick_off_counter = 0; } else { @@ -1892,7 +1894,7 @@ 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 (stick_on_counter > STICK_ON_OFF_COUNTER_LIMIT) { - do_arming_state_update(stat_pub, ¤t_status, mavlink_fd, ARMING_STATE_ARMED); + arming_state_transition(stat_pub, ¤t_status, mavlink_fd, ARMING_STATE_ARMED); stick_on_counter = 0; } else { diff --git a/apps/commander/state_machine_helper.c b/apps/commander/state_machine_helper.c index e5ef00e934..6c15bd7251 100644 --- a/apps/commander/state_machine_helper.c +++ b/apps/commander/state_machine_helper.c @@ -56,206 +56,205 @@ /** * Transition from one sytem state to another */ -void navigation_state_update(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd) -{ - int ret = ERROR; - navigation_state_t new_navigation_state; - - /* do the navigation state update depending on the arming state */ - switch (current_status->arming_state) { - - /* evaluate the navigation state when disarmed */ - case ARMING_STATE_STANDBY: - - /* Always accept manual mode */ - if (current_status->mode_switch == MODE_SWITCH_MANUAL) { - new_navigation_state = NAVIGATION_STATE_MANUAL_STANDBY; - - /* Accept SEATBELT if there is a local position estimate */ - } else if (current_status->mode_switch == MODE_SWITCH_SEATBELT) { - - if (current_status->flag_local_position_valid) { - new_navigation_state = NAVIGATION_STATE_SEATBELT_STANDBY; - } else { - /* or just fall back to manual */ - mavlink_log_critical(mavlink_fd, "Rej. SEATBELT: no local position"); - new_navigation_state = NAVIGATION_STATE_MANUAL_STANDBY; - } - - /* Accept AUTO if there is a global position estimate */ - } else if (current_status->mode_switch == MODE_SWITCH_AUTO) { - if (current_status->flag_local_position_valid) { - new_navigation_state = NAVIGATION_STATE_SEATBELT_STANDBY; - } else { - mavlink_log_critical(mavlink_fd, "Rej. AUTO: no global position"); - - /* otherwise fallback to seatbelt or even manual */ - if (current_status->flag_local_position_valid) { - new_navigation_state = NAVIGATION_STATE_SEATBELT_STANDBY; - } else { - mavlink_log_critical(mavlink_fd, "Rej. SEATBELT: no local position"); - new_navigation_state = NAVIGATION_STATE_MANUAL_STANDBY; - } - } - } - - break; - - /* evaluate the navigation state when armed */ - case ARMING_STATE_ARMED: - - /* Always accept manual mode */ - if (current_status->mode_switch == MODE_SWITCH_MANUAL) { - new_navigation_state = NAVIGATION_STATE_MANUAL; - - /* Accept SEATBELT if there is a local position estimate */ - } else if (current_status->mode_switch == MODE_SWITCH_SEATBELT - && current_status->return_switch == RETURN_SWITCH_NONE) { - - if (current_status->flag_local_position_valid) { - new_navigation_state = NAVIGATION_STATE_SEATBELT; - } else { - /* or just fall back to manual */ - mavlink_log_critical(mavlink_fd, "Rej. SEATBELT: no local position"); - new_navigation_state = NAVIGATION_STATE_MANUAL; - } - - /* Accept SEATBELT_DESCENT if there is a local position estimate and the return switch is on */ - } else if (current_status->mode_switch == MODE_SWITCH_SEATBELT - && current_status->return_switch == RETURN_SWITCH_RETURN) { - - if (current_status->flag_local_position_valid) { - new_navigation_state = NAVIGATION_STATE_SEATBELT_DESCENT; - } else { - /* descent not possible without baro information, fall back to manual */ - mavlink_log_critical(mavlink_fd, "Rej. SEATBELT_DESCENT: no local position"); - new_navigation_state = NAVIGATION_STATE_MANUAL; - } - - /* Accept LOITER if there is a global position estimate */ - } else if (current_status->mode_switch == MODE_SWITCH_AUTO - && current_status->return_switch == RETURN_SWITCH_NONE - && current_status->mission_switch == MISSION_SWITCH_NONE) { - - if (current_status->flag_global_position_valid) { - new_navigation_state = NAVIGATION_STATE_AUTO_LOITER; - } else { - mavlink_log_critical(mavlink_fd, "Rej. LOITER: no global position"); - - /* otherwise fallback to SEATBELT or even manual */ - if (current_status->flag_local_position_valid) { - new_navigation_state = NAVIGATION_STATE_SEATBELT; - } else { - mavlink_log_critical(mavlink_fd, "Rej. SEATBELT: no local position"); - new_navigation_state = NAVIGATION_STATE_MANUAL; - } - } - - /* Accept MISSION if there is a global position estimate and home position */ - } else if (current_status->mode_switch == MODE_SWITCH_AUTO - && current_status->return_switch == RETURN_SWITCH_NONE - && current_status->mission_switch == MISSION_SWITCH_MISSION) { - - if (current_status->flag_global_position_valid && current_status->flag_valid_home_position) { - new_navigation_state = NAVIGATION_STATE_AUTO_MISSION; - } else { - - /* spit out what exactly is unavailable */ - if (current_status->flag_global_position_valid) { - mavlink_log_critical(mavlink_fd, "Rej. MISSION: no home position"); - } else if (current_status->flag_valid_home_position) { - mavlink_log_critical(mavlink_fd, "Rej. MISSION: no global position"); - } else { - mavlink_log_critical(mavlink_fd, "Rej. MISSION: no global position and no home position"); - } - - /* otherwise fallback to SEATBELT or even manual */ - if (current_status->flag_local_position_valid) { - new_navigation_state = NAVIGATION_STATE_SEATBELT; - } else { - mavlink_log_critical(mavlink_fd, "Rej. SEATBELT: no local position"); - new_navigation_state = NAVIGATION_STATE_MANUAL; - } - } - - /* Go to RTL or land if global position estimate and home position is available */ - } else if (current_status->mode_switch == MODE_SWITCH_AUTO - && current_status->return_switch == RETURN_SWITCH_RETURN - && (current_status->mission_switch == MISSION_SWITCH_NONE || current_status->mission_switch == MISSION_SWITCH_MISSION)) { - - if (current_status->flag_global_position_valid && current_status->flag_valid_home_position) { - - /* after RTL go to LAND */ - if (current_status->flag_system_returned_to_home) { - new_navigation_state = NAVIGATION_STATE_AUTO_LAND; - } else { - new_navigation_state = NAVIGATION_STATE_AUTO_RTL; - } - - } else { - - /* spit out what exactly is unavailable */ - if (current_status->flag_global_position_valid) { - mavlink_log_critical(mavlink_fd, "Rej. RTL/LAND: no home position"); - } else if (current_status->flag_valid_home_position) { - mavlink_log_critical(mavlink_fd, "Rej. RTL/LAND: no global position"); - } else { - mavlink_log_critical(mavlink_fd, "Rej. RTL/LAND: no global position and no home position"); - } - - /* otherwise fallback to SEATBELT_DESCENT or even MANUAL */ - if (current_status->flag_local_position_valid) { - new_navigation_state = NAVIGATION_STATE_SEATBELT_DESCENT; - } else { - mavlink_log_critical(mavlink_fd, "Rej. SEATBELT: no local position"); - new_navigation_state = NAVIGATION_STATE_MANUAL; - } - } - } - break; - - case ARMING_STATE_ARMED_ERROR: - - // TODO work out fail-safe scenarios - break; - - case ARMING_STATE_STANDBY_ERROR: - - // TODO work out fail-safe scenarios - break; - - case ARMING_STATE_REBOOT: - - // XXX I don't think we should end up here - break; - - case ARMING_STATE_IN_AIR_RESTORE: - - // XXX not sure what to do here - break; - default: - break; - } - - - - /* Update the system state in case it has changed */ - if (current_status->navigation_state != new_navigation_state) { - - /* Check if the transition is valid */ - if (check_navigation_state_transition(current_status->navigation_state, new_navigation_state) == OK) { - current_status->navigation_state = new_navigation_state; - state_machine_publish(status_pub, current_status, mavlink_fd); - } else { - mavlink_log_critical(mavlink_fd, "System state transition not valid"); - } - } - - return; -} - -int check_arming_state_transition(struct vehicle_status_s *current_state, arming_state_t new_arming_state, const int mavlink_fd) { +//void navigation_state_update(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd) +//{ +// int ret = ERROR; +// navigation_state_t new_navigation_state; +// +// /* do the navigation state update depending on the arming state */ +// switch (current_status->arming_state) { +// +// /* evaluate the navigation state when disarmed */ +// case ARMING_STATE_STANDBY: +// +// /* Always accept manual mode */ +// if (current_status->mode_switch == MODE_SWITCH_MANUAL) { +// new_navigation_state = NAVIGATION_STATE_MANUAL_STANDBY; +// +// /* Accept SEATBELT if there is a local position estimate */ +// } else if (current_status->mode_switch == MODE_SWITCH_SEATBELT) { +// +// if (current_status->flag_local_position_valid) { +// new_navigation_state = NAVIGATION_STATE_SEATBELT_STANDBY; +// } else { +// /* or just fall back to manual */ +// mavlink_log_critical(mavlink_fd, "Rej. SEATBELT: no local position"); +// new_navigation_state = NAVIGATION_STATE_MANUAL_STANDBY; +// } +// +// /* Accept AUTO if there is a global position estimate */ +// } else if (current_status->mode_switch == MODE_SWITCH_AUTO) { +// if (current_status->flag_local_position_valid) { +// new_navigation_state = NAVIGATION_STATE_SEATBELT_STANDBY; +// } else { +// mavlink_log_critical(mavlink_fd, "Rej. AUTO: no global position"); +// +// /* otherwise fallback to seatbelt or even manual */ +// if (current_status->flag_local_position_valid) { +// new_navigation_state = NAVIGATION_STATE_SEATBELT_STANDBY; +// } else { +// mavlink_log_critical(mavlink_fd, "Rej. SEATBELT: no local position"); +// new_navigation_state = NAVIGATION_STATE_MANUAL_STANDBY; +// } +// } +// } +// +// break; +// +// /* evaluate the navigation state when armed */ +// case ARMING_STATE_ARMED: +// +// /* Always accept manual mode */ +// if (current_status->mode_switch == MODE_SWITCH_MANUAL) { +// new_navigation_state = NAVIGATION_STATE_MANUAL; +// +// /* Accept SEATBELT if there is a local position estimate */ +// } else if (current_status->mode_switch == MODE_SWITCH_SEATBELT +// && current_status->return_switch == RETURN_SWITCH_NONE) { +// +// if (current_status->flag_local_position_valid) { +// new_navigation_state = NAVIGATION_STATE_SEATBELT; +// } else { +// /* or just fall back to manual */ +// mavlink_log_critical(mavlink_fd, "Rej. SEATBELT: no local position"); +// new_navigation_state = NAVIGATION_STATE_MANUAL; +// } +// +// /* Accept SEATBELT_DESCENT if there is a local position estimate and the return switch is on */ +// } else if (current_status->mode_switch == MODE_SWITCH_SEATBELT +// && current_status->return_switch == RETURN_SWITCH_RETURN) { +// +// if (current_status->flag_local_position_valid) { +// new_navigation_state = NAVIGATION_STATE_SEATBELT_DESCENT; +// } else { +// /* descent not possible without baro information, fall back to manual */ +// mavlink_log_critical(mavlink_fd, "Rej. SEATBELT_DESCENT: no local position"); +// new_navigation_state = NAVIGATION_STATE_MANUAL; +// } +// +// /* Accept LOITER if there is a global position estimate */ +// } else if (current_status->mode_switch == MODE_SWITCH_AUTO +// && current_status->return_switch == RETURN_SWITCH_NONE +// && current_status->mission_switch == MISSION_SWITCH_NONE) { +// +// if (current_status->flag_global_position_valid) { +// new_navigation_state = NAVIGATION_STATE_AUTO_LOITER; +// } else { +// mavlink_log_critical(mavlink_fd, "Rej. LOITER: no global position"); +// +// /* otherwise fallback to SEATBELT or even manual */ +// if (current_status->flag_local_position_valid) { +// new_navigation_state = NAVIGATION_STATE_SEATBELT; +// } else { +// mavlink_log_critical(mavlink_fd, "Rej. SEATBELT: no local position"); +// new_navigation_state = NAVIGATION_STATE_MANUAL; +// } +// } +// +// /* Accept MISSION if there is a global position estimate and home position */ +// } else if (current_status->mode_switch == MODE_SWITCH_AUTO +// && current_status->return_switch == RETURN_SWITCH_NONE +// && current_status->mission_switch == MISSION_SWITCH_MISSION) { +// +// if (current_status->flag_global_position_valid && current_status->flag_valid_home_position) { +// new_navigation_state = NAVIGATION_STATE_AUTO_MISSION; +// } else { +// +// /* spit out what exactly is unavailable */ +// if (current_status->flag_global_position_valid) { +// mavlink_log_critical(mavlink_fd, "Rej. MISSION: no home position"); +// } else if (current_status->flag_valid_home_position) { +// mavlink_log_critical(mavlink_fd, "Rej. MISSION: no global position"); +// } else { +// mavlink_log_critical(mavlink_fd, "Rej. MISSION: no global position and no home position"); +// } +// +// /* otherwise fallback to SEATBELT or even manual */ +// if (current_status->flag_local_position_valid) { +// new_navigation_state = NAVIGATION_STATE_SEATBELT; +// } else { +// mavlink_log_critical(mavlink_fd, "Rej. SEATBELT: no local position"); +// new_navigation_state = NAVIGATION_STATE_MANUAL; +// } +// } +// +// /* Go to RTL or land if global position estimate and home position is available */ +// } else if (current_status->mode_switch == MODE_SWITCH_AUTO +// && current_status->return_switch == RETURN_SWITCH_RETURN +// && (current_status->mission_switch == MISSION_SWITCH_NONE || current_status->mission_switch == MISSION_SWITCH_MISSION)) { +// +// if (current_status->flag_global_position_valid && current_status->flag_valid_home_position) { +// +// /* after RTL go to LAND */ +// if (current_status->flag_system_returned_to_home) { +// new_navigation_state = NAVIGATION_STATE_AUTO_LAND; +// } else { +// new_navigation_state = NAVIGATION_STATE_AUTO_RTL; +// } +// +// } else { +// +// /* spit out what exactly is unavailable */ +// if (current_status->flag_global_position_valid) { +// mavlink_log_critical(mavlink_fd, "Rej. RTL/LAND: no home position"); +// } else if (current_status->flag_valid_home_position) { +// mavlink_log_critical(mavlink_fd, "Rej. RTL/LAND: no global position"); +// } else { +// mavlink_log_critical(mavlink_fd, "Rej. RTL/LAND: no global position and no home position"); +// } +// +// /* otherwise fallback to SEATBELT_DESCENT or even MANUAL */ +// if (current_status->flag_local_position_valid) { +// new_navigation_state = NAVIGATION_STATE_SEATBELT_DESCENT; +// } else { +// mavlink_log_critical(mavlink_fd, "Rej. SEATBELT: no local position"); +// new_navigation_state = NAVIGATION_STATE_MANUAL; +// } +// } +// } +// break; +// +// case ARMING_STATE_ARMED_ERROR: +// +// // TODO work out fail-safe scenarios +// break; +// +// case ARMING_STATE_STANDBY_ERROR: +// +// // TODO work out fail-safe scenarios +// break; +// +// case ARMING_STATE_REBOOT: +// +// // XXX I don't think we should end up here +// break; +// +// case ARMING_STATE_IN_AIR_RESTORE: +// +// // XXX not sure what to do here +// break; +// default: +// break; +// } +// +// +// +// /* Update the system state in case it has changed */ +// if (current_status->navigation_state != new_navigation_state) { +// +// /* Check if the transition is valid */ +// if (check_navigation_state_transition(current_status->navigation_state, new_navigation_state) == OK) { +// current_status->navigation_state = new_navigation_state; +// state_machine_publish(status_pub, current_status, mavlink_fd); +// } else { +// mavlink_log_critical(mavlink_fd, "System state transition not valid"); +// } +// } +// +// return; +//} +int arming_state_transition(int status_pub, struct vehicle_status_s *current_state, arming_state_t new_arming_state, const int mavlink_fd) { int ret = ERROR; /* only check transition if the new state is actually different from the current one */ @@ -324,20 +323,20 @@ int check_arming_state_transition(struct vehicle_status_s *current_state, arming * This functions does not evaluate any input flags but only checks if the transitions * are valid. */ -int check_navigation_state_transition(navigation_state_t current_navigation_state, navigation_state_t new_navigation_state) { +int navigation_state_transition(int status_pub, struct vehicle_status_s *current_state, navigation_state_t new_navigation_state, const int mavlink_fd) { int ret = ERROR; /* only check transition if the new state is actually different from the current one */ - if (new_navigation_state != current_navigation_state) { + if (new_navigation_state != current_state->navigation_state) { switch (new_navigation_state) { case NAVIGATION_STATE_INIT: /* transitions back to INIT are possible for calibration */ - if (current_navigation_state == NAVIGATION_STATE_MANUAL_STANDBY - || current_navigation_state == NAVIGATION_STATE_SEATBELT_STANDBY - || current_navigation_state == NAVIGATION_STATE_AUTO_STANDBY) { + if (current_state->navigation_state == NAVIGATION_STATE_MANUAL_STANDBY + || current_state->navigation_state == NAVIGATION_STATE_SEATBELT_STANDBY + || current_state->navigation_state == NAVIGATION_STATE_AUTO_STANDBY) { ret = OK; } @@ -346,93 +345,139 @@ int check_navigation_state_transition(navigation_state_t current_navigation_stat case NAVIGATION_STATE_MANUAL_STANDBY: /* transitions from INIT and other STANDBY states as well as MANUAL are possible */ - if (current_navigation_state == NAVIGATION_STATE_INIT - || current_navigation_state == NAVIGATION_STATE_SEATBELT_STANDBY - || current_navigation_state == NAVIGATION_STATE_AUTO_STANDBY - || current_navigation_state == NAVIGATION_STATE_MANUAL) { + if (current_state->navigation_state == NAVIGATION_STATE_INIT + || current_state->navigation_state == NAVIGATION_STATE_SEATBELT_STANDBY + || current_state->navigation_state == NAVIGATION_STATE_AUTO_STANDBY + || current_state->navigation_state == NAVIGATION_STATE_MANUAL) { - ret = OK; + /* need to be disarmed first */ + if (current_state->arming_state != ARMING_STATE_STANDBY) { + mavlink_log_critical(mavlink_fd, "Rej. MANUAL_STANDBY: not disarmed"); + } else { + ret = OK; + } } break; case NAVIGATION_STATE_MANUAL: - /* all transitions possible */ - ret = OK; + /* need to be armed first */ + if (current_state->arming_state != ARMING_STATE_ARMED) { + mavlink_log_critical(mavlink_fd, "Rej. MANUAL: not armed"); + } else { + ret = OK; + } break; case NAVIGATION_STATE_SEATBELT_STANDBY: /* transitions from INIT and other STANDBY states as well as SEATBELT and SEATBELT_DESCENT are possible */ - if (current_navigation_state == NAVIGATION_STATE_INIT - || current_navigation_state == NAVIGATION_STATE_MANUAL_STANDBY - || current_navigation_state == NAVIGATION_STATE_AUTO_STANDBY - || current_navigation_state == NAVIGATION_STATE_SEATBELT - || current_navigation_state == NAVIGATION_STATE_SEATBELT_DESCENT) { + if (current_state->navigation_state == NAVIGATION_STATE_INIT + || current_state->navigation_state == NAVIGATION_STATE_MANUAL_STANDBY + || current_state->navigation_state == NAVIGATION_STATE_AUTO_STANDBY + || current_state->navigation_state == NAVIGATION_STATE_SEATBELT + || current_state->navigation_state == NAVIGATION_STATE_SEATBELT_DESCENT) { - ret = OK; + /* need to be disarmed and have a position estimate */ + if (current_state->arming_state != ARMING_STATE_STANDBY) { + mavlink_log_critical(mavlink_fd, "Rej. SEATBELT_STANDBY: not disarmed"); + } else if (!current_state->flag_local_position_valid) { + mavlink_log_critical(mavlink_fd, "Rej. SEATBELT_STANDBY: no position estimate"); + } else { + ret = OK; + } } break; case NAVIGATION_STATE_SEATBELT: /* transitions from all AUTO modes except AUTO_STANDBY and except MANUAL_STANDBY and INIT*/ - if (current_navigation_state == NAVIGATION_STATE_SEATBELT_STANDBY - || current_navigation_state == NAVIGATION_STATE_SEATBELT_DESCENT - || current_navigation_state == NAVIGATION_STATE_MANUAL - || current_navigation_state == NAVIGATION_STATE_AUTO_LAND - || current_navigation_state == NAVIGATION_STATE_AUTO_LOITER - || current_navigation_state == NAVIGATION_STATE_AUTO_MISSION - || current_navigation_state == NAVIGATION_STATE_AUTO_READY - || current_navigation_state == NAVIGATION_STATE_AUTO_RTL - || current_navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF) { + if (current_state->navigation_state == NAVIGATION_STATE_SEATBELT_STANDBY + || current_state->navigation_state == NAVIGATION_STATE_SEATBELT_DESCENT + || current_state->navigation_state == NAVIGATION_STATE_MANUAL + || current_state->navigation_state == NAVIGATION_STATE_AUTO_LAND + || current_state->navigation_state == NAVIGATION_STATE_AUTO_LOITER + || current_state->navigation_state == NAVIGATION_STATE_AUTO_MISSION + || current_state->navigation_state == NAVIGATION_STATE_AUTO_READY + || current_state->navigation_state == NAVIGATION_STATE_AUTO_RTL + || current_state->navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF) { - ret = OK; + /* need to be armed and have a position estimate */ + if (current_state->arming_state != ARMING_STATE_ARMED) { + mavlink_log_critical(mavlink_fd, "Rej. SEATBELT: not armed"); + } else if (!current_state->flag_local_position_valid) { + mavlink_log_critical(mavlink_fd, "Rej. SEATBELT: no pos estimate"); + } else { + ret = OK; + } } break; case NAVIGATION_STATE_SEATBELT_DESCENT: /* transitions from all AUTO modes except AUTO_STANDBY and except MANUAL_STANDBY and INIT and SEATBELT_STANDBY */ - if (current_navigation_state == NAVIGATION_STATE_SEATBELT - || current_navigation_state == NAVIGATION_STATE_MANUAL - || current_navigation_state == NAVIGATION_STATE_AUTO_LAND - || current_navigation_state == NAVIGATION_STATE_AUTO_LOITER - || current_navigation_state == NAVIGATION_STATE_AUTO_MISSION - || current_navigation_state == NAVIGATION_STATE_AUTO_READY - || current_navigation_state == NAVIGATION_STATE_AUTO_RTL - || current_navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF) { + if (current_state->navigation_state == NAVIGATION_STATE_SEATBELT + || current_state->navigation_state == NAVIGATION_STATE_MANUAL + || current_state->navigation_state == NAVIGATION_STATE_AUTO_LAND + || current_state->navigation_state == NAVIGATION_STATE_AUTO_LOITER + || current_state->navigation_state == NAVIGATION_STATE_AUTO_MISSION + || current_state->navigation_state == NAVIGATION_STATE_AUTO_READY + || current_state->navigation_state == NAVIGATION_STATE_AUTO_RTL + || current_state->navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF) { - ret = OK; + /* need to be armed and have a position estimate */ + if (current_state->arming_state != ARMING_STATE_ARMED) { + mavlink_log_critical(mavlink_fd, "Rej. SEATBELT_DESCENT: not armed"); + } else if (!current_state->flag_local_position_valid) { + mavlink_log_critical(mavlink_fd, "Rej. SEATBELT_DESCENT: no pos estimate"); + } else { + ret = OK; + } } break; case NAVIGATION_STATE_AUTO_STANDBY: /* transitions from INIT or from other STANDBY modes or from AUTO READY */ - if (current_navigation_state == NAVIGATION_STATE_INIT - || current_navigation_state == NAVIGATION_STATE_MANUAL_STANDBY - || current_navigation_state == NAVIGATION_STATE_SEATBELT_STANDBY - || current_navigation_state == NAVIGATION_STATE_AUTO_READY) { + if (current_state->navigation_state == NAVIGATION_STATE_INIT + || current_state->navigation_state == NAVIGATION_STATE_MANUAL_STANDBY + || current_state->navigation_state == NAVIGATION_STATE_SEATBELT_STANDBY + || current_state->navigation_state == NAVIGATION_STATE_AUTO_READY) { - ret = OK; + /* need to be disarmed and have a position and home lock */ + if (current_state->arming_state != ARMING_STATE_STANDBY) { + mavlink_log_critical(mavlink_fd, "Rej. AUTO_STANDBY: not disarmed"); + } else if (!current_state->flag_global_position_valid) { + mavlink_log_critical(mavlink_fd, "Rej. AUTO_STANDBY: no pos lock"); + } else if (!current_state->flag_valid_home_position) { + mavlink_log_critical(mavlink_fd, "Rej. AUTO_STANDBY: no home pos"); + } else { + ret = OK; + } } break; case NAVIGATION_STATE_AUTO_READY: /* transitions from AUTO_STANDBY or AUTO_LAND */ - if (current_navigation_state == NAVIGATION_STATE_AUTO_STANDBY - || current_navigation_state == NAVIGATION_STATE_AUTO_LAND) { + if (current_state->navigation_state == NAVIGATION_STATE_AUTO_STANDBY + || current_state->navigation_state == NAVIGATION_STATE_AUTO_LAND) { - ret = OK; + // XXX flag test needed? + + /* need to be armed and have a position and home lock */ + if (current_state->arming_state != ARMING_STATE_ARMED) { + mavlink_log_critical(mavlink_fd, "Rej. AUTO_READY: not armed"); + } else { + ret = OK; + } } break; case NAVIGATION_STATE_AUTO_TAKEOFF: /* only transitions from AUTO_READY */ - if (current_navigation_state == NAVIGATION_STATE_AUTO_READY) { + if (current_state->navigation_state == NAVIGATION_STATE_AUTO_READY) { ret = OK; } @@ -441,49 +486,75 @@ int check_navigation_state_transition(navigation_state_t current_navigation_stat case NAVIGATION_STATE_AUTO_LOITER: /* from everywhere flying except AUTO_LAND and SEATBELT_DESCENT */ - if (current_navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF - || current_navigation_state == NAVIGATION_STATE_AUTO_MISSION - || current_navigation_state == NAVIGATION_STATE_AUTO_RTL - || current_navigation_state == NAVIGATION_STATE_SEATBELT - || current_navigation_state == NAVIGATION_STATE_MANUAL) { + if (current_state->navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF + || current_state->navigation_state == NAVIGATION_STATE_AUTO_MISSION + || current_state->navigation_state == NAVIGATION_STATE_AUTO_RTL + || current_state->navigation_state == NAVIGATION_STATE_SEATBELT + || current_state->navigation_state == NAVIGATION_STATE_MANUAL) { - ret = OK; + /* need to have a position and home lock */ + if (!current_state->flag_global_position_valid) { + mavlink_log_critical(mavlink_fd, "Rej. AUTO_LOITER: no pos lock"); + } else if (!current_state->flag_valid_home_position) { + mavlink_log_critical(mavlink_fd, "Rej. AUTO_LOITER: no home pos"); + } else { + ret = OK; + } } break; case NAVIGATION_STATE_AUTO_MISSION: /* from everywhere flying except AUTO_LAND and SEATBELT_DESCENT */ - if (current_navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF - || current_navigation_state == NAVIGATION_STATE_AUTO_LOITER - || current_navigation_state == NAVIGATION_STATE_AUTO_RTL - || current_navigation_state == NAVIGATION_STATE_SEATBELT - || current_navigation_state == NAVIGATION_STATE_MANUAL) { + if (current_state->navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF + || current_state->navigation_state == NAVIGATION_STATE_AUTO_LOITER + || current_state->navigation_state == NAVIGATION_STATE_AUTO_RTL + || current_state->navigation_state == NAVIGATION_STATE_SEATBELT + || current_state->navigation_state == NAVIGATION_STATE_MANUAL) { - ret = OK; + /* need to have a mission ready */ + if (!current_state->flag_auto_mission_available) { + mavlink_log_critical(mavlink_fd, "Rej. AUTO_MISSION: no mission available"); + } else { + ret = OK; + } } break; case NAVIGATION_STATE_AUTO_RTL: /* from everywhere flying except AUTO_LAND and SEATBELT_DESCENT */ - if (current_navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF - || current_navigation_state == NAVIGATION_STATE_AUTO_MISSION - || current_navigation_state == NAVIGATION_STATE_AUTO_LOITER - || current_navigation_state == NAVIGATION_STATE_SEATBELT - || current_navigation_state == NAVIGATION_STATE_MANUAL) { + if (current_state->navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF + || current_state->navigation_state == NAVIGATION_STATE_AUTO_MISSION + || current_state->navigation_state == NAVIGATION_STATE_AUTO_LOITER + || current_state->navigation_state == NAVIGATION_STATE_SEATBELT + || current_state->navigation_state == NAVIGATION_STATE_MANUAL) { - ret = OK; + /* need to have a position and home lock */ + if (!current_state->flag_global_position_valid) { + mavlink_log_critical(mavlink_fd, "Rej. AUTO_RTL: no pos lock"); + } else if (!current_state->flag_valid_home_position) { + mavlink_log_critical(mavlink_fd, "Rej. AUTO_RTL: no home pos"); + } else { + ret = OK; + } } break; case NAVIGATION_STATE_AUTO_LAND: /* after AUTO_RTL or when in AUTO_LOITER or AUTO_MISSION */ - if (current_navigation_state == NAVIGATION_STATE_AUTO_RTL - || current_navigation_state == NAVIGATION_STATE_AUTO_MISSION - || current_navigation_state == NAVIGATION_STATE_AUTO_LOITER) { + if (current_state->navigation_state == NAVIGATION_STATE_AUTO_RTL + || current_state->navigation_state == NAVIGATION_STATE_AUTO_MISSION + || current_state->navigation_state == NAVIGATION_STATE_AUTO_LOITER) { - ret = OK; + /* need to have a position and home lock */ + if (!current_state->flag_global_position_valid) { + mavlink_log_critical(mavlink_fd, "Rej. AUTO_LAND: no pos lock"); + } else if (!current_state->flag_valid_home_position) { + mavlink_log_critical(mavlink_fd, "Rej. AUTO_LAND: no home pos"); + } else { + ret = OK; + } } break; @@ -492,182 +563,71 @@ int check_navigation_state_transition(navigation_state_t current_navigation_stat } } + if (ret == OK) { + current_state->navigation_state = new_navigation_state; + state_machine_publish(status_pub, current_state, mavlink_fd); + } + return ret; } -///** -// * Transition from one arming state to another -// */ -//int do_arming_state_update(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, arming_state_t new_state) -//{ -// bool valid_transition = false; -// int ret = ERROR; -// -// warnx("Current state: %d, requested state: %d", current_status->arming_state, new_state); -// -// if (current_status->arming_state == new_state) { -// warnx("Arming state not changed"); -// valid_transition = true; -// -// } else { -// -// switch (new_state) { -// -// case ARMING_STATE_INIT: -// -// if (current_status->arming_state == ARMING_STATE_STANDBY) { -// -// mavlink_log_critical(mavlink_fd, "Switched to INIT ARMING STATE"); -// valid_transition = true; -// } -// break; -// -// case ARMING_STATE_STANDBY: -// -// // TODO check for sensors -// // XXX check if coming from reboot? -// if (current_status->arming_state == ARMING_STATE_INIT) { -// -// if (current_status->flag_system_sensors_initialized) { -// current_status->flag_system_armed = false; -// mavlink_log_critical(mavlink_fd, "Switched to STANDBY arming state"); -// valid_transition = true; -// } else { -// mavlink_log_critical(mavlink_fd, "REJ. STANDBY arming state, sensors not init."); -// } -// -// } else if (current_status->arming_state == ARMING_STATE_ARMED) { -// -// current_status->flag_system_armed = false; -// mavlink_log_critical(mavlink_fd, "Switched to STANDBY arming state"); -// valid_transition = true; -// } -// break; -// -// case ARMING_STATE_ARMED: -// -// if (current_status->arming_state == ARMING_STATE_STANDBY -// || current_status->arming_state == ARMING_STATE_IN_AIR_RESTORE) { -// current_status->flag_system_armed = true; -// mavlink_log_critical(mavlink_fd, "Switched to ARMED arming state"); -// valid_transition = true; -// } -// break; -// -// case ARMING_STATE_ABORT: -// -// if (current_status->arming_state == ARMING_STATE_ARMED) { -// -// mavlink_log_critical(mavlink_fd, "Switched to MISSION ABORT arming state"); -// valid_transition = true; -// } -// break; -// -// case ARMING_STATE_ERROR: -// -// if (current_status->arming_state == ARMING_STATE_ARMED -// || current_status->arming_state == ARMING_STATE_ABORT -// || current_status->arming_state == ARMING_STATE_INIT) { -// -// mavlink_log_critical(mavlink_fd, "Switched to ERROR arming state"); -// valid_transition = true; -// } -// break; -// -// case ARMING_STATE_REBOOT: -// -// if (current_status->arming_state == ARMING_STATE_STANDBY -// || current_status->arming_state == ARMING_STATE_ERROR) { -// -// valid_transition = true; -// mavlink_log_critical(mavlink_fd, "REBOOTING SYSTEM"); -// usleep(500000); -// up_systemreset(); -// /* SPECIAL CASE: NEVER RETURNS FROM THIS FUNCTION CALL */ -// } -// break; -// -// case ARMING_STATE_IN_AIR_RESTORE: -// -// if (current_status->arming_state == ARMING_STATE_INIT) { -// mavlink_log_critical(mavlink_fd, "Switched to IN-AIR-RESTORE arming state"); -// valid_transition = true; -// } -// break; -// default: -// warnx("Unknown arming state"); -// break; -// } -// } -// -// if (valid_transition) { -// current_status->arming_state = new_state; -// state_machine_publish(status_pub, current_status, mavlink_fd); -//// publish_armed_status(current_status); -// ret = OK; -// } else { -// mavlink_log_critical(mavlink_fd, "REJECTING invalid arming state transition"); -// } -// -// return ret; -//} -///** -// * Transition from one hil state to another -// */ -//int do_hil_state_update(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, hil_state_t new_state) -//{ -// bool valid_transition = false; -// int ret = ERROR; -// -// warnx("Current state: %d, requested state: %d", current_status->hil_state, new_state); -// -// if (current_status->hil_state == new_state) { -// warnx("Hil state not changed"); -// valid_transition = true; -// -// } else { -// -// switch (new_state) { -// -// case HIL_STATE_OFF: -// -// if (current_status->arming_state == ARMING_STATE_INIT -// || current_status->arming_state == ARMING_STATE_STANDBY) { -// -// current_status->flag_hil_enabled = false; -// mavlink_log_critical(mavlink_fd, "Switched to OFF hil state"); -// valid_transition = true; -// } -// break; -// -// case HIL_STATE_ON: -// -// if (current_status->arming_state == ARMING_STATE_INIT -// || current_status->arming_state == ARMING_STATE_STANDBY) { -// -// current_status->flag_hil_enabled = true; -// mavlink_log_critical(mavlink_fd, "Switched to ON hil state"); -// valid_transition = true; -// } -// break; -// -// default: -// warnx("Unknown hil state"); -// break; -// } -// } -// -// if (valid_transition) { -// current_status->hil_state = new_state; -// state_machine_publish(status_pub, current_status, mavlink_fd); -// ret = OK; -// } else { -// mavlink_log_critical(mavlink_fd, "REJECTING invalid hil state transition"); -// } -// -// return ret; -//} +/** +* Transition from one hil state to another +*/ +int hil_state_transition(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, hil_state_t new_state) +{ + bool valid_transition = false; + int ret = ERROR; + + warnx("Current state: %d, requested state: %d", current_status->hil_state, new_state); + + if (current_status->hil_state == new_state) { + warnx("Hil state not changed"); + valid_transition = true; + + } else { + + switch (new_state) { + + case HIL_STATE_OFF: + + if (current_status->arming_state == ARMING_STATE_INIT + || current_status->arming_state == ARMING_STATE_STANDBY) { + + current_status->flag_hil_enabled = false; + mavlink_log_critical(mavlink_fd, "Switched to OFF hil state"); + valid_transition = true; + } + break; + + case HIL_STATE_ON: + + if (current_status->arming_state == ARMING_STATE_INIT + || current_status->arming_state == ARMING_STATE_STANDBY) { + + current_status->flag_hil_enabled = true; + mavlink_log_critical(mavlink_fd, "Switched to ON hil state"); + valid_transition = true; + } + break; + + default: + warnx("Unknown hil state"); + break; + } + } + + if (valid_transition) { + current_status->hil_state = new_state; + state_machine_publish(status_pub, current_status, mavlink_fd); + ret = OK; + } else { + mavlink_log_critical(mavlink_fd, "REJECTING invalid hil state transition"); + } + + return ret; +} diff --git a/apps/commander/state_machine_helper.h b/apps/commander/state_machine_helper.h index f2928db098..5b57cffb73 100644 --- a/apps/commander/state_machine_helper.h +++ b/apps/commander/state_machine_helper.h @@ -53,7 +53,9 @@ void navigation_state_update(int status_pub, struct vehicle_status_s *current_st void state_machine_publish(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd); -int check_navigation_state_transition(navigation_state_t current_navigation_state, navigation_state_t new_navigation_state); -int check_arming_state_transition(struct vehicle_status_s current_state, arming_state_t new_arming_state); +int arming_state_transition(int status_pub, struct vehicle_status_s *current_state, arming_state_t new_arming_state, const int mavlink_fd); +int navigation_state_transition(int status_pub, struct vehicle_status_s *current_state, navigation_state_t new_navigation_state, const int mavlink_fd); -#endif /* STATE_MACHINE_HELPER_H_ */ +int hil_state_transition(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, hil_state_t new_state); + +#endif /* STATE_MACHINE_HELPER_H_ */ \ No newline at end of file diff --git a/apps/uORB/topics/vehicle_status.h b/apps/uORB/topics/vehicle_status.h index 29baff34bd..4955422441 100644 --- a/apps/uORB/topics/vehicle_status.h +++ b/apps/uORB/topics/vehicle_status.h @@ -193,6 +193,7 @@ struct vehicle_status_s bool flag_system_reboot_requested; bool flag_system_returned_to_home; + bool flag_auto_mission_available; bool flag_auto_enabled; bool flag_control_manual_enabled; /**< true if manual input is mixed in */ From cbfa64b59eef8362494d0753ce3567e804f2d682 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Fri, 22 Feb 2013 11:54:41 -0800 Subject: [PATCH 012/109] Checkpoint: Added switch handling --- apps/commander/commander.c | 162 ++++++++++++++++++++++++++++++++++++- 1 file changed, 160 insertions(+), 2 deletions(-) diff --git a/apps/commander/commander.c b/apps/commander/commander.c index 953ba7a1e9..38d2ffc962 100644 --- a/apps/commander/commander.c +++ b/apps/commander/commander.c @@ -1725,7 +1725,7 @@ int commander_thread_main(int argc, char *argv[]) state_changed = true; } - if (orb_check(ORB_ID(vehicle_gps_position), &new_data)) { + if (orb_check(gps_sub, &new_data)) { orb_copy(ORB_ID(vehicle_gps_position), gps_sub, &gps_position); @@ -1858,7 +1858,165 @@ int commander_thread_main(int argc, char *argv[]) } /* Now it's time to handle the stick inputs */ - #warning do that + + switch (current_status.arming_state) { + + /* evaluate the navigation state when disarmed */ + case ARMING_STATE_STANDBY: + + /* just manual, XXX this might depend on the return switch */ + if (current_status.mode_switch == MODE_SWITCH_MANUAL) { + + if (navigation_state_transition(stat_pub, ¤t_status, NAVIGATION_STATE_MANUAL_STANDBY, mavlink_fd) != OK) { + // These is not supposed to happen + warnx("ERROR: Navigation state MANUAL_STANDBY rejected"); + } + + /* Try seatbelt or fallback to manual */ + } else if (current_status.mode_switch == MODE_SWITCH_SEATBELT) { + + if (navigation_state_transition(stat_pub, ¤t_status, NAVIGATION_STATE_SEATBELT_STANDBY, mavlink_fd) != OK) { + // fallback to MANUAL_STANDBY + if (navigation_state_transition(stat_pub, ¤t_status, NAVIGATION_STATE_MANUAL_STANDBY, mavlink_fd) != OK) { + // These is not supposed to happen + warnx("ERROR: Navigation state MANUAL_STANDBY rejected"); + } + } + + /* Try auto or fallback to seatbelt or even manual */ + } else if (current_status.mode_switch == MODE_SWITCH_AUTO) { + + if (navigation_state_transition(stat_pub, ¤t_status, NAVIGATION_STATE_AUTO_STANDBY, mavlink_fd) != OK) { + // first fallback to SEATBELT_STANDY + if (navigation_state_transition(stat_pub, ¤t_status, NAVIGATION_STATE_SEATBELT_STANDBY, mavlink_fd) != OK) { + // or fallback to MANUAL_STANDBY + if (navigation_state_transition(stat_pub, ¤t_status, NAVIGATION_STATE_MANUAL_STANDBY, mavlink_fd) != OK) { + // These is not supposed to happen + warnx("ERROR: Navigation state MANUAL_STANDBY rejected"); + } + } + } + } + + break; + + /* evaluate the navigation state when armed */ + case ARMING_STATE_ARMED: + + /* Always accept manual mode */ + if (current_status.mode_switch == MODE_SWITCH_MANUAL) { + + if (navigation_state_transition(stat_pub, ¤t_status, NAVIGATION_STATE_MANUAL, mavlink_fd) != OK) { + // These is not supposed to happen + warnx("ERROR: Navigation state MANUAL rejected"); + } + + /* SEATBELT_STANDBY (fallback: MANUAL) */ + } else if (current_status.mode_switch == MODE_SWITCH_SEATBELT + && current_status.return_switch == RETURN_SWITCH_NONE) { + + if (navigation_state_transition(stat_pub, ¤t_status, NAVIGATION_STATE_SEATBELT, mavlink_fd) != OK) { + // fallback to MANUAL_STANDBY + if (navigation_state_transition(stat_pub, ¤t_status, NAVIGATION_STATE_MANUAL, mavlink_fd) != OK) { + // These is not supposed to happen + warnx("ERROR: Navigation state MANUAL rejected"); + } + } + + /* SEATBELT_DESCENT (fallback: MANUAL) */ + } else if (current_status.mode_switch == MODE_SWITCH_SEATBELT + && current_status.return_switch == RETURN_SWITCH_RETURN) { + + if (navigation_state_transition(stat_pub, ¤t_status, NAVIGATION_STATE_SEATBELT_DESCENT, mavlink_fd) != OK) { + // fallback to MANUAL_STANDBY + if (navigation_state_transition(stat_pub, ¤t_status, NAVIGATION_STATE_MANUAL, mavlink_fd) != OK) { + // These is not supposed to happen + warnx("ERROR: Navigation state MANUAL rejected"); + } + } + + /* AUTO_LOITER (fallback: SEATBELT, MANUAL) */ + } else if (current_status.mode_switch == MODE_SWITCH_AUTO + && current_status.return_switch == RETURN_SWITCH_NONE + && current_status.mission_switch == MISSION_SWITCH_NONE) { + + /* we might come from the disarmed state AUTO_STANDBY */ + if (navigation_state_transition(stat_pub, ¤t_status, NAVIGATION_STATE_AUTO_READY, mavlink_fd) != OK) { + if (navigation_state_transition(stat_pub, ¤t_status, NAVIGATION_STATE_SEATBELT, mavlink_fd) != OK) { + // fallback to MANUAL_STANDBY + if (navigation_state_transition(stat_pub, ¤t_status, NAVIGATION_STATE_MANUAL, mavlink_fd) != OK) { + // These is not supposed to happen + warnx("ERROR: Navigation state MANUAL rejected"); + } + } + /* or from some other armed state like SEATBELT or MANUAL */ + } else if (navigation_state_transition(stat_pub, ¤t_status, NAVIGATION_STATE_AUTO_LOITER, mavlink_fd) != OK) { + if (navigation_state_transition(stat_pub, ¤t_status, NAVIGATION_STATE_SEATBELT, mavlink_fd) != OK) { + // fallback to MANUAL_STANDBY + if (navigation_state_transition(stat_pub, ¤t_status, NAVIGATION_STATE_MANUAL, mavlink_fd) != OK) { + // These is not supposed to happen + warnx("ERROR: Navigation state MANUAL rejected"); + } + } + } + + /* AUTO_MISSION (fallback: SEATBELT, MANUAL) */ + } else if (current_status.mode_switch == MODE_SWITCH_AUTO + && current_status.return_switch == RETURN_SWITCH_NONE + && current_status.mission_switch == MISSION_SWITCH_MISSION) { + + if (navigation_state_transition(stat_pub, ¤t_status, NAVIGATION_STATE_AUTO_MISSION, mavlink_fd) != OK) { + if (navigation_state_transition(stat_pub, ¤t_status, NAVIGATION_STATE_SEATBELT, mavlink_fd) != OK) { + // fallback to MANUAL_STANDBY + if (navigation_state_transition(stat_pub, ¤t_status, NAVIGATION_STATE_MANUAL, mavlink_fd) != OK) { + // These is not supposed to happen + warnx("ERROR: Navigation state MANUAL rejected"); + } + } + } + + /* AUTO_RTL (fallback: SEATBELT_DESCENT, MANUAL) */ + } else if (current_status.mode_switch == MODE_SWITCH_AUTO + && current_status.return_switch == RETURN_SWITCH_RETURN + && (current_status.mission_switch == MISSION_SWITCH_NONE || current_status.mission_switch == MISSION_SWITCH_MISSION)) { + + if (navigation_state_transition(stat_pub, ¤t_status, NAVIGATION_STATE_AUTO_RTL, mavlink_fd) != OK) { + if (navigation_state_transition(stat_pub, ¤t_status, NAVIGATION_STATE_SEATBELT_DESCENT, mavlink_fd) != OK) { + // fallback to MANUAL_STANDBY + if (navigation_state_transition(stat_pub, ¤t_status, NAVIGATION_STATE_MANUAL, mavlink_fd) != OK) { + // These is not supposed to happen + warnx("ERROR: Navigation state MANUAL rejected"); + } + } + } + } + break; + + // XXX we might be missing something that triggers a transition from RTL to LAND + + case ARMING_STATE_ARMED_ERROR: + + // TODO work out fail-safe scenarios + break; + + case ARMING_STATE_STANDBY_ERROR: + + // TODO work out fail-safe scenarios + break; + + case ARMING_STATE_REBOOT: + + // XXX I don't think we should end up here + break; + + case ARMING_STATE_IN_AIR_RESTORE: + + // XXX not sure what to do here + break; + default: + break; + } + // navigation_state_update(stat_pub, ¤t_status, mavlink_fd); /* handle the case where RC signal was regained */ From 793b482e00013ea66bb1b0cdc0366bb720648938 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Fri, 22 Feb 2013 15:52:13 -0800 Subject: [PATCH 013/109] Checkpoint: Navigation states and arming seem to work --- apps/commander/commander.c | 43 ++++++++++++++------------- apps/commander/state_machine_helper.c | 25 ++++++++++++---- apps/sensors/sensors.cpp | 2 +- 3 files changed, 43 insertions(+), 27 deletions(-) diff --git a/apps/commander/commander.c b/apps/commander/commander.c index 38d2ffc962..2784e77dbe 100644 --- a/apps/commander/commander.c +++ b/apps/commander/commander.c @@ -812,13 +812,13 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta } if ((int)cmd->param1 & VEHICLE_MODE_FLAG_SAFETY_ARMED) { - if (OK == arming_state_transition(status_pub, current_vehicle_status, mavlink_fd, ARMING_STATE_ARMED)) { + if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_ARMED, mavlink_fd)) { result = VEHICLE_CMD_RESULT_ACCEPTED; } else { result = VEHICLE_CMD_RESULT_DENIED; } } else { - if (OK == arming_state_transition(status_pub, current_vehicle_status, mavlink_fd, ARMING_STATE_STANDBY)) { + if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_STANDBY, mavlink_fd)) { result = VEHICLE_CMD_RESULT_ACCEPTED; } else { result = VEHICLE_CMD_RESULT_DENIED; @@ -831,14 +831,14 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta /* request to arm */ if ((int)cmd->param1 == 1) { - if (OK == arming_state_transition(status_pub, current_vehicle_status, mavlink_fd, ARMING_STATE_ARMED)) { + if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_ARMED, mavlink_fd)) { result = VEHICLE_CMD_RESULT_ACCEPTED; } else { result = VEHICLE_CMD_RESULT_DENIED; } /* request to disarm */ } else if ((int)cmd->param1 == 0) { - if (OK == arming_state_transition(status_pub, current_vehicle_status, mavlink_fd, ARMING_STATE_STANDBY)) { + if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_STANDBY, mavlink_fd)) { result = VEHICLE_CMD_RESULT_ACCEPTED; } else { result = VEHICLE_CMD_RESULT_DENIED; @@ -851,7 +851,7 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta /* request for an autopilot reboot */ if ((int)cmd->param1 == 1) { - if (OK == arming_state_transition(status_pub, current_vehicle_status, mavlink_fd, ARMING_STATE_REBOOT)) { + if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_REBOOT, mavlink_fd)) { /* SPECIAL CASE: SYSTEM WILL NEVER RETURN HERE */ result = VEHICLE_CMD_RESULT_ACCEPTED; @@ -893,7 +893,7 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta /* gyro calibration */ if ((int)(cmd->param1) == 1) { - if (OK == arming_state_transition(status_pub, current_vehicle_status, mavlink_fd, ARMING_STATE_INIT)) { + if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_INIT, mavlink_fd)) { result = VEHICLE_CMD_RESULT_ACCEPTED; @@ -903,7 +903,7 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta mavlink_log_info(mavlink_fd, "finished gyro cal"); tune_confirm(); // XXX if this fails, go to ERROR - arming_state_transition(status_pub, ¤t_status, mavlink_fd, ARMING_STATE_STANDBY); + arming_state_transition(status_pub, ¤t_status, ARMING_STATE_STANDBY, mavlink_fd); } else { result = VEHICLE_CMD_RESULT_DENIED; @@ -915,7 +915,7 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta /* magnetometer calibration */ if ((int)(cmd->param2) == 1) { - if (OK == arming_state_transition(status_pub, current_vehicle_status, mavlink_fd, ARMING_STATE_INIT)) { + if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_INIT, mavlink_fd)) { result = VEHICLE_CMD_RESULT_ACCEPTED; @@ -925,7 +925,7 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta mavlink_log_info(mavlink_fd, "finished mag cal"); tune_confirm(); // XXX if this fails, go to ERROR - arming_state_transition(status_pub, ¤t_status, mavlink_fd, ARMING_STATE_STANDBY); + arming_state_transition(status_pub, ¤t_status, ARMING_STATE_STANDBY, mavlink_fd); } else { result = VEHICLE_CMD_RESULT_DENIED; @@ -986,7 +986,7 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta /* accel calibration */ if ((int)(cmd->param5) == 1) { - if (OK == arming_state_transition(status_pub, current_vehicle_status, mavlink_fd, ARMING_STATE_INIT)) { + if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_INIT, mavlink_fd)) { result = VEHICLE_CMD_RESULT_ACCEPTED; @@ -996,7 +996,7 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta mavlink_log_info(mavlink_fd, "finished acc cal"); tune_confirm(); // XXX what if this fails - arming_state_transition(status_pub, ¤t_status, mavlink_fd, ARMING_STATE_STANDBY); + arming_state_transition(status_pub, ¤t_status, ARMING_STATE_STANDBY, mavlink_fd); } else { result = VEHICLE_CMD_RESULT_DENIED; @@ -1647,8 +1647,9 @@ int commander_thread_main(int argc, char *argv[]) /* If in INIT state, try to proceed to STANDBY state */ if (current_status.arming_state == ARMING_STATE_INIT) { - //XXX what if this fails - arming_state_transition(stat_pub, ¤t_status, mavlink_fd, ARMING_STATE_STANDBY); + // XXX fix for now + current_status.flag_system_sensors_initialized = true; + arming_state_transition(stat_pub, ¤t_status, ARMING_STATE_STANDBY, mavlink_fd); } else { // XXX: Add emergency stuff if sensors are lost } @@ -1790,11 +1791,10 @@ int commander_thread_main(int argc, char *argv[]) * Check if manual control modes have to be switched */ if (!isfinite(sp_man.mode_switch)) { - warnx("mode sw not finite"); + warnx("mode sw not finite"); /* no valid stick position, go to default */ - } else if (sp_man.mode_switch < -STICK_ON_OFF_LIMIT) { /* bottom stick position, go to manual mode */ @@ -1806,6 +1806,7 @@ int commander_thread_main(int argc, char *argv[]) current_status.mode_switch = MODE_SWITCH_AUTO; } else { + /* center stick position, set seatbelt/simple control */ current_status.mode_switch = MODE_SWITCH_SEATBELT; } @@ -1869,7 +1870,7 @@ int commander_thread_main(int argc, char *argv[]) if (navigation_state_transition(stat_pub, ¤t_status, NAVIGATION_STATE_MANUAL_STANDBY, mavlink_fd) != OK) { // These is not supposed to happen - warnx("ERROR: Navigation state MANUAL_STANDBY rejected"); + warnx("ERROR: Navigation state MANUAL_STANDBY rejected 1"); } /* Try seatbelt or fallback to manual */ @@ -1879,7 +1880,7 @@ int commander_thread_main(int argc, char *argv[]) // fallback to MANUAL_STANDBY if (navigation_state_transition(stat_pub, ¤t_status, NAVIGATION_STATE_MANUAL_STANDBY, mavlink_fd) != OK) { // These is not supposed to happen - warnx("ERROR: Navigation state MANUAL_STANDBY rejected"); + warnx("ERROR: Navigation state MANUAL_STANDBY rejected 2"); } } @@ -1892,7 +1893,7 @@ int commander_thread_main(int argc, char *argv[]) // or fallback to MANUAL_STANDBY if (navigation_state_transition(stat_pub, ¤t_status, NAVIGATION_STATE_MANUAL_STANDBY, mavlink_fd) != OK) { // These is not supposed to happen - warnx("ERROR: Navigation state MANUAL_STANDBY rejected"); + warnx("ERROR: Navigation state MANUAL_STANDBY rejected 3"); } } } @@ -2040,7 +2041,8 @@ int commander_thread_main(int argc, char *argv[]) // ) && if ((sp_man.yaw < -STICK_ON_OFF_LIMIT) && (sp_man.throttle < STICK_THRUST_RANGE * 0.2f)) { if (stick_off_counter > STICK_ON_OFF_COUNTER_LIMIT) { - arming_state_transition(stat_pub, ¤t_status, mavlink_fd, ARMING_STATE_STANDBY); + printf("Try disarm\n"); + arming_state_transition(stat_pub, ¤t_status, ARMING_STATE_STANDBY, mavlink_fd); stick_off_counter = 0; } else { @@ -2052,7 +2054,8 @@ 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 (stick_on_counter > STICK_ON_OFF_COUNTER_LIMIT) { - arming_state_transition(stat_pub, ¤t_status, mavlink_fd, ARMING_STATE_ARMED); + printf("Try arm\n"); + arming_state_transition(stat_pub, ¤t_status, ARMING_STATE_ARMED, mavlink_fd); stick_on_counter = 0; } else { diff --git a/apps/commander/state_machine_helper.c b/apps/commander/state_machine_helper.c index 6c15bd7251..ae7f2a1c17 100644 --- a/apps/commander/state_machine_helper.c +++ b/apps/commander/state_machine_helper.c @@ -255,10 +255,13 @@ //} int arming_state_transition(int status_pub, struct vehicle_status_s *current_state, arming_state_t new_arming_state, const int mavlink_fd) { + int ret = ERROR; /* only check transition if the new state is actually different from the current one */ - if (new_arming_state != current_state->arming_state) { + if (new_arming_state == current_state->arming_state) { + ret = OK; + } else { switch (new_arming_state) { case ARMING_STATE_INIT: @@ -313,7 +316,13 @@ int arming_state_transition(int status_pub, struct vehicle_status_s *current_sta default: break; } + + if (ret == OK) { + current_state->arming_state = new_arming_state; + state_machine_publish(status_pub, current_state, mavlink_fd); + } } + return ret; } @@ -328,7 +337,9 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current int ret = ERROR; /* only check transition if the new state is actually different from the current one */ - if (new_navigation_state != current_state->navigation_state) { + if (new_navigation_state == current_state->navigation_state) { + ret = OK; + } else { switch (new_navigation_state) { case NAVIGATION_STATE_INIT: @@ -561,12 +572,14 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current default: break; } + + if (ret == OK) { + current_state->navigation_state = new_navigation_state; + state_machine_publish(status_pub, current_state, mavlink_fd); + } } - if (ret == OK) { - current_state->navigation_state = new_navigation_state; - state_machine_publish(status_pub, current_state, mavlink_fd); - } + return ret; } diff --git a/apps/sensors/sensors.cpp b/apps/sensors/sensors.cpp index ce404ee7e3..b53de8c467 100644 --- a/apps/sensors/sensors.cpp +++ b/apps/sensors/sensors.cpp @@ -432,7 +432,7 @@ Sensors::Sensors() : _parameter_handles.rc_map_throttle = param_find("RC_MAP_THROTTLE"); /* mandatory mode switches, mapped to channel 5 and 6 per default */ - _parameter_handles.rc_map_mode_sw = param_find("RC_MAP_OVER_SW"); + _parameter_handles.rc_map_mode_sw = param_find("RC_MAP_MODE_SW"); _parameter_handles.rc_map_return_sw = param_find("RC_MAP_RETURN_SW"); _parameter_handles.rc_map_flaps = param_find("RC_MAP_FLAPS"); From 0eca49a4f6d4a06868770c8b0c36094d889cb846 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Fri, 22 Feb 2013 19:46:47 -0800 Subject: [PATCH 014/109] Checkpoint: Separated all bools in vehicle status into conditions and flags, they should be protected --- apps/commander/commander.c | 107 +++++++++--------- apps/commander/state_machine_helper.c | 26 ++--- apps/drivers/blinkm/blinkm.cpp | 2 +- apps/drivers/px4io/px4io.cpp | 13 ++- apps/mavlink/mavlink.c | 4 +- apps/mavlink_onboard/mavlink.c | 2 +- .../multirotor_att_control_main.c | 7 +- apps/uORB/topics/vehicle_status.h | 43 +++---- 8 files changed, 102 insertions(+), 102 deletions(-) diff --git a/apps/commander/commander.c b/apps/commander/commander.c index 94e344da12..2fdcf4ce3f 100644 --- a/apps/commander/commander.c +++ b/apps/commander/commander.c @@ -323,8 +323,8 @@ void do_mag_calibration(int status_pub, struct vehicle_status_s *status) { /* set to mag calibration mode */ - status->flag_preflight_mag_calibration = true; - state_machine_publish(status_pub, status, mavlink_fd); + // status->flag_preflight_mag_calibration = true; + // state_machine_publish(status_pub, status, mavlink_fd); int sub_mag = orb_subscribe(ORB_ID(sensor_mag)); struct mag_report mag; @@ -559,8 +559,8 @@ void do_mag_calibration(int status_pub, struct vehicle_status_s *status) } /* disable calibration mode */ - status->flag_preflight_mag_calibration = false; - state_machine_publish(status_pub, status, mavlink_fd); + // status->flag_preflight_mag_calibration = false; + // state_machine_publish(status_pub, status, mavlink_fd); close(sub_mag); } @@ -568,8 +568,8 @@ void do_mag_calibration(int status_pub, struct vehicle_status_s *status) void do_gyro_calibration(int status_pub, struct vehicle_status_s *status) { /* set to gyro calibration mode */ - status->flag_preflight_gyro_calibration = true; - state_machine_publish(status_pub, status, mavlink_fd); + // status->flag_preflight_gyro_calibration = true; + // state_machine_publish(status_pub, status, mavlink_fd); const int calibration_count = 5000; @@ -621,8 +621,8 @@ void do_gyro_calibration(int status_pub, struct vehicle_status_s *status) gyro_offset[2] = gyro_offset[2] / calibration_count; /* exit gyro calibration mode */ - status->flag_preflight_gyro_calibration = false; - state_machine_publish(status_pub, status, mavlink_fd); + // status->flag_preflight_gyro_calibration = false; + // state_machine_publish(status_pub, status, mavlink_fd); if (isfinite(gyro_offset[0]) && isfinite(gyro_offset[1]) && isfinite(gyro_offset[2])) { @@ -679,8 +679,8 @@ void do_accel_calibration(int status_pub, struct vehicle_status_s *status) mavlink_log_info(mavlink_fd, "keep it level and still"); /* set to accel calibration mode */ - status->flag_preflight_accel_calibration = true; - state_machine_publish(status_pub, status, mavlink_fd); + // status->flag_preflight_accel_calibration = true; + // state_machine_publish(status_pub, status, mavlink_fd); const int calibration_count = 2500; @@ -787,8 +787,8 @@ void do_accel_calibration(int status_pub, struct vehicle_status_s *status) } /* exit accel calibration mode */ - status->flag_preflight_accel_calibration = false; - state_machine_publish(status_pub, status, mavlink_fd); + // status->flag_preflight_accel_calibration = false; + // state_machine_publish(status_pub, status, mavlink_fd); close(sub_sensor_combined); } @@ -1022,7 +1022,7 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta break; case VEHICLE_CMD_PREFLIGHT_STORAGE: { - if (current_status.flag_system_armed && + if (current_status.flag_fmu_armed && ((current_status.system_type == VEHICLE_TYPE_QUADROTOR) || (current_status.system_type == VEHICLE_TYPE_HEXAROTOR) || (current_status.system_type == VEHICLE_TYPE_OCTOROTOR))) { @@ -1327,7 +1327,8 @@ int commander_thread_main(int argc, char *argv[]) current_status.navigation_state = NAVIGATION_STATE_INIT; current_status.arming_state = ARMING_STATE_INIT; current_status.hil_state = HIL_STATE_OFF; - current_status.flag_system_armed = false; + current_status.flag_fmu_armed = false; + current_status.flag_io_armed = false; // XXX read this from somewhere /* neither manual nor offboard control commands have been received */ current_status.offboard_control_signal_found_once = false; @@ -1341,13 +1342,13 @@ int commander_thread_main(int argc, char *argv[]) current_status.flag_external_manual_override_ok = true; /* flag position info as bad, do not allow auto mode */ - current_status.flag_vector_flight_mode_ok = false; + // current_status.flag_vector_flight_mode_ok = false; /* set battery warning flag */ current_status.battery_warning = VEHICLE_BATTERY_WARNING_NONE; // XXX for now just set sensors as initialized - current_status.flag_system_sensors_initialized = true; + current_status.condition_system_sensors_initialized = true; /* advertise to ORB */ stat_pub = orb_advertise(ORB_ID(vehicle_status), ¤t_status); @@ -1501,7 +1502,7 @@ int commander_thread_main(int argc, char *argv[]) /* update parameters */ - if (!current_status.flag_system_armed) { + if (!current_status.flag_fmu_armed) { if (param_get(_param_sys_type, &(current_status.system_type)) != OK) { warnx("failed setting new system type"); } @@ -1654,7 +1655,7 @@ int commander_thread_main(int argc, char *argv[]) /* If in INIT state, try to proceed to STANDBY state */ if (current_status.arming_state == ARMING_STATE_INIT) { // XXX fix for now - current_status.flag_system_sensors_initialized = true; + current_status.condition_system_sensors_initialized = true; arming_state_transition(stat_pub, ¤t_status, ARMING_STATE_STANDBY, mavlink_fd); } else { // XXX: Add emergency stuff if sensors are lost @@ -1671,45 +1672,45 @@ int commander_thread_main(int argc, char *argv[]) */ /* store current state to reason later about a state change */ - bool vector_flight_mode_ok = current_status.flag_vector_flight_mode_ok; - bool global_pos_valid = current_status.flag_global_position_valid; - bool local_pos_valid = current_status.flag_local_position_valid; + // bool vector_flight_mode_ok = current_status.flag_vector_flight_mode_ok; + bool global_pos_valid = current_status.condition_global_position_valid; + bool local_pos_valid = current_status.condition_local_position_valid; /* check for global or local position updates, set a timeout of 2s */ if (hrt_absolute_time() - last_global_position_time < 2000000) { - current_status.flag_global_position_valid = true; + current_status.condition_global_position_valid = true; // XXX check for controller status and home position as well } else { - current_status.flag_global_position_valid = false; + current_status.condition_global_position_valid = false; } if (hrt_absolute_time() - last_local_position_time < 2000000) { - current_status.flag_local_position_valid = true; + current_status.condition_local_position_valid = true; // XXX check for controller status and home position as well } else { - current_status.flag_local_position_valid = false; + current_status.condition_local_position_valid = false; } /* * Consolidate global position and local position valid flags * for vector flight mode. */ - if (current_status.flag_local_position_valid || - current_status.flag_global_position_valid) { - current_status.flag_vector_flight_mode_ok = true; + // if (current_status.condition_local_position_valid || + // current_status.condition_global_position_valid) { + // current_status.flag_vector_flight_mode_ok = true; - } else { - current_status.flag_vector_flight_mode_ok = false; - } + // } else { + // current_status.flag_vector_flight_mode_ok = false; + // } - /* consolidate state change, flag as changed if required */ - if (vector_flight_mode_ok != current_status.flag_vector_flight_mode_ok || - global_pos_valid != current_status.flag_global_position_valid || - local_pos_valid != current_status.flag_local_position_valid) { - state_changed = true; - } + // /* consolidate state change, flag as changed if required */ + // if (vector_flight_mode_ok != current_status.flag_vector_flight_mode_ok || + // global_pos_valid != current_status.flag_global_position_valid || + // local_pos_valid != current_status.flag_local_position_valid) { + // state_changed = true; + // } /* * Mark the position of the first position lock as return to launch (RTL) @@ -1721,16 +1722,16 @@ int commander_thread_main(int argc, char *argv[]) * 2) The system has not aquired position lock before * 3) The system is not armed (on the ground) */ - if (!current_status.flag_valid_launch_position && - !vector_flight_mode_ok && current_status.flag_vector_flight_mode_ok && - !current_status.flag_system_armed) { - /* first time a valid position, store it and emit it */ + // if (!current_status.flag_valid_launch_position && + // !vector_flight_mode_ok && current_status.flag_vector_flight_mode_ok && + // !current_status.flag_system_armed) { + // first time a valid position, store it and emit it - // XXX implement storage and publication of RTL position - current_status.flag_valid_launch_position = true; - current_status.flag_auto_flight_mode_ok = true; - state_changed = true; - } + // // XXX implement storage and publication of RTL position + // current_status.flag_valid_launch_position = true; + // current_status.flag_auto_flight_mode_ok = true; + // state_changed = true; + // } if (orb_check(gps_sub, &new_data)) { @@ -1760,7 +1761,7 @@ int commander_thread_main(int argc, char *argv[]) && (vdop_m < vdop_threshold_m) // XXX note that vdop is 0 for mtk && !home_position_set && (hrt_absolute_time() - gps_position.timestamp_position < 2000000) - && !current_status.flag_system_armed) { + && !current_status.flag_fmu_armed) { warnx("setting home position"); /* copy position data to uORB home message, store it locally as well */ @@ -1876,7 +1877,7 @@ int commander_thread_main(int argc, char *argv[]) if (navigation_state_transition(stat_pub, ¤t_status, NAVIGATION_STATE_MANUAL_STANDBY, mavlink_fd) != OK) { // These is not supposed to happen - warnx("ERROR: Navigation state MANUAL_STANDBY rejected 1"); + warnx("ERROR: Navigation state MANUAL_STANDBY rejected"); } /* Try seatbelt or fallback to manual */ @@ -1886,7 +1887,7 @@ int commander_thread_main(int argc, char *argv[]) // fallback to MANUAL_STANDBY if (navigation_state_transition(stat_pub, ¤t_status, NAVIGATION_STATE_MANUAL_STANDBY, mavlink_fd) != OK) { // These is not supposed to happen - warnx("ERROR: Navigation state MANUAL_STANDBY rejected 2"); + warnx("ERROR: Navigation state MANUAL_STANDBY rejected"); } } @@ -1899,7 +1900,7 @@ int commander_thread_main(int argc, char *argv[]) // or fallback to MANUAL_STANDBY if (navigation_state_transition(stat_pub, ¤t_status, NAVIGATION_STATE_MANUAL_STANDBY, mavlink_fd) != OK) { // These is not supposed to happen - warnx("ERROR: Navigation state MANUAL_STANDBY rejected 3"); + warnx("ERROR: Navigation state MANUAL_STANDBY rejected"); } } } @@ -2047,7 +2048,6 @@ int commander_thread_main(int argc, char *argv[]) // ) && if ((sp_man.yaw < -STICK_ON_OFF_LIMIT) && (sp_man.throttle < STICK_THRUST_RANGE * 0.2f)) { if (stick_off_counter > STICK_ON_OFF_COUNTER_LIMIT) { - printf("Try disarm\n"); arming_state_transition(stat_pub, ¤t_status, ARMING_STATE_STANDBY, mavlink_fd); stick_off_counter = 0; @@ -2060,7 +2060,6 @@ 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 (stick_on_counter > STICK_ON_OFF_COUNTER_LIMIT) { - printf("Try arm\n"); arming_state_transition(stat_pub, ¤t_status, ARMING_STATE_ARMED, mavlink_fd); stick_on_counter = 0; @@ -2156,13 +2155,13 @@ int commander_thread_main(int argc, char *argv[]) current_status.offboard_control_signal_lost_interval = 0; /* arm / disarm on request */ - if (sp_offboard.armed && !current_status.flag_system_armed) { + if (sp_offboard.armed && !current_status.flag_fmu_armed) { #warning fix this // update_state_machine_arm(stat_pub, ¤t_status, mavlink_fd); /* switch to stabilized mode = takeoff */ // update_state_machine_mode_stabilized(stat_pub, ¤t_status, mavlink_fd); - } else if (!sp_offboard.armed && current_status.flag_system_armed) { + } else if (!sp_offboard.armed && current_status.flag_fmu_armed) { // update_state_machine_disarm(stat_pub, ¤t_status, mavlink_fd); } diff --git a/apps/commander/state_machine_helper.c b/apps/commander/state_machine_helper.c index ae7f2a1c17..61ebe8c165 100644 --- a/apps/commander/state_machine_helper.c +++ b/apps/commander/state_machine_helper.c @@ -278,7 +278,7 @@ int arming_state_transition(int status_pub, struct vehicle_status_s *current_sta || current_state->arming_state == ARMING_STATE_ARMED) { /* sensors need to be initialized for STANDBY state */ - if (current_state->flag_system_sensors_initialized) { + if (current_state->condition_system_sensors_initialized) { ret = OK; } else { mavlink_log_critical(mavlink_fd, "Rej. STANDBY state, sensors not initialized"); @@ -392,7 +392,7 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current /* need to be disarmed and have a position estimate */ if (current_state->arming_state != ARMING_STATE_STANDBY) { mavlink_log_critical(mavlink_fd, "Rej. SEATBELT_STANDBY: not disarmed"); - } else if (!current_state->flag_local_position_valid) { + } else if (!current_state->condition_local_position_valid) { mavlink_log_critical(mavlink_fd, "Rej. SEATBELT_STANDBY: no position estimate"); } else { ret = OK; @@ -416,7 +416,7 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current /* need to be armed and have a position estimate */ if (current_state->arming_state != ARMING_STATE_ARMED) { mavlink_log_critical(mavlink_fd, "Rej. SEATBELT: not armed"); - } else if (!current_state->flag_local_position_valid) { + } else if (!current_state->condition_local_position_valid) { mavlink_log_critical(mavlink_fd, "Rej. SEATBELT: no pos estimate"); } else { ret = OK; @@ -439,7 +439,7 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current /* need to be armed and have a position estimate */ if (current_state->arming_state != ARMING_STATE_ARMED) { mavlink_log_critical(mavlink_fd, "Rej. SEATBELT_DESCENT: not armed"); - } else if (!current_state->flag_local_position_valid) { + } else if (!current_state->condition_local_position_valid) { mavlink_log_critical(mavlink_fd, "Rej. SEATBELT_DESCENT: no pos estimate"); } else { ret = OK; @@ -458,9 +458,9 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current /* need to be disarmed and have a position and home lock */ if (current_state->arming_state != ARMING_STATE_STANDBY) { mavlink_log_critical(mavlink_fd, "Rej. AUTO_STANDBY: not disarmed"); - } else if (!current_state->flag_global_position_valid) { + } else if (!current_state->condition_global_position_valid) { mavlink_log_critical(mavlink_fd, "Rej. AUTO_STANDBY: no pos lock"); - } else if (!current_state->flag_valid_home_position) { + } else if (!current_state->condition_home_position_valid) { mavlink_log_critical(mavlink_fd, "Rej. AUTO_STANDBY: no home pos"); } else { ret = OK; @@ -504,9 +504,9 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current || current_state->navigation_state == NAVIGATION_STATE_MANUAL) { /* need to have a position and home lock */ - if (!current_state->flag_global_position_valid) { + if (!current_state->condition_global_position_valid) { mavlink_log_critical(mavlink_fd, "Rej. AUTO_LOITER: no pos lock"); - } else if (!current_state->flag_valid_home_position) { + } else if (!current_state->condition_home_position_valid) { mavlink_log_critical(mavlink_fd, "Rej. AUTO_LOITER: no home pos"); } else { ret = OK; @@ -524,7 +524,7 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current || current_state->navigation_state == NAVIGATION_STATE_MANUAL) { /* need to have a mission ready */ - if (!current_state->flag_auto_mission_available) { + if (!current_state-> condition_auto_mission_available) { mavlink_log_critical(mavlink_fd, "Rej. AUTO_MISSION: no mission available"); } else { ret = OK; @@ -542,9 +542,9 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current || current_state->navigation_state == NAVIGATION_STATE_MANUAL) { /* need to have a position and home lock */ - if (!current_state->flag_global_position_valid) { + if (!current_state->condition_global_position_valid) { mavlink_log_critical(mavlink_fd, "Rej. AUTO_RTL: no pos lock"); - } else if (!current_state->flag_valid_home_position) { + } else if (!current_state->condition_home_position_valid) { mavlink_log_critical(mavlink_fd, "Rej. AUTO_RTL: no home pos"); } else { ret = OK; @@ -559,9 +559,9 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current || current_state->navigation_state == NAVIGATION_STATE_AUTO_LOITER) { /* need to have a position and home lock */ - if (!current_state->flag_global_position_valid) { + if (!current_state->condition_global_position_valid) { mavlink_log_critical(mavlink_fd, "Rej. AUTO_LAND: no pos lock"); - } else if (!current_state->flag_valid_home_position) { + } else if (!current_state->condition_home_position_valid) { mavlink_log_critical(mavlink_fd, "Rej. AUTO_LAND: no home pos"); } else { ret = OK; diff --git a/apps/drivers/blinkm/blinkm.cpp b/apps/drivers/blinkm/blinkm.cpp index 6aff27e4ca..ceb2d987d4 100644 --- a/apps/drivers/blinkm/blinkm.cpp +++ b/apps/drivers/blinkm/blinkm.cpp @@ -529,7 +529,7 @@ BlinkM::led() } else { /* no battery warnings here */ - if(vehicle_status_raw.flag_system_armed == false) { + if(vehicle_status_raw.flag_fmu_armed == false) { /* system not armed */ led_color_1 = LED_RED; led_color_2 = LED_RED; diff --git a/apps/drivers/px4io/px4io.cpp b/apps/drivers/px4io/px4io.cpp index adb894371f..99f0f35b2d 100644 --- a/apps/drivers/px4io/px4io.cpp +++ b/apps/drivers/px4io/px4io.cpp @@ -446,7 +446,7 @@ PX4IO::init() } /* keep waiting for state change for 10 s */ - } while (!status.flag_system_armed); + } while (!status.flag_fmu_armed); /* regular boot, no in-air restart, init IO */ } else { @@ -658,11 +658,12 @@ PX4IO::io_set_arming_state() } else { clear |= PX4IO_P_SETUP_ARMING_ARM_OK; } - if (vstatus.flag_vector_flight_mode_ok) { - set |= PX4IO_P_SETUP_ARMING_VECTOR_FLIGHT_OK; - } else { - clear |= PX4IO_P_SETUP_ARMING_VECTOR_FLIGHT_OK; - } + // TODO fix this + // if (vstatus.flag_vector_flight_mode_ok) { + // set |= PX4IO_P_SETUP_ARMING_VECTOR_FLIGHT_OK; + // } else { + // clear |= PX4IO_P_SETUP_ARMING_VECTOR_FLIGHT_OK; + // } if (vstatus.flag_external_manual_override_ok) { set |= PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK; } else { diff --git a/apps/mavlink/mavlink.c b/apps/mavlink/mavlink.c index 34b267d56b..3423287283 100644 --- a/apps/mavlink/mavlink.c +++ b/apps/mavlink/mavlink.c @@ -246,9 +246,7 @@ get_mavlink_mode_and_state(uint8_t *mavlink_state, uint8_t *mavlink_mode) **/ /* set calibration state */ - if (v_status.flag_preflight_gyro_calibration || - v_status.flag_preflight_mag_calibration || - v_status.flag_preflight_accel_calibration) { + if (v_status.flag_preflight_calibration) { *mavlink_state = MAV_STATE_CALIBRATING; diff --git a/apps/mavlink_onboard/mavlink.c b/apps/mavlink_onboard/mavlink.c index c5a1e82a88..fbfce7dc95 100644 --- a/apps/mavlink_onboard/mavlink.c +++ b/apps/mavlink_onboard/mavlink.c @@ -290,7 +290,7 @@ get_mavlink_mode_and_state(const struct vehicle_status_s *v_status, const struct } /* set arming state */ - if (v_status->flag_system_armed) { + if (v_status->flag_fmu_armed) { *mavlink_mode |= MAV_MODE_FLAG_SAFETY_ARMED; } else { *mavlink_mode &= ~MAV_MODE_FLAG_SAFETY_ARMED; diff --git a/apps/multirotor_att_control/multirotor_att_control_main.c b/apps/multirotor_att_control/multirotor_att_control_main.c index da7550f797..79ca9fe2d2 100644 --- a/apps/multirotor_att_control/multirotor_att_control_main.c +++ b/apps/multirotor_att_control/multirotor_att_control_main.c @@ -247,7 +247,7 @@ mc_thread_main(int argc, char *argv[]) /* initialize to current yaw if switching to manual or att control */ if (state.flag_control_attitude_enabled != flag_control_attitude_enabled || state.flag_control_manual_enabled != flag_control_manual_enabled || - state.flag_system_armed != flag_system_armed) { + state.flag_fmu_armed != flag_system_armed) { att_sp.yaw_body = att.yaw; } @@ -291,7 +291,7 @@ mc_thread_main(int argc, char *argv[]) att_sp.pitch_body = manual.pitch; /* set attitude if arming */ - if (!flag_control_attitude_enabled && state.flag_system_armed) { + if (!flag_control_attitude_enabled && state.flag_fmu_armed) { att_sp.yaw_body = att.yaw; } @@ -395,7 +395,8 @@ mc_thread_main(int argc, char *argv[]) /* update state */ flag_control_attitude_enabled = state.flag_control_attitude_enabled; flag_control_manual_enabled = state.flag_control_manual_enabled; - flag_system_armed = state.flag_system_armed; + flag_system_armed = state.flag_fmu_armed; + // XXX add some logic to this perf_end(mc_loop_perf); } /* end of poll call for attitude updates */ diff --git a/apps/uORB/topics/vehicle_status.h b/apps/uORB/topics/vehicle_status.h index 4955422441..eba5a5047e 100644 --- a/apps/uORB/topics/vehicle_status.h +++ b/apps/uORB/topics/vehicle_status.h @@ -183,31 +183,35 @@ struct vehicle_status_s return_switch_pos_t return_switch; mission_switch_pos_t mission_switch; + + bool condition_system_emergency; + bool condition_system_in_air_restore; /**< true if we can restore in mid air */ + bool condition_system_sensors_initialized; + bool condition_system_returned_to_home; + bool condition_auto_mission_available; + bool condition_global_position_valid; /**< set to true by the commander app if the quality of the gps signal is good enough to use it in the position estimator */ + bool condition_launch_position_valid; /**< indicates a valid launch position */ + bool condition_home_position_valid; /**< indicates a valid home position (a valid home position is not always a valid launch) */ + bool condition_local_position_valid; - bool flag_system_armed; /**< true is motors / actuators are armed */ + bool flag_hil_enabled; /**< true if hardware in the loop simulation is enabled */ + bool flag_fmu_armed; /**< true is motors / actuators of FMU are armed */ + bool flag_io_armed; /**< true is motors / actuators of IO are armed */ bool flag_system_emergency; - bool flag_system_in_air_restore; /**< true if we can restore in mid air */ - bool flag_system_sensors_initialized; - bool flag_system_arming_requested; - bool flag_system_disarming_requested; - bool flag_system_reboot_requested; - bool flag_system_returned_to_home; - - bool flag_auto_mission_available; - bool flag_auto_enabled; + bool flag_preflight_calibration; bool flag_control_manual_enabled; /**< true if manual input is mixed in */ bool flag_control_offboard_enabled; /**< true if offboard control input is on */ - bool flag_hil_enabled; /**< true if hardware in the loop simulation is enabled */ - + bool flag_auto_enabled; bool flag_control_rates_enabled; /**< true if rates are stabilized */ bool flag_control_attitude_enabled; /**< true if attitude stabilization is mixed in */ bool flag_control_velocity_enabled; /**< true if speed (implies direction) is controlled */ bool flag_control_position_enabled; /**< true if position is controlled */ - bool flag_preflight_gyro_calibration; /**< true if gyro calibration is requested */ - bool flag_preflight_mag_calibration; /**< true if mag calibration is requested */ - bool flag_preflight_accel_calibration; + + // bool flag_preflight_gyro_calibration; /**< true if gyro calibration is requested */ + // bool flag_preflight_mag_calibration; /**< true if mag calibration is requested */ + // bool flag_preflight_accel_calibration; bool rc_signal_found_once; bool rc_signal_lost; /**< true if RC reception is terminally lost */ @@ -238,13 +242,10 @@ struct vehicle_status_s uint16_t errors_count3; uint16_t errors_count4; - bool flag_global_position_valid; /**< set to true by the commander app if the quality of the gps signal is good enough to use it in the position estimator */ - bool flag_local_position_valid; - bool flag_vector_flight_mode_ok; /**< position estimation, battery voltage and other critical subsystems are good for autonomous flight */ - bool flag_auto_flight_mode_ok; /**< conditions of vector flight mode apply plus a valid takeoff position lock has been aquired */ + // bool flag_vector_flight_mode_ok; /**< position estimation, battery voltage and other critical subsystems are good for autonomous flight */ + // bool flag_auto_flight_mode_ok; /**< conditions of vector flight mode apply plus a valid takeoff position lock has been aquired */ bool flag_external_manual_override_ok; /**< external override non-fatal for system. Only true for fixed wing */ - bool flag_valid_launch_position; /**< indicates a valid launch position */ - bool flag_valid_home_position; /**< indicates a valid home position (a valid home position is not always a valid launch) */ + }; /** From 34c84752d1ff7494529dfea8e72f3c090b451b3c Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Mon, 25 Feb 2013 09:24:30 -0800 Subject: [PATCH 015/109] Checkpoint: Added control flags --- apps/commander/state_machine_helper.c | 576 +++----------------------- 1 file changed, 59 insertions(+), 517 deletions(-) diff --git a/apps/commander/state_machine_helper.c b/apps/commander/state_machine_helper.c index 61ebe8c165..742b7fe072 100644 --- a/apps/commander/state_machine_helper.c +++ b/apps/commander/state_machine_helper.c @@ -53,207 +53,6 @@ #include "state_machine_helper.h" -/** - * Transition from one sytem state to another - */ -//void navigation_state_update(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd) -//{ -// int ret = ERROR; -// navigation_state_t new_navigation_state; -// -// /* do the navigation state update depending on the arming state */ -// switch (current_status->arming_state) { -// -// /* evaluate the navigation state when disarmed */ -// case ARMING_STATE_STANDBY: -// -// /* Always accept manual mode */ -// if (current_status->mode_switch == MODE_SWITCH_MANUAL) { -// new_navigation_state = NAVIGATION_STATE_MANUAL_STANDBY; -// -// /* Accept SEATBELT if there is a local position estimate */ -// } else if (current_status->mode_switch == MODE_SWITCH_SEATBELT) { -// -// if (current_status->flag_local_position_valid) { -// new_navigation_state = NAVIGATION_STATE_SEATBELT_STANDBY; -// } else { -// /* or just fall back to manual */ -// mavlink_log_critical(mavlink_fd, "Rej. SEATBELT: no local position"); -// new_navigation_state = NAVIGATION_STATE_MANUAL_STANDBY; -// } -// -// /* Accept AUTO if there is a global position estimate */ -// } else if (current_status->mode_switch == MODE_SWITCH_AUTO) { -// if (current_status->flag_local_position_valid) { -// new_navigation_state = NAVIGATION_STATE_SEATBELT_STANDBY; -// } else { -// mavlink_log_critical(mavlink_fd, "Rej. AUTO: no global position"); -// -// /* otherwise fallback to seatbelt or even manual */ -// if (current_status->flag_local_position_valid) { -// new_navigation_state = NAVIGATION_STATE_SEATBELT_STANDBY; -// } else { -// mavlink_log_critical(mavlink_fd, "Rej. SEATBELT: no local position"); -// new_navigation_state = NAVIGATION_STATE_MANUAL_STANDBY; -// } -// } -// } -// -// break; -// -// /* evaluate the navigation state when armed */ -// case ARMING_STATE_ARMED: -// -// /* Always accept manual mode */ -// if (current_status->mode_switch == MODE_SWITCH_MANUAL) { -// new_navigation_state = NAVIGATION_STATE_MANUAL; -// -// /* Accept SEATBELT if there is a local position estimate */ -// } else if (current_status->mode_switch == MODE_SWITCH_SEATBELT -// && current_status->return_switch == RETURN_SWITCH_NONE) { -// -// if (current_status->flag_local_position_valid) { -// new_navigation_state = NAVIGATION_STATE_SEATBELT; -// } else { -// /* or just fall back to manual */ -// mavlink_log_critical(mavlink_fd, "Rej. SEATBELT: no local position"); -// new_navigation_state = NAVIGATION_STATE_MANUAL; -// } -// -// /* Accept SEATBELT_DESCENT if there is a local position estimate and the return switch is on */ -// } else if (current_status->mode_switch == MODE_SWITCH_SEATBELT -// && current_status->return_switch == RETURN_SWITCH_RETURN) { -// -// if (current_status->flag_local_position_valid) { -// new_navigation_state = NAVIGATION_STATE_SEATBELT_DESCENT; -// } else { -// /* descent not possible without baro information, fall back to manual */ -// mavlink_log_critical(mavlink_fd, "Rej. SEATBELT_DESCENT: no local position"); -// new_navigation_state = NAVIGATION_STATE_MANUAL; -// } -// -// /* Accept LOITER if there is a global position estimate */ -// } else if (current_status->mode_switch == MODE_SWITCH_AUTO -// && current_status->return_switch == RETURN_SWITCH_NONE -// && current_status->mission_switch == MISSION_SWITCH_NONE) { -// -// if (current_status->flag_global_position_valid) { -// new_navigation_state = NAVIGATION_STATE_AUTO_LOITER; -// } else { -// mavlink_log_critical(mavlink_fd, "Rej. LOITER: no global position"); -// -// /* otherwise fallback to SEATBELT or even manual */ -// if (current_status->flag_local_position_valid) { -// new_navigation_state = NAVIGATION_STATE_SEATBELT; -// } else { -// mavlink_log_critical(mavlink_fd, "Rej. SEATBELT: no local position"); -// new_navigation_state = NAVIGATION_STATE_MANUAL; -// } -// } -// -// /* Accept MISSION if there is a global position estimate and home position */ -// } else if (current_status->mode_switch == MODE_SWITCH_AUTO -// && current_status->return_switch == RETURN_SWITCH_NONE -// && current_status->mission_switch == MISSION_SWITCH_MISSION) { -// -// if (current_status->flag_global_position_valid && current_status->flag_valid_home_position) { -// new_navigation_state = NAVIGATION_STATE_AUTO_MISSION; -// } else { -// -// /* spit out what exactly is unavailable */ -// if (current_status->flag_global_position_valid) { -// mavlink_log_critical(mavlink_fd, "Rej. MISSION: no home position"); -// } else if (current_status->flag_valid_home_position) { -// mavlink_log_critical(mavlink_fd, "Rej. MISSION: no global position"); -// } else { -// mavlink_log_critical(mavlink_fd, "Rej. MISSION: no global position and no home position"); -// } -// -// /* otherwise fallback to SEATBELT or even manual */ -// if (current_status->flag_local_position_valid) { -// new_navigation_state = NAVIGATION_STATE_SEATBELT; -// } else { -// mavlink_log_critical(mavlink_fd, "Rej. SEATBELT: no local position"); -// new_navigation_state = NAVIGATION_STATE_MANUAL; -// } -// } -// -// /* Go to RTL or land if global position estimate and home position is available */ -// } else if (current_status->mode_switch == MODE_SWITCH_AUTO -// && current_status->return_switch == RETURN_SWITCH_RETURN -// && (current_status->mission_switch == MISSION_SWITCH_NONE || current_status->mission_switch == MISSION_SWITCH_MISSION)) { -// -// if (current_status->flag_global_position_valid && current_status->flag_valid_home_position) { -// -// /* after RTL go to LAND */ -// if (current_status->flag_system_returned_to_home) { -// new_navigation_state = NAVIGATION_STATE_AUTO_LAND; -// } else { -// new_navigation_state = NAVIGATION_STATE_AUTO_RTL; -// } -// -// } else { -// -// /* spit out what exactly is unavailable */ -// if (current_status->flag_global_position_valid) { -// mavlink_log_critical(mavlink_fd, "Rej. RTL/LAND: no home position"); -// } else if (current_status->flag_valid_home_position) { -// mavlink_log_critical(mavlink_fd, "Rej. RTL/LAND: no global position"); -// } else { -// mavlink_log_critical(mavlink_fd, "Rej. RTL/LAND: no global position and no home position"); -// } -// -// /* otherwise fallback to SEATBELT_DESCENT or even MANUAL */ -// if (current_status->flag_local_position_valid) { -// new_navigation_state = NAVIGATION_STATE_SEATBELT_DESCENT; -// } else { -// mavlink_log_critical(mavlink_fd, "Rej. SEATBELT: no local position"); -// new_navigation_state = NAVIGATION_STATE_MANUAL; -// } -// } -// } -// break; -// -// case ARMING_STATE_ARMED_ERROR: -// -// // TODO work out fail-safe scenarios -// break; -// -// case ARMING_STATE_STANDBY_ERROR: -// -// // TODO work out fail-safe scenarios -// break; -// -// case ARMING_STATE_REBOOT: -// -// // XXX I don't think we should end up here -// break; -// -// case ARMING_STATE_IN_AIR_RESTORE: -// -// // XXX not sure what to do here -// break; -// default: -// break; -// } -// -// -// -// /* Update the system state in case it has changed */ -// if (current_status->navigation_state != new_navigation_state) { -// -// /* Check if the transition is valid */ -// if (check_navigation_state_transition(current_status->navigation_state, new_navigation_state) == OK) { -// current_status->navigation_state = new_navigation_state; -// state_machine_publish(status_pub, current_status, mavlink_fd); -// } else { -// mavlink_log_critical(mavlink_fd, "System state transition not valid"); -// } -// } -// -// return; -//} - int arming_state_transition(int status_pub, struct vehicle_status_s *current_state, arming_state_t new_arming_state, const int mavlink_fd) { int ret = ERROR; @@ -269,6 +68,7 @@ int arming_state_transition(int status_pub, struct vehicle_status_s *current_sta /* allow going back from INIT for calibration */ if (current_state->arming_state == ARMING_STATE_STANDBY) { ret = OK; + current_state->flag_system_armed = false; } break; case ARMING_STATE_STANDBY: @@ -280,10 +80,10 @@ int arming_state_transition(int status_pub, struct vehicle_status_s *current_sta /* sensors need to be initialized for STANDBY state */ if (current_state->condition_system_sensors_initialized) { ret = OK; + current_state->flag_system_armed = false; } else { mavlink_log_critical(mavlink_fd, "Rej. STANDBY state, sensors not initialized"); } - } break; case ARMING_STATE_ARMED: @@ -294,15 +94,17 @@ int arming_state_transition(int status_pub, struct vehicle_status_s *current_sta /* XXX conditions for arming? */ ret = OK; + current_state->flag_system_armed = true; } break; case ARMING_STATE_ARMED_ERROR: /* an armed error happens when ARMED obviously */ if (current_state->arming_state == ARMING_STATE_ARMED) { - ret = OK; - + /* XXX conditions for an error state? */ + ret = OK; + current_state->flag_system_armed = true; } break; case ARMING_STATE_STANDBY_ERROR: @@ -311,6 +113,7 @@ int arming_state_transition(int status_pub, struct vehicle_status_s *current_sta || current_state->arming_state == ARMING_STATE_INIT || current_state->arming_state == ARMING_STATE_ARMED_ERROR) { ret = OK; + current_state->flag_system_armed = false; } break; default: @@ -350,6 +153,10 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current || current_state->navigation_state == NAVIGATION_STATE_AUTO_STANDBY) { ret = OK; + current_state->flag_control_rates_enabled = false; + current_state->flag_control_attitude_enabled = false; + current_state->flag_control_velocity_enabled = false; + current_state->flag_control_position_enabled = false; } break; @@ -366,6 +173,10 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current mavlink_log_critical(mavlink_fd, "Rej. MANUAL_STANDBY: not disarmed"); } else { ret = OK; + current_state->flag_control_rates_enabled = true; + current_state->flag_control_attitude_enabled = true; + current_state->flag_control_velocity_enabled = false; + current_state->flag_control_position_enabled = false; } } break; @@ -377,6 +188,10 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current mavlink_log_critical(mavlink_fd, "Rej. MANUAL: not armed"); } else { ret = OK; + current_state->flag_control_rates_enabled = true; + current_state->flag_control_attitude_enabled = true; + current_state->flag_control_velocity_enabled = false; + current_state->flag_control_position_enabled = false; } break; @@ -396,6 +211,10 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current mavlink_log_critical(mavlink_fd, "Rej. SEATBELT_STANDBY: no position estimate"); } else { ret = OK; + current_state->flag_control_rates_enabled = true; + current_state->flag_control_attitude_enabled = true; + current_state->flag_control_velocity_enabled = true; + current_state->flag_control_position_enabled = false; } } break; @@ -420,6 +239,10 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current mavlink_log_critical(mavlink_fd, "Rej. SEATBELT: no pos estimate"); } else { ret = OK; + current_state->flag_control_rates_enabled = true; + current_state->flag_control_attitude_enabled = true; + current_state->flag_control_velocity_enabled = true; + current_state->flag_control_position_enabled = false; } } break; @@ -443,6 +266,10 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current mavlink_log_critical(mavlink_fd, "Rej. SEATBELT_DESCENT: no pos estimate"); } else { ret = OK; + current_state->flag_control_rates_enabled = true; + current_state->flag_control_attitude_enabled = true; + current_state->flag_control_velocity_enabled = true; + current_state->flag_control_position_enabled = false; } } break; @@ -464,6 +291,10 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current mavlink_log_critical(mavlink_fd, "Rej. AUTO_STANDBY: no home pos"); } else { ret = OK; + current_state->flag_control_rates_enabled = true; + current_state->flag_control_attitude_enabled = true; + current_state->flag_control_velocity_enabled = true; + current_state->flag_control_position_enabled = true; } } break; @@ -481,6 +312,10 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current mavlink_log_critical(mavlink_fd, "Rej. AUTO_READY: not armed"); } else { ret = OK; + current_state->flag_control_rates_enabled = true; + current_state->flag_control_attitude_enabled = true; + current_state->flag_control_velocity_enabled = true; + current_state->flag_control_position_enabled = true; } } break; @@ -491,6 +326,10 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current if (current_state->navigation_state == NAVIGATION_STATE_AUTO_READY) { ret = OK; + current_state->flag_control_rates_enabled = true; + current_state->flag_control_attitude_enabled = true; + current_state->flag_control_velocity_enabled = true; + current_state->flag_control_position_enabled = true; } break; @@ -510,6 +349,10 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current mavlink_log_critical(mavlink_fd, "Rej. AUTO_LOITER: no home pos"); } else { ret = OK; + current_state->flag_control_rates_enabled = true; + current_state->flag_control_attitude_enabled = true; + current_state->flag_control_velocity_enabled = true; + current_state->flag_control_position_enabled = true; } } break; @@ -528,6 +371,10 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current mavlink_log_critical(mavlink_fd, "Rej. AUTO_MISSION: no mission available"); } else { ret = OK; + current_state->flag_control_rates_enabled = true; + current_state->flag_control_attitude_enabled = true; + current_state->flag_control_velocity_enabled = true; + current_state->flag_control_position_enabled = true; } } break; @@ -548,6 +395,10 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current mavlink_log_critical(mavlink_fd, "Rej. AUTO_RTL: no home pos"); } else { ret = OK; + current_state->flag_control_rates_enabled = true; + current_state->flag_control_attitude_enabled = true; + current_state->flag_control_velocity_enabled = true; + current_state->flag_control_position_enabled = true; } } break; @@ -565,6 +416,10 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current mavlink_log_critical(mavlink_fd, "Rej. AUTO_LAND: no home pos"); } else { ret = OK; + current_state->flag_control_rates_enabled = true; + current_state->flag_control_attitude_enabled = true; + current_state->flag_control_velocity_enabled = true; + current_state->flag_control_position_enabled = true; } } break; @@ -683,38 +538,6 @@ void state_machine_publish(int status_pub, struct vehicle_status_s *current_stat //} -/* - * Private functions, update the state machine - */ -//void state_machine_emergency_always_critical(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd) -//{ -// warnx("EMERGENCY HANDLER\n"); -// /* Depending on the current state go to one of the error states */ -// -// if (current_status->state_machine == SYSTEM_STATE_PREFLIGHT || current_status->state_machine == SYSTEM_STATE_STANDBY || current_status->state_machine == SYSTEM_STATE_GROUND_READY) { -// do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_GROUND_ERROR); -// -// } else if (current_status->state_machine == SYSTEM_STATE_AUTO || current_status->state_machine == SYSTEM_STATE_MANUAL) { -// -// // DO NOT abort mission -// //do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_MISSION_ABORT); -// -// } else { -// warnx("Unknown system state: #%d\n", current_status->state_machine); -// } -//} - -//void state_machine_emergency(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd) //do not call state_machine_emergency_always_critical if we are in manual mode for these errors -//{ -// if (current_status->state_machine != SYSTEM_STATE_MANUAL) { //if we are in manual: user can react to errors themself -// state_machine_emergency_always_critical(status_pub, current_status, mavlink_fd); -// -// } else { -// //global_data_send_mavlink_statustext_message_out("[cmd] ERROR: take action immediately! (did not switch to error state because the system is in manual mode)", MAV_SEVERITY_CRITICAL); -// } -// -//} - // /* @@ -827,91 +650,6 @@ void state_machine_publish(int status_pub, struct vehicle_status_s *current_stat // } -// void update_state_machine_subsystem_healthy(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type) -// { -// current_status->onboard_control_sensors_health |= 1 << *subsystem_type; -// current_status->counter++; -// current_status->timestamp = hrt_absolute_time(); -// orb_publish(ORB_ID(vehicle_status), status_pub, current_status); - -// switch (*subsystem_type) { -// case SUBSYSTEM_TYPE_GYRO: -// //TODO state machine change (recovering) -// break; - -// case SUBSYSTEM_TYPE_ACC: -// //TODO state machine change -// break; - -// case SUBSYSTEM_TYPE_MAG: -// //TODO state machine change -// break; - -// case SUBSYSTEM_TYPE_GPS: -// //TODO state machine change -// break; - -// default: -// break; -// } - - -// } - - -// void update_state_machine_subsystem_unhealthy(int status_pub, struct vehicle_status_s *current_status, subsystem_type_t *subsystem_type) -// { -// bool previosly_healthy = (bool)(current_status->onboard_control_sensors_health & 1 << *subsystem_type); -// current_status->onboard_control_sensors_health &= ~(1 << *subsystem_type); -// current_status->counter++; -// current_status->timestamp = hrt_absolute_time(); -// orb_publish(ORB_ID(vehicle_status), status_pub, current_status); - -// /* if we received unhealthy message more than *_HEALTH_COUNTER_LIMIT, switch to error state */ - -// switch (*subsystem_type) { -// case SUBSYSTEM_TYPE_GYRO: -// //global_data_send_mavlink_statustext_message_out("Commander: gyro unhealthy", MAV_SEVERITY_CRITICAL); - -// if (previosly_healthy) //only throw emergency if previously healthy -// state_machine_emergency_always_critical(status_pub, current_status); - -// break; - -// case SUBSYSTEM_TYPE_ACC: -// //global_data_send_mavlink_statustext_message_out("Commander: accelerometer unhealthy", MAV_SEVERITY_CRITICAL); - -// if (previosly_healthy) //only throw emergency if previously healthy -// state_machine_emergency_always_critical(status_pub, current_status); - -// break; - -// case SUBSYSTEM_TYPE_MAG: -// //global_data_send_mavlink_statustext_message_out("Commander: magnetometer unhealthy", MAV_SEVERITY_CRITICAL); - -// if (previosly_healthy) //only throw emergency if previously healthy -// state_machine_emergency_always_critical(status_pub, current_status); - -// break; - -// case SUBSYSTEM_TYPE_GPS: -// // //TODO: remove this block -// // break; -// // /////////////////// -// //global_data_send_mavlink_statustext_message_out("Commander: GPS unhealthy", MAV_SEVERITY_CRITICAL); - -// // printf("previosly_healthy = %u\n", previosly_healthy); -// if (previosly_healthy) //only throw emergency if previously healthy -// state_machine_emergency(status_pub, current_status); - -// break; - -// default: -// break; -// } - -// } - ///* END SUBSYSTEM/EMERGENCY FUNCTIONS*/ // @@ -985,199 +723,3 @@ void state_machine_publish(int status_pub, struct vehicle_status_s *current_stat // // return ret; //} - -#if 0 - -void update_state_machine_got_position_fix(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd) -{ - /* Depending on the current state switch state */ - if (current_status->state_machine == SYSTEM_STATE_PREFLIGHT) { - do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_STANDBY); - } -} - -void update_state_machine_no_position_fix(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd) -{ - /* Depending on the current state switch state */ - if (current_status->state_machine == SYSTEM_STATE_STANDBY || current_status->state_machine == SYSTEM_STATE_GROUND_READY || current_status->state_machine == SYSTEM_STATE_AUTO) { - state_machine_emergency(status_pub, current_status, mavlink_fd); - } -} - -void update_state_machine_arm(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd) -{ - if (current_status->state_machine == SYSTEM_STATE_STANDBY) { - printf("[cmd] arming\n"); - do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_GROUND_READY); - } -} - -void update_state_machine_disarm(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd) -{ - if (current_status->state_machine == SYSTEM_STATE_GROUND_READY || current_status->state_machine == SYSTEM_STATE_MANUAL || current_status->state_machine == SYSTEM_STATE_PREFLIGHT) { - printf("[cmd] going standby\n"); - do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_STANDBY); - - } else if (current_status->state_machine == SYSTEM_STATE_STABILIZED || current_status->state_machine == SYSTEM_STATE_AUTO) { - printf("[cmd] MISSION ABORT!\n"); - do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_STANDBY); - } -} - -void update_state_machine_mode_manual(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd) -{ - int old_mode = current_status->flight_mode; - current_status->flight_mode = VEHICLE_FLIGHT_MODE_MANUAL; - - current_status->flag_control_manual_enabled = true; - - /* set behaviour based on airframe */ - if ((current_status->system_type == VEHICLE_TYPE_QUADROTOR) || - (current_status->system_type == VEHICLE_TYPE_HEXAROTOR) || - (current_status->system_type == VEHICLE_TYPE_OCTOROTOR)) { - - /* assuming a rotary wing, set to SAS */ - current_status->manual_control_mode = VEHICLE_MANUAL_CONTROL_MODE_SAS; - current_status->flag_control_attitude_enabled = true; - current_status->flag_control_rates_enabled = true; - - } else { - - /* assuming a fixed wing, set to direct pass-through */ - current_status->manual_control_mode = VEHICLE_MANUAL_CONTROL_MODE_DIRECT; - current_status->flag_control_attitude_enabled = false; - current_status->flag_control_rates_enabled = false; - } - - if (old_mode != current_status->flight_mode) state_machine_publish(status_pub, current_status, mavlink_fd); - - if (current_status->state_machine == SYSTEM_STATE_GROUND_READY || current_status->state_machine == SYSTEM_STATE_STABILIZED || current_status->state_machine == SYSTEM_STATE_AUTO) { - printf("[cmd] manual mode\n"); - do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_MANUAL); - } -} - -void update_state_machine_mode_stabilized(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd) -{ - if (current_status->state_machine == SYSTEM_STATE_GROUND_READY || current_status->state_machine == SYSTEM_STATE_STABILIZED || current_status->state_machine == SYSTEM_STATE_MANUAL || current_status->state_machine == SYSTEM_STATE_AUTO) { - int old_mode = current_status->flight_mode; - int old_manual_control_mode = current_status->manual_control_mode; - current_status->flight_mode = VEHICLE_FLIGHT_MODE_MANUAL; - current_status->manual_control_mode = VEHICLE_MANUAL_CONTROL_MODE_SAS; - current_status->flag_control_attitude_enabled = true; - current_status->flag_control_rates_enabled = true; - current_status->flag_control_manual_enabled = true; - - if (old_mode != current_status->flight_mode || - old_manual_control_mode != current_status->manual_control_mode) { - printf("[cmd] att stabilized mode\n"); - do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_MANUAL); - state_machine_publish(status_pub, current_status, mavlink_fd); - } - - } -} - -void update_state_machine_mode_guided(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd) -{ - if (!current_status->flag_vector_flight_mode_ok) { - mavlink_log_critical(mavlink_fd, "NO POS LOCK, REJ. GUIDED MODE"); - tune_error(); - return; - } - - if (current_status->state_machine == SYSTEM_STATE_GROUND_READY || current_status->state_machine == SYSTEM_STATE_MANUAL || current_status->state_machine == SYSTEM_STATE_AUTO) { - printf("[cmd] position guided mode\n"); - int old_mode = current_status->flight_mode; - current_status->flight_mode = VEHICLE_FLIGHT_MODE_STAB; - current_status->flag_control_manual_enabled = false; - current_status->flag_control_attitude_enabled = true; - current_status->flag_control_rates_enabled = true; - do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_STABILIZED); - - if (old_mode != current_status->flight_mode) state_machine_publish(status_pub, current_status, mavlink_fd); - - } -} - -void update_state_machine_mode_auto(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd) -{ - if (!current_status->flag_vector_flight_mode_ok) { - mavlink_log_critical(mavlink_fd, "NO POS LOCK, REJ. AUTO MODE"); - return; - } - - if (current_status->state_machine == SYSTEM_STATE_GROUND_READY || current_status->state_machine == SYSTEM_STATE_MANUAL || current_status->state_machine == SYSTEM_STATE_STABILIZED) { - printf("[cmd] auto mode\n"); - int old_mode = current_status->flight_mode; - current_status->flight_mode = VEHICLE_FLIGHT_MODE_AUTO; - current_status->flag_control_manual_enabled = false; - current_status->flag_control_attitude_enabled = true; - current_status->flag_control_rates_enabled = true; - do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_AUTO); - - if (old_mode != current_status->flight_mode) state_machine_publish(status_pub, current_status, mavlink_fd); - } -} - - - - -uint8_t update_state_machine_custom_mode_request(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, uint8_t custom_mode) //TODO: add more checks to avoid state switching in critical situations -{ - commander_state_machine_t current_system_state = current_status->state_machine; - - uint8_t ret = 1; - - switch (custom_mode) { - case SYSTEM_STATE_GROUND_READY: - break; - - case SYSTEM_STATE_STANDBY: - break; - - case SYSTEM_STATE_REBOOT: - printf("try to reboot\n"); - - if (current_system_state == SYSTEM_STATE_STANDBY - || current_system_state == SYSTEM_STATE_PREFLIGHT - || current_status->flag_hil_enabled) { - printf("system will reboot\n"); - mavlink_log_critical(mavlink_fd, "Rebooting.."); - usleep(200000); - do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_REBOOT); - ret = 0; - } - - break; - - case SYSTEM_STATE_AUTO: - printf("try to switch to auto/takeoff\n"); - - if (current_system_state == SYSTEM_STATE_GROUND_READY || current_system_state == SYSTEM_STATE_MANUAL) { - do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_AUTO); - printf("state: auto\n"); - ret = 0; - } - - break; - - case SYSTEM_STATE_MANUAL: - printf("try to switch to manual\n"); - - if (current_system_state == SYSTEM_STATE_GROUND_READY || current_system_state == SYSTEM_STATE_AUTO) { - do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_MANUAL); - printf("state: manual\n"); - ret = 0; - } - - break; - - default: - break; - } - - return ret; -} - -#endif From 0fe5aeb02c6ab0ac9c50f54d028cd5dde908e051 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Mon, 11 Mar 2013 10:39:57 -0700 Subject: [PATCH 016/109] Checkpoint: More state machine fixes, commited to wrong branch and now copied over --- apps/ardrone_interface/ardrone_interface.c | 4 +- apps/commander/commander.c | 32 +++++++++++---- apps/commander/state_machine_helper.c | 34 ++++++++++----- apps/mavlink/mavlink.c | 10 +++-- apps/mavlink_onboard/mavlink.c | 6 +++ apps/mavlink_onboard/mavlink_receiver.c | 2 +- .../multirotor_att_control_main.c | 41 ++++++++++--------- 7 files changed, 86 insertions(+), 43 deletions(-) diff --git a/apps/ardrone_interface/ardrone_interface.c b/apps/ardrone_interface/ardrone_interface.c index aeaf830def..264041e65a 100644 --- a/apps/ardrone_interface/ardrone_interface.c +++ b/apps/ardrone_interface/ardrone_interface.c @@ -331,7 +331,9 @@ int ardrone_interface_thread_main(int argc, char *argv[]) /* for now only spin if armed and immediately shut down * if in failsafe */ - if (armed.armed && !armed.lockdown) { + //XXX fix this + //if (armed.armed && !armed.lockdown) { + if (state.flag_fmu_armed) { ardrone_mixing_and_output(ardrone_write, &actuator_controls); } else { diff --git a/apps/commander/commander.c b/apps/commander/commander.c index 2fdcf4ce3f..a3e8fb7450 100644 --- a/apps/commander/commander.c +++ b/apps/commander/commander.c @@ -60,13 +60,9 @@ #include #include #include -#include -#include -#include -#include "state_machine_helper.h" -#include "systemlib/systemlib.h" #include #include + #include #include #include @@ -80,11 +76,19 @@ #include #include #include -#include +#include +#include +#include + +#include #include #include #include +#include + +#include "state_machine_helper.h" + /* XXX MOVE CALIBRATION TO SENSORS APP THREAD */ #include @@ -101,7 +105,7 @@ PARAM_DEFINE_FLOAT(TRIM_ROLL, 0.0f); PARAM_DEFINE_FLOAT(TRIM_PITCH, 0.0f); PARAM_DEFINE_FLOAT(TRIM_YAW, 0.0f); -#include + extern struct system_load_s system_load; /* Decouple update interval and hysteris counters, all depends on intervals */ @@ -120,6 +124,8 @@ extern struct system_load_s system_load; #define GPS_QUALITY_GOOD_HYSTERIS_TIME_MS 5000 #define GPS_QUALITY_GOOD_COUNTER_LIMIT (GPS_QUALITY_GOOD_HYSTERIS_TIME_MS*COMMANDER_MONITORING_LOOPSPERMSEC) +#define LOCAL_POSITION_TIMEOUT 1000000 /**< consider the local position estimate invalid after 1s */ + /* File descriptors */ static int leds; static int buzzer; @@ -1324,9 +1330,12 @@ int commander_thread_main(int argc, char *argv[]) /* make sure we are in preflight state */ memset(¤t_status, 0, sizeof(current_status)); + + current_status.navigation_state = NAVIGATION_STATE_INIT; current_status.arming_state = ARMING_STATE_INIT; current_status.hil_state = HIL_STATE_OFF; + current_status.flag_hil_enabled = false; current_status.flag_fmu_armed = false; current_status.flag_io_armed = false; // XXX read this from somewhere @@ -1542,6 +1551,13 @@ int commander_thread_main(int argc, char *argv[]) last_local_position_time = local_position.timestamp; } + /* set the condition to valid if there has recently been a local position estimate */ + if (hrt_absolute_time() - last_local_position_time < LOCAL_POSITION_TIMEOUT) { + current_status.condition_local_position_valid = true; + } else { + current_status.condition_local_position_valid = false; + } + /* update battery status */ orb_check(battery_sub, &new_data); if (new_data) { @@ -1565,7 +1581,7 @@ int commander_thread_main(int argc, char *argv[]) // current_status.state_machine == SYSTEM_STATE_AUTO || // current_status.state_machine == SYSTEM_STATE_MANUAL)) { // /* armed */ -// led_toggle(LED_BLUE); + led_toggle(LED_BLUE); } else if (counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0) { /* not armed */ diff --git a/apps/commander/state_machine_helper.c b/apps/commander/state_machine_helper.c index 742b7fe072..79394e2bae 100644 --- a/apps/commander/state_machine_helper.c +++ b/apps/commander/state_machine_helper.c @@ -40,19 +40,20 @@ #include #include +#include #include #include #include #include #include +#include #include #include #include #include "state_machine_helper.h" - int arming_state_transition(int status_pub, struct vehicle_status_s *current_state, arming_state_t new_arming_state, const int mavlink_fd) { int ret = ERROR; @@ -68,7 +69,7 @@ int arming_state_transition(int status_pub, struct vehicle_status_s *current_sta /* allow going back from INIT for calibration */ if (current_state->arming_state == ARMING_STATE_STANDBY) { ret = OK; - current_state->flag_system_armed = false; + current_state->flag_fmu_armed = false; } break; case ARMING_STATE_STANDBY: @@ -80,7 +81,7 @@ int arming_state_transition(int status_pub, struct vehicle_status_s *current_sta /* sensors need to be initialized for STANDBY state */ if (current_state->condition_system_sensors_initialized) { ret = OK; - current_state->flag_system_armed = false; + current_state->flag_fmu_armed = false; } else { mavlink_log_critical(mavlink_fd, "Rej. STANDBY state, sensors not initialized"); } @@ -94,7 +95,7 @@ int arming_state_transition(int status_pub, struct vehicle_status_s *current_sta /* XXX conditions for arming? */ ret = OK; - current_state->flag_system_armed = true; + current_state->flag_fmu_armed = true; } break; case ARMING_STATE_ARMED_ERROR: @@ -104,7 +105,7 @@ int arming_state_transition(int status_pub, struct vehicle_status_s *current_sta /* XXX conditions for an error state? */ ret = OK; - current_state->flag_system_armed = true; + current_state->flag_fmu_armed = true; } break; case ARMING_STATE_STANDBY_ERROR: @@ -113,7 +114,7 @@ int arming_state_transition(int status_pub, struct vehicle_status_s *current_sta || current_state->arming_state == ARMING_STATE_INIT || current_state->arming_state == ARMING_STATE_ARMED_ERROR) { ret = OK; - current_state->flag_system_armed = false; + current_state->flag_fmu_armed = false; } break; default: @@ -157,6 +158,7 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current current_state->flag_control_attitude_enabled = false; current_state->flag_control_velocity_enabled = false; current_state->flag_control_position_enabled = false; + current_state->flag_control_manual_enabled = false; } break; @@ -177,6 +179,7 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current current_state->flag_control_attitude_enabled = true; current_state->flag_control_velocity_enabled = false; current_state->flag_control_position_enabled = false; + current_state->flag_control_manual_enabled = true; } } break; @@ -192,6 +195,7 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current current_state->flag_control_attitude_enabled = true; current_state->flag_control_velocity_enabled = false; current_state->flag_control_position_enabled = false; + current_state->flag_control_manual_enabled = true; } break; @@ -215,6 +219,7 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current current_state->flag_control_attitude_enabled = true; current_state->flag_control_velocity_enabled = true; current_state->flag_control_position_enabled = false; + current_state->flag_control_manual_enabled = false; } } break; @@ -243,6 +248,7 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current current_state->flag_control_attitude_enabled = true; current_state->flag_control_velocity_enabled = true; current_state->flag_control_position_enabled = false; + current_state->flag_control_manual_enabled = false; } } break; @@ -270,6 +276,7 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current current_state->flag_control_attitude_enabled = true; current_state->flag_control_velocity_enabled = true; current_state->flag_control_position_enabled = false; + current_state->flag_control_manual_enabled = false; } } break; @@ -295,6 +302,7 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current current_state->flag_control_attitude_enabled = true; current_state->flag_control_velocity_enabled = true; current_state->flag_control_position_enabled = true; + current_state->flag_control_manual_enabled = false; } } break; @@ -316,6 +324,7 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current current_state->flag_control_attitude_enabled = true; current_state->flag_control_velocity_enabled = true; current_state->flag_control_position_enabled = true; + current_state->flag_control_manual_enabled = false; } } break; @@ -330,6 +339,7 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current current_state->flag_control_attitude_enabled = true; current_state->flag_control_velocity_enabled = true; current_state->flag_control_position_enabled = true; + current_state->flag_control_manual_enabled = false; } break; @@ -353,6 +363,7 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current current_state->flag_control_attitude_enabled = true; current_state->flag_control_velocity_enabled = true; current_state->flag_control_position_enabled = true; + current_state->flag_control_manual_enabled = false; } } break; @@ -375,6 +386,7 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current current_state->flag_control_attitude_enabled = true; current_state->flag_control_velocity_enabled = true; current_state->flag_control_position_enabled = true; + current_state->flag_control_manual_enabled = false; } } break; @@ -399,6 +411,7 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current current_state->flag_control_attitude_enabled = true; current_state->flag_control_velocity_enabled = true; current_state->flag_control_position_enabled = true; + current_state->flag_control_manual_enabled = false; } } break; @@ -420,6 +433,7 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current current_state->flag_control_attitude_enabled = true; current_state->flag_control_velocity_enabled = true; current_state->flag_control_position_enabled = true; + current_state->flag_control_manual_enabled = false; } } break; @@ -530,7 +544,7 @@ void state_machine_publish(int status_pub, struct vehicle_status_s *current_stat //void publish_armed_status(const struct vehicle_status_s *current_status) //{ // struct actuator_armed_s armed; -// armed.armed = current_status->flag_system_armed; +// armed.armed = current_status->flag_fmu_armed; // /* lock down actuators if required, only in HIL */ // armed.lockdown = (current_status->flag_hil_enabled) ? true : false; // orb_advert_t armed_pub = orb_advertise(ORB_ID(actuator_armed), &armed); @@ -669,7 +683,7 @@ void state_machine_publish(int status_pub, struct vehicle_status_s *current_stat //// printf("[cmd] Enabling HIL, locking down all actuators for safety.\n\t(Arming the system will not activate them while in HIL mode)\n"); //// //// } else if (current_status->state_machine != SYSTEM_STATE_STANDBY && -//// current_status->flag_system_armed) { +//// current_status->flag_fmu_armed) { //// //// mavlink_log_critical(mavlink_fd, "REJECTING HIL, disarm first!") //// @@ -694,7 +708,7 @@ void state_machine_publish(int status_pub, struct vehicle_status_s *current_stat // } // // /* vehicle is disarmed, mode requests arming */ -// if (!(current_status->flag_system_armed) && (mode & VEHICLE_MODE_FLAG_SAFETY_ARMED)) { +// if (!(current_status->flag_fmu_armed) && (mode & VEHICLE_MODE_FLAG_SAFETY_ARMED)) { // /* only arm in standby state */ // // XXX REMOVE // if (current_status->state_machine == SYSTEM_STATE_STANDBY || current_status->state_machine == SYSTEM_STATE_PREFLIGHT) { @@ -705,7 +719,7 @@ void state_machine_publish(int status_pub, struct vehicle_status_s *current_stat // } // // /* vehicle is armed, mode requests disarming */ -// if (current_status->flag_system_armed && !(mode & VEHICLE_MODE_FLAG_SAFETY_ARMED)) { +// if (current_status->flag_fmu_armed && !(mode & VEHICLE_MODE_FLAG_SAFETY_ARMED)) { // /* only disarm in ground ready */ // if (current_status->state_machine == SYSTEM_STATE_GROUND_READY) { // do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_STANDBY); diff --git a/apps/mavlink/mavlink.c b/apps/mavlink/mavlink.c index 3423287283..c443d5a4a3 100644 --- a/apps/mavlink/mavlink.c +++ b/apps/mavlink/mavlink.c @@ -230,10 +230,12 @@ get_mavlink_mode_and_state(uint8_t *mavlink_state, uint8_t *mavlink_mode) *mavlink_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED; } -// if (v_status.system_state == NAVIGATION_STATE_SEATBELT) { -// -// *mavlink_mode |= MAV_MODE_FLAG_DECODE_POSITION_GUIDED; -// } + if (v_status.navigation_state == NAVIGATION_STATE_SEATBELT + || v_status.navigation_state == NAVIGATION_STATE_SEATBELT_DESCENT + || v_status.navigation_state == NAVIGATION_STATE_SEATBELT_STANDBY) { + + *mavlink_mode |= MAV_MODE_FLAG_GUIDED_ENABLED; + } // // if (v_status.system_state == NAVIGATION_STATE_SEATBELT) { // diff --git a/apps/mavlink_onboard/mavlink.c b/apps/mavlink_onboard/mavlink.c index fbfce7dc95..057a573b13 100644 --- a/apps/mavlink_onboard/mavlink.c +++ b/apps/mavlink_onboard/mavlink.c @@ -296,6 +296,12 @@ get_mavlink_mode_and_state(const struct vehicle_status_s *v_status, const struct *mavlink_mode &= ~MAV_MODE_FLAG_SAFETY_ARMED; } + if (v_status->flag_control_velocity_enabled) { + *mavlink_mode |= MAV_MODE_FLAG_GUIDED_ENABLED; + } else { + *mavlink_mode &= ~MAV_MODE_FLAG_GUIDED_ENABLED; + } + // switch (v_status->state_machine) { // case SYSTEM_STATE_PREFLIGHT: // if (v_status->flag_preflight_gyro_calibration || diff --git a/apps/mavlink_onboard/mavlink_receiver.c b/apps/mavlink_onboard/mavlink_receiver.c index 0acbea6757..2d406fb9f5 100644 --- a/apps/mavlink_onboard/mavlink_receiver.c +++ b/apps/mavlink_onboard/mavlink_receiver.c @@ -328,4 +328,4 @@ receive_start(int uart) pthread_t thread; pthread_create(&thread, &receiveloop_attr, receive_thread, &uart); return thread; -} \ No newline at end of file +} diff --git a/apps/multirotor_att_control/multirotor_att_control_main.c b/apps/multirotor_att_control/multirotor_att_control_main.c index 79ca9fe2d2..3329c5c48e 100644 --- a/apps/multirotor_att_control/multirotor_att_control_main.c +++ b/apps/multirotor_att_control/multirotor_att_control_main.c @@ -150,7 +150,9 @@ mc_thread_main(int argc, char *argv[]) /* store last control mode to detect mode switches */ bool flag_control_manual_enabled = false; bool flag_control_attitude_enabled = false; - bool flag_system_armed = false; + bool flag_fmu_armed = false; + bool flag_control_position_enabled = false; + bool flag_control_velocity_enabled = false; /* store if yaw position or yaw speed has been changed */ bool control_yaw_position = true; @@ -162,7 +164,6 @@ mc_thread_main(int argc, char *argv[]) param_t failsafe_throttle_handle = param_find("MC_RCLOSS_THR"); float failsafe_throttle = 0.0f; - while (!thread_should_exit) { /* wait for a sensor update, check for exit condition every 500 ms */ @@ -247,7 +248,7 @@ mc_thread_main(int argc, char *argv[]) /* initialize to current yaw if switching to manual or att control */ if (state.flag_control_attitude_enabled != flag_control_attitude_enabled || state.flag_control_manual_enabled != flag_control_manual_enabled || - state.flag_fmu_armed != flag_system_armed) { + state.flag_fmu_armed != flag_fmu_armed) { att_sp.yaw_body = att.yaw; } @@ -313,20 +314,20 @@ mc_thread_main(int argc, char *argv[]) // * settings as well. // */ // -// /* only move setpoint if manual input is != 0 */ -// if ((manual.yaw < -0.01f || 0.01f < manual.yaw) && manual.throttle > 0.3f) { -// rates_sp.yaw = manual.yaw; -// control_yaw_position = false; -// first_time_after_yaw_speed_control = true; -// -// } else { -// if (first_time_after_yaw_speed_control) { -// att_sp.yaw_body = att.yaw; -// first_time_after_yaw_speed_control = false; -// } -// -// control_yaw_position = true; -// } + /* only move setpoint if manual input is != 0 */ + if ((manual.yaw < -0.01f || 0.01f < manual.yaw) && manual.throttle > 0.3f) { + rates_sp.yaw = manual.yaw; + control_yaw_position = false; + first_time_after_yaw_speed_control = true; + + } else { + if (first_time_after_yaw_speed_control) { + att_sp.yaw_body = att.yaw; + first_time_after_yaw_speed_control = false; + } + + control_yaw_position = true; + } // } // } @@ -337,6 +338,7 @@ mc_thread_main(int argc, char *argv[]) /* STEP 2: publish the controller output */ orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp); + if (motor_test_mode) { printf("testmode"); att_sp.roll_body = 0.0f; @@ -395,8 +397,9 @@ mc_thread_main(int argc, char *argv[]) /* update state */ flag_control_attitude_enabled = state.flag_control_attitude_enabled; flag_control_manual_enabled = state.flag_control_manual_enabled; - flag_system_armed = state.flag_fmu_armed; - // XXX add some logic to this + flag_control_position_enabled = state.flag_control_position_enabled; + flag_control_velocity_enabled = state.flag_control_velocity_enabled; + flag_fmu_armed = state.flag_fmu_armed; perf_end(mc_loop_perf); } /* end of poll call for attitude updates */ From 8e837dd1641941d670a30a6f4706a663efa03ca1 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Mon, 11 Mar 2013 11:09:49 -0700 Subject: [PATCH 017/109] Updated the state machine documentation --- Documentation/flight_mode_state_machine.odg | Bin 23463 -> 22619 bytes Documentation/flight_mode_state_machine.pdf | Bin 23031 -> 110086 bytes 2 files changed, 0 insertions(+), 0 deletions(-) diff --git a/Documentation/flight_mode_state_machine.odg b/Documentation/flight_mode_state_machine.odg index cbdad91c9c0e539eb5b02b700e4b9d8eab5efd50..2d119bd8c68ce1753bbf4777cdc5d263220d83ad 100644 GIT binary patch delta 21163 zcmZ^JQ*fpM*W?@9HYc`iO>8@vOl<3kGqG(<%!zGJFtKgh*!uqcF1EII&rP2?w|%O+ zx~fZCz}D)(kd);hATdE8SP%#;!6pGo5#m2!NB|f(WU*lSZPki2)(&&!(`bjt`lOeX zt5GwiJYu7*DRkixtdy94`8UJi=@ikTqip+32_E@xzwRuL?Z*(%H?9`*h+FkzEy$M* zQ2A~8gP+|OyeOZg;fV5@WFix3A-k?OMU>LXmCR5PMU%HT^`kGiucJ#xT()J>nnzHn zSOR>!vIYKjvsy>q;5$6AE86C(grqeU@WaQ5KklQ?U??aeRYt#|z+>+s4&V_GZd%hB zr=@xiRL3E%=kR`;3S+%{&kFhm!X?KsFx3IyOgC9!BVA+nf5KcGRk!7Ndd>ros4OhI?O0#+ zQ;jCx-)LX>N53U~mL#r07oC#4)3Owx=sPJbm3U$L4CI-X*ZoyKwCMpbt$8l}s{pMI zjSL1*$L4WfHlH^6e8F2>a6IBZbxsehcwar#=f?!8WMv|?y;=F)&HnB?#9CeXB3$O6 zOBV{Ae-`w?I|p5Ie@Ph5UBJ?1z#)pV91QGPC+F~AFc3%-5(N6+q{G6({)coi(El-r zAP~sqhpTInA2=+qp}XR|%Y*5cS1Y4($D*bNDK1l@@Xa-2NTeZT$ifGsndNjeQ7_SK zbSci3RJ@_sD!8;gEF9JRB)hVbexnE>E-#I#+bcAhLN-@*Memy%M1-WBM0RPi)$xf^ z`Sj2h@riFNgLYl{K}Y;Aj&QM2rgtf1>drd<_vBkVTIF5G{rhmgPe^DoAgv0 z&Dmzh9_{Zdsc?uzj3~ruT3Mln&(4cUgOKS1__q?qQ}5YByw54E0MaM@^t!9QswGW7 z+m2WJH;a0}U5qk~&VJ8?0D5R7#%0mLiZY*IcyzLaxABJd8!bXDEcDgCBtEXoH&*dM zaTrTP@L=!q3-S8feQ^niiu6?0M4!Ke;2)Tt2vb8Ut{Q%$hS5S|&Dw;))0Izo#7tqe z;`jPT?3&ty|rnF=15^Z~hRH>t7ixZ1$YZ3u9I;4p^LZKykDi7RlJW-u4jow0-v zK<`&?5~h~p+}$~SW5|@b*)D?t9k&Ww(urx_5?DR!K0b0UUEej5xYnndNiW`Lbr^L@v_HG*mh2z_>!%7=N|gm)xb5+&ag*FKa551!WbOBJcP1954{>`yIbev)uIX ze5th5v)T0kkG8kgR3S>2dFHd8HiJamt&GYXJpEgJ0FiDaW91J5%FLi;iN7i2}Q9zO{=GKOb^f}iEDy^>l^7L z$H!xE_G`1eRbvdrZGBz;1_SJ2*?g0LK?mNPA+r>*FtGAR?TmJl2llEByu{mzNVdeK z`kxj<6xE>aUz1iN`ynpdW@J_{yREsShnHq~vb|EyX@j{*7;!DgAy``pQLQ6p7;J@3 zkTOfusn3tDMaXg79C+c8nm~8LWh*)`naUlM$BbDSXoDbxnUnG~xnlU_YU-z9}p#~AYRBZ%n1?ltCe7>T-g z&SFZ$p0Q-!dMM5C&_Fv56Z~S}_n(4glu|FWg9L-FA9ylk2fx?$?hUvqU71YrM0T#C z!ms376qUc|lP+e)p5L6h$w|%22`bvrTPoxFN#VyiX#(iZ^{nj0vJoKp zbreEYM-HzYkkWMIEM*Y#S0h1m9F|?wc9m#cU7vVH{``~IxcB4qvHoJWR!v;z@_sr{ z1;zuZ+UoSTHFN2!yIp!5aDL_0H8whIfh;VKR4YjIG~i{aTVC*h*<_~a=2CSZZsArQ z=h|dORhV)ZP2cQ?}6jI)hLRa3hXe#!K2x)1@bo}|FMdy>(7xBC=fcxn&SWRiX;aZ5+HG} zHF2{mFG$6P-+f6G`$nm8`fmu)8IsZ8=j`Q-7@o(;5Z$5E*;rXRxr$F-<{i17yu85- zQb{t}fI`_S>*h>B)^G73NxB62{i|@{kNX#=3||KBwIT0^0i=oBtYZDiC6B0ua1#5^ zhZaCFDl53w+G0LD=;(NO2VUy+FJ@o7n}VkyYYgqSG`fV>1spW578@>1-|RVk z7+S8}?>x>*ott`2bA(@WI<(v@I^O=A`?V3A8+FkOU%eVen~gO!Jbhf9L%(Tl*;(ZM z=Mq@ow-?-Z|I5?uBQCZ(N>iTwIhY<^2Ja$n_kA*4b<> zs?Y3*JvXC$yf+y($~s=`+8Ayu*7Q>A*FKgjxCy zMxK$s(DlM${Z*GE0~xU3+u(Wn!Cv|A^Wp5F-AlbQ-+N_a?^YnKWucz?Rqg0nDQl=^ z-E-h5XW*#eptIoll~sZRcf{UALf2m+)xnUQa*)dztf#s?t8cC2SnKN{Ef2^MPL69B z*d99RJ27qAnAC(Bw0K(TNp-$5bH*-^JY-HgKh zS&ljV?szvHrSZZ)4SZ4a&|TF%@H6{=@vOa1U0iV+<+ydn4*Kg?Wj1&TPI%gXWmiX# zz!99>z&_^h=Suh+&;IqGx~eKb>+W$;lXtw_V>t}tyJeTXOh1H zc$n*SSVma?`Lo>or;XC?!T20+&s)m7^qw;H6xkp`HX}2|;F9Ow6xkqs8`p4wRkrvq z9Naf%Y`88vR`W`RS?%qvbB-7Y1N`P0lFc6&mX&^IiCHn@TIfi*g zeS4ddLj=!t+A8{XJNu@&)GEaHPP}gjwD(K^*@D$AUoX>22XrrP&8wQ>lSPD5B_!h- zZ90EN5dmG>7W2V5Q|@rMSDrP73d`$Hs(c9BKco>YE=hg4Pg-l~i zEVjd=^M^ZSfs}Mq-xgY$L%Y7M4P(&E-cR>E6zx=!i67Oroq!?Jgn_EtuGjNt)@j|T zENkC&c-Gew@TDX!d?hD9*KWgN*pS^7o-36%lk%_`ci$7_e{+^L@^p5P3!2t*ZK%2w z`S75y`LJm55+biq|LXhq`z#H}6@~E!BWhM^;Ptig>e)>8A=f7f9W=%?#p{uU9|NOR z*Z&qeZ;ZK{(J2dm(nmvClPF}t^QvE-%>-Q(->J%L2U9%bHf|MNf+RuyU zP;|2GHuCnck#?9r_4awAG!-7UvX#PJJv(`m&dFcG{7_543JSiP~ zoO?w;#}KdpoBb}uYvC(uUhk4jIh{^hXe4_^kAf2`I1GhBaSwMjk!Fi?eb(WmxvL#p zIu=i*y@d7Q6zM3MH^8K}Qp*EHY9&k+ZyQ9C)ra;am(jW>L+*2}&2y~47{-*V;a7Fu z>A-E9eBNTzf_)Tr{sO=geIVIQVn~8H8Tt^>Zg=n*tpsv4?tl8uw7!IAa%|vXlpDbD zH1R;KrJG`!A-%Ei9!af5{UE0bj7>ifjV*BsmlNOjB!2CrUs#TNPm#w>nQE-x+B-cB zOzzqXM-vZGr<0UivGlBF870sbb>cDUU-<`}J!P?JY`{5uoC2+1+{XZcrs1y{h^4K3}u7bNZsd z89bleublfs51iXPt%>W0{J+|JM*bOj%70iOj=&D&q|#91DQfTy8|Gu;xU88rEh|*H zLsgZ)R+dlbR0CznO*`6?lG9s2M(ahT^O|=P-BHH_)u1)`xHa0p1Hp%Pue2Ezub+X$*ieImHV2Z3DN~fAaFpM!b0!kj$@YbuzN`uaGyS%7ZO~>5d$V zUeQRNVUw^VBI6K7%{;k)_=>USR!ANozWMSO^1hHL|YUTJR!uvLW8Lmg|(w4|>@+ zJ^?PW-%IGf(_Unm-IhB=E)KYbKM$sR!ZbGJx?S6HzgQc|iPXc&V^0h)-H2)L8QO&9 z?|_f3^slmP=9v|=JF<(bH+Cnx&|E9vUTQIwG@4qGZS%WL6h?<(>kiIqf<@)giu0f5 zZiThnU%YaylZH%(B68%hR(pgiX%WRMhfbcnExE<(QN`<7v_}$gDl2^~lAO@4PpQMO z6zMVLtb7&2kepI3x=|+SM@ZHYDlhtYVZeO@VRYW0<;<*(NGNOOx1cy_9mvQ7#y9Do z$)gN=jC3A!QIg5ylE21T4H8)6{=(;?8G?mcpk}`CVL_{l#;OlPvx~Z@;U4shnXz%} zEV}EV`6S6qWiFojV*ENF(_JGboQ&4H6M_F5I!PoKm0b!?Q$DjFA{8(TrY${cWf z4cQqxpT|RY-9-LMI@Gli~~p{YfZDDuMAizh;Y*2vv)-fKKuJ0nwHEbz(l39xMEe#wTV}1 z3*Z-tRpP3U7w+JC4i?j7lhmS)lb8R9oH>ILQ z#4$2T;K?izNYa0vGh_c=3S*_@x5_txbyr%-t~#(AyO^JehOqD~WalYS$_f-I>8+Au z&9*iEScsHF%|~gnW+!jqLS33|rOX5*JT&cMVtufhKCFl|b9JIuXP0t!&~}hOMBY= z5JhPf8>4*7Pm^G(#d2KE@qr|jQFa9it{9S{0)2NyS#ssD>HE?F5 zvSb&3nfxuL-|J*d@%beDb-M-q_|n&6KhdYQSX#CnaLP8#{yQ>t$F6K=l2`kTW9YlP zE;TRjmnljQj_eMvz_;|(l5uKw=+MB$h(CX<9Sfq26N|Z)m#8OCF8oLXzgI0rn5+t(2 zjGajIY{?fxi4&r~$o85+Ow7N+VB`_QP}K@}sv2lp#7Z*{%Hvmyn@3w{e}>9v$3UN|Mf9H={@P`9bPW>yKiJ>hj_T9J0 zz=Q5seyG7f`4FL%J45=%4KU8B*Oc-!j5S9^F z8No~XpjM1jhDdq$qp^QA%wg?P3rZ2PE0s_DQaWHunmGPeGN?i~Nv7Qe>>x0+r;$Y3 zi#zxTjPL@j%|nLnTefCQ3D9R_8>>{9xENA4Eyceh^$7IVG4b8@wAW+TB?b0E3(2|n zIuANYb%_+Dg$qoxbsX0y0)q#HQ*bXe1;<>1qljpnXD_oz46=S0QB(bkcZ&BUOOoMq>d^&35k@JNOcmMq@BcZKJ86 z6X?4Aa!4u%$Vh1ii=WL3etRbB`lA;rp{r?pAzKP63IS@&kTd%G)(H295tE`jbMnR% z#YTFrbFA!Ci)k6Q8ln@;y)Z=C4?jO!!*HDBN7A>nZi?mJ6H{iwu|Ska>8#^HqQMVu zHCTYDd%_*$EA%z#P!vc;#PP}}Upg%rHgUCD_G1ypLO>p^?@GRlUzMkKxQMqMj5mMv zdwU>!i{gGnD`g#-C~6YKqYQExor0EGE)xb%&Y$srZVK3o{1U%CFgFm4+`yd}`0H3u z>RSfP5r2i2Pvtlo<^CF&U4czr*Ha|$UDpM2pU1p$$V_(Ez?3>@J?>z*T~3Z(Lt~Ji z#bBPV&Xk_WUrr4Bzu-TYLdv<%ei|nr{avE6w6-CfH=%Gw@QyX- zdL;jHjf2YsLcf4^=54oOrL1=)kM7q%VezaB;KJxkiSXWKj%gjr7A3EZdgseO_W}?J zd#-Zn`6_rio7}r|4%@EbZZW{AdxZKDQu=i2(Ej*PdYXBDD;;8_zXhJdFPV2Pm;*}{ zKGFORTci*+{OT=@qJ79dOY<(hX8m$(jau}CA6n&`m|gf8N=ljMFAyp}2z~Uef_hM4 zH~e(aUIVFX|0IkEc#-k%=0vi6h624C9uvBS>UO9FYf%1adxW0-1hX99zBsi5rd+hencqe&&HLsQ2Z3*SlZ2LjxRZCDXVQ7YQQYrlmZFsUwim-jm zaD%g^x{rm{x{4(_iEI)H2Du}=WRP)W;QXiqDzNnK_yebu16EtHpSzgWlK}HkI~?t^ zu6ArcT>YG<7`4un#UZD*y%-`f#8Ov+v@-Zayu1d3aj|ip`X3EElr~=V6_qbT#Gbjz z%p^xSttA~o<6xZ{>vdz>PzLXVS=EPM)(&n`_53;))PfRrx}J$5-!bCW1}88E`fj!T z3Xi@oC}x?<^J*KP$i%#O>H=6Aa=IAz#cXEi#{{DHRS_ECZU`&RIvTrE6Ml`7{2d0@ zmVz`Y@=#T6i77n@R{%!ztdqbH4a=}g z*G@(%5mp0=`AfZ={mWn@RzwNAyNYIH=8F08jt^+H!%3_V3+UC`U)b|469*#n0&;A6_SFHx?)xS_cRWz}2`*{JFfq{7zqX82hX*Z8ely zamgYj74waFx((h;>;a(D3mTDU85;SU;^f-+(gLMdxz^!MlerAdhw$@*Q^uc)M@2o5 z@>pW^VzWTsT8|v+@Lndza!55}LIBPzUlb>}h7~RAErm38HO8p8poLkC-q~XxZFsVX zE0(cv641iR6Owz++V(I;}~R|uoED?GN`Fjii-Sbwh4S6``13AUu|Za6zBY6O*S)l zSRp=LlHKw21m`?lx4Wtje-RxAL{g4o-#OaoU9~U2$i8QPEfK^(mw^J(_bLW91#aYc z#uU=4vb?<`(-q*3cn627QBEX~vm)rvNTbzZRdS6i@=-x*(XDb;s%(6V2=!=yF4?cS z_5lO@5;u}{IEg4pDam~}HPWUVX6$(Koq>hI3YKL~D%mLc6$%W+Q^ny+%Ez|!ZdP54 zZg*TU@Vtq`Rn}%1SEd9HPOg4iyZkdGEapt_ei#`+D_X!%+%{ZD)Y<>x!J1TwgHhy= zd9^T;WT6aE$6tWV*$|~g#Vs2pKbAdn#EUn5%DHPfB`ad1Tfv>;z$5z*>2A(z!0(zz z&_5|{yt(}Gk{`iay{L1QKXoivZuwh`3Qm{hP9>&Yh++bk0P+_0h(2s7%foW_^vhAG z)6ldSJQTpk#*Uu3sIKr#p!qvP1&;d#mXwcRAe53NFn4{ERwYUH=SscY-;5J4fz;e1 zaWU*)KPV97r;~=UBP!jQR5i-ftqauXhqw;H#?-NgInnJUY0$qBQc`MT6p{H&7}e@t zzNT8$@PSjj%Nr+HT2MlTNU#p6f5wv+DhHG};#&fI#6ha|ZiOL-;q}of|4i73aEOT1 zk=R|UOqYHh>j$6D{0)7Cufc$u6{COZQ&+Zb{V>7=H0Grfb#BrP91uCZPU^hQC|M}F=8RJ9JL9>&)iL|fs{YoZ%%yc)V z*#ie=A0kc!9;*G41ud77Iy5r)`fjpl>`T zfc#6i@Tn~@y}B%f8M@ZYMKO2NHOgn{oog>w$Cltn1k1fB=9qxpw#K;P7sQ1MNhK*L z5)=8CW2s;hCRUA*gbk}xWi+T*2*-&(^>`0x+vDZ_RX2HEA76wPgXzSEKv4mGBpMn} zBozRXcL>{^gQU<6PqT#N$eV<7HetS>P?`$4QfJ&IwsQ1kN33FRTGee?426Vg^{ zs_v|seQllR!U~-~mO&-8GC?+A-{%fc49oF*WXBy=Vzl&RfT}SiS_P zW}eWfV~>}7$M6Ii!s+utVFiFUbgHTMzQh#qxl*_;rZxFUF^wyL@YLRa<$2gxZGuP6{sueR~3z ztk;8PWA?GZU665a1O)1P7 z>&@E!BIdw1w2V#}o2z1*Z0A}&VYdEkIE(;$lxXWxVw}TEG(c8yZrV zebvKq;dfj{66J9anbYRr!lAM?jYYqW#`^qOP$)mfGA?(ojCxP*F5B|lX;{fDp5 zUo!cl?2ppj=MV_=jzM*lLiakLo(Zd)O9xG(l*7VXtH0QlVKJa2>WrNs2$P_eonc3A zDkIr#Iv^;bcnIQsX@LSpX?4H(;D9?vdeTX-IB{19CjP}J&GzYp{=IVo`=U`=Q`mh>#b*DUYaZ&9r-!0p zOifKh5IsFC`9#Qw7bD_R*zsK>C_i-MWVoBI2n5sCgf>b68d#$Mg-meWyH%k{!u#OkS4mg%O zeq+wHq=m9>hVW$|Of(^X&@nz^?BY6g%l+oLk%3y1n4SI!axa;~L_1dzR|xMSJ)fsk zdt}1x%sN@?I3rlfuTDn)Px|TL?tJy>p=@f{!^N|`SYPKiJc!1p$gEI4kP{n)C<|;$ zXx*PTi(+Uao;B12@}BmGY!5Jw%-`Ic&+)Ib6@Xxv;hoaaPQ$9Q1(H&`E5ne(!mcz`i zuaY_*I5}c*r)HHYd6OSwW!&f$<`u5kW-n9qrDO1v4IllOh$u2CZhUbK$0 znk%0nBAJ4QLHct@)6oj%GqjsE!Q0qV!HJ5mDj5xM29mBbgDyS4DPDQHIcJo%Sz)vv zHqIN}Hlsw>tlTu`r4@7YxhkYemn(LyHqMG8*PSfW_uAM`tX%rFWy`jWWWliMdQQfA z2vms-jbl=KPMYCAJcK;+#BULIQrRo=lXhB!AF=D};Qsi&^QP?mq)x`pZ6nEa1m?=nSuYH(Mtc zeew|mD}33Hr(4?Eph@Qx<2osEXtD;+hs5$#c6%SUOP^NYWaT*G*?RC38E!+d9?DsgmJ~6DKOWTsmDD+Oh=@mP-yJoylHV+_v!$<%} zKW?7MDQ<%o)yL*IHFme3S0+%d)MM1WEL0P<*f=|ZR z1qR`GcS6bcG=0{U7r0$r#3xQToCfO%(NMlC$L5bB-g2;sGslAVB++AM1ZA1vwnqk0 z_H-ZO&UL`9-pLk9WieTJ|G~-+BU1y`SiaMhoU z#R?QbVT^llINB$B%|g8vGd9E3y;E{_RibVzQa00WnuIFr09sDQ(48l;=6V)^%77{5 z6GS7T%`~SF-ZbkADXYd1afzu8QRP{%5W9rIR{XsMHg00_dl|ig>7n-1Bp92?S?VHds1K{!R4u`Z^svgBnb#jX5ktry zH~UFhqy+bw8w-Nu*Mp^`o{J~knbn_&G1Mw<-;&?yY>YJywOj>b z|Die-a3hR(8>Q|n<1lM#cD7-4C8!Nk(2VLH_wSEko@$)`<)I8=f^J z!xY}{lcIK{Ijj{)XPQD$CFMA8y=uzzab^yoO}E-Yb<|vKVMlVx?uDAbRA1&1e&enF zL4-t^i`BEHV`vO{ifo6Jax1oYF${|A8}@i7$=uq{uKkc_I<*CJs2}HTk~a166(oa) z$Ulk2SMKIbo`c6~HTqq2iuL>d8q?!KgF0Ye&wm|HQpU7-!uJW?Z8WoSM3GA3Ltz}@! z=a_J<`neChc6gl7Jr^S9QW%i)AUN>k4UuboWbP-v^oY4wk)?r=OmAp6lIiU+zcOD> zdUg=(-;hjCLBvwa12-4Xe}}Qo#$j34^EIJsG;GQ)o^YdBJIxb7Q$L*21)sLUoJja3 z#xc*3HSb3AqjTHg%siEQ2e`Xh-|lYhmF;5%E;usJB~Fb7C%C8mM;UoqCSP8xn&v}P%o(%^?Zl!Mx&&3D)2RWP<1!73x8)vj}g;ZV&N*j*-ys!l~| zbUX(K)wMtV(qyqQMX6)8TRWo3dxr=ilzg@(665s&Meqp(;f?Wc7YM(IhGFKsLRN{* ziOi65Mku(K%Il&0_0K(sWa9GsehP|QxBZqzj89rFkdU|eNU46%_gUcKTxSRsp@2~A zT)3IPG2ZQtWmFJC{*8@nvq*}3@O!vdPo;^co`9Ei`z0c0OT+i>uVa|!eE)4fwPXX< z3j`MgAi?+-72z|!DpQT6VNY=L8{_0z$!VL$BcJAvU9A-}feNZhM6IJS(eb;m`A5k# z)78Uy9J)6&sS7au0JKtF&J+^^Bjm!f9`d4QtJT9fMLXe3gNQmS`4ZxKE6KT~{aMe= z@gejSFFj(vn(c)0+qO6@mD!XCuzfsM8H|;nWJ;#l=%a zwmj}9NcCAv1mf7J3Ogwn7}WiUzrL+T+OhtOpt^FUmF3Ir#Hbm5z`;+8Fr#B2lmG^S z3lH0v*K{fOZP7p;kh$36}nLBy6O+DY@N)hI|?nOw>ZC;{@r#6B}ttH9CTmh{#nYmIqE zi(upX&Z(U?+!%l5D`KZZsJq{&Z5=59%LDpv&UBXFK7n62hQsP)^5ban9lhtWw%4$* z-mwJo2Su{g#l=)a8XBfr!GmKrszWxCO^i(oQ@WVg~6Q;>4Q4{_*u$e zQ1jdy$X}BovCU|&b-a#aOr4e8m!kRDZ|zm~o|)pPtCA)sGIhK`$i7YGz%p(Xs6Bk{fIZ*_11X78aYwpD@ojxO1B zdjE5Dr+^SUXtGj;n384!cVo9Ohf(QIYugxPPWa&@Ec_!ock3@cictaM8W?Xc$6y4Q zH^!8Sj1eTJx{MGnk~fj3BX>jr5KwbXi&|EK_7pQe!-CyxK$Vob`E7Nscl zZeL>G3c@D4tet;H-*O(29i;_V-XmXh1NWUcI2~zsD@5;r>#e6o$7p-4_w&=$05uD< zMjQK>Xp4hR7VO$0fo8nSkiOe7Qx(wqMz!29`_L^MrxA!@2{|10gj(HsRzYBo3N|38 zf1>XvR*aOY@Gg9*zMDD}XySW8NIJ3YdR3tluP(mR4Q*6l7)Uvxj5qbh?;GAJd zm{}avH?vDX4cb_(i-810nU4~y zC$OrsSIF8Eq|knlE}0SQAa!>ibNnpG1gy+~4EKqcm*J`B2g`mN&9NsZ4*do)Ux~OpM;}?TSv-fFrPfvh%Mdf) zG0AYfW7N{@H{IQVzG)eRp+Gv8Dg_zWptp$RyVi?1S?m9B2XAv%lm^2y=_?+9pIre;W$MnT>Fr$b`y2$3x#X;R&-IS4;#brsVTt!80%A$7`6uh=+e2^anvF z8yLN&v<>J}3lHGwTVjAlzjrZ{$EO`PBwEClV8)Zubs0GeAfzH3w`=A+G5aHoi#VYD z1qgM){4S5>4+TWBo+x^8bY=pOucQN*Pw9i8!HnA}o|z7G7c+|v;79wUt3k)y!^tj< zt`0!(+`G8^6{KS(@T*%f;Dih{-0Xjzu1FT-JP^!WYZxiu)#z)tvG?X7O@gA~uzdP( zF#kCu=iF1|pUjZ9w`8=Tt)W$$T}Ux~-g0)+3@N1s6F^W4|+ z#|O}T8}+sNyg_wcP6{FdNk_-V3>WU%$5Gdv^$s>TIxK~jM~r%8K0?(Rf)jX!HiVKK zcRe0&=KQF%rcfeQRFsfS)wK8B(a824$qgG6MnG(J?-s1<;7yTV?{R6cIEw|S#Xr5fm}u9oZj&)f{!*yiWe-F7AF+S;0HnVnDPb1Ev{-+iwp zvl?JPCWL~oyvqwSb@R4sM_`)*d^xyr)PhVqA_^;e`GWPl%n!rEMePnPb_LO#l{|0M?!R^FsLD;SG3oe&2pp@?W6B4!GoZI{x|L6^gaM zX~V_o``p)2d3FlZb50E4dtY2oJVQ~A-n)`x%Vi7lYa1DDZV**eGXiwXPaf_2KBXLj2NRm5X+w_h&2*cuGg*S_nSXU3x=T!{tSh@FY1_%C1M*}y<^614a zP~yF<20=485m?UxjyM*v+Thm*uO43;!Z}}KJ;F;fuxh(%NdV7V(gHz`T9--Gddq&U z2|9J*-1g0@MfLpw2#8bqooLM8(`wr)%!39ufMjwx8nk_~@L8y40|uA)@A=Q4b>nMV+*t|o8@sjCJbsVbMvkQ; z7aYvzfDR?ascfMQn9r(8`Y;%fU9Ul4@9s=&&<(sKh!{o41z2ag7PdU^AXZ2|^cE$T zhxvLpd@AQRnK|_k#1HS-1y9Y<1_$ktT)({Xe+4!7Y0FIFuxr{652k${qCWe286iCD z*Sk}w<6jHs!>}L+5|Bb6N~(@gu1k&3f3@Dig}uo7b7ja^tq{um9Smg9s#z;;3VxT_-NPJJez zM@XXW>zd)~b4@1S{7s+%9#9Sb$?*~gZq2Nm>>@!N!9}^I>!LoY_}*aOBCUO#%vRVP z0IB22=Ksit`^xQT-#XE?O-dXm_fE|RxT$**o{(V=79FW5|PkUUO-u+V=6CH#ae{E22J+@&ij*FgUiO@l_7v#wWGbhLW{6C?dfCWSl<0 z37N=2F!&rYddzklit<-%J6r!-Q_Tul+f_*uM(>Tk=oTuhhN~uX-@WA6RoOIh!$N_P zNDwFwTfLK@%@LP%!mXB@jMx80)0GPkw6v6v1DUe~O5|TgmkzWii!U>}8Su;eGvHg7 zpbG|Xv8a=v-t7wLB=h!Ar%)_PI(oaw&5jic@xLpI*u++e_Bj`k z;1#O9m|FzKn=AN6hls^$`W`I7PS7%!m|7mN3nE1h^f!{eu+}7*{3Ie%Wnl!-H@8|O zG}T}Xuphnnmh!Ba+xLCAdP2eiJ<)b!Q08YxnHPRmsXhbx@22T-(U zE>;J4Lq%+YngxeC#SlCAjUJ?wh*x$}F8|(xIK1}~2)SJO&vn?AB@J9JFP1Foc*-Hi zQh0ncdd8+hq5nF1icK5!rVL;b(O`x;NF0XY6SuB5co=JQU_7a7T@9U88q7u+E~2wV zVZzu-uC_QX+^4jx(6$vtMhkjyK>vfPuaegUtwTQAGZ0F$jESU-;~)R_K~4#k|6t(U z5JZ3KbFl$ylVV^L#8Y=77Flt0$H01FgFVO;2cyS(iDs>!Mo4+iXT6A&S|pbX$4 z9g2yckETEo*n+^owrdR}Co7kAED&72iO&V|twN7HfEWT!J_v&x!5j&ZYCvqrbd+S+ zL+j7a@_ut!CnuOvqNZy95qUBaEW~b)x40q-6xmdbz-dP2FHRyg-g%Z{7|Nq6CiS2PDp$bzicDb&m(5O z($Yc}TQdQ!m(?|CrWO*lXY3}JQj^h<4(^Wpgn#w_^m@L4Ndn-jw&&}*#{_k>Jc1RM z{tu)(74|MI$GLq;pGGkNutZ31o&&q&R1U^%a7ChTM+%M<`ea1Eqr{35Lv0o`-7%%Eo*KU ztD|wi`-R6=7xLW5$X<1I*4k>&iHZVmIxaZ~*OXPzwuna(na}Sv%7HM1jN}_t^~WLt z4Q#RV@^|0y@%M`hKQl8Y%oYy9TzKfbuz~vBUBWuRW+?I-O>Q|0y)l_!1uYDQK3MVm zkXEvssIs<+z9}8zOS(NeJUH!7Mq0HkuOr^?>#B8Nd)c4cFn1k;H}ZPATnHJ3na+;e zbNP+mBgEQ@M;Ik(69YwsUHYPet* zJmzG;i)^t_ttn+M_74`$US^Pti8G3EJ(;3i{{~uIh@Q5@DoOG0$(V(WnQejINt^-(T1Bm=2^N8Oi@-3Q$;zbZy z&V#r-QEXz}<#XqtP*B=fR{jUX!imQ^#ZP7D@&cIF^h=QHm2E-J#n#0R=2%AkKSNe-2L8=0+Esu>XlF z54LBl4mx?hi|VD;?P_~MIXc8~H`I204PDx5zrA~c=y=SsD<{Kkw{K3Wj|Oc=&H-&l zZzq-912Jn$Ou}EY#0`@isGo(=Xm#|UwiLNpEr&1ytV-(%t(pR>0@~{xRGnq40L|I1 zEn*zdi6atfh!xm%lkLHF8YEC`tP5&4G1*}(+DU_yFg5_4E;$7A^gRq+@1RmuA&89L z*R-#r+Xp4-=ZQliz=FshZ6^fhEND70di&VGQ3t|%2vxOJL4xn z0Y1Y20|R{_2myPRtsqZ1>t*r+l3_%*^EiwKMmcJPgUA?^0vXRKlWRPQVGDUHs>RqK zwOm*pPQX^0L=(gbng1>uz_ZbQK`2y*p^V%SGpkcm=C#&+@&uc#SPWZ*ATqbc0`8tI zAXmh;D*#D6tP4oec638};##0-;{@any?rxs+Qur7y-FjTR+lWl_aD1ykLqm%JrmrV zX54}7Hse4>=AsY{T$cC#kk5tPlLBS z?|8Yw#og#=mY7+CtD_HaMFfx%e?llZ*fJJUfhh@|JSf#NL2c zdR?z^?kT<7@pPqvn_UYcOCvo;59yYv>OBhVU~w^52jPtf^9Akm?y!MkfbntX;J#90 z8H|X*7CSxv%k{3=SwKH;+CX_5;!=p0*X7lf_9l047<&0Fr5v(G+zpS{n$AMQSD?e$9{yEtju)bsh6 zIkx35MCQ{Gt`z#{*%Jm@Ib|=r9#5J-)$RXR++LsJpZ{IkqW&sw-E&y;a%gr0rju?0 zG~?=ikiM`#hzuFSJqL@`9BT)6El^!Xk;yes6ptCyqjP8iw&=M@*=&~V*2}tI6HJkZ z5M87gE3lm=%U`1h#KXoC@+BVx1axlfxCGuWZDy@ubpNF1wv{Y+p4pSwA6ZVoFnjsBvkJ1 zGk-xYfL4EHFoy4a=oJ)}y)+@L7@QPd6GGnRIIJzp?6+1jna$3chP$x=3RR!!?QKkh z3~jsbA8ufO2HZ>|$4~5#^f9zRHA#H>V#L$`x)ftKM$7l{L{vmEkNWKMOP2GWR-UfL z@k1QRxC1-25ji_kIeI6ai60G4ntPHL2YWvjaZt}XSydCkWNqeU03TF}sdiSASHJ9z zfj_%A)jqBu)e6JB%`MJgY-)3e+Md*&s zq}^rq$Lo}AgMgte)S4SA#BvLT;I=`!{%)xskx(C?W1t` zX^}zr*3q+dOUpt@y8kp zad7q6OnCx9r9pa#S~Ky_4myZD<%_$$bYPhmH@eR>A6!89l!kW^klmobftFw%KwXWX zvG2}k$Uut>!%h&y>v2mk2OvCBKu6-Df=faICqJB6VPyr+XfKF49T0m&dWx4BR2Yd6 z(mwFmg{++tA;azCaoht#dg@L1rMMc8Wt_LYRty&OpjJEWE>GE0*}(BvpY}&VyUUkTBhXfv1Ud|F|A$0 zFTV|-ASpH!huc9ptTbb609`$aS2C>6Y+4N1V=Si9j{x4SEHQx+G<5G4fMqPJ{MiFP zQ5KoCb~z?E+}F(AzSbfwE#s<-ii&RV+FDx7GA)1C*8%gKVy>JXK_W z`Lbfmpt7b$R8&+{XvJ({kcvTqx9UBr&uX3`HW+YvcDAtN>+I}oYMMIqz3A=Ro*(DM zO@1Y`gs0pd40RSTPY~dzg%P(d91i#KLHV#z=!m_Re2#0i_X>i81Cj6|a;vPYY>iQ5 z54o&>pxMWp1m}0CMoHvGI*Pg$;-@-FgIzm2 zuiEUROvG+R`$JK2vj-lvzy|bu`G&KcYQliIsy+{I?2-EZPP3(~YUCSEz3RoM^e{CW zTy&BkjeSrWBUz+b)7cJG^ZHQ|ZJ6EoE1x+N>a=hn_KDNU)9>Fc*K-dR1~O)_V%MP- zNb)wO<GflEPVh_W5P5at1=L3bR!%y(sdR^*?~D%aP7q`%Z}j!WN)1h*4RCEL2C_VO{g@m3(VQrVkV zlfUKeQ9j+xh+{qU?)a2tLp(#L59K(yCrn#2lc78Llvje=K5<^l+-I}k8`IGt=h-XD z*yK)@*S6$HDiPGvkxEjb**(Z?*-q1~6XmT0&d@4?85RyWBjdHES?N0l*IT#7+~;nk z>2-qT=I7w_%E=;7o3@cRvbID{1NC;u(H@fdeh3lkY~!KyhpDaN&ra@>6hCnlo@X7r z7kHr>*94RFeaGR;bc8?hVB<~RDVqa_0<7MDF3m8b&&P5Cv+4^hQQ?(Y@~Bv zdM)OyWO7|0GrRf^c7Re7nOo9E9B?lw#RNiW||_sAyF-xd3_C! z+c)zr(sRX79J-6dlM`nw)EzP>q_-|~JPe#g+n}XqNDJT0oQn}H_i8$1|6XbHH`$CF zjYUv7C8r@;F3d*%x9#+13KbHpx)xFn_K)b;zdkUf^+$cGFQG*Y?uh= zNB#HNoZr}rto!=-7ITG!PS%}^*c=6}nP9EbJH1 z@RE!w&@O_5@1%b>tLEZ1O>_Wh##@toXcEUY4!WpvMi8YDEuRTawmyg17BZsEKX~}_ znP`3^E$|>>bSB_(6kd8+CcH6~m#42g9+n}CdK7j2RVJS7wfJVMjh8KRE903~tD{+2 zZrxdQpw+F6;--zNpN%3C)2jxvlyG)=zVd#B5IUSWaUp1YZe%2Xlp^9`+o->gQ$g*u za(Rn!ug0Y4cU}7wQph(gz2_#^X7gb6q)vD3DJz&jr#VS)h-#E5l-EJ`7s0ee40!D2 zC<4J#UAnY($yIvs=ysN&2wyhEojg+LGi4&~@9g0}vbn^Tbwd<@9BVPkNa`|pyv7~^ z4_Y|p&_-o+&rP-x>p<<3-X3aP?+2^#CNAbVBnE$)py~!tceoJVe{oL+sfZOAzj>k; z?-Gx-G`&1OmyP-0NRD;UI#gA)gK+~;YJ%g5nZYCUCgcmt{Qa{V0w4$LIS(85lCLaduXLxQ(y1^jKw{Z*S#1Jux?mc$Y&adtFEbQS#wPOT%eV^2r$fQ%-G zD*QMC`>^SK(W#MgMeJ#}cG;NLpegarD+4aYswEZT!dpb+LAH1NtpKk}=`C?I~?eX9YCdqxIPEniU@Ay4?{ZlT$D11QxA!Hy^Fm z+u-l4CP+K4fz_v+6Q3)VAC%^KDzs|PgL{rM0DX?Op4=@DVUb|g+gnYNIb#~NCEmR7 z%@Zc{fJ8K3)-T_CiYY>p9iizhhGO4Cz4}Wh^4?3Y3g&d!h9Wt4=XN;yIEL>!4CV*A zYc?6*O z5yF~WgO7jWpBF(;MuY*??Djq}pAgmd1xT_(@3O^dtW)WUKPsVQp`AJhrj!WB(5FjXieHffA4Y>hHwrp+xP;nn)Lgs2ws79s#}xLJ z3mh#~h-`nqWy!=4PUWZH_H?1IFBzUN%XQ|X8ttKJ6GY303eBw>&SC0uiI{a3*e_U~ zJ5jGxB(M_T=@JHgOKiBt5uzeYI(`tS7ouXJCU8@eDl5BRc}PeTK;WmMEd*L(9%Bjk z)*pP^IzZ6B(9!yX?tDBoX@wa=O{&l!EX&y9y&Xdw7^YDof2BCyV>4m+O81;9&WRT7 zUYEqg*^R>Qgy=3JH{=yYP%o-2BVF5sWVIAMy!Hb|DqXM$Y5#+Ha#d;n zJS(a!A2wR%mcsi2yWMpW+%`$kQV%I}B&8pcSzPFx)09$D+{7o|NYNp!?eu=)UAf2) zy4=B5N%(3(Dh|z}#2gIml`Xy}$B(C}m862X^iY=OrX{Isdr&`j)6jmRdm4-rEuUd< zWBoAhaY1~|n2Y3dh9#9jj#m17+j$%ixBmk6D?azndb{~^)-hroIb+Uu!BYNf*wysX z=;zr_`u4`BFQ!>C6C9{6ldUgJV+=a(ynVE{8Ow2MF$0zQ5$%i}V*2zcmKlgFokz{Os+J47)QSlM_16h|2%rY-v~TMH+xS3 zUso3$bsQiKfaYIZ-hb~AA^ndV008hN!tfh0vR!TepLP)dfZ;DAshb!(qwBx2i!%ZP z4U{qN>KvFMBa&aLuSQ_t3uO$}m>ScnP73-R`A-kZuf<+v4BVLeUt6FGhFOD!_bd$J2mrv-%g^Qh|Mh|~=B8j^y)MSz6h=U< Hf93aI+BrUS delta 22088 zcmaHyMNpm%u%$oTgG+FC2=2k%-2=hhU0>YYo#5^s+?^0U+}$C#%dNlWE@m^+Rb8j| zr|a~lpS603vNi}5WjQElED#7D1Y$_gNJLSD`cDi90EPH$7ObvEEmDuVpSHA!IQ6Qi z8X9&jsadk!5;VL|pO+XTsxW4Le zh`{{z=!04p#N%|Q?Dy{o69{Uq=N-*Xf9wc&D>+QREc~2LDqn;bGhK5LW2Ca)b}mTZ z%z`d>2KJW`p37i+tBK%4z#LR+X|#R;<&1K8g-9uX+3oS%aFt_3H=XK!lFFj#*Kh9B zlnS`pS10OHg!RQmegRD0qfgWFMM`l+N{ z;8JAly8X3-&Rv4ICa6=SYAZ)K1KTS8QZTO2gjLY$P3b5a<)HKNn+c8X*YD1l6Z&x$ za7UB&Z<+)QmRK_jn^o=81SK^PGlHOH^($kXtJTlKXEm0zXpNo1#F3b+jaosl*Ui=ZkxVWu)@N z7Gjw%JH2(L+Y^BbgkRnGzyb;4X*~`eEvr9jI^OPZ8j7^glE@vrTJ|bEpauF8ZmcGK zh&MEuyOb_Sn4Q=PAUMR3li=GcrIlrs5~aL()29#i($e8K#mRvRTW5?w+sa>&tXKDq zhll6pvaN((CMmtmU74y*;K!re=j*q9yy5vePJ9$0zu$TIND25+hGFm+m0Wgj=6_qJbEG*c$0YM}9kbq`rzXEZ~=<+L%GokjPm zmG6z*t){3jng4(vDpfD$aURX9>nQ1!J{K*Y)f=UL9c+g;TA}n2F-k-=VX=x#N|;=R zbW^=v+378pubhVE9I$8bq!cmT_hKy(_hJ*NOSSW7WwCMq?y=amUq-q7o$f`KF7by4 zex(l1ht7L?GQjwZ(#?83E= zq)VOc*=~uom_KK{uKH!9RD0e4BP3}m+`g2DMqPmXQ6t=AwO{Cv%9@Qe;@Icg$-87yO>M@TrbY54T`K*1~i z(gL85#0C(Z7LDZC{y`~1KM?N+t&q3q8KRcQ?Mx!dw=`KVXy*Q4#>KDUDptbbd$ilb zL^vwb(uHrhTJ_(_k50>Fgd(EAey)=Sk8Z;ZJW5<|g?snK%_00eH-;jqk4E;8U?lmM zhXK305n~eX5Oi;m8_}kAfE+5wmv&?hFCWeUj{#9mBb#PQy;^Q)v6SF8 zVF5T3?4Kn2m%2*4no?ac3kXo6!xt*^?6T7UzcVO3OmAlvjG(x4PjTLV~1cw_?JUSm~((0K; zS{q3o{*9@&d@d?3=9_@fy~}SBp^p?x2!Fr;{mD6XIG!r*LfegsB7*r*9G3$8jdP1Mcu9PwnpaFtF(V;_PJv&21xBLA-<&8_TKb-@VbRKZ)JIDFr^8sL zo#z{JkOw7|Z^D%@RW0I0p8UTlt%z^P>&+Kj2({8zySti=;|*;PI$7dyLfq43Snog@ z{bZMF!ggF3S#igt48VTCa*SI05=S=}!RBaFhVDk*d-Ae*HOrT?JV%a@9}qIOG7zZw zC~Qi;o%yesP}2*1la#`ziV-WaUKk;L7~9u~$}(#O&`S#tP^;|Ix0mMK>51ZLteJlU zqe&Zy+_}nFZu*_(EYn19%=E2h;{h`azZ*r;@HpjE%yUk$Ee|h5l;^sg1l?7h}tLb|Oz0kzbCd*sy(u)5k zF6Al!^5lOXPuvkz`sb)lX1=4z)s+Fo{hjI;pP%fZkBYKe~Zy`_xdc8qXX)ftw)5GkVa`> zrHR8gg`j-WAZK7_xSbSwzdJC>^viN+jlMq(C*Y#9Z+y0H?a`|id~>`7>U!T@>>d1H zuD9N;{D9t!>8*_#3nkRs1?K!3E&eC&;3};(kAq`WZl&yn&U@F+Qnz(;uD>{GUG#_}4a;%#J4C$9EPwpVNOVMPKVXU)=dUIy}&XYi-$;=64#z96^@v z96X-Y;sPtox38SPKCXCx4JLe#YqJjR_v^vw(T&UNjf;z;KlwEI!=~NOj*~eqZT`(Y zJ34-wdtmdEKfkj#X3|~W8ZVf#>(|~pP;WPy&)#=Bc9?4Nh&TUm=N4pMmP$3fv}Zc( zU_EuvW^4%3c7=^Cm7(UAoPookr<&ry3KqP#G-@j79vc!8>!C}4pu7hO=BjQoEfbF$G^PptpyU|p>jNm&FBB#7&8*dNB9P1olAdcBMW z`6AV^zO?&jmuld}T`IlTcXUSQA+}N2i{u8fjSDdy=6_y>RV!Wuo!|$ZXuMa*y~*t8 z&U!;I*k9-9<_44#@36942oedGJJ(t-8vr}+Wv<{c`xNBnuI_ntqZ(zKRm zm45o|=t(&6JZ<+fhnig^CuCb1PlDU@z7`Kg$&&n#Ffa)DbGPB@;?eZEewy@#&XVQe z9{tP}j1AXg-}GUU2Nb7R8sWnls#{ zW5i|ASYitj&MVn3@S9gVkvM}^z?v*}$2o1A;!-}=4*p6X_*KU2T z`L@0SUCVy@LKkQgM+!F`?9YSKzzV21ea6vxH;kL}hL5DPxV8K7)9+EK*gMnektL-G zN$$h-^_tq3VbsIF{rPCv`h)+Q|0j(nXYXLqR>%80*!7m*qj$^KHuoN>JMrOJU6^|Y@A{ZiY=3>}j&XHRb{r z@lQwnv)%k#O>*Qnn1?RNKS{~I=8f@(J8$NB<4*+1_g4=OE*=T-cDUeW_CmH?zdi^1 zfLy2{q+#K(f}$CSU8*9;t#vMbxCpH~8>QM*2ZKKXgIm4`EO1cchH+_@!aq*|?RR$I z%wvEA@d09U=)=dtWgS{z&nG@@Os==BYb5?J_}>oQ%;VM#Nm=~a1@LjZ=bQS@_rq>7 z{l129W=d#E>vq~0R`%f5xRc64fZ3fr-XM)`av#jzumghM68ISP1MOT_Qb2iPeY z4K6C)E~HyfzC>&JwEOeVDF+W0K3WVNWi#WhO+=A;zD?N=V5Z$F-I|%%b^*G~N1fG# zLytSR4io7I%5ex`LhEn6pRa)RTM#OO^4-$5|FrW9v81(+X!@oP_b~wTx(h7Tq5fN% z{0>smLvE_R?8~^f(0rJ-zjcMKRR8SXn|qM^(-&wMF!3^H&xJH+%XV71DM?ZZby|9bdf};O{ zN2(fttSzPdwBy+)Omii@_ITe{rxzZf57K-^gYbXSG1uQ-6A$hfa8&w*&dPpt#Oo-) zRY3+lEed8#QQLpwv{PJOAsz;&-WVb*?%BCT-!4d1^L9X`TfMVv*#SK-w+)`Vy|p{+ zQPpESV2S&--&LrSWnLfc2G#C*cc9!6u(nb3`^f^5o}qTP3K4$?Km!ee%DD1MLi%)) z#CxgftcwRtzdOILq-!2%AF7}%^zQy$l6guvnp3B9nK;F}>= znlaW{zp;0E9Gu>@2LOD()V!bm89jq$&;C>eD6ulNcv^+8fihaK3YX=St910j{E6LzFrL=ODk^@)>tZ)~LKX z_1{x(0cG01Z3%RA0bg%N-o$}t=MK+mhXykL&)=VMMZJ#MFTetH9C{#sjfx^-4!vDW zw`c>d+?sLAa%`bHOkrt6dBvo}blQk6Pu%ite=f&g)w)unGADI*)Emp?;q$df4{7Kxu@>&%3%HXwl_?Ag$mh& z@@vR}Mb%7V2GFKPN@pF0CSE6$!$2OF4>}=UHfh5u(i*51uxZFTS`#|V_u~R!g?<)d zZKpOMq{C$=amIcL(=0`HI$*2k>)_^BK(WpIsw_4+zJ}r$Zy`!)K2U~vSq6E@EOzUh zMt~%I6ouR2Mq3u6w};vmV*{rYuy5dtWHh-q#^Ge|1$a@vKCrjsLt-CGrr5EPB{5OY zB+`L|gwN&)$8HY;Xb$@bONG5^NbzPMpti5P>R~2z$jrPbGZGps25<;dl9dMo@pBk! z%gGT6+>=Gojh3v8v}AsnAzS8xvPc5VVYG-jg@2FbqQiS6A~8=wQ5UjZ96^u3QvS6c z=%8fj1R`hoLRRIQ|3aU=q}xIylm)~xlH-jXrmeo?JB56*h_S1f?h0KwSF{dA0F#|-QQod^I5#j zFznIsyI-YJdaNUthhhw*VhnKbyE%T|A~&UPy#Y_@LZA5DUf-T)54<0ww!&QJ@k+VO z9aY#oi&+}?KUt<90k!L*XWPL3ICW&J&u*dQSjWX=E5^oBHybRR~&&+5H#(;L`7mVo`!&_mbsSt)P>FA&%#02$Y=K&FD zt5D)HRVZg3v7e8MXo)t=)T*2*QW#bqd8g9D4Yr_WrdTA$D+@ds2sA2q&&v#E)b>bn z6cfha24-cUH3ah3JSFbV9nc(}Y|VK) zI6DX+0`C!+2sUzrf3##1!K!pp+as3k#B*UNFv$xdgR^Q@V_fqiD{?>WO!L;W>Orr-DfVg5-fTjSsNi4M(b71Rc}unIjwrX{dak3BM67uc zNN8&`l9Pb70e$${g~GASNkX+NC1`V3Q%~IYziRY^3O%OZO%2&r+WgvbYA+@CE)jDO z7RS2I$?oSA5O=XUiG6ng;Q8W6WxZBp{n(%M%t#C^%|T>;TA$I5BoD{qR;9OFB`{fh z+p;+%_e*-tI;Jb8*uiv}s6MIFHpR!I(C5vTcieM-nf+0BS#^A8(D$s#t_)f;g1EMV zIPRnE8ZZ{k%4sgv{=0?LQ*aHILEn#{D!U0^HWTHqmbr8^lTggfKrFS4b+E~gxf}x_ z*g{4boKnS~jN+kRlR5Mjn(atRyN5h$zn-yR;$J$E6eT6e+|9Cr3eN^Z4C8{CF>_GI zS<8^xl%9(8B8GnboJqy8mcH!y3JSJJ=4CNte;zob72P|LN$K5 z{MHl-dDX!I-!t5-Spf3gxos_ajbb^Zpb<-Qr5}SAR4`6euj_lZ!~d8P@JyS)qkblF z49>hgS;^$$v&gYgejffH&4c;V(ci#~UY_?B{(idXnaOn|4piS|P-5Yr0NAtXe)Epq zhK#rX!A8VC8`NreyO4MN?&U|7H%S~C%{~ggrG6fpe7Gt70T;1ke$_)62NOq* z`(Gy6@s4#Fs)`~Y16W~``6+)8G*m=K+G{)iw7i1dJ?XCI*uI_~`NGSdD)HmE(YgSd6so7(juH~5XX;qF+c96C-ie)cDUO%1iG5-p zA3Uf{_7mrd<+Kex1GabkBZwMyoXnzGI48FjP3SA@E|B3RD<pH(pL%-zOnYpN4$K*Xek9|Czpi(f)FYWNZA2J6Ja1)egYT6zUPg`$P-2R|-^pnA=Lg^< zbLZ#$UjeW3zvc>Xzn$kq8G^ZSc^?QV`0OoRX`v4+6#`llG$PqE@HCIouGe`^&P3XH z+XWq=k$&~PIO8aA2T&=Qv3NnT2=F2CyC~c#^r<)%G}EA*DxaZwaku7x91P(5^s%gw zJa4D4e_mNpUy+&{@*{&;*(v{~W3H43r747>asfPN%x_jm-_SD@5dh0wnfxazxe`4A>oSy|n@l8W#RmPfgP+mJU^~TAZl;G6 zEZmbPhZJ`qoI1Mxjv^zS3<@zL`b7RV* zDc=9^=Gy{Pw!jXX9c*6&JirNXw4Y=`EIbsws&0bRvXog_AK)((Mq7gKoRfiJd7kWzHj%T`~ik|E2) zgkGw+%Q<`XPbEvYZP0mTX@(SU5puD3xmmxFIa8Qip%q`Ik_TakhnZFyLr_q?7H8E= zdD2c5OvH^ms=nolEaZgL-MQsH2op>pYFxwasZ14V2{LJ*mOp4J(R$ES0lUTFKeA!- zDM_v7Ub#+ghMvs5Dg{|1ime?K*h&SM>hF|I1oJdo1(IgODJrHikVjzaeKh}RK>pU` z64fmEjpR&{YiHhulvfRBX7!Qvq^LtLM!8>V=9o@e?P=kVosDxVsgcYWflUs*uE)Y6VLt}mgaM?yuXCX$IP-Gh))ts`wJ>)u<08Oy zKinkU~8{S3lFg)M?T`ydtxyV}U!aa5axURCxFek=uB zsO-?DOB_m}K;G}F7P<}kIE`bwgqAErKP{zy-qvFb75x~M zuwTrszPQ)+U$FacpsKzHv^4U|@5GA#& zOTXY3V=a;Jq{Fy1BIHK(Jo${MYDM|5XjqQCf;eII@k&39Y7rN2@G~QW3Zc9eixPxt(TRv$nx%dFhJ9wq`~n)C$W2+)W<3r z{QB9#e8P|{|KDb(0NV^`3AuMdq4dc-k7a)&(N}0J?IKxvANg$aO*yNo{VQ~9-g5XB zs|;v~VAr4=<*(3I)p`RZPSu@Pillmsi1chqc+r-7hK!^V+j|Rz6ZnMq7<0yj>xoD- zZdE_qg8Z1v8~{Wc@})d6>21FbQJ;RCOlHc7+L7RPZ2~-RiNWa7yenc=CHycgl8ad> z*%^l(gL|yVQkbzf9j3}>l~~#QgHZ~KqkCxr&QUMaL!Kh9ob==q#-->DU-qcAH7ssb z|52+=7NK13_0V|s1a7Yidjgw(JxvPnH`Tu$;uQADD1r0j5z7bzjwcP<+oiZIjjX4v z^c6KM6yQf;P=~+s5@u7Y%O&}gIYd4_B`pgdX&qIuYZ4bDiY~un*XPO;plWqr8FCoj zfKXI9DDSH4*8B?}_gH<{o_01AcB3G3Yqd$=0;4%p^Mj3C8VOFhJ%-BL1iItZii&NaF#a%l{Oh2Pv`Q$)TaQ@B zJh48aQ!QM#SxqiLj+W+z21}h1_@zKuFD1*Vd;b$u)FFHja2wMwYtVD9O(@0Vhj&Uf z$q*d9Y7mNIdVxh?hg%?t#!aSfLp9rRk2N>t+la1)0pHr{4Wvfyp2@T~-~>glsu!0YOuGPrC$(0>N}A{?ud4kUd`RvHIz&R^`IPSS&BL z8uSB*BbDHQB8dQK`MQx^H3vx!0|R69SzoTYvCvKyW+7P29<`=`Xqn)9oxF)N8>S&E zuk}BZXGr7T6RkIm@@IRho%-4ptO=yT0C<4#Wl(z^1pD=i`-#P@QjYHIO~Q*A@cdjF z-s1_raBtX%E&d0kNci{La{{~dD;V=B@Z@AlO2AqmlKeVitw@TnrdVlQbRgv)sCS=_!>V?WedWfhocMWj>=S;K5GIp*SZalxsiuX*!V6fp$rp|0(%2Bc#HG53A(S>5S)gY0K)mu52$G*5NA{(8`HthmWs~<6P02O8SqPG1c^LZZ2xHt2 z`myfIjszw~lGnD z(UI-t0K`!dEKV>=JCO!BirE?K(taD=-xOgB{&&i>nIT{=1b#{)+$*CI|w@Olz z5i?Cu{^jo_6LFqU$Kvd4pb}7>KL9V{VKihl%;xMINA9u01u8kIFpSH|DVqO*PE!Uy zSfw>HPKQrbprEKIsAAy=I7hi6YH;it>d7z>q7UGP_;csvm=FBf*~dD%)GKLJ)#K$2 zt>WQ*7cpi)l5>P>&0Y|qu|V4VyKSwS9+Q*v125J3iO(UZWVgW~K3Q#6RsFBnH~sJE z6f{Q*7CO0TLfKeC*5}|Z;4c)!=h(PcNtnM-Ds@m=SrqYvnv~)Uz_8?2XTEZ!mXhPX zJ7ctuWnrEjfu*KmxqKCnknAO@_|Xc_D^X{S7|WQ zgv%i)9y6>qH@<0@GYV5QQXO((Y;+|sUaj~{We<@uHARq7aHe+6E`?X(u0Ll$=4>f3 z-i5=<(0?goZX${purz})GX46@Qzx-)km#3(1LAn%8LSRb^UY!V0fEMiU>aV7tJ%KG z_rz0C;WIQ+fiK;LEK!sghn-JnwO zioDosc|E5YHRk3p+f-UkCi!$G-OKLOd3&XF7CyO;G>Q=i@YQO6sw;}KgvP08q2PWJ zX^07VH8v!3E3mvHu2AyAY(DqVgw*Uj}9b;dipeLoEj(m%=)2XDJONHFtgZuAnWex|KQuvGDUGsu3 z;yF{&%E$w40Gg&fkWt=`yU8tHI_7Y>%|TO;Azd}d8*or5V&|$uX>sQ%1E2@JtLuswBk_s1X+*(_o2C@kvO$ zjK?Q%y!ZI?vN^>`4z{u3&+$K^P%A2olhI+)XawfIK&NJ3^ z+YBu0nJhMoNPoKYGK$kik4<_I$cex*y!RDZzkvFN1l2u z(i?>Wr0ZFV1T1OdGl^JfMQ^(*u2yi*p$%jr8|>YsJjq0w6VU4pNd{`Oaay~sqgBb1 z0@DfvE1aVxA4z517Vgt&W$#^--Te(VjpUJhvDRQ&)Df$bx?Qe=-Pl(fUmpJnji6U?1kP z2+nUC?lK5|+GyyPosMCfbnc52RJDm8EXfkpElaqC-`CdpvL4#A$d9?NLbaY>eyeykBrgv&G^{6IQl((!r^pHnDUbJ|~qGOX~L*nsSw z!imMK3#-@LRSqTIE`3W}Y{wWv)L!4BiE41}P)?fMUYtgp4C-Zw-!t3L5s@a(=uz() zu^cDBtXsr7B5u|)VZRZzuTlYgx>>JJPtE6QU{*#KA!E~ziQlG@sEnIO!fa#R6uM{U zAC|r^e}sHgk&KkE-tMAA=u3I~OCTs~phA}ml4qA_eyeR703Sx|< zjI;cywr-Gl%Arb@+;s^D%4zeFOD)Hv5mHwUOZ@qKFz!Ju7i^83x*sr4rAknQD&tRw zX_B(nKk2mO32I3-Lv3}YAfAPrepzPN)-8EzalVLD9{Js|YCdk?=%V-@<62W9bMK6R z=+3Y|p;;^cWQ_mEwb4Sr$Z7!7opnnlUwBQ@t~r;ZQ$MMdx3tnI3NqJz52J4EOqnFC z1v&HH*PM4Cri)FxNDBzch1EepG+o*Lr;^eu#l^EXp&vs#aoX72n8d zK?n;@WSfCo*u!4v;vQ(PeIO{w&?swr`n~Kxul;C>dbTOI;t;LD^&{r`UWC9g*Ux(5 zwdQ5}-JP;$Gn}$FN9p^-4al^^ZAXqqQy`d zPsX47V;tWeFQ8c8r-3?8_D;@M5IdgT?JljotFBeVSGe?b0V^6(B-P7{5p9tt6Q%3B zqqc`@#Y1N%%WjD(SEY2skssLVNM((>SP35R0$1Q+1Y{&)E?eS4ZeQi&n^bx#`kga~ zd({3suNmfK$AzM@2!4|#Eao6xO5n9c4C{cH{WbNQGJWoIy{>#B?R^F_AFU0XN5ux34BY^%yE4PHY%=p4rs0BODvXyRy}?yg}QP-M6JA(_i(qov>CA=qsakXW3_2gx%V1=o=c8WfuCTs8ET15 zYi!n{nfnbnW2GFUeb)ur_IebV=3h~YkI1HXTs_j@6*n!ea;IGXf$dR z*ycjyj^QOFz*$FWbRw|6XETdW)BrR~!8y9B;Bvb*6%oV8U>`D^b2Z?~o`$!Q-=25o{ z>RlySv#hZTKT!D#7xsFF@>o>9%LotkT`iZP`B7=H}I+Sf8~xe8%i{ zUD>f8;@94NgK+lkxkP}AB6NnMio~T=-~R zB}XfBUhfOtaD6!`=iNy|%pqe}7&;lPX~~InURzOQ(|T#L)T1OmXhs%Bo6k|GYz3yx z)xd^Jp#h;ZDTY*oat{p1UBc(ALO;1T^T#2E%6_xoSF&0qc3i5O`iq)4i@eg@_btAV zoxJAkUh=T9&%iHPPXEV#jy_+ZH=a$y9)V90briq^ddeeu2;VYAECR z`XlL;@{}9~X;~ht6uH$2WfJF)>CT&o?PLkaVhelshADlJ?VVhYnW#bsdBchIJJthz z2Ek$H${N_uOIySvh0a&aqL4R*pEYia?ZU&gNIA?_ZJm**P_9@KBo!9#uNi) z8900MhLx;m5e+g6a4=+w zJ5`&ADuKwVr_o67Bw9qnY-5BNJzuIgwm5Xuq0K~DMk(p3VOnIgK8Fe}g{780e88;KUn8J`CTZl)gvjS0an9bZ)G$@|n@s*S2Dy<-k}aV5=Le;$a) zD!`la9hU;M-o~TN^X2v5@vt>)W`jNaJ5*TR^YMcqW|POh>XNDSc8TvJ+X->nN-nZy z`76xHSMkfEim#wp^$5VvGEWo@q4WoyrskAy1Q+g=G0w0*^oQ#>N7rgwBh@y^_$}I-%!<51qJt0a8tDD?96tx)y$~v5y>)+l*wp=vfid- zFm?czy?}&Dd$KDA)6Y>EYx&9>=5vVQZ5)Jk{i6|`x1-&58zpz>4PUPBwu0Q}5yZ|I zdNC1BHNA~3d1t@yj~MtfUheG?d3T1?autcy^tMm! zmeI%z(RgSl(deh&TkvX3zv(hKIb1^qmlg|xmz9K91KU9!IrL!l5j($?=$g-8N(?|%S1 zDOJq+uM=q*2O`#Ds}Id&8x+g2X`DtJ_3JM*GpGJf^87-V)$Th7k0>@QqED>2PQjqf zOX?n)>X&BKUz*<)esX=MWMLn$0O>x7ib8E2Ik3V$!Pzc6;@Rz@cpu36Cyn#hgsJj* z(b&(2o8=2)j;84I;HMOd;SiqV*iHh$8FF!m4dIAu#5*Y|eIWki+2JG0-l5{>O1&!q za1JUC$qBku}aW5+dG`KhOdbKhrqwpt?c_ zHz5?CHzDGT{(<5m`8!4o3_c<{EJ?l@99-#dj#d3RCPp}vxGl-BzVqO=%6bKGia4`a z%%Y)pchX2l?oVUr802S=rZNW3Wd*h+1p2goo)sA@SEOQ1I1h$Os!pr;Ah#ST#_iaF zAI`n>>Ww_UnfV5pt-MJ)ZiNv-Jes}sFeu?W;t@bX_;eFiCMb0qcL||)De|LQu#wSF zEER8o)(xpmzjma_AIam*lXU?J3r1b{=y7!N!7agIuiDwikydPeoy`GlKmGGW$#SrW zUF%ewnUIdGXkj8cQn=a>$9wir&Kcc-ok7c=T6x&YL5&C|aN}{NlgW(s_S8W$V(+Fy zhXqtRA6=ktnQ&A0eYB`!f>JKfA5S@6EZ}#eG|N{Qlb^3H+#x^4i@X3@>Y$lHl{u75 zc-97sDX~dN0*kSedDkUa%zhsC?}=Yh6XpvA;Cn?4xX)m2Yp*6cTC)`{>6Wi{P%v%tNHu-j1dH)J%#vR2IYSNB++#!~-ScYv3n4f1?BpWyD3)J##K{FI?s&^G5aXrObC1eh7ZZIL16*Og9#gJ zNIJ}9WZ5+e*v!lHm(CiGht8ja?<87oJHr|W%oWpNA%2N`7|wJPWI8_)Oo^aIMw`4~ z*JAR`1qP?2m-brMMA_%0Pc^Hk?#pP4tiIm``)~Ot%?n9@IY4| zF)^0Hu@&m2G&@jK2YS|#l+1(;LChHVL42F(f-(2!QEtVTN&9;QRK()}r6B!mfB2ZZ zWJb@8z*ZMdr8P%JDBY#I3tnMIh5X%&O*K5%Urf6TVt%^>27zg?v`BsaDE%Gx8R+dp zYISTs${@r_U<*nh_#pdq*qTsNG<6pTKUDCWl=RBg9vAE3fU^Elj46BW|3Da!;tJf+ zsa%eJom&x9vqOCmvU4bN=)dfc*L-IiDn_`TtvVq&fMqejMjsJ0Y|Vm%{ACF#rFh6r z67G1xg_&&(K~BcI_t1cFMq>iSy(^>D;==!VyTp-xy3qteTQ3a46;DQ#NWvl_(X{lB z1gsx7wG9~H4*0T1dGJ#PYO&Noz-))*+yr=4p zZ?ersOFCK$TGkqgF+p~<5mSAq@O6Net^`6l0m*lI|V>m})ZI}|c_duY0xe!r><1?huO8WOUZun(zp{CfZgBm2XdF~hvWCLzG< z&&FSpZAr047~%&{Thr6)+y_@B`$Gg|TSYgw$4kmc4}a#OURzcn;3B-v2$Dk~x68xs z{?Ds9%|F~*UOZ8nd=4Hzb9-hzgKmJ{>0;z<(r>x}kH)5c=*2*_H0G=I(QREqcG zOjR7s&MK~2y15FNQa>4tCm#Hv=bing_)0#TWik2FAl39&_dzMSFdds z8LuGMYL7FKt&!&Uwu_xuqq48X($hm_stp9(NEa6kwwf1fd2hETnd()Aw5n`hgAen* zm5;_L_2-{0QO!b#4eB)8oTZteut;}F;G~3p?g=NC$eIb|B&#+=A!F4$X24>wIx=ET zrt~=~ign7Go={?d&Od>^Uf_07Xvc0Dx6#FTdZ6ZO`+?TKfnGh2f8oHIR*jUbUW*aI zJk)vkFBCl0lbmw;)say>!}S-~B2Sd`(D(ei7lRi4pvH6U?tUv+I@Q~kdM$7p6Wye0 zMX`t&U@xYt!TE%Fsj300A^g7S@C}eBvV~Je8Ixwi1 z`BB?I1yW~7P)hzhQ|yGoI>T#_*hLv^1>-Rl`cNZQG{)kWWG(+8l}Ou+sf!6t^kbM% zeWlxrwea7Cx?)xy-!SUUgVOGTR6yV zB0JM1q+(75f$ z*W!ri#+!kc+}af*&&$JRbkYX@?OLP|q~EovXsd-gd=EeoMYY?qMFRP3X<)1aMf#Vb zH0Px=1-v*zf2xNy6j3-p=ZuUIAgf;9Gl1It4l2pe+NEXPpRuJ;7Dl*gW;uPjZHyAK`8MJ#3&_U{pJb-DSyQ(_o=`b@G zoL3W|Q3k}fVPal+7aCj)1eQzfLQ;R^$Joqaa8A{|<$j{e zt^n$NzH`9*{FD!QG+%;**l8G63d-@9yG(bH@W}7>Nz}Cg zbN}+N{f%XO_TD8d|M~*cB2s@gEJuE(H?u)cEywoj&CJAf=hBR)j!fS&f~4anAq-7e z1hj9X#o9#gpSD7-KR)YG7FR&#%mhM3(*=41$p1S%V)Ox7xro8h65qMQVe@^wl-ziN z4H5(^ZkyD+)8lav^4f2Km5S@b2adN+|JN$pB>JCWG)#SO`sOd!xvyIO`b`}|x7*at zYqSj4=XXQ%MGaYyKc|$oeD8ceH^oiZWhv#ahBg=?OVi%gv78}w821x^u`{t?;M=+9 z-s!W$wjsR=!HW-|qrS&y(HY5t+hn2@R75x(dC)3Vu9x;_2} zUkX7!?Iu1$wSK0UzZ{7mmIw$Ra?%q=+=XF&NKR9ZS+o;32QIf;QKAEZo zM%zvW5pVpSolQwHLA)uaf}l&3|M9+Kqur#Nb8sT1=j}$89?z4AGM<(ew-7%nS&unJ z^H@vvnNZTD5hT;@%KN%ma5tJS@9X1?^UP#tml{1BMba5f(IM9R@9psu@C+GnxjhJm z%9V;!)b!!RM%#@9B}5Nfx5d7ou`f5D5E!n6>kHy1H9Nm+oqp!RD<)5mUyvD4 zB%M~3W2pG|(K@)@h7|w{>>u5E$aUU*$fF{=2I(3oz)(h5DKA>`X)!PO%eloVrh>G9G^q+mk={EZA{|AV3IqXB=^a7f8+h+{&->o{^PN9ytvS|NbDzEc?7hz# zbL%-d9I;>tz$p#h6(fiv`Y?v{Z1tvUA$?XiiAaNU_5h3`8x7c&&emEzD;jfG(*wjx z`;dWOuGE&XfDDgO;op&)jO@6l!dvl{49wKyHyHY?IgdRT@PSK@l%AXJ?~%Fo6p#?# zF#@yCVIne%fi9HSc0N`X!Q$$}?BYys?Y%WRwpRRzWkV1+uantTgtJ_vupslG+$X+* z1IFX|m8mtkwc#um$Hnb*0ZSmmO9t=Psi0$0tl z(gV7+O0c851cmXPl-S#Lbzf?_ch7}mz)BGmpvk$td{lQ(SuM_$(z4a!$*ukJUr@5w zmFnD<9?a|`twpErUh25Yu5Bn*-tXur0*c$GB21~sI-S}NfR{6$$HzuT-us?R;@E%H zpTdRphjo>MuKfz$FK{YVVa=|LMsz63)4^mm-#8vHl+Oe?EqJS;iYsf`%H><{E{ls% zvHRy{4+)*L%zei-O&RiCC4mX+-#hHSw^Jf_@8r|$m-8c256PD6j{DYc2Rt}1ctdeO zho9=>jU3C^1Y=wrZY)0<6G45W$Wv5j2!RljT$V5P06EOLphxbaK;@H{O!k^EiZTe1 znVo)Si&=svhh&|kqCJF#i@cy5=^X`~vj%d;>wh@vG=$;=(%SIkAZh@9Zz?pfa1f== z4TIZ(bCBLRNDcHk047!ECZ^j_z&`tcu5{AlD?)x780-Nxp7T8v6b_c4@g!R@9{w7x$XO}k63R7F|>7bc*2z3Qfa|MN; z|M99ESTQ~w*Wbzu^V!YrO;WsOAI9&r*Yf0jR9AdTom`hSJ`B;}v#TO6y6%h${e-~V zf{f>j2rbm^hXbFj>RJL!zI=TAGvz4#mW2CSknhCMli^e8#`VEHMQk?vkqb^JGgeMU z%dC8H4wP2(K8oXdWOLn)?9a?SjU1WbC!H-qy|BO%wG5$9FR!gK)7`_CBokFjkkClT zRB>in@;DDY)}c>bf>aBbQRt;0>rS*3ap7;zo@y!bSlIgYF-sktA-RL9D^vvO1s%ZG zGV4F79J_f2<%Lt>3v;YP_;EpLo0sJ$i%xEG)m3XW%ZdwL#-M6)6$HyoUd8G5CGv%u zt`4CD1;y96ViHiG4=bk)?uiT$H-WX&=l{Q*@gvfp70BF4T$SChM^3L%eig@%R*^YQ zgOuDnb$k@YMH=g1pLUsX$Qfg%m14)x%Bc@ApkdJ8;)1Mw7dD1(G46ln&YCy7$b1SRev>i7>TlgY zk*o-g4;Fs({Is!RbKpi0FcUHNva}ed#~KxnR*b29X zpBNx?uh`-Sc7~NXX<;(5ZF)_NU2^1k;nWOG*k}dY)q$ujL;;VFKtX}0ebakPZqW@- zC0d|YhuQDujv_bCKYdua$=`XtOpfO9Ack3bo5*)|M(ovAQoJs)ixHU_!GWW?d-uE> zQzx4HC`zJ%>pHgQ+T#jT#jT#64z|^Obz~Y0wP{a?Qk(Hkxg6I&Pt%RQ6;Z&)G#Eus zzG!_c${@DryM_TJANfp{SHUgheJ~SNOfByULJrJlCURx_-zbbUTzC^rG*xv;6jqz_ zwi15vv5bBdu7Ceogf=}@Hg4&Q0?=(%`oYsslmRK)pfTby$?%rY53&>L#2~xc-KjOde?RIWX8&=?W zlb=Rg7rvdlp&WcPy)ty-*iO@?X&83OGg-Z^g+Zs+%$2&Oo)djRY{k_aZu;pa)L^fD zxWR9qEHZ$3dpNCX0eqMhbHnI6u+Kc|W_ixqed~n?cGV;kzVO$ah;I;<>vF$`S3@jQuoi5VP-NBx6(TwLg<%>VT^I~E7&=15%O3r z*ZVA>qe8po{nkUIh+}83u*c-#w8<&n0C2Q*+S-_N2AANl?)Jw#4Rs+fE+9HZ%w7Ix zGp`tP=!-_&`K|^G*snT-;W6-V_Z*g_LS;;?JEJgI#XmtNIHs&)`7MyHOspz>Qf+W{ z90DE1eaSTaVf>ki>DNMo~_7{x>3wPc$ql0%3v_F*A_7D&NXB zCle_v{CQQ2%HI1@pm;`!?%n2d-VDbPQBzP)C2QQz>C=f^2>T_f<^4kqLo$*anS7dW zN1Y0DXx{hBwbL3D)TFNrr_8$S=B{1A6Ai7oBL~(%T{0}Ijip#fhGYC z$f)lspQKt?iX>%T@^@}*~ErRezX)YjskM+e<$a0Xq-cDpZh z^mRe79%jI>Z_U~2u>CU$&G~{NQvzS%rwY3?n)Bb6yGnmR+ov?A+iVKTwxBb;BrOen z^)k5K{eS3a{Xtha#o^wpR#~ z$jkrosWkucxiyV{V*#41I5ax$Q{Cl|_42F6&%gN-f7suv@|_nAPHqd$zN3` z>2>&J$Ro@0ZT>^Qli8oVPMSa9Q;BBEn`;H10FBpamzF8=(u zDX!#%*osF=-xwciSNRoL_LQT%-M@c6HS#vyW<{T-n4usxCP|8Vs2`Vnr7Tl>K{!^& zvdr^of+uemow()<^rhQc@=Rapw-i0PiGjKro_){aiZ83ZUj}zFWvWHl&bMR=mHL5D zXr-^Hh;^Mv`&f7uo%#FgqH9cpDlapQ`|KB+KCSNAE4CB4JdPJVzq`1UxKD1;CEKM7 zlUu)f@UhhUm}2&hpA;8v)A*RS{H2kN0}B_QS{Sdg{1`m`vU!KqV#=t^?f#u#%VKFO z*4SR?z|BY9w^c?5g3Z#9qt*`WQ&mh)zQ6rx)3}a*ak{@2H&m`J>m%O%DNKmACe^KA zu5M9b>1DBY2e{c(Zlfi_HA{4D)o|}+P9ik=x>6szJ$>DSZ8xYij<{`2ge~Yd2DxGp zQ@2Y(v9Gn<_4(Dx{VrOW2y7rlTf8BH4e_F!n>)`bn!$JaG#%c&K}F`tmOgxtr)p~8 zv<5%K6E(0&ZtXq+W5fn-k$qOx!tp3|X+maT9Qi?3iQmT!$iz4D2+fa1^xHyl0}6~l zZ?G!ZglC{;vUndCX`?UD-^fa`)RkS9cg5r$=d*F%VWH|!ujvoQDDCoDp*vT`yZN~s z57)IbYqts1tflzZHAYuY2i7NI<+dF|t*(7zob}?^_Su*=&v(<;#3PMiR4Z{0B zgnYjz&0imiJrXV@XdbW1HZ-+%eUUYvxfUsv=g_%9<#SmI*QI>K&Uk9*HCBs`H<0zT zdZIjuQQ<;tsXIvEettFC4|(Y2b8jR%r&5sQ(nF8#AHr`2O1l~D@7DWX4vkKV)bB#8 zJcD=qq|lnv&I5{GQYp+EM6|VTeaH^@lqxxH^eVOGp?IdnfEn74?n^|GoC5MkBdWNA zWO*-uS!rrH?m>#nKQP9~`AWGKz#VGg=xYM9t z6<53=O`8m!`+=8a?_5rUd5*^0Q(-02k?I;PO)1~&1vGY!McaHj^}7xZ(v@`R*3E@njQBM~!`)~Zq)V-6_H>CW|bV~i=-O_=DDtr>P!E2_N8E(I2Dx7s|F(|00k)lw4DQa4v0`d-x0hu>fy!YoXF zzp2BWoY->Ji+zBZPdu#5z1$TdPQn|YdGX5SvE7Mq+c?mxod*qhROd^cj#*yK>Xz1s z5RH_+ZzT53sjQfhhbtY=5?rdSHJ=$9O@b`$!KuYxOcDxlFG{L}vWVq3!71M{KFpW3 z%jU!+TGDy}g0VX?aftZ5fM}?;ofc2i`m=ZZYoreY3%!p`>wPb{kYD?vCd(4W!YMU? zK}jGM2B^oH%4`qc#m6_jE6+SVSt=i&D@`$1E_9Gg=Z=(vIS(JoFcM?)$B$e5&++?J zG^IB6h+GFn5h>h9=q}xXdNVx-%`IM$R$+m+ZEiC}jeLoLTDNM3 ziw#E_xL5imKwFhr;M)ujhcmY4w&B}5V^h?YEh9>;O5AX9sT)T#Q=Cn|0(o7ct*~xu z=JP(@7?MD>dV)Zh9)n>$^{)M>>W-NnF6F(gD@#&OiVR(V9rJ5B4lRLtWB81W*Jfv~fs|Kw-f<-}TMS8?_g(Kf+t`>`rQ_KS z%nP<94kn>i1!ZH&cQV&d=^3%rwm{smZ457x;3#R(r$)cwyZRkPwY2i$D8bQtAlN7LV&F(#h_S7pY%t!;EIyBw7~omO}(KSUgisDrTN;+GcSV zLHL-)Vta`eHh14Q`bGTXP`YHcGgS4oWH&UU)E# zMfO-^v}STQnW8For^N*xsZ$i}IT5@LMHKPM*sW!)iqkzFTiuT#+tHsDR4P~%@3ByG zH|m_a6npLbi-F?C&%ufp1ZpeE=Vt_uUD^VEBx)?>WFA>&n{q9?Pg5;i7nz>JWqTZ} zoc+JQ{n}W=9tr@n5*PnNKdrjQ$Qbl{bHZ*cMR}$w`7gamf{n4D1lJ67KqRyP?Y|T%|Lhw$(`NhkV^2nyGiKsEEB|K;0pq_j004-T zCNP`u{Z9H5=lC-KjCBYe2yV*1i2rU&AjF%nkR<33$`I^?eiQQF-**P}>JZ|Q^n_ms z=>NzE=@QtH>;$wa`F}C8{)16LmtczI{ErICW-KILx`cQnD`DC6FUB*dNS8oo#`k*w zXW^_a;TMus=%2H6R-EK-2L69A(diL#&14kMX6fHc2ms{&NOIEy055NUH;4bV1cXcG SEF`3+1Tk}dQncCGd;bTXi$;3@ diff --git a/Documentation/flight_mode_state_machine.pdf b/Documentation/flight_mode_state_machine.pdf index cd234ada73ce021351db8fdc8952dc20aad29ca8..af41953eec12e70623e824f8dfeb882b60d46882 100644 GIT binary patch literal 110086 zcmdS9V{oR=wl_RUCblQGZQHgrvF+pzCbn%(Y}>YN+_BB4{=3d|_FHvMy|uqSpH^S1 zx~^JXUERHYAX5|-r(>dLg&`X#8t5Eo9>|4ZCS)YEH?o4^tABvfV) zx3qCFbz%^=F?2B%H8r+3G3DonadvSsHME8C$ePlgh%p|4-#VwcS{Hm?d2c8v za!qbW7)eM&=7!XW;!%Hd9SV$lE`MlmX9@e$>wrjASFTCByjOUb^ZD?8)iNfhaK$A{ z_X=b`$_f7Zc;3qGN&jlPNBw&LIG^5&JZ%kBr1<2gnyS0LCnbE?@VoGRH3Q~!+;_>Q zevd~nscTKId_;LV!_N%0K2yDaWRVke0-pGTk8iRa4_coaw2s**KAuYmOwGU6BzE2( zIsi9`Hix->v*iNU{9C$H$F)o1cD6~DL&>q`o9mRh0Tu)esVbaTfU%h+&YgnFUq_Ae zvZ3S+cxKZg%i^uH=2p(}{1=_q`&3WS_cM<8`47CQPVEsfvrCX&2p#C3t{;`(y;WD* zZi=Nk1Eqg>qLo9DQr}<5y>C}d{ej(|wr@9qFYejfzONIJ_uk+80g>+|55#_BMV~4@ zj-KBuExB$r_U^70Mnfs3!Z)pACc-BLa#P|-gzQ|u65DMQOpSqUzY!83@%3wGRFZp= z)aT8V#E;drsyR;TtSk`NTp`Vt;>%dsxML>qNLH9lJzj%@12s|1wgi$fHI}2Xx#=S< zv3!#5iBU-q6xpkT(nYW`q)f}Qrwzs7i}!VWbKY!J$$*7KfnbG|`1?|^Y=vjDYmoAj z8A@uFEN8u&>{g?4Kkd@h_?lbW{a_fZ_M8328LB(%2|TK0nDwHW>sp0J9acs&^@c*o z_TGkxHz->NLZP+kxZ%Gj1I1_+ilOeOnG6fMuM0QqKu%Cge=u8*h!HMCSfqjrZzO@5 zB=L`WPUWtwI~c`i3^*h;i$JLR)KQo4ky4Wkj=;>=Mt67#*vBA4l2k zb&k~a(FxLsds6$^DZNVP%|l*o@thzCwFY6bfwjqAk+z97HEa|`rS|QAgPUD)Qgf!+ zY#Rr|4havRP9swtPTnFYDI+w?MU?l$J7q3XYtlL@C*WgLkpAYKft{js$&b@4TougG zl5PAsiN^@aVUN6C1QUEz$p@@#7fGizp+4r83+jPKa4bVw$_-7W{BE{m|Klr{ZGps6 zoB}7Kv_#aIU3_+hxCdMQAGRaEObM z2M3cx@Ej1kgG|>$@aGo5nAjWk6KM286H?L@^#y`F+2QIy2MW2*jDWACO=221AHN)R zJZjt9q>9dM1SN)Lr;}?@>u`T&QYe-D*IEC$Ue_9M%Hm295L;2s}pr8XR z_jd>B!j zaGd7qQH4g__qS0xQ^#c>seCb$w)aIbuh`R9{qKVaUqcg)oA;ZGe^S7I`7xpKzmMvS zOvE7dDT>iQNW!!!7EGe!$_z)xvhiFX$7BWW2f-nW9V&x-$vf%PQh{@;ltwL#yKc%S z8DtBo`3p!s>C}(}O}h5q|B8SKhrw5puWgIh3Tj4=l^Lgclhi{^3P;ze4z7b+Umj05 zk&HGysK{L!T_0HE$Dz^#O|h1a+!( zIln|v1QZahMCh0j#CeXKjJxp_1rCEy5M2$8)+nfCrEPRL{WT$yf+0fb(N7&pFYA$L z!1-}n!G#D5EJ{PT&~GVO>1f5rY=glg-^*9cvebV~E%^KhYh`hg(QBn1EH^ipgsB7l z7CH?spIzyn@O-=sUSqz*q5i`LM~S&k)kYR z|EY_!c|im*Y&H!6gWf<%LKU$pP3$)gNBEe+jCDx?YHrr0N<+c}A%O!bU(Bv0A)`1! zN{p&~WaKKYk-wHE`Pm-8pHCao@Gu=OQOLATajBSf$gX42>~#ylJGbA|A29gQ=-k8vHVD;rkU0JGtQOYhz;!$BcY^o^?)d3N z+%lVltvZe`_j)6K+hYm=L)(cf?g@{njfgrFR99oojK%fq{&~A;!-S0|hL7a$Lj5+{ z9KsaalS`~A+y_SI{RZE9WSjmC-Q{5gx?kC=WU8Kp|D0BcBiAFL61Mksiv^!U^+>v< zZG2?$s~AKX#R%Co2HMR|^};?j*zDNOR^|yBF`Q-j<;(aVlxl%KF`$xN*yj8(t-H$~ z@<;9LSQ1x46>H0U2oHQc#tTgkM4rx8$=x+Jd6EdXm-@HwnNayM~_##Hx3R5F<6Tu^ahqe^g!wXR@)Z~_b5OE!x+j1dx0BH@`TY0c_ICeySnQ41@pmSCbf%(;9QKyflJ*um>f;VWdb zFn^*eO>mg;c$BG|2I3Gj$DZn_D<-je7N~E1Qhi|lDPadcWCN*{%Orlqo?uEIm*0Lq zvVG6dK+pMIex2~u;p4Fe@meS~n;Q^Im0BoV+TC&g*|8~^N5W(gjukHAU@xdv!eJG+ zwuq*jcA;d<-&Wdl0VSV}7QaPgxH9qvTRbcnt501=NR4lLooay6nOpobGt12;j3zq{ zJGGZOtOtt{a10Z)EW>K>kyJAC+!aTxM%CwxbSh+*s4eo0J2DESaN^wM!c@kw;}%z# zMo75qtKk_JzCcGazj3OpM23{E{zwvnn^IJayuj*OKGUF@_T)WKuOVEHvEjsaEt$H; zk<$nTo-ud?Q(esWH+`(rMER{Qi;u5XN~>X~3mYw>T0mW#9$N9`(&XY~N@pBla#P>8 zz%?Q)i{X%Q;9WW8d9|j;M&~>N5Z=5IgHqDoo3t@|9{1+wa#npvaWUaX3lBAZVjO`c zQO9&oLN0HTC=uQCQMH)-Mp;8f3ED+Xmb|@NtmvNEU2S7=!T9=7aeiJkJTh0@vzap{ zQJzJL^GC?3ZSA5;(JN`AsJT03 zl8TzE2*l&32}EKXT5z7ZjaC=><7nchDWo5uvkz9n{5-^; ztq;{sNs`{47JdfjH^LyodtICsbC_NUeOPlQUO=!cH#2x35*qgxPEW$vKN-Ba{ zqgiiB+dz>b9WlB0$KOpk3#cd3C6D!;&KZT&E7BN9pF*+#e+WPSjH#W;e|6iYb|(Kc=rI5E<^K$r zxtQ4gJIVU*_FdJ}!IVK$!N|(g*ae0`#ns5=pMAh~f<=S~8R^+z2nh)pj1B)an3Q-hbf| z&i??HEa@rO+qT*6ysK=w`UB%7Kf&?gOfsbWFaBcXMds18q!-lcn6!_s)Vz?)aCNTC zBqXp4MuO;z8YnNuOGXe=k@1df8035bU|SgW((UMmfNm3fu}=)&Zx0GR?VlqmonwAI zA0KFkp&RdsgszP{cIyzA8*|~T4=aA&ZwoaL=^xLrJvh4i&WEe-mkbApJp|cb?<%sN zTw6|Xg*$0y&f@Qv7%K69qM94`=|cHm29t8JF5SGlKKD*X6y8ho@jN)aGw-oT2eHY6c$7%jmdU`5exF3OuRHNn*J@8vsh?kcp6w&-iJp zGbcJz^1x&)lVMb05eIn82*DHkRbN{)I^{fO8Y+V^)n;+Hk-|E*yhAv*$R_s$K(W1r zrmlm5x32386=R^!z>6r&s6h%()|rQ!a;p&GL30@)yO1=u$&o?p==y8Okj->;^M_8r zQg~}FAdWZJ5R;>k@Fzv?<^}}8)sWIn2*ypVVT{ZWwwOes+Y76ccJIJjrD?SF zJ-}*v7PR`J(cgM=Po;eUl>WSFFM&5z7)QI8jPv!dlgXTmhi-QgXMSveoQuB9-U6g# zzbH{>Z^#E<9a6~9bblGV zX!@<03(wEw#Xo?vIUAtZ2?%K{1RtQ*(-!Qqzsc9fBf!pC(nc(8NK`ztel1$g!dIlh zxsw{8=As{Xv$-kTx-WALovD=`o4W}CC_i>%CO79ps&-zBnT|Xc>@Txid*~;10mVE= z?h7X6`Khj7^pi3cz@!U!&c>v9wigGARgZz8lslqS*-je}MdtC0+Vv9=HFRCmGYH0T zo0n+BPb!1~sRrUYAEI24NEa)W1nJQGHhg1}KaH|@hgUfbMWJ!$H)TrTwu_Wy!TBU+ zF+Wfo$;)26HM1;4`z5Q04O66gpjpJ73X(q4EF57B=ZJ~#vC?O)hpEtZq+iri)H7z# zj>Aqk#r7j5rwg2#O&do(8lqy3`HKCS;>Ul(yb#|i9W`dp;g=MNzMOG>cPHJTud9gn z^i6+$+MH_+5JSu|#^mj!7Cy2qMNPik1(N3i%vhK1sSM?mMxCjkof`|NG*1KnuGz?1 zS`9|w$f><*&Y8@VEaUOhWjXU$U|-(!jlC970dc1-+JOrPT#=|5)4&TDx$V7$n#Urf zX|9i~08lP^w0W*jmJFxqs@VrTex@8}9t&doQHr69*TLp|$RN%!DUrqj$hq7|Gh>KS zZ+cvg2miEe0E&H7Waa{>w4n&M>rT7HdV8YGj8)rWlxfqU3* zduYxF#|=2>H|%1^tzQ5Yuk?~z$i^7OvZw;bO-z)JF%4C)k%#@|{gEL4*_lXgKT0vs z(r)nY*G1+X7Fwd)Q5O8-n04E)U7M1izLfIamyqWhny5Nijo_ipRg^ zt*^`8igxYpIL5=R*ts+0dg<;+Rs-fba=m}{Ngp0!J8BlG&%P0}GeqZ&XTjr(>3;3* zSb}$Yv3+pbJXkx6ysp@)*_W`O!0f|qSi%0nk- z^OZv_8#=>Gm+YJ3y#Iu1qb!?DCQMzZTanR>33rND`jD`IAQ`c)uj~vzJ)U66appj% ztSovdtw=D;PSZDOt~!N=ECZNGwvvIQb$SA;JTm&m{(>%R;W}txHitQNk7CWMOc=gx zViG{cU4&(HS~n@NxF>NSdW%pBayBYr{gbAU?@gH3cNx=CRdysOi#^{?XdV1AqVSBB>BSVB@@QpQe+G3`FD_<#O$0u}TpFR$O{vk7|G0M^cN(M^k6zAM43c$*0Sm+AuITih zcll3oTMd{M4uJD#qIvMKRvQ=%7yp4xz0h2?%lMc{pv<8P&o8aKCYSNC!P|V#;fG#g zitH%Li;TbAbiXsq{ox}Efdlul_kYxQ3G8?P$_`$=mN^^(wY)>TJbu4jDRL8pwssTN zPXA=dvA-N1zb1LA4g3+)=a<=@3smmHPwE~h5uYis8`Y-Ek+J3RnhjV9WUsO2s;&Pg z=5p1x0~dg<+>q*bcWb~$K}dCHuekpHNETW*z#*Xim_&_y?Vp2mo59AFTY1DXw122_ zX1VMYN6E=2$vWeQueo-g@0s)1dzOPZbKeH3?u3%zk;B1Xo3Rk+D6FMI`lLJ03+ek? zjUx8|Q2%%Iu#xb{Q~bM^+FkcJ87xGtAa%O_z*{OfXCZ*OCWx`}%!h%?(Z)hZP9OFx z=9*0&8B;7%Bet}M8W>#C<5O(coB^m#-v!|VA6+?r6?QXMpplK3VCQ7EPB46?1T zw^v3PmZq6Kkt1j{L-j}H(&|4+e)=k~l|);Q<#k7KbelSu+aR0r#uD>U<){MdpS|k^ zp4vS*it*%C8jXo+^?uuhtEg3g)K)QrJ)qSmtS>*!k-&)gil43_iGkf}S*5o^9K%59 z=HzXg!Dqt-2hE4EK6oz28zOrkwtWQ+K$IEb!-R!xKmdrs#sGWav1x+zVe_wVW{AuZ z%gV)Lg@OvxEpJJ7KkPm|DePz8rVv@Xfdg?Wl51E)%h1}Zo*`kD*es9YhZ}8o-{KMV6>o|z!j>J zz|~Y->Bvn6iCl^9*69W8zREr)v&>pf2pQe|L$I6>CE2w{a@E!klu3xE(A+KTpT0S8 zx-xDjrJL7IFm5H~q6P9FR=8i>(|mZ0r!W4kK1_3X2KFjUd_gnKf(g(brR{EE@qhH8 z^XJx!)6ttopRIY*$w-kus=(#4dm)w-A%8rA^Yeay<)$WxbBg`qpT>g)WT_&2G(8Ky z-sxLp9r;Pm5i|7 z2{vjop{w@6i~vdpcc(NuywamBcf*2rlhRdorLHnulRecZHU29$J)`Idn9?;jJ)ZQ`G(Dr-A9W4trqpBH zA9aZiIx>6^9T`@>T;BTmy}n9S-`Ao?^#~kQi=ATIOt%{0Wk zv%XD7O$KrDlIp%O>|XWkoWJ-v-tt@SPu{KBc+24!#17;_{4N= zC0j)w7kW0k->cwmo)&VyUf2dxU)bOpp60Atf1S)djb z(`E%e5SbA0B;{d0!^1 zE$s6uY~X6QF_HSg_Hc9l3WvC=FO1|(*BvK}%�(?=JR|$)xXoIb-Oll>`sI@?b1J zs@*_) zj8x<{O(%1hEOEZP6!wZ)rc_aADdGj3;MfXRP@boHWsg`G(Oz;t(IuFj=e-*{3O^vj z3<13t6*DOER?13XkfQV!&Q59C)h_mjMzobwYv$xv?ge*n_&yPu~>2A+U$W z+rICKkK^X&8+=~YCPrhp&`G^Zg$vkfD^&U~yrJW--$CN|@R4j)m){xpy-s^PP#YS5IJfm*9}qz`+PSp^@zwY~&-s z6cmAYOCZ7VeKr%K)476;_AnoATQa#g&p|N4r>)?d&f++^!n51T#A3EuXF<9zqFk8^ z?(T;#q>uMsX7G)%vqDz&+XLb*fF-ka?x@qSExFIY@gM!4o@D(|i?fEPY4q@^KX#EU zc%uz9@M?$n(}b2c6qOoADxd3YLQyP!y_7bo;X9A;Q)Imsbafgj_P_J1G8%(7u`*Vh zU)^B;!Z%iauk16sof+b1fpIjR7cPRex0Z3W%YrM{;t4jabMVnv=tkb#2_@Q^u zqnNfiwqvl?E6_I!{bdz<{mg@~A$jOfbGJF44&=mWM$JNp?|ubrzG9&_mFC?moL5I@ zy6NT^Bs~`q=K8(|yGS<)v~vboZl+mU278fbeWAg&j|?*E^b_HY)OXhL50p|&s4F$6 z*O(bEJIq>?>o~Gd7<6PB299LnuQpY&Gd}e2BbuT|yC|tyYSuEF&s~WM7inS2?HE|L zJHZH>25j`XoozdO=Lt$-lGq%U+cgdHuBSpVe9BV9*TSamMejI?O(emeukcH_#BW=9 zu+G+~LU@yP<}q9i>;t7%q&)fdPHIeuEN*bJGfB+8=`}>flw+P94U{|03xK5Q0c|Q1 z=MdY`nH|F&DEVBi!Fj~UpP>u1$scNTG5hUmJhYilH80~Uu@*bR-v(a+ZMOUpS9wjg z>ZzYrerBmn54t8FLOXt~u^p3aI1IXy;6GzbiiL&2nX_vOVD-NZ5W@~_jM0SlVV5!2 zYeoe|4pQ@y5A&cTg4zCS!`u%tK5d9IBHR{S^G_6Shf!KoN%Xlj?wV&XR@ zw`+q5UBI^`R1w8X%GB%6y{Osp>!t4MfbHitoM5K~Nb~qgy8)SIpu)Y@m&avoQB#fB zQ7B5e%uw{0(sj_9<@TPf?T7sE)bV z8taL{j@=6lFA(;*nvcaKLhUglCe+LecKc={c9{aAGOTU+uCa$mJ3SuKyc6U}AX0-L z*LvzUJ}lzx%qc5=QOrpq(>vdpqSPG?Ma?81su>w6=L*olWBu85UV(@#$&*h>Othaj zBW`tjUAUSpUC`tS-!2%&m4C;j^K+bPVK`2nNhN+^1;i<20`hdG6+A~V`^jQ`Q=Jxt z47nhRk!(^*QD(Q!Eu^RpbDUXbeVIhf4&F0_iIi8?`KA0>q>KMoXZWe>YiGA=gk+=Cq*ON3`QU+fUwZX0II;~H&gbI<9YWU zI@Oa@O@st|Mn+7*j1}H&xVg;rib))`)5>#RRUv9SW6R;Ou0*(xM3%&Ps3RmFFEz^E zEIC1_+w$s3a6^;OZhrWH58bJgGHgz@!YXwH_QqE<>w9453aERTNG!~DdJ3lP_LYgl zOr!|sp#BwM=f&WQpF!N>!>VW@fSzbHB~J9SnLwiNtOo_ubB%Yyh5z@(t1Ldh zbWG@iFQ=sH3KlfC<6PsEASV!i_?vL|W`N^Yt>au!HChaGKdGofB9gX-oAnd_f$!43EcNuL*4wdiP@93;B2y$o+Z>l^_28yw7b%`@R@! zar-Fz-k-@n_2XN*!Hqq{Yj|A0(A{ztZ(vP1d@;B8vhBw8^Ld*Hy^vP_*u-)p{2?hJ zHQB*&ptSES#4`C3B%h>vS(=UbRs@zE8;a6|iV2zG+E2D(enweryp!X!XYW+`o8ID( z#GLT3jGhaW3K}!usm?kHI@-nL!O;E<3p11fCi~_osYpj?r5BGxyKlpNa0Gx-c;isK zd24h~EnlYcOwM6szsLo7#yvQU=Vmc!?HlyR z$%yfDn#3V;M3YdeW}4v^-@mjFDYppc1-901y#jkWpfWS@zu zacs+rZOsdPm&r4m=~7bbZ!#$FgG>a}fd(P%tqKbYO6*ZWfW$>X9$WW-C<5IAaah+?3tf>JHB?sYS|*zTH-a!@A`30yYl#uUsB2T5Nl@X_MM zWO6YS4@;gL%D+3O#&*_}WMD2N`#JfcUf9Z(E>IiGP2SeBX%Ul~q_65S%L>m;-qlh` z)pC{V5!_NvDI<==peuE{OWsCKlc4TAn^6Z$ zUpi|VI5v15#N=f+{4>+Q{6%NoyX&^pNb(&ONMP z6W&4uT!(()0LXC}AO8hD=Pzz)v<{OQI#QILct6CN*j_H#SQrO+VVe>f=^yPdymZ2q zH*(w!NE4fF$67eGimv=vQ&&D_j6ofle*~TB!6o%uRZAg8T7_{qhixAI97_xgR>L}d>kdyofa_4aS<%AYbBP8212**y8TUuTQ*2bK~s92u`(k2^ucoHS%Xz{lQ~ z-&Er%Np6oeB}9IYyFno**n)q@G!UmP%9(TswqkIDz#yj9<9uk{mDfXV-w&g0-x>|g z>qYg^TqUI4)TI${RC<9Xwk)aHTydG%U|}J&meP$*(%b1^#oAm;m!oB7HdgC()7lrf zrMBh>tSszwSCT?;_OpxdRDE15kO76!Tu8di%UHT=X<+#^lEw%>3EU1}ERJ3}4RiZ6 zjB`7|=%{Lq5h5B%z;V9wEO8nL6>FU+?IuB=lIruzaF)lqH`2g`bQj3v>Ya97=`9m* z(4Z)4s0z}-FD&g>&6$_eKZj|d&aARoEhVJs-7wHvjI+r!xyY_20}?K21^Z2%|#f*G`w~&P}$x(BMr&<_IxPBrrUD1ik#ud6qfl)cj z-|$JI3OIF>KA{$P00v1g^~9{P4TRNxcr!o|MNmkFxqCum-P)5H4(|OeQ^)T@Vq?A7 zQxOxErzVK-m3RDa9d(t1S9+%9ucY z-YN`M{<3$VAmOj|;DOwUbL^aC<>eav{jb7MzhqV|7opoq4}U_?9 zlB0u_HC6GTFt}I{!Uy{jLHQ%CPC;lJ5pLY6z{Oc7&kdR&GOq7k@;GO zGvG9w#@S4?^$!u~Q?y|>cq^<^<3v47u*Aw{fo4i8d76K74+@-w>jIKd4u)xW6RONC zFxUtKk~`DEndU-Vhm$I^R5{FqCa1^8BFnLcB9{5@#Su-;bEV35&Y1|AFD=#=aM?aY zs%d$v((+5U^+gSh1T!ZX@B15$GKR~YDTTOeDEE)Vv(Ve*O7pVbH%b>Og}l|unUwgU zcX$erq&uCePqX}plrJB3x0K-2;|E8={WmV-8bDXlrakK0mR``1WxjMcguiL z=#UEi5`&Xp0%w-tGRB2DDZJ9QV9PZ{=&daLQHA)3%VHrfn{Fvz;z?Bwd^b^%Xlm!~ zd!*?|m#L^`!WYpRXNSjY`dkC#6hjzJ?Q&sbx9J?NRWGEpP4o8+?xI*t7b%NHv)p@~ za;0z6oJ=xxkDUl2^6$E@baTNF`rthJA{CZYIg1H1MzC5m)5s)c+9SS_gTmvhTnX`U zD%rQU5eB^y(x+VFdwJm3&zMvjW z)4;7{JeiRF!C0BE{OAgn1OlyH;}xZkjFc4Gwyb}uNR8yVlcc%eY)!3gG#T@v@};-_ z;?hf@uO^&H$}*K+FGJP zy2Dt@h>L38ifG+Sgs+7w;CyWD#zwt?62yoz$(yCt^%6gHpJ+#XC4QxKn z3B4wE6`=WW(86B9sG?1;NRpR1ek&&XW(8oD#cA)3>~*}fKIye)oDU*c>qHK?$4l3P z^XsLESnmua5~N@(cf)Ed>!n0>uD}TV`m){;I|D=Csqw+A7VM;5dvB_0+w1UX!PUYH zCF^Lq?r+K{A!})8ZDa?eHoNI%^i)<>s<@$2;6kk+8o+Sflz1hFm?yOibGz>`0s$Bi8DE*t^e0NjKhMV>Qi64V@!hXJrAojf zOFrVAwzpVFXh+1k^&bs?9$N>H9x>vAA(9`|2$l(Y_)JATh`nETzCK^}fYo|j-Jh2R z0zBVGDjyf^B!9jaFlPim?#cjerxQIs@AnHU_Srfkn5fYFc>KX4Ru52@{##?H)Evc^ zc+;PtYElI(+z-F;EjoLvC_!KPKM{2;b7av#z57BH)l5bhyJdXew((K(?niSolI`Mx!5DGdnn=W2n@H+%hUYpnql#E;@aIl|j00Jd*D zKS7rLNi}7y_%mS-ls~APxZz7x8tbcqPtO|l`glDT!ech~#z^2oAzfuHnIX5|{B+~( zNagJ!SIwaF&vaMhh;fIwd^je-1M8beh7t%D4XH7PXY@Q~4JfOEdv1%;O_Hu9lw~b6q zk5k8rulsrRdDGdqLQ(MMagz}2@SWK5%s;5BCZV)8jYnE4tjMcMCYiD;W#R@F#w!rs z=TV(XH-kq?C|G8^rgX+qB3?C3XhRw5WGkwd)hhO3yHDY&DS{g@hv4dSc8r3OOL(4t zjBM9`5dzgzkRpfL)fNw*Xm9rjA@>1z+AkTN@^NWvpwqs>L$3qz-XZDJ>CqDTeTk4k zzdL(SFAV0>d3e*;sUVEDmm?z|M+O{W@F{*RVR%^kG|@=&wnB4kb8j?AXZJmoS)TDJ zyPLNA=H0bH&lVFe6C6z&pqnUOu}g9=QG)wxt2^S$MU->|-Ccvbh{`|j>p9*^QtsDx zs0L$6!w^g;MydHUYcYcJvi<01LMBb)WpJ6UhhBs)Rkygure{k?g?esa z!WtCgt+hnip`9YNrJ?j$DwlU}qGY`5={LgPUI`ZjvB8!HUXaU8@6O-4k=lbX6zFLH z+e~}{w-rz>WyoEP}zCxB02wb>(r+LV#Rzi)#ZPFR`-b=H! ztrgvj~(*eN$?3C~Hz*aLAq(gAhN zoYV+|{wNJc|@jbljV4w4c8K<8xH9&$94cN>ZjN` zNGMa^kayWQ_$8(&-Q6o@n;$vayt+J$P=K;v^^ygR#mAOk-$4dMTV_cHjzx|k_hV=J zQ(&5Tz|wx}%P^iK?(PGYKqOntkAvSOxf%l~N>1gNM{%xE+ByEho76{e76)@vsi?wG znpBOWn4Pn}p8Ex-6(5)rJ<5_go>V`nr02>p5OcjQe~o>oUVF5XLVcF?{P|mA`U=Xh zB1HF4*Cwjl9N&NE9tq70wlXu(=8dpV7pCx<`29AD7k%gOtLb?<)T|b%=0>2SX0pXO zTVeyp-Xm+G!xBVEv|#r!4v7E4P!sw^HtNP{akb78D_a|ZcA{gEv-t-?sQRbYN#Ejw z#}J&-af5Z1?9@}SgH0S>oq$uo%=b&FUmHrqRCUIetA;$-7k@4$MweG^OtP{KFCUI$ zJZ(JMS|L2*W7ULkmAoFPF7Rw($w)kUCL&n`{0KXVY5nvVO>;B!6Kk&}lGx8iC)F z?^zI}4CkXX8%~*6`Hd%=kdDRg@GJ+#7A5gT7qvbS_ZCboMr6noRbj%BD0OValY56P zHITlM_fx%H)-!k40pK|;JQnN>L^vfC4UdG%VVFzBN$&@me30q{5a+1n!%*bx&@1=E zEn!&OBa#8#zG0fdo!6Q$81;@XogCcG{mDxcBvd#qpT-)R!|O(0%X3FSfmZZeB!*1a z#q$*=-@HZ5oMPrw!2lP$ghF1_yG&DnNdCm&;N}atwovIh%LvIHUQpVw&JPlO3%56U zTn8d(1%C?O1VW7}{cGL;ykycK%p2<3#-oeOmp&Y-rokRX2YkA#o559}r`^UJRu}lN|fDwfrwxo}DNuSQK;eS0M-jkJ~lHFTE zh|R}Q;eU9x8mhP^-OM^+kv4S@R=OuGLx+ZKP&Ham?@Aja*Z^dMC%5!R9aI#3?sD=I z*{Q^_X0VD!2vtHX5XIX)VTUp@U*mnyq;2fo>muenDKwB28vJMkLKsJ+Aed8qrv{P* zVtH}woR&B!XaosW^6y%M*Y7yT>4P%vU{wv_POe98kbL9hUbrY*?*%N(@j}q#CM#%P z?dIM2lB3f3Dbxbd3Aylg0e#fyQJW_VQcWo{$sp zi`bMR!F1)^ydztHF2su%hnGh5axfld#QE4HBzz=o!cMst>+(jemDKabm3<5T8B!(roOp?-xP3kdt?_%S zP#9tO_WroJ)F6n>{Q|yBEPR7}%e966FEYcwNcMl}3|uV#cGNTdKWC|D`cFOeO#jnV z&-5R;>NRBSaX65=?=^041)0b8P&H{4dq5A5wFMEy6r>XiZklVoM|Z9PBGoA!DFJ^a z5kh`@it{K;a|~!Tw7f-nEsI}IZl7LU@}Db$E->^|7;K;RBrUyk0@NW8ABqT7gM8@Q^L*MBtRlTibI3%a62#Tyyr;#K58+19g?WE|1D~DL4PLInC?0^Bqd+A- zo5E@@=S95GjINX#WD@Mf9-x;epmlHsQVh)z6rl}||E$8h#CADUp$B0u#!-^!)kEOR z?IrIDp%M;2#HEg)#0s`56}*q7kQ}KlKi_9v$b~RcqR1t!p9c#M^D`)-N}528(4v`w z{Xn-OWVWGdI6Spnsq5L%hemu`TE6Cdn-w_9bHyvKy@)c?xIcYBs`%rt%pvCrrxfc| zKN47BvG8y#kANs7{~(ST&jTSL96ozSr%2Q?;~jZ&f~5;J1TY2GkAoX@Xe6Cv5XQ6q zB*&^`ULu(j)k^o!>VM^APqBD*DS1`9Xr7}s?sdqggB4HUQ#!4sQl zBE?PYl$H7Fgk#&8h&3=c6_8Lq4V2-6J(e`O*J>B%Bo0+^9OX#iRw43Uj6FN#OOHxP zgp{OX6)Rfi1Zq7>Jty3&aYD{<@aXLQ&E8=1!bKH38J?69PawY%KB*WSUz2`Jlr?tv zphbf=kKcf(O_xfZ2!ckYqt^RPapOD-U3w}46=V!MEVy>i3vir2CgS|s8i%G;p8B** z?mddn?8dY(L?e_9O3IM;*(Z?)N-=QLXjh?nFJENZP<^^S>vgGd>OT;tl%Tixzci^w z=XqtTZ+e~>!|xJfWi<=tDl9@h*kUdGwj0jIyHF^^xp)lts)~-1ZL9kn4%w@l$oR;H zL|x%%M4M)rp4asd(LLrG>Rx>u%Nj0dDEJoH>#k{uBr8>}a^QPz>R*C7?tk12?niG| zln7*tjQ;xJ6Ljvys7J#<_XBzG#52m2s|%aS zF5I=El4hj^^RHa7C&ZX$n63PKR&B#WHOGWAYl2~dmb6fc$#iOv4rxmRDS2mu2bLSa zGi~smxs{k|w_N%Xi`*$;i6$~x|Ju^jG;_D@?)z6wYOnfH`KRTOk6gtNQBDcEJAOet zq>s(Pm!BZ3(`n@*WRUXi;|NZEKxKjL z(H_l&)V=3l(_c4J=_a=i;CB*B277v*t3tit=PYk2sSuQ@?RC`?t!=#;N3rygG!9H? z(5n;j3jTD(8XB#J93Kgy_2lBkyYZQ}~eI>66!QuSYsl(@QkOY<@%q0MSG z>SKb+BBlKuutAKOw+94}2YA>pS4Xs#=~0gd5!uc0ZVy2MK}DohQ)7Sv3>2CqBnw! z%vT#-n|VOb*ZED$b&4OsNB?*C7r^0T$cdX#3G=1(M@rL3Ul0i3ePXZV8sVyqHl`IO zcK6O%zutY2B51fD3ZV>1>Fkd>jV|`>=P%9i2k196YM%ha-Pl0;PLwo;Q?65(;N(;z zVsIMKr}DWGkT?2|>l%SLwU36_?&l--w}IHkKL!N)-wdHM9bU$C4>^pvvrXvbT&Jp@ zH4vELk&$(Y;`yb~jUXFc38sEYMg7fF7hOdtV z*8=$4+u>U=xi9Ns8?~&A7WOL+&u%n^(fDC?a)l)78_h*KgC`-q7?r&10z!X)01}vgs2RJa<|q z_Dq9UMctUiGtO%iUWYzUGbSG8BZi0Tu3f^2;;ed&_G>X<*>rJqJCHB*M=XtVOQO-$VX4Sd7SN08oKSFD4ELF7b^rdE zsI|{LZn6Tz>5#1y4zSwpF_dUStUdj8=Pvdl-wr$(C zZQHhO+qP}nJZ&3qe&3n7Gxwdj$^G~4q>}wqJ-aG9S(Q{)t*j^S9+(IuD5JQhV(Snj zw&G>ka|Sm524?3G_)k@F#PIW19;!oPx5TjN%&(p?)9wRs`laTih$H(s+J%zFYn6R{ z2)(-4xytpR**a6E5Cmydp?N5`q{KdeG519IJa_xS{8MiRtxCDTSTw4urr-!f>9ailql#*|IG)!Ud%B%{&> z>FYm5bg61f&Ou|R3U0--4mwT=Us_Gv1~ZFYanYj&n1?q3TauFP%KX}VN^>kFj-HN9 z1y!T;>8UwnV{ecPomf^16=l++q(?oT_MmkcYeijj%G9`6hN*%e=$W?(fs#Asv7+*I zsnO*#7tFd(VeXkZfyTD8UP>u4XnX9@#Og!8;~`DoS;nZviYJj6Q^p+0;}X*+{Ss3% zOVh}}`EQ0w`^5KR1z`%9dI(WS=0LH%{P;o62-nY?WKrLJ*9M#23NAvgAv)25l|1J! zokFfe^GSMeh-GzHrHQTEOZDCPe6=ZhgX-VYUw$5qteBMPrvM+J^m8~dDZvl@im)9F8l6f*LF~Fhh`#(@zZol^yT#Yu|-fk zmZ=A#(lC($I{L>&m_}|&MT(gp+1K>Wy;rt#?gP8e&GOQkmnp4RDvPe3nHob6GqQ&c zx+h+iY@~-8LFh>GF3P%@J&JB)Xa&!i}phJPfLzYfwfDw#L^1{&K-0u9nTHLm;H8D8ZO zpL3>Yp{l2_g*(gfC=ZdzN|iGI+l`#Pu75&KC#in{9*%}braLyZyI(?Nn@@Y4vZR3e zxGm7c0UXOX0TibS?pWARetQFxehQ7?iFI7^cv|}$&c$@t!w;(ZQScF2FJfp2;42MW zU0S>q5u2Yr1Ba;;M#R8Q&KDT9#YO2<5`;H&>mUw|d}o0A0L-Ga3W|vga?Ld8H5;!3 zw$MaeS41oqK*<*M7<^H+J2t$8<_Afp7YCpeIzfR-^>k$Rvt$~5lOMBG)&>Kx&6b9P1q7f zFAcp+M$G>U)%{LFkRi~spdeYvN=u{oI!dgGgxhaZ9{#W zPF)ISs*qqXF`;m+>1gNA?8?-P_ELAESDO~ml4Sba#PBImlh)9XgwbPuQpnZtOo<*Q zswoh!?|%AY!%3fcQ`gk=K*@@7=NyY5D35@dXmM(A=822Qe+(^K_poHC^OZ)WFml#_ z;;)#vWL(~2;F6m;)ZaZ=H9KC7F$IaQ+IASpeln9ZXeksbw(%K- zoilEO{dsOG4RdL=BlSHq&GJZ~Gq*EV3mLuo<)>o`_O8*1S4E=H^pnhTixa_U2Px5X z8xe0gww!5>R*ALnbMgAUqZd1wYm4LA%Y>p&8r3-w(@-)%(8y&+Qzo01iCnSFS%CCJ zHS+SLWkc77eA@KhZ!{8b?4%}I#i&`aL{u$Lo=sx6$|$j?nQU6V657m; z915vV#tRWz05FD?g0|W9Ee}93nk4Xe%v76Um+KnrPfxQ<+-A=~8H>l~QH7HXk_U$k zmEzP|U_K7CnhDF4VoVP+mi=IZxPw6zw5aW$){a(^l!X{rUd(E$ViAPN_ImW@iC+`i zOq3_BP}}1lwxUJ`#7989THzzb<;FQm$B>p_Wg2_-cxw{Q_Qscf2zCiX6o&$1ejU=Q&-9G{~X`J@PEs9F#LDEgW>g^uaJ zIvH%M`+A`+H}krhxVSv4KEK!ufN)O6fsNB9{3hWCSSDz%SG z%2TQ%`gM!(o)2OmY#t$h0Y(i{tyF(OJ;FS+m;#4CDjKXI0HgG9?^S;pwO4kZvmaDPc?qYnWyTb*NM+;6@YZWWJ+2cK%p6+p$#FCI?G$C8NNas_75T*DJi9n7wiC>AE+=v;NK2f+b&Qp=c&ou*%8Ds%ZW6g{ z;>xCd{TL;lGp<Q#uZ^@m>_HD7PXPH?Q8~dajfobT*xNH173mmJ>D?mvq(NyUC~7v8 zTdB6J)j$qmdyeOq6+gjv9ltKQ^2nomJ6#nkz?@Gahwup@5eqgL#o zvwnKC!7eZh8(p1x%gLCknDRZ^#g6!B`sk5XImMq-xk_jF!^0(Q+=Q_VsYnt8F9bHc zzkv^_5(VVYx#n-Uf5;h=8O)(R&W@q=s~{l+5-_pNu)^^1x9bFusTca{=Go?OhU^5r zF{N?wxjUwbLt8|RBqcf1am1y$m*mJcQ?*c1(x>9q@P(njlO=l;VrMyO6n|lh6*p%<@B_9tjzh1h;T;t{hkuP~(hfjC8h&rEPYM-1U`b?Ji_STajT8O1Mt@85}ovQ(<6{E~MTeynJz7EEGR$0WIAt_M8`alxu9 zliJ`-h(+XL<4aj4O%XQ>*?^YlPg&Z9zxr#&H^jGkMB-n^LNa_AMt3CmZ!9_Iz>Y*pOxRrnsoL6Y)vACmxX~;{icMFefEkR|coHGc9-;bNI#|YYK+m zwC9y!$P71X0SprX^I6oVyGNU4{G#st`;~iUMaQipTb;8(-SI3JcC-D3ZigX9TIZk! z99jT)+`M@M3{Q+Bb4X@3>(YV48Y_!@5H-S$k;rnT{3a38N< zuIbCUpQtnU>ZYdRi*BXL9b&KB-|UW`1s=2x?A2Z-H}IaHAG^G6Mn^vHpX-*yqwN{lPt&5Q&0hz}NL+dhx4-a7RV{W#)-(8F)SlSf z1*lXKTSyHIrog@^ec2(k1!5Df_s%Y@oYf#{r6-9dAs}-pve^xOtiT@a-t2XN(Qs~j>=uQx=JiBwM?W&1LOrn2C*U3%A6KgGcswjK_VQ*5!Knu0v*ppn zAs`e2&Tu(W7t0#e+T?}|<}>+4^%ZiWX4`O5{ReqtAfEl5Te+SA-~m6kPrV-T?)xp8 zu{p|&y(9%C*?z8%T#kGwAQ)2!Zd3@NGkJ~mS{<6Ut!tIU=D?Bau=!yu%unK_3Aebj zk|y(6$YisU1pBFV zl@0bsBP+|O&XY)3rEMl-d{#=345iQ^<%*09Rv?+_!kB|g=TL@pev9S^!dd9kMRG!i zAQ3}K_&}@T>%VEr>&H^uBE}3PO~Nzatz9SpXEyfrDWzfhI9>vSgBSQb18cF*Rh#c^ zzTcd|4e6HP^NzXUrqnm20P9_~>!ByP6{ zA#}>1UQ2Fm#3UQB44c<}Eq-5G zB}rD)HY&cFxLzFGd|wyF&h~VV6dgmbQX;}#Z7?<&&R%=^NHv6E@wOvU_hjIH>^@k= zbWwHt9?X{NvPbGnoEh_an2XX@f-FATw60ra4PdL!n}QTBnk;L&@6Ai^fXB#zQT=R8 zqwYjnslW!-Pp346F;xm)u7zK})sB4ILrX z5hY_p2kvNx1h3k<%BO991<9&F_{xhzJKi1+yYw%;?(iQ+%+0n?X{aeYnO%9+m>kVC z?%w(KZ#^cv8O1s$-mQO--^(0WSdN~@G-K5Bir3EHn)d~6=bxgBJbLI z8_G?#8;v7DhBC`FybC30JR`5xWXbP%T7_+mNfFRrzUS%^aJok^e%}?wA7%&==iq%@ z_XNgt`8;-=VD^+_-)5sxM#D#?1w}b!H?kBQsySK*HTxPC--YWb-vk|yq(g}~O08FN z)p8YH5>ppNXo;(Mp%J_tHPZIJJ~kqqAWrZUXIc?ok%+WVbWa3M+$1R$g-wyH7s;_F zS54s-xQcJdw0`0M%|~zCc|!K`fl<( zu6=U4ex5)dPvOX7{5;N2za-miyzd+uE#9qCzL*}{MA1gDOx1YX?-J9l>~{XCw~%)d z*B_iERL2@;7@~)*@k7ACDAniNj!*cIcVH~Q`E~xQgJs=XO;kG$%`Q!SrQC3Fo9nu{ z3;dTHU4BgG@=k_1Q=J;&+!a-oG3vD1N?I}jg)Y+eX(YkeL+X}75w6#3f3>SKrxEf78}A@v>Mg_>WfMC%z^y-k-< z%QBSeL7Ah7kgX*bpS;ptZXl}i>(;H`pUkAU%ln=wRGcm$?8y^wnbGwveN%peMT>u* zqLxkQGWl-6riUX8C@FKpj_JLku2iPJH`=c&)-!Xkwk$ACct^A?x2%1s6t3wbYhm4%Tx>(6G@q}J8EIlTOEj4#tMQ?D$^nFqwS2K&JCI35iN1%YLufe+~z~n7pmI z5_H<$#Qyy(-TPd0qZR3)q8rv{+Q1T-v`{ORjMYrR4-<)$ud|${ith1f>ujqs->_Dj z3PU{f+cd2oW<|#=jdka9_q}~P6@T3PVy9M~%nZ-uw$G2^mS?r)X;XlONobM4%OkQ{A#1JhUMFEwUwVHv0=QCA*iwQ&ajh`4*~v%*n;X~!;>JN)WLGz+p1 zxlA<^1>NDpjudgH)(5WMgR&@4oTthX?tWpHMWD~TMg5+!sX||Gt3)NTF%f{)0^jxr zXU8f|7rF70&d*+Bo|-aG>`upekC+%gF>tj1L!_mSH!q7~k^$bhm%*M`9y7%>`=>T$ zs;=$6l9)yoM(r?|t~(DX_S~5LhbTg(n$n$RMuk=Esc7o0WFAI6!TBy``Tk%WDbco6 z>dwj|jLOU0;aYPR{iI4~?Q=4)S8hk)`@ZvqmBb_kJV1K2`@7PZf4IZ(BRz5ZCA;n0 zbGns;=R@@8r7a`I>F@iw_bg1Yv^zWJ4cB8Nq-)bvno%un&MKE8$K2bJV@xf;;p+W@ zs9(lK=QHF%*NYa*FdDN2_uYapv{wI8 z7N6sJR+bDf2IL5;qf<+U3l+qe;kLEzmO~d+!ZJ`Nq>Dq?Vq1k*f%uwnD&@USOPi$+ zD`I1m63Zpj{|tH@kxl%VT?;IFKEP2GnB}Jlcx@>=hAL$>Yda4ckqSD8+HNMb;SpaP z4l>AnU8{k;VVD$=VLTNH$2AVcKBR6CF)uYBRNo#X! zxc^eAj1ucv16_;$c*Z|@5@blgeUAmn+8Nv@_ku&5l!Z+&ey~%i;|E zWRGJ9#=M5hI;stN(!!|{@lNNCw)U~|BzSIAe$_&>KlvrK&l8-eg|XIV``-lisev%fp9 z-W;2WcZsX?&9*0(;AnT=#4PYty~dk?nG%VD?&ZL zOVdOwn-Smx%gs2 zg?|>}r54qIsX)Y3(1mjl_JsL-KI$XC)D82yxHdrQZd3Z2M`6%DT%xnl40 z&$8yAb7_VNymPbaiXair@!Apgr}7SKH(R_|P28_<52GIT5hhvaq?zFgWxwV)pZof+ zg`Mj1o;Ql+;EW&hUa*;F)of*{=tqhrX%TMS=Jnut^`aTlk{J^ApKs^~u{`|fC1o?R z2oLe=1s+uWRMSQ%(X`?f)hBH2aQ3$l$~wu@N~BC7hURh2K^FH?M!!!;BX_rqSZG-obso$a4OwC{uyP3Jgg)n%Kv@S-ZjtQJ z0XSN2e7sWE*g6|&iHfdjoEvgv7w5I{&e*HfL zD$FeZ#kYXr|1VGxv9)m$GIlg{Ft>BEb@(Tg(YH3H72yBZLn&)#Y@?`e;|TMw+8d^h z04A7!QvPx9rDkSg2T(Jx(gElh=okU?bWE(;wBkzDJqS{Ew2TclOGGApb`0y3WQ=|E_dPM=@XHkaz5j#x7qLAy zq3RHHweJDjmFLL@OuNZQTU1)?*kx|%*k$gaPQn*sMC~^g!$)b-;5WI4#Ivj&ZTtJ_ z*kvvkQKN5w>+f~m{HJwZUO=O7)M>&c!C^be$0-uCaksY>Voz>v3#_y2OluZC))RK; z7S5}8s7sTY@GdK7R};wpfI>$4|FoU>$0*}pc>7;=&`J)@#{cvR=sOz!GxFbIXYSzW zBxt7Z@Gmq;>HqsXBRveQs=1MqnWH8P9UXw3kq*GX%m83wWd*R%F#%ZU83ByUi~we4 z761!9!#|CgjuF5@#{ghuWdkrXvHg2K3k);EzvVO0u>qJ_=mCt3YycK|HURU#zNcpg zFf*|NSQ%LX>~swO%*VvS3SeMn2Qad*{%c+s?SI4Tf9@f`KYL68hE~Z|*~a`|wQ2zW zfcAff@qbrSG5(*_RR8NTh@OFs0r2n9zkY;~j_p4}MgTL@|8`N7)#m7AY@5oAz3C!j zwb^2QI@B^PF~o?uyJb1e{eopk0E2AI z41we*s+d<)7>EN3OUy{iH@~s7w5>cn#Gk{DX7(5171m)uUam~Da~=2x16DL2+R47z z#cA)$H9HM{@U~}-?s3zYXs0OBi;n3r9c z$UWB|jR<(gXlZO@0ae>@=TvV8k(!$JKlw2;%UA=p22NOUS&@;6!3VOS&-f|7ZD9%xLj@ww1D!j*5&PMAja5&Gp-JiUogSDPo`A?}ad31zFg9p-l~n%}r0>w6;{e)D z`?gRJV@r!<-}qUKro`Ri%^a@Q_A~JfeKC3G-EMKVdvv<}(ya?_4t(QX0CIFCBZY^w zG@Tu%z|FXFSwJ1fKfw`o5UjyNPCXb*g*#zHtyn@dWJo!hh*Dm6a7@^riX-N86;jX-`dpyfBuv%`KI66f_?{6WUBp`22TA@lOO8sL+!oAvVUX0<~9PQaeWid z0G<0DNP)f*y!&!GIW@9?eoIgI9?R#Z2fo>&{@|;9*Rp>ReEJqY{pj-j%p-q@{_RbC z>*YvtYHj>T&0PA~C*7;ft^soI;|rBFo>`LvX5ZbOEB^?9ljma0J%V+t+*J6?dkz zWYmPrtGw97d8b7<)BEP7og~@UJ2A7l(u0xbiL_a zMPKpt1Ow^zGqJM~&_8=IaXIdA&%K$L)>fKo;=tB4@_gZ{0T}v9*!gb$o?~q`*i9}U zUPA<#Gy$FrkVeq58u%3vDM;d^Pg?b1Ue^x`a9JdiCorE`tV}(tKx%L>`7-~GE|UU3 z+~4o3e`&vJ$E$hNtFjnr%3A^kTm-P%!I_K+mPD2xSi=9Z5PVexBJg`=?HpSXlKF zwc2zc#1DGWGQGP2T>)rt8g|Buw4Q*4b04Ik}5tp2t& z4`)C})8EyxnFZQWgjGGWMwO)}^%o)>>KgYVENGoDn+Dk0jylgnUae?vxp#?OF+h;D zDf}2pyIGvZT!DyKBcZ+Y9Cs-B@Y$61f~}TAl&;hL(c}w7ERkI5`66I8p&zWbzCI0~ zk)LDEoDPEClQ2@mc|qJs-}^g)xi~u>95YU67FFBTJ!kR<%B15Ln?rHfRg{BRlQCkF zGUt+Pi6pDiuN958D^9hPEFdKeW;}AnpOXAhEz(%AOPAoVrmql007K293igw8oT;K}G?Lz-x#s4Xbw&kxo5B`!qRrE^a8 zdFHJY)U9gjJI)4|i;j_={eVhtYXc_0i) zRLf`98!@2s{=StLgR)(K4$YQcG+DK>7;>61SOyYeXi}riJ{n>GFC?YkW8~~MA$y#1 zdn;YMMPf&~Ltdp?mPhLYOUnb}ol#f_>C5g7>!a3{F z?~`8CvAxkY*qqVP2$c;BR z>QwZrjUlPpbVMV{kUukva-i9aH(Je#QWJ;~(aimUVl*;$I@hz9FjDAicrAE|&X8_H?qq%sHEf%UUwmc~;%ZWihQ zJkz$|wQq$4g(U$97-DPkIxx41cod&~)VfB04BSm~JqbFEgVt!8Y0TWmd9Org9@C-> zx>hpX>NiPBM{A!5esRqy+;oYL%Z~h$E;zHX~?&9SDb;I%3Xl`uRV9)Zr@g?&#mwv3+8m2}ZAZ{~A2!Kot* zq^R_*&N{%dC?a80C_wQ=mi3BUw; zD8@z#kMv9Eyy`1aGCR9j%4=X+)|g;W=8^SSF;iNRikNs*=C6%rr$~$QS0LA^%WPr= zQ-I%H`3$dc<%iAQ-5LVWEtSzt2*yTNp6CUw;+%70Cy=_soI0`R@QQ{Qqb=sy?@Z*R zcsswDid*GXg)W$AQObffF}A(fOaB&AXJAAsClCyfg+?UVAl~yPSG|OPecs>`DKSkS z6WDJcnc(=Rn15WYXHc2KX4(?#=l9x^)hh%)IJ%s38qTBFC!^vAjVQkc?JObz3Vg#YG6I{5M zFLMB$^2!g38+AzxkJ7q#qi6W9+y(kO(ka_mni4P$f>x%7H5h@Hik`h`SwpC77N+fu zAHl!aWsAzgsH-9Ijv_av!j9qD7Tcq2+gM^n%?>I>*ZGoKaZpBrp-fMJEC`i(l6Ng| ziS>HdC!BF!^Z#`*SQ!05INT08kJb)_{VW*!L$WL4}FFrmz-8hsTtmI<* zyW=*4T9LLpKsodGBzb79j&2hZb}g-lHx2NS-leAM@w22EE8%PvUAYp^^}@WJd(U42~g? zBmqIc8ON;;L2^m9K%Owtq62#kK)1J;7<}%)?)|OY@#n-$)ip=7ATZb!+x!R%>ns!7 zZ|llrB>uOR02CQ23a27@IoSP6Dd}(}W%yn}v1v|%AWj+~()DxDX7~YhtB5#P;#Ak_ zShm#Xkb-<7C&7vz)nZm81rP18qh}{1+=+^{;DBEJ=?J(Wn#IgU{y25bmywRyy0YT( zD;={^vI(e6All0YqGHlF3)gJ&^6#l(b<;3r_)6OyOgl|&9P5<)3#z0)(^fDf)jeI~ zJhkvOwTG*F5d&6n8b8bRpPs$8)?f43DBOYR)$3(l$;L^4s-&%GKRkm3VHv6EP@AJ3 z>i0nm|AI3W@t?&e6xae7|QjQ6wOt6mVnXAqxi0rgsDXVHBdtMFHVWw@P z&?FWd$1u_4dwcm8gqI-7oay%nkTnzbQWolxCE^0y=Hd!s0V4}F`-8-Pd5Af~j7DYv zMA^{CLT{SE&hT`$y`;%C1Kfg(VzS1cpJed!?jV)$*|JiZka%sLn8p)^Y7!c8)SfS9 zVx-!SHTGc&hU?D~U_IS!5r)G%1oMyJJh&Yi>kb8z>f-Ys{T?t0X`dU~1qMhu-{?%U zz6Kkl`F@`;X!fQXEVr1IUF27cWUU-M^xevr2n4_dS+l;AqgT+6EE;Z|t8 z9n6ep`-IpJ?$;U51nFox{U`)s1isBm9MCxOsZ-7^#eNG-l`{AMLCMY3XfNM*F4umM zL}6R?JA?`Z&qt1z&|tU*1g>%1yA{BlO61!kk#1&~iK=qc$(*8mb?iEEy5*Zgw!q}|^6F-ZU3`Yk)UH zemeUs5Z_ISjN+J#%258wFRh66*rU4Fu1ZnI_Xe3x7Y%iztD5v9$YUPdFx$n#|9#qd z!aVv=+@L#Fl*)Ioz6=QQi85y(9+HOj3V3;EHRIJa5UTPb=HS7HfNPU~-G7#g69c4~ zD|rE-)KUm1WzO<`&J_-bh>V#Z`<(+ z>o{GpzEpimd-1E4t0H=bbo&QQdf-LgdOadysk~CbL}}O5$+0@&uFNr0Io41pTpDRO z9!(ytFz+N1mZ^=wy5hk4jf1tbcT^x%okWH=b_6`lKA}XBc=D2k0cahoBuU*C=}Q9Q zOHN-y;3PU?BhrY-6r@EIT(JhxIR7%&U1y`Ym;K!P5*=rH z$7M*{RO%to-1^&GiZ9hl6iUkmPot3tf>ou83LlzTge&Ujnl69O`7UYD?D}Eth3(?X z1dL8GNshUk6M^RS@8P8Pzhafb`v$NKgG)0^#*e}`34`eK9p)smhdPL-6tbLT9CahR zorm68s#5ovWvE6F?3&W+)H5V;P~V<&WTWj>}1`z(>oexHCN`zz27H4z<1Q#ghr zv~GmH-E-cU7FTk6T3gO5kl$P9 z?XzG7?1zb!Vn7#)XHuCmDXb1+^Mjnmckl-7Ldba(hv0TxNj^l8{a`{i#Sp`@=wx#~ zYwx9wIuDU(MM@Eefzqw4p3?fb0ZUrHZg>_I%cS};=N;T5vw9{3S*5{G*P0@Q!R?Ye zb;Pj4es&dTFXz(+h@Bzc4Q?sgrFGy8q;>wRuf)4WUkPW+Q_A!TB=jFwKyRy&hDv+$ zGyS#H4FJJ=_#)$a9At^nq@Fj}xN~D;s{|+J#7{r%@1XeT>H9vUfk&qyl??z~NaTV(5DID$ajTCurG+G>cSGOATX0)GLoTGgQN{DGC$2L{NSi%9HZX%O|% zqJC``nJY-&+T1NpKcABv{w-Lc2e*4&2YI1wO1xc~YW7#(s_>uwqfVzx%Z;wS?FX4* z@XGd_=Fz8O7CQF2T_Wzbccq$N_-d;_2{>s*>;r_V=ke}&O$h5DMo?eVp%D_U z3Y6KCb?3Si8+X}x>~N=4ukA)*Ou=6Exx3=Tf?Jjn-kYSB3Hhn0Q8L1>6OFApU<=PJ zCY-BJ9$=tU!I#>qCenM7j{usxuNQ``$f5O6BEknK2bJ7TFKw21pVE#ttGw>AKPbW2+FAyLg8a`7+hsKit;cxx=dG0pSA|>Z89RA)WJ^X)5ztmyVWB zV)DEegJ3kXhr+Xi)>d73CnudTah1F$_MvGx!F|}{KcbP7TXZi)x>Brfj^_K3;L=)Z z+gQSLVn)y)bVLd@7!urH`s%D8sHla(?Co6P`8z7S(9ElByhh35QRBzDLQ*rfJy_3m z_eaCAS@pdU-@|G4j@h*3qvBMOKZXv$A4k(HoG%ur4u6Fk{URXbT#7*bMeoEZGVw|B zcMt^Lv*pvT*oQq%tP>h-$C537z5waE06;JTSLL+VBcNXV>#QP=jwZ(wmf^CZ;AIQA_tH?;6)<5Y2yrPNbARnT~OHqA+?8q1VcT zq@@61r6{LJ-0zvzv>U`Xrt~^<-2Q{<3c`w%=NL&LIPg@--j`NmGls~3XM0Ry(pVp1 zpw$2F!XlwL+_Y(Snky(tgutRk@YD;_AniZ_va@~d_~Dnm`mL-Zze8bXkr}(oevf46 z&Mj^^JcLcyl4$UWWqG*xf_FgV0%f5W0bZ%L83!dqeaMYjij}s)z}#4+ARa(R{MS%G zY2yVq!)lvu&)8QzLIEbNR6u&ix7XDiTN)CrX_@gclAZbjA@CnwfBcQqWkeB-q9Ey_ z6EWFE`}ao?q8GpPnwNDeeS}D1LE@96rVijZjwF@yS3xUDVmUaz!Fj!vy_-j!2mY&) zwNBYy7{rSykgQ2>>-#m_=0}<>xufKePK^Eih!oxtdb&V8v9D^!@K_uuvR&W9R`qz9@=hg!Or`jKaLe;md~W*)sJPzDyU`s^f~RMXMaK`{X>e z!Efv#G2mvx2Fo^eM)TBpnhyrI@I~*gn(xPtmTm7{tVBUk4AwsZj?uSFB;{2n7;(V(SG!%ls`0LbuLoJ{%9AWT$~CIHeXrezvj@(%XM-)i3UyMlX?Ua%83c#lUDq<89#|BFWe zB@E73M8mnbTrjlsgL26=Pul{M)nS;H0NXVRu|tM26S~?=Gx-;WO1Cnd!I?aMu>%OpmZ*tSLAFvn zMO;j*O_&6ZLwKpc!I!?f`#_KDZ{XOvc$?hisr5bE-pUQcM|CVdY>A{pu04OSHBk#S z2U$YV)r4@nkU#kywxu_p1|8K!4?!yqKVJ%l5bAEB9|w&Up#rCNTSxr;kxnPb!FQjL zA&RPXcjG;0*6qQy8B}2`VW-+Vsj%Te+yqo4k*|nG1nVJv?KN(3 zgs8VDvF)B*Ffm>0l@G2ps}e>Jww-~vw>gPlvvjPC8o2G5g-5;|!RMD@pJ~ADNvU?U z(HKhzg$Nt5q##xl3p-#j)TiJl8B^@^x*^L|42wPK&0NIg9fVgc?^3_}8`(GBVC3s& zWT7dF^WB7Be0S5-v9NCGPy?6`4XsBZY4JpzulWk#`mLIaJ}}U*UA|QE;cI&*tCu6= z;e7gOJSSR2n1aoSYBZDDB>wn7%Bb`c{kEpZ55}Li*;=Vl$L1%f!_^GgYIVKsU4fgS z>OEp4#pZ$fJbD!lMt%~690Iu{2c+JxkekDyy1H`Y!gZ63ec*htsz(Ps)Kzkm#-(an zj&R$FUU%H@m-7*Uj)#7c$wW-RU$d@f!{v@p!xwHKx14P{L>?)bzJX_Dv4E%m?%PU;=cFM)+VRfHly$J50_lYLfR-*o zpu_kzF_*!^dN#2mMH7}tf4EVz6}4v$ZF}A7k1@)QCVxb?i6il@R&2TcXHY%gR9eF4 z-}b8CydInsCcE|`B&2GCN3OfHt-lH{EK4fB{(u=@1WShSvT4)52wQsO7Hh=&e_l|v zMlI>nls$Si`U~2fV$z@nIag79-_tNi-LWed6Zk;-cQ#I$&dsKY3_9goGnPO#$*do+ zmPu}^e4kgHm&6*V49t>H2HcA9v8Gqzz0ge6D@EGyDR)wKgVmizV@0$!Qb@&nDP0jI zTL@{IyC>4Rt-WZIOA!Bm0RKM#z-oT=e7m*N6Xbn{wCd36%=fpyb4!RdIDKyXGYMHKNkp3>88by{~WE(QL0mo7Y*UEThX8GMUox***mEno3F z4coy~Sf3=hDif+hv}_(IP+rkFNirObl4{|A+SDvCG|4lWWY7P}O{qf7i!Ni=-UJN* zNg-kRMbrPKj5^$!bLC@n^L=}Xr;rF7(hqiau5`?Rcn!w?@^L4E$2Uxmg06ul^kFh= zL|h9DxAs6e-v7Oz*+mPjvu%2%Nu?WM8S=sR!7>~S>VSWJPIEPC{}|Cwlxxe*luHMq zLr=*21)8I4KUtDmH9GP#bL-wy94}i{9p^X`QJ-b|wNy@%inBy_Hys07itfAqN%f@! zY=7~MH&a_;PS~b8p=t!jCTQGLudRnuy&hZGRO#g_)Y{QbT<_DM&zJ-|u0W2wF9o>k z+7hN(l+i-GUumI}i@OVlBVJje-~q}dyaQIoOM?j2U{5SFtBz%LFGoyAOmAbIdBGo7 zsgxR=9#$N2;kVjIMF2a&AxL3;GlgexPqp+_|k!PMt=Z?<3RcrLQE9IXtBPg@guo7f3WH zOz7%-*27S%#uBQ%A+1{rro$5gMJqnf*U==0$?*`*-hNaE+@$YUO34V{;ZCwjKgQyB z6*#mB%ltj`vM^`77_QPlM_-5$uGxS&$JvgM!?LM<&Yy|G5$KhxoUa7 zh(ONAom3?4nq7NFq>MP)#22&N<3&sA9IJTh#vjNR{E`Z>K)hxYJu+WsQjWAFv91U~ zTt9c*ZZ1q98#fJ);@3*pG>!lG9p1l(Vx8R8Q0=v`@@@E2rtMIw2E#={OT-(>No{F5 z@2XvNTR6wR@Vx}#+z>BO8H>;POX&zq6l+5dV=g1?D9o;tOU^%4gSpW;$O2DC(&BBzAEsGf3+!61O4g zE^9&3R(3|X>8D~Q=+8vX0fjYZjEefOr2;5y8@}HCI%T$p2?A5clSr|gyXS0|YXtmL zKGSL$^Yg`vwWdhyesY+1h>RM=;If(?Pp9TZ#v;<>ekGYLfNV7!`dh+%=HM3Z`I zZPe`O#po4Z$KhL%;^axkX&|S=hNcyzOQa?*mWj<`H5o*U*&SaGfsq|PF;Nq;PkF{W zB7&c#)@{kvZLj7DINiarp0?=~$8l~>Zj|~v`_SdxId8u?T8w{~Ql?um`zTp(oUrSp z>a&reO`EZ5>&&#p`-*2_SCQgfTe(8z!@1KD~JfP@tmIa~^Lhqn0ihfNIbb%!{qq6m;vK9sgI+*f?W-RCz9m=F)0 z$K5JApfTF1*`K)_h?-t2qv<7bxE?(8uT(%-g?S4_Ctnx?Bk4ql~w4 zJoj?3T^YRlvb5oH?3IfXNiNF&d<}GjClt-=dc)oj+e!^@WqKj8w)vEt9IDb(QYL=k zWi-8})^0!lcwv!^I7V2jOfr&sD>4%yRDINS1pfn?Wobt@wBq7fhp1RhChqXVj_eav zO6r{d{qAA5>^l8-3;Bs+(!&qj#Wl?~Snh@J>C(r#SM<7F51pO><_CgPO{a6Gq2~K< z;s~W|svna{Rf)4Wkrn;1NjTP4f`gyP8lwJipavBXoR*4-)1$2EW+ivpWr_@{zx~ck zi57S?_Fvc8f+&Umct4AyT*nP-69!mV9q9~4kK}$0dW-%kL5ORS<+8cfG7nV0e^GSL z8@{L;?Vke?^1!k(;_msC@PKkqs8*B;KF^e+09%c{9v=#yjdsr?XMWIJePi=ecjHepQsQ2=i&JKhe?sAfVzkFQG#4{yD|w zatY+OJ$l&5ThmvgqRoM=XuC2@T^h6l?*#X{GG}pv+jvfW`kYiRokm699_)8j?bx{- zygJ_%Ur-CE`er9{M(jja!S1jU`ZVg}mGs{|i4T&LJ2pDI6gA`)@@eM=voyMy+yx`B zVSunEqS{mUia#Q?cHVbqB0Y)d&75IOpu*Sj z^vzxyQfT+Y=apo`I88I0y`d1y8CFpTBf+ZPjcz{}-YA}zHlh?Sltl9&zq{xJGrog{ zbIn5<`C!)*I^}PHdr4_PEF^OBf;33w9uIka)la!3HtdHR@>!{FrvL55mgaG zCx|Br$=-}fXn9TAKxWz)*-mnLy$g(CXjF{>Fy4^snwT|cazm)mmZ+3pZDJjan16K1 z6$Vh99iQYB3UIw}P_+r2;Z4ej@!ST|6+5QeFY&3C` zZyjLdwA5>qg7AZeyWNM*s8bA6_NC!*p9yhzCu(wL#MXJZEh+Uv=wo>6$GEhacVfc~ zyEXw!hDQv!8_LTX8fKgk%~nG%4dii{rS9$?0(f4!ZJ)D{L|XDEvP}ETML?ZkJ-Kw# z*&JQMB@mKu{3aZ0tI4y6D(*?I$ z5S_E|_BEImR9CdP#RCxNX71{}y2OQCclpmCgO^=n2Uo>1npSZwicq&i2afL&MJ$2F zBttw_Dyt*-s^W?KQRV><{%l*<+lB{$Tp#(rV3Xq2DsI3xKdFq=IVQ^cEUy)7dSloe zqM`OppkbxQieKXF4nr((Otic9FtrJ$_f~JbrdbY^tm}F5k;An#-L(llCwp9%-nKO~gcL?YB@}r#JJ*w{7SV_>?HF!%A52FENAocp(J)e}s&wkN20a9Vl0I_G& zBrHtg*kSpN@+f8_7~Td%nZ?Bx(D=v4MP~j@C3yAqlcnBtuw7wvDko3>e!V!02@76D z3IYLZzVEPlMzQ%wv?RGxwTsHyOqWZDIfmi2Vq|A5EH*WiK?qKxBam|g>hUvMmDg&l zCwHx53u;}*_Xo-x_@S3()p_R;T>l8L-`%4JRW8CK!`zbqQ~y?!w;VlUUuIaPaNCpC z$t=?uFy~Z#qyft`r-JnT@%LM^HP z%Nn{UBK5u8x81WFf89OgNq{cz?!cf1o_!Sd!&DpheAgK!z9G+hpp7I^ZBIJax8%4D zem_&!T{rGE$PbpR2D4dv_TZ1j8uRtZ9)F&Mw$>1u6ZyFIQjN&w6w<(MDm7VzdZV&F zx;vh+^Qwh53gvIE9{~o88UipPO!{+s|-IJSe-*M`eq-`qcKWP8-dPKXQ|na#$U-sWc~*d>Alf7S|{f)~>#ekq0M z+>5fBcikF|_g#t|Hqf3|N8;Qvkw-pwTGH_>%q3$*q-ylhje@mgM%)MEz)8Cf!KOOQ z{|Rfa(8(8oJ1mgFhkt*_y;IQlpy8%YWFHdeLe+m zVfh58Vsd>s-fe67ypRndf5(l;-o8NZ9@J1d({fq{d2j0ll5ZlP!2<{|{BB5@9iBAS z<4W-UmF7n{A~H8K(rL-&CvzGI#EkJcPOl+&RF&g*Mr|=t#-x^^n41|wShERH0~+Dm z*Wiv98KTh{Ju+AcZ@kCHQ7AgE5b8Gye$m^)(DRm=onn=*9|cmOXth_0J{SR-JMy+Z z`9`U0SkAol?~#IKKfBwQsD&V+|NSpqSYV}FOsQ!6r%=TSmD zTS}xSI73w1F&$?PvVYIPBYQb-zG)7EGn0QaajZxS>T->>BEgZ2@VAexAl*ufiFWtY z+Ru}_D8nNk_t)&@J*Ja z&RLqhoD;p^IV6nZI%TN6R*#wCXY=yR76|dN(tYU+pT!}~FPGRKx$|$&t)S~uQ3W&F zAXne;`n>2F5iA@Sd-JPeS-Hqp`B1!{-@t9lh`I%iX>Uny6E82JP#9ITF$pPeL%cr^ zEq}pbBye4;?k@Kjl~Y##Qsel+)=dIMqR$zxQ=^)*qfOXpa-x`_yB4W0DP7w1lmLQ0 zw~Z*L4hiC-RV4XS#P7imU8dV8S}?l#eF2TqNRxxVMN$6sO$uVls$NhuxiOBF><_Af zaCMP)y+iNS=j2Q%XF98usdAfsCF>_@NF{X$|5PL*D)^7dxS9)OFi2Vjayb28!UfS8 zGJT{ySrq&~(0F%4^Q*!dpbQ>|7Ew0pKCdegnBpOGp=&Q{tX}g%xwQ{5JJs)@LM-Qd z1hB`im`Tt@NLD;1F9Rp8VRo+#-q7DBYvvK4g+@1DV09H6A*X%plD8YyX%OyDo31wd zDxj8t3?Cl=O0#6Wae^}03H|45OBhs@(w$XYXWeo1?<8LGBb^ZJp-mo3!+&b9uqYov zjxsnpKI1(eAwe0foC%)5m+9(_(9 zW4+F@?O3a1Y(UEh!*!&Avq3R-=)m{Zk?J=;+FF*aA0f10xaj0To4OurV^e13n8T zLLYjT7uw%#<>fug!_?#K0LFsthuTS-sVqkem$Y=+)2on%VruCXUZW$opQw|TdfLm^ za+c7t<&{?`ke3-|yw1>ONF&ugiA&(A%dk_wKaAqdU20z#?SmiGE@ztfn0IbPD(+8! z^XG~r>`4|E(+GIE_UnuU@aowW4zq(L-wo{wK3tQ+(vboH=iO?eA21Tf3*6 z=nmN`-1pCgF_8y4;;mng=dK+QrRTf{AR=EU72hG{g`JEsVEYG1xzj%G-9W(f(x=2XhxVCQIS1ZXSC87ss?I+ZnAzhauiXS=D7=NtCFQ=alG@@F#BLno5@ z#Z~kuA!rz9vWb&Y8r@#mBQ~SY2y~8>?=Iq@60(%C9sU};+9`aiafEWoC2r-5P4*)g zYAYDP0K-1XL4ufa^@MoSMwpT3x%UpQJ9xu-Fxlv3itK>fSiRg>{PX35r{i1%n?T@! zES8+H<&W+O;j4>DLzwPG<>R;&cIVrSLoKcfgd4v#u;s;+P;$-wZg00mN;vov{o_Ta zX<=7RYcSoQcjCLL%#G_0itU7b_1v!dGMci1AJ9BybtF~%eWY@DmsPw&#<&+CT94ry zX|seJ$jvZ|%FkNPOE!VZxxowLN`%TO)3TYV89vnE)aSuI3~i|#S`Pjz7_&&wA?}A` z`t4IWoG(+{P2i+ZueDBNb& zrP|}c5h8vn5GniZ-phL&E|b0Oi?;NAXB&MJfTWvD17zuikO6EaL3P4k&JC`Zw{e+! z)%HR#46X&xBh3|D_1ELDQoLuBAD!t$CDt;#qy5kF#e59_+kCBciIoN=V%)3|; zrEA7}MN2V^#{9V)9RiEqiab}gs$=3Yx2`b zJF=wDkz4#i_m`bVzdz&bj{+icdzs75;hyQYxHO`f0sX+K4@WnHeET4`U<*_Fru3p* zL}3*PAN)X0Co##$oCH$WXE86}gY4K+icew@ZG;!ON4qhgb zQmYaG6Va96e&J~dPce|jiXVvW7U4hdQ-^UxPi5aofzi;UtR}v#eguMPIm|-&Hu@>iMNi7ORZSg87kz3c#0&(6@NqDaOg8+$ z`f@lsU9bgvJ}Lnd9S$2y8VM;?bc&OY z%z$H*<$FmS zx1n|j8L%0!d%4G}`@CC^1-*Kf`6?h=ZCNki6*c`mWt}D$Q1WfvStfToA0}IBL6knk zeZiN~oS4jg4KRtC3v7NM8I+77vWGs|TsaGwh30tOHx(U)eDoqkAOlZ{5m+-)c4)n8 zzfKcSo!+L!{ zF6GPexm)`q6~ez(YEno$kwv^*gfg(S0~M+wbjJO=`F7Fz5?koI;^jNP&B*{`PHN?P z;CsO|j3}}lTem+x9Tav${G+Le0t|y`kDL1Ha6tmRa>u}$g52ECLm2_vjoF0tx67p| z5@919@Jk}a{&{=sKevveRF^az<+C&!S#S;1?;1=YrL$ti!@nI+CsD@eSd*sQ8N7B# zTM-=XpQe8t4JQH95OZ6ypdBE*HLs|0t_(I zI8L?rebk6!Y$CPw=+?eYoFD+D&Z)4|@Rn7mHYh{Z*FSV@=Ltz(x2Wd5qM`W{ zU`CpYpD;WHhmSNNyBk?E%8Puf$gEhzHz2O4|L6yMUScUA$daIFd}*<@>dqi}I9!r^ z#}~s#-kz^rQC6sPRyfo{$`|cVoryJtszCM}Sz*M1$6JKI4h~CVSTR?~TzrAL7Pa$3e%f>^FaPXN{4*5qb#zg7B$xUpW!d!%)Av@o_?#xYX&rjXZW!x)i z5*MH~wll6u2R~^%%A8q7^W{Lm<77&S(p~x9kjjHAJ~^P@z$4^@Y&AJ9T$zM@%GS9H zXnV=dsvbCZR6vgQ1jY|`w%SXtPyF@gJj#j5)86QU%#m$)hJL!^YV;sjN8mS!sMqfs zbOM68W}ncd9*g8d;{dT?c#j0#RwJEyj#idUXN=mO^f3g^O;fEGU2{h56gsY@vk)!h zMa59KEfiH8PQM4*VJ1z{ILjr%HlrUBSiLfqpoAYTkl!YTbovOfJD(&! z(6j^)tjP{B3TWSEgV${RdT|Z)e!M-|mW(Zk^Dv=p(L;t9$VWzU7*u-1u6+0~XU$Z+ zi!B_~?t)5(+R{}Q+n|4ED%jlaC4zuwub(uE*Zsx)dLGbA7!QGvCxTd0(JyD%LsKNA zR9Blp=}^xhV?{QZ@nL-95x_LES^&=UiRFHZ>A0Aqq1lEmQLXZxl%!dTUm#NpaYhjb zBu`Qb2+FEJ_MV8@v)?HWAw9id&T2r$bZj_64QwP}5~;$k)3Nno~)@#j2g=%KU1_t2UB^E}V1 zP)%U!4>?^@HMa5Ip|uCW@noZL#*CPxI&)zq@g8<1+X%XfuIf0Y&2Va%umi_P*m{tp zYm(9+0(z+UwGcUawP8vECEAu5>6|^pwN7Iu$!^Marx@3#<29B<9V<8;b|Pz(j}~4l z3*h0(si@-{rLyF$kXfjK-w+A=I&yq_+A=2JqK;wtB4$dn3q0*{o~}{$v9Gsrj9Ed^ z7LuD=e&DDvMZeurwi6~0^NvIvM^3HLc}05;oVsz>97+0aQesytls!mE_i`&3wY8hw0-gj%kh zLi=%8PFr@M{0C^5`8(BJX_4%^;%#phMh&Xntl{dohAZKgojX1_4;rEBjF(PgC5s-L zWF9ys4x@hLp)k2wk$3etw60#3Eg_oI#mOf)YsIECUl%#eqsr&_;sG>B`kCd{_Q;63 zd#5dXDQQYH6yXwx{z}VuY&A)#)ivNu{|^8sK-j+o{n_CyLd=CxlJ{^(u5x)YGmi26 z+T|@aBIxt?RHuoLsIcdlxq`Agkj6VOGd1kyIDFj3fXCgq)v=p6F}* zV)tOE&Qt#4e)9~lq>|hAABneO*{>`AlC?|f-I1U%SEuXS$_~4bdUf+fV6Yx#Ow~D- z4rCz%*`J+qo8)>^POLDq(UBoqAW0N$=ee_sThdak=y*-(M$2okrV~%ld?rHuv|11#;7Ic%Psz%uOukB#XS4IV8Q+#Q%?`p) z+OS;wq=P_6!b$nbCsC?^g&qQ&Ie1>ikd|?H+e_VBWZ!CUj08nBGQFX~t)o54!YJwf zkkIz3mpN7l6aW-IGqs}GhpRW*5FOL9Z|K~<&$Jx6-68`*FO1}#;}IxC>F_N54sPt( ze&=Gvo6y>vkYop|D4Gl4n`?hQEM4fLPT9v$unUMcR(L3NU;+Z}ok}tF*S@}{zN4P# zSP09z&akqmSkjhTdJdHo3PbpeH~n+O(7<8R^{1K50UZD2&ksW`loX^t3gtj8#X4Xf zR>e=(-oSk;2Jo=A@_F;mg+L>yND9Y^nj}z`6t9BYwc(C5R8c?VIQ~KGZQ(gvK!iox zdB7!Hfb!~1NXy3;ZDX@3{1zsHq0wq-XK9Tr{R7Z<3HoDfBU9?L$D69}(nE$Dmi*hS zNz&$QyGyuyQAA?HZOYIA>Zo=o-6)oxo4@UT{b-|Yl!W3J&hvm+odjw^2FkUPJ?8T$ zDf;Mr!TADn{ONi+^}Z?yV?dzILP)V%m$OUoWq9eaci^dO#CN%WM9;&1IYvITF@h|D zX1E^Pa2AnI?>^?=X^*T>^b>VoYxneG_T!6cDfsD{IMPTgM$SnQC;bNsAT1?duzZnJ z5wE5L?=Z?+^`{ECp!^{>Z4=MPLog%mVohsU)YL;}9nxA|R&rc)p`W+sfU028ob1(p zHWdLQLa7PFhKWw}mrh8VE%@!o{yw-7Z(|tg> zay2LoT&XMf-jQ)Sn?LF+jp12MKgHjyt@RV7Nw=ZoiMI($lAxg zC<8$qV77D2kpzg9!=iK$?A4nfgl=QMA&o23-o${s+ydssCN2BcX19RaU(s zC}z#%ItvD!WAW3K30j$R$w0A*Y2m|$>Y_=|8>y6NAa#ruMUn<~J(?VPb&xYEE$Ylq-K;Ja%R9># zEL*|d{qFSqUVs6irO`cL$c?O$VI0({Nfs3daa+%H5>r_4NEbh|8SD~g8?>) z7znm^^`&aMpKo(D{rp!UJdgFJ|3Hmhyb=ynefB3x@Vw9cSRyjt&#a9~-ryLvX=cjf zB$sM<$$PnIsOyUHpJ0Z|YsY+{b(e}!k!IpoEerXtt}TF4&r}>X-l`yb~1fT-WfEEdmXrGI_s3ZtiAeY{qODdO`4 zc{LwUZgiuMHWyE4&~)OKFnAImRF1G3jF_saS#{JU3*h%IT;ggS!kmHiY||8 zsC&0m-GB&I&X2T-u|RkGqj8BBmTJ-?)wSI@m2c`}_otgBygS0v*|NKQMP!U)P+xw$ zLX5BcJ7*mQlVJ#%@(%x~BdG2t9CH!_j$hxL0wjFVGyQ?m)m;SFpi(NJlyppjPALkM zTSz{LhERNVmP=eBSs}CRuuiqkO{BR_){;BK8$nTm^bb-o-QG4aFBIupmR20&Od;sz zB6dQ9r{;U-WV@G9BRo0a24q1`$z+!NkqeHAFjL`kIqU4HjQG&(#^g#*pd*nwQXgE% zBj+l7FCCJC(%uh%iJz<3sTI#@1BOQfYf+#Yud&N+-aR3#ZV|g`U<*7Ko0qh*sYDZu zR1%NAO?t~GJ!j=inUIV6+2W&$-b9US3!UY-7f(2SWd#6PdP$95CE()KZ0w2}*0U8v zkrCfC*CFp+8~t3EPf?rf+l=)8_IjJ5GxitCB1#IiS;ACp3hy6$bO|amdi8WrHavp&!X^i{ zc~)D0efLjm>t+`OoAn;D@yB=;c3OFV>hkR!fa0?l3gRS1Rfn_VU>9=;4)q~WjOyU6 zmLpK`dHywd=%%zC_bjtd7FPZx3nB`t*WxE*VgMY}#8qioaO&2XLg7mi;F>HH`xMU! zu&F~uMCG8PZt<|AiU}I~%I?7XKRa^7$*TbN;CLUxGnPbT3`e!bE)5%Dfz#ieDnMUi zqx2zdDyd-m-%y+5hLQ=(5AveOak|M~5-7byO_m%ZM{q4lk0b9a-pIMbQx_409ZY5m zS@V?fGXjsTZt5(KGaeXU`I64AF__dg$>{cN(eqn$#C~L){U9zL^YM-zqQY1?A1_|p*x`g`JaoMxeF+jjYvZGl5<)u{ZFU}l>O=7M zf*KV|V83`qdK}fhT+n*W%;{$aFBvpjoGWmS(7FlLWq$zds7Kb(oPaa^r(*^uC$(jO z48^od-J=jv&!(|Z|@Q0F0+Yeh7c{eg8O)H z=x~&oBmHJ<8%6;XL+nsJ*h2Y)rYoc6h;?9EnNhlxPbsc^i9gvP(>TV?<1*%(xK-%^ zv~=Hsm3CjWvtRl}~rMrT*1A5No&nS(1C88%ycLIsIxMc0(h zo2!$SDB(ATds({KDdXeiB~L(hqKV$2u-lj$IoSu3X%?*u41P6eT`J^D&tgwqtHrjb zm+xu8d9PpQ1HndC&@8~yn^Zk3$7&P9_GNx@mpCYL3To%Yc&a@H3X-vvga?L{G^);1 z=WM`yJ3g)Wig(+(4H+V(*HqqAfg4g=lYG9Ji@FB}&?LmLfkmir;5r#}KxrT}I)bdR z@)|7{Wjfz&3=1tt#;sYJ11goMd0KY{FBMu&P{Xjp>>3X`wemd!QiECZ6~buV{F8Vf zS-2Mnwc`ij3?0i;{xQncZ}=hq$r>ORD5dOQ|Ky_(m~;nBozWGCxnn%pi`W9yp)|hI z{RWTlZIJ8S-H&RzbXKo~SRH_&=v6$7AjW!;Mnwlk#{3fCwEYxo${Kfs2rCQB5G^Wh zJ=d%}rfedM`lr4(=#>%7ExU8;6H;}C@27R|Fwu{VKk1`3FT4WrmVKzXV!rTLgVk|B z#Z{RdgQSf%E}pyWxx+~n(yMDa)N3j>-Y<*%crL@9bqV1p84qe6O>8LuD@M#)r7!Y) zXF6$Ec&Zz@c`GQ~o9E#yyhtvLEb)=ej}nAZJ__wqi^Y78PuXAQGL7}%S?o&{Ppm#v zh9aYRE=5t@_#r9Hx4@Q|!#n9Sv-oIEt3Vca=6XSo((j{yoFNt%; zzyx5jxKR$=#0WM5tex=)c=Iaw#!`vbqLupCfjOq=%fpN}Tqi~v$cT029ki`S=Qi$6 z(xqJ%ec=h50$e$1)PTJ2 z>5F67pLJiyPs;PfhDUpTVuN*!VIpX^O@=(d%U5!NlJKygb8_q-AgY$x7_Bw`X_UR~OPstc zwPg*H?=YWMuDI_t6P(6{TmrRD;loz|;959s5NN-uH8V@}h1G(MgP^+@Pf?nyN@T5d ziU%}EuYi$S(#q%L|GcS)74^Vqh7suRI=l{(OfHAd4EC^Lc!ytujn9>woJ=If&D(C= za<{mo^JrE+I`U|f*cB7##{;-oivz+P@HriDny#n*P6K0mMVm?a#GJhOmUYyi_f&xu zI%;QVcSAgLIb$HcG3l2DEzn;8Ul{#5SBl43_Fowbn!d*IlEt|`Z|epZjx!^VBBr9-0%|3*qltZm`eeze`l4dtY$J!v?n)v6;?(Rq{HDVInIFc_?K2=RaX+`%^ zn~tSg1B!?7iw)L8?HMsg!>lDL7~ZJnGr$v99C^=|3&^ zy9kkslnSa#qiJacqM9?GnIqK%ptXe8S=kK01Pc#o2hmIyyI7AipH<;(7kd(jlhEJl z4jw6iER39DgSMM;7fq0|&|?f9zP!PlbNNgUu>oF`>?y>cmh4aDu}5QnSW5H#XRfh& z4sft(fz;i_*+rVcslalxiwoW6?p=p3xx3B>|$)7jQZ@zfNl zZepfLOS(so+>4{!e%yx{V;cz3Vm$d%`wjvgL^En`e4<-LQ(2cdhJXwA`v)Jkya;BF zIU3$`#Bx=qQe=8U$2YCh5hp=brCc z(DL7FY#w>tFHGZ3MCKh^3lGMgRXbRdE9UnuOmTYtk3-5O-i8OApX*KvOqk~t3)X~` zIjoSq`1)EtN0jvb8g<>?72E0WhdGXyj8t%XaSZ7S5|ih%LLi3!hprahCf(z$)?zh(?Bfs$dnb2tP$64 zeHJx*jwMAdrnbot7ujGmJa1LsrE|DPZq`SBROrU~wi_Ku$gWwzsOG)siClgN0bzSN z;TPfy3Rm|?>J_@;E7cqZe)|uW;M6#Z$W8b7+Si^7Rnz_oo)~EC@tU5|x)FB`5S%4b z=Lx(Z<7;z2boYaJPg;_KUz}`0mMvX=^=xAf0*h$9qBkavc!h6*OOD4_~9$SLAUmUqd`n4 z79q~%B9iTm?CgX{ooD?Vua^|}Oz_7h((kzbHao@Vch>{aFl`{PTtxk;qGGH#V#&Mt z*}?!#g?G?QB{*L-2}92c%w%|fG73+z#xM_euj@jb^_eF%y`#f~xSCneQPs`!>q-Oy zLcCyrVTzX3vOoK}kZ*C6V)H|-Ud~JYAV~|#@A{CGD7U4L;$zxPiA^oo37>1M;?SgL z>F!G3NRQIzB!cY^m6!J(qy#TXX_s+hBDmt$ChOb>exaduqT0EXhIc^lH6C8WqN4^E zIV0XF$XCh19W5Sxj+?HfXy#TLa_SKlK!=B7JG7Z&mLLJJ_gm*He;WE(lRmU5c8L&V zY~~eH>e>4$8UO*xnF05&=hrQlyOVdtyyE5WPqVQ${%sf#sorvVYsJoaA_TDgeDZ5N zOTo~5icIL5b$&Q{_ZY9BX8r*wrsAqA6@!os?IB$7eYYI;OJ`%{!TRtGd?{*m(I^tU(Tf9T%~M*EKJhs*cozA|FbueyRcT;KT!ChVoCp*#Nf zo3&1O8<_3nR@f#zt}?7qVO`30d$i`~cs$k%LRhx&zat}5);PWByY3?A@Dltm$Arvu zn(=mqR4l^P^XMvTd`aK8#_Q-CFmVckl6=(7MO?A~#9$CPxMl_ia`@#(&+XiJO}ns* z8}tD5wWH`JsJ|~#&c#F9d#;lbAEJ8IThLVCSkvhpKSiSUX7B#tK|}6?l&s&F!C@Q+*BpQi5iD<$ zfZYDL{Xf2Hz|ncw5)Ype&rnteq&}g5QU>YR1#p*E3(7&O_oQFXm8es)v zoKf#8N=!|aull9x=<~&}Be&^kIR?DiTJ@Dk`#LDgL}~=|a@bV|mv6ma z2gQ{FBeZ>Olkt-_>EvdkqQ%A&{a8rCtm{$ZKq^5RItsU&oCh?-{|d#!pnORKE>N!smJ^Vv=STVA|2ZwOu} z%~Vc0?znb(_%bBg+k`YcJEksygbAkiY}Oj$d;@umM^SO?3@Z_)NFUB~$9w%JUutvh z$C&6b>&v}0@emZfU=VFishUcMtGs@52=k-3FN16aje>=5>TINH!AM<#KK0oP(d2K{7bIM?=mf8|I0P*V209NKPQ;gaV@}PiR^UN3utr0}^1V%B` z?S9t*f4_J^A)*}HUOVNX8gLUaTscvr$b-{L{4{m-UeR#~MceZR5Zkhz3%!zW-6JK0 zDZWznMvo;$y8VL%Zd+nS;9dK`@~X2#siu3$LS8p|fv@%aPZB4f*9pkol(jr>_~a~jGI z!;<9;U{8DQkK&yPp0Vz8waS6BGB=SSX4>QSSXXB{Jtf8Taaqi`22sW_*7lsDS0dEp zJ0ue}>&kis-S^|L*KhKn=gqZpvR7zl+6_1`Pv^d85hnfzk5a9{f?VzIHg7LC8`Re? zw#CaI6J1NAZ}{Tt-R#7NtzQeG3^V@;DSA~sxw@Mzio;#e3WtS1v;)3}2y#IsCh2O` zngSQ%P>qvpxQll>AG%G^xmf=~ILlAyBuXDJ*@S#Hf7JtJ9^dyyCFMpr$t;@Ana>oI55bgRK?F~ZI8SpH&8-LV6_ zwb~uE4XeaDxDBPWb^o9#-frQ1jc*-)s^C7QJhjtFcWh@Z(<~2L4Vjg{gI4t53JMY5 zh|6p9Xj8GXDfm#B1zAgbfrT);m*?-KyR80@Pk;9lWh>D-j-{=PBWy2vk)W28VGQC& zfSe^su_sqhm)9FteGrs?UQ)}DlDSq{HIC)`y*sY7t-|J zuLNf7`6)5SRx}iiVO^^7XAHM|nL|T~>9`irX5EYbTOB0VT&q@H@UHjny@5uOr*h!4 z|HOZ$In)_fjbY+93rr&02`1`>(YK?S|5h$f$jd$sl9cZIUY={lX|Oz!7y7U;eor5v zGY`S)JVn6Fa^EvzgUdtqOCZ7z9cIpyXJ4xa*5>Y&!oWvL`QM3lrbf~$u?qv<1Z|Hj z5n#q;5e=u@C9j}L)G4#hY5?+da`JnU`hiO_JEd?r;b__D;{oYV)nlEf1|cY92sz@) zHD8D4;<|X_mC53WShAioI@%PLx=h>Zf^HVR&GsNa5W5m=0$wW00hbbZM!vhK@;>nBcy0q5901!a$zuU6&I+ou4Fcw2AW(6FC+wWvw$rJ}{ zHaTrfbq2?JD~;JQPox`l4iT@weJ4MB37$47XPi^K)q@|^93QoE{Fn@om3ccYrGu?k z)7{V?pu+kwnbf+##U>`DbQT?~61fp*XAOW60FnSGtQ+L$c zbpMW-9YyzJLHvo2+p~ui@OrS)l}-a7!k1xfCmOUfqIJZ;q3v1A<0a6Zz|n|?^+7`(4lsogCzUV=n-x*RAF4ajh@?xXS5xk- zc;PW=`u_ZE^RZBy474flWwkxo$pB8ntPL+eCixhsx9Di?W!>QXVPWi(uu1If7$5Fi zJ>Sr0wx5GNN67pzoNdQh>eMB5_CIoa0*w-Yoq}U|x5O@>2+;};Qz`notC;1SFNtg| z8w)}*Q~Q#O0QYkg>EbYQmo5eUO?C}aMyi+ZlEHt=$$H_iBt_&ZhhtYz2?rL}XmtC2 zZ^;Y3HOgVcI@~eZYMlr6^OqzEipwJC^OClWvL)<`q_|WhEU#g$USh=US$_d`&$Es} z&;?)c$BL(CSWwLvU8g$>C)mMRdHE2&+OwWGOgQsDr7L-}6yjtes^V^~Agf;fpYe}X ziSHFV87Tev>BPhIDeNb}-f}8Y=MU64ojGee^G!6*cNEOGsjQ6X`m9@C$n*Vob%CQS zGmdhF((O4Lpi|@o2>G$BO99U5(L#-9Os_xrKOgV(@o#$%9_desHUOK_JgY9#pV;ZvQcJ>L~yR`nztar!t*;-Ws)4jt&aKic(6xXNX{1#p|17#-XUWKy{9x6rB9S+JX0k%0yz>b2JSfade7l_yvi@t+CgDaxa8z^TAU_msXxd< zn59|6nvv_!KP+~KSyKt}gl#80?l`Br9gy&c?=)*tGtzejit;dcreIqT7^>EXwV8B= zu%|I+4{`ba3vJVlv)=xtL)YvRs2b5tLZLBp$#BZu5W2}&!i;xn_nyYKKFSj-sro8w z2Ajb?2e7A4^cW@@mxHB|k6)?NpZ?);7LWEmbaviqP$TNQ{eK*bqu0_Ec&TZn`e+~& zd32Sndn2}GGDnlKmu7%z6*Fyy9??9wj#fnWmaa*`DSEGRq09RR8Y_ zizPZUGpy!Fg*vCqTnyEp8Op8;EzP6eypcJ83n04RRhV9c)bH8B$8_(Egik`)xOd|^ zUzbD6JPs~dP18y)!S%(*a`I7dc7j!bnP*MVG}YNS;2mDh$6z%@7kQ{Wa~Z2(>|QXh zik2*SiVQkEJo+a}i-A~x6P+|oAWarqWl;dE(Yi_XGQj@wOk`n;ZGJ&**`i{K2Uo4h z?NY4hG?62C6!i*LBT4}9>|7QWa8}q!-}cloJ{OeVzhFCK8nX9L-2pOCC8_Bx%`;=? zA=xSC*53{tiV?*7n+{?$c%#bIve0?ECn%Mj+(3wWSP+U)Yu)FMfb82ah}zrs^q$&| z-s`CZiLKC)PhB>oGaG$}0dSh4gQ!GZXyLB{1owC@xLxarRPDv;D6*wZY1jkVW+jm) zr&Kbv3o*cI@?ZD;XqEl9RAi9okik*+g1Va9W{d2;CIStXAc5A@!QWpY$tKCIL&H`q zk)HCWNWJK}C&_J?wcMXHaDss60v9%uh$N?q8$K#kJ-FTw(B1%MJINr{?bFTx=JRA4 zNF$*GDc0h8F#B+4PJc)`n#p;Dd6cMitiDAMDy@KwsxiXN@L2w0PTjTvBNdWx{y*}S ze3WWpJ^bcKD!-DhCRAEkvI3&``1?443>6l2FYb;%SC$IJ7p zOd(}#@N`_OKxoV1ZZgq{wlLH{#s-)}R7+ZgpbXvoFW%Z=s`Pmm#%z8dd~}q1^E!K3 z_bL(|E==pg}92G6z+Q3zpM8;_UY?N1H}$)-3$4Pk#I@Ct;~ zmOaDMMYslRLu9DVXn|;d_U)2s$y!G!&z1}IOZY_vf>3Yp;WqE@0bI?L#Eo1HdTeUB zJi_tLp){2;Z?7j)>PfBxbT|;&*XV_3IZbb2GnKBr-z-!mL@P3f1E{9m<>j_mR)%&< zZeBQ9J$DruvxvdgH9)Gr2Zpoy2u`r8uc(MF2}zG^rGk);fd?duSyqXYf zj_RrT{=K~`Jp3AN7+6Y|meVl<_X3Qt630Z>tWL0!ivA|7kjlJVF}}c*;~Yor{Bzf7 zf7D(-7K`s2U?dU@#btEa>Yt*fi7aF)xX)Sz>j941iZ!)XLS_dwXlVvspdx4G)toGK za5GH`nc7G7WzEkR0b+!ko|(O7&f8={QlA*;o@$h=`GDE2&H=pp^*<=ntqWpfN)4J> zl#a2EhbsSvwYNt&gOB#*I3v6kf>QN;!U29Q7&vlMu!Xh8w}Hjv>-&eW%oRoA;(oYw zbc?Pf;goP_x=^<;k$ALq)-kenP`vt$vp^!0CC97MKadXn>V&2w$sZw`xy^wievZZya_cHrI_DD%)2mC8 zc;-^V^9*E$Z>l{%VWyR_%sVFz_<^%_`(<^^FMOrPjSFlEXEaQ_ntkRtD?}~F%nx*% z)u>J^{ynBvM;i4%G?1Z+7?9MOJjqd02A!^CO*uBrLy-xYXjv2xu_} z>R`sAh*kcHS7x^jtSVhnYgBap>y%fB!Ln$7hFdE5oC#7f=mapEsYMlA?P6&xoY>N} zx{m(dd=*4KkG#GetHAh#LD$_)`sR%Dw>TL#6`+pzFx6U6_kM9!h2x1%G`D(Tgu1+z z-gcw9dtfPLk4En!bNKVBY>eaKTe9~vBOSEazz1?cdCxjxOM1(gBBb;J3{p%EH4u%hqbK_*d?AQK& z1tW(Rk~}rdbloaWS(0$Z0wJuJ;@8GPP^j|I{UN_J+&OC1ZV8bKnR^Q77qPU+j`~$) zr8rxhwVh0?be3k}qZ)9=VNIqCvxS2f2`A{oDb0(iC_}wPxU+$-FmLmIdM>W>{mnXn zz^j#PasRHLR0tRZXB7udX6H$?W4Vwy&kMr!AOtprrKWCNQ~Vg%Yu(?n=nYsZBbai; zPw~dZsg7%>H8j$(l*YI2T6ste$PM{Edvtr#2=J%DSW)u2!~gc_pfuwkbyYKK#d4lnTUhE;(q$d+rBT;EZJRNnapP0o)c}XmyQTTzDN(_HwQ4D8S~rTf4!)DrTLG*tLLMPKzuxACwFN3mHZ={ka5LA7xO&ysMe30`=@h|eAqN7+kpGv zg0XX}?*33PvXgNIWmOhOo zWdx~D%kAI@F=PSQv4QR`3Ou08m#~y3cN!=bRk3pmx}H^v@$6>UN^yVC35*}iwT=JT zjNP%qY;{A96dV$!2~l{0rC-DXS&8hwA<<&4&Bfg%yqPMj)~kQv`oU3X$?rm5z8N{$ z1nHvjur=PoQLgAY0yaH|4jdr2s^N*_=z1syFm_N@fcjnV-XizX$d*6KZz=oR`6U%V zLCRX6l*=(eYs-ETUtBsgr+w5$L%Mra}x|NzFKd7RkW;r#j{l^HQX{&Kp@7YDV}TFiwIw9V!p}2 zM_o%vv{VL73T*;4?McA^j*WgF&_(Mg_fexc zB>K0+_}%2oy35(s>rmXSX(WYi-%1NdRUD2yvP=s;5Z@b0T5b>5!~G?<(=2#>;Asf9 z#Z}K_wm%LM!XL2CRZ86F$yeI?V*qaop0~}Tf*YcTrX~%!f(P=*WBcYGyNVGa+zrlV z+o#>au9XUc#0PoE*pDV?+^CI2ORi=6+gsd;RVdEE{s+EMSXY~Z+di?s50V-gj0+ikGMmYda0+?iaZ&?GTy6neKbu2pmk#<`m}6*5SQk}9N8Mf|!hLS?apGf~yL$-pPAY0S04 z36@GPe6-P~m!?e;1~IL>?)aIpe^*N(LgakV6eR3Ixs&;l`RhHXv(CjeJNIkn6K02Q zyv9mB#Bcmz6G#BVb;?`I=jfjZ3g!vk?o1~5Bk}%TZKo7}M*P!MbC9uBMB)*TZ%=;c z19YWEuj%TcetDv|*_GzKf}7VFfrT&!b__R!)YHB!0^5%zBi|_&= z{8Da;^0HJ2Pr+iK)B=gJM?1fD04tBWnMRGdXj#kHRS6358{(01K+KbBC*+#kc0@C3 zJOUb3WG$?ZiyRgtjH1Qnv`Rk9{;89;r>ac%@})GmQ@2cs?Ap@r*gm5t>LmoXP>)-O zE^R`5x=%Uo<3zgrVhx(mS13EP+!y6f!7Tl>F?q~l;RSlKRpbBxr{6AP!SXS4Kz5ik zgShKD_w1ew`I@gvKw>%G3ev)=lD3v8N?YIlw4g=nVy)GY%?KE3cZKu6xS6E{L`yv) zgR)7mbw`!}_CZxzsnS|$59Whb2>vtwp`o?AKL(M&{4gmWt^+Ui8&Y_SMJ1&Vt=G~K z!Pq?LRZ5PdI?`S+SHv-8*$Oq-w!9U@NNmN1gQZm2(bINFYS#r=3eO~RP{B0+Gq9Sq zG7tmh)kyCWTG^6Vo(wa&L%J#8r%>So5T_1vyf(b+q7 zt!u$ciDrU1-asY)%Iz`FBYm+Oof>t(0cYffB#!X}zaXF)LSK{##T(feNZM8EZE99_ zFlY5h1B7+y6?pD&ELf^8&ylkA@+r}e@m@bOErC{B^+N_X3bVfHo;)| zQ2q^)t`TlBWY`mlT{@m=gd?=~Q3902Rw7F78=h$JB_3$5{Zy~#am2T(3Ax_B44Q?Z zWbCQf_|~Zw5f~c4Z?h|wNXxc@Q-EC8@tF40%l(F4vuBcCb=CvthO~Ry0nBy4%CWv8XIg6+nT+Xr|ht{BPhy^AUY>U9U;&Ga~ezBtUB zp~?dqtJ^fG!zSVN)ZIBrZ5R!osUDR0SlfXNDr+ag`W5E-h|UAj-HsMFLtj z7+PJ&M^eNThG~vL!e4B%A7fh9id_kxjJ{6FMqT8Q%_0w+rp{*X!)>lgq7Qv2oKfCeHgzVj9H2;jP8;3LSa z9V-r`^*Ahb#`RQuluhR@E}L0Oc2nAfjqNaai@y>&hLSdIrgk^_y%}ZVvu}Sc4r}F@ z1sa(-%@xTFwQO2zkxCF-bb1|r;%pF>#?&4VSsdZ{B@>0RuQ9ijF1I4(=%afz9migu5E^g#8-7uh7=qIBXS^ za>H4jRnWFyB}01Vfv*0X%3HcNgO9@@6lY5^JHTVi7nG>Z*u_0{ipK9?a_rXF(Yq$V}l6x?rC*bUZ_N+80O z60&H$`eKCP!sopu(9VswPb`v%v6HJi(XoItj;RZ^bz+9b$U*&Ij9bXq_67|wVm z0}WoHjRvRio(n(myIx9>!4CHI+3;(GX_IPxA3??va6$(Wo3=;iTH4|d9{iFymRl4f%H`Przxddu0J0HWBPR-SSN zj+Rd`n02)ymu&+Gh~lEgZCHX;Jme+8|ANibU zFEHUlj5fVC;5$Vj@CCic3Dw+D#!i(uN;-Yq@Nr4if|+FVDbHG4b zP}@!?6En3rl&4S(=kY+@@}QkrxvDjE{sbtj5B;$bf0t1c zZDpP_dEaQa_Q}XR821*3AMpP3=6@G)DU@!LuEYr}>kNVoor`Y>>nHUYf5b3|$-5B4 zz*+N+K;vRBASz2kQ-KUok+!?r!b7(rM`M+;ZA64VSyZ2W32G5;n=B6UW>5YQ*K-^d zF<5`5hQOZYm!}&A;YNT4hgbj00rPoe+1SShX(H_Q^Q=C-YdzHLY<7pca3!qdeBZ)V zbT6{Z!l!U_4nzPxN^+M7O>b?dF@!_ThGtt(Wjg-Lye= z|B&AniOvj4*xQ`sssURs33smzZu4v*{ERZ&uIw4`(ttw4Iw!$%?b3ErjxT@;$}L7q zv-6TrR!9DbzTs;$GK7}s`&16V+FuM2?fRg*PS(Udx0^dayGiZW^R{7WzVpjjM1{V8 zFG2Qg^1zB;*f0D6S)0Mfs8%RoYodPigFu&*_n8OoqzMkZsEI!875dr>xPXRJS(77c z68BmvOB?>m_=4^r$cIrt^c6{1VW<`f`%p)B>r@eSX)`#pDvH9#vlzh=4n=LyGQl$d zZ5Q2Abi*r@K_Qr&h z=wv44&G{r%y?sd@_kbMyx>QpS>j ziPam&3t63K6f|7K3qA_JL*0Nz69P2IQ!$V8To!#Sm%zczswD-@&CDgO8lWaZxDt%f zmYxH$`Wl0_@C0l)WSLJ|q=q$?$^3%3tRHv>ifX#}tLy(Uu!HU!GYz zpo;rRhQBftDS#y2d09s1uz~|kwgXlDF{k~d3c64XDEPli5>5d75_}^?=7i^h2q#cf zpWgDIDW_rsT5-hzH8cq;VIDZLNKL9HGz|oGj1J%`{j?+gBDvEB!BCFEPlk!7@P=+z z?C^|<(7fvl8X5uXXOZJywY_RUlb0D!9rm)cEvw0JD?JXdj1Q927U?p22YHj-VIKku zR@mWBlwoR<9Y?Ez%nAukTql71<8)lirC@w8$o!r{)tV?0y6DWssnw#EGdA`Yo$d0n z{S}mHdQ(wo$Bxu`WtT1VER2+I>gWTMXX(-a5kT(0!pKIV+1>IUkb+=(ecJiSQ8K@) z!9+U(w0}YW62BH3(f!oci8*s^P6H6%b&JQA+Z9|1YdBlyT%_t2JYHK6>UVQ@&&)7@ ze3}XGEbH~r77Cg0najUQ$Nq_pEcJHU^?0lVlE~pQCe1HZJi{PY#d=N(8f_ekC*-9P z16v+z$!k}{t3_38-DxYwofF9&cbn4&$L$$q5e$JcS?OzH(p5%nn*m90YAG-sH92Da z15|+>8P@!Iy6_TGX`Gl1DMVB=DO%WMQTLALK*;spq4{mIf8)Vz{3MZ;ScSV>Bmp^# z2383Qni)-k1JK8s4y|G)UHUKgL2^#==6^aT$Sz)xWTDBsXUSnoLGCRJ5`8NjyOArz z6_(20j|{a;;IrM9Tyg9(wBy{CHh?Gj5g7$HIaVe9#P&4mtw$8h2a+}0o{f+E!FENpNO2ow; zX{pzYUf~z0T5mY5lf<$C#{^xj5Jo~iN*+yD7=2=f*o?XRj*OFPB6SV<1*(T)6I$>f zT%}CBH?va?E&gPAH8J)u?{*yv`!&Ll z@NM#cz;B?@f(*uj(f{mKFs2SxhB5Lua$Vrrna~@-XmUP>6*{BX8~{_z*&?ovTKjtG z)17_cw=1JoleUqPPFgM@p6h4YpMr~vEdd57G7_~H8k4`)de`Pni9h32(}nRyXN|?c zX@}KjRMfoh08viBB9G^-D70+v>m%>}u;1;Y^`(UkgLvbI0TCMLP!`eB(K;T1%xNtf zYBHO`W1G3=?h0yYzwZQvF0Kl_0j#mA5YAQyG-Vm=&O|@5@aI*}%Qrwtjt+v}V_$O!CM>A1K%gFX6jlkROQ>89($F(8 z2&+$`{@7vthrY$|jD}_oduceuktI1kYXrIZ>Vy^wqy@FKNs*r0JT9a*)`pdeYZOxr zO;HWU>6*rwCA?CPG(MQ~{>q?QqC?TleWY2(+CiZA&?I1Z)|GDo z8`j;|Mi%nDx)RU5$65S}yjayiV_BYhnbv|;wH)wl<=-=d@mrQJwUIFyM@bLdmO2<% z%qm9K?@*Qja#cl{aNT?_2+xBwC73bg73RS#C^j~!<5|CC-T8Z<% zvCp4SO#3qcna?{k4QDp@BmpQaP$)dz(Z){Q2tV2ivv^<#;QJz<=uD9l;+Ug4O`KLV zDNs|^?;IBs@apHmh-n>uxkGmaK=gSg1iWbsuV*~tV`c6?5M&r|S`rJ%41Z_T4vLi= zj2O^|4GxwD$8z9ym5&rOBlI zEHN$;;42hT)DCfKj{rB93!_17Fz)20kno}_J-fm!N7Jo%UE3DbA%qn;57D)!ikev>?B}(x@o!%tDN|j$fJVV0K}lI0;kS4FLlMu!H1c6cFAgoasp@* zGGxL5*V|_4cs$~haQ=u5?uH8w&^q6_OQ>Xu&bDOUZRt*XwgUX`W!4E4L(w2cP8oFk z#P!QACf)FET()rSU`0S2o#l2V{lT8c+~*c^Hz7CJ&I|?ZkNLQS;4q93<8Sa*duOUQ zyIksXYwzQc)`}4afa`W7IGYT&3<3Cup<99CHSyz3#xtIlyyLT8UMG<}JH|O8BBKTy zIPc}$oQUtPGw^=xc8j??Z1F!fSxDeO2Iywa`w)CmVzk|QA(O;oz9vfaLG6sjnP1m$ zhkx2oMU%2JyDgGg+tqs2Z?@B>don5lM#Ji_O9mcWb6jpnhxzxSKH-q+GBG=nv7b)& z{IFRf#T${$6!pCYWC7f??G0aECfi`z@Aqy`CB*Q2!kYfK`^pCCfcMrz2EUJIkcjb2 z9L%rL&HhMqS*a*`$$X}>rkPj%xZDOQtR5$?l--_7I-W1M8eJXr4OD*)02gw#a?^SA za)xRWf0L}k3Ucg%tN2*$D;A=#y%Q5NpQeGsdJ<8w*l))Q`fkpbGv}VrZ6Ys9NIgVo zw}fyGZ}q9OepIHtnJQ~L-XcE6$i5+yvjqSpL)K^w z_5d0^4%VSrW`6Lut2BTeSly|lPCW+%+>cy^^i&th#Es?Nf!3|(8Rthwf`dgW;Od(o zh8|4r5+x*E^WXu{Bn=~!r?Q>y1t;P0IccObs#|(m>c2_#qK5>_5T}Qw+%GPEI|r4f zTGRH68Y6mIl|YAfji*PaRdePrvw8}54shCqEF>jR#V?tXlSqaK6{6ipSAiQnryheL zck(2wzbs20S1Zydh#jUYd$A^o=8CX#XlMy43d`LMFW>+csUy_a%5Lv99d%$pW#Iy< z^Z>4;?dC-7d>Wo;e!bJW5G&>D^^nt=#n7t_h?DX$e7#gpW>j~P4+ko+ z)6`O=iAmL;3L0^gmaFP8DbJecmio`FL-O=v%|#_10=<;y(c!P&0x(*>!3uj8El0Tc zIAsZ4aaGiwVA zJ?x|>SPgBkbs%f(hHS!rT)rBbr!q!E3zVaI(ni;nC`H)Hj(#;h`yt+~Dv@M3sLK3A z@Xql_(mY0j@wDS*Tr;@8TpXO8nHC2g1i|+4kJ*eM3LhJYf07CI)Ca_}XpY$1-7L~hIglY0FN}T_+}pAv!Ar|#VlkT ziM406{rVuPTyk~Jj5r&%^#R-yl9KVII0u4#d9m`e_|w-|ZA*!*w)CH564^a)MOE7? zA_}2r419%8*t8n1{G>cZOA>u_+AmP)wfnFbX*_v-B!k0^2_Xww*ZUydfd$*exAHI_ zc^D$Y1rc|z(v59LW)`Kn@>My`f$svfvSUuov|Dk4@f3?U$lm;~X)wgZT`ecwuvj)! z_>}Cn)b5H~jJAoPB{AM7t=#I)&$gR~^!Bh#DfS!K3tP&qI8)a<9p8vG7kyEvGj*Yu zq9(%D9TDmEMz)QzC%Tb6Htuk#E1ft-=C4u`!&Xr_L9TjJ=*@3FVDigDeMQCrRPt-nK4D{~2vWk|b38TeFMNPc!q-nGG3P`bqzSs?eBW&z3 z;iRpvOrzk_5gweC5WWn6w#KW*v-z+hp2*3jobq*JGH=G;8g-JosqLNfd5U zz5TuWPATO~uyzrZFa~3$N2z~+g;+r=~#${4VM#+7Qd~O5lR#|y8%$PN_ovL1)7s{c0t>kLvWHhI-5D)ww;5+d>1q~ z%greQ&BDC1x)z04duU2Ry5p5cMP4^Q9P7yvPKu0~Qta>=^dHQ1qz*+^N;=%7W#?09 znqUP9KAPV^%*%}Ca(u*0QxR=CfuilY{!(S{_q|$vm;1hGk`%QpmCbX_qRvpA11Pbn z<$T8ryn!ox1q_n*+UGA|hw=9Y7A?znxHP~cjB#Mr)TFn7K^=KW{29LsCbif&&jW;Z zqe*cXatsiBJA^_8GxwFwnuGcV*$ZV|ftZGm&I`341YdX@F5-Wnsj8Y-VEArU#k}jz z>#is{Us2^&RFwp>{+f3J(QbX*eupfJk5;=_>alj|z4?xV#X>ZgN2l;0mLiDM1tiG0 z*08_*XhjyZX^)dNG2LkzeaN7Qle5ni4h#F2)DasS$)maHy02^5+@?DP%07z-Sj zdegf6hWz?jA(?*u2T*YRO`R&f{5ef+1i!=kFio<%8)LSRwm)AHtnt0^mNZ0Szt@;D zmr7G>$C{lbgnD@T1G8y-1le#xN?}TBrjvc41)7$$n>uHxuzALpxKf-!cS8z z!J}d*(4$=6FxfGSd2F>15>5!yG5o_lXGn+RD(VycZ}H+Q{@qc)*Y@Nqx{^>(B*MX(d zg$?1Gb3))L$|G6@74>*bXRCbTHa5%1xZS)%+jgAt_iN*! zV4Ao+NKA3);f8vlNH*WtYjnw48H=K$JytD=j|~F#71xC^+?(4LsE3YP>>zyvf^c z*KTKOP~Hpb5D>t=xa`$|j3Q>MsH;m$((zcmu2`Tsn~rFy?m0l2>t|PPigk-_xrLXX zq;UJ|%Rrh-?kV#Rv*i$Yqg9ALS!LS@rSJ?89Xz!8jm^$g7F3-t&xS_2^M6~Pa5!>D zR0M2{~QQAlqM_5&kPVZgAr3K12>7ah7fza>asa!nF-i(RJu{xD`g|ADv`6=Zhl zW=&i*6orc-l1xUPs*j|^WGjiFvzK!l$Hpj%=Qwspls9c1RydDXCqHviu^C5Y=0 zT1c(e3d^l)+MCdEMd>lkfx|j?y@R&2IY{+rnHXI|=YIF-(^Ek4&i&s|d#doBML!>> z(|`p-l}Rr&kjhd1@a}1yk7};lEqAH8um+xl?tT-_EfbXIArC^h>wP{0C83aGL)lFk ziR9Zof!)B6R*R4G{B*KnX(ZT9bb~<#?GhbrBXJ&xR|1Ub)3HWJ_KL=+dLP5yS3jndV9J>suajHrr{*uX zNwP!U|JagycLrV&S04c53RGetP(Iaol^U1m6fP~k{uZAG#XSjc3|(_^LgOp;d5d*M zBmdGer9yr)1{?aV&RR@_F*IHy1>bmdVh8nR(-bAHLa!FN3=C^-R zI?;5Mz^uLvD67v~^~fqK@%xfO-vE&DF_;2ICX>UE0z|iu!?Gb6)a+BU=F=o9@$8K& z%enR7*fS0;@B$UNbTNHTv00TQS&w+sBDgzyGc~PIH88W)g`K={>~n zSQTD}K#-N3SG>9UbD;Wat{cS%d1JeNcU4$w5_YT|fxN*H;&{?W1y5d=7DpAzpke-5 z6sp_N;|p!X_6HuaNe6&db^4o3poR1Fas030W9ekWOPp>_E79E>tneHWBTR7t@-E5; z!Shl}y0yud-;#TyqEIoD2}@sK!%!SVVEI|mP$ID!^Y$bnBv(iUhCndm!3ekL_xh z`0^$bbU+SSVF*?Tj)=v$=1SIyMn3jf((o%JRiePTuZoZyhz2{z&q$t5N{k{%6JQr; z>__qBqA^WnQ!_HTV}p=qnZdcVz#*p5+0D~e&iu5K=8!hp-6~Udm|Y_yR1h`PqZL_y zA~K%8d?9+E548n|)}Tj%hXnO5ZU7XYI03~W(G96e`5x86|H4T}f(Ja;p?TuN-Q52g6IIHRl{S zYBa)G=Wri_eL1G+x^%R|s`P9nVMe9*p$gh@S;Js@3M>by5o40folu@b&xdC@!cV$! zMCH9i5}IYeZ-nffD|5g}eieJ<8w@elT!rlZg8IxYYGYduy?$`%VS< ziQ*q7OqK~H92B$7It_#(>HC9rN7DdNeqBi^l39S`A_F{fL5qO*O_zmr4Zr1`@)A5@ z2~*ydvNaWgfDimNy8Fz^Tl3zX39Lc1-*Q3LwiEhV2{DMfjO4FmnccW>xLV!VCz(Xn z1HYS(q4MDhK9Vns-YXLf97Ho~10Z}g8<+h*=&=af?o<$=`?dg!O+!$S($$YGvrK^g z;pVEi@Ieu~PmbsXEEb%9coU(Guwy!bE)1Afjs!l@g5f)MQNTQR(9n?InI4+9GaD9(HdZbrTdwu_C zLrnc5_!Tr?!`Fz%DsiuhB8P-fZiJ6Fas72v=6O^(_8bPy9RQ=h-MaZ}%R8Qo;)+hB zni|z-)Z~PHcc9!BgDuj4wZG+G0Je2^p|L|T6;E>92IYdD(XTM!v;t3Ldd`-*s;fix ziZToK%&TtB zPSwWpiKq-iDt)Z8l~L>Q?O+jSeY$D

lI`DW)!>%SWu*U@J>mQ322Yf-AdA#s7)J z)@Eg+QaaSuh7YC_S+AJ)rUk}>jVs*F?}a?sM?zG`qz;i{SZTPf%=|JzDj9rFOB39u zJxR$W}!JOlu!g4 zWB6hc#}<%H?tp>W@&Ji=J=V~D(LJys$v`cHaP&#IU}u=v`uC$!3$!FG_<^7H3;+Q1 z)>=5FYUii(G)Z;Bki1F`IqU6;P5`j49W37Y5y`LO&$7a|W(X=Sx@Y3_5goCrVNr-t zN_z2x;^7P}D~&oQ$x7%36A{(hGKIJ)#GsvUC3;n%cliXS!NAF>x4OEzovwu`Wy)I& zN=W4D{uSw15#(>q*YBh>JTl_mn@!_n_ouE|{{ZC_Ej!zHl)MYVVM21J@ZLgS=MYs< zcALw-Lq4NDWddXWvVQ+qpHoFr;yv|b;k80*{^N*eS5ON$l4|@)9GrI+le<55o`(*e zwkcuuUUp2@&U(KwL}6N+*-mNh+cE<&k054Wok>Inmy^iEvN6xBc68h%@W9?DhX$wA z72z6ss_U@)Z~N;2-yB?bP@6EMVBknn4Uum2pjl#fr1TMDJ~|taMyf~s)_g)_zyAaD z3_>%c(XtxjH%mnpjxJ(v&>JHBJI@9`7~^}oQB{H<25fK41>7|l**xSNWuQTo8Z+^Z z?iVY7i8(lDx{D@~tqeZS1rJ((A;X->uU(}%wKAQQ}SW%8jpGDhYtP50M88Z{XkcA6o^?3q`wOr+& z?r}~sb?IM&$>LM5 z%7N~oKgVh0l@&9|J0XjE&uiVKNK3_Dc6Sq!8{*W%b#E(qi>d*e!h!X+lbukQ5yl#( z*ydb+6&MAa8_i2Se4NGWejh8=IJ*BYN&3y>I(nM#CWW*TTwD3NZr5f(#w zUIS;11>0bzb|psXiza*4u-yTldSn&M?ewQlR|EoJI`|O7RzXS1A5NGg|8>_nNO#uF zwI*IQ?dR3`$Flm5sa=0?cx@{|NhG6+cdY|(v8-~z6liM;`rRjj_D)Rm< zYmU5X=hl;LNlVdd$KK7|_+|NU^RKz zTe^H8T2JXWZEn%=vh1m}bD-%&)oVuTXO?6{0V$1y^6ssivsf>Ht1cF0?aH(nQdgqv zalYYsvck9(@!>wJ2qQG0Hz2v-7uUMqLd*q z;P+p8$NEeba}VmP1l(5B87ZY2(&-o(YUxg-x)JL-Usai4_T}+`pgK$bwK0uFlPXV{*2!Rp;lZ*q-H-x6R!<77loMoOJq!`-3V*S=zjX;=b`X9I^3PkY3-f^_DsK z6B9K3!VF<3%7VMH3J4zWLXmf_L7oKL{Xv5xFj-Yqu zr>4C~^V_Tx5rN)_&m!agkpdlre;@7#-D*ozVEhw~Z8NF-WXY;1Ch2Q$gQj{bkqwRw zR1F=ARk2zbI#4XDI#0oiB6ZZ6)M<3E1R7%0xtzZd=$(#!CqeRIFg8f1fF^~|#N6G3IBh+N_(}bp{$gH9Iq$EYms<@$ZDrwqfq?V61KQPptOwqM72 zV^(7z$k!zRLbulM0RA%stx!QHW(T%O7?U1iJgg8lgg`OlL5T8IpoKNjNo}(4tmNyO z@mE<@7HzhArtv^zje#HVJXF9H`+Ye-egSVq_-7in>ZF5{I=>Z)3P_6<#jir^#(79^ zfl}qK^H!n^SbZRQ5ut!Z78}~~`Cd5Dozm?qSapr+4+nS7Gg5KRFFTFi7!wi&mSGjm zC<^`7G2i_9HmMkpF$+WI%<&Uf21rVKMJmXt*A%)uM=A9z9;OY2+k_D{HP%uy;j6Su zKnWIS9)?0454?2ORieb65i4sxH(`SOrP|~|lnq9mE)Uxex>mYk3g#}VTkQYApgN4{ zot0xjJcq)GxQoStV>_J?g{`j6Exz?P#udpHt_v)RHHwU3S%<5jV9i5?T!Oj&`2Jn@ z7HGV`;wwcukGbljtQ{SZdGLTPtCN>~fKu}1?qg%h+!`iAuISh7gq2W6H$-A~iu81B zkuZ$TR)?&kzkF*m81koqAp2m~&8kR0RAf*JKz+U8lkRPHTf#}2u~kP}usEz1hXIUx z-gc641eGi#TDdnE5K>nK7(b{gL=}JCL)FsO9Uc9El;uz7ONagJR+2rZlE`H1YJ%O} zQ2+TcC;;HqKIUOEJ>wGv$N$k6Iz4aeD5v_7QZhc|JE@0sqyB^DZilwq+=${LWYXK~ z1S!Zf^hYf#YTEW%+*dAI3D2pG_;{kpTd?6X)FvlRN1Oc76^JW}(HRyJ$#2j9pa26v z{J%$)cq%^0soUDHv3?67QLoV)%cQz#`^HZ0fh36*FFhsLPHq#k4rL*^vfLUeR?HR7 zJxZJbywF|7LTZ0GZUoTfZ5wk{1+c_gZ5 z$X_VO1tuBi8rJ>gaQHQSV4natw(tcuVMHDGEu~hCBh+D;w26isUcAEh4M&+;o3f-I zRf`s$m^*7gDvTAL1<^vN2u8+`fs98q>}Z2iL05w|?fYmA+ zEg2~V3li#=G<*@Ip>S0MUUh@b2n3uMmx!d<)&4BqU^h1uaX*QzcnW)oP`{5`|NDC`^Af!hbA=xVsMDR%KKg}SFt-CeW8t0xDd z!!#od-j!o_Gs3vo?ohtr=WoW&qvAKRGM4~_)Jt1?^NB61W^&Rl6;NcV_yc5B+rZl% zN0pVLa>Oi8@jsRKCqozO_h{D>S)lbs+O_xgY@S>un!A$OSi;aD!Q8Jl#}tt?B0r&; zHb0nGb+`NJn5(&mw-MFUgGmy11v26?+ArV8xUaU5)opinm}s3B%|MSz<=5@*dpzhd zeoI9RcBHsyL+!Jz`+joJ+)@jQj4)6{XiOYgx`+C(QtnnZmrcz076xbSxQ}s&IDu>3 z4`u}cWvmUtjk>mQY5ChTMYZu3BsOLifqlq%O6sHW1+A|eBy<^3&&SAC5%VTlCDPDX z%XDh>bj5#Np0->yhl_0xE_u6t#d8LH4ne+6nY-` zC>(sI7VmtW?>NmfKI!@2ASg~sbm3Jn_#fZ5ctSQsOFPXSu3%II@4bJgy>~}TsxmFE zbIPHWuYOrKDA5ZX7T~vH@4RoKo=P;8^0IipO1>>og~LMn+{}`%RkuNNXb4^+c2IOi ziRcCcW5&e848}?QmI5j?%=AXAX+n=#mLCy7=^@fq+e$tbPiiZ%c`KJ?AM6xKLI?b@ zDvu1%TqA6h=i;HAw_|nsRjo$I;zuI=xK0`p6QYaF^2=dR&08gz?G@j?E7<%pf!jUi zY0k4Kqq<_mEugFGS2O8iv2x&YhTjPXm?RuF4)_oNkqjWV13^;E@Wd0l#{ zlbGQFe5YVJcj*!fuyo@Ej47&QWE6pW^k!wxE zn_okH5I>W@H4ICv7x;7bE7ysc{iGj7678w|^5voG`ZK7kT^5p~+}JT^61FNL2)FW< ztevgA=ef4cCrxs7{V%!t(;gVRf9#A8*W1oS>C92-Va73kNK@2+`x&#r4eGhtXl}>atzTfa_O6BpNI2 z6Z<~AgJQhr_?>4I?&I6@EJl&Wn7+_dO?fuq0rJGk0{(VTEso0#1Ba1v=W~AV0IG{&n?kI2uP*n z_K}|tJUa8~(V2?6O)wHBUObDYt&H0tC_Dew#O4huV)mQVIWgcWdxR`nEnq2KEg(T1 zG-F|{p;LrH+ldaR|D=~f{&odPbv{-$E>Ug;Ml5ReZNL< zezCY>+GMe{Y6LkF(lZRn-qHzAP_|{)c2i7$6H?M8^v^0v%C6NdbTvfz#_x%zBhd09 z(Z0JEIajYds;RU=?3-r5laz-coD5Lcl#CAeep2J(vCVzAiu57ej0(fojt19ZA(acI zK<9AUFXSw(a(aGwh96GfbC_ix&GcZ`0IHAac$SQU*_R@XA<74k=mDH@m(h=olqR}{ zU>zvazTxz!<`~8ktMs^B1pb3}+Hw`gt7s#((S{v+&i0*SF!Ym^kgZBUhIM`-sc_!- zzeaR9ShH$F)>|(CnMCse!_EcUq;rUEw%U9Y^Rh(9L$rOkyn>iT6xKyF{62f$QLjhT z+9~z7*Ez#uw<2MZ&HyuJ{6{w+bX#sfvWW2f4{Llk&N4`c2Qw2K1Kx-YsSHnxavsbo zQS9^sboXr*?Sa^EP3!x8#(!l3#+q(o`lFng802t9w|pzwC|H))PnIITA}bk7$Y_4b zzK(ZK$0siCvJ5t!~>oThB~5SnvvQ8+!#`1YuWLe=ce^Ath+; zaG}b8FjJAV?(MG_6dXoGn{CA6y@j@RS+8BN@WmHo^#-*zwJZK5W!y$Biu3Wt3&3pZ zpl_gLEci$z=~&PfKLbb-*sOG5>=)-cQ=SPa3B@t@3+SDCDC*X_vFJ=t%Fi;3#!0+9 zt;fNi9IBL+4bkU~Pd1o;w<=+O0<^#UhQnzX!oY_F@=qlj#hocwNE0Dk$L}BK*1edy zaMF01T-qT%j7h^~MjS+^8knnA+Br#XzWdB6QFhg4CpTN)%L&KmHtHjb zwBXMF_26wvqG;(s=PqwtpZCu>fXGT|2#y?Wvdw)Z`_Zwp4O^Cr7Ns-QUzXxIG>rfs z>w%TsE|VsPUUiEJhsE-=5L*tj$&iTM2+rwy2^@9fjm4^_E2OjQ5LPn?1=DTp6eXL1 z5;UV$S)a|sDU41r$n3HcVw&)i%FWcw(3WN>reC%d{1>&F6EF+KddUc`W>b=#=O)7n z09^g=d8N~V&Vhjut-UM~=<00-6s0ISNCblC@0r-DfwERJS*b>_E8a)oLrpq1T*CL$ z-Pnr#ruYPfP{TiS6nqCT31rmbWCdkbjmA9g7DN0IK-PH;VY%I=2cIB`9Ei3*qB2Xai8NP7^wjKdd!&B=5nYCu1VdN<;# z4P>rZLlb0~%hm|}C2^N*hw|8nl=fys?8F90m7TqkE$QhgMeDkiom~ax&awewxI&=> z8XM{IvDK*@z>jkJAr9|Fw(cnWZ*|*~0FTm!Tfkauz9?KOwm^ub{le|Got`71 z0Ng=PatFCUW)SOlB^;>!9A1qXbEKS{o|cK+CLz1<_LvrHhEByy%p}f()V#d-w} zD!BKtLc-UMNzS8~)U|7oZ(vTo(C-Ha0B#0w7foQw3?xmZqU9jlF_AK*rUV;a>`vla z4W>9p9Y-dI2u{=s!3@C6&xj8czL<8>b%6PngW<`@an*U059xjg!P?%5E$6fN%xU>| zM9(FX3eY@DRxZ0396y2!$o`tJ6XRlG$)hkc5}{AOfg*HTa5NAUixyrS8NTlM2FN=* zw$kn;;;)hbnyXaJ_o1}HsQjrrH`n4Qx4rT+I?HP!Mv|)kJxJU69hn#BWt5{1&U63< zI{uW`wlgKN{{bN*wIk3`X+w<=%;k_HTTXAsVfE> z7>%#(63s2-VAZPu)o58YOaM`<(}O(P{2}L^jL)CiCXf-+TX;Q|4)+@kG_k>p#ygXW zWW5i0{pQt9<0Ne3U;8V0KGJwf0ur`m>25>-mI_axWnyMFFo&IAk%VIf{GrI3Hw7hs zBgedyh-E<9mEPcDR;$NOGZIcL1+%^@U;ZTo^6byfLHcqyLj03ULZF45ip^!oVcdlA zI?Mw6+`cmz(PO|k97sVn;mx_#@^U^iZL53Sy>d%&MJnpK%(6;8r*t#}CZK$7m~N&Q zuhZ#DWS1?J4n?yFDc$b%5+puRu?|(jN#S`9O}rSM^FR(pu;DrTN45rfEbw?r`4FB1 z__U6eoxQ_G)?JyRKTG}q7o0)RowQ%akYsNOOyn->9j23rOER`DC_0Xb<*2}nISi8? z5_foT%YkY7+^t!=H&_A9YMy#D@1l+9O&QLXutnG}`j9cYAe~@FMdfyD6)ndnb9KU| zI!&jXFY))&aQ?04=rm9VvIu{J@Lk33;uC@xbbvv6JkVnrJ_WADb`6C~FrX;>p@pY@b>^T>L4$V+Az0f7+)jj3C*?|;61lIr7`{JjIR1QNyv!>_kiddgs8ypyzO zrvu?<4_MwLj{6To1IHM>aZe7SBt7W@Ab>};N8h$SA(DQO5utT^H51eh>Duwq@*uY2 zTT}X%oimfEL}+w?t9$BXg%P>(CL)sC$7@X1$8n+#{j*fk%@ijn*KKpr0IygZNSNg7 zkAFp!WIqLZj(+ow!ttorj^i?1c`Ro+$F!*tK(h0_B(K=1F&hqPKLcwt(aqmkAM_k^$qym#q#ijR(p0mG#ov;7yZ5eI$Cw~;GGw@G7@uq^LM zS87T}0cXG9DW~JfDP&Dz6gYu3?E*Ajx!^KU5V`E;3)-t>8c)kPHrtGUtVL?_-ckol zW~P8egSA)Nmx*4h*G^~?E-sIADYV_hWtyX&q2p!US7^j(6ot9Nk7b9&G`0?xaGYdn z5`n|;pv}R%$f`-kmn`gK=;vw#zNX-ln+46#I>5t9chkdlT&~n!69^($mOe#7g+`0r z+`|36i)pq0tk2tW@&)fBV_Vd`{VCN@1hR3xj2~>^*-yH$QCdvS=fU&e|M7TMsWv)5 z(B|^1RYDKWKa|2+nN|;fLR1cbyvxuf59z%_2d!PVbReDTQ`Hb;E_zRtp~RlYbbmlm z$Cp;RcT#%on9p&kck2!shA(=VGtj-i$aYD2%OKc~i~QuCooBsoQy63XmKhki0-x0) zUzgq}M^PyO#}~WoMHK`*M*Ogf=?)%EWYqM`N*%FfRpjqgFuH0Dol-QPOl!lcZNIZl z7wdZgl^=(mdlkk@IdqkHe*E;T#W786_3o}acJcw;i?oPB#C}ODh!l$55Eg24VGcHi z1a2^SYU%z8T#RzTh99(Uo2cxpnxSaMZRHogQv|cY)Nlx>)HxB`)~v%{;#oRz;(;S= zuhOdPKmFO~+Dm6)8$ISPwq$JR;KDpgFBa+|Rl7BRh--5Gfu1t(ssTFdsV`g8#dn?q zo)y%E0`I)~upN*(DUQxJXZQS{95r(Up#K{JsUdh-^#cZnN~@S>3_p73tocKnF#{u= z8lFUpLdSmoE-4}xby^GI+HM9*T_!H8hcg=R2Q531bb?QKAgN%CSm@XNWC^zYv6;`V zW&~ieCiia~@f|WY58agY49jg?Auca19U<$mv7Ay*AHUC3;J$KbP1kWLj2Bkv@lNrW zv{t|WV#H$}Hfb60ul+SEn6X>4ZCiEDX;DMiP?``lcDWs_n=XQ7e2@y4tZO=BA;Pr4(y#Hoi%I$Sx{ zr}t>m_MD&GMec!kxKK<(fWCU$D96Q=*IE6LQ3e^3Ap=Q#C_p$=KR4*M1K0hK*<)86 z;sRMsNILMOcNw74NkSNi-DE59IxwUXikVcQbXciE^%w2*f#bq>ngsqqkyOJ4S5v{wYFTn7O0$M~S zEiOUe{QnBFLmf(FrgIFsR8QIC+>@4N^+InbOp7LDzPCL!Pq6Qd%IO`-ESIpyw{Q61 zg(-MS?UY9~dOv~oUp2n&_4p*G_3D|>EmrEu=kh}dtT1o8(dMOluyKX*^uuLezgO`p zluyKKLD$6Ca?V}MY+&k{iN(3aI7>?vj+gUcY0B`f+j_J&x;CzyWtg%n>b&oiG{boy z;MFL5?hyl0#gts14&PA9yj;1rPr{Ugp%TDTjXOrSnEe!{8Q+d)l|IJWd)2rykM@^)PgL}C*n^A_r}UC>q;(1i9Ud#Q_U^qjgLVSM z+7=l2X?j{%Nid79nRz{mM2{@$sAf_q9}VcRd2QZWvrk!QNih8s3Hr?8Q%%))9ayWs z8AoBu$%fh1_Ac?%(wcXtkC)nQ7EG?Wka>OM4`6`?{?|Q@`V)(0VO3QGFDCN0 z@crC`nZxI5w{A{wbz2E-KH;d;@j_)iR7!lOnvIzGEMXNlh-mLa#0%f4c{AXH2tHcy z0vU;uTayFXHNY=BJ9Bisq#`lEQqrvojw^f?{QFTY7>UUk`%FZ{sqGv+0EWj7*MMSz zeb-n6tS4Xe!(|L<(6uwtaP7QAqxV2<1iy*_au(n6iyv#eaFT%`0UA0HZ%(z-E3ClLeaq^8P!qJyD>7LSu6SJJ72Et zlntbsZ4JJfK`LaY2Pr3#C0M8>g+?c2K35*jJhcxK-_1Jo==B$@R;Dxfak0VY&N}O8 zyl0RxZF(TT&DZeZkAf4F-9_Aw{GbPXwSNc~!mrP>O>Qk`{w;`TPaJk(llPMt9~_XSEAO(dDv6Kbkn-H9gtO)nyrPTk@`#?@jQY$j7;2 zILEnsalzr5w(BAKA!(gPUd`p1cb=lF14&*iVgmYVG;z2L#6U8JRRndLX2B@T?uHnB zvoxt+QdW^OG~IoN3T~s4rqt9u8iT+J9a|Fnw-4}SQu?l-16MrSxJU%`JNE|g&XbsoqOZPNqD)Lw4x0c(|L?mL-x(n4oQr zucfoNi@4X{bP8t>d&VYf0m@zbR2yfQ*Tc~9klA)syrUo511x*tIRP8rut(P-8}BTnSk3uG}^rZ6Z2iwOJofGC|u?bi&8+v5VHcch1sE+iVf? zd2}U_b#;uiMf#5m+4r3yOZ_i?N3{dp2eD#SgOm9euq6uYajrvSen1^_4J`Cjcf1D0 zK>q)bWGCxu?;hm`INprP@|fGzMTevJhpFg8ae2+Qf8*RsPo+5MduI`AY=Hr?((GP^ zMX0Dzdq`H>c#)VHTtxl*)CrGs##6F531y8p?xZku)>rYEd7|Xj{!-TVf$n_zXDyQ3 zrWBUU5`PpUjafU$rpm8DB1H zmC{nB(5C$!)TZ2#Y2t5u4RgKjRvc9E#7D}aYK=LE@_{Q=9|=%J^l*FS*c8z#XURB8 zx(*1luRu4|_b3Vxj;y7=i&+}P*SB}ToEr3wFybBi^e3pZUuG#aq}w!eAVexvW+ozS zM3o3+X++(A4B<8jFzg$g}D!LERv4XZeQ7tXjrI*xN){jB;Qu|yOMzbkNc@y>^N$i5gQFb|%!$)~k_4MjK-U#f=Y}Wm z#2%^p-_)c?WG9|a5e%JYXJGU%D3a_=(5awE5EYS?DG$VPQ+}};YEF8Y&9uv7al?-C zY;9siv7NzyTxnzs`dZCeW)Ew6J)9oLX(O#EfB@duQQiLcy@(HNK5=c!3G_amN4nURnCx0Ap8ChbedQRi;_k!V=zd zUxgHpLU;M-5JjfwFbLNM?tCGD+yfj=bsHbS8y?X?0HD}x$B|WlXR!x*+p}H((dY_Z z=o#T;AjS(+3v@+%uoXN@|4Q~|+Gag1ACYjVIB|F19s1Js)4V_9gV^i8 z;&6j7J8+Xr%>SzhBzIR4kE7Yq;O7)Sq>B5)ZRZJWdo~Opy~RYuubZ@R`eXZ5r#bbQ zzP&@lChdr@oK-%m%jnFxBe#l@!b9x8q)$Hb#C9Oe=IEexN-? z#OU|r`1*n(^Mu#Pgf`57xu}U zWJ}!MZO;eTK_R{I8$&<-399q5unE9|3w#~+|6&heLdB7gM~p)%6&CCvSvitl9IoZN z(Q4O2?C&W^vX1NVkM44jh&z>)D+ub>_52cArjSWAP%M`jF3oGfV;7LF z0QrIz&vdRJ)4Ak)mzL7mWi?;gFCQZ=YTk_>s4ydeYoLgRbeA)XvqFz?y<3`N{xkHb zjJqLHt+IyB`Q!mUu`1;>>1s{XL_z0yaBwQpj85eaPS#kRb6)|r#n-Bbdj#cZc5SSwl;hw;yIsX@on4e=@8$c z)NzF~76E%S7O&a|Zy8PVdC97YYhzsltfZA+4k{PwL^XrF)2xQ44RKN$Q1KvY#8yii z^;S73!RSo&h|Pr#6T(}Bh6Iqny#&5j|MB6uX$y%1$j2J61=%wA-EU3Ni7KJ$SF`T~ za^8jQw&$0sZ~)jH^QSXdA^bXawWL zDEh%^7$Dn7Avc)_W&e=ivESXnri7DKW`%76f=!OJ8gU4o`|QfE-{NWWgogO0sUy2J zg&QoYXZ}nSdaJe-{$=QXt~xG0zvF6WtR3unAf?LArBtctateDS>1 z6X9Xdb;OAPd~UIH6Z^P@h$f4Rsbs8PsMLFsFOZDpBmY$7G=-CX=jp#lDm`9iRLzqE!zt;+UaIZ{PG6)=6VE6;;3OL{D_d8*TZZy^X2) zNuUh>gJ@h+83n-F=AJZLesflpD>Jm-iZwK;E=6)G?$j9{TG9Is5UXHmwd?G%^cyYN*KqEFWc zIU!P0z-JwU1>eDc4Dq~1%cG~=c#p?-{VRluReoX3S#Brr-m1Mby9$17sw|m+)SV1W z)8F`k)@dG9HhtM@*Ew>gxH(3soG@uN{(= z9sIREUwi=fH`ayBa@}5);(pq_we)jhjNb)KzkBGWAC0#UBnWQ_Y=kb_G8Q$5ROZAV1188Q7s66kxdS zeoAc)d>-87xN;K5dkbmY%>MzA%&iI)rFQ#A0~qVf$xeChu}UY97$6FNCFuc)a#M8r zNydqOCX%cAd@|2VM+o)En`z;Dl03A;uSQV%-frI7Gw{q5#g;0UwKq7jjf&lgXzwDe z3kxH&^(T!FXfgYctTu4jt?=e(J?ZKo(Y#A^%4MH5VyMJv%RpQBTXS{%!)Ym5kItCy zu9zW~))Q_3StPH{!y;*FM<{g_t>gg_3*GwK1+n!n z3HS4a{~8C z5N$*xeHow(j?0!^$NN@-h{7Ne2{P}Qr69Gk(smXIXPZ*nc{Xs?% zfiUrlxzI?YsU-XOH$rt`D=|Z~v;&^eAb1X|%;L4Fxra8dAH^EMPER6I>6>x<@50K~ zRD#6el=Ogi0+LddoLOf`^?fZJo%TZ~>nwGVEln}}V<8O>Bx-%fOOt(;0QT1UU%cq2 zZq3oVXq(IT&*-Sgln#svS_d0cIPcqYIq9h1W&}@}5owcf-*)%M&y7QH$)9--BxKnF zFL~*(9BDW@ntjwk^Wv$+>d$Q7iqIBli^%mx!fsRXw(p@u>4O`rP_MDWwxXrptj952 zn%mt|!l%Z%KM&{r_%D`=FJmQK&QXDzb7{FbZHRMg|{`UR97CS#$@z>^j-k(Wlm;}T! zo#iiGenusq{tl7A@2vm$E&Ecc#K#F!$?|59*u9i~3_@YYic!?%L=SOjK15*S#96NC zBal!OIJlGkLDJ-N`@^wPhr?o|umi0QcUd!GV*f|fkKuwdi(azOgi}COf5K%xFLUjW zZFE7f`($9!Kr#M@vxq8yoO zprAZPu?CT_I>x^!Fv$oJAOj%?LG5S(10Gz&^>jCP6b55vQ5vhsmdJ@2cY|J8tE0P^ z0L@TL*T-Xsokh-J>F8=~roa|0theo%;*%KB!kB`icPWrMXv%u; z*N$8Cj#SeyNwh3Azqd@J8-9BzNt|XK(G!=mbr<%vS4BspdnuGbnst6zt{i9hk219) zV|hv9%@YB{c4&|UWo?6ijza!XucGX)GPJ>a4_XOBPzQ=ROaAtAQA(s$Xn!kxm#LB- zB#>vyFwqAVk9HQY+67PZ_0#4HSO|GGo{+p9GB(FTr|8rKdUp)}B+nuEIA?LLY7tD3#SSw9hpLFp9%H0RXSPQ#e~M= z!x{sH7=ftT>S_4Iz7+@OFsE$=PM+Lp{11Hx?UtlL<^{Msqa{kA)piL9qZ5a=YACj& zCe-5$c1p)mWsY;gT#H`AYl;fu&4wkXMHH{E5^7PA!U0BqC_*Il;Z;M0Z7 z1l~BDCCbU{MhmvngQgWwd{$JvBztzZsTtOboV-uo+)Qj`8BShoQ;(5fO=rlDBY6Pr z>NB3OG=W7ag9$-%H=iGo3ExUPsn}Wqm8xR45>%52W9$o1vuYV|vC0_*q$ov&5tj z_OvP|auYpVETLUw2>`Q+KTr}aRFNJ=8J`H{hI~li`O0VZa#oyzuqk~czK`AM87$)O z15@*9RHOvamrrveZ0MTwQ-l4$sGN~3S$)Og;PZ+#if&}GA0f+nkqC>B6kh}Ben-Lp#ue_6PIwF>!lRzmo za7<`ECjdV;`!I0{5t$V&&Nz1;YUN_dd#)G~r5X+wrf@s|Npi9Y_%V?NdRK4Hw=Y=` zrNcZvEEj&Qh@?6OM(i|PtN^MEmE*r#-S7NQH9ExrkkN|r%%tUKk)Y}tPaa32+~y?> z0_pPRTzmb>QFSb6)Rn*qJm)RajqZFK^a_4$Um%mK$f`y5(^?}XrU55R^&li^-~&s7 zS7>y=e=TNDh{UeF^-XYyyn}za%#bBqwAz&rK?rtA!H8f6Eg0-Bmmg{{brE;z!CCD7 zo>V7OiSe-y41YSAj-Tlg36!J47GR=ek>qMO5g2_+y$ZD@Xac*(j2^{{C ze~j_qpC{RpdKuW7^fVK7^6S{200^&O)Rf;;U05h$O})tjl_VhihGX1~ePV9hnW#s; zXaDb0*x2${;8iIp4V~2u3L%`=0{^bS!vzY`q>R~pRWEwcv0w)i$U_hr>@Ez}TDqmx zkC>psj!krG(^h*oMNzp_xP=K=04kvqTg?IJnMg2+@%T}Zz0xorv1GJVOhF#-&O`*8 z1=cgRWcu8^2E<##`c1FVAme(yt;Dkas~e>km=a6e&IW|X1%~Y4%GHcC#5S*erOd$n z0&M1XsyR8%%=j88+>@AoAPTAgOD4_~eg`QrYZ4nmNT8xWCZQ7-V(M9|~m} z<>?%hk6)58{pwiy0w)asuS)2cUnW_Vj4lxVfm~N?ao+ZyAwU1s9e9qYe}u;ZQb)DZ zptcW5yt2(HM1;4X?qbp74yjgNpX^$hRnCq8y4*lJ=L!2#>p|hPQD@K-XUPxjrA!DZ z@lo857Pu@p%tR%~Q09Zt?v zuDNRqz8nlEuDl%Vl$@5O$)g`*`G}pW0~mpGjfwXNT|)Y(1o?v{{<%RoY-w29&=i^v zTT8DiiFLR4-K^WW!G|hAr&46oPnB7e4=aOkU$NVg{SJ$P2DV!e=^7dxEMuTQ>Xb$L zOMq-r51A!*Wx``Ao4$H+T^c5Tl8JNsTvYRh>B<|a&+{TeBOb_im6|ZGtt}P27f~|* zV7x#SHOX;B4OTOWFuKi@(5jd8zA_f@^pc#kr*9%zTyct?D1G@*twa`p@jnMx4PUxL z(>xQ0`>6tt<59H$>GROa2dMP#Ciu6Vc7UI7OLXEelOX@6tM+fXS9i_-cLjx7uVQ(r z9d0)y&wLJ@U!Is96!{~x?9+J38_%nVv~d>oXe5W^Ba-ccoW#qwy=SO;8u*~i#`t-< ziKdus`+r}Lq-EVzvkQC$OY!D~uCef+b)tKgSJKE?aPIMi{&9~i`J53BxR@Br*6clb z;0YdFAodaia%skP$0FKl>6C0Eh}e)!YNo)M$|L@M)Z3sqYdd-j#G?@kDhg;Cr{&gq?@8k#;w!3q~km%A*P zf7sS^i(j@7T^L%W5q6dy!j_CcVK>L-`H6ap-;4gmQ%mL?k=xmSAA{VWw`R-} z>JTG3JJ$jA!S+MR5wAQ40PD5~!t@DVsllcjYNwYzZ9pO$Me`Oxh{Q{SFLqj9-LO^~|*#U>{ zWE~RJdb>xg(AY&@vkGez)XntYzd>;mz?WJiF6_%NhZUEx29hxF>*kaTHDo{pq&Q`$ zKtnaVSiCfdxizUg&#`bs#1XMHETWeK6a+`WE$+bn^gi?%^G)kNqF3m3MA+BDbGjgV z)BhWXWf~Vq8^7P$wTt{VA&0}_ zN3*&>8+;IF>PK$rWJq8lRlqw^1h2j=<$l@ALp#D|-Z^80mK>4w5(Jf1eQb!9hg;8% z<(ssAUsp{uQ6KKWB#Urj26c$(f7vFIMLMDJ$3p3@`&%!ed%wGjczHCS_X%<9SLU!j zq*m_P3~UeS;epGQ8yY*>Q~-BTu~wYC30`F?G4`!Gi!;4Ee|cR!TS3%8!*pS;!~d~W zkOR9viE*{vWye7HgceRd;0rfk%&DHEIDFND9(WJHpr0BK@bNr(D%I}4t0UbI9#yz= zN3MH_sYz^ZDu4n+Oo$B{rNcwEuF0TJ;SE<&Z1SPsL*&f<#LB&+l;Ed~=}IOR8(qf! zV85#cWirZlyx|dtjx9zQIy^^dL|&X!<55OZ;mR1l?Eiu zy0)%3rQlW&u(22b^^cjQUD5&K#`WhOQ9dEZZnR{>47yeHvTX!->>^-x=3+5Qh<#-W zI0&Loo%$Ad!@~F!+PuV|IE9|F?qAO>tzJXm*1b6{+&8{kXlxRKYsg{Ux!SLh*b!d^ z>@)9!XH>?|X{T@pyy5Hmu>+CP|G$ppkGh4*mjZO} zQyGv#!2aFPs;jhoDeFyg>XtuT3N*{SRIe8iZf(T5S%6C8znDT=cGA6DXgM*)!1?{= zj>Jc~cI1I#rvi|i#dvSK{my2CXL!y2d)Ak#tnpWH2+fjT@Ik;T$Ek5q#dBB(E(_I7 z^NwjVmmG;bOVMP>pl$0DvLea{2zPR>ABv{;Q|tXHN|k&(XnPq;zb})wPshLhYcerX zg<|@1;+RC4F9JnJPzjpi&nYc5xKGp%=yKSRm!xPUe^e;IyVanJyTEWqd#Jyn8eFW& zu2@~vt#w#Qe-t)iN#{8t)w2I5;^?dy-rTOn--Lw3>Rbs2!MD+B6{4n)Ht;Ezy+Cwe@8)6#C2s+3I4yg5;M zx|Y!5Ckk^_uc*n-P`6ZgEW{k52$3Ty@t6G;+55($r%oa zo@2+O(E6v0QD1CwlNhq-{40pdAkyt=n~0Q@fW8LbN0RbH;*KYf2PcYcQKT?a86w86 zS&me@Y5Tyl^!$KSt-zE|ZR3Ct8h48poZ|#tA^QF1BW%403t#e#$Z=x-%Wl|%IB9`Z)^lVS4XX5o&! zy~LYGVVe4V4UAEhL@qeM%+I$xi96q&N7jQzZU^miC`~6S1ZTpUT5o1oK2GIWD9~ht za;MBxtS>nz7dPy9kw0oWqEd&jr?sd%yV2G{;y0!hyv!Elhz+*sE0l|I-*rfH?0kdC zD??J3E%>~i4wgoraO2Kb!%9~oMs_!h7xnu-rv1)eSxcCSMw*E-Pr{-QeJk5KizqJ z9SDLSxRv+pr4r+HmZjbh_=}h}Qwe76bxZhwLc8}VMASOg%3|Q{=>2Cad$|n1l-vNF zL-#(j?^Xkv3Jv^<+VoW+R{GJW>s%w&4PQ*aD3hbj6WPSWQ7=*YWv3l+bl0rH_PUrP zVOfq)T&HkGJA4uCa-1n+(5kj?j_D2L0VHmMqm38_Pz=dAfdoDu`sJ}81& zB7jm{MRxYQHobmiaAPVDA6&h;Y1!EALS(KG} zv^TtcLB1Q)1)LoOAk4g0mgi7VB-1FMz)dR7^YW!!tc7}T0L?-whbso&~jkYO@x|gd3iIh6x=k|E5>IE`0EgU zqafds6uD>UN1vrqy%)( zK8MXT(22&W8aeaNe)GE)%S3l+6!9q?wXBLu@6rnrs?%)B57PVpH#KBBu@x$+gibY6 z!UgA*`jl^bDbFXjVxqzXeU+Yc6NrB9DQ)=Mz%~skbfhK)U@Cb3cfw#E0Wj(nHIUF@ z!d%J-(T_SP;uOO+Cv$0<=NvNRocL!*uP?Z>&QcukoIzp7d+;Ryq62F41J5)_fEgWZ zE(9oIaUR(I(Kw#1Gi#j-LaC#RN_!DOKl*?KA>pstf)Uxb^ta0#R$gw|4v756w8h54 zAblvt0U-0C+=hs8^TFZ;H|&p-Hmt9JVIA8ms85H;-pJBl`GScGJ8$XT}h2VBx5}?v!$(rtXacK(R08krjrmoezK0Qf|$E zOm4$L@Sis;Me6)y{!-a#sTehbtoi&2q;@2Q_%U{^o=IZJ#QGZ5QNXte`+g4uC(OIj z_{xxMw@xfbCqDbJ3V=m&y#?Cqg_p_hB!Qc(q2`aQE#J5l9Ym=ZX5$@oPnWr<#_dz> z7Del6MKH)B6(~&qNS4JC0%%u#TbJ$0?$}L-9T$@&t_iC>K=qzR^Ttf2T(me-f$)!z z%opDnVw++{4_9nga6_iv@ZgMwo@>3O(Y5h7tWjm=v>47##FDNYVfb#fnf^gF%`!`g zJpoZR{4j|r2z*Vub$C1(7ia8p!@!O~V`ut$MPF-oRwu-p6IP#^Qr;ead>YB3g|b@duxD1up}du-@2$SLwZR0u5Bp9Yrci1lp`lewAQ<$8329-XZ~>C{y2UFqC@ktzqAIxb=IX z<44m9y(fD?Li4iFAz3t@lNUeZIM;wzl9=xlarA?^%n#o4NO_3FE=tZuUcl~6B<5#+ zdz{Z-Ss60yD_b2Sks6=FpsGqPV(t4{3~}AnrS(14ZA*c9p40e0KN6}DUUM|aWqH9a z$T9@~3|#DIqGt35k=7NLgaD3@h|0aVCt!(%&&N^hQD_JimOPD}h;zvKpLdA~Jzz*Y z?(I`-`_edRJU4(A;4r_V>do};uE|=AF+3N^G39w(Hu4L`>(>Hzq=dM}h{zzV= z|6@F^@AAo~oB+tq#5EHU%kqfM5kcz9^exVQR~ZNW!=^K+R0&1&(YP#>dz7yEvxn-K zZx71^^X&FP^zB2CwX3i<$xfMic6MHNp?PmMLLdEgFl(tvrb&@7D zSX{DXa=S2Xb7XFCLORvGBCD!K8A$9K6^6SdcL*C8d~X2{=++G3StumCuu~Tjr5k7> z@iHsBSb*ewhSVP?Xc?qpNda0ylUp6e^Z|pln9A}q;?<9?5`qQvGxdS7eaN3y-8mh$ zn@0W!PoppR4eVvELEjqYn8)~XppZ^KVN)trT;MjJmsI9=fOjyN86=_}Tm?!2x5VG% zt$glZw4j-Od5E=bS@H$R;j{M#LC%T5Xp|j-fow9g|6Dx@avOXg;C+#U2lnuAy zpm!f=OP3^SQO`?G$*-6xb*klUqn@0ky)V07l<~wln{LzP7F#og3>jANF4$5~~@D*-U7N0||GUZlc{|1Lj}wU`BL2Nm*8!5+Ti zs2HaYh_It@rvSaeTJ3$wI5p*nBMyrf7yJ>~MZ zgn^Z_8_slDg6(`hkrW^vbLBB3)4TCDaxHemnq!Q{Qj<9G(Oqj8eF;tT!@$}u04H$* zF}cV>lYM@(g@He0MgV$0F?ojiPARE`6Fe!L^ovS8GpDm)XAR~c;1vO zhMZcod(nx#OrQT=a>_Ih#uI}*dYN155Jb4uuK$VEL$Eq`0lu=J<=gOf(gSdH@doh5fXetb2cn&~lB zTSpx!4Y4;pqbTW6?i`|*i&Ab`P%4&*0;)|<11(S?H*7W?!TaW*UCx6PFfBKrZH0Nw z^oYpkE@_xG=30}$l=XL-akUhw!Cj8#X>6FU=P^dR>RloQA@)l1q^VZ}`zfN7(jfTa z4*}ZJ5+Xb3WcmuEp-wmdB@iX5z>|s=dh8?H21C0J7z=89msGGnqif5=Yrz^{Q|_MW zRseWD)PYpAA{k(Zmv@GPVYCdBRx$_>(5;Zm!in<>fWYqd2eGS8tD8qiNBm-w!0OJ= zaef9s+Skj5M^+kGZ6v_Xj~W|M(;pD94sK9kG5jkO8JL+3RgkG&%H*ZVn;<%G_a65P zSa>Z8%-C>z>7JH`u{re&TH8=N;3GW|UNxcM^HMgIgjlw_pQ4!PX6sD#36T1N258&PuA@YvG0tDk`Z6OR?T&3A2>$k^z;n6;reB ze$L1_3RXXrau?||aeg+eXc7$S4_jG2lr=Y**k$!r8jjZ%(twXsLg>dUPdIY=EBUow zlUW9(*6t#>>*d~VAsDBU9z}d>bw8awQ5>++ZTpT2Tjm(bn4ds$5dSeD4lUXHYjX96 zb?6Z!VQt(DDU2+_$}xE|fWEexM%_tK+vTxqgFetH6%a%SQ0W*m|qmA0Q@$ABTzfqh31L(E&r4tt_1)>+WD67)Oge zT{VvSb8#)`??gYyy%OuTi>`z%yf>jqXNU`t$DqfrLQ5hwlt9J1`g$e;v?F`Z*Zy?r zls|rxb4kNJHW7*FW2oxF$s64m4gY!@dhH*IbH_{H(zFxY3vm_R^J#q@&gnEa0(gX? zf(Cle-L|<4d<(laC5sb1l$rVQWN8`gFssL7G3|XYGac?k9t3VQ3T7HyCm&}EKV`I7 z(2zoT>Ggy2{b1mgnH(H70yIufrAmtm3mOWXpy7wULeb&xc7D@F5D0!Rpu=b?PlTpZp2N)qAgrAK${42#F2xrmg?+QWgaj z@wkT}qYQ=Ro(d)PpaF`Ypm6lL6)S3`J*2N%t5ljUJ#i*nPe}F&CaR$vjkyt`7E7LF z0%Z25yH*3axZVb%D0ZV<7atyhxKi&-vYy(2!+l$vaU~RsL9hD&OSChbU9yH}dp%RB zcgpvigy;+{etu^&j}~ni&wx!1TdTR_gl}o+^8J8=3A$W969_M%L^H&QEyN<@CWk~~ z*N`wl*K8&k%eLTG6~5`2gSY-Bm^^F`1%vpeVml*RX>LB&x-ll;>P&AmZi!EoB06}4I)UF0LZZXZ$icW>xu0}z?35%;^`K@eb+N^B9r(u zBg$27BMF{UaT@vn?Eo+iWyZ4Gpt)-4Z}Fy$C9B595c9J(-dm71L(5mw;31^1l zn6Z&Z_?S#vN}9$e^R8nzXAaI zIu;LvZ1gD{Vw{%67tzq%6(oIrcFvexZ}N=Zc#5%xq?+F`)L8)$-rorbjmMP%*yIPa z^|b#0y)uLs6=_f*=JI z9;7IIx<-$>5eWC>ZJH@!pMu7by$@pm%%Zoo0m{hDWPoZ%VA?${9NHp;Jvb3{7a#}* zQPMXL6|}t3pIF^l`jK4Wz6K7w3HnjwfP!drI;y#%MIXZ?$&bq6Wad*lT>AM8S!|o1 zi${nF*5jVy5Ac`dwKWY?2>yBaFsWfN;l+p;5%`DjI-vD$OMC9l?a&4P0OE4qps9&s zrOV%g;A__7QcOLe_wp9IX0oeV-1K5@Qv+K%^y#yZKg*SS)PQ2Gb`e0-6l%n>tldoa zT^@>k%aD%LQUo{n{}mMri{_Gk#(bn0M!U#qh4+E2MdNAWZC=%ivp*sDt(3q)(- z#%&KAp0ZG*zBWHIcsq2%lXu&#bbkdh2m{>MnvM09}h^8Col3hHG zh!g@|N9c9pImEO;Wnc*j9Sx#WDS*~g>_a%(eP+Sa(;MMDz%)rpkir}Y-oxASHe+56 z8ApwX*Wpy}77s zVOTg>Mz_W%2?(k2$mgc9G)MMzFuf6TGpq(w|Ec|ON9)$GO@u8Mz{ z<5KDF9($QuNIjnMr#MmXXjHAEX@IM%X^0Ho>3)(|g_EuNm;zUiNJz-B9C z@uf1Gq&uRGiL{#fuXdjXiE682Z0G@TJcD}n*o280Gty!Gpslg2V;Rd^V4DdkX=-=Z zVWu}Rb-|g{lF+6=629g_ae7U&3U<9_nWVL4BObkmEQj~mglm zk0llo4$IEqVN<^;5PP2$>gfwkmU#C&$zo=JU%^^q4XryGsc=Acs0VZof4Z%i3(m63 zM2>;;*E+}dt6lF&EolRIsO1p$q{K*=p{dPINT;mQ+Mn&2F2T=)Z_{6>yt6D9NRl@fUw8_j*^O>*&$U zKqwqD+$E={6~;siR`|z|&9&vC(U&?)xOAlNdZ;*G=eT!3}!@TLf``MO9JPJds;Mucah?;M?;% zq~rNwt-fsX7rNh)WuT;tsLiS(R0wt^_U9NvS?HwdV>v6t;s!eyqv4fhA+IW zg_lLSCl+G1pu)l64IGKOlUyrU64!%7n04=xslyH8R5*IQ$#=n5AMRvgrW3t=uth^& zP-&Vivx}hBzpWLqyt>0|qxNV}bu6?}MIy&>1^WrfubZ6Al#}3QAnI<=vX&es3_9hCJ((d5Sz_xN z-*hzEH-~A*+h>ZoR`+Y&v(w@Shax@o5HNoRGQA7ERkN>cDq#n0Sp<&fxRWvXE=>k8 z=h%zzLIYvvbxmAHy?Z6}>%v0frayfo5XRjtC}W{8dy4rwJ&7CO^a%#P=hnNt@?KQo> ztJC^{x?L>w7^np*Qb|vNtb!a~88HXL=K|j{5NZAsF-*%(rDd4@bUgBcCy0hjjDwUt z)%G`le0TE^hk_UE@8)A2(QyaJ3r(hq%#*QX4ahxMW|2GeWYmVYZeON)K??J!T;{zR1Q^zs^QI^Gs`_Grc3(J z*b{O1$GXtrC@*nQExHPI>&CjD7EQ`@ipk9WuM?|;g6;6qZ(@CK<;HL@z8m|lpW+ir z4gqNrejQRr8Tj)1!8cYW6;<9Vvt8(`JdR3-D+YDsWME-J2goK~7kAQ=y?%=tdWH#x z6E2Qz_@F9FvV+T$WudjSYwU1O3R@X*rZZSvTQVSQJT*wurF?bcIL37qAiYM8Daf4W zN#DNVP4@Y;Q3n>P`W%a!#jLTx4&UJ40cb*D!A5@!q5)&Q;ElAfHP^htx=EljeIro*{<7tuKyn1rqzZ`@xziMiPjUy zm33h;HG-$)#Y0lrO@ayxmgi=&V~HRnu2V-qEMVu<>t^*<6iAYGKvySFJ%4PD4+&&$ z7L&u3c~dxmUWspkz6%_~fWd3th6VD(c2fPPZ;P`wPm!sQ*pJZN>|FHM4>EUKMap5d zGTvrVGt|&9UErl!;}*T4Ybm$-t_lhF4(1A3Q2gIYw~k7N9RgD@i4Art+nD6D(NGgx zKp#NU@^$!Zcuo`rwZ;I~bHQEo{23}Slh^xjTU$FyENfStHlBj}0KLDBX4^SrYG$H0 z!XVt7Uwt_BNYSf1p7?*k-?37v^%Z1Wr5*fT8>(}f^HSCE7Tn1}>CoDz%F!D+^N<+U zSf`{;Fy~HPj&>7Q83!(G4>(a#D8m6a^@3xD>)P}2CL36fxU!O6fcPN1fPI3`F--d% zK(nYanh5N|6o}0B;*=s*C{);^axL?gDIclnn8ijCzF-3sKG%l8B{9$z?^7qvPJ9Ub;TWK7YU;L1lhFoVj$Q%SC8J5bK-%6KE?Dr8oS6o~hyJVSKuk`k z$X#W*qHC;2FtbSW@OkCNSof&#v|CHDJcCNK#)Eo-`M@M_zRG+TMNYC*iKys=EiTtN zBX)5u&Zo{|cmgyTj0ptNTyEMSqj{~mr;ZB}3r*7*NZ=hmr>ko=0%KXkKFPGxIoCjM zhPiP30_F#@Hd}0p#=o=aYAtQ5!^>luNn-0xe|Mhg;G=@tb~CG!>pf|69Xg2Mk)L>j zo;)}mn5`fPk_;C#NJidee}Ne-PgM8?0^_nLFfaV|5TO+9-fZUHa-> z6^J$BtZ8GL`oF&#oVWUd)KCbpud2|jKJ+@9?v2Xk@zESoEb;M<2)X*9{nppks_i{X z?8zgMNVZtBF(JLQCkn2kq>Vtsi8u7TOZB5`#0&1i(*9C#qUJK-3F_xW?OfcXjx-ObzTGe>`!}mKo z(u{>m)#RybT3W0b8fE+mRR*qum$lyM86|Zn?yvLS_e(A}Y z8#*^rZQ!`M`II=Kh?@ADV0w*bZ3?}8X$N>w-vdSkvSE}9g7o0EMpc3YdxEI*QRt%F zT4MqoT~M9JPR#uhShQ^@F%_CA8Inkucw(b_lNxd13i|1+L}Yku|3T*sFCJ37+G$-5 zwI&=8mhB;Kpyc{qG!96*NxrZK_2szyV)AmMAEQOC?_Ag$Wz_@MD_Jpr9{pO`8)e2-r4vhBTSaS z58VViX4~Nbb28@WGR7>H0dO8=py~<`*kCN9peddIpc$HJM4Q`a)`%{szpWxrd0;`b zp5x()40WwaPUX0AOFT+I%KGYI_Fm!vh+P`5)v7n!=N#bE7n$F6k)L(+D+~$?EsU;& z?E`0^aV~(uTSV>*`!a%3f?C%+yqcT)!vvD5y~qNrz;oI&>~ui5av3i;&f7-vU;6_E3F`rb!9CXkx_1LNXK zUzN)pm&OePHP85acA#?n3&(`_F&1Kd-StoRqS-}dXF-7H9Hh%f>xa+K!m-xx0RASM z_&t~BUamN3_;Hl6q2L#zE@M0X2o4)aKnXMO>3LP1dsV5s( zK;C~wrPJ>(=o9rTho+pU;%owR?ph3k%iNCl5%V~vU45qJg6x#vlUBD7ViidITc8?f zA5$lT$JUn}^3t%bp7mO-iiS+UOk}%FT%tI{wTLWdaVcOZ_C=0p_5X9Y5H~O^XTw-d zqJ9pa>lk{KOsa|kNYX1&!dAm2*Sxmw%VWaYg;@v%pF#jEb*mW2`l%{KM%L|}TfR)D z&z9+ee~lWo@=sW0)Zng^sFCR|LEhSm4d|~E)^}Df6GDyoocIbz*0Gz!`~jhnyhpmp z(zhUU5QM1E42I4D9`>c(VCW}GYErX8yS|pd3pK3BUTtu1n4uZ})BVExwW8Xqv~qrkwNVj0%Oz`3nC5#%XF& zyZ~FtG#`FK3V(M-T_iPLrny{0SOnLf^{|afu^e)C3vDbv@406Y9lQ&$%KZUHYsNq? z6%whD{Kr5N@{MuTH&d6?Wuoasnt`To;2v0Fqd}?E#MTpmurt#{Cp}@wDNTsf>S(ZT zR4 zQ_@T9LM26n^NqAb#|p14BM)5NqQl_X85zuOQGHATJL>G`b?0`NXD-FbO&=xCNFSsM zYaG88IfE1^r{ab!d{oMGr*z#avkib%2|Y5PV?0~MyZ2B%s#>>WZzu2tRk;Tl%eh{8 zrGxdiY~9OJ6pxQFL!l)dzq92Z8a+@REUYdV8;&L&3UaDNpP81-0nlrQmfk7mlQOFF z7qWl_bHE%|z;y{+5z{d|yQ&8=P6{h)iz13i z-H>;c%Rb3|DXuzfu04NoOML`6ToN?rQ5b={^b}nlP*UTvkg~}}^LD{fOzSChLKbJK z|JyFAe@Y27GISp^{Z!?;al z$|^<-N4D&DPcu!p7Zdp^Fgzt{G5t1TFz_nq7{*PZK+RE+k~%!e{MWrtP5~f1OnC16 ztZgWgTD4|V$tnQxbiB=U=;Ke(TmM!D{|v!| zJ9&4klE+X*&uy}>pqlMH+&(>1DdRF-7h5J)SIj3uc|DIP&SkZeNZIiO68boVh-e37rEuFU%7$IVcl86DGdj zWY;d3maRC+S{Zyby3RK6$cN%^N3brZqBwgH!7?H{^@q#$TySl~B(&ke+06FFPhgER zc?QdHhx_;n0({z}AIva1J zyRz+G`*y6Iab!pd!;3FHX>R(iU_i2KWXWvSu%|mA)j5!evIV9X>6e#MgLwWirkx>1 zn^Hp7Vk>FX1wE_af)$}C=GBf?bmNXrNjJ&kdtl6;F-((@4BO0+`}`(|HA_qC`Bjyc zw3QKMc?@g$ZiTDoLI&oTG}{H6dD}MHTkMVTFwVULSOk8|kPg{D4+tKVkujJ@W{71h zyLs%cJ0@oacek}eMoR5+v`Itww6{tZ`ypM=;45+LC`VaCX<#3sfAmSGk@J+!+W=%^ zt5msRpMF15;WAYx&QGrKp?cmmWK{Sc^&y0qZv%*Yw571>cVGuH#$4yf0F(-Sy5qet zzB{i$U%;Jt)x1B+x=7m-7;hff@Gyud0#K-BOXVY!mcdI(or!0Ynd=nhW(9@q!USE&pcVMxiLSJ}(b)AQQ8=phn?K zB(w;_#yQ36*DG#eY0fH(($~U(3Xef3e0!p*O}_j{Exs|h#0#x(1!e7NO9hwV@xP)e z{BIXzZ#_as3r5~`Y-feIm41~TzrL9eyf`-`bDELEzB2=!aZD1Tj+ZtfIv{8_7{ss4s=K zW0RRHflV&;OB-t{ZlVq}t(`a#daA?&^mjOH*yufz{AsD{Z*@`pJYU_xAmf!y19Z^~ zn>!JlDJdM2f^ZT0{=&T#5zpUGy`~>~?MBDA^3oLJb^8ai;_UF_55Cb z1?X|l?Q_3x-1R}sz9eWfo;qmsn2Co$^DA(d?|j51ocp0BR<}9)WB}1>9Hle30x@#@ z(dp49*AQ+6A){Qd0k#rC~ryGG4`8Fd*Jz*x;g zQhiYW0L{B`EY*_LO#rpl z@~OEqxnP+z%D_&xY7w1jZxTD6$W)$-Ov5rmSL&kaM8Iju^VYzfxQ25C+B_j8Padb? zN*Ius?A@m=YCQ^^!Qs(twoIGD0c$svJs=Gc%+AQTg)wV;R62Gx-19}0-p^_0$6Sm+mT{HwEgK*AO-v$P7;ibop zarshbVU@n?p4v7p_Lc=aIr%cDPHv6KI~f%?bPt6E9mt@53sd%#BJt_WzoLpLn!k_q1_<7B^<&POptzeCEfS{rjA zX$=*q?UPZ)i|B;P?SXmYGOT?BtaT8tQj#IV9Af>r6|ZJQD(&oRqa%0ZSQFR$@&wVj zSn}v{mY;*r237WH&|$KV%s7)#qAfU-Cq5;|ilg*>ySBrkaiEbC2-C(M@;2&1(8R8W z1wZ~0l{N{g_wb;i>9=Qz$T`fq)EM2$XG(vgU1G=#CCGF7aF zHDpxJLef}Y#)a3n#A$1w)kv_ z{SFSfz7JPVNL!Zox}FlsB9XigiHGPK6L#YI>okqBymO^1F(@^qX82`uXvBy8%=5g( zf_Wr6)uZcYejm~>n*p3bn~RYy$D0$G@}JB)<#f_LF`uFgH}qgTGF83$n(ZD$O}#eI zhB`y71qcwrKTyXA@SZGs+AIaqW*b_82 zTyR6}GMcFaB;i=f?eR`M0Q?M4_I8}!LZr3WK##HQAbP#y5N3tFq~TvuKbapo22svO zD7&U#)ybfWRv5-qNNh?M=E*pww5w5lDxHVSEU90S#$&cZLZKsFpW+@W@MN!yAU8jS z*gFxBD`pb%Ba(0h?_s1p(M0<=vYkkf4D0IKNtX4`c+7W@GCCt@i9$`UjSg%JO_|O! zg{@R-3sqr=c))w%a)i7>#;y$u3tolbyG7=FR@kGKr5tJCnan4`d3@f5FWtT# zL6l2x&TXde|ix|%GweFeR zsaXru2EZ)7BICW>R$%+at7{@FoO~iME;8iPgF0>Y+|MH)xbBZq@V*6naPdLI0={lc z@G;+dr@_))2irY06x-UxeWGU|gV3Jt@NIv0q@d(-xc!3SXTKIwR(XZ#*%QBf`H3W8 zh8DKaD}$5h5(oMF_ig`_iIZCowp5K$~;ic3ZbMcnX7NF5oz*SdZCUVb~71e=}@s?teR{xaiyw+23VY z10Tt_Y%YG6^{DuK%6X@@v1mVIx)IBl?G^+Tq9uPK?;lNFpdwxCWZuT;zlGWtCALxz zF;Lu)#`O254SURrPzyET;jG!VEm9j#JxbFd;HS!wk`}eGVX0kuh_5gC5vPRbX-FQx zmANRMCF*6F8ofTaKyKNv z^BvcN!ENDC4F<^*1c|)zI}(!WDC8Bed$|C4Yt{ca@OUIC8u+OEG`8@BM>4ZkZ%zv# zkFRjb8assYdOkCj=HiQgJ7i>?Q`T1@gv)&6u-uUh`mh~;Om4CFp5-J`c-)tE5UGPxfmWsG<(5bBT@}NeE7>mHbNYu! zdH$h6fpTNeU5%FQk9+W{MmiIAff$bFa2s=IXBxuM;$wyDd;BGbJ` zg>(C8=ya)yf+t!8Ty9dg7s8h^K~?Vo-;6RJR`*6aVf@t26Va8NH>&=?C;SsIV!Hd& zv}I7EDK9s1HvsU8>T^u$*VEsojULCkshXntuO#H1DZiL*(lNb!y6O zA;Jh9=O#m?G>n74J6QKGBytQ)MVsvltQ$bt=J_U;#21dOgNIhqu=!Mq#;d#ET4`(u zG3vaaR7qJCdtO!6w&^jB#^eJlCnSG0aOO-X|3z-nK0o6^FzX#>TC7Sn1Vio3P3>4b z6^l0AD*Y<5lrDu#Zi{Nh=Aa;;lAe@0YTfAc#sxjItP0{W1F^Mb>ajqr$4_pcT`zm1 z;#ZeW!wznFtU*G{>Tv4lpRzP{WT5qHAP^Diljs>fkCu%M=4Zcb47y#J6p|^@a0%SCb;EV?+4|+d@td~@6hP-;7$RVg!KZEzH+r!SN3qoK=e4t znZyar{1oJ%+)HzTIB440_=?)8fZn;Ay7_yt%>v=?}7WF^I9cToO*^;+2f2gFP`)0Qjy*Am&2Lk12T zZ1}O!v(2*B>}tWQu2^!bsNI>;tul0jQ@i6zfaUW2Y+2movZ^OO`^XVw>l}~q$BKyi z^a!`|=-O7`eEuX2cj6K;mrv&9ud@``kW>vFF%a0APa`mfF%#q>3YOya?djFV3~}|{ zFO69+q0WGiEi`n(E_nT62$#gA#g%MpRVk?8vTl;Xg-dgP>iyh^l1P8Nf$lK|wlYOY zUo2b!EZ8te`<&(F2Uk9{_szMwkWi+@Tz#;_`m49R;kJggeD0dn6z$&%Y!^>NxbPf4 zxHdSEGpWd+D?m`KYON^zpN?xrSWOObyaXu!cet{%Yi5 z#xLs9B@ofzr#}Y^BjSU9bJV@?doidVaz8rYhVPdm9l{cF7oTE;u!JW$`>PeBi1`rd z6H(A{&3R5zF<1dBqGWT$Yinb6c65><`Gs~=PQfuHI#!UiLLzG3?kdZ!S zVB85JlRs-uZ^}Z8>q3^N^K#(~-6$9?N}<|A$;N=D{PRH&+dm&$?HxB|U4olJ%dlVU zK0BqfnXRxQ`^Ba9RO%9c1G;+Kv_`by6;A-ZcfHn$Z5=JT%zF|DR2m|rSxNga#W1!~ zQJq|^*1LsN{BOzkwQ<7Ld7QALIoBB>L7=5HV;8XVbaS3IyNkkN~XOb{| zPwueB*WAEx{k|783C1y@q%Q(#hA2h;L-X~=6x~spI)MtBpBL9b_U8jmjKX4k^r`nT zF)(>B#{CABfC!21#pyf8RkO4R<^&?6CqG&kh92pbZ^+%y3@q$%3$T(p-@n|osjAqv zXMbL+^$^O~vRdxlu%-GZ3cqWz*?&htjKHv;Rb5dt|1$1|kSI|4xcO)I0KH@}jMEfye?U){-U-&`IwiJoqHOq;gs(*;Tv_(mcb z#%_+Eifetk^iH zSv_z(K2GjJTPr(|*VkBgu%$TYB=5hk8FsC5di~ool+u&tJcW*=2^l6zp-p!tyxzMd zf&VqVz?9Fu*Wj1y9VQdAD0tU4Roex0f;nvJ{5R*y-d=yGpfzGRUMSK}Pv33}OIdl0CoWBYh}%ts`%}Uq)MxDuIAM>8fLXhdl%}kp zJwy-3@8v9}m~BT|+$9rd+|$bm3ev?>+n4c@7;^er z#+`#)lg_J7Xv3GQ!|opw*q>-cc;3JI1D_1vAB60d$y9Z;dyGijc|Dl)Fq8zKRTK-p zcaVrtu(bT4DsFTLT^iEfG@K9CC4bHI<@B|l(vqkGVKz1Qsy#ya>5c`!_mk?;E>4yL zL0WjUs)Nq(Mow7FUyyJ*k?^+7X6Vc~#oA%e~O?Bu}B$ZyS|g zn5sip%J53cWv<1rqeM%(Pv>JsZ*sI|!h>|y-swuWgcQ&<=8I@t+U=zI=Cp_&bHOd9+8Fudj??kZMIw!}Lw>R_vpjFmPTLP(ksJm9bZWwziAywA86^_|f!Rxafy?rK{z!Kb7vs!io zcUBp|!kp=-zLtIq^}WkObrsUB#18ASTsO#jF?!vUf5o8~hMQ|VFm0PF0v00EZ1fsG z+cfjqT$LPJdC9L!*L^5!wo$Y-m5KGl@@Acbw;xoHnA1q_xV@lxHF@L_ln&6v;rCD>1g$Bl){C-+=O0Mhi zz;0NGc0BNX=a&e)+-oX&pE@H|K{grBV)FvhYCR3Z;Bk^alc%8~#Kzuh3UfJZs!yXc z*=d=;$u!tP`E);2fkdbScDCL$;g3Jt;xP!fj$RK~9BuQVslA~*tD7X$?M!lRnOt@G z0=#BA3TH^A(>ya>t>23g2sQ9h6E>^2L~1FI9)NxIW#e!4c*}A^oEc?O{fQxuBg)YK zSX(qGGRqUB1TI;8;tFE}0N#cc^O{*p#Xq{^6&pAuTU(NghKkNqSNm zPB3&|UC#7}*f<^K4R9R|mLpJQA*__5g8F0xl#!X8hi}r4&OufhS9xxJPOYvZ_mg#} zu7u>A?lAfck4h$Ph*+RU=#S$y3ARcH7p9nCHcfl(JG{O&lB@~s$4g7jD~_b8gO5!$ zyq|W%@c~|h)w7<@zVUWRv6;Y{n&`ZX?{Y1#cPP4pEJdpkwKt0}JA`-4lNbG|jtf;z{Z(u#?2Xlxt~u4)Ra-Ib&`?#z?@U zBHH?#-9BG31AluL#@P0?{W(H#oV{cYWY7>AwbwYr{GPyT2o#f<^~ma~#@+1yAgI93 zuH1#673T2w?~csMQ_$RFggHU1!oj9jWgH+p`%?*fh1>7p%~PEfQr`e73_?uYfQPI7 zJWb^7irVBD;D-WlLpm!l-{tFOT80%FE_M8_^KDSQFrhwc*F9RhS%+=%t3#9-#MuIB zGE(}`;=vZyn`V8`&z*qA8udI+JUg<*9WFGYf)VcPo~3d5;n@%`*PUkUzUOPW8_N+Q zw3L;b(-G<)qc{oN-^C|cd!owlqrYJQGnat5Z-Zh6=3SKlczMNbAu&PmWLc#MIX|5* z_om~!3kq(`>zg@u85^fJ2!Cgnar}w?0LF>5`%4-vr-R%<3~wb;oQAmEwHgcIMW1@o z)QW(zlQDCG!wA7e7q-uA-zzTHtrLTJMRP5KJ)a!ACuu&n)j`lsEp0rzQ86v5)l|}9 zqU?VY(3WeFUf8?f5RB0a+XUFSA#g157u;j3_fmq%>+>1&-(XA$rAq$WB@Z~G_G&M?)&7x@qK#G43y{B0 zZA9U87$tpKJ|N$Dup{XGojsJvs?7WFF-X>k&!V$R{-f%$gE$&cSM_vSnQ5cP+b}i0 zwt!Y2NU)cw7!&SeD>?r7V3%u9A>btK9d}O%vfdfkjii~h+|%kDKaYfNDOrmIAFdf-!FZ16lZ=YT{P;Z?wv+_;6#-#MZWuTonpLuc7%RI~uh>B_O| z?O^U#NyXNFc;t#3az-od*3zxF3g5^A^}m(34_y`m?RMt^lg~wy1}mK>sx;i|?+RoT zESK@kmk^%)$2*=>u3d$pB+tLNHJkj-}m$VegF70`o6 zS2%JsBmP@u?%?rzmx+k0yA}JbR9iyiycXIGltbLc)8HDlDmhiAugbo{L?65#4dGGI z{mKy96JDYftq@is@8RP>rAo^3A1${)VfJe`$ySkZh>sI)Dz zyVw2_J^{xuDW!Mfi?sKRvZ9cb?qt5A)_wM|@71JCa=xH+YpheR9Znn_Q!6R4xRy){ zSD;)Q`0Qosqu<(mrIO!qO~{I{tFKqJUhcVH#{1X9s>{cs(SO;m!Q3~hbU4n5R^Idq z$BV4kp66hLG1O0MJYutM2hU}{sz6Ial~sz75(2S7VlienUkF`sNv$!wi|PCV3!Jzv z?ohmNslQg{5=V|XF<*?oW?}t^!MhzAz^K+XzBzoT!uBd>?Ez9j(Djh(NMhL=YE;aM z>)mqRDfg52#I@J)yx17N0vr2SujFRyhR8}sl^TOm+J5ydcC@^*-J~8lm(p`@8tEG* z5=#3gE~YNL4>p^NP)QJ*0QSDVbZ^h6rl^3YwIy{qQwiZ+8I!Y}ILq~MHJ9{G!}OxB zRAnFeFc=v$FjSjbyP+Z-Mr@M^(otJ~(VuGIfJ5ghrsvC}8vI=DuduKWCfhfSr^=N~h2-mIx5hORd*k_}4QUEC2~`lM6FA9+L3%w6!PgT54 zt@VAb2Q!{6pKM>NvS%9T* zlZYGhwj$eeNN%f(uEf2syVHB3)o6Y3JQ(nPbfTi$#F7OXUxxau={L8M0E1ro=p9)VQuLMLH*)hL6hu_gBo3F1fERu zFRN_RR&sZ;9=(Md=z8P(x#`rM8w&QN(4VY0sS#iLIoByJvWfp83~Uq0eh?Qo@$SsJ zp+LRz;S4_7M{o~lUzYB&7WK99so?0~Fs8GZn;Bkv(}yTO?uOpRPsn1h4HH`MlXHly;|$JA*5ySvFF+_Ij^+ zjf`paCV%E7?%aLV)`sFgf(ZDjn=XHoubZu0CF-i+4g-PRdp|7hu$-rh5#>o&6-06ik(x0f}}|9k+#Y4 zp?vaOtbkALK6iSGcS0|#pIMpRb#kAhK;6ntnXi|-=4JOZeUvvB7jnw~nvuP0%|n9< zmCu&KZx9<&`}-OMM9eqXuGpJ+e$8aE-K>M9NsRaynQC_+@s`J@0Q3Il zTLX=7lMSyAi4IMEoDhbKd^$5)^}#0d;VV?Kim8u<&q(?W*!957(eW^sx?prAy% z&!Q;a##6OPrTuBw^dBp>NK-6c?>_CTx<7EmZpJzBi(kDZkmfdJ+vHOES6OGq9<(aX zi;++o6W?Ml)xNXvL|W@aHL+W4H_PrqrC(mT((RiUA`VUi2gj8K8K=)Uo91fU`Uwk- z6a@~v`1UQ`>aBmchIZ0^5k+RR+eY3)BEA<4N3CVe-3Tx+RI{;-&U!Tj__GbE!{yTfDTEev46Yk&5C}eC0pP zFW80@IXAYOiJ@mq9IY4>E-}1z>G2K)e~0Z49$ndXD+aW1dSl$Ayrb_1rb8Y$*A_Z!slG6P70&cM-#o@4HC~^esyuXkWW`)^RJMIf@3Z*1=+6(6o0{%&A7LIP z6$Yt0TLP7#l2c=n*1k^I=N;`c4+rB=jy(*5kk#?29q+t?+cu!qo{|rDx%euC!|Z<1 z@lk=PO0_e)Ot?=(n5HRpWr>Gele;!Nlr38b+w!hYy0OuxCz3#ot-{J&yD`_EdwPY( z<2emEy_8i$y3#JA3Lae7v}J9yyyqgbVyAhlCh52%r_EX!dd-jDpG%c^sPUZpW@TH+ zMuk0friVM_S<@#iSAU5RjBl5HC_*oPa5Zi#jBzJ>=(Vozwyk|5K0+6e648e zc`@#`Gdh9r@%}VdwfNg>bko%~G)Ui>p4w7%V>o4me3L)@omfSanvwU!Xn2yWG46`# z+0`j3Yo3tj-r1L!9Av?@KI|%mpAUSnMd!6?;gDHy#T{)2m-sm2+t*fSZrVskNZ%}J zDB;YB4RDWHsU)$Gl*egdS*MygQp)=m`GwdmvBY@3T%S`7s|{sU_bVx5ZH>Ng)aphk z9xNJDXfZmgw${bQ|0!egud*iVeMdjtd?$HpKHu}yn8uFmNQ)ZBbq?33r%5fL(`Sj# zm8UWtzjeSQtXT^tT>h}T#V?$kPNY5R7_r%kCkX|)}d6!XiODVv? z)_?Md?wB+$%*8Uii&?l!eea{Hcfz`S_0m2WGa>?^`rYJG0)=*7hr`ar5Pr*mo2FN^ zirlsl)3kA|QOJd`zkpqZ!RH_3tkCJ(@&(v^W!^xP0N?#i;$F>7KH`m)+`oMabzxLfT^=p+bT6FSMgJ=!Q8Z!Q+)X_$yUNDjqp@6 z>RGrrc%EfDH11VW74zuWCbe3>)AxYsy>DK>=$lNqYaqDg`W>mx=TGfM6T;V(9PcbR z#4A@;hG{wgwrow<%|EB}!BD*eSER;=q0cGo9+-b1d*)(+iPij|yJdR4RJfF%!zQjTn?e|g-|9VcrXb+4YTtq2?&&$LgRf5KJR1Dma7)F2^U>~DQ|^Tm@i+fU|4Nz3%HS{0 z^KDWxY1-jmvA0^VSjG9v3k$0+A_H*qV`aBTuHR}}VYF$Ju9;PA--ate7eB#_GL+Y- zlW%EH?Jkn(*%EaObC^?4-z*Pda=(CY0`}fYc(YnE|Bf=k)*FdefUj%CXIwv&TbPNf zhxTvr&woWJS)blqa=dzcHWb&ypSRg1``oa-b>S<;(AnuZq@aqH2${G?=R?u#^UCes zfiIO={z4P_dGl%BGLBIbzSs=X^>y2bRVT-2(N`VaGnM(PAAWs1ehKa;r?;nJs?4Og za;A7V@YpbVlzHhkw!1EyEabQ9ZPMGW;mBva!urlaaD(}KBH-*DOj2n33>bMdP+#I{ zl*NgoDFGc6%0+t1hA!gM7`f!1vU%54{FMzpfb;VN$uV#`k2aUWr>Yx8IaFGZ~&$_&{i%x(}~Kqk&u`w}oex<7a;D zDOCTFv1h7pA4h*63`*{!=gG_0O`NDLZ9*Ko@%n~KG2-opj-=ZXprukJ#0`A5a{ zqY0~=qmT1uRvN#9k;6vDzuonaZXDH}-0!rbo=^YXQ-|}(e-)p&n%)3Qz=WQ?iPFU% zIv9Bfyf-`ET$3PSGXG&pH2S5xZ_b$%ZAtxiUNMKz2d1@C(2edZv_sA+>Al*dLpTC- ztqINJOMiY@x9!-vs~fqH8A4k6?QN-?I9;xG%a1;76;9QmL4xCFZ&S->7}w8*XSrmEF%HKDHhVRf zPO6-lG!S@cwuQLz7Uqz?6pyRt_>t?%y2%kq8gYg?JH!GJE4LQpO)?7y>iV~RYPQlVxHh@3Nj@sdU8fRIVSjesWq$dA$+ODo(!He8F&qJTA2ZZP}K} zvRQ?$<}dpyGVOh521x;-j>1y8JhSIk1y2jeeZ5<;@FqU}(aR_peB0Rr*g0Ol-GlhV z`y2`0GGd!G4;(%O!xT|B&%Y<#Y$R%7XL+vr9r>8`pm*gb<;P(j&py3{Pkc^H@Gw?j z866WoXUjT1e_v!GJw~rVaVUwjF*xRI*u7?C4t^D{K{}%N;H{_C32${u$LFu^YdIPl} z=ERe`GE{Gr8n*6Uv|xEGnt43SG^oqIAC-_B=OVRIZbtY5;5aLg5l79H{HV6`gvV^) z8Xn~>erFF#Y?4b2yk$^gUU9BL?yZ8Y`yt`HsJpSL?KHoTnC@o>ls9Kg-w`M;1@e8l z-aU!ry7GwV&eW^>>TNdJyoz-5%Z{Drx1T1*o!>h2nfGr0onbqPfZc9dlJI<*?~aJyXBL^^dg&H#4uDjP#9uf7if22=A+~)kv|)H)=0RtM%oCnf6-Z z;Ix-a_>;)#(;HXRE*Q)w-(TQfZ7XD?Db4R^sVVIoR2-doE4(Z^GcRphci;+iSrG+B zb+co#7*vWY)T1AEg27NI00f{O4v$2EKgsANGLZ;3_Hn1eNpv@s7L{qspt-Z?4E9i; z;!1_<(_9aGFpVf~dJ1H!g9i*wa-ldd0W^%wqM)R;)~NT~e_-=RgN*`fcbl&&h#9&<6RpgTZQUg<6n3lbrP)cH#3!t%=~L%(Rmtcf z53h;}(Wc@_wU)l=NP1g~4{_hjw}ek-vEOw#vJI@i>8lK!gN|LZ5uvTt>8pHh=ar{Zk= z8Cve==RLDiJ(79#R>>a>J`a5!op154|HxThZo_Q5)O+GWZ{G!*ypy&`+`UX|*AZVc z*OQdu$Gt=6nER|gCp}E>ovJQ;QB+ctYg-o2G#dYk7`Svto4aA9c?^enEN^EFSLZ-i zd*j-fh*7UPq5fyvW5pSV_3K2KCri^^&vo4@)7c&~u-$F-{K{{;0)y0^igs_BSU3Ia z@X7nSWt88C{IeVZDK`K2#%z{1AkC)d|J)SEF?lD?1_Zwu#7+NAQ%p|&-{uJ4BF4OC zMFP^_^!)qHITi^op7__3-3}qBs?H)Vm#8N*tL0u4Fdp?UaGJ}DIAYUylzEzu)p2XF z@QR#gs&C(Z0Rgry2(BwAP(4=AWoMvNc&lim9g-5Af9*=Fcz`@VVru`su{$M??L-0`U`?$0|m{#_En z{tVb3*#>UpahL_A9*BZ~YfzX}C<*x6mwv90V{Z9Du?s?(+vf1`8lS5M2BLA>S-%{?74t?IJd4=4TIu zLc!RQ{^iDCKQy6mKN%qukf+(}QCR41WPf2G`@}Ae7B{g7A38$pBY;5s;I&%3aDP?+ z0J}mU|9b14u%r9;3mUQ*RCExD>@ophuO-9a2Mi87QEgdJ(ZN8V z0R&WqkfA;SKoFMtNHl=_PDcaZdjxlh&*tFYHX0}OzG zEI@_^NDx5&m_cFykD=BPz=hZm30q8&kbm@I9^+2dd^`vuCl+U zbr^IHcOnsP1UF_-+?ei=3|k+#rV(6=>PfSul65rTdjJ;0gQ}_u*QC3#sBSDKz%Gb> z@yzn<`u~jwIvBs=VV|r2rhyR4Erj@Qn+UPoM#%sAAzE%D6ck`fUely|xUm4Jq-HQ# z0D3Wqpo0znK41hJYRh$Vm^77c<}3o!Ta8WlPe7o)gJ4Gs`JYiNT4@mlba(t_itlfEMB&BtmTJOTo$QhuW$FbD+xPud^&5E#^QJ_Lxvp#GGH!YsoABCr_T z^1UIj>@52`7A$T#9|FW-am#Irz~Z5*?f10=H0Ez;f3789m-8V&JO;XnmdV3Hx4>^S zBnAytQp;)Bq=iKQQC5 z=s)G*aKG4v#h}n!s0`>{F{1fW0W=OyrqfvfJL8dI@I7w!bby_Kpn(=(2@thVBqSD( z#G*jFHdY(2NkC!Hcr6?nr-?)(@fs*q*gvyCX;zDFtLaF!b!K|F0&tQBh*ifBP@o2g sMWV1E28~6av`Jc6oF)QCz-mGXk;$SkSnM=|0PzGg0wyV`Z9szk4^h`um;e9( delta 21316 zcmZVEQ*b2!(;f+nM3M>F0UjmEWtpz_SAK?KpAc*_OhV;QHiZ?S{m8 z_;}Io)Djkl3n9@=DalCq%D9`JV)woa`$P-1u4G#?+d%$keJbu<9y@aCfwLWbe=6un zUv9YD>5~9xfgEzwggK|5BC*2ymS9Z)^oQexY@(&=l@mksf(fCEG7)mLW(Z( zGu@ga4{By&D1QDRUIw4n0oDe!^BO+LFQ?+2qA3vA97$H+JnP&e94qPuS{NjZ7wstd zmvqcS##>QsHpcWCQxa5`^h&R2&Ed)wAA#qG$x4A<_>(qij`lZ?a&WtfPGp%)nz?BC zb4qThfQ?4qi+}j`qEsbk+h3<5ZL@47BjH%KN|8)+*!?O^wr&6!J6tW zE*pNpu2RcU$W2P^5{VV%a}v{K>mnD;8GY&NZ7tId2+nRfSpsxz7!yo$(s5OqK;L+P z9GL}leLO$nBBn_!<&X5Hf!l~iGITSzd^?%%Z19Hr_J`9pAv}SFjGpj}2qeznU@#(r zK{$EyHOUj!t@`x8Mf9LasTSI5YKdq-+G+u?!)S9)$MND1JAoH`=VF#GrSc4H&Jyom z1QETqpED43a<=*F7#0Vlk)B4VnG8yXBkaAF`|(7T_Q7ztoSYUp@vJX|_4#@_|aXd(*w(Fspz7>bBZFpj-J_F^4tAfd}Cm+T5a0<;LQ zgsEB;>v_eoPnVSXcEX_G-F{;VIXwe!NXR^J{TCHdlt4LY<}lq8Y>DoNedT1rl}XbW z>%$~*!jEdFyB^li%ovOpOWN`6kiNTJkTJk+d_04pKyJkm z2;<=`TXU%!5x2>o9vtb}sTCV!l(Y!^pnoOt@%0cf`6q{tal$ZP=zOSpKdK90;pEGzLUrEaP{jzY!IQR-mw3qK+??qFp z;<(zhLALG%Fwz>Rlp`d8HweP}QRhF95pDK`q-9EI4KJ;EwFWkh4Drww+RZEUx z_%>B#%w2lpIh3WFUUxx(F<2a(4&C7)L3`x%Oq6H{{OBU`ASsuhvBlxTBRZ!K;kr}8 zCZVb8cdm)mNsQTf*N^{pffMg-#RToieiLa2BxU~) zlCmFFlo*vIV7zyhQNUMEnO_ZvANXBuQd?bp-+n}b1hJAjyZ+U3-!aq$yv>{^cgHlM z{y1mj^aONsAyeJz3-)?e=C?@oE=MuP29Rw^IuTRozJ@22=XYFiVywAdHKH=`3IB-Z z7`#?hL0f|k(&jNVpWuC*)LD2N)iAEVL#jqtmT{boJ$N=7Ztb2q>kbkN1J~VV?vuM~ zZI+$OH6wH`;c4bzq%^kynQb8L%Gg%k0*;lOarynphGrLnoBzm(`XPELTaRlimm+IxZ>#CYbE7 zGD;IjUi*XxDfL^=vMP=y#&}90XoYKn7blncrIRhx2xy+}0u^9^ox|>7U@Tdc#ogcb zGS=lPTuU7jsB4o-D$XKmZElHLhB44u_=v|wvC)%qu&Aq=XhQJtGZ96nN$FUp_*4oASEyi{Jjvs@7>`_o@F@fK=&mut;@;bARmPR=LRd?KfN3 zEAk|#*)VH=gUwR_q||kD44QmEGErheo;j`3hkNp`&NN|kHZ5jbUSyKf)b!qXpqezQ z)wBnB4X{d~R-;10}FxvlAV=8vlM>gXg$$L!L;xDz_JEGE$1Rs>l=YdRd#iu5>HT*a#1 zL3We_t*|F)w42{H~Y?`Uhx7NeXA#ZM*omYe3c@~S&?k9yj@$pv@D zfVHQ;Ow{tgKYp>@;urWWg3h+Jdg6JXp#XU z_5GR#htU~qhENGRC(Qa0HZJ$oL9cq3YS|nthE(rB$t-F18-`WCpUKwwlE={BMCEHl zsr9KItCxk^GMix?KxcH+5tvbvHz$2_tjhXEKSQS`_V*znHCiZwR7w-!*_TT`b!>62 zh&NZ`i#9m3HYdA0@#wCNE}Cv*wk!Hbjl>96@s&MtZomD@%(@+NnI2jAJmEac z?3rA|UgmySWy?$TVUp~V&djkfhQ^X5w50M(Wy}Z|&WMlH!Qz@u1t*9>oe6sl2304058iE>2%}NbnLZ$^W=Dy`F zJEr9@1dRu&aIqw=@A{J=bV4Y(oiajBUS>%^>cwT$M~HurM{Lg%B%~3J+4)^={$M$@ znxH?~P%e~t{CvdzsvA4@*{y<^h6i9aQ`Tzk$olL=DgRblAKuKwy0w4d!&cdUr1WF~ zq7fati{nzwwUTw8q7EEPQbwJgmebx2o+om}_}peR!%URX+@zii)wPJvGql~Rmf%j{ zf8Vgwyq>|)FK<0`3nApedW_9_z-?p|d5Wp19I&~(Lm}L@ZzTEhA?87i@Y%sE)*7Ep z8^Lf%h64BE%2achv^CT@g%>t?<{Q-mo5#Yt`a(6y52}+l7?c=U?>?p)8m5H5V92ij zeHbK78umGMSTarvu;gLOXOkaPQ=hr!Y5gG?nlX6cNT*a2GpFNQ-cO-p5}VQ=TzeTL zS96W`=G4cH=JcTI?C+61eZ4N%wqsj6x=-;fN|Qc?B;fOiTvxYAZ%FwI=WV|XB(KLA z)5ia3#d0}U7E>}{c~4fRck$ODJ(g;rtq`D|fF@<8u;NRpE0LmBl@UfDit0e2p!^B* zqu!+|1VU4C$2KieXIZ3$FgWfC-;>~&Acg4}aRBuZk>KYUs+I2r&Ne_|>Feyn{S86M z>EP@eu{%XO@L#}cI6tyh0tmKK1ZHS!D_LrR`g8dbFVn&Yy&(@hEfUiQZUUf_~;ubXZD<*}^sy}a*f4)~qlnGqH=V}=RpM)*O@#lZ$|W^d|X zYz+_3PQpa;pN5Z*QO?ZX!qt+51B7Pr6J%hD2wc#Uuc7F$+t@Okv4OmjAbWsAgt>Of zf%H5w{X-X6Wp4~SeDUpO7$~?)sy=j?=shfqNa5Lwe}gSvUZhGPu0puEcfsiJ<%ZCb z+t0h96M1!!;zM*H`1Q6X_;vc2>u^K$_3;wtAWXD=>Citv^Vw;I?kbB@S{P2n@|GQmC5B40-M_FM$@Q~=(5U~HcfAj%_ z#n6hPN6eGd@1>~=XG-w?wG{cJ*rDrG^+PRK&eg(ASV% z4d&2)9Y0sCX?G5i3HpVjs%x?lKEhkX;BdR1**k4QaUySt60bD49Q+i%ozTPggiO}* zaP!4WjeSZ5=F*WLj;)6;AAQg9medB#*H^#g6?r5s#NOtV*L4nAN;WdmIw;y60Fx^0 z?!WYI7FZ;BU(o+`?M4|Goa}*9eTOvGslB4uiFvD;9<4`{GgDK>6?Cv&b9`BrC=KiR znPrrE4#y1)3c#ItKa{O~jqttbJb7Q-^^|+Ci>10>DYu^S)=y_*qlk(^2qBE`W($ss z*@=Ci23tX}AFkIS?V@T@2AluB1M$T|wwZdn?Dm})W1~33cW=6QtiTSaLwn=Vv5Xh{ z?9So4Hw)mW6q~zS>rIkKT3#`y7+$YpUNI-_8p725Vysn?NLfdm?M|txn+3c51dxPT*dHTZAVI;fW{WUp z<5%C$GX;YQBIUixwZBm_L1jvOFe0m@*1iFNg0iO=zSD)#LL>gAw1O#Q==mj_ zyhVbNvScI}OWy#6)#p==TkEG=>y1%-!{5%noqgZb9VE*ekxI%ud1cHjB+Fabr+8^) zR&8y-^7LX17_OpvpfoCDSBKBqrehe^bd zu%x1nRj&LO4pEQbA`#oLBpWZYdcgEk0C1Im#wOmrHMC;!1F`^{(??Uzo7y#65r;$RHSP5XH>?gYM^hr3~AP4fNECo z^`ET=PjL(v_;qj0fQ&VZ*`3(2ZaBU%?zh_qxxNRUPZr&m2M?Z|9~g#5CX~U9SNjhx zF*Lz@8^mqY@gNCO!mG~-%~&_o?rzY}y*lI|##oy50;-GhT>Va()h zW+h8Njc0Xx@?uvbo=14aIhxWqvT~D`SW~U25IJMb0bzm815U4(|L9Voq}tZGIHQ4% z1HPEk4+XrihgGM)$jZ^rlNmL@6hMT|t7zrwN zcMD@S_~)H%K7?xe1xN4SiYg|O-^Hd}G0u#n7VXh~;7J`G6pj~EQ+ZGB5V z%@ZL^WJ`Yexvk}|9Gl&0T`@JjQ?>c(;c+WUbwB(VdCEIMG@I&*x`VIs)65ylReC2~ zZBdCUplvBs$}xiuw3ES(r@)q#sg?o-nxEUWSZYy4I~TbE_^u-dMOC#cpZR;fBTqdS zd!atPxx8!4oNN~F4%FfdamRFc73VSuRU^-o^K>a&OfiOytq?W$ikaBjL1US+XM##K zxyLih^~7Z!URi8fca9$GYN4R9!t{r14wH>MF`70HfYP%yWVt&qgRzDm!6 zH^yc&z|nlE6SDBbceVk*z51TKE9+_@FjF>LbV}K7q8KSrPURM3J_#5dr(HT;_MKI% z#`Bo{MSC05E^&DXa(8aBA#fC!@!TnzRnVDx-YcqhlJS8+B=G&fd&*K|6W}RtP8cBt z;+QA3OPC#j_V+ZKIj%mBDlZ+F9oU;sYnqvFFAF)=>Sr%Toh`5%7)XY@P>Xr1D zU_Ka`O`{66xbj(#TkHGVQ*#HKn^|k%no$LGT)F!sXcDkuQlWome<%HP`^p~2nM)BD z;6I5l1(-DY6%w-03^?D9$ZbghtW=#pzbY8;ZE3uBvcss*AwwpH>6-jfc1)9TjZvQl ztc(}hocjoz7!VnnoZLw}Q%m{AOilwPjjTPrZK?+iblMM*4ne0(Oz)lSnpD8BvZNaA zn3p0xsSb1LbLj}8?`9T~l!X_l>ebo=J}O;=SkrbvWf^=0r&F=W4(I#UOVP=%h`Sc@ zR%q&w*Ca(5-*TiR_4{19MEb3MHCdPCd4N@@ul;#^sU>tj(Xst8-wEyt>RGoukcTpZaP~USy{*Yz? zFyyNV){4+F0&6CG-Y!*ct@nY|@!;@S_I%S&VN=@E!#VQ(Za%GtG3Ee50ZMyzzj|yI z6vQ(Bb;s7Y37eTM%HQ5e zWLtR}+@mk$HJ;Aw-2I?9nKDTtcjw(mNH6F;DKmOXqudx+AFqQQ|1qPPINF>xnEnBL z?!HZp={dKdpt;$7>BrD%z7Zet>E`Y525&jf`mP<;>6=aFe=PKKpko#EYOMsyV2_Ux72~F zz$<2C7e;;EmT6=cVJt|md~Vj3S(lqa{n$hq2_}BSU0X#L&*A}0uc8mR>lA1$rBE+A zpIcUqC!AVE7@v%PL?1MY)m70asIlA!S=dDw3(zYAC(PP%nR8R9$uyj95eI|oZd;Ck zU+*KFG(SygPuqG9s)t1IxJIdi;^a}35*?!f2d{ntQrg&mQ3y93fVjZt4f@T`drrIu zt=OX(5|h}&a?LJR4W8SNfQ&AKZsRKNz!_FjXnso4!jMy4eqtS7>7u38GCY<~66{`z z{0}4Go2?@1PgxC%U=9jxd~zH?iAR@L&$XWRhn6;<(}lp5_n;gaez(V2O8nNhmo?pB zO-6s8ZojvdwXF{gD$lpaFyIUN?$R?Q;s)7QZS&kx?r_h`S&#qQ#v0>Rcgrj0ou^Bn zfV~!UyVX$8x;57-Gs$qZTBSz8a7!TH-}nIFFsz!2I#~hB_s-QUk?26rgONOrgw$~` zKl4VzhjTgePJvrotQKQq%8gi3fB^mdzq9tv1K6J$1WeiGP`YkLl(pPH7gw99WQ^*P z2pceQn`?vLu-n(E;}_+H?v@FQUAKDUgzd;`uou05A;&(-swYqq86lj7Pxh0&*6L{i z^O}QS9{{ffHtQlNJ)7WLq$P`Uoj7VoC$gkSM+RfeAj}=LAx{G#?vlMe>J%ssO=Af; zS=)&}Hvi6pkJdNMt^Worn9m8vqN)+#g*s_U{Q7Eq%w+)0NY20uC%}Us%jt@6N?~L| zpRzhexhAA53#d!Stx4R%T?=q-$1@OtSIVPcr8=a_HS5T_BGNQJZk z9a_P<0BLj&<8>Rp0)`{ZL@tg7T5omv8q2`xTa#EjUbtFBx%JBiuz0AvI&U{=QHwG1 z68(&o?h}J*e2moVCmD&2!0vuS#Gsez)URRVW-~7ND=yuYEn>~NC(nY-?kfu5yF=WQ zPP)QVuPP(^P};!Y;>zYR-1-TLnw}!Ra0S5sljnZGq5C%V`l-~u^^r&ypTQ59(0yX_ zitf*gDTgRn){j6Iol#6rxS%Tj=Z6J+o0$!0(GnkPpQ7?zr?kSk@#98A3C&Eg5l_cQ z%dByX1-{S{1@|p$hStvB&utSZ`L;kbIsuwI`4j>_l~aHX!!`Yo_>@u-X2hXaB1TZ(`d9-tJIHsBrbpmj+T32oUuiGRW_BwA#-iy%*71{k~+L3RwPCKq?3kUcYT+)8n zsl0=lC9nHrCQb;>pN+1e#aFveA&!6}jJzpt%QaJ);zCOl!j0*(J=m(9uhgeyE9^7D zqpPwd9mFTCD(8Mx8$XKDQR9>&=^#<<%1s8*KHDS}5hpD%mTRE0P?3>6dDNtO*Vg0Z zdqbFTflR1Qos_AlE?Lpg<+hbB|9PDB2_f$FxF1c|2MA0brs8>!H(@A=a@7IE`On~J zzlr06KDD<*i8=GPgTBVRdJLC#L8VNbKZ}es3?$|;Ugl8{;9>8pQc&peIVu1)U;()#@(;EE~<-><1qN?N*@Qq%io z_~ND~SV*kd`<1%2-E_)tepz-^Y2el}{itwZtxaaQaLI_m98Vm>vLjPa6z$ac3rh)W zOyKqxap2dsAeFeYP#ux5F$5LD1quc3o-cYM9~yr@-AfQ%u`A|9eqo1KmyXl_#FHv1 z;|GMCxcC$TYi4t=dLT_O>sQQx*}ZrJo; zhri)E3#i}#g*=WZdD?vvM$SJs@l(e*uY)#)Cd22p6690xfM2#F$TLH?VzWee+Os@eD+5gilKk?A8%>4~ zijvLvgE^~1{FNWOV|t)G29=T0PyDn~!|{ywP9*DlD9L*6`%qk-54hMAF6)}zViY*+ zV&}PBZV~uqB#&kPS^)8oTB#o;_z!I)QI)u+1cbIjTG&_w1kp9oS z_sLZNTc!M)&>Lh{8h*JCO(c=-jDs^ikXld!)rG)>B6q{AM|?dSh{QEJj3!ojCWgU|`#a)P#X2oPMttfz&Mc+^J4z@`#C)hA;GtHa zX6pN66&cgt;plEBy72>j!~GOV%nG1}Kp_Tq-(uR>evNB3;*mF`jN}La4*pU4l}=B# z-<>+ihD88xkVR7nlYfIf} z{xxVf^sc0OSQRDil%BDP;DeT9$@9$aH{A{6Y$o{+TJ!DKtG40B7+wv7OIj_ z`#VWWmv@0YA)hjAI4*9KBA*`v_Yukp<}BY%s-8oF$v^u1=>9_uILz!J=_i<-Zx;Ck zYn?jv$81nhu`19A*8N=h=4Y z!#%osZ2e(rS?-^3c9t%or)?%wJ~?vq1ItogDX$qBuX@b)qHK04uUp$Df$9l`)cT@1 zC%WgGL8uQywaiCxVD~HBY*X*#$+H@~|A+i#El{A>_R5J=$ml5wt|zMZSzclE2^j(B zpy|gBICx@z^{C*tx+)xLqwS_SkN?!f-HPe{)9+)cuQ+CVm64nsn#iCGDTf%ZZaZsn zT1c-=qXr&fO!K@FjMYMzk6`R{BCn#QHB)8U?w|Ij>!}PHrb58ytGh$)7N4q-@AZG! zs6$MqyurBAiXUyX)|#4&N1`J{YYwI-+Lg-=HaP8sN7x&LPm*#v@?1lIDq7D-dJH%; zBoCb16-G+E!@dy&g|km+v6GoC(eszeA3e2v!4wv%=z(U;FsM2Hi?Hl$tQ`MWX1V@9 zndSa}Wj2n!&2HnVrqu>=EJIe6%Dk$y)yjtYDAyWnJ9Ee6jhmcP(OLf{0GHzc$;C* zeSbrD_^uP<-|=xaPMnR&Q4Jw&>gzR6s$`{fqi*G0mHqlG7{*~8vsx~!0a`5=QNedJ z{x#e4kD&>}Ka~iWM*;rfAjfC=Lmx(-B|WISnMg@c_v6p2m7`?J%@2O>ek7ot)x1~P zG*;KomqLM`jnXW}Q)T3pWNblGsjtx*wJ;Mc^Ti8)$&CI5CBBxoh)-VsdS_WwPJKNN zNRk*2N&#p~2qX2#zM(vn7a%RF7yi26PDLw?2KK%(O6FFj1(s``cQ$3}{cXC7 zel($j+nk?fhkdd*eOa~u#15^21v;|Qi>{-oMN5EOlHZdSw561thO$6`Y6GI9rAf;X z-Q5E?q7+{Whv_~^GeDePC527fK7s)qYtuSUB=cgEE%fpznNf>JV5u7ZJM z`0gynkL|>+&VGC`rwAZ2WY(L4fxAoU9L)Eu69t`Rl1NZyN0P@ZIxpXi=Ml%*^u|s^ zo9*>r79T`j`l4^&Uc{*akr`v0XIR+{3=iU9OrsWGf$^$zlq-P#92KiVvbPq2BOLx5 z4Qm(X`+O!yQdF=8O`M-33g&6SLLNiDsN)$83!=Bu( z6R2Qtf)Axsj{IHi7DkzEQ_$I$mr!Qq?4NSGyo@_nzSX}2YhfcmX%@2Syt47!k=;Ki zgkOK!wfrgGIEwwX$dM=mA6Rr0YmKBg1!dffL`U&3Hcgs>SY(vX^GHD|fk?yKyTwSu zd(w@sw<$8xtJ6SBLolf@#7)07z{|x_t6BL%&?=1f^zSRGJ5(Jp`Elh3^=l><4F}gB zMc1Bxw@|F3X$al(<|6pbarGogT)i6DlT?lKDcm^s#m&c#1?F}p6vtM<7eY9FwX4GV zYbh%vEPjz*WalzycK}xidcB|X6!(gDAxTQ9%AAD^h4w%g6viq1S-`9ZKb$=rSG;Px z6rLJ#d_!$JtC4Lpqj{ZZF@+lrD~T`jcqHPhimt_|El(%DPGl{HVmm&6)e_%H*i*F> zF|u|@+-saKE_s%(-K6@!OlqIHTM}*eesQ0(;oZ(ft}p%NKKrnoeD{0%iJ^^w0m{fS z@iPzM9SP9a3Q*IbeD5FqkrU;pljV!~jPpirC~zqJysBV;IKqe6Eye4_?B!KQWCGkj zLxv3Osy`%mZ^d`m2(9$8BH#!oqhgHl&|4staqnS=Fzw-63Ot`QxWQsX5s0KTL`GIF zVCT4Xb`MAAZ<4?~xMWt^g;7>pA4{nRX=J2o3;>b8C-XsAZ<1DM#8GFprux!*d(mH@ zE<~on689(8z{Ul1#AD?9)(1Kw2xiK5@a>Jx_Zxy2%~+a$h8KHeH{l0=%g zh2v>lD-b1!VpiJK?$a6&b&ARgxX-HbGPc!=V!CXQ{88mhUeTb&agj8?mTRi&fzjvBkEU07p|6LGhpBt zFJXpro%Zo|X6xCtYQ<1Sz0UU==T#YTNem-yK!aeh*-c2|YgAM$t-%z}E`RdkkE0MZ zR{*LkS$Z?3E>E(z>f1*h$GM-@Mvw$PT(^mM-CqUuVfTl6z+CYuDb|(~P}r(bPrdLj z9Rk=QSu)qHqTSGBs-imlwbX6;)UspcRG9!XjN`h(pDIo!;jWriKDLbK(r@w+Ln-*<%pXQ z8qVtUJ=;;Ed!lpoq}=iXSY7HC;Vc9oZ>Fe2YA@%1(!%GEMdr$t;S+m@>!)XnlKiF? z>abmJO<0+;z)RzV75JE7DI3ffh|+FQx0&zY(vmZsC}EQ{70XcAojkTs$dg-kq*45vT#E_HzBIY;I;QAoG2Ey*@bbqj$26&t27Q5d7#a2Mo0uS2lubW4p z^Mi*o<*ACYUN(kQtObf2!R zkL^y{iTZc@u-`cxY*@JwrG0CC>hMz9yzeX_OlWfJq0A_0rI|xVFVO1_svnW7KoT5`wN*xa z8&yO3t_?) z6T573a1>%o@b(>p{{}ztugn&(fvN&AjI}8N|^~OCENgGh~Cw{}!I{5^b0tW7;UY|*wFXbre zNgkt%LF9qJNqSk{NUe>m#9112=l2Zp2d&(HGIN|oB&l>#F|3-h)A}NYi*#q}>5CGG z!ZMI1;cS*A%Xz8XCU9s7ll*fSMTa>XxkAP2d(wX!bgqk*{I5ywZ2QR1Z1}jy89mu_ z$320b^sq-Rd-*FZBN%@CT!}H)2|-y&wJ8uN!8e1QDHto}7X$AE2t-x+Hzc(7Lm-1V z0dg0+Aq-^h-H|ruaD^IJ*tH+TUb}TU>l%JWSqK10odS`n6jd3m0+BA$Vbwbwg#$>e z(-lf*8d?yKGRY!pu@N4_d z@MaJa%b7(lUt|vn1YkUn+BbF%Zo;(G*$bf3zbP_dKOpE5f$no>%F$VJwiY-jn*!=w z2b=H@Y1WEBmlLt%uuJk4_omiub34a}SWF6!SX-~#^?_Cc;BTP7O8ISAAPaB=Tc9Oh z=b~8Rn`2(!QijxC#;~Z3JO$e9Xx*ad?haqkU=vY+=Y*r)8EM2fV)~&bqD0HvfQa~| zv>&BJ1+|N!cn8gLcing(vGKD$VW6y>)c!~OtcJ-f&gZ_C4e2OsTG~Ik0BA-2hj>%M zDyDodc#s7+%$WNj{!paKQ-L3Iyo(6$(I~uYo#^Z9WSa3*C$A{Bb$x+ShO5M^s9KC* zYAazDdpfc5$sb_mCH?!n>*I0hW`NlK<}nqhR5{|x*9?Q(z;5WRA?t%R31r`qZAh`j zE`WDTCF`F}1<67MK1}?<`8{Y=+P_uliq5-eX^My|NM{FvdPRES_7gBNowbQgn2k4O z(_!*&Jb$}?yuBO0wZR#%Z>#El*htNxuYcI8XZJg)Dpuu4d43}{-2ar~s2Ps?@_H*J zE(&TP{<^<16nV$%39gG81p=#FUtc$#1%V%N#9!C97bB!}Q<;2(+ zO+gatUz0LOJM^_b_wf*MNrs_3ltZWof4RUKP?n<73U}TXDxdwe*uIH6zsE+=!PwwG zDb*4Ro5@8n)FKZ+HHzhm6C^PzI{l_Jyxue{ zw#H$?TmX$K=1eWhxP_U-YW(M!GPnJeK^^&_K4HNPU_%-;M_qMox>na3kuRawdHn;(;9Xyl1TJ0hdg94U{v5mrhVq`C(Uqa5vZq{@Avkh;X;ZX&hHi=F| zNTH4G5>s<93%!n%p{|U;$-0RabkR~VWjE4%0i^{)Me>6U==*(M?P;4(#6Ie9&fpN@ z7yaZws)3Rmk3ZTU$f383{f5}ANTE4g<7O+ai*%=n#;q%hxg}3(KXa5Xp#9a0IgcHK zp?AA#w1Av#_Pu%T9}ImI2A5ot4M6u7&tuq@-vQ&!)6u3Qm_LO>@6;dE{y(OSgD;0@ z;580`01pfe47+I%c9sa-DrYc#`G@e?o%wX*U<;MgK9F$ZuqH==J9Q<=yn&us93BG;+@PtG;hcc_cp-r!&-< z)++f^3A;P*<(XNb)$5dXpH`lh65AoBI8uZ{x&;lgOb<3Y7Z%k88kC52Sz zX~4&=V=)Pe5|0bH&Ig^y(IBZ~!Z907x6M(Y@8EzkA7aar5qpQa*M_g^dtc&#M+_!WE4H$DTNptf zH(A71I6o4GWd3FMY!C|Z9sVF~uS@Q29qgt^{T9W=!%gBx*qHNu(_9*3qK{F*7}#~3 z9OIv<>8DhW0d4&}H}cD9 z*)_Br7XRU}<*)!_J`{y%-&C}w-C;{Ld5@s!#P~PErFV|$CQZUD`W^&)>NO8bCe5lX zF!zz2eH#?(9LLG*5fvQ95{Kf8I;Ze~8!a2BZgS20YW3C3QSvaO7t}OxVQ9uh^VQ2O zKP(uuSq?v0d{9~O?0#gmYI}=EcvO9ey{FKgay~TdVel^d$srqVs*AM##G;+IWq|Ya z^ISR&x5;Z(RU?f%`xP9-rR(wWiYVbvzqbtQ#dbIMdL^Ssh|B}=W}Uh%YV!vAmRoH(VNLH+DK6=;lU|6G(de-Erd(aC~4|gD*e3WbH<-(mn&=Pa?VW9zkY=w4Ca}$-Bi>(3$=&mUG+`jzhIx=tf zKg<;!VedaUY`9Lnw0cebxn55?O6fu7?QgRTOiBq~YjHGi)(IZxvpqq@54!0qS1krT zv9cOJBsUOn?_LSdy}5t#AyBV}NHrzhNGA}$aya!)Khm^2H!Tnu6x!nAIBHsW-ZwyF z@*OQ|G}Pprc&Gy#li*;`Kg~_O#_iBY)4N^?2vFIQX#(qLFypIq6|@Kt+Fgr?ugPG` zd@NqjII*dGbs5au=51&yeV+H%GD46`F{X{tHP$J2tmw}A*s{2%rq-NG&#xl{yI5Xh z6>-#kui;HRvT{dl|4K0(sJW#Eiv+ch9oJlOzDM5u9rXc93a*VNA+QKu%1j%uHO9>S zE8^hQO8Cfd(!OwbDwDkko#q>wew*jCZyXT(eOS^d?Ttr9(LAnRcUR>Z$KpYeoc)6n zSh?v{uQEpmoNn_QSa+t{hk*wpE4JN?NtcUDPQ|Ftk>hVG2&I*<*>JwYbe;?bkPoTB zOMRbp^`!u2r=tG$K06pM_!Q9V!Gj+WsM2)YpfoEw?^(Z+QLuxgIL-;JhVRy(nf|6a z(sowezWZZ~PE{uk+{O;oA6ihO^{@3hXsSh2;+FtbGs=n?)c)bjgKyYnp%n^)zGwJ- z;1O^1f-9HPgSM@{>}KEFZNufJr1&F>m0Kl8^0WYO`cPKT;&>`W@KOd}^rsKm4*K>M z*dF)QPKzH{TK~{j9H-r%y6TK}Bxlk(zXl!}dV^JLgN|4zHh2i~)3IFPcn+G(AEooK zOU(3Nm)SGWnf1iks}GqeCvsw;n(X~&5_>EUg7O|7J%=mM(h#+hV!@-w5!89~d-W~4 zO63QJ9?f(YURB@Up$VR>;NHy5GVJ~|bfY!YS=(IE6| zlp64@wajf9CON3=Fu7q@tabOoV&QP!gPjX7%i>7nj_NdW!6$pEVCL8jNXHS?@$!5 zUw+RUx@EJqoO8wHb0R&MPAw==gn3V+(f_1o*`s@kkt*x#%o%$4b(hs`(3_49Z<129 zd2gKzUo!wCUD^7lqIm3YB(1T!M)LT{Dq$`RGt}`Dnl|C8`bSRH7Rq(j+KQ8X%nKzl zJtfXo#!p&vhk+#{MxBek&cR3V@ zOcq=eeSFfP@lXMX(&mY%bfl!NRNhvhrX<3*Osr_)%C!YJsI;%jeIr=Km<5X%)@YXE zR!!m9FaKn~tE*QU#gDrk8oHyS`fn2yP>mmjz|Hh5mqi_)i>4)mvligerbXg0ad-e6 z$2Ww4$4kqt=I6yL$XEsLQjQU~!M_Oapczwz1}!`Hu(|A<1nyB*uQ9b6sIM!*K#bWP z-gn_nlxx@*(9afUx9#AL?-J9>m;L%~v%HVaSv?;2qHc4?h6^7+>QN=&O|BOy6|wGb z^7rqTK^?#G&x0px6SRJ#k|_C@*-)(4H?~*{Zd=nAMm%;bA_MktfCr(jGqktKIzn6-bBEVgWF z;|t89!UBl+(NcH`D&~Lv226*23(zK_x3i33#8?TjE%2g&?$j}|{e5_G(?4nc)@aV| z;A1wB%1<}KW~qUfX(W4o(-15>uM*@oa&L7lqrzp6cqC-x&Fbabr15>c zgL=0s^X&-}nQlD{wcE0V(2^S`JO)H*Oo8!VvO$L56eK^d*M|}M$X(?o?WUTw0N>bD>P;gqY^N{X*Vmj;gfO4dN#Tq0UI@g?q4e@U4bw&~&mX(A z7SncjWLmFNxoP&|aohkW^%LcyhyM;Gcjm&*uO0ZI{|JbKi46};F5U?>Z!?m%Y2`bR zst?H6s}D>~OBl#0!iCBsGmlV7yQxJ}Yb+1~!jqC}P7ZYxjHBPw8~3u)K-(|q(a7;J zmBiRnr%Lx}$;-su{=_|?$*MRTu7XPv=Us3kG8{~O$J}RKrYnXSCL%+!-q6BGp|M3p z%Uy8G2p?t%3N(NpXXYmJkdet_mOGLdC=}<$LZm+|*Q4(W&qSp|#CWdtiq7v-6Yk4;n%*>mK>=Ug|yPrQIC%+23u=UWu_v+{yjWgyf&h>zsyVx zMPUjqx37#uR2*r(HiF|sk{?r|!BUX`79cjw2S=Bk_#nf6-xL7R`x_zE7B1S#g&G4?kCY+zh6stI-P;R0_($Nk@kF#mn$pboB!WJd zh@4(U`9V3pf#phR3}Q0;(1-7eDkhNDP>DqL_H9hAvNq49RK51Ags>F_#KE!>KmZkf zOLE@4aDcN(kxMPT$WQ!MX7*=EOtR?_w$Zy8w7w#oeXV5Uq{_0+#4%csbHJF}59M8| zWk1NnZe>!;@0y{0trW%50KT9_6sr<4Ej1YM6;__h$o2d0URDSM9!LJkI=+EAo<9YN zWP&ueKVPCOczihwaScY6U+)w>fGkUK>r=$zqcV(3N zO@@ZK2C5Ey1|KSmDYh`OO}WZ}0@x0OUmu@iGQb?9cMczMi3DO|imv`cMgJ&^SKEgA z1FOJsDnpX@lh2igk3@Gyrw&^Yjjg7qt&SFl3rLTvt_^Z*{^pGMA>*3_V3Q;=E1>)9 z12RmKC~rEqN5DC{a&2bW+f;j`p(b|wJ19W%nyD=j2y0>Uo%=WI_C8$Q>O^~cczPY` zea&q$pDP@{OnXpaLqnrcb|4>pCf!=cQ~B*WuFK)XU;3ldzh+p)4ddhEW5?-k569db zv2|(44UnKxp6lz3KHkBYynSEvNsVLeaQnFXPvRcHRkoeH^dz1w_kC;OF z+67~}J`XK#X*^A_Xz5v_5ZA_~YfENK&)Zn$8TOa$3B9h5$JM&l%i+wfR?SqF9ay&gg#o2DtS2P&NPfarA)tR(n$oa5m@Q4+TaNV%In z_qm_>cmv)a8&Ju9WWKJF80C;qC6;y<_f#gW_tfu9oHGv)V72Tf&~3nEQ=lO8%;T4-=sP zKjBh&c#XAOgr%<)O+wjQ($W>D>`o+(-1u-N)^hv=qiKHkXlp)L^=V?MJKnV+aq!Xb z)W^%DjiHbvG-$Owwgf34&EX5kQuw#%1G`zSyd_D2D2;v(^#+|B4)8cnQSfWWWOrKo zXZ^B9JdUkFwg_FrO&;(xx=_4Gqi51!!JoF6vNc7?nKhWjrp!Vbbr)h-GDC=O5es}#VIH&`?Q6oxmN1nWsGd{Ln3 zL89xMr|Eg3#!Io)qxro@&=5wVky{9Y9Wq_zjR;*kQP%YFnzN@J-|43D_6_?CCw|fB z=B-h3l#+ty%7R5qxp%I(Xb_^%w)5*3KKXaXD{D0S^}s-PQd`0;ql($_u0-i7Isesl z`HbdrT}j3iIV@!Gn+2e7t<(IbSB57Z=v6yjpj8)s$rNOz|G_L(9(4CvP2aGXYCgEG z_wnP?{m-Sm!^X$gj%89pgDP*d)a1ueX%p^VLOux_iMx8^4Y_HxYrW{$ME{$o=NWU} zo_{@e#sBzb4C%;OSZeS-){J0tZa6n%!k0=Q*_KKE(tQ=_`D78Tc;0Wy-QzLZ*SkMi zWcZl{?k);?>_-IFy;~+-mGk>9ZciVf3Q^ZXgc4`V<_9#jt}6N6R|+R&X$IKW>)IY< z6BR_L&1XeTV!s&*Sn~~2QDwI!>)e*fl9F=t}tC{qwIkQZM=cOPehQo@grMSKI z3Z}FfaC;-9)O=RZTwCm3w0yhbiuH;QO~+8xLx)wu_=2b;g1P`3q1CQDfBl&?s2uFn z>f?P?)tIC=4T|Y;(LB7*d#_?x5Y$Z4hPpyk(MoZGT2ynH(>g9 zIiK=fMp8F%hD=lF-ryODNIq9Uv+AqZt#9z>5;2wzmyvL#GYLxynck7bSMz-x`TpJa zKp~$*iGT=3s1%aKn&cGIf6J@=Xg4Gy6ldv&7P5R_VVuXnz%1InNX*74kq>Z6GL|Ng ziz1UxemYgKB(&m`ylbEbOjwE?-L9IGn(s7Cf?Co!jvThY>yd773Ivdsdph>-G~xZN3qGiyT+a|tBFG#A6o{jA>s9#YM;6&h}nn;9QA z1`mjwHU6AP$4;RN&taoMS-RI7e}2|7N++Hm%oi0%N9;h2&%lLFaY@QmzWHUS}I+U zH|{@g?S)8E@(Pwh2uA7<+D;xlkU&Zt?wj(H%E<$4(1>@6Zovhb4d5%uT9`Kh(j6wp&by`N6rXJuGE=3o1~bUNFRC`7^Sd$Fx_R;dS^&7qg^S7cM8|@ zcF?P{p9cWF|9H)z(AJ_G6%3VqhekUi}*-9eu%u{K#>u{1u2IumC zz|I3rZu`cdP;cLb1Fct(XC@D^I525_-S;3Izo<{EX!f!R>%N__?Biq2@u9juoqO>S z3odV;AH(u8_^CjD;6}ik5LsVQpB_7cae7jgUw&G<;i>R2N~h+LwmORlhvWSvCHgMH zonT5n8my(sye7X>ka$n#YK$Z$pFUe-GP)fhUP_!Bjm2v9$`-L9C!T3a+MBM1${V+^ zi0SHAhJa}Wlf(JzV;?pMN+$8ee%8~lV`pDgX77yo2smqx8Z+LKHmO>i#;J6?wVj?~ zQ&{7x8c+Xbe&YFV;E-rej-*;;y7!u=%?&-3f%|2wikl0o_h(~x$!VS>bI-9rr$qYW zWv4KUq8*yH+fjK;&}<2epxf|t%_fpObj*%+7!p0=JJck4d_*d|T;zPREfQ=ED;xGs7kjKRe2OkF^@tHv?+YP2I^wHmh{r?79FcOI zYj$O;*GG#pOf2N~MfZ~m@2uy!(x;pZV)eT;JIF^|{8)N(vnaIjkSD{{>T6Kmu-k}OroKhSVBm&eTJJT+TJorEe6c1z`_lP6R}>liY73xNqd3Ys|y0 z@FbOW*PWAaEPZrwhE1S1a7O{(-6%0W(+C-_%CV-n^!e@_>hbz+NYe9H^E%tKk6IvQ zS+6rH^wsS#NFbLNP!?M{4clwk1W%Kc{izEoIOUH>54nWNqKH*T(4H}(o+JFqYrlxe z;1687j$@%@lGIiZDLd{v{Hwz7;}Cuw2W)~HZ>p9`+dywy{rii}&jf<#EtPzqbCO)j zeB`+>MbbtShYP$5(OLHG%~LH8wwI$R1z-3{yJ;~5M-+SRXUi@6bXKK(>?YG>Ox2s` z;?e^i!#+M866KMXIe~nA-P_xB@>;9s*4+~=mTW7cD`> z@78~9!egO?!DGF^P1Pir$a_9dE9qX+2Awg!9Zeq&Em{y-fA}aS$m6DtZSrT8aa;8B zS$nH%gX66woFrHIH3q^&#?q$^>blX^-5Sl|n+x}3^okj)UU@ZNM=Yywj4N?XG!MC& zGnx$FZX1$()bdToZfAW#~=6jZ|2O;n{~uKSk)6jMA&l~ zeL;P(#P3CwKuJ zd^@xFi4}Q>mI}N9S5}?-7Q`h|-Cp19E0+sZD@(-c)HA%!@aKDPQQ0*EJC`+_9>y&u zYpirs&m#}ATx7fcf~BR4>2Ucsl_PS>HIm^0>@U41H3F_4+m169(-rB8GCBPDwoN&+ z;U)MeDW#CJ$lri)v?uz-si~lubMppGL9joR1*J4ik4PMjGZ@(KUgvkj|LC zPFZ?O@LEv2K_k93Gnhdv&B>Un9|thF$QI>CIdC$}leOpoL+Ae_>dM*fzg<4_p0bv@pwD!n&& zZFTWoeak#SbG+A9fBBK}6z^*@jVmlu#sBGr&GjeJ%rg!8GdG^E>J3?J7Mj)lPNb2F(Y z^_&e(Se&)g69X0@fYB^Wz|3zL?|VY=z!zpa>kyt}Q+FGW-Jzf-#!$SiWHd^g-XO`5 zZ~EbS`r7-4#u{C(mG!0{4_m9g)j;>(5W95F@uF^AZ8%b-^#u`XodC|<$ z$laLoT>a|VXv|{Do0Fg5oVy33CknIoZlSs1;9u9!Xbk-CfoeE{!WRn>i%GBr4j`d$ z04W>OW_ytx4u=9zLuN(Ae~8Eej9oT_0gj?XBtZCL_Tyt1(3qH~c;%CD#1As;65+C| zqb

2ZbYlAe7w{2$zy3917oiQRCn4F#k+)YZ6?EQ25@ww;?m^q6fj&2uC!BT~ybE zUBdYiDSF8e2{t&ew+u7Hc@n%lY;m4YIEL~$6=F!~PKF$VASsEd5T5@>GGO{4AM@e} zCnYl#a{T}Dboa)~4Sx>$r+ITVf|n~1ihyZ4dUz6{@V!fYLzup`ClLz%o-8b7I1PfK zJWqoN3H)$k2s@ASB|^a;Vf>^30((ql2$CX`0Xcc%5)k; znv#$XISj%4F!;;)4};&F!vV@n1_a6g$52=^A(BEr{t9$2^M+7M=D#BPc{d=%DHFnX z;LmJR(lQ}1_CL4ub18rb%487K8c~lSRsczhEF7frbBq0kS9n@e2k=BKGPYAOXP7!dNu=cOv395de_I{=x$R z_z#Ew@&_P*-+}`$h@S%4(+2<$4(uJ9e;e-|(BH7VEdI(v{T49@$LzI$U;N1;f3pQi z0OmIl5H1V+V@&k0c67#h>^+cU Date: Mon, 11 Mar 2013 11:42:23 -0700 Subject: [PATCH 018/109] Added missing prototype --- apps/commander/commander.c | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/apps/commander/commander.c b/apps/commander/commander.c index aaabe7f4b2..28f0999910 100644 --- a/apps/commander/commander.c +++ b/apps/commander/commander.c @@ -172,6 +172,9 @@ static void do_gyro_calibration(int status_pub, struct vehicle_status_s *status) static void do_mag_calibration(int status_pub, struct vehicle_status_s *status); static void do_rc_calibration(int status_pub, struct vehicle_status_s *status); static void do_accel_calibration(int status_pub, struct vehicle_status_s *status); +static void do_airspeed_calibration(int status_pub, struct vehicle_status_s *status); + + static void handle_command(int status_pub, struct vehicle_status_s *current_status, struct vehicle_command_s *cmd); int trigger_audio_alarm(uint8_t old_mode, uint8_t old_state, uint8_t new_mode, uint8_t new_state); @@ -801,7 +804,7 @@ void do_accel_calibration(int status_pub, struct vehicle_status_s *status) close(sub_sensor_combined); } -void do_airspeed_calibration(int status_pub, struct vehicle_status_s *status) +static void do_airspeed_calibration(int status_pub, struct vehicle_status_s *status) { /* announce change */ From 6039582928e199976074a4033336316c65737b66 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Fri, 22 Mar 2013 09:04:08 -0700 Subject: [PATCH 019/109] Compile error after merge fixed --- apps/controllib/fixedwing.cpp | 26 ++++++++++++-------------- 1 file changed, 12 insertions(+), 14 deletions(-) diff --git a/apps/controllib/fixedwing.cpp b/apps/controllib/fixedwing.cpp index 9edb84723a..56b42253e6 100644 --- a/apps/controllib/fixedwing.cpp +++ b/apps/controllib/fixedwing.cpp @@ -345,23 +345,21 @@ void BlockMultiModeBacksideAutopilot::update() #warning should be base on flags } else if (_status.navigation_state == NAVIGATION_STATE_MANUAL) { -// if (_status.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_DIRECT) { #warning fix the different manual modes - _actuators.control[CH_AIL] = _manual.roll; - _actuators.control[CH_ELV] = _manual.pitch; - _actuators.control[CH_RDR] = _manual.yaw; - _actuators.control[CH_THR] = _manual.throttle; -#warning please check whether this flag makes sense - } else if (_status.flag_control_attitude_enabled) { + _actuators.control[CH_AIL] = _manual.roll; + _actuators.control[CH_ELV] = _manual.pitch; + _actuators.control[CH_RDR] = _manual.yaw; + _actuators.control[CH_THR] = _manual.throttle; +#warning should be based on flags + } else if (_status.navigation_state == NAVIGATION_STATE_SEATBELT) { - _stabilization.update(_manual.roll, _manual.pitch, _manual.yaw, - _att.rollspeed, _att.pitchspeed, _att.yawspeed); + _stabilization.update(_manual.roll, _manual.pitch, _manual.yaw, + _att.rollspeed, _att.pitchspeed, _att.yawspeed); - _actuators.control[CH_AIL] = _stabilization.getAileron(); - _actuators.control[CH_ELV] = _stabilization.getElevator(); - _actuators.control[CH_RDR] = _stabilization.getRudder(); - _actuators.control[CH_THR] = _manual.throttle; - } + _actuators.control[CH_AIL] = _stabilization.getAileron(); + _actuators.control[CH_ELV] = _stabilization.getElevator(); + _actuators.control[CH_RDR] = _stabilization.getRudder(); + _actuators.control[CH_THR] = _manual.throttle; } // update all publications From b1e2011fcc068709574ddaab3d8bc831abcb7de8 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Fri, 22 Mar 2013 10:36:23 -0700 Subject: [PATCH 020/109] Added feedback before rebooting --- apps/systemcmds/reboot/reboot.c | 3 +++ 1 file changed, 3 insertions(+) diff --git a/apps/systemcmds/reboot/reboot.c b/apps/systemcmds/reboot/reboot.c index 47d3cd9486..cc380f94bd 100644 --- a/apps/systemcmds/reboot/reboot.c +++ b/apps/systemcmds/reboot/reboot.c @@ -47,6 +47,9 @@ __EXPORT int reboot_main(int argc, char *argv[]); int reboot_main(int argc, char *argv[]) { + printf("Rebooting now...\n"); + fflush(stdout); + usleep(5000); up_systemreset(); } From 3665d7b86fdbd8489099dafb436aaddcb816efde Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Mon, 25 Mar 2013 14:53:54 -0700 Subject: [PATCH 021/109] Improved command handling, added a low priority task and various fixes --- apps/commander/commander.c | 608 +++++++++++++------------- apps/commander/state_machine_helper.c | 16 + 2 files changed, 312 insertions(+), 312 deletions(-) diff --git a/apps/commander/commander.c b/apps/commander/commander.c index 388ba19642..27abb9d45c 100644 --- a/apps/commander/commander.c +++ b/apps/commander/commander.c @@ -143,16 +143,26 @@ static int daemon_task; /**< Handle of daemon task / thread */ static struct vehicle_status_s current_status; static orb_advert_t stat_pub; -/* XXX ? */ -// static uint16_t nofix_counter = 0; -// static uint16_t gotfix_counter = 0; - -/* XXX ? */ +/* timout until lowlevel failsafe */ static unsigned int failsafe_lowlevel_timeout_ms; +/* tasks waiting for low prio thread */ +enum { + LOW_PRIO_TASK_NONE = 0, + LOW_PRIO_TASK_PARAM_SAVE, + LOW_PRIO_TASK_PARAM_LOAD, + LOW_PRIO_TASK_GYRO_CALIBRATION, + LOW_PRIO_TASK_MAG_CALIBRATION, + LOW_PRIO_TASK_ALTITUDE_CALIBRATION, + LOW_PRIO_TASK_RC_CALIBRATION, + LOW_PRIO_TASK_ACCEL_CALIBRATION, + LOW_PRIO_TASK_AIRSPEED_CALIBRATION +} low_prio_task; + /* pthread loops */ static void *orb_receive_loop(void *arg); +static void *commander_low_prio_loop(void *arg); __EXPORT int commander_main(int argc, char *argv[]); @@ -161,18 +171,23 @@ __EXPORT int commander_main(int argc, char *argv[]); */ int commander_thread_main(int argc, char *argv[]); -static int buzzer_init(void); -static void buzzer_deinit(void); -static int led_init(void); -static void led_deinit(void); -static int led_toggle(int led); -static int led_on(int led); -static int led_off(int led); -static void do_gyro_calibration(int status_pub, struct vehicle_status_s *status); -static void do_mag_calibration(int status_pub, struct vehicle_status_s *status); -static void do_rc_calibration(int status_pub, struct vehicle_status_s *status); -static void do_accel_calibration(int status_pub, struct vehicle_status_s *status); -static void do_airspeed_calibration(int status_pub, struct vehicle_status_s *status); +int buzzer_init(void); +void buzzer_deinit(void); +int led_init(void); +void led_deinit(void); +int led_toggle(int led); +int led_on(int led); +int led_off(int led); +void tune_error(void); +void tune_positive(void); +void tune_neutral(void); +void tune_negative(void); +void do_reboot(void); +void do_gyro_calibration(void); +void do_mag_calibration(void); +void do_rc_calibration(void); +void do_accel_calibration(void); +void do_airspeed_calibration(void); static void handle_command(int status_pub, struct vehicle_status_s *current_status, struct vehicle_command_s *cmd); @@ -184,7 +199,7 @@ int trigger_audio_alarm(uint8_t old_mode, uint8_t old_state, uint8_t new_mode, u /** * Print the correct usage. */ -static void usage(const char *reason); +void usage(const char *reason); /** * Sort calibration values. @@ -196,7 +211,7 @@ static void usage(const char *reason); */ // static void cal_bsort(float a[], int n); -static int buzzer_init() +int buzzer_init() { buzzer = open("/dev/tone_alarm", O_WRONLY); @@ -208,13 +223,13 @@ static int buzzer_init() return 0; } -static void buzzer_deinit() +void buzzer_deinit() { close(buzzer); } -static int led_init() +int led_init() { leds = open(LED_DEVICE_PATH, 0); @@ -231,12 +246,12 @@ static int led_init() return 0; } -static void led_deinit() +void led_deinit() { close(leds); } -static int led_toggle(int led) +int led_toggle(int led) { static int last_blue = LED_ON; static int last_amber = LED_ON; @@ -248,59 +263,50 @@ static int led_toggle(int led) return ioctl(leds, ((led == LED_BLUE) ? last_blue : last_amber), led); } -static int led_on(int led) +int led_on(int led) { return ioctl(leds, LED_ON, led); } -static int led_off(int led) +int led_off(int led) { return ioctl(leds, LED_OFF, led); } -enum AUDIO_PATTERN { - AUDIO_PATTERN_ERROR = 2, - AUDIO_PATTERN_NOTIFY_POSITIVE = 3, - AUDIO_PATTERN_NOTIFY_NEUTRAL = 4, - AUDIO_PATTERN_NOTIFY_NEGATIVE = 5, - AUDIO_PATTERN_NOTIFY_CHARGE = 6 -}; -int trigger_audio_alarm(uint8_t old_mode, uint8_t old_state, uint8_t new_mode, uint8_t new_state) +void tune_error() { - -#warning add alarms for state transitions - /* Trigger alarm if going into any error state */ -// if (((new_state == SYSTEM_STATE_GROUND_ERROR) && (old_state != SYSTEM_STATE_GROUND_ERROR)) || -// ((new_state == SYSTEM_STATE_MISSION_ABORT) && (old_state != SYSTEM_STATE_MISSION_ABORT))) { -// ioctl(buzzer, TONE_SET_ALARM, 0); -// ioctl(buzzer, TONE_SET_ALARM, AUDIO_PATTERN_ERROR); -// } - - /* Trigger neutral on arming / disarming */ -// if (((new_state == SYSTEM_STATE_GROUND_READY) && (old_state != SYSTEM_STATE_GROUND_READY))) { -// ioctl(buzzer, TONE_SET_ALARM, 0); -// ioctl(buzzer, TONE_SET_ALARM, AUDIO_PATTERN_NOTIFY_NEUTRAL); -// } - - /* Trigger Tetris on being bored */ - - return 0; + ioctl(buzzer, TONE_SET_ALARM, 2); } -void tune_confirm(void) +void tune_positive() { ioctl(buzzer, TONE_SET_ALARM, 3); } -void tune_error(void) +void tune_neutral() { - /* XXX change alarm to 2 tones*/ ioctl(buzzer, TONE_SET_ALARM, 4); } -void do_rc_calibration(int status_pub, struct vehicle_status_s *status) +void tune_negative() { + ioctl(buzzer, TONE_SET_ALARM, 5); +} + +void do_reboot() +{ + mavlink_log_critical(mavlink_fd, "REBOOTING SYSTEM"); + usleep(500000); + up_systemreset(); + /* SPECIAL CASE: NEVER RETURNS FROM THIS FUNCTION CALL */ +} + + +void do_rc_calibration() +{ + mavlink_log_info(mavlink_fd, "trim calibration starting"); + if (current_status.offboard_control_signal_lost) { mavlink_log_critical(mavlink_fd, "TRIM CAL: ABORT. No RC signal."); return; @@ -311,7 +317,6 @@ void do_rc_calibration(int status_pub, struct vehicle_status_s *status) orb_copy(ORB_ID(manual_control_setpoint), sub_man, &sp); /* set parameters */ - float p = sp.roll; param_set(param_find("TRIM_ROLL"), &p); p = sp.pitch; @@ -320,22 +325,21 @@ void do_rc_calibration(int status_pub, struct vehicle_status_s *status) param_set(param_find("TRIM_YAW"), &p); /* store to permanent storage */ - /* auto-save to EEPROM */ + /* auto-save */ int save_ret = param_save_default(); if (save_ret != 0) { mavlink_log_critical(mavlink_fd, "TRIM CAL: WARN: auto-save of params failed"); } + tune_positive(); + mavlink_log_info(mavlink_fd, "trim calibration done"); } -void do_mag_calibration(int status_pub, struct vehicle_status_s *status) +void do_mag_calibration() { - - /* set to mag calibration mode */ - // status->flag_preflight_mag_calibration = true; - // state_machine_publish(status_pub, status, mavlink_fd); + mavlink_log_info(mavlink_fd, "mag calibration starting, hold still"); int sub_mag = orb_subscribe(ORB_ID(sensor_mag)); struct mag_report mag; @@ -350,15 +354,6 @@ void do_mag_calibration(int status_pub, struct vehicle_status_s *status) /* limit update rate to get equally spaced measurements over time (in ms) */ orb_set_interval(sub_mag, (calibration_interval / 1000) / calibration_maxcount); - // XXX old cal - // * FLT_MIN is not the most negative float number, - // * but the smallest number by magnitude float can - // * represent. Use -FLT_MAX to initialize the most - // * negative number - - // float mag_max[3] = {-FLT_MAX, -FLT_MAX, -FLT_MAX}; - // float mag_min[3] = {FLT_MAX, FLT_MAX, FLT_MAX}; - int fd = open(MAG_DEVICE_PATH, O_RDONLY); /* erase old calibration */ @@ -404,10 +399,6 @@ void do_mag_calibration(int status_pub, struct vehicle_status_s *status) return; } - tune_confirm(); - sleep(2); - tune_confirm(); - while (hrt_absolute_time() < calibration_deadline && calibration_counter < calibration_maxcount) { @@ -421,9 +412,9 @@ void do_mag_calibration(int status_pub, struct vehicle_status_s *status) axis_index++; char buf[50]; - sprintf(buf, "Please rotate around %c", axislabels[axis_index]); + sprintf(buf, "please rotate around %c", axislabels[axis_index]); mavlink_log_info(mavlink_fd, buf); - tune_confirm(); + tune_neutral(); axis_deadline += calibration_interval / 3; } @@ -559,28 +550,19 @@ void do_mag_calibration(int status_pub, struct vehicle_status_s *status) mavlink_log_info(mavlink_fd, "mag calibration done"); - tune_confirm(); - sleep(2); - tune_confirm(); - sleep(2); + tune_positive(); /* third beep by cal end routine */ } else { mavlink_log_info(mavlink_fd, "mag calibration FAILED (NaN in sphere fit)"); } - /* disable calibration mode */ - // status->flag_preflight_mag_calibration = false; - // state_machine_publish(status_pub, status, mavlink_fd); - close(sub_mag); } -void do_gyro_calibration(int status_pub, struct vehicle_status_s *status) +void do_gyro_calibration() { - /* set to gyro calibration mode */ - // status->flag_preflight_gyro_calibration = true; - // state_machine_publish(status_pub, status, mavlink_fd); + mavlink_log_info(mavlink_fd, "gyro calibration starting, hold still"); const int calibration_count = 5000; @@ -631,10 +613,6 @@ void do_gyro_calibration(int status_pub, struct vehicle_status_s *status) gyro_offset[1] = gyro_offset[1] / calibration_count; gyro_offset[2] = gyro_offset[2] / calibration_count; - /* exit gyro calibration mode */ - // status->flag_preflight_gyro_calibration = false; - // state_machine_publish(status_pub, status, mavlink_fd); - if (isfinite(gyro_offset[0]) && isfinite(gyro_offset[1]) && isfinite(gyro_offset[2])) { if (param_set(param_find("SENS_GYRO_XOFF"), &(gyro_offset[0])) @@ -671,10 +649,7 @@ void do_gyro_calibration(int status_pub, struct vehicle_status_s *status) // mavlink_log_info(mavlink_fd, buf); mavlink_log_info(mavlink_fd, "gyro calibration done"); - tune_confirm(); - sleep(2); - tune_confirm(); - sleep(2); + tune_positive(); /* third beep by cal end routine */ } else { @@ -684,14 +659,10 @@ void do_gyro_calibration(int status_pub, struct vehicle_status_s *status) close(sub_sensor_combined); } -void do_accel_calibration(int status_pub, struct vehicle_status_s *status) +void do_accel_calibration() { - /* announce change */ - - mavlink_log_info(mavlink_fd, "keep it level and still"); - /* set to accel calibration mode */ - // status->flag_preflight_accel_calibration = true; - // state_machine_publish(status_pub, status, mavlink_fd); + /* give directions */ + mavlink_log_info(mavlink_fd, "accel calibration starting, keep it level"); const int calibration_count = 2500; @@ -787,28 +758,19 @@ void do_accel_calibration(int status_pub, struct vehicle_status_s *status) //mavlink_log_info(mavlink_fd, buf); mavlink_log_info(mavlink_fd, "accel calibration done"); - tune_confirm(); - sleep(2); - tune_confirm(); - sleep(2); - /* third beep by cal end routine */ + tune_positive(); } else { mavlink_log_info(mavlink_fd, "accel calibration FAILED (NaN)"); } - /* exit accel calibration mode */ - // status->flag_preflight_accel_calibration = false; - // state_machine_publish(status_pub, status, mavlink_fd); - close(sub_sensor_combined); } -static void do_airspeed_calibration(int status_pub, struct vehicle_status_s *status) +void do_airspeed_calibration() { - /* announce change */ - - mavlink_log_info(mavlink_fd, "keep it still"); + /* give directions */ + mavlink_log_info(mavlink_fd, "airspeed calibration starting, keep it still"); const int calibration_count = 2500; @@ -857,11 +819,7 @@ static void do_airspeed_calibration(int status_pub, struct vehicle_status_s *sta //mavlink_log_info(mavlink_fd, buf); mavlink_log_info(mavlink_fd, "airspeed calibration done"); - tune_confirm(); - sleep(2); - tune_confirm(); - sleep(2); - /* third beep by cal end routine */ + tune_positive(); } else { mavlink_log_info(mavlink_fd, "airspeed calibration FAILED (NaN)"); @@ -870,16 +828,11 @@ static void do_airspeed_calibration(int status_pub, struct vehicle_status_s *sta close(sub_differential_pressure); } - - void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_status, struct vehicle_command_s *cmd) { /* result of the command */ uint8_t result = VEHICLE_CMD_RESULT_UNSUPPORTED; - /* announce command handling */ - tune_confirm(); - /* request to set different system mode */ switch (cmd->command) { case VEHICLE_CMD_DO_SET_MODE: @@ -935,12 +888,16 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta /* request for an autopilot reboot */ if ((int)cmd->param1 == 1) { if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_REBOOT, mavlink_fd)) { - /* SPECIAL CASE: SYSTEM WILL NEVER RETURN HERE */ + /* reboot is executed later, after positive tune is sent */ result = VEHICLE_CMD_RESULT_ACCEPTED; + tune_positive(); + /* SPECIAL CASE: SYSTEM WILL NEVER RETURN HERE */ + do_reboot(); } else { /* system may return here */ result = VEHICLE_CMD_RESULT_DENIED; + tune_negative(); } } } @@ -971,234 +928,155 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta // /* preflight calibration */ case VEHICLE_CMD_PREFLIGHT_CALIBRATION: { - bool handled = false; /* gyro calibration */ if ((int)(cmd->param1) == 1) { - if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_INIT, mavlink_fd)) { - - result = VEHICLE_CMD_RESULT_ACCEPTED; - - mavlink_log_info(mavlink_fd, "starting gyro cal"); - tune_confirm(); - do_gyro_calibration(status_pub, ¤t_status); - mavlink_log_info(mavlink_fd, "finished gyro cal"); - tune_confirm(); - // XXX if this fails, go to ERROR - arming_state_transition(status_pub, ¤t_status, ARMING_STATE_STANDBY, mavlink_fd); + /* check if no other task is scheduled */ + if(low_prio_task == LOW_PRIO_TASK_NONE) { + /* try to go to INIT/PREFLIGHT arming state */ + if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_INIT, mavlink_fd)) { + result = VEHICLE_CMD_RESULT_ACCEPTED; + /* now set the task for the low prio thread */ + low_prio_task = LOW_PRIO_TASK_GYRO_CALIBRATION; + } else { + result = VEHICLE_CMD_RESULT_DENIED; + } } else { - result = VEHICLE_CMD_RESULT_DENIED; + result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; } - - handled = true; } /* magnetometer calibration */ if ((int)(cmd->param2) == 1) { - if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_INIT, mavlink_fd)) { - - result = VEHICLE_CMD_RESULT_ACCEPTED; - - mavlink_log_info(mavlink_fd, "starting mag cal"); - tune_confirm(); - do_mag_calibration(status_pub, ¤t_status); - mavlink_log_info(mavlink_fd, "finished mag cal"); - tune_confirm(); - // XXX if this fails, go to ERROR - arming_state_transition(status_pub, ¤t_status, ARMING_STATE_STANDBY, mavlink_fd); + /* check if no other task is scheduled */ + if(low_prio_task == LOW_PRIO_TASK_NONE) { + /* try to go to INIT/PREFLIGHT arming state */ + if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_INIT, mavlink_fd)) { + result = VEHICLE_CMD_RESULT_ACCEPTED; + /* now set the task for the low prio thread */ + low_prio_task = LOW_PRIO_TASK_MAG_CALIBRATION; + } else { + result = VEHICLE_CMD_RESULT_DENIED; + } } else { - result = VEHICLE_CMD_RESULT_DENIED; + result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; } - - handled = true; } +#if 0 /* zero-altitude pressure calibration */ if ((int)(cmd->param3) == 1) { - /* transition to calibration state */ - // XXX add this again - // if (OK == do_arming_state_update(status_pub, ¤t_status, mavlink_fd, ARMING_STATE_INIT) - // && OK == do_navigation_state_update(status_pub, ¤t_status, mavlink_fd, NAVIGATION_STATE_INIT)) { - // - // // XXX implement this - // mavlink_log_info(mavlink_fd, "zero altitude cal. not implemented"); - // tune_confirm(); - // - // /* back to standby state */ - // do_arming_state_update(status_pub, ¤t_status, mavlink_fd, ARMING_STATE_STANDBY); - // do_navigation_state_update(status_pub, ¤t_status, mavlink_fd, NAVIGATION_STATE_STANDBY); - // - // } else { - // mavlink_log_critical(mavlink_fd, "REJECTING altitude calibration"); - // result = VEHICLE_CMD_RESULT_DENIED; - // } - handled = true; + /* check if no other task is scheduled */ + if(low_prio_task == LOW_PRIO_TASK_NONE) { + + /* try to go to INIT/PREFLIGHT arming state */ + if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_INIT, mavlink_fd)) { + result = VEHICLE_CMD_RESULT_ACCEPTED; + /* now set the task for the low prio thread */ + low_prio_task = LOW_PRIO_TASK_ALTITUDE_CALIBRATION; + } else { + result = VEHICLE_CMD_RESULT_DENIED; + } + } else { + result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; + } } +#endif +#if 0 /* trim calibration */ if ((int)(cmd->param4) == 1) { - /* transition to calibration state */ - // XXX add this again - // if (OK == do_arming_state_update(status_pub, ¤t_status, mavlink_fd, ARMING_STATE_INIT) - // && OK == do_navigation_state_update(status_pub, ¤t_status, mavlink_fd, NAVIGATION_STATE_INIT)) { - // mavlink_log_info(mavlink_fd, "starting trim cal"); - // tune_confirm(); - // do_rc_calibration(status_pub, ¤t_status); - // mavlink_log_info(mavlink_fd, "finished trim cal"); - // tune_confirm(); - // - // /* back to standby state */ - // do_arming_state_update(status_pub, ¤t_status, mavlink_fd, ARMING_STATE_STANDBY); - // do_navigation_state_update(status_pub, ¤t_status, mavlink_fd, NAVIGATION_STATE_STANDBY); - // - // result = VEHICLE_CMD_RESULT_ACCEPTED; - // - // } else { - // mavlink_log_critical(mavlink_fd, "REJECTING trim cal"); - // result = VEHICLE_CMD_RESULT_DENIED; - // } - handled = true; + /* check if no other task is scheduled */ + if(low_prio_task == LOW_PRIO_TASK_NONE) { + + /* try to go to INIT/PREFLIGHT arming state */ + if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_INIT, mavlink_fd)) { + result = VEHICLE_CMD_RESULT_ACCEPTED; + /* now set the task for the low prio thread */ + low_prio_task = LOW_PRIO_TASK_RC_CALIBRATION; + } else { + result = VEHICLE_CMD_RESULT_DENIED; + } + } else { + result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; + } } +#endif /* accel calibration */ if ((int)(cmd->param5) == 1) { - if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_INIT, mavlink_fd)) { - - result = VEHICLE_CMD_RESULT_ACCEPTED; - - mavlink_log_info(mavlink_fd, "starting acc cal"); - tune_confirm(); - do_accel_calibration(status_pub, ¤t_status); - mavlink_log_info(mavlink_fd, "finished acc cal"); - tune_confirm(); - // XXX what if this fails - arming_state_transition(status_pub, ¤t_status, ARMING_STATE_STANDBY, mavlink_fd); + /* check if no other task is scheduled */ + if(low_prio_task == LOW_PRIO_TASK_NONE) { + /* try to go to INIT/PREFLIGHT arming state */ + if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_INIT, mavlink_fd)) { + result = VEHICLE_CMD_RESULT_ACCEPTED; + /* now set the task for the low prio thread */ + low_prio_task = LOW_PRIO_TASK_ACCEL_CALIBRATION; + } else { + result = VEHICLE_CMD_RESULT_DENIED; + } } else { - result = VEHICLE_CMD_RESULT_DENIED; + result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; } - - - handled = true; } /* airspeed calibration */ if ((int)(cmd->param6) == 1) { - if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_INIT, mavlink_fd)) { - - result = VEHICLE_CMD_RESULT_ACCEPTED; - - mavlink_log_info(mavlink_fd, "starting airspeed cal"); - tune_confirm(); - do_airspeed_calibration(status_pub, ¤t_status); - mavlink_log_info(mavlink_fd, "finished airspeed cal"); - tune_confirm(); - // XXX if this fails, go to ERROR - arming_state_transition(status_pub, ¤t_status, ARMING_STATE_STANDBY, mavlink_fd); + /* check if no other task is scheduled */ + if(low_prio_task == LOW_PRIO_TASK_NONE) { + /* try to go to INIT/PREFLIGHT arming state */ + if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_INIT, mavlink_fd)) { + result = VEHICLE_CMD_RESULT_ACCEPTED; + /* now set the task for the low prio thread */ + low_prio_task = LOW_PRIO_TASK_AIRSPEED_CALIBRATION; + } else { + result = VEHICLE_CMD_RESULT_DENIED; + } } else { - result = VEHICLE_CMD_RESULT_DENIED; + result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; } - - handled = true; - } - - /* none found */ - if (!handled) { - //warnx("refusing unsupported calibration request\n"); - mavlink_log_critical(mavlink_fd, "CMD refusing unsup. calib. request"); - result = VEHICLE_CMD_RESULT_UNSUPPORTED; } } break; case VEHICLE_CMD_PREFLIGHT_STORAGE: { - if (current_status.flag_fmu_armed && - ((current_status.system_type == VEHICLE_TYPE_QUADROTOR) || - (current_status.system_type == VEHICLE_TYPE_HEXAROTOR) || - (current_status.system_type == VEHICLE_TYPE_OCTOROTOR))) { - /* do not perform expensive memory tasks on multirotors in flight */ - // XXX this is over-safe, as soon as cmd is in low prio thread this can be allowed - mavlink_log_info(mavlink_fd, "REJECTING save cmd while multicopter armed"); + if (((int)(cmd->param1)) == 0) { + + /* check if no other task is scheduled */ + if(low_prio_task == LOW_PRIO_TASK_NONE) { + low_prio_task = LOW_PRIO_TASK_PARAM_LOAD; + result = VEHICLE_CMD_RESULT_ACCEPTED; } else { + result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; + } - // XXX move this to LOW PRIO THREAD of commander app - /* Read all parameters from EEPROM to RAM */ - if (((int)(cmd->param1)) == 0) { + } else if (((int)(cmd->param1)) == 1) { - /* read all parameters from EEPROM to RAM */ - int read_ret = param_load_default(); - - if (read_ret == OK) { - //warnx("[mavlink pm] Loaded EEPROM params in RAM\n"); - mavlink_log_info(mavlink_fd, "OK loading params from"); - mavlink_log_info(mavlink_fd, param_get_default_file()); - result = VEHICLE_CMD_RESULT_ACCEPTED; - - } else if (read_ret == 1) { - mavlink_log_info(mavlink_fd, "OK no changes in"); - mavlink_log_info(mavlink_fd, param_get_default_file()); - result = VEHICLE_CMD_RESULT_ACCEPTED; - - } else { - if (read_ret < -1) { - mavlink_log_info(mavlink_fd, "ERR loading params from"); - mavlink_log_info(mavlink_fd, param_get_default_file()); - - } else { - mavlink_log_info(mavlink_fd, "ERR no param file named"); - mavlink_log_info(mavlink_fd, param_get_default_file()); - } - - result = VEHICLE_CMD_RESULT_FAILED; - } - - } else if (((int)(cmd->param1)) == 1) { - - /* write all parameters from RAM to EEPROM */ - int write_ret = param_save_default(); - - if (write_ret == OK) { - mavlink_log_info(mavlink_fd, "OK saved param file"); - mavlink_log_info(mavlink_fd, param_get_default_file()); - result = VEHICLE_CMD_RESULT_ACCEPTED; - - } else { - if (write_ret < -1) { - mavlink_log_info(mavlink_fd, "ERR params file does not exit:"); - mavlink_log_info(mavlink_fd, param_get_default_file()); - - } else { - mavlink_log_info(mavlink_fd, "ERR writing params to"); - mavlink_log_info(mavlink_fd, param_get_default_file()); - } - - result = VEHICLE_CMD_RESULT_FAILED; - } - - } else { - mavlink_log_info(mavlink_fd, "[pm] refusing unsupp. STOR request"); - result = VEHICLE_CMD_RESULT_UNSUPPORTED; - } + /* check if no other task is scheduled */ + if(low_prio_task == LOW_PRIO_TASK_NONE) { + low_prio_task = LOW_PRIO_TASK_PARAM_LOAD; + result = VEHICLE_CMD_RESULT_ACCEPTED; + } else { + result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; } } - break; + } break; default: { mavlink_log_critical(mavlink_fd, "[cmd] refusing unsupported command"); result = VEHICLE_CMD_RESULT_UNSUPPORTED; - /* announce command rejection */ - ioctl(buzzer, TONE_SET_ALARM, 4); } break; } @@ -1207,10 +1085,12 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta if (result == VEHICLE_CMD_RESULT_FAILED || result == VEHICLE_CMD_RESULT_DENIED || result == VEHICLE_CMD_RESULT_UNSUPPORTED) { - ioctl(buzzer, TONE_SET_ALARM, 5); + + tune_negative(); } else if (result == VEHICLE_CMD_RESULT_ACCEPTED) { - tune_confirm(); + + tune_positive(); } /* send any requested ACKs */ @@ -1324,8 +1204,7 @@ float battery_remaining_estimate_voltage(float voltage) return ret; } -static void -usage(const char *reason) +void usage(const char *reason) { if (reason) fprintf(stderr, "%s\n", reason); @@ -1405,6 +1284,7 @@ int commander_thread_main(int argc, char *argv[]) /* pthreads for command and subsystem info handling */ // pthread_t command_handling_thread; pthread_t subsystem_info_thread; + pthread_t commander_low_prio_thread; /* initialize */ if (led_init() != 0) { @@ -1477,6 +1357,16 @@ int commander_thread_main(int argc, char *argv[]) pthread_attr_setstacksize(&subsystem_info_attr, 2048); pthread_create(&subsystem_info_thread, &subsystem_info_attr, orb_receive_loop, ¤t_status); + pthread_attr_t commander_low_prio_attr; + pthread_attr_init(&commander_low_prio_attr); + pthread_attr_setstacksize(&commander_low_prio_attr, 2048); + + struct sched_param param; + /* low priority */ + param.sched_priority = SCHED_PRIORITY_DEFAULT - 50; + (void)pthread_attr_setschedparam(&commander_low_prio_attr, ¶m); + pthread_create(&commander_low_prio_thread, &commander_low_prio_attr, commander_low_prio_loop, NULL); + /* Start monitoring loop */ uint16_t counter = 0; @@ -1487,12 +1377,14 @@ int commander_thread_main(int argc, char *argv[]) bool critical_battery_voltage_actions_done = false; uint8_t low_voltage_counter = 0; uint16_t critical_voltage_counter = 0; - int16_t mode_switch_rc_value; float bat_remain = 1.0f; uint16_t stick_off_counter = 0; uint16_t stick_on_counter = 0; + /* XXX what exactly is this for? */ + uint64_t last_print_time = 0; + /* Subscribe to manual control data */ int sp_man_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); struct manual_control_setpoint_s sp_man; @@ -1562,7 +1454,9 @@ int commander_thread_main(int argc, char *argv[]) thread_running = true; uint64_t start_time = hrt_absolute_time(); - uint64_t failsave_ll_start_time = 0; + + /* XXX use this! */ + //uint64_t failsave_ll_start_time = 0; bool state_changed = true; bool param_init_forced = true; @@ -1727,7 +1621,7 @@ int commander_thread_main(int argc, char *argv[]) } else if (bat_remain < 0.2f && battery_voltage_valid && (counter % ((1000000 / COMMANDER_MONITORING_INTERVAL) / 2) == 0)) { /* For less than 20%, start be slightly annoying at 1 Hz */ ioctl(buzzer, TONE_SET_ALARM, 0); - tune_confirm(); + tune_positive(); } else if (bat_remain < 0.2f && battery_voltage_valid && (counter % ((1000000 / COMMANDER_MONITORING_INTERVAL) / 2) == 2)) { ioctl(buzzer, TONE_SET_ALARM, 0); @@ -1916,7 +1810,7 @@ int commander_thread_main(int argc, char *argv[]) /* mark home position as set */ home_position_set = true; - tune_confirm(); + tune_positive(); } } @@ -2206,13 +2100,19 @@ int commander_thread_main(int argc, char *argv[]) current_status.rc_signal_lost_interval = 0; } else { - static uint64_t last_print_time = 0; /* print error message for first RC glitch and then every 5 s / 5000 ms) */ if (!current_status.rc_signal_cutting_off || ((hrt_absolute_time() - last_print_time) > 5000000)) { /* only complain if the offboard control is NOT active */ current_status.rc_signal_cutting_off = true; mavlink_log_critical(mavlink_fd, "CRITICAL - NO REMOTE SIGNAL!"); + + if (!current_status.rc_signal_cutting_off) { + printf("Reason: not rc_signal_cutting_off\n"); + } else { + printf("last print time: %llu\n", last_print_time); + } + last_print_time = hrt_absolute_time(); } @@ -2270,7 +2170,7 @@ int commander_thread_main(int argc, char *argv[]) current_status.flag_control_manual_enabled = false; current_status.flag_control_offboard_enabled = true; state_changed = true; - tune_confirm(); + tune_positive(); mavlink_log_critical(mavlink_fd, "DETECTED OFFBOARD SIGNAL FIRST"); @@ -2278,7 +2178,7 @@ int commander_thread_main(int argc, char *argv[]) if (current_status.offboard_control_signal_lost) { mavlink_log_critical(mavlink_fd, "RECOVERY OFFBOARD CONTROL"); state_changed = true; - tune_confirm(); + tune_positive(); } } @@ -2298,7 +2198,6 @@ int commander_thread_main(int argc, char *argv[]) } } else { - static uint64_t last_print_time = 0; /* print error message for first RC glitch and then every 5 s / 5000 ms) */ if (!current_status.offboard_control_signal_weak || ((hrt_absolute_time() - last_print_time) > 5000000)) { @@ -2314,7 +2213,7 @@ int commander_thread_main(int argc, char *argv[]) if (current_status.offboard_control_signal_lost_interval > 100000) { current_status.offboard_control_signal_lost = true; current_status.failsave_lowlevel_start_time = hrt_absolute_time(); - tune_confirm(); + tune_positive(); /* kill motors after timeout */ if (hrt_absolute_time() - current_status.failsave_lowlevel_start_time > failsafe_lowlevel_timeout_ms * 1000) { @@ -2352,6 +2251,7 @@ int commander_thread_main(int argc, char *argv[]) /* Store old modes to detect and act on state transitions */ voltage_previous = current_status.voltage_battery; + /* XXX use this voltage_previous */ fflush(stdout); counter++; usleep(COMMANDER_MONITORING_INTERVAL); @@ -2360,6 +2260,7 @@ int commander_thread_main(int argc, char *argv[]) /* wait for threads to complete */ // pthread_join(command_handling_thread, NULL); pthread_join(subsystem_info_thread, NULL); + pthread_join(commander_low_prio_thread, NULL); /* close fds */ led_deinit(); @@ -2377,3 +2278,86 @@ int commander_thread_main(int argc, char *argv[]) return 0; } + + +static void *commander_low_prio_loop(void *arg) +{ + /* Set thread name */ + prctl(PR_SET_NAME, "commander low prio", getpid()); + + while (!thread_should_exit) { + + switch (low_prio_task) { + + case LOW_PRIO_TASK_PARAM_LOAD: + + if (0 == param_load_default()) { + mavlink_log_info(mavlink_fd, "Param load success"); + } else { + mavlink_log_critical(mavlink_fd, "Param load ERROR"); + tune_error(); + } + low_prio_task = LOW_PRIO_TASK_NONE; + break; + + case LOW_PRIO_TASK_PARAM_SAVE: + + if (0 == param_save_default()) { + mavlink_log_info(mavlink_fd, "Param save success"); + } else { + mavlink_log_critical(mavlink_fd, "Param save ERROR"); + tune_error(); + } + low_prio_task = LOW_PRIO_TASK_NONE; + break; + + case LOW_PRIO_TASK_GYRO_CALIBRATION: + + do_gyro_calibration(); + + low_prio_task = LOW_PRIO_TASK_NONE; + break; + + case LOW_PRIO_TASK_MAG_CALIBRATION: + + do_mag_calibration(); + + low_prio_task = LOW_PRIO_TASK_NONE; + break; + + case LOW_PRIO_TASK_ALTITUDE_CALIBRATION: + +// do_baro_calibration(); + + case LOW_PRIO_TASK_RC_CALIBRATION: + +// do_rc_calibration(); + + low_prio_task = LOW_PRIO_TASK_NONE; + break; + + case LOW_PRIO_TASK_ACCEL_CALIBRATION: + + do_accel_calibration(); + + low_prio_task = LOW_PRIO_TASK_NONE; + break; + + case LOW_PRIO_TASK_AIRSPEED_CALIBRATION: + + do_airspeed_calibration(); + + low_prio_task = LOW_PRIO_TASK_NONE; + break; + + case LOW_PRIO_TASK_NONE: + default: + /* slow down to 10Hz */ + usleep(100000); + break; + } + + } + + return 0; +} diff --git a/apps/commander/state_machine_helper.c b/apps/commander/state_machine_helper.c index 79394e2bae..ba01f84105 100644 --- a/apps/commander/state_machine_helper.c +++ b/apps/commander/state_machine_helper.c @@ -117,6 +117,22 @@ int arming_state_transition(int status_pub, struct vehicle_status_s *current_sta current_state->flag_fmu_armed = false; } break; + case ARMING_STATE_REBOOT: + + /* an armed error happens when ARMED obviously */ + if (current_state->arming_state == ARMING_STATE_INIT + || current_state->arming_state == ARMING_STATE_STANDBY + || current_state->arming_state == ARMING_STATE_STANDBY_ERROR) { + + ret = OK; + current_state->flag_fmu_armed = false; + + } + break; + case ARMING_STATE_IN_AIR_RESTORE: + + /* XXX implement */ + break; default: break; } From f8e5b0cb0e01f89e7cffcdb97f4d5daaae499f72 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Tue, 26 Mar 2013 10:44:49 -0700 Subject: [PATCH 022/109] Small fixes and comments --- apps/commander/commander.c | 48 ++++++++++++++++---------------------- 1 file changed, 20 insertions(+), 28 deletions(-) diff --git a/apps/commander/commander.c b/apps/commander/commander.c index 27abb9d45c..bde4f2856d 100644 --- a/apps/commander/commander.c +++ b/apps/commander/commander.c @@ -91,8 +91,6 @@ #include "state_machine_helper.h" - -/* XXX MOVE CALIBRATION TO SENSORS APP THREAD */ #include #include #include @@ -161,8 +159,8 @@ enum { /* pthread loops */ -static void *orb_receive_loop(void *arg); -static void *commander_low_prio_loop(void *arg); +void *orb_receive_loop(void *arg); +void *commander_low_prio_loop(void *arg); __EXPORT int commander_main(int argc, char *argv[]); @@ -190,7 +188,7 @@ void do_accel_calibration(void); void do_airspeed_calibration(void); -static void handle_command(int status_pub, struct vehicle_status_s *current_status, struct vehicle_command_s *cmd); +void handle_command(int status_pub, struct vehicle_status_s *current_status, struct vehicle_command_s *cmd); int trigger_audio_alarm(uint8_t old_mode, uint8_t old_state, uint8_t new_mode, uint8_t new_state); @@ -1101,7 +1099,7 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta } -static void *orb_receive_loop(void *arg) //handles status information coming from subsystems (present, enabled, health), these values do not indicate the quality (variance) of the signal +void *orb_receive_loop(void *arg) //handles status information coming from subsystems (present, enabled, health), these values do not indicate the quality (variance) of the signal { /* Set thread name */ prctl(PR_SET_NAME, "commander orb rcv", getpid()); @@ -1669,8 +1667,7 @@ int commander_thread_main(int argc, char *argv[]) /* If in INIT state, try to proceed to STANDBY state */ if (current_status.arming_state == ARMING_STATE_INIT) { - // XXX fix for now - current_status.condition_system_sensors_initialized = true; + // XXX check for sensors arming_state_transition(stat_pub, ¤t_status, ARMING_STATE_STANDBY, mavlink_fd); } else { // XXX: Add emergency stuff if sensors are lost @@ -1730,7 +1727,6 @@ int commander_thread_main(int argc, char *argv[]) // current_status.flag_vector_flight_mode_ok = false; // } - // XXX why is this needed? /* consolidate state change, flag as changed if required */ if (global_pos_valid != current_status.condition_global_position_valid || local_pos_valid != current_status.condition_local_position_valid || @@ -2030,12 +2026,12 @@ int commander_thread_main(int argc, char *argv[]) case ARMING_STATE_ARMED_ERROR: - // TODO work out fail-safe scenarios + // XXX work out fail-safe scenarios break; case ARMING_STATE_STANDBY_ERROR: - // TODO work out fail-safe scenarios + // XXX work out fail-safe scenarios break; case ARMING_STATE_REBOOT: @@ -2050,9 +2046,6 @@ int commander_thread_main(int argc, char *argv[]) default: break; } - - // navigation_state_update(stat_pub, ¤t_status, mavlink_fd); - /* handle the case where RC signal was regained */ if (!current_status.rc_signal_found_once) { current_status.rc_signal_found_once = true; @@ -2068,11 +2061,11 @@ int commander_thread_main(int argc, char *argv[]) * Check if left stick is in lower left position --> switch to standby state. * Do this only for multirotors, not for fixed wing aircraft. */ - // if (((current_status.system_type == VEHICLE_TYPE_QUADROTOR) || - // (current_status.system_type == VEHICLE_TYPE_HEXAROTOR) || - // (current_status.system_type == VEHICLE_TYPE_OCTOROTOR) - // ) && - if ((sp_man.yaw < -STICK_ON_OFF_LIMIT) && (sp_man.throttle < STICK_THRUST_RANGE * 0.2f)) { + if (((current_status.system_type == VEHICLE_TYPE_QUADROTOR) || + (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)) { if (stick_off_counter > STICK_ON_OFF_COUNTER_LIMIT) { arming_state_transition(stat_pub, ¤t_status, ARMING_STATE_STANDBY, mavlink_fd); stick_off_counter = 0; @@ -2186,15 +2179,15 @@ int commander_thread_main(int argc, char *argv[]) current_status.offboard_control_signal_lost = false; current_status.offboard_control_signal_lost_interval = 0; + // XXX check if this is correct /* arm / disarm on request */ if (sp_offboard.armed && !current_status.flag_fmu_armed) { -#warning fix this -// update_state_machine_arm(stat_pub, ¤t_status, mavlink_fd); - /* switch to stabilized mode = takeoff */ -// update_state_machine_mode_stabilized(stat_pub, ¤t_status, mavlink_fd); + + arming_state_transition(stat_pub, ¤t_status, ARMING_STATE_ARMED, mavlink_fd); } else if (!sp_offboard.armed && current_status.flag_fmu_armed) { -// update_state_machine_disarm(stat_pub, ¤t_status, mavlink_fd); + + arming_state_transition(stat_pub, ¤t_status, ARMING_STATE_STANDBY, mavlink_fd); } } else { @@ -2229,7 +2222,7 @@ int commander_thread_main(int argc, char *argv[]) current_status.timestamp = hrt_absolute_time(); - // XXX what is this? + // XXX this is missing /* If full run came back clean, transition to standby */ // if (current_status.state_machine == SYSTEM_STATE_PREFLIGHT && // current_status.flag_preflight_gyro_calibration == false && @@ -2241,8 +2234,7 @@ int commander_thread_main(int argc, char *argv[]) /* publish at least with 1 Hz */ if (counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0 || state_changed) { -#warning fix this -// publish_armed_status(¤t_status); + orb_publish(ORB_ID(vehicle_status), stat_pub, ¤t_status); state_changed = false; } @@ -2280,7 +2272,7 @@ int commander_thread_main(int argc, char *argv[]) } -static void *commander_low_prio_loop(void *arg) +void *commander_low_prio_loop(void *arg) { /* Set thread name */ prctl(PR_SET_NAME, "commander low prio", getpid()); From 3cb3fa224baffcbb5ea228287f3d5b4f226b813b Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Tue, 26 Mar 2013 15:37:13 -0700 Subject: [PATCH 023/109] Small fixes again --- apps/commander/commander.c | 6 +++++- apps/mavlink/mavlink.c | 6 +----- 2 files changed, 6 insertions(+), 6 deletions(-) diff --git a/apps/commander/commander.c b/apps/commander/commander.c index bde4f2856d..77853aa7ae 100644 --- a/apps/commander/commander.c +++ b/apps/commander/commander.c @@ -1082,7 +1082,8 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta /* supported command handling stop */ if (result == VEHICLE_CMD_RESULT_FAILED || result == VEHICLE_CMD_RESULT_DENIED || - result == VEHICLE_CMD_RESULT_UNSUPPORTED) { + result == VEHICLE_CMD_RESULT_UNSUPPORTED || + result == VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED) { tune_negative(); @@ -1828,16 +1829,19 @@ int commander_thread_main(int argc, char *argv[]) /* bottom stick position, go to manual mode */ current_status.mode_switch = MODE_SWITCH_MANUAL; + printf("mode switch: manual\n"); } else if (sp_man.mode_switch > STICK_ON_OFF_LIMIT) { /* top stick position, set auto/mission for all vehicle types */ current_status.mode_switch = MODE_SWITCH_AUTO; + printf("mode switch: auto\n"); } else { /* center stick position, set seatbelt/simple control */ current_status.mode_switch = MODE_SWITCH_SEATBELT; + printf("mode switch: seatbelt\n"); } // warnx("man ctrl mode: %d\n", (int)current_status.manual_control_mode); diff --git a/apps/mavlink/mavlink.c b/apps/mavlink/mavlink.c index 084ab0d322..2643cbf7ba 100644 --- a/apps/mavlink/mavlink.c +++ b/apps/mavlink/mavlink.c @@ -236,11 +236,6 @@ get_mavlink_mode_and_state(uint8_t *mavlink_state, uint8_t *mavlink_mode) *mavlink_mode |= MAV_MODE_FLAG_GUIDED_ENABLED; } -// -// if (v_status.system_state == NAVIGATION_STATE_SEATBELT) { -// -// *mavlink_mode |= MAV_MODE_FLAG_DECODE_POSITION_GUIDED; -// } /** @@ -577,6 +572,7 @@ int mavlink_thread_main(int argc, char *argv[]) default: usage(); + break; } } From 8567134d64f5d8e3c7aba7cebfdf56ffe56b44ed Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 7 Jun 2013 19:41:41 +0200 Subject: [PATCH 024/109] Made pwm command sending continously, improved failsafe logic --- src/modules/px4iofirmware/mixer.cpp | 16 +++++----- src/systemcmds/pwm/pwm.c | 48 +++++++++++++++++++++++++---- 2 files changed, 49 insertions(+), 15 deletions(-) diff --git a/src/modules/px4iofirmware/mixer.cpp b/src/modules/px4iofirmware/mixer.cpp index a2193b526b..123eb67786 100644 --- a/src/modules/px4iofirmware/mixer.cpp +++ b/src/modules/px4iofirmware/mixer.cpp @@ -98,7 +98,7 @@ mixer_tick(void) if (r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK) { isr_debug(1, "AP RX timeout"); } - r_status_flags &= ~(PX4IO_P_STATUS_FLAGS_FMU_OK | PX4IO_P_STATUS_FLAGS_RAW_PWM); + r_status_flags &= ~(PX4IO_P_STATUS_FLAGS_FMU_OK); r_status_alarms |= PX4IO_P_STATUS_ALARMS_FMU_LOST; } else { @@ -112,12 +112,11 @@ mixer_tick(void) * Decide which set of controls we're using. */ - /* do not mix if mixer is invalid or if RAW_PWM mode is on and FMU is good */ + /* do not mix if RAW_PWM mode is on and FMU is good */ if ((r_status_flags & PX4IO_P_STATUS_FLAGS_RAW_PWM) && - !(r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK)) { + (r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK)) { - /* don't actually mix anything - we already have raw PWM values or - not a valid mixer. */ + /* don't actually mix anything - we already have raw PWM values */ source = MIX_NONE; } else { @@ -196,10 +195,9 @@ mixer_tick(void) bool should_arm = ( /* FMU is armed */ (r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED) && /* IO is armed */ (r_status_flags & PX4IO_P_STATUS_FLAGS_ARMED) && - /* there is valid input via direct PWM or mixer */ (r_status_flags & (PX4IO_P_STATUS_FLAGS_RAW_PWM | PX4IO_P_STATUS_FLAGS_MIXER_OK)) && - /* IO initialised without error */ (r_status_flags & PX4IO_P_STATUS_FLAGS_INIT_OK) && - /* FMU is available or FMU is not available but override is an option */ - ((r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK) || (!(r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK) && (r_setup_arming & PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK) )) + /* there is valid input via direct PWM or mixer */ ((r_status_flags & (PX4IO_P_STATUS_FLAGS_RAW_PWM | PX4IO_P_STATUS_FLAGS_MIXER_OK)) || + /* or failsafe was set manually */ (r_setup_arming & PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM)) && + /* IO initialised without error */ (r_status_flags & PX4IO_P_STATUS_FLAGS_INIT_OK) ); if (should_arm && !mixer_servos_armed) { diff --git a/src/systemcmds/pwm/pwm.c b/src/systemcmds/pwm/pwm.c index e150b5a74b..570ca6aa93 100644 --- a/src/systemcmds/pwm/pwm.c +++ b/src/systemcmds/pwm/pwm.c @@ -180,7 +180,8 @@ pwm_main(int argc, char *argv[]) } /* iterate remaining arguments */ - unsigned channel = 0; + unsigned nchannels = 0; + unsigned channel[8] = {0}; while (argc--) { const char *arg = argv[0]; argv++; @@ -204,13 +205,15 @@ pwm_main(int argc, char *argv[]) } unsigned pwm_value = strtol(arg, &ep, 0); if (*ep == '\0') { - ret = ioctl(fd, PWM_SERVO_SET(channel), pwm_value); - if (ret != OK) - err(1, "PWM_SERVO_SET(%d)", channel); - channel++; + channel[nchannels] = pwm_value; + nchannels++; + + if (nchannels >= sizeof(channel) / sizeof(channel[0])) + err(1, "too many pwm values (max %d)", sizeof(channel) / sizeof(channel[0])); + continue; } - usage("unrecognised option"); + usage("unrecognized option"); } /* print verbose info */ @@ -250,5 +253,38 @@ pwm_main(int argc, char *argv[]) } fflush(stdout); } + + /* perform PWM output */ + if (nchannels) { + + /* Open console directly to grab CTRL-C signal */ + int console = open("/dev/console", O_NONBLOCK | O_RDONLY | O_NOCTTY); + if (!console) + err(1, "failed opening console"); + + warnx("Press CTRL-C or 'c' to abort."); + + while (1) { + for (int i = 0; i < nchannels; i++) { + ret = ioctl(fd, PWM_SERVO_SET(i), channel[i]); + if (ret != OK) + err(1, "PWM_SERVO_SET(%d)", i); + } + + /* abort on user request */ + char c; + if (read(console, &c, 1) == 1) { + if (c == 0x03 || c == 0x63) { + warnx("User abort\n"); + close(console); + exit(0); + } + } + + /* rate limit to ~ 20 Hz */ + usleep(50000); + } + } + exit(0); } \ No newline at end of file From d2c5990d6f0b1c3f4183a193c1c51250cbdfa127 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 9 Jun 2013 12:41:47 +0200 Subject: [PATCH 025/109] Fixed pwm count check --- src/systemcmds/pwm/pwm.c | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/systemcmds/pwm/pwm.c b/src/systemcmds/pwm/pwm.c index 570ca6aa93..619bd2c784 100644 --- a/src/systemcmds/pwm/pwm.c +++ b/src/systemcmds/pwm/pwm.c @@ -205,12 +205,12 @@ pwm_main(int argc, char *argv[]) } unsigned pwm_value = strtol(arg, &ep, 0); if (*ep == '\0') { + if (nchannels > sizeof(channel) / sizeof(channel[0])) + err(1, "too many pwm values (max %d)", sizeof(channel) / sizeof(channel[0])); + channel[nchannels] = pwm_value; nchannels++; - if (nchannels >= sizeof(channel) / sizeof(channel[0])) - err(1, "too many pwm values (max %d)", sizeof(channel) / sizeof(channel[0])); - continue; } usage("unrecognized option"); From 4ef87206eccd292eb5111bba7d5f39dd03f7e20c Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 9 Jun 2013 14:03:49 +0200 Subject: [PATCH 026/109] Code formatting and warning fixes --- src/drivers/blinkm/blinkm.cpp | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/src/drivers/blinkm/blinkm.cpp b/src/drivers/blinkm/blinkm.cpp index 3fabfd9a54..9fed1149d4 100644 --- a/src/drivers/blinkm/blinkm.cpp +++ b/src/drivers/blinkm/blinkm.cpp @@ -486,15 +486,15 @@ BlinkM::led() /* get number of used satellites in navigation */ num_of_used_sats = 0; - //for(int satloop=0; satloop<20; satloop++) { - for(int satloop=0; satloop checking cells\n"); for(num_of_cells = 2; num_of_cells < 7; num_of_cells++) { From b12678014ff9b500912ec44f6f9c771af3bdd217 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 9 Jun 2013 14:04:13 +0200 Subject: [PATCH 027/109] Fixed chan count logic --- src/systemcmds/pwm/pwm.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/systemcmds/pwm/pwm.c b/src/systemcmds/pwm/pwm.c index 619bd2c784..c42a36c7f3 100644 --- a/src/systemcmds/pwm/pwm.c +++ b/src/systemcmds/pwm/pwm.c @@ -274,7 +274,7 @@ pwm_main(int argc, char *argv[]) /* abort on user request */ char c; if (read(console, &c, 1) == 1) { - if (c == 0x03 || c == 0x63) { + if (c == 0x03 || c == 0x63 || c == 'q') { warnx("User abort\n"); close(console); exit(0); From 1deced7629e7d140a931c42657f75da512696c7e Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 9 Jun 2013 14:09:09 +0200 Subject: [PATCH 028/109] Added safety status feedback, disallow arming of a rotary wing with engaged safety --- src/drivers/px4io/px4io.cpp | 33 +++++++- src/modules/commander/commander.c | 82 +++++++++++++------- src/modules/commander/state_machine_helper.c | 35 ++++++++- src/modules/commander/state_machine_helper.h | 4 + src/modules/uORB/objects_common.cpp | 4 + src/modules/uORB/topics/actuator_controls.h | 7 +- src/modules/uORB/topics/safety.h | 60 ++++++++++++++ src/modules/uORB/topics/vehicle_status.h | 2 + 8 files changed, 189 insertions(+), 38 deletions(-) create mode 100644 src/modules/uORB/topics/safety.h diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 19163cebe3..bc65c4f259 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -80,6 +80,7 @@ #include #include #include +#include #include #include @@ -161,6 +162,7 @@ private: orb_advert_t _to_actuators_effective; ///< effective actuator controls topic orb_advert_t _to_outputs; ///< mixed servo outputs topic orb_advert_t _to_battery; ///< battery status / voltage + orb_advert_t _to_safety; ///< status of safety actuator_outputs_s _outputs; ///< mixed outputs actuator_controls_effective_s _controls_effective; ///< effective controls @@ -883,6 +885,25 @@ PX4IO::io_handle_status(uint16_t status) _status = status; } + /** + * Get and handle the safety status + */ + struct safety_s safety; + safety.timestamp = hrt_absolute_time(); + + if (status & PX4IO_P_STATUS_FLAGS_ARMED) { + safety.status = SAFETY_STATUS_UNLOCKED; + } else { + safety.status = SAFETY_STATUS_SAFE; + } + + /* lazily publish the safety status */ + if (_to_safety > 0) { + orb_publish(ORB_ID(safety), _to_safety, &safety); + } else { + _to_safety = orb_advertise(ORB_ID(safety), &safety); + } + return ret; } @@ -912,7 +933,7 @@ PX4IO::io_get_status() io_handle_status(regs[0]); io_handle_alarms(regs[1]); - + /* only publish if battery has a valid minimum voltage */ if (regs[2] > 3300) { battery_status_s battery_status; @@ -946,6 +967,7 @@ PX4IO::io_get_status() _to_battery = orb_advertise(ORB_ID(battery_status), &battery_status); } } + return ret; } @@ -1273,7 +1295,7 @@ PX4IO::print_status() 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", flags, - ((flags & PX4IO_P_STATUS_FLAGS_ARMED) ? " ARMED" : ""), + ((flags & PX4IO_P_STATUS_FLAGS_ARMED) ? " SAFETY_UNLOCKED" : ""), ((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" : ""), @@ -1634,6 +1656,11 @@ test(void) if (ioctl(fd, PWM_SERVO_GET_COUNT, (unsigned long)&servo_count)) err(1, "failed to get servo count"); + /* tell IO that its ok to disable its safety with the switch */ + ret = ioctl(fd, PWM_SERVO_SET_ARM_OK, 0); + if (ret != OK) + err(1, "PWM_SERVO_SET_ARM_OK"); + if (ioctl(fd, PWM_SERVO_ARM, 0)) err(1, "failed to arm servos"); @@ -1682,7 +1709,7 @@ test(void) /* Check if user wants to quit */ char c; if (read(console, &c, 1) == 1) { - if (c == 0x03 || c == 0x63) { + if (c == 0x03 || c == 0x63 || c == 'q') { warnx("User abort\n"); close(console); exit(0); diff --git a/src/modules/commander/commander.c b/src/modules/commander/commander.c index aab8f3e04d..937c515e69 100644 --- a/src/modules/commander/commander.c +++ b/src/modules/commander/commander.c @@ -81,6 +81,7 @@ #include #include #include +#include #include #include @@ -1281,6 +1282,8 @@ int commander_thread_main(int argc, char *argv[]) current_status.flag_vector_flight_mode_ok = false; /* set battery warning flag */ current_status.battery_warning = VEHICLE_BATTERY_WARNING_NONE; + /* set safety device detection flag */ + current_status.flag_safety_present = false; /* advertise to ORB */ stat_pub = orb_advertise(ORB_ID(vehicle_status), ¤t_status); @@ -1377,6 +1380,12 @@ int commander_thread_main(int argc, char *argv[]) memset(&battery, 0, sizeof(battery)); battery.voltage_v = 0.0f; + /* subscribe to safety topic */ + int safety_sub = orb_subscribe(ORB_ID(safety)); + struct safety_s safety; + memset(&safety, 0, sizeof(safety)); + safety.status = SAFETY_STATUS_NOT_PRESENT; + // uint8_t vehicle_state_previous = current_status.state_machine; float voltage_previous = 0.0f; @@ -1396,6 +1405,39 @@ int commander_thread_main(int argc, char *argv[]) /* Get current values */ bool new_data; + + /* update parameters */ + orb_check(param_changed_sub, &new_data); + + if (new_data || param_init_forced) { + param_init_forced = false; + /* parameters changed */ + orb_copy(ORB_ID(parameter_update), param_changed_sub, ¶m_changed); + + + /* update parameters */ + if (!current_status.flag_system_armed) { + if (param_get(_param_sys_type, &(current_status.system_type)) != OK) { + warnx("failed setting new system type"); + } + + /* disable manual override for all systems that rely on electronic stabilization */ + if (current_status.system_type == VEHICLE_TYPE_QUADROTOR || + current_status.system_type == VEHICLE_TYPE_HEXAROTOR || + current_status.system_type == VEHICLE_TYPE_OCTOROTOR) { + current_status.flag_external_manual_override_ok = false; + + } else { + current_status.flag_external_manual_override_ok = true; + } + + /* check and update system / component ID */ + param_get(_param_system_id, &(current_status.system_id)); + param_get(_param_component_id, &(current_status.component_id)); + + } + } + orb_check(sp_man_sub, &new_data); if (new_data) { @@ -1431,36 +1473,17 @@ int commander_thread_main(int argc, char *argv[]) handle_command(stat_pub, ¤t_status, &cmd); } - /* update parameters */ - orb_check(param_changed_sub, &new_data); + orb_check(safety_sub, &new_data); - if (new_data || param_init_forced) { - param_init_forced = false; - /* parameters changed */ - orb_copy(ORB_ID(parameter_update), param_changed_sub, ¶m_changed); + if (new_data) { + /* got safety change */ + orb_copy(ORB_ID(safety), safety_sub, &safety); - - /* update parameters */ - if (!current_status.flag_system_armed) { - if (param_get(_param_sys_type, &(current_status.system_type)) != OK) { - warnx("failed setting new system type"); - } - - /* disable manual override for all systems that rely on electronic stabilization */ - if (current_status.system_type == VEHICLE_TYPE_QUADROTOR || - current_status.system_type == VEHICLE_TYPE_HEXAROTOR || - current_status.system_type == VEHICLE_TYPE_OCTOROTOR) { - current_status.flag_external_manual_override_ok = false; - - } else { - current_status.flag_external_manual_override_ok = true; - } - - /* check and update system / component ID */ - param_get(_param_system_id, &(current_status.system_id)); - param_get(_param_component_id, &(current_status.component_id)); - - } + /* handle it */ + current_status.flag_safety_present = (safety.status != SAFETY_STATUS_NOT_PRESENT); + + if (current_status.flag_safety_present) + current_status.flag_safety_safe = (safety.status == SAFETY_STATUS_SAFE); } /* update global position estimate */ @@ -1699,7 +1722,8 @@ int commander_thread_main(int argc, char *argv[]) state_changed = true; } - if (orb_check(ORB_ID(vehicle_gps_position), &new_data)) { + orb_check(ORB_ID(vehicle_gps_position), &new_data); + if (new_data) { orb_copy(ORB_ID(vehicle_gps_position), gps_sub, &gps_position); diff --git a/src/modules/commander/state_machine_helper.c b/src/modules/commander/state_machine_helper.c index ab728c7bbb..ac603abfdc 100644 --- a/src/modules/commander/state_machine_helper.c +++ b/src/modules/commander/state_machine_helper.c @@ -65,6 +65,18 @@ static const char *system_state_txt[] = { }; +bool is_multirotor(const struct vehicle_status_s *current_status) +{ + return ((current_status->system_type == VEHICLE_TYPE_QUADROTOR) || + (current_status->system_type == VEHICLE_TYPE_HEXAROTOR) || + (current_status->system_type == VEHICLE_TYPE_OCTOROTOR)); +} + +bool is_rotary_wing(const struct vehicle_status_s *current_status) +{ + return is_multirotor(current_status); +} + /** * Transition from one state to another */ @@ -513,6 +525,25 @@ void update_state_machine_no_position_fix(int status_pub, struct vehicle_status_ void update_state_machine_arm(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd) { if (current_status->state_machine == SYSTEM_STATE_STANDBY) { + + /* reject arming if safety status is wrong */ + if (current_status->flag_safety_present) { + + /* + * The operator should not touch the vehicle if + * its armed, so we don't allow arming in the + * first place + */ + if (is_rotary_wing(current_status)) { + + /* safety is in safe position, disallow arming */ + if (current_status->flag_safety_safe) { + mavlink_log_critical(mavlink_fd, "DISENGAGE SAFETY BEFORE ARMING!"); + } + + } + } + printf("[cmd] arming\n"); do_state_update(status_pub, current_status, mavlink_fd, (commander_state_machine_t)SYSTEM_STATE_GROUND_READY); } @@ -538,9 +569,7 @@ void update_state_machine_mode_manual(int status_pub, struct vehicle_status_s *c current_status->flag_control_manual_enabled = true; /* set behaviour based on airframe */ - if ((current_status->system_type == VEHICLE_TYPE_QUADROTOR) || - (current_status->system_type == VEHICLE_TYPE_HEXAROTOR) || - (current_status->system_type == VEHICLE_TYPE_OCTOROTOR)) { + if (is_rotary_wing(current_status)) { /* assuming a rotary wing, set to SAS */ current_status->manual_control_mode = VEHICLE_MANUAL_CONTROL_MODE_SAS; diff --git a/src/modules/commander/state_machine_helper.h b/src/modules/commander/state_machine_helper.h index 2f2ccc7298..95b48d3261 100644 --- a/src/modules/commander/state_machine_helper.h +++ b/src/modules/commander/state_machine_helper.h @@ -47,6 +47,10 @@ #include #include +bool is_multirotor(const struct vehicle_status_s *current_status); + +bool is_rotary_wing(const struct vehicle_status_s *current_status); + /** * Switch to new state with no checking. * diff --git a/src/modules/uORB/objects_common.cpp b/src/modules/uORB/objects_common.cpp index 4197f6fb2d..e2df31651f 100644 --- a/src/modules/uORB/objects_common.cpp +++ b/src/modules/uORB/objects_common.cpp @@ -154,3 +154,7 @@ ORB_DEFINE(actuator_outputs_3, struct actuator_outputs_s); #include "topics/debug_key_value.h" ORB_DEFINE(debug_key_value, struct debug_key_value_s); + +/* status of the system safety device */ +#include "topics/safety.h" +ORB_DEFINE(safety, struct safety_s); diff --git a/src/modules/uORB/topics/actuator_controls.h b/src/modules/uORB/topics/actuator_controls.h index b7c4196c0a..745c5bc874 100644 --- a/src/modules/uORB/topics/actuator_controls.h +++ b/src/modules/uORB/topics/actuator_controls.h @@ -68,9 +68,10 @@ ORB_DECLARE(actuator_controls_3); /** global 'actuator output is live' control. */ struct actuator_armed_s { - bool armed; /**< Set to true if system is armed */ - bool ready_to_arm; /**< Set to true if system is ready to be armed */ - bool lockdown; /**< Set to true if actuators are forced to being disabled (due to emergency or HIL) */ + bool armed; /**< Set to true if system is armed */ + bool ready_to_arm; /**< Set to true if system is ready to be armed */ + bool throttle_locked; /**< Set to true if the trottle is locked to zero */ + bool lockdown; /**< Set to true if actuators are forced to being disabled (due to emergency or HIL) */ }; ORB_DECLARE(actuator_armed); diff --git a/src/modules/uORB/topics/safety.h b/src/modules/uORB/topics/safety.h new file mode 100644 index 0000000000..19e8e8d459 --- /dev/null +++ b/src/modules/uORB/topics/safety.h @@ -0,0 +1,60 @@ +/**************************************************************************** + * + * 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 safety.h + * + * Status of an attached safety device + */ + +#ifndef TOPIC_SAFETY_H +#define TOPIC_SAFETY_H + +#include +#include "../uORB.h" + +enum SAFETY_STATUS { + SAFETY_STATUS_NOT_PRESENT, + SAFETY_STATUS_SAFE, + SAFETY_STATUS_UNLOCKED +}; + +struct safety_s { + uint64_t timestamp; /**< output timestamp in us since system boot */ + enum SAFETY_STATUS status; +}; + +/* actuator output sets; this list can be expanded as more drivers emerge */ +ORB_DECLARE(safety); + +#endif /* TOPIC_SAFETY_H */ diff --git a/src/modules/uORB/topics/vehicle_status.h b/src/modules/uORB/topics/vehicle_status.h index c7c1048f60..07660614bb 100644 --- a/src/modules/uORB/topics/vehicle_status.h +++ b/src/modules/uORB/topics/vehicle_status.h @@ -214,6 +214,8 @@ struct vehicle_status_s bool flag_valid_launch_position; /**< indicates a valid launch position */ bool flag_valid_home_position; /**< indicates a valid home position (a valid home position is not always a valid launch) */ bool flag_airspeed_valid; /**< set to true by the commander app if there is a valid airspeed measurement available */ + bool flag_safety_present; /**< indicates that a safety switch is present */ + bool flag_safety_safe; /**< safety switch is in safe position */ }; /** From 8b67f88331a9dc65e5c947da177701317d77f8bd Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 9 Jun 2013 14:12:17 +0200 Subject: [PATCH 029/109] Play warning tune --- src/modules/commander/state_machine_helper.c | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/src/modules/commander/state_machine_helper.c b/src/modules/commander/state_machine_helper.c index ac603abfdc..e18c0edc3c 100644 --- a/src/modules/commander/state_machine_helper.c +++ b/src/modules/commander/state_machine_helper.c @@ -49,6 +49,7 @@ #include #include "state_machine_helper.h" +#include "commander.h" static const char *system_state_txt[] = { "SYSTEM_STATE_PREFLIGHT", @@ -539,6 +540,9 @@ void update_state_machine_arm(int status_pub, struct vehicle_status_s *current_s /* safety is in safe position, disallow arming */ if (current_status->flag_safety_safe) { mavlink_log_critical(mavlink_fd, "DISENGAGE SAFETY BEFORE ARMING!"); + + /* play warning tune */ + tune_error(); } } From 1028bd932cfd08366dd0dcb8c189ebcf88cce53a Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 10 Jun 2013 07:39:12 +0200 Subject: [PATCH 030/109] Extended vehicle detection --- src/modules/commander/state_machine_helper.c | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/src/modules/commander/state_machine_helper.c b/src/modules/commander/state_machine_helper.c index e18c0edc3c..0a6cfd0b5a 100644 --- a/src/modules/commander/state_machine_helper.c +++ b/src/modules/commander/state_machine_helper.c @@ -70,12 +70,14 @@ bool is_multirotor(const struct vehicle_status_s *current_status) { return ((current_status->system_type == VEHICLE_TYPE_QUADROTOR) || (current_status->system_type == VEHICLE_TYPE_HEXAROTOR) || - (current_status->system_type == VEHICLE_TYPE_OCTOROTOR)); + (current_status->system_type == VEHICLE_TYPE_OCTOROTOR) || + (current_status->system_type == VEHICLE_TYPE_TRICOPTER)); } bool is_rotary_wing(const struct vehicle_status_s *current_status) { - return is_multirotor(current_status); + return is_multirotor(current_status) || (current_status->system_type == VEHICLE_TYPE_HELICOPTER) + || (current_status->system_type == VEHICLE_TYPE_COAXIAL); } /** From eb76d116cc67c6354c29fa41e49b4cf9df1a472d Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 12 Jun 2013 12:30:42 +0200 Subject: [PATCH 031/109] Minor state machine improvements and fixes for IO safety / in-air restart handling --- src/drivers/px4io/px4io.cpp | 70 +++++++++++++++----- src/modules/commander/state_machine_helper.c | 3 + src/modules/mavlink/orb_listener.c | 2 +- src/modules/px4iofirmware/mixer.cpp | 11 +-- src/modules/px4iofirmware/protocol.h | 5 +- src/modules/px4iofirmware/registers.c | 13 ++-- src/modules/px4iofirmware/safety.c | 10 +-- 7 files changed, 78 insertions(+), 36 deletions(-) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index bc65c4f259..f033382a63 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -336,6 +336,7 @@ PX4IO::PX4IO() : _to_actuators_effective(0), _to_outputs(0), _to_battery(0), + _to_safety(0), _primary_pwm_device(false), _battery_amp_per_volt(90.0f/5.0f), // this matches the 3DR current sensor _battery_amp_bias(0), @@ -389,6 +390,30 @@ PX4IO::init() */ _retries = 2; + /* get IO's last seen FMU state */ + int val = io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING); + if (val == _io_reg_get_error) { + mavlink_log_emergency(_mavlink_fd, "[IO] ERROR! FAILED READING STATE"); + } + uint16_t arming = val; + + /* get basic software version */ + /* basic configuration */ + usleep(5000); + + unsigned proto_version = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_PROTOCOL_VERSION); + unsigned sw_version = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_SOFTWARE_VERSION); + + if (proto_version != PX4IO_P_CONFIG_PROTOCOL_VERSION) { + mavlink_log_emergency(_mavlink_fd, "[IO] ERROR! PROTO VER MISMATCH: v%u vs v%u\n", + proto_version, + PX4IO_P_CONFIG_PROTOCOL_VERSION); + + mavlink_log_emergency(_mavlink_fd, "[IO] Please update PX4IO firmware."); + log("protocol version mismatch (v%u on IO vs v%u on FMU)", proto_version, PX4IO_P_CONFIG_PROTOCOL_VERSION); + return 1; + } + /* get some parameters */ _max_actuators = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_ACTUATOR_COUNT); _max_controls = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_CONTROL_COUNT); @@ -414,21 +439,23 @@ PX4IO::init() * in this case. */ - uint16_t reg; + printf("arming 0x%04x%s%s%s%s\n", + arming, + ((arming & PX4IO_P_SETUP_ARMING_FMU_ARMED) ? " FMU_ARMED" : ""), + ((arming & PX4IO_P_SETUP_ARMING_IO_ARM_OK) ? " IO_ARM_OK" : ""), + ((arming & PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK) ? " MANUAL_OVERRIDE_OK" : ""), + ((arming & PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK) ? " INAIR_RESTART_OK" : "")); - /* get IO's last seen FMU state */ - ret = io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, ®, sizeof(reg)); - if (ret != OK) - return ret; /* * in-air restart is only tried if the IO board reports it is * already armed, and has been configured for in-air restart */ - if ((reg & PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK) && - (reg & PX4IO_P_SETUP_ARMING_FMU_ARMED)) { + if ((arming & PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK) && + (arming & PX4IO_P_SETUP_ARMING_FMU_ARMED)) { mavlink_log_emergency(_mavlink_fd, "[IO] RECOVERING FROM FMU IN-AIR RESTART"); + log("INAIR RESTART RECOVERY (needs commander app running)"); /* WARNING: COMMANDER app/vehicle status must be initialized. * If this fails (or the app is not started), worst-case IO @@ -482,7 +509,7 @@ PX4IO::init() cmd.confirmation = 1; /* send command once */ - (void)orb_advertise(ORB_ID(vehicle_command), &cmd); + orb_advert_t pub = orb_advertise(ORB_ID(vehicle_command), &cmd); /* spin here until IO's state has propagated into the system */ do { @@ -492,16 +519,22 @@ PX4IO::init() orb_copy(ORB_ID(vehicle_status), vstatus_sub, &status); } - /* wait 10 ms */ - usleep(10000); + /* wait 50 ms */ + usleep(50000); /* abort after 5s */ - if ((hrt_absolute_time() - try_start_time)/1000 > 50000) { + if ((hrt_absolute_time() - try_start_time)/1000 > 2000) { log("failed to recover from in-air restart (2), aborting IO driver init."); return 1; } - /* keep waiting for state change for 10 s */ + /* re-send if necessary */ + if (!status.flag_system_armed) { + orb_publish(ORB_ID(vehicle_command), pub, &cmd); + log("re-sending arm cmd"); + } + + /* keep waiting for state change for 2 s */ } while (!status.flag_system_armed); /* regular boot, no in-air restart, init IO */ @@ -540,7 +573,7 @@ PX4IO::init() return -errno; } - mavlink_log_info(_mavlink_fd, "[IO] init ok"); + mavlink_log_info(_mavlink_fd, "[IO] init ok (sw v.%u)", sw_version); return OK; } @@ -863,14 +896,14 @@ PX4IO::io_handle_status(uint16_t status) */ /* check for IO reset - force it back to armed if necessary */ - if (_status & PX4IO_P_STATUS_FLAGS_ARMED && !(status & PX4IO_P_STATUS_FLAGS_ARMED) + if (_status & PX4IO_P_STATUS_FLAGS_SAFETY_OFF && !(status & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) && !(status & PX4IO_P_STATUS_FLAGS_ARM_SYNC)) { /* set the arming flag */ - ret = io_reg_modify(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FLAGS, 0, PX4IO_P_STATUS_FLAGS_ARMED | PX4IO_P_STATUS_FLAGS_ARM_SYNC); + ret = io_reg_modify(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_FLAGS, 0, PX4IO_P_STATUS_FLAGS_SAFETY_OFF | PX4IO_P_STATUS_FLAGS_ARM_SYNC); /* set new status */ _status = status; - _status &= PX4IO_P_STATUS_FLAGS_ARMED; + _status &= PX4IO_P_STATUS_FLAGS_SAFETY_OFF; } else if (!(_status & PX4IO_P_STATUS_FLAGS_ARM_SYNC)) { /* set the sync flag */ @@ -891,7 +924,7 @@ PX4IO::io_handle_status(uint16_t status) struct safety_s safety; safety.timestamp = hrt_absolute_time(); - if (status & PX4IO_P_STATUS_FLAGS_ARMED) { + if (status & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) { safety.status = SAFETY_STATUS_UNLOCKED; } else { safety.status = SAFETY_STATUS_SAFE; @@ -1295,7 +1328,8 @@ PX4IO::print_status() 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", flags, - ((flags & PX4IO_P_STATUS_FLAGS_ARMED) ? " SAFETY_UNLOCKED" : ""), + ((flags & PX4IO_P_STATUS_FLAGS_OUTPUTS_ARMED) ? " OUTPUTS_ARMED" : ""), + ((flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) ? " SAFETY_OFF" : " SAFETY_SAFE"), ((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" : ""), diff --git a/src/modules/commander/state_machine_helper.c b/src/modules/commander/state_machine_helper.c index 0a6cfd0b5a..f42caad57b 100644 --- a/src/modules/commander/state_machine_helper.c +++ b/src/modules/commander/state_machine_helper.c @@ -545,6 +545,9 @@ void update_state_machine_arm(int status_pub, struct vehicle_status_s *current_s /* play warning tune */ tune_error(); + + /* abort */ + return; } } diff --git a/src/modules/mavlink/orb_listener.c b/src/modules/mavlink/orb_listener.c index 295cd5e28c..9b2d984f09 100644 --- a/src/modules/mavlink/orb_listener.c +++ b/src/modules/mavlink/orb_listener.c @@ -659,7 +659,7 @@ uorb_receive_thread(void *arg) /* handle the poll result */ if (poll_ret == 0) { - mavlink_missionlib_send_gcs_string("[mavlink] No telemetry data for 1 s"); + /* silent */ } else if (poll_ret < 0) { mavlink_missionlib_send_gcs_string("[mavlink] ERROR reading uORB data"); diff --git a/src/modules/px4iofirmware/mixer.cpp b/src/modules/px4iofirmware/mixer.cpp index 123eb67786..b654bdec49 100644 --- a/src/modules/px4iofirmware/mixer.cpp +++ b/src/modules/px4iofirmware/mixer.cpp @@ -194,7 +194,7 @@ mixer_tick(void) */ bool should_arm = ( /* FMU is armed */ (r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED) && - /* IO is armed */ (r_status_flags & PX4IO_P_STATUS_FLAGS_ARMED) && + /* IO is armed */ (r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) && /* there is valid input via direct PWM or mixer */ ((r_status_flags & (PX4IO_P_STATUS_FLAGS_RAW_PWM | PX4IO_P_STATUS_FLAGS_MIXER_OK)) || /* or failsafe was set manually */ (r_setup_arming & PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM)) && /* IO initialised without error */ (r_status_flags & PX4IO_P_STATUS_FLAGS_INIT_OK) @@ -204,11 +204,15 @@ mixer_tick(void) /* need to arm, but not armed */ up_pwm_servo_arm(true); mixer_servos_armed = true; + r_status_flags |= PX4IO_P_STATUS_FLAGS_OUTPUTS_ARMED; + isr_debug(5, "> armed"); } else if (!should_arm && mixer_servos_armed) { /* armed but need to disarm */ up_pwm_servo_arm(false); mixer_servos_armed = false; + r_status_flags &= ~(PX4IO_P_STATUS_FLAGS_OUTPUTS_ARMED); + isr_debug(5, "> disarmed"); } if (mixer_servos_armed) { @@ -263,9 +267,8 @@ static unsigned mixer_text_length = 0; void mixer_handle_text(const void *buffer, size_t length) { - /* do not allow a mixer change while fully armed */ - if (/* FMU is armed */ (r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED) && - /* IO is armed */ (r_status_flags & PX4IO_P_STATUS_FLAGS_ARMED)) { + /* do not allow a mixer change while outputs armed */ + if ((r_status_flags & PX4IO_P_STATUS_FLAGS_OUTPUTS_ARMED)) { return; } diff --git a/src/modules/px4iofirmware/protocol.h b/src/modules/px4iofirmware/protocol.h index 674f9dddd6..497e6af8e2 100644 --- a/src/modules/px4iofirmware/protocol.h +++ b/src/modules/px4iofirmware/protocol.h @@ -77,7 +77,7 @@ /* static configuration page */ #define PX4IO_PAGE_CONFIG 0 -#define PX4IO_P_CONFIG_PROTOCOL_VERSION 0 /* magic numbers TBD */ +#define PX4IO_P_CONFIG_PROTOCOL_VERSION 2 /* magic numbers TBD */ #define PX4IO_P_CONFIG_SOFTWARE_VERSION 1 /* magic numbers TBD */ #define PX4IO_P_CONFIG_BOOTLOADER_VERSION 2 /* get this how? */ #define PX4IO_P_CONFIG_MAX_TRANSFER 3 /* maximum I2C transfer size */ @@ -93,7 +93,7 @@ #define PX4IO_P_STATUS_CPULOAD 1 #define PX4IO_P_STATUS_FLAGS 2 /* monitoring flags */ -#define PX4IO_P_STATUS_FLAGS_ARMED (1 << 0) /* arm-ok and locally armed */ +#define PX4IO_P_STATUS_FLAGS_OUTPUTS_ARMED (1 << 0) /* arm-ok and locally armed */ #define PX4IO_P_STATUS_FLAGS_OVERRIDE (1 << 1) /* in manual override */ #define PX4IO_P_STATUS_FLAGS_RC_OK (1 << 2) /* RC input is valid */ #define PX4IO_P_STATUS_FLAGS_RC_PPM (1 << 3) /* PPM input is valid */ @@ -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_SAFETY_OFF (1 << 12) /* safety is off */ #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 */ diff --git a/src/modules/px4iofirmware/registers.c b/src/modules/px4iofirmware/registers.c index df7d6dcd35..a092fc93b5 100644 --- a/src/modules/px4iofirmware/registers.c +++ b/src/modules/px4iofirmware/registers.c @@ -317,9 +317,11 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value) * so that an in-air reset of FMU can not lead to a * lockup of the IO arming state. */ - if ((r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED) && !(value & PX4IO_P_SETUP_ARMING_FMU_ARMED)) { - r_status_flags &= ~PX4IO_P_STATUS_FLAGS_ARMED; - } + + // XXX do not reset IO's safety state by FMU for now + // if ((r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED) && !(value & PX4IO_P_SETUP_ARMING_FMU_ARMED)) { + // r_status_flags &= ~PX4IO_P_STATUS_FLAGS_ARMED; + // } r_setup_arming = value; @@ -367,9 +369,8 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value) case PX4IO_PAGE_RC_CONFIG: { - /* do not allow a RC config change while fully armed */ - if (/* FMU is armed */ (r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED) && - /* IO is armed */ (r_status_flags & PX4IO_P_STATUS_FLAGS_ARMED)) { + /* do not allow a RC config change while outputs armed */ + if ((r_status_flags & PX4IO_P_STATUS_FLAGS_OUTPUTS_ARMED)) { break; } diff --git a/src/modules/px4iofirmware/safety.c b/src/modules/px4iofirmware/safety.c index 4dbecc2744..95335f038a 100644 --- a/src/modules/px4iofirmware/safety.c +++ b/src/modules/px4iofirmware/safety.c @@ -110,7 +110,7 @@ safety_check_button(void *arg) * state machine, keep ARM_COUNTER_THRESHOLD the same * length in all cases of the if/else struct below. */ - if (safety_button_pressed && !(r_status_flags & PX4IO_P_STATUS_FLAGS_ARMED) && + if (safety_button_pressed && !(r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) && (r_setup_arming & PX4IO_P_SETUP_ARMING_IO_ARM_OK)) { if (counter < ARM_COUNTER_THRESHOLD) { @@ -118,18 +118,18 @@ safety_check_button(void *arg) } else if (counter == ARM_COUNTER_THRESHOLD) { /* switch to armed state */ - r_status_flags |= PX4IO_P_STATUS_FLAGS_ARMED; + r_status_flags |= PX4IO_P_STATUS_FLAGS_SAFETY_OFF; counter++; } - } else if (safety_button_pressed && (r_status_flags & PX4IO_P_STATUS_FLAGS_ARMED)) { + } else if (safety_button_pressed && (r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF)) { if (counter < ARM_COUNTER_THRESHOLD) { counter++; } else if (counter == ARM_COUNTER_THRESHOLD) { /* change to disarmed state and notify the FMU */ - r_status_flags &= ~PX4IO_P_STATUS_FLAGS_ARMED; + r_status_flags &= ~PX4IO_P_STATUS_FLAGS_SAFETY_OFF; counter++; } @@ -140,7 +140,7 @@ safety_check_button(void *arg) /* Select the appropriate LED flash pattern depending on the current IO/FMU arm state */ uint16_t pattern = LED_PATTERN_FMU_REFUSE_TO_ARM; - if (r_status_flags & PX4IO_P_STATUS_FLAGS_ARMED) { + if (r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) { if (r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED) { pattern = LED_PATTERN_IO_FMU_ARMED; From ec08dec8bae403f463ebf9e9a7b71b399ed7b97a Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Wed, 12 Jun 2013 12:47:00 +0200 Subject: [PATCH 032/109] Two hacks here to make it compile --- src/modules/gpio_led/gpio_led.c | 7 ++++--- src/modules/sdlog2/sdlog2.c | 20 +++++++++++++------- 2 files changed, 17 insertions(+), 10 deletions(-) diff --git a/src/modules/gpio_led/gpio_led.c b/src/modules/gpio_led/gpio_led.c index 43cbe74c74..542821e957 100644 --- a/src/modules/gpio_led/gpio_led.c +++ b/src/modules/gpio_led/gpio_led.c @@ -171,7 +171,8 @@ void gpio_led_cycle(FAR void *arg) /* select pattern for current status */ int pattern = 0; - if (priv->status.flag_system_armed) { + /* XXX fmu armed correct? */ + if (priv->status.flag_fmu_armed) { if (priv->status.battery_warning == VEHICLE_BATTERY_WARNING_NONE) { pattern = 0x3f; // ****** solid (armed) @@ -180,10 +181,10 @@ void gpio_led_cycle(FAR void *arg) } } else { - if (priv->status.state_machine == SYSTEM_STATE_PREFLIGHT) { + if (priv->status.arming_state == ARMING_STATE_STANDBY) { pattern = 0x00; // ______ off (disarmed, preflight check) - } else if (priv->status.state_machine == SYSTEM_STATE_STANDBY && + } else if (priv->status.arming_state == ARMING_STATE_STANDBY && priv->status.battery_warning == VEHICLE_BATTERY_WARNING_NONE) { pattern = 0x38; // ***___ slow blink (disarmed, ready) diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index a14bd6f80e..8c5ec092d1 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -861,11 +861,16 @@ int sdlog2_thread_main(int argc, char *argv[]) if (fds[ifds++].revents & POLLIN) { // Don't orb_copy, it's already done few lines above log_msg.msg_type = LOG_STAT_MSG; - log_msg.body.log_STAT.state = (unsigned char) buf_status.state_machine; - log_msg.body.log_STAT.flight_mode = (unsigned char) buf_status.flight_mode; - log_msg.body.log_STAT.manual_control_mode = (unsigned char) buf_status.manual_control_mode; - log_msg.body.log_STAT.manual_sas_mode = (unsigned char) buf_status.manual_sas_mode; - log_msg.body.log_STAT.armed = (unsigned char) buf_status.flag_system_armed; + // XXX fix this + // log_msg.body.log_STAT.state = (unsigned char) buf_status.state_machine; + // log_msg.body.log_STAT.flight_mode = (unsigned char) buf_status.flight_mode; + // log_msg.body.log_STAT.manual_control_mode = (unsigned char) buf_status.manual_control_mode; + // log_msg.body.log_STAT.manual_sas_mode = (unsigned char) buf_status.manual_sas_mode; + log_msg.body.log_STAT.state = 0; + log_msg.body.log_STAT.flight_mode = 0; + log_msg.body.log_STAT.manual_control_mode = 0; + log_msg.body.log_STAT.manual_sas_mode = 0; + log_msg.body.log_STAT.armed = (unsigned char) buf_status.flag_fmu_armed; /* XXX fmu armed correct? */ log_msg.body.log_STAT.battery_voltage = buf_status.voltage_battery; log_msg.body.log_STAT.battery_current = buf_status.current_battery; log_msg.body.log_STAT.battery_remaining = buf_status.battery_remaining; @@ -1165,8 +1170,9 @@ void handle_command(struct vehicle_command_s *cmd) void handle_status(struct vehicle_status_s *status) { - if (status->flag_system_armed != flag_system_armed) { - flag_system_armed = status->flag_system_armed; + /* XXX fmu armed correct? */ + if (status->flag_fmu_armed != flag_system_armed) { + flag_system_armed = status->flag_fmu_armed; if (flag_system_armed) { sdlog2_start_log(); From c3a8f177b6316a9cefd814e312742f47d3049739 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 12 Jun 2013 12:58:17 +0200 Subject: [PATCH 033/109] Software version check fixes --- src/drivers/px4io/px4io.cpp | 16 +++++++++++++--- src/modules/px4iofirmware/protocol.h | 7 +++++-- src/modules/px4iofirmware/registers.c | 4 ++-- 3 files changed, 20 insertions(+), 7 deletions(-) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index f033382a63..925041e0cd 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -404,13 +404,23 @@ PX4IO::init() unsigned proto_version = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_PROTOCOL_VERSION); unsigned sw_version = io_reg_get(PX4IO_PAGE_CONFIG, PX4IO_P_CONFIG_SOFTWARE_VERSION); - if (proto_version != PX4IO_P_CONFIG_PROTOCOL_VERSION) { + if (proto_version != PX4IO_P_CONFIG_PROTOCOL_VERSION_MAGIC) { mavlink_log_emergency(_mavlink_fd, "[IO] ERROR! PROTO VER MISMATCH: v%u vs v%u\n", proto_version, - PX4IO_P_CONFIG_PROTOCOL_VERSION); + PX4IO_P_CONFIG_PROTOCOL_VERSION_MAGIC); mavlink_log_emergency(_mavlink_fd, "[IO] Please update PX4IO firmware."); - log("protocol version mismatch (v%u on IO vs v%u on FMU)", proto_version, PX4IO_P_CONFIG_PROTOCOL_VERSION); + log("protocol version mismatch (v%u on IO vs v%u on FMU)", proto_version, PX4IO_P_CONFIG_PROTOCOL_VERSION_MAGIC); + return 1; + } + + if (sw_version != PX4IO_P_CONFIG_SOFTWARE_VERSION_MAGIC) { + mavlink_log_emergency(_mavlink_fd, "[IO] ERROR! SOFTWARE VER MISMATCH: v%u vs v%u\n", + proto_version, + PX4IO_P_CONFIG_SOFTWARE_VERSION_MAGIC); + + mavlink_log_emergency(_mavlink_fd, "[IO] Please update PX4IO firmware."); + log("software version mismatch (v%u on IO vs v%u on FMU)", sw_version, PX4IO_P_CONFIG_SOFTWARE_VERSION_MAGIC); return 1; } diff --git a/src/modules/px4iofirmware/protocol.h b/src/modules/px4iofirmware/protocol.h index 497e6af8e2..6c6c7b4e27 100644 --- a/src/modules/px4iofirmware/protocol.h +++ b/src/modules/px4iofirmware/protocol.h @@ -75,10 +75,13 @@ #define REG_TO_FLOAT(_reg) ((float)REG_TO_SIGNED(_reg) / 10000.0f) #define FLOAT_TO_REG(_float) SIGNED_TO_REG((int16_t)((_float) * 10000.0f)) +#define PX4IO_P_CONFIG_PROTOCOL_VERSION_MAGIC 2 +#define PX4IO_P_CONFIG_SOFTWARE_VERSION_MAGIC 2 + /* static configuration page */ #define PX4IO_PAGE_CONFIG 0 -#define PX4IO_P_CONFIG_PROTOCOL_VERSION 2 /* magic numbers TBD */ -#define PX4IO_P_CONFIG_SOFTWARE_VERSION 1 /* magic numbers TBD */ +#define PX4IO_P_CONFIG_PROTOCOL_VERSION 0 /* magic numbers */ +#define PX4IO_P_CONFIG_SOFTWARE_VERSION 1 /* magic numbers */ #define PX4IO_P_CONFIG_BOOTLOADER_VERSION 2 /* get this how? */ #define PX4IO_P_CONFIG_MAX_TRANSFER 3 /* maximum I2C transfer size */ #define PX4IO_P_CONFIG_CONTROL_COUNT 4 /* hardcoded max control count supported */ diff --git a/src/modules/px4iofirmware/registers.c b/src/modules/px4iofirmware/registers.c index a092fc93b5..148514455a 100644 --- a/src/modules/px4iofirmware/registers.c +++ b/src/modules/px4iofirmware/registers.c @@ -57,8 +57,8 @@ static void pwm_configure_rates(uint16_t map, uint16_t defaultrate, uint16_t alt * Static configuration parameters. */ static const uint16_t r_page_config[] = { - [PX4IO_P_CONFIG_PROTOCOL_VERSION] = 1, /* XXX hardcoded magic number */ - [PX4IO_P_CONFIG_SOFTWARE_VERSION] = 1, /* XXX hardcoded magic number */ + [PX4IO_P_CONFIG_PROTOCOL_VERSION] = PX4IO_P_CONFIG_PROTOCOL_VERSION_MAGIC, + [PX4IO_P_CONFIG_SOFTWARE_VERSION] = PX4IO_P_CONFIG_SOFTWARE_VERSION_MAGIC, [PX4IO_P_CONFIG_BOOTLOADER_VERSION] = 3, /* XXX hardcoded magic number */ [PX4IO_P_CONFIG_MAX_TRANSFER] = 64, /* XXX hardcoded magic number */ [PX4IO_P_CONFIG_CONTROL_COUNT] = PX4IO_CONTROL_CHANNELS, From 236053a600f5708aee0e5849f4fefc2380e7d101 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Thu, 13 Jun 2013 15:04:16 +0200 Subject: [PATCH 034/109] Fixed param save --- src/modules/commander/commander.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/commander/commander.c b/src/modules/commander/commander.c index a34e526a8b..c2a7242d11 100644 --- a/src/modules/commander/commander.c +++ b/src/modules/commander/commander.c @@ -955,7 +955,7 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta /* check if no other task is scheduled */ if(low_prio_task == LOW_PRIO_TASK_NONE) { - low_prio_task = LOW_PRIO_TASK_PARAM_LOAD; + low_prio_task = LOW_PRIO_TASK_PARAM_SAVE; result = VEHICLE_CMD_RESULT_ACCEPTED; } else { result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; From 90f5e30f2a177bed2ac08e76699ec3029292d640 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Fri, 14 Jun 2013 13:53:26 +0200 Subject: [PATCH 035/109] Introduced new actuator_safety topic --- .../ardrone_interface/ardrone_interface.c | 15 +- src/drivers/blinkm/blinkm.cpp | 21 ++- src/drivers/hil/hil.cpp | 17 +- src/drivers/mkblctrl/mkblctrl.cpp | 19 +- src/drivers/px4fmu/fmu.cpp | 27 +-- src/drivers/px4io/px4io.cpp | 41 ++--- src/modules/commander/commander.c | 170 ++++++++++-------- src/modules/commander/state_machine_helper.c | 25 ++- src/modules/commander/state_machine_helper.h | 3 +- src/modules/gpio_led/gpio_led.c | 16 +- src/modules/mavlink/orb_listener.c | 18 +- src/modules/mavlink/orb_topics.h | 3 +- src/modules/mavlink_onboard/mavlink.c | 8 +- src/modules/mavlink_onboard/orb_topics.h | 3 +- src/modules/mavlink_onboard/util.h | 2 +- .../multirotor_att_control_main.c | 21 ++- src/modules/sdlog2/sdlog2.c | 23 ++- src/modules/uORB/objects_common.cpp | 4 +- src/modules/uORB/topics/actuator_controls.h | 15 +- src/modules/uORB/topics/actuator_safety.h | 65 +++++++ .../uORB/topics/vehicle_control_mode.h | 96 ++++++++++ src/modules/uORB/topics/vehicle_status.h | 5 +- 22 files changed, 424 insertions(+), 193 deletions(-) create mode 100644 src/modules/uORB/topics/actuator_safety.h create mode 100644 src/modules/uORB/topics/vehicle_control_mode.h diff --git a/src/drivers/ardrone_interface/ardrone_interface.c b/src/drivers/ardrone_interface/ardrone_interface.c index 264041e65a..4a6f30a4f1 100644 --- a/src/drivers/ardrone_interface/ardrone_interface.c +++ b/src/drivers/ardrone_interface/ardrone_interface.c @@ -55,6 +55,7 @@ #include #include #include +#include #include @@ -247,13 +248,13 @@ int ardrone_interface_thread_main(int argc, char *argv[]) memset(&state, 0, sizeof(state)); struct actuator_controls_s actuator_controls; memset(&actuator_controls, 0, sizeof(actuator_controls)); - struct actuator_armed_s armed; - armed.armed = false; + struct actuator_safety_s safety; + safety.armed = false; /* subscribe to attitude, motor setpoints and system state */ int actuator_controls_sub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS); int state_sub = orb_subscribe(ORB_ID(vehicle_status)); - int armed_sub = orb_subscribe(ORB_ID(actuator_armed)); + int safety_sub = orb_subscribe(ORB_ID(actuator_safety)); printf("[ardrone_interface] Motors initialized - ready.\n"); fflush(stdout); @@ -322,18 +323,14 @@ int ardrone_interface_thread_main(int argc, char *argv[]) } else { /* MAIN OPERATION MODE */ - /* get a local copy of the vehicle state */ - orb_copy(ORB_ID(vehicle_status), state_sub, &state); /* get a local copy of the actuator controls */ orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_controls_sub, &actuator_controls); - orb_copy(ORB_ID(actuator_armed), armed_sub, &armed); + orb_copy(ORB_ID(actuator_safety), safety_sub, &safety); /* for now only spin if armed and immediately shut down * if in failsafe */ - //XXX fix this - //if (armed.armed && !armed.lockdown) { - if (state.flag_fmu_armed) { + if (safety.armed && !safety.lockdown) { ardrone_mixing_and_output(ardrone_write, &actuator_controls); } else { diff --git a/src/drivers/blinkm/blinkm.cpp b/src/drivers/blinkm/blinkm.cpp index 7a64b72a4e..1cfdcfbedd 100644 --- a/src/drivers/blinkm/blinkm.cpp +++ b/src/drivers/blinkm/blinkm.cpp @@ -116,6 +116,7 @@ #include #include #include +#include #include static const float MAX_CELL_VOLTAGE = 4.3f; @@ -376,6 +377,7 @@ BlinkM::led() static int vehicle_status_sub_fd; static int vehicle_gps_position_sub_fd; + static int actuator_safety_sub_fd; static int num_of_cells = 0; static int detected_cells_runcount = 0; @@ -386,6 +388,7 @@ BlinkM::led() static int led_interval = 1000; static int no_data_vehicle_status = 0; + static int no_data_actuator_safety = 0; static int no_data_vehicle_gps_position = 0; static bool topic_initialized = false; @@ -398,6 +401,9 @@ BlinkM::led() vehicle_status_sub_fd = orb_subscribe(ORB_ID(vehicle_status)); orb_set_interval(vehicle_status_sub_fd, 1000); + actuator_safety_sub_fd = orb_subscribe(ORB_ID(actuator_safety)); + orb_set_interval(actuator_safety_sub_fd, 1000); + vehicle_gps_position_sub_fd = orb_subscribe(ORB_ID(vehicle_gps_position)); orb_set_interval(vehicle_gps_position_sub_fd, 1000); @@ -452,12 +458,14 @@ BlinkM::led() if (led_thread_runcount == 15) { /* obtained data for the first file descriptor */ struct vehicle_status_s vehicle_status_raw; + struct actuator_safety_s actuator_safety; struct vehicle_gps_position_s vehicle_gps_position_raw; memset(&vehicle_status_raw, 0, sizeof(vehicle_status_raw)); memset(&vehicle_gps_position_raw, 0, sizeof(vehicle_gps_position_raw)); bool new_data_vehicle_status; + bool new_data_actuator_safety; bool new_data_vehicle_gps_position; orb_check(vehicle_status_sub_fd, &new_data_vehicle_status); @@ -471,6 +479,17 @@ BlinkM::led() no_data_vehicle_status = 3; } + orb_check(actuator_safety_sub_fd, &new_data_actuator_safety); + + if (new_data_actuator_safety) { + orb_copy(ORB_ID(actuator_safety), actuator_safety_sub_fd, &actuator_safety); + no_data_actuator_safety = 0; + } else { + no_data_actuator_safety++; + if(no_data_vehicle_status >= 3) + no_data_vehicle_status = 3; + } + orb_check(vehicle_gps_position_sub_fd, &new_data_vehicle_gps_position); if (new_data_vehicle_gps_position) { @@ -530,7 +549,7 @@ BlinkM::led() } else { /* no battery warnings here */ - if(vehicle_status_raw.flag_fmu_armed == false) { + if(actuator_safety.armed == false) { /* system not armed */ led_color_1 = LED_RED; led_color_2 = LED_RED; diff --git a/src/drivers/hil/hil.cpp b/src/drivers/hil/hil.cpp index d9aa772d4d..e851d8a52b 100644 --- a/src/drivers/hil/hil.cpp +++ b/src/drivers/hil/hil.cpp @@ -75,6 +75,7 @@ #include #include +#include #include #include @@ -108,7 +109,7 @@ private: int _current_update_rate; int _task; int _t_actuators; - int _t_armed; + int _t_safety; orb_advert_t _t_outputs; unsigned _num_outputs; bool _primary_pwm_device; @@ -161,7 +162,7 @@ HIL::HIL() : _current_update_rate(0), _task(-1), _t_actuators(-1), - _t_armed(-1), + _t_safety(-1), _t_outputs(0), _num_outputs(0), _primary_pwm_device(false), @@ -321,8 +322,8 @@ HIL::task_main() /* force a reset of the update rate */ _current_update_rate = 0; - _t_armed = orb_subscribe(ORB_ID(actuator_armed)); - orb_set_interval(_t_armed, 200); /* 5Hz update rate */ + _t_safety = orb_subscribe(ORB_ID(actuator_safety)); + orb_set_interval(_t_safety, 200); /* 5Hz update rate */ /* advertise the mixed control outputs */ actuator_outputs_s outputs; @@ -334,7 +335,7 @@ HIL::task_main() pollfd fds[2]; fds[0].fd = _t_actuators; fds[0].events = POLLIN; - fds[1].fd = _t_armed; + fds[1].fd = _t_safety; fds[1].events = POLLIN; unsigned num_outputs; @@ -426,15 +427,15 @@ HIL::task_main() /* how about an arming update? */ if (fds[1].revents & POLLIN) { - actuator_armed_s aa; + actuator_safety_s aa; /* get new value */ - orb_copy(ORB_ID(actuator_armed), _t_armed, &aa); + orb_copy(ORB_ID(actuator_safety), _t_safety, &aa); } } ::close(_t_actuators); - ::close(_t_armed); + ::close(_t_safety); /* make sure servos are off */ // up_pwm_servo_deinit(); diff --git a/src/drivers/mkblctrl/mkblctrl.cpp b/src/drivers/mkblctrl/mkblctrl.cpp index c67276f8af..093b53d426 100644 --- a/src/drivers/mkblctrl/mkblctrl.cpp +++ b/src/drivers/mkblctrl/mkblctrl.cpp @@ -74,6 +74,7 @@ #include #include #include +#include #include @@ -132,7 +133,7 @@ private: int _current_update_rate; int _task; int _t_actuators; - int _t_armed; + int _t_actuator_safety; unsigned int _motor; int _px4mode; int _frametype; @@ -240,7 +241,7 @@ MK::MK(int bus) : _update_rate(50), _task(-1), _t_actuators(-1), - _t_armed(-1), + _t_actuator_safety(-1), _t_outputs(0), _t_actuators_effective(0), _num_outputs(0), @@ -496,8 +497,8 @@ MK::task_main() /* force a reset of the update rate */ _current_update_rate = 0; - _t_armed = orb_subscribe(ORB_ID(actuator_armed)); - orb_set_interval(_t_armed, 200); /* 5Hz update rate */ + _t_actuator_safety = orb_subscribe(ORB_ID(actuator_safety)); + orb_set_interval(_t_actuator_safety, 200); /* 5Hz update rate */ /* advertise the mixed control outputs */ actuator_outputs_s outputs; @@ -519,7 +520,7 @@ MK::task_main() pollfd fds[2]; fds[0].fd = _t_actuators; fds[0].events = POLLIN; - fds[1].fd = _t_armed; + fds[1].fd = _t_actuator_safety; fds[1].events = POLLIN; log("starting"); @@ -625,13 +626,13 @@ MK::task_main() /* how about an arming update? */ if (fds[1].revents & POLLIN) { - actuator_armed_s aa; + actuator_safety_s as; /* get new value */ - orb_copy(ORB_ID(actuator_armed), _t_armed, &aa); + orb_copy(ORB_ID(actuator_safety), _t_actuator_safety, &as); /* update PWM servo armed status if armed and not locked down */ - mk_servo_arm(aa.armed && !aa.lockdown); + mk_servo_arm(as.armed && !as.lockdown); } @@ -639,7 +640,7 @@ MK::task_main() ::close(_t_actuators); ::close(_t_actuators_effective); - ::close(_t_armed); + ::close(_t_actuator_safety); /* make sure servos are off */ diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp index bf72892ebe..db4c87ddc7 100644 --- a/src/drivers/px4fmu/fmu.cpp +++ b/src/drivers/px4fmu/fmu.cpp @@ -69,6 +69,7 @@ #include #include #include +#include #include #include @@ -104,7 +105,7 @@ private: unsigned _current_update_rate; int _task; int _t_actuators; - int _t_armed; + int _t_actuator_safety; orb_advert_t _t_outputs; orb_advert_t _t_actuators_effective; unsigned _num_outputs; @@ -174,7 +175,7 @@ PX4FMU::PX4FMU() : _current_update_rate(0), _task(-1), _t_actuators(-1), - _t_armed(-1), + _t_actuator_safety(-1), _t_outputs(0), _t_actuators_effective(0), _num_outputs(0), @@ -376,8 +377,8 @@ PX4FMU::task_main() /* force a reset of the update rate */ _current_update_rate = 0; - _t_armed = orb_subscribe(ORB_ID(actuator_armed)); - orb_set_interval(_t_armed, 200); /* 5Hz update rate */ + _t_actuator_safety = orb_subscribe(ORB_ID(actuator_safety)); + orb_set_interval(_t_actuator_safety, 200); /* 5Hz update rate */ /* advertise the mixed control outputs */ actuator_outputs_s outputs; @@ -396,7 +397,7 @@ PX4FMU::task_main() pollfd fds[2]; fds[0].fd = _t_actuators; fds[0].events = POLLIN; - fds[1].fd = _t_armed; + fds[1].fd = _t_actuator_safety; fds[1].events = POLLIN; unsigned num_outputs = (_mode == MODE_2PWM) ? 2 : 4; @@ -499,13 +500,13 @@ PX4FMU::task_main() /* how about an arming update? */ if (fds[1].revents & POLLIN) { - actuator_armed_s aa; + actuator_safety_s as; /* get new value */ - orb_copy(ORB_ID(actuator_armed), _t_armed, &aa); + orb_copy(ORB_ID(actuator_safety), _t_actuator_safety, &as); /* update PWM servo armed status if armed and not locked down */ - bool set_armed = aa.armed && !aa.lockdown; + bool set_armed = as.armed && !as.lockdown; if (set_armed != _armed) { _armed = set_armed; up_pwm_servo_arm(set_armed); @@ -535,7 +536,7 @@ PX4FMU::task_main() ::close(_t_actuators); ::close(_t_actuators_effective); - ::close(_t_armed); + ::close(_t_actuator_safety); /* make sure servos are off */ up_pwm_servo_deinit(); @@ -1021,12 +1022,12 @@ fake(int argc, char *argv[]) if (handle < 0) errx(1, "advertise failed"); - actuator_armed_s aa; + actuator_safety_s as; - aa.armed = true; - aa.lockdown = false; + as.armed = true; + as.lockdown = false; - handle = orb_advertise(ORB_ID(actuator_armed), &aa); + handle = orb_advertise(ORB_ID(actuator_safety), &as); if (handle < 0) errx(1, "advertise failed 2"); diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 962a91c7fe..28a3eb917b 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -75,6 +75,7 @@ #include #include #include +#include #include #include #include @@ -152,7 +153,7 @@ private: /* subscribed topics */ int _t_actuators; ///< actuator controls topic - int _t_armed; ///< system armed control topic + int _t_actuator_safety; ///< system armed control topic int _t_vstatus; ///< system / vehicle status int _t_param; ///< parameter update topic @@ -327,7 +328,7 @@ PX4IO::PX4IO() : _status(0), _alarms(0), _t_actuators(-1), - _t_armed(-1), + _t_actuator_safety(-1), _t_vstatus(-1), _t_param(-1), _to_input_rc(0), @@ -433,20 +434,20 @@ PX4IO::init() * remains untouched (so manual override is still available). */ - int vstatus_sub = orb_subscribe(ORB_ID(vehicle_status)); + int safety_sub = orb_subscribe(ORB_ID(actuator_safety)); /* fill with initial values, clear updated flag */ - vehicle_status_s status; + struct actuator_safety_s armed; uint64_t try_start_time = hrt_absolute_time(); bool updated = false; - /* keep checking for an update, ensure we got a recent state, + /* keep checking for an update, ensure we got a arming information, not something that was published a long time ago. */ do { - orb_check(vstatus_sub, &updated); + orb_check(safety_sub, &updated); if (updated) { /* got data, copy and exit loop */ - orb_copy(ORB_ID(vehicle_status), vstatus_sub, &status); + orb_copy(ORB_ID(actuator_safety), safety_sub, &armed); break; } @@ -472,10 +473,10 @@ PX4IO::init() cmd.param6 = 0; cmd.param7 = 0; cmd.command = VEHICLE_CMD_COMPONENT_ARM_DISARM; - cmd.target_system = status.system_id; - cmd.target_component = status.component_id; - cmd.source_system = status.system_id; - cmd.source_component = status.component_id; + // cmd.target_system = status.system_id; + // cmd.target_component = status.component_id; + // cmd.source_system = status.system_id; + // cmd.source_component = status.component_id; /* ask to confirm command */ cmd.confirmation = 1; @@ -484,10 +485,10 @@ PX4IO::init() /* spin here until IO's state has propagated into the system */ do { - orb_check(vstatus_sub, &updated); + orb_check(safety_sub, &updated); if (updated) { - orb_copy(ORB_ID(vehicle_status), vstatus_sub, &status); + orb_copy(ORB_ID(actuator_safety), safety_sub, &armed); } /* wait 10 ms */ @@ -500,7 +501,7 @@ PX4IO::init() } /* keep waiting for state change for 10 s */ - } while (!status.flag_fmu_armed); + } while (!armed.armed); /* regular boot, no in-air restart, init IO */ } else { @@ -564,8 +565,8 @@ PX4IO::task_main() ORB_ID(actuator_controls_1)); orb_set_interval(_t_actuators, 20); /* default to 50Hz */ - _t_armed = orb_subscribe(ORB_ID(actuator_armed)); - orb_set_interval(_t_armed, 200); /* 5Hz update rate */ + _t_actuator_safety = orb_subscribe(ORB_ID(actuator_safety)); + orb_set_interval(_t_actuator_safety, 200); /* 5Hz update rate */ _t_vstatus = orb_subscribe(ORB_ID(vehicle_status)); orb_set_interval(_t_vstatus, 200); /* 5Hz update rate max. */ @@ -577,7 +578,7 @@ PX4IO::task_main() pollfd fds[4]; fds[0].fd = _t_actuators; fds[0].events = POLLIN; - fds[1].fd = _t_armed; + fds[1].fd = _t_actuator_safety; fds[1].events = POLLIN; fds[2].fd = _t_vstatus; fds[2].events = POLLIN; @@ -713,16 +714,16 @@ PX4IO::set_failsafe_values(const uint16_t *vals, unsigned len) int PX4IO::io_set_arming_state() { - actuator_armed_s armed; ///< system armed state + actuator_safety_s safety; ///< system armed state vehicle_status_s vstatus; ///< overall system state - orb_copy(ORB_ID(actuator_armed), _t_armed, &armed); + orb_copy(ORB_ID(actuator_safety), _t_actuator_safety, &safety); orb_copy(ORB_ID(vehicle_status), _t_vstatus, &vstatus); uint16_t set = 0; uint16_t clear = 0; - if (armed.armed && !armed.lockdown) { + if (safety.armed && !safety.lockdown) { set |= PX4IO_P_SETUP_ARMING_FMU_ARMED; } else { clear |= PX4IO_P_SETUP_ARMING_FMU_ARMED; diff --git a/src/modules/commander/commander.c b/src/modules/commander/commander.c index c2a7242d11..6812fb1fb8 100644 --- a/src/modules/commander/commander.c +++ b/src/modules/commander/commander.c @@ -73,8 +73,10 @@ #include #include #include +#include #include #include +#include #include #include #include @@ -137,10 +139,6 @@ static bool thread_should_exit = false; /**< daemon exit flag */ static bool thread_running = false; /**< daemon status flag */ static int daemon_task; /**< Handle of daemon task / thread */ -/* Main state machine */ -static struct vehicle_status_s current_status; -static orb_advert_t stat_pub; - /* timout until lowlevel failsafe */ static unsigned int failsafe_lowlevel_timeout_ms; @@ -187,7 +185,7 @@ void do_rc_calibration(void); void do_airspeed_calibration(void); -void handle_command(int status_pub, struct vehicle_status_s *current_status, struct vehicle_command_s *cmd); +void handle_command(int status_pub, struct vehicle_status_s *current_status, struct vehicle_command_s *cmd, int safety_pub, struct actuator_safety_s *safety); int trigger_audio_alarm(uint8_t old_mode, uint8_t old_state, uint8_t new_mode, uint8_t new_state); @@ -304,10 +302,11 @@ void do_rc_calibration() { mavlink_log_info(mavlink_fd, "trim calibration starting"); - if (current_status.offboard_control_signal_lost) { - mavlink_log_critical(mavlink_fd, "TRIM CAL: ABORT. No RC signal."); - return; - } + /* XXX fix this */ + // if (current_status.offboard_control_signal_lost) { + // mavlink_log_critical(mavlink_fd, "TRIM CAL: ABORT. No RC signal."); + // return; + // } int sub_man = orb_subscribe(ORB_ID(manual_control_setpoint)); struct manual_control_setpoint_s sp; @@ -718,7 +717,7 @@ void do_airspeed_calibration() close(diff_pres_sub); } -void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_status, struct vehicle_command_s *cmd) +void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_status, struct vehicle_command_s *cmd, int safety_pub, struct actuator_safety_s *safety) { /* result of the command */ uint8_t result = VEHICLE_CMD_RESULT_UNSUPPORTED; @@ -738,13 +737,13 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta } if ((int)cmd->param1 & VEHICLE_MODE_FLAG_SAFETY_ARMED) { - if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_ARMED, mavlink_fd)) { + if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_ARMED, safety_pub, safety, mavlink_fd)) { result = VEHICLE_CMD_RESULT_ACCEPTED; } else { result = VEHICLE_CMD_RESULT_DENIED; } } else { - if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_STANDBY, mavlink_fd)) { + if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_STANDBY, safety_pub, safety, mavlink_fd)) { result = VEHICLE_CMD_RESULT_ACCEPTED; } else { result = VEHICLE_CMD_RESULT_DENIED; @@ -757,14 +756,14 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta /* request to arm */ if ((int)cmd->param1 == 1) { - if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_ARMED, mavlink_fd)) { + if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_ARMED, safety_pub, safety, mavlink_fd)) { result = VEHICLE_CMD_RESULT_ACCEPTED; } else { result = VEHICLE_CMD_RESULT_DENIED; } /* request to disarm */ } else if ((int)cmd->param1 == 0) { - if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_STANDBY, mavlink_fd)) { + if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_STANDBY, safety_pub, safety, mavlink_fd)) { result = VEHICLE_CMD_RESULT_ACCEPTED; } else { result = VEHICLE_CMD_RESULT_DENIED; @@ -777,7 +776,7 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta /* request for an autopilot reboot */ if ((int)cmd->param1 == 1) { - if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_REBOOT, mavlink_fd)) { + if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_REBOOT, safety_pub, safety, mavlink_fd)) { /* reboot is executed later, after positive tune is sent */ result = VEHICLE_CMD_RESULT_ACCEPTED; tune_positive(); @@ -826,7 +825,7 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta if(low_prio_task == LOW_PRIO_TASK_NONE) { /* try to go to INIT/PREFLIGHT arming state */ - if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_INIT, mavlink_fd)) { + if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_INIT, safety_pub, safety, mavlink_fd)) { result = VEHICLE_CMD_RESULT_ACCEPTED; /* now set the task for the low prio thread */ low_prio_task = LOW_PRIO_TASK_GYRO_CALIBRATION; @@ -845,7 +844,7 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta if(low_prio_task == LOW_PRIO_TASK_NONE) { /* try to go to INIT/PREFLIGHT arming state */ - if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_INIT, mavlink_fd)) { + if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_INIT, safety_pub, safety, mavlink_fd)) { result = VEHICLE_CMD_RESULT_ACCEPTED; /* now set the task for the low prio thread */ low_prio_task = LOW_PRIO_TASK_MAG_CALIBRATION; @@ -865,7 +864,7 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta // if(low_prio_task == LOW_PRIO_TASK_NONE) { // /* try to go to INIT/PREFLIGHT arming state */ - // if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_INIT, mavlink_fd)) { + // if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_INIT, safety_pub, safety, mavlink_fd)) { // result = VEHICLE_CMD_RESULT_ACCEPTED; // /* now set the task for the low prio thread */ // low_prio_task = LOW_PRIO_TASK_ALTITUDE_CALIBRATION; @@ -886,7 +885,7 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta // if(low_prio_task == LOW_PRIO_TASK_NONE) { // /* try to go to INIT/PREFLIGHT arming state */ - // if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_INIT, mavlink_fd)) { + // if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_INIT, safety_pub, safety, mavlink_fd)) { // result = VEHICLE_CMD_RESULT_ACCEPTED; // /* now set the task for the low prio thread */ // low_prio_task = LOW_PRIO_TASK_RC_CALIBRATION; @@ -906,7 +905,7 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta if(low_prio_task == LOW_PRIO_TASK_NONE) { /* try to go to INIT/PREFLIGHT arming state */ - if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_INIT, mavlink_fd)) { + if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_INIT, safety_pub, safety, mavlink_fd)) { result = VEHICLE_CMD_RESULT_ACCEPTED; /* now set the task for the low prio thread */ low_prio_task = LOW_PRIO_TASK_ACCEL_CALIBRATION; @@ -925,7 +924,7 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta if(low_prio_task == LOW_PRIO_TASK_NONE) { /* try to go to INIT/PREFLIGHT arming state */ - if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_INIT, mavlink_fd)) { + if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_INIT, safety_pub, safety, mavlink_fd)) { result = VEHICLE_CMD_RESULT_ACCEPTED; /* now set the task for the low prio thread */ low_prio_task = LOW_PRIO_TASK_AIRSPEED_CALIBRATION; @@ -1189,17 +1188,30 @@ int commander_thread_main(int argc, char *argv[]) if (mavlink_fd < 0) { warnx("ERROR: Failed to open MAVLink log stream, start mavlink app first."); } - + + /* Main state machine */ + struct vehicle_status_s current_status; + orb_advert_t status_pub; /* make sure we are in preflight state */ memset(¤t_status, 0, sizeof(current_status)); + /* safety topic */ + struct actuator_safety_s safety; + orb_advert_t safety_pub; + /* Initialize safety with all false */ + memset(&safety, 0, sizeof(safety)); + + + /* flags for control apps */ + struct vehicle_control_mode_s control_mode; + orb_advert_t control_mode_pub; + + /* Initialize all flags to false */ + memset(&control_mode, 0, sizeof(control_mode)); current_status.navigation_state = NAVIGATION_STATE_INIT; current_status.arming_state = ARMING_STATE_INIT; current_status.hil_state = HIL_STATE_OFF; - current_status.flag_hil_enabled = false; - current_status.flag_fmu_armed = false; - current_status.flag_io_armed = false; // XXX read this from somewhere /* neither manual nor offboard control commands have been received */ current_status.offboard_control_signal_found_once = false; @@ -1222,16 +1234,18 @@ int commander_thread_main(int argc, char *argv[]) current_status.condition_system_sensors_initialized = true; /* advertise to ORB */ - stat_pub = orb_advertise(ORB_ID(vehicle_status), ¤t_status); + status_pub = orb_advertise(ORB_ID(vehicle_status), ¤t_status); /* publish current state machine */ - state_machine_publish(stat_pub, ¤t_status, mavlink_fd); + state_machine_publish(status_pub, ¤t_status, mavlink_fd); + + safety_pub = orb_advertise(ORB_ID(actuator_safety), &safety); /* home position */ orb_advert_t home_pub = -1; struct home_position_s home; memset(&home, 0, sizeof(home)); - if (stat_pub < 0) { + if (status_pub < 0) { warnx("ERROR: orb_advertise for topic vehicle_status failed (uorb app running?).\n"); warnx("exiting."); exit(ERROR); @@ -1333,6 +1347,7 @@ int commander_thread_main(int argc, char *argv[]) memset(&battery, 0, sizeof(battery)); battery.voltage_v = 0.0f; + // uint8_t vehicle_state_previous = current_status.state_machine; float voltage_previous = 0.0f; @@ -1386,9 +1401,11 @@ int commander_thread_main(int argc, char *argv[]) orb_copy(ORB_ID(vehicle_command), cmd_sub, &cmd); /* handle it */ - handle_command(stat_pub, ¤t_status, &cmd); + handle_command(status_pub, ¤t_status, &cmd, safety_pub, &safety); } + + /* update parameters */ orb_check(param_changed_sub, &new_data); @@ -1397,9 +1414,8 @@ int commander_thread_main(int argc, char *argv[]) /* parameters changed */ orb_copy(ORB_ID(parameter_update), param_changed_sub, ¶m_changed); - /* update parameters */ - if (!current_status.flag_fmu_armed) { + if (!safety.armed) { if (param_get(_param_sys_type, &(current_status.system_type)) != OK) { warnx("failed setting new system type"); } @@ -1417,7 +1433,6 @@ int commander_thread_main(int argc, char *argv[]) /* check and update system / component ID */ param_get(_param_system_id, &(current_status.system_id)); param_get(_param_component_id, &(current_status.component_id)); - } } @@ -1564,7 +1579,7 @@ int commander_thread_main(int argc, char *argv[]) mavlink_log_critical(mavlink_fd, "[cmd] EMERGENCY! CRITICAL BATTERY!"); current_status.battery_warning = VEHICLE_BATTERY_WARNING_ALERT; // XXX implement this - // state_machine_emergency(stat_pub, ¤t_status, mavlink_fd); + // state_machine_emergency(status_pub, ¤t_status, mavlink_fd); } critical_voltage_counter++; @@ -1579,7 +1594,7 @@ int commander_thread_main(int argc, char *argv[]) /* If in INIT state, try to proceed to STANDBY state */ if (current_status.arming_state == ARMING_STATE_INIT) { // XXX check for sensors - arming_state_transition(stat_pub, ¤t_status, ARMING_STATE_STANDBY, mavlink_fd); + arming_state_transition(status_pub, ¤t_status, ARMING_STATE_STANDBY, safety_pub, &safety, mavlink_fd); } else { // XXX: Add emergency stuff if sensors are lost } @@ -1694,7 +1709,7 @@ int commander_thread_main(int argc, char *argv[]) && (vdop_m < vdop_threshold_m) // XXX note that vdop is 0 for mtk && !home_position_set && (hrt_absolute_time() - gps_position.timestamp_position < 2000000) - && !current_status.flag_fmu_armed) { + && !safety.armed) { warnx("setting home position"); /* copy position data to uORB home message, store it locally as well */ @@ -1734,24 +1749,22 @@ int commander_thread_main(int argc, char *argv[]) warnx("mode sw not finite"); /* no valid stick position, go to default */ + current_status.mode_switch = MODE_SWITCH_MANUAL; } else if (sp_man.mode_switch < -STICK_ON_OFF_LIMIT) { /* bottom stick position, go to manual mode */ current_status.mode_switch = MODE_SWITCH_MANUAL; - printf("mode switch: manual\n"); } else if (sp_man.mode_switch > STICK_ON_OFF_LIMIT) { /* top stick position, set auto/mission for all vehicle types */ current_status.mode_switch = MODE_SWITCH_AUTO; - printf("mode switch: auto\n"); } else { /* center stick position, set seatbelt/simple control */ current_status.mode_switch = MODE_SWITCH_SEATBELT; - printf("mode switch: seatbelt\n"); } // warnx("man ctrl mode: %d\n", (int)current_status.manual_control_mode); @@ -1811,7 +1824,7 @@ int commander_thread_main(int argc, char *argv[]) /* just manual, XXX this might depend on the return switch */ if (current_status.mode_switch == MODE_SWITCH_MANUAL) { - if (navigation_state_transition(stat_pub, ¤t_status, NAVIGATION_STATE_MANUAL_STANDBY, mavlink_fd) != OK) { + if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_MANUAL_STANDBY, mavlink_fd) != OK) { // These is not supposed to happen warnx("ERROR: Navigation state MANUAL_STANDBY rejected"); } @@ -1819,9 +1832,9 @@ int commander_thread_main(int argc, char *argv[]) /* Try seatbelt or fallback to manual */ } else if (current_status.mode_switch == MODE_SWITCH_SEATBELT) { - if (navigation_state_transition(stat_pub, ¤t_status, NAVIGATION_STATE_SEATBELT_STANDBY, mavlink_fd) != OK) { + if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_SEATBELT_STANDBY, mavlink_fd) != OK) { // fallback to MANUAL_STANDBY - if (navigation_state_transition(stat_pub, ¤t_status, NAVIGATION_STATE_MANUAL_STANDBY, mavlink_fd) != OK) { + if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_MANUAL_STANDBY, mavlink_fd) != OK) { // These is not supposed to happen warnx("ERROR: Navigation state MANUAL_STANDBY rejected"); } @@ -1830,11 +1843,11 @@ int commander_thread_main(int argc, char *argv[]) /* Try auto or fallback to seatbelt or even manual */ } else if (current_status.mode_switch == MODE_SWITCH_AUTO) { - if (navigation_state_transition(stat_pub, ¤t_status, NAVIGATION_STATE_AUTO_STANDBY, mavlink_fd) != OK) { + if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_AUTO_STANDBY, mavlink_fd) != OK) { // first fallback to SEATBELT_STANDY - if (navigation_state_transition(stat_pub, ¤t_status, NAVIGATION_STATE_SEATBELT_STANDBY, mavlink_fd) != OK) { + if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_SEATBELT_STANDBY, mavlink_fd) != OK) { // or fallback to MANUAL_STANDBY - if (navigation_state_transition(stat_pub, ¤t_status, NAVIGATION_STATE_MANUAL_STANDBY, mavlink_fd) != OK) { + if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_MANUAL_STANDBY, mavlink_fd) != OK) { // These is not supposed to happen warnx("ERROR: Navigation state MANUAL_STANDBY rejected"); } @@ -1850,7 +1863,7 @@ int commander_thread_main(int argc, char *argv[]) /* Always accept manual mode */ if (current_status.mode_switch == MODE_SWITCH_MANUAL) { - if (navigation_state_transition(stat_pub, ¤t_status, NAVIGATION_STATE_MANUAL, mavlink_fd) != OK) { + if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_MANUAL, mavlink_fd) != OK) { // These is not supposed to happen warnx("ERROR: Navigation state MANUAL rejected"); } @@ -1859,9 +1872,9 @@ int commander_thread_main(int argc, char *argv[]) } else if (current_status.mode_switch == MODE_SWITCH_SEATBELT && current_status.return_switch == RETURN_SWITCH_NONE) { - if (navigation_state_transition(stat_pub, ¤t_status, NAVIGATION_STATE_SEATBELT, mavlink_fd) != OK) { + if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_SEATBELT, mavlink_fd) != OK) { // fallback to MANUAL_STANDBY - if (navigation_state_transition(stat_pub, ¤t_status, NAVIGATION_STATE_MANUAL, mavlink_fd) != OK) { + if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_MANUAL, mavlink_fd) != OK) { // These is not supposed to happen warnx("ERROR: Navigation state MANUAL rejected"); } @@ -1871,9 +1884,9 @@ int commander_thread_main(int argc, char *argv[]) } else if (current_status.mode_switch == MODE_SWITCH_SEATBELT && current_status.return_switch == RETURN_SWITCH_RETURN) { - if (navigation_state_transition(stat_pub, ¤t_status, NAVIGATION_STATE_SEATBELT_DESCENT, mavlink_fd) != OK) { + if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_SEATBELT_DESCENT, mavlink_fd) != OK) { // fallback to MANUAL_STANDBY - if (navigation_state_transition(stat_pub, ¤t_status, NAVIGATION_STATE_MANUAL, mavlink_fd) != OK) { + if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_MANUAL, mavlink_fd) != OK) { // These is not supposed to happen warnx("ERROR: Navigation state MANUAL rejected"); } @@ -1885,19 +1898,19 @@ int commander_thread_main(int argc, char *argv[]) && current_status.mission_switch == MISSION_SWITCH_NONE) { /* we might come from the disarmed state AUTO_STANDBY */ - if (navigation_state_transition(stat_pub, ¤t_status, NAVIGATION_STATE_AUTO_READY, mavlink_fd) != OK) { - if (navigation_state_transition(stat_pub, ¤t_status, NAVIGATION_STATE_SEATBELT, mavlink_fd) != OK) { + if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_AUTO_READY, mavlink_fd) != OK) { + if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_SEATBELT, mavlink_fd) != OK) { // fallback to MANUAL_STANDBY - if (navigation_state_transition(stat_pub, ¤t_status, NAVIGATION_STATE_MANUAL, mavlink_fd) != OK) { + if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_MANUAL, mavlink_fd) != OK) { // These is not supposed to happen warnx("ERROR: Navigation state MANUAL rejected"); } } /* or from some other armed state like SEATBELT or MANUAL */ - } else if (navigation_state_transition(stat_pub, ¤t_status, NAVIGATION_STATE_AUTO_LOITER, mavlink_fd) != OK) { - if (navigation_state_transition(stat_pub, ¤t_status, NAVIGATION_STATE_SEATBELT, mavlink_fd) != OK) { + } else if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_AUTO_LOITER, mavlink_fd) != OK) { + if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_SEATBELT, mavlink_fd) != OK) { // fallback to MANUAL_STANDBY - if (navigation_state_transition(stat_pub, ¤t_status, NAVIGATION_STATE_MANUAL, mavlink_fd) != OK) { + if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_MANUAL, mavlink_fd) != OK) { // These is not supposed to happen warnx("ERROR: Navigation state MANUAL rejected"); } @@ -1909,10 +1922,10 @@ int commander_thread_main(int argc, char *argv[]) && current_status.return_switch == RETURN_SWITCH_NONE && current_status.mission_switch == MISSION_SWITCH_MISSION) { - if (navigation_state_transition(stat_pub, ¤t_status, NAVIGATION_STATE_AUTO_MISSION, mavlink_fd) != OK) { - if (navigation_state_transition(stat_pub, ¤t_status, NAVIGATION_STATE_SEATBELT, mavlink_fd) != OK) { + if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_AUTO_MISSION, mavlink_fd) != OK) { + if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_SEATBELT, mavlink_fd) != OK) { // fallback to MANUAL_STANDBY - if (navigation_state_transition(stat_pub, ¤t_status, NAVIGATION_STATE_MANUAL, mavlink_fd) != OK) { + if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_MANUAL, mavlink_fd) != OK) { // These is not supposed to happen warnx("ERROR: Navigation state MANUAL rejected"); } @@ -1924,10 +1937,10 @@ int commander_thread_main(int argc, char *argv[]) && current_status.return_switch == RETURN_SWITCH_RETURN && (current_status.mission_switch == MISSION_SWITCH_NONE || current_status.mission_switch == MISSION_SWITCH_MISSION)) { - if (navigation_state_transition(stat_pub, ¤t_status, NAVIGATION_STATE_AUTO_RTL, mavlink_fd) != OK) { - if (navigation_state_transition(stat_pub, ¤t_status, NAVIGATION_STATE_SEATBELT_DESCENT, mavlink_fd) != OK) { + if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_AUTO_RTL, mavlink_fd) != OK) { + if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_SEATBELT_DESCENT, mavlink_fd) != OK) { // fallback to MANUAL_STANDBY - if (navigation_state_transition(stat_pub, ¤t_status, NAVIGATION_STATE_MANUAL, mavlink_fd) != OK) { + if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_MANUAL, mavlink_fd) != OK) { // These is not supposed to happen warnx("ERROR: Navigation state MANUAL rejected"); } @@ -1975,13 +1988,22 @@ int commander_thread_main(int argc, char *argv[]) * Check if left stick is in lower left position --> switch to standby state. * Do this only for multirotors, not for fixed wing aircraft. */ - if (((current_status.system_type == VEHICLE_TYPE_QUADROTOR) || - (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)) { + // printf("checking\n"); + if ((sp_man.yaw < -STICK_ON_OFF_LIMIT) && (sp_man.throttle < STICK_THRUST_RANGE * 0.2f)) { if (stick_off_counter > STICK_ON_OFF_COUNTER_LIMIT) { - arming_state_transition(stat_pub, ¤t_status, ARMING_STATE_STANDBY, mavlink_fd); + + + printf("System Type: %d\n", current_status.system_type); + + if((current_status.system_type == VEHICLE_TYPE_QUADROTOR) || + (current_status.system_type == VEHICLE_TYPE_HEXAROTOR) || + (current_status.system_type == VEHICLE_TYPE_OCTOROTOR) + ) { + arming_state_transition(status_pub, ¤t_status, ARMING_STATE_STANDBY, safety_pub, &safety, mavlink_fd); + + } else { + mavlink_log_critical(mavlink_fd, "STICK DISARM not allowed"); + } stick_off_counter = 0; } else { @@ -1993,7 +2015,7 @@ 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 (stick_on_counter > STICK_ON_OFF_COUNTER_LIMIT) { - arming_state_transition(stat_pub, ¤t_status, ARMING_STATE_ARMED, mavlink_fd); + arming_state_transition(status_pub, ¤t_status, ARMING_STATE_ARMED, safety_pub, &safety, mavlink_fd); stick_on_counter = 0; } else { @@ -2095,13 +2117,13 @@ int commander_thread_main(int argc, char *argv[]) // XXX check if this is correct /* arm / disarm on request */ - if (sp_offboard.armed && !current_status.flag_fmu_armed) { + if (sp_offboard.armed && !safety.armed) { - arming_state_transition(stat_pub, ¤t_status, ARMING_STATE_ARMED, mavlink_fd); + arming_state_transition(status_pub, ¤t_status, ARMING_STATE_ARMED, safety_pub, &safety, mavlink_fd); - } else if (!sp_offboard.armed && current_status.flag_fmu_armed) { + } else if (!sp_offboard.armed && safety.armed) { - arming_state_transition(stat_pub, ¤t_status, ARMING_STATE_STANDBY, mavlink_fd); + arming_state_transition(status_pub, ¤t_status, ARMING_STATE_STANDBY, safety_pub, &safety, mavlink_fd); } } else { @@ -2143,13 +2165,13 @@ int commander_thread_main(int argc, char *argv[]) // current_status.flag_preflight_mag_calibration == false && // current_status.flag_preflight_accel_calibration == false) { // /* All ok, no calibration going on, go to standby */ - // do_state_update(stat_pub, ¤t_status, mavlink_fd, SYSTEM_STATE_STANDBY); + // do_state_update(status_pub, ¤t_status, mavlink_fd, SYSTEM_STATE_STANDBY); // } /* publish at least with 1 Hz */ if (counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0 || state_changed) { - orb_publish(ORB_ID(vehicle_status), stat_pub, ¤t_status); + orb_publish(ORB_ID(vehicle_status), status_pub, ¤t_status); state_changed = false; } diff --git a/src/modules/commander/state_machine_helper.c b/src/modules/commander/state_machine_helper.c index daed81553d..fea7ee840b 100644 --- a/src/modules/commander/state_machine_helper.c +++ b/src/modules/commander/state_machine_helper.c @@ -54,7 +54,7 @@ #include "state_machine_helper.h" -int arming_state_transition(int status_pub, struct vehicle_status_s *current_state, arming_state_t new_arming_state, const int mavlink_fd) { +int arming_state_transition(int status_pub, struct vehicle_status_s *current_state, arming_state_t new_arming_state, int safety_pub, struct actuator_safety_s *safety, const int mavlink_fd) { int ret = ERROR; @@ -69,7 +69,7 @@ int arming_state_transition(int status_pub, struct vehicle_status_s *current_sta /* allow going back from INIT for calibration */ if (current_state->arming_state == ARMING_STATE_STANDBY) { ret = OK; - current_state->flag_fmu_armed = false; + safety->armed = false; } break; case ARMING_STATE_STANDBY: @@ -81,7 +81,7 @@ int arming_state_transition(int status_pub, struct vehicle_status_s *current_sta /* sensors need to be initialized for STANDBY state */ if (current_state->condition_system_sensors_initialized) { ret = OK; - current_state->flag_fmu_armed = false; + safety->armed = false; } else { mavlink_log_critical(mavlink_fd, "Rej. STANDBY state, sensors not initialized"); } @@ -95,7 +95,7 @@ int arming_state_transition(int status_pub, struct vehicle_status_s *current_sta /* XXX conditions for arming? */ ret = OK; - current_state->flag_fmu_armed = true; + safety->armed = true; } break; case ARMING_STATE_ARMED_ERROR: @@ -105,7 +105,7 @@ int arming_state_transition(int status_pub, struct vehicle_status_s *current_sta /* XXX conditions for an error state? */ ret = OK; - current_state->flag_fmu_armed = true; + safety->armed = true; } break; case ARMING_STATE_STANDBY_ERROR: @@ -114,7 +114,7 @@ int arming_state_transition(int status_pub, struct vehicle_status_s *current_sta || current_state->arming_state == ARMING_STATE_INIT || current_state->arming_state == ARMING_STATE_ARMED_ERROR) { ret = OK; - current_state->flag_fmu_armed = false; + safety->armed = false; } break; case ARMING_STATE_REBOOT: @@ -125,7 +125,7 @@ int arming_state_transition(int status_pub, struct vehicle_status_s *current_sta || current_state->arming_state == ARMING_STATE_STANDBY_ERROR) { ret = OK; - current_state->flag_fmu_armed = false; + safety->armed = false; } break; @@ -139,7 +139,12 @@ int arming_state_transition(int status_pub, struct vehicle_status_s *current_sta if (ret == OK) { current_state->arming_state = new_arming_state; - state_machine_publish(status_pub, current_state, mavlink_fd); + current_state->counter++; + current_state->timestamp = hrt_absolute_time(); + orb_publish(ORB_ID(vehicle_status), status_pub, current_state); + + safety->timestamp = hrt_absolute_time(); + orb_publish(ORB_ID(actuator_safety), safety_pub, safety); } } @@ -460,7 +465,9 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current if (ret == OK) { current_state->navigation_state = new_navigation_state; - state_machine_publish(status_pub, current_state, mavlink_fd); + current_state->counter++; + current_state->timestamp = hrt_absolute_time(); + orb_publish(ORB_ID(vehicle_status), status_pub, current_state); } } diff --git a/src/modules/commander/state_machine_helper.h b/src/modules/commander/state_machine_helper.h index 5b57cffb73..824a7529f0 100644 --- a/src/modules/commander/state_machine_helper.h +++ b/src/modules/commander/state_machine_helper.h @@ -46,6 +46,7 @@ #include #include +#include void navigation_state_update(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd); @@ -53,7 +54,7 @@ void navigation_state_update(int status_pub, struct vehicle_status_s *current_st void state_machine_publish(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd); -int arming_state_transition(int status_pub, struct vehicle_status_s *current_state, arming_state_t new_arming_state, const int mavlink_fd); +int arming_state_transition(int status_pub, struct vehicle_status_s *current_state, arming_state_t new_arming_state, int safety_pub, struct actuator_safety_s *safety, const int mavlink_fd); int navigation_state_transition(int status_pub, struct vehicle_status_s *current_state, navigation_state_t new_navigation_state, const int mavlink_fd); int hil_state_transition(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, hil_state_t new_state); diff --git a/src/modules/gpio_led/gpio_led.c b/src/modules/gpio_led/gpio_led.c index 542821e957..618095d0b8 100644 --- a/src/modules/gpio_led/gpio_led.c +++ b/src/modules/gpio_led/gpio_led.c @@ -51,6 +51,7 @@ #include #include #include +#include #include #include @@ -60,6 +61,8 @@ struct gpio_led_s { int pin; struct vehicle_status_s status; int vehicle_status_sub; + struct actuator_safety_s safety; + int actuator_safety_sub; bool led_state; int counter; }; @@ -168,11 +171,15 @@ void gpio_led_cycle(FAR void *arg) if (status_updated) orb_copy(ORB_ID(vehicle_status), priv->vehicle_status_sub, &priv->status); + orb_check(priv->vehicle_status_sub, &status_updated); + + if (status_updated) + orb_copy(ORB_ID(actuator_safety), priv->actuator_safety_sub, &priv->safety); + /* select pattern for current status */ int pattern = 0; - /* XXX fmu armed correct? */ - if (priv->status.flag_fmu_armed) { + if (priv->safety.armed) { if (priv->status.battery_warning == VEHICLE_BATTERY_WARNING_NONE) { pattern = 0x3f; // ****** solid (armed) @@ -181,11 +188,10 @@ void gpio_led_cycle(FAR void *arg) } } else { - if (priv->status.arming_state == ARMING_STATE_STANDBY) { + if (priv->safety.ready_to_arm) { pattern = 0x00; // ______ off (disarmed, preflight check) - } else if (priv->status.arming_state == ARMING_STATE_STANDBY && - priv->status.battery_warning == VEHICLE_BATTERY_WARNING_NONE) { + } else if (priv->safety.ready_to_arm && priv->status.battery_warning == VEHICLE_BATTERY_WARNING_NONE) { pattern = 0x38; // ***___ slow blink (disarmed, ready) } else { diff --git a/src/modules/mavlink/orb_listener.c b/src/modules/mavlink/orb_listener.c index d84406e7a2..a4a2ca3e5f 100644 --- a/src/modules/mavlink/orb_listener.c +++ b/src/modules/mavlink/orb_listener.c @@ -71,7 +71,7 @@ struct vehicle_local_position_s local_pos; struct vehicle_status_s v_status; struct rc_channels_s rc; struct rc_input_values rc_raw; -struct actuator_armed_s armed; +struct actuator_safety_s safety; struct mavlink_subscriptions mavlink_subs; @@ -109,7 +109,7 @@ static void l_global_position_setpoint(const struct listener *l); static void l_local_position_setpoint(const struct listener *l); static void l_attitude_setpoint(const struct listener *l); static void l_actuator_outputs(const struct listener *l); -static void l_actuator_armed(const struct listener *l); +static void l_actuator_safety(const struct listener *l); static void l_manual_control_setpoint(const struct listener *l); static void l_vehicle_attitude_controls(const struct listener *l); static void l_debug_key_value(const struct listener *l); @@ -133,7 +133,7 @@ static const struct listener listeners[] = { {l_actuator_outputs, &mavlink_subs.act_1_sub, 1}, {l_actuator_outputs, &mavlink_subs.act_2_sub, 2}, {l_actuator_outputs, &mavlink_subs.act_3_sub, 3}, - {l_actuator_armed, &mavlink_subs.armed_sub, 0}, + {l_actuator_safety, &mavlink_subs.safety_sub, 0}, {l_manual_control_setpoint, &mavlink_subs.man_control_sp_sub, 0}, {l_vehicle_attitude_controls, &mavlink_subs.actuators_sub, 0}, {l_debug_key_value, &mavlink_subs.debug_key_value, 0}, @@ -269,7 +269,7 @@ l_vehicle_status(const struct listener *l) { /* immediately communicate state changes back to user */ orb_copy(ORB_ID(vehicle_status), status_sub, &v_status); - orb_copy(ORB_ID(actuator_armed), mavlink_subs.armed_sub, &armed); + orb_copy(ORB_ID(actuator_safety), mavlink_subs.safety_sub, &safety); /* enable or disable HIL */ set_hil_on_off(v_status.flag_hil_enabled); @@ -466,7 +466,7 @@ l_actuator_outputs(const struct listener *l) act_outputs.output[7]); /* only send in HIL mode */ - if (mavlink_hil_enabled && armed.armed) { + if (mavlink_hil_enabled && safety.armed) { /* translate the current syste state to mavlink state and mode */ uint8_t mavlink_state = 0; @@ -538,9 +538,9 @@ l_actuator_outputs(const struct listener *l) } void -l_actuator_armed(const struct listener *l) +l_actuator_safety(const struct listener *l) { - orb_copy(ORB_ID(actuator_armed), mavlink_subs.armed_sub, &armed); + orb_copy(ORB_ID(actuator_safety), mavlink_subs.safety_sub, &safety); } void @@ -745,8 +745,8 @@ uorb_receive_start(void) orb_set_interval(mavlink_subs.act_3_sub, 100); /* 10Hz updates */ /* --- ACTUATOR ARMED VALUE --- */ - mavlink_subs.armed_sub = orb_subscribe(ORB_ID(actuator_armed)); - orb_set_interval(mavlink_subs.armed_sub, 100); /* 10Hz updates */ + mavlink_subs.safety_sub = orb_subscribe(ORB_ID(actuator_safety)); + orb_set_interval(mavlink_subs.safety_sub, 100); /* 10Hz updates */ /* --- MAPPED MANUAL CONTROL INPUTS --- */ mavlink_subs.man_control_sp_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); diff --git a/src/modules/mavlink/orb_topics.h b/src/modules/mavlink/orb_topics.h index d61cd43dce..e647b090ae 100644 --- a/src/modules/mavlink/orb_topics.h +++ b/src/modules/mavlink/orb_topics.h @@ -58,6 +58,7 @@ #include #include #include +#include #include #include #include @@ -72,7 +73,7 @@ struct mavlink_subscriptions { int act_3_sub; int gps_sub; int man_control_sp_sub; - int armed_sub; + int safety_sub; int actuators_sub; int local_pos_sub; int spa_sub; diff --git a/src/modules/mavlink_onboard/mavlink.c b/src/modules/mavlink_onboard/mavlink.c index 408a850d8c..4b6d811139 100644 --- a/src/modules/mavlink_onboard/mavlink.c +++ b/src/modules/mavlink_onboard/mavlink.c @@ -274,7 +274,7 @@ void mavlink_update_system(void) } void -get_mavlink_mode_and_state(const struct vehicle_status_s *v_status, const struct actuator_armed_s *armed, +get_mavlink_mode_and_state(const struct vehicle_status_s *v_status, const struct actuator_safety_s *safety, uint8_t *mavlink_state, uint8_t *mavlink_mode) { /* reset MAVLink mode bitfield */ @@ -290,7 +290,7 @@ get_mavlink_mode_and_state(const struct vehicle_status_s *v_status, const struct } /* set arming state */ - if (v_status->flag_fmu_armed) { + if (safety->armed) { *mavlink_mode |= MAV_MODE_FLAG_SAFETY_ARMED; } else { *mavlink_mode &= ~MAV_MODE_FLAG_SAFETY_ARMED; @@ -369,7 +369,7 @@ int mavlink_thread_main(int argc, char *argv[]) baudrate = 57600; struct vehicle_status_s v_status; - struct actuator_armed_s armed; + struct actuator_safety_s safety; /* work around some stupidity in task_create's argv handling */ argc -= 2; @@ -437,7 +437,7 @@ int mavlink_thread_main(int argc, char *argv[]) /* translate the current system state to mavlink state and mode */ uint8_t mavlink_state = 0; uint8_t mavlink_mode = 0; - get_mavlink_mode_and_state(&v_status, &armed, &mavlink_state, &mavlink_mode); + get_mavlink_mode_and_state(&v_status, &safety, &mavlink_state, &mavlink_mode); /* send heartbeat */ mavlink_msg_heartbeat_send(chan, mavlink_system.type, MAV_AUTOPILOT_PX4, mavlink_mode, v_status.navigation_state, mavlink_state); diff --git a/src/modules/mavlink_onboard/orb_topics.h b/src/modules/mavlink_onboard/orb_topics.h index f18f562439..fee5580b30 100644 --- a/src/modules/mavlink_onboard/orb_topics.h +++ b/src/modules/mavlink_onboard/orb_topics.h @@ -55,6 +55,7 @@ #include #include #include +#include #include #include #include @@ -69,7 +70,7 @@ struct mavlink_subscriptions { int act_3_sub; int gps_sub; int man_control_sp_sub; - int armed_sub; + int safety_sub; int actuators_sub; int local_pos_sub; int spa_sub; diff --git a/src/modules/mavlink_onboard/util.h b/src/modules/mavlink_onboard/util.h index 38a4db372b..17c3ba8201 100644 --- a/src/modules/mavlink_onboard/util.h +++ b/src/modules/mavlink_onboard/util.h @@ -50,5 +50,5 @@ extern volatile bool thread_should_exit; /** * Translate the custom state into standard mavlink modes and state. */ -extern void get_mavlink_mode_and_state(const struct vehicle_status_s *v_status, const struct actuator_armed_s *armed, +extern void get_mavlink_mode_and_state(const struct vehicle_status_s *v_status, const struct actuator_safety_s *safety, uint8_t *mavlink_state, uint8_t *mavlink_mode); diff --git a/src/modules/multirotor_att_control/multirotor_att_control_main.c b/src/modules/multirotor_att_control/multirotor_att_control_main.c index 3329c5c48e..44c2fb1d80 100644 --- a/src/modules/multirotor_att_control/multirotor_att_control_main.c +++ b/src/modules/multirotor_att_control/multirotor_att_control_main.c @@ -58,6 +58,7 @@ #include #include #include +#include #include #include #include @@ -84,13 +85,16 @@ static bool motor_test_mode = false; static orb_advert_t actuator_pub; -static struct vehicle_status_s state; + static int mc_thread_main(int argc, char *argv[]) { /* declare and safely initialize all structs */ + struct vehicle_status_s state; memset(&state, 0, sizeof(state)); + struct actuator_safety_s safety; + memset(&safety, 0, sizeof(safety)); struct vehicle_attitude_s att; memset(&att, 0, sizeof(att)); struct vehicle_attitude_setpoint_s att_sp; @@ -113,6 +117,7 @@ mc_thread_main(int argc, char *argv[]) int att_setpoint_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint)); int setpoint_sub = orb_subscribe(ORB_ID(offboard_control_setpoint)); int state_sub = orb_subscribe(ORB_ID(vehicle_status)); + int safety_sub = orb_subscribe(ORB_ID(actuator_safety)); int manual_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); int sensor_sub = orb_subscribe(ORB_ID(sensor_combined)); @@ -150,7 +155,7 @@ mc_thread_main(int argc, char *argv[]) /* store last control mode to detect mode switches */ bool flag_control_manual_enabled = false; bool flag_control_attitude_enabled = false; - bool flag_fmu_armed = false; + bool flag_armed = false; bool flag_control_position_enabled = false; bool flag_control_velocity_enabled = false; @@ -200,6 +205,12 @@ mc_thread_main(int argc, char *argv[]) orb_copy(ORB_ID(vehicle_status), state_sub, &state); } + orb_check(safety_sub, &updated); + + if (updated) { + orb_copy(ORB_ID(actuator_safety), safety_sub, &safety); + } + /* get a local copy of manual setpoint */ orb_copy(ORB_ID(manual_control_setpoint), manual_sub, &manual); /* get a local copy of attitude */ @@ -248,7 +259,7 @@ mc_thread_main(int argc, char *argv[]) /* initialize to current yaw if switching to manual or att control */ if (state.flag_control_attitude_enabled != flag_control_attitude_enabled || state.flag_control_manual_enabled != flag_control_manual_enabled || - state.flag_fmu_armed != flag_fmu_armed) { + safety.armed != flag_armed) { att_sp.yaw_body = att.yaw; } @@ -292,7 +303,7 @@ mc_thread_main(int argc, char *argv[]) att_sp.pitch_body = manual.pitch; /* set attitude if arming */ - if (!flag_control_attitude_enabled && state.flag_fmu_armed) { + if (!flag_control_attitude_enabled && safety.armed) { att_sp.yaw_body = att.yaw; } @@ -399,7 +410,7 @@ mc_thread_main(int argc, char *argv[]) flag_control_manual_enabled = state.flag_control_manual_enabled; flag_control_position_enabled = state.flag_control_position_enabled; flag_control_velocity_enabled = state.flag_control_velocity_enabled; - flag_fmu_armed = state.flag_fmu_armed; + flag_armed = safety.armed; perf_end(mc_loop_perf); } /* end of poll call for attitude updates */ diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 8c5ec092d1..41c2f67e56 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -60,6 +60,7 @@ #include #include +#include #include #include #include @@ -190,7 +191,7 @@ static int file_copy(const char *file_old, const char *file_new); static void handle_command(struct vehicle_command_s *cmd); -static void handle_status(struct vehicle_status_s *cmd); +static void handle_status(struct actuator_safety_s *safety); /** * Create folder for current logging session. Store folder name in 'log_folder'. @@ -623,6 +624,7 @@ int sdlog2_thread_main(int argc, char *argv[]) /* warning! using union here to save memory, elements should be used separately! */ union { + struct actuator_safety_s safety; struct vehicle_command_s cmd; struct sensor_combined_s sensor; struct vehicle_attitude_s att; @@ -643,6 +645,7 @@ int sdlog2_thread_main(int argc, char *argv[]) memset(&buf, 0, sizeof(buf)); struct { + int safety_sub; int cmd_sub; int status_sub; int sensor_sub; @@ -690,6 +693,12 @@ int sdlog2_thread_main(int argc, char *argv[]) fds[fdsc_count].events = POLLIN; fdsc_count++; + /* --- SAFETY STATUS --- */ + subs.safety_sub = orb_subscribe(ORB_ID(actuator_safety)); + fds[fdsc_count].fd = subs.safety_sub; + fds[fdsc_count].events = POLLIN; + fdsc_count++; + /* --- VEHICLE STATUS --- */ subs.status_sub = orb_subscribe(ORB_ID(vehicle_status)); fds[fdsc_count].fd = subs.status_sub; @@ -826,6 +835,7 @@ int sdlog2_thread_main(int argc, char *argv[]) int ifds = 0; int handled_topics = 0; + /* --- VEHICLE COMMAND - LOG MANAGEMENT --- */ if (fds[ifds++].revents & POLLIN) { orb_copy(ORB_ID(vehicle_command), subs.cmd_sub, &buf.cmd); @@ -838,7 +848,7 @@ int sdlog2_thread_main(int argc, char *argv[]) orb_copy(ORB_ID(vehicle_status), subs.status_sub, &buf_status); if (log_when_armed) { - handle_status(&buf_status); + handle_status(&buf.safety); } handled_topics++; @@ -870,7 +880,7 @@ int sdlog2_thread_main(int argc, char *argv[]) log_msg.body.log_STAT.flight_mode = 0; log_msg.body.log_STAT.manual_control_mode = 0; log_msg.body.log_STAT.manual_sas_mode = 0; - log_msg.body.log_STAT.armed = (unsigned char) buf_status.flag_fmu_armed; /* XXX fmu armed correct? */ + log_msg.body.log_STAT.armed = (unsigned char) buf.safety.armed; /* XXX fmu armed correct? */ log_msg.body.log_STAT.battery_voltage = buf_status.voltage_battery; log_msg.body.log_STAT.battery_current = buf_status.current_battery; log_msg.body.log_STAT.battery_remaining = buf_status.battery_remaining; @@ -1168,11 +1178,10 @@ void handle_command(struct vehicle_command_s *cmd) } } -void handle_status(struct vehicle_status_s *status) +void handle_status(struct actuator_safety_s *safety) { - /* XXX fmu armed correct? */ - if (status->flag_fmu_armed != flag_system_armed) { - flag_system_armed = status->flag_fmu_armed; + if (safety->armed != flag_system_armed) { + flag_system_armed = safety->armed; if (flag_system_armed) { sdlog2_start_log(); diff --git a/src/modules/uORB/objects_common.cpp b/src/modules/uORB/objects_common.cpp index 4197f6fb2d..2674354c32 100644 --- a/src/modules/uORB/objects_common.cpp +++ b/src/modules/uORB/objects_common.cpp @@ -137,7 +137,9 @@ ORB_DEFINE(actuator_controls_0, struct actuator_controls_s); ORB_DEFINE(actuator_controls_1, struct actuator_controls_s); ORB_DEFINE(actuator_controls_2, struct actuator_controls_s); ORB_DEFINE(actuator_controls_3, struct actuator_controls_s); -ORB_DEFINE(actuator_armed, struct actuator_armed_s); + +#include "topics/actuator_safety.h" +ORB_DEFINE(actuator_safety, struct actuator_safety_s); /* actuator controls, as set by actuators / mixers after limiting */ #include "topics/actuator_controls_effective.h" diff --git a/src/modules/uORB/topics/actuator_controls.h b/src/modules/uORB/topics/actuator_controls.h index b7c4196c0a..26b967237c 100644 --- a/src/modules/uORB/topics/actuator_controls.h +++ b/src/modules/uORB/topics/actuator_controls.h @@ -52,6 +52,9 @@ #define NUM_ACTUATOR_CONTROLS 8 #define NUM_ACTUATOR_CONTROL_GROUPS 4 /**< for sanity checking */ +/* control sets with pre-defined applications */ +#define ORB_ID_VEHICLE_ATTITUDE_CONTROLS ORB_ID(actuator_controls_0) + struct actuator_controls_s { uint64_t timestamp; float control[NUM_ACTUATOR_CONTROLS]; @@ -63,16 +66,4 @@ ORB_DECLARE(actuator_controls_1); ORB_DECLARE(actuator_controls_2); ORB_DECLARE(actuator_controls_3); -/* control sets with pre-defined applications */ -#define ORB_ID_VEHICLE_ATTITUDE_CONTROLS ORB_ID(actuator_controls_0) - -/** global 'actuator output is live' control. */ -struct actuator_armed_s { - bool armed; /**< Set to true if system is armed */ - bool ready_to_arm; /**< Set to true if system is ready to be armed */ - bool lockdown; /**< Set to true if actuators are forced to being disabled (due to emergency or HIL) */ -}; - -ORB_DECLARE(actuator_armed); - #endif \ No newline at end of file diff --git a/src/modules/uORB/topics/actuator_safety.h b/src/modules/uORB/topics/actuator_safety.h new file mode 100644 index 0000000000..b78eb4b7e8 --- /dev/null +++ b/src/modules/uORB/topics/actuator_safety.h @@ -0,0 +1,65 @@ +/**************************************************************************** + * + * Copyright (C) 2012 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 actuator_controls.h + * + * Actuator control topics - mixer inputs. + * + * Values published to these topics are the outputs of the vehicle control + * system, and are expected to be mixed and used to drive the actuators + * (servos, speed controls, etc.) that operate the vehicle. + * + * Each topic can be published by a single controller + */ + +#ifndef TOPIC_ACTUATOR_SAFETY_H +#define TOPIC_ACTUATOR_SAFETY_H + +#include +#include "../uORB.h" + +/** global 'actuator output is live' control. */ +struct actuator_safety_s { + + uint64_t timestamp; + + bool safety_off; /**< Set to true if safety is off */ + bool armed; /**< Set to true if system is armed */ + bool ready_to_arm; /**< Set to true if system is ready to be armed */ + bool lockdown; /**< Set to true if actuators are forced to being disabled (due to emergency or HIL) */ +}; + +ORB_DECLARE(actuator_safety); + +#endif \ No newline at end of file diff --git a/src/modules/uORB/topics/vehicle_control_mode.h b/src/modules/uORB/topics/vehicle_control_mode.h new file mode 100644 index 0000000000..eb35d08840 --- /dev/null +++ b/src/modules/uORB/topics/vehicle_control_mode.h @@ -0,0 +1,96 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Author: @author Lorenz Meier + * @author Petri Tanskanen + * @author Thomas Gubler + * @author Julian Oes + * + * 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 vehicle_control_mode.h + * Definition of the vehicle_control_mode uORB topic. + * + * All control apps should depend their actions based on the flags set here. + */ + +#ifndef VEHICLE_CONTROL_MODE +#define VEHICLE_CONTROL_MODE + +#include +#include +#include "../uORB.h" + +/** + * @addtogroup topics @{ + */ + + +/** + * state machine / state of vehicle. + * + * Encodes the complete system state and is set by the commander app. + */ +struct vehicle_control_mode_s +{ + /* use of a counter and timestamp recommended (but not necessary) */ + + uint16_t counter; /**< incremented by the writing thread everytime new data is stored */ + uint64_t timestamp; /**< in microseconds since system start, is set whenever the writing thread stores new data */ + + bool flag_external_manual_override_ok; /**< external override non-fatal for system. Only true for fixed wing */ + + bool flag_hil_enabled; /**< true if hardware in the loop simulation is enabled */ + bool flag_armed; /**< true is motors / actuators are armed */ + bool flag_safety_off; /**< true if safety is off */ + bool flag_system_emergency; + bool flag_preflight_calibration; + + bool flag_control_manual_enabled; /**< true if manual input is mixed in */ + bool flag_control_offboard_enabled; /**< true if offboard control input is on */ + bool flag_auto_enabled; + bool flag_control_rates_enabled; /**< true if rates are stabilized */ + bool flag_control_attitude_enabled; /**< true if attitude stabilization is mixed in */ + bool flag_control_velocity_enabled; /**< true if speed (implies direction) is controlled */ + bool flag_control_position_enabled; /**< true if position is controlled */ + + bool failsave_lowlevel; /**< Set to true if low-level failsafe mode is enabled */ + bool failsave_highlevel; +}; + +/** + * @} + */ + +/* register this as object request broker structure */ +ORB_DECLARE(vehicle_status); + +#endif diff --git a/src/modules/uORB/topics/vehicle_status.h b/src/modules/uORB/topics/vehicle_status.h index 6d3f8a8632..b19075921f 100644 --- a/src/modules/uORB/topics/vehicle_status.h +++ b/src/modules/uORB/topics/vehicle_status.h @@ -199,8 +199,8 @@ struct vehicle_status_s bool flag_external_manual_override_ok; /**< external override non-fatal for system. Only true for fixed wing */ bool flag_hil_enabled; /**< true if hardware in the loop simulation is enabled */ - bool flag_fmu_armed; /**< true is motors / actuators of FMU are armed */ - bool flag_io_armed; /**< true is motors / actuators of IO are armed */ + bool flag_armed; /**< true is motors / actuators are armed */ + bool flag_safety_off; /**< true if safety is off */ bool flag_system_emergency; bool flag_preflight_calibration; @@ -245,7 +245,6 @@ struct vehicle_status_s uint16_t errors_count2; uint16_t errors_count3; uint16_t errors_count4; - }; /** From 5b21362e1ffefe4e28579eb7a853fe5d22288760 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Fri, 14 Jun 2013 16:04:23 +0200 Subject: [PATCH 036/109] Arming with IO working now --- src/drivers/px4io/px4io.cpp | 11 +++++------ src/modules/commander/state_machine_helper.c | 4 ++++ 2 files changed, 9 insertions(+), 6 deletions(-) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 28a3eb917b..d728b7b762 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -728,12 +728,11 @@ PX4IO::io_set_arming_state() } else { clear |= PX4IO_P_SETUP_ARMING_FMU_ARMED; } - // TODO fix this -// if (armed.ready_to_arm) { -// set |= PX4IO_P_SETUP_ARMING_IO_ARM_OK; -// } else { -// clear |= PX4IO_P_SETUP_ARMING_IO_ARM_OK; -// } + if (safety.ready_to_arm) { + set |= PX4IO_P_SETUP_ARMING_IO_ARM_OK; + } else { + clear |= PX4IO_P_SETUP_ARMING_IO_ARM_OK; + } if (vstatus.flag_external_manual_override_ok) { set |= PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK; diff --git a/src/modules/commander/state_machine_helper.c b/src/modules/commander/state_machine_helper.c index fea7ee840b..4f2fbc9840 100644 --- a/src/modules/commander/state_machine_helper.c +++ b/src/modules/commander/state_machine_helper.c @@ -70,6 +70,7 @@ int arming_state_transition(int status_pub, struct vehicle_status_s *current_sta if (current_state->arming_state == ARMING_STATE_STANDBY) { ret = OK; safety->armed = false; + safety->ready_to_arm = false; } break; case ARMING_STATE_STANDBY: @@ -82,6 +83,7 @@ int arming_state_transition(int status_pub, struct vehicle_status_s *current_sta if (current_state->condition_system_sensors_initialized) { ret = OK; safety->armed = false; + safety->ready_to_arm = true; } else { mavlink_log_critical(mavlink_fd, "Rej. STANDBY state, sensors not initialized"); } @@ -115,6 +117,7 @@ int arming_state_transition(int status_pub, struct vehicle_status_s *current_sta || current_state->arming_state == ARMING_STATE_ARMED_ERROR) { ret = OK; safety->armed = false; + safety->ready_to_arm = false; } break; case ARMING_STATE_REBOOT: @@ -126,6 +129,7 @@ int arming_state_transition(int status_pub, struct vehicle_status_s *current_sta ret = OK; safety->armed = false; + safety->ready_to_arm = false; } break; From e556649f2ff6922a7a3b7751b68cdedd0d6254aa Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Fri, 14 Jun 2013 16:48:41 +0200 Subject: [PATCH 037/109] Beep when mode is not possible --- src/modules/commander/commander.c | 3 --- src/modules/commander/state_machine_helper.c | 19 +++++++++++++++++++ 2 files changed, 19 insertions(+), 3 deletions(-) diff --git a/src/modules/commander/commander.c b/src/modules/commander/commander.c index 6812fb1fb8..1d3f908073 100644 --- a/src/modules/commander/commander.c +++ b/src/modules/commander/commander.c @@ -1991,9 +1991,6 @@ int commander_thread_main(int argc, char *argv[]) // printf("checking\n"); if ((sp_man.yaw < -STICK_ON_OFF_LIMIT) && (sp_man.throttle < STICK_THRUST_RANGE * 0.2f)) { if (stick_off_counter > STICK_ON_OFF_COUNTER_LIMIT) { - - - printf("System Type: %d\n", current_status.system_type); if((current_status.system_type == VEHICLE_TYPE_QUADROTOR) || (current_status.system_type == VEHICLE_TYPE_HEXAROTOR) || diff --git a/src/modules/commander/state_machine_helper.c b/src/modules/commander/state_machine_helper.c index 4f2fbc9840..dcaf775b9d 100644 --- a/src/modules/commander/state_machine_helper.c +++ b/src/modules/commander/state_machine_helper.c @@ -198,6 +198,7 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current /* need to be disarmed first */ if (current_state->arming_state != ARMING_STATE_STANDBY) { mavlink_log_critical(mavlink_fd, "Rej. MANUAL_STANDBY: not disarmed"); + tune_negative(); } else { ret = OK; current_state->flag_control_rates_enabled = true; @@ -214,6 +215,7 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current /* need to be armed first */ if (current_state->arming_state != ARMING_STATE_ARMED) { mavlink_log_critical(mavlink_fd, "Rej. MANUAL: not armed"); + tune_negative(); } else { ret = OK; current_state->flag_control_rates_enabled = true; @@ -236,8 +238,10 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current /* need to be disarmed and have a position estimate */ if (current_state->arming_state != ARMING_STATE_STANDBY) { mavlink_log_critical(mavlink_fd, "Rej. SEATBELT_STANDBY: not disarmed"); + tune_negative(); } else if (!current_state->condition_local_position_valid) { mavlink_log_critical(mavlink_fd, "Rej. SEATBELT_STANDBY: no position estimate"); + tune_negative(); } else { ret = OK; current_state->flag_control_rates_enabled = true; @@ -265,8 +269,10 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current /* need to be armed and have a position estimate */ if (current_state->arming_state != ARMING_STATE_ARMED) { mavlink_log_critical(mavlink_fd, "Rej. SEATBELT: not armed"); + tune_negative(); } else if (!current_state->condition_local_position_valid) { mavlink_log_critical(mavlink_fd, "Rej. SEATBELT: no pos estimate"); + tune_negative(); } else { ret = OK; current_state->flag_control_rates_enabled = true; @@ -293,8 +299,10 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current /* need to be armed and have a position estimate */ if (current_state->arming_state != ARMING_STATE_ARMED) { mavlink_log_critical(mavlink_fd, "Rej. SEATBELT_DESCENT: not armed"); + tune_negative(); } else if (!current_state->condition_local_position_valid) { mavlink_log_critical(mavlink_fd, "Rej. SEATBELT_DESCENT: no pos estimate"); + tune_negative(); } else { ret = OK; current_state->flag_control_rates_enabled = true; @@ -317,10 +325,13 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current /* need to be disarmed and have a position and home lock */ if (current_state->arming_state != ARMING_STATE_STANDBY) { mavlink_log_critical(mavlink_fd, "Rej. AUTO_STANDBY: not disarmed"); + tune_negative(); } else if (!current_state->condition_global_position_valid) { mavlink_log_critical(mavlink_fd, "Rej. AUTO_STANDBY: no pos lock"); + tune_negative(); } else if (!current_state->condition_home_position_valid) { mavlink_log_critical(mavlink_fd, "Rej. AUTO_STANDBY: no home pos"); + tune_negative(); } else { ret = OK; current_state->flag_control_rates_enabled = true; @@ -343,6 +354,7 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current /* need to be armed and have a position and home lock */ if (current_state->arming_state != ARMING_STATE_ARMED) { mavlink_log_critical(mavlink_fd, "Rej. AUTO_READY: not armed"); + tune_negative(); } else { ret = OK; current_state->flag_control_rates_enabled = true; @@ -380,8 +392,10 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current /* need to have a position and home lock */ if (!current_state->condition_global_position_valid) { mavlink_log_critical(mavlink_fd, "Rej. AUTO_LOITER: no pos lock"); + tune_negative(); } else if (!current_state->condition_home_position_valid) { mavlink_log_critical(mavlink_fd, "Rej. AUTO_LOITER: no home pos"); + tune_negative(); } else { ret = OK; current_state->flag_control_rates_enabled = true; @@ -405,6 +419,7 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current /* need to have a mission ready */ if (!current_state-> condition_auto_mission_available) { mavlink_log_critical(mavlink_fd, "Rej. AUTO_MISSION: no mission available"); + tune_negative(); } else { ret = OK; current_state->flag_control_rates_enabled = true; @@ -428,8 +443,10 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current /* need to have a position and home lock */ if (!current_state->condition_global_position_valid) { mavlink_log_critical(mavlink_fd, "Rej. AUTO_RTL: no pos lock"); + tune_negative(); } else if (!current_state->condition_home_position_valid) { mavlink_log_critical(mavlink_fd, "Rej. AUTO_RTL: no home pos"); + tune_negative(); } else { ret = OK; current_state->flag_control_rates_enabled = true; @@ -450,8 +467,10 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current /* need to have a position and home lock */ if (!current_state->condition_global_position_valid) { mavlink_log_critical(mavlink_fd, "Rej. AUTO_LAND: no pos lock"); + tune_negative(); } else if (!current_state->condition_home_position_valid) { mavlink_log_critical(mavlink_fd, "Rej. AUTO_LAND: no home pos"); + tune_negative(); } else { ret = OK; current_state->flag_control_rates_enabled = true; From 9f5565de3221718ba12800a54ca1a0c06b7491ef Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Sat, 15 Jun 2013 19:41:54 +0200 Subject: [PATCH 038/109] Controllers should not access state machine anymore but access the vehicle_control_mode flags --- src/modules/commander/commander.c | 113 ++++++++------- src/modules/commander/state_machine_helper.c | 136 +++++++++--------- src/modules/commander/state_machine_helper.h | 4 +- src/modules/mavlink/orb_topics.h | 1 + src/modules/mavlink_onboard/mavlink.c | 12 +- src/modules/mavlink_onboard/orb_topics.h | 1 + src/modules/mavlink_onboard/util.h | 3 +- .../multirotor_att_control_main.c | 45 +++--- src/modules/uORB/objects_common.cpp | 3 + src/modules/uORB/topics/actuator_safety.h | 1 + .../uORB/topics/vehicle_control_mode.h | 9 +- src/modules/uORB/topics/vehicle_status.h | 18 +-- 12 files changed, 180 insertions(+), 166 deletions(-) diff --git a/src/modules/commander/commander.c b/src/modules/commander/commander.c index 1d3f908073..7853d2e790 100644 --- a/src/modules/commander/commander.c +++ b/src/modules/commander/commander.c @@ -1233,6 +1233,9 @@ int commander_thread_main(int argc, char *argv[]) // XXX for now just set sensors as initialized current_status.condition_system_sensors_initialized = true; + // XXX just disable offboard control for now + control_mode.flag_control_offboard_enabled = false; + /* advertise to ORB */ status_pub = orb_advertise(ORB_ID(vehicle_status), ¤t_status); /* publish current state machine */ @@ -1240,6 +1243,8 @@ int commander_thread_main(int argc, char *argv[]) safety_pub = orb_advertise(ORB_ID(actuator_safety), &safety); + control_mode_pub = orb_advertise(ORB_ID(vehicle_control_mode), &control_mode); + /* home position */ orb_advert_t home_pub = -1; struct home_position_s home; @@ -1824,7 +1829,7 @@ int commander_thread_main(int argc, char *argv[]) /* just manual, XXX this might depend on the return switch */ if (current_status.mode_switch == MODE_SWITCH_MANUAL) { - if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_MANUAL_STANDBY, mavlink_fd) != OK) { + if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_MANUAL_STANDBY, control_mode_pub, &control_mode, mavlink_fd) != OK) { // These is not supposed to happen warnx("ERROR: Navigation state MANUAL_STANDBY rejected"); } @@ -1832,9 +1837,9 @@ int commander_thread_main(int argc, char *argv[]) /* Try seatbelt or fallback to manual */ } else if (current_status.mode_switch == MODE_SWITCH_SEATBELT) { - if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_SEATBELT_STANDBY, mavlink_fd) != OK) { + if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_SEATBELT_STANDBY, control_mode_pub, &control_mode, mavlink_fd) != OK) { // fallback to MANUAL_STANDBY - if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_MANUAL_STANDBY, mavlink_fd) != OK) { + if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_MANUAL_STANDBY, control_mode_pub, &control_mode, mavlink_fd) != OK) { // These is not supposed to happen warnx("ERROR: Navigation state MANUAL_STANDBY rejected"); } @@ -1843,11 +1848,11 @@ int commander_thread_main(int argc, char *argv[]) /* Try auto or fallback to seatbelt or even manual */ } else if (current_status.mode_switch == MODE_SWITCH_AUTO) { - if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_AUTO_STANDBY, mavlink_fd) != OK) { + if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_AUTO_STANDBY, control_mode_pub, &control_mode, mavlink_fd) != OK) { // first fallback to SEATBELT_STANDY - if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_SEATBELT_STANDBY, mavlink_fd) != OK) { + if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_SEATBELT_STANDBY, control_mode_pub, &control_mode, mavlink_fd) != OK) { // or fallback to MANUAL_STANDBY - if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_MANUAL_STANDBY, mavlink_fd) != OK) { + if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_MANUAL_STANDBY, control_mode_pub, &control_mode, mavlink_fd) != OK) { // These is not supposed to happen warnx("ERROR: Navigation state MANUAL_STANDBY rejected"); } @@ -1863,7 +1868,7 @@ int commander_thread_main(int argc, char *argv[]) /* Always accept manual mode */ if (current_status.mode_switch == MODE_SWITCH_MANUAL) { - if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_MANUAL, mavlink_fd) != OK) { + if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_MANUAL, control_mode_pub, &control_mode, mavlink_fd) != OK) { // These is not supposed to happen warnx("ERROR: Navigation state MANUAL rejected"); } @@ -1872,9 +1877,9 @@ int commander_thread_main(int argc, char *argv[]) } else if (current_status.mode_switch == MODE_SWITCH_SEATBELT && current_status.return_switch == RETURN_SWITCH_NONE) { - if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_SEATBELT, mavlink_fd) != OK) { + if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_SEATBELT, control_mode_pub, &control_mode, mavlink_fd) != OK) { // fallback to MANUAL_STANDBY - if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_MANUAL, mavlink_fd) != OK) { + if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_MANUAL, control_mode_pub, &control_mode, mavlink_fd) != OK) { // These is not supposed to happen warnx("ERROR: Navigation state MANUAL rejected"); } @@ -1884,9 +1889,9 @@ int commander_thread_main(int argc, char *argv[]) } else if (current_status.mode_switch == MODE_SWITCH_SEATBELT && current_status.return_switch == RETURN_SWITCH_RETURN) { - if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_SEATBELT_DESCENT, mavlink_fd) != OK) { + if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_SEATBELT_DESCENT, control_mode_pub, &control_mode, mavlink_fd) != OK) { // fallback to MANUAL_STANDBY - if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_MANUAL, mavlink_fd) != OK) { + if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_MANUAL, control_mode_pub, &control_mode, mavlink_fd) != OK) { // These is not supposed to happen warnx("ERROR: Navigation state MANUAL rejected"); } @@ -1898,19 +1903,19 @@ int commander_thread_main(int argc, char *argv[]) && current_status.mission_switch == MISSION_SWITCH_NONE) { /* we might come from the disarmed state AUTO_STANDBY */ - if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_AUTO_READY, mavlink_fd) != OK) { - if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_SEATBELT, mavlink_fd) != OK) { + if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_AUTO_READY, control_mode_pub, &control_mode, mavlink_fd) != OK) { + if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_SEATBELT, control_mode_pub, &control_mode, mavlink_fd) != OK) { // fallback to MANUAL_STANDBY - if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_MANUAL, mavlink_fd) != OK) { + if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_MANUAL, control_mode_pub, &control_mode, mavlink_fd) != OK) { // These is not supposed to happen warnx("ERROR: Navigation state MANUAL rejected"); } } /* or from some other armed state like SEATBELT or MANUAL */ - } else if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_AUTO_LOITER, mavlink_fd) != OK) { - if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_SEATBELT, mavlink_fd) != OK) { + } else if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_AUTO_LOITER, control_mode_pub, &control_mode, mavlink_fd) != OK) { + if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_SEATBELT, control_mode_pub, &control_mode, mavlink_fd) != OK) { // fallback to MANUAL_STANDBY - if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_MANUAL, mavlink_fd) != OK) { + if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_MANUAL, control_mode_pub, &control_mode, mavlink_fd) != OK) { // These is not supposed to happen warnx("ERROR: Navigation state MANUAL rejected"); } @@ -1922,10 +1927,10 @@ int commander_thread_main(int argc, char *argv[]) && current_status.return_switch == RETURN_SWITCH_NONE && current_status.mission_switch == MISSION_SWITCH_MISSION) { - if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_AUTO_MISSION, mavlink_fd) != OK) { - if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_SEATBELT, mavlink_fd) != OK) { + if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_AUTO_MISSION, control_mode_pub, &control_mode, mavlink_fd) != OK) { + if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_SEATBELT, control_mode_pub, &control_mode, mavlink_fd) != OK) { // fallback to MANUAL_STANDBY - if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_MANUAL, mavlink_fd) != OK) { + if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_MANUAL, control_mode_pub, &control_mode, mavlink_fd) != OK) { // These is not supposed to happen warnx("ERROR: Navigation state MANUAL rejected"); } @@ -1937,10 +1942,10 @@ int commander_thread_main(int argc, char *argv[]) && current_status.return_switch == RETURN_SWITCH_RETURN && (current_status.mission_switch == MISSION_SWITCH_NONE || current_status.mission_switch == MISSION_SWITCH_MISSION)) { - if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_AUTO_RTL, mavlink_fd) != OK) { - if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_SEATBELT_DESCENT, mavlink_fd) != OK) { + if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_AUTO_RTL, control_mode_pub, &control_mode, mavlink_fd) != OK) { + if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_SEATBELT_DESCENT, control_mode_pub, &control_mode, mavlink_fd) != OK) { // fallback to MANUAL_STANDBY - if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_MANUAL, mavlink_fd) != OK) { + if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_MANUAL, control_mode_pub, &control_mode, mavlink_fd) != OK) { // These is not supposed to happen warnx("ERROR: Navigation state MANUAL rejected"); } @@ -2070,43 +2075,43 @@ int commander_thread_main(int argc, char *argv[]) if (!current_status.rc_signal_found_once && sp_offboard.timestamp != 0) { if ((hrt_absolute_time() - sp_offboard.timestamp) < 5000000) { - /* decide about attitude control flag, enable in att/pos/vel */ - bool attitude_ctrl_enabled = (sp_offboard.mode == OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE || - sp_offboard.mode == OFFBOARD_CONTROL_MODE_DIRECT_VELOCITY || - sp_offboard.mode == OFFBOARD_CONTROL_MODE_DIRECT_POSITION); + // /* decide about attitude control flag, enable in att/pos/vel */ + // bool attitude_ctrl_enabled = (sp_offboard.mode == OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE || + // sp_offboard.mode == OFFBOARD_CONTROL_MODE_DIRECT_VELOCITY || + // sp_offboard.mode == OFFBOARD_CONTROL_MODE_DIRECT_POSITION); - /* decide about rate control flag, enable it always XXX (for now) */ - bool rates_ctrl_enabled = true; + // /* decide about rate control flag, enable it always XXX (for now) */ + // bool rates_ctrl_enabled = true; - /* set up control mode */ - if (current_status.flag_control_attitude_enabled != attitude_ctrl_enabled) { - current_status.flag_control_attitude_enabled = attitude_ctrl_enabled; - state_changed = true; - } + // /* set up control mode */ + // if (current_status.flag_control_attitude_enabled != attitude_ctrl_enabled) { + // current_status.flag_control_attitude_enabled = attitude_ctrl_enabled; + // state_changed = true; + // } - if (current_status.flag_control_rates_enabled != rates_ctrl_enabled) { - current_status.flag_control_rates_enabled = rates_ctrl_enabled; - state_changed = true; - } + // if (current_status.flag_control_rates_enabled != rates_ctrl_enabled) { + // current_status.flag_control_rates_enabled = rates_ctrl_enabled; + // state_changed = true; + // } - /* handle the case where offboard control signal was regained */ - if (!current_status.offboard_control_signal_found_once) { - current_status.offboard_control_signal_found_once = true; - /* enable offboard control, disable manual input */ - current_status.flag_control_manual_enabled = false; - current_status.flag_control_offboard_enabled = true; - state_changed = true; - tune_positive(); + // /* handle the case where offboard control signal was regained */ + // if (!current_status.offboard_control_signal_found_once) { + // current_status.offboard_control_signal_found_once = true; + // /* enable offboard control, disable manual input */ + // current_status.flag_control_manual_enabled = false; + // current_status.flag_control_offboard_enabled = true; + // state_changed = true; + // tune_positive(); - mavlink_log_critical(mavlink_fd, "DETECTED OFFBOARD SIGNAL FIRST"); + // mavlink_log_critical(mavlink_fd, "DETECTED OFFBOARD SIGNAL FIRST"); - } else { - if (current_status.offboard_control_signal_lost) { - mavlink_log_critical(mavlink_fd, "RECOVERY OFFBOARD CONTROL"); - state_changed = true; - tune_positive(); - } - } + // } else { + // if (current_status.offboard_control_signal_lost) { + // mavlink_log_critical(mavlink_fd, "RECOVERY OFFBOARD CONTROL"); + // state_changed = true; + // tune_positive(); + // } + // } current_status.offboard_control_signal_weak = false; current_status.offboard_control_signal_lost = false; diff --git a/src/modules/commander/state_machine_helper.c b/src/modules/commander/state_machine_helper.c index dcaf775b9d..394ee67cc7 100644 --- a/src/modules/commander/state_machine_helper.c +++ b/src/modules/commander/state_machine_helper.c @@ -46,6 +46,7 @@ #include #include #include +#include #include #include #include @@ -161,7 +162,7 @@ int arming_state_transition(int status_pub, struct vehicle_status_s *current_sta * This functions does not evaluate any input flags but only checks if the transitions * are valid. */ -int navigation_state_transition(int status_pub, struct vehicle_status_s *current_state, navigation_state_t new_navigation_state, const int mavlink_fd) { +int navigation_state_transition(int status_pub, struct vehicle_status_s *current_state, navigation_state_t new_navigation_state, int control_mode_pub, struct vehicle_control_mode_s *control_mode, const int mavlink_fd) { int ret = ERROR; @@ -179,11 +180,11 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current || current_state->navigation_state == NAVIGATION_STATE_AUTO_STANDBY) { ret = OK; - current_state->flag_control_rates_enabled = false; - current_state->flag_control_attitude_enabled = false; - current_state->flag_control_velocity_enabled = false; - current_state->flag_control_position_enabled = false; - current_state->flag_control_manual_enabled = false; + control_mode->flag_control_rates_enabled = false; + control_mode->flag_control_attitude_enabled = false; + control_mode->flag_control_velocity_enabled = false; + control_mode->flag_control_position_enabled = false; + control_mode->flag_control_manual_enabled = false; } break; @@ -201,11 +202,11 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current tune_negative(); } else { ret = OK; - current_state->flag_control_rates_enabled = true; - current_state->flag_control_attitude_enabled = true; - current_state->flag_control_velocity_enabled = false; - current_state->flag_control_position_enabled = false; - current_state->flag_control_manual_enabled = true; + control_mode->flag_control_rates_enabled = true; + control_mode->flag_control_attitude_enabled = true; + control_mode->flag_control_velocity_enabled = false; + control_mode->flag_control_position_enabled = false; + control_mode->flag_control_manual_enabled = true; } } break; @@ -218,11 +219,11 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current tune_negative(); } else { ret = OK; - current_state->flag_control_rates_enabled = true; - current_state->flag_control_attitude_enabled = true; - current_state->flag_control_velocity_enabled = false; - current_state->flag_control_position_enabled = false; - current_state->flag_control_manual_enabled = true; + control_mode->flag_control_rates_enabled = true; + control_mode->flag_control_attitude_enabled = true; + control_mode->flag_control_velocity_enabled = false; + control_mode->flag_control_position_enabled = false; + control_mode->flag_control_manual_enabled = true; } break; @@ -244,11 +245,11 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current tune_negative(); } else { ret = OK; - current_state->flag_control_rates_enabled = true; - current_state->flag_control_attitude_enabled = true; - current_state->flag_control_velocity_enabled = true; - current_state->flag_control_position_enabled = false; - current_state->flag_control_manual_enabled = false; + control_mode->flag_control_rates_enabled = true; + control_mode->flag_control_attitude_enabled = true; + control_mode->flag_control_velocity_enabled = true; + control_mode->flag_control_position_enabled = false; + control_mode->flag_control_manual_enabled = false; } } break; @@ -275,11 +276,11 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current tune_negative(); } else { ret = OK; - current_state->flag_control_rates_enabled = true; - current_state->flag_control_attitude_enabled = true; - current_state->flag_control_velocity_enabled = true; - current_state->flag_control_position_enabled = false; - current_state->flag_control_manual_enabled = false; + control_mode->flag_control_rates_enabled = true; + control_mode->flag_control_attitude_enabled = true; + control_mode->flag_control_velocity_enabled = true; + control_mode->flag_control_position_enabled = false; + control_mode->flag_control_manual_enabled = false; } } break; @@ -305,11 +306,11 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current tune_negative(); } else { ret = OK; - current_state->flag_control_rates_enabled = true; - current_state->flag_control_attitude_enabled = true; - current_state->flag_control_velocity_enabled = true; - current_state->flag_control_position_enabled = false; - current_state->flag_control_manual_enabled = false; + control_mode->flag_control_rates_enabled = true; + control_mode->flag_control_attitude_enabled = true; + control_mode->flag_control_velocity_enabled = true; + control_mode->flag_control_position_enabled = false; + control_mode->flag_control_manual_enabled = false; } } break; @@ -334,11 +335,11 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current tune_negative(); } else { ret = OK; - current_state->flag_control_rates_enabled = true; - current_state->flag_control_attitude_enabled = true; - current_state->flag_control_velocity_enabled = true; - current_state->flag_control_position_enabled = true; - current_state->flag_control_manual_enabled = false; + control_mode->flag_control_rates_enabled = true; + control_mode->flag_control_attitude_enabled = true; + control_mode->flag_control_velocity_enabled = true; + control_mode->flag_control_position_enabled = true; + control_mode->flag_control_manual_enabled = false; } } break; @@ -357,11 +358,11 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current tune_negative(); } else { ret = OK; - current_state->flag_control_rates_enabled = true; - current_state->flag_control_attitude_enabled = true; - current_state->flag_control_velocity_enabled = true; - current_state->flag_control_position_enabled = true; - current_state->flag_control_manual_enabled = false; + control_mode->flag_control_rates_enabled = true; + control_mode->flag_control_attitude_enabled = true; + control_mode->flag_control_velocity_enabled = true; + control_mode->flag_control_position_enabled = true; + control_mode->flag_control_manual_enabled = false; } } break; @@ -372,11 +373,11 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current if (current_state->navigation_state == NAVIGATION_STATE_AUTO_READY) { ret = OK; - current_state->flag_control_rates_enabled = true; - current_state->flag_control_attitude_enabled = true; - current_state->flag_control_velocity_enabled = true; - current_state->flag_control_position_enabled = true; - current_state->flag_control_manual_enabled = false; + control_mode->flag_control_rates_enabled = true; + control_mode->flag_control_attitude_enabled = true; + control_mode->flag_control_velocity_enabled = true; + control_mode->flag_control_position_enabled = true; + control_mode->flag_control_manual_enabled = false; } break; @@ -398,11 +399,11 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current tune_negative(); } else { ret = OK; - current_state->flag_control_rates_enabled = true; - current_state->flag_control_attitude_enabled = true; - current_state->flag_control_velocity_enabled = true; - current_state->flag_control_position_enabled = true; - current_state->flag_control_manual_enabled = false; + control_mode->flag_control_rates_enabled = true; + control_mode->flag_control_attitude_enabled = true; + control_mode->flag_control_velocity_enabled = true; + control_mode->flag_control_position_enabled = true; + control_mode->flag_control_manual_enabled = false; } } break; @@ -422,11 +423,11 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current tune_negative(); } else { ret = OK; - current_state->flag_control_rates_enabled = true; - current_state->flag_control_attitude_enabled = true; - current_state->flag_control_velocity_enabled = true; - current_state->flag_control_position_enabled = true; - current_state->flag_control_manual_enabled = false; + control_mode->flag_control_rates_enabled = true; + control_mode->flag_control_attitude_enabled = true; + control_mode->flag_control_velocity_enabled = true; + control_mode->flag_control_position_enabled = true; + control_mode->flag_control_manual_enabled = false; } } break; @@ -449,11 +450,11 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current tune_negative(); } else { ret = OK; - current_state->flag_control_rates_enabled = true; - current_state->flag_control_attitude_enabled = true; - current_state->flag_control_velocity_enabled = true; - current_state->flag_control_position_enabled = true; - current_state->flag_control_manual_enabled = false; + control_mode->flag_control_rates_enabled = true; + control_mode->flag_control_attitude_enabled = true; + control_mode->flag_control_velocity_enabled = true; + control_mode->flag_control_position_enabled = true; + control_mode->flag_control_manual_enabled = false; } } break; @@ -473,11 +474,11 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current tune_negative(); } else { ret = OK; - current_state->flag_control_rates_enabled = true; - current_state->flag_control_attitude_enabled = true; - current_state->flag_control_velocity_enabled = true; - current_state->flag_control_position_enabled = true; - current_state->flag_control_manual_enabled = false; + control_mode->flag_control_rates_enabled = true; + control_mode->flag_control_attitude_enabled = true; + control_mode->flag_control_velocity_enabled = true; + control_mode->flag_control_position_enabled = true; + control_mode->flag_control_manual_enabled = false; } } break; @@ -491,6 +492,9 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current current_state->counter++; current_state->timestamp = hrt_absolute_time(); orb_publish(ORB_ID(vehicle_status), status_pub, current_state); + + control_mode->timestamp = hrt_absolute_time(); + orb_publish(ORB_ID(vehicle_control_mode), control_mode_pub, control_mode); } } diff --git a/src/modules/commander/state_machine_helper.h b/src/modules/commander/state_machine_helper.h index 824a7529f0..8d85360905 100644 --- a/src/modules/commander/state_machine_helper.h +++ b/src/modules/commander/state_machine_helper.h @@ -47,6 +47,7 @@ #include #include #include +#include void navigation_state_update(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd); @@ -54,8 +55,7 @@ void navigation_state_update(int status_pub, struct vehicle_status_s *current_st void state_machine_publish(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd); -int arming_state_transition(int status_pub, struct vehicle_status_s *current_state, arming_state_t new_arming_state, int safety_pub, struct actuator_safety_s *safety, const int mavlink_fd); -int navigation_state_transition(int status_pub, struct vehicle_status_s *current_state, navigation_state_t new_navigation_state, const int mavlink_fd); +int navigation_state_transition(int status_pub, struct vehicle_status_s *current_state, navigation_state_t new_navigation_state, int control_mode_pub, struct vehicle_control_mode_s *control_mode, const int mavlink_fd); int hil_state_transition(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, hil_state_t new_state); diff --git a/src/modules/mavlink/orb_topics.h b/src/modules/mavlink/orb_topics.h index e647b090ae..221957d465 100644 --- a/src/modules/mavlink/orb_topics.h +++ b/src/modules/mavlink/orb_topics.h @@ -54,6 +54,7 @@ #include #include #include +#include #include #include #include diff --git a/src/modules/mavlink_onboard/mavlink.c b/src/modules/mavlink_onboard/mavlink.c index 4b6d811139..dbea2be083 100644 --- a/src/modules/mavlink_onboard/mavlink.c +++ b/src/modules/mavlink_onboard/mavlink.c @@ -274,18 +274,18 @@ void mavlink_update_system(void) } void -get_mavlink_mode_and_state(const struct vehicle_status_s *v_status, const struct actuator_safety_s *safety, +get_mavlink_mode_and_state(const struct vehicle_control_mode_s *control_mode, const struct actuator_safety_s *safety, uint8_t *mavlink_state, uint8_t *mavlink_mode) { /* reset MAVLink mode bitfield */ *mavlink_mode = 0; /* set mode flags independent of system state */ - if (v_status->flag_control_manual_enabled) { + if (control_mode->flag_control_manual_enabled) { *mavlink_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED; } - if (v_status->flag_hil_enabled) { + if (safety->hil_enabled) { *mavlink_mode |= MAV_MODE_FLAG_HIL_ENABLED; } @@ -296,7 +296,7 @@ get_mavlink_mode_and_state(const struct vehicle_status_s *v_status, const struct *mavlink_mode &= ~MAV_MODE_FLAG_SAFETY_ARMED; } - if (v_status->flag_control_velocity_enabled) { + if (control_mode->flag_control_velocity_enabled) { *mavlink_mode |= MAV_MODE_FLAG_GUIDED_ENABLED; } else { *mavlink_mode &= ~MAV_MODE_FLAG_GUIDED_ENABLED; @@ -368,7 +368,9 @@ int mavlink_thread_main(int argc, char *argv[]) char *device_name = "/dev/ttyS1"; baudrate = 57600; + /* XXX this is never written? */ struct vehicle_status_s v_status; + struct vehicle_control_mode_s control_mode; struct actuator_safety_s safety; /* work around some stupidity in task_create's argv handling */ @@ -437,7 +439,7 @@ int mavlink_thread_main(int argc, char *argv[]) /* translate the current system state to mavlink state and mode */ uint8_t mavlink_state = 0; uint8_t mavlink_mode = 0; - get_mavlink_mode_and_state(&v_status, &safety, &mavlink_state, &mavlink_mode); + get_mavlink_mode_and_state(&control_mode, &safety, &mavlink_state, &mavlink_mode); /* send heartbeat */ mavlink_msg_heartbeat_send(chan, mavlink_system.type, MAV_AUTOPILOT_PX4, mavlink_mode, v_status.navigation_state, mavlink_state); diff --git a/src/modules/mavlink_onboard/orb_topics.h b/src/modules/mavlink_onboard/orb_topics.h index fee5580b30..f01f09e12d 100644 --- a/src/modules/mavlink_onboard/orb_topics.h +++ b/src/modules/mavlink_onboard/orb_topics.h @@ -52,6 +52,7 @@ #include #include #include +#include #include #include #include diff --git a/src/modules/mavlink_onboard/util.h b/src/modules/mavlink_onboard/util.h index 17c3ba8201..c6a2e52bf0 100644 --- a/src/modules/mavlink_onboard/util.h +++ b/src/modules/mavlink_onboard/util.h @@ -50,5 +50,6 @@ extern volatile bool thread_should_exit; /** * Translate the custom state into standard mavlink modes and state. */ -extern void get_mavlink_mode_and_state(const struct vehicle_status_s *v_status, const struct actuator_safety_s *safety, +extern void +get_mavlink_mode_and_state(const struct vehicle_control_mode_s *control_mode, const struct actuator_safety_s *safety, uint8_t *mavlink_state, uint8_t *mavlink_mode); diff --git a/src/modules/multirotor_att_control/multirotor_att_control_main.c b/src/modules/multirotor_att_control/multirotor_att_control_main.c index 44c2fb1d80..b4d7fb630a 100644 --- a/src/modules/multirotor_att_control/multirotor_att_control_main.c +++ b/src/modules/multirotor_att_control/multirotor_att_control_main.c @@ -57,7 +57,7 @@ #include #include #include -#include +#include #include #include #include @@ -91,8 +91,8 @@ static int mc_thread_main(int argc, char *argv[]) { /* declare and safely initialize all structs */ - struct vehicle_status_s state; - memset(&state, 0, sizeof(state)); + struct vehicle_control_mode_s control_mode; + memset(&control_mode, 0, sizeof(control_mode)); struct actuator_safety_s safety; memset(&safety, 0, sizeof(safety)); struct vehicle_attitude_s att; @@ -116,7 +116,7 @@ mc_thread_main(int argc, char *argv[]) int param_sub = orb_subscribe(ORB_ID(parameter_update)); int att_setpoint_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint)); int setpoint_sub = orb_subscribe(ORB_ID(offboard_control_setpoint)); - int state_sub = orb_subscribe(ORB_ID(vehicle_status)); + int control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode)); int safety_sub = orb_subscribe(ORB_ID(actuator_safety)); int manual_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); int sensor_sub = orb_subscribe(ORB_ID(sensor_combined)); @@ -181,7 +181,6 @@ mc_thread_main(int argc, char *argv[]) } else if (ret == 0) { /* no return value, ignore */ } else { - /* only update parameters if they changed */ if (fds[1].revents & POLLIN) { /* read from param to clear updated flag */ @@ -199,10 +198,10 @@ mc_thread_main(int argc, char *argv[]) /* get a local copy of system state */ bool updated; - orb_check(state_sub, &updated); + orb_check(control_mode_sub, &updated); if (updated) { - orb_copy(ORB_ID(vehicle_status), state_sub, &state); + orb_copy(ORB_ID(vehicle_control_mode), control_mode_sub, &control_mode); } orb_check(safety_sub, &updated); @@ -227,9 +226,8 @@ mc_thread_main(int argc, char *argv[]) /* get a local copy of the current sensor values */ orb_copy(ORB_ID(sensor_combined), sensor_sub, &raw); - /** STEP 1: Define which input is the dominating control input */ - if (state.flag_control_offboard_enabled) { + if (control_mode.flag_control_offboard_enabled) { /* offboard inputs */ if (offboard_sp.mode == OFFBOARD_CONTROL_MODE_DIRECT_RATES) { rates_sp.roll = offboard_sp.p1; @@ -252,13 +250,11 @@ mc_thread_main(int argc, char *argv[]) } - } else if (state.flag_control_manual_enabled) { - - if (state.flag_control_attitude_enabled) { - + } else if (control_mode.flag_control_manual_enabled) { + if (control_mode.flag_control_attitude_enabled) { /* initialize to current yaw if switching to manual or att control */ - if (state.flag_control_attitude_enabled != flag_control_attitude_enabled || - state.flag_control_manual_enabled != flag_control_manual_enabled || + if (control_mode.flag_control_attitude_enabled != flag_control_attitude_enabled || + control_mode.flag_control_manual_enabled != flag_control_manual_enabled || safety.armed != flag_armed) { att_sp.yaw_body = att.yaw; } @@ -266,6 +262,8 @@ mc_thread_main(int argc, char *argv[]) static bool rc_loss_first_time = true; /* if the RC signal is lost, try to stay level and go slowly back down to ground */ +/* XXX fix this */ +#if 0 if (state.rc_signal_lost) { /* the failsafe throttle is stored as a parameter, as it depends on the copter and the payload */ param_get(failsafe_throttle_handle, &failsafe_throttle); @@ -297,6 +295,7 @@ mc_thread_main(int argc, char *argv[]) rc_loss_first_time = false; } else { +#endif rc_loss_first_time = true; att_sp.roll_body = manual.roll; @@ -344,7 +343,9 @@ mc_thread_main(int argc, char *argv[]) att_sp.thrust = manual.throttle; att_sp.timestamp = hrt_absolute_time(); +#if 0 } +#endif /* STEP 2: publish the controller output */ orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp); @@ -378,7 +379,7 @@ mc_thread_main(int argc, char *argv[]) } /** STEP 3: Identify the controller setup to run and set up the inputs correctly */ - if (state.flag_control_attitude_enabled) { + if (control_mode.flag_control_attitude_enabled) { multirotor_control_attitude(&att_sp, &att, &rates_sp, control_yaw_position); orb_publish(ORB_ID(vehicle_rates_setpoint), rates_sp_pub, &rates_sp); @@ -405,11 +406,11 @@ mc_thread_main(int argc, char *argv[]) multirotor_control_rates(&rates_sp, gyro, &actuators); orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators); - /* update state */ - flag_control_attitude_enabled = state.flag_control_attitude_enabled; - flag_control_manual_enabled = state.flag_control_manual_enabled; - flag_control_position_enabled = state.flag_control_position_enabled; - flag_control_velocity_enabled = state.flag_control_velocity_enabled; + /* update control_mode */ + flag_control_attitude_enabled = control_mode.flag_control_attitude_enabled; + flag_control_manual_enabled = control_mode.flag_control_manual_enabled; + flag_control_position_enabled = control_mode.flag_control_position_enabled; + flag_control_velocity_enabled = control_mode.flag_control_velocity_enabled; flag_armed = safety.armed; perf_end(mc_loop_perf); @@ -427,7 +428,7 @@ mc_thread_main(int argc, char *argv[]) close(att_sub); - close(state_sub); + close(control_mode_sub); close(manual_sub); close(actuator_pub); close(att_sp_pub); diff --git a/src/modules/uORB/objects_common.cpp b/src/modules/uORB/objects_common.cpp index 2674354c32..313fbf6419 100644 --- a/src/modules/uORB/objects_common.cpp +++ b/src/modules/uORB/objects_common.cpp @@ -98,6 +98,9 @@ ORB_DEFINE(rc_channels, struct rc_channels_s); #include "topics/vehicle_command.h" ORB_DEFINE(vehicle_command, struct vehicle_command_s); +#include "topics/vehicle_control_mode.h" +ORB_DEFINE(vehicle_control_mode, struct vehicle_control_mode_s); + #include "topics/vehicle_local_position_setpoint.h" ORB_DEFINE(vehicle_local_position_setpoint, struct vehicle_local_position_setpoint_s); diff --git a/src/modules/uORB/topics/actuator_safety.h b/src/modules/uORB/topics/actuator_safety.h index b78eb4b7e8..3a107d41aa 100644 --- a/src/modules/uORB/topics/actuator_safety.h +++ b/src/modules/uORB/topics/actuator_safety.h @@ -58,6 +58,7 @@ struct actuator_safety_s { bool armed; /**< Set to true if system is armed */ bool ready_to_arm; /**< Set to true if system is ready to be armed */ bool lockdown; /**< Set to true if actuators are forced to being disabled (due to emergency or HIL) */ + bool hil_enabled; /**< Set to true if hardware-in-the-loop (HIL) is enabled */ }; ORB_DECLARE(actuator_safety); diff --git a/src/modules/uORB/topics/vehicle_control_mode.h b/src/modules/uORB/topics/vehicle_control_mode.h index eb35d08840..177e30898b 100644 --- a/src/modules/uORB/topics/vehicle_control_mode.h +++ b/src/modules/uORB/topics/vehicle_control_mode.h @@ -61,16 +61,11 @@ */ struct vehicle_control_mode_s { - /* use of a counter and timestamp recommended (but not necessary) */ - uint16_t counter; /**< incremented by the writing thread everytime new data is stored */ uint64_t timestamp; /**< in microseconds since system start, is set whenever the writing thread stores new data */ bool flag_external_manual_override_ok; /**< external override non-fatal for system. Only true for fixed wing */ - bool flag_hil_enabled; /**< true if hardware in the loop simulation is enabled */ - bool flag_armed; /**< true is motors / actuators are armed */ - bool flag_safety_off; /**< true if safety is off */ bool flag_system_emergency; bool flag_preflight_calibration; @@ -83,7 +78,7 @@ struct vehicle_control_mode_s bool flag_control_position_enabled; /**< true if position is controlled */ bool failsave_lowlevel; /**< Set to true if low-level failsafe mode is enabled */ - bool failsave_highlevel; + bool failsave_highlevel; /**< Set to true if high-level failsafe mode is enabled */ }; /** @@ -91,6 +86,6 @@ struct vehicle_control_mode_s */ /* register this as object request broker structure */ -ORB_DECLARE(vehicle_status); +ORB_DECLARE(vehicle_control_mode); #endif diff --git a/src/modules/uORB/topics/vehicle_status.h b/src/modules/uORB/topics/vehicle_status.h index b19075921f..2bcd57f4bb 100644 --- a/src/modules/uORB/topics/vehicle_status.h +++ b/src/modules/uORB/topics/vehicle_status.h @@ -199,18 +199,18 @@ struct vehicle_status_s bool flag_external_manual_override_ok; /**< external override non-fatal for system. Only true for fixed wing */ bool flag_hil_enabled; /**< true if hardware in the loop simulation is enabled */ - bool flag_armed; /**< true is motors / actuators are armed */ - bool flag_safety_off; /**< true if safety is off */ + //bool flag_armed; /**< true is motors / actuators are armed */ + //bool flag_safety_off; /**< true if safety is off */ bool flag_system_emergency; bool flag_preflight_calibration; - bool flag_control_manual_enabled; /**< true if manual input is mixed in */ - bool flag_control_offboard_enabled; /**< true if offboard control input is on */ - bool flag_auto_enabled; - bool flag_control_rates_enabled; /**< true if rates are stabilized */ - bool flag_control_attitude_enabled; /**< true if attitude stabilization is mixed in */ - bool flag_control_velocity_enabled; /**< true if speed (implies direction) is controlled */ - bool flag_control_position_enabled; /**< true if position is controlled */ + // bool flag_control_manual_enabled; /**< true if manual input is mixed in */ + // bool flag_control_offboard_enabled; /**< true if offboard control input is on */ + // bool flag_auto_enabled; + // bool flag_control_rates_enabled; /**< true if rates are stabilized */ + // bool flag_control_attitude_enabled; /**< true if attitude stabilization is mixed in */ + // bool flag_control_velocity_enabled; /**< true if speed (implies direction) is controlled */ + // bool flag_control_position_enabled; /**< true if position is controlled */ // bool flag_preflight_gyro_calibration; /**< true if gyro calibration is requested */ // bool flag_preflight_mag_calibration; /**< true if mag calibration is requested */ From 2b9fa731efd08a01effd87a636ae8e53994944f7 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Tue, 12 Mar 2013 12:13:02 -0700 Subject: [PATCH 039/109] Use the pid library in the rate controller and change de implementation of the D part Conflicts: src/modules/multirotor_att_control/multirotor_rate_control.c src/modules/systemlib/pid/pid.c src/modules/systemlib/pid/pid.h --- .../multirotor_attitude_control.c | 47 ++----------------- .../multirotor_rate_control.c | 35 +++++++++----- src/modules/systemlib/pid/pid.c | 26 ++++++---- src/modules/systemlib/pid/pid.h | 3 +- 4 files changed, 45 insertions(+), 66 deletions(-) diff --git a/src/modules/multirotor_att_control/multirotor_attitude_control.c b/src/modules/multirotor_att_control/multirotor_attitude_control.c index 76dbb36d34..1053ac7a31 100644 --- a/src/modules/multirotor_att_control/multirotor_attitude_control.c +++ b/src/modules/multirotor_att_control/multirotor_attitude_control.c @@ -57,50 +57,29 @@ PARAM_DEFINE_FLOAT(MC_YAWPOS_P, 0.3f); PARAM_DEFINE_FLOAT(MC_YAWPOS_I, 0.15f); PARAM_DEFINE_FLOAT(MC_YAWPOS_D, 0.0f); -//PARAM_DEFINE_FLOAT(MC_YAWPOS_AWU, 1.0f); -//PARAM_DEFINE_FLOAT(MC_YAWPOS_LIM, 3.0f); PARAM_DEFINE_FLOAT(MC_ATT_P, 0.2f); PARAM_DEFINE_FLOAT(MC_ATT_I, 0.0f); PARAM_DEFINE_FLOAT(MC_ATT_D, 0.05f); -//PARAM_DEFINE_FLOAT(MC_ATT_AWU, 0.05f); -//PARAM_DEFINE_FLOAT(MC_ATT_LIM, 0.4f); - -//PARAM_DEFINE_FLOAT(MC_ATT_XOFF, 0.0f); -//PARAM_DEFINE_FLOAT(MC_ATT_YOFF, 0.0f); struct mc_att_control_params { float yaw_p; float yaw_i; float yaw_d; - //float yaw_awu; - //float yaw_lim; float att_p; float att_i; float att_d; - //float att_awu; - //float att_lim; - - //float att_xoff; - //float att_yoff; }; struct mc_att_control_param_handles { param_t yaw_p; param_t yaw_i; param_t yaw_d; - //param_t yaw_awu; - //param_t yaw_lim; param_t att_p; param_t att_i; param_t att_d; - //param_t att_awu; - //param_t att_lim; - - //param_t att_xoff; - //param_t att_yoff; }; /** @@ -122,17 +101,10 @@ static int parameters_init(struct mc_att_control_param_handles *h) h->yaw_p = param_find("MC_YAWPOS_P"); h->yaw_i = param_find("MC_YAWPOS_I"); h->yaw_d = param_find("MC_YAWPOS_D"); - //h->yaw_awu = param_find("MC_YAWPOS_AWU"); - //h->yaw_lim = param_find("MC_YAWPOS_LIM"); h->att_p = param_find("MC_ATT_P"); h->att_i = param_find("MC_ATT_I"); h->att_d = param_find("MC_ATT_D"); - //h->att_awu = param_find("MC_ATT_AWU"); - //h->att_lim = param_find("MC_ATT_LIM"); - - //h->att_xoff = param_find("MC_ATT_XOFF"); - //h->att_yoff = param_find("MC_ATT_YOFF"); return OK; } @@ -142,17 +114,10 @@ static int parameters_update(const struct mc_att_control_param_handles *h, struc param_get(h->yaw_p, &(p->yaw_p)); param_get(h->yaw_i, &(p->yaw_i)); param_get(h->yaw_d, &(p->yaw_d)); - //param_get(h->yaw_awu, &(p->yaw_awu)); - //param_get(h->yaw_lim, &(p->yaw_lim)); param_get(h->att_p, &(p->att_p)); param_get(h->att_i, &(p->att_i)); param_get(h->att_d, &(p->att_d)); - //param_get(h->att_awu, &(p->att_awu)); - //param_get(h->att_lim, &(p->att_lim)); - - //param_get(h->att_xoff, &(p->att_xoff)); - //param_get(h->att_yoff, &(p->att_yoff)); return OK; } @@ -170,8 +135,8 @@ void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_s last_input = att_sp->timestamp; } - static int sensor_delay; - sensor_delay = hrt_absolute_time() - att->timestamp; +// static int sensor_delay; +// sensor_delay = hrt_absolute_time() - att->timestamp; static int motor_skip_counter = 0; @@ -190,10 +155,8 @@ void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_s parameters_init(&h); parameters_update(&h, &p); - pid_init(&pitch_controller, p.att_p, p.att_i, p.att_d, 1000.0f, - 1000.0f, PID_MODE_DERIVATIV_SET); - pid_init(&roll_controller, p.att_p, p.att_i, p.att_d, 1000.0f, - 1000.0f, PID_MODE_DERIVATIV_SET); + pid_init(&pitch_controller, p.att_p, p.att_i, p.att_d, 1000.0f, 1000.0f, PID_MODE_DERIVATIV_SET); + pid_init(&roll_controller, p.att_p, p.att_i, p.att_d, 1000.0f, 1000.0f, PID_MODE_DERIVATIV_SET); initialized = true; } @@ -205,7 +168,7 @@ void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_s /* apply parameters */ pid_set_parameters(&pitch_controller, p.att_p, p.att_i, p.att_d, 1000.0f, 1000.0f); - pid_set_parameters(&roll_controller, p.att_p, p.att_i, p.att_d, 1000.0f, 1000.0f); + pid_set_parameters(&roll_controller, p.att_p, p.att_i, p.att_d, 1000.0f, 1000.0f); } /* reset integral if on ground */ diff --git a/src/modules/multirotor_att_control/multirotor_rate_control.c b/src/modules/multirotor_att_control/multirotor_rate_control.c index deba1ac037..4ab92b9557 100644 --- a/src/modules/multirotor_att_control/multirotor_rate_control.c +++ b/src/modules/multirotor_att_control/multirotor_rate_control.c @@ -150,13 +150,11 @@ static int parameters_update(const struct mc_rate_control_param_handles *h, stru void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp, const float rates[], struct actuator_controls_s *actuators) { - static float roll_control_last = 0; - static float pitch_control_last = 0; static uint64_t last_run = 0; const float deltaT = (hrt_absolute_time() - last_run) / 1000000.0f; static uint64_t last_input = 0; - float dT_input = (hrt_absolute_time() - last_input) / 1000000.0f; +// float dT_input = (hrt_absolute_time() - last_input) / 1000000.0f; if (last_input != rate_sp->timestamp) { last_input = rate_sp->timestamp; @@ -166,9 +164,15 @@ void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp, static int motor_skip_counter = 0; + static PID_t pitch_rate_controller; + static PID_t roll_rate_controller; + static struct mc_rate_control_params p; static struct mc_rate_control_param_handles h; + float pitch_control_last = 0.0f; + float roll_control_last = 0.0f; + static bool initialized = false; /* initialize the pid controllers when the function is called for the first time */ @@ -176,39 +180,44 @@ void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp, parameters_init(&h); parameters_update(&h, &p); initialized = true; + + pid_init(&pitch_rate_controller, p.attrate_p, p.attrate_i, p.attrate_d, 1000.0f, + 1000.0f, PID_MODE_DERIVATIV_CALC); + pid_init(&roll_rate_controller, p.attrate_p, p.attrate_i, p.attrate_d, 1000.0f, + 1000.0f, PID_MODE_DERIVATIV_CALC); } /* load new parameters with lower rate */ if (motor_skip_counter % 2500 == 0) { /* update parameters from storage */ parameters_update(&h, &p); - // warnx("rate ctrl: p.yawrate_p: %8.4f, loop: %d Hz, input: %d Hz", - // (double)p.yawrate_p, (int)(1.0f/deltaT), (int)(1.0f/dT_input)); + pid_set_parameters(&pitch_rate_controller, p.attrate_p, p.attrate_i, p.attrate_d, 1000.0f, 1000.0f); + pid_set_parameters(&roll_rate_controller, p.attrate_p, p.attrate_i, p.attrate_d, 1000.0f, 1000.0f); } - /* calculate current control outputs */ - /* control pitch (forward) output */ - float pitch_control = p.attrate_p * (rate_sp->pitch - rates[1]) - (p.attrate_d * pitch_control_last); + float pitch_control = pid_calculate(&pitch_rate_controller, rate_sp->pitch , + rates[1], 0.0f, deltaT); + + /* control roll (left/right) output */ + float roll_control = pid_calculate(&roll_rate_controller, rate_sp->roll , + rates[0], 0.0f, deltaT); /* increase resilience to faulty control inputs */ if (isfinite(pitch_control)) { pitch_control_last = pitch_control; } else { - pitch_control = 0.0f; + pitch_control = pitch_control_last; warnx("rej. NaN ctrl pitch"); } - /* control roll (left/right) output */ - float roll_control = p.attrate_p * (rate_sp->roll - rates[0]) - (p.attrate_d * roll_control_last); - /* increase resilience to faulty control inputs */ if (isfinite(roll_control)) { roll_control_last = roll_control; } else { - roll_control = 0.0f; + roll_control = roll_control_last; warnx("rej. NaN ctrl roll"); } diff --git a/src/modules/systemlib/pid/pid.c b/src/modules/systemlib/pid/pid.c index 49315cdc9f..08e0ca46fa 100644 --- a/src/modules/systemlib/pid/pid.c +++ b/src/modules/systemlib/pid/pid.c @@ -51,13 +51,14 @@ __EXPORT void pid_init(PID_t *pid, float kp, float ki, float kd, float intmax, pid->intmax = intmax; pid->limit = limit; pid->mode = mode; - pid->count = 0; - pid->saturated = 0; - pid->last_output = 0; + pid->count = 0.0f; + pid->saturated = 0.0f; + pid->last_output = 0.0f; - pid->sp = 0; - pid->error_previous = 0; - pid->integral = 0; + pid->sp = 0.0f; + pid->error_previous_filtered = 0.0f; + pid->control_previous = 0.0f; + pid->integral = 0.0f; } __EXPORT int pid_set_parameters(PID_t *pid, float kp, float ki, float kd, float intmax, float limit) { @@ -136,15 +137,15 @@ __EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, flo // Calculated current error value float error = pid->sp - val; - if (isfinite(error)) { // Why is this necessary? DEW - pid->error_previous = error; - } + float error_filtered = 0.2f*error + 0.8f*pid->error_previous_filtered; // Calculate or measured current error derivative if (pid->mode == PID_MODE_DERIVATIV_CALC) { - d = (error - pid->error_previous) / dt; +// d = (error_filtered - pid->error_previous_filtered) / dt; + d = pid->error_previous_filtered - error_filtered; + pid->error_previous_filtered = error_filtered; } else if (pid->mode == PID_MODE_DERIVATIV_SET) { d = -val_dot; @@ -180,6 +181,11 @@ __EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, flo pid->last_output = output; } + pid->control_previous = pid->last_output; + + // if (isfinite(error)) { // Why is this necessary? DEW + // pid->error_previous = error; + // } return pid->last_output; } diff --git a/src/modules/systemlib/pid/pid.h b/src/modules/systemlib/pid/pid.h index 64d6688677..5790459c81 100644 --- a/src/modules/systemlib/pid/pid.h +++ b/src/modules/systemlib/pid/pid.h @@ -59,7 +59,8 @@ typedef struct { float intmax; float sp; float integral; - float error_previous; + float error_previous_filtered; + float control_previous; float last_output; float limit; uint8_t mode; From 8559315f4f8b6515f2ba9ceadf2aa3b73af41bdc Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Wed, 13 Mar 2013 10:53:20 -0700 Subject: [PATCH 040/109] Added a filter parameter to the pid function Conflicts: apps/multirotor_pos_control/multirotor_pos_control.c --- .../fixedwing_att_control_att.c | 8 ++++---- .../fixedwing_att_control_rate.c | 12 ++++++------ .../fixedwing_pos_control_main.c | 16 ++++++++-------- .../multirotor_attitude_control.c | 8 ++++---- .../multirotor_rate_control.c | 8 ++++---- src/modules/systemlib/pid/pid.c | 14 +++++++++++--- src/modules/systemlib/pid/pid.h | 5 +++-- 7 files changed, 40 insertions(+), 31 deletions(-) diff --git a/src/modules/fixedwing_att_control/fixedwing_att_control_att.c b/src/modules/fixedwing_att_control/fixedwing_att_control_att.c index 769b8b0a8d..a226757e0c 100644 --- a/src/modules/fixedwing_att_control/fixedwing_att_control_att.c +++ b/src/modules/fixedwing_att_control/fixedwing_att_control_att.c @@ -128,8 +128,8 @@ int fixedwing_att_control_attitude(const struct vehicle_attitude_setpoint_s *att if (!initialized) { parameters_init(&h); parameters_update(&h, &p); - pid_init(&roll_controller, p.roll_p, 0, 0, 0, p.rollrate_lim, PID_MODE_DERIVATIV_NONE); //P Controller - pid_init(&pitch_controller, p.pitch_p, 0, 0, 0, p.pitchrate_lim, PID_MODE_DERIVATIV_NONE); //P Controller + pid_init(&roll_controller, p.roll_p, 0, 0, 0, p.rollrate_lim, 0, PID_MODE_DERIVATIV_NONE); //P Controller + pid_init(&pitch_controller, p.pitch_p, 0, 0, 0, p.pitchrate_lim, 0, PID_MODE_DERIVATIV_NONE); //P Controller initialized = true; } @@ -137,8 +137,8 @@ int fixedwing_att_control_attitude(const struct vehicle_attitude_setpoint_s *att if (counter % 100 == 0) { /* update parameters from storage */ parameters_update(&h, &p); - pid_set_parameters(&roll_controller, p.roll_p, 0, 0, 0, p.rollrate_lim); - pid_set_parameters(&pitch_controller, p.pitch_p, 0, 0, 0, p.pitchrate_lim); + pid_set_parameters(&roll_controller, p.roll_p, 0, 0, 0, p.rollrate_lim, 0); + pid_set_parameters(&pitch_controller, p.pitch_p, 0, 0, 0, p.pitchrate_lim, 0); } /* Roll (P) */ diff --git a/src/modules/fixedwing_att_control/fixedwing_att_control_rate.c b/src/modules/fixedwing_att_control/fixedwing_att_control_rate.c index 4eccc118cb..79194e5151 100644 --- a/src/modules/fixedwing_att_control/fixedwing_att_control_rate.c +++ b/src/modules/fixedwing_att_control/fixedwing_att_control_rate.c @@ -179,9 +179,9 @@ int fixedwing_att_control_rates(const struct vehicle_rates_setpoint_s *rate_sp, if (!initialized) { parameters_init(&h); parameters_update(&h, &p); - pid_init(&roll_rate_controller, p.rollrate_p, p.rollrate_i, 0, p.rollrate_awu, 1, PID_MODE_DERIVATIV_NONE); // set D part to 0 because the controller layout is with a PI rate controller - pid_init(&pitch_rate_controller, p.pitchrate_p, p.pitchrate_i, 0, p.pitchrate_awu, 1, PID_MODE_DERIVATIV_NONE); // set D part to 0 because the contpitcher layout is with a PI rate contpitcher - pid_init(&yaw_rate_controller, p.yawrate_p, p.yawrate_i, 0, p.yawrate_awu, 1, PID_MODE_DERIVATIV_NONE); // set D part to 0 because the contpitcher layout is with a PI rate contpitcher + pid_init(&roll_rate_controller, p.rollrate_p, p.rollrate_i, 0, p.rollrate_awu, 1, 0, PID_MODE_DERIVATIV_NONE); // set D part to 0 because the controller layout is with a PI rate controller + pid_init(&pitch_rate_controller, p.pitchrate_p, p.pitchrate_i, 0, p.pitchrate_awu, 1, 0, PID_MODE_DERIVATIV_NONE); // set D part to 0 because the contpitcher layout is with a PI rate contpitcher + pid_init(&yaw_rate_controller, p.yawrate_p, p.yawrate_i, 0, p.yawrate_awu, 1, 0, PID_MODE_DERIVATIV_NONE); // set D part to 0 because the contpitcher layout is with a PI rate contpitcher initialized = true; } @@ -189,9 +189,9 @@ int fixedwing_att_control_rates(const struct vehicle_rates_setpoint_s *rate_sp, if (counter % 100 == 0) { /* update parameters from storage */ parameters_update(&h, &p); - pid_set_parameters(&roll_rate_controller, p.rollrate_p, p.rollrate_i, 0, p.rollrate_awu, 1); - pid_set_parameters(&pitch_rate_controller, p.pitchrate_p, p.pitchrate_i, 0, p.pitchrate_awu, 1); - pid_set_parameters(&yaw_rate_controller, p.yawrate_p, p.yawrate_i, 0, p.yawrate_awu, 1); + pid_set_parameters(&roll_rate_controller, p.rollrate_p, p.rollrate_i, 0, p.rollrate_awu, 1, 0); + pid_set_parameters(&pitch_rate_controller, p.pitchrate_p, p.pitchrate_i, 0, p.pitchrate_awu, 1, 0); + pid_set_parameters(&yaw_rate_controller, p.yawrate_p, p.yawrate_i, 0, p.yawrate_awu, 1, 0); } diff --git a/src/modules/fixedwing_pos_control/fixedwing_pos_control_main.c b/src/modules/fixedwing_pos_control/fixedwing_pos_control_main.c index 71c78f5b86..48ec3603ff 100644 --- a/src/modules/fixedwing_pos_control/fixedwing_pos_control_main.c +++ b/src/modules/fixedwing_pos_control/fixedwing_pos_control_main.c @@ -239,10 +239,10 @@ int fixedwing_pos_control_thread_main(int argc, char *argv[]) parameters_init(&h); parameters_update(&h, &p); - pid_init(&heading_controller, p.heading_p, 0.0f, 0.0f, 0.0f, 10000.0f, PID_MODE_DERIVATIV_NONE); //arbitrary high limit - pid_init(&heading_rate_controller, p.headingr_p, p.headingr_i, 0.0f, 0.0f, p.roll_lim, PID_MODE_DERIVATIV_NONE); - pid_init(&altitude_controller, p.altitude_p, 0.0f, 0.0f, 0.0f, p.pitch_lim, PID_MODE_DERIVATIV_NONE); - pid_init(&offtrack_controller, p.xtrack_p, 0.0f, 0.0f, 0.0f , 60.0f * M_DEG_TO_RAD, PID_MODE_DERIVATIV_NONE); //TODO: remove hardcoded value + pid_init(&heading_controller, p.heading_p, 0.0f, 0.0f, 0.0f, 10000.0f, 0.0f, PID_MODE_DERIVATIV_NONE); //arbitrary high limit + pid_init(&heading_rate_controller, p.headingr_p, p.headingr_i, 0.0f, 0.0f, p.roll_lim, 0.0f, PID_MODE_DERIVATIV_NONE); + pid_init(&altitude_controller, p.altitude_p, 0.0f, 0.0f, 0.0f, p.pitch_lim, 0.0f, PID_MODE_DERIVATIV_NONE); + pid_init(&offtrack_controller, p.xtrack_p, 0.0f, 0.0f, 0.0f , 60.0f * M_DEG_TO_RAD_F, 0.0f, PID_MODE_DERIVATIV_NONE); //TODO: remove hardcoded value /* error and performance monitoring */ perf_counter_t fw_interval_perf = perf_alloc(PC_INTERVAL, "fixedwing_pos_control_interval"); @@ -268,10 +268,10 @@ int fixedwing_pos_control_thread_main(int argc, char *argv[]) /* update parameters from storage */ parameters_update(&h, &p); - pid_set_parameters(&heading_controller, p.heading_p, 0, 0, 0, 10000.0f); //arbitrary high limit - pid_set_parameters(&heading_rate_controller, p.headingr_p, p.headingr_i, 0, 0, p.roll_lim); - pid_set_parameters(&altitude_controller, p.altitude_p, 0, 0, 0, p.pitch_lim); - pid_set_parameters(&offtrack_controller, p.xtrack_p, 0, 0, 0, 60.0f * M_DEG_TO_RAD); //TODO: remove hardcoded value + pid_set_parameters(&heading_controller, p.heading_p, 0, 0, 0, 10000.0f, 0.0f); //arbitrary high limit + pid_set_parameters(&heading_rate_controller, p.headingr_p, p.headingr_i, 0, 0, p.roll_lim, 0.0f); + pid_set_parameters(&altitude_controller, p.altitude_p, 0, 0, 0, p.pitch_lim, 0.0f); + pid_set_parameters(&offtrack_controller, p.xtrack_p, 0, 0, 0, 60.0f * M_DEG_TO_RAD_F, 0.0f); //TODO: remove hardcoded value } /* only run controller if attitude changed */ diff --git a/src/modules/multirotor_att_control/multirotor_attitude_control.c b/src/modules/multirotor_att_control/multirotor_attitude_control.c index 1053ac7a31..d7e1a3adcb 100644 --- a/src/modules/multirotor_att_control/multirotor_attitude_control.c +++ b/src/modules/multirotor_att_control/multirotor_attitude_control.c @@ -155,8 +155,8 @@ void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_s parameters_init(&h); parameters_update(&h, &p); - pid_init(&pitch_controller, p.att_p, p.att_i, p.att_d, 1000.0f, 1000.0f, PID_MODE_DERIVATIV_SET); - pid_init(&roll_controller, p.att_p, p.att_i, p.att_d, 1000.0f, 1000.0f, PID_MODE_DERIVATIV_SET); + pid_init(&pitch_controller, p.att_p, p.att_i, p.att_d, 1000.0f, 1000.0f, 0.0f, PID_MODE_DERIVATIV_SET); + pid_init(&roll_controller, p.att_p, p.att_i, p.att_d, 1000.0f, 1000.0f, 0.0f, PID_MODE_DERIVATIV_SET); initialized = true; } @@ -167,8 +167,8 @@ void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_s parameters_update(&h, &p); /* apply parameters */ - pid_set_parameters(&pitch_controller, p.att_p, p.att_i, p.att_d, 1000.0f, 1000.0f); - pid_set_parameters(&roll_controller, p.att_p, p.att_i, p.att_d, 1000.0f, 1000.0f); + pid_set_parameters(&pitch_controller, p.att_p, p.att_i, p.att_d, 1000.0f, 1000.0f, 0.0f); + pid_set_parameters(&roll_controller, p.att_p, p.att_i, p.att_d, 1000.0f, 1000.0f, 0.0f); } /* reset integral if on ground */ diff --git a/src/modules/multirotor_att_control/multirotor_rate_control.c b/src/modules/multirotor_att_control/multirotor_rate_control.c index 4ab92b9557..57aea726aa 100644 --- a/src/modules/multirotor_att_control/multirotor_rate_control.c +++ b/src/modules/multirotor_att_control/multirotor_rate_control.c @@ -182,17 +182,17 @@ void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp, initialized = true; pid_init(&pitch_rate_controller, p.attrate_p, p.attrate_i, p.attrate_d, 1000.0f, - 1000.0f, PID_MODE_DERIVATIV_CALC); + 1000.0f, 0.2f, PID_MODE_DERIVATIV_CALC); pid_init(&roll_rate_controller, p.attrate_p, p.attrate_i, p.attrate_d, 1000.0f, - 1000.0f, PID_MODE_DERIVATIV_CALC); + 1000.0f, 0.2f, PID_MODE_DERIVATIV_CALC); } /* load new parameters with lower rate */ if (motor_skip_counter % 2500 == 0) { /* update parameters from storage */ parameters_update(&h, &p); - pid_set_parameters(&pitch_rate_controller, p.attrate_p, p.attrate_i, p.attrate_d, 1000.0f, 1000.0f); - pid_set_parameters(&roll_rate_controller, p.attrate_p, p.attrate_i, p.attrate_d, 1000.0f, 1000.0f); + pid_set_parameters(&pitch_rate_controller, p.attrate_p, p.attrate_i, p.attrate_d, 1000.0f, 1000.0f, 0.2f); + pid_set_parameters(&roll_rate_controller, p.attrate_p, p.attrate_i, p.attrate_d, 1000.0f, 1000.0f, 0.2f); } /* control pitch (forward) output */ diff --git a/src/modules/systemlib/pid/pid.c b/src/modules/systemlib/pid/pid.c index 08e0ca46fa..d0e67d3ea3 100644 --- a/src/modules/systemlib/pid/pid.c +++ b/src/modules/systemlib/pid/pid.c @@ -43,7 +43,7 @@ #include __EXPORT void pid_init(PID_t *pid, float kp, float ki, float kd, float intmax, - float limit, uint8_t mode) + float limit, float diff_filter_factor, uint8_t mode) { pid->kp = kp; pid->ki = ki; @@ -51,6 +51,7 @@ __EXPORT void pid_init(PID_t *pid, float kp, float ki, float kd, float intmax, pid->intmax = intmax; pid->limit = limit; pid->mode = mode; + pid->diff_filter_factor = diff_filter_factor; pid->count = 0.0f; pid->saturated = 0.0f; pid->last_output = 0.0f; @@ -60,7 +61,7 @@ __EXPORT void pid_init(PID_t *pid, float kp, float ki, float kd, float intmax, pid->control_previous = 0.0f; pid->integral = 0.0f; } -__EXPORT int pid_set_parameters(PID_t *pid, float kp, float ki, float kd, float intmax, float limit) +__EXPORT int pid_set_parameters(PID_t *pid, float kp, float ki, float kd, float intmax, float limit, float diff_filter_factor) { int ret = 0; @@ -99,6 +100,13 @@ __EXPORT int pid_set_parameters(PID_t *pid, float kp, float ki, float kd, float ret = 1; } + if (isfinite(diff_filter_factor)) { + pid->limit = diff_filter_factor; + + } else { + ret = 1; + } + return ret; } @@ -137,7 +145,7 @@ __EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, flo // Calculated current error value float error = pid->sp - val; - float error_filtered = 0.2f*error + 0.8f*pid->error_previous_filtered; + float error_filtered = pid->diff_filter_factor*error + (1.0f-pid->diff_filter_factor)*pid->error_previous_filtered; // Calculate or measured current error derivative diff --git a/src/modules/systemlib/pid/pid.h b/src/modules/systemlib/pid/pid.h index 5790459c81..f89c36d759 100644 --- a/src/modules/systemlib/pid/pid.h +++ b/src/modules/systemlib/pid/pid.h @@ -64,12 +64,13 @@ typedef struct { float last_output; float limit; uint8_t mode; + float diff_filter_factor; uint8_t count; uint8_t saturated; } PID_t; -__EXPORT void pid_init(PID_t *pid, float kp, float ki, float kd, float intmax, float limit, uint8_t mode); -__EXPORT int pid_set_parameters(PID_t *pid, float kp, float ki, float kd, float intmax, float limit); +__EXPORT void pid_init(PID_t *pid, float kp, float ki, float kd, float intmax, float limit, float diff_filter_factor, uint8_t mode); +__EXPORT int pid_set_parameters(PID_t *pid, float kp, float ki, float kd, float intmax, float limit, float diff_filter_factor); //void pid_set(PID_t *pid, float sp); __EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, float dt); From 263b60c200b80af68fea7a491cb17e9db4f65135 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Sun, 16 Jun 2013 09:54:57 +0200 Subject: [PATCH 041/109] Hack to make flow controll to compile --- .../flow_position_control/flow_position_control_main.c | 4 +++- .../flow_position_estimator/flow_position_estimator_main.c | 4 ++++ src/examples/flow_speed_control/flow_speed_control_main.c | 2 ++ 3 files changed, 9 insertions(+), 1 deletion(-) diff --git a/src/examples/flow_position_control/flow_position_control_main.c b/src/examples/flow_position_control/flow_position_control_main.c index c177c8fd20..5246ea62b6 100644 --- a/src/examples/flow_position_control/flow_position_control_main.c +++ b/src/examples/flow_position_control/flow_position_control_main.c @@ -268,6 +268,8 @@ flow_position_control_thread_main(int argc, char *argv[]) /* get a local copy of local position */ orb_copy(ORB_ID(vehicle_local_position), vehicle_local_position_sub, &local_pos); +// XXX fix this +#if 0 if (vstatus.state_machine == SYSTEM_STATE_AUTO) { float manual_pitch = manual.pitch / params.rc_scale_pitch; // 0 to 1 @@ -527,7 +529,7 @@ flow_position_control_thread_main(int argc, char *argv[]) if(isfinite(manual.throttle)) speed_sp.thrust_sp = manual.throttle; } - +#endif /* measure in what intervals the controller runs */ perf_count(mc_interval_perf); perf_end(mc_loop_perf); diff --git a/src/examples/flow_position_estimator/flow_position_estimator_main.c b/src/examples/flow_position_estimator/flow_position_estimator_main.c index c0b16d2e78..2389c693d0 100644 --- a/src/examples/flow_position_estimator/flow_position_estimator_main.c +++ b/src/examples/flow_position_estimator/flow_position_estimator_main.c @@ -218,6 +218,8 @@ int flow_position_estimator_thread_main(int argc, char *argv[]) while (!thread_should_exit) { +// XXX fix this +#if 0 if (sensors_ready) { /*This runs at the rate of the sensors */ @@ -273,6 +275,7 @@ int flow_position_estimator_thread_main(int argc, char *argv[]) * -> accept sonar measurements after reaching calibration distance (values between 0.3m and 1.0m for some time) * -> minimum sonar value 0.3m */ + if (!vehicle_liftoff) { if (vstatus.flag_system_armed && att_sp.thrust > params.minimum_liftoff_thrust && sonar_new > 0.3f && sonar_new < 1.0f) @@ -437,6 +440,7 @@ int flow_position_estimator_thread_main(int argc, char *argv[]) } counter++; + #endif } printf("[flow position estimator] exiting.\n"); diff --git a/src/examples/flow_speed_control/flow_speed_control_main.c b/src/examples/flow_speed_control/flow_speed_control_main.c index 9648728c81..0be4b3d5a0 100644 --- a/src/examples/flow_speed_control/flow_speed_control_main.c +++ b/src/examples/flow_speed_control/flow_speed_control_main.c @@ -186,6 +186,7 @@ flow_speed_control_thread_main(int argc, char *argv[]) while (!thread_should_exit) { +#if 0 /* wait for first attitude msg to be sure all data are available */ if (sensors_ready) { @@ -340,6 +341,7 @@ flow_speed_control_thread_main(int argc, char *argv[]) } } } +#endif } printf("[flow speed control] ending now...\n"); From 68fb200f0bc7e995fd604ee9e345f37839d02ced Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Sun, 16 Jun 2013 09:55:28 +0200 Subject: [PATCH 042/109] Fixed pid bug, attitude was not controlled --- .../multirotor_attitude_control.c | 10 ++++++---- src/modules/systemlib/pid/pid.c | 5 ++--- 2 files changed, 8 insertions(+), 7 deletions(-) diff --git a/src/modules/multirotor_att_control/multirotor_attitude_control.c b/src/modules/multirotor_att_control/multirotor_attitude_control.c index d7e1a3adcb..4c8f72b8d5 100644 --- a/src/modules/multirotor_att_control/multirotor_attitude_control.c +++ b/src/modules/multirotor_att_control/multirotor_attitude_control.c @@ -155,8 +155,8 @@ void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_s parameters_init(&h); parameters_update(&h, &p); - pid_init(&pitch_controller, p.att_p, p.att_i, p.att_d, 1000.0f, 1000.0f, 0.0f, PID_MODE_DERIVATIV_SET); - pid_init(&roll_controller, p.att_p, p.att_i, p.att_d, 1000.0f, 1000.0f, 0.0f, PID_MODE_DERIVATIV_SET); + pid_init(&pitch_controller, p.att_p, p.att_i, p.att_d, 1000.0f, 1000.0f, 0.2f, PID_MODE_DERIVATIV_SET); + pid_init(&roll_controller, p.att_p, p.att_i, p.att_d, 1000.0f, 1000.0f, 0.2f, PID_MODE_DERIVATIV_SET); initialized = true; } @@ -167,8 +167,8 @@ void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_s parameters_update(&h, &p); /* apply parameters */ - pid_set_parameters(&pitch_controller, p.att_p, p.att_i, p.att_d, 1000.0f, 1000.0f, 0.0f); - pid_set_parameters(&roll_controller, p.att_p, p.att_i, p.att_d, 1000.0f, 1000.0f, 0.0f); + pid_set_parameters(&pitch_controller, p.att_p, p.att_i, p.att_d, 1000.0f, 1000.0f, 0.2f); + pid_set_parameters(&roll_controller, p.att_p, p.att_i, p.att_d, 1000.0f, 1000.0f, 0.2f); } /* reset integral if on ground */ @@ -188,6 +188,8 @@ void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_s rates_sp->roll = pid_calculate(&roll_controller, att_sp->roll_body , att->roll, att->rollspeed, deltaT); + // printf("rates_sp: %4.4f, att setpoint: %4.4f\n, pitch: %4.4f, pitchspeed: %4.4f, dT: %4.4f", rates_sp->pitch, att_sp->pitch_body, att->pitch, att->pitchspeed, deltaT); + if (control_yaw_position) { /* control yaw rate */ diff --git a/src/modules/systemlib/pid/pid.c b/src/modules/systemlib/pid/pid.c index d0e67d3ea3..0188a51c4f 100644 --- a/src/modules/systemlib/pid/pid.c +++ b/src/modules/systemlib/pid/pid.c @@ -101,7 +101,7 @@ __EXPORT int pid_set_parameters(PID_t *pid, float kp, float ki, float kd, float } if (isfinite(diff_filter_factor)) { - pid->limit = diff_filter_factor; + pid->diff_filter_factor = diff_filter_factor; } else { ret = 1; @@ -179,8 +179,7 @@ __EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, flo } // Calculate the output. Limit output magnitude to pid->limit - float output = (pid->error_previous * pid->kp) + (i * pid->ki) + (d * pid->kd); - + float output = (error * pid->kp) + (i * pid->ki) + (d * pid->kd); if (output > pid->limit) output = pid->limit; if (output < -pid->limit) output = -pid->limit; From 1ea9ff36402384d66879216a3bc7509651af0be6 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Wed, 13 Mar 2013 18:00:00 -0700 Subject: [PATCH 043/109] Added possibility to log pid control values Conflicts: apps/multirotor_pos_control/multirotor_pos_control.c src/drivers/ardrone_interface/ardrone_interface.c --- .../fixedwing_att_control/fixedwing_att_control_att.c | 4 ++-- .../fixedwing_att_control/fixedwing_att_control_rate.c | 6 +++--- .../fixedwing_pos_control/fixedwing_pos_control_main.c | 8 ++++---- .../multirotor_att_control/multirotor_attitude_control.c | 4 ++-- .../multirotor_att_control/multirotor_rate_control.c | 4 ++-- src/modules/systemlib/pid/pid.c | 8 ++++++-- src/modules/systemlib/pid/pid.h | 2 +- 7 files changed, 20 insertions(+), 16 deletions(-) diff --git a/src/modules/fixedwing_att_control/fixedwing_att_control_att.c b/src/modules/fixedwing_att_control/fixedwing_att_control_att.c index a226757e0c..7009356b78 100644 --- a/src/modules/fixedwing_att_control/fixedwing_att_control_att.c +++ b/src/modules/fixedwing_att_control/fixedwing_att_control_att.c @@ -142,7 +142,7 @@ int fixedwing_att_control_attitude(const struct vehicle_attitude_setpoint_s *att } /* Roll (P) */ - rates_sp->roll = pid_calculate(&roll_controller, att_sp->roll_body, att->roll, 0, 0); + rates_sp->roll = pid_calculate(&roll_controller, att_sp->roll_body, att->roll, 0, 0, NULL, NULL, NULL); /* Pitch (P) */ @@ -152,7 +152,7 @@ int fixedwing_att_control_attitude(const struct vehicle_attitude_setpoint_s *att /* set pitch plus feedforward roll compensation */ rates_sp->pitch = pid_calculate(&pitch_controller, att_sp->pitch_body + pitch_sp_rollcompensation, - att->pitch, 0, 0); + att->pitch, 0, 0, NULL, NULL, NULL); /* Yaw (from coordinated turn constraint or lateral force) */ rates_sp->yaw = (att->rollspeed * rates_sp->roll + 9.81f * sinf(att->roll) * cosf(att->pitch) + speed_body[0] * rates_sp->pitch * sinf(att->roll)) diff --git a/src/modules/fixedwing_att_control/fixedwing_att_control_rate.c b/src/modules/fixedwing_att_control/fixedwing_att_control_rate.c index 79194e5151..991d5ec5d1 100644 --- a/src/modules/fixedwing_att_control/fixedwing_att_control_rate.c +++ b/src/modules/fixedwing_att_control/fixedwing_att_control_rate.c @@ -196,11 +196,11 @@ int fixedwing_att_control_rates(const struct vehicle_rates_setpoint_s *rate_sp, /* roll rate (PI) */ - actuators->control[0] = pid_calculate(&roll_rate_controller, rate_sp->roll, rates[0], 0.0f, deltaT); + actuators->control[0] = pid_calculate(&roll_rate_controller, rate_sp->roll, rates[0], 0.0f, deltaT, NULL, NULL, NULL); /* pitch rate (PI) */ - actuators->control[1] = -pid_calculate(&pitch_rate_controller, rate_sp->pitch, rates[1], 0.0f, deltaT); + actuators->control[1] = -pid_calculate(&pitch_rate_controller, rate_sp->pitch, rates[1], 0.0f, deltaT, NULL, NULL, NULL); /* yaw rate (PI) */ - actuators->control[2] = pid_calculate(&yaw_rate_controller, rate_sp->yaw, rates[2], 0.0f, deltaT); + actuators->control[2] = pid_calculate(&yaw_rate_controller, rate_sp->yaw, rates[2], 0.0f, deltaT, NULL, NULL, NULL); counter++; diff --git a/src/modules/fixedwing_pos_control/fixedwing_pos_control_main.c b/src/modules/fixedwing_pos_control/fixedwing_pos_control_main.c index 48ec3603ff..9c19c6786e 100644 --- a/src/modules/fixedwing_pos_control/fixedwing_pos_control_main.c +++ b/src/modules/fixedwing_pos_control/fixedwing_pos_control_main.c @@ -319,7 +319,7 @@ int fixedwing_pos_control_thread_main(int argc, char *argv[]) // XXX what is xtrack_err.past_end? if (distance_res == OK /*&& !xtrack_err.past_end*/) { - float delta_psi_c = pid_calculate(&offtrack_controller, 0, xtrack_err.distance, 0.0f, 0.0f); //p.xtrack_p * xtrack_err.distance + float delta_psi_c = pid_calculate(&offtrack_controller, 0, xtrack_err.distance, 0.0f, 0.0f, NULL, NULL, NULL); //p.xtrack_p * xtrack_err.distance float psi_c = psi_track + delta_psi_c; float psi_e = psi_c - att.yaw; @@ -336,7 +336,7 @@ int fixedwing_pos_control_thread_main(int argc, char *argv[]) } /* calculate roll setpoint, do this artificially around zero */ - float delta_psi_rate_c = pid_calculate(&heading_controller, psi_e, 0.0f, 0.0f, 0.0f); + float delta_psi_rate_c = pid_calculate(&heading_controller, psi_e, 0.0f, 0.0f, 0.0f, NULL, NULL, NULL); float psi_rate_track = 0; //=V_gr/r_track , this will be needed for implementation of arc following float psi_rate_c = delta_psi_rate_c + psi_rate_track; @@ -359,7 +359,7 @@ int fixedwing_pos_control_thread_main(int argc, char *argv[]) float psi_rate_e_scaled = psi_rate_e * ground_speed / 9.81f; //* V_gr / g - attitude_setpoint.roll_body = pid_calculate(&heading_rate_controller, psi_rate_e_scaled, 0.0f, 0.0f, deltaT); + attitude_setpoint.roll_body = pid_calculate(&heading_rate_controller, psi_rate_e_scaled, 0.0f, 0.0f, deltaT, NULL, NULL, NULL); if (verbose) { printf("psi_rate_c %.4f ", (double)psi_rate_c); @@ -379,7 +379,7 @@ int fixedwing_pos_control_thread_main(int argc, char *argv[]) if (pos_updated) { //TODO: take care of relative vs. ab. altitude - attitude_setpoint.pitch_body = pid_calculate(&altitude_controller, global_setpoint.altitude, global_pos.alt, 0.0f, 0.0f); + attitude_setpoint.pitch_body = pid_calculate(&altitude_controller, global_setpoint.altitude, global_pos.alt, 0.0f, 0.0f, NULL, NULL, NULL); } diff --git a/src/modules/multirotor_att_control/multirotor_attitude_control.c b/src/modules/multirotor_att_control/multirotor_attitude_control.c index 4c8f72b8d5..51faaa8c0d 100644 --- a/src/modules/multirotor_att_control/multirotor_attitude_control.c +++ b/src/modules/multirotor_att_control/multirotor_attitude_control.c @@ -182,11 +182,11 @@ void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_s /* control pitch (forward) output */ rates_sp->pitch = pid_calculate(&pitch_controller, att_sp->pitch_body , - att->pitch, att->pitchspeed, deltaT); + att->pitch, att->pitchspeed, deltaT, NULL, NULL, NULL); /* control roll (left/right) output */ rates_sp->roll = pid_calculate(&roll_controller, att_sp->roll_body , - att->roll, att->rollspeed, deltaT); + att->roll, att->rollspeed, deltaT, NULL, NULL, NULL); // printf("rates_sp: %4.4f, att setpoint: %4.4f\n, pitch: %4.4f, pitchspeed: %4.4f, dT: %4.4f", rates_sp->pitch, att_sp->pitch_body, att->pitch, att->pitchspeed, deltaT); diff --git a/src/modules/multirotor_att_control/multirotor_rate_control.c b/src/modules/multirotor_att_control/multirotor_rate_control.c index 57aea726aa..322c5485ae 100644 --- a/src/modules/multirotor_att_control/multirotor_rate_control.c +++ b/src/modules/multirotor_att_control/multirotor_rate_control.c @@ -197,11 +197,11 @@ void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp, /* control pitch (forward) output */ float pitch_control = pid_calculate(&pitch_rate_controller, rate_sp->pitch , - rates[1], 0.0f, deltaT); + rates[1], 0.0f, deltaT, NULL, NULL, NULL); /* control roll (left/right) output */ float roll_control = pid_calculate(&roll_rate_controller, rate_sp->roll , - rates[0], 0.0f, deltaT); + rates[0], 0.0f, deltaT, NULL, NULL, NULL); /* increase resilience to faulty control inputs */ if (isfinite(pitch_control)) { diff --git a/src/modules/systemlib/pid/pid.c b/src/modules/systemlib/pid/pid.c index 0188a51c4f..3685b2aebb 100644 --- a/src/modules/systemlib/pid/pid.c +++ b/src/modules/systemlib/pid/pid.c @@ -124,7 +124,7 @@ __EXPORT int pid_set_parameters(PID_t *pid, float kp, float ki, float kd, float * @param dt * @return */ -__EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, float dt) +__EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, float dt, float *ctrl_p, float *ctrl_i, float *ctrl_d) { /* error = setpoint - actual_position integral = integral + (error*dt) @@ -152,7 +152,7 @@ __EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, flo if (pid->mode == PID_MODE_DERIVATIV_CALC) { // d = (error_filtered - pid->error_previous_filtered) / dt; - d = pid->error_previous_filtered - error_filtered; + d = error_filtered - pid->error_previous_filtered ; pid->error_previous_filtered = error_filtered; } else if (pid->mode == PID_MODE_DERIVATIV_SET) { d = -val_dot; @@ -194,6 +194,10 @@ __EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, flo // pid->error_previous = error; // } + *ctrl_p = (error * pid->kp); + *ctrl_i = (i * pid->ki); + *ctrl_d = (d * pid->kd); + return pid->last_output; } diff --git a/src/modules/systemlib/pid/pid.h b/src/modules/systemlib/pid/pid.h index f89c36d759..e3d9038cdd 100644 --- a/src/modules/systemlib/pid/pid.h +++ b/src/modules/systemlib/pid/pid.h @@ -72,7 +72,7 @@ typedef struct { __EXPORT void pid_init(PID_t *pid, float kp, float ki, float kd, float intmax, float limit, float diff_filter_factor, uint8_t mode); __EXPORT int pid_set_parameters(PID_t *pid, float kp, float ki, float kd, float intmax, float limit, float diff_filter_factor); //void pid_set(PID_t *pid, float sp); -__EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, float dt); +__EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, float dt, float *ctrl_p, float *ctrl_i, float *ctrl_d); __EXPORT void pid_reset_integral(PID_t *pid); From 562253c508d1a758207d21e116cbbc194d7e0721 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Sun, 16 Jun 2013 11:55:08 +0200 Subject: [PATCH 044/109] Fixed bug that I introduced in sdlog2 --- src/modules/sdlog2/sdlog2.c | 34 +++++++++++++++++++++++----------- 1 file changed, 23 insertions(+), 11 deletions(-) diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 41c2f67e56..940f30a46b 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -613,7 +613,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 = 15; + const ssize_t fdsc = 16; /* Sanity check variable and index */ ssize_t fdsc_count = 0; /* file descriptors to wait for */ @@ -622,9 +622,11 @@ int sdlog2_thread_main(int argc, char *argv[]) struct vehicle_status_s buf_status; memset(&buf_status, 0, sizeof(buf_status)); + struct actuator_safety_s buf_safety; + memset(&buf_safety, 0, sizeof(buf_safety)); + /* warning! using union here to save memory, elements should be used separately! */ union { - struct actuator_safety_s safety; struct vehicle_command_s cmd; struct sensor_combined_s sensor; struct vehicle_attitude_s att; @@ -645,9 +647,9 @@ int sdlog2_thread_main(int argc, char *argv[]) memset(&buf, 0, sizeof(buf)); struct { - int safety_sub; int cmd_sub; int status_sub; + int safety_sub; int sensor_sub; int att_sub; int att_sp_sub; @@ -693,7 +695,7 @@ int sdlog2_thread_main(int argc, char *argv[]) fds[fdsc_count].events = POLLIN; fdsc_count++; - /* --- SAFETY STATUS --- */ + /* --- SAFETY --- */ subs.safety_sub = orb_subscribe(ORB_ID(actuator_safety)); fds[fdsc_count].fd = subs.safety_sub; fds[fdsc_count].events = POLLIN; @@ -835,7 +837,6 @@ int sdlog2_thread_main(int argc, char *argv[]) int ifds = 0; int handled_topics = 0; - /* --- VEHICLE COMMAND - LOG MANAGEMENT --- */ if (fds[ifds++].revents & POLLIN) { orb_copy(ORB_ID(vehicle_command), subs.cmd_sub, &buf.cmd); @@ -843,22 +844,33 @@ int sdlog2_thread_main(int argc, char *argv[]) handled_topics++; } + /* --- SAFETY- LOG MANAGEMENT --- */ + if (fds[ifds++].revents & POLLIN) { + orb_copy(ORB_ID(actuator_safety), subs.safety_sub, &buf_safety); + + if (log_when_armed) { + handle_status(&buf_safety); + } + + handled_topics++; + } + /* --- VEHICLE STATUS - LOG MANAGEMENT --- */ if (fds[ifds++].revents & POLLIN) { orb_copy(ORB_ID(vehicle_status), subs.status_sub, &buf_status); - if (log_when_armed) { - handle_status(&buf.safety); - } + //if (log_when_armed) { + // handle_status(&buf_safety); + //} - handled_topics++; + //handled_topics++; } if (!logging_enabled || !check_data || handled_topics >= poll_ret) { continue; } - ifds = 1; // Begin from fds[1] again + ifds = 2; // Begin from fds[2] again pthread_mutex_lock(&logbuffer_mutex); @@ -880,7 +892,7 @@ int sdlog2_thread_main(int argc, char *argv[]) log_msg.body.log_STAT.flight_mode = 0; log_msg.body.log_STAT.manual_control_mode = 0; log_msg.body.log_STAT.manual_sas_mode = 0; - log_msg.body.log_STAT.armed = (unsigned char) buf.safety.armed; /* XXX fmu armed correct? */ + log_msg.body.log_STAT.armed = (unsigned char) buf_safety.armed; /* XXX fmu armed correct? */ log_msg.body.log_STAT.battery_voltage = buf_status.voltage_battery; log_msg.body.log_STAT.battery_current = buf_status.current_battery; log_msg.body.log_STAT.battery_remaining = buf_status.battery_remaining; From b52d561b11298abb2982b786676f49eea96259d8 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Sun, 16 Jun 2013 12:59:50 +0200 Subject: [PATCH 045/109] Added ctrl debugging values --- .../multirotor_rate_control.c | 16 +++- src/modules/sdlog2/sdlog2.c | 36 +++++++- src/modules/sdlog2/sdlog2_messages.h | 33 ++++++- src/modules/uORB/objects_common.cpp | 3 + .../uORB/topics/vehicle_control_debug.h | 87 +++++++++++++++++++ 5 files changed, 170 insertions(+), 5 deletions(-) create mode 100644 src/modules/uORB/topics/vehicle_control_debug.h diff --git a/src/modules/multirotor_att_control/multirotor_rate_control.c b/src/modules/multirotor_att_control/multirotor_rate_control.c index 322c5485ae..01bf383e27 100644 --- a/src/modules/multirotor_att_control/multirotor_rate_control.c +++ b/src/modules/multirotor_att_control/multirotor_rate_control.c @@ -54,6 +54,9 @@ #include #include #include +#include +#include + PARAM_DEFINE_FLOAT(MC_YAWRATE_P, 0.0f); /* same on Flamewheel */ PARAM_DEFINE_FLOAT(MC_YAWRATE_D, 0.0f); @@ -175,6 +178,11 @@ void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp, static bool initialized = false; + static struct vehicle_control_debug_s control_debug; + static orb_advert_t control_debug_pub; + + + /* initialize the pid controllers when the function is called for the first time */ if (initialized == false) { parameters_init(&h); @@ -185,6 +193,8 @@ void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp, 1000.0f, 0.2f, PID_MODE_DERIVATIV_CALC); pid_init(&roll_rate_controller, p.attrate_p, p.attrate_i, p.attrate_d, 1000.0f, 1000.0f, 0.2f, PID_MODE_DERIVATIV_CALC); + + control_debug_pub = orb_advertise(ORB_ID(vehicle_control_debug), &control_debug); } /* load new parameters with lower rate */ @@ -197,11 +207,11 @@ void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp, /* control pitch (forward) output */ float pitch_control = pid_calculate(&pitch_rate_controller, rate_sp->pitch , - rates[1], 0.0f, deltaT, NULL, NULL, NULL); + rates[1], 0.0f, deltaT, &control_debug.pitch_rate_p, &control_debug.pitch_rate_i, &control_debug.pitch_rate_d); /* control roll (left/right) output */ float roll_control = pid_calculate(&roll_rate_controller, rate_sp->roll , - rates[0], 0.0f, deltaT, NULL, NULL, NULL); + rates[0], 0.0f, deltaT, &control_debug.roll_rate_p, &control_debug.roll_rate_i, &control_debug.roll_rate_d); /* increase resilience to faulty control inputs */ if (isfinite(pitch_control)) { @@ -236,4 +246,6 @@ void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp, actuators->control[3] = rate_sp->thrust; motor_skip_counter++; + + orb_publish(ORB_ID(vehicle_control_debug), control_debug_pub, &control_debug); } diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 940f30a46b..844e02268b 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -74,6 +74,7 @@ #include #include #include +#include #include #include #include @@ -613,7 +614,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 = 16; + const ssize_t fdsc = 17; /* Sanity check variable and index */ ssize_t fdsc_count = 0; /* file descriptors to wait for */ @@ -639,6 +640,7 @@ int sdlog2_thread_main(int argc, char *argv[]) struct vehicle_global_position_s global_pos; struct vehicle_gps_position_s gps_pos; struct vehicle_vicon_position_s vicon_pos; + struct vehicle_control_debug_s control_debug; struct optical_flow_s flow; struct rc_channels_s rc; struct differential_pressure_s diff_pres; @@ -661,6 +663,7 @@ int sdlog2_thread_main(int argc, char *argv[]) int global_pos_sub; int gps_pos_sub; int vicon_pos_sub; + int control_debug_sub; int flow_sub; int rc_sub; } subs; @@ -680,6 +683,7 @@ int sdlog2_thread_main(int argc, char *argv[]) struct log_GPS_s log_GPS; struct log_ATTC_s log_ATTC; struct log_STAT_s log_STAT; + struct log_CTRL_s log_CTRL; struct log_RC_s log_RC; struct log_OUT0_s log_OUT0; } body; @@ -773,6 +777,12 @@ int sdlog2_thread_main(int argc, char *argv[]) fds[fdsc_count].events = POLLIN; fdsc_count++; + /* --- CONTROL DEBUG --- */ + subs.control_debug_sub = orb_subscribe(ORB_ID(vehicle_control_debug)); + fds[fdsc_count].fd = subs.control_debug_sub; + fds[fdsc_count].events = POLLIN; + fdsc_count++; + /* --- OPTICAL FLOW --- */ subs.flow_sub = orb_subscribe(ORB_ID(optical_flow)); fds[fdsc_count].fd = subs.flow_sub; @@ -1058,6 +1068,30 @@ int sdlog2_thread_main(int argc, char *argv[]) // TODO not implemented yet } + /* --- CONTROL DEBUG --- */ + if (fds[ifds++].revents & POLLIN) { + orb_copy(ORB_ID(vehicle_control_debug), subs.control_debug_sub, &buf.control_debug); + + log_msg.body.log_CTRL.roll_p = buf.control_debug.roll_p; + log_msg.body.log_CTRL.roll_i = buf.control_debug.roll_i; + log_msg.body.log_CTRL.roll_d = buf.control_debug.roll_d; + log_msg.body.log_CTRL.roll_rate_p = buf.control_debug.roll_rate_p; + log_msg.body.log_CTRL.roll_rate_i = buf.control_debug.roll_rate_i; + log_msg.body.log_CTRL.roll_rate_d = buf.control_debug.roll_rate_d; + log_msg.body.log_CTRL.pitch_p = buf.control_debug.pitch_p; + log_msg.body.log_CTRL.pitch_i = buf.control_debug.pitch_i; + log_msg.body.log_CTRL.pitch_d = buf.control_debug.pitch_d; + log_msg.body.log_CTRL.pitch_rate_p = buf.control_debug.pitch_rate_p; + log_msg.body.log_CTRL.pitch_rate_i = buf.control_debug.pitch_rate_i; + log_msg.body.log_CTRL.pitch_rate_d = buf.control_debug.pitch_rate_d; + log_msg.body.log_CTRL.yaw_p = buf.control_debug.yaw_p; + log_msg.body.log_CTRL.yaw_i = buf.control_debug.yaw_i; + log_msg.body.log_CTRL.yaw_d = buf.control_debug.yaw_d; + log_msg.body.log_CTRL.yaw_rate_p = buf.control_debug.yaw_rate_p; + log_msg.body.log_CTRL.yaw_rate_i = buf.control_debug.yaw_rate_i; + log_msg.body.log_CTRL.yaw_rate_d = buf.control_debug.yaw_rate_d; + } + /* --- FLOW --- */ if (fds[ifds++].revents & POLLIN) { orb_copy(ORB_ID(optical_flow), subs.flow_sub, &buf.flow); diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index 40763ee1e0..a80af33eff 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -156,14 +156,42 @@ struct log_STAT_s { unsigned char battery_warning; }; +/* --- CTRL - CONTROL DEBUG --- */ +#define LOG_CTRL_MSG 11 +struct log_CTRL_s { + float roll_p; + float roll_i; + float roll_d; + + float roll_rate_p; + float roll_rate_i; + float roll_rate_d; + + float pitch_p; + float pitch_i; + float pitch_d; + + float pitch_rate_p; + float pitch_rate_i; + float pitch_rate_d; + + float yaw_p; + float yaw_i; + float yaw_d; + + float yaw_rate_p; + float yaw_rate_i; + float yaw_rate_d; +}; + /* --- RC - RC INPUT CHANNELS --- */ -#define LOG_RC_MSG 11 +#define LOG_RC_MSG 12 struct log_RC_s { float channel[8]; }; /* --- OUT0 - ACTUATOR_0 OUTPUT --- */ -#define LOG_OUT0_MSG 12 +#define LOG_OUT0_MSG 13 struct log_OUT0_s { float output[8]; }; @@ -182,6 +210,7 @@ static const struct log_format_s log_formats[] = { LOG_FORMAT(GPS, "QBffLLfffff", "GPSTime,FixType,EPH,EPV,Lat,Lon,Alt,VelN,VelE,VelD,Cog"), LOG_FORMAT(ATTC, "ffff", "Roll,Pitch,Yaw,Thrust"), LOG_FORMAT(STAT, "BBBBBfffB", "State,FlightMode,CtlMode,SASMode,Armed,BatV,BatC,BatRem,BatWarn"), + LOG_FORMAT(CTRL, "ffffffffffffffffff", "RollP,RollI,RollD,RollRateP,RollRateI,RollRateD,PitchP,PitchI,PitchD,PitchRateP,PitchRateI,PitchRateD,YawP,YawI,YawD,YawRateP,YawRateI,YawRateD"), LOG_FORMAT(RC, "ffffffff", "Ch0,Ch1,Ch2,Ch3,Ch4,Ch5,Ch6,Ch7"), LOG_FORMAT(OUT0, "ffffffff", "Out0,Out1,Out2,Out3,Out4,Out5,Out6,Out7"), }; diff --git a/src/modules/uORB/objects_common.cpp b/src/modules/uORB/objects_common.cpp index 7dcb9cf93c..f1f07441d5 100644 --- a/src/modules/uORB/objects_common.cpp +++ b/src/modules/uORB/objects_common.cpp @@ -119,6 +119,9 @@ ORB_DEFINE(vehicle_attitude_setpoint, struct vehicle_attitude_setpoint_s); #include "topics/manual_control_setpoint.h" ORB_DEFINE(manual_control_setpoint, struct manual_control_setpoint_s); +#include "topics/vehicle_control_debug.h" +ORB_DEFINE(vehicle_control_debug, struct vehicle_control_debug_s); + #include "topics/offboard_control_setpoint.h" ORB_DEFINE(offboard_control_setpoint, struct offboard_control_setpoint_s); diff --git a/src/modules/uORB/topics/vehicle_control_debug.h b/src/modules/uORB/topics/vehicle_control_debug.h new file mode 100644 index 0000000000..6184284a43 --- /dev/null +++ b/src/modules/uORB/topics/vehicle_control_debug.h @@ -0,0 +1,87 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Author: @author Lorenz Meier + * + * 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 vehicle_control_debug.h + * For debugging purposes to log PID parts of controller + */ + +#ifndef TOPIC_VEHICLE_CONTROL_DEBUG_H_ +#define TOPIC_VEHICLE_CONTROL_DEBUG_H_ + +#include +#include "../uORB.h" + +/** + * @addtogroup topics + * @{ + */ +struct vehicle_control_debug_s +{ + uint64_t timestamp; /**< in microseconds since system start */ + + float roll_p; /**< roll P control part */ + float roll_i; /**< roll I control part */ + float roll_d; /**< roll D control part */ + + float roll_rate_p; /**< roll rate P control part */ + float roll_rate_i; /**< roll rate I control part */ + float roll_rate_d; /**< roll rate D control part */ + + float pitch_p; /**< pitch P control part */ + float pitch_i; /**< pitch I control part */ + float pitch_d; /**< pitch D control part */ + + float pitch_rate_p; /**< pitch rate P control part */ + float pitch_rate_i; /**< pitch rate I control part */ + float pitch_rate_d; /**< pitch rate D control part */ + + float yaw_p; /**< yaw P control part */ + float yaw_i; /**< yaw I control part */ + float yaw_d; /**< yaw D control part */ + + float yaw_rate_p; /**< yaw rate P control part */ + float yaw_rate_i; /**< yaw rate I control part */ + float yaw_rate_d; /**< yaw rate D control part */ + +}; /**< vehicle_control_debug */ + + /** + * @} + */ + +/* register this as object request broker structure */ +ORB_DECLARE(vehicle_control_debug); + +#endif From bd7f86bb6adf768169fe23b63d627a252586f6b7 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Sun, 16 Jun 2013 14:59:00 +0200 Subject: [PATCH 046/109] Tried to add ctrl debug values to sdlog2 (WIP) --- .../multirotor_rate_control.c | 1 + src/modules/sdlog2/sdlog2.c | 24 +++++++++++-------- src/modules/sdlog2/sdlog2_messages.h | 20 ++++++++-------- 3 files changed, 25 insertions(+), 20 deletions(-) diff --git a/src/modules/multirotor_att_control/multirotor_rate_control.c b/src/modules/multirotor_att_control/multirotor_rate_control.c index 01bf383e27..e8dcbacc7c 100644 --- a/src/modules/multirotor_att_control/multirotor_rate_control.c +++ b/src/modules/multirotor_att_control/multirotor_rate_control.c @@ -248,4 +248,5 @@ void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp, motor_skip_counter++; orb_publish(ORB_ID(vehicle_control_debug), control_debug_pub, &control_debug); + printf("Published control debug\n"); } diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 844e02268b..a86623304b 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -91,6 +91,7 @@ #define LOGBUFFER_WRITE_AND_COUNT(_msg) if (logbuffer_write(&lb, &log_msg, LOG_PACKET_SIZE(_msg))) { \ log_msgs_written++; \ + printf("size: %d\n", LOG_PACKET_SIZE(_msg)); \ } else { \ log_msgs_skipped++; \ /*printf("skip\n");*/ \ @@ -1071,25 +1072,28 @@ int sdlog2_thread_main(int argc, char *argv[]) /* --- CONTROL DEBUG --- */ if (fds[ifds++].revents & POLLIN) { orb_copy(ORB_ID(vehicle_control_debug), subs.control_debug_sub, &buf.control_debug); - - log_msg.body.log_CTRL.roll_p = buf.control_debug.roll_p; - log_msg.body.log_CTRL.roll_i = buf.control_debug.roll_i; - log_msg.body.log_CTRL.roll_d = buf.control_debug.roll_d; + printf("copied control debug\n"); + + // log_msg.body.log_CTRL.roll_p = buf.control_debug.roll_p; + // log_msg.body.log_CTRL.roll_i = buf.control_debug.roll_i; + // log_msg.body.log_CTRL.roll_d = buf.control_debug.roll_d; log_msg.body.log_CTRL.roll_rate_p = buf.control_debug.roll_rate_p; log_msg.body.log_CTRL.roll_rate_i = buf.control_debug.roll_rate_i; log_msg.body.log_CTRL.roll_rate_d = buf.control_debug.roll_rate_d; - log_msg.body.log_CTRL.pitch_p = buf.control_debug.pitch_p; - log_msg.body.log_CTRL.pitch_i = buf.control_debug.pitch_i; - log_msg.body.log_CTRL.pitch_d = buf.control_debug.pitch_d; + // log_msg.body.log_CTRL.pitch_p = buf.control_debug.pitch_p; + // log_msg.body.log_CTRL.pitch_i = buf.control_debug.pitch_i; + // log_msg.body.log_CTRL.pitch_d = buf.control_debug.pitch_d; log_msg.body.log_CTRL.pitch_rate_p = buf.control_debug.pitch_rate_p; log_msg.body.log_CTRL.pitch_rate_i = buf.control_debug.pitch_rate_i; log_msg.body.log_CTRL.pitch_rate_d = buf.control_debug.pitch_rate_d; - log_msg.body.log_CTRL.yaw_p = buf.control_debug.yaw_p; - log_msg.body.log_CTRL.yaw_i = buf.control_debug.yaw_i; - log_msg.body.log_CTRL.yaw_d = buf.control_debug.yaw_d; + // log_msg.body.log_CTRL.yaw_p = buf.control_debug.yaw_p; + // log_msg.body.log_CTRL.yaw_i = buf.control_debug.yaw_i; + // log_msg.body.log_CTRL.yaw_d = buf.control_debug.yaw_d; log_msg.body.log_CTRL.yaw_rate_p = buf.control_debug.yaw_rate_p; log_msg.body.log_CTRL.yaw_rate_i = buf.control_debug.yaw_rate_i; log_msg.body.log_CTRL.yaw_rate_d = buf.control_debug.yaw_rate_d; + + LOGBUFFER_WRITE_AND_COUNT(CTRL); } /* --- FLOW --- */ diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index a80af33eff..8930db33be 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -159,25 +159,25 @@ struct log_STAT_s { /* --- CTRL - CONTROL DEBUG --- */ #define LOG_CTRL_MSG 11 struct log_CTRL_s { - float roll_p; - float roll_i; - float roll_d; + // float roll_p; + // float roll_i; + // float roll_d; float roll_rate_p; float roll_rate_i; float roll_rate_d; - float pitch_p; - float pitch_i; - float pitch_d; + // float pitch_p; + // float pitch_i; + // float pitch_d; float pitch_rate_p; float pitch_rate_i; float pitch_rate_d; - float yaw_p; - float yaw_i; - float yaw_d; + // float yaw_p; + // float yaw_i; + // float yaw_d; float yaw_rate_p; float yaw_rate_i; @@ -210,7 +210,7 @@ static const struct log_format_s log_formats[] = { LOG_FORMAT(GPS, "QBffLLfffff", "GPSTime,FixType,EPH,EPV,Lat,Lon,Alt,VelN,VelE,VelD,Cog"), LOG_FORMAT(ATTC, "ffff", "Roll,Pitch,Yaw,Thrust"), LOG_FORMAT(STAT, "BBBBBfffB", "State,FlightMode,CtlMode,SASMode,Armed,BatV,BatC,BatRem,BatWarn"), - LOG_FORMAT(CTRL, "ffffffffffffffffff", "RollP,RollI,RollD,RollRateP,RollRateI,RollRateD,PitchP,PitchI,PitchD,PitchRateP,PitchRateI,PitchRateD,YawP,YawI,YawD,YawRateP,YawRateI,YawRateD"), + LOG_FORMAT(CTRL, "fffffffff", "RollRP,RollRI,RollRD,PitchRP,PitchRI,PitchRD,YawRP,YawRI,YawRD"), LOG_FORMAT(RC, "ffffffff", "Ch0,Ch1,Ch2,Ch3,Ch4,Ch5,Ch6,Ch7"), LOG_FORMAT(OUT0, "ffffffff", "Out0,Out1,Out2,Out3,Out4,Out5,Out6,Out7"), }; From 303694f5f7b7a03488c6075ff2bd67014badfacb Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Sun, 16 Jun 2013 09:55:28 +0200 Subject: [PATCH 047/109] Fixed pid bug, attitude was not controlled --- .../multirotor_attitude_control.c | 10 ++++++---- src/modules/systemlib/pid/pid.c | 5 ++--- 2 files changed, 8 insertions(+), 7 deletions(-) diff --git a/src/modules/multirotor_att_control/multirotor_attitude_control.c b/src/modules/multirotor_att_control/multirotor_attitude_control.c index d7e1a3adcb..4c8f72b8d5 100644 --- a/src/modules/multirotor_att_control/multirotor_attitude_control.c +++ b/src/modules/multirotor_att_control/multirotor_attitude_control.c @@ -155,8 +155,8 @@ void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_s parameters_init(&h); parameters_update(&h, &p); - pid_init(&pitch_controller, p.att_p, p.att_i, p.att_d, 1000.0f, 1000.0f, 0.0f, PID_MODE_DERIVATIV_SET); - pid_init(&roll_controller, p.att_p, p.att_i, p.att_d, 1000.0f, 1000.0f, 0.0f, PID_MODE_DERIVATIV_SET); + pid_init(&pitch_controller, p.att_p, p.att_i, p.att_d, 1000.0f, 1000.0f, 0.2f, PID_MODE_DERIVATIV_SET); + pid_init(&roll_controller, p.att_p, p.att_i, p.att_d, 1000.0f, 1000.0f, 0.2f, PID_MODE_DERIVATIV_SET); initialized = true; } @@ -167,8 +167,8 @@ void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_s parameters_update(&h, &p); /* apply parameters */ - pid_set_parameters(&pitch_controller, p.att_p, p.att_i, p.att_d, 1000.0f, 1000.0f, 0.0f); - pid_set_parameters(&roll_controller, p.att_p, p.att_i, p.att_d, 1000.0f, 1000.0f, 0.0f); + pid_set_parameters(&pitch_controller, p.att_p, p.att_i, p.att_d, 1000.0f, 1000.0f, 0.2f); + pid_set_parameters(&roll_controller, p.att_p, p.att_i, p.att_d, 1000.0f, 1000.0f, 0.2f); } /* reset integral if on ground */ @@ -188,6 +188,8 @@ void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_s rates_sp->roll = pid_calculate(&roll_controller, att_sp->roll_body , att->roll, att->rollspeed, deltaT); + // printf("rates_sp: %4.4f, att setpoint: %4.4f\n, pitch: %4.4f, pitchspeed: %4.4f, dT: %4.4f", rates_sp->pitch, att_sp->pitch_body, att->pitch, att->pitchspeed, deltaT); + if (control_yaw_position) { /* control yaw rate */ diff --git a/src/modules/systemlib/pid/pid.c b/src/modules/systemlib/pid/pid.c index d0e67d3ea3..0188a51c4f 100644 --- a/src/modules/systemlib/pid/pid.c +++ b/src/modules/systemlib/pid/pid.c @@ -101,7 +101,7 @@ __EXPORT int pid_set_parameters(PID_t *pid, float kp, float ki, float kd, float } if (isfinite(diff_filter_factor)) { - pid->limit = diff_filter_factor; + pid->diff_filter_factor = diff_filter_factor; } else { ret = 1; @@ -179,8 +179,7 @@ __EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, flo } // Calculate the output. Limit output magnitude to pid->limit - float output = (pid->error_previous * pid->kp) + (i * pid->ki) + (d * pid->kd); - + float output = (error * pid->kp) + (i * pid->ki) + (d * pid->kd); if (output > pid->limit) output = pid->limit; if (output < -pid->limit) output = -pid->limit; From c189ac1c85a272142f925339879f22563c092725 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Wed, 13 Mar 2013 18:00:00 -0700 Subject: [PATCH 048/109] Added possibility to log pid control values Conflicts: apps/multirotor_pos_control/multirotor_pos_control.c src/drivers/ardrone_interface/ardrone_interface.c --- .../fixedwing_att_control/fixedwing_att_control_att.c | 4 ++-- .../fixedwing_att_control/fixedwing_att_control_rate.c | 6 +++--- .../fixedwing_pos_control/fixedwing_pos_control_main.c | 8 ++++---- .../multirotor_att_control/multirotor_attitude_control.c | 4 ++-- .../multirotor_att_control/multirotor_rate_control.c | 4 ++-- src/modules/systemlib/pid/pid.c | 8 ++++++-- src/modules/systemlib/pid/pid.h | 2 +- 7 files changed, 20 insertions(+), 16 deletions(-) diff --git a/src/modules/fixedwing_att_control/fixedwing_att_control_att.c b/src/modules/fixedwing_att_control/fixedwing_att_control_att.c index a226757e0c..7009356b78 100644 --- a/src/modules/fixedwing_att_control/fixedwing_att_control_att.c +++ b/src/modules/fixedwing_att_control/fixedwing_att_control_att.c @@ -142,7 +142,7 @@ int fixedwing_att_control_attitude(const struct vehicle_attitude_setpoint_s *att } /* Roll (P) */ - rates_sp->roll = pid_calculate(&roll_controller, att_sp->roll_body, att->roll, 0, 0); + rates_sp->roll = pid_calculate(&roll_controller, att_sp->roll_body, att->roll, 0, 0, NULL, NULL, NULL); /* Pitch (P) */ @@ -152,7 +152,7 @@ int fixedwing_att_control_attitude(const struct vehicle_attitude_setpoint_s *att /* set pitch plus feedforward roll compensation */ rates_sp->pitch = pid_calculate(&pitch_controller, att_sp->pitch_body + pitch_sp_rollcompensation, - att->pitch, 0, 0); + att->pitch, 0, 0, NULL, NULL, NULL); /* Yaw (from coordinated turn constraint or lateral force) */ rates_sp->yaw = (att->rollspeed * rates_sp->roll + 9.81f * sinf(att->roll) * cosf(att->pitch) + speed_body[0] * rates_sp->pitch * sinf(att->roll)) diff --git a/src/modules/fixedwing_att_control/fixedwing_att_control_rate.c b/src/modules/fixedwing_att_control/fixedwing_att_control_rate.c index 79194e5151..991d5ec5d1 100644 --- a/src/modules/fixedwing_att_control/fixedwing_att_control_rate.c +++ b/src/modules/fixedwing_att_control/fixedwing_att_control_rate.c @@ -196,11 +196,11 @@ int fixedwing_att_control_rates(const struct vehicle_rates_setpoint_s *rate_sp, /* roll rate (PI) */ - actuators->control[0] = pid_calculate(&roll_rate_controller, rate_sp->roll, rates[0], 0.0f, deltaT); + actuators->control[0] = pid_calculate(&roll_rate_controller, rate_sp->roll, rates[0], 0.0f, deltaT, NULL, NULL, NULL); /* pitch rate (PI) */ - actuators->control[1] = -pid_calculate(&pitch_rate_controller, rate_sp->pitch, rates[1], 0.0f, deltaT); + actuators->control[1] = -pid_calculate(&pitch_rate_controller, rate_sp->pitch, rates[1], 0.0f, deltaT, NULL, NULL, NULL); /* yaw rate (PI) */ - actuators->control[2] = pid_calculate(&yaw_rate_controller, rate_sp->yaw, rates[2], 0.0f, deltaT); + actuators->control[2] = pid_calculate(&yaw_rate_controller, rate_sp->yaw, rates[2], 0.0f, deltaT, NULL, NULL, NULL); counter++; diff --git a/src/modules/fixedwing_pos_control/fixedwing_pos_control_main.c b/src/modules/fixedwing_pos_control/fixedwing_pos_control_main.c index 48ec3603ff..9c19c6786e 100644 --- a/src/modules/fixedwing_pos_control/fixedwing_pos_control_main.c +++ b/src/modules/fixedwing_pos_control/fixedwing_pos_control_main.c @@ -319,7 +319,7 @@ int fixedwing_pos_control_thread_main(int argc, char *argv[]) // XXX what is xtrack_err.past_end? if (distance_res == OK /*&& !xtrack_err.past_end*/) { - float delta_psi_c = pid_calculate(&offtrack_controller, 0, xtrack_err.distance, 0.0f, 0.0f); //p.xtrack_p * xtrack_err.distance + float delta_psi_c = pid_calculate(&offtrack_controller, 0, xtrack_err.distance, 0.0f, 0.0f, NULL, NULL, NULL); //p.xtrack_p * xtrack_err.distance float psi_c = psi_track + delta_psi_c; float psi_e = psi_c - att.yaw; @@ -336,7 +336,7 @@ int fixedwing_pos_control_thread_main(int argc, char *argv[]) } /* calculate roll setpoint, do this artificially around zero */ - float delta_psi_rate_c = pid_calculate(&heading_controller, psi_e, 0.0f, 0.0f, 0.0f); + float delta_psi_rate_c = pid_calculate(&heading_controller, psi_e, 0.0f, 0.0f, 0.0f, NULL, NULL, NULL); float psi_rate_track = 0; //=V_gr/r_track , this will be needed for implementation of arc following float psi_rate_c = delta_psi_rate_c + psi_rate_track; @@ -359,7 +359,7 @@ int fixedwing_pos_control_thread_main(int argc, char *argv[]) float psi_rate_e_scaled = psi_rate_e * ground_speed / 9.81f; //* V_gr / g - attitude_setpoint.roll_body = pid_calculate(&heading_rate_controller, psi_rate_e_scaled, 0.0f, 0.0f, deltaT); + attitude_setpoint.roll_body = pid_calculate(&heading_rate_controller, psi_rate_e_scaled, 0.0f, 0.0f, deltaT, NULL, NULL, NULL); if (verbose) { printf("psi_rate_c %.4f ", (double)psi_rate_c); @@ -379,7 +379,7 @@ int fixedwing_pos_control_thread_main(int argc, char *argv[]) if (pos_updated) { //TODO: take care of relative vs. ab. altitude - attitude_setpoint.pitch_body = pid_calculate(&altitude_controller, global_setpoint.altitude, global_pos.alt, 0.0f, 0.0f); + attitude_setpoint.pitch_body = pid_calculate(&altitude_controller, global_setpoint.altitude, global_pos.alt, 0.0f, 0.0f, NULL, NULL, NULL); } diff --git a/src/modules/multirotor_att_control/multirotor_attitude_control.c b/src/modules/multirotor_att_control/multirotor_attitude_control.c index 4c8f72b8d5..51faaa8c0d 100644 --- a/src/modules/multirotor_att_control/multirotor_attitude_control.c +++ b/src/modules/multirotor_att_control/multirotor_attitude_control.c @@ -182,11 +182,11 @@ void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_s /* control pitch (forward) output */ rates_sp->pitch = pid_calculate(&pitch_controller, att_sp->pitch_body , - att->pitch, att->pitchspeed, deltaT); + att->pitch, att->pitchspeed, deltaT, NULL, NULL, NULL); /* control roll (left/right) output */ rates_sp->roll = pid_calculate(&roll_controller, att_sp->roll_body , - att->roll, att->rollspeed, deltaT); + att->roll, att->rollspeed, deltaT, NULL, NULL, NULL); // printf("rates_sp: %4.4f, att setpoint: %4.4f\n, pitch: %4.4f, pitchspeed: %4.4f, dT: %4.4f", rates_sp->pitch, att_sp->pitch_body, att->pitch, att->pitchspeed, deltaT); diff --git a/src/modules/multirotor_att_control/multirotor_rate_control.c b/src/modules/multirotor_att_control/multirotor_rate_control.c index 57aea726aa..322c5485ae 100644 --- a/src/modules/multirotor_att_control/multirotor_rate_control.c +++ b/src/modules/multirotor_att_control/multirotor_rate_control.c @@ -197,11 +197,11 @@ void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp, /* control pitch (forward) output */ float pitch_control = pid_calculate(&pitch_rate_controller, rate_sp->pitch , - rates[1], 0.0f, deltaT); + rates[1], 0.0f, deltaT, NULL, NULL, NULL); /* control roll (left/right) output */ float roll_control = pid_calculate(&roll_rate_controller, rate_sp->roll , - rates[0], 0.0f, deltaT); + rates[0], 0.0f, deltaT, NULL, NULL, NULL); /* increase resilience to faulty control inputs */ if (isfinite(pitch_control)) { diff --git a/src/modules/systemlib/pid/pid.c b/src/modules/systemlib/pid/pid.c index 0188a51c4f..3685b2aebb 100644 --- a/src/modules/systemlib/pid/pid.c +++ b/src/modules/systemlib/pid/pid.c @@ -124,7 +124,7 @@ __EXPORT int pid_set_parameters(PID_t *pid, float kp, float ki, float kd, float * @param dt * @return */ -__EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, float dt) +__EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, float dt, float *ctrl_p, float *ctrl_i, float *ctrl_d) { /* error = setpoint - actual_position integral = integral + (error*dt) @@ -152,7 +152,7 @@ __EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, flo if (pid->mode == PID_MODE_DERIVATIV_CALC) { // d = (error_filtered - pid->error_previous_filtered) / dt; - d = pid->error_previous_filtered - error_filtered; + d = error_filtered - pid->error_previous_filtered ; pid->error_previous_filtered = error_filtered; } else if (pid->mode == PID_MODE_DERIVATIV_SET) { d = -val_dot; @@ -194,6 +194,10 @@ __EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, flo // pid->error_previous = error; // } + *ctrl_p = (error * pid->kp); + *ctrl_i = (i * pid->ki); + *ctrl_d = (d * pid->kd); + return pid->last_output; } diff --git a/src/modules/systemlib/pid/pid.h b/src/modules/systemlib/pid/pid.h index f89c36d759..e3d9038cdd 100644 --- a/src/modules/systemlib/pid/pid.h +++ b/src/modules/systemlib/pid/pid.h @@ -72,7 +72,7 @@ typedef struct { __EXPORT void pid_init(PID_t *pid, float kp, float ki, float kd, float intmax, float limit, float diff_filter_factor, uint8_t mode); __EXPORT int pid_set_parameters(PID_t *pid, float kp, float ki, float kd, float intmax, float limit, float diff_filter_factor); //void pid_set(PID_t *pid, float sp); -__EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, float dt); +__EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, float dt, float *ctrl_p, float *ctrl_i, float *ctrl_d); __EXPORT void pid_reset_integral(PID_t *pid); From 2cb928d87c385335de72bb83710583a288d05cc4 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Sun, 16 Jun 2013 12:59:50 +0200 Subject: [PATCH 049/109] Added ctrl debugging values Conflicts: src/modules/sdlog2/sdlog2.c --- .../multirotor_rate_control.c | 16 +++- src/modules/sdlog2/sdlog2.c | 36 +++++++- src/modules/sdlog2/sdlog2_messages.h | 28 +++++- src/modules/uORB/objects_common.cpp | 3 + .../uORB/topics/vehicle_control_debug.h | 87 +++++++++++++++++++ 5 files changed, 165 insertions(+), 5 deletions(-) create mode 100644 src/modules/uORB/topics/vehicle_control_debug.h diff --git a/src/modules/multirotor_att_control/multirotor_rate_control.c b/src/modules/multirotor_att_control/multirotor_rate_control.c index 322c5485ae..01bf383e27 100644 --- a/src/modules/multirotor_att_control/multirotor_rate_control.c +++ b/src/modules/multirotor_att_control/multirotor_rate_control.c @@ -54,6 +54,9 @@ #include #include #include +#include +#include + PARAM_DEFINE_FLOAT(MC_YAWRATE_P, 0.0f); /* same on Flamewheel */ PARAM_DEFINE_FLOAT(MC_YAWRATE_D, 0.0f); @@ -175,6 +178,11 @@ void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp, static bool initialized = false; + static struct vehicle_control_debug_s control_debug; + static orb_advert_t control_debug_pub; + + + /* initialize the pid controllers when the function is called for the first time */ if (initialized == false) { parameters_init(&h); @@ -185,6 +193,8 @@ void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp, 1000.0f, 0.2f, PID_MODE_DERIVATIV_CALC); pid_init(&roll_rate_controller, p.attrate_p, p.attrate_i, p.attrate_d, 1000.0f, 1000.0f, 0.2f, PID_MODE_DERIVATIV_CALC); + + control_debug_pub = orb_advertise(ORB_ID(vehicle_control_debug), &control_debug); } /* load new parameters with lower rate */ @@ -197,11 +207,11 @@ void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp, /* control pitch (forward) output */ float pitch_control = pid_calculate(&pitch_rate_controller, rate_sp->pitch , - rates[1], 0.0f, deltaT, NULL, NULL, NULL); + rates[1], 0.0f, deltaT, &control_debug.pitch_rate_p, &control_debug.pitch_rate_i, &control_debug.pitch_rate_d); /* control roll (left/right) output */ float roll_control = pid_calculate(&roll_rate_controller, rate_sp->roll , - rates[0], 0.0f, deltaT, NULL, NULL, NULL); + rates[0], 0.0f, deltaT, &control_debug.roll_rate_p, &control_debug.roll_rate_i, &control_debug.roll_rate_d); /* increase resilience to faulty control inputs */ if (isfinite(pitch_control)) { @@ -236,4 +246,6 @@ void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp, actuators->control[3] = rate_sp->thrust; motor_skip_counter++; + + orb_publish(ORB_ID(vehicle_control_debug), control_debug_pub, &control_debug); } diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index a14bd6f80e..347d5dd20f 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -73,6 +73,7 @@ #include #include #include +#include #include #include #include @@ -612,7 +613,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 = 15; + const ssize_t fdsc = 16; /* Sanity check variable and index */ ssize_t fdsc_count = 0; /* file descriptors to wait for */ @@ -635,6 +636,7 @@ int sdlog2_thread_main(int argc, char *argv[]) struct vehicle_global_position_s global_pos; struct vehicle_gps_position_s gps_pos; struct vehicle_vicon_position_s vicon_pos; + struct vehicle_control_debug_s control_debug; struct optical_flow_s flow; struct rc_channels_s rc; struct differential_pressure_s diff_pres; @@ -656,6 +658,7 @@ int sdlog2_thread_main(int argc, char *argv[]) int global_pos_sub; int gps_pos_sub; int vicon_pos_sub; + int control_debug_sub; int flow_sub; int rc_sub; } subs; @@ -675,6 +678,7 @@ int sdlog2_thread_main(int argc, char *argv[]) struct log_GPS_s log_GPS; struct log_ATTC_s log_ATTC; struct log_STAT_s log_STAT; + struct log_CTRL_s log_CTRL; struct log_RC_s log_RC; struct log_OUT0_s log_OUT0; } body; @@ -762,6 +766,12 @@ int sdlog2_thread_main(int argc, char *argv[]) fds[fdsc_count].events = POLLIN; fdsc_count++; + /* --- CONTROL DEBUG --- */ + subs.control_debug_sub = orb_subscribe(ORB_ID(vehicle_control_debug)); + fds[fdsc_count].fd = subs.control_debug_sub; + fds[fdsc_count].events = POLLIN; + fdsc_count++; + /* --- OPTICAL FLOW --- */ subs.flow_sub = orb_subscribe(ORB_ID(optical_flow)); fds[fdsc_count].fd = subs.flow_sub; @@ -1031,6 +1041,30 @@ int sdlog2_thread_main(int argc, char *argv[]) // TODO not implemented yet } + /* --- CONTROL DEBUG --- */ + if (fds[ifds++].revents & POLLIN) { + orb_copy(ORB_ID(vehicle_control_debug), subs.control_debug_sub, &buf.control_debug); + + log_msg.body.log_CTRL.roll_p = buf.control_debug.roll_p; + log_msg.body.log_CTRL.roll_i = buf.control_debug.roll_i; + log_msg.body.log_CTRL.roll_d = buf.control_debug.roll_d; + log_msg.body.log_CTRL.roll_rate_p = buf.control_debug.roll_rate_p; + log_msg.body.log_CTRL.roll_rate_i = buf.control_debug.roll_rate_i; + log_msg.body.log_CTRL.roll_rate_d = buf.control_debug.roll_rate_d; + log_msg.body.log_CTRL.pitch_p = buf.control_debug.pitch_p; + log_msg.body.log_CTRL.pitch_i = buf.control_debug.pitch_i; + log_msg.body.log_CTRL.pitch_d = buf.control_debug.pitch_d; + log_msg.body.log_CTRL.pitch_rate_p = buf.control_debug.pitch_rate_p; + log_msg.body.log_CTRL.pitch_rate_i = buf.control_debug.pitch_rate_i; + log_msg.body.log_CTRL.pitch_rate_d = buf.control_debug.pitch_rate_d; + log_msg.body.log_CTRL.yaw_p = buf.control_debug.yaw_p; + log_msg.body.log_CTRL.yaw_i = buf.control_debug.yaw_i; + log_msg.body.log_CTRL.yaw_d = buf.control_debug.yaw_d; + log_msg.body.log_CTRL.yaw_rate_p = buf.control_debug.yaw_rate_p; + log_msg.body.log_CTRL.yaw_rate_i = buf.control_debug.yaw_rate_i; + log_msg.body.log_CTRL.yaw_rate_d = buf.control_debug.yaw_rate_d; + } + /* --- FLOW --- */ if (fds[ifds++].revents & POLLIN) { orb_copy(ORB_ID(optical_flow), subs.flow_sub, &buf.flow); diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index 40763ee1e0..a3d35b5965 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -156,14 +156,37 @@ struct log_STAT_s { unsigned char battery_warning; }; +/* --- CTRL - CONTROL DEBUG --- */ +#define LOG_CTRL_MSG 11 +struct log_CTRL_s { + float roll_p; + float roll_i; + float roll_d; + float roll_rate_p; + float roll_rate_i; + float roll_rate_d; + float pitch_p; + float pitch_i; + float pitch_d; + float pitch_rate_p; + float pitch_rate_i; + float pitch_rate_d; + float yaw_p; + float yaw_i; + float yaw_d; + float yaw_rate_p; + float yaw_rate_i; + float yaw_rate_d; +}; + /* --- RC - RC INPUT CHANNELS --- */ -#define LOG_RC_MSG 11 +#define LOG_RC_MSG 12 struct log_RC_s { float channel[8]; }; /* --- OUT0 - ACTUATOR_0 OUTPUT --- */ -#define LOG_OUT0_MSG 12 +#define LOG_OUT0_MSG 13 struct log_OUT0_s { float output[8]; }; @@ -182,6 +205,7 @@ static const struct log_format_s log_formats[] = { LOG_FORMAT(GPS, "QBffLLfffff", "GPSTime,FixType,EPH,EPV,Lat,Lon,Alt,VelN,VelE,VelD,Cog"), LOG_FORMAT(ATTC, "ffff", "Roll,Pitch,Yaw,Thrust"), LOG_FORMAT(STAT, "BBBBBfffB", "State,FlightMode,CtlMode,SASMode,Armed,BatV,BatC,BatRem,BatWarn"), + LOG_FORMAT(CTRL, "ffffffffffffffffff", "RollP,RollI,RollD,RollRP,RollRI,RollRD,PitchP,PitchI,PitchD,PitchRP,PitchRI,PitchRD,YawP,YawI,YawD,YawRP,YawRI,YawRD"), LOG_FORMAT(RC, "ffffffff", "Ch0,Ch1,Ch2,Ch3,Ch4,Ch5,Ch6,Ch7"), LOG_FORMAT(OUT0, "ffffffff", "Out0,Out1,Out2,Out3,Out4,Out5,Out6,Out7"), }; diff --git a/src/modules/uORB/objects_common.cpp b/src/modules/uORB/objects_common.cpp index e22b58cad6..ddf9272ecb 100644 --- a/src/modules/uORB/objects_common.cpp +++ b/src/modules/uORB/objects_common.cpp @@ -116,6 +116,9 @@ ORB_DEFINE(vehicle_attitude_setpoint, struct vehicle_attitude_setpoint_s); #include "topics/manual_control_setpoint.h" ORB_DEFINE(manual_control_setpoint, struct manual_control_setpoint_s); +#include "topics/vehicle_control_debug.h" +ORB_DEFINE(vehicle_control_debug, struct vehicle_control_debug_s); + #include "topics/offboard_control_setpoint.h" ORB_DEFINE(offboard_control_setpoint, struct offboard_control_setpoint_s); diff --git a/src/modules/uORB/topics/vehicle_control_debug.h b/src/modules/uORB/topics/vehicle_control_debug.h new file mode 100644 index 0000000000..6184284a43 --- /dev/null +++ b/src/modules/uORB/topics/vehicle_control_debug.h @@ -0,0 +1,87 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Author: @author Lorenz Meier + * + * 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 vehicle_control_debug.h + * For debugging purposes to log PID parts of controller + */ + +#ifndef TOPIC_VEHICLE_CONTROL_DEBUG_H_ +#define TOPIC_VEHICLE_CONTROL_DEBUG_H_ + +#include +#include "../uORB.h" + +/** + * @addtogroup topics + * @{ + */ +struct vehicle_control_debug_s +{ + uint64_t timestamp; /**< in microseconds since system start */ + + float roll_p; /**< roll P control part */ + float roll_i; /**< roll I control part */ + float roll_d; /**< roll D control part */ + + float roll_rate_p; /**< roll rate P control part */ + float roll_rate_i; /**< roll rate I control part */ + float roll_rate_d; /**< roll rate D control part */ + + float pitch_p; /**< pitch P control part */ + float pitch_i; /**< pitch I control part */ + float pitch_d; /**< pitch D control part */ + + float pitch_rate_p; /**< pitch rate P control part */ + float pitch_rate_i; /**< pitch rate I control part */ + float pitch_rate_d; /**< pitch rate D control part */ + + float yaw_p; /**< yaw P control part */ + float yaw_i; /**< yaw I control part */ + float yaw_d; /**< yaw D control part */ + + float yaw_rate_p; /**< yaw rate P control part */ + float yaw_rate_i; /**< yaw rate I control part */ + float yaw_rate_d; /**< yaw rate D control part */ + +}; /**< vehicle_control_debug */ + + /** + * @} + */ + +/* register this as object request broker structure */ +ORB_DECLARE(vehicle_control_debug); + +#endif From 6f108e18d21b61ea6a3bf137f01023c594085494 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Sun, 16 Jun 2013 15:32:53 +0200 Subject: [PATCH 050/109] Just include the rate controls for now --- src/modules/sdlog2/sdlog2.c | 9 --------- src/modules/sdlog2/sdlog2_messages.h | 11 +---------- 2 files changed, 1 insertion(+), 19 deletions(-) diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 347d5dd20f..aff0fd4a73 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -1045,21 +1045,12 @@ int sdlog2_thread_main(int argc, char *argv[]) if (fds[ifds++].revents & POLLIN) { orb_copy(ORB_ID(vehicle_control_debug), subs.control_debug_sub, &buf.control_debug); - log_msg.body.log_CTRL.roll_p = buf.control_debug.roll_p; - log_msg.body.log_CTRL.roll_i = buf.control_debug.roll_i; - log_msg.body.log_CTRL.roll_d = buf.control_debug.roll_d; log_msg.body.log_CTRL.roll_rate_p = buf.control_debug.roll_rate_p; log_msg.body.log_CTRL.roll_rate_i = buf.control_debug.roll_rate_i; log_msg.body.log_CTRL.roll_rate_d = buf.control_debug.roll_rate_d; - log_msg.body.log_CTRL.pitch_p = buf.control_debug.pitch_p; - log_msg.body.log_CTRL.pitch_i = buf.control_debug.pitch_i; - log_msg.body.log_CTRL.pitch_d = buf.control_debug.pitch_d; log_msg.body.log_CTRL.pitch_rate_p = buf.control_debug.pitch_rate_p; log_msg.body.log_CTRL.pitch_rate_i = buf.control_debug.pitch_rate_i; log_msg.body.log_CTRL.pitch_rate_d = buf.control_debug.pitch_rate_d; - log_msg.body.log_CTRL.yaw_p = buf.control_debug.yaw_p; - log_msg.body.log_CTRL.yaw_i = buf.control_debug.yaw_i; - log_msg.body.log_CTRL.yaw_d = buf.control_debug.yaw_d; log_msg.body.log_CTRL.yaw_rate_p = buf.control_debug.yaw_rate_p; log_msg.body.log_CTRL.yaw_rate_i = buf.control_debug.yaw_rate_i; log_msg.body.log_CTRL.yaw_rate_d = buf.control_debug.yaw_rate_d; diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index a3d35b5965..8dd36c74d1 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -159,21 +159,12 @@ struct log_STAT_s { /* --- CTRL - CONTROL DEBUG --- */ #define LOG_CTRL_MSG 11 struct log_CTRL_s { - float roll_p; - float roll_i; - float roll_d; float roll_rate_p; float roll_rate_i; float roll_rate_d; - float pitch_p; - float pitch_i; - float pitch_d; float pitch_rate_p; float pitch_rate_i; float pitch_rate_d; - float yaw_p; - float yaw_i; - float yaw_d; float yaw_rate_p; float yaw_rate_i; float yaw_rate_d; @@ -205,7 +196,7 @@ static const struct log_format_s log_formats[] = { LOG_FORMAT(GPS, "QBffLLfffff", "GPSTime,FixType,EPH,EPV,Lat,Lon,Alt,VelN,VelE,VelD,Cog"), LOG_FORMAT(ATTC, "ffff", "Roll,Pitch,Yaw,Thrust"), LOG_FORMAT(STAT, "BBBBBfffB", "State,FlightMode,CtlMode,SASMode,Armed,BatV,BatC,BatRem,BatWarn"), - LOG_FORMAT(CTRL, "ffffffffffffffffff", "RollP,RollI,RollD,RollRP,RollRI,RollRD,PitchP,PitchI,PitchD,PitchRP,PitchRI,PitchRD,YawP,YawI,YawD,YawRP,YawRI,YawRD"), + LOG_FORMAT(CTRL, "fffffffff", "RollRP,RollRI,RollRD,PitchRP,PitchRI,PitchRD,YawRP,YawRI,YawRD"), LOG_FORMAT(RC, "ffffffff", "Ch0,Ch1,Ch2,Ch3,Ch4,Ch5,Ch6,Ch7"), LOG_FORMAT(OUT0, "ffffffff", "Out0,Out1,Out2,Out3,Out4,Out5,Out6,Out7"), }; From 38558f0f168ae0e486df4886533ea522c4ab18ed Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Sun, 16 Jun 2013 16:00:44 +0200 Subject: [PATCH 051/109] Count and write for control debug loging was missing (still not working) --- src/modules/sdlog2/sdlog2.c | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index aff0fd4a73..fed9597757 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -1054,6 +1054,8 @@ int sdlog2_thread_main(int argc, char *argv[]) log_msg.body.log_CTRL.yaw_rate_p = buf.control_debug.yaw_rate_p; log_msg.body.log_CTRL.yaw_rate_i = buf.control_debug.yaw_rate_i; log_msg.body.log_CTRL.yaw_rate_d = buf.control_debug.yaw_rate_d; + + LOGBUFFER_WRITE_AND_COUNT(CTRL); } /* --- FLOW --- */ From 216617431da72ce7b34911f63fa5958302a531e1 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Sun, 16 Jun 2013 16:18:40 +0200 Subject: [PATCH 052/109] Logging of ctrl debug values working now --- src/modules/sdlog2/sdlog2.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index fed9597757..1008c149fe 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -1044,7 +1044,7 @@ int sdlog2_thread_main(int argc, char *argv[]) /* --- CONTROL DEBUG --- */ if (fds[ifds++].revents & POLLIN) { orb_copy(ORB_ID(vehicle_control_debug), subs.control_debug_sub, &buf.control_debug); - + log_msg.msg_type = LOG_CTRL_MSG; log_msg.body.log_CTRL.roll_rate_p = buf.control_debug.roll_rate_p; log_msg.body.log_CTRL.roll_rate_i = buf.control_debug.roll_rate_i; log_msg.body.log_CTRL.roll_rate_d = buf.control_debug.roll_rate_d; From a25d68440d99b4214ae4477d07c23e852458c4d2 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Mon, 17 Jun 2013 21:01:25 +0200 Subject: [PATCH 053/109] Merge with att_fix --- .../attitude_estimator_ekf_params.c | 48 +++++++++---------- 1 file changed, 24 insertions(+), 24 deletions(-) diff --git a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.c b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.c index 7d3812abdb..52dac652be 100755 --- a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.c +++ b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.c @@ -44,42 +44,42 @@ /* Extended Kalman Filter covariances */ /* gyro process noise */ -PARAM_DEFINE_FLOAT(EKF_ATT_V2_Q0, 1e-4f); -PARAM_DEFINE_FLOAT(EKF_ATT_V2_Q1, 0.08f); -PARAM_DEFINE_FLOAT(EKF_ATT_V2_Q2, 0.009f); +PARAM_DEFINE_FLOAT(EKF_ATT_V3_Q0, 1e-4f); +PARAM_DEFINE_FLOAT(EKF_ATT_V3_Q1, 0.08f); +PARAM_DEFINE_FLOAT(EKF_ATT_V3_Q2, 0.009f); /* gyro offsets process noise */ -PARAM_DEFINE_FLOAT(EKF_ATT_V2_Q3, 0.005f); -PARAM_DEFINE_FLOAT(EKF_ATT_V2_Q4, 0.0f); +PARAM_DEFINE_FLOAT(EKF_ATT_V3_Q3, 0.005f); +PARAM_DEFINE_FLOAT(EKF_ATT_V3_Q4, 0.0f); /* gyro measurement noise */ -PARAM_DEFINE_FLOAT(EKF_ATT_V2_R0, 0.0008f); -PARAM_DEFINE_FLOAT(EKF_ATT_V2_R1, 0.8f); -PARAM_DEFINE_FLOAT(EKF_ATT_V2_R2, 1.0f); +PARAM_DEFINE_FLOAT(EKF_ATT_V3_R0, 0.0008f); +PARAM_DEFINE_FLOAT(EKF_ATT_V3_R1, 10000.0f); +PARAM_DEFINE_FLOAT(EKF_ATT_V3_R2, 1.0f); /* accelerometer measurement noise */ -PARAM_DEFINE_FLOAT(EKF_ATT_V2_R3, 0.0f); +PARAM_DEFINE_FLOAT(EKF_ATT_V3_R3, 0.0f); /* offsets in roll, pitch and yaw of sensor plane and body */ -PARAM_DEFINE_FLOAT(ATT_ROLL_OFFS, 0.0f); -PARAM_DEFINE_FLOAT(ATT_PITCH_OFFS, 0.0f); -PARAM_DEFINE_FLOAT(ATT_YAW_OFFS, 0.0f); +PARAM_DEFINE_FLOAT(ATT_ROLL_OFF3, 0.0f); +PARAM_DEFINE_FLOAT(ATT_PITCH_OFF3, 0.0f); +PARAM_DEFINE_FLOAT(ATT_YAW_OFF3, 0.0f); int parameters_init(struct attitude_estimator_ekf_param_handles *h) { /* PID parameters */ - h->q0 = param_find("EKF_ATT_V2_Q0"); - h->q1 = param_find("EKF_ATT_V2_Q1"); - h->q2 = param_find("EKF_ATT_V2_Q2"); - h->q3 = param_find("EKF_ATT_V2_Q3"); - h->q4 = param_find("EKF_ATT_V2_Q4"); + h->q0 = param_find("EKF_ATT_V3_Q0"); + h->q1 = param_find("EKF_ATT_V3_Q1"); + h->q2 = param_find("EKF_ATT_V3_Q2"); + h->q3 = param_find("EKF_ATT_V3_Q3"); + h->q4 = param_find("EKF_ATT_V3_Q4"); - h->r0 = param_find("EKF_ATT_V2_R0"); - h->r1 = param_find("EKF_ATT_V2_R1"); - h->r2 = param_find("EKF_ATT_V2_R2"); - h->r3 = param_find("EKF_ATT_V2_R3"); + h->r0 = param_find("EKF_ATT_V3_R0"); + h->r1 = param_find("EKF_ATT_V3_R1"); + h->r2 = param_find("EKF_ATT_V3_R2"); + h->r3 = param_find("EKF_ATT_V3_R3"); - h->roll_off = param_find("ATT_ROLL_OFFS"); - h->pitch_off = param_find("ATT_PITCH_OFFS"); - h->yaw_off = param_find("ATT_YAW_OFFS"); + h->roll_off = param_find("ATT_ROLL_OFF3"); + h->pitch_off = param_find("ATT_PITCH_OFF3"); + h->yaw_off = param_find("ATT_YAW_OFF3"); return OK; } From 52f8565f0b5326bedb8b8a970c987e6bf10d71f6 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Mon, 17 Jun 2013 21:02:52 +0200 Subject: [PATCH 054/109] Corrected number of ORB structs in sdlog2 --- src/modules/sdlog2/sdlog2.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 885155e0de..9cf6640bf9 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -600,7 +600,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 = 17; + const ssize_t fdsc = 19; /* Sanity check variable and index */ ssize_t fdsc_count = 0; From 2daff9ebbf066c0b14b85364ee056c3d8c7a7734 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Mon, 17 Jun 2013 21:03:55 +0200 Subject: [PATCH 055/109] Checkpoint: Quad is flying after PID lib changes --- .../multirotor_attitude_control.c | 6 +++--- .../multirotor_att_control/multirotor_rate_control.c | 10 ++++------ 2 files changed, 7 insertions(+), 9 deletions(-) diff --git a/src/modules/multirotor_att_control/multirotor_attitude_control.c b/src/modules/multirotor_att_control/multirotor_attitude_control.c index 6da808da49..88cc651488 100644 --- a/src/modules/multirotor_att_control/multirotor_attitude_control.c +++ b/src/modules/multirotor_att_control/multirotor_attitude_control.c @@ -183,12 +183,12 @@ void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_s /* control pitch (forward) output */ rates_sp->pitch = pid_calculate(&pitch_controller, att_sp->pitch_body , - att->pitch, att->pitchspeed, deltaT, &control_debug->roll_p, &control_debug->roll_i, &control_debug->roll_d); + att->pitch, att->pitchspeed, deltaT, &control_debug->pitch_p, &control_debug->pitch_i, &control_debug->pitch_d); /* control roll (left/right) output */ rates_sp->roll = pid_calculate(&roll_controller, att_sp->roll_body , - att->roll, att->rollspeed, deltaT, &control_debug->pitch_p, &control_debug->pitch_i, &control_debug->pitch_d); - + att->roll, att->rollspeed, deltaT, &control_debug->roll_p, &control_debug->roll_i, &control_debug->roll_d); + // printf("rates_sp: %4.4f, att setpoint: %4.4f\n, pitch: %4.4f, pitchspeed: %4.4f, dT: %4.4f", rates_sp->pitch, att_sp->pitch_body, att->pitch, att->pitchspeed, deltaT); if (control_yaw_position) { diff --git a/src/modules/multirotor_att_control/multirotor_rate_control.c b/src/modules/multirotor_att_control/multirotor_rate_control.c index 8cd97d7c16..a39d6f13d5 100644 --- a/src/modules/multirotor_att_control/multirotor_rate_control.c +++ b/src/modules/multirotor_att_control/multirotor_rate_control.c @@ -186,10 +186,8 @@ void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp, parameters_update(&h, &p); initialized = true; - pid_init(&pitch_rate_controller, p.attrate_p, p.attrate_i, p.attrate_d, 1000.0f, - 1000.0f, 0.2f, PID_MODE_DERIVATIV_CALC); - pid_init(&roll_rate_controller, p.attrate_p, p.attrate_i, p.attrate_d, 1000.0f, - 1000.0f, 0.2f, PID_MODE_DERIVATIV_CALC); + pid_init(&pitch_rate_controller, p.attrate_p, p.attrate_i, p.attrate_d, 1.0f, 1.0f, 0.2f, PID_MODE_DERIVATIV_CALC); + pid_init(&roll_rate_controller, p.attrate_p, p.attrate_i, p.attrate_d, 1.0f, 1.0f, 0.2f, PID_MODE_DERIVATIV_CALC); } @@ -197,8 +195,8 @@ void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp, if (motor_skip_counter % 2500 == 0) { /* update parameters from storage */ parameters_update(&h, &p); - pid_set_parameters(&pitch_rate_controller, p.attrate_p, p.attrate_i, p.attrate_d, 1000.0f, 1000.0f, 0.2f); - pid_set_parameters(&roll_rate_controller, p.attrate_p, p.attrate_i, p.attrate_d, 1000.0f, 1000.0f, 0.2f); + pid_set_parameters(&pitch_rate_controller, p.attrate_p, p.attrate_i, p.attrate_d, 1.0f, 1.0f, 0.2f); + pid_set_parameters(&roll_rate_controller, p.attrate_p, p.attrate_i, p.attrate_d, 1.0f, 1.0f, 0.2f); } /* control pitch (forward) output */ From c874f681080e0e68d62a31e88c80240350f8a595 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Mon, 17 Jun 2013 21:03:55 +0200 Subject: [PATCH 056/109] Checkpoint: Quad is flying after PID lib changes Conflicts: src/modules/multirotor_att_control/multirotor_attitude_control.c --- .../multirotor_attitude_control.c | 2 +- .../multirotor_att_control/multirotor_rate_control.c | 10 ++++------ 2 files changed, 5 insertions(+), 7 deletions(-) diff --git a/src/modules/multirotor_att_control/multirotor_attitude_control.c b/src/modules/multirotor_att_control/multirotor_attitude_control.c index 51faaa8c0d..0dad103167 100644 --- a/src/modules/multirotor_att_control/multirotor_attitude_control.c +++ b/src/modules/multirotor_att_control/multirotor_attitude_control.c @@ -187,7 +187,7 @@ void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_s /* control roll (left/right) output */ rates_sp->roll = pid_calculate(&roll_controller, att_sp->roll_body , att->roll, att->rollspeed, deltaT, NULL, NULL, NULL); - + // printf("rates_sp: %4.4f, att setpoint: %4.4f\n, pitch: %4.4f, pitchspeed: %4.4f, dT: %4.4f", rates_sp->pitch, att_sp->pitch_body, att->pitch, att->pitchspeed, deltaT); if (control_yaw_position) { diff --git a/src/modules/multirotor_att_control/multirotor_rate_control.c b/src/modules/multirotor_att_control/multirotor_rate_control.c index 01bf383e27..9135a93514 100644 --- a/src/modules/multirotor_att_control/multirotor_rate_control.c +++ b/src/modules/multirotor_att_control/multirotor_rate_control.c @@ -189,10 +189,8 @@ void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp, parameters_update(&h, &p); initialized = true; - pid_init(&pitch_rate_controller, p.attrate_p, p.attrate_i, p.attrate_d, 1000.0f, - 1000.0f, 0.2f, PID_MODE_DERIVATIV_CALC); - pid_init(&roll_rate_controller, p.attrate_p, p.attrate_i, p.attrate_d, 1000.0f, - 1000.0f, 0.2f, PID_MODE_DERIVATIV_CALC); + pid_init(&pitch_rate_controller, p.attrate_p, p.attrate_i, p.attrate_d, 1.0f, 1.0f, 0.2f, PID_MODE_DERIVATIV_CALC); + pid_init(&roll_rate_controller, p.attrate_p, p.attrate_i, p.attrate_d, 1.0f, 1.0f, 0.2f, PID_MODE_DERIVATIV_CALC); control_debug_pub = orb_advertise(ORB_ID(vehicle_control_debug), &control_debug); } @@ -201,8 +199,8 @@ void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp, if (motor_skip_counter % 2500 == 0) { /* update parameters from storage */ parameters_update(&h, &p); - pid_set_parameters(&pitch_rate_controller, p.attrate_p, p.attrate_i, p.attrate_d, 1000.0f, 1000.0f, 0.2f); - pid_set_parameters(&roll_rate_controller, p.attrate_p, p.attrate_i, p.attrate_d, 1000.0f, 1000.0f, 0.2f); + pid_set_parameters(&pitch_rate_controller, p.attrate_p, p.attrate_i, p.attrate_d, 1.0f, 1.0f, 0.2f); + pid_set_parameters(&roll_rate_controller, p.attrate_p, p.attrate_i, p.attrate_d, 1.0f, 1.0f, 0.2f); } /* control pitch (forward) output */ From cc452834c0dabd2689f5f102ce1cbbe714f056dd Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Tue, 18 Jun 2013 00:30:10 +0200 Subject: [PATCH 057/109] First try to prevent motors from stopping when armed --- src/drivers/px4io/px4io.cpp | 108 ++++++++++++++++++++++++++ src/modules/px4iofirmware/mixer.cpp | 20 ++++- src/modules/px4iofirmware/protocol.h | 6 ++ src/modules/px4iofirmware/px4io.h | 2 + src/modules/px4iofirmware/registers.c | 58 ++++++++++++++ 5 files changed, 192 insertions(+), 2 deletions(-) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index d728b7b762..6a596a9876 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -125,6 +125,16 @@ public: */ int set_failsafe_values(const uint16_t *vals, unsigned len); + /** + * Set the minimum PWM signals when armed + */ + int set_min_values(const uint16_t *vals, unsigned len); + + /** + * Set the maximum PWM signal when armed + */ + int set_max_values(const uint16_t *vals, unsigned len); + /** * Print the current status of IO */ @@ -711,6 +721,34 @@ PX4IO::set_failsafe_values(const uint16_t *vals, unsigned len) return io_reg_set(PX4IO_PAGE_FAILSAFE_PWM, 0, vals, len); } +int +PX4IO::set_min_values(const uint16_t *vals, unsigned len) +{ + uint16_t regs[_max_actuators]; + + if (len > _max_actuators) + /* fail with error */ + return E2BIG; + + /* copy values to registers in IO */ + return io_reg_set(PX4IO_PAGE_CONTROL_MIN_PWM, 0, vals, len); +} + +int +PX4IO::set_max_values(const uint16_t *vals, unsigned len) +{ + uint16_t regs[_max_actuators]; + + if (len > _max_actuators) + /* fail with error */ + return E2BIG; + + /* copy values to registers in IO */ + return io_reg_set(PX4IO_PAGE_CONTROL_MAX_PWM, 0, vals, len); +} + + + int PX4IO::io_set_arming_state() { @@ -1792,6 +1830,76 @@ px4io_main(int argc, char *argv[]) exit(0); } + if (!strcmp(argv[1], "min")) { + + if (argc < 3) { + errx(1, "min command needs at least one channel value (PWM)"); + } + + if (g_dev != nullptr) { + + /* set values for first 8 channels, fill unassigned channels with 900. */ + uint16_t min[8]; + + for (int i = 0; i < sizeof(min) / sizeof(min[0]); i++) + { + /* set channel to commanline argument or to 900 for non-provided channels */ + if (argc > i + 2) { + min[i] = atoi(argv[i+2]); + if (min[i] < 900 || min[i] > 1200) { + errx(1, "value out of range of 900 < value < 1200. Aborting."); + } + } else { + /* a zero value will the default */ + min[i] = 900; + } + } + + int ret = g_dev->set_min_values(min, sizeof(min) / sizeof(min[0])); + + if (ret != OK) + errx(ret, "failed setting min values"); + } else { + errx(1, "not loaded"); + } + exit(0); + } + + if (!strcmp(argv[1], "max")) { + + if (argc < 3) { + errx(1, "max command needs at least one channel value (PWM)"); + } + + if (g_dev != nullptr) { + + /* set values for first 8 channels, fill unassigned channels with 2100. */ + uint16_t max[8]; + + for (int i = 0; i < sizeof(max) / sizeof(max[0]); i++) + { + /* set channel to commanline argument or to 2100 for non-provided channels */ + if (argc > i + 2) { + max[i] = atoi(argv[i+2]); + if (max[i] < 1800 || max[i] > 2100) { + errx(1, "value out of range of 1800 < value < 2100. Aborting."); + } + } else { + /* a zero value will the default */ + max[i] = 2100; + } + } + + int ret = g_dev->set_max_values(max, sizeof(max) / sizeof(max[0])); + + if (ret != OK) + errx(ret, "failed setting max values"); + } else { + errx(1, "not loaded"); + } + exit(0); + } + if (!strcmp(argv[1], "recovery")) { if (g_dev != nullptr) { diff --git a/src/modules/px4iofirmware/mixer.cpp b/src/modules/px4iofirmware/mixer.cpp index a2193b526b..e257e7cb85 100644 --- a/src/modules/px4iofirmware/mixer.cpp +++ b/src/modules/px4iofirmware/mixer.cpp @@ -59,6 +59,11 @@ extern "C" { */ #define FMU_INPUT_DROP_LIMIT_US 200000 +/* + * Time that the ESCs need to initialize + */ + #define ESC_INIT_TIME_US 500000 + /* XXX need to move the RC_CHANNEL_FUNCTION out of rc_channels.h and into systemlib */ #define ROLL 0 #define PITCH 1 @@ -69,6 +74,8 @@ extern "C" { /* current servo arm/disarm state */ static bool mixer_servos_armed = false; +static uint64_t time_armed; + /* selected control values and count for mixing */ enum mixer_source { MIX_NONE, @@ -176,8 +183,16 @@ mixer_tick(void) /* save actuator values for FMU readback */ r_page_actuators[i] = FLOAT_TO_REG(outputs[i]); - /* scale to servo output */ - r_page_servos[i] = (outputs[i] * 600.0f) + 1500; + /* scale to control range after init time */ + if (mixer_servos_armed && (hrt_elapsed_time(&time_armed) > ESC_INIT_TIME_US)) { + r_page_servos[i] = (outputs[i] + * (r_page_servo_control_max[i] - r_page_servo_control_min[i])/2 + + (r_page_servo_control_max[i] + r_page_servo_control_min[i])/2); + + /* but use init range from 900 to 2100 right after arming */ + } else { + r_page_servos[i] = (outputs[i] * 600 + 1500); + } } for (unsigned i = mixed; i < IO_SERVO_COUNT; i++) @@ -206,6 +221,7 @@ mixer_tick(void) /* need to arm, but not armed */ up_pwm_servo_arm(true); mixer_servos_armed = true; + time_armed = hrt_absolute_time(); } else if (!should_arm && mixer_servos_armed) { /* armed but need to disarm */ diff --git a/src/modules/px4iofirmware/protocol.h b/src/modules/px4iofirmware/protocol.h index 674f9dddd6..1c9715451b 100644 --- a/src/modules/px4iofirmware/protocol.h +++ b/src/modules/px4iofirmware/protocol.h @@ -183,6 +183,12 @@ /* PWM failsafe values - zero disables the output */ #define PX4IO_PAGE_FAILSAFE_PWM 105 /* 0..CONFIG_ACTUATOR_COUNT-1 */ +/* PWM minimum values for certain ESCs */ +#define PX4IO_PAGE_CONTROL_MIN_PWM 106 /* 0..CONFIG_ACTUATOR_COUNT-1 */ + + /* PWM maximum values for certain ESCs */ +#define PX4IO_PAGE_CONTROL_MAX_PWM 107 /* 0..CONFIG_ACTUATOR_COUNT-1 */ + /** * As-needed mixer data upload. * diff --git a/src/modules/px4iofirmware/px4io.h b/src/modules/px4iofirmware/px4io.h index 272cdb7bf4..3e4027c9bd 100644 --- a/src/modules/px4iofirmware/px4io.h +++ b/src/modules/px4iofirmware/px4io.h @@ -77,6 +77,8 @@ extern volatile uint16_t r_page_setup[]; /* PX4IO_PAGE_SETUP */ extern volatile uint16_t r_page_controls[]; /* PX4IO_PAGE_CONTROLS */ extern uint16_t r_page_rc_input_config[]; /* PX4IO_PAGE_RC_INPUT_CONFIG */ extern uint16_t r_page_servo_failsafe[]; /* PX4IO_PAGE_FAILSAFE_PWM */ +extern uint16_t r_page_servo_control_min[]; /* PX4IO_PAGE_CONTROL_MIN_PWM */ +extern uint16_t r_page_servo_control_max[]; /* PX4IO_PAGE_CONTROL_MAX_PWM */ /* * Register aliases. diff --git a/src/modules/px4iofirmware/registers.c b/src/modules/px4iofirmware/registers.c index df7d6dcd35..1fcfb2906e 100644 --- a/src/modules/px4iofirmware/registers.c +++ b/src/modules/px4iofirmware/registers.c @@ -184,6 +184,22 @@ uint16_t r_page_rc_input_config[MAX_CONTROL_CHANNELS * PX4IO_P_RC_CONFIG_STRIDE */ uint16_t r_page_servo_failsafe[IO_SERVO_COUNT] = { 0 }; +/** + * PAGE 106 + * + * minimum PWM values when armed + * + */ +uint16_t r_page_servo_control_min[IO_SERVO_COUNT] = { 900, 900, 900, 900, 900, 900, 900, 900 }; + +/** + * PAGE 107 + * + * maximum PWM values when armed + * + */ +uint16_t r_page_servo_control_max[IO_SERVO_COUNT] = { 2100, 2100, 2100, 2100, 2100, 2100, 2100, 2100 }; + void registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num_values) { @@ -247,6 +263,42 @@ registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num } break; + case PX4IO_PAGE_CONTROL_MIN_PWM: + + /* copy channel data */ + while ((offset < IO_SERVO_COUNT) && (num_values > 0)) { + + if (*values > 1200) + r_page_servo_control_min[offset] = 1200; + else if (*values < 900) + r_page_servo_control_min[offset] = 900; + else + r_page_servo_control_min[offset] = *values; + + offset++; + num_values--; + values++; + } + break; + + case PX4IO_PAGE_CONTROL_MAX_PWM: + + /* copy channel data */ + while ((offset < IO_SERVO_COUNT) && (num_values > 0)) { + + if (*values > 1200) + r_page_servo_control_max[offset] = 1200; + else if (*values < 900) + r_page_servo_control_max[offset] = 900; + else + r_page_servo_control_max[offset] = *values; + + offset++; + num_values--; + values++; + } + break; + /* handle text going to the mixer parser */ case PX4IO_PAGE_MIXERLOAD: mixer_handle_text(values, num_values * sizeof(*values)); @@ -583,6 +635,12 @@ registers_get(uint8_t page, uint8_t offset, uint16_t **values, unsigned *num_val case PX4IO_PAGE_FAILSAFE_PWM: SELECT_PAGE(r_page_servo_failsafe); break; + case PX4IO_PAGE_CONTROL_MIN_PWM: + SELECT_PAGE(r_page_servo_control_min); + break; + case PX4IO_PAGE_CONTROL_MAX_PWM: + SELECT_PAGE(r_page_servo_control_max); + break; default: return -1; From b5f4f1ee808c176c5dc0705b76584b438f151650 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Tue, 18 Jun 2013 10:00:08 +0200 Subject: [PATCH 058/109] Adressed performance concern and fixed a copy paste bug --- src/drivers/px4io/px4io.cpp | 4 ++-- src/modules/px4iofirmware/mixer.cpp | 11 +++++++++-- src/modules/px4iofirmware/registers.c | 18 +++++++++++++----- 3 files changed, 24 insertions(+), 9 deletions(-) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 6a596a9876..be4dd5d197 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -1851,7 +1851,7 @@ px4io_main(int argc, char *argv[]) } } else { /* a zero value will the default */ - min[i] = 900; + min[i] = 0; } } @@ -1886,7 +1886,7 @@ px4io_main(int argc, char *argv[]) } } else { /* a zero value will the default */ - max[i] = 2100; + max[i] = 0; } } diff --git a/src/modules/px4iofirmware/mixer.cpp b/src/modules/px4iofirmware/mixer.cpp index e257e7cb85..62a6ebf133 100644 --- a/src/modules/px4iofirmware/mixer.cpp +++ b/src/modules/px4iofirmware/mixer.cpp @@ -62,7 +62,7 @@ extern "C" { /* * Time that the ESCs need to initialize */ - #define ESC_INIT_TIME_US 500000 + #define ESC_INIT_TIME_US 2000000 /* XXX need to move the RC_CHANNEL_FUNCTION out of rc_channels.h and into systemlib */ #define ROLL 0 @@ -75,6 +75,7 @@ extern "C" { static bool mixer_servos_armed = false; static uint64_t time_armed; +static bool init_complete = false; /* selected control values and count for mixing */ enum mixer_source { @@ -177,6 +178,10 @@ mixer_tick(void) /* mix */ mixed = mixer_group.mix(&outputs[0], IO_SERVO_COUNT); + if (!init_complete && mixer_servos_armed && (hrt_elapsed_time(&time_armed) > ESC_INIT_TIME_US)) { + init_complete = true; + } + /* scale to PWM and update the servo outputs as required */ for (unsigned i = 0; i < mixed; i++) { @@ -184,7 +189,7 @@ mixer_tick(void) r_page_actuators[i] = FLOAT_TO_REG(outputs[i]); /* scale to control range after init time */ - if (mixer_servos_armed && (hrt_elapsed_time(&time_armed) > ESC_INIT_TIME_US)) { + if (init_complete) { r_page_servos[i] = (outputs[i] * (r_page_servo_control_max[i] - r_page_servo_control_min[i])/2 + (r_page_servo_control_max[i] + r_page_servo_control_min[i])/2); @@ -222,11 +227,13 @@ mixer_tick(void) up_pwm_servo_arm(true); mixer_servos_armed = true; time_armed = hrt_absolute_time(); + init_complete = false; } else if (!should_arm && mixer_servos_armed) { /* armed but need to disarm */ up_pwm_servo_arm(false); mixer_servos_armed = false; + init_complete = false; } if (mixer_servos_armed) { diff --git a/src/modules/px4iofirmware/registers.c b/src/modules/px4iofirmware/registers.c index 1fcfb2906e..bc1c839016 100644 --- a/src/modules/px4iofirmware/registers.c +++ b/src/modules/px4iofirmware/registers.c @@ -268,7 +268,11 @@ registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num /* copy channel data */ while ((offset < IO_SERVO_COUNT) && (num_values > 0)) { - if (*values > 1200) + if (*values == 0) + /* set to default */ + r_page_servo_control_min[offset] = 900; + + else if (*values > 1200) r_page_servo_control_min[offset] = 1200; else if (*values < 900) r_page_servo_control_min[offset] = 900; @@ -286,10 +290,14 @@ registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num /* copy channel data */ while ((offset < IO_SERVO_COUNT) && (num_values > 0)) { - if (*values > 1200) - r_page_servo_control_max[offset] = 1200; - else if (*values < 900) - r_page_servo_control_max[offset] = 900; + if (*values == 0) + /* set to default */ + r_page_servo_control_max[offset] = 2100; + + else if (*values > 2100) + r_page_servo_control_max[offset] = 2100; + else if (*values < 1800) + r_page_servo_control_max[offset] = 1800; else r_page_servo_control_max[offset] = *values; From f3ce61d7405a7edc1e5bb2aadf2ccec5bed90feb Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Wed, 19 Jun 2013 09:35:19 +0200 Subject: [PATCH 059/109] Forgot to remove some debug stuff --- src/modules/px4iofirmware/mixer.cpp | 6 +----- 1 file changed, 1 insertion(+), 5 deletions(-) diff --git a/src/modules/px4iofirmware/mixer.cpp b/src/modules/px4iofirmware/mixer.cpp index e3ec2fa76f..6752a8128d 100644 --- a/src/modules/px4iofirmware/mixer.cpp +++ b/src/modules/px4iofirmware/mixer.cpp @@ -179,14 +179,12 @@ mixer_tick(void) if (mixer_servos_armed && !esc_init_done && !esc_init_active) { esc_init_time = hrt_absolute_time(); esc_init_active = true; - isr_debug(1, "start counting now"); } /* after waiting long enough for the ESC initialization, we can disable the ESC initialization phase */ if (!esc_init_done && esc_init_active && mixer_servos_armed && (hrt_elapsed_time(&esc_init_time) > ESC_INIT_TIME_US)) { esc_init_active = false; esc_init_done = true; - isr_debug(1, "time is up"); } /* mix */ @@ -251,9 +249,7 @@ mixer_tick(void) r_status_flags &= ~(PX4IO_P_STATUS_FLAGS_OUTPUTS_ARMED); isr_debug(5, "> disarmed"); - esc_init_active = false; - isr_debug(1, "disarming, and init aborted"); - } + esc_init_active = false; } if (mixer_servos_armed) { /* update the servo outputs. */ From e2ff60b0a6dbcd714d57e781d9fe174b110a6237 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Wed, 19 Jun 2013 09:35:59 +0200 Subject: [PATCH 060/109] Use rollacc and pitchacc from the estimator instead of differentiating in the controller --- .../multirotor_att_control_main.c | 17 ++++++++++++----- .../multirotor_rate_control.c | 12 ++++++------ .../multirotor_rate_control.h | 2 +- 3 files changed, 19 insertions(+), 12 deletions(-) diff --git a/src/modules/multirotor_att_control/multirotor_att_control_main.c b/src/modules/multirotor_att_control/multirotor_att_control_main.c index c3b2f5d2a2..dd38242d32 100644 --- a/src/modules/multirotor_att_control/multirotor_att_control_main.c +++ b/src/modules/multirotor_att_control/multirotor_att_control_main.c @@ -394,7 +394,8 @@ mc_thread_main(int argc, char *argv[]) /* measure in what intervals the controller runs */ perf_count(mc_interval_perf); - float gyro[3]; + float rates[3]; + float rates_acc[3]; /* get current rate setpoint */ bool rates_sp_valid = false; @@ -405,11 +406,17 @@ mc_thread_main(int argc, char *argv[]) } /* apply controller */ - gyro[0] = att.rollspeed; - gyro[1] = att.pitchspeed; - gyro[2] = att.yawspeed; + rates[0] = att.rollspeed; + rates[1] = att.pitchspeed; + rates[2] = att.yawspeed; - multirotor_control_rates(&rates_sp, gyro, &actuators, &control_debug_pub, &control_debug); + rates_acc[0] = att.rollacc; + rates_acc[1] = att.pitchacc; + rates_acc[2] = att.yawacc; + + + + multirotor_control_rates(&rates_sp, rates, rates_acc, &actuators, &control_debug_pub, &control_debug); orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators); orb_publish(ORB_ID(vehicle_control_debug), control_debug_pub, &control_debug); diff --git a/src/modules/multirotor_att_control/multirotor_rate_control.c b/src/modules/multirotor_att_control/multirotor_rate_control.c index a39d6f13d5..e37ede3e09 100644 --- a/src/modules/multirotor_att_control/multirotor_rate_control.c +++ b/src/modules/multirotor_att_control/multirotor_rate_control.c @@ -151,7 +151,7 @@ static int parameters_update(const struct mc_rate_control_param_handles *h, stru } void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp, - const float rates[], struct actuator_controls_s *actuators, + const float rates[], const float rates_acc[], struct actuator_controls_s *actuators, const orb_advert_t *control_debug_pub, struct vehicle_control_debug_s *control_debug) { static uint64_t last_run = 0; @@ -186,8 +186,8 @@ void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp, parameters_update(&h, &p); initialized = true; - pid_init(&pitch_rate_controller, p.attrate_p, p.attrate_i, p.attrate_d, 1.0f, 1.0f, 0.2f, PID_MODE_DERIVATIV_CALC); - pid_init(&roll_rate_controller, p.attrate_p, p.attrate_i, p.attrate_d, 1.0f, 1.0f, 0.2f, PID_MODE_DERIVATIV_CALC); + pid_init(&pitch_rate_controller, p.attrate_p, p.attrate_i, p.attrate_d, 1.0f, 1.0f, 0.2f, PID_MODE_DERIVATIV_SET); + pid_init(&roll_rate_controller, p.attrate_p, p.attrate_i, p.attrate_d, 1.0f, 1.0f, 0.2f, PID_MODE_DERIVATIV_SET); } @@ -201,11 +201,11 @@ void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp, /* control pitch (forward) output */ float pitch_control = pid_calculate(&pitch_rate_controller, rate_sp->pitch , - rates[1], 0.0f, deltaT, &control_debug->pitch_rate_p, &control_debug->pitch_rate_i, &control_debug->pitch_rate_d); + rates[1], rates_acc[1], deltaT, &control_debug->pitch_rate_p, &control_debug->pitch_rate_i, &control_debug->pitch_rate_d); /* control roll (left/right) output */ float roll_control = pid_calculate(&roll_rate_controller, rate_sp->roll , - rates[0], 0.0f, deltaT, &control_debug->roll_rate_p, &control_debug->roll_rate_i, &control_debug->roll_rate_d); + rates[0], rates_acc[0], deltaT, &control_debug->roll_rate_p, &control_debug->roll_rate_i, &control_debug->roll_rate_d); /* increase resilience to faulty control inputs */ if (isfinite(pitch_control)) { @@ -225,7 +225,7 @@ void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp, warnx("rej. NaN ctrl roll"); } - /* control yaw rate */ + /* control yaw rate */ //XXX use library here and use rates_acc[2] float yaw_rate_control = p.yawrate_p * (rate_sp->yaw - rates[2]); /* increase resilience to faulty control inputs */ diff --git a/src/modules/multirotor_att_control/multirotor_rate_control.h b/src/modules/multirotor_att_control/multirotor_rate_control.h index 465549540c..fc741c3786 100644 --- a/src/modules/multirotor_att_control/multirotor_rate_control.h +++ b/src/modules/multirotor_att_control/multirotor_rate_control.h @@ -52,7 +52,7 @@ #include void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp, - const float rates[], struct actuator_controls_s *actuators, + const float rates[], const float rates_acc[], struct actuator_controls_s *actuators, const orb_advert_t *control_debug_pub, struct vehicle_control_debug_s *control_debug); #endif /* MULTIROTOR_RATE_CONTROL_H_ */ From 8d6cc86b4f37773c9c4db77b9666fa2a075c1871 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Wed, 19 Jun 2013 10:01:16 +0200 Subject: [PATCH 061/109] Cherry-picked commit e2ff60b0a6dbcd714d57e781d9fe174b110a6237: use rateacc values --- .../multirotor_att_control_main.c | 26 +++++++++++++++---- .../multirotor_rate_control.c | 22 ++++++---------- .../multirotor_rate_control.h | 4 ++- 3 files changed, 32 insertions(+), 20 deletions(-) diff --git a/src/modules/multirotor_att_control/multirotor_att_control_main.c b/src/modules/multirotor_att_control/multirotor_att_control_main.c index d94c0a69c9..38775ed1f5 100644 --- a/src/modules/multirotor_att_control/multirotor_att_control_main.c +++ b/src/modules/multirotor_att_control/multirotor_att_control_main.c @@ -63,6 +63,7 @@ #include #include #include +#include #include #include #include @@ -83,6 +84,7 @@ static int mc_task; static bool motor_test_mode = false; static orb_advert_t actuator_pub; +static orb_advert_t control_debug_pub; static struct vehicle_status_s state; @@ -107,6 +109,9 @@ mc_thread_main(int argc, char *argv[]) struct actuator_controls_s actuators; memset(&actuators, 0, sizeof(actuators)); + struct vehicle_control_debug_s control_debug; + memset(&control_debug, 0, sizeof(control_debug)); + /* subscribe to attitude, motor setpoints and system state */ int att_sub = orb_subscribe(ORB_ID(vehicle_attitude)); int param_sub = orb_subscribe(ORB_ID(parameter_update)); @@ -133,6 +138,8 @@ mc_thread_main(int argc, char *argv[]) for (unsigned i = 0; i < NUM_ACTUATOR_CONTROLS; i++) { actuators.control[i] = 0.0f; } + + control_debug_pub = orb_advertise(ORB_ID(vehicle_control_debug), &control_debug); actuator_pub = orb_advertise(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, &actuators); orb_advert_t att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &att_sp); @@ -372,7 +379,8 @@ mc_thread_main(int argc, char *argv[]) /* measure in what intervals the controller runs */ perf_count(mc_interval_perf); - float gyro[3]; + float rates[3]; + float rates_acc[3]; /* get current rate setpoint */ bool rates_sp_valid = false; @@ -383,13 +391,21 @@ mc_thread_main(int argc, char *argv[]) } /* apply controller */ - gyro[0] = att.rollspeed; - gyro[1] = att.pitchspeed; - gyro[2] = att.yawspeed; + rates[0] = att.rollspeed; + rates[1] = att.pitchspeed; + rates[2] = att.yawspeed; - multirotor_control_rates(&rates_sp, gyro, &actuators); + rates_acc[0] = att.rollacc; + rates_acc[1] = att.pitchacc; + rates_acc[2] = att.yawacc; + + + + multirotor_control_rates(&rates_sp, rates, rates_acc, &actuators, &control_debug_pub, &control_debug); orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators); + orb_publish(ORB_ID(vehicle_control_debug), control_debug_pub, &control_debug); + /* update state */ flag_control_attitude_enabled = state.flag_control_attitude_enabled; flag_control_manual_enabled = state.flag_control_manual_enabled; diff --git a/src/modules/multirotor_att_control/multirotor_rate_control.c b/src/modules/multirotor_att_control/multirotor_rate_control.c index 9135a93514..e37ede3e09 100644 --- a/src/modules/multirotor_att_control/multirotor_rate_control.c +++ b/src/modules/multirotor_att_control/multirotor_rate_control.c @@ -55,7 +55,7 @@ #include #include #include -#include + PARAM_DEFINE_FLOAT(MC_YAWRATE_P, 0.0f); /* same on Flamewheel */ @@ -151,7 +151,8 @@ static int parameters_update(const struct mc_rate_control_param_handles *h, stru } void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp, - const float rates[], struct actuator_controls_s *actuators) + const float rates[], const float rates_acc[], struct actuator_controls_s *actuators, + const orb_advert_t *control_debug_pub, struct vehicle_control_debug_s *control_debug) { static uint64_t last_run = 0; const float deltaT = (hrt_absolute_time() - last_run) / 1000000.0f; @@ -178,10 +179,6 @@ void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp, static bool initialized = false; - static struct vehicle_control_debug_s control_debug; - static orb_advert_t control_debug_pub; - - /* initialize the pid controllers when the function is called for the first time */ if (initialized == false) { @@ -189,10 +186,9 @@ void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp, parameters_update(&h, &p); initialized = true; - pid_init(&pitch_rate_controller, p.attrate_p, p.attrate_i, p.attrate_d, 1.0f, 1.0f, 0.2f, PID_MODE_DERIVATIV_CALC); - pid_init(&roll_rate_controller, p.attrate_p, p.attrate_i, p.attrate_d, 1.0f, 1.0f, 0.2f, PID_MODE_DERIVATIV_CALC); + pid_init(&pitch_rate_controller, p.attrate_p, p.attrate_i, p.attrate_d, 1.0f, 1.0f, 0.2f, PID_MODE_DERIVATIV_SET); + pid_init(&roll_rate_controller, p.attrate_p, p.attrate_i, p.attrate_d, 1.0f, 1.0f, 0.2f, PID_MODE_DERIVATIV_SET); - control_debug_pub = orb_advertise(ORB_ID(vehicle_control_debug), &control_debug); } /* load new parameters with lower rate */ @@ -205,11 +201,11 @@ void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp, /* control pitch (forward) output */ float pitch_control = pid_calculate(&pitch_rate_controller, rate_sp->pitch , - rates[1], 0.0f, deltaT, &control_debug.pitch_rate_p, &control_debug.pitch_rate_i, &control_debug.pitch_rate_d); + rates[1], rates_acc[1], deltaT, &control_debug->pitch_rate_p, &control_debug->pitch_rate_i, &control_debug->pitch_rate_d); /* control roll (left/right) output */ float roll_control = pid_calculate(&roll_rate_controller, rate_sp->roll , - rates[0], 0.0f, deltaT, &control_debug.roll_rate_p, &control_debug.roll_rate_i, &control_debug.roll_rate_d); + rates[0], rates_acc[0], deltaT, &control_debug->roll_rate_p, &control_debug->roll_rate_i, &control_debug->roll_rate_d); /* increase resilience to faulty control inputs */ if (isfinite(pitch_control)) { @@ -229,7 +225,7 @@ void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp, warnx("rej. NaN ctrl roll"); } - /* control yaw rate */ + /* control yaw rate */ //XXX use library here and use rates_acc[2] float yaw_rate_control = p.yawrate_p * (rate_sp->yaw - rates[2]); /* increase resilience to faulty control inputs */ @@ -244,6 +240,4 @@ void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp, actuators->control[3] = rate_sp->thrust; motor_skip_counter++; - - orb_publish(ORB_ID(vehicle_control_debug), control_debug_pub, &control_debug); } diff --git a/src/modules/multirotor_att_control/multirotor_rate_control.h b/src/modules/multirotor_att_control/multirotor_rate_control.h index 03dec317a3..fc741c3786 100644 --- a/src/modules/multirotor_att_control/multirotor_rate_control.h +++ b/src/modules/multirotor_att_control/multirotor_rate_control.h @@ -49,8 +49,10 @@ #include #include #include +#include void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp, - const float rates[], struct actuator_controls_s *actuators); + const float rates[], const float rates_acc[], struct actuator_controls_s *actuators, + const orb_advert_t *control_debug_pub, struct vehicle_control_debug_s *control_debug); #endif /* MULTIROTOR_RATE_CONTROL_H_ */ From 53eb76f4d558d345cc85b8f30e232b9bca2f0e51 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Wed, 19 Jun 2013 10:34:41 +0200 Subject: [PATCH 062/109] Fixed numeration that was introduced through merge, I should add new log messages to the end --- src/modules/sdlog2/sdlog2_messages.h | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index a83a725148..b6537b2cdd 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -190,14 +190,14 @@ struct log_OUT0_s { }; /* --- AIRS - AIRSPEED --- */ -#define LOG_AIRS_MSG 13 +#define LOG_AIRS_MSG 14 struct log_AIRS_s { float indicated_airspeed; float true_airspeed; }; /* --- ARSP - ATTITUDE RATE SET POINT --- */ -#define LOG_ARSP_MSG 14 +#define LOG_ARSP_MSG 15 struct log_ARSP_s { float roll_rate_sp; float pitch_rate_sp; From d6c15b16792f3f087a0d934677615949c9e12688 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Wed, 19 Jun 2013 10:34:41 +0200 Subject: [PATCH 063/109] Fixed numeration that was introduced through merge, I should add new log messages to the end --- src/modules/sdlog2/sdlog2_messages.h | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index bd69445b54..2b61378ed7 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -187,14 +187,14 @@ struct log_OUT0_s { }; /* --- AIRS - AIRSPEED --- */ -#define LOG_AIRS_MSG 13 +#define LOG_AIRS_MSG 14 struct log_AIRS_s { float indicated_airspeed; float true_airspeed; }; /* --- ARSP - ATTITUDE RATE SET POINT --- */ -#define LOG_ARSP_MSG 14 +#define LOG_ARSP_MSG 15 struct log_ARSP_s { float roll_rate_sp; float pitch_rate_sp; From c1049483a82857bebb012607578add9492446321 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Wed, 19 Jun 2013 12:01:22 +0200 Subject: [PATCH 064/109] Added integral reset for rate controller --- .../multirotor_att_control/multirotor_rate_control.c | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/src/modules/multirotor_att_control/multirotor_rate_control.c b/src/modules/multirotor_att_control/multirotor_rate_control.c index e37ede3e09..a0266e1b3f 100644 --- a/src/modules/multirotor_att_control/multirotor_rate_control.c +++ b/src/modules/multirotor_att_control/multirotor_rate_control.c @@ -199,6 +199,12 @@ void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp, pid_set_parameters(&roll_rate_controller, p.attrate_p, p.attrate_i, p.attrate_d, 1.0f, 1.0f, 0.2f); } + /* reset integral if on ground */ + if (rate_sp->thrust < 0.01f) { + pid_reset_integral(&pitch_rate_controller); + pid_reset_integral(&roll_rate_controller); + } + /* control pitch (forward) output */ float pitch_control = pid_calculate(&pitch_rate_controller, rate_sp->pitch , rates[1], rates_acc[1], deltaT, &control_debug->pitch_rate_p, &control_debug->pitch_rate_i, &control_debug->pitch_rate_d); From ece93ab62834be7f46501b1d31733cf58b5b1188 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Wed, 19 Jun 2013 12:01:22 +0200 Subject: [PATCH 065/109] Added integral reset for rate controller --- .../multirotor_att_control/multirotor_rate_control.c | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/src/modules/multirotor_att_control/multirotor_rate_control.c b/src/modules/multirotor_att_control/multirotor_rate_control.c index e37ede3e09..a0266e1b3f 100644 --- a/src/modules/multirotor_att_control/multirotor_rate_control.c +++ b/src/modules/multirotor_att_control/multirotor_rate_control.c @@ -199,6 +199,12 @@ void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp, pid_set_parameters(&roll_rate_controller, p.attrate_p, p.attrate_i, p.attrate_d, 1.0f, 1.0f, 0.2f); } + /* reset integral if on ground */ + if (rate_sp->thrust < 0.01f) { + pid_reset_integral(&pitch_rate_controller); + pid_reset_integral(&roll_rate_controller); + } + /* control pitch (forward) output */ float pitch_control = pid_calculate(&pitch_rate_controller, rate_sp->pitch , rates[1], rates_acc[1], deltaT, &control_debug->pitch_rate_p, &control_debug->pitch_rate_i, &control_debug->pitch_rate_d); From 23858a6726f151cc6d67ecda0d42c7374839d80f Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Wed, 19 Jun 2013 22:59:40 +0200 Subject: [PATCH 066/109] Added functionality to enable PWM output for stupid ESCs even when safety is not off, arming button functionality remains as is --- src/drivers/px4io/px4io.cpp | 71 +++++++++++++-- src/modules/px4iofirmware/mixer.cpp | 125 ++++++++++++++++++-------- src/modules/px4iofirmware/protocol.h | 6 +- src/modules/px4iofirmware/px4io.h | 1 + src/modules/px4iofirmware/registers.c | 39 +++++++- 5 files changed, 197 insertions(+), 45 deletions(-) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index bce193bcac..0ea90beb4b 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -136,6 +136,11 @@ public: */ int set_max_values(const uint16_t *vals, unsigned len); + /** + * Set an idle PWM signal that is active right after startup, even when SAFETY_SAFE + */ + int set_idle_values(const uint16_t *vals, unsigned len); + /** * Print the current status of IO */ @@ -567,7 +572,8 @@ PX4IO::init() io_reg_modify(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING, PX4IO_P_SETUP_ARMING_FMU_ARMED | PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK | - PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK, 0); + PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK | + PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE, 0); /* publish RC config to IO */ ret = io_set_rc_config(); @@ -793,6 +799,20 @@ PX4IO::set_max_values(const uint16_t *vals, unsigned len) return io_reg_set(PX4IO_PAGE_CONTROL_MAX_PWM, 0, vals, len); } +int +PX4IO::set_idle_values(const uint16_t *vals, unsigned len) +{ + uint16_t regs[_max_actuators]; + + if (len > _max_actuators) + /* fail with error */ + return E2BIG; + + printf("Sending IDLE values\n"); + + /* copy values to registers in IO */ + return io_reg_set(PX4IO_PAGE_IDLE_PWM, 0, vals, len); +} int @@ -1441,12 +1461,14 @@ PX4IO::print_status() /* setup and state */ printf("features 0x%04x\n", io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_FEATURES)); uint16_t arming = io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_ARMING); - printf("arming 0x%04x%s%s%s%s\n", + printf("arming 0x%04x%s%s%s%s%s%s\n", arming, ((arming & PX4IO_P_SETUP_ARMING_FMU_ARMED) ? " FMU_ARMED" : ""), ((arming & PX4IO_P_SETUP_ARMING_IO_ARM_OK) ? " IO_ARM_OK" : ""), ((arming & PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK) ? " MANUAL_OVERRIDE_OK" : ""), - ((arming & PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK) ? " INAIR_RESTART_OK" : "")); + ((arming & PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM) ? " FAILSAFE_CUSTOM" : ""), + ((arming & PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK) ? " INAIR_RESTART_OK" : ""), + ((arming & PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE) ? " ALWAYS_PWM_ENABLE" : "")); printf("rates 0x%04x default %u alt %u relays 0x%04x\n", io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_RATES), io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_DEFAULTRATE), @@ -1473,8 +1495,10 @@ PX4IO::print_status() } printf("failsafe"); for (unsigned i = 0; i < _max_actuators; i++) - printf(" %u", io_reg_get(PX4IO_PAGE_FAILSAFE_PWM, i)); - printf("\n"); + printf(" %u\n", io_reg_get(PX4IO_PAGE_FAILSAFE_PWM, i)); + printf("idle values"); + for (unsigned i = 0; i < _max_actuators; i++) + printf(" %u\n", io_reg_get(PX4IO_PAGE_IDLE_PWM, i)); } int @@ -1973,6 +1997,41 @@ px4io_main(int argc, char *argv[]) exit(0); } + if (!strcmp(argv[1], "idle")) { + + if (argc < 3) { + errx(1, "max command needs at least one channel value (PWM)"); + } + + if (g_dev != nullptr) { + + /* set values for first 8 channels, fill unassigned channels with 0. */ + uint16_t idle[8]; + + for (int i = 0; i < sizeof(idle) / sizeof(idle[0]); i++) + { + /* set channel to commanline argument or to 0 for non-provided channels */ + if (argc > i + 2) { + idle[i] = atoi(argv[i+2]); + if (idle[i] < 900 || idle[i] > 2100) { + errx(1, "value out of range of 900 < value < 2100. Aborting."); + } + } else { + /* a zero value will the default */ + idle[i] = 0; + } + } + + int ret = g_dev->set_idle_values(idle, sizeof(idle) / sizeof(idle[0])); + + if (ret != OK) + errx(ret, "failed setting idle values"); + } else { + errx(1, "not loaded"); + } + exit(0); + } + if (!strcmp(argv[1], "recovery")) { if (g_dev != nullptr) { @@ -2100,5 +2159,5 @@ px4io_main(int argc, char *argv[]) monitor(); 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', 'min, 'max', 'idle' or 'update'"); } diff --git a/src/modules/px4iofirmware/mixer.cpp b/src/modules/px4iofirmware/mixer.cpp index 6752a8128d..1f118ea2f0 100644 --- a/src/modules/px4iofirmware/mixer.cpp +++ b/src/modules/px4iofirmware/mixer.cpp @@ -63,6 +63,7 @@ extern "C" { * Time that the ESCs need to initialize */ #define ESC_INIT_TIME_US 1000000 + #define ESC_RAMP_TIME_US 2000000 /* XXX need to move the RC_CHANNEL_FUNCTION out of rc_channels.h and into systemlib */ #define ROLL 0 @@ -73,10 +74,17 @@ extern "C" { /* current servo arm/disarm state */ static bool mixer_servos_armed = false; - +static bool should_arm = false; +static bool should_always_enable_pwm = false; static uint64_t esc_init_time; -static bool esc_init_active = false; -static bool esc_init_done = false; + +enum esc_state_e { + ESC_OFF, + ESC_INIT, + ESC_RAMP, + ESC_ON +}; +static esc_state_e esc_state; /* selected control values and count for mixing */ enum mixer_source { @@ -175,18 +183,48 @@ mixer_tick(void) float outputs[IO_SERVO_COUNT]; unsigned mixed; - /* after arming, some ESCs need an initalization period, count the time from here */ - if (mixer_servos_armed && !esc_init_done && !esc_init_active) { - esc_init_time = hrt_absolute_time(); - esc_init_active = true; + uint16_t ramp_promille; + + /* update esc init state, but only if we are truely armed and not just PWM enabled */ + if (mixer_servos_armed && should_arm) { + + switch (esc_state) { + + /* after arming, some ESCs need an initalization period, count the time from here */ + case ESC_OFF: + esc_init_time = hrt_absolute_time(); + esc_state = ESC_INIT; + break; + + /* after waiting long enough for the ESC initialization, we can start with the ramp to start the ESCs */ + case ESC_INIT: + if (hrt_elapsed_time(&esc_init_time) > ESC_INIT_TIME_US) { + esc_state = ESC_RAMP; + } + break; + + /* then ramp until the min speed is reached */ + case ESC_RAMP: + if (hrt_elapsed_time(&esc_init_time) > (ESC_INIT_TIME_US + ESC_RAMP_TIME_US)) { + esc_state = ESC_ON; + } + break; + + case ESC_ON: + default: + + break; + } + } else { + esc_state = ESC_OFF; } - /* after waiting long enough for the ESC initialization, we can disable the ESC initialization phase */ - if (!esc_init_done && esc_init_active && mixer_servos_armed && (hrt_elapsed_time(&esc_init_time) > ESC_INIT_TIME_US)) { - esc_init_active = false; - esc_init_done = true; + /* do the calculations during the ramp for all at once */ + if(esc_state == ESC_RAMP) { + ramp_promille = (1000*(hrt_elapsed_time(&esc_init_time)-ESC_INIT_TIME_US))/ESC_RAMP_TIME_US; } + /* mix */ mixed = mixer_group.mix(&outputs[0], IO_SERVO_COUNT); @@ -196,21 +234,27 @@ mixer_tick(void) /* save actuator values for FMU readback */ r_page_actuators[i] = FLOAT_TO_REG(outputs[i]); - /* XXX maybe this check for an armed FMU could be achieved a little less messy */ - if (source == MIX_FMU && !(r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED) && (r_setup_arming & PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM)) { - r_page_servos[i] = r_page_servo_failsafe[i]; - } - /* during ESC initialization, use low PWM */ - else if (esc_init_active) { - r_page_servos[i] = (outputs[i] * 600 + 1500); - - /* afterwards use min and max values */ - } else { - r_page_servos[i] = (outputs[i] - * (r_page_servo_control_max[i] - r_page_servo_control_min[i])/2 - + (r_page_servo_control_max[i] + r_page_servo_control_min[i])/2); - } + switch (esc_state) { + case ESC_INIT: + r_page_servos[i] = (outputs[i] * 600 + 1500); + break; + case ESC_RAMP: + r_page_servos[i] = (outputs[i] + * (ramp_promille*r_page_servo_control_max[i] + (1000-ramp_promille)*2100 - ramp_promille*r_page_servo_control_min[i] - (1000-ramp_promille)*900)/2/1000 + + (ramp_promille*r_page_servo_control_max[i] + (1000-ramp_promille)*2100 + ramp_promille*r_page_servo_control_min[i] + (1000-ramp_promille)*900)/2/1000); + break; + + case ESC_ON: + r_page_servos[i] = (outputs[i] + * (r_page_servo_control_max[i] - r_page_servo_control_min[i])/2 + + (r_page_servo_control_max[i] + r_page_servo_control_min[i])/2); + break; + + case ESC_OFF: + default: + break; + } } for (unsigned i = mixed; i < IO_SERVO_COUNT; i++) r_page_servos[i] = 0; @@ -225,36 +269,43 @@ mixer_tick(void) * XXX correct behaviour for failsafe may require an additional case * here. */ - bool should_arm = ( + should_arm = ( /* IO initialised without error */ (r_status_flags & PX4IO_P_STATUS_FLAGS_INIT_OK) && - /* IO is armed */ (r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) && - /* and either FMU is armed */ ( ( (r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED) && - /* and there is valid input via direct PWM or mixer */ (r_status_flags & (PX4IO_P_STATUS_FLAGS_RAW_PWM | PX4IO_P_STATUS_FLAGS_MIXER_OK))) || - - /* or failsafe was set manually */ (r_setup_arming & PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM) ) + /* and IO is armed */ (r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) && + /* and FMU is armed */ ( (r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED) && + /* and there is valid input via direct PWM or mixer */ ((r_status_flags & (PX4IO_P_STATUS_FLAGS_RAW_PWM | PX4IO_P_STATUS_FLAGS_MIXER_OK)) || + /* or failsafe was set manually */ (r_setup_arming & PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM) ) ) ); + should_always_enable_pwm = (r_setup_arming & PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE) + && (r_status_flags & PX4IO_P_STATUS_FLAGS_INIT_OK) + && (r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK); - if (should_arm && !mixer_servos_armed) { + if ((should_arm || should_always_enable_pwm) && !mixer_servos_armed) { /* need to arm, but not armed */ up_pwm_servo_arm(true); mixer_servos_armed = true; r_status_flags |= PX4IO_P_STATUS_FLAGS_OUTPUTS_ARMED; - isr_debug(5, "> armed"); + isr_debug(5, "> PWM enabled"); - } else if (!should_arm && mixer_servos_armed) { + } else if ((!should_arm && !should_always_enable_pwm) && mixer_servos_armed) { /* armed but need to disarm */ up_pwm_servo_arm(false); mixer_servos_armed = false; r_status_flags &= ~(PX4IO_P_STATUS_FLAGS_OUTPUTS_ARMED); - isr_debug(5, "> disarmed"); + isr_debug(5, "> PWM disabled"); - esc_init_active = false; } + } - if (mixer_servos_armed) { + if (mixer_servos_armed && should_arm) { /* update the servo outputs. */ for (unsigned i = 0; i < IO_SERVO_COUNT; i++) up_pwm_servo_set(i, r_page_servos[i]); + + } else if (mixer_servos_armed && should_always_enable_pwm) { + /* set the idle servo outputs. */ + for (unsigned i = 0; i < IO_SERVO_COUNT; i++) + up_pwm_servo_set(i, r_page_servo_idle[i]); } } diff --git a/src/modules/px4iofirmware/protocol.h b/src/modules/px4iofirmware/protocol.h index b430541973..0e477a200d 100644 --- a/src/modules/px4iofirmware/protocol.h +++ b/src/modules/px4iofirmware/protocol.h @@ -155,6 +155,7 @@ #define PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK (1 << 2) /* OK to switch to manual override via override RC channel */ #define PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM (1 << 3) /* use custom failsafe values, not 0 values of mixer */ #define PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK (1 << 4) /* OK to try in-air restart */ +#define PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE (1 << 5) /* Output of PWM right after startup enabled to help ESCs initialize and prevent them from beeping */ #define PX4IO_P_SETUP_PWM_RATES 2 /* bitmask, 0 = low rate, 1 = high rate */ #define PX4IO_P_SETUP_PWM_DEFAULTRATE 3 /* 'low' PWM frame output rate in Hz */ @@ -190,9 +191,12 @@ /* PWM minimum values for certain ESCs */ #define PX4IO_PAGE_CONTROL_MIN_PWM 106 /* 0..CONFIG_ACTUATOR_COUNT-1 */ - /* PWM maximum values for certain ESCs */ +/* PWM maximum values for certain ESCs */ #define PX4IO_PAGE_CONTROL_MAX_PWM 107 /* 0..CONFIG_ACTUATOR_COUNT-1 */ +/* PWM idle values that are active, even when SAFETY_SAFE */ +#define PX4IO_PAGE_IDLE_PWM 108 /* 0..CONFIG_ACTUATOR_COUNT-1 */ + /** * As-needed mixer data upload. * diff --git a/src/modules/px4iofirmware/px4io.h b/src/modules/px4iofirmware/px4io.h index 3e4027c9bd..042e7fe66f 100644 --- a/src/modules/px4iofirmware/px4io.h +++ b/src/modules/px4iofirmware/px4io.h @@ -79,6 +79,7 @@ extern uint16_t r_page_rc_input_config[]; /* PX4IO_PAGE_RC_INPUT_CONFIG */ extern uint16_t r_page_servo_failsafe[]; /* PX4IO_PAGE_FAILSAFE_PWM */ extern uint16_t r_page_servo_control_min[]; /* PX4IO_PAGE_CONTROL_MIN_PWM */ extern uint16_t r_page_servo_control_max[]; /* PX4IO_PAGE_CONTROL_MAX_PWM */ +extern uint16_t r_page_servo_idle[]; /* PX4IO_PAGE_IDLE_PWM */ /* * Register aliases. diff --git a/src/modules/px4iofirmware/registers.c b/src/modules/px4iofirmware/registers.c index 379cf09e3e..bd13f3b7d8 100644 --- a/src/modules/px4iofirmware/registers.c +++ b/src/modules/px4iofirmware/registers.c @@ -147,7 +147,8 @@ volatile uint16_t r_page_setup[] = PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK | \ PX4IO_P_SETUP_ARMING_INAIR_RESTART_OK | \ PX4IO_P_SETUP_ARMING_IO_ARM_OK) | \ - PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM + PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM | \ + PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE #define PX4IO_P_SETUP_RATES_VALID ((1 << IO_SERVO_COUNT) - 1) #define PX4IO_P_SETUP_RELAYS_VALID ((1 << PX4IO_RELAY_CHANNELS) - 1) @@ -201,6 +202,14 @@ uint16_t r_page_servo_control_min[IO_SERVO_COUNT] = { 900, 900, 900, 900, 900, */ uint16_t r_page_servo_control_max[IO_SERVO_COUNT] = { 2100, 2100, 2100, 2100, 2100, 2100, 2100, 2100 }; +/** + * PAGE 108 + * + * idle PWM values for difficult ESCs + * + */ +uint16_t r_page_servo_idle[IO_SERVO_COUNT] = { 900, 900, 900, 900, 900, 900, 900, 900 }; + void registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num_values) { @@ -308,6 +317,31 @@ registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num } break; + case PX4IO_PAGE_IDLE_PWM: + + /* copy channel data */ + while ((offset < IO_SERVO_COUNT) && (num_values > 0)) { + + if (*values == 0) + /* set to default */ + r_page_servo_idle[offset] = 0; + + else if (*values < 900) + r_page_servo_idle[offset] = 900; + else if (*values > 2100) + r_page_servo_idle[offset] = 2100; + else + r_page_servo_idle[offset] = *values; + + /* flag the failsafe values as custom */ + r_setup_arming |= PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE; + + offset++; + num_values--; + values++; + } + break; + /* handle text going to the mixer parser */ case PX4IO_PAGE_MIXERLOAD: mixer_handle_text(values, num_values * sizeof(*values)); @@ -651,6 +685,9 @@ registers_get(uint8_t page, uint8_t offset, uint16_t **values, unsigned *num_val case PX4IO_PAGE_CONTROL_MAX_PWM: SELECT_PAGE(r_page_servo_control_max); break; + case PX4IO_PAGE_IDLE_PWM: + SELECT_PAGE(r_page_servo_idle); + break; default: return -1; From 9b6c9358ed072459ac61feed271a209c8c5dea23 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Thu, 20 Jun 2013 01:13:49 +0200 Subject: [PATCH 067/109] First try for an ESC calibration tool --- makefiles/config_px4fmu_default.mk | 1 + src/drivers/px4io/px4io.cpp | 5 +- src/modules/px4iofirmware/mixer.cpp | 13 +- src/systemcmds/esc_calib/esc_calib.c | 250 +++++++++++++++++++++++++++ src/systemcmds/esc_calib/module.mk | 41 +++++ 5 files changed, 303 insertions(+), 7 deletions(-) create mode 100644 src/systemcmds/esc_calib/esc_calib.c create mode 100644 src/systemcmds/esc_calib/module.mk diff --git a/makefiles/config_px4fmu_default.mk b/makefiles/config_px4fmu_default.mk index 43288537cb..e8104b3511 100644 --- a/makefiles/config_px4fmu_default.mk +++ b/makefiles/config_px4fmu_default.mk @@ -46,6 +46,7 @@ MODULES += systemcmds/param MODULES += systemcmds/perf MODULES += systemcmds/preflight_check MODULES += systemcmds/pwm +MODULES += systemcmds/esc_calib MODULES += systemcmds/reboot MODULES += systemcmds/top MODULES += systemcmds/tests diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 0ea90beb4b..6d3c32ed95 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -1496,9 +1496,10 @@ PX4IO::print_status() printf("failsafe"); for (unsigned i = 0; i < _max_actuators; i++) printf(" %u\n", io_reg_get(PX4IO_PAGE_FAILSAFE_PWM, i)); - printf("idle values"); + printf("\nidle values"); for (unsigned i = 0; i < _max_actuators; i++) - printf(" %u\n", io_reg_get(PX4IO_PAGE_IDLE_PWM, i)); + printf(" %u", io_reg_get(PX4IO_PAGE_IDLE_PWM, i)); + printf("\n"); } int diff --git a/src/modules/px4iofirmware/mixer.cpp b/src/modules/px4iofirmware/mixer.cpp index 1f118ea2f0..fe91667790 100644 --- a/src/modules/px4iofirmware/mixer.cpp +++ b/src/modules/px4iofirmware/mixer.cpp @@ -270,11 +270,14 @@ mixer_tick(void) * here. */ should_arm = ( - /* IO initialised without error */ (r_status_flags & PX4IO_P_STATUS_FLAGS_INIT_OK) && - /* and IO is armed */ (r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) && - /* and FMU is armed */ ( (r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED) && - /* and there is valid input via direct PWM or mixer */ ((r_status_flags & (PX4IO_P_STATUS_FLAGS_RAW_PWM | PX4IO_P_STATUS_FLAGS_MIXER_OK)) || - /* or failsafe was set manually */ (r_setup_arming & PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM) ) ) + /* IO initialised without error */ (r_status_flags & PX4IO_P_STATUS_FLAGS_INIT_OK) + /* and IO is armed */ && (r_status_flags & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) + /* and FMU is armed */ && ( + ((r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED) + /* and there is valid input via or mixer */ && (r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK) ) + /* or direct PWM is set */ || (r_status_flags & PX4IO_P_STATUS_FLAGS_RAW_PWM) + /* or failsafe was set manually */ || (r_setup_arming & PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM) + ) ); should_always_enable_pwm = (r_setup_arming & PX4IO_P_SETUP_ARMING_ALWAYS_PWM_ENABLE) diff --git a/src/systemcmds/esc_calib/esc_calib.c b/src/systemcmds/esc_calib/esc_calib.c new file mode 100644 index 0000000000..188fa4e371 --- /dev/null +++ b/src/systemcmds/esc_calib/esc_calib.c @@ -0,0 +1,250 @@ +/**************************************************************************** + * + * 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 esc_calib.c + * + * Tool for ESC calibration + */ + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include + +#include + +#include "systemlib/systemlib.h" +#include "systemlib/err.h" +#include "drivers/drv_pwm_output.h" + +static void usage(const char *reason); +__EXPORT int esc_calib_main(int argc, char *argv[]); + +#define MAX_CHANNELS 8 + +static void +usage(const char *reason) +{ + if (reason != NULL) + warnx("%s", reason); + errx(1, + "usage:\n" + "esc_calib [-d ] \n" + " PWM output device (defaults to " PWM_OUTPUT_DEVICE_PATH ")\n" + " Provide channels (e.g.: 1 2 3 4)\n" + ); + +} + +int +esc_calib_main(int argc, char *argv[]) +{ + const char *dev = PWM_OUTPUT_DEVICE_PATH; + char *ep; + bool channels_selected[MAX_CHANNELS] = {false}; + int ch; + int ret; + char c; + + if (argc < 2) + usage(NULL); + + while ((ch = getopt(argc, argv, "d:")) != EOF) { + switch (ch) { + + case 'd': + dev = optarg; + argc--; + break; + + default: + usage(NULL); + } + } + + if(argc < 1) { + usage("no channels provided"); + } + + while (--argc) { + const char *arg = argv[argc]; + unsigned channel_number = strtol(arg, &ep, 0); + + if (*ep == '\0') { + if (channel_number > MAX_CHANNELS || channel_number <= 0) { + err(1, "invalid channel number: %d", channel_number); + } else { + channels_selected[channel_number-1] = true; + + } + } + } + + /* Wait for confirmation */ + int console = open("/dev/console", O_NONBLOCK | O_RDONLY | O_NOCTTY); + if (!console) + err(1, "failed opening console"); + + warnx("ATTENTION, please remove or fix props before starting calibration!\n" + "\n" + "Also press the arming switch first for safety off\n" + "\n" + "Do you really want to start calibration: y or n?\n"); + + /* wait for user input */ + while (1) { + + if (read(console, &c, 1) == 1) { + if (c == 'y' || c == 'Y') { + + break; + } else if (c == 0x03 || c == 0x63 || c == 'q') { + warnx("ESC calibration exited"); + close(console); + exit(0); + } else if (c == 'n' || c == 'N') { + warnx("ESC calibration aborted"); + close(console); + exit(0); + } else { + warnx("Unknown input, ESC calibration aborted"); + close(console); + exit(0); + } + } + /* rate limit to ~ 20 Hz */ + usleep(50000); + } + + /* open for ioctl only */ + int fd = open(dev, 0); + if (fd < 0) + err(1, "can't open %s", dev); + + // XXX arming not necessaire at the moment + // /* Then arm */ + // ret = ioctl(fd, PWM_SERVO_SET_ARM_OK, 0); + // if (ret != OK) + // err(1, "PWM_SERVO_SET_ARM_OK"); + + // ret = ioctl(fd, PWM_SERVO_ARM, 0); + // if (ret != OK) + // err(1, "PWM_SERVO_ARM"); + + + + + /* Wait for user confirmation */ + warnx("Set high PWM\n" + "Connect battery now and hit ENTER after the ESCs confirm the first calibration step"); + + while (1) { + + /* First set high PWM */ + for (unsigned i = 0; i Date: Thu, 20 Jun 2013 11:30:10 +0400 Subject: [PATCH 068/109] Att rate PID fix --- .../multirotor_att_control/multirotor_rate_control.c | 9 +++++---- src/modules/systemlib/pid/pid.c | 12 ++++++++---- src/modules/systemlib/pid/pid.h | 5 ++++- 3 files changed, 17 insertions(+), 9 deletions(-) diff --git a/src/modules/multirotor_att_control/multirotor_rate_control.c b/src/modules/multirotor_att_control/multirotor_rate_control.c index a0266e1b3f..8f26a20141 100644 --- a/src/modules/multirotor_att_control/multirotor_rate_control.c +++ b/src/modules/multirotor_att_control/multirotor_rate_control.c @@ -179,6 +179,7 @@ void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp, static bool initialized = false; + float diff_filter_factor = 1.0f; /* initialize the pid controllers when the function is called for the first time */ if (initialized == false) { @@ -186,8 +187,8 @@ void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp, parameters_update(&h, &p); initialized = true; - pid_init(&pitch_rate_controller, p.attrate_p, p.attrate_i, p.attrate_d, 1.0f, 1.0f, 0.2f, PID_MODE_DERIVATIV_SET); - pid_init(&roll_rate_controller, p.attrate_p, p.attrate_i, p.attrate_d, 1.0f, 1.0f, 0.2f, PID_MODE_DERIVATIV_SET); + pid_init(&pitch_rate_controller, p.attrate_p, p.attrate_i, p.attrate_d, 1.0f, 1.0f, diff_filter_factor, PID_MODE_DERIVATIV_CALC_NO_SP); + pid_init(&roll_rate_controller, p.attrate_p, p.attrate_i, p.attrate_d, 1.0f, 1.0f, diff_filter_factor, PID_MODE_DERIVATIV_CALC_NO_SP); } @@ -195,8 +196,8 @@ void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp, if (motor_skip_counter % 2500 == 0) { /* update parameters from storage */ parameters_update(&h, &p); - pid_set_parameters(&pitch_rate_controller, p.attrate_p, p.attrate_i, p.attrate_d, 1.0f, 1.0f, 0.2f); - pid_set_parameters(&roll_rate_controller, p.attrate_p, p.attrate_i, p.attrate_d, 1.0f, 1.0f, 0.2f); + pid_set_parameters(&pitch_rate_controller, p.attrate_p, p.attrate_i, p.attrate_d, 1.0f, 1.0f, diff_filter_factor); + pid_set_parameters(&roll_rate_controller, p.attrate_p, p.attrate_i, p.attrate_d, 1.0f, 1.0f, diff_filter_factor); } /* reset integral if on ground */ diff --git a/src/modules/systemlib/pid/pid.c b/src/modules/systemlib/pid/pid.c index 3685b2aebb..ada364e372 100644 --- a/src/modules/systemlib/pid/pid.c +++ b/src/modules/systemlib/pid/pid.c @@ -144,15 +144,19 @@ __EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, flo // Calculated current error value float error = pid->sp - val; - - float error_filtered = pid->diff_filter_factor*error + (1.0f-pid->diff_filter_factor)*pid->error_previous_filtered; + float error_filtered; // Calculate or measured current error derivative if (pid->mode == PID_MODE_DERIVATIV_CALC) { -// d = (error_filtered - pid->error_previous_filtered) / dt; - d = error_filtered - pid->error_previous_filtered ; + error_filtered = pid->error_previous_filtered + (error - pid->error_previous_filtered) * pid->diff_filter_factor; + d = (error_filtered - pid->error_previous_filtered) / fmaxf(dt, 0.003f); // fail-safe for too low dt + pid->error_previous_filtered = error_filtered; + } else if (pid->mode == PID_MODE_DERIVATIV_CALC_NO_SP) { + + error_filtered = pid->error_previous_filtered + (val - pid->error_previous_filtered) * pid->diff_filter_factor; + d = (error_filtered - pid->error_previous_filtered) / fmaxf(dt, 0.003f); // fail-safe for too low dt pid->error_previous_filtered = error_filtered; } else if (pid->mode == PID_MODE_DERIVATIV_SET) { d = -val_dot; diff --git a/src/modules/systemlib/pid/pid.h b/src/modules/systemlib/pid/pid.h index e3d9038cdd..eb9df96cc9 100644 --- a/src/modules/systemlib/pid/pid.h +++ b/src/modules/systemlib/pid/pid.h @@ -47,8 +47,11 @@ /* PID_MODE_DERIVATIV_CALC calculates discrete derivative from previous error * val_dot in pid_calculate() will be ignored */ #define PID_MODE_DERIVATIV_CALC 0 +/* PID_MODE_DERIVATIV_CALC_NO_SP calculates discrete derivative from previous value, setpoint derivative is ignored + * val_dot in pid_calculate() will be ignored */ +#define PID_MODE_DERIVATIV_CALC_NO_SP 1 /* Use PID_MODE_DERIVATIV_SET if you have the derivative already (Gyros, Kalman) */ -#define PID_MODE_DERIVATIV_SET 1 +#define PID_MODE_DERIVATIV_SET 2 // Use PID_MODE_DERIVATIV_NONE for a PI controller (vs PID) #define PID_MODE_DERIVATIV_NONE 9 From 462a9c84540e9258a5ab4270b258e5809cb5f88b Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Thu, 20 Jun 2013 11:32:25 +0400 Subject: [PATCH 069/109] sdlog2: add angular accelerations to ATT message --- src/modules/sdlog2/sdlog2.c | 3 +++ src/modules/sdlog2/sdlog2_messages.h | 5 ++++- 2 files changed, 7 insertions(+), 1 deletion(-) diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index f89b49acf9..3c3f1a0743 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -986,6 +986,9 @@ int sdlog2_thread_main(int argc, char *argv[]) log_msg.body.log_ATT.roll_rate = buf.att.rollspeed; log_msg.body.log_ATT.pitch_rate = buf.att.pitchspeed; log_msg.body.log_ATT.yaw_rate = buf.att.yawspeed; + log_msg.body.log_ATT.roll_acc = buf.att.rollacc; + log_msg.body.log_ATT.pitch_acc = buf.att.pitchacc; + log_msg.body.log_ATT.yaw_acc = buf.att.yawacc; LOGBUFFER_WRITE_AND_COUNT(ATT); } diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index 2b61378ed7..43645c302b 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -63,6 +63,9 @@ struct log_ATT_s { float roll_rate; float pitch_rate; float yaw_rate; + float roll_acc; + float pitch_acc; + float yaw_acc; }; /* --- ATSP - ATTITUDE SET POINT --- */ @@ -206,7 +209,7 @@ struct log_ARSP_s { static const struct log_format_s log_formats[] = { LOG_FORMAT(TIME, "Q", "StartTime"), - LOG_FORMAT(ATT, "ffffff", "Roll,Pitch,Yaw,RollRate,PitchRate,YawRate"), + LOG_FORMAT(ATT, "fffffffff", "Roll,Pitch,Yaw,RollR,PitchR,YawR,RollA,PitchA,YawA"), LOG_FORMAT(ATSP, "ffff", "RollSP,PitchSP,YawSP,ThrustSP"), LOG_FORMAT(IMU, "fffffffff", "AccX,AccY,AccZ,GyroX,GyroY,GyroZ,MagX,MagY,MagZ"), LOG_FORMAT(SENS, "ffff", "BaroPres,BaroAlt,BaroTemp,DiffPres"), From dec1fdbde0c7bb6f3eacae97ab9656f77294cbfc Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Thu, 20 Jun 2013 11:52:05 +0400 Subject: [PATCH 070/109] Cleanup: remove useless angular rates from attitude rate controller --- .../multirotor_att_control/multirotor_att_control_main.c | 8 +------- .../multirotor_att_control/multirotor_rate_control.c | 6 +++--- .../multirotor_att_control/multirotor_rate_control.h | 2 +- 3 files changed, 5 insertions(+), 11 deletions(-) diff --git a/src/modules/multirotor_att_control/multirotor_att_control_main.c b/src/modules/multirotor_att_control/multirotor_att_control_main.c index 38775ed1f5..8ba3241ad5 100644 --- a/src/modules/multirotor_att_control/multirotor_att_control_main.c +++ b/src/modules/multirotor_att_control/multirotor_att_control_main.c @@ -395,13 +395,7 @@ mc_thread_main(int argc, char *argv[]) rates[1] = att.pitchspeed; rates[2] = att.yawspeed; - rates_acc[0] = att.rollacc; - rates_acc[1] = att.pitchacc; - rates_acc[2] = att.yawacc; - - - - multirotor_control_rates(&rates_sp, rates, rates_acc, &actuators, &control_debug_pub, &control_debug); + multirotor_control_rates(&rates_sp, rates, &actuators, &control_debug_pub, &control_debug); orb_publish(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_pub, &actuators); orb_publish(ORB_ID(vehicle_control_debug), control_debug_pub, &control_debug); diff --git a/src/modules/multirotor_att_control/multirotor_rate_control.c b/src/modules/multirotor_att_control/multirotor_rate_control.c index 8f26a20141..641d833f08 100644 --- a/src/modules/multirotor_att_control/multirotor_rate_control.c +++ b/src/modules/multirotor_att_control/multirotor_rate_control.c @@ -151,7 +151,7 @@ static int parameters_update(const struct mc_rate_control_param_handles *h, stru } void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp, - const float rates[], const float rates_acc[], struct actuator_controls_s *actuators, + const float rates[], struct actuator_controls_s *actuators, const orb_advert_t *control_debug_pub, struct vehicle_control_debug_s *control_debug) { static uint64_t last_run = 0; @@ -208,11 +208,11 @@ void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp, /* control pitch (forward) output */ float pitch_control = pid_calculate(&pitch_rate_controller, rate_sp->pitch , - rates[1], rates_acc[1], deltaT, &control_debug->pitch_rate_p, &control_debug->pitch_rate_i, &control_debug->pitch_rate_d); + rates[1], 0.0f, deltaT, &control_debug->pitch_rate_p, &control_debug->pitch_rate_i, &control_debug->pitch_rate_d); /* control roll (left/right) output */ float roll_control = pid_calculate(&roll_rate_controller, rate_sp->roll , - rates[0], rates_acc[0], deltaT, &control_debug->roll_rate_p, &control_debug->roll_rate_i, &control_debug->roll_rate_d); + rates[0], 0.0f, deltaT, &control_debug->roll_rate_p, &control_debug->roll_rate_i, &control_debug->roll_rate_d); /* increase resilience to faulty control inputs */ if (isfinite(pitch_control)) { diff --git a/src/modules/multirotor_att_control/multirotor_rate_control.h b/src/modules/multirotor_att_control/multirotor_rate_control.h index fc741c3786..465549540c 100644 --- a/src/modules/multirotor_att_control/multirotor_rate_control.h +++ b/src/modules/multirotor_att_control/multirotor_rate_control.h @@ -52,7 +52,7 @@ #include void multirotor_control_rates(const struct vehicle_rates_setpoint_s *rate_sp, - const float rates[], const float rates_acc[], struct actuator_controls_s *actuators, + const float rates[], struct actuator_controls_s *actuators, const orb_advert_t *control_debug_pub, struct vehicle_control_debug_s *control_debug); #endif /* MULTIROTOR_RATE_CONTROL_H_ */ From bf0de775329acfc8c450b2958222a83f2a32f977 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Thu, 20 Jun 2013 12:33:35 +0400 Subject: [PATCH 071/109] Critical bugfix in PID --- src/modules/systemlib/pid/pid.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/systemlib/pid/pid.c b/src/modules/systemlib/pid/pid.c index ada364e372..2aafe06366 100644 --- a/src/modules/systemlib/pid/pid.c +++ b/src/modules/systemlib/pid/pid.c @@ -155,7 +155,7 @@ __EXPORT float pid_calculate(PID_t *pid, float sp, float val, float val_dot, flo pid->error_previous_filtered = error_filtered; } else if (pid->mode == PID_MODE_DERIVATIV_CALC_NO_SP) { - error_filtered = pid->error_previous_filtered + (val - pid->error_previous_filtered) * pid->diff_filter_factor; + error_filtered = pid->error_previous_filtered + (- val - pid->error_previous_filtered) * pid->diff_filter_factor; d = (error_filtered - pid->error_previous_filtered) / fmaxf(dt, 0.003f); // fail-safe for too low dt pid->error_previous_filtered = error_filtered; } else if (pid->mode == PID_MODE_DERIVATIV_SET) { From 53dec130c49f69f10527ab55d69de46ee26c58f6 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Mon, 24 Jun 2013 09:49:46 +0200 Subject: [PATCH 072/109] Adapted flow estimator, position and velocity control to new state machine --- .../flow_position_control_main.c | 21 +++++------ .../flow_position_estimator_main.c | 36 +++++++++++-------- .../flow_speed_control_main.c | 22 +++++++----- .../multirotor_att_control_main.c | 2 +- .../multirotor_attitude_control.c | 8 ++--- 5 files changed, 50 insertions(+), 39 deletions(-) diff --git a/src/examples/flow_position_control/flow_position_control_main.c b/src/examples/flow_position_control/flow_position_control_main.c index 5246ea62b6..ecc133f63f 100644 --- a/src/examples/flow_position_control/flow_position_control_main.c +++ b/src/examples/flow_position_control/flow_position_control_main.c @@ -55,7 +55,8 @@ #include #include #include -#include +#include +#include #include #include #include @@ -158,7 +159,8 @@ flow_position_control_thread_main(int argc, char *argv[]) const float time_scale = powf(10.0f,-6.0f); /* structures */ - struct vehicle_status_s vstatus; + struct actuator_safety_s safety; + struct vehicle_control_mode_s control_mode; struct vehicle_attitude_s att; struct manual_control_setpoint_s manual; struct filtered_bottom_flow_s filtered_flow; @@ -169,7 +171,8 @@ flow_position_control_thread_main(int argc, char *argv[]) /* subscribe to attitude, motor setpoints and system state */ int parameter_update_sub = orb_subscribe(ORB_ID(parameter_update)); int vehicle_attitude_sub = orb_subscribe(ORB_ID(vehicle_attitude)); - int vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status)); + int safety_sub = orb_subscribe(ORB_ID(actuator_safety)); + int control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode)); int manual_control_setpoint_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); int filtered_bottom_flow_sub = orb_subscribe(ORB_ID(filtered_bottom_flow)); int vehicle_local_position_sub = orb_subscribe(ORB_ID(vehicle_local_position)); @@ -258,7 +261,7 @@ flow_position_control_thread_main(int argc, char *argv[]) perf_begin(mc_loop_perf); /* get a local copy of the vehicle state */ - orb_copy(ORB_ID(vehicle_status), vehicle_status_sub, &vstatus); + orb_copy(ORB_ID(actuator_safety), safety_sub, &safety); /* get a local copy of manual setpoint */ orb_copy(ORB_ID(manual_control_setpoint), manual_control_setpoint_sub, &manual); /* get a local copy of attitude */ @@ -268,9 +271,7 @@ flow_position_control_thread_main(int argc, char *argv[]) /* get a local copy of local position */ orb_copy(ORB_ID(vehicle_local_position), vehicle_local_position_sub, &local_pos); -// XXX fix this -#if 0 - if (vstatus.state_machine == SYSTEM_STATE_AUTO) + if (control_mode.flag_control_velocity_enabled) { float manual_pitch = manual.pitch / params.rc_scale_pitch; // 0 to 1 float manual_roll = manual.roll / params.rc_scale_roll; // 0 to 1 @@ -492,7 +493,7 @@ flow_position_control_thread_main(int argc, char *argv[]) /* store actual height for speed estimation */ last_local_pos_z = local_pos.z; - speed_sp.thrust_sp = thrust_control; + speed_sp.thrust_sp = thrust_control; //manual.throttle; speed_sp.timestamp = hrt_absolute_time(); /* publish new speed setpoint */ @@ -529,7 +530,6 @@ flow_position_control_thread_main(int argc, char *argv[]) if(isfinite(manual.throttle)) speed_sp.thrust_sp = manual.throttle; } -#endif /* measure in what intervals the controller runs */ perf_count(mc_interval_perf); perf_end(mc_loop_perf); @@ -578,7 +578,8 @@ flow_position_control_thread_main(int argc, char *argv[]) close(parameter_update_sub); close(vehicle_attitude_sub); close(vehicle_local_position_sub); - close(vehicle_status_sub); + close(safety_sub); + close(control_mode_sub); close(manual_control_setpoint_sub); close(speed_sp_pub); diff --git a/src/examples/flow_position_estimator/flow_position_estimator_main.c b/src/examples/flow_position_estimator/flow_position_estimator_main.c index 2389c693d0..8f84307a7a 100644 --- a/src/examples/flow_position_estimator/flow_position_estimator_main.c +++ b/src/examples/flow_position_estimator/flow_position_estimator_main.c @@ -56,7 +56,8 @@ #include #include #include -#include +#include +#include #include #include #include @@ -143,8 +144,8 @@ int flow_position_estimator_thread_main(int argc, char *argv[]) printf("[flow position estimator] starting\n"); /* rotation matrix for transformation of optical flow speed vectors */ - static const int8_t rotM_flow_sensor[3][3] = {{ 0, 1, 0 }, - { -1, 0, 0 }, + static const int8_t rotM_flow_sensor[3][3] = {{ 0, -1, 0 }, + { 1, 0, 0 }, { 0, 0, 1 }}; // 90deg rotated const float time_scale = powf(10.0f,-6.0f); static float speed[3] = {0.0f, 0.0f, 0.0f}; @@ -158,8 +159,10 @@ int flow_position_estimator_thread_main(int argc, char *argv[]) static float sonar_lp = 0.0f; /* subscribe to vehicle status, attitude, sensors and flow*/ - struct vehicle_status_s vstatus; - memset(&vstatus, 0, sizeof(vstatus)); + struct actuator_safety_s safety; + memset(&safety, 0, sizeof(safety)); + struct vehicle_control_mode_s control_mode; + memset(&control_mode, 0, sizeof(control_mode)); struct vehicle_attitude_s att; memset(&att, 0, sizeof(att)); struct vehicle_attitude_setpoint_s att_sp; @@ -170,8 +173,11 @@ int flow_position_estimator_thread_main(int argc, char *argv[]) /* subscribe to parameter changes */ int parameter_update_sub = orb_subscribe(ORB_ID(parameter_update)); - /* subscribe to vehicle status */ - int vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status)); + /* subscribe to safety topic */ + int safety_sub = orb_subscribe(ORB_ID(actuator_safety)); + + /* subscribe to safety topic */ + int control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode)); /* subscribe to attitude */ int vehicle_attitude_sub = orb_subscribe(ORB_ID(vehicle_attitude)); @@ -218,8 +224,7 @@ int flow_position_estimator_thread_main(int argc, char *argv[]) while (!thread_should_exit) { -// XXX fix this -#if 0 + if (sensors_ready) { /*This runs at the rate of the sensors */ @@ -265,7 +270,8 @@ int flow_position_estimator_thread_main(int argc, char *argv[]) /* got flow, updating attitude and status as well */ orb_copy(ORB_ID(vehicle_attitude), vehicle_attitude_sub, &att); orb_copy(ORB_ID(vehicle_attitude_setpoint), vehicle_attitude_setpoint_sub, &att_sp); - orb_copy(ORB_ID(vehicle_status), vehicle_status_sub, &vstatus); + orb_copy(ORB_ID(actuator_safety), safety_sub, &safety); + orb_copy(ORB_ID(vehicle_control_mode), control_mode_sub, &control_mode); /* vehicle state estimation */ float sonar_new = flow.ground_distance_m; @@ -278,12 +284,12 @@ int flow_position_estimator_thread_main(int argc, char *argv[]) if (!vehicle_liftoff) { - if (vstatus.flag_system_armed && att_sp.thrust > params.minimum_liftoff_thrust && sonar_new > 0.3f && sonar_new < 1.0f) + if (safety.armed && att_sp.thrust > params.minimum_liftoff_thrust && sonar_new > 0.3f && sonar_new < 1.0f) vehicle_liftoff = true; } else { - if (!vstatus.flag_system_armed || (att_sp.thrust < params.minimum_liftoff_thrust && sonar_new <= 0.3f)) + if (!safety.armed || (att_sp.thrust < params.minimum_liftoff_thrust && sonar_new <= 0.3f)) vehicle_liftoff = false; } @@ -350,7 +356,7 @@ int flow_position_estimator_thread_main(int argc, char *argv[]) } /* filtering ground distance */ - if (!vehicle_liftoff || !vstatus.flag_system_armed) + if (!vehicle_liftoff || !safety.armed) { /* not possible to fly */ sonar_valid = false; @@ -440,7 +446,6 @@ int flow_position_estimator_thread_main(int argc, char *argv[]) } counter++; - #endif } printf("[flow position estimator] exiting.\n"); @@ -448,7 +453,8 @@ int flow_position_estimator_thread_main(int argc, char *argv[]) close(vehicle_attitude_setpoint_sub); close(vehicle_attitude_sub); - close(vehicle_status_sub); + close(safety_sub); + close(control_mode_sub); close(parameter_update_sub); close(optical_flow_sub); diff --git a/src/examples/flow_speed_control/flow_speed_control_main.c b/src/examples/flow_speed_control/flow_speed_control_main.c index 0be4b3d5a0..4f60e34f2c 100644 --- a/src/examples/flow_speed_control/flow_speed_control_main.c +++ b/src/examples/flow_speed_control/flow_speed_control_main.c @@ -55,7 +55,8 @@ #include #include #include -#include +#include +#include #include #include #include @@ -155,7 +156,8 @@ flow_speed_control_thread_main(int argc, char *argv[]) uint32_t counter = 0; /* structures */ - struct vehicle_status_s vstatus; + struct actuator_safety_s safety; + struct vehicle_control_mode_s control_mode; struct filtered_bottom_flow_s filtered_flow; struct vehicle_bodyframe_speed_setpoint_s speed_sp; @@ -164,7 +166,8 @@ flow_speed_control_thread_main(int argc, char *argv[]) /* subscribe to attitude, motor setpoints and system state */ int parameter_update_sub = orb_subscribe(ORB_ID(parameter_update)); int vehicle_attitude_sub = orb_subscribe(ORB_ID(vehicle_attitude)); - int vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status)); + int safety_sub = orb_subscribe(ORB_ID(actuator_safety)); + int control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode)); int filtered_bottom_flow_sub = orb_subscribe(ORB_ID(filtered_bottom_flow)); int vehicle_bodyframe_speed_setpoint_sub = orb_subscribe(ORB_ID(vehicle_bodyframe_speed_setpoint)); @@ -186,7 +189,6 @@ flow_speed_control_thread_main(int argc, char *argv[]) while (!thread_should_exit) { -#if 0 /* wait for first attitude msg to be sure all data are available */ if (sensors_ready) { @@ -227,14 +229,16 @@ flow_speed_control_thread_main(int argc, char *argv[]) { perf_begin(mc_loop_perf); - /* get a local copy of the vehicle state */ - orb_copy(ORB_ID(vehicle_status), vehicle_status_sub, &vstatus); + /* get a local copy of the safety topic */ + orb_copy(ORB_ID(actuator_safety), safety_sub, &safety); + /* get a local copy of the control mode */ + orb_copy(ORB_ID(vehicle_control_mode), control_mode_sub, &control_mode); /* get a local copy of filtered bottom flow */ orb_copy(ORB_ID(filtered_bottom_flow), filtered_bottom_flow_sub, &filtered_flow); /* get a local copy of bodyframe speed setpoint */ orb_copy(ORB_ID(vehicle_bodyframe_speed_setpoint), vehicle_bodyframe_speed_setpoint_sub, &speed_sp); - if (vstatus.state_machine == SYSTEM_STATE_AUTO) + if (control_mode.flag_control_velocity_enabled) { /* calc new roll/pitch */ float pitch_body = -(speed_sp.vx - filtered_flow.vx) * params.speed_p; @@ -341,7 +345,6 @@ flow_speed_control_thread_main(int argc, char *argv[]) } } } -#endif } printf("[flow speed control] ending now...\n"); @@ -352,7 +355,8 @@ flow_speed_control_thread_main(int argc, char *argv[]) close(vehicle_attitude_sub); close(vehicle_bodyframe_speed_setpoint_sub); close(filtered_bottom_flow_sub); - close(vehicle_status_sub); + close(safety_sub); + close(control_mode_sub); close(att_sp_pub); perf_print_counter(mc_loop_perf); diff --git a/src/modules/multirotor_att_control/multirotor_att_control_main.c b/src/modules/multirotor_att_control/multirotor_att_control_main.c index 41a9f1df5c..4467d2d821 100644 --- a/src/modules/multirotor_att_control/multirotor_att_control_main.c +++ b/src/modules/multirotor_att_control/multirotor_att_control_main.c @@ -331,7 +331,7 @@ mc_thread_main(int argc, char *argv[]) // */ // /* only move setpoint if manual input is != 0 */ - if ((manual.yaw < -0.01f || 0.01f < manual.yaw) && manual.throttle > 0.3f) { + if ((manual.yaw < -0.01f || 0.01f < manual.yaw) && manual.throttle > 0.1f) { rates_sp.yaw = manual.yaw; control_yaw_position = false; first_time_after_yaw_speed_control = true; diff --git a/src/modules/multirotor_att_control/multirotor_attitude_control.c b/src/modules/multirotor_att_control/multirotor_attitude_control.c index 752f292e31..4a1129e1f3 100644 --- a/src/modules/multirotor_att_control/multirotor_attitude_control.c +++ b/src/modules/multirotor_att_control/multirotor_attitude_control.c @@ -156,8 +156,8 @@ void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_s parameters_init(&h); parameters_update(&h, &p); - pid_init(&pitch_controller, p.att_p, p.att_i, p.att_d, 1000.0f, 1000.0f, 0.2f, PID_MODE_DERIVATIV_SET); - pid_init(&roll_controller, p.att_p, p.att_i, p.att_d, 1000.0f, 1000.0f, 0.2f, PID_MODE_DERIVATIV_SET); + pid_init(&pitch_controller, p.att_p, p.att_i, p.att_d, 1000.0f, 1000.0f, 1.0f, PID_MODE_DERIVATIV_SET); + pid_init(&roll_controller, p.att_p, p.att_i, p.att_d, 1000.0f, 1000.0f, 1.0f, PID_MODE_DERIVATIV_SET); initialized = true; } @@ -168,8 +168,8 @@ void multirotor_control_attitude(const struct vehicle_attitude_setpoint_s *att_s parameters_update(&h, &p); /* apply parameters */ - pid_set_parameters(&pitch_controller, p.att_p, p.att_i, p.att_d, 1000.0f, 1000.0f, 0.2f); - pid_set_parameters(&roll_controller, p.att_p, p.att_i, p.att_d, 1000.0f, 1000.0f, 0.2f); + pid_set_parameters(&pitch_controller, p.att_p, p.att_i, p.att_d, 1000.0f, 1000.0f, 1.0f); + pid_set_parameters(&roll_controller, p.att_p, p.att_i, p.att_d, 1000.0f, 1000.0f, 1.0f); } /* reset integral if on ground */ From 60e9ea977d90a05826c86eea14f4a2807851d49c Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Mon, 24 Jun 2013 09:50:55 +0200 Subject: [PATCH 073/109] Conditions where set wrongly in the first 2s after boot --- src/modules/commander/commander.c | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/modules/commander/commander.c b/src/modules/commander/commander.c index edcdf7e546..9d3adaa1d4 100644 --- a/src/modules/commander/commander.c +++ b/src/modules/commander/commander.c @@ -1658,7 +1658,7 @@ int commander_thread_main(int argc, char *argv[]) /* check for global or local position updates, set a timeout of 2s */ - if (hrt_absolute_time() - last_global_position_time < 2000000) { + if (hrt_absolute_time() - last_global_position_time < 2000000 && hrt_absolute_time() > 2000000) { current_status.condition_global_position_valid = true; // XXX check for controller status and home position as well @@ -1666,7 +1666,7 @@ int commander_thread_main(int argc, char *argv[]) current_status.condition_global_position_valid = false; } - if (hrt_absolute_time() - last_local_position_time < 2000000) { + if (hrt_absolute_time() - last_local_position_time < 2000000 && hrt_absolute_time() > 2000000) { current_status.condition_local_position_valid = true; // XXX check for controller status and home position as well @@ -1675,7 +1675,7 @@ int commander_thread_main(int argc, char *argv[]) } /* Check for valid airspeed/differential pressure measurements */ - if (hrt_absolute_time() - last_diff_pres_time < 2000000) { + if (hrt_absolute_time() - last_diff_pres_time < 2000000 && hrt_absolute_time() > 2000000) { current_status.condition_airspeed_valid = true; } else { From d563960267ab1145d5100f9dcfad6035205e4021 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Mon, 24 Jun 2013 17:24:04 +0200 Subject: [PATCH 074/109] Added Flow messages to sdlog2 --- src/modules/sdlog2/sdlog2.c | 10 ++++++++-- src/modules/sdlog2/sdlog2_messages.h | 10 ++++++++++ 2 files changed, 18 insertions(+), 2 deletions(-) diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index edcba4e7d0..33b3c657b3 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -616,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 = 19; + const ssize_t fdsc = 20; /* Sanity check variable and index */ ssize_t fdsc_count = 0; @@ -686,6 +686,7 @@ int sdlog2_thread_main(int argc, char *argv[]) struct log_SENS_s log_SENS; struct log_LPOS_s log_LPOS; struct log_LPSP_s log_LPSP; + struct log_FLOW_s log_FLOW; struct log_GPS_s log_GPS; struct log_ATTC_s log_ATTC; struct log_STAT_s log_STAT; @@ -1129,7 +1130,12 @@ int sdlog2_thread_main(int argc, char *argv[]) /* --- FLOW --- */ if (fds[ifds++].revents & POLLIN) { orb_copy(ORB_ID(optical_flow), subs.flow_sub, &buf.flow); - // TODO not implemented yet + log_msg.msg_type = LOG_FLOW_MSG; + log_msg.body.log_FLOW.flow_comp_x_m = buf.flow.flow_comp_x_m; + log_msg.body.log_FLOW.flow_comp_y_m = buf.flow.flow_comp_y_m; + log_msg.body.log_FLOW.ground_distance_m = buf.flow.ground_distance_m; + log_msg.body.log_FLOW.quality = buf.flow.quality; + LOGBUFFER_WRITE_AND_COUNT(FLOW); } /* --- RC CHANNELS --- */ diff --git a/src/modules/sdlog2/sdlog2_messages.h b/src/modules/sdlog2/sdlog2_messages.h index 04eae40b04..755d21d55d 100644 --- a/src/modules/sdlog2/sdlog2_messages.h +++ b/src/modules/sdlog2/sdlog2_messages.h @@ -206,6 +206,15 @@ struct log_ARSP_s { float pitch_rate_sp; float yaw_rate_sp; }; + +/* --- FLOW - FLOW DATA --- */ +#define LOG_FLOW_MSG 16 +struct log_FLOW_s { + float flow_comp_x_m; + float flow_comp_y_m; + float ground_distance_m; + uint8_t quality; +}; #pragma pack(pop) /* construct list of all message formats */ @@ -226,6 +235,7 @@ static const struct log_format_s log_formats[] = { LOG_FORMAT(OUT0, "ffffffff", "Out0,Out1,Out2,Out3,Out4,Out5,Out6,Out7"), LOG_FORMAT(AIRS, "ff", "IndSpeed,TrueSpeed"), LOG_FORMAT(ARSP, "fff", "RollRateSP,PitchRateSP,YawRateSP"), + LOG_FORMAT(FLOW, "fffB", "FlowX,FlowY,SonAlt,FQual") }; static const int log_formats_num = sizeof(log_formats) / sizeof(struct log_format_s); From 794a2248f02e014bc81e977da5a4eee7d71902b5 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Mon, 24 Jun 2013 17:24:49 +0200 Subject: [PATCH 075/109] Only some small cleanup to att control --- .../multirotor_att_control_main.c | 98 +++++-------------- 1 file changed, 22 insertions(+), 76 deletions(-) diff --git a/src/modules/multirotor_att_control/multirotor_att_control_main.c b/src/modules/multirotor_att_control/multirotor_att_control_main.c index 4467d2d821..25447aaba2 100644 --- a/src/modules/multirotor_att_control/multirotor_att_control_main.c +++ b/src/modules/multirotor_att_control/multirotor_att_control_main.c @@ -256,7 +256,7 @@ mc_thread_main(int argc, char *argv[]) } - } else if (control_mode.flag_control_manual_enabled) { + } else if (control_mode.flag_control_manual_enabled) { if (control_mode.flag_control_attitude_enabled) { /* initialize to current yaw if switching to manual or att control */ if (control_mode.flag_control_attitude_enabled != flag_control_attitude_enabled || @@ -268,90 +268,34 @@ mc_thread_main(int argc, char *argv[]) static bool rc_loss_first_time = true; /* if the RC signal is lost, try to stay level and go slowly back down to ground */ -/* XXX fix this */ -#if 0 - if (state.rc_signal_lost) { - /* the failsafe throttle is stored as a parameter, as it depends on the copter and the payload */ - param_get(failsafe_throttle_handle, &failsafe_throttle); - att_sp.roll_body = 0.0f; - att_sp.pitch_body = 0.0f; - /* - * Only go to failsafe throttle if last known throttle was - * high enough to create some lift to make hovering state likely. - * - * This is to prevent that someone landing, but not disarming his - * multicopter (throttle = 0) does not make it jump up in the air - * if shutting down his remote. - */ - if (isfinite(manual.throttle) && manual.throttle > 0.2f) { - att_sp.thrust = failsafe_throttle; + rc_loss_first_time = true; - } else { - att_sp.thrust = 0.0f; - } + att_sp.roll_body = manual.roll; + att_sp.pitch_body = manual.pitch; - /* keep current yaw, do not attempt to go to north orientation, - * since if the pilot regains RC control, he will be lost regarding - * the current orientation. - */ - if (rc_loss_first_time) - att_sp.yaw_body = att.yaw; + /* set attitude if arming */ + if (!flag_control_attitude_enabled && safety.armed) { + att_sp.yaw_body = att.yaw; + } - rc_loss_first_time = false; + /* only move setpoint if manual input is != 0 */ + if ((manual.yaw < -0.01f || 0.01f < manual.yaw) && manual.throttle > 0.1f) { + rates_sp.yaw = manual.yaw; + control_yaw_position = false; + first_time_after_yaw_speed_control = true; } else { -#endif - rc_loss_first_time = true; - - att_sp.roll_body = manual.roll; - att_sp.pitch_body = manual.pitch; - - /* set attitude if arming */ - if (!flag_control_attitude_enabled && safety.armed) { + if (first_time_after_yaw_speed_control) { att_sp.yaw_body = att.yaw; + first_time_after_yaw_speed_control = false; } - /* act if stabilization is active or if the (nonsense) direct pass through mode is set */ -#warning fix this -// if (state.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_SAS || -// state.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_DIRECT) { -// -// if (state.manual_sas_mode == VEHICLE_MANUAL_SAS_MODE_ROLL_PITCH_ABS_YAW_RATE) { -// rates_sp.yaw = manual.yaw; -// control_yaw_position = false; -// -// } else { -// /* -// * This mode SHOULD be the default mode, which is: -// * VEHICLE_MANUAL_SAS_MODE_ROLL_PITCH_ABS_YAW_ABS -// * -// * However, we fall back to this setting for all other (nonsense) -// * settings as well. -// */ -// - /* only move setpoint if manual input is != 0 */ - if ((manual.yaw < -0.01f || 0.01f < manual.yaw) && manual.throttle > 0.1f) { - rates_sp.yaw = manual.yaw; - control_yaw_position = false; - first_time_after_yaw_speed_control = true; - - } else { - if (first_time_after_yaw_speed_control) { - att_sp.yaw_body = att.yaw; - first_time_after_yaw_speed_control = false; - } - - control_yaw_position = true; - } -// } -// } - - att_sp.thrust = manual.throttle; - att_sp.timestamp = hrt_absolute_time(); -#if 0 + control_yaw_position = true; } -#endif + + att_sp.thrust = manual.throttle; + att_sp.timestamp = hrt_absolute_time(); /* STEP 2: publish the controller output */ orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp); @@ -369,7 +313,9 @@ mc_thread_main(int argc, char *argv[]) } } else { -#warning fix this + + //XXX TODO add acro mode here + /* manual rate inputs, from RC control or joystick */ // if (state.flag_control_rates_enabled && // state.manual_control_mode == VEHICLE_MANUAL_CONTROL_MODE_RATES) { From 2096da2d4da2d4482dcdb328d35255101fc722bd Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Tue, 25 Jun 2013 13:11:15 +0200 Subject: [PATCH 076/109] Don't interrupt a playing tune unless its a repeated one --- src/drivers/stm32/tone_alarm/tone_alarm.cpp | 29 ++++++++++++++++++--- 1 file changed, 25 insertions(+), 4 deletions(-) diff --git a/src/drivers/stm32/tone_alarm/tone_alarm.cpp b/src/drivers/stm32/tone_alarm/tone_alarm.cpp index ac5511e60a..e8c6fd5426 100644 --- a/src/drivers/stm32/tone_alarm/tone_alarm.cpp +++ b/src/drivers/stm32/tone_alarm/tone_alarm.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * 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 @@ -241,6 +241,8 @@ private: static const unsigned _default_ntunes; static const uint8_t _note_tab[]; + unsigned _default_tune_number; // number of currently playing default tune (0 for none) + const char *_user_tune; const char *_tune; // current tune string @@ -456,6 +458,11 @@ const char * const ToneAlarm::_default_tunes[] = { "O1B8O2G+8E8B8G+8O3E8O2B8O3E8O2B8O3G+8E8B8" "O3G+8O4E4P8E16E16E8E8E8E8E4P8E16E4P8O2E16" "O2E2P64", + "MNT75L1O2G", //arming warning + "MBNT100a8", //battery warning slow + "MBNT255a8a8a8a8a8a8a8a8a8a8a8a8a8a8a8a8" + "a8a8a8a8a8a8a8a8a8a8a8a8a8a8a8a8" + "a8a8a8a8a8a8a8a8a8a8a8a8a8a8a8a8", //battery warning fast // XXX why is there a break before a repetition }; const unsigned ToneAlarm::_default_ntunes = sizeof(_default_tunes) / sizeof(_default_tunes[0]); @@ -471,6 +478,7 @@ extern "C" __EXPORT int tone_alarm_main(int argc, char *argv[]); ToneAlarm::ToneAlarm() : CDev("tone_alarm", "/dev/tone_alarm"), + _default_tune_number(0), _user_tune(nullptr), _tune(nullptr), _next(nullptr) @@ -803,8 +811,12 @@ tune_error: // stop (and potentially restart) the tune tune_end: stop_note(); - if (_repeat) + if (_repeat) { start_tune(_tune); + } else { + _tune = nullptr; + _default_tune_number = 0; + } return; } @@ -873,8 +885,17 @@ ToneAlarm::ioctl(file *filp, int cmd, unsigned long arg) _tune = nullptr; _next = nullptr; } else { - // play the selected tune - start_tune(_default_tunes[arg - 1]); + /* don't interrupt alarms unless they are repeated */ + if (_tune != nullptr && !_repeat) { + /* abort and let the current tune finish */ + result = -EBUSY; + } else if (_repeat && _default_tune_number == arg) { + /* requested repeating tune already playing */ + } else { + // play the selected tune + _default_tune_number = arg; + start_tune(_default_tunes[arg - 1]); + } } } else { result = -EINVAL; From a6ba7e448586c556009132887503e6830c20029e Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Tue, 25 Jun 2013 13:15:38 +0200 Subject: [PATCH 077/109] Dropped superseded safety topic, added warning tones before arming --- src/drivers/px4io/px4io.cpp | 15 +++--- src/modules/commander/commander.c | 64 +++++++++++++++-------- src/modules/uORB/objects_common.cpp | 3 -- src/modules/uORB/topics/actuator_safety.h | 2 +- src/modules/uORB/topics/safety.h | 60 --------------------- 5 files changed, 53 insertions(+), 91 deletions(-) delete mode 100644 src/modules/uORB/topics/safety.h diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 6d3c32ed95..df2f18270d 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -81,7 +81,6 @@ #include #include #include -#include #include #include @@ -990,20 +989,24 @@ PX4IO::io_handle_status(uint16_t status) /** * Get and handle the safety status */ - struct safety_s safety; + struct actuator_safety_s safety; safety.timestamp = hrt_absolute_time(); + orb_copy(ORB_ID(actuator_safety), _t_actuator_safety, &safety); + if (status & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) { - safety.status = SAFETY_STATUS_UNLOCKED; + safety.safety_off = true; + safety.safety_switch_available = true; } else { - safety.status = SAFETY_STATUS_SAFE; + safety.safety_off = false; + safety.safety_switch_available = true; } /* lazily publish the safety status */ if (_to_safety > 0) { - orb_publish(ORB_ID(safety), _to_safety, &safety); + orb_publish(ORB_ID(actuator_safety), _to_safety, &safety); } else { - _to_safety = orb_advertise(ORB_ID(safety), &safety); + _to_safety = orb_advertise(ORB_ID(actuator_safety), &safety); } return ret; diff --git a/src/modules/commander/commander.c b/src/modules/commander/commander.c index 9d3adaa1d4..4c884aed36 100644 --- a/src/modules/commander/commander.c +++ b/src/modules/commander/commander.c @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Copyright (C) 2013 PX4 Development Team. All rights reserved. * Author: Petri Tanskanen * Lorenz Meier * Thomas Gubler @@ -79,7 +79,6 @@ #include #include #include -#include #include #include @@ -1159,6 +1158,9 @@ int commander_thread_main(int argc, char *argv[]) commander_initialized = false; bool home_position_set = false; + bool battery_tune_played = false; + bool arm_tune_played = false; + /* set parameters */ failsafe_lowlevel_timeout_ms = 0; param_get(param_find("SYS_FAILSAVE_LL"), &failsafe_lowlevel_timeout_ms); @@ -1248,6 +1250,9 @@ int commander_thread_main(int argc, char *argv[]) safety_pub = orb_advertise(ORB_ID(actuator_safety), &safety); + /* but also subscribe to it */ + int safety_sub = orb_subscribe(ORB_ID(actuator_safety)); + control_mode_pub = orb_advertise(ORB_ID(vehicle_control_mode), &control_mode); /* home position */ @@ -1477,6 +1482,13 @@ int commander_thread_main(int argc, char *argv[]) } } + /* update safety topic */ + orb_check(safety_sub, &new_data); + + if (new_data) { + orb_copy(ORB_ID(actuator_safety), safety_sub, &safety); + } + /* update global position estimate */ orb_check(global_position_sub, &new_data); @@ -1573,25 +1585,6 @@ int commander_thread_main(int argc, char *argv[]) last_idle_time = system_load.tasks[0].total_runtime; } - // // XXX Export patterns and threshold to parameters - /* Trigger audio event for low battery */ - if (bat_remain < 0.1f && battery_voltage_valid && (counter % ((1000000 / COMMANDER_MONITORING_INTERVAL) / 4) == 0)) { - /* For less than 10%, start be really annoying at 5 Hz */ - ioctl(buzzer, TONE_SET_ALARM, 0); - ioctl(buzzer, TONE_SET_ALARM, 3); - - } else if (bat_remain < 0.1f && battery_voltage_valid && (counter % ((1000000 / COMMANDER_MONITORING_INTERVAL) / 4) == 2)) { - ioctl(buzzer, TONE_SET_ALARM, 0); - - } else if (bat_remain < 0.2f && battery_voltage_valid && (counter % ((1000000 / COMMANDER_MONITORING_INTERVAL) / 2) == 0)) { - /* For less than 20%, start be slightly annoying at 1 Hz */ - ioctl(buzzer, TONE_SET_ALARM, 0); - tune_positive(); - - } else if (bat_remain < 0.2f && battery_voltage_valid && (counter % ((1000000 / COMMANDER_MONITORING_INTERVAL) / 2) == 2)) { - ioctl(buzzer, TONE_SET_ALARM, 0); - } - /* Check battery voltage */ /* write to sys_status */ if (battery_voltage_valid) { @@ -2194,6 +2187,8 @@ int commander_thread_main(int argc, char *argv[]) } + + current_status.counter++; current_status.timestamp = hrt_absolute_time(); @@ -2219,6 +2214,33 @@ int commander_thread_main(int argc, char *argv[]) /* Store old modes to detect and act on state transitions */ voltage_previous = current_status.voltage_battery; + + + /* play tone according to evaluation result */ + /* check if we recently armed */ + if (!arm_tune_played && safety.armed && ( !safety.safety_switch_available || (safety.safety_off && safety.safety_switch_available))) { + ioctl(buzzer, TONE_SET_ALARM, 12); + arm_tune_played = true; + + // // XXX Export patterns and threshold to parameters + /* Trigger audio event for low battery */ + } else if (bat_remain < 0.1f && battery_voltage_valid) { + ioctl(buzzer, TONE_SET_ALARM, 14); + battery_tune_played = true; + } else if (bat_remain < 0.2f && battery_voltage_valid) { + ioctl(buzzer, TONE_SET_ALARM, 13); + battery_tune_played = true; + } else if(battery_tune_played) { + ioctl(buzzer, TONE_SET_ALARM, 0); + battery_tune_played = false; + } + + /* reset arm_tune_played when disarmed */ + if (!(safety.armed && ( !safety.safety_switch_available || (safety.safety_off && safety.safety_switch_available)))) { + arm_tune_played = false; + } + + /* XXX use this voltage_previous */ fflush(stdout); counter++; diff --git a/src/modules/uORB/objects_common.cpp b/src/modules/uORB/objects_common.cpp index b5dafd0caa..b602b17149 100644 --- a/src/modules/uORB/objects_common.cpp +++ b/src/modules/uORB/objects_common.cpp @@ -169,6 +169,3 @@ ORB_DEFINE(actuator_outputs_3, struct actuator_outputs_s); #include "topics/debug_key_value.h" ORB_DEFINE(debug_key_value, struct debug_key_value_s); -/* status of the system safety device */ -#include "topics/safety.h" -ORB_DEFINE(safety, struct safety_s); diff --git a/src/modules/uORB/topics/actuator_safety.h b/src/modules/uORB/topics/actuator_safety.h index 3a107d41aa..c431217ab1 100644 --- a/src/modules/uORB/topics/actuator_safety.h +++ b/src/modules/uORB/topics/actuator_safety.h @@ -53,7 +53,7 @@ struct actuator_safety_s { uint64_t timestamp; - + bool safety_switch_available; /**< Set to true if a safety switch is connected */ bool safety_off; /**< Set to true if safety is off */ bool armed; /**< Set to true if system is armed */ bool ready_to_arm; /**< Set to true if system is ready to be armed */ diff --git a/src/modules/uORB/topics/safety.h b/src/modules/uORB/topics/safety.h deleted file mode 100644 index 19e8e8d459..0000000000 --- a/src/modules/uORB/topics/safety.h +++ /dev/null @@ -1,60 +0,0 @@ -/**************************************************************************** - * - * 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 safety.h - * - * Status of an attached safety device - */ - -#ifndef TOPIC_SAFETY_H -#define TOPIC_SAFETY_H - -#include -#include "../uORB.h" - -enum SAFETY_STATUS { - SAFETY_STATUS_NOT_PRESENT, - SAFETY_STATUS_SAFE, - SAFETY_STATUS_UNLOCKED -}; - -struct safety_s { - uint64_t timestamp; /**< output timestamp in us since system boot */ - enum SAFETY_STATUS status; -}; - -/* actuator output sets; this list can be expanded as more drivers emerge */ -ORB_DECLARE(safety); - -#endif /* TOPIC_SAFETY_H */ From 9ce2b62eb57b519348c4b2fcd58af09999e504e7 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Tue, 25 Jun 2013 14:45:27 +0200 Subject: [PATCH 078/109] Beep when arming or disarming with RC --- src/modules/commander/commander.c | 16 +++++++++------- 1 file changed, 9 insertions(+), 7 deletions(-) diff --git a/src/modules/commander/commander.c b/src/modules/commander/commander.c index 4c884aed36..4a5eb23add 100644 --- a/src/modules/commander/commander.c +++ b/src/modules/commander/commander.c @@ -2033,9 +2033,11 @@ int commander_thread_main(int argc, char *argv[]) (current_status.system_type == VEHICLE_TYPE_OCTOROTOR) ) { arming_state_transition(status_pub, ¤t_status, ARMING_STATE_STANDBY, safety_pub, &safety, mavlink_fd); + tune_positive(); } else { mavlink_log_critical(mavlink_fd, "STICK DISARM not allowed"); + tune_negative(); } stick_off_counter = 0; @@ -2050,6 +2052,7 @@ int commander_thread_main(int argc, char *argv[]) if (stick_on_counter > STICK_ON_OFF_COUNTER_LIMIT) { arming_state_transition(status_pub, ¤t_status, ARMING_STATE_ARMED, safety_pub, &safety, mavlink_fd); stick_on_counter = 0; + tune_positive(); } else { stick_on_counter++; @@ -2219,17 +2222,16 @@ int commander_thread_main(int argc, char *argv[]) /* play tone according to evaluation result */ /* check if we recently armed */ if (!arm_tune_played && safety.armed && ( !safety.safety_switch_available || (safety.safety_off && safety.safety_switch_available))) { - ioctl(buzzer, TONE_SET_ALARM, 12); - arm_tune_played = true; + if (ioctl(buzzer, TONE_SET_ALARM, 12) == OK) + arm_tune_played = true; - // // XXX Export patterns and threshold to parameters /* Trigger audio event for low battery */ } else if (bat_remain < 0.1f && battery_voltage_valid) { - ioctl(buzzer, TONE_SET_ALARM, 14); - battery_tune_played = true; + if (ioctl(buzzer, TONE_SET_ALARM, 14) == OK) + battery_tune_played = true; } else if (bat_remain < 0.2f && battery_voltage_valid) { - ioctl(buzzer, TONE_SET_ALARM, 13); - battery_tune_played = true; + if (ioctl(buzzer, TONE_SET_ALARM, 13) == OK) + battery_tune_played = true; } else if(battery_tune_played) { ioctl(buzzer, TONE_SET_ALARM, 0); battery_tune_played = false; From 0ecc9c4bf4f2bf9fe1d99b5cbdf398718d2dccdd Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Tue, 25 Jun 2013 16:30:35 +0200 Subject: [PATCH 079/109] Shrinking the main commander file a bit --- .../commander/accelerometer_calibration.c | 14 +- .../commander/accelerometer_calibration.h | 1 - src/modules/commander/airspeed_calibration.c | 111 ++++ src/modules/commander/airspeed_calibration.h | 46 ++ src/modules/commander/baro_calibration.c | 54 ++ src/modules/commander/baro_calibration.h | 46 ++ src/modules/commander/calibration_routines.c | 1 + src/modules/commander/commander.c | 513 +----------------- src/modules/commander/commander.h | 4 - src/modules/commander/commander_helper.c | 129 +++++ src/modules/commander/commander_helper.h | 66 +++ src/modules/commander/gyro_calibration.c | 151 ++++++ src/modules/commander/gyro_calibration.h | 46 ++ src/modules/commander/mag_calibration.c | 278 ++++++++++ src/modules/commander/mag_calibration.h | 46 ++ src/modules/commander/module.mk | 8 +- src/modules/commander/rc_calibration.c | 83 +++ src/modules/commander/rc_calibration.h | 46 ++ src/modules/commander/state_machine_helper.c | 68 +-- src/modules/commander/state_machine_helper.h | 12 +- 20 files changed, 1159 insertions(+), 564 deletions(-) create mode 100644 src/modules/commander/airspeed_calibration.c create mode 100644 src/modules/commander/airspeed_calibration.h create mode 100644 src/modules/commander/baro_calibration.c create mode 100644 src/modules/commander/baro_calibration.h create mode 100644 src/modules/commander/commander_helper.c create mode 100644 src/modules/commander/commander_helper.h create mode 100644 src/modules/commander/gyro_calibration.c create mode 100644 src/modules/commander/gyro_calibration.h create mode 100644 src/modules/commander/mag_calibration.c create mode 100644 src/modules/commander/mag_calibration.h create mode 100644 src/modules/commander/rc_calibration.c create mode 100644 src/modules/commander/rc_calibration.h diff --git a/src/modules/commander/accelerometer_calibration.c b/src/modules/commander/accelerometer_calibration.c index 231739962c..7bae8ad405 100644 --- a/src/modules/commander/accelerometer_calibration.c +++ b/src/modules/commander/accelerometer_calibration.c @@ -70,15 +70,25 @@ */ #include "accelerometer_calibration.h" +#include "commander_helper.h" +#include +#include #include +#include +#include +#include +#include + + #include #include #include #include +#include +#include #include -void do_accel_calibration(int mavlink_fd); int do_accel_calibration_measurements(int mavlink_fd, float accel_offs[3], float accel_scale[3]); int detect_orientation(int mavlink_fd, int sub_sensor_combined); int read_accelerometer_avg(int sensor_combined_sub, float accel_avg[3], int samples_num); @@ -355,7 +365,7 @@ int mat_invert3(float src[3][3], float dst[3][3]) { float det = src[0][0] * (src[1][1] * src[2][2] - src[1][2] * src[2][1]) - src[0][1] * (src[1][0] * src[2][2] - src[1][2] * src[2][0]) + src[0][2] * (src[1][0] * src[2][1] - src[1][1] * src[2][0]); - if (det == 0.0) + if (det == 0.0f) return ERROR; // Singular matrix dst[0][0] = (src[1][1] * src[2][2] - src[1][2] * src[2][1]) / det; diff --git a/src/modules/commander/accelerometer_calibration.h b/src/modules/commander/accelerometer_calibration.h index 6a920c4ef1..4175a25f4f 100644 --- a/src/modules/commander/accelerometer_calibration.h +++ b/src/modules/commander/accelerometer_calibration.h @@ -9,7 +9,6 @@ #define ACCELEROMETER_CALIBRATION_H_ #include -#include void do_accel_calibration(int mavlink_fd); diff --git a/src/modules/commander/airspeed_calibration.c b/src/modules/commander/airspeed_calibration.c new file mode 100644 index 0000000000..feaf11aee7 --- /dev/null +++ b/src/modules/commander/airspeed_calibration.c @@ -0,0 +1,111 @@ +/**************************************************************************** + * + * 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 airspeed_calibration.c + * Airspeed sensor calibration routine + */ + +#include "airspeed_calibration.h" +#include "commander_helper.h" + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +void do_airspeed_calibration(int mavlink_fd) +{ + /* give directions */ + mavlink_log_info(mavlink_fd, "airspeed calibration starting, keep it still"); + + const int calibration_count = 2500; + + int diff_pres_sub = orb_subscribe(ORB_ID(differential_pressure)); + struct differential_pressure_s diff_pres; + + int calibration_counter = 0; + float diff_pres_offset = 0.0f; + + while (calibration_counter < calibration_count) { + + /* wait blocking for new data */ + struct pollfd fds[1] = { { .fd = diff_pres_sub, .events = POLLIN } }; + + int poll_ret = poll(fds, 1, 1000); + + if (poll_ret) { + orb_copy(ORB_ID(differential_pressure), diff_pres_sub, &diff_pres); + diff_pres_offset += diff_pres.differential_pressure_pa; + calibration_counter++; + + } else if (poll_ret == 0) { + /* any poll failure for 1s is a reason to abort */ + mavlink_log_info(mavlink_fd, "airspeed calibration aborted"); + return; + } + } + + diff_pres_offset = diff_pres_offset / calibration_count; + + if (isfinite(diff_pres_offset)) { + + if (param_set(param_find("SENS_DPRES_OFF"), &(diff_pres_offset))) { + mavlink_log_critical(mavlink_fd, "Setting offs failed!"); + } + + /* auto-save to EEPROM */ + int save_ret = param_save_default(); + + if (save_ret != 0) { + warn("WARNING: auto-save of params to storage failed"); + } + + //char buf[50]; + //sprintf(buf, "[cmd] accel cal: x:%8.4f y:%8.4f z:%8.4f\n", (double)accel_offset[0], (double)accel_offset[1], (double)accel_offset[2]); + //mavlink_log_info(mavlink_fd, buf); + mavlink_log_info(mavlink_fd, "airspeed calibration done"); + + tune_positive(); + + } else { + mavlink_log_info(mavlink_fd, "airspeed calibration FAILED (NaN)"); + } + + close(diff_pres_sub); +} diff --git a/src/modules/commander/airspeed_calibration.h b/src/modules/commander/airspeed_calibration.h new file mode 100644 index 0000000000..92f5651ec1 --- /dev/null +++ b/src/modules/commander/airspeed_calibration.h @@ -0,0 +1,46 @@ +/**************************************************************************** + * + * 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 gyro_calibration.h + * Airspeed sensor calibration routine + */ + +#ifndef AIRSPEED_CALIBRATION_H_ +#define AIRSPEED_CALIBRATION_H_ + +#include + +void do_airspeed_calibration(int mavlink_fd); + +#endif /* AIRSPEED_CALIBRATION_H_ */ \ No newline at end of file diff --git a/src/modules/commander/baro_calibration.c b/src/modules/commander/baro_calibration.c new file mode 100644 index 0000000000..a705947948 --- /dev/null +++ b/src/modules/commander/baro_calibration.c @@ -0,0 +1,54 @@ +/**************************************************************************** + * + * 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 baro_calibration.c + * Barometer calibration routine + */ + +#include "baro_calibration.h" + +#include +#include +#include +#include +#include +#include +#include +#include + +void do_baro_calibration(int mavlink_fd) +{ + // TODO implement this + return; +} diff --git a/src/modules/commander/baro_calibration.h b/src/modules/commander/baro_calibration.h new file mode 100644 index 0000000000..ac0f4be368 --- /dev/null +++ b/src/modules/commander/baro_calibration.h @@ -0,0 +1,46 @@ +/**************************************************************************** + * + * 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 mag_calibration.h + * Barometer calibration routine + */ + +#ifndef BARO_CALIBRATION_H_ +#define BARO_CALIBRATION_H_ + +#include + +void do_baro_calibration(int mavlink_fd); + +#endif /* BARO_CALIBRATION_H_ */ \ No newline at end of file diff --git a/src/modules/commander/calibration_routines.c b/src/modules/commander/calibration_routines.c index a269386378..799cd42697 100644 --- a/src/modules/commander/calibration_routines.c +++ b/src/modules/commander/calibration_routines.c @@ -217,3 +217,4 @@ int sphere_fit_least_squares(const float x[], const float y[], const float z[], return 0; } + diff --git a/src/modules/commander/commander.c b/src/modules/commander/commander.c index 4a5eb23add..41baa34d5d 100644 --- a/src/modules/commander/commander.c +++ b/src/modules/commander/commander.c @@ -91,15 +91,15 @@ #include #include +#include "commander_helper.h" #include "state_machine_helper.h" - -#include -#include -#include -#include - #include "calibration_routines.h" #include "accelerometer_calibration.h" +#include "gyro_calibration.h" +#include "mag_calibration.h" +#include "baro_calibration.h" +#include "rc_calibration.h" +#include "airspeed_calibration.h" PARAM_DEFINE_INT32(SYS_FAILSAVE_LL, 0); /**< Go into low-level failsafe after 0 ms */ //PARAM_DEFINE_INT32(SYS_FAILSAVE_HL, 0); /**< Go into high-level failsafe after 0 ms */ @@ -110,6 +110,9 @@ PARAM_DEFINE_FLOAT(TRIM_YAW, 0.0f); extern struct system_load_s system_load; +#define LOW_VOLTAGE_BATTERY_HYSTERESIS_TIME_MS 1000.0f +#define CRITICAL_VOLTAGE_BATTERY_HYSTERESIS_TIME_MS 100.0f + /* Decouple update interval and hysteris counters, all depends on intervals */ #define COMMANDER_MONITORING_INTERVAL 50000 #define COMMANDER_MONITORING_LOOPSPERMSEC (1/(COMMANDER_MONITORING_INTERVAL/1000.0f)) @@ -130,7 +133,6 @@ extern struct system_load_s system_load; /* File descriptors */ static int leds; -static int buzzer; static int mavlink_fd; /* flags */ @@ -167,27 +169,17 @@ __EXPORT int commander_main(int argc, char *argv[]); */ int commander_thread_main(int argc, char *argv[]); -int buzzer_init(void); -void buzzer_deinit(void); int led_init(void); void led_deinit(void); int led_toggle(int led); int led_on(int led); int led_off(int led); -void tune_error(void); -void tune_positive(void); -void tune_neutral(void); -void tune_negative(void); void do_reboot(void); -void do_gyro_calibration(void); -void do_mag_calibration(void); -void do_rc_calibration(void); -void do_airspeed_calibration(void); void handle_command(int status_pub, struct vehicle_status_s *current_status, struct vehicle_command_s *cmd, int safety_pub, struct actuator_safety_s *safety); -int trigger_audio_alarm(uint8_t old_mode, uint8_t old_state, uint8_t new_mode, uint8_t new_state); +// int trigger_audio_alarm(uint8_t old_mode, uint8_t old_state, uint8_t new_mode, uint8_t new_state); @@ -206,23 +198,6 @@ void usage(const char *reason); */ // static void cal_bsort(float a[], int n); -int buzzer_init() -{ - buzzer = open("/dev/tone_alarm", O_WRONLY); - - if (buzzer < 0) { - warnx("Buzzer: open fail\n"); - return ERROR; - } - - return 0; -} - -void buzzer_deinit() -{ - close(buzzer); -} - int led_init() { @@ -268,27 +243,6 @@ int led_off(int led) return ioctl(leds, LED_OFF, led); } - -void tune_error() -{ - ioctl(buzzer, TONE_SET_ALARM, 2); -} - -void tune_positive() -{ - ioctl(buzzer, TONE_SET_ALARM, 3); -} - -void tune_neutral() -{ - ioctl(buzzer, TONE_SET_ALARM, 4); -} - -void tune_negative() -{ - ioctl(buzzer, TONE_SET_ALARM, 5); -} - void do_reboot() { mavlink_log_critical(mavlink_fd, "REBOOTING SYSTEM"); @@ -298,424 +252,6 @@ void do_reboot() } -void do_rc_calibration() -{ - mavlink_log_info(mavlink_fd, "trim calibration starting"); - - /* XXX fix this */ - // if (current_status.offboard_control_signal_lost) { - // mavlink_log_critical(mavlink_fd, "TRIM CAL: ABORT. No RC signal."); - // return; - // } - - int sub_man = orb_subscribe(ORB_ID(manual_control_setpoint)); - struct manual_control_setpoint_s sp; - orb_copy(ORB_ID(manual_control_setpoint), sub_man, &sp); - - /* set parameters */ - float p = sp.roll; - param_set(param_find("TRIM_ROLL"), &p); - p = sp.pitch; - param_set(param_find("TRIM_PITCH"), &p); - p = sp.yaw; - param_set(param_find("TRIM_YAW"), &p); - - /* store to permanent storage */ - /* auto-save */ - int save_ret = param_save_default(); - - if (save_ret != 0) { - mavlink_log_critical(mavlink_fd, "TRIM CAL: WARN: auto-save of params failed"); - } - - tune_positive(); - - mavlink_log_info(mavlink_fd, "trim calibration done"); -} - -void do_mag_calibration() -{ - mavlink_log_info(mavlink_fd, "mag calibration starting, hold still"); - - int sub_mag = orb_subscribe(ORB_ID(sensor_mag)); - struct mag_report mag; - - /* 45 seconds */ - uint64_t calibration_interval = 45 * 1000 * 1000; - - /* maximum 2000 values */ - const unsigned int calibration_maxcount = 500; - unsigned int calibration_counter = 0; - - /* limit update rate to get equally spaced measurements over time (in ms) */ - orb_set_interval(sub_mag, (calibration_interval / 1000) / calibration_maxcount); - - int fd = open(MAG_DEVICE_PATH, O_RDONLY); - - /* erase old calibration */ - struct mag_scale mscale_null = { - 0.0f, - 1.0f, - 0.0f, - 1.0f, - 0.0f, - 1.0f, - }; - - if (OK != ioctl(fd, MAGIOCSSCALE, (long unsigned int)&mscale_null)) { - warn("WARNING: failed to set scale / offsets for mag"); - mavlink_log_info(mavlink_fd, "failed to set scale / offsets for mag"); - } - - /* calibrate range */ - if (OK != ioctl(fd, MAGIOCCALIBRATE, fd)) { - warnx("failed to calibrate scale"); - } - - close(fd); - - /* calibrate offsets */ - - // uint64_t calibration_start = hrt_absolute_time(); - - uint64_t axis_deadline = hrt_absolute_time(); - uint64_t calibration_deadline = hrt_absolute_time() + calibration_interval; - - const char axislabels[3] = { 'X', 'Y', 'Z'}; - int axis_index = -1; - - float *x = (float *)malloc(sizeof(float) * calibration_maxcount); - float *y = (float *)malloc(sizeof(float) * calibration_maxcount); - float *z = (float *)malloc(sizeof(float) * calibration_maxcount); - - if (x == NULL || y == NULL || z == NULL) { - warnx("mag cal failed: out of memory"); - mavlink_log_info(mavlink_fd, "mag cal failed: out of memory"); - warnx("x:%p y:%p z:%p\n", x, y, z); - return; - } - - while (hrt_absolute_time() < calibration_deadline && - calibration_counter < calibration_maxcount) { - - /* wait blocking for new data */ - struct pollfd fds[1] = { { .fd = sub_mag, .events = POLLIN } }; - - /* user guidance */ - if (hrt_absolute_time() >= axis_deadline && - axis_index < 3) { - - axis_index++; - - char buf[50]; - sprintf(buf, "please rotate around %c", axislabels[axis_index]); - mavlink_log_info(mavlink_fd, buf); - tune_neutral(); - - axis_deadline += calibration_interval / 3; - } - - if (!(axis_index < 3)) { - break; - } - - // int axis_left = (int64_t)axis_deadline - (int64_t)hrt_absolute_time(); - - // if ((axis_left / 1000) == 0 && axis_left > 0) { - // char buf[50]; - // sprintf(buf, "[cmd] %d seconds left for axis %c", axis_left, axislabels[axis_index]); - // mavlink_log_info(mavlink_fd, buf); - // } - - int poll_ret = poll(fds, 1, 1000); - - if (poll_ret) { - orb_copy(ORB_ID(sensor_mag), sub_mag, &mag); - - x[calibration_counter] = mag.x; - y[calibration_counter] = mag.y; - z[calibration_counter] = mag.z; - - /* get min/max values */ - - // if (mag.x < mag_min[0]) { - // mag_min[0] = mag.x; - // } - // else if (mag.x > mag_max[0]) { - // mag_max[0] = mag.x; - // } - - // if (raw.magnetometer_ga[1] < mag_min[1]) { - // mag_min[1] = raw.magnetometer_ga[1]; - // } - // else if (raw.magnetometer_ga[1] > mag_max[1]) { - // mag_max[1] = raw.magnetometer_ga[1]; - // } - - // if (raw.magnetometer_ga[2] < mag_min[2]) { - // mag_min[2] = raw.magnetometer_ga[2]; - // } - // else if (raw.magnetometer_ga[2] > mag_max[2]) { - // mag_max[2] = raw.magnetometer_ga[2]; - // } - - calibration_counter++; - - } else if (poll_ret == 0) { - /* any poll failure for 1s is a reason to abort */ - mavlink_log_info(mavlink_fd, "mag cal canceled (timed out)"); - break; - } - } - - float sphere_x; - float sphere_y; - float sphere_z; - float sphere_radius; - - sphere_fit_least_squares(x, y, z, calibration_counter, 100, 0.0f, &sphere_x, &sphere_y, &sphere_z, &sphere_radius); - - free(x); - free(y); - free(z); - - if (isfinite(sphere_x) && isfinite(sphere_y) && isfinite(sphere_z)) { - - fd = open(MAG_DEVICE_PATH, 0); - - struct mag_scale mscale; - - if (OK != ioctl(fd, MAGIOCGSCALE, (long unsigned int)&mscale)) - warn("WARNING: failed to get scale / offsets for mag"); - - mscale.x_offset = sphere_x; - mscale.y_offset = sphere_y; - mscale.z_offset = sphere_z; - - if (OK != ioctl(fd, MAGIOCSSCALE, (long unsigned int)&mscale)) - warn("WARNING: failed to set scale / offsets for mag"); - - close(fd); - - /* announce and set new offset */ - - if (param_set(param_find("SENS_MAG_XOFF"), &(mscale.x_offset))) { - warnx("Setting X mag offset failed!\n"); - } - - if (param_set(param_find("SENS_MAG_YOFF"), &(mscale.y_offset))) { - warnx("Setting Y mag offset failed!\n"); - } - - if (param_set(param_find("SENS_MAG_ZOFF"), &(mscale.z_offset))) { - warnx("Setting Z mag offset failed!\n"); - } - - if (param_set(param_find("SENS_MAG_XSCALE"), &(mscale.x_scale))) { - warnx("Setting X mag scale failed!\n"); - } - - if (param_set(param_find("SENS_MAG_YSCALE"), &(mscale.y_scale))) { - warnx("Setting Y mag scale failed!\n"); - } - - if (param_set(param_find("SENS_MAG_ZSCALE"), &(mscale.z_scale))) { - warnx("Setting Z mag scale failed!\n"); - } - - /* auto-save to EEPROM */ - int save_ret = param_save_default(); - - if (save_ret != 0) { - warn("WARNING: auto-save of params to storage failed"); - mavlink_log_info(mavlink_fd, "FAILED storing calibration"); - } - - warnx("\tscale: %.6f %.6f %.6f\n \toffset: %.6f %.6f %.6f\nradius: %.6f GA\n", - (double)mscale.x_scale, (double)mscale.y_scale, (double)mscale.z_scale, - (double)mscale.x_offset, (double)mscale.y_offset, (double)mscale.z_offset, (double)sphere_radius); - - char buf[52]; - sprintf(buf, "mag off: x:%.2f y:%.2f z:%.2f Ga", (double)mscale.x_offset, - (double)mscale.y_offset, (double)mscale.z_offset); - mavlink_log_info(mavlink_fd, buf); - - sprintf(buf, "mag scale: x:%.2f y:%.2f z:%.2f", (double)mscale.x_scale, - (double)mscale.y_scale, (double)mscale.z_scale); - mavlink_log_info(mavlink_fd, buf); - - mavlink_log_info(mavlink_fd, "mag calibration done"); - - tune_positive(); - /* third beep by cal end routine */ - - } else { - mavlink_log_info(mavlink_fd, "mag calibration FAILED (NaN in sphere fit)"); - } - - close(sub_mag); -} - -void do_gyro_calibration() -{ - mavlink_log_info(mavlink_fd, "gyro calibration starting, hold still"); - - const int calibration_count = 5000; - - int sub_sensor_combined = orb_subscribe(ORB_ID(sensor_combined)); - struct sensor_combined_s raw; - - int calibration_counter = 0; - float gyro_offset[3] = {0.0f, 0.0f, 0.0f}; - - /* set offsets to zero */ - int fd = open(GYRO_DEVICE_PATH, 0); - struct gyro_scale gscale_null = { - 0.0f, - 1.0f, - 0.0f, - 1.0f, - 0.0f, - 1.0f, - }; - - if (OK != ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gscale_null)) - warn("WARNING: failed to set scale / offsets for gyro"); - - close(fd); - - while (calibration_counter < calibration_count) { - - /* wait blocking for new data */ - struct pollfd fds[1] = { { .fd = sub_sensor_combined, .events = POLLIN } }; - - int poll_ret = poll(fds, 1, 1000); - - if (poll_ret) { - orb_copy(ORB_ID(sensor_combined), sub_sensor_combined, &raw); - gyro_offset[0] += raw.gyro_rad_s[0]; - gyro_offset[1] += raw.gyro_rad_s[1]; - gyro_offset[2] += raw.gyro_rad_s[2]; - calibration_counter++; - - } else if (poll_ret == 0) { - /* any poll failure for 1s is a reason to abort */ - mavlink_log_info(mavlink_fd, "gyro calibration aborted, retry"); - return; - } - } - - gyro_offset[0] = gyro_offset[0] / calibration_count; - gyro_offset[1] = gyro_offset[1] / calibration_count; - gyro_offset[2] = gyro_offset[2] / calibration_count; - - if (isfinite(gyro_offset[0]) && isfinite(gyro_offset[1]) && isfinite(gyro_offset[2])) { - - if (param_set(param_find("SENS_GYRO_XOFF"), &(gyro_offset[0])) - || param_set(param_find("SENS_GYRO_YOFF"), &(gyro_offset[1])) - || param_set(param_find("SENS_GYRO_ZOFF"), &(gyro_offset[2]))) { - mavlink_log_critical(mavlink_fd, "Setting gyro offsets failed!"); - } - - /* set offsets to actual value */ - fd = open(GYRO_DEVICE_PATH, 0); - struct gyro_scale gscale = { - gyro_offset[0], - 1.0f, - gyro_offset[1], - 1.0f, - gyro_offset[2], - 1.0f, - }; - - if (OK != ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gscale)) - warn("WARNING: failed to set scale / offsets for gyro"); - - close(fd); - - /* auto-save to EEPROM */ - int save_ret = param_save_default(); - - if (save_ret != 0) { - warn("WARNING: auto-save of params to storage failed"); - } - - // char buf[50]; - // sprintf(buf, "cal: x:%8.4f y:%8.4f z:%8.4f", (double)gyro_offset[0], (double)gyro_offset[1], (double)gyro_offset[2]); - // mavlink_log_info(mavlink_fd, buf); - mavlink_log_info(mavlink_fd, "gyro calibration done"); - - tune_positive(); - /* third beep by cal end routine */ - - } else { - mavlink_log_info(mavlink_fd, "gyro calibration FAILED (NaN)"); - } - - close(sub_sensor_combined); -} - - -void do_airspeed_calibration() -{ - /* give directions */ - mavlink_log_info(mavlink_fd, "airspeed calibration starting, keep it still"); - - const int calibration_count = 2500; - - int diff_pres_sub = orb_subscribe(ORB_ID(differential_pressure)); - struct differential_pressure_s diff_pres; - - int calibration_counter = 0; - float diff_pres_offset = 0.0f; - - while (calibration_counter < calibration_count) { - - /* wait blocking for new data */ - struct pollfd fds[1] = { { .fd = diff_pres_sub, .events = POLLIN } }; - - int poll_ret = poll(fds, 1, 1000); - - if (poll_ret) { - orb_copy(ORB_ID(differential_pressure), diff_pres_sub, &diff_pres); - diff_pres_offset += diff_pres.differential_pressure_pa; - calibration_counter++; - - } else if (poll_ret == 0) { - /* any poll failure for 1s is a reason to abort */ - mavlink_log_info(mavlink_fd, "airspeed calibration aborted"); - return; - } - } - - diff_pres_offset = diff_pres_offset / calibration_count; - - if (isfinite(diff_pres_offset)) { - - if (param_set(param_find("SENS_DPRES_OFF"), &(diff_pres_offset))) { - mavlink_log_critical(mavlink_fd, "Setting offs failed!"); - } - - /* auto-save to EEPROM */ - int save_ret = param_save_default(); - - if (save_ret != 0) { - warn("WARNING: auto-save of params to storage failed"); - } - - //char buf[50]; - //sprintf(buf, "[cmd] accel cal: x:%8.4f y:%8.4f z:%8.4f\n", (double)accel_offset[0], (double)accel_offset[1], (double)accel_offset[2]); - //mavlink_log_info(mavlink_fd, buf); - mavlink_log_info(mavlink_fd, "airspeed calibration done"); - - tune_positive(); - - } else { - mavlink_log_info(mavlink_fd, "airspeed calibration FAILED (NaN)"); - } - - close(diff_pres_sub); -} void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_status, struct vehicle_command_s *cmd, int safety_pub, struct actuator_safety_s *safety) { @@ -1182,7 +718,7 @@ int commander_thread_main(int argc, char *argv[]) warnx("ERROR: Failed to initialize leds"); } - if (buzzer_init() != 0) { + if (buzzer_init() != OK) { warnx("ERROR: Failed to initialize buzzer"); } @@ -1246,7 +782,12 @@ int commander_thread_main(int argc, char *argv[]) /* advertise to ORB */ status_pub = orb_advertise(ORB_ID(vehicle_status), ¤t_status); /* publish current state machine */ - state_machine_publish(status_pub, ¤t_status, mavlink_fd); + + /* publish the new state */ + current_status.counter++; + current_status.timestamp = hrt_absolute_time(); + orb_publish(ORB_ID(vehicle_status), status_pub, ¤t_status); + safety_pub = orb_advertise(ORB_ID(actuator_safety), &safety); @@ -1715,7 +1256,7 @@ int commander_thread_main(int argc, char *argv[]) // state_changed = true; // } - orb_check(ORB_ID(vehicle_gps_position), &new_data); + orb_check(gps_sub, &new_data); if (new_data) { @@ -2222,18 +1763,18 @@ int commander_thread_main(int argc, char *argv[]) /* play tone according to evaluation result */ /* check if we recently armed */ if (!arm_tune_played && safety.armed && ( !safety.safety_switch_available || (safety.safety_off && safety.safety_switch_available))) { - if (ioctl(buzzer, TONE_SET_ALARM, 12) == OK) + if (tune_arm() == OK) arm_tune_played = true; /* Trigger audio event for low battery */ } else if (bat_remain < 0.1f && battery_voltage_valid) { - if (ioctl(buzzer, TONE_SET_ALARM, 14) == OK) + if (tune_critical_bat() == OK) battery_tune_played = true; } else if (bat_remain < 0.2f && battery_voltage_valid) { - if (ioctl(buzzer, TONE_SET_ALARM, 13) == OK) + if (tune_low_bat() == OK) battery_tune_played = true; } else if(battery_tune_played) { - ioctl(buzzer, TONE_SET_ALARM, 0); + tune_stop(); battery_tune_played = false; } @@ -2305,25 +1846,25 @@ void *commander_low_prio_loop(void *arg) case LOW_PRIO_TASK_GYRO_CALIBRATION: - do_gyro_calibration(); + do_gyro_calibration(mavlink_fd); low_prio_task = LOW_PRIO_TASK_NONE; break; case LOW_PRIO_TASK_MAG_CALIBRATION: - do_mag_calibration(); + do_mag_calibration(mavlink_fd); low_prio_task = LOW_PRIO_TASK_NONE; break; case LOW_PRIO_TASK_ALTITUDE_CALIBRATION: - // do_baro_calibration(); + // do_baro_calibration(mavlink_fd); case LOW_PRIO_TASK_RC_CALIBRATION: - // do_rc_calibration(); + // do_rc_calibration(mavlink_fd); low_prio_task = LOW_PRIO_TASK_NONE; break; @@ -2337,7 +1878,7 @@ void *commander_low_prio_loop(void *arg) case LOW_PRIO_TASK_AIRSPEED_CALIBRATION: - do_airspeed_calibration(); + do_airspeed_calibration(mavlink_fd); low_prio_task = LOW_PRIO_TASK_NONE; break; diff --git a/src/modules/commander/commander.h b/src/modules/commander/commander.h index 04b4e72ab3..6e57c0ba5f 100644 --- a/src/modules/commander/commander.h +++ b/src/modules/commander/commander.h @@ -49,10 +49,6 @@ #ifndef COMMANDER_H_ #define COMMANDER_H_ -#define LOW_VOLTAGE_BATTERY_HYSTERESIS_TIME_MS 1000.0f -#define CRITICAL_VOLTAGE_BATTERY_HYSTERESIS_TIME_MS 100.0f -void tune_confirm(void); -void tune_error(void); #endif /* COMMANDER_H_ */ diff --git a/src/modules/commander/commander_helper.c b/src/modules/commander/commander_helper.c new file mode 100644 index 0000000000..a75b5dec3c --- /dev/null +++ b/src/modules/commander/commander_helper.c @@ -0,0 +1,129 @@ +/**************************************************************************** + * + * Copyright (C) 2013 PX4 Development Team. All rights reserved. + * Author: Thomas Gubler + * Julian Oes + * + * 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 commander_helper.c + * Commander helper functions implementations + */ + +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +#include "commander_helper.h" + +bool is_multirotor(const struct vehicle_status_s *current_status) +{ + return ((current_status->system_type == VEHICLE_TYPE_QUADROTOR) || + (current_status->system_type == VEHICLE_TYPE_HEXAROTOR) || + (current_status->system_type == VEHICLE_TYPE_OCTOROTOR) || + (current_status->system_type == VEHICLE_TYPE_TRICOPTER)); +} + +bool is_rotary_wing(const struct vehicle_status_s *current_status) +{ + return is_multirotor(current_status) || (current_status->system_type == VEHICLE_TYPE_HELICOPTER) + || (current_status->system_type == VEHICLE_TYPE_COAXIAL); +} + +static int buzzer; + +int buzzer_init() +{ + buzzer = open("/dev/tone_alarm", O_WRONLY); + + if (buzzer < 0) { + warnx("Buzzer: open fail\n"); + return ERROR; + } + + return OK; +} + +void buzzer_deinit() +{ + close(buzzer); +} + +void tune_error() +{ + ioctl(buzzer, TONE_SET_ALARM, 2); +} + +void tune_positive() +{ + ioctl(buzzer, TONE_SET_ALARM, 3); +} + +void tune_neutral() +{ + ioctl(buzzer, TONE_SET_ALARM, 4); +} + +void tune_negative() +{ + ioctl(buzzer, TONE_SET_ALARM, 5); +} + +int tune_arm() +{ + return ioctl(buzzer, TONE_SET_ALARM, 12); +} + +int tune_critical_bat() +{ + return ioctl(buzzer, TONE_SET_ALARM, 14); +} + +int tune_low_bat() +{ + return ioctl(buzzer, TONE_SET_ALARM, 13); +} + +void tune_stop() +{ + ioctl(buzzer, TONE_SET_ALARM, 0); +} + diff --git a/src/modules/commander/commander_helper.h b/src/modules/commander/commander_helper.h new file mode 100644 index 0000000000..ea96d72a6a --- /dev/null +++ b/src/modules/commander/commander_helper.h @@ -0,0 +1,66 @@ +/**************************************************************************** + * + * Copyright (C) 2013 PX4 Development Team. All rights reserved. + * Author: Thomas Gubler + * Julian Oes + * + * 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 commander_helper.h + * Commander helper functions definitions + */ + +#ifndef COMMANDER_HELPER_H_ +#define COMMANDER_HELPER_H_ + +#include +#include +#include +#include + + +bool is_multirotor(const struct vehicle_status_s *current_status); +bool is_rotary_wing(const struct vehicle_status_s *current_status); + +int buzzer_init(void); +void buzzer_deinit(void); + +void tune_error(void); +void tune_positive(void); +void tune_neutral(void); +void tune_negative(void); +int tune_arm(void); +int tune_critical_bat(void); +int tune_low_bat(void); + +void tune_stop(void); + +#endif /* COMMANDER_HELPER_H_ */ diff --git a/src/modules/commander/gyro_calibration.c b/src/modules/commander/gyro_calibration.c new file mode 100644 index 0000000000..f452910c9a --- /dev/null +++ b/src/modules/commander/gyro_calibration.c @@ -0,0 +1,151 @@ +/**************************************************************************** + * + * 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 gyro_calibration.c + * Gyroscope calibration routine + */ + +#include "gyro_calibration.h" +#include "commander_helper.h" + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + + +void do_gyro_calibration(int mavlink_fd) +{ + mavlink_log_info(mavlink_fd, "gyro calibration starting, hold still"); + + const int calibration_count = 5000; + + int sub_sensor_combined = orb_subscribe(ORB_ID(sensor_combined)); + struct sensor_combined_s raw; + + int calibration_counter = 0; + float gyro_offset[3] = {0.0f, 0.0f, 0.0f}; + + /* set offsets to zero */ + int fd = open(GYRO_DEVICE_PATH, 0); + struct gyro_scale gscale_null = { + 0.0f, + 1.0f, + 0.0f, + 1.0f, + 0.0f, + 1.0f, + }; + + if (OK != ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gscale_null)) + warn("WARNING: failed to set scale / offsets for gyro"); + + close(fd); + + while (calibration_counter < calibration_count) { + + /* wait blocking for new data */ + struct pollfd fds[1] = { { .fd = sub_sensor_combined, .events = POLLIN } }; + + int poll_ret = poll(fds, 1, 1000); + + if (poll_ret) { + orb_copy(ORB_ID(sensor_combined), sub_sensor_combined, &raw); + gyro_offset[0] += raw.gyro_rad_s[0]; + gyro_offset[1] += raw.gyro_rad_s[1]; + gyro_offset[2] += raw.gyro_rad_s[2]; + calibration_counter++; + + } else if (poll_ret == 0) { + /* any poll failure for 1s is a reason to abort */ + mavlink_log_info(mavlink_fd, "gyro calibration aborted, retry"); + return; + } + } + + gyro_offset[0] = gyro_offset[0] / calibration_count; + gyro_offset[1] = gyro_offset[1] / calibration_count; + gyro_offset[2] = gyro_offset[2] / calibration_count; + + if (isfinite(gyro_offset[0]) && isfinite(gyro_offset[1]) && isfinite(gyro_offset[2])) { + + if (param_set(param_find("SENS_GYRO_XOFF"), &(gyro_offset[0])) + || param_set(param_find("SENS_GYRO_YOFF"), &(gyro_offset[1])) + || param_set(param_find("SENS_GYRO_ZOFF"), &(gyro_offset[2]))) { + mavlink_log_critical(mavlink_fd, "Setting gyro offsets failed!"); + } + + /* set offsets to actual value */ + fd = open(GYRO_DEVICE_PATH, 0); + struct gyro_scale gscale = { + gyro_offset[0], + 1.0f, + gyro_offset[1], + 1.0f, + gyro_offset[2], + 1.0f, + }; + + if (OK != ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gscale)) + warn("WARNING: failed to set scale / offsets for gyro"); + + close(fd); + + /* auto-save to EEPROM */ + int save_ret = param_save_default(); + + if (save_ret != 0) { + warn("WARNING: auto-save of params to storage failed"); + } + + // char buf[50]; + // sprintf(buf, "cal: x:%8.4f y:%8.4f z:%8.4f", (double)gyro_offset[0], (double)gyro_offset[1], (double)gyro_offset[2]); + // mavlink_log_info(mavlink_fd, buf); + mavlink_log_info(mavlink_fd, "gyro calibration done"); + + tune_positive(); + /* third beep by cal end routine */ + + } else { + mavlink_log_info(mavlink_fd, "gyro calibration FAILED (NaN)"); + } + + close(sub_sensor_combined); +} diff --git a/src/modules/commander/gyro_calibration.h b/src/modules/commander/gyro_calibration.h new file mode 100644 index 0000000000..cd262507db --- /dev/null +++ b/src/modules/commander/gyro_calibration.h @@ -0,0 +1,46 @@ +/**************************************************************************** + * + * 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 gyro_calibration.h + * Gyroscope calibration routine + */ + +#ifndef GYRO_CALIBRATION_H_ +#define GYRO_CALIBRATION_H_ + +#include + +void do_gyro_calibration(int mavlink_fd); + +#endif /* GYRO_CALIBRATION_H_ */ \ No newline at end of file diff --git a/src/modules/commander/mag_calibration.c b/src/modules/commander/mag_calibration.c new file mode 100644 index 0000000000..dbd31a7e75 --- /dev/null +++ b/src/modules/commander/mag_calibration.c @@ -0,0 +1,278 @@ +/**************************************************************************** + * + * 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 mag_calibration.c + * Magnetometer calibration routine + */ + +#include "mag_calibration.h" +#include "commander_helper.h" +#include "calibration_routines.h" + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + + +void do_mag_calibration(int mavlink_fd) +{ + mavlink_log_info(mavlink_fd, "mag calibration starting, hold still"); + + int sub_mag = orb_subscribe(ORB_ID(sensor_mag)); + struct mag_report mag; + + /* 45 seconds */ + uint64_t calibration_interval = 45 * 1000 * 1000; + + /* maximum 2000 values */ + const unsigned int calibration_maxcount = 500; + unsigned int calibration_counter = 0; + + /* limit update rate to get equally spaced measurements over time (in ms) */ + orb_set_interval(sub_mag, (calibration_interval / 1000) / calibration_maxcount); + + int fd = open(MAG_DEVICE_PATH, O_RDONLY); + + /* erase old calibration */ + struct mag_scale mscale_null = { + 0.0f, + 1.0f, + 0.0f, + 1.0f, + 0.0f, + 1.0f, + }; + + if (OK != ioctl(fd, MAGIOCSSCALE, (long unsigned int)&mscale_null)) { + warn("WARNING: failed to set scale / offsets for mag"); + mavlink_log_info(mavlink_fd, "failed to set scale / offsets for mag"); + } + + /* calibrate range */ + if (OK != ioctl(fd, MAGIOCCALIBRATE, fd)) { + warnx("failed to calibrate scale"); + } + + close(fd); + + /* calibrate offsets */ + + // uint64_t calibration_start = hrt_absolute_time(); + + uint64_t axis_deadline = hrt_absolute_time(); + uint64_t calibration_deadline = hrt_absolute_time() + calibration_interval; + + const char axislabels[3] = { 'X', 'Y', 'Z'}; + int axis_index = -1; + + float *x = (float *)malloc(sizeof(float) * calibration_maxcount); + float *y = (float *)malloc(sizeof(float) * calibration_maxcount); + float *z = (float *)malloc(sizeof(float) * calibration_maxcount); + + if (x == NULL || y == NULL || z == NULL) { + warnx("mag cal failed: out of memory"); + mavlink_log_info(mavlink_fd, "mag cal failed: out of memory"); + warnx("x:%p y:%p z:%p\n", x, y, z); + return; + } + + while (hrt_absolute_time() < calibration_deadline && + calibration_counter < calibration_maxcount) { + + /* wait blocking for new data */ + struct pollfd fds[1] = { { .fd = sub_mag, .events = POLLIN } }; + + /* user guidance */ + if (hrt_absolute_time() >= axis_deadline && + axis_index < 3) { + + axis_index++; + + char buf[50]; + sprintf(buf, "please rotate around %c", axislabels[axis_index]); + mavlink_log_info(mavlink_fd, buf); + tune_neutral(); + + axis_deadline += calibration_interval / 3; + } + + if (!(axis_index < 3)) { + break; + } + + // int axis_left = (int64_t)axis_deadline - (int64_t)hrt_absolute_time(); + + // if ((axis_left / 1000) == 0 && axis_left > 0) { + // char buf[50]; + // sprintf(buf, "[cmd] %d seconds left for axis %c", axis_left, axislabels[axis_index]); + // mavlink_log_info(mavlink_fd, buf); + // } + + int poll_ret = poll(fds, 1, 1000); + + if (poll_ret) { + orb_copy(ORB_ID(sensor_mag), sub_mag, &mag); + + x[calibration_counter] = mag.x; + y[calibration_counter] = mag.y; + z[calibration_counter] = mag.z; + + /* get min/max values */ + + // if (mag.x < mag_min[0]) { + // mag_min[0] = mag.x; + // } + // else if (mag.x > mag_max[0]) { + // mag_max[0] = mag.x; + // } + + // if (raw.magnetometer_ga[1] < mag_min[1]) { + // mag_min[1] = raw.magnetometer_ga[1]; + // } + // else if (raw.magnetometer_ga[1] > mag_max[1]) { + // mag_max[1] = raw.magnetometer_ga[1]; + // } + + // if (raw.magnetometer_ga[2] < mag_min[2]) { + // mag_min[2] = raw.magnetometer_ga[2]; + // } + // else if (raw.magnetometer_ga[2] > mag_max[2]) { + // mag_max[2] = raw.magnetometer_ga[2]; + // } + + calibration_counter++; + + } else if (poll_ret == 0) { + /* any poll failure for 1s is a reason to abort */ + mavlink_log_info(mavlink_fd, "mag cal canceled (timed out)"); + break; + } + } + + float sphere_x; + float sphere_y; + float sphere_z; + float sphere_radius; + + sphere_fit_least_squares(x, y, z, calibration_counter, 100, 0.0f, &sphere_x, &sphere_y, &sphere_z, &sphere_radius); + + free(x); + free(y); + free(z); + + if (isfinite(sphere_x) && isfinite(sphere_y) && isfinite(sphere_z)) { + + fd = open(MAG_DEVICE_PATH, 0); + + struct mag_scale mscale; + + if (OK != ioctl(fd, MAGIOCGSCALE, (long unsigned int)&mscale)) + warn("WARNING: failed to get scale / offsets for mag"); + + mscale.x_offset = sphere_x; + mscale.y_offset = sphere_y; + mscale.z_offset = sphere_z; + + if (OK != ioctl(fd, MAGIOCSSCALE, (long unsigned int)&mscale)) + warn("WARNING: failed to set scale / offsets for mag"); + + close(fd); + + /* announce and set new offset */ + + if (param_set(param_find("SENS_MAG_XOFF"), &(mscale.x_offset))) { + warnx("Setting X mag offset failed!\n"); + } + + if (param_set(param_find("SENS_MAG_YOFF"), &(mscale.y_offset))) { + warnx("Setting Y mag offset failed!\n"); + } + + if (param_set(param_find("SENS_MAG_ZOFF"), &(mscale.z_offset))) { + warnx("Setting Z mag offset failed!\n"); + } + + if (param_set(param_find("SENS_MAG_XSCALE"), &(mscale.x_scale))) { + warnx("Setting X mag scale failed!\n"); + } + + if (param_set(param_find("SENS_MAG_YSCALE"), &(mscale.y_scale))) { + warnx("Setting Y mag scale failed!\n"); + } + + if (param_set(param_find("SENS_MAG_ZSCALE"), &(mscale.z_scale))) { + warnx("Setting Z mag scale failed!\n"); + } + + /* auto-save to EEPROM */ + int save_ret = param_save_default(); + + if (save_ret != 0) { + warn("WARNING: auto-save of params to storage failed"); + mavlink_log_info(mavlink_fd, "FAILED storing calibration"); + } + + warnx("\tscale: %.6f %.6f %.6f\n \toffset: %.6f %.6f %.6f\nradius: %.6f GA\n", + (double)mscale.x_scale, (double)mscale.y_scale, (double)mscale.z_scale, + (double)mscale.x_offset, (double)mscale.y_offset, (double)mscale.z_offset, (double)sphere_radius); + + char buf[52]; + sprintf(buf, "mag off: x:%.2f y:%.2f z:%.2f Ga", (double)mscale.x_offset, + (double)mscale.y_offset, (double)mscale.z_offset); + mavlink_log_info(mavlink_fd, buf); + + sprintf(buf, "mag scale: x:%.2f y:%.2f z:%.2f", (double)mscale.x_scale, + (double)mscale.y_scale, (double)mscale.z_scale); + mavlink_log_info(mavlink_fd, buf); + + mavlink_log_info(mavlink_fd, "mag calibration done"); + + tune_positive(); + /* third beep by cal end routine */ + + } else { + mavlink_log_info(mavlink_fd, "mag calibration FAILED (NaN in sphere fit)"); + } + + close(sub_mag); +} \ No newline at end of file diff --git a/src/modules/commander/mag_calibration.h b/src/modules/commander/mag_calibration.h new file mode 100644 index 0000000000..fd2085f14a --- /dev/null +++ b/src/modules/commander/mag_calibration.h @@ -0,0 +1,46 @@ +/**************************************************************************** + * + * 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 mag_calibration.h + * Magnetometer calibration routine + */ + +#ifndef MAG_CALIBRATION_H_ +#define MAG_CALIBRATION_H_ + +#include + +void do_mag_calibration(int mavlink_fd); + +#endif /* MAG_CALIBRATION_H_ */ \ No newline at end of file diff --git a/src/modules/commander/module.mk b/src/modules/commander/module.mk index fe44e955ad..fef8e366b2 100644 --- a/src/modules/commander/module.mk +++ b/src/modules/commander/module.mk @@ -38,6 +38,12 @@ MODULE_COMMAND = commander SRCS = commander.c \ state_machine_helper.c \ + commander_helper.c \ calibration_routines.c \ - accelerometer_calibration.c + accelerometer_calibration.c \ + gyro_calibration.c \ + mag_calibration.c \ + baro_calibration.c \ + rc_calibration.c \ + airspeed_calibration.c diff --git a/src/modules/commander/rc_calibration.c b/src/modules/commander/rc_calibration.c new file mode 100644 index 0000000000..9aa6b86fe1 --- /dev/null +++ b/src/modules/commander/rc_calibration.c @@ -0,0 +1,83 @@ +/**************************************************************************** + * + * 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 rc_calibration.c + * Remote Control calibration routine + */ + +#include "rc_calibration.h" +#include "commander_helper.h" + +#include +#include +#include +#include +#include +#include + + +void do_rc_calibration(int mavlink_fd) +{ + mavlink_log_info(mavlink_fd, "trim calibration starting"); + + /* XXX fix this */ + // if (current_status.offboard_control_signal_lost) { + // mavlink_log_critical(mavlink_fd, "TRIM CAL: ABORT. No RC signal."); + // return; + // } + + int sub_man = orb_subscribe(ORB_ID(manual_control_setpoint)); + struct manual_control_setpoint_s sp; + orb_copy(ORB_ID(manual_control_setpoint), sub_man, &sp); + + /* set parameters */ + float p = sp.roll; + param_set(param_find("TRIM_ROLL"), &p); + p = sp.pitch; + param_set(param_find("TRIM_PITCH"), &p); + p = sp.yaw; + param_set(param_find("TRIM_YAW"), &p); + + /* store to permanent storage */ + /* auto-save */ + int save_ret = param_save_default(); + + if (save_ret != 0) { + mavlink_log_critical(mavlink_fd, "TRIM CAL: WARN: auto-save of params failed"); + } + + tune_positive(); + + mavlink_log_info(mavlink_fd, "trim calibration done"); +} \ No newline at end of file diff --git a/src/modules/commander/rc_calibration.h b/src/modules/commander/rc_calibration.h new file mode 100644 index 0000000000..6505c73647 --- /dev/null +++ b/src/modules/commander/rc_calibration.h @@ -0,0 +1,46 @@ +/**************************************************************************** + * + * 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 rc_calibration.h + * Remote Control calibration routine + */ + +#ifndef RC_CALIBRATION_H_ +#define RC_CALIBRATION_H_ + +#include + +void do_rc_calibration(int mavlink_fd); + +#endif /* RC_CALIBRATION_H_ */ \ No newline at end of file diff --git a/src/modules/commander/state_machine_helper.c b/src/modules/commander/state_machine_helper.c index 87aad6270c..c15fc91a05 100644 --- a/src/modules/commander/state_machine_helper.c +++ b/src/modules/commander/state_machine_helper.c @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Copyright (C) 2013 PX4 Development Team. All rights reserved. * Author: Thomas Gubler * Julian Oes * @@ -54,21 +54,8 @@ #include #include "state_machine_helper.h" -#include "commander.h" +#include "commander_helper.h" -bool is_multirotor(const struct vehicle_status_s *current_status) -{ - return ((current_status->system_type == VEHICLE_TYPE_QUADROTOR) || - (current_status->system_type == VEHICLE_TYPE_HEXAROTOR) || - (current_status->system_type == VEHICLE_TYPE_OCTOROTOR) || - (current_status->system_type == VEHICLE_TYPE_TRICOPTER)); -} - -bool is_rotary_wing(const struct vehicle_status_s *current_status) -{ - return is_multirotor(current_status) || (current_status->system_type == VEHICLE_TYPE_HELICOPTER) - || (current_status->system_type == VEHICLE_TYPE_COAXIAL); -} int arming_state_transition(int status_pub, struct vehicle_status_s *current_state, arming_state_t new_arming_state, int safety_pub, struct actuator_safety_s *safety, const int mavlink_fd) { @@ -568,7 +555,11 @@ int hil_state_transition(int status_pub, struct vehicle_status_s *current_status if (valid_transition) { current_status->hil_state = new_state; - state_machine_publish(status_pub, current_status, mavlink_fd); + + current_status->counter++; + current_status->timestamp = hrt_absolute_time(); + + orb_publish(ORB_ID(vehicle_status), status_pub, current_status); ret = OK; } else { mavlink_log_critical(mavlink_fd, "REJECTING invalid hil state transition"); @@ -579,50 +570,6 @@ int hil_state_transition(int status_pub, struct vehicle_status_s *current_status -void state_machine_publish(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd) -{ - /* publish the new state */ - current_status->counter++; - current_status->timestamp = hrt_absolute_time(); - - /* assemble state vector based on flag values */ -// if (current_status->flag_control_rates_enabled) { -// current_status->onboard_control_sensors_present |= 0x400; -// -// } else { -// current_status->onboard_control_sensors_present &= ~0x400; -// } - -// current_status->onboard_control_sensors_present |= (current_status->flag_control_attitude_enabled) ? 0x800 : 0; -// current_status->onboard_control_sensors_present |= (current_status->flag_control_attitude_enabled) ? 0x1000 : 0; -// current_status->onboard_control_sensors_present |= (current_status->flag_control_velocity_enabled || current_status->flag_control_position_enabled) ? 0x2000 : 0; -// current_status->onboard_control_sensors_present |= (current_status->flag_control_velocity_enabled || current_status->flag_control_position_enabled) ? 0x4000 : 0; -// -// current_status->onboard_control_sensors_enabled |= (current_status->flag_control_rates_enabled) ? 0x400 : 0; -// current_status->onboard_control_sensors_enabled |= (current_status->flag_control_attitude_enabled) ? 0x800 : 0; -// current_status->onboard_control_sensors_enabled |= (current_status->flag_control_attitude_enabled) ? 0x1000 : 0; -// current_status->onboard_control_sensors_enabled |= (current_status->flag_control_velocity_enabled || current_status->flag_control_position_enabled) ? 0x2000 : 0; -// current_status->onboard_control_sensors_enabled |= (current_status->flag_control_velocity_enabled || current_status->flag_control_position_enabled) ? 0x4000 : 0; - - orb_publish(ORB_ID(vehicle_status), status_pub, current_status); -} - -// void publish_armed_status(const struct vehicle_status_s *current_status) -// { -// struct actuator_armed_s armed; -// armed.armed = current_status->flag_system_armed; -// -// /* XXX allow arming by external components on multicopters only if not yet armed by RC */ -// /* XXX allow arming only if core sensors are ok */ -// armed.ready_to_arm = true; -// -// /* lock down actuators if required, only in HIL */ -// armed.lockdown = (current_status->flag_hil_enabled) ? true : false; -// orb_advert_t armed_pub = orb_advertise(ORB_ID(actuator_armed), &armed); -// orb_publish(ORB_ID(actuator_armed), armed_pub, &armed); -// } - - // /* // * Wrapper functions (to be used in the commander), all functions assume lock on current_status // */ @@ -805,3 +752,4 @@ void state_machine_publish(int status_pub, struct vehicle_status_s *current_stat // // return ret; //} + diff --git a/src/modules/commander/state_machine_helper.h b/src/modules/commander/state_machine_helper.h index b015c4efe2..b553a4b56a 100644 --- a/src/modules/commander/state_machine_helper.h +++ b/src/modules/commander/state_machine_helper.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Copyright (C) 2013 PX4 Development Team. All rights reserved. * Author: Thomas Gubler * Julian Oes * @@ -50,15 +50,7 @@ #include -void navigation_state_update(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd); - -bool is_multirotor(const struct vehicle_status_s *current_status); - -bool is_rotary_wing(const struct vehicle_status_s *current_status); - -//int do_arming_state_update(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, arming_state_t new_state); - -void state_machine_publish(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd); +int arming_state_transition(int status_pub, struct vehicle_status_s *current_state, arming_state_t new_arming_state, int safety_pub, struct actuator_safety_s *safety, const int mavlink_fd); int navigation_state_transition(int status_pub, struct vehicle_status_s *current_state, navigation_state_t new_navigation_state, int control_mode_pub, struct vehicle_control_mode_s *control_mode, const int mavlink_fd); From 76346bfe19c816491a6982abfa10f48cd9d258f6 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Thu, 27 Jun 2013 20:20:27 +0200 Subject: [PATCH 080/109] Corrected merge mistake --- src/modules/sdlog2/sdlog2.c | 1 - 1 file changed, 1 deletion(-) diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index bdb98a24e7..8a07855c05 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -686,7 +686,6 @@ int sdlog2_thread_main(int argc, char *argv[]) struct log_SENS_s log_SENS; struct log_LPOS_s log_LPOS; struct log_LPSP_s log_LPSP; - struct log_FLOW_s log_FLOW; struct log_GPS_s log_GPS; struct log_ATTC_s log_ATTC; struct log_STAT_s log_STAT; From 1b38cf715d85b15f2200d49b64fbe22a05b71937 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Mon, 15 Jul 2013 22:15:15 +0200 Subject: [PATCH 081/109] Renamed actuator_safety back to actuator_armed, compiling but untested --- .../ardrone_interface/ardrone_interface.c | 19 ++-- src/drivers/blinkm/blinkm.cpp | 83 ++++++++++-------- src/drivers/hil/hil.cpp | 18 ++-- src/drivers/mkblctrl/mkblctrl.cpp | 20 ++--- src/drivers/px4fmu/fmu.cpp | 28 +++--- src/drivers/px4io/px4io.cpp | 37 ++++---- .../flow_position_control_main.c | 10 +-- .../flow_position_estimator_main.c | 20 ++--- .../flow_speed_control_main.c | 12 +-- src/modules/commander/commander.c | 87 ++++++++++--------- src/modules/commander/commander_helper.h | 2 +- src/modules/commander/state_machine_helper.c | 26 +++--- src/modules/commander/state_machine_helper.h | 4 +- src/modules/gpio_led/gpio_led.c | 14 +-- src/modules/mavlink/orb_listener.c | 18 ++-- src/modules/mavlink/orb_topics.h | 3 +- src/modules/mavlink_onboard/mavlink.c | 10 +-- src/modules/mavlink_onboard/orb_topics.h | 2 +- src/modules/mavlink_onboard/util.h | 2 +- .../multirotor_att_control_main.c | 18 ++-- src/modules/sdlog2/sdlog2.c | 32 +++---- src/modules/uORB/objects_common.cpp | 9 +- .../{actuator_safety.h => actuator_armed.h} | 22 ++--- src/modules/uORB/topics/safety.h | 57 ++++++++++++ .../uORB/topics/vehicle_control_mode.h | 6 +- 25 files changed, 316 insertions(+), 243 deletions(-) rename src/modules/uORB/topics/{actuator_safety.h => actuator_armed.h} (73%) create mode 100644 src/modules/uORB/topics/safety.h diff --git a/src/drivers/ardrone_interface/ardrone_interface.c b/src/drivers/ardrone_interface/ardrone_interface.c index 187820372b..b88f61ce80 100644 --- a/src/drivers/ardrone_interface/ardrone_interface.c +++ b/src/drivers/ardrone_interface/ardrone_interface.c @@ -53,9 +53,10 @@ #include #include #include -#include +#include #include -#include +#include +#include #include @@ -244,17 +245,15 @@ int ardrone_interface_thread_main(int argc, char *argv[]) int led_counter = 0; /* declare and safely initialize all structs */ - struct vehicle_status_s state; - memset(&state, 0, sizeof(state)); struct actuator_controls_s actuator_controls; memset(&actuator_controls, 0, sizeof(actuator_controls)); - struct actuator_safety_s safety; - safety.armed = false; + struct actuator_armed_s armed; + //XXX is this necessairy? + armed.armed = false; /* subscribe to attitude, motor setpoints and system state */ int actuator_controls_sub = orb_subscribe(ORB_ID_VEHICLE_ATTITUDE_CONTROLS); - int state_sub = orb_subscribe(ORB_ID(vehicle_status)); - int safety_sub = orb_subscribe(ORB_ID(actuator_safety)); + int armed_sub = orb_subscribe(ORB_ID(actuator_armed)); printf("[ardrone_interface] Motors initialized - ready.\n"); fflush(stdout); @@ -325,12 +324,12 @@ int ardrone_interface_thread_main(int argc, char *argv[]) /* get a local copy of the actuator controls */ orb_copy(ORB_ID_VEHICLE_ATTITUDE_CONTROLS, actuator_controls_sub, &actuator_controls); - orb_copy(ORB_ID(actuator_safety), safety_sub, &safety); + orb_copy(ORB_ID(actuator_armed), armed_sub, &armed); /* for now only spin if armed and immediately shut down * if in failsafe */ - if (safety.armed && !safety.lockdown) { + if (armed.armed && !armed.lockdown) { ardrone_mixing_and_output(ardrone_write, &actuator_controls); } else { diff --git a/src/drivers/blinkm/blinkm.cpp b/src/drivers/blinkm/blinkm.cpp index a11f884773..e83a87aa93 100644 --- a/src/drivers/blinkm/blinkm.cpp +++ b/src/drivers/blinkm/blinkm.cpp @@ -116,7 +116,8 @@ #include #include #include -#include +#include +#include #include static const float MAX_CELL_VOLTAGE = 4.3f; @@ -376,8 +377,9 @@ BlinkM::led() { static int vehicle_status_sub_fd; + static int vehicle_control_mode_sub_fd; static int vehicle_gps_position_sub_fd; - static int actuator_safety_sub_fd; + static int actuator_armed_sub_fd; static int num_of_cells = 0; static int detected_cells_runcount = 0; @@ -388,7 +390,8 @@ BlinkM::led() static int led_interval = 1000; static int no_data_vehicle_status = 0; - static int no_data_actuator_safety = 0; + static int no_data_vehicle_control_mode = 0; + static int no_data_actuator_armed = 0; static int no_data_vehicle_gps_position = 0; static bool topic_initialized = false; @@ -401,8 +404,11 @@ BlinkM::led() vehicle_status_sub_fd = orb_subscribe(ORB_ID(vehicle_status)); orb_set_interval(vehicle_status_sub_fd, 1000); - actuator_safety_sub_fd = orb_subscribe(ORB_ID(actuator_safety)); - orb_set_interval(actuator_safety_sub_fd, 1000); + vehicle_control_mode_sub_fd = orb_subscribe(ORB_ID(vehicle_control_mode)); + orb_set_interval(vehicle_control_mode_sub_fd, 1000); + + actuator_armed_sub_fd = orb_subscribe(ORB_ID(actuator_armed)); + orb_set_interval(actuator_armed_sub_fd, 1000); vehicle_gps_position_sub_fd = orb_subscribe(ORB_ID(vehicle_gps_position)); orb_set_interval(vehicle_gps_position_sub_fd, 1000); @@ -458,14 +464,16 @@ BlinkM::led() if (led_thread_runcount == 15) { /* obtained data for the first file descriptor */ struct vehicle_status_s vehicle_status_raw; - struct actuator_safety_s actuator_safety; + struct vehicle_control_mode_s vehicle_control_mode; + struct actuator_armed_s actuator_armed; struct vehicle_gps_position_s vehicle_gps_position_raw; memset(&vehicle_status_raw, 0, sizeof(vehicle_status_raw)); memset(&vehicle_gps_position_raw, 0, sizeof(vehicle_gps_position_raw)); bool new_data_vehicle_status; - bool new_data_actuator_safety; + bool new_data_vehicle_control_mode; + bool new_data_actuator_armed; bool new_data_vehicle_gps_position; orb_check(vehicle_status_sub_fd, &new_data_vehicle_status); @@ -479,15 +487,26 @@ BlinkM::led() no_data_vehicle_status = 3; } - orb_check(actuator_safety_sub_fd, &new_data_actuator_safety); + orb_check(vehicle_control_mode_sub_fd, &new_data_vehicle_control_mode); - if (new_data_actuator_safety) { - orb_copy(ORB_ID(actuator_safety), actuator_safety_sub_fd, &actuator_safety); - no_data_actuator_safety = 0; + if (new_data_vehicle_control_mode) { + orb_copy(ORB_ID(vehicle_control_mode), vehicle_control_mode_sub_fd, &vehicle_control_mode); + no_data_vehicle_control_mode = 0; } else { - no_data_actuator_safety++; - if(no_data_vehicle_status >= 3) - no_data_vehicle_status = 3; + no_data_vehicle_control_mode++; + if(no_data_vehicle_control_mode >= 3) + no_data_vehicle_control_mode = 3; + } + + orb_check(actuator_armed_sub_fd, &new_data_actuator_armed); + + if (new_data_actuator_armed) { + orb_copy(ORB_ID(actuator_armed), actuator_armed_sub_fd, &actuator_armed); + no_data_actuator_armed = 0; + } else { + no_data_actuator_armed++; + if(no_data_actuator_armed >= 3) + no_data_actuator_armed = 3; } orb_check(vehicle_gps_position_sub_fd, &new_data_vehicle_gps_position); @@ -549,7 +568,7 @@ BlinkM::led() } else { /* no battery warnings here */ - if(actuator_safety.armed == false) { + if(actuator_armed.armed == false) { /* system not armed */ led_color_1 = LED_RED; led_color_2 = LED_RED; @@ -573,25 +592,21 @@ BlinkM::led() led_color_8 = LED_OFF; led_blink = LED_BLINK; - /* handle 4th led - flightmode indicator */ -#warning fix this -// switch((int)vehicle_status_raw.flight_mode) { -// case VEHICLE_FLIGHT_MODE_MANUAL: -// led_color_4 = LED_AMBER; -// break; -// -// case VEHICLE_FLIGHT_MODE_STAB: -// led_color_4 = LED_YELLOW; -// break; -// -// case VEHICLE_FLIGHT_MODE_HOLD: -// led_color_4 = LED_BLUE; -// break; -// -// case VEHICLE_FLIGHT_MODE_AUTO: -// led_color_4 = LED_GREEN; -// break; -// } + if(new_data_vehicle_control_mode || no_data_vehicle_control_mode < 3) { + + //XXX please check + if (vehicle_control_mode.flag_control_position_enabled) + led_color_4 = LED_GREEN; + else if (vehicle_control_mode.flag_control_velocity_enabled) + led_color_4 = LED_BLUE; + else if (vehicle_control_mode.flag_control_attitude_enabled) + led_color_4 = LED_YELLOW; + else if (vehicle_control_mode.flag_control_manual_enabled) + led_color_4 = LED_AMBER; + else + led_color_4 = LED_OFF; + + } if(new_data_vehicle_gps_position || no_data_vehicle_gps_position < 3) { /* handling used sat�s */ diff --git a/src/drivers/hil/hil.cpp b/src/drivers/hil/hil.cpp index 15fbf8c882..3e30e32927 100644 --- a/src/drivers/hil/hil.cpp +++ b/src/drivers/hil/hil.cpp @@ -75,7 +75,7 @@ #include #include -#include +#include #include #include @@ -109,7 +109,7 @@ private: int _current_update_rate; int _task; int _t_actuators; - int _t_safety; + int _t_armed; orb_advert_t _t_outputs; unsigned _num_outputs; bool _primary_pwm_device; @@ -162,7 +162,7 @@ HIL::HIL() : _current_update_rate(0), _task(-1), _t_actuators(-1), - _t_safety(-1), + _t_armed(-1), _t_outputs(0), _num_outputs(0), _primary_pwm_device(false), @@ -322,8 +322,8 @@ HIL::task_main() /* force a reset of the update rate */ _current_update_rate = 0; - _t_safety = orb_subscribe(ORB_ID(actuator_safety)); - orb_set_interval(_t_safety, 200); /* 5Hz update rate */ + _t_armed = orb_subscribe(ORB_ID(actuator_armed)); + orb_set_interval(_t_armed, 200); /* 5Hz update rate */ /* advertise the mixed control outputs */ actuator_outputs_s outputs; @@ -335,7 +335,7 @@ HIL::task_main() pollfd fds[2]; fds[0].fd = _t_actuators; fds[0].events = POLLIN; - fds[1].fd = _t_safety; + fds[1].fd = _t_armed; fds[1].events = POLLIN; unsigned num_outputs; @@ -427,15 +427,15 @@ HIL::task_main() /* how about an arming update? */ if (fds[1].revents & POLLIN) { - actuator_safety_s aa; + actuator_armed_s aa; /* get new value */ - orb_copy(ORB_ID(actuator_safety), _t_safety, &aa); + orb_copy(ORB_ID(actuator_armed), _t_armed, &aa); } } ::close(_t_actuators); - ::close(_t_safety); + ::close(_t_armed); /* make sure servos are off */ // up_pwm_servo_deinit(); diff --git a/src/drivers/mkblctrl/mkblctrl.cpp b/src/drivers/mkblctrl/mkblctrl.cpp index 3fc1054e6c..06930db388 100644 --- a/src/drivers/mkblctrl/mkblctrl.cpp +++ b/src/drivers/mkblctrl/mkblctrl.cpp @@ -74,7 +74,7 @@ #include #include #include -#include +#include #include #include @@ -135,7 +135,7 @@ private: int _current_update_rate; int _task; int _t_actuators; - int _t_actuator_safety; + int _t_actuator_armed; unsigned int _motor; int _px4mode; int _frametype; @@ -248,7 +248,7 @@ MK::MK(int bus) : _update_rate(50), _task(-1), _t_actuators(-1), - _t_actuator_safety(-1), + _t_actuator_armed(-1), _t_outputs(0), _t_actuators_effective(0), _t_esc_status(0), @@ -513,8 +513,8 @@ MK::task_main() /* force a reset of the update rate */ _current_update_rate = 0; - _t_actuator_safety = orb_subscribe(ORB_ID(actuator_safety)); - orb_set_interval(_t_actuator_safety, 200); /* 5Hz update rate */ + _t_actuator_armed = orb_subscribe(ORB_ID(actuator_armed)); + orb_set_interval(_t_actuator_armed, 200); /* 5Hz update rate */ /* advertise the mixed control outputs */ actuator_outputs_s outputs; @@ -540,7 +540,7 @@ MK::task_main() pollfd fds[2]; fds[0].fd = _t_actuators; fds[0].events = POLLIN; - fds[1].fd = _t_actuator_safety; + fds[1].fd = _t_actuator_armed; fds[1].events = POLLIN; log("starting"); @@ -651,13 +651,13 @@ MK::task_main() /* how about an arming update? */ if (fds[1].revents & POLLIN) { - actuator_safety_s as; + actuator_armed_s aa; /* get new value */ - orb_copy(ORB_ID(actuator_safety), _t_actuator_safety, &as); + orb_copy(ORB_ID(actuator_armed), _t_actuator_armed, &aa); /* update PWM servo armed status if armed and not locked down */ - mk_servo_arm(as.armed && !as.lockdown); + mk_servo_arm(aa.armed && !aa.lockdown); } @@ -700,7 +700,7 @@ MK::task_main() //::close(_t_esc_status); ::close(_t_actuators); ::close(_t_actuators_effective); - ::close(_t_actuator_safety); + ::close(_t_actuator_armed); /* make sure servos are off */ diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp index d0499d2fd6..41c8c8bb72 100644 --- a/src/drivers/px4fmu/fmu.cpp +++ b/src/drivers/px4fmu/fmu.cpp @@ -69,7 +69,7 @@ #include #include #include -#include +#include #include #include @@ -105,7 +105,7 @@ private: unsigned _current_update_rate; int _task; int _t_actuators; - int _t_actuator_safety; + int _t_actuator_armed; orb_advert_t _t_outputs; orb_advert_t _t_actuators_effective; unsigned _num_outputs; @@ -175,7 +175,7 @@ PX4FMU::PX4FMU() : _current_update_rate(0), _task(-1), _t_actuators(-1), - _t_actuator_safety(-1), + _t_actuator_armed(-1), _t_outputs(0), _t_actuators_effective(0), _num_outputs(0), @@ -377,8 +377,8 @@ PX4FMU::task_main() /* force a reset of the update rate */ _current_update_rate = 0; - _t_actuator_safety = orb_subscribe(ORB_ID(actuator_safety)); - orb_set_interval(_t_actuator_safety, 200); /* 5Hz update rate */ + _t_actuator_armed = orb_subscribe(ORB_ID(actuator_armed)); + orb_set_interval(_t_actuator_armed, 200); /* 5Hz update rate */ /* advertise the mixed control outputs */ actuator_outputs_s outputs; @@ -397,7 +397,7 @@ PX4FMU::task_main() pollfd fds[2]; fds[0].fd = _t_actuators; fds[0].events = POLLIN; - fds[1].fd = _t_actuator_safety; + fds[1].fd = _t_actuator_armed; fds[1].events = POLLIN; unsigned num_outputs = (_mode == MODE_2PWM) ? 2 : 4; @@ -500,13 +500,13 @@ PX4FMU::task_main() /* how about an arming update? */ if (fds[1].revents & POLLIN) { - actuator_safety_s as; + actuator_armed_s aa; /* get new value */ - orb_copy(ORB_ID(actuator_safety), _t_actuator_safety, &as); + orb_copy(ORB_ID(actuator_armed), _t_actuator_armed, &aa); /* update PWM servo armed status if armed and not locked down */ - bool set_armed = as.armed && !as.lockdown; + bool set_armed = aa.armed && !aa.lockdown; if (set_armed != _armed) { _armed = set_armed; up_pwm_servo_arm(set_armed); @@ -536,7 +536,7 @@ PX4FMU::task_main() ::close(_t_actuators); ::close(_t_actuators_effective); - ::close(_t_actuator_safety); + ::close(_t_actuator_armed); /* make sure servos are off */ up_pwm_servo_deinit(); @@ -1022,12 +1022,12 @@ fake(int argc, char *argv[]) if (handle < 0) errx(1, "advertise failed"); - actuator_safety_s as; + actuator_armed_s aa; - as.armed = true; - as.lockdown = false; + aa.armed = true; + aa.lockdown = false; - handle = orb_advertise(ORB_ID(actuator_safety), &as); + handle = orb_advertise(ORB_ID(actuator_armed), &aa); if (handle < 0) errx(1, "advertise failed 2"); diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 94f2318211..ea732eccd1 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -75,7 +75,8 @@ #include #include #include -#include +#include +#include #include #include #include @@ -178,7 +179,7 @@ private: /* subscribed topics */ int _t_actuators; ///< actuator controls topic - int _t_actuator_safety; ///< system armed control topic + int _t_actuator_armed; ///< system armed control topic int _t_vstatus; ///< system / vehicle status int _t_param; ///< parameter update topic @@ -360,7 +361,7 @@ PX4IO::PX4IO() : _status(0), _alarms(0), _t_actuators(-1), - _t_actuator_safety(-1), + _t_actuator_armed(-1), _t_vstatus(-1), _t_param(-1), _to_input_rc(0), @@ -505,9 +506,9 @@ PX4IO::init() * remains untouched (so manual override is still available). */ - int safety_sub = orb_subscribe(ORB_ID(actuator_safety)); + int safety_sub = orb_subscribe(ORB_ID(actuator_armed)); /* fill with initial values, clear updated flag */ - struct actuator_safety_s safety; + struct actuator_armed_s safety; uint64_t try_start_time = hrt_absolute_time(); bool updated = false; @@ -518,7 +519,7 @@ PX4IO::init() if (updated) { /* got data, copy and exit loop */ - orb_copy(ORB_ID(actuator_safety), safety_sub, &safety); + orb_copy(ORB_ID(actuator_armed), safety_sub, &safety); break; } @@ -559,7 +560,7 @@ PX4IO::init() orb_check(safety_sub, &updated); if (updated) { - orb_copy(ORB_ID(actuator_safety), safety_sub, &safety); + orb_copy(ORB_ID(actuator_armed), safety_sub, &safety); } /* wait 50 ms */ @@ -643,8 +644,8 @@ PX4IO::task_main() ORB_ID(actuator_controls_1)); orb_set_interval(_t_actuators, 20); /* default to 50Hz */ - _t_actuator_safety = orb_subscribe(ORB_ID(actuator_safety)); - orb_set_interval(_t_actuator_safety, 200); /* 5Hz update rate */ + _t_actuator_armed = orb_subscribe(ORB_ID(actuator_armed)); + orb_set_interval(_t_actuator_armed, 200); /* 5Hz update rate */ _t_vstatus = orb_subscribe(ORB_ID(vehicle_status)); orb_set_interval(_t_vstatus, 200); /* 5Hz update rate max. */ @@ -656,7 +657,7 @@ PX4IO::task_main() pollfd fds[4]; fds[0].fd = _t_actuators; fds[0].events = POLLIN; - fds[1].fd = _t_actuator_safety; + fds[1].fd = _t_actuator_armed; fds[1].events = POLLIN; fds[2].fd = _t_vstatus; fds[2].events = POLLIN; @@ -832,21 +833,21 @@ PX4IO::set_idle_values(const uint16_t *vals, unsigned len) int PX4IO::io_set_arming_state() { - actuator_safety_s safety; ///< system armed state + actuator_armed_s armed; ///< system armed state vehicle_status_s vstatus; ///< overall system state - orb_copy(ORB_ID(actuator_safety), _t_actuator_safety, &safety); + orb_copy(ORB_ID(actuator_armed), _t_actuator_armed, &armed); orb_copy(ORB_ID(vehicle_status), _t_vstatus, &vstatus); uint16_t set = 0; uint16_t clear = 0; - if (safety.armed && !safety.lockdown) { + if (armed.armed && !armed.lockdown) { set |= PX4IO_P_SETUP_ARMING_FMU_ARMED; } else { clear |= PX4IO_P_SETUP_ARMING_FMU_ARMED; } - if (safety.ready_to_arm) { + if (armed.ready_to_arm) { set |= PX4IO_P_SETUP_ARMING_IO_ARM_OK; } else { clear |= PX4IO_P_SETUP_ARMING_IO_ARM_OK; @@ -1004,10 +1005,10 @@ PX4IO::io_handle_status(uint16_t status) /** * Get and handle the safety status */ - struct actuator_safety_s safety; + struct safety_s safety; safety.timestamp = hrt_absolute_time(); - orb_copy(ORB_ID(actuator_safety), _t_actuator_safety, &safety); + orb_copy(ORB_ID(safety), _to_safety, &safety); if (status & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) { safety.safety_off = true; @@ -1019,9 +1020,9 @@ PX4IO::io_handle_status(uint16_t status) /* lazily publish the safety status */ if (_to_safety > 0) { - orb_publish(ORB_ID(actuator_safety), _to_safety, &safety); + orb_publish(ORB_ID(safety), _to_safety, &safety); } else { - _to_safety = orb_advertise(ORB_ID(actuator_safety), &safety); + _to_safety = orb_advertise(ORB_ID(safety), &safety); } return ret; diff --git a/src/examples/flow_position_control/flow_position_control_main.c b/src/examples/flow_position_control/flow_position_control_main.c index 0b94045822..621d3253f0 100644 --- a/src/examples/flow_position_control/flow_position_control_main.c +++ b/src/examples/flow_position_control/flow_position_control_main.c @@ -55,7 +55,7 @@ #include #include #include -#include +#include #include #include #include @@ -159,7 +159,7 @@ flow_position_control_thread_main(int argc, char *argv[]) const float time_scale = powf(10.0f,-6.0f); /* structures */ - struct actuator_safety_s safety; + struct actuator_armed_s armed; struct vehicle_control_mode_s control_mode; struct vehicle_attitude_s att; struct manual_control_setpoint_s manual; @@ -171,7 +171,7 @@ flow_position_control_thread_main(int argc, char *argv[]) /* subscribe to attitude, motor setpoints and system state */ int parameter_update_sub = orb_subscribe(ORB_ID(parameter_update)); int vehicle_attitude_sub = orb_subscribe(ORB_ID(vehicle_attitude)); - int safety_sub = orb_subscribe(ORB_ID(actuator_safety)); + int armed_sub = orb_subscribe(ORB_ID(actuator_armed)); int control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode)); int manual_control_setpoint_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); int filtered_bottom_flow_sub = orb_subscribe(ORB_ID(filtered_bottom_flow)); @@ -261,7 +261,7 @@ flow_position_control_thread_main(int argc, char *argv[]) perf_begin(mc_loop_perf); /* get a local copy of the vehicle state */ - orb_copy(ORB_ID(actuator_safety), safety_sub, &safety); + orb_copy(ORB_ID(actuator_armed), armed_sub, &armed); /* get a local copy of manual setpoint */ orb_copy(ORB_ID(manual_control_setpoint), manual_control_setpoint_sub, &manual); /* get a local copy of attitude */ @@ -578,7 +578,7 @@ flow_position_control_thread_main(int argc, char *argv[]) close(parameter_update_sub); close(vehicle_attitude_sub); close(vehicle_local_position_sub); - close(safety_sub); + close(armed_sub); close(control_mode_sub); close(manual_control_setpoint_sub); close(speed_sp_pub); diff --git a/src/examples/flow_position_estimator/flow_position_estimator_main.c b/src/examples/flow_position_estimator/flow_position_estimator_main.c index 557fdf37c4..5e80066a70 100644 --- a/src/examples/flow_position_estimator/flow_position_estimator_main.c +++ b/src/examples/flow_position_estimator/flow_position_estimator_main.c @@ -56,7 +56,7 @@ #include #include #include -#include +#include #include #include #include @@ -159,8 +159,8 @@ int flow_position_estimator_thread_main(int argc, char *argv[]) static float sonar_lp = 0.0f; /* subscribe to vehicle status, attitude, sensors and flow*/ - struct actuator_safety_s safety; - memset(&safety, 0, sizeof(safety)); + struct actuator_armed_s armed; + memset(&armed, 0, sizeof(armed)); struct vehicle_control_mode_s control_mode; memset(&control_mode, 0, sizeof(control_mode)); struct vehicle_attitude_s att; @@ -173,8 +173,8 @@ int flow_position_estimator_thread_main(int argc, char *argv[]) /* subscribe to parameter changes */ int parameter_update_sub = orb_subscribe(ORB_ID(parameter_update)); - /* subscribe to safety topic */ - int safety_sub = orb_subscribe(ORB_ID(actuator_safety)); + /* subscribe to armed topic */ + int armed_sub = orb_subscribe(ORB_ID(actuator_armed)); /* subscribe to safety topic */ int control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode)); @@ -270,7 +270,7 @@ int flow_position_estimator_thread_main(int argc, char *argv[]) /* got flow, updating attitude and status as well */ orb_copy(ORB_ID(vehicle_attitude), vehicle_attitude_sub, &att); orb_copy(ORB_ID(vehicle_attitude_setpoint), vehicle_attitude_setpoint_sub, &att_sp); - orb_copy(ORB_ID(actuator_safety), safety_sub, &safety); + orb_copy(ORB_ID(actuator_armed), armed_sub, &armed); orb_copy(ORB_ID(vehicle_control_mode), control_mode_sub, &control_mode); /* vehicle state estimation */ @@ -284,12 +284,12 @@ int flow_position_estimator_thread_main(int argc, char *argv[]) if (!vehicle_liftoff) { - if (safety.armed && att_sp.thrust > params.minimum_liftoff_thrust && sonar_new > 0.3f && sonar_new < 1.0f) + if (armed.armed && att_sp.thrust > params.minimum_liftoff_thrust && sonar_new > 0.3f && sonar_new < 1.0f) vehicle_liftoff = true; } else { - if (!safety.armed || (att_sp.thrust < params.minimum_liftoff_thrust && sonar_new <= 0.3f)) + if (!armed.armed || (att_sp.thrust < params.minimum_liftoff_thrust && sonar_new <= 0.3f)) vehicle_liftoff = false; } @@ -356,7 +356,7 @@ int flow_position_estimator_thread_main(int argc, char *argv[]) } /* filtering ground distance */ - if (!vehicle_liftoff || !safety.armed) + if (!vehicle_liftoff || !armed.armed) { /* not possible to fly */ sonar_valid = false; @@ -453,7 +453,7 @@ int flow_position_estimator_thread_main(int argc, char *argv[]) close(vehicle_attitude_setpoint_sub); close(vehicle_attitude_sub); - close(safety_sub); + close(armed_sub); close(control_mode_sub); close(parameter_update_sub); close(optical_flow_sub); diff --git a/src/examples/flow_speed_control/flow_speed_control_main.c b/src/examples/flow_speed_control/flow_speed_control_main.c index 8032a6264a..6af955cd7d 100644 --- a/src/examples/flow_speed_control/flow_speed_control_main.c +++ b/src/examples/flow_speed_control/flow_speed_control_main.c @@ -55,7 +55,7 @@ #include #include #include -#include +#include #include #include #include @@ -156,7 +156,7 @@ flow_speed_control_thread_main(int argc, char *argv[]) uint32_t counter = 0; /* structures */ - struct actuator_safety_s safety; + struct actuator_armed_s armed; struct vehicle_control_mode_s control_mode; struct filtered_bottom_flow_s filtered_flow; struct vehicle_bodyframe_speed_setpoint_s speed_sp; @@ -166,7 +166,7 @@ flow_speed_control_thread_main(int argc, char *argv[]) /* subscribe to attitude, motor setpoints and system state */ int parameter_update_sub = orb_subscribe(ORB_ID(parameter_update)); int vehicle_attitude_sub = orb_subscribe(ORB_ID(vehicle_attitude)); - int safety_sub = orb_subscribe(ORB_ID(actuator_safety)); + int armed_sub = orb_subscribe(ORB_ID(actuator_armed)); int control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode)); int filtered_bottom_flow_sub = orb_subscribe(ORB_ID(filtered_bottom_flow)); int vehicle_bodyframe_speed_setpoint_sub = orb_subscribe(ORB_ID(vehicle_bodyframe_speed_setpoint)); @@ -229,8 +229,8 @@ flow_speed_control_thread_main(int argc, char *argv[]) { perf_begin(mc_loop_perf); - /* get a local copy of the safety topic */ - orb_copy(ORB_ID(actuator_safety), safety_sub, &safety); + /* get a local copy of the armed topic */ + orb_copy(ORB_ID(actuator_armed), armed_sub, &armed); /* get a local copy of the control mode */ orb_copy(ORB_ID(vehicle_control_mode), control_mode_sub, &control_mode); /* get a local copy of filtered bottom flow */ @@ -355,7 +355,7 @@ flow_speed_control_thread_main(int argc, char *argv[]) close(vehicle_attitude_sub); close(vehicle_bodyframe_speed_setpoint_sub); close(filtered_bottom_flow_sub); - close(safety_sub); + close(armed_sub); close(control_mode_sub); close(att_sp_pub); diff --git a/src/modules/commander/commander.c b/src/modules/commander/commander.c index 2b0b3a415e..c4ee605ccf 100644 --- a/src/modules/commander/commander.c +++ b/src/modules/commander/commander.c @@ -76,9 +76,10 @@ #include #include #include -#include +#include #include #include +#include #include #include @@ -177,7 +178,7 @@ int led_off(int led); void do_reboot(void); -void handle_command(int status_pub, struct vehicle_status_s *current_status, struct vehicle_command_s *cmd, int safety_pub, struct actuator_safety_s *safety); +void handle_command(int status_pub, struct vehicle_status_s *current_status, struct vehicle_command_s *cmd, int armed_pub, struct actuator_armed_s *armed); // int trigger_audio_alarm(uint8_t old_mode, uint8_t old_state, uint8_t new_mode, uint8_t new_state); @@ -253,7 +254,7 @@ void do_reboot() -void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_status, struct vehicle_command_s *cmd, int safety_pub, struct actuator_safety_s *safety) +void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_status, struct vehicle_command_s *cmd, int armed_pub, struct actuator_armed_s *armed) { /* result of the command */ uint8_t result = VEHICLE_CMD_RESULT_UNSUPPORTED; @@ -273,13 +274,13 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta } if ((int)cmd->param1 & VEHICLE_MODE_FLAG_SAFETY_ARMED) { - if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_ARMED, safety_pub, safety, mavlink_fd)) { + if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_ARMED, armed_pub, armed, mavlink_fd)) { result = VEHICLE_CMD_RESULT_ACCEPTED; } else { result = VEHICLE_CMD_RESULT_DENIED; } } else { - if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_STANDBY, safety_pub, safety, mavlink_fd)) { + if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_STANDBY, armed_pub, armed, mavlink_fd)) { result = VEHICLE_CMD_RESULT_ACCEPTED; } else { result = VEHICLE_CMD_RESULT_DENIED; @@ -292,14 +293,14 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta /* request to arm */ if ((int)cmd->param1 == 1) { - if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_ARMED, safety_pub, safety, mavlink_fd)) { + if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_ARMED, armed_pub, armed, mavlink_fd)) { result = VEHICLE_CMD_RESULT_ACCEPTED; } else { result = VEHICLE_CMD_RESULT_DENIED; } /* request to disarm */ } else if ((int)cmd->param1 == 0) { - if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_STANDBY, safety_pub, safety, mavlink_fd)) { + if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_STANDBY, armed_pub, armed, mavlink_fd)) { result = VEHICLE_CMD_RESULT_ACCEPTED; } else { result = VEHICLE_CMD_RESULT_DENIED; @@ -312,7 +313,7 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta /* request for an autopilot reboot */ if ((int)cmd->param1 == 1) { - if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_REBOOT, safety_pub, safety, mavlink_fd)) { + if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_REBOOT, armed_pub, armed, mavlink_fd)) { /* reboot is executed later, after positive tune is sent */ result = VEHICLE_CMD_RESULT_ACCEPTED; tune_positive(); @@ -361,7 +362,7 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta if(low_prio_task == LOW_PRIO_TASK_NONE) { /* try to go to INIT/PREFLIGHT arming state */ - if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_INIT, safety_pub, safety, mavlink_fd)) { + if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_INIT, armed_pub, armed, mavlink_fd)) { result = VEHICLE_CMD_RESULT_ACCEPTED; /* now set the task for the low prio thread */ low_prio_task = LOW_PRIO_TASK_GYRO_CALIBRATION; @@ -380,7 +381,7 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta if(low_prio_task == LOW_PRIO_TASK_NONE) { /* try to go to INIT/PREFLIGHT arming state */ - if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_INIT, safety_pub, safety, mavlink_fd)) { + if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_INIT, armed_pub, armed, mavlink_fd)) { result = VEHICLE_CMD_RESULT_ACCEPTED; /* now set the task for the low prio thread */ low_prio_task = LOW_PRIO_TASK_MAG_CALIBRATION; @@ -400,7 +401,7 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta // if(low_prio_task == LOW_PRIO_TASK_NONE) { // /* try to go to INIT/PREFLIGHT arming state */ - // if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_INIT, safety_pub, safety, mavlink_fd)) { + // if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_INIT, armed_pub, armed, mavlink_fd)) { // result = VEHICLE_CMD_RESULT_ACCEPTED; // /* now set the task for the low prio thread */ // low_prio_task = LOW_PRIO_TASK_ALTITUDE_CALIBRATION; @@ -421,7 +422,7 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta // if(low_prio_task == LOW_PRIO_TASK_NONE) { // /* try to go to INIT/PREFLIGHT arming state */ - // if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_INIT, safety_pub, safety, mavlink_fd)) { + // if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_INIT, armed_pub, armed, mavlink_fd)) { // result = VEHICLE_CMD_RESULT_ACCEPTED; // /* now set the task for the low prio thread */ // low_prio_task = LOW_PRIO_TASK_RC_CALIBRATION; @@ -441,7 +442,7 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta if(low_prio_task == LOW_PRIO_TASK_NONE) { /* try to go to INIT/PREFLIGHT arming state */ - if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_INIT, safety_pub, safety, mavlink_fd)) { + if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_INIT, armed_pub, armed, mavlink_fd)) { result = VEHICLE_CMD_RESULT_ACCEPTED; /* now set the task for the low prio thread */ low_prio_task = LOW_PRIO_TASK_ACCEL_CALIBRATION; @@ -460,7 +461,7 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta if(low_prio_task == LOW_PRIO_TASK_NONE) { /* try to go to INIT/PREFLIGHT arming state */ - if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_INIT, safety_pub, safety, mavlink_fd)) { + if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_INIT, armed_pub, armed, mavlink_fd)) { result = VEHICLE_CMD_RESULT_ACCEPTED; /* now set the task for the low prio thread */ low_prio_task = LOW_PRIO_TASK_AIRSPEED_CALIBRATION; @@ -734,12 +735,11 @@ int commander_thread_main(int argc, char *argv[]) /* make sure we are in preflight state */ memset(¤t_status, 0, sizeof(current_status)); - /* safety topic */ - struct actuator_safety_s safety; - orb_advert_t safety_pub; - /* Initialize safety with all false */ - memset(&safety, 0, sizeof(safety)); - + /* armed topic */ + struct actuator_armed_s armed; + orb_advert_t armed_pub; + /* Initialize armed with all false */ + memset(&armed, 0, sizeof(armed)); /* flags for control apps */ struct vehicle_control_mode_s control_mode; @@ -768,8 +768,8 @@ int commander_thread_main(int argc, char *argv[]) /* set battery warning flag */ current_status.battery_warning = VEHICLE_BATTERY_WARNING_NONE; - /* set safety device detection flag */ + /* set safety device detection flag */ /* XXX do we need this? */ //current_status.flag_safety_present = false; @@ -789,10 +789,7 @@ int commander_thread_main(int argc, char *argv[]) orb_publish(ORB_ID(vehicle_status), status_pub, ¤t_status); - safety_pub = orb_advertise(ORB_ID(actuator_safety), &safety); - - /* but also subscribe to it */ - int safety_sub = orb_subscribe(ORB_ID(actuator_safety)); + armed_pub = orb_advertise(ORB_ID(actuator_armed), &armed); control_mode_pub = orb_advertise(ORB_ID(vehicle_control_mode), &control_mode); @@ -844,6 +841,11 @@ int commander_thread_main(int argc, char *argv[]) /* XXX what exactly is this for? */ uint64_t last_print_time = 0; + /* Subscribe to safety topic */ + int safety_sub = orb_subscribe(ORB_ID(safety)); + struct safety_s safety; + memset(&safety, 0, sizeof(safety)); + /* Subscribe to manual control data */ int sp_man_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); struct manual_control_setpoint_s sp_man; @@ -917,7 +919,6 @@ int commander_thread_main(int argc, char *argv[]) /* XXX use this! */ //uint64_t failsave_ll_start_time = 0; - enum VEHICLE_MANUAL_SAS_MODE manual_sas_mode; bool state_changed = true; bool param_init_forced = true; @@ -936,7 +937,7 @@ int commander_thread_main(int argc, char *argv[]) /* update parameters */ - if (!safety.armed) { + if (!armed.armed) { if (param_get(_param_sys_type, &(current_status.system_type)) != OK) { warnx("failed setting new system type"); } @@ -990,7 +991,7 @@ int commander_thread_main(int argc, char *argv[]) orb_copy(ORB_ID(vehicle_command), cmd_sub, &cmd); /* handle it */ - handle_command(status_pub, ¤t_status, &cmd, safety_pub, &safety); + handle_command(status_pub, ¤t_status, &cmd, armed_pub, &armed); } @@ -1003,7 +1004,7 @@ int commander_thread_main(int argc, char *argv[]) orb_copy(ORB_ID(parameter_update), param_changed_sub, ¶m_changed); /* update parameters */ - if (!safety.armed) { + if (!armed.armed) { if (param_get(_param_sys_type, &(current_status.system_type)) != OK) { warnx("failed setting new system type"); } @@ -1028,7 +1029,7 @@ int commander_thread_main(int argc, char *argv[]) orb_check(safety_sub, &new_data); if (new_data) { - orb_copy(ORB_ID(actuator_safety), safety_sub, &safety); + orb_copy(ORB_ID(safety), safety_sub, &safety); } /* update global position estimate */ @@ -1170,7 +1171,7 @@ int commander_thread_main(int argc, char *argv[]) /* If in INIT state, try to proceed to STANDBY state */ if (current_status.arming_state == ARMING_STATE_INIT) { // XXX check for sensors - arming_state_transition(status_pub, ¤t_status, ARMING_STATE_STANDBY, safety_pub, &safety, mavlink_fd); + arming_state_transition(status_pub, ¤t_status, ARMING_STATE_STANDBY, armed_pub, &armed, mavlink_fd); } else { // XXX: Add emergency stuff if sensors are lost } @@ -1287,7 +1288,7 @@ int commander_thread_main(int argc, char *argv[]) && (vdop_m < vdop_threshold_m) // XXX note that vdop is 0 for mtk && !home_position_set && (hrt_absolute_time() - gps_position.timestamp_position < 2000000) - && !safety.armed) { + && !armed.armed) { warnx("setting home position"); /* copy position data to uORB home message, store it locally as well */ @@ -1579,7 +1580,7 @@ int commander_thread_main(int argc, char *argv[]) mavlink_log_critical(mavlink_fd, "DISARM DENY, go manual mode first"); tune_negative(); } else { - arming_state_transition(status_pub, ¤t_status, ARMING_STATE_STANDBY, safety_pub, &safety, mavlink_fd); + arming_state_transition(status_pub, ¤t_status, ARMING_STATE_STANDBY, armed_pub, &armed, mavlink_fd); tune_positive(); } @@ -1595,13 +1596,12 @@ int commander_thread_main(int argc, char *argv[]) } } - /* check if left stick is in lower right position --> arm */ - if (current_status.flag_control_manual_enabled && - current_status.manual_sas_mode == VEHICLE_MANUAL_SAS_MODE_ROLL_PITCH_ABS_YAW_ABS && + /* check if left stick is in lower right position and we're in manual --> arm */ + if (!control_mode.flag_control_position_enabled && !control_mode.flag_control_velocity_enabled && sp_man.yaw > STICK_ON_OFF_LIMIT && sp_man.throttle < STICK_THRUST_RANGE * 0.1f) { if (stick_on_counter > STICK_ON_OFF_COUNTER_LIMIT) { - arming_state_transition(status_pub, ¤t_status, ARMING_STATE_ARMED, safety_pub, &safety, mavlink_fd); + arming_state_transition(status_pub, ¤t_status, ARMING_STATE_ARMED, armed_pub, &armed, mavlink_fd); stick_on_counter = 0; tune_positive(); @@ -1704,13 +1704,13 @@ int commander_thread_main(int argc, char *argv[]) // XXX check if this is correct /* arm / disarm on request */ - if (sp_offboard.armed && !safety.armed) { + if (sp_offboard.armed && !armed.armed) { - arming_state_transition(status_pub, ¤t_status, ARMING_STATE_ARMED, safety_pub, &safety, mavlink_fd); + arming_state_transition(status_pub, ¤t_status, ARMING_STATE_ARMED, armed_pub, &armed, mavlink_fd); - } else if (!sp_offboard.armed && safety.armed) { + } else if (!sp_offboard.armed && armed.armed) { - arming_state_transition(status_pub, ¤t_status, ARMING_STATE_STANDBY, safety_pub, &safety, mavlink_fd); + arming_state_transition(status_pub, ¤t_status, ARMING_STATE_STANDBY, armed_pub, &armed, mavlink_fd); } } else { @@ -1772,7 +1772,7 @@ int commander_thread_main(int argc, char *argv[]) /* play tone according to evaluation result */ /* check if we recently armed */ - if (!arm_tune_played && safety.armed && ( !safety.safety_switch_available || (safety.safety_off && safety.safety_switch_available))) { + if (!arm_tune_played && armed.armed && ( !safety.safety_switch_available || (safety.safety_off && safety.safety_switch_available))) { if (tune_arm() == OK) arm_tune_played = true; @@ -1789,7 +1789,7 @@ int commander_thread_main(int argc, char *argv[]) } /* reset arm_tune_played when disarmed */ - if (!(safety.armed && ( !safety.safety_switch_available || (safety.safety_off && safety.safety_switch_available)))) { + if (!(armed.armed && ( !safety.safety_switch_available || (safety.safety_off && safety.safety_switch_available)))) { arm_tune_played = false; } @@ -1812,6 +1812,7 @@ int commander_thread_main(int argc, char *argv[]) close(sp_offboard_sub); close(global_position_sub); close(sensor_sub); + close(safety_sub); close(cmd_sub); warnx("exiting"); diff --git a/src/modules/commander/commander_helper.h b/src/modules/commander/commander_helper.h index ea96d72a6a..b06e851691 100644 --- a/src/modules/commander/commander_helper.h +++ b/src/modules/commander/commander_helper.h @@ -43,7 +43,7 @@ #include #include -#include +#include #include diff --git a/src/modules/commander/state_machine_helper.c b/src/modules/commander/state_machine_helper.c index c15fc91a05..0b241d108d 100644 --- a/src/modules/commander/state_machine_helper.c +++ b/src/modules/commander/state_machine_helper.c @@ -57,7 +57,7 @@ #include "commander_helper.h" -int arming_state_transition(int status_pub, struct vehicle_status_s *current_state, arming_state_t new_arming_state, int safety_pub, struct actuator_safety_s *safety, const int mavlink_fd) { +int arming_state_transition(int status_pub, struct vehicle_status_s *current_state, arming_state_t new_arming_state, int armed_pub, struct actuator_armed_s *armed, const int mavlink_fd) { int ret = ERROR; @@ -73,8 +73,8 @@ int arming_state_transition(int status_pub, struct vehicle_status_s *current_sta /* allow going back from INIT for calibration */ if (current_state->arming_state == ARMING_STATE_STANDBY) { ret = OK; - safety->armed = false; - safety->ready_to_arm = false; + armed->armed = false; + armed->ready_to_arm = false; } break; case ARMING_STATE_STANDBY: @@ -86,8 +86,8 @@ int arming_state_transition(int status_pub, struct vehicle_status_s *current_sta /* sensors need to be initialized for STANDBY state */ if (current_state->condition_system_sensors_initialized) { ret = OK; - safety->armed = false; - safety->ready_to_arm = true; + armed->armed = false; + armed->ready_to_arm = true; } else { mavlink_log_critical(mavlink_fd, "Rej. STANDBY state, sensors not initialized"); } @@ -101,7 +101,7 @@ int arming_state_transition(int status_pub, struct vehicle_status_s *current_sta /* XXX conditions for arming? */ ret = OK; - safety->armed = true; + armed->armed = true; } break; case ARMING_STATE_ARMED_ERROR: @@ -111,7 +111,7 @@ int arming_state_transition(int status_pub, struct vehicle_status_s *current_sta /* XXX conditions for an error state? */ ret = OK; - safety->armed = true; + armed->armed = true; } break; case ARMING_STATE_STANDBY_ERROR: @@ -120,8 +120,8 @@ int arming_state_transition(int status_pub, struct vehicle_status_s *current_sta || current_state->arming_state == ARMING_STATE_INIT || current_state->arming_state == ARMING_STATE_ARMED_ERROR) { ret = OK; - safety->armed = false; - safety->ready_to_arm = false; + armed->armed = false; + armed->ready_to_arm = false; } break; case ARMING_STATE_REBOOT: @@ -132,8 +132,8 @@ int arming_state_transition(int status_pub, struct vehicle_status_s *current_sta || current_state->arming_state == ARMING_STATE_STANDBY_ERROR) { ret = OK; - safety->armed = false; - safety->ready_to_arm = false; + armed->armed = false; + armed->ready_to_arm = false; } break; @@ -151,8 +151,8 @@ int arming_state_transition(int status_pub, struct vehicle_status_s *current_sta current_state->timestamp = hrt_absolute_time(); orb_publish(ORB_ID(vehicle_status), status_pub, current_state); - safety->timestamp = hrt_absolute_time(); - orb_publish(ORB_ID(actuator_safety), safety_pub, safety); + armed->timestamp = hrt_absolute_time(); + orb_publish(ORB_ID(actuator_armed), armed_pub, armed); } } diff --git a/src/modules/commander/state_machine_helper.h b/src/modules/commander/state_machine_helper.h index b553a4b56a..e591d2fef9 100644 --- a/src/modules/commander/state_machine_helper.h +++ b/src/modules/commander/state_machine_helper.h @@ -46,11 +46,11 @@ #include #include -#include +#include #include -int arming_state_transition(int status_pub, struct vehicle_status_s *current_state, arming_state_t new_arming_state, int safety_pub, struct actuator_safety_s *safety, const int mavlink_fd); +int arming_state_transition(int status_pub, struct vehicle_status_s *current_state, arming_state_t new_arming_state, int armed_pub, struct actuator_armed_s *armed, const int mavlink_fd); int navigation_state_transition(int status_pub, struct vehicle_status_s *current_state, navigation_state_t new_navigation_state, int control_mode_pub, struct vehicle_control_mode_s *control_mode, const int mavlink_fd); diff --git a/src/modules/gpio_led/gpio_led.c b/src/modules/gpio_led/gpio_led.c index 97b585cb77..6eb4257058 100644 --- a/src/modules/gpio_led/gpio_led.c +++ b/src/modules/gpio_led/gpio_led.c @@ -51,7 +51,7 @@ #include #include #include -#include +#include #include #include #include @@ -63,8 +63,8 @@ struct gpio_led_s { int pin; struct vehicle_status_s status; int vehicle_status_sub; - struct actuator_safety_s safety; - int actuator_safety_sub; + struct actuator_armed_s armed; + int actuator_armed_sub; bool led_state; int counter; }; @@ -233,12 +233,12 @@ void gpio_led_cycle(FAR void *arg) orb_check(priv->vehicle_status_sub, &status_updated); if (status_updated) - orb_copy(ORB_ID(actuator_safety), priv->actuator_safety_sub, &priv->safety); + orb_copy(ORB_ID(actuator_armed), priv->actuator_armed_sub, &priv->armed); /* select pattern for current status */ int pattern = 0; - if (priv->safety.armed) { + if (priv->armed.armed) { if (priv->status.battery_warning == VEHICLE_BATTERY_WARNING_NONE) { pattern = 0x3f; // ****** solid (armed) @@ -247,10 +247,10 @@ void gpio_led_cycle(FAR void *arg) } } else { - if (priv->safety.ready_to_arm) { + if (priv->armed.ready_to_arm) { pattern = 0x00; // ______ off (disarmed, preflight check) - } else if (priv->safety.ready_to_arm && priv->status.battery_warning == VEHICLE_BATTERY_WARNING_NONE) { + } else if (priv->armed.ready_to_arm && priv->status.battery_warning == VEHICLE_BATTERY_WARNING_NONE) { pattern = 0x38; // ***___ slow blink (disarmed, ready) } else { diff --git a/src/modules/mavlink/orb_listener.c b/src/modules/mavlink/orb_listener.c index 95bc8fdc0d..57a5d5dd50 100644 --- a/src/modules/mavlink/orb_listener.c +++ b/src/modules/mavlink/orb_listener.c @@ -70,7 +70,7 @@ struct vehicle_local_position_s local_pos; struct vehicle_status_s v_status; struct rc_channels_s rc; struct rc_input_values rc_raw; -struct actuator_safety_s safety; +struct actuator_armed_s armed; struct actuator_controls_effective_s actuators_0; struct vehicle_attitude_s att; @@ -110,7 +110,7 @@ static void l_global_position_setpoint(const struct listener *l); static void l_local_position_setpoint(const struct listener *l); static void l_attitude_setpoint(const struct listener *l); static void l_actuator_outputs(const struct listener *l); -static void l_actuator_safety(const struct listener *l); +static void l_actuator_armed(const struct listener *l); static void l_manual_control_setpoint(const struct listener *l); static void l_vehicle_attitude_controls(const struct listener *l); static void l_debug_key_value(const struct listener *l); @@ -135,7 +135,7 @@ static const struct listener listeners[] = { {l_actuator_outputs, &mavlink_subs.act_1_sub, 1}, {l_actuator_outputs, &mavlink_subs.act_2_sub, 2}, {l_actuator_outputs, &mavlink_subs.act_3_sub, 3}, - {l_actuator_safety, &mavlink_subs.safety_sub, 0}, + {l_actuator_armed, &mavlink_subs.armed_sub, 0}, {l_manual_control_setpoint, &mavlink_subs.man_control_sp_sub, 0}, {l_vehicle_attitude_controls, &mavlink_subs.actuators_sub, 0}, {l_debug_key_value, &mavlink_subs.debug_key_value, 0}, @@ -269,7 +269,7 @@ l_vehicle_status(const struct listener *l) { /* immediately communicate state changes back to user */ orb_copy(ORB_ID(vehicle_status), status_sub, &v_status); - orb_copy(ORB_ID(actuator_safety), mavlink_subs.safety_sub, &safety); + orb_copy(ORB_ID(actuator_armed), mavlink_subs.armed_sub, &armed); /* enable or disable HIL */ set_hil_on_off(v_status.flag_hil_enabled); @@ -466,7 +466,7 @@ l_actuator_outputs(const struct listener *l) act_outputs.output[7]); /* only send in HIL mode */ - if (mavlink_hil_enabled && safety.armed) { + if (mavlink_hil_enabled && armed.armed) { /* translate the current syste state to mavlink state and mode */ uint8_t mavlink_state = 0; @@ -538,9 +538,9 @@ l_actuator_outputs(const struct listener *l) } void -l_actuator_safety(const struct listener *l) +l_actuator_armed(const struct listener *l) { - orb_copy(ORB_ID(actuator_safety), mavlink_subs.safety_sub, &safety); + orb_copy(ORB_ID(actuator_armed), mavlink_subs.armed_sub, &armed); } void @@ -759,8 +759,8 @@ uorb_receive_start(void) orb_set_interval(mavlink_subs.act_3_sub, 100); /* 10Hz updates */ /* --- ACTUATOR ARMED VALUE --- */ - mavlink_subs.safety_sub = orb_subscribe(ORB_ID(actuator_safety)); - orb_set_interval(mavlink_subs.safety_sub, 100); /* 10Hz updates */ + mavlink_subs.armed_sub = orb_subscribe(ORB_ID(actuator_armed)); + orb_set_interval(mavlink_subs.armed_sub, 100); /* 10Hz updates */ /* --- MAPPED MANUAL CONTROL INPUTS --- */ mavlink_subs.man_control_sp_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); diff --git a/src/modules/mavlink/orb_topics.h b/src/modules/mavlink/orb_topics.h index 3b968b9110..506f73105d 100644 --- a/src/modules/mavlink/orb_topics.h +++ b/src/modules/mavlink/orb_topics.h @@ -60,7 +60,7 @@ #include #include #include -#include +#include #include #include #include @@ -79,6 +79,7 @@ struct mavlink_subscriptions { int man_control_sp_sub; int safety_sub; int actuators_sub; + int armed_sub; int local_pos_sub; int spa_sub; int spl_sub; diff --git a/src/modules/mavlink_onboard/mavlink.c b/src/modules/mavlink_onboard/mavlink.c index 2d5a64ee87..9ed2c6c12e 100644 --- a/src/modules/mavlink_onboard/mavlink.c +++ b/src/modules/mavlink_onboard/mavlink.c @@ -273,7 +273,7 @@ void mavlink_update_system(void) } void -get_mavlink_mode_and_state(const struct vehicle_control_mode_s *control_mode, const struct actuator_safety_s *safety, +get_mavlink_mode_and_state(const struct vehicle_control_mode_s *control_mode, const struct actuator_armed_s *armed, uint8_t *mavlink_state, uint8_t *mavlink_mode) { /* reset MAVLink mode bitfield */ @@ -284,12 +284,12 @@ get_mavlink_mode_and_state(const struct vehicle_control_mode_s *control_mode, co *mavlink_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED; } - if (safety->hil_enabled) { + if (control_mode->flag_system_hil_enabled) { *mavlink_mode |= MAV_MODE_FLAG_HIL_ENABLED; } /* set arming state */ - if (safety->armed) { + if (armed->armed) { *mavlink_mode |= MAV_MODE_FLAG_SAFETY_ARMED; } else { *mavlink_mode &= ~MAV_MODE_FLAG_SAFETY_ARMED; @@ -370,7 +370,7 @@ int mavlink_thread_main(int argc, char *argv[]) /* XXX this is never written? */ struct vehicle_status_s v_status; struct vehicle_control_mode_s control_mode; - struct actuator_safety_s safety; + struct actuator_armed_s armed; /* work around some stupidity in task_create's argv handling */ argc -= 2; @@ -438,7 +438,7 @@ int mavlink_thread_main(int argc, char *argv[]) /* translate the current system state to mavlink state and mode */ uint8_t mavlink_state = 0; uint8_t mavlink_mode = 0; - get_mavlink_mode_and_state(&control_mode, &safety, &mavlink_state, &mavlink_mode); + get_mavlink_mode_and_state(&control_mode, &armed, &mavlink_state, &mavlink_mode); /* send heartbeat */ mavlink_msg_heartbeat_send(chan, mavlink_system.type, MAV_AUTOPILOT_PX4, mavlink_mode, v_status.navigation_state, mavlink_state); diff --git a/src/modules/mavlink_onboard/orb_topics.h b/src/modules/mavlink_onboard/orb_topics.h index f01f09e12d..1b49c9ce4e 100644 --- a/src/modules/mavlink_onboard/orb_topics.h +++ b/src/modules/mavlink_onboard/orb_topics.h @@ -56,7 +56,7 @@ #include #include #include -#include +#include #include #include #include diff --git a/src/modules/mavlink_onboard/util.h b/src/modules/mavlink_onboard/util.h index c6a2e52bf0..c84b6fd26e 100644 --- a/src/modules/mavlink_onboard/util.h +++ b/src/modules/mavlink_onboard/util.h @@ -51,5 +51,5 @@ extern volatile bool thread_should_exit; * Translate the custom state into standard mavlink modes and state. */ extern void -get_mavlink_mode_and_state(const struct vehicle_control_mode_s *control_mode, const struct actuator_safety_s *safety, +get_mavlink_mode_and_state(const struct vehicle_control_mode_s *control_mode, const struct actuator_armed_s *armed, uint8_t *mavlink_state, uint8_t *mavlink_mode); diff --git a/src/modules/multirotor_att_control/multirotor_att_control_main.c b/src/modules/multirotor_att_control/multirotor_att_control_main.c index 70a61fc4ec..20502c0eae 100644 --- a/src/modules/multirotor_att_control/multirotor_att_control_main.c +++ b/src/modules/multirotor_att_control/multirotor_att_control_main.c @@ -58,7 +58,7 @@ #include #include #include -#include +#include #include #include #include @@ -94,8 +94,8 @@ mc_thread_main(int argc, char *argv[]) /* declare and safely initialize all structs */ struct vehicle_control_mode_s control_mode; memset(&control_mode, 0, sizeof(control_mode)); - struct actuator_safety_s safety; - memset(&safety, 0, sizeof(safety)); + struct actuator_armed_s armed; + memset(&armed, 0, sizeof(armed)); struct vehicle_attitude_s att; memset(&att, 0, sizeof(att)); struct vehicle_attitude_setpoint_s att_sp; @@ -121,7 +121,7 @@ mc_thread_main(int argc, char *argv[]) int att_setpoint_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint)); int setpoint_sub = orb_subscribe(ORB_ID(offboard_control_setpoint)); int control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode)); - int safety_sub = orb_subscribe(ORB_ID(actuator_safety)); + int armed_sub = orb_subscribe(ORB_ID(actuator_armed)); int manual_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); int sensor_sub = orb_subscribe(ORB_ID(sensor_combined)); @@ -210,10 +210,10 @@ mc_thread_main(int argc, char *argv[]) orb_copy(ORB_ID(vehicle_control_mode), control_mode_sub, &control_mode); } - orb_check(safety_sub, &updated); + orb_check(armed_sub, &updated); if (updated) { - orb_copy(ORB_ID(actuator_safety), safety_sub, &safety); + orb_copy(ORB_ID(actuator_armed), armed_sub, &armed); } /* get a local copy of manual setpoint */ @@ -261,7 +261,7 @@ mc_thread_main(int argc, char *argv[]) /* initialize to current yaw if switching to manual or att control */ if (control_mode.flag_control_attitude_enabled != flag_control_attitude_enabled || control_mode.flag_control_manual_enabled != flag_control_manual_enabled || - safety.armed != flag_armed) { + armed.armed != flag_armed) { att_sp.yaw_body = att.yaw; } @@ -275,7 +275,7 @@ mc_thread_main(int argc, char *argv[]) att_sp.pitch_body = manual.pitch; /* set attitude if arming */ - if (!flag_control_attitude_enabled && safety.armed) { + if (!flag_control_attitude_enabled && armed.armed) { att_sp.yaw_body = att.yaw; } @@ -366,7 +366,7 @@ mc_thread_main(int argc, char *argv[]) flag_control_manual_enabled = control_mode.flag_control_manual_enabled; flag_control_position_enabled = control_mode.flag_control_position_enabled; flag_control_velocity_enabled = control_mode.flag_control_velocity_enabled; - flag_armed = safety.armed; + flag_armed = armed.armed; perf_end(mc_loop_perf); } /* end of poll call for attitude updates */ diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index 80ee3adeef..b20d38b5ee 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -60,7 +60,7 @@ #include #include -#include +#include #include #include #include @@ -194,7 +194,7 @@ static int file_copy(const char *file_old, const char *file_new); static void handle_command(struct vehicle_command_s *cmd); -static void handle_status(struct actuator_safety_s *safety); +static void handle_status(struct actuator_armed_s *armed); /** * Create folder for current logging session. Store folder name in 'log_folder'. @@ -628,8 +628,8 @@ int sdlog2_thread_main(int argc, char *argv[]) struct vehicle_status_s buf_status; memset(&buf_status, 0, sizeof(buf_status)); - struct actuator_safety_s buf_safety; - memset(&buf_safety, 0, sizeof(buf_safety)); + struct actuator_armed_s buf_armed; + memset(&buf_armed, 0, sizeof(buf_armed)); /* warning! using union here to save memory, elements should be used separately! */ union { @@ -659,7 +659,7 @@ int sdlog2_thread_main(int argc, char *argv[]) struct { int cmd_sub; int status_sub; - int safety_sub; + int armed_sub; int sensor_sub; int att_sub; int att_sp_sub; @@ -717,9 +717,9 @@ int sdlog2_thread_main(int argc, char *argv[]) fds[fdsc_count].events = POLLIN; fdsc_count++; - /* --- SAFETY --- */ - subs.safety_sub = orb_subscribe(ORB_ID(actuator_safety)); - fds[fdsc_count].fd = subs.safety_sub; + /* --- ACTUATOR ARMED --- */ + subs.armed_sub = orb_subscribe(ORB_ID(actuator_armed)); + fds[fdsc_count].fd = subs.armed_sub; fds[fdsc_count].events = POLLIN; fdsc_count++; @@ -896,12 +896,12 @@ int sdlog2_thread_main(int argc, char *argv[]) handled_topics++; } - /* --- SAFETY- LOG MANAGEMENT --- */ + /* --- ARMED- LOG MANAGEMENT --- */ if (fds[ifds++].revents & POLLIN) { - orb_copy(ORB_ID(actuator_safety), subs.safety_sub, &buf_safety); + orb_copy(ORB_ID(actuator_armed), subs.armed_sub, &buf_armed); if (log_when_armed) { - handle_status(&buf_safety); + handle_status(&buf_armed); } handled_topics++; @@ -912,7 +912,7 @@ int sdlog2_thread_main(int argc, char *argv[]) orb_copy(ORB_ID(vehicle_status), subs.status_sub, &buf_status); //if (log_when_armed) { - // handle_status(&buf_safety); + // handle_status(&buf_armed); //} //handled_topics++; @@ -944,7 +944,7 @@ int sdlog2_thread_main(int argc, char *argv[]) log_msg.body.log_STAT.flight_mode = 0; log_msg.body.log_STAT.manual_control_mode = 0; log_msg.body.log_STAT.manual_sas_mode = 0; - log_msg.body.log_STAT.armed = (unsigned char) buf_safety.armed; /* XXX fmu armed correct? */ + log_msg.body.log_STAT.armed = (unsigned char) buf_armed.armed; /* XXX fmu armed correct? */ log_msg.body.log_STAT.battery_voltage = buf_status.voltage_battery; log_msg.body.log_STAT.battery_current = buf_status.current_battery; log_msg.body.log_STAT.battery_remaining = buf_status.battery_remaining; @@ -1348,10 +1348,10 @@ void handle_command(struct vehicle_command_s *cmd) } } -void handle_status(struct actuator_safety_s *safety) +void handle_status(struct actuator_armed_s *armed) { - if (safety->armed != flag_system_armed) { - flag_system_armed = safety->armed; + if (armed->armed != flag_system_armed) { + flag_system_armed = armed->armed; if (flag_system_armed) { sdlog2_start_log(); diff --git a/src/modules/uORB/objects_common.cpp b/src/modules/uORB/objects_common.cpp index a061fe676f..ed77bb7eff 100644 --- a/src/modules/uORB/objects_common.cpp +++ b/src/modules/uORB/objects_common.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * 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 @@ -77,6 +77,9 @@ ORB_DEFINE(home_position, struct home_position_s); #include "topics/vehicle_status.h" ORB_DEFINE(vehicle_status, struct vehicle_status_s); +#include "topics/safety.h" +ORB_DEFINE(safety, struct safety_s); + #include "topics/battery_status.h" ORB_DEFINE(battery_status, struct battery_status_s); @@ -153,8 +156,8 @@ ORB_DEFINE(actuator_controls_1, struct actuator_controls_s); ORB_DEFINE(actuator_controls_2, struct actuator_controls_s); ORB_DEFINE(actuator_controls_3, struct actuator_controls_s); -#include "topics/actuator_safety.h" -ORB_DEFINE(actuator_safety, struct actuator_safety_s); +#include "topics/actuator_armed.h" +ORB_DEFINE(actuator_armed, struct actuator_armed_s); /* actuator controls, as set by actuators / mixers after limiting */ #include "topics/actuator_controls_effective.h" diff --git a/src/modules/uORB/topics/actuator_safety.h b/src/modules/uORB/topics/actuator_armed.h similarity index 73% rename from src/modules/uORB/topics/actuator_safety.h rename to src/modules/uORB/topics/actuator_armed.h index c431217ab1..6e944ffee3 100644 --- a/src/modules/uORB/topics/actuator_safety.h +++ b/src/modules/uORB/topics/actuator_armed.h @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * 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 @@ -32,35 +32,27 @@ ****************************************************************************/ /** - * @file actuator_controls.h + * @file actuator_armed.h * - * Actuator control topics - mixer inputs. + * Actuator armed topic * - * Values published to these topics are the outputs of the vehicle control - * system, and are expected to be mixed and used to drive the actuators - * (servos, speed controls, etc.) that operate the vehicle. - * - * Each topic can be published by a single controller */ -#ifndef TOPIC_ACTUATOR_SAFETY_H -#define TOPIC_ACTUATOR_SAFETY_H +#ifndef TOPIC_ACTUATOR_ARMED_H +#define TOPIC_ACTUATOR_ARMED_H #include #include "../uORB.h" /** global 'actuator output is live' control. */ -struct actuator_safety_s { +struct actuator_armed_s { uint64_t timestamp; - bool safety_switch_available; /**< Set to true if a safety switch is connected */ - bool safety_off; /**< Set to true if safety is off */ bool armed; /**< Set to true if system is armed */ bool ready_to_arm; /**< Set to true if system is ready to be armed */ bool lockdown; /**< Set to true if actuators are forced to being disabled (due to emergency or HIL) */ - bool hil_enabled; /**< Set to true if hardware-in-the-loop (HIL) is enabled */ }; -ORB_DECLARE(actuator_safety); +ORB_DECLARE(actuator_armed); #endif \ No newline at end of file diff --git a/src/modules/uORB/topics/safety.h b/src/modules/uORB/topics/safety.h new file mode 100644 index 0000000000..a5d21cd4ad --- /dev/null +++ b/src/modules/uORB/topics/safety.h @@ -0,0 +1,57 @@ +/**************************************************************************** + * + * 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 safety.h + * + * Safety topic to pass safety state from px4io driver to commander + * This concerns only the safety button of the px4io but has nothing to do + * with arming/disarming. + */ + +#ifndef TOPIC_SAFETY_H +#define TOPIC_SAFETY_H + +#include +#include "../uORB.h" + +struct safety_s { + + uint64_t timestamp; + bool safety_switch_available; /**< Set to true if a safety switch is connected */ + bool safety_off; /**< Set to true if safety is off */ +}; + +ORB_DECLARE(safety); + +#endif \ No newline at end of file diff --git a/src/modules/uORB/topics/vehicle_control_mode.h b/src/modules/uORB/topics/vehicle_control_mode.h index 177e30898b..02bf5568ab 100644 --- a/src/modules/uORB/topics/vehicle_control_mode.h +++ b/src/modules/uORB/topics/vehicle_control_mode.h @@ -69,9 +69,13 @@ struct vehicle_control_mode_s bool flag_system_emergency; bool flag_preflight_calibration; + // XXX needs yet to be set by state machine helper + bool flag_system_hil_enabled; + bool flag_control_manual_enabled; /**< true if manual input is mixed in */ bool flag_control_offboard_enabled; /**< true if offboard control input is on */ - bool flag_auto_enabled; + // XXX shouldn't be necessairy + //bool flag_auto_enabled; bool flag_control_rates_enabled; /**< true if rates are stabilized */ bool flag_control_attitude_enabled; /**< true if attitude stabilization is mixed in */ bool flag_control_velocity_enabled; /**< true if speed (implies direction) is controlled */ From 3e161049ac4e953f8c0084b1872b544de6189f5d Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Tue, 16 Jul 2013 09:24:21 +0200 Subject: [PATCH 082/109] Got rid of useless orb_receive_loop, moved some helper functions --- src/modules/commander/commander.c | 196 ++++++----------------- src/modules/commander/commander_helper.c | 46 ++++++ src/modules/commander/commander_helper.h | 7 +- 3 files changed, 103 insertions(+), 146 deletions(-) diff --git a/src/modules/commander/commander.c b/src/modules/commander/commander.c index c4ee605ccf..a74c9ebe93 100644 --- a/src/modules/commander/commander.c +++ b/src/modules/commander/commander.c @@ -132,8 +132,7 @@ extern struct system_load_s system_load; #define LOCAL_POSITION_TIMEOUT 1000000 /**< consider the local position estimate invalid after 1s */ -/* File descriptors */ -static int leds; +/* Mavlink file descriptors */ static int mavlink_fd; /* flags */ @@ -159,8 +158,9 @@ enum { } low_prio_task; -/* pthread loops */ -void *orb_receive_loop(void *arg); +/** + * Loop that runs at a lower rate and priority for calibration and parameter tasks. + */ void *commander_low_prio_loop(void *arg); __EXPORT int commander_main(int argc, char *argv[]); @@ -170,88 +170,16 @@ __EXPORT int commander_main(int argc, char *argv[]); */ int commander_thread_main(int argc, char *argv[]); -int led_init(void); -void led_deinit(void); -int led_toggle(int led); -int led_on(int led); -int led_off(int led); -void do_reboot(void); - - +/** + * React to commands that are sent e.g. from the mavlink module. + */ void handle_command(int status_pub, struct vehicle_status_s *current_status, struct vehicle_command_s *cmd, int armed_pub, struct actuator_armed_s *armed); -// int trigger_audio_alarm(uint8_t old_mode, uint8_t old_state, uint8_t new_mode, uint8_t new_state); - - - /** * Print the correct usage. */ void usage(const char *reason); -/** - * Sort calibration values. - * - * Sorts the calibration values with bubble sort. - * - * @param a The array to sort - * @param n The number of entries in the array - */ -// static void cal_bsort(float a[], int n); - - -int led_init() -{ - leds = open(LED_DEVICE_PATH, 0); - - if (leds < 0) { - warnx("LED: open fail\n"); - return ERROR; - } - - if (ioctl(leds, LED_ON, LED_BLUE) || ioctl(leds, LED_ON, LED_AMBER)) { - warnx("LED: ioctl fail\n"); - return ERROR; - } - - return 0; -} - -void led_deinit() -{ - close(leds); -} - -int led_toggle(int led) -{ - static int last_blue = LED_ON; - static int last_amber = LED_ON; - - if (led == LED_BLUE) last_blue = (last_blue == LED_ON) ? LED_OFF : LED_ON; - - if (led == LED_AMBER) last_amber = (last_amber == LED_ON) ? LED_OFF : LED_ON; - - return ioctl(leds, ((led == LED_BLUE) ? last_blue : last_amber), led); -} - -int led_on(int led) -{ - return ioctl(leds, LED_ON, led); -} - -int led_off(int led) -{ - return ioctl(leds, LED_OFF, led); -} - -void do_reboot() -{ - mavlink_log_critical(mavlink_fd, "REBOOTING SYSTEM"); - usleep(500000); - up_systemreset(); - /* SPECIAL CASE: NEVER RETURNS FROM THIS FUNCTION CALL */ -} - void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_status, struct vehicle_command_s *cmd, int armed_pub, struct actuator_armed_s *armed) @@ -317,8 +245,10 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta /* reboot is executed later, after positive tune is sent */ result = VEHICLE_CMD_RESULT_ACCEPTED; tune_positive(); - /* SPECIAL CASE: SYSTEM WILL NEVER RETURN HERE */ - do_reboot(); + mavlink_log_critical(mavlink_fd, "REBOOTING SYSTEM"); + usleep(500000); + up_systemreset(); + /* SPECIAL CASE: NEVER RETURNS FROM THIS FUNCTION CALL */ } else { /* system may return here */ @@ -526,59 +456,6 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta } -void *orb_receive_loop(void *arg) //handles status information coming from subsystems (present, enabled, health), these values do not indicate the quality (variance) of the signal -{ - /* Set thread name */ - prctl(PR_SET_NAME, "commander orb rcv", getpid()); - - /* Subscribe to command topic */ - int subsys_sub = orb_subscribe(ORB_ID(subsystem_info)); - struct subsystem_info_s info; - - struct vehicle_status_s *vstatus = (struct vehicle_status_s *)arg; - - while (!thread_should_exit) { - struct pollfd fds[1] = { { .fd = subsys_sub, .events = POLLIN } }; - - if (poll(fds, 1, 5000) == 0) { - /* timeout, but this is no problem, silently ignore */ - } else { - /* got command */ - orb_copy(ORB_ID(subsystem_info), subsys_sub, &info); - - warnx("Subsys changed: %d\n", (int)info.subsystem_type); - - /* mark / unmark as present */ - if (info.present) { - vstatus->onboard_control_sensors_present |= info.subsystem_type; - - } else { - vstatus->onboard_control_sensors_present &= ~info.subsystem_type; - } - - /* mark / unmark as enabled */ - if (info.enabled) { - vstatus->onboard_control_sensors_enabled |= info.subsystem_type; - - } else { - vstatus->onboard_control_sensors_enabled &= ~info.subsystem_type; - } - - /* mark / unmark as ok */ - if (info.ok) { - vstatus->onboard_control_sensors_health |= info.subsystem_type; - - } else { - vstatus->onboard_control_sensors_health &= ~info.subsystem_type; - } - } - } - - close(subsys_sub); - - return NULL; -} - /* * Provides a coarse estimate of remaining battery power. * @@ -709,9 +586,7 @@ int commander_thread_main(int argc, char *argv[]) /* welcome user */ warnx("[commander] starting"); - /* pthreads for command and subsystem info handling */ - // pthread_t command_handling_thread; - pthread_t subsystem_info_thread; + /* pthread for slow low prio thread */ pthread_t commander_low_prio_thread; /* initialize */ @@ -807,12 +682,6 @@ int commander_thread_main(int argc, char *argv[]) // XXX needed? mavlink_log_info(mavlink_fd, "system is running"); - /* create pthreads */ - pthread_attr_t subsystem_info_attr; - pthread_attr_init(&subsystem_info_attr); - pthread_attr_setstacksize(&subsystem_info_attr, 2048); - pthread_create(&subsystem_info_thread, &subsystem_info_attr, orb_receive_loop, ¤t_status); - pthread_attr_t commander_low_prio_attr; pthread_attr_init(&commander_low_prio_attr); pthread_attr_setstacksize(&commander_low_prio_attr, 2048); @@ -905,6 +774,11 @@ int commander_thread_main(int argc, char *argv[]) memset(&battery, 0, sizeof(battery)); battery.voltage_v = 0.0f; + /* Subscribe to subsystem info topic */ + int subsys_sub = orb_subscribe(ORB_ID(subsystem_info)); + struct subsystem_info_s info; + memset(&info, 0, sizeof(info)); + // uint8_t vehicle_state_previous = current_status.state_machine; float voltage_previous = 0.0f; @@ -1073,6 +947,39 @@ int commander_thread_main(int argc, char *argv[]) } } + /* update subsystem */ + orb_check(subsys_sub, &new_data); + + if (new_data) { + orb_copy(ORB_ID(subsystem_info), subsys_sub, &info); + + warnx("Subsys changed: %d\n", (int)info.subsystem_type); + + /* mark / unmark as present */ + if (info.present) { + current_status.onboard_control_sensors_present |= info.subsystem_type; + + } else { + current_status.onboard_control_sensors_present &= ~info.subsystem_type; + } + + /* mark / unmark as enabled */ + if (info.enabled) { + current_status.onboard_control_sensors_enabled |= info.subsystem_type; + + } else { + current_status.onboard_control_sensors_enabled &= ~info.subsystem_type; + } + + /* mark / unmark as ok */ + if (info.ok) { + current_status.onboard_control_sensors_health |= info.subsystem_type; + + } else { + current_status.onboard_control_sensors_health &= ~info.subsystem_type; + } + } + /* Slow but important 8 Hz checks */ if (counter % ((1000000 / COMMANDER_MONITORING_INTERVAL) / 8) == 0) { @@ -1801,8 +1708,6 @@ int commander_thread_main(int argc, char *argv[]) } /* wait for threads to complete */ - // pthread_join(command_handling_thread, NULL); - pthread_join(subsystem_info_thread, NULL); pthread_join(commander_low_prio_thread, NULL); /* close fds */ @@ -1814,6 +1719,7 @@ int commander_thread_main(int argc, char *argv[]) close(sensor_sub); close(safety_sub); close(cmd_sub); + close(subsys_sub); warnx("exiting"); fflush(stdout); diff --git a/src/modules/commander/commander_helper.c b/src/modules/commander/commander_helper.c index a75b5dec3c..fb5c478850 100644 --- a/src/modules/commander/commander_helper.c +++ b/src/modules/commander/commander_helper.c @@ -51,6 +51,7 @@ #include #include #include +#include #include "commander_helper.h" @@ -127,3 +128,48 @@ void tune_stop() ioctl(buzzer, TONE_SET_ALARM, 0); } +static int leds; + +int led_init() +{ + leds = open(LED_DEVICE_PATH, 0); + + if (leds < 0) { + warnx("LED: open fail\n"); + return ERROR; + } + + if (ioctl(leds, LED_ON, LED_BLUE) || ioctl(leds, LED_ON, LED_AMBER)) { + warnx("LED: ioctl fail\n"); + return ERROR; + } + + return 0; +} + +void led_deinit() +{ + close(leds); +} + +int led_toggle(int led) +{ + static int last_blue = LED_ON; + static int last_amber = LED_ON; + + if (led == LED_BLUE) last_blue = (last_blue == LED_ON) ? LED_OFF : LED_ON; + + if (led == LED_AMBER) last_amber = (last_amber == LED_ON) ? LED_OFF : LED_ON; + + return ioctl(leds, ((led == LED_BLUE) ? last_blue : last_amber), led); +} + +int led_on(int led) +{ + return ioctl(leds, LED_ON, led); +} + +int led_off(int led) +{ + return ioctl(leds, LED_OFF, led); +} \ No newline at end of file diff --git a/src/modules/commander/commander_helper.h b/src/modules/commander/commander_helper.h index b06e851691..71a257c86d 100644 --- a/src/modules/commander/commander_helper.h +++ b/src/modules/commander/commander_helper.h @@ -60,7 +60,12 @@ void tune_negative(void); int tune_arm(void); int tune_critical_bat(void); int tune_low_bat(void); - void tune_stop(void); +int led_init(void); +void led_deinit(void); +int led_toggle(int led); +int led_on(int led); +int led_off(int led); + #endif /* COMMANDER_HELPER_H_ */ From 08926019ea4203760a225e957d27328862182ce1 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Tue, 16 Jul 2013 09:35:31 +0200 Subject: [PATCH 083/109] Just some reordering in commander --- src/modules/commander/commander.c | 183 ++++++++--------------- src/modules/commander/commander_helper.c | 43 ++++++ src/modules/commander/commander_helper.h | 9 ++ 3 files changed, 118 insertions(+), 117 deletions(-) diff --git a/src/modules/commander/commander.c b/src/modules/commander/commander.c index a74c9ebe93..d7bf6ac57f 100644 --- a/src/modules/commander/commander.c +++ b/src/modules/commander/commander.c @@ -159,16 +159,19 @@ enum { /** - * Loop that runs at a lower rate and priority for calibration and parameter tasks. + * The daemon app only briefly exists to start + * the background job. The stack size assigned in the + * Makefile does only apply to this management task. + * + * The actual stack size should be set in the call + * to task_create(). */ -void *commander_low_prio_loop(void *arg); - __EXPORT int commander_main(int argc, char *argv[]); /** - * Mainloop of commander. + * Print the correct usage. */ -int commander_thread_main(int argc, char *argv[]); +void usage(const char *reason); /** * React to commands that are sent e.g. from the mavlink module. @@ -176,11 +179,67 @@ int commander_thread_main(int argc, char *argv[]); void handle_command(int status_pub, struct vehicle_status_s *current_status, struct vehicle_command_s *cmd, int armed_pub, struct actuator_armed_s *armed); /** - * Print the correct usage. + * Mainloop of commander. */ -void usage(const char *reason); +int commander_thread_main(int argc, char *argv[]); + +/** + * Loop that runs at a lower rate and priority for calibration and parameter tasks. + */ +void *commander_low_prio_loop(void *arg); +int commander_main(int argc, char *argv[]) +{ + if (argc < 1) + usage("missing command"); + + if (!strcmp(argv[1], "start")) { + + if (thread_running) { + warnx("commander already running\n"); + /* this is not an error */ + exit(0); + } + + thread_should_exit = false; + daemon_task = task_spawn_cmd("commander", + SCHED_DEFAULT, + SCHED_PRIORITY_MAX - 40, + 3000, + commander_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("\tcommander is running\n"); + + } else { + warnx("\tcommander not started\n"); + } + + exit(0); + } + + usage("unrecognized command"); + exit(1); +} + +void usage(const char *reason) +{ + if (reason) + fprintf(stderr, "%s\n", reason); + + fprintf(stderr, "usage: daemon {start|stop|status} [-p ]\n\n"); + exit(1); +} void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_status, struct vehicle_command_s *cmd, int armed_pub, struct actuator_armed_s *armed) { @@ -456,116 +515,6 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta } -/* - * Provides a coarse estimate of remaining battery power. - * - * The estimate is very basic and based on decharging voltage curves. - * - * @return the estimated remaining capacity in 0..1 - */ -float battery_remaining_estimate_voltage(float voltage); - -PARAM_DEFINE_FLOAT(BAT_V_EMPTY, 3.2f); -PARAM_DEFINE_FLOAT(BAT_V_FULL, 4.05f); -PARAM_DEFINE_FLOAT(BAT_N_CELLS, 3); - -float battery_remaining_estimate_voltage(float voltage) -{ - float ret = 0; - static param_t bat_volt_empty; - static param_t bat_volt_full; - static param_t bat_n_cells; - static bool initialized = false; - static unsigned int counter = 0; - static float ncells = 3; - // XXX change cells to int (and param to INT32) - - if (!initialized) { - bat_volt_empty = param_find("BAT_V_EMPTY"); - bat_volt_full = param_find("BAT_V_FULL"); - bat_n_cells = param_find("BAT_N_CELLS"); - initialized = true; - } - - static float chemistry_voltage_empty = 3.2f; - static float chemistry_voltage_full = 4.05f; - - if (counter % 100 == 0) { - param_get(bat_volt_empty, &chemistry_voltage_empty); - param_get(bat_volt_full, &chemistry_voltage_full); - param_get(bat_n_cells, &ncells); - } - - counter++; - - ret = (voltage - ncells * chemistry_voltage_empty) / (ncells * (chemistry_voltage_full - chemistry_voltage_empty)); - - /* limit to sane values */ - ret = (ret < 0) ? 0 : ret; - ret = (ret > 1) ? 1 : ret; - return ret; -} - -void usage(const char *reason) -{ - if (reason) - fprintf(stderr, "%s\n", reason); - - fprintf(stderr, "usage: daemon {start|stop|status} [-p ]\n\n"); - exit(1); -} - -/** - * The daemon app only briefly exists to start - * the background job. The stack size assigned in the - * Makefile does only apply to this management task. - * - * The actual stack size should be set in the call - * to task_create(). - */ -int commander_main(int argc, char *argv[]) -{ - if (argc < 1) - usage("missing command"); - - if (!strcmp(argv[1], "start")) { - - if (thread_running) { - warnx("commander already running\n"); - /* this is not an error */ - exit(0); - } - - thread_should_exit = false; - daemon_task = task_spawn_cmd("commander", - SCHED_DEFAULT, - SCHED_PRIORITY_MAX - 40, - 3000, - commander_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("\tcommander is running\n"); - - } else { - warnx("\tcommander not started\n"); - } - - exit(0); - } - - usage("unrecognized command"); - exit(1); -} - int commander_thread_main(int argc, char *argv[]) { /* not yet initialized */ diff --git a/src/modules/commander/commander_helper.c b/src/modules/commander/commander_helper.c index fb5c478850..199f73e6c8 100644 --- a/src/modules/commander/commander_helper.c +++ b/src/modules/commander/commander_helper.c @@ -49,6 +49,7 @@ #include #include #include +#include #include #include #include @@ -172,4 +173,46 @@ int led_on(int led) int led_off(int led) { return ioctl(leds, LED_OFF, led); +} + + +PARAM_DEFINE_FLOAT(BAT_V_EMPTY, 3.2f); +PARAM_DEFINE_FLOAT(BAT_V_FULL, 4.05f); +PARAM_DEFINE_FLOAT(BAT_N_CELLS, 3); + +float battery_remaining_estimate_voltage(float voltage) +{ + float ret = 0; + static param_t bat_volt_empty; + static param_t bat_volt_full; + static param_t bat_n_cells; + static bool initialized = false; + static unsigned int counter = 0; + static float ncells = 3; + // XXX change cells to int (and param to INT32) + + if (!initialized) { + bat_volt_empty = param_find("BAT_V_EMPTY"); + bat_volt_full = param_find("BAT_V_FULL"); + bat_n_cells = param_find("BAT_N_CELLS"); + initialized = true; + } + + static float chemistry_voltage_empty = 3.2f; + static float chemistry_voltage_full = 4.05f; + + if (counter % 100 == 0) { + param_get(bat_volt_empty, &chemistry_voltage_empty); + param_get(bat_volt_full, &chemistry_voltage_full); + param_get(bat_n_cells, &ncells); + } + + counter++; + + ret = (voltage - ncells * chemistry_voltage_empty) / (ncells * (chemistry_voltage_full - chemistry_voltage_empty)); + + /* limit to sane values */ + ret = (ret < 0) ? 0 : ret; + ret = (ret > 1) ? 1 : ret; + return ret; } \ No newline at end of file diff --git a/src/modules/commander/commander_helper.h b/src/modules/commander/commander_helper.h index 71a257c86d..c621b98232 100644 --- a/src/modules/commander/commander_helper.h +++ b/src/modules/commander/commander_helper.h @@ -68,4 +68,13 @@ int led_toggle(int led); int led_on(int led); int led_off(int led); +/** + * Provides a coarse estimate of remaining battery power. + * + * The estimate is very basic and based on decharging voltage curves. + * + * @return the estimated remaining capacity in 0..1 + */ +float battery_remaining_estimate_voltage(float voltage); + #endif /* COMMANDER_HELPER_H_ */ From 6dc3fcd1ad108bc830231c1da50982e18eb7f799 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Tue, 16 Jul 2013 10:05:51 +0200 Subject: [PATCH 084/109] Some more commander cleanup, param update handling code was doubled --- src/modules/commander/commander.c | 79 ++++++++++--------------------- 1 file changed, 24 insertions(+), 55 deletions(-) diff --git a/src/modules/commander/commander.c b/src/modules/commander/commander.c index d7bf6ac57f..b457d98a18 100644 --- a/src/modules/commander/commander.c +++ b/src/modules/commander/commander.c @@ -39,11 +39,6 @@ * @file commander.c * Main system state machine implementation. * - * @author Petri Tanskanen - * @author Lorenz Meier - * @author Thomas Gubler - * @author Julian Oes - * */ #include "commander.h" @@ -656,9 +651,23 @@ int commander_thread_main(int argc, char *argv[]) uint16_t stick_off_counter = 0; uint16_t stick_on_counter = 0; - /* XXX what exactly is this for? */ + /* To remember when last notification was sent */ uint64_t last_print_time = 0; + float voltage_previous = 0.0f; + + uint64_t last_idle_time = 0; + + uint64_t start_time = 0; + + /* XXX use this! */ + //uint64_t failsave_ll_start_time = 0; + + bool state_changed = true; + bool param_init_forced = true; + + bool new_data = false; + /* Subscribe to safety topic */ int safety_sub = orb_subscribe(ORB_ID(safety)); struct safety_s safety; @@ -702,6 +711,7 @@ int commander_thread_main(int argc, char *argv[]) struct sensor_combined_s sensors; memset(&sensors, 0, sizeof(sensors)); + /* Subscribe to differential pressure topic */ int diff_pres_sub = orb_subscribe(ORB_ID(differential_pressure)); struct differential_pressure_s diff_pres; memset(&diff_pres, 0, sizeof(diff_pres)); @@ -717,7 +727,7 @@ int commander_thread_main(int argc, char *argv[]) struct parameter_update_s param_changed; memset(¶m_changed, 0, sizeof(param_changed)); - /* subscribe to battery topic */ + /* Subscribe to battery topic */ int battery_sub = orb_subscribe(ORB_ID(battery_status)); struct battery_status_s battery; memset(&battery, 0, sizeof(battery)); @@ -728,28 +738,14 @@ int commander_thread_main(int argc, char *argv[]) struct subsystem_info_s info; memset(&info, 0, sizeof(info)); - // uint8_t vehicle_state_previous = current_status.state_machine; - float voltage_previous = 0.0f; - - uint64_t last_idle_time = 0; - /* now initialized */ commander_initialized = true; thread_running = true; - uint64_t start_time = hrt_absolute_time(); - - /* XXX use this! */ - //uint64_t failsave_ll_start_time = 0; - - bool state_changed = true; - bool param_init_forced = true; + start_time = hrt_absolute_time(); while (!thread_should_exit) { - /* Get current values */ - bool new_data; - /* update parameters */ orb_check(param_changed_sub, &new_data); @@ -758,11 +754,10 @@ int commander_thread_main(int argc, char *argv[]) /* parameters changed */ orb_copy(ORB_ID(parameter_update), param_changed_sub, ¶m_changed); - /* update parameters */ if (!armed.armed) { if (param_get(_param_sys_type, &(current_status.system_type)) != OK) { - warnx("failed setting new system type"); + warnx("failed getting new system type"); } /* disable manual override for all systems that rely on electronic stabilization */ @@ -816,37 +811,6 @@ int commander_thread_main(int argc, char *argv[]) /* handle it */ handle_command(status_pub, ¤t_status, &cmd, armed_pub, &armed); } - - - /* update parameters */ - orb_check(param_changed_sub, &new_data); - - if (new_data || param_init_forced) { - param_init_forced = false; - /* parameters changed */ - orb_copy(ORB_ID(parameter_update), param_changed_sub, ¶m_changed); - - /* update parameters */ - if (!armed.armed) { - if (param_get(_param_sys_type, &(current_status.system_type)) != OK) { - warnx("failed setting new system type"); - } - - /* disable manual override for all systems that rely on electronic stabilization */ - if (current_status.system_type == VEHICLE_TYPE_QUADROTOR || - current_status.system_type == VEHICLE_TYPE_HEXAROTOR || - current_status.system_type == VEHICLE_TYPE_OCTOROTOR) { - current_status.flag_external_manual_override_ok = false; - - } else { - current_status.flag_external_manual_override_ok = true; - } - - /* check and update system / component ID */ - param_get(_param_system_id, &(current_status.system_id)); - param_get(_param_component_id, &(current_status.component_id)); - } - } /* update safety topic */ orb_check(safety_sub, &new_data); @@ -1664,11 +1628,16 @@ int commander_thread_main(int argc, char *argv[]) buzzer_deinit(); close(sp_man_sub); close(sp_offboard_sub); + close(local_position_sub); close(global_position_sub); + close(gps_sub); close(sensor_sub); close(safety_sub); close(cmd_sub); close(subsys_sub); + close(diff_pres_sub); + close(param_changed_sub); + close(battery_sub); warnx("exiting"); fflush(stdout); From 76edfa896b57782d7a4333bcf7a582952cb0fae4 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Tue, 16 Jul 2013 13:24:02 +0200 Subject: [PATCH 085/109] Fixed disarming bug, use flag instead of mode switch --- src/modules/commander/commander.c | 8 +++----- 1 file changed, 3 insertions(+), 5 deletions(-) diff --git a/src/modules/commander/commander.c b/src/modules/commander/commander.c index b457d98a18..144fda79ac 100644 --- a/src/modules/commander/commander.c +++ b/src/modules/commander/commander.c @@ -1387,8 +1387,7 @@ int commander_thread_main(int argc, char *argv[]) * Check if left stick is in lower left position --> switch to standby state. * Do this only for multirotors, not for fixed wing aircraft. */ - // printf("checking\n"); - if ((sp_man.yaw < -STICK_ON_OFF_LIMIT) && (sp_man.throttle < STICK_THRUST_RANGE * 0.2f)) { + if ((sp_man.yaw < -STICK_ON_OFF_LIMIT) && (sp_man.throttle < STICK_THRUST_RANGE * 0.1f)) { if (stick_off_counter > STICK_ON_OFF_COUNTER_LIMIT) { @@ -1396,7 +1395,7 @@ int commander_thread_main(int argc, char *argv[]) (current_status.system_type == VEHICLE_TYPE_HEXAROTOR) || (current_status.system_type == VEHICLE_TYPE_OCTOROTOR) ) { - if (current_status.mode_switch != MODE_SWITCH_MANUAL) { + if (control_mode.flag_control_position_enabled || control_mode.flag_control_velocity_enabled) { mavlink_log_critical(mavlink_fd, "DISARM DENY, go manual mode first"); tune_negative(); } else { @@ -1417,8 +1416,7 @@ int commander_thread_main(int argc, char *argv[]) } /* check if left stick is in lower right position and we're in manual --> arm */ - if (!control_mode.flag_control_position_enabled && !control_mode.flag_control_velocity_enabled && - sp_man.yaw > STICK_ON_OFF_LIMIT && + if (sp_man.yaw > STICK_ON_OFF_LIMIT && sp_man.throttle < STICK_THRUST_RANGE * 0.1f) { if (stick_on_counter > STICK_ON_OFF_COUNTER_LIMIT) { arming_state_transition(status_pub, ¤t_status, ARMING_STATE_ARMED, armed_pub, &armed, mavlink_fd); From bcdedd9a35a5b9ebf3851a0d472adab8d3e7edac Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Tue, 16 Jul 2013 18:55:32 +0200 Subject: [PATCH 086/109] Changed location of lots of flags and conditions, needs testing and more work --- src/drivers/blinkm/blinkm.cpp | 2 +- src/drivers/px4io/px4io.cpp | 18 +-- .../attitude_estimator_ekf_main.cpp | 14 +- .../attitude_estimator_so3_comp_main.cpp | 14 +- src/modules/commander/commander.c | 122 ++++++++---------- src/modules/commander/state_machine_helper.c | 11 +- src/modules/commander/state_machine_helper.h | 2 +- src/modules/controllib/fixedwing.cpp | 24 ++-- .../fixedwing_backside_main.cpp | 6 +- src/modules/mavlink/mavlink.c | 15 ++- src/modules/mavlink/orb_listener.c | 5 +- src/modules/mavlink_onboard/mavlink.c | 4 +- src/modules/sdlog2/sdlog2.c | 4 +- src/modules/sensors/sensors.cpp | 32 ++--- .../uORB/topics/vehicle_control_mode.h | 3 - src/modules/uORB/topics/vehicle_status.h | 37 ++---- 16 files changed, 149 insertions(+), 164 deletions(-) diff --git a/src/drivers/blinkm/blinkm.cpp b/src/drivers/blinkm/blinkm.cpp index e83a87aa93..9bb020a6b4 100644 --- a/src/drivers/blinkm/blinkm.cpp +++ b/src/drivers/blinkm/blinkm.cpp @@ -536,7 +536,7 @@ BlinkM::led() /* looking for lipo cells that are connected */ printf(" checking cells\n"); for(num_of_cells = 2; num_of_cells < 7; num_of_cells++) { - if(vehicle_status_raw.voltage_battery < num_of_cells * MAX_CELL_VOLTAGE) break; + if(vehicle_status_raw.battery_voltage < num_of_cells * MAX_CELL_VOLTAGE) break; } printf(" cells found:%d\n", num_of_cells); diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index ea732eccd1..827b0bb009 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -77,7 +77,7 @@ #include #include #include -#include +#include #include #include #include @@ -180,7 +180,7 @@ private: /* subscribed topics */ int _t_actuators; ///< actuator controls topic int _t_actuator_armed; ///< system armed control topic - int _t_vstatus; ///< system / vehicle status + int _t_vehicle_control_mode; ///< vehicle control mode topic int _t_param; ///< parameter update topic /* advertised topics */ @@ -362,7 +362,7 @@ PX4IO::PX4IO() : _alarms(0), _t_actuators(-1), _t_actuator_armed(-1), - _t_vstatus(-1), + _t_vehicle_control_mode(-1), _t_param(-1), _to_input_rc(0), _to_actuators_effective(0), @@ -647,8 +647,8 @@ PX4IO::task_main() _t_actuator_armed = orb_subscribe(ORB_ID(actuator_armed)); orb_set_interval(_t_actuator_armed, 200); /* 5Hz update rate */ - _t_vstatus = orb_subscribe(ORB_ID(vehicle_status)); - orb_set_interval(_t_vstatus, 200); /* 5Hz update rate max. */ + _t_vehicle_control_mode = orb_subscribe(ORB_ID(vehicle_control_mode)); + orb_set_interval(_t_vehicle_control_mode, 200); /* 5Hz update rate max. */ _t_param = orb_subscribe(ORB_ID(parameter_update)); orb_set_interval(_t_param, 500); /* 2Hz update rate max. */ @@ -659,7 +659,7 @@ PX4IO::task_main() fds[0].events = POLLIN; fds[1].fd = _t_actuator_armed; fds[1].events = POLLIN; - fds[2].fd = _t_vstatus; + fds[2].fd = _t_vehicle_control_mode; fds[2].events = POLLIN; fds[3].fd = _t_param; fds[3].events = POLLIN; @@ -834,10 +834,10 @@ int PX4IO::io_set_arming_state() { actuator_armed_s armed; ///< system armed state - vehicle_status_s vstatus; ///< overall system state + vehicle_control_mode_s control_mode; ///< vehicle_control_mode orb_copy(ORB_ID(actuator_armed), _t_actuator_armed, &armed); - orb_copy(ORB_ID(vehicle_status), _t_vstatus, &vstatus); + orb_copy(ORB_ID(vehicle_control_mode), _t_vehicle_control_mode, &control_mode); uint16_t set = 0; uint16_t clear = 0; @@ -853,7 +853,7 @@ PX4IO::io_set_arming_state() clear |= PX4IO_P_SETUP_ARMING_IO_ARM_OK; } - if (vstatus.flag_external_manual_override_ok) { + if (control_mode.flag_external_manual_override_ok) { set |= PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK; } else { clear |= PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK; diff --git a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp index d8b40ac3b5..d1b941ffe2 100755 --- a/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp +++ b/src/modules/attitude_estimator_ekf/attitude_estimator_ekf_main.cpp @@ -57,7 +57,7 @@ #include #include #include -#include +#include #include #include @@ -214,8 +214,8 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds memset(&raw, 0, sizeof(raw)); struct vehicle_attitude_s att; memset(&att, 0, sizeof(att)); - struct vehicle_status_s state; - memset(&state, 0, sizeof(state)); + struct vehicle_control_mode_s control_mode; + memset(&control_mode, 0, sizeof(control_mode)); uint64_t last_data = 0; uint64_t last_measurement = 0; @@ -228,8 +228,8 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds /* subscribe to param changes */ int sub_params = orb_subscribe(ORB_ID(parameter_update)); - /* subscribe to system state*/ - int sub_state = orb_subscribe(ORB_ID(vehicle_status)); + /* subscribe to control mode*/ + int sub_control_mode = orb_subscribe(ORB_ID(vehicle_control_mode)); /* advertise attitude */ orb_advert_t pub_att = orb_advertise(ORB_ID(vehicle_attitude), &att); @@ -281,9 +281,9 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds /* XXX this is seriously bad - should be an emergency */ } else if (ret == 0) { /* check if we're in HIL - not getting sensor data is fine then */ - orb_copy(ORB_ID(vehicle_status), sub_state, &state); + orb_copy(ORB_ID(vehicle_control_mode), sub_control_mode, &control_mode); - if (!state.flag_hil_enabled) { + if (!control_mode.flag_system_hil_enabled) { fprintf(stderr, "[att ekf] WARNING: Not getting sensors - sensor app running?\n"); } diff --git a/src/modules/attitude_estimator_so3_comp/attitude_estimator_so3_comp_main.cpp b/src/modules/attitude_estimator_so3_comp/attitude_estimator_so3_comp_main.cpp index 3ca50fb39c..2a06f1628b 100755 --- a/src/modules/attitude_estimator_so3_comp/attitude_estimator_so3_comp_main.cpp +++ b/src/modules/attitude_estimator_so3_comp/attitude_estimator_so3_comp_main.cpp @@ -35,7 +35,7 @@ #include #include #include -#include +#include #include #include @@ -545,8 +545,8 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds struct vehicle_attitude_s att; memset(&att, 0, sizeof(att)); - struct vehicle_status_s state; - memset(&state, 0, sizeof(state)); + struct vehicle_control_mode_s control_mode; + memset(&control_mode, 0, sizeof(control_mode)); uint64_t last_data = 0; uint64_t last_measurement = 0; @@ -559,8 +559,8 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds /* subscribe to param changes */ int sub_params = orb_subscribe(ORB_ID(parameter_update)); - /* subscribe to system state*/ - int sub_state = orb_subscribe(ORB_ID(vehicle_status)); + /* subscribe to control mode */ + int sub_control_mode = orb_subscribe(ORB_ID(vehicle_control_mode)); /* advertise attitude */ orb_advert_t pub_att = orb_advertise(ORB_ID(vehicle_attitude), &att); @@ -610,9 +610,9 @@ const unsigned int loop_interval_alarm = 6500; // loop interval in microseconds /* XXX this is seriously bad - should be an emergency */ } else if (ret == 0) { /* check if we're in HIL - not getting sensor data is fine then */ - orb_copy(ORB_ID(vehicle_status), sub_state, &state); + orb_copy(ORB_ID(vehicle_control_mode), sub_control_mode, &control_mode); - if (!state.flag_hil_enabled) { + if (!control_mode.flag_system_hil_enabled) { fprintf(stderr, "[att so3_comp] WARNING: Not getting sensors - sensor app running?\n"); } diff --git a/src/modules/commander/commander.c b/src/modules/commander/commander.c index 144fda79ac..c4dc495f6a 100644 --- a/src/modules/commander/commander.c +++ b/src/modules/commander/commander.c @@ -171,7 +171,7 @@ void usage(const char *reason); /** * React to commands that are sent e.g. from the mavlink module. */ -void handle_command(int status_pub, struct vehicle_status_s *current_status, struct vehicle_command_s *cmd, int armed_pub, struct actuator_armed_s *armed); +void handle_command(int status_pub, struct vehicle_status_s *current_status, int control_mode_pub, struct vehicle_control_mode_s *current_control_mode, struct vehicle_command_s *cmd, int armed_pub, struct actuator_armed_s *armed); /** * Mainloop of commander. @@ -236,7 +236,7 @@ void usage(const char *reason) exit(1); } -void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_status, struct vehicle_command_s *cmd, int armed_pub, struct actuator_armed_s *armed) +void handle_command(int status_pub, struct vehicle_status_s *current_status, int control_mode_pub, struct vehicle_control_mode_s *current_control_mode, struct vehicle_command_s *cmd, int armed_pub, struct actuator_armed_s *armed) { /* result of the command */ uint8_t result = VEHICLE_CMD_RESULT_UNSUPPORTED; @@ -248,7 +248,7 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta /* request to activate HIL */ if ((int)cmd->param1 & VEHICLE_MODE_FLAG_HIL_ENABLED) { - if (OK == hil_state_transition(status_pub, current_vehicle_status, mavlink_fd, HIL_STATE_ON)) { + if (OK == hil_state_transition(HIL_STATE_ON, status_pub, current_status, control_mode_pub, current_control_mode, mavlink_fd)) { result = VEHICLE_CMD_RESULT_ACCEPTED; } else { result = VEHICLE_CMD_RESULT_DENIED; @@ -256,13 +256,13 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta } if ((int)cmd->param1 & VEHICLE_MODE_FLAG_SAFETY_ARMED) { - if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_ARMED, armed_pub, armed, mavlink_fd)) { + if (OK == arming_state_transition(status_pub, current_status, ARMING_STATE_ARMED, armed_pub, armed, mavlink_fd)) { result = VEHICLE_CMD_RESULT_ACCEPTED; } else { result = VEHICLE_CMD_RESULT_DENIED; } } else { - if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_STANDBY, armed_pub, armed, mavlink_fd)) { + if (OK == arming_state_transition(status_pub, current_status, ARMING_STATE_STANDBY, armed_pub, armed, mavlink_fd)) { result = VEHICLE_CMD_RESULT_ACCEPTED; } else { result = VEHICLE_CMD_RESULT_DENIED; @@ -275,14 +275,14 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta /* request to arm */ if ((int)cmd->param1 == 1) { - if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_ARMED, armed_pub, armed, mavlink_fd)) { + if (OK == arming_state_transition(status_pub, current_status, ARMING_STATE_ARMED, armed_pub, armed, mavlink_fd)) { result = VEHICLE_CMD_RESULT_ACCEPTED; } else { result = VEHICLE_CMD_RESULT_DENIED; } /* request to disarm */ } else if ((int)cmd->param1 == 0) { - if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_STANDBY, armed_pub, armed, mavlink_fd)) { + if (OK == arming_state_transition(status_pub, current_status, ARMING_STATE_STANDBY, armed_pub, armed, mavlink_fd)) { result = VEHICLE_CMD_RESULT_ACCEPTED; } else { result = VEHICLE_CMD_RESULT_DENIED; @@ -295,7 +295,7 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta /* request for an autopilot reboot */ if ((int)cmd->param1 == 1) { - if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_REBOOT, armed_pub, armed, mavlink_fd)) { + if (OK == arming_state_transition(status_pub, current_status, ARMING_STATE_REBOOT, armed_pub, armed, mavlink_fd)) { /* reboot is executed later, after positive tune is sent */ result = VEHICLE_CMD_RESULT_ACCEPTED; tune_positive(); @@ -346,7 +346,7 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta if(low_prio_task == LOW_PRIO_TASK_NONE) { /* try to go to INIT/PREFLIGHT arming state */ - if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_INIT, armed_pub, armed, mavlink_fd)) { + if (OK == arming_state_transition(status_pub, current_status, ARMING_STATE_INIT, armed_pub, armed, mavlink_fd)) { result = VEHICLE_CMD_RESULT_ACCEPTED; /* now set the task for the low prio thread */ low_prio_task = LOW_PRIO_TASK_GYRO_CALIBRATION; @@ -365,7 +365,7 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta if(low_prio_task == LOW_PRIO_TASK_NONE) { /* try to go to INIT/PREFLIGHT arming state */ - if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_INIT, armed_pub, armed, mavlink_fd)) { + if (OK == arming_state_transition(status_pub, current_status, ARMING_STATE_INIT, armed_pub, armed, mavlink_fd)) { result = VEHICLE_CMD_RESULT_ACCEPTED; /* now set the task for the low prio thread */ low_prio_task = LOW_PRIO_TASK_MAG_CALIBRATION; @@ -426,7 +426,7 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta if(low_prio_task == LOW_PRIO_TASK_NONE) { /* try to go to INIT/PREFLIGHT arming state */ - if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_INIT, armed_pub, armed, mavlink_fd)) { + if (OK == arming_state_transition(status_pub, current_status, ARMING_STATE_INIT, armed_pub, armed, mavlink_fd)) { result = VEHICLE_CMD_RESULT_ACCEPTED; /* now set the task for the low prio thread */ low_prio_task = LOW_PRIO_TASK_ACCEL_CALIBRATION; @@ -445,7 +445,7 @@ void handle_command(int status_pub, struct vehicle_status_s *current_vehicle_sta if(low_prio_task == LOW_PRIO_TASK_NONE) { /* try to go to INIT/PREFLIGHT arming state */ - if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_INIT, armed_pub, armed, mavlink_fd)) { + if (OK == arming_state_transition(status_pub, current_status, ARMING_STATE_INIT, armed_pub, armed, mavlink_fd)) { result = VEHICLE_CMD_RESULT_ACCEPTED; /* now set the task for the low prio thread */ low_prio_task = LOW_PRIO_TASK_AIRSPEED_CALIBRATION; @@ -580,7 +580,7 @@ int commander_thread_main(int argc, char *argv[]) current_status.offboard_control_signal_lost = true; /* allow manual override initially */ - current_status.flag_external_manual_override_ok = true; + control_mode.flag_external_manual_override_ok = true; /* flag position info as bad, do not allow auto mode */ // current_status.flag_vector_flight_mode_ok = false; @@ -637,32 +637,24 @@ int commander_thread_main(int argc, char *argv[]) pthread_create(&commander_low_prio_thread, &commander_low_prio_attr, commander_low_prio_loop, NULL); /* Start monitoring loop */ - uint16_t counter = 0; - - /* Initialize to 0.0V */ - float battery_voltage = 0.0f; - bool battery_voltage_valid = false; - bool low_battery_voltage_actions_done = false; - bool critical_battery_voltage_actions_done = false; - uint8_t low_voltage_counter = 0; - uint16_t critical_voltage_counter = 0; - float bat_remain = 1.0f; - - uint16_t stick_off_counter = 0; - uint16_t stick_on_counter = 0; + unsigned counter = 0; + unsigned low_voltage_counter = 0; + unsigned critical_voltage_counter = 0; + unsigned stick_off_counter = 0; + unsigned stick_on_counter = 0; /* To remember when last notification was sent */ uint64_t last_print_time = 0; float voltage_previous = 0.0f; + bool low_battery_voltage_actions_done; + bool critical_battery_voltage_actions_done; + uint64_t last_idle_time = 0; uint64_t start_time = 0; - /* XXX use this! */ - //uint64_t failsave_ll_start_time = 0; - bool state_changed = true; bool param_init_forced = true; @@ -764,10 +756,10 @@ int commander_thread_main(int argc, char *argv[]) if (current_status.system_type == VEHICLE_TYPE_QUADROTOR || current_status.system_type == VEHICLE_TYPE_HEXAROTOR || current_status.system_type == VEHICLE_TYPE_OCTOROTOR) { - current_status.flag_external_manual_override_ok = false; + control_mode.flag_external_manual_override_ok = false; } else { - current_status.flag_external_manual_override_ok = true; + control_mode.flag_external_manual_override_ok = true; } /* check and update system / component ID */ @@ -809,7 +801,7 @@ int commander_thread_main(int argc, char *argv[]) orb_copy(ORB_ID(vehicle_command), cmd_sub, &cmd); /* handle it */ - handle_command(status_pub, ¤t_status, &cmd, armed_pub, &armed); + handle_command(status_pub, ¤t_status, control_mode_pub, &control_mode, &cmd, armed_pub, &armed); } /* update safety topic */ @@ -848,16 +840,20 @@ int commander_thread_main(int argc, char *argv[]) orb_check(battery_sub, &new_data); if (new_data) { orb_copy(ORB_ID(battery_status), battery_sub, &battery); - battery_voltage = battery.voltage_v; - battery_voltage_valid = true; + current_status.battery_voltage = battery.voltage_v; + current_status.condition_battery_voltage_valid = true; /* * Only update battery voltage estimate if system has * been running for two and a half seconds. */ - if (hrt_absolute_time() - start_time > 2500000) { - bat_remain = battery_remaining_estimate_voltage(battery_voltage); - } + + } + + if (hrt_absolute_time() - start_time > 2500000 && current_status.condition_battery_voltage_valid) { + current_status.battery_remaining = battery_remaining_estimate_voltage(current_status.battery_voltage); + } else { + current_status.battery_voltage = 0.0f; } /* update subsystem */ @@ -897,14 +893,15 @@ int commander_thread_main(int argc, char *argv[]) if (counter % ((1000000 / COMMANDER_MONITORING_INTERVAL) / 8) == 0) { /* XXX if armed */ - if ((false /*current_status.state_machine == SYSTEM_STATE_GROUND_READY || - current_status.state_machine == SYSTEM_STATE_AUTO || - current_status.state_machine == SYSTEM_STATE_MANUAL*/)) { + if (armed.armed) { /* armed, solid */ led_on(LED_AMBER); - } else if (counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0) { - /* not armed */ + } else if (armed.ready_to_arm && (counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0)) { + /* ready to arm */ + led_toggle(LED_AMBER); + } else if (counter % (100000 / COMMANDER_MONITORING_INTERVAL) == 0) { + /* not ready to arm, something is wrong */ led_toggle(LED_AMBER); } @@ -926,15 +923,16 @@ int commander_thread_main(int argc, char *argv[]) led_off(LED_BLUE); } - /* toggle GPS led at 5 Hz in HIL mode */ - if (current_status.flag_hil_enabled) { - /* hil enabled */ - led_toggle(LED_BLUE); - } else if (bat_remain < 0.3f && (low_voltage_counter > LOW_VOLTAGE_BATTERY_COUNTER_LIMIT)) { - /* toggle arming (red) at 5 Hz on low battery or error */ - led_toggle(LED_AMBER); - } + // /* toggle GPS led at 5 Hz in HIL mode */ + // if (current_status.flag_hil_enabled) { + // /* hil enabled */ + // led_toggle(LED_BLUE); + + // } else if (bat_remain < 0.3f && (low_voltage_counter > LOW_VOLTAGE_BATTERY_COUNTER_LIMIT)) { + // /* toggle arming (red) at 5 Hz on low battery or error */ + // led_toggle(LED_AMBER); + // } } @@ -948,35 +946,29 @@ int commander_thread_main(int argc, char *argv[]) last_idle_time = system_load.tasks[0].total_runtime; } - /* Check battery voltage */ - /* write to sys_status */ - if (battery_voltage_valid) { - current_status.voltage_battery = battery_voltage; - - } else { - current_status.voltage_battery = 0.0f; - } + /* if battery voltage is getting lower, warn using buzzer, etc. */ - if (battery_voltage_valid && (bat_remain < 0.15f /* XXX MAGIC NUMBER */) && (false == low_battery_voltage_actions_done)) { //TODO: add filter, or call emergency after n measurements < VOLTAGE_BATTERY_MINIMAL_MILLIVOLTS + if (current_status.condition_battery_voltage_valid && (current_status.battery_remaining < 0.15f /* XXX MAGIC NUMBER */) && (false == low_battery_voltage_actions_done)) { //TODO: add filter, or call emergency after n measurements < VOLTAGE_BATTERY_MINIMAL_MILLIVOLTS if (low_voltage_counter > LOW_VOLTAGE_BATTERY_COUNTER_LIMIT) { low_battery_voltage_actions_done = true; mavlink_log_critical(mavlink_fd, "[cmd] WARNING! LOW BATTERY!"); current_status.battery_warning = VEHICLE_BATTERY_WARNING_WARNING; + tune_low_bat(); } low_voltage_counter++; } /* Critical, this is rather an emergency, change state machine */ - else if (battery_voltage_valid && (bat_remain < 0.1f /* XXX MAGIC NUMBER */) && (false == critical_battery_voltage_actions_done && true == low_battery_voltage_actions_done)) { + else if (current_status.condition_battery_voltage_valid && (current_status.battery_remaining < 0.1f /* XXX MAGIC NUMBER */) && (false == critical_battery_voltage_actions_done && true == low_battery_voltage_actions_done)) { if (critical_voltage_counter > CRITICAL_VOLTAGE_BATTERY_COUNTER_LIMIT) { critical_battery_voltage_actions_done = true; mavlink_log_critical(mavlink_fd, "[cmd] EMERGENCY! CRITICAL BATTERY!"); current_status.battery_warning = VEHICLE_BATTERY_WARNING_ALERT; - // XXX implement this - // state_machine_emergency(status_pub, ¤t_status, mavlink_fd); + tune_critical_bat(); + // XXX implement state change here } critical_voltage_counter++; @@ -1585,7 +1577,7 @@ int commander_thread_main(int argc, char *argv[]) /* Store old modes to detect and act on state transitions */ - voltage_previous = current_status.voltage_battery; + voltage_previous = current_status.battery_voltage; /* play tone according to evaluation result */ @@ -1595,10 +1587,10 @@ int commander_thread_main(int argc, char *argv[]) arm_tune_played = true; /* Trigger audio event for low battery */ - } else if (bat_remain < 0.1f && battery_voltage_valid) { + } else if (current_status.battery_remaining < 0.1f && current_status.condition_battery_voltage_valid) { if (tune_critical_bat() == OK) battery_tune_played = true; - } else if (bat_remain < 0.2f && battery_voltage_valid) { + } else if (current_status.battery_remaining < 0.2f && current_status.condition_battery_voltage_valid) { if (tune_low_bat() == OK) battery_tune_played = true; } else if(battery_tune_played) { diff --git a/src/modules/commander/state_machine_helper.c b/src/modules/commander/state_machine_helper.c index 0b241d108d..792ead8f3c 100644 --- a/src/modules/commander/state_machine_helper.c +++ b/src/modules/commander/state_machine_helper.c @@ -510,7 +510,7 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current /** * Transition from one hil state to another */ -int hil_state_transition(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, hil_state_t new_state) +int hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_status_s *current_status, int control_mode_pub, struct vehicle_control_mode_s *current_control_mode, const int mavlink_fd) { bool valid_transition = false; int ret = ERROR; @@ -530,7 +530,7 @@ int hil_state_transition(int status_pub, struct vehicle_status_s *current_status if (current_status->arming_state == ARMING_STATE_INIT || current_status->arming_state == ARMING_STATE_STANDBY) { - current_status->flag_hil_enabled = false; + current_control_mode->flag_system_hil_enabled = false; mavlink_log_critical(mavlink_fd, "Switched to OFF hil state"); valid_transition = true; } @@ -541,7 +541,7 @@ int hil_state_transition(int status_pub, struct vehicle_status_s *current_status if (current_status->arming_state == ARMING_STATE_INIT || current_status->arming_state == ARMING_STATE_STANDBY) { - current_status->flag_hil_enabled = true; + current_control_mode->flag_system_hil_enabled = true; mavlink_log_critical(mavlink_fd, "Switched to ON hil state"); valid_transition = true; } @@ -558,8 +558,11 @@ int hil_state_transition(int status_pub, struct vehicle_status_s *current_status current_status->counter++; current_status->timestamp = hrt_absolute_time(); - orb_publish(ORB_ID(vehicle_status), status_pub, current_status); + + current_control_mode->timestamp = hrt_absolute_time(); + orb_publish(ORB_ID(vehicle_control_mode), control_mode_pub, current_control_mode); + ret = OK; } else { mavlink_log_critical(mavlink_fd, "REJECTING invalid hil state transition"); diff --git a/src/modules/commander/state_machine_helper.h b/src/modules/commander/state_machine_helper.h index e591d2fef9..75fdd224c4 100644 --- a/src/modules/commander/state_machine_helper.h +++ b/src/modules/commander/state_machine_helper.h @@ -54,6 +54,6 @@ int arming_state_transition(int status_pub, struct vehicle_status_s *current_sta int navigation_state_transition(int status_pub, struct vehicle_status_s *current_state, navigation_state_t new_navigation_state, int control_mode_pub, struct vehicle_control_mode_s *control_mode, const int mavlink_fd); -int hil_state_transition(int status_pub, struct vehicle_status_s *current_status, const int mavlink_fd, hil_state_t new_state); +int hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_status_s *current_state, int control_mode_pub, struct vehicle_control_mode_s *current_control_mode, const int mavlink_fd); #endif /* STATE_MACHINE_HELPER_H_ */ diff --git a/src/modules/controllib/fixedwing.cpp b/src/modules/controllib/fixedwing.cpp index 9435959e9b..0cfcfd51d1 100644 --- a/src/modules/controllib/fixedwing.cpp +++ b/src/modules/controllib/fixedwing.cpp @@ -39,6 +39,7 @@ #include "fixedwing.hpp" +#if 0 namespace control { @@ -277,11 +278,12 @@ void BlockMultiModeBacksideAutopilot::update() // This is not a hack, but a design choice. /* do not limit in HIL */ - if (!_status.flag_hil_enabled) { - /* limit to value of manual throttle */ - _actuators.control[CH_THR] = (_actuators.control[CH_THR] < _manual.throttle) ? - _actuators.control[CH_THR] : _manual.throttle; - } +#warning fix this + // if (!_status.flag_hil_enabled) { + // /* limit to value of manual throttle */ + // _actuators.control[CH_THR] = (_actuators.control[CH_THR] < _manual.throttle) ? + // _actuators.control[CH_THR] : _manual.throttle; + // } } else if (_status.navigation_state == NAVIGATION_STATE_MANUAL) { @@ -343,11 +345,12 @@ void BlockMultiModeBacksideAutopilot::update() // This is not a hack, but a design choice. /* do not limit in HIL */ - if (!_status.flag_hil_enabled) { - /* limit to value of manual throttle */ - _actuators.control[CH_THR] = (_actuators.control[CH_THR] < _manual.throttle) ? - _actuators.control[CH_THR] : _manual.throttle; - } +#warning fix this + // if (!_status.flag_hil_enabled) { + // /* limit to value of manual throttle */ + // _actuators.control[CH_THR] = (_actuators.control[CH_THR] < _manual.throttle) ? + // _actuators.control[CH_THR] : _manual.throttle; + // } #warning fix this // } @@ -382,3 +385,4 @@ BlockMultiModeBacksideAutopilot::~BlockMultiModeBacksideAutopilot() } // namespace control +#endif \ No newline at end of file diff --git a/src/modules/fixedwing_backside/fixedwing_backside_main.cpp b/src/modules/fixedwing_backside/fixedwing_backside_main.cpp index 4803a526eb..677a867712 100644 --- a/src/modules/fixedwing_backside/fixedwing_backside_main.cpp +++ b/src/modules/fixedwing_backside/fixedwing_backside_main.cpp @@ -150,12 +150,14 @@ int control_demo_thread_main(int argc, char *argv[]) using namespace control; - fixedwing::BlockMultiModeBacksideAutopilot autopilot(NULL, "FWB"); +#warning fix this +// fixedwing::BlockMultiModeBacksideAutopilot autopilot(NULL, "FWB"); thread_running = true; while (!thread_should_exit) { - autopilot.update(); +#warning fix this +// autopilot.update(); } warnx("exiting."); diff --git a/src/modules/mavlink/mavlink.c b/src/modules/mavlink/mavlink.c index a2758a45c7..5f683c4437 100644 --- a/src/modules/mavlink/mavlink.c +++ b/src/modules/mavlink/mavlink.c @@ -191,7 +191,7 @@ get_mavlink_mode_and_state(uint8_t *mavlink_state, uint8_t *mavlink_mode) **/ /* HIL */ - if (v_status.flag_hil_enabled) { + if (v_status.hil_state == HIL_STATE_ON) { *mavlink_mode |= MAV_MODE_FLAG_HIL_ENABLED; } @@ -234,11 +234,11 @@ get_mavlink_mode_and_state(uint8_t *mavlink_state, uint8_t *mavlink_mode) **/ /* set calibration state */ - if (v_status.flag_preflight_calibration) { + if (v_status.preflight_calibration) { *mavlink_state = MAV_STATE_CALIBRATING; - } else if (v_status.flag_system_emergency) { + } else if (v_status.system_emergency) { *mavlink_state = MAV_STATE_EMERGENCY; @@ -677,7 +677,10 @@ int mavlink_thread_main(int argc, char *argv[]) mavlink_msg_heartbeat_send(chan, mavlink_system.type, MAV_AUTOPILOT_PX4, mavlink_mode, v_status.navigation_state, mavlink_state); /* switch HIL mode if required */ - set_hil_on_off(v_status.flag_hil_enabled); + if (v_status.hil_state == HIL_STATE_ON) + set_hil_on_off(true); + else if (v_status.hil_state == HIL_STATE_OFF) + set_hil_on_off(false); /* send status (values already copied in the section above) */ mavlink_msg_sys_status_send(chan, @@ -685,8 +688,8 @@ int mavlink_thread_main(int argc, char *argv[]) v_status.onboard_control_sensors_enabled, v_status.onboard_control_sensors_health, v_status.load, - v_status.voltage_battery * 1000.0f, - v_status.current_battery * 1000.0f, + v_status.battery_voltage * 1000.0f, + v_status.battery_current * 1000.0f, v_status.battery_remaining, v_status.drop_rate_comm, v_status.errors_comm, diff --git a/src/modules/mavlink/orb_listener.c b/src/modules/mavlink/orb_listener.c index 57a5d5dd50..d088d421ed 100644 --- a/src/modules/mavlink/orb_listener.c +++ b/src/modules/mavlink/orb_listener.c @@ -272,7 +272,10 @@ l_vehicle_status(const struct listener *l) orb_copy(ORB_ID(actuator_armed), mavlink_subs.armed_sub, &armed); /* enable or disable HIL */ - set_hil_on_off(v_status.flag_hil_enabled); + if (v_status.hil_state == HIL_STATE_ON) + set_hil_on_off(true); + else if (v_status.hil_state == HIL_STATE_OFF) + set_hil_on_off(false); /* translate the current syste state to mavlink state and mode */ uint8_t mavlink_state = 0; diff --git a/src/modules/mavlink_onboard/mavlink.c b/src/modules/mavlink_onboard/mavlink.c index 9ed2c6c12e..c5dbd00dd9 100644 --- a/src/modules/mavlink_onboard/mavlink.c +++ b/src/modules/mavlink_onboard/mavlink.c @@ -449,8 +449,8 @@ int mavlink_thread_main(int argc, char *argv[]) v_status.onboard_control_sensors_enabled, v_status.onboard_control_sensors_health, v_status.load, - v_status.voltage_battery * 1000.0f, - v_status.current_battery * 1000.0f, + v_status.battery_voltage * 1000.0f, + v_status.battery_current * 1000.0f, v_status.battery_remaining, v_status.drop_rate_comm, v_status.errors_comm, diff --git a/src/modules/sdlog2/sdlog2.c b/src/modules/sdlog2/sdlog2.c index b20d38b5ee..ab39830192 100644 --- a/src/modules/sdlog2/sdlog2.c +++ b/src/modules/sdlog2/sdlog2.c @@ -945,8 +945,8 @@ int sdlog2_thread_main(int argc, char *argv[]) log_msg.body.log_STAT.manual_control_mode = 0; log_msg.body.log_STAT.manual_sas_mode = 0; log_msg.body.log_STAT.armed = (unsigned char) buf_armed.armed; /* XXX fmu armed correct? */ - log_msg.body.log_STAT.battery_voltage = buf_status.voltage_battery; - log_msg.body.log_STAT.battery_current = buf_status.current_battery; + log_msg.body.log_STAT.battery_voltage = buf_status.battery_voltage; + log_msg.body.log_STAT.battery_current = buf_status.battery_current; log_msg.body.log_STAT.battery_remaining = buf_status.battery_remaining; log_msg.body.log_STAT.battery_warning = (unsigned char) buf_status.battery_warning; LOGBUFFER_WRITE_AND_COUNT(STAT); diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index acc0c2717b..5e334638f4 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -73,7 +73,7 @@ #include #include #include -#include +#include #include #include #include @@ -165,7 +165,7 @@ private: int _baro_sub; /**< raw baro data subscription */ int _airspeed_sub; /**< airspeed subscription */ int _diff_pres_sub; /**< raw differential pressure subscription */ - int _vstatus_sub; /**< vehicle status subscription */ + int _vcontrol_mode_sub; /**< vehicle control mode subscription */ int _params_sub; /**< notification of parameter updates */ int _manual_control_sub; /**< notification of manual control updates */ @@ -352,9 +352,9 @@ private: void diff_pres_poll(struct sensor_combined_s &raw); /** - * Check for changes in vehicle status. + * Check for changes in vehicle control mode. */ - void vehicle_status_poll(); + void vehicle_control_mode_poll(); /** * Check for changes in parameters. @@ -411,7 +411,7 @@ Sensors::Sensors() : _mag_sub(-1), _rc_sub(-1), _baro_sub(-1), - _vstatus_sub(-1), + _vcontrol_mode_sub(-1), _params_sub(-1), _manual_control_sub(-1), @@ -941,21 +941,21 @@ Sensors::diff_pres_poll(struct sensor_combined_s &raw) } void -Sensors::vehicle_status_poll() +Sensors::vehicle_control_mode_poll() { - struct vehicle_status_s vstatus; - bool vstatus_updated; + struct vehicle_control_mode_s vcontrol_mode; + bool vcontrol_mode_updated; - /* Check HIL state if vehicle status has changed */ - orb_check(_vstatus_sub, &vstatus_updated); + /* Check HIL state if vehicle control mode has changed */ + orb_check(_vcontrol_mode_sub, &vcontrol_mode_updated); - if (vstatus_updated) { + if (vcontrol_mode_updated) { - orb_copy(ORB_ID(vehicle_status), _vstatus_sub, &vstatus); + orb_copy(ORB_ID(vehicle_control_mode), _vcontrol_mode_sub, &vcontrol_mode); /* switching from non-HIL to HIL mode */ //printf("[sensors] Vehicle mode: %i \t AND: %i, HIL: %i\n", vstatus.mode, vstatus.mode & VEHICLE_MODE_FLAG_HIL_ENABLED, hil_enabled); - if (vstatus.flag_hil_enabled && !_hil_enabled) { + if (vcontrol_mode.flag_system_hil_enabled && !_hil_enabled) { _hil_enabled = true; _publishing = false; @@ -1348,12 +1348,12 @@ Sensors::task_main() _rc_sub = orb_subscribe(ORB_ID(input_rc)); _baro_sub = orb_subscribe(ORB_ID(sensor_baro)); _diff_pres_sub = orb_subscribe(ORB_ID(differential_pressure)); - _vstatus_sub = orb_subscribe(ORB_ID(vehicle_status)); + _vcontrol_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode)); _params_sub = orb_subscribe(ORB_ID(parameter_update)); _manual_control_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); /* rate limit vehicle status updates to 5Hz */ - orb_set_interval(_vstatus_sub, 200); + orb_set_interval(_vcontrol_mode_sub, 200); /* * do advertisements @@ -1406,7 +1406,7 @@ Sensors::task_main() perf_begin(_loop_perf); /* check vehicle status for changes to publication state */ - vehicle_status_poll(); + vehicle_control_mode_poll(); /* check parameters for updates */ parameter_update_poll(); diff --git a/src/modules/uORB/topics/vehicle_control_mode.h b/src/modules/uORB/topics/vehicle_control_mode.h index 02bf5568ab..8481a624fd 100644 --- a/src/modules/uORB/topics/vehicle_control_mode.h +++ b/src/modules/uORB/topics/vehicle_control_mode.h @@ -66,9 +66,6 @@ struct vehicle_control_mode_s bool flag_external_manual_override_ok; /**< external override non-fatal for system. Only true for fixed wing */ - bool flag_system_emergency; - bool flag_preflight_calibration; - // XXX needs yet to be set by state machine helper bool flag_system_hil_enabled; diff --git a/src/modules/uORB/topics/vehicle_status.h b/src/modules/uORB/topics/vehicle_status.h index 2bcd57f4bb..ec08a6c13a 100644 --- a/src/modules/uORB/topics/vehicle_status.h +++ b/src/modules/uORB/topics/vehicle_status.h @@ -177,14 +177,11 @@ struct vehicle_status_s int32_t system_id; /**< system id, inspired by MAVLink's system ID field */ int32_t component_id; /**< subsystem / component id, inspired by MAVLink's component ID field */ - /* system flags - these represent the state predicates */ - mode_switch_pos_t mode_switch; return_switch_pos_t return_switch; mission_switch_pos_t mission_switch; - - bool condition_system_emergency; + bool condition_battery_voltage_valid; bool condition_system_in_air_restore; /**< true if we can restore in mid air */ bool condition_system_sensors_initialized; bool condition_system_returned_to_home; @@ -195,28 +192,6 @@ struct vehicle_status_s bool condition_local_position_valid; bool condition_airspeed_valid; /**< set to true by the commander app if there is a valid airspeed measurement available */ - // bool condition_auto_flight_mode_ok; /**< conditions of auto flight mode apply plus a valid takeoff position lock has been aquired */ - bool flag_external_manual_override_ok; /**< external override non-fatal for system. Only true for fixed wing */ - - bool flag_hil_enabled; /**< true if hardware in the loop simulation is enabled */ - //bool flag_armed; /**< true is motors / actuators are armed */ - //bool flag_safety_off; /**< true if safety is off */ - bool flag_system_emergency; - bool flag_preflight_calibration; - - // bool flag_control_manual_enabled; /**< true if manual input is mixed in */ - // bool flag_control_offboard_enabled; /**< true if offboard control input is on */ - // bool flag_auto_enabled; - // bool flag_control_rates_enabled; /**< true if rates are stabilized */ - // bool flag_control_attitude_enabled; /**< true if attitude stabilization is mixed in */ - // bool flag_control_velocity_enabled; /**< true if speed (implies direction) is controlled */ - // bool flag_control_position_enabled; /**< true if position is controlled */ - - // bool flag_preflight_gyro_calibration; /**< true if gyro calibration is requested */ - // bool flag_preflight_mag_calibration; /**< true if mag calibration is requested */ - // bool flag_preflight_accel_calibration; - // bool flag_preflight_airspeed_calibration; - bool rc_signal_found_once; bool rc_signal_lost; /**< true if RC reception is terminally lost */ bool rc_signal_cutting_off; /**< true if RC reception is weak / cutting off */ @@ -230,14 +205,20 @@ struct vehicle_status_s bool failsave_lowlevel; /**< Set to true if low-level failsafe mode is enabled */ bool failsave_highlevel; + bool preflight_calibration; + + bool system_emergency; + /* see SYS_STATUS mavlink message for the following */ uint32_t onboard_control_sensors_present; uint32_t onboard_control_sensors_enabled; uint32_t onboard_control_sensors_health; + float load; - float voltage_battery; - float current_battery; + float battery_voltage; + float battery_current; float battery_remaining; + enum VEHICLE_BATTERY_WARNING battery_warning; /**< current battery warning mode, as defined by VEHICLE_BATTERY_WARNING enum */ uint16_t drop_rate_comm; uint16_t errors_comm; From da1bf69ce249f22df16e7ccb21d17c08bcbbbedf Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 19 Jul 2013 13:07:51 +0200 Subject: [PATCH 087/109] Added gyro scale estimation (but param is NOT written) --- src/modules/commander/gyro_calibration.c | 80 ++++++++++++++++++++++++ 1 file changed, 80 insertions(+) diff --git a/src/modules/commander/gyro_calibration.c b/src/modules/commander/gyro_calibration.c index f452910c9a..865f74ab44 100644 --- a/src/modules/commander/gyro_calibration.c +++ b/src/modules/commander/gyro_calibration.c @@ -104,6 +104,86 @@ void do_gyro_calibration(int mavlink_fd) gyro_offset[1] = gyro_offset[1] / calibration_count; gyro_offset[2] = gyro_offset[2] / calibration_count; + + + + /*** --- SCALING --- ***/ + + mavlink_log_info(mavlink_fd, "offset calibration finished. Rotate for scale 130x"); + mavlink_log_info(mavlink_fd, "or do not rotate and wait for 5 seconds to skip."); + warnx("offset calibration finished. Rotate for scale 30x, or do not rotate and wait for 5 seconds to skip."); + + unsigned rotations_count = 30; + float gyro_integral = 0.0f; + float baseline_integral = 0.0f; + + // XXX change to mag topic + orb_copy(ORB_ID(sensor_combined), sub_sensor_combined, &raw); + + float mag_last = -atan2f(raw.magnetometer_ga[1],raw.magnetometer_ga[0]); + if (mag_last > M_PI_F) mag_last -= 2*M_PI_F; + if (mag_last < -M_PI_F) mag_last += 2*M_PI_F; + + + uint64_t last_time = hrt_absolute_time(); + uint64_t start_time = hrt_absolute_time(); + + while ((int)fabsf(baseline_integral / (2.0f * M_PI_F)) < rotations_count) { + + /* abort this loop if not rotated more than 180 degrees within 5 seconds */ + if ((fabsf(baseline_integral / (2.0f * M_PI_F)) < 0.6f) + && (hrt_absolute_time() - start_time > 5 * 1e6)) + break; + + /* wait blocking for new data */ + struct pollfd fds[1] = { { .fd = sub_sensor_combined, .events = POLLIN } }; + + int poll_ret = poll(fds, 1, 1000); + + if (poll_ret) { + + float dt_ms = (hrt_absolute_time() - last_time) / 1e3f; + last_time = hrt_absolute_time(); + + orb_copy(ORB_ID(sensor_combined), sub_sensor_combined, &raw); + + // XXX this is just a proof of concept and needs world / body + // transformation and more + + //math::Vector2f magNav(raw.magnetometer_ga); + + // calculate error between estimate and measurement + // apply declination correction for true heading as well. + //float mag = -atan2f(magNav(1),magNav(0)); + float mag = -atan2f(raw.magnetometer_ga[1],raw.magnetometer_ga[0]); + if (mag > M_PI_F) mag -= 2*M_PI_F; + if (mag < -M_PI_F) mag += 2*M_PI_F; + + float diff = mag - mag_last; + + if (diff > M_PI_F) diff -= 2*M_PI_F; + if (diff < -M_PI_F) diff += 2*M_PI_F; + + baseline_integral += diff; + mag_last = mag; + // Jump through some timing scale hoops to avoid + // operating near the 1e6/1e8 max sane resolution of float. + gyro_integral += (raw.gyro_rad_s[2] * dt_ms) / 1e3f; + + warnx("dbg: b: %6.4f, g: %6.4f", baseline_integral, gyro_integral); + + // } else if (poll_ret == 0) { + // /* any poll failure for 1s is a reason to abort */ + // mavlink_log_info(mavlink_fd, "gyro calibration aborted, retry"); + // return; + } + } + + float gyro_scale = baseline_integral / gyro_integral; + warnx("gyro scale: yaw (z): %6.4f", gyro_scale); + mavlink_log_info(mavlink_fd, "gyro scale: yaw (z): %6.4f", gyro_scale); + + if (isfinite(gyro_offset[0]) && isfinite(gyro_offset[1]) && isfinite(gyro_offset[2])) { if (param_set(param_find("SENS_GYRO_XOFF"), &(gyro_offset[0])) From 68ab69de010adbe37b37558824ca013ecd0d569a Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 20 Jul 2013 13:47:51 +0200 Subject: [PATCH 088/109] moved commander to C++, preparation for better gyro scale calibration respecting the current attitude for more accurate results --- ...ration.c => accelerometer_calibration.cpp} | 59 +++++++++------- ...calibration.c => airspeed_calibration.cpp} | 6 +- ...aro_calibration.c => baro_calibration.cpp} | 2 +- ...on_routines.c => calibration_routines.cpp} | 2 +- .../commander/{commander.c => commander.cpp} | 19 +++-- ...ommander_helper.c => commander_helper.cpp} | 13 ++-- .../{commander.h => commander_params.c} | 28 ++++---- ...yro_calibration.c => gyro_calibration.cpp} | 70 ++++++++++++++++--- ...{mag_calibration.c => mag_calibration.cpp} | 6 +- src/modules/commander/module.mk | 21 +++--- .../{rc_calibration.c => rc_calibration.cpp} | 2 +- ...hine_helper.c => state_machine_helper.cpp} | 7 +- src/modules/systemlib/systemlib.h | 7 +- 13 files changed, 156 insertions(+), 86 deletions(-) rename src/modules/commander/{accelerometer_calibration.c => accelerometer_calibration.cpp} (91%) rename src/modules/commander/{airspeed_calibration.c => airspeed_calibration.cpp} (97%) rename src/modules/commander/{baro_calibration.c => baro_calibration.cpp} (98%) rename src/modules/commander/{calibration_routines.c => calibration_routines.cpp} (99%) rename src/modules/commander/{commander.c => commander.cpp} (99%) rename src/modules/commander/{commander_helper.c => commander_helper.cpp} (97%) rename src/modules/commander/{commander.h => commander_params.c} (76%) rename src/modules/commander/{gyro_calibration.c => gyro_calibration.cpp} (83%) rename src/modules/commander/{mag_calibration.c => mag_calibration.cpp} (98%) rename src/modules/commander/{rc_calibration.c => rc_calibration.cpp} (99%) rename src/modules/commander/{state_machine_helper.c => state_machine_helper.cpp} (99%) diff --git a/src/modules/commander/accelerometer_calibration.c b/src/modules/commander/accelerometer_calibration.cpp similarity index 91% rename from src/modules/commander/accelerometer_calibration.c rename to src/modules/commander/accelerometer_calibration.cpp index bc9d24956f..df92d51d20 100644 --- a/src/modules/commander/accelerometer_calibration.c +++ b/src/modules/commander/accelerometer_calibration.cpp @@ -33,7 +33,7 @@ ****************************************************************************/ /** - * @file accelerometer_calibration.c + * @file accelerometer_calibration.cpp * * Implementation of accelerometer calibration. * @@ -113,8 +113,6 @@ #include #include #include - - #include #include #include @@ -123,6 +121,12 @@ #include #include +/* oddly, ERROR is not defined for c++ */ +#ifdef ERROR +# undef ERROR +#endif +static const int ERROR = -1; + int do_accel_calibration_measurements(int mavlink_fd, float accel_offs[3], float accel_scale[3]); int detect_orientation(int mavlink_fd, int sub_sensor_combined); int read_accelerometer_avg(int sensor_combined_sub, float accel_avg[3], int samples_num); @@ -229,7 +233,10 @@ int do_accel_calibration_measurements(int mavlink_fd, float accel_offs[3], float sprintf(str, "meas started: %s", orientation_strs[orient]); mavlink_log_info(mavlink_fd, str); read_accelerometer_avg(sensor_combined_sub, &(accel_ref[orient][0]), samples_num); - str_ptr = sprintf(str, "meas result for %s: [ %.2f %.2f %.2f ]", orientation_strs[orient], accel_ref[orient][0], accel_ref[orient][1], accel_ref[orient][2]); + str_ptr = sprintf(str, "meas result for %s: [ %.2f %.2f %.2f ]", orientation_strs[orient], + (double)accel_ref[orient][0], + (double)accel_ref[orient][1], + (double)accel_ref[orient][2]); mavlink_log_info(mavlink_fd, str); data_collected[orient] = true; tune_neutral(); @@ -274,7 +281,9 @@ int detect_orientation(int mavlink_fd, int sub_sensor_combined) { float accel_err_thr = 5.0f; /* still time required in us */ int64_t still_time = 2000000; - struct pollfd fds[1] = { { .fd = sub_sensor_combined, .events = POLLIN } }; + struct pollfd fds[1]; + fds[0].fd = sub_sensor_combined; + fds[0].events = POLLIN; hrt_abstime t_start = hrt_absolute_time(); /* set timeout to 30s */ @@ -342,29 +351,29 @@ int detect_orientation(int mavlink_fd, int sub_sensor_combined) { } } - if ( fabs(accel_ema[0] - CONSTANTS_ONE_G) < accel_err_thr && - fabs(accel_ema[1]) < accel_err_thr && - fabs(accel_ema[2]) < accel_err_thr ) + if ( fabsf(accel_ema[0] - CONSTANTS_ONE_G) < accel_err_thr && + fabsf(accel_ema[1]) < accel_err_thr && + fabsf(accel_ema[2]) < accel_err_thr ) return 0; // [ g, 0, 0 ] - if ( fabs(accel_ema[0] + CONSTANTS_ONE_G) < accel_err_thr && - fabs(accel_ema[1]) < accel_err_thr && - fabs(accel_ema[2]) < accel_err_thr ) + if ( fabsf(accel_ema[0] + CONSTANTS_ONE_G) < accel_err_thr && + fabsf(accel_ema[1]) < accel_err_thr && + fabsf(accel_ema[2]) < accel_err_thr ) return 1; // [ -g, 0, 0 ] - if ( fabs(accel_ema[0]) < accel_err_thr && - fabs(accel_ema[1] - CONSTANTS_ONE_G) < accel_err_thr && - fabs(accel_ema[2]) < accel_err_thr ) + if ( fabsf(accel_ema[0]) < accel_err_thr && + fabsf(accel_ema[1] - CONSTANTS_ONE_G) < accel_err_thr && + fabsf(accel_ema[2]) < accel_err_thr ) return 2; // [ 0, g, 0 ] - if ( fabs(accel_ema[0]) < accel_err_thr && - fabs(accel_ema[1] + CONSTANTS_ONE_G) < accel_err_thr && - fabs(accel_ema[2]) < accel_err_thr ) + if ( fabsf(accel_ema[0]) < accel_err_thr && + fabsf(accel_ema[1] + CONSTANTS_ONE_G) < accel_err_thr && + fabsf(accel_ema[2]) < accel_err_thr ) return 3; // [ 0, -g, 0 ] - if ( fabs(accel_ema[0]) < accel_err_thr && - fabs(accel_ema[1]) < accel_err_thr && - fabs(accel_ema[2] - CONSTANTS_ONE_G) < accel_err_thr ) + if ( fabsf(accel_ema[0]) < accel_err_thr && + fabsf(accel_ema[1]) < accel_err_thr && + fabsf(accel_ema[2] - CONSTANTS_ONE_G) < accel_err_thr ) return 4; // [ 0, 0, g ] - if ( fabs(accel_ema[0]) < accel_err_thr && - fabs(accel_ema[1]) < accel_err_thr && - fabs(accel_ema[2] + CONSTANTS_ONE_G) < accel_err_thr ) + if ( fabsf(accel_ema[0]) < accel_err_thr && + fabsf(accel_ema[1]) < accel_err_thr && + fabsf(accel_ema[2] + CONSTANTS_ONE_G) < accel_err_thr ) return 5; // [ 0, 0, -g ] mavlink_log_info(mavlink_fd, "ERROR: invalid orientation"); @@ -376,7 +385,9 @@ int detect_orientation(int mavlink_fd, int sub_sensor_combined) { * Read specified number of accelerometer samples, calculate average and dispersion. */ int read_accelerometer_avg(int sensor_combined_sub, float accel_avg[3], int samples_num) { - struct pollfd fds[1] = { { .fd = sensor_combined_sub, .events = POLLIN } }; + struct pollfd fds[1]; + fds[0].fd = sensor_combined_sub; + fds[0].events = POLLIN; int count = 0; float accel_sum[3] = { 0.0f, 0.0f, 0.0f }; diff --git a/src/modules/commander/airspeed_calibration.c b/src/modules/commander/airspeed_calibration.cpp similarity index 97% rename from src/modules/commander/airspeed_calibration.c rename to src/modules/commander/airspeed_calibration.cpp index feaf11aee7..df08292e32 100644 --- a/src/modules/commander/airspeed_calibration.c +++ b/src/modules/commander/airspeed_calibration.cpp @@ -32,7 +32,7 @@ ****************************************************************************/ /** - * @file airspeed_calibration.c + * @file airspeed_calibration.cpp * Airspeed sensor calibration routine */ @@ -65,7 +65,9 @@ void do_airspeed_calibration(int mavlink_fd) while (calibration_counter < calibration_count) { /* wait blocking for new data */ - struct pollfd fds[1] = { { .fd = diff_pres_sub, .events = POLLIN } }; + struct pollfd fds[1]; + fds[0].fd = diff_pres_sub; + fds[0].events = POLLIN; int poll_ret = poll(fds, 1, 1000); diff --git a/src/modules/commander/baro_calibration.c b/src/modules/commander/baro_calibration.cpp similarity index 98% rename from src/modules/commander/baro_calibration.c rename to src/modules/commander/baro_calibration.cpp index a705947948..d7515b3d9b 100644 --- a/src/modules/commander/baro_calibration.c +++ b/src/modules/commander/baro_calibration.cpp @@ -32,7 +32,7 @@ ****************************************************************************/ /** - * @file baro_calibration.c + * @file baro_calibration.cpp * Barometer calibration routine */ diff --git a/src/modules/commander/calibration_routines.c b/src/modules/commander/calibration_routines.cpp similarity index 99% rename from src/modules/commander/calibration_routines.c rename to src/modules/commander/calibration_routines.cpp index 799cd42697..be38ea1046 100644 --- a/src/modules/commander/calibration_routines.c +++ b/src/modules/commander/calibration_routines.cpp @@ -33,7 +33,7 @@ ****************************************************************************/ /** - * @file calibration_routines.c + * @file calibration_routines.cpp * Calibration routines implementations. * * @author Lorenz Meier diff --git a/src/modules/commander/commander.c b/src/modules/commander/commander.cpp similarity index 99% rename from src/modules/commander/commander.c rename to src/modules/commander/commander.cpp index c4dc495f6a..253b6295f1 100644 --- a/src/modules/commander/commander.c +++ b/src/modules/commander/commander.cpp @@ -36,13 +36,11 @@ ****************************************************************************/ /** - * @file commander.c + * @file commander.cpp * Main system state machine implementation. * */ -#include "commander.h" - #include #include #include @@ -97,12 +95,11 @@ #include "rc_calibration.h" #include "airspeed_calibration.h" -PARAM_DEFINE_INT32(SYS_FAILSAVE_LL, 0); /**< Go into low-level failsafe after 0 ms */ -//PARAM_DEFINE_INT32(SYS_FAILSAVE_HL, 0); /**< Go into high-level failsafe after 0 ms */ -PARAM_DEFINE_FLOAT(TRIM_ROLL, 0.0f); -PARAM_DEFINE_FLOAT(TRIM_PITCH, 0.0f); -PARAM_DEFINE_FLOAT(TRIM_YAW, 0.0f); - +/* oddly, ERROR is not defined for c++ */ +#ifdef ERROR +# undef ERROR +#endif +static const int ERROR = -1; extern struct system_load_s system_load; @@ -160,8 +157,10 @@ enum { * * The actual stack size should be set in the call * to task_create(). + * + * @ingroup apps */ -__EXPORT int commander_main(int argc, char *argv[]); +extern "C" __EXPORT int commander_main(int argc, char *argv[]); /** * Print the correct usage. diff --git a/src/modules/commander/commander_helper.c b/src/modules/commander/commander_helper.cpp similarity index 97% rename from src/modules/commander/commander_helper.c rename to src/modules/commander/commander_helper.cpp index 199f73e6c8..9427bd8925 100644 --- a/src/modules/commander/commander_helper.c +++ b/src/modules/commander/commander_helper.cpp @@ -34,7 +34,7 @@ ****************************************************************************/ /** - * @file commander_helper.c + * @file commander_helper.cpp * Commander helper functions implementations */ @@ -56,6 +56,12 @@ #include "commander_helper.h" +/* oddly, ERROR is not defined for c++ */ +#ifdef ERROR +# undef ERROR +#endif +static const int ERROR = -1; + bool is_multirotor(const struct vehicle_status_s *current_status) { return ((current_status->system_type == VEHICLE_TYPE_QUADROTOR) || @@ -175,11 +181,6 @@ int led_off(int led) return ioctl(leds, LED_OFF, led); } - -PARAM_DEFINE_FLOAT(BAT_V_EMPTY, 3.2f); -PARAM_DEFINE_FLOAT(BAT_V_FULL, 4.05f); -PARAM_DEFINE_FLOAT(BAT_N_CELLS, 3); - float battery_remaining_estimate_voltage(float voltage) { float ret = 0; diff --git a/src/modules/commander/commander.h b/src/modules/commander/commander_params.c similarity index 76% rename from src/modules/commander/commander.h rename to src/modules/commander/commander_params.c index 6e57c0ba5f..6832fc5ce3 100644 --- a/src/modules/commander/commander.h +++ b/src/modules/commander/commander_params.c @@ -1,10 +1,7 @@ /**************************************************************************** * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * Author: Petri Tanskanen - * Lorenz Meier - * Thomas Gubler - * Julian Oes + * Copyright (c) 2013 PX4 Development Team. All rights reserved. + * Author: Lorenz Meier * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -36,19 +33,22 @@ ****************************************************************************/ /** - * @file commander.h - * Main system state machine definition. + * @file commander_params.c + * + * Parameters defined by the sensors task. * - * @author Petri Tanskanen * @author Lorenz Meier * @author Thomas Gubler * @author Julian Oes - * */ -#ifndef COMMANDER_H_ -#define COMMANDER_H_ +#include +#include - - -#endif /* COMMANDER_H_ */ +PARAM_DEFINE_INT32(SYS_FAILSAVE_LL, 0); /**< Go into low-level failsafe after 0 ms */ +PARAM_DEFINE_FLOAT(TRIM_ROLL, 0.0f); +PARAM_DEFINE_FLOAT(TRIM_PITCH, 0.0f); +PARAM_DEFINE_FLOAT(TRIM_YAW, 0.0f); +PARAM_DEFINE_FLOAT(BAT_V_EMPTY, 3.2f); +PARAM_DEFINE_FLOAT(BAT_V_FULL, 4.05f); +PARAM_DEFINE_FLOAT(BAT_N_CELLS, 3); diff --git a/src/modules/commander/gyro_calibration.c b/src/modules/commander/gyro_calibration.cpp similarity index 83% rename from src/modules/commander/gyro_calibration.c rename to src/modules/commander/gyro_calibration.cpp index 865f74ab44..9e6909db07 100644 --- a/src/modules/commander/gyro_calibration.c +++ b/src/modules/commander/gyro_calibration.cpp @@ -32,7 +32,7 @@ ****************************************************************************/ /** - * @file gyro_calibration.c + * @file gyro_calibration.cpp * Gyroscope calibration routine */ @@ -82,7 +82,9 @@ void do_gyro_calibration(int mavlink_fd) while (calibration_counter < calibration_count) { /* wait blocking for new data */ - struct pollfd fds[1] = { { .fd = sub_sensor_combined, .events = POLLIN } }; + struct pollfd fds[1]; + fds[0].fd = sub_sensor_combined; + fds[0].events = POLLIN; int poll_ret = poll(fds, 1, 1000); @@ -105,6 +107,49 @@ void do_gyro_calibration(int mavlink_fd) gyro_offset[2] = gyro_offset[2] / calibration_count; + if (isfinite(gyro_offset[0]) && isfinite(gyro_offset[1]) && isfinite(gyro_offset[2])) { + + if (param_set(param_find("SENS_GYRO_XOFF"), &(gyro_offset[0])) + || param_set(param_find("SENS_GYRO_YOFF"), &(gyro_offset[1])) + || param_set(param_find("SENS_GYRO_ZOFF"), &(gyro_offset[2]))) { + mavlink_log_critical(mavlink_fd, "Setting gyro offsets failed!"); + } + + /* set offsets to actual value */ + fd = open(GYRO_DEVICE_PATH, 0); + struct gyro_scale gscale = { + gyro_offset[0], + 1.0f, + gyro_offset[1], + 1.0f, + gyro_offset[2], + 1.0f, + }; + + if (OK != ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gscale)) + warn("WARNING: failed to set scale / offsets for gyro"); + + close(fd); + + /* auto-save to EEPROM */ + int save_ret = param_save_default(); + + if (save_ret != 0) { + warnx("WARNING: auto-save of params to storage failed"); + mavlink_log_critical(mavlink_fd, "gyro store failed"); + // XXX negative tune + return; + } + + mavlink_log_info(mavlink_fd, "gyro calibration done"); + + tune_positive(); + /* third beep by cal end routine */ + + } else { + mavlink_log_info(mavlink_fd, "offset cal FAILED (NaN)"); + return; + } /*** --- SCALING --- ***/ @@ -136,7 +181,9 @@ void do_gyro_calibration(int mavlink_fd) break; /* wait blocking for new data */ - struct pollfd fds[1] = { { .fd = sub_sensor_combined, .events = POLLIN } }; + struct pollfd fds[1]; + fds[0].fd = sub_sensor_combined; + fds[0].events = POLLIN; int poll_ret = poll(fds, 1, 1000); @@ -180,27 +227,28 @@ void do_gyro_calibration(int mavlink_fd) } float gyro_scale = baseline_integral / gyro_integral; + float gyro_scales[] = { gyro_scale, gyro_scale, gyro_scale }; warnx("gyro scale: yaw (z): %6.4f", gyro_scale); mavlink_log_info(mavlink_fd, "gyro scale: yaw (z): %6.4f", gyro_scale); - if (isfinite(gyro_offset[0]) && isfinite(gyro_offset[1]) && isfinite(gyro_offset[2])) { + if (isfinite(gyro_scales[0]) && isfinite(gyro_scales[1]) && isfinite(gyro_scales[2])) { - if (param_set(param_find("SENS_GYRO_XOFF"), &(gyro_offset[0])) - || param_set(param_find("SENS_GYRO_YOFF"), &(gyro_offset[1])) - || param_set(param_find("SENS_GYRO_ZOFF"), &(gyro_offset[2]))) { - mavlink_log_critical(mavlink_fd, "Setting gyro offsets failed!"); + if (param_set(param_find("SENS_GYRO_XSCALE"), &(gyro_scales[0])) + || param_set(param_find("SENS_GYRO_YSCALE"), &(gyro_scales[1])) + || param_set(param_find("SENS_GYRO_ZSCALE"), &(gyro_scales[2]))) { + mavlink_log_critical(mavlink_fd, "Setting gyro scale failed!"); } /* set offsets to actual value */ fd = open(GYRO_DEVICE_PATH, 0); struct gyro_scale gscale = { gyro_offset[0], - 1.0f, + gyro_scales[0], gyro_offset[1], - 1.0f, + gyro_scales[1], gyro_offset[2], - 1.0f, + gyro_scales[2], }; if (OK != ioctl(fd, GYROIOCSSCALE, (long unsigned int)&gscale)) diff --git a/src/modules/commander/mag_calibration.c b/src/modules/commander/mag_calibration.cpp similarity index 98% rename from src/modules/commander/mag_calibration.c rename to src/modules/commander/mag_calibration.cpp index dbd31a7e75..9a25103f8c 100644 --- a/src/modules/commander/mag_calibration.c +++ b/src/modules/commander/mag_calibration.cpp @@ -32,7 +32,7 @@ ****************************************************************************/ /** - * @file mag_calibration.c + * @file mag_calibration.cpp * Magnetometer calibration routine */ @@ -120,7 +120,9 @@ void do_mag_calibration(int mavlink_fd) calibration_counter < calibration_maxcount) { /* wait blocking for new data */ - struct pollfd fds[1] = { { .fd = sub_mag, .events = POLLIN } }; + struct pollfd fds[1]; + fds[0].fd = sub_mag; + fds[0].events = POLLIN; /* user guidance */ if (hrt_absolute_time() >= axis_deadline && diff --git a/src/modules/commander/module.mk b/src/modules/commander/module.mk index fef8e366b2..91d75121eb 100644 --- a/src/modules/commander/module.mk +++ b/src/modules/commander/module.mk @@ -36,14 +36,15 @@ # MODULE_COMMAND = commander -SRCS = commander.c \ - state_machine_helper.c \ - commander_helper.c \ - calibration_routines.c \ - accelerometer_calibration.c \ - gyro_calibration.c \ - mag_calibration.c \ - baro_calibration.c \ - rc_calibration.c \ - airspeed_calibration.c +SRCS = commander.cpp \ + commander_params.c \ + state_machine_helper.cpp \ + commander_helper.cpp \ + calibration_routines.cpp \ + accelerometer_calibration.cpp \ + gyro_calibration.cpp \ + mag_calibration.cpp \ + baro_calibration.cpp \ + rc_calibration.cpp \ + airspeed_calibration.cpp diff --git a/src/modules/commander/rc_calibration.c b/src/modules/commander/rc_calibration.cpp similarity index 99% rename from src/modules/commander/rc_calibration.c rename to src/modules/commander/rc_calibration.cpp index a21d3f558b..0de411713b 100644 --- a/src/modules/commander/rc_calibration.c +++ b/src/modules/commander/rc_calibration.cpp @@ -32,7 +32,7 @@ ****************************************************************************/ /** - * @file rc_calibration.c + * @file rc_calibration.cpp * Remote Control calibration routine */ diff --git a/src/modules/commander/state_machine_helper.c b/src/modules/commander/state_machine_helper.cpp similarity index 99% rename from src/modules/commander/state_machine_helper.c rename to src/modules/commander/state_machine_helper.cpp index 792ead8f3c..3cf60a99ac 100644 --- a/src/modules/commander/state_machine_helper.c +++ b/src/modules/commander/state_machine_helper.cpp @@ -34,7 +34,7 @@ ****************************************************************************/ /** - * @file state_machine_helper.c + * @file state_machine_helper.cpp * State machine helper functions implementations */ @@ -56,6 +56,11 @@ #include "state_machine_helper.h" #include "commander_helper.h" +/* oddly, ERROR is not defined for c++ */ +#ifdef ERROR +# undef ERROR +#endif +static const int ERROR = -1; int arming_state_transition(int status_pub, struct vehicle_status_s *current_state, arming_state_t new_arming_state, int armed_pub, struct actuator_armed_s *armed, const int mavlink_fd) { diff --git a/src/modules/systemlib/systemlib.h b/src/modules/systemlib/systemlib.h index 0194b5e529..356215b0e2 100644 --- a/src/modules/systemlib/systemlib.h +++ b/src/modules/systemlib/systemlib.h @@ -42,11 +42,12 @@ #include #include -/** Reboots the board */ -extern void up_systemreset(void) noreturn_function; - __BEGIN_DECLS +/** Reboots the board */ +//extern void up_systemreset(void) noreturn_function; +#include <../arch/common/up_internal.h> + /** Sends SIGUSR1 to all processes */ __EXPORT void killall(void); From eb52eae153bc83144ddb5d6d03fdbb22aa0c9203 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 20 Jul 2013 13:48:19 +0200 Subject: [PATCH 089/109] Code style for BlinkM --- src/drivers/blinkm/blinkm.cpp | 10 ++++++---- 1 file changed, 6 insertions(+), 4 deletions(-) diff --git a/src/drivers/blinkm/blinkm.cpp b/src/drivers/blinkm/blinkm.cpp index 9bb020a6b4..aa94be4938 100644 --- a/src/drivers/blinkm/blinkm.cpp +++ b/src/drivers/blinkm/blinkm.cpp @@ -866,11 +866,13 @@ BlinkM::get_firmware_version(uint8_t version[2]) return transfer(&msg, sizeof(msg), version, 2); } +void blinkm_usage(); + void blinkm_usage() { - fprintf(stderr, "missing command: try 'start', 'systemstate', 'ledoff', 'list' or a script name {options}\n"); - fprintf(stderr, "options:\n"); - fprintf(stderr, "\t-b --bus i2cbus (3)\n"); - fprintf(stderr, "\t-a --blinkmaddr blinkmaddr (9)\n"); + warnx("missing command: try 'start', 'systemstate', 'ledoff', 'list' or a script name {options}"); + warnx("options:"); + warnx("\t-b --bus i2cbus (3)"); + warnx("\t-a --blinkmaddr blinkmaddr (9)"); } int From 963abd66badf71a925f80e12312c429d64999424 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Mon, 22 Jul 2013 21:48:21 +0400 Subject: [PATCH 090/109] commander: WIP, SEATBELT renamed to ASSISTED, added SIMPLE mode, added ASSISTED switch, some cleanup and reorganizing --- src/modules/commander/commander.cpp | 190 ++++++++++-------- .../commander/state_machine_helper.cpp | 66 ++++-- src/modules/mavlink/mavlink.c | 12 +- .../uORB/topics/manual_control_setpoint.h | 1 + src/modules/uORB/topics/vehicle_status.h | 17 +- 5 files changed, 178 insertions(+), 108 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 253b6295f1..25c5a84ea8 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1005,17 +1005,15 @@ int commander_thread_main(int argc, char *argv[]) /* check for global or local position updates, set a timeout of 2s */ - if (hrt_absolute_time() - last_global_position_time < 2000000 && hrt_absolute_time() > 2000000) { + if (hrt_absolute_time() - last_global_position_time < 2000000 && hrt_absolute_time() > 2000000 && global_position.valid) { current_status.condition_global_position_valid = true; - // XXX check for controller status and home position as well } else { current_status.condition_global_position_valid = false; } - if (hrt_absolute_time() - last_local_position_time < 2000000 && hrt_absolute_time() > 2000000) { + if (hrt_absolute_time() - last_local_position_time < 2000000 && hrt_absolute_time() > 2000000 && local_position.valid) { current_status.condition_local_position_valid = true; - // XXX check for controller status and home position as well } else { current_status.condition_local_position_valid = false; @@ -1154,7 +1152,7 @@ int commander_thread_main(int argc, char *argv[]) } else { /* center stick position, set seatbelt/simple control */ - current_status.mode_switch = MODE_SWITCH_SEATBELT; + current_status.mode_switch = MODE_SWITCH_ASSISTED; } // warnx("man ctrl mode: %d\n", (int)current_status.manual_control_mode); @@ -1167,21 +1165,32 @@ int commander_thread_main(int argc, char *argv[]) /* this switch is not properly mapped, set default */ current_status.return_switch = RETURN_SWITCH_NONE; - } else if (sp_man.return_switch < -STICK_ON_OFF_LIMIT) { - - /* bottom stick position, set altitude hold */ - current_status.return_switch = RETURN_SWITCH_NONE; - } else if (sp_man.return_switch > STICK_ON_OFF_LIMIT) { /* top stick position */ current_status.return_switch = RETURN_SWITCH_RETURN; } else { - /* center stick position, set default */ + /* center or bottom stick position, set default */ current_status.return_switch = RETURN_SWITCH_NONE; } + /* check assisted switch */ + if (!isfinite(sp_man.assisted_switch)) { + + /* this switch is not properly mapped, set default */ + current_status.assisted_switch = ASSISTED_SWITCH_SEATBELT; + + } else if (sp_man.assisted_switch > STICK_ON_OFF_LIMIT) { + + /* top stick position */ + current_status.assisted_switch = ASSISTED_SWITCH_SIMPLE; + + } else { + /* center or bottom stick position, set default */ + current_status.assisted_switch = ASSISTED_SWITCH_SEATBELT; + } + /* check mission switch */ if (!isfinite(sp_man.mission_switch)) { @@ -1219,10 +1228,10 @@ int commander_thread_main(int argc, char *argv[]) warnx("ERROR: Navigation state MANUAL_STANDBY rejected"); } - /* Try seatbelt or fallback to manual */ - } else if (current_status.mode_switch == MODE_SWITCH_SEATBELT) { + /* Try assisted or fallback to manual */ + } else if (current_status.mode_switch == MODE_SWITCH_ASSISTED) { - if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_SEATBELT_STANDBY, control_mode_pub, &control_mode, mavlink_fd) != OK) { + if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_ASSISTED_STANDBY, control_mode_pub, &control_mode, mavlink_fd) != OK) { // fallback to MANUAL_STANDBY if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_MANUAL_STANDBY, control_mode_pub, &control_mode, mavlink_fd) != OK) { // These is not supposed to happen @@ -1235,7 +1244,7 @@ int commander_thread_main(int argc, char *argv[]) if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_AUTO_STANDBY, control_mode_pub, &control_mode, mavlink_fd) != OK) { // first fallback to SEATBELT_STANDY - if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_SEATBELT_STANDBY, control_mode_pub, &control_mode, mavlink_fd) != OK) { + if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_ASSISTED_STANDBY, control_mode_pub, &control_mode, mavlink_fd) != OK) { // or fallback to MANUAL_STANDBY if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_MANUAL_STANDBY, control_mode_pub, &control_mode, mavlink_fd) != OK) { // These is not supposed to happen @@ -1250,89 +1259,104 @@ int commander_thread_main(int argc, char *argv[]) /* evaluate the navigation state when armed */ case ARMING_STATE_ARMED: - /* Always accept manual mode */ if (current_status.mode_switch == MODE_SWITCH_MANUAL) { - + /* MANUAL */ if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_MANUAL, control_mode_pub, &control_mode, mavlink_fd) != OK) { // These is not supposed to happen warnx("ERROR: Navigation state MANUAL rejected"); } - /* SEATBELT_STANDBY (fallback: MANUAL) */ - } else if (current_status.mode_switch == MODE_SWITCH_SEATBELT - && current_status.return_switch == RETURN_SWITCH_NONE) { - - if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_SEATBELT, control_mode_pub, &control_mode, mavlink_fd) != OK) { - // fallback to MANUAL_STANDBY - if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_MANUAL, control_mode_pub, &control_mode, mavlink_fd) != OK) { - // These is not supposed to happen - warnx("ERROR: Navigation state MANUAL rejected"); - } - } - - /* SEATBELT_DESCENT (fallback: MANUAL) */ - } else if (current_status.mode_switch == MODE_SWITCH_SEATBELT - && current_status.return_switch == RETURN_SWITCH_RETURN) { - - if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_SEATBELT_DESCENT, control_mode_pub, &control_mode, mavlink_fd) != OK) { - // fallback to MANUAL_STANDBY - if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_MANUAL, control_mode_pub, &control_mode, mavlink_fd) != OK) { - // These is not supposed to happen - warnx("ERROR: Navigation state MANUAL rejected"); - } - } - - /* AUTO_LOITER (fallback: SEATBELT, MANUAL) */ - } else if (current_status.mode_switch == MODE_SWITCH_AUTO - && current_status.return_switch == RETURN_SWITCH_NONE - && current_status.mission_switch == MISSION_SWITCH_NONE) { - - /* we might come from the disarmed state AUTO_STANDBY */ - if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_AUTO_READY, control_mode_pub, &control_mode, mavlink_fd) != OK) { - if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_SEATBELT, control_mode_pub, &control_mode, mavlink_fd) != OK) { - // fallback to MANUAL_STANDBY + } else if (current_status.mode_switch == MODE_SWITCH_ASSISTED) { + /* ASSISTED */ + if (current_status.return_switch == RETURN_SWITCH_RETURN) { + /* ASSISTED_DESCENT */ + if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_ASSISTED_DESCENT, control_mode_pub, &control_mode, mavlink_fd) != OK) { + // fallback to MANUAL if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_MANUAL, control_mode_pub, &control_mode, mavlink_fd) != OK) { - // These is not supposed to happen - warnx("ERROR: Navigation state MANUAL rejected"); + // These is not supposed to happen + warnx("ERROR: Navigation state MANUAL rejected"); } } - /* or from some other armed state like SEATBELT or MANUAL */ - } else if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_AUTO_LOITER, control_mode_pub, &control_mode, mavlink_fd) != OK) { - if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_SEATBELT, control_mode_pub, &control_mode, mavlink_fd) != OK) { - // fallback to MANUAL_STANDBY - if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_MANUAL, control_mode_pub, &control_mode, mavlink_fd) != OK) { - // These is not supposed to happen - warnx("ERROR: Navigation state MANUAL rejected"); + + } else { + if (current_status.assisted_switch == ASSISTED_SWITCH_SIMPLE) { + /* ASSISTED_SIMPLE */ + if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_ASSISTED_SIMPLE, control_mode_pub, &control_mode, mavlink_fd) != OK) { + // fallback to ASSISTED_SEATBELT + if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_ASSISTED_SEATBELT, control_mode_pub, &control_mode, mavlink_fd) != OK) { + // fallback to MANUAL + if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_MANUAL, control_mode_pub, &control_mode, mavlink_fd) != OK) { + // These is not supposed to happen + warnx("ERROR: Navigation state MANUAL rejected"); + } + } + } + } else { + /* ASSISTED_SEATBELT */ + if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_ASSISTED_SEATBELT, control_mode_pub, &control_mode, mavlink_fd) != OK) { + // fallback to MANUAL + if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_MANUAL, control_mode_pub, &control_mode, mavlink_fd) != OK) { + // These is not supposed to happen + warnx("ERROR: Navigation state MANUAL rejected"); + } } } } - /* AUTO_MISSION (fallback: SEATBELT, MANUAL) */ - } else if (current_status.mode_switch == MODE_SWITCH_AUTO - && current_status.return_switch == RETURN_SWITCH_NONE - && current_status.mission_switch == MISSION_SWITCH_MISSION) { - - if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_AUTO_MISSION, control_mode_pub, &control_mode, mavlink_fd) != OK) { - if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_SEATBELT, control_mode_pub, &control_mode, mavlink_fd) != OK) { - // fallback to MANUAL_STANDBY - if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_MANUAL, control_mode_pub, &control_mode, mavlink_fd) != OK) { - // These is not supposed to happen - warnx("ERROR: Navigation state MANUAL rejected"); + } else if (current_status.mode_switch == MODE_SWITCH_AUTO) { + /* AUTO */ + if (current_status.return_switch == RETURN_SWITCH_RETURN) { + /* AUTO_RTL */ + if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_AUTO_RTL, control_mode_pub, &control_mode, mavlink_fd) != OK) { + // fallback to ASSISTED_DESCENT + if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_ASSISTED_DESCENT, control_mode_pub, &control_mode, mavlink_fd) != OK) { + // fallback to MANUAL + if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_MANUAL, control_mode_pub, &control_mode, mavlink_fd) != OK) { + // These is not supposed to happen + warnx("ERROR: Navigation state MANUAL rejected"); + } } } - } - - /* AUTO_RTL (fallback: SEATBELT_DESCENT, MANUAL) */ - } else if (current_status.mode_switch == MODE_SWITCH_AUTO - && current_status.return_switch == RETURN_SWITCH_RETURN - && (current_status.mission_switch == MISSION_SWITCH_NONE || current_status.mission_switch == MISSION_SWITCH_MISSION)) { - - if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_AUTO_RTL, control_mode_pub, &control_mode, mavlink_fd) != OK) { - if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_SEATBELT_DESCENT, control_mode_pub, &control_mode, mavlink_fd) != OK) { - // fallback to MANUAL_STANDBY - if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_MANUAL, control_mode_pub, &control_mode, mavlink_fd) != OK) { - // These is not supposed to happen - warnx("ERROR: Navigation state MANUAL rejected"); + } else { + if (current_status.mission_switch == MISSION_SWITCH_MISSION) { + /* AUTO_MISSION */ + if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_AUTO_MISSION, control_mode_pub, &control_mode, mavlink_fd) != OK) { + // fallback to ASSISTED_SEATBELT + if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_ASSISTED_SEATBELT, control_mode_pub, &control_mode, mavlink_fd) != OK) { + // fallback to MANUAL + if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_MANUAL, control_mode_pub, &control_mode, mavlink_fd) != OK) { + // These is not supposed to happen + warnx("ERROR: Navigation state MANUAL rejected"); + } + } + } + } else { + // TODO check this + if (current_status.navigation_state == NAVIGATION_STATE_AUTO_STANDBY + || current_status.navigation_state == NAVIGATION_STATE_AUTO_LAND) { + /* AUTO_READY */ + if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_AUTO_READY, control_mode_pub, &control_mode, mavlink_fd) != OK) { + /* AUTO_READY */ + if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_ASSISTED_SEATBELT, control_mode_pub, &control_mode, mavlink_fd) != OK) { + // fallback to MANUAL + if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_MANUAL, control_mode_pub, &control_mode, mavlink_fd) != OK) { + // These is not supposed to happen + warnx("ERROR: Navigation state MANUAL rejected"); + } + } + } + } else { + /* AUTO_LOITER */ + if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_AUTO_LOITER, control_mode_pub, &control_mode, mavlink_fd) != OK) { + // fallback to ASSISTED_SEATBELT + if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_ASSISTED_SEATBELT, control_mode_pub, &control_mode, mavlink_fd) != OK) { + // fallback to MANUAL + if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_MANUAL, control_mode_pub, &control_mode, mavlink_fd) != OK) { + // These is not supposed to happen + warnx("ERROR: Navigation state MANUAL rejected"); + } + } + } } } } diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 3cf60a99ac..60ab01536f 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -184,7 +184,7 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current /* transitions back to INIT are possible for calibration */ if (current_state->navigation_state == NAVIGATION_STATE_MANUAL_STANDBY - || current_state->navigation_state == NAVIGATION_STATE_SEATBELT_STANDBY + || current_state->navigation_state == NAVIGATION_STATE_ASSISTED_STANDBY || current_state->navigation_state == NAVIGATION_STATE_AUTO_STANDBY) { ret = OK; @@ -200,7 +200,7 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current /* transitions from INIT and other STANDBY states as well as MANUAL are possible */ if (current_state->navigation_state == NAVIGATION_STATE_INIT - || current_state->navigation_state == NAVIGATION_STATE_SEATBELT_STANDBY + || current_state->navigation_state == NAVIGATION_STATE_ASSISTED_STANDBY || current_state->navigation_state == NAVIGATION_STATE_AUTO_STANDBY || current_state->navigation_state == NAVIGATION_STATE_MANUAL) { @@ -235,14 +235,15 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current } break; - case NAVIGATION_STATE_SEATBELT_STANDBY: + case NAVIGATION_STATE_ASSISTED_STANDBY: /* transitions from INIT and other STANDBY states as well as SEATBELT and SEATBELT_DESCENT are possible */ if (current_state->navigation_state == NAVIGATION_STATE_INIT || current_state->navigation_state == NAVIGATION_STATE_MANUAL_STANDBY || current_state->navigation_state == NAVIGATION_STATE_AUTO_STANDBY - || current_state->navigation_state == NAVIGATION_STATE_SEATBELT - || current_state->navigation_state == NAVIGATION_STATE_SEATBELT_DESCENT) { + || current_state->navigation_state == NAVIGATION_STATE_ASSISTED_SEATBELT + || current_state->navigation_state == NAVIGATION_STATE_ASSISTED_SIMPLE + || current_state->navigation_state == NAVIGATION_STATE_ASSISTED_DESCENT) { /* need to be disarmed and have a position estimate */ if (current_state->arming_state != ARMING_STATE_STANDBY) { @@ -262,11 +263,12 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current } break; - case NAVIGATION_STATE_SEATBELT: + case NAVIGATION_STATE_ASSISTED_SEATBELT: /* transitions from all AUTO modes except AUTO_STANDBY and except MANUAL_STANDBY and INIT*/ - if (current_state->navigation_state == NAVIGATION_STATE_SEATBELT_STANDBY - || current_state->navigation_state == NAVIGATION_STATE_SEATBELT_DESCENT + if (current_state->navigation_state == NAVIGATION_STATE_ASSISTED_STANDBY + || current_state->navigation_state == NAVIGATION_STATE_ASSISTED_SIMPLE + || current_state->navigation_state == NAVIGATION_STATE_ASSISTED_DESCENT || current_state->navigation_state == NAVIGATION_STATE_MANUAL || current_state->navigation_state == NAVIGATION_STATE_AUTO_LAND || current_state->navigation_state == NAVIGATION_STATE_AUTO_LOITER @@ -293,10 +295,43 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current } break; - case NAVIGATION_STATE_SEATBELT_DESCENT: + case NAVIGATION_STATE_ASSISTED_SIMPLE: + + /* transitions from all AUTO modes except AUTO_STANDBY and except MANUAL_STANDBY and INIT*/ + if (current_state->navigation_state == NAVIGATION_STATE_ASSISTED_STANDBY + || current_state->navigation_state == NAVIGATION_STATE_ASSISTED_SEATBELT + || current_state->navigation_state == NAVIGATION_STATE_ASSISTED_DESCENT + || current_state->navigation_state == NAVIGATION_STATE_MANUAL + || current_state->navigation_state == NAVIGATION_STATE_AUTO_LAND + || current_state->navigation_state == NAVIGATION_STATE_AUTO_LOITER + || current_state->navigation_state == NAVIGATION_STATE_AUTO_MISSION + || current_state->navigation_state == NAVIGATION_STATE_AUTO_READY + || current_state->navigation_state == NAVIGATION_STATE_AUTO_RTL + || current_state->navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF) { + + /* need to be armed and have a position estimate */ + if (current_state->arming_state != ARMING_STATE_ARMED) { + mavlink_log_critical(mavlink_fd, "Rej. SEATBELT: not armed"); + tune_negative(); + } else if (!current_state->condition_local_position_valid) { + mavlink_log_critical(mavlink_fd, "Rej. SEATBELT: no pos estimate"); + tune_negative(); + } else { + ret = OK; + control_mode->flag_control_rates_enabled = true; + control_mode->flag_control_attitude_enabled = true; + control_mode->flag_control_velocity_enabled = true; + control_mode->flag_control_position_enabled = false; + control_mode->flag_control_manual_enabled = false; + } + } + break; + + case NAVIGATION_STATE_ASSISTED_DESCENT: /* transitions from all AUTO modes except AUTO_STANDBY and except MANUAL_STANDBY and INIT and SEATBELT_STANDBY */ - if (current_state->navigation_state == NAVIGATION_STATE_SEATBELT + if (current_state->navigation_state == NAVIGATION_STATE_ASSISTED_SEATBELT + || current_state->navigation_state == NAVIGATION_STATE_ASSISTED_SIMPLE || current_state->navigation_state == NAVIGATION_STATE_MANUAL || current_state->navigation_state == NAVIGATION_STATE_AUTO_LAND || current_state->navigation_state == NAVIGATION_STATE_AUTO_LOITER @@ -328,7 +363,7 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current /* transitions from INIT or from other STANDBY modes or from AUTO READY */ if (current_state->navigation_state == NAVIGATION_STATE_INIT || current_state->navigation_state == NAVIGATION_STATE_MANUAL_STANDBY - || current_state->navigation_state == NAVIGATION_STATE_SEATBELT_STANDBY + || current_state->navigation_state == NAVIGATION_STATE_ASSISTED_STANDBY || current_state->navigation_state == NAVIGATION_STATE_AUTO_READY) { /* need to be disarmed and have a position and home lock */ @@ -395,7 +430,8 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current if (current_state->navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF || current_state->navigation_state == NAVIGATION_STATE_AUTO_MISSION || current_state->navigation_state == NAVIGATION_STATE_AUTO_RTL - || current_state->navigation_state == NAVIGATION_STATE_SEATBELT + || current_state->navigation_state == NAVIGATION_STATE_ASSISTED_SEATBELT + || current_state->navigation_state == NAVIGATION_STATE_ASSISTED_SIMPLE || current_state->navigation_state == NAVIGATION_STATE_MANUAL) { /* need to have a position and home lock */ @@ -422,7 +458,8 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current if (current_state->navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF || current_state->navigation_state == NAVIGATION_STATE_AUTO_LOITER || current_state->navigation_state == NAVIGATION_STATE_AUTO_RTL - || current_state->navigation_state == NAVIGATION_STATE_SEATBELT + || current_state->navigation_state == NAVIGATION_STATE_ASSISTED_SEATBELT + || current_state->navigation_state == NAVIGATION_STATE_ASSISTED_SIMPLE || current_state->navigation_state == NAVIGATION_STATE_MANUAL) { /* need to have a mission ready */ @@ -446,7 +483,8 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current if (current_state->navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF || current_state->navigation_state == NAVIGATION_STATE_AUTO_MISSION || current_state->navigation_state == NAVIGATION_STATE_AUTO_LOITER - || current_state->navigation_state == NAVIGATION_STATE_SEATBELT + || current_state->navigation_state == NAVIGATION_STATE_ASSISTED_SEATBELT + || current_state->navigation_state == NAVIGATION_STATE_ASSISTED_SIMPLE || current_state->navigation_state == NAVIGATION_STATE_MANUAL) { /* need to have a position and home lock */ diff --git a/src/modules/mavlink/mavlink.c b/src/modules/mavlink/mavlink.c index db8ee90678..ae8e168e18 100644 --- a/src/modules/mavlink/mavlink.c +++ b/src/modules/mavlink/mavlink.c @@ -221,9 +221,9 @@ get_mavlink_mode_and_state(uint8_t *mavlink_state, uint8_t *mavlink_mode) *mavlink_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED; } - if (v_status.navigation_state == NAVIGATION_STATE_SEATBELT - || v_status.navigation_state == NAVIGATION_STATE_SEATBELT_DESCENT - || v_status.navigation_state == NAVIGATION_STATE_SEATBELT_STANDBY) { + if (v_status.navigation_state == NAVIGATION_STATE_ASSISTED_SEATBELT + || v_status.navigation_state == NAVIGATION_STATE_ASSISTED_DESCENT + || v_status.navigation_state == NAVIGATION_STATE_ASSISTED_STANDBY) { *mavlink_mode |= MAV_MODE_FLAG_GUIDED_ENABLED; } @@ -248,15 +248,15 @@ get_mavlink_mode_and_state(uint8_t *mavlink_state, uint8_t *mavlink_mode) || v_status.navigation_state == NAVIGATION_STATE_AUTO_MISSION || v_status.navigation_state == NAVIGATION_STATE_AUTO_RTL || v_status.navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF - || v_status.navigation_state == NAVIGATION_STATE_SEATBELT - || v_status.navigation_state == NAVIGATION_STATE_SEATBELT_DESCENT + || v_status.navigation_state == NAVIGATION_STATE_ASSISTED_SEATBELT + || v_status.navigation_state == NAVIGATION_STATE_ASSISTED_DESCENT || v_status.navigation_state == NAVIGATION_STATE_MANUAL) { *mavlink_state = MAV_STATE_ACTIVE; } else if (v_status.navigation_state == NAVIGATION_STATE_AUTO_STANDBY || v_status.navigation_state == NAVIGATION_STATE_AUTO_READY // XXX correct? - || v_status.navigation_state == NAVIGATION_STATE_SEATBELT_STANDBY + || v_status.navigation_state == NAVIGATION_STATE_ASSISTED_STANDBY || v_status.navigation_state == NAVIGATION_STATE_MANUAL_STANDBY) { *mavlink_state = MAV_STATE_STANDBY; diff --git a/src/modules/uORB/topics/manual_control_setpoint.h b/src/modules/uORB/topics/manual_control_setpoint.h index cfee81ea2b..eac6d6e986 100644 --- a/src/modules/uORB/topics/manual_control_setpoint.h +++ b/src/modules/uORB/topics/manual_control_setpoint.h @@ -58,6 +58,7 @@ struct manual_control_setpoint_s { float mode_switch; /**< mode 3 position switch (mandatory): manual, assisted, auto */ float return_switch; /**< land 2 position switch (mandatory): land, no effect */ + float assisted_switch; /**< assisted 2 position switch (optional): seatbelt, simple */ float mission_switch; /**< mission 2 position switch (optional): mission, loiter */ /** diff --git a/src/modules/uORB/topics/vehicle_status.h b/src/modules/uORB/topics/vehicle_status.h index ec08a6c13a..9b91e834e0 100644 --- a/src/modules/uORB/topics/vehicle_status.h +++ b/src/modules/uORB/topics/vehicle_status.h @@ -60,12 +60,13 @@ /* State Machine */ typedef enum { - NAVIGATION_STATE_INIT=0, + NAVIGATION_STATE_INIT = 0, NAVIGATION_STATE_MANUAL_STANDBY, NAVIGATION_STATE_MANUAL, - NAVIGATION_STATE_SEATBELT_STANDBY, - NAVIGATION_STATE_SEATBELT, - NAVIGATION_STATE_SEATBELT_DESCENT, + NAVIGATION_STATE_ASSISTED_STANDBY, + NAVIGATION_STATE_ASSISTED_SEATBELT, + NAVIGATION_STATE_ASSISTED_SIMPLE, + NAVIGATION_STATE_ASSISTED_DESCENT, NAVIGATION_STATE_AUTO_STANDBY, NAVIGATION_STATE_AUTO_READY, NAVIGATION_STATE_AUTO_TAKEOFF, @@ -98,7 +99,7 @@ typedef enum { typedef enum { MODE_SWITCH_MANUAL = 0, - MODE_SWITCH_SEATBELT, + MODE_SWITCH_ASSISTED, MODE_SWITCH_AUTO } mode_switch_pos_t; @@ -107,6 +108,11 @@ typedef enum { RETURN_SWITCH_RETURN } return_switch_pos_t; +typedef enum { + ASSISTED_SWITCH_SEATBELT = 0, + ASSISTED_SWITCH_SIMPLE +} assisted_switch_pos_t; + typedef enum { MISSION_SWITCH_NONE = 0, MISSION_SWITCH_MISSION @@ -179,6 +185,7 @@ struct vehicle_status_s mode_switch_pos_t mode_switch; return_switch_pos_t return_switch; + assisted_switch_pos_t assisted_switch; mission_switch_pos_t mission_switch; bool condition_battery_voltage_valid; From 55fd19f2813110d14d536943503851255c997b6f Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Mon, 22 Jul 2013 22:37:10 +0400 Subject: [PATCH 091/109] sensors: ASSISTED switch channel added --- src/modules/sensors/sensor_params.c | 1 + src/modules/sensors/sensors.cpp | 14 +++++++++++++- src/modules/uORB/topics/rc_channels.h | 24 ++++++++++++++---------- 3 files changed, 28 insertions(+), 11 deletions(-) diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c index 3ed9efc8b4..d0af9e17bf 100644 --- a/src/modules/sensors/sensor_params.c +++ b/src/modules/sensors/sensor_params.c @@ -167,6 +167,7 @@ PARAM_DEFINE_INT32(RC_MAP_YAW, 4); PARAM_DEFINE_INT32(RC_MAP_MODE_SW, 5); PARAM_DEFINE_INT32(RC_MAP_RETURN_SW, 6); +PARAM_DEFINE_INT32(RC_MAP_ASSIST_SW, 0); PARAM_DEFINE_INT32(RC_MAP_MISSIO_SW, 0); //PARAM_DEFINE_INT32(RC_MAP_OFFB_SW, 0); diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 89d5cd374c..b38dc8d890 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -208,6 +208,7 @@ private: int rc_map_mode_sw; int rc_map_return_sw; + int rc_map_assisted_sw; int rc_map_mission_sw; // int rc_map_offboard_ctrl_mode_sw; @@ -256,6 +257,7 @@ private: param_t rc_map_mode_sw; param_t rc_map_return_sw; + param_t rc_map_assisted_sw; param_t rc_map_mission_sw; // param_t rc_map_offboard_ctrl_mode_sw; @@ -464,6 +466,7 @@ Sensors::Sensors() : _parameter_handles.rc_map_flaps = param_find("RC_MAP_FLAPS"); /* optional mode switches, not mapped per default */ + _parameter_handles.rc_map_assisted_sw = param_find("RC_MAP_ASSIST_SW"); _parameter_handles.rc_map_mission_sw = param_find("RC_MAP_MISSIO_SW"); // _parameter_handles.rc_map_offboard_ctrl_mode_sw = param_find("RC_MAP_OFFB_SW"); @@ -617,6 +620,10 @@ Sensors::parameters_update() warnx("Failed getting return sw chan index"); } + if (param_get(_parameter_handles.rc_map_assisted_sw, &(_parameters.rc_map_assisted_sw)) != OK) { + warnx("Failed getting assisted sw chan index"); + } + if (param_get(_parameter_handles.rc_map_mission_sw, &(_parameters.rc_map_mission_sw)) != OK) { warnx("Failed getting mission sw chan index"); } @@ -673,6 +680,7 @@ Sensors::parameters_update() _rc.function[MODE] = _parameters.rc_map_mode_sw - 1; _rc.function[RETURN] = _parameters.rc_map_return_sw - 1; + _rc.function[ASSISTED] = _parameters.rc_map_assisted_sw - 1; _rc.function[MISSION] = _parameters.rc_map_mission_sw - 1; _rc.function[FLAPS] = _parameters.rc_map_flaps - 1; @@ -1142,6 +1150,7 @@ Sensors::ppm_poll() manual_control.mode_switch = NAN; manual_control.return_switch = NAN; + manual_control.assisted_switch = NAN; manual_control.mission_switch = NAN; // manual_control.auto_offboard_input_switch = NAN; @@ -1249,7 +1258,10 @@ Sensors::ppm_poll() /* land switch input */ manual_control.return_switch = limit_minus_one_to_one(_rc.chan[_rc.function[RETURN]].scaled); - /* land switch input */ + /* assisted switch input */ + manual_control.assisted_switch = limit_minus_one_to_one(_rc.chan[_rc.function[ASSISTED]].scaled); + + /* mission switch input */ manual_control.mission_switch = limit_minus_one_to_one(_rc.chan[_rc.function[MISSION]].scaled); /* flaps */ diff --git a/src/modules/uORB/topics/rc_channels.h b/src/modules/uORB/topics/rc_channels.h index a0bb25af46..2167e44a2b 100644 --- a/src/modules/uORB/topics/rc_channels.h +++ b/src/modules/uORB/topics/rc_channels.h @@ -53,9 +53,12 @@ /** * The number of RC channel inputs supported. * Current (Q1/2013) radios support up to 18 channels, - * leaving at a sane value of 14. + * leaving at a sane value of 15. + * This number can be greater then number of RC channels, + * because single RC channel can be mapped to multiple + * functions, e.g. for various mode switches. */ -#define RC_CHANNELS_MAX 14 +#define RC_CHANNELS_MAX 15 /** * This defines the mapping of the RC functions. @@ -70,14 +73,15 @@ enum RC_CHANNELS_FUNCTION YAW = 3, MODE = 4, RETURN = 5, - MISSION = 6, - OFFBOARD_MODE = 7, - FLAPS = 8, - AUX_1 = 9, - AUX_2 = 10, - AUX_3 = 11, - AUX_4 = 12, - AUX_5 = 13, + ASSISTED = 6, + MISSION = 7, + OFFBOARD_MODE = 8, + FLAPS = 9, + AUX_1 = 10, + AUX_2 = 11, + AUX_3 = 12, + AUX_4 = 13, + AUX_5 = 14, RC_CHANNELS_FUNCTION_MAX /**< indicates the number of functions. There can be more functions than RC channels. */ }; From 7c1693a877d96ee1476dcb3a7f5cb5e8dfb27492 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Tue, 23 Jul 2013 14:56:12 +0400 Subject: [PATCH 092/109] commander: don't notify user about rejected disarm to not confuse, flag_control_altitude_enabled added, flags values fixed --- src/modules/commander/commander.cpp | 39 +++++++------------ .../commander/state_machine_helper.cpp | 24 +++++++++--- .../uORB/topics/vehicle_control_mode.h | 1 + 3 files changed, 33 insertions(+), 31 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 25c5a84ea8..1978d87824 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1398,30 +1398,18 @@ int commander_thread_main(int argc, char *argv[]) } } - /* - * Check if left stick is in lower left position --> switch to standby state. - * Do this only for multirotors, not for fixed wing aircraft. - */ - if ((sp_man.yaw < -STICK_ON_OFF_LIMIT) && (sp_man.throttle < STICK_THRUST_RANGE * 0.1f)) { - + /* Check if left stick is in lower left position and we're in manual mode --> switch to standby state. + * Do this only for multirotors, not for fixed wing aircraft. + * TODO allow disarm when landed + */ + if (sp_man.yaw < -STICK_ON_OFF_LIMIT && sp_man.throttle < STICK_THRUST_RANGE * 0.1f && + control_mode.flag_control_manual_enabled && + ((current_status.system_type == VEHICLE_TYPE_QUADROTOR) || + (current_status.system_type == VEHICLE_TYPE_HEXAROTOR) || + (current_status.system_type == VEHICLE_TYPE_OCTOROTOR))) { if (stick_off_counter > STICK_ON_OFF_COUNTER_LIMIT) { - - if((current_status.system_type == VEHICLE_TYPE_QUADROTOR) || - (current_status.system_type == VEHICLE_TYPE_HEXAROTOR) || - (current_status.system_type == VEHICLE_TYPE_OCTOROTOR) - ) { - if (control_mode.flag_control_position_enabled || control_mode.flag_control_velocity_enabled) { - mavlink_log_critical(mavlink_fd, "DISARM DENY, go manual mode first"); - tune_negative(); - } else { - arming_state_transition(status_pub, ¤t_status, ARMING_STATE_STANDBY, armed_pub, &armed, mavlink_fd); - tune_positive(); - } - - } else { - mavlink_log_critical(mavlink_fd, "DISARM not allowed"); - tune_negative(); - } + arming_state_transition(status_pub, ¤t_status, ARMING_STATE_STANDBY, armed_pub, &armed, mavlink_fd); + tune_positive(); stick_off_counter = 0; } else { @@ -1430,9 +1418,8 @@ int commander_thread_main(int argc, char *argv[]) } } - /* check if left stick is in lower right position and we're in manual --> arm */ - if (sp_man.yaw > STICK_ON_OFF_LIMIT && - sp_man.throttle < STICK_THRUST_RANGE * 0.1f) { + /* check if left stick is in lower right position and we're in manual mode --> arm */ + if (sp_man.yaw > STICK_ON_OFF_LIMIT && sp_man.throttle < STICK_THRUST_RANGE * 0.1f) { if (stick_on_counter > STICK_ON_OFF_COUNTER_LIMIT) { arming_state_transition(status_pub, ¤t_status, ARMING_STATE_ARMED, armed_pub, &armed, mavlink_fd); stick_on_counter = 0; diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 60ab01536f..4a7aa66b70 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -192,6 +192,7 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current control_mode->flag_control_attitude_enabled = false; control_mode->flag_control_velocity_enabled = false; control_mode->flag_control_position_enabled = false; + control_mode->flag_control_altitude_enabled = false; control_mode->flag_control_manual_enabled = false; } break; @@ -214,6 +215,7 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current control_mode->flag_control_attitude_enabled = true; control_mode->flag_control_velocity_enabled = false; control_mode->flag_control_position_enabled = false; + control_mode->flag_control_altitude_enabled = false; control_mode->flag_control_manual_enabled = true; } } @@ -231,6 +233,7 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current control_mode->flag_control_attitude_enabled = true; control_mode->flag_control_velocity_enabled = false; control_mode->flag_control_position_enabled = false; + control_mode->flag_control_altitude_enabled = false; control_mode->flag_control_manual_enabled = true; } break; @@ -258,7 +261,8 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current control_mode->flag_control_attitude_enabled = true; control_mode->flag_control_velocity_enabled = true; control_mode->flag_control_position_enabled = false; - control_mode->flag_control_manual_enabled = false; + control_mode->flag_control_altitude_enabled = true; + control_mode->flag_control_manual_enabled = true; } } break; @@ -290,7 +294,8 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current control_mode->flag_control_attitude_enabled = true; control_mode->flag_control_velocity_enabled = true; control_mode->flag_control_position_enabled = false; - control_mode->flag_control_manual_enabled = false; + control_mode->flag_control_altitude_enabled = true; + control_mode->flag_control_manual_enabled = true; } } break; @@ -321,8 +326,9 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current control_mode->flag_control_rates_enabled = true; control_mode->flag_control_attitude_enabled = true; control_mode->flag_control_velocity_enabled = true; - control_mode->flag_control_position_enabled = false; - control_mode->flag_control_manual_enabled = false; + control_mode->flag_control_position_enabled = true; + control_mode->flag_control_altitude_enabled = true; + control_mode->flag_control_manual_enabled = true; } } break; @@ -353,7 +359,8 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current control_mode->flag_control_attitude_enabled = true; control_mode->flag_control_velocity_enabled = true; control_mode->flag_control_position_enabled = false; - control_mode->flag_control_manual_enabled = false; + control_mode->flag_control_altitude_enabled = true; + control_mode->flag_control_manual_enabled = true; } } break; @@ -382,6 +389,7 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current control_mode->flag_control_attitude_enabled = true; control_mode->flag_control_velocity_enabled = true; control_mode->flag_control_position_enabled = true; + control_mode->flag_control_altitude_enabled = true; control_mode->flag_control_manual_enabled = false; } } @@ -405,6 +413,7 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current control_mode->flag_control_attitude_enabled = true; control_mode->flag_control_velocity_enabled = true; control_mode->flag_control_position_enabled = true; + control_mode->flag_control_altitude_enabled = true; control_mode->flag_control_manual_enabled = false; } } @@ -420,6 +429,7 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current control_mode->flag_control_attitude_enabled = true; control_mode->flag_control_velocity_enabled = true; control_mode->flag_control_position_enabled = true; + control_mode->flag_control_altitude_enabled = true; control_mode->flag_control_manual_enabled = false; } break; @@ -447,6 +457,7 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current control_mode->flag_control_attitude_enabled = true; control_mode->flag_control_velocity_enabled = true; control_mode->flag_control_position_enabled = true; + control_mode->flag_control_altitude_enabled = true; control_mode->flag_control_manual_enabled = false; } } @@ -472,6 +483,7 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current control_mode->flag_control_attitude_enabled = true; control_mode->flag_control_velocity_enabled = true; control_mode->flag_control_position_enabled = true; + control_mode->flag_control_altitude_enabled = true; control_mode->flag_control_manual_enabled = false; } } @@ -500,6 +512,7 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current control_mode->flag_control_attitude_enabled = true; control_mode->flag_control_velocity_enabled = true; control_mode->flag_control_position_enabled = true; + control_mode->flag_control_altitude_enabled = true; control_mode->flag_control_manual_enabled = false; } } @@ -524,6 +537,7 @@ int navigation_state_transition(int status_pub, struct vehicle_status_s *current control_mode->flag_control_attitude_enabled = true; control_mode->flag_control_velocity_enabled = true; control_mode->flag_control_position_enabled = true; + control_mode->flag_control_altitude_enabled = true; control_mode->flag_control_manual_enabled = false; } } diff --git a/src/modules/uORB/topics/vehicle_control_mode.h b/src/modules/uORB/topics/vehicle_control_mode.h index 8481a624fd..d83eb45d9a 100644 --- a/src/modules/uORB/topics/vehicle_control_mode.h +++ b/src/modules/uORB/topics/vehicle_control_mode.h @@ -77,6 +77,7 @@ struct vehicle_control_mode_s bool flag_control_attitude_enabled; /**< true if attitude stabilization is mixed in */ bool flag_control_velocity_enabled; /**< true if speed (implies direction) is controlled */ bool flag_control_position_enabled; /**< true if position is controlled */ + bool flag_control_altitude_enabled; /**< true if altitude is controlled */ bool failsave_lowlevel; /**< Set to true if low-level failsafe mode is enabled */ bool failsave_highlevel; /**< Set to true if high-level failsafe mode is enabled */ From 8c1067a017714394955892e3159c8e0c61bd4ba1 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sun, 28 Jul 2013 23:12:16 +0400 Subject: [PATCH 093/109] State machine rewritten, compiles, WIP --- src/modules/commander/commander.cpp | 1226 ++++++++--------- .../commander/state_machine_helper.cpp | 779 +++++------ src/modules/commander/state_machine_helper.h | 12 +- src/modules/mavlink/mavlink.c | 55 +- src/modules/uORB/topics/vehicle_status.h | 55 +- 5 files changed, 944 insertions(+), 1183 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 1978d87824..b2336cbf6f 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -177,6 +177,12 @@ void handle_command(int status_pub, struct vehicle_status_s *current_status, int */ int commander_thread_main(int argc, char *argv[]); +void check_mode_switches(struct manual_control_setpoint_s *sp_man, struct vehicle_status_s *current_status); + +transition_result_t check_main_state_machine(struct vehicle_status_s *current_status); + +transition_result_t check_navigation_state_machine(struct vehicle_status_s *current_status, struct vehicle_control_mode_s *control_mode); + /** * Loop that runs at a lower rate and priority for calibration and parameter tasks. */ @@ -198,11 +204,11 @@ int commander_main(int argc, char *argv[]) thread_should_exit = false; daemon_task = task_spawn_cmd("commander", - SCHED_DEFAULT, - SCHED_PRIORITY_MAX - 40, - 3000, - commander_thread_main, - (argv) ? (const char **)&argv[2] : (const char **)NULL); + SCHED_DEFAULT, + SCHED_PRIORITY_MAX - 40, + 3000, + commander_thread_main, + (argv) ? (const char **)&argv[2] : (const char **)NULL); exit(0); } @@ -242,244 +248,171 @@ void handle_command(int status_pub, struct vehicle_status_s *current_status, int /* request to set different system mode */ switch (cmd->command) { - case VEHICLE_CMD_DO_SET_MODE: + case VEHICLE_CMD_DO_SET_MODE: + break; - /* request to activate HIL */ - if ((int)cmd->param1 & VEHICLE_MODE_FLAG_HIL_ENABLED) { + case VEHICLE_CMD_COMPONENT_ARM_DISARM: + break; - if (OK == hil_state_transition(HIL_STATE_ON, status_pub, current_status, control_mode_pub, current_control_mode, mavlink_fd)) { + case VEHICLE_CMD_PREFLIGHT_REBOOT_SHUTDOWN: + break; + + case VEHICLE_CMD_PREFLIGHT_CALIBRATION: + + /* gyro calibration */ + if ((int)(cmd->param1) == 1) { + + /* check if no other task is scheduled */ + if (low_prio_task == LOW_PRIO_TASK_NONE) { + + /* try to go to INIT/PREFLIGHT arming state */ + if (OK == arming_state_transition(current_status, ARMING_STATE_INIT, armed, mavlink_fd)) { + // TODO publish state result = VEHICLE_CMD_RESULT_ACCEPTED; + /* now set the task for the low prio thread */ + low_prio_task = LOW_PRIO_TASK_GYRO_CALIBRATION; + } else { result = VEHICLE_CMD_RESULT_DENIED; } - } - if ((int)cmd->param1 & VEHICLE_MODE_FLAG_SAFETY_ARMED) { - if (OK == arming_state_transition(status_pub, current_status, ARMING_STATE_ARMED, armed_pub, armed, mavlink_fd)) { - result = VEHICLE_CMD_RESULT_ACCEPTED; - } else { - result = VEHICLE_CMD_RESULT_DENIED; - } } else { - if (OK == arming_state_transition(status_pub, current_status, ARMING_STATE_STANDBY, armed_pub, armed, mavlink_fd)) { + result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; + } + } + + /* magnetometer calibration */ + if ((int)(cmd->param2) == 1) { + + /* check if no other task is scheduled */ + if (low_prio_task == LOW_PRIO_TASK_NONE) { + + /* try to go to INIT/PREFLIGHT arming state */ + if (OK == arming_state_transition(current_status, ARMING_STATE_INIT, armed, mavlink_fd)) { + // TODO publish state result = VEHICLE_CMD_RESULT_ACCEPTED; + /* now set the task for the low prio thread */ + low_prio_task = LOW_PRIO_TASK_MAG_CALIBRATION; + } else { result = VEHICLE_CMD_RESULT_DENIED; } + + } else { + result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; } + } - break; + // /* zero-altitude pressure calibration */ + // if ((int)(cmd->param3) == 1) { - case VEHICLE_CMD_COMPONENT_ARM_DISARM: + // /* check if no other task is scheduled */ + // if(low_prio_task == LOW_PRIO_TASK_NONE) { - /* request to arm */ - if ((int)cmd->param1 == 1) { - if (OK == arming_state_transition(status_pub, current_status, ARMING_STATE_ARMED, armed_pub, armed, mavlink_fd)) { + // /* try to go to INIT/PREFLIGHT arming state */ + // if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_INIT, armed_pub, armed, mavlink_fd)) { + // result = VEHICLE_CMD_RESULT_ACCEPTED; + // /* now set the task for the low prio thread */ + // low_prio_task = LOW_PRIO_TASK_ALTITUDE_CALIBRATION; + // } else { + // result = VEHICLE_CMD_RESULT_DENIED; + // } + // } else { + // result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; + // } + // } + + // /* trim calibration */ + // if ((int)(cmd->param4) == 1) { + + // /* check if no other task is scheduled */ + // if(low_prio_task == LOW_PRIO_TASK_NONE) { + + // /* try to go to INIT/PREFLIGHT arming state */ + // if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_INIT, armed_pub, armed, mavlink_fd)) { + // result = VEHICLE_CMD_RESULT_ACCEPTED; + // /* now set the task for the low prio thread */ + // low_prio_task = LOW_PRIO_TASK_RC_CALIBRATION; + // } else { + // result = VEHICLE_CMD_RESULT_DENIED; + // } + // } else { + // result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; + // } + // } + + + /* accel calibration */ + if ((int)(cmd->param5) == 1) { + + /* check if no other task is scheduled */ + if (low_prio_task == LOW_PRIO_TASK_NONE) { + + /* try to go to INIT/PREFLIGHT arming state */ + if (OK == arming_state_transition(current_status, ARMING_STATE_INIT, armed, mavlink_fd)) { + // TODO publish state result = VEHICLE_CMD_RESULT_ACCEPTED; + /* now set the task for the low prio thread */ + low_prio_task = LOW_PRIO_TASK_ACCEL_CALIBRATION; + } else { result = VEHICLE_CMD_RESULT_DENIED; } - /* request to disarm */ - } else if ((int)cmd->param1 == 0) { - if (OK == arming_state_transition(status_pub, current_status, ARMING_STATE_STANDBY, armed_pub, armed, mavlink_fd)) { + + } else { + result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; + } + } + + /* airspeed calibration */ + if ((int)(cmd->param6) == 1) { + + /* check if no other task is scheduled */ + if (low_prio_task == LOW_PRIO_TASK_NONE) { + + /* try to go to INIT/PREFLIGHT arming state */ + if (OK == arming_state_transition(current_status, ARMING_STATE_INIT, armed, mavlink_fd)) { + // TODO publish state result = VEHICLE_CMD_RESULT_ACCEPTED; + /* now set the task for the low prio thread */ + low_prio_task = LOW_PRIO_TASK_AIRSPEED_CALIBRATION; + } else { result = VEHICLE_CMD_RESULT_DENIED; } + + } else { + result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; } + } - break; + break; - case VEHICLE_CMD_PREFLIGHT_REBOOT_SHUTDOWN: { + case VEHICLE_CMD_PREFLIGHT_STORAGE: - /* request for an autopilot reboot */ - if ((int)cmd->param1 == 1) { - if (OK == arming_state_transition(status_pub, current_status, ARMING_STATE_REBOOT, armed_pub, armed, mavlink_fd)) { - /* reboot is executed later, after positive tune is sent */ - result = VEHICLE_CMD_RESULT_ACCEPTED; - tune_positive(); - mavlink_log_critical(mavlink_fd, "REBOOTING SYSTEM"); - usleep(500000); - up_systemreset(); - /* SPECIAL CASE: NEVER RETURNS FROM THIS FUNCTION CALL */ + if (((int)(cmd->param1)) == 0) { + /* check if no other task is scheduled */ + if (low_prio_task == LOW_PRIO_TASK_NONE) { + low_prio_task = LOW_PRIO_TASK_PARAM_LOAD; + result = VEHICLE_CMD_RESULT_ACCEPTED; - } else { - /* system may return here */ - result = VEHICLE_CMD_RESULT_DENIED; - tune_negative(); - } - } - } - break; - - // /* request to land */ - // case VEHICLE_CMD_NAV_LAND: - // { - // //TODO: add check if landing possible - // //TODO: add landing maneuver - // - // if (0 == update_state_machine_custom_mode_request(status_pub, current_vehicle_status, SYSTEM_STATE_ARMED)) { - // result = VEHICLE_CMD_RESULT_ACCEPTED; - // } } - // break; - // - // /* request to takeoff */ - // case VEHICLE_CMD_NAV_TAKEOFF: - // { - // //TODO: add check if takeoff possible - // //TODO: add takeoff maneuver - // - // if (0 == update_state_machine_custom_mode_request(status_pub, current_vehicle_status, SYSTEM_STATE_AUTO)) { - // result = VEHICLE_CMD_RESULT_ACCEPTED; - // } - // } - // break; - // - /* preflight calibration */ - case VEHICLE_CMD_PREFLIGHT_CALIBRATION: - - /* gyro calibration */ - if ((int)(cmd->param1) == 1) { - - /* check if no other task is scheduled */ - if(low_prio_task == LOW_PRIO_TASK_NONE) { - - /* try to go to INIT/PREFLIGHT arming state */ - if (OK == arming_state_transition(status_pub, current_status, ARMING_STATE_INIT, armed_pub, armed, mavlink_fd)) { - result = VEHICLE_CMD_RESULT_ACCEPTED; - /* now set the task for the low prio thread */ - low_prio_task = LOW_PRIO_TASK_GYRO_CALIBRATION; - } else { - result = VEHICLE_CMD_RESULT_DENIED; - } - } else { - result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; - } - } - - /* magnetometer calibration */ - if ((int)(cmd->param2) == 1) { - - /* check if no other task is scheduled */ - if(low_prio_task == LOW_PRIO_TASK_NONE) { - - /* try to go to INIT/PREFLIGHT arming state */ - if (OK == arming_state_transition(status_pub, current_status, ARMING_STATE_INIT, armed_pub, armed, mavlink_fd)) { - result = VEHICLE_CMD_RESULT_ACCEPTED; - /* now set the task for the low prio thread */ - low_prio_task = LOW_PRIO_TASK_MAG_CALIBRATION; - } else { - result = VEHICLE_CMD_RESULT_DENIED; - } - } else { - result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; - } + } else { + result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; } - // /* zero-altitude pressure calibration */ - // if ((int)(cmd->param3) == 1) { + } else if (((int)(cmd->param1)) == 1) { - // /* check if no other task is scheduled */ - // if(low_prio_task == LOW_PRIO_TASK_NONE) { + /* check if no other task is scheduled */ + if (low_prio_task == LOW_PRIO_TASK_NONE) { + low_prio_task = LOW_PRIO_TASK_PARAM_SAVE; + result = VEHICLE_CMD_RESULT_ACCEPTED; - // /* try to go to INIT/PREFLIGHT arming state */ - // if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_INIT, armed_pub, armed, mavlink_fd)) { - // result = VEHICLE_CMD_RESULT_ACCEPTED; - // /* now set the task for the low prio thread */ - // low_prio_task = LOW_PRIO_TASK_ALTITUDE_CALIBRATION; - // } else { - // result = VEHICLE_CMD_RESULT_DENIED; - // } - // } else { - // result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; - // } - // } - - - - // /* trim calibration */ - // if ((int)(cmd->param4) == 1) { - - // /* check if no other task is scheduled */ - // if(low_prio_task == LOW_PRIO_TASK_NONE) { - - // /* try to go to INIT/PREFLIGHT arming state */ - // if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_INIT, armed_pub, armed, mavlink_fd)) { - // result = VEHICLE_CMD_RESULT_ACCEPTED; - // /* now set the task for the low prio thread */ - // low_prio_task = LOW_PRIO_TASK_RC_CALIBRATION; - // } else { - // result = VEHICLE_CMD_RESULT_DENIED; - // } - // } else { - // result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; - // } - // } - - - /* accel calibration */ - if ((int)(cmd->param5) == 1) { - - /* check if no other task is scheduled */ - if(low_prio_task == LOW_PRIO_TASK_NONE) { - - /* try to go to INIT/PREFLIGHT arming state */ - if (OK == arming_state_transition(status_pub, current_status, ARMING_STATE_INIT, armed_pub, armed, mavlink_fd)) { - result = VEHICLE_CMD_RESULT_ACCEPTED; - /* now set the task for the low prio thread */ - low_prio_task = LOW_PRIO_TASK_ACCEL_CALIBRATION; - } else { - result = VEHICLE_CMD_RESULT_DENIED; - } - } else { - result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; - } + } else { + result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; } + } - /* airspeed calibration */ - if ((int)(cmd->param6) == 1) { - - /* check if no other task is scheduled */ - if(low_prio_task == LOW_PRIO_TASK_NONE) { - - /* try to go to INIT/PREFLIGHT arming state */ - if (OK == arming_state_transition(status_pub, current_status, ARMING_STATE_INIT, armed_pub, armed, mavlink_fd)) { - result = VEHICLE_CMD_RESULT_ACCEPTED; - /* now set the task for the low prio thread */ - low_prio_task = LOW_PRIO_TASK_AIRSPEED_CALIBRATION; - } else { - result = VEHICLE_CMD_RESULT_DENIED; - } - } else { - result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; - } - } - break; - - case VEHICLE_CMD_PREFLIGHT_STORAGE: - - if (((int)(cmd->param1)) == 0) { - - /* check if no other task is scheduled */ - if(low_prio_task == LOW_PRIO_TASK_NONE) { - low_prio_task = LOW_PRIO_TASK_PARAM_LOAD; - result = VEHICLE_CMD_RESULT_ACCEPTED; - } else { - result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; - } - - - } else if (((int)(cmd->param1)) == 1) { - - /* check if no other task is scheduled */ - if(low_prio_task == LOW_PRIO_TASK_NONE) { - low_prio_task = LOW_PRIO_TASK_PARAM_SAVE; - result = VEHICLE_CMD_RESULT_ACCEPTED; - } else { - result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; - } - } break; default: @@ -546,12 +479,12 @@ int commander_thread_main(int argc, char *argv[]) if (mavlink_fd < 0) { warnx("ERROR: Failed to open MAVLink log stream, start mavlink app first."); } - + /* Main state machine */ - struct vehicle_status_s current_status; + struct vehicle_status_s status; orb_advert_t status_pub; /* make sure we are in preflight state */ - memset(¤t_status, 0, sizeof(current_status)); + memset(&status, 0, sizeof(status)); /* armed topic */ struct actuator_armed_s armed; @@ -566,17 +499,18 @@ int commander_thread_main(int argc, char *argv[]) /* Initialize all flags to false */ memset(&control_mode, 0, sizeof(control_mode)); - current_status.navigation_state = NAVIGATION_STATE_INIT; - current_status.arming_state = ARMING_STATE_INIT; - current_status.hil_state = HIL_STATE_OFF; + status.main_state = MAIN_STATE_MANUAL; + status.navigation_state = NAVIGATION_STATE_STANDBY; + status.arming_state = ARMING_STATE_INIT; + status.hil_state = HIL_STATE_OFF; /* neither manual nor offboard control commands have been received */ - current_status.offboard_control_signal_found_once = false; - current_status.rc_signal_found_once = false; + status.offboard_control_signal_found_once = false; + status.rc_signal_found_once = false; /* mark all signals lost as long as they haven't been found */ - current_status.rc_signal_lost = true; - current_status.offboard_control_signal_lost = true; + status.rc_signal_lost = true; + status.offboard_control_signal_lost = true; /* allow manual override initially */ control_mode.flag_external_manual_override_ok = true; @@ -585,31 +519,31 @@ int commander_thread_main(int argc, char *argv[]) // current_status.flag_vector_flight_mode_ok = false; /* set battery warning flag */ - current_status.battery_warning = VEHICLE_BATTERY_WARNING_NONE; - + status.battery_warning = VEHICLE_BATTERY_WARNING_NONE; + /* set safety device detection flag */ /* XXX do we need this? */ //current_status.flag_safety_present = false; // XXX for now just set sensors as initialized - current_status.condition_system_sensors_initialized = true; + status.condition_system_sensors_initialized = true; // XXX just disable offboard control for now control_mode.flag_control_offboard_enabled = false; /* advertise to ORB */ - status_pub = orb_advertise(ORB_ID(vehicle_status), ¤t_status); + status_pub = orb_advertise(ORB_ID(vehicle_status), &status); /* publish current state machine */ - + /* publish the new state */ - current_status.counter++; - current_status.timestamp = hrt_absolute_time(); - orb_publish(ORB_ID(vehicle_status), status_pub, ¤t_status); + status.counter++; + status.timestamp = hrt_absolute_time(); + orb_publish(ORB_ID(vehicle_status), status_pub, &status); armed_pub = orb_advertise(ORB_ID(actuator_armed), &armed); - control_mode_pub = orb_advertise(ORB_ID(vehicle_control_mode), &control_mode); + control_mode_pub = orb_advertise(ORB_ID(vehicle_control_mode), &control_mode); /* home position */ orb_advert_t home_pub = -1; @@ -686,7 +620,7 @@ int commander_thread_main(int argc, char *argv[]) memset(&local_position, 0, sizeof(local_position)); uint64_t last_local_position_time = 0; - /* + /* * The home position is set based on GPS only, to prevent a dependency between * position estimator and commander. RAW GPS is more than good enough for a * non-flying vehicle. @@ -747,23 +681,28 @@ int commander_thread_main(int argc, char *argv[]) /* update parameters */ if (!armed.armed) { - if (param_get(_param_sys_type, &(current_status.system_type)) != OK) { + if (param_get(_param_sys_type, &(status.system_type)) != OK) { warnx("failed getting new system type"); } /* disable manual override for all systems that rely on electronic stabilization */ - if (current_status.system_type == VEHICLE_TYPE_QUADROTOR || - current_status.system_type == VEHICLE_TYPE_HEXAROTOR || - current_status.system_type == VEHICLE_TYPE_OCTOROTOR) { + if (status.system_type == VEHICLE_TYPE_COAXIAL || + status.system_type == VEHICLE_TYPE_HELICOPTER || + status.system_type == VEHICLE_TYPE_TRICOPTER || + status.system_type == VEHICLE_TYPE_QUADROTOR || + status.system_type == VEHICLE_TYPE_HEXAROTOR || + status.system_type == VEHICLE_TYPE_OCTOROTOR) { control_mode.flag_external_manual_override_ok = false; + status.is_rotary_wing = true; } else { control_mode.flag_external_manual_override_ok = true; + status.is_rotary_wing = false; } /* check and update system / component ID */ - param_get(_param_system_id, &(current_status.system_id)); - param_get(_param_component_id, &(current_status.component_id)); + param_get(_param_system_id, &(status.system_id)); + param_get(_param_component_id, &(status.component_id)); } } @@ -800,7 +739,7 @@ int commander_thread_main(int argc, char *argv[]) orb_copy(ORB_ID(vehicle_command), cmd_sub, &cmd); /* handle it */ - handle_command(status_pub, ¤t_status, control_mode_pub, &control_mode, &cmd, armed_pub, &armed); + handle_command(status_pub, &status, control_mode_pub, &control_mode, &cmd, armed_pub, &armed); } /* update safety topic */ @@ -830,34 +769,37 @@ int commander_thread_main(int argc, char *argv[]) /* set the condition to valid if there has recently been a local position estimate */ if (hrt_absolute_time() - last_local_position_time < LOCAL_POSITION_TIMEOUT) { - current_status.condition_local_position_valid = true; + status.condition_local_position_valid = true; + } else { - current_status.condition_local_position_valid = false; + status.condition_local_position_valid = false; } /* update battery status */ orb_check(battery_sub, &new_data); + if (new_data) { orb_copy(ORB_ID(battery_status), battery_sub, &battery); - current_status.battery_voltage = battery.voltage_v; - current_status.condition_battery_voltage_valid = true; + status.battery_voltage = battery.voltage_v; + status.condition_battery_voltage_valid = true; /* * Only update battery voltage estimate if system has * been running for two and a half seconds. */ - + } - if (hrt_absolute_time() - start_time > 2500000 && current_status.condition_battery_voltage_valid) { - current_status.battery_remaining = battery_remaining_estimate_voltage(current_status.battery_voltage); + if (hrt_absolute_time() - start_time > 2500000 && status.condition_battery_voltage_valid) { + status.battery_remaining = battery_remaining_estimate_voltage(status.battery_voltage); + } else { - current_status.battery_voltage = 0.0f; + status.battery_voltage = 0.0f; } /* update subsystem */ orb_check(subsys_sub, &new_data); - + if (new_data) { orb_copy(ORB_ID(subsystem_info), subsys_sub, &info); @@ -865,26 +807,26 @@ int commander_thread_main(int argc, char *argv[]) /* mark / unmark as present */ if (info.present) { - current_status.onboard_control_sensors_present |= info.subsystem_type; + status.onboard_control_sensors_present |= info.subsystem_type; } else { - current_status.onboard_control_sensors_present &= ~info.subsystem_type; + status.onboard_control_sensors_present &= ~info.subsystem_type; } /* mark / unmark as enabled */ if (info.enabled) { - current_status.onboard_control_sensors_enabled |= info.subsystem_type; + status.onboard_control_sensors_enabled |= info.subsystem_type; } else { - current_status.onboard_control_sensors_enabled &= ~info.subsystem_type; + status.onboard_control_sensors_enabled &= ~info.subsystem_type; } /* mark / unmark as ok */ if (info.ok) { - current_status.onboard_control_sensors_health |= info.subsystem_type; + status.onboard_control_sensors_health |= info.subsystem_type; } else { - current_status.onboard_control_sensors_health &= ~info.subsystem_type; + status.onboard_control_sensors_health &= ~info.subsystem_type; } } @@ -899,6 +841,7 @@ int commander_thread_main(int argc, char *argv[]) } else if (armed.ready_to_arm && (counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0)) { /* ready to arm */ led_toggle(LED_AMBER); + } else if (counter % (100000 / COMMANDER_MONITORING_INTERVAL) == 0) { /* not ready to arm, something is wrong */ led_toggle(LED_AMBER); @@ -908,7 +851,7 @@ int commander_thread_main(int argc, char *argv[]) /* toggle GPS (blue) led at 1 Hz if GPS present but no lock, make is solid once locked */ if ((hrt_absolute_time() - gps_position.timestamp_position < 2000000) - && (gps_position.fix_type == GPS_FIX_TYPE_3D)) { + && (gps_position.fix_type == GPS_FIX_TYPE_3D)) { /* GPS lock */ led_on(LED_BLUE); @@ -940,20 +883,20 @@ int commander_thread_main(int argc, char *argv[]) uint64_t interval_runtime = system_load.tasks[0].total_runtime - last_idle_time; if (last_idle_time > 0) - current_status.load = 1000 - (interval_runtime / 1000); //system load is time spent in non-idle + status.load = 1000 - (interval_runtime / 1000); //system load is time spent in non-idle last_idle_time = system_load.tasks[0].total_runtime; } - + /* if battery voltage is getting lower, warn using buzzer, etc. */ - if (current_status.condition_battery_voltage_valid && (current_status.battery_remaining < 0.15f /* XXX MAGIC NUMBER */) && (false == low_battery_voltage_actions_done)) { //TODO: add filter, or call emergency after n measurements < VOLTAGE_BATTERY_MINIMAL_MILLIVOLTS + if (status.condition_battery_voltage_valid && (status.battery_remaining < 0.15f /* XXX MAGIC NUMBER */) && (false == low_battery_voltage_actions_done)) { //TODO: add filter, or call emergency after n measurements < VOLTAGE_BATTERY_MINIMAL_MILLIVOLTS if (low_voltage_counter > LOW_VOLTAGE_BATTERY_COUNTER_LIMIT) { low_battery_voltage_actions_done = true; mavlink_log_critical(mavlink_fd, "[cmd] WARNING! LOW BATTERY!"); - current_status.battery_warning = VEHICLE_BATTERY_WARNING_WARNING; + status.battery_warning = VEHICLE_BATTERY_WARNING_WARNING; tune_low_bat(); } @@ -961,11 +904,11 @@ int commander_thread_main(int argc, char *argv[]) } /* Critical, this is rather an emergency, change state machine */ - else if (current_status.condition_battery_voltage_valid && (current_status.battery_remaining < 0.1f /* XXX MAGIC NUMBER */) && (false == critical_battery_voltage_actions_done && true == low_battery_voltage_actions_done)) { + else if (status.condition_battery_voltage_valid && (status.battery_remaining < 0.1f /* XXX MAGIC NUMBER */) && (false == critical_battery_voltage_actions_done && true == low_battery_voltage_actions_done)) { if (critical_voltage_counter > CRITICAL_VOLTAGE_BATTERY_COUNTER_LIMIT) { critical_battery_voltage_actions_done = true; mavlink_log_critical(mavlink_fd, "[cmd] EMERGENCY! CRITICAL BATTERY!"); - current_status.battery_warning = VEHICLE_BATTERY_WARNING_ALERT; + status.battery_warning = VEHICLE_BATTERY_WARNING_ALERT; tune_critical_bat(); // XXX implement state change here } @@ -980,9 +923,11 @@ int commander_thread_main(int argc, char *argv[]) /* End battery voltage check */ /* If in INIT state, try to proceed to STANDBY state */ - if (current_status.arming_state == ARMING_STATE_INIT) { + if (status.arming_state == ARMING_STATE_INIT) { // XXX check for sensors - arming_state_transition(status_pub, ¤t_status, ARMING_STATE_STANDBY, armed_pub, &armed, mavlink_fd); + arming_state_transition(&status, ARMING_STATE_STANDBY, &armed, mavlink_fd); + //TODO publish state + } else { // XXX: Add emergency stuff if sensors are lost } @@ -999,32 +944,32 @@ int commander_thread_main(int argc, char *argv[]) /* store current state to reason later about a state change */ // bool vector_flight_mode_ok = current_status.flag_vector_flight_mode_ok; - bool global_pos_valid = current_status.condition_global_position_valid; - bool local_pos_valid = current_status.condition_local_position_valid; - bool airspeed_valid = current_status.condition_airspeed_valid; + bool global_pos_valid = status.condition_global_position_valid; + bool local_pos_valid = status.condition_local_position_valid; + bool airspeed_valid = status.condition_airspeed_valid; /* check for global or local position updates, set a timeout of 2s */ if (hrt_absolute_time() - last_global_position_time < 2000000 && hrt_absolute_time() > 2000000 && global_position.valid) { - current_status.condition_global_position_valid = true; + status.condition_global_position_valid = true; } else { - current_status.condition_global_position_valid = false; + status.condition_global_position_valid = false; } if (hrt_absolute_time() - last_local_position_time < 2000000 && hrt_absolute_time() > 2000000 && local_position.valid) { - current_status.condition_local_position_valid = true; + status.condition_local_position_valid = true; } else { - current_status.condition_local_position_valid = false; + status.condition_local_position_valid = false; } /* Check for valid airspeed/differential pressure measurements */ if (hrt_absolute_time() - last_diff_pres_time < 2000000 && hrt_absolute_time() > 2000000) { - current_status.condition_airspeed_valid = true; + status.condition_airspeed_valid = true; } else { - current_status.condition_airspeed_valid = false; + status.condition_airspeed_valid = false; } /* @@ -1040,9 +985,9 @@ int commander_thread_main(int argc, char *argv[]) // } /* consolidate state change, flag as changed if required */ - if (global_pos_valid != current_status.condition_global_position_valid || - local_pos_valid != current_status.condition_local_position_valid || - airspeed_valid != current_status.condition_airspeed_valid) { + if (global_pos_valid != status.condition_global_position_valid || + local_pos_valid != status.condition_local_position_valid || + airspeed_valid != status.condition_airspeed_valid) { state_changed = true; } @@ -1059,7 +1004,7 @@ int commander_thread_main(int argc, char *argv[]) // if (!current_status.flag_valid_launch_position && // !vector_flight_mode_ok && current_status.flag_vector_flight_mode_ok && // !current_status.flag_system_armed) { - // first time a valid position, store it and emit it + // first time a valid position, store it and emit it // // XXX implement storage and publication of RTL position // current_status.flag_valid_launch_position = true; @@ -1068,6 +1013,7 @@ int commander_thread_main(int argc, char *argv[]) // } orb_check(gps_sub, &new_data); + if (new_data) { @@ -1093,11 +1039,11 @@ int commander_thread_main(int argc, char *argv[]) */ if (gps_position.fix_type == GPS_FIX_TYPE_3D - && (hdop_m < hdop_threshold_m) - && (vdop_m < vdop_threshold_m) // XXX note that vdop is 0 for mtk - && !home_position_set - && (hrt_absolute_time() - gps_position.timestamp_position < 2000000) - && !armed.armed) { + && (hdop_m < hdop_threshold_m) + && (vdop_m < vdop_threshold_m) // XXX note that vdop is 0 for mtk + && !home_position_set + && (hrt_absolute_time() - gps_position.timestamp_position < 2000000) + && !armed.armed) { warnx("setting home position"); /* copy position data to uORB home message, store it locally as well */ @@ -1114,6 +1060,7 @@ int commander_thread_main(int argc, char *argv[]) /* announce new home position */ if (home_pub > 0) { orb_publish(ORB_ID(home_position), home_pub, &home); + } else { home_pub = orb_advertise(ORB_ID(home_position), &home); } @@ -1125,326 +1072,136 @@ int commander_thread_main(int argc, char *argv[]) } /* ignore RC signals if in offboard control mode */ - if (!current_status.offboard_control_signal_found_once && sp_man.timestamp != 0) { - /* Start RC state check */ - + if (!status.offboard_control_signal_found_once && sp_man.timestamp != 0) { + /* start RC state check */ if ((hrt_absolute_time() - sp_man.timestamp) < 100000) { - - /* - * Check if manual control modes have to be switched - */ - if (!isfinite(sp_man.mode_switch)) { - - warnx("mode sw not finite"); - /* no valid stick position, go to default */ - current_status.mode_switch = MODE_SWITCH_MANUAL; - - } else if (sp_man.mode_switch < -STICK_ON_OFF_LIMIT) { - - /* bottom stick position, go to manual mode */ - current_status.mode_switch = MODE_SWITCH_MANUAL; - - } else if (sp_man.mode_switch > STICK_ON_OFF_LIMIT) { - - /* top stick position, set auto/mission for all vehicle types */ - current_status.mode_switch = MODE_SWITCH_AUTO; - - } else { - - /* center stick position, set seatbelt/simple control */ - current_status.mode_switch = MODE_SWITCH_ASSISTED; - } - - // warnx("man ctrl mode: %d\n", (int)current_status.manual_control_mode); - - /* - * Check if land/RTL is requested - */ - if (!isfinite(sp_man.return_switch)) { - - /* this switch is not properly mapped, set default */ - current_status.return_switch = RETURN_SWITCH_NONE; - - } else if (sp_man.return_switch > STICK_ON_OFF_LIMIT) { - - /* top stick position */ - current_status.return_switch = RETURN_SWITCH_RETURN; - - } else { - /* center or bottom stick position, set default */ - current_status.return_switch = RETURN_SWITCH_NONE; - } - - /* check assisted switch */ - if (!isfinite(sp_man.assisted_switch)) { - - /* this switch is not properly mapped, set default */ - current_status.assisted_switch = ASSISTED_SWITCH_SEATBELT; - - } else if (sp_man.assisted_switch > STICK_ON_OFF_LIMIT) { - - /* top stick position */ - current_status.assisted_switch = ASSISTED_SWITCH_SIMPLE; - - } else { - /* center or bottom stick position, set default */ - current_status.assisted_switch = ASSISTED_SWITCH_SEATBELT; - } - - /* check mission switch */ - if (!isfinite(sp_man.mission_switch)) { - - /* this switch is not properly mapped, set default */ - current_status.mission_switch = MISSION_SWITCH_NONE; - - } else if (sp_man.mission_switch > STICK_ON_OFF_LIMIT) { - - /* top switch position */ - current_status.mission_switch = MISSION_SWITCH_MISSION; - - } else if (sp_man.mission_switch < -STICK_ON_OFF_LIMIT) { - - /* bottom switch position */ - current_status.mission_switch = MISSION_SWITCH_NONE; - - } else { - - /* center switch position, set default */ - current_status.mission_switch = MISSION_SWITCH_NONE; // XXX default? - } - - /* Now it's time to handle the stick inputs */ - - switch (current_status.arming_state) { - - /* evaluate the navigation state when disarmed */ - case ARMING_STATE_STANDBY: - - /* just manual, XXX this might depend on the return switch */ - if (current_status.mode_switch == MODE_SWITCH_MANUAL) { - - if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_MANUAL_STANDBY, control_mode_pub, &control_mode, mavlink_fd) != OK) { - // These is not supposed to happen - warnx("ERROR: Navigation state MANUAL_STANDBY rejected"); - } - - /* Try assisted or fallback to manual */ - } else if (current_status.mode_switch == MODE_SWITCH_ASSISTED) { - - if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_ASSISTED_STANDBY, control_mode_pub, &control_mode, mavlink_fd) != OK) { - // fallback to MANUAL_STANDBY - if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_MANUAL_STANDBY, control_mode_pub, &control_mode, mavlink_fd) != OK) { - // These is not supposed to happen - warnx("ERROR: Navigation state MANUAL_STANDBY rejected"); - } - } - - /* Try auto or fallback to seatbelt or even manual */ - } else if (current_status.mode_switch == MODE_SWITCH_AUTO) { - - if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_AUTO_STANDBY, control_mode_pub, &control_mode, mavlink_fd) != OK) { - // first fallback to SEATBELT_STANDY - if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_ASSISTED_STANDBY, control_mode_pub, &control_mode, mavlink_fd) != OK) { - // or fallback to MANUAL_STANDBY - if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_MANUAL_STANDBY, control_mode_pub, &control_mode, mavlink_fd) != OK) { - // These is not supposed to happen - warnx("ERROR: Navigation state MANUAL_STANDBY rejected"); - } - } - } - } - - break; - - /* evaluate the navigation state when armed */ - case ARMING_STATE_ARMED: - - if (current_status.mode_switch == MODE_SWITCH_MANUAL) { - /* MANUAL */ - if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_MANUAL, control_mode_pub, &control_mode, mavlink_fd) != OK) { - // These is not supposed to happen - warnx("ERROR: Navigation state MANUAL rejected"); - } - - } else if (current_status.mode_switch == MODE_SWITCH_ASSISTED) { - /* ASSISTED */ - if (current_status.return_switch == RETURN_SWITCH_RETURN) { - /* ASSISTED_DESCENT */ - if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_ASSISTED_DESCENT, control_mode_pub, &control_mode, mavlink_fd) != OK) { - // fallback to MANUAL - if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_MANUAL, control_mode_pub, &control_mode, mavlink_fd) != OK) { - // These is not supposed to happen - warnx("ERROR: Navigation state MANUAL rejected"); - } - } - - } else { - if (current_status.assisted_switch == ASSISTED_SWITCH_SIMPLE) { - /* ASSISTED_SIMPLE */ - if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_ASSISTED_SIMPLE, control_mode_pub, &control_mode, mavlink_fd) != OK) { - // fallback to ASSISTED_SEATBELT - if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_ASSISTED_SEATBELT, control_mode_pub, &control_mode, mavlink_fd) != OK) { - // fallback to MANUAL - if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_MANUAL, control_mode_pub, &control_mode, mavlink_fd) != OK) { - // These is not supposed to happen - warnx("ERROR: Navigation state MANUAL rejected"); - } - } - } - } else { - /* ASSISTED_SEATBELT */ - if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_ASSISTED_SEATBELT, control_mode_pub, &control_mode, mavlink_fd) != OK) { - // fallback to MANUAL - if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_MANUAL, control_mode_pub, &control_mode, mavlink_fd) != OK) { - // These is not supposed to happen - warnx("ERROR: Navigation state MANUAL rejected"); - } - } - } - } - - } else if (current_status.mode_switch == MODE_SWITCH_AUTO) { - /* AUTO */ - if (current_status.return_switch == RETURN_SWITCH_RETURN) { - /* AUTO_RTL */ - if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_AUTO_RTL, control_mode_pub, &control_mode, mavlink_fd) != OK) { - // fallback to ASSISTED_DESCENT - if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_ASSISTED_DESCENT, control_mode_pub, &control_mode, mavlink_fd) != OK) { - // fallback to MANUAL - if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_MANUAL, control_mode_pub, &control_mode, mavlink_fd) != OK) { - // These is not supposed to happen - warnx("ERROR: Navigation state MANUAL rejected"); - } - } - } - } else { - if (current_status.mission_switch == MISSION_SWITCH_MISSION) { - /* AUTO_MISSION */ - if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_AUTO_MISSION, control_mode_pub, &control_mode, mavlink_fd) != OK) { - // fallback to ASSISTED_SEATBELT - if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_ASSISTED_SEATBELT, control_mode_pub, &control_mode, mavlink_fd) != OK) { - // fallback to MANUAL - if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_MANUAL, control_mode_pub, &control_mode, mavlink_fd) != OK) { - // These is not supposed to happen - warnx("ERROR: Navigation state MANUAL rejected"); - } - } - } - } else { - // TODO check this - if (current_status.navigation_state == NAVIGATION_STATE_AUTO_STANDBY - || current_status.navigation_state == NAVIGATION_STATE_AUTO_LAND) { - /* AUTO_READY */ - if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_AUTO_READY, control_mode_pub, &control_mode, mavlink_fd) != OK) { - /* AUTO_READY */ - if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_ASSISTED_SEATBELT, control_mode_pub, &control_mode, mavlink_fd) != OK) { - // fallback to MANUAL - if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_MANUAL, control_mode_pub, &control_mode, mavlink_fd) != OK) { - // These is not supposed to happen - warnx("ERROR: Navigation state MANUAL rejected"); - } - } - } - } else { - /* AUTO_LOITER */ - if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_AUTO_LOITER, control_mode_pub, &control_mode, mavlink_fd) != OK) { - // fallback to ASSISTED_SEATBELT - if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_ASSISTED_SEATBELT, control_mode_pub, &control_mode, mavlink_fd) != OK) { - // fallback to MANUAL - if (navigation_state_transition(status_pub, ¤t_status, NAVIGATION_STATE_MANUAL, control_mode_pub, &control_mode, mavlink_fd) != OK) { - // These is not supposed to happen - warnx("ERROR: Navigation state MANUAL rejected"); - } - } - } - } - } - } - } - break; - - // XXX we might be missing something that triggers a transition from RTL to LAND - - case ARMING_STATE_ARMED_ERROR: - - // XXX work out fail-safe scenarios - break; - - case ARMING_STATE_STANDBY_ERROR: - - // XXX work out fail-safe scenarios - break; - - case ARMING_STATE_REBOOT: - - // XXX I don't think we should end up here - break; - - case ARMING_STATE_IN_AIR_RESTORE: - - // XXX not sure what to do here - break; - default: - break; - } /* handle the case where RC signal was regained */ - if (!current_status.rc_signal_found_once) { - current_status.rc_signal_found_once = true; - mavlink_log_critical(mavlink_fd, "DETECTED RC SIGNAL FIRST TIME."); + if (!status.rc_signal_found_once) { + status.rc_signal_found_once = true; + mavlink_log_critical(mavlink_fd, "[cmd] detected RC signal first time"); } else { - if (current_status.rc_signal_lost) { + if (status.rc_signal_lost) { mavlink_log_critical(mavlink_fd, "[cmd] RECOVERY - RC SIGNAL GAINED!"); } } - /* Check if left stick is in lower left position and we're in manual mode --> switch to standby state. - * Do this only for multirotors, not for fixed wing aircraft. - * TODO allow disarm when landed - */ - if (sp_man.yaw < -STICK_ON_OFF_LIMIT && sp_man.throttle < STICK_THRUST_RANGE * 0.1f && - control_mode.flag_control_manual_enabled && - ((current_status.system_type == VEHICLE_TYPE_QUADROTOR) || - (current_status.system_type == VEHICLE_TYPE_HEXAROTOR) || - (current_status.system_type == VEHICLE_TYPE_OCTOROTOR))) { - if (stick_off_counter > STICK_ON_OFF_COUNTER_LIMIT) { - arming_state_transition(status_pub, ¤t_status, ARMING_STATE_STANDBY, armed_pub, &armed, mavlink_fd); - tune_positive(); - stick_off_counter = 0; + status.rc_signal_cutting_off = false; + status.rc_signal_lost = false; + status.rc_signal_lost_interval = 0; + + transition_result_t res; // store all transitions results here + + /* arm/disarm by RC */ + bool arming_state_changed; + + res = TRANSITION_NOT_CHANGED; + + /* check if left stick is in lower left position and we are in MANUAL or AUTO mode -> disarm + * do it only for rotary wings */ + if (status.is_rotary_wing && + (status.arming_state == ARMING_STATE_ARMED || status.arming_state == ARMING_STATE_ARMED_ERROR) && + (status.main_state == MAIN_STATE_MANUAL || status.navigation_state == NAVIGATION_STATE_AUTO_READY)) { + if (sp_man.yaw < -STICK_ON_OFF_LIMIT && sp_man.throttle < STICK_THRUST_RANGE * 0.1f) { + if (stick_off_counter > STICK_ON_OFF_COUNTER_LIMIT) { + /* disarm to STANDBY if ARMED or to STANDBY_ERROR if ARMED_ERROR */ + arming_state_t new_arming_state = (status.arming_state == ARMING_STATE_ARMED ? ARMING_STATE_STANDBY : ARMING_STATE_STANDBY_ERROR); + res = arming_state_transition(&status, new_arming_state, &armed, mavlink_fd); + + if (res == TRANSITION_CHANGED) + stick_off_counter = 0; + + } else { + stick_off_counter++; + stick_on_counter = 0; + } } else { - stick_off_counter++; - stick_on_counter = 0; - } - } - - /* check if left stick is in lower right position and we're in manual mode --> arm */ - if (sp_man.yaw > STICK_ON_OFF_LIMIT && sp_man.throttle < STICK_THRUST_RANGE * 0.1f) { - if (stick_on_counter > STICK_ON_OFF_COUNTER_LIMIT) { - arming_state_transition(status_pub, ¤t_status, ARMING_STATE_ARMED, armed_pub, &armed, mavlink_fd); - stick_on_counter = 0; - tune_positive(); - - } else { - stick_on_counter++; stick_off_counter = 0; } } - current_status.rc_signal_cutting_off = false; - current_status.rc_signal_lost = false; - current_status.rc_signal_lost_interval = 0; + /* check if left stick is in lower right position and we're in manual mode -> arm */ + if (status.arming_state == ARMING_STATE_STANDBY && + status.main_state == MAIN_STATE_MANUAL) { + if (sp_man.yaw > STICK_ON_OFF_LIMIT && sp_man.throttle < STICK_THRUST_RANGE * 0.1f) { + if (stick_on_counter > STICK_ON_OFF_COUNTER_LIMIT) { + res = arming_state_transition(&status, ARMING_STATE_ARMED, &armed, mavlink_fd); + + if (res == TRANSITION_CHANGED) + stick_on_counter = 0; + + } else { + stick_on_counter++; + stick_off_counter = 0; + } + + } else { + stick_on_counter = 0; + } + } + + if (res == TRANSITION_CHANGED) { + if (status.arming_state == ARMING_STATE_ARMED) { + mavlink_log_info(mavlink_fd, "[cmd] ARMED by RC"); + + } else { + mavlink_log_info(mavlink_fd, "[cmd] DISARMED by RC"); + } + + tune_positive(); + orb_publish(ORB_ID(actuator_armed), armed_pub, &armed); + } + + /* fill current_status according to mode switches */ + check_mode_switches(&sp_man, &status); + + /* evaluate the main state machine */ + res = check_main_state_machine(&status); + + /* we should not get DENIED here */ + if (res == TRANSITION_DENIED) { + mavlink_log_critical(mavlink_fd, "[cmd] ERROR: main denied: arm %d main %d mode_sw %d", status.arming_state, status.main_state, status.mode_switch); + } + + bool main_state_changed = (res == TRANSITION_CHANGED); + + /* evaluate the navigation state machine */ + res = check_navigation_state_machine(&status, &control_mode); + + /* we should not get DENIED here */ + if (res == TRANSITION_DENIED) { + mavlink_log_critical(mavlink_fd, "[cmd] ERROR: nav denied: arm %d main %d nav %d", status.arming_state, status.main_state, status.navigation_state); + } + + bool navigation_state_changed = (res == TRANSITION_CHANGED); + + if (arming_state_changed || main_state_changed || navigation_state_changed) { + /* publish new vehicle status */ + status.counter++; + status.timestamp = hrt_absolute_time(); + orb_publish(ORB_ID(vehicle_status), status_pub, &status); + mavlink_log_info(mavlink_fd, "[cmd] state: arm %s, main %d, nav %d", status.arming_state, status.main_state, status.navigation_state); + } + + if (navigation_state_changed) { + /* publish new navigation state */ + control_mode.counter++; + control_mode.timestamp = hrt_absolute_time(); + orb_publish(ORB_ID(vehicle_control_mode), control_mode_pub, &control_mode); + } } else { /* print error message for first RC glitch and then every 5 s / 5000 ms) */ - if (!current_status.rc_signal_cutting_off || ((hrt_absolute_time() - last_print_time) > 5000000)) { + if (!status.rc_signal_cutting_off || ((hrt_absolute_time() - last_print_time) > 5000000)) { /* only complain if the offboard control is NOT active */ - current_status.rc_signal_cutting_off = true; + status.rc_signal_cutting_off = true; mavlink_log_critical(mavlink_fd, "CRITICAL - NO REMOTE SIGNAL!"); - if (!current_status.rc_signal_cutting_off) { + if (!status.rc_signal_cutting_off) { printf("Reason: not rc_signal_cutting_off\n"); + } else { printf("last print time: %llu\n", last_print_time); } @@ -1453,12 +1210,12 @@ int commander_thread_main(int argc, char *argv[]) } /* flag as lost and update interval since when the signal was lost (to initiate RTL after some time) */ - current_status.rc_signal_lost_interval = hrt_absolute_time() - sp_man.timestamp; + status.rc_signal_lost_interval = hrt_absolute_time() - sp_man.timestamp; /* if the RC signal is gone for a full second, consider it lost */ - if (current_status.rc_signal_lost_interval > 1000000) { - current_status.rc_signal_lost = true; - current_status.failsave_lowlevel = true; + if (status.rc_signal_lost_interval > 1000000) { + status.rc_signal_lost = true; + status.failsave_lowlevel = true; state_changed = true; } @@ -1477,7 +1234,7 @@ int commander_thread_main(int argc, char *argv[]) /* State machine update for offboard control */ - if (!current_status.rc_signal_found_once && sp_offboard.timestamp != 0) { + if (!status.rc_signal_found_once && sp_offboard.timestamp != 0) { if ((hrt_absolute_time() - sp_offboard.timestamp) < 5000000) { // /* decide about attitude control flag, enable in att/pos/vel */ @@ -1518,42 +1275,44 @@ int commander_thread_main(int argc, char *argv[]) // } // } - current_status.offboard_control_signal_weak = false; - current_status.offboard_control_signal_lost = false; - current_status.offboard_control_signal_lost_interval = 0; + status.offboard_control_signal_weak = false; + status.offboard_control_signal_lost = false; + status.offboard_control_signal_lost_interval = 0; // XXX check if this is correct /* arm / disarm on request */ if (sp_offboard.armed && !armed.armed) { - arming_state_transition(status_pub, ¤t_status, ARMING_STATE_ARMED, armed_pub, &armed, mavlink_fd); + arming_state_transition(&status, ARMING_STATE_ARMED, &armed, mavlink_fd); + // TODO publish state } else if (!sp_offboard.armed && armed.armed) { - arming_state_transition(status_pub, ¤t_status, ARMING_STATE_STANDBY, armed_pub, &armed, mavlink_fd); + arming_state_transition(&status, ARMING_STATE_STANDBY, &armed, mavlink_fd); + // TODO publish state } } else { /* print error message for first RC glitch and then every 5 s / 5000 ms) */ - if (!current_status.offboard_control_signal_weak || ((hrt_absolute_time() - last_print_time) > 5000000)) { - current_status.offboard_control_signal_weak = true; + if (!status.offboard_control_signal_weak || ((hrt_absolute_time() - last_print_time) > 5000000)) { + status.offboard_control_signal_weak = true; mavlink_log_critical(mavlink_fd, "CRIT:NO OFFBOARD CONTROL!"); last_print_time = hrt_absolute_time(); } /* flag as lost and update interval since when the signal was lost (to initiate RTL after some time) */ - current_status.offboard_control_signal_lost_interval = hrt_absolute_time() - sp_offboard.timestamp; + status.offboard_control_signal_lost_interval = hrt_absolute_time() - sp_offboard.timestamp; /* if the signal is gone for 0.1 seconds, consider it lost */ - if (current_status.offboard_control_signal_lost_interval > 100000) { - current_status.offboard_control_signal_lost = true; - current_status.failsave_lowlevel_start_time = hrt_absolute_time(); + if (status.offboard_control_signal_lost_interval > 100000) { + status.offboard_control_signal_lost = true; + status.failsave_lowlevel_start_time = hrt_absolute_time(); tune_positive(); /* kill motors after timeout */ - if (hrt_absolute_time() - current_status.failsave_lowlevel_start_time > failsafe_lowlevel_timeout_ms * 1000) { - current_status.failsave_lowlevel = true; + if (hrt_absolute_time() - status.failsave_lowlevel_start_time > failsafe_lowlevel_timeout_ms * 1000) { + status.failsave_lowlevel = true; state_changed = true; } } @@ -1563,8 +1322,8 @@ int commander_thread_main(int argc, char *argv[]) - current_status.counter++; - current_status.timestamp = hrt_absolute_time(); + status.counter++; + status.timestamp = hrt_absolute_time(); // XXX this is missing @@ -1580,36 +1339,39 @@ int commander_thread_main(int argc, char *argv[]) /* publish at least with 1 Hz */ if (counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0 || state_changed) { - orb_publish(ORB_ID(vehicle_status), status_pub, ¤t_status); + orb_publish(ORB_ID(vehicle_status), status_pub, &status); state_changed = false; } /* Store old modes to detect and act on state transitions */ - voltage_previous = current_status.battery_voltage; + voltage_previous = status.battery_voltage; /* play tone according to evaluation result */ /* check if we recently armed */ - if (!arm_tune_played && armed.armed && ( !safety.safety_switch_available || (safety.safety_off && safety.safety_switch_available))) { + if (!arm_tune_played && armed.armed && (!safety.safety_switch_available || (safety.safety_off && safety.safety_switch_available))) { if (tune_arm() == OK) arm_tune_played = true; - /* Trigger audio event for low battery */ - } else if (current_status.battery_remaining < 0.1f && current_status.condition_battery_voltage_valid) { + /* Trigger audio event for low battery */ + + } else if (status.battery_remaining < 0.1f && status.condition_battery_voltage_valid) { if (tune_critical_bat() == OK) battery_tune_played = true; - } else if (current_status.battery_remaining < 0.2f && current_status.condition_battery_voltage_valid) { + + } else if (status.battery_remaining < 0.2f && status.condition_battery_voltage_valid) { if (tune_low_bat() == OK) battery_tune_played = true; - } else if(battery_tune_played) { + + } else if (battery_tune_played) { tune_stop(); battery_tune_played = false; } /* reset arm_tune_played when disarmed */ - if (!(armed.armed && ( !safety.safety_switch_available || (safety.safety_off && safety.safety_switch_available)))) { + if (!(armed.armed && (!safety.safety_switch_available || (safety.safety_off && safety.safety_switch_available)))) { arm_tune_played = false; } @@ -1647,6 +1409,172 @@ int commander_thread_main(int argc, char *argv[]) return 0; } +void +check_mode_switches(struct manual_control_setpoint_s *sp_man, struct vehicle_status_s *current_status) +{ + /* main mode switch */ + if (!isfinite(sp_man->mode_switch)) { + warnx("mode sw not finite"); + current_status->mode_switch = MODE_SWITCH_MANUAL; + + } else if (sp_man->mode_switch > STICK_ON_OFF_LIMIT) { + current_status->mode_switch = MODE_SWITCH_AUTO; + + } else { + current_status->mode_switch = MODE_SWITCH_MANUAL; + } + + /* land switch */ + if (!isfinite(sp_man->return_switch)) { + current_status->return_switch = RETURN_SWITCH_NONE; + + } else if (sp_man->return_switch > STICK_ON_OFF_LIMIT) { + current_status->return_switch = RETURN_SWITCH_RETURN; + + } else { + current_status->return_switch = RETURN_SWITCH_NONE; + } + + /* assisted switch */ + if (!isfinite(sp_man->assisted_switch)) { + current_status->assisted_switch = ASSISTED_SWITCH_SEATBELT; + + } else if (sp_man->assisted_switch > STICK_ON_OFF_LIMIT) { + current_status->assisted_switch = ASSISTED_SWITCH_EASY; + + } else { + current_status->assisted_switch = ASSISTED_SWITCH_SEATBELT; + } + + /* mission switch */ + if (!isfinite(sp_man->mission_switch)) { + current_status->mission_switch = MISSION_SWITCH_MISSION; + + } else if (sp_man->mission_switch > STICK_ON_OFF_LIMIT) { + current_status->mission_switch = MISSION_SWITCH_NONE; + + } else { + current_status->mission_switch = MISSION_SWITCH_MISSION; + } +} + +transition_result_t +check_main_state_machine(struct vehicle_status_s *current_status) +{ + /* evaluate the main state machine */ + transition_result_t res = TRANSITION_DENIED; + + switch (current_status->mode_switch) { + case MODE_SWITCH_MANUAL: + res = main_state_transition(current_status, MAIN_STATE_MANUAL, mavlink_fd); + // TRANSITION_DENIED is not possible here + break; + + case MODE_SWITCH_ASSISTED: + if (current_status->assisted_switch == ASSISTED_SWITCH_EASY) { + res = main_state_transition(current_status, MAIN_STATE_EASY, mavlink_fd); + + if (res != TRANSITION_DENIED) + break; // changed successfully or already in this state + + // else fallback to SEATBELT + } + + res = main_state_transition(current_status, MAIN_STATE_SEATBELT, mavlink_fd); + + if (res != TRANSITION_DENIED) + break; // changed successfully or already in this mode + + // else fallback to MANUAL + res = main_state_transition(current_status, MAIN_STATE_MANUAL, mavlink_fd); + // TRANSITION_DENIED is not possible here + break; + + case MODE_SWITCH_AUTO: + res = main_state_transition(current_status, MAIN_STATE_AUTO, mavlink_fd); + + if (res != TRANSITION_DENIED) + break; // changed successfully or already in this state + + // else fallback to SEATBELT (EASY likely will not work too) + res = main_state_transition(current_status, MAIN_STATE_SEATBELT, mavlink_fd); + + if (res != TRANSITION_DENIED) + break; // changed successfully or already in this state + + // else fallback to MANUAL + res = main_state_transition(current_status, MAIN_STATE_MANUAL, mavlink_fd); + // TRANSITION_DENIED is not possible here + break; + + default: + break; + } + + return res; +} + +transition_result_t +check_navigation_state_machine(struct vehicle_status_s *current_status, struct vehicle_control_mode_s *control_mode) +{ + transition_result_t res = TRANSITION_DENIED; + + if (current_status->arming_state == ARMING_STATE_ARMED || current_status->arming_state == ARMING_STATE_ARMED_ERROR) { + /* ARMED */ + switch (current_status->main_state) { + case MAIN_STATE_MANUAL: + res = navigation_state_transition(current_status, current_status->is_rotary_wing ? NAVIGATION_STATE_STABILIZE : NAVIGATION_STATE_DIRECT, control_mode, mavlink_fd); + break; + + case MAIN_STATE_SEATBELT: + res = navigation_state_transition(current_status, NAVIGATION_STATE_ALTHOLD, control_mode, mavlink_fd); + break; + + case MAIN_STATE_EASY: + res = navigation_state_transition(current_status, NAVIGATION_STATE_VECTOR, control_mode, mavlink_fd); + break; + + case MAIN_STATE_AUTO: + if (current_status->navigation_state != NAVIGATION_STATE_AUTO_TAKEOFF) { + /* don't act while taking off */ + if (current_status->condition_landed) { + /* if landed: transitions only to AUTO_READY are allowed */ + res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_READY, control_mode, mavlink_fd); + // TRANSITION_DENIED is not possible here + break; + + } else { + /* if not landed: act depending on switches */ + if (current_status->return_switch == RETURN_SWITCH_RETURN) { + /* RTL */ + res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_RTL, control_mode, mavlink_fd); + + } else { + if (current_status->mission_switch == MISSION_SWITCH_MISSION) { + /* MISSION */ + res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_MISSION, control_mode, mavlink_fd); + + } else { + /* LOITER */ + res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_LOITER, control_mode, mavlink_fd); + } + } + } + } + + break; + + default: + break; + } + + } else { + /* DISARMED */ + res = navigation_state_transition(current_status, NAVIGATION_STATE_STANDBY, control_mode, mavlink_fd); + } + + return res; +} void *commander_low_prio_loop(void *arg) { @@ -1657,72 +1585,76 @@ void *commander_low_prio_loop(void *arg) switch (low_prio_task) { - case LOW_PRIO_TASK_PARAM_LOAD: + case LOW_PRIO_TASK_PARAM_LOAD: - if (0 == param_load_default()) { - mavlink_log_info(mavlink_fd, "Param load success"); - } else { - mavlink_log_critical(mavlink_fd, "Param load ERROR"); - tune_error(); - } - low_prio_task = LOW_PRIO_TASK_NONE; - break; + if (0 == param_load_default()) { + mavlink_log_info(mavlink_fd, "Param load success"); - case LOW_PRIO_TASK_PARAM_SAVE: + } else { + mavlink_log_critical(mavlink_fd, "Param load ERROR"); + tune_error(); + } - if (0 == param_save_default()) { - mavlink_log_info(mavlink_fd, "Param save success"); - } else { - mavlink_log_critical(mavlink_fd, "Param save ERROR"); - tune_error(); - } - low_prio_task = LOW_PRIO_TASK_NONE; - break; + low_prio_task = LOW_PRIO_TASK_NONE; + break; - case LOW_PRIO_TASK_GYRO_CALIBRATION: + case LOW_PRIO_TASK_PARAM_SAVE: - do_gyro_calibration(mavlink_fd); + if (0 == param_save_default()) { + mavlink_log_info(mavlink_fd, "Param save success"); - low_prio_task = LOW_PRIO_TASK_NONE; - break; + } else { + mavlink_log_critical(mavlink_fd, "Param save ERROR"); + tune_error(); + } - case LOW_PRIO_TASK_MAG_CALIBRATION: + low_prio_task = LOW_PRIO_TASK_NONE; + break; - do_mag_calibration(mavlink_fd); + case LOW_PRIO_TASK_GYRO_CALIBRATION: - low_prio_task = LOW_PRIO_TASK_NONE; - break; + do_gyro_calibration(mavlink_fd); - case LOW_PRIO_TASK_ALTITUDE_CALIBRATION: + low_prio_task = LOW_PRIO_TASK_NONE; + break; - // do_baro_calibration(mavlink_fd); + case LOW_PRIO_TASK_MAG_CALIBRATION: - case LOW_PRIO_TASK_RC_CALIBRATION: + do_mag_calibration(mavlink_fd); - // do_rc_calibration(mavlink_fd); + low_prio_task = LOW_PRIO_TASK_NONE; + break; - low_prio_task = LOW_PRIO_TASK_NONE; - break; + case LOW_PRIO_TASK_ALTITUDE_CALIBRATION: - case LOW_PRIO_TASK_ACCEL_CALIBRATION: + // do_baro_calibration(mavlink_fd); - do_accel_calibration(mavlink_fd); + case LOW_PRIO_TASK_RC_CALIBRATION: - low_prio_task = LOW_PRIO_TASK_NONE; - break; + // do_rc_calibration(mavlink_fd); - case LOW_PRIO_TASK_AIRSPEED_CALIBRATION: + low_prio_task = LOW_PRIO_TASK_NONE; + break; - do_airspeed_calibration(mavlink_fd); + case LOW_PRIO_TASK_ACCEL_CALIBRATION: - low_prio_task = LOW_PRIO_TASK_NONE; - break; + do_accel_calibration(mavlink_fd); - case LOW_PRIO_TASK_NONE: - default: - /* slow down to 10Hz */ - usleep(100000); - break; + low_prio_task = LOW_PRIO_TASK_NONE; + break; + + case LOW_PRIO_TASK_AIRSPEED_CALIBRATION: + + do_airspeed_calibration(mavlink_fd); + + low_prio_task = LOW_PRIO_TASK_NONE; + break; + + case LOW_PRIO_TASK_NONE: + default: + /* slow down to 10Hz */ + usleep(100000); + break; } } diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 4a7aa66b70..043ccfd0c9 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -62,102 +62,110 @@ #endif static const int ERROR = -1; -int arming_state_transition(int status_pub, struct vehicle_status_s *current_state, arming_state_t new_arming_state, int armed_pub, struct actuator_armed_s *armed, const int mavlink_fd) { - - - int ret = ERROR; +transition_result_t +arming_state_transition(struct vehicle_status_s *status, arming_state_t new_arming_state, struct actuator_armed_s *armed, const int mavlink_fd) +{ + transition_result_t ret = TRANSITION_DENIED; /* only check transition if the new state is actually different from the current one */ - if (new_arming_state == current_state->arming_state) { - ret = OK; + if (new_arming_state == status->arming_state) { + ret = TRANSITION_NOT_CHANGED; + } else { switch (new_arming_state) { - case ARMING_STATE_INIT: + case ARMING_STATE_INIT: - /* allow going back from INIT for calibration */ - if (current_state->arming_state == ARMING_STATE_STANDBY) { - ret = OK; + /* allow going back from INIT for calibration */ + if (status->arming_state == ARMING_STATE_STANDBY) { + ret = TRANSITION_CHANGED; + armed->armed = false; + armed->ready_to_arm = false; + } + + break; + + case ARMING_STATE_STANDBY: + + /* allow coming from INIT and disarming from ARMED */ + if (status->arming_state == ARMING_STATE_INIT + || status->arming_state == ARMING_STATE_ARMED) { + + /* sensors need to be initialized for STANDBY state */ + if (status->condition_system_sensors_initialized) { + ret = TRANSITION_CHANGED; armed->armed = false; - armed->ready_to_arm = false; + armed->ready_to_arm = true; + + } else { + mavlink_log_critical(mavlink_fd, "rej. STANDBY state, sensors not initialized"); } - break; - case ARMING_STATE_STANDBY: + } - /* allow coming from INIT and disarming from ARMED */ - if (current_state->arming_state == ARMING_STATE_INIT - || current_state->arming_state == ARMING_STATE_ARMED) { + break; - /* sensors need to be initialized for STANDBY state */ - if (current_state->condition_system_sensors_initialized) { - ret = OK; - armed->armed = false; - armed->ready_to_arm = true; - } else { - mavlink_log_critical(mavlink_fd, "Rej. STANDBY state, sensors not initialized"); - } - } - break; - case ARMING_STATE_ARMED: + case ARMING_STATE_ARMED: - /* allow arming from STANDBY and IN-AIR-RESTORE */ - if (current_state->arming_state == ARMING_STATE_STANDBY - || current_state->arming_state == ARMING_STATE_IN_AIR_RESTORE) { + /* allow arming from STANDBY and IN-AIR-RESTORE */ + if (status->arming_state == ARMING_STATE_STANDBY + || status->arming_state == ARMING_STATE_IN_AIR_RESTORE) { + ret = TRANSITION_CHANGED; + armed->armed = true; + armed->ready_to_arm = false; + } - /* XXX conditions for arming? */ - ret = OK; - armed->armed = true; - } - break; - case ARMING_STATE_ARMED_ERROR: + break; - /* an armed error happens when ARMED obviously */ - if (current_state->arming_state == ARMING_STATE_ARMED) { - - /* XXX conditions for an error state? */ - ret = OK; - armed->armed = true; - } - break; - case ARMING_STATE_STANDBY_ERROR: - /* a disarmed error happens when in STANDBY or in INIT or after ARMED_ERROR */ - if (current_state->arming_state == ARMING_STATE_STANDBY - || current_state->arming_state == ARMING_STATE_INIT - || current_state->arming_state == ARMING_STATE_ARMED_ERROR) { - ret = OK; - armed->armed = false; - armed->ready_to_arm = false; - } - break; - case ARMING_STATE_REBOOT: + case ARMING_STATE_ARMED_ERROR: - /* an armed error happens when ARMED obviously */ - if (current_state->arming_state == ARMING_STATE_INIT - || current_state->arming_state == ARMING_STATE_STANDBY - || current_state->arming_state == ARMING_STATE_STANDBY_ERROR) { + /* an armed error happens when ARMED obviously */ + if (status->arming_state == ARMING_STATE_ARMED) { + ret = TRANSITION_CHANGED; + armed->armed = true; + armed->ready_to_arm = false; + } - ret = OK; - armed->armed = false; - armed->ready_to_arm = false; + break; - } - break; - case ARMING_STATE_IN_AIR_RESTORE: + case ARMING_STATE_STANDBY_ERROR: - /* XXX implement */ - break; - default: - break; + /* a disarmed error happens when in STANDBY or in INIT or after ARMED_ERROR */ + if (status->arming_state == ARMING_STATE_STANDBY + || status->arming_state == ARMING_STATE_INIT + || status->arming_state == ARMING_STATE_ARMED_ERROR) { + ret = TRANSITION_CHANGED; + armed->armed = false; + armed->ready_to_arm = false; + } + + break; + + case ARMING_STATE_REBOOT: + + /* an armed error happens when ARMED obviously */ + if (status->arming_state == ARMING_STATE_INIT + || status->arming_state == ARMING_STATE_STANDBY + || status->arming_state == ARMING_STATE_STANDBY_ERROR) { + ret = TRANSITION_CHANGED; + armed->armed = false; + armed->ready_to_arm = false; + } + + break; + + case ARMING_STATE_IN_AIR_RESTORE: + + /* XXX implement */ + break; + + default: + break; } - if (ret == OK) { - current_state->arming_state = new_arming_state; - current_state->counter++; - current_state->timestamp = hrt_absolute_time(); - orb_publish(ORB_ID(vehicle_status), status_pub, current_state); - - armed->timestamp = hrt_absolute_time(); - orb_publish(ORB_ID(actuator_armed), armed_pub, armed); + if (ret == TRANSITION_CHANGED) { + hrt_abstime t = hrt_absolute_time(); + status->arming_state = new_arming_state; + armed->timestamp = t; } } @@ -165,400 +173,230 @@ int arming_state_transition(int status_pub, struct vehicle_status_s *current_sta } - -/* - * This functions does not evaluate any input flags but only checks if the transitions - * are valid. - */ -int navigation_state_transition(int status_pub, struct vehicle_status_s *current_state, navigation_state_t new_navigation_state, int control_mode_pub, struct vehicle_control_mode_s *control_mode, const int mavlink_fd) { - - int ret = ERROR; +transition_result_t +main_state_transition(struct vehicle_status_s *current_state, main_state_t new_main_state, const int mavlink_fd) +{ + transition_result_t ret = TRANSITION_DENIED; /* only check transition if the new state is actually different from the current one */ - if (new_navigation_state == current_state->navigation_state) { - ret = OK; + if (new_main_state == current_state->main_state) { + ret = TRANSITION_NOT_CHANGED; + } else { - switch (new_navigation_state) { - case NAVIGATION_STATE_INIT: + switch (new_main_state) { + case MAIN_STATE_MANUAL: + ret = TRANSITION_CHANGED; + break; - /* transitions back to INIT are possible for calibration */ - if (current_state->navigation_state == NAVIGATION_STATE_MANUAL_STANDBY - || current_state->navigation_state == NAVIGATION_STATE_ASSISTED_STANDBY - || current_state->navigation_state == NAVIGATION_STATE_AUTO_STANDBY) { + case MAIN_STATE_SEATBELT: - ret = OK; - control_mode->flag_control_rates_enabled = false; - control_mode->flag_control_attitude_enabled = false; - control_mode->flag_control_velocity_enabled = false; - control_mode->flag_control_position_enabled = false; - control_mode->flag_control_altitude_enabled = false; - control_mode->flag_control_manual_enabled = false; - } - break; + /* need position estimate */ + // TODO only need altitude estimate really + if (!current_state->condition_local_position_valid) { + mavlink_log_critical(mavlink_fd, "rej. SEATBELT: no position estimate"); + tune_negative(); - case NAVIGATION_STATE_MANUAL_STANDBY: + } else { + ret = TRANSITION_CHANGED; + } - /* transitions from INIT and other STANDBY states as well as MANUAL are possible */ - if (current_state->navigation_state == NAVIGATION_STATE_INIT - || current_state->navigation_state == NAVIGATION_STATE_ASSISTED_STANDBY - || current_state->navigation_state == NAVIGATION_STATE_AUTO_STANDBY - || current_state->navigation_state == NAVIGATION_STATE_MANUAL) { + break; - /* need to be disarmed first */ - if (current_state->arming_state != ARMING_STATE_STANDBY) { - mavlink_log_critical(mavlink_fd, "Rej. MANUAL_STANDBY: not disarmed"); - tune_negative(); - } else { - ret = OK; - control_mode->flag_control_rates_enabled = true; - control_mode->flag_control_attitude_enabled = true; - control_mode->flag_control_velocity_enabled = false; - control_mode->flag_control_position_enabled = false; - control_mode->flag_control_altitude_enabled = false; - control_mode->flag_control_manual_enabled = true; - } - } - break; + case MAIN_STATE_EASY: - case NAVIGATION_STATE_MANUAL: + /* need position estimate */ + if (!current_state->condition_local_position_valid) { + mavlink_log_critical(mavlink_fd, "rej. EASY: no position estimate"); + tune_negative(); - /* need to be armed first */ - if (current_state->arming_state != ARMING_STATE_ARMED) { - mavlink_log_critical(mavlink_fd, "Rej. MANUAL: not armed"); - tune_negative(); - } else { - ret = OK; - control_mode->flag_control_rates_enabled = true; - control_mode->flag_control_attitude_enabled = true; - control_mode->flag_control_velocity_enabled = false; - control_mode->flag_control_position_enabled = false; - control_mode->flag_control_altitude_enabled = false; - control_mode->flag_control_manual_enabled = true; - } - break; + } else { + ret = TRANSITION_CHANGED; + } - case NAVIGATION_STATE_ASSISTED_STANDBY: + break; - /* transitions from INIT and other STANDBY states as well as SEATBELT and SEATBELT_DESCENT are possible */ - if (current_state->navigation_state == NAVIGATION_STATE_INIT - || current_state->navigation_state == NAVIGATION_STATE_MANUAL_STANDBY - || current_state->navigation_state == NAVIGATION_STATE_AUTO_STANDBY - || current_state->navigation_state == NAVIGATION_STATE_ASSISTED_SEATBELT - || current_state->navigation_state == NAVIGATION_STATE_ASSISTED_SIMPLE - || current_state->navigation_state == NAVIGATION_STATE_ASSISTED_DESCENT) { + case MAIN_STATE_AUTO: - /* need to be disarmed and have a position estimate */ - if (current_state->arming_state != ARMING_STATE_STANDBY) { - mavlink_log_critical(mavlink_fd, "Rej. SEATBELT_STANDBY: not disarmed"); - tune_negative(); - } else if (!current_state->condition_local_position_valid) { - mavlink_log_critical(mavlink_fd, "Rej. SEATBELT_STANDBY: no position estimate"); - tune_negative(); - } else { - ret = OK; - control_mode->flag_control_rates_enabled = true; - control_mode->flag_control_attitude_enabled = true; - control_mode->flag_control_velocity_enabled = true; - control_mode->flag_control_position_enabled = false; - control_mode->flag_control_altitude_enabled = true; - control_mode->flag_control_manual_enabled = true; - } - } - break; + /* need position estimate */ + if (!current_state->condition_local_position_valid) { + mavlink_log_critical(mavlink_fd, "rej. AUTO: no position estimate"); + tune_negative(); - case NAVIGATION_STATE_ASSISTED_SEATBELT: + } else { + ret = TRANSITION_CHANGED; + } - /* transitions from all AUTO modes except AUTO_STANDBY and except MANUAL_STANDBY and INIT*/ - if (current_state->navigation_state == NAVIGATION_STATE_ASSISTED_STANDBY - || current_state->navigation_state == NAVIGATION_STATE_ASSISTED_SIMPLE - || current_state->navigation_state == NAVIGATION_STATE_ASSISTED_DESCENT - || current_state->navigation_state == NAVIGATION_STATE_MANUAL - || current_state->navigation_state == NAVIGATION_STATE_AUTO_LAND - || current_state->navigation_state == NAVIGATION_STATE_AUTO_LOITER - || current_state->navigation_state == NAVIGATION_STATE_AUTO_MISSION - || current_state->navigation_state == NAVIGATION_STATE_AUTO_READY - || current_state->navigation_state == NAVIGATION_STATE_AUTO_RTL - || current_state->navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF) { - - /* need to be armed and have a position estimate */ - if (current_state->arming_state != ARMING_STATE_ARMED) { - mavlink_log_critical(mavlink_fd, "Rej. SEATBELT: not armed"); - tune_negative(); - } else if (!current_state->condition_local_position_valid) { - mavlink_log_critical(mavlink_fd, "Rej. SEATBELT: no pos estimate"); - tune_negative(); - } else { - ret = OK; - control_mode->flag_control_rates_enabled = true; - control_mode->flag_control_attitude_enabled = true; - control_mode->flag_control_velocity_enabled = true; - control_mode->flag_control_position_enabled = false; - control_mode->flag_control_altitude_enabled = true; - control_mode->flag_control_manual_enabled = true; - } - } - break; - - case NAVIGATION_STATE_ASSISTED_SIMPLE: - - /* transitions from all AUTO modes except AUTO_STANDBY and except MANUAL_STANDBY and INIT*/ - if (current_state->navigation_state == NAVIGATION_STATE_ASSISTED_STANDBY - || current_state->navigation_state == NAVIGATION_STATE_ASSISTED_SEATBELT - || current_state->navigation_state == NAVIGATION_STATE_ASSISTED_DESCENT - || current_state->navigation_state == NAVIGATION_STATE_MANUAL - || current_state->navigation_state == NAVIGATION_STATE_AUTO_LAND - || current_state->navigation_state == NAVIGATION_STATE_AUTO_LOITER - || current_state->navigation_state == NAVIGATION_STATE_AUTO_MISSION - || current_state->navigation_state == NAVIGATION_STATE_AUTO_READY - || current_state->navigation_state == NAVIGATION_STATE_AUTO_RTL - || current_state->navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF) { - - /* need to be armed and have a position estimate */ - if (current_state->arming_state != ARMING_STATE_ARMED) { - mavlink_log_critical(mavlink_fd, "Rej. SEATBELT: not armed"); - tune_negative(); - } else if (!current_state->condition_local_position_valid) { - mavlink_log_critical(mavlink_fd, "Rej. SEATBELT: no pos estimate"); - tune_negative(); - } else { - ret = OK; - control_mode->flag_control_rates_enabled = true; - control_mode->flag_control_attitude_enabled = true; - control_mode->flag_control_velocity_enabled = true; - control_mode->flag_control_position_enabled = true; - control_mode->flag_control_altitude_enabled = true; - control_mode->flag_control_manual_enabled = true; - } - } - break; - - case NAVIGATION_STATE_ASSISTED_DESCENT: - - /* transitions from all AUTO modes except AUTO_STANDBY and except MANUAL_STANDBY and INIT and SEATBELT_STANDBY */ - if (current_state->navigation_state == NAVIGATION_STATE_ASSISTED_SEATBELT - || current_state->navigation_state == NAVIGATION_STATE_ASSISTED_SIMPLE - || current_state->navigation_state == NAVIGATION_STATE_MANUAL - || current_state->navigation_state == NAVIGATION_STATE_AUTO_LAND - || current_state->navigation_state == NAVIGATION_STATE_AUTO_LOITER - || current_state->navigation_state == NAVIGATION_STATE_AUTO_MISSION - || current_state->navigation_state == NAVIGATION_STATE_AUTO_READY - || current_state->navigation_state == NAVIGATION_STATE_AUTO_RTL - || current_state->navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF) { - - /* need to be armed and have a position estimate */ - if (current_state->arming_state != ARMING_STATE_ARMED) { - mavlink_log_critical(mavlink_fd, "Rej. SEATBELT_DESCENT: not armed"); - tune_negative(); - } else if (!current_state->condition_local_position_valid) { - mavlink_log_critical(mavlink_fd, "Rej. SEATBELT_DESCENT: no pos estimate"); - tune_negative(); - } else { - ret = OK; - control_mode->flag_control_rates_enabled = true; - control_mode->flag_control_attitude_enabled = true; - control_mode->flag_control_velocity_enabled = true; - control_mode->flag_control_position_enabled = false; - control_mode->flag_control_altitude_enabled = true; - control_mode->flag_control_manual_enabled = true; - } - } - break; - - case NAVIGATION_STATE_AUTO_STANDBY: - - /* transitions from INIT or from other STANDBY modes or from AUTO READY */ - if (current_state->navigation_state == NAVIGATION_STATE_INIT - || current_state->navigation_state == NAVIGATION_STATE_MANUAL_STANDBY - || current_state->navigation_state == NAVIGATION_STATE_ASSISTED_STANDBY - || current_state->navigation_state == NAVIGATION_STATE_AUTO_READY) { - - /* need to be disarmed and have a position and home lock */ - if (current_state->arming_state != ARMING_STATE_STANDBY) { - mavlink_log_critical(mavlink_fd, "Rej. AUTO_STANDBY: not disarmed"); - tune_negative(); - } else if (!current_state->condition_global_position_valid) { - mavlink_log_critical(mavlink_fd, "Rej. AUTO_STANDBY: no pos lock"); - tune_negative(); - } else if (!current_state->condition_home_position_valid) { - mavlink_log_critical(mavlink_fd, "Rej. AUTO_STANDBY: no home pos"); - tune_negative(); - } else { - ret = OK; - control_mode->flag_control_rates_enabled = true; - control_mode->flag_control_attitude_enabled = true; - control_mode->flag_control_velocity_enabled = true; - control_mode->flag_control_position_enabled = true; - control_mode->flag_control_altitude_enabled = true; - control_mode->flag_control_manual_enabled = false; - } - } - break; - - case NAVIGATION_STATE_AUTO_READY: - - /* transitions from AUTO_STANDBY or AUTO_LAND */ - if (current_state->navigation_state == NAVIGATION_STATE_AUTO_STANDBY - || current_state->navigation_state == NAVIGATION_STATE_AUTO_LAND) { - - // XXX flag test needed? - - /* need to be armed and have a position and home lock */ - if (current_state->arming_state != ARMING_STATE_ARMED) { - mavlink_log_critical(mavlink_fd, "Rej. AUTO_READY: not armed"); - tune_negative(); - } else { - ret = OK; - control_mode->flag_control_rates_enabled = true; - control_mode->flag_control_attitude_enabled = true; - control_mode->flag_control_velocity_enabled = true; - control_mode->flag_control_position_enabled = true; - control_mode->flag_control_altitude_enabled = true; - control_mode->flag_control_manual_enabled = false; - } - } - break; - - case NAVIGATION_STATE_AUTO_TAKEOFF: - - /* only transitions from AUTO_READY */ - if (current_state->navigation_state == NAVIGATION_STATE_AUTO_READY) { - - ret = OK; - control_mode->flag_control_rates_enabled = true; - control_mode->flag_control_attitude_enabled = true; - control_mode->flag_control_velocity_enabled = true; - control_mode->flag_control_position_enabled = true; - control_mode->flag_control_altitude_enabled = true; - control_mode->flag_control_manual_enabled = false; - } - break; - - case NAVIGATION_STATE_AUTO_LOITER: - - /* from everywhere flying except AUTO_LAND and SEATBELT_DESCENT */ - if (current_state->navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF - || current_state->navigation_state == NAVIGATION_STATE_AUTO_MISSION - || current_state->navigation_state == NAVIGATION_STATE_AUTO_RTL - || current_state->navigation_state == NAVIGATION_STATE_ASSISTED_SEATBELT - || current_state->navigation_state == NAVIGATION_STATE_ASSISTED_SIMPLE - || current_state->navigation_state == NAVIGATION_STATE_MANUAL) { - - /* need to have a position and home lock */ - if (!current_state->condition_global_position_valid) { - mavlink_log_critical(mavlink_fd, "Rej. AUTO_LOITER: no pos lock"); - tune_negative(); - } else if (!current_state->condition_home_position_valid) { - mavlink_log_critical(mavlink_fd, "Rej. AUTO_LOITER: no home pos"); - tune_negative(); - } else { - ret = OK; - control_mode->flag_control_rates_enabled = true; - control_mode->flag_control_attitude_enabled = true; - control_mode->flag_control_velocity_enabled = true; - control_mode->flag_control_position_enabled = true; - control_mode->flag_control_altitude_enabled = true; - control_mode->flag_control_manual_enabled = false; - } - } - break; - - case NAVIGATION_STATE_AUTO_MISSION: - - /* from everywhere flying except AUTO_LAND and SEATBELT_DESCENT */ - if (current_state->navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF - || current_state->navigation_state == NAVIGATION_STATE_AUTO_LOITER - || current_state->navigation_state == NAVIGATION_STATE_AUTO_RTL - || current_state->navigation_state == NAVIGATION_STATE_ASSISTED_SEATBELT - || current_state->navigation_state == NAVIGATION_STATE_ASSISTED_SIMPLE - || current_state->navigation_state == NAVIGATION_STATE_MANUAL) { - - /* need to have a mission ready */ - if (!current_state-> condition_auto_mission_available) { - mavlink_log_critical(mavlink_fd, "Rej. AUTO_MISSION: no mission available"); - tune_negative(); - } else { - ret = OK; - control_mode->flag_control_rates_enabled = true; - control_mode->flag_control_attitude_enabled = true; - control_mode->flag_control_velocity_enabled = true; - control_mode->flag_control_position_enabled = true; - control_mode->flag_control_altitude_enabled = true; - control_mode->flag_control_manual_enabled = false; - } - } - break; - - case NAVIGATION_STATE_AUTO_RTL: - - /* from everywhere flying except AUTO_LAND and SEATBELT_DESCENT */ - if (current_state->navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF - || current_state->navigation_state == NAVIGATION_STATE_AUTO_MISSION - || current_state->navigation_state == NAVIGATION_STATE_AUTO_LOITER - || current_state->navigation_state == NAVIGATION_STATE_ASSISTED_SEATBELT - || current_state->navigation_state == NAVIGATION_STATE_ASSISTED_SIMPLE - || current_state->navigation_state == NAVIGATION_STATE_MANUAL) { - - /* need to have a position and home lock */ - if (!current_state->condition_global_position_valid) { - mavlink_log_critical(mavlink_fd, "Rej. AUTO_RTL: no pos lock"); - tune_negative(); - } else if (!current_state->condition_home_position_valid) { - mavlink_log_critical(mavlink_fd, "Rej. AUTO_RTL: no home pos"); - tune_negative(); - } else { - ret = OK; - control_mode->flag_control_rates_enabled = true; - control_mode->flag_control_attitude_enabled = true; - control_mode->flag_control_velocity_enabled = true; - control_mode->flag_control_position_enabled = true; - control_mode->flag_control_altitude_enabled = true; - control_mode->flag_control_manual_enabled = false; - } - } - break; - - case NAVIGATION_STATE_AUTO_LAND: - /* after AUTO_RTL or when in AUTO_LOITER or AUTO_MISSION */ - if (current_state->navigation_state == NAVIGATION_STATE_AUTO_RTL - || current_state->navigation_state == NAVIGATION_STATE_AUTO_MISSION - || current_state->navigation_state == NAVIGATION_STATE_AUTO_LOITER) { - - /* need to have a position and home lock */ - if (!current_state->condition_global_position_valid) { - mavlink_log_critical(mavlink_fd, "Rej. AUTO_LAND: no pos lock"); - tune_negative(); - } else if (!current_state->condition_home_position_valid) { - mavlink_log_critical(mavlink_fd, "Rej. AUTO_LAND: no home pos"); - tune_negative(); - } else { - ret = OK; - control_mode->flag_control_rates_enabled = true; - control_mode->flag_control_attitude_enabled = true; - control_mode->flag_control_velocity_enabled = true; - control_mode->flag_control_position_enabled = true; - control_mode->flag_control_altitude_enabled = true; - control_mode->flag_control_manual_enabled = false; - } - } - break; - - default: - break; + break; } - if (ret == OK) { - current_state->navigation_state = new_navigation_state; - current_state->counter++; - current_state->timestamp = hrt_absolute_time(); - orb_publish(ORB_ID(vehicle_status), status_pub, current_state); - - control_mode->timestamp = hrt_absolute_time(); - orb_publish(ORB_ID(vehicle_control_mode), control_mode_pub, control_mode); + if (ret == TRANSITION_CHANGED) { + current_state->main_state = new_main_state; } } - + return ret; +} + +transition_result_t +navigation_state_transition(struct vehicle_status_s *current_status, navigation_state_t new_navigation_state, struct vehicle_control_mode_s *control_mode, const int mavlink_fd) +{ + transition_result_t ret = TRANSITION_DENIED; + + /* only check transition if the new state is actually different from the current one */ + if (new_navigation_state == current_status->navigation_state) { + ret = TRANSITION_NOT_CHANGED; + + } else { + + switch (new_navigation_state) { + case NAVIGATION_STATE_STANDBY: + ret = TRANSITION_CHANGED; + control_mode->flag_control_rates_enabled = false; + control_mode->flag_control_attitude_enabled = false; + control_mode->flag_control_velocity_enabled = false; + control_mode->flag_control_position_enabled = false; + control_mode->flag_control_altitude_enabled = false; + control_mode->flag_control_manual_enabled = false; + break; + + case NAVIGATION_STATE_DIRECT: + ret = TRANSITION_CHANGED; + control_mode->flag_control_rates_enabled = true; + control_mode->flag_control_attitude_enabled = false; + control_mode->flag_control_velocity_enabled = false; + control_mode->flag_control_position_enabled = false; + control_mode->flag_control_altitude_enabled = false; + control_mode->flag_control_manual_enabled = true; + break; + + case NAVIGATION_STATE_STABILIZE: + ret = TRANSITION_CHANGED; + control_mode->flag_control_rates_enabled = true; + control_mode->flag_control_attitude_enabled = true; + control_mode->flag_control_velocity_enabled = false; + control_mode->flag_control_position_enabled = false; + control_mode->flag_control_altitude_enabled = false; + control_mode->flag_control_manual_enabled = true; + break; + + case NAVIGATION_STATE_ALTHOLD: + ret = TRANSITION_CHANGED; + control_mode->flag_control_rates_enabled = true; + control_mode->flag_control_attitude_enabled = true; + control_mode->flag_control_velocity_enabled = false; + control_mode->flag_control_position_enabled = false; + control_mode->flag_control_altitude_enabled = true; + control_mode->flag_control_manual_enabled = true; + break; + + case NAVIGATION_STATE_VECTOR: + ret = TRANSITION_CHANGED; + control_mode->flag_control_rates_enabled = true; + control_mode->flag_control_attitude_enabled = true; + control_mode->flag_control_velocity_enabled = true; + control_mode->flag_control_position_enabled = true; + control_mode->flag_control_altitude_enabled = true; + control_mode->flag_control_manual_enabled = true; + break; + + case NAVIGATION_STATE_AUTO_READY: + ret = TRANSITION_CHANGED; + control_mode->flag_control_rates_enabled = true; + control_mode->flag_control_attitude_enabled = true; + control_mode->flag_control_velocity_enabled = true; + control_mode->flag_control_position_enabled = true; + control_mode->flag_control_altitude_enabled = true; + control_mode->flag_control_manual_enabled = false; + break; + + case NAVIGATION_STATE_AUTO_TAKEOFF: + + /* only transitions from AUTO_READY */ + if (current_status->navigation_state != NAVIGATION_STATE_AUTO_READY) { + ret = TRANSITION_CHANGED; + control_mode->flag_control_rates_enabled = true; + control_mode->flag_control_attitude_enabled = true; + control_mode->flag_control_velocity_enabled = true; + control_mode->flag_control_position_enabled = true; + control_mode->flag_control_altitude_enabled = true; + control_mode->flag_control_manual_enabled = false; + } + + break; + + case NAVIGATION_STATE_AUTO_LOITER: + + /* deny transitions from landed states */ + if (current_status->navigation_state != NAVIGATION_STATE_STANDBY && + current_status->navigation_state != NAVIGATION_STATE_AUTO_READY) { + ret = TRANSITION_CHANGED; + control_mode->flag_control_rates_enabled = true; + control_mode->flag_control_attitude_enabled = true; + control_mode->flag_control_velocity_enabled = true; + control_mode->flag_control_position_enabled = true; + control_mode->flag_control_altitude_enabled = true; + control_mode->flag_control_manual_enabled = false; + } + + break; + + case NAVIGATION_STATE_AUTO_MISSION: + + /* deny transitions from landed states */ + if (current_status->navigation_state != NAVIGATION_STATE_STANDBY && + current_status->navigation_state != NAVIGATION_STATE_AUTO_READY) { + ret = TRANSITION_CHANGED; + control_mode->flag_control_rates_enabled = true; + control_mode->flag_control_attitude_enabled = true; + control_mode->flag_control_velocity_enabled = true; + control_mode->flag_control_position_enabled = true; + control_mode->flag_control_altitude_enabled = true; + control_mode->flag_control_manual_enabled = false; + } + + break; + + case NAVIGATION_STATE_AUTO_RTL: + + /* deny transitions from landed states */ + if (current_status->navigation_state != NAVIGATION_STATE_STANDBY && + current_status->navigation_state != NAVIGATION_STATE_AUTO_READY) { + ret = TRANSITION_CHANGED; + control_mode->flag_control_rates_enabled = true; + control_mode->flag_control_attitude_enabled = true; + control_mode->flag_control_velocity_enabled = true; + control_mode->flag_control_position_enabled = true; + control_mode->flag_control_altitude_enabled = true; + control_mode->flag_control_manual_enabled = false; + } + + break; + + case NAVIGATION_STATE_AUTO_LAND: + + /* deny transitions from landed states */ + if (current_status->navigation_state != NAVIGATION_STATE_STANDBY && + current_status->navigation_state != NAVIGATION_STATE_AUTO_READY) { + ret = TRANSITION_CHANGED; + control_mode->flag_control_rates_enabled = true; + control_mode->flag_control_attitude_enabled = true; + control_mode->flag_control_velocity_enabled = true; + control_mode->flag_control_position_enabled = true; + control_mode->flag_control_altitude_enabled = true; + control_mode->flag_control_manual_enabled = false; + } + + break; + + default: + break; + } + + if (ret == TRANSITION_CHANGED) { + current_status->navigation_state = new_navigation_state; + } + } return ret; } @@ -582,31 +420,33 @@ int hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_s switch (new_state) { - case HIL_STATE_OFF: + case HIL_STATE_OFF: - if (current_status->arming_state == ARMING_STATE_INIT - || current_status->arming_state == ARMING_STATE_STANDBY) { + if (current_status->arming_state == ARMING_STATE_INIT + || current_status->arming_state == ARMING_STATE_STANDBY) { - current_control_mode->flag_system_hil_enabled = false; - mavlink_log_critical(mavlink_fd, "Switched to OFF hil state"); - valid_transition = true; - } - break; + current_control_mode->flag_system_hil_enabled = false; + mavlink_log_critical(mavlink_fd, "Switched to OFF hil state"); + valid_transition = true; + } - case HIL_STATE_ON: + break; - if (current_status->arming_state == ARMING_STATE_INIT - || current_status->arming_state == ARMING_STATE_STANDBY) { + case HIL_STATE_ON: - current_control_mode->flag_system_hil_enabled = true; - mavlink_log_critical(mavlink_fd, "Switched to ON hil state"); - valid_transition = true; - } - break; + if (current_status->arming_state == ARMING_STATE_INIT + || current_status->arming_state == ARMING_STATE_STANDBY) { - default: - warnx("Unknown hil state"); - break; + current_control_mode->flag_system_hil_enabled = true; + mavlink_log_critical(mavlink_fd, "Switched to ON hil state"); + valid_transition = true; + } + + break; + + default: + warnx("Unknown hil state"); + break; } } @@ -621,6 +461,7 @@ int hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_s orb_publish(ORB_ID(vehicle_control_mode), control_mode_pub, current_control_mode); ret = OK; + } else { mavlink_log_critical(mavlink_fd, "REJECTING invalid hil state transition"); } diff --git a/src/modules/commander/state_machine_helper.h b/src/modules/commander/state_machine_helper.h index 75fdd224c4..b59b66c8ba 100644 --- a/src/modules/commander/state_machine_helper.h +++ b/src/modules/commander/state_machine_helper.h @@ -49,10 +49,18 @@ #include #include +typedef enum { + TRANSITION_DENIED = -1, + TRANSITION_NOT_CHANGED = 0, + TRANSITION_CHANGED -int arming_state_transition(int status_pub, struct vehicle_status_s *current_state, arming_state_t new_arming_state, int armed_pub, struct actuator_armed_s *armed, const int mavlink_fd); +} transition_result_t; -int navigation_state_transition(int status_pub, struct vehicle_status_s *current_state, navigation_state_t new_navigation_state, int control_mode_pub, struct vehicle_control_mode_s *control_mode, const int mavlink_fd); +transition_result_t arming_state_transition(struct vehicle_status_s *current_state, arming_state_t new_arming_state, struct actuator_armed_s *armed, const int mavlink_fd); + +transition_result_t main_state_transition(struct vehicle_status_s *current_state, main_state_t new_main_state, const int mavlink_fd); + +transition_result_t navigation_state_transition(struct vehicle_status_s *current_status, navigation_state_t new_navigation_state, struct vehicle_control_mode_s *control_mode, const int mavlink_fd); int hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_status_s *current_state, int control_mode_pub, struct vehicle_control_mode_s *current_control_mode, const int mavlink_fd); diff --git a/src/modules/mavlink/mavlink.c b/src/modules/mavlink/mavlink.c index ae8e168e18..9132d1b491 100644 --- a/src/modules/mavlink/mavlink.c +++ b/src/modules/mavlink/mavlink.c @@ -196,13 +196,7 @@ get_mavlink_mode_and_state(uint8_t *mavlink_state, uint8_t *mavlink_mode) } /* autonomous mode */ - if (v_status.navigation_state == NAVIGATION_STATE_AUTO_LAND - || v_status.navigation_state == NAVIGATION_STATE_AUTO_LOITER - || v_status.navigation_state == NAVIGATION_STATE_AUTO_MISSION - || v_status.navigation_state == NAVIGATION_STATE_AUTO_READY - || v_status.navigation_state == NAVIGATION_STATE_AUTO_RTL - || v_status.navigation_state == NAVIGATION_STATE_AUTO_STANDBY - || v_status.navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF) { + if (v_status.main_state == MAIN_STATE_AUTO) { *mavlink_mode |= MAV_MODE_FLAG_AUTO_ENABLED; } @@ -215,15 +209,14 @@ get_mavlink_mode_and_state(uint8_t *mavlink_state, uint8_t *mavlink_mode) *mavlink_mode &= ~MAV_MODE_FLAG_SAFETY_ARMED; } - if (v_status.navigation_state == NAVIGATION_STATE_MANUAL - || v_status.navigation_state == NAVIGATION_STATE_MANUAL_STANDBY) { + if (v_status.main_state == MAIN_STATE_MANUAL + || v_status.main_state == MAIN_STATE_SEATBELT + || v_status.main_state == MAIN_STATE_EASY) { *mavlink_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED; } - if (v_status.navigation_state == NAVIGATION_STATE_ASSISTED_SEATBELT - || v_status.navigation_state == NAVIGATION_STATE_ASSISTED_DESCENT - || v_status.navigation_state == NAVIGATION_STATE_ASSISTED_STANDBY) { + if (v_status.navigation_state == MAIN_STATE_EASY) { *mavlink_mode |= MAV_MODE_FLAG_GUIDED_ENABLED; } @@ -235,41 +228,25 @@ get_mavlink_mode_and_state(uint8_t *mavlink_state, uint8_t *mavlink_mode) /* set calibration state */ if (v_status.preflight_calibration) { - *mavlink_state = MAV_STATE_CALIBRATING; - } else if (v_status.system_emergency) { - *mavlink_state = MAV_STATE_EMERGENCY; - - // XXX difference between active and armed? is AUTO_READY active? - } else if (v_status.navigation_state == NAVIGATION_STATE_AUTO_LAND - || v_status.navigation_state == NAVIGATION_STATE_AUTO_LOITER - || v_status.navigation_state == NAVIGATION_STATE_AUTO_MISSION - || v_status.navigation_state == NAVIGATION_STATE_AUTO_RTL - || v_status.navigation_state == NAVIGATION_STATE_AUTO_TAKEOFF - || v_status.navigation_state == NAVIGATION_STATE_ASSISTED_SEATBELT - || v_status.navigation_state == NAVIGATION_STATE_ASSISTED_DESCENT - || v_status.navigation_state == NAVIGATION_STATE_MANUAL) { - - *mavlink_state = MAV_STATE_ACTIVE; - - } else if (v_status.navigation_state == NAVIGATION_STATE_AUTO_STANDBY - || v_status.navigation_state == NAVIGATION_STATE_AUTO_READY // XXX correct? - || v_status.navigation_state == NAVIGATION_STATE_ASSISTED_STANDBY - || v_status.navigation_state == NAVIGATION_STATE_MANUAL_STANDBY) { - - *mavlink_state = MAV_STATE_STANDBY; - - } else if (v_status.navigation_state == NAVIGATION_STATE_INIT) { - + } else if (v_status.arming_state == ARMING_STATE_INIT + || v_status.arming_state == ARMING_STATE_IN_AIR_RESTORE + || v_status.arming_state == ARMING_STATE_STANDBY_ERROR) { // TODO review *mavlink_state = MAV_STATE_UNINIT; + } else if (v_status.arming_state == ARMING_STATE_ARMED) { + *mavlink_state = MAV_STATE_ACTIVE; + } else if (v_status.arming_state == ARMING_STATE_ARMED_ERROR) { + *mavlink_state = MAV_STATE_CRITICAL; + } else if (v_status.arming_state == ARMING_STATE_STANDBY) { + *mavlink_state = MAV_STATE_STANDBY; + } else if (v_status.arming_state == ARMING_STATE_REBOOT) { + *mavlink_state = MAV_STATE_POWEROFF; } else { - warnx("Unknown mavlink state"); *mavlink_state = MAV_STATE_CRITICAL; } - } diff --git a/src/modules/uORB/topics/vehicle_status.h b/src/modules/uORB/topics/vehicle_status.h index 9b91e834e0..e7feaa98eb 100644 --- a/src/modules/uORB/topics/vehicle_status.h +++ b/src/modules/uORB/topics/vehicle_status.h @@ -58,29 +58,28 @@ * @addtogroup topics @{ */ -/* State Machine */ +/* main state machine */ typedef enum { - NAVIGATION_STATE_INIT = 0, - NAVIGATION_STATE_MANUAL_STANDBY, - NAVIGATION_STATE_MANUAL, - NAVIGATION_STATE_ASSISTED_STANDBY, - NAVIGATION_STATE_ASSISTED_SEATBELT, - NAVIGATION_STATE_ASSISTED_SIMPLE, - NAVIGATION_STATE_ASSISTED_DESCENT, - NAVIGATION_STATE_AUTO_STANDBY, - NAVIGATION_STATE_AUTO_READY, - NAVIGATION_STATE_AUTO_TAKEOFF, - NAVIGATION_STATE_AUTO_LOITER, - NAVIGATION_STATE_AUTO_MISSION, - NAVIGATION_STATE_AUTO_RTL, - NAVIGATION_STATE_AUTO_LAND -} navigation_state_t; + MAIN_STATE_MANUAL = 0, + MAIN_STATE_SEATBELT, + MAIN_STATE_EASY, + MAIN_STATE_AUTO, +} main_state_t; +/* navigation state machine */ typedef enum { - MANUAL_STANDBY = 0, - MANUAL_READY, - MANUAL_IN_AIR -} manual_state_t; + NAVIGATION_STATE_STANDBY = 0, // standby state, disarmed + NAVIGATION_STATE_DIRECT, // true manual control, no any stabilization + NAVIGATION_STATE_STABILIZE, // attitude stabilization + NAVIGATION_STATE_ALTHOLD, // attitude + altitude stabilization + NAVIGATION_STATE_VECTOR, // attitude + altitude + position stabilization + NAVIGATION_STATE_AUTO_READY, // AUTO, landed, reeady for takeoff + NAVIGATION_STATE_AUTO_TAKEOFF, // detect takeoff using land detector and switch to desired AUTO mode + NAVIGATION_STATE_AUTO_LOITER, // pause mission + NAVIGATION_STATE_AUTO_MISSION, // fly mission + NAVIGATION_STATE_AUTO_RTL, // Return To Launch, when home position switch to LAND + NAVIGATION_STATE_AUTO_LAND // land and switch to AUTO_READY when landed (detect using land detector) +} navigation_state_t; typedef enum { ARMING_STATE_INIT = 0, @@ -103,16 +102,16 @@ typedef enum { MODE_SWITCH_AUTO } mode_switch_pos_t; +typedef enum { + ASSISTED_SWITCH_SEATBELT = 0, + ASSISTED_SWITCH_EASY +} assisted_switch_pos_t; + typedef enum { RETURN_SWITCH_NONE = 0, RETURN_SWITCH_RETURN } return_switch_pos_t; -typedef enum { - ASSISTED_SWITCH_SEATBELT = 0, - ASSISTED_SWITCH_SIMPLE -} assisted_switch_pos_t; - typedef enum { MISSION_SWITCH_NONE = 0, MISSION_SWITCH_MISSION @@ -175,7 +174,8 @@ struct vehicle_status_s uint64_t failsave_lowlevel_start_time; /**< time when the lowlevel failsafe flag was set */ // uint64_t failsave_highlevel_begin; TO BE COMPLETED - navigation_state_t navigation_state; /**< current system state */ + main_state_t main_state; /**< main state machine */ + navigation_state_t navigation_state; /**< navigation state machine */ arming_state_t arming_state; /**< current arming state */ hil_state_t hil_state; /**< current hil state */ @@ -183,6 +183,8 @@ struct vehicle_status_s int32_t system_id; /**< system id, inspired by MAVLink's system ID field */ int32_t component_id; /**< subsystem / component id, inspired by MAVLink's component ID field */ + bool is_rotary_wing; + mode_switch_pos_t mode_switch; return_switch_pos_t return_switch; assisted_switch_pos_t assisted_switch; @@ -198,6 +200,7 @@ struct vehicle_status_s bool condition_home_position_valid; /**< indicates a valid home position (a valid home position is not always a valid launch) */ bool condition_local_position_valid; bool condition_airspeed_valid; /**< set to true by the commander app if there is a valid airspeed measurement available */ + bool condition_landed; /**< true if vehicle is landed, always true if disarmed */ bool rc_signal_found_once; bool rc_signal_lost; /**< true if RC reception is terminally lost */ From 02d4480e8ed7c6c6460f95f531aeef2725951663 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Wed, 31 Jul 2013 20:58:27 +0400 Subject: [PATCH 094/109] commander rewrite almost completed, WIP --- src/modules/commander/commander.cpp | 873 ++++++++---------- src/modules/commander/commander_helper.cpp | 6 +- .../commander/state_machine_helper.cpp | 70 +- src/modules/commander/state_machine_helper.h | 12 +- 4 files changed, 460 insertions(+), 501 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index b2336cbf6f..2012abcc09 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -73,7 +73,6 @@ #include #include #include -#include #include #include @@ -124,6 +123,26 @@ extern struct system_load_s system_load; #define LOCAL_POSITION_TIMEOUT 1000000 /**< consider the local position estimate invalid after 1s */ +#define PRINT_INTERVAL 5000000 +#define PRINT_MODE_REJECT_INTERVAL 2000000 + +// TODO include mavlink headers +/** @brief These flags encode the MAV mode. */ +#ifndef HAVE_ENUM_MAV_MODE_FLAG +#define HAVE_ENUM_MAV_MODE_FLAG +enum MAV_MODE_FLAG { + MAV_MODE_FLAG_CUSTOM_MODE_ENABLED = 1, /* 0b00000001 Reserved for future use. | */ + MAV_MODE_FLAG_TEST_ENABLED = 2, /* 0b00000010 system has a test mode enabled. This flag is intended for temporary system tests and should not be used for stable implementations. | */ + MAV_MODE_FLAG_AUTO_ENABLED = 4, /* 0b00000100 autonomous mode enabled, system finds its own goal positions. Guided flag can be set or not, depends on the actual implementation. | */ + MAV_MODE_FLAG_GUIDED_ENABLED = 8, /* 0b00001000 guided mode enabled, system flies MISSIONs / mission items. | */ + MAV_MODE_FLAG_STABILIZE_ENABLED = 16, /* 0b00010000 system stabilizes electronically its attitude (and optionally position). It needs however further control inputs to move around. | */ + MAV_MODE_FLAG_HIL_ENABLED = 32, /* 0b00100000 hardware in the loop simulation. All motors / actuators are blocked, but internal software is full operational. | */ + MAV_MODE_FLAG_MANUAL_INPUT_ENABLED = 64, /* 0b01000000 remote control input is enabled. | */ + MAV_MODE_FLAG_SAFETY_ARMED = 128, /* 0b10000000 MAV safety set to armed. Motors are enabled / running / can start. Ready to fly. | */ + MAV_MODE_FLAG_ENUM_END = 129, /* | */ +}; +#endif + /* Mavlink file descriptors */ static int mavlink_fd; @@ -135,9 +154,13 @@ static int daemon_task; /**< Handle of daemon task / thread */ /* timout until lowlevel failsafe */ static unsigned int failsafe_lowlevel_timeout_ms; +static unsigned int leds_counter; +/* To remember when last notification was sent */ +static uint64_t last_print_mode_reject_time = 0; + /* tasks waiting for low prio thread */ -enum { +typedef enum { LOW_PRIO_TASK_NONE = 0, LOW_PRIO_TASK_PARAM_SAVE, LOW_PRIO_TASK_PARAM_LOAD, @@ -147,8 +170,9 @@ enum { LOW_PRIO_TASK_RC_CALIBRATION, LOW_PRIO_TASK_ACCEL_CALIBRATION, LOW_PRIO_TASK_AIRSPEED_CALIBRATION -} low_prio_task; +} low_prio_task_t; +static low_prio_task_t low_prio_task = LOW_PRIO_TASK_NONE; /** * The daemon app only briefly exists to start @@ -170,17 +194,21 @@ void usage(const char *reason); /** * React to commands that are sent e.g. from the mavlink module. */ -void handle_command(int status_pub, struct vehicle_status_s *current_status, int control_mode_pub, struct vehicle_control_mode_s *current_control_mode, struct vehicle_command_s *cmd, int armed_pub, struct actuator_armed_s *armed); +void handle_command(struct vehicle_status_s *status, struct vehicle_control_mode_s *control_mode, struct vehicle_command_s *cmd, struct actuator_armed_s *armed); /** * Mainloop of commander. */ int commander_thread_main(int argc, char *argv[]); +void toggle_status_leds(vehicle_status_s *status, actuator_armed_s *armed, vehicle_gps_position_s *gps_position); + void check_mode_switches(struct manual_control_setpoint_s *sp_man, struct vehicle_status_s *current_status); transition_result_t check_main_state_machine(struct vehicle_status_s *current_status); +void print_reject_mode(const char *msg); + transition_result_t check_navigation_state_machine(struct vehicle_status_s *current_status, struct vehicle_control_mode_s *control_mode); /** @@ -241,15 +269,72 @@ void usage(const char *reason) exit(1); } -void handle_command(int status_pub, struct vehicle_status_s *current_status, int control_mode_pub, struct vehicle_control_mode_s *current_control_mode, struct vehicle_command_s *cmd, int armed_pub, struct actuator_armed_s *armed) +void handle_command(struct vehicle_status_s *status, struct vehicle_control_mode_s *control_mode, struct vehicle_command_s *cmd, struct actuator_armed_s *armed) { /* result of the command */ uint8_t result = VEHICLE_CMD_RESULT_UNSUPPORTED; /* request to set different system mode */ switch (cmd->command) { - case VEHICLE_CMD_DO_SET_MODE: - break; + case VEHICLE_CMD_DO_SET_MODE: { + uint8_t base_mode = (int) cmd->param1; + uint8_t custom_mode = (int) cmd->param2; + // TODO remove debug code + mavlink_log_critical(mavlink_fd, "[cmd] command setmode: %d %d", base_mode, custom_mode); + /* set arming state */ + transition_result_t arming_res = TRANSITION_NOT_CHANGED; + + if (base_mode & MAV_MODE_FLAG_SAFETY_ARMED) { + arming_res = arming_state_transition(status, ARMING_STATE_ARMED, armed); + if (arming_res == TRANSITION_CHANGED) { + mavlink_log_info(mavlink_fd, "[cmd] ARMED by command"); + } + + } else { + if (status->arming_state == ARMING_STATE_ARMED || status->arming_state == ARMING_STATE_ARMED_ERROR) { + arming_state_t new_arming_state = (status->arming_state == ARMING_STATE_ARMED ? ARMING_STATE_STANDBY : ARMING_STATE_STANDBY_ERROR); + arming_res = arming_state_transition(status, new_arming_state, armed); + if (arming_res == TRANSITION_CHANGED) { + mavlink_log_info(mavlink_fd, "[cmd] DISARMED by command"); + } + } else { + arming_res = TRANSITION_NOT_CHANGED; + } + } + + /* set main state */ + transition_result_t main_res = TRANSITION_DENIED; + + if (base_mode & MAV_MODE_FLAG_AUTO_ENABLED) { + /* AUTO */ + main_res = main_state_transition(status, MAIN_STATE_AUTO); + + } else if (base_mode & MAV_MODE_FLAG_MANUAL_INPUT_ENABLED) { + if (base_mode & MAV_MODE_FLAG_GUIDED_ENABLED) { + /* EASY */ + main_res = main_state_transition(status, MAIN_STATE_EASY); + + } else if (base_mode & MAV_MODE_FLAG_STABILIZE_ENABLED) { + if (custom_mode & 1) { // TODO add custom mode flags definition + /* SEATBELT */ + main_res = main_state_transition(status, MAIN_STATE_SEATBELT); + + } else { + /* MANUAL */ + main_res = main_state_transition(status, MAIN_STATE_MANUAL); + } + } + } + + if (arming_res != TRANSITION_DENIED && main_res != TRANSITION_DENIED) { + result = VEHICLE_CMD_RESULT_ACCEPTED; + + } else { + result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; + } + + break; + } case VEHICLE_CMD_COMPONENT_ARM_DISARM: break; @@ -257,20 +342,39 @@ void handle_command(int status_pub, struct vehicle_status_s *current_status, int case VEHICLE_CMD_PREFLIGHT_REBOOT_SHUTDOWN: break; - case VEHICLE_CMD_PREFLIGHT_CALIBRATION: + case VEHICLE_CMD_PREFLIGHT_CALIBRATION: { + low_prio_task_t new_low_prio_task = LOW_PRIO_TASK_NONE; - /* gyro calibration */ - if ((int)(cmd->param1) == 1) { + if ((int)(cmd->param1) == 1) { + /* gyro calibration */ + new_low_prio_task = LOW_PRIO_TASK_GYRO_CALIBRATION; - /* check if no other task is scheduled */ - if (low_prio_task == LOW_PRIO_TASK_NONE) { + } else if ((int)(cmd->param2) == 1) { + /* magnetometer calibration */ + new_low_prio_task = LOW_PRIO_TASK_MAG_CALIBRATION; + } else if ((int)(cmd->param3) == 1) { + /* zero-altitude pressure calibration */ + //new_low_prio_task = LOW_PRIO_TASK_ALTITUDE_CALIBRATION; + } else if ((int)(cmd->param4) == 1) { + /* RC calibration */ + new_low_prio_task = LOW_PRIO_TASK_RC_CALIBRATION; + + } else if ((int)(cmd->param5) == 1) { + /* accelerometer calibration */ + new_low_prio_task = LOW_PRIO_TASK_ACCEL_CALIBRATION; + + } else if ((int)(cmd->param6) == 1) { + /* airspeed calibration */ + new_low_prio_task = LOW_PRIO_TASK_AIRSPEED_CALIBRATION; + } + + /* check if we have new task and no other task is scheduled */ + if (low_prio_task == LOW_PRIO_TASK_NONE && new_low_prio_task != LOW_PRIO_TASK_NONE) { /* try to go to INIT/PREFLIGHT arming state */ - if (OK == arming_state_transition(current_status, ARMING_STATE_INIT, armed, mavlink_fd)) { - // TODO publish state + if (TRANSITION_DENIED != arming_state_transition(status, ARMING_STATE_INIT, armed)) { result = VEHICLE_CMD_RESULT_ACCEPTED; - /* now set the task for the low prio thread */ - low_prio_task = LOW_PRIO_TASK_GYRO_CALIBRATION; + low_prio_task = new_low_prio_task; } else { result = VEHICLE_CMD_RESULT_DENIED; @@ -279,159 +383,55 @@ void handle_command(int status_pub, struct vehicle_status_s *current_status, int } else { result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; } + + break; } - /* magnetometer calibration */ - if ((int)(cmd->param2) == 1) { + case VEHICLE_CMD_PREFLIGHT_STORAGE: { + low_prio_task_t new_low_prio_task = LOW_PRIO_TASK_NONE; - /* check if no other task is scheduled */ - if (low_prio_task == LOW_PRIO_TASK_NONE) { - - /* try to go to INIT/PREFLIGHT arming state */ - if (OK == arming_state_transition(current_status, ARMING_STATE_INIT, armed, mavlink_fd)) { - // TODO publish state - result = VEHICLE_CMD_RESULT_ACCEPTED; - /* now set the task for the low prio thread */ - low_prio_task = LOW_PRIO_TASK_MAG_CALIBRATION; - - } else { - result = VEHICLE_CMD_RESULT_DENIED; - } - - } else { - result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; - } - } - - // /* zero-altitude pressure calibration */ - // if ((int)(cmd->param3) == 1) { - - // /* check if no other task is scheduled */ - // if(low_prio_task == LOW_PRIO_TASK_NONE) { - - // /* try to go to INIT/PREFLIGHT arming state */ - // if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_INIT, armed_pub, armed, mavlink_fd)) { - // result = VEHICLE_CMD_RESULT_ACCEPTED; - // /* now set the task for the low prio thread */ - // low_prio_task = LOW_PRIO_TASK_ALTITUDE_CALIBRATION; - // } else { - // result = VEHICLE_CMD_RESULT_DENIED; - // } - // } else { - // result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; - // } - // } - - // /* trim calibration */ - // if ((int)(cmd->param4) == 1) { - - // /* check if no other task is scheduled */ - // if(low_prio_task == LOW_PRIO_TASK_NONE) { - - // /* try to go to INIT/PREFLIGHT arming state */ - // if (OK == arming_state_transition(status_pub, current_vehicle_status, ARMING_STATE_INIT, armed_pub, armed, mavlink_fd)) { - // result = VEHICLE_CMD_RESULT_ACCEPTED; - // /* now set the task for the low prio thread */ - // low_prio_task = LOW_PRIO_TASK_RC_CALIBRATION; - // } else { - // result = VEHICLE_CMD_RESULT_DENIED; - // } - // } else { - // result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; - // } - // } - - - /* accel calibration */ - if ((int)(cmd->param5) == 1) { - - /* check if no other task is scheduled */ - if (low_prio_task == LOW_PRIO_TASK_NONE) { - - /* try to go to INIT/PREFLIGHT arming state */ - if (OK == arming_state_transition(current_status, ARMING_STATE_INIT, armed, mavlink_fd)) { - // TODO publish state - result = VEHICLE_CMD_RESULT_ACCEPTED; - /* now set the task for the low prio thread */ - low_prio_task = LOW_PRIO_TASK_ACCEL_CALIBRATION; - - } else { - result = VEHICLE_CMD_RESULT_DENIED; - } - - } else { - result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; - } - } - - /* airspeed calibration */ - if ((int)(cmd->param6) == 1) { - - /* check if no other task is scheduled */ - if (low_prio_task == LOW_PRIO_TASK_NONE) { - - /* try to go to INIT/PREFLIGHT arming state */ - if (OK == arming_state_transition(current_status, ARMING_STATE_INIT, armed, mavlink_fd)) { - // TODO publish state - result = VEHICLE_CMD_RESULT_ACCEPTED; - /* now set the task for the low prio thread */ - low_prio_task = LOW_PRIO_TASK_AIRSPEED_CALIBRATION; - - } else { - result = VEHICLE_CMD_RESULT_DENIED; - } - - } else { - result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; - } - } - - break; - - case VEHICLE_CMD_PREFLIGHT_STORAGE: - - if (((int)(cmd->param1)) == 0) { - /* check if no other task is scheduled */ - if (low_prio_task == LOW_PRIO_TASK_NONE) { + if (((int)(cmd->param1)) == 0) { low_prio_task = LOW_PRIO_TASK_PARAM_LOAD; - result = VEHICLE_CMD_RESULT_ACCEPTED; - } else { - result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; - } - - - } else if (((int)(cmd->param1)) == 1) { - - /* check if no other task is scheduled */ - if (low_prio_task == LOW_PRIO_TASK_NONE) { + } else if (((int)(cmd->param1)) == 1) { low_prio_task = LOW_PRIO_TASK_PARAM_SAVE; + } + + /* check if we have new task and no other task is scheduled */ + if (low_prio_task == LOW_PRIO_TASK_NONE && new_low_prio_task != LOW_PRIO_TASK_NONE) { result = VEHICLE_CMD_RESULT_ACCEPTED; + low_prio_task = new_low_prio_task; } else { result = VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED; } - } - break; + break; + } default: - mavlink_log_critical(mavlink_fd, "[cmd] refusing unsupported command"); - result = VEHICLE_CMD_RESULT_UNSUPPORTED; break; } /* supported command handling stop */ - if (result == VEHICLE_CMD_RESULT_FAILED || - result == VEHICLE_CMD_RESULT_DENIED || - result == VEHICLE_CMD_RESULT_UNSUPPORTED || - result == VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED) { + if (result == VEHICLE_CMD_RESULT_ACCEPTED) { + tune_positive(); + } else { tune_negative(); - } else if (result == VEHICLE_CMD_RESULT_ACCEPTED) { + if (result == VEHICLE_CMD_RESULT_DENIED) { + mavlink_log_critical(mavlink_fd, "[cmd] command denied: %u", cmd->command); - tune_positive(); + } else if (result == VEHICLE_CMD_RESULT_FAILED) { + mavlink_log_critical(mavlink_fd, "[cmd] command failed: %u", cmd->command); + + } else if (result == VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED) { + mavlink_log_critical(mavlink_fd, "[cmd] command temporarily rejected: %u", cmd->command); + + } else if (result == VEHICLE_CMD_RESULT_UNSUPPORTED) { + mavlink_log_critical(mavlink_fd, "[cmd] command unsupported: %u", cmd->command); + } } /* send any requested ACKs */ @@ -515,16 +515,9 @@ int commander_thread_main(int argc, char *argv[]) /* allow manual override initially */ control_mode.flag_external_manual_override_ok = true; - /* flag position info as bad, do not allow auto mode */ - // current_status.flag_vector_flight_mode_ok = false; - /* set battery warning flag */ status.battery_warning = VEHICLE_BATTERY_WARNING_NONE; - /* set safety device detection flag */ - /* XXX do we need this? */ - //current_status.flag_safety_present = false; - // XXX for now just set sensors as initialized status.condition_system_sensors_initialized = true; @@ -535,12 +528,11 @@ int commander_thread_main(int argc, char *argv[]) status_pub = orb_advertise(ORB_ID(vehicle_status), &status); /* publish current state machine */ - /* publish the new state */ + /* publish initial state */ status.counter++; status.timestamp = hrt_absolute_time(); orb_publish(ORB_ID(vehicle_status), status_pub, &status); - armed_pub = orb_advertise(ORB_ID(actuator_armed), &armed); control_mode_pub = orb_advertise(ORB_ID(vehicle_control_mode), &control_mode); @@ -556,8 +548,7 @@ int commander_thread_main(int argc, char *argv[]) exit(ERROR); } - // XXX needed? - mavlink_log_info(mavlink_fd, "system is running"); + mavlink_log_info(mavlink_fd, "[cmd] started"); pthread_attr_t commander_low_prio_attr; pthread_attr_init(&commander_low_prio_attr); @@ -577,9 +568,10 @@ int commander_thread_main(int argc, char *argv[]) unsigned stick_on_counter = 0; /* To remember when last notification was sent */ - uint64_t last_print_time = 0; + uint64_t last_print_control_time = 0; - float voltage_previous = 0.0f; + enum VEHICLE_BATTERY_WARNING battery_warning_previous = VEHICLE_BATTERY_WARNING_NONE; + bool armed_previous = false; bool low_battery_voltage_actions_done; bool critical_battery_voltage_actions_done; @@ -588,10 +580,10 @@ int commander_thread_main(int argc, char *argv[]) uint64_t start_time = 0; - bool state_changed = true; + bool status_changed = true; bool param_init_forced = true; - bool new_data = false; + bool updated = false; /* Subscribe to safety topic */ int safety_sub = orb_subscribe(ORB_ID(safety)); @@ -612,13 +604,11 @@ int commander_thread_main(int argc, char *argv[]) int global_position_sub = orb_subscribe(ORB_ID(vehicle_global_position)); struct vehicle_global_position_s global_position; memset(&global_position, 0, sizeof(global_position)); - uint64_t last_global_position_time = 0; /* Subscribe to local position data */ int local_position_sub = orb_subscribe(ORB_ID(vehicle_local_position)); struct vehicle_local_position_s local_position; memset(&local_position, 0, sizeof(local_position)); - uint64_t last_local_position_time = 0; /* * The home position is set based on GPS only, to prevent a dependency between @@ -670,11 +660,12 @@ int commander_thread_main(int argc, char *argv[]) start_time = hrt_absolute_time(); while (!thread_should_exit) { + hrt_abstime t = hrt_absolute_time(); /* update parameters */ - orb_check(param_changed_sub, &new_data); + orb_check(param_changed_sub, &updated); - if (new_data || param_init_forced) { + if (updated || param_init_forced) { param_init_forced = false; /* parameters changed */ orb_copy(ORB_ID(parameter_update), param_changed_sub, ¶m_changed); @@ -703,94 +694,96 @@ int commander_thread_main(int argc, char *argv[]) /* check and update system / component ID */ param_get(_param_system_id, &(status.system_id)); param_get(_param_component_id, &(status.component_id)); - + status_changed = true; } } - orb_check(sp_man_sub, &new_data); + orb_check(sp_man_sub, &updated); - if (new_data) { + if (updated) { orb_copy(ORB_ID(manual_control_setpoint), sp_man_sub, &sp_man); } - orb_check(sp_offboard_sub, &new_data); + orb_check(sp_offboard_sub, &updated); - if (new_data) { + if (updated) { orb_copy(ORB_ID(offboard_control_setpoint), sp_offboard_sub, &sp_offboard); } - orb_check(sensor_sub, &new_data); + orb_check(sensor_sub, &updated); - if (new_data) { + if (updated) { orb_copy(ORB_ID(sensor_combined), sensor_sub, &sensors); } - orb_check(diff_pres_sub, &new_data); + orb_check(diff_pres_sub, &updated); - if (new_data) { + if (updated) { orb_copy(ORB_ID(differential_pressure), diff_pres_sub, &diff_pres); last_diff_pres_time = diff_pres.timestamp; } - orb_check(cmd_sub, &new_data); + orb_check(cmd_sub, &updated); - if (new_data) { + if (updated) { /* got command */ orb_copy(ORB_ID(vehicle_command), cmd_sub, &cmd); /* handle it */ - handle_command(status_pub, &status, control_mode_pub, &control_mode, &cmd, armed_pub, &armed); + handle_command(&status, &control_mode, &cmd, &armed); } /* update safety topic */ - orb_check(safety_sub, &new_data); + orb_check(safety_sub, &updated); - if (new_data) { + if (updated) { orb_copy(ORB_ID(safety), safety_sub, &safety); } /* update global position estimate */ - orb_check(global_position_sub, &new_data); + orb_check(global_position_sub, &updated); - if (new_data) { + if (updated) { /* position changed */ orb_copy(ORB_ID(vehicle_global_position), global_position_sub, &global_position); - last_global_position_time = global_position.timestamp; } /* update local position estimate */ - orb_check(local_position_sub, &new_data); + orb_check(local_position_sub, &updated); - if (new_data) { + if (updated) { /* position changed */ orb_copy(ORB_ID(vehicle_local_position), local_position_sub, &local_position); - last_local_position_time = local_position.timestamp; } /* set the condition to valid if there has recently been a local position estimate */ - if (hrt_absolute_time() - last_local_position_time < LOCAL_POSITION_TIMEOUT) { - status.condition_local_position_valid = true; + if (t - local_position.timestamp < LOCAL_POSITION_TIMEOUT) { + if (!status.condition_local_position_valid) { + status.condition_local_position_valid = true; + status_changed = true; + } } else { - status.condition_local_position_valid = false; + if (status.condition_local_position_valid) { + status.condition_local_position_valid = false; + status_changed = true; + } } /* update battery status */ - orb_check(battery_sub, &new_data); + orb_check(battery_sub, &updated); - if (new_data) { + if (updated) { orb_copy(ORB_ID(battery_status), battery_sub, &battery); status.battery_voltage = battery.voltage_v; status.condition_battery_voltage_valid = true; - - /* - * Only update battery voltage estimate if system has - * been running for two and a half seconds. - */ - } - if (hrt_absolute_time() - start_time > 2500000 && status.condition_battery_voltage_valid) { + /* + * Only update battery voltage estimate if system has + * been running for two and a half seconds. + */ + if (t - start_time > 2500000 && status.condition_battery_voltage_valid) { status.battery_remaining = battery_remaining_estimate_voltage(status.battery_voltage); } else { @@ -798,12 +791,12 @@ int commander_thread_main(int argc, char *argv[]) } /* update subsystem */ - orb_check(subsys_sub, &new_data); + orb_check(subsys_sub, &updated); - if (new_data) { + if (updated) { orb_copy(ORB_ID(subsystem_info), subsys_sub, &info); - warnx("Subsys changed: %d\n", (int)info.subsystem_type); + warnx("subsystem changed: %d\n", (int)info.subsystem_type); /* mark / unmark as present */ if (info.present) { @@ -828,55 +821,11 @@ int commander_thread_main(int argc, char *argv[]) } else { status.onboard_control_sensors_health &= ~info.subsystem_type; } + + status_changed = true; } - /* Slow but important 8 Hz checks */ - if (counter % ((1000000 / COMMANDER_MONITORING_INTERVAL) / 8) == 0) { - - /* XXX if armed */ - if (armed.armed) { - /* armed, solid */ - led_on(LED_AMBER); - - } else if (armed.ready_to_arm && (counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0)) { - /* ready to arm */ - led_toggle(LED_AMBER); - - } else if (counter % (100000 / COMMANDER_MONITORING_INTERVAL) == 0) { - /* not ready to arm, something is wrong */ - led_toggle(LED_AMBER); - } - - if (hrt_absolute_time() - gps_position.timestamp_position < 2000000) { - - /* toggle GPS (blue) led at 1 Hz if GPS present but no lock, make is solid once locked */ - if ((hrt_absolute_time() - gps_position.timestamp_position < 2000000) - && (gps_position.fix_type == GPS_FIX_TYPE_3D)) { - /* GPS lock */ - led_on(LED_BLUE); - - } else if ((counter + 4) % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0) { - /* no GPS lock, but GPS module is aquiring lock */ - led_toggle(LED_BLUE); - } - - } else { - /* no GPS info, don't light the blue led */ - led_off(LED_BLUE); - } - - - // /* toggle GPS led at 5 Hz in HIL mode */ - // if (current_status.flag_hil_enabled) { - // /* hil enabled */ - // led_toggle(LED_BLUE); - - // } else if (bat_remain < 0.3f && (low_voltage_counter > LOW_VOLTAGE_BATTERY_COUNTER_LIMIT)) { - // /* toggle arming (red) at 5 Hz on low battery or error */ - // led_toggle(LED_AMBER); - // } - - } + toggle_status_leds(&status, &armed, &gps_position); if (counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0) { /* compute system load */ @@ -888,29 +837,26 @@ int commander_thread_main(int argc, char *argv[]) last_idle_time = system_load.tasks[0].total_runtime; } - - /* if battery voltage is getting lower, warn using buzzer, etc. */ - if (status.condition_battery_voltage_valid && (status.battery_remaining < 0.15f /* XXX MAGIC NUMBER */) && (false == low_battery_voltage_actions_done)) { //TODO: add filter, or call emergency after n measurements < VOLTAGE_BATTERY_MINIMAL_MILLIVOLTS - + if (status.condition_battery_voltage_valid && status.battery_remaining < 0.15f && !low_battery_voltage_actions_done) { + //TODO: add filter, or call emergency after n measurements < VOLTAGE_BATTERY_MINIMAL_MILLIVOLTS if (low_voltage_counter > LOW_VOLTAGE_BATTERY_COUNTER_LIMIT) { low_battery_voltage_actions_done = true; - mavlink_log_critical(mavlink_fd, "[cmd] WARNING! LOW BATTERY!"); + mavlink_log_critical(mavlink_fd, "[cmd] WARNING: LOW BATTERY"); status.battery_warning = VEHICLE_BATTERY_WARNING_WARNING; - tune_low_bat(); + status_changed = true; } low_voltage_counter++; - } - /* Critical, this is rather an emergency, change state machine */ - else if (status.condition_battery_voltage_valid && (status.battery_remaining < 0.1f /* XXX MAGIC NUMBER */) && (false == critical_battery_voltage_actions_done && true == low_battery_voltage_actions_done)) { + } else if (status.condition_battery_voltage_valid && status.battery_remaining < 0.1f && !critical_battery_voltage_actions_done && low_battery_voltage_actions_done) { + /* critical battery voltage, this is rather an emergency, change state machine */ if (critical_voltage_counter > CRITICAL_VOLTAGE_BATTERY_COUNTER_LIMIT) { critical_battery_voltage_actions_done = true; - mavlink_log_critical(mavlink_fd, "[cmd] EMERGENCY! CRITICAL BATTERY!"); + mavlink_log_critical(mavlink_fd, "[cmd] EMERGENCY: CRITICAL BATTERY"); status.battery_warning = VEHICLE_BATTERY_WARNING_ALERT; - tune_critical_bat(); - // XXX implement state change here + arming_state_transition(&status, ARMING_STATE_ARMED_ERROR, &armed); + status_changed = true; } critical_voltage_counter++; @@ -923,10 +869,9 @@ int commander_thread_main(int argc, char *argv[]) /* End battery voltage check */ /* If in INIT state, try to proceed to STANDBY state */ - if (status.arming_state == ARMING_STATE_INIT) { + if (status.arming_state == ARMING_STATE_INIT && low_prio_task == LOW_PRIO_TASK_NONE) { // XXX check for sensors - arming_state_transition(&status, ARMING_STATE_STANDBY, &armed, mavlink_fd); - //TODO publish state + arming_state_transition(&status, ARMING_STATE_STANDBY, &armed); } else { // XXX: Add emergency stuff if sensors are lost @@ -950,14 +895,14 @@ int commander_thread_main(int argc, char *argv[]) /* check for global or local position updates, set a timeout of 2s */ - if (hrt_absolute_time() - last_global_position_time < 2000000 && hrt_absolute_time() > 2000000 && global_position.valid) { + if (t - global_position.timestamp < 2000000 && t > 2000000 && global_position.valid) { status.condition_global_position_valid = true; } else { status.condition_global_position_valid = false; } - if (hrt_absolute_time() - last_local_position_time < 2000000 && hrt_absolute_time() > 2000000 && local_position.valid) { + if (t - local_position.timestamp < 2000000 && t > 2000000 && local_position.valid) { status.condition_local_position_valid = true; } else { @@ -965,66 +910,23 @@ int commander_thread_main(int argc, char *argv[]) } /* Check for valid airspeed/differential pressure measurements */ - if (hrt_absolute_time() - last_diff_pres_time < 2000000 && hrt_absolute_time() > 2000000) { + if (t - last_diff_pres_time < 2000000 && t > 2000000) { status.condition_airspeed_valid = true; } else { status.condition_airspeed_valid = false; } - /* - * Consolidate global position and local position valid flags - * for vector flight mode. - */ - // if (current_status.condition_local_position_valid || - // current_status.condition_global_position_valid) { - // current_status.flag_vector_flight_mode_ok = true; - - // } else { - // current_status.flag_vector_flight_mode_ok = false; - // } - - /* consolidate state change, flag as changed if required */ - if (global_pos_valid != status.condition_global_position_valid || - local_pos_valid != status.condition_local_position_valid || - airspeed_valid != status.condition_airspeed_valid) { - state_changed = true; - } - - /* - * Mark the position of the first position lock as return to launch (RTL) - * position. The MAV will return here on command or emergency. - * - * Conditions: - * - * 1) The system aquired position lock just now - * 2) The system has not aquired position lock before - * 3) The system is not armed (on the ground) - */ - // if (!current_status.flag_valid_launch_position && - // !vector_flight_mode_ok && current_status.flag_vector_flight_mode_ok && - // !current_status.flag_system_armed) { - // first time a valid position, store it and emit it - - // // XXX implement storage and publication of RTL position - // current_status.flag_valid_launch_position = true; - // current_status.flag_auto_flight_mode_ok = true; - // state_changed = true; - // } - - orb_check(gps_sub, &new_data); - - if (new_data) { - + orb_check(gps_sub, &updated); + if (updated) { orb_copy(ORB_ID(vehicle_gps_position), gps_sub, &gps_position); /* check for first, long-term and valid GPS lock -> set home position */ float hdop_m = gps_position.eph_m; float vdop_m = gps_position.epv_m; - /* check if gps fix is ok */ - // XXX magic number + /* check if GPS fix is ok */ float hdop_threshold_m = 4.0f; float vdop_threshold_m = 8.0f; @@ -1038,15 +940,12 @@ int commander_thread_main(int argc, char *argv[]) * position to the current position. */ - if (gps_position.fix_type == GPS_FIX_TYPE_3D - && (hdop_m < hdop_threshold_m) - && (vdop_m < vdop_threshold_m) // XXX note that vdop is 0 for mtk - && !home_position_set - && (hrt_absolute_time() - gps_position.timestamp_position < 2000000) + if (!home_position_set && gps_position.fix_type == GPS_FIX_TYPE_3D && + (hdop_m < hdop_threshold_m) && (vdop_m < vdop_threshold_m) && // XXX note that vdop is 0 for mtk + (t - gps_position.timestamp_position < 2000000) && !armed.armed) { - warnx("setting home position"); - /* copy position data to uORB home message, store it locally as well */ + // TODO use global position estimate home.lat = gps_position.lat; home.lon = gps_position.lon; home.alt = gps_position.alt; @@ -1057,6 +956,11 @@ int commander_thread_main(int argc, char *argv[]) home.s_variance_m_s = gps_position.s_variance_m_s; home.p_variance_m = gps_position.p_variance_m; + double home_lat_d = home.lat * 1e-7; + double home_lon_d = home.lon * 1e-7; + warnx("home: lat = %.7f, lon = %.7f", home_lat_d, home_lon_d); + mavlink_log_info(mavlink_fd, "[cmd] home: %.7f, %.7f", home_lat_d, home_lon_d); + /* announce new home position */ if (home_pub > 0) { orb_publish(ORB_ID(home_position), home_pub, &home); @@ -1073,16 +977,18 @@ int commander_thread_main(int argc, char *argv[]) /* ignore RC signals if in offboard control mode */ if (!status.offboard_control_signal_found_once && sp_man.timestamp != 0) { - /* start RC state check */ - if ((hrt_absolute_time() - sp_man.timestamp) < 100000) { + /* start RC input check */ + if ((t - sp_man.timestamp) < 100000) { /* handle the case where RC signal was regained */ if (!status.rc_signal_found_once) { status.rc_signal_found_once = true; mavlink_log_critical(mavlink_fd, "[cmd] detected RC signal first time"); + status_changed = true; } else { if (status.rc_signal_lost) { - mavlink_log_critical(mavlink_fd, "[cmd] RECOVERY - RC SIGNAL GAINED!"); + mavlink_log_critical(mavlink_fd, "[cmd] RC signal regained"); + status_changed = true; } } @@ -1093,8 +999,6 @@ int commander_thread_main(int argc, char *argv[]) transition_result_t res; // store all transitions results here /* arm/disarm by RC */ - bool arming_state_changed; - res = TRANSITION_NOT_CHANGED; /* check if left stick is in lower left position and we are in MANUAL or AUTO mode -> disarm @@ -1106,16 +1010,15 @@ int commander_thread_main(int argc, char *argv[]) if (stick_off_counter > STICK_ON_OFF_COUNTER_LIMIT) { /* disarm to STANDBY if ARMED or to STANDBY_ERROR if ARMED_ERROR */ arming_state_t new_arming_state = (status.arming_state == ARMING_STATE_ARMED ? ARMING_STATE_STANDBY : ARMING_STATE_STANDBY_ERROR); - res = arming_state_transition(&status, new_arming_state, &armed, mavlink_fd); - - if (res == TRANSITION_CHANGED) - stick_off_counter = 0; + res = arming_state_transition(&status, new_arming_state, &armed); + stick_off_counter = 0; } else { stick_off_counter++; - stick_on_counter = 0; } + stick_on_counter = 0; + } else { stick_off_counter = 0; } @@ -1126,16 +1029,15 @@ int commander_thread_main(int argc, char *argv[]) status.main_state == MAIN_STATE_MANUAL) { if (sp_man.yaw > STICK_ON_OFF_LIMIT && sp_man.throttle < STICK_THRUST_RANGE * 0.1f) { if (stick_on_counter > STICK_ON_OFF_COUNTER_LIMIT) { - res = arming_state_transition(&status, ARMING_STATE_ARMED, &armed, mavlink_fd); - - if (res == TRANSITION_CHANGED) - stick_on_counter = 0; + res = arming_state_transition(&status, ARMING_STATE_ARMED, &armed); + stick_on_counter = 0; } else { stick_on_counter++; - stick_off_counter = 0; } + stick_off_counter = 0; + } else { stick_on_counter = 0; } @@ -1148,9 +1050,6 @@ int commander_thread_main(int argc, char *argv[]) } else { mavlink_log_info(mavlink_fd, "[cmd] DISARMED by RC"); } - - tune_positive(); - orb_publish(ORB_ID(actuator_armed), armed_pub, &armed); } /* fill current_status according to mode switches */ @@ -1159,83 +1058,54 @@ int commander_thread_main(int argc, char *argv[]) /* evaluate the main state machine */ res = check_main_state_machine(&status); - /* we should not get DENIED here */ - if (res == TRANSITION_DENIED) { + if (res == TRANSITION_CHANGED) { + mavlink_log_info(mavlink_fd, "[cmd] main state: %d", status.main_state); + tune_positive(); + + } else if (res == TRANSITION_DENIED) { + /* DENIED here indicates bug in the commander */ + warnx("ERROR: main denied: arm %d main %d mode_sw %d", status.arming_state, status.main_state, status.mode_switch); mavlink_log_critical(mavlink_fd, "[cmd] ERROR: main denied: arm %d main %d mode_sw %d", status.arming_state, status.main_state, status.mode_switch); } - bool main_state_changed = (res == TRANSITION_CHANGED); - - /* evaluate the navigation state machine */ - res = check_navigation_state_machine(&status, &control_mode); - - /* we should not get DENIED here */ - if (res == TRANSITION_DENIED) { - mavlink_log_critical(mavlink_fd, "[cmd] ERROR: nav denied: arm %d main %d nav %d", status.arming_state, status.main_state, status.navigation_state); - } - - bool navigation_state_changed = (res == TRANSITION_CHANGED); - - if (arming_state_changed || main_state_changed || navigation_state_changed) { - /* publish new vehicle status */ - status.counter++; - status.timestamp = hrt_absolute_time(); - orb_publish(ORB_ID(vehicle_status), status_pub, &status); - mavlink_log_info(mavlink_fd, "[cmd] state: arm %s, main %d, nav %d", status.arming_state, status.main_state, status.navigation_state); - } - - if (navigation_state_changed) { - /* publish new navigation state */ - control_mode.counter++; - control_mode.timestamp = hrt_absolute_time(); - orb_publish(ORB_ID(vehicle_control_mode), control_mode_pub, &control_mode); - } - } else { - /* print error message for first RC glitch and then every 5 s / 5000 ms) */ - if (!status.rc_signal_cutting_off || ((hrt_absolute_time() - last_print_time) > 5000000)) { - /* only complain if the offboard control is NOT active */ - status.rc_signal_cutting_off = true; - mavlink_log_critical(mavlink_fd, "CRITICAL - NO REMOTE SIGNAL!"); - + /* print error message for first RC glitch and then every 5s */ + if (!status.rc_signal_cutting_off || (t - last_print_control_time) > PRINT_INTERVAL) { + // TODO remove debug code if (!status.rc_signal_cutting_off) { - printf("Reason: not rc_signal_cutting_off\n"); + warnx("Reason: not rc_signal_cutting_off\n"); } else { - printf("last print time: %llu\n", last_print_time); + warnx("last print time: %llu\n", last_print_control_time); } - last_print_time = hrt_absolute_time(); + /* only complain if the offboard control is NOT active */ + status.rc_signal_cutting_off = true; + mavlink_log_critical(mavlink_fd, "[cmd] CRITICAL: NO RC CONTROL"); + + last_print_control_time = t; } /* flag as lost and update interval since when the signal was lost (to initiate RTL after some time) */ - status.rc_signal_lost_interval = hrt_absolute_time() - sp_man.timestamp; + status.rc_signal_lost_interval = t - sp_man.timestamp; /* if the RC signal is gone for a full second, consider it lost */ if (status.rc_signal_lost_interval > 1000000) { status.rc_signal_lost = true; status.failsave_lowlevel = true; - state_changed = true; + status_changed = true; } - - // if (hrt_absolute_time() - current_status.failsave_ll_start_time > failsafe_lowlevel_timeout_ms*1000) { - // publish_armed_status(¤t_status); - // } } } - - - - /* End mode switch */ - + /* END mode switch */ /* END RC state check */ - - /* State machine update for offboard control */ + // TODO check this + /* state machine update for offboard control */ if (!status.rc_signal_found_once && sp_offboard.timestamp != 0) { - if ((hrt_absolute_time() - sp_offboard.timestamp) < 5000000) { + if ((t - sp_offboard.timestamp) < 5000000) { // TODO 5s is too long ? // /* decide about attitude control flag, enable in att/pos/vel */ // bool attitude_ctrl_enabled = (sp_offboard.mode == OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE || @@ -1282,87 +1152,94 @@ int commander_thread_main(int argc, char *argv[]) // XXX check if this is correct /* arm / disarm on request */ if (sp_offboard.armed && !armed.armed) { - - arming_state_transition(&status, ARMING_STATE_ARMED, &armed, mavlink_fd); - // TODO publish state + arming_state_transition(&status, ARMING_STATE_ARMED, &armed); } else if (!sp_offboard.armed && armed.armed) { - - arming_state_transition(&status, ARMING_STATE_STANDBY, &armed, mavlink_fd); - // TODO publish state + arming_state_transition(&status, ARMING_STATE_STANDBY, &armed); } } else { - /* print error message for first RC glitch and then every 5 s / 5000 ms) */ - if (!status.offboard_control_signal_weak || ((hrt_absolute_time() - last_print_time) > 5000000)) { + /* print error message for first offboard signal glitch and then every 5s */ + if (!status.offboard_control_signal_weak || ((t - last_print_control_time) > PRINT_INTERVAL)) { status.offboard_control_signal_weak = true; - mavlink_log_critical(mavlink_fd, "CRIT:NO OFFBOARD CONTROL!"); - last_print_time = hrt_absolute_time(); + mavlink_log_critical(mavlink_fd, "[cmd] CRITICAL: NO OFFBOARD CONTROL"); + last_print_control_time = t; } /* flag as lost and update interval since when the signal was lost (to initiate RTL after some time) */ - status.offboard_control_signal_lost_interval = hrt_absolute_time() - sp_offboard.timestamp; + status.offboard_control_signal_lost_interval = t - sp_offboard.timestamp; /* if the signal is gone for 0.1 seconds, consider it lost */ if (status.offboard_control_signal_lost_interval > 100000) { status.offboard_control_signal_lost = true; - status.failsave_lowlevel_start_time = hrt_absolute_time(); + status.failsave_lowlevel_start_time = t; tune_positive(); /* kill motors after timeout */ - if (hrt_absolute_time() - status.failsave_lowlevel_start_time > failsafe_lowlevel_timeout_ms * 1000) { + if (t - status.failsave_lowlevel_start_time > failsafe_lowlevel_timeout_ms * 1000) { status.failsave_lowlevel = true; - state_changed = true; + status_changed = true; } } } } + /* evaluate the navigation state machine */ + transition_result_t res = check_navigation_state_machine(&status, &control_mode); - - - status.counter++; - status.timestamp = hrt_absolute_time(); - - - // XXX this is missing - /* If full run came back clean, transition to standby */ - // if (current_status.state_machine == SYSTEM_STATE_PREFLIGHT && - // current_status.flag_preflight_gyro_calibration == false && - // current_status.flag_preflight_mag_calibration == false && - // current_status.flag_preflight_accel_calibration == false) { - // /* All ok, no calibration going on, go to standby */ - // do_state_update(status_pub, ¤t_status, mavlink_fd, SYSTEM_STATE_STANDBY); - // } - - /* publish at least with 1 Hz */ - if (counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0 || state_changed) { - - orb_publish(ORB_ID(vehicle_status), status_pub, &status); - state_changed = false; + if (res == TRANSITION_DENIED) { + /* DENIED here indicates bug in the commander */ + warnx("ERROR: nav denied: arm %d main %d nav %d", status.arming_state, status.main_state, status.navigation_state); + mavlink_log_critical(mavlink_fd, "[cmd] ERROR: nav denied: arm %d main %d nav %d", status.arming_state, status.main_state, status.navigation_state); } + /* check which state machines for changes, clear "changed" flag */ + bool arming_state_changed = check_arming_state_changed(); + bool main_state_changed = check_main_state_changed(); + bool navigation_state_changed = check_navigation_state_changed(); + if (arming_state_changed || main_state_changed || navigation_state_changed) { + mavlink_log_info(mavlink_fd, "[cmd] state: arm %d, main %d, nav %d", status.arming_state, status.main_state, status.navigation_state); + status_changed = true; + } - /* Store old modes to detect and act on state transitions */ - voltage_previous = status.battery_voltage; + /* publish arming state */ + if (arming_state_changed) { + armed.timestamp = t; + orb_publish(ORB_ID(actuator_armed), armed_pub, &armed); + } + /* publish control mode */ + if (navigation_state_changed) { + /* publish new navigation state */ + control_mode.counter++; + control_mode.timestamp = t; + orb_publish(ORB_ID(vehicle_control_mode), control_mode_pub, &control_mode); + } - /* play tone according to evaluation result */ - /* check if we recently armed */ - if (!arm_tune_played && armed.armed && (!safety.safety_switch_available || (safety.safety_off && safety.safety_switch_available))) { + /* publish vehicle status at least with 1 Hz */ + if (counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0 || status_changed) { + status.counter++; + status.timestamp = t; + orb_publish(ORB_ID(vehicle_status), status_pub, &status); + status_changed = false; + } + + /* play arming and battery warning tunes */ + if (!arm_tune_played && armed.armed && (!safety.safety_switch_available || safety.safety_off)) { + /* play tune when armed */ if (tune_arm() == OK) arm_tune_played = true; - /* Trigger audio event for low battery */ - - } else if (status.battery_remaining < 0.1f && status.condition_battery_voltage_valid) { - if (tune_critical_bat() == OK) + } else if (status.battery_warning == VEHICLE_BATTERY_WARNING_WARNING) { + /* play tune on battery warning */ + if (tune_low_bat() == OK) battery_tune_played = true; - } else if (status.battery_remaining < 0.2f && status.condition_battery_voltage_valid) { - if (tune_low_bat() == OK) + } else if (status.battery_remaining == VEHICLE_BATTERY_WARNING_ALERT) { + /* play tune on battery critical */ + if (tune_critical_bat() == OK) battery_tune_played = true; } else if (battery_tune_played) { @@ -1375,8 +1252,10 @@ int commander_thread_main(int argc, char *argv[]) arm_tune_played = false; } + /* store old modes to detect and act on state transitions */ + battery_warning_previous = status.battery_warning; + armed_previous = armed.armed; - /* XXX use this voltage_previous */ fflush(stdout); counter++; usleep(COMMANDER_MONITORING_INTERVAL); @@ -1409,6 +1288,46 @@ int commander_thread_main(int argc, char *argv[]) return 0; } +void +toggle_status_leds(vehicle_status_s *status, actuator_armed_s *armed, vehicle_gps_position_s *gps_position) +{ + if (leds_counter % 2 == 0) { + /* run at 10Hz, full cycle is 16 ticks = 10/16Hz */ + if (armed->armed) { + /* armed, solid */ + led_on(LED_AMBER); + + } else if (armed->ready_to_arm) { + /* ready to arm, blink at 2.5Hz */ + if (leds_counter % 8 == 0) { + led_toggle(LED_AMBER); + } + + } else { + /* not ready to arm, blink at 10Hz */ + led_toggle(LED_AMBER); + } + + if (status->condition_global_position_valid) { + /* position estimated, solid */ + led_on(LED_BLUE); + + } else if (leds_counter == 0) { + /* waiting for position estimate, short blink at 1.25Hz */ + led_on(LED_BLUE); + + } else { + /* no position estimator available, off */ + led_off(LED_BLUE); + } + } + + leds_counter++; + + if (leds_counter >= 16) + leds_counter = 0; +} + void check_mode_switches(struct manual_control_setpoint_s *sp_man, struct vehicle_status_s *current_status) { @@ -1420,8 +1339,11 @@ check_mode_switches(struct manual_control_setpoint_s *sp_man, struct vehicle_sta } else if (sp_man->mode_switch > STICK_ON_OFF_LIMIT) { current_status->mode_switch = MODE_SWITCH_AUTO; - } else { + } else if (sp_man->mode_switch < -STICK_ON_OFF_LIMIT) { current_status->mode_switch = MODE_SWITCH_MANUAL; + + } else { + current_status->mode_switch = MODE_SWITCH_ASSISTED; } /* land switch */ @@ -1466,44 +1388,49 @@ check_main_state_machine(struct vehicle_status_s *current_status) switch (current_status->mode_switch) { case MODE_SWITCH_MANUAL: - res = main_state_transition(current_status, MAIN_STATE_MANUAL, mavlink_fd); + res = main_state_transition(current_status, MAIN_STATE_MANUAL); // TRANSITION_DENIED is not possible here break; case MODE_SWITCH_ASSISTED: if (current_status->assisted_switch == ASSISTED_SWITCH_EASY) { - res = main_state_transition(current_status, MAIN_STATE_EASY, mavlink_fd); + res = main_state_transition(current_status, MAIN_STATE_EASY); if (res != TRANSITION_DENIED) break; // changed successfully or already in this state // else fallback to SEATBELT + print_reject_mode("EASY"); } - res = main_state_transition(current_status, MAIN_STATE_SEATBELT, mavlink_fd); + res = main_state_transition(current_status, MAIN_STATE_SEATBELT); if (res != TRANSITION_DENIED) break; // changed successfully or already in this mode + if (current_status->assisted_switch != ASSISTED_SWITCH_EASY) // don't print both messages + print_reject_mode("SEATBELT"); + // else fallback to MANUAL - res = main_state_transition(current_status, MAIN_STATE_MANUAL, mavlink_fd); + res = main_state_transition(current_status, MAIN_STATE_MANUAL); // TRANSITION_DENIED is not possible here break; case MODE_SWITCH_AUTO: - res = main_state_transition(current_status, MAIN_STATE_AUTO, mavlink_fd); + res = main_state_transition(current_status, MAIN_STATE_AUTO); if (res != TRANSITION_DENIED) break; // changed successfully or already in this state // else fallback to SEATBELT (EASY likely will not work too) - res = main_state_transition(current_status, MAIN_STATE_SEATBELT, mavlink_fd); + print_reject_mode("AUTO"); + res = main_state_transition(current_status, MAIN_STATE_SEATBELT); if (res != TRANSITION_DENIED) break; // changed successfully or already in this state // else fallback to MANUAL - res = main_state_transition(current_status, MAIN_STATE_MANUAL, mavlink_fd); + res = main_state_transition(current_status, MAIN_STATE_MANUAL); // TRANSITION_DENIED is not possible here break; @@ -1514,6 +1441,20 @@ check_main_state_machine(struct vehicle_status_s *current_status) return res; } +void +print_reject_mode(const char *msg) +{ + hrt_abstime t = hrt_absolute_time(); + + if (t - last_print_mode_reject_time > PRINT_MODE_REJECT_INTERVAL) { + last_print_mode_reject_time = t; + char s[80]; + sprintf(s, "[cmd] WARNING: reject %s", msg); + mavlink_log_critical(mavlink_fd, s); + tune_negative(); + } +} + transition_result_t check_navigation_state_machine(struct vehicle_status_s *current_status, struct vehicle_control_mode_s *control_mode) { @@ -1523,15 +1464,15 @@ check_navigation_state_machine(struct vehicle_status_s *current_status, struct v /* ARMED */ switch (current_status->main_state) { case MAIN_STATE_MANUAL: - res = navigation_state_transition(current_status, current_status->is_rotary_wing ? NAVIGATION_STATE_STABILIZE : NAVIGATION_STATE_DIRECT, control_mode, mavlink_fd); + res = navigation_state_transition(current_status, current_status->is_rotary_wing ? NAVIGATION_STATE_STABILIZE : NAVIGATION_STATE_DIRECT, control_mode); break; case MAIN_STATE_SEATBELT: - res = navigation_state_transition(current_status, NAVIGATION_STATE_ALTHOLD, control_mode, mavlink_fd); + res = navigation_state_transition(current_status, NAVIGATION_STATE_ALTHOLD, control_mode); break; case MAIN_STATE_EASY: - res = navigation_state_transition(current_status, NAVIGATION_STATE_VECTOR, control_mode, mavlink_fd); + res = navigation_state_transition(current_status, NAVIGATION_STATE_VECTOR, control_mode); break; case MAIN_STATE_AUTO: @@ -1539,7 +1480,7 @@ check_navigation_state_machine(struct vehicle_status_s *current_status, struct v /* don't act while taking off */ if (current_status->condition_landed) { /* if landed: transitions only to AUTO_READY are allowed */ - res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_READY, control_mode, mavlink_fd); + res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_READY, control_mode); // TRANSITION_DENIED is not possible here break; @@ -1547,16 +1488,16 @@ check_navigation_state_machine(struct vehicle_status_s *current_status, struct v /* if not landed: act depending on switches */ if (current_status->return_switch == RETURN_SWITCH_RETURN) { /* RTL */ - res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_RTL, control_mode, mavlink_fd); + res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_RTL, control_mode); } else { if (current_status->mission_switch == MISSION_SWITCH_MISSION) { /* MISSION */ - res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_MISSION, control_mode, mavlink_fd); + res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_MISSION, control_mode); } else { /* LOITER */ - res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_LOITER, control_mode, mavlink_fd); + res = navigation_state_transition(current_status, NAVIGATION_STATE_AUTO_LOITER, control_mode); } } } @@ -1570,7 +1511,7 @@ check_navigation_state_machine(struct vehicle_status_s *current_status, struct v } else { /* DISARMED */ - res = navigation_state_transition(current_status, NAVIGATION_STATE_STANDBY, control_mode, mavlink_fd); + res = navigation_state_transition(current_status, NAVIGATION_STATE_STANDBY, control_mode); } return res; @@ -1579,75 +1520,61 @@ check_navigation_state_machine(struct vehicle_status_s *current_status, struct v void *commander_low_prio_loop(void *arg) { /* Set thread name */ - prctl(PR_SET_NAME, "commander low prio", getpid()); + prctl(PR_SET_NAME, "commander_low_prio", getpid()); while (!thread_should_exit) { switch (low_prio_task) { - case LOW_PRIO_TASK_PARAM_LOAD: if (0 == param_load_default()) { - mavlink_log_info(mavlink_fd, "Param load success"); + mavlink_log_info(mavlink_fd, "[cmd] parameters loaded"); } else { - mavlink_log_critical(mavlink_fd, "Param load ERROR"); + mavlink_log_critical(mavlink_fd, "[cmd] parameters load ERROR"); tune_error(); } - - low_prio_task = LOW_PRIO_TASK_NONE; break; case LOW_PRIO_TASK_PARAM_SAVE: if (0 == param_save_default()) { - mavlink_log_info(mavlink_fd, "Param save success"); + mavlink_log_info(mavlink_fd, "[cmd] parameters saved"); } else { - mavlink_log_critical(mavlink_fd, "Param save ERROR"); + mavlink_log_critical(mavlink_fd, "[cmd] parameters save error"); tune_error(); } - - low_prio_task = LOW_PRIO_TASK_NONE; break; case LOW_PRIO_TASK_GYRO_CALIBRATION: do_gyro_calibration(mavlink_fd); - - low_prio_task = LOW_PRIO_TASK_NONE; break; case LOW_PRIO_TASK_MAG_CALIBRATION: do_mag_calibration(mavlink_fd); - - low_prio_task = LOW_PRIO_TASK_NONE; break; case LOW_PRIO_TASK_ALTITUDE_CALIBRATION: // do_baro_calibration(mavlink_fd); + break; case LOW_PRIO_TASK_RC_CALIBRATION: // do_rc_calibration(mavlink_fd); - - low_prio_task = LOW_PRIO_TASK_NONE; break; case LOW_PRIO_TASK_ACCEL_CALIBRATION: do_accel_calibration(mavlink_fd); - - low_prio_task = LOW_PRIO_TASK_NONE; break; case LOW_PRIO_TASK_AIRSPEED_CALIBRATION: do_airspeed_calibration(mavlink_fd); - - low_prio_task = LOW_PRIO_TASK_NONE; break; case LOW_PRIO_TASK_NONE: @@ -1657,6 +1584,8 @@ void *commander_low_prio_loop(void *arg) break; } + low_prio_task = LOW_PRIO_TASK_NONE; + } return 0; diff --git a/src/modules/commander/commander_helper.cpp b/src/modules/commander/commander_helper.cpp index 9427bd8925..681a11568f 100644 --- a/src/modules/commander/commander_helper.cpp +++ b/src/modules/commander/commander_helper.cpp @@ -213,7 +213,7 @@ float battery_remaining_estimate_voltage(float voltage) ret = (voltage - ncells * chemistry_voltage_empty) / (ncells * (chemistry_voltage_full - chemistry_voltage_empty)); /* limit to sane values */ - ret = (ret < 0) ? 0 : ret; - ret = (ret > 1) ? 1 : ret; + ret = (ret < 0.0f) ? 0.0f : ret; + ret = (ret > 1.0f) ? 1.0f : ret; return ret; -} \ No newline at end of file +} diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 043ccfd0c9..06cd060c5d 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -62,8 +62,12 @@ #endif static const int ERROR = -1; +static bool arming_state_changed = true; +static bool main_state_changed = true; +static bool navigation_state_changed = true; + transition_result_t -arming_state_transition(struct vehicle_status_s *status, arming_state_t new_arming_state, struct actuator_armed_s *armed, const int mavlink_fd) +arming_state_transition(struct vehicle_status_s *status, arming_state_t new_arming_state, struct actuator_armed_s *armed) { transition_result_t ret = TRANSITION_DENIED; @@ -96,9 +100,6 @@ arming_state_transition(struct vehicle_status_s *status, arming_state_t new_armi ret = TRANSITION_CHANGED; armed->armed = false; armed->ready_to_arm = true; - - } else { - mavlink_log_critical(mavlink_fd, "rej. STANDBY state, sensors not initialized"); } } @@ -163,18 +164,28 @@ arming_state_transition(struct vehicle_status_s *status, arming_state_t new_armi } if (ret == TRANSITION_CHANGED) { - hrt_abstime t = hrt_absolute_time(); status->arming_state = new_arming_state; - armed->timestamp = t; + arming_state_changed = true; } } return ret; } +bool +check_arming_state_changed() +{ + if (arming_state_changed) { + arming_state_changed = false; + return true; + + } else { + return false; + } +} transition_result_t -main_state_transition(struct vehicle_status_s *current_state, main_state_t new_main_state, const int mavlink_fd) +main_state_transition(struct vehicle_status_s *current_state, main_state_t new_main_state) { transition_result_t ret = TRANSITION_DENIED; @@ -193,11 +204,7 @@ main_state_transition(struct vehicle_status_s *current_state, main_state_t new_m /* need position estimate */ // TODO only need altitude estimate really - if (!current_state->condition_local_position_valid) { - mavlink_log_critical(mavlink_fd, "rej. SEATBELT: no position estimate"); - tune_negative(); - - } else { + if (current_state->condition_local_position_valid) { ret = TRANSITION_CHANGED; } @@ -206,11 +213,7 @@ main_state_transition(struct vehicle_status_s *current_state, main_state_t new_m case MAIN_STATE_EASY: /* need position estimate */ - if (!current_state->condition_local_position_valid) { - mavlink_log_critical(mavlink_fd, "rej. EASY: no position estimate"); - tune_negative(); - - } else { + if (current_state->condition_local_position_valid) { ret = TRANSITION_CHANGED; } @@ -219,11 +222,7 @@ main_state_transition(struct vehicle_status_s *current_state, main_state_t new_m case MAIN_STATE_AUTO: /* need position estimate */ - if (!current_state->condition_local_position_valid) { - mavlink_log_critical(mavlink_fd, "rej. AUTO: no position estimate"); - tune_negative(); - - } else { + if (current_state->condition_local_position_valid) { ret = TRANSITION_CHANGED; } @@ -232,14 +231,27 @@ main_state_transition(struct vehicle_status_s *current_state, main_state_t new_m if (ret == TRANSITION_CHANGED) { current_state->main_state = new_main_state; + main_state_changed = true; } } return ret; } +bool +check_main_state_changed() +{ + if (main_state_changed) { + main_state_changed = false; + return true; + + } else { + return false; + } +} + transition_result_t -navigation_state_transition(struct vehicle_status_s *current_status, navigation_state_t new_navigation_state, struct vehicle_control_mode_s *control_mode, const int mavlink_fd) +navigation_state_transition(struct vehicle_status_s *current_status, navigation_state_t new_navigation_state, struct vehicle_control_mode_s *control_mode) { transition_result_t ret = TRANSITION_DENIED; @@ -395,12 +407,24 @@ navigation_state_transition(struct vehicle_status_s *current_status, navigation_ if (ret == TRANSITION_CHANGED) { current_status->navigation_state = new_navigation_state; + navigation_state_changed = true; } } return ret; } +bool +check_navigation_state_changed() +{ + if (navigation_state_changed) { + navigation_state_changed = false; + return true; + + } else { + return false; + } +} /** * Transition from one hil state to another diff --git a/src/modules/commander/state_machine_helper.h b/src/modules/commander/state_machine_helper.h index b59b66c8ba..c8c77e5d8a 100644 --- a/src/modules/commander/state_machine_helper.h +++ b/src/modules/commander/state_machine_helper.h @@ -56,11 +56,17 @@ typedef enum { } transition_result_t; -transition_result_t arming_state_transition(struct vehicle_status_s *current_state, arming_state_t new_arming_state, struct actuator_armed_s *armed, const int mavlink_fd); +transition_result_t arming_state_transition(struct vehicle_status_s *current_state, arming_state_t new_arming_state, struct actuator_armed_s *armed); -transition_result_t main_state_transition(struct vehicle_status_s *current_state, main_state_t new_main_state, const int mavlink_fd); +bool check_arming_state_changed(); -transition_result_t navigation_state_transition(struct vehicle_status_s *current_status, navigation_state_t new_navigation_state, struct vehicle_control_mode_s *control_mode, const int mavlink_fd); +transition_result_t main_state_transition(struct vehicle_status_s *current_state, main_state_t new_main_state); + +bool check_main_state_changed(); + +transition_result_t navigation_state_transition(struct vehicle_status_s *current_status, navigation_state_t new_navigation_state, struct vehicle_control_mode_s *control_mode); + +bool check_navigation_state_changed(); int hil_state_transition(hil_state_t new_state, int status_pub, struct vehicle_status_s *current_state, int control_mode_pub, struct vehicle_control_mode_s *current_control_mode, const int mavlink_fd); From 40c56ab61e04fe73aff3a84d20ffc81e102373f3 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Tue, 6 Aug 2013 21:10:41 +0200 Subject: [PATCH 095/109] Corrected bug in px4io driver that lead to hang of FMU-IO communication --- src/drivers/px4io/px4io.cpp | 2 -- 1 file changed, 2 deletions(-) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 827b0bb009..bc3f1dcc68 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -1008,8 +1008,6 @@ PX4IO::io_handle_status(uint16_t status) struct safety_s safety; safety.timestamp = hrt_absolute_time(); - orb_copy(ORB_ID(safety), _to_safety, &safety); - if (status & PX4IO_P_STATUS_FLAGS_SAFETY_OFF) { safety.safety_off = true; safety.safety_switch_available = true; From 32439d748ad169f6f9956fb3248535730e0374a4 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Wed, 7 Aug 2013 20:21:49 +0200 Subject: [PATCH 096/109] commander: set mode using base_mode and custom_mode --- src/modules/commander/commander.cpp | 58 ++++++++++++++++++++--------- 1 file changed, 41 insertions(+), 17 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 2012abcc09..7ede3e1e64 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -126,10 +126,6 @@ extern struct system_load_s system_load; #define PRINT_INTERVAL 5000000 #define PRINT_MODE_REJECT_INTERVAL 2000000 -// TODO include mavlink headers -/** @brief These flags encode the MAV mode. */ -#ifndef HAVE_ENUM_MAV_MODE_FLAG -#define HAVE_ENUM_MAV_MODE_FLAG enum MAV_MODE_FLAG { MAV_MODE_FLAG_CUSTOM_MODE_ENABLED = 1, /* 0b00000001 Reserved for future use. | */ MAV_MODE_FLAG_TEST_ENABLED = 2, /* 0b00000010 system has a test mode enabled. This flag is intended for temporary system tests and should not be used for stable implementations. | */ @@ -141,7 +137,13 @@ enum MAV_MODE_FLAG { MAV_MODE_FLAG_SAFETY_ARMED = 128, /* 0b10000000 MAV safety set to armed. Motors are enabled / running / can start. Ready to fly. | */ MAV_MODE_FLAG_ENUM_END = 129, /* | */ }; -#endif + +enum PX4_CUSTOM_MODE { + PX4_CUSTOM_MODE_MANUAL = 1, + PX4_CUSTOM_MODE_SEATBELT, + PX4_CUSTOM_MODE_EASY, + PX4_CUSTOM_MODE_AUTO, +}; /* Mavlink file descriptors */ static int mavlink_fd; @@ -277,8 +279,9 @@ void handle_command(struct vehicle_status_s *status, struct vehicle_control_mode /* request to set different system mode */ switch (cmd->command) { case VEHICLE_CMD_DO_SET_MODE: { - uint8_t base_mode = (int) cmd->param1; - uint8_t custom_mode = (int) cmd->param2; + uint8_t base_mode = (uint8_t) cmd->param1; + uint32_t custom_mode = (uint32_t) cmd->param2; + // TODO remove debug code mavlink_log_critical(mavlink_fd, "[cmd] command setmode: %d %d", base_mode, custom_mode); /* set arming state */ @@ -286,6 +289,7 @@ void handle_command(struct vehicle_status_s *status, struct vehicle_control_mode if (base_mode & MAV_MODE_FLAG_SAFETY_ARMED) { arming_res = arming_state_transition(status, ARMING_STATE_ARMED, armed); + if (arming_res == TRANSITION_CHANGED) { mavlink_log_info(mavlink_fd, "[cmd] ARMED by command"); } @@ -294,9 +298,11 @@ void handle_command(struct vehicle_status_s *status, struct vehicle_control_mode if (status->arming_state == ARMING_STATE_ARMED || status->arming_state == ARMING_STATE_ARMED_ERROR) { arming_state_t new_arming_state = (status->arming_state == ARMING_STATE_ARMED ? ARMING_STATE_STANDBY : ARMING_STATE_STANDBY_ERROR); arming_res = arming_state_transition(status, new_arming_state, armed); + if (arming_res == TRANSITION_CHANGED) { mavlink_log_info(mavlink_fd, "[cmd] DISARMED by command"); } + } else { arming_res = TRANSITION_NOT_CHANGED; } @@ -305,21 +311,37 @@ void handle_command(struct vehicle_status_s *status, struct vehicle_control_mode /* set main state */ transition_result_t main_res = TRANSITION_DENIED; - if (base_mode & MAV_MODE_FLAG_AUTO_ENABLED) { - /* AUTO */ - main_res = main_state_transition(status, MAIN_STATE_AUTO); + if (base_mode & MAV_MODE_FLAG_CUSTOM_MODE_ENABLED) { + /* use autopilot-specific mode */ + if (custom_mode == PX4_CUSTOM_MODE_MANUAL) { + /* MANUAL */ + main_res = main_state_transition(status, MAIN_STATE_MANUAL); - } else if (base_mode & MAV_MODE_FLAG_MANUAL_INPUT_ENABLED) { - if (base_mode & MAV_MODE_FLAG_GUIDED_ENABLED) { + } else if (custom_mode == PX4_CUSTOM_MODE_SEATBELT) { + /* SEATBELT */ + main_res = main_state_transition(status, MAIN_STATE_SEATBELT); + + } else if (custom_mode == PX4_CUSTOM_MODE_EASY) { /* EASY */ main_res = main_state_transition(status, MAIN_STATE_EASY); - } else if (base_mode & MAV_MODE_FLAG_STABILIZE_ENABLED) { - if (custom_mode & 1) { // TODO add custom mode flags definition - /* SEATBELT */ - main_res = main_state_transition(status, MAIN_STATE_SEATBELT); + } else if (custom_mode == PX4_CUSTOM_MODE_AUTO) { + /* AUTO */ + main_res = main_state_transition(status, MAIN_STATE_AUTO); + } - } else { + } else { + /* use base mode */ + if (base_mode & MAV_MODE_FLAG_AUTO_ENABLED) { + /* AUTO */ + main_res = main_state_transition(status, MAIN_STATE_AUTO); + + } else if (base_mode & MAV_MODE_FLAG_MANUAL_INPUT_ENABLED) { + if (base_mode & MAV_MODE_FLAG_GUIDED_ENABLED) { + /* EASY */ + main_res = main_state_transition(status, MAIN_STATE_EASY); + + } else if (base_mode & MAV_MODE_FLAG_STABILIZE_ENABLED) { /* MANUAL */ main_res = main_state_transition(status, MAIN_STATE_MANUAL); } @@ -1534,6 +1556,7 @@ void *commander_low_prio_loop(void *arg) mavlink_log_critical(mavlink_fd, "[cmd] parameters load ERROR"); tune_error(); } + break; case LOW_PRIO_TASK_PARAM_SAVE: @@ -1545,6 +1568,7 @@ void *commander_low_prio_loop(void *arg) mavlink_log_critical(mavlink_fd, "[cmd] parameters save error"); tune_error(); } + break; case LOW_PRIO_TASK_GYRO_CALIBRATION: From 36679fbb39a57139c03cce85d7d0fbd25383a98a Mon Sep 17 00:00:00 2001 From: Jean Cyr Date: Sat, 10 Aug 2013 11:22:08 -0400 Subject: [PATCH 097/109] Some DSM satellites are fussier about bind pulse timing These values work better --- src/drivers/px4io/px4io.cpp | 3 +-- src/modules/px4iofirmware/dsm.c | 4 ++-- 2 files changed, 3 insertions(+), 4 deletions(-) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index ae56b70b36..5089ce3c7f 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -1445,9 +1445,8 @@ PX4IO::ioctl(file * /*filep*/, int cmd, unsigned long arg) 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); + usleep(50000); io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_DSM, dsm_bind_send_pulses | (arg << 4)); break; diff --git a/src/modules/px4iofirmware/dsm.c b/src/modules/px4iofirmware/dsm.c index ab6e3fec43..b2c0db4254 100644 --- a/src/modules/px4iofirmware/dsm.c +++ b/src/modules/px4iofirmware/dsm.c @@ -125,9 +125,9 @@ dsm_bind(uint16_t cmd, int pulses) case dsm_bind_send_pulses: for (int i = 0; i < pulses; i++) { stm32_gpiowrite(usart1RxAsOutp, false); - up_udelay(50); + up_udelay(25); stm32_gpiowrite(usart1RxAsOutp, true); - up_udelay(50); + up_udelay(25); } break; case dsm_bind_reinit_uart: From 1d408b80ad70bd8ea873ce7215c8a92a62461b0f Mon Sep 17 00:00:00 2001 From: Jean Cyr Date: Sun, 11 Aug 2013 17:19:54 -0400 Subject: [PATCH 098/109] Support DSM bind via QGroundControl --- Documentation/dsm_bind.odt | Bin 27043 -> 27123 bytes Documentation/dsm_bind.pdf | Bin 34300 -> 323311 bytes src/drivers/px4io/px4io.cpp | 46 +++++++++++++++------------- src/modules/sensors/sensor_params.c | 1 + src/modules/sensors/sensors.cpp | 12 -------- 5 files changed, 26 insertions(+), 33 deletions(-) diff --git a/Documentation/dsm_bind.odt b/Documentation/dsm_bind.odt index 66ea1f1bee22eba9943f6077e7fcec6c6ef6e182..587a38883bff56f458ed0feed62a1731b69a4459 100644 GIT binary patch delta 5385 zcmZ9QRa6v!qD6;J=?+QhQo0n7ZWwat5~QV(7-%Nb7 z*Lvske4mefcF`=#)htS#78>|U@fvZA0sx?)0|3|n0Kmo0%Zks>#W@bBlCZ)^)?@e= zT-Ua;!Q&F@JXzh!rwi2o`=dX0U=9;6_h`76e~@jwIAd+YWDWPvtlGL=FU;RXLt`W~ z2|un$71F}CuE}tTHY|l(Byay}ZU;A4O4w~(?>2Crq^-f@7-qSK29DuD7tH0?(@?Z; zBR3Cll_$(C+VKIT@F2~{ZT@0peSE47m|qJ>s~X1mvXM$qbcA~Z`pFtWu*W6M8=IWm zJdvRZu@Jo%(ttPph`jWZni)!VKDlz0r=2N2-9OeKcXEJzBXuyl(owVTmfqeMV+Nh# zr6c;z<||^}8*#l62Rk0;SI9LVQfcCLd427xgGPMCKo4O6zYqHr(Pd`s=OW}dy*ndY zb!2Rg$i7d?zUoa!ps0b92<&!L8HH#KTs?2!+eeYEd}-N_dor@=rI<)Mql@a|(V`vn ztz@ZJBM1*Zko(6ok!wMh<8(RPVeS>tuowj&=ic_C?+0E>fXhRsyixMyI!niQ^?f!V z2+#yt7prPM&A`asQn0uS_2YwOdwnct_lgt}YwmUP*j5hKv>e}wLmeA!vH&}XJyMPN zmiukQ4ZCvyb}w&aBIRbdynFQWv2SOZr;n-w^?RE+NL|!nAu*ECR{1%hJJ2@=>*Kt=BI|Kd#Rt!Zv|&Qi*no6td6k@0)G}R z2_89={C*Pbc`v2jGV@TtE^q;J1y%{8Qw;))u)M+G4w??pH1nEbAXaCJf74A*js}~Z zU1NPcL?v5YuN15dbR|jHR?*|;YTiHG=#&X?Pe|+B4A><{UW>i9 zeyG>H*YPkd<0E`bc4fb&{4%ZKqc3Ug#2H~4X`$@_G1y!Era!-F1ZFQL6`4D;K1=!g zjX_434VN~<@uaY`Fi$a^!`V6I{Pc!rJo#T&%qU^yGfNaE2vf!@(unBCujhY#>L{XH zdhgPi`=B0{en>BpblUZ9!~@dSt8Xb!!>{?xxYxzSTzyEY0gFzjA^(b2kwVJQ!q9qA zv-%u|K3HXs!Vs3R^1B>$XC-xLf7&zaFzpY=D^ZBC25RitBe(jR39XA>L6xGbm*fdu z8o5Lwr|Jo5_|1=|Y<1EJUOTHC8u*n1*OuBB&Qhc&)5OIcbN1@wR4d6u47T6N zK?$IjT(>-|@N+t=qdEgi?Q785s;Arxr6X_U7(XqvSN^BN5N(0VuX&1>mrFi=8Whc; zM}1=-|9eZ;AfzR0(4{5o?rn;Dzac}}B=~il#&$~%_(t)^S(vniAvBr+-v~5#S5Q(r z_LC66AxxD{iwhO<7nAYyw`h0>0TDslDDLN#u3_(Q7zay*lqG2M;P#Y2v+t+_bKh4; zvjSI*faIK?$g@zAS{vM%-UhE#2)$a`|6zzos#foC3;5c<85%s!B{VZpH65XC0)|Ty zmlCZR;8qDLP-C|*l|S-bJI9`R%QjkGY$2RBiw%uSyGab*sLs-DI+BR22PtD?CGXTY zImWd8ODp<=$w6G; zu}Mr!I3F6`kjOFAByiFBiz3^dJI{@>gO)1e2OW&@O(Vk32hW?}Jm$rSd03Wd?Jyyd z;-kdcEMW}kR-aeqqYI~d9WB!`V$4U_a$Sz4o_9%} zGpCZzGV}7-Ndp5b`2stC#;NHO-XIYTBbZd}m1o|n9~%_$wbM`7wbb<}3^~`P#^xym zfiQ+gbUmYgR^@_VHE5j*3e`Q2F{_D<(vUh8x{7F_uW7k@W~+6ZXX#N08(YoEK2-A% zF$DjI9JXuh(N8_yo4yyzjf|2~4;@bm$XsPQo9=54T64z1YsQyQLJ+kIDdoCUgC>hi zOF)|c{H2}^+hMHPhG+wtvs6F*_Y$@DlKNLtoBUasc;&zWud}w&k~pyG8APxh9^;j2w*eiJqunUXtZ`gG6_6#U$3<`ajpt3)L@DjB$fc2wcA z=@RbYW4mCCZY?>?wJ*UxCJ(5S-$ntH#WlZF3L3*zj7mjG_+u>J;F8W}IDG@!s5Z5U zcrSW}3vos+20kJORtzt&updG^@cMA1vK`%kMNnMM5clzelm^AsA^Z*#8`p`l0*+8% zF_mpdBKz|*T2bzP0nsET>%#BpP&Y-`^=}?H+Aa7)GzEXczfCFid|67GTZM`O;F2 zH_n4%is7y^<}kYN)8mh5i|aYD8B;pqzSL3yfW+4O3*O7;Q>6ghgby9GjiYZZDjz?<}CFaKOMu3U!{ z+Y3uZoE0>6)H0Omii-NNLQ!wtHT7M;q}P{JBffQxUnoo@UUF32-|I)H4CCRG_&f=D_y3)+q64>sEa21=&Gg>?x|jmRH)@Pzm`~3!{aQ{a}J4z!BT6jRKK@325r0lc9)xoJL&pNeiMw=@_|J8w78+i}D!8SY5nHi#p8h=84a{xH1a)J%!LQEKbt2 z_=_0|rD`&+geVi)jYKEfVnljv&+D(e1V1Tgq(TIr9SzPady{}cSESZ9MLjs?3n8V7 z39f)JGtxz&y3{)g!|~RsW|8&m7ecU{(%1{I=^sA;0C;&p18A$Ep_2hH|MwCw0dNG{ zu8gn&fCzd302crNczXFe+j;(n4abbt+_uF?did*JWVLO+A;PRB6SCr`Phr*+jTxu1 zHf2}=rEiw^;!~b(9`!GnR1IeSo{qA~pf<+96K?n(5R+QJCBOlxrNEA*oytnP z+~Lx>G{;*AGGVfp!tJW6v9LnEnzns7&o1Ky`CJS3FY}euAMq|3oz)R(IXL#@?Zn^8 zcIBnp>OrRLDiykX~HwTHgWU^{6c$S=DQtL_OP} zW0|W4wQL|2J^pZ-+Ub z8^sh&lZh0Ue=zB_H`%Q~vz0ga#pi-p{`ub>M+)3A#lpU_^O&!5 zZ++z$^Tq87enj+vTQBEW)PLf+I-xM@+t=Y~v}GJ{hbG~YAGP#vN+t{Y52%M`KBGf! zJ#mZN!jJj4Sdt;W>Sy(F)4`BXmFjBrTv5y!-;s} z^Zp%pfUQ=;s@00S%PXNYQa-tpXLS=T6J^@+3$CQDN|1y|5qBH>n(Z`>l+Li#fF7en zcG)OLU!i0)I|UgmpqQtmT<3wajU7R<$v9lr1>z?z<%vr;zF@R7G3w5^l~K^nsd^4x zEvY&pb_b~@Z68!~Fx)EUWyhqJed!P`JOqKQwDH0acQ>8^98Ed}?4bo7}`z^c4 z2OKxMVSh!+9-%)RmK5jiW}?p8>GPwdZRxglw?+h`XO!qNsif!0`oNQHew&W8vo2YUe}IRk_U4f^^pZG z5eZiAA_%)AM!Az31#k{q%E>e|yw<_Hf9Q7xJ>a4$x_JX5)IO%BkN8@ts41!6YH#7b z@@3Cm=~@tEVZ)|oVBgYy2&UQh@~}NpU9oMuNIzvuoQC_?J3^F&S(h=kN_iw>9b1dO zKt(T;$gvZ-uqF|y6w!K|6BHF&Fvqbco@Cp&CSnXtl%++Zhtyk1L#b8^O$nUTx%qnJ zFtdP&hAMK32R;`bK4H#Mtx&+sA9bfy_%{*izUJ5-uGYTo*m@+J9qIe3PRwGOo{P?| zVwW&$g~q$)S#8U}1s_Xs-b(x+Ae$=fWGGQ|vXe74DZS*&19aYLazlTvl6y6qpDh6! z+)s0jPbKW}TjjW()r=>d0}VW%--z74s}nyD=O!rwmf919NAq56^~Z@S%$&VovqnbW z@OG+P_n>hlIEQ15i0|gq`-k)W0@I@7=qkik8LCg4I<*elh(}`#-Vf$SsJe@b{)N0A zB+7=W$-H<~uas#I21kTpy}{q~pWtcu#Od};ZQ~);*FP872WABcvw>@(qQpFIMFo)3 z?&#CqY)Av1d<^$9t&jAPkAu8LpkpV%jIiC|Zo*w_55SZxDqMt%;QP*f#S`<}&QZ_6 zV$SDKH?s4ES_~xXTZy}v;l#U>iA!VS+-Wg0f>)x9L1A#YRP9_VL|}@njZR+8zJ*;w zMp+F%^_cyw5g~3H6&U$lK)f*l2tJ-!WH7bv3Vw{KD2tw+i$^|{K4ZAmqg;izH>SRJ zp1|4I1wh9{nM{9Yxsb8eqs63nShQL+4=WSk{^Nd{d5OB=BN03NgmB=N>?>0`P1&56 zj14cWQi#e0mo>jp#2;vig6wVS%(3h7N?l6YS*ptjhQ+@!1J6139b#hBR7F$d9!skw zBFJ5wH9>kVsu9FPl*C(*Z?&l3?@D*LEM5t$G3^}*T$6fz@I_}(PoF8=L~CWxNh~?& zBnIB!MNvM4$eW6VDEJG8mv7c9r%g*B7jOBqx%HQByxBH_HRQzxW7YX}HUq+LH2+Yg zN}5%2O>U_=fF0LYUCvtnK+7B)Kl6SSSO|J28Y%MxFuWu=V4C8Mh$67<{4;Knar(9J z410aGCK!a6RS1|FEjy=rEHiKjNx+r9wWR&Cojf>kGTd`HMw;7TWbfB-$!{c-(f_D_ zIHD0as)|n`7xClbaNwPfC63llb5_&Re^_~~QqR-h`oNW6s&=eq?}1KaG?_b`XivE! znE_sRBh0!gr=3Ap9G*Cg)WaZI5#fBz<;8w1giJ8(ElAy%5{WM&6qDupEiP zCDuQ#LFW^ClYt~%&5`7K-|5S)d06;xHD~-VbG$|D2D-cW!OvuKqF@rxPi(O|wWAoN zNO&I0O7(dIh2aDII4fuCk<5c|WN9{Zz+*^$^H<7={V2}=Ny-$MgAxib1?Mvh?f(~6 c|4mnl(&yK-|DRd@8+gjM&-9oI4*zNX2a;lVrvLx| delta 5307 zcmZA5RaBG>pf%tT7`jVoVd(A->2es5P6v^0hOQToM!H)XM7p~}Mx?t#kQPbFv%df0 zKWm+Rxv%%dv-ZWzi=vqqwJ=m5M4P|Dd`3C-9E%#aT?9YI1qR`ev7pfV0&X-@v1eV-eAs zstQ#@-lX6}J=>Z2-4$fSL)1swFs;G{bUQb6SYGzc8M~jP9hQlq*fjv>J$s=@o}QgqE8`kScK?&A7z@I8*zv#y75Op4~8+WV@G*Mrna;zDY+Fo8||% z6jw}A(%imr*?5imtsr!$F*f9{i|FKNlEcY`odVT#@#+2{luW7--iy^({ja>FVV(9I zK~2PU14eRhdL8K~o&k)hNz_7~lRABl;^E0CJh`;I4%P)uL4=zl`8BUy>3PyK%&20G z+}qp<1G>fgeoGS9--RWJsBe zNux~dUSG<;oVIqoe%ReKNQTW{6>_8uNpm`xQH)#=<`pi`Qv#T#FG1{$_RA)DmKfbj z)t;xDC7LL6uk?f`u8~O8d*8CwgcxS26-j@LS=y!_+Zh;Ba@;32waipWyevj6;o1zI z9DHFXEDjwj)#Z{{EQ}%QvsMUi12OBe&@?OobG#W<$gS+C=7X6T?RETx$?rRF>wLgD zU5ViV0|a`0ZqNezXabfWQ*P6+K%iy_2!sOyfh?Sz+%28l|J%W?(VElBC|Nhpq}Qw3 zmn&d~?xVx(pCVyl@2w!iG?itc^y8Sir`@DO`qoHkm2{&#i4)k}(-|ooYS>>~+ze z{R$h^z;lOsswF=WQilo3#x9|%V|kvw)j7uL!-UkG{cTs#nrLNs{w|{gn)a1kcd=o0 z#x3SP{nYoF_j_Bj1zz<9=!SMXFW#!t=pwy}xweB$Kk^-%rxyw|#?_c=C?X zsI~$n%Gp^q5u6SlS>#>y%gwpXIAzuUmXzvmLTNj)y@foz%BjzoEaHCy{&t(_P2}hquBUw0DO-63qRdnE zv?tnmcW{8sywa81@{J#XSx%f%KH{4uLSnf})|UXsWeKIuA%`{2yBve#RLMTkCc^J* z6!ixi2=)9T^F97z7t+yZS^{*vFVM1_OkRKIa%cckt0X$w6pFmQu%SWm3BQ&4?#;kl zp@I!GbYB{r(m`+to7oa-3SV|6P;n(vI<+9<(8xFN^r`>B9qY+bCyDE(8mC{Wakzge zDldU<&bOGT^T*1x?4|O5Ag(DCi{V)WXLsZyntL}cY_54@M2IOOy2!cwiqy<<;aA$x zQt}imuGgGLr=O*UtPS%ToO=(z5?(nx(i4Ddna=e)Cxl&PujznK|?qwZL&EqOqka{^_|R zYpS(U-ECYw4HZR0orP);pv9U?7;}?SVnJ2tKY^xIn?9Mh!YFYVo6{AcIhh-~fe`@7 zYPx*s{?h$$axhU(oNxFPcq=kQXmz`tI5QL(5H&%EUAUo5 z2O*hRs8}N!C=I5hDACSR%7spBH(a!RGYE@v*!88(^Ix=^(_urW4MRzs>je0v;lT1~ zX!YsLX_ftvF#Foorx)*ti{2yA*IKnG+9?jlO)SE+!slW^M{$MMq18gavqS^~k7T|_ zqmLbTwA5s5>5+ythS(-zst!aVGdKL7=}l|E0;_&_N)#9)RC>b0^|5fOceoAFu2y!5 zDnF_+XJBkx;?M+5=TQf4Pk?ia%rUr_&Aw#(fUWZEXG4AF+@1-08rf`~0Hv3`E~y-B z={<*)SJPYXzR!)ZRPz+aDdDO+V!{nVFcT6+Xj}u8{N1Joqi)Zs4`8WjuT%fj z&0zh=#<4lhXM}ak1hA==Fd3SxZdr!DK!a68XDowSBFMenLd}(@urMt!#_^j5_$!d) zciDlo`wKBPYe)6^869YweXlqyJ6Uz?7d3OG5^TANGIg3^y)sVlc50F3!pH>%3H?~tWaB%`eVSO zSl$}FL)lW$5gbMMss!ZY`|~@$G{Uf!L}FqHkEBeTP>2sgO8Z0gK6=2@9%4zY!k=24)f z=raBhXU2;cEe^A3^a(g4o@3`p_lw9alggNb(@6_D5#A-m!EwX_jX~9>= ziML%1l*j$dudh@h88f|Qc*P(@|NGs$QcP44)_16Jgl(KfFW*J>R>jnuQ%K44dUAu_ z?U%LAl^43oW|Xy1J7>X}+(?8{?9s~kCJZ;qf1czW>mB}LJI>w+A69Nkv|Nb*VnHLK*urP zmgF5IBg=guWL|7=G}l)5w3xO)OvgfeNLwE*w2u!doKENUSoaKT+5wg`%vj zvL&Bzl^v3Fw_u~q_(7>uH#dJN88se?LVMMP3YFe{iC)o9u~-G09^45P_D|1g&>oB0 zolz)r&oF5*+7128OQ6!eg0*z`ua6bN0WoE^fl3+@>4w7k5M1al#OowJa{-;Tf)8?! zVYMjgw6oyW65YE25|;J}r|Q|_kjYn+`Rb(FYYNkl&9mC3H z*$wc#BGI){VN#8gOUGfj@~w%d#DOHTa;2$Uw@Ed=)XaYBE8uAwqtY<-={(I271gG&i9zQv6gF=2a@3gC2f(h-m- zrM`g!CGbig`niyske=E&D`Y`2Kj>Ug;@e8uBN!7CSR$@)Sl7*HBj_93Uov7`e_FSW z4)Poe-a22vQO57S_phD%=-#_TNB%bUTgg|tvO#gZ!qOYw)fD|ggk_C?#Wg;ZWYUF(un`t~$z zaztXwTxkMxr8_+lZX37yR3ocpU$BOfq5sEls8QArrvW|h16n9v6r-16>i`BXYGL0t zsb_K0UxT?Kj@F?k=qiP>XtmR~uV^K*%Ne;kvL(o@qK80UrFZ+%})izEfk&DqGWMs-XQRXyOdLGOM+i|y7SQGC5 zLXO^XhD_f}uq~1c_>r*qZ4BHQ+ULs=KKH}#(;QUoMuK9hHn#k%gkS?9@rJ~2RCEwI zlsdI$P{m?NU^wnEDBJbLdAI^QKo0ocYBjqoh*8eqze>qQ+fb!oC~M?d-@4bj&LqE` z4v$mv=xbE9zx>NtLlkCzCAOVo--r$Wr8oKR6~17tv%{bEw@*MH}402Pq73mzCMZW%i;U#AxX+h0*ehYm637t6KWlfkmo2>uKV}r8`8RQNL&UH4OZ?Z1tyA&5wox^t31)xGDNqP*K*9vL z1qS6&)Dy;UUYCiHjHl=`O56b{@OiS4?ri?T75`j9H(^lYbn(9pkVC#@}3buK_D z`m93>J~wj4&YU%xNubvhYM5@#`|}ynKVaEU3cyoFX_V_mlHBVuif|1+{rr2g(ZAPA zQmvAsk(nn($lN>_gCnp`s@n6&k?@LnzU7#C>K?ybvYworbw33$yhqed`0PQ*s6vKU zZzimWs9Ix={l!TBY9FBbME*4b*`~E{$s0a-oP&DTV!tLG#(#}NEy-$*Ie}nK6gs#- zZ$+zC_u1bU13%DDedTkNk{nD-278S4f8=)>V*3DW8gZQi(W2S3?bIcGi z*3FtrbpY=u7hrOk9&ex5YYu|NuPP5zc@bk7nnjaKJ!{yje|{BiOCU17l*}4mZsfh| zHP)LEWQ9&c)m}aqG=oPdXh6axx#XAjx??PL0vkqmQ}KlXK4HM2Md#=f6Slu|?Ro|AC$~y<-C|V}Ea*vg$H_o*>jf#4t4fhzuN}5G zOju60rDldAh1Op3XENBKrM=DTpc--)_hz`Vykw!<1V4vx?afqCARa(~)vO9c+(WTX)9-Uv##e^bBpEnT` zJ9K!Mos=VLw8FVELP`a}2U3<_HfGQf@p2^Lv5(SBKgt8grP5=`+W z@W*ErXgb-@0_VqukA~hSN3X=2Iu3xysJHf@%=i+0cj`}APCseL^4Ho(%G|Xcd>ps} zY(1|dMXK&^hZi6gCxul~1^q8Y1ly(xrcfUkR4KiCM__z@*Ba*m*lifUj&1wQ#Xc6g zY$r?EAhG`ct(Z(?tNa3*Y-kIi`oAyJ|E4)P-IkW>|C;50dROM`x8{a^DxQ*#jS diff --git a/Documentation/dsm_bind.pdf b/Documentation/dsm_bind.pdf index e62d1ed830316bd893f05c269a7f97384f94c502..76155569e396b715c00ff62f95dbd206f0f88b7e 100644 GIT binary patch literal 323311 zcmeFa2UJr_*DyRG6zS3hR3cp|p@Z}e0g>Jj3?V=$2_&H@5V4?wieN!OL=;7oq98>S zP(e^Z5s(g2Y=EesfYh9SC4lAL`#jHm-tYVV@Bi1jvsQ9u_UzfSXZFmTz4w_pN6OCB zTtQh;jgw8PtgNhzlT8_+gdq3^akA;^BAsHwut*a$8683hK-!@Luq1>ELJ8r3+=0di z$YSvd&W>9VNJla;0z-BpVzCYc0vVyAL`Shf6N6EBe}b+qJ&N^1kckmkeSM^D1UW2% zjKY(#crwXzt(IS*NJpfFlcSHFskxEzYK92`?}sDf2zZnq7Ei|chKNn=FTgos6x2}Es#r5%D6Y|!`! ze>8?1LBtZZQD!D|7{L!4qP zufRCjuy{Xu&(Zqmw=Sf29nyplfhQwWkk&Xqk|#oyRtll4NxSSq=>Jljmh-3LNINtU zODj&pM>=3hga{%AOVZUvniKG3+C>?mPR}z}LC}dJ%~cU}){y3E2xT=|p1wZP&DJ*v zi=h=jg`xwn${Gk|jnynWA_3!wC3_<6OwEx_*l6+!it)_)BiA= zNE()Gc*(CWj%epX^-RStcjo(l6VEL9R>InFw)=V2#T(iKf->3lV`EQe<{R0Ek?A87 zBVHElkY$LpUj%vrK!Q`5y=E*2Z}Tul&adp0nYimZ9ln9^ubs}oRk>_Xd_ ze8;;g;VyH^vCAjK7(5jxCz9pLNS~f{>jvM-xVtUF!}ZY4gksN$H)8TwxAjF|vwAsC z&u5rewFd8<_}+mZJ@pnkl1bSlwkyqb8p9|w@=lcYJeNRlEhnC zj?E9>3{RfXF=H<7#vU4d;OO$?(7kn8$gW4nv!sLBqj%phj|+Y*AgX_rI{vog zQ1|7?tK7rE@pl{IW!Wx7I_@45&Nk!fklaTkaH5-%1mvQ{NllUMqWf*lpTXl6lBW;O zl{CD6UC0;vK)bJA^x57{jikxo1P%w=$Z#)$wWj2e#DWs+neD7cwNw;z8c&3ilY9BZ zEG%LsHEBrV)DWx4uwW9995bBsLo`p_U?O9d_bRhLC&kw-JY=Y8lfJ2d%yWahYYTa z#&d%6(gzrJeiO8(^q!t)j&t|h$iKU3mfdmF^Un#d0q^1cny7dB#N;p6-sj0CxYi!J zE^pf(lBl7)b+RbleO+=(&_T111r}Q{aYeLOwzzx!bA^Wto2+0T{zgQ}n7Au}}_v5Ru0`+}rTJ0fSBCf;s~#xgLQN{>W;eV>m!MHYUE zj2mv!@)&m<{0=vGDHiMFGkY^l-~2-plYXAq*{!d2zP~%~oj9%B*~GNWHQnQ2C1Geq zxohWk&rNZ9YZF(Z&(}1pW%gq!|7;Bhc-C(2+_~{*HIIz9$bTj`aTkc5ZFTkVR}L9< z{%}HrnY#$(qj1$%tUK`#&;9h+rk z7}m*BRhSO9+N;0A7SLG4>CrY%X>GvpmF7Wm1x-{}EZT4aWa9YERTQw4$AZH0bvG zqR)h(hkhV>U=3S@UzO6qG{h%zQk?1<1?eXM(@5_QvjiK0PHoN2OI_7pUL-}`VOK|TF7cixic=Fy;n3bW5 z4ojT8x<^enIikF8HhM^&)j?24ZTf0?b>4UhF0!mMD7ElBUykkD(Dc5G(rU%0g!Dp0sSS^w#%r(kN-Ck}H2dO|&QK0&bKamWo* z3jZDm0R7@yE8Hm*jP9m8+-V1 zU+w1;d|GqDl}ho}`dq>zbpiuw?s|sT3+f+`U5liU=A3iH{+Uq5fR4j(lhpFKM{R`Z zEfYJlu9P+(oSPL3J;9Zzv|}tvH=|bQN!J!_e{(O}u!KPwZw<^I&&zu^pF^~@KXm%^ zkoB^RCQr`EAgf1d8}^^xspnH;;XVC2lT(1%+Yb#9zJC26D!KHPMUr7o>to;7ebJ8L z$GB$%GehOYC2OAE-De-*vM1U4?mg|Ycpxd@%d?mxbLgWwq@g^Y{V4*Kcr=cOz2GGsHKGr_X%XHBTz-S+~=ayiH- zz9`!3%Atpo=%%^%N9FtP8HtoiailavCaT*`$n5Ri)H^PSIlJr2NukmYSrIo2p4YPt zjOL5%Iu;ffSdlVNfn5|-byj&acKzPQ^Ik=}C!@^I=nCyK;6H;lIlhV2(-2?0H6ZXp zik@|F>6gcquAVB=W6yh%k^&1JJHi7>H!KFao~nK$Z+=Sh#W8!~Xteg3@LOh&zvZ6i z35@AIe!kFazuIBh-H+K5Yo%{F+V$;PCVoDcDL zV}}wuGT5-r>faP3w~N|dAAxm57tbF+YeBmBhixgC*w<+8d4 z=cQF2y0#!|;vVZ9yxkLY6{c_?zwN-;JQjHw4KD?K!0bs2}U+UOwfM%{Yb$s+3w#ICicHW2lwVmM_6_Wy>;r*e+kl zU2S3k$89brQggRy{H~}u>T&*e)BDaW(@8t1gO$Q^aZQFkoKcKddXn7dPVCF|zOVy( zKL?4bevsX+sXbjRVrce!E0=oE9`Ok!$qAozkgmRmU0*%Lo}aI{bxUQKDZi(^x1*WN z$2V7epe;AvTrmvl%sZ0YaHoiGX^Xl{sEpW`z;#WQUj4~F+7S@^LQVWzx+1)$v90OE zG~IMf-=muqG_@7!7~xB%*%(g52rL~+Q{s{4%1C202}{Q?GBzA^73&0Z3OI-Uv^^{YcXanPN#8A})+fAR_3xEz%rEB#})5(L{u*3UUW} zHLk3zg>=RFkpoExwN)MXrw&g8{E6S6gz5Frnf(_GnunkRNVMuuWON7)V}uU~!6KB9 zMkEYPi`P_FqFER8xPpqh64C@6W{IV>jGko_89-+Yp{}BYv_VI&B$PE(l<4)ku3)Mv zDIwifOVMaJlChyK2z3=Yjxk+3S5Q}0rdNPaR#DQT(cXoiXPe_fuqp^Ox}or^T22G} zsoDQZOe+6MOn+NEbaU!oiAP0cO+4zW*2_=v&@S4cpNv{M3Y`Z}gfh)Z;IIqvQyz548i)U4^8Vo01Z7IAmd3WqFN=W*%G8Nq zfiks0hyCCcY3hnlR#H-;Wje-?$kWVh-J`XN5LOK4@$ zMW`yPBiC*&D(XlRnm5fc42{7eO|g+U4A#NI7-@_nlkBiW6GCVh0Z*GLT5G->Qxm6E zPY%t|M+_s-d^-q#bO`CEZ%0KI>T02nSYtkHz&T!OE~!7x|}%rN*WU(CPq4&(jFJ51zP-WgzPG+^QkKWMXe ztkV09W2hVY1UTqi`a&fNb;#$wo#l^)>`{UwYDZ=x22tW@2&pJQ^m*6mQ zz`z58^T411zzEo&dYAzKFeVs1tTqr1U<`~*%q;6z+1O!!%!C03_*&+A0Aqx~85kLu zSeVx_!8uf6fPn|jxP@1l$;ckfCmx=l!pxtVbK6)#Qo!M%uc{hxf2#?LRGQ=D@Dzrm zE_k`*fVAl;CqJ`CSI8PdZO$XuucZg;+9Te4GnYA?d+5%Y`d4qKua-UTcsJwXADMXe zT6sg~`&ms3*MO+Q>3J26T_3*l00ssa10y4yCl+QVbvg@MlzADA?9t)kd`v0{sr)n! z9y+v6s!I9}6ZfZ?I0|4$YEx3oGzM6t)gQH)o^rY(2>k|12E&1hLYMU^*p6;)6QL}(Ro2XPLJT=YNd@IJ*_8Zddw~I%8Reg#!)Ns7eq#< zi>n@fT(&9G|1j;9bzC%~Q?TlhSL>L|cwBAnOhtQSi%XJ0_RKPOn`BB zl5qKl72(0x@9Z~?6Jk;AeSh~xuayHzGVOF4O7f~?8fq&_GR?{XCC^?p=yQFbB3JwC zpjTUOzfp8yi$9k|@6{-eF|B#U`X@~{GBe9PF$Wm$~^8 zva2X2dl({LT=shTzN}pR1jC#^{SvcPlxjBeU zDZ0C`GeVS-RnizbeSNy1cVXFJ(PS3czOb0C+vPaHU@hXs^1!OOF2Lhy-4)3XF6|K^ zx9(s@*)uj9E^B63nYeb`&j?PcDssKi@G1XGxB}C?YCeA*uVpN8ipx7QiAiF_HDUFN=&-lN?@b#sNltv3*`mcI5P$HM-m zsPwF#Yn;n{9-nI-!4V1&K>2@qU1ddfKuMm40Mt6;oZSV1x0zIV2;B62>Ap;%qH0H4 z7Rn{_j3m=gJN4$>qn8O16WzAWT#p<3-4=Sv<(rSrOcak%q!@Y!B4+*iX2>;*=YmSw zmV8GVHQV-T?itMX868|onJe?9=Ij*ndLrl=mpK*wWIC{XNW8fvH~eF1K-<#0?2fEM zuZpk_X5>AEJ#PrwAKn`Rdt5ZTYnI|rzDU{8K+#)1c)Df6>8pOhzNqr2Tn#KFqhQNr z&CfM0i;;uVNo%a0tNzOrfz+QuM2 z7=b{4?BMc&ClF}c8vp;9!txidcI?!?ptcS(CpH%mmHS}KEzEC^K=5+njTpVX z>)&|kw`4B$)qdJ+@i}`=fRuY=%nkD>sdii>Ei)+5VyWy91b9wC!1+k=4PmjbokC0I zw#zQ!R)pL%)K;kNKaC5|Ql+UquPvX}RIyKm&n9o~+~Iy+Cn|OES>vY@sSqIDhQOyz z%G)zh=n=n}``LF|cRfB=xfo}z-m2l>OkjEFzMR@t`&rOn`A*AHl9lIdqa_6Hc0*wH zqrq2HeD|2xcO5b6KmZkGFmd7x1j1CLLH6yYJ@=fc?hIB$vuADu+U=Hc&#dc zp=Y|gvt=xQ`2nxvog3JQ_%nwISJE@h^{(bnv&wIu=tTwl-2NO~zP))*d8u;kl-VnT z$s(C$?;tDmnVIBq2=o*|VCx7)wesAH#(jk^y z%2PL=Iq%|y#N2DWIjIm(KvU}dAkg6}H~!mjNXs0POtYdu?K94<`6E5&58(AoH5&K1 z?b5;K>J^gX1n&{rT+&cBVxEN%=-=V9e6$n-(GD|JgUj`gA<&&^@cocCN=hwk^!Xxo zJ|^VGmCy*MXXASd+A4hfzw6ClmXUXsIkV3d)9+uaH#GS?2#j3LUXmbC<8y)vbh)z5 zWHfshQLmhA&b;lx?$#W4TyP7tvcDXJa*U3`F(`d!U>ZSX8oMz#_nOm^4^T0et&~84+uEEPRmNaU5q*R z$z#Z9vsr*+n@Zrds^IbSzEv3;9#-&=wwBe-9u4dq8Sj15v`^-JZrTM1^xVy!4^F4z zk<_WJZ>GDPPxs#}`SPr!T)XP=|Jh^>FW9WO?p@%hv+mR9Fx63_`r?dx^cJxJ@8Ss>Y0ITcGR&rWv;x;Uc@VPG~ zi7rvgYz1-Zo>lK%7WMXYH&xqGzGb)gER9#v6rEo*AE17>p(KHNW9GO;zmc2p~OQiWhoy^>v003KnF(3j0 zAcH`F1w?=VmS6`^04hKQXaXf*3S0pKkmz|hfCVIg0Cs=~Z~za;bSPM9gAT0%Sjlf~ z;Qgfywqdm0AS3_)FP+mcGJcIMU=rd>{E=`7Aldv#m=N(MKNEQJ&%_Sj5YmsFodM*? z9|^P25YrzC6rTK(v@s?)z}7bivuZX1030lgO#q;qc)-CAf$$?l_!<(1Xj$}NLB#(F zW*qWoJY%AtlQW)dF5wVDlh%)59O8%gE4TwGgiMFqMTZzV{6J@m#A3(B+fE|r(umkJALgo+EIuXMDvRX$>$O_z!h%t0usg^bZ;h*U7^{e(N z&6cOz>-5nnP0!<6!KMX+eXBC(q$g=Mc{CkU_(ys(o$5Dwnw9=fCrE?Jtjqzn)o~60 z108pUj%5ju{{8?4Lj3({7B{`n3MA)z#t(=J9qs|Z4~RM)qVN+!r#VNb5C#DBmjOOX z%m2%)|D`-y-Hhw#b<^4k1AxJB<+s{zG|$HBl&0f~(2{V}5AIjOFaR*xtYQPs^vQ+; zWDo+dzz-+@N?agC)6y?4S*4*l+bP(eqZ< z3Kn_-W%>(kmH;9lBJ4L1Gl7T;z~NVC3Wu2^jk+xz1_0nhN012?SUi@9CS(2R+{c8i zEuU;F$TSEo2NfEC_LR=<6k6y14zVExtS!_lILzn}vJ*PsH!Lm;HY9|$ z{R>61bh6p8ws5Rl1OJB18b~1SF$%#2to0Q4O5-ipzyJUiKde7Gg1(TkMq-KNzd(0c z1OJZB?i)b&JO0wSJS+5#Eq-Ls`gkV+PrI^`31PMoWD@q*fx{Mp^{3tVeur}S63Api z=%3*1MA}ohKY;18N|rt_IcUS#V3HnRfzy16D?@k#9gl_q*r!%!)FqnN5dhc_038ni z&2zT_{Nt(n$5Z!@r|ut5-HMm=A5Yysp1S{=ZoYp!b!!g3e>`>nc?LU<=(Ep!q^zG!u&(7y&;(0KNcA_jCo)ePaYbqQ}Aj00RF30|88cBTxs5t42EA z##r%XumDCFj8+dc%*nNS@(E8;MALRKDPjnr$Y^vJQdv<633Lsj!_b&uEE(a84Zz{` z#J}9D6-VIw^u%4&ZIo=nOt67C%RNM_xt8;YvW1UG%!*eK}R76>xr*ymvFPOLzobVScJNwk^)9aLm8o=rl_o@v|Vkx zJVHfDSqZ7Ef>hE}P*$StnNU)upTt5S5aRlrY^=IOKYwipQ}fl@h<^Th;=i2eic+N= z=RzuP-@YBGq=HmYQ6T#HE0ALFWOTFwo+Lr1u|i~Ra}SPyN6?6(eF+g{J#q21c7%rg zp!}CTwub13`GFS}K@6eS?}tHRS9XSxkjje6KX9W%!)$DRp#LS=(9j>)B(hl~`QKxc z=-bQI&U%?eV)0~&HO9i$wx#?vi67Z?`O^x8X|JAtb0QEz)}-~*vHaWSmS6C(rf4!& zPn@>fML|hZK}pkTZMTbplA5-X(hBO@VYt7COz^|`$NWcBKa93Nff$M=>xtuNN7ax) zVb}nAbv8EIC_IUb#$&K3Q$6wM&@e?D&QDui#mv-LQ%gfj#Z1#oMOk_Kc6DPdHBB=u zBV!dMBU9zIvZe$~1Z`j6T3NsUqU?$X2p#f2qwzy3+H~&)?XaPqxT^RsaFmJ3>RCfQ zaoQyZ)>2Pj~%S;h*mM zufjjw^cX6*{J zS4jMl_jdvhCX7f10A>dOQ1!!-FaW>~0sumy$YC@%?U@q4Z!j&+Kzks{Pjqwl0Dxfw zEgrBEm#5>b#0_b2zfim%01Pyq!~8=1Xz_Xgz>&xZ+G9!h4gf%6Bn}$|0Q?mIKr$pE z6bAtPNj85d7EJ-1I~{<=#{jU>W_sZLlWzM<5B}TCzZqz< zT|xb3PnQ`?u=ywKXZQ&tTn0dE1_1cUpD^F^08|_TU_UD}pwvX|FwrARKMvymalv20 zFMP;5VEwT~EFObJxZto+w8JVt-qeD?;eWGUexdL`EcS=6GOhste7RJ>@1qFTx9|h_ zr)I#&#}43L7im72A9CBl>H=sp&sFTh%DAV8zu3OOkV1RHOTy6~+5-~@Cj=&f7`cK@ z->1L?*Z?=+2f|=8kN~pu%}ts>2N(h~U7A!qotR3FCta!^B`xFgchCObezD zGlT7bIl$auyI}z^JS+kh3p)TyhNZzWU^%dS*lpNd*hAP;SSzdtHVhkseT6MBz!^9g z_!%}aNHZWAv>1#StQnjbb}^$Z3?CV0-~i4J z7ldztE5NnjCU9H02izY{gvY~^;Ai2N;YIK&cq6lRtpvr4gQv)Z!yvBt2b zu;#K>v9_?jV_jt9VUuCgV{>8)VoP8<$5zDln601f8#_C@IQw>Xdv+ZAKK66$x7eSs z53|p6@N#V9FyZjxh~PNEk;n0Xqle=gCnu*gry-{YCzoYeiw-mP#w>S45?z7yb+%4P_>si-JuQyqbULU_cbN$`*UF&Cf_;{3g zYr4ZoqCx+EBcqW5a?lLfA|=SU6RLhHryPv`Sj+R&0}JGV)|k@vD0F; zV&ez_gb^YHaSlxtWf-nltg+UQ<05Gs*<`=h*GXnpE9qqmGXY&D&?;#+f*7X0(Z%ZK>b}tv*F)>&=nd&3^mprD(jPL|Vt_WdY%pRd zVd!Ug&G3WKR-+)J0;A8yO2$OvGUM+i+9q)(4@}{v7N$w2FU)w&oXs-KUYm=X2bkY9 zpR`c7*lY2?lF8EAGS#vRwF!km<)bF8G_B&S>a97f9j!C0M|N!6LEKSg!(d}&bIPWF zr_@gT&I(&#i?TgsJ76bc7jAdYp4r~cKGXiagR(=cL!%>~qmSba$5|(1rxd3?XIbY6 z=SMEwE?zGAF5g{EUDI5L+>mZ@ZcXkY?l|{K4;Bv>k82(?o@SnBJl}b#dnI~x?vmLR zz3ZvBhY79ySg8 z!C%im&3`mNFW^+bSfD}R*}w^$2`&RS6=WHd9rQieHaIUB3ULi74qX?zJM=D|7axqT zCu}A}6I#RM!V<%V!nMQCgijK!iPuOl(k@aZnV%d+ZjO+RI217)sUMjcITz&;RT|9` zjgM}Q*%os&=EENIJy-WK?#1kV6uTuhA$B; z=lhlRr|q9U;Ci6y;HHE74!${Lap-0uS0XX7>#)w@>?4dva7SJoRXuwCC^gA1=~=Q; z^4a92W0+&ljw>HOcN{w5f8s@oM#{yL%qQ_DJ5vo(ucz^(?MZuc%H~x0X~gNI)6-{s z&OAMm5tz}DX`ESnVdI6P7rtltWwl*2yjYaIDLXlP z;S%mrcaB9)`DN+LXRk0{iM;YY*Cn^{s@B!~Ya-W@uPx`{^Mxn~^)DVW|33yl}$ zEt)NrFSoy}YSnG6X)|bh*lyb1@Cx;+sbgnHYo~K(cb8Y!P`6+AXirGbRBvSOV&A@g zrv9XX^#f;LZ+v}uaO+_4kj7B;u<3B~h~r4#8_b)JZ%J>L-W`0;`Tpz&u@Cv9Dx>$u z%*S4idyc>R81`{-BJmT?rwgBDKi~Oc_@#N$ZSvg|aSHl+?3?hnylJ)RM>BRauV+JN z7r!UX3C~@h*P3r!a9#MY7`4Q*bZ&X;auwB@+7E?75Yz`u03!n&4rgG5!{LmKjEqdI z%uGy7Ozi8{F|%^Ab8>RBb8v92=jY+#=Huqz;N8H>Cm<*!B*eudEGjH0!Y?Q!NFxHH zm1JUKV`gR(w(yxPEZ2a&qI<>Jt$ib7+`ennV1+E0f5E9 z7~p`BhlzKKvJo?%J(@*4oL?m&HD{fK@$H8K4y}`ts=mbitZafp8-zuqq-A8cZc|g& z(A3(lZDML>ZefYCa&&TbadmU|!1!VP0|IeDByvP#RCLUq0|yT!9zJq3DecthGiTG! zozJ*@CHLyJyzBWlN=omPl~+{Wt$I{f|G1&?$7s!FR_(PP*QDJYKqo0ddvP^!}kA=TK1!1Kil=&fz8E*B-u+0 z|J#3C)4qJ|^B?G*k0Q5J1HaLD*~!kbf*_HdDI=TY>}uP*EUx!@csHtbhL8@O=RYN^ zrK@zas^JReYR_`9{r&x&+$vhlrIrFwItPtTHMpO5+FKS|H)4=T;UgD{zvyf-58kyc z&fyvJzTT|Io!Qqs zAKuh2L!M%PD0M8&dX_mM^%{4m>S5%8+B40^@4&~IGNda5;l&qjY`8XDxm6OLVsgOI z^+jqd+tjgS()Iq}F1z3^59zF{UgME9GOrj*?}#Kn4c7|oIo1?fw!gSP3M)1k5dCpz zu)e9c1z)@$)>~sN%_a10{9xi5SIYX8t;SG(+y0fxj4- zt9T%?YasimdQ+B_PM^+JP}Na{%uV`a-}Z)6RpXV`Vc1T+n;oOza+2(S{1trR3HC5J zJncSne6Mp!+nqk=f$*5Puv>NOp9sfnJmkG|yZRL`LZbnDkhj`gaK!L{^tXv9SD|u; zTKOs7v|O{?4aqpPaQFpaxkFHuVeSr&SZsS>mRFig^w$5z?|@z!M_lN z`OMkIzZw*X?t56X0rhqYnS4heUIlMwN5QAaAipercp(*4Vyj;B-wH{7?K z6mo&U!FH1lUF{6Z_9Y>C(%5X&ca^6yM{>_j4D8xuY*=ZRcF&sOP=tF8<#g;BN4q)B zDWM(rrv|90Bh|?IE^m#p_Rgrzx4}ZVq5F5wXT6ri&O2qmPkYA z)SqjbS5BQs5WG@oS~7OlA?W1M;?$Zqb;6--TPK`0cA0rg-H6>j)sS;NxYxNMbGUW_ z0z$!O_mtl(jH|J@U!Ej}$TU6jK=;u_^CwqdCpdqf;LxZje+e_=xBKm9|Kzc+{EZie zPn;WUZ?yCQ8p;86GRMF6PEjk!f|#7rV!+KIaY3XS$U%r>V+E7VD-CC~>TR zy3tyMyK0m14fnH)v&ZgCUd$PJBHby|QO3SQL`0k54Gf9dN_{6fG|7ibH9x<}q z1=O>eSH*dwm$=W|x49PUeXm14qWexY{{4b^iKa@jOW8M6TV7JsSLc>9>G9{El}4)P zzroc8a&ttJ?0vY$dtc;BeLtp{T8zJQ=U%4mEuK?+MvURSqhAu*0~<6gDk~RUl0dQV zS8EJ^X+!IiQQ#7LWSeFG(|b`YQB7Z$x{ETBz`SN16){&aoU%ST_)+}E+ph`>3HS)Pn>qn6v&7sI zw@x1%BndRPqp-ZoMbkpx_#!)WOjJG}SI_PZc5N$AzxRnNqU%mIKKQ77Z7D}s_k&I$ zlzf{&O>rdZuJ z+zb!Bx)c@~>`>aKODO28D~P^EDIFInPbP#_MWl^g^fUAA&9pCyl;C#K>V460_sfyZ z3d^~8La{=u0w&)L0)B-(ckdFoC0qMTP}lFsF;6YNY`u0-b#wgpeD&#Ne%O=P4e5`I zhn^y)?xg4!2HY6zi!1!pDOT=j5w)%F`EBNo0dQpMb=ATj7UH{+TODj+fG&LyZM<9 z>~|Gz6c|180Rm#R9dnx0Mg<)R7za(W0oh`yIS5Q0nS*8sZ*3cUK}?fahwldy0};m~>suV=+~ zEt(Gqzu%|l@$c@9l@K_xSxbN8lwkVsq8MB7YpD+PV9(4a1=Hzwj;FClCP=0Bjz>Gf ziY{&7aQi&AFZfom!IaEWmgWJ0+YAy?}C7-Od|y5HeG_i*B2>sZr+LzxXkwk0!v&kDf0m| zsfJNS=MLz7NGh1--2337^kj8sZiy&!2lmz(#T~(SJ8Z7LdS~e!zxztty`*mtI5POv zt7anvx@>GAfC_&3`z@x}VD2ddq=beImQO^9EucF-zLaaBs!*Pq&DlZ#lhsH)7qJIS zv&mlGNBikQ;LZ1iT4o4zzssYH+IS~;?^<*U?5sgdb{(1y9#h0r+@Fff-gum_J;{u* z)?C1oD*W-G&$Qr_CY6&L0(^#>QjXP#Me97yC@a4@vWZu8G1*SQ`pK}AS?I8o+3r#V z-q=#u=#yRaf#K|gh!$?o(LLFTpYUpn)_03?_LUYrl@7q&bzuoU@7I6u)9!8wfx3?n zVAK~4%-GR;v2-MkU~~KM=FpDUm0OOg7$1L82bWB6DTF`(1mIrAr4I|E(=6Y@(o8#u z8zLCj-+{gISdyKia3}dqiA3Xhu0Pl57Z$-;6%@ZG_#G>+@Ap4l-XRP>wahus zy311{Ov){++Gs;QW%%NK0SFio_$Wg!Auy!|`{?AF27x-gXgTWs*U}Jpm;-^4KnNu6 znRH!dx^Dx4111otf5}p4?`;hMj+vq21@`Dn2rwUpz|kvz?Apy`W--Vm)vo=uqY5L0 zB@r>Ibjn;nF%#vbXZ_%un!%C{*wZ2Kwsx|`n+*b&Dvs7p22$oS4E{gCR}2J>8oV95 zFA4!)!V$`Jg=vA$@67v0n+$(W9x~`XRd>LmszcG>j;?6Yd@cUoxAKYV)~Skwl$t0=KUG337=`$Zk$-ipAumJh_EAXs>vRXJ?t< zq5JNXpe<={9Qk^^1lisb%M&gK4!-*)DIb;A;$uY{w2urH4I%JyemHwc5CXIOls^Ji z2lpSr|KNRe{y>$4z^QAKgyo|(TOiOjzk3FLi>w^|zj0nmaVz7bC6u(cbe> zXR49Fld`>un+aKO=NH?F4gKaV-<2A#SI=FU9C;pLUN!Y1fqPc|BOyN~;}~WltyE>3 zfx%Y!6UyVqm)m{h&Qtk=n((isQ|gg>hv(mK-tbC3;Pwg6bv8Zs4)u5wQ`AEGxQ3>x zE~nkqb#KMT3yemcdL2!1pDIcxtDm~*X3L>1io)tBJX4Z?d%mmbeik@a)A~I=SLQH*^J-h)llbQtj>Br!7i$86t5s6I5O~xxRk6(D!$%o>2!VPRpM$ZUg>zSCuTFlLZQNgG z%ekEXuNE!0N{VRQ574^9fyEa({Rf&`4t4F&sHoB0)oFput@Q9)w?4Zy8KdJVs zH@ZJwzl!xDsaENAVvC&F%fd{1W@5NM2|+;0dN^e{v4whUfAyXfZ87R$?bn!#auB$s z4S`WX^$F)UHJlJQ)jZ|1oX`aU!`sEamuPI(XMZKX=7hj)&3c0c&&#ftT(jp+QFmrO zteusfI}8D9vej=&=!oW0I`Xi=OuHil+B~Ld&HoR2uIAi)6e0IxEw`@O^`~!f88S)l zDqks{M|nnPTfVyF5<)s4KPA)gc#qkNDt@mbx`7#9F=Eo{S;8qU=T2HzbyPl-B zQocIXY=A&_2n0r-$aP((d5++Xv7!)=IX_an2rs54V7woD>kJLf>$V6$pnDT#nq6de z8<0CZcXU{cH|U~9YpnaQ0ngZ;oq4%q-+RMFijGU?Fy0zK;Utz)`yV_TE_m~>-(0wI z)FPa2O3oTIKwv@E8$jR@?{^5CeMP}65g>5+&=B|179?dpbhTUlCgN=J&=-+`=sv3{ zvE#AsIYFU01rR_VFrS?6r99sjgU8|sikio(84i7@-oyC^$IKrcHGh@%qBMxD|F~JK zmHg^u{G;@R7sZ6D5A`Ov)ifF$EY7t`H2A^A$l7rxO_R1V(eB;SgA7JxKMm|0~2d+5vUQWI+Qyb#BqRDY=KouLrq2dL5Yk;Jdfg zbsmoEBG*WZXQ)VzvHN^Q(QREL|%L@Fk(ZA!;`FA)pt>6!S}ICpShlxhi7Pq?;KX31WiaVVt$3Y}lXFdvsN9{gqPQy`z2zz@ULmZj z?aa>#Cb)mmZ!5aF%_^BcQC(K|dH=-MtG%$kKh|Y1EI3yP0WS2;Zr8{gT2;; zo}(Vt(e|;fVu}3KACDWq9~xw54LG56l`*`ZFwfeLIXZZP3E=<8K*bP+IjEO#OlHdV zXsrG6-TbG%@>`-Oc-v$z4Zi2O?OyWu-urtVzqOfQ+R_UoCTvw=0z@a zRgumab~kVG7FNhj3DhB`VRSO}iXW#ijArxbO4o=^Z6uLG)O)o&v!EZ>Rh48G#dFfM{J6@M{TtNlt!h7MV{UXd z+|G?vY!z~vE5=bq545;x*s2#Qb+ zst0qzy{c<*u?8`DxL%jSw}mO$;W$Q4`4S86tfyrObA;0$@LGMN3$}I7HLd0pvpJ_f znNwdpCpWY|pO!yP?t67qTG}*1<(_J(pv(NZ3^2N+lW$TG9pC6$f#MZi4E*ftt|aXI zN{Ms7d01S~vfmuiZ1d*HYgsvpS7PEbLJJ~W`v>OZ$v2V&tmgxdUVwdfaw+?_^Znng z1^>76C{Pdm-Qa7l_Fel50R3}UzrBK(_GzsD{B5h%Pg?2g|8Mn#^7lcEnLRQL9 zNNg;556}M7;z!WB2Ks61hq4dK^BZk25#$$tj=ry}&o*?H8mFzl@P1dp@~M|M(GCl@ zE51q)60?c#WL_)i112i!ZPud9?e87wsZnMXl!WX(=IR;v8%Z1@K32BhMjDX1{c7kp z1!dJ=v4)Y+1Fw=^DEr;cW$Enx>;hMaHNK}_b?afoq+xKhNmZf$+{R~Trl5qWmPQLP zWqi_rndsHqPtpbgYj0b4Jl~DEgEaXvun)IUQe1A~+O1MSeedHS@>aS|_m`Y3K?lo< zr<)d*H#joB6Ok|}a9xLhajMp>*X62|eB;I+L#a5Rc!Mx+8P@NYD3rW zQSW#lVzM}YLN(MIPC*tDCq|Z%vs!y!|m_vy%15= z%UpBd=8a>|n~q+StdqQafnfD`K0(ZX{Ne+oKx~eB_PDy4@wx8aNunV-ElGZSUPZ(s zBT@WJ_0W6$YyCv**>X_te6Zzqnl18ZY_;oE6JvCqd1&25UCvaGT=67ob|yMCS$C}Xo%bctF1>P@*ZCg9 zngN$@aMD!0oN;uqVV_ekkck9BV3R#RNs^zP&dw>LL>=Tw5PN{(gk9@wR% zQ+yT(vkO;H4L*h^p1_6Zcy)(-G)C=B?Z%&_rrh5?=G(=7DL{y@#PM z6be=qYw8`G>>8|c+nE9q+WPQVdDv$*Pn(bQf zUxG7?cO2d*<*N&kM&|o)*&xQ%IMh`U3-YbiCcQ4$=i@hP2J-ayiyRbk@OefSl#0%tqU3I zvlFv4bQt_Vv+W20x6xQC7G@o!Op(tpvZXYwjH)WH1F2YMz|_b8rXlPIcJwP&9&7#9 zB-nY#0~=l;)h7{I5p#}Uq{&19T}m5ADWhrz=K?0clE+x{>(Sw~kz=i1=9*k&I44tI zsYQBln~OF@le=}(BoiB;Y+^2r_VzccA<|>2&(oZc?e=BP8T!kvN)3?#1CjENonx%F zVY6G}Z>|Ou$waJ;E>Iaua^dKRFOWAK`@JQhUgI*FI!IPvS?5P4eN&4rjkoC`e!G=o z_I90HZ!Z6x*8OBGv&Gy%G~zU2opWa}Iv3eY&8&4F&~t_n7X>~|G&(iuo5Ccc_K4{R za!8?x=p6!kQwpEw zZN7|vn8RPyEwB`p*%K|}h1fQK9v@_zY|m8n5q+9%P7@IjJe05%Ei~OSWUSJ7w-S?f zaV1N|N2#PH9{~7{c^&xW{R*i-DD~S`o1hh?@ApE;GGG=XV5+54JW2=2m1zKu0TbeA+)=izRz}0+9eQ1OdK$zt9@Q97GSq2BZ`O zA4Czv9K;Gl2LvC40fY{O9)$5%AwzSs=eJnN1lXTnkELU!r>AFOVxb36(=n0K(UFql z(@L1@{4Y7At;}_7fx7=)^}kP1FL0axG^PGC9w)7gxrwfXmIXe;3uwn5bL}%)CqBbZ z6b&&Ie1;d~2Q~cPu1e}U*of&m;xqhHWxu0?3YnYRn3>z?(n>xPK{K8INj!f(OxxUn zR#q2iLo1@IrK4;0lYcJrix)LBF*MVaHPEuq{dw_&nVGqb*0WpxNL$$$J_B_AOevaM z8Ec!H8`J)J_x43%ZJ?`b^K7;at%MfP%KW$VGvJTSZ)pL;XB^Yt1hOWEIxmY4o1b}q z)+1o3rEjHWN-JcjZ)>IdPpyCU*DqH~+L~HF!(TGI;4I+N<1@ZMru;LM=Zjx|m5}}g zjVk-o-j}z#f8H!X6J67n`|tvEgU|4?Fu`Yd(TLCZ0?vZZ_;Ua78DDN1KI6+B!)N?i zfbj+M0-y1xEsQU)KKP73wJ`lG&h)c5)6e2eKZ`T{EY9>&{9jCezVw^vKe0h%X{GS8(n-WU7)U^ovxKNIsUWbo^g%-w9;pfytG0E|DTNf&p!Se&VBLY?+`dI zjsMMIFRuL??Zf}s0{Xvd!S72@=|4v5U*3nmQu2HF``?s2s9J%Z`4 zl>8n+{x?gQ|4Pa41MxQ{KZoqUbtbWA#G=1a_1ucT+bEX5Qu5!76~JF9`IqtWAKlCP zS4#fT-#Iyd_spM;TDHGZ@;eIHZ|?n5CG>wa^j^Zx-}UStN`As2{~k47qUwtlmH&%? z_CEksFH!LCTJuL|`FCqxg6QAX^2cEO6D@zyegD+Ie~iXI(emH)+&_lnpJ@4Sy6zw2 z@lUk;H+}byQS>KT{?U0=@EKkr_w&^C|MFvg8B{EE&mH=of2=Pi|B^AiKwtez{p*3=smx+n)_le>Kb?;Xv z`t{`$e5RM#`afeWZXU$fW);eT$x1c4DuH-GL<^$$OcDg(0N<*ZSF3Z{=@APGh%v~6 z8KmeRWn13fTGl70Cb#D#e-B^4wIZXAVIeqCV{{&{e04q=*V0miv6sedtT$c!*?4Gw zE)0h-zRtZ|Iyi6bmej5!JFds&QyJQP(TqCU56#iB|AQ$iP92bw;ebQ>&^$`tiI6s{H z$hQ#l4G{@?9xvJ(H%aTK8x5B_e%BCD<;Xe?a?)GWNhG(7KzuH%bS?Zsl#w7cojF#d zzQ~Vmc}?g7xk1ENK${4Bl=oxU1OnY}<{ko?)&<&`wdSzcAw1t)1+$YK5wn1<-X)B# zTcwu1>(k-<>My(!KbjV!BeA_BWpZ~VWp?K*9@Nzu*0SiDe^5p*MI<<$C%R~Tet(DD z9h#mYH6az}Yp2d~*vwh}p4F8{Laop;Xc@y^=K(w-EjXFg6vubnD}DU#b% z0QO)HIqb&auh|E-5klhTlmwwrf+2*UFJQ84`Jk`hE;9Uf#^!Dx*&_gDQe?OFf-;8WQ8Lj{O-5I#jSJD{$ zk|LxHb08SNR))H+cG$a|iqG zo^~NiGJ(X7J`OEutMP*M^+ZlYE+D^usM^q}>)OtwzU!bK=(sCE-7UOz+c9=xBB*lb z?HvKzt1N@R_Oc`BoV$gB6gJ$s98})h6=<}z^-s3tWnCIq-rIu19$j8S3O<%$G&w|M zD{hfat`^z{f%m*^TbLX26Y1cytR|S`Aq$0N+uh4o=X)@5PszuR)m>VAAfixr0=5W< zxD}(vrsEb0_sYRZR!oi9AtCq=KJ@XwPV3;a|4RI0AN1@(sga9oQcS^>><%GexLr>t zOVd>G9ML|PQc*A8$1G~_LfRaZ-vFe1T8`Hncd@H*GK(374KmDoh#aWyuaaw=Y59q>Nfa^Elg_&eWDZ}X>$B5| z4%RP-CQ-*b>G%Yf=H8jDF=%eR3im+o&QuqF&6lsXUE@^Dr4T@%?+j4Gs%AbjH_ofJ zJ)1V$G$sGoKvNWWU>v_dL?K{A0D@%|-NCJUA*7&D%Az*5h;4?dCqF$s{F!2o0!HAK z37%3#`}F?f+I+&>*?0tts?S)93Lj{h1{~PD@wTGVskQ|DYzJ%H6i2eF3vw>;}Wb&D_&W8iJH1gjJU8@+47L% z|Ikt8tB(<-d{rVXiX;80;;~PP{muM{vTn$?U2jr4ZBF?F|FIRA_owGtxS*-xKiDwB zNsgu4$!5`p=bf|yZa3#s$L1b!8!97U?x8hUeM(}<1d3zN638<7FpKg?va6@1-B30Z z16?3CC}?kjJI&syDW<8PK9 zr_8v|3TN)46%Xm3_KF-gpDuPW&%&ihzi6;?8M&RD*ADTpH!owmVumM40m-JDEp|O_ z{PPf#+${7KD!$l5#e6miT5 zRW>x#tU;J99Lq*-E-fSXv-jyY*Y}9l25#N=DK}u7<|9{$c!>+TZWd-`H=gw!n|Ftz z^vljtUv3Va&6t-R{RdonR$rY_we;D7#T&Pz8Z+T-FCE?9S3H!CTpU`f*_)&dR3LF& z${!>x+Ap`nUs4~)?(d{n9&od)GmI=USTDl^yssKc50>^CHfPo2&1tzOokwlgE4*pW zzNsL7w~yfHy-_=uc$m<^3xvcNvN8GLdbhiz^gB=`WvH|$ zeLb$5CVo#TzNpf&vS@u)Z$?kh+fjC1Xhw*pqbZ|eYRay7YHED;hY|Pm52N?>o88@z zieSUcd3lC|(^cS%UPZ5cj({XoJ*3BxH>Jaii_^&cIbqWgh0OpTBwq%0a&~|@0y&~@ zgt4*t6=yf#3R}vHda^4FQlWKKOoNY^yd)!L_;B=roU0_`ftQuM0v!C~7eGd0s;TE< zcy1et4LXsC;o?=^7+5|S#AEh}Oq3KMH6Z*Q8R}u-R@|_<3p6+Bk9%0e3E8+; z*xsyqqRroBu5fz@bkAgWGJLo1WgGt{ws4}c#+AA<>FuGH^` zyZk>FDbzqNL)dK#Y~*8BXV54m!I=tkjM++?gF1m6_ zqAeBa+fm#qyd|HOkT5~Y9%Wyq?n0CI_PZb&JWcy8pU4W1i8iV9O}*o z7nLzqtmpa!t#%FRud%2gF9X*DWiax}bLaYY2cyQ<)-peSZRpgKSch4UjVUeo5fm%~ zgNC*#_g2CEgdZ?-vufVAHBrCpQw7af-mx}+2S5MwE{YQ+IWT}i^NkR1_ z(oAl1#>zSZJRoj*Ppg96%DruJfp*eF5S~1Fpg0W&in(|V$HYPz z3pzqFJ39r1LQY8N*ql*~dL0YZ2a27A`3VpnD6yPO)hg5y+uYe8wCYX$=qjNsRIa2v ziGEcwD!E+RZ9qH~S)=sSTwx`>Zdj#eQd%l=44Za;8;zi5EUUU+b@rSBR@+-trR>N= z(%AKn`<+J2(hHqI*$^U!TYZ8O%^a9C!#%Sm^s`la7UnJVc@(ujc5jOn6-)589ab+k zOE!ucz5*a>?_ZJ1o0$)Har8sZQ(8;(?R+KN>9d~+JJX9rBrRpuN z-)$Eg_Drk_{&;q)?$FRauk%4k1)d>GUXuIrb4*pfq|yq2l(x4kN3&;6ahT!YLp91g zC2avUoZaG68WI(~;04lSPYPD6G|O3Myqm@Dt^0gqDz@v&)$-!|!m5jJwJi;MUpkV8 zpYm@<#m+h4rD#)Rd$lfMCwB+OQQmcOU}HM-@r`RcY01Uty~&9;n6yjlVp}Ienc@z;u4($M{&@Szur=7F$c*+)>>( zl#gM{YF&gj5j-B88I>mZiF*16?47JJ6C9)#f;vzMNGDIllvG$0y)l=P3vq+G`*zl( zd=` zR!mh8O=*MBs7^K=yaVG^Dof(}zn=zCPZcRD+TF5L4clz)AP-d-amUSIWXY57khdqs z?FUga7b%t1(EqET^*!wk`3WSQZ`)YHg=9kKmi`Z{F za&ijN35_LXw6(N^lA!)kLp4s-d=)PH@5*T2NW8byp<;^3j1=z@%rRyS9!3X>jFz<8 z%C47&vc*2$L4^^;m=zRnn_C&Y!z9iVv9Y^EJ3m;CVL8*BP>Qy-FkWZAu*57;EuC^O zN@UoDny!_s(J`SqDiDDn?5*-618Qq4e-w9{Va7@mr!eYWo}F0~S_eY!Ye6~}3*qW7 zSq$3@f5SFo1+uSeLgr18?)Bl!n;QixBoz}Er8mOr;%bgeB% zjjLmNozdEK>3!JkxIlF>I+IIHW$kWTyh&)sa8S|SY4vN;#j$)P^`w|sOcmQ9pzO@k z*&#zm;kiHxw?h2ewA3WG-JOG46Z_eH7Tv1I?lwuKv#pKwUdWLw4E@Q(Vpe3MBpP-l zfm4EaxQrPH(vQQRwFIzo&k#2wshZ!ghY)F1(~pTv-U@JiGK?PByopK%$FRZ=Kv zbVbFWp|+M|PFa$3HdXeTmxaZdo`ee=h*P~g6J+Xn35E~aDi6FHg+8Z`x#tbJ2#%H? zL%r7Vl@U$$4LOceJlmq7FcYx?iCwf+XJ;2Q813>iE;jq-S)Hv79qbpu$0CA1j;2>S zH5_Xa%$fGo>hmN^w8ULVqkd9BE)g)5WI~=Qw^A0>22wlryjh(8sMAATbcP$zr@*#p zLJec>lIUcH0uE*Ze?l%eBz>-omqYNbrb2VOW5sm@SJ3NWWlvuI|x zuprAkDSqYQ<&DfIJI=gw?gq>>S?W|<@DD%VQ12L0*8q$gtOqb~)$c_#cE*^SO~{o_5y+$Nhnw9Rh&;04waQ`iRYmKxe!||`%}6_qH?Q+ zd{HXpr5>z4WUjJuhlQ${6az-X?~O|&6ryy7Dx$7q2H(jf&PIrY1Y&^f!ZQcLYygJQ z6mb;I5~bn<(JZ?9@3nQUzJ6uZ5CW)Pxkw7R+jvH)EHI!=vFS5imThcOBwG6<)>l(E zTep+!)T;|hsr1Y#8p3-X1g)3^`b%tBc+?KeB0ztwD!6S+t&d_WA&GX_pJzZR&l5j6 zDM+^LD$z$s7P7#}Boj)`>Ftc2jg!K2Vk_6xJ5H8cFAZHa@2hxc)?8!z;KN2!P=Ei> ze-h@^9(hMo*%k|(2N z>zDKvl&5O)C^bsN8?Ms`zn%9Z-bv%TK5W8yGb1EB$ww0MwB!uQ%<~dn6^U(GfO2ky z^m;|5uus@4Vr(&Q3ctM=GNEn3ZBbWs((~u1ic964B+-OVE*$Ne5KG}~D$yQ4J)=6W zya(Sj%Os_PYJ@{W&Hk);fNHVVLFZn$h;+7>#(Re83YLV*!EM(cE?G&Xrj8aDCQz`5 z@cN5j{`{9Alup#wv3*+BG3<5Y34Z8N$Q)^fDP>+^1-JG0amk(Mo1H8ymM94Lb=Bt| zB4WnK=B8`;5bVBFAmEJ+C@E8{F>SrW}VaIk3|Ybzyc^$v7?AQ-Yq?*{ofmUF zj`PfgUEH-oXXzR4q2>sN0Fl~4Hm4jT`k_96h+-inZdh44VMXto?*s1pDM6g_qt>QH zSw+L3_7qA%EP1Xr46BX5hI|ZLDv;w7jqVsT2p=ijX(%ir zfg30iPBa-i5w$D`Y+zNd@Bd0cQ$#_U$2p@tRIshQwQbrz(T#pFPX^Z1di+r~sxL;K z;)nc?oE+VF^IW(QRj2s#eq3LGY`k@eP}u}v!)V%2nK@Z9I+I}f{>CADVlO~azjs(@ z46-t#zq}7$7UOiQc!<2A{(7^WEyuy!A(Gu)W!YNrm|=aJl6$}m;rugs^x>Rg%`plE zHuxG4Y?#a2L4AyUM_wpz)@seDR-S0NcTbn1?f}e0{DC!LXOq&3G1#WI2<)pAR}T`ZtmTzvBe( zA&^o^s@-yN6Ce3SQ)3acSLc#~uLG4Gg{+cr*$UM;nop}kZvz@_O4T0=L-8UzQm|{% zR7u1A0Q=bnf*}mUQ3*PM@)SU7(ZD!X;zO8)l)JP~kR{@!Q^F}D8nIe3MG_cA3UeVw z)nh_Z&SpO11K#3!MEWr&mL(e}CXQcAJ}iSMxK=<qq=YVD8Pe9%%H`hbMNnYhBImjupya1qHIpnawCi)WQ$VNlOJ9-O%3}Y z^1mJK=OLZT7mv*82q^+qF=6o6+v5u!q@cB+Exs@tAgQWO2z&IP>?o|Q5S%cxh{dwP0sHi#2ws*BQX3u!is422PTv^w_?<5N5JKQ z2;j5`U$_|42Tu0T_9seS&^(y@NI@QNwiK0Z3i0^fjo zORX{{9F{3Z5~u*eBas_Nh1jk>_nm zhv}~M+o3EOQ*VL254FciU{OU(Z*e+o)?ddJ5NiZfZNo*Px)O|M(h4ZoWQPBiI`fIf3iAt(uOnh+Hm>fg7yif>k_1Uu;9~T3b-)^zBJ=gjb(*jw7ldYlnx#S5oFGE&E?g8eJ#iFDI6wZ69QXHhJ?7_ zLk)%HFwADkhs^_+KDSBlArr2SwhVeY2p1%*RsCf8Tw7`79{j2w^nL3gjNL>HT2t?N zI0snI-i4B-Wp996bk7xMiZ=|ez#?piaMz&H<93XZ?Mmt=g2O?VyNzYZQI5@fi@VdR zSX#_2XAX6K2G70osiGNE0W01j4pl4-P&Nf%k(N4*nsg$M5a*3h)P&!muOsm1-&z8aclo$k=dreDC07YA3qkZO2`a3G5iun2e4W z!vT_ls$JyZYB_RI*$Gk}Y84^T8AE(Y1EZqpqJI_53+(Jr*w(c!=Co{&D;1`hy_yDio~JmI--w@ zG*~TFsfEl^p$m?PHJC%i>EE)JkZ`}hnONA4IzKKT5V_;-J;!Y_d2+QmSY5KqQQuQg z@+cKh4R(O_O=EoAKsF6H*nz33T}rEy85=$vFrhdM5QXWI4?KSt+(gb|6>xymfI0FN zd)_+apn~T|#)y%IYLElegkS3RTAzV_%W1|^IJ6*yuV6dmQMx+=9UJyLBhrT!(N>P_ zl;m=_DIk!~-(H6a{WUu$Xt?`4qYm0GC+JbHkG=@{EMb)df?hq?TziDD!MT(qWc)@9 zT=gOSk;gJ+)z}G6YGeU@t$T0}9>_Po$mVihqY?ds)#2|RE2v&>R}uP)2g~H*h;ZLN zjldoV-ABuE_v0pGU)7T~rFuMk%~+4-$9?j7Z6O5?K03oe1>e}y40QIvFt?C{7DoW@ z;e6GbfPN|&0uC-s{Q5kkqc@P?^-dbd!#Mlj9$wK z#wX5Nb9AFzG#(k6m=Ub6l0EEcZIC3}y>fS=7TZG>cxZ+N5ja?PFTJ&KTV0vZ&to~T z85tQbPs*1LG|hx9!p*HZk1j(GE#V>kNlCpx3-)Fw(>dDEt29P_yvwf>PJngbWj=;A zeY2?fCft1y3qHt`Kep7qTAsfsO}5}x4FHKO zz2xdfYlH5io*J0020g;X2I@C-st1>P0?NJ}l^x5b6`G(y|DI{ok3^rOn`DWS6e49QGj|1A>5xbqf)(Xl0zzv`+!>0iY9wEO z(7wxyJCrtI6^B1n3jm(AgmZkL?v}@oHsapCywfoXW_Ew2zHnm7>yEIDc#d<8n|{<44)&mSk2u=ee)Jyk39}XI{O-Dz z*WJS%>7304cN^(MFg+f6P~vHuyj?)@9WNzJ6T}`@`@SL~@o zuTlh}Jk(RH27FC1%U#YqUS>^$Y0KzVZ0#QYFf1wT+`;xu&asGwOL{q_YA)* zoeydY=@k*_k3SL#5?>N>jn$de9in7=O7+r(-)|PQ;p45cn3;%$`w;i0cLj&caD{m z`xFoR6MN9_9sE9LS0`7XZYfXfD`59v9!RH1ueb+!;IE;PQa8d+dO%vc+~Mdt{XxRd zdO-2E@St40++Q7oyT7vV)BtfwbjLg zG<8!7>@neqer4c(z@zNA45X#M74{nWk<>%~n$e@|tPHGWs1@NF5veiCJ>DJd7;tY6 zvb?pd|3;7p^W69lr4!-sBs`<;O{+d>B91i_>FAP$d+ZF89dv%Wo@}1Scp`fZbiNh8 zaf(Nco!#W93cQyrf42_$>f`%qy7BkOUqzT-ZHaPzj5fj^&j7y#WPO(KbBGBT&q>4{ zP9usTctp_c{)ErX4%E%VD`4>944lKq5Ps$ioWhZvbc0$-9Tt@QPuIgIDLQehis-ny z*9d;kc`Q6v-*=#)w+tS`pK&}gzJBn3ryOdpI@ z(7n&S#wmwn7qP|XMP-K;TkEVvGlAA4MQ^<^#nDga2cQs>N;IO(7xdce41vda8bm3;)uR>A6!GMkb}`SL`NCu zT+;rsizc?0sic?sW`NUI2{qt0^6e-0ub1utx?MsQ#i$pASgqu7Da*(Mse7GY&Bdvp z1e!&8t2pWWCAi+$V&QSju3TE!Kd(EP^vCI!J8$;YEUpR!tJU0GtR5bS|0X9d8MOAN~1k4 zbEg#d_Vpv$^3pAChKT`GVV(dtPM0ajGZwdrWK)*qtNG`DM*j^86Ez( z_S-OUHJ>fI+I`{N>y95g+^hsnHay!omU`TQ>}I`2PulX-xggcBpN%Eiu=itw^l3!)@n-TU*=`UcDYbs@i@DNC z1qz_-`g|^@l2YHi`81ZKFB-SrWJO)gS;N(7*&r)I7z>EV@-253e!*W-t^;xQ4TqN0 z(zWg^ZT8qBD+tsY{$S5HR&evq3?&a=R(N55g>$TCgarG01jq)QyjX0uf5vr|KX9Yy2<&RI|o7=JJ`Hr(x<~ z!GhzEH;CiJ)&K=D*{}Yva8YW010=!vJ4sFu?5Vhl1O|KiZ0fSwKvtYFC%PUbVz00+ztqrUB)NuO}OT5ZnJn$E4BM4C50Nm!b7q zP0ZT0no+WSjPdrOlZH`>CT?+*%Z@i@XXR}j?G@_Q*r-65YsG}1&3D{e`rP5Peoe=^ z;jYX?zKx5mq=_01Z4YUyXxBMuH}VGXsQktjfS&qU9Do0UZ-gCkCu{ZLW}F_9xa^3++TZs^MKx3PMMY7O|3Sw4ln_75 zL=h9e5H^{wo5aTIPErOjEr`almv;jy>ue4*HE^M)m-)WgO<;C*&VcX{+lBA(8ta#! ztnY;Dp7gL`Fcj6-9>RKLs5)R+*$QJ*q;B3N`%PG)(zN;3%^K5qVp9%Oe0?UT)!!#> z;-*AzDkG$HZ{*^h$)o4i69P_e{wY#(PxvvgH~(u0#q#<|+S>PsIBU+8OYaE54Rr%} zG(K+5fcJjVd>9=GzHd77HQ!az`yrGQoX2Xp>N-doAeeheRA8|5pw7Nk`NoZ;vZb}h zX_De!a&~-Hj!@{pUDNf+!Pp>Vo0`yRAfSaoyIE+s4fdW+JoKJSyo-S&Zmxnkutd~E zLfiQ5!2b2Zb^K^RYcR_r$~1Y6Pz8KAqf-2}`XZF^TYLU2E7e}g%kr;8;_}0RmnD<@ z%JtAjO{b}&1;;t32#XNR3OBx4)!5X+)t{;v3+;peUL%Z7S|UOZ_xWjf&GW|GAakq~ zSo-E~F)IOGdKvVnzAh#pTYZ&z6D6`s>{CC<1W%<(*>s8G-gHr#RxR#K$zFr4T5ajx zwCz$usY>|X^m@ZZof%iDO866+{oW6$?P-Lih71*e&7Pmx(r5{(%^u8>6JKrWXbI_~ zd+lRo!-dh3lWHxldz2YhhKkhoG_us#v-b6o@|Sv^eE*>jrQyQok4!~rdbN@>wtG`A z7axTiYztNL_H6gg4Q=*LE*flaiI$wI&6<^K)k{>$+=5>0`DNl0MB_!LM!JgBy~h4e z>a0)RQLkV%~3-{t2rz^J1mcc*#WGPosODqJK4E&wg*4vf<*@l9MN5 zQ!rvvuvy}6Zh=|iZbUue+ysF$yN;Cq{!T>mHaw4QE{(3OOZDugL}$IVdvK&UOM$s9mt^JMK`QAeO(`pKw)W z)xRxGQ!be7&r#q>K@~}iV-n2~rI3&@jQSX+YV{*3ig;F3Z@@J#G! z`&zn6(Xl6A`8!OHko5rR8c}lanFT~?XQL$-i6!1)&&X)k$jR&CyI%4!7r$Ec??H+J z*6T0x1-8TT>|OVf&qL^exrsaQ4r^g~_ObgCP}5rRQ?x!{!Imtw@3LlD{7t?Nb7n=E zb>zCipp-&H{zK7MCQMlvI0|EI^ULd{Pu1hWg<1S#i(_wV+I~$Qi<0JHZ1*JFlw(hqQ|3^tauY{@nC-_=pTM@I`%viN%F~ zPRQ*wWDi?%7Y$_(tH|x=V>nFvQ&=`6ldO_*7jwyL4f{CFa`zySluL9T=Ypwvaqg~ zj)jx2mrnUIj>`6^C6mNbut8G2m>oAFZbUt@Oi1`h7b%g{n=7GH@+gq|ShU+(_*dkX zR8qPChJyJ+4oUtbcx+<246>Y&*GCEIdKe18hYgZE!-;QIJL&ai>~6CD=3LXO`hxO~O0oXG$kdH>3eU`q^_*g?tNb4Xhm1SR=X#I0 z1FzIQ?K1 zyBRqt5x+-U{`VLpX$lPYHwAPz@G`4`GbmDVuz_+S(~YGGsz(jlr>&nU#6S9q7SNHS zkmR&y%O%9XM$2f){4V^_Ah$j86dj(^?k?{0!;q*wTOc6@I$A0YdMsNYU(_e@&&d~z zO9saMoO$EI=3Fb;*rdWmH)cCIpcM1vA7o^ZXN%m)Rzh%EPDg@yT2AY>?y-p}F$>nX zusruvCU%CtXN10W1kifK>`;gMP|Nx@_*kLxVd1#b@v$^NSH_s4`Y(voc5T7OkMc75 zYT+dX35h`kJ2}n;5s9z`1p)I7ElNQ5&YG<=R0rji-NTq1#{0z; zmEFVh>$g+i#;_~z{y1u;XO)4*p(V^U)0zC4^80LfN9_<6wy&ow-_Udu*S#yc^Lw5U z-dFAKsu`>$U8A;yHjcRulhV3Dal4*48(i6A{}GUN-SaHDNbWfFP!v|El0#!zq15r5 zUGF7Xk7gZvhB@rMSDb~V*zY$1w{SRqidx{~*I}>KPFSZ`C{#m@+iGUnI%h5vv2z~f z-wM6X^7=G*DD_J7X!cV%BG9~oJ89$|XM4s1F4vOMOje^&i^eW|^W70|5JwhU&eh7E z%M#)OP~TzbSmmI(hjO`1?efmZ**-O~)i52E9gIU$c;6?b0#4lnOjkl1AG|Ef~22rL+p4uD*t8*o0mZ|6I#Y zt!uyyd8*Kbs#s!~U0vgdSn6-XAEG9DTUT%tN1lrjYbZAnG6j<#!VhI2@ar4=e1u8@ z29J`fL8aB6r;budaJVg1Y|KWBjC4;sAf^UCuZ5j15X;?l?-aj(rmcNG*SPCCE2i!_ zZpqc~bU?so1`xzZchl)Q(aBx1J6Rj0R`(LC zz>HtDQuU;y4`Z%_Rb0nZ#Zy384!&9?{l5N>2OGEeom$gd(e{dE%9(6DA8x3oJu516RJq zEjG+K>w9c))?p3ph@fORKg4Ff-Mm);axVFkw~Yo zpQ7SdByw#&un7T!n}zuD5CzW*s6yCr5%ZAq?iZpzJ|U6Jg{(i)Y53LVX(p`oTgNrE z8=4b@Yfd?ovqmxId0@NolABtBG;sliC>#{sBX61Wnu`T|*u}M_QV6lqZiOt)NDo9E z38(5PvRuEwYTTo3=9Yw+@8gr>5K_w8uiXL+R%A*G)Zy-r$=-Xq*Ul&0oE>ulzUOix zQoiSMy*`H3fb0gjJ|7BEf84mE(>k4}k6){aRKym1nurMHg_WcBh%wa1jpe(2`V*uv zkSTzfI!7P%+rP&SG80=%^r^~z72fEu@G-m);hb`@DcPD%i*#FgY|Zd?Kay(2ZNx*W zdsrF#v9LGtiBQpRxR-UBS3`lhpiuOQa*GmWPBxP3KaiqrLliDqUCI2xs;@Si%jO&? zcj}f0#q8F|)>6KgxGBqW`lam7Qp`OC@fnJ{y=Ejbb_ztNS`KK9MY9Fs_Gu zvFsjX1?pi>B^Pa~GH(^$^yzC+u4VD`Id3cn^>~ZWTN^AliZ< zZv7o=$2C)O7^8)rL!CCBkfOi8aLP>nSr7@-FrqP(!WwpR@Y%6sar=z}8cmyTzJIlE zbi2)}PQFqa`&GeNJKNhe)ntGBwR;2cjb){}-aXoGBh^Fw(ql#w>hkI~g`$aa_Rw$2 zN*s0kN-FnE+}_}6T|BGS*PJaL?%il7&w;gLejLh@ksGFr+)-l3Zqf6dyzp%nI@ceD6A!{J}suntGU`rg_ubvUbiU%)1rlS+ugQJ^X@oz%};WH~nCw zHcwXQGfO%|gcJu*)HU}N308Rjb?AF))!H86b|Q4zb`{N(wc6F~QB6JXF9>=p33Gp! z2@T>Znyn1t0vZmFcW*Om9c)*xz^Gt-vdqm>WtXMosvZx{eFS$$ek8dck~>sJv}au; zPu_W)Q`!L}uRG8TcW{h7m4r8#q$Zu*FsN$bHoaX}@-AX^tL|AoxU?;0f4o(h#+GF*v=uT;d>-LrR?e<(t3J91_u&3C zwncnFRLQ8PKRBF_LeHUZH$yc;rpgh$NbOe(d7%`yMKuC-1&uLbox3GI@+Fm#OW$eu zJb8!h?+Hq4WoH)17p!NB$k!V~M$pq3x%FM*PjI^J(U-Mc{Qo<~^=At(QW)9wZHBj# ze>Lep38EQE^u>o0lIhuiR^2~B`Y`@g@bEda>j!GIintkl{oiI{yTy&ROorzcrS_+{ zA!UVjk6?N#fmTaOxy86M!}zU#>Mt4t)G38u=c`sR_D zB-*qDb;5AsncXj)yQ0Oa+8b{v!C~aoUMtWD37@8LV5rsvpj}+$KzE?MHlsP}JN+=D8Ie z2+!%r;ojI7!|tOadc*!U1G(fLS*F2wslhjPY&tyWET`1*4#ss({Bp257fEFzwLU{5 zR&8(ofmZaeR#=56@jKyzIHkzghj;WoZ~M`^sY39DAhM2AFRP6;8vl~GQ++GB?@)Nu zx?8S(Qu}AJRJUvyChT7N*$>&`eR1o<(fm{CKa(DNn7d5jt#CUybsNXA&|N$3%Cj#i zBl|9TTZ|lOb2o#-`oD>i>{@P=gvx%%J!$aJoGx~4+ow&1#V0XIK*0B+j{cIq1%fDE|JRMYkJ>*$86#aZ2?%r zA6D*_qAG}hqTNv;#1G^&K;iDN5F88+20NYo-r2|sLZC3}70(DmvInx|Tgh&#y6?LG znnNW%9E0Bpz7IW$o(L$SaIIAJsW&r}9x6#w*2IsiV}4tAR?2Jt*HZ&L4Dto?rZiO> zAGV*omioW$VAjFMulJXeVgB*w32x->Ebv-C-7(WT9DDq(lxy}D&&JW`oyMwBM1at8 zPwN`gHNfV$XcV2y#}~|-O#AmeH!LilSTHPYLX!}-S@0xnMw2k=a$6N`5tn8F`DpAW$@?jPcscG#wjt@5SL&1OKs? zDDF7))XlRtaSi|01@*~iWcBR&RZ9Tv>VQ&k*-B9c^Kte6q2(=L<7&1&-`I)m#LNsa zGc(5=vmG-tGcz+YJ7$id&CG4)n3);d*yg8v-#Pc5J9F;Ln|XTD(qC);_O6n;dRO%V zspH|-0xi}eSA~BpDG7Jr!i0H*hyXrF1e=72#Y*sY5eylXpH5*^yZCJM(R9FJg|q?7 zrL#P@3v+T|t>AJIIWYx-raY#mL;}&fD1Kfzw(s+I6JcWV7<61+8Wh;a8O6eMyiZ+4 z0m`oOa$jB4HNY_s&pPSY(ZVcTN9yon(9#CpfU`Fy(~!Zf@aKD9dp4OiLRpV>Pf+di-fC z*7h>^XRWk4HyHBpPPo}^z;?rl!F7l*$1B0=wi^VF0-+{dNfSH-dlIaTf%e@)7B9}d zb&v0Vt!+@UMTP}8xH}x7GX_ZJxBO?47JnhesszIA5Tv8?{nJ5fK_Ti31EF}MCxNlw zAc&C0qB!#dsM0DKBP7z(!^Q(F1T*BHe6c+y#>h5WPet~gqd)aIU zY+8H{-rti9BY7Rv($%d0QQzoI*y-Nl1v7MoarmO9VqE^SW!Rfb;uop&g5TW}WUxHF zPK@{Da0LHqdr(uzTbz*59et7fw8{L}3p58vy0~NMSUx9pK*!wv4XATA`7T{uC4Kot z^7u7&L4&d~&DQz(@8+c;i%fE*!w7DgKx|g$MUp~&@l;FhR=t78ajrgr%e1FtXX>2Q zUvjKk(>ig}>~(=y)Pkn>@Z5dweR0p*wfi`Jdg0Q2dOrn%;|tdd-QRcoKK38oBoUpk z>eFc#f;Qa?KPQ>0&OX!M>>^V0DUb+4wTCeRmy#uoH!fBe#9l_hEZOF!}#RG zwi?V)ui%hea(-$Yw@hifxTU z$DeGvW7TNUqx+PA(jjTpQ${0>`jpv(zvN}z-ssV5+GzAIw26hg{|r({=X~j7V(z#c z?>3p2xE)5}cNF!*_s~)oaCH7IL+dgtW9j${SMk(#<4+Zz79AR@o^`3hB^#%rFm%omDQl`9KqAvWt=nbv_I<7GU zA5@OKQnX$==qiRulGN7sR5(56Y3s@gs%bWRbj&?-?K~%%?cK~(RyIzbZQbzw&a!<` z2WcPNr=CsUE^HKw581mN-xuAE!o5eobZB83jZ99XK$uU zG;HFzhmYp~uA;&PySb+rU5*<&PZu-;pIhjfcYSpZFV%F;e{?^X?9R5Pv3dV=^tt%R zjO^~g30$zAPfJ?%*5)w;e>L?*cuWUzb|06n0j@G}M7(&k4v3Qj9vi1WV?E!O9;e@j z7_L^}2u_)1qcbc9pI9eU6`5tkMX2(l4aWOE8!G2nNz~VxrE?6(Nl3Iw3BX28zvpf= z@7SonYvI0jKl74$Z%cNU*V2$ttN=x`(#_G(YzzxzWf~$PIGATB{)XyV6blEtU1)3X zlNZ$nbpTGncZ6uh^jOqcMMEAb8S#oT`j>`vG1%kNs-6saO_II6~oXs1B z_h(?f5w4W1P%AYRJ5(`6UL!?z=$BFhM~ifA+#rgUUd|A)ps*0Jfk%V77W~pJfsh>^ZL8P!M(E?C}ko4p%rHG_s~E{*jrau{ zS}Oqx?c-W$Wg!@TLnJtLn(kcGf)-^(D!2(%)}w%0p6!?$ssx1qs=x?$W*d#bgXC*m zn0$YK>Gt;Q)s=;NOI+Bq4j*4!7}nJWMDN9z)0DY&Z=~A?A9|u zm+|+h*dVQsYfe}7>Sd!ED!Lnzg>P3yU|56GKmg3%POoTD9J^skw3chenT&>b?T0wgbNjQ||!{Va5D(=LQBy zaLtC>s+GJ%r%i^-8tSf?sy{=-$T3sYl?`V!)Mxr51ypZq^WyIKDi00#kxn1gzdt*F ziE`hu6=z6G7&F;(#A((3H*Pc6)L4}Ftn?L22c!U9h{+WVT^NTug6g*~R*KRr6|K>h z44Ioo#!fRYgqfQnH(t@-8AyL5Bn(RwdYL;e6^kC=6 z%%S7PFAH$^*n+}LucqM%Ndv47;#+>2z|k??e0Mj8Mw-~l$Kex)NaNWooO~#|;b-ZV zK5!d(VaV7dziBu2O&h;VeAM~>0)JR}&8-kNHc3>SEB>c@?142rDX_du&f7khLg-Q~ zp>v#&;!{^AKXIhU>S=76Hhz|vp~F%yZR$EQ&Y&?*{;SPcC9U!#QAKAaT7o8js!wSRB@9{X_IZ&19nQuJ*V#mJ*V`FFXWJ# zPk#HKD7lrQ)sdi108e>a43_bSdt9Fvx^CgrCl&YbK9M=o#ZNWcjSmwz`BB3iQR9u> zr=%fiR2`z*`=(+`0sQ(^G_?h&nkEh(+4#6q+IF5Z__1yJv7RE$uTxCoH_e4jH226Hi#i z`cE55-TLbzB=kk0>lTeKbs%ia1iCeB5zxB}!F8?++oc`Z@T+1TCt!LOK}k33A7j$3 z2NvJmCDEVB!g(nf+eMy_T3O8^kze<<{LzcRPk!RP>n6)Tr^2btX75&|f_k-&UyjnQv( zO7tN|qbw_m`2-j%iuqU=7|P$FFbd!>c%<{|(f^Vanr{-+&PrdQXI5CM=x6_yx<;g` z)x`#Y4BS`)z*@8U7&eoIo^H19A3{4@$70D{#BQRgRKmW0vM_3DMGBZo+-NGB0`!Zc z;z?Sg8y2E+&}j6#`w&@X_~ctzA%2CGsOQ7yy<5oS4-eqe$ODe2SDg&vmo2%0=ab=MwU!8~+$}JDD66+@3b2VGv?CD8m_u5;ESPq`dOsG0JI~mT6t0&6K8FC}N!P*6VdddQk zOt~k>`vPxV@(K==`L31bg3CY6`6C=XqI(L=!<3c%Ks3(7Jjo-(&11}~QPP5#cYe~I z>2|^_!@%(g#_EuV@{P3uGOJyX~ zWqVrIYWo2`I`g!g>%=I9RG5U)JSLAFatw}+5zx_d-8oqdK0i=6!_}A-i`{RvC(K5| z>Ipe&jnao657scRlXcp*n4;bC2w}KcdsL~(sy=LeR=rtRC&aiKh1Atx+IZm6ZL(nC zI=7hxH3jCtUC7{s9lKvR<7q7|K_W3YXEDNeM!BK0$Qn0G z*E$AjA&glw(#7X~R(3Y-9I zCIc0}hM^A=L(N61QVGBgNaTa(ml^q#BjZ-C6!8Qs`z36^We7x#+l3t7bD{qQ@@t6& zto``fj9yW%0+#H+gxXOv`*jum2q^`WKlm^ZiwaUC@pXhAhn{1G1xf^fsgQ}Fc9KY8 zsgQ~Gw<)n8IfQ+{Wk^JK+jbnQpJ*apw#gMGC6!S!O%b;ar zzJv051PSU|B;Z9M+(b;cNDN^qJ{bMzS)gd5ljt|e$3_L1upVT|>6vdX9C+I4R&i=9&!l{qUzn{L{%F7@hiy}^j+-!Nb zd?@J;u}2yA&tQUM2wg1{zcwL#tYfateqY^USBIe8cFFUJ;M|(JqIiK?fyhFO;Vxj7 zsJ5x-g^L)axVA>~XiDH)Rae5aScUYSx`5H@MPA~!YL4xp&>m&P0>r;KvGRIY(XhDB#U~QJ5ophbF4s;Mo|$in!Kx?EGGet)~=S zLp`{NzH1c;A{_uHtRO=7-hbm&YG4)nBaN(QkfK(WR7?v+*b=`R=-1&ng$XxeaB)a+ z37#hMH}U@=27dVWL#^=PtYKVE|6UF`?FqDztfvgT5LcaS1G5Ym#y_6zuEE-ga@Q%v zo+Xp=?hniTd$(04=)2i{FtsAuuBH)m_tf+~yv@|w@QuVa+J+X^zU7ES<6hy4P(diD zI^k@8AtC8xSkh&$h{y2<0e5+9wkXF~!i1x7z4j}1LNyVs1Mv&H#M=uw+uq!!cE`o+Wxb-~gNf=5+HWXC(ETVL1(b3p0NCvyfLS}f_yyb# z|Du-2v4N=7M}u<}T!IblA!LFS?`=$ISmw-nt+VKseYRP#n{~}{U2Rx>lMWVR-&WZ;!>Sb=(0WAC&V3{@? z+hFfKZo$YRC$=kYGd}!}Ly*jS$Re=aS;6rWZ?<6UEvs4${gP+Pe#~f;=3(hfZIL$}|@YuDa%ePV!##BY?J@J*k zpb5~i*Ry@6p_eQsZ{|An7-CKytxcTPg&x($!KQEvk~Iqmk!+F=AuA6iQ;8|cOxRqe zTP+ncNHZczJQ4cA|Gff6^6qOnA=!LTk!QRz(!an7hd3xxic_W=8af!C!6_crQ@Qdc zzd7A@c6k4+iD>A=if=zKh9@&BAJ#!K^9$IEk%0Y!x>qfEA8DC8lB%%A}*^B zgNk_IeKc|AZ7Ll=c5lNjv&`r@-*SWR2d%6HAG`M&RO zTBG-=YMb>{NmsqOqSBSQ!m)Nf95fV&`&o3lckc7M7zE4m1~5DR?@}+EDn*Yq)*HOo zAsJ#>CW`4A=%$z<<(D3*`&xBW<8-jBQGMeDO!Z}DbTl+kB%_*?2JcC=?_}wt?{F6V ztcrM>E1?x51(tPXnRFEtQU9&WQ&$#CS3w$eGW@q2NgA~?T)Qc}ZQYcH8$EBs z(U9@QN^3)MneV;=JEYhzTJ~YE#7m3vW+YD;Db#6JXhB$Ud~W<;i6_rCin= zcKhBXa&18%bR4-U7TNSt$n2aa3~zDevolsAxMT3|bKOa=n(YZ% zbu$y?5+sS=VWeo*jI}5v%pE1U>G9d0lWytaJr5*ux)D&#ovulBwT6BiFJo-~$qBY> zz7la?LOc#@eB2{B6w&85tHREC6qOmSoUJGC`+z8erkr^v==5Y+{W)ApS<9EQ-&#q& ze9+#OYNiEIQ+1%VAS&kFzJo-*K&SEK%OSaNKIQpZ~i;e$>WdZ_VWEWlmZZbMJ2B>8FiAD^LFsx2b%X*5&kXx8AdI3vQ3BDZlA~XWE&i z8HUc(BAs90(4Mmd^ykP~&^GH9mOZidUy*EITwLHgghapL?vSh5M#k{48;uOP+0T!M zv!$%&{n`SYNQB|q$th|$hwgfIDpP{q0PLX4SV{bfsT~wFRJR^)=KK-}f#A4-PhM{6 zux5Q4Ob86W-4H(PFt9?oZ{8-ebTO5$TzYmpA6ORugj&%k^OSn8D7`K1BwoOdc#WPg z|0%p#6<1RD;3{z`+rW|cI7&zBX~*MfCwRNzaMXEs)Omc=xjy5ChUPQvZIO!6$w|=J zOyqglBKkfoXH7tmjQ!!GK~@=YE&bSeL4Je~U2_$!oD1U^{nqO6o5Z^7+SlA z;TnZV5`?*2A5t>Rm0^mDzOk1mJE zmgh59^uGH-&4&|=97Y-^dN%4rKXc*)kuj!ECP0E$FOo-3aB}+rq|ds%Q_Jz~=)!P*Sz!%6z{Nr>ql zZZI6Q4qeAx1#;s#D~;iew}$7~c3R+I;IwX#E_&^GPBl~(vMWt5wTUhE;e!!Yj38ID zu47`UY}<6%itI$5IQ|m!r`6b?-R)0YDAJ)9 z&T}@sU>4A@bpog>X!x%nAAdabH3Q7!%3yyfVz_EH_Gj|bYeZDoVpfXJQ_#A`RZP%@ z#?IbQ^2^f|PL?~8sl*obCG1dc?zKFa4*A?MZ^TCqo1)8~@)CR38>Otr9{UV{I^HLD z9U4h!TnP16GY;ENaPFJ!t*e@i%xhb6K)UZLJD)1)Yny;8s8-VX&;Dza&KIOehBgt6 z#py=XIH~GdfBZWX7o2w_R=ZZKI@5Ko#pB)dwA5@fA;>h&nHsQH4QfL{?;`w;CF>kK!;?6#h9s*c>-SKmxAcj4k$ccSveF4J3T%qq-`0>*#8}rp z{}r76GrGU}&+r<$rqA&kQRJdNRC3;F<5zRi01i~EEuZNps!z)4V7xpaa0q}&V^(?V zI^-a4{b7&{1m!Jxkb#}DTiTD4S3_^H!!CXeR(A6B;eJE$hR;>5AuZ3Y{UMjfgl4tn z1O6ZkMt*~?Ow#`e)CBVaYz+y#r7o$}k#ZqsvX?%XfFaH04M|0#GhPdXuZPRB@sTJ4 zyPAp|#iP=)``+F4QITYJ0wnpEB;_fbI(!-aRI%XG5!CwTJNyu))g+&2B=yLTf01d> zc+ieP0g#WP$b1rt7!z*?Oc(3(8WS%eXm}-a$&Qm4%zB*H3^ep&6w5XYmZfJ?Z=GAH zXn1d1MRIwqhYWZTR~?pOMRIv*1K=;?*9lnKZ!kL$E_=L?Kdd?me5havJLj^R;J#0> zGF(W|!7FgI(K~_>$#aC3X}!-W({5(Ras2>abh*2OYyd*uVIpL9_!QRyrbd!}VI=%g z8!i#!o*G6k#g}M^5vP2Xy~JH0f0nvLPeQOM*wt;nv%9k^N|6(FC~EW40rn_h@HKc5 zRwT;B$)(2KNgL9c;q;Y#!DVI=pcIg>M(bwq{kg>9tMo!qtK)6!U5$=2-=~^1fG?+k!QLnGm*!gC~u;P<^2e<=pEqShaYU3NeZaj5j+wezR;2S+IobH6B zHV7aOk%v--Mny!3%;X4;$&AXNmKx)-#u)xMXc)KHnH$f@s24RlI2&KyX&A2SqIJPhDu$g@hlkkk{`3%%6&rakEZ_nvf4-7sWHRNG-V(4TVR-&K&TKBc&P zH<7g|i2EcYMO6IBl9=W+ew3%E4zawlzowxO^T5VlTpasdSJf#(KcRM7~g>jVar zdAgD3kRtk2@E=b~mu#8%Xa=zt4ZV7DuK1VW?n8a_^{^pG4Fs2n5dA+IpyPjn=_h8w z9qh@!Vr0TZ{8In{3%q=Y4W@fR#_l$X-mE2pFmkuXj}9~1QtuHo1WTOx^~rCQ9(YIo zu*TGe|7#_*5xYZa^*OY^FlZZH(4h|c!g#bGOhYg`e}gALZb!QaY~LoN>glv6T0=tm z2FLdqska~SL310$9i1>}Xu>Yjrq*nE!wG=R2R6ENLl?3M1t})5NFWX3R*~-Rg7v0? z1rrWS`*FVQ50{;=Ry;*l2ozSZEt5Zz z5O(L*Kpro8KgAI4@CtlYUXVKO$*eQaDcBZ$x<4?^5N?(k;`qh5s9t%KE66jSNQ?X6 z=nc7fTl#h*?4EA3I=SfN)!fDUSyVqDmVdszlP8`xGEeZMGwi8|S*dC>w>eC~`{-`- z)!L{!NZk-2WSei3;MRlgxu=W&V!TdpGvBNPhv0ERUEm>zYPy>#Z%YK-J3J6c@roE$ zK(sfE3XBXzRPe9xC6#p{8PzSIi}#pc!QX3JpoNIgAJZL~FfhyjqM?uUii!_9t{?3R z#vL^hfc!>~6Ig11!uN5i@6XB|Ir4v2Nh>-@RMttXkpAH!S-~JVPhQ?e_Fo}QZpCP6 z#kJK_rkDNk=0p^`R{sa=&#OI63awi1`{63Bp1@IITGq*>XlB>n=I5g*l{`-9qz%n1 zZS9V1R#;9G-Yv_D^n(~qZdWKw6*mmqSwzPt=POS*-iI>jP-^}Hho-TuALY%Hu*TV0 z+|qCCr8sdpei`rh{m`W&c zl#=W+P7%m4YKurxm4S6*uj1tMHD}p1Tcllh?}e}nhsxk`cV!ukcz;{~@fYf&j~4b{ z|J2Rz{0F7)LVN)j3f)CzdOzkEsZY}8H~{q4e`-rT)zoL1eSc&H#}&exl^s14f}65Y zelCY+dOWK7(ztqR(fpKoOpbc}lhSr2RZ(J^@^zw=ZY~l=N!GTwzV_3*vG{SK6C0p= z@~;x+7km7dL$l;J#;kJGeKJM4|4H@=v%Qm(;vBCP5kQ@&#bBNcja+yhflneXSxdfahTVIh~+dRqNH-^0Z|*+;hfXbww9`x0~o@7>E>36rO1vAxL(DmPS^M z{(6SnE^cl1(A@lcvn}VlCPfvWsEGVZ8se!? z2J=tRp>fn9dZ^-BxEV=kOYN0FjO3x8UOFI&fj?T?nA<2y$zZp)^smPJ; z`k2Fm?Hc@3Uu_A2!^3q*M^`1WOZIoONnhK+u&H*% z`jM?a#vTozR@=%nwRih|QD(j>tnA#~3@Ry8msSRghS;5VYOpnE6w1cFPl*Pzp@7;V zU@ZzDYovEvBs&-)7Rj4Ajd}zllQ~F=t8l4xF2mN?e{{9~V(EJRt|6UVNiOAGU~Cgd zWkD)+yH+Wn_aGM=?4|-3Iw`>+FD58VTLobS>7-;ZlQjh8w z@DMc8h)*XIO>F9Pf~%ZZTRA)kEX zEQ~%U#ysQZ7+sptK9(29d1jQrUIk&1MiWfIy}f=mGQ(e8Ww~#P?xZ)e8(Ugsxo2T- zQX`(Ji0`B`VjNoK`hIkUj=%EW!>isq;1wUAdbYA+qUk`v1lzQ6zK z7sC?6yB>R#3q?zm!!$t%s$u@7WR241mB>G7{S6Yd}}= zu`7h<+sychTPUflVzwp226g($GrWS~X5HRUW&-ngK#%XoTFtXRG8RLsq@?v_0eY2 z2tw6_M&;YORE;swP!+=}~^4(SvIR2@aLCFvf70ar)yxATvrm~hT{Q`(*U$t z7ptDNGbT?IkisnDrfzA6&Pf;TlBam`)UdsNQiF2Cje{#|0A^CvEDH==zOcsHpOiba z0%g>1a({K@5#$aVAS_nKsMVWSb{RmK*Je*^vMkn=%(eWrBITDSxkY#UX%Zou_G7>) z_OLbb%Zq!k+E6ZC3K2G+k4(xB#|;`*+ND2Fte zsuIWu-8|aX>(iUNr_z?+CA(4PbwbQ?pz1u8=wVCS97E^$-2S@n=$6N;klLbQo2tEy z=n^)%xK?|?)%yGAiCjwY1)E!@_xgm>&7S3hNtb7Z?rwToK& z>0TCtVBProo^5KD2){_DF-bDoXu_&6t1#=HSN_X1T`T`@3de|-uZ|{rQmv#cY^X^@ zVuRv?qu0a1!(@*c@c|qg&3e@+tnZgE!#V{lD0Q&IAE0pZzJ5eE_7Ch3pO*gON$sFY z6S-qJIkW)g-3+_1_$|h`7WI4tbwL@HPl1Y4UYjQ*OZHSi!~**IWb}cMMT{ge06RF0 z8Cyg~8Ge8XUIPAof=hi&%t$e?o0CvA6^yU9)%{Fi*sJG_20}72OAsO-&ONI&-t6l4t&(i6W>q|e zZZLb3-Qc8@Cp@J+MKba+@qAxx3es&oTY7Iw@iMx?imK@L!SZc%Ti0pBF!QpiokZ6- zoe;JnU8QB^F25YnS~cJ7XP5!I?S`4Wa+KW4m-IYkMDHA}hyQRRp{d8P`G9dGB(mY2 zM1EUBep|Btx7~;AeW;SH^}#rgp|EvB7ivf2DU53E>q@$7|Ls~RtMl9@)$pjbxjIGJ z?+ssn#bsLfL4Hf}(lVp$(qbHf9lq0(yVLV=6Bp~;?=AH>!eN#s)46UvxVvuTItc+R zD1Oow*g5{bfd2T9VSe%i`HrG=!8s=kiHeEuNE))cNEn^Gi}f1+WhYQX+ddp_QY73{jS!t12o=_{g^Y!72&v%_Sjoq(YhIyC zONk~QA3-?`L^Iy`!UA&q%lM6V%HM5dRTjV0UCe=D_PgwCDugs-YESe7Eo}uiB~aBVextD_OZc zOtd&>W~g$|e)*Tkk+!iv>Pef#8yo`2)S?e=VrXQ*Ahyb3mk;)72HT>b=*@8oE8h}b z6QQ~gtb!txvPb1G6gMr7*w9P4>9a8l0-OSz0?e`TvBoiC0-<_63R=CIR)1*kA-Wmu zL=Gb)-~m+AP{?{7TfF~9F(r_2o!yKmK@KD#+TW) zOOKS0rEZnL{I8ZeY_@EaE;f11Wy-4KPkBX_(>I$QWX6Pb!$cHvHdOD9IAsKZ$?^`> zG2DbX($ zIt3CnH9`_k0KACSx7}$#AWWA?3}Po68&i& zgB<9>ID`CjS2=sI1G)j>ksu~rfWYhuEMt%OeG+1X`ztq$u@YN9Hy75#4CG-xx#l(z z7ZKK$uy%0N46e;IX4=*d?a(m?sA}M!|AfmUs2*@>Aj$n!uk06ohDC!;YKkz%e_Gs_EZ?C`vH@$W zhm#{%mEw8Hty#$kBsK!t#eBqWebvIe`#IlhtlqrCp=7xp8!J;fv+-w*7+Pk5mF?!2 z;%d$!Jg2Uu=K(o%mmyvmT}TgJn=tZk7tdSFzla{aC|cixj^ZHB4n@N27(WVinemVF z1j$CEgyhrhsslpntv@{rp#E8}48hDz)=*cvsSJIVs)%%8#|Yn~foJ4MF$Hx!p~M~`RprZj%@0rTb2x{}f^ezN9RZK8{6z(b#B(Bj3>{B`-M1}oi8 zuGmX`xF05HY9%0U>3*mocT4H=4OeWzjPKB?q`N0=DWc|^uCb47JDykihV(N+mkHi8 zAouw4Ti4Ed%-x5Y;|R?%;8VuyOUSDrkSDCu%OzPIC>RAkT=xnCn`r^ct-}49axHc? zL0;GH>`KU)k3hkbQWoCv>(4hd1*kNIuoanQItcT!sX8Co701w*8<*t>QWpoP%Oqk{ zh)O-(9O)bI@`$Ibf@d|};R$mi1}SE31MAZT@MLGkjh7D7*ODw-4F9%vj44X!mBacC zsFwJ1Jy9=a&3aqBm8O>9*Vbg8_UPj=jYFDL7e2=&x!p&%bm#V@a&tFvOS;TCE;gTW zcgwE1VM>^DMveEQ^u)tuJ+IB~xF??J4JiT)6VI>E+XnFBn4$hFKu-96l>W~$Yz`+w zTz5vY_nlTO-e0T-I6A`4x9~xoaY$FhZxICG;p8VS3GZxDfk_lSZh?=7;c-sDA?}@B z{CtL3MXaH+D5JRn=93+47l-IJE^!_F!v7H`wsA@7WEU~W{lKCuk4l;wkUZJh>(-`3 zW+{GSqRAj@boi||aCVhc|1_fUx>xDzsOl#>*XQaT%~+fD^cb(kS~@rt&9gTgyIG`2 zJ5-*cR7eCYPuZOtEoWvgl*0fJSRR0DtzZh~#vLpVxY(VtGr8F}Oe;*~VuF!f7(Tr6 zG&)&|26Ht!Wv_6{o#VS*>^?s1LMRAwcY=njU|6#8lg?#WL*xieGss+=65CprJTf1r znBsQFGt6AMZe&Z8hIh?C_&Gz6bZq2^LmB-J2Xv4_>Gi=bdg8UzPPzuXH7s$y;~JCK zsFhnFEApOS14filltsyls(+J1S5)M6Db2Y>KFQ6qwN_wS79yVPpeHHcDG-$gVwZ&n zR1o8CJ7>4ViD@Um&|H5T@T$qoG@Os#Kitx8*m~Uaj|7)$W~8@$sLJr8u4XDeu}mti zW)?f~Y;qwT<6*jV)qJv>Uv4)gDZen)ILVCgN?(M#ojQR?%4ae6s<$OtIF9li1_m8X zT|>AJ7nM_;!UMl9_@%otg);F+yK>pUlQJdV5%p4))HCB6F{R!O8t~1cF(sVK3nR`d zeTykB0d;7v99K$gR$eoqm7j7D8ZFbkO_rLMm(B@mo{-&r2Rsz)Nh}YjO3A2q@+F}6Z=`D8l)+yNK0)=E>=}kmV(}xSfKi2PRjBl zhauXAI1Yyq+`mA)o4+ok0=0z_XkU(h#tUxWfzkK#{)~wH&uLsi0=_Zp9h+W&Mf5Ve zj^~Wzav$roKV!-+JG~6ZF&clIc%t8t)?hsbkO(St2flp$ri%=a;nVMj5e(OXFjV{c zVMom$j4^GG0$EFkEF`83$?ofqE2uC}_HkZ;fL#Ht@tZDkSy05!f0-Jm((za1l&`wPRE#(KmSsAhADpyntSb?b1!^Iuhf^nY)cu} zC%R>w(J!HfPgH`2@pKE1g{o&lCzZ8`H)7$!oK4y=-Mp!rVuy(4>`t1^Ml5*C`4)iRGQ*yw?^~cVSK*_|;A4y{K zNVJWn_$vSVud)A_&e^G<$HP2A6$~@{=YcNdJ;_YmZ?mQ5mSVD1W#t&`jRhsDb90fI znut|-J$ApEo#6F&71#k~TX355KH9QFJ#kzS9>;F^{C3Mj<0*u@0P5t) z&)!;O3dqiu9{-8Zp8!@q|GhA$kOM-Y!QnxBhR5e{dMZ< zmyJyKGvPBwe{hgSYrds2HewspryDfjqtGXz2|$VTYHx3gJbSGUtd;#apPHF!Ak9Gy2lE3$rO=vjtChzaPo>!_=Q4C_tF-M!qIB!t4~Ev{ z_T~1CiX|VIOOjsKrn*Uv_^GzbENeHI?l9ls*JPU%R$0P}#GrP)y7iwMukcaY8$@Y5 z^Tl#54|TGZ$`4x8mo0mm7Y`fi6DAiBmy-^lT{l=~KLc)`*sk99;mFIA`TINW@B_)uj%Ps%Qn%k5A?>o_MYL_i1nUV; z@H@V~iys^CR}(glg1SnWek0xHCX`V}z!wt4VvFmO5i$=XC^jr($1r1$we6Y@89`Ez z4xz(JQ!$GIg8|j`w#dGlN?qI}TWk)QpU>f4LSZ?43h85p-oWcu9uV3Ha@C!=IC_T4z zjk8h>8%EHL%!e|qnJ4MccV(;7^6iyfi>`$9VT6pLT8Ok2jW&+D9$R^_a+&?HPpzggD7iGL$7p}b( z^=^YD?}}bZ8$|0KC0A%&!Ny!yBlqQe%SdgIqT$* z25~%jXRMLB1NvZCu77dBKyE_uYb}C5R{+I%;{l=k@SF_s>bQ&Hw9}7s<8C%jO|lqA z-s}c_D+A^9-CJG8er7u0j~8WMH@?M!KE$fM`}ZO{5^sT(e{DU&MBXWvb`-N6U1`T; zfk$lCZ&SvfDQfepcP&4ruq*11tphJbMcrY1s)tV-BhisEN@APqJ2Ff@ps|U$3Q4cb ztCwD~%&z1=D?rPyQ#aR(X3 zxAeo3X!Cvo20ZvM8bGb%oc_BaLy`bWeklh&)ZfM${)SADXamCmfC^zr?>tlg9Owqi zZiu4`ckQnoLFY#DUTLTWCuQ zytY|8b|W((Z%vVFoFp$9f*NJ8f2X?jG{|!<$v3OvN1Pg7>I+~k(XvlrthSMn$sIR8 ze#H=1Ei~=|IbuvSU6k_RZ_Fn9sZl3vjkRF(t~kAT9PR293RrlYzekQ!wLKTtiyu$W zNIDqxr?YG1Ka}%Zw9nL+bAY9K=YDx1QEfW%1^Y5#|oXS5J)BTAx~Hysaw8v+j+kBN&9sZ*v+mo|_ow z;~d4TpyV8lO3atTv{CytpWvAFjHAPx$`^)#sL2UBkUSKdFvKE#!Io^df zb>y5x051|~gO}RPbKSg%KP-!UOYVK#klSVxMMyslz+wf5IX||8&NPnUw9VaTiNLxM zQ+GC}JWZ@@oSe5GrkW#m2d{-oxujfV#{1-r(uCbcAWhGlIFS#9yEplOev6T{So4%^ z%asExBJR;vcjYQ&1@opxOua6ZS}!A{HCb|N=hIoP9X zf+Ep~>LElY&k2)US`rik{tP|UgEn8bqk@9IsRw=lMNPW>YJd+@&g#)m4N#w!54iko z@Zy_6m21Xzd<#OYA=y%=i=8fXsTUcF{n&-6I1d$I_e>=MjG+DUQ-RTE9lgNSbNT>5hKu^lo> zM#^>HJ4*U9txE3aCvgv0P44>)@2N4F3CE{dbMcnFjYL_qzxBI@Dn+#YLWG{LklO&h^LZ1Uw%fTupQk$W2vqdngjgrE-j~Dn=kz#_avtS+QZq zB3Ptxi8MtWbs@6NIyL_}l4UvIj}@{uNwao~_F5isf9u&+Nij6ve{QpdN0j!r5rR^R zt-yqYk3{Dte4sCc%l}NxqaA?FclZ&3KnbObPs&PJMd{bumTN2woXhub%vAm3eDXr< z3u#DwiStG{uJ>el_z(+GMmlEAEgV$1YsDj!x3dvP#iFmf_$R}9L05&mY$jxCZnQBj ziqqu>?v%^sb48d#O^e?dZ!9LS>uwJT3rpz>j!QJO42`JmjqswMOd1Nu{qOX5LOh$}$qz4dp(pC8WhiNuFch(*AFFQ@GVE^7~ZAAI&kGEgoUpl~CWHG8fBK+CvQ|}7d1_AU|-YY&;Q4_u$ z`&6a|c>^Yl{FT?BWi{4Q8+|+k^ak!xNBeACWG}0o-|nelFThI09&H*F?xaR$vWi9TxgSTO$gI17p*woXLeVW8N% zL(4(Ghx$gn{OLgM?CadPC$ud6D0AKY6JpTDHqutUbtOx5xU!n7t)9rYu~i!N8ar^k zrFFJV%&OivX1l(n0%#=;ROVcTzV&KRTmf|RZR@jbB|(ln<5pu88a9-V&R7@{9EjEs zS-2<`5)##n9TvN*K^aFM#s^t@9MZKqgqUUDU-8a;Q}4c;Jm@4-hj(yrWu4axz9@dA zXT7d}4>n7v0H8BSP=FOFosV#s;@yg zpY&-i@Kk(}D*Fh_ek3Rza^$Wji6luI4!yOcQ+dCm)dBQ};H zolQ^672zUaNbDQq*0mum4L1E{>i45^D56mkBgpYC0D3!&M+6;>Q;n)KQ>2!WC)dm$ zw6571E~JwP5Hy?H94-T(zT6#*IBw>9pA z;0(Nda;t&>*J}MFt zd^P1v2Koq*+S7o;P?YBLIX@-Xe~Lrbw`9Sa|M8Zk8>cy+cIm2MUUUc@-8Nt&0pzp6 zoe^B9yIbmUB)B|Yx0hPCO5q`Dl%n2$X#_>!DfUy4d+TY+wB~3Y%&uQkAg~cEAAdM| zA|WvSm0Wk*(_$`l^ia^1;B*BQXcq`li<2=w4dFWtK^63sPEZ>wCLJyW7n2T?>cCG1 z0jwo5EY1t0Gvdec=*~YOiWY)%NJTYiC_jcE6Re2`x+(tK@FbYdGpr}?;|W@_Acaon zcn_$Mql{^&2-PvGOZz1!(Yv9*Sg3!Z#cO+3ZDggx*j*0z>{7w$P7FKRA-`EWyotsZ zM<+D$ML3zdHZX=@BaICI#!e9V;+GsU2Pds5M=6Py5hzA*hL^>MV7yX99xqY&cbXLn)fy znD8U0nHWh%?RR)xnTj3Gq_1Rf7*X)0m|k)<@FfU4kSP1)HNI|02bz)_4J!NpkGLwW z0`%&u#+I88$plR4lrx{|CG%w)zQIQi^`n`h#J!#iu}glY_Ppa;h#j5jD>Nn67s-K= zd8k!1FPH))e|*Wu$Nly?`@@@$s7<9d-0w9L=~#zi2~FR=pHL3&!^S0f{aoUW@v~5L zC8>#60g2QRSpoQwfI(Rgh5PEp>nsJJ`~=$WuQtf*y=gZ;SXC9bXg7XD9_g%h|HTx^ z-1M_4QuTZx&G^cd$fMx=qLk6$p^k@@K_DsfgnCx#zV$#%bmca*MeopDZ68I=Wz^Sk zRb=I?DWQ{K%8+-8^qR9dippcT+Q|M;J+;;wO>YkDAFebNQEB>>H>a4yWca0}vB*+|iKYQOwBN@&Wj$`F1&B!vUPSnTIfB-k z)as&eM4ilg>#C;r6+0UYQk-5f4a)) zoR$wl%XIibd%naH2d}0NsDieK9)b!Z}TI z-EW8ya}3ZW=h*g49N6vU@wR#F<-_x*LrANFJt&+hDhX~2^n7(;7^hr!~L}^2$C(V=tmwT;Q z77t=hZf{O*FN5dG^Y1Wq)c(~7-?3)zR4GqTZV22(E(Zd4k;xTGIQFR@f!_S_plV`5*MW zK70Hc82?DbQhjGzJ$5SgDa(YV`c4n+&@^k)ry)?t(6vHwL1E+?r2c#_)hHDwyDrY2 zE4$3G(J^ReJ9Rykmd}2~l?^*Zs7$CV%y$HJTY|H%mWhnqb7)PG&DB+KEqcb0`Y}5T78W)Bm(A(d^})^{vs2yxM3%#eZbb+$7zTKtQFBZU}TRc z3UA){;gCI}B_A8tg)Jy2PjuVY;NUx>2B4?HyB!o8pfk+p`5EFLix-F2-QGVKmk{2w z@qOck{hQOp4ODycQ*Hu0nWBVAF8gB50@d(1(cK?XQ^IYxQT2Tu^B!8V?=Ifab$=^Z zD)3$_oTx3Z?w*)fENJSk!SE-m#h(75@HXSQX^8qFaY`r_f${^lPX!Ix#CsNuW&<(L zo`seZk}W$Wd}qt=aD8I(N`-si4N;(_ z#kq{kGsi)6OOIJanK11@bd zW<}|mx5E2)M@UVbl(i4}M}$~~8VtP+XNiUG__AfGkR3% z*B;t`pz#SCS&hCamzi}m->*~M7jl`fnlv7@wlsY6RTL|zq`}rB(;kx`{jB~h zz})U(=Pssxe6KnT5W0`YAKv0TxECKP?p!wA%*I#bQFo-nS#RUKXi?EHKPVT*y5M_6 zUGc3cttn?_PP9Lby4EqTVZ2MeAKVW%+}@bkP_KA@IIKA<6W8aYfnAA3GrZ;7E zk@`|@SaulyHayX_`r?EkeB1rzJvsP#8dxU$-TU2nfakHGyLT_o3Am-#O|Fb1n22_!4`BHmXovMXYghiveEQ>^$O9D74M*4S zISAbc#xo0w_v>;`ck3a;eN#>1HOI2F$(K~7j%m*8tBPmb_8fsH>tc4Ugg`qo&zAdd}ZWQrpfba8wY{npZe`-U&LWjM>$5%aA7#?v$IjK8AJzs&Ij zXXIUxYYMnY>f8pWd0*zffOa`@>*BC1D3q<0SlSCRRZ`*NCerD}kR7PdQ5L;@*}st9 zcBot*x%`f$R|by=7wmj>EdC5yh|t2+Zua&>0~cC^@W51o3kcdFdWKO1Ly-sLI7D|X`_@pG>*fcpu<}gO+N1KF zPuQaZt}imKY4P>94ULrBM);byv}Pf zI>hpBU^ub8bR_kNz|&IaHMU-xwL3?`;5k=&GK!eS`3KAx3AOF9$p&Xn-E*|H0`nh( z-Bw_|1%&n`LvuNNdj*~x*w#6rSX^MPcS_#G@C|=V~U4kRZaUc z*|@(Scy<_N$QYVW4sd>0S7t6?a<60Hgo=d>| zOjtSt$JV4viv*%74N@6036PK$F#dyzwx~=~1YX*F67k_@7Sf8`e#tl!5r0d2HZ{3` zN^g!XtRQGRwixjmvV^n>cVV*9WQ`Cn%`ZKo+|-ztwuof5T-EVM#4+>yGGb18W>G(+ z=>FA3vUjFYcL-FVf*)Y}@@Zi|b(E6Uu?D3r?oy%*p{l$!MZ3pn~;`|EBepG7S*s?AJ( z!F`}BM9i;%NsBnA>ZL#%Q~%rp$3vJ|0TYR6&O$w~EOGZf#0Le!PV+$5)f&KblhyFD&M?Yzu`N-j_gu*4Fza-L`@9WHaso`MXPv7p>Sp+gbF7Vo zlDST^(jF&|60#26XTgUKUTJK?TQ_qQY>xx`BskK-#??7EM^l>+Drbsf=S{RG=hg73 zFBUrI0yox%OFr8WpKZ>YSC4~Rm9({Z%H1rdp@N364a0BDzV56?IQN8kk06-&?Hqqi zjh*QNb^;}=_P(Z760h7X0u|7mRE3zkP34=$*^q>LI1SIbGH`uoUZ>NI;tKbpZ3G&c z0c$|g2Ta?2f`+cZCXmhn4Ns-C+GZGZ1|7J<+a=RDap{^C+IsgE9U3gvqxdlgNk*D4-ymJSrwqftBO zYgzsnu3E@~>KA*H`-$0Z}|UT;)zTuO!0Rl+eT%-%T_ zc1A7rrJ2b)Nk*j{CX3HVlo?x(=GWH@Y7oRM7Zmo^+|mcf9@Aax#&Uj(_>{oSCNwg? zt9{ia^Um}8o%!&!aHC6Y;1OZuL!6oJ-TFflS)D1_b>E)5b%-rGBr?s+q`0-t+I-rk zuExw{C%LuwvP9=`mgMo6cPV{zpRK2uUhY~&rSR?uOgBG4+F|5!D{N2h$DNS}sLYMZP35t(KhhF?i!jJS) znP>w&&I9HkukIUIdjV8%ZH(zQ{6y604rdOG+)ng*-|4$1 zA$iuV$6MCNWo0WLS zRy7V4gU4P{hWfrxqBNSB4}(^BbAp_#UG4F1?8+f|A>kP1j3i?)#Lu)sDc0v9cNDYB zC@$*|QMaS?u^O>XY|8LGptQ3{aD~syE0gSf$Vs3J^yR}hvNP1stTRmv+Bp7v6*6?Ux<9MJQl{U!f zC}?_AweScxPqa$+vSYAT%Vt)m!|1)Q`d&+q7wqj0HNWvKz!QdfMZ$a?5)C7^y|%4= zmr7Qm!uw_Vdlvt>ud{Gv%>V^VQ=IVZlGmo@Rn3PItN?Z_30HChADuJs2Y{dkj;KoY zG++AgAyuUhW-8i{77%b3a?2j{-r%{c-zN@=a)nT@>^#r&uhHqJVC9zPtrOK+tPZ1B z5w9e!&0kVsY+mJOJq+A}P)w+$-xvnGkbcvnhfDi{6E00f(u@GCLFc5}uUmJ;StnK@ zsJE*(MPS1}uGxw_-z=;CD0c~Qt&z7cd247Bo0#%qiC%&C+s3C_Nj^9z!@_@^~xS&_#1mp0`MaUTlt1jOn-1glM27x0ki{T-V@B} z=^*U*1U{%GwEcMHsnkW}+%Q=OxS5h!zS+^D^4ia_^*S$!Zn@y!x?!><8$Y6e(NI1g z;rDfg)rRG+7sX2?TasIdDm7`0-WJmzSn@x(#hw(~&-Wsn6zuGES4auFLSGRRxeHdS zb{=ZHJbUS0maK|Jp|ZaZ_G^h6fn}O1#S8ORuo5?`%dFD+Fr$Ln^xEup$BHWLaC+qP zQKK%{Z{$*WLxvIH8aA5t+~>`3l@~x$8BLb4TU#DSO}S1}R97;Z2)qt(UpRJ=8zIWA zG2rz?>8Z}e;yE-wjyKn#y@#KN<<;AV$4z7Y$j-K|Akdk%`g}`dTZDo9itEY&M*-_S z7M}C2|9E7F991&l<3t|Tdny|DP<4yi$?12VC3&rGrv8N{*0ME41-VMvXw&UHiJ08_ z^x9bbZIK*kHhkgTtS3fL6rT1+-$&p1RZR0B;Z-&$;{rzbirEQ_$TPql9HpOLN0~n7 z9Vumy%nBzILj+TyE$%7V&K)J{3D2(#4P$VlDJ+G6@WRpTy{i11e+msr%7BRP4q68; zd7<1O8)i9Y-F>CQ534zSUib~s&##m=ybol#1zp&FCEeh_Ve=m}z}5c5mA{E@A?U9; zk{QqaD^xC${VL9CrZ`A4;Xuz9H@)zzN}q25A$)cEghK=_`ZXn`|8m><34+Kc`gse0 z4)b*cPvpo;VrxQSBYl?J^%+XE-cFP+oEqDbwG8cU0wnY&Amy2K)Vwn&i5>mza_}B6 z6QFQLhTt}Tt@sdiOet$yrZz=gxeirS?POMfGT)F&_Xr04_ClX)V)uhcZCxU_RZUvdp3vmpeI(}k7KhC*$iw*fwsxeP@PXiuUn9nM zF0YEZQ2{;^cy3{3MczkS{YZUX^8C?o8x4vZp+O$)3y%^PIbEUhg)eTsIScDN(3^vp zFPK%|at5h3m_lDBPF%`tO}MHR?ErN-z-x8BoC-YhQFqu zPl7a3^%w9WJpAkxzOJ(B-{D30_(2pn9`f&5EX2a+Mzb%g{(ZP4Gklv5Y-KYcoaTMC ztZ9-^!`$ff(f+!SKwGG=u2`qWBr54(Bv^RWCeEIyvKjd(@OH)9E#%e*1&Z*|18Fkd zKEv?Q^~B?4Tlm5iF8-=XT+C0|_=_>L*vvM6e4Z0GQg;cewsg8nmfKGHaz=C*D^0_3 z2Q8?O6>?`Lm?fUpAfo@>U3fE+mB=2H=W-OrGgxGhXMknC3OIszp!Y1n?)HO?Lcri` zD=vMfiXCsX9=!5Q#;+kkF%F)5xc zHXB$*+P%An%^7ph!SHK?bG?ALm)b!|ORr-T2T5f(`m9vY!E*_h7@DTOFFMA3Y+M%j zy|z5s2BecO>YqO7&%?}Cx^l2Q$**%5{~5^`{O!YSB8q>_rf7WzX4(YK4;co=E1L`c zg(0lJqF)-?DrG&@>dE>3@prDGp5r;o{!1&z- zjZyK@RL$Wd5ihHvBgE~+n#nyrZh(|oGj4v!qnU!vmM+2u^uxxGmuL>*FSbypi+w(2 zmOTvK>o#3{#O*~rRRCk%z%MJFU?46lTA9A)BzDpFk(ohp3DQ}4!xfsNFF=mb+xBc_ zKJEu^%zs`=TozTDs(SyW+j0 zt=u2ywb|ctYODvWEEAbkOFv*%xctU&JMgz$dZ(tX%xJZe{7#&M8zDw9b0aWFg)f}> z!f9B2i1UX!Ij$ly+nfAg-7g4;l;1PkOS``sdS$Yo`#+SUSfT#$D1v-XUAZiTxvcxZ zykq7mMPtQp_%s>U#%f1acKtM3Xgjn+=IKncBf7&s3i@kav;3Jil2|bE%HRF~d^1tDYVee| zUPJ_`)wg#Q#qDRQ(S`Q(l}6ReOY-WrvlBQ_h;=i9lv|ROdq{!6ZA?2Q9L4lBl}U11 z4)B23Ls#l!Fu#IoMp=oyvo0+V^Gd2lY=j#v?F@JT*R(|NWfs}^5}d4-bkLG?&?4)i zN8iLQoX|-CGL6-_Ws2EM=sz7C;pITgottZ- zF3+xN+vs~h8;Nd+q;jC#78Zq7AZe{7B%GXzyt(ETEIXMU6v=C(jAtks6=u$I6g_xr zM&LPc>(#R_w%t%<-8{c@cjcMdu)Im`CN`29&Aok?te(tzmM+^WnXC7FoZVTA!KgIu zI-&3Gz3~Gb*CTF{Pv}sq(sB0*eOoF2ou%dVM*HZhj}hzfCpV<@!0k-L^kAYDsivvAmr4N%1`Yjj_uKssXU31f z%14n|pY#JUh06?v+tL3Z->IqZ?}K{kqD9xM)TfsNoKxCzIbe`FsNK4u&UCe9XCfcX z7MpSz3+0sRlC4L*=DmnKoIH}~`B5+qu#APD3EkH6H$0_fq?L4L8zVh>uoFn_@>*Xt zPJ|IHA-bGsqVPFbi_V!NW_y@O>-KV-89}z{?B-j@NR(pp{@IPGeb83QZV1oDxAF^1 zw$tPiL}Ydquta2bUgL8n_Z0_#A?uzclLMU|@3fjXJqsp7Iz0=lGd2B{^M+w;>!5Vr z0jVS0^eX&#`&)vCN8Hh}QkD2c)cGrF<3}$jjA~o(*tq+Tt&1YrB1xh*PQHJ^GoVYR zxGJ3_jX5m(i#dEK-xnxqXxWz;zb?WTLQ2_qn_F!H1FQzjP|_9Q7cCOm7%%2tWn)Yg zU^NwKyl~}~YIr)?A&nL~Gg7$AG!QZepCc;MQH|lj&KXQIVWh(S?}#20H?s>zz%(BA zzYU|%^xHGsw3pye+%)qtoz`!M{wKtlw=WtMvTt8B9%raCu=cffdSK7#i3;t#A+Xs0 z0WPBqbngwguz&OFZ*%v>59y7B?GC(G7S%cb=8c@b@>2p`?3ML>u;ay0vwKf*I%kpD z&>l(EO^E^73}FlAe7ntO-6Auqy;hV6aqGhx?IOLouad_M;G2GBx!Ka@^pRS}`oe?7SslBaF=mFj=NOksyAL2Y3OFf1|e zzrZC}RA1pK@t-Y>OVK6lT6%}~WZmcbyvz0btKcj<80@PFj$b#K1Q0L-kJ}9F1U!ja zkRQ!UT=9GDcp^BZcnSYZ05QNgN~b;h?Frf#?U+)L^3HV4U~W{ssJvm!VAl&qka|9* z!S674*B73x!s&ZXO1(QeJuJF_5Hwa_-ZK6xb+*)6Fv;;(^|0SfAcim zihkb1Xqft6poyX9$0?=o3Y|Pe0&cGj5#?V4*;k~soO8l3l|6O5UruO5Z0~ICB%Zw{i^E z$}g13wrinx5k}XqWif&n>gBW{Atup{z?(s|sTS z9xSU5KTepUo7*qtYLDlV6X~?Z;aHWl#@XC5X66Xdnn$gg2R|^`5ax!^6fDta?7@3k zD-<&*Kb`w+Q-v%)YB#+P7(7w)DUh{rYz9q!Cj9g&{CgnEIH$l^dhlFHyzU`sE5o?h z3UJ1WG)3NMe3vRImk0PNS%g~dD4yQ+eO%LzGS2Xy8#bRDe0dtQl0B(KBe!Fle)KQ? z+(7)~B%ORrN3O&W6b2t)hv|fgFTbeeY#UmybvzXw;1MAlD`SLFH>+ogi9cTG1Hl37 zEebRkDfnEtPA!vADf#3zN-W|m9(hW6N?w_{fHD%N?kAnsvwfN#Lxa0MD&zb$X_$!e z=SxI2MPeWm&93MtC&h#+I$3K#3r<@)4NrMXJR)jd;^|wKcvTv8z=|(?3 zRb2eslcn37vq%7wY<6o-ro5BD#eeF1c0Q;@CTSUO5hgWAnc6sAy3vVJ$|D`T@ItKx zV#Jf{_rv}(iYqqS%ppj3h!3c?uV;0SaW3twWj+TYshjJU9VWK%HL5mu@oil8PVR%( z-)ix{h?a^k@B2W6hF;zV^KZe7R$d9ijAo%qsBa>Em>dd&hE`qy2 zL@2){5h#6>baq&F`;46Fwjy3QlsQy-@bcx$Ow4C~{M1Z*{5<^C&tLY(tXG7=^@GC> zl`yO8z9YXxj3wU%bFUZ4_9c@4z#a)9Jqz8jFt4^pzM4&2%*!~A)6mdRnZ0iHZtV-X z`i8@7(lt<1{J!D?VewZ)W|Q`J8s%J?ENDoT`qcjn%Uv_(^D2UyS=4*0**R6-FNp9O z9A6OONjS!GRQo=+zg8$zUDKrh2_Hila{lT&@^ew_UEeoYlJ3-hNk!jkm}IJ6`)JVb z!7EXP-00K%TD#R0PQB_vehwH@R^4G_KL1MKK7cA}l9XO~kX3Xc_;`bQzpVJScO4-$ zgiZj(yUZL*(t~;q9!wc>`KlT@TNL}y_rEJmJfJ?ky3D9^x=k(|%8-*+0m#oquvdMb zVM#hsXT#%AgzUVMLgDa9P3ie0iNRBUlr zttj=*D{3l#H0FjpR1>>~+CqH7FCS3}Et^o1HRO*_$f&5Zk1xp-GVv}3^*MoL;ogJz zcfU6(X-$psFGF5o`%=S+BltZ-zuT-_3QX)Ky{WI5+84%bi_- zSBK#jDK~0`_8%+sH2CKm(H_I}3?rKCu|Prb(__iKWha&kdLvV2e#coZSEZxu1*O?3 zI_pf0CjBu2b4I-@2v<}sVK#h%MF6w!jea(K4KPG&&x;$~ZLUexMpewb+24a&L$CND z5A;iVaYjoUiTr|Fa#6rIqn1X}D6NdmDYJ%)*~c~JRnvwZjZ*t~%hXL$v%kWUk$RnV z)LIQT<9M{BO@VlO?8$EyRSTU5IXVTZdYeu0Md0kRfrqFAJ(gSHN*}#hpO6DFnM?L> z_a6NZQ3uC;4Z1Q>#gl?8npQ?H2Fhdo;yh?enIWtAQL7q5c-J}^dXaE5% z8|r~gEp9R;T0%^jIdK0LH%ysj{|8(^npJu)3%yc>p2`U^WmfgrgIZ8`rh6gsmrzE5 z@oBSFk*iTx4_LM?DX2%ZO7AJ)$c2OwkUOmg>qRi820OVVC5{;IeQ$S3i$L+>APRM- z=yS^xqEL78|L5?h(qA?If`x6EMTg7BS2Bp_DzEeq&n-v23g!m~TAmKbucPuCL&tr0 ze__HBc#(~DmOI@yuId!*^Y*LE8TjM5?Su800Lb30+F-r3M+Yn&xya>{1teHdkhIOd;CZZ0oU{KkjYq@wkHsmb84FV-TZppM6|v4UG>w zD%eT?XDHzYE8+&{9|(RE&8yu0c$qqot~~3=JpfJie zLU)eHisGE$4EiskAcc|qQz6l(Lg!8ql)Nq>rJQ+|L%XCBI!5_VYf+t?JWF)v$W_0% zybWT}sayfc2-+H>W_>g(3hLWWYl}(8>u z=h$@lpW|=ch2APK?C!o0P5Yh~ckcB6C89gWX329+ybTiSS7adBdm)i#ocNGz$V2cG zS+OKimvP!hq9$d=7DDYEyu6O}^7{4}o_S^{;meVTwIN(t-@T#yk%)hxSf^~4tFD~? zl>+jWDM1=66<4xcH(<`ROV|8T?nv$^vhw?z zFmw8QgPTrsrJMEhUL|3jq|%5fxktwPJoD!Byj_Kyi}8#fV&Qymj%AM{_+-y`qWRt( zzdH`&llMt_;F^2UEZi(S$J5N;%s|+r!Cs;o8yS@Z;Lk;_zoHxLov? z0%j)}nr@6;4T7P6#cCM`dvQ&>aZOUG(&q-+gH)hI0=pvlK)%unb+6z2@xWT1|lvH4(2QaKNx-r#8<}%cFUb>N{M^T)hi{OoR zR8`QX`){uS-2g7Hk=fhOAkPmXT08WNh(N^b?WIt%JN`RY5Ps9D&x$RC3q;;zgav{t z>q_5r^HWG(iKEwzY_5oY%+VEtkb(%yI<-W+1){o?1W0>V)X9-4_17dHm+1sh7}oJq z!^Z7|Mm|3XWwpw)AYyUY*DlLFDvUkioj)Sni$TPXkAXXDp4y+$>~Y+o<Oq zz4n{{UxhVX?XbbI-pzVAwKXP|&b*UP+L5FBf`IVR)iWrZX>&aL8kHSJX*a4Tie8V} ziHawNUN8TNoOUp5i%OXrIep9eWm7#R|B43`VADlz!{w@W`0-(+N8Z%I*=yY9;$5HA zx-G=vAWGEpn85Ukd0%juce`oJb2_ywO5cghw)m=pm_2os$A;V>sug){QlOEXC#t>%PVy_~TEzFALpc)PBty+$FXt5|X#)t3{lM-@4b zcgW9A&dRU%dX|j$-Y;}R5dd$Abl}b^OaQeve|`XH0>uYJ7C}f@dd(n$o|JlRET!!= z>b~r&gHY6#u(H*CC{j!4KF4QGb1yQBtRIA$zu@q%h<}oq(D_prTjb0SoF*Q_1nGmH zAv&ZL?`d8B5=)TpWPO)cmQRkMtdw2H1CWhI6V3)*9?=FGzK)(5gcL*WMD93OkCEm~ zaN&OX)J(TKL}FUlS|{j6Gw*O<4QHC4Vr$=Z#SVlgv-UEv0AkOfypFe!9zrJof6N{X z5!}@F)D?_k?+VV|Ro4p_TYKoHeEdHDeoqA>$bR|$cg{EUE!Pk=7TpTB?mmHT-a8mM38iFI=Hmt=nDY7w#y;Xz3vs7X!~a6Zu1daf$q)pE{wY%T1Gx zHt?dv%SuseF}>a!5AUKkFPKm|-3}fW>yfY40At&nUXbuuD`RT5Z#hC$?!*smGOpFVC zNC6cUUn1?FQs}f8@l)mu8ac&X0q+hf=V*(Co-b@NHr`1lWbBk)uw-Yf%ip1ye-Ds$ z-(ktRy^8|g6#}@xn~WVFX_M0}vIF0Cz2OLS=7_{6F)e>)U5w_;RRiGr?4I<2U3t&s zU9)UK0^RbJXIU@)|_@@H7>pUFy1anGH;IB?*&@VwH~jLLIi z4^~FLS!&;OyNSFTOxd=OsUTBAIi;SnaK@Tzw-kts6(fi+FD)sK{ zhXd0$<$~0An;+j@{czIz1q;>hrcxHF-<|Xt1#4d-@5axdMt8155JYgaB;^0zfhq5U z;J0M5f`*uZf{(xT-70SgSs>Slf5kS|zc#H;Kl47A)&!dfcOQM5WBuZ=TKT(xj7`wgssg6!GwB z;1WjO_5%!?XC6qTKXQ65OlP#w)laM^G)$Ip-wTcBRhdBX&v#^_M*v!7QX&JT7YhUz zVGm8F)yC5AP5*u@pOW8j-eL|74S|M+24!tW&bMT3M}9@#pSA3mZ8|Buw}LmdYkwY3 z%T#8>`?J(PvTugn9HcdQw&tr1l&X0Yjn0Jb>3m{tUEHi|y1TTwAQ91Mj^A78u&||V zX&QCBGQ+&MNbVT&9i!Y1CP`o#iM9SEkKwb%T$ETM% zOD*%>DXkEf)@^L^u+F_I&mL>9rjq08-L=b5b-y?}F8TI?%!K(oe_fl5z#l6{b0V_0 zsqWtzU${s5DAmM$;sHb5*Y}l-PKn}LLy{rlkai_z03CoXEKg0o?0hKnLN#qSPBNwR zy6ggJsbzHj3U~{!UEV&t1&Q+e|2ka`9T;s}dAO)(&p7`eM{cjErYFW5e^=%gz-adi z;pIiQ?7c@b@MUex?L^X^29h4qo(5e+(zp70iLPK+6C8>V{uOHS2>z*FoUk39vQ4K{ zUv7KCc*Ds&+V?qC5bsof$H8te@=3ecnV`Yw)dOWwH0{_Muna^D!kOQ`W0>FhCSA&c zvk^Z*bl@G(g6l&ZcoK>}Ve*|V=hp?=tu^E-d?$Y=`9|b30T}hfJH|b-<%1U{!V>%1 z{c}yG>>p2Xrq$%ncBXYE1FJ*>Ra)lnhbT7db<>W6ZF~^ssBhkSnGR@*?E+94&ZM{EK5*`}|gFhq5t2vi&>u(ZDwAjw<#m>M*0k2KPyp&oX1lT>xTc-16| zyuAhw&FmeKAlhDs(+&wA>nz^(#CiN|X~(L%@*c|7;#an233V6L+#mS7T&jjlF6&7& zy^iFGwR{DQOze~cT~tA>ylX1L=UWS0SxTZfS*$^*d#x3&_kuS+gKlg=xw`FqJm>tWsrZGYEKo>hQ&MIM_1u}-}?MOqBrpTCfqL00X!o&ixF8;i7vbRuoc zk|%A=cLi#K6&Wy`e=5>OdhCsgsh05MOOv)1UdiW=pgcnpw!tJ7G4ao{`d_S_byyqg z*0+&T9Ev-%P$j}?~uRu|GQOe(4J z@1U3fYpo9Z_BlrstU_=Q{;?y#yH-dfJ2`xq5&1X(io$Yn{$imo3d+%3eR%)N5$_4t zwCm6?ad!vZ@(XWmqo17$C#-% z!k5j5{Jh3kyvt_8wG*q=%dW+jSk*Mc4^eYpdhU{;ohFxuUBWd)mqwJ$ZN-kFlr8ps zyD{I$pZ^i`e9-;TRi}wPp}*?!y6l&O=OJB2GSJfzCO6O{`S9ip{L{1PQteYdQ7h0l za`F@^)VFks+*`UJ0noiGQRU-D{vExQOUpazoR)1Jz_$b0{&aE{Z5_#Px7z$! zCgrda0|&GAyEDAj93WwY$f7uCB&dX1^kU(Ki#O-?J4k(9kmw3PZ>qa?ST9 zW2?x8K*62-EVFX&4{2CEA&5fV0uOz%QNSfl@zi#LX(!k)ZHs{F)k7Afr6%B`Lia6)oEQXRCb`I2@ zPpD-kg!bLbVN(?j*HVgBh;Iq3|B(8Xij(*8Ay|HgSmmtpf!kd3UaI@L@AF(L7S?R3*?|5i- zRcM_$4!3Yx`8kK8Lyn2{=mq*>3D9QT8Wof6K{07OV314fU8PwpcA=@XM#=4K) z+9al-GtO-`m=1|<+?p)nL$aVeoz+*m5J;!V&G@c8%LXxTYcr1b`!q5n%1l4WjBlG}x3JX zv2g&OS|lD(QXf6x$BsIE!jFvu4Qdg6bpLFV%IPP(o*6)aTu12q&5@_S;0`QrcFv!J zt(Pr%!_v-P&o+q={x$mNaer8>KoL^t$7I%?D;TyPlJAFq<5p2*7l@V`Vw>4(DwuOB zdM7lt>g02P{lPm&Wcb$m$hkeaYhb${IAI zE`A|Iq|<)pX=42SMVbz0z2p0FhENM!c3e3l3OO37KoX`llPjhrR{s0u8X%hhuJp16 zx!XJ9y~2|7tx=dp^M*cmBTjkU>oyG zpd2_e%tGcAj^CaDk|%*pP91GTA_lvU+m82bUton`r2$MT%t1CcO37PSe|+Jymw$&T zD|^=sRnl{gW?aX5y4c5wP<;?attMZf?s#|PLlv;(*lACniiVPV(Vus9|J=^p`oNzr z&n?wG?&h#)Sf`)tFxM@Ogr%TEohzao7ZdL~L&>oZcwF0XGcm!|;fB9l6LcN`oB=+4~n`K`K`W|nNSep=dG zY5O;VEBw>nXEEt!dmH9+Q{y~irL5V1W=QX`nID2C9r-9=(cf=Z-_%ZuuBszAkrztEDx+_oWWW<)0?DGRaq&j9qHF*oRgW zBOL8CM2PJVkJY>RLjf;@W*rq8StoCdLEsB|3$WvDl#`^h+NM1Lm$XajyI0QmT+)@C z^69(?jmjxW>La8R2VB0BC>5{Mxe*GLx5GXTp?r9q&WVtMvlaGa*Y`tL{1EXN2@(ir zE0i6iJVJy7V$WjjI_r#YA|4|$xuzZ=@@+ynllnxJ<06&Lh@gkF6;cvQVfl`VjC>RY zDO_oPPouDm>+MCC*ZF~$kx|gds5bbD`2rdROGQ6Q4lXabj!&tLj7JZK0bMXSB6@g9 zcH|K6PGl5@D|7GDY!v8BCgrAfV_+NKu@}=G7b)(Q`aaKF4Yhnif(j`YN)^rNmu7<| z8WRjszFjC|K~vp@ugAJq09)*19s?DWoOjB%4M2zMA&;qeA=|$MqheFvKj^)Mq3`hf zt^QBE9nL-jzCHYYrvLWvyUu5+I@+qX)gyoqykf^BK#OGSs~9$t2mOeIFD2TUQhJbh zUG?T3EJXy%vqOwT_rO!DaOA&Z_Z4}s?xbVF1_FCO{Zx%VZy~$iEu z-~NQhYwg0cuf?V~bR%MT%yQzay=T!jXxB1XX{IkZ2E^=pXEm!uHgj^oG8FGb;v^6g)GQ^V|Nu(L}*nmLJ$e+d-jQky0Oe zs0B<%-yPkLG?~o0$=yWE2~Kr8hFttgcN|w;lP6fYB=HHn6yOda736je(T>5E6L})v zrIjq@*x=H@tp^luc%B$Smt|Ktad0sTpMPIjwqwv>YWwg`xanF8e7m@l2FB7{Ej9Sj zXQJ&c#|XAuXD<}W`qEu-690~J?6*kc7&^(E?`+%^B|^3TcC5K~A9gdcC+m?Kbw+++ zqL#%Zk$0rQcLS5lKHXj7Lv*(y%yY^9ayq)rhiJaS#MJ3|ERr04xk^65If}ev{eB~p z;GC4-yDnz0d@DA^Dn(t7Mrpf1%silr@;vYHX}E?FRe^S}Y15+2g$g98(*?@Yq~mQT z0;xf7F4Umaa!yHhJoWKN3*TutWvAfGGU@s-5FbG@#%>~DCf6Od9sgUD`!0hZk82UV zv$J$H`J{S9ES{6V-Az6!l%~Y0U2ArV;qXVd17b+i<>?2v2|av8ix zr`xD}Fo*9pd>rydF@RiV{SAY(3V(E8lv>0$r{`a2a(_uboR8swPxN(WVapxS3ia6% zcQzHn=J~hMRIVYcLQj<*tm0v3@?F{0y>Hi0fQy20&-cWs9280ZkYk0J$DK2o zuI=}4#*tDbrH;m=w+8{5x~iU5dEHw|eUc1qf)y38YEQVegDuERrjf%x4<*mIfFaKc zCGS~IU^?l?zcaut0VpI?rohN!qi3srVZ}uPIUK@0XHUo4cjSsR{52a%J9fTMJ~&Fc#;bbD=vRPIcGCRe!FYz)bTyAjDC>TluBYQjiO#FXget$hRKIpvS;Ks~ z7Q?!?YIK)wZA*{zqy)!ltL={=sjd6!%gSjRE3+) zc`Lcm8LFslE{dwEkC|sFo^$n=Fft&l1lF$*V#*BMQL}#xQO*p@QSQ={?m$wT7t{Kt zrA@;-%VLXf!VoO^l0ZsFNsO%sk02wnR4<~->E*b}(4D$GHd`hT<+2uXId7gyrLL#9 zpa&Xy01ZFb<|%ZRL;UHPoe?raI~{mkT)on6!6rMEg_VLN3WfuV8-JVjhdb8$v+*BQF30$eOL)j4j!U6X1>dRm z=kXY@|BNDKwQZeQo_4P%g+8@_j1SB%V732;?qLBXgR83jN-zvXh`x3#-YZB$Pu$p z(7B9LlR@aYx)bs;-(%@_8$z0|n6T&wk>_4~YTt)^ssLs1O;C1vAnv{>Z}J-QO9sK~ zTYcwS?5dw*4OI=6O4B_1M^IR429YmyTnc1nIp!Eq0HY-24;_-Ry0?`ed}#>W5vT4> zMGC;wAqJ~cfZ9=R?`l-*3kA>$mo#%zh&GU*vL=fVJX{%U{{19)0kB~rFHZ?37dfxV zqMRCEP4p0=lmYG>1G*T}Y9h=j_H(<`X0d>!Nzw$gRA20V5USHP0~#;9 zm<0XLz)D7*IX9PvNiz>q3`r-w2?NM6t65fqS>EUlQY07Z{B>klS6C@HW=3S${=x5)MG}&! zXoivQ$?j$^lXQ|$RHR~&A*dBV50SW{?7LKI3OW(~eE1cJz3%y2s#3ao|Jk639+LmZ zFm<>(U}_+a&b&y^ zxPYrf&S_NN&L3QjRT!sCzksW>KJhNNXz3!GhQ3mRRQT?YDS;4o^ed5eu$0_<>JY~% z(huNVkaGmZ7U2IPb#LlDCU}p$`x(T(yK2}Njrf^d?K64$8XPS$J%0|pmVNpfbiNIa z06B+GtkZS}Q^`Iu!g}JN8D{x-AdZuzFT$Na&fyhn^u4h=(a{uAwKx?(auY92`HF7S zec|gswCko?Y$w+U$)5ZSnw*W>W9@Z0)|!YWm)PIN(5`>YCsf{b&oh&e@@iu4@u^9UjaY{t<2?icgHA$m{CczC(jTH|C7t*XFKwi@az_vNlK$IQB1l`+pfi+^TH zUIh6enY>fuiyQb-9NddDp(gV6oIpJz@N%|Gzx}v^vf5RJT95w%`Uaa&4=^mq_k-|) zv_pTe_r`1Rgbey_eD3%mf9b}3{i*&;GVt;F6G+C4qf}!|FvM~nuh-FSl|mc*rbsZP zF*??BF5=;^;5;GLrF-*(jiOOyB~5oz@P+J!{A~E;_J}@km1ZD09q}<-=Y>+(E)x(jzaoZQFK38I^Q)N3`b#FuVRl~r**ig9UJKH;Y!5%PKG{o2;$DRi z4opwk(KQ_=1+b>N7jw+%bM!AhWRYAn*_$<1&727geXB0jI-4~c+2wOyB#n81a4;^` zC2aE~f3d?WAF(y9s!d%{Eq#r`STV`=E}-gJzlNYoMkpSm2jf-KxUn0o9?@P3Wm=Q? zUZ%ckaqzPp%=5r$gTiy9RTd#|90;izcV{E0=Re#rr9|7aDi|PyqU&)r`snNu8KR4sw}= zy4x1K@K+)mfJQ%+2iD|eTJ`OM;A^`B3aZt5VLL09iWicg3F@dK58Tv`xI&l^XOZK& zdOQXBAzP~h2RqeWid)zB!U=O$ey9Gcr%5Y)&HJQ3R;3I$#&R!Yn50X?81Dv~br!RR zL}zd7*KHjl9IQetWe#>GJ?`!M>jY~mn^kpWg9IxpZ5-mNs+^s)4}TCsz7(EE*f2vH zX>^&bNbUmtW`q?|f2Y~EHi&&^&A+IPHp;YjKf`KF&@^;9m# z*<hz1v!75X;(Ce!D zGvKCz;IZC^l~Ys2rwHQIPoMu2D!LNs`4JsBc6py)j$;(g^pb#P>u3{Fm?bNpS)nS+ ze$o~n$IXa+Qt6*VE6My{CFno_)TSVE5qo<*QXgx3CNdTC7lI>GQGUX*H_ zz#*vkhB2_h_`jmde89qm^p;Q|h|>^*t1ZESFd6gMGX;IK^~wQW7e(FyCoCY;h+G#T zj;_4L|LpY}`;PMB4$dO~3a|LDwj34Ve2(QhNs5(FwcU)j`QXEmlX5x3^%!fkdr0-Q z1zu%kLzx9U8F@}1zS3YWX-)taeR`ehxOpB{*$AqD1kn~6KsBIT9bhzp=poL!)wrE% zJ9S;2{^Q!1x{z{o2j@Q`iW`f?*IF*&9oXW$ClNt)*{r5hvH~dCFa7@wZqJsIZ|gML zA|n0F8L;jDBUC_!jMJpoZ9FB+Hpq8NL=Joox@U5d!;-)8?RASX>~DI_0b|P;#E3v5 zV^)>pOMbE;Se;%zsx53|R_Y7te}ji;#wCr1s1t+(=a}fL4+@fGhk@j+R7S)e!H<0S zM$!&vn;(hr5SFDmBw)RdJ<-uVLX%!gH9FMQmkd(hhHE$RId8kB6}^jo6(Rb-9@nFO3Z@nJ<|g<8$)n zI2|49KaK7_d=nte&Td2E&OqNzW>OVFZ0LTD<3E_%fo!0q)4PdV{Z z$ne~RWJ0E>jYsU@{6p4PF@$3%7kkq55SONH#=w@wkSXGq^$e=bz6 z`ejfIV`q_oxmO*N@3_&%>Dh*E1@MNWJ|GJ#XS{fOCK;ScKZ-}|ks+!0c`F$AFJf|O z7m4-XVZNjb^Y5@b*eFokPGC!0zc^4?51$4WoTwQCm`%F7dJ{*U0p|#8j|=kO&*v%} zV1vS5E-9h&jU!-HQo3nVJt#7vz5E)9`;Qn>Tb1MfE6!te8w1J0)z$KJecy>o(W|b|eE zQ69CkQ)4&I@uLT4QAUV)Zc@$f$p77iHl)P*3%8W$JyZSx8ISgXz z@OfxT>OOf0Y6R(daOy}o4AZ)-s+I9|7O4H^OIn&}{XN}eP!d5z0A{`Zp1GUf0iBp5 zdJ-2tkia`gn>-lEW7=sTuOF$G_*6zM zEin=F9&`9Brl)C0Yvn#b!pLESKSgMo(z8DF9`jcjLLx!1v5zhcx-E6s6caU{&kt4C z7;Qo)G!^hkZl6m1pj#_9ME^`2*^l6)CB}j-kmryxn~3{9V5MMi>yDY~YK4Db3Jly; zFKJ8|?%{1-sGRq#UMEvE?O^0sA&A+nTD7iP-5usxSWOcB*h!-?XHnUtp(C9;Nogx8 z-dHuNZ^O%={yK9B8T=X?fAxiO&fzR!c#G3}VXHh^SBZ}3b1%|+r&kI8WZt)mShO>8 zd*{3m_f)qiwt7U4B;9_gS6iDr``?%+*hjKS`!#~M(b+o1d9q%;p&+%|m9GA^6b|?7U>h~s69Pjk`w*2*|)3v`d)?=tvdHUHS|Q{R`VJW65ojAjHx z=g)Ymx#aE8`7iWF46AOvzr-zSOj%Gg-P7CGlbT!{T-5tec3F8zptpy(hbXCG9$c(TffgxtYcrGuX3_?w#U<{zpS|oZaP3bK+H(H(YSn7+F0MB_w;cMxCZ>m z_rT=9#2DEvxLy*YPd}kiWiLJ@K84=3Pd};g-5yh(fL`i&@yk)rUD}u|AKi<*XGpkK z>4ZPhTLLh~UlDDgosVGFNnrEAVfnYqmlHp3_NQ6WaCnXpgyVn@t8wy%4@)6qkrWB@ zNt&_-aFbLaV zrL<5LU!kT3c9uL!^)-)qAX%7qw>@1^w@8`dVb%AWTE^6TA7adXi?`@sosP7)0bTU< zZjGZ;bRCXaI5oW#ni2jFW~k{U^-SX%SJvqNW-idwdz!j|_w=9F6ZIZQm3c<}H*<|L zVV)+&)6@wpcc6YZ$)%oxNYtuXL)_I$>+n(+YgOLY#^C83QO^ZZ8R&1!fV|#@jn|8w zX#Kgca*y&hN$~vfsO5${1HM87=4s8X5=U%Ia8#7O(#!0cDByOyu3~K)r*Q>-t8DU0 z)ri@b()*vwU4sy#cZr-P6{D?*19fPY=wqrQ6z;Pq;=>Fq$;_8X2Cgi{b{|s_hGGikSwWMBw%;|xa+K1(Pa2g ztm!pah0^3}&!1Jv6yE#dGNI5oI4j0;u40HLsaiFw;$X47GOJTz6@8S8#yz#J(#K$6 zPHW86?c>l0Qk{4sNk|ZRpM`;G1uMra(Y`*kd$FMjd)OXHr=7=G(Ni$?&d_trr`x8* zMBf&B?HG&g)9^Hot1-xY8p(CN@v$?oewoe{yot-fzq3q!?trDJU220$yBdX_?3hEW%seN2}J`gnNj_)*tGPcH5+JMs4 z*2#v&;w!t%QA6I<{-_4Qj%{%0SNbv@i?&-6x6@ zblL@c85V}k5V0i> zDi=CkBK_|7zp}m&0qjrAx0jhhb2YS@x||4PXDQsYI`lkhWU<}0msy18zX#Pbt+0gF z^iv;{eX3`AYnf<@IWA6$UlZvopxWX>=#YY>YyK*l=(+o=NeSL9EaCL*V^dbchL|w7 z=wpRw?B`cvcG!1Da2Q-o^YB}EGHE?}K7j7V4aZ`4ca$59_g9>p23Y1Z0@616sfHW9 zs+y^f*If9!Fyg{{NhDH*($3KhZJZ{R=5CAK`6=mI2pnjK4Uq7(nn{a%tz*hVW;eVB z`+M04vRW}N25HSnE7K0o&8i@6r3+mr7i(de%h64-XO+OGwceY+|5nBAS5Y?$u?+ZN z>1e?7KM)F+_WW1|_urt`^S*>U)sLVAH&t=SXNPatT7Mlgr9B+1 z_ST2l>3`zt9IU#5}9Gtp(SgS?|y6{^vUte(J9BrY2i~b%~q;f$?aN)V+ z13zo)iBXOu)FFA9pP&sF9qs*rk<_+w#x4>m7L_p05XaZ-L-buFoqUvVS zYG0KDUK8l+|J;zO|K9&E2EaW)fj~fgOYN)b|IT2HE??r|4zkw&s#+b>Tf2r*jWfi_ zd~nqZa>=TTD>VhWlB6xjWX+^tgpD4Yt%(SzEej5{boUqg zo7ODdDCe=m8V}NPT!k;L%8&JCJ3u1^4x8-=`k~b>RP2~6i<6ymb?7rgZrN{#RB1PJ zqeBnIvesAvY4;v54%_*5Ifhh(iE>iW)rOJWzPr!U+Shx-YBViw(q6ce5O3z>2)hxa zl_z#HGm$9jb*aPw51P804wH!V8ZtsPK8U{E01nna1S&46id=GX4r98fv04Td!-2PO zGY_=&LwN^KIC&)+%#H#{<|<5c+&Cu80CY&>0wnL>nd+Vz2bS{2$PbC@iW1rE-g#(q z2+u5{U$?k67>LFqYNc8xa-2z-IXOX6vj%Kd%{Su3>98%_{P>N6roO-OMJLHY6e%vk z`w+-7La?(;rhm#@fMn9J_lhn~m^`qRbr}Up!1m5m8m>|zcJ`SkhLWHuiC|fC;A%{* z!RZX-xw#zHaO$H>pV?=g9LcSo;APN=_JtfD`Vf!2mQ@i`EyJ8?V);1M%rP5$kq_fC z!$ne0bRxAUrjmwfu?b{XO98>EfazR4_WG@6Th-giG4UGJdCRsw$^>DKR8-dymtI!f zh%FUSru+ONya-GDiAV%gZY6X}r6uvlJL&rh4SHd$b>hMw{0>GCvC?rxo?w}Sh;MNt zd<Rv#xom@HL}SZz z42L9UIe)A>YwibTHJt+l&3-ysX@BRK-S_N%GW885tg-kDetKx>Rs@n&Edi*y@qfB# z@T1Is_-IDCBrjuIod8(xTHXRE@3CN|JaHQ^YPgOWxXSObV{5ouQJ9E*I~i1;Y5f4~(qRLo`UREhx6^{f~jn3zOQyw%#`hyd0)V%6n2d{myJh))`2rA>6{;Yie>; zmq)f0uo$bwhZPzYlJj9)db}#g+lC*HHfAU=|A_M`EOc6uOXlw-*^o#UTxO{u3vs}{ zg+B{_esv=yt+$qCW}3wTIS%RZvQIRhh;~NtzHk->C|(0Cge6OHWXLmxgn-Y!&)lx zp6g%-+^~<2`4%K|eYvoJ`zgcHEe`bM8Ro-80aR-I%Dl=^Z5$5!T8^ z$k{H$OL6ot7ulytVRS0Dxpb%|QoH2`@-@GdX!aHa|1L?p!&$LSRp}$`-YHXe{pWR# zR0Ay7cN2BkS?J6>Eg2^*)zeH&NrvXIDZ*yk|E6TWis)V32Y!+~fM zfF4ahr~5tAg(w+u^IEL~W%BSTQA+?YsC3Vb{U+Q`R*MQ7Zwaj?8dUT#xWE8&s-@C_ z;zl1kMl%hA_b8ROO>Ht8UPOHb$24MH?ji{mZOji`=s+o}OFO`CPy9MO`$Q2IRqum0 zk5X|DUmpKEoZz88ho7Me!M_m-_jd=qWb#`-A}{dyawN1QOuMMM0z=aPPeXQxXP=p2 z^5?La6BpG;d$>P_Bm{5K7m;AYZyT~YtosxJm?r6_7@yNxNt*;sQcVY9!rY_TivvrG z%rrQ^nejT?VU}u_v+f2Z7;(4DJ#d{%C0d78TSb2!!JNBVGJtMX~SX--- z7kfya?IJCF`NZZEPYjdQk>gQ!#9(Frs9K*n(BBdVwka-UA93lMv;E>Ekr;}8JT_uPKqL(g;QdDoedUNh=nm4>&ic}QjwC+ z%aI}A&ds1xAO9ATsHPmlumdk8pGS;6&J~X}ju@h;F^(AGA*PH?zXLBNoSuV>e_Uke z3rWbxpi@6y`1Z$(*Wmv0PI)MoEmTYTHP|_-1hwK(jQF+U%GnG~0n{4we~%0W^)+;< zCZ+!i6z1iq6I_W$vEu(kWu6C84^as4s|ct3jx!WNTocitP2-4L@(2@%ZrvJBry)k5 zY3PR-c4de5d|ZtA+fQH6kLd$PxLvbpVCCye!&uA_3*e;?UfpK9CMB3tqdt!I^p3j@ z+R{f+m9!wU$4heLe-KXRkX=5^ym#YSGl=1g{xfTp^f>Mq^8VKCklR_W%M0dQW5>51 zq{$7P54^b?Hv4!(ljW5nk7p)n-{D<*$8P7}&=J5R=8`>=I2Xy6Y@w-e0aQK!y4s^H& z*_P6yUNmpUi2^pa8a)=ec1C?w zQWN=bO8S(Hus2ZZqJCxY_Rb(NQ~Nprq~jEce4-+YqwVwFa^?|i62|Xio0hY-40y(d z-}8o9r$xG9(**h`Y^J&*&5{+wNs?0B_<=sm|FGT2p(Z!>bvus3^DuS_S`H6X@0U@d zvfc68mQ2-ar$6}aCjs$rYo|X-9g;0Sd$sLLs=lm*{VeS)fcCl#7i46axXlz*D{C%+ z^z#wm*gJtV9VfSKgr{zCW~a0K^FsllODapJbAUC6)u{GxX)In!{#y+pr=I;TnjkVT zxt)+x&2i82|G^IQ;k?I4NVKsryD|I81@ncm5o$gpzgIn2i`{ge$}^AHaK~7czFiDf z&+<}Ef*g%z`Nn}b>|?tT7dgCi8!;c+5wBwdYA31#wXTtnBdsoyMmvHgisDgqnt%3^ zr9}(pku;PFKnMh2r3S(2b2b4e7JYcH=62KKc%M+U1Rdc3Z9Za>U$s{M{(zr6ZZ%^g z);5yp^4qiC>&BBeO~Dv5dYp{Kxt#6rIQh$f^+RV@Py>C!oJa-B`UOWk@~IIjYyr1l zc2n}ZS#Dmsgmrs(7`(Yjg-n%G1_=nkE0PXV8j)T!w?3n*Xigj0)~N;u;TdKe@4Mnn z#~5-XcbH@!)p5$3j`4UB>Y|j#u&;Y`eXZ~whIqnJuAGC9=BeZcy`mvxy_XlH7pL=0 zaIADDXH)yl?HsZ1ksi``^e*0Nei@iisTSo4|IGG^;k_w??as@%8?CLlFt=P4DLb8X z(~NPt5vs>6S-sV(Q(*!8pCP=zx*xduu(s|va=5MB4;`(WKBpF~WCZUMf|D#8#$l^L zCuBBgvl;Csv1oj%h63JTv-Ym$?lnh*MEo^jHF6@G@T}x^s5}h_7Jak|#bd{{6FB^F z6AXZok>2;yN#L?Qa4Xf4V@^BX(Q_drMKaa}A9KUp+-%6l=C-+~%M?(ul8CW_)ryao zh9pcu0H$-rmiVXT!Kj>j)d@uM!lzl5ri;sNlZ|88POw|!ISt?RsNKk1)vXC@wMLYZ zPK->B=c_YrGKNk0(DyFIF095j$svTAAwVy22CrkY=XgxZ_R>AG10z%5Ty#{52iU*N zOm`GQMx=Q%L^UzL!Q7}V?$9aO=3BAt1&+!++}`!{Zow=M3BYR})f zFStSP-l)2DlQ#1QIV>rxT#gyh4FvMT8YH0`KGSXAEn07MLvZom(|K_-+1uoDseV6c z@brb=PZQdJ@0qJ(T9+PS$?&vrnseJiuSxz6|7Y(I&M==R)c%dB_ULh+6`+C=d_&^CXR`otjOXx(C; z-7Gwa(X*y3h|z7i52uK{b9Hc~2j_rnsb1fRd`NTSAp?y@S1ov zHt`eLW2k`!$Kt*WUE3u!khSen2!@{)HIS9EzOVgXJO4J;cFA!;aX|rHY`g%qG2G@j ze0)P-hHr+F&6dw5Thr!br+`uNv}*gNyAB@jb9{QoBU(Co_E*8EZ*hi`*~ne}Qw+_a zl;mhMzE)t0w|h#)LAZjSr@wY!(Yn%?N(589Wv8@Gg0ppCD$zi__~v3ZhS|wwm)(xR zM`1?Cx&0bJ=Lt~dqvG2m8Bmvv^~q&gE(cf1ovI*%@I5h zh3b{irkqer7&}M8I#ddPw78dW%r|hmAgLMlf!yIM)ml2zV+lqZY(cmi#@BemKIBH+ zxcN~-t!!9b?T;+{s8o-)!;^zYu+EM^1JUFHoIZCW2?+xc$Cy!Z%LKstYn(Whtr@`91mueKC; zW`$7|0#yys#Su!1~JXi9bk<)%1l+Q1o7-EZDESX?Fn|xx7X>{Piv=aWU z?M4ldhIXjB3}R?d47kia5+PX3ZVOU_Mxs%vEK4u|-vnUi9z7!v`PL@O(C{D7Z~FTB zfFwOraqzXGsQCJMZiC2|DFf?^t4j~FWbE9mXA@jZ(H^0|F5&VaHx2o;ylq}avp1a> zp2KXAWC{_Q2%J@ahd@3Fso9yi86CbMa57%NZt`I=>q%Q5WxQG`dI9|8#w6BleG+#Y zX_S7V*7&KjbMXyGC|aMixt$aT;->Jytd0h7Z+4lvSllD9P`vFmG3%XK++Egg1a?W) ztJt7O^ZIZOIov%rSd*U4bzF%bF9q7Ua_nxEu0Yoo9S$%R5BA#{^*ZKaIimbC{h(26>1pE`$Y!z)!o)YP-t#|{*>)9JF=%MiBn4)qKj=)iz18QrW zes>DBq}d4AC1Ef2>{tdgbe0;cacrzSvRl;x91s=eB6m2P0Nzky_pUy37kUm3T04#u zqKxjA_*1;~X8)+47JTjT|g564A zh}3a~Len?x^kr;JBT_!I1?o$sM&ec;~gQ~X~69SLB+D!RS6?|W^5hw&$I+R+t z7_$U1E|b>a9DM9HeA$5H9iVf3S=gqmxgkGQ9NLrq@AQS*0BESf|+(gZ5=h;o~Nx|n(fl) z5<1tm9zGjBBMLI(8LSA2N>EQwU+Eb@ zPQdOU-qbda;_Is&t(76)pOHA6}8XJuOGAeCkXiUqy7cko`R+oZYSvO(QBvJoWt9@Vah4b z4C42d?a%p=AdC)J$*|whvZ1s1pd0ZF_|}S@KTnfAsr?hwX^bcCk|I-U&M5;b4IY6j z!D)S4XcEfAA#Fd~WA%l3eV2W0?8&g4bR2#8SXTp*O%bMQmK9)VDn`vNTX z0FuwB<$d-6JT{G+W5SQY$vfAkQMH$P;7y*Rct)8C9L({Fo@ed7;~=5+hM4Z1l<{-|tA z#^3nH_9B4)<$#Ql0-Cc*e9XV3TrL73c_c*>9x@J-Qvof}zR0-fhKDR=)4NXv#l6>w z1;stkkg(#O%MjpSQ1=1?UcEeKgP-*wmyQ7U9OLkdP@qQU(A-1QQJqd~o5Xk5kKUx^*N5jpBm-Pt(2rfZ4y0^X@>g07ev=YL^ z(z_DERMfMQ$6r^|wu28^%ULYfjp^og7oz0UH}XWAQ=Lw2yTGr?E)fpc&mB&R^6)!> zq6#MnQOE4I;oD`mCCG?qtT{`GXavZ*AHbs17@9o>lDiPwh87-uK)N}GX8?#{Pc;fc zd!-i*dGGt4xglmCz7Vayt{iAYaA@o z?k>UIEr9^Rg9i`p?k){9-nd(1jk`B;2JCEkWWVD(=X=kei@WYswN_VE_mWw~n8zs~ zQnO{#Wy-HQb-SIF>9y(DwX6e)6p8^A0TrY=Torbiy_m8z(rB^(ulv5D-z0v4?c^?TW{0hW*XHa0i8gg+2*I-asuC3NtGnF|yjdW50;nnvHshM}{&`9bA6m5$ihR`9OKZc+IV1^D1V z!q3VQQE2VpuVQb(ct4It^Zten4! zRp=H#tD6y`_MWBg<7TbP3lE{trS3VH*2m0LztvRab6~ebe5|Rs2=rblDrfXU7JbH_ zUqj;cvK8LgZCC5an(Ns;GRoJ(b|E_^!WYXhl|MgXSNHm1YmAU0Y8ne!M!^^F{zMQA z+6BYbquKcBwGSOhZGF~SQB_B(>)1b{yJBB>?)u^B)inO2WVZ;padUoSiz&d-Yquln z+b=+pr?^DYuyfy_)(ZY~6MKu6CP(riTPqA&yb`gmCH0aXd# z9LjcC44XayX>%#2jb1fh(laB39+kf?bEg-DyI{gGI?BIKReq2y^rf0i%fRnWUf|NZb1ldxPyF#xsh`_oLG^`5;sRc+-HtIB$5F<4(ehv5>&aP+gt1Eaw{nQ^ae$4 zHcI2cGER*EnW06r*1@~Q7}i(pdzq{aKKjNE$X|jCob+)WnZrVO6_5Zde6B1UcIYWc zdF_}#`W^E+%?;~$!>6T##tkS_U?3+8?o7#0#}&gRM|kZHqq98qkf0p5M<_3(AG>+c zcb+dSp&YZrUs6p|xT%K*)jYvTpPoov3T|o)9IdS#9SWr!v2B#Xh)E3X8+2aI4M$(v zIAny>94BH7-NhNIPxaU+eh)L)7zMVmW z6{bV_lF@$g-Nf5igG6IWEdD%d@iB3<0q+bkCHb_pQ>pXNt8@y}4FgqT)5$=Mnhg*p z;KZzBR{o3QXAlp_ELD&s-_d@HDcC{3Wxecc(TauoGej`H{^EKW z%nt6e>ktLs>ojphVB=UMsA@m+h|7O+z(emyMb;41lJ? z_n;S;88EO(2vaH@jkFYB3^bTzk?6vM;D000U=NJ z@IAn%k1vLnohi|PPTCr-2|slX*@L8QhBtAsERE;;Jae<@xqYa@x@YYv+q`?yhFn!@ z-t6$WU;yjN<4Z4r2q`hzfRXkO`0C3=_rnxfNEasO%LO>`|OYC23_z}PdB_N4fuH6EOXo}Iij;o5vQy8 zhzmkaeZlN8DOakj#ng8K5$!DU$4!w`VH*{bY6z9c4N73;pKbP_jSIf|7pruye0%oK z?Rc9Je80TW-Cg4ofbXWrU@JqJC@Mp7Qp3NKE3S_Yp)2@513`L5bYDd{-HK#A!WK?` zH8QO=nQ!jsb=8Vhemi%&Sq!AAFCF!)#QkQ9&)YJsm0fN*DbIFjyl!(_>+}!Xhj=h( z;0uV|_mBAR^s{v5d(D~ebST#;2xkoe9BVkTOsI+~1j>`5U)HsM814iVF!@7fh7rsh z(yfcyYgK-98zm^}F#5YQr9((+KZ}5E^Tfb903a5ej;{ygtH9Q8I zaMngPV#;TC7TfCIBJ0X+W9JOerDZpnia8_OCMuqZok7HAJZC+;9eoZ}QZ58Kk z;%N%0t}0_CC00`L~{tG<~i2B1l9|s3F3D0|PjPq?A zJJwrto)zqRENp14=`g56`-1?eG~cf4 z%EV_Adv)v}6FYTS23r}H-)AhpJNp_CR9#IMFMgMvJ7>(YS zmqp~<$sP%QY$KNnyKFVGO8+CwW*I7>?HP934n*2y(HIOd!YWs3UJKB+(A= z8Pt`$itSZE5ExCL2>S{N(JL@OG`e`|49rL&K_fuCNUdvxPfyYTJc7p^|5-M9R?~Qf zaR8j{#4bILJr8?>mpXdp^f8Z`Wf*xFxzql@`2ed_$#sWx_h9va_6DAg_5Jp9xL|M9 z2X(c}`|Go;+l!YSRHRoT;vz{R*%Bf=++;k+xxYuI%_M}!TF)XS^l&OZidsd5&?{C@h*|}Q&@+3;?L~!5)v+yjJ@llMczNdp7rPSVT(UJI=LJ8n+>1KzFS_C)WPEslL|H*x>r@ zqGz)0bih2!^6E5+XGQdeL7*!I!}x-b-+MoO2JKfkfXDxgfjyP}I2c4X=)Wpik5C#2 zQ~ri$hQRiVmI3nutg@}pIX3EO5;E6=35iYN#V6dCeb=$Ze6{a^DT+Bh1dVv7PB9nQ_f47}*JvKs zv}Y*-69*Ip{rYnIiM1v80Y;#t;fd4L^|Pwnqzf>&+hfo5#YCd*m9Jj{7&HPyV5bYX zi*O$y>Ptf-@lXF$*k^xPu#kq3ml(nu4#@>18SSx0d&Fhy?sNC}z+6uA*WR-dH^sMk z^Vih@QWx6NFH1eOy>kjK1eT<4N}_#=vX^PM>m>izCNwu&Ry(%7wMZLzVpOk0lEHG;FTUA;^* z8qSqWDv35N%ATb)ua!(HsYRmPuI+sD7N*V$MN7U6m`H10Es0+eEn1X4O4}l$+Dy6h z)>^ijDkQe0Q}^9i;`7Akq-(@$5m*t>LMVPu&4LaCLV!ttNv8OTQ}lfXb*=)TSsNeGEr<8w$6 zi#YAIC;q$X<&Z)NH2*hPM)hh;3t{=7Jf+24c-8hf8JsZkyso{Z9M{JGDehnY^5C!W zlDzLTG2RH-OOGKWb*h(BeV?6Keu}}KMccN-lqq>0&)mP!TjPDcigw@Xc3%|pu(RIA zb6qC;cX`_DOMDF?dvISDjJ$f8=lrE5cCcD)=@`<$ImQpD0udmV}0hsi+ZCn=P_;EEXK7}OD8 zjouCnRl7rDSGBzV8=Jb zG*y*LVMt+oY-B57(=e7)l2rPt#Z$xPFVG`Na>6ZEbOXjfNSD9*+DU~ekZ}YbAV=-> z;ZKWA08Os~AB=$}fc7Ucj^V@QK6!mu!@y4;x%%2e!N!nb4<9b|N&CYZI)1`P{l8dc zju(LMTSPiD`6P-<;* z0pXu^XaS*J%jgsJ-+Df>F=X1Kg+CUwqARSC9?$w;S%`#(e$p0s+;|lbN$o}6DTXP4 zu9t`lCc`Hgz53Q8#wIW}od_$FG?EQ3lQhyY%$C3TxGC=z)@j5>oaK0ocFP*xX@p6f zb%YxLq~;R^Gb2PQJeK~yw~kdJ@FPdv!g?NSKE@3Pe&VB)Q@UV=XMH042&P65Q{W2eRKSct*9*@V=L_k| zy5Vvl2Gb&_Db$B_s$iC8u989exK9|+R_O-zH)i?<4(N_ucg8nB2RdD1XAJLJq{I)= zL#}v$+)S4b-`t8B z)ZR*r!^^X(%LUBqSviC0V#0e#l36G_J))XGblGJ~s8| z2|u+iDT8joqVAvar%S^wk(It`cH}AQ1L{4LFlfZgm3jm(C~2MCqy3>|`XlNF?RYA0 zdZK(Hxn?SzjHJ1uWQ=0c*D>-~$_zI690_Wf59a8SG4lUkEybkRvFupN95$L937X%l z|DWo587EO7ozPDnOOf#&{`Y&Vh!8zoA|p#9E-8eL5H0`xs~HDfF5^KrFsTCGv1}5ZhMJCgnj7N>puK~)hde@ zj9&NjNayr1vKPkkhvfd0?d+rfGyz4ujYTxyD7j!;WqN?cHQ4D|hsmHZA#LQq&&aQ*yufI4ee zPy=g$Ri{<*{+9D{5{&!rh*cij{-=}kOskTQ`SZk0B!9mECXybB(Ym?W2{hX#v~rAE zfm*OaSCQadKPEmtJ{IcugoFgRjg#pCy*xA_<=26kEmC6Az(i5Mb-c#Lz}Vf{Php;0i~{8wIoRq=*KMZ`t7!$z}n}o&IL} zZ#lm4M#%&TopFB?fE$Ipy&)7ye>CsTO+Sa5q1BVcSE!Vh^|*n5gzyeOR=Ts+C{V^z5aqjHDcwss_n#htd`hc$E_?GHgxTXyh7 z3>g55vxct%2_uj`nb}VXiZr}CTp*mZtq zgQIHXhXCH~40itN+@PtmktAZxabZi3Pco<_f0%x!l}3J%;NYs?)a3Se3Uv5uit=3i z=_(WT(WhsYf3R11eAXU`LG*ACl4IEFgYnNoEzthn53{{68U(!D5j3sxaFGKYY|rxZ zH|3dK8vBcIh(F=%EGbQQeB829D7wr<$Kv68Gq6l!+~ZRknj(K&9^~@uR=2hEyw|GM zPB{(BA|Txi2NS*JVaY1@yJ^gBu^N$aPzU#L^;>Qna!5lKR#kWVR!i;aHV3in;D|wcffyH%F-{m}UII1yFK9M1ATc9!GgdCdZvBy@dqjY`TQ3z8<(pFn3 zW^UnlohAB2Xnjq5tkV5f9|`|;7L_kjgkAQD7_D#iiCE(>S_FGxHEnXZPbhTp^7CbT z%)q-~!sy`wl}OFW_3y@u>_0GzL@O-*w{XE(yt}7ROx(Tm3%Yz<)AIp`pAFBcRU;o! zb`ow?>maAgY5rYIac8oqI9E}KL!#S4xtIfeW3q;Kx1>c|DdVUkYN!IHcGy&m%Y}BD z2s|dn(03|zIO?2$ScTtRq|p2qh`V{U%UuzwRE$}Wr4d+%}=<` zk68ZBZtfNn$obE>LcnkJ3oQEyru+0LNw-Yt{5|a$ef*){4Y7Pep~ugNyGU{XA5>%b zEThe2PKqVcko!&z`>H72b>F&Ak%N-fHU21)N^|b3VsM|L05z^_B=GGz&s4Ks+IMkLL6P(#78WfI$t=Ui_NU%R|cfNctZ zgEI>{tmY?ov%F%McQvB}0Q^z`WY&@0_E&Es#IUGVvTvKUrIM^As_4yYDTZpZXXDxC zqfNEOto|^u(stQNjvss`Y6DM?DsHuPggdRp>#gDI-hnyD7XrV|f43ue08Lp}w7%Qy(t6=6145cz)(5?uNrkPMCSZlPDJNhN8T;_v_j?zu;m*k}hYNeo3+-Dh zq;2uXi+WCTkDJZ+Z19`R5U5P)5L=f!@whM`rt}3(6wY-B2{*iY91(T-tK4o3F1?zHzC~AF(5Jk%WwXqd#cEY28wblUWnD$(1iImi{Fm&t zP!lN5C4^{c)t0DTjPP3x z!ya9>hb6@?ydcgU-u8rBI|P*Vg>fbH3cVG{$anj3Ml-aNQ~nM21E zOlObP8hU5|Q`*B`k3DUGFbyqU2BIDbORvE$d`srYuXDFl-DoQGQj0q*Jke2UE$Vkw zxh|z$R{6`6X)Wq=Wxp<^T%KHSO7c>RIkY(WyZQdE<6#y6xFWm%Dwq{1RvCAVkH399 znv`-02SB{hAS(lE4C0#@X}Jq=Fxy%W%kbCYO-*er4BrPj^Q0fP;;cK)<8SDc7u{X& zI$!h$eQ11t+PeXOJzeqxpV(hF?0GcAvkySb=myg9Q^tLqyk+ukH7@m{)8|X$1k-E! zk)e9+IL8!Tcs_@f%T2c`S0w|>N6ogY^SV%?Iz|x4d%8cSer&&J<&qxiJuvOIr>Px8 zbK7p2Ywr(P+H&V~AC6@5Pd<%nNv*6Xvf86v1|Rc!n;NNKQ(c%@H|e^jd$;M#+Nxd5 z$KAP@wCP+6T?)RsYrXZneU55&PTj96`e#p4smd18J@bm~T_*e}`cr1ASpsxO?jz*H(6NDRbY7 z=RIR7;CH%s3124|EC(LHQoGdIV#k@~$k02zbiQxxTjurXme7;vqTw4EZt)bP~utJq|4=(+x||r`iik zST(ug9l85)04#g9ANfFG+3nfx8NU3D(?D)7pvB%$HBPVaL`ub{pr29YI)}DRc_X3l zR`LJF%)K3p)k`{&Qt~nHXOy|lp=o2I3#}5}fcZ%gw^o%HQjue5Rge-bQ>^y#qSNdA zME`E$XFs&Rzx#@COFSS#c}pxOctF4TyS*u`muLVm--|Q-P@zaWbeCzvUQ;jQ9IH`2 z0}-pSTvujxp3jB%;Dp%Mrl@T|RAn}whf}G928VqkGnbH!?B%2w%7AJ_3K5c^reiz~ z!F4Q9Ahkx6p*<%JdRqGy?~ZcWh1AsS(%7`V)uMa;m%DNO1R-ID)-n^DA{XH;qfoV zQ{msexyl@^vPZe_W;HkFMGSKAee#vmSw6WOK z?3v(tcygfgZ)Sf}Uh_ZWhEvUL@tyh`0aOqW@#sVIYu?qzGzOi+ZZx`qZGeFaMBOiH zt12^CP2kBZy&0mij^(wQE4W+RTQc1s-%i1`^DCUaFPtmkZWtF`x4Wf^4Ww~&PZ$f& zmrTB&tkPD41Ay6=05$a}O7+Bob`Pk`;RJSMHYc|v2C$rv)5EC}k$G=F$!c?-DT_S) z`g&H~5FwMfyUOX4EES*XMuYvKTdrvejRe8HlOUb@@&OW;YXF1q%m!-Ht-&8AR8S$c z_AE=kp3Oh`x|pKF4<^|N(JDAkzexXiTg(aXt&D&z@LACBEK>>mo0B9tCh zpBOEKG{NqgY+o9w*eiLYjIG5I=Vb|%>$7!b_}9StBcuy4H;YEXXA+$p8k;SD5FrB1 z+aU|t9~(Vv9wZ}4Y6z--W+6o~co&G4<%jmZv#HX^&%T*}q}SBn;zIvz6b!n?+b9^( zq4xaiI_0$3G-$MA+YxA%Ibd32l3|ciT*6Ta^(Hsq?orhMcT3$&|((EK)i|3t0m&i;!xLpccM zqM`l%XmMestiB_>pr?*&*HQAWUy(4LNQpZ|iigFs{G@d+r5_50uKo0g!`O$m*5~_d zAbG`N6oQ*?@wp+&0$_F=k0rBmTEQ31xgPh?#hjJgwY}2~LnyRcC2HKZb;W{>o);@y zmPeOv^!MCa!x?AXu>wJ35cwNINna<%Qe!)ESG!lIr1aj>QY7PJ!0I#R|04ZlsuJ~C z-*;@zywEaiMQ|rj!DF1E#|1VWf6-4~d6n`l#*QT(W`66Pk$YU_qV=hyNrz3qbdxWt z_D(C1HgX8F^mOKO(YMInvdl`_l6X7`SiQ+y;{M;IRaRlJ4yKjT&3guvT8(n98S9SJ z*+C<$x`o!|JJg@);Tvj1bf!ig?K90PwH(e5rs`VOmb}o&qG~Lwth%r`Z-~d>4`No; zrOEqdl{&x0ez6sC!T_U|3$Ck(>7p(tAC6qJux&gekDi&MMdAmQ@^Dpw+_=cTm#lFB zzeU=hCeURLd!)5`-?!-{Q)?B{5=Ut%!f|w9br&;ojp97L&f=CtM3m5aBf%_S$bKUo?fTWS6^#NF&Fuz@upzy~d? zFjy(S?jM)ZpVjt5!nqNT7|IBMO~>n-lJA)d-_PbZptDic!#Pp?GrO{EGd zFbQDYM>~h1wsA=UFY`jMmzxyCNH|T?VCs}JArv+R z>!%4}MHGik#@h7!EV!QaU1DYNM!t5+HbUj=5;`gLR1YKAQ=%R` zZ^z5ymWem1bGSyl5W2h~kgXoD>?bysXLi0l+kiJs=fAv$x&;Q`S}inBSJCecLHGUd znBpf9rL31tcq4>4^M5*jg~BG;#ok>WhaQRnPp1d73dg_PEa1gDmEuq7$BXcnRX<$Z zT6M4Ru=pGQh_x9qm7=pJumKX)W8CX*Gu*xs>;^G~dR8{J>P}DZ-gxNfmF@02d$*L9 zO`o6ti&=GZQ`{<0R0i7lOO|Ra(h7z&ZoanQ=do)+kgvKi(|Jgl6f`>rY1}!gKBR;% zMP?n(L`^jMXr}W<^AMy4R_&!D?@Rioh7SkTW`m~AXB!2y(u>I1d0p9@IV@HyvI?BN z>q}9m&%rm%RW*kfw|d~|5!JKZS!YwLvaqq+EsG`4>5kG|xAsPX#?$8vg0s|=8#G;e z>>%Y}q5kv!Z%khUa7DuqM9W`i6_uf28;x5<&qFlJxog|w-E5{-3pDvuLh~$+ zl}lDsG}ve2WcLjoklo0ZzVcV#T=YDJEPQ52;0$%-oL|Ux{WQGnQ;!FgzBY$mRP$%= zGv@3|nL{njp#+~i#pi|Rh{rCNxNOf&I)F#VR!PH=-aw%x3VbU>BQBd$`+Keg#N?%i=bAvdB0{)xdS$x14A_Xxa#MFExPgqq>_T zht3QRbar|9f}ZP*ZMQmyu3Pf{{4omQ*PJxh@_pV@%a+K~w`&2e^S-E*w>r92?P90z z)&ejnT$ZiDNGZXNRl%J>3V@s?CElWbdisVOibc82*$q`F(L+$J&PXldoQu#r!{92; z73wXz>zdjXdt*J^<&9@sMGI=t(R_1j4;P44*KOop__}5l=$@{cVf%nqEE+F^BaA8E z-xG>xW%LQ!J!+L(kDIKoWCgq5lHY<;bGbTYBZw>q}$PRXjYsP zId*qz_3mrE>TI@{6R`(cY1mUidDn8Tc$*2ER^r^STmVHCO^~)8m$p5u?w59NDO;9{ zDpf}>)o+(}nnKjfhE~~U`&VmbjgK4m!VIH{l$z(E*DCRtzN!1_%~ZU3SIJiH$E{YC z)zdpFIo_3tW6ZU{RW7eLz^o7qJ5t%sKJ`GQBXBVVTz^iL`AV z5TsWnwF3@UJ=MPy*&~;BJOAiB7ihaz9!rv$(f zcmntb#QUVlBvj8vb7+m3O2OY_%-KlH)0;O@SSQ#LLz02loA#~lDgPuHM~}uVj86{5 zIa>75U>n?67ZPRqf%Mn)aga4Q*eW&EvgjDCjny3$lckJi{qxBa2Ps{fz5#mo1`5Wy z{$vj;R=)0g)tTY@D(m!@0zxh}ap_bfg3EV|y_5sIvj=`U`wua|r3lxRx9_`Q6C1q@ z1sqK>rC;_#%vd+%HYP7j`_rAF(aoz%V<$Ojr|xwGHonjGx)P2yn$efXDmfJHX{&=O zYvre~`}d30?rZm5e^VP!bMr!<6Vl+N_MKk7C9d|bbdhiEaeTar{dDItgQ>`xnysVv zGFND(cC0qXDf3pgK%#n9Ee=z;DM-;x&z%G@vKPYVa` zR8x$e{uYyovtFfPIwd%;&YXnV@?Hyn-%n?;u4%q(DA%OIUc-uXZvMXYBAg(~o|~u3 zg|O#k!}~BwE7a2~=srm}YH_PHcJlXb%FH#s3!zA&NQ}Mo_0|~c1TbdG&UGHx32L?? z%!Og*em~0vyl=<{^Q9a%3|LZ1r87ci2yAhU8uj};Vzkzc_bm8mcx^@lw$g6U_{yjH?RD@VvQzRtR ztu_DUnxV0FEVCAisZ@_u-w%4cDy+pr$NOO;{`nwp0V8eWu+aNx8C1OGz%Wi>M&RyA zNEn8r8jTI|=^>yQdh~9#>*y5rVQ8gtZqYH+fRR%bkAuPmvl3BMz}HSzF5<@A`Gi(xH4hF z?&;~AQO9k2Q8rj4D%<7R4vnm7yONo~xHTFS><4Ce5&mz%;MR85>Aim!i})Xy)A~a_ zdB|Q*Uuu)yw-qjUmI)OucmwbLP*0oxhX1+r-*$f7q&bZoxLsp;s2>z$_)8K$?YX?+ z`aolPjChOw5G>@mwW_-Sx28~>+Jg!Ui0dv=Q_=k#Y{eodYF?awgoh=N<2Fx!qTCI$Gt&Prmsjco6p&^Fd+t9Z%^!q)|YTX47mKbK{vdI zL#CosE39F}5oB+D!^dJ3-1X0l-vem4*voo(SUq^wzmB2`sp9ezK=kcuC3^7VD*Wv1 zK8~yk331&fJ5MKJwi3lwYW{1CWV9IVeGi5nkyUFA<$%9Vh5fE{^kWzKQ}@{Gafd}h+CVfw;W^4535*bU{n8Lzfec+1)COZW}syiw0~@6r}U1K{gAIRC;FJf4)F;|!Z;fNCr0Y0;^;g^hjA^xS6Jl=eqM4K$ZNS2@#o8)Pj(xIyO%y_*-E2?>&@?DISMQ^$I zSk!LM4CI6wFbgt6`sbM2{g?_Qv&}e;Xm&^B(;7_whQM)jb5d_vsr2zNNcDQ6c$$O& z=sPr;BQIx2WKKGJP^4e_gga0>P9#x0Hux6?OM=yT+nA!LdB==dIP2^*lcdx1Z45|d zD94O{$2&@N!`}h#Lf#QUJ`?qdH+p4-)`F;@jO^SwyH&Q;Md%?>tBi>rEMsd&Q8Ucr z9EtC!rs#Q#cXa8w_o-N;j0qGs!RLZc+!Q^RRJBTyKn`+!SS!or$Pb-RcVuUur%cSZ=T!h0GoLQ{Py?wMYc$cDlHEvP(q*pWqzwEzvze=3oLJKuC^RzLLv zsSpLaa^=z_Xc%PiG-gyKVaA9v2md?9zZ>E!N(+aWi!&qB%l;Age`DSU7G?8a zYf6V!&G}ZiQt3xeoOb|=n#>x53ewEK!djt5_fnRwM3fU3HxgSp#WB4e*g*RNGrIMe znu*ZC$dTsqGod{uNFuY9MFGQ^y(f#G!@?cu!}C(f=ZD`g4av?j-=6z4n!31}M&eTT zL7PF)^;rm1S&x-hN2P7~0lIrXeWTmIHKR7Hf)QP?yQ4a_6fG zRn_vD^_ClNM;l~Ixw(8P}l!uIj!X+ zN@vF#v-ci;ALYuo;Ia8TGO0{An@Pw&tgVaRFEO#(#a&?YPd2q1N3u!kkZ~=eHMK+Y z55?8)S|NqSVXlwu$U$N-vD29spB3%rP^^s_z9^RFyeLlDMyZIm7YQRXBpkI`?x#L1 zAo3JQ9B(=?DBCwy>OOZP*P3qmQ2mLV7N$YL399PSBvdWwJyY(XRyUA>#5r%y!0wPM zIy}*vh|jG=W@fQ29iCDU#rlWIP;P5tY#bIA78PZ0y;SCqI}XsCbxE}%=&Rkf0`)7K z%5Yv3@J;WZmS>1j&_xN2mz))Z8IRbg1u0)h*Nr7Rlpr;z25Qp@m&PGa_XX}61I%ab zJl0fnQQNNN}6^c`z09rjy4b0rE~Z@ z%?Vw+K=fsiHfNrm2`wO-Mfw+i5Qa(s(b-28HcJY@a9ZeK;_!Wl|64s_+&u=#bxQN%hHbdP>mp2k06%`Wj}qKqbc5sEuyn({4YC<<3lK$j{GuB5 zpCp{AB|?$i8}ckjOZ+>}Zl|)4>bj0r z&a?I82HJx8NZZ2Oij^O;Dp|S8h)RV41t-R}ro)8_B`1)@hfLt~=h;GhO-I#miy7vrZPcQ+h*{Im)m z$qT=UU`6GVMpN0DjKzi+T5R@{=f2mwIRA-bqC66`KN9&y1?e~T@w}vMrJ=I|(;Jue zh?xl>V^RxC!TBq-m?Xm(B`b#zncB@VO7_G4?5Vvt2?Euaq~&SG-e}F|w$7fL{n-u% z_6GKEFfXY0&@d55-g_}oV+$x;iOAFHDYTMq(YQ9M`N9OW9$Np{pL1MInJwjo8`7^^ z@zVZ`XphFw<9Vd3?I&fA$AGE|tF}T5%Mn;)kGvFQd73Dr^hF$AMDnfTizsw2W_A4H zjt{0q%W*O+tr9()c(^qFdYzx}ZV`qn-v%hK>LY!LVwD$!5tVb{>OcX`g1LAJ&4SkU z>{9R|m;ubDTs=s@A7FKRm2$1)w}c_hbU*h4H4bbCDDhTsUlJO9HeyYE&hKkm?le4j zQL|?oeJ-| zIfj%YC)RiDBduqn?1~*)fVM}j3!k783$ea0d1|`%&h@UK2O=*Z;d8u-o?;e<;`>Ae zEo8Oa9UN3Mn_TmRW{F0F4$&GBtOFJOH~MwgL`eRNoxcawjxK}l?~fMW@Mg@wcgs?{ ze5k+(NtNIGYDk6oyXD4R+8%eL3t-j`w>%Qi4&7b33*=mhbY&tmiZd#92vdou=x0{t z?x6*$Z{QaF-E`j36y@%t2EuyyFT(cczoO&ZHRkHZrs8mBMRFn76Z=LB%k(X^lDU?v z2ODS+1lKef(H`|iuqMlI@t&&cmeV9&A<;zH6fZ`D^yzP$Mvrm)mM_b|gK_-BGON^m z-$dIhJp)#~^D^~oRg)4igcP6MbZP$jb`%H^mZ@04tb&1y@{j+Jss^sCi$6v~mvWj$Q_i*>`+F+_G6xHLp?8w2o8N%9;F|R4{i|)+CBk z70dYtx$W#yn9W05;BpOH_rUS6l*2#2*v#sCxA?O(dM7`-k@bFV1Ju-3FyO95F{vuy ztgJ{Br|elf;I4%=sqoQRS%FAJSqx@URj5Wqe(^kUjyCVYD(Ez#zo5jyq9|pKHm~;I zWx_O^vx<_~u=OvF8faXmL| z7a*~G2X?&vuhB0fn*KkBzmSCg82ygbW>5h$BxN1XxkjLR0seMOD6>5aegWtC*SeigUTb=*~+tFr0=WpI}?GzKvjtF~OZ(?&)B>31GNE{nmLC_^bT#tsTA+9 z?D?U=%8GcMjw3p#pyTvAW!k8qnEueJ>@wW5%eWDL-B<-7l3HQG56aA+g(k}R31eD` z%E|6`r)K6STp{9@>28Oq=I;Da(-`jj*K!w2+htfxb(h0xAFbuD5uuIp8oC-wH7!Ra z$)ytSm(b7hJ3JIbVZ3Kh?p`Z&KZC9Kp6GPgAN{jHmG9S2^O(39Ct*cV8`i8WiA^Df z(qc?RCMjP6gWWunQf4AM_%CyC_p@MC^d&aGH^&2U)e$yfWQ_ge3fv7x7MSQ>>tjgw zx!*28>RlsZ!dggklda~Ph5(STDormfj7H8|55&y6w?&x7aemYcUWm!QJ#A(jx?Z{$ z;8_j5wd>+Cu$Rekw%c#5Nz=xUV^HFuQOv4EU1Wtf>rzOf&0X>hcFUsCqDpVm;>~g} z#@kSAIX`a&zR3%=GW)#{vwB-S%y9LOWU8azRM-r$!qqE1G}#QX7*pEjJ{>z~gKe9~ zKzad>UGweIWe1*i^YGoSy|4h(%4=<~>abprb8_`7AIJ6ic>756ev!iI0SbI?j*(C5?m5IIREsOv-dgQ-sk`1%&fWQ&b1!N zlbNjNUiVsJltTqw(%kdT>!9prjJ?qPZ6j;CWjhc+OnZC zhy3V;4X*ZH`EvZr6%_cqnTx5Is=v8?#B{c{`7-3VT%%9b>_wvS5whC0M@;WS)nhip zLuoE3BE5|X9_!m`WOo9i%s2-IjD6PIYsvkHacZR5G=Og3cBI3 zahaF6{!U{?_gg($`<%PknVfajMxt8u<3;NAu;2lS(DKp!A)!FzOW=BAP1O?0CH?A5 z0Hb_Zp?Ru>TC2Kb@Qg7_?#5N|?(+p*$j@c|T^USWE5waO0}VnwS|Z+#DQmEURzi!qpBY@!lT`j|O>G zl~%nXT$LM0bY4o6mS3sydmk;fYJm5K}rQ+(W|L_0`IS{4d>O>q8w~(=~yh#szpB|&RLPtb@{EMGN z4QA7&5^e+MPLFk4xhBehtTuPxy;dc44gNwFn*L<%0TVl}*sK0O)q$%~5Z9_7NuP@l zxZ}!&2OQ!i-E!ZLX^r5=v9$YA1oY~cH{(&*0R?w>(*l^lScy;69cW8m4L7 zM)`iB*NuGSTtCwF)@iiC?|E`U8BTHtz4JL?TI(srlA4J&#Yf88nMDFSc04E>5flG# z#XyqzPJ*c#7l^ai$iwG&Z&_nzBW2C?@BWK%BBZ`1nOX4gV;Yr_+=_fwsQ$Xq0PeL{7%Up&wavobZK^Qtzcv-rfPq(aWYQ(LWhus3p)$L;yRl@;9&cr zwDoXKsoHbjN?*fQ?5qQSZ}afO9%y>yUg;ow?v*H2#L9s~mFI)F2WrLiSOHoH1tC;! zFUZarN2GMXN(iZ`Sv#$@FGQ?`cqw$NiIDjdlRhI?<=%Hfh=4K)wQ^GO=yK|Y=g+=D zDn8H;WM-pz(r!IaoBOY(+z!5l6wpbouW;a~iFplnXcA|-{-IiYtp;;D;`L&4U0y7G zLqlSMMG!oq9J>cdRTpt+dx|K54RE?Kb3e#4d3rI{_XuSzT&mU@CgL`xx;lSOJOy7q zFm|f9LFU>XPWm~SqR~ScHOzBCp$jYQ19I(J!bWqnXWRowTrpA^j-P$?!%mX)RC- zc-M_1OXyjDNwQ32licjtfREC zOIM8@F9L|~LnL-jmJ?uIhHv?xT1$#03uTwqbZK&vh^jccsiHQ)wBpQrec3+YWj8zp zdD{Hity&BkvRad*lU$O(+UY=U-x?oH>1NTQfKkUy(Anz2kdLR7s=8~O!QQq=>dL$fKYwecHJ&;>M`)=9AS2kicrT?>>ssOFnyThcg zjBO~~$!5kf%?7zpokT4vOJ?&CPN*WBo|2aJiWY_;%p-_1Xec48xAnn2XD$vT zeg6$w^mWG)ThWLhvqJ~=u^AK4{^{Sak%;a+^K*dpts{aH*Xt2pEtu>$(L)8<^HL|p z+;wORUzs{ILX2M4LgB}}rpvlR{jZr!5DqUs#r$|>E6se#Uzp42G2sYkIX`^zY)Pwy z{W^Z(q5_jhvai0a<>^r^ryd) zc+d~aH|1cpbv&W*LQK|d?0&b*095*-4vYY%~BhNYWVlZ zeWjw-J^Wm8I6!<)`UUFd2h9RA-#dB?2Lt>A2l^C|X-#&1Xc$CIm$?>nDwzabs zW|ZhhdDlXVUU}sp_19t5x;4Tg#-oFHn+oPiPlY#)GcGHpB_B;R>*hsu=f8FMx8U8{ z8N7(s%L>%ayo#`3dz5ye?ae-u8ChoPKTZC5BvyCXc>Rn*cVRt#CcPC7=6IAg-vUQc z-l8Pp+uJBVd&KKy#dcphJQdzFxncX)-}&D@%zxhzcx_jQC1<;^SiidZQ%9$rF+e45~OR+Y7(YvVS4?T$|Pv@FD&V7YTTHnVrrk;3G1I5 z)a4&|_+j$+ku9H{11l)*!clPFSAnZn{Sr^HZ+fKS18J?(fO;?;O7w^hIl7AzL?0@9 z4BovS@yvyIFTNEvkfiAQvFprue7&H$XSz$%HtkPvQLmnvw%yy-aR`TZW%@pCZrJt8 zYpog>(~zVTbek3#)@S6%_?}@bK7bH#-R62#c2eu$v5hmEItGeS)a(GWZklg{9dg`B zOw);zl8^*DQaIQ*Qn{kkf9$~muDYiO{aG&3>laa-UiCR>$b|G<{V>_RxF`KRaHle7 zQ}+CMTb9b%oAdhtdv|G_Bj%`0-(Jm$niFIs=Dg&`l3{uLVa-cBCUBNw{-oUWmcJv zX8E6ucc|X#qKFKh2m^T~43`h1m9GXS}GRXyOBX$*8)G`NG2ECP)N>0ut5 zEsoNC`+QXcgMyW!hML|Knfqs7vmpYZn`Y%L)9tBIZp}wnyh)t-}qHJt-nxV-Bo&mfA2vX{T;z3$GJb4XO*+bxucSK zdey)Pr;oGceqc@hpnR7^1+h$DMgsxbj0O#tULfgv=3tXMxgT=7!$7qikP}}9W=Jk8 z^l{ckIaW_Tf)`U2-r~y00~14+eQsoDx9*Y9u6#(FG(T*Yo81xf$7aKkPBm^R5+=CB zX`FkaOS@(lV&wRxgb4X9mKYghTs3Nt&A?^m{mXRz@A3(q!pd8!G&_aW76$>;*VCr4 zT`|P?hN#8(zPL@PTkqcx%==+QeY#V9~9$lIm}|gKz-c*U7`1HwI;yeO$CySQDEq0Awk4PV1Z_EAVc8|Yoy6^ z@UOB)u8}3DuC*rMsi8@qMgu-wdq6G4{}X6e?O2Q)LrY>^Y9E4yJ16BE=5YU#mRw++ zEPeJtVN-Ta!`?{>K&@QQO*4I22)3#HK`pjv^tnMuxtt#l+`h}tF|wTKu$HNf;xPy< z$avf={cys|u=O0P5ObqcB=Ml$sz*my|C+6B{j9#a5)Qmx05S%aHe)|7uqO|s;9mDo z@-pt;&CxHX<)z_@-aXFL8H$3S1q1uTy-lc{hOkkmw8JVU&3Q<#F@( zWga*%`h1Z&K%^5Tf4+zdu|H_W{`ZJJH||C;2T&e1M=r}<^TL4-|C9O`*C#q`W(pAL zMkO6KQ$T1p0C*rJ>#2jIbIfy1p_% zVDWPbg~c8;K9m-zN`cj$FW_d;rXbw#vZBuyo@W{MZib&5g}~(ecsJbmv!XAIhBwWn zr&ru!2j6vQs3yBKg^tUm^TOTn3hZ5)unVF^jk~waRVm&6j+s>{0ZmM*)89RsunX*m z#f>}vRFUIy=}zCriQJk#sEQ2#dCK?ko^;*~_aV_m0aYMXfkde*1buEVxpL>-59_a9 zaEhE464j4RsQo`d?66_|NZX3#L-uG<{fvZ3u|F`&tXdbDAS9|E@;`^}0cKSSKviUd zTsk-WkNeG%veUvXWz!uZ6Nms+Osc}&1L6NginEuvRfQ(-AS|PcT|G`SKr%W~_g#%n z2@Snt3noi8A1G~>X#+WAnhBE;_~nRile8_!n}B5;N9P@|?tUd~_Li&-qq2QpzG3mL z$KVY$gmwccJE`Chm`p_S?HBEf1`~_yN8Iba50|6QOyD-((Lz>r@ z$@%6htQm0VRzD4inswi2A)Z7y02FsLKub5|3t3mi!4b#+5!5*t+BM=PS0r=pgT7tGln`l2LG~x{_`LI5^!MJ;$`M3I* z543Yx=>)rZ}6&>1W> zc}%qq&BGUasVd~R4+5iPx&$j|DOO>JlwNGa-JEoxApl(B)8oHZnkGAf!g7Ph@fC1< zDVNAgx#v!XbgaLE>0m2?n+3lnc=PeV6ql@+V6aE0nIU2d?2mP-tN-1}8= zaIH<9ou}|s@$BPAAP(eE&si5Iu_r09T)=w$elHiK)v4P)t_R@d8^E)k3ikz2OK+fW zG==$Xw0kU~mhqEdP&UTQebc5@{KLZYicLnmq)pvOm^&=x*?us)1)whfgqwTSQ}%+B zfh?_Th2P{-@$BoE@vByMU{ca8(32l#pNRCxB1mk?=pi1~IgNq;nU^nxjRUZu-TI=m zn`TT|>x)#}+ZUwLG3-l0HaVMY%5cU*YCnps5dYcZFb5SFWhJ(0XYwK6IsWZq)1K)+ zGUjVhT4c&!nWYQiKQha;VsF6Iv1paqJJeYRf2)KU!p6@@hLjt16T8VUqB8=OH`PvJ zA8PfpFUi{Wc2aOi&1kb5f*l33{>~OD&w6!ax(MPpAoP6*dQmYw6KO10vx^!()>zhc zrRu{Yt8>z(j0ow>vf0wzUvDl18Lqf*XYHj?xA|aMvUR?$HokfEy0dRUKH}G#3twa) z(b-VwpSi_Y(i94B^Fd-I(hyk9qh zWke7y;(}d$qj?_$ET1w!I(D6FZk{^1UMbDw5ClP1RiAzt=R#ll283NTe3(0xh3qnc zhISL<=69=Cr2)%m3_=@w;oZmLqcZSH#Pb5KS9jX8#kc%Mv4917?rjLW-lJRXI<^Ip z0JVnSTasL!=&Z&#h)Vwc7o}sOWKM2>=Z;jFBnXR-euR9TJzgYB1b@V_pw;yT4Bbv? zf`3$KZzz^N$;^$kF9-gfpEr;Je_XZY{|F#Pb=_2uba__%Z1S%wZo-KpiYpo%)ov`~ ztk;IEhNcAyqivzWkmm6>lVV05#JU+>lk=_x(Z!I{W(NEu!V}jT0Zu=M0AMImyl+S( zzzOJBXT2isH(^05!DiVN^49{9W`(7T&0cU{)nSLs3czyK%h9{8xQ_A9tyV5x!bTDp z3Z3U#68O&RT*flHCV8e?z6d&*dH#>mw@>?QV4Uw*E>+j2i$7!o8;xJf@jVRP6#OI*V3~-><0q&6Ty*@sFy%Z841o8nMu+~Fj0y}gM z%Fj95;+(U?oZ*J&!fhe77u@k5ZiR*+;TO}-?`SqmyRPlO2NE5Z|1DiyqO%%>vUmjO z7OZ_GknL*6nP)aQGF`Akf`!iyfPmI!013m|7sIQ}vv>!`1BpL}ulpch|D@Q*801a# z9~lBGyhw?m20gMah4T@!Z4~9l*Sn6(_dMJVW9xpG`DZhV;I#^~46zoHch)UE`;YTJV&d*o z5DsNE>8$!@^~{Po=?7ug^KSx#N*rCqgiN8NuOnIKtGI{ixCg4ZhpM=*=mx5uCTI>m z3a<$L!JGHZLw}HzZ^)`Bd{sl|1Fn*BuzpSj-RqVI5~bi|(&j8T%QQx&w52Px-ebwj zOTDjoZcCf~S>-1;_XBVlJ~17*FgvdJQ+t_WFC{Y(1Q2=!VlQO{p>0-fRtbjBF$CM% zWB8sU{#}R-&ig>8tn4RQZOJTEt#!3m2y5_WRxuXPOdjs#v(0rK?k%_5wL()v_?c)) zl>Hn^SJ0g89s*M_AR79GDOZU6oYP9);sd|AkMx44gARlrSv-e(g`H%xjp@gbnvn?K^#Wz~S=c?v4k^H!goXAtvYnRn8Ekz56!p5ekVfj}7eQ5f`}? zaT+Z@gkZiHWar?}Xy znaKYPFS2WA7lLb|{!gfo`ME+CL62$WMIlY#NH1iaN?bU{0`cTYTP(Wv6e8P;bB~^O zwDKW6EhB(F;6c?rdfLEFF>mbrwyY~0XqVf8;6X5gjRoDtmy5Uf=BGtQNU@o{iAQRqvFl}kJ$877SW#n+TifCP^s zgQ>Q#0WXmh{fiilhHryRTu5D{u&A#UTyU@1U8t@(R($Z6u@x@v)?9;(c)tu`xi-G! zrEKWMXf%3~Oz@WslRhwQw>>cJpnl0q8Hp97M;eK>3cFwVgp>f|U*ii5%ly?7iU!+<$T7S z;S0O+8ZOyI$D1X=c(n4G(d(8V_aXGvIG}D8X;W3`r!#WL>L&g!x@e+CT$0 z>SF?vQ# zeVDd5tsleieF)}8FrTuYkpXr95RT9{#b|C%?r=It{CvV~H^hB3TH3m@YX)huJ2>Ae z#av1IlqxV3ETb;fD`KtZjumz>*=;knJW=xLPQ&JV|CV*|YqJM(D{k3Txa|)VUvjjM z?7AE`ooY=4VlHLKOq3t167HmaB3XfYG#DFyLXtV>*N5&F884{$GDLa+%C6zT2zMx62z_w&{V` zn^`I8o8zEk*vhq_=v~luJWO7TJY(z5BR3teu(Oax7$*^efUYzMiXw+`6eJ5iONRAp zM*d5zPeXam9ZI&j&XuvUiomHdZ@W3)l)mofOek698ZTL${sk=$ka@eti7NFgAE8{) zi~M7asZ)I`_Zd?3%!W-@sEuW`X$vVce+cf4(Pa->Lk{f0Av zYmVzp-qBRl8Gd|`a{0$Qo;g&xbzrE)g%}3@`;w1t4+1>oe8W=9)sG+BwFyWO+_3Je zv3*196zN7fg!SwE&mdr&^K@?5z@x|8vGgu9r|gA{+iNCxB1x%$jL{L6I`%|LwX;r= zu6k%NGdcN>Y+Nk4XAl8jk1XL3?N>zjcIxUO!8G6Wb`m0H`HvEs7;j<-P~ek1-^-#Y>x-#wg~BZst&@kGm$;ApL!+dbvq-+eaAd}f zK2@Tpo)3UdQCSSTS<7X1Z)?8d6=ool$MbR2qEuSv`&n*UgYdE z@ylvz(6IVT=6+`w(q$X8*;}2_?J$`$#k_JC+^&YDuV$Wi*ysr&G+|^fKQP+5VI0{@ z{#-UdFv;BBjnI&2THIX)TJ1ra>bmN^GmB2?Z`04NEN<^Um_HDTEwmSD5dq)Ti}BrM zF;3qde~vkLbj{Kqq}EP#&VGQe?iMz^l59q&tZwR_eQ)dj>C77`;ng5zuP0;-{h>Gl z|1})vc7#Me{B|`uA^niG>GSP?KXd*@6C{P@Kw?fLY$qKZ|5awiS*hclN{^-+wzOn{ z|I(ZMlV~&PbE%4Z*KKvW-MBjpuNGMfeb;2&#^2YPKnx+Y+IQ+Qp?6%*_I!@FH9COO zlY3(_uX3#q7~6@np~v%h{Q}}uF>&0vEJIrdCgScnSE>2iFage;k%c3YJr2!JG92w$ zp)4%a?RNbh!1vLt(UB9H|Ke4V+-)NPx z5l9eYQG)jG9BTWJe9Rey+n##ywPtPF-FKu*sBuofK0SZp|JpFmNgaeUhCY(SxuD1S z(f4W#K+ft1i>vp-^e_4OGqY~3Zko5>Ob@~OGOfNMp=7N))jX5bv|#! z^geV{qB`c#KkU5iwV&yS#=7DyGp5P91HOBxA79YwUVpy15ex_Mz4~Ut=PkkpV!0!q zb_YM_Q`{cQ4=3L;O zjxw?ddqKKonukY7LfcoMz`!VG2Ll*jwf0lIbdjKAwv_}dzp77a6kdTCD7BRv zycU@c#83F}+&U;4u998uzCMM1 zl~jMtSQbjB{W>nSRQn>H+j(FAjsKuVR@FoS_bG?cAK@JLHOX+p^px-gS}?Gf>v?H_ zuiA1j^TN-Vv~Pd}0Vj=KmB9prnB_mjR36)FiZwlyG8-^l7Li$UB@Vy@46p@~?ru4)8iT19yc-#z=& z@Le7rHJTG)Vs&E@04r~2S1WHNd%N{Z$CyxzHpS$u9dk^ETKXn zV=p}x8Ar~;omOL&sKJW5o*R*5wNMpfm;@LSaMJJne~?FaeVjUC0tRt0Ni4bR{U5~y z3{=&19p2x&3H&GN2z&wN(3joM9TYWc>=qqGFGZ=QT(`LM7%G=dJw-TJrI*E2Cj#mO zpgL^PYyHe(Rd12Hp3+ynp)qNRfNB9Jo!*hbIsB8m6vm8EhZno}F_^vgJ$m@xUvy3;ds z9U*bqKH7))QMw0VI|Wi*&x~KS+D@d&{~MG92b`2l-_cet5B7OHQS!ei zKLg@*&ffB%a(jEMqO7yRWlxSmkyxKe=LT}}lI*D$cG{3u4fPU%BAwGTPOMtaLhV;& z_;%XrCE}D)u^N-kA>`!6+3HwfoL+@=44e@WgR4I#P9wtKBc9u7Exl)n|4cKf_Gv7^ zPD?#kqci~H=b359pR_YZGnSb>C>qrYStFE3KO!Ii6Fk2o= zh_mqht62QsclmErAb&7~s>3v9g-`x~lI1-5;VWa>!^h`XUPJ+Sy&VtywdtST^LdU* z)vm0L8*WOCKJfp1&5RVxuKYAPOJA?y?MdG5bVC_-z8w zm}N-K1`Ca%YR(<4^Y-0E!2-!?^@&>bmQ3{RWX7(7^v^rsnb@Rt{zu<*)l;Er)V5@< zzx;YC)$2{!Ifeq~%fKDv7K1qU~3jl;W{} z&h_n26gv7F`i$Kl0{SMmtbdXrw>-NI%hK|POn6%82uWRgnr_v%ptqcN`1~lONPcWG zok)geM*KP^^LSR=>A&U@T8x?t^2fZ#y*32 zhRIbC;2>%pUYBxI){r28y1C!@Ti1^#sD7&b3u8gO~lmP{~3*qjAdcpFF< zg4oTxj?SOKJig5HKL?NN4@6IyKd1c`gk=V}JvB%UN;D(VBM}ZNYbW#!YL%S+K!l|d zK$_!EnEs%kL(Jv=N=J@vhldNn!u^mHCNILwAj&5=Cc7v6QqM5*%)n>Y;=A|N@#xyI z{GRkLZ-&Ty+BIV(F6IA%5z+qMB^Xk6uf!G|RJ|hE@BsYjMW7*$a4^p&`wy*nPR9XF zFVig~YOl6R(e0RDu+%!-4j#3PZF4^3dmPsO!dn7o7?YsmQ3YKohJ8o)z?F06wLk65 zea{g8Y`U4F>7Si=ktO&tPuMt{US4N4uFvVXv~kAW2g}CHl!y{FQ1F9od$6A01FOO> zpE|WhNF;o}^mlng!hY!W2zJC@-h)%V&#Jld{A zT%#JS)5TvmOW*wNa&2BE8A3H|x=U3v97&Fy zVG|1PICdYS9JbXjvVBdaqmLqxVa8(ibB6hc!P{plC^$I%Qg{2~g$4R6czSAn$;!C= zc>i6%h6(RZ8M=i_$OKd7*PMRY%|tUp0CMpGI8`q%Vs08DoP1_h< zg2zQ)ae;+-CcFmRuYAT`&vnK*&sE0_yee!luPbb;rV^0K51WLHir z955bd8!%NP>!W>`V_W~Reh@adggaRw0;ZrovVFT-G8%t)fS{S-Bq7!E& zcUAH5n~&thFmp3bkKieLCvgMco?=@C9YBWsY8Mvn&pDm-Ywp##pD2bYe%}h$DtS6F zW3Y2LDC%Y>_iN+j{3_YNbP|GSs3-aq-RTdP8{$A)999i-w64corFfOyzB(`U8gXUw zlWsJU`pTK(|Cz;Nm8(cY333(Y)#W;xK_+-Q>*AA|74DovA6Kw3C4=Tx9H}?*gixTS zAcV_P=M2}1jv`x~BrBaR6vAOyoo=15Rqxg0gU*AJiJWj&`lbnuAt8`S%W_3au6m1e z<0{7^Mty-yjl*|c#l&PSI##D|?ZVciLRR|HaV ztdTtAmgDHkd<^%JEwotnU6_2&8)qmlRl*u#HC(OA=@bc^dEu#APvY5-XFk+mll!81 z)g>Y@732~6b5?-O7@1z&FX0oYQ zB7Z=mZErnq(_qsQ*_Yu&pg-;ERU6OO*`@)qBtlMw zX#@YZ{jad5NfyAJLR-cG*>dI<_9-q*aBrS*(MW8k`e@i%?e(11;(ENdqlL7DB-k~v zqpw68q)@N@_^M@;{-VyKd^579-mZ=>8{m~X2@+J)uS#%GPiXO{x+>*qGgar}U}0VP9j`o5-Up@0{Y8;epwT>f1ZT_X6+eijul z5n`M)q81>-(AB9o7{C^iNvl!8Ic7xIw*=QZcHsCJ>m{I3DYVhZC09%=lx*8#mI@N) zOJbh-w%JBrB0kzeh#Wt6I9B9H0&Ug@7DGkKKZk3TKiV&DnrLBI4z`KxEdjqkuojPv zczrHT8`;?E@Uzk_aXfLE+}|*UHx>Ji_~!Oa;6IkqKh3`Q)Bxd}H=>p*o66p5EG`%! z9yL5|nPv46!ZI%OncYhpo3ObW)q1XeS}gXgtY$lv#pQ#W1E=}q$Im9UDXE%cCiCeV zvZK2f)plvqep+*uhBJQ5K1x!~qY`zjBmNrQbuAaA~&GNWv%o`lDl1pi_i1l70<}eJKfFAP|Ux(3!m|vx4pA28uo-jc#kl!agCn0>+^~UJrz8%N<>$>mLo_#2Cbi0$+FBB!;Jj11I<){5h z2p2`2)N!B1*Zr_NGwu&V7bJa_^Y(X@Gx05IV7)CFYKcs=^`y__g%;(7no>lu(#@uM zLGk7#UpG_>h9+4d)NOFAyxozzEvdX!Y`yc@Ge6b1E1Oy0d3-BTX+K$V6Jq}URQygB z;BhK{hnR_Np)Fm42Tq-q`ZV8RwP>EUPWoPpy*I6#x-&>OY=y{?7#c>Mqt22v8rrPD z4XtzY0*t02GgXn(WFPhMBMf!ka+TyIO`6+!W}`4G>^5>l?fgy&T1zpj#wOOrql&>4i)zq{QTbP)57{t!a}5` z-@IsNtXX$1+AvPJR>;I6YtgmEx3yJSBEf49sp6bo=ibwny{@%nyxHhm;tv=~ajN#$ zR|>4x_n2F3-s3Xl-SGD~aal4hS!kVZ%U`c*7;S4?r)eo#mtVpIGXAPj z1T@oox(zKlCHw0R`Rhp`D^QN!C%t%TfO@qNs>N5aEZ%jDAH9PiJ?ox*dx<<<4f-uk zz(M3k>c_kLys`c~7CF-l4%B_+aEoS=B06Jx&;!C82M|_Np30e)nP=%lz%z9=>D7LW zI65NkIA~=SE_bwa9U4o=ld(37s5IX1>^SIjk$o>2BdzIbPZz_m88$b5-wl1?mshb& zxc4YxzaG=KnQs<_MeJ@`DTAx!S+GC*DfcS>#*5*358CNqjTIZ>E_~CODkWM-s+)2s zH0ZV0qi!hB>21d}8Q6HKQ(QVr;iWi9fSsu}nVm`>l)PAO=ZRx7x-?xJy=ymUNPq|v zs?*V2&_Xz5uq3XxD#SK>Fu)Mb#?R#JUY9As-RznSlKVDio$J9-e5If3!LfWrVlz!l zZ7mxvyBp`lsTO7PaC>{zI%K4Cq1a-RL|fyA(Qey$T`EB)&mp}KX4(4TqL8lCyaj?& zp?=cHx%S=Ai)nW`t70a);}3V=HeX9hW2}qK~d*HNoKO&n&g7hS`pqyU;ni!+=d?bs3@G0J_OHA7L53C;?uPo*T>PUw0y{YJFGo% z$gGXx-0#n80XNc@Yu=x}j+B1v$rDAM_jG#ftu5!O!NZh!|IS)*0U6fjDLD!`(*|Mr zr2IHNUU{9TQ9Q{kB|Xhxo_LQU1b9C_7OpW3?N6HEn9l4!o1mV)?T5Y@w2Iw}k1{@s zV=W5M^|uV%$r*}nZQR3uv-dgKyB>wFu%kI`q|u$$w5s?9qbNoxE>sg}tiY_X)3)pN zG~Q=T{BgXdnvo5&f<_nkw*PQqaUcLW+t1k2K|ZW$W| z=2YuRj(^-rUu-U|Y^W{;S|RqJSgrOnL*kU1i%LfTL@?9}zS!H7!IcN^waT#fnHB-> z87(�aP~gm&#&yCF;*oX+=CzeSt;L0-!Tu3Tm7)@zK+CODY>+g#bfaa~2w={cERx zR4Q3y&AFk7itqA7asZ{$Q{FHA#=NoQfYz`m zJ^tba;h*#-m!9xuUQSTS)M-R3B1WOo@X#guBQwNjXf@o$!gSO7+`)!X0SYup-5F?G zU8>pH^%~(^9(F_ILAfRbe2T`7d$}^Um*(wEYtbeaO~?6`A6sUR1xA<{FSc&CsRS=hnUqgoZFXxxKY&GVS#ziVLF-o-K!j>5W%tYEQqJI7 z)Kwgh)G<9Mcr{Zlj{Q}UO_7D@_bvTZDxyKD5_*dc7O7p%h>$*CVe#IO0-w4GnqZrs z#++yen9Wd8n_&gS#>NqFU=GWio9!4n?3}Awsm;8pB?uPrDXonTNn!BauUc6|a@BH$ zpjU-D6r@h=Huetf=n9P){l>_`BP?=j(}HuQH1?-f^oC2UCo};uA|dSe^FSLYdU^##W=LZ_&u) zap9R_TKVQX$0nW`K_lO4p4Svo;}m2fQ%g=tho;C{`W@;r|6iUpHl@Pzo;fxxXSRl> zo!KVMZvgwpb=99|Yx(A(eLME1wZ@9y#5^2z9}%R7Ic|5G7ZmMVo2R&`&hK|CZno6< zeinj;Ig&iu%}|=F`m6$7cT^)Y%qW@5WFLmsI-cOB)6SW&dWwlexJ#*}&%X})+zO>< z(u99<1AbJ^8EfQwp~mT!6hR@sDdzZtbxfc@utsv#rAfBr5pRJ^jqs{l_-oonuWXXt z44L6E5yI@|(2yUFpT8Zud{lscIoA6KREs~h{Ada3~iRJ7dPh(JEgoZ`VJV#}Q7 zA(B#VS<3`9Zbtp8-a2iX{6Pu7Y4Z4CTh|Z9{#Q|j&CvO_rYmdXY=2|dcg79=7OvhI6K$-G z#k`|!QjK-0PRl6^>cF4=YOd9}%QU+wroa~eKv%=XB2Rl)g~b%l=I=$j&gE)I9T`eR zyns>t{?!dapV76=Rt~+pG3IP`>6^rwW#+mZ+|-zN56`5Smir?lT}^o$@Iq;h*v; zt7Wh&tz-bL8`zbt>y=07@zqYz>{L!D5jLXGQ0^nX&*&w$tW9Z{nD?>orrSoEbDInV zeY14a28c<&x%{<%H|WVWW?CA`isnZQXW@Og?NlaBO0isGp*y}mM2cBrL$pSGrYan? zFu+y?Opx1ab--_oRsn)&8s=p~S1nsZRFLCk6H-bVr|Xxngcq%bd|%yf z)J|nsso6-6@HF$cy}#NP3y8hV+h!ftja)oQ-R2py6?bZS9bndEm-@seTevoq1uvA8 ztTB4w6_Sm@mA&$-^$~_GQ~4vD3-Q^PQ(qa5xW?oHRF;_7`%&NJ)eU;qqle~TYMmwEH&PK~{9%j#q!LDLBC==Z7%oC6W0OY6PQ*S-+*R=*bpJ)VV7i5= zPz;Uy>}mKiEpN4{h!KBGbvgCzh)c$z<@e{xm1&5#3(RK{fOi0N)iS7=5BcE?(!1X6 zpDIO%wp>Wqb6FS%?CU6TFSAa`;O2x^(fxiV=AhW9r^f?us<=P$kN|p|OSE?gJZams z8qGPLeXiso;Qe{gm0MBQJHF5l(q__6xSwYr`>WSpc}g^Ake(5p5p`BLS2=gqO6`$E z#@*muk>%Kcs$?)}FR-o%%q;^4ZXa>4>+ab;g2h#^JPo?hJu~evS^BWd#CFDfCDU%V zWVMV{l45qJ_|-ioj%h&r9JEj}zDxKt>?r={j)ptOT7YpeZ{p8whF^IJIPTUNDUxr! z6$RW`n;v~F;+jnS8sTo$RPc@>=7RRA#Hs||GE-LQCHwPtGj3XBhfndM*n7s|`T#9D zvhv93Zd&-a?stb8Ip3J`l=Q#q4<;wQ?a@fl;?;gT`Bla4o54@vcV8?yo_Q!Pj$s$+ z&M`YCdcJM`**bQA`-{7lI1`bQ%}=*6tnIJv+D*JFK9POTm3$=eGM}myvs)NcNi}9- zsErbG9C9C*o@jOI4-w9;ZDSgvY>#Su=A@9e?!s)SaO5MPk6U3d84EjFhYhvoZRbsEGaZ&xcR(AXHu?`b1xq-!RAfeg>Tn z_&>#PmAMdjOLJZ{-H*AJ_Jrgs1ZUvM-Am6eYF|WNGB7v4s_F;5Vqm+&{i+gxarEv; zSPcJE;=Va{mRC488_SyMTiy%Q6Yn+P6Hxqe$XbH^+Hy8hr2p;3ztPFC#p}C;l(6*MU6qHgX zu{EoEZupkvd=_(!b>oTK7r%J1;p=ZT&-=ayGM5d@2;M2{2+G)m_y2zYKS030%gEnX zz&n^pyD`_8OL$|xF`o#=$Bl)=Wn5xhLPTS+v6x84Qe!Fc7@syiO=M$*@p=Vpv=&t`mF2O=6$8P24H&7Wat<#lzw;aY#HRj)>>Qi{fQ*T$~VZ zN-T&`GD*o&nv^N!NO@ABv{WjUmP@OoD#Lp1hRLzP)Ci1afuH|g{C*zfE@XKk zvixpj`8~+;WMuih$nq3qc`C9z4O#vGvOFDGo`Ed?1hPC6S)PR~e;8SwjV%8pvOEV_ z{s^*sF&P5MMXo=JT+c(U=OfoYgOR z;Pel{9w?hDk5OA2DB31*ThrOT@Cka`##PdH1OQqSSs1_-A*p*gfXIEGZui%o7flxv%)g z5!d_^?i>D_(pnWX?Vbv-(z;uqe-^obQR)d8=@AhhFiD#}9yFhr7)TaV0%=m;Y>4TB zOff5vBW;@v(HzK=cHV-RA1J&XD61n|#3g~HVsW5!K|udec-`Uc6k>T`)or07P({aR z9&7;zg|9K%BQ^$_ZwJ1X=q9lxuo}@8a3X|&G!wqIXrE7rZWHBzf1&5f>7jLEFcAGX z_~htLUog5`>Pr14_gBLv#>R={kB25u_G`r7icf1RW8qjxGS({ z7WM}A-wveR!E*6HU{E|1IIW4@z-GQ!h>0(e2F9oh(edj>B9F#GAHE`{AAdLru zv&y(0xG@WpfvLHWCW4*P&0x1gf}Dp9Zos24*t<}DJ`9o}7%wFR6Xzi*m@=#V8l==< z`b>CC!7VZ0KFb$tzB1k6I=^_92Z!l-+BVOs;2BR<@Eo-bk2QF~;|QMcGzTwvRtK-rIl$u#UZ=jDw%H>E zCq4cU@kB!gPgf{``ee`gP!hE_&&E)yXLBgS(;v$AY!Bt)`~;x(?HLFac=m*fJo`f> zp25&E&ymnd&+$+t&XIHuqVodn!;`@*&*_lGb2e1(8HHz$g_=B9LQS;qJ=fGS=p5&{ z5o+~Jh1zlMiOsb$#%Q`s3T;k_%>^31Rp-sq#*nCarahR{^ygdtAo8^4yOK1aqPHZlwM+$jKsE@X7?i?E%JGt0@o`TM? zv3WHHxy(OKF+E4pIa8Y(yAZK0iVV`#VB651!Xg$~L>=&&q@j!}-v!O)Q089F6*heqTLq4UUP@ZHpADdgVJ zMR`l;vb;4kF7F6UP+uhP3f)Azh}nVW7pX6m_l8;O=i~#xf6YeZLt!KJTlD^z51eOP zbL?u`zi7549}SzR-;_^;ljY%X8nyB1`)PjKmNVf@`CK?hz7WonFNF&cSHnx?>)}#) zGQ8YN!mGT7aFw_>Z2edtq}9utp!#w8?3nMK&ZE^C+d9V|Pq&Bq;TS&bSqu16Z;}d3 zr>bKS!#qDZ(^rCjjQPl!{*dOyav%@Z6Z+1Z5$vWM@$HBn^zDisrm+p>2aRcbd!xsE z2ckoC?0ko!r@V*5*L+8#Bfb;S^VCQBhNBmKXQG#B+~GSH9mkl0`m_tt3E!pYP2bfH z)_1+b=$q^?`AJ7Io&WuYjx>KlN2WiiBZtn>{?v{vJNZ8WSYOEev-y!{*@gLd>)N?v={};(#oLM{FNQeeoMz{%omI8VsTjG zEgy`C#V}gz;jiy-(s<0@)FIKf`dd5v)AvJuYUBR)j;P<=(dAb<*89U98~tlLHe((d zH^!jFiL)>_7Sv+DS)g@`#dHhAb^di7{V~jo>tZq509RN;HNh`?1;rtv+OE2-0Up~Z9^1=S9?psPVcg?G=1O7u-{u5j#6&V zDi87?Ea5J1eR#dMDZJ6!8s6+}5BFm%KG)vBH_-FcXLyzHfHxf8<6RrxKQE2}c-Mso z5k27}bpAtIq&5sOws%wbxVJBSlG>klTllnhXZWmlcX*Wko}zW6^QSYe;hxSL?JQ9ibo{uDYFGkR2y_X~Dv|R6aBnvSSF?(-D z@)b6+L@`E+6;q^4Nsg2&X^{#VJ1993o01o4R0<<4%F;+1`ULRN)Q+e>R+dL(S`TGa zB&bwHIu&cA8~dF07xbaxh-^@rBfZqF6lY|sB1Lv!et%?_5{>Ltx*`W?45+M+9HKE5 zg|abnG{#M3bL5259~nkFqQ5iX81qNU_Q)A!AaYLG6S<)5k6cm)BUkA)nxB@T9En`V z-)`F2D#s&}%E>5EPDc&O*=PdgmNFVmqI0-17EM*IL^G%_ru{-~NVyixrd&{NM02q( zrlJKtE?Pv(^2J9>XbkR4j4tz~L|6LKqm{m_sKsZF)=&Ff7++t0w29Wqw|r_#Z_ z&V83z%kAd&Fki!+mG!u@(t|rIui?&0FYc^-6L(g=g*z*KxU;eqcUHcGJ1g69XXSgi zv$7L+R!-BM73MnKSz-P{cUGA9=*|lB0q(4DxU*uwofQ-Atfb@4N+#~CJcc_fPvXwX zXK`m`68%aCAf$!zF@LTyE{4Rbke}F&4ALUQ*!~7Zk9DjjtM7hLY z<*)OT0^wU!5e!0tkR+rE8A7&@D-;MtLW!_USSeHr7NK5f5?Y0J!7V6!5TsKG3u}dS zLXWVC?^Z?V6SfIEh26qF;h=C>I3^4Ur-TvVyl_#tEQ|{i!c7;;x5dO|beUYqt~6IB zFR0?mapk!RT}xf1ybMC?M03(>^oj5Uy3W670J#0^zsox4^I#ht6dJnFH z-izy?DP%QiBlnSZkThHkeE?TOAH>zrPvB~3Ca#7)gsY(s<7#L&u7-XRS3@7c)lf68 zhAzg{&|F*%eH2$i^T6hYNIp3U@;I)B7UF8?6Sx}sXu8CGK zAtpj9nGU9dyohU})wm{FgKMI-xEe}#Qh!A1m>)AgCiS=`YR5Iv23!+;iCf3LN}9mZ zHI}s59Nvhxe^CVSpgiPYvy+VJL-W$lpl8@ zKN4`Ad=a$k6<`u&Ln3hDHQ*EFK@zZF2k?n9AesCC8KaBam z0K@30KY*j2j%(l#!pL7H892t7Od^c&LpZuwIIa)lh-Nb{F-_!?IHHfhhzi8a$S|V0 zxXS$~vmQn<4@dDa9L0QG)BY6mBjx}pAdIodSaNGT*4*IkUUOrfOgTAcyfblHQk?0| zEEMxR$#*Vs7CXyO%AFM;)o{i(M;e`pYocqS&K8h15Wy*f4mvxX-P5wc**j0RIJY`? zfb4Sa)#QLChnz=2P5_1%kTcG6&I{9W$$52Ju4^(mS9ro3_yj&_T2lE8O|s`nE?>YG zX;Q*316j#e&Jzn?&o_azg0zFUd4&)2Yx#A2&$Mjf`{v0uekZ>hWFLP}l{I6k@Q3+h zAVYvt3&;q6p1(LPm-+E&nb73sToG8o$e$8S(~>Nt37O6^A!mO1LY`14ES;88kmbUv zIZ_3%3J#QJkktYVwjc?9Au4nU>+vpCHVT`C{<*RyP1r6B2z!M6!XU~K;kYIzh10@W zVN@6su7ECGK&}ZlgsGUgI9Gg3peIpWiIm^26v|vzx+}|NcJ{jRT}zz9u3}di<%g?` zzv(J>Rk$hyr>oj!6Z&0^pi5mXt~M%^DJ}syQ0bCgLB7w`>FVZ_TpL`yw5MHLTw7f` zT)SL*T?d?5u0yV)t`n|dzTS1lbZE=>ld)%9x-Ly?; zi#E*Ji*^E5;O=u)xVO1?x_7U+;ob-5ufp-5`>^|%dkF4`cb{^PIJ4a6-51@L-Q(^F z_sunf@PrCgL{>CXdlgM$GDw=33D(-FTCA8O=Am7}lf**j0dc8V3bNdJR9q!iiPowX z(IGZF&$+ra`=b_kU0f|XMM?CFQRe}7k=P}!7dHyc;%2d5+%67?d&K?XpfD;P5s$~- zYXE*affc2(_ZuMhl=_Z?c$zYhGFTle%-4^M0VYU^0DK36zbTnv4BHld6GHU6Y?1(s zPt)FrXru2$(30txLmBk#h%G|1_J#z$BLN(yQYsyi1nDSnbwC)T?@S2Cr8AVLphwlW zC&V1-s(V7XAzc@yq)Ff-=W65k(YGn!nWMnaHvZ=Hn-tleTs{Nep@_XXp}i-;w|d&a zPxQs!l5lm3`#tMCJ@j1)eq#2!5^lF=2s~LO_>d&t;5h}}sns(AvW@28i|AVvd=Gj% zWH9FJLmvYk&vThyroLg}oRnEOKhCd|jh+(OBqzfi3F0a_jn9xXc z__f}6Z=yFvDD|dG8@yR-#=K^4zITbY*jwf;_f~kT;fl@M=xy<~c?GZR4T5wE1Kw_7 z(7VCg>)qnr>fPbp<=yK&;63C$>OBEDhrMUK=e!rZm%LZK*Q+;rCl#U?lmsP7NmVkG zY$aDIP>Q^rN{O&vS?0N{tW+u$3%^pSSDKVorCo6=^#8E3R#~U?D4Uc%xM!QPQ`xQT zQw}PJg=XcLG6XqJd76|F<-BrHxvY#U6Ut3tm5=oqeI{SBFU^wKC92+YxY(7R{NYj$>;Y)eO3agT5oa zLVF0iKbFzi0^lqG9p3j1FU74u8w& z@mGuiW8OIY6=TF-F?ZlEmpk#7O9IBcCX9LS!k9M^f3w_;zgF%cOkzu-6WfIE9r9{@FOAZ#U1b)3i^S=>uXmtl5^8-mh`qmNc8wHT{6?fC0_tz&ulPQo#A8 zX1{hG+Eg|nkm)ZEF1v(%L31_ zq}=K=YTp*{?-nTE0&|lE<|PaCp`}>s-&lV_AJTc!0(IxlsL!_8RM2u_WzhM}0`r4K z(CRgJoaT(t^n9X?lZDP3v<@^)=Y%aY?zgF9XX#e`o26H4TP)84bM`FEn3u5)`SW4f zA>5i@7noylZjE8?92=W27nmn&x2gEpd9qWOJr`~eZq0!+<~!Pc%1aB(Ef(N~1=?eQ zIl%&Rg5{FRQ44U;0zTaWKE0M`TsEjSOHm8yTJYVqz|GoJ)fd&q{AGRV z=Pb8uq_#lyTlD^EA2`>x=GfJ=f6;8Kwn#gIU#(rH*?H{#CiQu>aDOd*4o%lQeAbN%E@U%7UMkDTcfbG3WJ z+O=%eM^ld2U|ei4rZl#p{Gc(74X)dA)v>bytc7rd{;-v(JhMTc*r0th?yy0hV@v^D zvO(E4XsZqS-3INkLH%v?xxgVC)YAsfx6%5+^K4KrTMqzyhOG|(+_&z5BltJlZoodx ze`@}c`bZj^38~ui&fW5(Se8}>y=H^B*>+6JH`fo(@xjwEOw11t2^f#rPHAl&nQ{MB z)y8e-0nkqZZ3o1`wh1lIjv3gy0OrPmClW-kYlVy{+?Ys}{yK7DvsrtJ=O>{;n3ryB5Z~cHfNq4gwBq+@4w9J`FIo zwJ=V#Fh;d7KDFmn-p;ePCM`|}zSdp_z*zC;g!yp{^$|B!SXp)cL))Vq2ft}G0!*q6 zS(5=UhgxauIHJ~(mP_r*3bCR!Pqi1c9pE>AL4~zc&1+q*_7iP04}RDRF`Lz@-S1G# z<*%x+LM&wkJGDX_WrY~Z3UQ4UVixOqHFmH<%)!HWTOm%c4ybLWKAPGQ^~cuzYCWtF zBUm9uupZa?{Jh$SR)`g>r&YVMLY!b7)AG~yL5#ppsxhE-N{y)~>bP6ntb;kVE>W|K zOPW1t{-_T6uny+ZI^a$n%=vZs>NT35mQlAvows6RTLKtAN z^LkyQ>Wi(-stwh(s9dOPQ~RP0=FK`;D=Vn_?mCz!>tK$ogXh%Ia<&yg5efdr#T9I6%tfF)3Fkg)G?;~pl@R=Ij4`9n*se=zU_bkz#hPU4TFFq zfa8FZfYX4p+Hn*xrX8;Ut^sZUrU)@|fOz%1F%gggNC#vA%z%8r5qAfOY_4cGwa1#AIq1?&Lq0_+7G02~4w1)Km31I_@>0WJV80j>hB z111TW55NEbd%7bDkP65EWCL=wV*#KDPy$#6SP7^ESOE1}{wD3ut$=ob8=wHffVFD7 z?^p-u0c-;F0k#2le#CJ%U?1Qh;4t7AUH0)UJ0z{PmrVmxp$9=I3}T#N@U#se4Qp}ctDVmxp$el_{Nz0khY zUTR-%UuCZXSnUpbvwgMQX_xGNd(_@#UvJ-N-)!%5eRi*^%#9;wW~MIm#Us_RWrJht1K5XmPYT z1c&ShIyxQQjt!1p#}>y{#}3CX$6m()$05g2#|g)<tgQcOqp{b#@p}oP~pfrRV);6qb=xNxrAo?1% zHSBEI-LOxSgCH~0hc!6{GSqOYVFc3WHM!Vu88Hqr0dljEZ8SET8j~B-8Z#Sn8uMUI zxPQSfF%a!b3`YDCLp*+oApyU{V8SmkB;uDClJH9m$@nFP6#NpyefSNA`%Rgq$H@bx zCrr!9W2P^fR+G<}+D$I9!X%k|Gk8)ncv3TX(#7CO7lS8VoT#2#44!l`c+$mLnG}O*59*2!4Bm#cyxuOogUG!kJc>%8B0etm%0Yhu`5y zFui1IhH-e=^a^><TUguWB&O!br8GEBQbKgxX;bROpX zGnWr~RIR5zfVH*Jwx}i0t5D7tXnw3`p8jt^m*DvlOh3*&jrnQGh1jcwdI8VhgXhbs z)}6=mcfqs%K%f5pU-cEB-{HzYU(kEt>MOK$Q?KZsz|pCrTK@v*#n{7(^9==8Exr8`iA|>(h!gp?VpfS%&EvOxIxg3B0Qi?|Mp2 zqkazaJcoInW`=2+c^A*WiX|+?d%G~rVrlQ7 z{w?PJ9rtt4*SVj;)k5wx=p346{)qZdc>X2sH+U6tl0RYo&!WDBdLPy7pD|}KrdQJZ z%&+jwvzY!hmi9WPx8wOzy!TzIb@yWWH>lr4{RZa0!10iCh5l`3PPk zKSA5et%38&G!MC;@1XBnQ_cKFI|Evmi{&JvPQiP>fO2B0lsg6s9CqAZ*GUoiMfqt>39@Ay0Q&5{wXJ82? zEWv~&q+Mg1<8@T#WwqaHxr zjhdG8D)v$r&7=D-sJmz$<{d2I9Zdfg)4!!{VJ_&0C?`>0(0?1XfchoW$QJz-D93_If-RX;$5S7brjExYG)3DCfLUN z^+%CuL#PGRFQNV=)ySWdDl=&s_=!2;w zm{m0s#&b?`&Ru#DmL{;QfD}QR1(1~{F1;y;1*9n`3MwB8%1059rXVPwND&n2@PU9d zMVd6JOOf8BiGUO-y61kGU+zEe^FEWwB$-Sm<=(sNCnHHll1%nUmR2lo0#^~k^Q{N| z>mr;!kET|RK-SbNte0G}7khbJ%ws1XcJg5-pZL(CSX)Eg9SIcAY{ce9_;~n~1lrO0 z+vsPVSh_@VtTypXe8Q*N`?cqf3|2GyBKfb&MWW-K5{aF`OnmNe3O>wabRzlNtzsmB zTbo3}Mvv+0G-RUcpJPR7|xK$2?vSy%v#Yg&BTY{bm3u6 z{P{lXVjHVwGozTtin!PPh0IHv6I9kNozJ+YRx+0V≪-44MnFS`%+a!2l`nDU~ z8=Ea_FgBm;B&F#Rb-EASPoT>`!ZmhEC)T6Q4}Jmmp#5el*<%|qCXzd>wGHqb;Eu3} z^pjZql+tNnOjexNlAJT2=$$NkiuaXZu$)!4Q7i<>%)XB#A{VWbJeKFS zX6G_3R$_E7391?J7r`1P)k-4vg8ONu5VfXaXFZZ1+_^|@Qr<{Phk<>;USL(MO$O_M zC-K-!a^q?98kWymERHpU2Y#e)Bak$p)+|A@U3?g17c>Kjy~WZ)Jw{6PAHogXt`0ff z)x~3tgHKqeWAVQ|rF)$Gy`79=z4TKTVWv)VmNWE9!Lu5Bcks3_mObWm*^%khli)h= zTkK3nKLuXI^sw@^*r_gU`H$*9E%r4}Nw0{Uqo#|+$UJG~p|hg7;7qU^>u7PJhcd=KfsEpNlf_yX80KfiNh>w6Qv?1EV_8F{`i}F=DQ2e(I=Qjj z1-yMZUUHz8>OrXx=mYZlj1nhHu(Gz#v=x0{%EvAiG4$G~ae zMDQDMrjheQ8kW^P4ezCieVt!4b$-0D01FG?^WgIs(V4KB#g$ zm3Z3^4x?A)k)Jf1Eq+Z;&JcU)_rx|kG+ zjj=|=LnB(Txlw(l_bD%G7Gx6)Ybr1EguDS}o(SwYM}NrKdzG9LO)c5e4JS#x1mxsz zX2?0&Oyd0MPQuBYt`&9DIbF9!bE({U%}Uv!G^cm78omQ->GFzYcwI5Gu`og~mPO?H z2G%&+>lx@*C+fP2Z}l?qyxt104HDJnKABCEn>p%>{#0gvn_w^)or1)b7ERp)NdSM7 zSvv-Q4tXIg+#&V~!>56Rz(;7eGUvC?<&L8pOJ3E7*iQ$Ob(gUtte1?fCvXoMKn5Ns zs5>I*!-@0+c9w!$K~6q;Cw7j4o59!Fso8Ti89tZ}e;I`PH`t4N${E|_;tc;&f-eW> zg2TZpQYvo+!7=8dDVn*#T46hj1F+B^T$RAN;BZhZEJ5>aYAqCrMUgZVN$?`k;$C$+ zgVn)7yj{G++eHWNBh{p?_l4XMOg5rx0WGG31Lbu^rx7j4Ey{>~qm}##Tn^3!hlAE; zP(a>Q!WG2QiXhqIZETJM+km^U$sN-;@6Ai#a&Rs}*O$Xy zU14yF8XU(%if#ZgR+1Rf*m;FXX6h30B;usxZ?~U#-1gH2gs@0 ze;CUX;T`3!Vek94hu*uKi@I|@*^ACzv1y7kqJ7-Tb0u&J*bD3c%IYg3Jv6!Gy(L`a zR)nk<|0veDyV+658RoUbNmZh#E30E1r8^Ti1?&ZO05_pyJ#6wzE5Y=jA1FFqsWm-; zBFTeKZs51W(u!R##U%0_^15;`CjI|SB*7rg*z@2U$pFW>3m-@Sjetl?puDcE4Qt^O ziL`gkKzbrK)GSrM_D+_Ia})>vzqnD>#jLW{@E@( zX@b~{-84hVJo55$4x8^Y-t(;1T=ME-=PQG=tGvd%EZk;~D_{mW;R=|+O1}bTkl~kF z&TFc9S8Uq-ai#1pjOash-AY#XaeBK=US`ZZLEV6!yw2;*8scG9I863d^vA(hQ)?1- z+&k?Uyf>6LsAnX%gl#EZO064-d?kDbm<~<|{}zlrFMBKcBP8}>SrWAb4}c@Fa79+A z3FQUGMx%`obAM3S@?Vi`lHLZ_iJV=m!H%r!+epR+Tf~~k4SSY<9(;w{?i81!$QaxK zH88j8mn!6y-2^T3gK{$?=R_W2%lSJ18p=LIR~C~q}Ey1AcxD7(X}SbLTC z3TJtSwC-1-54@=8x=0K9cdXHiGFubaq63toi(ri<};&uA}HtWv4ihM}~mZG1#^%_CyW)lpF z;jk00nzseb6|=zN@2u92!h_#~_pIavJhlg&H^?WvIoo+`Ywy54g?<=o2-atGsnX#Xh9tOoJ<+aTeF@-E|C8IM<^i_-XwT(BkLSL5` zbR{Hh~>!U91g~x0)bB&@_}68+-1xo(ZPnpIhjsfVqPcO6~&k zmVN|DH{xvrd^mo17R$4da7wXzgv53Yd|MO`@ad=R6&VjK&lUi2$GJIn<8IhZ?xD6cmC{DUfaJ+_XnqJT3x>*^ z!naTwgV%G2oqE`*hn?E!pXUBvOeCfllFdkl85@(Hdm+gMzQH@m6TFi=!M*qa?&c%p zCacGYzTX_pZM3N5{O@n&PWKb{xV%`-D|XC7@NN!~FjB)v4I{N^A(o3^xd>xF!PrlT56#nJKP(*V1wWK|#LiHMc&r)x z5j;~UY=ZX$pXT*uOE{IWG?5dI*+aXQ2Q|$REZo4F=>JLI)`1_GQWo2r!JITC|KAq5 zX-8=21bsPV&$>N8k%6N4xH}sx-LiYqWZuf~vL*8?;GlM0$-50}q zbEx$Y{%V^TL(J4bcoBGhZu&mf4Eo>6TaYeGKdZ|e z8O=-$r4L1dauy#D^!L-sKXL}px$#L?G`E6fsauR%x#2#uABrRxAQ5733PT0my9BjH zFuES_HwFD;@CU<3!OtW!Be^S)U}H@9G;j&_v8nH4M80_5>fE9KcUbT8&6x+{bL1O1 zppI{a+w()__4rHjeWAGRVdmcWM9b^MH_3O*;>#?ygtv@WmhVc%TY$E$%%zl;iGPdc zw^*1%=?M9LRD1>!iC&ww{6Z3UJ_@(1`Q zk`7=MdWA%}RxW_wQ~DQJ158K058hDv=Iuu^4g3aq8okPotDcb=g;u^~~$iPjuQNX#j5n7GqxCj8Y^X1pO4S zAEo^=@5nbLkW@&-q2;;>qj(mbPbjTI>2Aq*-eUM(mmeq{h2|)DI&s(soj8~wWAR+f z>~Oh=(S3ou2G|jKHTYySzruT)k++vom^F;$ipx$>vc)OrFN);bK}8~a1NGgC9*=wg zl7WKWY(dQ$*Q{kfKR%ooow3*-EWwJ{<3^WxSbX9#MpT$}A)_;R@*cbZYv(kQOVs*_ zv6q8)07rv^!ExY7>K3Hd2K=@j`AmBL9h{Zo%|NnL`eyqeBf5*^BDfyRg@u5gXELr7 zmzVHxK5FGck~=QnX{in`50Y&>a;zd_*i2QI{?}zaidQS*0k^V#07qi8ub?^$Pjwt? zhrk|@*j$Exjxv_g@RjI~jb=zs;LFix5Awc@hTs92i(Zr#NA4`QrS&BG#(ME7Raep6 z364Pikc%tUBiX>L5{V%a^kQNl3o~^_+A?F{4UpHxLSrz$vkb@Enil=6^k)mWC*i$E zEV|iQ2Y!p@bg&4LwQjY7kAi)X9|g(qp4(@i2CsqX(N&3aNNV_MYpEhUck!o8Ke3W`QMDf z?NHx?o2Xj@&6;4NNZt}%M%|AN`?B{9rEkE`$o}OWLGmbA5B&x3QrLVK$wD|M1dnxL zcXNr%PKlljmd}@&ix5pWzsKV8U5M}C+tF_gzXUD={|C+|`>|_;oGA1OBAMMYcmbQ+ z!RP4P%k+Oq{44pq82OWKJw~HwSGYU5l}B>YwFSS;&eDc_xElFNIJ=2=Mlzx2{Cv~x z(O@1V%%ESIxmyeVn%FN=m>rSqL-UtJ{0Qzq=P&BkK<8*;E<~Qj9>jj3vmu`+7**wa z@|D1{X0)tOz5!U2PuxcH=Kl@v;U(aO!lf47x0fH4_jdgj-%G5>_bLzbI+2x6g(}JK z0=x}^{-?ZIEvA$gLhq1~Z*^wynfMNF_k4P5Z*$pXTzo$JlW>0?e5*NR+w%D={<_ps zt^9RLzPF#%%8Q!ryasGk@(HQs{?$NP+#_fLULALerrIy)b);7PAST~w5gvSjl(moc?dxY;GB-vmF=AKTIc*c<~tgw4n0-OOKs z&CY1n6AWI$+9_~5_&zvG%WG!|L4O^*fV@!oH;}~mbf&1bcQmam3V4*13U&}6;fu{2A@jIqF@IM6o0Df0)tbVBE zcU6B`%XNIomL+|As`P*I$5(7OkkUo36#DaGf%~7=bICY1?vE!ZcOUqiL4`=-QB!~C~ zBy0JSbL#oIB`5gu9gkTu3crT0C42ZKBzvG=2W)`knsEOwsbw{Vr~8F$=?#CEExK8-06_(%JAmv~?PtvB+n^zku%oYk^NA|A4XNL-H)R z5qTN-tDy4$E$Vf&i;i~?$wydlqkafYJfjQ3zlIN`R}ch9U-1@?PZf0jD zyeFtZW=g+>9T)%jCu|76icQw0o`g<)dYFo2K3ENWoZdF0G>+sa7uB@;K72YBC^d}T z$7bNeS-+uMFXKVC3hsffKS^!{VY497`!ypyiKGXXx1c!_-T}-*t>VauYW*&fQeYd9 zIr3Re-X?0br*B^OPwGq%k7-tuZUnl0fjvl* z8?{^YuID2`EW~CACb1p^cCnDm6La}$7?PLZuFuXhtODH$WY;hu{lr(g4@fQzPQl4h zO34k;436Zp<8<(dpuPw$6bu67Hw8V=ABZFiI?oG+>(ML%K0#@2_%rajXifu|KFW|etTHw>j zKVU5RkUR@+L|z8|D(E~wi+UaHqT?Mz@(~u?s2@TT&**~iui-=K)kJtZur#%DQtJw( zX~^F|a)(m<5LQ6qm)XIPtqVy!Ci$UgB|4WO^WAGK4&bRc| zI{;q}RtND+&>DUX3u}>N#U@eYV<9{Q8Z=$Z6h(h2_z$&+4f8S*`lj(@(h!gs3(yZL z6D^E1ypE(Km=|OoWAx29-`;>916zQM+BBy0Ir_=yyjw_SQi_N5TW}Xkj4^Boze+9Q zTu(x$KCP!BnGaS2AE!UfD2*fe349-%PF>`NQTSL3d^oEhbWt}Rbg^^~bUjG|M}s}E zvjyw`=0USKrNobZ7fC6w4TwK|V$a(|t@iYxE6Cb3#I`>i3wY1l0PaNYy!04Y1s&GD zcDy<0RtjDXT!vwcrY3eSV~6ko;r1 zgKh?yBb@_e*1~Pnb+Pdu@Equ}!6k44emji*Ym^fC{)?1SOOwM*KX4`bN8wLWis#J; zI2prt>uw2hRzX0_2d;1X!5x&6@$_CeSx(o36V*Oh&Lf|Mj5;7k=}ZuxXjX)71iCy) zPS)fo?bfF2+ei=#u^EC%tf_#k9+C%QE_)0^@)F$j*^Qkz*PTG}stM^Q-q3wOa$0ao zP$?ZM`IgsCt67+D%3af+Sjm&T!2Bk1oyMzIFTKm+XS_$%3bI=q9|W@Y15Z|^nPpLn z#J_}mw274bibUS={uPNg4tYH|n!!3jlZyNhztk(brpyx?8t1+NzJ<4A7tg7-Hsa-V~( zwS4>QDU(O`NAZK-Kwj;<$=LrHohH29^}^02a0l28{0e_+J;d@M@}i|11nx0Cl1e}`}{Cp?|fy6|Q2U1;_c?p=Tfl&+=LX(ai;Zdvj z-*z6eio|4%3|@ntJd0!olI4v368v}S4rkP_!3WUK z)3j1RH2pzH+~{2VbO4`6rz5@n07)DAiT89DBzI}ujdU_zDvq@?NL-7hu=6P%K*y_x z%^v9Nl0JC!(=P&xWnzFXoVS-3v|bp9U# z|BYIQ;03`k=pTb;2Op!AqG&e8{`XiuO=%tQBWzZIe~IP-;a)-3PH`geTW}TF9$bLs zBFsw>Ec^oQrVsBi(waz0NAi1ZeC6fApK$Ycw8TojY78AFF#Uk|VT!5-g15K0Mhhl24Jkz^B1=)K53f6vgTIr1hFNG7Z`M!AD>=X1K!v1=ffAC>U^QVIP<`1T}xmgpNYs{ay}Yha-|rIXRz0iTIHO}=g7kHlIUE&hPce(H9N z+mpHXEn~-%9+thOV4wISO5PKsZh2bF4jM|C3%@0vnUh!r65A)}Kj#zuD0NpL;iTlb z-8MJKuIGJ@&NqTVNBqy89>m0&Nn#y!r7dRMe2(S4i5)~N?}1N%PXoySnlq2zk=Uyl z?X~@E&CWYMl22gS?^Fx)N5}2i+uK50uhXmVl$`v@+9y_?tYV!A}|HxZ`lHUl-kIP^Cp)LIGN;&m;YtVPQInm-j=-c_SX9MfWfXMMq zfOyOwht6F5+>6<43SJ|;tV;AIM~f|fB&ZKEJ9Fsm z^UUw6#2UAA=hY_~mqhmFWrm_t4awfPz1wD9mK%rGs+E${eivKn&KMs>@_RRCO)ZG_ zlXdG`$y1Ts?=n>lC1+9j1#vp#-H6{@_V};J72lQ81;nObOYAGfUt(ACS2u&Kylm$M z74O~=Z{Y6pT;ygRrS3DSPmu8Wm(ORKhEF2QRwR51qq1T@tJqiAj9!pm8bn*2`0 z+L^~+M!luvZ`iI8{P=3)+EjB|&%_u5|*Qk4mKHsFYB=X#X z^4Iz7=qiF&8SfBEzXxN;ACp#Mh2^iR#xBd>$PKs2U#Ug^g|LcXtcLv6R{3p9R75mw z4<}=Pq{V#5OUClzu>tgf?}^DTL+yxOkZ+5@E5WOTTae#@m&cBim!>~$;AP?MuyYXo zO!y(>H^CQ>jG%O-#mBJl7#1qSD+|xO6E+u&?Xi*+;ZX{_s=f6J=EuTQ^uG%I@5H=M zq14WP=9K^p4-hAgzYXsIZUS#&r-1yTB(?~?7z;nbM_cSHzxa+?2jc~qg?aLu9g8+L zqWA5MK~Uo4y!=K!^J+L&>dH6qA}=!zuOq*Ck#F`!l`}6|ekoI8xQ_6s9oRDJA@k@^ zzL%c4I&-b~UnEiGh&d9QRwo6C@ISzd(U(Z-P*?Qt!+xcRzmuG~htetZr?>n~=1k%+ z%5Je?R7(7oxdF|S7RO~u+ztk3f|WCW1Fxfb-s1R#ocpelKg8cV=N+Xx_kFvE%J=Hq zU0?S)_2}69O>dI=K?QZ6Zk3{*soT7Dib_py)h0y^YudPein`RgRoxWNp8Mi)m8|lq ztm+~4n9W}eR7=&)=B~HZN9t2+yT6*OzEz9VO0_}#YH!k~5;gMMeXxRhToqSO+Z^}2 z>Z;zcJK8wgpZ}__)GW1Ftx_A+PIX9L9;lJ5va5=A?@LkXsj8>v21S`AcF)oisytx=oRZhPB4 zo2XGxORq*e2#DXPmets19T4~dCVpQuUpMq5`6Q8UzhwOp-J+f;_V16-h17FA6>sYOs|9J*PUUH&ie6z8a&xP)T+d8>(ii1!{%*S#4MQ)d_hqq8CNg zedeFOWW2{YMA;)EwqueUhPl^)JgS+y4<X5b*->u194MlM*9{`I@(D(@K>fmL*nd-l1cARfAZYSdZ9< z*qqpw*ohcN>`i=+IHYBlCe73c;yB_A;sWAw;(FpX;$Gr0;sxSOJdh^pk;fn>28p^| zhi>hI$%)~_2x25L53vxj7%_%efmn@Lvt7q#?SgfQjfgFY9f+~S-o$~#Vfdjg!I8wV z#7V>%#JR+U#HGYl#P!5@;`WZ-_wFVhARZ&0AzmV0C*CDK@)+X8AW?Vh(!4_`IWe3V z(aC#1l9-2Bh**pmL##loMyyG!OKd=F+7-uMs5P+@F^<@mIFLA;IEFZZIE^^BYnu)& zLko#ZiK~d~iSfkk#NEUL#AC!WUE4S76uLyb>(S&yO$;YS5(^Pyh}DR7yLK&5&}>R< zOYB1IMSPDqj5vxofjEOW4_U}uLR>}MK-@;$O*}+AMZ843Nqm4TY;mGVOiBzVW+vt$ z79y4)mM2z2Mp?Cq^@xp#&53P^orrP7-o*EaLy*zd2;vyx1mZN}Y~ljq65=<+wZx5G zyEW}gt;EC2gdzbn3uSe&F znf!&B=`T$7g7kVyvslFEw-Ubl+d4?_UXp*C2G)mZuue_)KlD1>f6cKXj{HY$cC2py zNt+WZ?SIZP$o&GOyC|r~{&@ZO`R|Vn2I*gbNneOXQQ7oIjWP|D=6ckCC*V2;gfJGt z3b3-QCTq+(vHol%o5mKg^=ubA!LDQA2;&jF058jH^0s^&kLUaNIeu3JL`o4UqD3W9 zSF{vy;sY^O%ofW_J&05UlO&i{%_V}q?1|y)D8dy zI)P$;YFoP#wsqNrpAQ?8aQ`XpCv2yj@~1Y&hIDfiu21(M;rFN4iP{kfw@+U_QM+!U z_6LdDdlR-ZWKPtsmZ;q&VgIroNVq=R?S$>TjS{sRCu%oM*#CTy3D@WAL2ZuJL(*qG zE>P*2Eojmj19M+WtY0GicjJA#%eO~-d+B*w7cSx33cjuB+eAlQcy_{GN7YW)E@md& zzF3EZ?GlOhSF%o`cA~RX@@ArTBCnRxiP~X_+EEGHr4yayn6`f!wfLf3yF~2~3AZmd zGEqD6`WcDZGZVFECu~=khi6oy=bEukp(UF{nX)M^z{apKz+{+ALe)f42-sD1yj?|a z)a&&Id?&!Juq(LTb#~pm>&|WhXLr~g5W1W$2Znx0zl1fNH(t1?D5`j)iYMhM@SPw% zQ{z2b=U}S`6nD$GFS!-mYHoG+Rkx;F%YEIg>o#y3x=r0CZZo%y+sTb{ySwkY1KeS5 zS+}BF$*t^Gam%|^-5PE=_ht7Lx2@aR?c#QIW8HRcH@AoTj@!!Z>GpGbxwYLl+HT-W}BCd?g;=CcX&*{1iV0il60YA;>TBUm(Qq z@q1tkQ<&fgSGb@>8j%KEkx68QFi}Vpg5;vCC<`g<#&%;!X*ac7Kq@=V?g44-_wDx~ zgFW6J4;k$#_7sS)XWBC%lRex10y5hx?bQ%zZ?-o>PJ5rd4|3T@?W2&}K4G7LJoYL3 z6y&up*cTw5eaXH8`RylIh!u7$$AV}llamRGI+0E!cz5*kpKS=D|G>BW*TtRx~ZPZxk_9L5(w@Gv~L7s;->dEVdhx8to& z$6JGr_hmZX-`2cBYhI-_HUGRuZ=)mJPDl779pP_lcF~&Mv}P}@;dp+P_cR{2xOcBI zjECXbPl2OT*p=+cIC|IX^$@^g^0JlyFPk!vgRUG5h4}gRu~+Y5(`7F7W8o}4^k+F( z4j9Plvj#ATwPWpJ2Ou$Fh{-C;fN z#e2bbybtdS8~8vz5H|9`d@yX{WB3?|=b!P?!;!r@7OX-*Wmn!}$Zvbx;I#CY?#RI*ZOC!gMyB zO(fCTb#{?d=hQhxGM!uJ7Rhy9omZsL`E`DgQWw+(MJgSoqeN;Qt)oR6T}&4f;ktw_ zA=2tnx|B$#V|0v2ugmJPB7-ik%ZrSOp#t$ft+s zAtJvXriY0F`Xl|3D5yWyAB#eIlpZAt>oIzah|**ASW!fe)8j<6o}ed)qI!~^B#P-N zdWtBnr|D^;gr1>ih?07yo+(P{*?P7pt>@~wB1X^C^F$fFKraww^+LT+l+%m#Vo_dy zt-ls8>1BGEsGwKq6{4bEsaJ|hdbM6HD(kg+t*D~E)8C1z`g{GosHQjRO`^KqtT&4q zdaK?lUe-V8AH*wqhu$Gx)jRc0QB(h>b5jp~r>C8GaUg=qNR!E^EbtI(JIdl$4rE}?AkXq-_ zc_59>r}II$E}#q0FWVM|bh?Nxf`wdBT@*6t;<`9w)FpLEh|r~VX~?9@=rS0yylj?* zvRPJLSyzT^x}k0ek-C{~2HABB-2!sxR=O4B)a`UT$fY~z4v<@S(w!iWj@7Y{SI6l% z$ftYg9*|%6)qSCW?yvhpK|N3pghG0-9t?%`P(2i)^l&{Kis%t~1Vrmk^e0eMkJh81 znEq6M3dQwj`ZFk@$LsM>Qcu(qF=|iNlcBVps;5GXp01}u8U4Bb9LnlhdKQ$^bMzc2 zufNb=z)O0*o(~oDm-CQF(%uhItpW%d6cMR)PrUq_Zeo28EWqqg4-lcQq1aGQvfV3A>Pt2@)Q@bp zP=B&*H`jBk5&~UPGp^wOR z4}FZbJ8n0UY;5QgvfVEjQ2e=!S~EW-y@TJk4*MGGS&CUG~XlBeUHrWJ@UElky*Y+X8RtQ<9lST?~yNj zkIeTyvcUJqm%c|9`X2ep_sAIRf7j3y^2ki`$h;Ty$YS3kOMH)f?R#XY?~!G`M^^YA z`NkhBD}B$b@;$TK_sqAxXV&5vK^` z!1`hsY+%>e4R)K|WxukA>7j+JAftQ;@L zLpeE7PK5Guikt#3$!T&LRFI#`&!M86C1*h;IY-Wc%JK{O1yqsq<$S0rzm#7>HTjkN z3aZP+axv79U(2uIWw}f)gIDAVxdL95E9FY4DObzYP)n|nYoNAVC)dGi()(5CI&zcT z1h31@ax>JGTjf@GL;fItfO>L=+yQUOU2+%Hmv`k|cuU@w_o0D&ARj=(fD>?_k(tfR z25+0$&Fs+F%xUI?CT4CkH#9Z#nt7p_ncvJ0&CP;lL1bvxr#)TA4-7qR`qb zZWf0&W=XRov^7hcrQsd3j9CWSnJ<|yL3^{7SqnPY_3V1k(dp`Rg-%X4ryF#3x;x#W zi__ET30+A;#FB>SMj9fHG(>mO5Ismk^dt?@i!{W$q#=5fhUh~YqAzKPexxD#lZF^T z8sa_D5bu+Q7)Tmo5NU`HNJ9)J4Kai?#8A=@!$?DXNE%`|X^4+VLyRB|@iA$Lk)$C$ zAq_E#G{k7q5MxL~d`cQ(ENO_(NJESx4KbcH#01h16G=l%A`LN_G{h9r5K~D*Od}03 zoixM@(h#4MhL}kjViswL*`y)nkcOB`8sZDm5c5bw%qI=8fHcIHq#+iPhWLs!#3IrV zi%CN)Ar0|0X^5qyA(oMbSWX&Z1!;(HNJFe74Y7(e#A?zI-;#z{LmFZ&X^3^CA=ZwFSRlR<8^tCOFE)!UVyoCDeh}Nmj^|~2&vRmyn1y?sL;ttud@&z$ z{X&5?JZA0h@%v7R%fIh^7H%~k*RB$)#kXROSS!|v_2N6RL45zb`hxa8Tg=A2&lPiV z?+e5N-1}Dol>r{%4&1xf4{z`JAo+9m`ka1{&rbP1^FH@CD;xTpSBZH4A?XCZ4c>== zFbF9Jy*Uvk0bY496{f*-tUf=7 znJ^1x!yK3kUtldd9~Quuun@k2MX(qv)30GEEQ95+0=|KjSf{RrZ($9rg>|qVNBai& z9yY=zh=MqU_TsygIM1lh9htkj=^y_0VlEA zJq>5zES!V$Z~-o2?Ry!1fh%wouEBM+ zSc})@ukkwkbzYah!Rzrid42vCZ@?S!M*MBwm^a}~c{ARex8N;#E8d#7;cfXlyd7`P zJMfOY6YtEY@dNxIKg197Bm5{o#*gz8{3JicPxCYUEI-H3^9%eUzr-)|Uxdl8@TgTx17uox}Ii%DXUSS-F4OT{;0rT9_o6uZPvVz<~M_KKgyKCxdM z5C_E}aabG?N5wI5T$~Um|L%51Nu!jp+t_35HGVes8T*X`#zEtdao9Lw95s#^$Bh%l zN#m4p+BjpJHO?95jSI#_-SI>Lpb{RaBK!WmQF0Rn=5=RYSe3UQw^AnyQwntzJ`g)a$CQdPCJy zZ>sw0E!99ZRE^Zzs^^R(%+N%z#qw1tOt1hamidEfIoa(N6 zsGh19F1=MB)mQaX{nY^Vo_b#mRD;w9YOorjhN@xeLp5A|q(-QZ)kyV;8l^_7G3rw_ zR(+<%sqt!pny4nJ$!dz4s-~&wYKHn;%~bQ%0`;X@sJ>E*)MB+neXW+NWoo%vp}tWo z)he}GeXG`}wQ8MOuf9_o)c0zm+N9#uX0=6aRom1LYP;H@epEZvF7=bzt@fzB>Swi2 z?NZ-b?uB#jBrn;|w zRS(od^+-KdPt?;O1euzpW~(`BuKGgFQ@7M@bw}M*_skwDCPEb8DtG%bIP?vF2J|So5s;)|b{o>nm%Kwb)u>eQhnZmRakp z_11UR2J3rkqqWJ3x3*b7SUapAt)12`>nCfswa40P{cP>C4p=9x)7Dw*ymisKY+bW% zShuX()*b7vbV?FaTl`;qO!u|PFJ{6|Z-QF3Z8miHK z;$>8$2Sk1G790|d#M^L$^6xRxNpynam?HA7zbxbhn}NMcb+oc1U<|_KZOYTbs0r+^nqxYg}3rMr)EJ zSd*I8q@gw8w1%TfEWit3!Y#(V|HGjqFA1Di;*~(~s=O)~{8j!cNOQC~8Up4Da|I~# zx_KS<$K_Hy+y9vioiIQD|CpVhpP&E8(0|R*|1(+I%fo-Oiu;e|)1<1oYN=YQcQHG8 zS+yeO(;AfNYGKwIK{>P@=1(tkKFgaeo|7@FJ~wCf{y${PzvjyEm?i(nkAKcR|F7lB z-*X{m&2gCB|C}?+VQu17CeP{;uPUjDb;+-ORq_yPk~gs)nL-uGRIEoH;OMQ3Blc}? zbpDoEdHaM})ef_h*h%eVc5*v~ozhNar?%7B;dWX(ot@s!U}v-=>`ZoMJByvw&Spp2 z+3g&5PCM6gbMc_(XJZ}Grndf8PG0cWtgKxfrjz_7H*dDKSX(JWZ+~u%-j7-Opmo?f zY90SATkDiM6=m!z|1xJk{%h9OsdXA1ZrA^}vUh^qZ9o1`XK#%5)fs0gFn5Qu9FUyV zWOX15JwKL%HDWCwH*3XOL1EU0wSg%14r>oZSVz_iim~48Q>efuu*uMfEyKLuly~8= z(3i*YJ}`jyAD9|g$_fXT2jW@Pz?Q%+RzI*Wu#Yth91I+0%>zdR$5|_^3Ldkzfu|~p z4OY>rBwM3Ot1|5SV9sDJ79Y$L%+Iz23kD0a?ZN0^G}{p@9xTp&^q%%$JA-9|W!bJ^ z`CxhWQ?OF765AcD8m!9p1ZxCqu)V=L!8+{c;2Xg=*uG%>V12ef_;#=vI}mIWY{QO* zjF7>OnN7`R?6}#&Y|T!ZZOvGA){Hab*mcaU6WI;St?SqW%&qr126yW%Zd(nlUc99@ z%f<)VQ|(iHkn@o@gQS#5GUGQ}gsZ zf=9Z!L?t(`Tfi;m=5dR<`Q3bO5x0~MBKdgqvP3^F(;oD-1IIpv&!2N6}OefRJ3^JpPkeOs=nMG!m*<_^5E_2A7GMCIP^T@n1pUf`{$bzzv zEX?+>z3gYUkL_m%*g#ZZzeA(G{+?ww8kYlbcI+*0X?A?q=J69 zr15^L1;Uxj!XPb1nQ%zYGO!Ggfu2>(h!LkIL}0|J51Hucs>~E?vaoin9b~1aSF^Eh zEDj=BAJzw>(oi-Oa!)#>fFbJB%&5>Z4v&}gWW`1EVfTZR=b3dds51L0HwRzk;jmn_6 z)fX~h1{?@kz3c`#F}qEN++Hq%{FsaOK|#zwN1+I2q2myZdFV70#awh2ilKTu55+Mb zU4#;tlP*I^%u84B9C=T*m*fq8@5?s7FW%LVAwge?^WwOz*CWAeqDnKUN0NFJz*@CF zUZ-Baf>?ugh7k43q<%TnucXwkkUQhE; zPYY5{qp7FGFo(}VwKE4bQyJ=YMe22B%&m8!I%d|#sI$BtRpo8{r=V)$_1S@XR0QAQ ztO4`>Z`~Nbmqq-oa^aO5$EvU@f7-G!D+Is&)t~nshy2g+M=4$k{o#4Vpz|!rHv#gE zBHskbHzBO!8)Dq@JY>X_??SR%W^2 zJl0qiT*F!^3EaerB^>V16X^x8Kj7(OC~x+#7}T=fwJXA5&2@SfrLn8H)bn3}J*Qi_0Hd*2Z`LI78ljY~f`;JO<2uI07;I;h4r~ zO|uHmjx3Y+j5ieUp5=!5cue4WAto$LJOQ@5EpHWD1H_K+%d(v=S7p42o8g zqE&#Rm7-`Bq-YhQXk}8gvM5^F6s~G7)7h36s?j`v`S9VDg{NWR1~ee{|jur zm%I5ZP!4tUaMa3^P!BIa)%z{RsO_j;$BJ=>AJN_K)w4pU>c+LZ}3=cokF=B~T+^Rh9r# zoq%1<1=0rtS@wZB%LDVYAM*L=0mv7im5}?Q2O(dGRzdEMRzn_u)+)=PE~;X1XU68k5nbdKT(w-AEl~rIgncw@)>9$)Z%Wazapr= zVyM3osJ~LExiYA^a;VFnp#Jti{Z&By?d58PDgx(S6wbRCoOf|J?-Fp{CE>hF!FiX4 z^De`kcNlF0%LUwxd$tmw+hl-)c3<|}P%&s-;?OE2xZlIw$3rE@UxjjQ2foS~Z_9U?wo8i;(8Mrxafm`Bi+zPj5;0%5S zgCWEaW{5D}GTt%XGo%?Z4DRzm~c@86@Ji|Q0Ag<&3DRJnh6lmSFK12h0D;7N+ z`1PsuO5oAi^jpBAJ3x=_2tB$J^ytpeqq{(l?g~A+8}#Vz(4%`mk3JiEbWiBfy`V>* zL;nPPxi>BV{I@^!-z%a2PGX1x|Gfstc@mlbS2X(H)8OBtC#WGX=99E{VEpty`hGZ@ z(r`9ap!_;eej_NqE0o^@#xGtl2ADf`=S!hcxc{RNL+lam-LWMiH*qa|Yteshn`3qa zdJu$GgFvexL2dIu-7=tVnNYU^P`83mw?a_2!cey&P`9E`w_;GY;!w8|P`8p$w^Bfd z;)wL;(bFl638QC#xb*Yt<(?*hHj6-;MWM}N&}K=WKZ{eC6Fw^eVv})1__FG`y^O60 zXw?*G)x6NEfhPg}!TAT-uz}1VW+*d^8NrNXMvZ62u3)ZYu3{!LQUilg2YP7v|=Q6k~$(q z(j;jj(j+~S9wGztYGp}ABx6L5G=*e_D8L+BRgx9S3Q>cZw(2m`b`s3A)rOh2dN9+L zfSI-|m}#pIGi?p-iBX5Oz{r<1~MIH-CDq`TT7UA%Z6FERxs<< z8fM+jgju%^FzeP4X5BiCWiMj~!C2Pd@nAlF2IC391Mwg{0XvK};7h@13b7+#c6#uB zemedM%*~VW-N;ey69j$)^r%LnVZ$UQeuE?l#(WMxLgMBJ@4+kZz4%d*5=j|v1S9Mr zso?j(teA7}7J!umAPCS^2%-wCVKJfxY+*T~2{gAFF#?v5j2Hv`uSZPA_EFH6Al4iK z0Sym^V}h6%CXFd$8l;)yr$6Dz&;R4+FaMKY_@C^X|B37y3`B<%G?*gH7y*&wjK7p! zNaIHJJikuJ1*39Lu6;h^_AgJYjh|`*ePCwTg7ICWv6flDuYK8d!O%CzTX4T^2<`!= zVQkRmjJaZ-m^bEw1p*Hk4SeZJ?la==5%;<9L;}5hO(uUN?o;H6#C_)cU1EPjHryxD zUy<`4iTiYVA^}_ab zL|>ESH>C76eU@-+#IT3|?2Y|kZ_E^t1`+1BWcCM|{w@8u6FOz(rsy zA{9=gNCoroX{Onw4fCVCSW$|U7ied{1}?9Dh2&KR#NN0>+gHhe?`E z<`KqbyR(D|L2iXAO!M-K2@Q)3iH(Y62@ni!Bd;*8TToy`RAeAaj*#KD(1b-D!vZ)_ zF;T&>3hXFObQH%gHVk}1nNZ|jCkabUydD^&;2std33^pqQn6BHC*RipdY?+j+s@rREXC#`BZ+}dvR6sNVu4K>u+^_4{lwOX3D+p zz34D`@pN0kN0^qc$TBkTZ#T_0+zDCfHQn%OL>6kECABW6hne7zanz(>WFxlajuNx) ziqYN%W9uuLA9Avq7cMac!@u=Z$p@#Kev`FmQUKGY{)r;mt1;z!$#^?bh z149I62rST-Fwrq!>F5(ILKnP=nEPeSC`xuB90>a_bplJ9{`o5s5)!`m6+xVT*>~(W z%Z|jg9A>xq%=9f+S;nPSCSkSVS{++PLQ@aUX_Uh^dA{Fyvn^NMp+9wFVwiUC@thfb z(ZRXdM36@1*|r0}F%v67n!;W)b05%{O@le{8nsGS49<>jm|1zS_G6RkljrI#C!b2b zl?n_mx-afs_44vSaz#de%{3982jLO1Zh^ORwrl!?Es}pm*(vpFf6%^kY0+${>t(Jv zA7^L34anLpg4YzTwkMCa1Xt3k4wiKXynpeerLOkrx;tk~Du0f-wlwg`fa1JG({5NP zy-s#3*J5jIz>77Pq*z|qFTJXxd{5zG&=8?jUgLEnK5V=8GLTbLsDDhK;qtT<@}5Og%`uiW;`lokNVpd*B`Zd zf7iS@)niqE(H?EKa=x3qzTxXnd7ito0uq)Jg*1ayj&z%SYrE>jJM#r~&sAr>+{CuC zr5swOK<|6?y!q(sy3UIxn?@A(nb9Ir#jmtIN+X>#SaV4Abo<$hnQy&maYt8k?|!|A2% zpk4FOS74<)`Ry$?n`qiI^O5+GI<4v!!#XD6F$Puust5p8Mb>HTe+#U}E_1aAl?bd7 zLe-oJPz8XBu$ur?rhg5pzT0!`_Yj==J@kqhU8%e4Vph7=ij@x*h%R{?>i>fJGu!jD zkG)Q4=Q}&WEw2O%QU`=OJc{D>niPAljvGAt+vHAKRoVY9W~eCxff8|zkZA_UaW%<7CPsSv7* zFjNkCuYE^V=g9mTIm#pUr0E5e_=K0L-4t8Ni0+xG#kcKeQEe0rE8>hC4%bol-94;z z>x5?b$Ge8&?KALmT|AeawyOjv&~;S~`m(KrmicrU-zl@M+cH(iYvlvmv%%Ld7ua~d zcHphKhwuC4%`c`U_YQTum03leEwD)9^_GjSd$PeYq8tP0RrUqF4Ai6`da=GmFJA}? zG`_8B8QX@m15rsa63_)pk`Vu@k#Ag8EFD5?Y#poozK-3Zq5wz$-Gv2*1^C4VDOkkC zhDLG1ViO@s5rl~WOJAQg#XujRl>X)+^E<80|h~>7@J%2f1VDMayVAs4(;Oi_im(v$D#M zC(c@uZ_9i8qw3vVap{-$#aOMrnSAHf@gbwK^FG$UR#lqan-rQWwWpjDdVLH68z#)@F|mP?0w_ z#f4YrWvaRNFWYB7WOLcr)TFRJ!KYljFzbTgCQsAm1AM-u>tCQ|0hp1w1e3e;gt>tb zg&+Yo0eik12hm~R#EqKJG(w1n@AK`1C`zWlj)6NEZ^O8~e7wqXx~{f9`~HrF(<)d| zKbaou)FC9t-z0*OaXA{|j>Lgm5H@1*6}T}Dq%1V|RL|{G75<=kpXQ$J-CIVujDg!s z!j`ZuVi#GY&G_r!Hh!4{Oq+{J5ZF8>0Gl0QLs(4!Hj{q`*tiRZJ=W_Vqc03ayr)cG zt!7j8G|Ie2|4{f-hF)aF%;BfLanBs4>D*+4d+iI4by%g!?JHb%Bq`26V5aAAw4}nb z;6Zd_L;c&tLo+$U)1O$ZZo7|*hg~QyP|$f#cRB5OQRjjEwPVqbD)>uC<(~H%*3a@9 z%C;>L+9po6=8}kf zskbsO+;4=mD~+^ zd$hMi`$Vgj03&GQ&O5)AycbgA3o_aMa+$n+mv)@v8%$+nI!7>soy=Bf*cEWDzFOSYJP=*#wbg6o_N*dlJJQ_Y z%cUW-*m9$1I(p)*4>`s{ucK;ALsH(k)n*xp1<5hicL{U{zK*(N({Ww=apGxm{q+&; zyYlM`570)0)y*q=-`(H4+NP0rp>5DYbEj&{0jFnm@rj+Z$$SwqNi6vuhDTRl@kpPo zKxN>LQ5P}Y6-^YyWj)yz>R~P0Hf5jB>daR>$nR6|qU2y&=o&m+r!jsJBAZh=B)0sG z*cz21>6gPRY*~7Fzdl%Eb`x3UZ+rDp`uPTl_Y6)}bE#PsW*$BomY>rjP$5un>_WZK zVn(F!@B;jP{sq6qLMKD~miZ^}n=l4pB|vOL17Zpnzge(qKya)7T=@GV^xj>(xaRI1 zyRDil7U@dfKi>1;%uZKjm&!|B5>6`2!K-_&I#k9I3W860fAPo`ow;4wa_hkzK7`tB zWKsX};{zGI%wYz3$BT@1`3nXr>vj#j4w2FRu>4WF?2|`MrNzz4?rmA`tuOOkSx|MQ z+LB!I?x)4uLT*m_)yBO#?MmMy8(sASY0k6V@LrPkNciT>MC7_x-o&o=t8V1h^(*GC zdV5Xy74@)tgj>D!=G}IPy=}0dx<+tCZtr!TWc!kLYxfA+itwfEUi)nJ(oblftP3?2 z5g=@y9qv}PX*{jtvAarcsRb*cJ^${sHQS2)utT!^H6MoaYtc(evph!MQCdzZ(7&Mf zelR0@{uO$EH!%DPy#*$sH)uhKT9=%IVYj^eR-xa-m@qsB0 z9d8nw_DL-B*LYr%uQ8b?LCXKUnOuZ?$_x5A?Z(UOy1sVmh)G^>03ToZ1VQ5^4AA6YE?Q`0`%7eY}V z*Xu8?{dCv0HBx2R^Go32$O%1(@eTS;s0_vA~P&zDhT@GxoE?>I8t|%u? z>ekkE!f{G^`W4j3qIsFBO-0YwwkhnjWl$x+k|v7NcmoI5#t-f;jk_J(-QBIRrg3ZB zp*gs_ySux)d*lA%-n}zB6K{53OvJ|fwHe=!tgouR%&4r$-1MY9`XT7xc-UdYPD2~i zS<6v=Z?vBv(2}BwpBKe;`-7LLAd+wE3~O>ai=pwj-SI?H9kJz?)HfVs8kvBDhpB16xf1J8hFUtkN8;wq*VG?2XH|Kf>P#?N)g!BEzS*ce9xWx z(;{%MD@6XlfoQSqU$NWtioJPKY*rY%VZXjXet{4FNietm$yOBLNg6K2UK||YE_6)` zkfv9c1wGFV;Ah|xVfEfbqROj24VL^?fmk1Dxu38YVrw^OFv|g{j(Z1=%G!@~(<1vU z8Thv=h<7dmBUKaF?d-<1Hl$*#r7T)%R{-~rtO6buHE$e z*SmAF&N?pG;2g9y3rJM0)QHk`t*cJHM~q-3KK~AG1DfgcGSr_o$4hOQ+*=GeJi}7H zO3&UGS01ofa7;|NE$$;{PjYhKum>OBF8bb;U&2-)i6sAxG#Gyf%azw&wsiTB({MRE zm|&Psn7~%f;PM#sNH_Oew_Q~r7BJq|pfUaZyU_7C^|VuTSl%Ym=WaFxy@&Qa>~Ew) zOgY*T;rlWT6V+_ixwlnoVB{hP%DE;XpQ3yPH*5kmi1}p8t-nvE$8lOLSUv3}Rvgm6 zZd{&u;Q3d-m~8&%$5$BGEgrw%UgKVQc1-A>X2#s&Tb`FB$TI?qD$+h*^nw7=)D#g; z>AbVg9i4$zmErx?GLEQ(FDgH~&_f)n~fESYe6m-ODADPa4VHx|Op^P_X9JwB>2ULx8XT z;Xw;x3j;=C>u>QDk%%vU4b~-@lTi@O>`h%dxTcaSdbCWWY{?p^jDBqs5%OU|a| z0%Vr1u8zD+OdcK{j2>)^4$c-#EId3sOw6oItgH;qrsfPTUiPj=o(%Rb82_0VqltqZlc$j*6AL3V(?5im{&Dj^ zDE%wD|G@mOv_V!DmaZ=U`v}heJoC>$o_7Db!7Fd%Y-46BsOs!y#?SQsqW_DGNdJGZ z{~P4LW&iiN{#*8cpX^^3I5lUfagC<0tqWy zS2JfO30osqGchw02UD|u8v$(W?Ei5BcxL*>kJ$$?e-(fH5u9>D)*FJ0f$;4BSaU2| z@DA}a6i|XY_8ZpUz$YZ%8oL2z(dckemksrCAb0v=xR+~fi889(s-c$oPD2NpudiSs zFf7gb%!%s9lAhVm>F_Zt+*~@>Ie4wXUJo2X3BH;E^?M6X#9ocE?#7p1?3~wGCohge z0^VuzJ>hIN*l4;t-L#ZaXl$^p77dQWkktu$7amChB=$EXR+UFYIF$uMI^RIjJ^KTC zzLq5TKMSI|j`3L;FJ-l<#BHvVzE3Q}b#@%&^X%L&`)AhO>u)Zo-*jEXR+6e$WX z(%y8k70ipU;+)sxiOI_TiMyG)at;_3K{@iv;2OZ7@Y4vHE*G>ANK_}UaTt!==G^Gi*wA%cKo*F9NI^P zgo0$+5&z2)aWMaf68+npCgoruWhMpvZ%W3)%FW61ulli&vhZ+lvvB_B8lL}v_e@k< zec-v8mm3+UV6#dvrNWz)BK@r-5{I0xL`G>wiU5Owkq#1-{CvjPFb8F%pL)Y zHMWX$T50Fx=COix-b$Cc-!{d6g^*h=^m=js`FinrK=HW#=%wH_b(_g*F;2P{#e$<> zZ!6<;@KobadBznD^w;ds?2U+L8BpVj^0fz=7{K5~@~E;Nj)N5?uEI^v_fqiK2nls` zK`%!~{)TeTcOrxA2GLopEzZJoVYhgE-fVkZI)mzPgO==CLTm>wuefyYj?QNGn^PqI z*9<0aO`g`id*G0QP_!RhX*J`|xZiUI)#%!LG9EhmH0AkBTKijMj~HG~io&r6of$3Y z$&B>-{ua>3JzxJwWP5E7wmJIyEJ=Wy(UMaG);XH8vgEz9+Jw9lf<6#E>@)P8{vG?K zzW+LIP;V^1)Z5R;vJEvj3!f=akT;g>NmFlweLm@PqeW ztUUklsxInENR@HkQaVgfCsAX@enNK*^DnB$JGH^JR?D=M;T_zSEs?@Mck$vroXRo353sD_v%1nB*ZZl~06Ri9ob4RbfG$5K1lKDZhR*pB$ z`MGua>2K3}bnnzH`kjp#4fpzrSmoceH|I0F|LeskQ@n&Y**oRWM{!gc%)a`}PPId` zRY!R`Su-u$uLP5Wb@-f-42EaRs1JXZE~BqbK{lRRaAU&JEak`?_{%HS(cJUfdP1eK za)@2612!6Ye|wMWgh=u}h$zqn?jVoCuUc!g-PNb!tf?$3mu%;3=QGO?EYmp_(u31Q z)BDn^*bQttjZm=%cBcge__JSd6V8HYQi>AvQVjDQ=JO zCn)YTw8t>*%Rb3;MmTJz+)TVV`(bs(T^;&7*S(2ab^NfJz%gN z&caD<^VUkLg^b*46^C?l+(i_ry~q4Ewul9UbIt{9KdC=IEvP4eat-MToerMqB_0Ql zsa~JUg<5Q9=5kdPC8(-u>WPYJ{8qbZW|)Jq1UlLvU3T;OF2{L1a!F(ob!OM$?e&j$ zXV$s7mnV>blS1f*IY$Or6kLZwnKJhn@Fr=|C+Or}*sj%Kz1ffK$|fK;H#Ar9IJo5Q ziC5Ucw>kasDcx^A(?>*JXyMLdY2W=yinG=rrm6o3rBna>w()lLuk|s*KqtP#wWFpY zvW=ZLw~06vB`iZbrPFO>hp_$?1443T`W6Ue5cRP-UPxvzL#Wb4xM^Z04P7~>vQpHS$Zu~brB#%6(9X(Txx@;BZy44)D#sQ0YL8?!JvNXz=NpeWrP&{g?m9(WL0670KOjSe9wkq@Fr z0ekcFQ|eW@CQ!UjH{>;QTsjsm6aRF8_?cc?>jdYUk;$3}Zy zMAK(LAmM>97_x4?)D9sTQFsU0lrXsfy9h z!{-Gn%N=oRBAnfD=ayrFW5j;N+0b?t14Uuf8C+!)ES0nX{|Q%u4$GZ3m3D^;C#$V0JI%;aQHg zK(Kz+p9j{&V=BX+V}iGsO64T!s&Ai|VyY;w@LP}ZsFN)#6-CsDI31d=zZcfJ-_sv_ zGb$r1@1H^+(=nGq?#xzLUq~r}-AH5fk7+ zr1mU$BOtiPg!?D?WPY+Y$s~}z*!u9W9>4*Xb@0*9P;(nSyX@ltYFe`etezT*aZdW_ zt-{O`&?%waE^ikO!m*%Y7h~I@SnUzmRNI<`7PAWC1`?Ca6D#GhuS?KNvRn7B8$$J6 zaDBsESH&zJ!A7glSHjevd>|z_B8agn5_CJ+6>i~RQ0{EM-t1RrF3=;Rfif{kS}=7Y zj|^Q<`i+bo)jksn9`t4cSC~a7X`Z?!r>~o)1(7Rf2vQ>_=rEY}GRUW|SR+c@NrpaQ zt*#r`O6#TP7KXr)OzMHvyX!85*(4G-$rG)i#o)#F1~jjI5JG561?gIW4i;FQ9Nb#{ zfBFRQlnCn!qaC_;6fY*KpAbz=^0>*ztw~;ot-ogLeD#pmR`Xfv=xhKAfVv2CJYBvb zCA5$X^_p`qP~-oct9w1bRjk+UJ50vTyPrp)(U-8{Ldt7`xx$YMCS%`pKj@Vt+kJuw zL(1nih`7yPrWj}e*(MqX0nQ4zB_rHGSvBgc*oF$v7MkS)fTV zAOf6B-lYd(OR*S4c!9=~jAH=uAa)rRod`JKX+bLGfE@5NKlR&yE6}>2I1a2GVFi*) zA|n+~0wjS{rIZ;506;2P7U_s^P$p134S<=C%QV0Z9F$R}6%PjtfB@1g5)m_?rvjQp z<3xak6bmQ<9fX`>oC<&iAtxD!0fGu}Da1PgULfQ|;{ZSl=q?4E1K3q;?X>)FJ@Dc93VjR5F=#gcxugL?MOFB%TSV0rgAm#PjBCj?)++5 zqXaS#Vi^D%I1Wn95BuFq6D*9_0pJDJgDmrv^0z_EYS)ZF-h$)byEwoY87HxboPwt| zfPca+HPA0{7arJ|vdahTOxh&?b|&xA0a0b0q#~FKp2`6Jal3y2{>i(Fpf}|Rru-+} z2qCGPGyr`5QwJbN<|Z6KB6|}DAd$J@1NzYoz=PVE27Ey0DZ6^W&A44V;AZ@;JaF^( zt{YH4VOI^PAGgZ|YNsEN1+~)*Ac4*kcD;bv3As>)%R&`e{l4=*!ln1;FA4z zC8WFQ-`xJ`==Hht_oMINJ#_39E#;Af@&Dk)|IBD@=Wk=*fnDg>8fr=t5u*d2?xuTl z`-P*|+KzbJukH$-ME=nrJ{Ou5Sl%4xqu}_^AWr2=U2t@+DUv1h!6IgsXnl;O9gP*c zr94f^m-=)|tQi?v9pRP(u?bUMY#DrLZJGLCmMm&oV)eg|&y|c3j_JX$Vz=Rk=o2WE z*g=O%mEpy4mYmAa$^tX=;I#S57-zPyLd*q>g|BLu^5p3<0Mb&jQi@ViTIzHW_B2b3 z88I+h_-Gi_uk}6Qz3{={bwM?Gi z>(}QnP{|p9jH%)FDB0ktgT_0rcqQ8WX?)yS;G>8 zQe<>-!K341RNybPY#4Z9cezhMQazeZ!hFiv-^#g!)W$};dSY2SwCzF8U0;bPc}2{G zR4%W?{K;R!kB5|Wv5rHPbSaO0Mtvpk7-qOsA2<#=$#~^l!$vzPUg+>XMO=R$Pl@@1 zY5nupaS~rMo&8B)qK+Yw-m{NYSp7uqSY{-}NI*$j%9l-IB)^P{0yohH6kt0f>vN*6 zFgirmsSMr_i~V|mH~{!5lAC(+hZ9UX(W((EA?AU;A?*r3b0NkTW}i1{U!ZUQtu<}bo~$*_l5YlOe%pK0j=VL- zk}gS4wnJCcl_N<{+?8?$OH3z>u_!$QxFq6=H&Y;%@>P$}nInJ^vBf-yk)oA!KOKoT z=eYeh9mFcYd58J%C`4EEJ3%<7((V2HB_Y`Dn+B#O1BZd=|M-qzEU~eYGjr?CC`+s1 zg)iD7Sqz2gkzv>vS@N#fGf>uCIhMrHwu~G(qVr-L1mWhG9A6hOC&<#J6_#d(&A-Z1 z7ul4Y;d?7_%&_BMs(LF0c$fT-ABo2E{s;K2$g_93ta){!PV zpOgNWN4@{mOvS=CytcSb4vY+T1=s%;6)QoDi+zzR5L78x4q>^ZW9IVdgLX!PG6hJ=L8S0d(CqVzs07}kfisaQKbzJgy=|i7rizv9rU0%$E~O7%+bS&_Fa2OY*cYy8CY`Cj z?WTI$UNgK~w?EyoEl5@cV?VN?vti%#d~RNlui38?&C-sfW$P?r&03BS#t^6WzVlt< zp2tG9DL;G}b??E3S`8q*uJ)=p`gY4Z6?Ax?opp$kt$r&p1>ODL?c3eaEzrGj=$GBr z?E&TSXX0X;=&(+pNq7s<7s-+BPX72z;;wR4&I`xW|xM zlp|Zqjq4dW|HRFD!D>cFg|pV~)$Y^o^u>ih>cq$u-2<}E;qKDKB*+Vyqfsn64x#e> z3OcVF89_rzjb)ivnJbxSGx0w>)$I*D-RVW_4%VogHj*$IXzX<5X&I|+50}>N)G0sT z&oU|=X3YXA0}Co-p(2kM5i^4!8EH~+Qnz@Sn4A6pnTr~L)0)hD(@IJtscK3%sVmwz zsme+Oshipa#lT`zInnnLShc|KHfn!r|L`l#XcMREG6R_r9>QVG0@-xNpfC{!VF#OH zkfIo9P!hYnk}VbQsI1S8ArdfHFb)q*;F{krp)VcQwGUBSA6q$;8(|cG#+yYRi6`tm z$3MC3RYff=?`}q^yg6y{6f`b#nfQ~ux|MUTl4LA&B@Qf`w6ox+Tr-#(t68NYMTWzp zqQ3I745sK?yEtWx?SW+Y87B$h3Aeb}D=p3)RY|=uCQX`2PFd zxdohML#1IUjAc}V4#DDgsYYoaHETAXahX5U{43>86T#vIelA^*1#*2InQZp(R#C=) z^#kBQ=O$L48{yTFom$e$irIyPx-vGqLK^XY3fvs+Kv`EiXK@$%FjT`_*&25jxGDbkMHe80^Ysrs(}R*rn6^-x7s3YXqMXcf#}`E;-TIfwb_fwnCq)WC8>88o9e z8eA^j0>MqfjkXMviDV1I32_$a65!UYZHQ8huL)ZYqYg0^Xx0rfM5xBqgrSF-gqwuI zhPVy%>ee+xorYnDstbhsL!1gb4s#lS*X{ZTyBM-G(5hRN6haj;={u|vsvIO0bV5LQ zciMM6W=LGLDCj|`g6{}Q$N)%_fSCXi8W;&k3kdQ+^lnE(TqOu(*bs=IK;iF#zu}Q# z;2>JQ`~M;N4Q&eT8%R%zISery$o>ay8yXY{=tlAkFeOEhf?59#^%bojvgSMTHdN+! z_OGzq5PXKvWRP@*kmCUeVGvOPkkin--}zdRs_?%ZtCXhUh3B9HroDrO?j=c{jVkzAOvLqkpt}yAqb5M(FHFED+nnF zCkQ17;|KqS{RH7j;)%A5){fK;-Hy@@+m6r<*^b%{*N)f@^#osXLrX?&f#3lSC^d;;i z(8tm8C*f<3$*0Tzn#_RAge0_p7{cpz zf^+;o>i&C7!J*BgpH_iCTzclzOMvo0f&H?FPc$6FIGYv-6_blX}3 z&rYk&1Y{P2Z*2mi&vnPgz0TjNc(mV?j9##brNLGCM6hm2?H47O}-gttFn+y0WA zG7KG5_D(IF!lM~C5yiw&CDbBD;H2i;#^|0E_ae5$yNS(JEU{9&Ukpa=c>ETCNu+M}`74@jR! zng}{J>qCfZMil%lc!+*biS+6}zRE`1WuA1?+#+dek9hQpcjhJbj?*2(-;TZFwU#(x*>`RqHaaGi4*R;~m1rjPJ%p?oqWLtp>i24ad}u zzTdq@k~tx^c1Y7_`NPJnsP+nUk!wG;caG2qPq%N?h(kT$@)Va`WauN&aEY`=^GWDK z?4=y(%v77=DALiZZ&p>qwSqLtqHgy>Fa+hO&7&1`?J0Sa-vXk}@ZN{Z0jJ#xMMxGUE<$;f)7So?)z$#I(Vyp|qQ)p~DVA7|Yk zTj;>OqE+yPwoCm6x(T}&4}IDI8!w|fWrQGx9^+}53E6I=%G(}=7;~Cg9n?duP!FcX z5vG89hG)$#<>XJCDm0Z!^{z4qP@}GGB4f9sY_UfkS1#L-a7TWvD^`wbwrNqI=G)dzpC@k1~) zwffiuG$sI>K#&rB^@E0!jxM>VYI!vkCqba~Y8#I-EsF>NwgFS*HG(#zUr~m@$NiZ= zQbHA{;P${`ybO0?>0o3!J7;I4M4PL|e_{D2&D?gx0OFK@i=(kcmP7x3jADpJ?42t% z1!{$1CWotUUc+&G4ank3#+1Bvq1d!c8IlA;8PcB;9A6P$t~>rg|{fGuv} z`eKpA=C@a-p-{g5@X!Klv5?#v%^SUF$tiItug~dqnp~J0L{OD3=;ZZGn$eTL#BUxO z8R2Sf)?9Y^@{qjd@8uaXwzfosm-Y2PE-Getc!H+oY32Ti-|h1FZr!zm94|w$yq2zJ z6r37b@j#7Lcx_qZ3?3K_ckewqrr{f3E1{s>d-XFFP`It$6ab0;lBcu$et z=J8I8+2)lk>giDZ#l-5lRwH|;>s|7;T7g+t@<$E@w|B0-?N8*t8u+2-3$OI=-Nw>) z)a~c1japjV-R?t=w0NuvUO|M^QWM@4Bi855>Yl~p9NN*mMEbn<{HFC1mJR6B2>|>0 z^m>!Lb!Caai?|2w5-mi7lXYb0;WNqz?xIKE^ZJM$+RnlX5&bNx@$5n@7q0#jo;udj z`{NUk9qlw+UE4deEG}gjV$rI5MegS(HjZ!?FNlOj@Vvk>9vo_pImeu&KyzQ1G82VHQv4tMK1$ue6wIc4JLO_-o zY-)AOOwC-UUE4@DtzSQJoQxmJ&(n}EC$25#Ei74WOFuam{4-h;>`oOLOFq{6x&FAo z-v~QhtlvplpH{Cc`!pk5nNVe8;j5q69`V?FUdkt`@t_7-vx~ zEZ$)==R#R`Kb|acdnDoXHD{J#(t$^?tYNGCj`boU4*s*?E;XMa8Uaoi3snUBaRaH59pC$yM=4cf7KFm z#58$L6~CJxdof4%tqbea%0x52Ax0m2V{ET^${UE>2%Nh0ORUqpAvshgeNs*wc&O+m zkc!!ATEg_@*gBe-D+@6byjvR)Idma^C}?_J5u%b`~-U6N(& zTG&tNnKc-rr*va{fR|O`bOTh#V`XW_&c*oZOV1OjjJI%;IlqclaLNrnE>M5Osgiip zwH-AHu1PQk4sl70jb=6{3oysU~5Q-M~qxnR(nhhRf~98 zlD~KsI29IIT&w)bnS&iWm?z+l4WD`2*p_p9bv8!M@>R%${)pHl&WQiL9nGnk$4mE2 zfGP^+hPZc6aF!lgz0BKQ*5xk)NN6H!6Cp4DT8^oVA;1JL)O8|nQdc|0vf=QYfwJ66 zCRl)d=I zT=v)6l1!HITArU{-u4#3v69xmMz=fnCOy#;HM5xx`3OmT8)r_!W97p+X|ta3>*z*K zWZct}CTj@!7z9xhGWW8th^DC-e^xEtL{1=N-#v+Qdek2*$7g%Emi3rUTRJ86zM zy=Qm&(j%l@8WdVxN?#H<#Dpn0mr$=rWCdmQFs>#U)$uGChOGQ!$Zdrg@-7(i_Vvhd z-e2WV|4hTNz+@^;wP^YhOzQ=X$fCqiO?_Eot-smzRVx4O_#}3%5TR$^*=kAyS|;dB z#a#mKCY6UzC|=c+))Q1z!kkV2Le805;MwBiRaOR=)KfD~Vw#Oh7fX#^Q}8@p=YR8p z-s7%>oXLK|bx3q7$IQVUm-fON4KyNK6oW>w}n{yrAR(v z?U=BTlwRH$M^)yK|Buzae@Lbnr1Pj6Evv4`PB55kgP z>wWqoCywIs%gG|$qXTdyi{(9}K-M8ODM{bQ_qLY6!kl)^{<*+zvEIjTwdQ@eudZMXO+L?^sm5e`lEferrSZnZb)Rp4dr>4Sn|MP{ryZ?%(0r}-D&b%nT|pgFBnIb zbTN*iH-Vxz!L&*9IZb+(O*Q@rMC+2jQ&}xr)zmG2%c#*(Fr*fa9rpgT7Jt=G7vDG} zu5lahe;;I186Bn&j#DJF1#@}I=hOpct85w^Q9ojV5^?^-<6=ubac#LlnYx3_`BQI4k+nd~43UX77$?m*o^61_B0_rd7quVe zEfuQNdSbEkxJpn$5gfH*)&aj;^6mWgez-pmc9L=JgUyjvY`=P;RcJ&>V?@d3XX}^V zYLnXyUp{lBD-+e`0c~p|*oZz#fa`&5rZkcG1q1wtkPJPBU70FEIxE zjB{4!>t4HH%b0uaJ791Fd4fyf=4}%Z*9X1&phr`pLx-bPSDbiBqh*e>;Y~sYwVJ(` zvEMCQib)#(fMu5DgeJA9CQI9Dj_uXe6VpQ{zhKPnWK&t&QD^H>C)z~8jHN5dqEU(2 zSfi*P_Suq({lSr3IFP$an(Sa#koaba{^D)AtpGX2U)v3k;y;qI(&6r zhmhYST2L%nP${}FO+>3k6uRsT)FJ1|%x{vwkTw8QDVv~r-fTiJ`alYQ572?)CJZcE zw$^>kMpn`}8%_|sKUR%_x3vsQr^}|YDnyriNi@aCi_TYOO_Qz)dzFpSv;h!^d(b@z zz4Ae&kJEAuHC#E zYA7TSI~IWuvZJBVe{a@{90`+yuBziEJ*k0VY12p9{ZG|G?tzwq&1@PipJY10%h?-8 zS!LTzt6IbU3x~hIcr>5(uDrzhx3CYD#Kb0&oAk$=j1Z0M@BO zADNd>W*hmU!lC!25zDrR{`L=`m0l9eZIj0xnMcE`T%QW2;dsJRkj^}*3dm=%Z|M7v zxGd6ywDY>NcFapL4UoiDp$kq5v}(y!h$@AznBZkbL2-6`I!K$eY83}c74)=k` z8m@*&cd#7H`8PIJ5eW&2TJjYN4q`9%aD068)F=-7CiNlW{fbW4VWQgzbD|WISxg@* zX=Zb-YtyLTHar5wl~TRECw)|9+2n?g8Fz1(mjOvfci4MFQ`Z-s`X(sNHLU#`n4PU#Z+sYF2@F&s+1+26jJku|g&E^UwYdPjOgf zCwmsp<_ReEjhp5#PduFiU)-V>u7pEM4GaCh`+m{_6H6%45aa*GCx+&4)`~!M9NC(`P)koML(QOAi!!%`xhMmz#(M$>o$Ex?jpvu&>>e5N#z;tvEYo*>7xHoj0u@1WAsEXkRyN{QK zrq;3)j)|`Jw$*ZYNeLWH#~U@b6#ql;ywJv6i@mUEahFNokE#{qz+kEJcpG#UsP}e&PEz6u&cDE5zYm_oOkB0e3^wN-uF{l9MlY= zrAH~fW-tD+bCp;XBDyaM+#UtPWVa0qnCwP5LwORu+Z5E}O2 zBuZiqsx1NnpsS{^`_7N@Rq7+W3bNAVcEIHlM>ZFD&Sj&xb@TR{0GlyAS1fqF)WLe8B@L$SVbQiZNUF<6@E9OgGv@`aZlEbkRgVOt)it zY{>|g_(YERbm7|h^IHO?S>(qVfwP+;a)DHtS@M>4?70oAzh-BC_0Hb0_c+{@s%n<0 zj+Fib7PbX4>Xe9yGw3?D)-v#(Hh6x;J#BzO-p~97#koj0Y^HDARHE!q3MOj1T-*A|Mf8(X6fPp;||Os-YIXlZLk+eSm^p#4-IV|5!R z2Wgp!`t*gsO$OGDpbt<4ldH!x`tsBkCKmPhHhC>NSqR`j#kz*0ocMokim{s}@ruR?YLrmvT((m4{r`NIWPH_SFmZ zyc*n?Qj6jhQ+=hCHEY>RLDqjIh+c?(29%mhwx5L*+`uBREjx0C4wbT`B5$r@P1v}B zx8lP5{t~c1GOvuZfDu9s=A)xNHU{t_C(gfYf5O)betOwcKME8^YYpm-5~k=rtYc)F z$=*#LP%8Hbd#B7d24RH(>dS7CVjPINg&NM1Mb&DT&+|RJ0-DsE8hD9hV!uE0l4<>k zFI#|xYF#CaC6slO`b*kr%uL&D7i14l5($8vg zohX|*u#T_RlP1VgQuS|i#~8T}=!v4L`$8Q&LjRV+08xud*tRhW`YmH4oYNvuT4eCM zMM6{m44;OUjZZ!3(%*?^z|@;ItI_$?VO=5gV(!LpV%TmBzv|1f01--;mgn4PMC(~s z<4p+sQZiD_sim8lJZpS$+x}cf8YMben6aS$fz(30%v6$1X&bFT8Kd5fJ^LQVdP~94 zXd^@u53w||Dr1|On%A&h)t4bUO>%+^q4`<&xzp2pc2yjk42`qf`{b!E`XoZFX)Y(_ z$v?3NTW$NN$L*K1xn*S0dMbGn7v-BNV~MT2*de8>N}VJyZI@%!tV_%~j}1$4wI}Qa zw99ONQ>+Cslg$)0r6p$S0TWWn4Mx3PcuD-K&EwSF5zfp_>@~|vvJ2EG?Z0I>Q1a*p zHtM(Va46M4_ssT(Za{}!ZT!(bSY&K^E7jnpz z*{f>YY%cE?C)xfBlVi#%o0Okv<Yk|j_73^h&O=wd0} zTe*{Cn(E@n1QjQ?1{6XMez_#+L1VqVMc7Ep!7QpPca|= zu1KQKGLS*jY0oKIe+Q@C$D|A1H=L6T!E0O@UARbwPe99zF}R1*(G_|Bz5U=v&(l8j zHi0HGAw4-C z?-7^0UGO1y`RV&O5NJ5mPQJkr5B_jZ=UwBfpopB>^7_*cVR5d+t(G~nQ%&H4yn|0U zCC;YLz9LTDh^}52^p4wb;=t!m9{P9rmh%U{ydX>i+1Cq90ok7Qd70#x0=r0;+g6v3 zPP{!KHnW+03FCo6kxJ})==U#6{sk0_`5d~XXeEe|hs|JqLT?K%3-P&SGF=J*me5JP z&T6ogi*kzPg>Wn%#j&^0l7K^Toy>5>u~t0p@+?u>BJE9~Oa17xMKhlf@Q^!5+W+?S z;A1pq_H>dJzBps^=VCB=xzS~c?dTV>V#KF)<<@RN4A#EUoA06;gnoKiO(Z>!!Irt z1}Q%;@Icnoc<6Z+KsMr?TP7-8{-tJ_Vz{Rkciy+Rz zD=J(n5u{yx{dyqpme-WWmDQEw zgU~GO@WOdlXo6CA&Zbm9br%Ay+o%^oF0W^ezo+rzR7>E*-sl-t;ADmHM9Xx)H|EYa zuON#EWE@h>tq0l0IJeeB*7Q(!EVTrZ^mb}>F*aIL%FAhQJJ9ieQx~8xW5IP zw!<9L&`{2>ciM3~b0eOkzjQiGUp{%I$FbacseZ}=rGr*tyTJe(P#Fe{#H6xe$N7n< zI$}1XLvLb@Rg+euc$?U&SXb>|SFMQIF0N6##x6v#6O4F={Vscy+eC9`m25Dq!yj4a ztsHKKy+5GhZ{#o&Fh{>TZZY&_{0q4u zXe?GYOrJKtZHbt@WbZdWo=&%E$^Id6j>ClMfWbql_P{otz~I)ECpTyuFDD-tZ8&Qp z5n;RP{idU0dXChP*ThtYuc>;#`8O46tkHnKs=SWsck+uy)$iG~q&0P_)^s*TBKXBP zWg8l+<4XDj)=H4o!KoT3T@$Aw^yc9yGd!%1=@@DzKw$H?+(MFsOsySG)1twqo_%8* zOx*-Sn4s{{LkrPOsbOm;yEbUi8$Cu`?nA%_`m;)o&1Iw$nS9WN_K3%g-uYB>e;y%C zrGPb5#PUJYxoA)C+nAN22FLiokXyuRwpdM>+8#&36m1W^pL@!S?a-Rum+#eF$HA6` zy>*^hfS1IH7_Zt3SkVg#3E7LrglJj*@INY5ES66kxfngx(^Ni#$u;T&MS{x9gw7&2 zt+#`5vM3TkgQo+2Td{ErmpX=+>(t`i7|aFAKWAx#t;UH9O5$aZ=J&}+h?LB=#mp$d zwNOK7C+q#A39e(UyjtX?Q9s9S;!GX)Tj!{vs|b+UCR{yj4&my-QWN-_ywq8X)7(VI z-clB5E&Sh<*B)V8@6Ix1XnIvv)D{};;wJF|(`>i^3_mk8m~1+{kF*=}nfZ!7n%`!F zM|u{{J$ItgHTD`Ig$~A<(Ab>t)Y;fw$=TNuqkdO|5HOoo6;r%j6*9W+ESWAbet(Ku z_cOp5s)bQ7{05-_fx(!!vQpq*{ne|)DXzW3?%B)KkuAH^tVz#qv;?BuWKs3!*h4?SCd*b*K~5EoJ$y!u z7&pHREycL=+Ly7KXX(vdCX`TFUvaA{e;;JV67Rb7d?m(4cDBQv~bGBETAg!qyZ_HXfG{M2F?gz5NiN0|RCB@rZn_K4f zgfvO~o78gB5?pC+i(B6n$}r*t4^8+Rph{UBG_v7nlX3!8}tKCoJ$a zOiy9PpBnM1O zD@R>T7Hf6rTffRMw$`SGO6;iZb7i>lEzna9O~enlau?R6BHhQhQV@EiJ!$A4%2&o4 z(f~1_Zw{x9!^w$I2C^jl{&G72$ODyTC-MK1BDqj(q$Y)?LEJSETlfW-LiNjLSuja!5tQ4met&DG z;Cc;rAjUW%kF(F2nXx9^N54BMCa-m8B(xbh(zy%P_7LylF-N5r7siZy8YM4}wG0SS>>$|=xU z7~M?XqGNB;9yxQ)szH5>8YvI=9>x?ShAVrtzM@LEHo)Y8yB|${{h;Bst3?MHn4Pyb z!}x-epT!Vt+{x>0QoKjx(gwvwqp|L6gA{Wv2mnoY@Ix@d^LF_scmjW(`W@6x4a)|5zh3F=!pCn(ctn&40lmSPC2 z82eZTf%P}$ne5I%x7=>}g7^|OCV5WEF}+stt0Ez-(NkRb(V0BNi^Xm=UiLZx3En+> z#{rZnPeYu8wWIt=l_ z7wkAV>Nl=c6d^zRR2IGW{~$?)y^ypqQp; z*XENMWm(1A!~)~v&DL&@T|3#UE(P1~JuU%l5=~XfSgDRz&uZFt*bPtG`?XO&iit~k z1x#Qk4Lnjh@sICf3EM?D2K_Cq4IOaM?dI}U3mnLZ?Zxr<@5CR5VSFRF22UTmaW_JB z^83|u*Hp>R_yTdot-hGgU4YgvDRAdFc5vbn3>t_qqW&^7_j8;k3m0no&SebUO)Q<{ ziJsp<$EE(1({yACU84!}XlEhrukEF7bQlEU?MmJ<2$da+P=&utAYf?f>l+`b7d6P^ zdz+rH&3>CAY`Zh?iRuy@X6L;1n+YVyD_qqlBe>*GoPkmEF#!4}{u1wuD41I#`YSGP zD?G!~&89uWJc`GEW>njp_^d7h`PMX?^ygdZ!ro_Q{FQ`|m|v7f5_3GQ*)5!%Z%MOY z_SE&HJIaG&VkwB#PrI(ftw9uvcUe)Al;9?sjfSeAiS?(VnfXcR7iB{R*fDGba&ReZyt0*J@}fIBj5_~0h~aLkB)G4; z?sK{g*{2~;lzdJWpBxIlg4X+Rw{J{0{AFo7JwE4dz?9UA8U4{a~j= z2*#Z_y}D!dRJ<*~6;~n{wiRkG{m$Qc-Aa~=Lo{O7j2n!_;4bT^Q@!sa zw;Y~Sdy`gu9tOA}Ut%$1Ie60jj7aO^lud*wqwyW~?rQutTPDS4)X0jm~8Z~JBhThmUsmqrJ9O^l6g#jLea(Er;Y-<>Rs+1uHRQ) z0z=&lJuUW5EBhp4D9nkjlt{2Nhx*y*KAL|=PLusM3jb-30i~$-ghi&OtBg%s?ylgo zFTVLV1NyR;7UInLr1L-4TGBkVEm=d(w=2zruD3t1RcJQmCgo2Fi=u23&c)M+Pg1K% z%fbV?52k2v$RB0O-+qH9licD?Gt>^2+SODR7~$TPEtq?o6Y+AbTNerL;v59+{p32R($=^Fl&=Jm?j^4oVHCKl>Jlk{y>9l5`8?>Iee3?VEPMl2gH#=FB6 z0DafqO2Y$>U};@Z0%e=bR>VhcMEAVuFDHt4!oGj#6oB2t3RmvpdQ!?#hW{P&g+6L0 zHmW_~n@)VUdcwBWWl!1Cq_roKL;BYo`pmzTRcqu)U#KUde#W3!p6vHm+O?xZHSJ3_ z7#g-H+6l`lqMRx}mMkpgq^lm%|iBJ#iXZAn*nLJw@D#+p5Nz1y9PW3rO8y$kUlZ53ar6X^d@`)-%H>sZ_ia%{?kg}) zosdq--twK=aO!O8DKBrAb;as)av3^T936PbR%UhBeC$yWDBxVP&;RQ%kMaZ?8Y%0U zLyR~X@p+xIW(A&US@5Qg$#wac;RAK22{*f8ra|X4nn`1ni3=+Gmy$cWq4kVWefWS9 z?A20V#8R)jNSanyvk(by#?utK{9PSw-iF$CLw6tv%+AMfnZK!tr+21uD_l{bhn{vENTD~_||66V|#*o0}bS_Orrl_y>4}OAP zgIVyWh(A{C<<8zJ2gO=&iz7#%buYSwua@?{GM`LHR@dNM@)X*I&GK2;Nldw+8~7^p zI=Z`|Vy7Wn!7rMe)H-%=RW)vkoc_X>MRd#1K^Eo4$v;clqg`lM>HrB?9xids~VCu~i?YPL+|eaJwf&@D<< zhl%zS-w4rNgOJ>E_Aq5lLKUd}h_JX1-Dm(a5UN%ge0X{qlwdrp2hzDt~`%~F`-ke-2V)5zN}LF2|KARJS$2I4>1 zQ&#g=i?qdm40JzsKX&t6H8GEQ&afWoLPf^<{5$ujUV1E^`=KM!DCpCfJ-1MMf4dcK z;vDqeh~%%0nD1SKdMVS4gQ9&=ejWGG@2|UBg=l%uBBnXnPT6fP%L@fh>&EHK9&X*^ zNuykr|3?AUFL|~a=3Eg&H*XT)NeUk*z{&N#bq8=ge*)S(lLH!V6T@(Cx#T}p+`7U( zX^2YS2tA7dEcH%wrUv225Zig|pMAr`@M0MoEt8VS8-?Hnr*mLeIc1UvcfjmGqW&o~ z*C0x_I*2C#^6rOQ9%T|DFpOK$ zz73E8BN&R19;5U25yaR}haozM<%d3-tw~`};;aT!c{BkNc*p^ZsZJJ6(8;|D7-#^s z3*_od%CoeC@FfQ!*I~cpS(@EHx{>)g(it+kRGs)Pys!rxxftw5&V6V!f!|V_BTE}Z z_S1St1#&$X4x!Z2S#)coytQT=IJM_AT>0e7^czW1Kwda%-eh!Eb!bK zpwW*89{Oj*SRhWx$A!BI>^);vpxL`6z+O7#Dj#cXk(Z65LJU+xuz|ZzkyZ|)xNj(5ZQ6p z2NC82-*+*F1A#a4^c#`)5Jp#vm7zE-7|dU|VUZqUK+XfiQFnc$gIgFNz1k+*fQ5C!aiftDiIz$58?ngP*N%cd!8c{o|E+&;Ub$7jx5_0W=q;# zq}>@kApF7jesa70Cz62(;sD@<>l=W}?v8evsQtq@){O(G7n;b$;3RBLz0_0+d|}>+>WkcpMFuoTxHI|P?S8B;T5Hu zB8tlYmBP9k_%_q+}t-W(vg_ZTm!~3?Z#QL8wB&EbLPqPP+h_VLZ zyNyFg+z7?mkgkle<{&U`gAm773WN7g#8uZM|0Oo0o3{RIV1|mp`)R#ngM^5pFF~sk zu;_1N2q0w8AnEwhhd9gLSBdyn5^!~b=XI`FURx20k}Nu9GzL{nYD^e% ze-I*i0xWeYV~j)Fq2Xg`PfbC?yq7mu~~F;GzN{p1bMk&_Y|AlquH;aVx5*RodxMA$znO zqZm$@x@wu-pOW3iP;_%-9_@*NTA8L<8qu)#)e_wkEAjwu0~du*ajDZiH!25bh0s|V zBebdYl*0M+%NGR?kqr-OBYGEkqWQnp$lhwy<-OFx?$l(%<{uSf{A8(5pW*gKApMv| z(aoA7I5APA=3=o(f=;8;F`UAb!xb+YQMqu1F$C_DQi%#yVg?ZO5~PjuKl?&aGJ?tx zGpl8C37Uxyc4_fE)Q~OR+osR06KQTNYwndZ`4vnHY90#3c$6 z3D_(Q!_+T^$g9J?Bz7~hdPsjABw3h;SLeGB?Yb$!GnV3M=Y~=mpMNdb0?bf**P88Zpk!jCo37YO|z7Y_l?lEYZ>% za@{?zHB*2ex_QiLDt4XN@V9s7;ecR{VR$lE**>K|E9g;E^XU~t4`rwYqpFO#TRQYz z(L6bNG0n=#Qbe+B`9q^bbhr>Q>C-<&a)Mu>@zPbTgxEHx94ub^uYAL?&;A7=vcErU zdSbIL+eRk(-CGfF1mN!lVO}3|D%?xy(OG0#&kw?tC~!y%e&vu8j2b~Fw?+0(ZKh>i z-IeN280j$bz@4Bx{%$B}QY8@v;!BCWUqt>ZUKSDI+Q8I}jX_({QQu^S z|Lktf=e4B1F}uH7JQR-p3nc8g3<*oB0oliM9}L5ZvxjXqUB=mlZ6qAW@o`+C3rU7$ zlcUI8GF%4^OP+Z-$ETL#m-`NeHN?KiskGZkBG&Td{YpvhRzKXkx;Ji$iBmJt$o9D1 zExzBO2hcQNrrXlGfSg7#3X{8s+Hkt$Gr9cu_Z^lzBmXyDz>j|)aZY{A zd%&}DXWGb~UvK4?Sfq20{B+_zMjOo{;mPP?X0u7vqR%M`&Q+VnQTV}9PU1_;&}r1+ zpp~+5K({xK;s{IXvtNx;HiRI)-go*+H8p&3AIEI+w1yM8vOMy7AWxq5tX&|zJJ9Gv z=qewe40PL-s_^BKJz0;ff1=pjkHNm4lXk~)CD$!hJ5HGR4H}e1T|}i;mUXUxCy!L)IzlU@d2bIlOg$9Qa&m{7PH#+asBnL+aV6 zvjBlHjqQ8RqB>*x$cxKyZ$-TX@{J|g0acy;_w}mYu52dmk2~KW4V) zHm9NI*vF*6PJIFFOiQpDrsu*DZr#k_M`O2KpJd+5nQ`(Cq(y6^53B%9U8OG24 z<#d(S#JwIupVZETTqXrXxluaoDu0n=DHxnL=+b4|{q#ubZI9KJYwBr4=-FN0UddLP zpfFJhcyu(tgXjHd$vVzVy2AjMhA$GkTL00cFDRS5W~i-+v?^vI`$q{EGp)&NmhLQ{ z=Ni@>f?e8M6K~kJjyo=Z& zRf2zC2;ef+ zbDPSi6CuN*u7yqA&D9Hiy_|YLHWpI~*2xD%#VI8ZRHfUg{t8BtX*T(p?_{n%_QjtB zzGhtv(4EdqJs?cM_gL>pHa7h-B$6dvx3(Ox9uvQ(+B{Lnuf@wIL_mA{s;^;^Pb&Gl zb&HCH@$Ln_;#9z!bR#iAQ8y56-rd!_WjCAro*>IFk{9zHUAtc{n$E6{cdoCWvk0FQ z1HqFeqS~mNMqu3$^RAcMwWX*xw4=c+;vd-?a__c#+@tR$t-7Gr@VZLQc@^*gI^m znT*Q^b3_mLvFIT)mlR)rI$y`D?+9&aq`(iy3{_h>@yiI&>j>)JH^zG3jkO9RYt2}z znx{>p5aU=vi?I<})tS3KHN85=~zD>IWF&9IrMX7*}jMxi}z(6Xym#@REWA)>Zn z1je_k+I(E$OBVU26{lm?dWZ|leUZPgOea_I^HlziZP6c1WziV46ebOd`)nks*V^F= zBWBHShKJW&#VomKh4Jn}J^tcEp{-5Rra1ntrJEA`2|MB+>D+D!bW!pe6U7A&NL@*# z>0wO{1cnCh&g$BGwm(|4-Zi!dIk9HyqBgb%6^U!@7z9=viBT|9-a#_6t~awM>yVn- zA=+9@GN&28= z7C-$Ix1yi2`^G5Jm>v@Y$8hR@QkE{A=y{6BejT5OG7xIt3frgQT2l%s1suW&echpp z{Y94hNy$3Kh?eJw-amDVKJicuWYxu*J(yW{DAl9$Gcq6RXyn$jN6zP$a3W2Y;r^jx zRyem+kIssrhE=3>r>L1e6)Br9%UKqgb-ZVH(Oq&b=E{C&6E$FnZJ5X z>xIsW{v+dGwn5!a{T~?&tPB}CQxlVfSsa<3S&5VWV-@X8C^?&seRpL^FRq}w4`6>x z2m}t&4;w-lMr2*$6gm44qxIvmQA(MbPL{J+F z#0f!Sb7)Q#bVmbz<^!XI7a@Y~D8NGSA+zB{K1XPB>a$D~pgVF94@r>n)okhy{4q5$ zFflqYMo48?Q66B99B3;9zKJaI2HK*5pHaZi2;gUO@H6c{E!gL;?Mo#CPM;V+p73CO zm=NwEHCQl8L=h5Tt_*lW{I5gtJ+LB*o`&Y^L7q}sBdR+3FH;>^<{s&E`r=NxuptCt zMHN5>B*+KAoG_@37$S-hVge)46j}5JJRt{p!a;&iLuMn3Fe^T%DN6Pz)@UFy{>>Cj+!41^XiixrP-z4Pm|gWEvX)(kWmfNc;xcB7%ExAsG=x-oO(i;7Kg-1OaqM z0)7?)3&Dk`MilY;r^@}J0jt(nBZI#nO0*5lH2_as{u#DT_Q!ybeUMNA zp3s6kDIlkl#tFvywfCCbnNpM7)*ryn?Ek#eaYWIx*`5ig?IT1h{GZ(iz!QB?8z}^W z2_ZY5RlA@p1>b~Ga)7_UO4I-u;34(JeXoEyeoz}4hI{clW=K|e(JNr??VtDb zCxG;$hlId~>>x_a08cbQcO5}0EtB6CbXQnptRg)IM2pKO<>_R;=w5~MWrP(i1Cjow z-g1K1Q9}w5CHjWODL`$65Hdsw%E%%P02XAbmieUqE%R1k(!>mDe%CAfAi)t)Gypuo{^zc{zd96?w#tp@ zD0Sq2u}xyMs`jY6=(@m5+;7S-f!C2kfTojkLvvF9xc?as5>&z3fl9aNpqZur$}G1c zr3erZ|P!v;U|T;4qr*>`m;Eq5%h ztaPeNKdd@{mtHPw+c!5I96?ek_(IM=cZR*_h(Ecs4G4?0v5`pEQF)z%n|`p-|5`=j~o=GzMk|u;vhIcXst*v)oa& zDB^5wo%ObZPC?fB!;u<-?&=j?VYCxz7#e4 zb<^d1J8GZyS_4y!bIrXJwRS^w-P8gE@oB#Ol}ekd8kgRimqvy5%4f-&R=Y!Kx#t8c z546XhG-=}r{wkoA_eZD@ZrSGkmy0fbje#LL+iBF{OZM zpNqAj^ob}=y3baZW{Ibqp7RL;eBVKvmo_aK8A(aKm}?3G4B^Dll+->qWy~E#wVGWj^e4V%B*9%#~B!KcB^MLZFA-hdX^T zVn#7y!X3Uv>d=(AYGd)T$9vsK(XBXD;cafh7kn|Bg34X)L@w}3 zXC9=VdbeUchg~c`DR)WHT<~LR3tz}uBPCVCJ9dtU2V)9|+X`2Zk@Hz)EdMI|eKC*j zdZy@YH*Rq4_H{s^wr(as>x>3r`y^|#jtf%N8Pi_9SC1L1DZE7;RHzcn|KOg*h7FDx z(T85Nr8;li10%8A9A5GZ?a|!in0an% z>;cni<<}Qwn6YkxpYNRd-@a+zN3KPW(2=(sn&hb)Qjzqsq~J&AxIO-C zPFzt%Y57cNOr^0*c!G;Cmn{AEJ>Lv5ZNXtI{iP~XfiX|%I@!W;h&JzSfHiWLV=d}a z$++EE&`+rMg3_0^y3_U-sSPue;U)7eG6 zjl^1*(GTcsvxLuWlERke!LZTO9}LpzUM;ng4Y7-G?d7PhZW6@}n%n}_Y_IW{R%v~` zU?0)rV07{3b)}yG#osCz644yO#yorc21~a5{b|dK&FDqmLV;`oN^xJKV7i@ERQOWk zA)lsXtc0A0?Nh-G4BYgV=5E4KZG>|jXy>6u$pZaCKK@O({xMIjudOd}uK~&&&5hy9 zy;^59ringgd1rup+so}pW1T%3td#C%4FA8sn%eo6mmPcSF3(09iO%-zISbb4Dp(vA z-edc7_>cQ6ZkD=h$;P3($(Q@3#;Ka+ER)H_uBcn|RhrRUVIifw*WdfjPA+i%y?9__wjrvW{G0IS1|9gC7$?pbemyJS zyYuzWSOoMKL8Zl2E+^IrAxLPF<;^WHZr1?L@mvdr1Y`ca1OqrxF~pq;mr6IG8%ppH zZQ0km8E zXUEGY4{u)I)}R4hgdb-+fIJWMlLPj(&*xH!A3Q9hL~ImSnbXco3D16Us}EGh-kv*H zzNLUpbSm;z(?IpT6nS$wzaoBGE=edH0<1_H~crYCulS(J5@ZvYw{9c6`u<| zOPLXIhiM9JV5x{|uNYgTA!3(`gy^H*pO(@{G!oJ$SD|L>qQIg&Qb&}IXw29YDH3Xo zZvrx}(ht;KO1+k`37Mxv1xmx;@ePRK5D?9$@dfZseg@gAw8!Duv}^Ll1+!vA?911h zSf*5-L`$>bM;>T(vp$7#oWFQ@&5Gkg>#6eh&**W24lhEK|KQ=46LmrL82MaPScgr8)nuHrz3 z(?1ZkjaTR0?6m8R$AxRq7-lcZ=5lez15GHNSX_lqSis6tClHAJ{r6~deqO-GEVWc^ z>v~}^7M42NDM&X{sMvI~xr!aRL;c_N(VxLB1jLRQ(aj055-t41e~UlE*B4*vSz1Qw z)1M!>DJc};vK5u{df|qgC06sBz}^WK3T%^S`W{D_J7U^eF%l6_wx)JO1?0rrA`Sc70BLc`=(WYg>s+#j_<4LqO+p2x_PBk z!4R5I6)QjO$IRH=Ck}YeE(HGX5E1`X~Sb`dZG@3TX4^Nivl7uUtob(_X zfs~7VZIyoCsrKAQ#!(to{GRty1Gl_$I;Q)d5=ROa`UO9iUJo4|REfi!=MqTHujx^< z+sov8b6j3~ES_s0)fzzp{@?=2dV83oQ>2)c2g!oQeftp-pGLtohtPR0jl=jnph0gX zYG&x5JL$s_ogkNc09EAr6q0+0**+$x<77F|t8fK7v zhY%Oh7PG;t$e0(jqpYMPe~Zz5xvl`YN%F*#KzAz6$$8gv3U`buy!Ls4QgU1Gd@RHq zri`@0V^oaHEY)b?Z(Zu@&4J+nY)=9fztjHgUE`enB9sHX|ay zG^K+jyc1z)j2d`%-)xynPo+5n}6@6`i!Y)=%Uc2C{v${@SHPgVBK;asN_tWt0MckG` zz0yU((pzn6*TNmK0W+ST1+6i$gvdANZ^M_ny{ExYs{C-nC(vdY|dM}sY& zB|kS5bgSJ zE6QDovmZ0+t(iwNCPyH?-59zO(_uhddG7a}I;-X_`w(Bk8T1+>cqL8Qqzy{?b_^mh zDq8>#d?CYGO)y|;Aig9R@E2Y+*P}$KIumx+!;oZ81T97_`B%hVBJFp}$zW_}aPa+! zjzYOfUXb$!=oOpZ^!4~x6d{r0 z3%$Q<-7Rn9?<5NuqPXAF@`bLLG6iCtU0X7hRP=rkXvPi0>!a-H)X@~m^MbDtOLTWV6vuL=r8eVGnN z^3ZISh%0WkZ8CbT=`;JRrCyrvGwyTG=H@7Wf9HaVXC*w7o=8MDTf7^7%R^P9Pyt#=fmleL*0EL{yj_D6-8ueuZ%VE9y|LaZQd`pNyN z<4iV=O~XeMHw!m^P|dB#l;hMc<6yI*F`v(}N}Y*;w~hD4xho5z?v_@XHcqu)?fy}M zW#$O;pl(mqt=>_W_Eqh>wr1_I){)j_t*{UD^mXxS@adi5I`rlp_o=Jti?L>h&9#BI z^Z5o~W80B*#ix<9yU}dB(YxVqRb!JLv^C2;x5Ghg?xFhCp{(Kjqk#e#8lPiPPnCT$ za9>EVx~+z;G~Kpmy{qT? zYGW20%qmTR@Mi4E*X=bicQv1e0z@Uffr_qg zYysk^9kCWlOS;{7k_r7y^LrQ4pQ&wLmzcJJ7pV5?uac{(&FL?}zt5rjbajJ^Mt1^t z0dsYtf2A(Fc}I)To1$`3PiCHnH>~O<#$Rk^X;)A?20oESapVpL#zm@GTqlkgokm()N<_dn@-QSDUY0 z5eE8;>gRvsvfm$;d21LKAKzVTYkYn@C(zRl@_RzLX1e#M&IXBncOGl0Ib~Y~FB2Q*nSt`mu-Le-yT4~NS;tB8-o}uST`}W6G zWbWyXS3uoF%o{L8Dfxowz@OdIY@l?47o+XcIoC!HZkAg zvk%(>L7`sxACwr)zG=@CoZV>6d8Pnq29nyYc|7r(56@0ft@(Y4!>}A>_w2$d<{d;m zpbtfT%*wxk476uUV>L<940guccT|Es6~KCV2Hx7PG)rvps^@3X*w!G8#QfD`+v!$L z+JezBXQ{||V&+sNG5!EyDBFq(o3oX1eODiYiE!B;AGhs5dBtk(a!Os7QMgpS(*k?*buZS%&aL5tZ+5Tk3n*9X;Hy2kI~Eqd&P@K&9l zGlWW;jD`e~Rmua~!SAfUpRDxNSCU(U|AFd?FG7mu-p1%g z+1}p?*Nq;3W;DH4Gg=Yf^y;8UFCyB{pF^de^~xEm#y%Y3a>+j{OO)o1ZL-@-Og|~= zpY1U8KEz-7?;92}%TlJXivqN`6qVsmSE`Jtu!{SNPLHcR%yOkDjAV;h3WwaJ@uy2vp(dTq-+=tcWg1S1_n! zG`;o-SGgKjc#CJ1Oh>PhY1I{d`WYC*b)r-;dZG1{9ak`g2q_8bE52d$Kx3d7Fc?S# zf^{RaLsW$cB`C8HeM!q9c(a>6875y$024~)g=i(?oSicUnR3Y2q=SshDb*9hKNjOL zu1cuu!UE+H1em+k>tNiVb*_ulcXz#C53Ub zs)MJ14?!1YIbI7%x#kG-lq3bwf$%{XFqoI?l9=DnG7?KLsVjwE;!B+RY{4t63BluC zrp6d`!T36_%=ONn>Ye%P8%S1sMCzS|>MsfO44Joe<+?POe9u$o2eiCc@V8~;y1p{G zp7v#=n%P!Gm*$6KgwSWT6a-H{X*?wRjSW~BWxV^(52C)DNJFi9#ZY^@7T(mo0`ay< z zn<<8+>`F~s7t?+TF@^HoCq))s%go81_K*kI{FZ)Go99y%uNhSBqW`n~dA=*bMWM7W z?Ns$sFU}XC3RB7J9S!m5t`Pd4b@_Z^ogD2cF)Pw`1?UZ)CEg!+>7_PBybh2>uXU0< z;@xstrbEr?N-~dP_hARLmte5qNW){p49swwiHi-NJ7HYU$oHL#alo{0$Y5MIB0)GZ zgdNx)VZPy(fFf_>AY=%F9jq!`RaokM*VqKW9>_AF55W%!fd!M;A^n6EZti`qqLxB7+x)Nr9gM zpaACpRROUNJU~{E&^&4oY(D%fyfYvhpbIb!M}v8>7(hOK+2p!;+3eIB7~v=UjC2f- z_MqnE3k*d077wuo3;>=UbPNfa$@IeeGr^cJQlq-?_>y!Bj-ZCUK=@{yn^>6X$V>I0 z)xB3PU?}ZRe6R$7ABb`a!gXXT!tVM*J^X{!Goxp1ipoQ_ld2wE;eB1y<>A7=FjF#fCZ8Wpj>?p0dbB*=FL8 z%s|v^xzj=PZ@aXS_?GQ#I$S@i!es+!?+71pcRe4DYOb%rIPE4 zz(gEeo(KKZO9{4INt9>2zb+oDEqQKUR7p38XZ8tV#6fs@{>n*uNqf)n5T zP~;pj$3JANHf%c>GZ%tU^hP57$`|zLcu{o&T3`HMp+DV~lN#(v1G)bMx#s{arh^s- z8K=fR)8$<~;_7vGywY8$>fFQr$d+PCYgibO9O-LBkk`V8fhJN)(OIXzAGTU%-<_U*3B zJFneF)YkSqFVc^lh)O(mC~Og~#9_0spfx3zmJOVoB-d9|aCoL93^H6^%i>eT=H)59 zNbf|7OXES?A58b;TT=H(jnroex){sVpX61eeivKt?uulMK?#|AJoiA>!eDk6KnTVB zU-dGH*^Ci8HH9}CcQl`7<=)IMTcul5RZWJnWC8rhVx&=p7}9fyCAN|CZBnP}RQFEU z|0e%<``Y$_@3q7RWi=_&i^l}#D|!Z8h#4I=Oa#Cti9Y?hh&=TNt-7b^s#jShDc^SW znY@aCL$CT`@(-6Kud>R`dZ9n{LO8Vg^zyX)KZHUu=a6^joxQy%aok9oiMq+VWdz-Q zJohh-N7rZK+wvBr1{V#^8ijf>W<>2TPK1P+epr1EQA~l8hqnxC0r-YnnUQ^iOG2WD zS%t?9Ck^`ourfmh4TjYKc+G@4;d4-?;WWb60c1dVDvTVotq)u1)9@~^3*pZIIG~Oh zbu;oBCIs0J)ejNO)eP@Pu!aKp;D-rjYDNPSgG51aAR3S-kPf&TmL0AewhiD2Glgq{ zgJSl_EE*v1KD@X;(>G(SVL>SD0C(^&eYaE~Bp@5mIn1Twl@=80-}eNrKKI-`VBw)g z`8~=KvC6#TT(=TfH?V%CVaU?3ZP?Y(#~Je z#Paz~pzeF9vFJv{3?J!y3>`kr0B@Nhn6?Log8LFg=n8jy!N(H*h9kd+Md;#c%Kosasud?f@ePeNfnER7Mv` zOdaPEB$(I(&uaz2siHfNF-WoFO+{P$BrI})qF)yfb`pkeJm#6(eqnWDi!Z~r{2FDE zVaBSc9`j+c1?zDv1l#pDtRVJ2JC1LBx6-IF&DBTuY_fzAH^HRAQLxg#K)-KC7HR2D zmLgEOIP& zIMI#m{OkKy&`%$l0C=!S`{XYu*6iX3hRv; zRQhlE^yxLrRVut2)%YG&(!>X{B$;negmK6<2St1tCyv4a5n1Aao2cq9D1CL+MLI}5 z2%U1`w;)2*KCU1})d6~be488p>=45ZQ-0zd$UX?MXzT>!pP$MBOavnZp`?SXjl!CM zb=}yeiMP9BB?IdKM>JsqBv=SY{{_9>*YqP91n~F~430K|$d30{{U~ez5&wEcjF7}< zJ(z(`$tE_i7(oeQ=r1Uu&AR~O4gJnAH|rd9hgQb||Gb$bbXSfY1^NS;JPmP`I1X)C z5N&R{DuP~Kk4uv`eVt%gx>{t5AnA{cVN`FQ9T3~Z_wQtJ@TsRmPxYDPx4aY8T{>zj)K6G2F*GLBHzvG{ zvdyBIW0fn^Pw+8ECbk;mC+oz&#tle(8VgyYM2g+pmvLjn9N813VP<{!@x{k`&Hu=#o)H3!@zIN5%>9HNsc(P_i(1 zE_P-%A>;!tSfm5&e{*`^NC{V>-kQ&f1EwA9wr5Iw!j) zJLjS8juPe~Q{J`*Cw{0L-6nkR8)KmdL|97P;(ZM0sma5gEeI=$bkt(}P~XFf&R3BT zvkaok3!CZ|Trv@&SV;+rK)?6E-$51E#=0s~(AtYrlFJ#a6etx`q_J$b>~PgE_)pFn z!k+0qVRTo$s?y!AvwxGoi{Aw$tl>7JGiA}J*c)VuF-t!(HQ<-&VJ8Tg+MfON5C;goaE#-I zMKrdDIuoq-StLv0VqT_be9Ml-aKc(}N|JhgiJYh~NW z{xnfyu8nPD8)v`3>nzIuaU zMAjK6HZtNVjTXC;lKeuXlM>?TSH4y^W=#I-qlex1adq!!a|YtGsh|sPV8>eBmbyJyBy$*Es{mlpLuy{^1IiK~V3LOJjcL+5f)4{F@TcUjbYLpf5EcG#8v5|1S*cLuU3}2}0Yzd1JdyWHaM`{twja zFpr>xu3pEKrdtyJbcdX)lMXUWz9mbw2WO|*Wo$o;8T$&vNH(kmW<3dY5|+X68@;m0Hrwc49< z614?3Xnsh)?`rLK`G(4Wpv5*x%<_WRa*il)e2r(kkn1t&zb~Bxu2`GHckEVzNGb2@ zgJduNoYByj`}<`i)t|ARKS4vK5t)3$dE5;O$& z1cC=`Ttlz~cc%$1jYA`ih2YRY2ri8~jk`1icMI;WjXRBfwf8q_~dmMs(P8;-1>1v8=*6aI6WEH*9h$|?P z)SxS3R?!G8l2zCE|52&-Bt%S|Fi4#YFFlS}{ZtXNo(Rw4I++Tu1Hv!X1V788du%(N zOoZnHg)i0$iIu$72~zr}Q#uZ3SdBdQ?L1Dt?u3h^J?1TrMb9k0BPYCGUL5-}+}mvz zhgY|HI7-mOz#O`W9LEhfoWZ|YhT?b{S9PRuA?LY}^Ody6cocunCbHZ5fyE~xUVlzR zAP3gc4%}l2{iw%Z*!6gPM$C5Ito4L-tC!^#+-Z2U2T%n}1LIjD&ky@hkKf^JQHyWq z?K{sh!v^Gwiu0-Cb@jjsX%E0Tsi8C^Q5x@p}Bs@hrBJaaz~EwP3F7jxYV zHTud&XoomS)GL0H;1y*GB9!HfOou4h^!wvpj%LuTf_uoqc+o5hKM7uEJU z<~7a36T|lRUZlh;EcL1sTC3JxxXUthc`Ic4;mG4>cdw_{#T=g5bt)u=r8r{Rb=lU7 zG`}J2PaRU%m+4}yU}*oQ-zbFDAG5kcuHWnUuE!4#X9HjpPRao4?h~Yy+oXtdHcy0; z(u0Efl8q|=Qn`DI3+IUu6)~CrsmK(FeY(fni&FLtK+#T6XFi1Juz}(Sk~b^O8{q%c z4`AnsAr(HEMT&o2^0(~b7jJY^rN@QLt`^5cCk}HiHLZ`JrTLDm5g;>29YE`MW6Tt+ zV*Wr0Q$&=9@obz+jP=hL3ck8g9thSI#p?7S(k%E!L^zuDR9F3DxGqG`{u&UK>^{+% ztxGj|S`aT&BV4qp!Ww1mp+4}xtS0)h6{*0d1u-%(;k^wNhA2Ue&$@v*|3Y}^53K7_ zO`q1s_tXfNZaT6?fjkyE*XgPFPU=H@6ol>89Py)4-2d;DntET`d-NEJg{~vMhWZ$1 z+tm5C2GQXQ$I>Wg?>nVqU%ac|3vxY&Zu{bjBUonwGed25IIhm+DgFowiXRMe^)tg= zu55V?iCf0W;Z}dk&+sp7^bLs{|3i`U*56xKrA|QAPxR!9vRyb9M*UZn6voFH|z=E2{Bwvt!nG_QXN;9KQme;Ft>h@UQ zULf>w@yQ;JGN0Vy2XNKIkm~%-vwpTUV=oC}VUU@H-M zq>(P}I3#hZqnax3zp5P0y>;Ej8|OpDrBgLuFlqcy{WHiWAIk_oUOh6Mm8_WD*z-PI z%`Oc^+4;DfWv5Ut-tBD3@QF3vjQwksLL2vsaRPZYj(?XDGS#AYx&QaL@KwQP!pY^2 zT~iK|_?LwYwtg$~XxLlj5x1qGTlWRY`(-ZQDt($?VVyf-oLh1Hex=<_5ysku| z|6bSzw%gSgpn7kYI}_Ok9zCJAyIo1GC8SBNkCwR>7ZdxBh0^dv`&#J>)1tXIMJ1nW zY*nn#3#)%yo}yEl70tSn9iHp^)Ojuvqs&}etDLQ?miL7>)bf@2ORD&C|@g4m#GXHIIGtTUDLfE{6=MLGV`_+*Ly_&{is}&qk!IS1P-J};|3uw%)sGy2VuIL`peT8lE9;-8Z6pvl{ z70$`Iuk{WzoyfIIsfU!G#dLi;up~6>hoU~=9{%*lsLyw!m#A{i6Y5d?H*PbERS5j4 z(NN|@PJG-?8uaFnN0pxQmd=<V#t%iqBcWnEzcU0jN#TAcXC5dmaBYBKj zb@%Ixv!_!=SDTkcqh|tFM^!}G&f};9PL5r)4|mJ&w0rUcP);C}(>il2A6A?giEEr# z5zu{C+Rle%rDHM@pqA&N?)PNgQUh*f=2YLcj^Rv92SByNqP>h$BMe=J4L znjN!wL1LzH(02rZP4-vJ7kZ2{(rrW-wW{TD-)#wO?uA~=GtS7h0WmKB(PV#Rf8ohA z^9cdmyHI79Kwxb`nBp_FgN#XK%^2e{)!B2=n0P)HipyfFTK`b>k0r~Qc$wt{ ztvcNwIdRt}V#g2*XK2SY^;GlHPb3lJY83R*um7?|O({m_cX32Y=Z?bk3>@=y z1(jI6yYW}UDLiSpQR*$gC90C>!yQ^t#1wOU+(f$S|c<} zV{Dyg=!I(gB5k=$QU+&e0JXhgtd+(I@EJN5*S?TcXz!mfLxb$-q3#3K3*_uf?KHBM&c89(Dh0qq3>3hT<^$9n+-~Vx zG0$R2GCM<9TvTb4jV8Kvs#?`kauCH*3?%6UetR2EyJMOPikJy-s}NWq2h}Zx zDNZk#r$`bvW|LOt(#M}d(0_B4P1oWIbwpJ7V7ZHg7-Xv2TaPyAfe$qay$Y#n=xcjC zk5=!?-(M2Xe6G#2)gzJ+_rC*_!O6@X;2Yy8y!wK3)4U43a z8dCj~wi?pPdz$;xw(Hqp0rYe%u`u8MOZ`;h!!X~uOTe|wb!1Wf+Xs)b5nR3ksNB^< z*S;5_lic0u&dN1m0oKt@S*wD2P*5j7H5i{vw@1ze?OZl)Za*$5=taE^tKH19VwOZ zJ@^qEYIh!x`^qEnM{p?j##dto6lcaWEE*)3?ZcP-?vUvtiD9~p)KU(F{!-jP)2X|* zD$r7hUVf4;Z|{xTfZTr6q=@W0^Z|S$D-VIo6Nusd_Vok*ALxRjY}LuTdPC{7S72{t;Vw8P-Mo@sAMh|3Mmhl#mZQ**Gp?wx z>k95j1t;IbtbZ!TmedNQjsGyk09YF;`WF!NrN>sgSD${z*$Q6 z*^Lt{L#}H?eRf%8HeBQ2lSWg2YKlrlS@Ddb#%uu@QhoJ}M!n*s_ZKsR%%4O>iRc=G zPK4vRvQ%1hNOkd&{l}X9$Hsj_=G(K!-1b?n2iVT?d9jD|sUBczZst?fgt5qKR3zd% z-;lGthrN#Flt{{uzT5>I2$(i@{#rJ6nTQ}tK@cGgog=&&L?=x)K<8Oz(u+mQHk0^Y~QQZ3N*QtIcoeW14|A1$XQQ@|JIHM1aIJrJ*6aR(pr zc*xp0ZPun@E;T=XYvA_g)T<@+kTV_5mkv*;6WNNraH^3>V~a^}-rUsp5YS?~?5!zr zvMtu1XRd>6Xht>9F_+c$7n^NmT{u}Mk=j+HCR+sQfwnB;shu}vqV8s_Uo=QkO{`(F zR1>a7#V(n=Sisur_3RW@tC zWpPR-40w@jDc~PNja~GM#xcX?Uo2*F05)st&+^ImCo@=Vrk?OR7Wt`RGyCG_YZl!} z6D}95k!yHmhuN#2L@>7iYo?1Yc_)|h=H}Hs5!Uu<2PyS48U1{PrwT=0G0e*wN%ix4 z{H>*;fM#M?ZvW|s&U8V64zqQ97$jz7Tw}8b%s2sE$9axa4B@xwem<*p*B+uVxc7dP2PH9qXKkpk>P-VHnK!>JWJQa6x8 z9h%!fz1}lcG%L=XQc_*4M(1u=%wej__Ip;eMUnMGQa7bW&)W5;i-L)z$DrMbYqPQ4?0&GjMU0!E^wsHz ztEV)WqUuV`|MGX~@;>UBW=F|XwG~EGNLB0`yl(9nqD&I}1L`P&%w-;W#{J$Yv!ECdb0!XHU{C()^{kuwtGp1@ zs%OdG!IjKA%JG1^6xLla&hJXrp?F2*aP;Jg%3+!Eu&Kk_t$rEjYJL?cv}o~5#8tAV z5e3S6tm@VBDHtftKl5_yk93{hI%k(w;yWA~_DTX> zq+upUJc5~7@08K|MzBg^lRJ=i%}p>}=K>xX;j*T^`_kHP;TKbOLj0bTZ%X$`4j625T9DiS{Dnk6#$EE#+jQhakHD8rI~Y|wuc-%i{i>j@Gt_BB{Y zY0wv&6T!(wNf+5u-1E|8hwtOqlY-ed`N$m0@U~k!c5^SKt?jw{fwcR%r_#*!Isc4f zX}7)HW9dD+oWs9EBW?^VGw4$(6J*woR2lrhZ=72#6s8veFX#JO#*%DVHUf|zM&*p4 zaz^9@Z1O00)o_mPBRR~5e%n)~1J-i~U6=ri3jdo2d=k*yLVIVeYzYzTF<+nMf z+iJ$C5J8p=iVRg1fp$_d?J>ReMDI)wm#o~r{N`Q9`QrB3=a{?2@|?W&y)B^HHq&;? zK5bNQzb?`TlOL6%I8WrJ^t*PmLkX$k*9#b{IB_cL4zz&Bqy7fEOJ@QJ)g@)gqhiUs zs{RBFuRZ+OP=3CV|!7CjP#wiX-{nhsp+CJ$=`1-qN{cBagrW9_1x?N3HIv6CT*PA6C}DjtZ#?4q1WE2y4& z6n0|3*}~EdGuCJC1{>=$bb~dEZs2+hX<*GbrsDos?WLN^L5mf^3ON3NAWL5?|5T_6 zjDLx+qsdsCAqZ@&&4)5np~8Q(bCKs}Y2+G=EN!2x=M*Miq@s#E{N-y*b|`6)NN6kZ z#(#L0k}6EP9Q|nZ{#dy!G&6JMy%zy>M~@?yVq4+$bH$Eb{%00zA5#C?sm0h{`)`-G zubJ68rycO<*O3KKdV&wwWyH;RKsI-Ldn3|#6De)g@;AlG!IMT`RspN4?G`6;6Z3}t zZf~dBTi*r*ckdIMM+v+gBr}N<>1OtXYFsA%xyqhFD=bWZkP18x4h~mrpRMN|WNX9` z`-RKNBdq5F*a30_xCwXy=%muz!=iBI!jyi6u5PZyI0z1xKU(QinG^bwYXt;{rFZJP z%q)K|S&fPovEgugw$>m164u@loKitz`!HcKr{E z5T!smy6v?lprNhM;^m9^ywtwJ(FZ&tu&CDZ#F#6wQZH; zoIlu5^|@JUElzxrPHYf_FG4|W|pHf?SCXoQ^dF} z$G<1}^;Woo0O#FfL+jgsz&%@)3ZOFlGJFzJq06qh-4)YOwB}ohc%C4%Jzr1r;X_OJ zgF<4Mo&(DR{?!Vwpw5_#aJ(QiK!Thk$s=d$=Lrm&voNV|I{S<@#2u{{izsdsJDY1 zx2PblR*=xfHKo%Xj)15}q*C;{+Qv13fT&cYQmCDC;Cu)8^iI&j2PWLAm}W?*JNjjk)S*Bri+Wggo%6YCq-gh;b)=?qb#nG3MM>mo>sy47{Q4eusp4P3 za_wQs?2e(dPVA00vX*d7nAV|xNA{0r-D!WUH`w#^L+8=4=Z8<&53iGr(?c#JU_=p7 zFsoYu{nv*f-gFBjO|OaP%dh~(YV9=@AP3v}spNz9>$(nwDO=+1R*!Hr?A-o>&BqmF6`nXVV5?oYV9BfN+ z>8%D;fsTEi;56q}c|sSK_4ezpw=a9y=D!Is_7{E2$oO9W^+yPKc>QbrH!u9ql7qxu zgUkz_1dx6W61^%h9-Isr+t~78J8K@F%pca&*uHzbQZVK#LzmP5l@}Oe`(-+?WBIRg zJe6)|>Fn+eoUq{AEPg>@pZ|a}^9nBDIvlf~$}lUyUgZqbM3+*(QI@~YPPMqN@ITck zEZ8tbVk?4s{m7K@t;KB%o`L-u1P3RLN^zXQWa1{4PZcsrUGmGQ92hpve7K=wmV_SXxXkv||0%;Y3dh0Up1$Z> zFpA$)Tm$y$NsjU#Oavb>Ui|n!l*W*U#*ioWBN^Qf=~p=_Pp$q74gD$dH(dsE#E1;O z1#IsO_H%#F3^A;4ejJ~#XtFLD{v*j-cnW7Bc^;DE!dX^SV-s}Ajgo2!mV0j$46+L4 zxWS=$Y=4y-izWXjG}oRz1IHN67;=)VH1@2gWl9eheXXK5(+rQO4a4+1WQ?!(e<~%B zPyWW38d&@KQi2TUBOb+b>af+<6hX8Q!Dw&M|A>k=|9>P|G&D~sYd6Lc_5T08<~aKI za?W*7uSnOwGT(ci#3)WL`y=#?Y1aQ<{SvgH3EGkW8R%c--)ZB;roWD(InniPBqu6K@A{`P`rbf4$XK2PaHwE%(Wv9sPCs?v9aeaAr1GSSy> zuTclMa+j1fQrd+B!UP9H^L5WiYm*+yBy738jB0h2I)q#&)ehqShSF-)E@ zYMwD_!sy_1g{=2)7()X@gzhBsC_M3+Nqv~rA`xNJ>=(xB((PM1Yo0N#g}vgA@0BTD z5y(94ivESk-u)LnnCvos?~gkMPCCLT%L2n0fKc}1lNX{bzj}zTufLZR#NJ##r8Zp2 z){Je-`aPDyDSB7#JOjEsIq1wTHw&VQP!N5JZ#2uabXl0$!iy$(*-SOnL zC{?>wl!~v{b>Vl>qMk0L>+4t4Vyo*6Uh1O`>OaCUiUP}yRX%m7d}3Q_@6o$<$E3Dj zU1#-b4vhLl_U+%ua2>y*n!)UZNWo=z6#imJhG$5I6N4s4hJ9&XBa(Y?VKpNV*UV?d4Vi@YmV0v{E&7@B{V)`u z+Nkz@dmjQ)`C%fx_dY99e!UK7=grth`Cd2y=DFaB79w(qz3iHMCVc0f0wNZ?RK@|> zedC0rl5QJX?HAmcf;y3AV?_^KaNSVBdkOk*&-FIy6;5dpvZWLFB=_&6McJ-)yrE0U zV+>r4rsuQJ7FB+ynly%1Q$>D{21U07v;rn9R$wiIyN?6cOuTZr9*(73GLY?TzUf#PkR>SZjI;Y)qI{`h2G0aw>Z-bA~U{ zu|V%*MMX<%5iWZwe4nMyCR};$q2FdmvWtIL5Q{ucCO=Ox-F=C$sLy(y1TmG$iziG| zcQU)(@F`_hxOw6|S}&yy2@Uy%jX@Fe9h39;Esl=z7k2!(%$g+{A8gz`skL|RBvgJC z95_^gJzs@|aq#Pd<<500)XR9}5nVsNia)neUinC%A0ke{EJ0X+G5MCfNCIc#Te33d zp2RcDmk}A9+w%N6Il#b1^2IuACsg&B+~C^VjY16R(%lZL-%a@bPf?#gA{;<#@c} zZ+ImzYJX`mj)v2iK|)#mE-865p470G-S2mMk2DJ zfhy(CxCQqUwUL*TEcwyJvIm-#QzSMGEgZBu-nm_;uCssTf%`?2f3dGsOPxnYU*Vwe zD_dox@K-g+sLXhy9>lDyX0KV3!8_sC=#aUT#TN7g5>58?|6q0}EaDS=dt!_Guj6y0M_Nn^$S90Wgn<3b29yNFj~l-ZJZw z@9nYLxxt(~C8&4M?~ORhmklnP27KIRkJxS(&03q3emzfo8T0L?QSB?U@GGS@?nE5G zidEvPsufaqBJ*d{JEG_#9he-%Tf0&fATYBlWwl@JC{AfGd4>pd;;36%Tof9_`R9C$ z`^*3MwJ=uHFKrU^sCE+$Dym?HuTSLzzWnnY;dqudbp+Yok9B@7R_ zvuY5(ORd*uOM63m{(|aIYWQ=}4ZJr{8dsl_?yv%Nimvr81)%4dz>oKZtBK+K)8rZz@}?@6q(@WF)8%gNM}G4_ zLKgtOX>4a!U5E9tD)T>Z%ZzaH<#eN2<(LpZZ5p69NjPHJ*B_5-(hN7nG~cRMzmD%E#48u~d#2*>4vnhwT{xal$lfV0H;{OzgNIcD-RIlzopM9>?D=D<=&xdwRIVNLSVN+L-gnPJ{cY{+%1!k`Qq#jNIo@iURBz z8;wyPhB)<3Gd;GYr{1emsY9>7Y+H!Q9Yl&)JJ#rX>Z|j#5GX)v*1Qhoi4QyMH*F{O z-V64zty&C%e|nz>x8=X-%Y@{o(zE>>@9KiCpXH=vniA0ZqoG9Tr_07W*dEx?T00-69X!>)j-77RQSp&Nbs4Y0urERN*@(j3Uyz2M%2t zu~j32%4A-^uNJXhd?dX|ZCOOD*@H8AX0KQWtVk``l2%H`Dxn4ya(9aRK_U*{eMI|ou1;W6b3U^@_4oq%$a?&=GqA6h1 ztV4CcQg7No_hu?wo0eGn+}7-4I8k)bn%f@l&T|7vOdiUuHrJ)2Z?Ip&8k}Y_9;`-o zZQEi*fGT|bvsdO*KarwsgSt+OH4TSvjrsM<25JstQY1GWCRy{lWU6Nr!sq^uYr+~9 zz&2vFUoU?k@yb#65J!)${fMqU;zY3GXVdXq-1IxiE?5>PURff^`WsRZ9%=FGqxj(E z0({|Cf|~bqUBbaQ6kx0?>0>K*0jJl+d%)90;Fdve$ZZXEY288fyn;}|5+#IJAm&Yt zB(Asi!YkkJ;==bh!XxvETEryn{aDsV#&P?bibp1jwQlkAQVz=0 zwYMJ)chIkXy(W=u)gfGd6`Qk7AdcHL_5qqmxQbK9U@lcZa1g4)?wG&4qS)Facpm_U zU?Zy|yfl=yk{=X>Twi&zht!GeM#b}&Yq6FK$ z4~xe+RL+~rla$V1PNaV@Y~vMhfTSih&ZE&xAluv_Una>hdh>XXE1YCA-;;S8BeG%- zwE~h06ld=`|CvdwfE&0vQ_Ep#Adkc<;bFyO*>Z4oNau=4>`>I@%e)}cP3Vo;pZR*Z z313%(*JBBaBh?1Y7!W*}b*$%_#!_IK<|2}jS^PN1ek+J*SLy?Wsfz9Ib;m80w}AGX zNyl!s@->R|T=Ew@T%dxAsFUwPaarA>D6B)(Fi{*wx5r(NlXI3d{1V@dn*eaf_NPT^ z4=Vlj#}k3qN{>UgO0hTW%WkD7=!b6ZThJ>VQ=M5FYYht@o?j@&z&mKGeA=Mmtql3iDAC) z-LZVRih%xP=LMQ;O(zJ`*U~n^WKGsBT|G%=$SYl9j>i=Yh2mG+$xeqZpcv#(&>kE? zv85m519D~syQ9FQ#XtvxRQq2yL8t4>obt|ZPj5Gx>YACZBZDN8vNg}AJ zYP@Km3f{vrov-+8k9)(5)>0luKVc;c+nTTvH^)qiAzqqJrb+R5?zYc=)@x41xwRU2 zRm*vl66AKw@7!ySQ?XL`k`(B95O(VYxolgESp}!tspxZp)YMT*)6bixwjb`t8^6}2 zGps!x&SgyAe7YY+t=Irm4q9uZcMdG{FMR72ExqFlPJInn&YiwENIk+X+GDQut|=a~ zdGNP_u3MYz+6#Q*KlJT0{B)&Vo1iPIiDLkCN!ApbdeAGJ?~1E~KC6RlrMPZG3l>A9 z!J*c+C6;-K1+H;~1397cA|ck;q1GiO`|_{cHuMjBXBVQYN|Kw)a(%lg`;0Y9>RE;w zScc%JSiTIP#2)we?q>T>h`b4qG-7K4)K(O_wjeC5Rf^RzZIN3LG&dx@%PXaEhfz|F zQKQ9lXP22PO?(2%PN|=(mw@s;^n%<{J<;!>&7|y3;lQv{clx`$#;QlD5)Gr06`>!z zfw{}gdgoDY^ol-#Cw@Qzsu#<4burnp^&8HCuS-sHSi9`61q}5W>?WG0` zm=E7Xg@d}DpA^;0d>~|Tzj`7T)3w|w%_;T%CW#iqfng-n`}{ggrKf;)iu5AFxeJmV zF{ZiCRmx}g#uGXZ9bWr3Ho3#>wT_b=%*cx(x$m6rer*!O$Z3A6Ht!ykF4%43vqx4& zQbu+G#f9=nKRfd}d%7-s_UX)c{b^Or>|A|s^THqx8?P^Di!>ooD@BXEmDkrEXrXUi z%QH`=m3S~j9eA%*q14+9VM7heKhNHFq@?GC+EEzRNzA4Z?08kKE?p zp9h4KEGT~1ZeB=7H-B;E4%@V?Y;sh;jP{OzQZOFz1X9q+B-K)|%B09D#*8b-u*G63 zXtgjcbDNw}-bC*9(K+RkCKPYdSLH1*;U-3yg-&dZz(eB9fTm`87D-y#2|5W$l<>AG z?K5*ECz7?Q?xM7&J=NUXsDv}UW=h9%a2x(fF2!DYAq3<@bwZPNBbDQnQ`8_>DOx$J zPd-aFYe!GpMAJ0*y?3b>=&YB+QMr7_?DW>DcL7h}jezlCI7N7Da$q!mH2#*~XPunE z4`Hd5v|HkOvpVLX2D7C$=KDpiV6}Ok1C;7q#J=I@>4)5VIh~P~nFko$-q7Cd$M$U9 zJZ^9Je(=7Gsl}~|XY;g5%Y8J3iLZ?>4Ry6x6<=@gfr6{5VuhNktf-ZJRqIcs+1vf< zR6FY0fz1zeu}p@lvOKY6MO%BHZnVImjq;^hS_Leos`r}rigudQMXpXM)@5@827C12 zn1vEMO}+tE%;LybiZT%8<9A@H}BTd`54c7$^FCKEWlahYq9)*5bB*fZfipou?; zHJ}2RlrJuVNQ;k`oxv$;KC)I>7sTxWkRNI^ZL5dX^d|6-33~;CRy7)% zvU+?1L$EDPne7kxx!~e6=WXHQD(6h+%*n2XPAgk5m<`NUb?8v)P+Hz7)aq7R1L3z# z<<(TmhdCp_3C`K$ZF>g`dkfYV;6@>l5}#p_;Xgn>0PUnk*x*x`7ZSKFy8Q%{2$=Gi zAlrqyQPoKc;lo0wyoU)a!oaP7p(HbrmFA6Rn+RtjeoS~zYT)oEP_uLA%^YMwV2h%P zp1%jSKGn(SH8IZFGX3V@rMvVp{j`5D6V|&wlROjME0=a^ zE&Xr^TQ?wNIu?YuNkw|PdEOrs@Qsq&Xq^T@I`)f}UfKs3ERt`E*lfG6H@|X~u>Z-| zXSb?wl2$!(n&&YxyY0^I&OSrzan?M&AC8zt@Ex!;yEU6#PA+*ad0v8-ZkKLd$&VzC z)S61J8tR4*SLm7p7H8*o=nm!)u~%`6L-VVYm5&?V2b6We5C(*kXKX?!rlwFK}|Zjypfn z)zg(;ZU|M1RH`BFOb=74BJNI)6tiUy8wne!e0<}}`_OzZ?PYgIcMJOfU>N|WY48*W z-Ro$=;E>B?_-}a3rJzycj@!Ftmg0rC!*8&=e0S4X?~V=%>=Slj z(%dBOmNmp2iNP{VyvN<@rrykQ1H4_a(WTzp_8ln%K+kxDZ?exQUmbbpo08LRJkP#c z5lbb$;6-vpig0;Y;BDWR9u1$Q6bp1DrXvsLrVU3s6iZ!~qlQix?wohPx>83?;Amj( zm3p(#DPj!wqu+yU*>ulE4NX4+#6;$e^xEn{8}D&eVItF~jUD78RFwN~_e_$Y-ngg2xI6M>XufZ)Z&&_}qlA2Q04}FXq8MHcNyT zs>>dHD?&%2_g3D=Wg6P8#<{x+!eTP^sOV_9)|xuKP{aGr2h-;RZIM&dj~fEsb@xkO z!OuaLo))}!0aN-(KJSGgx9L|^59>;nBV=RtL5?83~>9z5JbU z0WN-9ZLlY^_GC}TY2oTF3iS-}iQzl+;n~4JD_9sfLG!RoQ;O^QSxVfxegD0S&hz7;hc3JV^xA$~)S0>8Tu5}SEQ*D`v3 zF%fghz&z|*B)0nSShBWwwSLuSPbnsi)Xge+8LMn3HR}m^C31Iz-m~nzb&93*Atb61 zVVCc@gJiKDxdGmyhZwW(i6Pm+YkFOI^#;O>RNI!{6G`s)0Bby&ho4Sx_8gM5-?6=X zEOLm}X5tMC?=r8D(Py|M4cf7E0W^k+^P|K<+WZoTC7uzpAM$vncy~G6xvOrZ=nWbe zSL;|3g;ki(3BT=rv7|3-9-o-I=2$iO0U!UVJsUmRlD>R*N>8Wy>_^_$08Bi!`<9X^Q(;!Z1cGVc2K%G)0P+!Z3b3cfshk@d&=>c6OCU#f8>Ec-X=) z6{ces>CIo`q#6_YJR=1}?t3C6RdvTA zQjyd>MeOW+wfREOx$B+~c0`LJety!}a>GI&58CS7A$QQqa5rg(gT;CTp5G&vG+J6O z386a;tr#yCZ(qHbgJ8EyS*6yv_7-9bVSYC`ze&lZ`2yIXP3?|t;=IBGE%BS?Xu)U> zst`FQ>n{BTBDsB5W<-I}9OTGN? z)gmVyjdYa}i`O}&z}&~|@HNjM zY)esPe4z_cLS8y8xC$vDEuG$5g_Ka1PFv^k))df3KhmUVfKF3KT%txK*bfMIKn(3am#V|8b~5`La}Mr1j%-g5w%xwfC$qB?9gpSm~w< zk&IT^b1!q}ifY?c4rUk>W*GQJYekyq$PVMRkBML7Y1NfmRL%fklC+aLi)_q=Y(?Rg zd5x9fMFC9(vi0Fuu49YUd)X*8i*IC~``+?=PP8s3YiE?0RED%9z=%xpBXTAgdBRS; z;*{>k_7U#&SJ0IsrU}w~Dzk;23HM<|b$q6z9QrZq%4B(1p*0!Z_@s+Dm&8y;z&+E# zSKr?wHasbs&Jz^<+eHoM0HvYqt*15Leojw(nrkTqZi{|UwRf^uQk6^GR;`KktdwUp z(PWuqHovVvK}DyTHS^{&v}3GeWXI--F5)Z;YKsjPVit>jR9qHUTf%V)Xbv~TH^cLa zvYP>g?M@*5HvmW z+Xw45nOcn2kXyukR%yZ|j?=!U=tVffc#q-~M`asy1d5D;^hny4b+TkC7P_ShG9Y47 zSQ}uWh?nePq^Z3uFnLB_rIq55x9Ow1uS=*r|xYp`b~|zR2y6 zIdK|^Gk-bzZr z>Cw@kDSeE~@A%q}hUx{v7!$g(Oxn87B_$mQ@uH=2t%@@O^Q(UD`#?ie*?Tu~mBL;7{gdD1y^`3~#?~;x&F*KC^N1T& zektip|5<60CC>m4rpSonkW{*_>%A2BdtL&Jip&stUV^c&@YcW?2d)PKa|uEle(W2I zHh+w>3^XFI7dJP?uZ|V`5ZE^LPjBK@K1ww|qf-CnPwmgi=Wj?T>RSfDWghg$PC_T= z#1%|JCkNnm190g%aOt^l$y8$r==6WTwR-JPua5B{#_Jc}@0Tl*R#^^yboK;XRXNW- zc%c8#=K8MlyX^yMUn&U$bu4Ze+Mqw@pnt__)M*q=7%7Kt1rsmcv%~RdH)h4LkEA(R zZ~FR6ycozG4e_^H=W#Z5&D)nRe|uxh*X0v)Pyf~T zcBtSBg@uVLsB?eI)K%_zQ$wH;dnNIc@Pn=b-~4;U73Evyx16wH`E4xD#h14)V;3;< z#uL|FJOdF=ZSP&h_NF-u{HNuPLcA`;tLZ{9upKOb>ujk~fD)kK@WTr{rJ zGS}^L^kcBM$d7s@^ebDbH0`o?RX|#E5r!(Di8;dE9P!*7A!#ns@lbNFS50#W>Rccm zSx6vRYl&Im4-^<4r89UV`=cb47Dm{SCSNRzftyWa^!$O{>%t*aI z`;2w=nS$g!nuqZZZ7mZmlZkhSA{=dcJ-J|h=bn^jYu!@#l!%*$%sd)*#* z%x8uj{e%k>7^KzJV9(d)CL-#_SsT7fJX(K5ikheWU5=Zl|FNvKCFj+a=9=2p1b}TN z$hjrRebu>Z1XT1 zg>JnE5d#fzCIvgE=a*z4=)Skev<7o}xz{|~=2}Kc7)OmckHevqm zXmKUJPJ6k6IAAM&{lXn6Ib-2ZzGyt_7t>2WVHULU^?Bvj%GQeDdv`DX1mV%Qj4|Qr zSxJ739tXRXdkIqrh7urp+Mr*N_9Sa}6t!w+=jHnF@E zT!eCoZukJ7Q^eSj(=zhg4VR7u{krk(DZL686XhH+OTLS}Vfe9}v60`_g=)*LC=+TO@<fH6%JwK9=^*S-UcE$8$Bc_AH3bSyqoX3iLKte(Cw zkVE!5L(gW^g;gS`O|*MDAA2Hz-iDS--4=(f`e(+&heDY>qAzlfIC)+l%g*&nah9hB z!=kSj>yIhHc`fC?VlLdOsdvc6!FvkcZATgSqQ+nW`zD?lzvvrs^Wc1G?}^_> zg$2g1Gy1=qUI$tRMXWwUEZYl2MO*L9=s-_cy{Dq*5oXh*uglegb0iV7C7;0E)i-mS z&HBM#UbQ&z_WOgww=d90#R}CvL`QG3@a!W!%bKV3SuBHe?6weFyb!;J+=Y9RKGOS)N&YIN zp7)g-^`i$>=7um{K=e2&Fq3N7r372EL-&Pkfqk>`*?^q@LfxETcv4x~d+ZbB+dt16 z+N5!%tOF)1FIM7z4h3&pzfSorb}aiLwaZpJ_BY&Yu5{vyZu>c1+g`kMy?bZWjflAI zdFud9jUQs#;g>0IA9}xmjec|C*>uW2>1+TF$gH|2j_-Q~KUkObvDp;ZkDJ6%jq2Q* z$vO$B1txhV6cIiB`;QOV7iv9Pssk{rUQ9&Q*|uSq-EWHmxj68@#mBgIeT1)4&)7Ks zzFfy~AoOb}KI$41?3t;%)FJkl@WfvV?(DsD6P*>$T^VIJMt@4<4QK((;381(?Gol) zHJcvS4vz*n@qJH4sea!$yg$B9&1lX)j?V|sRtnI2*VG&RZu|Rg@Me@DbG%Xat^vc!5{QHDMF5 zU(O?TD5WzeCOimiyi=N0yQs0#OF7_r?UbbD**KrtcwTS&+3kN-CTc$-F>9a4%jCyY zNorIokYB!)?bL0`iY|tTH<3hzl(+c$O*r1GEbigYvvaNT%T1fc7_d)=b{n(z*#B1n zU$cA9f;WZ=`GN9xRUl4v&@B)pg2Sy=3sV^Y1q>F_>C;QQE?Gfn7o;#S0<9ZhKYXOl+YsAb$62 zi~fAaVasLfyZN-~&aW#G5p8mNGZBB|yA1|wq62p4Q9;kYl@tBU(PGRmN9Q`OGv0gt zmJ(AAq#*!)*ubV?;4==69-^AeYOIhR+~b@58XT`ds{v%rAM6N5w&%&?IQC3fJ@xlj z^6i%K2-Er*SML|G?TX>Qt2)zbI$a;IhWS^rnss>U2M?Y>twD@LBX3@wGY#ic_Klo9 ze+B|XHW$X|jpU6Cl>DU!wS1N{brg|x z)9U(`>n&`Lp`a)MJ8y;{N9F~eBZtZ6pttjptWloHY`6f%*C57WH_ywe6Y_~orspVS zs+d4?{i|$X{+930u&=IX{s6`dy}_zgTZ~3UFw8{ZYLq80aTlV)ns2Okn^yR&$%wND zD4fe#w2$jf_L21=A4OFD<8x=|63gqf{=@HQ{s;m{?8f{4xu>$ddm;phBCpRM2QpoG zwLMZ9XR<7`#H0W+yKPwW!6WbdPD$QbzppQ!P82P)mghv%xu|67-4V4waq7jP*J03M zAl%>Mp`!4hfHTZ(mwcO=sdDLI&e*;f)jF1^%tGxgcpbd3Q;Ipf;r4e^xhkdckfa{4 zc{(KXr0}FrAGLWpGzx8YBfpM1Pk~?=945K_S))@)C&w28wL=Z-W#40jO#;~1(|5j@ zj5|*_N7o)&LET*CjA?tRdy;lic5rqkK2lesilKf_)UJx#49>Wthtp8&>o9W$Hdl!# zZSx^{w@e&7p($jqOpUZ71^4k840plK326oag?Ny;PP?2}T8y<$JHdTQjMityRt47y z(^B!DQnM5C*Pv%Q+GKD}s+s>_iy@Izjk7wIK)1Yi@?s9RRGc=koa8b;&-ad;*XL*<@D{6pBz6>^2-t4F1Z6<0DVqz+`Ln0-MmvAL6>xupi8=q@hzp5<}4tIKyttq zhyd!DVllp|q)cbR_oNQ>Ofj&7*cpIUKqqu2-{YO!YFbQQ6lSPp`PzvEI>v)3Wck|J z1vN^3CUr;oMsL z{d@f)P%|D(HOk|QEl@L_aCUL)1+@Tm(y37H@I1LfjZ*TLI?uL1WnPTpi@vM zotSKoJ@f|DNjJL^aUyvr0P3Xs&?7HQNHn2&;CZ5TGf6p4ddYrmcu#y!6gJ5(QAsvg zCSRtXL>&uMGM2lvhuVXB=z8TZ?V$$G_Rns9Ae8N$|9Cl6c-maE=(msPHLJwZe4Cw7f3}&yHh7gx=9056-UWQ_!#u!4M^b9 zFfbAZ%RD@IJ$ZGYV_Qn6GM7#h&7hUPw{)$%!#|uH6%g8+87;=Dm3$eBf~D%AoFdI- zUD8gD6JltcSfy38`Mie~2iga0hu@x>7|X`Wrmd;-d2{jtfDXz~yGH3mm7HqIr>6Df z@v`Za-vK}erT@%IrhS*g5Bhvw1Ky{mb+_@dX)oHRruB|wm7Hqgr>6B9+I(IE6R2I| zmYk?#%ln9vMS|m(_NsL!>I|NZ3s8eKGgSu9h7qX2nplR-zGXp9K)uzMDsD_}qdSQs z$+aa3RAo*4^(heaz{{l|CExNZu~fvgTTUuKjY?mx2<9CZE6jI|ZlA-uGIQCWpo{kX zyy&a-ccYRpTGHLYnSoE_3m?8G10OJ91YF64Tqv;upQ@euP2jGME>2wQ81RgSNq6U* zc2&t!2SpyiY%-BaMTO;kCe(ZQbS)&G21mlo(32?IOXQNqRFLT9=XW@q&W)6~N4?fM87{VU z%sAD=eWv5Twt@Rp$Lz>v5yu=#H^!^{I^_Jdf_>F)KFclWp1Tj_2rj#`AG&5Dg z(O0v%F4$P@VrjnS5TJNd@w&97nb$fE?w!S*#k}TLl@p%Y8T}dkz+Wddv9C4DFa4(W zh9#1}V2M@9n62b7uc`DE+nT?#&$94zpj3dnEOLRW38~OF+&a={y0l%Y2d_f92*djO zbM2)*3vk2^eV*6&hxnTmtNf7dxIWHpZc!oO1}C%vXLM(%=>74FTX#wfleOP|iqzN}d?kA)T0al`)N&v!1imSr_? zBve_S?Wfm@U~z1Un%^=-yl+wTSj7Ik{{e!ib$;_kIAiCd5{Ybxf!$)_#QD$M5q65M{% zCXOK^n|=mX;H7%DxUFi6mEe4O6{0voARa%z-DTSYW-kaJ6?@$^72G0$@-Cwo>GR=dnZ_v(&4yyT81%XpEM z&AoQPlC!AIt94%WEd-7`bKBxi*p7@pdrDa)r(2u9>b`h8mX3uC=*B_4o4ut!dyM#( zfArjoDu~XBicdEKBg)&24HB*cN*u-W;(#)-E-CjMBVBqMENqLS^P*U!&8$GYrav;@ zD@lPEw_O?=QQ9xfZzmVO?rG10O9THr@7V4X&Tw21^u$4$|2qhve3H1JOjAU>DF2Q~kduCiYLYWS3-c$WQZJV$)exCb91Dw7F~0uE*7E!M zOjw-oJ*Y|)?@(2f02BA?nbSP46r!lqxU`hD!T&O>R>SZ_!z(CT;A=a4h(tdfqzR*#tXhjhuv zO44LWY~+$YDQw(K(sCh>`Th`TOTk%|iDuzTBP8LnUoV1>57V9_? z=YkZS6kWQfIH{Op0*3^dY9r}H4mU#}nM8h!UMRUfjV_)}H)mh&J8}~#bk2(axry`( z5Ch2(@plWH#0E^^#jiOpaa$pZ{~y!mmRt;_&3W-f+1do#1Q!Z8du_+QemedHa(3Mg zeHA~Zz>=N7q{S3GMp=KdIc6S{{Gh~qix8X(F29ldO#?y_pi(oGW>WP)@#9NG!R4$F zr_iLh(mv3hA8MO!Q{v`v;&Z}BkL@2Zb&XOp#f|)d-nxfZAycTvfLlD)hVw)@@@0wq zCz^C#XKbgcO(qB0)1FL)t&c;*-OBUDv^gSu2e0%xKhYS8p+JcR=9kh#vj zUSeix)C2MnrX}A|T~>V}2oa>{h{yuHrD5gq3oSSf{RHPH!J9KLFPs=hOTmox0aw+Q zwS%PCMjYuFgxf3Hp&Ybz;+Rk`+eB5(qF#_T8g01>SK~^xtaUNa3cn@%y`+=OZLiZ! z$xR76qm9ffkRxg2F0l7&^|=j_iBse-4_7HB23vZ%QWzT;J08^o%c)S4`oDE8}} zj&)mco5ETAX>@L9dab!U>b0VaG&-QH3r7w^=Jfn*MP{4%1C{pW@J$@*^~21j_z1cR zr38)dl$XB71;G2d2`Hp|Q0Gely5iZARL0Zg%tnhfrPVD#;}7MfMNCXkP*Cc`rR;PX z5-lvPn(7B()k~x9gH7Wlq>P4foPqu>zt9lnF1URG`%ElsIr#5_j9pkjM>2*(Srh&R z7P5y_Ps)P4;a|fLABwPp5HAI?{SdT6g+ql~m22_Ovao#aG~a$vhvW!g6ZCP2tCW9K zP+@ljhew(Xlg~zrMSTsX-!Nx*pHY3Slsr>j>5G^BJ!fdS$yhzW;Fj4XYomB^ixvR`$g6J!E*PRartZDSx{=?<*M#A zpY`xg&evcx=64_MSL8FbJBCE=K$VqSua#Sls#E`lzZpE}+1oQew`YEehG_Y#tbo0= z&p4`r*rj`CJUBsOM!8SzjE{(sezbpmgZlxZ~ zL!F6TFLq#Zu`aHERK+=<+W zdeL(#yh%3mz;$VsB%>khb!kPb|K0P#`ft3Z|34W=miYAyURXaY|7TXTCaqX~_+PLt zZf9i0|0YEJ&rkmW+7&k}jdf|3XrrO$hM3HWce9_YAK%j61`W^2@rK{cm|i08st?Kk zHU#V9zC={~U-PDKV-^wvEg2vdKna)-HQ0kR5_ zl`kdhD=Xlq&#=g$@p78lL!ws+mI{WKbDdcxIR>4ooS7oWySwoZjIZXe=F5(g1Y)!( z&nsibdpXUUEF2ep9Tt9iHo5C9nyGw--L;Plpt>b}KJE*qJc`=QJSK|w?EuT(6`y38 z0}S89o`bJW8UANL7w_BQI+ZdyuZ$rp)J}2%;x~+RfqvQJ{$eCBaN3VGqeZf37>87b zRa7t%@3W*R{(CXXB8Mj4rh^(__ouq)yD(0O4;^vl!5G33b$HY+0Ua#I6uNk{F%`dG z)b7b4krexzBo^v06C73q+J59A@3A8TqZeKb+YGi1vP+vg|J=dtr*36-m*4L2iCrf| zb@)bb_eJ(&#vbVFF*Pv$^E|s_vU_VsBatPMny%7PkVs*CH{Y<#m2wJ>`I1 z3`3}SW(&z+6Yr%I+Tv8(21AH?Sz9TgEQ=#neJUk{X97!L(`GXQJ6em4M~1LiRoxE$ zOQ~wWLg+zC3rT*HOaLbqZLmb@LUwVg_DJzaak^W_^&8Uf2WBd%ln7HBcr@z;96O5wmo4VnTG=sWMETcfJNJUNoy78mTlKktQYnYW9o|VE} zs+9W?84K#4>WayZ0Y4q2pKK!Kfz)*C@!MV$obr2PG>dCi=L24hqT~CIAh!c|IHH6t zn8o#krnu`;*Dtgn+{~XOoEX$9K_RT`?BCMfU>Eu@J_eR|jD#dZ5ST7UGv^{b}tI{%oz+K;=|(F$SwZ8$J>*NLi>;y#e8LZGVWAc-;y zb`d%8`+^08moj1HfBre^sv|aJq(UXO%%f!KcDuZN7Yil?jknj)3gPpb9S?YTu_;;{ z52T+P+Ty4$B!QSBO(e4zyioSYeuHF)Lb*xCBe*$VnS77;mZgXyxUuXw7c@&w6mFVsO2cj8TsTfoQEt4@f$mF2|W^`zEGiycd+V``K491xF&a@ zCgM@|(i`F_A|_(|f#xK|hePQ@?P8H=(Z|K3zemd|Ag_=v=7M7es7g9RcQgv=U-J;b zEqi`NF7V0Z6)1KACtQjsPR@$fCymPAv%|Tmn}VEd1C<#L3(DoOs%oZ(ZwyRgl#zCEci`sd*$6-cQql zdW$qmn%slZMm;`GTZ?A3Q?Z__!8zDqy6s;2&#i+mTIO|R&aGY(G81p7Je#t0CccX7 zsov6Ogusuwnv;%v%Iyr#=cXl>5#@r zpk_^(cF4vid%D%<=+6-53xjswtNsO;w2jT1NjHL1E|OWqZKV!KF0$)a6L3pRyD-~-UMtj9 z4k;hDImxdk8eAxNh0jE_c`|-xfn~R z{-3BPt>F$PjN$3hrxAHs4K#g2^%4G|7^JiEl;KNdXlFfIPikit6{wy1Xi%fYvdgjM zSrYxxv#Jcqn^?*IoCZ&aFe#a-FE|{_D94;=*-YjXjjOcI zdmUq>yRRpgT=kyzs-hG+1)c4p0lDH za5}4*>g_2&jI3)zLrU$Zz^I=yY7RJj2xNcX*mniRg5npHtUm@kxBq=}g2wZ`;HRcK z@gQz>7ZpXj3Lntyo<%eEHGf_FMa*RVsa10d;j%o_2BS!O`MvW&o29&l=_>t_a|_`i z?UE~E^UZ45e0ML}DmGP2yMBR0!oG48KbB3z_NRA#m9E84TDQ~wGx_f<1O|RXB*sNL zRTi7@v7i8&;{&D^SR7_Am>lhMxRxq@TZ}U8WsP5>N%^XN6Srw4IwUaGS|o^G|HP5g zbX1sqhAg;b(><{Zz|EuOWXI zYU+z*a!r$?fr{s+K7C4i@b)&qKQ8uV6l0SKCbYAv&`Jy`c#yg7X;?!uQz}!t|Nw?A!cN4a*$kuss~YgFr7#dz9g{cby#(le=xz zXFDPjJHDNEWTsE7d%Pn2kTtrQ?Tt=(7^B_|hD~xBp4+vcAm4`1vKk)!qPL@0nX+5YvU=I)sLAXd|JwX*5BcU&cajAS38B6unMExZYYh12H^$f-_Jy`H^pJAu zrVHEl?DA4Cvc6l@7&hRKt64k#4q6&HyS{o5xwXnnr@wDHygnW)JNYy}Z*1iR{ihxF zVEDG4SLEFzsLij92^-jl)ZQ=rS{?if17Xb`c%Rh7 zA71S*{ws(L(aNdpwK1B;f9je8vV~JjDOp)c!8{I{Olv) z(UUo>$foFQp}6yd-0Zyg&i>^#`{2OokT*iaC*Y^8o~eJ9 z3p834!;%34bM$kcwd{rm1AWsFqXT;``;vN$`w6?4!jAg?otrOucW0>!&#dk!7G{+* zwfYn}f06_nyJ<(;NJn^sZ$Vh^wP+T8 z<{=LC(n9O)dV8`>b}?{8>kNR;7UBz;(Z#F`<6iHs2qbo@5OH0tg6*Kta|yj;z#w7Avs`;k8u zjJ;|TJA3!UKjeZoYJ)aQuzyouGf7FZ|E!ykU||?fDU7j4IPe@h+6C#>ii=bp5$XK6 z06?AsDyupI!S9bjVmpv-ZM!wAoV(aRS{u1GT{r2D{ao|Dqb=uI7>I>aXsLR4Q{mJt z5rB3VOi6#dfxy`DMiSAfOp(gGAO0+Wh{6lSjtmTPtA?(>S5YFJ-AAe-DPSec{B4bs zX_Wl+#^D^|2{V)~1S6d+b7xnd-;8(Z;q10gpEWL7F-^?>b!p{k@E0SQnEmTs-m_QC zYsHuiZKcFo&-(0)GtXL(Q{bqf7}Lrc5N3CSvS$#x>lqIDrd*2+u)&4&TwyoP5@Xcb z3S7K2@nPuYrpqR={6Y4|l(l;`2A8YzwkO;KC@@W!%NZSl&mC8X=rt2kk^&`5LKN?T*^Xl*1^ zK({#b6#B!LDrxW!TA?^@aMvW_Bsez`gmMP}N{Sffg0P+XpfDXF~~7~9P7ofYv? zv|Slf;_tuT&~+mP_*hQH32lDn!&FbZkm(&gBxVv zOSSX6Eeojx`)sW=oWvLj8K6l2=d{H{UkPKN$h1rQK&|?mmEOddFpbSz6kq z)qO0qz(0-jy6`gM93h`W0=%ibwgEi<5tX_2qleq{;1@fW`ATMD_x@y5N#kn3Qxc2U zgb~iTmGfgX6@os-9Nn`51zsY<;PyaQ^|0-Id@YOm)XWPLb?G^Fe9L*XvyhaCX|HV^|mFw6l!T7=f)Xu~`Ba7>~ia+DZwNQXb! z*Tfvb_WWVP!5>&hno%66Hk@j$%9HGZL+=gM{X#fUMNGKw7g(upj3rQMe}v2|{;;W7zO6BLQk{kssXF?JMSccD z+CG-2f!f4C-aZxgZ`i1GGgBXhsAe$Dwb?KRnp6qCrHR)_0!xuoKGX3PYJWcuMvKdPrJVJZ~3qBX->DWSvD`cJTv zs4e0jcJ_gtrwOQM$YW`Q?}xc;V3o0(CwHi}P`2codfgz=eu*3+KBs~RshjZE37=gj z6IluXCFvzn|5h<+y-$lrQ86EYKJW4*VtFnqK18+FdR}UcN367ei9PATwc~>3s8pAX0qJ@B3I{1snv=)W5bHv9AoaD; zLSFp(qmx~a1;&c&4JoR<@1y#tTbGvpsqUW}L{iLRYEF~|R~g>VO0H_k$k_*M0<4*2 zI)_uH+QJOS@a_j#jtERE2&kshrPUr|*%C9Dc%%MJS^z_M9bq^5I@}e*FW`f~{R&gx zE)^y;?P1JGtNyb`wRTQjeEeZ^LpK$ZI!fHcR>Yir`XCkTI(*U?9U!aUsE5u{vKNN$IKO{)Ez;`!ulr*KN)|F zWPtW|wcY~$oFMX>2>!ZF`n?Q>DXuBhLR1I2_m9Id0tM#npte_jw&G-{+rO8YtIRMh zCl2~IfYEp2GlF^S#^~FBuyT;tLIER0wH$+;Jgl?u5IgV$oHW$oD(ECv7(W31zgzsH z9M&=-tlFTQvBz>LPCLKB!^-aM6rM(1MUJ^NQJGg!r~F$B6yo<}W+P(@{O)ZX#_M_X z$C~75X26FTkO3Db#RoYBZ(^Icm3TX$#|btFGE3mZZ_YTXHWW0{EluStxqz_rgc5zE z+E7ZmViw zbN%v~Q;a2gt`c{D_GM;z)ryq!mSHX@cV`ks_y_HySvNZI zA|K@r)%Pq(7x)bPE%6bj`?qd5m^q!=;ch4D5cTW(lWtPobT2M5*3Vbwdd~j+;4hW9 zUI?lVopnN{vCAmcA*s6Eeeet7F(FgB*)>KXxpTP%pP?3U65mvDOTQ?bmjOR(TB_jvcxT_bjC0~9ivji*jJh42<=Jy zia|%pfOTtAX~Vu6ibr0Bg_+X`Q9UnlfeR93*hIONS*UJ?(KSQ%hJ`CQh8r~>98ll2 zJkPDeCV+qIGr=NmF79)AsM;dn$L9oUz!ukY`0Jap@H)++#YR~AE1OdBn$2P=j{d!n z4#`@J?H&?XHnL+!oswZbPYY>ZV@WmqmAnBrSub$Gcn|FR>CkUX?n3}*;JYlfnA9#- zCD3o?gYXP%SEu<;8?}3Zw{O=ZgB$$ANA5MU5hLtv+HPH~$}< z+2@7bhcAm4_1#)8jpyC|l@G_jmxq-L_T3*3ydLZp-MzX7cN>WJycg`vLBwzqogdH_}@_b;pHH26v6=zqJKbN&qF})b7)=oS~X(N5y_@3@3xA3>%v?8W$V%;i|!k=d%m zz$k)S)}_#Q>N81>rG%jP{S(MM`y9`0!jkoEZ#nK6?XCZr)frE8(G{OE-Rm~Zo#3+K zwI;7s*}f*$vLxx>pB2|K01_lU@2gsDdU|_>BIsc*VDb2=bpA47VGct#Br` zMsKa?<=o_XjTW?iY~EX4LLPKR|B!mKdBr33{JB_}V_f>)FY;Ij<=A_U)PShgu zz3DnIjJS0k4tIV;J3;x6d5rXG4P1EZn@?q z<$hmi6ZhVKs6TN!aE@j6*lhb5AoV~~49YmcOkPYjDeldA^@AGRn$Y9Z1Or5KUP~>r z$bQ_@kr&~od~iOor>H0Q<^`~vmVzL(nDpJzA12Est0}>m0hEkFzT8h`@&b>61tp=N zUKM~mFgt+j$w}Q8TATdAcuJ}fVn?Wz~Itddxhw@Jy0UAB5JIxL) zpm9Xje*q_X9p3OykpYIHw3y>ItiBM4B8y`I1pm zQ07O+bj$?g)8x|_G6LZ4truEK*b9ZBT8g5+HU~I!@3>5Hu_P;U9igEMUr)2SX}1!c zbLnibL1RyDNli2vV^(gZT}Cx&;6s^qy_ShwmPs-JiU?5V zCiBB29F*gUroTX#B|k;>ki*vykXUw@b+>PuY;!2!``y?$MeC_B%qw#xYCY+4%ndZK z97j0|DrbUXu9gyp}J;bwaL+^}Itd zd_w+RG%g6a0bxtBH>71p_5(8zPXzXfL|S9qmbsa*tv0kv?0vK^u%bM+wyGM3BHTJB+>mTVwNkD2e zQMocxs27a)$h?#3taFiAS&uZSlu6lhMbIre)YFFf+bKd*1W~C>QXElnP&C2M+-Kq; zwcsCbtk~tf#dL{zYqjt!s*#o&Nfk)VDXlk#e`m85l5)C|j(M~)nG((aacWVLKPAi2 zLUx{%v=?7&Ysak{3NbP_I&IU|f(YL;XouW?sr3<uxJ#e2)*XNr$H*#FJX zIuqHrpW8;k7E@V3uFzx&%kkq>Jw28AtK6`9g(jPp{IYu95(SSut9lNNoU(kFR3|E}42L;5PS^KyJyg;BP;Y2rhe8{mR3#jt`!pJrc8twDORfEm zR}T#^Kg_4lHWsgXbu>TBm%^ct9$FETcIeJrd&V;d=To*AOE|nW4y}mEI=tPOg1MQf z@81y2cRoU%3DV%a5?DKvb?@`POx95C$Oa_o-td}Fn^3KE<>}sJ^k)n8BX*RtUBSGj z4e3PktYX{O6u*Z)qh298EaE`}hx~`K!BISXsyL>jo#paBu>+THc@|eyi!0WbVtcF` za?T(9FP2<>vQKHC#J!qLF6DC{+mUlbx@YbcZ-5tkO{;+ndmfuOs zMALE0SewB5ZN&G9Oc8JCdDh8OCq0jL5%_;X6gDt!BFb*YyLzcHA`e3aLNqd)9PPIp zWBHRJ55r{MwT5?C<@dtL;I7a0vs)@IYa$e6p5!US+`I0ZeC?+kawaNmDif@iDUWh4 zKoN%4KyDX->sWtX64(5cKlGHwE(vsJMwr*h{)%3r;XcD6@4_z4|73^lFOgj*MBWrP zU1m+&2=3s+eI`X-4BHL>ILBM4DrV!|v3uCvf&0<^cfPxB6=lPSmk|Wm&^dUS1mwZe ze>PY3Tc^ayRRCVbVT<$y;Zx#0lz-Iff9b)+yV2$Na-;$P!_WYg5KRzc5G*ZVp-lCN zF&H)t@b7rUOQl&8T;Y#$2l!kuzj%wAEyAku>ET6yf_Zxy`xuL6{UCsD7tl4E!xYg? zSdu$hEQ}8e2vi@#2|{V>;omLbGl=%xpM`r$&{(%MAh=p;{0& zx7^#yAZ>q9++*S6QnlIPvTsN2EOGudqJNNQL4ISa-POoWCbPrHHHHUmImXs;VNU+T zcft+U^x$J24)!RH4zOo++(nU&(GJ}Jtw}7S)EZ(IK>9G9bS82(b_M8rJsZ0Rj0m5O z-2hy1{bXIxTpX^A`!19Lv&jhlEq0nJ;FS@`mA=vd>;J z;a&pb>EHR5~{1nev8cE{J zKpStMK-iA)y4O~$DR`825l>sXQeU)`0$z)7W$6z>4f_2yel2e2K~+lVp0rr58)ZWC zDt;_&ZlH>JBw&N$l5$CH+4HOQ3W7uOnCk3!Rml@1-$Z`a*LA9>!7qF~qbP0sBEp9G zel(>fJQG0sYvY1NU1A2CR)(MC0r7d4MZ+&IO+oBwOZGZpo9);uZxlzA%tS}Vnd=n% zS=1Ped9(*Fdag%4P&T9grV9~>d1(rd6K)4EbBNLLyNS?C=&56}g+clW_=&smRwMm} zJ->H{hqx09@6gn?mnbLM_vy{p<*$26cPEXR7_KfE%w%%P_@F^CR#W{VK)8K&GiFx( zgz7Rl0wLgVK^1=V(coM@)0)mwwmpK2j^diOqh-Vyj>AIx@dQKWpki={*l^sdol~*c zV&48Nr9jq92B?*vZ}EmM`G&>~*D%8vvFBL%=GdZV=t6(;0^4_O!UBU%4)o3MG6X#E zlqs}m^81F=JHD6Z+FzaQS@Iq2K>8K_`Do@LBgvlMSy6lzC%QarmxlPEIRiT-)DWtmlrJ6ac&irzVU(*OhbRj%=$Wi! zf0$up-yCAs=v{r~&j=iz(K2C-7eU^A-{--*7CULYDXi_vyVpwU2oVD+_555==?>^z z*I5Nz+S|@Td76)(^}pda<39p{F@X~P6|?Kcml`gw)Er-Ka6s@ET0Mam%RS$Y4(M7s#lsS-PrwGG(f~PO z?_Ert`D)%uwbjhu#ZlYdu;SlvTH&ReRb}VuIij0OZg3lPNn59IqOGNzs=da&#=i2* zO{<(KOY90~m6zw_pBgn=46xCU#|QQby;IelkZuhfxaW=JSr&dJSr&e zJe)VA?BmND;T`pvY8dXkAJ2W(%2$bP-6SW3ds)-zXCa7AJ4H z;HEf^Xt>7EaK>$-GqL~1R?sx$o#_81^Ppaw(yce`EEGfVy0P1GS{S>jAD;Ur>l}7x zYQWL=m=XG4$2O4v{KFT~0SS`T`)Oq!jraG(fwVa}qhl{n3o3XAQX>0C$prqbB{aEb($5cH<*TDc>8gAU!W@Fp78{4*Rn@wZewsD#? zww;qkC)SBIYra|Y&)n>*efg~Q4j^-!iASa(aOg>Tb&E(5N9ZzAw?5ml$Lo(5JX%+g zs^O}@mnd3ROnTm{TKN3cYE#V$R9x3GV^H|6Zc@8!`6g}puR~A2pL$wMzqCGu4_cf1 zyL$wmlRa#6n2S!9`XTL>AiQamLuh3rpMu@@b@84NPSe>fKga%Qsre6OME4DD6M+tD z8dqIISWo<2+DFqjzAe5j8}Tm7tt{pP!oR|Hxw3*&!dAJkeDK=b-+Na&>~bLhbGd)X zvDmG6XH$&POfw4_cDR|Be-VsgQrPRyj|e;IcQ9?ii8Tr{J;H;ZCnM=g5MsfJe-oX|wrr%%;?}INH`6skqhSw;Hiyrf(9#<;pHE-nP&=^CuxB zuoZj6%YMVr0l9;1HB2UKC-O3U2idy5tn)A9a9o)b`Dv{dW=EPWy8`AT{u^U+6d(I5 zqD2|5F7C4ue1dUy{claja0fK$jcMtkKp_Xx^_FxYq(>5yWOhz}NQdsh`e>nAUmq7(r{&mI7d1?yyCE147fg>R%aQ#V;Fz@K0GK(a z>DBRBiMPz6IR=-0+{yINwTBi|U|odc(Xaa79By<51ep^@E-xdQgk1Y^T^JXQ2Kd_W zE%~)D&gopqM`zvAcbq;LYYy?QS{JEA%Scp~v%@f5Xt=G&F8w`Url>@RuYC9pGl~~X zh*v1Ao{D_l2^0U6+`_k6m;U63zeH}By>+QNB@jlZFj&GYi#W%y0`g>(+t6iOV*} zUPnrvEec!~u1RltKvXy5T8?~4(qW~mZbs5!UFKq1-GCi|-D+C5AhfojciW3}{rL!7R|xN5FVeV?J%bx85B++P<~n;T#OPyA!O%!u^#vw7sFcjd2tc zOxi#KxA+(cQlZ3JM7|Dirai_rty8{z%A)@i{uw`eo8@1?dF zMk;{p)$M?Z5G+<%CwIW6k8o5ZzIo(`<>zs%qXKlnBL4E?EM@^ zSi1u3ilU!ZHdDy^87?wexET@HHV4qljx9_Q6L4M@Jez)(bVJ;>K!pbrp6r`kv!--6 zf0Xs>5}`eAMy$K|ck->Vy^a~TrAKoASRvBU(2mRvhRdyPc>ntT-CY~!4*ko#{*3DI z(&_q5p~r7&DDZ*rWkOchmRSCSfkQ#lL295@)E1n@PBloU_sQO6$k!M0>4Q3v-*8Mpy;-%(PHHrS*K+rr9q#1Mj1$cO{~{RK4}+f4nyRPZ&WShSno@+yaKpI zGC7oR5tA^CkJ?n2i)9G0pBqYx;)hA%DM0l!<=tI$l^N@c^0sa_-$5U8J()r$(M`&&cZa4AJmagdR~fHz^%QV1H^Bj*_acK7nb zX1NjM2|maBMzj@@IyVG_Mf&1!L1BGGN`M$G42?z33KCx4i9AS1e;(t2csj#@cmlh* z&&9$F-Epv%uDyIus-24_hUFCwNZ@~ETb(RB8$%N1-sTVrTJ^gaUn+F|w7H;#cJRG> z@;oK{^?(H2bBZ))CSm{|bXa$u46A5=qYZ+Uhd7!OT}fS6ZA}_nyLSv@@g&(YZEm=o zN-|ufd7&_+IX}@Py>4gQ?00g|FLYQUsH{5)i9kIch%}e}bL?&9)O4zO`H2Ec25CSb z1AK0ws4^T2+(d@}+q~QTy@wI(K41k_;Xrxi&A)tvT3rj6C}O<#L78nPq1O)zFoO=h zd72H3cwzwkOohF_v0Q~TZ$UV&ho0xJjcJoAzw2YrV6t7z_P9p*G0N6L?X`;bHI;!b zH+hu_k8PMLD37aU;uAAC>m4(uJqLq}9wOg;C6fbhtQC@iE~ohod|Ed_Ev2^Ekb$CZ*2@L;6XvfZ(?D%&5rwitaIo=^3Sr#qx#QJ_C#F^o!rlw zVaBTgwKC4ZiiP*PV}mqDfIUXETt+CpUM93uMqb#Nv%HA6Z*1f2bzfJdyfzfcBDIPo zGjqU$cK?d8XO)IGTH%>M4CUKHzXnFbv6S=YUuiQdQ)%|*A$)!`>vOZ@$h@6^`$YGg ze5zNjru3(peWLrUg29W3?eUhHd%WpWB?o=B#JSyJ^W-swIe4$tGlQsuuaH^y^-nVC z>xfq`@xW^_fA6rJi-=dRu>8vr&#=Txq|^6o;FPHlkHX>4f-yn)v&C~V#{}B|xy|;J zXdagvJ(N_S>9(=~?f@Kj`~6p|_tk}-Sy{|Tm=so78o9Knl%$hb`F7cO{r5Oo^)gwAaB)fbiQK*2Lf^VP zk~z{r&sE*?I?vh39A|F-$BT_t>$!sLlfZk{gOW$};oG*B&wtZY=DkOMf301WEMC$~ z1{c7Sc-w7u)f46k9$l=o=|p>^VAZRB^x)Q47p&(`N{dDXaF@+TtieC--d&6q z85&qQe54oAIg*2WVab<>D;6=s~fy4{jAMmlUj5F;=z~cZbQ467&7&R7&P?(;*Uuqd~|gF zu*>gX5>85!B){q`;?K+rQ`VKV+JlrQ@j0_C4;Zt}KdSDt>PT0);62;gx&`U1N6PY0 zQ*-J>gOi(Yf2FmmsLhJXZBYDP3Qi!M+3_Qh zoz$hJvo1b-|3!MdU6u#;TcIweaktHyDmW(lF>`hJ}?s zS%L~P+?W~Z3)!i6THnF)YBfm5sUegpX0oeK64Bk4GyZO&eD8lR*%KLW z{M{n_E+pd6`C0OssMaC3L1kH(7OsZ!^{oGq8l)(H8hr4CIzZbR^nQ(V4E(n^jI%Nq zkRH)8_F0~VaqvRR^;qWPfN+05D;_n(rW z5J{r-2nXH9t>1fZ%KPD{zJe|3HP&4GQ_LPtF8Vd8J`pJvDGsR|J~1g_AzC3uAx`iQ zVF6*50ahcdV1>_q_5tRB9|K$i6azd#WJVYw1S0q%#3H!LRDF`=k}HxAl3L_6sv--N z3+xNruI42Q@;BY=S9;c&T`qS(0?JiptaP6YMl>BF(hib^Q^w}3{aSCj1FDG@xc(EszhL;X2MuTEN5*6+q z9$;6@_@rcETR&+UnL_SPX~a_dpM^HA9{t&9I|M?e)VGh3QeU|GII$Y>hxdW$T(5%@ zCx=l|>(}Yk zS=BjR(C>4j1?}jx>eTiPH+wZFwNUG(`&zazr#6SOYiHh8AP)<06YVzdOS_kLkH&i? zd9ekx`E!~s`s_&`fkHrJ+0kz$Pr3XHsteISy*wPiYt%ISbgyaExQ4if)@L$%$pg*} zi3JG-oXDT3_Q_>eU?-dlngw(@Lj*(k9zG9u5RMTKAmh{KIUM+f;+JF6@Wt)HW3$!4 z=YhJ(jqLzASk8kXw8@S8@zIYH_nH5?Llay#k8dO+^@R>N8t<$5$dJgyH6NyYev;@6 zae~-c)Fb?-bMwPO()wkuFDi^`KZZcfpGJ#Ktr7X42X!i%?lK4=U#Y%$<=lm&$PsDQ za-P`cjdqiH_%}*XvLb^;6asg+6Kwu@d!z_a4>kcXoT+N2{L$|kR_ZH=(Kfzpk!gEr zTt`GvVvE1+)PVB=XSj2n;39Gw&7(oG(r5UzT8KaKBQ#9;t)(ZQZJu~Y`9n4AK$H4k zNb9dLr7_wbiIlt}R2+kd*+M#+7UR=#d^^OH>xB3Fd3+lIs0^~;-M7{oV&_y77U#yt z5|2ZyyyD#;fET^_EWPyIP;l7axQ2KOZ!lQ(WjxbH2dO`8vteADZ13AEZ5KoU?hEE_ zI63~)7bTc4QJ>~!50}6EeVi;f7ig+X4w%rucmGpJ`I zuc_jnJTHKqA3lFRWC&zz9Gq2kfi!@}357AKHq-lgT3enkMp=jgCZ-QB5(v>0XM{u) zb<~HNNL(I(IZ@|5*q`paZ)gL`H+^UPrXZGnopi{nhadl`K!82p+7y5eCU8+5r-MWs zQfo?0ID#4sp(Uu0y?z65v@s#MQt3eBgwzzHISV$jUpd_PItW8$6je$(Cgt69B4&ir zymy-j^M0emcuy@UhQ0ShnZUU5iNTBwMJ6D0QL~XP{t+34rRtisIqZTGoyQf%Y~H=N zCp+x`cMpI7I!TcikBnw5V%XL=@_^y_mTHmzxNb(#n!9$`r19&zv;Zxdds;PGfz&R@ zDczEzysM)$Io2VOruv4ssF?R$;NX851IJ-6z;7;?J{TJLaJK$v&KKfa%pl$Q#;B^@ zt*CsVB@KP-c<#j_TwU-Qd+W<|>1si!F#5Zikb`PHyUU?cZ5DhmE=MootlO%r5Nj^u za^Cx+hl=ry+_P~?{A7{wJTc{+x0@bzE~+NG+Qe5n@W`Td!UVam87W)3E>gM3KP-Nj zi1tkZ`a>zltV!Qmh9b5%b|Q%_*~ z_+FI5T0%-U*I!rNUstAwzafgb?D2XZk94%4xGfOsh3AF?e^jL6Q#H+?-sYs%mF%sQ z1NMb}o-l=!)GC?$PBioir4CShV;w32)mV}FVIrl+lRwi&1+Hz+_sp<7qR&S!IqPGd zp16-_kW;*hN<}95Zg>9v+u4|^z?sImah~z)`Ukalb(wMDx`KAnbpEJI)baheS8v== z1ET=seT$3`qpt}vTSt_ptt;laC306Rv820aMF)29fO2hYT~X{v@#9^L-jUauUT5zN zeLwOVeyDv`xQW-;H8_a^{5hk|o53X+O0;m^gPXYR!=` z?5F1D=Tj3Ljf*W;;A{~1eNYV2nxTOtq`M0F)(P;dkt@mRK>T_}RVZ7*Uz0`o@P zzHd6M+b9}~>#6VPvybgvFdk4M;`&VBLYSve1)DA03-K+@n?cAjI zLg%nC6GQ$$nCifl`p2!NFv&B`o$7;~%1Dn>QhRS~DhmLAU&vCVwKY3yd#>(sjANv- zf&ErgD4%XH3dpb>OGJ52-c9Jo-rw1JJ8IC^K(uCAVeq_@@`dqEVu0jyOgYHQE6*}n zJ@1?HqWNkQGtC92C*+$dLA1ZJl$ zazDhbNk0)^8

L0`~qG*PXCkYqpTzxpA+$FU;8z9vFCt{3bmMT-qwp-8}jPJubYp zXs2&_+dRnpZQH!5E!~Igf_@1dnefR+d@_8f_y69*ur9tZwaRzr7Ly2hxJOeWtPaK{ zUka-l*N}VCwR*G%?JpZCIQ}dt$0WEg^n_ne9N>4M_em#iiuarT_)T0EvfY(pXx>_& zy#10VTiFW71PtIsnO(}WlV4~h*+vY442mkC0HWNeK32@FTWiz3l8Pt8X{Q>_wZI9L zp71pOsO*{(_xmp;<$AR|EML5z*GU$ZO+Q_ti~3VsB|NmF|I5t2BPD6+O7A4Ph7#)0 zvo|L82j96c!st+dQdB8#u-CrQB>ZZv%zIrq3*`>vOgRaW81Y{=PGjPK|D{bTH@JRR zF8nDrn<>cC){}1YI6PeJs?rG7JebJOGKK*_-V~xoymoNL(5Q3T#t%?1F z%^4T&VPCDeo~zs*af-oXMnl|ps%Kj3DTgaDN8VoUe;R`MF@9+=`3)zcm54cep+R|D z=LimOke|3UWAUb6KaPCbCGrrD!=XeCjn}F)cjq2Z2>YzejkF`&p&c{B?)iQwnh;yM z1o^}~2S;wsiU0Q<`Z*pW($CP2`y~1p_&T zFQ{Wn4*u@`9+sjPh&HYWqv|K)?tME);os+2OYvNPmXJH^_lmQvscpHpzOuJ|LNI`N z89;_wW1eK4=H7G!MiRNV1sPiB^Y;at$G3Tt2hxo0#r~lF?MPBBJkq7SdiDX6-L*y*BpmfyeIXkxhXs* z&!VEls@N*#BJz%!!UFLYav$CrXZzt%DRG=kz-QZDTpP|b?*;Lay>1}q4cC|>TzQxC znmv-dn6!CZxV-UH)9f|6@kV{yG_qIYS`u(Qb)!_T%oyKw&}M4ry&^NrB(lXlTkLPG$VOTKq zr}fQS=_R6HGf$)IMS?$IWwX(^gaW~bq8q~J+23KcZ&~_! zQw&=xkgnFhzr@E)z_)@fgZ8TEQEgT#^K3%q&XmvIlo?;bek>fu^l@J6ZG!OnykFnq zc&IS^lto_{iJ951+m)E^|9`Lw%JU1AQo7k25Q$8~WS7 zrRi^Jx!qwt|A_Z>p686#fTzSev+g*O^64_48jpWHE68POgVJm>_*Zg>7 zpqw0@m8sqRfNCl^=Q6xA6?X#^tn74a{Nc45`_2Q7(fqUkx%%;VlSXu~qZSoo`(zem z9&T6Fy0vRdJi#!&$MJS{p613xG$iy__bQF9C_g}#0-ED?1Dxry)wM@vB;Gf}Z3IoX5+3g-{CD;ElV~a(AJnX|3&APDQIAzhKbZV-mzE}Xl+G;irttFaVyu=` zgzi(UvwC^EBkF&~!?CYWl!cIc4~5jE^u#dcI16YJ5=h8~DU3%AYbtz361;Ew)3+9* z6m6ajkd-dR1L<#f_1k|oT?YN-uH`Y{$*++l?l}DFW?x#_HVN$Y8!^F>@h;Rg+yJv@ zJ`-XuuiJ#d+y&&PLoLui*eo}wv(z3{xY%2{xlpyJ^#?y8`=Xk(Ud_OrllF`lO)%I9T6XCcB!4se zYhkV*so}ra*y*1vyN&HHTTO8C9(OH@?}ggy*@*7ywWGVA3Es69b}B&EDqw97k*DR^})y_ zyHOT2Apdlwt&+{fR6q7x^=-HfnSX~NflA>yjfC-&^f&X`07 zPh9upGthSVyk_wjh5L60#abfP@Qu0##}63(gHQ2ire6Vxx@}} zQwRAx!JjA^+mW+W^V;U_u)+c2FXX-`_H)hRFJ%M&JTcjv{>scNJhVEncK+i6*#jUy z(gf=Qi48*Db!_6f0OHRbm+T-*8-2(ih!aGRativuQmc4j3^!~c>{tl#kTT#l4qav; z4KM+dSR#Pshtu`_U03$J686p3A6DtddbZY_gtdx3WI?n9ixLZOWl{a77dQy#Jz+CA z?5~Y`$|=fY=$EBzEpq%mW8@NcPnZ^RQF0jJkT(ERcC>g-3~rvTiQ0#EMN~OzJ-qKl ziLc>75htrU?|W&|Hp*2*Fq$867b5<7e?fJwq=bS1uDk&8kS{Pm0CB#%2*NrvUd@mH zW9ZeGFNFudu^g0eyy4|B&sBGlBKO0+oER|=qmCb;Uw~-pX99!aW>Wbj?-AZRrZ2|C zw#IMOX}}$yjsV;i7UF&$gz`|g+JX*0;xu&fytxo7p#w^glrcA?amc4YR`wa~I^nCE zl~+rm+>yXk5n(y_@dMsN|5-9JwE_>#7El_uYsZ;Z3l|2L_Sng`7GGygZ?vjRnDUjF zh}_m6g1kp@uN<(Vu3cVNoij4{4@qF^sM7N<+alW!ev=Lp!aQ|b$#jD^QL)eAAE{GUnx*t(s-w}+y%wa zWUY`am(3dzfk&5r&AmRXM6LI;k;k27cdC@ji^hsh&_UgC%(d&6O3($v&XLRPxhecN z%2lWoxJ`UR{@Dq%#4P1lR1J*v<|EoEdm-E?*YIq)9ryF4cLEFD(VJ^x@3sS`%VU6U z;23r$Vg|>kyvuG>162M@-9yN3=+RhJeR-Qj!rwpLDVxy%zUA3rD9PR}qRj@d;zO1@ zQ$R1dFr=~ceZNf??#!Dw3!Nj!me=ZowAQ)yG^8%ozx@}&d2^0^lp9iPrsdT6Ee8}y zH~Qa$O?l|R(=DfMP-bGlvSkR0vY~Lc5LP*MJsNbZ+MsII_^lVTcwNcTguvyYS9e#17I&S zd59Ew2c1X>S{UW)!G&esrvPj545HT?EK@-A52?i;p%owam1FXw3)2AZ_Z)>YhcWiN zJ>b4=$MOV`Kv`Nz3>7BT=akU{+b+JiH#|_ydPV%%xWpZ{3(Ekb!FVfE`V)uzi;UV zYCiyDSt@r%Hn8b&Cr%s8LCvXA7BuVwLy-%(KioI6_+1X%ku9J>)wW{vDxCa&Z^zwS zpIuCx=jGE#tb4@xPXAoYD>>88{Ch`213A9hkid-lZT5RWR$_imW=r)B%7DTaiJ&>- z0e%pUGC^yYw`7ak^CiKDm%RNzs`_l}k^O^yQc8ZD5_;j6)4!_;%$o5%p%1HBxb5-h zw!f!X1~m-%$}V2CmjT}cH{a5F=?i^NU$(*SXQzE2O!&@F@50Bc53xBsd4clXPJ+a^ zD_iH3g%bsGTNHZfsD+x`65|n$#BUD&5|`Rzqy84;`R{z%bQQIWIqN+@>fe1txSRB3 zR&!hl7n8X2)FdpFMT=w2mnBaea&W zn!C=?1rB;#?hkMN58E63ZG`XzI1w(Ud?h?N7AZ|An2lx>psZ-25+l2#m`1_NVW zg~(Z!J4E{<92wG+(oB-iC9yu(1J9A$4tOVH>ngPdL$}G@vYe;SIye1z)nN$F)p4!! z@B6F_Pw?BfUrHP7%k$}Gu>iM8K|PmM0cB_x<)Gcm4{OM1~H(@>7x|(9mG7V9PNDg0J}!#3?XznRevlB9=?_IgSy3hhCA;acK~Bk zY>5pfr?L3lw^}KbAHK5M{9wSi&d$&IZjKfe7pp_}o#bVVdu)OMb%i{{QBdc6zcW@i z5@=9Ndw=URQ`{K!>nCr?*jpeOe}9k(%}rT}KaxQWNINQ6Y~gn31h_17%>36xGM-XC;w74=aa)4!`2qho@`s*pxZAmXfN}j87_$EF14fFAzq~gWL%7zILh1n zae2Q_%WUm<4cMexStAn^LRVhY{TRd&ly^)r(=+6~HrH3DtPCBl4+ec$F}@GIO7gYP z&^ro6752lJ2>6X`r8rmrO`YZEMDzSqbm6$W2MC!!S}YeNwso)bk-XF{gv@==a-)eT z37K7E943?9!+O9`Iwp8Rm&Tns+iZaunwF$07zlQuIDl(b%MZ8H+=texQv|n$%ed#`TxgEs%^JJb6h`|XNV;J@Bhx??(O4_T+baBD>`WE#eLI$ji?nA=e1Tibd< zX77b@i{BIoOiHtV7{xYb&`A$O-m?d8@b;nfL0rgf8O}4!|3VCc`NaIhdL=*#Wbnhw zRK5;kpsXyl=H@K$9!K33cR^Z*!{0hE7A5>)U)@h?VYICWekO}KSpxc4gIpN{a+h?% z3k-Ma48GWTS47#v6*RhR+#M>&YkBv`l=YoCpZUA+24)iHuatQ8i%=H?T#s73@|N1j z&jNaLE@I+Rsw3VRTrT<=qxTS}p_@0TtYKk5ZCdEqyC-D!lN8+KIwXQ}wKTs(P-o=9 zqoIX=td$Jkub-lolkUd#NN$0@R{D#91cy^G+3+LDGul#Q-Q(88-O7OG?AEjnFQshn z%xr`hswbsi?Z>mi|PcW%pD%eaeQfQ(Ptw3We@xlae%^QbzhIkKIOFHp6)^}V2 z)q#OP8rhBrD}$>+(CrUU;(9BpAj%1<4WW4Ls;pCZFcp{`)ctIM4K4-N0^8f_e{LaI zI7I}7ZU%sQ_bWpasCZ$x%e@GAiTbj&Navk#aP+3|8#})_$mmNGzhvl1AJ&yJwO-Fb z^H7M8ZbMOv+85`$3N+jTS8Pl!g0S+ZOVyL{x}aYu#`_?4Amjwrh{g2E)pLVjyn4nx zq=x-XVB1@ZVilcP*tLuC=epLf2P7Ohz(D*H>SqQZX8%B9sHsgWx|8{X`SUpM+#f!% zOJ^~wF#zfn?TuyIhVTDjcF7FFX`6%@g#3aJM&O5&*oz}!d^-K>9C_#go{k#TMq!xJ z|DFW#MrQ8+j%ah@!PNc~?z31gm5dtZ!-AaT9Yr$sr*k6&^l;HbcKk5~)PT}eXTArO z%rx8QJ%9qj1s(erfagH7XYN|jUV(jwP+`ey$uNZofl6Dcyl3v?%l}a}-SB^vZ9)HU zWo*~GCuFrl=8qKrPub?}|EFy3>@fy9S`mZ{vqmPN;m@6#Ccc$z#%=gciuU6wpN#E# z_FHU!8}W#U`(^4}pc&Utv~2=y(W=BQl}MK(*38MQwRB%9+w{1Ymype z!fhBqIgrw&;K!B@kN#lc@$9i~Tq)5B=}jer@t}z*xREJs$#`L}6vP#3EWH(AmrMs+ zS`;0&Gp}Vwb=v9t%y~dxI!6n3;zgZZ+Q&?$es|0lmYO(tneZ)$wzzSyfPatNT-|`N zrfp{t7qFp)84HQW_qUX?h9m*iE6e812)5h6?<@u?Z`CAJ1AYG#^S@8H(%ceu|1&d zpg_iABIb@N{CC;yp_^lAN)I)>3@Iw8vu^4NH{Za5X;hBw>gN7 zc86ASPQN`X2lv_$SM}39gmfA5XdtG;^2whHj!QhpN|kSirs;Rd;d;Ywm8z zUJBQzH=kc-K1ZP8yFQoDXP>x zgDfO7PzerG$3EgL*MX!#%r2c@!P?1fuAyq}$Q~e(xL09hR7DaD(BG4ml_E{#Pj{`M zli3@j!2hjzthv$*&6Ofj)3yhbJ?Zkg!8@|03s%FQ(@;;As$e}EpbO*{fJDTV(3an6 zV9Qvn%+Cy$^V>_{;g>*Fn~=q?&k-w>a^oPd5>*Z#M0IwEHb$Mrw?o_~64n2ahay{` zmX$!q>X$eo4;&;8r_tm+8ILof)ry${oz4xqiVuMP1=&PRUn1bO}sve`RFw(0j-XOFysLm~BV z&0{AQ*u1Tj>9R&eO7D3)4tDn;&1LH0hg=48_$FT%^LM-;oM+)jq}JaF-ett?^72@> z%Q-O!;*HE?HMdcI)a*zzEWhDa5BiZ!jcaH!vum6!Sx6MnjE#c`ID@Px)o+!u1JG`*H+f|7+hX_G-nI$LVhv1d1%=A$!Z=a*R3 zHAK^m8?%Bqau0JXe(L`0DBcyn5>{GTFf6;ZVVIDoK6z26u1FELxNf;^E{g)OaWKt8 z5bXy08AjxjLsW%d!Q<6IC7+8v+#6%(mLkh6s&=#&Ap$w!G|%>o*c?C##&|YV^9KZI z?b!;Y+Ot$|U`J;+8ujR9FF_wnHoxheDF+w@N9x5NrQ#`Y|L#}I^R>yfJd2y1cCm_N zHm=D@+!t~lT^d}1FXW`pk-uS}+rv95yIOYEt$aP@*;juTlC8n;a0eaFGlBg#t215= z{q|3WS;9z)_DGkUj}iOfd$3;17GJdm7rhb(j$nfnKDdiOzTw`j0b_5Dt8{03=y;{U zCk2!9gU7O#puO<23^)of6OS$C0~3#YCN4vM4whh}bGO`^^aGp>){>G~aKr0TlGqpG z<|G(epUfAPS8~t4v?D!JCpUcW$Kbck4*sMW%bUh9V@b;PP;lV<` ziBHcd-yK2IkxMvpILQca9Cjgq$4{A!nMJ=jaF`n>Qcga64#18OEpz zPT>PMuPERPZAInqBzLvIOfxW*z)Ylqpt z$70iW2GXy@?dv#F^vQFb)wfr$==bo*>BNJO*w3XtbTsJFM#5A8i=BMz&Kl;;XKCX>jDKSZls<{0NzPWK-N-T(4k7PPM+?DS72*-7P+1p+TEMJW$bwk(H3k zvB!kO#KacS%EDq}rx2y*xhEO_yo|Y+e9PW_68L&auACEI5js^8*-?M+n7c@Lh%vzg z;DlX53&49}UM$qOFCA-<0>r{7plJ}4aPz3TLQuDt_2{1SfqHHH_-dnnDZuUQED+WlrVH(UH#zGi4EPf!7d8=w4b6b4bq*q}>@W!gNQWtZ@4#?i zS?@MK!BO6r`~HY=u{B{Vtm4uH{0?XiONFk4pS}3wXfcP8N7JR_QV#SEJMV1^tV!?J zbg2Xe0ZPLvVOYFQ=lj&ZLlXmh0C{0BFbbZhzctu3#F}bMm;kR~$?zQ}jM#Q8S>)>y zO^rY@fN4T!oM){j!+iZh3OVA-oJv8y+Sj5nlcY2ljq8n1d$;@D+iiUJ?HKPVT2(HdR=CFa_XdfG z@H71_s(F-10$)&gb1AB?$sPfuDmr*V0PJuBj_lYJqQ7!8HjvKjYUcvmikI%P>iy4??m$#Vz zmeLW+wg&Wi?#xdoPtQ`D_#FAv0Uk#^r;~GKLSGk{OAT|kQk!m{C8v{rooVkMGDYZi zUWL`PE=d2W+56bk2ub77el4>;)T(?TU6+v*gxD zz*HXDmX#TC^POb>GU>ooR4|rCY#(0`D(9-A7f?d45qVItN18Su^LAOTktYwN`V2-( z2^Qv`^v5}Iqb5o8h-z*k3JQzIn>#@}ZjE~)`F5mvf5xWuL}CH9$DI=9!%fEd?K7<& z`WUX&9H0a@!c0SATMf-jiqV_58vbuldi1C*+m&i<<+?cbS9H)ncf)b}V4U7OuNK4U zBSW*yy{4lplPMuUhJQHI6O8_++W^;5fc?bFF3ZW=RL*8gMN(b0{{g|P-mFV{-oCe6 z)L@-1ll|&=PJ4@VsA}FbyLhlCkWx3-F*+Ad@$5o@lLNnrpV zNaZ0k!FX#^TBAP-tkc&7tOk)9-=%4leQzozi`a)`)8#C~ROxdkVM{fQ(s4(t#B0zJ zH05fT6V&Ny7#LM{G`)4kcfa^_%KU1KnsgpC15K;X8efOmxfGGZkIsHRP}ixXuE6S4 zmu}lLpD*@s8my#yYIarBo}X2Ld4*8g)6M!LGj_I}lFBbKycGIooE$qx7rpd*nRcDL zn|n=Kc2=(9yw`UwuXIK~**L#V@uTwpyv-SV;-7nLBdlqgJZzJGIzbU{k zYe*?&N?fs>raz`)G+Ap-Cvvog57O$<98>*4U&f(^PG53RjfcGiuMs8>Qq=e@e>Sa= zBY)1ArZ~aRkY3ErpE9Y+m!C+l0+XLluO^irepS-zp;V)o{=lS>UUO!kkyvxOy&RDP z68xlz^fa=Qs`n7KlW_D1Iuj-KfH;#Sey9g?3e_X3X`j^~s(PO_ATG6n>zKv+t2>$H zS>_RY@E%HodU%nGAbOyY%R>2Mjmot><^1xE*2-^cCJj}RHIq4 zA*jDDYUb3+EWYJ5$}ErNRLv}v=|f$!U;Xvh8V*SHHj-H}P)m6(c$-G5D2or$RC?dao)hTLh``ja^lY{(E*cD|Qs4Mp~S> z7QNEnA1S|S<#yGB2`wtOPQAv(yyuC3rB15pc12FA`;JR_FXsQsrB|hT)EfAjVff@B z-H~|`3$jy8EdIt-dF;Z_UY4QqOHbXNR`Rl12&x59iWV+I-8Xv85{`Pa!I=rODmCr)0WDA4i04h*zsV4?Y{+gD)89xV9Vvd5p9F)zY%TAf34NHX>`W$yJ0U4gkkVGr34Nrj z>>MVC;UOue24Bo6w|~YhOf6pbEDlRUQj(#hh2inCv02##Oe~y+#w*2)Ul>iSjE1I9 zAUKSnr>9Y~bkSJvEQiKRLQ?)hP$)o3XTT;fBI4h{CrP;F@ZC?tgefO&#A5JaXl{Fn-rkwo%6q5b%XI$0vu#MGi;hOH#o8aRQ#3~9d(}NJoFV?NOo|t6qxUqxm*7P>h~+$ZA1lyzy1=gO9qv`W(hYa zgHm79gzM)dQ$Mb0_w5X3L|Avft_=*;*PzSo&o_%a5qB^ccceQbe_hoHNa$38uwpO(D+qA^3^d`R)9gejJWXD=&SarT&*?T@!XQ(Nw!2gIx-?`Cq-y zfXu(1=QA{?*QIp=^>IL5Z+&m5+a8PYlFBn%zo<`9mX#^6TfcbYvhiu-1CL+&=c84^ zTdSpCT)`iifU&PZtB-=9| zU$c>0D|bez@{0NkXbH^u6{{!|p8OeVbv~5>9@Y=wFd@P*#YKPexxX#XL?eEVH4hj5FS&?gn#xy zNC}aytR&P!l0NZ5LtCu)6+;?6a20^w3)J0k{2Srv2vnfNb4>p*4J)G6Fi{p_;UDV@ z2H|KH6Jf>>7SkV$W^>KZ8GqPyP#8?80{#Xa66(P0{qepLs*56BuYLb+Z%SMjjogIJ z-JHAusc%Z?5KVl8`94izje8L#)P(y82;v?3JV7IhqHw`V0=V)5c0pKqQ5-HfvjE?& zZ@Bvy22l(y*bS~c(hngul_VMuD-vg>E22Ur1U1rUst;LqbvadIE0{Iab`?3bI5p*V z<$AT;HT8Ct1sa!{a1Uu`Rs7YJE0XQ1j%v9pIPIE_>RT%)52>%Zmm0mxPVF+6ns3WN zo}$DW(aWZnRTO7cUMp;#2Kg#9D|DWE`Ks0{e4a*zD!40Dp1Ovr+ACaMVxLQoditBI zKO8-DWWZ281wkrnYssghukbq>-t)@m13iU7a!+d#541vB!BrxQh5w7)whoG`dAmiE z5C{?=c<{l3+rZ!!+%>obcXtmS+})kv1b265aCdiiZcTpg`+d3Ro_ng!Key^uojo;G z-Mt@K&w5tx?mg89h3BLfX|HnSL&7VZ=Nzr#+5Kx*-<3kv{SnuYm7==+W7ohJu~+lB zI;y)>i<%j=!;lH(bqL-Io-vx1tT1&ia#8@0!h=b5*MnGXFN z=J=K2YJDY}GjNBw4uc#PX_cdDyCs)1(1+O$1C8dIm1SzPM}_!CCoFMN7z)!%dMBXU zB-8xHUpl7AYCE0^i&KmaBT)J2F5^?I$Vq`xP?tYVjB%1gM)dM4O>_xZWfJrAlT1c` zkrm9OE+jkdh=Et^8Dx^e^J82V>{&D=gZE7vlTLQ9FH2m)6_CN;D`3mWi^sy!w%WKE-sSREw!RrDF$?srw_SYuK8pv)M#ASZ!u;#v_gpoJ7#xPCQiX}13FlOGoHnG$&W!};;F=Kel zuwdT6F@@W(eBS6L1!^agT^}ihWT%kbFgyiqCzsudE-_>$y1~3Gv3N)AbEk2B;;$Wi zrjcf|{1kLem%?&z-godHz@@19#UWMoU#E?6=Ks9W8rW zw)9QP%Vq@5dz3P6@kVY$Rta^Bi<`&SH!hDYP%B?gBc0|rH}J2+UHV$kR>V$YofbGZ z)2>Hdc3TiugifQK=9@M&ugippg5@x#<)}?X1mw^VMM&hx4}@EcnZ+j(jm?<_nCX%k zaR{Y;8U`DN8wMGMeG#q_suHmiv=g=p+6mqX-wE0Y+X>l;!2B5=6do2H5*{J_Q#!O2 zBMseylY^auj05x_ZJ)g45#QjEq<32Sh2ka?X!jCd)^YBU)|#>T2(v883!h?-_RMA; zW!xy`G@16DQ=}Q|dmuq}U^^YRe1zXJ>in(f(gL-OSw?s1Gin{u9=j_YUM#H59F|bo)R)oEd8w462~<^=aduj9(W7J{KH8^n!+hQ4McJDlR`Kf9H~X3bd)PP5 zi^KYw!pJmQ%}^?ddr2S8=egA!sAotUWQc67?<%#iMS@52yqAcBtNzkaIJfDvv2K;K zP7&_gCftsjGxTaUapqdRS#%t#fW@ zUeY3k7%yoPu6Z|-Cbt9`a4u%hHgYVe{s7`?Mt%KIFg_pvRgm3k@eL>bjr zd6^mBYK^UVV0pvR&gls?f)dRjKGA(^Rd}1aEo>;(|F`^xcJQc)Zk5yaD}ZBG*dLB7 zR;t_QQ-nh$&sLn<33Me~tsn6mj(V5H9Ofdwi90H1`|%t~hA(gqDo{WXFKjatZnXC_ zE9sy_KC|E;-?j6ysLu5*Od*z{m$OhyiI#uIfdlA$z|iugoT;C#6}{fm`2yI6;cMTa zb7B$Ge&<;@U6DjPR%31%JObH9dEC!7X<@6wQn8s^>+#X=M(s2=s*f~=HDimrj1#TK z+TG?v`3O~kT1YLy?EK|{0lMkqXXB#HAZ89`q!7O^e))Pqq4muI8u<=nFm(fwlgQ-)*b7vP8G^GwX~9OxcWBuU)ULXYB0qT-su`OWrgr$OFUFaU7?jKMm~f*mnuw z8TGB{gjZ>AV24-jo);A_iNx99HjVwtUwLm;aDeXt;!vX%^Jw)K$zj?Km(VlEph>dp zLiMx#1Ya%kc7{36|^}8-b&!D zjrFe}P)BDJn|%6jRXC|*LC%!4>iq1GwBZ`9)7oGmmipU871{7R>lr9t6Z$YW9y%gBakD`v#Sm-m1=;q z*LV|@d=wH@Y5O=T-;sZyvbD{kT7n{&+7cZ9q41H}Pq+j*#X8s*GZn~>D}6o=Ef08` z?!OCQy4s12E0=~2`s`a78CE_a*ze8GNWK+Ypl7*$SVOn9aae=072nk8vL!wea$L#1 z6|-w_p{INV16@IWvt{;R;EoNyPR#}jzxf^SqtHD(0bnej{9EKjEqA!LX zw_*f_9@O+3^BHbhqVC{bF{MzbRPRrYFzmh^4*49Xd?lAR8RcMbd6hU54e7f{7x3<*=qy%Lp?dV9_`ie@*gSIa$gm{)42=NnCPiH!@c!#l}=3PFre7IoG;Lzp=Ms$aN z725T)BT)Mtu!oGVLi;gbR|S9g2M^w(QI~c2b;|eD>gR&Vn+%)xuh=_c$afm9?O2mg zI}*q$0PZ!3{kI70zX^SX@RnuTb;Hk;zr*!iesb@_Ax9`N(u~2at%|4o=%yN1Zyh@= z1jE+B$P*><<_ONk?~3^cuaDp03k?+48gCsma^vg8iT9~<1|FMuy6se3Ei5ydR!r{f zbW4^{w5J71Au>iey@GYZ5_d#fYudZxP=_y<^z`;hwpLAR56=k+gKV7F?zTfoq|j|; zcgm=*!A9HMXx+%(cSPvCxjo3L8-eEM0z~Hm!cVd)PqKnfktCk|7`$6Sgv93pj2nTv zJ;*&lgo6~G6k1QRK6hEe8-Y)+)LO%!vu_~oh@`ugM+SKO(9No0U;j$<_^v?}sKfJl zP?zHzTvMgvzAw}n2(Pz|1EpZ&}fgHOc zk7DAyRBxxoqtCDqV`UiI!+kAdJjUYtp&$>up%9OE)ITkPoq{a_FnU<}#C6rr;&vC~ zo$Kb5R20qCo0ZMo!V6oEe>!xB*D^iMBDao1vC7Twm2bFm%1nONt%~Y-w^22PvBl1us z`W%j?XAa#;&pe=nq~5ZQskU9eH{SV!9(BnP@1wWk=ZtQ*&x=)BvKqyvp=lKyn_v~+ zN~RfIys%rQ@ScTGy(4E?jn((P4Yj%ajf%gkYC%ZN> zgPk7SgtvL5lD+UtDMdOKH<4lWE2Z_A;H1)t;c+)z+|7@bQfD5O5u1{gGHV}nQ`Q4@ zQ#M6(Q#Sf^)z`>%)i-D=OFP-!ha86Y)yuYNb$0{pf;2yk(%y)(Ma&hiH|v)BU*5-? zrD~g#;n5$nepR!~u3l1~w2jH#UWsQ(p|M~QQnM+lUgDgz1?O(BTrZN~(T9@5ZYKGi z4|h6a>Tne^q*H_%q4icFY{Dvi>{nP;bBFGa3uk<3GrqIBD5V{n!t#^H3-L{&7$)_g zt-6Y$d9ivF=_r}Cvv^A1depihC&?{`eQ@e$i04NC{w~5qmMaSNb)&DE<_yT=hgdKX-AO{wnCI4QAF@8eNU>PJZt0nr`l*s#(hMh$j@?WP-celOasS_jq(EbE+DJNZ54 zb57!KkJ{`uW;A9rTrQ0*(=XkhymP8*nqEG6**dXzYI0kCKKXiE;Wov6l71D6bwixK z*^s^~#C@ULOspkWo~XP;y-wyr)*`=Rp=EOFNI&1yMZFn!J?OIC0>6@`B~%{ml;BjS zJYT$_czr??ku8WWf=!t|3WjbJou}ZuzXc%l7b5HD8dcdA4ltIcnU2YE`it179}JK* z7IHmH0zE^b)GV%yBk;PE$nJNi%2}S@fukyz=pD(DF5zBQI&? zqfXE=BB>+qhSqlB@_5L$=&~fSgG|uWn8G`e1FVMh%ghNvlZmz&pVv6Yhdh5g9W=j@ zpJ$)RxeU<=F2zPd#71KBc92C!4nu@ZA~`Oqwk5n$%iq2@{btu`vDHi9u3V>1~GolHNUOoy9HPs-(Hl5jJOx*4WAPU=67 z$Nmj%bh=>QHIH^UvKD+@?y_G1ct+35pxQ6sn47KAvz%i^D2_hVT1%kTHxWMQxg%t&s;9$K=L-9FPpD&{z`8vl?nw1?2@c-yG?a8nYKAN8!e9#QK&4vxMi8zA4k!d#0w)w_?nZCEUIL%*9tfzbZ zV#6gGQ$K2=(3L_KPH2K!70UvOx-x4JYMe?|X!45TSf*)E27d`xGf|kar<7ShpQ68Z z--dl1mU>EQfb%=X#OrV!t+Z9bK<~ehD*h#_)A!0 zM05R@=q;_&j2@sD+~E^O^Zt{_!O_n?LqFaV@22OYEU9=Vn{^R&N`aG20hBt;j@|pv#1Fs@-zRR|HaV)uml? zjE!cw7j*^Ao=q!^xKo#IDgwP?HjJ_R_i;g&NzbQt)*2(d_eWH$yO$IM4Zqx95_s8q zF*n?G6caCnzM+g_DGGJGW{WY7Vky5QIQJGZrVfzIpdGtCvEnY=de;`uOFiG(FIup#j=oYc^c|d}di8qg#V(JX^xY>M`LB7cJEQDk z1yL31DClH9*@CjMu)(Nb<%lW7-wOq0{{M8bt(kD^VUZs-r6MdLFY?Y>KBL<;OLRS< zsKuTxSXk;PM#UdXQT`{{a;|7U9lsjcLgWGoSX+w%BjqjPhpG((ts&>tyW}{Lw)C89%Q}(1NpN=QjPy~A zEX}V75aX{10tPwiZ-XYu4%4#i$4X)ohvY#l8KYIUjt@lCv2xs3-v>g)u%GzZa=@7; z2`m;FrfHMszoIL}w&vFeoX5KRf;2ty}p9v;8fQEj5+w^WdAgQ=(6(4|PkpB<*~^jQBbc zLqnxlQWTz3cHY2IRXYq*SVa(f*S{S?VL(NPPpWnO+)WI`lfLXV(JZhR!7$I&$F+>F z=N0A1?u&FMSI4h9N({j7)&U{7nhK7>-H0Znb5>E!`tKe$I7O}&PVc`6&a=!V`#E?@ zcgyF<4pf1*X`!@OMnniDr{=pt5y6)wv8R4g0& zLea+k_#w|UJaOx?LM^i-U_G(jQTOulHy-=U89ESZL$i50PZ2zo>_UfIV?1-{K3Hj> z+b>Ip7i9G!Bv`km*%RtkEzC_^m!E?bqFqvU#5hQhLd|9V`%D}J-Nj#VO6cWr=o<(~ z#im!VnJu*ZXdj;Nq4U1- zi-84#FTd5X!LZxUh3zu>nyYRt+&%k=sU30FL2ElJ-wfrrnM@|m+F}>WBNw@qa+2pF z)uyFmP)3^}aRZ+(qGm0ZMdD#E$xb_}?{zqyJsufH(rCfL{C)=YZ;tY{#9czKUGkUV zPJq?h4fh|wi-c{Bq}^(YoO}n2B=omqhFQZ&N##T+$7nOmv?mlR#E1k z3Mc8@x^0cTFW*OGVM@ux`tg0SlZ+pBST`b+sPS2mTqN%~I4&6NK)1mpY?UhvZW9z4 z35_?OlgJsPP7C2S-idUoVW(#lBL*_E=sqoQN6I1d2^*LLgOd*jVI*8U4PswcJKuI7 zW_hD!T#B^(Dh?#UIn_J-diRxrkAOT7O5bLV8=ZfhkH;c_PxSm1I;;Cfnzn$ajDxaI z2nkA__pG4F2IB3>M65C&^-;W{02I)Txq|cUdbv#Z<05${C+f4tG5%|;v8L4H8TKp) zD6_)C!08=v)ka3>1=Hwt%tC{f(X?VCQiz25xH4;(Y{%>er#fqf;6f33QinVWTkc!1 z>#o!Xj$A)`{AeS_!0Df4i9NHeDPCZB0UGI|%PG5^8_HiW(R5Wg+Ywqw&s!2Ovp9WT z9Uw9)5h-@q2&}V68q(BXwbMs3gqMYl@82%WJOtijA@5UAcy28v3oU z#G|<{56hy1mWyma(kLjH*g(pG)U?+hG&>lG#PB7p?6C3n3-&55U)`OOO5_aP(&R{- zSy+Wn0;1|l&^CNT{oUU9bdCCh?U9+1UIMP=W(=u$?r#_;OQ*sVYShbRQXFnJ;wkkt z42q}p;k&cQScC#U{DJ%Xr!`CHp^WcjG>_;0L6~nD^DYKX70%1D)fzUAf9xx(co)9o z`<7=GQ2eS~@t`I(upx~4RnanYP#2+yPxdW`x@_LvtnBq-x`(HTb>r%U#ABuQPmJ2t zVhKir&oL3x=YorFa^TL2M3Zb+=o>` z1g?1p8Qtbkdy+D%s+5xav3FbzpkNkH)GM<7$kZnd$>x$wr!{HG!tuPB`}VO~bAoBK zk=mSyu$=%+m7b)y;XaWD_g zdNSQK={KtctyilrEP5!DF2knz`xZ)XJ3>B@i4g?PyM>iG^0BKN5i36PISFaPO&<1s zY0(#KVBPb7j?-enRpA@&3)1?T5o7zq)<)L5q22Z^rzchJYdRGT&fK*FLC8~L-xn<{ z0BQSD14qNc;saauHxCbu=AUIe-ng;v-aup1Ao)dqM9M`!QU96a_nO=D$TYP?Mo_!+ z4zK9Gj6fi3D*E~C zR~59XLgqvjp4meExj*)7e)kL#=t5^(XeliuWJ?h=T6XAZ-qO88-6dh%3S>Msf2rqnNAqZz^lT~mLAimF*H;4KAom65?7Pgp1G>&rqte*aBE1Jda zSA~5K0(^b3kklC+46b)=VpP71+xmp&k!1Nc1t}BS*!*ourxz5e zM?Y^E44@HEE)lvIosXrZ_|KHys{yl%x=&yhQcU#m%vG_6su>PtyiH8Mn`3HW+Q(R@ zXIN4TF6M$EhS zT9emerhEV(E%G@WWqvg8Z(yREQnAlXV(lg)Mi+67Sl2JzwstK)nwAe%3`*aB4phI= z-h!3{ILeHxZOPoz+2zE?}9jZnZk!7c9ul$~)Rtv5yU z_Su*Qk$l^Fxo4@vv!K~IC|*Ys=Z<7@T);2g#T{fHz^HSWjinYCW*;WI z=(Qg+i+h8LNZWs!B;{#W-*0)Aa+|Tn3PA3E2dLcEe-k|PUg`^3>|2U;UL&v9(3RPN z6DODl`wdbV!h{{15*(Z|2_6db)YXMC6N=v+jU-J1ysIDhDkaBxm|BL*QrdGcl_T{L zf42JJ5ppYnT)Z70foOP3gDeQl13v{dQq-+?;vU=&o?TkcZv&FHaCF@Du8ItCJsu}I zZ-_z8y@OJ>&btKX%4&DnXE${lYLmJy>%xJOwPCVi9=0K2&|D-t$WXINV_Y&c1Q?QB zmz8MjGRgtB43rb0EoD&bnaAD4U)Ez|*d~S0Yyo=P8Z03!xZL{>4Zj55YG*3nhwgU* zhJVa_n>W)b4JJ1RANoJNEmC>iiS_o#I@ecMp(NZ)8yli@$|~h|%|E1`s)>;Mm~z#M z7%qC_m3?a!wk~sJxFT*f=vL?*nOlB_7zE9q?G>T@`x$YuO*5Tzr`;Xt!uzVk-NPyb zS}nJF^A-2tm5dr89Au}fUkZxrJ$b2&c7JW1Y4?R0=bYprFX5^svZ~|Z!cd@F9!W;o z)={=ZG6M<{g~dF|6n#umMpx6=#^3S{*j0Q#tZAOH^dXJF^|NngS;P!>a!=JQ?s7P; z3*qq%P5q8$Cm!OLP}3u1F%+WCg1j-3w*!-*L)8L);?iAa3E?C^6NU!d|;5i{yNr1!WAyArjzLOqYtm_5eBk8u@eJHCcFv#@X{E^b#%3Ga%n zY|^V^5(h-1BJ<97Yt3XgZa)D=fLw7!#tQVFO+f5#9PT82B4tYzCw>gYg@nG@VpDJu zv<|g{8ymz>yAOw^haKKN9VSOVz=b%6awiYWL>-RTm701O2pl8w%PsZaJ>-+cBZFnLc7YSk zo?J=^!VR{SAHy15(Q83(;0Ta?`Mo25dX3Y++RHv}CbDvG!S&>M#L#kMI4;gG>7O<7 z62s7-Rn)|%ZMhgyqsA0m zYg`d3PqAib+*S)4r_sb^fG$_g>lyUT==Mbfq@QACYs7KVHe^Xnsh0w@=j+h-ICXY` zT-zta#h))xPo0t<5qdQh;;G#3>fiqE_bRv$TD1?OS<(Ux?(diIjgZ&ak@(MiS?nWF z<=KaQ^yTW-E|27T{2pXHbN&V;o$yvw!oOgmG+(8*XUX z;bGS?MvQ^y$NR--4ICP{fHxt=EZv`PB4}TD)A}YnV{{)-e%KWwVS7HUzlAQpYYD(< zO0)e2)33AL!M$&ZJ>LC9)n!1FA*m$z?jVY(TKCEEXRU9}ZjyLxMnzkrH6}wU(F0_P-#LBF4JeCL`zWalmZ?$UyUSNwDK5;N_k_%3-++v9!#d1 z5{{G!_9m}CaEM*3t%l)Q--L^rwy0V-mE3K=)LoUsx8CK-GwyU}T;#kIvdULn>7I|o zIjo-FQj7imPD-nWK#Qzj5W8)c2LC$yX|1XDywlONT?t%MAJG z25hMcwYMYTcAM_>d2_YPb}h#Gg|qBKh+Lo`uGSIJjnvS0y*e-l-toDm58um&Yjs3t zM0f*BeLH(w109P$Ln~cVM0gM*fC2Dl#KlGb&BfY)Udq5;M_(Pw(X9MCZgrXJuaA&ye~T?|Vq|P@_rK0y`^V-_B4>+# zJUFCuY|RYxdA`{?7;w@5KbHR|hyec|JpU)j|JL(===$G!{tsRMThBkAC3!(%9xnR- z`o{A7_cz+WQlFdHiI|t?e|{1G|2+xR0SxK@5Cf|w0KlXU00LPw0RTo;bpVK&831Bp z(gXm2%<2FZMi6AgrVe0cV}qDNMj&&J52*CUYC6EpHXZoik z7IgqKJESHcJBvDii2<_8!~|Joh1ABx%<``-6EnLy0LTPM#l-Rl+8<}L|5Tt=12s}0rNF4xVVf&{SY!D`ZAclY6vO)@FgY0fzqanTE# znA;oJ(hHmG*c%8M=vnC-{CmS-Vf*`j;hc*0U3BmJj{e#P%6G{P<4Do(d{FSAh~gg+ z0ZP8`P~^A>0)oEppr8=Cbdc=h{N5bGz#u^zz`%U*`{V@$^~Q7k!`lB`?9Bh7`6F-u z!(Trjkn!J2|Exk-Vq#!{5C@^?e=PIQ{J;Gn6#w@OGWuuRKsF3ytNgo3Alu_lOa{oN zfNY0Ug#QTbUxocgT7NYb^cR&7sr~f|LhQen%YW~c z01yPU{Qp7&%#1(==6?ji2w-GoWn>5b-&Jvx2Ir10w9vbHaKLUW349wZiTd3Thz-l$ z|LNq@$krBYKO&qDsUHbIG*Yies6!DQ3ci>o`0yP8Aeb&D$oP$NPE#W>6##aRD)7g< zgF)BK;>efuviHoZ^zNujpKo$rq`0RY>~9xApfP)7CNy0^bU3+RG)Bg&D??~n+){&G zGC8(dr>RTL=QConpBC*tMEx$h&GZ)NoBGDhXReJm5u<%Wz`^ekl;dp_u8M!aG0EGKubt`It=1&{XtRDZ2wf9;uwFkAQF4| zh4J)Z60T28^Xf$r$^m;S3x-bk7mjGzr zi?me3t}TEThdG={A#5p&2(OGR=c6RWY7h+DUh#Gw=_0yo4@C28Krc;7x^`MZ63lGg zvXsdijU*q&t$MfsHJewkM=_S=f2Hm73$ce~x>ZrKU1AOIu zd?HDszxX}7CrIp3-D9r-NAP_F|AZEc=?Y@PhuW3wxQ-Y(QLASEj52iV)YYRh0P20?J?Qvb1Fh z4EWq)uogs&wFjqO#GWY-l4a({Ut*!~`5G2>?Gd-Xevr4?17!LBinzf>%<>%1;t!PL zv1hxU<;g=~e#0Se{1(v1)g9*xr|B6(be>{SwBU73dHvguW0O3##xPHZjdYrb~CLeFURW&;U8EmW*jGw7TzsW z+GAPxw+hf3bDFTWz%G-Z%kkaH*VOvnObm@b^RMQ#@vj!NnOK2&{XF@~`FV3^Ge7K| zE~wi_Y6TKxTa0&I!Ca9nl(mS7_z~o>X7%k^?Dg^UX5lY5a!7M#ksD)O<&Mjk)ZM^% z2)kuwfSbm>;GcbN_+=(|_S%ejuOwWNA23?VEpp2D3CFGc(({osS0~>dJ3NFV@xRhu zxpBOI_C_-EDq!Ltn?x~2#P>I~mvobIeZV-`&w!VC{pS22w!Tm9Uyy;G?kVs50JpwR z?pKiUDczI*f0)>;@Avr^5Mrf!YB)cTcY=9Q(qDqku4McQylA+d_158a}GQ3n=&xL1KIi27LSQ%bouIHa;S0tTaUexrLsI#jGzXC5( zuIGxgtFBHkGJ*^*4cBwz*_A*i7#U86mz3){^X$sE6O0Tm!%OjJ0@VpdhLzzR4^4mh zo>tK2B$nQ)?fl@mzW?Z7aEkT!1TDQ)#`%F~egDy~05VZ_e$ZOqck?ee#YlfCJGV|WL9apJnGJ_ocs#fNVuNy*Y{6hWmd_!JQB{XG&{koDCw;d z&JT3!`%QiYr>N-(Jk1*hogtpd&uNbCEYeg&tW(qC%NuIl^?+Gx3+E!Ov~ z5M;b0oFCLX!K-$!)GF}4C z54fG+)lccIQqB)7>-&p-1y*S3FAo@7`$3c7yaH{;^wtY3mxtCb7gu4EU@vGHtXB^U zYJz2J}t$mf1d#9@3ZQC4XxSu`|Xxjp}_Y#O>%jiTXw zZ2WOD=f9=pf8pv_=yxl|L{y$_tBY>aqWwd{Kc>#k{C@?W0)MneuF(aMV2B)K9DUYRmcswQ^D~ z9Vy$S$)Dx#YE2>5-vaCY0+FgZ5!PI13bwAP(Gpux7o$_av9WUSF2@w7dpn&py+riS zQO^uI@wA`-018o77n~rJ?LuvrD@)KR;G8p2W8Il%ZJ{#MKZ5?dV^Wkcp=nsJ!YY|Ey!N7oGl7qOC1cE5R$ynwMBXzp7bL9cD z??-HZOGv)2jy$Ai;iuIxIYw7O@R#$y$cl=pbdTL~ z1s1FyJY-~;cE9?Hk+yvayGx3j%MaNI?wYbN{i~I^BjWkv;}!iOtJxA=)7QMbWUs); zEnprBZuIQgu4bMCM zrD~~{U}n?#$(fstx_-<0M$H!aZ(V7m(L24a=bGG77+n~md@sR&Wx#Hvi|Opr)4*!$ zfqs#c;%|{Z)czDLIO)f{VWiZ^0%6r#VD_&jjO-QhpkvS1E<-kDWI!?J4E4O%S5Z_k zG>9H<{^-F+MEP%zyV>%rLJFg&9xJ&yYX{So=u0d=(kN!4b>7u&yp=Bq8-rcIm+Sum zwWa)HHgBd|(1lA}MNdIAk0;mbepM14n=&3I^H^9-@5&Ts-|Kmo+~8PrS>Co+xh^B4 zJ=DH6ni5{e>$Mn)UltLIu&QAFVC7o8i{8^K%tw2{@U?Q{$`FxoXQck2=t|3k+s*!T zrbc3SN;*9@d_XJ+4u4~@&aSUPHx0>ck)>2GJI6c#;lvXoa%}}z+~hCi=_?EAE0^3! z(^1m<17hJC30=2J;X`sg!iaS358Ui!{DG%-l{|bU^k|2eC7fw2Ys*--8OdaQLKbD&nn%c>iSqq*Wj7YTIpsfN8=&e z+fMB6W-wS!)ziPsbck-&6_#9twq4lHP(SLt$bJaVqoJWWtMq5f6yQaEPBk5~^r2QR zr%c{BgRcLrk+-c7eAX_Q@MzqxQNYWw(4@*KIMEP(p#x11HL`;N(N@R35(r79q6 ze!0rLh$h&Bfyb`B2HqZV3#OdMU#T7Qo)-|Lx zT!1%Fte?y7z|g8S-YfJOQA)hY)`9l)r<@m3^~bO=rk*?c!5g9`R6b38?IS=h%hOfW zS)M5LnmD4B4V|rsfsw5t-y=GeG&aph^-JFLuw?fDed^r2 z<0faW4VjoKIiI`v5ddLI4?x^k?0o}x73>pd1anKZ9zAzcP!0(2Pqbp>YehH zU2T`Uzo6&u?c5bKhw=v_E4~e;4xDyWC<-_c^_?K2R1@b1{Xh)o{);@cM3)b8Uft!IG*mvG%TVd1>8j>r|CtTt#8 zCj{XWO!*1sQ;iIl8jt9EuB)U#p389~0^y!6qYY=KYcq~Qi~;X%#uFyS6W11GwXkZ1 zsnxfd0oB6v=d0iGH~~dX_vl{D_%Y`41~OKry-GYN0>|5E)Hw^ebkCLOp9*CJw}>2E z3Rj$bjMHSXKb6c%7lv9Yq~y$AClNLKd4}Z5QXAvRW98zJ#wFZs${(rUftW`mS3W(u z8}MY5kFpk~Ie((IQ`K|A}kKlXv z)>u1EnRRS;0O1g#qFT*r>0_!N3fv`6WI_=`{+GnR%zb=<7L76wHv3n}-jsKyjN z|D+YnXsXraR1Fppw$LNx#I8n)@~RU1CatCRviChl1qgrcH_~uQdQ{Run18kLgl>sh ziJSS;T+M;~to`lP-M-nhcbWHz_c_#T5q8-}Ct>$qHBe97hIEjOYVm@mV;%QOtCHq| zm)>i_!|7gm_%UzG>(fg!CqbjQS2nrgR;4BP>Q+Rs^f5v5yu|!ZFgAi_nEj`q82n_P zSZ`*0F+~>s<3XlE515*jmQvtEDbgfLWrz(w!%kW}Ve zvykCi)Qtni*{no)hxR?=u^v@Vj*LtqFv!w@6BQbTVBA;}7Zxy(0X#_r_GuA{j-e5+ zm(3|VQplQxmAa_4ReL$EcHgdk8Zx<$0Ry{C8U}w^&Ekw{hD?eZN0yoUk9+bjy7(T& z&mP4`IZ8M+k%2pv81d46`_*FLuX=dvj7FJc%@;>D+#lxd0p}~#5X z7_rbMwOW+H=F!FfTzn*=)qWqwcTO@h+t$%)R>h;?`52x{wc|PZQfpTHJY7%4(rHyL zT?EWo&&#FqxA?96J0@tVbTMLECd}F4h%; zG0v#-`(%Wevf5jj)$e0=%8blzU>gRjOzshfoEGvTw{MlLakxim8%0;LTyuQ^yGL+r zx||8H2F|y%;>y6v`4v(y4VVoK3;wP^uVAF`dqBrj&(y-yz*46+Szx$hhp%ehoVnPr zM{-9=ae>Gx`{hQ2I!AFVw5Y5|rH-oc;54@V8pBZOZ0Wpa>t6h2oMy)?O6I=nxBD{M zWwGSKC+&!C_Y9Zu8fc8rW%ZxYGOB*m13J)&nXc2FpPygsm>A=KpQV51&Zs*}@8pj^ zEvCzQvO!wi&ti+<(-6VcP-O{DjL{h~wsBAyYfgp9EtE#{>W;rtFt>20z6iv;OE*>6 zlbBYnX7RsoU3o}5G&BeRa$sSL#ND-42CWdxpFtL7o54X1}hGZhfQ8fo+^n| zy0Qiv#AT~{O(24qSy&5*S!!#+)FtIBaNu2x2^0C_)(vaeBeTf#tdz<4jqcWs*jiWC z;lPD7{}+3sj&A;*QGQ>sqmr7HU!BLo38Zx^dD%_4_*6^FLYD!ADViq>*6;(g;hTp3 z8*HJ^bL6eH$cA4v3420$f?mbG=!3GEifLZHy9d3ze2t=ZdM6)cb)G7pV%5PGn4|mj z!NH2Ani@x0zG$}x&7H9R%hp|QlPw-b4;dnJ!rn+vSt>l$0KSr}+(ugD=KsaDI0k0| zFh=;|OD?$Bwr$%N+qR82w(Y#JZQHhO+wOdA|FknZyED)3(m$h}v`09>P_InQOxL(o zcFf>Yd-;HIXV%=%Xz?$%e?n$FJRQ5d*WEuE9bJrQp6HOrB=eI*9wAVbGAOAM+6sv3 zmVra;zsaK<&`d_urs&WDak{dawTzlegHo#qHanp@>b=oOx+poBIhE}C8IFGWr(*Vq);|Hx^c%zEnqefTr4 zrDvWzA{}AUzwIZhi933&0F(gnb|zkc&-?u2(i!#wT!R^Ue*2Jg9k*d}UEPAw$p~Gv z4sHkIaw&Tho%?iVH`jBdIv;jJ_!MXx=YCp4{6UUx7OPh={N69-cuRy-4sON{0_GGc z`=$wd6QB%jytYJkd6b(-p~^WcPdfR@I@!soIE~Dr?8vYZ zHLs46mogzeP(2d?S0(dAS1Gf^tav@{gP9A;G-SU&a}MnkTjGW|TjU*YtXIjSeD&>e zT|bs4c$(hN3;w})Emspxz zX2+&AGn2>O8v*x1^hD0BXyp(pwr)^ZATeU~Zea$yl2~CH;Tm>I!VPcyk*X8q&sc4AMaNT8lfS-3N@}rvjH4uDXxy_iaevHuk=k_ zN+t4(BHi#Ce6pDu)B3iAN1Ws#ChhR&c-!*s5f-YCyh4 zk^n@QJ!@B#k@pnOX&~{fI-j&n#WYhPuuHZR;fLX>P$%r**YFqidkm>QXXR%3hAYu#9TMRNe583$c84X$!LtDplkKJab9|??>?0cd-C5!BvK@)?pd)h#hMh5 zNm3)sq^(|Qtpewy$^-hk2VGxdDCgj&uSi9OxzO;;NZs1KzjX`6?%W8=%;#b9#0+^F z4ah4m%sR)qvRql}(RPe%6U2so*Fl-iMKs^>_eg58uSp%)FXJ7fTflCS6ZRxs%m8NE z8QsQ!op4)fQoG%{csc=D+ov94ggGW=KxulQPMPv%4f288k-gP-ZQ%QNB9Nn{=~D6; zC*@=fjpn94@Y2XN0Z^uoM2*w1R@JDcm;~flur6w0lE(}%Nnpa2PIoL|f{L9*mMW^( zSewvtWZ!H~o7ts)%Xy>rM)+wdFO{^6$f%ByCogNM;9k@@_j!T}+={cJV69~8G>l?& zWrSz{$H-^CY0hKIW5kpJx##Etw1|~t?$}fH9r3U$f7y|x9LBXS&P(=8bo@K>2b-RG zb11}UFlwBEFL`HThf&73q&2W>X8g_Ar|TJGMydN?VeR)3DcZzU2n%0)O589TMQCzO zvfQFQn+#{){?bmvN%9#FwwrT%&9&;l(gxg!Ls3WUe3F)W(>+yB_D=Oz@H}kAs^9>X z2|DMZu#_}cnAwxgIN0i`jY)ld8q6YH2);Sf@R#I(EXGYmvM7!Jje^=7#$a34pxbF$)hp$ZmXGw)_c}eF4HsAeSOuXuxhZ?{?jM)vL5ts!I2V8B{yBi&i=pV zP^e{}+@vO@{xyQ5B&Mt${mrqC{bn=rE@5`&X`VFhlhQowTUhn##?X?Zep%NB<@!0W z+m9M6-C5j?+MQaAhf74Di14VeQPNdX$jLAx4TLj!(LCB@pXi|iUZKZ2C>vfk|F7?| zzt9OrJgKkU2jZ~T_)iff6S-?e&~jo zfWx_u3|nTK#;dYm8YJ=Or5eE+-yk}nD_O)GJ}gDV-3pIk89}AE_Q#m3`+FIDheM9! zHfAte0ckbE58iR>P}3bknE}jMQ7fh2?y{R=iA_)x*s)uGuUu^yqnGgJ#18(sls4mi zbBjMHvA0jimqsy=lZA+5+5JU8>u}%4YA~F?I9c=k>3kr=~&6lrv#Y0RGDvu(v>n z#pPNwC$~B-g*$?6c$7XxdG7VvjK|E%<+Sjsr^^LTIj6KrAP2<9=Yld!r~ z@-&D06K0LSPj6K9@UwVR!6mCUC%6$X$yNoJPXRMq-UYJd=}cI|-Vmwbi(jXrWtQ4| zdLu;ULTo3rLMJ!-5Y`g$0@ya(N3)wCsC0YM3x^va(aK6aGLfjr7bYX2be8N&)ukKCDPDm)*o@HciAV$g@ zL~a)|d%Vf9ljb=XlUc4vWPbV?(hD?Elco1bW7GED9iw$r5 z99n1d^STUe&(JQRDl3a=YL@Jgvi|7bv6Fi&c)D2n z+D&=AU+B=p-$`qOd9JeE+{t7AA#^g9NFwEH6gCeyXKZSX&`DM_&zzRBSx`J)$J0kB zvTo_J((9@gmoFO%13k% zf47_>?6*igms`fvWQ5KP-(tYp5J@JcU(D=%}8@}wyQSZa5HX>5q7E(c1W>J z&cOrbL7|OHo8dY~c)PVYAjSq`gKZc?xa%g%tD-cm@Mn2a>!vE$4RCVlG|l&M+3>>m zv2BIQYPnq6IjFIzd^hw^jM{coOHPJ9qan#rA;e~-A%)DzEx7%v43k2Z13?*kKBy1V+Y3f5D_?d`X@2oz4a`t`W>%g*3g1-izb-i%;@S7AsJrWwUb8vuiuQW)&iX3!!cDX|h$!f}kWET!ZCOB0V5NrR>)TWWSTnNkVjlY6v%k`>)Qj567K zvM(CRl$1u=aJl!bBvPq&4AldeTkj5S zPWIIaJth~;>3lRVUkmFN76TC+lSHH2mLAk_EHvh{->YVJJVL!p<}&$p8OUr6KTcM@ zzmpyAkhy(^bSN!ynpSVG*AU`p9n|)H2|qB$4t?l`oJ>_2;p9!$OJ89*;ijSvS?F{{ z2Uvkk2gz7)<18Z1_%?y=o8qB@uqY!79RH*XPrY#lLz+_mbBwGdX(UuSHhG zA(0#4x?$PVakw#zlVf!@Ie38azt<^0 zWN8*vUDM)i?u-q)mkh)T2?ud&{$+IfaPRaHDyq{MPvT9G%u%M^~}E z&p}F;z#Ym7j7%U9h{De31HICEqIF=nu;Jhahcoc719FW7>`(Z?yb$k0GrW&OQ-d+6l$z=MiUUwS-@R1$L_M;?eoEmB z(@FVo!Kl2J)4_K*i{l=DWPgzJaYuT-DN*{fruEgscTBVg{Z2OZ^J>rP6{|a>X77T& zwfZYnI|p8GVCBavl}MLSh0*tas{dX5|9Jm-A_|&3mW$hsHL5f%iUP2+BsWn^C1TL| zgZnxR%GgRY@IQ@OE7$z>7aMY7{Dsf+*ubp4e zCK`c1uO}ZrQJW&>kv#`9>o5)?My>HfP(W<%D}qv>5`{N#Dlry9M7u6uD2g4?Xf7O| z5_%WLhGH@Ie;}HT#91lB*OE*NjhI#c`tE!I1>G&7?xkE&0@&zr)lYxf_m6<9FwZo&ItpkyOc&8#bTg9x=)RcomlM190mlax=oT#8{=+q|~Z zsswj~BC@K5F;etoiq)ECG)0e(PMxd=cAXyfhA8rZjquvw@J4z`mW(tqwvE~IUs4vS z`oXOQchv6s2R2h#@&}49)5=~VI~DV)nuUo+)HUDklM}`Wh^Zhv^?z=)lzdS8wO~x$XNpQDLC2oWylAtJlUVA`+)3*oK%H8 ztL7$_G&8lcNi+r+X2*M76ee^`>?xZRwk{W#11RnT-TU?Y@with&7?H!Wh~jLe#5Tq z)l%eQ=o$@d8Cov#Mh0wJn_}y{@l|sh=07HI$7I=KRGkdezVG*JnX~J=DAs7%`mFls z?`%;l>OrpAZA<1&=NUmN|DsNV-lQjv4HqY|u8y0@vV}qs z%9h#k#>_43`at1|$tIZs@({Jv`NaPQM7E@BW(gHLMC4lOh?>ETF+sHTsFDtEi@aDb zYgC$E!khll!dnoo*EPg4#;S`@S&_;%U^84QZf+`0X%4v+&2tVcEuW^*@3B#bDVkch zZGe&G;%23Dhiy-JK5-3#-5j1a1k0ZeDzz&hkB<~9tP=a!(LjTbfqJk^Ortm9t#2{@ zq4~(^r8gNd4(&Hqur(_E(jwD(DQa@oOBg;JQ#zs_)l2Rn`av7lxTGJryxa_T0|j#n z`tafzpR;B_|1`}aLQW+7I@Q$Yaqs!giTbM7md6|8+72?`@>Fj;Pr|(>pSv;d#dwE zH_LzQ7WfVQ^Xu{EjXak>Q4mZc0CvW_9Pyghtl&(u zmw%9-7*6Bq?2P)w?KzgigE>P(v##1$ZY+0M0m@yj+^1S@iRxNduCU~9E6!NH1{rtx z_!jsV+0uzaB~DqjO@KfW2`ZL7L_IvkfF)+IW-eWJuvZG1x~kH7ZUg?V-VvF)rOpwG z8lnCkl-jPj+CoPdufZ`cB`YDBu~Np+go-eaz5PU5_q{4eU-pMAzPpI>bmvB3hvHH< z5P(=tD@j1CpBle9N0A^SmdI#KgDI}Ks;36C!*Fx)9;cgD6mqUU`cs_2hq^-K+eA>{ zA;G>T)SReejL#|ZKI-6@kwtuySaFE<)gf^omFR?J3!6HI>PV`MsW!IcXle`9lDRR1 zd;H#pyoq@Y>V}!15_G_Z{Ejy^ z$Du=gMvr{C$g|GJBu_mRpTP0G0wuBpnRIfxDhZhsG_1l7am6l8rb+Ni*}5i!W@$)q zi+)+Q12ac^r-r|xr`ngIIiqQ6`B-+UV=#qqJkB!BmI75vEZHH8aYmhJTCXbUJk+r3 z80SE->C%pK*|F~E-$cW5(DLLmXC?b`l5K_s6|;q`>H^sU-2&o*#RBTW7u$TwYRZ}_ ziRI?B)FNm8;ImNObr`ahQ<=I}Ot^P5P8pps;>emzP$cJYWg)5A9xu?NI$^;H?;2L_ zb*(e-<$-&xDV4t$f50UkKW@wfnj+HJZq#*rdbD~x@fE}a*r}{b){{%2N~5ffdV!*b zvXXo@4r6@LHSRQ?*?KK@DAFj7qa3XytA<*pTG3oOU0Pj^Ck7K=J=$GbS-PO`vPk6; zz@@<}s8g|BdaabB!apywqHKB2vX#-5;l?Uga5NV@w={Q=|B;VBY;Jfwm^7F)(l}@x zy%xOs525p)F08kRDM*8Vz5xEqNAJa^S?`JeiU)ictoNWe#QW>&udlU!k)(K$;p3t> zjX*hu8ng$g(R)At#oTI$cSbVeI*f3rc}66EPC%McK$8^1h#%+ME&#W4al@GftzqBa z`H`Zx1EjO*KddJ3g<-k@R&&_U6~}#Fp#9KOAzax8ykY zvem_5puhgA==q0Mzs;241#0`)x>uBAguhq}dHLZ|ALZz$dbFrhVpv(MhZk||<2(R_B zm(W&o_ixQFA}|Z@#od}$bplHLJp(m9UVTF1zNWw01+7^Ee5u%_`?|lQj_w70+1$Y^ z{#JT{Bca{T3E(u!2!O}@O7LgwT@}F10sW0JA>ak$+N;nNS7AbvG5<{?i;{$mgYQ2O z@b*xf=s^PMM4M*i?*o{vL3n=>?BTQ5>}|v6db%s{n-jodz)XXwp(4ClaXvaxGuf63 z1T5$`C|)Z3k>2-+FPNhW8geQ_nd$h8*gjZu-7u}ptDM+SS~BPh;41q2}Q z9n9oYJL?mo_Nzhwb3M}UUI4v*q~_HGqXK&KSPk6u{wWu(bS2jo#Ex1Y<}rc8hmGSH z-1E&aYSL^TPiS3Oq65xCobamU)9}$3otdhyg~vXtMp!HKPSqS43|hwk$+IY()u6#A zNQ2i}m3nyH4}KCSG?5jHk(bNcEshOuzLq%w!NRi}W%JMM57qgR`HEll5S2^r1(@t7 z)hySn=i-zfh?AOdf$A*VqHC}JQub&N44Dfe<`liKPHemXY*@}8OP^gf1?lI)itG`- zY6JWq$gi6Q6*U1VUTcY~)d-&40N_R4#YS2OewK5~B43x_M=zV8MkFYHZ^B2P1GZJH zsZSY{8JH#5VT8|Fur_ZG8Z+u{F|a@zq0a%hrg>Rq=A@2rx%#KdOt>FM;PF>J2sI7N zrysp|7Y624$EhBkd9ZGagOCvzxGK5(G~Ga=D<{(*6!V&N0gN@y9R;`YNYY4^Cp_N~>hL-{#8)>{)8H4*l zBVcw!^$3P36lIZDVo`n@>OV*g)fgbWlRgy!?2J|~1fb?o7aqj#YG77ryk39pcsqkd zh2_*)9uvkmbhx3cJs7*jPO zol9`vXzL<(nzL}dRJn+svZXG&a|O1Lu({DRd=t4kC@d2v8-kf2e|CRmaUjIcr&IlT z%Zt)zgkHYvKUcOy2#?EmAt$OtZ%=02f~9%B>+G{O^&8>rPM(!>eP7#za~H~AyzB+3 zti{5+3CPl|bjC>G!$JG?jIPWde7NgwdDLvbV+6*!T-~iFg1b zSI26XefE;DmG9RIE6-OCwALcQN435e7yVI$K70xJ=?+66FrS#l>~_~@bUG1|x^v)P zrt0{j=Vn!D@jj2GAlJ=G5;pgz+F|{!kI40oRF*aW{`6XPU`eb0)w}CzHNQT!IP&`- zrrblLF){4{-qdHP`JtLLXKr%#m-n&%>g=1GAT??(r^RTAjv|Wu%q4=yFo}marD%2< zqHL{+_^z5vwOGO&wkdxG`gXiwUg|bj-xgTp3lp}ZvYL8*@DqDqqKiMSdKdoGF?5zyc|C^*@`3xVa1hSsJ&H!!iT?Xt`hXwY{2&T@aXxK075O)Dgq9ur++~& z^leE2A6!3`=JZ*x=zYly)>7wBS5#p}aYh zR@+B51PnZOekL9(A4_*@g18N{Mkb2rPC+AO#%`g4mADn}bU|o7+1K|ebj*8!8;Ar_ z@HZtvu=ere2^3+^^RS@YZ5cUXL~}?fus>1EnB$}<^SmIUNJvqnu%5Sy;2X5LT0&RB zgl|dkR_wg*2MjcRbe{S;3o63wyT5E8*#e7Kmz)y~=%2;vrdxvuSFZ!3x*0yCKj7C6 zqUH$lE&TSf=#mM|dtOST{lO+znnI{=;erqO7qJ!WVBZ9z!bJu^CV2_YksobFM+A2u z0hmdcj{tdBE@KcbTIVI7X!@MkKK`Q*J0O3^p2yA%*pz_9d5!#Kq15l;e{2Kn_t+)O z%0xa?Mec^??Gy5pgcwGDqMR@SK4*j@TNgrx^!IWE;_QIVWPyy9!`qZXjIf&c%f)v> z@qVvTv#9Vxx2YR>>Q#ff6aF@74Pd{A$%a65n1m^G1#jT2@I&}1gg%FU>_NajnIVg4KZh%Fx(V_p4pOe23_NN;y~{-tEmn350cQGCI*fX z?jqkvOJ8!VvDFk#siIX-I0Cm|Jy=@moRY`m==QQ4*aJ~6hS{CVPz)Gp7-l;q5P3#W z`W~J?+y3NB^gn#DB85!*FtA5*!Ds6+l7nAdte&{&fNs!N^&siBPr)X@MQa6c!he)- z7_sbm(=m3k8=o9!hIm0jO@oRubU*Aa@SMqn?qC->?f5jk5e?0{f5?W&Z~w|h--ch~ z8}&`yd*gZV-u?~z@TldVjo*$JHBg_8ANk9ZI}V6!R3u<#q(tIX|Zm(n^vFk|tu!M>jlLalD5hZq$i0u-qrt;{`quwJfA zoMIxmCZ2Je>WGP`cBX{9b|##>PR_&W^abkeXBS^UD#IFd&jhocCqL!x?WPDuvNq-_ceN-eUoGeYCN@dY@BmEwe5!5DtT%XAz7>_{-e$@x;}WLiiMhX z6VIiPD`BI;M|x}I%gv?d9eq=dHEVdVOOoRnM33Wfu;dDotwF9a%oV#9%$bdFifp!H z6=Uui;#Rqc7xock#4oWIa~J2eq`(KEm1wVBc_+ah!JWW=K9h#Ez)HG zM+nOunu-9jx4}~w`kc{^VA`PZcXb=TNG!>kyDmN9QDH_U5$)i=PGjb^f3AsOg{e5? zW}~9_gl%$=oT%{@>=wN1Rhm7nAL9iz!OmTe$K8 zu8ulNTfa(4t~joOG8%GO*1>HYx8USERg0p_xCLL|$)5r>J+?3(oR^?vpujN)!*vJ~h^JNZAkWOX~Cew^wsj%5NT z8LS5%&+=s8PL8|nA;z>AX!-6n^Yfy*FTq&Hq<6BrmA1GqTm4Mu$KI;hS*Ajqy0~v+ zL%O|;H6^0bP-{9?&n~aJq)gkf)0*g$GV|mA1I7ZFBu1w&rT*J74r2llPxzdI+~x>D zB%k*!{=|<|^#11won*fzV}KMkDt8BZ#hyIwPZ&gZ3xO?G4fAa~Me#emgX0@DmB5p_C=ucVu=zZ`BQ;Vyo%It4FWRxzn@<8Es@WjkfNCAW>OU9qOAakG2L z9=o2f)u4WH&+46Q9=Xsiald$WnRYz*xfQG(@LX;gK68I`+NH5`#(KDAv~bCJx&=01 ze_l;9Tidp{;UDIJow%NOZ3pJo=vU#LxZGYkY~SNC-neZ)JO@7buLR$7jSEkYY#$u! zo-p4~J}98y0Nl>#Qpdgey!%lP06X66$){hl-e?z|Q|LNIo$QmTIsq>+Z{=3?FWqm& z)EghO-YcwIA0p?N_EUpYvD!|vPKZYkNu+5mb4`Dblva>#a)VpTy&WUxRW2d}K7-?Y z=bFa7M)DmnJ3DBh=@RLu6E5rD6KKllQth%xw~n4v!%3U8E>NY>n$k#Tnh-yOt7*4Y zQixWtDW8Bt`Y^n|ok6zYO>e@-Ct4CRe6-FC`j-&3iRaTN!vR3@J ze_Cj*u12v()+PHf)2$_0f0P)m) z+T6^h%eLE4X~VstLZz-7)T zI)g_cOUw%-OH9vkZ&@0k+JIDMXVWs(i>h8yQ;OzNd&H2ypGY}0R8=q%wOK(O!^&9yNQ#J`<_`9pRZL?=zi7Pw6 ze)r{u9j>$96Rx3|akBfh^*sZZ<>)sHezR^~`pAk^q|?sh^G3p^TfAfl(OaoamRGNv zVO2y-b#z{V6*4Ao$Q)<^dFYJh$qQ-v#im@m1-<9*#;ZFVFu=MK5;alCid`XNr%$Bw zyoM`MftyV;wZj*-RY}Kl)ehwm(Vd7Ja}E=CO*Uqu(*=lWSG3g$N5{%dF{kSn-h(Y@v_}ruh^P!vge4O_P2h{?j|M+5xM^bk~ z^8|3jr{Ty?{D8XF#1qLI-G-Ao(L2=!0%-hr=&lGzEt*>cI`3F()sauQEvcT5J!~zi zdckx-YNeNqx}j%1tX4woR^BFkKsKT-HZ)tx72U)i6_yOYEUKQ&y@ywg6}(W}RPfUJ zM8B1fTz1)6y>NU&&zk7~lBcfEs&@<@+}S=7ZG2|fe9LVT%|iKcts0R@FLE8-D#qxDf?;j;bMrp(w)9R=aJ{OMZ!c-t-Yof zxS4#!u2j|j!tL08-;Tfu(+<<&h1<{((H_ec(Q)jOW1r+od&+l3aY1#%cTsv;e%XCm zD)wU{uA?UVqH>3KWqv;Iy5n4VJ%2Dw#3x$&(NwUdvkq8c^w0jPbouDg!g+QHwRam{ z=zkA1{M;?H7PP)}+0y8l*>m7~&e7JqcC1q4{Cq^=W^-wBM&M|6P0w3;rW1c5rb-+J zOFlbw*nZG)zC6=h`9fjD886-M8mr40N^s9Xx*4Sr=Y*f*fcoi^pA0=pyiId^gg5cK zoxJ3bYN$H^pBSc*=KiOoIo@cRV$m@051^jnnss%)Er5gG!Dc5Pa>6@uRBeOJ5>uhW z%|^0%sng*NvFL`KPaJ|fh~C#Y>*1VZ?s)5Y&rv=v3hymWjD1_OolQU3H1?!Z{OFc$p7a>5xZyqOwX`_n-R%gi-Smjg zr2YW6Ha#|f1gLPcOFm;sJ#|hxzK!RsR#$?5gtu2lL|t?uuOAroyn}Zoev1l0V}>7L z?IPibTed<@Pi7&)Fb9%`5Ht;ITK!f4-5)X_(ywCWn_x}RJYY?+qOD6gDbl@YT2{1b zj&6X~7fLO<yA-I{|;<~$*_w;)X5N*%|l;C=7)H* zITJBM;%%s-wr5#gTgBzTyR^6@d?%Q9m$IC&;;A3rJ~qEBNCdswb0|^J3VGgC4Rg*6 za=!RA_khZ+=R#@Eq~2pGzAIz@_Z|FfOM)FOFUR6ioIkK5em%Pt+u{LfOV-)s;H;N( zGaa0+fBLU$r_GZ2o{cmX6sU0D_3$5m#MQb{5d@;>w~cZ}{zJ${K{|aIjyRL zXm+v}26rlJtKkPVo3-*!G)N=Qj-lTV5_uz9hU?4zW&^y!^w*fGk%`>uk8w8G_C8HE zWS-RXsI11&8*1WBB%O#$X`2b)Up;}(7>zKMF?N!G1M`pY=Z3423{#fDMmu4g&N@rG z3ENwpDO#_UH@%(B`k(0jC1KjGD`u==P`D~9tTzECEBhyjiM{8`r4`MXrjwe-<}7o1 zbwIyk=|=J0Nkpd%Jiz-N11aba#7fO2Zh zz=X`O>xz1Df#ovVUpOTyqp?Q$%SIycdC3z&lKe=Jhqwg_B_{;0pi7ohkJ<4v#<4;HX?Yp93<;9w+RsH$t}nZDf=c%z2QdPf-DQsXI#s8 zL>KVyOZFka;>s&iTM&D(bdyuQhq47`oUg&cqPFy__FJTEejEUiWVy4Wj5XWVnf73u;EBKsq$@&a}Z4!NnmZ@*W z3C!&49=ADUtHZO!e&PEtyXJ4DzC_q}yhDACUnTWPl?@Xg%yA5Gm-3En2qWIVX%!#R zg7nX{9A)4c;yrx}XQKDKg zd2cKW-Y~MLt2}{AsnOtiRi9v|-FO?JvD13K%c=kDbxin)I^vyx*tU2>n4tZ3N{P*{ z{7k*$r}VaR)_x@R)^ZklH2Qo$m7c@D%{0`F_Q>2pb7D?8@Wso-$(-y#&qT_cMi}J7 z{b2dYT&-NC`6fEZ)k|5BwV?7bD>e>?nZR_iH60^eB9)d(NnK24Nn%N@q+XM+Z!@+U zdrr_H^^$r{#-ZR;c5FO!9~(vb-|ln&=KSU)yM1^)NtejMWV$EyhGxD)^@d=6$T>l0`qLApH)!}a^og#4 z1JI-Z`Ud+3^N#mS_rNn@zp+}w08CFAPj6sTj6aL+QZF6M7d%|UHBg$RsgGjNkBhf++_~14A=&Y0fqoetcTs#-A8y%(W9Lgg>+d81Dved zu)&0+qoymYoYO8!&7iAC#+qwPNe%0Lq=D8MF)>GAOS(Wqm4r6|$1CAzh{}^K!#TnWbXOGjjrPNJ=pTh^q7^DqZ;mgKau zS%_1}!ci7fC+57XSu^vFhTjepbC0P{cJ~uH>av#X-}}`bVfPfBkT|&;@5~`Ff>l|Q zVkWi?WrK2k_ywc|B(_iohRD+usa050!%7)Q3ZtwU$U9I!s$tY>aCO4N6x6rTj#H}cP=kjkuq}de z<;Z4FpStViGaxhQH2J`jYUU`Mka_GBUHUzn%)n)Q)n^~)E}m+0pAgS;?;btP&oaJ8 zFXUZ@F!GSs%b@@syt%fk|DxRbG4y|&OTVg?>LjBbq8?{fbN(zcl~SjiDH_SL%f>9X zY;{8z7_H|;vg&wxtDxXN(!wvHVjA(P_n=v|TiFPDh`E>*yYqTSMI3kxvsa6 z6`HAuXk8aoEaP=q1AO65feQSKl9AlRK-SE~jr?W!XtzFe8tlg^1Z=mcqmRZPU9Pj7 zrnjQIyePMj{>Z3-arh6mgVQvAFW-|AhDSFNdC7EU&cD+;ayE0T?9Rz4b;fh_=TVT*%qy2fP3 zxs|Gyl_Q02;Jy2f;5oA_eQ5{JoLeE!&`L>5&%LvruSfBX^P9|$z$ky|cW&n_yUc^y zjb1I^7`v3o$%k#wCC(%D%eyVHd{6TGOp@CBaQO4bj0gHVeSVjx)u-@w(Kq4^%n$30 z)Q_w;89%~N*F!J4rPXZLFq5OTs<2LZ&NC$0#qXk)w;OkM)!L5XeV^tvOvV=c!y2N6 zCBA#xG6v5Ul|ge)qa9B-Ws=hmszTv4z~|p*vnD-}-&Ix_+2&``p9)74Uw7ax--Gv% zz7amWw06%Wp;ayIF3sNA{LGF3>Yd#jNuxY}GP4H$*!?OW@i$jrgEzV!k*7_l@t4;V z;yC&R_m9u*Ssc}s`FpVM@q6KK%Pu);{&PJc8}4g8pB)!Wo14NO_>D*#8vX;j-(x%{ zcFQknZ=yf#_k=$o_lQ5o_g3FkN9RXP+7U=D*n2zO*&?PllgsSBDi}SB% z{P=^i=_+}^=|XOg9`E=0SjTg-`p$I(iCy_mEBk=&d`An>Qb!LFvGq)tBVz3oMMaCD zm^r+W@7c@9`sczkVL9LS0P0%nKlfOb!_Z^idpTv4-Oq*87hriB`aHcN+vpRp%3fjZ zq5ZAsK`w(H<|jdyue()oZ{@S&#d4@$zNd`Kt43`x=qhy4=kX4edSDpiN>~fI zZnG+)XuB=~)jfi}$Ku;AO7vuw{&%!kEYs(Z%!b~J&k#W^bi25Q$P0xY10M5t)P@v0 zMYeNbeWtDa^gP_S-5KOjeG%#S_)LLU{DU+7y;VE<8{AgpXUesxPv{Gg5Yu+#?{s&1 zBA)0MA|bZz$lsjy`B#Qpi7ikupvR7{xPNe2V~S!>W0GT*o(;r%=Z;rTSNB(sSNACF zFCkYiR&P#ST5&>gM=l5A4uTGn4k`~i4-yU{4$2SO4^oV*;rT~+$Zca>{?!|8XEky* zb80v_o~+^)IUP?^W)RO%W&$=DV}&S#Kw;=$URh#)S^VmRX6W(D$%*3FsP;$Hn9$r+ z?!fQ2<~re8rC-zF7=sshrFpU2lw?!mA?_Z5x$B0Ja;!{Hk!xoEAd_Cvgt^j&xl_!o z?HcW4GpcYCj&!T23SkrWaube}6ZkDqy%e)mcz`r}$M{Z%4R%onLXQlJU}o`&yFl}s zD02*G0GTr2xaa*v)4=@aRC&-a9h@+xjew`$`1hSPUy!&4Q|i|n@PA<8j_BUMrV{`ZCN+H&7A)_7}o9=oSU-1R&{n zLlZC6VLwq{L2U@x<5HnCgmSf7NTLRy1A;UH@i0gQ$nKck1$xcL81H+4mWoI5)+a}L z=2CH=(PEo3At^;_pw{LRr2%l1(vY*yd4hDi3+}~to^YOW zo(3DuMkIUwpyskn-GxZpl4Ar_k+{9PUbYHH0P34EJ&fb%j_krU(?X}O#vr^*>}hWnfhW8ywkJT7O;`Er3gPjsF&R9=xiF0YhV%Yp1+@I7d4{V;lC&a*DwWp>3qa|~yo%j!yYrMYabELXC# z*OjaFm&@*&>nd`|*!paHP9^m_%^ueRZ5FwzT#H@HT(#7%Feb&Lu6oxRSJ1WIwTYm^ z)#=*lOaL$94FQdf^Qx=M)$QtW9gMV8@EVl}%J?`hA-ImX`p|Y<{jSp(Yc%=-*MRGS zYtS|98g<=3{av?R z>7H4WhThg=^JIB)J$AWG-sqX@DFV$;=f;4aiucH#GS31DJYG(vrwV$t5_DC(@j9u{ zv)HrDQwtL6q)Jb{%sgv6LC<>N2R)lS9iC3$?(}qdx;;IfgJ9oDo+F+Boj#P$_9sAY1VrlcageOjL8oU(kK{?c^?elT zD_(7@4)o>2WaX;)bJa1hPWHFH4${LM@f^hm`$NYO{rs@wb-*Z~gKRCrU)|F$p)S_|ZtD2Zx3lhq_f@10cge!EQ5Cdj z%}%uE8vwUe7>7DUd)Rt9D@vDFzc!(c@%Fpcw^@nmSZJWhNm1i;fJ}gy_Pf?5Jpt;4#_GQI`LI6Dd~bXDQ`e-A)gy8a>*Hi~yd&a2%hzhY zL>J#Rf9|*bKPY}cdp@zgAGc;c%(!NS*RT1e%EWNH*J3*ZyW0nF0B9pXj9TxQt{z@( zGsv;KLPqNcGVNbPB&U@r%DDzKW-abR8U!-i1YtPp4wE3v_ zPns{XP3mCJpG>zGO!{~GJYB!3I@?_lcB1y9eMQ6=n<8!tI1F$M-~?l-{Scp0;jGGse5pq! z;qi4!KFWJu$%}m3C4eg`|0=+BfLosc#uPakX!54>ivH8hs%(Jiap8F*$`#rYY)Q6M zTZV0hE!&oFE3nOj;Ifs%vmZjKZ6WY#C@i(Du&uPMwgrGLwYAwc+P2uXJvH>ykZlKI zj1|hZoABOG)jpp2c&2SX$XjT0eX8Ep3*oTsm@e$c&wAedNnd)ygGldom`d+(NThc- zJWg+KNTK&OOry{ErqO47|AyYxkV9{2$fNf&JWKCq$fq|m*y+s-v+11-&;8ZwWs$}f36AC<44X^}Y`6Tat@KhZ@!$ah21_1keYJ>8eVU9@O{9qX>d?sT! zV>BY%czk(?zOCnHjBB(m$2|kS?TtRk&FBpt%DX!Z^hxe0`XqNOy}Kic-q(>rZ|ayv z@90RQcX6cCTR3d==8a5x+s4!Mo{bs*-{f*^KkJ?RAJ7w9SxI`{)Nv)GdjTAwr^0ke zr6`Azl0IKc*Gd4cOs@=a@-T#_Yw|Q%C`pr}N&rrxLo27%j5mk;Hs}i@zl;Adxb>g% zKWC}@FZf~hq_NT1$eu^<$%>+X6n%^p)0;z*020)9a(p^eUWD@C2l+kxVP@cufUIOH zlL9#s39J2Ky~#N1H7fTuVtlI%v$MIn9A{a=)YDUQt>>rarX^U{D{<~23_xbMw=Lh@`7|$Qz4={oMA^$_r>|a3}1GK51nW;5V zpxKXEbo5U^zj&B&8@PVDyYsR~p>{uGX&C@BJ_fSyo5ooSSjp5BC0OIEaT+~!!_*D* z96vSQnq|$xXEoO-;wP>@aot*Pt$*Uy6SpuQK8MQc1C*yyxz^mN>#Sw^fMp}|V?KR| zOi$y}c)gBk<}|ZLE7~dgS?jSqX`q)<|NZotY?oR;>NWPnm{qpQQ`bQb8S^Q@Iv02i zQyUSYD+a(z!xoF1PfShsbz7N*DSBGDfIR! zYxEzY|G*xL{xJGOmPGHDdi+oFag_jN0JY@h(&;-U3+b~A1*+$xcdxQF^oawGNxFQw zY$?7gC6tc?Z}e@jXde6#)laphQf>IYQ!`mQBTE;_(v4*4CbD!3S$Y&%dNf#i1B)d| z*ak*zFi;zwqPp>YL#Vk@_miaKYSNlxbs-+ls#MMO`4p`==2Cf|M6(CA_1=1OY@6EO z`>5^=;;mElKBnWHR{L9@@35+Ooyx;s{rYok2CXIOzAZ44Bu0`rlEjlFfh3tp2EM`9 zaJLRrOEk%k`sLt>>xWu zy8?V)GK))PEanJ7NzD0JYg}UN5rmjiafukiQ%cPKxI{=hqNK;A$EC;i#HB!L55(!1 z2XbW+o*6e2xfo*3V+fb1$x~B6QkWcTTuQ7Jb7O5#6V;58uq~D^SiS&luCsteT3)x{ zYE!JS!}8Q zq%DjZCMdx-0Yq-pB8Ql0AG6^vxmu5HmsF{FIsbmr%{|4bV34iM@`WrgBLq~7b(Qm0Vw4L*F3t{h7yEGT+kGrO;mqvjBz0_NLP( zMDa|jv^aAzH?&SU&mukmIEooy=NJ{@hWRdI8)K$iR$}ac&`G$+SY-@Ckbz!c+zAx& zx{TdGcN=>Ex`ESeJP0XAC=^k-oe&PHp+gONiJH6)kQG!VVd_E3foVM;>!7iZBm|B9 zK%F+Cx6$@v59^(WxI}6pem0gFpoNBw09ydIsq_wj-D` z1Rz7c)Jb^cG?gt86CtFCBH(5*Uc|pB;R1vK;;j=i#dI-UxFF_<8^oCao5ii7`X}@& zIU%n@4T)+L`_~I$Vvnmc>tKlff67W$`;{C61O7{VoBU3N`JAv=O^lW0oJ|P zu1OFA02$$ceEu5HE)_}v{3;;55THh(W4k9Iu9EmA_^xQsZ3Dj@+{#vbs|dfDuZB=b zJR_`*dmvQvhlCjZfDj`j2uYL+!+bSoC4W>6+tsiOu_|#h(3opG$lAakfHn4jXovVH zNx-y2Kpo{z3aP|92~-Au@~_?Z^nTSly4?ic5t_&A#$(3k5>WUPRu(+ZaWtCfllv#^ zAs@}e9Wd61CJ+x3uEtbu3)1Y45za|mpTUY)<=ctVjd&b)Kf?-Vd;rTDE&@&^Ia%X% zD04iGI7Tc>oSTH%cnp>`Zo#r*fase*%amGej6>Z>J~QB3^q6ra%E4L~{(*hs2^Z>S zgqI*^9M87fC{@{m&<+Fa81WXu%6>)JJ!~g@miSYMrdJ3<>nYN=*_iW0piuPp_ zJ%no%ITHh@Ct~g<(UnAZ5zay^@If=_=%7sAX7m%ryM|!{Sq}1HrZB=pleJeHmXL)LjWU7nCw!Aq z8Kqi?zD_izHdCo)N^K^7Gv#k4O^PrTW{{uiC0db8rTU1jR`QE>(wXvzG;_%V^dZf9 z!4^3FOB25b&i5vEVtc87dI?_~$Nv%Z5+24LgWl^Qx(a6;&D=hk`+W*c{A$Ikkbg)T z-%Ycad;-!?b}sn`J8`hoAkl+FXHk9{DZmPi`LopC-a@6&OI)YXoG{=#iW5J=(I+1j zhrWybOdJCIA@Q%1H=82fq8>obhggc8HBLutz`8^`X>o5Lap#jIx)m3KmCu3AxEe)W|95xpfM#HM^LE4 zJ~5|ZKNz^JiW@{L>2mc${l8OHA< z{2j#HIikpBe-J$emKDap=W+aPZX3==4y*-^WB&~F3GP3M{s?K_Nj0Li z+~i-yUKMAcXXbF+3%S>kBY^%~3jYg`nZo^w$|_iqvy<>;%!zz%GqxR0grmeikN%ub zz*cknk>*aKm*AhkHX;oP}%Q+%*Xi?=%c`Mb3&egHosDU6-rox zw6Ij@L=P-{4e)NvDeNQ869!n9#Wwoy_&*q)gHijO2m-|t!+gMx6aE$PbCBk(gtr-B z78n%zF$0{MjXy)$P;HPPf120?bf#DW^lTApdB;!+bSLHcI?{$_1L$cgH9#K2D+cIW z!*;_0$g>9X2;b!Y1@IKX2lz$9TENW)1K{JZbK`{13NJzHz9YbHL0B$)8|W_?T0p{U zfrRG1?-b=3$p-!RlxrLl&~52KNQeDzbCwdbqQ|+E*Id;Dvn?&!xyRdz9jqr z_!+`7z%L0u2E12*Im`c%@HND+TSGk`6&?f52?4A~cm^@oAbgiJM49hkKZ}nDUxz%8 z2tR>5RRVf5D|$t7Nc=Y%FR&lOD#Mq7ew{|`2?LJEQW300JO(E>P8=2wQvbjyS0CKoC;R@ib!XWXFV%rV>3X&UyRg`BHjY2k# zqdFGrXe?kpa+Tr-G!{QWO@tmA3plq6zZQOk^LOGj&RJX?_}6Ity-X`b1<~(P>TKdv z)4F_uaxz*4=yrf-l!qaojZ&V?l>bXa?<_bBWk`a6gX-$yKbm+-UbSA{*}VB6d}MIWl`JGf^MHjvCH{GW~(k64(h$VdM8 zh@l_+^Y6tTY_IW;fZsReV5^OIfAN}O2x$eI5{QqU>U~TV))K!9IbsU&-$f30LaZbV zHo#|~zv44!l$rPi!jQ`MkRSUO;{0F4+^>{UhC<9^!2c{wh@*&U&HB)^0{t+xtB-bQ z8MvQ>brjk-5s&ovL6w6znf9EUU~dbpgH;Gu8QQmCFTt9Qd-WvRDcv!pAdRVjX?KBj z0ZyUyZaU%rgLvXa+Rp@Otyk84BXLqlj)PWqBhCfP1AB-}+NGmz1GIys+ud>6<+5&S z#c_3?kMw!MrwNngjGt6XDH4?b8ltr*pQT78Y*q90)7b|(29mIc=tl`BQZ3Vn|1RYp zF@VjC&qfTRZ$ykU4%YDu;($M7kEj?qr|~^DkUA503kr?-Cq|WB0oo7bBw9)3srSTW z;>QzRL-;WE4EFH&Y!i5}@pq7(aGL&}WYP}7u$B6}g6MA%ZX^5~!apMXE6OvE=&MA( zNBBv?U#9)Ag;G0-vx#UrTNz1?aW!!+5~lyg;#>fHneb7{BN9$z+!&pSCO)A4e4Owj zv?pf_R-%bpl=^$J##<`iN*v&i(Y_9L;Pih|uv5ee2Tug0xO3wdiybhpe_?|4!mt3b zc$x4j#C$hmVH#qSg5M+#VUbau*@%sV4Ox_0Nc>|&e}(w(QhuVv_lfg1rG7}9IV8bK zbSKg6l=^ex@WgqVWSWToTjFdWeU_3mq78pb{Aa0@(w4o%zd`)xh*L%qYKVS@Qbpoa z5vP~vuM^%yHBKk~KN9B-<&PpqHBF^6v zeuK(V---j&-mg);O6@a=UrhXM#Q8LF<`X_aWnU)TMws#&{wLv`#GyPQ(Gx>xZxhGS z-Wcg7RFLGwct^ok!nqSM%yiy?^V;wWydg0xKrCJ+yb3YjjaZn5*reb$i9=XqlxH?# zBVn-Z6=c;b$eve_ZLc6}N1R1v3#rsGqQ63=-X&>7i|;FvDgWD)=R@Mmp<1j&cM{!B zsXr$UPn@SovWfV=CC&!Yc`4OGwBc`w|16bK+PjzdH;Dfnamq+S4bjg~sz{tF;`9>z zb;7%-#_7cWN8-?3lOc+55%FIk{s)BD{y+A!sZ*y;ovL%Xs=8^xi3YtF_#F5ah*3~OK_|f;_=H1}pFyeK>m2C!1-}*e z@Y*j^T8PbOhfA!3pEe^nFu>|Ykk0P3jQ2$%7YUQ%thT8&@T&}m$Mxl)MCegS>QlY z)#;8n-)kfDFgqac+N_gA^)@g?qOw9FEhEva<5X~fDpq$&kVFn--Y9JZ{%+9y!Cwh^ z(CS%m7NPVtICY`H1@t`7<50RE90eRt=rn_W6PyXKCl=a3Gk@@FqL$v5<>0>nzacnn zp`jP(8Yor4=?2b5(364LXt4_Ti^2H-c{^YT_yfSd3mgSbG-#ZD(C5InKzLXU1)T(Y z;0+E*eg>s_uXCW^7yMS>j|UD1Vif2_&;`K9;7pNdhw&;DA<=@-F}pz~8Js)d%m-%# zFb#F#TQv{;orrez)_uTl3jQ2$%7YUQ%thT8U=-Z$9LT6T-4XOHoFBFaUc)MV z8t9KWnRJpI6>C2gD+g7s7fc){w{!{QGYZ7&OWlC=UM~1p#WAcsD4x-)o}dFDvm7`} zKU2lY)iazg^+hebD`4`KD-hZqJB3=1$4)YDmam7YpX={R{n5l74V}(B@1{GYvzIQj}NS3a_waLQI?7owUoaG`*lBH=cT~6 zrdm=nT>_cnkPpNDXbD=^z1RtogZOIabO2X#>3mCfcj3k2<$PVZa3%&1HT*a9dIyk8D{JS zj7S1${h@QnV=l&O=}nv?0%!_Cs3? z`c=!gzexHa`0XG$QMVI%{4fKNZh54eDUr-ukv<07o9tP|RT!&g2 z`o0n}yP*Nkao7e#v=uVDAn$=b`av`NMz5o-vgq$YSk)4e*+$gQ7P%MCZY*mNDbIkD zq5mxOU?!OH1czfbsvlrU2XOH8N31oa&h?1sLMW{zV~zJTGL>jWnT$De9i!h1xeL!| z6bIPZAEi?3(FcBZ)DtM7}@cle5$frulz8}gMg zY6l>r$KWP#vQawT7+=|XQxKGU%A4^+Zkg2 zj?%(FqF6wGgiN@MCb>IS1_Pe}cK{~=Bk?5k0q9)ddWkd^sDR@Px;Rh+=7FDuy65DM zLumLQPfmcZum>3|X~h||Gx)ARZ}2S=sfJPuG~@tB0>k7JE~fy+(5`F^FdEkOL@!nV zB~5rmfh%>}z^Q}MAbEO5ufa2}YE!|XXk{`=BhhX(-4gkpj_{b|p9JSJ=v>e_pr?aA z0IN=cb~T5<9(l5=fb#&7gVEnQ;6&(lg8mtG!{l>ns)6%Lo{A`0@Ixd#b3@v%PJrbB zypq7)@?2MW$9KwXkKXNp{*#d33(h22syOrVXiGi^A?%`T9km{yEpLnk!TI4G@blr@ zd{}6O%t+7!*<5-0$}~AT(0m8JT84I?=+E&nbB*x}qV97upGah~Zfzr&}LSA}os zbev?~!YG(fIz*Pz`sk^P96?h@T?6-!t%LJS_K1e)d|8)nz&YwZ=$Q|G1T>e_Yq76T z*8_G+&D|gq%ccnQ!c53jn*pVU5|S1+%Tg2gIg)Ss3Ai8F5htrpAYU1DF3=4gRw3gi zIh3$zGFPMY6a0K#73(o&3F?NRNBTMKTD`xhTN3yZ{Q2mGr_mSr4&+VLC9w0hVeK76 zA}oRAZM5Yr{cH-9bH=n)a@0di-1ixmNIlh0^1duj`REd67Nv3Qw4l9E>LF2ageM=t zW24dTCir}~9>199d6)@`L0dq}S@ZzWw_J}4%(OdtuEX43uID<;*xATDm^sQN@b6<> z4(ORmo+T>XfzH5}XuYPK)goSGt@W6_uJU;$oe6tFVNWdV(LfiM+C)@Z5WmP6uq0lO zJDg6B1cqUEd>wV?K|WtTs}gOELTn;-c&5C^A+kQ9MKi3Th?maNeoH^eF|U^#c2y$J zlb+;`h~j1v&0E2_EotT;QSE^ES}850rJ>V;RzlFOqwKF`f_#rfXm$~`9!V4tC;e7m z#)4@9;&Cf1xdaWJQ8xPOr;*L2KkuEuYkSQgR=Yz8XcdmQjJAuvveG8>i zfawy=twATCbf2zMp0ralyxl<3Du)Jl@b3XbWvRFaN%_hs&A=E9MQM9r25^Z)We4!2 zM3LuRq_vV}A0YD)tptKz1v*vUDcA~N1K>xAd?W1il(c#md=2-MCKAP+LUKw1&x0Q% zPaTCN68q!H)N*yMj53LI5%}wX+kuZIs>o)n6D%1EP70n~67`+1;RZZ*06lW1V*aS* z&{k=ja25vr7IYl!^hdj|c?rR3jM9nFc^vbzJ7jLdhmFv?u8=W9<`(+(5|M=`VrmU` zNuCJ7SKmUWrF?gS-9)XKkQ^!RVia$MDFfvRt%`de^O1ZdNZo^zG)MSf_Buw+dC}|s zIH8sH)WztGWbSgMDtpZiF%x9uNkS{>4Q4M(d5XN3sVkTRNo9>_Y`FXI3pRnjre*Z`Lw0_TAumnI^}b1ep1S?`tbif zJagY9-%;U5VE^|RyYA@kAoOEtji~Xz9H{`YDM@zn8pBwwe$}C6Da+& zNzMlq&xzCukSPM0Gr&d?l@(&IW1=0UZNfrHPnD>)lqh=9MBo)^tr8?TVujX3o{~vB zIXLtKaFf)=?C4!l`W}Askp2;#k^bq4x+|dPHE;!<@-L(<1m4w4rJc$`^g2k5gHCD5 z8t5M)YteqPcg$C!=ob~7=eo^!J|3*2^rz^BlpiVetF`2PRt+_JB+oIDz{h)Hyg&M~ z7g!(uABS0*1Hb*DdI`QFb*kf0Ivm&+GpZuqtO%2Bu|SMzAm(2(qgiB05>3*U`mCZmNo(|8?WWOO!e?u)F_6dViaheqpCKFrV{HP^vu=U)8` z7Bczz&Ru^~0-9F>@qCtEGi;Ms4gA*F?@z*c)L>`}gJd2`vtYv?##yD5T#uc;0@{Mo zJ5rK71ZN;@aMSw*opoT3Bl!N1zm3-Ipfen_KiVn}dO9R;LDB?09bwN9l-39BqVtWm zBtC-&LLf5)^ag0y2b_<#g5(E zDx4U5CsPU z3tB%(_J(9d%$7^wZ^c>hA-qw;angH8)}o$}p9z~`n;Hk&3-)`UR3EQLphv?en}Dlv z614#M0OPwHGIzkq2j__)BjuMH^C}PW67lAbvI8fa79gxu?!)E>peKPoV9-g3jwQe( z=nuv0%EN4w{8`4ksX8YVwCqtV&aa9?#sXR~&VHqgBi>SYh+1`E^S6ezOArNlXa%tV z&MQ5ZLGOiRXVi5DcE;%7l+V&nN}7=$*j0(Vzbs|Hp+}D-nj1+DUxslx1?+%%TUzoh z6HpiaH>IOiBiKAkpU=R481aFyM?Zhw3mfz(a0Qltz1OY^5hd^ZEA^m z5{BOSK|>GpTEqN&h<5XV=YToD3qWsZn2Wl5fG5x*W~R~x+Ez%uu&=7%^iuCg#H<^v zwidVtPc!%6Y;X^r<9iW;H&6(%BUz>ug!NAD9ZB4yh9$?7h(5_N{Yj6$F_FonUwrS> z1Ts|qHCo5^A@1^xp@IVWZw8V=#GVu(MTvv_C3s>VDWo(~m^c#`;wXO0H{)dqu~8=Z z%@%`J2q#qP4UY(Qm)~|lxoVU#;zWu?MGZ(IV}O~!DZn|vEa39!__#h~eQaDpZ?YBm zYg|HHD%l4-9G8+9Pfh~Q3Blgv3h-8ZVpKf22YfOhCOVG10)9vq#U=v8vn=u#Qc&{6 zIN1mGpE!R}CW1Gygm?ZzyD!l41v-3zI6F4<*nfeVfkj9~;!OfbJrY8~Nd)OeqDeeS zB_r|mg;``F`H^t>S~8gc51UNR`W62&ggj}N--s3~bW24d-?1_kI;{V;vl9H(i(NMC zHQ8-5t_St2lcrybW(#?7E#anLU4!-dt_wt4Lci+@D{G5><=TJtt>T`j2XxUM$uz%|Fb z6W2B7UvXV)-bEB~-7S9Q2wfQ)%Ip!0%t}&m)b*=f7kY4+D9+6);>Bai$e7B}5LmNJHa${PMDXZWR(?5V}w z@Jo5aFCKQQUR^hJ@(wcPEPK!+6#L`Zkp`9_PnOYvG z_JViTpk`|41l1IB=M8G6b^(->yJ%1|@x7Lc#D@fm@dziKNH3AqQ^_}Ef_BN)(q&sq zS8Od^wY7B3*3xxbOE+vS-Td5=_CQe0ElmXPp+U{m9)W5qc#jS0zq9C;&7#{ji|*Jg z`rT&HA2y5b+AO+fv*^Cfq9-Nq!@uxXImA%@km2Uo)2JC?>|=Ma+2* zQdJT6XJwISOWdE8EO9*`Bo-+L1!gHXZS`&${WdGplyAitWh+aC{BmU#u~TxC98y?W ztE?kV$_8a4aaJ}f+k}3K)u;=RYq&;OYdINTMf_eAtW;JCog0NbRdy;n#rjN)wlWKU zp)J38C&yl7daPyb6FHq|xYP^EG4nM8RZY7l%ikMi+6Q;|CZO3`g< zNA2YPiE8MFkf8T@87gPFiI?N$Wj#}8@-uxv-Dy?oO?{{ztwqym2F;`s=p;IgeoJT3 zIdlQdqD$yfx`M8vIdm;uPdC$T^jEr<=F-FTI6Y0z(M$9iy+!{Jx*pLd^cj7@sxog@ zpEYESSrgWjHD@hYOV);UU|+GWtS5_NeOP~%$Wqu4HiC_2X>2ST$HudXY%-h5rnB$Z zOg5X%W%JqJ*dn%=eb0VkKeN?r9oxjVux)Gy+s*c|gX}0f$xgF8cAi~fH`pC^k3C|~ z*emvqedL6z+`={P#GSbdcjcvcSzdux;#IgO_u|!g4erbRc}*U~>+*)YF>k`V^WHp$ zr`WHz-)O&CQ#7vGX^xt+R$MEkmDN17%34*;TeE5bTA)@>Yovu}p<1}sR*TR&Yu&V7 zTC~!-CeQ4uB0nTF}j+rCdKI*x`vdX>*zY-N;lC>q$K@?{z6L8opdKDP500} z#EtH!`$-vkh#n$k=`nhYl%uEUDdJAg(zB#Iy+|(-4|=0$k?O1gYe0NhBi4x2V8JYy zSXl@QA-=2`YexK7C<`V2ER2Pb0M?qdCN)`m)}GX2omeMQn{{DbNFeLMdXOL%$s$P| z7RzEuUDi+RT>KtCM1+iW79|r_AUFCgs~ZH25HG=u~{UX&0%v$EBQqe(wZ$` z3rHKbkS!!_Sr*G8?N~O;Chg@HO-Ki}oGm92Y!zEYIm-`pX17TUyUXs9SoV-TBz@RZ_LTHxFWE~H$KJBHq#yf}{Ym<>0#-ocIpvfL;AUnwk)?e!{_J8s# zPARlJ^`teZKYd9j)9>h9`Zv0m{y>+}4Rj0LPIrlD+fR?s6Z8x{PcPH!^ftXK0NvftT#_Lx0qui1O{iJLg%g}4K~%ffjvTAr8Y{dp}GA*dcalE<=}_M5~F z!mQbAg|(tu39YnNPOG3**L<~_T98&>YpgZZT4=4bc3Ma6YpuK1TZ_>Kh`uHxvk_!A z6LOk@oJNt;ROBv(ti_S9%*a<3b@{$Ack|Xj`VdNzzrJdtgxA=`K%+jt|}R7W20K^~}qJYYp0@I@Z*Lmu!)9tc1l zsEIsK8+jlQc_0XRpbqjtUF3m!$OH9}2O1y`G(;X~ggnp~c_0{hpb7Fo2=YKv{KLXihrAP7%6RlgJO&^hYat$MBOU`0k3opXI*7-* zh{t+}$NGrJ28hRoh{r~V$Hs`qV8mk+#A68Ju_@xQ8RD@y;xQEQ*aGnwhInj=cnn88 zwn99%Mm)AbJhnwVwnHSgM}^?2Z`h zff($G80>`@?2Qi2n`dVVHh-wfQC`fAogJV8z}*5w2c~L zqta|tx{VrZqcUvNxX+aQ*C!3?B`C3L7kB*|gs@L6pD5z~EOw*f9xXO2;y!IhEaLte zi!U37;cTcf3BXR@$^NK_Us01j_)LlUAkPnuiC-_pUhOL69O-Bg?V?P-(g)ZN$~^?Z zZonjJUY9#SJbkcWUwv2XCRfSR4JBLOCz7-Fh3y^x%QFdSlYHvrLA-^38yP1MN3df) zYCB1|Bfo73>USIU$7hNUk<$O|4qSicX2R1p4bPQ?vy^AFwAx4QtHv2moFMC{K6aprG3Bt>U7R6H^jnQ{9^+CMWs$mbJR+!o~h`Nn&#E)VRb1 z-}2USvYZxm{j0ovVq&U$<3Xu?6O-dohgr*(aIn_42KtHr`TJV^x|DG6^%Fm93a0p9 z7wa(iN+{t)m5v>Ji&~4wAC{taU-eGu8<)^0RW##db(BnVQFHs4=mCid(Vq>o`)7t# zu$I>iEA?f0bc}n4xIPI&t9#ofjjib>537T108J)FXu7E|5qvu(-DDzLhK)MceM8fr zP5G^Vc>Bm#^OfIPU1$+sqhf=YUB@eIu@Buc zdeGITJ6BA1Y`edb*Ng1C4i(CsYFzO{BWR>rM%B z3c9+}^Hpr!>L$N}{3@YKH^rOAFZ^@c`l!+A@4IEEWsIA)_Qlp&Kb#0$+IC!tD&t#U zwZ0+sU**29pZ06!IKAzJ1h%f`o)HyywvVkpzx7eCZ~PN7n)6dxC&Mz8 zgiP||?(x?I|}!IAHi|L^BpdSSAMOs-dNLXlGq-b6jR-pE>5+E~jt?eDUDrInw6FDob|F4LghV~F+RNg?sw%=L^NY}-az>0;ne$Cp^j(Ledl&EQl=v(v-?p%t35|OeY`Yf)%0sY)9TObwPd^_^Jld}(d=s9O*aQ~_Uw**eVVRkswz3;J|V;c95UA}$G_Gtl!Ur?u!!(X4i z5qy0_{>_`K^50xP>#!l|+>G08HU};lSv{}*DrQ>lQ(#8`TnOPD=)ck-eoO0&fUeK+K%?G8sF$)y*;C9-1t37w_Yw= zd3;Q8@?b}iV}^?y6KUj_-lh``kP8cc8AnXynSWzc%Y+joa!hSMzW}RWkW4tfR)5=1 zYuecVC$>3Y;)toMwrbP1{bzCdPsIPn+_Td<@z18o%RBVXn%IU^*!^o>nfmL#ZWQ=3 z<-7FCf6Q?v9WJ`1JJvg1X8X>!!BgkveGDvh=a=`lAD!tg#&WZn1ek^9>o_E%?*Z(;8 z{1wH4Ls`nf(tdyIIU}>K`Y$HIkAOqX$K$rqpQCK}sjkq13&+^Dw*Df!XZP3{3C@i$zNzn{kwod_Nhw8t}N>CJika{@Zbwg zR&LD={!lR{AZO6pf!1krHjn*~d?)`{?Yni7=CyNo+%|CIp;u?`OuAP!Ddp_rn|nt* zdEP0Y%b4_sPUn;91LxL{t~yK^5zbg4jHq5N>+!?Yi^ z@7&WbqONbxJKhOLp0}#z_M+_I5t(ap$DdnQ?eVIkj=S$H9)0h`y^Q3Dbk~Gx2eW2x zELC<-;>^pDOZT+wzxvSQ`ZI2?ud#CYkf1*O$cXes)ahE(>=u=Sug&(@Gv5BB%MU%T zr1&M*DqC>SbAQG5ZDKnjdZCfvUw3o;GB$s|({?dSop<~jDH5$42Xlh_s z8)6i!b)4sUUH_SBd}Px%^h$c-V0T*O!`hH$zvzNF``Z$H(5&9zY^%ic zr!YFTJ@bx*0ft>}^@m<azdsd6Q~sTKIpo^5oXt;*EHF!@!^-1LXm z+>rrmI3%j`iRbf2&CLr(Lzd5f?%AC@^Ip&_ahKMkkP~hTLGi^4hjS23p3Fi+6Q}@* zaS$nqrJyG3<5x15P<6YqOq(Ci5-~NAM^w1XZ$31aw3G2tu~xn8x3@E!o46T{pAWYjV1mI!7JtgATU{SgQ`>c{xSj3MhqNa309xyUMSArI?$vRkb;kTLsE~W&^y! zp#IZY%@@>{B{zJ=d@spwc~U%i8#C8wpY?IMZ#Q4J#7ryw-CPB>d)_t2@Qm3~vBj6_ z@dKOoCC;EAqgfHW$MTD7JA#bkpO%f&nZu^b?qenv+sxXEkKc5NRv2lFuySv+Bw8HIUdnhk3n5@k#o3-fBF zeQJ02pk#-vcqQC)+X^%@szFaIF_>9X$r(6pbqwSH^Qm@rc+~9U9$Ck4q=JaAO{2Y1 zLqXM%hBMA}uZ!aYFT9^Rcj->tgDt@tivl_EO}>tD4l?^~3Z00<#k}O9VzawRwW#d- za-#JgjRKiR25B!gTJ6iYd~dV$!Axlg8`589>l2sj=eRG|h;x`*SMgkc@eAC$S|g6% z-o{-Hx9ryN9||nEB5_?ZY&gm=AkZkGf6rJ-zrYxYi2aC5wYD+;m}>oTV9es7RUGG3 zFlS0E(c)8cVL(RizJYpZt)shRRg(8HKXRhdkac?Q zN7+f6n)tAb$HGU1dPQup#S{Ko9i((vuAG8`zZBW!T-Bjb{P4JwTsQZjfEmWfY?-%u z?Ysxsy?sIPz1Qbg+M{TMREcA>OT(px(7v4K=M}mw7`P3(HN&Kp#AbHX)k38RH2iUx z3u2$N=kwNw@7iYt!JdiDBWfkouPY&Kxh00@Gs3=nPCKBIs`~7xj7_AIezFv4k&KP+ z?8Q5bme2YvCya;o^b35&j{|!?4>4-1rNSQUmpF_}=H60zd!(=A`Q(>{<1=;yufBPH z6MZMr=>4a6GeY_9KE11ciz8_7qeh*p5qejPGgQbcnL;nl8%=eFz0!W#k=x9urkrox z|0P3vk|ceiuHo_XFRAWtTXOEvzZ9ldAji{Omh6X)N*nk;6Dfo`pgR(e-nisykS;A# zJzbk0Iqo6Ux3@dwaf`B$%+Ta8oIozX7Wk2NmJLqm@AFa3b(E=l1})s1spOv1l2&Y! zDZ)fdEeeihk1XGX&8T*EJ&q;95PUsoVpaZdu8vpan{;M{`WyJ-T|N)*aguRc-bqGS zwYDGHC0(@jMzq<19+;RZYuace!gfW(0GfbI8y0i@f%o%RT$% z2Nw>^V2=54TuN`>ALY{v=yv{8gTp*3KGv3Gl9JVS%aLQ~DT=w&gbsSEL9%^tC=XQu z3HAwWh|6?mE3^?8QY$~(cC~VET;~n}O4MsFxvKY;C*5|-Q8DOE1jBRYk2bVb;kIjh zv@Vv2JviDoC>BxvWVt`0P6Myyle?Aww2?DsJg3P15AYA5>mUMT0FnUVfr^6`HjK~# zLgfLwc+L>2Hf724t6xqPj7u4~VZ|jO(`2PaJmukf4tGi=aidX3dE;gccmB>%&%B=c z$*&oHaF;tGg>8oln~-<%OhHzV!SKqe*^$OZ=dxKrlCrzhiU-3QxO4#0_pYMt1hDnU zGJ3(|{I}Mkg%06eq?y-C)V@O11-<(T6xF6M_?4RFup!fomJ7>O&lk=WDxGjlEgR)V?px z7^mQb-5XqVYHu)1B&MA+GTiyHq98*%;6sS0dQ0t2%>WJ@muP_QVI=ND*lx?C7rj~n zgfAou@Ldpy;4=!p7h$A?6|Ll#?CtDr(D~9ypN-kIcq^3Sm@%*RDardM)e_9Fj1QHU z4Qn|)SuUCV=3gfGc%gTtdyJ8^;_3f7c86;G`6sy(5w6ci8+_E5WZcK4kWU)u=}

^yU}j^Z4oiwrxy82D7!5$ zO7~qP(`seN`B6iBP(hj7zTPy2Zyn?3u0%zjx*UBnwx#6rCX?7R4!6$>$-F3h7Do0~ zne~dB?&8nPXDhQqEpf3EsRVllortBZ4QV(lE_OH)00OLyR7T zt^7>0nN(Om3*Mv+r+w#DMDi3|XaNFr0^MJ5ZMivY+3c@YD+ zCP>p3L$oBHcBK<_UA6S^uGV-IK|}#9=Pc_?v850hSb#IdhH5YCEH8p3&~QXqs=e&0 z8Ylu-GhtZEi$JBpQb-V31kEl-Cs@epD5?D6L?>9ti&!!kwz5E=larIQ6I7Z;Cjr4I z6bcA}03i@5I>AE9-i6A*I!jUQ5C0iKiD-|flWiGf8Wpe_5sRZaFyuu<0Dl%kvHcO3 zYA?N3m^7Y70Xk!CfnaG6a5WHc)$^xFe^mFA=HJEEA(Jc__WwBpea&Vqku&8F4_Qqt z-I_>1>(L#EazGaK-&6$nKak%^eo21A^-J;_u3wTrwj~`U6|@}i#~zFR*B(uz668gk zL=@0}e-g<2bP{L*pgy9TR8c+lyS;D92?GUa65VR z@}6twY#tI+TGcPPxJ(WX&cBlH>m*Iu&v&@>J(Fe=1u94bg+ez1{Eqf-b1SGn9RFaX z-GgnD+zU2)w(!(c&J}L|vw)!7LP_~E1DltX)4ex{n$q~)wlt3PuLQ#+uL0TE*mAfiE#-oh|dDPU;&5T2_F(EGrb{{G|55DshTxfIQ+hV^*=F(1W zp54;-Akx5d&D4XDlKn|DA1?NKTD?=?TnyenY$U4w?dVBA_y(O#dY6~Fx1Hq?If>hO z%Q3ERamahuwkr?diWIutgaXIcdEK|Tsl&G;7f^L@nY(p+rn7nH%VWh(ZrT{ISuc)F z@5+(-O_eV;wzIkQZ|=UbYdhP00DJ1j;{lr+*@Oo!&zf^oZM$KPY~LnvitFJvzqlDl9YN30H~Y}faZy=A338%Mog zc{cl@wk5=fpK>z?;}(Co!RLb@16{z`bD4@UQ`~j`Xni`EkCQ9@cIHcG>;q7BxI?(e z>kS%`Lvj|}&^r-jxJ>p7nAA-OHfA|{#+P>XG4L2_Oml4WoYbA0x@Cs_a<&aJ2c>xp zysS&3D|*T`-wjRaI8%y*&}?ri(_iwwADuE;*s!$OBL<*ZFDApSxTE^I)OETd$J;$` zH@p=KJlzcF+;i>b#dj7s)RfE=yPfaC9Z?JfkROCQt=K5txVzD%(WLPVBq9~#D54s? zWx!-WWmij$p*C?-LQ}%+gvf++b?s_%B7@LPY=^i&you5^zPjAEj#Pw{ zKCnq_)|EbY*bW7wt4XFeV%y@ELO-+?ijTiGtXj+i`;S=f@OIaArNFpuV@hU>Y z4VWx4eEd~l*ReJcC*29ZqM)K+In|PrCHfwxmy0)=yfitajP+OaF2yL{eQ*UAfV=bV zRK=NFP<1^qH?dHiD~&r6-D)zjuFx~4We3Y@+-vM5QY2y|_$Bs9@akLWkyEr%s#6kE zNczbw;#SP&JgY>j;pUh|;z-?RJy%q*H~R6D)QLoCy>$IagV?tEmhDYdEtW0L57p`k zmPaf@tsJfEtR9iSSf{mFwH74%wOLpmB~P|Zw0OkFKi+#k+awu7Qa3PdGj>X&W>0LA z&x(mRO)`z{rMUZ|F$&5nmzTLVwZEtVRL9krzu-nZMGOcW1PQ_v4|E7j`F`|O@l~s= ztL&((t;8RJ?NtcA7`!uhILRofA<0d{O;aJlFe33@OJol+3_K5h1D}Mfw3NQMN<2rj zbKBwK^D0R5jBHp@2JGp;g@aFv@XDFfxt-J&%4|btKuhBw8~K%e zC37r|dAl@ok|}*1hFv?xwA#PnXg0C9K7ztCJ*Q(N^0!92ZCu-uFll8UAYZfch~A-o zvMDDZLZgDhC&;py(E?FNqGzLnqy13|PGI}Dg-`Q~?4H^8^t<+Q+08H{ZAYEL>~`6w zGdu_GzYYA*Fa{na59Zg+6!CU4+pPWDZsy)XG7B5rT3=O?u4(%NtC8iojq{nYSA_f0 znZk4PQ+=Z==J|8<-swAR-2Fls$bM`b)!d?g{#1C}i@21yqc4YEj$U)ah#$&4qC*jZ%aiRKx#aOirelAiR!YBWGKPakYEkSjffTr6Xd1!P7O0R!UaZ z0@6~_<2>44VWp#L2AXEk=x}U!U@Z6^IV>M&A3DjTRb9a{Fu!JU)g7=xDjs-`bfh(Q z-_0fCvWJJ_jkg=?gqngu2OYgYS`Ld_s zxqWrrt>{?x_APDfwd_oG1)j`XPuz*i`d5RAUGep*XS%ZbN* zg66_`F7+ekXZ+MPv9*hsZj6(aymi+o<73TE{7#avhmNpm-y;0|Xyw)luTn36RcBQd z|EX4@FqFcx&5{{*K5yNMJX;(BVtY{P-w_>Alsv%xluO z+}F1k+ZRy~={jFgJmxktk5o9RAnq(an=uwR9bX_k>$&>RJ9+33qIF(oVCtJ)>q2&H zmd966;QaNa1REVnmR$|*JbE7_mlw7i*(@tnWi1a!|1%-#!J z0PgpYa((n^x=NC1&|`na-rqF2IL1@=)A_gK4NksJ#gm77wd;m2z3>oUdAO1}8a$c2 zjf2d=@qEMl^_j)Lf8a#@^uYOFsRt+o0{MSVJ=p$4Y5awHuq9Xk!P3w*M!=R}A$yii zBRJrRbhIWJPp8?_EEuIv00uNV0RRDkp>nJC|FZcTYG92@`EO(e0sn)guyvr@tg$Nu zJdkKZq!6hLdmvaEyjBnaFN>!Wu?!j=y%txOX!m_!Lq=QSUFhHQ+mQb#WbHEWXQzKo zru-=IFKGQt)HqXYWij8Y)1%RB{vBZJ&Hm*_RKhZd=v7#yKr&KbupSs83q#0)ky0R- zEC{ql#eC23-Ez%SlSUw0xcqNCf3p0?IACoxHUBdb6v}rpY1L9ziH3Jry@P5>Xa@%} zK^7zfhbk$96{VC8tSy$#K!exT$d8Y!i)!@) zo<_Bx(J8C*L7)0z^0W4D>ing5^nWP7ll+qWhU=H)H(b9Yzv22N`3=`E$#1xRN&au( z`hM*HQ)b*eQeQpoeY#EFC4^D zGX#lT{RU%Ie&7c|qM)$#Ius1NJ_iJafUd_BfQ0r@ff12|0tJI$EP26T z)_H)zGAwz)5SCtpf}xQ0m_or&6srz~V3`{j&T=oIU<8QO4uNFVAz`dKnZKEbj0}q% z1iZdqz(_b8vd#{SM1qj()+!i@gvqR1TM#4&hFI4x5F`WyUWWk$2|<8ad?850x_*Hm zp`i8nL6A`JdS56Q#?ot0FdT(kw=SVz83g>Nc`)c$vJH{WwfgfAU9u|?fB*q?Xfy@@ z3;+RifNE3=8elco>R8EG4y>r83}0KNwm;k=hJXkv8`^E`{ QfKhNb7rU64vbM_q0N1E{^Z)<= literal 34300 zcmcG#1yogC_wY+8DcyMll;(8M-3`)*jzf0|(y53b0wPE%jnW~~D2Q|kh|+?9bSP4H zz$5s)_x*q4es_H1JA3?gt+nP}vDchyuer`GO<8#Xuppf1R@aaCTp~CC1aP%Cvhe@_<((|NY-DY$T&-=y#fdz>nPse zW_<&vsOFm*{yg-Vfy-yu5!0pSgY5S4Afm(rc~n#&`MB`GkAnfnOME`=wmx}Z zELpy+&QW*&_S-N1FjVrjoX!JTFR?Ng{dB6VdE_nIS=D}-@5~8Yyz$ii$iea8#nC`Z z>oS|^#;KQMi=B`5CSA?jlVzU?U@S%WK~`)=ib&1FqwXIrW-cw81Fs{$06VJpB;L~c z13UB1-UOsZ$?b107K}x!w76?{(5jrZip34@ZT^l{(g^ew=sd}eSY>MgrG%u8+T`m2`$^uz z12bcu^(EiB;Hl}L?WT-=;(>x{n%yiu)_r2iI>JSli2T1MYcDz_DPQ<7> z(_LBpK=Zmy@$x12ourC*z~&c;2Scy-a~tI-f6$j7UNV^TcvpM4lp%b%yb}Asago~j*GG{>*&-{*y5h41x16h37!1*M* z-ZFyhd4JSUgMd1VH-C-y8L8e4VIH$;dAC!pn%c(;t@8(Lhy?eBf^6m%)uRnVx2lRl zWO4h7%aFt9nEDgvXWo=%TWr~M-gG~FfnSv$)^emd_&4k%%E)}cX=M}Nqf#mhf{W=D8o3SqksLw zY?mIvVaMPG0?ALOQ&E0$&L46R5}x9;o%4{TRqZIA4qyIt4G-rY zTUAOP@gsbLG7n)jrJg9g(HDKzzp==yy-NqkBj&^%K?z%Th!n)Z)z zF%Rl`1&Ss%3JNE>y17grXuj9Rrc}=6bhtTOP;x4wAVw!_HXmPY2cbc;T=Z@5Ewl*1KL-X>7UgLMo+wRiyEnIG4~*uTOK+)hU`!P5RTB+|k;<@o z9Ho)I|FOB=((B4`Xk}pTAN2;D$p}!sm7nm|8o~|LlA~=>cJHUfdG7fQ{{}FGSTRg; z3ou!UP6tSHG)c3J0w%la@Ah~G>$Kw*{h)bt#*g=Q_|tn2ux64!GEvpi$s}QHDw2Xg zj{M_|WU+6uNjN!avQtYR}a4&JG(q4o+zd6VbQh`0wi(VqeQY z5iN|saqKy=4&3;5c|P*qdFFwrR}byD$1bHP1&gJF&t8T)m{f zQ>Jtpz3O?MF!FsvM&e7FDx?1UC6b6_9}}BJf>+ve_sw|Hi`0B64!){?P@aDEhwne!uBK@{P0w@!bw2 zs@6*a?w`^Ux?$EwF#ZLA@k{$esQyb0*bK@|k}$v@Ie9`;_{RS^&%NrLeX%gQkvrqt z?_P8(JR2-4%4Ms}&k4$mKfXOCX^1_7DI0;eCARPBRfF3M&b*woD86f%J=LlhE>3}N;n9rq zQ$h3nbfpK8bxzXp#C^hYXWY9TMsw#`iSkPQ=2O%|KwgbY8%34UHv@l^YKBwuGv%|0 z5+T3dd()^IS%<>}C(zBC-Tl$VMVY}A**t5QqzUmdoM$f`I|-!bAL&UIw#U`5 z#O6V)yY{MK-`P4Cw;jKR{_Go8C`m5Gd1mbyft?SWH{JDC=c%;=hTf>}YCL1vY&MSq zrs4(w%|AZvebtR>h>oWExmq&)(`h^IzNE0Sg#&R{o>TQi@wy-PS@ZL=*LnwMIZt8`;i|wSdUrDkC)p=}qR)n^Um)mBG#&-LN>is19^4Mt2 zQVSYT{w*-MYgoCRwZof7S{KP=q;=b8i~m3=3I2BJk*x7eAO4Vd1eM0@>6)#iIn$xQ z`W>!r)V}?A-Y`;Pe~16%Q*EurMttPga@VrJd|-@-czS0o}!U-JcvaG5!!uu|TlA6UZF2Q5*{!)Ol}p<{U8sQ*Z~Tgyn* zw_)WZawPru{>d3NlZM$1_0NT)+!W)ocax@TKh+-wgikYQ71rcz%g3#g` z$B?cGmfIa&ZEW=nQfmBy@<;`KystNC-)QGK!VJ`feff6z4Qj-V0#+aY!1DiAwwsYE z@pRH(Y3rw3cXek=_Nv|#u6l%Lk22p6_BsOX+_1e$hL3`6*QY*jjf)EB7YoB1v@vhq zO?p^5<-EvJ?ch)GcjVtwPUKB7r0NtV=f+mE4$n$M&vrVooE*MP??Xm2O%^MiLKlLW zrF|>1VuT_E%Lf}XdpeUzE9+@0*tkn@191aiC>p6#4=?xG+7XWk60te4pla0j4;?w5 zwz`!k%xwnW)`Q9%ua*N1Gboz`J;v+OIO2*2;&Wet4v=xXL?7?l4X{UfR^8N2$aY`) z@?Dn`GLlR@71v`M*ZC!bmw`XJ)rWA!c0^dk;&~g{-RZ9ts>Nd252^#O85X|s(5qVz z>bntk96kh{7ATZ-RPLLw^&K4E^NcGRZB+2$LZKJD$`|e%->B}jX`y<=AEzG{$Yk`* z17f*B7v8(ScJFpHdG*WTDmg_@CWpgevKmU#y(Po%E+O{zuK+eEf9IU-qgN5z0;!x5 zGjpr!YWT8abaderyTES>@_n`6+DdesRKQZD+Vm`0z9pIB^?YJcjumfTQVESs5|K)L zL(k!S?9*;}xdHU4NCjXus)|-VCB~RHP)#6RUrhclOQaX#HSJomVKDZJtSNa}@OBW!&7B25*e}&*k7Ltr?dy zV#xOaY8w~pKc6!;F4pItK<6Jm=SL_6A#`~UEc9RBM-b5e&IeMTfr={)l3I^j=k`bv2}}ib-z-7WwDUc)hm+yeU|r`HQ+d-7xgAD{dO#hRia-= ze8yM&Xe4bPU26ENFOKYK$y3+9-wQJ(6HP%!Dh}vCbfD3sX;juk=sMZcwXE78c8p_x zK>nvG3H?&veu(2dRx`rVId!eZ#1*K&DcnS8OjKavTQ9WS8*E-2K<`gOg}ZrxSZ;QX zbkBWTzjG1>&A;QjO?MB)d(u@$Ce-8TVbK7zn&Ku6h&&Y;VsvELY@WcwNH2e#uzyMZ z6=~Ddr1srf&*#Q#A7E{UPOta6IDg!FcjwkXQ+dLHNNs>h6M1$6{v@?!@WYC|-HoUv z^EXv5aC;R?b{({Dq#0y9YIDy(QUs}x*iapKT7KW?T7Qn{SkyKWWYEaPCgzGgISdwy zJS=x|BTsJ!DV2(feQBIIZJ;#T7gxDw>3p`;?WMW2-Z5(>T0E@wgHyaBQ)wfW(v6)y z%PPy}8+T(yveI8?4(s()yyI#Nzk> z6iovYs<*3+Y_m#rDd0t(scW#YvsvR>5J^{I&45_18iCBUE;1`sPN>O|Ef=#Jr> zz@sZ>*H$9$*H0_8@pXSuhKLh+AgKn;K50=p z2#|&m7dF=RNsl$Ct6=J_N16>(_h@fF9MThasmOkZB_wv26I}WYDl{m%lf5{dnxRM1 zsX^I(fLnKeyz_0X1vp8Ah;(|WogZ(@{^fSiER++W=>ENClHft@5Ms0xovtzx=SuS} zC9N)s(~wfvg3hWo5(_VQcpCDTi?ZEu|dwOWB&bawFh?bSHe>hrw4O11LcE2sW(0< zQosS;czK>1H>gZYyJ`;i*c00y0oi*R(g8J6 zLV#d(Plsv6qBl=~h>71!c_E-C#WEVxU3G9ny_ssa*NilvpUZ2&%)9BeCY26(dH<+9 zH~KV9{|4rhD#f#LR&R{%J!*Sw27&u>lzu6M>Y1uc#YmsR)V#*#2R?_5ST`rfcg%k-xVe=`gL|C8a13_>9P zoxuwOul=tap4Zzy6QY2NS_fQIyC(psVsGsUFurIM?SIOlO=JQ9%D8&FcmW{)l5qaW zeMBHc) z2y_J$iToEZH1rB68hQl+4ZQ+{hF$?jL$83Kp;v&>&`S_V^j{EY=oL^j^a?l{dIby( zy#$OvUjc`s|6pZh#l^*ihC;xXC@CqSp&-x|5G3*nC=B@*I1>E_V`F192nxFbf&~2q z0!RPB!NI}R)z!?*3=IW?ui!r43=Iv@AQ0pdQ&Uqk6b`;bPEHOD0wb@0fFM^uALT82Az~F)=h02D*fwpC1i@fUbZdz<&Wj(O1Bs=u037$Q4i!_%Cn}`VW$ll4uYF zc8QFPjD>{-+VWz9LLh%ZfY5(9e|Nx0Ug1n3eyJv|HtqoJXJ_Bh`l5C|FqgI^*p zE{+C4A(sdX3!|Z6&=n8_;tCK1@)s}^eFX%Jz5;|qUjhLmE>TfYK|{dsD?m`hUqE2= zB@ozo=KcVJp|1dg&{u$<=quoG^dIisyN8B?VOKzq;7jc6?3|sQ-QC@NeSOgoDC80e z2?;a^0=h&*L_|SB0S$qG|AK&_|A08}grF-F6&2AC80-=x5-B7kgoc8VS3p3ZO9BD{ zjEs!X5E$|bC>(MH1P=KN7>53Xva<5syLZu082l1>d3jDwPBa7w{TDDa zHa6zw=4cQUdIb~#`3newz5)zI|G~-02@Qq9{{n-cuYe%YSHNNDDY^b?$R$2LK74$9IyyS&i=UsL zpQWWG8iGLn1&%;p!gH}fK!_`#Q0NsPDCi0(2>us10)2_s`6ehRh=#ynSGZhkP$=XI z2o!#awY4=G0ta8>?d^?*K*3i)LC8x`DAcW6x6n{1=n4n|dIb~&yX4NDJ7^FHafyL} z0U82_UIB$7FVWJ{QdLz&L!hv~Kw#)U*xTEqK_J)_5Gdjj7Z(>a1O~mt%gYN51w$@z zbaX_6Ac(&}k?6m`(V#y-!SE|U5a?f^VDu$W24}Cy;Fs{2T#jG@jv;00;z5 z1eCV-^3=5Pka2Z(b9Ff%aHEL+IL-gtvMDCG*?Tk%#q>WH^1Lo7{a-(J zYe~sGEYj@O5RiO`g#KZbCaxB?ai8QuJVUS)RT)kYdoe}u{jrB^X@G}`Sf^gw5$*wNm;SX?9GfjqXyg5fo zyc}UXH;&d_mZR$&GkE67(ZYDS!XGU6yz|IJEIY`1~WV?nqVg{=ekLBnoSSBQ5X4AM> zLEqo$G7BH6BuIoVIMAas+Xi13F?PHju{=B;julqmkys0RaSeHcSBNVu6uN|#u&+0p zoBcdilkQ>qleI#4eUf~C(mpL<2zA2;#pa@_^L%K}%Ja7#V+^2piL zj!cHnDkFmUw|9cDPOf{YHL!OVSe{%DYk~_kY$o&Dm=EiQ+pUffNg4@9W|_T+>M~oC zQ7UYIct$U3Z8z%8(D6F{?R?bY9ES_@U7e-EbcJ|^lH&oJh0YmDwqVCk5mUDzt4OOk zC@6Q^RCwiJ?tA9~8#%wX-nlD#eCNl7MGQ|C>FkZcntiAd4O08Hz@2{Nf8eWM4e}L%a}W%A zyv+z<8!ZrTo)Vgqwbz_vAo1YF&d+sg?aq#o-aevdz&W7_$IJ&uVc%XbwF2N44hYo5 z^lt(8$6u>`*_3A|{C=GLp|9mGKV6VOKE_g5z{P;T%kT%7@y?syU~tjm&*KJWw`fhT zp&l>Ls1|;#vf}$6);*p=FFvuWI34dMqeVksjTz7DJ9&;eDO6almPUp+hWW^i%&zX( zM@G$Ak;Y{9^z~N(K{@ehyzMf))x8sHGGp6nUfv2mzCr0kPdtO(?#`RIIp1@ZRsa1% z2>7Vi@AwgpAI~?N?EZw=N7^p0RyJ;es~AWQb*Y<lEE>`I)2L+FnI65wv8mE&6 zn4!sk?nHWjX*XtryT!2VN$*5RynOZhMK0zG$ZlxujUm%Pp2C2@2UBD5a`BZ%?GiV% zzpmQ*x^=YE^d(n1(Qs3l6+U1S{W*qw-P~+>w%%RxBW4c1DlYQxpMG~->E-Mgs^`C!?ixAgozt4JMGG>j-DS8&M06Wi zqVn}cV^jL=jgYUzvl*#5)f%0+1DxTNk8AmFr9OS=J3wqBt^EGf|ET5q`HB!I| zt1`M-FTw&yNn~NgJV+W?4=7_LCG#GNK213rYOt(#>LH+Ge%2nh-=@SiQ*(EaYxO`7 zHEXo3^VpBslYqvRV>IJIrqcU~zCiW@Fg2kLCip2m3E2aYV%avX1~(0s`E*NBeX0%O zG$FXGxJF25v&pLy790^A{W2=)_V7BkJL;^2O*TBnAyN_5*0$!qSW=E_vv?mqjQ#0RcOHmiYvPybatnhH7)|Ud0l5x83iAQ`t zN4qWwrNOr8yRhhy*teYdWe0Vvq#VT?k116bE4d#~&D*qU#8gW7j^1o2<7;=~hSsWxP4t(7}Zw*0K>da={O^03MKASciN1YNW7M;TL`LzSC^_ z_a!&=qIm)Y?h}!0E%^C<-3d=gA$iB!;#(P_clK16BzoS1oS@7En#c(Dlzi{b6(dgK zI$`a)xVu(dV9a={S9|%?Sp5#3Hto#~vwB|E2e-D&+fGLb3t}45xlf5@2CkWPGH7n) zvm6wV!uqv!;)BQ^Jv%6l^6z>c^6bMNd`q-qm+6NFi;Fz{@ghd%q5S-OqJj8O$D8&h zmfoV*Cq)a>zkWSKFCa)CIucAJ?Fh*}7Gujxj#K{PHX?Yf{S~a6Q9498WZb`@^Hy+< zs%SH2Zewnr=;a%i#$T~an|-IRD0@MnCq)ZEipPmzsf4>vEIN{;*E?k+ml`_7>7Vq@ zI2wRDJ|7f361i2gGQ=iM69|+X>#s|pIq~qk`+1^SX#VQt{=(oQ~xZ-nsH26+%xfC{^(aNjcUGN~n)`!z0PFNvqCa z72}!rxtjnzP5x5h^s1!Ts}Glo(ms@tEu4A%%*Xirekv7uQpBNN!GX3et!X5K`m7~p zF9m&$7ry!OcYeM+(ze%N_zu)cM@d)Bs(Uz3Wdm*GS-monh6|6AlGp7EeG%O>bl9pD zT|xe8x&1*eZIVLrxD6ty%eK>&)$z5Xoj9$rai(u60{Y=YtONZIT`nT?==VS9#tTI> zfREuW#SUoUl9!8i-9xOpa4`qlK2*cYijNB)(`MYO@w58;q?7>8ECntBEAc6Ty2&-N z!bRKi=0a3$TyEBrvBK234G%OV=+ejN**Fp;9>iwd@pf${67=(=wk)xbcGIr?SYh!GGm0%3u)jiT*7EcdRj0tPe@Bk4QJXlQ^ETlRjSOzN z?15(s{w}H~$Zmas<;|SHbjTd@pFl02+2AsP?>)1MGyQXJ5hZi;x!!u8=cyuaymM>A zTz(w9Gbj*TeQ#(Vt3i%4djA9HlM(g2+)ERVHcWrisFteX>uILF$uKTZ5RxYS{o2-% zwf|PZ&rFND5BCAU^&#B0M>qk}O5MY}WtC(&OhQUAMiW1HM#LsidaUq1+Z^@M16&&* ze};wUD1T8}j-Bb9D#FrFLI>Z3!^BAZaD3yLT?3vTv!RT;%1RGxk~Jl;U%Zv-!7Xv?CCR+0DDYj}rOf~*^rzmzZ+(D_esWxvoaU{}J^x%Y0GdwFFmbXR*p zk~t$6M_6zy3r=KQN5}G1SX|ofM~)UXQM}Z6#Yfw@2EV}==wy@{ta(g)87p7^EYxymV!QV_K zUu)dR5H1;ObhHO(k!}qBG3j4Oz6GHSZtq3ibI}N7s1jy?w22 z$x7v0sLh;MN+J&XJOzW>6nY!Ta+^78FQn8h4S+vLg`hvv|FshmgqyMr!h(XH&Po&4UQ z5FILLEoDjXHqv9Yob3j)4PJcVa5iODh5<)^Y5U#OLPJjz6I5hv0DokEo=Uu4#xwcP z3nLoN`QEu9C+|#d=y?V*<&TYh`U(Z{mI;-O&+a@JfTX|pxM|NF|H|(5t8cER`tBNa zOFK*TpLZHvo>gRJzy>}_CMo?iRKF&GDSVp5Z|{Kq*pi~h zg7SCPjaQRkE_6s6yHmLm&8lL%S6EzpcjbFcl`>+#kms;*HMUEkdS!yb$ZzI#T4FwQ zrbXdJUlie}u=nX}j}k|G$JI2xv^7S^?#CC`5&G6CPseQ$Fy@jEv1xj&#`;egYV!4+ zzhZEEcFVsE;NqHK?1$Q(lvSH%sf*=({wPE-{`=jn1Ik}R2e!|@h0O;B7#oL`=cte* zHbAGPv@w)7RalZoYj{+(F|?H(X;ClxVVvIiH;bS*lrroUS5;+%xh}3IE!1SRF^mTV zq@WjS*zt~}(t4A8DY7+HCQF65Xo_8jZ|TWub=7mM1JCw&l^@=Z(5W>_7*$Wr6C{br z4eu1i+8r(~tMGSVw`mgN6V&c1F2H}yAEvZ=M|09*Rfs9cOzX`(Fq&LazK_-#nn|UF{*KF-?2rD*QLl7T&y;5m2A<`S;)~0RvW?|f z3BS&>x(^;R%qR)~s}@Qs_msvsu&MeOoZ1&63%KSab5~O8J7kq>^hdBY-kK{r6sIk< z9J|fdjz43$9pnBxNkNzVkXKLTGdb$<_bN!sxT}s!|o0~LQ;11`PZ_2Tfk+GmRM^zo6 zvzZMv6$Mc|AInRUI-EPdCh^N@Rr>RFB~Mx6OJOa4LR=rO`taOK3m3rN;F(TAo?}a4 z0Ln_HRbC=zC-vHyDSk^WnJ)iuc4gU8Qw!>GQ`IASc2w62U=|J1T6pzpkZ*%r#@kbG z^Op8{YhhxU`W!1$0~2O)O-@Sy&J6g_9gJTdh){T&@{!OQ&&}fN#-DQk=PRO(VLY#d zf2!pFM-WjeN~$=vsb!k*=t1A8!)8e9{M=NTeHU9oyE6n#5z0j%%zF3Gr!3yB*eK|S z#WPckv_ecj`oL*BF6sXL>-4BC0Cibe7oMmt(lbaa8rS*f zHnYb=J8V7rf@xnV-G}@sh+JP(p>)>PBD*bh()I1Qu{7_;i-JSf3sHa`i#=1`VG-Tv zS@o{RQrf<>pdU4zLO!EXanQ;yA8qH~Wn@tjP}Z~v*JYkW?bdLPVf((CHv9g$mhC|b zrtwy{2ot@#vf!Dmv3yKWFZ8sT6a|Hmrbg39zK?nXOs4+AkiZWEZ&z;TY4#SOLn{xp{a8JQ;rC>AP=s=_We%Qqb-7RsjBgTnY0s))XE*-bSdWGVuxY4%eCG> zAO4Mpa%Y0f;XLZ#K8)o6OPhlQM17oB*84V#n57hXPNx@10bOo28*36xeVSVip1k+< zj5nQfxnr?@Y;K!;H}Ra&VZU&CB3p&I2Ol)K?{3+fM6R+ZyVEmO3k9IR^~aT%0)3o% z)XrXEk=>mZgL#`nBrWOSw^{dm$J>Kp6Cb**B&=ytg?)c=VRs5Ji^1p)_^RjoxWwa? zNM{b$-_M*l_MC2tgF-rrbjN3jP5x!LVnaAXv;v;*ypOwX1 zA=*X&A`fNbaiVgGN|^x`oixizR>9BZA}tLJ{VT*hNqv$T)DKkN5Z)1Sn3QoO|2=wQ z<$QqTI`!YD8nM)FPERP~SL&O_w%{#Pi%!lb7<}#_Kg387J7`Wm$dL_fc!zx!N^$?G zLVE)J!qYE<6!ME{>Lui-b(17h|Tqkgm94gljR{q^bC3LkQ2acrJEp21Nk1WopIKm`@KK4iV#<~b{4-Pu~CM}}LRyq)g!M}I+J=A*gfB&rft?~H|TAO>%AnW~;w3XVI>SVn9 zK}R({zu)$l1P6H^u8BM;zCrzd=1a!!cnb%lk?k?#_s1Cp^TTlsR9Kmo?;dIY(jV5l zy;NW4KyA)kz8Zp)%CUxh4fIYp7t7WadHaP3)c&|?J@Cg>vP*lo)x75E@TS!}Y+MKU~0N&Bp)R)rZyK@I5?+OB4 ztc;Mg@wD==ck^=f09-6N0N!!cb+JGH&lljL5w!nVe{!Due3IM4(@VzA!UF&Xs#;v0 zBLA5u_cR8;p-=z>EChf%GKK51p|Qm^MACbR@ z_E*=k_O`Y*9yTskHlD^rU?2vJ1Y)2tAs_~dgaI)S6ad=6o`SqfEXbd5Q9Jg zF)$Dah=IV*&%ozrsPia@^Rz+;AO?bj5CJhT7|P^d`TN(BjsNvK42K~8{SJ>xxlDn` zuMIY7%W&`DWDqH3heU`tNR&f`q_EfbZU+#?|1d<{lVJZ;CPQ!KJ~Tb61{-Lf(OHn8jK1l zS^>k<{9T`)bV7TI4qoOKyg7L3#ILfhB$wr1D8{3~&*rG{k+|p0!OP(rR{EsRiN8P3#$Cpt zRi6p<#O6A9s81a;3Uw2F?8sp!9r5^B^5|y!2X}me9ENM@ct&kHq}K{cW{Z^Vq;t)g zu+{A3Ii?8gq;qHI@L)NQ)r^SM6k+c26KZWnIJjxw*#jBUzO%DBNm4UTzUv^Im2Q39 z3ee<Wh_H|DT;20lVtd5Qq=}0R{nJ za1;Oq2LYf$5C9A*1b{-305}MI5q=&ADFg?=A<%zAba@SfzyVMY8~}x#D+lO>EF@S6 z0D~d`a4-Y_MS%e@1PTCwgD!x7%CiKtq&*Pov=Q94qa}Wps918pA zIUqOy0fQ1D5b%pM5F`Wuhrupnoy$8Tq+23 zE*1oWpO@p}>|Fnz%Rb*BAm=CNLeHNKInNppbN*x@@V~Y0zZUIZw86RI`fKAZF4ZlZ zZT=@yC@t;kXAA-efRQKw5)K6*P#}}@VF$(&03!lQd0L&jdJ!lP5m3g$?TUh~W>jA)E2odnya_ZQ4asN}^bL(U1gGx<=8yz)*> zRQxz!(|7=Q+S63_X8Fk0clq-xw zF7?Y4^#zA@EuTkwPfwxbLEDxZ#7+UnWUOYJUt3Nyx;|yij8~f#OvSBF{@llH+27j~ zJqj2&nR~nte4IG z=2MQndjw)FA=AQ9K_~C6qB^?BkZ}0=Oxe`F;;u^|@j-H#c*C+|0Nd2~ODnI^M797uGoX_q#BI&Sk2qd~AE$Aa=`|oc()UOtvykF@)f?-K73*zJ-&RV?IFr+L%(CloF7Iv+UsEu8zLiTM?a>M3ia8D#+>Rc#*l%=8?5cgsy7xMM#Wh5(h_S3I zw#Z5RiAGYAQ8i;xo4wcBjKr@xy?WKY08AtFQROX(G&bdf&flX|^XcDxHp2&h%bh?{ zsq)9{^Ah=&9!xs%XlabTjCE5g>FX@oSDDmPlu5BcrC8awmQw?;+SXx6f^PCQ9Z?7V zfv7j2RXcj3iBBT}pofh2jTlH@$VbVJ65fxOPL2=dP6{QF6$}#&_5TpX5aS;zcCX>q z*H8Ce(U1-yf&t{I*$*2DC*FrJ#%SL~;jUSl>RLIdm+rt5@^ts2%)>e2Xs5sUD-%&N}guIzs%B}!!%FNa9Xt(CuSkYJ!)3)wuU^ye&gI}%J zKsj6|yoj|%Wt~!^oz0x=b+sLjM5uPYIR`g2?Z@Xh)RHfvrHOJYIBIeh7@QTW%Qek4 z-+jum6Y~A-G+^B6vp~4Xv=y)=TUs?LG=_9E0t-xix>pxi*B!9MwPp1!XvcKb)ZKXr zg7Ca+f8amoKi4#YfBe>bEcm^@x08)|9`2OEbaw@~Od8ex0udebs_%JWABvawL74O6 zM6Gv;&CyVwYjx%u&kcuVudcnxMTR@;nXL)V69|VikSe|CS3mvo*o}ycauXC+hK}&O zE9QnlzUxPrb2_UT33@p;o4#M=T2Xe&?bs)IPDLi+&sR{gm7*qY0 zDUdMCjscccm*J7+6PR^O`mUKcm^<@+Mx1v+)NaS&c4FO9QR0XjEp7YVn_RZv9zIYP z%}~Cj^UO;y{6>DK$42g3#Uu+F!EVp)6>Bja0aYR8Opk2Z!XeKs2wL|w=Tq(2$Vnrb zl9Xi+mF{ode8apF*EAz1aMG$^&!#6gRh1jcpKYAvI)EFo2?q7lJ7oa z!{SJKFR@zpCWGoVVHQmtIqhe>xUI3?mB{AEXa#=f)2BhqL3Uq4Dm!rQKI<0Z%uDc4Jc(#LAuv zyRW`;q-%EmopN}0vzFAo@>s&%L#d^i`1r}P)k0S%ncx@EH$EP7HANE7e)}BLALr{m zoOx+9rx6}J_p4%8q0=|u^-ojhtRsuV8+)fn3YUJd#<$ZsXSaSSHl??uzZH7xXS1Sw z;&TwR5wy3wmBqB^X}3$acJ`U*jKnqTd)jx{{z2RxSyo;1Hy7UwfNAvCa^x_OVe#hV z%d|1@Sk7nNEa?yQJUWn)N-aUsCw7+9??|;3M?fZ3zABXdrWPeb4*YPD!qGysPbWc|j+9Vsg$XaB6l({Ig#A{^HJCf$0R!Mf0^XD6iA=wb87_8k} zT6J)R`V@SRsRMFzEX!docqRs`Z^(!@B!V%EQgdPWdU`G~p<;}T-nY=ISB7F(pKBHN z%^cznFcRbVGr?tK&VA#-EO`<`Ty30m7^{cMaz)%c3)-4Q53t)>wud`@VQaN)FQu@n zD-#+9YU|motK5mO<7U*mo7{D(3}pA^aHr&g;BaVD@T$tjXp&zquU&s?eBjdWa=PN5 zOOwgv|1~@lyQCZD!p!7my5)N$T;zkoQM*}XPAmW3ee(7sP5ck7A=n~ZR*>x`eo{YT_uog2yoekMYbs?B_Fxrk zV_H@A5l5Pp^gS()oFD6b^-H8eWImo^?PO!-`y38bWi+O^Y_V6kYT-$KA&)ROdNx2( zLhNBYULSVJ%xWDzxdc8gtlR1XYMZ}Ih;mUaL03M3G6avNPdYj&f&W8g?uREsjcGb6 z4nhxflhQ;5#v-3>!Gg(3A3qg)`n8J6rYDI9ntkxD;XUcZBe4m)$?2+X?X`p0-=4EL zm6O-ozgIPMJr|4){^FV^|9J4Zx=AXypTumpali~;-%=&-{rtz{F}PD8xy8@H&a$A- zL}Gwe3jY2?n3|5_7{hZykhq?R>Fl@0LE?c9aSKiaGn3ksO6rU6MGL#XaGG#jEt!04 zvmDy68FHwNY^}1wpUvDvSHl4y4sPN?H;tPbCwXN<34#b(!fz52;{-<5FtZM1A1Nq*k zk1Ibwr9(#s`blm)(UYd70~j9p;USDqHZydp;<5)if!4qy#65;)Rw$E}GFcPitBqrK*T!}znu)y@!Z)M}x^3a{=@~Mn;Vtn$tO6?H*3Q;g@|=JA z8lQSTJ&V01x8Pr09kgWX3P5Fk7H!;DEV<1S5{5TeinOGBv{3zC#){JPg{ded0e;76 z4Dbh8fFD`BP-v_-rC|G++I%TdW_YpG{Jb5!>m)~~H!!Q|<%QAZvg zccq1g2XR2C>BFhav5wIE$r=Txj`X^VzioU!^TYQu79`%0Pg6Q4la1)m-uY(A_Uj|&m3WA@9X_E=&{+(QeaFl@SIKya60nNOEg{XV#La(!ZT+@& z?*F=Q|1a>ayM;Rk^pBN0KQ9=-2jTnY%AF6w_wOtByVL*4%AHH#KUVHY!~+6GLKA;L z6@SAPe?S;W;NlMP_!mZk1S{?U4*owN-cX7`MIA5@kz}KNbyG86psT_(W;go;|)JW_tRBd zSbvh85}maxm9@pqyOAhPrs&7d-<0S!&pMe2LGEEl;>0@Roz&;C&%RKdT}#_{$fWB{ ziWgZFbC%4S-T%BH@QVg(vGw-r)$O_O!Flt+t$vN!8uqhIS3$G>qzS%Mhh{4Ome;Hc zRfixv9{5$Fpz9j>W?$l@pH-LQdj1f=bt{#vUww^sxf@sPru}S68~AA(hkG z1bx}Xc!AijI*`xhjxC?hO6p>>`&giM%-)ott(~CB=NTG@l(jrH_Bc^~VNns6lkoV0 zjN6;yvfIM~G5=pWEq)vBEqtZ=H6yiNflhn%`q8$0AEBkwwO?vq?0K!|t)#ep(VwK6 zL~xfjTw1K8=zrg_xu_?;ne@6i)t`JgX{T#GhjZ(@_tOpR%HiW*>@+_^a7d1Y&?RVb ziFDfgGVx`icxk7^HoBOPda|`V+Htaa=WtR#YR93jtCmi-t%L7-=HYR4F{0U%X>Rqw z$`^v2Bcj9NnpQsBZ{O*^*YLI;bVMJiK8)Hyd@-J+;%oO_#8s_5wV=d054mmhYH|{Z zdR|5C*V%#rGBv2VStnBilg zy0isSf=0kiX+v?~7R*qMVl>!|I6WtF+e}bwq&~M)HF-48jmogaK|FbacAQ|_ zxt?LZ+(DYJ=;2uO4zp`~J?lKf{HOi${V)evzT(s|+6mgRft`$D+R5}`kq_RA?i%Nx z_NnGs=d0?+>NlRvVN!isMrT|2mVPUK7O~l9-K3(LWGuHTda=7W=l}S0=}_C3{<3F+ zoZj$yB%a>;Y~|eNsyn1Av+xF1xcs%~qTsV3lXk|{iI{=V;k!-C;!=IjYl2MM>#HWd zk1Z9uUus$$SMx!B6*rOS#$5iF6(F(O#JDnEpX*4z#oC`M#kM*v6=!3%x0Ce~HCl^5 z&VK6f`B|;(miX81ooGk$aT+z%ehcsklfgIR7xOu*@*@;=@e5(i5V(DcxnkP%X;Iai zif_Zz06$!Q@b$++(o?`2AZEmQn8OHg^o`i}cquPp{$O`^yu`A zedXKPdc6{CI|3NfbJe}$J@GiRgRZI0W5+Dd*m`heg-a#oGhrPcNaU6!5#p7s7`y?PIn$B7C6rW?#R187FwaTaak3&Dr<}eMx3*k z42hMB^*!yYvwGUw8SmSx#ZKB6T=kNM`qh!xbFbL|!V&X9V#`vY**J#-p{#wKsJQvj z2QOK^aqjg*{$$z>;wGcRwVb$VR%V>0@0Y<|Q|Atz%vZ}4OXdN9l&aWQ&Y|2Z=8b9b zdHlLqFeyUmqlE8E);JtNk6Lj)M!o7>2KIxnG;rl}*h4&IwA7+h#UE+mz!L6B(!iL% zQ&6>mzRMe2z&wrD`Z}<*cID|R(5JX13tVd`UOaljbD<`-qXfjvo=i`7=eT; zV$BZwaRe1Sm~1)cvHsGbewgz5youRQ9m^=bqwM)6NDlaxYN*irE%ol!!PY@ks<}lM znccE&?Mw!>rpj?flTdNkF+KaOZE^rnm>B@j!59>LpZ=x6YrLYQC}Me8T{JPHS8FhK zzrw3ds4ATRnu9F~DoF6JOoOnWyJRCL+#G%h9cA6j%^t6#}>N z;2>;Ciga@#d$lY@=6sYfn&A1ivQc>LDzwktZeIP@TkF&h5pz+^k)JzXz>2GWeIcX~ z0o(dU_zWJUonxMG{dBsRxQGnjs8Wrgd;G9ScSxQ_mC7Ptx)Sf!F7Wns#H{8qkPRe|*$&$}=nNw2h^0{7|X z0{cLb=jQ-wiLs^dPF3DjqFlHJ4MJtLiQopCouaWs%Osft5qgCDxv zffqFHv9u;wQ&D9@s;k(ixD+{X#T@B}m1er?5eca26$5-vxnJ9>iUPQ+N}6Bj2u)0w z?`&^5>xt~qRh1FE8(D8oHz!yjIQ|S3hI}Ew{+akvK9l-D=~1F9%R}n>RROQ$Z`Usk zvAE~sZTq+j&ZOzdBVU^YSI=4m3hp}z2on-12vNRacn-{6FI65>?@wF9+y-u9q}+>= z`O*`X1N8Izgvb0O`RXWH{R-ugRx z`5>W4Hq?0RTc~ozp$2NqkZj53nZVV@k9CVQQUFj_pYDk(tk=t$3N|;7mqM zIwXBgtj7Pd*K(ZQM62NlPe7cz@!@*Qd;dspjvvbFu8UWj+0$F^=Ei(2H7%vYWLw5t zH(m!z-YgPqqHe1zRp-@vBJ4E{R|t>ae=hkP+L$^*_ep1teu3T&pH@woSe_{lZ(*F4 zu~EXfvy2NjUYsg-`B_(J5cm_vJ#kGO91_qGG3T}tKdIIiflYw(Y9XofAY?~qP}Rl! zB{BR_f+qBXGS@rQiy_4hE8J*rSvzd|=|NP@6stYkedw2+l`pjrYvo;4-rSvEDMTqD z$^!j`_A{beq9GZwK&E5gst@nxIy>(}-lg__J*U&j#1-oxa*_HP>mN2a@?7;B^t)o+ zPnHR$F$qL#H@ex8-%Dl>y{x#0p0RkQQ`x!~uU(*`gOS-JqPlvAl?gCSFUj|x2X+gn*eYC<#g4vKGAO0JXIo-$4f%gzH5A)IlD_)?+)wrnc@F3v8kG?A6bS&V#5tcl@a+l{lCBuL`qY*rhZ6ffn zB)_Ni@p6lfBSnOl^!sy>(cbS1CUQ^r;v773RRb1KVqiy1Y&z?GBW;d~(Py%x&XNGX zvY~QcyEKL2B2_20eH?PxDb(|7CFrLZk4xA#41jx<1AK0ximQYEI7l`e$6bTo9OkJrK>+74@9Q z=oJ>lGMV0KVQDVg9^TZt4 zmlmkpL{Fk(;n-7t%t{5;o56gw6@a$J`qvAMTSslA1To5rPqaXloJgb(mk(MQ3p?l|qk1+E?PNJPAO71^`7XkqC!NbyX|{F>d^b=U}DuvBt5{ zz8)h_*O$g7ngq;PS`=)taY2HS6C6$giUDQ5Xipp!39$;0lh-HKN~Y=2Np2f|$xS%w z&n`-zAhbhCv-@EQ_y;5==rmR|54r;b@XwBn+6dHk#<>)pUwJW@iI=!iC2RA>Qep9Nr zM^lKeZT?HYORYuq-4^BHats zmpzaEMz9-&*qNvHArq10PNryangcY>pZ-`Nt?s7(x-fXK7L{R9+4b$)wDsrD(}ZdS zT1mP~g zm4;W}oO)adc9$oAi=pEEo&HUwpiugQzFl6;l7$Dxc=PStOlG8BER`Hh9EQ+QkK85s z+3@`h`vSwV?93cv7O`!yR8L~AwVrGymkywY>S=mJ+IlzxXE*-Ommwa|j3~Cfr~L7Q zZ`oD}JYi*<-IbE9jNOrQjY*9dTor1L+-paH7|Sg0v}hs>x3n5^>>=MxaIxADj;X1( zbRVbfo(D#Fp^31FmF}v)cpPP|nSW`rfg@m+d196>5R&M|U7YXzYvK_%L|Z*KOI*HI zkZK?h7zu<6i6N^k%ZY){6q@*&&a^;irzVLEtMV6zh>dPi_%7X94>Lf4-3&v$EB^D)m z9cGDGnB>Q=OB(AHLg?^*@lLjgI(1l5XfIwcYx;(=YFBWw!wy z|JC~*Jm+n7J_i{}cxw|h#qRT=qfH`?2;6-Yn{zC`E6aoH+ZkVeCz#)1_%7+T)6#vR zb^0^>2CSE2u-y-(3xm?wu{_6zj+}h_vHh_4=ZQT<(>>zl#?(0YI0e%yFsKykBGDc={?w6oE!+11v zfQ~izQzsXMo&%6?Sk%&GMIFs^d+F+)$CF=*=8B^;qdJ&qkR8VSjRIOBf|8oKP(cf?{g@9 z@w7!qe+9Wn&}SJN4Iba%qvFg%u87H+&gvm! z1bwY8Ny>3>KS4_D1OAQ5d z6$L%!>*n^M+o5Opdx{86`e*bBeD77J)GwYDpwhzE8|9jyuebIgv%1|TO7+!G5yUWRYv#MHc5@KMisZ#kySK^HEVL5}ltCokgEUf614G zW;2Zvb9+PFA`uT`iE=tCMcLs#0NwNeONtTv6g;8`G|m>34*|*IjJP~&_RKB~b+dC5 z3+69A3(YoJ)OT1~=#N|%KKTr7IF5-DUp53fUTse8E>!>t>4tr{%pNWmlO|0ogDUW_8r*D$R4>I={idv4U|TXEfD zgS(Ia4Z~gT{4sU7^osuknQ--#(I!`xkjS~4!@gJRiSB%9y`c~_9j};K^U$TcxCx(3 z5tsg`klgtYwWTt!-gr}V&kOT9NW4tW=xNi`xzGOUNyS#(W%$_i7&=JKpXDk{L`lS4 z050&2`#DrQ-dp(6#Mxz2)HV5mUthrbb@!Z{h@4k`$;vnNF$wb`(K@!u#Iki816h6F zb5s|(s=VIMnMK(z#)Qg)D^yAeCxYD1_m%ka9=fdemiCqkOvbl6y}-0uw{9DFTUZPvQ>eWb6A?ZU$j^Oy9iE*|`k zndn!~>zka{T!U5NWMs0Y#FET`gz8>O`U z481gnJIu$6CAFF}!_Xw5Oj{l^pm`5M*IgwV22{SxcAm?@`xrh*qNLyNv@Uh@>q>9kyZUR(I8k*Ia|wNO z>SZFEKl*c4d6XVWl*%7wQd$=*FnAQ6#4|H~PI#VLU*B@#T3uV)jh$esAkM(P$*u_u zEX=Zo#?*d3t@)JY=9?6OR72>C0(wn)b^0WFT~5EAN`du4cS4F5->B?9)sa#M(lNsQdCs_{QMZCf*-2hpNkB8{yUtePT~l zJ!R*m_K7znHc$k_zkDbMuD)q{MPCL!kp+FiAUb$*bRhrv7L@1o2qcTz1uVW7oR41c=sKqtQSVBKX}LQ=x_3VV)#*Vf$p1H z851)Rbs#XujSzA03=!-gRdtffliGH*<9-2-(}dcfx%@DZj)l-}d@_>j!0d0=Omrj2)Gx(U?frn+?0l`Pxl^$jkuP{x+sOGH043);so$7GM3!-`jKb{fJ4Xo7U zEem!yt>Gf*k)KOg{1ATq#`#MSu@cIW*!^EZLqt1^YiSGTj!y?@G4`<%!)><>jcUOL z+zAcI1GutKwyY+(i`e^Q3@(r7T0HECNIg5sPB(+UkM%*|oB}-H@yv$`Dn**xc89gY zLuXJb@y%T0Be|_uP%8$vgHnPd^LO_=#U0HB> zMh2s+IjbY6dDqTs@-p92k7$fX4;0Y_IeC?u*cTky-Q-M;0M%yrZ~<a`dqzEk*T{l9 zb%kZn=I24#g^>vKs-t|k9MA(HVf%&l7HnFraO+=xeLh(keYw(IuwK=T{aSA0cKl|M z+043>CbXmg>>%!bap3AZ>q2q4H;8caZCDY?_nv7UbB#F?-IYg)4ov14VFDmnBu4W>qx*ST$b!DdTcLXB-+$SY$aqZx?%y+`sk^ zMDD7!qbos7orFP)_aHempD7ozHkrW4&G+nwH6|{r>T2TguYPG?BVDV61aURofUk}G z>>MW*!^O2Mq*zX@QtT2G;`e?QF2{hb?7OrR;hALyTf&y&B5555M-5hG$!D)}CkF*v zXGpZYbagVw6h?@1$)2pLeZU|Gj=>zZ*zjn{zbWhx_s6|{SIU$gdhh8Fg5vXg`x$MO zv{3d&JM@>$c0-c!uL1q0GT>6f=9gf-*Og1T)H~WF8j^s8@s)ehR1;+vWQcLve8xc> zs;y^&#~=>h7Do84?XImdZ|6hOfqNHWHIGufm!NGsBk^-%n*y#8zF7QSlG*~VW=J3V zpuA7`q+aoe-AgG4^UawdbL+4Bcyso)0_?-ReOzLqk0fnN83qE_LCncSAr}&(rklq$ zA$+~lvE**q4tpkHq$xZOlYRoUe0(3lA$Y^)M%hg>5WA2Xck6C=BT!i`VAr#I%R z3BPfZ$wu-}ywx}sz1+Ryk$zZGd$7{ImPYDEu|?PB0!i28;QnJ3ycekG0gZ~oCz*{t zDKdt1&B{S_=KvIDH#iZx{_~3*R6{x6uEvqZ1&{Ny&>FE?&yDl}_v(*Ti$TpnyoK&W ziy{}p2B%P=%u@e}A1?hvG%XUIv@iWAez~2M?54(Vyq03~_uI#QFGh3O-6?b>G&j^c zsk}ibtOLE>^qClMdN<2e^Tb#xbCOr=WpVSq_dDSokG7x6{XWMPP4Tl>HE_PJeX@Ns zdPiZ4nHO0W#$$s?+pjrZ53DjDTyKh^%@a2Q>sddu8uB!{~3wYlX z{21?qsIfcb_$3SP21Ud`7iOy=%A$7EV!_^E9p$D`Kx=xe+_1b1_5q3S@OS=u62o0t zxvM~2A}z9CxQkg*_$a+kFz#{mSKfbMo)fXG_Tzw#byWB+$AR?HjFx zPUVedQKkhv;zK`qU5&8zn_!7X4o;=wfO=cU7}emm`@>XJoLybh7^ExhG$>?q8u^|B z+CGo{%>^*t5^6{kOhpSiw%__On46p`)ub;PJ*)LKZMZ3FhX(IFcug4_ywjPg&^j+n zm)x(!mAg2oCs96&!6hD1ngv|GH-bCXU!;tisU{z(kgcP&rprQuEBDGqoSamWTJS!% zhxl?B-zs6l;9OyB>F(aXeic#uQE5;y|^VdKghQ_@(Fl7(7}M75pA& z7P2?yam`kWyT-;Q`k5rC9&Z-R=<;2JHV|z>-9RuiJ7G^wD%CYujz28zF z7#%ERi0lc?xQ`>|uZK%R=^GCQg(7*nLL%Dnyuv#Sai7f*(O}Dme}6Flcq^!e09R*K zhjN$@5kIgnHnuz;X`^mCyfFLTOLl~GQ`f4NFK%-n*z%&oU~pdP%U+wsC#B-2MTZtI zq3qR($-FzR*nM<1vHoV>35K;2l$^<3_5Z z^u9s8k6jqaMfs>B<%q6zO?l(M@TZAP``fAHDf%*{R++G$3%v_c;aiP2L8A#~# zrXCgwJFeM=ge1KvVjy>7l=*HntVg^sPLNDE@*O*w_XYfA&pC*9D^l_(k}#(KK%!U} zKl-u^#}$h&K!Z>gK>W%_Emo!kv7(clcNKrhLC!dgV}%*Zz(o{Ar6+-7nx)vvT`uy9 z&)AW^d&BII_&GpEV?mwM_E(#8xW37Q?FWvvhW!oF2aV^S`qqxN+w{CHH+z1)T~zIl z%T0H|2+QqTWH9A^Goz(bdDEvyBpnNvB>XuY&1U zb6ndKbGZi7;(G*S)8qH0#_>hE>DLH*&HHM(##4J?8otlr^1TX7Rx~Oj?^nU^c6w5{ zq;2aN)QfkuU-ZRD&9>+rtXy4noeg|vYY)jL^x}EX_$VaXNhxo63^xS$1O8LMK`%p% zIdN&>MwvJL>lfK^22UDVNo?N>pXV)1^XZjc97PJ&k=*^V8HFp`TUbTCPAF<~_$&_Z z?Fjoe)=;rO&4kt`Gek)MMr+SVF>occUom!KCty4mYD4vrAnXfR{$Q2Kb-yDtw;QF4 z2$M(&DtsjDKI)qwY#|1t<&A#J6>VeK$XScMC(Y9=Sd_ta#;rdqsA8d{lJXq)1^>7NwuZg?~iq`EOpa}&&8|?guQwBB0>4%5C zu2vuHmr^2*C$HSKzt#O{-)|Ra4Ez|!Vn4gdrM$0CN4diKof;`sK=PECuQHY|HCky1!;?PiHX};qgMf8w!EaJkC!!kP~c1C3=S&2i6ZLs8S=nWyO|nTK>Ps^0mu=BwrA<+fRD z!9(3OAp_ayFPWPoW)-LhPMPCJ>f<+yn2YC9n^LdNvl~W|R|%F5WcR}nz2mU6)s*gr zFM^hH$5#vv%6(nVioEao?r$mkfO(4u<(MVMvZa1R@dLl&b`OWchjz6jzyQ**J(rP$ zepS@Sg)Hpxd&3c+Wpu?DtlZk%o~3fK;J(GI?gwy`6bz$DlFhJErIOpYtKH=S&ivPz zU04rHlCbNjD4cOyKg8*u04PIMi8=MpTl0i)t8r%Du$1UIVC8(n7dIs(Lf%3?tCnJN&&f<`L~AvbOq(&Y=RZ?YqR{%_+X9t3RlmnI5?|g`zx=G) zkf+#w1ifPY3Oh3N38Tdaeb)|e51tA`Es8R}td()%{sg#>@!3d_$}XX<3>$)E+f>P{;2>ZrK~x{nBS2sdi~;Q?3&Zgw{^qnFZI{6DnxFg zW=)g659gNi^RE2|QlgT+J<3noO}{D^?Ov~C=j*{ABVD*R1RH{FB*3TA><2&RM%kq6 zk`J6U^Y{M zkaeX3;z^a7av2<~K_Qh7K^ClM#NTP1nTK$(6aewtyJPf%Pb0Vs0Qiqt%;0vS<$DW z%S~ml_O<;%5pI27n~ky4GTnvB#t-itlf4}~*O=!JwR}9J`^p1X?^U%_7K9x2Z|$do zr1Yn{H}_6>ZGd&50QXtjk6#7_T>?LxdJNbdC%{L!>>g%SYK@ls)DlEfqB_AeKubD! zF&@lzQ-k+I2Jhff%yj8p5=Y~cZi^q@3kNygN)Hy$li7t=GAj;^5{zWal40v+$Mnax zt<_3bKhY_E{1kgE;mt`(c@UQsd6`GP_9=g0NveIDhO5?qanP~aoKem~~f1HL{JkP0Et;dwaQ%3`v|ydVnNpF4{sEGj8!ILl2_IaV`_` z+LURoYvmrh`dT5WzB;#phT;CxzFnJqy+R9}_XWLU;(S~LH?pOu^FiNE7DS!94}D$q zpOVI86nUPu4k?T6T|WLv`*ZT5Om2bN(`A3|1GM023)lJiiWu#+%yP}uz5Ta{Z!Y6J zYjo};=7S7!V$%om{2dvwz|B`t&f-tqY%nYxvM+f!h69>8&vM3k3^DS7r!#S<uJ+unGP3vR3C1m<;2Ucp1l)<&0hUg0V+ z+Gf<|3MhM6YIpvM;LQ0zbJ#cFsY?`zN&VupuxRqo+9|v5i!Di+CEi#!>h33($<1pV z)9@($isf4rO}&nUf4(4oR}tmUoy>PTyZ@?{@~=(Ix+=2nZgie za1IYsM-Gd-lGrW)xQn?6qm+iSEF)6EL(s$C(H`z<3h=PEb8r##5CPtq2s*e3BE=ve z;7-NWRs^W4q6Tn9Ana_dT>)I20-O*I2$UUQ;cRLTcSkte0wA2cKv7&_XPAYchP3RT z9Xi7-M1WSVu8x8rkh{A(r#qAr;cN+l2nYy(z+50ME)Hjy1&52LgR7|rhl2|f@J~By z-M`HoTsZGy;xtFtgRFHy5Kb`YFGqjvi$~U>K{_@yL%6w$0KqUGGhR~|9|v3j4(5PB z;9MNYsvjIM2uuJh0EhE%nf>L+@!tac4^NIT3lM}83IYNgVHSdF&Ip*BIow$UDCuTr z=Vk2(K$am=bGCN4D>M#3UYpq;Eyy?lAb_kh9PV!F3;qT7qPaRj&6$~?5FutJ2(Qrp z;!TE`{~fubo3q{BV*@h>!R_G4AYDKZP6!AHfSC(gAe`+@T}6PV|4K4)@B&4JVdjFa z)~hhwYf9G1!3U|&_OuE02~0A zi@mX#wF8V30dxK1F4U`R^mR(=$ivA&|r#Nq+13E~-11ch2v~og-7E^1mAH%w&)<7?Rut zei!o{ch|iGD=z46rm#jhAg3Z0X+h*X2Id3vgTWB4yD1C^1_N1fK`IE?|3gCy`6T>D z##R3Idv`^5q22j)K{niR7D(9I=D-YF|y$dJ}>l5lgiLj32TutA#NMdyj+oLT=alIGzIx4;DhpkQ3&KieM%fR~2{ z$^)dun|HD=QSt#%~zdXGE=nu>%02TOMkB^`GcfVjhem=f` z=m~&-mqDOVzJK(`!^O?}yDb#@k355Uz{oN%|B)*Qlp9$o@pl=|KV&@oJiq(p0z>|h z0|=Cx|M$E=xWUkW^v4Ym_=k)KBJjHpD1;mN&BMRqb9FYgwu3w43JU{3n$})$02B-W zX&?};fV(ep0YC~476`zfb!HT#0s6vRT(aC?KCpz8G`}=AFAq027dMv-R7P5s3(PCU i!zC;F|JE=>mLhaioctl(nullptr, DSM_BIND_START, pulses); - 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"); - g_dev->ioctl(nullptr, DSM_BIND_STOP, 0); - g_dev->ioctl(nullptr, DSM_BIND_POWER_UP, 0); - close(console); - exit(0); - } - } - } + exit(0); + } void diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c index 133cda8d65..bd431c9ebd 100644 --- a/src/modules/sensors/sensor_params.c +++ b/src/modules/sensors/sensor_params.c @@ -156,6 +156,7 @@ 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 */ +PARAM_DEFINE_INT32(RC_DSM_BIND, 0); /* 0 = Idle, 1 = Start DSM2 bind, 2 = Start DSMX bind */ /* 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 f7b41b1207..42268b9715 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -257,8 +257,6 @@ private: float battery_voltage_scaling; - int rc_rl1_DSM_VCC_control; - } _parameters; /**< local copies of interesting parameters */ struct { @@ -308,8 +306,6 @@ private: param_t battery_voltage_scaling; - param_t rc_rl1_DSM_VCC_control; - } _parameter_handles; /**< handles for interesting parameters */ @@ -544,9 +540,6 @@ 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(); } @@ -738,11 +731,6 @@ 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 35ec651cee89081ceaca63a1641541d8ed0a1467 Mon Sep 17 00:00:00 2001 From: Jean Cyr Date: Sun, 11 Aug 2013 18:07:25 -0400 Subject: [PATCH 099/109] Remove unused IOCTLs --- src/drivers/drv_pwm_output.h | 5 +---- src/drivers/px4io/px4io.cpp | 6 ------ 2 files changed, 1 insertion(+), 10 deletions(-) diff --git a/src/drivers/drv_pwm_output.h b/src/drivers/drv_pwm_output.h index 52a6674031..ec9d4ca098 100644 --- a/src/drivers/drv_pwm_output.h +++ b/src/drivers/drv_pwm_output.h @@ -118,11 +118,8 @@ ORB_DECLARE(output_pwm); /** 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) +#define DSM_BIND_POWER_UP _IOC(_PWM_SERVO_BASE, 8) /** 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 cb61d4aae6..10ab10ef31 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -1472,12 +1472,6 @@ PX4IO::ioctl(file * /*filep*/, int cmd, unsigned long arg) io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_DSM, dsm_bind_reinit_uart); 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; From 0b935550439a17856f5218fdcd6be8b864cfd346 Mon Sep 17 00:00:00 2001 From: Jean Cyr Date: Sun, 11 Aug 2013 21:16:55 -0400 Subject: [PATCH 100/109] Tell mavlink about bind results --- src/drivers/px4io/px4io.cpp | 24 +++++++++++++++++------- 1 file changed, 17 insertions(+), 7 deletions(-) diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 10ab10ef31..960ac06ff4 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -577,9 +577,11 @@ void PX4IO::task_main() { hrt_abstime last_poll_time = 0; + int mavlink_fd = ::open(MAVLINK_LOG_DEVICE, 0); log("starting"); + /* * Subscribe to the appropriate PWM output topic based on whether we are the * primary PWM output or not. @@ -679,16 +681,24 @@ PX4IO::task_main() */ if (fds[3].revents & POLLIN) { parameter_update_s pupdate; - int32_t dsm_bind_param; + int32_t dsm_bind_val; + param_t dsm_bind_param; // See if bind parameter has been set, and reset it to 0 - param_get(param_find("RC_DSM_BIND"), &dsm_bind_param); - if (dsm_bind_param) { - if (((dsm_bind_param == 1) || (dsm_bind_param == 2)) && !_system_armed) { - ioctl(nullptr, DSM_BIND_START, dsm_bind_param == 1 ? 3 : 7); + param_get(dsm_bind_param = param_find("RC_DSM_BIND"), &dsm_bind_val); + if (dsm_bind_val) { + if (!_system_armed) { + if ((dsm_bind_val == 1) || (dsm_bind_val == 2)) { + mavlink_log_info(mavlink_fd, "[IO] binding dsm%c rx", dsm_bind_val == 1 ? '2' : 'x'); + ioctl(nullptr, DSM_BIND_START, dsm_bind_val == 1 ? 3 : 7); + } else { + mavlink_log_info(mavlink_fd, "[IO] invalid bind type, bind request rejected"); + } + } else { + mavlink_log_info(mavlink_fd, "[IO] system armed, bind request rejected"); } - dsm_bind_param = 0; - param_set(param_find("RC_DSM_BIND"), &dsm_bind_param); + dsm_bind_val = 0; + param_set(dsm_bind_param, &dsm_bind_val); } /* copy to reset the notification */ From d7730a3444b1c4277bca24988402839a98a52fdc Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Wed, 14 Aug 2013 10:59:22 +0200 Subject: [PATCH 101/109] commander, mavlink: fixed base_mode and custom_mode in mavlink --- src/modules/commander/commander.cpp | 14 +++----- src/modules/mavlink/mavlink.c | 52 ++++++++++++++--------------- src/modules/mavlink/orb_listener.c | 22 ++++++------ src/modules/mavlink/util.h | 2 +- 4 files changed, 43 insertions(+), 47 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 7ede3e1e64..82b5754054 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -84,6 +84,7 @@ #include #include +#include "px4_custom_mode.h" #include "commander_helper.h" #include "state_machine_helper.h" #include "calibration_routines.h" @@ -138,13 +139,6 @@ enum MAV_MODE_FLAG { MAV_MODE_FLAG_ENUM_END = 129, /* | */ }; -enum PX4_CUSTOM_MODE { - PX4_CUSTOM_MODE_MANUAL = 1, - PX4_CUSTOM_MODE_SEATBELT, - PX4_CUSTOM_MODE_EASY, - PX4_CUSTOM_MODE_AUTO, -}; - /* Mavlink file descriptors */ static int mavlink_fd; @@ -1321,8 +1315,10 @@ toggle_status_leds(vehicle_status_s *status, actuator_armed_s *armed, vehicle_gp } else if (armed->ready_to_arm) { /* ready to arm, blink at 2.5Hz */ - if (leds_counter % 8 == 0) { - led_toggle(LED_AMBER); + if (leds_counter & 8) { + led_on(LED_AMBER); + } else { + led_off(LED_AMBER); } } else { diff --git a/src/modules/mavlink/mavlink.c b/src/modules/mavlink/mavlink.c index 9132d1b491..6d9ca1120e 100644 --- a/src/modules/mavlink/mavlink.c +++ b/src/modules/mavlink/mavlink.c @@ -64,6 +64,7 @@ #include #include #include +#include #include "waypoints.h" #include "orb_topics.h" @@ -181,10 +182,11 @@ set_hil_on_off(bool hil_enabled) } void -get_mavlink_mode_and_state(uint8_t *mavlink_state, uint8_t *mavlink_mode) +get_mavlink_mode_and_state(uint8_t *mavlink_state, uint8_t *mavlink_base_mode, uint32_t *mavlink_custom_mode) { /* reset MAVLink mode bitfield */ - *mavlink_mode = 0; + *mavlink_base_mode = 0; + *mavlink_custom_mode = 0; /** * Set mode flags @@ -192,36 +194,31 @@ get_mavlink_mode_and_state(uint8_t *mavlink_state, uint8_t *mavlink_mode) /* HIL */ if (v_status.hil_state == HIL_STATE_ON) { - *mavlink_mode |= MAV_MODE_FLAG_HIL_ENABLED; + *mavlink_base_mode |= MAV_MODE_FLAG_HIL_ENABLED; } - /* autonomous mode */ - if (v_status.main_state == MAIN_STATE_AUTO) { - *mavlink_mode |= MAV_MODE_FLAG_AUTO_ENABLED; - } - - /* set arming state */ + /* arming state */ if (v_status.arming_state == ARMING_STATE_ARMED || v_status.arming_state == ARMING_STATE_ARMED_ERROR) { - *mavlink_mode |= MAV_MODE_FLAG_SAFETY_ARMED; - - } else { - *mavlink_mode &= ~MAV_MODE_FLAG_SAFETY_ARMED; + *mavlink_base_mode |= MAV_MODE_FLAG_SAFETY_ARMED; } - if (v_status.main_state == MAIN_STATE_MANUAL - || v_status.main_state == MAIN_STATE_SEATBELT - || v_status.main_state == MAIN_STATE_EASY) { - - *mavlink_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED; + /* main state */ + *mavlink_base_mode |= MAV_MODE_FLAG_CUSTOM_MODE_ENABLED; + if (v_status.main_state == MAIN_STATE_MANUAL) { + *mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | (v_status.is_rotary_wing ? MAV_MODE_FLAG_STABILIZE_ENABLED : 0); + *mavlink_custom_mode = PX4_CUSTOM_MODE_MANUAL; + } else if (v_status.main_state == MAIN_STATE_SEATBELT) { + *mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED; + *mavlink_custom_mode = PX4_CUSTOM_MODE_SEATBELT; + } else if (v_status.main_state == MAIN_STATE_EASY) { + *mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED; + *mavlink_custom_mode = PX4_CUSTOM_MODE_EASY; + } else if (v_status.main_state == MAIN_STATE_AUTO) { + *mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED; + *mavlink_custom_mode = PX4_CUSTOM_MODE_AUTO; } - if (v_status.navigation_state == MAIN_STATE_EASY) { - - *mavlink_mode |= MAV_MODE_FLAG_GUIDED_ENABLED; - } - - /** * Set mavlink state **/ @@ -645,11 +642,12 @@ int mavlink_thread_main(int argc, char *argv[]) /* translate the current system state to mavlink state and mode */ uint8_t mavlink_state = 0; - uint8_t mavlink_mode = 0; - get_mavlink_mode_and_state(&mavlink_state, &mavlink_mode); + uint8_t mavlink_base_mode = 0; + uint32_t mavlink_custom_mode = 0; + get_mavlink_mode_and_state(&mavlink_state, &mavlink_base_mode, &mavlink_custom_mode); /* send heartbeat */ - mavlink_msg_heartbeat_send(chan, mavlink_system.type, MAV_AUTOPILOT_PX4, mavlink_mode, v_status.navigation_state, mavlink_state); + mavlink_msg_heartbeat_send(chan, mavlink_system.type, MAV_AUTOPILOT_PX4, mavlink_base_mode, mavlink_custom_mode, mavlink_state); /* switch HIL mode if required */ if (v_status.hil_state == HIL_STATE_ON) diff --git a/src/modules/mavlink/orb_listener.c b/src/modules/mavlink/orb_listener.c index d088d421ed..2a260861d3 100644 --- a/src/modules/mavlink/orb_listener.c +++ b/src/modules/mavlink/orb_listener.c @@ -279,15 +279,16 @@ l_vehicle_status(const struct listener *l) /* translate the current syste state to mavlink state and mode */ uint8_t mavlink_state = 0; - uint8_t mavlink_mode = 0; - get_mavlink_mode_and_state(&mavlink_state, &mavlink_mode); + uint8_t mavlink_base_mode = 0; + uint32_t mavlink_custom_mode = 0; + get_mavlink_mode_and_state(&mavlink_state, &mavlink_base_mode, &mavlink_custom_mode); /* send heartbeat */ mavlink_msg_heartbeat_send(chan, mavlink_system.type, MAV_AUTOPILOT_PX4, - mavlink_mode, - v_status.navigation_state, + mavlink_base_mode, + mavlink_custom_mode, mavlink_state); } @@ -473,8 +474,9 @@ l_actuator_outputs(const struct listener *l) /* translate the current syste state to mavlink state and mode */ uint8_t mavlink_state = 0; - uint8_t mavlink_mode = 0; - get_mavlink_mode_and_state(&mavlink_state, &mavlink_mode); + uint8_t mavlink_base_mode = 0; + uint32_t mavlink_custom_mode = 0; + get_mavlink_mode_and_state(&mavlink_state, &mavlink_base_mode, &mavlink_custom_mode); /* HIL message as per MAVLink spec */ @@ -491,7 +493,7 @@ l_actuator_outputs(const struct listener *l) -1, -1, -1, - mavlink_mode, + mavlink_base_mode, 0); } else if (mavlink_system.type == MAV_TYPE_HEXAROTOR) { @@ -505,7 +507,7 @@ l_actuator_outputs(const struct listener *l) ((act_outputs.output[5] - 900.0f) / 600.0f) / 2.0f, -1, -1, - mavlink_mode, + mavlink_base_mode, 0); } else if (mavlink_system.type == MAV_TYPE_OCTOROTOR) { @@ -519,7 +521,7 @@ l_actuator_outputs(const struct listener *l) ((act_outputs.output[5] - 900.0f) / 600.0f) / 2.0f, ((act_outputs.output[6] - 900.0f) / 600.0f) / 2.0f, ((act_outputs.output[7] - 900.0f) / 600.0f) / 2.0f, - mavlink_mode, + mavlink_base_mode, 0); } else { @@ -533,7 +535,7 @@ l_actuator_outputs(const struct listener *l) (act_outputs.output[5] - 1500.0f) / 500.0f, (act_outputs.output[6] - 1500.0f) / 500.0f, (act_outputs.output[7] - 1500.0f) / 500.0f, - mavlink_mode, + mavlink_base_mode, 0); } } diff --git a/src/modules/mavlink/util.h b/src/modules/mavlink/util.h index a4ff06a883..5e5ee8261c 100644 --- a/src/modules/mavlink/util.h +++ b/src/modules/mavlink/util.h @@ -51,4 +51,4 @@ extern mavlink_wpm_storage *wpm; /** * Translate the custom state into standard mavlink modes and state. */ -extern void get_mavlink_mode_and_state(uint8_t *mavlink_state, uint8_t *mavlink_mode); +extern void get_mavlink_mode_and_state(uint8_t *mavlink_state, uint8_t *mavlink_base_mode, uint32_t *mavlink_custom_mode); From 3a21cacdbbf507f47a45035af7fda04ac787f9b0 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Wed, 14 Aug 2013 16:00:35 +0200 Subject: [PATCH 102/109] Fix bug where IO was in override mode for copter (workaround was to disconnect and reconnect Rx --- src/modules/px4iofirmware/controls.c | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/modules/px4iofirmware/controls.c b/src/modules/px4iofirmware/controls.c index 43d96fb067..fbd82a4c6d 100644 --- a/src/modules/px4iofirmware/controls.c +++ b/src/modules/px4iofirmware/controls.c @@ -300,6 +300,8 @@ controls_tick() { } else { r_status_flags &= ~PX4IO_P_STATUS_FLAGS_OVERRIDE; } + } else { + r_status_flags &= ~PX4IO_P_STATUS_FLAGS_OVERRIDE; } } From 9505654f9103c8965891991514ea690b3e6aea25 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Wed, 14 Aug 2013 17:57:01 +0200 Subject: [PATCH 103/109] commander/px4_custom_mode.h added --- src/modules/commander/px4_custom_mode.h | 18 ++++++++++++++++++ 1 file changed, 18 insertions(+) create mode 100644 src/modules/commander/px4_custom_mode.h diff --git a/src/modules/commander/px4_custom_mode.h b/src/modules/commander/px4_custom_mode.h new file mode 100644 index 0000000000..4d4859a5c5 --- /dev/null +++ b/src/modules/commander/px4_custom_mode.h @@ -0,0 +1,18 @@ +/* + * px4_custom_mode.h + * + * Created on: 09.08.2013 + * Author: ton + */ + +#ifndef PX4_CUSTOM_MODE_H_ +#define PX4_CUSTOM_MODE_H_ + +enum PX4_CUSTOM_MODE { + PX4_CUSTOM_MODE_MANUAL = 1, + PX4_CUSTOM_MODE_SEATBELT, + PX4_CUSTOM_MODE_EASY, + PX4_CUSTOM_MODE_AUTO, +}; + +#endif /* PX4_CUSTOM_MODE_H_ */ From 53def47216d5bbacdfdb76428c024ba3feaea64e Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Thu, 8 Aug 2013 17:23:51 +0200 Subject: [PATCH 104/109] Fixed gyro scale calibration (it was storing scale even though the scale calibration was skipped --- src/modules/commander/gyro_calibration.cpp | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) diff --git a/src/modules/commander/gyro_calibration.cpp b/src/modules/commander/gyro_calibration.cpp index 9e6909db07..60b747ee0f 100644 --- a/src/modules/commander/gyro_calibration.cpp +++ b/src/modules/commander/gyro_calibration.cpp @@ -177,8 +177,12 @@ void do_gyro_calibration(int mavlink_fd) /* abort this loop if not rotated more than 180 degrees within 5 seconds */ if ((fabsf(baseline_integral / (2.0f * M_PI_F)) < 0.6f) - && (hrt_absolute_time() - start_time > 5 * 1e6)) - break; + && (hrt_absolute_time() - start_time > 5 * 1e6)) { + mavlink_log_info(mavlink_fd, "gyro scale calibration skipped"); + mavlink_log_info(mavlink_fd, "gyro calibration done"); + tune_positive(); + return; + } /* wait blocking for new data */ struct pollfd fds[1]; From 27d0885453711a3d3ab6abf3cf227afc837e14bd Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Wed, 14 Aug 2013 22:38:14 +0200 Subject: [PATCH 105/109] gyro_calibration: confusing typo in mavlink message fixed --- src/modules/commander/gyro_calibration.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/commander/gyro_calibration.cpp b/src/modules/commander/gyro_calibration.cpp index 60b747ee0f..f1afb0107e 100644 --- a/src/modules/commander/gyro_calibration.cpp +++ b/src/modules/commander/gyro_calibration.cpp @@ -154,7 +154,7 @@ void do_gyro_calibration(int mavlink_fd) /*** --- SCALING --- ***/ - mavlink_log_info(mavlink_fd, "offset calibration finished. Rotate for scale 130x"); + mavlink_log_info(mavlink_fd, "offset calibration finished. Rotate for scale 30x"); mavlink_log_info(mavlink_fd, "or do not rotate and wait for 5 seconds to skip."); warnx("offset calibration finished. Rotate for scale 30x, or do not rotate and wait for 5 seconds to skip."); From 0bbc4b7012c72fda61dca01a897552e9483a4f5f Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 15 Aug 2013 08:42:08 +0200 Subject: [PATCH 106/109] Build fixes --- makefiles/config_px4fmu-v1_default.mk | 2 +- src/drivers/px4io/px4io.cpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/makefiles/config_px4fmu-v1_default.mk b/makefiles/config_px4fmu-v1_default.mk index 2f70d001d8..33ffba6bfd 100644 --- a/makefiles/config_px4fmu-v1_default.mk +++ b/makefiles/config_px4fmu-v1_default.mk @@ -76,7 +76,7 @@ MODULES += examples/flow_position_estimator # # Vehicle Control # -MODULES += modules/segway +#MODULES += modules/segway # XXX needs state machine update MODULES += modules/fixedwing_backside MODULES += modules/fixedwing_att_control MODULES += modules/fixedwing_pos_control diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp index 2953639bf2..65e8fa4b6b 100644 --- a/src/drivers/px4io/px4io.cpp +++ b/src/drivers/px4io/px4io.cpp @@ -870,7 +870,7 @@ PX4IO::io_set_arming_state() uint16_t set = 0; uint16_t clear = 0; - _system_armed = vstatus.flag_system_armed; + _system_armed = armed.armed; if (armed.armed && !armed.lockdown) { set |= PX4IO_P_SETUP_ARMING_FMU_ARMED; From cc9f1e81adaa71c5f86f56df45cf14f8bc8e7abc Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 15 Aug 2013 09:52:22 +0200 Subject: [PATCH 107/109] Rejecting arming if safety switch is not in safe position, added reboot command --- src/modules/commander/commander.cpp | 49 +++++++++++++------ .../commander/state_machine_helper.cpp | 21 ++++++-- src/modules/commander/state_machine_helper.h | 6 ++- 3 files changed, 58 insertions(+), 18 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 82b5754054..d4c2c4c844 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -265,7 +265,7 @@ void usage(const char *reason) exit(1); } -void handle_command(struct vehicle_status_s *status, struct vehicle_control_mode_s *control_mode, struct vehicle_command_s *cmd, struct actuator_armed_s *armed) +void handle_command(struct vehicle_status_s *status, const struct safety_s *safety, struct vehicle_control_mode_s *control_mode, struct vehicle_command_s *cmd, struct actuator_armed_s *armed) { /* result of the command */ uint8_t result = VEHICLE_CMD_RESULT_UNSUPPORTED; @@ -282,7 +282,7 @@ void handle_command(struct vehicle_status_s *status, struct vehicle_control_mode transition_result_t arming_res = TRANSITION_NOT_CHANGED; if (base_mode & MAV_MODE_FLAG_SAFETY_ARMED) { - arming_res = arming_state_transition(status, ARMING_STATE_ARMED, armed); + arming_res = arming_state_transition(status, safety, ARMING_STATE_ARMED, armed); if (arming_res == TRANSITION_CHANGED) { mavlink_log_info(mavlink_fd, "[cmd] ARMED by command"); @@ -291,7 +291,7 @@ void handle_command(struct vehicle_status_s *status, struct vehicle_control_mode } else { if (status->arming_state == ARMING_STATE_ARMED || status->arming_state == ARMING_STATE_ARMED_ERROR) { arming_state_t new_arming_state = (status->arming_state == ARMING_STATE_ARMED ? ARMING_STATE_STANDBY : ARMING_STATE_STANDBY_ERROR); - arming_res = arming_state_transition(status, new_arming_state, armed); + arming_res = arming_state_transition(status, safety, new_arming_state, armed); if (arming_res == TRANSITION_CHANGED) { mavlink_log_info(mavlink_fd, "[cmd] DISARMED by command"); @@ -356,6 +356,23 @@ void handle_command(struct vehicle_status_s *status, struct vehicle_control_mode break; case VEHICLE_CMD_PREFLIGHT_REBOOT_SHUTDOWN: + if (is_safe(status, safety, armed)) { + + if (((int)(cmd->param1)) == 1) { + /* reboot */ + up_systemreset(); + } else if (((int)(cmd->param1)) == 3) { + /* reboot to bootloader */ + + // XXX implement + result = VEHICLE_CMD_RESULT_UNSUPPORTED; + } else { + result = VEHICLE_CMD_RESULT_DENIED; + } + + } else { + result = VEHICLE_CMD_RESULT_DENIED; + } break; case VEHICLE_CMD_PREFLIGHT_CALIBRATION: { @@ -388,7 +405,7 @@ void handle_command(struct vehicle_status_s *status, struct vehicle_control_mode /* check if we have new task and no other task is scheduled */ if (low_prio_task == LOW_PRIO_TASK_NONE && new_low_prio_task != LOW_PRIO_TASK_NONE) { /* try to go to INIT/PREFLIGHT arming state */ - if (TRANSITION_DENIED != arming_state_transition(status, ARMING_STATE_INIT, armed)) { + if (TRANSITION_DENIED != arming_state_transition(status, safety, ARMING_STATE_INIT, armed)) { result = VEHICLE_CMD_RESULT_ACCEPTED; low_prio_task = new_low_prio_task; @@ -407,10 +424,10 @@ void handle_command(struct vehicle_status_s *status, struct vehicle_control_mode low_prio_task_t new_low_prio_task = LOW_PRIO_TASK_NONE; if (((int)(cmd->param1)) == 0) { - low_prio_task = LOW_PRIO_TASK_PARAM_LOAD; + new_low_prio_task = LOW_PRIO_TASK_PARAM_LOAD; } else if (((int)(cmd->param1)) == 1) { - low_prio_task = LOW_PRIO_TASK_PARAM_SAVE; + new_low_prio_task = LOW_PRIO_TASK_PARAM_SAVE; } /* check if we have new task and no other task is scheduled */ @@ -605,6 +622,8 @@ int commander_thread_main(int argc, char *argv[]) int safety_sub = orb_subscribe(ORB_ID(safety)); struct safety_s safety; memset(&safety, 0, sizeof(safety)); + safety.safety_switch_available = false; + safety.safety_off = false; /* Subscribe to manual control data */ int sp_man_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); @@ -746,7 +765,7 @@ int commander_thread_main(int argc, char *argv[]) orb_copy(ORB_ID(vehicle_command), cmd_sub, &cmd); /* handle it */ - handle_command(&status, &control_mode, &cmd, &armed); + handle_command(&status, &safety, &control_mode, &cmd, &armed); } /* update safety topic */ @@ -871,7 +890,7 @@ int commander_thread_main(int argc, char *argv[]) critical_battery_voltage_actions_done = true; mavlink_log_critical(mavlink_fd, "[cmd] EMERGENCY: CRITICAL BATTERY"); status.battery_warning = VEHICLE_BATTERY_WARNING_ALERT; - arming_state_transition(&status, ARMING_STATE_ARMED_ERROR, &armed); + arming_state_transition(&status, &safety, ARMING_STATE_ARMED_ERROR, &armed); status_changed = true; } @@ -887,7 +906,7 @@ int commander_thread_main(int argc, char *argv[]) /* If in INIT state, try to proceed to STANDBY state */ if (status.arming_state == ARMING_STATE_INIT && low_prio_task == LOW_PRIO_TASK_NONE) { // XXX check for sensors - arming_state_transition(&status, ARMING_STATE_STANDBY, &armed); + arming_state_transition(&status, &safety, ARMING_STATE_STANDBY, &armed); } else { // XXX: Add emergency stuff if sensors are lost @@ -1026,7 +1045,7 @@ int commander_thread_main(int argc, char *argv[]) if (stick_off_counter > STICK_ON_OFF_COUNTER_LIMIT) { /* disarm to STANDBY if ARMED or to STANDBY_ERROR if ARMED_ERROR */ arming_state_t new_arming_state = (status.arming_state == ARMING_STATE_ARMED ? ARMING_STATE_STANDBY : ARMING_STATE_STANDBY_ERROR); - res = arming_state_transition(&status, new_arming_state, &armed); + res = arming_state_transition(&status, &safety, new_arming_state, &armed); stick_off_counter = 0; } else { @@ -1045,7 +1064,7 @@ int commander_thread_main(int argc, char *argv[]) status.main_state == MAIN_STATE_MANUAL) { if (sp_man.yaw > STICK_ON_OFF_LIMIT && sp_man.throttle < STICK_THRUST_RANGE * 0.1f) { if (stick_on_counter > STICK_ON_OFF_COUNTER_LIMIT) { - res = arming_state_transition(&status, ARMING_STATE_ARMED, &armed); + res = arming_state_transition(&status, &safety, ARMING_STATE_ARMED, &armed); stick_on_counter = 0; } else { @@ -1168,10 +1187,10 @@ int commander_thread_main(int argc, char *argv[]) // XXX check if this is correct /* arm / disarm on request */ if (sp_offboard.armed && !armed.armed) { - arming_state_transition(&status, ARMING_STATE_ARMED, &armed); + arming_state_transition(&status, &safety, ARMING_STATE_ARMED, &armed); } else if (!sp_offboard.armed && armed.armed) { - arming_state_transition(&status, ARMING_STATE_STANDBY, &armed); + arming_state_transition(&status, &safety, ARMING_STATE_STANDBY, &armed); } } else { @@ -1243,7 +1262,7 @@ int commander_thread_main(int argc, char *argv[]) } /* play arming and battery warning tunes */ - if (!arm_tune_played && armed.armed && (!safety.safety_switch_available || safety.safety_off)) { + if (!arm_tune_played && armed.armed) { /* play tune when armed */ if (tune_arm() == OK) arm_tune_played = true; @@ -1540,6 +1559,8 @@ void *commander_low_prio_loop(void *arg) /* Set thread name */ prctl(PR_SET_NAME, "commander_low_prio", getpid()); + low_prio_task = LOW_PRIO_TASK_NONE; + while (!thread_should_exit) { switch (low_prio_task) { diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 06cd060c5d..163aceed2e 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -67,7 +67,7 @@ static bool main_state_changed = true; static bool navigation_state_changed = true; transition_result_t -arming_state_transition(struct vehicle_status_s *status, arming_state_t new_arming_state, struct actuator_armed_s *armed) +arming_state_transition(struct vehicle_status_s *status, const struct safety_s *safety, arming_state_t new_arming_state, struct actuator_armed_s *armed) { transition_result_t ret = TRANSITION_DENIED; @@ -108,8 +108,10 @@ arming_state_transition(struct vehicle_status_s *status, arming_state_t new_armi case ARMING_STATE_ARMED: /* allow arming from STANDBY and IN-AIR-RESTORE */ - if (status->arming_state == ARMING_STATE_STANDBY - || status->arming_state == ARMING_STATE_IN_AIR_RESTORE) { + if ((status->arming_state == ARMING_STATE_STANDBY + || status->arming_state == ARMING_STATE_IN_AIR_RESTORE) + && (!safety->safety_switch_available || safety->safety_off)) /* only allow arming if safety is off */ + { ret = TRANSITION_CHANGED; armed->armed = true; armed->ready_to_arm = false; @@ -172,6 +174,19 @@ arming_state_transition(struct vehicle_status_s *status, arming_state_t new_armi return ret; } +bool is_safe(const struct vehicle_status_s *current_state, const struct safety_s *safety, const struct actuator_armed_s *armed) +{ + // System is safe if: + // 1) Not armed + // 2) Armed, but in software lockdown (HIL) + // 3) Safety switch is present AND engaged -> actuators locked + if (!armed->armed || (armed->armed && armed->lockdown) || (safety->safety_switch_available && !safety->safety_off)) { + return true; + } else { + return false; + } +} + bool check_arming_state_changed() { diff --git a/src/modules/commander/state_machine_helper.h b/src/modules/commander/state_machine_helper.h index c8c77e5d8a..a38c2497eb 100644 --- a/src/modules/commander/state_machine_helper.h +++ b/src/modules/commander/state_machine_helper.h @@ -47,6 +47,7 @@ #include #include #include +#include #include typedef enum { @@ -56,7 +57,10 @@ typedef enum { } transition_result_t; -transition_result_t arming_state_transition(struct vehicle_status_s *current_state, arming_state_t new_arming_state, struct actuator_armed_s *armed); +transition_result_t arming_state_transition(struct vehicle_status_s *current_state, const struct safety_s *safety, + arming_state_t new_arming_state, struct actuator_armed_s *armed); + +bool is_safe(const struct vehicle_status_s *current_state, const struct safety_s *safety, const struct actuator_armed_s *armed); bool check_arming_state_changed(); From e5af29be1706b1d20d6bafe8c481213c76cc0d34 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Wed, 14 Aug 2013 12:38:25 +0200 Subject: [PATCH 108/109] Fixed param save and load --- src/modules/commander/commander.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 82b5754054..5b3654bcde 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -407,10 +407,10 @@ void handle_command(struct vehicle_status_s *status, struct vehicle_control_mode low_prio_task_t new_low_prio_task = LOW_PRIO_TASK_NONE; if (((int)(cmd->param1)) == 0) { - low_prio_task = LOW_PRIO_TASK_PARAM_LOAD; + new_low_prio_task = LOW_PRIO_TASK_PARAM_LOAD; } else if (((int)(cmd->param1)) == 1) { - low_prio_task = LOW_PRIO_TASK_PARAM_SAVE; + new_low_prio_task = LOW_PRIO_TASK_PARAM_SAVE; } /* check if we have new task and no other task is scheduled */ From 39ae01dd07d53e3509826ae3737fc6a509adec34 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Wed, 14 Aug 2013 13:29:42 +0200 Subject: [PATCH 109/109] Fix the low priority loop, calibration routines work again --- src/modules/commander/commander.cpp | 12 ++++++++---- 1 file changed, 8 insertions(+), 4 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 5b3654bcde..c6e209f0f3 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1552,7 +1552,7 @@ void *commander_low_prio_loop(void *arg) mavlink_log_critical(mavlink_fd, "[cmd] parameters load ERROR"); tune_error(); } - + low_prio_task = LOW_PRIO_TASK_NONE; break; case LOW_PRIO_TASK_PARAM_SAVE: @@ -1564,37 +1564,43 @@ void *commander_low_prio_loop(void *arg) mavlink_log_critical(mavlink_fd, "[cmd] parameters save error"); tune_error(); } - + low_prio_task = LOW_PRIO_TASK_NONE; break; case LOW_PRIO_TASK_GYRO_CALIBRATION: do_gyro_calibration(mavlink_fd); + low_prio_task = LOW_PRIO_TASK_NONE; break; case LOW_PRIO_TASK_MAG_CALIBRATION: do_mag_calibration(mavlink_fd); + low_prio_task = LOW_PRIO_TASK_NONE; break; case LOW_PRIO_TASK_ALTITUDE_CALIBRATION: // do_baro_calibration(mavlink_fd); + low_prio_task = LOW_PRIO_TASK_NONE; break; case LOW_PRIO_TASK_RC_CALIBRATION: // do_rc_calibration(mavlink_fd); + low_prio_task = LOW_PRIO_TASK_NONE; break; case LOW_PRIO_TASK_ACCEL_CALIBRATION: do_accel_calibration(mavlink_fd); + low_prio_task = LOW_PRIO_TASK_NONE; break; case LOW_PRIO_TASK_AIRSPEED_CALIBRATION: do_airspeed_calibration(mavlink_fd); + low_prio_task = LOW_PRIO_TASK_NONE; break; case LOW_PRIO_TASK_NONE: @@ -1604,8 +1610,6 @@ void *commander_low_prio_loop(void *arg) break; } - low_prio_task = LOW_PRIO_TASK_NONE; - } return 0;