From 7f4f9a5f5f6c4921453f3e0c98531bcb4e44d981 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Fri, 31 Jan 2014 22:44:05 +0100 Subject: [PATCH 001/329] ACRO mode implemented --- src/modules/commander/commander.cpp | 35 +++++- .../commander/state_machine_helper.cpp | 4 + .../mc_att_control/mc_att_control_main.cpp | 102 +++++++++++++----- .../mc_att_control/mc_att_control_params.c | 3 + src/modules/sensors/sensor_params.c | 1 + src/modules/sensors/sensors.cpp | 14 +++ .../uORB/topics/manual_control_setpoint.h | 1 + src/modules/uORB/topics/rc_channels.h | 11 +- src/modules/uORB/topics/vehicle_status.h | 8 ++ 9 files changed, 144 insertions(+), 35 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index c39833713d..89294fc76c 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -619,9 +619,10 @@ int commander_thread_main(int argc, char *argv[]) char *main_states_str[MAIN_STATE_MAX]; main_states_str[0] = "MANUAL"; - main_states_str[1] = "SEATBELT"; - main_states_str[2] = "EASY"; - main_states_str[3] = "AUTO"; + main_states_str[1] = "ACRO"; + main_states_str[2] = "SEATBELT"; + main_states_str[3] = "EASY"; + main_states_str[4] = "AUTO"; char *arming_states_str[ARMING_STATE_MAX]; arming_states_str[0] = "INIT"; @@ -1510,6 +1511,17 @@ check_mode_switches(struct manual_control_setpoint_s *sp_man, struct vehicle_sta } else { status->mission_switch = MISSION_SWITCH_MISSION; } + + /* acro switch */ + if (!isfinite(sp_man->acro_switch)) { + status->acro_switch = ACRO_SWITCH_NONE; + + } else if (sp_man->acro_switch > STICK_ON_OFF_LIMIT) { + status->acro_switch = ACRO_SWITCH_ACRO; + + } else { + status->acro_switch = ACRO_SWITCH_NORMAL; + } } transition_result_t @@ -1520,7 +1532,11 @@ set_main_state_rc(struct vehicle_status_s *status) switch (status->mode_switch) { case MODE_SWITCH_MANUAL: - res = main_state_transition(status, MAIN_STATE_MANUAL); + if (status->acro_switch == ACRO_SWITCH_ACRO) { + res = main_state_transition(status, MAIN_STATE_ACRO); + } else { + res = main_state_transition(status, MAIN_STATE_MANUAL); + } // TRANSITION_DENIED is not possible here break; @@ -1600,6 +1616,17 @@ set_control_mode() control_mode.flag_control_velocity_enabled = false; break; + case MAIN_STATE_ACRO: + control_mode.flag_control_manual_enabled = true; + control_mode.flag_control_auto_enabled = false; + control_mode.flag_control_rates_enabled = true; + control_mode.flag_control_attitude_enabled = false; + control_mode.flag_control_altitude_enabled = false; + control_mode.flag_control_climb_rate_enabled = false; + control_mode.flag_control_position_enabled = false; + control_mode.flag_control_velocity_enabled = false; + break; + case MAIN_STATE_SEATBELT: control_mode.flag_control_manual_enabled = true; control_mode.flag_control_auto_enabled = false; diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 43d0e023e3..fd966a0682 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -230,6 +230,10 @@ main_state_transition(struct vehicle_status_s *status, main_state_t new_main_sta ret = TRANSITION_CHANGED; break; + case MAIN_STATE_ACRO: + ret = TRANSITION_CHANGED; + break; + case MAIN_STATE_SEATBELT: /* need at minimum altitude estimate */ diff --git a/src/modules/mc_att_control/mc_att_control_main.cpp b/src/modules/mc_att_control/mc_att_control_main.cpp index a0accb8552..5f862652a3 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -161,7 +161,12 @@ private: param_t yaw_rate_i; param_t yaw_rate_d; param_t yaw_ff; + param_t roll_scale_acro; + param_t pitch_scale_acro; + param_t yaw_scale_acro; + param_t rc_scale_roll; + param_t rc_scale_pitch; param_t rc_scale_yaw; } _params_handles; /**< handles for interesting parameters */ @@ -171,8 +176,9 @@ private: math::Vector<3> rate_i; /**< I gain for angular rate error */ math::Vector<3> rate_d; /**< D gain for angular rate error */ float yaw_ff; /**< yaw control feed-forward */ + math::Vector<3> scale_acro; /**< scale for ACRO mode */ - float rc_scale_yaw; + math::Vector<3> rc_scale; /**< scale for MANUAL mode */ } _params; /** @@ -275,6 +281,8 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() : _params.rate_p.zero(); _params.rate_i.zero(); _params.rate_d.zero(); + _params.scale_acro.zero(); + _params.rc_scale.zero(); _R_sp.identity(); _R.identity(); @@ -286,21 +294,26 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() : I.identity(); - _params_handles.roll_p = param_find("MC_ROLL_P"); - _params_handles.roll_rate_p = param_find("MC_ROLLRATE_P"); - _params_handles.roll_rate_i = param_find("MC_ROLLRATE_I"); - _params_handles.roll_rate_d = param_find("MC_ROLLRATE_D"); - _params_handles.pitch_p = param_find("MC_PITCH_P"); - _params_handles.pitch_rate_p = param_find("MC_PITCHRATE_P"); - _params_handles.pitch_rate_i = param_find("MC_PITCHRATE_I"); - _params_handles.pitch_rate_d = param_find("MC_PITCHRATE_D"); - _params_handles.yaw_p = param_find("MC_YAW_P"); - _params_handles.yaw_rate_p = param_find("MC_YAWRATE_P"); - _params_handles.yaw_rate_i = param_find("MC_YAWRATE_I"); - _params_handles.yaw_rate_d = param_find("MC_YAWRATE_D"); - _params_handles.yaw_ff = param_find("MC_YAW_FF"); + _params_handles.roll_p = param_find("MC_ROLL_P"); + _params_handles.roll_rate_p = param_find("MC_ROLLRATE_P"); + _params_handles.roll_rate_i = param_find("MC_ROLLRATE_I"); + _params_handles.roll_rate_d = param_find("MC_ROLLRATE_D"); + _params_handles.pitch_p = param_find("MC_PITCH_P"); + _params_handles.pitch_rate_p = param_find("MC_PITCHRATE_P"); + _params_handles.pitch_rate_i = param_find("MC_PITCHRATE_I"); + _params_handles.pitch_rate_d = param_find("MC_PITCHRATE_D"); + _params_handles.yaw_p = param_find("MC_YAW_P"); + _params_handles.yaw_rate_p = param_find("MC_YAWRATE_P"); + _params_handles.yaw_rate_i = param_find("MC_YAWRATE_I"); + _params_handles.yaw_rate_d = param_find("MC_YAWRATE_D"); + _params_handles.yaw_ff = param_find("MC_YAW_FF"); + _params_handles.roll_scale_acro = param_find("MC_ROLL_S_ACRO"); + _params_handles.pitch_scale_acro = param_find("MC_PITCH_S_ACRO"); + _params_handles.yaw_scale_acro = param_find("MC_YAW_S_ACRO"); - _params_handles.rc_scale_yaw = param_find("RC_SCALE_YAW"); + _params_handles.rc_scale_roll = param_find("RC_SCALE_ROLL"); + _params_handles.rc_scale_pitch = param_find("RC_SCALE_PITCH"); + _params_handles.rc_scale_yaw = param_find("RC_SCALE_YAW"); /* fetch initial parameter values */ parameters_update(); @@ -344,6 +357,10 @@ MulticopterAttitudeControl::parameters_update() _params.rate_i(0) = v; param_get(_params_handles.roll_rate_d, &v); _params.rate_d(0) = v; + param_get(_params_handles.roll_scale_acro, &v); + _params.scale_acro(0) = v; + param_get(_params_handles.rc_scale_roll, &v); + _params.rc_scale(0) = v; /* pitch */ param_get(_params_handles.pitch_p, &v); @@ -354,6 +371,10 @@ MulticopterAttitudeControl::parameters_update() _params.rate_i(1) = v; param_get(_params_handles.pitch_rate_d, &v); _params.rate_d(1) = v; + param_get(_params_handles.pitch_scale_acro, &v); + _params.scale_acro(1) = v; + param_get(_params_handles.rc_scale_pitch, &v); + _params.rc_scale(1) = v; /* yaw */ param_get(_params_handles.yaw_p, &v); @@ -364,10 +385,11 @@ MulticopterAttitudeControl::parameters_update() _params.rate_i(2) = v; param_get(_params_handles.yaw_rate_d, &v); _params.rate_d(2) = v; - param_get(_params_handles.yaw_ff, &_params.yaw_ff); - - param_get(_params_handles.rc_scale_yaw, &_params.rc_scale_yaw); + param_get(_params_handles.yaw_scale_acro, &v); + _params.scale_acro(2) = v; + param_get(_params_handles.rc_scale_yaw, &v); + _params.rc_scale(2) = v; return OK; } @@ -463,6 +485,7 @@ MulticopterAttitudeControl::control_attitude(float dt) if (_v_control_mode.flag_control_manual_enabled) { /* manual input, set or modify attitude setpoint */ + vehicle_manual_poll(); if (_v_control_mode.flag_control_velocity_enabled || _v_control_mode.flag_control_climb_rate_enabled) { /* in assisted modes poll 'vehicle_attitude_setpoint' topic and modify it */ @@ -488,16 +511,16 @@ MulticopterAttitudeControl::control_attitude(float dt) // reset_yaw_sp = true; //} } else { - float yaw_dz_scaled = YAW_DEADZONE * _params.rc_scale_yaw; - if (_params.rc_scale_yaw > 0.001f && fabs(_manual_control_sp.yaw) > yaw_dz_scaled) { + float yaw_dz_scaled = YAW_DEADZONE * _params.rc_scale(2); + if (_params.rc_scale(2) > 0.001f && fabs(_manual_control_sp.yaw) > yaw_dz_scaled) { /* move yaw setpoint */ - yaw_sp_move_rate = _manual_control_sp.yaw / _params.rc_scale_yaw; + yaw_sp_move_rate = _manual_control_sp.yaw / _params.rc_scale(2); if (_manual_control_sp.yaw > 0.0f) { yaw_sp_move_rate -= YAW_DEADZONE; } else { yaw_sp_move_rate += YAW_DEADZONE; } - yaw_sp_move_rate *= _params.rc_scale_yaw; + yaw_sp_move_rate *= _params.rc_scale(2); _v_att_sp.yaw_body = _wrap_pi(_v_att_sp.yaw_body + yaw_sp_move_rate * dt); _v_att_sp.R_valid = false; publish_att_sp = true; @@ -743,7 +766,6 @@ MulticopterAttitudeControl::task_main() parameter_update_poll(); vehicle_control_mode_poll(); arming_status_poll(); - vehicle_manual_poll(); if (_v_control_mode.flag_control_attitude_enabled) { control_attitude(dt); @@ -764,9 +786,37 @@ MulticopterAttitudeControl::task_main() } else { /* attitude controller disabled */ - // TODO poll 'attitude_rates_setpoint' topic - _rates_sp.zero(); - _thrust_sp = 0.0f; + if (_v_control_mode.flag_control_manual_enabled) { + /* manual rates control, ACRO mode */ + vehicle_manual_poll(); + + _rates_sp(0) = _manual_control_sp.roll; + _rates_sp(1) = _manual_control_sp.pitch; + _rates_sp(2) = _manual_control_sp.yaw; + + /* rescale controls for ACRO mode */ + _rates_sp = _rates_sp.edivide(_params.rc_scale).emult(_params.scale_acro); + _thrust_sp = _manual_control_sp.throttle; + + /* publish attitude rates setpoint */ + _v_rates_sp.roll = _rates_sp(0); + _v_rates_sp.pitch = _rates_sp(1); + _v_rates_sp.yaw = _rates_sp(2); + _v_rates_sp.thrust = _thrust_sp; + _v_rates_sp.timestamp = hrt_absolute_time(); + + if (_v_rates_sp_pub > 0) { + orb_publish(ORB_ID(vehicle_rates_setpoint), _v_rates_sp_pub, &_v_rates_sp); + + } else { + _v_rates_sp_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint), &_v_rates_sp); + } + + } else { + // TODO poll 'attitude_rates_setpoint' topic + _rates_sp.zero(); + _thrust_sp = 0.0f; + } } if (_v_control_mode.flag_control_rates_enabled) { diff --git a/src/modules/mc_att_control/mc_att_control_params.c b/src/modules/mc_att_control/mc_att_control_params.c index 27a45b6bbc..f3a4022c84 100644 --- a/src/modules/mc_att_control/mc_att_control_params.c +++ b/src/modules/mc_att_control/mc_att_control_params.c @@ -54,3 +54,6 @@ PARAM_DEFINE_FLOAT(MC_YAWRATE_P, 0.3f); PARAM_DEFINE_FLOAT(MC_YAWRATE_I, 0.0f); PARAM_DEFINE_FLOAT(MC_YAWRATE_D, 0.0f); PARAM_DEFINE_FLOAT(MC_YAW_FF, 0.5f); +PARAM_DEFINE_FLOAT(MC_ROLL_S_ACRO, 5.0f); +PARAM_DEFINE_FLOAT(MC_PITCH_S_ACRO, 5.0f); +PARAM_DEFINE_FLOAT(MC_YAW_S_ACRO, 3.0f); diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c index 30659fd3a0..e72b48a881 100644 --- a/src/modules/sensors/sensor_params.c +++ b/src/modules/sensors/sensor_params.c @@ -449,6 +449,7 @@ PARAM_DEFINE_INT32(RC_MAP_MODE_SW, 0); PARAM_DEFINE_INT32(RC_MAP_RETURN_SW, 0); PARAM_DEFINE_INT32(RC_MAP_ASSIST_SW, 0); PARAM_DEFINE_INT32(RC_MAP_MISSIO_SW, 0); +PARAM_DEFINE_INT32(RC_MAP_ACRO_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 bd665b592b..50ddec8a94 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -246,6 +246,7 @@ private: int rc_map_return_sw; int rc_map_assisted_sw; int rc_map_mission_sw; + int rc_map_acro_sw; // int rc_map_offboard_ctrl_mode_sw; @@ -296,6 +297,7 @@ private: param_t rc_map_return_sw; param_t rc_map_assisted_sw; param_t rc_map_mission_sw; + param_t rc_map_acro_sw; // param_t rc_map_offboard_ctrl_mode_sw; @@ -515,6 +517,7 @@ Sensors::Sensors() : /* 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_acro_sw = param_find("RC_MAP_ACRO_SW"); // _parameter_handles.rc_map_offboard_ctrl_mode_sw = param_find("RC_MAP_OFFB_SW"); @@ -669,6 +672,10 @@ Sensors::parameters_update() warnx("Failed getting mission sw chan index"); } + if (param_get(_parameter_handles.rc_map_acro_sw, &(_parameters.rc_map_acro_sw)) != OK) { + warnx("Failed getting acro sw chan index"); + } + if (param_get(_parameter_handles.rc_map_flaps, &(_parameters.rc_map_flaps)) != OK) { warnx("Failed getting flaps chan index"); } @@ -700,6 +707,7 @@ Sensors::parameters_update() _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[ACRO] = _parameters.rc_map_acro_sw - 1; _rc.function[FLAPS] = _parameters.rc_map_flaps - 1; @@ -1290,6 +1298,7 @@ Sensors::rc_poll() manual_control.return_switch = NAN; manual_control.assisted_switch = NAN; manual_control.mission_switch = NAN; + manual_control.acro_switch = NAN; // manual_control.auto_offboard_input_switch = NAN; manual_control.flaps = NAN; @@ -1428,6 +1437,11 @@ Sensors::rc_poll() manual_control.mission_switch = limit_minus_one_to_one(_rc.chan[_rc.function[MISSION]].scaled); } + /* acro switch input */ + if (_rc.function[ACRO] >= 0) { + manual_control.acro_switch = limit_minus_one_to_one(_rc.chan[_rc.function[ACRO]].scaled); + } + /* return switch input */ if (_rc.function[RETURN] >= 0) { manual_control.return_switch = limit_minus_one_to_one(_rc.chan[_rc.function[RETURN]].scaled); diff --git a/src/modules/uORB/topics/manual_control_setpoint.h b/src/modules/uORB/topics/manual_control_setpoint.h index eac6d6e986..190dc01c8e 100644 --- a/src/modules/uORB/topics/manual_control_setpoint.h +++ b/src/modules/uORB/topics/manual_control_setpoint.h @@ -60,6 +60,7 @@ struct manual_control_setpoint_s { 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 */ + float acro_switch; /**< acro 2 position switch (optional): normal, acro */ /** * Any of the channels below may not be available and be set to NaN diff --git a/src/modules/uORB/topics/rc_channels.h b/src/modules/uORB/topics/rc_channels.h index 086b2ef150..beb7008ab9 100644 --- a/src/modules/uORB/topics/rc_channels.h +++ b/src/modules/uORB/topics/rc_channels.h @@ -68,11 +68,12 @@ enum RC_CHANNELS_FUNCTION ASSISTED = 6, MISSION = 7, OFFBOARD_MODE = 8, - FLAPS = 9, - AUX_1 = 10, - AUX_2 = 11, - AUX_3 = 12, - AUX_4 = 13, + ACRO = 9, + FLAPS = 10, + AUX_1 = 11, + AUX_2 = 12, + AUX_3 = 13, + AUX_4 = 14, AUX_5 = 14, RC_CHANNELS_FUNCTION_MAX /**< indicates the number of functions. There can be more functions than RC channels. */ }; diff --git a/src/modules/uORB/topics/vehicle_status.h b/src/modules/uORB/topics/vehicle_status.h index 1b3639e309..5cb0bd8a20 100644 --- a/src/modules/uORB/topics/vehicle_status.h +++ b/src/modules/uORB/topics/vehicle_status.h @@ -63,6 +63,7 @@ /* main state machine */ typedef enum { MAIN_STATE_MANUAL = 0, + MAIN_STATE_ACRO, MAIN_STATE_SEATBELT, MAIN_STATE_EASY, MAIN_STATE_AUTO, @@ -116,6 +117,12 @@ typedef enum { MISSION_SWITCH_MISSION } mission_switch_pos_t; +typedef enum { + ACRO_SWITCH_NONE = 0, + ACRO_SWITCH_NORMAL, + ACRO_SWITCH_ACRO +} acro_switch_pos_t; + enum VEHICLE_MODE_FLAG { VEHICLE_MODE_FLAG_SAFETY_ARMED = 128, VEHICLE_MODE_FLAG_MANUAL_INPUT_ENABLED = 64, @@ -192,6 +199,7 @@ struct vehicle_status_s return_switch_pos_t return_switch; assisted_switch_pos_t assisted_switch; mission_switch_pos_t mission_switch; + acro_switch_pos_t acro_switch; bool condition_battery_voltage_valid; bool condition_system_in_air_restore; /**< true if we can restore in mid air */ From 2923bdf39fd6e424523f0b6b47bef3cabcdc0645 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sat, 1 Feb 2014 11:25:29 +0100 Subject: [PATCH 002/329] commander: allow disarming in ACRO without landing (as in MANUAL) --- src/modules/commander/commander.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 89294fc76c..30f75b9fb1 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1120,7 +1120,7 @@ int commander_thread_main(int argc, char *argv[]) * 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.condition_landed) && + (status.main_state == MAIN_STATE_MANUAL || status.main_state == MAIN_STATE_ACRO || status.condition_landed) && sp_man.yaw < -STICK_ON_OFF_LIMIT && sp_man.throttle < STICK_THRUST_RANGE * 0.1f) { if (stick_off_counter > STICK_ON_OFF_COUNTER_LIMIT) { From 21300874ac6d337d85c49704e9233fdd17192171 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sun, 2 Feb 2014 23:07:48 +0100 Subject: [PATCH 003/329] mc_att_control: reset yaw setpoint after ACRO --- src/modules/mc_att_control/mc_att_control_main.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/modules/mc_att_control/mc_att_control_main.cpp b/src/modules/mc_att_control/mc_att_control_main.cpp index 21e1016625..77531cbb92 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -802,6 +802,8 @@ MulticopterAttitudeControl::task_main() _rates_sp = _rates_sp.edivide(_params.rc_scale).emult(_params.scale_acro); _thrust_sp = _manual_control_sp.throttle; + _reset_yaw_sp = true; + /* publish attitude rates setpoint */ _v_rates_sp.roll = _rates_sp(0); _v_rates_sp.pitch = _rates_sp(1); From 6e7136c6b3f953c26cf75c9f8b777a6a7c84ea9a Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Tue, 4 Feb 2014 21:50:26 +0100 Subject: [PATCH 004/329] rc_channels topic: bug fixed; sensors: minor cleanup --- src/modules/sensors/sensors.cpp | 2 +- src/modules/uORB/topics/rc_channels.h | 34 +++++++++++++-------------- 2 files changed, 18 insertions(+), 18 deletions(-) diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index b37a744ca2..73f14225de 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -674,7 +674,7 @@ Sensors::parameters_update() } if (param_get(_parameter_handles.rc_map_acro_sw, &(_parameters.rc_map_acro_sw)) != OK) { - warnx("Failed getting acro sw chan index"); + warnx(paramerr); } if (param_get(_parameter_handles.rc_map_flaps, &(_parameters.rc_map_flaps)) != OK) { diff --git a/src/modules/uORB/topics/rc_channels.h b/src/modules/uORB/topics/rc_channels.h index beb7008ab9..36106751e2 100644 --- a/src/modules/uORB/topics/rc_channels.h +++ b/src/modules/uORB/topics/rc_channels.h @@ -45,12 +45,12 @@ /** * The number of RC channel inputs supported. * Current (Q4/2013) radios support up to 18 channels, - * leaving at a sane value of 15. + * leaving at a sane value of 16. * 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_MAPPED_MAX 15 +#define RC_CHANNELS_MAPPED_MAX 16 /** * This defines the mapping of the RC functions. @@ -60,21 +60,21 @@ enum RC_CHANNELS_FUNCTION { THROTTLE = 0, - ROLL = 1, - PITCH = 2, - YAW = 3, - MODE = 4, - RETURN = 5, - ASSISTED = 6, - MISSION = 7, - OFFBOARD_MODE = 8, - ACRO = 9, - FLAPS = 10, - AUX_1 = 11, - AUX_2 = 12, - AUX_3 = 13, - AUX_4 = 14, - AUX_5 = 14, + ROLL, + PITCH, + YAW, + MODE, + RETURN, + ASSISTED, + MISSION, + OFFBOARD_MODE, + ACRO, + FLAPS, + AUX_1, + AUX_2, + AUX_3, + AUX_4, + AUX_5, RC_CHANNELS_FUNCTION_MAX /**< indicates the number of functions. There can be more functions than RC channels. */ }; From 3b2b270a40e0d8528339fe7cde5e0af91684fb97 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Tue, 4 Feb 2014 21:55:12 +0100 Subject: [PATCH 005/329] mavlink: custom mode ACRO added --- src/modules/commander/px4_custom_mode.h | 1 + src/modules/mavlink/mavlink.c | 3 +++ 2 files changed, 4 insertions(+) diff --git a/src/modules/commander/px4_custom_mode.h b/src/modules/commander/px4_custom_mode.h index b60a7e0b9c..d69ab30677 100644 --- a/src/modules/commander/px4_custom_mode.h +++ b/src/modules/commander/px4_custom_mode.h @@ -13,6 +13,7 @@ enum PX4_CUSTOM_MAIN_MODE { PX4_CUSTOM_MAIN_MODE_SEATBELT, PX4_CUSTOM_MAIN_MODE_EASY, PX4_CUSTOM_MAIN_MODE_AUTO, + PX4_CUSTOM_MAIN_MODE_ACRO, }; enum PX4_CUSTOM_SUB_MODE_AUTO { diff --git a/src/modules/mavlink/mavlink.c b/src/modules/mavlink/mavlink.c index 4d975066f7..22465a3dae 100644 --- a/src/modules/mavlink/mavlink.c +++ b/src/modules/mavlink/mavlink.c @@ -212,6 +212,9 @@ get_mavlink_mode_and_state(uint8_t *mavlink_state, uint8_t *mavlink_base_mode, u 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); custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_MANUAL; + } else if (v_status.main_state == MAIN_STATE_ACRO) { + *mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED; + custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_ACRO; } else if (v_status.main_state == MAIN_STATE_SEATBELT) { *mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED; custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_SEATBELT; From 9315796020339906d30580892f57abcdc1238b0c Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 12 Feb 2014 07:55:22 +0100 Subject: [PATCH 006/329] Enable S.BUS TX pin --- nuttx-configs/px4io-v2/include/board.h | 2 -- 1 file changed, 2 deletions(-) diff --git a/nuttx-configs/px4io-v2/include/board.h b/nuttx-configs/px4io-v2/include/board.h index 4b9c906385..17e77918b4 100755 --- a/nuttx-configs/px4io-v2/include/board.h +++ b/nuttx-configs/px4io-v2/include/board.h @@ -103,8 +103,6 @@ #define GPIO_USART2_RTS 0xffffffff #undef GPIO_USART2_CK #define GPIO_USART2_CK 0xffffffff -#undef GPIO_USART3_TX -#define GPIO_USART3_TX 0xffffffff #undef GPIO_USART3_CK #define GPIO_USART3_CK 0xffffffff #undef GPIO_USART3_CTS From 500ac69ee46ad582eee5a4321bd53665e17032da Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 12 Feb 2014 08:13:53 +0100 Subject: [PATCH 007/329] Build test code for S.BUS output, send test characters once S.BUS1 or S.BUS2 is enabled --- src/modules/px4iofirmware/mixer.cpp | 14 ++++++++++++++ src/modules/px4iofirmware/px4io.h | 2 ++ src/modules/px4iofirmware/sbus.c | 14 +++++++++++++- 3 files changed, 29 insertions(+), 1 deletion(-) diff --git a/src/modules/px4iofirmware/mixer.cpp b/src/modules/px4iofirmware/mixer.cpp index f39fcf7ec6..3eaecc38b9 100644 --- a/src/modules/px4iofirmware/mixer.cpp +++ b/src/modules/px4iofirmware/mixer.cpp @@ -254,10 +254,24 @@ mixer_tick(void) for (unsigned i = 0; i < PX4IO_SERVO_COUNT; i++) up_pwm_servo_set(i, r_page_servos[i]); + /* set S.BUS1 or S.BUS2 outputs */ + if (r_setup_features & PX4IO_P_SETUP_FEATURES_SBUS1_OUT) + sbus1_output(r_page_servos, PX4IO_SERVO_COUNT); + + if (r_setup_features & PX4IO_P_SETUP_FEATURES_SBUS2_OUT) + sbus2_output(r_page_servos, PX4IO_SERVO_COUNT); + } else if (mixer_servos_armed && should_always_enable_pwm) { /* set the disarmed servo outputs. */ for (unsigned i = 0; i < PX4IO_SERVO_COUNT; i++) up_pwm_servo_set(i, r_page_servo_disarmed[i]); + + /* set S.BUS1 or S.BUS2 outputs */ + if (r_setup_features & PX4IO_P_SETUP_FEATURES_SBUS1_OUT) + sbus1_output(r_page_servos, PX4IO_SERVO_COUNT); + + if (r_setup_features & PX4IO_P_SETUP_FEATURES_SBUS2_OUT) + sbus2_output(r_page_servos, PX4IO_SERVO_COUNT); } } diff --git a/src/modules/px4iofirmware/px4io.h b/src/modules/px4iofirmware/px4io.h index bb224f3880..0e9fee8aeb 100644 --- a/src/modules/px4iofirmware/px4io.h +++ b/src/modules/px4iofirmware/px4io.h @@ -218,6 +218,8 @@ extern bool dsm_input(uint16_t *values, uint16_t *num_values); extern void dsm_bind(uint16_t cmd, int pulses); extern int sbus_init(const char *device); extern bool sbus_input(uint16_t *values, uint16_t *num_values, bool *sbus_failsafe, bool *sbus_frame_drop, uint16_t max_channels); +extern bool sbus1_output(uint16_t *values, uint16_t num_values); +extern bool sbus2_output(uint16_t *values, uint16_t num_values); /** global debug level for isr_debug() */ extern volatile uint8_t debug_level; diff --git a/src/modules/px4iofirmware/sbus.c b/src/modules/px4iofirmware/sbus.c index f6ec542eb0..86240d36ab 100644 --- a/src/modules/px4iofirmware/sbus.c +++ b/src/modules/px4iofirmware/sbus.c @@ -93,7 +93,7 @@ int sbus_init(const char *device) { if (sbus_fd < 0) - sbus_fd = open(device, O_RDONLY | O_NONBLOCK); + sbus_fd = open(device, O_RDWR | O_NONBLOCK); if (sbus_fd >= 0) { struct termios t; @@ -117,6 +117,18 @@ sbus_init(const char *device) return sbus_fd; } +bool +sbus1_output(uint16_t *values, uint16_t num_values) +{ + write(sbus_fd, 'A', 1); +} + +bool +sbus2_output(uint16_t *values, uint16_t num_values) +{ + write(sbus_fd, 'B', 1); +} + bool sbus_input(uint16_t *values, uint16_t *num_values, bool *sbus_failsafe, bool *sbus_frame_drop, uint16_t max_channels) { From 85ec7fb40aab728ba477ffd75f48c2c731fb56fa Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 12 Feb 2014 08:47:01 +0100 Subject: [PATCH 008/329] test loop --- src/modules/px4iofirmware/sbus.c | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/src/modules/px4iofirmware/sbus.c b/src/modules/px4iofirmware/sbus.c index 86240d36ab..39230d274c 100644 --- a/src/modules/px4iofirmware/sbus.c +++ b/src/modules/px4iofirmware/sbus.c @@ -114,6 +114,14 @@ sbus_init(const char *device) debug("S.Bus: open failed"); } + ENABLE_SBUS_OUT(true); + + while (1) { + const char* hello = 'HELLO WORLD'; + if (write(sbus_fd, hello, strlen(hello)) != strlen(hello)) + break; + } + return sbus_fd; } From 3bcf34098a9fd07c0693e918396d2e3fde0fa1e8 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 12 Feb 2014 08:50:44 +0100 Subject: [PATCH 009/329] Fix quotation marks --- src/modules/px4iofirmware/sbus.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/px4iofirmware/sbus.c b/src/modules/px4iofirmware/sbus.c index 39230d274c..6608392d04 100644 --- a/src/modules/px4iofirmware/sbus.c +++ b/src/modules/px4iofirmware/sbus.c @@ -117,7 +117,7 @@ sbus_init(const char *device) ENABLE_SBUS_OUT(true); while (1) { - const char* hello = 'HELLO WORLD'; + const char* hello = "HELLO WORLD"; if (write(sbus_fd, hello, strlen(hello)) != strlen(hello)) break; } From bc3f95fc07fb01edecfa9147023a354f1f237e92 Mon Sep 17 00:00:00 2001 From: px4dev Date: Wed, 12 Feb 2014 00:48:15 -0800 Subject: [PATCH 010/329] Turn off JTAG completely in a vain attempt to get PB4 free for SBUS enable. --- nuttx-configs/px4io-v2/nsh/defconfig | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/nuttx-configs/px4io-v2/nsh/defconfig b/nuttx-configs/px4io-v2/nsh/defconfig index 4111e70eba..2c63b88190 100755 --- a/nuttx-configs/px4io-v2/nsh/defconfig +++ b/nuttx-configs/px4io-v2/nsh/defconfig @@ -104,7 +104,7 @@ CONFIG_ARMV7M_CMNVECTOR=y # CONFIG_STM32_JTAG_SW_ENABLE - Set JTAG-DP disabled and SW-DP enabled # CONFIG_STM32_DFU=n -CONFIG_STM32_JTAG_FULL_ENABLE=y +CONFIG_STM32_JTAG_FULL_ENABLE=n CONFIG_STM32_JTAG_NOJNTRST_ENABLE=n CONFIG_STM32_JTAG_SW_ENABLE=n From ca2ad0051d2c5a31aa6050cc88f5c7d6c2997036 Mon Sep 17 00:00:00 2001 From: px4dev Date: Wed, 12 Feb 2014 00:48:53 -0800 Subject: [PATCH 011/329] Be more enthusiastic with the sbus enable pin. Still no love. --- src/modules/px4iofirmware/sbus.c | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/src/modules/px4iofirmware/sbus.c b/src/modules/px4iofirmware/sbus.c index 6608392d04..32be93d4c9 100644 --- a/src/modules/px4iofirmware/sbus.c +++ b/src/modules/px4iofirmware/sbus.c @@ -114,12 +114,14 @@ sbus_init(const char *device) debug("S.Bus: open failed"); } - ENABLE_SBUS_OUT(true); + stm32_configgpio(GPIO_SBUS_OENABLE); while (1) { + ENABLE_SBUS_OUT(true); const char* hello = "HELLO WORLD"; if (write(sbus_fd, hello, strlen(hello)) != strlen(hello)) break; + ENABLE_SBUS_OUT(false); } return sbus_fd; From cec6b8925e727710884042f9b45c105aa8c4d5af Mon Sep 17 00:00:00 2001 From: px4dev Date: Wed, 12 Feb 2014 22:10:45 -0800 Subject: [PATCH 012/329] Don't leave all JTAG off... it will make you very sad --- nuttx-configs/px4io-v2/nsh/defconfig | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/nuttx-configs/px4io-v2/nsh/defconfig b/nuttx-configs/px4io-v2/nsh/defconfig index 2c63b88190..e6563e5878 100755 --- a/nuttx-configs/px4io-v2/nsh/defconfig +++ b/nuttx-configs/px4io-v2/nsh/defconfig @@ -106,7 +106,7 @@ CONFIG_ARMV7M_CMNVECTOR=y CONFIG_STM32_DFU=n CONFIG_STM32_JTAG_FULL_ENABLE=n CONFIG_STM32_JTAG_NOJNTRST_ENABLE=n -CONFIG_STM32_JTAG_SW_ENABLE=n +CONFIG_STM32_JTAG_SW_ENABLE=y # # Individual subsystems can be enabled: From dd432e66032c3cb1cb6f65536c28af1dd9f97317 Mon Sep 17 00:00:00 2001 From: px4dev Date: Wed, 12 Feb 2014 22:11:09 -0800 Subject: [PATCH 013/329] Remove the s.bus test loop... makes it very hard to update the firmware. --- src/modules/px4iofirmware/sbus.c | 11 ----------- 1 file changed, 11 deletions(-) diff --git a/src/modules/px4iofirmware/sbus.c b/src/modules/px4iofirmware/sbus.c index 32be93d4c9..0e7dc621cf 100644 --- a/src/modules/px4iofirmware/sbus.c +++ b/src/modules/px4iofirmware/sbus.c @@ -113,17 +113,6 @@ sbus_init(const char *device) } else { debug("S.Bus: open failed"); } - - stm32_configgpio(GPIO_SBUS_OENABLE); - - while (1) { - ENABLE_SBUS_OUT(true); - const char* hello = "HELLO WORLD"; - if (write(sbus_fd, hello, strlen(hello)) != strlen(hello)) - break; - ENABLE_SBUS_OUT(false); - } - return sbus_fd; } From 6a1f91e6254e14c52b77406b12b76e2a233aedf8 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 13 Feb 2014 08:22:05 +0100 Subject: [PATCH 014/329] Make SBUS output exclusive --- src/modules/px4iofirmware/mixer.cpp | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/src/modules/px4iofirmware/mixer.cpp b/src/modules/px4iofirmware/mixer.cpp index 3eaecc38b9..b175c3bc8c 100644 --- a/src/modules/px4iofirmware/mixer.cpp +++ b/src/modules/px4iofirmware/mixer.cpp @@ -255,11 +255,12 @@ mixer_tick(void) up_pwm_servo_set(i, r_page_servos[i]); /* set S.BUS1 or S.BUS2 outputs */ - if (r_setup_features & PX4IO_P_SETUP_FEATURES_SBUS1_OUT) - sbus1_output(r_page_servos, PX4IO_SERVO_COUNT); - if (r_setup_features & PX4IO_P_SETUP_FEATURES_SBUS2_OUT) + if (r_setup_features & PX4IO_P_SETUP_FEATURES_SBUS2_OUT) { sbus2_output(r_page_servos, PX4IO_SERVO_COUNT); + } else if (r_setup_features & PX4IO_P_SETUP_FEATURES_SBUS1_OUT) { + sbus1_output(r_page_servos, PX4IO_SERVO_COUNT); + } } else if (mixer_servos_armed && should_always_enable_pwm) { /* set the disarmed servo outputs. */ From 91c55503a860ffc02a2687c141e2cfc68a43b3cc Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 13 Feb 2014 08:25:49 +0100 Subject: [PATCH 015/329] Ensure only either S.BUS1 or S.BUS2 can be active at a time --- src/modules/px4iofirmware/registers.c | 15 ++++++++++++--- 1 file changed, 12 insertions(+), 3 deletions(-) diff --git a/src/modules/px4iofirmware/registers.c b/src/modules/px4iofirmware/registers.c index 1335f52e13..f780868398 100644 --- a/src/modules/px4iofirmware/registers.c +++ b/src/modules/px4iofirmware/registers.c @@ -462,9 +462,18 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value) #ifdef ENABLE_SBUS_OUT ENABLE_SBUS_OUT(value & (PX4IO_P_SETUP_FEATURES_SBUS1_OUT | PX4IO_P_SETUP_FEATURES_SBUS2_OUT)); - /* disable the conflicting options */ - if (value & (PX4IO_P_SETUP_FEATURES_SBUS1_OUT | PX4IO_P_SETUP_FEATURES_SBUS2_OUT)) { - value &= ~(PX4IO_P_SETUP_FEATURES_PWM_RSSI | PX4IO_P_SETUP_FEATURES_ADC_RSSI); + /* disable the conflicting options with SBUS 1 */ + if (value & (PX4IO_P_SETUP_FEATURES_SBUS1_OUT)) { + value &= ~(PX4IO_P_SETUP_FEATURES_PWM_RSSI | + PX4IO_P_SETUP_FEATURES_ADC_RSSI | + PX4IO_P_SETUP_FEATURES_SBUS2_OUT); + } + + /* disable the conflicting options with SBUS 2 */ + if (value & (PX4IO_P_SETUP_FEATURES_SBUS2_OUT)) { + value &= ~(PX4IO_P_SETUP_FEATURES_PWM_RSSI | + PX4IO_P_SETUP_FEATURES_ADC_RSSI | + PX4IO_P_SETUP_FEATURES_SBUS1_OUT); } #endif From 88fe2d3052eccd542ffb5d3473d720b66b8679fd Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Mon, 24 Feb 2014 19:04:26 +0100 Subject: [PATCH 016/329] mission feasibility checker: add check for waypoint altitude relative to home position altitude --- .../navigator/mission_feasibility_checker.cpp | 43 +++++++++++++++---- .../navigator/mission_feasibility_checker.h | 7 +-- src/modules/navigator/navigator_main.cpp | 2 +- 3 files changed, 40 insertions(+), 12 deletions(-) diff --git a/src/modules/navigator/mission_feasibility_checker.cpp b/src/modules/navigator/mission_feasibility_checker.cpp index eaafa217de..41670e2474 100644 --- a/src/modules/navigator/mission_feasibility_checker.cpp +++ b/src/modules/navigator/mission_feasibility_checker.cpp @@ -62,7 +62,7 @@ MissionFeasibilityChecker::MissionFeasibilityChecker() : _mavlink_fd(-1), _capab } -bool MissionFeasibilityChecker::checkMissionFeasible(bool isRotarywing, dm_item_t dm_current, size_t nMissionItems, Geofence &geofence) +bool MissionFeasibilityChecker::checkMissionFeasible(bool isRotarywing, dm_item_t dm_current, size_t nMissionItems, Geofence &geofence, float home_alt) { /* Init if not done yet */ init(); @@ -75,24 +75,24 @@ bool MissionFeasibilityChecker::checkMissionFeasible(bool isRotarywing, dm_item_ if (isRotarywing) - return checkMissionFeasibleRotarywing(dm_current, nMissionItems, geofence); + return checkMissionFeasibleRotarywing(dm_current, nMissionItems, geofence, home_alt); else - return checkMissionFeasibleFixedwing(dm_current, nMissionItems, geofence); + return checkMissionFeasibleFixedwing(dm_current, nMissionItems, geofence, home_alt); } -bool MissionFeasibilityChecker::checkMissionFeasibleRotarywing(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence) +bool MissionFeasibilityChecker::checkMissionFeasibleRotarywing(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence, float home_alt) { - return checkGeofence(dm_current, nMissionItems, geofence); + return (checkGeofence(dm_current, nMissionItems, geofence) && checkHomePositionAltitude(dm_current, nMissionItems, home_alt)); } -bool MissionFeasibilityChecker::checkMissionFeasibleFixedwing(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence) +bool MissionFeasibilityChecker::checkMissionFeasibleFixedwing(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence, float home_alt) { /* Update fixed wing navigation capabilites */ updateNavigationCapabilities(); // warnx("_nav_caps.landing_slope_angle_rad %.4f, _nav_caps.landing_horizontal_slope_displacement %.4f", _nav_caps.landing_slope_angle_rad, _nav_caps.landing_horizontal_slope_displacement); - return (checkFixedWingLanding(dm_current, nMissionItems) && checkGeofence(dm_current, nMissionItems, geofence)); + return (checkFixedWingLanding(dm_current, nMissionItems) && checkGeofence(dm_current, nMissionItems, geofence) && checkHomePositionAltitude(dm_current, nMissionItems, home_alt)); } bool MissionFeasibilityChecker::checkGeofence(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence) @@ -108,7 +108,7 @@ bool MissionFeasibilityChecker::checkGeofence(dm_item_t dm_current, size_t nMiss return false; } - if (!geofence.inside(missionitem.lat, missionitem.lon, missionitem.altitude)) { //xxx: handle relative altitude + if (!geofence.inside(missionitem.lat, missionitem.lon, missionitem.altitude)) { mavlink_log_info(_mavlink_fd, "#audio: Geofence violation waypoint %d", i); return false; } @@ -118,6 +118,33 @@ bool MissionFeasibilityChecker::checkGeofence(dm_item_t dm_current, size_t nMiss return true; } +bool MissionFeasibilityChecker::checkHomePositionAltitude(dm_item_t dm_current, size_t nMissionItems, float home_alt, bool throw_error) +{ + /* Check if all all waypoints are above the home altitude, only return false if bool throw_error = true */ + for (size_t i = 0; i < nMissionItems; i++) { + static struct mission_item_s missionitem; + const ssize_t len = sizeof(struct mission_item_s); + + if (dm_read(dm_current, i, &missionitem, len) != len) { + /* not supposed to happen unless the datamanager can't access the SD card, etc. */ + if (throw_error) { + return false; + } else { + return true; + } + } + + if (home_alt > missionitem.altitude) { + mavlink_log_info(_mavlink_fd, "#audio: warning waypoint %d below home", i); + if (throw_error) { + return false; + } else { + return true; + } + } + } +} + bool MissionFeasibilityChecker::checkFixedWingLanding(dm_item_t dm_current, size_t nMissionItems) { /* Go through all mission items and search for a landing waypoint diff --git a/src/modules/navigator/mission_feasibility_checker.h b/src/modules/navigator/mission_feasibility_checker.h index 7a0b2a2966..819dcf9c3a 100644 --- a/src/modules/navigator/mission_feasibility_checker.h +++ b/src/modules/navigator/mission_feasibility_checker.h @@ -59,14 +59,15 @@ private: /* Checks for all airframes */ bool checkGeofence(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence); + bool checkHomePositionAltitude(dm_item_t dm_current, size_t nMissionItems, float home_alt, bool throw_error = false); /* Checks specific to fixedwing airframes */ - bool checkMissionFeasibleFixedwing(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence); + bool checkMissionFeasibleFixedwing(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence, float home_alt); bool checkFixedWingLanding(dm_item_t dm_current, size_t nMissionItems); void updateNavigationCapabilities(); /* Checks specific to rotarywing airframes */ - bool checkMissionFeasibleRotarywing(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence); + bool checkMissionFeasibleRotarywing(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence, float home_alt); public: MissionFeasibilityChecker(); @@ -75,7 +76,7 @@ public: /* * Returns true if mission is feasible and false otherwise */ - bool checkMissionFeasible(bool isRotarywing, dm_item_t dm_current, size_t nMissionItems, Geofence &geofence); + bool checkMissionFeasible(bool isRotarywing, dm_item_t dm_current, size_t nMissionItems, Geofence &geofence, float home_alt); }; diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index c572972b58..11181ff642 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -521,7 +521,7 @@ Navigator::offboard_mission_update(bool isrotaryWing) dm_current = DM_KEY_WAYPOINTS_OFFBOARD_1; } - missionFeasiblityChecker.checkMissionFeasible(isrotaryWing, dm_current, (size_t)offboard_mission.count, _geofence); + missionFeasiblityChecker.checkMissionFeasible(isrotaryWing, dm_current, (size_t)offboard_mission.count, _geofence, _home_pos.alt); _mission.set_offboard_dataman_id(offboard_mission.dataman_id); _mission.set_current_offboard_mission_index(offboard_mission.current_index); From 053ad5b6388ef653d1dfe255ea4f3eb00aeccaba Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Mon, 24 Feb 2014 22:19:21 +0100 Subject: [PATCH 017/329] mission feasibility checker: remove audio warning for waypoint below home altitude --- src/modules/navigator/mission_feasibility_checker.cpp | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/modules/navigator/mission_feasibility_checker.cpp b/src/modules/navigator/mission_feasibility_checker.cpp index 41670e2474..b41e9d355d 100644 --- a/src/modules/navigator/mission_feasibility_checker.cpp +++ b/src/modules/navigator/mission_feasibility_checker.cpp @@ -135,10 +135,11 @@ bool MissionFeasibilityChecker::checkHomePositionAltitude(dm_item_t dm_current, } if (home_alt > missionitem.altitude) { - mavlink_log_info(_mavlink_fd, "#audio: warning waypoint %d below home", i); if (throw_error) { + mavlink_log_info(_mavlink_fd, "Waypoint %d below home", i); return false; } else { + mavlink_log_info(_mavlink_fd, "#audio: warning waypoint %d below home", i); return true; } } From c6e2ad918b50f01ec9e26ccd7fdb88c7c0d2a60c Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Tue, 25 Feb 2014 00:34:51 +0100 Subject: [PATCH 018/329] mission feasibility checker: add missing default return in checkHomePositionAltitude --- src/modules/navigator/mission_feasibility_checker.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/modules/navigator/mission_feasibility_checker.cpp b/src/modules/navigator/mission_feasibility_checker.cpp index b41e9d355d..02e35f1a8b 100644 --- a/src/modules/navigator/mission_feasibility_checker.cpp +++ b/src/modules/navigator/mission_feasibility_checker.cpp @@ -144,6 +144,8 @@ bool MissionFeasibilityChecker::checkHomePositionAltitude(dm_item_t dm_current, } } } + + return true; } bool MissionFeasibilityChecker::checkFixedWingLanding(dm_item_t dm_current, size_t nMissionItems) From 40394c6744765d9383b5c2aa6d32162b32bda567 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Tue, 11 Mar 2014 10:40:08 +0100 Subject: [PATCH 019/329] navigator: RTL: set loiter wp instead of normal waypoint in RTL_STATE_DESCEND --- src/modules/navigator/navigator_main.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 16eca8d791..6b6f3f3ff3 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -1375,7 +1375,7 @@ Navigator::set_rtl_item() _mission_item.yaw = NAN; _mission_item.loiter_radius = _parameters.loiter_radius; _mission_item.loiter_direction = 1; - _mission_item.nav_cmd = NAV_CMD_WAYPOINT; + _mission_item.nav_cmd = NAV_CMD_LOITER_TIME_LIMIT; _mission_item.acceptance_radius = _parameters.acceptance_radius; _mission_item.time_inside = _parameters.rtl_land_delay < 0.0f ? 0.0f : _parameters.rtl_land_delay; _mission_item.pitch_min = 0.0f; From e8efbc7f3fe22d2a11189fd83c7252347a6d7f04 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Tue, 11 Mar 2014 10:37:15 +0100 Subject: [PATCH 020/329] navigator: RTL: set normal waypoint instead of takeoff waypoint in RTL_STATE_CLIMB --- src/modules/navigator/navigator_main.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 6b6f3f3ff3..2f058b6c92 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -1315,7 +1315,7 @@ Navigator::set_rtl_item() _mission_item.yaw = NAN; _mission_item.loiter_radius = _parameters.loiter_radius; _mission_item.loiter_direction = 1; - _mission_item.nav_cmd = NAV_CMD_TAKEOFF; + _mission_item.nav_cmd = NAV_CMD_WAYPOINT; _mission_item.acceptance_radius = _parameters.acceptance_radius; _mission_item.time_inside = 0.0f; _mission_item.pitch_min = 0.0f; From 3317259e798b31ff407e07188f3080f6dbccf478 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sat, 15 Mar 2014 11:56:04 +0100 Subject: [PATCH 021/329] integrate range finder into fw landing --- .../fw_pos_control_l1_main.cpp | 90 +++++++++++++++---- .../fw_pos_control_l1_params.c | 10 +++ .../fw_pos_control_l1/landingslope.cpp | 34 +++++-- src/modules/fw_pos_control_l1/landingslope.h | 31 ++++++- 4 files changed, 138 insertions(+), 27 deletions(-) diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index 7f13df7854..32e00659bc 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -89,6 +89,7 @@ #include #include #include +#include #include "landingslope.h" @@ -134,6 +135,7 @@ private: int _params_sub; /**< notification of parameter updates */ int _manual_control_sub; /**< notification of manual control updates */ int _sensor_combined_sub; /**< for body frame accelerations */ + int _range_finder_sub; /**< range finder subscription */ orb_advert_t _attitude_sp_pub; /**< attitude setpoint */ orb_advert_t _nav_capabilities_pub; /**< navigation capabilities publication */ @@ -147,6 +149,7 @@ private: struct vehicle_global_position_s _global_pos; /**< global vehicle position */ struct position_setpoint_triplet_s _pos_sp_triplet; /**< triplet of mission items */ struct sensor_combined_s _sensor_combined; /**< for body frame accelerations */ + struct range_finder_report _range_finder; /**< range finder report */ perf_counter_t _loop_perf; /**< loop performance counter */ @@ -181,7 +184,7 @@ private: /* Landingslope object */ Landingslope landingslope; - float flare_curve_alt_last; + float flare_curve_alt_rel_last; /* heading hold */ float target_bearing; @@ -239,6 +242,7 @@ private: float land_flare_alt_relative; float land_thrust_lim_alt_relative; float land_heading_hold_horizontal_distance; + float range_finder_rel_alt; } _parameters; /**< local copies of interesting parameters */ @@ -283,6 +287,7 @@ private: param_t land_flare_alt_relative; param_t land_thrust_lim_alt_relative; param_t land_heading_hold_horizontal_distance; + param_t range_finder_rel_alt; } _parameter_handles; /**< handles for interesting parameters */ @@ -308,6 +313,12 @@ private: */ bool vehicle_airspeed_poll(); + /** + * Check for range finder updates. + */ + bool range_finder_poll(); + + /** * Check for position updates. */ @@ -328,6 +339,11 @@ private: */ void navigation_capabilities_publish(); + /** + * Get the relative alt either from the difference between estimate and waypoint or from the laser range finder + */ + float get_relative_landingalt(float land_setpoint_alt, float current_alt, const struct range_finder_report &range_finder, float range_finder_use_relative_alt); + /** * Control position. */ @@ -384,6 +400,7 @@ FixedwingPositionControl::FixedwingPositionControl() : _control_mode_sub(-1), _params_sub(-1), _manual_control_sub(-1), + _range_finder_sub(-1), /* publications */ _attitude_sp_pub(-1), @@ -403,7 +420,7 @@ FixedwingPositionControl::FixedwingPositionControl() : launch_detected(false), last_manual(false), usePreTakeoffThrust(false), - flare_curve_alt_last(0.0f), + flare_curve_alt_rel_last(0.0f), launchDetector(), _airspeed_error(0.0f), _airspeed_valid(false), @@ -417,7 +434,8 @@ FixedwingPositionControl::FixedwingPositionControl() : _control_mode(), _global_pos(), _pos_sp_triplet(), - _sensor_combined() + _sensor_combined(), + _range_finder() { _nav_capabilities.turn_distance = 0.0f; @@ -442,6 +460,7 @@ FixedwingPositionControl::FixedwingPositionControl() : _parameter_handles.land_flare_alt_relative = param_find("FW_LND_FLALT"); _parameter_handles.land_thrust_lim_alt_relative = param_find("FW_LND_TLALT"); _parameter_handles.land_heading_hold_horizontal_distance = param_find("FW_LND_HHDIST"); + _parameter_handles.range_finder_rel_alt = param_find("FW_LND_RFRALT"); _parameter_handles.time_const = param_find("FW_T_TIME_CONST"); _parameter_handles.min_sink_rate = param_find("FW_T_SINK_MIN"); @@ -531,6 +550,8 @@ FixedwingPositionControl::parameters_update() param_get(_parameter_handles.land_thrust_lim_alt_relative, &(_parameters.land_thrust_lim_alt_relative)); param_get(_parameter_handles.land_heading_hold_horizontal_distance, &(_parameters.land_heading_hold_horizontal_distance)); + param_get(_parameter_handles.range_finder_rel_alt, &(_parameters.range_finder_rel_alt)); + _l1_control.set_l1_damping(_parameters.l1_damping); _l1_control.set_l1_period(_parameters.l1_period); _l1_control.set_l1_roll_limit(math::radians(_parameters.roll_limit)); @@ -626,6 +647,20 @@ FixedwingPositionControl::vehicle_airspeed_poll() return airspeed_updated; } +bool +FixedwingPositionControl::range_finder_poll() +{ + /* check if there is a range finder measurement */ + bool range_finder_updated; + orb_check(_range_finder_sub, &range_finder_updated); + + if (range_finder_updated) { + orb_copy(ORB_ID(sensor_range_finder), _range_finder_sub, &_range_finder); + } + + return range_finder_updated; +} + void FixedwingPositionControl::vehicle_attitude_poll() { @@ -754,6 +789,23 @@ void FixedwingPositionControl::navigation_capabilities_publish() } } +float FixedwingPositionControl::get_relative_landingalt(float land_setpoint_alt, float current_alt, const struct range_finder_report &range_finder, float range_finder_use_relative_alt) +{ + float rel_alt_estimated = current_alt - land_setpoint_alt; + + /* only use range finder if: + * parameter (range_finder_use_relative_alt) > 0 + * the measurement is valid + * the estimated relative altitude (from global altitude estimate and landing waypoint) <= range_finder_use_relative_alt + */ + if (range_finder_use_relative_alt < 0 || !range_finder.valid || rel_alt_estimated > range_finder_use_relative_alt ) { + return rel_alt_estimated; + } + + return range_finder.distance; + +} + bool FixedwingPositionControl::control_position(const math::Vector<2> ¤t_position, const math::Vector<2> &ground_speed, const struct position_setpoint_triplet_s &pos_sp_triplet) @@ -896,12 +948,14 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi /* Calculate distance (to landing waypoint) and altitude of last ordinary waypoint L */ float L_wp_distance = get_distance_to_next_waypoint(prev_wp(0), prev_wp(1), curr_wp(0), curr_wp(1)); - float L_altitude = landingslope.getLandingSlopeAbsoluteAltitude(L_wp_distance, _pos_sp_triplet.current.alt); + float L_altitude_rel = landingslope.getLandingSlopeRelativeAltitude(L_wp_distance); float bearing_airplane_currwp = get_bearing_to_next_waypoint(current_position(0), current_position(1), curr_wp(0), curr_wp(1)); - float landing_slope_alt_desired = landingslope.getLandingSlopeAbsoluteAltitudeSave(wp_distance, bearing_lastwp_currwp, bearing_airplane_currwp, _pos_sp_triplet.current.alt); + float landing_slope_alt_rel_desired = landingslope.getLandingSlopeRelativeAltitudeSave(wp_distance, bearing_lastwp_currwp, bearing_airplane_currwp); - if ( (_global_pos.alt < _pos_sp_triplet.current.alt + landingslope.flare_relative_alt()) || land_noreturn_vertical) { //checking for land_noreturn to avoid unwanted climb out + float relative_alt = get_relative_landingalt(_pos_sp_triplet.current.alt, _global_pos.alt, _range_finder, _parameters.range_finder_rel_alt); + + if ( (relative_alt < landingslope.flare_relative_alt()) || land_noreturn_vertical) { //checking for land_noreturn to avoid unwanted climb out /* land with minimal speed */ @@ -911,7 +965,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi /* kill the throttle if param requests it */ throttle_max = _parameters.throttle_max; - if (_global_pos.alt < _pos_sp_triplet.current.alt + landingslope.motor_lim_relative_alt() || land_motor_lim) { + if (relative_alt < landingslope.motor_lim_relative_alt() || land_motor_lim) { throttle_max = math::min(throttle_max, _parameters.throttle_land_max); if (!land_motor_lim) { land_motor_lim = true; @@ -920,16 +974,16 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi } - float flare_curve_alt = landingslope.getFlareCurveAltitudeSave(wp_distance, bearing_lastwp_currwp, bearing_airplane_currwp, _pos_sp_triplet.current.alt); + float flare_curve_alt_rel = landingslope.getFlareCurveRelativeAltitudeSave(wp_distance, bearing_lastwp_currwp, bearing_airplane_currwp); /* avoid climbout */ - if ((flare_curve_alt_last < flare_curve_alt && land_noreturn_vertical) || land_stayonground) + if ((flare_curve_alt_rel_last < flare_curve_alt_rel && land_noreturn_vertical) || land_stayonground) { - flare_curve_alt = pos_sp_triplet.current.alt; + flare_curve_alt_rel = 0.0f; // stay on ground land_stayonground = true; } - _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, flare_curve_alt, calculate_target_airspeed(airspeed_land), + _tecs.update_pitch_throttle(_R_nb, _att.pitch, relative_alt, flare_curve_alt_rel, calculate_target_airspeed(airspeed_land), _airspeed.indicated_airspeed_m_s, eas2tas, false, flare_pitch_angle_rad, 0.0f, throttle_max, throttle_land, @@ -941,7 +995,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi } //warnx("Landing: flare, _global_pos.alt %.1f, flare_curve_alt %.1f, flare_curve_alt_last %.1f, flare_length %.1f, wp_distance %.1f", _global_pos.alt, flare_curve_alt, flare_curve_alt_last, flare_length, wp_distance); - flare_curve_alt_last = flare_curve_alt; + flare_curve_alt_rel_last = flare_curve_alt_rel; } else { /* intersect glide slope: @@ -949,20 +1003,20 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi * if current position is higher or within 10m of slope follow the glide slope * if current position is below slope -10m continue on maximum of previous wp altitude or L_altitude until the intersection with the slope * */ - float altitude_desired = _global_pos.alt; - if (_global_pos.alt > landing_slope_alt_desired - 10.0f) { + float altitude_desired_rel = relative_alt; + if (relative_alt > landing_slope_alt_rel_desired - 10.0f) { /* stay on slope */ - altitude_desired = landing_slope_alt_desired; + altitude_desired_rel = landing_slope_alt_rel_desired; if (!land_onslope) { mavlink_log_info(_mavlink_fd, "#audio: Landing, on slope"); land_onslope = true; } } else { /* continue horizontally */ - altitude_desired = math::max(_global_pos.alt, L_altitude); + altitude_desired_rel = math::max(relative_alt, L_altitude_rel); } - _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, altitude_desired, calculate_target_airspeed(airspeed_approach), + _tecs.update_pitch_throttle(_R_nb, _att.pitch, relative_alt, altitude_desired_rel, calculate_target_airspeed(airspeed_approach), _airspeed.indicated_airspeed_m_s, eas2tas, false, math::radians(_parameters.pitch_limit_min), _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, @@ -1185,6 +1239,7 @@ FixedwingPositionControl::task_main() _airspeed_sub = orb_subscribe(ORB_ID(airspeed)); _params_sub = orb_subscribe(ORB_ID(parameter_update)); _manual_control_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); + _range_finder_sub = orb_subscribe(ORB_ID(sensor_range_finder)); /* rate limit vehicle status updates to 5Hz */ orb_set_interval(_control_mode_sub, 200); @@ -1264,6 +1319,7 @@ FixedwingPositionControl::task_main() vehicle_setpoint_poll(); vehicle_sensor_combined_poll(); vehicle_airspeed_poll(); + range_finder_poll(); // vehicle_baro_poll(); math::Vector<2> ground_speed(_global_pos.vel_n, _global_pos.vel_e); diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c b/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c index 0909135e15..8ec0f0027d 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c @@ -375,3 +375,13 @@ PARAM_DEFINE_FLOAT(FW_LND_TLALT, 5.0f); * @group L1 Control */ PARAM_DEFINE_FLOAT(FW_LND_HHDIST, 15.0f); + +/** + * Landing relative altitude threshold for range finder measurements + * + * range finder measurements will only be used below FW_LND_RFRALT estimated relative altitude (gobal_pos.alt - landing_waypoint.alt) + * set to -1 to disable + * + * @group L1 Control + */ +PARAM_DEFINE_FLOAT(FW_LND_RFRALT, 30.0f); diff --git a/src/modules/fw_pos_control_l1/landingslope.cpp b/src/modules/fw_pos_control_l1/landingslope.cpp index e5f7023ae1..8ce465fe8c 100644 --- a/src/modules/fw_pos_control_l1/landingslope.cpp +++ b/src/modules/fw_pos_control_l1/landingslope.cpp @@ -69,26 +69,46 @@ void Landingslope::calculateSlopeValues() _horizontal_slope_displacement = (_flare_length - _d1); } -float Landingslope::getLandingSlopeAbsoluteAltitude(float wp_distance, float wp_altitude) +float Landingslope::getLandingSlopeRelativeAltitude(float wp_landing_distance) { - return Landingslope::getLandingSlopeAbsoluteAltitude(wp_distance, wp_altitude, _horizontal_slope_displacement, _landing_slope_angle_rad); + return Landingslope::getLandingSlopeRelativeAltitude(wp_landing_distance, _horizontal_slope_displacement, _landing_slope_angle_rad); } -float Landingslope::getLandingSlopeAbsoluteAltitudeSave(float wp_distance, float bearing_lastwp_currwp, float bearing_airplane_currwp, float wp_altitude) +float Landingslope::getLandingSlopeRelativeAltitudeSave(float wp_landing_distance, float bearing_lastwp_currwp, float bearing_airplane_currwp) { /* If airplane is in front of waypoint return slope altitude, else return waypoint altitude */ if (fabsf(bearing_airplane_currwp - bearing_lastwp_currwp) < math::radians(90.0f)) - return getLandingSlopeAbsoluteAltitude(wp_distance, wp_altitude); + return getLandingSlopeRelativeAltitude(wp_landing_distance); + else + return 0.0f; +} + +float Landingslope::getLandingSlopeAbsoluteAltitude(float wp_landing_distance, float wp_altitude) +{ + return Landingslope::getLandingSlopeAbsoluteAltitude(wp_landing_distance, wp_altitude, _horizontal_slope_displacement, _landing_slope_angle_rad); +} + +float Landingslope::getLandingSlopeAbsoluteAltitudeSave(float wp_landing_distance, float bearing_lastwp_currwp, float bearing_airplane_currwp, float wp_altitude) +{ + /* If airplane is in front of waypoint return slope altitude, else return waypoint altitude */ + if (fabsf(bearing_airplane_currwp - bearing_lastwp_currwp) < math::radians(90.0f)) + return getLandingSlopeAbsoluteAltitude(wp_landing_distance, wp_altitude); else return wp_altitude; } -float Landingslope::getFlareCurveAltitudeSave(float wp_landing_distance, float bearing_lastwp_currwp, float bearing_airplane_currwp, float wp_landing_altitude) +float Landingslope::getFlareCurveRelativeAltitudeSave(float wp_landing_distance, float bearing_lastwp_currwp, float bearing_airplane_currwp) { /* If airplane is in front of waypoint return flare curve altitude, else return waypoint altitude */ if (fabsf(bearing_airplane_currwp - bearing_lastwp_currwp) < math::radians(90.0f)) - return wp_landing_altitude + _H0 * expf(-math::max(0.0f, _flare_length - wp_landing_distance)/_flare_constant) - _H1_virt; + return _H0 * expf(-math::max(0.0f, _flare_length - wp_landing_distance)/_flare_constant) - _H1_virt; else - return wp_landing_altitude; + return 0.0f; +} + +float Landingslope::getFlareCurveAbsoluteAltitudeSave(float wp_landing_distance, float bearing_lastwp_currwp, float bearing_airplane_currwp, float wp_landing_altitude) +{ + + return wp_landing_altitude + getFlareCurveRelativeAltitudeSave(wp_landing_distance, bearing_lastwp_currwp, bearing_airplane_currwp); } diff --git a/src/modules/fw_pos_control_l1/landingslope.h b/src/modules/fw_pos_control_l1/landingslope.h index 76d65a55f2..b54fd501cf 100644 --- a/src/modules/fw_pos_control_l1/landingslope.h +++ b/src/modules/fw_pos_control_l1/landingslope.h @@ -63,11 +63,26 @@ public: Landingslope() {} ~Landingslope() {} + + /** + * + * @return relative altitude of point on landing slope at distance to landing waypoint=wp_landing_distance + */ + float getLandingSlopeRelativeAltitude(float wp_landing_distance); + + /** + * + * @return relative altitude of point on landing slope at distance to landing waypoint=wp_landing_distance + * Performs check if aircraft is in front of waypoint to avoid climbout + */ + float getLandingSlopeRelativeAltitudeSave(float wp_landing_distance, float bearing_lastwp_currwp, float bearing_airplane_currwp); + + /** * * @return Absolute altitude of point on landing slope at distance to landing waypoint=wp_landing_distance */ - float getLandingSlopeAbsoluteAltitude(float wp_distance, float wp_altitude); + float getLandingSlopeAbsoluteAltitude(float wp_landing_distance, float wp_altitude); /** * @@ -76,13 +91,22 @@ public: */ float getLandingSlopeAbsoluteAltitudeSave(float wp_landing_distance, float bearing_lastwp_currwp, float bearing_airplane_currwp, float wp_landing_altitude); + /** + * + * @return Relative altitude of point on landing slope at distance to landing waypoint=wp_landing_distance + */ + __EXPORT static float getLandingSlopeRelativeAltitude(float wp_landing_distance, float horizontal_slope_displacement, float landing_slope_angle_rad) + { + return (wp_landing_distance - horizontal_slope_displacement) * tanf(landing_slope_angle_rad); //flare_relative_alt is negative + } + /** * * @return Absolute altitude of point on landing slope at distance to landing waypoint=wp_landing_distance */ __EXPORT static float getLandingSlopeAbsoluteAltitude(float wp_landing_distance, float wp_landing_altitude, float horizontal_slope_displacement, float landing_slope_angle_rad) { - return (wp_landing_distance - horizontal_slope_displacement) * tanf(landing_slope_angle_rad) + wp_landing_altitude; //flare_relative_alt is negative + return getLandingSlopeRelativeAltitude(wp_landing_distance, horizontal_slope_displacement, landing_slope_angle_rad) + wp_landing_altitude; } /** @@ -95,8 +119,9 @@ public: } + float getFlareCurveRelativeAltitudeSave(float wp_distance, float bearing_lastwp_currwp, float bearing_airplane_currwp); - float getFlareCurveAltitudeSave(float wp_distance, float bearing_lastwp_currwp, float bearing_airplane_currwp, float wp_altitude); + float getFlareCurveAbsoluteAltitudeSave(float wp_distance, float bearing_lastwp_currwp, float bearing_airplane_currwp, float wp_altitude); void update(float landing_slope_angle_rad, float flare_relative_alt, From b70008242bf6959387958ea1b34a5e6ad89bfe27 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sat, 15 Mar 2014 15:28:37 +0100 Subject: [PATCH 022/329] add missing $ in rcS script --- ROMFS/px4fmu_common/init.d/rcS | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 55e2a886e1..7c3524fef0 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -466,7 +466,7 @@ then sh /etc/init.d/rc.interface # Start standard fixedwing apps - if [ LOAD_DEFAULT_APPS == yes ] + if [ $LOAD_DEFAULT_APPS == yes ] then sh /etc/init.d/rc.fw_apps fi @@ -525,7 +525,7 @@ then sh /etc/init.d/rc.interface # Start standard multicopter apps - if [ LOAD_DEFAULT_APPS == yes ] + if [ $LOAD_DEFAULT_APPS == yes ] then sh /etc/init.d/rc.mc_apps fi From 37d2cff83d3ecd697afb13a991b3c96133178318 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sat, 15 Mar 2014 16:03:26 +0100 Subject: [PATCH 023/329] set range finder to disabled as default --- src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c b/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c index 8ec0f0027d..9e2402447a 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c @@ -384,4 +384,4 @@ PARAM_DEFINE_FLOAT(FW_LND_HHDIST, 15.0f); * * @group L1 Control */ -PARAM_DEFINE_FLOAT(FW_LND_RFRALT, 30.0f); +PARAM_DEFINE_FLOAT(FW_LND_RFRALT, -1.0f); From 5894d72aa8077eae559f4444adb9203d59754f5f Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sat, 15 Mar 2014 16:39:18 +0100 Subject: [PATCH 024/329] fw landing: do not use relative altitudes in TECS --- src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index 32e00659bc..244b82ee1e 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -983,7 +983,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi land_stayonground = true; } - _tecs.update_pitch_throttle(_R_nb, _att.pitch, relative_alt, flare_curve_alt_rel, calculate_target_airspeed(airspeed_land), + _tecs.update_pitch_throttle(_R_nb, _att.pitch, _pos_sp_triplet.current.alt + relative_alt, _pos_sp_triplet.current.alt + flare_curve_alt_rel, calculate_target_airspeed(airspeed_land), _airspeed.indicated_airspeed_m_s, eas2tas, false, flare_pitch_angle_rad, 0.0f, throttle_max, throttle_land, @@ -1016,7 +1016,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi altitude_desired_rel = math::max(relative_alt, L_altitude_rel); } - _tecs.update_pitch_throttle(_R_nb, _att.pitch, relative_alt, altitude_desired_rel, calculate_target_airspeed(airspeed_approach), + _tecs.update_pitch_throttle(_R_nb, _att.pitch, _pos_sp_triplet.current.alt + relative_alt, _pos_sp_triplet.current.alt + altitude_desired_rel, calculate_target_airspeed(airspeed_approach), _airspeed.indicated_airspeed_m_s, eas2tas, false, math::radians(_parameters.pitch_limit_min), _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, From 3074bced5666934abf348c9fb2fc4bd4f4b6ca57 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sat, 15 Mar 2014 16:59:20 +0100 Subject: [PATCH 025/329] fix comment for FW_LND_RFRALT --- src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c b/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c index 9e2402447a..534e8d110d 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c @@ -377,10 +377,11 @@ PARAM_DEFINE_FLOAT(FW_LND_TLALT, 5.0f); PARAM_DEFINE_FLOAT(FW_LND_HHDIST, 15.0f); /** - * Landing relative altitude threshold for range finder measurements + * Relative altitude threshold for range finder measurements for use during landing * - * range finder measurements will only be used below FW_LND_RFRALT estimated relative altitude (gobal_pos.alt - landing_waypoint.alt) - * set to -1 to disable + * range finder measurements will only be used if the estimated relative altitude (gobal_pos.alt - landing_waypoint.alt) is < FW_LND_RFRALT + * set to < 0 to disable + * the correct value of this parameter depends on your range measuring device as well as on the terrain at the landing location * * @group L1 Control */ From 1362d5f195bc02f8f9c3ad0988768d547c705748 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sun, 30 Mar 2014 20:37:28 +0400 Subject: [PATCH 026/329] px4fmu: support all actuator control groups, dynamically subscribe to required topics --- src/drivers/px4fmu/fmu.cpp | 264 ++++++++++++++++++++++--------------- 1 file changed, 161 insertions(+), 103 deletions(-) diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp index a70ff6c5c5..9a1da39cbe 100644 --- a/src/drivers/px4fmu/fmu.cpp +++ b/src/drivers/px4fmu/fmu.cpp @@ -120,19 +120,25 @@ private: uint32_t _pwm_alt_rate_channels; unsigned _current_update_rate; int _task; - int _t_actuators; - int _t_actuator_armed; - orb_advert_t _t_outputs; + int _armed_sub; + orb_advert_t _outputs_pub; + actuator_armed_s _armed; unsigned _num_outputs; bool _primary_pwm_device; volatile bool _task_should_exit; - bool _armed; + bool _servo_armed; bool _pwm_on; MixerGroup *_mixers; - actuator_controls_s _controls; + uint32_t _groups_required; + uint32_t _groups_subscribed; + int _control_subs[NUM_ACTUATOR_CONTROL_GROUPS]; + actuator_controls_s _controls[NUM_ACTUATOR_CONTROL_GROUPS]; + orb_id_t _control_topics[NUM_ACTUATOR_CONTROL_GROUPS]; + pollfd _poll_fds[NUM_ACTUATOR_CONTROL_GROUPS]; + unsigned _poll_fds_num; pwm_limit_t _pwm_limit; uint16_t _failsafe_pwm[_max_actuators]; @@ -149,7 +155,7 @@ private: uint8_t control_group, uint8_t control_index, float &input); - + void subscribe(); int set_pwm_rate(unsigned rate_map, unsigned default_rate, unsigned alt_rate); int pwm_ioctl(file *filp, int cmd, unsigned long arg); @@ -216,15 +222,17 @@ PX4FMU::PX4FMU() : _pwm_alt_rate_channels(0), _current_update_rate(0), _task(-1), - _t_actuators(-1), - _t_actuator_armed(-1), - _t_outputs(0), + _control_subs({-1}), + _poll_fds_num(0), + _armed_sub(-1), + _outputs_pub(-1), _num_outputs(0), _primary_pwm_device(false), _task_should_exit(false), - _armed(false), + _servo_armed(false), _pwm_on(false), _mixers(nullptr), + _groups_required(0), _failsafe_pwm({0}), _disarmed_pwm({0}), _num_failsafe_set(0), @@ -235,6 +243,14 @@ PX4FMU::PX4FMU() : _max_pwm[i] = PWM_DEFAULT_MAX; } + _control_topics[0] = ORB_ID(actuator_controls_0); + _control_topics[1] = ORB_ID(actuator_controls_1); + _control_topics[2] = ORB_ID(actuator_controls_2); + _control_topics[3] = ORB_ID(actuator_controls_3); + + memset(_controls, 0, sizeof(_controls)); + memset(_poll_fds, 0, sizeof(_poll_fds)); + _debug_enabled = true; } @@ -447,33 +463,43 @@ PX4FMU::set_pwm_alt_channels(uint32_t channels) return set_pwm_rate(channels, _pwm_default_rate, _pwm_alt_rate); } +void +PX4FMU::subscribe() +{ + /* subscribe/unsubscribe to required actuator control groups */ + uint32_t sub_groups = _groups_required & ~_groups_subscribed; + uint32_t unsub_groups = _groups_subscribed & ~_groups_required; + _poll_fds_num = 0; + for (unsigned i = 0; i < NUM_ACTUATOR_CONTROL_GROUPS; i++) { + if (sub_groups & (1 << i)) { + warnx("subscribe to actuator_controls_%d", i); + _control_subs[i] = orb_subscribe(_control_topics[i]); + } + if (unsub_groups & (1 << i)) { + warnx("unsubscribe from actuator_controls_%d", i); + ::close(_control_subs[i]); + _control_subs[i] = -1; + } + + if (_control_subs[i] > 0) { + _poll_fds[_poll_fds_num].fd = _control_subs[i]; + _poll_fds[_poll_fds_num].events = POLLIN; + _poll_fds_num++; + } + } +} + void PX4FMU::task_main() { - /* - * Subscribe to the appropriate PWM output topic based on whether we are the - * primary PWM output or not. - */ - _t_actuators = orb_subscribe(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS : - ORB_ID(actuator_controls_1)); /* force a reset of the update rate */ _current_update_rate = 0; - _t_actuator_armed = orb_subscribe(ORB_ID(actuator_armed)); - orb_set_interval(_t_actuator_armed, 200); /* 5Hz update rate */ + _armed_sub = orb_subscribe(ORB_ID(actuator_armed)); /* advertise the mixed control outputs */ actuator_outputs_s outputs; memset(&outputs, 0, sizeof(outputs)); - /* advertise the mixed control outputs */ - _t_outputs = orb_advertise(_primary_pwm_device ? ORB_ID_VEHICLE_CONTROLS : ORB_ID(actuator_outputs_1), - &outputs); - - pollfd fds[2]; - fds[0].fd = _t_actuators; - fds[0].events = POLLIN; - fds[1].fd = _t_actuator_armed; - fds[1].events = POLLIN; #ifdef HRT_PPM_CHANNEL // rc input, published to ORB @@ -491,6 +517,12 @@ PX4FMU::task_main() /* loop until killed */ while (!_task_should_exit) { + if (_groups_subscribed != _groups_required) { + subscribe(); + _groups_subscribed = _groups_required; + /* force setting update rate */ + _current_update_rate = 0; + } /* * Adjust actuator topic update rate to keep up with @@ -515,7 +547,11 @@ PX4FMU::task_main() } debug("adjusted actuator update interval to %ums", update_rate_in_ms); - orb_set_interval(_t_actuators, update_rate_in_ms); + for (unsigned i = 0; i < NUM_ACTUATOR_CONTROL_GROUPS; i++) { + if (_control_subs[i] > 0) { + orb_set_interval(_control_subs[i], update_rate_in_ms); + } + } // set to current max rate, even if we are actually checking slower/faster _current_update_rate = max_rate; @@ -523,7 +559,7 @@ PX4FMU::task_main() /* sleep waiting for data, stopping to check for PPM * input at 100Hz */ - int ret = ::poll(&fds[0], 2, CONTROL_INPUT_DROP_LIMIT_MS); + int ret = ::poll(_poll_fds, _poll_fds_num, CONTROL_INPUT_DROP_LIMIT_MS); /* this would be bad... */ if (ret < 0) { @@ -537,90 +573,99 @@ PX4FMU::task_main() } else { - /* do we have a control update? */ - if (fds[0].revents & POLLIN) { - - /* get controls - must always do this to avoid spinning */ - orb_copy(_primary_pwm_device ? ORB_ID_VEHICLE_ATTITUDE_CONTROLS : ORB_ID(actuator_controls_1), _t_actuators, &_controls); - - /* can we mix? */ - if (_mixers != nullptr) { - - unsigned num_outputs; - - switch (_mode) { - case MODE_2PWM: - num_outputs = 2; - break; - - case MODE_4PWM: - num_outputs = 4; - break; - - case MODE_6PWM: - num_outputs = 6; - break; - - default: - num_outputs = 0; - break; + /* get controls for required topics */ + unsigned poll_id = 0; + for (unsigned i = 0; i < NUM_ACTUATOR_CONTROL_GROUPS; i++) { + if (_control_subs[i] > 0) { + if (_poll_fds[poll_id].revents & POLLIN) { + orb_copy(_control_topics[i], _control_subs[i], &_controls[i]); } - - /* do mixing */ - outputs.noutputs = _mixers->mix(&outputs.output[0], num_outputs); - outputs.timestamp = hrt_absolute_time(); - - /* iterate actuators */ - for (unsigned i = 0; i < num_outputs; i++) { - /* last resort: catch NaN, INF and out-of-band errors */ - if (i >= outputs.noutputs || - !isfinite(outputs.output[i]) || - outputs.output[i] < -1.0f || - outputs.output[i] > 1.0f) { - /* - * Value is NaN, INF or out of band - set to the minimum value. - * This will be clearly visible on the servo status and will limit the risk of accidentally - * spinning motors. It would be deadly in flight. - */ - outputs.output[i] = -1.0f; - } - } - - uint16_t pwm_limited[num_outputs]; - - pwm_limit_calc(_armed, num_outputs, _disarmed_pwm, _min_pwm, _max_pwm, outputs.output, pwm_limited, &_pwm_limit); - - /* output to the servos */ - for (unsigned i = 0; i < num_outputs; i++) { - up_pwm_servo_set(i, pwm_limited[i]); - } - - /* and publish for anyone that cares to see */ - orb_publish(_primary_pwm_device ? ORB_ID_VEHICLE_CONTROLS : ORB_ID(actuator_outputs_1), _t_outputs, &outputs); + poll_id++; } } - /* how about an arming update? */ - if (fds[1].revents & POLLIN) { - actuator_armed_s aa; + /* check arming state */ + bool updated = false; + orb_check(_armed_sub, &updated); - /* get new value */ - orb_copy(ORB_ID(actuator_armed), _t_actuator_armed, &aa); + if (updated) { + orb_copy(ORB_ID(actuator_armed), _armed_sub, &_armed); /* update the armed status and check that we're not locked down */ - bool set_armed = aa.armed && !aa.lockdown; + bool set_armed = _armed.armed && !_armed.lockdown; - if (_armed != set_armed) - _armed = set_armed; + if (_servo_armed != set_armed) + _servo_armed = set_armed; /* update PWM status if armed or if disarmed PWM values are set */ - bool pwm_on = (aa.armed || _num_disarmed_set > 0); + bool pwm_on = (_armed.armed || _num_disarmed_set > 0); if (_pwm_on != pwm_on) { _pwm_on = pwm_on; up_pwm_servo_arm(pwm_on); } } + + /* can we mix? */ + if (_mixers != nullptr) { + + unsigned num_outputs; + + switch (_mode) { + case MODE_2PWM: + num_outputs = 2; + break; + + case MODE_4PWM: + num_outputs = 4; + break; + + case MODE_6PWM: + num_outputs = 6; + break; + + default: + num_outputs = 0; + break; + } + + /* do mixing */ + outputs.noutputs = _mixers->mix(&outputs.output[0], num_outputs); + outputs.timestamp = hrt_absolute_time(); + + /* iterate actuators */ + for (unsigned i = 0; i < num_outputs; i++) { + /* last resort: catch NaN, INF and out-of-band errors */ + if (i >= outputs.noutputs || + !isfinite(outputs.output[i]) || + outputs.output[i] < -1.0f || + outputs.output[i] > 1.0f) { + /* + * Value is NaN, INF or out of band - set to the minimum value. + * This will be clearly visible on the servo status and will limit the risk of accidentally + * spinning motors. It would be deadly in flight. + */ + outputs.output[i] = -1.0f; + } + } + + uint16_t pwm_limited[num_outputs]; + + pwm_limit_calc(_servo_armed, num_outputs, _disarmed_pwm, _min_pwm, _max_pwm, outputs.output, pwm_limited, &_pwm_limit); + + /* output to the servos */ + for (unsigned i = 0; i < num_outputs; i++) { + up_pwm_servo_set(i, pwm_limited[i]); + } + + /* publish mixed control outputs */ + if (_outputs_pub < 0) { + _outputs_pub = orb_advertise(_primary_pwm_device ? ORB_ID_VEHICLE_CONTROLS : ORB_ID(actuator_outputs_1), &outputs); + } else { + + orb_publish(_primary_pwm_device ? ORB_ID_VEHICLE_CONTROLS : ORB_ID(actuator_outputs_1), _outputs_pub, &outputs); + } + } } #ifdef HRT_PPM_CHANNEL @@ -661,8 +706,13 @@ PX4FMU::task_main() } - ::close(_t_actuators); - ::close(_t_actuator_armed); + for (unsigned i = 0; i < NUM_ACTUATOR_CONTROL_GROUPS; i++) { + if (_control_subs > 0) { + ::close(_control_subs[i]); + _control_subs[i] = -1; + } + } + ::close(_armed_sub); /* make sure servos are off */ up_pwm_servo_deinit(); @@ -684,7 +734,7 @@ PX4FMU::control_callback(uintptr_t handle, { const actuator_controls_s *controls = (actuator_controls_s *)handle; - input = controls->control[control_index]; + input = controls[control_group].control[control_index]; return 0; } @@ -1052,6 +1102,7 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg) if (_mixers != nullptr) { delete _mixers; _mixers = nullptr; + _groups_required = 0; } break; @@ -1060,18 +1111,20 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg) mixer_simple_s *mixinfo = (mixer_simple_s *)arg; SimpleMixer *mixer = new SimpleMixer(control_callback, - (uintptr_t)&_controls, mixinfo); + (uintptr_t)_controls, mixinfo); if (mixer->check()) { delete mixer; + _groups_required = 0; ret = -EINVAL; } else { if (_mixers == nullptr) _mixers = new MixerGroup(control_callback, - (uintptr_t)&_controls); + (uintptr_t)_controls); _mixers->add_mixer(mixer); + _mixers->groups_required(_groups_required); } break; @@ -1082,9 +1135,10 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg) unsigned buflen = strnlen(buf, 1024); if (_mixers == nullptr) - _mixers = new MixerGroup(control_callback, (uintptr_t)&_controls); + _mixers = new MixerGroup(control_callback, (uintptr_t)_controls); if (_mixers == nullptr) { + _groups_required = 0; ret = -ENOMEM; } else { @@ -1095,7 +1149,11 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg) debug("mixer load failed with %d", ret); delete _mixers; _mixers = nullptr; + _groups_required = 0; ret = -EINVAL; + } else { + + _mixers->groups_required(_groups_required); } } From db37d3a4959c6f0888708bae4b4efd66c668e5b1 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Mon, 7 Apr 2014 13:36:55 +0400 Subject: [PATCH 027/329] fmu driver: bugs fixed --- src/drivers/px4fmu/fmu.cpp | 45 +++++++++++++++++++------------------- 1 file changed, 23 insertions(+), 22 deletions(-) diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp index 9a1da39cbe..8aa4473d41 100644 --- a/src/drivers/px4fmu/fmu.cpp +++ b/src/drivers/px4fmu/fmu.cpp @@ -233,6 +233,7 @@ PX4FMU::PX4FMU() : _pwm_on(false), _mixers(nullptr), _groups_required(0), + _groups_subscribed(0), _failsafe_pwm({0}), _disarmed_pwm({0}), _num_failsafe_set(0), @@ -584,28 +585,6 @@ PX4FMU::task_main() } } - /* check arming state */ - bool updated = false; - orb_check(_armed_sub, &updated); - - if (updated) { - orb_copy(ORB_ID(actuator_armed), _armed_sub, &_armed); - - /* update the armed status and check that we're not locked down */ - bool set_armed = _armed.armed && !_armed.lockdown; - - if (_servo_armed != set_armed) - _servo_armed = set_armed; - - /* update PWM status if armed or if disarmed PWM values are set */ - bool pwm_on = (_armed.armed || _num_disarmed_set > 0); - - if (_pwm_on != pwm_on) { - _pwm_on = pwm_on; - up_pwm_servo_arm(pwm_on); - } - } - /* can we mix? */ if (_mixers != nullptr) { @@ -668,6 +647,28 @@ PX4FMU::task_main() } } + /* check arming state */ + bool updated = false; + orb_check(_armed_sub, &updated); + + if (updated) { + orb_copy(ORB_ID(actuator_armed), _armed_sub, &_armed); + + /* update the armed status and check that we're not locked down */ + bool set_armed = _armed.armed && !_armed.lockdown; + + if (_servo_armed != set_armed) + _servo_armed = set_armed; + + /* update PWM status if armed or if disarmed PWM values are set */ + bool pwm_on = (_armed.armed || _num_disarmed_set > 0); + + if (_pwm_on != pwm_on) { + _pwm_on = pwm_on; + up_pwm_servo_arm(pwm_on); + } + } + #ifdef HRT_PPM_CHANNEL // see if we have new PPM input data From 0594343b9ccbb964808469041a1356414e281cbf Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sat, 12 Apr 2014 16:13:34 +0200 Subject: [PATCH 028/329] QU4D startup script --- .../init.d/10017_steadidrone_qu4d | 35 +++++++++++++++++++ ROMFS/px4fmu_common/init.d/rc.autostart | 5 +++ 2 files changed, 40 insertions(+) create mode 100644 ROMFS/px4fmu_common/init.d/10017_steadidrone_qu4d diff --git a/ROMFS/px4fmu_common/init.d/10017_steadidrone_qu4d b/ROMFS/px4fmu_common/init.d/10017_steadidrone_qu4d new file mode 100644 index 0000000000..5728a09205 --- /dev/null +++ b/ROMFS/px4fmu_common/init.d/10017_steadidrone_qu4d @@ -0,0 +1,35 @@ +#!nsh +# +# Steadidrone QU4D +# +# Thomas Gubler +# Lorenz Meier +# + +sh /etc/init.d/rc.mc_defaults + +if [ $DO_AUTOCONFIG == yes ] +then + # TODO tune roll/pitch separately + param set MC_ROLL_P 7.0 + param set MC_ROLLRATE_P 0.13 + param set MC_ROLLRATE_I 0.0 + param set MC_ROLLRATE_D 0.004 + param set MC_PITCH_P 7.0 + param set MC_PITCHRATE_P 0.13 + param set MC_PITCHRATE_I 0.0 + param set MC_PITCHRATE_D 0.004 + param set MC_YAW_P 0.5 + param set MC_YAWRATE_P 0.2 + param set MC_YAWRATE_I 0.0 + param set MC_YAWRATE_D 0.0 + + param set BAT_N_CELLS 4 +fi + +set MIXER FMU_quad_w + +set PWM_MIN 1210 +set PWM_MAX 1900 + +set PWM_OUTPUTS 1234 diff --git a/ROMFS/px4fmu_common/init.d/rc.autostart b/ROMFS/px4fmu_common/init.d/rc.autostart index 5ec735d393..b365bd6420 100644 --- a/ROMFS/px4fmu_common/init.d/rc.autostart +++ b/ROMFS/px4fmu_common/init.d/rc.autostart @@ -195,6 +195,11 @@ then sh /etc/init.d/10016_3dr_iris fi +if param compare SYS_AUTOSTART 10017 +then + sh /etc/init.d/10017_steadidrone_qu4d +fi + # # Hexa Coaxial # From e2989bc091c1a4e287534947095a1777d689ddfa Mon Sep 17 00:00:00 2001 From: eneadev Date: Sat, 19 Apr 2014 13:08:29 +0200 Subject: [PATCH 029/329] Update drv_hrt.c TIM3 was not working properly with a custom application and I got it work changing TIM3 HRT_TIMER_POWER_BIT from RCC_APB2ENR_TIM3EN to RCC_APB1ENR_TIM3EN because on datasheet TIM3 is on APB1, I think that you should check also the others --- src/drivers/stm32/drv_hrt.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/drivers/stm32/drv_hrt.c b/src/drivers/stm32/drv_hrt.c index b7c9b89a45..f324b341e1 100644 --- a/src/drivers/stm32/drv_hrt.c +++ b/src/drivers/stm32/drv_hrt.c @@ -94,7 +94,7 @@ #elif HRT_TIMER == 3 # define HRT_TIMER_BASE STM32_TIM3_BASE # define HRT_TIMER_POWER_REG STM32_RCC_APB1ENR -# define HRT_TIMER_POWER_BIT RCC_APB2ENR_TIM3EN +# define HRT_TIMER_POWER_BIT RCC_APB1ENR_TIM3EN # define HRT_TIMER_VECTOR STM32_IRQ_TIM3 # define HRT_TIMER_CLOCK STM32_APB1_TIM3_CLKIN # if CONFIG_STM32_TIM3 From edd16afead95bbf236d974ad895e10cc2ef70033 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 19 Apr 2014 15:49:29 +0200 Subject: [PATCH 030/329] Add filter parameters and multicopter defaults to parametrize Pauls estimator correctly when running for multicopters. Estimator itself not updated yet, will be next step. --- ROMFS/px4fmu_common/init.d/rc.mc_defaults | 6 + .../fw_att_pos_estimator_main.cpp | 44 ++++++ .../fw_att_pos_estimator_params.c | 131 ++++++++++++++++++ 3 files changed, 181 insertions(+) diff --git a/ROMFS/px4fmu_common/init.d/rc.mc_defaults b/ROMFS/px4fmu_common/init.d/rc.mc_defaults index 4db62607a9..c1f9db7d1c 100644 --- a/ROMFS/px4fmu_common/init.d/rc.mc_defaults +++ b/ROMFS/px4fmu_common/init.d/rc.mc_defaults @@ -35,6 +35,12 @@ then param set MPC_TILT_MAX 1.0 param set MPC_LAND_SPEED 1.0 param set MPC_LAND_TILT 0.3 + + param set PE_VELNE_NOISE 0.5 + param set PE_VELNE_NOISE 0.7 + param set PE_POSNE_NOISE 0.5 + param set PE_POSD_NOISE 1.0 + fi set PWM_RATE 400 diff --git a/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp b/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp index 840cd585ec..96db3f20cc 100644 --- a/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp +++ b/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp @@ -199,6 +199,17 @@ private: int32_t height_delay_ms; int32_t mag_delay_ms; int32_t tas_delay_ms; + float velne_noise; + float veld_noise; + float posne_noise; + float posd_noise; + float mag_noise; + float gyro_pnoise; + float acc_pnoise; + float gbias_pnoise; + float abias_pnoise; + float mage_pnoise; + float magb_pnoise; } _parameters; /**< local copies of interesting parameters */ struct { @@ -207,6 +218,17 @@ private: param_t height_delay_ms; param_t mag_delay_ms; param_t tas_delay_ms; + param_t velne_noise; + param_t veld_noise; + param_t posne_noise; + param_t posd_noise; + param_t mag_noise; + param_t gyro_pnoise; + param_t acc_pnoise; + param_t gbias_pnoise; + param_t abias_pnoise; + param_t mage_pnoise; + param_t magb_pnoise; } _parameter_handles; /**< handles for interesting parameters */ AttPosEKF *_ekf; @@ -302,6 +324,17 @@ FixedwingEstimator::FixedwingEstimator() : _parameter_handles.height_delay_ms = param_find("PE_HGT_DELAY_MS"); _parameter_handles.mag_delay_ms = param_find("PE_MAG_DELAY_MS"); _parameter_handles.tas_delay_ms = param_find("PE_TAS_DELAY_MS"); + _parameter_handles.velne_noise = param_find("PE_VELNE_NOISE"); + _parameter_handles.veld_noise = param_find("PE_VELD_NOISE"); + _parameter_handles.posne_noise = param_find("PE_POSNE_NOISE"); + _parameter_handles.posd_noise = param_find("PE_POSD_NOISE"); + _parameter_handles.mag_noise = param_find("PE_MAG_NOISE"); + _parameter_handles.gyro_pnoise = param_find("PE_GYRO_PNOISE"); + _parameter_handles.acc_pnoise = param_find("PE_ACC_PNOISE"); + _parameter_handles.gbias_pnoise = param_find("PE_GBIAS_PNOISE"); + _parameter_handles.abias_pnoise = param_find("PE_ABIAS_PNOISE"); + _parameter_handles.mage_pnoise = param_find("PE_MAGE_PNOISE"); + _parameter_handles.magb_pnoise = param_find("PE_MAGB_PNOISE"); /* fetch initial parameter values */ parameters_update(); @@ -366,6 +399,17 @@ FixedwingEstimator::parameters_update() param_get(_parameter_handles.height_delay_ms, &(_parameters.height_delay_ms)); param_get(_parameter_handles.mag_delay_ms, &(_parameters.mag_delay_ms)); param_get(_parameter_handles.tas_delay_ms, &(_parameters.tas_delay_ms)); + param_get(_parameter_handles.velne_noise, &(_parameters.velne_noise)); + param_get(_parameter_handles.veld_noise, &(_parameters.veld_noise)); + param_get(_parameter_handles.posne_noise, &(_parameters.posne_noise)); + param_get(_parameter_handles.posd_noise, &(_parameters.posd_noise)); + param_get(_parameter_handles.mag_noise, &(_parameters.mag_noise)); + param_get(_parameter_handles.gyro_pnoise, &(_parameters.gyro_pnoise)); + param_get(_parameter_handles.acc_pnoise, &(_parameters.acc_pnoise)); + param_get(_parameter_handles.gbias_pnoise, &(_parameters.gbias_pnoise)); + param_get(_parameter_handles.abias_pnoise, &(_parameters.abias_pnoise)); + param_get(_parameter_handles.mage_pnoise, &(_parameters.mage_pnoise)); + param_get(_parameter_handles.magb_pnoise, &(_parameters.magb_pnoise)); return OK; } diff --git a/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_params.c b/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_params.c index 6138ef39cd..9d01a095c8 100644 --- a/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_params.c +++ b/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_params.c @@ -115,3 +115,134 @@ PARAM_DEFINE_INT32(PE_TAS_DELAY_MS, 210); */ PARAM_DEFINE_FLOAT(PE_GPS_ALT_WGT, 0.9f); +/** + * Velocity noise in north-east (horizontal) direction. + * + * Generic default: 0.3, multicopters: 0.5, ground vehicles: 0.5 + * + * @min 0.05 + * @max 5.0 + * @group Position Estimator + */ +PARAM_DEFINE_FLOAT(PE_VELNE_NOISE, 0.3f); + +/** + * Velocity noise in down (vertical) direction + * + * Generic default: 0.5, multicopters: 0.7, ground vehicles: 0.7 + * + * @min 0.05 + * @max 5.0 + * @group Position Estimator + */ +PARAM_DEFINE_FLOAT(PE_VELD_NOISE, 0.5f); + +/** + * Position noise in north-east (horizontal) direction + * + * Generic defaults: 0.5, multicopters: 0.5, ground vehicles: 0.5 + * + * @min 0.1 + * @max 10.0 + * @group Position Estimator + */ +PARAM_DEFINE_FLOAT(PE_POSNE_NOISE, 0.5f); + +/** + * Position noise in down (vertical) direction + * + * Generic defaults: 0.5, multicopters: 1.0, ground vehicles: 1.0 + * + * @min 0.1 + * @max 10.0 + * @group Position Estimator + */ +PARAM_DEFINE_FLOAT(PE_POSD_NOISE, 0.5f); + +/** + * Magnetometer measurement noise + * + * Generic defaults: 0.05, multicopters: 0.05, ground vehicles: 0.05 + * + * @min 0.1 + * @max 10.0 + * @group Position Estimator + */ +PARAM_DEFINE_FLOAT(PE_MAG_NOISE, 0.05f); + +/** + * Gyro process noise + * + * Generic defaults: 0.015, multicopters: 0.015, ground vehicles: 0.015. + * This noise controls how much the filter trusts the gyro measurements. + * Increasing it makes the filter trust the gyro less and other sensors more. + * + * @min 0.001 + * @max 0.05 + * @group Position Estimator + */ +PARAM_DEFINE_FLOAT(PE_GYRO_PNOISE, 0.015f); + +/** + * Accelerometer process noise + * + * Generic defaults: 0.25, multicopters: 0.25, ground vehicles: 0.25. + * Increasing this value makes the filter trust the accelerometer less + * and other sensors more. + * + * @min 0.05 + * @max 1.0 + * @group Position Estimator + */ +PARAM_DEFINE_FLOAT(PE_ACC_PNOISE, 0.25f); + +/** + * Gyro bias estimate process noise + * + * Generic defaults: 1e-07f, multicopters: 1e-07f, ground vehicles: 1e-07f. + * Increasing this value will make the gyro bias converge faster but noisier. + * + * @min 0.0000001 + * @max 0.00001 + * @group Position Estimator + */ +PARAM_DEFINE_FLOAT(PE_GBIAS_PNOISE, 1e-07f); + +/** + * Accelerometer bias estimate process noise + * + * Generic defaults: 0.0001f, multicopters: 0.0001f, ground vehicles: 0.0001f. + * Increasing this value makes the bias estimation faster and noisier. + * + * @min 0.0001 + * @max 0.001 + * @group Position Estimator + */ +PARAM_DEFINE_FLOAT(PE_ABIAS_PNOISE, 0.0001f); + +/** + * Magnetometer earth frame offsets process noise + * + * Generic defaults: 0.0001, multicopters: 0.0001, ground vehicles: 0.0001. + * Increasing this value makes the magnetometer earth bias estimate converge + * faster but also noisier. + * + * @min 0.0001 + * @max 0.01 + * @group Position Estimator + */ +PARAM_DEFINE_FLOAT(PE_MAGE_PNOISE, 0.0003f); + +/** + * Magnetometer body frame offsets process noise + * + * Generic defaults: 0.0003, multicopters: 0.0003, ground vehicles: 0.0003. + * Increasing this value makes the magnetometer body bias estimate converge faster + * but also noisier. + * + * @min 0.0001 + * @max 0.01 + * @group Position Estimator + */ +PARAM_DEFINE_FLOAT(PE_MAGB_PNOISE, 0.0003f); + From 7b61c927f0420cb3b519972221654176e7c9274b Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 19 Apr 2014 16:13:13 +0200 Subject: [PATCH 031/329] Renamed FW filter to EKF to express its generic properties, switched multicopters over to this filter for first tests. --- ROMFS/px4fmu_common/init.d/rc.fw_apps | 2 +- ROMFS/px4fmu_common/init.d/rc.mc_apps | 3 ++- makefiles/config_px4fmu-v1_default.mk | 2 +- makefiles/config_px4fmu-v2_default.mk | 2 +- .../estimator.cpp | 0 .../estimator.h | 0 .../fw_att_pos_estimator_main.cpp | 6 +++--- .../fw_att_pos_estimator_params.c | 0 .../module.mk | 2 +- 9 files changed, 9 insertions(+), 8 deletions(-) rename src/modules/{fw_att_pos_estimator => ekf_att_pos_estimator}/estimator.cpp (100%) rename src/modules/{fw_att_pos_estimator => ekf_att_pos_estimator}/estimator.h (100%) rename src/modules/{fw_att_pos_estimator => ekf_att_pos_estimator}/fw_att_pos_estimator_main.cpp (99%) rename src/modules/{fw_att_pos_estimator => ekf_att_pos_estimator}/fw_att_pos_estimator_params.c (100%) rename src/modules/{fw_att_pos_estimator => ekf_att_pos_estimator}/module.mk (97%) diff --git a/ROMFS/px4fmu_common/init.d/rc.fw_apps b/ROMFS/px4fmu_common/init.d/rc.fw_apps index 429abc5ec8..9aca3fc5f4 100644 --- a/ROMFS/px4fmu_common/init.d/rc.fw_apps +++ b/ROMFS/px4fmu_common/init.d/rc.fw_apps @@ -6,7 +6,7 @@ # # Start the attitude and position estimator # -fw_att_pos_estimator start +ekf_att_pos_estimator start # # Start attitude controller diff --git a/ROMFS/px4fmu_common/init.d/rc.mc_apps b/ROMFS/px4fmu_common/init.d/rc.mc_apps index ed3939757a..c75281fcdd 100644 --- a/ROMFS/px4fmu_common/init.d/rc.mc_apps +++ b/ROMFS/px4fmu_common/init.d/rc.mc_apps @@ -5,7 +5,8 @@ # attitude_estimator_ekf start -position_estimator_inav start +ekf_att_pos_estimator start +#position_estimator_inav start mc_att_control start mc_pos_control start diff --git a/makefiles/config_px4fmu-v1_default.mk b/makefiles/config_px4fmu-v1_default.mk index 532e978d04..1daf8277ea 100644 --- a/makefiles/config_px4fmu-v1_default.mk +++ b/makefiles/config_px4fmu-v1_default.mk @@ -70,7 +70,7 @@ MODULES += modules/gpio_led # Estimation modules (EKF/ SO3 / other filters) # MODULES += modules/attitude_estimator_ekf -MODULES += modules/fw_att_pos_estimator +MODULES += modules/ekf_att_pos_estimator MODULES += modules/position_estimator_inav #MODULES += examples/flow_position_estimator diff --git a/makefiles/config_px4fmu-v2_default.mk b/makefiles/config_px4fmu-v2_default.mk index e13421acc5..7f0c59515a 100644 --- a/makefiles/config_px4fmu-v2_default.mk +++ b/makefiles/config_px4fmu-v2_default.mk @@ -79,7 +79,7 @@ MODULES += modules/gpio_led # MODULES += modules/attitude_estimator_ekf MODULES += modules/attitude_estimator_so3 -MODULES += modules/fw_att_pos_estimator +MODULES += modules/ekf_att_pos_estimator MODULES += modules/position_estimator_inav MODULES += examples/flow_position_estimator diff --git a/src/modules/fw_att_pos_estimator/estimator.cpp b/src/modules/ekf_att_pos_estimator/estimator.cpp similarity index 100% rename from src/modules/fw_att_pos_estimator/estimator.cpp rename to src/modules/ekf_att_pos_estimator/estimator.cpp diff --git a/src/modules/fw_att_pos_estimator/estimator.h b/src/modules/ekf_att_pos_estimator/estimator.h similarity index 100% rename from src/modules/fw_att_pos_estimator/estimator.h rename to src/modules/ekf_att_pos_estimator/estimator.h diff --git a/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp similarity index 99% rename from src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp rename to src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp index 96db3f20cc..7857a04693 100644 --- a/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp @@ -90,7 +90,7 @@ * * @ingroup apps */ -extern "C" __EXPORT int fw_att_pos_estimator_main(int argc, char *argv[]); +extern "C" __EXPORT int ekf_att_pos_estimator_main(int argc, char *argv[]); __EXPORT uint32_t millis(); @@ -1255,10 +1255,10 @@ int FixedwingEstimator::trip_nan() { return ret; } -int fw_att_pos_estimator_main(int argc, char *argv[]) +int ekf_att_pos_estimator_main(int argc, char *argv[]) { if (argc < 1) - errx(1, "usage: fw_att_pos_estimator {start|stop|status}"); + errx(1, "usage: ekf_att_pos_estimator {start|stop|status}"); if (!strcmp(argv[1], "start")) { diff --git a/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_params.c b/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_params.c similarity index 100% rename from src/modules/fw_att_pos_estimator/fw_att_pos_estimator_params.c rename to src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_params.c diff --git a/src/modules/fw_att_pos_estimator/module.mk b/src/modules/ekf_att_pos_estimator/module.mk similarity index 97% rename from src/modules/fw_att_pos_estimator/module.mk rename to src/modules/ekf_att_pos_estimator/module.mk index c992959e0d..30955d0ddc 100644 --- a/src/modules/fw_att_pos_estimator/module.mk +++ b/src/modules/ekf_att_pos_estimator/module.mk @@ -35,7 +35,7 @@ # Main Attitude and Position Estimator for Fixed Wing Aircraft # -MODULE_COMMAND = fw_att_pos_estimator +MODULE_COMMAND = ekf_att_pos_estimator SRCS = fw_att_pos_estimator_main.cpp \ fw_att_pos_estimator_params.c \ From 86a0862af6412906611ed295cae4604e7111b1e9 Mon Sep 17 00:00:00 2001 From: TickTock- Date: Sat, 19 Apr 2014 13:07:09 -0700 Subject: [PATCH 032/329] Added rc_map_failsafe to enable use of channels other than throttle for failsafe. --- src/modules/sensors/sensor_params.c | 29 +++++++++++++++++++++++++++++ src/modules/sensors/sensors.cpp | 19 +++++++++++-------- 2 files changed, 40 insertions(+), 8 deletions(-) diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c index 14f7ac812f..ff121c51ef 100644 --- a/src/modules/sensors/sensor_params.c +++ b/src/modules/sensors/sensor_params.c @@ -535,6 +535,35 @@ PARAM_DEFINE_INT32(RC_MAP_ROLL, 1); */ PARAM_DEFINE_INT32(RC_MAP_PITCH, 2); +/** + * Failsafe channel mapping. + * + * The RC mapping index indicates which rc function + * should be used for detecting the failsafe condition + * + * @min 0 + * @max 14 + * + * mapping (from rc_channels.h) + * THROTTLE = 0, + ROLL = 1, + PITCH = 2, + YAW = 3, + MODE = 4, + RETURN = 5, + ASSISTED = 6, + MISSION = 7, + OFFBOARD_MODE = 8, + FLAPS = 9, + AUX_1 = 10, + AUX_2 = 11, + AUX_3 = 12, + AUX_4 = 13, + AUX_5 = 14, + * + */ +PARAM_DEFINE_INT32(RC_MAP_FAILSAFE, 0); //Default to throttle function + /** * Throttle control channel mapping. * diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 4083b8b4f5..fa3905f608 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -242,6 +242,7 @@ private: int rc_map_pitch; int rc_map_yaw; int rc_map_throttle; + int rc_map_failsafe; int rc_map_mode_sw; int rc_map_return_sw; @@ -290,6 +291,7 @@ private: param_t rc_map_pitch; param_t rc_map_yaw; param_t rc_map_throttle; + param_t rc_map_failsafe; param_t rc_map_mode_sw; param_t rc_map_return_sw; @@ -512,6 +514,7 @@ Sensors::Sensors() : /* 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_failsafe = param_find("RC_MAP_FAILSAFE"); // _parameter_handles.rc_map_offboard_ctrl_mode_sw = param_find("RC_MAP_OFFB_SW"); @@ -650,6 +653,10 @@ Sensors::parameters_update() warnx(paramerr); } + if (param_get(_parameter_handles.rc_map_failsafe, &(_parameters.rc_map_failsafe)) != OK) { + warnx(paramerr); + } + if (param_get(_parameter_handles.rc_map_mode_sw, &(_parameters.rc_map_mode_sw)) != OK) { warnx(paramerr); } @@ -704,7 +711,6 @@ Sensors::parameters_update() _rc.function[AUX_2] = _parameters.rc_map_aux2 - 1; _rc.function[AUX_3] = _parameters.rc_map_aux3 - 1; _rc.function[AUX_4] = _parameters.rc_map_aux4 - 1; - _rc.function[AUX_5] = _parameters.rc_map_aux5 - 1; /* gyro offsets */ param_get(_parameter_handles.gyro_offset[0], &(_parameters.gyro_offset[0])); @@ -1310,8 +1316,8 @@ Sensors::rc_poll() } /* check for failsafe */ - if ((rc_input.rc_failsafe) || ((_parameters.rc_fs_thr != 0) && (((rc_input.values[_rc.function[THROTTLE]] < _parameters.min[_rc.function[THROTTLE]]) && (rc_input.values[_rc.function[THROTTLE]] < _parameters.rc_fs_thr)) - || ((rc_input.values[_rc.function[THROTTLE]] > _parameters.max[_rc.function[THROTTLE]]) && (rc_input.values[_rc.function[THROTTLE]] > _parameters.rc_fs_thr))))) { + if ((rc_input.rc_failsafe) || ((_parameters.rc_fs_thr != 0) && (((rc_input.values[_rc.function[_parameters.rc_map_failsafe]] < _parameters.min[_rc.function[_parameters.rc_map_failsafe]]) && (rc_input.values[_rc.function[_parameters.rc_map_failsafe]] < _parameters.rc_fs_thr)) + || ((rc_input.values[_rc.function[_parameters.rc_map_failsafe]] > _parameters.max[_rc.function[_parameters.rc_map_failsafe]]) && (rc_input.values[_rc.function[_parameters.rc_map_failsafe]] > _parameters.rc_fs_thr))))) { /* do not publish manual control setpoints when there are none */ return; } @@ -1434,7 +1440,8 @@ Sensors::rc_poll() manual_control.return_switch = limit_minus_one_to_one(_rc.chan[_rc.function[RETURN]].scaled); } -// if (_rc.function[OFFBOARD_MODE] >= 0) { + + // if (_rc.function[OFFBOARD_MODE] >= 0) { // manual_control.auto_offboard_input_switch = limit_minus_one_to_one(_rc.chan[_rc.function[OFFBOARD_MODE]].scaled); // } @@ -1455,10 +1462,6 @@ Sensors::rc_poll() manual_control.aux4 = limit_minus_one_to_one(_rc.chan[_rc.function[AUX_4]].scaled); } - if (_rc.function[AUX_5] >= 0) { - manual_control.aux5 = limit_minus_one_to_one(_rc.chan[_rc.function[AUX_5]].scaled); - } - /* copy from mapped manual control to control group 3 */ actuator_group_3.control[0] = manual_control.roll; actuator_group_3.control[1] = manual_control.pitch; From dca1e7fc611bb44caf1fc586e45105d170955de2 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 19 Apr 2014 22:40:37 +0200 Subject: [PATCH 033/329] Decomission unmaintained position estimator --- .../att_pos_estimator_ekf/KalmanNav.cpp | 815 ------------------ .../att_pos_estimator_ekf/KalmanNav.hpp | 192 ----- .../att_pos_estimator_ekf/kalman_main.cpp | 157 ---- src/modules/att_pos_estimator_ekf/module.mk | 42 - src/modules/att_pos_estimator_ekf/params.c | 49 -- 5 files changed, 1255 deletions(-) delete mode 100644 src/modules/att_pos_estimator_ekf/KalmanNav.cpp delete mode 100644 src/modules/att_pos_estimator_ekf/KalmanNav.hpp delete mode 100644 src/modules/att_pos_estimator_ekf/kalman_main.cpp delete mode 100644 src/modules/att_pos_estimator_ekf/module.mk delete mode 100644 src/modules/att_pos_estimator_ekf/params.c diff --git a/src/modules/att_pos_estimator_ekf/KalmanNav.cpp b/src/modules/att_pos_estimator_ekf/KalmanNav.cpp deleted file mode 100644 index 668bac5d9c..0000000000 --- a/src/modules/att_pos_estimator_ekf/KalmanNav.cpp +++ /dev/null @@ -1,815 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file KalmanNav.cpp - * - * Kalman filter navigation code - */ - -#include - -#include "KalmanNav.hpp" -#include -#include - -// constants -// Titterton pg. 52 -static const float omega = 7.2921150e-5f; // earth rotation rate, rad/s -static const float R0 = 6378137.0f; // earth radius, m -static const float g0 = 9.806f; // standard gravitational accel. m/s^2 -static const int8_t ret_ok = 0; // no error in function -static const int8_t ret_error = -1; // error occurred - -KalmanNav::KalmanNav(SuperBlock *parent, const char *name) : - SuperBlock(parent, name), - // subscriptions - _sensors(&getSubscriptions(), ORB_ID(sensor_combined), 5), // limit to 200 Hz - _gps(&getSubscriptions(), ORB_ID(vehicle_gps_position), 100), // limit to 10 Hz - _param_update(&getSubscriptions(), ORB_ID(parameter_update), 1000), // limit to 1 Hz - // publications - _pos(&getPublications(), ORB_ID(vehicle_global_position)), - _localPos(&getPublications(), ORB_ID(vehicle_local_position)), - _att(&getPublications(), ORB_ID(vehicle_attitude)), - // timestamps - _pubTimeStamp(hrt_absolute_time()), - _predictTimeStamp(hrt_absolute_time()), - _attTimeStamp(hrt_absolute_time()), - _outTimeStamp(hrt_absolute_time()), - // frame count - _navFrames(0), - // miss counts - _miss(0), - // accelerations - fN(0), fE(0), fD(0), - // state - phi(0), theta(0), psi(0), - vN(0), vE(0), vD(0), - lat(0), lon(0), alt(0), - lat0(0), lon0(0), alt0(0), - // parameters for ground station - _vGyro(this, "V_GYRO"), - _vAccel(this, "V_ACCEL"), - _rMag(this, "R_MAG"), - _rGpsVel(this, "R_GPS_VEL"), - _rGpsPos(this, "R_GPS_POS"), - _rGpsAlt(this, "R_GPS_ALT"), - _rPressAlt(this, "R_PRESS_ALT"), - _rAccel(this, "R_ACCEL"), - _magDip(this, "ENV_MAG_DIP"), - _magDec(this, "ENV_MAG_DEC"), - _g(this, "ENV_G"), - _faultPos(this, "FAULT_POS"), - _faultAtt(this, "FAULT_ATT"), - _attitudeInitialized(false), - _positionInitialized(false), - _attitudeInitCounter(0) -{ - using namespace math; - - F.zero(); - G.zero(); - V.zero(); - HAtt.zero(); - RAtt.zero(); - HPos.zero(); - RPos.zero(); - - // initial state covariance matrix - P0.identity(); - P0 *= 0.01f; - P = P0; - - // initial state - phi = 0.0f; - theta = 0.0f; - psi = 0.0f; - vN = 0.0f; - vE = 0.0f; - vD = 0.0f; - lat = 0.0f; - lon = 0.0f; - alt = 0.0f; - - // initialize rotation quaternion with a single raw sensor measurement - _sensors.update(); - q = init( - _sensors.accelerometer_m_s2[0], - _sensors.accelerometer_m_s2[1], - _sensors.accelerometer_m_s2[2], - _sensors.magnetometer_ga[0], - _sensors.magnetometer_ga[1], - _sensors.magnetometer_ga[2]); - - // initialize dcm - C_nb = q.to_dcm(); - - // HPos is constant - HPos(0, 3) = 1.0f; - HPos(1, 4) = 1.0f; - HPos(2, 6) = 1.0e7f * M_RAD_TO_DEG_F; - HPos(3, 7) = 1.0e7f * M_RAD_TO_DEG_F; - HPos(4, 8) = 1.0f; - HPos(5, 8) = 1.0f; - - // initialize all parameters - updateParams(); -} - -math::Quaternion KalmanNav::init(float ax, float ay, float az, float mx, float my, float mz) -{ - float initialRoll, initialPitch; - float cosRoll, sinRoll, cosPitch, sinPitch; - float magX, magY; - float initialHdg, cosHeading, sinHeading; - - initialRoll = atan2(-ay, -az); - initialPitch = atan2(ax, -az); - - cosRoll = cosf(initialRoll); - sinRoll = sinf(initialRoll); - cosPitch = cosf(initialPitch); - sinPitch = sinf(initialPitch); - - magX = mx * cosPitch + my * sinRoll * sinPitch + mz * cosRoll * sinPitch; - - magY = my * cosRoll - mz * sinRoll; - - initialHdg = atan2f(-magY, magX); - - cosRoll = cosf(initialRoll * 0.5f); - sinRoll = sinf(initialRoll * 0.5f); - - cosPitch = cosf(initialPitch * 0.5f); - sinPitch = sinf(initialPitch * 0.5f); - - cosHeading = cosf(initialHdg * 0.5f); - sinHeading = sinf(initialHdg * 0.5f); - - float q0 = cosRoll * cosPitch * cosHeading + sinRoll * sinPitch * sinHeading; - float q1 = sinRoll * cosPitch * cosHeading - cosRoll * sinPitch * sinHeading; - float q2 = cosRoll * sinPitch * cosHeading + sinRoll * cosPitch * sinHeading; - float q3 = cosRoll * cosPitch * sinHeading - sinRoll * sinPitch * cosHeading; - - return math::Quaternion(q0, q1, q2, q3); - -} - -void KalmanNav::update() -{ - using namespace math; - - struct pollfd fds[1]; - fds[0].fd = _sensors.getHandle(); - fds[0].events = POLLIN; - - // poll for new data - int ret = poll(fds, 1, 1000); - - if (ret < 0) { - // XXX this is seriously bad - should be an emergency - return; - - } else if (ret == 0) { // timeout - return; - } - - // get new timestamp - uint64_t newTimeStamp = hrt_absolute_time(); - - // check updated subscriptions - if (_param_update.updated()) updateParams(); - - bool gpsUpdate = _gps.updated(); - bool sensorsUpdate = _sensors.updated(); - - // get new information from subscriptions - // this clears update flag - updateSubscriptions(); - - // initialize attitude when sensors online - if (!_attitudeInitialized && sensorsUpdate) { - if (correctAtt() == ret_ok) _attitudeInitCounter++; - - if (_attitudeInitCounter > 100) { - warnx("initialized EKF attitude"); - warnx("phi: %8.4f, theta: %8.4f, psi: %8.4f", - double(phi), double(theta), double(psi)); - _attitudeInitialized = true; - } - } - - // initialize position when gps received - if (!_positionInitialized && - _attitudeInitialized && // wait for attitude first - gpsUpdate && - _gps.fix_type > 2 - //&& _gps.counter_pos_valid > 10 - ) { - vN = _gps.vel_n_m_s; - vE = _gps.vel_e_m_s; - vD = _gps.vel_d_m_s; - setLatDegE7(_gps.lat); - setLonDegE7(_gps.lon); - setAltE3(_gps.alt); - // set reference position for - // local position - lat0 = lat; - lon0 = lon; - alt0 = alt; - // XXX map_projection has internal global - // states that multiple things could change, - // should make map_projection take reference - // lat/lon and not have init - map_projection_init(lat0, lon0); - _positionInitialized = true; - warnx("initialized EKF state with GPS"); - warnx("vN: %8.4f, vE: %8.4f, vD: %8.4f, lat: %8.4f, lon: %8.4f, alt: %8.4f", - double(vN), double(vE), double(vD), - lat, lon, double(alt)); - } - - // prediction step - // using sensors timestamp so we can account for packet lag - float dt = (_sensors.timestamp - _predictTimeStamp) / 1.0e6f; - //printf("dt: %15.10f\n", double(dt)); - _predictTimeStamp = _sensors.timestamp; - - // don't predict if time greater than a second - if (dt < 1.0f) { - predictState(dt); - predictStateCovariance(dt); - // count fast frames - _navFrames += 1; - } - - // count times 100 Hz rate isn't met - if (dt > 0.01f) _miss++; - - // gps correction step - if (_positionInitialized && gpsUpdate) { - correctPos(); - } - - // attitude correction step - if (_attitudeInitialized // initialized - && sensorsUpdate // new data - && _sensors.timestamp - _attTimeStamp > 1e6 / 50 // 50 Hz - ) { - _attTimeStamp = _sensors.timestamp; - correctAtt(); - } - - // publication - if (newTimeStamp - _pubTimeStamp > 1e6 / 50) { // 50 Hz - _pubTimeStamp = newTimeStamp; - - updatePublications(); - } - - // output - if (newTimeStamp - _outTimeStamp > 10e6) { // 0.1 Hz - _outTimeStamp = newTimeStamp; - //printf("nav: %4d Hz, miss #: %4d\n", - // _navFrames / 10, _miss / 10); - _navFrames = 0; - _miss = 0; - } -} - -void KalmanNav::updatePublications() -{ - using namespace math; - - // global position publication - _pos.timestamp = _pubTimeStamp; - _pos.time_gps_usec = _gps.timestamp_position; - _pos.global_valid = true; - _pos.lat = lat * M_RAD_TO_DEG; - _pos.lon = lon * M_RAD_TO_DEG; - _pos.alt = float(alt); - _pos.vel_n = vN; - _pos.vel_e = vE; - _pos.vel_d = vD; - _pos.yaw = psi; - - // local position publication - float x; - float y; - bool landed = alt < (alt0 + 0.1); // XXX improve? - map_projection_project(lat, lon, &x, &y); - _localPos.timestamp = _pubTimeStamp; - _localPos.xy_valid = true; - _localPos.z_valid = true; - _localPos.v_xy_valid = true; - _localPos.v_z_valid = true; - _localPos.x = x; - _localPos.y = y; - _localPos.z = alt0 - alt; - _localPos.vx = vN; - _localPos.vy = vE; - _localPos.vz = vD; - _localPos.yaw = psi; - _localPos.xy_global = true; - _localPos.z_global = true; - _localPos.ref_timestamp = _pubTimeStamp; - _localPos.ref_lat = getLatDegE7(); - _localPos.ref_lon = getLonDegE7(); - _localPos.ref_alt = 0; - _localPos.landed = landed; - - // attitude publication - _att.timestamp = _pubTimeStamp; - _att.roll = phi; - _att.pitch = theta; - _att.yaw = psi; - _att.rollspeed = _sensors.gyro_rad_s[0]; - _att.pitchspeed = _sensors.gyro_rad_s[1]; - _att.yawspeed = _sensors.gyro_rad_s[2]; - // TODO, add gyro offsets to filter - _att.rate_offsets[0] = 0.0f; - _att.rate_offsets[1] = 0.0f; - _att.rate_offsets[2] = 0.0f; - - for (int i = 0; i < 3; i++) for (int j = 0; j < 3; j++) - _att.R[i][j] = C_nb(i, j); - - for (int i = 0; i < 4; i++) _att.q[i] = q(i); - - _att.R_valid = true; - _att.q_valid = true; - - // selectively update publications, - // do NOT call superblock do-all method - if (_positionInitialized) { - _pos.update(); - _localPos.update(); - } - - if (_attitudeInitialized) - _att.update(); -} - -int KalmanNav::predictState(float dt) -{ - using namespace math; - - // trig - float sinL = sinf(lat); - float cosL = cosf(lat); - float cosLSing = cosf(lat); - - // prevent singularity - if (fabsf(cosLSing) < 0.01f) { - if (cosLSing > 0) cosLSing = 0.01; - else cosLSing = -0.01; - } - - // attitude prediction - if (_attitudeInitialized) { - Vector<3> w(_sensors.gyro_rad_s); - - // attitude - q = q + q.derivative(w) * dt; - - // renormalize quaternion if needed - if (fabsf(q.length() - 1.0f) > 1e-4f) { - q.normalize(); - } - - // C_nb update - C_nb = q.to_dcm(); - - // euler update - Vector<3> euler = C_nb.to_euler(); - phi = euler.data[0]; - theta = euler.data[1]; - psi = euler.data[2]; - - // specific acceleration in nav frame - Vector<3> accelB(_sensors.accelerometer_m_s2); - Vector<3> accelN = C_nb * accelB; - fN = accelN(0); - fE = accelN(1); - fD = accelN(2); - } - - // position prediction - if (_positionInitialized) { - // neglects angular deflections in local gravity - // see Titerton pg. 70 - float R = R0 + float(alt); - float LDot = vN / R; - float lDot = vE / (cosLSing * R); - float rotRate = 2 * omega + lDot; - - // XXX position prediction using speed - float vNDot = fN - vE * rotRate * sinL + - vD * LDot; - float vDDot = fD - vE * rotRate * cosL - - vN * LDot + _g.get(); - float vEDot = fE + vN * rotRate * sinL + - vDDot * rotRate * cosL; - - // rectangular integration - vN += vNDot * dt; - vE += vEDot * dt; - vD += vDDot * dt; - lat += double(LDot * dt); - lon += double(lDot * dt); - alt += double(-vD * dt); - } - - return ret_ok; -} - -int KalmanNav::predictStateCovariance(float dt) -{ - using namespace math; - - // trig - float sinL = sinf(lat); - float cosL = cosf(lat); - float cosLSq = cosL * cosL; - float tanL = tanf(lat); - - // prepare for matrix - float R = R0 + float(alt); - float RSq = R * R; - - // F Matrix - // Titterton pg. 291 - - F(0, 1) = -(omega * sinL + vE * tanL / R); - F(0, 2) = vN / R; - F(0, 4) = 1.0f / R; - F(0, 6) = -omega * sinL; - F(0, 8) = -vE / RSq; - - F(1, 0) = omega * sinL + vE * tanL / R; - F(1, 2) = omega * cosL + vE / R; - F(1, 3) = -1.0f / R; - F(1, 8) = vN / RSq; - - F(2, 0) = -vN / R; - F(2, 1) = -omega * cosL - vE / R; - F(2, 4) = -tanL / R; - F(2, 6) = -omega * cosL - vE / (R * cosLSq); - F(2, 8) = vE * tanL / RSq; - - F(3, 1) = -fD; - F(3, 2) = fE; - F(3, 3) = vD / R; - F(3, 4) = -2 * (omega * sinL + vE * tanL / R); - F(3, 5) = vN / R; - F(3, 6) = -vE * (2 * omega * cosL + vE / (R * cosLSq)); - F(3, 8) = (vE * vE * tanL - vN * vD) / RSq; - - F(4, 0) = fD; - F(4, 2) = -fN; - F(4, 3) = 2 * omega * sinL + vE * tanL / R; - F(4, 4) = (vN * tanL + vD) / R; - F(4, 5) = 2 * omega * cosL + vE / R; - F(4, 6) = 2 * omega * (vN * cosL - vD * sinL) + - vN * vE / (R * cosLSq); - F(4, 8) = -vE * (vN * tanL + vD) / RSq; - - F(5, 0) = -fE; - F(5, 1) = fN; - F(5, 3) = -2 * vN / R; - F(5, 4) = -2 * (omega * cosL + vE / R); - F(5, 6) = 2 * omega * vE * sinL; - F(5, 8) = (vN * vN + vE * vE) / RSq; - - F(6, 3) = 1 / R; - F(6, 8) = -vN / RSq; - - F(7, 4) = 1 / (R * cosL); - F(7, 6) = vE * tanL / (R * cosL); - F(7, 8) = -vE / (cosL * RSq); - - F(8, 5) = -1; - - // G Matrix - // Titterton pg. 291 - G(0, 0) = -C_nb(0, 0); - G(0, 1) = -C_nb(0, 1); - G(0, 2) = -C_nb(0, 2); - G(1, 0) = -C_nb(1, 0); - G(1, 1) = -C_nb(1, 1); - G(1, 2) = -C_nb(1, 2); - G(2, 0) = -C_nb(2, 0); - G(2, 1) = -C_nb(2, 1); - G(2, 2) = -C_nb(2, 2); - - G(3, 3) = C_nb(0, 0); - G(3, 4) = C_nb(0, 1); - G(3, 5) = C_nb(0, 2); - G(4, 3) = C_nb(1, 0); - G(4, 4) = C_nb(1, 1); - G(4, 5) = C_nb(1, 2); - G(5, 3) = C_nb(2, 0); - G(5, 4) = C_nb(2, 1); - G(5, 5) = C_nb(2, 2); - - // continuous prediction equations - // for discrete time EKF - // http://en.wikipedia.org/wiki/Extended_Kalman_filter - P = P + (F * P + P * F.transposed() + G * V * G.transposed()) * dt; - - return ret_ok; -} - -int KalmanNav::correctAtt() -{ - using namespace math; - - // trig - float cosPhi = cosf(phi); - float cosTheta = cosf(theta); - // float cosPsi = cosf(psi); - float sinPhi = sinf(phi); - float sinTheta = sinf(theta); - // float sinPsi = sinf(psi); - - // mag predicted measurement - // choosing some typical magnetic field properties, - // TODO dip/dec depend on lat/ lon/ time - //float dip = _magDip.get() / M_RAD_TO_DEG_F; // dip, inclination with level - float dec = _magDec.get() / M_RAD_TO_DEG_F; // declination, clockwise rotation from north - - // compensate roll and pitch, but not yaw - // XXX take the vectors out of the C_nb matrix to avoid singularities - math::Matrix<3,3> C_rp; - C_rp.from_euler(phi, theta, 0.0f);//C_nb.transposed(); - - // mag measurement - Vector<3> magBody(_sensors.magnetometer_ga); - - // transform to earth frame - Vector<3> magNav = C_rp * magBody; - - // calculate error between estimate and measurement - // apply declination correction for true heading as well. - float yMag = -atan2f(magNav(1),magNav(0)) - psi - dec; - if (yMag > M_PI_F) yMag -= 2*M_PI_F; - if (yMag < -M_PI_F) yMag += 2*M_PI_F; - - // accel measurement - Vector<3> zAccel(_sensors.accelerometer_m_s2); - float accelMag = zAccel.length(); - zAccel.normalize(); - - // ignore accel correction when accel mag not close to g - Matrix<4,4> RAttAdjust = RAtt; - - bool ignoreAccel = fabsf(accelMag - _g.get()) > 1.1f; - - if (ignoreAccel) { - RAttAdjust(1, 1) = 1.0e10; - RAttAdjust(2, 2) = 1.0e10; - RAttAdjust(3, 3) = 1.0e10; - - } else { - //printf("correcting attitude with accel\n"); - } - - // accel predicted measurement - Vector<3> zAccelHat = (C_nb.transposed() * Vector<3>(0, 0, -_g.get())).normalized(); - - // calculate residual - Vector<4> y(yMag, zAccel(0) - zAccelHat(0), zAccel(1) - zAccelHat(1), zAccel(2) - zAccelHat(2)); - - // HMag - HAtt(0, 2) = 1; - - // HAccel - HAtt(1, 1) = cosTheta; - HAtt(2, 0) = -cosPhi * cosTheta; - HAtt(2, 1) = sinPhi * sinTheta; - HAtt(3, 0) = sinPhi * cosTheta; - HAtt(3, 1) = cosPhi * sinTheta; - - // compute correction - // http://en.wikipedia.org/wiki/Extended_Kalman_filter - Matrix<4, 4> S = HAtt * P * HAtt.transposed() + RAttAdjust; // residual covariance - Matrix<9, 4> K = P * HAtt.transposed() * S.inversed(); - Vector<9> xCorrect = K * y; - - // check correciton is sane - for (size_t i = 0; i < xCorrect.get_size(); i++) { - float val = xCorrect(i); - - if (isnan(val) || isinf(val)) { - // abort correction and return - warnx("numerical failure in att correction"); - // reset P matrix to P0 - P = P0; - return ret_error; - } - } - - // correct state - if (!ignoreAccel) { - phi += xCorrect(PHI); - theta += xCorrect(THETA); - } - - psi += xCorrect(PSI); - - // attitude also affects nav velocities - if (_positionInitialized) { - vN += xCorrect(VN); - vE += xCorrect(VE); - vD += xCorrect(VD); - } - - // update state covariance - // http://en.wikipedia.org/wiki/Extended_Kalman_filter - P = P - K * HAtt * P; - - // fault detection - float beta = y * (S.inversed() * y); - - if (beta > _faultAtt.get()) { - warnx("fault in attitude: beta = %8.4f", (double)beta); - warnx("y:"); y.print(); - } - - // update quaternions from euler - // angle correction - q.from_euler(phi, theta, psi); - - return ret_ok; -} - -int KalmanNav::correctPos() -{ - using namespace math; - - // residual - Vector<6> y; - y(0) = _gps.vel_n_m_s - vN; - y(1) = _gps.vel_e_m_s - vE; - y(2) = double(_gps.lat) - double(lat) * 1.0e7 * M_RAD_TO_DEG; - y(3) = double(_gps.lon) - double(lon) * 1.0e7 * M_RAD_TO_DEG; - y(4) = _gps.alt / 1.0e3f - alt; - y(5) = _sensors.baro_alt_meter - alt; - - // compute correction - // http://en.wikipedia.org/wiki/Extended_Kalman_filter - Matrix<6,6> S = HPos * P * HPos.transposed() + RPos; // residual covariance - Matrix<9,6> K = P * HPos.transposed() * S.inversed(); - Vector<9> xCorrect = K * y; - - // check correction is sane - for (size_t i = 0; i < xCorrect.get_size(); i++) { - float val = xCorrect(i); - - if (!isfinite(val)) { - // abort correction and return - warnx("numerical failure in gps correction"); - // fallback to GPS - vN = _gps.vel_n_m_s; - vE = _gps.vel_e_m_s; - vD = _gps.vel_d_m_s; - setLatDegE7(_gps.lat); - setLonDegE7(_gps.lon); - setAltE3(_gps.alt); - // reset P matrix to P0 - P = P0; - return ret_error; - } - } - - // correct state - vN += xCorrect(VN); - vE += xCorrect(VE); - vD += xCorrect(VD); - lat += double(xCorrect(LAT)); - lon += double(xCorrect(LON)); - alt += xCorrect(ALT); - - // update state covariance - // http://en.wikipedia.org/wiki/Extended_Kalman_filter - P = P - K * HPos * P; - - // fault detetcion - float beta = y * (S.inversed() * y); - - static int counter = 0; - if (beta > _faultPos.get() && (counter % 10 == 0)) { - warnx("fault in gps: beta = %8.4f", (double)beta); - warnx("Y/N: vN: %8.4f, vE: %8.4f, lat: %8.4f, lon: %8.4f, alt: %8.4f, baro: %8.4f", - double(y(0) / sqrtf(RPos(0, 0))), - double(y(1) / sqrtf(RPos(1, 1))), - double(y(2) / sqrtf(RPos(2, 2))), - double(y(3) / sqrtf(RPos(3, 3))), - double(y(4) / sqrtf(RPos(4, 4))), - double(y(5) / sqrtf(RPos(5, 5)))); - } - counter++; - - return ret_ok; -} - -void KalmanNav::updateParams() -{ - using namespace math; - using namespace control; - SuperBlock::updateParams(); - - // gyro noise - V(0, 0) = _vGyro.get(); // gyro x, rad/s - V(1, 1) = _vGyro.get(); // gyro y - V(2, 2) = _vGyro.get(); // gyro z - - // accel noise - V(3, 3) = _vAccel.get(); // accel x, m/s^2 - V(4, 4) = _vAccel.get(); // accel y - V(5, 5) = _vAccel.get(); // accel z - - // magnetometer noise - float noiseMin = 1e-6f; - float noiseMagSq = _rMag.get() * _rMag.get(); - - if (noiseMagSq < noiseMin) noiseMagSq = noiseMin; - - RAtt(0, 0) = noiseMagSq; // normalized direction - - // accelerometer noise - float noiseAccelSq = _rAccel.get() * _rAccel.get(); - - // bound noise to prevent singularities - if (noiseAccelSq < noiseMin) noiseAccelSq = noiseMin; - - RAtt(1, 1) = noiseAccelSq; // normalized direction - RAtt(2, 2) = noiseAccelSq; - RAtt(3, 3) = noiseAccelSq; - - // gps noise - float R = R0 + float(alt); - float cosLSing = cosf(lat); - - // prevent singularity - if (fabsf(cosLSing) < 0.01f) { - if (cosLSing > 0) cosLSing = 0.01; - else cosLSing = -0.01; - } - - float noiseVel = _rGpsVel.get(); - float noiseLatDegE7 = 1.0e7f * M_RAD_TO_DEG_F * _rGpsPos.get() / R; - float noiseLonDegE7 = noiseLatDegE7 / cosLSing; - float noiseGpsAlt = _rGpsAlt.get(); - float noisePressAlt = _rPressAlt.get(); - - // bound noise to prevent singularities - if (noiseVel < noiseMin) noiseVel = noiseMin; - - if (noiseLatDegE7 < noiseMin) noiseLatDegE7 = noiseMin; - - if (noiseLonDegE7 < noiseMin) noiseLonDegE7 = noiseMin; - - if (noiseGpsAlt < noiseMin) noiseGpsAlt = noiseMin; - - if (noisePressAlt < noiseMin) noisePressAlt = noiseMin; - - RPos(0, 0) = noiseVel * noiseVel; // vn - RPos(1, 1) = noiseVel * noiseVel; // ve - RPos(2, 2) = noiseLatDegE7 * noiseLatDegE7; // lat - RPos(3, 3) = noiseLonDegE7 * noiseLonDegE7; // lon - RPos(4, 4) = noiseGpsAlt * noiseGpsAlt; // h - RPos(5, 5) = noisePressAlt * noisePressAlt; // h - // XXX, note that RPos depends on lat, so updateParams should - // be called if lat changes significantly -} diff --git a/src/modules/att_pos_estimator_ekf/KalmanNav.hpp b/src/modules/att_pos_estimator_ekf/KalmanNav.hpp deleted file mode 100644 index caf93bc787..0000000000 --- a/src/modules/att_pos_estimator_ekf/KalmanNav.hpp +++ /dev/null @@ -1,192 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -/** - * @file KalmanNav.hpp - * - * kalman filter navigation code - */ - -#pragma once - -//#define MATRIX_ASSERT -//#define VECTOR_ASSERT - -#include - -#include -#include -#include -#include -#include - -#include -#include -#include -#include -#include -#include - -#include -#include -#include - -#include -#include -#include - -/** - * Kalman filter navigation class - * http://en.wikipedia.org/wiki/Extended_Kalman_filter - * Discrete-time extended Kalman filter - */ -class KalmanNav : public control::SuperBlock -{ -public: - /** - * Constructor - */ - KalmanNav(SuperBlock *parent, const char *name); - - /** - * Deconstuctor - */ - - virtual ~KalmanNav() {}; - - math::Quaternion init(float ax, float ay, float az, float mx, float my, float mz); - - /** - * The main callback function for the class - */ - void update(); - - - /** - * Publication update - */ - virtual void updatePublications(); - - /** - * State prediction - * Continuous, non-linear - */ - int predictState(float dt); - - /** - * State covariance prediction - * Continuous, linear - */ - int predictStateCovariance(float dt); - - /** - * Attitude correction - */ - int correctAtt(); - - /** - * Position correction - */ - int correctPos(); - - /** - * Overloaded update parameters - */ - virtual void updateParams(); -protected: - // kalman filter - math::Matrix<9,9> F; /**< Jacobian(f,x), where dx/dt = f(x,u) */ - math::Matrix<9,6> G; /**< noise shaping matrix for gyro/accel */ - math::Matrix<9,9> P; /**< state covariance matrix */ - math::Matrix<9,9> P0; /**< initial state covariance matrix */ - math::Matrix<6,6> V; /**< gyro/ accel noise matrix */ - math::Matrix<4,9> HAtt; /**< attitude measurement matrix */ - math::Matrix<4,4> RAtt; /**< attitude measurement noise matrix */ - math::Matrix<6,9> HPos; /**< position measurement jacobian matrix */ - math::Matrix<6,6> RPos; /**< position measurement noise matrix */ - // attitude - math::Matrix<3,3> C_nb; /**< direction cosine matrix from body to nav frame */ - math::Quaternion q; /**< quaternion from body to nav frame */ - // subscriptions - uORB::Subscription _sensors; /**< sensors sub. */ - uORB::Subscription _gps; /**< gps sub. */ - uORB::Subscription _param_update; /**< parameter update sub. */ - // publications - uORB::Publication _pos; /**< position pub. */ - uORB::Publication _localPos; /**< local position pub. */ - uORB::Publication _att; /**< attitude pub. */ - // time stamps - uint64_t _pubTimeStamp; /**< output data publication time stamp */ - uint64_t _predictTimeStamp; /**< prediction time stamp */ - uint64_t _attTimeStamp; /**< attitude correction time stamp */ - uint64_t _outTimeStamp; /**< output time stamp */ - // frame count - uint16_t _navFrames; /**< navigation frames completed in output cycle */ - // miss counts - uint16_t _miss; /**< number of times fast prediction loop missed */ - // accelerations - float fN, fE, fD; /**< navigation frame acceleration */ - // states - enum {PHI = 0, THETA, PSI, VN, VE, VD, LAT, LON, ALT}; /**< state enumeration */ - float phi, theta, psi; /**< 3-2-1 euler angles */ - float vN, vE, vD; /**< navigation velocity, m/s */ - double lat, lon; /**< lat, lon radians */ - // parameters - float alt; /**< altitude, meters */ - double lat0, lon0; /**< reference latitude and longitude */ - float alt0; /**< refeerence altitude (ground height) */ - control::BlockParamFloat _vGyro; /**< gyro process noise */ - control::BlockParamFloat _vAccel; /**< accelerometer process noise */ - control::BlockParamFloat _rMag; /**< magnetometer measurement noise */ - control::BlockParamFloat _rGpsVel; /**< gps velocity measurement noise */ - control::BlockParamFloat _rGpsPos; /**< gps position measurement noise */ - control::BlockParamFloat _rGpsAlt; /**< gps altitude measurement noise */ - control::BlockParamFloat _rPressAlt; /**< press altitude measurement noise */ - control::BlockParamFloat _rAccel; /**< accelerometer measurement noise */ - control::BlockParamFloat _magDip; /**< magnetic inclination with level */ - control::BlockParamFloat _magDec; /**< magnetic declination, clockwise rotation */ - control::BlockParamFloat _g; /**< gravitational constant */ - control::BlockParamFloat _faultPos; /**< fault detection threshold for position */ - control::BlockParamFloat _faultAtt; /**< fault detection threshold for attitude */ - // status - bool _attitudeInitialized; - bool _positionInitialized; - uint16_t _attitudeInitCounter; - // accessors - int32_t getLatDegE7() { return int32_t(lat * 1.0e7 * M_RAD_TO_DEG); } - void setLatDegE7(int32_t val) { lat = val / 1.0e7 / M_RAD_TO_DEG; } - int32_t getLonDegE7() { return int32_t(lon * 1.0e7 * M_RAD_TO_DEG); } - void setLonDegE7(int32_t val) { lon = val / 1.0e7 / M_RAD_TO_DEG; } - int32_t getAltE3() { return int32_t(alt * 1.0e3); } - void setAltE3(int32_t val) { alt = double(val) / 1.0e3; } -}; diff --git a/src/modules/att_pos_estimator_ekf/kalman_main.cpp b/src/modules/att_pos_estimator_ekf/kalman_main.cpp deleted file mode 100644 index 3d20d4d2d6..0000000000 --- a/src/modules/att_pos_estimator_ekf/kalman_main.cpp +++ /dev/null @@ -1,157 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. - * Author: James Goppert - * - * 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 kalman_main.cpp - * Combined attitude / position estimator. - * - * @author James Goppert - */ - -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include "KalmanNav.hpp" - -static bool thread_should_exit = false; /**< Deamon exit flag */ -static bool thread_running = false; /**< Deamon status flag */ -static int daemon_task; /**< Handle of deamon task / thread */ - -/** - * Deamon management function. - */ -extern "C" __EXPORT int att_pos_estimator_ekf_main(int argc, char *argv[]); - -/** - * Mainloop of deamon. - */ -int kalman_demo_thread_main(int argc, char *argv[]); - -/** - * Print the correct usage. - */ -static void usage(const char *reason); - -static void -usage(const char *reason) -{ - if (reason) - fprintf(stderr, "%s\n", reason); - - warnx("usage: att_pos_estimator_ekf {start|stop|status} [-p ]"); - exit(1); -} - -/** - * The deamon 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 att_pos_estimator_ekf_main(int argc, char *argv[]) -{ - - if (argc < 1) - usage("missing command"); - - if (!strcmp(argv[1], "start")) { - - if (thread_running) { - warnx("already running"); - /* this is not an error */ - exit(0); - } - - thread_should_exit = false; - - daemon_task = task_spawn_cmd("att_pos_estimator_ekf", - SCHED_DEFAULT, - SCHED_PRIORITY_MAX - 30, - 8192, - kalman_demo_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("is running\n"); - exit(0); - - } else { - warnx("not started\n"); - exit(1); - } - - } - - usage("unrecognized command"); - exit(1); -} - -int kalman_demo_thread_main(int argc, char *argv[]) -{ - - warnx("starting"); - - using namespace math; - - thread_running = true; - - KalmanNav nav(NULL, "KF"); - - while (!thread_should_exit) { - nav.update(); - } - - warnx("exiting."); - - thread_running = false; - - return 0; -} diff --git a/src/modules/att_pos_estimator_ekf/module.mk b/src/modules/att_pos_estimator_ekf/module.mk deleted file mode 100644 index 8d4a40d950..0000000000 --- a/src/modules/att_pos_estimator_ekf/module.mk +++ /dev/null @@ -1,42 +0,0 @@ -############################################################################ -# -# Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. -# -# Redistribution and use in source and binary forms, with or without -# modification, are permitted provided that the following conditions -# are met: -# -# 1. Redistributions of source code must retain the above copyright -# notice, this list of conditions and the following disclaimer. -# 2. Redistributions in binary form must reproduce the above copyright -# notice, this list of conditions and the following disclaimer in -# the documentation and/or other materials provided with the -# distribution. -# 3. Neither the name PX4 nor the names of its contributors may be -# used to endorse or promote products derived from this software -# without specific prior written permission. -# -# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS -# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT -# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS -# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE -# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, -# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, -# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS -# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED -# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT -# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN -# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE -# POSSIBILITY OF SUCH DAMAGE. -# -############################################################################ - -# -# Full attitude / position Extended Kalman Filter -# - -MODULE_COMMAND = att_pos_estimator_ekf - -SRCS = kalman_main.cpp \ - KalmanNav.cpp \ - params.c diff --git a/src/modules/att_pos_estimator_ekf/params.c b/src/modules/att_pos_estimator_ekf/params.c deleted file mode 100644 index 4af5edeade..0000000000 --- a/src/modules/att_pos_estimator_ekf/params.c +++ /dev/null @@ -1,49 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2012, 2013 PX4 Development Team. All rights reserved. - * - * Redistribution and use in source and binary forms, with or without - * modification, are permitted provided that the following conditions - * are met: - * - * 1. Redistributions of source code must retain the above copyright - * notice, this list of conditions and the following disclaimer. - * 2. Redistributions in binary form must reproduce the above copyright - * notice, this list of conditions and the following disclaimer in - * the documentation and/or other materials provided with the - * distribution. - * 3. Neither the name PX4 nor the names of its contributors may be - * used to endorse or promote products derived from this software - * without specific prior written permission. - * - * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS - * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS - * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE - * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, - * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, - * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS - * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED - * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT - * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN - * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE - * POSSIBILITY OF SUCH DAMAGE. - * - ****************************************************************************/ - -#include - -/*PARAM_DEFINE_FLOAT(NAME,0.0f);*/ -PARAM_DEFINE_FLOAT(KF_V_GYRO, 0.008f); -PARAM_DEFINE_FLOAT(KF_V_ACCEL, 1.0f); -PARAM_DEFINE_FLOAT(KF_R_MAG, 0.8f); -PARAM_DEFINE_FLOAT(KF_R_GPS_VEL, 0.5f); -PARAM_DEFINE_FLOAT(KF_R_GPS_POS, 2.0f); -PARAM_DEFINE_FLOAT(KF_R_GPS_ALT, 3.0f); -PARAM_DEFINE_FLOAT(KF_R_PRESS_ALT, 0.1f); -PARAM_DEFINE_FLOAT(KF_R_ACCEL, 1.0f); -PARAM_DEFINE_FLOAT(KF_FAULT_POS, 10.0f); -PARAM_DEFINE_FLOAT(KF_FAULT_ATT, 10.0f); -PARAM_DEFINE_FLOAT(KF_ENV_G, 9.765f); -PARAM_DEFINE_FLOAT(KF_ENV_MAG_DIP, 60.0f); -PARAM_DEFINE_FLOAT(KF_ENV_MAG_DEC, 0.0f); From ce56d75bc606015728f59a3e811fa48ff9db2979 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 20 Apr 2014 01:37:31 +0200 Subject: [PATCH 034/329] Updated filter to most recent version with accel scale estimation, exposed crucial parameters for cross-vehicle support --- .../ekf_att_pos_estimator/estimator.cpp | 1610 +++++++++++------ src/modules/ekf_att_pos_estimator/estimator.h | 120 +- .../fw_att_pos_estimator_main.cpp | 25 +- .../fw_att_pos_estimator_params.c | 14 +- 4 files changed, 1188 insertions(+), 581 deletions(-) diff --git a/src/modules/ekf_att_pos_estimator/estimator.cpp b/src/modules/ekf_att_pos_estimator/estimator.cpp index c312173938..ebe30cae0e 100644 --- a/src/modules/ekf_att_pos_estimator/estimator.cpp +++ b/src/modules/ekf_att_pos_estimator/estimator.cpp @@ -1,5 +1,5 @@ #include "estimator.h" - +//#include #include float Vector3f::length(void) const @@ -69,6 +69,25 @@ Vector3f operator*( Mat3f matIn, Vector3f vecIn) return vecOut; } +// overload * operator to provide a matrix product +Mat3f operator*( Mat3f matIn1, Mat3f matIn2) +{ + Mat3f matOut; + matOut.x.x = matIn1.x.x*matIn2.x.x + matIn1.x.y*matIn2.y.x + matIn1.x.z*matIn2.z.x; + matOut.x.y = matIn1.x.x*matIn2.x.y + matIn1.x.y*matIn2.y.y + matIn1.x.z*matIn2.z.y; + matOut.x.z = matIn1.x.x*matIn2.x.z + matIn1.x.y*matIn2.y.z + matIn1.x.z*matIn2.z.z; + + matOut.y.x = matIn1.y.x*matIn2.x.x + matIn1.y.y*matIn2.y.x + matIn1.y.z*matIn2.z.x; + matOut.y.y = matIn1.y.x*matIn2.x.y + matIn1.y.y*matIn2.y.y + matIn1.y.z*matIn2.z.y; + matOut.y.z = matIn1.y.x*matIn2.x.z + matIn1.y.y*matIn2.y.z + matIn1.y.z*matIn2.z.z; + + matOut.z.x = matIn1.z.x*matIn2.x.x + matIn1.z.y*matIn2.y.x + matIn1.z.z*matIn2.z.x; + matOut.z.y = matIn1.z.x*matIn2.x.y + matIn1.z.y*matIn2.y.y + matIn1.z.z*matIn2.z.y; + matOut.z.z = matIn1.z.x*matIn2.x.z + matIn1.z.y*matIn2.y.z + matIn1.z.z*matIn2.z.z; + + return matOut; +} + // overload % operator to provide a vector cross product Vector3f operator%( Vector3f vecIn1, Vector3f vecIn2) { @@ -109,7 +128,6 @@ void swap_var(float &d1, float &d2) AttPosEKF::AttPosEKF() : fusionModeGPS(0), covSkipCount(0), - EAS2TAS(1.0f), statesInitialised(false), fuseVelData(false), fusePosData(false), @@ -120,10 +138,11 @@ AttPosEKF::AttPosEKF() : staticMode(true), useAirspeed(true), useCompass(true), + useRangeFinder(true), numericalProtection(true), storeIndex(0) { - + InitialiseParameters(); } AttPosEKF::~AttPosEKF() @@ -143,8 +162,6 @@ void AttPosEKF::UpdateStrapdownEquationsNED() float q12; float q13; float q23; - Mat3f Tbn; - Mat3f Tnb; float rotationMag; float qUpdated[4]; float quatMag; @@ -157,7 +174,7 @@ void AttPosEKF::UpdateStrapdownEquationsNED() correctedDelAng.z = dAngIMU.z - states[12]; dVelIMU.x = dVelIMU.x; dVelIMU.y = dVelIMU.y; - dVelIMU.z = dVelIMU.z; + dVelIMU.z = dVelIMU.z - states[13]; // Save current measurements Vector3f prevDelAng = correctedDelAng; @@ -177,8 +194,8 @@ void AttPosEKF::UpdateStrapdownEquationsNED() } else { - deltaQuat[0] = cos(0.5f*rotationMag); - float rotScaler = (sin(0.5f*rotationMag))/rotationMag; + deltaQuat[0] = cosf(0.5f*rotationMag); + float rotScaler = (sinf(0.5f*rotationMag))/rotationMag; deltaQuat[1] = correctedDelAng.x*rotScaler; deltaQuat[2] = correctedDelAng.y*rotScaler; deltaQuat[3] = correctedDelAng.z*rotScaler; @@ -260,11 +277,6 @@ void AttPosEKF::UpdateStrapdownEquationsNED() void AttPosEKF::CovariancePrediction(float dt) { // scalars - float windVelSigma; - float dAngBiasSigma; - // float dVelBiasSigma; - float magEarthSigma; - float magBodySigma; float daxCov; float dayCov; float dazCov; @@ -284,28 +296,29 @@ void AttPosEKF::CovariancePrediction(float dt) float dax_b; float day_b; float daz_b; + float dvz_b; // arrays - float processNoise[21]; - float SF[14]; + float processNoise[n_states]; + float SF[15]; float SG[8]; float SQ[11]; - float SPP[13] = {0}; - float nextP[21][21]; + float SPP[8] = {0}; + float nextP[n_states][n_states]; // calculate covariance prediction process noise - const float yawVarScale = 1.0f; - windVelSigma = dt*0.1f; - dAngBiasSigma = dt*5.0e-7f; - magEarthSigma = dt*3.0e-4f; - magBodySigma = dt*3.0e-4f; - for (uint8_t i= 0; i<=9; i++) processNoise[i] = 1.0e-9f; - for (uint8_t i=10; i<=12; i++) processNoise[i] = dAngBiasSigma; - if (onGround) processNoise[12] = dAngBiasSigma * yawVarScale; - for (uint8_t i=13; i<=14; i++) processNoise[i] = windVelSigma; - for (uint8_t i=15; i<=17; i++) processNoise[i] = magEarthSigma; - for (uint8_t i=18; i<=20; i++) processNoise[i] = magBodySigma; - for (uint8_t i= 0; i<=20; i++) processNoise[i] = sq(processNoise[i]); + for (uint8_t i= 0; i<=9; i++) processNoise[i] = 1.0e-9f; + for (uint8_t i=10; i<=12; i++) processNoise[i] = dt * dAngBiasSigma; + // scale gyro bias noise when on ground to allow for faster bias estimation + for (uint8_t i=10; i<=12; i++) processNoise[i] = dt * dAngBiasSigma; + processNoise[13] = dVelBiasSigma; + for (uint8_t i=14; i<=15; i++) processNoise[i] = dt * windVelSigma; + for (uint8_t i=16; i<=18; i++) processNoise[i] = dt * magEarthSigma; + for (uint8_t i=19; i<=21; i++) processNoise[i] = dt * magBodySigma; + processNoise[22] = dt * sqrtf(sq(states[4]) + sq(states[5])) * gndHgtSigma; + + // square all sigmas + for (unsigned i = 0; i < n_states; i++) processNoise[i] = sq(processNoise[i]); // set variables used to calculate covariance growth dvx = summedDelVel.x; @@ -321,29 +334,33 @@ void AttPosEKF::CovariancePrediction(float dt) dax_b = states[10]; day_b = states[11]; daz_b = states[12]; - daxCov = sq(dt*1.4544411e-2f); - dayCov = sq(dt*1.4544411e-2f); - dazCov = sq(dt*1.4544411e-2f); + dvz_b = states[13]; + gyroProcessNoise = ConstrainFloat(gyroProcessNoise, 1e-3f, 5e-2f); + daxCov = sq(dt*gyroProcessNoise); + dayCov = sq(dt*gyroProcessNoise); + dazCov = sq(dt*gyroProcessNoise); if (onGround) dazCov = dazCov * sq(yawVarScale); - dvxCov = sq(dt*0.5f); - dvyCov = sq(dt*0.5f); - dvzCov = sq(dt*0.5f); + accelProcessNoise = ConstrainFloat(accelProcessNoise, 5e-2, 1.0f); + dvxCov = sq(dt*accelProcessNoise); + dvyCov = sq(dt*accelProcessNoise); + dvzCov = sq(dt*accelProcessNoise); // Predicted covariance calculation - SF[0] = 2*dvx*q1 + 2*dvy*q2 + 2*dvz*q3; - SF[1] = 2*dvx*q3 + 2*dvy*q0 - 2*dvz*q1; - SF[2] = 2*dvx*q0 - 2*dvy*q3 + 2*dvz*q2; - SF[3] = day/2 - day_b/2; - SF[4] = daz/2 - daz_b/2; - SF[5] = dax/2 - dax_b/2; - SF[6] = dax_b/2 - dax/2; - SF[7] = daz_b/2 - daz/2; - SF[8] = day_b/2 - day/2; - SF[9] = q1/2; - SF[10] = q2/2; - SF[11] = q3/2; - SF[12] = 2*dvz*q0; - SF[13] = 2*dvy*q1; + SF[0] = dvz - dvz_b; + SF[1] = 2*q3*SF[0] + 2*dvx*q1 + 2*dvy*q2; + SF[2] = 2*dvx*q3 - 2*q1*SF[0] + 2*dvy*q0; + SF[3] = 2*q2*SF[0] + 2*dvx*q0 - 2*dvy*q3; + SF[4] = day/2 - day_b/2; + SF[5] = daz/2 - daz_b/2; + SF[6] = dax/2 - dax_b/2; + SF[7] = dax_b/2 - dax/2; + SF[8] = daz_b/2 - daz/2; + SF[9] = day_b/2 - day/2; + SF[10] = 2*q0*SF[0]; + SF[11] = q1/2; + SF[12] = q2/2; + SF[13] = q3/2; + SF[14] = 2*dvy*q1; SG[0] = q0/2; SG[1] = sq(q3); @@ -366,169 +383,183 @@ void AttPosEKF::CovariancePrediction(float dt) SQ[9] = sq(SG[0]); SQ[10] = sq(q1); - SPP[0] = SF[12] + SF[13] - 2*dvx*q2; - SPP[1] = 2*dvx*q0 - 2*dvy*q3 + 2*dvz*q2; - SPP[2] = 2*dvx*q3 + 2*dvy*q0 - 2*dvz*q1; - SPP[3] = SF[11]; - SPP[4] = SF[10]; - SPP[5] = SF[9]; - SPP[6] = SF[7]; - SPP[7] = SF[8]; + SPP[0] = SF[10] + SF[14] - 2*dvx*q2; + SPP[1] = 2*q2*SF[0] + 2*dvx*q0 - 2*dvy*q3; + SPP[2] = 2*dvx*q3 - 2*q1*SF[0] + 2*dvy*q0; + SPP[3] = 2*q0*q1 - 2*q2*q3; + SPP[4] = 2*q0*q2 + 2*q1*q3; + SPP[5] = sq(q0) - sq(q1) - sq(q2) + sq(q3); + SPP[6] = SF[13]; + SPP[7] = SF[12]; - nextP[0][0] = P[0][0] + P[1][0]*SF[6] + P[2][0]*SPP[7] + P[3][0]*SPP[6] + P[10][0]*SPP[5] + P[11][0]*SPP[4] + P[12][0]*SPP[3] + (daxCov*SQ[10])/4 + SF[6]*(P[0][1] + P[1][1]*SF[6] + P[2][1]*SPP[7] + P[3][1]*SPP[6] + P[10][1]*SPP[5] + P[11][1]*SPP[4] + P[12][1]*SPP[3]) + SPP[7]*(P[0][2] + P[1][2]*SF[6] + P[2][2]*SPP[7] + P[3][2]*SPP[6] + P[10][2]*SPP[5] + P[11][2]*SPP[4] + P[12][2]*SPP[3]) + SPP[6]*(P[0][3] + P[1][3]*SF[6] + P[2][3]*SPP[7] + P[3][3]*SPP[6] + P[10][3]*SPP[5] + P[11][3]*SPP[4] + P[12][3]*SPP[3]) + SPP[5]*(P[0][10] + P[1][10]*SF[6] + P[2][10]*SPP[7] + P[3][10]*SPP[6] + P[10][10]*SPP[5] + P[11][10]*SPP[4] + P[12][10]*SPP[3]) + SPP[4]*(P[0][11] + P[1][11]*SF[6] + P[2][11]*SPP[7] + P[3][11]*SPP[6] + P[10][11]*SPP[5] + P[11][11]*SPP[4] + P[12][11]*SPP[3]) + SPP[3]*(P[0][12] + P[1][12]*SF[6] + P[2][12]*SPP[7] + P[3][12]*SPP[6] + P[10][12]*SPP[5] + P[11][12]*SPP[4] + P[12][12]*SPP[3]) + (dayCov*sq(q2))/4 + (dazCov*sq(q3))/4; - nextP[0][1] = P[0][1] + SQ[8] + P[1][1]*SF[6] + P[2][1]*SPP[7] + P[3][1]*SPP[6] + P[10][1]*SPP[5] + P[11][1]*SPP[4] + P[12][1]*SPP[3] + SF[5]*(P[0][0] + P[1][0]*SF[6] + P[2][0]*SPP[7] + P[3][0]*SPP[6] + P[10][0]*SPP[5] + P[11][0]*SPP[4] + P[12][0]*SPP[3]) + SF[4]*(P[0][2] + P[1][2]*SF[6] + P[2][2]*SPP[7] + P[3][2]*SPP[6] + P[10][2]*SPP[5] + P[11][2]*SPP[4] + P[12][2]*SPP[3]) + SPP[7]*(P[0][3] + P[1][3]*SF[6] + P[2][3]*SPP[7] + P[3][3]*SPP[6] + P[10][3]*SPP[5] + P[11][3]*SPP[4] + P[12][3]*SPP[3]) + SPP[3]*(P[0][11] + P[1][11]*SF[6] + P[2][11]*SPP[7] + P[3][11]*SPP[6] + P[10][11]*SPP[5] + P[11][11]*SPP[4] + P[12][11]*SPP[3]) - SPP[4]*(P[0][12] + P[1][12]*SF[6] + P[2][12]*SPP[7] + P[3][12]*SPP[6] + P[10][12]*SPP[5] + P[11][12]*SPP[4] + P[12][12]*SPP[3]) - (q0*(P[0][10] + P[1][10]*SF[6] + P[2][10]*SPP[7] + P[3][10]*SPP[6] + P[10][10]*SPP[5] + P[11][10]*SPP[4] + P[12][10]*SPP[3]))/2; - nextP[0][2] = P[0][2] + SQ[7] + P[1][2]*SF[6] + P[2][2]*SPP[7] + P[3][2]*SPP[6] + P[10][2]*SPP[5] + P[11][2]*SPP[4] + P[12][2]*SPP[3] + SF[3]*(P[0][0] + P[1][0]*SF[6] + P[2][0]*SPP[7] + P[3][0]*SPP[6] + P[10][0]*SPP[5] + P[11][0]*SPP[4] + P[12][0]*SPP[3]) + SF[5]*(P[0][3] + P[1][3]*SF[6] + P[2][3]*SPP[7] + P[3][3]*SPP[6] + P[10][3]*SPP[5] + P[11][3]*SPP[4] + P[12][3]*SPP[3]) + SPP[6]*(P[0][1] + P[1][1]*SF[6] + P[2][1]*SPP[7] + P[3][1]*SPP[6] + P[10][1]*SPP[5] + P[11][1]*SPP[4] + P[12][1]*SPP[3]) - SPP[3]*(P[0][10] + P[1][10]*SF[6] + P[2][10]*SPP[7] + P[3][10]*SPP[6] + P[10][10]*SPP[5] + P[11][10]*SPP[4] + P[12][10]*SPP[3]) + SPP[5]*(P[0][12] + P[1][12]*SF[6] + P[2][12]*SPP[7] + P[3][12]*SPP[6] + P[10][12]*SPP[5] + P[11][12]*SPP[4] + P[12][12]*SPP[3]) - (q0*(P[0][11] + P[1][11]*SF[6] + P[2][11]*SPP[7] + P[3][11]*SPP[6] + P[10][11]*SPP[5] + P[11][11]*SPP[4] + P[12][11]*SPP[3]))/2; - nextP[0][3] = P[0][3] + SQ[6] + P[1][3]*SF[6] + P[2][3]*SPP[7] + P[3][3]*SPP[6] + P[10][3]*SPP[5] + P[11][3]*SPP[4] + P[12][3]*SPP[3] + SF[4]*(P[0][0] + P[1][0]*SF[6] + P[2][0]*SPP[7] + P[3][0]*SPP[6] + P[10][0]*SPP[5] + P[11][0]*SPP[4] + P[12][0]*SPP[3]) + SF[3]*(P[0][1] + P[1][1]*SF[6] + P[2][1]*SPP[7] + P[3][1]*SPP[6] + P[10][1]*SPP[5] + P[11][1]*SPP[4] + P[12][1]*SPP[3]) + SF[6]*(P[0][2] + P[1][2]*SF[6] + P[2][2]*SPP[7] + P[3][2]*SPP[6] + P[10][2]*SPP[5] + P[11][2]*SPP[4] + P[12][2]*SPP[3]) + SPP[4]*(P[0][10] + P[1][10]*SF[6] + P[2][10]*SPP[7] + P[3][10]*SPP[6] + P[10][10]*SPP[5] + P[11][10]*SPP[4] + P[12][10]*SPP[3]) - SPP[5]*(P[0][11] + P[1][11]*SF[6] + P[2][11]*SPP[7] + P[3][11]*SPP[6] + P[10][11]*SPP[5] + P[11][11]*SPP[4] + P[12][11]*SPP[3]) - (q0*(P[0][12] + P[1][12]*SF[6] + P[2][12]*SPP[7] + P[3][12]*SPP[6] + P[10][12]*SPP[5] + P[11][12]*SPP[4] + P[12][12]*SPP[3]))/2; - nextP[0][4] = P[0][4] + P[1][4]*SF[6] + P[2][4]*SPP[7] + P[3][4]*SPP[6] + P[10][4]*SPP[5] + P[11][4]*SPP[4] + P[12][4]*SPP[3] + SF[2]*(P[0][0] + P[1][0]*SF[6] + P[2][0]*SPP[7] + P[3][0]*SPP[6] + P[10][0]*SPP[5] + P[11][0]*SPP[4] + P[12][0]*SPP[3]) + SF[0]*(P[0][1] + P[1][1]*SF[6] + P[2][1]*SPP[7] + P[3][1]*SPP[6] + P[10][1]*SPP[5] + P[11][1]*SPP[4] + P[12][1]*SPP[3]) + SPP[0]*(P[0][2] + P[1][2]*SF[6] + P[2][2]*SPP[7] + P[3][2]*SPP[6] + P[10][2]*SPP[5] + P[11][2]*SPP[4] + P[12][2]*SPP[3]) - SPP[2]*(P[0][3] + P[1][3]*SF[6] + P[2][3]*SPP[7] + P[3][3]*SPP[6] + P[10][3]*SPP[5] + P[11][3]*SPP[4] + P[12][3]*SPP[3]); - nextP[0][5] = P[0][5] + P[1][5]*SF[6] + P[2][5]*SPP[7] + P[3][5]*SPP[6] + P[10][5]*SPP[5] + P[11][5]*SPP[4] + P[12][5]*SPP[3] + SF[1]*(P[0][0] + P[1][0]*SF[6] + P[2][0]*SPP[7] + P[3][0]*SPP[6] + P[10][0]*SPP[5] + P[11][0]*SPP[4] + P[12][0]*SPP[3]) + SF[0]*(P[0][2] + P[1][2]*SF[6] + P[2][2]*SPP[7] + P[3][2]*SPP[6] + P[10][2]*SPP[5] + P[11][2]*SPP[4] + P[12][2]*SPP[3]) + SF[2]*(P[0][3] + P[1][3]*SF[6] + P[2][3]*SPP[7] + P[3][3]*SPP[6] + P[10][3]*SPP[5] + P[11][3]*SPP[4] + P[12][3]*SPP[3]) - SPP[0]*(P[0][1] + P[1][1]*SF[6] + P[2][1]*SPP[7] + P[3][1]*SPP[6] + P[10][1]*SPP[5] + P[11][1]*SPP[4] + P[12][1]*SPP[3]); - nextP[0][6] = P[0][6] + P[1][6]*SF[6] + P[2][6]*SPP[7] + P[3][6]*SPP[6] + P[10][6]*SPP[5] + P[11][6]*SPP[4] + P[12][6]*SPP[3] + SF[1]*(P[0][1] + P[1][1]*SF[6] + P[2][1]*SPP[7] + P[3][1]*SPP[6] + P[10][1]*SPP[5] + P[11][1]*SPP[4] + P[12][1]*SPP[3]) + SF[0]*(P[0][3] + P[1][3]*SF[6] + P[2][3]*SPP[7] + P[3][3]*SPP[6] + P[10][3]*SPP[5] + P[11][3]*SPP[4] + P[12][3]*SPP[3]) + SPP[0]*(P[0][0] + P[1][0]*SF[6] + P[2][0]*SPP[7] + P[3][0]*SPP[6] + P[10][0]*SPP[5] + P[11][0]*SPP[4] + P[12][0]*SPP[3]) - SPP[1]*(P[0][2] + P[1][2]*SF[6] + P[2][2]*SPP[7] + P[3][2]*SPP[6] + P[10][2]*SPP[5] + P[11][2]*SPP[4] + P[12][2]*SPP[3]); - nextP[0][7] = P[0][7] + P[1][7]*SF[6] + P[2][7]*SPP[7] + P[3][7]*SPP[6] + P[10][7]*SPP[5] + P[11][7]*SPP[4] + P[12][7]*SPP[3] + dt*(P[0][4] + P[1][4]*SF[6] + P[2][4]*SPP[7] + P[3][4]*SPP[6] + P[10][4]*SPP[5] + P[11][4]*SPP[4] + P[12][4]*SPP[3]); - nextP[0][8] = P[0][8] + P[1][8]*SF[6] + P[2][8]*SPP[7] + P[3][8]*SPP[6] + P[10][8]*SPP[5] + P[11][8]*SPP[4] + P[12][8]*SPP[3] + dt*(P[0][5] + P[1][5]*SF[6] + P[2][5]*SPP[7] + P[3][5]*SPP[6] + P[10][5]*SPP[5] + P[11][5]*SPP[4] + P[12][5]*SPP[3]); - nextP[0][9] = P[0][9] + P[1][9]*SF[6] + P[2][9]*SPP[7] + P[3][9]*SPP[6] + P[10][9]*SPP[5] + P[11][9]*SPP[4] + P[12][9]*SPP[3] + dt*(P[0][6] + P[1][6]*SF[6] + P[2][6]*SPP[7] + P[3][6]*SPP[6] + P[10][6]*SPP[5] + P[11][6]*SPP[4] + P[12][6]*SPP[3]); - nextP[0][10] = P[0][10] + P[1][10]*SF[6] + P[2][10]*SPP[7] + P[3][10]*SPP[6] + P[10][10]*SPP[5] + P[11][10]*SPP[4] + P[12][10]*SPP[3]; - nextP[0][11] = P[0][11] + P[1][11]*SF[6] + P[2][11]*SPP[7] + P[3][11]*SPP[6] + P[10][11]*SPP[5] + P[11][11]*SPP[4] + P[12][11]*SPP[3]; - nextP[0][12] = P[0][12] + P[1][12]*SF[6] + P[2][12]*SPP[7] + P[3][12]*SPP[6] + P[10][12]*SPP[5] + P[11][12]*SPP[4] + P[12][12]*SPP[3]; - nextP[0][13] = P[0][13] + P[1][13]*SF[6] + P[2][13]*SPP[7] + P[3][13]*SPP[6] + P[10][13]*SPP[5] + P[11][13]*SPP[4] + P[12][13]*SPP[3]; - nextP[0][14] = P[0][14] + P[1][14]*SF[6] + P[2][14]*SPP[7] + P[3][14]*SPP[6] + P[10][14]*SPP[5] + P[11][14]*SPP[4] + P[12][14]*SPP[3]; - nextP[0][15] = P[0][15] + P[1][15]*SF[6] + P[2][15]*SPP[7] + P[3][15]*SPP[6] + P[10][15]*SPP[5] + P[11][15]*SPP[4] + P[12][15]*SPP[3]; - nextP[0][16] = P[0][16] + P[1][16]*SF[6] + P[2][16]*SPP[7] + P[3][16]*SPP[6] + P[10][16]*SPP[5] + P[11][16]*SPP[4] + P[12][16]*SPP[3]; - nextP[0][17] = P[0][17] + P[1][17]*SF[6] + P[2][17]*SPP[7] + P[3][17]*SPP[6] + P[10][17]*SPP[5] + P[11][17]*SPP[4] + P[12][17]*SPP[3]; - nextP[0][18] = P[0][18] + P[1][18]*SF[6] + P[2][18]*SPP[7] + P[3][18]*SPP[6] + P[10][18]*SPP[5] + P[11][18]*SPP[4] + P[12][18]*SPP[3]; - nextP[0][19] = P[0][19] + P[1][19]*SF[6] + P[2][19]*SPP[7] + P[3][19]*SPP[6] + P[10][19]*SPP[5] + P[11][19]*SPP[4] + P[12][19]*SPP[3]; - nextP[0][20] = P[0][20] + P[1][20]*SF[6] + P[2][20]*SPP[7] + P[3][20]*SPP[6] + P[10][20]*SPP[5] + P[11][20]*SPP[4] + P[12][20]*SPP[3]; - nextP[1][0] = P[1][0] + SQ[8] + P[0][0]*SF[5] + P[2][0]*SF[4] + P[3][0]*SPP[7] + P[11][0]*SPP[3] - P[12][0]*SPP[4] - (P[10][0]*q0)/2 + SF[6]*(P[1][1] + P[0][1]*SF[5] + P[2][1]*SF[4] + P[3][1]*SPP[7] + P[11][1]*SPP[3] - P[12][1]*SPP[4] - (P[10][1]*q0)/2) + SPP[7]*(P[1][2] + P[0][2]*SF[5] + P[2][2]*SF[4] + P[3][2]*SPP[7] + P[11][2]*SPP[3] - P[12][2]*SPP[4] - (P[10][2]*q0)/2) + SPP[6]*(P[1][3] + P[0][3]*SF[5] + P[2][3]*SF[4] + P[3][3]*SPP[7] + P[11][3]*SPP[3] - P[12][3]*SPP[4] - (P[10][3]*q0)/2) + SPP[5]*(P[1][10] + P[0][10]*SF[5] + P[2][10]*SF[4] + P[3][10]*SPP[7] + P[11][10]*SPP[3] - P[12][10]*SPP[4] - (P[10][10]*q0)/2) + SPP[4]*(P[1][11] + P[0][11]*SF[5] + P[2][11]*SF[4] + P[3][11]*SPP[7] + P[11][11]*SPP[3] - P[12][11]*SPP[4] - (P[10][11]*q0)/2) + SPP[3]*(P[1][12] + P[0][12]*SF[5] + P[2][12]*SF[4] + P[3][12]*SPP[7] + P[11][12]*SPP[3] - P[12][12]*SPP[4] - (P[10][12]*q0)/2); - nextP[1][1] = P[1][1] + P[0][1]*SF[5] + P[2][1]*SF[4] + P[3][1]*SPP[7] + P[11][1]*SPP[3] - P[12][1]*SPP[4] + daxCov*SQ[9] - (P[10][1]*q0)/2 + SF[5]*(P[1][0] + P[0][0]*SF[5] + P[2][0]*SF[4] + P[3][0]*SPP[7] + P[11][0]*SPP[3] - P[12][0]*SPP[4] - (P[10][0]*q0)/2) + SF[4]*(P[1][2] + P[0][2]*SF[5] + P[2][2]*SF[4] + P[3][2]*SPP[7] + P[11][2]*SPP[3] - P[12][2]*SPP[4] - (P[10][2]*q0)/2) + SPP[7]*(P[1][3] + P[0][3]*SF[5] + P[2][3]*SF[4] + P[3][3]*SPP[7] + P[11][3]*SPP[3] - P[12][3]*SPP[4] - (P[10][3]*q0)/2) + SPP[3]*(P[1][11] + P[0][11]*SF[5] + P[2][11]*SF[4] + P[3][11]*SPP[7] + P[11][11]*SPP[3] - P[12][11]*SPP[4] - (P[10][11]*q0)/2) - SPP[4]*(P[1][12] + P[0][12]*SF[5] + P[2][12]*SF[4] + P[3][12]*SPP[7] + P[11][12]*SPP[3] - P[12][12]*SPP[4] - (P[10][12]*q0)/2) + (dayCov*sq(q3))/4 + (dazCov*sq(q2))/4 - (q0*(P[1][10] + P[0][10]*SF[5] + P[2][10]*SF[4] + P[3][10]*SPP[7] + P[11][10]*SPP[3] - P[12][10]*SPP[4] - (P[10][10]*q0)/2))/2; - nextP[1][2] = P[1][2] + SQ[5] + P[0][2]*SF[5] + P[2][2]*SF[4] + P[3][2]*SPP[7] + P[11][2]*SPP[3] - P[12][2]*SPP[4] - (P[10][2]*q0)/2 + SF[3]*(P[1][0] + P[0][0]*SF[5] + P[2][0]*SF[4] + P[3][0]*SPP[7] + P[11][0]*SPP[3] - P[12][0]*SPP[4] - (P[10][0]*q0)/2) + SF[5]*(P[1][3] + P[0][3]*SF[5] + P[2][3]*SF[4] + P[3][3]*SPP[7] + P[11][3]*SPP[3] - P[12][3]*SPP[4] - (P[10][3]*q0)/2) + SPP[6]*(P[1][1] + P[0][1]*SF[5] + P[2][1]*SF[4] + P[3][1]*SPP[7] + P[11][1]*SPP[3] - P[12][1]*SPP[4] - (P[10][1]*q0)/2) - SPP[3]*(P[1][10] + P[0][10]*SF[5] + P[2][10]*SF[4] + P[3][10]*SPP[7] + P[11][10]*SPP[3] - P[12][10]*SPP[4] - (P[10][10]*q0)/2) + SPP[5]*(P[1][12] + P[0][12]*SF[5] + P[2][12]*SF[4] + P[3][12]*SPP[7] + P[11][12]*SPP[3] - P[12][12]*SPP[4] - (P[10][12]*q0)/2) - (q0*(P[1][11] + P[0][11]*SF[5] + P[2][11]*SF[4] + P[3][11]*SPP[7] + P[11][11]*SPP[3] - P[12][11]*SPP[4] - (P[10][11]*q0)/2))/2; - nextP[1][3] = P[1][3] + SQ[4] + P[0][3]*SF[5] + P[2][3]*SF[4] + P[3][3]*SPP[7] + P[11][3]*SPP[3] - P[12][3]*SPP[4] - (P[10][3]*q0)/2 + SF[4]*(P[1][0] + P[0][0]*SF[5] + P[2][0]*SF[4] + P[3][0]*SPP[7] + P[11][0]*SPP[3] - P[12][0]*SPP[4] - (P[10][0]*q0)/2) + SF[3]*(P[1][1] + P[0][1]*SF[5] + P[2][1]*SF[4] + P[3][1]*SPP[7] + P[11][1]*SPP[3] - P[12][1]*SPP[4] - (P[10][1]*q0)/2) + SF[6]*(P[1][2] + P[0][2]*SF[5] + P[2][2]*SF[4] + P[3][2]*SPP[7] + P[11][2]*SPP[3] - P[12][2]*SPP[4] - (P[10][2]*q0)/2) + SPP[4]*(P[1][10] + P[0][10]*SF[5] + P[2][10]*SF[4] + P[3][10]*SPP[7] + P[11][10]*SPP[3] - P[12][10]*SPP[4] - (P[10][10]*q0)/2) - SPP[5]*(P[1][11] + P[0][11]*SF[5] + P[2][11]*SF[4] + P[3][11]*SPP[7] + P[11][11]*SPP[3] - P[12][11]*SPP[4] - (P[10][11]*q0)/2) - (q0*(P[1][12] + P[0][12]*SF[5] + P[2][12]*SF[4] + P[3][12]*SPP[7] + P[11][12]*SPP[3] - P[12][12]*SPP[4] - (P[10][12]*q0)/2))/2; - nextP[1][4] = P[1][4] + P[0][4]*SF[5] + P[2][4]*SF[4] + P[3][4]*SPP[7] + P[11][4]*SPP[3] - P[12][4]*SPP[4] - (P[10][4]*q0)/2 + SF[2]*(P[1][0] + P[0][0]*SF[5] + P[2][0]*SF[4] + P[3][0]*SPP[7] + P[11][0]*SPP[3] - P[12][0]*SPP[4] - (P[10][0]*q0)/2) + SF[0]*(P[1][1] + P[0][1]*SF[5] + P[2][1]*SF[4] + P[3][1]*SPP[7] + P[11][1]*SPP[3] - P[12][1]*SPP[4] - (P[10][1]*q0)/2) + SPP[0]*(P[1][2] + P[0][2]*SF[5] + P[2][2]*SF[4] + P[3][2]*SPP[7] + P[11][2]*SPP[3] - P[12][2]*SPP[4] - (P[10][2]*q0)/2) - SPP[2]*(P[1][3] + P[0][3]*SF[5] + P[2][3]*SF[4] + P[3][3]*SPP[7] + P[11][3]*SPP[3] - P[12][3]*SPP[4] - (P[10][3]*q0)/2); - nextP[1][5] = P[1][5] + P[0][5]*SF[5] + P[2][5]*SF[4] + P[3][5]*SPP[7] + P[11][5]*SPP[3] - P[12][5]*SPP[4] - (P[10][5]*q0)/2 + SF[1]*(P[1][0] + P[0][0]*SF[5] + P[2][0]*SF[4] + P[3][0]*SPP[7] + P[11][0]*SPP[3] - P[12][0]*SPP[4] - (P[10][0]*q0)/2) + SF[0]*(P[1][2] + P[0][2]*SF[5] + P[2][2]*SF[4] + P[3][2]*SPP[7] + P[11][2]*SPP[3] - P[12][2]*SPP[4] - (P[10][2]*q0)/2) + SF[2]*(P[1][3] + P[0][3]*SF[5] + P[2][3]*SF[4] + P[3][3]*SPP[7] + P[11][3]*SPP[3] - P[12][3]*SPP[4] - (P[10][3]*q0)/2) - SPP[0]*(P[1][1] + P[0][1]*SF[5] + P[2][1]*SF[4] + P[3][1]*SPP[7] + P[11][1]*SPP[3] - P[12][1]*SPP[4] - (P[10][1]*q0)/2); - nextP[1][6] = P[1][6] + P[0][6]*SF[5] + P[2][6]*SF[4] + P[3][6]*SPP[7] + P[11][6]*SPP[3] - P[12][6]*SPP[4] - (P[10][6]*q0)/2 + SF[1]*(P[1][1] + P[0][1]*SF[5] + P[2][1]*SF[4] + P[3][1]*SPP[7] + P[11][1]*SPP[3] - P[12][1]*SPP[4] - (P[10][1]*q0)/2) + SF[0]*(P[1][3] + P[0][3]*SF[5] + P[2][3]*SF[4] + P[3][3]*SPP[7] + P[11][3]*SPP[3] - P[12][3]*SPP[4] - (P[10][3]*q0)/2) + SPP[0]*(P[1][0] + P[0][0]*SF[5] + P[2][0]*SF[4] + P[3][0]*SPP[7] + P[11][0]*SPP[3] - P[12][0]*SPP[4] - (P[10][0]*q0)/2) - SPP[1]*(P[1][2] + P[0][2]*SF[5] + P[2][2]*SF[4] + P[3][2]*SPP[7] + P[11][2]*SPP[3] - P[12][2]*SPP[4] - (P[10][2]*q0)/2); - nextP[1][7] = P[1][7] + P[0][7]*SF[5] + P[2][7]*SF[4] + P[3][7]*SPP[7] + P[11][7]*SPP[3] - P[12][7]*SPP[4] - (P[10][7]*q0)/2 + dt*(P[1][4] + P[0][4]*SF[5] + P[2][4]*SF[4] + P[3][4]*SPP[7] + P[11][4]*SPP[3] - P[12][4]*SPP[4] - (P[10][4]*q0)/2); - nextP[1][8] = P[1][8] + P[0][8]*SF[5] + P[2][8]*SF[4] + P[3][8]*SPP[7] + P[11][8]*SPP[3] - P[12][8]*SPP[4] - (P[10][8]*q0)/2 + dt*(P[1][5] + P[0][5]*SF[5] + P[2][5]*SF[4] + P[3][5]*SPP[7] + P[11][5]*SPP[3] - P[12][5]*SPP[4] - (P[10][5]*q0)/2); - nextP[1][9] = P[1][9] + P[0][9]*SF[5] + P[2][9]*SF[4] + P[3][9]*SPP[7] + P[11][9]*SPP[3] - P[12][9]*SPP[4] - (P[10][9]*q0)/2 + dt*(P[1][6] + P[0][6]*SF[5] + P[2][6]*SF[4] + P[3][6]*SPP[7] + P[11][6]*SPP[3] - P[12][6]*SPP[4] - (P[10][6]*q0)/2); - nextP[1][10] = P[1][10] + P[0][10]*SF[5] + P[2][10]*SF[4] + P[3][10]*SPP[7] + P[11][10]*SPP[3] - P[12][10]*SPP[4] - (P[10][10]*q0)/2; - nextP[1][11] = P[1][11] + P[0][11]*SF[5] + P[2][11]*SF[4] + P[3][11]*SPP[7] + P[11][11]*SPP[3] - P[12][11]*SPP[4] - (P[10][11]*q0)/2; - nextP[1][12] = P[1][12] + P[0][12]*SF[5] + P[2][12]*SF[4] + P[3][12]*SPP[7] + P[11][12]*SPP[3] - P[12][12]*SPP[4] - (P[10][12]*q0)/2; - nextP[1][13] = P[1][13] + P[0][13]*SF[5] + P[2][13]*SF[4] + P[3][13]*SPP[7] + P[11][13]*SPP[3] - P[12][13]*SPP[4] - (P[10][13]*q0)/2; - nextP[1][14] = P[1][14] + P[0][14]*SF[5] + P[2][14]*SF[4] + P[3][14]*SPP[7] + P[11][14]*SPP[3] - P[12][14]*SPP[4] - (P[10][14]*q0)/2; - nextP[1][15] = P[1][15] + P[0][15]*SF[5] + P[2][15]*SF[4] + P[3][15]*SPP[7] + P[11][15]*SPP[3] - P[12][15]*SPP[4] - (P[10][15]*q0)/2; - nextP[1][16] = P[1][16] + P[0][16]*SF[5] + P[2][16]*SF[4] + P[3][16]*SPP[7] + P[11][16]*SPP[3] - P[12][16]*SPP[4] - (P[10][16]*q0)/2; - nextP[1][17] = P[1][17] + P[0][17]*SF[5] + P[2][17]*SF[4] + P[3][17]*SPP[7] + P[11][17]*SPP[3] - P[12][17]*SPP[4] - (P[10][17]*q0)/2; - nextP[1][18] = P[1][18] + P[0][18]*SF[5] + P[2][18]*SF[4] + P[3][18]*SPP[7] + P[11][18]*SPP[3] - P[12][18]*SPP[4] - (P[10][18]*q0)/2; - nextP[1][19] = P[1][19] + P[0][19]*SF[5] + P[2][19]*SF[4] + P[3][19]*SPP[7] + P[11][19]*SPP[3] - P[12][19]*SPP[4] - (P[10][19]*q0)/2; - nextP[1][20] = P[1][20] + P[0][20]*SF[5] + P[2][20]*SF[4] + P[3][20]*SPP[7] + P[11][20]*SPP[3] - P[12][20]*SPP[4] - (P[10][20]*q0)/2; - nextP[2][0] = P[2][0] + SQ[7] + P[0][0]*SF[3] + P[3][0]*SF[5] + P[1][0]*SPP[6] - P[10][0]*SPP[3] + P[12][0]*SPP[5] - (P[11][0]*q0)/2 + SF[6]*(P[2][1] + P[0][1]*SF[3] + P[3][1]*SF[5] + P[1][1]*SPP[6] - P[10][1]*SPP[3] + P[12][1]*SPP[5] - (P[11][1]*q0)/2) + SPP[7]*(P[2][2] + P[0][2]*SF[3] + P[3][2]*SF[5] + P[1][2]*SPP[6] - P[10][2]*SPP[3] + P[12][2]*SPP[5] - (P[11][2]*q0)/2) + SPP[6]*(P[2][3] + P[0][3]*SF[3] + P[3][3]*SF[5] + P[1][3]*SPP[6] - P[10][3]*SPP[3] + P[12][3]*SPP[5] - (P[11][3]*q0)/2) + SPP[5]*(P[2][10] + P[0][10]*SF[3] + P[3][10]*SF[5] + P[1][10]*SPP[6] - P[10][10]*SPP[3] + P[12][10]*SPP[5] - (P[11][10]*q0)/2) + SPP[4]*(P[2][11] + P[0][11]*SF[3] + P[3][11]*SF[5] + P[1][11]*SPP[6] - P[10][11]*SPP[3] + P[12][11]*SPP[5] - (P[11][11]*q0)/2) + SPP[3]*(P[2][12] + P[0][12]*SF[3] + P[3][12]*SF[5] + P[1][12]*SPP[6] - P[10][12]*SPP[3] + P[12][12]*SPP[5] - (P[11][12]*q0)/2); - nextP[2][1] = P[2][1] + SQ[5] + P[0][1]*SF[3] + P[3][1]*SF[5] + P[1][1]*SPP[6] - P[10][1]*SPP[3] + P[12][1]*SPP[5] - (P[11][1]*q0)/2 + SF[5]*(P[2][0] + P[0][0]*SF[3] + P[3][0]*SF[5] + P[1][0]*SPP[6] - P[10][0]*SPP[3] + P[12][0]*SPP[5] - (P[11][0]*q0)/2) + SF[4]*(P[2][2] + P[0][2]*SF[3] + P[3][2]*SF[5] + P[1][2]*SPP[6] - P[10][2]*SPP[3] + P[12][2]*SPP[5] - (P[11][2]*q0)/2) + SPP[7]*(P[2][3] + P[0][3]*SF[3] + P[3][3]*SF[5] + P[1][3]*SPP[6] - P[10][3]*SPP[3] + P[12][3]*SPP[5] - (P[11][3]*q0)/2) + SPP[3]*(P[2][11] + P[0][11]*SF[3] + P[3][11]*SF[5] + P[1][11]*SPP[6] - P[10][11]*SPP[3] + P[12][11]*SPP[5] - (P[11][11]*q0)/2) - SPP[4]*(P[2][12] + P[0][12]*SF[3] + P[3][12]*SF[5] + P[1][12]*SPP[6] - P[10][12]*SPP[3] + P[12][12]*SPP[5] - (P[11][12]*q0)/2) - (q0*(P[2][10] + P[0][10]*SF[3] + P[3][10]*SF[5] + P[1][10]*SPP[6] - P[10][10]*SPP[3] + P[12][10]*SPP[5] - (P[11][10]*q0)/2))/2; - nextP[2][2] = P[2][2] + P[0][2]*SF[3] + P[3][2]*SF[5] + P[1][2]*SPP[6] - P[10][2]*SPP[3] + P[12][2]*SPP[5] + dayCov*SQ[9] + (dazCov*SQ[10])/4 - (P[11][2]*q0)/2 + SF[3]*(P[2][0] + P[0][0]*SF[3] + P[3][0]*SF[5] + P[1][0]*SPP[6] - P[10][0]*SPP[3] + P[12][0]*SPP[5] - (P[11][0]*q0)/2) + SF[5]*(P[2][3] + P[0][3]*SF[3] + P[3][3]*SF[5] + P[1][3]*SPP[6] - P[10][3]*SPP[3] + P[12][3]*SPP[5] - (P[11][3]*q0)/2) + SPP[6]*(P[2][1] + P[0][1]*SF[3] + P[3][1]*SF[5] + P[1][1]*SPP[6] - P[10][1]*SPP[3] + P[12][1]*SPP[5] - (P[11][1]*q0)/2) - SPP[3]*(P[2][10] + P[0][10]*SF[3] + P[3][10]*SF[5] + P[1][10]*SPP[6] - P[10][10]*SPP[3] + P[12][10]*SPP[5] - (P[11][10]*q0)/2) + SPP[5]*(P[2][12] + P[0][12]*SF[3] + P[3][12]*SF[5] + P[1][12]*SPP[6] - P[10][12]*SPP[3] + P[12][12]*SPP[5] - (P[11][12]*q0)/2) + (daxCov*sq(q3))/4 - (q0*(P[2][11] + P[0][11]*SF[3] + P[3][11]*SF[5] + P[1][11]*SPP[6] - P[10][11]*SPP[3] + P[12][11]*SPP[5] - (P[11][11]*q0)/2))/2; - nextP[2][3] = P[2][3] + SQ[3] + P[0][3]*SF[3] + P[3][3]*SF[5] + P[1][3]*SPP[6] - P[10][3]*SPP[3] + P[12][3]*SPP[5] - (P[11][3]*q0)/2 + SF[4]*(P[2][0] + P[0][0]*SF[3] + P[3][0]*SF[5] + P[1][0]*SPP[6] - P[10][0]*SPP[3] + P[12][0]*SPP[5] - (P[11][0]*q0)/2) + SF[3]*(P[2][1] + P[0][1]*SF[3] + P[3][1]*SF[5] + P[1][1]*SPP[6] - P[10][1]*SPP[3] + P[12][1]*SPP[5] - (P[11][1]*q0)/2) + SF[6]*(P[2][2] + P[0][2]*SF[3] + P[3][2]*SF[5] + P[1][2]*SPP[6] - P[10][2]*SPP[3] + P[12][2]*SPP[5] - (P[11][2]*q0)/2) + SPP[4]*(P[2][10] + P[0][10]*SF[3] + P[3][10]*SF[5] + P[1][10]*SPP[6] - P[10][10]*SPP[3] + P[12][10]*SPP[5] - (P[11][10]*q0)/2) - SPP[5]*(P[2][11] + P[0][11]*SF[3] + P[3][11]*SF[5] + P[1][11]*SPP[6] - P[10][11]*SPP[3] + P[12][11]*SPP[5] - (P[11][11]*q0)/2) - (q0*(P[2][12] + P[0][12]*SF[3] + P[3][12]*SF[5] + P[1][12]*SPP[6] - P[10][12]*SPP[3] + P[12][12]*SPP[5] - (P[11][12]*q0)/2))/2; - nextP[2][4] = P[2][4] + P[0][4]*SF[3] + P[3][4]*SF[5] + P[1][4]*SPP[6] - P[10][4]*SPP[3] + P[12][4]*SPP[5] - (P[11][4]*q0)/2 + SF[2]*(P[2][0] + P[0][0]*SF[3] + P[3][0]*SF[5] + P[1][0]*SPP[6] - P[10][0]*SPP[3] + P[12][0]*SPP[5] - (P[11][0]*q0)/2) + SF[0]*(P[2][1] + P[0][1]*SF[3] + P[3][1]*SF[5] + P[1][1]*SPP[6] - P[10][1]*SPP[3] + P[12][1]*SPP[5] - (P[11][1]*q0)/2) + SPP[0]*(P[2][2] + P[0][2]*SF[3] + P[3][2]*SF[5] + P[1][2]*SPP[6] - P[10][2]*SPP[3] + P[12][2]*SPP[5] - (P[11][2]*q0)/2) - SPP[2]*(P[2][3] + P[0][3]*SF[3] + P[3][3]*SF[5] + P[1][3]*SPP[6] - P[10][3]*SPP[3] + P[12][3]*SPP[5] - (P[11][3]*q0)/2); - nextP[2][5] = P[2][5] + P[0][5]*SF[3] + P[3][5]*SF[5] + P[1][5]*SPP[6] - P[10][5]*SPP[3] + P[12][5]*SPP[5] - (P[11][5]*q0)/2 + SF[1]*(P[2][0] + P[0][0]*SF[3] + P[3][0]*SF[5] + P[1][0]*SPP[6] - P[10][0]*SPP[3] + P[12][0]*SPP[5] - (P[11][0]*q0)/2) + SF[0]*(P[2][2] + P[0][2]*SF[3] + P[3][2]*SF[5] + P[1][2]*SPP[6] - P[10][2]*SPP[3] + P[12][2]*SPP[5] - (P[11][2]*q0)/2) + SF[2]*(P[2][3] + P[0][3]*SF[3] + P[3][3]*SF[5] + P[1][3]*SPP[6] - P[10][3]*SPP[3] + P[12][3]*SPP[5] - (P[11][3]*q0)/2) - SPP[0]*(P[2][1] + P[0][1]*SF[3] + P[3][1]*SF[5] + P[1][1]*SPP[6] - P[10][1]*SPP[3] + P[12][1]*SPP[5] - (P[11][1]*q0)/2); - nextP[2][6] = P[2][6] + P[0][6]*SF[3] + P[3][6]*SF[5] + P[1][6]*SPP[6] - P[10][6]*SPP[3] + P[12][6]*SPP[5] - (P[11][6]*q0)/2 + SF[1]*(P[2][1] + P[0][1]*SF[3] + P[3][1]*SF[5] + P[1][1]*SPP[6] - P[10][1]*SPP[3] + P[12][1]*SPP[5] - (P[11][1]*q0)/2) + SF[0]*(P[2][3] + P[0][3]*SF[3] + P[3][3]*SF[5] + P[1][3]*SPP[6] - P[10][3]*SPP[3] + P[12][3]*SPP[5] - (P[11][3]*q0)/2) + SPP[0]*(P[2][0] + P[0][0]*SF[3] + P[3][0]*SF[5] + P[1][0]*SPP[6] - P[10][0]*SPP[3] + P[12][0]*SPP[5] - (P[11][0]*q0)/2) - SPP[1]*(P[2][2] + P[0][2]*SF[3] + P[3][2]*SF[5] + P[1][2]*SPP[6] - P[10][2]*SPP[3] + P[12][2]*SPP[5] - (P[11][2]*q0)/2); - nextP[2][7] = P[2][7] + P[0][7]*SF[3] + P[3][7]*SF[5] + P[1][7]*SPP[6] - P[10][7]*SPP[3] + P[12][7]*SPP[5] - (P[11][7]*q0)/2 + dt*(P[2][4] + P[0][4]*SF[3] + P[3][4]*SF[5] + P[1][4]*SPP[6] - P[10][4]*SPP[3] + P[12][4]*SPP[5] - (P[11][4]*q0)/2); - nextP[2][8] = P[2][8] + P[0][8]*SF[3] + P[3][8]*SF[5] + P[1][8]*SPP[6] - P[10][8]*SPP[3] + P[12][8]*SPP[5] - (P[11][8]*q0)/2 + dt*(P[2][5] + P[0][5]*SF[3] + P[3][5]*SF[5] + P[1][5]*SPP[6] - P[10][5]*SPP[3] + P[12][5]*SPP[5] - (P[11][5]*q0)/2); - nextP[2][9] = P[2][9] + P[0][9]*SF[3] + P[3][9]*SF[5] + P[1][9]*SPP[6] - P[10][9]*SPP[3] + P[12][9]*SPP[5] - (P[11][9]*q0)/2 + dt*(P[2][6] + P[0][6]*SF[3] + P[3][6]*SF[5] + P[1][6]*SPP[6] - P[10][6]*SPP[3] + P[12][6]*SPP[5] - (P[11][6]*q0)/2); - nextP[2][10] = P[2][10] + P[0][10]*SF[3] + P[3][10]*SF[5] + P[1][10]*SPP[6] - P[10][10]*SPP[3] + P[12][10]*SPP[5] - (P[11][10]*q0)/2; - nextP[2][11] = P[2][11] + P[0][11]*SF[3] + P[3][11]*SF[5] + P[1][11]*SPP[6] - P[10][11]*SPP[3] + P[12][11]*SPP[5] - (P[11][11]*q0)/2; - nextP[2][12] = P[2][12] + P[0][12]*SF[3] + P[3][12]*SF[5] + P[1][12]*SPP[6] - P[10][12]*SPP[3] + P[12][12]*SPP[5] - (P[11][12]*q0)/2; - nextP[2][13] = P[2][13] + P[0][13]*SF[3] + P[3][13]*SF[5] + P[1][13]*SPP[6] - P[10][13]*SPP[3] + P[12][13]*SPP[5] - (P[11][13]*q0)/2; - nextP[2][14] = P[2][14] + P[0][14]*SF[3] + P[3][14]*SF[5] + P[1][14]*SPP[6] - P[10][14]*SPP[3] + P[12][14]*SPP[5] - (P[11][14]*q0)/2; - nextP[2][15] = P[2][15] + P[0][15]*SF[3] + P[3][15]*SF[5] + P[1][15]*SPP[6] - P[10][15]*SPP[3] + P[12][15]*SPP[5] - (P[11][15]*q0)/2; - nextP[2][16] = P[2][16] + P[0][16]*SF[3] + P[3][16]*SF[5] + P[1][16]*SPP[6] - P[10][16]*SPP[3] + P[12][16]*SPP[5] - (P[11][16]*q0)/2; - nextP[2][17] = P[2][17] + P[0][17]*SF[3] + P[3][17]*SF[5] + P[1][17]*SPP[6] - P[10][17]*SPP[3] + P[12][17]*SPP[5] - (P[11][17]*q0)/2; - nextP[2][18] = P[2][18] + P[0][18]*SF[3] + P[3][18]*SF[5] + P[1][18]*SPP[6] - P[10][18]*SPP[3] + P[12][18]*SPP[5] - (P[11][18]*q0)/2; - nextP[2][19] = P[2][19] + P[0][19]*SF[3] + P[3][19]*SF[5] + P[1][19]*SPP[6] - P[10][19]*SPP[3] + P[12][19]*SPP[5] - (P[11][19]*q0)/2; - nextP[2][20] = P[2][20] + P[0][20]*SF[3] + P[3][20]*SF[5] + P[1][20]*SPP[6] - P[10][20]*SPP[3] + P[12][20]*SPP[5] - (P[11][20]*q0)/2; - nextP[3][0] = P[3][0] + SQ[6] + P[0][0]*SF[4] + P[1][0]*SF[3] + P[2][0]*SF[6] + P[10][0]*SPP[4] - P[11][0]*SPP[5] - (P[12][0]*q0)/2 + SF[6]*(P[3][1] + P[0][1]*SF[4] + P[1][1]*SF[3] + P[2][1]*SF[6] + P[10][1]*SPP[4] - P[11][1]*SPP[5] - (P[12][1]*q0)/2) + SPP[7]*(P[3][2] + P[0][2]*SF[4] + P[1][2]*SF[3] + P[2][2]*SF[6] + P[10][2]*SPP[4] - P[11][2]*SPP[5] - (P[12][2]*q0)/2) + SPP[6]*(P[3][3] + P[0][3]*SF[4] + P[1][3]*SF[3] + P[2][3]*SF[6] + P[10][3]*SPP[4] - P[11][3]*SPP[5] - (P[12][3]*q0)/2) + SPP[5]*(P[3][10] + P[0][10]*SF[4] + P[1][10]*SF[3] + P[2][10]*SF[6] + P[10][10]*SPP[4] - P[11][10]*SPP[5] - (P[12][10]*q0)/2) + SPP[4]*(P[3][11] + P[0][11]*SF[4] + P[1][11]*SF[3] + P[2][11]*SF[6] + P[10][11]*SPP[4] - P[11][11]*SPP[5] - (P[12][11]*q0)/2) + SPP[3]*(P[3][12] + P[0][12]*SF[4] + P[1][12]*SF[3] + P[2][12]*SF[6] + P[10][12]*SPP[4] - P[11][12]*SPP[5] - (P[12][12]*q0)/2); - nextP[3][1] = P[3][1] + SQ[4] + P[0][1]*SF[4] + P[1][1]*SF[3] + P[2][1]*SF[6] + P[10][1]*SPP[4] - P[11][1]*SPP[5] - (P[12][1]*q0)/2 + SF[5]*(P[3][0] + P[0][0]*SF[4] + P[1][0]*SF[3] + P[2][0]*SF[6] + P[10][0]*SPP[4] - P[11][0]*SPP[5] - (P[12][0]*q0)/2) + SF[4]*(P[3][2] + P[0][2]*SF[4] + P[1][2]*SF[3] + P[2][2]*SF[6] + P[10][2]*SPP[4] - P[11][2]*SPP[5] - (P[12][2]*q0)/2) + SPP[7]*(P[3][3] + P[0][3]*SF[4] + P[1][3]*SF[3] + P[2][3]*SF[6] + P[10][3]*SPP[4] - P[11][3]*SPP[5] - (P[12][3]*q0)/2) + SPP[3]*(P[3][11] + P[0][11]*SF[4] + P[1][11]*SF[3] + P[2][11]*SF[6] + P[10][11]*SPP[4] - P[11][11]*SPP[5] - (P[12][11]*q0)/2) - SPP[4]*(P[3][12] + P[0][12]*SF[4] + P[1][12]*SF[3] + P[2][12]*SF[6] + P[10][12]*SPP[4] - P[11][12]*SPP[5] - (P[12][12]*q0)/2) - (q0*(P[3][10] + P[0][10]*SF[4] + P[1][10]*SF[3] + P[2][10]*SF[6] + P[10][10]*SPP[4] - P[11][10]*SPP[5] - (P[12][10]*q0)/2))/2; - nextP[3][2] = P[3][2] + SQ[3] + P[0][2]*SF[4] + P[1][2]*SF[3] + P[2][2]*SF[6] + P[10][2]*SPP[4] - P[11][2]*SPP[5] - (P[12][2]*q0)/2 + SF[3]*(P[3][0] + P[0][0]*SF[4] + P[1][0]*SF[3] + P[2][0]*SF[6] + P[10][0]*SPP[4] - P[11][0]*SPP[5] - (P[12][0]*q0)/2) + SF[5]*(P[3][3] + P[0][3]*SF[4] + P[1][3]*SF[3] + P[2][3]*SF[6] + P[10][3]*SPP[4] - P[11][3]*SPP[5] - (P[12][3]*q0)/2) + SPP[6]*(P[3][1] + P[0][1]*SF[4] + P[1][1]*SF[3] + P[2][1]*SF[6] + P[10][1]*SPP[4] - P[11][1]*SPP[5] - (P[12][1]*q0)/2) - SPP[3]*(P[3][10] + P[0][10]*SF[4] + P[1][10]*SF[3] + P[2][10]*SF[6] + P[10][10]*SPP[4] - P[11][10]*SPP[5] - (P[12][10]*q0)/2) + SPP[5]*(P[3][12] + P[0][12]*SF[4] + P[1][12]*SF[3] + P[2][12]*SF[6] + P[10][12]*SPP[4] - P[11][12]*SPP[5] - (P[12][12]*q0)/2) - (q0*(P[3][11] + P[0][11]*SF[4] + P[1][11]*SF[3] + P[2][11]*SF[6] + P[10][11]*SPP[4] - P[11][11]*SPP[5] - (P[12][11]*q0)/2))/2; - nextP[3][3] = P[3][3] + P[0][3]*SF[4] + P[1][3]*SF[3] + P[2][3]*SF[6] + P[10][3]*SPP[4] - P[11][3]*SPP[5] + (dayCov*SQ[10])/4 + dazCov*SQ[9] - (P[12][3]*q0)/2 + SF[4]*(P[3][0] + P[0][0]*SF[4] + P[1][0]*SF[3] + P[2][0]*SF[6] + P[10][0]*SPP[4] - P[11][0]*SPP[5] - (P[12][0]*q0)/2) + SF[3]*(P[3][1] + P[0][1]*SF[4] + P[1][1]*SF[3] + P[2][1]*SF[6] + P[10][1]*SPP[4] - P[11][1]*SPP[5] - (P[12][1]*q0)/2) + SF[6]*(P[3][2] + P[0][2]*SF[4] + P[1][2]*SF[3] + P[2][2]*SF[6] + P[10][2]*SPP[4] - P[11][2]*SPP[5] - (P[12][2]*q0)/2) + SPP[4]*(P[3][10] + P[0][10]*SF[4] + P[1][10]*SF[3] + P[2][10]*SF[6] + P[10][10]*SPP[4] - P[11][10]*SPP[5] - (P[12][10]*q0)/2) - SPP[5]*(P[3][11] + P[0][11]*SF[4] + P[1][11]*SF[3] + P[2][11]*SF[6] + P[10][11]*SPP[4] - P[11][11]*SPP[5] - (P[12][11]*q0)/2) + (daxCov*sq(q2))/4 - (q0*(P[3][12] + P[0][12]*SF[4] + P[1][12]*SF[3] + P[2][12]*SF[6] + P[10][12]*SPP[4] - P[11][12]*SPP[5] - (P[12][12]*q0)/2))/2; - nextP[3][4] = P[3][4] + P[0][4]*SF[4] + P[1][4]*SF[3] + P[2][4]*SF[6] + P[10][4]*SPP[4] - P[11][4]*SPP[5] - (P[12][4]*q0)/2 + SF[2]*(P[3][0] + P[0][0]*SF[4] + P[1][0]*SF[3] + P[2][0]*SF[6] + P[10][0]*SPP[4] - P[11][0]*SPP[5] - (P[12][0]*q0)/2) + SF[0]*(P[3][1] + P[0][1]*SF[4] + P[1][1]*SF[3] + P[2][1]*SF[6] + P[10][1]*SPP[4] - P[11][1]*SPP[5] - (P[12][1]*q0)/2) + SPP[0]*(P[3][2] + P[0][2]*SF[4] + P[1][2]*SF[3] + P[2][2]*SF[6] + P[10][2]*SPP[4] - P[11][2]*SPP[5] - (P[12][2]*q0)/2) - SPP[2]*(P[3][3] + P[0][3]*SF[4] + P[1][3]*SF[3] + P[2][3]*SF[6] + P[10][3]*SPP[4] - P[11][3]*SPP[5] - (P[12][3]*q0)/2); - nextP[3][5] = P[3][5] + P[0][5]*SF[4] + P[1][5]*SF[3] + P[2][5]*SF[6] + P[10][5]*SPP[4] - P[11][5]*SPP[5] - (P[12][5]*q0)/2 + SF[1]*(P[3][0] + P[0][0]*SF[4] + P[1][0]*SF[3] + P[2][0]*SF[6] + P[10][0]*SPP[4] - P[11][0]*SPP[5] - (P[12][0]*q0)/2) + SF[0]*(P[3][2] + P[0][2]*SF[4] + P[1][2]*SF[3] + P[2][2]*SF[6] + P[10][2]*SPP[4] - P[11][2]*SPP[5] - (P[12][2]*q0)/2) + SF[2]*(P[3][3] + P[0][3]*SF[4] + P[1][3]*SF[3] + P[2][3]*SF[6] + P[10][3]*SPP[4] - P[11][3]*SPP[5] - (P[12][3]*q0)/2) - SPP[0]*(P[3][1] + P[0][1]*SF[4] + P[1][1]*SF[3] + P[2][1]*SF[6] + P[10][1]*SPP[4] - P[11][1]*SPP[5] - (P[12][1]*q0)/2); - nextP[3][6] = P[3][6] + P[0][6]*SF[4] + P[1][6]*SF[3] + P[2][6]*SF[6] + P[10][6]*SPP[4] - P[11][6]*SPP[5] - (P[12][6]*q0)/2 + SF[1]*(P[3][1] + P[0][1]*SF[4] + P[1][1]*SF[3] + P[2][1]*SF[6] + P[10][1]*SPP[4] - P[11][1]*SPP[5] - (P[12][1]*q0)/2) + SF[0]*(P[3][3] + P[0][3]*SF[4] + P[1][3]*SF[3] + P[2][3]*SF[6] + P[10][3]*SPP[4] - P[11][3]*SPP[5] - (P[12][3]*q0)/2) + SPP[0]*(P[3][0] + P[0][0]*SF[4] + P[1][0]*SF[3] + P[2][0]*SF[6] + P[10][0]*SPP[4] - P[11][0]*SPP[5] - (P[12][0]*q0)/2) - SPP[1]*(P[3][2] + P[0][2]*SF[4] + P[1][2]*SF[3] + P[2][2]*SF[6] + P[10][2]*SPP[4] - P[11][2]*SPP[5] - (P[12][2]*q0)/2); - nextP[3][7] = P[3][7] + P[0][7]*SF[4] + P[1][7]*SF[3] + P[2][7]*SF[6] + P[10][7]*SPP[4] - P[11][7]*SPP[5] - (P[12][7]*q0)/2 + dt*(P[3][4] + P[0][4]*SF[4] + P[1][4]*SF[3] + P[2][4]*SF[6] + P[10][4]*SPP[4] - P[11][4]*SPP[5] - (P[12][4]*q0)/2); - nextP[3][8] = P[3][8] + P[0][8]*SF[4] + P[1][8]*SF[3] + P[2][8]*SF[6] + P[10][8]*SPP[4] - P[11][8]*SPP[5] - (P[12][8]*q0)/2 + dt*(P[3][5] + P[0][5]*SF[4] + P[1][5]*SF[3] + P[2][5]*SF[6] + P[10][5]*SPP[4] - P[11][5]*SPP[5] - (P[12][5]*q0)/2); - nextP[3][9] = P[3][9] + P[0][9]*SF[4] + P[1][9]*SF[3] + P[2][9]*SF[6] + P[10][9]*SPP[4] - P[11][9]*SPP[5] - (P[12][9]*q0)/2 + dt*(P[3][6] + P[0][6]*SF[4] + P[1][6]*SF[3] + P[2][6]*SF[6] + P[10][6]*SPP[4] - P[11][6]*SPP[5] - (P[12][6]*q0)/2); - nextP[3][10] = P[3][10] + P[0][10]*SF[4] + P[1][10]*SF[3] + P[2][10]*SF[6] + P[10][10]*SPP[4] - P[11][10]*SPP[5] - (P[12][10]*q0)/2; - nextP[3][11] = P[3][11] + P[0][11]*SF[4] + P[1][11]*SF[3] + P[2][11]*SF[6] + P[10][11]*SPP[4] - P[11][11]*SPP[5] - (P[12][11]*q0)/2; - nextP[3][12] = P[3][12] + P[0][12]*SF[4] + P[1][12]*SF[3] + P[2][12]*SF[6] + P[10][12]*SPP[4] - P[11][12]*SPP[5] - (P[12][12]*q0)/2; - nextP[3][13] = P[3][13] + P[0][13]*SF[4] + P[1][13]*SF[3] + P[2][13]*SF[6] + P[10][13]*SPP[4] - P[11][13]*SPP[5] - (P[12][13]*q0)/2; - nextP[3][14] = P[3][14] + P[0][14]*SF[4] + P[1][14]*SF[3] + P[2][14]*SF[6] + P[10][14]*SPP[4] - P[11][14]*SPP[5] - (P[12][14]*q0)/2; - nextP[3][15] = P[3][15] + P[0][15]*SF[4] + P[1][15]*SF[3] + P[2][15]*SF[6] + P[10][15]*SPP[4] - P[11][15]*SPP[5] - (P[12][15]*q0)/2; - nextP[3][16] = P[3][16] + P[0][16]*SF[4] + P[1][16]*SF[3] + P[2][16]*SF[6] + P[10][16]*SPP[4] - P[11][16]*SPP[5] - (P[12][16]*q0)/2; - nextP[3][17] = P[3][17] + P[0][17]*SF[4] + P[1][17]*SF[3] + P[2][17]*SF[6] + P[10][17]*SPP[4] - P[11][17]*SPP[5] - (P[12][17]*q0)/2; - nextP[3][18] = P[3][18] + P[0][18]*SF[4] + P[1][18]*SF[3] + P[2][18]*SF[6] + P[10][18]*SPP[4] - P[11][18]*SPP[5] - (P[12][18]*q0)/2; - nextP[3][19] = P[3][19] + P[0][19]*SF[4] + P[1][19]*SF[3] + P[2][19]*SF[6] + P[10][19]*SPP[4] - P[11][19]*SPP[5] - (P[12][19]*q0)/2; - nextP[3][20] = P[3][20] + P[0][20]*SF[4] + P[1][20]*SF[3] + P[2][20]*SF[6] + P[10][20]*SPP[4] - P[11][20]*SPP[5] - (P[12][20]*q0)/2; - nextP[4][0] = P[4][0] + P[0][0]*SF[2] + P[1][0]*SF[0] + P[2][0]*SPP[0] - P[3][0]*SPP[2] + SF[6]*(P[4][1] + P[0][1]*SF[2] + P[1][1]*SF[0] + P[2][1]*SPP[0] - P[3][1]*SPP[2]) + SPP[7]*(P[4][2] + P[0][2]*SF[2] + P[1][2]*SF[0] + P[2][2]*SPP[0] - P[3][2]*SPP[2]) + SPP[6]*(P[4][3] + P[0][3]*SF[2] + P[1][3]*SF[0] + P[2][3]*SPP[0] - P[3][3]*SPP[2]) + SPP[5]*(P[4][10] + P[0][10]*SF[2] + P[1][10]*SF[0] + P[2][10]*SPP[0] - P[3][10]*SPP[2]) + SPP[4]*(P[4][11] + P[0][11]*SF[2] + P[1][11]*SF[0] + P[2][11]*SPP[0] - P[3][11]*SPP[2]) + SPP[3]*(P[4][12] + P[0][12]*SF[2] + P[1][12]*SF[0] + P[2][12]*SPP[0] - P[3][12]*SPP[2]); - nextP[4][1] = P[4][1] + P[0][1]*SF[2] + P[1][1]*SF[0] + P[2][1]*SPP[0] - P[3][1]*SPP[2] + SF[5]*(P[4][0] + P[0][0]*SF[2] + P[1][0]*SF[0] + P[2][0]*SPP[0] - P[3][0]*SPP[2]) + SF[4]*(P[4][2] + P[0][2]*SF[2] + P[1][2]*SF[0] + P[2][2]*SPP[0] - P[3][2]*SPP[2]) + SPP[7]*(P[4][3] + P[0][3]*SF[2] + P[1][3]*SF[0] + P[2][3]*SPP[0] - P[3][3]*SPP[2]) + SPP[3]*(P[4][11] + P[0][11]*SF[2] + P[1][11]*SF[0] + P[2][11]*SPP[0] - P[3][11]*SPP[2]) - SPP[4]*(P[4][12] + P[0][12]*SF[2] + P[1][12]*SF[0] + P[2][12]*SPP[0] - P[3][12]*SPP[2]) - (q0*(P[4][10] + P[0][10]*SF[2] + P[1][10]*SF[0] + P[2][10]*SPP[0] - P[3][10]*SPP[2]))/2; - nextP[4][2] = P[4][2] + P[0][2]*SF[2] + P[1][2]*SF[0] + P[2][2]*SPP[0] - P[3][2]*SPP[2] + SF[3]*(P[4][0] + P[0][0]*SF[2] + P[1][0]*SF[0] + P[2][0]*SPP[0] - P[3][0]*SPP[2]) + SF[5]*(P[4][3] + P[0][3]*SF[2] + P[1][3]*SF[0] + P[2][3]*SPP[0] - P[3][3]*SPP[2]) + SPP[6]*(P[4][1] + P[0][1]*SF[2] + P[1][1]*SF[0] + P[2][1]*SPP[0] - P[3][1]*SPP[2]) - SPP[3]*(P[4][10] + P[0][10]*SF[2] + P[1][10]*SF[0] + P[2][10]*SPP[0] - P[3][10]*SPP[2]) + SPP[5]*(P[4][12] + P[0][12]*SF[2] + P[1][12]*SF[0] + P[2][12]*SPP[0] - P[3][12]*SPP[2]) - (q0*(P[4][11] + P[0][11]*SF[2] + P[1][11]*SF[0] + P[2][11]*SPP[0] - P[3][11]*SPP[2]))/2; - nextP[4][3] = P[4][3] + P[0][3]*SF[2] + P[1][3]*SF[0] + P[2][3]*SPP[0] - P[3][3]*SPP[2] + SF[4]*(P[4][0] + P[0][0]*SF[2] + P[1][0]*SF[0] + P[2][0]*SPP[0] - P[3][0]*SPP[2]) + SF[3]*(P[4][1] + P[0][1]*SF[2] + P[1][1]*SF[0] + P[2][1]*SPP[0] - P[3][1]*SPP[2]) + SF[6]*(P[4][2] + P[0][2]*SF[2] + P[1][2]*SF[0] + P[2][2]*SPP[0] - P[3][2]*SPP[2]) + SPP[4]*(P[4][10] + P[0][10]*SF[2] + P[1][10]*SF[0] + P[2][10]*SPP[0] - P[3][10]*SPP[2]) - SPP[5]*(P[4][11] + P[0][11]*SF[2] + P[1][11]*SF[0] + P[2][11]*SPP[0] - P[3][11]*SPP[2]) - (q0*(P[4][12] + P[0][12]*SF[2] + P[1][12]*SF[0] + P[2][12]*SPP[0] - P[3][12]*SPP[2]))/2; - nextP[4][4] = P[4][4] + P[0][4]*SF[2] + P[1][4]*SF[0] + P[2][4]*SPP[0] - P[3][4]*SPP[2] + dvyCov*sq(SG[7] - 2*q0*q3) + dvzCov*sq(SG[6] + 2*q0*q2) + SF[2]*(P[4][0] + P[0][0]*SF[2] + P[1][0]*SF[0] + P[2][0]*SPP[0] - P[3][0]*SPP[2]) + SF[0]*(P[4][1] + P[0][1]*SF[2] + P[1][1]*SF[0] + P[2][1]*SPP[0] - P[3][1]*SPP[2]) + SPP[0]*(P[4][2] + P[0][2]*SF[2] + P[1][2]*SF[0] + P[2][2]*SPP[0] - P[3][2]*SPP[2]) - SPP[2]*(P[4][3] + P[0][3]*SF[2] + P[1][3]*SF[0] + P[2][3]*SPP[0] - P[3][3]*SPP[2]) + dvxCov*sq(SG[1] + SG[2] - SG[3] - SG[4]); - nextP[4][5] = P[4][5] + SQ[2] + P[0][5]*SF[2] + P[1][5]*SF[0] + P[2][5]*SPP[0] - P[3][5]*SPP[2] + SF[1]*(P[4][0] + P[0][0]*SF[2] + P[1][0]*SF[0] + P[2][0]*SPP[0] - P[3][0]*SPP[2]) + SF[0]*(P[4][2] + P[0][2]*SF[2] + P[1][2]*SF[0] + P[2][2]*SPP[0] - P[3][2]*SPP[2]) + SF[2]*(P[4][3] + P[0][3]*SF[2] + P[1][3]*SF[0] + P[2][3]*SPP[0] - P[3][3]*SPP[2]) - SPP[0]*(P[4][1] + P[0][1]*SF[2] + P[1][1]*SF[0] + P[2][1]*SPP[0] - P[3][1]*SPP[2]); - nextP[4][6] = P[4][6] + SQ[1] + P[0][6]*SF[2] + P[1][6]*SF[0] + P[2][6]*SPP[0] - P[3][6]*SPP[2] + SF[1]*(P[4][1] + P[0][1]*SF[2] + P[1][1]*SF[0] + P[2][1]*SPP[0] - P[3][1]*SPP[2]) + SF[0]*(P[4][3] + P[0][3]*SF[2] + P[1][3]*SF[0] + P[2][3]*SPP[0] - P[3][3]*SPP[2]) + SPP[0]*(P[4][0] + P[0][0]*SF[2] + P[1][0]*SF[0] + P[2][0]*SPP[0] - P[3][0]*SPP[2]) - SPP[1]*(P[4][2] + P[0][2]*SF[2] + P[1][2]*SF[0] + P[2][2]*SPP[0] - P[3][2]*SPP[2]); - nextP[4][7] = P[4][7] + P[0][7]*SF[2] + P[1][7]*SF[0] + P[2][7]*SPP[0] - P[3][7]*SPP[2] + dt*(P[4][4] + P[0][4]*SF[2] + P[1][4]*SF[0] + P[2][4]*SPP[0] - P[3][4]*SPP[2]); - nextP[4][8] = P[4][8] + P[0][8]*SF[2] + P[1][8]*SF[0] + P[2][8]*SPP[0] - P[3][8]*SPP[2] + dt*(P[4][5] + P[0][5]*SF[2] + P[1][5]*SF[0] + P[2][5]*SPP[0] - P[3][5]*SPP[2]); - nextP[4][9] = P[4][9] + P[0][9]*SF[2] + P[1][9]*SF[0] + P[2][9]*SPP[0] - P[3][9]*SPP[2] + dt*(P[4][6] + P[0][6]*SF[2] + P[1][6]*SF[0] + P[2][6]*SPP[0] - P[3][6]*SPP[2]); - nextP[4][10] = P[4][10] + P[0][10]*SF[2] + P[1][10]*SF[0] + P[2][10]*SPP[0] - P[3][10]*SPP[2]; - nextP[4][11] = P[4][11] + P[0][11]*SF[2] + P[1][11]*SF[0] + P[2][11]*SPP[0] - P[3][11]*SPP[2]; - nextP[4][12] = P[4][12] + P[0][12]*SF[2] + P[1][12]*SF[0] + P[2][12]*SPP[0] - P[3][12]*SPP[2]; - nextP[4][13] = P[4][13] + P[0][13]*SF[2] + P[1][13]*SF[0] + P[2][13]*SPP[0] - P[3][13]*SPP[2]; - nextP[4][14] = P[4][14] + P[0][14]*SF[2] + P[1][14]*SF[0] + P[2][14]*SPP[0] - P[3][14]*SPP[2]; - nextP[4][15] = P[4][15] + P[0][15]*SF[2] + P[1][15]*SF[0] + P[2][15]*SPP[0] - P[3][15]*SPP[2]; - nextP[4][16] = P[4][16] + P[0][16]*SF[2] + P[1][16]*SF[0] + P[2][16]*SPP[0] - P[3][16]*SPP[2]; - nextP[4][17] = P[4][17] + P[0][17]*SF[2] + P[1][17]*SF[0] + P[2][17]*SPP[0] - P[3][17]*SPP[2]; - nextP[4][18] = P[4][18] + P[0][18]*SF[2] + P[1][18]*SF[0] + P[2][18]*SPP[0] - P[3][18]*SPP[2]; - nextP[4][19] = P[4][19] + P[0][19]*SF[2] + P[1][19]*SF[0] + P[2][19]*SPP[0] - P[3][19]*SPP[2]; - nextP[4][20] = P[4][20] + P[0][20]*SF[2] + P[1][20]*SF[0] + P[2][20]*SPP[0] - P[3][20]*SPP[2]; - nextP[5][0] = P[5][0] + P[0][0]*SF[1] + P[2][0]*SF[0] + P[3][0]*SF[2] - P[1][0]*SPP[0] + SF[6]*(P[5][1] + P[0][1]*SF[1] + P[2][1]*SF[0] + P[3][1]*SF[2] - P[1][1]*SPP[0]) + SPP[7]*(P[5][2] + P[0][2]*SF[1] + P[2][2]*SF[0] + P[3][2]*SF[2] - P[1][2]*SPP[0]) + SPP[6]*(P[5][3] + P[0][3]*SF[1] + P[2][3]*SF[0] + P[3][3]*SF[2] - P[1][3]*SPP[0]) + SPP[5]*(P[5][10] + P[0][10]*SF[1] + P[2][10]*SF[0] + P[3][10]*SF[2] - P[1][10]*SPP[0]) + SPP[4]*(P[5][11] + P[0][11]*SF[1] + P[2][11]*SF[0] + P[3][11]*SF[2] - P[1][11]*SPP[0]) + SPP[3]*(P[5][12] + P[0][12]*SF[1] + P[2][12]*SF[0] + P[3][12]*SF[2] - P[1][12]*SPP[0]); - nextP[5][1] = P[5][1] + P[0][1]*SF[1] + P[2][1]*SF[0] + P[3][1]*SF[2] - P[1][1]*SPP[0] + SF[5]*(P[5][0] + P[0][0]*SF[1] + P[2][0]*SF[0] + P[3][0]*SF[2] - P[1][0]*SPP[0]) + SF[4]*(P[5][2] + P[0][2]*SF[1] + P[2][2]*SF[0] + P[3][2]*SF[2] - P[1][2]*SPP[0]) + SPP[7]*(P[5][3] + P[0][3]*SF[1] + P[2][3]*SF[0] + P[3][3]*SF[2] - P[1][3]*SPP[0]) + SPP[3]*(P[5][11] + P[0][11]*SF[1] + P[2][11]*SF[0] + P[3][11]*SF[2] - P[1][11]*SPP[0]) - SPP[4]*(P[5][12] + P[0][12]*SF[1] + P[2][12]*SF[0] + P[3][12]*SF[2] - P[1][12]*SPP[0]) - (q0*(P[5][10] + P[0][10]*SF[1] + P[2][10]*SF[0] + P[3][10]*SF[2] - P[1][10]*SPP[0]))/2; - nextP[5][2] = P[5][2] + P[0][2]*SF[1] + P[2][2]*SF[0] + P[3][2]*SF[2] - P[1][2]*SPP[0] + SF[3]*(P[5][0] + P[0][0]*SF[1] + P[2][0]*SF[0] + P[3][0]*SF[2] - P[1][0]*SPP[0]) + SF[5]*(P[5][3] + P[0][3]*SF[1] + P[2][3]*SF[0] + P[3][3]*SF[2] - P[1][3]*SPP[0]) + SPP[6]*(P[5][1] + P[0][1]*SF[1] + P[2][1]*SF[0] + P[3][1]*SF[2] - P[1][1]*SPP[0]) - SPP[3]*(P[5][10] + P[0][10]*SF[1] + P[2][10]*SF[0] + P[3][10]*SF[2] - P[1][10]*SPP[0]) + SPP[5]*(P[5][12] + P[0][12]*SF[1] + P[2][12]*SF[0] + P[3][12]*SF[2] - P[1][12]*SPP[0]) - (q0*(P[5][11] + P[0][11]*SF[1] + P[2][11]*SF[0] + P[3][11]*SF[2] - P[1][11]*SPP[0]))/2; - nextP[5][3] = P[5][3] + P[0][3]*SF[1] + P[2][3]*SF[0] + P[3][3]*SF[2] - P[1][3]*SPP[0] + SF[4]*(P[5][0] + P[0][0]*SF[1] + P[2][0]*SF[0] + P[3][0]*SF[2] - P[1][0]*SPP[0]) + SF[3]*(P[5][1] + P[0][1]*SF[1] + P[2][1]*SF[0] + P[3][1]*SF[2] - P[1][1]*SPP[0]) + SF[6]*(P[5][2] + P[0][2]*SF[1] + P[2][2]*SF[0] + P[3][2]*SF[2] - P[1][2]*SPP[0]) + SPP[4]*(P[5][10] + P[0][10]*SF[1] + P[2][10]*SF[0] + P[3][10]*SF[2] - P[1][10]*SPP[0]) - SPP[5]*(P[5][11] + P[0][11]*SF[1] + P[2][11]*SF[0] + P[3][11]*SF[2] - P[1][11]*SPP[0]) - (q0*(P[5][12] + P[0][12]*SF[1] + P[2][12]*SF[0] + P[3][12]*SF[2] - P[1][12]*SPP[0]))/2; - nextP[5][4] = P[5][4] + SQ[2] + P[0][4]*SF[1] + P[2][4]*SF[0] + P[3][4]*SF[2] - P[1][4]*SPP[0] + SF[2]*(P[5][0] + P[0][0]*SF[1] + P[2][0]*SF[0] + P[3][0]*SF[2] - P[1][0]*SPP[0]) + SF[0]*(P[5][1] + P[0][1]*SF[1] + P[2][1]*SF[0] + P[3][1]*SF[2] - P[1][1]*SPP[0]) + SPP[0]*(P[5][2] + P[0][2]*SF[1] + P[2][2]*SF[0] + P[3][2]*SF[2] - P[1][2]*SPP[0]) - SPP[2]*(P[5][3] + P[0][3]*SF[1] + P[2][3]*SF[0] + P[3][3]*SF[2] - P[1][3]*SPP[0]); - nextP[5][5] = P[5][5] + P[0][5]*SF[1] + P[2][5]*SF[0] + P[3][5]*SF[2] - P[1][5]*SPP[0] + dvxCov*sq(SG[7] + 2*q0*q3) + dvzCov*sq(SG[5] - 2*q0*q1) + SF[1]*(P[5][0] + P[0][0]*SF[1] + P[2][0]*SF[0] + P[3][0]*SF[2] - P[1][0]*SPP[0]) + SF[0]*(P[5][2] + P[0][2]*SF[1] + P[2][2]*SF[0] + P[3][2]*SF[2] - P[1][2]*SPP[0]) + SF[2]*(P[5][3] + P[0][3]*SF[1] + P[2][3]*SF[0] + P[3][3]*SF[2] - P[1][3]*SPP[0]) - SPP[0]*(P[5][1] + P[0][1]*SF[1] + P[2][1]*SF[0] + P[3][1]*SF[2] - P[1][1]*SPP[0]) + dvyCov*sq(SG[1] - SG[2] + SG[3] - SG[4]); - nextP[5][6] = P[5][6] + SQ[0] + P[0][6]*SF[1] + P[2][6]*SF[0] + P[3][6]*SF[2] - P[1][6]*SPP[0] + SF[1]*(P[5][1] + P[0][1]*SF[1] + P[2][1]*SF[0] + P[3][1]*SF[2] - P[1][1]*SPP[0]) + SF[0]*(P[5][3] + P[0][3]*SF[1] + P[2][3]*SF[0] + P[3][3]*SF[2] - P[1][3]*SPP[0]) + SPP[0]*(P[5][0] + P[0][0]*SF[1] + P[2][0]*SF[0] + P[3][0]*SF[2] - P[1][0]*SPP[0]) - SPP[1]*(P[5][2] + P[0][2]*SF[1] + P[2][2]*SF[0] + P[3][2]*SF[2] - P[1][2]*SPP[0]); - nextP[5][7] = P[5][7] + P[0][7]*SF[1] + P[2][7]*SF[0] + P[3][7]*SF[2] - P[1][7]*SPP[0] + dt*(P[5][4] + P[0][4]*SF[1] + P[2][4]*SF[0] + P[3][4]*SF[2] - P[1][4]*SPP[0]); - nextP[5][8] = P[5][8] + P[0][8]*SF[1] + P[2][8]*SF[0] + P[3][8]*SF[2] - P[1][8]*SPP[0] + dt*(P[5][5] + P[0][5]*SF[1] + P[2][5]*SF[0] + P[3][5]*SF[2] - P[1][5]*SPP[0]); - nextP[5][9] = P[5][9] + P[0][9]*SF[1] + P[2][9]*SF[0] + P[3][9]*SF[2] - P[1][9]*SPP[0] + dt*(P[5][6] + P[0][6]*SF[1] + P[2][6]*SF[0] + P[3][6]*SF[2] - P[1][6]*SPP[0]); - nextP[5][10] = P[5][10] + P[0][10]*SF[1] + P[2][10]*SF[0] + P[3][10]*SF[2] - P[1][10]*SPP[0]; - nextP[5][11] = P[5][11] + P[0][11]*SF[1] + P[2][11]*SF[0] + P[3][11]*SF[2] - P[1][11]*SPP[0]; - nextP[5][12] = P[5][12] + P[0][12]*SF[1] + P[2][12]*SF[0] + P[3][12]*SF[2] - P[1][12]*SPP[0]; - nextP[5][13] = P[5][13] + P[0][13]*SF[1] + P[2][13]*SF[0] + P[3][13]*SF[2] - P[1][13]*SPP[0]; - nextP[5][14] = P[5][14] + P[0][14]*SF[1] + P[2][14]*SF[0] + P[3][14]*SF[2] - P[1][14]*SPP[0]; - nextP[5][15] = P[5][15] + P[0][15]*SF[1] + P[2][15]*SF[0] + P[3][15]*SF[2] - P[1][15]*SPP[0]; - nextP[5][16] = P[5][16] + P[0][16]*SF[1] + P[2][16]*SF[0] + P[3][16]*SF[2] - P[1][16]*SPP[0]; - nextP[5][17] = P[5][17] + P[0][17]*SF[1] + P[2][17]*SF[0] + P[3][17]*SF[2] - P[1][17]*SPP[0]; - nextP[5][18] = P[5][18] + P[0][18]*SF[1] + P[2][18]*SF[0] + P[3][18]*SF[2] - P[1][18]*SPP[0]; - nextP[5][19] = P[5][19] + P[0][19]*SF[1] + P[2][19]*SF[0] + P[3][19]*SF[2] - P[1][19]*SPP[0]; - nextP[5][20] = P[5][20] + P[0][20]*SF[1] + P[2][20]*SF[0] + P[3][20]*SF[2] - P[1][20]*SPP[0]; - nextP[6][0] = P[6][0] + P[1][0]*SF[1] + P[3][0]*SF[0] + P[0][0]*SPP[0] - P[2][0]*SPP[1] + SF[6]*(P[6][1] + P[1][1]*SF[1] + P[3][1]*SF[0] + P[0][1]*SPP[0] - P[2][1]*SPP[1]) + SPP[7]*(P[6][2] + P[1][2]*SF[1] + P[3][2]*SF[0] + P[0][2]*SPP[0] - P[2][2]*SPP[1]) + SPP[6]*(P[6][3] + P[1][3]*SF[1] + P[3][3]*SF[0] + P[0][3]*SPP[0] - P[2][3]*SPP[1]) + SPP[5]*(P[6][10] + P[1][10]*SF[1] + P[3][10]*SF[0] + P[0][10]*SPP[0] - P[2][10]*SPP[1]) + SPP[4]*(P[6][11] + P[1][11]*SF[1] + P[3][11]*SF[0] + P[0][11]*SPP[0] - P[2][11]*SPP[1]) + SPP[3]*(P[6][12] + P[1][12]*SF[1] + P[3][12]*SF[0] + P[0][12]*SPP[0] - P[2][12]*SPP[1]); - nextP[6][1] = P[6][1] + P[1][1]*SF[1] + P[3][1]*SF[0] + P[0][1]*SPP[0] - P[2][1]*SPP[1] + SF[5]*(P[6][0] + P[1][0]*SF[1] + P[3][0]*SF[0] + P[0][0]*SPP[0] - P[2][0]*SPP[1]) + SF[4]*(P[6][2] + P[1][2]*SF[1] + P[3][2]*SF[0] + P[0][2]*SPP[0] - P[2][2]*SPP[1]) + SPP[7]*(P[6][3] + P[1][3]*SF[1] + P[3][3]*SF[0] + P[0][3]*SPP[0] - P[2][3]*SPP[1]) + SPP[3]*(P[6][11] + P[1][11]*SF[1] + P[3][11]*SF[0] + P[0][11]*SPP[0] - P[2][11]*SPP[1]) - SPP[4]*(P[6][12] + P[1][12]*SF[1] + P[3][12]*SF[0] + P[0][12]*SPP[0] - P[2][12]*SPP[1]) - (q0*(P[6][10] + P[1][10]*SF[1] + P[3][10]*SF[0] + P[0][10]*SPP[0] - P[2][10]*SPP[1]))/2; - nextP[6][2] = P[6][2] + P[1][2]*SF[1] + P[3][2]*SF[0] + P[0][2]*SPP[0] - P[2][2]*SPP[1] + SF[3]*(P[6][0] + P[1][0]*SF[1] + P[3][0]*SF[0] + P[0][0]*SPP[0] - P[2][0]*SPP[1]) + SF[5]*(P[6][3] + P[1][3]*SF[1] + P[3][3]*SF[0] + P[0][3]*SPP[0] - P[2][3]*SPP[1]) + SPP[6]*(P[6][1] + P[1][1]*SF[1] + P[3][1]*SF[0] + P[0][1]*SPP[0] - P[2][1]*SPP[1]) - SPP[3]*(P[6][10] + P[1][10]*SF[1] + P[3][10]*SF[0] + P[0][10]*SPP[0] - P[2][10]*SPP[1]) + SPP[5]*(P[6][12] + P[1][12]*SF[1] + P[3][12]*SF[0] + P[0][12]*SPP[0] - P[2][12]*SPP[1]) - (q0*(P[6][11] + P[1][11]*SF[1] + P[3][11]*SF[0] + P[0][11]*SPP[0] - P[2][11]*SPP[1]))/2; - nextP[6][3] = P[6][3] + P[1][3]*SF[1] + P[3][3]*SF[0] + P[0][3]*SPP[0] - P[2][3]*SPP[1] + SF[4]*(P[6][0] + P[1][0]*SF[1] + P[3][0]*SF[0] + P[0][0]*SPP[0] - P[2][0]*SPP[1]) + SF[3]*(P[6][1] + P[1][1]*SF[1] + P[3][1]*SF[0] + P[0][1]*SPP[0] - P[2][1]*SPP[1]) + SF[6]*(P[6][2] + P[1][2]*SF[1] + P[3][2]*SF[0] + P[0][2]*SPP[0] - P[2][2]*SPP[1]) + SPP[4]*(P[6][10] + P[1][10]*SF[1] + P[3][10]*SF[0] + P[0][10]*SPP[0] - P[2][10]*SPP[1]) - SPP[5]*(P[6][11] + P[1][11]*SF[1] + P[3][11]*SF[0] + P[0][11]*SPP[0] - P[2][11]*SPP[1]) - (q0*(P[6][12] + P[1][12]*SF[1] + P[3][12]*SF[0] + P[0][12]*SPP[0] - P[2][12]*SPP[1]))/2; - nextP[6][4] = P[6][4] + SQ[1] + P[1][4]*SF[1] + P[3][4]*SF[0] + P[0][4]*SPP[0] - P[2][4]*SPP[1] + SF[2]*(P[6][0] + P[1][0]*SF[1] + P[3][0]*SF[0] + P[0][0]*SPP[0] - P[2][0]*SPP[1]) + SF[0]*(P[6][1] + P[1][1]*SF[1] + P[3][1]*SF[0] + P[0][1]*SPP[0] - P[2][1]*SPP[1]) + SPP[0]*(P[6][2] + P[1][2]*SF[1] + P[3][2]*SF[0] + P[0][2]*SPP[0] - P[2][2]*SPP[1]) - SPP[2]*(P[6][3] + P[1][3]*SF[1] + P[3][3]*SF[0] + P[0][3]*SPP[0] - P[2][3]*SPP[1]); - nextP[6][5] = P[6][5] + SQ[0] + P[1][5]*SF[1] + P[3][5]*SF[0] + P[0][5]*SPP[0] - P[2][5]*SPP[1] + SF[1]*(P[6][0] + P[1][0]*SF[1] + P[3][0]*SF[0] + P[0][0]*SPP[0] - P[2][0]*SPP[1]) + SF[0]*(P[6][2] + P[1][2]*SF[1] + P[3][2]*SF[0] + P[0][2]*SPP[0] - P[2][2]*SPP[1]) + SF[2]*(P[6][3] + P[1][3]*SF[1] + P[3][3]*SF[0] + P[0][3]*SPP[0] - P[2][3]*SPP[1]) - SPP[0]*(P[6][1] + P[1][1]*SF[1] + P[3][1]*SF[0] + P[0][1]*SPP[0] - P[2][1]*SPP[1]); - nextP[6][6] = P[6][6] + P[1][6]*SF[1] + P[3][6]*SF[0] + P[0][6]*SPP[0] - P[2][6]*SPP[1] + dvxCov*sq(SG[6] - 2*q0*q2) + dvyCov*sq(SG[5] + 2*q0*q1) + SF[1]*(P[6][1] + P[1][1]*SF[1] + P[3][1]*SF[0] + P[0][1]*SPP[0] - P[2][1]*SPP[1]) + SF[0]*(P[6][3] + P[1][3]*SF[1] + P[3][3]*SF[0] + P[0][3]*SPP[0] - P[2][3]*SPP[1]) + SPP[0]*(P[6][0] + P[1][0]*SF[1] + P[3][0]*SF[0] + P[0][0]*SPP[0] - P[2][0]*SPP[1]) - SPP[1]*(P[6][2] + P[1][2]*SF[1] + P[3][2]*SF[0] + P[0][2]*SPP[0] - P[2][2]*SPP[1]) + dvzCov*sq(SG[1] - SG[2] - SG[3] + SG[4]); - nextP[6][7] = P[6][7] + P[1][7]*SF[1] + P[3][7]*SF[0] + P[0][7]*SPP[0] - P[2][7]*SPP[1] + dt*(P[6][4] + P[1][4]*SF[1] + P[3][4]*SF[0] + P[0][4]*SPP[0] - P[2][4]*SPP[1]); - nextP[6][8] = P[6][8] + P[1][8]*SF[1] + P[3][8]*SF[0] + P[0][8]*SPP[0] - P[2][8]*SPP[1] + dt*(P[6][5] + P[1][5]*SF[1] + P[3][5]*SF[0] + P[0][5]*SPP[0] - P[2][5]*SPP[1]); - nextP[6][9] = P[6][9] + P[1][9]*SF[1] + P[3][9]*SF[0] + P[0][9]*SPP[0] - P[2][9]*SPP[1] + dt*(P[6][6] + P[1][6]*SF[1] + P[3][6]*SF[0] + P[0][6]*SPP[0] - P[2][6]*SPP[1]); - nextP[6][10] = P[6][10] + P[1][10]*SF[1] + P[3][10]*SF[0] + P[0][10]*SPP[0] - P[2][10]*SPP[1]; - nextP[6][11] = P[6][11] + P[1][11]*SF[1] + P[3][11]*SF[0] + P[0][11]*SPP[0] - P[2][11]*SPP[1]; - nextP[6][12] = P[6][12] + P[1][12]*SF[1] + P[3][12]*SF[0] + P[0][12]*SPP[0] - P[2][12]*SPP[1]; - nextP[6][13] = P[6][13] + P[1][13]*SF[1] + P[3][13]*SF[0] + P[0][13]*SPP[0] - P[2][13]*SPP[1]; - nextP[6][14] = P[6][14] + P[1][14]*SF[1] + P[3][14]*SF[0] + P[0][14]*SPP[0] - P[2][14]*SPP[1]; - nextP[6][15] = P[6][15] + P[1][15]*SF[1] + P[3][15]*SF[0] + P[0][15]*SPP[0] - P[2][15]*SPP[1]; - nextP[6][16] = P[6][16] + P[1][16]*SF[1] + P[3][16]*SF[0] + P[0][16]*SPP[0] - P[2][16]*SPP[1]; - nextP[6][17] = P[6][17] + P[1][17]*SF[1] + P[3][17]*SF[0] + P[0][17]*SPP[0] - P[2][17]*SPP[1]; - nextP[6][18] = P[6][18] + P[1][18]*SF[1] + P[3][18]*SF[0] + P[0][18]*SPP[0] - P[2][18]*SPP[1]; - nextP[6][19] = P[6][19] + P[1][19]*SF[1] + P[3][19]*SF[0] + P[0][19]*SPP[0] - P[2][19]*SPP[1]; - nextP[6][20] = P[6][20] + P[1][20]*SF[1] + P[3][20]*SF[0] + P[0][20]*SPP[0] - P[2][20]*SPP[1]; - nextP[7][0] = P[7][0] + P[4][0]*dt + SF[6]*(P[7][1] + P[4][1]*dt) + SPP[7]*(P[7][2] + P[4][2]*dt) + SPP[6]*(P[7][3] + P[4][3]*dt) + SPP[5]*(P[7][10] + P[4][10]*dt) + SPP[4]*(P[7][11] + P[4][11]*dt) + SPP[3]*(P[7][12] + P[4][12]*dt); - nextP[7][1] = P[7][1] + P[4][1]*dt + SF[5]*(P[7][0] + P[4][0]*dt) + SF[4]*(P[7][2] + P[4][2]*dt) + SPP[7]*(P[7][3] + P[4][3]*dt) + SPP[3]*(P[7][11] + P[4][11]*dt) - SPP[4]*(P[7][12] + P[4][12]*dt) - (q0*(P[7][10] + P[4][10]*dt))/2; - nextP[7][2] = P[7][2] + P[4][2]*dt + SF[3]*(P[7][0] + P[4][0]*dt) + SF[5]*(P[7][3] + P[4][3]*dt) + SPP[6]*(P[7][1] + P[4][1]*dt) - SPP[3]*(P[7][10] + P[4][10]*dt) + SPP[5]*(P[7][12] + P[4][12]*dt) - (q0*(P[7][11] + P[4][11]*dt))/2; - nextP[7][3] = P[7][3] + P[4][3]*dt + SF[4]*(P[7][0] + P[4][0]*dt) + SF[3]*(P[7][1] + P[4][1]*dt) + SF[6]*(P[7][2] + P[4][2]*dt) + SPP[4]*(P[7][10] + P[4][10]*dt) - SPP[5]*(P[7][11] + P[4][11]*dt) - (q0*(P[7][12] + P[4][12]*dt))/2; - nextP[7][4] = P[7][4] + P[4][4]*dt + SF[0]*(P[7][1] + P[4][1]*dt) + SF[2]*(P[7][0] + P[4][0]*dt) + SPP[0]*(P[7][2] + P[4][2]*dt) - SPP[2]*(P[7][3] + P[4][3]*dt); - nextP[7][5] = P[7][5] + P[4][5]*dt + SF[1]*(P[7][0] + P[4][0]*dt) + SF[0]*(P[7][2] + P[4][2]*dt) + SF[2]*(P[7][3] + P[4][3]*dt) - SPP[0]*(P[7][1] + P[4][1]*dt); - nextP[7][6] = P[7][6] + P[4][6]*dt + SF[1]*(P[7][1] + P[4][1]*dt) + SF[0]*(P[7][3] + P[4][3]*dt) + SPP[0]*(P[7][0] + P[4][0]*dt) - SPP[1]*(P[7][2] + P[4][2]*dt); + nextP[0][0] = P[0][0] + P[1][0]*SF[7] + P[2][0]*SF[9] + P[3][0]*SF[8] + P[10][0]*SF[11] + P[11][0]*SPP[7] + P[12][0]*SPP[6] + (daxCov*SQ[10])/4 + SF[7]*(P[0][1] + P[1][1]*SF[7] + P[2][1]*SF[9] + P[3][1]*SF[8] + P[10][1]*SF[11] + P[11][1]*SPP[7] + P[12][1]*SPP[6]) + SF[9]*(P[0][2] + P[1][2]*SF[7] + P[2][2]*SF[9] + P[3][2]*SF[8] + P[10][2]*SF[11] + P[11][2]*SPP[7] + P[12][2]*SPP[6]) + SF[8]*(P[0][3] + P[1][3]*SF[7] + P[2][3]*SF[9] + P[3][3]*SF[8] + P[10][3]*SF[11] + P[11][3]*SPP[7] + P[12][3]*SPP[6]) + SF[11]*(P[0][10] + P[1][10]*SF[7] + P[2][10]*SF[9] + P[3][10]*SF[8] + P[10][10]*SF[11] + P[11][10]*SPP[7] + P[12][10]*SPP[6]) + SPP[7]*(P[0][11] + P[1][11]*SF[7] + P[2][11]*SF[9] + P[3][11]*SF[8] + P[10][11]*SF[11] + P[11][11]*SPP[7] + P[12][11]*SPP[6]) + SPP[6]*(P[0][12] + P[1][12]*SF[7] + P[2][12]*SF[9] + P[3][12]*SF[8] + P[10][12]*SF[11] + P[11][12]*SPP[7] + P[12][12]*SPP[6]) + (dayCov*sq(q2))/4 + (dazCov*sq(q3))/4; + nextP[0][1] = P[0][1] + SQ[8] + P[1][1]*SF[7] + P[2][1]*SF[9] + P[3][1]*SF[8] + P[10][1]*SF[11] + P[11][1]*SPP[7] + P[12][1]*SPP[6] + SF[6]*(P[0][0] + P[1][0]*SF[7] + P[2][0]*SF[9] + P[3][0]*SF[8] + P[10][0]*SF[11] + P[11][0]*SPP[7] + P[12][0]*SPP[6]) + SF[5]*(P[0][2] + P[1][2]*SF[7] + P[2][2]*SF[9] + P[3][2]*SF[8] + P[10][2]*SF[11] + P[11][2]*SPP[7] + P[12][2]*SPP[6]) + SF[9]*(P[0][3] + P[1][3]*SF[7] + P[2][3]*SF[9] + P[3][3]*SF[8] + P[10][3]*SF[11] + P[11][3]*SPP[7] + P[12][3]*SPP[6]) + SPP[6]*(P[0][11] + P[1][11]*SF[7] + P[2][11]*SF[9] + P[3][11]*SF[8] + P[10][11]*SF[11] + P[11][11]*SPP[7] + P[12][11]*SPP[6]) - SPP[7]*(P[0][12] + P[1][12]*SF[7] + P[2][12]*SF[9] + P[3][12]*SF[8] + P[10][12]*SF[11] + P[11][12]*SPP[7] + P[12][12]*SPP[6]) - (q0*(P[0][10] + P[1][10]*SF[7] + P[2][10]*SF[9] + P[3][10]*SF[8] + P[10][10]*SF[11] + P[11][10]*SPP[7] + P[12][10]*SPP[6]))/2; + nextP[0][2] = P[0][2] + SQ[7] + P[1][2]*SF[7] + P[2][2]*SF[9] + P[3][2]*SF[8] + P[10][2]*SF[11] + P[11][2]*SPP[7] + P[12][2]*SPP[6] + SF[4]*(P[0][0] + P[1][0]*SF[7] + P[2][0]*SF[9] + P[3][0]*SF[8] + P[10][0]*SF[11] + P[11][0]*SPP[7] + P[12][0]*SPP[6]) + SF[8]*(P[0][1] + P[1][1]*SF[7] + P[2][1]*SF[9] + P[3][1]*SF[8] + P[10][1]*SF[11] + P[11][1]*SPP[7] + P[12][1]*SPP[6]) + SF[6]*(P[0][3] + P[1][3]*SF[7] + P[2][3]*SF[9] + P[3][3]*SF[8] + P[10][3]*SF[11] + P[11][3]*SPP[7] + P[12][3]*SPP[6]) + SF[11]*(P[0][12] + P[1][12]*SF[7] + P[2][12]*SF[9] + P[3][12]*SF[8] + P[10][12]*SF[11] + P[11][12]*SPP[7] + P[12][12]*SPP[6]) - SPP[6]*(P[0][10] + P[1][10]*SF[7] + P[2][10]*SF[9] + P[3][10]*SF[8] + P[10][10]*SF[11] + P[11][10]*SPP[7] + P[12][10]*SPP[6]) - (q0*(P[0][11] + P[1][11]*SF[7] + P[2][11]*SF[9] + P[3][11]*SF[8] + P[10][11]*SF[11] + P[11][11]*SPP[7] + P[12][11]*SPP[6]))/2; + nextP[0][3] = P[0][3] + SQ[6] + P[1][3]*SF[7] + P[2][3]*SF[9] + P[3][3]*SF[8] + P[10][3]*SF[11] + P[11][3]*SPP[7] + P[12][3]*SPP[6] + SF[5]*(P[0][0] + P[1][0]*SF[7] + P[2][0]*SF[9] + P[3][0]*SF[8] + P[10][0]*SF[11] + P[11][0]*SPP[7] + P[12][0]*SPP[6]) + SF[4]*(P[0][1] + P[1][1]*SF[7] + P[2][1]*SF[9] + P[3][1]*SF[8] + P[10][1]*SF[11] + P[11][1]*SPP[7] + P[12][1]*SPP[6]) + SF[7]*(P[0][2] + P[1][2]*SF[7] + P[2][2]*SF[9] + P[3][2]*SF[8] + P[10][2]*SF[11] + P[11][2]*SPP[7] + P[12][2]*SPP[6]) - SF[11]*(P[0][11] + P[1][11]*SF[7] + P[2][11]*SF[9] + P[3][11]*SF[8] + P[10][11]*SF[11] + P[11][11]*SPP[7] + P[12][11]*SPP[6]) + SPP[7]*(P[0][10] + P[1][10]*SF[7] + P[2][10]*SF[9] + P[3][10]*SF[8] + P[10][10]*SF[11] + P[11][10]*SPP[7] + P[12][10]*SPP[6]) - (q0*(P[0][12] + P[1][12]*SF[7] + P[2][12]*SF[9] + P[3][12]*SF[8] + P[10][12]*SF[11] + P[11][12]*SPP[7] + P[12][12]*SPP[6]))/2; + nextP[0][4] = P[0][4] + P[1][4]*SF[7] + P[2][4]*SF[9] + P[3][4]*SF[8] + P[10][4]*SF[11] + P[11][4]*SPP[7] + P[12][4]*SPP[6] + SF[3]*(P[0][0] + P[1][0]*SF[7] + P[2][0]*SF[9] + P[3][0]*SF[8] + P[10][0]*SF[11] + P[11][0]*SPP[7] + P[12][0]*SPP[6]) + SF[1]*(P[0][1] + P[1][1]*SF[7] + P[2][1]*SF[9] + P[3][1]*SF[8] + P[10][1]*SF[11] + P[11][1]*SPP[7] + P[12][1]*SPP[6]) + SPP[0]*(P[0][2] + P[1][2]*SF[7] + P[2][2]*SF[9] + P[3][2]*SF[8] + P[10][2]*SF[11] + P[11][2]*SPP[7] + P[12][2]*SPP[6]) - SPP[2]*(P[0][3] + P[1][3]*SF[7] + P[2][3]*SF[9] + P[3][3]*SF[8] + P[10][3]*SF[11] + P[11][3]*SPP[7] + P[12][3]*SPP[6]) - SPP[4]*(P[0][13] + P[1][13]*SF[7] + P[2][13]*SF[9] + P[3][13]*SF[8] + P[10][13]*SF[11] + P[11][13]*SPP[7] + P[12][13]*SPP[6]); + nextP[0][5] = P[0][5] + P[1][5]*SF[7] + P[2][5]*SF[9] + P[3][5]*SF[8] + P[10][5]*SF[11] + P[11][5]*SPP[7] + P[12][5]*SPP[6] + SF[2]*(P[0][0] + P[1][0]*SF[7] + P[2][0]*SF[9] + P[3][0]*SF[8] + P[10][0]*SF[11] + P[11][0]*SPP[7] + P[12][0]*SPP[6]) + SF[1]*(P[0][2] + P[1][2]*SF[7] + P[2][2]*SF[9] + P[3][2]*SF[8] + P[10][2]*SF[11] + P[11][2]*SPP[7] + P[12][2]*SPP[6]) + SF[3]*(P[0][3] + P[1][3]*SF[7] + P[2][3]*SF[9] + P[3][3]*SF[8] + P[10][3]*SF[11] + P[11][3]*SPP[7] + P[12][3]*SPP[6]) - SPP[0]*(P[0][1] + P[1][1]*SF[7] + P[2][1]*SF[9] + P[3][1]*SF[8] + P[10][1]*SF[11] + P[11][1]*SPP[7] + P[12][1]*SPP[6]) + SPP[3]*(P[0][13] + P[1][13]*SF[7] + P[2][13]*SF[9] + P[3][13]*SF[8] + P[10][13]*SF[11] + P[11][13]*SPP[7] + P[12][13]*SPP[6]); + nextP[0][6] = P[0][6] + P[1][6]*SF[7] + P[2][6]*SF[9] + P[3][6]*SF[8] + P[10][6]*SF[11] + P[11][6]*SPP[7] + P[12][6]*SPP[6] + SF[2]*(P[0][1] + P[1][1]*SF[7] + P[2][1]*SF[9] + P[3][1]*SF[8] + P[10][1]*SF[11] + P[11][1]*SPP[7] + P[12][1]*SPP[6]) + SF[1]*(P[0][3] + P[1][3]*SF[7] + P[2][3]*SF[9] + P[3][3]*SF[8] + P[10][3]*SF[11] + P[11][3]*SPP[7] + P[12][3]*SPP[6]) + SPP[0]*(P[0][0] + P[1][0]*SF[7] + P[2][0]*SF[9] + P[3][0]*SF[8] + P[10][0]*SF[11] + P[11][0]*SPP[7] + P[12][0]*SPP[6]) - SPP[1]*(P[0][2] + P[1][2]*SF[7] + P[2][2]*SF[9] + P[3][2]*SF[8] + P[10][2]*SF[11] + P[11][2]*SPP[7] + P[12][2]*SPP[6]) - (sq(q0) - sq(q1) - sq(q2) + sq(q3))*(P[0][13] + P[1][13]*SF[7] + P[2][13]*SF[9] + P[3][13]*SF[8] + P[10][13]*SF[11] + P[11][13]*SPP[7] + P[12][13]*SPP[6]); + nextP[0][7] = P[0][7] + P[1][7]*SF[7] + P[2][7]*SF[9] + P[3][7]*SF[8] + P[10][7]*SF[11] + P[11][7]*SPP[7] + P[12][7]*SPP[6] + dt*(P[0][4] + P[1][4]*SF[7] + P[2][4]*SF[9] + P[3][4]*SF[8] + P[10][4]*SF[11] + P[11][4]*SPP[7] + P[12][4]*SPP[6]); + nextP[0][8] = P[0][8] + P[1][8]*SF[7] + P[2][8]*SF[9] + P[3][8]*SF[8] + P[10][8]*SF[11] + P[11][8]*SPP[7] + P[12][8]*SPP[6] + dt*(P[0][5] + P[1][5]*SF[7] + P[2][5]*SF[9] + P[3][5]*SF[8] + P[10][5]*SF[11] + P[11][5]*SPP[7] + P[12][5]*SPP[6]); + nextP[0][9] = P[0][9] + P[1][9]*SF[7] + P[2][9]*SF[9] + P[3][9]*SF[8] + P[10][9]*SF[11] + P[11][9]*SPP[7] + P[12][9]*SPP[6] + dt*(P[0][6] + P[1][6]*SF[7] + P[2][6]*SF[9] + P[3][6]*SF[8] + P[10][6]*SF[11] + P[11][6]*SPP[7] + P[12][6]*SPP[6]); + nextP[0][10] = P[0][10] + P[1][10]*SF[7] + P[2][10]*SF[9] + P[3][10]*SF[8] + P[10][10]*SF[11] + P[11][10]*SPP[7] + P[12][10]*SPP[6]; + nextP[0][11] = P[0][11] + P[1][11]*SF[7] + P[2][11]*SF[9] + P[3][11]*SF[8] + P[10][11]*SF[11] + P[11][11]*SPP[7] + P[12][11]*SPP[6]; + nextP[0][12] = P[0][12] + P[1][12]*SF[7] + P[2][12]*SF[9] + P[3][12]*SF[8] + P[10][12]*SF[11] + P[11][12]*SPP[7] + P[12][12]*SPP[6]; + nextP[0][13] = P[0][13] + P[1][13]*SF[7] + P[2][13]*SF[9] + P[3][13]*SF[8] + P[10][13]*SF[11] + P[11][13]*SPP[7] + P[12][13]*SPP[6]; + nextP[0][14] = P[0][14] + P[1][14]*SF[7] + P[2][14]*SF[9] + P[3][14]*SF[8] + P[10][14]*SF[11] + P[11][14]*SPP[7] + P[12][14]*SPP[6]; + nextP[0][15] = P[0][15] + P[1][15]*SF[7] + P[2][15]*SF[9] + P[3][15]*SF[8] + P[10][15]*SF[11] + P[11][15]*SPP[7] + P[12][15]*SPP[6]; + nextP[0][16] = P[0][16] + P[1][16]*SF[7] + P[2][16]*SF[9] + P[3][16]*SF[8] + P[10][16]*SF[11] + P[11][16]*SPP[7] + P[12][16]*SPP[6]; + nextP[0][17] = P[0][17] + P[1][17]*SF[7] + P[2][17]*SF[9] + P[3][17]*SF[8] + P[10][17]*SF[11] + P[11][17]*SPP[7] + P[12][17]*SPP[6]; + nextP[0][18] = P[0][18] + P[1][18]*SF[7] + P[2][18]*SF[9] + P[3][18]*SF[8] + P[10][18]*SF[11] + P[11][18]*SPP[7] + P[12][18]*SPP[6]; + nextP[0][19] = P[0][19] + P[1][19]*SF[7] + P[2][19]*SF[9] + P[3][19]*SF[8] + P[10][19]*SF[11] + P[11][19]*SPP[7] + P[12][19]*SPP[6]; + nextP[0][20] = P[0][20] + P[1][20]*SF[7] + P[2][20]*SF[9] + P[3][20]*SF[8] + P[10][20]*SF[11] + P[11][20]*SPP[7] + P[12][20]*SPP[6]; + nextP[0][21] = P[0][21] + P[1][21]*SF[7] + P[2][21]*SF[9] + P[3][21]*SF[8] + P[10][21]*SF[11] + P[11][21]*SPP[7] + P[12][21]*SPP[6]; + nextP[0][22] = P[0][22] + P[1][22]*SF[7] + P[2][22]*SF[9] + P[3][22]*SF[8] + P[10][22]*SF[11] + P[11][22]*SPP[7] + P[12][22]*SPP[6]; + nextP[1][0] = P[1][0] + SQ[8] + P[0][0]*SF[6] + P[2][0]*SF[5] + P[3][0]*SF[9] + P[11][0]*SPP[6] - P[12][0]*SPP[7] - (P[10][0]*q0)/2 + SF[7]*(P[1][1] + P[0][1]*SF[6] + P[2][1]*SF[5] + P[3][1]*SF[9] + P[11][1]*SPP[6] - P[12][1]*SPP[7] - (P[10][1]*q0)/2) + SF[9]*(P[1][2] + P[0][2]*SF[6] + P[2][2]*SF[5] + P[3][2]*SF[9] + P[11][2]*SPP[6] - P[12][2]*SPP[7] - (P[10][2]*q0)/2) + SF[8]*(P[1][3] + P[0][3]*SF[6] + P[2][3]*SF[5] + P[3][3]*SF[9] + P[11][3]*SPP[6] - P[12][3]*SPP[7] - (P[10][3]*q0)/2) + SF[11]*(P[1][10] + P[0][10]*SF[6] + P[2][10]*SF[5] + P[3][10]*SF[9] + P[11][10]*SPP[6] - P[12][10]*SPP[7] - (P[10][10]*q0)/2) + SPP[7]*(P[1][11] + P[0][11]*SF[6] + P[2][11]*SF[5] + P[3][11]*SF[9] + P[11][11]*SPP[6] - P[12][11]*SPP[7] - (P[10][11]*q0)/2) + SPP[6]*(P[1][12] + P[0][12]*SF[6] + P[2][12]*SF[5] + P[3][12]*SF[9] + P[11][12]*SPP[6] - P[12][12]*SPP[7] - (P[10][12]*q0)/2); + nextP[1][1] = P[1][1] + P[0][1]*SF[6] + P[2][1]*SF[5] + P[3][1]*SF[9] + P[11][1]*SPP[6] - P[12][1]*SPP[7] + daxCov*SQ[9] - (P[10][1]*q0)/2 + SF[6]*(P[1][0] + P[0][0]*SF[6] + P[2][0]*SF[5] + P[3][0]*SF[9] + P[11][0]*SPP[6] - P[12][0]*SPP[7] - (P[10][0]*q0)/2) + SF[5]*(P[1][2] + P[0][2]*SF[6] + P[2][2]*SF[5] + P[3][2]*SF[9] + P[11][2]*SPP[6] - P[12][2]*SPP[7] - (P[10][2]*q0)/2) + SF[9]*(P[1][3] + P[0][3]*SF[6] + P[2][3]*SF[5] + P[3][3]*SF[9] + P[11][3]*SPP[6] - P[12][3]*SPP[7] - (P[10][3]*q0)/2) + SPP[6]*(P[1][11] + P[0][11]*SF[6] + P[2][11]*SF[5] + P[3][11]*SF[9] + P[11][11]*SPP[6] - P[12][11]*SPP[7] - (P[10][11]*q0)/2) - SPP[7]*(P[1][12] + P[0][12]*SF[6] + P[2][12]*SF[5] + P[3][12]*SF[9] + P[11][12]*SPP[6] - P[12][12]*SPP[7] - (P[10][12]*q0)/2) + (dayCov*sq(q3))/4 + (dazCov*sq(q2))/4 - (q0*(P[1][10] + P[0][10]*SF[6] + P[2][10]*SF[5] + P[3][10]*SF[9] + P[11][10]*SPP[6] - P[12][10]*SPP[7] - (P[10][10]*q0)/2))/2; + nextP[1][2] = P[1][2] + SQ[5] + P[0][2]*SF[6] + P[2][2]*SF[5] + P[3][2]*SF[9] + P[11][2]*SPP[6] - P[12][2]*SPP[7] - (P[10][2]*q0)/2 + SF[4]*(P[1][0] + P[0][0]*SF[6] + P[2][0]*SF[5] + P[3][0]*SF[9] + P[11][0]*SPP[6] - P[12][0]*SPP[7] - (P[10][0]*q0)/2) + SF[8]*(P[1][1] + P[0][1]*SF[6] + P[2][1]*SF[5] + P[3][1]*SF[9] + P[11][1]*SPP[6] - P[12][1]*SPP[7] - (P[10][1]*q0)/2) + SF[6]*(P[1][3] + P[0][3]*SF[6] + P[2][3]*SF[5] + P[3][3]*SF[9] + P[11][3]*SPP[6] - P[12][3]*SPP[7] - (P[10][3]*q0)/2) + SF[11]*(P[1][12] + P[0][12]*SF[6] + P[2][12]*SF[5] + P[3][12]*SF[9] + P[11][12]*SPP[6] - P[12][12]*SPP[7] - (P[10][12]*q0)/2) - SPP[6]*(P[1][10] + P[0][10]*SF[6] + P[2][10]*SF[5] + P[3][10]*SF[9] + P[11][10]*SPP[6] - P[12][10]*SPP[7] - (P[10][10]*q0)/2) - (q0*(P[1][11] + P[0][11]*SF[6] + P[2][11]*SF[5] + P[3][11]*SF[9] + P[11][11]*SPP[6] - P[12][11]*SPP[7] - (P[10][11]*q0)/2))/2; + nextP[1][3] = P[1][3] + SQ[4] + P[0][3]*SF[6] + P[2][3]*SF[5] + P[3][3]*SF[9] + P[11][3]*SPP[6] - P[12][3]*SPP[7] - (P[10][3]*q0)/2 + SF[5]*(P[1][0] + P[0][0]*SF[6] + P[2][0]*SF[5] + P[3][0]*SF[9] + P[11][0]*SPP[6] - P[12][0]*SPP[7] - (P[10][0]*q0)/2) + SF[4]*(P[1][1] + P[0][1]*SF[6] + P[2][1]*SF[5] + P[3][1]*SF[9] + P[11][1]*SPP[6] - P[12][1]*SPP[7] - (P[10][1]*q0)/2) + SF[7]*(P[1][2] + P[0][2]*SF[6] + P[2][2]*SF[5] + P[3][2]*SF[9] + P[11][2]*SPP[6] - P[12][2]*SPP[7] - (P[10][2]*q0)/2) - SF[11]*(P[1][11] + P[0][11]*SF[6] + P[2][11]*SF[5] + P[3][11]*SF[9] + P[11][11]*SPP[6] - P[12][11]*SPP[7] - (P[10][11]*q0)/2) + SPP[7]*(P[1][10] + P[0][10]*SF[6] + P[2][10]*SF[5] + P[3][10]*SF[9] + P[11][10]*SPP[6] - P[12][10]*SPP[7] - (P[10][10]*q0)/2) - (q0*(P[1][12] + P[0][12]*SF[6] + P[2][12]*SF[5] + P[3][12]*SF[9] + P[11][12]*SPP[6] - P[12][12]*SPP[7] - (P[10][12]*q0)/2))/2; + nextP[1][4] = P[1][4] + P[0][4]*SF[6] + P[2][4]*SF[5] + P[3][4]*SF[9] + P[11][4]*SPP[6] - P[12][4]*SPP[7] - (P[10][4]*q0)/2 + SF[3]*(P[1][0] + P[0][0]*SF[6] + P[2][0]*SF[5] + P[3][0]*SF[9] + P[11][0]*SPP[6] - P[12][0]*SPP[7] - (P[10][0]*q0)/2) + SF[1]*(P[1][1] + P[0][1]*SF[6] + P[2][1]*SF[5] + P[3][1]*SF[9] + P[11][1]*SPP[6] - P[12][1]*SPP[7] - (P[10][1]*q0)/2) + SPP[0]*(P[1][2] + P[0][2]*SF[6] + P[2][2]*SF[5] + P[3][2]*SF[9] + P[11][2]*SPP[6] - P[12][2]*SPP[7] - (P[10][2]*q0)/2) - SPP[2]*(P[1][3] + P[0][3]*SF[6] + P[2][3]*SF[5] + P[3][3]*SF[9] + P[11][3]*SPP[6] - P[12][3]*SPP[7] - (P[10][3]*q0)/2) - SPP[4]*(P[1][13] + P[0][13]*SF[6] + P[2][13]*SF[5] + P[3][13]*SF[9] + P[11][13]*SPP[6] - P[12][13]*SPP[7] - (P[10][13]*q0)/2); + nextP[1][5] = P[1][5] + P[0][5]*SF[6] + P[2][5]*SF[5] + P[3][5]*SF[9] + P[11][5]*SPP[6] - P[12][5]*SPP[7] - (P[10][5]*q0)/2 + SF[2]*(P[1][0] + P[0][0]*SF[6] + P[2][0]*SF[5] + P[3][0]*SF[9] + P[11][0]*SPP[6] - P[12][0]*SPP[7] - (P[10][0]*q0)/2) + SF[1]*(P[1][2] + P[0][2]*SF[6] + P[2][2]*SF[5] + P[3][2]*SF[9] + P[11][2]*SPP[6] - P[12][2]*SPP[7] - (P[10][2]*q0)/2) + SF[3]*(P[1][3] + P[0][3]*SF[6] + P[2][3]*SF[5] + P[3][3]*SF[9] + P[11][3]*SPP[6] - P[12][3]*SPP[7] - (P[10][3]*q0)/2) - SPP[0]*(P[1][1] + P[0][1]*SF[6] + P[2][1]*SF[5] + P[3][1]*SF[9] + P[11][1]*SPP[6] - P[12][1]*SPP[7] - (P[10][1]*q0)/2) + SPP[3]*(P[1][13] + P[0][13]*SF[6] + P[2][13]*SF[5] + P[3][13]*SF[9] + P[11][13]*SPP[6] - P[12][13]*SPP[7] - (P[10][13]*q0)/2); + nextP[1][6] = P[1][6] + P[0][6]*SF[6] + P[2][6]*SF[5] + P[3][6]*SF[9] + P[11][6]*SPP[6] - P[12][6]*SPP[7] - (P[10][6]*q0)/2 + SF[2]*(P[1][1] + P[0][1]*SF[6] + P[2][1]*SF[5] + P[3][1]*SF[9] + P[11][1]*SPP[6] - P[12][1]*SPP[7] - (P[10][1]*q0)/2) + SF[1]*(P[1][3] + P[0][3]*SF[6] + P[2][3]*SF[5] + P[3][3]*SF[9] + P[11][3]*SPP[6] - P[12][3]*SPP[7] - (P[10][3]*q0)/2) + SPP[0]*(P[1][0] + P[0][0]*SF[6] + P[2][0]*SF[5] + P[3][0]*SF[9] + P[11][0]*SPP[6] - P[12][0]*SPP[7] - (P[10][0]*q0)/2) - SPP[1]*(P[1][2] + P[0][2]*SF[6] + P[2][2]*SF[5] + P[3][2]*SF[9] + P[11][2]*SPP[6] - P[12][2]*SPP[7] - (P[10][2]*q0)/2) - (sq(q0) - sq(q1) - sq(q2) + sq(q3))*(P[1][13] + P[0][13]*SF[6] + P[2][13]*SF[5] + P[3][13]*SF[9] + P[11][13]*SPP[6] - P[12][13]*SPP[7] - (P[10][13]*q0)/2); + nextP[1][7] = P[1][7] + P[0][7]*SF[6] + P[2][7]*SF[5] + P[3][7]*SF[9] + P[11][7]*SPP[6] - P[12][7]*SPP[7] - (P[10][7]*q0)/2 + dt*(P[1][4] + P[0][4]*SF[6] + P[2][4]*SF[5] + P[3][4]*SF[9] + P[11][4]*SPP[6] - P[12][4]*SPP[7] - (P[10][4]*q0)/2); + nextP[1][8] = P[1][8] + P[0][8]*SF[6] + P[2][8]*SF[5] + P[3][8]*SF[9] + P[11][8]*SPP[6] - P[12][8]*SPP[7] - (P[10][8]*q0)/2 + dt*(P[1][5] + P[0][5]*SF[6] + P[2][5]*SF[5] + P[3][5]*SF[9] + P[11][5]*SPP[6] - P[12][5]*SPP[7] - (P[10][5]*q0)/2); + nextP[1][9] = P[1][9] + P[0][9]*SF[6] + P[2][9]*SF[5] + P[3][9]*SF[9] + P[11][9]*SPP[6] - P[12][9]*SPP[7] - (P[10][9]*q0)/2 + dt*(P[1][6] + P[0][6]*SF[6] + P[2][6]*SF[5] + P[3][6]*SF[9] + P[11][6]*SPP[6] - P[12][6]*SPP[7] - (P[10][6]*q0)/2); + nextP[1][10] = P[1][10] + P[0][10]*SF[6] + P[2][10]*SF[5] + P[3][10]*SF[9] + P[11][10]*SPP[6] - P[12][10]*SPP[7] - (P[10][10]*q0)/2; + nextP[1][11] = P[1][11] + P[0][11]*SF[6] + P[2][11]*SF[5] + P[3][11]*SF[9] + P[11][11]*SPP[6] - P[12][11]*SPP[7] - (P[10][11]*q0)/2; + nextP[1][12] = P[1][12] + P[0][12]*SF[6] + P[2][12]*SF[5] + P[3][12]*SF[9] + P[11][12]*SPP[6] - P[12][12]*SPP[7] - (P[10][12]*q0)/2; + nextP[1][13] = P[1][13] + P[0][13]*SF[6] + P[2][13]*SF[5] + P[3][13]*SF[9] + P[11][13]*SPP[6] - P[12][13]*SPP[7] - (P[10][13]*q0)/2; + nextP[1][14] = P[1][14] + P[0][14]*SF[6] + P[2][14]*SF[5] + P[3][14]*SF[9] + P[11][14]*SPP[6] - P[12][14]*SPP[7] - (P[10][14]*q0)/2; + nextP[1][15] = P[1][15] + P[0][15]*SF[6] + P[2][15]*SF[5] + P[3][15]*SF[9] + P[11][15]*SPP[6] - P[12][15]*SPP[7] - (P[10][15]*q0)/2; + nextP[1][16] = P[1][16] + P[0][16]*SF[6] + P[2][16]*SF[5] + P[3][16]*SF[9] + P[11][16]*SPP[6] - P[12][16]*SPP[7] - (P[10][16]*q0)/2; + nextP[1][17] = P[1][17] + P[0][17]*SF[6] + P[2][17]*SF[5] + P[3][17]*SF[9] + P[11][17]*SPP[6] - P[12][17]*SPP[7] - (P[10][17]*q0)/2; + nextP[1][18] = P[1][18] + P[0][18]*SF[6] + P[2][18]*SF[5] + P[3][18]*SF[9] + P[11][18]*SPP[6] - P[12][18]*SPP[7] - (P[10][18]*q0)/2; + nextP[1][19] = P[1][19] + P[0][19]*SF[6] + P[2][19]*SF[5] + P[3][19]*SF[9] + P[11][19]*SPP[6] - P[12][19]*SPP[7] - (P[10][19]*q0)/2; + nextP[1][20] = P[1][20] + P[0][20]*SF[6] + P[2][20]*SF[5] + P[3][20]*SF[9] + P[11][20]*SPP[6] - P[12][20]*SPP[7] - (P[10][20]*q0)/2; + nextP[1][21] = P[1][21] + P[0][21]*SF[6] + P[2][21]*SF[5] + P[3][21]*SF[9] + P[11][21]*SPP[6] - P[12][21]*SPP[7] - (P[10][21]*q0)/2; + nextP[1][22] = P[1][22] + P[0][22]*SF[6] + P[2][22]*SF[5] + P[3][22]*SF[9] + P[11][22]*SPP[6] - P[12][22]*SPP[7] - (P[10][22]*q0)/2; + nextP[2][0] = P[2][0] + SQ[7] + P[0][0]*SF[4] + P[1][0]*SF[8] + P[3][0]*SF[6] + P[12][0]*SF[11] - P[10][0]*SPP[6] - (P[11][0]*q0)/2 + SF[7]*(P[2][1] + P[0][1]*SF[4] + P[1][1]*SF[8] + P[3][1]*SF[6] + P[12][1]*SF[11] - P[10][1]*SPP[6] - (P[11][1]*q0)/2) + SF[9]*(P[2][2] + P[0][2]*SF[4] + P[1][2]*SF[8] + P[3][2]*SF[6] + P[12][2]*SF[11] - P[10][2]*SPP[6] - (P[11][2]*q0)/2) + SF[8]*(P[2][3] + P[0][3]*SF[4] + P[1][3]*SF[8] + P[3][3]*SF[6] + P[12][3]*SF[11] - P[10][3]*SPP[6] - (P[11][3]*q0)/2) + SF[11]*(P[2][10] + P[0][10]*SF[4] + P[1][10]*SF[8] + P[3][10]*SF[6] + P[12][10]*SF[11] - P[10][10]*SPP[6] - (P[11][10]*q0)/2) + SPP[7]*(P[2][11] + P[0][11]*SF[4] + P[1][11]*SF[8] + P[3][11]*SF[6] + P[12][11]*SF[11] - P[10][11]*SPP[6] - (P[11][11]*q0)/2) + SPP[6]*(P[2][12] + P[0][12]*SF[4] + P[1][12]*SF[8] + P[3][12]*SF[6] + P[12][12]*SF[11] - P[10][12]*SPP[6] - (P[11][12]*q0)/2); + nextP[2][1] = P[2][1] + SQ[5] + P[0][1]*SF[4] + P[1][1]*SF[8] + P[3][1]*SF[6] + P[12][1]*SF[11] - P[10][1]*SPP[6] - (P[11][1]*q0)/2 + SF[6]*(P[2][0] + P[0][0]*SF[4] + P[1][0]*SF[8] + P[3][0]*SF[6] + P[12][0]*SF[11] - P[10][0]*SPP[6] - (P[11][0]*q0)/2) + SF[5]*(P[2][2] + P[0][2]*SF[4] + P[1][2]*SF[8] + P[3][2]*SF[6] + P[12][2]*SF[11] - P[10][2]*SPP[6] - (P[11][2]*q0)/2) + SF[9]*(P[2][3] + P[0][3]*SF[4] + P[1][3]*SF[8] + P[3][3]*SF[6] + P[12][3]*SF[11] - P[10][3]*SPP[6] - (P[11][3]*q0)/2) + SPP[6]*(P[2][11] + P[0][11]*SF[4] + P[1][11]*SF[8] + P[3][11]*SF[6] + P[12][11]*SF[11] - P[10][11]*SPP[6] - (P[11][11]*q0)/2) - SPP[7]*(P[2][12] + P[0][12]*SF[4] + P[1][12]*SF[8] + P[3][12]*SF[6] + P[12][12]*SF[11] - P[10][12]*SPP[6] - (P[11][12]*q0)/2) - (q0*(P[2][10] + P[0][10]*SF[4] + P[1][10]*SF[8] + P[3][10]*SF[6] + P[12][10]*SF[11] - P[10][10]*SPP[6] - (P[11][10]*q0)/2))/2; + nextP[2][2] = P[2][2] + P[0][2]*SF[4] + P[1][2]*SF[8] + P[3][2]*SF[6] + P[12][2]*SF[11] - P[10][2]*SPP[6] + dayCov*SQ[9] + (dazCov*SQ[10])/4 - (P[11][2]*q0)/2 + SF[4]*(P[2][0] + P[0][0]*SF[4] + P[1][0]*SF[8] + P[3][0]*SF[6] + P[12][0]*SF[11] - P[10][0]*SPP[6] - (P[11][0]*q0)/2) + SF[8]*(P[2][1] + P[0][1]*SF[4] + P[1][1]*SF[8] + P[3][1]*SF[6] + P[12][1]*SF[11] - P[10][1]*SPP[6] - (P[11][1]*q0)/2) + SF[6]*(P[2][3] + P[0][3]*SF[4] + P[1][3]*SF[8] + P[3][3]*SF[6] + P[12][3]*SF[11] - P[10][3]*SPP[6] - (P[11][3]*q0)/2) + SF[11]*(P[2][12] + P[0][12]*SF[4] + P[1][12]*SF[8] + P[3][12]*SF[6] + P[12][12]*SF[11] - P[10][12]*SPP[6] - (P[11][12]*q0)/2) - SPP[6]*(P[2][10] + P[0][10]*SF[4] + P[1][10]*SF[8] + P[3][10]*SF[6] + P[12][10]*SF[11] - P[10][10]*SPP[6] - (P[11][10]*q0)/2) + (daxCov*sq(q3))/4 - (q0*(P[2][11] + P[0][11]*SF[4] + P[1][11]*SF[8] + P[3][11]*SF[6] + P[12][11]*SF[11] - P[10][11]*SPP[6] - (P[11][11]*q0)/2))/2; + nextP[2][3] = P[2][3] + SQ[3] + P[0][3]*SF[4] + P[1][3]*SF[8] + P[3][3]*SF[6] + P[12][3]*SF[11] - P[10][3]*SPP[6] - (P[11][3]*q0)/2 + SF[5]*(P[2][0] + P[0][0]*SF[4] + P[1][0]*SF[8] + P[3][0]*SF[6] + P[12][0]*SF[11] - P[10][0]*SPP[6] - (P[11][0]*q0)/2) + SF[4]*(P[2][1] + P[0][1]*SF[4] + P[1][1]*SF[8] + P[3][1]*SF[6] + P[12][1]*SF[11] - P[10][1]*SPP[6] - (P[11][1]*q0)/2) + SF[7]*(P[2][2] + P[0][2]*SF[4] + P[1][2]*SF[8] + P[3][2]*SF[6] + P[12][2]*SF[11] - P[10][2]*SPP[6] - (P[11][2]*q0)/2) - SF[11]*(P[2][11] + P[0][11]*SF[4] + P[1][11]*SF[8] + P[3][11]*SF[6] + P[12][11]*SF[11] - P[10][11]*SPP[6] - (P[11][11]*q0)/2) + SPP[7]*(P[2][10] + P[0][10]*SF[4] + P[1][10]*SF[8] + P[3][10]*SF[6] + P[12][10]*SF[11] - P[10][10]*SPP[6] - (P[11][10]*q0)/2) - (q0*(P[2][12] + P[0][12]*SF[4] + P[1][12]*SF[8] + P[3][12]*SF[6] + P[12][12]*SF[11] - P[10][12]*SPP[6] - (P[11][12]*q0)/2))/2; + nextP[2][4] = P[2][4] + P[0][4]*SF[4] + P[1][4]*SF[8] + P[3][4]*SF[6] + P[12][4]*SF[11] - P[10][4]*SPP[6] - (P[11][4]*q0)/2 + SF[3]*(P[2][0] + P[0][0]*SF[4] + P[1][0]*SF[8] + P[3][0]*SF[6] + P[12][0]*SF[11] - P[10][0]*SPP[6] - (P[11][0]*q0)/2) + SF[1]*(P[2][1] + P[0][1]*SF[4] + P[1][1]*SF[8] + P[3][1]*SF[6] + P[12][1]*SF[11] - P[10][1]*SPP[6] - (P[11][1]*q0)/2) + SPP[0]*(P[2][2] + P[0][2]*SF[4] + P[1][2]*SF[8] + P[3][2]*SF[6] + P[12][2]*SF[11] - P[10][2]*SPP[6] - (P[11][2]*q0)/2) - SPP[2]*(P[2][3] + P[0][3]*SF[4] + P[1][3]*SF[8] + P[3][3]*SF[6] + P[12][3]*SF[11] - P[10][3]*SPP[6] - (P[11][3]*q0)/2) - SPP[4]*(P[2][13] + P[0][13]*SF[4] + P[1][13]*SF[8] + P[3][13]*SF[6] + P[12][13]*SF[11] - P[10][13]*SPP[6] - (P[11][13]*q0)/2); + nextP[2][5] = P[2][5] + P[0][5]*SF[4] + P[1][5]*SF[8] + P[3][5]*SF[6] + P[12][5]*SF[11] - P[10][5]*SPP[6] - (P[11][5]*q0)/2 + SF[2]*(P[2][0] + P[0][0]*SF[4] + P[1][0]*SF[8] + P[3][0]*SF[6] + P[12][0]*SF[11] - P[10][0]*SPP[6] - (P[11][0]*q0)/2) + SF[1]*(P[2][2] + P[0][2]*SF[4] + P[1][2]*SF[8] + P[3][2]*SF[6] + P[12][2]*SF[11] - P[10][2]*SPP[6] - (P[11][2]*q0)/2) + SF[3]*(P[2][3] + P[0][3]*SF[4] + P[1][3]*SF[8] + P[3][3]*SF[6] + P[12][3]*SF[11] - P[10][3]*SPP[6] - (P[11][3]*q0)/2) - SPP[0]*(P[2][1] + P[0][1]*SF[4] + P[1][1]*SF[8] + P[3][1]*SF[6] + P[12][1]*SF[11] - P[10][1]*SPP[6] - (P[11][1]*q0)/2) + SPP[3]*(P[2][13] + P[0][13]*SF[4] + P[1][13]*SF[8] + P[3][13]*SF[6] + P[12][13]*SF[11] - P[10][13]*SPP[6] - (P[11][13]*q0)/2); + nextP[2][6] = P[2][6] + P[0][6]*SF[4] + P[1][6]*SF[8] + P[3][6]*SF[6] + P[12][6]*SF[11] - P[10][6]*SPP[6] - (P[11][6]*q0)/2 + SF[2]*(P[2][1] + P[0][1]*SF[4] + P[1][1]*SF[8] + P[3][1]*SF[6] + P[12][1]*SF[11] - P[10][1]*SPP[6] - (P[11][1]*q0)/2) + SF[1]*(P[2][3] + P[0][3]*SF[4] + P[1][3]*SF[8] + P[3][3]*SF[6] + P[12][3]*SF[11] - P[10][3]*SPP[6] - (P[11][3]*q0)/2) + SPP[0]*(P[2][0] + P[0][0]*SF[4] + P[1][0]*SF[8] + P[3][0]*SF[6] + P[12][0]*SF[11] - P[10][0]*SPP[6] - (P[11][0]*q0)/2) - SPP[1]*(P[2][2] + P[0][2]*SF[4] + P[1][2]*SF[8] + P[3][2]*SF[6] + P[12][2]*SF[11] - P[10][2]*SPP[6] - (P[11][2]*q0)/2) - (sq(q0) - sq(q1) - sq(q2) + sq(q3))*(P[2][13] + P[0][13]*SF[4] + P[1][13]*SF[8] + P[3][13]*SF[6] + P[12][13]*SF[11] - P[10][13]*SPP[6] - (P[11][13]*q0)/2); + nextP[2][7] = P[2][7] + P[0][7]*SF[4] + P[1][7]*SF[8] + P[3][7]*SF[6] + P[12][7]*SF[11] - P[10][7]*SPP[6] - (P[11][7]*q0)/2 + dt*(P[2][4] + P[0][4]*SF[4] + P[1][4]*SF[8] + P[3][4]*SF[6] + P[12][4]*SF[11] - P[10][4]*SPP[6] - (P[11][4]*q0)/2); + nextP[2][8] = P[2][8] + P[0][8]*SF[4] + P[1][8]*SF[8] + P[3][8]*SF[6] + P[12][8]*SF[11] - P[10][8]*SPP[6] - (P[11][8]*q0)/2 + dt*(P[2][5] + P[0][5]*SF[4] + P[1][5]*SF[8] + P[3][5]*SF[6] + P[12][5]*SF[11] - P[10][5]*SPP[6] - (P[11][5]*q0)/2); + nextP[2][9] = P[2][9] + P[0][9]*SF[4] + P[1][9]*SF[8] + P[3][9]*SF[6] + P[12][9]*SF[11] - P[10][9]*SPP[6] - (P[11][9]*q0)/2 + dt*(P[2][6] + P[0][6]*SF[4] + P[1][6]*SF[8] + P[3][6]*SF[6] + P[12][6]*SF[11] - P[10][6]*SPP[6] - (P[11][6]*q0)/2); + nextP[2][10] = P[2][10] + P[0][10]*SF[4] + P[1][10]*SF[8] + P[3][10]*SF[6] + P[12][10]*SF[11] - P[10][10]*SPP[6] - (P[11][10]*q0)/2; + nextP[2][11] = P[2][11] + P[0][11]*SF[4] + P[1][11]*SF[8] + P[3][11]*SF[6] + P[12][11]*SF[11] - P[10][11]*SPP[6] - (P[11][11]*q0)/2; + nextP[2][12] = P[2][12] + P[0][12]*SF[4] + P[1][12]*SF[8] + P[3][12]*SF[6] + P[12][12]*SF[11] - P[10][12]*SPP[6] - (P[11][12]*q0)/2; + nextP[2][13] = P[2][13] + P[0][13]*SF[4] + P[1][13]*SF[8] + P[3][13]*SF[6] + P[12][13]*SF[11] - P[10][13]*SPP[6] - (P[11][13]*q0)/2; + nextP[2][14] = P[2][14] + P[0][14]*SF[4] + P[1][14]*SF[8] + P[3][14]*SF[6] + P[12][14]*SF[11] - P[10][14]*SPP[6] - (P[11][14]*q0)/2; + nextP[2][15] = P[2][15] + P[0][15]*SF[4] + P[1][15]*SF[8] + P[3][15]*SF[6] + P[12][15]*SF[11] - P[10][15]*SPP[6] - (P[11][15]*q0)/2; + nextP[2][16] = P[2][16] + P[0][16]*SF[4] + P[1][16]*SF[8] + P[3][16]*SF[6] + P[12][16]*SF[11] - P[10][16]*SPP[6] - (P[11][16]*q0)/2; + nextP[2][17] = P[2][17] + P[0][17]*SF[4] + P[1][17]*SF[8] + P[3][17]*SF[6] + P[12][17]*SF[11] - P[10][17]*SPP[6] - (P[11][17]*q0)/2; + nextP[2][18] = P[2][18] + P[0][18]*SF[4] + P[1][18]*SF[8] + P[3][18]*SF[6] + P[12][18]*SF[11] - P[10][18]*SPP[6] - (P[11][18]*q0)/2; + nextP[2][19] = P[2][19] + P[0][19]*SF[4] + P[1][19]*SF[8] + P[3][19]*SF[6] + P[12][19]*SF[11] - P[10][19]*SPP[6] - (P[11][19]*q0)/2; + nextP[2][20] = P[2][20] + P[0][20]*SF[4] + P[1][20]*SF[8] + P[3][20]*SF[6] + P[12][20]*SF[11] - P[10][20]*SPP[6] - (P[11][20]*q0)/2; + nextP[2][21] = P[2][21] + P[0][21]*SF[4] + P[1][21]*SF[8] + P[3][21]*SF[6] + P[12][21]*SF[11] - P[10][21]*SPP[6] - (P[11][21]*q0)/2; + nextP[2][22] = P[2][22] + P[0][22]*SF[4] + P[1][22]*SF[8] + P[3][22]*SF[6] + P[12][22]*SF[11] - P[10][22]*SPP[6] - (P[11][22]*q0)/2; + nextP[3][0] = P[3][0] + SQ[6] + P[0][0]*SF[5] + P[1][0]*SF[4] + P[2][0]*SF[7] - P[11][0]*SF[11] + P[10][0]*SPP[7] - (P[12][0]*q0)/2 + SF[7]*(P[3][1] + P[0][1]*SF[5] + P[1][1]*SF[4] + P[2][1]*SF[7] - P[11][1]*SF[11] + P[10][1]*SPP[7] - (P[12][1]*q0)/2) + SF[9]*(P[3][2] + P[0][2]*SF[5] + P[1][2]*SF[4] + P[2][2]*SF[7] - P[11][2]*SF[11] + P[10][2]*SPP[7] - (P[12][2]*q0)/2) + SF[8]*(P[3][3] + P[0][3]*SF[5] + P[1][3]*SF[4] + P[2][3]*SF[7] - P[11][3]*SF[11] + P[10][3]*SPP[7] - (P[12][3]*q0)/2) + SF[11]*(P[3][10] + P[0][10]*SF[5] + P[1][10]*SF[4] + P[2][10]*SF[7] - P[11][10]*SF[11] + P[10][10]*SPP[7] - (P[12][10]*q0)/2) + SPP[7]*(P[3][11] + P[0][11]*SF[5] + P[1][11]*SF[4] + P[2][11]*SF[7] - P[11][11]*SF[11] + P[10][11]*SPP[7] - (P[12][11]*q0)/2) + SPP[6]*(P[3][12] + P[0][12]*SF[5] + P[1][12]*SF[4] + P[2][12]*SF[7] - P[11][12]*SF[11] + P[10][12]*SPP[7] - (P[12][12]*q0)/2); + nextP[3][1] = P[3][1] + SQ[4] + P[0][1]*SF[5] + P[1][1]*SF[4] + P[2][1]*SF[7] - P[11][1]*SF[11] + P[10][1]*SPP[7] - (P[12][1]*q0)/2 + SF[6]*(P[3][0] + P[0][0]*SF[5] + P[1][0]*SF[4] + P[2][0]*SF[7] - P[11][0]*SF[11] + P[10][0]*SPP[7] - (P[12][0]*q0)/2) + SF[5]*(P[3][2] + P[0][2]*SF[5] + P[1][2]*SF[4] + P[2][2]*SF[7] - P[11][2]*SF[11] + P[10][2]*SPP[7] - (P[12][2]*q0)/2) + SF[9]*(P[3][3] + P[0][3]*SF[5] + P[1][3]*SF[4] + P[2][3]*SF[7] - P[11][3]*SF[11] + P[10][3]*SPP[7] - (P[12][3]*q0)/2) + SPP[6]*(P[3][11] + P[0][11]*SF[5] + P[1][11]*SF[4] + P[2][11]*SF[7] - P[11][11]*SF[11] + P[10][11]*SPP[7] - (P[12][11]*q0)/2) - SPP[7]*(P[3][12] + P[0][12]*SF[5] + P[1][12]*SF[4] + P[2][12]*SF[7] - P[11][12]*SF[11] + P[10][12]*SPP[7] - (P[12][12]*q0)/2) - (q0*(P[3][10] + P[0][10]*SF[5] + P[1][10]*SF[4] + P[2][10]*SF[7] - P[11][10]*SF[11] + P[10][10]*SPP[7] - (P[12][10]*q0)/2))/2; + nextP[3][2] = P[3][2] + SQ[3] + P[0][2]*SF[5] + P[1][2]*SF[4] + P[2][2]*SF[7] - P[11][2]*SF[11] + P[10][2]*SPP[7] - (P[12][2]*q0)/2 + SF[4]*(P[3][0] + P[0][0]*SF[5] + P[1][0]*SF[4] + P[2][0]*SF[7] - P[11][0]*SF[11] + P[10][0]*SPP[7] - (P[12][0]*q0)/2) + SF[8]*(P[3][1] + P[0][1]*SF[5] + P[1][1]*SF[4] + P[2][1]*SF[7] - P[11][1]*SF[11] + P[10][1]*SPP[7] - (P[12][1]*q0)/2) + SF[6]*(P[3][3] + P[0][3]*SF[5] + P[1][3]*SF[4] + P[2][3]*SF[7] - P[11][3]*SF[11] + P[10][3]*SPP[7] - (P[12][3]*q0)/2) + SF[11]*(P[3][12] + P[0][12]*SF[5] + P[1][12]*SF[4] + P[2][12]*SF[7] - P[11][12]*SF[11] + P[10][12]*SPP[7] - (P[12][12]*q0)/2) - SPP[6]*(P[3][10] + P[0][10]*SF[5] + P[1][10]*SF[4] + P[2][10]*SF[7] - P[11][10]*SF[11] + P[10][10]*SPP[7] - (P[12][10]*q0)/2) - (q0*(P[3][11] + P[0][11]*SF[5] + P[1][11]*SF[4] + P[2][11]*SF[7] - P[11][11]*SF[11] + P[10][11]*SPP[7] - (P[12][11]*q0)/2))/2; + nextP[3][3] = P[3][3] + P[0][3]*SF[5] + P[1][3]*SF[4] + P[2][3]*SF[7] - P[11][3]*SF[11] + P[10][3]*SPP[7] + (dayCov*SQ[10])/4 + dazCov*SQ[9] - (P[12][3]*q0)/2 + SF[5]*(P[3][0] + P[0][0]*SF[5] + P[1][0]*SF[4] + P[2][0]*SF[7] - P[11][0]*SF[11] + P[10][0]*SPP[7] - (P[12][0]*q0)/2) + SF[4]*(P[3][1] + P[0][1]*SF[5] + P[1][1]*SF[4] + P[2][1]*SF[7] - P[11][1]*SF[11] + P[10][1]*SPP[7] - (P[12][1]*q0)/2) + SF[7]*(P[3][2] + P[0][2]*SF[5] + P[1][2]*SF[4] + P[2][2]*SF[7] - P[11][2]*SF[11] + P[10][2]*SPP[7] - (P[12][2]*q0)/2) - SF[11]*(P[3][11] + P[0][11]*SF[5] + P[1][11]*SF[4] + P[2][11]*SF[7] - P[11][11]*SF[11] + P[10][11]*SPP[7] - (P[12][11]*q0)/2) + SPP[7]*(P[3][10] + P[0][10]*SF[5] + P[1][10]*SF[4] + P[2][10]*SF[7] - P[11][10]*SF[11] + P[10][10]*SPP[7] - (P[12][10]*q0)/2) + (daxCov*sq(q2))/4 - (q0*(P[3][12] + P[0][12]*SF[5] + P[1][12]*SF[4] + P[2][12]*SF[7] - P[11][12]*SF[11] + P[10][12]*SPP[7] - (P[12][12]*q0)/2))/2; + nextP[3][4] = P[3][4] + P[0][4]*SF[5] + P[1][4]*SF[4] + P[2][4]*SF[7] - P[11][4]*SF[11] + P[10][4]*SPP[7] - (P[12][4]*q0)/2 + SF[3]*(P[3][0] + P[0][0]*SF[5] + P[1][0]*SF[4] + P[2][0]*SF[7] - P[11][0]*SF[11] + P[10][0]*SPP[7] - (P[12][0]*q0)/2) + SF[1]*(P[3][1] + P[0][1]*SF[5] + P[1][1]*SF[4] + P[2][1]*SF[7] - P[11][1]*SF[11] + P[10][1]*SPP[7] - (P[12][1]*q0)/2) + SPP[0]*(P[3][2] + P[0][2]*SF[5] + P[1][2]*SF[4] + P[2][2]*SF[7] - P[11][2]*SF[11] + P[10][2]*SPP[7] - (P[12][2]*q0)/2) - SPP[2]*(P[3][3] + P[0][3]*SF[5] + P[1][3]*SF[4] + P[2][3]*SF[7] - P[11][3]*SF[11] + P[10][3]*SPP[7] - (P[12][3]*q0)/2) - SPP[4]*(P[3][13] + P[0][13]*SF[5] + P[1][13]*SF[4] + P[2][13]*SF[7] - P[11][13]*SF[11] + P[10][13]*SPP[7] - (P[12][13]*q0)/2); + nextP[3][5] = P[3][5] + P[0][5]*SF[5] + P[1][5]*SF[4] + P[2][5]*SF[7] - P[11][5]*SF[11] + P[10][5]*SPP[7] - (P[12][5]*q0)/2 + SF[2]*(P[3][0] + P[0][0]*SF[5] + P[1][0]*SF[4] + P[2][0]*SF[7] - P[11][0]*SF[11] + P[10][0]*SPP[7] - (P[12][0]*q0)/2) + SF[1]*(P[3][2] + P[0][2]*SF[5] + P[1][2]*SF[4] + P[2][2]*SF[7] - P[11][2]*SF[11] + P[10][2]*SPP[7] - (P[12][2]*q0)/2) + SF[3]*(P[3][3] + P[0][3]*SF[5] + P[1][3]*SF[4] + P[2][3]*SF[7] - P[11][3]*SF[11] + P[10][3]*SPP[7] - (P[12][3]*q0)/2) - SPP[0]*(P[3][1] + P[0][1]*SF[5] + P[1][1]*SF[4] + P[2][1]*SF[7] - P[11][1]*SF[11] + P[10][1]*SPP[7] - (P[12][1]*q0)/2) + SPP[3]*(P[3][13] + P[0][13]*SF[5] + P[1][13]*SF[4] + P[2][13]*SF[7] - P[11][13]*SF[11] + P[10][13]*SPP[7] - (P[12][13]*q0)/2); + nextP[3][6] = P[3][6] + P[0][6]*SF[5] + P[1][6]*SF[4] + P[2][6]*SF[7] - P[11][6]*SF[11] + P[10][6]*SPP[7] - (P[12][6]*q0)/2 + SF[2]*(P[3][1] + P[0][1]*SF[5] + P[1][1]*SF[4] + P[2][1]*SF[7] - P[11][1]*SF[11] + P[10][1]*SPP[7] - (P[12][1]*q0)/2) + SF[1]*(P[3][3] + P[0][3]*SF[5] + P[1][3]*SF[4] + P[2][3]*SF[7] - P[11][3]*SF[11] + P[10][3]*SPP[7] - (P[12][3]*q0)/2) + SPP[0]*(P[3][0] + P[0][0]*SF[5] + P[1][0]*SF[4] + P[2][0]*SF[7] - P[11][0]*SF[11] + P[10][0]*SPP[7] - (P[12][0]*q0)/2) - SPP[1]*(P[3][2] + P[0][2]*SF[5] + P[1][2]*SF[4] + P[2][2]*SF[7] - P[11][2]*SF[11] + P[10][2]*SPP[7] - (P[12][2]*q0)/2) - (sq(q0) - sq(q1) - sq(q2) + sq(q3))*(P[3][13] + P[0][13]*SF[5] + P[1][13]*SF[4] + P[2][13]*SF[7] - P[11][13]*SF[11] + P[10][13]*SPP[7] - (P[12][13]*q0)/2); + nextP[3][7] = P[3][7] + P[0][7]*SF[5] + P[1][7]*SF[4] + P[2][7]*SF[7] - P[11][7]*SF[11] + P[10][7]*SPP[7] - (P[12][7]*q0)/2 + dt*(P[3][4] + P[0][4]*SF[5] + P[1][4]*SF[4] + P[2][4]*SF[7] - P[11][4]*SF[11] + P[10][4]*SPP[7] - (P[12][4]*q0)/2); + nextP[3][8] = P[3][8] + P[0][8]*SF[5] + P[1][8]*SF[4] + P[2][8]*SF[7] - P[11][8]*SF[11] + P[10][8]*SPP[7] - (P[12][8]*q0)/2 + dt*(P[3][5] + P[0][5]*SF[5] + P[1][5]*SF[4] + P[2][5]*SF[7] - P[11][5]*SF[11] + P[10][5]*SPP[7] - (P[12][5]*q0)/2); + nextP[3][9] = P[3][9] + P[0][9]*SF[5] + P[1][9]*SF[4] + P[2][9]*SF[7] - P[11][9]*SF[11] + P[10][9]*SPP[7] - (P[12][9]*q0)/2 + dt*(P[3][6] + P[0][6]*SF[5] + P[1][6]*SF[4] + P[2][6]*SF[7] - P[11][6]*SF[11] + P[10][6]*SPP[7] - (P[12][6]*q0)/2); + nextP[3][10] = P[3][10] + P[0][10]*SF[5] + P[1][10]*SF[4] + P[2][10]*SF[7] - P[11][10]*SF[11] + P[10][10]*SPP[7] - (P[12][10]*q0)/2; + nextP[3][11] = P[3][11] + P[0][11]*SF[5] + P[1][11]*SF[4] + P[2][11]*SF[7] - P[11][11]*SF[11] + P[10][11]*SPP[7] - (P[12][11]*q0)/2; + nextP[3][12] = P[3][12] + P[0][12]*SF[5] + P[1][12]*SF[4] + P[2][12]*SF[7] - P[11][12]*SF[11] + P[10][12]*SPP[7] - (P[12][12]*q0)/2; + nextP[3][13] = P[3][13] + P[0][13]*SF[5] + P[1][13]*SF[4] + P[2][13]*SF[7] - P[11][13]*SF[11] + P[10][13]*SPP[7] - (P[12][13]*q0)/2; + nextP[3][14] = P[3][14] + P[0][14]*SF[5] + P[1][14]*SF[4] + P[2][14]*SF[7] - P[11][14]*SF[11] + P[10][14]*SPP[7] - (P[12][14]*q0)/2; + nextP[3][15] = P[3][15] + P[0][15]*SF[5] + P[1][15]*SF[4] + P[2][15]*SF[7] - P[11][15]*SF[11] + P[10][15]*SPP[7] - (P[12][15]*q0)/2; + nextP[3][16] = P[3][16] + P[0][16]*SF[5] + P[1][16]*SF[4] + P[2][16]*SF[7] - P[11][16]*SF[11] + P[10][16]*SPP[7] - (P[12][16]*q0)/2; + nextP[3][17] = P[3][17] + P[0][17]*SF[5] + P[1][17]*SF[4] + P[2][17]*SF[7] - P[11][17]*SF[11] + P[10][17]*SPP[7] - (P[12][17]*q0)/2; + nextP[3][18] = P[3][18] + P[0][18]*SF[5] + P[1][18]*SF[4] + P[2][18]*SF[7] - P[11][18]*SF[11] + P[10][18]*SPP[7] - (P[12][18]*q0)/2; + nextP[3][19] = P[3][19] + P[0][19]*SF[5] + P[1][19]*SF[4] + P[2][19]*SF[7] - P[11][19]*SF[11] + P[10][19]*SPP[7] - (P[12][19]*q0)/2; + nextP[3][20] = P[3][20] + P[0][20]*SF[5] + P[1][20]*SF[4] + P[2][20]*SF[7] - P[11][20]*SF[11] + P[10][20]*SPP[7] - (P[12][20]*q0)/2; + nextP[3][21] = P[3][21] + P[0][21]*SF[5] + P[1][21]*SF[4] + P[2][21]*SF[7] - P[11][21]*SF[11] + P[10][21]*SPP[7] - (P[12][21]*q0)/2; + nextP[3][22] = P[3][22] + P[0][22]*SF[5] + P[1][22]*SF[4] + P[2][22]*SF[7] - P[11][22]*SF[11] + P[10][22]*SPP[7] - (P[12][22]*q0)/2; + nextP[4][0] = P[4][0] + P[0][0]*SF[3] + P[1][0]*SF[1] + P[2][0]*SPP[0] - P[3][0]*SPP[2] - P[13][0]*SPP[4] + SF[7]*(P[4][1] + P[0][1]*SF[3] + P[1][1]*SF[1] + P[2][1]*SPP[0] - P[3][1]*SPP[2] - P[13][1]*SPP[4]) + SF[9]*(P[4][2] + P[0][2]*SF[3] + P[1][2]*SF[1] + P[2][2]*SPP[0] - P[3][2]*SPP[2] - P[13][2]*SPP[4]) + SF[8]*(P[4][3] + P[0][3]*SF[3] + P[1][3]*SF[1] + P[2][3]*SPP[0] - P[3][3]*SPP[2] - P[13][3]*SPP[4]) + SF[11]*(P[4][10] + P[0][10]*SF[3] + P[1][10]*SF[1] + P[2][10]*SPP[0] - P[3][10]*SPP[2] - P[13][10]*SPP[4]) + SPP[7]*(P[4][11] + P[0][11]*SF[3] + P[1][11]*SF[1] + P[2][11]*SPP[0] - P[3][11]*SPP[2] - P[13][11]*SPP[4]) + SPP[6]*(P[4][12] + P[0][12]*SF[3] + P[1][12]*SF[1] + P[2][12]*SPP[0] - P[3][12]*SPP[2] - P[13][12]*SPP[4]); + nextP[4][1] = P[4][1] + P[0][1]*SF[3] + P[1][1]*SF[1] + P[2][1]*SPP[0] - P[3][1]*SPP[2] - P[13][1]*SPP[4] + SF[6]*(P[4][0] + P[0][0]*SF[3] + P[1][0]*SF[1] + P[2][0]*SPP[0] - P[3][0]*SPP[2] - P[13][0]*SPP[4]) + SF[5]*(P[4][2] + P[0][2]*SF[3] + P[1][2]*SF[1] + P[2][2]*SPP[0] - P[3][2]*SPP[2] - P[13][2]*SPP[4]) + SF[9]*(P[4][3] + P[0][3]*SF[3] + P[1][3]*SF[1] + P[2][3]*SPP[0] - P[3][3]*SPP[2] - P[13][3]*SPP[4]) + SPP[6]*(P[4][11] + P[0][11]*SF[3] + P[1][11]*SF[1] + P[2][11]*SPP[0] - P[3][11]*SPP[2] - P[13][11]*SPP[4]) - SPP[7]*(P[4][12] + P[0][12]*SF[3] + P[1][12]*SF[1] + P[2][12]*SPP[0] - P[3][12]*SPP[2] - P[13][12]*SPP[4]) - (q0*(P[4][10] + P[0][10]*SF[3] + P[1][10]*SF[1] + P[2][10]*SPP[0] - P[3][10]*SPP[2] - P[13][10]*SPP[4]))/2; + nextP[4][2] = P[4][2] + P[0][2]*SF[3] + P[1][2]*SF[1] + P[2][2]*SPP[0] - P[3][2]*SPP[2] - P[13][2]*SPP[4] + SF[4]*(P[4][0] + P[0][0]*SF[3] + P[1][0]*SF[1] + P[2][0]*SPP[0] - P[3][0]*SPP[2] - P[13][0]*SPP[4]) + SF[8]*(P[4][1] + P[0][1]*SF[3] + P[1][1]*SF[1] + P[2][1]*SPP[0] - P[3][1]*SPP[2] - P[13][1]*SPP[4]) + SF[6]*(P[4][3] + P[0][3]*SF[3] + P[1][3]*SF[1] + P[2][3]*SPP[0] - P[3][3]*SPP[2] - P[13][3]*SPP[4]) + SF[11]*(P[4][12] + P[0][12]*SF[3] + P[1][12]*SF[1] + P[2][12]*SPP[0] - P[3][12]*SPP[2] - P[13][12]*SPP[4]) - SPP[6]*(P[4][10] + P[0][10]*SF[3] + P[1][10]*SF[1] + P[2][10]*SPP[0] - P[3][10]*SPP[2] - P[13][10]*SPP[4]) - (q0*(P[4][11] + P[0][11]*SF[3] + P[1][11]*SF[1] + P[2][11]*SPP[0] - P[3][11]*SPP[2] - P[13][11]*SPP[4]))/2; + nextP[4][3] = P[4][3] + P[0][3]*SF[3] + P[1][3]*SF[1] + P[2][3]*SPP[0] - P[3][3]*SPP[2] - P[13][3]*SPP[4] + SF[5]*(P[4][0] + P[0][0]*SF[3] + P[1][0]*SF[1] + P[2][0]*SPP[0] - P[3][0]*SPP[2] - P[13][0]*SPP[4]) + SF[4]*(P[4][1] + P[0][1]*SF[3] + P[1][1]*SF[1] + P[2][1]*SPP[0] - P[3][1]*SPP[2] - P[13][1]*SPP[4]) + SF[7]*(P[4][2] + P[0][2]*SF[3] + P[1][2]*SF[1] + P[2][2]*SPP[0] - P[3][2]*SPP[2] - P[13][2]*SPP[4]) - SF[11]*(P[4][11] + P[0][11]*SF[3] + P[1][11]*SF[1] + P[2][11]*SPP[0] - P[3][11]*SPP[2] - P[13][11]*SPP[4]) + SPP[7]*(P[4][10] + P[0][10]*SF[3] + P[1][10]*SF[1] + P[2][10]*SPP[0] - P[3][10]*SPP[2] - P[13][10]*SPP[4]) - (q0*(P[4][12] + P[0][12]*SF[3] + P[1][12]*SF[1] + P[2][12]*SPP[0] - P[3][12]*SPP[2] - P[13][12]*SPP[4]))/2; + nextP[4][4] = P[4][4] + P[0][4]*SF[3] + P[1][4]*SF[1] + P[2][4]*SPP[0] - P[3][4]*SPP[2] - P[13][4]*SPP[4] + dvyCov*sq(SG[7] - 2*q0*q3) + dvzCov*sq(SG[6] + 2*q0*q2) + SF[3]*(P[4][0] + P[0][0]*SF[3] + P[1][0]*SF[1] + P[2][0]*SPP[0] - P[3][0]*SPP[2] - P[13][0]*SPP[4]) + SF[1]*(P[4][1] + P[0][1]*SF[3] + P[1][1]*SF[1] + P[2][1]*SPP[0] - P[3][1]*SPP[2] - P[13][1]*SPP[4]) + SPP[0]*(P[4][2] + P[0][2]*SF[3] + P[1][2]*SF[1] + P[2][2]*SPP[0] - P[3][2]*SPP[2] - P[13][2]*SPP[4]) - SPP[2]*(P[4][3] + P[0][3]*SF[3] + P[1][3]*SF[1] + P[2][3]*SPP[0] - P[3][3]*SPP[2] - P[13][3]*SPP[4]) - SPP[4]*(P[4][13] + P[0][13]*SF[3] + P[1][13]*SF[1] + P[2][13]*SPP[0] - P[3][13]*SPP[2] - P[13][13]*SPP[4]) + dvxCov*sq(SG[1] + SG[2] - SG[3] - SG[4]); + nextP[4][5] = P[4][5] + SQ[2] + P[0][5]*SF[3] + P[1][5]*SF[1] + P[2][5]*SPP[0] - P[3][5]*SPP[2] - P[13][5]*SPP[4] + SF[2]*(P[4][0] + P[0][0]*SF[3] + P[1][0]*SF[1] + P[2][0]*SPP[0] - P[3][0]*SPP[2] - P[13][0]*SPP[4]) + SF[1]*(P[4][2] + P[0][2]*SF[3] + P[1][2]*SF[1] + P[2][2]*SPP[0] - P[3][2]*SPP[2] - P[13][2]*SPP[4]) + SF[3]*(P[4][3] + P[0][3]*SF[3] + P[1][3]*SF[1] + P[2][3]*SPP[0] - P[3][3]*SPP[2] - P[13][3]*SPP[4]) - SPP[0]*(P[4][1] + P[0][1]*SF[3] + P[1][1]*SF[1] + P[2][1]*SPP[0] - P[3][1]*SPP[2] - P[13][1]*SPP[4]) + SPP[3]*(P[4][13] + P[0][13]*SF[3] + P[1][13]*SF[1] + P[2][13]*SPP[0] - P[3][13]*SPP[2] - P[13][13]*SPP[4]); + nextP[4][6] = P[4][6] + SQ[1] + P[0][6]*SF[3] + P[1][6]*SF[1] + P[2][6]*SPP[0] - P[3][6]*SPP[2] - P[13][6]*SPP[4] + SF[2]*(P[4][1] + P[0][1]*SF[3] + P[1][1]*SF[1] + P[2][1]*SPP[0] - P[3][1]*SPP[2] - P[13][1]*SPP[4]) + SF[1]*(P[4][3] + P[0][3]*SF[3] + P[1][3]*SF[1] + P[2][3]*SPP[0] - P[3][3]*SPP[2] - P[13][3]*SPP[4]) + SPP[0]*(P[4][0] + P[0][0]*SF[3] + P[1][0]*SF[1] + P[2][0]*SPP[0] - P[3][0]*SPP[2] - P[13][0]*SPP[4]) - SPP[1]*(P[4][2] + P[0][2]*SF[3] + P[1][2]*SF[1] + P[2][2]*SPP[0] - P[3][2]*SPP[2] - P[13][2]*SPP[4]) - (sq(q0) - sq(q1) - sq(q2) + sq(q3))*(P[4][13] + P[0][13]*SF[3] + P[1][13]*SF[1] + P[2][13]*SPP[0] - P[3][13]*SPP[2] - P[13][13]*SPP[4]); + nextP[4][7] = P[4][7] + P[0][7]*SF[3] + P[1][7]*SF[1] + P[2][7]*SPP[0] - P[3][7]*SPP[2] - P[13][7]*SPP[4] + dt*(P[4][4] + P[0][4]*SF[3] + P[1][4]*SF[1] + P[2][4]*SPP[0] - P[3][4]*SPP[2] - P[13][4]*SPP[4]); + nextP[4][8] = P[4][8] + P[0][8]*SF[3] + P[1][8]*SF[1] + P[2][8]*SPP[0] - P[3][8]*SPP[2] - P[13][8]*SPP[4] + dt*(P[4][5] + P[0][5]*SF[3] + P[1][5]*SF[1] + P[2][5]*SPP[0] - P[3][5]*SPP[2] - P[13][5]*SPP[4]); + nextP[4][9] = P[4][9] + P[0][9]*SF[3] + P[1][9]*SF[1] + P[2][9]*SPP[0] - P[3][9]*SPP[2] - P[13][9]*SPP[4] + dt*(P[4][6] + P[0][6]*SF[3] + P[1][6]*SF[1] + P[2][6]*SPP[0] - P[3][6]*SPP[2] - P[13][6]*SPP[4]); + nextP[4][10] = P[4][10] + P[0][10]*SF[3] + P[1][10]*SF[1] + P[2][10]*SPP[0] - P[3][10]*SPP[2] - P[13][10]*SPP[4]; + nextP[4][11] = P[4][11] + P[0][11]*SF[3] + P[1][11]*SF[1] + P[2][11]*SPP[0] - P[3][11]*SPP[2] - P[13][11]*SPP[4]; + nextP[4][12] = P[4][12] + P[0][12]*SF[3] + P[1][12]*SF[1] + P[2][12]*SPP[0] - P[3][12]*SPP[2] - P[13][12]*SPP[4]; + nextP[4][13] = P[4][13] + P[0][13]*SF[3] + P[1][13]*SF[1] + P[2][13]*SPP[0] - P[3][13]*SPP[2] - P[13][13]*SPP[4]; + nextP[4][14] = P[4][14] + P[0][14]*SF[3] + P[1][14]*SF[1] + P[2][14]*SPP[0] - P[3][14]*SPP[2] - P[13][14]*SPP[4]; + nextP[4][15] = P[4][15] + P[0][15]*SF[3] + P[1][15]*SF[1] + P[2][15]*SPP[0] - P[3][15]*SPP[2] - P[13][15]*SPP[4]; + nextP[4][16] = P[4][16] + P[0][16]*SF[3] + P[1][16]*SF[1] + P[2][16]*SPP[0] - P[3][16]*SPP[2] - P[13][16]*SPP[4]; + nextP[4][17] = P[4][17] + P[0][17]*SF[3] + P[1][17]*SF[1] + P[2][17]*SPP[0] - P[3][17]*SPP[2] - P[13][17]*SPP[4]; + nextP[4][18] = P[4][18] + P[0][18]*SF[3] + P[1][18]*SF[1] + P[2][18]*SPP[0] - P[3][18]*SPP[2] - P[13][18]*SPP[4]; + nextP[4][19] = P[4][19] + P[0][19]*SF[3] + P[1][19]*SF[1] + P[2][19]*SPP[0] - P[3][19]*SPP[2] - P[13][19]*SPP[4]; + nextP[4][20] = P[4][20] + P[0][20]*SF[3] + P[1][20]*SF[1] + P[2][20]*SPP[0] - P[3][20]*SPP[2] - P[13][20]*SPP[4]; + nextP[4][21] = P[4][21] + P[0][21]*SF[3] + P[1][21]*SF[1] + P[2][21]*SPP[0] - P[3][21]*SPP[2] - P[13][21]*SPP[4]; + nextP[4][22] = P[4][22] + P[0][22]*SF[3] + P[1][22]*SF[1] + P[2][22]*SPP[0] - P[3][22]*SPP[2] - P[13][22]*SPP[4]; + nextP[5][0] = P[5][0] + P[0][0]*SF[2] + P[2][0]*SF[1] + P[3][0]*SF[3] - P[1][0]*SPP[0] + P[13][0]*SPP[3] + SF[7]*(P[5][1] + P[0][1]*SF[2] + P[2][1]*SF[1] + P[3][1]*SF[3] - P[1][1]*SPP[0] + P[13][1]*SPP[3]) + SF[9]*(P[5][2] + P[0][2]*SF[2] + P[2][2]*SF[1] + P[3][2]*SF[3] - P[1][2]*SPP[0] + P[13][2]*SPP[3]) + SF[8]*(P[5][3] + P[0][3]*SF[2] + P[2][3]*SF[1] + P[3][3]*SF[3] - P[1][3]*SPP[0] + P[13][3]*SPP[3]) + SF[11]*(P[5][10] + P[0][10]*SF[2] + P[2][10]*SF[1] + P[3][10]*SF[3] - P[1][10]*SPP[0] + P[13][10]*SPP[3]) + SPP[7]*(P[5][11] + P[0][11]*SF[2] + P[2][11]*SF[1] + P[3][11]*SF[3] - P[1][11]*SPP[0] + P[13][11]*SPP[3]) + SPP[6]*(P[5][12] + P[0][12]*SF[2] + P[2][12]*SF[1] + P[3][12]*SF[3] - P[1][12]*SPP[0] + P[13][12]*SPP[3]); + nextP[5][1] = P[5][1] + P[0][1]*SF[2] + P[2][1]*SF[1] + P[3][1]*SF[3] - P[1][1]*SPP[0] + P[13][1]*SPP[3] + SF[6]*(P[5][0] + P[0][0]*SF[2] + P[2][0]*SF[1] + P[3][0]*SF[3] - P[1][0]*SPP[0] + P[13][0]*SPP[3]) + SF[5]*(P[5][2] + P[0][2]*SF[2] + P[2][2]*SF[1] + P[3][2]*SF[3] - P[1][2]*SPP[0] + P[13][2]*SPP[3]) + SF[9]*(P[5][3] + P[0][3]*SF[2] + P[2][3]*SF[1] + P[3][3]*SF[3] - P[1][3]*SPP[0] + P[13][3]*SPP[3]) + SPP[6]*(P[5][11] + P[0][11]*SF[2] + P[2][11]*SF[1] + P[3][11]*SF[3] - P[1][11]*SPP[0] + P[13][11]*SPP[3]) - SPP[7]*(P[5][12] + P[0][12]*SF[2] + P[2][12]*SF[1] + P[3][12]*SF[3] - P[1][12]*SPP[0] + P[13][12]*SPP[3]) - (q0*(P[5][10] + P[0][10]*SF[2] + P[2][10]*SF[1] + P[3][10]*SF[3] - P[1][10]*SPP[0] + P[13][10]*SPP[3]))/2; + nextP[5][2] = P[5][2] + P[0][2]*SF[2] + P[2][2]*SF[1] + P[3][2]*SF[3] - P[1][2]*SPP[0] + P[13][2]*SPP[3] + SF[4]*(P[5][0] + P[0][0]*SF[2] + P[2][0]*SF[1] + P[3][0]*SF[3] - P[1][0]*SPP[0] + P[13][0]*SPP[3]) + SF[8]*(P[5][1] + P[0][1]*SF[2] + P[2][1]*SF[1] + P[3][1]*SF[3] - P[1][1]*SPP[0] + P[13][1]*SPP[3]) + SF[6]*(P[5][3] + P[0][3]*SF[2] + P[2][3]*SF[1] + P[3][3]*SF[3] - P[1][3]*SPP[0] + P[13][3]*SPP[3]) + SF[11]*(P[5][12] + P[0][12]*SF[2] + P[2][12]*SF[1] + P[3][12]*SF[3] - P[1][12]*SPP[0] + P[13][12]*SPP[3]) - SPP[6]*(P[5][10] + P[0][10]*SF[2] + P[2][10]*SF[1] + P[3][10]*SF[3] - P[1][10]*SPP[0] + P[13][10]*SPP[3]) - (q0*(P[5][11] + P[0][11]*SF[2] + P[2][11]*SF[1] + P[3][11]*SF[3] - P[1][11]*SPP[0] + P[13][11]*SPP[3]))/2; + nextP[5][3] = P[5][3] + P[0][3]*SF[2] + P[2][3]*SF[1] + P[3][3]*SF[3] - P[1][3]*SPP[0] + P[13][3]*SPP[3] + SF[5]*(P[5][0] + P[0][0]*SF[2] + P[2][0]*SF[1] + P[3][0]*SF[3] - P[1][0]*SPP[0] + P[13][0]*SPP[3]) + SF[4]*(P[5][1] + P[0][1]*SF[2] + P[2][1]*SF[1] + P[3][1]*SF[3] - P[1][1]*SPP[0] + P[13][1]*SPP[3]) + SF[7]*(P[5][2] + P[0][2]*SF[2] + P[2][2]*SF[1] + P[3][2]*SF[3] - P[1][2]*SPP[0] + P[13][2]*SPP[3]) - SF[11]*(P[5][11] + P[0][11]*SF[2] + P[2][11]*SF[1] + P[3][11]*SF[3] - P[1][11]*SPP[0] + P[13][11]*SPP[3]) + SPP[7]*(P[5][10] + P[0][10]*SF[2] + P[2][10]*SF[1] + P[3][10]*SF[3] - P[1][10]*SPP[0] + P[13][10]*SPP[3]) - (q0*(P[5][12] + P[0][12]*SF[2] + P[2][12]*SF[1] + P[3][12]*SF[3] - P[1][12]*SPP[0] + P[13][12]*SPP[3]))/2; + nextP[5][4] = P[5][4] + SQ[2] + P[0][4]*SF[2] + P[2][4]*SF[1] + P[3][4]*SF[3] - P[1][4]*SPP[0] + P[13][4]*SPP[3] + SF[3]*(P[5][0] + P[0][0]*SF[2] + P[2][0]*SF[1] + P[3][0]*SF[3] - P[1][0]*SPP[0] + P[13][0]*SPP[3]) + SF[1]*(P[5][1] + P[0][1]*SF[2] + P[2][1]*SF[1] + P[3][1]*SF[3] - P[1][1]*SPP[0] + P[13][1]*SPP[3]) + SPP[0]*(P[5][2] + P[0][2]*SF[2] + P[2][2]*SF[1] + P[3][2]*SF[3] - P[1][2]*SPP[0] + P[13][2]*SPP[3]) - SPP[2]*(P[5][3] + P[0][3]*SF[2] + P[2][3]*SF[1] + P[3][3]*SF[3] - P[1][3]*SPP[0] + P[13][3]*SPP[3]) - SPP[4]*(P[5][13] + P[0][13]*SF[2] + P[2][13]*SF[1] + P[3][13]*SF[3] - P[1][13]*SPP[0] + P[13][13]*SPP[3]); + nextP[5][5] = P[5][5] + P[0][5]*SF[2] + P[2][5]*SF[1] + P[3][5]*SF[3] - P[1][5]*SPP[0] + P[13][5]*SPP[3] + dvxCov*sq(SG[7] + 2*q0*q3) + dvzCov*sq(SG[5] - 2*q0*q1) + SF[2]*(P[5][0] + P[0][0]*SF[2] + P[2][0]*SF[1] + P[3][0]*SF[3] - P[1][0]*SPP[0] + P[13][0]*SPP[3]) + SF[1]*(P[5][2] + P[0][2]*SF[2] + P[2][2]*SF[1] + P[3][2]*SF[3] - P[1][2]*SPP[0] + P[13][2]*SPP[3]) + SF[3]*(P[5][3] + P[0][3]*SF[2] + P[2][3]*SF[1] + P[3][3]*SF[3] - P[1][3]*SPP[0] + P[13][3]*SPP[3]) - SPP[0]*(P[5][1] + P[0][1]*SF[2] + P[2][1]*SF[1] + P[3][1]*SF[3] - P[1][1]*SPP[0] + P[13][1]*SPP[3]) + SPP[3]*(P[5][13] + P[0][13]*SF[2] + P[2][13]*SF[1] + P[3][13]*SF[3] - P[1][13]*SPP[0] + P[13][13]*SPP[3]) + dvyCov*sq(SG[1] - SG[2] + SG[3] - SG[4]); + nextP[5][6] = P[5][6] + SQ[0] + P[0][6]*SF[2] + P[2][6]*SF[1] + P[3][6]*SF[3] - P[1][6]*SPP[0] + P[13][6]*SPP[3] + SF[2]*(P[5][1] + P[0][1]*SF[2] + P[2][1]*SF[1] + P[3][1]*SF[3] - P[1][1]*SPP[0] + P[13][1]*SPP[3]) + SF[1]*(P[5][3] + P[0][3]*SF[2] + P[2][3]*SF[1] + P[3][3]*SF[3] - P[1][3]*SPP[0] + P[13][3]*SPP[3]) + SPP[0]*(P[5][0] + P[0][0]*SF[2] + P[2][0]*SF[1] + P[3][0]*SF[3] - P[1][0]*SPP[0] + P[13][0]*SPP[3]) - SPP[1]*(P[5][2] + P[0][2]*SF[2] + P[2][2]*SF[1] + P[3][2]*SF[3] - P[1][2]*SPP[0] + P[13][2]*SPP[3]) - (sq(q0) - sq(q1) - sq(q2) + sq(q3))*(P[5][13] + P[0][13]*SF[2] + P[2][13]*SF[1] + P[3][13]*SF[3] - P[1][13]*SPP[0] + P[13][13]*SPP[3]); + nextP[5][7] = P[5][7] + P[0][7]*SF[2] + P[2][7]*SF[1] + P[3][7]*SF[3] - P[1][7]*SPP[0] + P[13][7]*SPP[3] + dt*(P[5][4] + P[0][4]*SF[2] + P[2][4]*SF[1] + P[3][4]*SF[3] - P[1][4]*SPP[0] + P[13][4]*SPP[3]); + nextP[5][8] = P[5][8] + P[0][8]*SF[2] + P[2][8]*SF[1] + P[3][8]*SF[3] - P[1][8]*SPP[0] + P[13][8]*SPP[3] + dt*(P[5][5] + P[0][5]*SF[2] + P[2][5]*SF[1] + P[3][5]*SF[3] - P[1][5]*SPP[0] + P[13][5]*SPP[3]); + nextP[5][9] = P[5][9] + P[0][9]*SF[2] + P[2][9]*SF[1] + P[3][9]*SF[3] - P[1][9]*SPP[0] + P[13][9]*SPP[3] + dt*(P[5][6] + P[0][6]*SF[2] + P[2][6]*SF[1] + P[3][6]*SF[3] - P[1][6]*SPP[0] + P[13][6]*SPP[3]); + nextP[5][10] = P[5][10] + P[0][10]*SF[2] + P[2][10]*SF[1] + P[3][10]*SF[3] - P[1][10]*SPP[0] + P[13][10]*SPP[3]; + nextP[5][11] = P[5][11] + P[0][11]*SF[2] + P[2][11]*SF[1] + P[3][11]*SF[3] - P[1][11]*SPP[0] + P[13][11]*SPP[3]; + nextP[5][12] = P[5][12] + P[0][12]*SF[2] + P[2][12]*SF[1] + P[3][12]*SF[3] - P[1][12]*SPP[0] + P[13][12]*SPP[3]; + nextP[5][13] = P[5][13] + P[0][13]*SF[2] + P[2][13]*SF[1] + P[3][13]*SF[3] - P[1][13]*SPP[0] + P[13][13]*SPP[3]; + nextP[5][14] = P[5][14] + P[0][14]*SF[2] + P[2][14]*SF[1] + P[3][14]*SF[3] - P[1][14]*SPP[0] + P[13][14]*SPP[3]; + nextP[5][15] = P[5][15] + P[0][15]*SF[2] + P[2][15]*SF[1] + P[3][15]*SF[3] - P[1][15]*SPP[0] + P[13][15]*SPP[3]; + nextP[5][16] = P[5][16] + P[0][16]*SF[2] + P[2][16]*SF[1] + P[3][16]*SF[3] - P[1][16]*SPP[0] + P[13][16]*SPP[3]; + nextP[5][17] = P[5][17] + P[0][17]*SF[2] + P[2][17]*SF[1] + P[3][17]*SF[3] - P[1][17]*SPP[0] + P[13][17]*SPP[3]; + nextP[5][18] = P[5][18] + P[0][18]*SF[2] + P[2][18]*SF[1] + P[3][18]*SF[3] - P[1][18]*SPP[0] + P[13][18]*SPP[3]; + nextP[5][19] = P[5][19] + P[0][19]*SF[2] + P[2][19]*SF[1] + P[3][19]*SF[3] - P[1][19]*SPP[0] + P[13][19]*SPP[3]; + nextP[5][20] = P[5][20] + P[0][20]*SF[2] + P[2][20]*SF[1] + P[3][20]*SF[3] - P[1][20]*SPP[0] + P[13][20]*SPP[3]; + nextP[5][21] = P[5][21] + P[0][21]*SF[2] + P[2][21]*SF[1] + P[3][21]*SF[3] - P[1][21]*SPP[0] + P[13][21]*SPP[3]; + nextP[5][22] = P[5][22] + P[0][22]*SF[2] + P[2][22]*SF[1] + P[3][22]*SF[3] - P[1][22]*SPP[0] + P[13][22]*SPP[3]; + nextP[6][0] = P[6][0] + P[1][0]*SF[2] + P[3][0]*SF[1] + P[0][0]*SPP[0] - P[2][0]*SPP[1] - P[13][0]*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + SF[7]*(P[6][1] + P[1][1]*SF[2] + P[3][1]*SF[1] + P[0][1]*SPP[0] - P[2][1]*SPP[1] - P[13][1]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) + SF[9]*(P[6][2] + P[1][2]*SF[2] + P[3][2]*SF[1] + P[0][2]*SPP[0] - P[2][2]*SPP[1] - P[13][2]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) + SF[8]*(P[6][3] + P[1][3]*SF[2] + P[3][3]*SF[1] + P[0][3]*SPP[0] - P[2][3]*SPP[1] - P[13][3]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) + SF[11]*(P[6][10] + P[1][10]*SF[2] + P[3][10]*SF[1] + P[0][10]*SPP[0] - P[2][10]*SPP[1] - P[13][10]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) + SPP[7]*(P[6][11] + P[1][11]*SF[2] + P[3][11]*SF[1] + P[0][11]*SPP[0] - P[2][11]*SPP[1] - P[13][11]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) + SPP[6]*(P[6][12] + P[1][12]*SF[2] + P[3][12]*SF[1] + P[0][12]*SPP[0] - P[2][12]*SPP[1] - P[13][12]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))); + nextP[6][1] = P[6][1] + P[1][1]*SF[2] + P[3][1]*SF[1] + P[0][1]*SPP[0] - P[2][1]*SPP[1] - P[13][1]*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + SF[6]*(P[6][0] + P[1][0]*SF[2] + P[3][0]*SF[1] + P[0][0]*SPP[0] - P[2][0]*SPP[1] - P[13][0]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) + SF[5]*(P[6][2] + P[1][2]*SF[2] + P[3][2]*SF[1] + P[0][2]*SPP[0] - P[2][2]*SPP[1] - P[13][2]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) + SF[9]*(P[6][3] + P[1][3]*SF[2] + P[3][3]*SF[1] + P[0][3]*SPP[0] - P[2][3]*SPP[1] - P[13][3]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) + SPP[6]*(P[6][11] + P[1][11]*SF[2] + P[3][11]*SF[1] + P[0][11]*SPP[0] - P[2][11]*SPP[1] - P[13][11]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) - SPP[7]*(P[6][12] + P[1][12]*SF[2] + P[3][12]*SF[1] + P[0][12]*SPP[0] - P[2][12]*SPP[1] - P[13][12]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) - (q0*(P[6][10] + P[1][10]*SF[2] + P[3][10]*SF[1] + P[0][10]*SPP[0] - P[2][10]*SPP[1] - P[13][10]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))))/2; + nextP[6][2] = P[6][2] + P[1][2]*SF[2] + P[3][2]*SF[1] + P[0][2]*SPP[0] - P[2][2]*SPP[1] - P[13][2]*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + SF[4]*(P[6][0] + P[1][0]*SF[2] + P[3][0]*SF[1] + P[0][0]*SPP[0] - P[2][0]*SPP[1] - P[13][0]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) + SF[8]*(P[6][1] + P[1][1]*SF[2] + P[3][1]*SF[1] + P[0][1]*SPP[0] - P[2][1]*SPP[1] - P[13][1]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) + SF[6]*(P[6][3] + P[1][3]*SF[2] + P[3][3]*SF[1] + P[0][3]*SPP[0] - P[2][3]*SPP[1] - P[13][3]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) + SF[11]*(P[6][12] + P[1][12]*SF[2] + P[3][12]*SF[1] + P[0][12]*SPP[0] - P[2][12]*SPP[1] - P[13][12]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) - SPP[6]*(P[6][10] + P[1][10]*SF[2] + P[3][10]*SF[1] + P[0][10]*SPP[0] - P[2][10]*SPP[1] - P[13][10]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) - (q0*(P[6][11] + P[1][11]*SF[2] + P[3][11]*SF[1] + P[0][11]*SPP[0] - P[2][11]*SPP[1] - P[13][11]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))))/2; + nextP[6][3] = P[6][3] + P[1][3]*SF[2] + P[3][3]*SF[1] + P[0][3]*SPP[0] - P[2][3]*SPP[1] - P[13][3]*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + SF[5]*(P[6][0] + P[1][0]*SF[2] + P[3][0]*SF[1] + P[0][0]*SPP[0] - P[2][0]*SPP[1] - P[13][0]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) + SF[4]*(P[6][1] + P[1][1]*SF[2] + P[3][1]*SF[1] + P[0][1]*SPP[0] - P[2][1]*SPP[1] - P[13][1]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) + SF[7]*(P[6][2] + P[1][2]*SF[2] + P[3][2]*SF[1] + P[0][2]*SPP[0] - P[2][2]*SPP[1] - P[13][2]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) - SF[11]*(P[6][11] + P[1][11]*SF[2] + P[3][11]*SF[1] + P[0][11]*SPP[0] - P[2][11]*SPP[1] - P[13][11]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) + SPP[7]*(P[6][10] + P[1][10]*SF[2] + P[3][10]*SF[1] + P[0][10]*SPP[0] - P[2][10]*SPP[1] - P[13][10]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) - (q0*(P[6][12] + P[1][12]*SF[2] + P[3][12]*SF[1] + P[0][12]*SPP[0] - P[2][12]*SPP[1] - P[13][12]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))))/2; + nextP[6][4] = P[6][4] + SQ[1] + P[1][4]*SF[2] + P[3][4]*SF[1] + P[0][4]*SPP[0] - P[2][4]*SPP[1] - P[13][4]*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + SF[3]*(P[6][0] + P[1][0]*SF[2] + P[3][0]*SF[1] + P[0][0]*SPP[0] - P[2][0]*SPP[1] - P[13][0]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) + SF[1]*(P[6][1] + P[1][1]*SF[2] + P[3][1]*SF[1] + P[0][1]*SPP[0] - P[2][1]*SPP[1] - P[13][1]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) + SPP[0]*(P[6][2] + P[1][2]*SF[2] + P[3][2]*SF[1] + P[0][2]*SPP[0] - P[2][2]*SPP[1] - P[13][2]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) - SPP[2]*(P[6][3] + P[1][3]*SF[2] + P[3][3]*SF[1] + P[0][3]*SPP[0] - P[2][3]*SPP[1] - P[13][3]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) - SPP[4]*(P[6][13] + P[1][13]*SF[2] + P[3][13]*SF[1] + P[0][13]*SPP[0] - P[2][13]*SPP[1] - P[13][13]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))); + nextP[6][5] = P[6][5] + SQ[0] + P[1][5]*SF[2] + P[3][5]*SF[1] + P[0][5]*SPP[0] - P[2][5]*SPP[1] - P[13][5]*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + SF[2]*(P[6][0] + P[1][0]*SF[2] + P[3][0]*SF[1] + P[0][0]*SPP[0] - P[2][0]*SPP[1] - P[13][0]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) + SF[1]*(P[6][2] + P[1][2]*SF[2] + P[3][2]*SF[1] + P[0][2]*SPP[0] - P[2][2]*SPP[1] - P[13][2]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) + SF[3]*(P[6][3] + P[1][3]*SF[2] + P[3][3]*SF[1] + P[0][3]*SPP[0] - P[2][3]*SPP[1] - P[13][3]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) - SPP[0]*(P[6][1] + P[1][1]*SF[2] + P[3][1]*SF[1] + P[0][1]*SPP[0] - P[2][1]*SPP[1] - P[13][1]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) + SPP[3]*(P[6][13] + P[1][13]*SF[2] + P[3][13]*SF[1] + P[0][13]*SPP[0] - P[2][13]*SPP[1] - P[13][13]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))); + nextP[6][6] = P[6][6] + P[1][6]*SF[2] + P[3][6]*SF[1] + P[0][6]*SPP[0] - P[2][6]*SPP[1] - P[13][6]*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + dvxCov*sq(SG[6] - 2*q0*q2) + dvyCov*sq(SG[5] + 2*q0*q1) - SPP[5]*(P[6][13] + P[1][13]*SF[2] + P[3][13]*SF[1] + P[0][13]*SPP[0] - P[2][13]*SPP[1] - P[13][13]*SPP[5]) + SF[2]*(P[6][1] + P[1][1]*SF[2] + P[3][1]*SF[1] + P[0][1]*SPP[0] - P[2][1]*SPP[1] - P[13][1]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) + SF[1]*(P[6][3] + P[1][3]*SF[2] + P[3][3]*SF[1] + P[0][3]*SPP[0] - P[2][3]*SPP[1] - P[13][3]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) + SPP[0]*(P[6][0] + P[1][0]*SF[2] + P[3][0]*SF[1] + P[0][0]*SPP[0] - P[2][0]*SPP[1] - P[13][0]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) - SPP[1]*(P[6][2] + P[1][2]*SF[2] + P[3][2]*SF[1] + P[0][2]*SPP[0] - P[2][2]*SPP[1] - P[13][2]*(sq(q0) - sq(q1) - sq(q2) + sq(q3))) + dvzCov*sq(SG[1] - SG[2] - SG[3] + SG[4]); + nextP[6][7] = P[6][7] + P[1][7]*SF[2] + P[3][7]*SF[1] + P[0][7]*SPP[0] - P[2][7]*SPP[1] - P[13][7]*SPP[5] + dt*(P[6][4] + P[1][4]*SF[2] + P[3][4]*SF[1] + P[0][4]*SPP[0] - P[2][4]*SPP[1] - P[13][4]*SPP[5]); + nextP[6][8] = P[6][8] + P[1][8]*SF[2] + P[3][8]*SF[1] + P[0][8]*SPP[0] - P[2][8]*SPP[1] - P[13][8]*SPP[5] + dt*(P[6][5] + P[1][5]*SF[2] + P[3][5]*SF[1] + P[0][5]*SPP[0] - P[2][5]*SPP[1] - P[13][5]*SPP[5]); + nextP[6][9] = P[6][9] + P[1][9]*SF[2] + P[3][9]*SF[1] + P[0][9]*SPP[0] - P[2][9]*SPP[1] - P[13][9]*SPP[5] + dt*(P[6][6] + P[1][6]*SF[2] + P[3][6]*SF[1] + P[0][6]*SPP[0] - P[2][6]*SPP[1] - P[13][6]*SPP[5]); + nextP[6][10] = P[6][10] + P[1][10]*SF[2] + P[3][10]*SF[1] + P[0][10]*SPP[0] - P[2][10]*SPP[1] - P[13][10]*SPP[5]; + nextP[6][11] = P[6][11] + P[1][11]*SF[2] + P[3][11]*SF[1] + P[0][11]*SPP[0] - P[2][11]*SPP[1] - P[13][11]*SPP[5]; + nextP[6][12] = P[6][12] + P[1][12]*SF[2] + P[3][12]*SF[1] + P[0][12]*SPP[0] - P[2][12]*SPP[1] - P[13][12]*SPP[5]; + nextP[6][13] = P[6][13] + P[1][13]*SF[2] + P[3][13]*SF[1] + P[0][13]*SPP[0] - P[2][13]*SPP[1] - P[13][13]*SPP[5]; + nextP[6][14] = P[6][14] + P[1][14]*SF[2] + P[3][14]*SF[1] + P[0][14]*SPP[0] - P[2][14]*SPP[1] - P[13][14]*SPP[5]; + nextP[6][15] = P[6][15] + P[1][15]*SF[2] + P[3][15]*SF[1] + P[0][15]*SPP[0] - P[2][15]*SPP[1] - P[13][15]*SPP[5]; + nextP[6][16] = P[6][16] + P[1][16]*SF[2] + P[3][16]*SF[1] + P[0][16]*SPP[0] - P[2][16]*SPP[1] - P[13][16]*SPP[5]; + nextP[6][17] = P[6][17] + P[1][17]*SF[2] + P[3][17]*SF[1] + P[0][17]*SPP[0] - P[2][17]*SPP[1] - P[13][17]*SPP[5]; + nextP[6][18] = P[6][18] + P[1][18]*SF[2] + P[3][18]*SF[1] + P[0][18]*SPP[0] - P[2][18]*SPP[1] - P[13][18]*SPP[5]; + nextP[6][19] = P[6][19] + P[1][19]*SF[2] + P[3][19]*SF[1] + P[0][19]*SPP[0] - P[2][19]*SPP[1] - P[13][19]*SPP[5]; + nextP[6][20] = P[6][20] + P[1][20]*SF[2] + P[3][20]*SF[1] + P[0][20]*SPP[0] - P[2][20]*SPP[1] - P[13][20]*SPP[5]; + nextP[6][21] = P[6][21] + P[1][21]*SF[2] + P[3][21]*SF[1] + P[0][21]*SPP[0] - P[2][21]*SPP[1] - P[13][21]*SPP[5]; + nextP[6][22] = P[6][22] + P[1][22]*SF[2] + P[3][22]*SF[1] + P[0][22]*SPP[0] - P[2][22]*SPP[1] - P[13][22]*SPP[5]; + nextP[7][0] = P[7][0] + P[4][0]*dt + SF[7]*(P[7][1] + P[4][1]*dt) + SF[9]*(P[7][2] + P[4][2]*dt) + SF[8]*(P[7][3] + P[4][3]*dt) + SF[11]*(P[7][10] + P[4][10]*dt) + SPP[7]*(P[7][11] + P[4][11]*dt) + SPP[6]*(P[7][12] + P[4][12]*dt); + nextP[7][1] = P[7][1] + P[4][1]*dt + SF[6]*(P[7][0] + P[4][0]*dt) + SF[5]*(P[7][2] + P[4][2]*dt) + SF[9]*(P[7][3] + P[4][3]*dt) + SPP[6]*(P[7][11] + P[4][11]*dt) - SPP[7]*(P[7][12] + P[4][12]*dt) - (q0*(P[7][10] + P[4][10]*dt))/2; + nextP[7][2] = P[7][2] + P[4][2]*dt + SF[4]*(P[7][0] + P[4][0]*dt) + SF[8]*(P[7][1] + P[4][1]*dt) + SF[6]*(P[7][3] + P[4][3]*dt) + SF[11]*(P[7][12] + P[4][12]*dt) - SPP[6]*(P[7][10] + P[4][10]*dt) - (q0*(P[7][11] + P[4][11]*dt))/2; + nextP[7][3] = P[7][3] + P[4][3]*dt + SF[5]*(P[7][0] + P[4][0]*dt) + SF[4]*(P[7][1] + P[4][1]*dt) + SF[7]*(P[7][2] + P[4][2]*dt) - SF[11]*(P[7][11] + P[4][11]*dt) + SPP[7]*(P[7][10] + P[4][10]*dt) - (q0*(P[7][12] + P[4][12]*dt))/2; + nextP[7][4] = P[7][4] + P[4][4]*dt + SF[1]*(P[7][1] + P[4][1]*dt) + SF[3]*(P[7][0] + P[4][0]*dt) + SPP[0]*(P[7][2] + P[4][2]*dt) - SPP[2]*(P[7][3] + P[4][3]*dt) - SPP[4]*(P[7][13] + P[4][13]*dt); + nextP[7][5] = P[7][5] + P[4][5]*dt + SF[2]*(P[7][0] + P[4][0]*dt) + SF[1]*(P[7][2] + P[4][2]*dt) + SF[3]*(P[7][3] + P[4][3]*dt) - SPP[0]*(P[7][1] + P[4][1]*dt) + SPP[3]*(P[7][13] + P[4][13]*dt); + nextP[7][6] = P[7][6] + P[4][6]*dt + SF[2]*(P[7][1] + P[4][1]*dt) + SF[1]*(P[7][3] + P[4][3]*dt) + SPP[0]*(P[7][0] + P[4][0]*dt) - SPP[1]*(P[7][2] + P[4][2]*dt) - SPP[5]*(P[7][13] + P[4][13]*dt); nextP[7][7] = P[7][7] + P[4][7]*dt + dt*(P[7][4] + P[4][4]*dt); nextP[7][8] = P[7][8] + P[4][8]*dt + dt*(P[7][5] + P[4][5]*dt); nextP[7][9] = P[7][9] + P[4][9]*dt + dt*(P[7][6] + P[4][6]*dt); @@ -543,13 +574,15 @@ void AttPosEKF::CovariancePrediction(float dt) nextP[7][18] = P[7][18] + P[4][18]*dt; nextP[7][19] = P[7][19] + P[4][19]*dt; nextP[7][20] = P[7][20] + P[4][20]*dt; - nextP[8][0] = P[8][0] + P[5][0]*dt + SF[6]*(P[8][1] + P[5][1]*dt) + SPP[7]*(P[8][2] + P[5][2]*dt) + SPP[6]*(P[8][3] + P[5][3]*dt) + SPP[5]*(P[8][10] + P[5][10]*dt) + SPP[4]*(P[8][11] + P[5][11]*dt) + SPP[3]*(P[8][12] + P[5][12]*dt); - nextP[8][1] = P[8][1] + P[5][1]*dt + SF[5]*(P[8][0] + P[5][0]*dt) + SF[4]*(P[8][2] + P[5][2]*dt) + SPP[7]*(P[8][3] + P[5][3]*dt) + SPP[3]*(P[8][11] + P[5][11]*dt) - SPP[4]*(P[8][12] + P[5][12]*dt) - (q0*(P[8][10] + P[5][10]*dt))/2; - nextP[8][2] = P[8][2] + P[5][2]*dt + SF[3]*(P[8][0] + P[5][0]*dt) + SF[5]*(P[8][3] + P[5][3]*dt) + SPP[6]*(P[8][1] + P[5][1]*dt) - SPP[3]*(P[8][10] + P[5][10]*dt) + SPP[5]*(P[8][12] + P[5][12]*dt) - (q0*(P[8][11] + P[5][11]*dt))/2; - nextP[8][3] = P[8][3] + P[5][3]*dt + SF[4]*(P[8][0] + P[5][0]*dt) + SF[3]*(P[8][1] + P[5][1]*dt) + SF[6]*(P[8][2] + P[5][2]*dt) + SPP[4]*(P[8][10] + P[5][10]*dt) - SPP[5]*(P[8][11] + P[5][11]*dt) - (q0*(P[8][12] + P[5][12]*dt))/2; - nextP[8][4] = P[8][4] + P[5][4]*dt + SF[0]*(P[8][1] + P[5][1]*dt) + SF[2]*(P[8][0] + P[5][0]*dt) + SPP[0]*(P[8][2] + P[5][2]*dt) - SPP[2]*(P[8][3] + P[5][3]*dt); - nextP[8][5] = P[8][5] + P[5][5]*dt + SF[1]*(P[8][0] + P[5][0]*dt) + SF[0]*(P[8][2] + P[5][2]*dt) + SF[2]*(P[8][3] + P[5][3]*dt) - SPP[0]*(P[8][1] + P[5][1]*dt); - nextP[8][6] = P[8][6] + P[5][6]*dt + SF[1]*(P[8][1] + P[5][1]*dt) + SF[0]*(P[8][3] + P[5][3]*dt) + SPP[0]*(P[8][0] + P[5][0]*dt) - SPP[1]*(P[8][2] + P[5][2]*dt); + nextP[7][21] = P[7][21] + P[4][21]*dt; + nextP[7][22] = P[7][22] + P[4][22]*dt; + nextP[8][0] = P[8][0] + P[5][0]*dt + SF[7]*(P[8][1] + P[5][1]*dt) + SF[9]*(P[8][2] + P[5][2]*dt) + SF[8]*(P[8][3] + P[5][3]*dt) + SF[11]*(P[8][10] + P[5][10]*dt) + SPP[7]*(P[8][11] + P[5][11]*dt) + SPP[6]*(P[8][12] + P[5][12]*dt); + nextP[8][1] = P[8][1] + P[5][1]*dt + SF[6]*(P[8][0] + P[5][0]*dt) + SF[5]*(P[8][2] + P[5][2]*dt) + SF[9]*(P[8][3] + P[5][3]*dt) + SPP[6]*(P[8][11] + P[5][11]*dt) - SPP[7]*(P[8][12] + P[5][12]*dt) - (q0*(P[8][10] + P[5][10]*dt))/2; + nextP[8][2] = P[8][2] + P[5][2]*dt + SF[4]*(P[8][0] + P[5][0]*dt) + SF[8]*(P[8][1] + P[5][1]*dt) + SF[6]*(P[8][3] + P[5][3]*dt) + SF[11]*(P[8][12] + P[5][12]*dt) - SPP[6]*(P[8][10] + P[5][10]*dt) - (q0*(P[8][11] + P[5][11]*dt))/2; + nextP[8][3] = P[8][3] + P[5][3]*dt + SF[5]*(P[8][0] + P[5][0]*dt) + SF[4]*(P[8][1] + P[5][1]*dt) + SF[7]*(P[8][2] + P[5][2]*dt) - SF[11]*(P[8][11] + P[5][11]*dt) + SPP[7]*(P[8][10] + P[5][10]*dt) - (q0*(P[8][12] + P[5][12]*dt))/2; + nextP[8][4] = P[8][4] + P[5][4]*dt + SF[1]*(P[8][1] + P[5][1]*dt) + SF[3]*(P[8][0] + P[5][0]*dt) + SPP[0]*(P[8][2] + P[5][2]*dt) - SPP[2]*(P[8][3] + P[5][3]*dt) - SPP[4]*(P[8][13] + P[5][13]*dt); + nextP[8][5] = P[8][5] + P[5][5]*dt + SF[2]*(P[8][0] + P[5][0]*dt) + SF[1]*(P[8][2] + P[5][2]*dt) + SF[3]*(P[8][3] + P[5][3]*dt) - SPP[0]*(P[8][1] + P[5][1]*dt) + SPP[3]*(P[8][13] + P[5][13]*dt); + nextP[8][6] = P[8][6] + P[5][6]*dt + SF[2]*(P[8][1] + P[5][1]*dt) + SF[1]*(P[8][3] + P[5][3]*dt) + SPP[0]*(P[8][0] + P[5][0]*dt) - SPP[1]*(P[8][2] + P[5][2]*dt) - SPP[5]*(P[8][13] + P[5][13]*dt); nextP[8][7] = P[8][7] + P[5][7]*dt + dt*(P[8][4] + P[5][4]*dt); nextP[8][8] = P[8][8] + P[5][8]*dt + dt*(P[8][5] + P[5][5]*dt); nextP[8][9] = P[8][9] + P[5][9]*dt + dt*(P[8][6] + P[5][6]*dt); @@ -564,13 +597,15 @@ void AttPosEKF::CovariancePrediction(float dt) nextP[8][18] = P[8][18] + P[5][18]*dt; nextP[8][19] = P[8][19] + P[5][19]*dt; nextP[8][20] = P[8][20] + P[5][20]*dt; - nextP[9][0] = P[9][0] + P[6][0]*dt + SF[6]*(P[9][1] + P[6][1]*dt) + SPP[7]*(P[9][2] + P[6][2]*dt) + SPP[6]*(P[9][3] + P[6][3]*dt) + SPP[5]*(P[9][10] + P[6][10]*dt) + SPP[4]*(P[9][11] + P[6][11]*dt) + SPP[3]*(P[9][12] + P[6][12]*dt); - nextP[9][1] = P[9][1] + P[6][1]*dt + SF[5]*(P[9][0] + P[6][0]*dt) + SF[4]*(P[9][2] + P[6][2]*dt) + SPP[7]*(P[9][3] + P[6][3]*dt) + SPP[3]*(P[9][11] + P[6][11]*dt) - SPP[4]*(P[9][12] + P[6][12]*dt) - (q0*(P[9][10] + P[6][10]*dt))/2; - nextP[9][2] = P[9][2] + P[6][2]*dt + SF[3]*(P[9][0] + P[6][0]*dt) + SF[5]*(P[9][3] + P[6][3]*dt) + SPP[6]*(P[9][1] + P[6][1]*dt) - SPP[3]*(P[9][10] + P[6][10]*dt) + SPP[5]*(P[9][12] + P[6][12]*dt) - (q0*(P[9][11] + P[6][11]*dt))/2; - nextP[9][3] = P[9][3] + P[6][3]*dt + SF[4]*(P[9][0] + P[6][0]*dt) + SF[3]*(P[9][1] + P[6][1]*dt) + SF[6]*(P[9][2] + P[6][2]*dt) + SPP[4]*(P[9][10] + P[6][10]*dt) - SPP[5]*(P[9][11] + P[6][11]*dt) - (q0*(P[9][12] + P[6][12]*dt))/2; - nextP[9][4] = P[9][4] + P[6][4]*dt + SF[0]*(P[9][1] + P[6][1]*dt) + SF[2]*(P[9][0] + P[6][0]*dt) + SPP[0]*(P[9][2] + P[6][2]*dt) - SPP[2]*(P[9][3] + P[6][3]*dt); - nextP[9][5] = P[9][5] + P[6][5]*dt + SF[1]*(P[9][0] + P[6][0]*dt) + SF[0]*(P[9][2] + P[6][2]*dt) + SF[2]*(P[9][3] + P[6][3]*dt) - SPP[0]*(P[9][1] + P[6][1]*dt); - nextP[9][6] = P[9][6] + P[6][6]*dt + SF[1]*(P[9][1] + P[6][1]*dt) + SF[0]*(P[9][3] + P[6][3]*dt) + SPP[0]*(P[9][0] + P[6][0]*dt) - SPP[1]*(P[9][2] + P[6][2]*dt); + nextP[8][21] = P[8][21] + P[5][21]*dt; + nextP[8][22] = P[8][22] + P[5][22]*dt; + nextP[9][0] = P[9][0] + P[6][0]*dt + SF[7]*(P[9][1] + P[6][1]*dt) + SF[9]*(P[9][2] + P[6][2]*dt) + SF[8]*(P[9][3] + P[6][3]*dt) + SF[11]*(P[9][10] + P[6][10]*dt) + SPP[7]*(P[9][11] + P[6][11]*dt) + SPP[6]*(P[9][12] + P[6][12]*dt); + nextP[9][1] = P[9][1] + P[6][1]*dt + SF[6]*(P[9][0] + P[6][0]*dt) + SF[5]*(P[9][2] + P[6][2]*dt) + SF[9]*(P[9][3] + P[6][3]*dt) + SPP[6]*(P[9][11] + P[6][11]*dt) - SPP[7]*(P[9][12] + P[6][12]*dt) - (q0*(P[9][10] + P[6][10]*dt))/2; + nextP[9][2] = P[9][2] + P[6][2]*dt + SF[4]*(P[9][0] + P[6][0]*dt) + SF[8]*(P[9][1] + P[6][1]*dt) + SF[6]*(P[9][3] + P[6][3]*dt) + SF[11]*(P[9][12] + P[6][12]*dt) - SPP[6]*(P[9][10] + P[6][10]*dt) - (q0*(P[9][11] + P[6][11]*dt))/2; + nextP[9][3] = P[9][3] + P[6][3]*dt + SF[5]*(P[9][0] + P[6][0]*dt) + SF[4]*(P[9][1] + P[6][1]*dt) + SF[7]*(P[9][2] + P[6][2]*dt) - SF[11]*(P[9][11] + P[6][11]*dt) + SPP[7]*(P[9][10] + P[6][10]*dt) - (q0*(P[9][12] + P[6][12]*dt))/2; + nextP[9][4] = P[9][4] + P[6][4]*dt + SF[1]*(P[9][1] + P[6][1]*dt) + SF[3]*(P[9][0] + P[6][0]*dt) + SPP[0]*(P[9][2] + P[6][2]*dt) - SPP[2]*(P[9][3] + P[6][3]*dt) - SPP[4]*(P[9][13] + P[6][13]*dt); + nextP[9][5] = P[9][5] + P[6][5]*dt + SF[2]*(P[9][0] + P[6][0]*dt) + SF[1]*(P[9][2] + P[6][2]*dt) + SF[3]*(P[9][3] + P[6][3]*dt) - SPP[0]*(P[9][1] + P[6][1]*dt) + SPP[3]*(P[9][13] + P[6][13]*dt); + nextP[9][6] = P[9][6] + P[6][6]*dt + SF[2]*(P[9][1] + P[6][1]*dt) + SF[1]*(P[9][3] + P[6][3]*dt) + SPP[0]*(P[9][0] + P[6][0]*dt) - SPP[1]*(P[9][2] + P[6][2]*dt) - SPP[5]*(P[9][13] + P[6][13]*dt); nextP[9][7] = P[9][7] + P[6][7]*dt + dt*(P[9][4] + P[6][4]*dt); nextP[9][8] = P[9][8] + P[6][8]*dt + dt*(P[9][5] + P[6][5]*dt); nextP[9][9] = P[9][9] + P[6][9]*dt + dt*(P[9][6] + P[6][6]*dt); @@ -585,13 +620,15 @@ void AttPosEKF::CovariancePrediction(float dt) nextP[9][18] = P[9][18] + P[6][18]*dt; nextP[9][19] = P[9][19] + P[6][19]*dt; nextP[9][20] = P[9][20] + P[6][20]*dt; - nextP[10][0] = P[10][0] + P[10][1]*SF[6] + P[10][2]*SPP[7] + P[10][3]*SPP[6] + P[10][10]*SPP[5] + P[10][11]*SPP[4] + P[10][12]*SPP[3]; - nextP[10][1] = P[10][1] + P[10][0]*SF[5] + P[10][2]*SF[4] + P[10][3]*SPP[7] + P[10][11]*SPP[3] - P[10][12]*SPP[4] - (P[10][10]*q0)/2; - nextP[10][2] = P[10][2] + P[10][0]*SF[3] + P[10][3]*SF[5] + P[10][1]*SPP[6] - P[10][10]*SPP[3] + P[10][12]*SPP[5] - (P[10][11]*q0)/2; - nextP[10][3] = P[10][3] + P[10][0]*SF[4] + P[10][1]*SF[3] + P[10][2]*SF[6] + P[10][10]*SPP[4] - P[10][11]*SPP[5] - (P[10][12]*q0)/2; - nextP[10][4] = P[10][4] + P[10][1]*SF[0] + P[10][0]*SF[2] + P[10][2]*SPP[0] - P[10][3]*SPP[2]; - nextP[10][5] = P[10][5] + P[10][0]*SF[1] + P[10][2]*SF[0] + P[10][3]*SF[2] - P[10][1]*SPP[0]; - nextP[10][6] = P[10][6] + P[10][1]*SF[1] + P[10][3]*SF[0] + P[10][0]*SPP[0] - P[10][2]*SPP[1]; + nextP[9][21] = P[9][21] + P[6][21]*dt; + nextP[9][22] = P[9][22] + P[6][22]*dt; + nextP[10][0] = P[10][0] + P[10][1]*SF[7] + P[10][2]*SF[9] + P[10][3]*SF[8] + P[10][10]*SF[11] + P[10][11]*SPP[7] + P[10][12]*SPP[6]; + nextP[10][1] = P[10][1] + P[10][0]*SF[6] + P[10][2]*SF[5] + P[10][3]*SF[9] + P[10][11]*SPP[6] - P[10][12]*SPP[7] - (P[10][10]*q0)/2; + nextP[10][2] = P[10][2] + P[10][0]*SF[4] + P[10][1]*SF[8] + P[10][3]*SF[6] + P[10][12]*SF[11] - P[10][10]*SPP[6] - (P[10][11]*q0)/2; + nextP[10][3] = P[10][3] + P[10][0]*SF[5] + P[10][1]*SF[4] + P[10][2]*SF[7] - P[10][11]*SF[11] + P[10][10]*SPP[7] - (P[10][12]*q0)/2; + nextP[10][4] = P[10][4] + P[10][1]*SF[1] + P[10][0]*SF[3] + P[10][2]*SPP[0] - P[10][3]*SPP[2] - P[10][13]*SPP[4]; + nextP[10][5] = P[10][5] + P[10][0]*SF[2] + P[10][2]*SF[1] + P[10][3]*SF[3] - P[10][1]*SPP[0] + P[10][13]*SPP[3]; + nextP[10][6] = P[10][6] + P[10][1]*SF[2] + P[10][3]*SF[1] + P[10][0]*SPP[0] - P[10][2]*SPP[1] - P[10][13]*SPP[5]; nextP[10][7] = P[10][7] + P[10][4]*dt; nextP[10][8] = P[10][8] + P[10][5]*dt; nextP[10][9] = P[10][9] + P[10][6]*dt; @@ -606,13 +643,15 @@ void AttPosEKF::CovariancePrediction(float dt) nextP[10][18] = P[10][18]; nextP[10][19] = P[10][19]; nextP[10][20] = P[10][20]; - nextP[11][0] = P[11][0] + P[11][1]*SF[6] + P[11][2]*SPP[7] + P[11][3]*SPP[6] + P[11][10]*SPP[5] + P[11][11]*SPP[4] + P[11][12]*SPP[3]; - nextP[11][1] = P[11][1] + P[11][0]*SF[5] + P[11][2]*SF[4] + P[11][3]*SPP[7] + P[11][11]*SPP[3] - P[11][12]*SPP[4] - (P[11][10]*q0)/2; - nextP[11][2] = P[11][2] + P[11][0]*SF[3] + P[11][3]*SF[5] + P[11][1]*SPP[6] - P[11][10]*SPP[3] + P[11][12]*SPP[5] - (P[11][11]*q0)/2; - nextP[11][3] = P[11][3] + P[11][0]*SF[4] + P[11][1]*SF[3] + P[11][2]*SF[6] + P[11][10]*SPP[4] - P[11][11]*SPP[5] - (P[11][12]*q0)/2; - nextP[11][4] = P[11][4] + P[11][1]*SF[0] + P[11][0]*SF[2] + P[11][2]*SPP[0] - P[11][3]*SPP[2]; - nextP[11][5] = P[11][5] + P[11][0]*SF[1] + P[11][2]*SF[0] + P[11][3]*SF[2] - P[11][1]*SPP[0]; - nextP[11][6] = P[11][6] + P[11][1]*SF[1] + P[11][3]*SF[0] + P[11][0]*SPP[0] - P[11][2]*SPP[1]; + nextP[10][21] = P[10][21]; + nextP[10][22] = P[10][22]; + nextP[11][0] = P[11][0] + P[11][1]*SF[7] + P[11][2]*SF[9] + P[11][3]*SF[8] + P[11][10]*SF[11] + P[11][11]*SPP[7] + P[11][12]*SPP[6]; + nextP[11][1] = P[11][1] + P[11][0]*SF[6] + P[11][2]*SF[5] + P[11][3]*SF[9] + P[11][11]*SPP[6] - P[11][12]*SPP[7] - (P[11][10]*q0)/2; + nextP[11][2] = P[11][2] + P[11][0]*SF[4] + P[11][1]*SF[8] + P[11][3]*SF[6] + P[11][12]*SF[11] - P[11][10]*SPP[6] - (P[11][11]*q0)/2; + nextP[11][3] = P[11][3] + P[11][0]*SF[5] + P[11][1]*SF[4] + P[11][2]*SF[7] - P[11][11]*SF[11] + P[11][10]*SPP[7] - (P[11][12]*q0)/2; + nextP[11][4] = P[11][4] + P[11][1]*SF[1] + P[11][0]*SF[3] + P[11][2]*SPP[0] - P[11][3]*SPP[2] - P[11][13]*SPP[4]; + nextP[11][5] = P[11][5] + P[11][0]*SF[2] + P[11][2]*SF[1] + P[11][3]*SF[3] - P[11][1]*SPP[0] + P[11][13]*SPP[3]; + nextP[11][6] = P[11][6] + P[11][1]*SF[2] + P[11][3]*SF[1] + P[11][0]*SPP[0] - P[11][2]*SPP[1] - P[11][13]*SPP[5]; nextP[11][7] = P[11][7] + P[11][4]*dt; nextP[11][8] = P[11][8] + P[11][5]*dt; nextP[11][9] = P[11][9] + P[11][6]*dt; @@ -627,13 +666,15 @@ void AttPosEKF::CovariancePrediction(float dt) nextP[11][18] = P[11][18]; nextP[11][19] = P[11][19]; nextP[11][20] = P[11][20]; - nextP[12][0] = P[12][0] + P[12][1]*SF[6] + P[12][2]*SPP[7] + P[12][3]*SPP[6] + P[12][10]*SPP[5] + P[12][11]*SPP[4] + P[12][12]*SPP[3]; - nextP[12][1] = P[12][1] + P[12][0]*SF[5] + P[12][2]*SF[4] + P[12][3]*SPP[7] + P[12][11]*SPP[3] - P[12][12]*SPP[4] - (P[12][10]*q0)/2; - nextP[12][2] = P[12][2] + P[12][0]*SF[3] + P[12][3]*SF[5] + P[12][1]*SPP[6] - P[12][10]*SPP[3] + P[12][12]*SPP[5] - (P[12][11]*q0)/2; - nextP[12][3] = P[12][3] + P[12][0]*SF[4] + P[12][1]*SF[3] + P[12][2]*SF[6] + P[12][10]*SPP[4] - P[12][11]*SPP[5] - (P[12][12]*q0)/2; - nextP[12][4] = P[12][4] + P[12][1]*SF[0] + P[12][0]*SF[2] + P[12][2]*SPP[0] - P[12][3]*SPP[2]; - nextP[12][5] = P[12][5] + P[12][0]*SF[1] + P[12][2]*SF[0] + P[12][3]*SF[2] - P[12][1]*SPP[0]; - nextP[12][6] = P[12][6] + P[12][1]*SF[1] + P[12][3]*SF[0] + P[12][0]*SPP[0] - P[12][2]*SPP[1]; + nextP[11][21] = P[11][21]; + nextP[11][22] = P[11][22]; + nextP[12][0] = P[12][0] + P[12][1]*SF[7] + P[12][2]*SF[9] + P[12][3]*SF[8] + P[12][10]*SF[11] + P[12][11]*SPP[7] + P[12][12]*SPP[6]; + nextP[12][1] = P[12][1] + P[12][0]*SF[6] + P[12][2]*SF[5] + P[12][3]*SF[9] + P[12][11]*SPP[6] - P[12][12]*SPP[7] - (P[12][10]*q0)/2; + nextP[12][2] = P[12][2] + P[12][0]*SF[4] + P[12][1]*SF[8] + P[12][3]*SF[6] + P[12][12]*SF[11] - P[12][10]*SPP[6] - (P[12][11]*q0)/2; + nextP[12][3] = P[12][3] + P[12][0]*SF[5] + P[12][1]*SF[4] + P[12][2]*SF[7] - P[12][11]*SF[11] + P[12][10]*SPP[7] - (P[12][12]*q0)/2; + nextP[12][4] = P[12][4] + P[12][1]*SF[1] + P[12][0]*SF[3] + P[12][2]*SPP[0] - P[12][3]*SPP[2] - P[12][13]*SPP[4]; + nextP[12][5] = P[12][5] + P[12][0]*SF[2] + P[12][2]*SF[1] + P[12][3]*SF[3] - P[12][1]*SPP[0] + P[12][13]*SPP[3]; + nextP[12][6] = P[12][6] + P[12][1]*SF[2] + P[12][3]*SF[1] + P[12][0]*SPP[0] - P[12][2]*SPP[1] - P[12][13]*SPP[5]; nextP[12][7] = P[12][7] + P[12][4]*dt; nextP[12][8] = P[12][8] + P[12][5]*dt; nextP[12][9] = P[12][9] + P[12][6]*dt; @@ -648,13 +689,15 @@ void AttPosEKF::CovariancePrediction(float dt) nextP[12][18] = P[12][18]; nextP[12][19] = P[12][19]; nextP[12][20] = P[12][20]; - nextP[13][0] = P[13][0] + P[13][1]*SF[6] + P[13][2]*SPP[7] + P[13][3]*SPP[6] + P[13][10]*SPP[5] + P[13][11]*SPP[4] + P[13][12]*SPP[3]; - nextP[13][1] = P[13][1] + P[13][0]*SF[5] + P[13][2]*SF[4] + P[13][3]*SPP[7] + P[13][11]*SPP[3] - P[13][12]*SPP[4] - (P[13][10]*q0)/2; - nextP[13][2] = P[13][2] + P[13][0]*SF[3] + P[13][3]*SF[5] + P[13][1]*SPP[6] - P[13][10]*SPP[3] + P[13][12]*SPP[5] - (P[13][11]*q0)/2; - nextP[13][3] = P[13][3] + P[13][0]*SF[4] + P[13][1]*SF[3] + P[13][2]*SF[6] + P[13][10]*SPP[4] - P[13][11]*SPP[5] - (P[13][12]*q0)/2; - nextP[13][4] = P[13][4] + P[13][1]*SF[0] + P[13][0]*SF[2] + P[13][2]*SPP[0] - P[13][3]*SPP[2]; - nextP[13][5] = P[13][5] + P[13][0]*SF[1] + P[13][2]*SF[0] + P[13][3]*SF[2] - P[13][1]*SPP[0]; - nextP[13][6] = P[13][6] + P[13][1]*SF[1] + P[13][3]*SF[0] + P[13][0]*SPP[0] - P[13][2]*SPP[1]; + nextP[12][21] = P[12][21]; + nextP[12][22] = P[12][22]; + nextP[13][0] = P[13][0] + P[13][1]*SF[7] + P[13][2]*SF[9] + P[13][3]*SF[8] + P[13][10]*SF[11] + P[13][11]*SPP[7] + P[13][12]*SPP[6]; + nextP[13][1] = P[13][1] + P[13][0]*SF[6] + P[13][2]*SF[5] + P[13][3]*SF[9] + P[13][11]*SPP[6] - P[13][12]*SPP[7] - (P[13][10]*q0)/2; + nextP[13][2] = P[13][2] + P[13][0]*SF[4] + P[13][1]*SF[8] + P[13][3]*SF[6] + P[13][12]*SF[11] - P[13][10]*SPP[6] - (P[13][11]*q0)/2; + nextP[13][3] = P[13][3] + P[13][0]*SF[5] + P[13][1]*SF[4] + P[13][2]*SF[7] - P[13][11]*SF[11] + P[13][10]*SPP[7] - (P[13][12]*q0)/2; + nextP[13][4] = P[13][4] + P[13][1]*SF[1] + P[13][0]*SF[3] + P[13][2]*SPP[0] - P[13][3]*SPP[2] - P[13][13]*SPP[4]; + nextP[13][5] = P[13][5] + P[13][0]*SF[2] + P[13][2]*SF[1] + P[13][3]*SF[3] - P[13][1]*SPP[0] + P[13][13]*SPP[3]; + nextP[13][6] = P[13][6] + P[13][1]*SF[2] + P[13][3]*SF[1] + P[13][0]*SPP[0] - P[13][2]*SPP[1] - P[13][13]*SPP[5]; nextP[13][7] = P[13][7] + P[13][4]*dt; nextP[13][8] = P[13][8] + P[13][5]*dt; nextP[13][9] = P[13][9] + P[13][6]*dt; @@ -669,13 +712,15 @@ void AttPosEKF::CovariancePrediction(float dt) nextP[13][18] = P[13][18]; nextP[13][19] = P[13][19]; nextP[13][20] = P[13][20]; - nextP[14][0] = P[14][0] + P[14][1]*SF[6] + P[14][2]*SPP[7] + P[14][3]*SPP[6] + P[14][10]*SPP[5] + P[14][11]*SPP[4] + P[14][12]*SPP[3]; - nextP[14][1] = P[14][1] + P[14][0]*SF[5] + P[14][2]*SF[4] + P[14][3]*SPP[7] + P[14][11]*SPP[3] - P[14][12]*SPP[4] - (P[14][10]*q0)/2; - nextP[14][2] = P[14][2] + P[14][0]*SF[3] + P[14][3]*SF[5] + P[14][1]*SPP[6] - P[14][10]*SPP[3] + P[14][12]*SPP[5] - (P[14][11]*q0)/2; - nextP[14][3] = P[14][3] + P[14][0]*SF[4] + P[14][1]*SF[3] + P[14][2]*SF[6] + P[14][10]*SPP[4] - P[14][11]*SPP[5] - (P[14][12]*q0)/2; - nextP[14][4] = P[14][4] + P[14][1]*SF[0] + P[14][0]*SF[2] + P[14][2]*SPP[0] - P[14][3]*SPP[2]; - nextP[14][5] = P[14][5] + P[14][0]*SF[1] + P[14][2]*SF[0] + P[14][3]*SF[2] - P[14][1]*SPP[0]; - nextP[14][6] = P[14][6] + P[14][1]*SF[1] + P[14][3]*SF[0] + P[14][0]*SPP[0] - P[14][2]*SPP[1]; + nextP[13][21] = P[13][21]; + nextP[13][22] = P[13][22]; + nextP[14][0] = P[14][0] + P[14][1]*SF[7] + P[14][2]*SF[9] + P[14][3]*SF[8] + P[14][10]*SF[11] + P[14][11]*SPP[7] + P[14][12]*SPP[6]; + nextP[14][1] = P[14][1] + P[14][0]*SF[6] + P[14][2]*SF[5] + P[14][3]*SF[9] + P[14][11]*SPP[6] - P[14][12]*SPP[7] - (P[14][10]*q0)/2; + nextP[14][2] = P[14][2] + P[14][0]*SF[4] + P[14][1]*SF[8] + P[14][3]*SF[6] + P[14][12]*SF[11] - P[14][10]*SPP[6] - (P[14][11]*q0)/2; + nextP[14][3] = P[14][3] + P[14][0]*SF[5] + P[14][1]*SF[4] + P[14][2]*SF[7] - P[14][11]*SF[11] + P[14][10]*SPP[7] - (P[14][12]*q0)/2; + nextP[14][4] = P[14][4] + P[14][1]*SF[1] + P[14][0]*SF[3] + P[14][2]*SPP[0] - P[14][3]*SPP[2] - P[14][13]*SPP[4]; + nextP[14][5] = P[14][5] + P[14][0]*SF[2] + P[14][2]*SF[1] + P[14][3]*SF[3] - P[14][1]*SPP[0] + P[14][13]*SPP[3]; + nextP[14][6] = P[14][6] + P[14][1]*SF[2] + P[14][3]*SF[1] + P[14][0]*SPP[0] - P[14][2]*SPP[1] - P[14][13]*SPP[5]; nextP[14][7] = P[14][7] + P[14][4]*dt; nextP[14][8] = P[14][8] + P[14][5]*dt; nextP[14][9] = P[14][9] + P[14][6]*dt; @@ -690,13 +735,15 @@ void AttPosEKF::CovariancePrediction(float dt) nextP[14][18] = P[14][18]; nextP[14][19] = P[14][19]; nextP[14][20] = P[14][20]; - nextP[15][0] = P[15][0] + P[15][1]*SF[6] + P[15][2]*SPP[7] + P[15][3]*SPP[6] + P[15][10]*SPP[5] + P[15][11]*SPP[4] + P[15][12]*SPP[3]; - nextP[15][1] = P[15][1] + P[15][0]*SF[5] + P[15][2]*SF[4] + P[15][3]*SPP[7] + P[15][11]*SPP[3] - P[15][12]*SPP[4] - (P[15][10]*q0)/2; - nextP[15][2] = P[15][2] + P[15][0]*SF[3] + P[15][3]*SF[5] + P[15][1]*SPP[6] - P[15][10]*SPP[3] + P[15][12]*SPP[5] - (P[15][11]*q0)/2; - nextP[15][3] = P[15][3] + P[15][0]*SF[4] + P[15][1]*SF[3] + P[15][2]*SF[6] + P[15][10]*SPP[4] - P[15][11]*SPP[5] - (P[15][12]*q0)/2; - nextP[15][4] = P[15][4] + P[15][1]*SF[0] + P[15][0]*SF[2] + P[15][2]*SPP[0] - P[15][3]*SPP[2]; - nextP[15][5] = P[15][5] + P[15][0]*SF[1] + P[15][2]*SF[0] + P[15][3]*SF[2] - P[15][1]*SPP[0]; - nextP[15][6] = P[15][6] + P[15][1]*SF[1] + P[15][3]*SF[0] + P[15][0]*SPP[0] - P[15][2]*SPP[1]; + nextP[14][21] = P[14][21]; + nextP[14][22] = P[14][22]; + nextP[15][0] = P[15][0] + P[15][1]*SF[7] + P[15][2]*SF[9] + P[15][3]*SF[8] + P[15][10]*SF[11] + P[15][11]*SPP[7] + P[15][12]*SPP[6]; + nextP[15][1] = P[15][1] + P[15][0]*SF[6] + P[15][2]*SF[5] + P[15][3]*SF[9] + P[15][11]*SPP[6] - P[15][12]*SPP[7] - (P[15][10]*q0)/2; + nextP[15][2] = P[15][2] + P[15][0]*SF[4] + P[15][1]*SF[8] + P[15][3]*SF[6] + P[15][12]*SF[11] - P[15][10]*SPP[6] - (P[15][11]*q0)/2; + nextP[15][3] = P[15][3] + P[15][0]*SF[5] + P[15][1]*SF[4] + P[15][2]*SF[7] - P[15][11]*SF[11] + P[15][10]*SPP[7] - (P[15][12]*q0)/2; + nextP[15][4] = P[15][4] + P[15][1]*SF[1] + P[15][0]*SF[3] + P[15][2]*SPP[0] - P[15][3]*SPP[2] - P[15][13]*SPP[4]; + nextP[15][5] = P[15][5] + P[15][0]*SF[2] + P[15][2]*SF[1] + P[15][3]*SF[3] - P[15][1]*SPP[0] + P[15][13]*SPP[3]; + nextP[15][6] = P[15][6] + P[15][1]*SF[2] + P[15][3]*SF[1] + P[15][0]*SPP[0] - P[15][2]*SPP[1] - P[15][13]*SPP[5]; nextP[15][7] = P[15][7] + P[15][4]*dt; nextP[15][8] = P[15][8] + P[15][5]*dt; nextP[15][9] = P[15][9] + P[15][6]*dt; @@ -711,13 +758,15 @@ void AttPosEKF::CovariancePrediction(float dt) nextP[15][18] = P[15][18]; nextP[15][19] = P[15][19]; nextP[15][20] = P[15][20]; - nextP[16][0] = P[16][0] + P[16][1]*SF[6] + P[16][2]*SPP[7] + P[16][3]*SPP[6] + P[16][10]*SPP[5] + P[16][11]*SPP[4] + P[16][12]*SPP[3]; - nextP[16][1] = P[16][1] + P[16][0]*SF[5] + P[16][2]*SF[4] + P[16][3]*SPP[7] + P[16][11]*SPP[3] - P[16][12]*SPP[4] - (P[16][10]*q0)/2; - nextP[16][2] = P[16][2] + P[16][0]*SF[3] + P[16][3]*SF[5] + P[16][1]*SPP[6] - P[16][10]*SPP[3] + P[16][12]*SPP[5] - (P[16][11]*q0)/2; - nextP[16][3] = P[16][3] + P[16][0]*SF[4] + P[16][1]*SF[3] + P[16][2]*SF[6] + P[16][10]*SPP[4] - P[16][11]*SPP[5] - (P[16][12]*q0)/2; - nextP[16][4] = P[16][4] + P[16][1]*SF[0] + P[16][0]*SF[2] + P[16][2]*SPP[0] - P[16][3]*SPP[2]; - nextP[16][5] = P[16][5] + P[16][0]*SF[1] + P[16][2]*SF[0] + P[16][3]*SF[2] - P[16][1]*SPP[0]; - nextP[16][6] = P[16][6] + P[16][1]*SF[1] + P[16][3]*SF[0] + P[16][0]*SPP[0] - P[16][2]*SPP[1]; + nextP[15][21] = P[15][21]; + nextP[15][22] = P[15][22]; + nextP[16][0] = P[16][0] + P[16][1]*SF[7] + P[16][2]*SF[9] + P[16][3]*SF[8] + P[16][10]*SF[11] + P[16][11]*SPP[7] + P[16][12]*SPP[6]; + nextP[16][1] = P[16][1] + P[16][0]*SF[6] + P[16][2]*SF[5] + P[16][3]*SF[9] + P[16][11]*SPP[6] - P[16][12]*SPP[7] - (P[16][10]*q0)/2; + nextP[16][2] = P[16][2] + P[16][0]*SF[4] + P[16][1]*SF[8] + P[16][3]*SF[6] + P[16][12]*SF[11] - P[16][10]*SPP[6] - (P[16][11]*q0)/2; + nextP[16][3] = P[16][3] + P[16][0]*SF[5] + P[16][1]*SF[4] + P[16][2]*SF[7] - P[16][11]*SF[11] + P[16][10]*SPP[7] - (P[16][12]*q0)/2; + nextP[16][4] = P[16][4] + P[16][1]*SF[1] + P[16][0]*SF[3] + P[16][2]*SPP[0] - P[16][3]*SPP[2] - P[16][13]*SPP[4]; + nextP[16][5] = P[16][5] + P[16][0]*SF[2] + P[16][2]*SF[1] + P[16][3]*SF[3] - P[16][1]*SPP[0] + P[16][13]*SPP[3]; + nextP[16][6] = P[16][6] + P[16][1]*SF[2] + P[16][3]*SF[1] + P[16][0]*SPP[0] - P[16][2]*SPP[1] - P[16][13]*SPP[5]; nextP[16][7] = P[16][7] + P[16][4]*dt; nextP[16][8] = P[16][8] + P[16][5]*dt; nextP[16][9] = P[16][9] + P[16][6]*dt; @@ -732,13 +781,15 @@ void AttPosEKF::CovariancePrediction(float dt) nextP[16][18] = P[16][18]; nextP[16][19] = P[16][19]; nextP[16][20] = P[16][20]; - nextP[17][0] = P[17][0] + P[17][1]*SF[6] + P[17][2]*SPP[7] + P[17][3]*SPP[6] + P[17][10]*SPP[5] + P[17][11]*SPP[4] + P[17][12]*SPP[3]; - nextP[17][1] = P[17][1] + P[17][0]*SF[5] + P[17][2]*SF[4] + P[17][3]*SPP[7] + P[17][11]*SPP[3] - P[17][12]*SPP[4] - (P[17][10]*q0)/2; - nextP[17][2] = P[17][2] + P[17][0]*SF[3] + P[17][3]*SF[5] + P[17][1]*SPP[6] - P[17][10]*SPP[3] + P[17][12]*SPP[5] - (P[17][11]*q0)/2; - nextP[17][3] = P[17][3] + P[17][0]*SF[4] + P[17][1]*SF[3] + P[17][2]*SF[6] + P[17][10]*SPP[4] - P[17][11]*SPP[5] - (P[17][12]*q0)/2; - nextP[17][4] = P[17][4] + P[17][1]*SF[0] + P[17][0]*SF[2] + P[17][2]*SPP[0] - P[17][3]*SPP[2]; - nextP[17][5] = P[17][5] + P[17][0]*SF[1] + P[17][2]*SF[0] + P[17][3]*SF[2] - P[17][1]*SPP[0]; - nextP[17][6] = P[17][6] + P[17][1]*SF[1] + P[17][3]*SF[0] + P[17][0]*SPP[0] - P[17][2]*SPP[1]; + nextP[16][21] = P[16][21]; + nextP[16][22] = P[16][22]; + nextP[17][0] = P[17][0] + P[17][1]*SF[7] + P[17][2]*SF[9] + P[17][3]*SF[8] + P[17][10]*SF[11] + P[17][11]*SPP[7] + P[17][12]*SPP[6]; + nextP[17][1] = P[17][1] + P[17][0]*SF[6] + P[17][2]*SF[5] + P[17][3]*SF[9] + P[17][11]*SPP[6] - P[17][12]*SPP[7] - (P[17][10]*q0)/2; + nextP[17][2] = P[17][2] + P[17][0]*SF[4] + P[17][1]*SF[8] + P[17][3]*SF[6] + P[17][12]*SF[11] - P[17][10]*SPP[6] - (P[17][11]*q0)/2; + nextP[17][3] = P[17][3] + P[17][0]*SF[5] + P[17][1]*SF[4] + P[17][2]*SF[7] - P[17][11]*SF[11] + P[17][10]*SPP[7] - (P[17][12]*q0)/2; + nextP[17][4] = P[17][4] + P[17][1]*SF[1] + P[17][0]*SF[3] + P[17][2]*SPP[0] - P[17][3]*SPP[2] - P[17][13]*SPP[4]; + nextP[17][5] = P[17][5] + P[17][0]*SF[2] + P[17][2]*SF[1] + P[17][3]*SF[3] - P[17][1]*SPP[0] + P[17][13]*SPP[3]; + nextP[17][6] = P[17][6] + P[17][1]*SF[2] + P[17][3]*SF[1] + P[17][0]*SPP[0] - P[17][2]*SPP[1] - P[17][13]*SPP[5]; nextP[17][7] = P[17][7] + P[17][4]*dt; nextP[17][8] = P[17][8] + P[17][5]*dt; nextP[17][9] = P[17][9] + P[17][6]*dt; @@ -753,13 +804,15 @@ void AttPosEKF::CovariancePrediction(float dt) nextP[17][18] = P[17][18]; nextP[17][19] = P[17][19]; nextP[17][20] = P[17][20]; - nextP[18][0] = P[18][0] + P[18][1]*SF[6] + P[18][2]*SPP[7] + P[18][3]*SPP[6] + P[18][10]*SPP[5] + P[18][11]*SPP[4] + P[18][12]*SPP[3]; - nextP[18][1] = P[18][1] + P[18][0]*SF[5] + P[18][2]*SF[4] + P[18][3]*SPP[7] + P[18][11]*SPP[3] - P[18][12]*SPP[4] - (P[18][10]*q0)/2; - nextP[18][2] = P[18][2] + P[18][0]*SF[3] + P[18][3]*SF[5] + P[18][1]*SPP[6] - P[18][10]*SPP[3] + P[18][12]*SPP[5] - (P[18][11]*q0)/2; - nextP[18][3] = P[18][3] + P[18][0]*SF[4] + P[18][1]*SF[3] + P[18][2]*SF[6] + P[18][10]*SPP[4] - P[18][11]*SPP[5] - (P[18][12]*q0)/2; - nextP[18][4] = P[18][4] + P[18][1]*SF[0] + P[18][0]*SF[2] + P[18][2]*SPP[0] - P[18][3]*SPP[2]; - nextP[18][5] = P[18][5] + P[18][0]*SF[1] + P[18][2]*SF[0] + P[18][3]*SF[2] - P[18][1]*SPP[0]; - nextP[18][6] = P[18][6] + P[18][1]*SF[1] + P[18][3]*SF[0] + P[18][0]*SPP[0] - P[18][2]*SPP[1]; + nextP[17][21] = P[17][21]; + nextP[17][22] = P[17][22]; + nextP[18][0] = P[18][0] + P[18][1]*SF[7] + P[18][2]*SF[9] + P[18][3]*SF[8] + P[18][10]*SF[11] + P[18][11]*SPP[7] + P[18][12]*SPP[6]; + nextP[18][1] = P[18][1] + P[18][0]*SF[6] + P[18][2]*SF[5] + P[18][3]*SF[9] + P[18][11]*SPP[6] - P[18][12]*SPP[7] - (P[18][10]*q0)/2; + nextP[18][2] = P[18][2] + P[18][0]*SF[4] + P[18][1]*SF[8] + P[18][3]*SF[6] + P[18][12]*SF[11] - P[18][10]*SPP[6] - (P[18][11]*q0)/2; + nextP[18][3] = P[18][3] + P[18][0]*SF[5] + P[18][1]*SF[4] + P[18][2]*SF[7] - P[18][11]*SF[11] + P[18][10]*SPP[7] - (P[18][12]*q0)/2; + nextP[18][4] = P[18][4] + P[18][1]*SF[1] + P[18][0]*SF[3] + P[18][2]*SPP[0] - P[18][3]*SPP[2] - P[18][13]*SPP[4]; + nextP[18][5] = P[18][5] + P[18][0]*SF[2] + P[18][2]*SF[1] + P[18][3]*SF[3] - P[18][1]*SPP[0] + P[18][13]*SPP[3]; + nextP[18][6] = P[18][6] + P[18][1]*SF[2] + P[18][3]*SF[1] + P[18][0]*SPP[0] - P[18][2]*SPP[1] - P[18][13]*SPP[5]; nextP[18][7] = P[18][7] + P[18][4]*dt; nextP[18][8] = P[18][8] + P[18][5]*dt; nextP[18][9] = P[18][9] + P[18][6]*dt; @@ -774,13 +827,15 @@ void AttPosEKF::CovariancePrediction(float dt) nextP[18][18] = P[18][18]; nextP[18][19] = P[18][19]; nextP[18][20] = P[18][20]; - nextP[19][0] = P[19][0] + P[19][1]*SF[6] + P[19][2]*SPP[7] + P[19][3]*SPP[6] + P[19][10]*SPP[5] + P[19][11]*SPP[4] + P[19][12]*SPP[3]; - nextP[19][1] = P[19][1] + P[19][0]*SF[5] + P[19][2]*SF[4] + P[19][3]*SPP[7] + P[19][11]*SPP[3] - P[19][12]*SPP[4] - (P[19][10]*q0)/2; - nextP[19][2] = P[19][2] + P[19][0]*SF[3] + P[19][3]*SF[5] + P[19][1]*SPP[6] - P[19][10]*SPP[3] + P[19][12]*SPP[5] - (P[19][11]*q0)/2; - nextP[19][3] = P[19][3] + P[19][0]*SF[4] + P[19][1]*SF[3] + P[19][2]*SF[6] + P[19][10]*SPP[4] - P[19][11]*SPP[5] - (P[19][12]*q0)/2; - nextP[19][4] = P[19][4] + P[19][1]*SF[0] + P[19][0]*SF[2] + P[19][2]*SPP[0] - P[19][3]*SPP[2]; - nextP[19][5] = P[19][5] + P[19][0]*SF[1] + P[19][2]*SF[0] + P[19][3]*SF[2] - P[19][1]*SPP[0]; - nextP[19][6] = P[19][6] + P[19][1]*SF[1] + P[19][3]*SF[0] + P[19][0]*SPP[0] - P[19][2]*SPP[1]; + nextP[18][21] = P[18][21]; + nextP[18][22] = P[18][22]; + nextP[19][0] = P[19][0] + P[19][1]*SF[7] + P[19][2]*SF[9] + P[19][3]*SF[8] + P[19][10]*SF[11] + P[19][11]*SPP[7] + P[19][12]*SPP[6]; + nextP[19][1] = P[19][1] + P[19][0]*SF[6] + P[19][2]*SF[5] + P[19][3]*SF[9] + P[19][11]*SPP[6] - P[19][12]*SPP[7] - (P[19][10]*q0)/2; + nextP[19][2] = P[19][2] + P[19][0]*SF[4] + P[19][1]*SF[8] + P[19][3]*SF[6] + P[19][12]*SF[11] - P[19][10]*SPP[6] - (P[19][11]*q0)/2; + nextP[19][3] = P[19][3] + P[19][0]*SF[5] + P[19][1]*SF[4] + P[19][2]*SF[7] - P[19][11]*SF[11] + P[19][10]*SPP[7] - (P[19][12]*q0)/2; + nextP[19][4] = P[19][4] + P[19][1]*SF[1] + P[19][0]*SF[3] + P[19][2]*SPP[0] - P[19][3]*SPP[2] - P[19][13]*SPP[4]; + nextP[19][5] = P[19][5] + P[19][0]*SF[2] + P[19][2]*SF[1] + P[19][3]*SF[3] - P[19][1]*SPP[0] + P[19][13]*SPP[3]; + nextP[19][6] = P[19][6] + P[19][1]*SF[2] + P[19][3]*SF[1] + P[19][0]*SPP[0] - P[19][2]*SPP[1] - P[19][13]*SPP[5]; nextP[19][7] = P[19][7] + P[19][4]*dt; nextP[19][8] = P[19][8] + P[19][5]*dt; nextP[19][9] = P[19][9] + P[19][6]*dt; @@ -795,13 +850,15 @@ void AttPosEKF::CovariancePrediction(float dt) nextP[19][18] = P[19][18]; nextP[19][19] = P[19][19]; nextP[19][20] = P[19][20]; - nextP[20][0] = P[20][0] + P[20][1]*SF[6] + P[20][2]*SPP[7] + P[20][3]*SPP[6] + P[20][10]*SPP[5] + P[20][11]*SPP[4] + P[20][12]*SPP[3]; - nextP[20][1] = P[20][1] + P[20][0]*SF[5] + P[20][2]*SF[4] + P[20][3]*SPP[7] + P[20][11]*SPP[3] - P[20][12]*SPP[4] - (P[20][10]*q0)/2; - nextP[20][2] = P[20][2] + P[20][0]*SF[3] + P[20][3]*SF[5] + P[20][1]*SPP[6] - P[20][10]*SPP[3] + P[20][12]*SPP[5] - (P[20][11]*q0)/2; - nextP[20][3] = P[20][3] + P[20][0]*SF[4] + P[20][1]*SF[3] + P[20][2]*SF[6] + P[20][10]*SPP[4] - P[20][11]*SPP[5] - (P[20][12]*q0)/2; - nextP[20][4] = P[20][4] + P[20][1]*SF[0] + P[20][0]*SF[2] + P[20][2]*SPP[0] - P[20][3]*SPP[2]; - nextP[20][5] = P[20][5] + P[20][0]*SF[1] + P[20][2]*SF[0] + P[20][3]*SF[2] - P[20][1]*SPP[0]; - nextP[20][6] = P[20][6] + P[20][1]*SF[1] + P[20][3]*SF[0] + P[20][0]*SPP[0] - P[20][2]*SPP[1]; + nextP[19][21] = P[19][21]; + nextP[19][22] = P[19][22]; + nextP[20][0] = P[20][0] + P[20][1]*SF[7] + P[20][2]*SF[9] + P[20][3]*SF[8] + P[20][10]*SF[11] + P[20][11]*SPP[7] + P[20][12]*SPP[6]; + nextP[20][1] = P[20][1] + P[20][0]*SF[6] + P[20][2]*SF[5] + P[20][3]*SF[9] + P[20][11]*SPP[6] - P[20][12]*SPP[7] - (P[20][10]*q0)/2; + nextP[20][2] = P[20][2] + P[20][0]*SF[4] + P[20][1]*SF[8] + P[20][3]*SF[6] + P[20][12]*SF[11] - P[20][10]*SPP[6] - (P[20][11]*q0)/2; + nextP[20][3] = P[20][3] + P[20][0]*SF[5] + P[20][1]*SF[4] + P[20][2]*SF[7] - P[20][11]*SF[11] + P[20][10]*SPP[7] - (P[20][12]*q0)/2; + nextP[20][4] = P[20][4] + P[20][1]*SF[1] + P[20][0]*SF[3] + P[20][2]*SPP[0] - P[20][3]*SPP[2] - P[20][13]*SPP[4]; + nextP[20][5] = P[20][5] + P[20][0]*SF[2] + P[20][2]*SF[1] + P[20][3]*SF[3] - P[20][1]*SPP[0] + P[20][13]*SPP[3]; + nextP[20][6] = P[20][6] + P[20][1]*SF[2] + P[20][3]*SF[1] + P[20][0]*SPP[0] - P[20][2]*SPP[1] - P[20][13]*SPP[5]; nextP[20][7] = P[20][7] + P[20][4]*dt; nextP[20][8] = P[20][8] + P[20][5]*dt; nextP[20][9] = P[20][9] + P[20][6]*dt; @@ -816,6 +873,54 @@ void AttPosEKF::CovariancePrediction(float dt) nextP[20][18] = P[20][18]; nextP[20][19] = P[20][19]; nextP[20][20] = P[20][20]; + nextP[20][21] = P[20][21]; + nextP[20][22] = P[20][22]; + nextP[21][0] = P[21][0] + P[21][1]*SF[7] + P[21][2]*SF[9] + P[21][3]*SF[8] + P[21][10]*SF[11] + P[21][11]*SPP[7] + P[21][12]*SPP[6]; + nextP[21][1] = P[21][1] + P[21][0]*SF[6] + P[21][2]*SF[5] + P[21][3]*SF[9] + P[21][11]*SPP[6] - P[21][12]*SPP[7] - (P[21][10]*q0)/2; + nextP[21][2] = P[21][2] + P[21][0]*SF[4] + P[21][1]*SF[8] + P[21][3]*SF[6] + P[21][12]*SF[11] - P[21][10]*SPP[6] - (P[21][11]*q0)/2; + nextP[21][3] = P[21][3] + P[21][0]*SF[5] + P[21][1]*SF[4] + P[21][2]*SF[7] - P[21][11]*SF[11] + P[21][10]*SPP[7] - (P[21][12]*q0)/2; + nextP[21][4] = P[21][4] + P[21][1]*SF[1] + P[21][0]*SF[3] + P[21][2]*SPP[0] - P[21][3]*SPP[2] - P[21][13]*SPP[4]; + nextP[21][5] = P[21][5] + P[21][0]*SF[2] + P[21][2]*SF[1] + P[21][3]*SF[3] - P[21][1]*SPP[0] + P[21][13]*SPP[3]; + nextP[21][6] = P[21][6] + P[21][1]*SF[2] + P[21][3]*SF[1] + P[21][0]*SPP[0] - P[21][2]*SPP[1] - P[21][13]*SPP[5]; + nextP[21][7] = P[21][7] + P[21][4]*dt; + nextP[21][8] = P[21][8] + P[21][5]*dt; + nextP[21][9] = P[21][9] + P[21][6]*dt; + nextP[21][10] = P[21][10]; + nextP[21][11] = P[21][11]; + nextP[21][12] = P[21][12]; + nextP[21][13] = P[21][13]; + nextP[21][14] = P[21][14]; + nextP[21][15] = P[21][15]; + nextP[21][16] = P[21][16]; + nextP[21][17] = P[21][17]; + nextP[21][18] = P[21][18]; + nextP[21][19] = P[21][19]; + nextP[21][20] = P[21][20]; + nextP[21][21] = P[21][21]; + nextP[21][22] = P[21][22]; + nextP[22][0] = P[22][0] + P[22][1]*SF[7] + P[22][2]*SF[9] + P[22][3]*SF[8] + P[22][10]*SF[11] + P[22][11]*SPP[7] + P[22][12]*SPP[6]; + nextP[22][1] = P[22][1] + P[22][0]*SF[6] + P[22][2]*SF[5] + P[22][3]*SF[9] + P[22][11]*SPP[6] - P[22][12]*SPP[7] - (P[22][10]*q0)/2; + nextP[22][2] = P[22][2] + P[22][0]*SF[4] + P[22][1]*SF[8] + P[22][3]*SF[6] + P[22][12]*SF[11] - P[22][10]*SPP[6] - (P[22][11]*q0)/2; + nextP[22][3] = P[22][3] + P[22][0]*SF[5] + P[22][1]*SF[4] + P[22][2]*SF[7] - P[22][11]*SF[11] + P[22][10]*SPP[7] - (P[22][12]*q0)/2; + nextP[22][4] = P[22][4] + P[22][1]*SF[1] + P[22][0]*SF[3] + P[22][2]*SPP[0] - P[22][3]*SPP[2] - P[22][13]*SPP[4]; + nextP[22][5] = P[22][5] + P[22][0]*SF[2] + P[22][2]*SF[1] + P[22][3]*SF[3] - P[22][1]*SPP[0] + P[22][13]*SPP[3]; + nextP[22][6] = P[22][6] + P[22][1]*SF[2] + P[22][3]*SF[1] + P[22][0]*SPP[0] - P[22][2]*SPP[1] - P[22][13]*SPP[5]; + nextP[22][7] = P[22][7] + P[22][4]*dt; + nextP[22][8] = P[22][8] + P[22][5]*dt; + nextP[22][9] = P[22][9] + P[22][6]*dt; + nextP[22][10] = P[22][10]; + nextP[22][11] = P[22][11]; + nextP[22][12] = P[22][12]; + nextP[22][13] = P[22][13]; + nextP[22][14] = P[22][14]; + nextP[22][15] = P[22][15]; + nextP[22][16] = P[22][16]; + nextP[22][17] = P[22][17]; + nextP[22][18] = P[22][18]; + nextP[22][19] = P[22][19]; + nextP[22][20] = P[22][20]; + nextP[22][21] = P[22][21]; + nextP[22][22] = P[22][22]; for (unsigned i = 0; i < n_states; i++) { @@ -826,16 +931,24 @@ void AttPosEKF::CovariancePrediction(float dt) // setting the coresponding covariance terms to zero. if (onGround || !useCompass) { - zeroRows(nextP,15,20); - zeroCols(nextP,15,20); + zeroRows(nextP,16,21); + zeroCols(nextP,16,21); } // If on ground or not using airspeed sensing, inhibit wind velocity // covariance growth. if (onGround || !useAirspeed) { - zeroRows(nextP,13,14); - zeroCols(nextP,13,14); + zeroRows(nextP,14,15); + zeroCols(nextP,14,15); + } + + // If on ground, inhibit terrain offset updates by + // setting the coresponding covariance terms to zero. + if (onGround) + { + zeroRows(nextP,22,22); + zeroCols(nextP,22,22); } // If the total position variance exceds 1E6 (1000m), then stop covariance @@ -857,7 +970,7 @@ void AttPosEKF::CovariancePrediction(float dt) if (onGround || staticMode) { // copy the portion of the variances we want to // propagate - for (unsigned i = 0; i < 14; i++) { + for (unsigned i = 0; i <= 13; i++) { P[i][i] = nextP[i][i]; // force symmetry for observable states @@ -866,7 +979,7 @@ void AttPosEKF::CovariancePrediction(float dt) { for (uint8_t j = 0; j < i; j++) { - if ((i > 12) || (j > 12)) { + if ((i > 13) || (j > 13)) { P[i][j] = 0.0f; } else { P[i][j] = 0.5f * (nextP[i][j] + nextP[j][i]); @@ -946,12 +1059,12 @@ void AttPosEKF::FuseVelposNED() // Estimate the GPS Velocity, GPS horiz position and height measurement variances. velErr = 0.2f*accNavMag; // additional error in GPS velocities caused by manoeuvring posErr = 0.2f*accNavMag; // additional error in GPS position caused by manoeuvring - R_OBS[0] = 0.04f + sq(velErr); + R_OBS[0] = sq(vneSigma) + sq(velErr); R_OBS[1] = R_OBS[0]; - R_OBS[2] = 0.08f + sq(velErr); - R_OBS[3] = R_OBS[2]; - R_OBS[4] = 4.0f + sq(posErr); - R_OBS[5] = 4.0f; + R_OBS[2] = sq(vdSigma) + sq(velErr); + R_OBS[3] = sq(posNeSigma) + sq(posErr); + R_OBS[4] = R_OBS[3]; + R_OBS[5] = sq(posDSigma) + sq(posErr); // Set innovation variances to zero default for (uint8_t i = 0; i<=5; i++) @@ -1046,11 +1159,11 @@ void AttPosEKF::FuseVelposNED() // Limit range of states modified when on ground if(!onGround) { - indexLimit = 20; + indexLimit = 22; } else { - indexLimit = 12; + indexLimit = 13; } // Fuse measurements sequentially for (obsIndex=0; obsIndex<=5; obsIndex++) @@ -1080,6 +1193,12 @@ void AttPosEKF::FuseVelposNED() { Kfusion[i] = P[i][stateIndex]*SK; } + + // Don't update Z accel bias state unless using a height observation (GPS velocities can be biased) + if (obsIndex != 5) { + Kfusion[13] = 0; + } + // Calculate state corrections and re-normalise the quaternions for (uint8_t i = 0; i<=indexLimit; i++) { @@ -1117,24 +1236,38 @@ void AttPosEKF::FuseVelposNED() ForceSymmetry(); ConstrainVariances(); - //printf("velh: %s, posh: %s, hgth: %s\n", ((velHealth) ? "OK" : "FAIL"), ((posHealth) ? "OK" : "FAIL"), ((hgtHealth) ? "OK" : "FAIL")); } void AttPosEKF::FuseMagnetometer() { - uint8_t obsIndex; - uint8_t indexLimit; + static uint8_t obsIndex; + static float MagPred[3]; + static float SH_MAG[9]; + static float q0 = 0.0f; + static float q1 = 0.0f; + static float q2 = 0.0f; + static float q3 = 1.0f; + static float magN = 0.4f; + static float magE = 0.0f; + static float magD = 0.3f; + static float magXbias = 0.0f; + static float magYbias = 0.0f; + static float magZbias = 0.0f; + float R_MAG = sq(magMeasurementSigma); float DCM[3][3] = { {1.0f,0.0f,0.0f} , {0.0f,1.0f,0.0f} , {0.0f,0.0f,1.0f} }; - float MagPred[3] = {0.0f,0.0f,0.0f}; float SK_MX[6]; float SK_MY[5]; float SK_MZ[6]; - float SH_MAG[9] = {0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f}; + float H_MAG[n_states]; + for (uint8_t i = 0; i < n_states; i++) { + H_MAG[i] = 0.0f; + } + uint8_t indexLimit; // Perform sequential fusion of Magnetometer measurements. // This assumes that the errors in the different components are @@ -1147,46 +1280,30 @@ void AttPosEKF::FuseMagnetometer() // Limit range of states modified when on ground if(!onGround) { - indexLimit = 20; + indexLimit = 22; } else { - indexLimit = 12; + indexLimit = 13; } - static float q0 = 0.0f; - static float q1 = 0.0f; - static float q2 = 0.0f; - static float q3 = 1.0f; - static float magN = 0.4f; - static float magE = 0.0f; - static float magD = 0.3f; - - static float R_MAG = 0.0025f; - - float H_MAG[21] = {0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f,0.0f}; - // Sequential fusion of XYZ components to spread processing load across // three prediction time steps. // Calculate observation jacobians and Kalman gains if (fuseMagData) { - static float magXbias = 0.0f; - static float magYbias = 0.0f; - static float magZbias = 0.0f; - // Copy required states to local variable names q0 = statesAtMagMeasTime[0]; q1 = statesAtMagMeasTime[1]; q2 = statesAtMagMeasTime[2]; q3 = statesAtMagMeasTime[3]; - magN = statesAtMagMeasTime[15]; - magE = statesAtMagMeasTime[16]; - magD = statesAtMagMeasTime[17]; - magXbias = statesAtMagMeasTime[18]; - magYbias = statesAtMagMeasTime[19]; - magZbias = statesAtMagMeasTime[20]; + magN = statesAtMagMeasTime[16]; + magE = statesAtMagMeasTime[17]; + magD = statesAtMagMeasTime[18]; + magXbias = statesAtMagMeasTime[19]; + magYbias = statesAtMagMeasTime[20]; + magZbias = statesAtMagMeasTime[21]; // rotate predicted earth components into body axes and calculate // predicted measurments @@ -1204,7 +1321,7 @@ void AttPosEKF::FuseMagnetometer() MagPred[2] = DCM[2][0]*magN + DCM[2][1]*magE + DCM[2][2]*magD + magZbias; // scale magnetometer observation error with total angular rate - R_MAG = 0.0025f + sq(0.05f*dAngIMU.length()/dtIMU); + R_MAG = sq(magMeasurementSigma) + sq(0.05f*dAngIMU.length()/dtIMU); // Calculate observation jacobians SH_MAG[0] = 2*magD*q3 + 2*magE*q2 + 2*magN*q1; @@ -1217,135 +1334,145 @@ void AttPosEKF::FuseMagnetometer() SH_MAG[7] = 2*magN*q0; SH_MAG[8] = 2*magE*q3; - for (uint8_t i=0; i<=20; i++) H_MAG[i] = 0; + for (uint8_t i = 0; i < n_states; i++) H_MAG[i] = 0; H_MAG[0] = SH_MAG[7] + SH_MAG[8] - 2*magD*q2; H_MAG[1] = SH_MAG[0]; H_MAG[2] = 2*magE*q1 - 2*magD*q0 - 2*magN*q2; H_MAG[3] = SH_MAG[2]; - H_MAG[15] = SH_MAG[5] - SH_MAG[4] - SH_MAG[3] + SH_MAG[6]; - H_MAG[16] = 2*q0*q3 + 2*q1*q2; - H_MAG[17] = 2*q1*q3 - 2*q0*q2; - H_MAG[18] = 1.0f; + H_MAG[16] = SH_MAG[5] - SH_MAG[4] - SH_MAG[3] + SH_MAG[6]; + H_MAG[17] = 2*q0*q3 + 2*q1*q2; + H_MAG[18] = 2*q1*q3 - 2*q0*q2; + H_MAG[19] = 1.0f; // Calculate Kalman gain - SK_MX[0] = 1/(P[18][18] + R_MAG + P[1][18]*SH_MAG[0] + P[3][18]*SH_MAG[2] - P[15][18]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) - (2*magD*q0 - 2*magE*q1 + 2*magN*q2)*(P[18][2] + P[1][2]*SH_MAG[0] + P[3][2]*SH_MAG[2] - P[15][2]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[16][2]*(2*q0*q3 + 2*q1*q2) - P[17][2]*(2*q0*q2 - 2*q1*q3) - P[2][2]*(2*magD*q0 - 2*magE*q1 + 2*magN*q2) + P[0][2]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + (SH_MAG[7] + SH_MAG[8] - 2*magD*q2)*(P[18][0] + P[1][0]*SH_MAG[0] + P[3][0]*SH_MAG[2] - P[15][0]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[16][0]*(2*q0*q3 + 2*q1*q2) - P[17][0]*(2*q0*q2 - 2*q1*q3) - P[2][0]*(2*magD*q0 - 2*magE*q1 + 2*magN*q2) + P[0][0]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + SH_MAG[0]*(P[18][1] + P[1][1]*SH_MAG[0] + P[3][1]*SH_MAG[2] - P[15][1]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[16][1]*(2*q0*q3 + 2*q1*q2) - P[17][1]*(2*q0*q2 - 2*q1*q3) - P[2][1]*(2*magD*q0 - 2*magE*q1 + 2*magN*q2) + P[0][1]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + SH_MAG[2]*(P[18][3] + P[1][3]*SH_MAG[0] + P[3][3]*SH_MAG[2] - P[15][3]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[16][3]*(2*q0*q3 + 2*q1*q2) - P[17][3]*(2*q0*q2 - 2*q1*q3) - P[2][3]*(2*magD*q0 - 2*magE*q1 + 2*magN*q2) + P[0][3]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) - (SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6])*(P[18][15] + P[1][15]*SH_MAG[0] + P[3][15]*SH_MAG[2] - P[15][15]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[16][15]*(2*q0*q3 + 2*q1*q2) - P[17][15]*(2*q0*q2 - 2*q1*q3) - P[2][15]*(2*magD*q0 - 2*magE*q1 + 2*magN*q2) + P[0][15]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + P[16][18]*(2*q0*q3 + 2*q1*q2) - P[17][18]*(2*q0*q2 - 2*q1*q3) - P[2][18]*(2*magD*q0 - 2*magE*q1 + 2*magN*q2) + (2*q0*q3 + 2*q1*q2)*(P[18][16] + P[1][16]*SH_MAG[0] + P[3][16]*SH_MAG[2] - P[15][16]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[16][16]*(2*q0*q3 + 2*q1*q2) - P[17][16]*(2*q0*q2 - 2*q1*q3) - P[2][16]*(2*magD*q0 - 2*magE*q1 + 2*magN*q2) + P[0][16]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) - (2*q0*q2 - 2*q1*q3)*(P[18][17] + P[1][17]*SH_MAG[0] + P[3][17]*SH_MAG[2] - P[15][17]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[16][17]*(2*q0*q3 + 2*q1*q2) - P[17][17]*(2*q0*q2 - 2*q1*q3) - P[2][17]*(2*magD*q0 - 2*magE*q1 + 2*magN*q2) + P[0][17]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + P[0][18]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)); + SK_MX[0] = 1/(P[19][19] + R_MAG + P[1][19]*SH_MAG[0] + P[3][19]*SH_MAG[2] - P[16][19]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) - (2*magD*q0 - 2*magE*q1 + 2*magN*q2)*(P[19][2] + P[1][2]*SH_MAG[0] + P[3][2]*SH_MAG[2] - P[16][2]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[17][2]*(2*q0*q3 + 2*q1*q2) - P[18][2]*(2*q0*q2 - 2*q1*q3) - P[2][2]*(2*magD*q0 - 2*magE*q1 + 2*magN*q2) + P[0][2]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + (SH_MAG[7] + SH_MAG[8] - 2*magD*q2)*(P[19][0] + P[1][0]*SH_MAG[0] + P[3][0]*SH_MAG[2] - P[16][0]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[17][0]*(2*q0*q3 + 2*q1*q2) - P[18][0]*(2*q0*q2 - 2*q1*q3) - P[2][0]*(2*magD*q0 - 2*magE*q1 + 2*magN*q2) + P[0][0]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + SH_MAG[0]*(P[19][1] + P[1][1]*SH_MAG[0] + P[3][1]*SH_MAG[2] - P[16][1]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[17][1]*(2*q0*q3 + 2*q1*q2) - P[18][1]*(2*q0*q2 - 2*q1*q3) - P[2][1]*(2*magD*q0 - 2*magE*q1 + 2*magN*q2) + P[0][1]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + SH_MAG[2]*(P[19][3] + P[1][3]*SH_MAG[0] + P[3][3]*SH_MAG[2] - P[16][3]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[17][3]*(2*q0*q3 + 2*q1*q2) - P[18][3]*(2*q0*q2 - 2*q1*q3) - P[2][3]*(2*magD*q0 - 2*magE*q1 + 2*magN*q2) + P[0][3]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) - (SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6])*(P[19][16] + P[1][16]*SH_MAG[0] + P[3][16]*SH_MAG[2] - P[16][16]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[17][16]*(2*q0*q3 + 2*q1*q2) - P[18][16]*(2*q0*q2 - 2*q1*q3) - P[2][16]*(2*magD*q0 - 2*magE*q1 + 2*magN*q2) + P[0][16]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + P[17][19]*(2*q0*q3 + 2*q1*q2) - P[18][19]*(2*q0*q2 - 2*q1*q3) - P[2][19]*(2*magD*q0 - 2*magE*q1 + 2*magN*q2) + (2*q0*q3 + 2*q1*q2)*(P[19][17] + P[1][17]*SH_MAG[0] + P[3][17]*SH_MAG[2] - P[16][17]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[17][17]*(2*q0*q3 + 2*q1*q2) - P[18][17]*(2*q0*q2 - 2*q1*q3) - P[2][17]*(2*magD*q0 - 2*magE*q1 + 2*magN*q2) + P[0][17]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) - (2*q0*q2 - 2*q1*q3)*(P[19][18] + P[1][18]*SH_MAG[0] + P[3][18]*SH_MAG[2] - P[16][18]*(SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]) + P[17][18]*(2*q0*q3 + 2*q1*q2) - P[18][18]*(2*q0*q2 - 2*q1*q3) - P[2][18]*(2*magD*q0 - 2*magE*q1 + 2*magN*q2) + P[0][18]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)) + P[0][19]*(SH_MAG[7] + SH_MAG[8] - 2*magD*q2)); SK_MX[1] = SH_MAG[3] + SH_MAG[4] - SH_MAG[5] - SH_MAG[6]; SK_MX[2] = 2*magD*q0 - 2*magE*q1 + 2*magN*q2; SK_MX[3] = SH_MAG[7] + SH_MAG[8] - 2*magD*q2; SK_MX[4] = 2*q0*q2 - 2*q1*q3; SK_MX[5] = 2*q0*q3 + 2*q1*q2; - Kfusion[0] = SK_MX[0]*(P[0][18] + P[0][1]*SH_MAG[0] + P[0][3]*SH_MAG[2] + P[0][0]*SK_MX[3] - P[0][2]*SK_MX[2] - P[0][15]*SK_MX[1] + P[0][16]*SK_MX[5] - P[0][17]*SK_MX[4]); - Kfusion[1] = SK_MX[0]*(P[1][18] + P[1][1]*SH_MAG[0] + P[1][3]*SH_MAG[2] + P[1][0]*SK_MX[3] - P[1][2]*SK_MX[2] - P[1][15]*SK_MX[1] + P[1][16]*SK_MX[5] - P[1][17]*SK_MX[4]); - Kfusion[2] = SK_MX[0]*(P[2][18] + P[2][1]*SH_MAG[0] + P[2][3]*SH_MAG[2] + P[2][0]*SK_MX[3] - P[2][2]*SK_MX[2] - P[2][15]*SK_MX[1] + P[2][16]*SK_MX[5] - P[2][17]*SK_MX[4]); - Kfusion[3] = SK_MX[0]*(P[3][18] + P[3][1]*SH_MAG[0] + P[3][3]*SH_MAG[2] + P[3][0]*SK_MX[3] - P[3][2]*SK_MX[2] - P[3][15]*SK_MX[1] + P[3][16]*SK_MX[5] - P[3][17]*SK_MX[4]); - Kfusion[4] = SK_MX[0]*(P[4][18] + P[4][1]*SH_MAG[0] + P[4][3]*SH_MAG[2] + P[4][0]*SK_MX[3] - P[4][2]*SK_MX[2] - P[4][15]*SK_MX[1] + P[4][16]*SK_MX[5] - P[4][17]*SK_MX[4]); - Kfusion[5] = SK_MX[0]*(P[5][18] + P[5][1]*SH_MAG[0] + P[5][3]*SH_MAG[2] + P[5][0]*SK_MX[3] - P[5][2]*SK_MX[2] - P[5][15]*SK_MX[1] + P[5][16]*SK_MX[5] - P[5][17]*SK_MX[4]); - Kfusion[6] = SK_MX[0]*(P[6][18] + P[6][1]*SH_MAG[0] + P[6][3]*SH_MAG[2] + P[6][0]*SK_MX[3] - P[6][2]*SK_MX[2] - P[6][15]*SK_MX[1] + P[6][16]*SK_MX[5] - P[6][17]*SK_MX[4]); - Kfusion[7] = SK_MX[0]*(P[7][18] + P[7][1]*SH_MAG[0] + P[7][3]*SH_MAG[2] + P[7][0]*SK_MX[3] - P[7][2]*SK_MX[2] - P[7][15]*SK_MX[1] + P[7][16]*SK_MX[5] - P[7][17]*SK_MX[4]); - Kfusion[8] = SK_MX[0]*(P[8][18] + P[8][1]*SH_MAG[0] + P[8][3]*SH_MAG[2] + P[8][0]*SK_MX[3] - P[8][2]*SK_MX[2] - P[8][15]*SK_MX[1] + P[8][16]*SK_MX[5] - P[8][17]*SK_MX[4]); - Kfusion[9] = SK_MX[0]*(P[9][18] + P[9][1]*SH_MAG[0] + P[9][3]*SH_MAG[2] + P[9][0]*SK_MX[3] - P[9][2]*SK_MX[2] - P[9][15]*SK_MX[1] + P[9][16]*SK_MX[5] - P[9][17]*SK_MX[4]); - Kfusion[10] = SK_MX[0]*(P[10][18] + P[10][1]*SH_MAG[0] + P[10][3]*SH_MAG[2] + P[10][0]*SK_MX[3] - P[10][2]*SK_MX[2] - P[10][15]*SK_MX[1] + P[10][16]*SK_MX[5] - P[10][17]*SK_MX[4]); - Kfusion[11] = SK_MX[0]*(P[11][18] + P[11][1]*SH_MAG[0] + P[11][3]*SH_MAG[2] + P[11][0]*SK_MX[3] - P[11][2]*SK_MX[2] - P[11][15]*SK_MX[1] + P[11][16]*SK_MX[5] - P[11][17]*SK_MX[4]); - Kfusion[12] = SK_MX[0]*(P[12][18] + P[12][1]*SH_MAG[0] + P[12][3]*SH_MAG[2] + P[12][0]*SK_MX[3] - P[12][2]*SK_MX[2] - P[12][15]*SK_MX[1] + P[12][16]*SK_MX[5] - P[12][17]*SK_MX[4]); - Kfusion[13] = SK_MX[0]*(P[13][18] + P[13][1]*SH_MAG[0] + P[13][3]*SH_MAG[2] + P[13][0]*SK_MX[3] - P[13][2]*SK_MX[2] - P[13][15]*SK_MX[1] + P[13][16]*SK_MX[5] - P[13][17]*SK_MX[4]); - Kfusion[14] = SK_MX[0]*(P[14][18] + P[14][1]*SH_MAG[0] + P[14][3]*SH_MAG[2] + P[14][0]*SK_MX[3] - P[14][2]*SK_MX[2] - P[14][15]*SK_MX[1] + P[14][16]*SK_MX[5] - P[14][17]*SK_MX[4]); - Kfusion[15] = SK_MX[0]*(P[15][18] + P[15][1]*SH_MAG[0] + P[15][3]*SH_MAG[2] + P[15][0]*SK_MX[3] - P[15][2]*SK_MX[2] - P[15][15]*SK_MX[1] + P[15][16]*SK_MX[5] - P[15][17]*SK_MX[4]); - Kfusion[16] = SK_MX[0]*(P[16][18] + P[16][1]*SH_MAG[0] + P[16][3]*SH_MAG[2] + P[16][0]*SK_MX[3] - P[16][2]*SK_MX[2] - P[16][15]*SK_MX[1] + P[16][16]*SK_MX[5] - P[16][17]*SK_MX[4]); - Kfusion[17] = SK_MX[0]*(P[17][18] + P[17][1]*SH_MAG[0] + P[17][3]*SH_MAG[2] + P[17][0]*SK_MX[3] - P[17][2]*SK_MX[2] - P[17][15]*SK_MX[1] + P[17][16]*SK_MX[5] - P[17][17]*SK_MX[4]); - Kfusion[18] = SK_MX[0]*(P[18][18] + P[18][1]*SH_MAG[0] + P[18][3]*SH_MAG[2] + P[18][0]*SK_MX[3] - P[18][2]*SK_MX[2] - P[18][15]*SK_MX[1] + P[18][16]*SK_MX[5] - P[18][17]*SK_MX[4]); - Kfusion[19] = SK_MX[0]*(P[19][18] + P[19][1]*SH_MAG[0] + P[19][3]*SH_MAG[2] + P[19][0]*SK_MX[3] - P[19][2]*SK_MX[2] - P[19][15]*SK_MX[1] + P[19][16]*SK_MX[5] - P[19][17]*SK_MX[4]); - Kfusion[20] = SK_MX[0]*(P[20][18] + P[20][1]*SH_MAG[0] + P[20][3]*SH_MAG[2] + P[20][0]*SK_MX[3] - P[20][2]*SK_MX[2] - P[20][15]*SK_MX[1] + P[20][16]*SK_MX[5] - P[20][17]*SK_MX[4]); + Kfusion[0] = SK_MX[0]*(P[0][19] + P[0][1]*SH_MAG[0] + P[0][3]*SH_MAG[2] + P[0][0]*SK_MX[3] - P[0][2]*SK_MX[2] - P[0][16]*SK_MX[1] + P[0][17]*SK_MX[5] - P[0][18]*SK_MX[4]); + Kfusion[1] = SK_MX[0]*(P[1][19] + P[1][1]*SH_MAG[0] + P[1][3]*SH_MAG[2] + P[1][0]*SK_MX[3] - P[1][2]*SK_MX[2] - P[1][16]*SK_MX[1] + P[1][17]*SK_MX[5] - P[1][18]*SK_MX[4]); + Kfusion[2] = SK_MX[0]*(P[2][19] + P[2][1]*SH_MAG[0] + P[2][3]*SH_MAG[2] + P[2][0]*SK_MX[3] - P[2][2]*SK_MX[2] - P[2][16]*SK_MX[1] + P[2][17]*SK_MX[5] - P[2][18]*SK_MX[4]); + Kfusion[3] = SK_MX[0]*(P[3][19] + P[3][1]*SH_MAG[0] + P[3][3]*SH_MAG[2] + P[3][0]*SK_MX[3] - P[3][2]*SK_MX[2] - P[3][16]*SK_MX[1] + P[3][17]*SK_MX[5] - P[3][18]*SK_MX[4]); + Kfusion[4] = SK_MX[0]*(P[4][19] + P[4][1]*SH_MAG[0] + P[4][3]*SH_MAG[2] + P[4][0]*SK_MX[3] - P[4][2]*SK_MX[2] - P[4][16]*SK_MX[1] + P[4][17]*SK_MX[5] - P[4][18]*SK_MX[4]); + Kfusion[5] = SK_MX[0]*(P[5][19] + P[5][1]*SH_MAG[0] + P[5][3]*SH_MAG[2] + P[5][0]*SK_MX[3] - P[5][2]*SK_MX[2] - P[5][16]*SK_MX[1] + P[5][17]*SK_MX[5] - P[5][18]*SK_MX[4]); + Kfusion[6] = SK_MX[0]*(P[6][19] + P[6][1]*SH_MAG[0] + P[6][3]*SH_MAG[2] + P[6][0]*SK_MX[3] - P[6][2]*SK_MX[2] - P[6][16]*SK_MX[1] + P[6][17]*SK_MX[5] - P[6][18]*SK_MX[4]); + Kfusion[7] = SK_MX[0]*(P[7][19] + P[7][1]*SH_MAG[0] + P[7][3]*SH_MAG[2] + P[7][0]*SK_MX[3] - P[7][2]*SK_MX[2] - P[7][16]*SK_MX[1] + P[7][17]*SK_MX[5] - P[7][18]*SK_MX[4]); + Kfusion[8] = SK_MX[0]*(P[8][19] + P[8][1]*SH_MAG[0] + P[8][3]*SH_MAG[2] + P[8][0]*SK_MX[3] - P[8][2]*SK_MX[2] - P[8][16]*SK_MX[1] + P[8][17]*SK_MX[5] - P[8][18]*SK_MX[4]); + Kfusion[9] = SK_MX[0]*(P[9][19] + P[9][1]*SH_MAG[0] + P[9][3]*SH_MAG[2] + P[9][0]*SK_MX[3] - P[9][2]*SK_MX[2] - P[9][16]*SK_MX[1] + P[9][17]*SK_MX[5] - P[9][18]*SK_MX[4]); + Kfusion[10] = SK_MX[0]*(P[10][19] + P[10][1]*SH_MAG[0] + P[10][3]*SH_MAG[2] + P[10][0]*SK_MX[3] - P[10][2]*SK_MX[2] - P[10][16]*SK_MX[1] + P[10][17]*SK_MX[5] - P[10][18]*SK_MX[4]); + Kfusion[11] = SK_MX[0]*(P[11][19] + P[11][1]*SH_MAG[0] + P[11][3]*SH_MAG[2] + P[11][0]*SK_MX[3] - P[11][2]*SK_MX[2] - P[11][16]*SK_MX[1] + P[11][17]*SK_MX[5] - P[11][18]*SK_MX[4]); + Kfusion[12] = SK_MX[0]*(P[12][19] + P[12][1]*SH_MAG[0] + P[12][3]*SH_MAG[2] + P[12][0]*SK_MX[3] - P[12][2]*SK_MX[2] - P[12][16]*SK_MX[1] + P[12][17]*SK_MX[5] - P[12][18]*SK_MX[4]); + // Only height measurements are allowed to modify the Z delta velocity bias state. This improves the stability of the estimate + Kfusion[13] = 0.0f;//SK_MX[0]*(P[13][19] + P[13][1]*SH_MAG[0] + P[13][3]*SH_MAG[2] + P[13][0]*SK_MX[3] - P[13][2]*SK_MX[2] - P[13][16]*SK_MX[1] + P[13][17]*SK_MX[5] - P[13][18]*SK_MX[4]); + Kfusion[14] = SK_MX[0]*(P[14][19] + P[14][1]*SH_MAG[0] + P[14][3]*SH_MAG[2] + P[14][0]*SK_MX[3] - P[14][2]*SK_MX[2] - P[14][16]*SK_MX[1] + P[14][17]*SK_MX[5] - P[14][18]*SK_MX[4]); + Kfusion[15] = SK_MX[0]*(P[15][19] + P[15][1]*SH_MAG[0] + P[15][3]*SH_MAG[2] + P[15][0]*SK_MX[3] - P[15][2]*SK_MX[2] - P[15][16]*SK_MX[1] + P[15][17]*SK_MX[5] - P[15][18]*SK_MX[4]); + Kfusion[16] = SK_MX[0]*(P[16][19] + P[16][1]*SH_MAG[0] + P[16][3]*SH_MAG[2] + P[16][0]*SK_MX[3] - P[16][2]*SK_MX[2] - P[16][16]*SK_MX[1] + P[16][17]*SK_MX[5] - P[16][18]*SK_MX[4]); + Kfusion[17] = SK_MX[0]*(P[17][19] + P[17][1]*SH_MAG[0] + P[17][3]*SH_MAG[2] + P[17][0]*SK_MX[3] - P[17][2]*SK_MX[2] - P[17][16]*SK_MX[1] + P[17][17]*SK_MX[5] - P[17][18]*SK_MX[4]); + Kfusion[18] = SK_MX[0]*(P[18][19] + P[18][1]*SH_MAG[0] + P[18][3]*SH_MAG[2] + P[18][0]*SK_MX[3] - P[18][2]*SK_MX[2] - P[18][16]*SK_MX[1] + P[18][17]*SK_MX[5] - P[18][18]*SK_MX[4]); + Kfusion[19] = SK_MX[0]*(P[19][19] + P[19][1]*SH_MAG[0] + P[19][3]*SH_MAG[2] + P[19][0]*SK_MX[3] - P[19][2]*SK_MX[2] - P[19][16]*SK_MX[1] + P[19][17]*SK_MX[5] - P[19][18]*SK_MX[4]); + Kfusion[20] = SK_MX[0]*(P[20][19] + P[20][1]*SH_MAG[0] + P[20][3]*SH_MAG[2] + P[20][0]*SK_MX[3] - P[20][2]*SK_MX[2] - P[20][16]*SK_MX[1] + P[20][17]*SK_MX[5] - P[20][18]*SK_MX[4]); + Kfusion[21] = SK_MX[0]*(P[21][19] + P[21][1]*SH_MAG[0] + P[21][3]*SH_MAG[2] + P[21][0]*SK_MX[3] - P[21][2]*SK_MX[2] - P[21][16]*SK_MX[1] + P[21][17]*SK_MX[5] - P[21][18]*SK_MX[4]); + Kfusion[22] = SK_MX[0]*(P[22][19] + P[22][1]*SH_MAG[0] + P[22][3]*SH_MAG[2] + P[22][0]*SK_MX[3] - P[22][2]*SK_MX[2] - P[22][16]*SK_MX[1] + P[22][17]*SK_MX[5] - P[22][18]*SK_MX[4]); varInnovMag[0] = 1.0f/SK_MX[0]; innovMag[0] = MagPred[0] - magData.x; // reset the observation index to 0 (we start by fusing the X // measurement) obsIndex = 0; + fuseMagData = false; } else if (obsIndex == 1) // we are now fusing the Y measurement { // Calculate observation jacobians - for (unsigned int i=0; i 1e-12f) { - for (uint8_t j= 0; j<=3; j++) + for (uint8_t j= 0; j <= 3; j++) { float quatMagInv = 1.0f/quatMag; states[j] = states[j] * quatMagInv; @@ -1513,38 +1642,38 @@ void AttPosEKF::FuseAirspeed() // correct the covariance P = (I - K*H)*P // take advantage of the empty columns in H to reduce the // number of operations - for (uint8_t i = 0; i<=20; i++) + for (uint8_t i = 0; i <= 22; i++) { - for (uint8_t j = 0; j<=3; j++) KH[i][j] = 0.0; - for (uint8_t j = 4; j<=6; j++) + for (uint8_t j = 0; j <= 3; j++) KH[i][j] = 0.0; + for (uint8_t j = 4; j <= 6; j++) { KH[i][j] = Kfusion[i] * H_TAS[j]; } - for (uint8_t j = 7; j<=12; j++) KH[i][j] = 0.0; - for (uint8_t j = 13; j<=14; j++) + for (uint8_t j = 7; j <= 13; j++) KH[i][j] = 0.0; + for (uint8_t j = 14; j <= 15; j++) { KH[i][j] = Kfusion[i] * H_TAS[j]; } - for (uint8_t j = 15; j<=20; j++) KH[i][j] = 0.0; + for (uint8_t j = 16; j <= 22; j++) KH[i][j] = 0.0; } - for (uint8_t i = 0; i<=20; i++) + for (uint8_t i = 0; i <= 22; i++) { - for (uint8_t j = 0; j<=20; j++) + for (uint8_t j = 0; j <= 22; j++) { KHP[i][j] = 0.0; - for (uint8_t k = 4; k<=6; k++) + for (uint8_t k = 4; k <= 6; k++) { KHP[i][j] = KHP[i][j] + KH[i][k] * P[k][j]; } - for (uint8_t k = 13; k<=14; k++) + for (uint8_t k = 14; k <= 15; k++) { KHP[i][j] = KHP[i][j] + KH[i][k] * P[k][j]; } } } - for (uint8_t i = 0; i<=20; i++) + for (uint8_t i = 0; i <= 22; i++) { - for (uint8_t j = 0; j<=20; j++) + for (uint8_t j = 0; j <= 22; j++) { P[i][j] = P[i][j] - KHP[i][j]; } @@ -1556,6 +1685,343 @@ void AttPosEKF::FuseAirspeed() ConstrainVariances(); } +void AttPosEKF::FuseRangeFinder() +{ + + // Local variables + float rngPred; + float SH_RNG[5]; + float H_RNG[23]; + float SK_RNG[6]; + float cosRngTilt; + const float R_RNG = 0.25f; // 0.5 m2 rangefinder measurement variance + + // Copy required states to local variable names + float q0 = statesAtRngTime[0]; + float q1 = statesAtRngTime[1]; + float q2 = statesAtRngTime[2]; + float q3 = statesAtRngTime[3]; + float pd = statesAtRngTime[9]; + float ptd = statesAtRngTime[22]; + + // Need to check that our range finder tilt angle is less than 30 degrees and we are using range finder data + SH_RNG[4] = sin(rngFinderPitch); + cosRngTilt = - Tbn.z.x * SH_RNG[4] + Tbn.z.z * cos(rngFinderPitch); + if (useRangeFinder && cosRngTilt > 0.87f) + { + // Calculate observation jacobian and Kalman gain ignoring all states other than the terrain offset + // This prevents the range finder measurement modifying any of the other filter states and significantly reduces computations + SH_RNG[0] = SH_RNG[4]*(2*q0*q2 - 2*q1*q3) - sq(q0) + sq(q1) + sq(q2) - sq(q3); + SH_RNG[1] = pd - ptd; + SH_RNG[2] = 1/sq(SH_RNG[0]); + SH_RNG[3] = 1/SH_RNG[0]; + for (uint8_t i = 0; i < n_states; i++) { + H_RNG[i] = 0.0f; + Kfusion[i] = 0.0f; + } + H_RNG[22] = -SH_RNG[3]; + SK_RNG[0] = 1/(R_RNG + SH_RNG[3]*(P[9][9]*SH_RNG[3] - P[22][9]*SH_RNG[3] + P[0][9]*SH_RNG[1]*SH_RNG[2]*(2*q0 - 2*q2*SH_RNG[4]) - P[1][9]*SH_RNG[1]*SH_RNG[2]*(2*q1 - 2*q3*SH_RNG[4]) - P[2][9]*SH_RNG[1]*SH_RNG[2]*(2*q2 + 2*q0*SH_RNG[4]) + P[3][9]*SH_RNG[1]*SH_RNG[2]*(2*q3 + 2*q1*SH_RNG[4])) - SH_RNG[3]*(P[9][22]*SH_RNG[3] - P[22][22]*SH_RNG[3] + P[0][22]*SH_RNG[1]*SH_RNG[2]*(2*q0 - 2*q2*SH_RNG[4]) - P[1][22]*SH_RNG[1]*SH_RNG[2]*(2*q1 - 2*q3*SH_RNG[4]) - P[2][22]*SH_RNG[1]*SH_RNG[2]*(2*q2 + 2*q0*SH_RNG[4]) + P[3][22]*SH_RNG[1]*SH_RNG[2]*(2*q3 + 2*q1*SH_RNG[4])) + SH_RNG[1]*SH_RNG[2]*(2*q0 - 2*q2*SH_RNG[4])*(P[9][0]*SH_RNG[3] - P[22][0]*SH_RNG[3] + P[0][0]*SH_RNG[1]*SH_RNG[2]*(2*q0 - 2*q2*SH_RNG[4]) - P[1][0]*SH_RNG[1]*SH_RNG[2]*(2*q1 - 2*q3*SH_RNG[4]) - P[2][0]*SH_RNG[1]*SH_RNG[2]*(2*q2 + 2*q0*SH_RNG[4]) + P[3][0]*SH_RNG[1]*SH_RNG[2]*(2*q3 + 2*q1*SH_RNG[4])) - SH_RNG[1]*SH_RNG[2]*(2*q1 - 2*q3*SH_RNG[4])*(P[9][1]*SH_RNG[3] - P[22][1]*SH_RNG[3] + P[0][1]*SH_RNG[1]*SH_RNG[2]*(2*q0 - 2*q2*SH_RNG[4]) - P[1][1]*SH_RNG[1]*SH_RNG[2]*(2*q1 - 2*q3*SH_RNG[4]) - P[2][1]*SH_RNG[1]*SH_RNG[2]*(2*q2 + 2*q0*SH_RNG[4]) + P[3][1]*SH_RNG[1]*SH_RNG[2]*(2*q3 + 2*q1*SH_RNG[4])) - SH_RNG[1]*SH_RNG[2]*(2*q2 + 2*q0*SH_RNG[4])*(P[9][2]*SH_RNG[3] - P[22][2]*SH_RNG[3] + P[0][2]*SH_RNG[1]*SH_RNG[2]*(2*q0 - 2*q2*SH_RNG[4]) - P[1][2]*SH_RNG[1]*SH_RNG[2]*(2*q1 - 2*q3*SH_RNG[4]) - P[2][2]*SH_RNG[1]*SH_RNG[2]*(2*q2 + 2*q0*SH_RNG[4]) + P[3][2]*SH_RNG[1]*SH_RNG[2]*(2*q3 + 2*q1*SH_RNG[4])) + SH_RNG[1]*SH_RNG[2]*(2*q3 + 2*q1*SH_RNG[4])*(P[9][3]*SH_RNG[3] - P[22][3]*SH_RNG[3] + P[0][3]*SH_RNG[1]*SH_RNG[2]*(2*q0 - 2*q2*SH_RNG[4]) - P[1][3]*SH_RNG[1]*SH_RNG[2]*(2*q1 - 2*q3*SH_RNG[4]) - P[2][3]*SH_RNG[1]*SH_RNG[2]*(2*q2 + 2*q0*SH_RNG[4]) + P[3][3]*SH_RNG[1]*SH_RNG[2]*(2*q3 + 2*q1*SH_RNG[4]))); + SK_RNG[1] = 2*q1 - 2*q3*SH_RNG[4]; + SK_RNG[2] = 2*q0 - 2*q2*SH_RNG[4]; + SK_RNG[3] = 2*q3 + 2*q1*SH_RNG[4]; + SK_RNG[4] = 2*q2 + 2*q0*SH_RNG[4]; + SK_RNG[5] = SH_RNG[2]; + Kfusion[22] = SK_RNG[0]*(P[22][9]*SH_RNG[3] - P[22][22]*SH_RNG[3] + P[22][0]*SH_RNG[1]*SK_RNG[2]*SK_RNG[5] - P[22][1]*SH_RNG[1]*SK_RNG[1]*SK_RNG[5] - P[22][2]*SH_RNG[1]*SK_RNG[4]*SK_RNG[5] + P[22][3]*SH_RNG[1]*SK_RNG[3]*SK_RNG[5]); + + // Calculate the measurement innovation + rngPred = (ptd - pd)/cosRngTilt; + innovRng = rngPred - rngMea; + //printf("mea=%5.1f, pred=%5.1f, pd=%5.1f, ptd=%5.2f\n", rngMea, rngPred, pd, ptd); + + // Check the innovation for consistency and don't fuse if > 5Sigma + if ((innovRng*innovRng*SK_RNG[0]) < 25) + { + // correct the state vector + states[22] = states[22] - Kfusion[22] * innovRng; + + // correct the covariance P = (I - K*H)*P + P[22][22] = P[22][22] - Kfusion[22] * H_RNG[22] * P[22][22]; + P[22][22] = ConstrainFloat(P[22][22], 0.0f, 10000.0f); + } + } + +} + +void AttPosEKF::FuseOpticalFlow() +{ + static uint8_t obsIndex; + static float losPred[2]; + static float SH_LOS[13]; + static float SK_LOS[17]; + static float q0 = 0.0f; + static float q1 = 0.0f; + static float q2 = 0.0f; + static float q3 = 1.0f; + static float vn = 0.0f; + static float ve = 0.0f; + static float vd = 0.0f; + static float pd = 0.0f; + static float ptd = 0.0f; + static float R_LOS = 0.01f; + // Transformation matrix from body to navigation axes + Mat3f DCM; + // Transformation matrix from sensor to body axes + // assume camera is aligned with Z body axis plus a misalignment + // defined by 3 small angles about X, Y and Z body axis + Mat3f Tsb; + Tsb.x.y = -a3; + Tsb.y.x = a3; + Tsb.x.z = a2; + Tsb.z.x = -a2; + Tsb.y.z = -a1; + Tsb.z.y = a1; + // Transformation matrix from navigation to sensor axes + Mat3f Tns; + float H_LOS[n_states]; + for (uint8_t i = 0; i < n_states; i++) { + H_LOS[i] = 0.0f; + } + Vector3f velNED; + Vector3f relVelSensor; + +// Perform sequential fusion of optical flow measurements only when in the air and tilt is less than 30 deg. + if (useOpticalFlow && (fuseOptData || obsIndex == 1) && !onGround && Tbn.z.z > 0.866f) + { + // Sequential fusion of XY components to spread processing load across + // three prediction time steps. + + // Calculate observation jacobians and Kalman gains + if (fuseOptData) + { + // Copy required states to local variable names + q0 = statesAtLosMeasTime[0]; + q1 = statesAtLosMeasTime[1]; + q2 = statesAtLosMeasTime[2]; + q3 = statesAtLosMeasTime[3]; + vn = statesAtLosMeasTime[4]; + ve = statesAtLosMeasTime[5]; + vd = statesAtLosMeasTime[6]; + pd = statesAtLosMeasTime[9]; + ptd = statesAtLosMeasTime[2]; + velNED.x = vn; + velNED.y = ve; + velNED.z = vd; + + // rotate predicted earth components into body axes and calculate + // predicted measurments + float q01 = q0 * q1; + float q02 = q0 * q2; + float q03 = q0 * q3; + float q12 = q1 * q2; + float q13 = q1 * q3; + float q23 = q2 * q3; + DCM.x.x = q0 + q1 - q2 - q3; + DCM.y.y = q0 - q1 + q2 - q3; + DCM.z.z = q0 - q1 - q2 + q3; + DCM.x.y = 2*(q12 - q03); + DCM.x.z = 2*(q13 + q02); + DCM.y.x = 2*(q12 + q03); + DCM.y.z = 2*(q23 - q01); + DCM.z.x = 2*(q13 - q02); + DCM.z.y = 2*(q23 + q01); + // calculate transformation from NED to sensor axes + Tns = DCM*Tsb; + Tns = Tns.transpose(); + // calculate range from ground plain to centre of sensor fov assuming flat earth + float range = ((ptd - pd)/Tns.z.z); + // calculate relative velocity in sensor frame + relVelSensor = Tns*velNED; + // divide by range to get predicted angular LOS rates relative to X and Y axes + losPred[0] = relVelSensor.y/range; + losPred[1] = -relVelSensor.x/range; + + // scale optical flow observation error with total angular rate + R_LOS = 0.01f;// + sq(0.05f*dAngIMU.length()/dtIMU); + + // Calculate observation jacobians + SH_LOS[0] = a1*(2*q0*q1 + 2*q2*q3) + a2*(2*q0*q2 - 2*q1*q3) - sq(q0) + sq(q1) + sq(q2) - sq(q3); + SH_LOS[1] = vd*(a2*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + 2*q0*q2 - 2*q1*q3 - a3*(2*q0*q1 + 2*q2*q3)) - ve*(a3*(sq(q0) - sq(q1) + sq(q2) - sq(q3)) + 2*q0*q3 + 2*q1*q2 + a2*(2*q0*q1 - 2*q2*q3)) + vn*(a2*(2*q0*q2 + 2*q1*q3) + a3*(2*q0*q3 - 2*q1*q2) - sq(q0) - sq(q1) + sq(q2) + sq(q3)); + SH_LOS[2] = ve*(a1*(2*q0*q1 - 2*q2*q3) + a3*(2*q0*q3 + 2*q1*q2) - sq(q0) + sq(q1) - sq(q2) + sq(q3)) - vd*(a1*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + 2*q0*q1 + 2*q2*q3 + a3*(2*q0*q2 - 2*q1*q3)) + vn*(a3*(sq(q0) + sq(q1) - sq(q2) - sq(q3)) + 2*q0*q3 - 2*q1*q2 - a1*(2*q0*q2 + 2*q1*q3)); + SH_LOS[3] = 1/(pd - ptd); + SH_LOS[4] = 2*q1 - 2*a2*q3 + 2*a3*q2; + SH_LOS[5] = 2*a2*q2 - 2*q0 + 2*a3*q3; + SH_LOS[6] = 2*q2 + 2*a2*q0 - 2*a3*q1; + SH_LOS[7] = 1/sq(pd - ptd); + SH_LOS[8] = 2*q2 + 2*a1*q3 - 2*a3*q1; + SH_LOS[9] = 2*q3 - 2*a1*q2 + 2*a3*q0; + SH_LOS[10] = 2*a1*q1 - 2*q0 + 2*a3*q3; + SH_LOS[11] = 2*q3 + 2*a2*q1 + 2*a3*q0; + SH_LOS[12] = 2*q1 + 2*a1*q0 + 2*a3*q2; + + for (uint8_t i = 0; i < n_states; i++) H_LOS[i] = 0; + H_LOS[0] = - SH_LOS[2]*SH_LOS[3]*(2*a1*q1 - 2*q0 + 2*a2*q2) - SH_LOS[0]*SH_LOS[3]*(ve*SH_LOS[10] - vd*SH_LOS[12] + vn*SH_LOS[9]); + H_LOS[1] = - SH_LOS[2]*SH_LOS[3]*(2*q1 + 2*a1*q0 - 2*a2*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[10] + ve*SH_LOS[12] - vn*SH_LOS[8]); + H_LOS[2] = SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[9] + ve*SH_LOS[8] + vn*SH_LOS[12]) - SH_LOS[2]*SH_LOS[3]*(2*q2 + 2*a2*q0 + 2*a1*q3); + H_LOS[3] = SH_LOS[2]*SH_LOS[3]*(2*q3 - 2*a1*q2 + 2*a2*q1) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[8] - ve*SH_LOS[9] + vn*SH_LOS[10]); + H_LOS[4] = -SH_LOS[0]*SH_LOS[3]*(a3*(sq(q0) + sq(q1) - sq(q2) - sq(q3)) + 2*q0*q3 - 2*q1*q2 - a1*(2*q0*q2 + 2*q1*q3)); + H_LOS[5] = -SH_LOS[0]*SH_LOS[3]*(a1*(2*q0*q1 - 2*q2*q3) + a3*(2*q0*q3 + 2*q1*q2) - sq(q0) + sq(q1) - sq(q2) + sq(q3)); + H_LOS[6] = SH_LOS[0]*SH_LOS[3]*(a1*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + 2*q0*q1 + 2*q2*q3 + a3*(2*q0*q2 - 2*q1*q3)); + H_LOS[9] = SH_LOS[0]*SH_LOS[2]*SH_LOS[7]; + H_LOS[22] = -SH_LOS[0]*SH_LOS[2]*SH_LOS[7]; + + // Calculate Kalman gain + SK_LOS[0] = 1/(R_LOS + (SH_LOS[2]*SH_LOS[3]*(2*a1*q1 - 2*q0 + 2*a2*q2) + SH_LOS[0]*SH_LOS[3]*(ve*SH_LOS[10] - vd*SH_LOS[12] + vn*SH_LOS[9]))*(P[0][0]*(SH_LOS[2]*SH_LOS[3]*(2*a1*q1 - 2*q0 + 2*a2*q2) + SH_LOS[0]*SH_LOS[3]*(ve*SH_LOS[10] - vd*SH_LOS[12] + vn*SH_LOS[9])) + P[1][0]*(SH_LOS[2]*SH_LOS[3]*(2*q1 + 2*a1*q0 - 2*a2*q3) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[10] + ve*SH_LOS[12] - vn*SH_LOS[8])) + P[2][0]*(SH_LOS[2]*SH_LOS[3]*(2*q2 + 2*a2*q0 + 2*a1*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[9] + ve*SH_LOS[8] + vn*SH_LOS[12])) - P[3][0]*(SH_LOS[2]*SH_LOS[3]*(2*q3 - 2*a1*q2 + 2*a2*q1) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[8] - ve*SH_LOS[9] + vn*SH_LOS[10])) - P[9][0]*SH_LOS[0]*SH_LOS[2]*SH_LOS[7] + P[22][0]*SH_LOS[0]*SH_LOS[2]*SH_LOS[7] + P[4][0]*SH_LOS[0]*SH_LOS[3]*(a3*(sq(q0) + sq(q1) - sq(q2) - sq(q3)) + 2*q0*q3 - 2*q1*q2 - a1*(2*q0*q2 + 2*q1*q3)) + P[5][0]*SH_LOS[0]*SH_LOS[3]*(a1*(2*q0*q1 - 2*q2*q3) + a3*(2*q0*q3 + 2*q1*q2) - sq(q0) + sq(q1) - sq(q2) + sq(q3)) - P[6][0]*SH_LOS[0]*SH_LOS[3]*(a1*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + 2*q0*q1 + 2*q2*q3 + a3*(2*q0*q2 - 2*q1*q3))) + (SH_LOS[2]*SH_LOS[3]*(2*q1 + 2*a1*q0 - 2*a2*q3) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[10] + ve*SH_LOS[12] - vn*SH_LOS[8]))*(P[0][1]*(SH_LOS[2]*SH_LOS[3]*(2*a1*q1 - 2*q0 + 2*a2*q2) + SH_LOS[0]*SH_LOS[3]*(ve*SH_LOS[10] - vd*SH_LOS[12] + vn*SH_LOS[9])) + P[1][1]*(SH_LOS[2]*SH_LOS[3]*(2*q1 + 2*a1*q0 - 2*a2*q3) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[10] + ve*SH_LOS[12] - vn*SH_LOS[8])) + P[2][1]*(SH_LOS[2]*SH_LOS[3]*(2*q2 + 2*a2*q0 + 2*a1*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[9] + ve*SH_LOS[8] + vn*SH_LOS[12])) - P[3][1]*(SH_LOS[2]*SH_LOS[3]*(2*q3 - 2*a1*q2 + 2*a2*q1) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[8] - ve*SH_LOS[9] + vn*SH_LOS[10])) - P[9][1]*SH_LOS[0]*SH_LOS[2]*SH_LOS[7] + P[22][1]*SH_LOS[0]*SH_LOS[2]*SH_LOS[7] + P[4][1]*SH_LOS[0]*SH_LOS[3]*(a3*(sq(q0) + sq(q1) - sq(q2) - sq(q3)) + 2*q0*q3 - 2*q1*q2 - a1*(2*q0*q2 + 2*q1*q3)) + P[5][1]*SH_LOS[0]*SH_LOS[3]*(a1*(2*q0*q1 - 2*q2*q3) + a3*(2*q0*q3 + 2*q1*q2) - sq(q0) + sq(q1) - sq(q2) + sq(q3)) - P[6][1]*SH_LOS[0]*SH_LOS[3]*(a1*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + 2*q0*q1 + 2*q2*q3 + a3*(2*q0*q2 - 2*q1*q3))) + (SH_LOS[2]*SH_LOS[3]*(2*q2 + 2*a2*q0 + 2*a1*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[9] + ve*SH_LOS[8] + vn*SH_LOS[12]))*(P[0][2]*(SH_LOS[2]*SH_LOS[3]*(2*a1*q1 - 2*q0 + 2*a2*q2) + SH_LOS[0]*SH_LOS[3]*(ve*SH_LOS[10] - vd*SH_LOS[12] + vn*SH_LOS[9])) + P[1][2]*(SH_LOS[2]*SH_LOS[3]*(2*q1 + 2*a1*q0 - 2*a2*q3) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[10] + ve*SH_LOS[12] - vn*SH_LOS[8])) + P[2][2]*(SH_LOS[2]*SH_LOS[3]*(2*q2 + 2*a2*q0 + 2*a1*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[9] + ve*SH_LOS[8] + vn*SH_LOS[12])) - P[3][2]*(SH_LOS[2]*SH_LOS[3]*(2*q3 - 2*a1*q2 + 2*a2*q1) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[8] - ve*SH_LOS[9] + vn*SH_LOS[10])) - P[9][2]*SH_LOS[0]*SH_LOS[2]*SH_LOS[7] + P[22][2]*SH_LOS[0]*SH_LOS[2]*SH_LOS[7] + P[4][2]*SH_LOS[0]*SH_LOS[3]*(a3*(sq(q0) + sq(q1) - sq(q2) - sq(q3)) + 2*q0*q3 - 2*q1*q2 - a1*(2*q0*q2 + 2*q1*q3)) + P[5][2]*SH_LOS[0]*SH_LOS[3]*(a1*(2*q0*q1 - 2*q2*q3) + a3*(2*q0*q3 + 2*q1*q2) - sq(q0) + sq(q1) - sq(q2) + sq(q3)) - P[6][2]*SH_LOS[0]*SH_LOS[3]*(a1*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + 2*q0*q1 + 2*q2*q3 + a3*(2*q0*q2 - 2*q1*q3))) - (SH_LOS[2]*SH_LOS[3]*(2*q3 - 2*a1*q2 + 2*a2*q1) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[8] - ve*SH_LOS[9] + vn*SH_LOS[10]))*(P[0][3]*(SH_LOS[2]*SH_LOS[3]*(2*a1*q1 - 2*q0 + 2*a2*q2) + SH_LOS[0]*SH_LOS[3]*(ve*SH_LOS[10] - vd*SH_LOS[12] + vn*SH_LOS[9])) + P[1][3]*(SH_LOS[2]*SH_LOS[3]*(2*q1 + 2*a1*q0 - 2*a2*q3) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[10] + ve*SH_LOS[12] - vn*SH_LOS[8])) + P[2][3]*(SH_LOS[2]*SH_LOS[3]*(2*q2 + 2*a2*q0 + 2*a1*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[9] + ve*SH_LOS[8] + vn*SH_LOS[12])) - P[3][3]*(SH_LOS[2]*SH_LOS[3]*(2*q3 - 2*a1*q2 + 2*a2*q1) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[8] - ve*SH_LOS[9] + vn*SH_LOS[10])) - P[9][3]*SH_LOS[0]*SH_LOS[2]*SH_LOS[7] + P[22][3]*SH_LOS[0]*SH_LOS[2]*SH_LOS[7] + P[4][3]*SH_LOS[0]*SH_LOS[3]*(a3*(sq(q0) + sq(q1) - sq(q2) - sq(q3)) + 2*q0*q3 - 2*q1*q2 - a1*(2*q0*q2 + 2*q1*q3)) + P[5][3]*SH_LOS[0]*SH_LOS[3]*(a1*(2*q0*q1 - 2*q2*q3) + a3*(2*q0*q3 + 2*q1*q2) - sq(q0) + sq(q1) - sq(q2) + sq(q3)) - P[6][3]*SH_LOS[0]*SH_LOS[3]*(a1*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + 2*q0*q1 + 2*q2*q3 + a3*(2*q0*q2 - 2*q1*q3))) - SH_LOS[0]*SH_LOS[2]*SH_LOS[7]*(P[0][9]*(SH_LOS[2]*SH_LOS[3]*(2*a1*q1 - 2*q0 + 2*a2*q2) + SH_LOS[0]*SH_LOS[3]*(ve*SH_LOS[10] - vd*SH_LOS[12] + vn*SH_LOS[9])) + P[1][9]*(SH_LOS[2]*SH_LOS[3]*(2*q1 + 2*a1*q0 - 2*a2*q3) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[10] + ve*SH_LOS[12] - vn*SH_LOS[8])) + P[2][9]*(SH_LOS[2]*SH_LOS[3]*(2*q2 + 2*a2*q0 + 2*a1*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[9] + ve*SH_LOS[8] + vn*SH_LOS[12])) - P[3][9]*(SH_LOS[2]*SH_LOS[3]*(2*q3 - 2*a1*q2 + 2*a2*q1) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[8] - ve*SH_LOS[9] + vn*SH_LOS[10])) - P[9][9]*SH_LOS[0]*SH_LOS[2]*SH_LOS[7] + P[22][9]*SH_LOS[0]*SH_LOS[2]*SH_LOS[7] + P[4][9]*SH_LOS[0]*SH_LOS[3]*(a3*(sq(q0) + sq(q1) - sq(q2) - sq(q3)) + 2*q0*q3 - 2*q1*q2 - a1*(2*q0*q2 + 2*q1*q3)) + P[5][9]*SH_LOS[0]*SH_LOS[3]*(a1*(2*q0*q1 - 2*q2*q3) + a3*(2*q0*q3 + 2*q1*q2) - sq(q0) + sq(q1) - sq(q2) + sq(q3)) - P[6][9]*SH_LOS[0]*SH_LOS[3]*(a1*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + 2*q0*q1 + 2*q2*q3 + a3*(2*q0*q2 - 2*q1*q3))) + SH_LOS[0]*SH_LOS[2]*SH_LOS[7]*(P[0][22]*(SH_LOS[2]*SH_LOS[3]*(2*a1*q1 - 2*q0 + 2*a2*q2) + SH_LOS[0]*SH_LOS[3]*(ve*SH_LOS[10] - vd*SH_LOS[12] + vn*SH_LOS[9])) + P[1][22]*(SH_LOS[2]*SH_LOS[3]*(2*q1 + 2*a1*q0 - 2*a2*q3) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[10] + ve*SH_LOS[12] - vn*SH_LOS[8])) + P[2][22]*(SH_LOS[2]*SH_LOS[3]*(2*q2 + 2*a2*q0 + 2*a1*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[9] + ve*SH_LOS[8] + vn*SH_LOS[12])) - P[3][22]*(SH_LOS[2]*SH_LOS[3]*(2*q3 - 2*a1*q2 + 2*a2*q1) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[8] - ve*SH_LOS[9] + vn*SH_LOS[10])) - P[9][22]*SH_LOS[0]*SH_LOS[2]*SH_LOS[7] + P[22][22]*SH_LOS[0]*SH_LOS[2]*SH_LOS[7] + P[4][22]*SH_LOS[0]*SH_LOS[3]*(a3*(sq(q0) + sq(q1) - sq(q2) - sq(q3)) + 2*q0*q3 - 2*q1*q2 - a1*(2*q0*q2 + 2*q1*q3)) + P[5][22]*SH_LOS[0]*SH_LOS[3]*(a1*(2*q0*q1 - 2*q2*q3) + a3*(2*q0*q3 + 2*q1*q2) - sq(q0) + sq(q1) - sq(q2) + sq(q3)) - P[6][22]*SH_LOS[0]*SH_LOS[3]*(a1*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + 2*q0*q1 + 2*q2*q3 + a3*(2*q0*q2 - 2*q1*q3))) + SH_LOS[0]*SH_LOS[3]*(a3*(sq(q0) + sq(q1) - sq(q2) - sq(q3)) + 2*q0*q3 - 2*q1*q2 - a1*(2*q0*q2 + 2*q1*q3))*(P[0][4]*(SH_LOS[2]*SH_LOS[3]*(2*a1*q1 - 2*q0 + 2*a2*q2) + SH_LOS[0]*SH_LOS[3]*(ve*SH_LOS[10] - vd*SH_LOS[12] + vn*SH_LOS[9])) + P[1][4]*(SH_LOS[2]*SH_LOS[3]*(2*q1 + 2*a1*q0 - 2*a2*q3) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[10] + ve*SH_LOS[12] - vn*SH_LOS[8])) + P[2][4]*(SH_LOS[2]*SH_LOS[3]*(2*q2 + 2*a2*q0 + 2*a1*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[9] + ve*SH_LOS[8] + vn*SH_LOS[12])) - P[3][4]*(SH_LOS[2]*SH_LOS[3]*(2*q3 - 2*a1*q2 + 2*a2*q1) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[8] - ve*SH_LOS[9] + vn*SH_LOS[10])) - P[9][4]*SH_LOS[0]*SH_LOS[2]*SH_LOS[7] + P[22][4]*SH_LOS[0]*SH_LOS[2]*SH_LOS[7] + P[4][4]*SH_LOS[0]*SH_LOS[3]*(a3*(sq(q0) + sq(q1) - sq(q2) - sq(q3)) + 2*q0*q3 - 2*q1*q2 - a1*(2*q0*q2 + 2*q1*q3)) + P[5][4]*SH_LOS[0]*SH_LOS[3]*(a1*(2*q0*q1 - 2*q2*q3) + a3*(2*q0*q3 + 2*q1*q2) - sq(q0) + sq(q1) - sq(q2) + sq(q3)) - P[6][4]*SH_LOS[0]*SH_LOS[3]*(a1*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + 2*q0*q1 + 2*q2*q3 + a3*(2*q0*q2 - 2*q1*q3))) + SH_LOS[0]*SH_LOS[3]*(a1*(2*q0*q1 - 2*q2*q3) + a3*(2*q0*q3 + 2*q1*q2) - sq(q0) + sq(q1) - sq(q2) + sq(q3))*(P[0][5]*(SH_LOS[2]*SH_LOS[3]*(2*a1*q1 - 2*q0 + 2*a2*q2) + SH_LOS[0]*SH_LOS[3]*(ve*SH_LOS[10] - vd*SH_LOS[12] + vn*SH_LOS[9])) + P[1][5]*(SH_LOS[2]*SH_LOS[3]*(2*q1 + 2*a1*q0 - 2*a2*q3) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[10] + ve*SH_LOS[12] - vn*SH_LOS[8])) + P[2][5]*(SH_LOS[2]*SH_LOS[3]*(2*q2 + 2*a2*q0 + 2*a1*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[9] + ve*SH_LOS[8] + vn*SH_LOS[12])) - P[3][5]*(SH_LOS[2]*SH_LOS[3]*(2*q3 - 2*a1*q2 + 2*a2*q1) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[8] - ve*SH_LOS[9] + vn*SH_LOS[10])) - P[9][5]*SH_LOS[0]*SH_LOS[2]*SH_LOS[7] + P[22][5]*SH_LOS[0]*SH_LOS[2]*SH_LOS[7] + P[4][5]*SH_LOS[0]*SH_LOS[3]*(a3*(sq(q0) + sq(q1) - sq(q2) - sq(q3)) + 2*q0*q3 - 2*q1*q2 - a1*(2*q0*q2 + 2*q1*q3)) + P[5][5]*SH_LOS[0]*SH_LOS[3]*(a1*(2*q0*q1 - 2*q2*q3) + a3*(2*q0*q3 + 2*q1*q2) - sq(q0) + sq(q1) - sq(q2) + sq(q3)) - P[6][5]*SH_LOS[0]*SH_LOS[3]*(a1*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + 2*q0*q1 + 2*q2*q3 + a3*(2*q0*q2 - 2*q1*q3))) - SH_LOS[0]*SH_LOS[3]*(a1*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + 2*q0*q1 + 2*q2*q3 + a3*(2*q0*q2 - 2*q1*q3))*(P[0][6]*(SH_LOS[2]*SH_LOS[3]*(2*a1*q1 - 2*q0 + 2*a2*q2) + SH_LOS[0]*SH_LOS[3]*(ve*SH_LOS[10] - vd*SH_LOS[12] + vn*SH_LOS[9])) + P[1][6]*(SH_LOS[2]*SH_LOS[3]*(2*q1 + 2*a1*q0 - 2*a2*q3) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[10] + ve*SH_LOS[12] - vn*SH_LOS[8])) + P[2][6]*(SH_LOS[2]*SH_LOS[3]*(2*q2 + 2*a2*q0 + 2*a1*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[9] + ve*SH_LOS[8] + vn*SH_LOS[12])) - P[3][6]*(SH_LOS[2]*SH_LOS[3]*(2*q3 - 2*a1*q2 + 2*a2*q1) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[8] - ve*SH_LOS[9] + vn*SH_LOS[10])) - P[9][6]*SH_LOS[0]*SH_LOS[2]*SH_LOS[7] + P[22][6]*SH_LOS[0]*SH_LOS[2]*SH_LOS[7] + P[4][6]*SH_LOS[0]*SH_LOS[3]*(a3*(sq(q0) + sq(q1) - sq(q2) - sq(q3)) + 2*q0*q3 - 2*q1*q2 - a1*(2*q0*q2 + 2*q1*q3)) + P[5][6]*SH_LOS[0]*SH_LOS[3]*(a1*(2*q0*q1 - 2*q2*q3) + a3*(2*q0*q3 + 2*q1*q2) - sq(q0) + sq(q1) - sq(q2) + sq(q3)) - P[6][6]*SH_LOS[0]*SH_LOS[3]*(a1*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + 2*q0*q1 + 2*q2*q3 + a3*(2*q0*q2 - 2*q1*q3)))); + SK_LOS[1] = 1/(R_LOS + (SH_LOS[1]*SH_LOS[3]*(2*a1*q1 - 2*q0 + 2*a2*q2) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[6] - ve*SH_LOS[11] + vn*SH_LOS[5]))*(P[0][0]*(SH_LOS[1]*SH_LOS[3]*(2*a1*q1 - 2*q0 + 2*a2*q2) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[6] - ve*SH_LOS[11] + vn*SH_LOS[5])) + P[1][0]*(SH_LOS[1]*SH_LOS[3]*(2*q1 + 2*a1*q0 - 2*a2*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[11] + ve*SH_LOS[6] + vn*SH_LOS[4])) + P[2][0]*(SH_LOS[1]*SH_LOS[3]*(2*q2 + 2*a2*q0 + 2*a1*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[5] + ve*SH_LOS[4] - vn*SH_LOS[6])) - P[3][0]*(SH_LOS[1]*SH_LOS[3]*(2*q3 - 2*a1*q2 + 2*a2*q1) - SH_LOS[0]*SH_LOS[3]*(ve*SH_LOS[5] - vd*SH_LOS[4] + vn*SH_LOS[11])) - P[9][0]*SH_LOS[0]*SH_LOS[1]*SH_LOS[7] + P[22][0]*SH_LOS[0]*SH_LOS[1]*SH_LOS[7] + P[4][0]*SH_LOS[0]*SH_LOS[3]*(a2*(2*q0*q2 + 2*q1*q3) + a3*(2*q0*q3 - 2*q1*q2) - sq(q0) - sq(q1) + sq(q2) + sq(q3)) - P[5][0]*SH_LOS[0]*SH_LOS[3]*(a3*(sq(q0) - sq(q1) + sq(q2) - sq(q3)) + 2*q0*q3 + 2*q1*q2 + a2*(2*q0*q1 - 2*q2*q3)) + P[6][0]*SH_LOS[0]*SH_LOS[3]*(a2*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + 2*q0*q2 - 2*q1*q3 - a3*(2*q0*q1 + 2*q2*q3))) + (SH_LOS[1]*SH_LOS[3]*(2*q1 + 2*a1*q0 - 2*a2*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[11] + ve*SH_LOS[6] + vn*SH_LOS[4]))*(P[0][1]*(SH_LOS[1]*SH_LOS[3]*(2*a1*q1 - 2*q0 + 2*a2*q2) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[6] - ve*SH_LOS[11] + vn*SH_LOS[5])) + P[1][1]*(SH_LOS[1]*SH_LOS[3]*(2*q1 + 2*a1*q0 - 2*a2*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[11] + ve*SH_LOS[6] + vn*SH_LOS[4])) + P[2][1]*(SH_LOS[1]*SH_LOS[3]*(2*q2 + 2*a2*q0 + 2*a1*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[5] + ve*SH_LOS[4] - vn*SH_LOS[6])) - P[3][1]*(SH_LOS[1]*SH_LOS[3]*(2*q3 - 2*a1*q2 + 2*a2*q1) - SH_LOS[0]*SH_LOS[3]*(ve*SH_LOS[5] - vd*SH_LOS[4] + vn*SH_LOS[11])) - P[9][1]*SH_LOS[0]*SH_LOS[1]*SH_LOS[7] + P[22][1]*SH_LOS[0]*SH_LOS[1]*SH_LOS[7] + P[4][1]*SH_LOS[0]*SH_LOS[3]*(a2*(2*q0*q2 + 2*q1*q3) + a3*(2*q0*q3 - 2*q1*q2) - sq(q0) - sq(q1) + sq(q2) + sq(q3)) - P[5][1]*SH_LOS[0]*SH_LOS[3]*(a3*(sq(q0) - sq(q1) + sq(q2) - sq(q3)) + 2*q0*q3 + 2*q1*q2 + a2*(2*q0*q1 - 2*q2*q3)) + P[6][1]*SH_LOS[0]*SH_LOS[3]*(a2*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + 2*q0*q2 - 2*q1*q3 - a3*(2*q0*q1 + 2*q2*q3))) + (SH_LOS[1]*SH_LOS[3]*(2*q2 + 2*a2*q0 + 2*a1*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[5] + ve*SH_LOS[4] - vn*SH_LOS[6]))*(P[0][2]*(SH_LOS[1]*SH_LOS[3]*(2*a1*q1 - 2*q0 + 2*a2*q2) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[6] - ve*SH_LOS[11] + vn*SH_LOS[5])) + P[1][2]*(SH_LOS[1]*SH_LOS[3]*(2*q1 + 2*a1*q0 - 2*a2*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[11] + ve*SH_LOS[6] + vn*SH_LOS[4])) + P[2][2]*(SH_LOS[1]*SH_LOS[3]*(2*q2 + 2*a2*q0 + 2*a1*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[5] + ve*SH_LOS[4] - vn*SH_LOS[6])) - P[3][2]*(SH_LOS[1]*SH_LOS[3]*(2*q3 - 2*a1*q2 + 2*a2*q1) - SH_LOS[0]*SH_LOS[3]*(ve*SH_LOS[5] - vd*SH_LOS[4] + vn*SH_LOS[11])) - P[9][2]*SH_LOS[0]*SH_LOS[1]*SH_LOS[7] + P[22][2]*SH_LOS[0]*SH_LOS[1]*SH_LOS[7] + P[4][2]*SH_LOS[0]*SH_LOS[3]*(a2*(2*q0*q2 + 2*q1*q3) + a3*(2*q0*q3 - 2*q1*q2) - sq(q0) - sq(q1) + sq(q2) + sq(q3)) - P[5][2]*SH_LOS[0]*SH_LOS[3]*(a3*(sq(q0) - sq(q1) + sq(q2) - sq(q3)) + 2*q0*q3 + 2*q1*q2 + a2*(2*q0*q1 - 2*q2*q3)) + P[6][2]*SH_LOS[0]*SH_LOS[3]*(a2*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + 2*q0*q2 - 2*q1*q3 - a3*(2*q0*q1 + 2*q2*q3))) - (SH_LOS[1]*SH_LOS[3]*(2*q3 - 2*a1*q2 + 2*a2*q1) - SH_LOS[0]*SH_LOS[3]*(ve*SH_LOS[5] - vd*SH_LOS[4] + vn*SH_LOS[11]))*(P[0][3]*(SH_LOS[1]*SH_LOS[3]*(2*a1*q1 - 2*q0 + 2*a2*q2) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[6] - ve*SH_LOS[11] + vn*SH_LOS[5])) + P[1][3]*(SH_LOS[1]*SH_LOS[3]*(2*q1 + 2*a1*q0 - 2*a2*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[11] + ve*SH_LOS[6] + vn*SH_LOS[4])) + P[2][3]*(SH_LOS[1]*SH_LOS[3]*(2*q2 + 2*a2*q0 + 2*a1*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[5] + ve*SH_LOS[4] - vn*SH_LOS[6])) - P[3][3]*(SH_LOS[1]*SH_LOS[3]*(2*q3 - 2*a1*q2 + 2*a2*q1) - SH_LOS[0]*SH_LOS[3]*(ve*SH_LOS[5] - vd*SH_LOS[4] + vn*SH_LOS[11])) - P[9][3]*SH_LOS[0]*SH_LOS[1]*SH_LOS[7] + P[22][3]*SH_LOS[0]*SH_LOS[1]*SH_LOS[7] + P[4][3]*SH_LOS[0]*SH_LOS[3]*(a2*(2*q0*q2 + 2*q1*q3) + a3*(2*q0*q3 - 2*q1*q2) - sq(q0) - sq(q1) + sq(q2) + sq(q3)) - P[5][3]*SH_LOS[0]*SH_LOS[3]*(a3*(sq(q0) - sq(q1) + sq(q2) - sq(q3)) + 2*q0*q3 + 2*q1*q2 + a2*(2*q0*q1 - 2*q2*q3)) + P[6][3]*SH_LOS[0]*SH_LOS[3]*(a2*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + 2*q0*q2 - 2*q1*q3 - a3*(2*q0*q1 + 2*q2*q3))) - SH_LOS[0]*SH_LOS[1]*SH_LOS[7]*(P[0][9]*(SH_LOS[1]*SH_LOS[3]*(2*a1*q1 - 2*q0 + 2*a2*q2) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[6] - ve*SH_LOS[11] + vn*SH_LOS[5])) + P[1][9]*(SH_LOS[1]*SH_LOS[3]*(2*q1 + 2*a1*q0 - 2*a2*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[11] + ve*SH_LOS[6] + vn*SH_LOS[4])) + P[2][9]*(SH_LOS[1]*SH_LOS[3]*(2*q2 + 2*a2*q0 + 2*a1*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[5] + ve*SH_LOS[4] - vn*SH_LOS[6])) - P[3][9]*(SH_LOS[1]*SH_LOS[3]*(2*q3 - 2*a1*q2 + 2*a2*q1) - SH_LOS[0]*SH_LOS[3]*(ve*SH_LOS[5] - vd*SH_LOS[4] + vn*SH_LOS[11])) - P[9][9]*SH_LOS[0]*SH_LOS[1]*SH_LOS[7] + P[22][9]*SH_LOS[0]*SH_LOS[1]*SH_LOS[7] + P[4][9]*SH_LOS[0]*SH_LOS[3]*(a2*(2*q0*q2 + 2*q1*q3) + a3*(2*q0*q3 - 2*q1*q2) - sq(q0) - sq(q1) + sq(q2) + sq(q3)) - P[5][9]*SH_LOS[0]*SH_LOS[3]*(a3*(sq(q0) - sq(q1) + sq(q2) - sq(q3)) + 2*q0*q3 + 2*q1*q2 + a2*(2*q0*q1 - 2*q2*q3)) + P[6][9]*SH_LOS[0]*SH_LOS[3]*(a2*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + 2*q0*q2 - 2*q1*q3 - a3*(2*q0*q1 + 2*q2*q3))) + SH_LOS[0]*SH_LOS[1]*SH_LOS[7]*(P[0][22]*(SH_LOS[1]*SH_LOS[3]*(2*a1*q1 - 2*q0 + 2*a2*q2) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[6] - ve*SH_LOS[11] + vn*SH_LOS[5])) + P[1][22]*(SH_LOS[1]*SH_LOS[3]*(2*q1 + 2*a1*q0 - 2*a2*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[11] + ve*SH_LOS[6] + vn*SH_LOS[4])) + P[2][22]*(SH_LOS[1]*SH_LOS[3]*(2*q2 + 2*a2*q0 + 2*a1*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[5] + ve*SH_LOS[4] - vn*SH_LOS[6])) - P[3][22]*(SH_LOS[1]*SH_LOS[3]*(2*q3 - 2*a1*q2 + 2*a2*q1) - SH_LOS[0]*SH_LOS[3]*(ve*SH_LOS[5] - vd*SH_LOS[4] + vn*SH_LOS[11])) - P[9][22]*SH_LOS[0]*SH_LOS[1]*SH_LOS[7] + P[22][22]*SH_LOS[0]*SH_LOS[1]*SH_LOS[7] + P[4][22]*SH_LOS[0]*SH_LOS[3]*(a2*(2*q0*q2 + 2*q1*q3) + a3*(2*q0*q3 - 2*q1*q2) - sq(q0) - sq(q1) + sq(q2) + sq(q3)) - P[5][22]*SH_LOS[0]*SH_LOS[3]*(a3*(sq(q0) - sq(q1) + sq(q2) - sq(q3)) + 2*q0*q3 + 2*q1*q2 + a2*(2*q0*q1 - 2*q2*q3)) + P[6][22]*SH_LOS[0]*SH_LOS[3]*(a2*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + 2*q0*q2 - 2*q1*q3 - a3*(2*q0*q1 + 2*q2*q3))) + SH_LOS[0]*SH_LOS[3]*(a2*(2*q0*q2 + 2*q1*q3) + a3*(2*q0*q3 - 2*q1*q2) - sq(q0) - sq(q1) + sq(q2) + sq(q3))*(P[0][4]*(SH_LOS[1]*SH_LOS[3]*(2*a1*q1 - 2*q0 + 2*a2*q2) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[6] - ve*SH_LOS[11] + vn*SH_LOS[5])) + P[1][4]*(SH_LOS[1]*SH_LOS[3]*(2*q1 + 2*a1*q0 - 2*a2*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[11] + ve*SH_LOS[6] + vn*SH_LOS[4])) + P[2][4]*(SH_LOS[1]*SH_LOS[3]*(2*q2 + 2*a2*q0 + 2*a1*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[5] + ve*SH_LOS[4] - vn*SH_LOS[6])) - P[3][4]*(SH_LOS[1]*SH_LOS[3]*(2*q3 - 2*a1*q2 + 2*a2*q1) - SH_LOS[0]*SH_LOS[3]*(ve*SH_LOS[5] - vd*SH_LOS[4] + vn*SH_LOS[11])) - P[9][4]*SH_LOS[0]*SH_LOS[1]*SH_LOS[7] + P[22][4]*SH_LOS[0]*SH_LOS[1]*SH_LOS[7] + P[4][4]*SH_LOS[0]*SH_LOS[3]*(a2*(2*q0*q2 + 2*q1*q3) + a3*(2*q0*q3 - 2*q1*q2) - sq(q0) - sq(q1) + sq(q2) + sq(q3)) - P[5][4]*SH_LOS[0]*SH_LOS[3]*(a3*(sq(q0) - sq(q1) + sq(q2) - sq(q3)) + 2*q0*q3 + 2*q1*q2 + a2*(2*q0*q1 - 2*q2*q3)) + P[6][4]*SH_LOS[0]*SH_LOS[3]*(a2*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + 2*q0*q2 - 2*q1*q3 - a3*(2*q0*q1 + 2*q2*q3))) - SH_LOS[0]*SH_LOS[3]*(a3*(sq(q0) - sq(q1) + sq(q2) - sq(q3)) + 2*q0*q3 + 2*q1*q2 + a2*(2*q0*q1 - 2*q2*q3))*(P[0][5]*(SH_LOS[1]*SH_LOS[3]*(2*a1*q1 - 2*q0 + 2*a2*q2) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[6] - ve*SH_LOS[11] + vn*SH_LOS[5])) + P[1][5]*(SH_LOS[1]*SH_LOS[3]*(2*q1 + 2*a1*q0 - 2*a2*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[11] + ve*SH_LOS[6] + vn*SH_LOS[4])) + P[2][5]*(SH_LOS[1]*SH_LOS[3]*(2*q2 + 2*a2*q0 + 2*a1*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[5] + ve*SH_LOS[4] - vn*SH_LOS[6])) - P[3][5]*(SH_LOS[1]*SH_LOS[3]*(2*q3 - 2*a1*q2 + 2*a2*q1) - SH_LOS[0]*SH_LOS[3]*(ve*SH_LOS[5] - vd*SH_LOS[4] + vn*SH_LOS[11])) - P[9][5]*SH_LOS[0]*SH_LOS[1]*SH_LOS[7] + P[22][5]*SH_LOS[0]*SH_LOS[1]*SH_LOS[7] + P[4][5]*SH_LOS[0]*SH_LOS[3]*(a2*(2*q0*q2 + 2*q1*q3) + a3*(2*q0*q3 - 2*q1*q2) - sq(q0) - sq(q1) + sq(q2) + sq(q3)) - P[5][5]*SH_LOS[0]*SH_LOS[3]*(a3*(sq(q0) - sq(q1) + sq(q2) - sq(q3)) + 2*q0*q3 + 2*q1*q2 + a2*(2*q0*q1 - 2*q2*q3)) + P[6][5]*SH_LOS[0]*SH_LOS[3]*(a2*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + 2*q0*q2 - 2*q1*q3 - a3*(2*q0*q1 + 2*q2*q3))) + SH_LOS[0]*SH_LOS[3]*(a2*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + 2*q0*q2 - 2*q1*q3 - a3*(2*q0*q1 + 2*q2*q3))*(P[0][6]*(SH_LOS[1]*SH_LOS[3]*(2*a1*q1 - 2*q0 + 2*a2*q2) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[6] - ve*SH_LOS[11] + vn*SH_LOS[5])) + P[1][6]*(SH_LOS[1]*SH_LOS[3]*(2*q1 + 2*a1*q0 - 2*a2*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[11] + ve*SH_LOS[6] + vn*SH_LOS[4])) + P[2][6]*(SH_LOS[1]*SH_LOS[3]*(2*q2 + 2*a2*q0 + 2*a1*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[5] + ve*SH_LOS[4] - vn*SH_LOS[6])) - P[3][6]*(SH_LOS[1]*SH_LOS[3]*(2*q3 - 2*a1*q2 + 2*a2*q1) - SH_LOS[0]*SH_LOS[3]*(ve*SH_LOS[5] - vd*SH_LOS[4] + vn*SH_LOS[11])) - P[9][6]*SH_LOS[0]*SH_LOS[1]*SH_LOS[7] + P[22][6]*SH_LOS[0]*SH_LOS[1]*SH_LOS[7] + P[4][6]*SH_LOS[0]*SH_LOS[3]*(a2*(2*q0*q2 + 2*q1*q3) + a3*(2*q0*q3 - 2*q1*q2) - sq(q0) - sq(q1) + sq(q2) + sq(q3)) - P[5][6]*SH_LOS[0]*SH_LOS[3]*(a3*(sq(q0) - sq(q1) + sq(q2) - sq(q3)) + 2*q0*q3 + 2*q1*q2 + a2*(2*q0*q1 - 2*q2*q3)) + P[6][6]*SH_LOS[0]*SH_LOS[3]*(a2*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + 2*q0*q2 - 2*q1*q3 - a3*(2*q0*q1 + 2*q2*q3)))); + SK_LOS[2] = a2*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + 2*q0*q2 - 2*q1*q3 - a3*(2*q0*q1 + 2*q2*q3); + SK_LOS[3] = a3*(sq(q0) + sq(q1) - sq(q2) - sq(q3)) + 2*q0*q3 - 2*q1*q2 - a1*(2*q0*q2 + 2*q1*q3); + SK_LOS[4] = a3*(sq(q0) - sq(q1) + sq(q2) - sq(q3)) + 2*q0*q3 + 2*q1*q2 + a2*(2*q0*q1 - 2*q2*q3); + SK_LOS[5] = a2*(2*q0*q2 + 2*q1*q3) + a3*(2*q0*q3 - 2*q1*q2) - sq(q0) - sq(q1) + sq(q2) + sq(q3); + SK_LOS[6] = SH_LOS[1]*SH_LOS[3]*(2*q3 - 2*a1*q2 + 2*a2*q1) - SH_LOS[0]*SH_LOS[3]*(ve*SH_LOS[5] - vd*SH_LOS[4] + vn*SH_LOS[11]); + SK_LOS[7] = a1*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + 2*q0*q1 + 2*q2*q3 + a3*(2*q0*q2 - 2*q1*q3); + SK_LOS[8] = a1*(2*q0*q1 - 2*q2*q3) + a3*(2*q0*q3 + 2*q1*q2) - sq(q0) + sq(q1) - sq(q2) + sq(q3); + SK_LOS[9] = SH_LOS[2]*SH_LOS[3]*(2*q3 - 2*a1*q2 + 2*a2*q1) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[8] - ve*SH_LOS[9] + vn*SH_LOS[10]); + SK_LOS[10] = SH_LOS[2]*SH_LOS[3]*(2*q1 + 2*a1*q0 - 2*a2*q3) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[10] + ve*SH_LOS[12] - vn*SH_LOS[8]); + SK_LOS[11] = SH_LOS[2]*SH_LOS[3]*(2*a1*q1 - 2*q0 + 2*a2*q2) + SH_LOS[0]*SH_LOS[3]*(ve*SH_LOS[10] - vd*SH_LOS[12] + vn*SH_LOS[9]); + SK_LOS[12] = SH_LOS[2]*SH_LOS[3]*(2*q2 + 2*a2*q0 + 2*a1*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[9] + ve*SH_LOS[8] + vn*SH_LOS[12]); + SK_LOS[13] = SH_LOS[1]*SH_LOS[3]*(2*a1*q1 - 2*q0 + 2*a2*q2) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[6] - ve*SH_LOS[11] + vn*SH_LOS[5]); + SK_LOS[14] = SH_LOS[1]*SH_LOS[3]*(2*q1 + 2*a1*q0 - 2*a2*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[11] + ve*SH_LOS[6] + vn*SH_LOS[4]); + SK_LOS[15] = SH_LOS[1]*SH_LOS[3]*(2*q2 + 2*a2*q0 + 2*a1*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[5] + ve*SH_LOS[4] - vn*SH_LOS[6]); + SK_LOS[16] = SH_LOS[0]; + + Kfusion[0] = -SK_LOS[0]*(P[0][0]*SK_LOS[11] + P[0][1]*SK_LOS[10] - P[0][3]*SK_LOS[9] + P[0][2]*SK_LOS[12] - P[0][9]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[0][22]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[0][4]*SH_LOS[3]*SK_LOS[3]*SK_LOS[16] + P[0][5]*SH_LOS[3]*SK_LOS[8]*SK_LOS[16] - P[0][6]*SH_LOS[3]*SK_LOS[7]*SK_LOS[16]); + Kfusion[1] = -SK_LOS[0]*(P[1][0]*SK_LOS[11] + P[1][1]*SK_LOS[10] - P[1][3]*SK_LOS[9] + P[1][2]*SK_LOS[12] - P[1][9]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[1][22]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[1][4]*SH_LOS[3]*SK_LOS[3]*SK_LOS[16] + P[1][5]*SH_LOS[3]*SK_LOS[8]*SK_LOS[16] - P[1][6]*SH_LOS[3]*SK_LOS[7]*SK_LOS[16]); + Kfusion[2] = -SK_LOS[0]*(P[2][0]*SK_LOS[11] + P[2][1]*SK_LOS[10] - P[2][3]*SK_LOS[9] + P[2][2]*SK_LOS[12] - P[2][9]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[2][22]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[2][4]*SH_LOS[3]*SK_LOS[3]*SK_LOS[16] + P[2][5]*SH_LOS[3]*SK_LOS[8]*SK_LOS[16] - P[2][6]*SH_LOS[3]*SK_LOS[7]*SK_LOS[16]); + Kfusion[3] = -SK_LOS[0]*(P[3][0]*SK_LOS[11] + P[3][1]*SK_LOS[10] - P[3][3]*SK_LOS[9] + P[3][2]*SK_LOS[12] - P[3][9]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[3][22]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[3][4]*SH_LOS[3]*SK_LOS[3]*SK_LOS[16] + P[3][5]*SH_LOS[3]*SK_LOS[8]*SK_LOS[16] - P[3][6]*SH_LOS[3]*SK_LOS[7]*SK_LOS[16]); + Kfusion[4] = -SK_LOS[0]*(P[4][0]*SK_LOS[11] + P[4][1]*SK_LOS[10] - P[4][3]*SK_LOS[9] + P[4][2]*SK_LOS[12] - P[4][9]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[4][22]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[4][4]*SH_LOS[3]*SK_LOS[3]*SK_LOS[16] + P[4][5]*SH_LOS[3]*SK_LOS[8]*SK_LOS[16] - P[4][6]*SH_LOS[3]*SK_LOS[7]*SK_LOS[16]); + Kfusion[5] = -SK_LOS[0]*(P[5][0]*SK_LOS[11] + P[5][1]*SK_LOS[10] - P[5][3]*SK_LOS[9] + P[5][2]*SK_LOS[12] - P[5][9]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[5][22]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[5][4]*SH_LOS[3]*SK_LOS[3]*SK_LOS[16] + P[5][5]*SH_LOS[3]*SK_LOS[8]*SK_LOS[16] - P[5][6]*SH_LOS[3]*SK_LOS[7]*SK_LOS[16]); + Kfusion[6] = -SK_LOS[0]*(P[6][0]*SK_LOS[11] + P[6][1]*SK_LOS[10] - P[6][3]*SK_LOS[9] + P[6][2]*SK_LOS[12] - P[6][9]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[6][22]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[6][4]*SH_LOS[3]*SK_LOS[3]*SK_LOS[16] + P[6][5]*SH_LOS[3]*SK_LOS[8]*SK_LOS[16] - P[6][6]*SH_LOS[3]*SK_LOS[7]*SK_LOS[16]); + Kfusion[7] = -SK_LOS[0]*(P[7][0]*SK_LOS[11] + P[7][1]*SK_LOS[10] - P[7][3]*SK_LOS[9] + P[7][2]*SK_LOS[12] - P[7][9]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[7][22]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[7][4]*SH_LOS[3]*SK_LOS[3]*SK_LOS[16] + P[7][5]*SH_LOS[3]*SK_LOS[8]*SK_LOS[16] - P[7][6]*SH_LOS[3]*SK_LOS[7]*SK_LOS[16]); + Kfusion[8] = -SK_LOS[0]*(P[8][0]*SK_LOS[11] + P[8][1]*SK_LOS[10] - P[8][3]*SK_LOS[9] + P[8][2]*SK_LOS[12] - P[8][9]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[8][22]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[8][4]*SH_LOS[3]*SK_LOS[3]*SK_LOS[16] + P[8][5]*SH_LOS[3]*SK_LOS[8]*SK_LOS[16] - P[8][6]*SH_LOS[3]*SK_LOS[7]*SK_LOS[16]); + Kfusion[9] = -SK_LOS[0]*(P[9][0]*SK_LOS[11] + P[9][1]*SK_LOS[10] - P[9][3]*SK_LOS[9] + P[9][2]*SK_LOS[12] - P[9][9]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[9][22]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[9][4]*SH_LOS[3]*SK_LOS[3]*SK_LOS[16] + P[9][5]*SH_LOS[3]*SK_LOS[8]*SK_LOS[16] - P[9][6]*SH_LOS[3]*SK_LOS[7]*SK_LOS[16]); + Kfusion[10] = -SK_LOS[0]*(P[10][0]*SK_LOS[11] + P[10][1]*SK_LOS[10] - P[10][3]*SK_LOS[9] + P[10][2]*SK_LOS[12] - P[10][9]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[10][22]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[10][4]*SH_LOS[3]*SK_LOS[3]*SK_LOS[16] + P[10][5]*SH_LOS[3]*SK_LOS[8]*SK_LOS[16] - P[10][6]*SH_LOS[3]*SK_LOS[7]*SK_LOS[16]); + Kfusion[11] = -SK_LOS[0]*(P[11][0]*SK_LOS[11] + P[11][1]*SK_LOS[10] - P[11][3]*SK_LOS[9] + P[11][2]*SK_LOS[12] - P[11][9]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[11][22]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[11][4]*SH_LOS[3]*SK_LOS[3]*SK_LOS[16] + P[11][5]*SH_LOS[3]*SK_LOS[8]*SK_LOS[16] - P[11][6]*SH_LOS[3]*SK_LOS[7]*SK_LOS[16]); + Kfusion[12] = -SK_LOS[0]*(P[12][0]*SK_LOS[11] + P[12][1]*SK_LOS[10] - P[12][3]*SK_LOS[9] + P[12][2]*SK_LOS[12] - P[12][9]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[12][22]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[12][4]*SH_LOS[3]*SK_LOS[3]*SK_LOS[16] + P[12][5]*SH_LOS[3]*SK_LOS[8]*SK_LOS[16] - P[12][6]*SH_LOS[3]*SK_LOS[7]*SK_LOS[16]); + Kfusion[13] = -SK_LOS[0]*(P[13][0]*SK_LOS[11] + P[13][1]*SK_LOS[10] - P[13][3]*SK_LOS[9] + P[13][2]*SK_LOS[12] - P[13][9]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[13][22]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[13][4]*SH_LOS[3]*SK_LOS[3]*SK_LOS[16] + P[13][5]*SH_LOS[3]*SK_LOS[8]*SK_LOS[16] - P[13][6]*SH_LOS[3]*SK_LOS[7]*SK_LOS[16]); + Kfusion[14] = -SK_LOS[0]*(P[14][0]*SK_LOS[11] + P[14][1]*SK_LOS[10] - P[14][3]*SK_LOS[9] + P[14][2]*SK_LOS[12] - P[14][9]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[14][22]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[14][4]*SH_LOS[3]*SK_LOS[3]*SK_LOS[16] + P[14][5]*SH_LOS[3]*SK_LOS[8]*SK_LOS[16] - P[14][6]*SH_LOS[3]*SK_LOS[7]*SK_LOS[16]); + Kfusion[15] = -SK_LOS[0]*(P[15][0]*SK_LOS[11] + P[15][1]*SK_LOS[10] - P[15][3]*SK_LOS[9] + P[15][2]*SK_LOS[12] - P[15][9]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[15][22]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[15][4]*SH_LOS[3]*SK_LOS[3]*SK_LOS[16] + P[15][5]*SH_LOS[3]*SK_LOS[8]*SK_LOS[16] - P[15][6]*SH_LOS[3]*SK_LOS[7]*SK_LOS[16]); + Kfusion[16] = -SK_LOS[0]*(P[16][0]*SK_LOS[11] + P[16][1]*SK_LOS[10] - P[16][3]*SK_LOS[9] + P[16][2]*SK_LOS[12] - P[16][9]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[16][22]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[16][4]*SH_LOS[3]*SK_LOS[3]*SK_LOS[16] + P[16][5]*SH_LOS[3]*SK_LOS[8]*SK_LOS[16] - P[16][6]*SH_LOS[3]*SK_LOS[7]*SK_LOS[16]); + Kfusion[17] = -SK_LOS[0]*(P[17][0]*SK_LOS[11] + P[17][1]*SK_LOS[10] - P[17][3]*SK_LOS[9] + P[17][2]*SK_LOS[12] - P[17][9]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[17][22]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[17][4]*SH_LOS[3]*SK_LOS[3]*SK_LOS[16] + P[17][5]*SH_LOS[3]*SK_LOS[8]*SK_LOS[16] - P[17][6]*SH_LOS[3]*SK_LOS[7]*SK_LOS[16]); + Kfusion[18] = -SK_LOS[0]*(P[18][0]*SK_LOS[11] + P[18][1]*SK_LOS[10] - P[18][3]*SK_LOS[9] + P[18][2]*SK_LOS[12] - P[18][9]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[18][22]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[18][4]*SH_LOS[3]*SK_LOS[3]*SK_LOS[16] + P[18][5]*SH_LOS[3]*SK_LOS[8]*SK_LOS[16] - P[18][6]*SH_LOS[3]*SK_LOS[7]*SK_LOS[16]); + Kfusion[19] = -SK_LOS[0]*(P[19][0]*SK_LOS[11] + P[19][1]*SK_LOS[10] - P[19][3]*SK_LOS[9] + P[19][2]*SK_LOS[12] - P[19][9]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[19][22]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[19][4]*SH_LOS[3]*SK_LOS[3]*SK_LOS[16] + P[19][5]*SH_LOS[3]*SK_LOS[8]*SK_LOS[16] - P[19][6]*SH_LOS[3]*SK_LOS[7]*SK_LOS[16]); + Kfusion[20] = -SK_LOS[0]*(P[20][0]*SK_LOS[11] + P[20][1]*SK_LOS[10] - P[20][3]*SK_LOS[9] + P[20][2]*SK_LOS[12] - P[20][9]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[20][22]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[20][4]*SH_LOS[3]*SK_LOS[3]*SK_LOS[16] + P[20][5]*SH_LOS[3]*SK_LOS[8]*SK_LOS[16] - P[20][6]*SH_LOS[3]*SK_LOS[7]*SK_LOS[16]); + Kfusion[21] = -SK_LOS[0]*(P[21][0]*SK_LOS[11] + P[21][1]*SK_LOS[10] - P[21][3]*SK_LOS[9] + P[21][2]*SK_LOS[12] - P[21][9]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[21][22]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[21][4]*SH_LOS[3]*SK_LOS[3]*SK_LOS[16] + P[21][5]*SH_LOS[3]*SK_LOS[8]*SK_LOS[16] - P[21][6]*SH_LOS[3]*SK_LOS[7]*SK_LOS[16]); + Kfusion[22] = -SK_LOS[0]*(P[22][0]*SK_LOS[11] + P[22][1]*SK_LOS[10] - P[22][3]*SK_LOS[9] + P[22][2]*SK_LOS[12] - P[22][9]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[22][22]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[22][4]*SH_LOS[3]*SK_LOS[3]*SK_LOS[16] + P[22][5]*SH_LOS[3]*SK_LOS[8]*SK_LOS[16] - P[22][6]*SH_LOS[3]*SK_LOS[7]*SK_LOS[16]); + varInnovLOS[0] = 1.0f/SK_LOS[0]; + innovLOS[0] = losPred[0] - losData[0]; + + // reset the observation index to 0 (we start by fusing the X + // measurement) + obsIndex = 0; + fuseOptData = false; + } + else if (obsIndex == 1) // we are now fusing the Y measurement + { + // Calculate observation jacobians + for (uint8_t i = 0; i < n_states; i++) H_LOS[i] = 0; + H_LOS[0] = SH_LOS[1]*SH_LOS[3]*(2*a1*q1 - 2*q0 + 2*a2*q2) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[6] - ve*SH_LOS[11] + vn*SH_LOS[5]); + H_LOS[1] = SH_LOS[1]*SH_LOS[3]*(2*q1 + 2*a1*q0 - 2*a2*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[11] + ve*SH_LOS[6] + vn*SH_LOS[4]); + H_LOS[2] = SH_LOS[1]*SH_LOS[3]*(2*q2 + 2*a2*q0 + 2*a1*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[5] + ve*SH_LOS[4] - vn*SH_LOS[6]); + H_LOS[3] = SH_LOS[0]*SH_LOS[3]*(ve*SH_LOS[5] - vd*SH_LOS[4] + vn*SH_LOS[11]) - SH_LOS[1]*SH_LOS[3]*(2*q3 - 2*a1*q2 + 2*a2*q1); + H_LOS[4] = SH_LOS[0]*SH_LOS[3]*(a2*(2*q0*q2 + 2*q1*q3) + a3*(2*q0*q3 - 2*q1*q2) - sq(q0) - sq(q1) + sq(q2) + sq(q3)); + H_LOS[5] = -SH_LOS[0]*SH_LOS[3]*(a3*(sq(q0) - sq(q1) + sq(q2) - sq(q3)) + 2*q0*q3 + 2*q1*q2 + a2*(2*q0*q1 - 2*q2*q3)); + H_LOS[6] = SH_LOS[0]*SH_LOS[3]*(a2*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + 2*q0*q2 - 2*q1*q3 - a3*(2*q0*q1 + 2*q2*q3)); + H_LOS[9] = -SH_LOS[0]*SH_LOS[1]*SH_LOS[7]; + H_LOS[22] = SH_LOS[0]*SH_LOS[1]*SH_LOS[7]; + + // Calculate Kalman gains + Kfusion[0] = SK_LOS[1]*(P[0][0]*SK_LOS[13] - P[0][3]*SK_LOS[6] + P[0][1]*SK_LOS[14] + P[0][2]*SK_LOS[15] - P[0][9]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[0][22]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[0][6]*SH_LOS[3]*SK_LOS[2]*SK_LOS[16] + P[0][4]*SH_LOS[3]*SK_LOS[5]*SK_LOS[16] - P[0][5]*SH_LOS[3]*SK_LOS[4]*SK_LOS[16]); + Kfusion[1] = SK_LOS[1]*(P[1][0]*SK_LOS[13] - P[1][3]*SK_LOS[6] + P[1][1]*SK_LOS[14] + P[1][2]*SK_LOS[15] - P[1][9]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[1][22]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[1][6]*SH_LOS[3]*SK_LOS[2]*SK_LOS[16] + P[1][4]*SH_LOS[3]*SK_LOS[5]*SK_LOS[16] - P[1][5]*SH_LOS[3]*SK_LOS[4]*SK_LOS[16]); + Kfusion[2] = SK_LOS[1]*(P[2][0]*SK_LOS[13] - P[2][3]*SK_LOS[6] + P[2][1]*SK_LOS[14] + P[2][2]*SK_LOS[15] - P[2][9]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[2][22]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[2][6]*SH_LOS[3]*SK_LOS[2]*SK_LOS[16] + P[2][4]*SH_LOS[3]*SK_LOS[5]*SK_LOS[16] - P[2][5]*SH_LOS[3]*SK_LOS[4]*SK_LOS[16]); + Kfusion[3] = SK_LOS[1]*(P[3][0]*SK_LOS[13] - P[3][3]*SK_LOS[6] + P[3][1]*SK_LOS[14] + P[3][2]*SK_LOS[15] - P[3][9]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[3][22]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[3][6]*SH_LOS[3]*SK_LOS[2]*SK_LOS[16] + P[3][4]*SH_LOS[3]*SK_LOS[5]*SK_LOS[16] - P[3][5]*SH_LOS[3]*SK_LOS[4]*SK_LOS[16]); + Kfusion[4] = SK_LOS[1]*(P[4][0]*SK_LOS[13] - P[4][3]*SK_LOS[6] + P[4][1]*SK_LOS[14] + P[4][2]*SK_LOS[15] - P[4][9]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[4][22]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[4][6]*SH_LOS[3]*SK_LOS[2]*SK_LOS[16] + P[4][4]*SH_LOS[3]*SK_LOS[5]*SK_LOS[16] - P[4][5]*SH_LOS[3]*SK_LOS[4]*SK_LOS[16]); + Kfusion[5] = SK_LOS[1]*(P[5][0]*SK_LOS[13] - P[5][3]*SK_LOS[6] + P[5][1]*SK_LOS[14] + P[5][2]*SK_LOS[15] - P[5][9]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[5][22]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[5][6]*SH_LOS[3]*SK_LOS[2]*SK_LOS[16] + P[5][4]*SH_LOS[3]*SK_LOS[5]*SK_LOS[16] - P[5][5]*SH_LOS[3]*SK_LOS[4]*SK_LOS[16]); + Kfusion[6] = SK_LOS[1]*(P[6][0]*SK_LOS[13] - P[6][3]*SK_LOS[6] + P[6][1]*SK_LOS[14] + P[6][2]*SK_LOS[15] - P[6][9]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[6][22]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[6][6]*SH_LOS[3]*SK_LOS[2]*SK_LOS[16] + P[6][4]*SH_LOS[3]*SK_LOS[5]*SK_LOS[16] - P[6][5]*SH_LOS[3]*SK_LOS[4]*SK_LOS[16]); + Kfusion[7] = SK_LOS[1]*(P[7][0]*SK_LOS[13] - P[7][3]*SK_LOS[6] + P[7][1]*SK_LOS[14] + P[7][2]*SK_LOS[15] - P[7][9]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[7][22]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[7][6]*SH_LOS[3]*SK_LOS[2]*SK_LOS[16] + P[7][4]*SH_LOS[3]*SK_LOS[5]*SK_LOS[16] - P[7][5]*SH_LOS[3]*SK_LOS[4]*SK_LOS[16]); + Kfusion[8] = SK_LOS[1]*(P[8][0]*SK_LOS[13] - P[8][3]*SK_LOS[6] + P[8][1]*SK_LOS[14] + P[8][2]*SK_LOS[15] - P[8][9]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[8][22]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[8][6]*SH_LOS[3]*SK_LOS[2]*SK_LOS[16] + P[8][4]*SH_LOS[3]*SK_LOS[5]*SK_LOS[16] - P[8][5]*SH_LOS[3]*SK_LOS[4]*SK_LOS[16]); + Kfusion[9] = SK_LOS[1]*(P[9][0]*SK_LOS[13] - P[9][3]*SK_LOS[6] + P[9][1]*SK_LOS[14] + P[9][2]*SK_LOS[15] - P[9][9]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[9][22]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[9][6]*SH_LOS[3]*SK_LOS[2]*SK_LOS[16] + P[9][4]*SH_LOS[3]*SK_LOS[5]*SK_LOS[16] - P[9][5]*SH_LOS[3]*SK_LOS[4]*SK_LOS[16]); + Kfusion[10] = SK_LOS[1]*(P[10][0]*SK_LOS[13] - P[10][3]*SK_LOS[6] + P[10][1]*SK_LOS[14] + P[10][2]*SK_LOS[15] - P[10][9]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[10][22]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[10][6]*SH_LOS[3]*SK_LOS[2]*SK_LOS[16] + P[10][4]*SH_LOS[3]*SK_LOS[5]*SK_LOS[16] - P[10][5]*SH_LOS[3]*SK_LOS[4]*SK_LOS[16]); + Kfusion[11] = SK_LOS[1]*(P[11][0]*SK_LOS[13] - P[11][3]*SK_LOS[6] + P[11][1]*SK_LOS[14] + P[11][2]*SK_LOS[15] - P[11][9]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[11][22]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[11][6]*SH_LOS[3]*SK_LOS[2]*SK_LOS[16] + P[11][4]*SH_LOS[3]*SK_LOS[5]*SK_LOS[16] - P[11][5]*SH_LOS[3]*SK_LOS[4]*SK_LOS[16]); + Kfusion[12] = SK_LOS[1]*(P[12][0]*SK_LOS[13] - P[12][3]*SK_LOS[6] + P[12][1]*SK_LOS[14] + P[12][2]*SK_LOS[15] - P[12][9]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[12][22]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[12][6]*SH_LOS[3]*SK_LOS[2]*SK_LOS[16] + P[12][4]*SH_LOS[3]*SK_LOS[5]*SK_LOS[16] - P[12][5]*SH_LOS[3]*SK_LOS[4]*SK_LOS[16]); + Kfusion[13] = SK_LOS[1]*(P[13][0]*SK_LOS[13] - P[13][3]*SK_LOS[6] + P[13][1]*SK_LOS[14] + P[13][2]*SK_LOS[15] - P[13][9]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[13][22]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[13][6]*SH_LOS[3]*SK_LOS[2]*SK_LOS[16] + P[13][4]*SH_LOS[3]*SK_LOS[5]*SK_LOS[16] - P[13][5]*SH_LOS[3]*SK_LOS[4]*SK_LOS[16]); + Kfusion[14] = SK_LOS[1]*(P[14][0]*SK_LOS[13] - P[14][3]*SK_LOS[6] + P[14][1]*SK_LOS[14] + P[14][2]*SK_LOS[15] - P[14][9]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[14][22]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[14][6]*SH_LOS[3]*SK_LOS[2]*SK_LOS[16] + P[14][4]*SH_LOS[3]*SK_LOS[5]*SK_LOS[16] - P[14][5]*SH_LOS[3]*SK_LOS[4]*SK_LOS[16]); + Kfusion[15] = SK_LOS[1]*(P[15][0]*SK_LOS[13] - P[15][3]*SK_LOS[6] + P[15][1]*SK_LOS[14] + P[15][2]*SK_LOS[15] - P[15][9]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[15][22]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[15][6]*SH_LOS[3]*SK_LOS[2]*SK_LOS[16] + P[15][4]*SH_LOS[3]*SK_LOS[5]*SK_LOS[16] - P[15][5]*SH_LOS[3]*SK_LOS[4]*SK_LOS[16]); + Kfusion[16] = SK_LOS[1]*(P[16][0]*SK_LOS[13] - P[16][3]*SK_LOS[6] + P[16][1]*SK_LOS[14] + P[16][2]*SK_LOS[15] - P[16][9]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[16][22]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[16][6]*SH_LOS[3]*SK_LOS[2]*SK_LOS[16] + P[16][4]*SH_LOS[3]*SK_LOS[5]*SK_LOS[16] - P[16][5]*SH_LOS[3]*SK_LOS[4]*SK_LOS[16]); + Kfusion[17] = SK_LOS[1]*(P[17][0]*SK_LOS[13] - P[17][3]*SK_LOS[6] + P[17][1]*SK_LOS[14] + P[17][2]*SK_LOS[15] - P[17][9]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[17][22]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[17][6]*SH_LOS[3]*SK_LOS[2]*SK_LOS[16] + P[17][4]*SH_LOS[3]*SK_LOS[5]*SK_LOS[16] - P[17][5]*SH_LOS[3]*SK_LOS[4]*SK_LOS[16]); + Kfusion[18] = SK_LOS[1]*(P[18][0]*SK_LOS[13] - P[18][3]*SK_LOS[6] + P[18][1]*SK_LOS[14] + P[18][2]*SK_LOS[15] - P[18][9]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[18][22]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[18][6]*SH_LOS[3]*SK_LOS[2]*SK_LOS[16] + P[18][4]*SH_LOS[3]*SK_LOS[5]*SK_LOS[16] - P[18][5]*SH_LOS[3]*SK_LOS[4]*SK_LOS[16]); + Kfusion[19] = SK_LOS[1]*(P[19][0]*SK_LOS[13] - P[19][3]*SK_LOS[6] + P[19][1]*SK_LOS[14] + P[19][2]*SK_LOS[15] - P[19][9]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[19][22]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[19][6]*SH_LOS[3]*SK_LOS[2]*SK_LOS[16] + P[19][4]*SH_LOS[3]*SK_LOS[5]*SK_LOS[16] - P[19][5]*SH_LOS[3]*SK_LOS[4]*SK_LOS[16]); + Kfusion[20] = SK_LOS[1]*(P[20][0]*SK_LOS[13] - P[20][3]*SK_LOS[6] + P[20][1]*SK_LOS[14] + P[20][2]*SK_LOS[15] - P[20][9]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[20][22]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[20][6]*SH_LOS[3]*SK_LOS[2]*SK_LOS[16] + P[20][4]*SH_LOS[3]*SK_LOS[5]*SK_LOS[16] - P[20][5]*SH_LOS[3]*SK_LOS[4]*SK_LOS[16]); + Kfusion[21] = SK_LOS[1]*(P[21][0]*SK_LOS[13] - P[21][3]*SK_LOS[6] + P[21][1]*SK_LOS[14] + P[21][2]*SK_LOS[15] - P[21][9]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[21][22]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[21][6]*SH_LOS[3]*SK_LOS[2]*SK_LOS[16] + P[21][4]*SH_LOS[3]*SK_LOS[5]*SK_LOS[16] - P[21][5]*SH_LOS[3]*SK_LOS[4]*SK_LOS[16]); + Kfusion[22] = SK_LOS[1]*(P[22][0]*SK_LOS[13] - P[22][3]*SK_LOS[6] + P[22][1]*SK_LOS[14] + P[22][2]*SK_LOS[15] - P[22][9]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[22][22]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[22][6]*SH_LOS[3]*SK_LOS[2]*SK_LOS[16] + P[22][4]*SH_LOS[3]*SK_LOS[5]*SK_LOS[16] - P[22][5]*SH_LOS[3]*SK_LOS[4]*SK_LOS[16]); + varInnovLOS[1] = 1.0f/SK_LOS[1]; + innovLOS[1] = losPred[1] - losData[1]; + } + + // Check the innovation for consistency and don't fuse if > 5Sigma + if ((innovLOS[obsIndex]*innovLOS[obsIndex]/varInnovMag[obsIndex]) < 25.0) + { + // correct the state vector + for (uint8_t j = 0; j < n_states; j++) + { + states[j] = states[j] - Kfusion[j] * innovLOS[obsIndex]; + } + // normalise the quaternion states + float quatMag = sqrt(states[0]*states[0] + states[1]*states[1] + states[2]*states[2] + states[3]*states[3]); + if (quatMag > 1e-12) + { + for (uint8_t j= 0; j<=3; j++) + { + float quatMagInv = 1.0f/quatMag; + states[j] = states[j] * quatMagInv; + } + } + // correct the covariance P = (I - K*H)*P + // take advantage of the empty columns in KH to reduce the + // number of operations + for (uint8_t i = 0; i < n_states; i++) + { + for (uint8_t j = 0; j <= 6; j++) + { + KH[i][j] = Kfusion[i] * H_LOS[j]; + } + for (uint8_t j = 7; j <= 8; j++) + { + KH[i][j] = 0.0f; + } + KH[i][9] = Kfusion[i] * H_LOS[9]; + for (uint8_t j = 10; j <= 21; j++) + { + KH[i][j] = 0.0f; + } + KH[i][22] = Kfusion[i] * H_LOS[22]; + } + for (uint8_t i = 0; i < n_states; i++) + { + for (uint8_t j = 0; j < n_states; j++) + { + KHP[i][j] = 0.0f; + for (uint8_t k = 0; k <= 6; k++) + { + KHP[i][j] = KHP[i][j] + KH[i][k] * P[k][j]; + } + KHP[i][j] = KHP[i][j] + KH[i][9] * P[9][j]; + KHP[i][j] = KHP[i][j] + KH[i][22] * P[2][j]; + } + } + } + for (uint8_t i = 0; i < n_states; i++) + { + for (uint8_t j = 0; j < n_states; j++) + { + P[i][j] = P[i][j] - KHP[i][j]; + } + } + } + obsIndex = obsIndex + 1; + + ForceSymmetry(); + ConstrainVariances(); +} + void AttPosEKF::zeroRows(float (&covMat)[n_states][n_states], uint8_t first, uint8_t last) { uint8_t row; @@ -1619,51 +2085,54 @@ void AttPosEKF::ResetStoredStates() } // Output the state vector stored at the time that best matches that specified by msec -int AttPosEKF::RecallStates(float statesForFusion[n_states], uint64_t msec) +int AttPosEKF::RecallStates(float* statesForFusion, uint64_t msec) { int ret = 0; - // int64_t bestTimeDelta = 200; - // unsigned bestStoreIndex = 0; - // for (unsigned storeIndex = 0; storeIndex < data_buffer_size; storeIndex++) - // { - // // The time delta can also end up as negative number, - // // since we might compare future to past or past to future - // // therefore cast to int64. - // int64_t timeDelta = (int64_t)msec - (int64_t)statetimeStamp[storeIndex]; - // if (timeDelta < 0) { - // timeDelta = -timeDelta; - // } + int64_t bestTimeDelta = 200; + unsigned bestStoreIndex = 0; + for (unsigned storeIndex = 0; storeIndex < data_buffer_size; storeIndex++) + { + // Work around a GCC compiler bug - we know 64bit support on ARM is + // sketchy in GCC. + uint64_t timeDelta; - // if (timeDelta < bestTimeDelta) - // { - // bestStoreIndex = storeIndex; - // bestTimeDelta = timeDelta; - // } - // } - // if (bestTimeDelta < 200) // only output stored state if < 200 msec retrieval error - // { - // for (uint8_t i=0; i < n_states; i++) { - // if (isfinite(storedStates[i][bestStoreIndex])) { - // statesForFusion[i] = storedStates[i][bestStoreIndex]; - // } else if (isfinite(states[i])) { - // statesForFusion[i] = states[i]; - // } else { - // // There is not much we can do here, except reporting the error we just - // // found. - // ret++; - // } - // } - // else // otherwise output current state - // { - for (uint8_t i=0; i < n_states; i++) { + if (msec > statetimeStamp[storeIndex]) { + timeDelta = msec - statetimeStamp[storeIndex]; + } else { + timeDelta = statetimeStamp[storeIndex] - msec; + } + + if (timeDelta < bestTimeDelta) + { + bestStoreIndex = storeIndex; + bestTimeDelta = timeDelta; + } + } + if (bestTimeDelta < 200) // only output stored state if < 200 msec retrieval error + { + for (unsigned i=0; i < n_states; i++) { + if (isfinite(storedStates[i][bestStoreIndex])) { + statesForFusion[i] = storedStates[i][bestStoreIndex]; + } else if (isfinite(states[i])) { + statesForFusion[i] = states[i]; + } else { + // There is not much we can do here, except reporting the error we just + // found. + ret++; + } + } + } + else // otherwise output current state + { + for (unsigned i = 0; i < n_states; i++) { if (isfinite(states[i])) { statesForFusion[i] = states[i]; } else { ret++; } } - // } + } return ret; } @@ -1746,7 +2215,7 @@ void AttPosEKF::calcvelNED(float (&velNED)[3], float gpsCourse, float gpsGndSpd, velNED[2] = gpsVelD; } -void AttPosEKF::calcposNED(float (&posNED)[3], float lat, float lon, float hgt, float latRef, float lonRef, float hgtRef) +void AttPosEKF::calcposNED(float (&posNED)[3], double lat, double lon, float hgt, double latRef, double lonRef, float hgtRef) { posNED[0] = earthRadius * (lat - latRef); posNED[1] = earthRadius * cos(latRef) * (lon - lonRef); @@ -1783,23 +2252,25 @@ void AttPosEKF::CovarianceInit() P[1][1] = 0.25f * sq(1.0f*deg2rad); P[2][2] = 0.25f * sq(1.0f*deg2rad); P[3][3] = 0.25f * sq(10.0f*deg2rad); - P[4][4] = sq(0.7); + P[4][4] = sq(0.7f); P[5][5] = P[4][4]; - P[6][6] = sq(0.7); - P[7][7] = sq(15.0); + P[6][6] = sq(0.7f); + P[7][7] = sq(15.0f); P[8][8] = P[7][7]; - P[9][9] = sq(5.0); - P[10][10] = sq(0.1*deg2rad*dtIMU); + P[9][9] = sq(5.0f); + P[10][10] = sq(0.1f*deg2rad*dtIMU); P[11][11] = P[10][10]; P[12][12] = P[10][10]; - P[13][13] = sq(8.0f); - P[14][4] = P[13][13]; - P[15][15] = sq(0.02f); - P[16][16] = P[15][15]; - P[17][17] = P[15][15]; - P[18][18] = sq(0.02f); - P[19][19] = P[18][18]; - P[20][20] = P[18][18]; + P[13][13] = sq(0.2f*dtIMU); + P[14][14] = sq(8.0f); + P[15][14] = P[14][14]; + P[16][16] = sq(0.02f); + P[17][17] = P[16][16]; + P[18][18] = P[16][16]; + P[19][19] = sq(0.02f); + P[20][20] = P[19][19]; + P[21][21] = P[19][19]; + P[22][22] = sq(0.5f); } float AttPosEKF::ConstrainFloat(float val, float min, float max) @@ -1818,45 +2289,52 @@ void AttPosEKF::ConstrainVariances() // 4-6: Velocity - m/sec (North, East, Down) // 7-9: Position - m (North, East, Down) // 10-12: Delta Angle bias - rad (X,Y,Z) - // 13-14: Wind Vector - m/sec (North,East) - // 15-17: Earth Magnetic Field Vector - gauss (North, East, Down) - // 18-20: Body Magnetic Field Vector - gauss (X,Y,Z) + // 13: Delta Velocity bias - m/s (Z) + // 14-15: Wind Vector - m/sec (North,East) + // 16-18: Earth Magnetic Field Vector - gauss (North, East, Down) + // 19-21: Body Magnetic Field Vector - gauss (X,Y,Z) + // 22: Terrain offset - m // Constrain quaternion variances - for (unsigned i = 0; i < 4; i++) { + for (unsigned i = 0; i <= 3; i++) { P[i][i] = ConstrainFloat(P[i][i], 0.0f, 1.0f); } - // Constrain velocitie variances - for (unsigned i = 4; i < 7; i++) { + // Constrain velocity variances + for (unsigned i = 4; i <= 6; i++) { P[i][i] = ConstrainFloat(P[i][i], 0.0f, 1.0e3f); } // Constrain position variances - for (unsigned i = 7; i < 10; i++) { + for (unsigned i = 7; i <= 9; i++) { P[i][i] = ConstrainFloat(P[i][i], 0.0f, 1.0e6f); } - // Angle bias variances - for (unsigned i = 10; i < 13; i++) { - P[i][i] = ConstrainFloat(P[i][i], 0.0f, sq(0.175f * dtIMU)); + // Constrain delta angle bias variances + for (unsigned i = 10; i <= 12; i++) { + P[i][i] = ConstrainFloat(P[i][i], 0.0f, sq(0.12f * dtIMU)); } + // Constrain delta velocity bias variance + P[13][13] = ConstrainFloat(P[13][13], 0.0f, sq(1.0f * dtIMU)); + // Wind velocity variances - for (unsigned i = 13; i < 15; i++) { + for (unsigned i = 14; i <= 15; i++) { P[i][i] = ConstrainFloat(P[i][i], 0.0f, 1.0e3f); } // Earth magnetic field variances - for (unsigned i = 15; i < 18; i++) { + for (unsigned i = 16; i <= 18; i++) { P[i][i] = ConstrainFloat(P[i][i], 0.0f, 1.0f); } // Body magnetic field variances - for (unsigned i = 18; i < 21; i++) { + for (unsigned i = 19; i <= 21; i++) { P[i][i] = ConstrainFloat(P[i][i], 0.0f, 1.0f); } + // Constrain terrain offset variance + P[22][22] = ConstrainFloat(P[22][22], 0.0f, 10000.0f); } void AttPosEKF::ConstrainStates() @@ -1870,23 +2348,24 @@ void AttPosEKF::ConstrainStates() // 4-6: Velocity - m/sec (North, East, Down) // 7-9: Position - m (North, East, Down) // 10-12: Delta Angle bias - rad (X,Y,Z) - // 13-14: Wind Vector - m/sec (North,East) - // 15-17: Earth Magnetic Field Vector - gauss (North, East, Down) - // 18-20: Body Magnetic Field Vector - gauss (X,Y,Z) - + // 13: Delta Velocity bias - m/s (Z) + // 14-15: Wind Vector - m/sec (North,East) + // 16-18: Earth Magnetic Field Vector - gauss (North, East, Down) + // 19-21: Body Magnetic Field Vector - gauss (X,Y,Z) + // 22: Terrain offset - m // Constrain quaternion - for (unsigned i = 0; i < 4; i++) { + for (unsigned i = 0; i <= 3; i++) { states[i] = ConstrainFloat(states[i], -1.0f, 1.0f); } // Constrain velocities to what GPS can do for us - for (unsigned i = 4; i < 7; i++) { + for (unsigned i = 4; i <= 6; i++) { states[i] = ConstrainFloat(states[i], -5.0e2f, 5.0e2f); } // Constrain position to a reasonable vehicle range (in meters) - for (unsigned i = 7; i < 9; i++) { + for (unsigned i = 7; i <= 8; i++) { states[i] = ConstrainFloat(states[i], -1.0e6f, 1.0e6f); } @@ -1894,26 +2373,32 @@ void AttPosEKF::ConstrainStates() states[9] = ConstrainFloat(states[9], -4.0e4f, 1.0e4f); // Angle bias limit - set to 8 degrees / sec - for (unsigned i = 10; i < 13; i++) { + for (unsigned i = 10; i <= 12; i++) { states[i] = ConstrainFloat(states[i], -0.12f * dtIMU, 0.12f * dtIMU); } + // Constrain delta velocity bias + states[13] = ConstrainFloat(states[13], -1.0f * dtIMU, 1.0f * dtIMU); + // Wind velocity limits - assume 120 m/s max velocity - for (unsigned i = 13; i < 15; i++) { + for (unsigned i = 14; i <= 15; i++) { states[i] = ConstrainFloat(states[i], -120.0f, 120.0f); } // Earth magnetic field limits (in Gauss) - for (unsigned i = 15; i < 18; i++) { + for (unsigned i = 16; i <= 18; i++) { states[i] = ConstrainFloat(states[i], -1.0f, 1.0f); } // Body magnetic field variances (in Gauss). // the max offset should be in this range. - for (unsigned i = 18; i < 21; i++) { + for (unsigned i = 19; i <= 21; i++) { states[i] = ConstrainFloat(states[i], -0.5f, 0.5f); } + // Constrain terrain offset + states[22] = ConstrainFloat(states[22], -1000.0f, 1000.0f); + } void AttPosEKF::ForceSymmetry() @@ -2001,7 +2486,7 @@ void AttPosEKF::ResetVelocity(void) void AttPosEKF::FillErrorReport(struct ekf_status_report *err) { - for (int i = 0; i < n_states; i++) + for (unsigned i = 0; i < n_states; i++) { err->states[i] = states[i]; } @@ -2173,14 +2658,15 @@ void AttPosEKF::InitializeDynamic(float (&initvelNED)[3]) // write to state vector for (uint8_t j=0; j<=3; j++) states[j] = initQuat[j]; // quaternions - for (uint8_t j=0; j<=2; j++) states[j+4] = initvelNED[j]; // velocities - for (uint8_t j=0; j<=7; j++) states[j+7] = 0.0f; // positiions, dAngBias, windVel - states[15] = initMagNED.x; // Magnetic Field North - states[16] = initMagNED.y; // Magnetic Field East - states[17] = initMagNED.z; // Magnetic Field Down - states[18] = magBias.x; // Magnetic Field Bias X - states[19] = magBias.y; // Magnetic Field Bias Y - states[20] = magBias.z; // Magnetic Field Bias Z + for (uint8_t j=4; j<=6; j++) states[j] = initvelNED[j-4]; // velocities + for (uint8_t j=7; j<=15; j++) states[j] = 0.0f; // positions, dAngBias, dVelBias, windVel + states[16] = initMagNED.x; // Magnetic Field North + states[17] = initMagNED.y; // Magnetic Field East + states[18] = initMagNED.z; // Magnetic Field Down + states[19] = magBias.x; // Magnetic Field Bias X + states[20] = magBias.y; // Magnetic Field Bias Y + states[21] = magBias.z; // Magnetic Field Bias Z + states[22] = 0.0f; // terrain height statesInitialised = true; @@ -2239,10 +2725,10 @@ void AttPosEKF::ZeroVariables() void AttPosEKF::GetFilterState(struct ekf_status_report *state) { - memcpy(state, ¤t_ekf_state, sizeof(state)); + memcpy(state, ¤t_ekf_state, sizeof(*state)); } void AttPosEKF::GetLastErrorState(struct ekf_status_report *last_error) { - memcpy(last_error, &last_ekf_error, sizeof(last_error)); + memcpy(last_error, &last_ekf_error, sizeof(*last_error)); } diff --git a/src/modules/ekf_att_pos_estimator/estimator.h b/src/modules/ekf_att_pos_estimator/estimator.h index e62f1a98a5..6effe062d4 100644 --- a/src/modules/ekf_att_pos_estimator/estimator.h +++ b/src/modules/ekf_att_pos_estimator/estimator.h @@ -45,14 +45,9 @@ Vector3f operator*(Vector3f vecIn1, float sclIn1); void swap_var(float &d1, float &d2); -const unsigned int n_states = 21; +const unsigned int n_states = 23; const unsigned int data_buffer_size = 50; -const float covTimeStepMax = 0.07f; // maximum time allowed between covariance predictions -const float covDelAngMax = 0.02f; // maximum delta angle between covariance predictions - -// extern bool staticMode; - enum GPS_FIX { GPS_FIX_NOFIX = 0, GPS_FIX_2D = 2, @@ -82,6 +77,79 @@ public: AttPosEKF(); ~AttPosEKF(); + + + /* ############################################## + * + * M A I N F I L T E R P A R A M E T E R S + * + * ########################################### */ + + /* + * parameters are defined here and initialised in + * the InitialiseParameters() (which is just 20 lines down) + */ + + float covTimeStepMax; // maximum time allowed between covariance predictions + float covDelAngMax; // maximum delta angle between covariance predictions + float rngFinderPitch; // pitch angle of laser range finder in radians. Zero is aligned with the Z body axis. Positive is RH rotation about Y body axis. + float a1; // optical flow sensor misalgnment angle about X axis (rad) + float a2; // optical flow sensor misalgnment angle about Y axis (rad) + float a3; // optical flow sensor misalgnment angle about Z axis (rad) + + float yawVarScale; + float windVelSigma; + float dAngBiasSigma; + float dVelBiasSigma; + float magEarthSigma; + float magBodySigma; + float gndHgtSigma; + + float vneSigma; + float vdSigma; + float posNeSigma; + float posDSigma; + float magMeasurementSigma; + float airspeedMeasurementSigma; + + float gyroProcessNoise; + float accelProcessNoise; + + float EAS2TAS; // ratio f true to equivalent airspeed + + void InitialiseParameters() + { + covTimeStepMax = 0.07f; // maximum time allowed between covariance predictions + covDelAngMax = 0.02f; // maximum delta angle between covariance predictions + rngFinderPitch = 0.0f; // pitch angle of laser range finder in radians. Zero is aligned with the Z body axis. Positive is RH rotation about Y body axis. + a1 = 0.0f; // optical flow sensor misalgnment angle about X axis (rad) + a2 = 0.0f; // optical flow sensor misalgnment angle about Y axis (rad) + a3 = 0.0f; // optical flow sensor misalgnment angle about Z axis (rad) + + EAS2TAS = 1.0f; + + yawVarScale = 1.0f; + windVelSigma = 0.1f; + dAngBiasSigma = 5.0e-7f; + dVelBiasSigma = 1e-4f; + magEarthSigma = 3.0e-4f; + magBodySigma = 3.0e-4f; + gndHgtSigma = 0.02f; // assume 2% terrain gradient 1-sigma + + vneSigma = 0.2f; + vdSigma = 0.3f; + posNeSigma = 2.0f; + posDSigma = 2.0f; + + magMeasurementSigma = 0.05; + airspeedMeasurementSigma = 1.4f; + gyroProcessNoise = 1.4544411e-2f; + accelProcessNoise = 0.5f; + } + + + + // Global variables float KH[n_states][n_states]; // intermediate result used for covariance updates float KHP[n_states][n_states]; // intermediate result used for covariance updates @@ -96,6 +164,8 @@ public: float statesAtHgtTime[n_states]; // States at the effective measurement time for the hgtMea measurement float statesAtMagMeasTime[n_states]; // filter satates at the effective measurement time float statesAtVtasMeasTime[n_states]; // filter states at the effective measurement time + float statesAtRngTime[n_states]; // filter states at the effective measurement time + float statesAtLosMeasTime[n_states]; // filter states at the effective measurement time Vector3f correctedDelAng; // delta angles about the xyz body axes corrected for errors (rad) Vector3f correctedDelVel; // delta velocities along the XYZ body axes corrected for errors (m/s) @@ -104,6 +174,10 @@ public: float accNavMag; // magnitude of navigation accel (- used to adjust GPS obs variance (m/s^2) Vector3f earthRateNED; // earths angular rate vector in NED (rad/s) Vector3f angRate; // angular rate vector in XYZ body axes measured by the IMU (rad/s) + + Mat3f Tbn; // transformation matrix from body to NED coordinates + Mat3f Tnb; // transformation amtrix from NED to body coordinates + Vector3f accel; // acceleration vector in XYZ body axes measured by the IMU (m/s^2) Vector3f dVelIMU; Vector3f dAngIMU; @@ -115,26 +189,30 @@ public: float velNED[3]; // North, East, Down velocity obs (m/s) float posNE[2]; // North, East position obs (m) float hgtMea; // measured height (m) + float rngMea; // Ground distance float posNED[3]; // North, East Down position (m) - float innovMag[3]; // innovation output - float varInnovMag[3]; // innovation variance output + float innovMag[3]; // innovation output for magnetometer measurements + float varInnovMag[3]; // innovation variance output for magnetometer measurements + float varInnovLOS[2]; // innovation variance output for optical flow measurements Vector3f magData; // magnetometer flux radings in X,Y,Z body axes - float innovVtas; // innovation output + float innovVtas; // innovation output for true airspeed measurements + float innovRng; ///< Range finder innovation for rnge finder measurements + float innovLOS[2]; // Innovations for optical flow LOS rate measurements + float losData[2]; // Optical flow LOS rate measurements float varInnovVtas; // innovation variance output float VtasMeas; // true airspeed measurement (m/s) - float latRef; // WGS-84 latitude of reference point (rad) - float lonRef; // WGS-84 longitude of reference point (rad) + double latRef; // WGS-84 latitude of reference point (rad) + double lonRef; // WGS-84 longitude of reference point (rad) float hgtRef; // WGS-84 height of reference point (m) Vector3f magBias; // states representing magnetometer bias vector in XYZ body axes - uint8_t covSkipCount; // Number of state prediction frames (IMU daya updates to skip before doing the covariance prediction - float EAS2TAS; // ratio f true to equivalent airspeed + unsigned covSkipCount; // Number of state prediction frames (IMU daya updates to skip before doing the covariance prediction // GPS input data variables float gpsCourse; float gpsVelD; - float gpsLat; - float gpsLon; + double gpsLat; + double gpsLon; float gpsHgt; uint8_t GPSstatus; @@ -148,11 +226,15 @@ public: bool fuseHgtData; // this boolean causes the hgtMea obs to be fused bool fuseMagData; // boolean true when magnetometer data is to be fused bool fuseVtasData; // boolean true when airspeed data is to be fused + bool fuseRngData; ///< true when range data is fused + bool fuseOptData; // true when optical flow data is fused bool onGround; ///< boolean true when the flight vehicle is on the ground (not flying) bool staticMode; ///< boolean true if no position feedback is fused bool useAirspeed; ///< boolean true if airspeed data is being used bool useCompass; ///< boolean true if magnetometer data is being used + bool useRangeFinder; ///< true when rangefinder is being used + bool useOpticalFlow; // true when optical flow data is being used struct ekf_status_report current_ekf_state; struct ekf_status_report last_ekf_error; @@ -172,6 +254,10 @@ void FuseMagnetometer(); void FuseAirspeed(); +void FuseRangeFinder(); + +void FuseOpticalFlow(); + void zeroRows(float (&covMat)[n_states][n_states], uint8_t first, uint8_t last); void zeroCols(float (&covMat)[n_states][n_states], uint8_t first, uint8_t last); @@ -192,7 +278,7 @@ void StoreStates(uint64_t timestamp_ms); * time-wise where valid states were updated and invalid remained at the old * value. */ -int RecallStates(float statesForFusion[n_states], uint64_t msec); +int RecallStates(float *statesForFusion, uint64_t msec); void ResetStoredStates(); @@ -206,7 +292,7 @@ static void quat2eul(float (&eul)[3], const float (&quat)[4]); static void calcvelNED(float (&velNED)[3], float gpsCourse, float gpsGndSpd, float gpsVelD); -static void calcposNED(float (&posNED)[3], float lat, float lon, float hgt, float latRef, float lonRef, float hgtRef); +static void calcposNED(float (&posNED)[3], double lat, double lon, float hgt, double latRef, double lonRef, float hgtRef); static void calcLLH(float (&posNED)[3], float lat, float lon, float hgt, float latRef, float lonRef, float hgtRef); diff --git a/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp index 7857a04693..8715ba49a4 100644 --- a/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp @@ -210,6 +210,7 @@ private: float abias_pnoise; float mage_pnoise; float magb_pnoise; + float eas_noise; } _parameters; /**< local copies of interesting parameters */ struct { @@ -229,6 +230,7 @@ private: param_t abias_pnoise; param_t mage_pnoise; param_t magb_pnoise; + param_t eas_noise; } _parameter_handles; /**< handles for interesting parameters */ AttPosEKF *_ekf; @@ -335,6 +337,7 @@ FixedwingEstimator::FixedwingEstimator() : _parameter_handles.abias_pnoise = param_find("PE_ABIAS_PNOISE"); _parameter_handles.mage_pnoise = param_find("PE_MAGE_PNOISE"); _parameter_handles.magb_pnoise = param_find("PE_MAGB_PNOISE"); + _parameter_handles.eas_noise = param_find("PE_EAS_NOISE"); /* fetch initial parameter values */ parameters_update(); @@ -410,6 +413,25 @@ FixedwingEstimator::parameters_update() param_get(_parameter_handles.abias_pnoise, &(_parameters.abias_pnoise)); param_get(_parameter_handles.mage_pnoise, &(_parameters.mage_pnoise)); param_get(_parameter_handles.magb_pnoise, &(_parameters.magb_pnoise)); + param_get(_parameter_handles.eas_noise, &(_parameters.eas_noise)); + + if (_ekf) { + // _ekf->yawVarScale = 1.0f; + // _ekf->windVelSigma = 0.1f; + _ekf->dAngBiasSigma = _parameters.gbias_pnoise; + _ekf->dVelBiasSigma = _parameters.abias_pnoise; + _ekf->magEarthSigma = _parameters.mage_pnoise; + _ekf->magBodySigma = _parameters.magb_pnoise; + // _ekf->gndHgtSigma = 0.02f; + _ekf->vneSigma = _parameters.velne_noise; + _ekf->vdSigma = _parameters.veld_noise; + _ekf->posNeSigma = _parameters.posne_noise; + _ekf->posDSigma = _parameters.posd_noise; + _ekf->magMeasurementSigma = _parameters.mag_noise; + _ekf->gyroProcessNoise = _parameters.gyro_pnoise; + _ekf->accelProcessNoise = _parameters.acc_pnoise; + _ekf->airspeedMeasurementSigma = _parameters.eas_noise; + } return OK; } @@ -473,6 +495,7 @@ FixedwingEstimator::task_main() orb_set_interval(_sensor_combined_sub, 4); #endif + /* sets also parameters in the EKF object */ parameters_update(); /* set initial filter state */ @@ -914,7 +937,7 @@ FixedwingEstimator::task_main() // perform a covariance prediction if the total delta angle has exceeded the limit // or the time limit will be exceeded at the next IMU update - if ((dt >= (covTimeStepMax - _ekf->dtIMU)) || (_ekf->summedDelAng.length() > covDelAngMax)) { + if ((dt >= (_ekf->covTimeStepMax - _ekf->dtIMU)) || (_ekf->summedDelAng.length() > _ekf->covDelAngMax)) { _ekf->CovariancePrediction(dt); _ekf->summedDelAng = _ekf->summedDelAng.zero(); _ekf->summedDelVel = _ekf->summedDelVel.zero(); diff --git a/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_params.c b/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_params.c index 9d01a095c8..cfcd99858e 100644 --- a/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_params.c +++ b/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_params.c @@ -116,7 +116,19 @@ PARAM_DEFINE_INT32(PE_TAS_DELAY_MS, 210); PARAM_DEFINE_FLOAT(PE_GPS_ALT_WGT, 0.9f); /** - * Velocity noise in north-east (horizontal) direction. + * Airspeed measurement noise. + * + * Increasing this value will make the filter trust this sensor + * less and trust other sensors more. + * + * @min 0.5 + * @max 5.0 + * @group Position Estimator + */ +PARAM_DEFINE_FLOAT(PE_EAS_NOISE, 1.4f); + +/** + * Velocity measurement noise in north-east (horizontal) direction. * * Generic default: 0.3, multicopters: 0.5, ground vehicles: 0.5 * From 46a796fb86986c5172ab4d85d1902e7648afd651 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 20 Apr 2014 03:04:56 +0200 Subject: [PATCH 035/329] Added home position switch on GPS position - gives a more reliable home position setup --- src/modules/commander/commander.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index ce6de88ef9..7ac7aff0f2 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1092,9 +1092,9 @@ int commander_thread_main(int argc, char *argv[]) && global_position.global_valid) { /* copy position data to uORB home message, store it locally as well */ - home.lat = global_position.lat; - home.lon = global_position.lon; - home.alt = global_position.alt; + home.lat = gps_position.lat / (double)1e7; + home.lon = gps_position.lon / (double)1e7; + home.alt = gps_position.alt; warnx("home: lat = %.7f, lon = %.7f, alt = %.4f ", home.lat, home.lon, (double)home.alt); mavlink_log_info(mavlink_fd, "[cmd] home: %.7f, %.7f, %.4f", home.lat, home.lon, (double)home.alt); From 7cad27a0243a806aa374ffda4ef9e99a854e1c16 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 20 Apr 2014 03:06:13 +0200 Subject: [PATCH 036/329] Changed home position set to depend on the commander home position switch --- .../fw_att_pos_estimator_main.cpp | 22 ++++++++++++++----- 1 file changed, 16 insertions(+), 6 deletions(-) diff --git a/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp index 19333accd1..8f39a6330a 100644 --- a/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp @@ -73,6 +73,7 @@ #include #include #include +#include #include #include #include @@ -152,6 +153,7 @@ private: int _params_sub; /**< notification of parameter updates */ int _manual_control_sub; /**< notification of manual control updates */ int _mission_sub; + int _home_sub; /**< home position as defined by commander / user */ orb_advert_t _att_pub; /**< vehicle attitude */ orb_advert_t _global_pos_pub; /**< global position */ @@ -476,6 +478,7 @@ FixedwingEstimator::task_main() _gps_sub = orb_subscribe(ORB_ID(vehicle_gps_position)); _vstatus_sub = orb_subscribe(ORB_ID(vehicle_status)); _params_sub = orb_subscribe(ORB_ID(parameter_update)); + _home_sub = orb_subscribe(ORB_ID(home_position)); /* rate limit vehicle status updates to 5Hz */ orb_set_interval(_vstatus_sub, 200); @@ -864,20 +867,27 @@ FixedwingEstimator::task_main() if (hrt_elapsed_time(&start_time) > 100000) { - if (!_gps_initialized && (_ekf->GPSstatus == 3)) { + bool home_set; + orb_check(_home_sub, &home_set); + + if (home_set && !_gps_initialized && _gps.fix_type > 2) { _ekf->velNED[0] = _gps.vel_n_m_s; _ekf->velNED[1] = _gps.vel_e_m_s; _ekf->velNED[2] = _gps.vel_d_m_s; - double lat = _gps.lat * 1e-7; - double lon = _gps.lon * 1e-7; - float alt = _gps.alt * 1e-3; + struct home_position_s home; + + orb_copy(ORB_ID(home_position), _home_sub, &home); + + double lat = home.lat; + double lon = home.lon; + float alt = home.alt; _ekf->InitialiseFilter(_ekf->velNED); // Initialize projection - _local_pos.ref_lat = _gps.lat; - _local_pos.ref_lon = _gps.lon; + _local_pos.ref_lat = home.lat * 1e7; + _local_pos.ref_lon = home.lon * 1e7; _local_pos.ref_alt = alt; _local_pos.ref_timestamp = _gps.timestamp_position; From 9c5dbeef3a8c9024d710e95dcce3d877ba357656 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 20 Apr 2014 03:39:43 +0200 Subject: [PATCH 037/329] Proper zero init of the filter --- src/modules/ekf_att_pos_estimator/estimator.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/modules/ekf_att_pos_estimator/estimator.cpp b/src/modules/ekf_att_pos_estimator/estimator.cpp index ebe30cae0e..94fb31457d 100644 --- a/src/modules/ekf_att_pos_estimator/estimator.cpp +++ b/src/modules/ekf_att_pos_estimator/estimator.cpp @@ -2711,6 +2711,8 @@ void AttPosEKF::ZeroVariables() states[i] = 0.0f; // state matrix } + correctedDelAng.zero(); + for (unsigned i = 0; i < data_buffer_size; i++) { for (unsigned j = 0; j < n_states; j++) { From b37d0f8f2e3cb0962155113b07a01830c189d4ce Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 20 Apr 2014 03:41:34 +0200 Subject: [PATCH 038/329] Safety checks, prepared to use GPS variance --- .../fw_att_pos_estimator_main.cpp | 47 +++++++++++++++---- 1 file changed, 38 insertions(+), 9 deletions(-) diff --git a/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp index 19333accd1..9d00f1d17b 100644 --- a/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp @@ -319,7 +319,7 @@ FixedwingEstimator::FixedwingEstimator() : _ekf(nullptr) { - _mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); + last_run = hrt_absolute_time(); _parameter_handles.vel_delay_ms = param_find("PE_VEL_DELAY_MS"); _parameter_handles.pos_delay_ms = param_find("PE_POS_DELAY_MS"); @@ -461,6 +461,7 @@ float dt = 0.0f; // time lapsed since last covariance prediction void FixedwingEstimator::task_main() { + _mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); _ekf = new AttPosEKF(); @@ -600,17 +601,23 @@ FixedwingEstimator::task_main() last_run = _gyro.timestamp; /* guard against too large deltaT's */ - if (deltaT > 1.0f) + if (!isfinite(deltaT) || deltaT > 1.0f || deltaT < 0.000001f) { deltaT = 0.01f; + warnx("TS fail"); + } // Always store data, independent of init status /* fill in last data set */ _ekf->dtIMU = deltaT; - _ekf->angRate.x = _gyro.x; - _ekf->angRate.y = _gyro.y; - _ekf->angRate.z = _gyro.z; + if (isfinite(_gyro.x) && + isfinite(_gyro.y) && + isfinite(_gyro.z)) { + _ekf->angRate.x = _gyro.x; + _ekf->angRate.y = _gyro.y; + _ekf->angRate.z = _gyro.z; + } _ekf->accel.x = _accel.x; _ekf->accel.y = _accel.y; @@ -643,16 +650,22 @@ FixedwingEstimator::task_main() last_run = _sensor_combined.timestamp; /* guard against too large deltaT's */ - if (deltaT > 1.0f || deltaT < 0.000001f) + if (!isfinite(deltaT) || deltaT > 1.0f || deltaT < 0.000001f) { deltaT = 0.01f; + warnx("TS fail"); + } // Always store data, independent of init status /* fill in last data set */ _ekf->dtIMU = deltaT; - _ekf->angRate.x = _sensor_combined.gyro_rad_s[0]; - _ekf->angRate.y = _sensor_combined.gyro_rad_s[1]; - _ekf->angRate.z = _sensor_combined.gyro_rad_s[2]; + if (isfinite(_sensor_combined.gyro_rad_s[0]) && + isfinite(_sensor_combined.gyro_rad_s[1]) && + isfinite(_sensor_combined.gyro_rad_s[2])) { + _ekf->angRate.x = _sensor_combined.gyro_rad_s[0]; + _ekf->angRate.y = _sensor_combined.gyro_rad_s[1]; + _ekf->angRate.z = _sensor_combined.gyro_rad_s[2]; + } _ekf->accel.x = _sensor_combined.accelerometer_m_s2[0]; _ekf->accel.y = _sensor_combined.accelerometer_m_s2[1]; @@ -725,6 +738,21 @@ FixedwingEstimator::task_main() _ekf->gpsLat = math::radians(_gps.lat / (double)1e7); _ekf->gpsLon = math::radians(_gps.lon / (double)1e7) - M_PI; _ekf->gpsHgt = _gps.alt / 1e3f; + + // if (_gps.s_variance_m_s > 0.25f && _gps.s_variance_m_s < 100.0f * 100.0f) { + // _ekf->vneSigma = sqrtf(_gps.s_variance_m_s); + // } else { + // _ekf->vneSigma = _parameters.velne_noise; + // } + + // if (_gps.p_variance_m > 0.25f && _gps.p_variance_m < 100.0f * 100.0f) { + // _ekf->posNeSigma = sqrtf(_gps.p_variance_m); + // } else { + // _ekf->posNeSigma = _parameters.posne_noise; + // } + + // warnx("vel: %8.4f pos: %8.4f", _gps.s_variance_m_s, _gps.p_variance_m); + newDataGps = true; } @@ -851,6 +879,7 @@ FixedwingEstimator::task_main() } else { _estimator_status_pub = orb_advertise(ORB_ID(estimator_status), &rep); } + } From 906abbcbb6215e9ae30c51efe3b813a71a963615 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 20 Apr 2014 21:04:05 +0200 Subject: [PATCH 039/329] mavlink: Only write to TX buf if space is available. This is working around a NuttX issue where overflowing the TX buf leads to being unable to send any further data --- src/modules/mavlink/mavlink_main.cpp | 13 +++++++++++-- 1 file changed, 11 insertions(+), 2 deletions(-) diff --git a/src/modules/mavlink/mavlink_main.cpp b/src/modules/mavlink/mavlink_main.cpp index a9f5f4de76..2d71bdce60 100644 --- a/src/modules/mavlink/mavlink_main.cpp +++ b/src/modules/mavlink/mavlink_main.cpp @@ -189,9 +189,18 @@ mavlink_send_uart_bytes(mavlink_channel_t channel, const uint8_t *ch, int length /* If the wait until transmit flag is on, only transmit after we've received messages. Otherwise, transmit all the time. */ if (instance->should_transmit()) { - ssize_t ret = write(uart, ch, desired); + + /* check if there is space in the buffer, let it overflow else */ + if (!ioctl(uart, FIONWRITE, (unsigned long)&buf_free)) { + + if (desired > buf_free) { + desired = buf_free; + } + } + + ssize_t ret = write(uart, ch, desired); if (ret != desired) { - // XXX do something here, but change to using FIONWRITE and OS buf size for detection + warnx("TX FAIL"); } } From 1e202a228437c1fa8a04185a5476dcb3d4308759 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 21 Apr 2014 01:18:13 +0200 Subject: [PATCH 040/329] Updated estimator, not using optical flow for now until proven on the bench --- .../ekf_att_pos_estimator/estimator.cpp | 334 ++---------------- src/modules/ekf_att_pos_estimator/estimator.h | 23 +- 2 files changed, 26 insertions(+), 331 deletions(-) diff --git a/src/modules/ekf_att_pos_estimator/estimator.cpp b/src/modules/ekf_att_pos_estimator/estimator.cpp index 94fb31457d..2a7dab890b 100644 --- a/src/modules/ekf_att_pos_estimator/estimator.cpp +++ b/src/modules/ekf_att_pos_estimator/estimator.cpp @@ -7,13 +7,11 @@ float Vector3f::length(void) const return sqrt(x*x + y*y + z*z); } -Vector3f Vector3f::zero(void) const +void Vector3f::zero(void) { - Vector3f ret = *this; - ret.x = 0.0; - ret.y = 0.0; - ret.z = 0.0; - return ret; + x = 0.0f; + y = 0.0f; + z = 0.0f; } Mat3f::Mat3f() { @@ -69,25 +67,6 @@ Vector3f operator*( Mat3f matIn, Vector3f vecIn) return vecOut; } -// overload * operator to provide a matrix product -Mat3f operator*( Mat3f matIn1, Mat3f matIn2) -{ - Mat3f matOut; - matOut.x.x = matIn1.x.x*matIn2.x.x + matIn1.x.y*matIn2.y.x + matIn1.x.z*matIn2.z.x; - matOut.x.y = matIn1.x.x*matIn2.x.y + matIn1.x.y*matIn2.y.y + matIn1.x.z*matIn2.z.y; - matOut.x.z = matIn1.x.x*matIn2.x.z + matIn1.x.y*matIn2.y.z + matIn1.x.z*matIn2.z.z; - - matOut.y.x = matIn1.y.x*matIn2.x.x + matIn1.y.y*matIn2.y.x + matIn1.y.z*matIn2.z.x; - matOut.y.y = matIn1.y.x*matIn2.x.y + matIn1.y.y*matIn2.y.y + matIn1.y.z*matIn2.z.y; - matOut.y.z = matIn1.y.x*matIn2.x.z + matIn1.y.y*matIn2.y.z + matIn1.y.z*matIn2.z.z; - - matOut.z.x = matIn1.z.x*matIn2.x.x + matIn1.z.y*matIn2.y.x + matIn1.z.z*matIn2.z.x; - matOut.z.y = matIn1.z.x*matIn2.x.y + matIn1.z.y*matIn2.y.y + matIn1.z.z*matIn2.z.y; - matOut.z.z = matIn1.z.x*matIn2.x.z + matIn1.z.y*matIn2.y.z + matIn1.z.z*matIn2.z.z; - - return matOut; -} - // overload % operator to provide a vector cross product Vector3f operator%( Vector3f vecIn1, Vector3f vecIn2) { @@ -1685,6 +1664,19 @@ void AttPosEKF::FuseAirspeed() ConstrainVariances(); } +void AttPosEKF::zeroRows(float (&covMat)[n_states][n_states], uint8_t first, uint8_t last) +{ + uint8_t row; + uint8_t col; + for (row=first; row<=last; row++) + { + for (col=0; col 0.866f) - { - // Sequential fusion of XY components to spread processing load across - // three prediction time steps. - - // Calculate observation jacobians and Kalman gains - if (fuseOptData) - { - // Copy required states to local variable names - q0 = statesAtLosMeasTime[0]; - q1 = statesAtLosMeasTime[1]; - q2 = statesAtLosMeasTime[2]; - q3 = statesAtLosMeasTime[3]; - vn = statesAtLosMeasTime[4]; - ve = statesAtLosMeasTime[5]; - vd = statesAtLosMeasTime[6]; - pd = statesAtLosMeasTime[9]; - ptd = statesAtLosMeasTime[2]; - velNED.x = vn; - velNED.y = ve; - velNED.z = vd; - - // rotate predicted earth components into body axes and calculate - // predicted measurments - float q01 = q0 * q1; - float q02 = q0 * q2; - float q03 = q0 * q3; - float q12 = q1 * q2; - float q13 = q1 * q3; - float q23 = q2 * q3; - DCM.x.x = q0 + q1 - q2 - q3; - DCM.y.y = q0 - q1 + q2 - q3; - DCM.z.z = q0 - q1 - q2 + q3; - DCM.x.y = 2*(q12 - q03); - DCM.x.z = 2*(q13 + q02); - DCM.y.x = 2*(q12 + q03); - DCM.y.z = 2*(q23 - q01); - DCM.z.x = 2*(q13 - q02); - DCM.z.y = 2*(q23 + q01); - // calculate transformation from NED to sensor axes - Tns = DCM*Tsb; - Tns = Tns.transpose(); - // calculate range from ground plain to centre of sensor fov assuming flat earth - float range = ((ptd - pd)/Tns.z.z); - // calculate relative velocity in sensor frame - relVelSensor = Tns*velNED; - // divide by range to get predicted angular LOS rates relative to X and Y axes - losPred[0] = relVelSensor.y/range; - losPred[1] = -relVelSensor.x/range; - - // scale optical flow observation error with total angular rate - R_LOS = 0.01f;// + sq(0.05f*dAngIMU.length()/dtIMU); - - // Calculate observation jacobians - SH_LOS[0] = a1*(2*q0*q1 + 2*q2*q3) + a2*(2*q0*q2 - 2*q1*q3) - sq(q0) + sq(q1) + sq(q2) - sq(q3); - SH_LOS[1] = vd*(a2*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + 2*q0*q2 - 2*q1*q3 - a3*(2*q0*q1 + 2*q2*q3)) - ve*(a3*(sq(q0) - sq(q1) + sq(q2) - sq(q3)) + 2*q0*q3 + 2*q1*q2 + a2*(2*q0*q1 - 2*q2*q3)) + vn*(a2*(2*q0*q2 + 2*q1*q3) + a3*(2*q0*q3 - 2*q1*q2) - sq(q0) - sq(q1) + sq(q2) + sq(q3)); - SH_LOS[2] = ve*(a1*(2*q0*q1 - 2*q2*q3) + a3*(2*q0*q3 + 2*q1*q2) - sq(q0) + sq(q1) - sq(q2) + sq(q3)) - vd*(a1*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + 2*q0*q1 + 2*q2*q3 + a3*(2*q0*q2 - 2*q1*q3)) + vn*(a3*(sq(q0) + sq(q1) - sq(q2) - sq(q3)) + 2*q0*q3 - 2*q1*q2 - a1*(2*q0*q2 + 2*q1*q3)); - SH_LOS[3] = 1/(pd - ptd); - SH_LOS[4] = 2*q1 - 2*a2*q3 + 2*a3*q2; - SH_LOS[5] = 2*a2*q2 - 2*q0 + 2*a3*q3; - SH_LOS[6] = 2*q2 + 2*a2*q0 - 2*a3*q1; - SH_LOS[7] = 1/sq(pd - ptd); - SH_LOS[8] = 2*q2 + 2*a1*q3 - 2*a3*q1; - SH_LOS[9] = 2*q3 - 2*a1*q2 + 2*a3*q0; - SH_LOS[10] = 2*a1*q1 - 2*q0 + 2*a3*q3; - SH_LOS[11] = 2*q3 + 2*a2*q1 + 2*a3*q0; - SH_LOS[12] = 2*q1 + 2*a1*q0 + 2*a3*q2; - - for (uint8_t i = 0; i < n_states; i++) H_LOS[i] = 0; - H_LOS[0] = - SH_LOS[2]*SH_LOS[3]*(2*a1*q1 - 2*q0 + 2*a2*q2) - SH_LOS[0]*SH_LOS[3]*(ve*SH_LOS[10] - vd*SH_LOS[12] + vn*SH_LOS[9]); - H_LOS[1] = - SH_LOS[2]*SH_LOS[3]*(2*q1 + 2*a1*q0 - 2*a2*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[10] + ve*SH_LOS[12] - vn*SH_LOS[8]); - H_LOS[2] = SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[9] + ve*SH_LOS[8] + vn*SH_LOS[12]) - SH_LOS[2]*SH_LOS[3]*(2*q2 + 2*a2*q0 + 2*a1*q3); - H_LOS[3] = SH_LOS[2]*SH_LOS[3]*(2*q3 - 2*a1*q2 + 2*a2*q1) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[8] - ve*SH_LOS[9] + vn*SH_LOS[10]); - H_LOS[4] = -SH_LOS[0]*SH_LOS[3]*(a3*(sq(q0) + sq(q1) - sq(q2) - sq(q3)) + 2*q0*q3 - 2*q1*q2 - a1*(2*q0*q2 + 2*q1*q3)); - H_LOS[5] = -SH_LOS[0]*SH_LOS[3]*(a1*(2*q0*q1 - 2*q2*q3) + a3*(2*q0*q3 + 2*q1*q2) - sq(q0) + sq(q1) - sq(q2) + sq(q3)); - H_LOS[6] = SH_LOS[0]*SH_LOS[3]*(a1*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + 2*q0*q1 + 2*q2*q3 + a3*(2*q0*q2 - 2*q1*q3)); - H_LOS[9] = SH_LOS[0]*SH_LOS[2]*SH_LOS[7]; - H_LOS[22] = -SH_LOS[0]*SH_LOS[2]*SH_LOS[7]; - - // Calculate Kalman gain - SK_LOS[0] = 1/(R_LOS + (SH_LOS[2]*SH_LOS[3]*(2*a1*q1 - 2*q0 + 2*a2*q2) + SH_LOS[0]*SH_LOS[3]*(ve*SH_LOS[10] - vd*SH_LOS[12] + vn*SH_LOS[9]))*(P[0][0]*(SH_LOS[2]*SH_LOS[3]*(2*a1*q1 - 2*q0 + 2*a2*q2) + SH_LOS[0]*SH_LOS[3]*(ve*SH_LOS[10] - vd*SH_LOS[12] + vn*SH_LOS[9])) + P[1][0]*(SH_LOS[2]*SH_LOS[3]*(2*q1 + 2*a1*q0 - 2*a2*q3) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[10] + ve*SH_LOS[12] - vn*SH_LOS[8])) + P[2][0]*(SH_LOS[2]*SH_LOS[3]*(2*q2 + 2*a2*q0 + 2*a1*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[9] + ve*SH_LOS[8] + vn*SH_LOS[12])) - P[3][0]*(SH_LOS[2]*SH_LOS[3]*(2*q3 - 2*a1*q2 + 2*a2*q1) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[8] - ve*SH_LOS[9] + vn*SH_LOS[10])) - P[9][0]*SH_LOS[0]*SH_LOS[2]*SH_LOS[7] + P[22][0]*SH_LOS[0]*SH_LOS[2]*SH_LOS[7] + P[4][0]*SH_LOS[0]*SH_LOS[3]*(a3*(sq(q0) + sq(q1) - sq(q2) - sq(q3)) + 2*q0*q3 - 2*q1*q2 - a1*(2*q0*q2 + 2*q1*q3)) + P[5][0]*SH_LOS[0]*SH_LOS[3]*(a1*(2*q0*q1 - 2*q2*q3) + a3*(2*q0*q3 + 2*q1*q2) - sq(q0) + sq(q1) - sq(q2) + sq(q3)) - P[6][0]*SH_LOS[0]*SH_LOS[3]*(a1*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + 2*q0*q1 + 2*q2*q3 + a3*(2*q0*q2 - 2*q1*q3))) + (SH_LOS[2]*SH_LOS[3]*(2*q1 + 2*a1*q0 - 2*a2*q3) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[10] + ve*SH_LOS[12] - vn*SH_LOS[8]))*(P[0][1]*(SH_LOS[2]*SH_LOS[3]*(2*a1*q1 - 2*q0 + 2*a2*q2) + SH_LOS[0]*SH_LOS[3]*(ve*SH_LOS[10] - vd*SH_LOS[12] + vn*SH_LOS[9])) + P[1][1]*(SH_LOS[2]*SH_LOS[3]*(2*q1 + 2*a1*q0 - 2*a2*q3) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[10] + ve*SH_LOS[12] - vn*SH_LOS[8])) + P[2][1]*(SH_LOS[2]*SH_LOS[3]*(2*q2 + 2*a2*q0 + 2*a1*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[9] + ve*SH_LOS[8] + vn*SH_LOS[12])) - P[3][1]*(SH_LOS[2]*SH_LOS[3]*(2*q3 - 2*a1*q2 + 2*a2*q1) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[8] - ve*SH_LOS[9] + vn*SH_LOS[10])) - P[9][1]*SH_LOS[0]*SH_LOS[2]*SH_LOS[7] + P[22][1]*SH_LOS[0]*SH_LOS[2]*SH_LOS[7] + P[4][1]*SH_LOS[0]*SH_LOS[3]*(a3*(sq(q0) + sq(q1) - sq(q2) - sq(q3)) + 2*q0*q3 - 2*q1*q2 - a1*(2*q0*q2 + 2*q1*q3)) + P[5][1]*SH_LOS[0]*SH_LOS[3]*(a1*(2*q0*q1 - 2*q2*q3) + a3*(2*q0*q3 + 2*q1*q2) - sq(q0) + sq(q1) - sq(q2) + sq(q3)) - P[6][1]*SH_LOS[0]*SH_LOS[3]*(a1*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + 2*q0*q1 + 2*q2*q3 + a3*(2*q0*q2 - 2*q1*q3))) + (SH_LOS[2]*SH_LOS[3]*(2*q2 + 2*a2*q0 + 2*a1*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[9] + ve*SH_LOS[8] + vn*SH_LOS[12]))*(P[0][2]*(SH_LOS[2]*SH_LOS[3]*(2*a1*q1 - 2*q0 + 2*a2*q2) + SH_LOS[0]*SH_LOS[3]*(ve*SH_LOS[10] - vd*SH_LOS[12] + vn*SH_LOS[9])) + P[1][2]*(SH_LOS[2]*SH_LOS[3]*(2*q1 + 2*a1*q0 - 2*a2*q3) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[10] + ve*SH_LOS[12] - vn*SH_LOS[8])) + P[2][2]*(SH_LOS[2]*SH_LOS[3]*(2*q2 + 2*a2*q0 + 2*a1*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[9] + ve*SH_LOS[8] + vn*SH_LOS[12])) - P[3][2]*(SH_LOS[2]*SH_LOS[3]*(2*q3 - 2*a1*q2 + 2*a2*q1) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[8] - ve*SH_LOS[9] + vn*SH_LOS[10])) - P[9][2]*SH_LOS[0]*SH_LOS[2]*SH_LOS[7] + P[22][2]*SH_LOS[0]*SH_LOS[2]*SH_LOS[7] + P[4][2]*SH_LOS[0]*SH_LOS[3]*(a3*(sq(q0) + sq(q1) - sq(q2) - sq(q3)) + 2*q0*q3 - 2*q1*q2 - a1*(2*q0*q2 + 2*q1*q3)) + P[5][2]*SH_LOS[0]*SH_LOS[3]*(a1*(2*q0*q1 - 2*q2*q3) + a3*(2*q0*q3 + 2*q1*q2) - sq(q0) + sq(q1) - sq(q2) + sq(q3)) - P[6][2]*SH_LOS[0]*SH_LOS[3]*(a1*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + 2*q0*q1 + 2*q2*q3 + a3*(2*q0*q2 - 2*q1*q3))) - (SH_LOS[2]*SH_LOS[3]*(2*q3 - 2*a1*q2 + 2*a2*q1) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[8] - ve*SH_LOS[9] + vn*SH_LOS[10]))*(P[0][3]*(SH_LOS[2]*SH_LOS[3]*(2*a1*q1 - 2*q0 + 2*a2*q2) + SH_LOS[0]*SH_LOS[3]*(ve*SH_LOS[10] - vd*SH_LOS[12] + vn*SH_LOS[9])) + P[1][3]*(SH_LOS[2]*SH_LOS[3]*(2*q1 + 2*a1*q0 - 2*a2*q3) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[10] + ve*SH_LOS[12] - vn*SH_LOS[8])) + P[2][3]*(SH_LOS[2]*SH_LOS[3]*(2*q2 + 2*a2*q0 + 2*a1*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[9] + ve*SH_LOS[8] + vn*SH_LOS[12])) - P[3][3]*(SH_LOS[2]*SH_LOS[3]*(2*q3 - 2*a1*q2 + 2*a2*q1) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[8] - ve*SH_LOS[9] + vn*SH_LOS[10])) - P[9][3]*SH_LOS[0]*SH_LOS[2]*SH_LOS[7] + P[22][3]*SH_LOS[0]*SH_LOS[2]*SH_LOS[7] + P[4][3]*SH_LOS[0]*SH_LOS[3]*(a3*(sq(q0) + sq(q1) - sq(q2) - sq(q3)) + 2*q0*q3 - 2*q1*q2 - a1*(2*q0*q2 + 2*q1*q3)) + P[5][3]*SH_LOS[0]*SH_LOS[3]*(a1*(2*q0*q1 - 2*q2*q3) + a3*(2*q0*q3 + 2*q1*q2) - sq(q0) + sq(q1) - sq(q2) + sq(q3)) - P[6][3]*SH_LOS[0]*SH_LOS[3]*(a1*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + 2*q0*q1 + 2*q2*q3 + a3*(2*q0*q2 - 2*q1*q3))) - SH_LOS[0]*SH_LOS[2]*SH_LOS[7]*(P[0][9]*(SH_LOS[2]*SH_LOS[3]*(2*a1*q1 - 2*q0 + 2*a2*q2) + SH_LOS[0]*SH_LOS[3]*(ve*SH_LOS[10] - vd*SH_LOS[12] + vn*SH_LOS[9])) + P[1][9]*(SH_LOS[2]*SH_LOS[3]*(2*q1 + 2*a1*q0 - 2*a2*q3) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[10] + ve*SH_LOS[12] - vn*SH_LOS[8])) + P[2][9]*(SH_LOS[2]*SH_LOS[3]*(2*q2 + 2*a2*q0 + 2*a1*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[9] + ve*SH_LOS[8] + vn*SH_LOS[12])) - P[3][9]*(SH_LOS[2]*SH_LOS[3]*(2*q3 - 2*a1*q2 + 2*a2*q1) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[8] - ve*SH_LOS[9] + vn*SH_LOS[10])) - P[9][9]*SH_LOS[0]*SH_LOS[2]*SH_LOS[7] + P[22][9]*SH_LOS[0]*SH_LOS[2]*SH_LOS[7] + P[4][9]*SH_LOS[0]*SH_LOS[3]*(a3*(sq(q0) + sq(q1) - sq(q2) - sq(q3)) + 2*q0*q3 - 2*q1*q2 - a1*(2*q0*q2 + 2*q1*q3)) + P[5][9]*SH_LOS[0]*SH_LOS[3]*(a1*(2*q0*q1 - 2*q2*q3) + a3*(2*q0*q3 + 2*q1*q2) - sq(q0) + sq(q1) - sq(q2) + sq(q3)) - P[6][9]*SH_LOS[0]*SH_LOS[3]*(a1*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + 2*q0*q1 + 2*q2*q3 + a3*(2*q0*q2 - 2*q1*q3))) + SH_LOS[0]*SH_LOS[2]*SH_LOS[7]*(P[0][22]*(SH_LOS[2]*SH_LOS[3]*(2*a1*q1 - 2*q0 + 2*a2*q2) + SH_LOS[0]*SH_LOS[3]*(ve*SH_LOS[10] - vd*SH_LOS[12] + vn*SH_LOS[9])) + P[1][22]*(SH_LOS[2]*SH_LOS[3]*(2*q1 + 2*a1*q0 - 2*a2*q3) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[10] + ve*SH_LOS[12] - vn*SH_LOS[8])) + P[2][22]*(SH_LOS[2]*SH_LOS[3]*(2*q2 + 2*a2*q0 + 2*a1*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[9] + ve*SH_LOS[8] + vn*SH_LOS[12])) - P[3][22]*(SH_LOS[2]*SH_LOS[3]*(2*q3 - 2*a1*q2 + 2*a2*q1) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[8] - ve*SH_LOS[9] + vn*SH_LOS[10])) - P[9][22]*SH_LOS[0]*SH_LOS[2]*SH_LOS[7] + P[22][22]*SH_LOS[0]*SH_LOS[2]*SH_LOS[7] + P[4][22]*SH_LOS[0]*SH_LOS[3]*(a3*(sq(q0) + sq(q1) - sq(q2) - sq(q3)) + 2*q0*q3 - 2*q1*q2 - a1*(2*q0*q2 + 2*q1*q3)) + P[5][22]*SH_LOS[0]*SH_LOS[3]*(a1*(2*q0*q1 - 2*q2*q3) + a3*(2*q0*q3 + 2*q1*q2) - sq(q0) + sq(q1) - sq(q2) + sq(q3)) - P[6][22]*SH_LOS[0]*SH_LOS[3]*(a1*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + 2*q0*q1 + 2*q2*q3 + a3*(2*q0*q2 - 2*q1*q3))) + SH_LOS[0]*SH_LOS[3]*(a3*(sq(q0) + sq(q1) - sq(q2) - sq(q3)) + 2*q0*q3 - 2*q1*q2 - a1*(2*q0*q2 + 2*q1*q3))*(P[0][4]*(SH_LOS[2]*SH_LOS[3]*(2*a1*q1 - 2*q0 + 2*a2*q2) + SH_LOS[0]*SH_LOS[3]*(ve*SH_LOS[10] - vd*SH_LOS[12] + vn*SH_LOS[9])) + P[1][4]*(SH_LOS[2]*SH_LOS[3]*(2*q1 + 2*a1*q0 - 2*a2*q3) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[10] + ve*SH_LOS[12] - vn*SH_LOS[8])) + P[2][4]*(SH_LOS[2]*SH_LOS[3]*(2*q2 + 2*a2*q0 + 2*a1*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[9] + ve*SH_LOS[8] + vn*SH_LOS[12])) - P[3][4]*(SH_LOS[2]*SH_LOS[3]*(2*q3 - 2*a1*q2 + 2*a2*q1) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[8] - ve*SH_LOS[9] + vn*SH_LOS[10])) - P[9][4]*SH_LOS[0]*SH_LOS[2]*SH_LOS[7] + P[22][4]*SH_LOS[0]*SH_LOS[2]*SH_LOS[7] + P[4][4]*SH_LOS[0]*SH_LOS[3]*(a3*(sq(q0) + sq(q1) - sq(q2) - sq(q3)) + 2*q0*q3 - 2*q1*q2 - a1*(2*q0*q2 + 2*q1*q3)) + P[5][4]*SH_LOS[0]*SH_LOS[3]*(a1*(2*q0*q1 - 2*q2*q3) + a3*(2*q0*q3 + 2*q1*q2) - sq(q0) + sq(q1) - sq(q2) + sq(q3)) - P[6][4]*SH_LOS[0]*SH_LOS[3]*(a1*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + 2*q0*q1 + 2*q2*q3 + a3*(2*q0*q2 - 2*q1*q3))) + SH_LOS[0]*SH_LOS[3]*(a1*(2*q0*q1 - 2*q2*q3) + a3*(2*q0*q3 + 2*q1*q2) - sq(q0) + sq(q1) - sq(q2) + sq(q3))*(P[0][5]*(SH_LOS[2]*SH_LOS[3]*(2*a1*q1 - 2*q0 + 2*a2*q2) + SH_LOS[0]*SH_LOS[3]*(ve*SH_LOS[10] - vd*SH_LOS[12] + vn*SH_LOS[9])) + P[1][5]*(SH_LOS[2]*SH_LOS[3]*(2*q1 + 2*a1*q0 - 2*a2*q3) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[10] + ve*SH_LOS[12] - vn*SH_LOS[8])) + P[2][5]*(SH_LOS[2]*SH_LOS[3]*(2*q2 + 2*a2*q0 + 2*a1*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[9] + ve*SH_LOS[8] + vn*SH_LOS[12])) - P[3][5]*(SH_LOS[2]*SH_LOS[3]*(2*q3 - 2*a1*q2 + 2*a2*q1) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[8] - ve*SH_LOS[9] + vn*SH_LOS[10])) - P[9][5]*SH_LOS[0]*SH_LOS[2]*SH_LOS[7] + P[22][5]*SH_LOS[0]*SH_LOS[2]*SH_LOS[7] + P[4][5]*SH_LOS[0]*SH_LOS[3]*(a3*(sq(q0) + sq(q1) - sq(q2) - sq(q3)) + 2*q0*q3 - 2*q1*q2 - a1*(2*q0*q2 + 2*q1*q3)) + P[5][5]*SH_LOS[0]*SH_LOS[3]*(a1*(2*q0*q1 - 2*q2*q3) + a3*(2*q0*q3 + 2*q1*q2) - sq(q0) + sq(q1) - sq(q2) + sq(q3)) - P[6][5]*SH_LOS[0]*SH_LOS[3]*(a1*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + 2*q0*q1 + 2*q2*q3 + a3*(2*q0*q2 - 2*q1*q3))) - SH_LOS[0]*SH_LOS[3]*(a1*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + 2*q0*q1 + 2*q2*q3 + a3*(2*q0*q2 - 2*q1*q3))*(P[0][6]*(SH_LOS[2]*SH_LOS[3]*(2*a1*q1 - 2*q0 + 2*a2*q2) + SH_LOS[0]*SH_LOS[3]*(ve*SH_LOS[10] - vd*SH_LOS[12] + vn*SH_LOS[9])) + P[1][6]*(SH_LOS[2]*SH_LOS[3]*(2*q1 + 2*a1*q0 - 2*a2*q3) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[10] + ve*SH_LOS[12] - vn*SH_LOS[8])) + P[2][6]*(SH_LOS[2]*SH_LOS[3]*(2*q2 + 2*a2*q0 + 2*a1*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[9] + ve*SH_LOS[8] + vn*SH_LOS[12])) - P[3][6]*(SH_LOS[2]*SH_LOS[3]*(2*q3 - 2*a1*q2 + 2*a2*q1) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[8] - ve*SH_LOS[9] + vn*SH_LOS[10])) - P[9][6]*SH_LOS[0]*SH_LOS[2]*SH_LOS[7] + P[22][6]*SH_LOS[0]*SH_LOS[2]*SH_LOS[7] + P[4][6]*SH_LOS[0]*SH_LOS[3]*(a3*(sq(q0) + sq(q1) - sq(q2) - sq(q3)) + 2*q0*q3 - 2*q1*q2 - a1*(2*q0*q2 + 2*q1*q3)) + P[5][6]*SH_LOS[0]*SH_LOS[3]*(a1*(2*q0*q1 - 2*q2*q3) + a3*(2*q0*q3 + 2*q1*q2) - sq(q0) + sq(q1) - sq(q2) + sq(q3)) - P[6][6]*SH_LOS[0]*SH_LOS[3]*(a1*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + 2*q0*q1 + 2*q2*q3 + a3*(2*q0*q2 - 2*q1*q3)))); - SK_LOS[1] = 1/(R_LOS + (SH_LOS[1]*SH_LOS[3]*(2*a1*q1 - 2*q0 + 2*a2*q2) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[6] - ve*SH_LOS[11] + vn*SH_LOS[5]))*(P[0][0]*(SH_LOS[1]*SH_LOS[3]*(2*a1*q1 - 2*q0 + 2*a2*q2) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[6] - ve*SH_LOS[11] + vn*SH_LOS[5])) + P[1][0]*(SH_LOS[1]*SH_LOS[3]*(2*q1 + 2*a1*q0 - 2*a2*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[11] + ve*SH_LOS[6] + vn*SH_LOS[4])) + P[2][0]*(SH_LOS[1]*SH_LOS[3]*(2*q2 + 2*a2*q0 + 2*a1*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[5] + ve*SH_LOS[4] - vn*SH_LOS[6])) - P[3][0]*(SH_LOS[1]*SH_LOS[3]*(2*q3 - 2*a1*q2 + 2*a2*q1) - SH_LOS[0]*SH_LOS[3]*(ve*SH_LOS[5] - vd*SH_LOS[4] + vn*SH_LOS[11])) - P[9][0]*SH_LOS[0]*SH_LOS[1]*SH_LOS[7] + P[22][0]*SH_LOS[0]*SH_LOS[1]*SH_LOS[7] + P[4][0]*SH_LOS[0]*SH_LOS[3]*(a2*(2*q0*q2 + 2*q1*q3) + a3*(2*q0*q3 - 2*q1*q2) - sq(q0) - sq(q1) + sq(q2) + sq(q3)) - P[5][0]*SH_LOS[0]*SH_LOS[3]*(a3*(sq(q0) - sq(q1) + sq(q2) - sq(q3)) + 2*q0*q3 + 2*q1*q2 + a2*(2*q0*q1 - 2*q2*q3)) + P[6][0]*SH_LOS[0]*SH_LOS[3]*(a2*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + 2*q0*q2 - 2*q1*q3 - a3*(2*q0*q1 + 2*q2*q3))) + (SH_LOS[1]*SH_LOS[3]*(2*q1 + 2*a1*q0 - 2*a2*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[11] + ve*SH_LOS[6] + vn*SH_LOS[4]))*(P[0][1]*(SH_LOS[1]*SH_LOS[3]*(2*a1*q1 - 2*q0 + 2*a2*q2) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[6] - ve*SH_LOS[11] + vn*SH_LOS[5])) + P[1][1]*(SH_LOS[1]*SH_LOS[3]*(2*q1 + 2*a1*q0 - 2*a2*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[11] + ve*SH_LOS[6] + vn*SH_LOS[4])) + P[2][1]*(SH_LOS[1]*SH_LOS[3]*(2*q2 + 2*a2*q0 + 2*a1*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[5] + ve*SH_LOS[4] - vn*SH_LOS[6])) - P[3][1]*(SH_LOS[1]*SH_LOS[3]*(2*q3 - 2*a1*q2 + 2*a2*q1) - SH_LOS[0]*SH_LOS[3]*(ve*SH_LOS[5] - vd*SH_LOS[4] + vn*SH_LOS[11])) - P[9][1]*SH_LOS[0]*SH_LOS[1]*SH_LOS[7] + P[22][1]*SH_LOS[0]*SH_LOS[1]*SH_LOS[7] + P[4][1]*SH_LOS[0]*SH_LOS[3]*(a2*(2*q0*q2 + 2*q1*q3) + a3*(2*q0*q3 - 2*q1*q2) - sq(q0) - sq(q1) + sq(q2) + sq(q3)) - P[5][1]*SH_LOS[0]*SH_LOS[3]*(a3*(sq(q0) - sq(q1) + sq(q2) - sq(q3)) + 2*q0*q3 + 2*q1*q2 + a2*(2*q0*q1 - 2*q2*q3)) + P[6][1]*SH_LOS[0]*SH_LOS[3]*(a2*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + 2*q0*q2 - 2*q1*q3 - a3*(2*q0*q1 + 2*q2*q3))) + (SH_LOS[1]*SH_LOS[3]*(2*q2 + 2*a2*q0 + 2*a1*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[5] + ve*SH_LOS[4] - vn*SH_LOS[6]))*(P[0][2]*(SH_LOS[1]*SH_LOS[3]*(2*a1*q1 - 2*q0 + 2*a2*q2) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[6] - ve*SH_LOS[11] + vn*SH_LOS[5])) + P[1][2]*(SH_LOS[1]*SH_LOS[3]*(2*q1 + 2*a1*q0 - 2*a2*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[11] + ve*SH_LOS[6] + vn*SH_LOS[4])) + P[2][2]*(SH_LOS[1]*SH_LOS[3]*(2*q2 + 2*a2*q0 + 2*a1*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[5] + ve*SH_LOS[4] - vn*SH_LOS[6])) - P[3][2]*(SH_LOS[1]*SH_LOS[3]*(2*q3 - 2*a1*q2 + 2*a2*q1) - SH_LOS[0]*SH_LOS[3]*(ve*SH_LOS[5] - vd*SH_LOS[4] + vn*SH_LOS[11])) - P[9][2]*SH_LOS[0]*SH_LOS[1]*SH_LOS[7] + P[22][2]*SH_LOS[0]*SH_LOS[1]*SH_LOS[7] + P[4][2]*SH_LOS[0]*SH_LOS[3]*(a2*(2*q0*q2 + 2*q1*q3) + a3*(2*q0*q3 - 2*q1*q2) - sq(q0) - sq(q1) + sq(q2) + sq(q3)) - P[5][2]*SH_LOS[0]*SH_LOS[3]*(a3*(sq(q0) - sq(q1) + sq(q2) - sq(q3)) + 2*q0*q3 + 2*q1*q2 + a2*(2*q0*q1 - 2*q2*q3)) + P[6][2]*SH_LOS[0]*SH_LOS[3]*(a2*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + 2*q0*q2 - 2*q1*q3 - a3*(2*q0*q1 + 2*q2*q3))) - (SH_LOS[1]*SH_LOS[3]*(2*q3 - 2*a1*q2 + 2*a2*q1) - SH_LOS[0]*SH_LOS[3]*(ve*SH_LOS[5] - vd*SH_LOS[4] + vn*SH_LOS[11]))*(P[0][3]*(SH_LOS[1]*SH_LOS[3]*(2*a1*q1 - 2*q0 + 2*a2*q2) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[6] - ve*SH_LOS[11] + vn*SH_LOS[5])) + P[1][3]*(SH_LOS[1]*SH_LOS[3]*(2*q1 + 2*a1*q0 - 2*a2*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[11] + ve*SH_LOS[6] + vn*SH_LOS[4])) + P[2][3]*(SH_LOS[1]*SH_LOS[3]*(2*q2 + 2*a2*q0 + 2*a1*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[5] + ve*SH_LOS[4] - vn*SH_LOS[6])) - P[3][3]*(SH_LOS[1]*SH_LOS[3]*(2*q3 - 2*a1*q2 + 2*a2*q1) - SH_LOS[0]*SH_LOS[3]*(ve*SH_LOS[5] - vd*SH_LOS[4] + vn*SH_LOS[11])) - P[9][3]*SH_LOS[0]*SH_LOS[1]*SH_LOS[7] + P[22][3]*SH_LOS[0]*SH_LOS[1]*SH_LOS[7] + P[4][3]*SH_LOS[0]*SH_LOS[3]*(a2*(2*q0*q2 + 2*q1*q3) + a3*(2*q0*q3 - 2*q1*q2) - sq(q0) - sq(q1) + sq(q2) + sq(q3)) - P[5][3]*SH_LOS[0]*SH_LOS[3]*(a3*(sq(q0) - sq(q1) + sq(q2) - sq(q3)) + 2*q0*q3 + 2*q1*q2 + a2*(2*q0*q1 - 2*q2*q3)) + P[6][3]*SH_LOS[0]*SH_LOS[3]*(a2*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + 2*q0*q2 - 2*q1*q3 - a3*(2*q0*q1 + 2*q2*q3))) - SH_LOS[0]*SH_LOS[1]*SH_LOS[7]*(P[0][9]*(SH_LOS[1]*SH_LOS[3]*(2*a1*q1 - 2*q0 + 2*a2*q2) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[6] - ve*SH_LOS[11] + vn*SH_LOS[5])) + P[1][9]*(SH_LOS[1]*SH_LOS[3]*(2*q1 + 2*a1*q0 - 2*a2*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[11] + ve*SH_LOS[6] + vn*SH_LOS[4])) + P[2][9]*(SH_LOS[1]*SH_LOS[3]*(2*q2 + 2*a2*q0 + 2*a1*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[5] + ve*SH_LOS[4] - vn*SH_LOS[6])) - P[3][9]*(SH_LOS[1]*SH_LOS[3]*(2*q3 - 2*a1*q2 + 2*a2*q1) - SH_LOS[0]*SH_LOS[3]*(ve*SH_LOS[5] - vd*SH_LOS[4] + vn*SH_LOS[11])) - P[9][9]*SH_LOS[0]*SH_LOS[1]*SH_LOS[7] + P[22][9]*SH_LOS[0]*SH_LOS[1]*SH_LOS[7] + P[4][9]*SH_LOS[0]*SH_LOS[3]*(a2*(2*q0*q2 + 2*q1*q3) + a3*(2*q0*q3 - 2*q1*q2) - sq(q0) - sq(q1) + sq(q2) + sq(q3)) - P[5][9]*SH_LOS[0]*SH_LOS[3]*(a3*(sq(q0) - sq(q1) + sq(q2) - sq(q3)) + 2*q0*q3 + 2*q1*q2 + a2*(2*q0*q1 - 2*q2*q3)) + P[6][9]*SH_LOS[0]*SH_LOS[3]*(a2*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + 2*q0*q2 - 2*q1*q3 - a3*(2*q0*q1 + 2*q2*q3))) + SH_LOS[0]*SH_LOS[1]*SH_LOS[7]*(P[0][22]*(SH_LOS[1]*SH_LOS[3]*(2*a1*q1 - 2*q0 + 2*a2*q2) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[6] - ve*SH_LOS[11] + vn*SH_LOS[5])) + P[1][22]*(SH_LOS[1]*SH_LOS[3]*(2*q1 + 2*a1*q0 - 2*a2*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[11] + ve*SH_LOS[6] + vn*SH_LOS[4])) + P[2][22]*(SH_LOS[1]*SH_LOS[3]*(2*q2 + 2*a2*q0 + 2*a1*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[5] + ve*SH_LOS[4] - vn*SH_LOS[6])) - P[3][22]*(SH_LOS[1]*SH_LOS[3]*(2*q3 - 2*a1*q2 + 2*a2*q1) - SH_LOS[0]*SH_LOS[3]*(ve*SH_LOS[5] - vd*SH_LOS[4] + vn*SH_LOS[11])) - P[9][22]*SH_LOS[0]*SH_LOS[1]*SH_LOS[7] + P[22][22]*SH_LOS[0]*SH_LOS[1]*SH_LOS[7] + P[4][22]*SH_LOS[0]*SH_LOS[3]*(a2*(2*q0*q2 + 2*q1*q3) + a3*(2*q0*q3 - 2*q1*q2) - sq(q0) - sq(q1) + sq(q2) + sq(q3)) - P[5][22]*SH_LOS[0]*SH_LOS[3]*(a3*(sq(q0) - sq(q1) + sq(q2) - sq(q3)) + 2*q0*q3 + 2*q1*q2 + a2*(2*q0*q1 - 2*q2*q3)) + P[6][22]*SH_LOS[0]*SH_LOS[3]*(a2*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + 2*q0*q2 - 2*q1*q3 - a3*(2*q0*q1 + 2*q2*q3))) + SH_LOS[0]*SH_LOS[3]*(a2*(2*q0*q2 + 2*q1*q3) + a3*(2*q0*q3 - 2*q1*q2) - sq(q0) - sq(q1) + sq(q2) + sq(q3))*(P[0][4]*(SH_LOS[1]*SH_LOS[3]*(2*a1*q1 - 2*q0 + 2*a2*q2) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[6] - ve*SH_LOS[11] + vn*SH_LOS[5])) + P[1][4]*(SH_LOS[1]*SH_LOS[3]*(2*q1 + 2*a1*q0 - 2*a2*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[11] + ve*SH_LOS[6] + vn*SH_LOS[4])) + P[2][4]*(SH_LOS[1]*SH_LOS[3]*(2*q2 + 2*a2*q0 + 2*a1*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[5] + ve*SH_LOS[4] - vn*SH_LOS[6])) - P[3][4]*(SH_LOS[1]*SH_LOS[3]*(2*q3 - 2*a1*q2 + 2*a2*q1) - SH_LOS[0]*SH_LOS[3]*(ve*SH_LOS[5] - vd*SH_LOS[4] + vn*SH_LOS[11])) - P[9][4]*SH_LOS[0]*SH_LOS[1]*SH_LOS[7] + P[22][4]*SH_LOS[0]*SH_LOS[1]*SH_LOS[7] + P[4][4]*SH_LOS[0]*SH_LOS[3]*(a2*(2*q0*q2 + 2*q1*q3) + a3*(2*q0*q3 - 2*q1*q2) - sq(q0) - sq(q1) + sq(q2) + sq(q3)) - P[5][4]*SH_LOS[0]*SH_LOS[3]*(a3*(sq(q0) - sq(q1) + sq(q2) - sq(q3)) + 2*q0*q3 + 2*q1*q2 + a2*(2*q0*q1 - 2*q2*q3)) + P[6][4]*SH_LOS[0]*SH_LOS[3]*(a2*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + 2*q0*q2 - 2*q1*q3 - a3*(2*q0*q1 + 2*q2*q3))) - SH_LOS[0]*SH_LOS[3]*(a3*(sq(q0) - sq(q1) + sq(q2) - sq(q3)) + 2*q0*q3 + 2*q1*q2 + a2*(2*q0*q1 - 2*q2*q3))*(P[0][5]*(SH_LOS[1]*SH_LOS[3]*(2*a1*q1 - 2*q0 + 2*a2*q2) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[6] - ve*SH_LOS[11] + vn*SH_LOS[5])) + P[1][5]*(SH_LOS[1]*SH_LOS[3]*(2*q1 + 2*a1*q0 - 2*a2*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[11] + ve*SH_LOS[6] + vn*SH_LOS[4])) + P[2][5]*(SH_LOS[1]*SH_LOS[3]*(2*q2 + 2*a2*q0 + 2*a1*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[5] + ve*SH_LOS[4] - vn*SH_LOS[6])) - P[3][5]*(SH_LOS[1]*SH_LOS[3]*(2*q3 - 2*a1*q2 + 2*a2*q1) - SH_LOS[0]*SH_LOS[3]*(ve*SH_LOS[5] - vd*SH_LOS[4] + vn*SH_LOS[11])) - P[9][5]*SH_LOS[0]*SH_LOS[1]*SH_LOS[7] + P[22][5]*SH_LOS[0]*SH_LOS[1]*SH_LOS[7] + P[4][5]*SH_LOS[0]*SH_LOS[3]*(a2*(2*q0*q2 + 2*q1*q3) + a3*(2*q0*q3 - 2*q1*q2) - sq(q0) - sq(q1) + sq(q2) + sq(q3)) - P[5][5]*SH_LOS[0]*SH_LOS[3]*(a3*(sq(q0) - sq(q1) + sq(q2) - sq(q3)) + 2*q0*q3 + 2*q1*q2 + a2*(2*q0*q1 - 2*q2*q3)) + P[6][5]*SH_LOS[0]*SH_LOS[3]*(a2*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + 2*q0*q2 - 2*q1*q3 - a3*(2*q0*q1 + 2*q2*q3))) + SH_LOS[0]*SH_LOS[3]*(a2*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + 2*q0*q2 - 2*q1*q3 - a3*(2*q0*q1 + 2*q2*q3))*(P[0][6]*(SH_LOS[1]*SH_LOS[3]*(2*a1*q1 - 2*q0 + 2*a2*q2) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[6] - ve*SH_LOS[11] + vn*SH_LOS[5])) + P[1][6]*(SH_LOS[1]*SH_LOS[3]*(2*q1 + 2*a1*q0 - 2*a2*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[11] + ve*SH_LOS[6] + vn*SH_LOS[4])) + P[2][6]*(SH_LOS[1]*SH_LOS[3]*(2*q2 + 2*a2*q0 + 2*a1*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[5] + ve*SH_LOS[4] - vn*SH_LOS[6])) - P[3][6]*(SH_LOS[1]*SH_LOS[3]*(2*q3 - 2*a1*q2 + 2*a2*q1) - SH_LOS[0]*SH_LOS[3]*(ve*SH_LOS[5] - vd*SH_LOS[4] + vn*SH_LOS[11])) - P[9][6]*SH_LOS[0]*SH_LOS[1]*SH_LOS[7] + P[22][6]*SH_LOS[0]*SH_LOS[1]*SH_LOS[7] + P[4][6]*SH_LOS[0]*SH_LOS[3]*(a2*(2*q0*q2 + 2*q1*q3) + a3*(2*q0*q3 - 2*q1*q2) - sq(q0) - sq(q1) + sq(q2) + sq(q3)) - P[5][6]*SH_LOS[0]*SH_LOS[3]*(a3*(sq(q0) - sq(q1) + sq(q2) - sq(q3)) + 2*q0*q3 + 2*q1*q2 + a2*(2*q0*q1 - 2*q2*q3)) + P[6][6]*SH_LOS[0]*SH_LOS[3]*(a2*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + 2*q0*q2 - 2*q1*q3 - a3*(2*q0*q1 + 2*q2*q3)))); - SK_LOS[2] = a2*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + 2*q0*q2 - 2*q1*q3 - a3*(2*q0*q1 + 2*q2*q3); - SK_LOS[3] = a3*(sq(q0) + sq(q1) - sq(q2) - sq(q3)) + 2*q0*q3 - 2*q1*q2 - a1*(2*q0*q2 + 2*q1*q3); - SK_LOS[4] = a3*(sq(q0) - sq(q1) + sq(q2) - sq(q3)) + 2*q0*q3 + 2*q1*q2 + a2*(2*q0*q1 - 2*q2*q3); - SK_LOS[5] = a2*(2*q0*q2 + 2*q1*q3) + a3*(2*q0*q3 - 2*q1*q2) - sq(q0) - sq(q1) + sq(q2) + sq(q3); - SK_LOS[6] = SH_LOS[1]*SH_LOS[3]*(2*q3 - 2*a1*q2 + 2*a2*q1) - SH_LOS[0]*SH_LOS[3]*(ve*SH_LOS[5] - vd*SH_LOS[4] + vn*SH_LOS[11]); - SK_LOS[7] = a1*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + 2*q0*q1 + 2*q2*q3 + a3*(2*q0*q2 - 2*q1*q3); - SK_LOS[8] = a1*(2*q0*q1 - 2*q2*q3) + a3*(2*q0*q3 + 2*q1*q2) - sq(q0) + sq(q1) - sq(q2) + sq(q3); - SK_LOS[9] = SH_LOS[2]*SH_LOS[3]*(2*q3 - 2*a1*q2 + 2*a2*q1) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[8] - ve*SH_LOS[9] + vn*SH_LOS[10]); - SK_LOS[10] = SH_LOS[2]*SH_LOS[3]*(2*q1 + 2*a1*q0 - 2*a2*q3) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[10] + ve*SH_LOS[12] - vn*SH_LOS[8]); - SK_LOS[11] = SH_LOS[2]*SH_LOS[3]*(2*a1*q1 - 2*q0 + 2*a2*q2) + SH_LOS[0]*SH_LOS[3]*(ve*SH_LOS[10] - vd*SH_LOS[12] + vn*SH_LOS[9]); - SK_LOS[12] = SH_LOS[2]*SH_LOS[3]*(2*q2 + 2*a2*q0 + 2*a1*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[9] + ve*SH_LOS[8] + vn*SH_LOS[12]); - SK_LOS[13] = SH_LOS[1]*SH_LOS[3]*(2*a1*q1 - 2*q0 + 2*a2*q2) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[6] - ve*SH_LOS[11] + vn*SH_LOS[5]); - SK_LOS[14] = SH_LOS[1]*SH_LOS[3]*(2*q1 + 2*a1*q0 - 2*a2*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[11] + ve*SH_LOS[6] + vn*SH_LOS[4]); - SK_LOS[15] = SH_LOS[1]*SH_LOS[3]*(2*q2 + 2*a2*q0 + 2*a1*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[5] + ve*SH_LOS[4] - vn*SH_LOS[6]); - SK_LOS[16] = SH_LOS[0]; - - Kfusion[0] = -SK_LOS[0]*(P[0][0]*SK_LOS[11] + P[0][1]*SK_LOS[10] - P[0][3]*SK_LOS[9] + P[0][2]*SK_LOS[12] - P[0][9]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[0][22]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[0][4]*SH_LOS[3]*SK_LOS[3]*SK_LOS[16] + P[0][5]*SH_LOS[3]*SK_LOS[8]*SK_LOS[16] - P[0][6]*SH_LOS[3]*SK_LOS[7]*SK_LOS[16]); - Kfusion[1] = -SK_LOS[0]*(P[1][0]*SK_LOS[11] + P[1][1]*SK_LOS[10] - P[1][3]*SK_LOS[9] + P[1][2]*SK_LOS[12] - P[1][9]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[1][22]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[1][4]*SH_LOS[3]*SK_LOS[3]*SK_LOS[16] + P[1][5]*SH_LOS[3]*SK_LOS[8]*SK_LOS[16] - P[1][6]*SH_LOS[3]*SK_LOS[7]*SK_LOS[16]); - Kfusion[2] = -SK_LOS[0]*(P[2][0]*SK_LOS[11] + P[2][1]*SK_LOS[10] - P[2][3]*SK_LOS[9] + P[2][2]*SK_LOS[12] - P[2][9]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[2][22]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[2][4]*SH_LOS[3]*SK_LOS[3]*SK_LOS[16] + P[2][5]*SH_LOS[3]*SK_LOS[8]*SK_LOS[16] - P[2][6]*SH_LOS[3]*SK_LOS[7]*SK_LOS[16]); - Kfusion[3] = -SK_LOS[0]*(P[3][0]*SK_LOS[11] + P[3][1]*SK_LOS[10] - P[3][3]*SK_LOS[9] + P[3][2]*SK_LOS[12] - P[3][9]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[3][22]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[3][4]*SH_LOS[3]*SK_LOS[3]*SK_LOS[16] + P[3][5]*SH_LOS[3]*SK_LOS[8]*SK_LOS[16] - P[3][6]*SH_LOS[3]*SK_LOS[7]*SK_LOS[16]); - Kfusion[4] = -SK_LOS[0]*(P[4][0]*SK_LOS[11] + P[4][1]*SK_LOS[10] - P[4][3]*SK_LOS[9] + P[4][2]*SK_LOS[12] - P[4][9]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[4][22]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[4][4]*SH_LOS[3]*SK_LOS[3]*SK_LOS[16] + P[4][5]*SH_LOS[3]*SK_LOS[8]*SK_LOS[16] - P[4][6]*SH_LOS[3]*SK_LOS[7]*SK_LOS[16]); - Kfusion[5] = -SK_LOS[0]*(P[5][0]*SK_LOS[11] + P[5][1]*SK_LOS[10] - P[5][3]*SK_LOS[9] + P[5][2]*SK_LOS[12] - P[5][9]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[5][22]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[5][4]*SH_LOS[3]*SK_LOS[3]*SK_LOS[16] + P[5][5]*SH_LOS[3]*SK_LOS[8]*SK_LOS[16] - P[5][6]*SH_LOS[3]*SK_LOS[7]*SK_LOS[16]); - Kfusion[6] = -SK_LOS[0]*(P[6][0]*SK_LOS[11] + P[6][1]*SK_LOS[10] - P[6][3]*SK_LOS[9] + P[6][2]*SK_LOS[12] - P[6][9]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[6][22]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[6][4]*SH_LOS[3]*SK_LOS[3]*SK_LOS[16] + P[6][5]*SH_LOS[3]*SK_LOS[8]*SK_LOS[16] - P[6][6]*SH_LOS[3]*SK_LOS[7]*SK_LOS[16]); - Kfusion[7] = -SK_LOS[0]*(P[7][0]*SK_LOS[11] + P[7][1]*SK_LOS[10] - P[7][3]*SK_LOS[9] + P[7][2]*SK_LOS[12] - P[7][9]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[7][22]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[7][4]*SH_LOS[3]*SK_LOS[3]*SK_LOS[16] + P[7][5]*SH_LOS[3]*SK_LOS[8]*SK_LOS[16] - P[7][6]*SH_LOS[3]*SK_LOS[7]*SK_LOS[16]); - Kfusion[8] = -SK_LOS[0]*(P[8][0]*SK_LOS[11] + P[8][1]*SK_LOS[10] - P[8][3]*SK_LOS[9] + P[8][2]*SK_LOS[12] - P[8][9]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[8][22]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[8][4]*SH_LOS[3]*SK_LOS[3]*SK_LOS[16] + P[8][5]*SH_LOS[3]*SK_LOS[8]*SK_LOS[16] - P[8][6]*SH_LOS[3]*SK_LOS[7]*SK_LOS[16]); - Kfusion[9] = -SK_LOS[0]*(P[9][0]*SK_LOS[11] + P[9][1]*SK_LOS[10] - P[9][3]*SK_LOS[9] + P[9][2]*SK_LOS[12] - P[9][9]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[9][22]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[9][4]*SH_LOS[3]*SK_LOS[3]*SK_LOS[16] + P[9][5]*SH_LOS[3]*SK_LOS[8]*SK_LOS[16] - P[9][6]*SH_LOS[3]*SK_LOS[7]*SK_LOS[16]); - Kfusion[10] = -SK_LOS[0]*(P[10][0]*SK_LOS[11] + P[10][1]*SK_LOS[10] - P[10][3]*SK_LOS[9] + P[10][2]*SK_LOS[12] - P[10][9]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[10][22]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[10][4]*SH_LOS[3]*SK_LOS[3]*SK_LOS[16] + P[10][5]*SH_LOS[3]*SK_LOS[8]*SK_LOS[16] - P[10][6]*SH_LOS[3]*SK_LOS[7]*SK_LOS[16]); - Kfusion[11] = -SK_LOS[0]*(P[11][0]*SK_LOS[11] + P[11][1]*SK_LOS[10] - P[11][3]*SK_LOS[9] + P[11][2]*SK_LOS[12] - P[11][9]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[11][22]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[11][4]*SH_LOS[3]*SK_LOS[3]*SK_LOS[16] + P[11][5]*SH_LOS[3]*SK_LOS[8]*SK_LOS[16] - P[11][6]*SH_LOS[3]*SK_LOS[7]*SK_LOS[16]); - Kfusion[12] = -SK_LOS[0]*(P[12][0]*SK_LOS[11] + P[12][1]*SK_LOS[10] - P[12][3]*SK_LOS[9] + P[12][2]*SK_LOS[12] - P[12][9]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[12][22]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[12][4]*SH_LOS[3]*SK_LOS[3]*SK_LOS[16] + P[12][5]*SH_LOS[3]*SK_LOS[8]*SK_LOS[16] - P[12][6]*SH_LOS[3]*SK_LOS[7]*SK_LOS[16]); - Kfusion[13] = -SK_LOS[0]*(P[13][0]*SK_LOS[11] + P[13][1]*SK_LOS[10] - P[13][3]*SK_LOS[9] + P[13][2]*SK_LOS[12] - P[13][9]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[13][22]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[13][4]*SH_LOS[3]*SK_LOS[3]*SK_LOS[16] + P[13][5]*SH_LOS[3]*SK_LOS[8]*SK_LOS[16] - P[13][6]*SH_LOS[3]*SK_LOS[7]*SK_LOS[16]); - Kfusion[14] = -SK_LOS[0]*(P[14][0]*SK_LOS[11] + P[14][1]*SK_LOS[10] - P[14][3]*SK_LOS[9] + P[14][2]*SK_LOS[12] - P[14][9]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[14][22]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[14][4]*SH_LOS[3]*SK_LOS[3]*SK_LOS[16] + P[14][5]*SH_LOS[3]*SK_LOS[8]*SK_LOS[16] - P[14][6]*SH_LOS[3]*SK_LOS[7]*SK_LOS[16]); - Kfusion[15] = -SK_LOS[0]*(P[15][0]*SK_LOS[11] + P[15][1]*SK_LOS[10] - P[15][3]*SK_LOS[9] + P[15][2]*SK_LOS[12] - P[15][9]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[15][22]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[15][4]*SH_LOS[3]*SK_LOS[3]*SK_LOS[16] + P[15][5]*SH_LOS[3]*SK_LOS[8]*SK_LOS[16] - P[15][6]*SH_LOS[3]*SK_LOS[7]*SK_LOS[16]); - Kfusion[16] = -SK_LOS[0]*(P[16][0]*SK_LOS[11] + P[16][1]*SK_LOS[10] - P[16][3]*SK_LOS[9] + P[16][2]*SK_LOS[12] - P[16][9]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[16][22]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[16][4]*SH_LOS[3]*SK_LOS[3]*SK_LOS[16] + P[16][5]*SH_LOS[3]*SK_LOS[8]*SK_LOS[16] - P[16][6]*SH_LOS[3]*SK_LOS[7]*SK_LOS[16]); - Kfusion[17] = -SK_LOS[0]*(P[17][0]*SK_LOS[11] + P[17][1]*SK_LOS[10] - P[17][3]*SK_LOS[9] + P[17][2]*SK_LOS[12] - P[17][9]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[17][22]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[17][4]*SH_LOS[3]*SK_LOS[3]*SK_LOS[16] + P[17][5]*SH_LOS[3]*SK_LOS[8]*SK_LOS[16] - P[17][6]*SH_LOS[3]*SK_LOS[7]*SK_LOS[16]); - Kfusion[18] = -SK_LOS[0]*(P[18][0]*SK_LOS[11] + P[18][1]*SK_LOS[10] - P[18][3]*SK_LOS[9] + P[18][2]*SK_LOS[12] - P[18][9]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[18][22]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[18][4]*SH_LOS[3]*SK_LOS[3]*SK_LOS[16] + P[18][5]*SH_LOS[3]*SK_LOS[8]*SK_LOS[16] - P[18][6]*SH_LOS[3]*SK_LOS[7]*SK_LOS[16]); - Kfusion[19] = -SK_LOS[0]*(P[19][0]*SK_LOS[11] + P[19][1]*SK_LOS[10] - P[19][3]*SK_LOS[9] + P[19][2]*SK_LOS[12] - P[19][9]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[19][22]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[19][4]*SH_LOS[3]*SK_LOS[3]*SK_LOS[16] + P[19][5]*SH_LOS[3]*SK_LOS[8]*SK_LOS[16] - P[19][6]*SH_LOS[3]*SK_LOS[7]*SK_LOS[16]); - Kfusion[20] = -SK_LOS[0]*(P[20][0]*SK_LOS[11] + P[20][1]*SK_LOS[10] - P[20][3]*SK_LOS[9] + P[20][2]*SK_LOS[12] - P[20][9]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[20][22]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[20][4]*SH_LOS[3]*SK_LOS[3]*SK_LOS[16] + P[20][5]*SH_LOS[3]*SK_LOS[8]*SK_LOS[16] - P[20][6]*SH_LOS[3]*SK_LOS[7]*SK_LOS[16]); - Kfusion[21] = -SK_LOS[0]*(P[21][0]*SK_LOS[11] + P[21][1]*SK_LOS[10] - P[21][3]*SK_LOS[9] + P[21][2]*SK_LOS[12] - P[21][9]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[21][22]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[21][4]*SH_LOS[3]*SK_LOS[3]*SK_LOS[16] + P[21][5]*SH_LOS[3]*SK_LOS[8]*SK_LOS[16] - P[21][6]*SH_LOS[3]*SK_LOS[7]*SK_LOS[16]); - Kfusion[22] = -SK_LOS[0]*(P[22][0]*SK_LOS[11] + P[22][1]*SK_LOS[10] - P[22][3]*SK_LOS[9] + P[22][2]*SK_LOS[12] - P[22][9]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[22][22]*SH_LOS[2]*SH_LOS[7]*SK_LOS[16] + P[22][4]*SH_LOS[3]*SK_LOS[3]*SK_LOS[16] + P[22][5]*SH_LOS[3]*SK_LOS[8]*SK_LOS[16] - P[22][6]*SH_LOS[3]*SK_LOS[7]*SK_LOS[16]); - varInnovLOS[0] = 1.0f/SK_LOS[0]; - innovLOS[0] = losPred[0] - losData[0]; - - // reset the observation index to 0 (we start by fusing the X - // measurement) - obsIndex = 0; - fuseOptData = false; - } - else if (obsIndex == 1) // we are now fusing the Y measurement - { - // Calculate observation jacobians - for (uint8_t i = 0; i < n_states; i++) H_LOS[i] = 0; - H_LOS[0] = SH_LOS[1]*SH_LOS[3]*(2*a1*q1 - 2*q0 + 2*a2*q2) + SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[6] - ve*SH_LOS[11] + vn*SH_LOS[5]); - H_LOS[1] = SH_LOS[1]*SH_LOS[3]*(2*q1 + 2*a1*q0 - 2*a2*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[11] + ve*SH_LOS[6] + vn*SH_LOS[4]); - H_LOS[2] = SH_LOS[1]*SH_LOS[3]*(2*q2 + 2*a2*q0 + 2*a1*q3) - SH_LOS[0]*SH_LOS[3]*(vd*SH_LOS[5] + ve*SH_LOS[4] - vn*SH_LOS[6]); - H_LOS[3] = SH_LOS[0]*SH_LOS[3]*(ve*SH_LOS[5] - vd*SH_LOS[4] + vn*SH_LOS[11]) - SH_LOS[1]*SH_LOS[3]*(2*q3 - 2*a1*q2 + 2*a2*q1); - H_LOS[4] = SH_LOS[0]*SH_LOS[3]*(a2*(2*q0*q2 + 2*q1*q3) + a3*(2*q0*q3 - 2*q1*q2) - sq(q0) - sq(q1) + sq(q2) + sq(q3)); - H_LOS[5] = -SH_LOS[0]*SH_LOS[3]*(a3*(sq(q0) - sq(q1) + sq(q2) - sq(q3)) + 2*q0*q3 + 2*q1*q2 + a2*(2*q0*q1 - 2*q2*q3)); - H_LOS[6] = SH_LOS[0]*SH_LOS[3]*(a2*(sq(q0) - sq(q1) - sq(q2) + sq(q3)) + 2*q0*q2 - 2*q1*q3 - a3*(2*q0*q1 + 2*q2*q3)); - H_LOS[9] = -SH_LOS[0]*SH_LOS[1]*SH_LOS[7]; - H_LOS[22] = SH_LOS[0]*SH_LOS[1]*SH_LOS[7]; - - // Calculate Kalman gains - Kfusion[0] = SK_LOS[1]*(P[0][0]*SK_LOS[13] - P[0][3]*SK_LOS[6] + P[0][1]*SK_LOS[14] + P[0][2]*SK_LOS[15] - P[0][9]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[0][22]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[0][6]*SH_LOS[3]*SK_LOS[2]*SK_LOS[16] + P[0][4]*SH_LOS[3]*SK_LOS[5]*SK_LOS[16] - P[0][5]*SH_LOS[3]*SK_LOS[4]*SK_LOS[16]); - Kfusion[1] = SK_LOS[1]*(P[1][0]*SK_LOS[13] - P[1][3]*SK_LOS[6] + P[1][1]*SK_LOS[14] + P[1][2]*SK_LOS[15] - P[1][9]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[1][22]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[1][6]*SH_LOS[3]*SK_LOS[2]*SK_LOS[16] + P[1][4]*SH_LOS[3]*SK_LOS[5]*SK_LOS[16] - P[1][5]*SH_LOS[3]*SK_LOS[4]*SK_LOS[16]); - Kfusion[2] = SK_LOS[1]*(P[2][0]*SK_LOS[13] - P[2][3]*SK_LOS[6] + P[2][1]*SK_LOS[14] + P[2][2]*SK_LOS[15] - P[2][9]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[2][22]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[2][6]*SH_LOS[3]*SK_LOS[2]*SK_LOS[16] + P[2][4]*SH_LOS[3]*SK_LOS[5]*SK_LOS[16] - P[2][5]*SH_LOS[3]*SK_LOS[4]*SK_LOS[16]); - Kfusion[3] = SK_LOS[1]*(P[3][0]*SK_LOS[13] - P[3][3]*SK_LOS[6] + P[3][1]*SK_LOS[14] + P[3][2]*SK_LOS[15] - P[3][9]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[3][22]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[3][6]*SH_LOS[3]*SK_LOS[2]*SK_LOS[16] + P[3][4]*SH_LOS[3]*SK_LOS[5]*SK_LOS[16] - P[3][5]*SH_LOS[3]*SK_LOS[4]*SK_LOS[16]); - Kfusion[4] = SK_LOS[1]*(P[4][0]*SK_LOS[13] - P[4][3]*SK_LOS[6] + P[4][1]*SK_LOS[14] + P[4][2]*SK_LOS[15] - P[4][9]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[4][22]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[4][6]*SH_LOS[3]*SK_LOS[2]*SK_LOS[16] + P[4][4]*SH_LOS[3]*SK_LOS[5]*SK_LOS[16] - P[4][5]*SH_LOS[3]*SK_LOS[4]*SK_LOS[16]); - Kfusion[5] = SK_LOS[1]*(P[5][0]*SK_LOS[13] - P[5][3]*SK_LOS[6] + P[5][1]*SK_LOS[14] + P[5][2]*SK_LOS[15] - P[5][9]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[5][22]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[5][6]*SH_LOS[3]*SK_LOS[2]*SK_LOS[16] + P[5][4]*SH_LOS[3]*SK_LOS[5]*SK_LOS[16] - P[5][5]*SH_LOS[3]*SK_LOS[4]*SK_LOS[16]); - Kfusion[6] = SK_LOS[1]*(P[6][0]*SK_LOS[13] - P[6][3]*SK_LOS[6] + P[6][1]*SK_LOS[14] + P[6][2]*SK_LOS[15] - P[6][9]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[6][22]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[6][6]*SH_LOS[3]*SK_LOS[2]*SK_LOS[16] + P[6][4]*SH_LOS[3]*SK_LOS[5]*SK_LOS[16] - P[6][5]*SH_LOS[3]*SK_LOS[4]*SK_LOS[16]); - Kfusion[7] = SK_LOS[1]*(P[7][0]*SK_LOS[13] - P[7][3]*SK_LOS[6] + P[7][1]*SK_LOS[14] + P[7][2]*SK_LOS[15] - P[7][9]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[7][22]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[7][6]*SH_LOS[3]*SK_LOS[2]*SK_LOS[16] + P[7][4]*SH_LOS[3]*SK_LOS[5]*SK_LOS[16] - P[7][5]*SH_LOS[3]*SK_LOS[4]*SK_LOS[16]); - Kfusion[8] = SK_LOS[1]*(P[8][0]*SK_LOS[13] - P[8][3]*SK_LOS[6] + P[8][1]*SK_LOS[14] + P[8][2]*SK_LOS[15] - P[8][9]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[8][22]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[8][6]*SH_LOS[3]*SK_LOS[2]*SK_LOS[16] + P[8][4]*SH_LOS[3]*SK_LOS[5]*SK_LOS[16] - P[8][5]*SH_LOS[3]*SK_LOS[4]*SK_LOS[16]); - Kfusion[9] = SK_LOS[1]*(P[9][0]*SK_LOS[13] - P[9][3]*SK_LOS[6] + P[9][1]*SK_LOS[14] + P[9][2]*SK_LOS[15] - P[9][9]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[9][22]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[9][6]*SH_LOS[3]*SK_LOS[2]*SK_LOS[16] + P[9][4]*SH_LOS[3]*SK_LOS[5]*SK_LOS[16] - P[9][5]*SH_LOS[3]*SK_LOS[4]*SK_LOS[16]); - Kfusion[10] = SK_LOS[1]*(P[10][0]*SK_LOS[13] - P[10][3]*SK_LOS[6] + P[10][1]*SK_LOS[14] + P[10][2]*SK_LOS[15] - P[10][9]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[10][22]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[10][6]*SH_LOS[3]*SK_LOS[2]*SK_LOS[16] + P[10][4]*SH_LOS[3]*SK_LOS[5]*SK_LOS[16] - P[10][5]*SH_LOS[3]*SK_LOS[4]*SK_LOS[16]); - Kfusion[11] = SK_LOS[1]*(P[11][0]*SK_LOS[13] - P[11][3]*SK_LOS[6] + P[11][1]*SK_LOS[14] + P[11][2]*SK_LOS[15] - P[11][9]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[11][22]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[11][6]*SH_LOS[3]*SK_LOS[2]*SK_LOS[16] + P[11][4]*SH_LOS[3]*SK_LOS[5]*SK_LOS[16] - P[11][5]*SH_LOS[3]*SK_LOS[4]*SK_LOS[16]); - Kfusion[12] = SK_LOS[1]*(P[12][0]*SK_LOS[13] - P[12][3]*SK_LOS[6] + P[12][1]*SK_LOS[14] + P[12][2]*SK_LOS[15] - P[12][9]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[12][22]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[12][6]*SH_LOS[3]*SK_LOS[2]*SK_LOS[16] + P[12][4]*SH_LOS[3]*SK_LOS[5]*SK_LOS[16] - P[12][5]*SH_LOS[3]*SK_LOS[4]*SK_LOS[16]); - Kfusion[13] = SK_LOS[1]*(P[13][0]*SK_LOS[13] - P[13][3]*SK_LOS[6] + P[13][1]*SK_LOS[14] + P[13][2]*SK_LOS[15] - P[13][9]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[13][22]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[13][6]*SH_LOS[3]*SK_LOS[2]*SK_LOS[16] + P[13][4]*SH_LOS[3]*SK_LOS[5]*SK_LOS[16] - P[13][5]*SH_LOS[3]*SK_LOS[4]*SK_LOS[16]); - Kfusion[14] = SK_LOS[1]*(P[14][0]*SK_LOS[13] - P[14][3]*SK_LOS[6] + P[14][1]*SK_LOS[14] + P[14][2]*SK_LOS[15] - P[14][9]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[14][22]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[14][6]*SH_LOS[3]*SK_LOS[2]*SK_LOS[16] + P[14][4]*SH_LOS[3]*SK_LOS[5]*SK_LOS[16] - P[14][5]*SH_LOS[3]*SK_LOS[4]*SK_LOS[16]); - Kfusion[15] = SK_LOS[1]*(P[15][0]*SK_LOS[13] - P[15][3]*SK_LOS[6] + P[15][1]*SK_LOS[14] + P[15][2]*SK_LOS[15] - P[15][9]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[15][22]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[15][6]*SH_LOS[3]*SK_LOS[2]*SK_LOS[16] + P[15][4]*SH_LOS[3]*SK_LOS[5]*SK_LOS[16] - P[15][5]*SH_LOS[3]*SK_LOS[4]*SK_LOS[16]); - Kfusion[16] = SK_LOS[1]*(P[16][0]*SK_LOS[13] - P[16][3]*SK_LOS[6] + P[16][1]*SK_LOS[14] + P[16][2]*SK_LOS[15] - P[16][9]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[16][22]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[16][6]*SH_LOS[3]*SK_LOS[2]*SK_LOS[16] + P[16][4]*SH_LOS[3]*SK_LOS[5]*SK_LOS[16] - P[16][5]*SH_LOS[3]*SK_LOS[4]*SK_LOS[16]); - Kfusion[17] = SK_LOS[1]*(P[17][0]*SK_LOS[13] - P[17][3]*SK_LOS[6] + P[17][1]*SK_LOS[14] + P[17][2]*SK_LOS[15] - P[17][9]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[17][22]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[17][6]*SH_LOS[3]*SK_LOS[2]*SK_LOS[16] + P[17][4]*SH_LOS[3]*SK_LOS[5]*SK_LOS[16] - P[17][5]*SH_LOS[3]*SK_LOS[4]*SK_LOS[16]); - Kfusion[18] = SK_LOS[1]*(P[18][0]*SK_LOS[13] - P[18][3]*SK_LOS[6] + P[18][1]*SK_LOS[14] + P[18][2]*SK_LOS[15] - P[18][9]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[18][22]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[18][6]*SH_LOS[3]*SK_LOS[2]*SK_LOS[16] + P[18][4]*SH_LOS[3]*SK_LOS[5]*SK_LOS[16] - P[18][5]*SH_LOS[3]*SK_LOS[4]*SK_LOS[16]); - Kfusion[19] = SK_LOS[1]*(P[19][0]*SK_LOS[13] - P[19][3]*SK_LOS[6] + P[19][1]*SK_LOS[14] + P[19][2]*SK_LOS[15] - P[19][9]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[19][22]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[19][6]*SH_LOS[3]*SK_LOS[2]*SK_LOS[16] + P[19][4]*SH_LOS[3]*SK_LOS[5]*SK_LOS[16] - P[19][5]*SH_LOS[3]*SK_LOS[4]*SK_LOS[16]); - Kfusion[20] = SK_LOS[1]*(P[20][0]*SK_LOS[13] - P[20][3]*SK_LOS[6] + P[20][1]*SK_LOS[14] + P[20][2]*SK_LOS[15] - P[20][9]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[20][22]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[20][6]*SH_LOS[3]*SK_LOS[2]*SK_LOS[16] + P[20][4]*SH_LOS[3]*SK_LOS[5]*SK_LOS[16] - P[20][5]*SH_LOS[3]*SK_LOS[4]*SK_LOS[16]); - Kfusion[21] = SK_LOS[1]*(P[21][0]*SK_LOS[13] - P[21][3]*SK_LOS[6] + P[21][1]*SK_LOS[14] + P[21][2]*SK_LOS[15] - P[21][9]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[21][22]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[21][6]*SH_LOS[3]*SK_LOS[2]*SK_LOS[16] + P[21][4]*SH_LOS[3]*SK_LOS[5]*SK_LOS[16] - P[21][5]*SH_LOS[3]*SK_LOS[4]*SK_LOS[16]); - Kfusion[22] = SK_LOS[1]*(P[22][0]*SK_LOS[13] - P[22][3]*SK_LOS[6] + P[22][1]*SK_LOS[14] + P[22][2]*SK_LOS[15] - P[22][9]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[22][22]*SH_LOS[1]*SH_LOS[7]*SK_LOS[16] + P[22][6]*SH_LOS[3]*SK_LOS[2]*SK_LOS[16] + P[22][4]*SH_LOS[3]*SK_LOS[5]*SK_LOS[16] - P[22][5]*SH_LOS[3]*SK_LOS[4]*SK_LOS[16]); - varInnovLOS[1] = 1.0f/SK_LOS[1]; - innovLOS[1] = losPred[1] - losData[1]; - } - - // Check the innovation for consistency and don't fuse if > 5Sigma - if ((innovLOS[obsIndex]*innovLOS[obsIndex]/varInnovMag[obsIndex]) < 25.0) - { - // correct the state vector - for (uint8_t j = 0; j < n_states; j++) - { - states[j] = states[j] - Kfusion[j] * innovLOS[obsIndex]; - } - // normalise the quaternion states - float quatMag = sqrt(states[0]*states[0] + states[1]*states[1] + states[2]*states[2] + states[3]*states[3]); - if (quatMag > 1e-12) - { - for (uint8_t j= 0; j<=3; j++) - { - float quatMagInv = 1.0f/quatMag; - states[j] = states[j] * quatMagInv; - } - } - // correct the covariance P = (I - K*H)*P - // take advantage of the empty columns in KH to reduce the - // number of operations - for (uint8_t i = 0; i < n_states; i++) - { - for (uint8_t j = 0; j <= 6; j++) - { - KH[i][j] = Kfusion[i] * H_LOS[j]; - } - for (uint8_t j = 7; j <= 8; j++) - { - KH[i][j] = 0.0f; - } - KH[i][9] = Kfusion[i] * H_LOS[9]; - for (uint8_t j = 10; j <= 21; j++) - { - KH[i][j] = 0.0f; - } - KH[i][22] = Kfusion[i] * H_LOS[22]; - } - for (uint8_t i = 0; i < n_states; i++) - { - for (uint8_t j = 0; j < n_states; j++) - { - KHP[i][j] = 0.0f; - for (uint8_t k = 0; k <= 6; k++) - { - KHP[i][j] = KHP[i][j] + KH[i][k] * P[k][j]; - } - KHP[i][j] = KHP[i][j] + KH[i][9] * P[9][j]; - KHP[i][j] = KHP[i][j] + KH[i][22] * P[2][j]; - } - } - } - for (uint8_t i = 0; i < n_states; i++) - { - for (uint8_t j = 0; j < n_states; j++) - { - P[i][j] = P[i][j] - KHP[i][j]; - } - } - } - obsIndex = obsIndex + 1; - - ForceSymmetry(); - ConstrainVariances(); -} - -void AttPosEKF::zeroRows(float (&covMat)[n_states][n_states], uint8_t first, uint8_t last) -{ - uint8_t row; - uint8_t col; - for (row=first; row<=last; row++) - { - for (col=0; col Date: Mon, 21 Apr 2014 01:18:34 +0200 Subject: [PATCH 041/329] ekf_att_pos_estimator: Using right app name --- src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp index 0dafc4311e..59ce45e16a 100644 --- a/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp @@ -1211,7 +1211,7 @@ FixedwingEstimator::start() ASSERT(_estimator_task == -1); /* start the task */ - _estimator_task = task_spawn_cmd("fw_att_pos_estimator", + _estimator_task = task_spawn_cmd("ekf_att_pos_estimator", SCHED_DEFAULT, SCHED_PRIORITY_MAX - 40, 6000, From c08544721a0ac4293786ce410dcd0084e1f7cfe6 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 21 Apr 2014 01:20:35 +0200 Subject: [PATCH 042/329] att_pos_estimator_ekf: Update filter to new filter API --- .../ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp index 59ce45e16a..8287757196 100644 --- a/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp @@ -978,8 +978,8 @@ FixedwingEstimator::task_main() // or the time limit will be exceeded at the next IMU update if ((dt >= (_ekf->covTimeStepMax - _ekf->dtIMU)) || (_ekf->summedDelAng.length() > _ekf->covDelAngMax)) { _ekf->CovariancePrediction(dt); - _ekf->summedDelAng = _ekf->summedDelAng.zero(); - _ekf->summedDelVel = _ekf->summedDelVel.zero(); + _ekf->summedDelAng.zero(); + _ekf->summedDelVel.zero(); dt = 0.0f; } From aa3aafb1e5d060593529af72bcf22d6351374df9 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 21 Apr 2014 10:47:15 +0200 Subject: [PATCH 043/329] Added debug macro for EKF. Fixed mag state handling which was only partially stored in correct states and not properly reset on init / dynamic reset --- .../ekf_att_pos_estimator/estimator.cpp | 139 +++++++++++++----- src/modules/ekf_att_pos_estimator/estimator.h | 19 ++- 2 files changed, 123 insertions(+), 35 deletions(-) diff --git a/src/modules/ekf_att_pos_estimator/estimator.cpp b/src/modules/ekf_att_pos_estimator/estimator.cpp index 2a7dab890b..18e17d4d9f 100644 --- a/src/modules/ekf_att_pos_estimator/estimator.cpp +++ b/src/modules/ekf_att_pos_estimator/estimator.cpp @@ -1,7 +1,37 @@ #include "estimator.h" -//#include #include +// Define EKF_DEBUG here to enable the debug print calls +// if the macro is not set, these will be completely +// optimized out by the compiler. +//#define EKF_DEBUG + +#ifdef EKF_DEBUG +#include + +static void +ekf_debug_print(const char *fmt, va_list args) +{ + fprintf(stderr, "%s: ", "[ekf]"); + vfprintf(stderr, fmt, args); + + fprintf(stderr, "\n"); +} + +static void +ekf_debug(const char *fmt, ...) +{ + va_list args; + + va_start(args, fmt); + ekf_debug_print(fmt, args); +} + +#else + +static void ekf_debug(const char *fmt, ...) { while(0){} } +#endif + float Vector3f::length(void) const { return sqrt(x*x + y*y + z*z); @@ -15,6 +45,10 @@ void Vector3f::zero(void) } Mat3f::Mat3f() { + identity(); +} + +void Mat3f::identity() { x.x = 1.0f; x.y = 0.0f; x.z = 0.0f; @@ -122,6 +156,7 @@ AttPosEKF::AttPosEKF() : storeIndex(0) { InitialiseParameters(); + ZeroVariables(); } AttPosEKF::~AttPosEKF() @@ -1219,26 +1254,23 @@ void AttPosEKF::FuseVelposNED() void AttPosEKF::FuseMagnetometer() { - static uint8_t obsIndex; - static float MagPred[3]; - static float SH_MAG[9]; - static float q0 = 0.0f; - static float q1 = 0.0f; - static float q2 = 0.0f; - static float q3 = 1.0f; - static float magN = 0.4f; - static float magE = 0.0f; - static float magD = 0.3f; - static float magXbias = 0.0f; - static float magYbias = 0.0f; - static float magZbias = 0.0f; - float R_MAG = sq(magMeasurementSigma); - float DCM[3][3] = - { - {1.0f,0.0f,0.0f} , - {0.0f,1.0f,0.0f} , - {0.0f,0.0f,1.0f} - }; + + float &q0 = magstate.q0; + float &q1 = magstate.q1; + float &q2 = magstate.q2; + float &q3 = magstate.q3; + float &magN = magstate.magN; + float &magE = magstate.magE; + float &magD = magstate.magD; + float &magXbias = magstate.magXbias; + float &magYbias = magstate.magYbias; + float &magZbias = magstate.magZbias; + unsigned &obsIndex = magstate.obsIndex; + Mat3f &DCM = magstate.DCM; + float *MagPred = &magstate.MagPred[0]; + float &R_MAG = magstate.R_MAG; + float *SH_MAG = &magstate.SH_MAG[0]; + float SK_MX[6]; float SK_MY[5]; float SK_MZ[6]; @@ -1286,18 +1318,18 @@ void AttPosEKF::FuseMagnetometer() // rotate predicted earth components into body axes and calculate // predicted measurments - DCM[0][0] = q0*q0 + q1*q1 - q2*q2 - q3*q3; - DCM[0][1] = 2*(q1*q2 + q0*q3); - DCM[0][2] = 2*(q1*q3-q0*q2); - DCM[1][0] = 2*(q1*q2 - q0*q3); - DCM[1][1] = q0*q0 - q1*q1 + q2*q2 - q3*q3; - DCM[1][2] = 2*(q2*q3 + q0*q1); - DCM[2][0] = 2*(q1*q3 + q0*q2); - DCM[2][1] = 2*(q2*q3 - q0*q1); - DCM[2][2] = q0*q0 - q1*q1 - q2*q2 + q3*q3; - MagPred[0] = DCM[0][0]*magN + DCM[0][1]*magE + DCM[0][2]*magD + magXbias; - MagPred[1] = DCM[1][0]*magN + DCM[1][1]*magE + DCM[1][2]*magD + magYbias; - MagPred[2] = DCM[2][0]*magN + DCM[2][1]*magE + DCM[2][2]*magD + magZbias; + DCM.x.x = q0*q0 + q1*q1 - q2*q2 - q3*q3; + DCM.x.y = 2*(q1*q2 + q0*q3); + DCM.x.z = 2*(q1*q3-q0*q2); + DCM.y.x = 2*(q1*q2 - q0*q3); + DCM.y.y = q0*q0 - q1*q1 + q2*q2 - q3*q3; + DCM.y.z = 2*(q2*q3 + q0*q1); + DCM.z.x = 2*(q1*q3 + q0*q2); + DCM.z.y = 2*(q2*q3 - q0*q1); + DCM.z.z = q0*q0 - q1*q1 - q2*q2 + q3*q3; + MagPred[0] = DCM.x.x*magN + DCM.x.y*magE + DCM.x.z*magD + magXbias; + MagPred[1] = DCM.y.x*magN + DCM.y.y*magE + DCM.y.z*magD + magYbias; + MagPred[2] = DCM.z.x*magN + DCM.z.y*magE + DCM.z.z*magD + magZbias; // scale magnetometer observation error with total angular rate R_MAG = sq(magMeasurementSigma) + sq(0.05f*dAngIMU.length()/dtIMU); @@ -2206,22 +2238,44 @@ void AttPosEKF::FillErrorReport(struct ekf_status_report *err) bool AttPosEKF::StatesNaN(struct ekf_status_report *err_report) { bool err = false; + // check all integrators + if (!isfinite(summedDelAng.x) || !isfinite(summedDelAng.y) || !isfinite(summedDelAng.z)) { + err_report->statesNaN = true; + ekf_debug("summedDelAng NaN: x: %f y: %f z: %f", summedDelAng.x, summedDelAng.y, summedDelAng.z); + err = true; + } // delta angles + + if (!isfinite(correctedDelAng.x) || !isfinite(correctedDelAng.y) || !isfinite(correctedDelAng.z)) { + err_report->statesNaN = true; + ekf_debug("correctedDelAng NaN: x: %f y: %f z: %f", correctedDelAng.x, correctedDelAng.y, correctedDelAng.z); + err = true; + } // delta angles + + if (!isfinite(summedDelVel.x) || !isfinite(summedDelVel.y) || !isfinite(summedDelVel.z)) { + err_report->statesNaN = true; + ekf_debug("summedDelVel NaN: x: %f y: %f z: %f", summedDelVel.x, summedDelVel.y, summedDelVel.z); + err = true; + } // delta velocities + // check all states and covariance matrices for (unsigned i = 0; i < n_states; i++) { for (unsigned j = 0; j < n_states; j++) { if (!isfinite(KH[i][j])) { err_report->covarianceNaN = true; + ekf_debug("KH NaN"); err = true; } // intermediate result used for covariance updates if (!isfinite(KHP[i][j])) { err_report->covarianceNaN = true; + ekf_debug("KHP NaN"); err = true; } // intermediate result used for covariance updates if (!isfinite(P[i][j])) { err_report->covarianceNaN = true; + ekf_debug("P NaN"); err = true; } // covariance matrix } @@ -2229,12 +2283,14 @@ bool AttPosEKF::StatesNaN(struct ekf_status_report *err_report) { if (!isfinite(Kfusion[i])) { err_report->kalmanGainsNaN = true; + ekf_debug("Kfusion NaN"); err = true; } // Kalman gains if (!isfinite(states[i])) { err_report->statesNaN = true; + ekf_debug("states NaN: i: %u val: %f", i, states[i]); err = true; } // state matrix } @@ -2264,6 +2320,7 @@ int AttPosEKF::CheckAndBound() // Reset the filter if the states went NaN if (StatesNaN(&last_ekf_error)) { + ekf_debug("re-initializing dynamic"); InitializeDynamic(velNED); @@ -2362,7 +2419,15 @@ void AttPosEKF::InitializeDynamic(float (&initvelNED)[3]) initMagNED.y = DCM.y.x*initMagXYZ.x + DCM.y.y*initMagXYZ.y + DCM.y.z*initMagXYZ.z; initMagNED.z = DCM.z.x*initMagXYZ.x + DCM.z.y*initMagXYZ.y + DCM.z.z*initMagXYZ.z; - + magstate.q3 = 1.0f; + magstate.magN = 0.4f; + magstate.magE = 0.0f; + magstate.magD = 0.3f; + magstate.magXbias = 0.0f; + magstate.magYbias = 0.0f; + magstate.magZbias = 0.0f; + magstate.R_MAG = sq(magMeasurementSigma); + magstate.DCM.identity(); // write to state vector for (uint8_t j=0; j<=3; j++) states[j] = initQuat[j]; // quaternions @@ -2420,6 +2485,8 @@ void AttPosEKF::ZeroVariables() } correctedDelAng.zero(); + summedDelAng.zero(); + summedDelVel.zero(); for (unsigned i = 0; i < data_buffer_size; i++) { @@ -2430,6 +2497,10 @@ void AttPosEKF::ZeroVariables() statetimeStamp[i] = 0; } + memset(&magstate, 0, sizeof(magstate)); + magstate.q0 = 1.0f; + magstate.DCM.identity(); + memset(¤t_ekf_state, 0, sizeof(current_ekf_state)); } diff --git a/src/modules/ekf_att_pos_estimator/estimator.h b/src/modules/ekf_att_pos_estimator/estimator.h index 3a8f45d9af..734ad0c05d 100644 --- a/src/modules/ekf_att_pos_estimator/estimator.h +++ b/src/modules/ekf_att_pos_estimator/estimator.h @@ -33,6 +33,7 @@ public: Mat3f(); + void identity(); Mat3f transpose(void) const; }; @@ -140,7 +141,23 @@ public: accelProcessNoise = 0.5f; } - + struct { + unsigned obsIndex; + float MagPred[3]; + float SH_MAG[9]; + float q0; + float q1; + float q2; + float q3; + float magN; + float magE; + float magD; + float magXbias; + float magYbias; + float magZbias; + float R_MAG; + Mat3f DCM; + } magstate; // Global variables From 595eb679b30442b52ccc7a2c2ce7ade7b5e5c6c9 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 21 Apr 2014 11:02:27 +0200 Subject: [PATCH 044/329] ekf_att_pos_estimator: Fixed mag initialization, now starts with initial measurement instead of defaults for faster convergence --- .../ekf_att_pos_estimator/estimator.cpp | 19 +++++++++++-------- 1 file changed, 11 insertions(+), 8 deletions(-) diff --git a/src/modules/ekf_att_pos_estimator/estimator.cpp b/src/modules/ekf_att_pos_estimator/estimator.cpp index 18e17d4d9f..95afbc181d 100644 --- a/src/modules/ekf_att_pos_estimator/estimator.cpp +++ b/src/modules/ekf_att_pos_estimator/estimator.cpp @@ -2419,15 +2419,18 @@ void AttPosEKF::InitializeDynamic(float (&initvelNED)[3]) initMagNED.y = DCM.y.x*initMagXYZ.x + DCM.y.y*initMagXYZ.y + DCM.y.z*initMagXYZ.z; initMagNED.z = DCM.z.x*initMagXYZ.x + DCM.z.y*initMagXYZ.y + DCM.z.z*initMagXYZ.z; - magstate.q3 = 1.0f; - magstate.magN = 0.4f; - magstate.magE = 0.0f; - magstate.magD = 0.3f; - magstate.magXbias = 0.0f; - magstate.magYbias = 0.0f; - magstate.magZbias = 0.0f; + magstate.q0 = initQuat[0]; + magstate.q1 = initQuat[1]; + magstate.q2 = initQuat[2]; + magstate.q3 = initQuat[3]; + magstate.magN = magData.x; + magstate.magE = magData.y; + magstate.magD = magData.z; + magstate.magXbias = magBias.x; + magstate.magYbias = magBias.y; + magstate.magZbias = magBias.z; magstate.R_MAG = sq(magMeasurementSigma); - magstate.DCM.identity(); + magstate.DCM = DCM; // write to state vector for (uint8_t j=0; j<=3; j++) states[j] = initQuat[j]; // quaternions From 8ae50a4ba524f840dc7d96b79c5dbad1b7be15f1 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 20 Apr 2014 03:06:13 +0200 Subject: [PATCH 045/329] Changed home position set to depend on the commander home position switch --- .../fw_att_pos_estimator_main.cpp | 22 ++++++++++++++----- 1 file changed, 16 insertions(+), 6 deletions(-) diff --git a/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp b/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp index 8ea6118022..e9c036e858 100644 --- a/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp +++ b/src/modules/fw_att_pos_estimator/fw_att_pos_estimator_main.cpp @@ -73,6 +73,7 @@ #include #include #include +#include #include #include #include @@ -152,6 +153,7 @@ private: int _params_sub; /**< notification of parameter updates */ int _manual_control_sub; /**< notification of manual control updates */ int _mission_sub; + int _home_sub; /**< home position as defined by commander / user */ orb_advert_t _att_pub; /**< vehicle attitude */ orb_advert_t _global_pos_pub; /**< global position */ @@ -410,6 +412,7 @@ FixedwingEstimator::task_main() _gps_sub = orb_subscribe(ORB_ID(vehicle_gps_position)); _vstatus_sub = orb_subscribe(ORB_ID(vehicle_status)); _params_sub = orb_subscribe(ORB_ID(parameter_update)); + _home_sub = orb_subscribe(ORB_ID(home_position)); /* rate limit vehicle status updates to 5Hz */ orb_set_interval(_vstatus_sub, 200); @@ -797,20 +800,27 @@ FixedwingEstimator::task_main() if (hrt_elapsed_time(&start_time) > 100000) { - if (!_gps_initialized && (_ekf->GPSstatus == 3)) { + bool home_set; + orb_check(_home_sub, &home_set); + + if (home_set && !_gps_initialized && _gps.fix_type > 2) { _ekf->velNED[0] = _gps.vel_n_m_s; _ekf->velNED[1] = _gps.vel_e_m_s; _ekf->velNED[2] = _gps.vel_d_m_s; - double lat = _gps.lat * 1e-7; - double lon = _gps.lon * 1e-7; - float alt = _gps.alt * 1e-3; + struct home_position_s home; + + orb_copy(ORB_ID(home_position), _home_sub, &home); + + double lat = home.lat; + double lon = home.lon; + float alt = home.alt; _ekf->InitialiseFilter(_ekf->velNED); // Initialize projection - _local_pos.ref_lat = _gps.lat; - _local_pos.ref_lon = _gps.lon; + _local_pos.ref_lat = home.lat * 1e7; + _local_pos.ref_lon = home.lon * 1e7; _local_pos.ref_alt = alt; _local_pos.ref_timestamp = _gps.timestamp_position; From 37b133e231e945c978dd66fd5daed0f12caa8073 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 20 Apr 2014 03:04:56 +0200 Subject: [PATCH 046/329] Added home position switch on GPS position - gives a more reliable home position setup --- src/modules/commander/commander.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index ce6de88ef9..7ac7aff0f2 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1092,9 +1092,9 @@ int commander_thread_main(int argc, char *argv[]) && global_position.global_valid) { /* copy position data to uORB home message, store it locally as well */ - home.lat = global_position.lat; - home.lon = global_position.lon; - home.alt = global_position.alt; + home.lat = gps_position.lat / (double)1e7; + home.lon = gps_position.lon / (double)1e7; + home.alt = gps_position.alt; warnx("home: lat = %.7f, lon = %.7f, alt = %.4f ", home.lat, home.lon, (double)home.alt); mavlink_log_info(mavlink_fd, "[cmd] home: %.7f, %.7f, %.4f", home.lat, home.lon, (double)home.alt); From 706d08055d2bf63d16c72e86cd60f0e33a20bc4f Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 22 Apr 2014 01:18:30 +0200 Subject: [PATCH 047/329] Better / cleaner initialization of the attitude estimator --- .../ekf_att_pos_estimator/estimator.cpp | 24 ++++++++----- src/modules/ekf_att_pos_estimator/estimator.h | 2 +- .../fw_att_pos_estimator_main.cpp | 34 ++++++++++++------- 3 files changed, 38 insertions(+), 22 deletions(-) diff --git a/src/modules/ekf_att_pos_estimator/estimator.cpp b/src/modules/ekf_att_pos_estimator/estimator.cpp index 95afbc181d..5f4bf07293 100644 --- a/src/modules/ekf_att_pos_estimator/estimator.cpp +++ b/src/modules/ekf_att_pos_estimator/estimator.cpp @@ -4,7 +4,7 @@ // Define EKF_DEBUG here to enable the debug print calls // if the macro is not set, these will be completely // optimized out by the compiler. -//#define EKF_DEBUG +#define EKF_DEBUG #ifdef EKF_DEBUG #include @@ -2263,21 +2263,29 @@ bool AttPosEKF::StatesNaN(struct ekf_status_report *err_report) { if (!isfinite(KH[i][j])) { err_report->covarianceNaN = true; - ekf_debug("KH NaN"); err = true; } // intermediate result used for covariance updates + if (err) { + ekf_debug("KH NaN"); + } + if (!isfinite(KHP[i][j])) { err_report->covarianceNaN = true; - ekf_debug("KHP NaN"); err = true; } // intermediate result used for covariance updates + if (err) { + ekf_debug("KHP NaN"); + } + if (!isfinite(P[i][j])) { err_report->covarianceNaN = true; - ekf_debug("P NaN"); err = true; } // covariance matrix + if (err) { + ekf_debug("P NaN"); + } } if (!isfinite(Kfusion[i])) { @@ -2461,12 +2469,12 @@ void AttPosEKF::InitializeDynamic(float (&initvelNED)[3]) summedDelVel.z = 0.0f; } -void AttPosEKF::InitialiseFilter(float (&initvelNED)[3]) +void AttPosEKF::InitialiseFilter(float (&initvelNED)[3], double referenceLat, double referenceLon, float referenceHgt) { //store initial lat,long and height - latRef = gpsLat; - lonRef = gpsLon; - hgtRef = gpsHgt; + latRef = referenceLat; + lonRef = referenceLon; + hgtRef = referenceHgt; memset(&last_ekf_error, 0, sizeof(last_ekf_error)); diff --git a/src/modules/ekf_att_pos_estimator/estimator.h b/src/modules/ekf_att_pos_estimator/estimator.h index 734ad0c05d..5e60e506f6 100644 --- a/src/modules/ekf_att_pos_estimator/estimator.h +++ b/src/modules/ekf_att_pos_estimator/estimator.h @@ -308,7 +308,7 @@ void OnGroundCheck(); void CovarianceInit(); -void InitialiseFilter(float (&initvelNED)[3]); +void InitialiseFilter(float (&initvelNED)[3], double referenceLat, double referenceLon, float referenceHgt); float ConstrainFloat(float val, float min, float max); diff --git a/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp index 8287757196..c8005f3d3f 100644 --- a/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp @@ -606,7 +606,6 @@ FixedwingEstimator::task_main() /* guard against too large deltaT's */ if (!isfinite(deltaT) || deltaT > 1.0f || deltaT < 0.000001f) { deltaT = 0.01f; - warnx("TS fail"); } @@ -650,14 +649,14 @@ FixedwingEstimator::task_main() IMUmsec = _sensor_combined.timestamp / 1e3f; float deltaT = (_sensor_combined.timestamp - last_run) / 1e6f; - last_run = _sensor_combined.timestamp; /* guard against too large deltaT's */ if (!isfinite(deltaT) || deltaT > 1.0f || deltaT < 0.000001f) { deltaT = 0.01f; - warnx("TS fail"); } + last_run = _sensor_combined.timestamp; + // Always store data, independent of init status /* fill in last data set */ _ekf->dtIMU = deltaT; @@ -823,6 +822,8 @@ FixedwingEstimator::task_main() */ int check = _ekf->CheckAndBound(); + const char* ekfname = "[ekf] "; + switch (check) { case 0: /* all ok */ @@ -830,27 +831,34 @@ FixedwingEstimator::task_main() case 1: { const char* str = "NaN in states, resetting"; - warnx(str); - mavlink_log_critical(_mavlink_fd, str); + warnx("%s%s", ekfname, str); + mavlink_log_critical(_mavlink_fd, "%s%s", ekfname, str); break; } case 2: { const char* str = "stale IMU data, resetting"; - warnx(str); - mavlink_log_critical(_mavlink_fd, str); + warnx("%s%s", ekfname, str); + mavlink_log_critical(_mavlink_fd, "%s%s", ekfname, str); break; } case 3: { - const char* str = "switching dynamic / static state"; - warnx(str); - mavlink_log_critical(_mavlink_fd, str); + const char* str = "switching to dynamic state"; + warnx("%s%s", ekfname, str); + mavlink_log_critical(_mavlink_fd, "%s%s", ekfname, str); break; } + + default: + { + const char* str = "unknown reset condition"; + warnx("%s%s", ekfname, str); + mavlink_log_critical(_mavlink_fd, "%s%s", ekfname, str); + } } - // If non-zero, we got a problem + // If non-zero, we got a filter reset if (check) { struct ekf_status_report ekf_report; @@ -912,7 +920,7 @@ FixedwingEstimator::task_main() double lon = home.lon; float alt = home.alt; - _ekf->InitialiseFilter(_ekf->velNED); + _ekf->InitialiseFilter(_ekf->velNED, math::radians(lat), math::radians(lon) - M_PI, alt); // Initialize projection _local_pos.ref_lat = home.lat * 1e7; @@ -942,7 +950,7 @@ FixedwingEstimator::task_main() _ekf->posNE[0] = _ekf->posNED[0]; _ekf->posNE[1] = _ekf->posNED[1]; - _ekf->InitialiseFilter(_ekf->velNED); + _ekf->InitialiseFilter(_ekf->velNED, 0.0, 0.0, 0.0f); } } From db15e2811ea01dd023ae930e6e7a73c1a370cecf Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 22 Apr 2014 01:36:32 +0200 Subject: [PATCH 048/329] commander: Fix altitude initialisation, do not depend on global pos valid flag. --- src/modules/commander/commander.cpp | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 7ac7aff0f2..077184bfbd 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1088,13 +1088,12 @@ int commander_thread_main(int argc, char *argv[]) if (!status.condition_home_position_valid && gps_position.fix_type >= 3 && (gps_position.eph_m < hdop_threshold_m) && (gps_position.epv_m < vdop_threshold_m) && - (hrt_absolute_time() < gps_position.timestamp_position + POSITION_TIMEOUT) && !armed.armed - && global_position.global_valid) { + (hrt_absolute_time() < gps_position.timestamp_position + POSITION_TIMEOUT) && !armed.armed) { /* copy position data to uORB home message, store it locally as well */ home.lat = gps_position.lat / (double)1e7; home.lon = gps_position.lon / (double)1e7; - home.alt = gps_position.alt; + home.alt = gps_position.alt / (float)1e3; warnx("home: lat = %.7f, lon = %.7f, alt = %.4f ", home.lat, home.lon, (double)home.alt); mavlink_log_info(mavlink_fd, "[cmd] home: %.7f, %.7f, %.4f", home.lat, home.lon, (double)home.alt); From 904ada124baea8ef744535053a0c3b40871565e3 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 22 Apr 2014 02:15:33 +0200 Subject: [PATCH 049/329] ekf: Put reset statements after variable zero operation to ensure values get initialized correctly --- src/modules/ekf_att_pos_estimator/estimator.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/modules/ekf_att_pos_estimator/estimator.cpp b/src/modules/ekf_att_pos_estimator/estimator.cpp index 5f4bf07293..99de161aac 100644 --- a/src/modules/ekf_att_pos_estimator/estimator.cpp +++ b/src/modules/ekf_att_pos_estimator/estimator.cpp @@ -2407,10 +2407,6 @@ void AttPosEKF::InitializeDynamic(float (&initvelNED)[3]) ZeroVariables(); - ResetVelocity(); - ResetPosition(); - ResetHeight(); - // Calculate initial filter quaternion states from raw measurements float initQuat[4]; Vector3f initMagXYZ; @@ -2452,6 +2448,10 @@ void AttPosEKF::InitializeDynamic(float (&initvelNED)[3]) states[21] = magBias.z; // Magnetic Field Bias Z states[22] = 0.0f; // terrain height + ResetVelocity(); + ResetPosition(); + ResetHeight(); + statesInitialised = true; // initialise the covariance matrix From 39a0d4e54db1252678eb8f4ebc872c589da2d9e7 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 22 Apr 2014 02:24:29 +0200 Subject: [PATCH 050/329] Better error handling / reporting in filter --- .../ekf_att_pos_estimator/estimator.cpp | 20 ++++++++++--------- 1 file changed, 11 insertions(+), 9 deletions(-) diff --git a/src/modules/ekf_att_pos_estimator/estimator.cpp b/src/modules/ekf_att_pos_estimator/estimator.cpp index 99de161aac..14761831cf 100644 --- a/src/modules/ekf_att_pos_estimator/estimator.cpp +++ b/src/modules/ekf_att_pos_estimator/estimator.cpp @@ -2243,18 +2243,21 @@ bool AttPosEKF::StatesNaN(struct ekf_status_report *err_report) { err_report->statesNaN = true; ekf_debug("summedDelAng NaN: x: %f y: %f z: %f", summedDelAng.x, summedDelAng.y, summedDelAng.z); err = true; + goto out; } // delta angles if (!isfinite(correctedDelAng.x) || !isfinite(correctedDelAng.y) || !isfinite(correctedDelAng.z)) { err_report->statesNaN = true; ekf_debug("correctedDelAng NaN: x: %f y: %f z: %f", correctedDelAng.x, correctedDelAng.y, correctedDelAng.z); err = true; + goto out; } // delta angles if (!isfinite(summedDelVel.x) || !isfinite(summedDelVel.y) || !isfinite(summedDelVel.z)) { err_report->statesNaN = true; ekf_debug("summedDelVel NaN: x: %f y: %f z: %f", summedDelVel.x, summedDelVel.y, summedDelVel.z); err = true; + goto out; } // delta velocities // check all states and covariance matrices @@ -2264,28 +2267,24 @@ bool AttPosEKF::StatesNaN(struct ekf_status_report *err_report) { err_report->covarianceNaN = true; err = true; - } // intermediate result used for covariance updates - if (err) { ekf_debug("KH NaN"); - } + goto out; + } // intermediate result used for covariance updates if (!isfinite(KHP[i][j])) { err_report->covarianceNaN = true; err = true; - } // intermediate result used for covariance updates - if (err) { ekf_debug("KHP NaN"); - } + goto out; + } // intermediate result used for covariance updates if (!isfinite(P[i][j])) { err_report->covarianceNaN = true; err = true; - } // covariance matrix - if (err) { ekf_debug("P NaN"); - } + } // covariance matrix } if (!isfinite(Kfusion[i])) { @@ -2293,6 +2292,7 @@ bool AttPosEKF::StatesNaN(struct ekf_status_report *err_report) { err_report->kalmanGainsNaN = true; ekf_debug("Kfusion NaN"); err = true; + goto out; } // Kalman gains if (!isfinite(states[i])) { @@ -2300,9 +2300,11 @@ bool AttPosEKF::StatesNaN(struct ekf_status_report *err_report) { err_report->statesNaN = true; ekf_debug("states NaN: i: %u val: %f", i, states[i]); err = true; + goto out; } // state matrix } +out: if (err) { FillErrorReport(err_report); } From 125f0b2f88c390bfde92ebe5423a0913e0e1b114 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 22 Apr 2014 02:25:38 +0200 Subject: [PATCH 051/329] Added trap to filter to catch NaN handling --- .../ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp index c8005f3d3f..ac34af48a4 100644 --- a/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp @@ -858,6 +858,11 @@ FixedwingEstimator::task_main() } } + // XXX trap for testing + if (check == 1 || check == 2) { + errx(1, "NUMERIC ERROR IN FILTER"); + } + // If non-zero, we got a filter reset if (check) { From 5b7209639d42290dc5b1e3fc53fc81a455d75b51 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 22 Apr 2014 09:56:38 +0200 Subject: [PATCH 052/329] Revert "HIL: Increased MAVLink link wait time based on previous experience that this is timing sensitive." This reverts commit 78bf7ed969624eacea35ae2bf402596aecb3c5a6. --- ROMFS/px4fmu_common/init.d/rcS | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index ccac0a2631..2ab1e2e96f 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -403,8 +403,7 @@ then then if [ $HIL == yes ] then - # Sleep required to ensure the link is up - sleep 5 + sleep 1 set MAVLINK_FLAGS "-r 10000 -d /dev/ttyACM0" else # Normal mode, use baudrate 57600 (default) and data rate 1000 bytes/s From bd637697e4880d3efbb79c2c05647564d3582cde Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 22 Apr 2014 10:26:26 +0200 Subject: [PATCH 053/329] Removed verbose print --- src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp index ac34af48a4..ad7cb3687a 100644 --- a/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp @@ -920,6 +920,7 @@ FixedwingEstimator::task_main() struct home_position_s home; orb_copy(ORB_ID(home_position), _home_sub, &home); + warnx("HOME SET"); double lat = home.lat; double lon = home.lon; From 1e80e624916a0eb1b13adccb4f700adeeee66bba Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 22 Apr 2014 10:26:44 +0200 Subject: [PATCH 054/329] ekf: Better variable zeroing --- src/modules/ekf_att_pos_estimator/estimator.cpp | 13 +++++++++++-- 1 file changed, 11 insertions(+), 2 deletions(-) diff --git a/src/modules/ekf_att_pos_estimator/estimator.cpp b/src/modules/ekf_att_pos_estimator/estimator.cpp index 14761831cf..ac9abf5ca3 100644 --- a/src/modules/ekf_att_pos_estimator/estimator.cpp +++ b/src/modules/ekf_att_pos_estimator/estimator.cpp @@ -153,8 +153,16 @@ AttPosEKF::AttPosEKF() : useCompass(true), useRangeFinder(true), numericalProtection(true), - storeIndex(0) + storeIndex(0), + gpsHgt(0.0f), + baroHgt(0.0f), + GPSstatus(0), + VtasMeas(0.0f), { + velNED[0] = 0.0f; + velNED[1] = 0.0f; + velNED[2] = 0.0f; + InitialiseParameters(); ZeroVariables(); } @@ -1967,7 +1975,7 @@ void AttPosEKF::calcLLH(float (&posNED)[3], float lat, float lon, float hgt, flo void AttPosEKF::OnGroundCheck() { - onGround = (((sq(velNED[0]) + sq(velNED[1]) + sq(velNED[2])) < 4.0f) && (VtasMeas < 8.0f)); + onGround = (((sq(velNED[0]) + sq(velNED[1]) + sq(velNED[2])) < 4.0f) && (VtasMeas < 6.0f)); if (staticMode) { staticMode = !(GPSstatus > GPS_FIX_2D); } @@ -2515,6 +2523,7 @@ void AttPosEKF::ZeroVariables() magstate.DCM.identity(); memset(¤t_ekf_state, 0, sizeof(current_ekf_state)); + } void AttPosEKF::GetFilterState(struct ekf_status_report *state) From 4585df1182083c39f2439bb7b88953dcc3575240 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 22 Apr 2014 11:02:31 +0200 Subject: [PATCH 055/329] Robustified filter init / sequencing --- src/modules/ekf_att_pos_estimator/estimator.cpp | 6 ++++-- src/modules/ekf_att_pos_estimator/estimator.h | 1 + .../fw_att_pos_estimator_main.cpp | 13 +++++++++---- 3 files changed, 14 insertions(+), 6 deletions(-) diff --git a/src/modules/ekf_att_pos_estimator/estimator.cpp b/src/modules/ekf_att_pos_estimator/estimator.cpp index ac9abf5ca3..5de22fdaec 100644 --- a/src/modules/ekf_att_pos_estimator/estimator.cpp +++ b/src/modules/ekf_att_pos_estimator/estimator.cpp @@ -153,11 +153,12 @@ AttPosEKF::AttPosEKF() : useCompass(true), useRangeFinder(true), numericalProtection(true), + refSet(false), storeIndex(0), gpsHgt(0.0f), baroHgt(0.0f), GPSstatus(0), - VtasMeas(0.0f), + VtasMeas(0.0f) { velNED[0] = 0.0f; velNED[1] = 0.0f; @@ -1977,7 +1978,7 @@ void AttPosEKF::OnGroundCheck() { onGround = (((sq(velNED[0]) + sq(velNED[1]) + sq(velNED[2])) < 4.0f) && (VtasMeas < 6.0f)); if (staticMode) { - staticMode = !(GPSstatus > GPS_FIX_2D); + staticMode = (!refSet || (GPSstatus < GPS_FIX_3D)); } } @@ -2485,6 +2486,7 @@ void AttPosEKF::InitialiseFilter(float (&initvelNED)[3], double referenceLat, do latRef = referenceLat; lonRef = referenceLon; hgtRef = referenceHgt; + refSet = true; memset(&last_ekf_error, 0, sizeof(last_ekf_error)); diff --git a/src/modules/ekf_att_pos_estimator/estimator.h b/src/modules/ekf_att_pos_estimator/estimator.h index 5e60e506f6..e118ae5734 100644 --- a/src/modules/ekf_att_pos_estimator/estimator.h +++ b/src/modules/ekf_att_pos_estimator/estimator.h @@ -211,6 +211,7 @@ public: double latRef; // WGS-84 latitude of reference point (rad) double lonRef; // WGS-84 longitude of reference point (rad) float hgtRef; // WGS-84 height of reference point (m) + bool refSet; ///< flag to indicate if the reference position has been set Vector3f magBias; // states representing magnetometer bias vector in XYZ body axes unsigned covSkipCount; // Number of state prediction frames (IMU daya updates to skip before doing the covariance prediction diff --git a/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp index ad7cb3687a..19111a9b42 100644 --- a/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp @@ -192,6 +192,7 @@ private: bool _initialized; bool _gps_initialized; + uint64_t _gps_start_time; int _mavlink_fd; @@ -720,6 +721,9 @@ FixedwingEstimator::task_main() } else { + /* store time of valid GPS measurement */ + _gps_start_time = hrt_absolute_time(); + /* check if we had a GPS outage for a long time */ if (hrt_elapsed_time(&last_gps) > 5 * 1000 * 1000) { _ekf->ResetPosition(); @@ -859,7 +863,7 @@ FixedwingEstimator::task_main() } // XXX trap for testing - if (check == 1 || check == 2) { + if (check == 1) { errx(1, "NUMERIC ERROR IN FILTER"); } @@ -907,7 +911,7 @@ FixedwingEstimator::task_main() // XXX we rather want to check all updated - if (hrt_elapsed_time(&start_time) > 100000) { + if (hrt_elapsed_time(&_gps_start_time) > 50000) { bool home_set; orb_check(_home_sub, &home_set); @@ -920,7 +924,6 @@ FixedwingEstimator::task_main() struct home_position_s home; orb_copy(ORB_ID(home_position), _home_sub, &home); - warnx("HOME SET"); double lat = home.lat; double lon = home.lon; @@ -942,7 +945,9 @@ FixedwingEstimator::task_main() // XXX this is not multithreading safe map_projection_init(lat, lon); - mavlink_log_info(_mavlink_fd, "[position estimator] init ref: lat=%.7f, lon=%.7f, alt=%.2f", lat, lon, alt); + mavlink_log_info(_mavlink_fd, "[ekf] ref: LA %.4f,LO %.4f,ALT %.2f", lat, lon, (double)alt); + warnx("[ekf] HOME/REF: LA %8.4f,LO %8.4f,ALT %8.2f V: %8.4f %8.4f %8.4f", lat, lon, (double)alt, + (double)_ekf->velNED[0], (double)_ekf->velNED[1], (double)_ekf->velNED[2]); _gps_initialized = true; From a30411e9f2438018a08c0965261067940f88be10 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Tue, 22 Apr 2014 11:02:53 +0200 Subject: [PATCH 056/329] Fixed printing in attitude control --- src/modules/fw_att_control/fw_att_control_main.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/src/modules/fw_att_control/fw_att_control_main.cpp b/src/modules/fw_att_control/fw_att_control_main.cpp index 2f84dc963b..5ded7aa76f 100644 --- a/src/modules/fw_att_control/fw_att_control_main.cpp +++ b/src/modules/fw_att_control/fw_att_control_main.cpp @@ -771,7 +771,7 @@ FixedwingAttitudeControl::task_main() _actuators.control[1] = (isfinite(pitch_u)) ? pitch_u + _parameters.trim_pitch : _parameters.trim_pitch; if (!isfinite(pitch_u)) { warnx("pitch_u %.4f, _yaw_ctrl.get_desired_rate() %.4f, airspeed %.4f, airspeed_scaling %.4f, roll_sp %.4f, pitch_sp %.4f, _roll_ctrl.get_desired_rate() %.4f, _pitch_ctrl.get_desired_rate() %.4f att_sp.roll_body %.4f", - pitch_u, _yaw_ctrl.get_desired_rate(), airspeed, airspeed_scaling, roll_sp, pitch_sp, _roll_ctrl.get_desired_rate(), _pitch_ctrl.get_desired_rate(), _att_sp.roll_body); + (double)pitch_u, (double)_yaw_ctrl.get_desired_rate(), (double)airspeed, (double)airspeed_scaling, (double)roll_sp, (double)pitch_sp, (double)_roll_ctrl.get_desired_rate(), (double)_pitch_ctrl.get_desired_rate(), (double)_att_sp.roll_body); } float yaw_u = _yaw_ctrl.control_bodyrate(_att.roll, _att.pitch, @@ -780,16 +780,16 @@ FixedwingAttitudeControl::task_main() _parameters.airspeed_min, _parameters.airspeed_max, airspeed, airspeed_scaling, lock_integrator); _actuators.control[2] = (isfinite(yaw_u)) ? yaw_u + _parameters.trim_yaw : _parameters.trim_yaw; if (!isfinite(yaw_u)) { - warnx("yaw_u %.4f", yaw_u); + warnx("yaw_u %.4f", (double)yaw_u); } /* throttle passed through */ _actuators.control[3] = (isfinite(throttle_sp)) ? throttle_sp : 0.0f; if (!isfinite(throttle_sp)) { - warnx("throttle_sp %.4f", throttle_sp); + warnx("throttle_sp %.4f", (double)throttle_sp); } } else { - warnx("Non-finite setpoint roll_sp: %.4f, pitch_sp %.4f", roll_sp, pitch_sp); + warnx("Non-finite setpoint roll_sp: %.4f, pitch_sp %.4f", (double)roll_sp, (double)pitch_sp); } /* From 7e621070ca0f002e2e1ccd863c31a24166ece0c2 Mon Sep 17 00:00:00 2001 From: TickTock- Date: Tue, 22 Apr 2014 18:23:27 -0700 Subject: [PATCH 057/329] renamed mission_switch to loiter_switch and assisted_switch to easy_switch --- src/modules/commander/commander.cpp | 10 ++++---- src/modules/sensors/sensor_params.c | 4 ++-- src/modules/sensors/sensors.cpp | 24 +++++++++---------- .../uORB/topics/manual_control_setpoint.h | 4 ++-- src/modules/uORB/topics/rc_channels.h | 4 ++-- 5 files changed, 23 insertions(+), 23 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index e7cf2b3fac..ee818d0f5f 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1231,12 +1231,12 @@ int commander_thread_main(int argc, char *argv[]) } else { /* MISSION switch */ - if (sp_man.mission_switch == SWITCH_POS_ON) { + if (sp_man.loiter_switch == SWITCH_POS_ON) { /* stick is in LOITER position */ status.set_nav_state = NAV_STATE_LOITER; status.set_nav_state_timestamp = hrt_absolute_time(); - } else if (sp_man.mission_switch != SWITCH_POS_NONE) { + } else if (sp_man.loiter_switch != SWITCH_POS_NONE) { /* stick is in MISSION position */ status.set_nav_state = NAV_STATE_MISSION; status.set_nav_state_timestamp = hrt_absolute_time(); @@ -1296,7 +1296,7 @@ int commander_thread_main(int argc, char *argv[]) // TODO remove this hack /* flight termination in manual mode if assisted switch is on easy position */ - if (!status.is_rotary_wing && parachute_enabled && armed.armed && status.main_state == MAIN_STATE_MANUAL && sp_man.assisted_switch == SWITCH_POS_ON) { + if (!status.is_rotary_wing && parachute_enabled && armed.armed && status.main_state == MAIN_STATE_MANUAL && sp_man.easy_switch == SWITCH_POS_ON) { if (TRANSITION_CHANGED == failsafe_state_transition(&status, FAILSAFE_STATE_TERMINATION)) { tune_positive(armed.armed); } @@ -1528,7 +1528,7 @@ set_main_state_rc(struct vehicle_status_s *status, struct manual_control_setpoin break; case SWITCH_POS_MIDDLE: // ASSISTED - if (sp_man->assisted_switch == SWITCH_POS_ON) { + if (sp_man->easy_switch == SWITCH_POS_ON) { res = main_state_transition(status, MAIN_STATE_EASY); if (res != TRANSITION_DENIED) { @@ -1545,7 +1545,7 @@ set_main_state_rc(struct vehicle_status_s *status, struct manual_control_setpoin break; // changed successfully or already in this mode } - if (sp_man->assisted_switch != SWITCH_POS_ON) { + if (sp_man->easy_switch != SWITCH_POS_ON) { print_reject_mode(status, "SEATBELT"); } diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c index c04e176a12..a34f819235 100644 --- a/src/modules/sensors/sensor_params.c +++ b/src/modules/sensors/sensor_params.c @@ -620,7 +620,7 @@ PARAM_DEFINE_INT32(RC_MAP_RETURN_SW, 0); * @max 18 * @group Radio Calibration */ -PARAM_DEFINE_INT32(RC_MAP_ASSIST_SW, 0); +PARAM_DEFINE_INT32(RC_MAP_EASY_SW, 0); /** * Mission switch channel mapping. @@ -629,7 +629,7 @@ PARAM_DEFINE_INT32(RC_MAP_ASSIST_SW, 0); * @max 18 * @group Radio Calibration */ -PARAM_DEFINE_INT32(RC_MAP_MISSIO_SW, 0); +PARAM_DEFINE_INT32(RC_MAP_LOITER_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 04b74a6f57..469fc5ca04 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -253,8 +253,8 @@ private: int rc_map_mode_sw; int rc_map_return_sw; - int rc_map_assisted_sw; - int rc_map_mission_sw; + int rc_map_easy_sw; + int rc_map_loiter_sw; // int rc_map_offboard_ctrl_mode_sw; @@ -296,8 +296,8 @@ 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_easy_sw; + param_t rc_map_loiter_sw; // param_t rc_map_offboard_ctrl_mode_sw; @@ -507,8 +507,8 @@ 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_easy_sw = param_find("RC_MAP_EASY_SW"); + _parameter_handles.rc_map_loiter_sw = param_find("RC_MAP_LOITER_SW"); // _parameter_handles.rc_map_offboard_ctrl_mode_sw = param_find("RC_MAP_OFFB_SW"); @@ -650,11 +650,11 @@ Sensors::parameters_update() warnx(paramerr); } - if (param_get(_parameter_handles.rc_map_assisted_sw, &(_parameters.rc_map_assisted_sw)) != OK) { + if (param_get(_parameter_handles.rc_map_easy_sw, &(_parameters.rc_map_easy_sw)) != OK) { warnx(paramerr); } - if (param_get(_parameter_handles.rc_map_mission_sw, &(_parameters.rc_map_mission_sw)) != OK) { + if (param_get(_parameter_handles.rc_map_loiter_sw, &(_parameters.rc_map_loiter_sw)) != OK) { warnx(paramerr); } @@ -681,8 +681,8 @@ 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[EASY] = _parameters.rc_map_easy_sw - 1; + _rc.function[LOITER] = _parameters.rc_map_loiter_sw - 1; _rc.function[FLAPS] = _parameters.rc_map_flaps - 1; @@ -1415,8 +1415,8 @@ Sensors::rc_poll() /* mode switches */ manual.mode_switch = get_rc_switch_position(MODE); - manual.assisted_switch = get_rc_switch_position(ASSISTED); - manual.mission_switch = get_rc_switch_position(MISSION); + manual.easy_switch = get_rc_switch_position(EASY); + manual.loiter_switch = get_rc_switch_position(LOITER); manual.return_switch = get_rc_switch_position(RETURN); /* publish manual_control_setpoint topic */ diff --git a/src/modules/uORB/topics/manual_control_setpoint.h b/src/modules/uORB/topics/manual_control_setpoint.h index 2b3a337b29..b6257e22a6 100644 --- a/src/modules/uORB/topics/manual_control_setpoint.h +++ b/src/modules/uORB/topics/manual_control_setpoint.h @@ -78,8 +78,8 @@ struct manual_control_setpoint_s { switch_pos_t mode_switch; /**< mode 3 position switch (mandatory): manual, assisted, auto */ switch_pos_t return_switch; /**< land 2 position switch (mandatory): land, no effect */ - switch_pos_t assisted_switch; /**< assisted 2 position switch (optional): seatbelt, simple */ - switch_pos_t mission_switch; /**< mission 2 position switch (optional): mission, loiter */ + switch_pos_t easy_switch; /**< assisted 2 position switch (optional): seatbelt, simple */ + switch_pos_t loiter_switch; /**< mission 2 position switch (optional): mission, loiter */ }; /**< manual control inputs */ /** diff --git a/src/modules/uORB/topics/rc_channels.h b/src/modules/uORB/topics/rc_channels.h index 3246a39dd6..d99203ff65 100644 --- a/src/modules/uORB/topics/rc_channels.h +++ b/src/modules/uORB/topics/rc_channels.h @@ -64,8 +64,8 @@ enum RC_CHANNELS_FUNCTION { YAW = 3, MODE = 4, RETURN = 5, - ASSISTED = 6, - MISSION = 7, + EASY = 6, + LOITER = 7, OFFBOARD_MODE = 8, FLAPS = 9, AUX_1 = 10, From 831a7c4a833c68b1d418344e2f3aae2c80894b1a Mon Sep 17 00:00:00 2001 From: TickTock- Date: Tue, 22 Apr 2014 20:53:07 -0700 Subject: [PATCH 058/329] Added RC_MAP_FAILSAFE parameter for customizing failsafe channel. Default to THROTTLE --- src/modules/commander/commander.cpp | 2 +- src/modules/sensors/sensors.cpp | 51 +++++++++++++++++++++-------- 2 files changed, 39 insertions(+), 14 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index ee818d0f5f..43a4ed8ab1 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1230,7 +1230,7 @@ int commander_thread_main(int argc, char *argv[]) status.set_nav_state_timestamp = hrt_absolute_time(); } else { - /* MISSION switch */ + /* LOITER switch */ if (sp_man.loiter_switch == SWITCH_POS_ON) { /* stick is in LOITER position */ status.set_nav_state = NAV_STATE_LOITER; diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 469fc5ca04..8b9aee7951 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -175,7 +175,8 @@ private: /** * Get switch position for specified function. */ - switch_pos_t get_rc_switch_position(enum RC_CHANNELS_FUNCTION func); + switch_pos_t get_rc_sw3pos_position(enum RC_CHANNELS_FUNCTION func); + switch_pos_t get_rc_sw2pos_position(enum RC_CHANNELS_FUNCTION func); /** * Gather and publish RC input data. @@ -250,6 +251,7 @@ private: int rc_map_pitch; int rc_map_yaw; int rc_map_throttle; + int rc_map_failsafe; int rc_map_mode_sw; int rc_map_return_sw; @@ -293,6 +295,7 @@ private: param_t rc_map_pitch; param_t rc_map_yaw; param_t rc_map_throttle; + param_t rc_map_failsafe; param_t rc_map_mode_sw; param_t rc_map_return_sw; @@ -499,6 +502,7 @@ Sensors::Sensors() : _parameter_handles.rc_map_pitch = param_find("RC_MAP_PITCH"); _parameter_handles.rc_map_yaw = param_find("RC_MAP_YAW"); _parameter_handles.rc_map_throttle = param_find("RC_MAP_THROTTLE"); + _parameter_handles.rc_map_failsafe = param_find("RC_MAP_FAILSAFE"); /* mandatory mode switches, mapped to channel 5 and 6 per default */ _parameter_handles.rc_map_mode_sw = param_find("RC_MAP_MODE_SW"); @@ -642,6 +646,10 @@ Sensors::parameters_update() warnx(paramerr); } + if (param_get(_parameter_handles.rc_map_failsafe, &(_parameters.rc_map_failsafe)) != OK) { + warnx(paramerr); + } + if (param_get(_parameter_handles.rc_map_mode_sw, &(_parameters.rc_map_mode_sw)) != OK) { warnx(paramerr); } @@ -1275,7 +1283,7 @@ Sensors::get_rc_value(enum RC_CHANNELS_FUNCTION func, float min_value, float max } switch_pos_t -Sensors::get_rc_switch_position(enum RC_CHANNELS_FUNCTION func) +Sensors::get_rc_sw3pos_position(enum RC_CHANNELS_FUNCTION func) { if (_rc.function[func] >= 0) { float value = _rc.chan[_rc.function[func]].scaled; @@ -1294,6 +1302,23 @@ Sensors::get_rc_switch_position(enum RC_CHANNELS_FUNCTION func) } } +switch_pos_t +Sensors::get_rc_sw2pos_position(enum RC_CHANNELS_FUNCTION func) +{ + if (_rc.function[func] >= 0) { + float value = _rc.chan[_rc.function[func]].scaled; + if (value > STICK_ON_OFF_LIMIT) { + return SWITCH_POS_ON; + + } else { + return SWITCH_POS_OFF; + } + + } else { + return SWITCH_POS_NONE; + } +} + void Sensors::rc_poll() { @@ -1318,13 +1343,13 @@ Sensors::rc_poll() /* signal looks good */ signal_lost = false; - /* check throttle failsafe */ - int8_t thr_ch = _rc.function[THROTTLE]; - if (_parameters.rc_fs_thr > 0 && thr_ch >= 0) { - /* throttle failsafe configured */ - if ((_parameters.rc_fs_thr < _parameters.min[thr_ch] && rc_input.values[thr_ch] < _parameters.rc_fs_thr) || - (_parameters.rc_fs_thr > _parameters.max[thr_ch] && rc_input.values[thr_ch] > _parameters.rc_fs_thr)) { - /* throttle failsafe triggered, signal is lost by receiver */ + /* check failsafe */ + int8_t fs_ch = _rc.function[_parameters.rc_map_failsafe]; + if (_parameters.rc_fs_thr > 0 && fs_ch >= 0) { + /* failsafe configured */ + if ((_parameters.rc_fs_thr < _parameters.min[fs_ch] && rc_input.values[fs_ch] < _parameters.rc_fs_thr) || + (_parameters.rc_fs_thr > _parameters.max[fs_ch] && rc_input.values[fs_ch] > _parameters.rc_fs_thr)) { + /* failsafe triggered, signal is lost by receiver */ signal_lost = true; } } @@ -1414,10 +1439,10 @@ Sensors::rc_poll() manual.aux5 = get_rc_value(AUX_5, -1.0, 1.0); /* mode switches */ - manual.mode_switch = get_rc_switch_position(MODE); - manual.easy_switch = get_rc_switch_position(EASY); - manual.loiter_switch = get_rc_switch_position(LOITER); - manual.return_switch = get_rc_switch_position(RETURN); + manual.mode_switch = get_rc_sw3pos_position(MODE); + manual.easy_switch = get_rc_sw2pos_position(EASY); + manual.loiter_switch = get_rc_sw2pos_position(LOITER); + manual.return_switch = get_rc_sw2pos_position(RETURN); /* publish manual_control_setpoint topic */ if (_manual_control_pub > 0) { From 81c03154b96cd3a087873de1583356df5fb4dc88 Mon Sep 17 00:00:00 2001 From: TickTock- Date: Tue, 22 Apr 2014 21:49:29 -0700 Subject: [PATCH 059/329] Added ASSISTED, AUTO, EASY, RETURN, & LOITER programmable thresholds to enable various user mode switch configs (orig., 2x3, 1x6, etc). --- src/modules/sensors/sensor_params.c | 80 ++++++++++++++++++++++++++++ src/modules/sensors/sensors.cpp | 81 +++++++++++++++++++++-------- 2 files changed, 138 insertions(+), 23 deletions(-) diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c index a34f819235..48e5d80e73 100644 --- a/src/modules/sensors/sensor_params.c +++ b/src/modules/sensors/sensor_params.c @@ -684,3 +684,83 @@ PARAM_DEFINE_INT32(RC_MAP_AUX3, 0); * @group Radio Calibration */ PARAM_DEFINE_INT32(RC_FAILS_THR, 0); + +/** + * Threshold for selecting assisted mode + * + * min:-1 + * max:+1 + * + * 0-1 indicate where in the full channel range the threshold sits + * 0 : min + * 1 : max + * sign indicates polarity of comparison + * positive : true when channel>th + * negative : true when channelth + * negative : true when channelth + * negative : true when channelth + * negative : true when channelth + * negative : true when channel= 0) { - float value = _rc.chan[_rc.function[func]].scaled; - if (value > STICK_ON_OFF_LIMIT) { + float value = 0.5f * _rc.chan[_rc.function[func]].scaled + 0.5f; + if (on_inv ? value < on_th : value > on_th) { return SWITCH_POS_ON; - } else if (value < -STICK_ON_OFF_LIMIT) { - return SWITCH_POS_OFF; + } else if (mid_inv ? value < mid_th : value > mid_th) { + return SWITCH_POS_MIDDLE; } else { - return SWITCH_POS_MIDDLE; + return SWITCH_POS_OFF; } } else { @@ -1303,11 +1338,11 @@ Sensors::get_rc_sw3pos_position(enum RC_CHANNELS_FUNCTION func) } switch_pos_t -Sensors::get_rc_sw2pos_position(enum RC_CHANNELS_FUNCTION func) +Sensors::get_rc_sw2pos_position(enum RC_CHANNELS_FUNCTION func, float on_th, bool on_inv) { if (_rc.function[func] >= 0) { - float value = _rc.chan[_rc.function[func]].scaled; - if (value > STICK_ON_OFF_LIMIT) { + float value = 0.5f * _rc.chan[_rc.function[func]].scaled + 0.5f; + if (on_inv ? value < on_th : value > on_th) { return SWITCH_POS_ON; } else { @@ -1345,10 +1380,10 @@ Sensors::rc_poll() /* check failsafe */ int8_t fs_ch = _rc.function[_parameters.rc_map_failsafe]; - if (_parameters.rc_fs_thr > 0 && fs_ch >= 0) { + if (_parameters.rc_fails_thr > 0 && fs_ch >= 0) { /* failsafe configured */ - if ((_parameters.rc_fs_thr < _parameters.min[fs_ch] && rc_input.values[fs_ch] < _parameters.rc_fs_thr) || - (_parameters.rc_fs_thr > _parameters.max[fs_ch] && rc_input.values[fs_ch] > _parameters.rc_fs_thr)) { + if ((_parameters.rc_fails_thr < _parameters.min[fs_ch] && rc_input.values[fs_ch] < _parameters.rc_fails_thr) || + (_parameters.rc_fails_thr > _parameters.max[fs_ch] && rc_input.values[fs_ch] > _parameters.rc_fails_thr)) { /* failsafe triggered, signal is lost by receiver */ signal_lost = true; } @@ -1439,10 +1474,10 @@ Sensors::rc_poll() manual.aux5 = get_rc_value(AUX_5, -1.0, 1.0); /* mode switches */ - manual.mode_switch = get_rc_sw3pos_position(MODE); - manual.easy_switch = get_rc_sw2pos_position(EASY); - manual.loiter_switch = get_rc_sw2pos_position(LOITER); - manual.return_switch = get_rc_sw2pos_position(RETURN); + manual.mode_switch = get_rc_sw3pos_position(MODE, _parameters.rc_auto_th, _parameters.rc_auto_inv, _parameters.rc_assisted_th, _parameters.rc_assisted_inv); + manual.easy_switch = get_rc_sw2pos_position(EASY, _parameters.rc_easy_th, _parameters.rc_easy_inv); + manual.return_switch = get_rc_sw2pos_position(RETURN, _parameters.rc_return_th, _parameters.rc_return_inv); + manual.loiter_switch = get_rc_sw2pos_position(LOITER, _parameters.rc_loiter_th, _parameters.rc_loiter_inv); /* publish manual_control_setpoint topic */ if (_manual_control_pub > 0) { From 971e8fc4ffc6fc50ffaf257c473dfa86f5dc2d11 Mon Sep 17 00:00:00 2001 From: TickTock- Date: Tue, 22 Apr 2014 23:19:04 -0700 Subject: [PATCH 060/329] Made failsafe more intuitive. Default (0) maps to whatever channel is throttle. If a non-zero value is entered, a direct channel map is used so use --- src/modules/sensors/sensor_params.c | 23 ++++------------------- src/modules/sensors/sensors.cpp | 5 ++++- 2 files changed, 8 insertions(+), 20 deletions(-) diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c index 48e5d80e73..a19180ad4b 100644 --- a/src/modules/sensors/sensor_params.c +++ b/src/modules/sensors/sensor_params.c @@ -538,28 +538,13 @@ PARAM_DEFINE_INT32(RC_MAP_PITCH, 2); /** * Failsafe channel mapping. * - * The RC mapping index indicates which rc function - * should be used for detecting the failsafe condition + * The RC mapping index indicates which channel is used for failsafe + * If 0, whichever channel is mapped to throttle is used + * otherwise the value indicates the specific rc channel to use * * @min 0 - * @max 14 + * @max 18 * - * mapping (from rc_channels.h) - * THROTTLE = 0, - ROLL = 1, - PITCH = 2, - YAW = 3, - MODE = 4, - RETURN = 5, - ASSISTED = 6, - MISSION = 7, - OFFBOARD_MODE = 8, - FLAPS = 9, - AUX_1 = 10, - AUX_2 = 11, - AUX_3 = 12, - AUX_4 = 13, - AUX_5 = 14, * */ PARAM_DEFINE_INT32(RC_MAP_FAILSAFE, 0); //Default to throttle function diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index caf0ff6fe0..28c08422e1 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -1379,7 +1379,10 @@ Sensors::rc_poll() signal_lost = false; /* check failsafe */ - int8_t fs_ch = _rc.function[_parameters.rc_map_failsafe]; + int8_t fs_ch = _rc.function[_parameters.rc_map_failsafe]; // get channel mapped to throttle + if (_parameters.rc_map_failsafe>0){ // if not 0, use channel number instead of rc.function mapping + fs_ch = _parameters.rc_map_failsafe - 1; + } if (_parameters.rc_fails_thr > 0 && fs_ch >= 0) { /* failsafe configured */ if ((_parameters.rc_fails_thr < _parameters.min[fs_ch] && rc_input.values[fs_ch] < _parameters.rc_fails_thr) || From 08e5ed5a1d630f840a838006bab6b1d1ae6aa6c1 Mon Sep 17 00:00:00 2001 From: ufoncz Date: Fri, 25 Apr 2014 21:14:12 +0200 Subject: [PATCH 061/329] added version nsh command, it can replace hw_ver sss --- makefiles/config_px4fmu-v2_default.mk | 1 + src/systemcmds/version/module.mk | 43 ++++++++ src/systemcmds/version/version.c | 138 ++++++++++++++++++++++++++ 3 files changed, 182 insertions(+) create mode 100644 src/systemcmds/version/module.mk create mode 100644 src/systemcmds/version/version.c diff --git a/makefiles/config_px4fmu-v2_default.mk b/makefiles/config_px4fmu-v2_default.mk index e13421acc5..7ec6a2fc68 100644 --- a/makefiles/config_px4fmu-v2_default.mk +++ b/makefiles/config_px4fmu-v2_default.mk @@ -65,6 +65,7 @@ MODULES += systemcmds/nshterm MODULES += systemcmds/mtd MODULES += systemcmds/hw_ver MODULES += systemcmds/dumpfile +MODULES += systemcmds/version # # General system control diff --git a/src/systemcmds/version/module.mk b/src/systemcmds/version/module.mk new file mode 100644 index 0000000000..3dac09239a --- /dev/null +++ b/src/systemcmds/version/module.mk @@ -0,0 +1,43 @@ +############################################################################ +# +# Copyright (c) 2014 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. +# +############################################################################ + +# +# Show and test hardware version +# + +MODULE_COMMAND = version +SRCS = version.c + +MODULE_STACKSIZE = 1024 + +MAXOPTIMIZATION = -Os diff --git a/src/systemcmds/version/version.c b/src/systemcmds/version/version.c new file mode 100644 index 0000000000..38730b10dc --- /dev/null +++ b/src/systemcmds/version/version.c @@ -0,0 +1,138 @@ +/**************************************************************************** + * + * Copyright (C) 2012 PX4 Development Team. All rights reserved. + * Author: Vladimir Kulla + * + * 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 version.c + * + * Version nsh command, unified way of showing versions of HW, SW, Build, Toolchain etc + */ + +#include + +#include +#include +#include +#include + +__EXPORT int version_main(int argc, char *argv[]); + +static void usage(const char *reason) +{ + if (reason != NULL) { + fprintf(stderr, "%s\n", reason); + } + + fprintf(stderr, "usage: version {hw|git|date|gcc|all}\n\n"); +} + +static void version_githash(int bShowPrefix) +{ + if (bShowPrefix == 1) { + printf("FW git-hash:"); + } + printf("%s\n", FW_GIT); +} + +static void version_hwarch(int bShowPrefix) +{ + if (bShowPrefix == 1) { + printf("HW arch: "); + } + printf("%s\n", HW_ARCH); +} + +static void version_date(int bShowPrefix) +{ + if (bShowPrefix == 1) { + printf("Build date: "); + } + printf("%s %s\n", __DATE__, __TIME__); +} + +static void version_gcc(int bShowPrefix) +{ + if (bShowPrefix == 1) { + printf("GCC used: "); + //printf("Built with GCC : %d.%d.%d\n", __GNUC__, __GNUC_MINOR__, __GNUC_PATCHLEVEL__); + } + printf("%s\n", __VERSION__); +} +static char sz_ver_hw_str[] = "hw"; + +int version_main(int argc, char *argv[]) +{ + if (argc >= 2) + { + if (argv[1] != NULL) + { + if (!strncmp(argv[1], sz_ver_hw_str, strlen(sz_ver_hw_str))) + { + version_hwarch(0); + } + else if (!strncmp(argv[1], "git", 3)) + { + version_githash(0); + } + else if (!strncmp(argv[1], "date", 3)) + { + version_date(0); + } + else if (!strncmp(argv[1], "gcc", 3)) + { + version_gcc(0); + } + else if (!strncmp(argv[1], "all", 3)) + { + printf("Pixhawk version info\n"); + version_hwarch(1); + version_date(1); + version_githash(1); + version_gcc(1); + } + else + { + printf("unknown command: %s\n", argv[1]); + } + } + else + { + usage("Error, input parameter NULL.\n"); + } + } + else + { + usage("Error, not enough parameters."); + } + return OK; +} From fd95adc710cd04855b8f7c2abaf73d9220cd497e Mon Sep 17 00:00:00 2001 From: ufoncz Date: Fri, 25 Apr 2014 21:34:45 +0200 Subject: [PATCH 062/329] corrections before xmerge --- src/systemcmds/version/version.c | 18 ++++++++++++------ 1 file changed, 12 insertions(+), 6 deletions(-) diff --git a/src/systemcmds/version/version.c b/src/systemcmds/version/version.c index 38730b10dc..37a51dbfc0 100644 --- a/src/systemcmds/version/version.c +++ b/src/systemcmds/version/version.c @@ -45,6 +45,13 @@ #include #include +static char sz_ver_hw_str[] = "hw"; +static char sz_ver_git_str[] = "git"; +static char sz_ver_date_str[] = "date"; +static char sz_ver_gcc_str[] = "gcc"; +static char sz_ver_all_str[] = "all"; + + __EXPORT int version_main(int argc, char *argv[]); static void usage(const char *reason) @@ -59,7 +66,7 @@ static void usage(const char *reason) static void version_githash(int bShowPrefix) { if (bShowPrefix == 1) { - printf("FW git-hash:"); + printf("FW git-hash: "); } printf("%s\n", FW_GIT); } @@ -88,7 +95,6 @@ static void version_gcc(int bShowPrefix) } printf("%s\n", __VERSION__); } -static char sz_ver_hw_str[] = "hw"; int version_main(int argc, char *argv[]) { @@ -100,19 +106,19 @@ int version_main(int argc, char *argv[]) { version_hwarch(0); } - else if (!strncmp(argv[1], "git", 3)) + else if (!strncmp(argv[1], sz_ver_git_str, strlen(sz_ver_git_str))) { version_githash(0); } - else if (!strncmp(argv[1], "date", 3)) + else if (!strncmp(argv[1], sz_ver_date_str, strlen(sz_ver_date_str))) { version_date(0); } - else if (!strncmp(argv[1], "gcc", 3)) + else if (!strncmp(argv[1], sz_ver_gcc_str, strlen(sz_ver_gcc_str))) { version_gcc(0); } - else if (!strncmp(argv[1], "all", 3)) + else if (!strncmp(argv[1], sz_ver_all_str, strlen(sz_ver_all_str))) { printf("Pixhawk version info\n"); version_hwarch(1); From 85cfeca194aeb834e0d3ede0bd5894c77a65dac6 Mon Sep 17 00:00:00 2001 From: TickTock- Date: Sat, 26 Apr 2014 04:40:03 -0700 Subject: [PATCH 063/329] updated flight modes diagram --- Documentation/rc_mode_switch.odg | Bin 22232 -> 19757 bytes Documentation/rc_mode_switch.pdf | Bin 28728 -> 30076 bytes 2 files changed, 0 insertions(+), 0 deletions(-) diff --git a/Documentation/rc_mode_switch.odg b/Documentation/rc_mode_switch.odg index 29d738c39c9598003b84fe5bd1565cfdff644ee0..70dc503c18eb76c1a4ca5feeb44149e8118b980d 100644 GIT binary patch literal 19757 zcmbTe1ym%>vM$=V>)`G%z~JugHn_VxH16)MgS)%CJA=E+z~JsOz+?V>_F3oMweMZ` zz5Z6!%FfC!GAgU9IwCSF6r{nx(EtEw03b*!T_gvLZjc550Q}uPs{l3@HYQH)_9h1Q z_SP0g22K`sw)8Hx#&mWDKnoz9oxO>zv7M2#jft%jotdM7y}5-EP~rb2@KeTrf3QAF z!gjW%7G}~_39qr5Iu^FQ%`P6kfS|39>J zva_@P|E}d}}1$739Pb zU~ynSn;=L^h$sR8AfH7*6ZGFJ%&*@p3;TcU3>DIzW@K@}tj%W56J%JTInbgX@X;HmqNUtA zrjsodHb{IAN))_x5|ewNZvRub%gRqk%72k+Jd;LjZ+1?s+L68;JB!uw?x#(tL=4)H zc9SsZ&)+UT_s_Ka+7X+n91U8P=W5Ecy2&Jk-*1>RF>#674vJD?+B8A8Fx+RHHzA`N zXw;(lVCdBHX&(73ze#*6k`vMLs9%UAlA+e0!>ve9xYI5KdcgcKm@WV zA=Y{%*rf*uN)A{A!&6sk!u%4=I5Og!b%Z znd5Y$nDv#xRo8BLFbeJX&19II}GVL;RlE0B6f^pIV_S5ZzRU(7O1`bAQfuxGKv&Q zVGQc}f<}D?V~U+L9m?8VVwtvODSPzxQ~l;+&G z2-$+hG%kClLojCJAv>hRK(k`_Pp(Gb&r?uiV7Zh~s-4(;sP1Q$-q0*i1sXpR0gPxI zQ+eLkT8e|@7B7mk-F0I}nJioh`w?D4ww%r#aH2g}FM&`U>g0i-=8-Ie=r;7}duEE% z4odOUH7{FVu{VCcn*d65xC9GNhVQ3U5vNrg2iY$=`FkR^YiM*%7xF7O= zSbNLHI8AB1({@;K3D}aE`G&GlR_BK-_Jt%ch@t+g42ifHKJ%Tjg$6tkcLaW;wCAo};QXMX#z06`Z4A&9>uTvX~$ z5`rY!U(%FB{wK+dOztnKgUsQRq!R!tz$9A_?)OG$JVGY@dFvaYk^w5;<2MtDK!`>! zBqIQ|FX9hEir{`kp2R4iy(fgLnBmBPfV0RqI6#H3aqOAiCrSK)_Aluz;M?b;;^FIO zIki^sk?@I}9z@G;|5rQj32f70MEj*;*`cMt8{TsJLTCb^Y=lVM+8^@!fU`DW5iua^ z%{u_Cmi_Y_dQgRzL~qc`$|qa!mx1)cSyrwHCsoO$y8`%u-0{#M<@JR=LHYS;Kq@~I zeYiMnaM>gv=ZT-p35z3w;v#n^0_?5J8K+1+jwmWY_{^S~NrX%T;ip`F`ITd&9&F)i zxy3Evw~!>egY%WgV4i`Q<~+f!Lz{j$N3mWEA`Ow#;M^xyt}Vfy`RkXrLf36`5!7H5 zoUPv6OICQYmPt-gKII3Z&vP`pI(dJ-1n()rzmfsr@l3EKOkl0C%it#m4O;RF3@*2I zheGrtkRiedTon-iP5%}yJ@RO1U`Yu>P#S=?4wV{SLj!cx+l>nccAE zL7WLD`YPSBw2!BRObyo~+sXBpVv(^Kxte`+DS>nrd00rj>$ z0uWc=O5DNyL+RwZ|Ey+&?uR%U=EF7z9i{9m#E?m2d_d$eV|$x_K={}%n9FX2C4&Z= zYP0Un1bhgBJADsbzjdfchH`_#-+>heeGF#hm+iX}&V6nA*>#Xv1Gp^sDjBi`HIko0 zl>(!0L8u(WjXyQWA2V%Igmme_&#(IX_-p7ceny5{lFzi2FTqcKxjb;9oEf33gspEh z0tIyH7#YZw%3g`31ZgY_W)#Sj)BEyh^>Q=6rdvDx9;9_{d&9Leu*#aUge`X%g}z*<-*8vdF9 z6_4J=o)S6ONOya=>g#DIV)y5rPFhRXKbQmpWJBKGhhbsmNx=Q00reWBW*^0BSLVI>804Tih!gI)>-Mj9~zh z&=&U!6jTwlTjODaFaTWtpq)4H5bZj8*&>Dnpy;eKgfDh?4?5$M7Rs_U(-Uo-f}M^} zEf4>*ofD-TK%#!9@2_mo=O*_W=V@IH&-s zXmrEWxn`G4tZ<7-{CVgzhIpgGJ$&sCV1j4I&0Z(1e#j(uwdW54!~E!8ILg|+j}t9T%=*F&ddVh@U#(>u zJJBvdhoUD$m_s#p#D`UV_J1I-cO!REbsA7;m(?F#F7)-abCi4B4Mxr{b6V;5?^?aC z`@C}4Wtg_SepVAaLK60Y=Q-{V(d&7A!0WY_$l6-NCc}yp6A??^Unnt2R3)lj9(IT4 z5V3-)FOoC#0yB?t6k{W_QzeC}1!}cP54Z#F4hbh*y9p~9sbrhpxTb%X92#t)-<^71 zpC#eJnqBVTChc@*_)bQfnGq){-4)QffJhVnqVl>?%cw;g`{LC>)Jy6YHy>wypiq%Bj%{ib3?2835HEh*9f1A&w&tUZ& z2StV*W;$34_OMMz`&d??H>{&#vV=V!3}R->LoffzPbom`L^Tu@RRl)0K>kP{${W9K z74o;N#Hp8nm>D2Z(Kf%t3KkJYW!StBdgY&l!WZkzJe%b-LOUaTC>^a|3`=0N22c>@ z4GnQ{B}8U)KB70{N1f4fbxi07Wo1 zxOD(T9AB-*3K5l`6Z1B0F6t5}ggucy)UkUm05MwDIZm;SK;K&d5IWJ25xh_hrH+dx zK;h=nu+yU_srOJ>PcHV`vF)-3-266m0$b*H`7DxG;*?vMBp;REM4fI!uji#(IeHd; zZLR%v7GcStF25-aNNblzm^&nu-x+F!#*fYmJp|py9M5HU4h<2Fj)L0rembP6w!Ld;b>LTL<3yx-Bni5zaDGYeIopy>r@L0qdNrXfJ$D~Yy9)%7 zJwAaSs+%9}Ki^)ev_EXSUw+|x`LKq3U~T`)y!w$rgvGQug}JYT;9@rsQh@<54B zx`=2ouICTDPAFIgn#pdxPWLYCJF^Hu?!sv}Pg}va8-+2}KRiX*q+Fy_xuA-o6O4Y) zu3VYnzIZu5r3km~eS9j%VP}eo!WcUXiq3M1A5uH-z!>i z>#v1>9IZXwvC0k+NU#D&74F!;;1l2_XAe2Hnki!m+wc>p4PkPXQF2M#pzQwO&uUk3 zdMm&PH}=dZog;7)f9oBmLq7Mfl?x-1siYKg3x8t`h6q9Lb4n(vXJ`E;@-TFznD8!Z zz!uuJK~W}lf{qd(!xxNdYAz{7o)y0+b5{_ zXi&+<0q{r)GMa@F1{@5Jk(EP`eXFzu(XAK9nF1w-p{&LSL^3(Xv8=NOeFMk+ zLdH4B6x5b+s{);&WgALFZ`MR?U3`aoBiRJpaN$#D&$U2zfh8)dhAdB4OCqH&+%T1^ zx0Ib?gvh&5eizJMm}^|C5QNvF=gnZ7Vfu`KP(=Vv*Ih}~z<$6P*az?2vA)yo4>9?tvaA9#Hs4;jV?L3r5x7JCRSwB0VQL z8|3Q!T>LS(;7%G$M6=79HV-&WW2&6h);(-Ggb)V0!_~^=am~TtY&(8y!LziCEZqz> zT`C5$+(x5o#;o7JxYLc?uyEceD=g>GB0^fKdqaILyL975!f*~R8hF&DXW%)WkW9Is z`jyW5+O8Ftrb*W_r}vC(neQ_&QMYvrt5pNlw7kYXPdqQC_u!@rm)i?G?)mZW6NMbpV9H~IBfjSar(2zZP4K+>f}+oY3t^6xi=ePEw=9!ZR{2ZZW-;@^rip# z8JR;1GaDb`6=(iUc38Azk3>&qIYwB=LFOI z9o%Qs;q_JA7JleD^}-?%HzRxb-ksF1g`3BhZ}urtB-vrCT8c|iv1%Um%cV^vX|Dt+CLuw2dds@ zNH2XO+Kke$g78&Q>z5$eP*jVv_fHJQuJK|{(gOy+R*brfZ&@=XC<4Jvw1)5X6TnNl zceY0VH280Z8I%uLJ2K80M0y#swZES1IDh-A{@Gn@U_L*qX5tGLHzi`u(0u>&wrspM zdGEg&ihOT~_LK1ag}3Qlqx{P~8|MdE*>XOF$vDnpog0iqIOvX>)o0f<2`;;iBjgy2 z6}uz59G8mLkt+z|zw`R>%u&`69Uyk@byxC7{7-BA$v*hne42dFw2Lq&q{whKkFLhM3$2J5WCUnRsCith}sxd6vmOrKGq0MItm%q&_}8n5HAxL!az2E z)`twal1RBD&r!%CR-Hqj^5D8Dk@9r#o8DvN6+vv`ZpC9scy_hWyfouY= zT|;L3p@aNq{l_bZeQdX?+)cVi-{CfyPrr2{ zoOu3pal?)K*jTbGPHrJsLS7h;hgm^qoI>;FB#Ks$q!P!GH2VwQ;&{5+`X;YV;LIQd zaHmxdRU;VqDG5j<2nb^(Sj-@Y{2v11&%Ord6?=A6U_GC~e1N2=oJh5he!%~JDhTeg zm64sT(|=9(oa>xAuD7DUZox`SMa~>Xr$OAM5dY1VGc$Q7axk}M9@?zE4F zfT1V3?Y|Ar_}xw=r8hV&RUI@tew@jRL3a={3S3}iPW<+p^`e8H^RprwVpwT;LM8>L z>Br_xyQvRiR^9LK74%-D^)#A1+|94AzmJF`{zQ)LA0NZ;hL6G>e5pKrA(NX#X9IHj zVsq3jOk^U5)`kXuiMk?7)QKBCDkn_*R=!V%`ABwTjYG<0*>nO$R!Gv=T4~_Y@tsk= z%$s+LyQO->Juqz&iu7n>%5c|F3B_zPBZ7nGKnH79SoqB{Q7awm!_~9FR;@4pwBd*K zn6jz-0c51~q6Im9os@jgbMZzCC34R-NU)ZxSa0mxrMXLpesjY)u-I%#D zqI~J#51x9899I(4uv+3k2FV2AuKkkRsh*wP#ROD6!&1LX$JL6clo zwQvEeb_Y;NzrLyZOYQKCxq~|@@-|$4XF%lznLrN7KAT4vh8%NH-GYo0!GtNUXc^b9 zpnhzK#Ongt>~7rT6${US1sVkAkeBAqut7XrvuW?2pt1d|1q!(k=r(gprkk_GUehHe zyrrX&$T&9SH58tY{5bBi5*9Q6f#6vcu@y-u1j=U!BW(A|lu3uE*4;%Un494b;FxRi z;BWJTz3(1~+BjDqCftGIO3nyj?LpDkeq5=mL?eu3&*MBTGeucjsi1oq z2Qw@i!eun?pSe`(4&U~A;_xGTz6)??nQ#!xHt|?iV&ld}lTMpy8c3R2qN84PQ5l-I ze4Qj#MN1W}A_E^YnnTmfNEC9mt!`12m2idQ>5%mvrbI4q<-~C(I)VG6Ac-O3^A=aS!yTWeuHa-mqp^fJs-x z9ArY__e45t_Gy7rfx)t})%!}ub5o;p+&vo|_Q}p_zZULh)Hhr!a%=0y^Oy@-8nW3Z zPPyMhA^R#yH`41^2Ot+MecpDZBTzS~S#|y>^by=R&k!*ci6K&AC4gk{ka45d9l2IY zQqcwWd(%ou?uP@+)+QH6;mb2=44OAbC^g4J$BzOrw8kG|mEO83f28VI7Pe%<&)O&q zeoAulw1i}N=Va`M&yWAYu<_YD`|8W;MgQkjJg0RdYd2(UT06OE-%U;%b%o+wM&1Q0 zh0Kea$pV5G$3Kg9#RLiFoZo!5_Dnp@t8!{CE55f*C!sN`v!m*Alqn{9hHb;~!I?xW z+IbY1zOgyMddDHnOt=!pYRo}M3cDWF%IIJtVn9i{^r!F8XmANJC#IAwqR@W&s9KQ3 zWaTOR`jM3NHGRHW*GRbb{W{%#Qj$|55c&^Bptalt0xG2|yToJxheV*hi5A@Ik5*ga zaXH8`&gm(reYD18d)x%L5Rr~N`Au;rzGh`8VXTt}DJ9p6`Yw42xzh1wq?1RhL*{HA z8_1dGKBW%^bguDh}+hvcJWgvi(@v zgbZXJbDw%IxnKP_)_8YIe6sLHNfx{n~vtK20B+Fq4X?Zk=yDvUgTlpY~Q zNhPa4y?Za%H%nkKT`!;)|9HLhb$ms)Rjf743>|{u80M87OI=GQQ_amdz9b=S)%te9 zOUc~JWQ;#=;!kjDs}`nCg{6{^col0yzS)N;gv1@xZ7`RC`V&sVH0ZV=u9Y3mfGo2p zxR6&b{*kMS?tB zDcsnxy62pmfOxw7<)rF!>>XK51>vtNMHd2bRP*$x$yK{dyDIJ9*EQGi1QvfRvEqsN zMoDEoxC~ycB!Bd-aMFtZGKonZ)NiL>N`BpeWo8Xw{dE?EN9B zLL|1BIveMrQ=g_ABA_TV<=233Mk^^s_$gn}^EBZ_CxjcUC7&~QC%-&Do#P{iFg;i= zbv2Lr<w=ADurti|n>wB!84M`%7(q$wx`>UG$MZ20xLL>49f6E>xzG z?m9^=H5{3AA_!yu&si<${Vh$@x5VR|1j$M-&5+)LhHVFdiBbDQ_tCfhjMWrb&~$98 z^e_<>C34Xva=wb(PIVuzh=p%mbyATxG;f|(78|mE%78sX%ZZ>{GnX7R=QYQaU)sld ztQ;UGqHg3b4l8HleChYVf+B2Y#|LJ+^y{49mxR@!ET;S_u-_+u+o8>`Csm?7oj2z& zOHa!+E!!fcTS1mTz3B}_GfN{%t*2>KnHz|ndb*mxZ9VzQ(&=yhAg=CPHoIvJwDJ}x zPX4gYKZ;Y@R2Q;5I$v#+g|MD1VlQ1yM_FEzGfXoSTWdFygZ)1CSa%uh;%JSBUWgMT zRkr&DSP-XS@hFszG1DzNJZ&^i^k6vVQovG(J1yxkmx#S(HylTs2VGM3Ya+G@Mk83v zcxZC;C8~^$(|C>$XWW+$&bqh-QkhtK62PjKQ^o@RM-8P$8Uhp<`QVkg_q-#Azo!EP z67UbO0AP&SIJw&wMFEXA06Od0p`PIMb^(DvkS`A%u11dbA07Y96<%3Ka&;x-yWqup)9#!EO#cNX*tDtuTpLX4qL{X-rh|s? zFAx8wS}dwj24Mbwwnwn8woo_s%!8>s{vl}oa*1u?Q#CW%f3Uq!?6FeSdeXwqdo`>E*Nsl z6ekPWRqIIR-k0zc{|tE!o}9F8`z?DRzHPUGWPQ(dgx|A^w4CF4KnxS7AzqxR1pDTp zHr>Hz5SZ}wH^{p*`fL=|W#ij=Cr{ZMiSG-|5exB^XlBA#WBGFhBt85KmB{SkXinHJ z5{{k;xim^JPVF&?R7a7sx-%4c|5Nm5{wnSVW$m# zca1+PEi`G5FDflmT&NomOAX|RVie=TaKY~?qnf%^oa$lgUu@xW;+FeEs;lQN_Y42$ zbu6f2%qDSd47e=q@z=tdTwD10!7A>Gn1bSRORf8}S@+k;F5|&Sx8*Hq>CzM4_RDB9 zak3rV=V2zYjebPo=JtjzVZwU;NQAd%zhK-_K z`{q;n0{ai=8LtPwHBG2StJ0>gbYh)kDW?L8L*6O@Yeq5aLAI^#1Lkm@FgOV!dC+zw z3DTsA5kYZ6`7QvzdCk+vsoH0Rr(%n`fH9k6b5>(gGd9<#s4)8)Dx1>Ia+>_y()Pm6 z14C>|xi89Obs%Ko?ulme&&%``sy0q09TXYU^{Uf|M9y79mJ$U4sNoh8mGIC~1M*c) z>PHS?&G~r|cQ!cX64tZ!00U5U>fp{XiRG6XA;C&OYhW+Fu^;yl-iDyw2U!%gL`A8Y zF;iwB2r|Ma-l_!db_d$Zk*kO+g;_9rS^mMD{V%<8j>$hkyn37f5C*^bzcBNkPej8$ zz&5q>I8h<%@8>`I1UJG&V!YPw1owe;?`LIRyxZI9@QYOa#*K5LG`H#81m=JLb) z3rL2r?T>P5YJ3p7)Ku@_oAmMxPuZq4og2$jhp@1a2>Nvt6pWy3sQjA(n(3~`D1AsW zz(25$A#bf)z{)db!hJ*9{IQAH3XOU?D2R&Ekqy-+5n8ETa+x+Er6 zE~{{dzwcH47r^w(39ve1UCENXWU&gm8$do+&P@}IyBq@Q{{=9>s_<)&{q7@Vi9)&; zwJtHfq~|rP28IIQAM|o63Jh)cyQJAVw|YfC+AcYxwVafao)2aUwH_V|Oi=E(^(y@f zU^`_5SM9N`GD&ZOmfLDi<^?o+vkhjIPyeH!V~xJ9naXY_$;d+}ev--6%`0~-GV46r8LsR3@3SMO9BxrmvIW)c)IVNa?9 zuC2%g;}Y-5r9`v>0S)qr-Aoukam`?47P3*tcx3SICz1u){i0;#n7oRJBvl>spx@1| zCO(@pm{Pi(nvU$p5@*~6{zsTQbf2j1vtL$oK2%aoESDoKq8|*9C-i#poAR5YjF}p0 zc%sZUS*2*xPqlXK?*3~?1shLm(?N?OC6b`Mb-wWbT`x*FP=P3n3;;Z1g8bKd(f_mz z75+2!05oxOvamG+{#|Q&uB`+7#WmOM>pP@uN$+x6#mPu9fvsp}Dh;{b3l4=V?RWVH z{YYszj@PfY8@B>L@fg$J7ATx2^n*LfiYw zi2b_X?d3OY#`wszn(xPpuP+%AQr_vdN4S=VPp0q0AD&RURVXY1vzEnkdNom;Z3-6* z)0R5?=T)1FEeat27JSF%h4;^3m2a^uQD%D80X#X|x_YT5&srKE-y};pWjTPu{H^5}n z_*O=1czeSG`3)KG4K^nZ?ta69Z_x?$cttc zbo*4Y^Rs@m`b-{`{Mdd*de*(@S+*N=1ri495}XBpg_|CG@saOVPV{#2^F%NvDgQU7_#Tib>bV7NXMjHu3+?J&9_GK zm!Pm6Q`3E5vD*{cl|XS_9iH=pe}#oR-jf}7_+dllN0MI6YshPF_KP=nR)~WOAMx2O zUDR73iI0EUMb>3^2F+KNY@Kl;e^#gLF?jQM=d+appAd5kdSA4zOVr`2MY;21WK|sA zv+Q|Nfno!+VV4Ue)b@STj=&H+Mu%7ZvL)moC9_-2B~I1w)l31xG7GK+r#keOOOXxBE#FD4#8r;+YsH7sDpLEf`~917B!FtWN8Pw)EduqP#E z6YTI6J%Tt}vPpso{d1fSoABPSVF#X3&-5l6lWtUZUoPQ`k4zOgboTZ+TU!!AwO8`< zxS8KZv>*BdzrOPtw)&~+k0@N5(Rm8{gP|`yT+lF7x&Bj$=oIZ~68FRwOG27jV zSy!H!(Q#tDFYtBfkql+WC!~H;D&dH4OUVnb;5K{s+$dDIWtkHDmD||I=TDx>qCz1L z@MGc_J$?EZ)y!wD8Th)ahN1XlKtvV;Zl#smzV;@y2P3z4dkM6sRRz${-LNF@?F8Cx@;+>`d|!Ux z2fkg_gkVe;`$!Mwp-*O^H9fc~W$4o#%TL#{4PI{@h!WGEc0M=-@LqgWhV1lHP3I@3 zf4sz`60q;5fkK_%1nM_eIUh>bnYMUNeOi!5gAX|m_cxLm`0FM(6a_*Hs1hAgNMYy# z9hRq@2PesY5Z_N*Q8M#HzQYTGNXUmIDh*LIn>{tB4mzEjAot@ zbMFswdp^HpntzdH`N}VpmoXC{d#gk8Sa}kiG_^B8BymF)dB*ZF^FjvO4+Z^eW_pX< z;plM4R_FO9O%OHa<3khru9}2gZd1M0@SDAncXzlxVfZG6>kL7tN)%Xh&9c}&n5@Q&cpF+M%(_|W{WBd(~MFD7lJ zx1Ea^Anlan1svBOzOzi71DEZXxjgQORgU5r+&7BzWcwmD?d!5C9_dY`gq;WC%hu=` zM?CMzWPDqqQNfqxmi4?-%prDA0*O%>IQ)QZAxqqxQmy`-EEx)AzO+^4Sg|(f zyJR~q%2;^P1fdnMm)M!fLmPx2VY#paO8uZ=!+{u1Fkex{Tt7AW*>KP;lXzpTcvBl8iVf4n&_ijR zP(T3! z3<*_BzsF}|>j_HoBPw>$+QefQ5C571@lhoiTe$s++%BYMO%={x%uKy<)uZ>8o<_3v zgKNcl!!I_+ya0X%J@}1L0KUgUK+?e@h9;J11Lx)223!!3s*&!Z@WR@Rx=tf}_&>o*3|Y3@JAex!f-KGutv!4beY z*RPa>qy0IFbmm@3BFH<7eDwnynKB@@i+m+7$Yr4P5o1f;kI-hfbh3!ka!SBw9gP6$2OFyr16p;B^_^sBiVVB3 zkC)&38(%{tXs3qPGRsNn{_7f|R-K&bgGArEdOkAL0*;n~kz2b`s^x`}OivQRgJ$b? z2WW!Tf}%xj3Jr=OyiDZ|@dfFHZ><$MQutdl3lIdeHh)w-n^9Zcy*$BRuHimiOK5|Z zrsH(oL1$Id7QLl1g_}Fd= z195f3ODL7XqP4nrEv2l6r%{SmHnq)dIf@9#eXqbQ%fUT4y;DUL9_(Zhjh>;s^Cw_06rt`{5!=u!fjU0xr zkb-y1Tie?j7hcrfNWJZoLvX^ICTO{q!ORHO3T0)xu(=Y?5uk138E(RvBI4neqLA5bFu((BY)fii-exZEYrwybaCmL}SI4pPvL+X((stS1i?o~01ocUZV3km5p3Gm@{g z>U&E^4vg5?wHTJ1D=v^sA>c;F$g(5M5sa1YmsWTZP*$cDZ_YqypA?pcb24w32Mzi^lXMzz0kVq@!TQIZs z=1*g@L@lS=dM04Ntsr1I6G?rOj~?sH;iZ8!-x& zV^y0C7UEpjl?6u(70{)IaRDLMIK|6swL6vTy|Ci|&4u{wAXL4;1NK0cI5q+lp(1UZpn z(DZjr)wSQN74H}-193~9Qguu!^KDxQ-K3f5l3o3X;f`hLXBvSXk~8;5~BfoY;z3zEcm6jX4(aP zi8IBrGGwK?<*6I@%sFsBI|yR!ptFocpRLiJRKIQcLSk0LdZ8&5(vypkQ{gk) zR@A;`PAsYM&=u&g_u=bajP8>qg5x6&dz_OUY_#QMIcx>@bNZIPNw5|?P|&wjOE%5P zX`(J+89Kv9eF4<0gwMFOCN_Kr8aIk0nAa!1fe528y``_S9Ze=z2Q%G!l%8ULl=xXq z?ahqzSS)y&c)u`jLG|wi=W_3Hm0|w-S9EJ%2shaKYJ0>~MZ1+}gTAHRCr1LOJUE>rp%SGG z|I)_b!kc+La_+kduD2VeD#0!x-r{lf@dgtlN&pKt>wzNZ7Yo7+r9_iCroLUdipSdIZ{L{f-!h2=m&EV8y0hY=o z2W!)Lsw)HxGRucbweLQ-Ql;^7MkI>PVvtFNVT@>myA>}Q;dzUW_}WmlfdUziUo zDnAZSynP;Y>B?{B92UIYzbfgBq(_xe8ucZHRZ6O7b3W!cvZ|+rGam1}SXvq~_i*R_ zFQWHJ^2AYyD2`xW^ zDJ?3v*ixn^^Qq&zu;*^#i5l7E9)@f^@Q>@t=L%-u>>RzF%I)|2uz9S#HnYR-1d8}$ zap23auQsC>>QK~LaI=P)iIMRyN>*bqxxy96^%_Y*R^WsbTTHNu*%qrM*y=7I|5oO~@=#PrKLnQR! zhMuZC_Nrd$?P-d?`ybgeCWj7gghrbPrw)O7(%gZ2h-ym-R?4WaNlJK?Hv^F~c6FR8 z#^v5&O|sM{UZTH^EyGBl8Z@OeLBKbLS~Hj8tL7tXUhnsh+Vx^*@XSQyVwe#wLdQ** zc$MkCXt7gLSW+T^i8mD zf4e)N@*+*kp9>ff-(}o&7-=01l>pN|0_!h$#Jb7a=G;r{f;^Lmiwlq~Ca8nTqmKXc z9jV4kQkqyrT!M@DxS#5KDic%k?RAp>97cPJBoj0Qkj`6zu-2{1Q;c1&t!s0~jcDQ# zRIp@eM44rDs!JTbzgIl!l;t(LX+-owwj(di`|JQg$RKYFNtx?~Iiby|Cs9!VGY%c& zXz%ft!7-%Vj)*@H`-hAAk~_4GZkUY*nwQgCagUVbwZ8{U8B@o6j=JDw{?3KCc?pb9 zdt~+HMR;fAforuOMI|-oss}g1NN-jT*cBl{`6>r_v`Ph~GI~JlgIesoVf0tB%$|3T zV;+k zlTqEeVaBEIS7M$*Lwm!%uU4H=5SX=+-p=?t^32xb9G7GcQhFt7)rk>9knJ}j7R?1h zZcpB3%93QWf%hSEBs`98g{t|~FdhS_)bL4HOr*A8G^Zr0O;b?qmql82or7-djw{1u zEeT1zsz?va`|}bE;{9X&KSe<|fyZSkG*S?Fi|X+TGSF_;^If^bFK(%*y0bB~>be5u zR5VxPgsv5V1$t0hOX&AInTcW1B~W%hlRBn$>G}K9CWg&+|i)>dwwSL|` z%p)eQNyult<37sd?m>r-8Chuaek?7j@@T3;n?ype{mtF*4P0{zH0n*3h=s1PK30Rq z!pj1JzZ0Jc;qjXsada0Y_Ke%a3nG`LZz$GdZXo>2j3jgOr6}dg@2ZZ-(XA6Z;aNEd ziZ2Ey5@9kM^Iw9Jq4Eg(G=8l~8V#VzC<|tG#2%nFhElNb}738D_!zV!{s5Vg6b#hsDL5bVZfui*3_J+>>-Zk{YECVtZuJPRC9uCs zqussWG7!%N=TZWGl>u=?%(i!{{2-Hh1iDV=((y_Qe`2S@rWZxG`Cq~bB_%1G7;Z*vBoaMv4!{CGC(*{Ns&pdYz zAF&W!(`>}RhVM@I%Tfy|t!;JBMwm4PMQB*Kw%k@uD;p%~G$QjWTSPBa2Pt!Qr*u@Z z4Rm3~3rC@XS53By6O1}cj5FWmD6dSvMriK(ZSqc{S_mssh;~hN7F()@{qlRfYX55T zqZxw=*za20(n2}!&nkt$C#eLEgJU?4Jo~T=PN`zwcDGZlaa13Fz9d-IzhVaGEWk47 zGqUtU9Pq}}FY%G(_LK+I@``+wyLONosY$HB2QHPB6Zst}MEvpy90|pS*eP+U zseAT#u?C5}q*(*xrN)qx$(f`SmTx~$LP#Cj1812q5e?^_+zek&j^Zz@B;(X z*GTUPvvQ5~mxq=25q}oLCdW#s5;Rm3zQWLS^W2we+C-vv7Nj3a?>MT}BK6*K(y=+d z>Ct!S)?}2P!HY9Vv$Op-hqtz8na0`d_f{>G9Tro3dew=Pc#yAVZ4W^_@rcDw4HO|? z{dqF7&_<6GpYZsi`NE;k%qVhf)-$^!CIpR7oJ30G#qah#9b7eGU1HTefN{kRJUPA^ zKn9cejPDPZUYNV1RW)DWCoU~8d)^E6vK!NklgOsM_kW0Z4;;|q6fnb<0G-zQ%{e_% zl<&RB?rR>;y}iiY9J!q3lT^FaPfWp*QHu5BM{>r^4)`*hqy2>0-*TKHioA(GY$Ed@oHp4`{ksg6$-Rb!p7qb(N4VobBV!C+B+Xz+3@9 zvOBTn@GjE^kl;XEh4u0gtFXv};=)9+kcH4=E|(lY=wxb&BJbt;4XoIy z^+KM}gvrwb0+*S1Lv{j@orpogCB+3~fN8a=5Jl@ZK94VV;)fL|`-O^J6fGUD-}=5< z_mf;-bi7UFUAKEOUkh|MxWW!6@s~T8!wXVF@CTSD08CY*Zyw(m3R$K|M!Ga!W;@#& zr7=1Ho53~rbA@KI)jg!{vC_Uz{H7a!2>;q9WP1Z@#-Di}Rv77F_?f|imxPgyfrQA!*2vEIGn~&$qM|JJje~@r50=Ny z)YQVrgzMi)Exvtbzc3*Bq}l?x{&k3##M#l7>!0)&T($-_CO|GHBd-6M2ZQT>w&wb$ zqkq-7SzFjz@sgN3IoWg3)4RI5(z!Cz**TigGjeir(*JXm&_Rfyh|FkkT zqW?^J@i)^3ke-o_k^bLp|7!SutJL4N|5GLV&wLf10{_Xy@qa7he{%k(EIT{9|7UPV zgTez6Twtdg85o%8L-;UP7w2Z?5Njhyt!{FDZUNApNja&=mL}$9<`QcRNG;f>$Ofbr zrKA8If*C15Hyi7x>!&1^BmMTHfXJ-SB#N?t>uwQh64otN(GB7mK1tMdE5F;ZiQ)4R=OB;PG>L7a4Q}ZB4 zr`Y)eMo& zwE|(BO-iyA%snQ$1}3^jW+4V9Rz?<91{QjzMmG8|d5AkwQ!+ubX}T%U(_riZLJSQ& zjLm&bjDn$NV<z{AN{*T7KM$RNbf+{(xd5l%>o zK)wJ6Z+?+oNM>@jLP&mcHrVYjQHZ(lG6HmHU#|5?PWmfhlZ)r2Ku2G>EUwJS=xg}2pgI*@;IPrsH- zub=(Lb?0NZZR6|e;}!PN`BWI6`hKHvZQtbmTg1GwHv|O9x1KZX`*tcxY}3ATbBy}5 zY^2xaH`agNB9J%dl_SfW_p9amZth)dWX3tm*)B|fvA`V9p4mIOHG?nx5Bt75k7-fS zkCz`0nM73VS;HK(`Gs4}@5Q(IK@~)2P48V%;6X9>m_Zc;Ba;XN?jw$Xfr|uS2Ogp8 zLQ(=guLi*n1TxVN5<;A61hE^r>kK=Z2z0Iy0<;5}=%(N}UQ6D!~#EEL9>e zfB~Vud`S1k;DBeTUF@ vzzKL-8L)x`wf11TU>-y+4nTz|0(83LC|U!&S=m6!1c1;C*qD6i0pbAwjzL(g literal 22232 zcmaI6W0WLK7q(k%+qR8q+qN-n+ty6mwx;cA+qP}ncAtLc{l4?>oVr$4)XKGYL}X@E z>^paaf;0#yDgXcp0L;Xr2=@k8xvb+dNW4@dvgmTCxw3%LPA3RtMI#y|M&B+LK9~LdUqS^ zs6^SA3`WG@s}Ho$N9a!gutxW08Gh@kREAVXW#mPHZI4)Eo#dAfTp;afVq;;X*g=5zw7EsF1-j1gQueu@pSvZh@p*TnIche7f& zwdbg=pL?Gx;lm)SS~mIXcg~cVJL7EEMiMWoL9-}lPezksRW9e*?eRtP#-b#;%uy>I z#R2Z;0y`?{C|%Q*za-TWa8{>VQc2} z-v~L-(stTqL-oD-tIy+OUM$=kxS^Tay||9uDgenAIFV5uq(~x-FZol**M|&56PV4w zWZkuh$G)cUikpjzi{0t&Wo4+vF4jRra)VMAEee+u-UaFAbg=*OkRK}thks*w0GPA- zP>JH62+0FJC|1~APL86zu|=0%NX!z|zTXP;Id&;fIUk6$&7$FBrXkeY=>Uu5R)sxT z2(&@Spg~Kwvg$A89dw~(gI4I}WiU9bBNUhj>9t4To71BOo)r2pH--s|rypP8y{h>S zgSu*oYsVT5Dr?^>F(-l@r zcErSyVx20bsCLjp0{ozxAuvc0Kk1U@^7#qXY4^5i2>5>rJZp!xn^Zg>j27>%JbMs; zdWh&e!)^%EH-~Rknn$y8H1dfXHlMaY7&`V8P+`KEV3P65zW2ng?P>eqc27|aAR^}1 z08;x{pLNAX{zNkbZ0?~01<0aFBe&}tNZlh{Zvz++kh1Z@Jg-QAQK;y1i`riFrjc~X z8B8ac^KKDM>Qa&5U+4(wgV+0MswbztlTJn%Y`}S>@qc8t^S+`__Y8}JLUQD}b=qO^|@FJjEK1ur|kVS_7-*T&?OXY)qK!yp6SVP!=Uw1NtS7;;voGS*K( z7o*=N86I>->w-d){i0dObL~cEE&{lkbHz<9tZ(7n$A;$*J3#56fmvW?bXXq#{`#Fx z)L|;O1WUzygANwiCvqY!x-tBP$D{e^e5{++mW|AVcPm-?@JF>58;TcabT$V_P%{|U zM#4PNhpbx1fA5UHI{NQQMgCBJr233=sMIntO)IOQNS;}&2EWqjR$S`+Hg1{`{iPdt zyscdZ+zki^>d{jY8-@_N=p}&#W167<5(>N~4y66dcwShbPD!P;0rMrrJ)iFbJY+v3KqC=syLOF{b!Y_sh5ugtpeMyFvHV$JDhMWVt3IsU7y z3*$Zj;q8*&O`^Q{YBoX$NDCXYV#kP2`|tH~_aBguI;1`IQ!&NGFWQ8-AHr?(a07aT zB;+Yxe9&Pey%m(5SKB=l9+=kUi!7HaDVrj#Fc}koMRxd?Tl7Zb|1Eit&U}aJ?768Eb z%p64G8aU3dM>6^-+ zFq-mWau3rwgZ+FO1nhrY7XweI8S2c#g&W3I5}ArUhYW#oLi)!vD>Xk`b!ZQ1hY|N% zww&B%<{nkr(NT8{PG28;1P83X)F|On?pCT_|Z+&4Hz_TzIt-Fp(2Km0zeMo{iw$sNz^1$kcV9a;dHpdXpBGIw9x+rasLHS?h zsPUgO^UpZ{P4+_To-cJ^000Wv|KDT}^F7%c+1Wb(Cmg=#`U~B&_A3V5aAT>YXilH>!rZU_=k>Emfnl>K2+S2^=1` zo@VkvV=XAJsrM7(CNv%sZJ%Vty$9>A#(e*sNK+hJ$;!1m99quc0;+qZW<_xmj}V`F z@blueuJKp)~ zuu?1a>FFY#zgfhDQny%UAYw$`TQ_^Rs>hnTqLr?A<(mwx-WG2B zbVVJnJK4sZU?Ti>S{Fmh^l`N}(Woa{-f7~n5?)#{4CkfJl0bii@GDm#$!r+htBf8= z)Fau4UXs5)oL|INC_lb=f!a1LTUCQcwd!;xtqQ+}pqG?>Skv&)-2`3%W1fLwG)yiH&tgc+9&=0zr zN`II!>P@?MBgatfds|PFoAX3_D&?vlFDmeuL1u?y#pvakJ6P9L58%+m97>x4((>MY z{u-pqsWY9qSUZF5s8YEl<>oCIbuHZ>p)F5d;c0ug1L3q#UdIjAQ+Zr16$i3nTmE5sqs9e+K zscXmIK2)e7S|3|_kV&(K*@(bDg;VKbj-Tr~6hKGhR_UF^n7b!mXu21O7(oYGhb&hX z=JZmkm1epk4HXf?uqjmy9=Pd9Zoab!bF?UCHfxdRBZnOx$ri&3v0`CkV$4n2nIb)Q z&ad06-t-j2ij8Usj^SwTKp9M5iZWlapjAIIUPQZQLeYjz6V}>A4m`$yBY!@Tb5{*D z&mwrI^KU3fJsIaQa{|20PEjy-S5X|j)AB&T(P|-us3!cI znWl#nVtR5$wUm+K`_&k3VH*0)Y<(K^!Ylr?^#0y`yi8LC%d+Ws`Yl71d>8V~nfy6Q z!;79?*k{trg1Vu1R=ax>Fsm8eU)&BBl_3V_SoKB0c@*c^@oC*CEvrOq3=TOrCkL1x zjY7zkXR?WIv9wWX&Ihle;EUkMld=;zc3g7?Y>S;tn{2RS)T_|$ZDtHJT{S;@k6gDd z&nF!W(`5+eR1_^Gd@ZGXZ3$Qa7i-hLjI%~au->@=ag>q}*e^nT@=+2aE2y!z^%7w@ zNDyzyV$4cpMqB2Ct=`+_zG5n8x&C`SiozCVI{1jJ=ek#4W0cyqAy&#Y+5-*ssnQkzm`9;lERZ1OKF*|hm=`dWxJ^27*f3MndC3^x>m0_40)8e6Yq*B7&J zRi3mGj|R>*IkE3nrhmramT{{7kk~&@O-}vv$&*uqd6`3Mu` zo!$R@DB*0+Zwy&o#P>pJy7+^y64Pq*vP+^NTDiOttHTW7vNm7X=)_+O}_M_q~Q30dK2vB&eTIJ3=~M8gTu z3G}E(ZEK^XT}E+3u_pQQwpzDAY#)OpQDk-s~Bx}6t$|8v60N83_+*=e} zw|OYyR;JzqO$qD?Aj90)1jxc_K zF0ftMd}b$lhwLo}E#pOKz3rp(dbj05=O@3wY0^dHEl%DiBlsb_`Tr0?gO(e9`u!Z6 zmPxMoM3SvA!EE+zeL#%xjObQYCDc9)Ey3!_YIh>t$s(yE2>K|&2`i4h9fM3=;)*Is zO8v}R_HOx>0S*?ZPUR=a@vOQDssphDvM^{1&~tCLe6<)>71Uj|Id0MP>jhnKJ9Z~` z6~EZe?b=H({=V4JJ1f6e7Gp88kAA`Auyq7~8fE>gQM`YxJ%n{< z@AC`meX2S0dzv)x^Tudpsfe)KtnUgu?+mn{@n$ekO}8j3Xf#W9Wx2bE{koeZ67u5n zVAn$qtPwu#AH-Na@VZZeyDQmtl5naMRZ6Vo~4PBR(B(!6Ty zz|F7iDmFlDTLDv-iM8M;Ef_?2F9Nkxf2=vQK*G^>e&sMh-K|>m)^ikYeMv1eBS{1K z^o3m^O}t@M&m78m6r`~TR4c{OB4C;;6p(oo6dxE8@73k4lpxEPhf7C4bxN8H0I-r` zJ7Rssl~DiQ`PzGv?`nqt2mFHr0^FT%aI}v}O*B|3+sH~{V3jeFEaB{_sfmAUlK&l) z66XJ4McK@gkQ5hsW1sqA@`*)={*OmkLf3nR;K;<;eO5TLP%+NzKg6nwb9t0iYM(p4w^!o9!@&#>F#pxw3ew` z%lhWwltyUBSl_&OpS--_$F4%nXW=6vAu-+8!W0nG#v9Pb^0a(Dv;01p%ISsyP6hjh z7bF$WPf%SnVcKYx}Fk(D7OqY{69~JyO(ZAwIEEBeF4#!)za!j;zum3-4au5-ORA zEKC}kY78>RAhObf&!6eVu*~3V#8*FpA=^xaHe|j_-Vj5E=fxu5;uUF4&v;j5Hr=JN zjYL98o)-tYq7WiWG+lNtE*RagP!SU|5vtdbnC=TzRrN?$Rn>xc`b@593<@Gq(hzvx z-!3pl!@Y51!TkBX(>g=-8)@8!{>j+j_s^dzvE;L{C8KS0fkab-vguA=KV>Fa*|1T+dK6iDn2*rDZfDQE7zyfkXWq4Sv`^L(*C0(~$tmVAkQl5(B2X z(;4sCEdXmp0Qi5TNC2;*Kd>MxKvB*4M^mR%T4w1LczDANsKJpHsO0(?mtpU$@wpU~ z986@MPDhAi`u${3DuE~h{s$LG5w?E~UEuUvUawRIlo9JZ4q?I>H@sq17a$hwAG{#3 zysk2kDSTH@&<7)b$*7G9JrYY|_W)r3Lx&jbykGlyW^U0RCRt(WPdUmlWy7gC;YSi! zP&F$LIqGw_^8YaFRtDL2iDalM8T`#!0Q+0r$I)gJqH3ZXfe_{&Uf_gY_wKR5k&W&O z$?sabf*OL@pn?7%|HBhP+gW54L;8vXpDqLTq*(>|6?oMAA4C`? zpU@K!E)cFRM`wDyL4FwAp@w5Lzx~&Ui{EYCzSLw8i=Lubo*J-!maopfz7yjRkW7zv zC?H7Rxf3*?|C1>Hzw0tvjNA@(v;e?=2POajP%(G0F|;+Xuy$f_{=Yxz?QPA%6y(I= zps}I9P2eOYM3ex4@4Pzz&;SYi{mrl6C=3A5vPp^vs<>xfWI=hOn7j^cNiruGJIbc# z+ek4;*-<7Y&q62Gtt(JE(h*YU<&{?)eX1|%PCfMNiXtfrE`gzmAc`tdK5Zle+tgrm zT-D2_*QH4^e@QA;C+CAnWyC(c1)LZ&UvXc_WN*FkUGdy?_WiNI79iY&F7AQ>1l&_7 z01*FzLj!gf1b_tEu_*ze>U$T!&zA?q%P@LUF{qDjf`lKxn_%?&+fC|`&#sgNUg7L8 z1oY3VKk0!B_;2)q2q9-_Ub+=>+4o9;^((s6VVjy%KyMUyQ~@a?UmiGVjCRd=E@Okr zPRzia+~Yr)!#)md?|5O5rQ~y%rwM5-NOc_vQ}Bss-ZoRKIb2L+A6%G!@nrW9*%D)HG;pU6Zq6+7@Ajm+(qxM9*C za7dIi-%EWwAFlzPnWikLrn7!b1(Jbxy$E9Pv({#1NtZ<&M=+YVVxyr%z#6Y`QVy%cfVi(hfiCsTP<>@v?ds_SU3ZNSdfXcPi{CTrsa9 zSp~Kl@4eq&4z*vbn&Rit8o{^&qa=omUzW!mOWGD5jYxN3(^S>D8`tB0-U`X67gdLv#a;csI>5L)r7G))Ow9 z)W5uyx4-%YsR5$Vm>6L^!KdNoxm#g^!i)IoZU6VF{7*VKT=d}sX0y<6NUxF41m{5K zrN!GJ6tLro6ocE}IX)~&ko&|k+S?fXJuz0>(hF?}nJcx*J1~fuFfyiy%&hGlm3VvM zfw;Qd-6A@du_trR;<8@!s2YR)vqh`B#sxIdjhMOC;~Pl~wJSim^yo`eBp|k* z{q1NmNZk#YE+8j4*p9Cqw`osU>`SySYhzBFQVwNCKy`SYwfqp)l_$_0S3HoG0}S4=cT=e5p#2Z3{4NfkUMx86lYFZww-M> z>Mw-%hw1_Kw;da?mB8X|-g$?r)XRj$xCRsoLQzEnXPPVFNXPhTVmGp8F=|RAO71s9 zQl2i0+Zd||N}!FSSE`kBKiIgK2bXZr@bJ|}<-Mq6KuJJmw&=_U%? zl}+S-=D!0UGU$Nayg!>fMgyPI%TC(+Fw!DY2;G7@?6uSFoIyuOCZ%w{-oYSJEK0!p zuKj#HBjs90#+rPRLcoz>lsvQzj&h{aIHKQe{CUwpXM{nAh19d!xHEV6VEPLYifojC ztujhV+H%DwwzR_s4Z3b1p2IKcU`QSZv{yMz^{GALO8d#O5z52?h>5V^@R>-_zGTrl zW#_h^2}JttjM_0~r^g{5!aq3ty>C1~qEXch2EHM&p_qq9S;4!;e9O&>u4XJpBYF3R z|IVh5r3!ZM)$Ih(+<0#k2Otg$RXrIO!_SdlT~_MS2W|k#2(p08+jF<_Lo!lry%Dzd ztxM7L)5-}7QlZJaUf^7J zX2X8I@iYDC+#7|k`m8Gw;3Mi)hgM)ER>DUbv6jUXPeV*RVN<9QlQGum+EKsL1x5Dy zx+VX?>lN(3!r>sygpC9#)n_7BJ`zs*m=HEo2FbLpyOk-6wqs%% zP)*rW6Eidz?{YRxcC%oksSoZnvP-^??PQEHf1B(<4J0NsBmB<@j+K{2l_~omC_=I~ zK9j%(0PMu%6{pTVhOEWM8`kByDfET9POd{$HorlP#018vATrhS{lrp!XvYj4ADWb(4*?64E zW`h-N4oKKr$dD#3zAn?zdB9-u-bChWiz~lBt05(l4Xshb9YvpS?Z{r$@42Sv_ZksSQwmXu%MM@*h zt*1Aic`OJjfL*nJ4D9&5?eJ=I6bHS_Zf=u()AM;fJB#zRMAfcM4Y;M*_+-ql^;jb& zi&3o`Eu1m3WSrkY;d~6lIt`ReB~Eoq1sHMZLNWWN&lMIl1~85Ma59lbIlLK;ENxt% zEn9FD=8=_%Avx8ew@Uw0T&XdU1=gb?^x2WO@Hr34`+2%J4FBt0#9!5k z4vM5K&5w&Air!+1Iu;tZqDF#6KRgzapsW=kCE$plA_49Qsx+CBQo5Cgg+-JF2nq(D zi&rv()ANU`PCj2`ySvBs;lSARxubCd!GQJPchBinaw*8V1N25vEE&_)aA`>3I<_P< zwM6p=z8Tg+n_3CrmWK)4o3z8&qXfsUhVs^qvL>_GWfQuG_7wuV8WMG2y-t$zRk+e(;0nb8o zosUytWKhKlZ?EHy+`=M@!80fKZWXn2jhYM@4*1M7A|8Q_e36%9y(RH-s`&n0v8dcg z%o>Ggy~56`$PL%<0sN8q^(paXTCFJF3woqN8%9Sn>Ssvvzm@s3D#VNqY=B0728$oc zxQ3y28F9S2M255U!`nYF6W-C@BS*%I4dkD;%Qn@*X9&^aO&Cdb0~~=U->7N!SH5_( znm^O7ZXeKLMlye4UGFoGylmYU2oT@JIdPsw@cT5mlvtQcRXBxkr)8XgF!JxnsvC7Y z+)3LfY7$e@3kHx9_qV_i9BUbj6Zl8FbZc}Om8$4g1(WfP5EziPs3{~yQSVRw=-rBA z$_0`yY}KKwErbq2)#0mPq1&u6GfO2OaM^hM35YQnKd(Kwcp`B6)a`rsdqx^s-Tc7Q zyLh=lTN>?bvC^d(hj%l6S{jgb{bhF$ejko>A3)+j!_{SJn{{_CozVJw;%V__Si(N% zBd=*spo-^B3A}^YC`LiCXX?yt34S=0*Ed#ax{vgO(EsxmbUmCeAwXUCXj#%N#rHZ! z^p3qSI_GxZi3UP{gJz3F+n+nQ(+=*^S}NKEo8-`^)m9)>%D4g8-z5hWI`=Dp`IwG+ z60K$zVumyb^5eif2Dj@wSls`r0v+96j2m(0W%4L^hT8{HmLx0gu*3!-w~r1_7@Ivp36ywM|s+%GSG{ z<-XwFWSF1LPgDT7fhvIWQn)rderPkS#s$dJIe1|}3NRdt`7<9w1qhSN%wxm~3>SGi zBcOJpch`N_&X=a=9U#-w^h6_7Sys)&O5I-98n?Zw84z<0deNH;89&2v+uJ3fdZf zwtrPlZ63YX-$)DJ-=4r^zdm$QQ2fOMwVIk5d%|dE0_^ZJ!a+UW+A|Q@-?m zLjeq8;=eTrq0h*2QVhG5EGnQ+`R8>)ZFg$O7jq!-A&zwVu?0)wIDBaYx`7X$i|Akd zs?nEAeWq=(f^I&*zLq}6pWZMz90jUx72-BdyuJL#Odh$7;7IO>t%Orb(xWio@!RAsU=ypY9mFD1Xq44%qD{`OY6erd1m+fCAJu1^y@G#DM*OBJQJe-ptH> zSzQa#pW6_8E<^Z@9gk$`up@tqxd_Z2|DLMRDl zOw2>`s@RiVikM#=nG!on)v)g!YFXA(UlV{!#XTE$qgx*5_A>zLbNT%s{S-({R0wRw@{M%Gz)E;_!*)O_oJlQEL zpBTpV&E0o~;-dGDgBfO`wb5rcUMXO(ZOQv<@|dOgTI-jd_|+}eW@@&)5Xp5dz%)!N ztn*AD}Oo7emU{>r`?&0vK0Dfz3lDO?f!zf$$K^{O# zn(8*xPCA8y4Wx0f#Rc}}mQ`9x)v*kTIN-l?WACUl1xWX`L*h1YJ&!8>1$@k&d(sAi zPvSrMQ^5}I4&=kw@6!5p^KLy5CI(3WqpG=I<$t<)s(jc|ITAB8$uuS7_UU427J!8C zlR4mP0OQRXWbm)AzV*>aeASmxhLd9|S$3-XS>X>8vpW=If3FO_;|_-y{CTrG6(HxH zD$X{erN-sld5gtgQi6t*s$NCZ=YPaat8~@dZ$Epkcs~|=uHKc~jP=`*sL56tn_g$I3#n+AR-;nW`$xDVm;y?(qwYm0QC;AZ}s32^2EBZ(5S!~0$l4Kn&`g% z6S=!M5>UNXPo6bWWWyK;COKvHS#}6z3W!Ds_<`MDvpYy|M+chZql5TD2xD*ygs;lL ztdco$6eB^s^=`Lu{pIEGy&>yu3!UWtPLKBGH9uZ+hGKDX!h=ay)yY)eYxP!faKg?n z>1h$f;X`*nqlX0)P<6#Noy0u_%HMW4)B7-mYipY0^EK!FlauYm0w56)1gasU@}^91 z&Zr>%80B*wMKAfckfvzTRg-Rs!FC4mtBDOS7EDT^JixkGg2tqZ`=()VDDqC&P>dWC z{zHRGGYRM?^+eDN~zWzZHN4n}z(= zH{zIFE{7BmI{Jm<%0P{$FRDFSASQeEOcvA3 zU;^Fk`VBw1q(H54;mr&}lMbC?D|0w6dna&W*wb5ll|I+oQsw)yTRO~qrJd2gl9mD% zb2i@L@X|A{Ki>4P^KoiLW_0d6Ou3bd6^ywfXi`Q3WzilYa$b(4=lI{(%s8zGPJ$+t zww%Nv0o%w&=6s+7Syx<8NV>1Pw_|eN!k(|*6)@^K3qWf%0xyBTzTTQxw<%HK16ryG zj6PDjgxmwS7EH-Z_xClCI%;TgqAX@>NyF<|bqkYuNklA|W&1Rrea6r*`CNVK(}_n) znrrxk(9QOZ>_jZz+S=JivWWcGgqrYTa3(69vM&jLpJ9Oah4~{cWh8kzt{D`i=vi)bVKw{F|HxCwZNb?Gdi$#`UE?>PF&DRw~I#CJ~0x2v0!^J&`E@Y$24*K(xl5`si`T zDl3+tNYQHu7A?EUlp=LkLCtHgb3J>uuGk3PZj3w5rz!Yv_Jz&;!w<|eej=aFx!)%S zNt=F=hex_{@kFodKls_?0@fjm8eLat0H{bT%`ZoLqYg~15n6Cq72T(D1cJi5pvA6g z;!#jso%|`ZjcSA_K;Yzcybs1I6@U4yve>LU=)QhQ4xT#npoyP2i_qyCGVmXmx-W| z!?T*ps-n>`-Neb<)_FbB3`Qmrhmt@f&MHz8V6I8%2x`DMMX1!o)uYWHri8%5Fs9l= znb!UR-Fmr?t#Y3VS!0OSs^|HkVm@!#0y>aG7ZC<8AX`O8tKvhM z!|$^1|MNxCHYdO}nn&k>GMjJylKfb)qw{&wjCu;+MrEn-u6R;2UaR?IM@@~pwriH1-AUSG}1O37cIK&-(wod(1{%MZ_#_v^^UxVL>Le31@E zUNoq-KyqY&yOVE@R`+#Vi0doF27-QYLI*!4sgX+TI-<$%UHRwm@W37T!qr^AaU^OM zk7^bVb1g%@SCa%guzuM!oTjR#SM6%_hX`p=eC5ka%c5ySqBTcmTg?8bNr}i{UOn4r zLGiwlTuU6?U+ai0^al16wADRk1wN*cN-y7AJRIfpN+hRPH4x+suFj4`P&G!wRaC)0 zKPxzWvF*vM6Z;Xlt8aXKK!6V@yTM_hM;K;m*2+i?>r|1Z4zwO7>JwQ93K(i}pC#p4 zHpEAhe)lWIJN*%a1z`UmGa`0di)Mkesi0p9_g@|bVFTSW#C^H;@C)a=Ra-gkc_2?d zlu&Go73SQ;FKE(e?hdza!9|68B%#Sz9dHaAkRsB-AIu{bXA=FPh#3VO^Rm!G;8RKe zL**jwsZ(*tkD2N0V!}i|hIT;nV2vKUvq3{kSGnO%ukFa^*?2GP-85%bYqZvi!fN7- z;p6sDSzDWvM!iE-=j&uZCj{_%l4%=k3%YH;i^4e>&Xe^L3Xm#R8|s&@K3yOs6d^v$ zUyR>OZ*={O0^iGPp_G@~>At4MS0`GN-+fkhe4yPb@r&jS?ypbvdxrW_xZ&Je+cUe^ z_ZVxfMsRtR08uHh=Yfa_L)91>iq=Me)Faw)lf69UzhB4ew-<7V(#Fye>M{Aug+7Ak zXu2De8%ZPkdcumEE+{psO6UBA3_aA@Ow%-0O1{~3diyE0l!bm!q6&lSR|q{Mp56Ok z%($b|?%?aS=+(m{os?_>%D#?+@+ucIPnYA^Y?6TzP5iHxU^0`z3O zzyv1CZ^_!l$9K=k^d`>eA}4yF3ok;QA%;%(i@@|I-Dt1tb7%F|PX2YqYaS}t^<33` zVG0h!0P$}T1bytrjWcw_2DfnbMWc1#g4NWkkS7KQaRtqPAvcF<{U>KO4Ld@sz-DmM z2P>~*Km{Gn*VO^Y6x|^EWo63n@9Sh{kYI6G# zUGOL#gVEl&bsK^Ui#~-w91hN(9WA7e(0|Kj(hCJ*>DjX{(vAGQx^5=?CTGp5gaFQ{ z)8>*;(MhH_R6fMm6=vuPnPE8H&GkoVQawZq!9OOy1e1= zHbE6=F9AfO%8(IC5q4SOm#jq^+1Rm!IlILgXVLfa{R>oSNohVWG4;}wGYO!m*Yx!B zU3Y7x!NshkMo9kvYK0lpYGKDZKa4DXdSb8|Y0o=KVw`j!Jtq8_k2#5!luo$a;=_ZH zS&n>Yp39*^eh-0ymYDghfa7+j{DT?;{%9usmf(Ah&(gHld#^=i(2t$OK&o zyatD1nqO{?43pKd|J2Wy>HdS;fg&{{g~+a|JXzdaRP)XkJ3Iy2ueY+jzr0T&-PPSq zWs?*N<`o=U&)JjJo|@3P4M);&KSCN|0#7rQ6b)g=%uy`Pv@I<^)?f`w3)_2sQCGKS zTCQx29kPHW^iOIr3A=&&r+?ry>FkFf>deCX#z33}EvkvHO5bcdR0&(Z9BY?L(DT;V zxYgLWH;YtJ>)h?OcF&(Uwjir4oUKhPn2>pT!I+v`l}}dn3z@8E3TIx2_l{=v6^jU` zbUCv7d^WA-rU8u$E@fFiBr%=zM|m9eqDQ--kyrv4D9T$ps#5P2YqhB%ipbINC(#EA zhltUl(92XY?uS*h1@U;v0noW#t#&|&)86em^^OD66Ki+GC_SyV2(JqXM$E4 zltkmn+RMnsjA<4$^hA*3t14<5ig^ofB6Eq2Bg9bqi|kJQX6eG&@}nP*Wpf2H4@0Ac z2t2v3TaB~&KRYzbEZY=HfoWXf7`GK;pXgp)^!<}}>S?jl(oQP)pX?=#_)nt|_6jY?v)9SP38(4K6C_QlR9M z;N6c$WR<4j!mu|sEVacnxCzR1|2G>BJ0yJCm+g8uQcvw?&x-WI8$)k=>qk^8n|~mo zkg$TZ{(~YatC%pB4aK4{UnXPIy1#(dd*&BB6j`A;7KV9McfWNC%rH2}DFb3j6(nqi zWO(nsqp5V66%Ibe4h}cVPnita!8r?D=p2TsQh&I+E}S;5VAL$D`#sqfhM4pZl%A>2WB_$U>0;MO>8{ zh1FaiG;9EQ{vVSy%H118fJoWgRJ$Y=Q}g=n#4Ad%SNq~FR?iX=TwCk!J(b8@qh0q? zB(mgH0@H>~TcKF)3>%Q$E<=M4{-oixib3lwV$a;dWvtt!Dk;T+M$+7-(jd#eh0ar&X^AS6BCA;$~oemCQnKyeC~ zAmOC|JMi%aI~bMb@IeZg3*YEUPLrzEs&=e_^dLBJ?S2aJ*NPEtA54@M-7g^MBngH7 zn~JZ(n{bS1eD=IuB1v6w{xb4@lA>~vW*tRp(~%_qy!dkQW>(8e6{2Z#DzCYgImX_D zANRvxrV!3{Ye~+Gh1)0APY4A*^_>=xB2ZlCO7EFO4^mjbm zd-3(fi#C>dMugiC=I1BAaDF#+ShY6UMcjK3u5^R;+x-Vc8Ct?6{*i)y3S18`N^aqpKp76-~XI01v1Ii(HOkxxZ~~_;j3`9 zALm<@;x_?Ad?swRy4hhc(7^c+PGD?47X)Jbv*^$|bpJJH5rbcmH{;q7Hm|~-imEQs z>@FjO90z_kb+mND35+V%+#YBPI?Zke9UcxRIz4Tc6HXB&Hh5N@LD{Bgg@TBD)P~uM z_Zz3@7n(em5wD*knE_88l!YJW2PSnDKC4;dhWRP>j$@RTjO=2`f;CjYyzY%#;@+e@8`6+TlU3>JETLz`u|jN-QjR;UweWC(S?ZKdkKT6 z6QYeei5dinZlaB02vI^3y^DTzqjw2LNDy73MvLBwPLxQ(H&riT-6|lNTiIgM`OyyNGS;9CH}{N3 z?`U>krj}oSQY~@NOIx+$Z-Xl;yo=7Sh!Wo}BKZ>p-bEgbW7kG%YVjhONI448B|u?j zQ!}hh0FEVXd*fF5PohhcUXB4aBXt3bgeWub@mYBTWZjdQ%4iG8m#oD&hzXt=l#S4W zWIh4dXY>`C->Kck(WiG0&bI;ud^+%=jA`BoeO}3Z1z-_>r(Ch{qJ_#L%2l22(>>=e z9{3Y@=>t+~cJ+Jxsl3Yxbk!AAqs>x3b~fgG=UV62J7{BbI(b1VV@ODn#;BoL^kFoweno%ys}m=f zBDJmoU%0UF*-)Gn*if|!zQ;mVGh zRn#%FjT!>!Fl}e}V{RYB?-5dhD#u6(gH-`d$&B zaN?!dK<{d->usg8TgLUZI0@(IjK6H%?s;=U1K>AaYP0`H);N3hY2ugd8L5FJ;x+NR zMpBY;u&+fP*Z3D%{jZE_aCMc-4CF;!QWYR`0szgrXO9?V#EAuEN|f8>i^93YsFwM; zE8FPD-HV*_MsE8a5z{z!gGSNar0b^6U-qRY@TTF8uiWl`n)9$$X$yeF%mHh1jA3CC z7KZL^HQ-Fv@fOOCfbcXaadXv5GFBYfEaU=Kfu*DXRYHj*TQoy*K^6e|QalS0bPUib zaNi$!jKje+xQq|h3r%ZUiutgPg?i_U5@FC%(3=-Ai0l=j6(g?ByvEcRyE|8rvN5y? zjejqdh`gZO5C#s(=MXK{VR|U44H3}a%eFO{fl=c5Y(0+(=k}$h>UvMnBqMIs@7EX# zzap1eN=W(iqeV>B7X|NGdSS5RR4<9IAZ&u6tZX;oUAR*OeW*cg)7N29!V1=htAxvD zcj?K4(ra+mMPzq(9w}eAW6~K6TibeE5WXGx2)umvlTh-;;9{HNwoJBS;5^&xQSQ)l zwLY_#a|$Il__vVUzKNw<&!UVEx}-@v~6pXD!d)Yjk%{1Tr0O z#TVWr$J7(6C_)rUwYf^ar7h5rMN?!0-z9j)P( z&i@Y*Q^Wn&@L*>8&*Pnk!i3}ub)M`F=Q(4UnwwkNS)y0Q5q{kq0f+x@A3pZaGIaFy z08G6&#(uv@=S{}S&e7Dx@-$@UO$Jk$?(E`jXZcg(W#Zlhe5{K6&89YNWF;|`AVDVj zidvg%uFSwnBD5b8WAd1ZoT4uh2N#E*_qvln&$kp#iPr!XUvh5ka{63|m7SB%lL?ZL zi>`|Iz1`xiyZK`%saj%b|M*(+E?Bv_PX9j2=gmo?QO%UG4}v@T8Q6|GB(K{bLm62` zT}XFFpa5xVf_lXl=ciX^cvZxDP)kuwvmz6n6!oitVl$Q-a|&!t@7*hx64j-)d|c}y z>Se`mXWJI-ki(P3_uye6r&+;{acq=Ar1Ctwu4hm1>rg7jl5w4U_=}UiO_uU9sO2VMJGWwR%z^q#7ymVd4sX+%$2rwh^yW>5Qva$r7?&bapu9JpYyf49N}X5ll8trad|y7w(DsvY2*wn8ic8bhzxRj0w zi;}@oIYi9ZtTgqE? z9lAI{3vqq|EUWZsN1z8<5{{#-U$dwmP6hh(YU!MOiV<$k71emm=t<5u=zn)me({(H ztH&K2@{l4DVunRQa${*&a#jfB5Xmmwl5LOFKPF9{-2QG1q7bB?tsPkEJ1W&G zZ++0}3YQm)UszRp4~8Xmk*G@^In&l_*@b!~DEpAQ-7VQXge*%5Pxa1hMB>V_Ug+=d zJqg_XEX2d!f}0Sfmk(O*se2%n;j??C*B~^_p(%v6?1;l?yhaJQsGHcwKMECB8vl9~ z=VVk_wA#6YW$-h;?#0NKHE?vfS=hmg2ir%<6NkZ9^{H*lsv$yZHYuWt3L*BDT(&A* zFTf7GW708Na{lqb@(*vx(v-4B;n!Gud(F(9SbchCZO|4(zAq`^mZHEXCP+bsL}_02 zo8Ilti&ib*T-87tB(&fc$L6U9Pi`gPv#M4C*IDw4LX-!OxcbcH54Y-?S-ZAoIJ(xEhT+x(H z=T>+H{Fb<`m{%!XC#c1Lxr3H1wkXkG^9t`V86VLSvLF3Xd^M@Wmx49}52DL%3iA1m zT*CiMR#V)YIp|OZy=3FgI1o1w`Vr{u`B%~SiBrn{{m|mvnk<$UqgGpue;}i2% z@MT@-HNhK^K!;b|Pq#nhN?fz2$V|(Y+ljy`y<6sUNON_sv{p-c^m98mH1q>5mT71X z&Wr?&xsaY5cikAtHZYEBz2T<(4&tHHd#r4cp*`FPQ|rAz9H3hZ1!^^`&)NYu@o}Mx zSN>yS>TEeUo4;K-p=!)~ycpdSOLYnVAeR)^y)08QKGD+glOG2%#tjBf^@VZ~aFfW; zk|zVjd%Um)+Ah3x*G*zO{YJMjPxe~1LHtoO?~)$CW}TnPco+Z@a7ewigB z^GI};++rlk+1Z?Y-KwFR^Drr{`nDfjg(EegyZW0gj_Uh#q{zbg| zFQ|Vs9K7w*38NbpttX>zalvwk%gm#Mj0{4rs?la@aeqdrCRIV0gR#e-hU659z0|w2 zIHG2+;aJEi{6tiZ=%&g5@>-JF0H7~?7>85)juCmDD8a;V*@UDG-L-FaImChfV~it7 zyMeNA!vuleZH#HA3JP;gJ3!>62eiV)0tT%HKtN6neG^6iURdpif%*%w#GNAn`_kQhyHQmEr6)u$MQe#aNBOs}&JmGaame}+nK+u7 zn~DDTt=%-b)_Bjv>Bq!K%uB(Wjn2w)BX}M(!tzj}X& zMtyYND(*+pCf~LEBgxi=qg1`aJt{BPe$!|mjm$x;Ir$f&q+J+cw_rkeACxw$W!XS$ zaM$By}8Y{Y>J;8+vTGcb?6^K3EpUi{;5g8rvE;go2ln zYoyzwKctC}(xqd!a4;xL2P-iSk>9VeA9~1c5!yCeADb>#YeK}D8{030ny7z>3t`4j zZhme3ja_06alkWi!N+=&f_mUMuT_y1Pm8;Pm0W|o;e7$J&P#@nE37im$uZU&=qRur z-6A0CHe*{q2rg=QHEBNnuAlaJE^YM$8`GYLjl5S77CFs}&dYEp(HV}0qRuUT z6^PPxds7Ffm8J7fk)vYmMu&Gyg!Ko!EE~SXx%lksru}cyzX6o~pui`@(iZiRAvoilyZw(q)>*?0x4D5JhH_Gc{N}0Z&(wJeH77%e1w(geP$}Iw7i;hp{aZV_K zcEJ@^bb%_F@JKf33ig8~gzFPFpZ%`Hl_Y)~yHN-2i9-+b41ryzI>f=~CrK{JD)n(} z6i8}0v;a!j3#9ZDHPnP&;aof9OSq2MVJ?7)ND7rJ1z>B{6+H@wpH?^HDaF!|)$vrm zjXT5u{j@h)6JYLoKQC0nPk@<3e4JSbLd2AF#1rQhQBMv3!(RPf#>-0t?0cHabaPY5 zKO$BeU#n7c;!D$(#N+Y)$n9&&+7O!^W}NW)8Po#6LK``{NpTVz6VwY>6oB8WQDPF( zGdtC$JVl)@#;q(?lPl^(=o1Q2rJ5t8NuWitx-F(Yf%i!i0ZC{@S&P(X~1= zBlPgUs9c=_Px;bcn>x>c3Dm!D@%*coGZX-TVO?hiQu;f|_!|fNbHvZU004%Coteiu z&VJ=&|JF~%IevcSX#dvF=X3o0%GrMJ2gBOVj0ODnAi@5`;ePK4LxIlB=p0XHobKOx zi9W~68OQsNm(wR5GxjM_!?3(FYd^=!Z#dsSJx_nWK}O}jhw6;+{dsD??v)tMcV@b1 uFJ}z!boA3(`gJ?N?4)NVhJJg`-B<6ZUA%<3j1>L5ftDBzMRz>|0RIEb>V_2n diff --git a/Documentation/rc_mode_switch.pdf b/Documentation/rc_mode_switch.pdf index 856dd55c59af22e3595048dcfb5dd322c8cb2326..0dfa43cbf3320e8ac2e3d8247cb67dac7b5885c1 100644 GIT binary patch delta 28991 zcmZU3V{o8NxNL0Owrz7`+qTU&_Qp0gwzaXbv28oqaFd+xRGqqiZq=acnSWjNRL}Hu z#~$dv3(!PaK>trXMLS~ei`vq9zq`aIHjA~frx#a{Q#|(gz<4D(HH#(K8g^gca6+o= zwuV6|r|Z2bXy_FwZE`YAvg&%S&HLxmrFj{(TbtUm`;1zqcrfs>dVagB@GUq${2lo5 zGI2*-_JWT7Cuo-{c;G`boP`AVoIO9ig~R{;*fg2~Xxyo!9|b-Q17CY#K94aT1XP+% zwT~ilXE5P;u_S5cc{$c{Kiz9H3uf!$lpFO&H3AD_6cXXjd}HQYcX!KhLI+t#AB^^E zS>$>&5y*Qv<2*pQIn*nI>hlQg|0a|Z@2P!h5xneAMH^gD!Sz~y92YDa31J7DygxiD z%||lvg`p8zI~8 zG5I2Jx4F9KS(ao8?d02|t)^uVU^Vd~x(_4s*>}m@B`4COU>1N1T9w$R1qB%G;W}?+f#>VNR~3E?(La0p8%A~>MuQVnP6VuK_>POWI49ca z-nhI{W%!sbx+YM+JnONW_Lc4g@(-f=emLMY?#{qRmY{y*U0-_z4hLe-<`ozaHSW5Y z+X)1Uu-`VA(o5qs?vA;g2D{!M#-Sqs&mj*Gn@?MZ_dU>)J8s|tl`kThIc59;#~_#f zCbscSrYp~=|pLNu)2{m?R&^P%LXp8h(52+$JL7sf)+SoX}i zT2)~*x{Yrl*vgWj)&{KY_&)S$S~-xh-Kf2)7P&$(l+Aw0>U3gZ^_lqhqHsL`vTACa zL}SYaQKVCPq1$%bQH{Caj<2 zqfGVQ)98Fin;bj4UV;0tZi}t}a2ij|A^39m!wSH`9eH~G5HRgiB8((%>>J*h2~;9~ z&Vbonzg>v>WO{z?2Gz?I^jZOU{-f`>kDCZzT0mKG*Kt{!`v6pK+#)UV1Ql8N=D~bM z*gEJx3P|{y7NdNLGq*UlXZ!hxGw1%>ANh!JY_m17_CaT*!Fowqhov+8Mepj;n%$dl@>A z%1<1j3KkKyOZf7&udY}^5gRxBhtDFNkT zrBl#LDFI{gThJUSKtS13t~{p%6`5z#um7#={APX5<*4lR3wG(g@Z4+s(BKx7I6MDC zL+TV%c<&)-tu-`Vmvt2j~^#i>Xli}}3 z5VUHZ@Sk(euOIn(#e-lj$aO>UY<6X`3&`sBJ3qr^!>#ANfis#@D?H$tPq7Q$^{=xq<3-{JUb>4&mnIm1#Zu*z?BkHM_&c9xjGw zT`&9k(hUF|KKPgF4X#w3aHed5vq>BB-ND@bV`y{l7wd1 z^G|K)zRP*h-9gCN8Hz=4q|r>JPByNp#aAV^E|VueV(eX|!EM#TtjL|@`!GYn6%S6M z@+5A7J*QTQWz|L}bdrrFJq3|Lxm!ABvOi$!?%}x5cN%xJt062`-9ax6li}X|Jz^{4 zra|UV2l$qJChJ+e`KE)Vefj33UnP~25MF)FW}}AsJU&+aR(0xMw3eRh(t7m3MvrOT zP`}SjKbw7e3gb@;gpeH%6O&%_I#mvHoK+c)qBi*Hj}m7=xtdfV({ns4<#?gBqJxf+ zdQAPvZ&a*Uu}aL7efg1aybds%UF)@2S0vtwfC5=4|DMLfd4;Z}k!rsue;-$Zml*I; z^kM}RJoh4QtdM;X{bM!#I-wjm(#M!0l1}aZYLFNLcd#1~S_+oG?&SP~(7jVn9gu+V zn<_o*mEhE42W@==?GC1P*OU|DArs=`WKaD#M#t+4RCuf>S6d$h{mbuZz5TeIcw>Pb zKrd7Wc^~UjyGmpgAHU6Nwxri>z5aI2>ME4uaX9Uqe27{%>r`%7$pH;GrqTVHUxM%M zE#PVS65^!JIgft1`Uvyn-wUMkDj$G^y>q5C_Hd^CaSDabWQgIm(4ac)8jR>jaZQKW zpcs}GUW5)RpNDK1PTHy%7WcZsvLg8g6p4YxK`nYqz@dS#F5N%Wf2s@@bNQvj>S; z^Trf{As&tK$4W6!Anv&DBx~VeRsNBZMeA)A{`+qKAvOQ}Q0!iYGRk}PXvrvMzi1LD zoz1w)uifA6cZ3y(v$yar`r|QI&KGD+o@uq|0Ng}1^xaV~$|4+A2}wYL@3&x)NUaNl z9kisZDq8%MGzYUlO#)j*Px=KEi^;BV$fu|~YvKqHMjUC?i)(`(M{3gH-g(UX9htvU z#ci!AbQDN9?FIf#&l{3E5!ZpH#OeXd`q2OJaK@C;Mg#G~Z6d}c-ghQ%hnJo5+_ z^YGB{ZT?(!Ldq3=;?)*e-c__Tg&%!i3r7I4`t9`h89TFXwRttKoz*O0&GgB**_R}j z(O`GKaO-%Ca=;{9=1(^J*SVrET+TaH+evbHTp^cnAB_o>ur>qc*>^%N?)Y`Yqy?dV zUJJ|yzF7WFrWjvrZ>W$Ob-C#v0OqF`sd^@yk0s|tB0d^6Ei%KYIk|&QpquI%#BP;m z@3}VAHqRMf7SHLzMJ_#H*Cr8s;M%WunmaU=TZ}xXfh)HIl?(x z+3q+)Fc)&h|GK4@tSjnjF#8A6gO7qGVoxx7?Z6M_-f2yIdbE zxNU$&3}& z#W*HyzOou7^eeMK5xtb1T-lT>`)Q+566x)*r&{Q^wmq>z!k>|w5s#^qPLop08-5c5 zq?Fo@-tn!3OGoOulCaH(ufq<<8*smh?AVu#4oCA4_4MT!iO_drkpz8{dJKP*IqU<% zge?L$@@tyQIj5HgUyxhS8{unb?MJ2}u{$g+GsXNNZ-EaWdCVw|F;$%S)LZeZHsf+D z@y;3I@1v#Cv{cK?5f`dg7$pqV&DXlMu5?^wuc@75MCvW?*wIA-;I5WkmOH?*+@ z*}`lor56rz?kyjg!LUPj<7RAAyrIYCCEMk5A`_>ZT41QKnc5@Nr5Z;&rlofw@^4%r z&*CrV-2H0+MejFW0z1S0ywt!d4}Rb!S=>BC+%IV7OkPj6Z|9RU!K&DwZ#Ae93kN0C z-Ff`aq$iiIV@%W5JWV$ysn7PZ4VD_q<5is=+BpTQhDj^l`*Su%UHe~(046V#id zS2P08kAxp(n9X8dTN+XZsdTHm_IcUL+XxQ)V)v?n`kMEUZY1qXkSkl|=N=|jON)We zKg=K(>BERK63f+bGpw?B=3{%PKNI z%1fvfAFIs$)~8uC?Rv5tA8W=N+nK%CNX13fzy4ziD$VGPqo#7a_DT0+KxYRevS~}! zZ)0xfWBH%Ip;O6qn5+=`A{=UYh{Ugtu*pL`0NyEWZXchRoOqniUt=#x(ZgLFvWh10 zM_sx^@D(v#Vl$fQI76Pg2K7{Af}n*)(QSgCA$OQl%l*2O z$P&;YRBvNAnwf^OL3IPn01u7K>S_=TKr6h{4HIQ>QHzC+reK6~xT?h6-y~d>3~jG< z(a&4s-+Ap;6l%u`(q(Bw04v(7p?SH9_oBc_5$AE(eb7G2+ZPUT`{{@U6El-rUDx!< z1)Io-#Vg&GrH4)h%mlWbBWQC+4Vg18N*r^IvaAHyZ%-sIACWPn)~K+g|f}>9hz0W`1*ruYdJ^2{Aqo{j+EiMSNvL zA9`NM!`Z6`AD5S&RpvQcX87Jp)N~J+)5q(z z3y>cu#&)g5{`@2mPPum1GBevZ^Zi;;$-wvg_6QJ%<}R8=y^}^z;C`OIWt;*^i;<^V zfh#pG8ytWa`6X{xy>B2)LQ$T0$yDO?P~ev&bQ7 z$Jf1;qB4Q@wm({ue2Tz-Gn-*c(^Ezl(T;^dEYO^1qFS^ymxc?IKK9B$qg& zOyBg=S0PDUTEjT?jPWy<0Nsu`vg+laY#q@n=^YIS1kx#(qJ30k@E^b&J>e+r>T>6$m&Qp*>d>TaZuqqFX-2iD7@Iv)MGiGBJ%Zp57J zBu)&2+XMcMo$EO`P`3A3-NM@N$r2pB7d_Q}?F$w&lol?LPEtr03q-7cp$g{fVp{~G zbQjRG%_Usw@{GsKm<+V+Hl3I|CsM=pH6iB^>B#?lzl^ zKGz4+xXhuL5~M^$evmt!vo2=r>hx(H+WUkP%}dP`9|C9xdDr-=1S?*0jn+O9q$42; zKt&juK5S6{nZDsgaTNWc8N?C4k{PqFwjE_qUWn7GFk@mEa%M1B|*!z6u!oCoTf?m(gq^$t0ywrMiQsJujZ@g z91x+`ip_c}YKEe~utVD6FrpZ44JIz5w@iS7d8M+K-3T|_CfiW8f9RYgqgKD1oj4E54)E{m*T4pjXVai-TGv$B%}opJ zU%nHC8_-W;gTed%st-sUSpu^K!HEoxt;XLiuO;RDyU>~zmyO#}njTEQ6Oucjp%c0% zQrId{;o`ECmKHZDD?jqfVNA>5*gj)`0cnkl?*;bvb^bo10isJ^;O(NuMxc>FEj?s7 zA(P)CIUwUEX(m2l@z-#1`Izr2P}0Fie&^2p+WQ%0m9+cqJ` z$GxB_peBx-aO)LiU7`7|>HTbqUWI@q+qxG^2-p!)yPs%5jgU|Mv9~U8$n}HLRMEKq zF|$;bHURMg)#8OPyBWW4>=EH@J+N?hz%+gzJuF%Xe8|zk5FL*_kwsp$ge$4#iW(frT(J#Ji~n`Mb>+e^ndau0??Zv8W4T7b)nL z#z;m?5cU>SIW#Cfi{z9P?XeL;pJkGjth5*J`Q z#|>s27M~;9szu%fx>ZJS1xs&~)zW>|2OJrb^Q|8FC7|gOR)&_UaCy530Sc_TcJb=?SN_U|eOJFxDshrf5SQW$FI1hhNDxhy0Dy zbvjNPJ0aVT^}%+$azz6^^(Wr=lH;JCW_BhF8#^xxvSvh4;RXJ;0%RU_*0Wh_9Iz`} zdzA!r;Ly}OEI5}na`yK%~f>k8D0>JaMwUoqRsWpu#TSDaW^pz>;qcN3ADu-Y^NbX~u zsd0~O%J6UqsrVbEx7uxMHkK}b+QoF^=Mh}1_TMV8*C~oLt$kk_AEMX&&c`3-ic5h! z{DZ-5=RR@yo0fVJ=yRD`H<=mvgJ6E+B^FOF)QZPK&;h#j%t}m69jigDh|-`3EoaY+ zlwNSs2jYgU5Bt9g4siXSShMo7as599=lTB>ocF(qDzlW0y}N}gvy{DwyM=^>nUlH2 z|0z5x2|EWDFPD%IoSVC=g^2^4ckWh?k1^)*I*yQJDp6TWi2tXYbMS*vX*SBdF* zd&_He6J~t<(bISPaof%PGRAfT(cdWXm8+53smbLpDzP(< zum-2+3naCViaVD6JM{8t11_gVF1tSzVMn@aMr=;=z((iJR>L`r50Ug))BNM6?kPdf zF&4AfwNXDO)NEvoUGu6O3?ohW=fYbo|H{+LcMbkCbJ%AAqb^_f-;d+l6_OU^h)XudS zf9s{b`#xa&R4@Wr_h~VX7imJ2Z`581OE>NPZfFD*+mGC=xAMM8pE!Jx+DlI7rp_?^ z31)nWu0;*bR%zm-ilOKDo&s86qphKqwnC@|0Ii_++e7O*c#RV2pmZWp=)&vLGu;;K zaA%4a+%BZ;;T}+{?Xay1h`a=}a7QClT>EN^4l%h}F}wQ^(ouG(Q5t z;AiK)nZG31F-7Zlf{7ohuk~96%ECN{j_$2%711UfN+ z5rN@I`xIZiI&{{9Bm-!5lReR3)+E_*Ku-YN%s+_l6n*&J%MPu#`Of`uaPQ6(Q~Xb~ zPuwwQPNIbJB)Fzjf$>@qwk;PD|Iu_rmNuk=-7L8e@_4Uk@kJ*`^8t>El4lkFyX<1 z{rWrMJKB&v`eYtjD#|;BWXvO3;341vgcoL4{0RPmnQ1!B8MiLpZZEr|bgo)qOY`;S z;A?v9HPj#JPTmhRqXo;8X;)X1b#YE zt?n_Y($%2OlAi9lobePu>G#PJkx@_ZZfOfZDM`VPi+tFc1_4-Hi2s=9SidN}+ja-i zR-^EG)BhWSDn6dFeRhIuEiKVT7(%0Hk20Wkli3M*yC$8`WBoJ8; z=_t+VP56!>2&=C`%798gLh^-ZbIOe_d?OYHK;_pFWtb|$6ym$Ed-c6?KQQTrBR$Z) z2Ys+UeUZGua;FP30rG>7!QgeG`1222P%WC7GvA=I;sp5-s{c$99WOgX{aNkn!7JI& z%{LtPZhA)jG)q42`KfPOGmb0pNiw|Qdi0vG)0G->!V9Vimp%SFDU9JF>-#>u;s z=1TFJFmJlPx`(tccZLW++AZjx@fnP@X8%UYj>B`)b9Dms$OsMVLH;9#T|$1BYBWg5 zbo5-5eZ|ux$epbRHTyxDo#{uX6{f)<+z#_onz@LY0<={Y7w1s%AGG&QwF-V#>gC=> z({)fJKL|XQF<^%|UZ7Wls#c!s#F_8qIK(@25s6e;U5m@%EaLGUsF5`p^U&m8#i65W zkA;avanNW@K>!E!Pe}a5cPm>?!(0Z#JKk8Gla9A*ii{J^aua7{TSwi+Got@V)La@y1`}g0U5?!gmu*ncZyCCoTB7w4tyi1d1&1~*A)w(wrw5jH`;yV+T$%-&0BaUcx&0qyqoQ9ti0un|$|CEqjQ*Cem_DNZPyBj_aNWfZbs)VfrJxyLZ zOaD`{zoG{cZAO*j%c3J?9!h!SX0{G4igvh> zNLj(UjT)&D>>Q%un%okw@NMd7X3e*^CCpkQ63L;i-NtR5K3kW5*Kk-x#Bbab+z}}3 z`~Y+48N(UQRisc|af3hxKgzKNplDo_cE%mEM z4HuYi(jk_*Jb%j*lU2&j`3?>OOCIax=`Q0mA}@OaNZ?fY&M7&0Zu;}Guo)_WvDXFsq8@ltikr$T5YsB3;Q}Uk?v}@v9clg^De0s+g}to=zGC+ zmb1zf7sL{-+~Oh17sxO3$a~7$DT0%`Gl3X_tJK9Fz)tR$Qt5!lDvBKOvlPo{1X)Y)*kSs{HlTfIe3s~EQ}AQt4-;Dy57bbWrBL*7J= zA;_gOff^;Ea}GNKd^YhrTTPv<{Oy673ri_EFNs>?^f?(nZCzl>`P*Msyid;<6@seY zym43{MTgp{@Bg5;>%E>#u$SG=0|S9wK9f&`5GkmPLpg03gN^5Y@?i{`a@LHszxb)P zMa@_Qj*!TjpYH{+#4guW@--Sh*SB#R`PM6ktUb9KFWt-IMF^c%b!y@RE8=ijEqskG zOc>UekSmgm%Z%Y+I6NDuVt<;FLqbN#ytGVQ^E<$vVX(ag%%L$LQwTt70tygmi6)wj zv&RFy1JSsWZpR~%zl*!K57e{L(CFnr2Mn^M%DTGUsBsE;gQh_Jw>uL&-%VQ^1bdr* z)vOeLKw7l=244Pg+J6PE8GdYz74_urE*!2%g8UM|re@-z?zW`DqSmCb6R_*yuH^Q+ zbrC+iekS_sf3Yl>ZHc!N1ekau)|b7C;KzrnqgpDpQ%PV@V#G_r54DUjt=?5)G=wl^ z@Ng1HjZlWOq|%_>`wF_6crn2X5({Al)obg4DC)+XK+#0+7D6NBVFiMB@;0X^c)iTEv#G5uzIChDd3C ziPSH_n6WGPl=HsCz&`=aVnb%w{(6|ef%kus*|u>9qNK_O60{gd;h!>f!nAVKDNyRj zMEY7eide#8T(`hy)ow3qoBsu6CwjfSfvZj50V^&W(5;+-i4iUa=%iB&-@6lNl+J~lM4xeG&k?~jS8 z*+rw5jD9u*Oet?>K4a%;o=%L?MM+f(6DS+ekP0q}K?sB_a52z+kX9r{RVK@F>vS$2 zvF`r7hcAwn;f&-B@-m4)Cvl$|xg$GGdPob6O(3|+^>j|UHmxYsO7+^so;af>)IX!% zTXp;OhAx{>k#qohc7yN-6zPJ7DoR+d-)UwP^2Kcy2zd1B_$KdE3PPH6-R^SBdWTbN%C7h|og58klzCQ8PRxEH#_6fB*Rg_y zDW#_-^MUs(S6I#&hdTLKC_XRc^i2geU!}q4cb{Bu5AG~(?#|{1UU^=9#o+_KV^iIh zJt5#h%g`dD*r1x2{}vpX#0umhf_EfA3xFvf65itQarJmr5zuh+L6KR0z*u zi}`K6qE&cLIoI840DHC(U@K^R0T_l!Fvm5|gflACxr5H5X*yCrKaR6hv!+fEwGW(q zPcgyu&-vS=sr_hrNyMg1rls)!heiq zdRxPCNaMz)pstH{kz<9Ak9QSmb3?wmCSq~0;t}P23=%!{&(@*j*-oU?Gs!b4 zqk2#6hHlpVhAptw@26FY9^i>8TIYrQ97k=|k(5lC;DP*7@sj+};jqBpXe$?^A|ps;WQ7ua3c#F==c-UV&jfN8PH=4$2jO4jNUb| z$d5Rwp@Q;2azY+g5&bk)+cU^EHj*R3zOYy+p?7h(}2V&GL=Uw{Dau0-~Y?0GP z-iq|BQCL#<21EILGwNuEh|k>`ooKOgpwX454Z1U!SAee8B0HM@<>cnTS4qP~@T}PB zhDh}mmm@5=`(4zb4-mlStkvF$OXog3p~?x5aiJ0LhGF7LhJsS;!p=CDf(k44c!gv5 zotgLS@5#rqtmS3Mvk$#xV_dVUxO^X}6r8W*Do7%kvvzew3h9ReHE8_WVkMENxBZ4U z0O~CpV?yO*!S(aeUN+2POd&k!v`flL)jp zOjy>LJ%&o}jC=>|o+V5CY2~bUuKMYT zZw;i082y(`Szy`k6a$@**C1EY8A036lp1WzN;boTed*IQSNhtVrpae-t{=6`K`-Fg z^rc_c1S)&Gt}!2=&n|WgNLVpO2k~eK%vj7~ndtY+PEW3H*#}at4m?&i@zn{jhIIev`em#>KfW+; zkwWI-2N&kBYTN~S&EeC8q@-w$}5s=0!%pCgVRF);1|J73oW5lzfsIcR;^>; zT+_=T$imYQ9Li14&DK5A-7`GP!ncmC@h~{L z4PC@WJ(#}Bt-#Aiz}yz{+7q?MQqo?z5s;dtwUp3fSR4%k&3#s(OyYXxUAnkTBj}|~ zttbJv+Jw@4Tq76B>7P-KFM-&cUOGe{!(n^MKbc0=%`Mah;m=^>fI@NST>AYFLFu5E z-@fV@nU;dH%|kbxiBYcn(iR%I4wRmYp>%NqjTHwb@|A_%R#LfVk=@AI=>F0!9T4L8 z8Ju=&ilu6s?S8Jl6@IS&IcRw6HtQ~=DvZ&`AMmMxuk?ARwcfWIxjUb+<#~RWMlStF zE<%r8oXAe^{gL;&-sx`3_akCzbajb=LD*{bLIYaM(_f1B(t?Hy+7DrdPdC zyP7?yx$gsN51M%6RT!y4eNdegPF09J??Uk^U$GU#vyw~uL6eLvZNzhMCMQjh*W&JH zYy7@k@^3RB)$Jl=4OEsverZaJ49E72y9gV#_a*2AIQbV+^aZd zkG+Gzg(0>?7iWGVt+VC<>n%TWg&WEVHhW6ib~d{uHaFf zi-Q0u_Ju+{+Ud;t>{A#u4!|lqtC!%gJ{F+~;XAW?%!G;WCo{p!7=dMM;U(lIkNB$xva_U0iv^~po-7_8&@2IRflPc@LSk)#iX z85-j5zwS-wxePMv^sAIO>+WV@j8L<%x;X;vUWJOK;dy7Y&QwEY3L*1E;TJ^BI0)r) z(9$ayOl48?1kX@@Af~?P4V0aQ5?O_4*1o2>cRY+;@|V7&y+8g`fy!BSov)vJre32<7xRDd0yoYiTM&5 z3bQp=FD`N*V#1e-mgmb9bhBT<7blbVX#!90b2C4X5fJ$gi+NPopYmh4KxkU%_vNbh z=xo#XG|_M?PcM3RZ^kFiJ1y9C0@TJTOryx2+QyRhghX)nDgY@#s%N&O9qn*Huwy40 z`Y>_Y=#yQfQO5W{_C-%WZJwB+uqlsbvrC#_ub_!LdVkt4Q*iSXZnsZYpi2buN`pbb zh;ecM6Jt^Q+aeDf#0V*z`H$9knr&-jLBeV5qXBW$?|!#j8o3x6ITiE$hl61g7J=!9i=qeB;(;my3u3F19pWR}39gT|0`#^Tz9 zrnjhjG+-8jVobL9Sc!iCW#K`NY?uu?kor|<6G zxa1kwA=xfu()pRHS=WE*@W6Wi`ivvYDm=om;wNkY0R!azr5eYC-7G|iRAhHk!Y$)~m5rYV& z|BN=}JG`z47nE&5(RY7MrO`gNUlOxp~cFIe$jidUAbM0TO*MABZ%1f$w%H6cv6`FI(5xjR)m zRZut0s>3WMWmgdg5t$kpemN&KO%5sti&3ZlZu4704VSLvS@gv0t8Zv(Dh6B62BcI> zNaOT}vs%bzeo3O5cj|!9Fxsm3Ms0U0)nLF=ylG>|n4?l;z4c`lGec=}*vL+H>P~jz zaVjwg_vq5qQl2e~Ks#^lb63Y1ipFY)0@^sS_a>w{i*Vn}A%wf}F6O=BDkgLt*F~ON zt5)X>Z{E;9g)IvCZtgWu)+^s5!zChYNGOGRj)AaZgZ<)KBRv?Fb@f?rc-zpkbR@IppHibk&f zUD33@t>4+dZMe>6-0WvI@>xVYobLiXbv=MxN$I^5qk1-R)_mMu!km7aK*aDZe)s9> zV-AD8K*~b%;NQ#6l*WgrkE2vn?5skerlF%ZyQ*a{Y^2Sp;5a#*w#i=Is<7eI0(5?- zu|_QUveav3tVBR#LX&9$>I}6cn9;UMC8tQU;`zDK-s6wIPo0Qn7B4v-tlbF&)$^jLc;`nZnDA3R@uKDmZ%8+E8>tZx} zTI;Fr?4&h#9DFhrN!hw&Ff&vUg`FG$lj^Th0ZPg>VK6K9JqU4t7z8Lbp$yXQlg%Sh zwc+lZen{1!sV)EW6!i3HQX;ddqfS{nxMT^{i$4i%QEHWrO!7Z)FNe(~=*@Y9^RP4h28; zoZr30b#R#ls$><&)vC6Nrkh{PMcw=(vcbu~0k>7AwCNFsT}6Drm;^(yj929W1;nmG z)@Y1#dcAq?ErUth$7Fl(Tj z2+1u^_*s%O>VSiN*I`yl>mk$UmXj`>zRNdi+tcx#eOT_u=S36PCs34;P}|#Z+;o3x zDARdvsZ4}dQ5!??4@0~9BI{iEwnF`JW8Wq+%wEJ9B!58_bP+q#cA!z!y!2?&u0jxL z{VxyGGSL`Nx?zUNvK&%QKK{RF!>6sgi}BBP;341*{K#f({w#!h_mi5(CZ%fY<^B}} zS`@!9Xs1&DPKv)(4aq91#A3uX45K4qsK^Fs#n;{4+u(62LrKEZ-^7-J{B+69!$! z8Qf&->yHk6XxXX#4*ysJa{14(efV%wJGNTwJpAheX#)oUBs&Msl+)Q5Ge#Xu@XqnMSMMF{2 zhlB4Ey)#sPI|B3l<`R$VCGK%LXbceR-qVE}c+GF(#YGiObiqIC%Ms_%#|)RT$D-Ds z@GCrE!<5aIAfQjfgOfOH9m>-h&*wF6+XdJ}wYurB4Lcrag#u9!b^0Tt8JkMEko`!#J2cE(ER* zl`2-SR9l29x-i;foG{h+A{9EWwcl1evg!L0W^s;N_Lx#fLbT%YQIe{9hdeWmD7QVL zx!WX)X01^Ffh0@)QtqIQnm<%7KX>SEOzB$L4t=S` zsoJ$8rcry8EQ}bFZ(dmVE#KfIYHbo-%>J6avxZW!yj?uj?1OeQ5YEN%Ja>lrq_6e2 zOx?1@J`#`8rD_efoJ}zT7+^sUf5=1g^&8&vi|v4qy=MLt_F28>&$@FrrgfG$rCq_B z7@frzp}y|&xPQ*`m_s*#zfXyp%>HDqE4r4@1p0Wfh~4j%XOlzz zP9@l&!LWe#PsUqjLSarVk5+gPXf#N-`)kr`Qq(n06A??)l(DE~{o?+}7Ed9I z4Ih~#2xxS2u~s~iB~X_b2*8ELs`Jfii?T!CSdz@fQ@l$CJ=)-YJPu2Ne{4N2KP062 z$u4Y_3u|(LS2Bn|a&npab>Hn!m<)9npkwQD5q7ux3-3h zUjhSQlwq0-zQ$*ts`#Wx$%*@pGl1cu>Tlo6$aOP6Px}p^!|YTc#}h@EsXM^qc~%hm z%U6(T_-56}@AIY4|GIr{eZI3~E3)k~+Nf=%vas&gR_jfYV-b^bMn|f=3N<7Z^+^)1 zSPmQiT=JUMQ`uem_vhKzR{}Ty|IPXCGO%B)uToJ#gKh`IIXpU?j_JUd{GOuf^*zU; zdP*cN)hP`?Gs)^#DZ0(N0Stk7?@$GIXFTIR`d!!~=u8tLwkRC$QRGOpcVH6m#wS?e2%(kvXzPR7BMJ=BZN6a{XQz_hEy z438}Rh=)2Zw8PezIgGVx2eO5;_~x_G%Ssib)6QH|t*=$W+o48vf%p3bXM(-I^Hk*+ z5iq!Q5`Bt3%Uu$E`*-*2-|~y$u2SpSe|a)EQA>zmKBAUENg7lNwNA?-ovaJnLo7JC zG_DMSUlHm=8qzf~7<518nxyHU9f?fwxwyN=R*UYgo8xuMAIITm9rLYr1sx`|z3Ofx7+ zTD3Q0uB-ENhN{m$EPOuLkWQyGt#wA5y($ssaZ$Z!$3>~Z#mZD%wgDBnaX!+8*cI^f zG#JhIDMYf~qgd=Ga1uo1AhZ#HiqLTrpM8KTQ~4kOAX`T@_gGROBd)uVe~d7EAYqqh ziH;idwqHyM7Fc{ofCIkpes||37x6ikPUk)e@=Fnl-4@&I-ny@iO!{HGHTw@&(CfhY z$)n%tVK;Yt<~POi5T2-_J+v%NKM)K>)TRNVDW#)x(_o$j{)>?0800^L&OHbXn#L0( z#r51{I)n~36+w+jQz~7N-iBVMe5-Ct`~BCqv_`$NBPBs>7<62vFF(WZgu2+1sP@n4 zR4Y36<*d4_4fz)^pWr4DgFW3sMrW_m?#bUA&bqp+9qthuN7*U)YCB0+G=PE8yWn5Y z>$(TOsvDxaj`z4{?e)WB{>47|$Vo!RnqkmnB;^Z`w^PhKX_UN3w7fBl%Ivq`WBQr` z(u`7$w1lj4lG_U-l#-pF$<~ zj69k*2|!B)VNA9pU09VrD&J6=(u~72Bh|EoRJKPYB_EgbTu0?ZFN*D=RNCa~C3z|_ z^~08%y+V$DU#uJLZzP?|e~lv8w;&@^-y)bs1}S7kQ}gQ!%{Ofl2e=gdpTfQZs*Pw} zx40Kt3Y0>z7PkjVYmN=Q$Gg)k63Vl5c{#f^6vOOsbc~q8u`dw@3 ziDnG*wQLN9M9Uw8L1#@ew>Fxeo3wWrI>j>YKYUKy>D%7YdtgUpWH(m|v%@D(8nGRU z@rdox%Y>T>2=9+)Y(O&pK{F-AQor%_gas-On4mgPO8fw=V8Va1=b45n{@W0ra!VE; zqkE<2lMp6orl zz>H#jg&GF3Lcf*!UAoCpg{Ix9hP$QiT1b7=4%wF?UL1>$PF;)V*(3f+Z zPGL8f%1<0$^sEuL!+bEML&nWFBwU!^s^VmQC^bdK#a>cEd;3KqR)EG;A-9=n8yJ~K z{q7vcGSfhStpF;DQna~p&ZUdE}x9kUt7xf)-oVvNX;crXpOBdyk zeVu-4`vn<<2sJSoo3eBYiX4X7jX4y-yt>vxN>7Gz0T1X%=gGXrZ`D%5o^(W{n~f!J z>`!9lDUH6>6-s_0%h^?3)&wPWocHFJ*>dl#gE>@~Z}vjm(+l`M22o_N*6RE{;UB9JU4O(kOEY zv$WJAI<lCW6h3+l79$P5|JpMpH8q0lPd79mtRC@+p{^(U{-0bb(}G;6yDZTT+#*I?sqk;ZNA%ceS+ zPL7@wghiN#uUm^|5$4n>5Z2;zU;vEAOG!}X<|N*3WcMXYevR?@C0zqhl(d=mTPINv z{fpGGG|s0#Ox}^`H6qNRhLL-vX(QER=oSg^+KMvYUT^iGg<=gg^(>K2mtSqk(eByi z@VDrAdYk!vWv%7^#8c4-gGTCh#BrtBdeaKx7$5k&Qk2{p9I)LJe%C8*b$mUWLv;(~ zYju+moIP#wQxZn6smo{>xO%zF>Jr1wy$n>zji%l>O*{;B%P`qAOaFBtz1-B2G49xQ z`@NULy?F;gIBYeVl=G(D(QAbBia@2|;F9;pY+h4^NS-wz@-@I-_N}Z~g5_Hxzm?7F;#kVB@r%R?@3!0m}FfhvFuz>LU=;CQpTzfyhR>LN{lZ_(fD~rhQ6~L!$iZBdDz%%^8%O{uae3cS4FH4U4|Q)lhNqsnT;j$de}H%zjek{MVViZ)aOFw8`IOv89THvl@oWSU@L&Lwj zh9ynP5~#6|3_x9S>xSk)SaF{n6Z^gTE$A8kZIBuc#V0zkFyJ&2QR-UiZNsfvH#ZUY zpJONPa&CfYeJ6Php2a4(ZmRHe?g1HthGy|;zD1!UdcTWU^)-$Cwy|LshFQyL80BpZ z6#LNTqqjL5BD|UvC~13o(9NFlxF>ow=9I6j4yUxiH42*Ksi#8;bMV`66b=R`V9HpN)q&{JbJ|mJS;AE6|f>==9s>lkL z9+y~f)6qqIs>Li?*!=NH9tKD19Qj-oNk?lO+0qH#BCN1dYUa=ea)AB>a`?Sz+a2O# zmA=&P)>g{#_K3;&hK0`_VWwv4J=Z=RPn&Ir+)D0_!+;$?<;vx{9F~&d@z@8~u#wVD zkN*=+`H6uHFCF;WZaqoA;`|VIa3K?&W3I5c>?V!)k>O+cma%h!ONp%eXO@tqgKZTB z5PlijaDZW_pHCDE)Ph7$dISuq`NRJap4>!2njJ{sNZO6A~gT~aZt zQURSRB!EV16FaB3%VXdHz(8vdvbet-Og!o;n}u5%C#3Mc+P>K31S z5bVMqxCQIK&$8F|)K5#^;V&O4qB^iBkgkwoF${IXP&_%p;?P5-pE@F^#v?W_khH_X z?-?21Meat)63B$=H5@wNcOZtjjmYvPt$V}++zkhd9wh)c-)y!UseuX6&8@w;-S-rVfGnc4_sNwe+3hq4^-Bn?h%h~1x>TX zY%~($asTGa$Zp!%WO&+SsDB$2LX%1+bo8T(-;`(X$4??~~#B}6%>m)=bEi;)flvBbaIqW$88MqIfXj3r!{4>*iyi@{CP|9DQ3AgFIHaDkKHse3x<41xrq z<=9QS%sZbiH$5*zr@a;7C@P2#iZ!?snv?nHm+7uFFZ%7kxC4HXw?1Gpwh z?`bpU?NyTZ4CY!`RMUdn2t)CM1Ra`nq()+IZzZjKkeAU*dxP)KJ0T|*R7Op-^k>Oi zFs2TwBFXD0O<&r^Df7X>Rau$FaB9%S}`4z3&&5m^|$r6R!F}~xQ<*!fyo6t;8y(!a*FO0`CP$6C$md2PTWELZl~=_8%sCj-O*7y zCrSGXegm;)MZWKLF$vwL(+4>&rZ*g#X!6ss;%JTP#Toe(jRLMvu@jiN>}R$S>F|2Vx~nt)<))fR;=55cudeET|0R&Fm*`Spwr^@o zr||1f7r!H6qV-%`z7-?M(~q_v#;z|P6M1nZIh#sU^*tn{wC2Z!F!+1)CsDa z#ZIA{?M~V=^54bU)ps+3S^@*&ACFO}CN?QZvI$0;R>OXyY`r#5NYCW}eR6*Uyqf-L|nax;WnhEXY!nX`Ln zHKpb{tz10x<#!%1C%yb8Q#o5cj%3*<`wObG;U|lp)G1`(e#y>nYu`=CQx82SG+dSG zFf`h1H^aW5ASO1aRh9K#vM2E9i}8>7;q5xlUyA1NXOc-BP&K=)IgE3=N{o_1WtbV< z^uZmeL2LA8yyyd?Rybp6c29GMlMO(64~xL@(u~VfvoJJzG}HV@`*R=Os76O}gWebx z`X&A2-qAz#%dMXf8MDFCoo-@`dm)BZ4LhVQ!?T=AXa{dpGUh`PmTI-Ow6%2CsgJ?7 zq8tG>0d@zgyG^TT`}Y*wbX+YW`kBS6-ZlR-k8M2fn=img{|d#o_pGwc3nNKcUo$2d zyVyMwxsp4SGqQlgdox}|CtL!$SZz5qiZvTzsTthhLPqAv$7oG2G4vv3CE7Ycjf!)! z>Gy>zNRCWZ+q#Dru!W2Nx^7v@gi@qW0 zj(;K8_%0IY$2Tkzx8MFr!xe#xsci_QjqTkHDf8PKsHIF1WDrOA$(2tl97^}`eV0cL zrlWBxtssKX37ec5CWh*7Lz*#j7+02}Y*N=%u&{-z`+~-W6<4kHY@pJ3 zV@}7}<~|d*Yq8$ewRy|w^e85|<>2yz^7Twv0_p+4*30Rs{bFX-cD>{@q0%g!k{1_a zn?_Om*1cTXLc{}+;PbqFR$${WCxj|?oUL}=-R+PS3#PrYwOCA}TaaM=u8O{beiGq6 zer!|9$5AsrzzR)5ds1{9KJz}>Zn%8$bSkV0p$79z=W!?D+I;ET26}<($^-a8K`}sc z<&7e+taGm^>E1)|AxDQyx;JjTe>^wkR zE^QScOM!(XhxVp262paypE(G=4C|uzv+Ff|ltSZf$Z-c2sE`t&+Qd?PqI9VOV|$P*SjAGNN@}-g z-^A@bnG?fKUnMn{fgIFuaX7`7$51i>DV}O9R;s8^c;>y~MT-oYH;rwU$rw^N1UA`Y zW;UDg;jG!FP6k`>M1uS1RU6p`CfAA~pco}z$`Ph=w}bQkzJqeQgev{@pG)I=u9?6= zckjl5VswkE;|lYsPjJjhW7159Yh(lk#wterwMFUHaf*bqBD{`gn_7l@SB)*K3G%@& zTyG--s6|gb@2$tGD+~UB#e?6b2Zu*q-q-a%H4%*Ryoy`-qE3IDW_3met~YjKF=VJo z{^89Dz+F=<0U~46%VP)fwru3@I5J>^ z`p%5hr0Q>m#$fbA!xbM}Dnb?oSnR6|Y`wPN{hX2Pz4_k%^383<=2?xkHpEp}d9bWO zO2f8*>zZT0;&&2$^qiC|0;9san^4vs9Cv$34^*cc`*!oW)`Hf&zxdO9m0tiPG;1N* z0X>FMmKeI&lT9b1Fty*jid8^Cg09zcM?@7-6L_Xv8Bghy;%A?JHlY{7a&afAe#~^Z zT;%vObq%x0HrK;?&2cQWD#&{xe9~uWh{h$%D(o)ftl}7_m@jRgW&_RKeq(^{9yZ9)g`6yTf5fu*OY{Psu5!C?Gx!l66>pa7G*lmE zA|@hGHMy!JN}V@8`r!5YK3Z1Z?_|L2)Aw=X38_@kUhY2m(8@Bki3D!I<$QQqhahW3 zZd)XeaVUgL4P6p0bebQ22M0cVlF|4O8KNDWAc;=AHlug*axOXUrklgbk=!=t13N6C+FK`S+@p6dujP9AzumszL=Dxz%9B(Wry>y7) z|CZ{7D~V?%=B!B%9q7Jr%1I(L}m?Uk9c|{B2KeZsS=pW&S;{vJpFuziS2zCjdg`g>pMl znUBAz5G4DcSq2?PZBj=tTZEI1GGpTW$t1df+U`t!>h<$Kwb?}6tys09W{!H1Wp~Pe z&@uL&p-asDN{$$4$IC_!$PWV=}@+b zNZ#mEl2us;A6#%tLs^IQxw$5H6?09cqMuv_c!u5AtTs;l?2foSR8r2|&R-c2KEvI> zdHLG1Ya}cy7&U_9RxN&OKOTM&FR3D7SzpsnYsu^aps;fIFstIUREmdhG!B)72(9AN z2h)8gxwNQ%zs|;XXeS;`F>$k0J-^h>blx{H5G z@gTFH9*K(R|5}!xK1I{L9Kl?JDjxTf7>dp`Ra++6M*4!&+&xLweu(BK^UJ*j^X+P8TxK}}M$SqOcU>p4NxMg9G`p1i?)BpYwN zH;nQ*=i@a#tF@OIyY-(-IfS>Jbu8Nv=a*^f06SG@M^$;B&o(XAzMqWGgz}7vKuCz# zd#2zuq~t5|w-?hNI>q@CekN}j-q>5&?Vc|{yF|IQJBb>LnrltAEV`}TWE0+XAOi26 zP=@islKJWy*g-tTm#W>QD@fhyYi&Nj>FRDW!R09luZ^j75ih9mJ|7`{SEI`?Pnk>> z*dxYKW{H1Yz_8N0qyo^@pATRSb|CNPUK|T^xXf&QRH6G+W@vIB%S%=R`~23tG8Kkn zQFn$2JB8=F&M>u*qozlAn`^Azd7AjU^W}Qwxj}WiFmPF<%r?g`#l!uQkcuC!0n`!& zb?cwQuqrMg+!Bs%Fl>iGl1xwO+4pQBfN?Y=rk=X_2do2VWSDg##0`z{_eoJX){+rt z$L}G}tLX&n=*1$Z^5-+m62%wgg{6x~j=t5irMO=m2CL7+E7#{UYUr(wyR$H{{X@LT z9Y~PfF7DNEx7B4s>ehD4bG)S6QzEyzs}mZyH}0CmYT8dB4h(Owa@C9G4s6<7fp-KF zLc=(e8&^rr$VnvQ(qy}#r{-Wa%}n)hj?I!dwKIux+M$k9r1z0SChEjSv95PWcb{qY zAWl-~bJ0{wVBSxRGMbRSn|t zq`dA?>H!$WQsgx8(sXf+gxA`eK-D+e^sDp@0`I(l*eU+>x4D|3XBUW(!s0S-7eL)) z+l5Le6q*gIk{~P1vFe|06xLsR$TZsm4c{{D68cu&CZb7ezS&QIx@(BsVssH2No_BI zE-*+wGtiUDZ}e+Z(#%xOE!0OVlFCnF!`}Lhz|r~v>27oqXZW^ta_bt~?1p>CNsm=4yO8$6X1+Q;uloz=GJO z<+~HD3l)nAY{OF3wlssaV&t)WtyZGQ{W?r#o?7&=SJMMqg_({AT(R`KGW!eTcLJLo=wTDRpD z5AHsF*r75sbW(WMYWVO>j=mM#IK&^8Y`p-~7!Gf)gpUQH1y-wtxPdLp^~~K#jc){fOx@`&gXb&V~H20sO#h{W{Q1PT0{h%)J>E zGcws5VX}*MeToF>MOfmUS756Ul7t99Ne1YT?n`Bjy>+Ca4-?#!#8}z8cA^#T? zoB;UvcGk-?YNZvzUzkrjW+&Q^jetF0z1)X4mh@M&A2|+FM=)yw>bPsK+9$kkxPa=d z$p{;q2AZK281_%PR*pE?H(DMcb{cCIw}JQxzknF$OL~L2F(7{6N}pV;#GeI9{`^He z7)T>eOZXfplzNBoVfCJcf0jfZse>;Pae5->QETE2{0lNC5m)FjR{oajZJaS4ev0V+ zm^1nV9EIQ`k_;?HUzA>XJVXa4X_;kK=t zB}8>ZDO6OH7o4mv&(1_zAY4$h#k3U}_tL(;F%(&x>lyb%Fsx_s<`Xg|_Y=lM98fX| z$?PU<@eF|7H{$O>e4>3cqHY{(gAqIXq*!|{U&;V`6B=JZ z#nln*;MoR7Wy4cdcHi+ey{ZcJ&tLn$dbQ)OOS}oNHE8=~mo?Et2-2z($so9WM6n zq$esMK1t*FTVP`$>Q0)!s_DG~?=q}o0oI`ihuhqsbh*#9-T5sTO8koG1wM<6)eh@W z4!_j@h>E%U1INWhWAgC*V%L5=A6wuoz21%5u(fD~8Q&=K9QmTH<8x~LqbGIC6xi{m zccK{Z{hVVosyn|suwf?&s=4w^V7LqR)O0^Ai$urDWv)OZ4V=6t$c}W$YnaeDC#aMyW z(gSX5FBPtOkL!>;>V&bQLhZ790(~sKf(4_73W(Zh1Xh{dB6dT$vN2uSLk7on#@ov| ziLBlMtTLr~`Xr->42}j5$CO{=skQcs5AW^N9}e%taq7xT!Q{1$rN$Y{E2ff`8g=?_ zFNg7!x$UK=rxdo==jyhL<17sO-FJc6-+AKfBN2Jqsn>DC*j?Upq;?|))QefR%=jk0 z76)Q>DoY`~N1s1i*o~-|t<^Hyz58v4)CP;6Ij`Jhnoy?9ZZv!N7W3X_%Y9cQgQK(F zWAIS0Wcy9DL;do%Zay{EN*(0QE5y9{)A-1JzvN#&WVj`D4#qYU9jVc(0UKZs)g$c;YDO6WX;UG&`A;SS3LRVjH9DoU(vrZgfL#s zh@vrm5>Wcl!%rR%So=JKD8WuicfH zX@K_+_y>$SkSl!NbK?IEEwR&p2tgeo1ccEMWZFptdOLCvBu`3NpYN)7kVh_^(1CX) zvZW6-0P|jk_`sVv6X7FFpEpU@Mmp~w$PXAU?05WB4O+h%0k4!Abrn8HYZFI3pBUD)L# zqp1RNH&~;2i1D!cq8Jvzy%RYRxL*6Lu}3YebvLgTi~Ya8};W^tCw61ZVvV72rB9 zCPHchwoOU@QiNbou1 zoq*5BS3)bpT^vs@5}f-xC2gB5;6YGXeRuU6Oel(nHY_%d>x<26AHnTa)!@+WGf++V zla0&gWnO*-d``w~)LGtuWtJvt%jC))% zrD=NhHm>P${x-2`Kw**ew3LKzveGUfncJP)H=cln7lv#)CUb#inqO2T$0#zYYHAA6 z)mN!|8ga~0^&9$mD~ixofTTo-ULqt^Qng)TBQe0M33HyU@0B~tx=H@1KCi!@E$K2` zeyVLwpCZ_+f*+uA4)v!SD!CV&Ecm6yxDy5!&eaQ7`xU~1+Vk3f^!Qwf4!i-137AxP zY%daqR5-7wu5JwaQ14kB_~kxvb8HSk#D%GZxwtiiWpkN2oN{KPz~Z?zLr_Eow|t;o zwh1CGPR31pcT+%`%c?jsR>L6c4|8VA8~N*|c!uVwy-Z6+?_l>PXw_dq7%xcDcY;Gy zH8z7JWaR2?cIA9lPrdD%*>6C7yJ%XznRA)wRrC=B3y`V@B}>di)?} zA8jl3UeQA_V5c}ePkK5zQ<9QnmSr3E3Kbg1;_8D^%zi_x;uV`9tgPf8=})9RImh$p z#cq?D>TIQ-OlsMRgyQ)3Ed#GUR4^7<_G0iws04Od$_o@gbr_!xN6`SBrslR<)=o`J zzcZWyms5VvTFz>0mD^Xuc~mQDe&MF8h&E^vaUA?kcCPwNJBaau>S__C^caeWlR&5A zhMO>Q=bR>!6lD~A#YWln{_`kAXB3|}xF`9lB7!Hre9>vbm1g2Qt(MPM_LQ=;EP=VS zRVzV0q~Yb~*>WVYaq=~wZo#QK*51*;B)w%!@!W`9>YbSQc)R@IRX`0rX-sam`B&9< zS4?rt?!@gryR-ucp6H5IBjwd}U)h^WGZab7KrH79gML>U&(>R%_ef8Tq+Eh0wsD8a z(aF2yv=2>MrXl1p!8Dvi&H_13T|FU9)XrebkyK&Guai?c_EsWb-&biE%elArD}{5{ zx^9=9%vg%ZeEm;yQ!}^%^)Dt}q6uc`w%a~`V10e$45Qh)lBgNk`c2<|5QOTjKt%S4 zrtrz0Acx!P^bqs#wzUklmAw2q`GDWM7CS<3E4IBc_;N2-@KTkR%uk$b@||FB8$JIR ztv1(7^T0nU2P@tMP@X1>_=+M`I_HK?$TsAlH6#WvYjYq%R4xH=tiU+lUhCs=_pxuV zca*TlD{5RehSbB}uMd7Vdx#jQZXct)s>|{4?^|@y<8an0i^A%DM_{?-A?U32ZbhkN zlIosBH@v|~Vj!38&b}ds(fo?4bbe)MU*9h2Mj%GHHGCEeD4;4hGu>jUW{<5UK!>Q2 zzCpK$u!uOr7^5Um?aAQML{vyMEiF&YufXEdY!rtX{R)Ak5|Clh_OFjgeZo`c@Av498L5x&V{zXUd=JR2)b<5h(mRz+X?Y&}!<>0mv{th;)lTE=_+ILM> zEH_vU<+;Xi0cAzt6*cvp0; zRo&G?XGkDjZPfS^DYN;t`yrpGGphXCFZ))+++;1r; z35gjVD5j?MZJbF6W=X9!i~u7pB3(>}Jbs@=MA;yi0!oH`|YceLOH za8vfImXJTTHNJZkZAb6wi1k2x);tzJB_His4aJjqRz-P{y)GDBMUr|RIxWZpJ&JD) zYMHH_I1oH*J|aC+OCC#}(of3E-H5n}z5oqnCUgw>@v|8MZVJt{D32F}Xir26_wG=# zEWhkuC;tN!_;r?znVFf1m>EDsEMV|I8Y?3s(O(%8)2sY-4`yNcr|xzC-+eX+V~mz@ zECBikIOyfA6f8~t^2`$b104Sjm;WZ8f907MVjyAw{|BBR{})d}R+jce41Yajk?Eyv zt@Pv!?A7R{1%&A34V>*Y{tbq&Mls_iAK$b5wWgOfu(NWo)ibaoVt~Zlo9IA6Vn$5x z{@+TO_kX$XW&ah^Ofg5MABhPbX|1BePOu89MjJnxZf`9naOTO0h zUv~c*i{;gf=QaPT*{9F{G?cbAaQv^}|6<4zgJVwoYR6_y{rT@0$P#HF)AL(7ybb~Q zU(6U-W4z5jL$SnUnnU3K5f3@$kLkylb8|X0ArOcK1OhQLy*9xZ1`Bd1P>hVl=eH2h z>nZu#Sivl`AeI<+i;qMg8Le42k|J1G6$QFvQeakOFKBuL=HcM3(=f zV`h3S?%!k}MmEORk@|NT6Wc3p{#^!U1F`+@cpxUW|A+@>VPtuQ1qcNEYho5gw*Q?N z%)-R_A2L=D>njZZtr##X1pFVxutHt|{lD#4*j}gke|!c19)Vd|nf}w6jpaWQvw>gB z{lCut@MU8JF~pc#;Q=6eSu3kobo}$}lU~%)(29uRAN0(~^r8YpYDC}oSePMTFoOUq zA2Xu>h*gMzk&l;&j~T+p3KnD#VrJk%{{Nf&MT~%z9>1}Ho|&D41ra?nFPK-5k(VFL z$Hd6XC&nzmdm(X literal 28728 zcmdSAbC4%d)9=|frfu85wrz9Twrx+_wr$(CIc?jv?mTbYyYG$IySsmH#i^)_tcr@v zI43K3>76A=?5J7W_*J{TuwM-u}Z8279#P3c(NQTU$O+S5kh>hmuk2+*pcWZ)fC zhPc3ZWeQjQOVTjMxgEr}_}@Cbi_9CYK%lx%8DyQ3=o(e~g%#6***$pQ%;{&@9`?%6 z&-pU-VgbG12e&IXC4P99Lq9#=j(|@?E`qc&5Z`n#h#P5?0spSAlf9Fhu;`z!BQNY> zK6N{*?4J2R0*TYw(9?V2H0`QKFLGL5_u+!bqRhzP92Snm$94n0`5Woc{i4n7Mpt5t zTW^VxCw2y9rk-ADavl+T_ORj^_`xs+JR_YX9IguOzK03)maoylR2;TQ*6CyL&e7#Z zqv}~2>%D|>ay1`!z9I9mm|*pciiPHeyTjR(Zg(i5CQ zF1EE&sNX)m5)oZ8Jca}es2QbyrjV8P@I^0$$&Cp&8aZ%fSvmQ^nGR*rNNoGFh%5tm z!)@wFj*dh6FH>W%T%&G3Cg@Sd{Z9fXi+hQ3qK+t>hvRUM)Y8ai;RnkY@B;Y;7(644 z|HYf*Rbz|J+KF*VpLz<^{V!CnKOD*i^2F^Ju%uZF^CF3I*XH--OrH~6L{U-C>Dj*I zaFY;e)m@?9^mtNT6gcQ4Q!_E6US}m^w$!hTPpGf4+;4>*hvEdf zZfS!}7%pkQqkPZ&0!Uu{hI&Ae?y{_Ni+zGjWr8`vsqq^fB*> zjiBkl`5%@4C7z5|%VAZ=cC%~V!MP?6>T1NlCUQeV*C@?CkM9`P`ex48vyZNZ#6W^2 z8*wbN57rsh)`F^A1Zp5Kbu_jPkkcXA%1N`L!W$PethBj1gw+~M0qNSdfi4i(`x>4J z02%&8+ip>`Q3+J-#w3WUPg^k2rUc}`rg>-K5cY^LIL9qwll~`I4sACeq>)7OPPN5% zz%gg^Xd}De1u|SKANv&Pl+KDA-8(?zdM+ z)VAjX8zn21MJjjK3HGXywW*TADbI)6D+wbQ?GCU&)`_V6H9fcBZxP`pfdW{oky^*EX327CS&f z#uBI;Jiv6;kIhkfHhE)2!bxAm4+x8eKF1Ucn0Xw^bKVtJC$o)pHZCZzcQ^c%Kn1tm z;9jgyckZ&|$a;gwtf&zyRF2@%s^fufP5hKq0uN|JLs!!pc3;Qkz)|U_%#L|p0GtwS z77$3Ind4ix-I&&wt##~<1M?or|mGn67g+L{4Kv!3WBHhat-Oea|^bG zUNCz}*2Mc;@^LmvbbP}7h!g^PlFjV4J@;8_BhIU%B34GZ? z9P9cdx}=|`9)iLCM?!su&L*Bkn*F|Y1nCOt{79WgzYig?+<^?^KaKfw9eKD0n<0$3 zH%8qMScEjpi^jTc_7w#@5YB8lV0Jf0_8?9IB`W~in2c1IbkuCXN_lX9NI0gAh;y8>W;y&vfmeOI_Lr6sa+Lu!s*B4eY>NTi&F`ZPOke7^d zNoODu$t9FBD2pa;ofHbrq^HrYu;b0e>1pil(6LBZIBt^0rcFp<-<|tXpU@nNB|&C{V18^N7~57?FH_TbZQ746~CHYGVcUM(a;SJhLoY z4^2?1Az}l&Hfl3E#HM~-uiw2RcU+m*&w58l6HbMuwcjcJ{IgSL+^1c0@W=rb`^Xd^ zdc)N_5XXaviNJ`O=*!~9``a-A^!PpInr%A|vb{chy6+n6NjLwZx2@;HtOqeJH< zb(k zA5kMzNv_M1cOlJ&!?|PI`qN-7WRS}x&t%fRVK5}eNL!xq@s@ZL4$BnDm0@&Gb;=Oek zpxsi1?$NZV*BO@t_@f}z8rErk5;@fO$8qJX=;XoQ^%~1)IDL3TEH?ZFRhH;l*J!HD z26o1qm{oH!_(_z*@7LRT$)XI2IvZs$$FO;LHwnY>@C|mWO&NN)McVa6u#_;It zRvO;zQmOY%rQ|*7|6aCY7B(H ztJDx{5bMhTWgb)(K)wSoB&TxJwGW7j04g0MmNgOljf$T&vc6(0gLNz&$ke>Av27pchmFF zHbgbzs)jt>DPyaDzRxZEK>XO|-Zj!qrnt5RKClSBLkW>1cU73EB)Ay(5o~5mABg~< zrw)QM6ab$E+3i-_$={vqwxZK$ixCP3|9h6i@ggmYU%G=!>yuafP;Z1e=DO}he&+|X zRPUR++wU8@ME)nI2lpHDGH|yW#>Ce6f1<-b>p!#s^B>6gkHO5y1o+SG|8lbYH;brv z*qbn@%Ntsn7&*f*D7zRs|HmO_=V$}NAS7hxu0_bg#7M}-@?RHLW+r+@LLC?eaYs8B z`+p>(%%I|EVC!UW;AmoN2x{wHK{{PX;OK~}YfZ0unR zQul@0sdZns;hi)X1ibMx6#qnsp5WP{0PfxCK4oJ6*QFX)NTtDpZ@8#Fr{^<9c2Zq5Sy{HvINVMHa3Ahbu zrvm?-Z>!h0O3RX;?`NIt;p{B8;wZgyf}UErAKs~+84M-P{(5dQlJ+d2;I~7ZDAtAH zu-A7Fiy@0$fQw1dAWTaCu);K&o)|_VyuHI;q&iFWExKrf5@|E+01XnR2beDFgB52# zOujAIMMwP%oCY%^;H9`)oGwj!or@lCZ>-Itv!FP65AQdJ&s&?V29>Yg&(By23Nv>!244utU)G>Z193~fXTujpcUl&v6u(M>eR<+AB2i6{k8w1dh*`oJ^j)ybv_cvLolJvLcN=Agk!OoIa<&L3yT+- zA@lX+ywqgwUVAMkd1f}O7RS5yoHXJeEZ5XGD~(sRmj&<7eoOLEN1$<*o@zvrY@tH( z{C(PR*QyC5|1`P$P@hFF4UdwPr_}#Dv9)BC;R5Pi#j2$IVu?)Nf4sGn`l|6#X#RED zr?3?g?n26B6)uZ%Zo_?_k8K2{=n18X zupo~vz(_7wN1RVGFUo#wHVO`4TTZgLa_ToK;0sf{3~6@P`6)rSi%$FtU%4}kdD7*K zK9zw~p;mB29{UwwMo+hse1*YdILVXQdf8ndkFB>gaa(bEln!qDq#LW@1);nAp@pP! zmyksjkVwx66(rO!V+`15Aui`4l?5X^FBe=iw=5G71*{SLUTFBzc_PjHJqTPH&p73L zCL;{65;4{NlWOxBO1!$T8E1s_pXK1~XLg$=f%o{%Vrix@&wJNFA^I{@yKw6A5Cbl) zG@uOAPuxzS?vuv%sTAD9(x32-m8s7P84I0)!Hb!xAAhItlHjZZH+tRR&UREd;#xu z!Z`y>B1QMey-)UbN%7THs)DnKSM2EY^-WZq@NV8-qoQQwCr&<;E+c>`qAq|6kHp== zPC}qYJw5JHe7~5fIt4aI27_I)M3&Ym_vZO`$0g0bNW6;sY0vGrJ(mM{cc zb4XMaJk}+J8CP)_b#a+Rl3~tMlVw}8vHpT46H|+E%_vOnf@TA2s8O`0)4%A%ET&fA zeQRqRIAq&BeU(?}&n?_&#Y{#TAq9a)9CVf_6bGn4vq+xP)h~clHEvXH!Iy$4ACAT7 zfb)m?=JbcogaX1d5Da`Xh$%b3wAma8EaOaC_l>}X%2wz+!~-9bYGLF4cA~6oXDfH} zEz?O&kJW4s z{!{;|?hY%a<}3#Vn7XJF{V=_f1plV9v@f^q4J$%UzvNUV2ut=W&-8 z>DZU$%b8gniwX*BNa6A${{Y?5uaJ+qbyZa4PO7?aWO@l*){|{uODPxGc`7Fm7mV5iSnVKl3%cR$!YwM!n(MK(v*9oh>t9p6$HeBufS4L#^N_+2KEQbm&<>M*%=_IV4P{v|?0 zdSn@A3z5=N3-~aENc?4ZBg74JcYk7HUfe(y2 z?Yf>t;W_t6;&NKuk);fsKHa-@LK8vU`BNl63R4)kUeI`{@b5~ObyLy_8S7F0JwA2T7kv$-*F7mh@85A+P| zM~)qd%M)G_Bt!v`K<)*Dxsa;SZoT2KoN=tpT0M~ig-4uw-KX=}I;Ni)bk2Px(_To? zE_v|qf)}zZ%^+q!_*0QwR}z$wN-EvMMKV8LOhX0uW{gip>?p?c^nkI!h`56USsi#x zgiRp&jWoBFY^tUuuC?ZJW|Dymn44NJRJsU_T6x6rx6-TjIf7us*`2T5lDpKOXaZfn z)Vs`|nB0&b(7vcl;{Ssh`j4jhkB(zv0DbA~Qsdnwq0fRs+3DGTk=FfRnwoq$!{Z;9qE2cJm@iBE} z2ix%V^Wneo>y>XzPd}MTXEr^7@qQOpKZ*P~+{d0i@b@pR!qp=*Lp9e+P8?xm_lk}HfCg7~!v zgnGuh=PsCJEZS_YnyKT)p>quvyK;gECY-KeK*CLtkclzPKuH^chKQ*i@a^|Pfh*!G z0ldd}cv3hPs<6*2k-NyWo%NcvT}t+bjO|v6EEHR4X3y*ifhWYUuY9{i?+oOUuSddw z9egc%F)u%7N?O3F-~^fhK>&R27w{EMuM&U{gv{$7BeWqs&w5IDudlZc+XSpL+YKzQ z6I(?I87-i!0NVkrm!KYfT!8pyrAXERRxME7M#9ML;I!W|Gkhwa`De$M;4JriTd zd#NMOlH$qY2>vX?j1GKr&fnP`)b4UK>Ixyou2TuC*p-8Q=k&zF8%6qrT_83WO4o-` zBi|OWWr3Own&*R%_l@`wtobtwaZTncvrlXc+O;WFAMG2=Hvp_1Kyjc^6xS5@(Z)tP zhy&SQio^=qa}%`9-^bXMYkr9N^6LlHPjxCPS7OzlSkW2ro$4LDWk3eL3+4x^H~L3J zEWzUt|DN#$=QE z{UdLKUy=K!c)Gx9hpd1TSN1yZNUSgU2WLT3w)^!4)2c~_MtI`j$T8x_Z`-q2;**WJ z+oPT1TgTn9hBP?1Q~S{zjM}+s!mUpJI&iniUAtWSTpL}(z3_S?cTI%8;qpz4zSe*8 z__F#D{9yg01JvAQUqw`gkM_yywLS?f!kqJ2L^G(DC0ivvirJ4mcW3kpd`{QCt?+Yq z*&GYoeNIi4Ww}tQ5}971mM11fvwZHjy+0pJ-z(Ck^)gH*}c3S0)P zzunxC_HSvEMstDAT5@O(8uK@SluYQyn!f5f5Cd>A`(=Z$3G8Rc`Y@OUg*M@Hxdr8J z`vj-!Mi{`Uo^h7QW(c>;`DhQ9;N}v8U3xvZW7LnN8cX7txpUH-jVtE10!LBPv%s=@mTPd zNn6OaL9Xe6puI3MVn+%S0h7IbIP}K|h4-(d4cv?&R>5z+VI}1F(n0kR=oBgWAPsVLJa-p`Z8my5OV+oYExcz1B&c-s` z^H|k4hlTNx4EO@IB=8FJbF~FMi~bdiC#`2J=NilhpDE-`+OL5rrVYhwE65jOtQGhQ z)=@E-+2Z1&QMNp0=hpsq*l6XfTwa(w@%B zYAx}H3nNXVqI!etOSzi-l%7GPz?YcwM_rEvw`*iBx0tN%_Ma`nR+pyQwY*fVR$Df% zn_RBWpDpW*mA^s+x3uFp%DpadU7k3E4g=1h+hR4f*<0bYp=#L9*wXiy7}^ zVx`FKlnw3|37&+q`c@PfE%aXUg!=drSpt9NNB&|plB{L3_Dk3dmd)9+uXEXK>GoQ@ z&h|3rxwl?R5WBWc=YX(S`Q?@0;0LNZ!N{il4b_klN5z7Ko423^=#w^*07~;&tgs_m z=rLiYcWcoHm9wJ-;*aG_jK5TUFsn;v-C#UChgRR+z;J(^y`6t%gs4QPmHyx<#@$O2 zRQ7+dY+^;apra|G$BTi;8bo0zO}ab27zw=%Y-9alt%e7LHkKhf?J zeKu@eoj4ol7tiQo)Jh7f)_`ptb+&ZDAa~qs>H6&Vs^GSd>Jx!>f-eo<=%jx6d9in* zlgrLyLjws4@tf0NB&%9$`QaM4l1z_-{x{HQAy#8!bbPisdDRrp>x~>PzLc^}-6B|0 zbP>G&Pr~o=$p>r*WqZ0$#cNyz9tg0lztu*>BWcx7S~wjjcn>5Y_-mg2T@t#R>s%dt z(;Qpjf$R|6qt=z++H@m>`ij4t=dYo*nOs`KTV-;YKY%#&M^dmFUw+Am{nb;-++lTJ81?*Ex;?w_L4_QE%Tl6JB zWNn4VzHot}DdenYChG0wt)#!9c~&%)O11rbbf?cB`k2?2N_pRVJUV`bvjz%>)1hESvXLhcIfDd#ooSkbjF-@ZT=@a=2JGS}7yIOQjNOj<)Jwjf zso&SucpBO@>$-&ud?)tct0-3wmX>t1u50{84)j|7O(tiD=IUO_wgE8`RQW^_DCMA2 z^+QmA*I&W*d#y=r?a2V)1W;BnOp|{P)Ii1u0R5&GkLjI&9yblCc*JEd0u)76s8BQ? z#auEWViFV_-g$P_K(WUiijyEgsvu%9wGm^F`}$k+-oGc;_Au7#ACrYIGR$Z@ocUiu z6YN3O>@c3igzgs)^pE%Iy4`G~d$cF2j;ieouGrvM#(Szc9VK6k&~J+v!58bV;eqKU z_K`Qhkix>@48h?RXI8CS-KO1)ylTw%6$K6A_I&_^Ip)Ti07a8jQ9sFy*gyoOia$Qs z%Xjo47VCvwKlFMs{E1@CzSv4P;~nx;5sL7l_jyX{7=ew<4R}LnvxluCfdNnHW<4J` zcvlFRHb_yb0+X`vC^?hJaZ?bI_{>+xd*A%+M7(3nKSL1vx9wlz5YFFC2=lQllWWk1gL8XBW)PQl0X2@5G$(m+|zZKFrt#xH#9> zI^WM%1ISxrT8J!(HHJ2=qClp>1i?35W6n{>oFkA)6T(ca{9p6xuG8q+CwnyX{PG1Zm4jJM+yzl z#r7=vKdexcwdFN(awYVfnLTJ|k>JztVXG$W&=a57Hi^$x0>x}{IA$j>Z#-7@Hf(NK zy(1leTh^$x!U)wgLRZNXp*JbVlPEM98p_=gI@|CT5Z)ZF86};OJEv8tf3*_+(rDC_ z2FDW6$TZCjk(CLPF|tLkTsB>#%0AD=liTN!z!z8LF^|R$dec=jOc{lXc_tEzyxYdyFt9=2&J?-Ss z2=%VROxu<81M!rfH8s$;epnV~LwuY!RG`Y3vq}5pi67b!4n?RQ zM&V*@Qv-TIAhg=g++s`q%k*X>+ttqBIiZ@R1I~3Y{gwQ?r9lef8V9HuR0*W4#l{3r zQzP`kvYd1P|0Yf<>ne=H9*p}MjNqw3Db^2=6B>N7-{?AQ1`9d+{q#4obaT+>XAASY zxiRn!#S9)(hQYj1lia}Cecso)@xt2XvzZ2*loQB*(e#S52Sr`CAoV=*a9FClCwQZ# zm$-`*|F8TRx<=V>;~1(>eX;US<}>@njx|i}o$B_;A20ITF4N}m0#Xa0$pZ2}=f&1m zX3{%xEbN=axZ)Q~=6Ghe88|sg4gT*M#1yWHs>*^5&%w|c;kZNSL4We(rUhh?*==C_ zqJH8|S}05kEz%3{QuvnIuCV`}HruAgNE!z&5h0z8KDX`w=R0=T784+Uet0H;T?42}+3M8b3~+$SA7U5A z8vdgzIU#D#8DqbeMUxqM9EmJG;U{g{`=fHhi`W(Ub5g^|neP~9WOVy*r)Z3@?XVk> zXBdc`B3@Y&1+!M_0VJ-nYo;s#v4oWXCy^9Bj@tB^y;RCQnTE17)rtDOAZ z(X=4Mql!^zi2F;PL10#ec@Ly)7`2s0OIlLU1IQs9W!5qVX3ZbYSoSJ!K^<;Xzu9^G zIE^~(d+*Dr!nlzZI_0-yUh5wV{?!}EI$G$3btq6-=_09ycB1yvr?W45Am&7Ua(^t; zPgA$tiqWqiT-p{#I2cIPwQdHd!`;c9Q3DH`L-xLgbB+x`swaVcW+F(+Be9zd%O$?d zT)JN32mA<8ME&{z0{Ym!(MhYzSeV=mGI%6$c8?lql}*Kjl3!b2<8?sLSq9%#UApg%;7bi@X5C0+kv&08d?t+yha1vFmt}`pr zU1LjDxGGZD>}gq%&KP`d-##e&Dj`RY1vh5z#dBmLjO+as8GYAi_f7t-a}jBj+x~v_ zHPCaRE4{NSzotzSdU zl7K^@lfcQvL+vK>?Odapk?CR44^OWZ6hX$-zR41o3Q=G|$PX{F+LaicD4>fSRswA9 zJ%THo4`RiVdg%D(A$*Azujo-IeT>;i%>0m|k)ocuj^2^!eL<_HS=*#nT|1+b+#0@N z+1}r=?3?K8bKUIGs^PZkW^7!{^6r0G#*7J1IWL4OS_arXoP-cnL}`Qn3w#m44kihh zHNuNa&JBG4ixnugRLU`hS7>P9!5y}jhZ~Z{D^gJmE$^T^lk^QZ_V3cpTqZUSWoiu2 zhYgUV-0#}NEkg;e+V%IBwD6V&g7oAu2P(1d|JxP=&jY@4pxT8O*@okmQ3L##{J0F* zzmq~m+aI_dO4VV*N!O4FH7%Y+gP72l<2Wy>lC3k+yDHg4ZddD_IO<-yHCTjF>fn&qI z-F4S6esm-a8*-!7_`X{4%s^R+A7D*VWs1<+i_WR2kpO;{@Uv=wkIo*l*@NS+bECsG=>%aZ5Fy|3;>4id*e z=F}@Sx!GG}a*d@de=bYzL~yusO`%^N+E@yjWTbO7(Q>}chY_ZEFIo6D7pXGehf4r9 znSH&aF?VGOQ5R1ujlA!E{$4JHjv0fDCsT4=6=!OH0KZ&^%&LEs{Ii<(LthOmSM2hK z?X$tRAf(-;kKP4EJ~xW&>b*!@{0Rh)_K73mOpx#8`8ZtM0g$J*dPx$Np zLx6wg59g{P`TGzpNJUGpVy7jV{PS)9?ON~ zR{UEqE@JP%rYzk>YBbfQ2pzq`}YAIgb>dXzcc z8(2GH9GC*NX)0iD*9%*>IUKnAu9;3cUFkT#fzF-hgy;uPA3;OojF|D7ab0GBR$8@o z#jNv`)AZfU!!(RGcGtWY#{8EC?#WaGC6=P_05Uv*LYI^UJ;W}maH&Wz;*nAKh^RVP z@OhQ>@je(AlS2uw)Hw08lj3JSxRbIqn4_r@HK30^Z7hG?lB@z0?|4Dc6-!=mxJk7* zW@0j~Fu)%m2z!gpbI8hbTwe3~Ej+ppN$|ru<9&k2(RLlF<%i-I+7*#{klTalcNj}B za z6!SDKmda(y&G2;QHsQ7|DP@p!!Lb`_q5_4sPHB+DMr0HM5bmTcR5V{oF>QjqTK|pZ zN)-5RCx?~gLA+=#1OHXX)D)9mAjlkf&@YWlrh%?9cNej2+G=H+x+ob-{L0jUvg`$Z z$ZTHDiPyw(V(f1b3MM1~)6OWyCz#U6)W)FRGIfGOU;^qL(ms(xoD+HNx& z?sN(K6UD2){2cz(%O_EcJ^H^++icOxY~&LM>pRgk-^^!m#{se?gD6E5Hs3j9>`b5Sk>6TIN33qm<>`D zwt9kkvU#8xopGnozUQ-mR=%X5ElWdGG&q=&>S{nkLjX!oW$Ba&r4uDLmik#N*wHDxJ3T7hS6Br=iy8=xac< z@TffIL*d81U*9A{Y)lrj^~lx6{wx%v6(Wnyx4Fmn174)-3gUhVCl4$|j1?7+S8ox# z4wvbAhoH(sWS-&(kPb_iac>pe!RuZgIp4;`qw1JK1{;t&zcKDKDt6|R$)Z*-dnjlh zhQM*ueXwJ^rr`m`!SKG}BqTAzEiT1G0&zsd7*&NBYka=uW`!m2iRxaN(#xFY4B;xg zmGlxV@iEjuitRo-+V9^SAg*E6nXfXU$|dQRp;Ak>U9(M^w7cL=i{yz~P()2)f0D1{ zl&6~j4Hi@xK3N4=cUP_Jx>GfwCRp5wkHc~+f9g|c`HCHSE&Vl?AX5%Z?L$m%1Hg|F zl+Br40$j$(ojncqg33NG^)j8cJQcZnvoF|Rl-cvfrAkvXv-QG5h?1Kk>}E_n(S!XSM3zHL9B)T=fo5H zZ%aO=NH^kN^5~Z@{A%_?epS0}ST`7Bg*+UQ^x%*tR;3Txw?=@cF;gYVOZX4NVNtZ- z-oNiSzW4Vv9Br)gH*vKJZT{-c_cZ(BIH{4J*@Du$KyTZS7J z(yKw)Ejlf=Hd(TTU=51A5*DULRthV_>1@$8WSQoz!nUvu+qUw^c*Y-ZhLkeXBO=H~qVuC731W~W4X=gfM#Pe4`xbL^PDp{tyHZ2W_Go2#^ArMx z%iZd?qD43r8<2n|%Y9hWDs6e{j8`6v)O2{<&%7e--%jap{+hVHtK#+=OeQKj z4`)7jk8%G8hrLOm(J+= z^(%b(3Q91fsXID+nItl_fpB=jLgiW>J4l?Cd9*VH8JadU=(OvdsUXl0P?w@YQyCQh zK2vZlsgR?D7KNw5Gzh1_l}ND?@P#I>jZn(*f6e~<@}}@hRUHO5iy_)Gf9H_lm8^ClzNRY}W$ndnu7R8>&1}+M0?01E=^t9J> zQRdukVLHav&guViP+weG8Ts>2Z)C>&db;*fR<#xN>xHVy5V2?789M8fIQC-eJ-2OE zyR?m&KE4#Dc-^pCz4U7WOtCn%QPxOMu2*`h@|SCZm@`D~XCoYwKC zB(Xu^*{u$fFEfWMv^fvXGGDo=?d)^+U8_R^Na+l@PN94py?dCJKq&N^8X1 zkvoLz`gTUyxpPxsbRK%BP*T@E{+)Be|8#NLn)k7`m@C<4Rb^%Ak)v$N6hBnlnX;lv zYS-XYG1MH^u=50&3cq<@8k6Y(#`SpM-`6~234 zLnu3FGr0Za=&*fQY?x8e1|dTTcxPZ)65!q%{Mo^chwE7q4}FdG`xfrHz*0CmU(ZeOuTI=G zJg2#vpjbtoRA=D2dxB&D_7z83N}}L;E@b?@1U_HPt*iZ&B<6D7cwNF7b9xl;su;3V z$Cb=6k$ePvkC{~zv@`nwWJ}*Kl#sF9%$4Ys>*t`o)ZR!?5V7>xf}Yeua?Rg(`k&1t_2i zT*M^a4&!OOJuBVroAK`Wc8r$qA}iMeGMv!6`3;KcO#YAEny>D+!_vuP_|C7rZSJw0 zW~A6}MQBt!%_kLs8!@XTOlxnI+L?Ch*Iov`WfDGxn^6^OcbZjLF6!O^^6h z+DWu`zzJYhf5+S>z$w72$*u;VmR_>LdBL;j{jYmns<3CXeRJ`&6U2TK1et_9?`D9C+$@2EL(9ATAvKJahpZ`d1{pglEOG$~rf1^0S=0iy~Jsk2(Y{ zn@+n2S~0R%0zDQo1^oN3w#dJ+g&9ctTJ#InO9)2K91EBvL6{_~m?WfU(;OHcn2>36 zWG18m5a86*<#oKRxm$&`QOnuo7p?tUCvOX1M5cDK7$r0h!EZ3J7nT~oHj33iI2D=P zL$qa@;Y|;5pKjrM-$9?XpH6W2qh4@V8$A|tx>xMBg|#_;j#=+wJ!8M$#5rKK^!%=# zcD~P>)eHoTkaL{~$bB&WC9Y1^#X75LfMydzY@z`cMZUMia~%RFeQ}@ z@xhfN=S<@qEeahY{r6=L1m8@bs$)!#-^MFm80ZQMk90M=COS)d%ukAYty3Y9g}66m z+}1~rN*Znp)&L+guacUkN`6;GP<--K5OOERJvH&zh2#uKeAv?8i6xE@s7| zMy9$YBZ)|EQM-?%=Bi%BhFS!&(rpSzIfm+(8sNC3T5%#{ZY?90r79#S7K&1`ZkHqy zBS9?0P^f6&FE%99py&sQ%r-br22VC8d@`0rTus}Zs;M$1k72$lQ`%^N_gZUkBo&si zrOLdD*}bG>W@w*8ME^PJ!?>%h|A@6S!1?}ma+!UCZF%YuFR>Ih+p7D9=9&CZz(6oU zuxcax7W~xOT6M8!?D1I~8;%W#ss4l3ic_XG;W1tLit9E5FzI5Vc2kv8p7A^~J?hvL z*O5o-9n(Ow>pP*#R`_gDgGxvbIyF&$nod5Uc3N&vX>>WI@0dd#vcgVWz5r)6qAlE!1z+>v9 zqR(S*d)y$CfHe{KTJ$hDwIjbseqCA;8R5dmGKY+?D^RoJ{6;--dO)UP{pvHGbsx7Q z{5EbkNAU&WGcntR&VN9`&mrekv`OsltXFpKjznKzmoJZ6EU(Du&j;L>`f(;K3jADQ zy?g!MFlbU#QdJn0H1850=`FeOhYE@$+=M9+*ck{F@a6nVtR|a^jvFUuD{uO8X@=#L z`|1)bJUg7yYbF#*86}wTClB=@&|iwqA76qC)h1C%PK~yuLGLhR`wtqhiR`S@vGX6+ zffjf3_{T1#7>qP5({KN(uwD8XF6Ywg1y06MOR#t)LhyP3Py;Qyw&@13U=m8brDHz! z5cpdJ^~ftB%tiVVAbh?|pUKFpL9{*^hrPhJAjuQUrAA6#N#I!g0TTK4hWf|i0*X(A zwEVk()$5X7yV>_@w~L_HdF2pEIMO!T_X*YsH+#L}_(_^4Ct=qm^g3<%b76UOdl(}q zrG+)RgK&S62B+0wVbukEZ*^FW?p`*NyP64k!nwTth~)2VcH_fB>Ee8$*&C(fefk-* zgj|J3+Nos6iJ_&@^ECbRg!--1q7YGNKAD|=Y_{{X{}bgQV6FLl7qB2AIaHt{TJhUR zkazkIbnQI?s|mElyb)95t9 zj6ogz-t|y;Q>PnoHJn_!6i25JqRH`M+6vXfn!PPJgJpy?9JOz0ABk=gC3@!NQhcP7 zG!duyI9MiyJavLm3k-`oHaTKz^!`Xh35#NmK9&W+1?ElH6yJ2aie(Z8=SsMm)YU!k zU7kd-j8ZNPnfwRTbB<c}Vvk7u35ls!2-Bt}gHD>P(bt#rX$6b6t!r`T ze1%#L-ObDE21iv}uZ+s9jvx$!5sN4 zpmaZwHRRDwh4L+Xkk|$Zl1MhW)2g%?xi7pOx@1GucwQeNGfvot$#9fTG7^mo8NAi} zQNGw4NJVI$z)?@{SQRO+&A^a^z;X5`m+hCVRNfPbtkp{Fu0J6qt}sfW5<8nWRYJpY z`hRX*vm+gq_R@G{2ITDKb--IPckdhM!0B@bQTAu?ybD4Y{Pz21`Y|qd&~7}h!w1}6 z&2M-q@M;s#y4`wQlW2K9@HKjGK8y%098vtpLwJP{`PRn2ljP!f1Fv2+-krSKM9!^L--HyK={LW>U)#@No3I$cWB zdlF88VJ+FDEr198m%g{$WDpho!bUQHk(F%~7YY**63E6vLoan#`Srn|IDM3E^0kU7 z7 zazDXl;*-L*7+7e0me_Rj=cwc6Ex5y|%kt{8g5PEpv9Qrt<6N%V_B$VRqwzRzL-zzB z(#l&;#u>AT(pk(kj+12zpsVq`A4`uvz=@wvm}9G8vy4(>BZP`|hZOwBsg6p4ivSC88{2#r2by!s0_b-i-1|WjMARtIM!vHgMHxkkv z(p^JIx1>l)mxOd9NJ^Jacn`e3-|rj0=id9=KkhuwnX~sgYp=EUsx$M&C-Z2ugUECm^YhrcGNc; z{3~NSm^ArJ&necUIgwF@$8{zmVee?x=>7bwe4fzGHcfoA*q0Z}q*e{58HiWm$r9F! z{-_BpP7 z`0<1B=@_6n#T}`Gj~*+im`mU7cfgtgr>V%P=*y;Ag0RnN8K7c2J4UPM&r5tXwMcLe zf%}AogHnf zJeSkiS?49MUS+9}f)rclFd6x|o}RvFmvs~y9bJGzjL&{l3l7AQxAqGDaFwgUM)Ml|KC!mz z#=$A6`9-%&ozeR-%bC4t>^57edX3Hlioey zIGr|VIff|qRcJYzd8wQ%Z{O^vi7HRim+t*Ar)EtlN%+&#)NrzA)Mq2&UTk?{ z56ws!evBc>UZ*-p3`lI52nA1CwX+)|WHYOXD_;rHk32VRYAjI`tfa*Muz)k*=S3XV-I5!70*AYJeE>O zcyveUy?wcTWHkBqm*JA<9a0AjL$O~-x$_D(rxWtkXW(e$CAr>B3h(`{_c6h`PT-uK zfRjORCU?O4dsdY_o#GIv(;WXkWIwXikS9dWQ&%l1Z{;x~29t`;)LOWbaPO9wunEBs zw-O2^&lST3N&7K!y(Jgo{V!bCd-$2{@~^PVneSaiOdT&+x=#Hhjhz3HJfvVGU*Zw} zlyy_w%6CvcmZtA)zEPD$)=zo#*trv=|EbIFzOY2pzI!&~Jz}l0nr~I$nc~f+yl$`! zCu3d>oY7=<W=-*)=+t6L!&NX^{ix&E%6IYtk486gRhJ`ga!tpilAUE? z7n0Q(PROQT0)G3i>lZk0?P}xtFr2rARGsRb%d+tM!TmIAuAu`a;l;0WgrVKgE-1Lm zoVAj4AF@;Y!)fPKr@%E1HpU76BBYhey4hVP=@SqNpWeBc#jpKfKj;SC^!)(gy3do< z4#hZVK4iLdatpf-T<(jrnOP{dNxt{+B00R*+x&WZk#1B`zCf8%VOB@UOE-&aPbSic zN)cR%sfK;OJ6d0*wBU#i?<<&4crNMmJ#Z5G#R63T@5tnGcg+>sKDz2HOWiJ6W=p!(#VsR z_=Uf%27B7B#3a(|bDZ#~&|GMD)KkJYqqNNT2@RjoMP9DPy;FF8c|gRZ@=QT&Sb>J| zo*@~A(~Mz-`Z~M7TS)XndoAo|d0Tg$nbHvvTA)>W8_9D%ID74IIM?0MaSzLmb#t!f zGOe>FvwJ~>$$q1^`RmW^8mF_Rro0Y|Iu|Pv7Ke>P^~&(vvYpo(NR*1D%y=G?;9^v~ zt&fQ1{w16Bpr+1JgluV#I9WFAz|-G>f?fkn=tdjx@npY|M<~XSa!K(u1vG)Cl=D&2#LCK%sMM^^Gmm_?8r_P#U+3r9DiC z!*WJdse(Fd?B>>0 zU%>^t!R~!}3X?>oyxwBQ5fTe!jv~a&57*f<@gI>tK6^TJ?B@rg9fc=TO#C~}GQz34KFiVEk)69ZiopNGa)5wy4vpS#cZ22Y|}5>EPOw0Ygf6nSeNkz2bR zOu@fvT#g6hfA4*}5_f+D8&BJADbi2U!*xe#ti-Xw)$lm7kGD2c_nvhSc5MP)c)6B2 zYuPjwM)QpJ2K=hv+NIp3Kt^TRv!Zcwop)>XDryWHWj^TDM3>2Nco4H#&-Ma)xEbfn zc#?HoE&RRclWJU=M;)odD1m<82&e8JCzvZYmk(Bib3s)a!?0J1M_wm#u$+z!$JS22 zAV2CZ$2@#76pY~J?v7~_@DXIY ziTC=0kSbMrA)8MJg?E(%b-&2HOZrV&b@otH+`=5LRcZtM{*V31Nt~KqW@8#qI9s{- zWR70@m#JMlaa-)9J_GZ*0|~waHaj*L_4y{}qLh zuWbt*{S)y%ABOTy+!z5)J|Bj@L%}^l+Se6WxJqCVy%AL>qR-Jr^w@g>gA8zbvjM_W ztWs&MJdHHVwuH{-KQT;#YZcF4)W}yWx{+|_)O#jJJLDCmYJc8Is~RrqPZ=&-D_6rA zdhOPcL6n-K_E}maEB8*;kUWKFM6Q@5*+={&j`XZh6QkE2ElXIf=mmO)cUrOXsJ&BF z*WP&WVtnyJusE>zOBjDjSZL2zpjQ1ZuTYil={Wi*m`H&z(b7RdBTB(fP=$7&IEFGI z>obBD?Nz`Jo`)@L_mQB4J|EfI_m_9pzHOx;FVAvVmr0LiE&$I18;FjaY1b4OT#z^| zJI|enc7n{hUVP&X249?Y))s@0N4!q4cqUzD(qkT*b;K;OvqY-%&3Y5A3MTaI5t+aC zVawUiO&^#$jugUv&U5Y}_+Wv3VzXQ#g0^29^oEyDe}utOYe7Fh$Qzwv#?#(Wj39jZ zzIB2nyx<#U2jUA|_}p9#V|6#z9*bi0NS!_K@G%QdG7(}XKPu0E3Uj{fXWcd+Ju|W-Q~P2y>^(&vWc9KX zcNS{5ACu75S8I@(^7D$^Q`{2eXSNY`Ks*K*^jhwnO0V6B%95g-ra)g@e@7RpZC8L~ ze9T)J4kzB6sHclQh#zYM85-LI8FRc2hqQ-vFZS4F8q#tzUQO%H`k?nF)8i0{U<*in zVSe>(K2g?rE&U$%JO!TJz#>_!)62WVo%W*>#~gjjUex>0%{LcVeL;=$yI&_=W=}53 zzt-=&FaLVTNx1;`ec%%KRUxM&;T!ewi*Ew+bv+FSbL;WRx{Jcz-oqWr%#9r<%Vbra zp%!Wzj8BPpoKKs-eIHeG{amz{BK`|SbYJ#8wZ*vln)D;iT~X%-->2y2?P2>-&f#1M z%HNmo?-vzZIh}I}PHv5z$6b9t=()CXH$J}jII$xgf2FEN-*-W`!d}KpbdfeMvwan5Zy2d`xh{-0yetB9HAE@&)Y|tBz0cP=D7P*? z*{OO}zdWM2SF~vLi(rS1<)AI2=&8p#?XDnlM=#vV&fF>1#NHuNS%0C$ptE_mfr$`l z*H3lp@*;S}2?!DhT|#@Z0vjVpr(t z+cS)1b=P>bD_ep`T}+u0?n3UEk{mfXMn@r7a$ET*E+hC5XiJfSccx~*BtaCgCO3F_Ne%w|#g&`~bz$emOKHl$>=S)5dQ zcdOR^Wu1YU=msnCj(`)?l>e>@BTv@Or9&OJ%hiXayR|jZTM$h!x^&w|o*|cY2d9eb zyZP%f#Qbs#;#E(@uTD{Oz!Fs-?2LBkTVBVJ$%ws?$@=1(_b4S(><8gi#t3o0P}Q@r_GZgT$w|HbD)*4GXhDu)f=&>Z;gUg^QuiPAmw7pQj(PQPBrXgOfV@MD zyD6!#9x59*-WyrBeECcXE$marr7&Fvl1V4qyuj-vD%ID;v~-7NkA6(~1n9Lod{wOS zOc2f|k{(hVklxC@e(+YD;w_on^qm(^O6v7nN4Y}n%t*57u-d&C-=G`eeU3~TXaD{- zG$vz`RA`w*9{8r1|3dmc=lJ`FAuJXcj<}t1IA5Nus(4Tts6(kSI9~liV|aBuMZM}E z`XX{TGfG91j!ng&$|`0e*$5gS@MiDZhJsFU_eytuX|YwAk&+4fEFIp1y4gt0%rYw< zBZc|IExJ~mBt3M8j#&4mN+JT?>e$>C3D0L5Je7-^3yyAw{f)9Vv$Kv1RSTqZ9#$%s z*m9nWb772iIhW=dn^(AG+N4FO;vTPE=}xFUD7plMzqIC8sh~NQQ1`h78kDOmK(FFj zV{*Q-$r(IeAhUYBm`vSU$x!KUqxA>v?K9;Ph|m zo~%vueUjXHRejhAkm$17ndjo$!;+~@exXvfK1f*Dyfaijv>8^g+vmEW$f6B2Fq zV!{)t`q1BE`p|rrE<5DyskV;IG}pfvQFn#aW*y*XD_tIEtroeKuj8Nmy6pZ+R`>R5 zu}`D=cyGe%v$H{{9UZ4t%gD}L;9eysXTA1*Z%KuWjc^0E>)Mfa{h_2OYY*~!Mbtc6 z?B&$Wo(KysQXhRc9k=ivd=~Uhxq>E<6;C};|8bFtoI9~jpH?}abf~Shsq&?28d7o6 zeC5dZ&$F`)+1T1*Ksb+L9ogjM(!p8+f$yLO$_&lK*0GcZqi z)8yO=DEu|6lli-+{Fjfbl2hn}{46`^EW&vrij==esMPhRtIbx0 zm5^0(>w8EyjFq&p4{5GdPuq1}pQl-lRQYickKd8nMuCV z`Cjm0({88ZD>YqqF@_fG;)P|G2L~O0nI~q-3iAiPMK)vNw1&2%+6Ld{Coa+k%1>On z{3qoh0WU>)C7QU(**|!%ThV#Mc?%bti($p|^XF&lJdR#amM`|MT-cE-j^+|C+|1a~ z(bd7i*vG<_g8)ECZ0L45LdN32=xQ)Jw%9{5iKb`ebJ5<^3N%|XP0;U*y~Bx^QuVeSlm(q{VjMl9C7Tj#@wM~hD2$O%Nn&>HMR7$i6m?~NR=Pxyk^n%3%7 zlc(TW3;_;Xl#3kXhwQZ|7RD30W1z19l?VFHH0Y>!iZ6h-5%tIUgEndb%;N&>hEKb7 zLPxZ_bVz+3K~%E!$|@n_=*2^CS$$^mrEC))=cl+3Z|NS%Fpc*myTvr|k3I+femA
KP+GbXrIKM>qc4B zN1Dab9jh`8-gmS#~cbz z-#hA?f}jC{{>I%pc%rA(3F~~qSs;)!jhfg7#2&-&FQMjn@*D-$xU$u0ze!)rJ&d zNV>XNJOv1CdnVfqUIACXb7d7#OsV|z3qj**spt4IRH0rYG;rYNC%KDJgYj5HB8Bq> zYe=a*|H!q*?W}ns=k|DLoNxJJWpAQR5R9>(_YJ2XIeWyRCkH|Pr{2e^P0t>>=+?8K z$2yqnK_OCc@#^Mg-0*K>cY*mo4cxx0_YPHxbL)2*)dSZlA!uv+dov<6?9J86$;@Ic z2?3jtgRkVojm^LfL<%&P7hN{JsQt0D$LJcYm0o@^g4WWEbOx)|GRw1&!sv`dSumg4 zKBO9HJFS|>>&sE=9Iig^dg5c71W7!1{)dE6A{^A%5^*h%E7N+Ax7S}9xUiIggDGv+;DgHZ; zK7spvB`!&1%OZ=xwoCz5?|`iW4qmsr5}ncq;q*G40qBN+65u2+v5?+KaTD2i$zw;j;m6Ap-iBJsA*R z&c_0B#dsZjmid-_E9>#8x-EM2Z+cXTaigN(A&myf#kDfIxy5$H$`dj&iPGCq4`L$?;&_D(fOAQ*ao2nbtW`bD3?9=D!kxI$>L&9 z%JaJJzKw?nDMd-%z738JY=S*j1?>8ZvbXyhuzR$Wqrv{67(>n9f z1!^-+Ob$e2T6po(_=17EI`WUYPKLDqP)7V?_6XRx1ZTPaS>u~|)9SsfZ}7$V&Guf# z3LGiJKY)+nNNqC=Z#&f&8_)b>p|!aewl;tU|Dp^qXb{h*PsX#g^9!R(>sC z!@1e!(iO5i1ai{8EdecxeA!$oiq6Ilb;xm^lCk13vb&NmyDcL+CdH?b2*zcqISb6` z(c|nl4+Z*J?kU#X#+XX!W%>`|A0IpKUaz>erZbuh=Xm@JrUj*J?2h>+BKyG6>|>=; ze^h=q^f9a@sUX%LQy?}_AT^wp4Qy^mQ?mQu7g0v{cI@PN?OY=DkS+yUIa51iqhhCq z4uFKNO07nWa2RB8+z=Rsg+SPuJSX`1>pat#rzbI{uU>%pN;nvfn0oPm!q%@j+d8Ot zWh;(s^k>W}&Qngb7>zhQ4~<~+@zX!EabEN0hEyV$+>9@J-NwiX!z0S_?)&x+Jk;h~(gV+qnYF0!rdx;h${z={J6ztMPk zS!GB@v%YkL@ub2>fyxPT1^Ad8xg3x}C@iNVrX$#YMV3gIKNrPy z_lnQHA;2}JU-`Is4ped1a_1?XkX^r6QdMWL0GB`B(Ny%5?p9EtYBAn(EX5Xnw~4>7 z>G6B>V*62E5&9s^ZV({AE$Im|FaQoq79ILwXg=t50<}#fv#r$&ENDC8km`{-0kERODe` zA?zv{x!+HgZaM-1|I0C%P|Qq@!8hFr1`lXhGrJ#E^0HEA5~*1vtS}RUecU5D19XyG z_0}p&^lJM^-aMoCLql@RjnKiJWiQmLzUL;~7rma356~H-L(1H5_m^NWrLuC~(S}dB zo`ioUc)$2Uf>Y)Mmtd7anKGyB#FbzulC_X#W^rOn$^yBd3R;gA`V7LAUk zAsX4sd=3H?#YlQ4_}(=;w~m7>Nisn#?4zeC#eClV<6qp3wOcHF$w)^}_VY_hXVFR<^P&f9BnLPqj&o0=P zOR8o2+bAFYufcfixN?3g3cK7H84u3L+Cq7cb1J6C+(K}~{`H-%h zaX(CNuOBflS@&h1_%r;5P)d3}-H1L6uGKsXOWi)3;QGAQot=zc{(M>gV?ru2v%`x2 zWVW!P%KlRwHY9@?%)p{lADsU66Sv-qNoYp`xQ zw!E;?Gm7$dce%j9Sj*Dqjrd?qqPQ32`g`~I_WgLIFZ{hGfk#QpkcF`@{;MRwvU4c) z%)-|gX>QyT{NWno3{<>tuEt}qwN2IBlmOCS%+KRzf7#Q&?G2)RC1lkXq5rW}=0G{u zxC25HF3j7rfDq=4ckYUase<(QMKsp?%sDwIA$ipZYEI!TCf$Tu#p8f#SPz_fyk5xSeMmh2)_JE?>I-~U`-aE7Q-e8FtG25LQ_Y@rOA7-V8K{>^llqK(e0hzTW zQBIk(m)!G6?FmM$3?YZwS1YsKdJI25--~}Q&y(@meNCI8=g66%ns~0a+qHnjcZV>Lv4($cBhfH{>NN=*jjg885Zu!jXfmIz}Vz z5-fJJsq5`5dPdkfFw7p6-9=qc`N!@SFgRd{b0e$&bx250XLF`x3kiJSrHmW?OB z8r#vI_5ZM^f=U1f*nD>}_A)NXdpU)57{3VvkC$&N_oWg!WQ~bNcbr8FGk_{F|5@#iUx4T<^ud^$nW^FA(EQm~N-iGnhS^1|ol&HAZHel( zbcLfWm7DeD+s7L}etI>Mp)=&%c{7cd>pF?W3m$*?2jSW~dsJls zEPN3>R>c!97KZ$NwarlqS!P@O-8OUKJKfoI%;Vk&-?b6m*h)YZ5@+b}3L>Hv~oW+*P+Ob^3 zAv(i!?elO-fklDB`hdIrX)Jj9D97M-%9vzSL&~x{2kC^!boe?Bap~D=zFe8+lZ8+- zeGEGH?&n9PtJ9nGdhV;!u6>vnJ@ALRB<@SQ{`vc`@VP_%j$LfMLo)37z zhIwB@HEnBUj*;*_iAWezQXw)Mt})>v^R!~J=%4FIjQC+s%Fph`37Fk68t9U{4R-Pt zBTGk36A&=^xP}hv&;k2?5YQqOUSl%g`eDfShwo40+tkvg1c2jDr6iomslZo5%Y=?H zS&`Y=-O7EOR#rwu0Y_iyr?KKIyALceM*^cM)c9_)1CBU6q$C_E9>*b{>m_t;FZPWk z;q{XkY8ES6;-vDF56bI*$icO-{%#*bV?0*1JC&Bmi1euuw62IXX6QgOf0T`uGqHg7 zqP*NyswgAXX8(2Mxq2Yl)&t%p6SFUKII@~&JXEVR0^()2JPWGS(u}?jM*NXL@7%Gxu7g2X(|C|lQMAxGzxue?lZAj;xnPnSn{%UaT^^!%z2=YJbOt=?vR@vU zNvlCILR%{?`M%p>lbm3#iO6*M<`lb!AA4i%JEjWb;iaaAp^Ah(gjT<4<&diOK3UxI zX~pGcTf%xOfWk0(hSt%KRm9qiW_=JG7zA$~R`()C6dt$&c_4XRXq?l8J}c(Y9DMmY z#93frY>fVKiX|te(Q$Tnnvm0iw95{D5<(#(ofa2ws^ICOp{ z!LmYAYn`Lsq1_2DHpGYa`|G8*1R&_Bnlen{GF1p@j);^jd5<%a*(WEh-JCR6*)nn? z2wKB8=8rQbRTzTTu!$E}fGZ9+bJPXUSK)~6U++;!8*^j!Zv>LI10V!-szGCAwSAGJ zTUg%22-u66sH^3I=?&`u+i0Y(Sk8=&#^(jLv-&q_k3Xmx;t=zq5d}((hILGiVCVD5 zbsQe`X7Ov>q-gKCbXN4|YKl>N7i!^bB6JPA!g6}oA#`sYzY4TN*3c6k8#K~bN z&zk9U<-xE7nlIX7a>cVaV!XGq51-(U>%KslG+Cc`OBZSWx_9dH7ET0ZofQ7(KIr?5 z-?Z^?hJ4`7xiFKkk)w5-44WTZ?PKJtw*jhaNS}a{>%CF<5EfbwK}LnNfS6J|bOn;i z8dVqug^f)a3A{$FR7kF-p@1RZ_V%c}V!><(WKOb_4}n)~y}Su>ivob;R)i)w!I;Y&78kbhcWRwgpKpvwX}|zy*h3(@!HTf?>kR22 z?&nw}uQ6!;05H3yGS`U?qG7wn`7@a?vp_V3?rGSM*K+-qU0&u&G(*Ylz)4P>Nnl^= z;=2biBo?A)ogU#1M5DWxoz~!J@k_}1oClI96#GiNLRApt^K+c4;({tN)U&ah1Zfa~ z1&u{&%DVv1i_J9uF!)*)>pRW#*DCW>_7xOsm%S-pY z{XM6GW-c)U4BOwwO!zuSLAIRN|@wn`odiVL zLza+g5RBLhHd3qul)!AMeZy~N+QOw=aJC3b^Xjib>wd9aB}=i z#yjio;>kAkpfnM_u?r*+;;r2rY_8opy)-KsA2iGF?yvL1ji)mk#^6Biy%l^gYfegM zM^^U!U(N{fOS#hd9T{}>Bvb{2VDZVzB>(zU%=t5JyY`|9S!xW!Tk}~`m1_~b!GW?u z8_z^F%yE#ga<%1N>v+?$UQpxdl|Bbws{AtF$~l4$aq#DVKH={KVfr$GnyZ%&Qgc2c z6OS)qHhw3H-|yhtj_$zUR-OI8qwCDZCzb>2;+YCXyodW0p9A<{Ti%w^UO3Whg${{o ztM&@tziU!7P3Oz+jM7#uJ|c(TmDIiB5~s+9vN!E<8wqbF3wF0v^&Cdvv$Zq3wKv)L z<^&X~KlR z%!qgLr}ZG^cnIIFrj%T}Cf5y)-P{zu;#|(7jx2Xm9{RY=2`Y z>&O(ef&T#bj0H~SJ zphWXe?M;~=K|-1CzvM_|%!FNK?Ef4IdIBbF00|Sy|6}r-vgH4b-c(cJ0sp;{|GCxw zd(!B?6ASMDI(MWZ4+LQX{wt(u2%LIE`Luf{ok3CTYcK5 z??I~xaH{B_CjJB>Rb1Zxyr~6{0Knh;-u*w4`ul+>J2O|K|Ica(_NoLRLfHfs1srS@ Uc_`?=vrymRdSApYIJ4h8=0U#Gu+ zH~sfS6aW17?~~y_Oz$TqngPE}0)zLh3SmJ0`k&L!-xW6g`P*O1D0C)A^E~}te(`+~ z`pYTgd7@;`vB(WMohue{EQ(N~b7-(R`{`OSKc8J0p<=m=tNcDO6`LRz(Df9fYrlV+ zVhjq$7|XxHTVa-Ztr$jpYZ&VSZfK}d0be^i*yR%aD|8um}|_#_p8)P4*fR2ZPC^@USxf8E87hx3JhW1fVXeyK3RF^^XDON~JT zJjihxPgD;a)EEngH6tD<(Wmo(bt4|Iq4$7IBOWNbrPIyPhzFeQJ>Y!PboXrNIC9f- z{{-rI(DXax@virP4^7YgDx>p&AMkT9z5T(C*dKb1IAVWDz5QVxu^+Lw@kBRbfAqcm zabPva+}odj%!vJo+6tEXV!#8b&Y$M=8h<3uEm2{CxChr^N0~KKCFut$Ht)tlr9!l5 zJam-vGmSs{jw9~I^Gel?Va@J|QK?9CdBNQ7sZ^;#Z$GlAx9UNg76?5^u80}-2X#fj z=^M1Jph=@k;tYpRXq|UEYfa(x4Bx3J^u)Ij>wlkrI@U};Y6*Kt&$V@>$1XmgC-`BO z{dh!;4nd_3$F`yiQR(S?EZ;2F`mCBAbyF&RospxCO{FK*?WiZD^e(AxM}4PC?~d(t zcCpu)V|#60?6vXOUKo$M#ZQ?4@vQFM64k-X7aaO**o?18S3WMM{2k zXf5)8y-H7a*52Gepz7fYWxEQ%)q}fGdNv;iQ-6|@F9u0oGm4U}8zh?156~+l%BWZL z4yj^t;{iQnfM^(t$>JiC#3UKyH z1@2Y^>m_r~ycYunQDNo?97t{fdrDQI%+|zz$6S%)NP0jK13EJ|%o+I$EpUvum0U2~ z+bM=>RrmvHVv!8^dU6@l8u@v4_gB>Ru9dy}sqEILz(xG)CGZA*BG1>r1A)8ktF{p3sWB(V1^&*SR6!lrmvU}-nMHH_Of z%qthcB2(bvTN@NE+S%Ct60BVOGH-=5nmvN?cyA46J%n5IW2pjNZ*16}Js)sbWWT;t z=R5Q`*8}Y{rttMr=Mk?Yx=Z$7Ik=XeGF&I}`hMbw`}@>fZKfyQ%I^~<2XH@sp}Uje zg2a6|6tKFvj5s%#a37rr0dkywkU2RF5Pmz>mTEuf^6#EAL;26iOPZVOqTT^K78cBA&&zm;E%EVYn~}Bf6X+P z1FCS9e_r?(A1+VnnLaKQpHzZ~cTGg!|R2xzh*EmMljm zPNe!YNpkKk@3tr!Nb}Brig7+^(eG(6X+Ef!q#zaJOhEb36;sJZ(iX`MYrjnLMx~Sc z72#JHJPoV4>b&SPFIuZ)tNCqb&uHHLE!)49cL!9=^Zm*p#9dp{ZO;oxeits`QO>;; zRU$c}~4`b0%R%Wuh}qz|jB^$!miH z5?zM9B)8ni0u&S~nUix0my^e3g@bp>B@k#EjQWIOj0`Sl16f`k2`~DN)mDFI>o$`1 zh>Ie}N9x_W&sf`XNWMtMj(BG^*!_Xt0zashcjmYnK_5;$u)EUO$@|wzC>@h1FZU~E zai%PEL+{Qr%+*F9D-c_=Z>2z32=gqLKyo3Y7-6=0N>yPi*3Y4N^>C(a^Ig!J1W7Bdg}tVQqe1 zdqrB~g+r%(>GIvaJA-p;sZG@8ex~Xod|7zyhozw;N(lH7E)+JA@pzf2i6*Tgem#BQ z>`#jaV>c0fO(pB`^%B|28sxj(B{JY3JtCP_%nIFcR~dCts$74wb!tL&Gqmjp#Y_2d zS;d!s(Sh7j7g%$jrq_j?QBV8XBaIbIZx?=CLY`7(T(o84L@V+R$arz8U?bTKb-cx% zc&{bAZRD37U*1oclE}&xYm0JsRIJD?m0MHEw{$mDAJ$apAl*{EuBlKx-cqFvDh~3u zRJ5k@Ywh!z3SW{PdII059eN_(`P6P$n9{v}=gVSSQDwXG>4t?f-Vof3=vP#n6yNLq zvO(onJ`HOsax1;oesGpIz@aR+(w;W7_kJ}$pV!-HJE4+K%CthnsUQ*QKB4mXF2h?T zbod^~$j7t}Um$p-QytwK&q!}G<>&xaQ)7&*~lDnjz5n_IvvE~^M!1_a{^E0hwE z0I!|i!wch2Z{^0q$P@nGjQ>kh`G@I$hu9tolQ<6@0y8#~R}V9PFHB`_XLM*WATu;D zF*ZIv3UhRFWnpa!c%1CLd3+Vs**JdAnSJiey>s_9$s{+qS#BVZ1qennvZ~0wNRUV( z2@tkG2ndP_E`Ye9t!u6OzLWp~17e|;+FGU1*5%b!r7bGjqP&(;6?5CUOIpAnt4xbU2z&A=i>;?d~!i+bIY-tZgL?s|6;hu zkOfe2P3k%cj&FeD!Uan@SKsY@s|SvshU2}9+vheL6Uz}cvW}^@J>(NkjFGJgxC`BfKE!t_JCOxXSqCkDj5^T$TqBRu^V8oz5Ax9!&<7rkM+fnK zl7zdrqEB!LujNLt!r4K->7-~cd(EVr+uEAqTzA!sIK79}c;2EpouJ@tsXczo? z(97t5J*){Q(vPQ4pfD;&7s7bD(Qoj6F4em}RZkHOSSm%eaHbvo6#Wt%#&P^J(k^I1 zrCR3u*&=-wSvD04MP-qLD+~WBhY}{An>KbtLVC8A+fYp&z54VI4+l;uZKh z{5Jjz8A;}lACW(EKjfd`eh>^iS-;!|-H$DPDlr;*IzYd>=lD594Fx zd@_wJB42O|xaHi-{0R6>dIx_2ebz~g^?4wvAtT5{ z_+3U?$#SxV+)290+vFQg;vAfd^K)g~Sgw(4Px52=Is8igLH-l|6JfUSy6};xic7=|VvqRW(h%u9X_7QqYLsr5c1dr_O+cML zM?2Ar{dpcA;ns7bxt-`XQpJbKZ^&B~Rl`Ha6_$!+ zq94CT+W5`Hi@OowpMlZU;zG;`K6C?a54h){_lL+&xbgf6VG3RV z9B?(-fR?A%qcy@z{&#pj;_wVq#2*3aww9~p^I?B2NR`G;lDp5IU%++|4x!KB z_Y*icPk0G!=HEe6QGNQB^jkpPB_Lt%LzknA&`}uW=kSCJxc#VqDm8&@OON4}!FWGJ zlhaS7^RS8*q!*)!XfJv~5>T_0ggNfQzk~5!g<8p!bSKxEY6Cpn4p^`NTPs2S++>ZM zKJ9#~{=9SR&KX`iY-mk&RpsD86$1y9C(BApiW5bJ@q&CaFE=L^jf6wNfZyl!xDA(6 z*Bq)MOQOJY1f%kQ(eW`&W>=!Ai%-NaxL^Q1jyFR^bH9qFE)%N8oZY6&Y+}vKvs+rw za^81ZT6RmTuO&9jI#f5H+#DS@yAF~2eN!SM-Sgd zkgP+ex+3vWqr1ZKQM3nLT+!&}maa*YXO12f&ChQbP~L?{&W&H*h2kT+TuIghjbxAM z5=VAP>@j8=9SyqK+*ZDS^DR9Fy1Xf=wZvPRXV2{7nj7dd+(~%usIK6ZM?+^0;pU!^ zGdK1>8|5~Cj}En&^l>s;dQNq^(OC*|gahPV4FQT|$u&H#h$S z0R_a5|LyFW=1h%PWc&kBkkUyXsloXy+?7mrm6cHvkw(ItLLbj#$29}WSM`v1e3@aw z4$y~xCIN1n8-`Z^`t$SY1mE0aq08Z*Yu)6T_OXdBk8Vd+MY5rbG|@BrvuFI%>6vxe zGkupg#etH$nTqW1k`sOZTt>hY*6#_ z|HF%V(kJLuY!N$ z%;u)e%{}RLm&Z*bzIiu!g}kzP+32P$;q;{U+#Ky1b4vr@Y5^V&OhM3yZE?J5@-_=^ znmTLdZUgM6P19y>Czyf~%4Mss_JU1wzL2-NX^cAdrQ=pU6aBd5*mPd$t!8wLP163i5A0fEL2 z)?~gr-(3U+hDG;VliUBTC7{#D*92ol@cop0m0pxMyBgM>5~l5vVbECczV)*-r4`{=YfGWc3scS+=Q3An8I5$Xp>~M^jP65mBaEQ}`L6l#R3sZ?gA(zW3 z=k}yi(DM<~0r--&1>lsWXgWZD3FiZ1Px@H5%SEOGaH?C^8F0j^rh^qliWafaEL>K& zu8=D%2oZXxkdxjfq|#f2Ol-o;nubB9BYFpVE0nzz0N+>+dMhN-&cX3Np)(Bl%2Mgw zQDgj^r$fmJ#>u4p*H|~c7j|_<<0*O=J{md>J5*n1z^}fpx6V^rkxce~!a;3?Cs>Q! z!Pj8Jra!PE;aYb=xYh%k z(~8v^1wPpFVDr~zZqiWY&aVuB>LN;kU?Av?a|20nA|7``^^hSo)roliBV_Y|H?I8k z@5h%+zc_vJmFY__8IV7J?oarU8}FWQ&ts`U!k&r0TJyl$IYosNR;HHY!8hD8%pvuz zp-$ZC%D4!R7=yii%iwji5k&) zh0TY;=0jogq5MD22P0z!C`w-qn~@XQ8PU$U^!sLHf^n*G{K=z#bS`?cGf`*s)(sv6 z81m<5wnzEGZy)56-@e7&AnbV|RsVcSe*tsY9VU64`8z!Sk@P1*0bsy~HA_vnX7V%T*JM6GCpO^qS6B1r z$Yc16}ES`TarQnI#oBR?>?@#+KHM7p@{MqK*H zi<-MyHlYAH*)VueGKrHQ7rSL^Of8Ii|E@8$vQ=q=m9-K`NFb^Yc7*|~v_V=MXQ1VX z*Gf)*A8cNFd~%l;z#JRQ0nksuueS9GReVN3CmW!wa#vxP{y26&@Jo*D`Q^7MVbAIH z{I%bV;n$sB2bj7SA<+f%Y;ZwKlgk`Hw1k!0h&>(|RyBq)*{Qo3{eRv~0hpT4tr7}p z_EfnvPC=N+io=Oyg{TgZPJ+XrhzE-KW*1w3@C`uq$K9Ff`6@d--`djy3sCYPD?kA3 z-)|f~yx$FLQ8H;u0u;58Y~@KNoj#G394mQN3apeV6~!5kYOIAw=a@KY2{cC;iIC9)!d!G1+2& z4%4xmtT@Tm+4{OVdoYdtrAJYFB+RPQDCad%61%TIGpNur0ioX$Z4@d*NvHC-Ph z$N%fjZFkmNkKr7M%I~Qi0m04IAnE+(EmKoUSAv0byg(hL0zzIF%=^bv$RbdeFAm&4i zqmDD!2;-ySw>SOXr{rNOxpIJX<=z#LAM)-ugRPrtbL>g?!|@p(m{c64W5IAmr^sfre*?DeR;

XaifFog`;B!ax`dpZ1Cfa;qm(BV+-dl46j_TbV86 zJX_M`Od}9q6y35&OK5x2KBjce*71|p`D}G(lP3k()Ub07kv*7Ar(nxWC%Zp`B?e=s z%dl6x?hIhlF_kt}!x`=oSXp5VGUm$*lqO>nx5ao(cvakQoG=`+(12%ukV(b@N0;$; z?eF^EoeHno}9wd~lVL z3pAW7a$*lzW+_P4KD9uJBYUs|VeGIxnu%H^ZptM75dR_1ZLzf#%P~pYFMX(STQscE zW5XpKlE}5vI*CX>aJ~J14hZ^_jmyKZ!N1UPn+T5s4c0}D*B=FRJ;*>$Ubabw~2MR)3VyYBYqW)`~v z`3X-n5m6GQIDt@U#KTt(PM}t>!}S4cqBliJv?!nt$Sp3yMM6NJtZFl9?f@k>H^5Py z@OTnvwh^|>t`XCfsN%T&i&_JU3Z1$`$dBdZMnUBV&0oYtg#}OtHdAy!#G)i|djv<0 z2hada6)m=Z_fD)(_m-eJT z>vlTH^itaGJbKU4NM*i8ugPabn=jMGfB~HFP$iKz_GGEm(f{1WoQh4An0nSarqgZ#QhK0Qtcg@OR8+^ zv^Y^%oJiDE4;fMgR%}p8B;p04-xmzMB#|1$OW`|vx)(pxVXv0~Kd`SDYEj8KJ6Ekdw4(D<%31rMSPz`f;Q}j6Y&K^lVHO|v@9aLpEI#gg z>^>$QFxY$qh-m+gVdqw}eO0x+A27(?mz3K3c#*x&$+h>P2-{m_x>0QkTZF9w*a9Hk zZ$}TKF2q+L3r#{Fq7z8)m{7R|ajZF0_E9K*BaZ%-6-S?E#nCCtu$Km=Up>md-A_5E zE{N@*#KJT$IQbcWgvW5P25JV z3(3K(T^P(#ZIE`OfTAauFcq9<(Y`56geofSuo7Z5l!%MjG8xIsF&O$l%eRKKb!K%W|n# z5^VMNg*TBCMRC9y7ja>q;qyg<(J0Rwyw4GIMEPfeyPU5&xnM99C1#H0p6Hzzv?4Qw znaU-`boU(Ztl*r`jL0R?n}hceBb>`|o?M6G&rX^@J86D8X^f`*2~)xnl|@W{DWaLLM9ZKza`d2vToj>5{nosY4_)Y2^@@u>V|M->E6My_L_3+ML;aNXT{WEn4SL5go z?D#?IBb)a5UZ8y)h0qBrx7EGKN5&cBe3u%R`gn(y3zh>4hM1dE_GD4&$)c3X+g+5N zJqej&*G*wrr8tz0jNx_C8bC)bN6ZL@f05AlI7R+b-=>za(f`$x7Vgh~4q1MfU@UL! z?+eMg(wHNu5!oc2O8~R^`OL|frDjrk=lI2UHhi9XEwu?>wfDisiwED3x=GmM^tA3; z@=~g|_jwNAa_#Kv{W_Jr^MHm|fliZyzSudJEX*O;)<;+mk6CjlWTHxaE?R~`RoUE-e+ULY*Wxyp01 z=YH-!=l!v#@#Exa_ghXc@}Y>~Ga@{73T!W_WsC6Wl3K%sFdy~iYFspzSB!+~B9t&O z#*w@r!+Vh7J*Z}cmyBgbq%uRT9M0_{&n$7C0wA+a!pg{`Xd|V6Oppy7I4IfRjTxUpY7R^N)_F-ucm1Jo1%4;_`F$RlV|qXZ|vK$;TTW`!gYfzc~Ff zy!3Y;;py9syguOJJ0DGbamP!kPdD$SSPdg2t)ejnvUWFerBk*sX)kO}n*@N_deXg_ z6I|a3#*38jN|{Z6B-*+zTkF{1c_w^0RuV09*-55=x+ueE09#W5ZS&1;>i&Sm=}n6{ zLP}A>UFu1MYS9pPt!GH+LNwNWp=WGpCc4Bu({oA4xL3Z{MKa1-m4PGSq`z9I)(AZ+Ic{`?_Ldv$t?eE)1Omji{)?cpj9LGPM)oqk0w>h$ z&@91%Mxf~7jz-kyE<5v=kE^`Q*st<>VCMy+3{!i54C4n=SN-zC!pJZc2S5AW#CUAL z$3IUkeJS;Nu@v;BUK93wTYt}8e=X!b=#8ZQ_V=5+xu1M9hHt#ZY#n?0F&Zuav23BqR2^n488&+laQK$H}6F8VePMSeQ_w{TG?27M(0)Xwd$OgNOAkeRTC(Zre7= zqyhHIDW(zjMG4r-nEdD?D$tK6>yP(;CXc#Vps(-R<+fQ^%S=LQ05QvlIxEofB5K7j z8)qo{)l?T#i7yqON0`wQ@cVs%U_8Nzk`tBzx(YxeuC8VG!mWG9uDGCP(R=f8)#y#v zuF2^NEj@hGre`M^O0ZyWEO_~W_Suz7+7>*T$hm&{nCEVsuzrHisYeQn)TIM|&TUv8 zT7L65tNEgVt52N1@!Vnf{gRkbGQQ%1rb{QDdj;jDO|WFtQR@du_$jb|)H6aq3XwHx?U@XEhs#lL5`IoD)g&fnF8#|!``rta=;{;!7)o%ZvI)9=x>eR6c$?)FIMGX;iH&vP`Tk|F$#u%6Gs7Sk@2jG(263(LG5P;#fn3vvco~uUnXW9R@IQ|v;)IG z$Al1_oAT-*)m@;uH659ldOP<<-3_$cCj$&cE}G-o2Tb7ZWh^aq{Qg$eWoq zqo2KD1MV!4lVzfRSQdz8fk+m}$vW3#Ss`Z_93?T}* zvw$~Cb4C{MWC3>;(6h#*oCTDg^nY37fuM`{qx@0jPr;8&;Vt2mNrJK&S3*%!;kbBi zO!UWK)`7m`k+7j2F2Y-i9xft9pprR@wz#pIr}W|u(PhJb%~n53HEtiJ8aGuHf|PRH zgi?-Mqg3o>>z+GnP`R`IeRmITv~ojoza$hU#6m?|qBzPP5bb+FlszCyqknGtfG8u) zs7fCIwJDoTq8fcvG`qw`=@Uv2sfx4P#~Clg85hKh@L`0vpobC3L-lAP$aH#_9L54H zc!-RwH3(&YAj1gXGGWCu2Or}ICaDlxCQ-Pss0Xj!kx!*oazfu?$mABwxj1U{t7OY( zzxA}2X|KyWfFJ6BBkIN*#|>%>Q9+ks4JT;h37_VUVvp|6u*7<5VGi{*}i)9r2FsaeHT{5QJ`NI11ISyjQ8#GhbSJrRCA<&i;_btg~IHFM^eMu zSn&dRhS(s_7u#gH+8FK`9;gY8HpY3z1xANv3$v9eMx&=OFeS7^SfaETOFT;gEuky0 zUl9d={ZbC>8ue0bG1n@zs*5!>7~>^3kZb>3ij&#wimxm$oo`=ZlzGmgObAnF3l(Mn zG4GjVWNJ@jT`(uIE|?SDbcQo-8V2mQoP|ZzgCvY3Loy|fI&5sevNZTZpjp~t3DwX5 zb~0jf+Lj+9KW8D*oYd#wVdSMT@`^F?ax(INvZ;vi3<@y1vfy!)JPFd+s?P3aN7m5b zNcGUn>Ubu^Ly5{*o@_h?=JVN-HtX-9F-Dq0rA!s3DwhkFD?Dy&VDVtDF%&2S*-m8S zWqMT&qmWUL-}H+=;J{UXz4^n`@!i`uZrHx##*N#F7Z=~QD)pz{gMYma=VJYh*WdVm z?Jr*cHFS1N`WW{i=t*vrgZEi$RZv%o^lE*SF4XvHVwaF<>J;D9*nH9=v?_CbO|kuX zZwhaD-w%J}{mA!4@UP*Ia*pJs^8$H!$w*zGE;25%EN@GmG>{bP0|UcJjXsWy*2nlR zj9sG6(C6zPiJt_%!6%&t_H#~$;X=`Wm_u?S)ejndC@T&r@FJ5mRdo6L@$ z4c+$>eL$T}Os02w7@>IB)Or}9cnT#JjG}prBc|_@4oM$MX^GF1>ZOT5bh(UwVwtWk zLsbAg>U3r$LKQ}DvhHXmi-&G*s#Z~zoY6{t z4OPZL8}~370-6-=M;Y9()&tkRwQ}K`*Eii=v7^^~e&wnsp1x}JBO4yP<@93@VQ%x} z^NI7DF~sx6uYdOHdv6?|TJ$)7kQuokGyFgu4_d)I6!VkmT%*vaOn0<$i-dNi)gk+- zA(cVZQELi?a$>YB_Pi^6<2x1M2YZHx2glC$jE|fjo9vk#o)T;JEQvJ7R*S3sr^u<0 z0g9bV4+bX%XrdGsh`F{H4;#cV_-IU(&>r$E!eDOEX=H9KHdO}3_%3gMjCTZWwm;h) zZ@VFb7Obs*WL`tvcIJu{_>4`lPH&|YmsNM^SdZk<5Jgd#wtAPeMY0h4m+ zwakJl@D&VKE>cDn&M0UtSgYKo+)((W_qp;{I9&-wLcu}f%HIwOQ8JwnqY|s3+45{< zwmRD}Tbr#flou)s)rF3Q+CsfM(OvAKp}4}*A%(Nl21iSxrKB_7S-7t72kHabohA2_ z-!SyWb|ik02$W>Cu!1ZQ&jN*6ATwSuI|MO1C^0)sk*b*=Sf1S4S#oia zrt%Rp;pZI#b0XBcQ4lU?0xDb|o*14J-Wom>7G2@IaC`W}FrOE`Jxszc!(!tHCSK4QA7QoVD9Nw!Czq z z*}D*hZ1s+Rt+u9(e8Px?jfhc89_^dSsL(8QF&)9h`tCAmV;41v60{30mfq!Jve(6u zU0ekpB1|<(g2IC{+}OB$e3m0A?> znC@)HNycviN}*LeAU7`f$`fwGV|WctEYQs;Qc9$MC>91lk?VuQd}ln03gWsZm#R@* zQmm+Al8>T1BZsPyN$MuG3#LYvCD*TCkNPbX)WU8&Q$ZVfhX(9rpg2)HkknMuINf(b z((t2k59ZCO-|o8Us;{ zft_*1%)5fjiDOxJ(lI;0?l$eynLMu#vJRsUc2FRjRq3JX3R^mCPo|j31Aenx(}{gI z2Zh|GW}n#K?X*2O`>hHJxh;0Ax;czAQ4+D}Qjw}_B748WjR~*vZZ%^Ie#IEe4fK0| z@SySbLE}x9V3`RzfP!&`n4(s|<-=ussY)(#U+TWi&C&6&q{ky!uh5aKI&i{L^75;V zSWa=ZLXTd|E3D>4P4S9RCF~J+#ETBa;gmfF@^U^YCPy7PpnDfdWpdJ4jcTOf@;S~? z+*r|)#>?XzBVA+N7kMsqP4O&}TIBhEo;Bi?Qm4FI+~eBi`GEjbEO8Zk zDtyDxP|p?e2Kip@9_=aoGdF5ZLOvLqKtt|F(W2qCA$=~ycQW#VmXC|%@rQiRJWfOYJD^NOm& zolukRY5Wv*wtKOAt$VXub*nswFr5y2V!j*SsYsrzuyaq0qx5IX5%?Fid>o7WNP?oO zGLW-sxT#`1Zij$8APz6I=BX~H`E$1AF(}9XP4VKDe9) zgNp265F%Le@UqLTId#@OkFIHd^j!!-u;_8osITffW$3s`U#73qIlTuzWvS*w6}PL` zszmJ}(=BD98@Ic!b<_0u>6XI~aFejiPVdA|?Zl_Nr{*yU8y zxUTA{ERXr{Hfa!M75P)gbycwlk(@rVO)~9TPbPVfzQA&L7pyOEZ;%9sw@ZWQ9k-)l zWRLx%zMJ=57i8DD(?@owCU2r)nIKcj%la)9;bBJHI^yvbr$+B`Fi`LqVLk8)+GLmm}-8A#rl$BzV9Svf@H%w2UjseTTTm)jqKVZ8(q&;LI3gIE! z=Jj`81J+L-HI8R;XpWw&WA;`rSX;n^RaM4o{a$w}3feZ8J6==e9_p_0$L%*Mh!MHC z?()UuH{7se=T2{bvLyGBhm7-DA0=~d!P4T?t+(|4V0?K5Fg*$M>f@kSNATZwqabx- zGc+<|Lt&-MHauAAmd`8^R<-k57j`)?O`BSVIK+Fvm=1wKgq?m#W+>5Yo0B2K_MWi7 z&cQcdwe#E#G*;4W@SvzQR?%=?Y@~N&aH@A|u*usL{1N$o5qH1-xbb*Ill8E=khF0N zg_RoRe~_cfE_J7-1+)#?UkK+cnB!`9UF+gpm}<9)K`fW63A#nyc1O?&q#&2efzI?X z239!*?U7Lv*DG`t@X?cQ0PeJfw%(Q&~he6Ove@rrC2N)GlT_n zEGBv_I_>y>0y^0^Lhpz#jQJV0`I(*OXEf(8JS1Uizmbf6#(b0#bK7SuIoqlm9IZak zMtO3q;oTSQQ2SYMwh3?u4qwy&GW9fsqad|_AeX~Ye#$wOv zV^mw?>bB*4@ssyb|LpkmrWgL0w>5n2tWD28e#63l+whIS7Z2eatUiy)`mK*d7cKtz z@85pqx=fVh7<e?$WZWp1qYaWCBPyk3X7E}ELNiQ#Yz+wn@}ROox)Q8DS{Oi zD`7c*p3DiXuvm%0-m0QmaTC}}S*0{S5;zec%K{Gvx&r9{A0R$9c|JBBJ~ny2BKwUV z4e;X)~zY{14^b3>_W@1e6e>aSk35!tuzn{|2&SbLcV2<6jXJR5w(J2)HYG~#>XG%okJekc-7R~uI_ya zMz%IJnKTK%M+Q3AQj1*}86HVCj2>LI13l!FVQ;ymhn$xo&frXr^n-=<;a$HOKWp#$HO1$~p?|5#d+}G;`T2Xjrw=!O zY`*)Yms5Eunum2Gs0XitjB=w7tnn3IZ16bFSMwwJsr)>?lNS}YtjLP)bt^jJWbBC1 zkOWedk}Wcp3rsKek^(oIGdG(>H=9NGzn{Oa`dLYs5+yp9%Y%v?^>z zGYk5#QnOpwu4h*2tkkq@ILTshYK@^UBlvSvpNn&Fv9QY{Iz+GKahVQFGhNJodUu5@ zl9Bf#p@WgIL3eD#Oo@-~aK)(5{lHoht1a=(aBWpNOSfFaWtI%8HfSMfiYK6lJjIS; ztymwT4bf|y_qiPLtQS-0L@iJ zT-fLJ7I{=39Jn-4O^Y0=&*4zLG=0S((i@~i4(G&-u%Qw&>{sSVO5YTP6(tq~1c?5Lm%5~=TwnrlIS835$<(l

QC6>|Wn_4MPLX-K+O60Y>`l-+#R$cQL z@m@q3&uMAAGpkYLIMpz_LPtY<^I)DPvq(q1`K^3km{Ayg-g{;7&pdOge_HTHSxOeh zzsVX77wEo^1aB_u9#S81;3DTBy)_lV z{$YtdZN-nB*8a`i?dmH{U&Y5e2Rpxh8p0wm#J0^xL;IU;VPToFqI0_*@0lhyu}1aw ztK+OkaFhu|G2-!LQNK#mJEITB6qDhMp1gr!q8d)1u&Fk+jr6GDUpA0+@>V4bV1~Y7 zEYi`_L>r6LEyRB8us(wv@{-Zm-SlPTcv7pZ;OBBILctk=<1Cm%cHF!*$5P%zNCX1X z4^lwaQ1&;SFvszqvX?D1OpI)K9f5&8p)#eaiMUmsm(rAeNJlVhT%XDTyqjNWXp zU*mN&+%;RDIL{S&be8{~MK~sNr8?b;pp#~B(Rg#LGTKI+GQxIbF2l!6v^tt6>7Ygyt)Z<|9{DiT&1OBMEZ!%K@8yZz`Tn z78uG54iv60uITfYFGG#@8ut1dX+lZ|2h-X@TBX`rVh9ezuU3Uu?W@(Tm%i#mcBwm!HOXH4ITQ`D7ZF-bd<# z8H2}!^&AE^Hd7rbEv^f%*zkx{e3kgYwZ=JmK&QNll?BFVWvp&m&Tv_U${epuylri@ z60~xhC8GD%MbPCTS8aO9r_5esFHVa#@0bz#$*K4RCEQELq{ETk)QOx*fBJZ32IzCJ z#&~UNC$Y{evp9UKxZzqIo99pk3LK}rpBrk-KKiZ%^Z41 zxzCC3_vq+$2RUTawHljijnv>+Hl~J*RvPALAUVdTnyVO;!zvi9EVBhu8p5RXAJ^KU zZ=+cD^GvE(n5j!HZlG@?DwP@hy$)Saqk8Fy7}RI(pfL^l7^{|;C%x|1FlJ#)!FM|` zt6T7J<20KUGnyMM5BeUFzB-K^F)n??n%J9@`gnG5wmfm6wBYgVApZD$636&??of*4N(8cdwDrTo+zUjVei^?qv{*H00hSZ3%lvex&nq9p!Y)}!$4laG_oX1!`zJI(*2wo z&mT?d`5xUrOC(RuOs-5iNH3zG2HDW!IN$x$8NR|_gFYxN_RLJwObuk=iMTWP!l8Ys z*2R)Opv2{5x+ADgGA6pD+ka_!n!};`WN=cS-Nv-^3G2%VbCuGG)FAz%#^4A8cbihg zO0<2yTt&=NGT>iN5b=#~RRUq1_x0O}{RXZyFgARJ*q9XcY`@o450oYau#xG8SrZ#S zk$$qWOO}^m?kn+N$!tkTcmL5~WUb)aD_;?Yn31Tzz5T4+&Po z$M`1iuHXBl7hdmA74dNQ>>dw!IDU9i?RyE(2wyy=3C$iSN;iBhS|o{J_mZ%FfPDxU}r6@@stkI+ewG z*iHkl=3cZNyURX?-&gjz_5#l!5%Bq5t7oW#1Y$?wY7~J; zb-4XgIjDmUVkd$dzGxa4)p(ttB7wJ02d8ofBJP@CQN=x(LoHf=-hM0u#(KXSJ{KPQ z;St0=>M}mMC^A;%jO2`vUfTPwb^fE_O-tEq5^4 z(Y{~{YN33_7}OU#0L`F?Qw4~9Pi`xI$l;7vM@MN~jp^3ZB|6{(g~s~!u8HaJ`4|r9 zK0-p9kk&Jb`Z(~^v5C4j9X^L~uQz?n0`;BY06iQ64*&OD?zk{(G?87Za^i9yrL$rk-8)ykQp(&dM~pc&N(pGgK~pUu=~^3|nXKBL?`LoBTj-TU4Zwcw8i-ZZrq%fx8D+N{Y@u0IKuRk1ONB+iQ{pQX z3VcCQc?XsvqLAP|^y+DDSy6wH_%Qa@FfED~@o3yq3^wj@5cDGg-9f?rNU2JkX8-ul zHbepem28~;UuH^;lXh!m;3 zxu@8N%+}LZxd}OH8#MVj?j61vD{Pi()-j}VkL2h#?B8XVv9cQR_r$~-z(>sR>>sz6 z55sk_;)m+)M7kvfehSRFbE-9&wOMU*maGx6x>U6!?yM2ko%`fN^L`FA9W9D~IdQOv zRw89Vk;;!Y=^Sqe{<2hpM1+n_FnZ5P9}#cwE++{`s5Brr{)CxHXZ+3FZ7@hoslGV6HDoE!i9dGp|6-av% zqyA|KA2BLb7n%V0-t&7R;5+(fkD>F=IJxyj_4oUYaO{YGWxQPUKnpqcc(0~4w5HXT zA5-F_Lj5|&OX2iU)tI2G86%OIf>c$mN^Yr=rf8koqG5X0fvLcIR_HSGv;o5C!CRjw zY{^U!cIBajIE}1um66`Mg-eBVKMl@04fz8jw1jpY`Hy)Yd76_%9Pis@GAWbT5!wlV z2!6-Sqisq{WmT`~aG1=}kAG3|AS~fb=f~pX5@+VUe#28Fo&qiqYJ6uO_5L)zccEli ztlo{0Y7}i>XbL?t0P+F&IUmT6f zKdKd29&vZs6&d*x4Ze#T;h(C+M(< zl_f&Rz)sKB#2R4*Y=8oErFAU~zyka?FM57k6J2vD1pqVHT-V4B!~!$`j$24#Mi_uc z%*YI70YM=U2#5&+foX!p5xV9kdi<6~<^~`L7MR~o&%hD^Vu!F|0hq*pbQqybOjuw+ zU28D|6C-29%|PI)Ji@?23B(4BxlsUY$6^2l-9paE-Cz^{i7d?Y7YO3t(84TG=KlfE z_^+6AnvuPhd60X4CtQB9+vz9?EtZ*gUPzI=2)@p#K0&_2_CiiCT#(55;=CuVU>JR^ z%+2tWPq2Qnuquu8plTe>$(t+qXmYLZiW;W1eN}=T2g}`#w^8e?V;>QW$)21!;yNFK zJvncE);%{g;T)oD5BzRh*U-B^HhwdHBXlzh|ra3Y)Yxxjgiaz_KCAk;l}j|mdfQK2WnMPn_i%Z}>UVNQEUVJ*W7HDC z&`$&1o;Wq*aM?Gx9S$E>?G7APmCATv!=*g1J)}H>jZDtf*^JNCznGkJo|>I=0&ULD zp*acv>tiymGwh+cK<2Bb(>PbS^#y4jfjs`#csrJBR#Y+`m~iGZNyaP5LwIBZy3F;r zL%1a5N^-ta+jM84j?UVA(4JX@8OR=NM)-_CLno@ZHGx zEeDW=1weq`Ed0Oe3%X5jIV`Y(m7=A|%`Qfu+l={p4*dn5TZ9S3bYtbeGyI8c2|$bA z$Okdqi~ywm1}F!}3bp_g`3(_hv4d_vFXe!Y0}DGLMCB2{7D@n%u&6v>)?0W*<@|4^yCc?>R@1hCa{8m69O!5p=)Fy_{T~7kCTAr-x41fdsEGBbl*Uu%L4qs z8vu9?OpobyCUORLR`#}f26iB(Kj6lt4D?NOZ@|-UATHTh03WflYW`n=ri=duO$Q4C z%Li20fo{MrZ|44IA^K-%{{NoOtw;VcAEv(mj|y7Z16F|oRS}4Ppg7-J=O+F^0ERrU zjKA9T_UdoL{xJ>$Rxm-B8*qsTi--V~nHebSg22HJ94x>A#NSQ1XJ!K24Fn!S{SV`- zsHy>?0Rw}8A4mT5%WYhMr2z;3*8GUa0Xb$+Q5=u4`E_u`G+k*w`;PrH#Y~}zCi%tGWG~_6U&>>zP0J!Jox9{0zhgA z0{fK#ck$o8V*&gH2b=|zHwYXKm<`A~eS1BF|6KpmuK>?2AK+USz~?a7@8ibjH~#w-)InK7M6eVL0I8Xz_owpV}-N)Yb+}}@B!%Wd{8(H_OCg!vcrJf_?tW^D;p~i zBLAep|HTIbT=Gvoz;yrA0R@uuU-Dq=fQA2|1I`Tpr>&rHmVem=3TOS7U4Yxtzvl;M z18#8t(Sfkl1wOsm-jr&2leY$d%fWJ1RzL~3t)W<8aZ5uh5cC#080ZLr)InUV%q(nh z2$Y$X4I<0}6NK}N2*TL;Sy%)hB7$rXI2#Yv|C!|`2ZgNk1dR>!% Date: Sat, 26 Apr 2014 05:49:40 -0700 Subject: [PATCH 064/329] Added switch priority flowchart --- Documentation/rc_mode_switch.odg | Bin 19757 -> 20160 bytes Documentation/rc_mode_switch.pdf | Bin 30076 -> 39286 bytes 2 files changed, 0 insertions(+), 0 deletions(-) diff --git a/Documentation/rc_mode_switch.odg b/Documentation/rc_mode_switch.odg index 70dc503c18eb76c1a4ca5feeb44149e8118b980d..5c4b617e609a916e1b88893f3e72c31fbc152f5e 100644 GIT binary patch delta 15127 zcmaKz1ymee)8}UfcXzko?(XhRkOX&k%|L(v!QI^n65QPh65K7gyW5iI*>Cs#cK7W4 zot`^W)%V=)J~Mr*{?k>F2JFfNBC5zkKw<&_umC`fSuCP5sKzYb2uL0aMt^@78?X8I z^^pIsl^V$Nuao0%HvuG%ACp6b|KEe(O;47X;2J9d}c$O z-XcVRsX0>&5RNO}M)WrluDC5r00Ap$rUkaHBQPEjBA^eSO$!C@gSgC6P`{5+o2W-g zVoEj1o`i;laxgGd)<2OY@&>U1JIzN79g-3?(0$W~#hjv5m z6G-|*l=yTzl}T}WZhvit0yHQZ?60LFvekZFs6Y=)GbYJ7WhE!Bv$n*4rN3Cg=4IpO zZ>@K0Tf*e2I>aNsuIysMTVU{4Rp=zBC#*94{Uu5AvX)lt&xU72a$1;L>8DnhhayUg zMK%3DCQRCF84C+=3m*VJis1FoMm80zgu|}l97=bh(%BTMfD}Ila5YoBFtG+x!on=v z0tLW06GHLxNaxkTsz*Hggh#{rn1kQ#qw-G;o?^)$HAI*`)Gdr)88Bqo7N@}pg*Ggt6uURO zEe1q#l~y= zp$bJq+lf>d$JS9rF#-zh^`T+}!MxQCHaseT5RvvnjI+0=gU*-v)lP-c4|k33as83Y zVS;dm!u;&Y3}B<%fq|)0BAsVk%8doM$5K6Z7Zl4!zLiCRW*IgY1|O9oXAM!>L*zxp z@wI!27ECPJQsv#m{Ncmf@1Hh0j~R}HEJX<0V%d6qfjD&^fLs3H*QVqO0i5s)2`W7!+@~U0h%@-y z%CqakW$@U3s8CKtj}?+|-{-f-D8Qm#=9jmN_P3~W{+PDOHljjlVnL({E=?G43ChCaJ}&w3$@`iPgN}{AXn6 zbS4tdI;Q7Q7Tx0bD-@4_HLkRfn`Z5kEx$I7@ zI0PJCB!~g1F5B5Ln@DaaP$l?fAI_RSN#MHm3x0XILCE`<;rHl1+EEi7fPD{kqF8K9 zp)=j6-;f}SpdAIKGJm59Q0@uI%M3lesB%=a9%*BIzOwJ;dwISlMHqol;H;CaV$t{8 zgv%9k`fR|R^?q=FkRZJ`uVjG$r=<1S2oc=FLSS8W;K5#p`+I-Yf)dE{_q)yj%|~!p z_ij}|w7l-!Ol6L_pnENeT7^UNjk5qxG|BQ;TwM5Rd zmX|Y5UgV$YySWb@B-Iu-!ROIhS8@?KgOpC*H!_zU=oI@iD4kF9=@YW4M|drc;76~*lxGUT*_TJxtk!yMka9!nv$;6*S2BQH{?NK|Q+EUk;!SzbL z87k^pQNrMM1eQWc$h3^KZzLVw`21YWPgq@?mW$UgB#arXeXx92H+#|qXP>jYsrpB( zQ_=NJtW?4xeSN*vl(Kk0!-cGyn%YQh!l;KW6`X)FuRJ zf>aC=p^#{W&5v<#iMU?^;mLpGQ`D(7n(&p`wwh~OpMzdYD4I&nOYt8Ev;pO^J>sVev0B$t*J`IZI#l7jukos;_Hyn9mjqW`t07^+P2O!=e)v?BN6-*g3xY? zh$b^r6t6wIO(Jv+p+?7%Uq&;mLEkAVQN_dGKdprdYfA(n4Q6gae<#zBioQqEUd?r) zs|yqvRlSez3Xz1XU8Q&hCQGFKQMp<{qe3SDIb5g#j9?>6j5!{!z}yq*L}IK>(RYGf zIxXF<99KkA4(>yzi&~quiU~Mk*4}$<)Umb`X$9eJRB)feESor)C;I%kEHCZBTXW?aP_F-lG@^U~~@nBeln(&!sAb$l&9 zzn)x*au#%DTn%OHX=PWYMQMMDg7A-IbB6ER50F_WvmN(h^b*YK^oq1{vI&J(+;0b3 zrmsMTgtYLvME%-*vj#>4M`N3k~esAI6HcF zMy1t|jU174M-{hUu2T(NPh*^U23X*1wEc*ep1mBmK{aj!+ zOmZO=ICRtZ8r|K^RM;#Fs~>T&?o)a5zE4HY)ih;g21Yueb-T{2m9Wdt)R;UV>hsD7 zg%YOONFvw&0F!eCf>`Io)uED3!$+! zVN~DluO{9%4`C0@DXGW(LDaWeLG%)IVve~iDd(qp2oWG5H2Yx3ClgBd>gM)^L_SOy z$<%uAH5ui3hluDNi!mnDjrVq*I5 zL3;A`;rfHlao3qbVBy}Q*_a$=)UOKwcyJfyURFR-$oP>IF6~U2 zTgDK!ec9h~Y;c0+@_t5^wqh07&4ZZCHRWsrR(gvI4XXpcyosD$B;YhyPBl0gHU#~* zaFv`Q_MqIVnI?`R+el%(KfN024WnYDDdyILV~{S0Xu$uGX+>IqU|sxuUlcFs#ygoi zROyDd*Gw}&%+Rl_C2yb6jm|iJPVPIb2{?Qif@AHwmk7u>w#GOc=r1?01}Ox|_hbc5A| zTLy#j(C2hhcQJCtw9DYJP$7}Mp?vRVf6o&Do4(*!QqK)?HEazS9;mQLHkYO~(lk8D z^-tF`$d*w;g4aEor~zA0PvTtft46jli_LETu>$2~4h>k1J_b6Pss@e znmCR}Z$&4BKfSa+h{1`_hQX*Z16-#f=G<@})+b$>tGT;3f-t*FZlNpQIBxW7N7lT> zuW(2U+t>az3O*-4?2yH~R(y@imZ-hSe3Y$p)6#! zZ|mHaoi#D06W9a^;|}IGDum3{FwF~F5|E`7iwlY*ubpQtI9ue6sofhW0l%0PcEz~N z@6t;er8F_F`z<^YLi0lzuKsy4^w@I@mEMrjW6fH}8u6&$yH@F$b1KouV!5H2A&yyv zx5XuC4#i4?tq;~UX4UMhPbCkz-FmRQNiDFp3ipNU`d4HZ(oI%+FkzrDwut*%a9@i(#@d{$EJt2YjfHDWxf2Xp?Ahh>R*<|p8M<=KWnsJ%x z*6$GJ0a4mM#xiTTRBM+f}OG zLOYp#ujPkb8`~ZqphUL4A?yo4osQaDh*gdM5n*ywA!Q zENDY>bovZe$ZjQHP^Rt;@K--mJ$lokah>-~aN)S7CDuiJ<#@apm_jYN`aY0H`u2?6 z_V#BiS}qN2JxBK*VverE@EP;U2(-GVzNTiSsNEGW*3Or3#^)v^iB9;Q)HFDqIv2HW z7wBH13vwuad1b=s(41R(jsDDNp7WkKwD0`p?l#x1<5b{lyxu*DL&7?eqED^6UT?ro zf%#?Zoz-Iag0M9x6D;Q^<{2Ze?=lKlg@9u91V_EKJ`69Ir6YGt%EeaV$ z5D!H!+Kr|in%j>`m+wGRAmV_7rQH7n(WrF50xQvPb-3*M3|fM!jZjCW1A} zR94d=%v6EwCDVFs#kgl4y+0fss9idbV&5yYrRSD=idje1YITJ@;|pwuNR#9^A|#JA zAu!e+wWc^Xv()c~a z6?2B|PY@N_a^u)$7D+Q?g6F}2fhn9Cv)4oVEnve;uCPB_S9G#VMTGaa417&TdYtV1 zMQ5Hz58!85Lk7_+H_U4&XF^{P2DA!5$>^5`fbO@I3Lwh=8v&*Y;O~l;k_Yy-G1fa3 zw{2rugB2kn$@e15+KL2x{R3Y$j}d%`VcSV~pS1xjNjy%nk>2q+TnrO1M;h#Um)0C7 z1R9nj-dBvEnXibU{Wl0cy$o=>I!l zK`+^A{R3I$*%A(>Y5*O9R#gPoCAU=f4ueQfc)U=$MC@vTw9U{lYHJ~_v|@Ql;T9NU zuF`XJsxUTzq=JsuAMVM;%XO7l5QhrqbWw|FsOc8vmZzenO8p99zcvNeDhH^AEhTze z!_i@}ltXPE9O8WAB`&@jNwu!6hO1BWzW!uSFRdLU!-EVK)9)4qItuu;HrI?w=5!`# z7tnW-e)w&gg`>+0bwV@2uT27Z0PPnLqq3C%_)S0z2N?wmK*E88=YbUae`%Bdmst7# zH&BU)jlDrwwD7gZsU711-kYqXl0>DL;n#n4VOUt$cU?F>3!enM?7YPj=eJ&EXz5N} z=B5cm{8^8f1qne-~SER8> zWTpp6`T--%B_<5=emZ2YkX~TPX#0L`2SDLOy0A8r&Aax;`P}XDE|>M*@y0>;;i{Rv zp4t4>wZ5+1ljjE9p7M+>W5e7cBhL);<9F+M#ZVZNfQ^+j&c$^PiY>E)TlZXt%xU7M zus=8+d%Ln`4Q{r=gRNXeZp{sIFQ*<#bhQ;0xi@t-Cc4QqiFQJF9(K=!&ArXabsdQ1 z3$(Hq$9!uf$$=p}$V^iC#l_UuloZ1H5kIZc@(b!vJ#}Zm2wJ)6Gii#_4K~y1k`xi= zG*zv2CN|m+mIhjs4)tw~y_uRO{aS_Efxn|hA!d*Zjw zEK!Iw)twazrVqWFHC?k*BEW#)9X*%17$B@GP8_%DulXb@x& z-6%{cb5_D;jCjb&(AvC4#Sk3$SUBaH$J3v(kTLoqE5FFaazD@(fYr&EU}^y+Sh8pXxXj*AZV)UMlMGre&@CO{^4=f4Iqb~_`)@`l*%^3}l4xNguDpiwMIA>Nrw`#N&)ldbGmzrF!pAWyF&mdH6$O(G zwot|;1mSy8^5x@Kg8Z!_7U8f z@xpG`KrLzlUu%ZG#I=A zSJ`a$BDP8pStX;Q{>%#j`dGxMmKeuT?h_Uo)|eGsVG+H_H) z8{zC{cI&UIXJr7bI0Qb;sYIh-GV_~2QK%-}Xi5ynSt$ZqwdEKQ)x&1^#!}cjPH_B*JWJKMMk{QHwAi@SaTuQPPG-z2E=cSg+ zq!*8V!cZM8=Vh_bKk)R>lPFkV^3JoFqK`s8D~)-X4R?ymlxDs+ysXJaU*1Ew0Fsvz z7ek+(h0DBa6S9$(T}LAy_k9J~Yyn=?zFF7|PdVE^lkk}w(R7`%+t0GZNIyg~=9T+C zda?7n4nuOB92t9-cgAuN>c*OQ`Chq^DaN^+?3H_ZaB63?)M{J>BX6id1u!a`9NINP zDeYBk6+sSmdFp)L4hpNTSLWl6s|hspk3;we|z?cmhc zwe;>A%k@cS(b>FNCO-x>c*bm*`eya;AtJLl@;hGy_QV1=AMuu^cI~I)ep5!X>?ika zWz4%1Wa$FYhT#5#hK0)*V-qIyM-PlIt~S@quGcyX|fvGV1=jh3W%dTv9R{( z{tGG9;05pE@T&-ZMY_>7{(kv(Ro`OlcmS$`U@q>7iyMqwX|z!h6iAnHUB!*y3D zfoMy$FyO5+&LGl`Vp(q6TIsd`g>$^M#OXB$xUFlc0AJ^*wgtH&g4-DyS#VQXNzEmb zM!tQM_z@dr0lxCZvi0M)@s|;|6CD|Z1GmeUz-tp-jsj^51LW6-w>Rzm&6zsd8OcK{Ag6l9SiN+82)8-Z-Rm*9@LoiHnaIKEMm=szX~J5e zc6GX$_a!2{-H%yv%GY_tq~VC^`w0v0HY@#;~_S z;46)c4n}SEqBykHvk0+pT^K$7eN?io)IeT!VGqJ*G6O_I|KcOkigk&F(iwk{tZ*)2{N$gCoe#HDKN!TaBf(!Qs7s9flXI`^CtJs4*S zj5utshcuI|eCSl%yxaDE^l#jD$PhkpSai@mXt-aN!8a^RctiU#969j1uIb#wU=x02 zj1o@5q+KZ~4NF9NgXGxU-5K7461&y@#6k^jL&!km>bpB@1czz7qLWvg2S#vu71-1? zv{&ofgB3i+pZ7wVDQz8!6PEkG{1j)eUtw6J;&TYxb?iW!ZJ z*@t3T9&wIE&uc&^Yu>jL#PM=;a}j(szN-G+028XLmb_9%9y$g3KH_JDRhe zeO>s0bbx=@2V!xEmg^!^6EUzQkj`xw^Hk{>=1hW7{k8vL9>ZVzB<5T<(xNWHEQa=Y zwfN$GdT)Gsa)N3J@%%5oZ07Rj=&9EV;61&Zz@btYSzbgnD zAn32(t%1k8Tb!`Y$DIWNNMjibTCmm_4}7GIg?%4XV}jfG*BqcvRS$~t>)qWc!EE_j zMq@vU(WkiRB0dZ5=tB|Jf97d_dcJ_hRx;YeOhHUJ-cBut%O1nNSE$wFYQiU|(c7N- zV^stCja)c=QsqDWFVxY~^~5mPHfSx2J6-5;Tpvj7Z+Ho8t+Dj=E3#`kzgJS7y82K(k#Jz%>gz^oOtwQp+sAtQb!wArOzO8sq#hTl8 z7eIUuem1Fc`@#->rDQ-<8$LzdZqaE*-Pn)=;2u!&T5btbwMJI%*7dfLe!$a zyuj+uK&Iv(n)-ZXg>wUL+lenTRv6ow$8{X_%ouj>-JFLpKjs?L99>OtA%>)@o5hpA z(6(oi&A=6KhOBc&~srrXg*dlh6~|<+%FtWy<*YgbyRR}ruOkF($0ZF zf-)Fg@X41&zbh?DrPC zB$6$OECksy`CI+N!RvNsf>YW6oL+}B0NoH!418w-R4k3h#^5}rT3Enl32=+fTz>xavr=Xe(Il#i3^%tXKvBgfwYFG-vzytn) zM;e}{UvHmjx&oNo=y_(YDdqJe;scu!15J=#7P5qiSO<&%)IZEZWLX2N4BAY-M#CJ~ zfj@7WAXb)3ucuB9!o)!R!wGPRSb$iA&nuS@Wr|?p+rGL$DDem!kZc3+5B`~fjL>ln zFhL4TxjTv=F>i5Vj5#^HJZe-lHnhGV|FXY~2bX<9`8l>6H=U4uS!4?RS#x)0#}zL3 z#(n#uio}qD%~k^15ZVw7ZFmZsq6d8lO#CleGntM`UTAFqqyMtb%Hk2b{8f{(YW-J2 zVdD^EJ|0Im6|C5Lnvh5ru0QrW4KN};q1@qy42em|ip|d;%!a7GnFEKS@F3pYX;Ykf zGZv^g1Ao-NjHjY1m?pq>i^P@H1d)b;kHb~25|X9Fc)peMP2ro(qH3kDB|bEiG6%Iu zWFl~<%Ge)GirgYN;=H5n&~&8O`w%pn;QU+02B8{tIwVCXhdt*iEdNT$;$lzA^SbuF zm4^r^SxMf$6^;&Uf<0OZ?EH(exBDuvYovsu-u|HoL+y*l%dhz(J%A1vp!3dxIzXo$ z+bBK$ljBb0*Cl*4j8M9>HC236Eh~Q=D@Qj$Vlck4y%1>VU$VG{uqB^z%7-)V^dvu? z?vPYLEmp-(0Dk_9AYeh<6tm1$PU@hKp^}F;wc!9WZk(zBCY+``R8CeRmY0FmD_?Ee ziOGQoJgS8kH-#@4u&X2gsOw}%`wykWKWC1B4U2G=Yo9f~n>Vi9%lZagjNNSSr7y#g zGW^3a9I0)(;cbWdLc`gY7-((o`K@)J!M`X24C=-6=#%hl?fgRNQajfC@S}zOP6ZZE z^e;1j>3>m%PDeol&VwJ$YgE7cAK7|-RQ#?NoP2bUPgAysvb%qtAo}YilaK(QuaMOa7Fgl#Sgn-*`lC6#Hn2)E|}2 z`d)_BL>0ukV+zE23Z(ke(*BKgX-ak<{6fm8p7xzwuboSzZqmg7E9VamS*X}4A^jks zs}ieWBOC@995*lm!;laNJA>y0KW~j^3iI?N+Vs^6FjoNw!x3F4?qAUIEqhxB6|Z#J zk&XV0S;cwMi58;)hv%WeeC+hkOsvMudydoI-Ak=S#;(>zAp)P|vk3;7ejVl)ysa3^sI(!)gy=a4o?{)SP*Xd^kXNT^q zoj4{s&M(0r4J-W4(E;*RM1CmT`4q2rc(f*0u{vw zdM5auRXex#gG@;>K`rc~r%A;;-u>g>+zmBbEPP2wM^E;1iCXLP?aleE$xAyP(lbGN zb9=kJOU-&@RBOJAeG?ySJD#h#%x`X3N5LRAhRG75~X>ep!|MmpT|W zIGve+MX4?*?b|1Ay=9Y-_IS#f#;tT_FWvjSpW0KDYn&GdSJdg|)Xrz-y$f0CaPgN% zZtIRD3+kN?#nUX~Ow>VsLP6DFl|E*c>HMoP4;QN7fpB>|%Aenu*i(UwHBm%9GZUzc zCp*!3cML`=x+gPP$VP>ey{Ei_AX6~Dl#%V;D`~F0eFKc;5E@1!N|H5_D32T0nVCV4 zqzzbLL0-nU_4#DC1dri`Hy@VWnpjDcFdFJ z8siL9v~RA)W3aVN)zlad(qG8Sm>euPpEYk49 z2>AWh$J37NK;@qH08?KDR}7xRsv1Q(`a4zaINX1nzmdVana|527JqI}g$;whF57W+`2o1~y8+oxo_*qj4L4z_9^3BQZC*vVnEYdN+^3S?yGZhC;GQD|;A}wKvFo%Ks*E2Bcdup7>^LM}OA$)1C?{ z9_(-P!^POkxH$La6c+3-b`v^QwyoToLgbJ+DjL;(7A1_c_a&pimKoBVI^_PEcT#xC6MI)R{zE2Aog2GS2Vq4g`|!tb&AODjHl@6egx0>Ym^hSTH9mttJ-FFf zYGler*6u{F@wl-dk@Iw{!U;U>78Dba#OUX;D)~Kl(`Q#iz0+JEt|nYp=BvBA`cy2R zq!KNh%uCRU+DI%Ng$MjRW6(8vSjnnnSG(!FV|Jbr&LZE_X{WZvd~8B;$>h{hqen^L z2D;#f&zWo~tnEvM9HP@q*FF!Y6kse06xIja?N6gYQ%Bhbx0A*sBN|ecUD-${M5aU6 zv51S$R&!;_)SoPbn&_j^xpzN5DqWr4q}6d>op$cSyy%_zu(wt}4C()7U24l8yFXBl z5_T5Seyqx0<)Og~GR*xJqG?+rbBu)dNkqb!k_wU8aFq!cnWqJlMgLq!Vi@d)Jt04{ z8!KRT$7rBS?l#cDTZAkfIYmIg=;InPq(cYn{YgNJRB(;Sfa`}L+ZVP!g>O?so9qvc zJC%}fCa1a@Tq1Oo$&AR-?o#gMw6ZcP^gsGWKZO-n(RE;nIUEp0p~iQU<$uKCAtm8R z@i+$gTqmJxd$DgU39p~XPz_!vZ;qA9RX!-I`zZ(4%DUM$ipF@XYIiCvksjewEofaH zW6aQwX8tG}C1+v*?L~RHt5jY}s?GlU$aCdDw6%MeOw7L2;mB%^@ldV8l#Ask&M-O+ zDlQR!!%qm0;7S&bZ$uI8*t8|{)5g(0-X&)IGKiPm@+`1IOEc;|2od}$f6$GCDEZ>a`17Ez?2|U46V<7Ig~^!i6OeM!ywD7_B|bf)T6LR_ z>@JeVfX#eNC87KL^|-;;!$bKkxD3Z=3rCtBi;=X@*}r?a*pq~SnK@vRYMXPTFat%Sm`STZ(L!6xE_uG&p%a{3;}L1{P04l-Kleu4c1#t042iMS#Rm572W_%DF7zDvL4=a1&!wL^v0X&er zF4WGcLZ20LsSmz>8|2KlFg8a2ILVS7-QYO0J4MK8LE32tKLMeTo<@rcKd;U8SRgK6 zlujlrEc*5(dr=a&#?WDd@1}8SXBSeTL(T3}UHDIW*6fd_*Y|3)Bj z+aE$urwTM$TGJabvW4YMjDWq6fx1#Am{z|Au#H0chUE-qbTmHCx1G_yNqzkJfgu(# zHwsap#Aryz1C`RLVW!zC*gI342(Q5Zjq zBu&0)QbB{;R8E{6c5-~5Oomh32pfy@Wj8evV|6H;q~LrwIN*H==gSyko5QsxlBZ3bg>Og z0Zbhh*;*Gh?VR2y_HdAV?dVQl!O8omd zLm$8a>;}ui=C9MF1Gt}Kki15r`TfD{mdacwI*5jC8t2bs!ps6u6uPIOKVQrAn|FDc zE6@xjw*w|PbtZtlEemVo!7&isYxD?rAZp#ctkil(i{C=l=RA-^A=p>q<*I@ppI>8C zDA)XCgBuE1YENCoJlivAwUTmg$he6I<9gcDIq@ zjFT3zh00dWJAT~Zw6kRS9A#)H^?0PK0$2xA z1kS2~47~w1&cnwSQ_2TuTZ%6bUYWS4gPM_FHHTt<#`^pjGDaQ*LJLiI~9SM9LNop0CR=I%#A zado5#)OL~C~r&szMd@1rve9PwuKEy$v|C!d80y5nUty`)8pkH}%O;zN(P#3>lrQ1+%Rc0K;hWZv$! zvW~+De71UKxB4a~-{k?&z)5J3G5QIlQdRmC$Yd(}>1=^#_x~VV!eIavSs{i}9_qX8iKe zz7$tSKTgi-RADgTZ!_YZyeU0MIUd5dt4Sr-&WSaHV>dU2Z#b88sKZNLl*g;NMGCM! z$(){SPU>9Oka+Nuim3_%V(Oel*~-tWCAsbAvN!Z`(N5tX7O7tOZTFXEI!7m%9p~2H zZbGMVx0)^XI^sUW9!R@9X1+xUJcEN_z<)MwSDD?`h#$#L=9%9>e-qn7_06+YWB@== z3-I3(+gcXBG+5ZbMUhwM;Op8cu_y?5PCgD!K6Wl9Zq|5fDN1No8CFG3*7!szW*~cf zzf>9oD+_D9ku=+Tp=E8Nv<~oZ!R49EKNXpb@`UlCvNV4!HnQq}yDnMzza5gC%-`;> z-$m{pf2N$&UrVMOVLVWt?q8?Ge_5jC>Hg0`%|Uq{(!YoKpBXyB-=zG1H5S}>5(PZC zzdQYNw(+70Sl}E60Kmn})z!-0!iCAx&Q?Vp2*Lzl0{(rdC{2QLt5@6fz3_B_74WW& z{%33Ce;?`UWo!1|#{FBQ`tQ}shXerrZtuhYqp8131$JhxM*qx4|9?yu<{P<+1q1-r XAOL{(0@VL#gtBpp)X=VSe@*`b8vd+n delta 14755 zcma)@Wmp_dx2^|w0t9yr1PJc#5;VBG1a}M4xcej!+=IKjySrN;xVv-6`|iEZxBr~$ z^nJ}#&#HQ=T4rk2s_N=UgD6dgKvtB2g2n`a;6b23?Korw$UtqdAq06Gfbq5$2Uh+2 ze!%I0q0|H&*fc+qa0h}gjF<}+=)Z;WctF6yX{UqI9!OO^rxZf&j(bDa4G#499Cg~Ol zTo{p2oZ#K?b7zHvW24T0k(sCSlNodv< zXsf#4dm&@ZeI8-LFSr-2QbarG`h0l&W%d$o-UULqYKY|K834goDJX)ZehEX6Cc(n^ zn&4xC;w}FF41*0I3Cp~%wK{SOw*jP{JSIaykjp z#UQEub*awP@8PlEu4Me8&sysi_+RCrA1#ZSW|HbNE>(Z5)m4;AlI-xCR|owqpIoDqXZBb)r-9g zclDMiwF*~Z=>~F@OFt{c?Cyu4`>nt%9UBIish-?gV9>u>fqfu^(x_pUH#4VqSH|ls zL4|Plb=~__VqO%-mFf*knfgFO0egjDi!iul5fn~ zVobE$d))&HvSk-X_gv9Wl^Upj8VTaJuSvllty5sN=7t5{#j`-rA3e--9j^Rhe`Ipg zvzwod6H=r=8Lh_?SjDuzGR)Fl&>u{`Jn~ZW!K^FnPmL3vEaLix-eaqqU%n z(94=fE=VDsbTT=y!IYd4wzEqmX2(33!6se(Om2c@fzi6Ez9 z`hkLZMbk8AvEa0^Tt+=MjjmlFVYa|?jgPN!9 zJynElfNm6UW--l>YV365DGepES1NL>V-_6oP}*0f9_aA7#lUC5Tujg}yuOwisJ>Zir6WYg7czz#Awh zl(&+8tt93*tND>+;6@Dy6rzLttpl9lNoQvTt0dJDQk~&J!gXSZ(7tb<`ISgP(2K;< zI3Tsx0X69+5SmFbr4~rznPU0FB&1NR4Oj=l;5z2WcN-BT>?$Mz^H)TGLHi~`k;eQh z8k5R?6PZ)W{}nY;I=qQ={UL=|3pGi!BeoC3QF2ERr5>)aMw+beOARRc74hK~{ zNZyDjLHm+=lA%L(9FQ#HgrR}_k0PFtK*fMb%#r?^Nb-jMujtwT(_5Eu^YLvzu|oKk zzf!W!ZPBgxz21v0w z&~o3MxDR|~57-=0lxBHbP!KO&kGRFE@WnBR!zQ-WO~YlYN#5!eR9M)5*N!JzCO^9- z`W*b-Zs&O6Hi)l#qAp9g71*`uLUNSoz$VocI}FOabK_YPZl6AXc+PcO|0;$NgpR+~ zk$K9FOj$q5EiRyPL;7}>=4S^l_ovY9c|>Qj5PY8TwiJQ~Bda+c3|4XoADx1VY(3ym zeTijBu>%%`B>yl1&tW3pHU_xy)V^0FP1y}l&yG1P-y5v@!Xxs&z^^jzn-yE8SFe2| zONA1Dl&PQFCR9eFMQRuAxJbiI=Y{2hVv8CI+GpJZxev+W%drqZI^H+r!lT9CSsz^ zvImRPO(4?YOUUx2L-7wdcQ~RAL`lfoAa+5y&NI=>$J*i6oz!yBY0gLK;5E4Z>HkM(hDrA?QQ$pU47dcme|5(1EO>6VAGDB&x#N!?h9y&uAtC3@A}G*h?Zvs|ML#`O7tqG~bScG{wX+ z6%)xCBDTA;9_yD8HVrfZgh}@XJzBi#f+u+up`b81v~Lus3Vn(bULsa5eodH(0}>dJ zLC{N-TO`F5=IDNy6{P(9w<<#*5kE(7&9Z^RfOtwCS2nrf*I-V${h*MQ8RRgL(U!WasUuN?;7WdJpJ+MKT z!0>{%Zb3yIzB0T5Ja#_Pe0Y}1nWS-z&qX4wjso4!1at^SS^UJxx=4ALua64Zy19L4 zFIeYupXcPI$P9{8iG>wPk%MKn2S&=N6H69^B-;!)r0(N!k05`ZRNsy zCGKbqmaF^Srx;nVO|y?J!Qfa-cv2*>=EqBznrsver!$`oZFP${AB7k=C>xBf73E+u zxq|_vuM&r^09f=^#bRI=d)=QPiF+hWoCi8KO`_v5a+7{rM{@5z6*`^voeMN_4U$Tc zT5-|>wyE@w#wqUYiv!n0zH3Mk+^VSCi>>BDS)$-k-ZIah#75as9Z1v_TdxP&S~yj? zD~!^$n!g(gS2m(t19!y_NO5}0ugG>wJMEuf@HQhh0Sw)0bozOXTh|i<1Dy<&b`Qh; z|lzp);2qgXGUL2wVw?!Cr*i$g= zgMS#4F4ekuNgeSJJTFyK*Gkd#`wEZ}dI_nQ3{)*DQR)2ryH;frtVSv?jzQ&6?=8@1 z5lg298+}H7ddjA_KBU;;BtzBX_1OeB4W{LKBRVSmkg^v*KuxW`=@48P{tSv4DTAY- z0m8A%gtGfP(VqqNO3^;0fyW;FqbHn5^VS8W7H~ z;B>WrGtEKK8^XbSt*(wm$|p5vph}Q}VbT3j@sP))+zRh`IhWRW4YC@&6~r><)}~Dx zPr(pR*m8lIo7E48IlGX)qWs>NBs?Iy0klbN`TyLFGlC$*mA1mZVdz@*g_I&6w8loo zz!b`s)~dF5&jcw!aUd;&U}6O-G#5x|e4Sa>=`%6rAYtrD4dC`YGC`4t zilC5@>Xe|FGB^zaOra0%uGJgu`qKJ0B~?@se;gZ6%b~wsCXV3A{wbP#=LH^f2STL< zXnaR1^{P8OPu+{K(ugW5?9bCk@_Y0I&FCN+TEB&Qz|#00VH9irY&p?K(Ywv?ocG|; zL@+p-6&+y}f*}2_`U#{9BZ0S+({Ybjc${P{rC;#8c%=?GQ3S*g8_mJN_|l7N)D#p{ z*yqL7V+fsLBm8{S#duHhPoN8c1|aSE-@;W4+m0p7O6aqrAyNu`@_0QpB;W5Z8SB!{ zhnrUS`sE?@?N_hPn=`}_?LGmoYOAkJ!_N<;Ix+J%D9Se`2qvY+HO8xg$aXWu_)|C#C!?^NSd5ynV9?zR;OtHA%Ep|-B~^%k za-prD+`bMO;Deq{?`3Qh?{3wvPtEyDKO)Q7Q53zL8oN7v=;U;FQStS>mP`lO)gUnA z)da#OzlGM|#y^$QHd^XKW`2;g>*8M6nqUhCh?6UHkZy3?WYZA`NA3$;ms)_;r)ma;-4& z!j}M~iXixYCGCr#BH!bQyhk-5phSzg96Q%Bw8NDE3jC#-t|KS{loN5D7J zZ*WAwUYJ{M;*4?Gmlm`G$_UR zZDHy~3It!FkZha(Xd|=L_s?DQ)}AT_uw6!BX+39dmf(K zTrWG^-(|hp>$Ke&w2c#d=vZCeIKcV(0_`*4@c1Zci`;vjcwzx2NCDF4uRSPy>yL)3 z1AAHC<#m&Dn4?0~gi=N0CTxV(Un#x76A!M3Ou^L(j5*yqu(7lz!#zLf@$q7ft#xv4 zL`QGDSaH546EIeUj&{%+TKi?A;KNn<^y(+CM43|?6_Y$FY5x|c?2W8<@gBxzZXNo? zO|iq|+kjnp@+oa19~}@1s--u2sTy%QWq9GJ@k>JfWRyaEgS#Q?l0vGVGFkEa&W`(! zpW4&rY&GlgQ5lOskfa$YYl_y(TOelRwaS0}$w=%=b(F6Z@SAYeyIkeBM>_sb%EI|< z7}FvA*-Cc=sW8Y5cdNIy>Ec~C9s8-UnTt33H@QxgtRogsB!NF!U4&oJmr)&HHm>y+ zvitoG%l#-{1sZ*7eK2+Ma7I2;;;$d{4FF9s02^A}NulK*CmRsVL^*xbi3YonPTFi_Dji`8PXK)?k`ktiqcg35#xFB_7IG@A=eU0Zu%K9FLi_~J{0O= z19T^_u>pG8JihGOB$h^7O84aM3MFyqVQ)=39%Gcu$QO*Nom?&!eBRJZu!)aIGH1Z) ziR$~cD{%z|avE39$fa8eKSAAgv5N4v*hj%2Fp^((U52InX`=b8-!uNXEO2sYKb0Sw zVJCXPX@;E@{OJ$-NwXmLn;{*hPkDYsHUYo(=juh1nGb4O<)1Ibj9wH~bXt78b&rpK z_Q=AYA_lkj_YwHR29S2%mmEG&%8z1kIB^4bT$MAxNGe)LMM%*5$TLc?ZtTEb5efKH z(KZ9lE#;myJ_U++s!-7LldtMSh8wwAXa!jSlXSe(rv04z zp}w8n$p~B((_EKp^Vx!H_kjI;0s>CmbR%|wwP+5zPP3D;K~-(p`-;AauMQpgW(&$TC!Xmfji9RSP19#`XgdF(3a5oUbAC)3-j5`{knrD zRV5ar;ZUgWVT|!SN)kpLA{#bm-yvM|y*drL{p$Jib!Y3#w@#L?NZOM!2q974#_|$i zV5j6VGr@%z_a(r_EYX|7pgZ)gpKGvZ>2(C&Ea#}rBB4?tLe78S1DWJxk^sY*5A_zc zXP;u6WQz%dL@(yF6WK|_$OobD;lmdHt_y6KucE6ztrt;kT*`Li zuOJDeCq!_!;20}j&ooq{Q2NuS@$cqYfJkdARV*))Af|aEq?EetBi9nWzRM0zd_gqN z7a`s>Q!X;OT0YAXJc5`gig9x-LuoThER2&@8l$iEA4kd5FcZZ~DWM0Ar!cirz#=ZT z#oC6qP1h%{;v;*5(ZnsLJ+k*zpVw+XFiM_6T28gGuc$b57vrBZtuk;(Dp6rV1CeKj zDlKeg>I{;cbuw-6Pl>#)I5*-X!T^y-PVCtG2+{FM^B}tWYWjz9wrR|9PFddUpgz9W z#7k>L3w?iVnFHRrg{p-@j@;o@}( z^Rb!nF~OFNF6U@VnQuLB9mX?(6_vI1?Q!%8Jssub9k=|S-r#Lj1U5h7I`!Y6|fg&TF$0Y2KS10H>kW)lu~H zNCunk*%40Nao6!RM*_3vr$mXjUc%GoO17Cb*|4L=4~E0iynOY+Y2Fzr+hNl~zp-t6 zwvIjm>|TsdkCGV;D`}g-gX20sYPa3xbubn_9Lp-W;wDgf@v>OJ@ZamlcfEQGR~ zaFI|TE=M*nJJ^UBQd7)5b?zGX%%NsP7jT5<+K=y*36q;H+=ZRrQm{WJPuJ-gi&ngx zC)8L<6T~A0 zi#2B{tV%iy)TzLU;vU?5R(30{YE}3kUocen?%>vHmo=TwhHb{M#J$fcCx>~hX6}#X zIb81xRC2U}6_MM(98bk3%T+u3I*}*E+7>d&N6;xapsw4>^9$t-=Pd4tk8eBCYXf|Rcf9FOV-^%opp(j#S9bUcUY@5fknJwoq zO1?gx0*;SZwn`O7sUf`xTz&j-b#RFQYQfo+CUDHy{@Qf7gd)v*nnNQRWDc|p1ST2j0+NQ0i$vA((0 zHL+EkNMYs{Brd=Ww{q&-jTza#tb^8iPSDu<{KaiMg%9xxNCilP`x>58?n08u_NSw2 z_c2$L(ZwXc&j2M?Vo8jz$&sUrcByuyI)Bc~&*O+Ko-A?WNCgHcpND?n93B5Tvb;F?FRdKRuq|BM+dA_f&m8o5py5>qpeVn#0Ou zC!B47C77N@d70CnJ;0s*zM?DtH9zPoYR?Z_kkr|1$Fq(AF4b6XnY@A)iBbj(!Q3@G zsV%d;riJkg-p`1aF7eU|?#QWLcMuvGu;29x-q&g3mAfJrYxkpcy1#(mOiv5Lw8;Z8AlI zKLh{*NY=mVVO7XvMnUZA8HH@nPwMF9GBA*6XgiOr9mmZXg6+)^u%Lgy1wqvqmMnx; z8Uc~2u?%!Dk*ab2vp%?Vk!5SEtKJPMdhKxEVg8F*Qyphhc&Yvd!4aQyALvk+e;@+& zoUHyJBWq;hidw~M0ralSX{fybtC&(1y4LIMbNBzzpu^m>u=R863Fm$~TyyEJOY^AZ<`B;qRP>q5FiqAEWIF#UQ zVlmRFvXioPIiRI22@(tR56HfVOQdSHfeZZfEGndGbocgpbsZfoWT5hYp$3%?0c4xC zC*ynj!UZMnU6$p@te)6GCc4NSu|>QPXnJ6#f0pueJi0W&p-I zAyy5sJT9*6gO%Um?waQ~o<@BcY%QQ+dsNLsgFvlkIqckljT;JJ+QXL6RFdnn5Z+tj(2AAbypx3BAM)a$v2KnPoY zhP_H+Oh;#*H@U2G_LRGj`Z7}&J`-1hX)cO8m_3z4-Yz&(g2pL{>5S(p<>;wcsw3ba zDx~q=^Bqro=qFs~j!Y<6BBnjAd*S}Itybq;)w`A`yX@5Gi{NldrEnWqjP>0S!!P&O?o_zwwm-rxX-W2$P^s#H=RbAOHyv=-S@Fj zu5_V_)-_ePijq_bMxZ?3gQklGe&mtsfs0*&(C1_fEihw%2ypoIM|s9#I-c~IglAbZLSB_~|GEJ4hd~eQdZv}UP5Zh-#vJ=s zmkF;M-z6=$8mofZj|>tmlnIAIO1<8y{!7Nu%Yn8H9^GG&S`hH##j@b-$m3-wz~O?976u+?E|9JZky( zDwLHFl#(7`^XdC}#$q)aXVc~nDdSaYErU|?59K1lC7_0Y4k8o4mv@hFMOgopGLlxRxWL?mB{cv7 z4dspJQlYEO?#3dtQnC_JHk=N&f4H*$tzX17`V`2o&kcfL@}2&Rx!*Jx6K@C4%4Jusa~EG6h4 z?4v0&6DX`qa^XedU~63HzhP)OUG3c^2S2Hyh*KtJ0^=&R;)^FLRCK)!a(Sm@WGaQl z9>~`nD*pmyP(%#iHpjS8e)p2Y&1tKKFm!eqCmnL#1y%hE6lhWOG0=YV7B*io*^5?> zj7Zw^98nWn5%doo+)6^d>s_u%wk{1`QMa~pE|~QP1r*0UsUi)#`$8ks+l?K{{{pp9 zNPN~5<0kw4S=e%2{qAcH-PUBac?od$A8iFZ2GCM7s=SprTtpMSfU6Yc?j(aI1pULr z;$k+LVY8?dBTS7KR!Hm;Mt|Us&w+ag?t#wG0%V!AZxlIY6(vGhKh|wiZs#|BZ1gR6 z+?@Gif{=CVy4|e5)+J%G5Q^OKmlkS+ll=lX6#!P{U)E0THN~G5G@%S&|KTOr&x54$ zlZM3ahgMk&_lSk*luk$;FTpe9O9;X~P|aVUKue-6n$X5sRn7$wvpAWUrh)z=_7u9% zI!ZhUu5q3`%A^Z0@DOiorXmPSY6YROQ4To9p+UFZk6+N)tC7&8y;_>DUDZtj0FiqkK^bO-7(ipu^9LR8 zGh@#4F87B&h$%z;%{w+*f%qJBXdEyIUScJnRHm{#OW>go)U;p5W zZB}1NbiZhGvi8AAa1FV6^~8HPNcB?;Tco*unLl5~x}JWb={=yW`T9vZf4wcOz8NV9# z;p3!SHuv7q1WsMm786eb%HgBs+z&Oo!?l&kRhi?9QFQjrkg6nu{);^B7co@Ia`nVKi-@qG+)pQSpd$9Z*ZrJvn^m>f3JLIbaT#YJK}c4o)=9AsIw3&!gw znbhfA5i;hRwsq`OLf&7CP`~yEYEPwncSg7JCBE9 z$qQUdp&>D6-zi;0#xJ+1Z(Co5PEl!>jK+hFZ0JsVDr~7>z<{q+v6GR9QGV9*0lXbB zdL24iw5`znlgKV@L^!&Yv%ei@*C#dCni1NJI^^z(ZP@K-h2L8jCf{t>&IdmS*-L=N zh?8-w_V~c;$xGsMJl^%VlSs=ap{mtI?wf*TmNmXZpL(=`pk40W?Uf%)f|Vm&N7W3Iv7#hxsv=o>8iwWY^??Dcg*2lz{!X(QFn*3^eweR2ka|SMyWPXqaPYq;#@DDE_I7)1b?+~dgfXIDU$x+`%E+nY zS2Y@pKG_?47nOe)_92kdjiWzC*US<`K*3_`wX8Us0d|J)KFX9+)z+S_R;|aY%{6e= z_dUzaql>U;)m@Fc;C#dmK23s>dro#x7-Fy_Ls(jrla@Td44@Lp*RyRL>&fx{^ElwTC>h~Rql}jY0dUl~#ga{XvY1@vYZeQnxu@N4{Nnq( z@)OwS9c`Ysn$jC-e$6GhvTHpVJ?h(`a1!qSdKu2Kl`qKsGH|c6Cm8Lzm1%2YO$6wJVc$ncteiT?Tv)#8 zKnSAD=jH|dfGi^@lTw$egUrwQLJ7wcEww-630XG1f`{H2wdeN!EjG8P?9GY5`e?6O z)dLt+??~VW3FH+|bjN5d&z z0p550=2xC0IiM_Si?ltG+4`1ixy-?Q{fzwg0jpC2U1q>sEZ z03!M}6EP4UMU|$9VtqM^?rBm`zI$#${eWN~OvES3LBkzK|Xa zv3FnnuGswmI*FBaDjs{HJ>&9);dqkA@Zitn_g@A(P*b@4xu?36zvJt?&HfyDl#mPa zPokaughr$Gk7=b^$O?4rF1W>B)A$DLT{e#6fRiq~AsyfycjUKOyxWpQ6sBNMVyq;_ zdwM9VvJC}DY&Hu<^SCX?gnX7UiQ#^7uq!iRmX%vyNrxoJa*FzR`Mx|0R7XIzXnM`F z9TaRoE}?2y%A4Itb-rk1qruJKYbzSNHz_Ato+!(ZnuSY#|m7Fcfjeh#~~!{@!j(3QRiT01z7N(C3R(uu20s2 z02-U1UegpR2WlZdo&d@V!Y8~jLxRd^9sH$noWUzCiICbrE}{>)_%kx#p0%{KHY7T$ zvyyn(DUaezI8I!DE{l^Aq#eS}abj~OnIS~q$k$hkU$P&RBiPS~Eo~OX))tI%#@Ve* z`M$%sqZqv(4Lskfz%g%NtM9>16-Wo)}4m(zI0oN_13?lx*O8Ik5+c*bR z<7Zn(TquimDHDRVZ43kubMNj!AS&m{2S;cr!tezVX%6C7u9i9ZM|zE|xjhGFJiH2Q zOYQ|%Se9UDV-qyFe%5g25)Yi6!^%6K1i`?^*j?gNYG2dH3}Md?5VAVE6-ms$zI%IG z5s)?FX|5#n5jrM;1djaV>8X3IN6?tith<@br-Sc_`xPM~tk5n2DD$SeHzzY+IFh-I zl;GkgiYw^VY$DxysS>xty>R1R)JMmiP_kwP-SAnp(Q(vCPm7`>d;luLsv;dK*rl>H z4T%iSzf~Rk1V+AQjGxtNb1c()X2SuV2ldlVh(^vg=$nKT8I3I&+;`{JeuDUsI0obf zk=fT0tQQ6YVzUUqKA2X`bU>LE$qMP+QHAL#!`VCQI5pg=u}cDdRIIH$Mzfmg^PsSR zRnMUv;t0(F^`w4huLPI{BgJ4{jbJufv@{wqmPVhUkpK^2hE!o$Q#A009d661F%Gy5kBow;jK@RhThF z(HHRmcfrF<8eNPJQyfcSNNrqZl~0TM71he#79v^`ao=Vi@m~0SNBOub)*N?&Wl8Wt zS^Zrb!I4YAc6uPp(oRbun*m3SJ%vHz{E5`0nDtEU4;`VSPjh#!#?HC-9BYx=S{X4E zCcS5necrpk<5slZoh6FnEk0+gvmIiT^;EvVS9k=US0j!oMK-h zXq;?2H)}@x;?Vnd%uSjXV40QgZCU$n#}mB{c-;q_05wFYaQAZh^v)q(%q*0aZ7VZr@=Qi&giJh~MboWH^l~eo*YJzev zruC_KuZnp(*F$jm1aT@*uYT6u3ZA-)3wB)^pXnLK1Q`faJ#Dq&G))A(RVK)M{F|$t zxB__h!q=LecF3`AX@|1ZH10IHXTV@_Xx+_tcR+=sVF zrPsBbrE7FYW$Sc~!`IIrJ=;TTbFe($Ik5cLDafhzfjA)De(^NFnKr4qFW?+#N(Jlf ziQw?0-`#}RWglqmJGTVYC&Rm;Hv-(gRI`^7J_;}Oy7i0AQ&WM@t#0l0F0a(mmN~Be zeMkI79Jj|F)(BA*8@B*SLnbwyw77at2^6>v{Ja~^Te0$3m6X{Y9A>;dJ}T?>Cr1`i8+U?3OQbc@xo;#+X3FpVZI^p zyn6WW_FOOht!aBc8LiC=hX+5A$G1z#;%oh+7n()olF)7#tj-L6|FYggV8P5zVM&S% z`eiBGo_g2(MbvW>e4tKwx`i!Q<@DsX@b-W*ke;ExQL_GW9XgG>)o8xk7XKk`U()HJ z50E5&Y3~XUbg+LuNaQJh_e^$kI1fPnjcIS}?CDleK_GrL(Eo~QtGjuV;o$?dlf`Jd z+&8OD#iF4Q*?3sEc$l~t*jd2Ol9X^vQmmYEY%E}EDMlzJK1Q&x6eie4DiD%`9egFl z^0vsYnnqd!;_qTV|Ih!d2JDa}1kZe?`D=Okto%1omXZ0JWXeeWP5$P?%KnqjBrEaP z!t`eOE=&7wrr3X5$mM8J{~pF*;0|Fgy8gFwc1w$7%u&J6B0 z)`~I^keDD$(0@np|9Ksd|3~z92bB(dD2EUHt(|hKTixVs4dend2!!~*3Q_WaKu*pc z)}~JXS^2-~CI7vIInW@`-`AV`Kei4L5Xi>V+3-JA|9wbdUQw%95FpSR6bOX%?mt8z RP?#hbEKd!)DfQR-e*h5g2-E-o diff --git a/Documentation/rc_mode_switch.pdf b/Documentation/rc_mode_switch.pdf index 0dfa43cbf3320e8ac2e3d8247cb67dac7b5885c1..3254d6140b3b516870c230c610f90940800249aa 100644 GIT binary patch delta 31923 zcmZ7dWl$YW8?J#uaDoJavv7BJ4+JN;dvJG`f#B}$?hrh<1$TFM*PshHyx-pYoT^hb z-B&+StGoKg-2GI~0-r$M*M0hJjU6X|1f1*2#hA3%tS{EMTS4i@i762Y7eK7mLb<0H z`_ccZq;)_l;1zDa-khYM(XB17_#SO!_sf8JT}g;QAWnW~9*t(>E|UoGeZG|N^p=|3 zpJowyfJ1H_0iI(@fT#Z5uI!tD_m^2fkm7CY`aBFDnWjsifqtoZ9s&DqR*3%%L-tv1DcNbvOw3_^mP;yoRMWs@o|#@*HL7CO1HO#3O#JK3a2W zY`FaXbWHTU9=uu9ssDPXBGcLleD&&35PE&tz4Z06NG23|ebetM1}k+I$8W)OQts7( zp*p~;@?Bof8qeBcdjIx z=4r|F&+0@(K%^{Y#(l)MZG_*lszV^Rn?tEegeHac&stGt(HbLyOJdHy(eOPrVgV|e z!@+skPBMp@79n+-*|L%~caX~wj?;de4u_|Gi)7jh#YKf z>zFo}`a^&sn>46(?4@F0Kje+UQMxR=Q$4NZAg`+exynIzpA>d?O;BjoGDdW4vsQrf z)uc!^rBKg{BT8!_$lZ2wCF49#C}ELsB^g^`p|?fA+Jhtllcxz_G8)xRXR<5P-mnU4 zz0o_pO>QRF-{5a*UkrEL?@(J|u!Gg#$ZT$B&B?ki(|2-gtm$lD+%4KXk6^MJX*I2J zBwSD8igB$e_CAN=_qgqDb8#&iUxYfe-E5E>KCPSDgK@HC@``lh|I;BB#L zi(G9pRAp_U?PvptR{O|!Y3`jgl`z=24C>t`Ck0y}u&o=lSm)ExZ72V$%c!nQY^q%H z(PV1#7!AMR7hbCSd2iC+pe1^166CoW=)Ekfu14J~Hx%owIr_)Ofbk-o)>*q-r#q!_ zO@2dSqEDZ!#m>U%-=gFIgAHS++>nP%h9)0ar>n_pG3N$gKeQ{6RH437ZNNQ(`BAT)xU>NH8n~kRUk@XwCnP4^~bO+(j{jG=qt8CQHZnq23U7X z{S@KIqw-X;PbH)<{uIPqzvA$%gfB)RY+D?l_lmunso;m?%=e1%VqaD~?OLp~p!kU` zIUN^;y;zkNn_%6)MYj2jTDgbZSK*Z5S8r#1nU@6CJ%vv=Bj?tMl;Ox+O8n+J&hN>H z|4E9UD1K5xWmk&9i}h?c^ipxxq~2TKLS7_}`&F3xgwqUIb2PDrF%S5weq zwW5RVpkWFbZB|iN@E4#y%`{~~U$l~S+i+RfX~>5*ol|bxtf0b=iQ7U0m}v2F_!)LW z@NID!@peL?;c=OtKmnnVdl*|dVttZ&P@n)>oM9_J6NM`A8$1Ktx%jwt7!6nsW+1*9 zFSZ>wS%jgRwbZV~>m!4UKEU(;;P2}JNw6UrBQ03uuV<|11<+s|mulv~^eGFma?BM8SWe!eWZ0R<{> zWsQG;7%c6;AC~w?QHt;DA6~93n>BPe2oM{@Abg^tMn;-6|FVVq zA89FZ9DjYMEzvXH zEQ($g2e1?U>&AEb=jWVdsOR!f(_)P7dsbOpm2@13rp2&$mrEmLcKK5oHC$LZ9M~PJ z1pZY1NVJmW2>S{XZTs1l?(}eR)Nz&0JbP$CqX$5Bs!2 zel?zwf(-LU+IlMVlEq<`Cat&=w}px7fq6^*4B(Q+%;0CR{`EKqI-YT{AqufF#?s?0k zC=_n~;T#)^xM>lo!{5K>EpZ4Ey-@_TtHP&6h7Yfr=F7DR_MG6?EjcPtMtEfH8b4J8 zIHJJlR7J9*lkbp{uk8&;5{Ji#7&$~hw*il-ru4RPyWg?NRvV)HyTj?sohdVziKeSy z$x^jX0^)rs*f}R(QBqup#Lxx35NSKL zq`IpnvdMeei1|uBdhQ3)?>b^79qX~g2FD@K^{{9GXP;Zxjhjmi2ac>c6?;=mfTH0i z`o)#C3ddNdH0>o)!s6QlY>G-PUzV9EoAoNoBiDEb6N~+G-Pw9ruTlCw7O20gYvWdu z?g?M?7>J&~Wkb>}wdWyo3OU3ZGSiiUL;W2dDvfF2n4KTSQVE`Y_w3y!WibW?X4sV3 zd1I&ahyv8$&^Jzv>gN3eA6rw`fJzkdvBkmPCTppnY5%~6_0%~sUP)791Ts8H=EH|n z3GzYkbQ%q2{?}6_5z9t}IpB z$PMtHs=(+NXGZs073f%Jq<;@=k=6_NbBgeuf|OrUjrU8shf1}c_AO3~fNo+XF<+3H z?=t=)PTkEY%HKf@BP&`;BFJ@(bVLqPq1f6cOldKw*r>)T6ESv$g?d>Q3{8uJokU)j zPjpq}6#;8xO_W>}0qr@T&0>+=lbt#0X0yYqYuIFhh}`)u%lcvBZty|p(t8xoFRDMou?w%>rQXc zhAngNLt*z;)&~s4pd1$+jg-Wo*ehZ8zGcSL!ox3tq6e7qaxs4MfNl-h{WuIFZz!2Dww9?|y{dz+~QQ!tw$YCOqA14H^V;zP#`5ktxQ zvCBj`%>M%s^KczV+P^W4evqVINYIdeum^3M8{{GZ`}G6MTu79?4&1)>R(_%=KH7bX z;o}CRAUHlPf;-T(h`bsc!$-7y_9*we2FhSG9zg^AXb*2Cie=!wL6sUDN3SFILKB{B z;65=fvH}peZ$HQUpJi`-)5H}@mJiuL4;M#@Cp_vjb(SEHQ(#svFG$>d17d&K`#SLd z1$`&O9=sPB5OxKkHx*2QpWJTyB>o24=1FCk?D-E7NkT9DA(v`6Ps)LZ2CS}RAB)*U zU%u%J{SE32n?Vl#baU#H5(R{LN9=POxDLX}HiO{CE*=AG{|7&d!y_%;6 zimq*F3j(mVxfK`&{qeTBy_v?2a>cn(HU-#tbKwWK@omN7Fh1Nd6Gwg@TS2W&U=X-_ z6(w8%6=@IZbTfDMLrd;!#*acF?sa@bDbjTKV#ouLybmD;e5?w#kw&-EjEA!b31gb| zrsXiCkd*=X2RYjelYS@0n>On;##^)g_k>Z1NuPK&4niQ!q_ePfG+sdRCyqR}jmLL; zx|UG-Yc+Bw5P49rc!vId#b2*uW%;|xss^ff$22Y%DYZboEef7jKK-|S#iQ@KQeP>u z$>(W?%ZMd`Pbld6GsmoT#&V=`_I*0B>dP6Ko% zlmuhKUB=XD*n?1O#NzAa7}+`}{_-rkfZW-lX#TM#^O3K>t56{{0ANg@D z+%JDE>`~%Q%yW3Ra!HO#ZkDO{lC|d_pR?yb^W#Q%SpG)Xg32Rz@1r#&@aJY)WUm(J z06iM#!%5it@q?C^{A`7Ct^55Ox*m#*<(2nGBN2|6rj3)G?T(_}O?Klc&GUQ&h$9R{ zH9jq0^K;k899I^#wOUH{8jJa!K96V?EehIM>}SqcFnPeLd3ABvYVLc_SOJruvuYFJ z;lAH12~2do$*L3N9rqTah!^I2C!3Z&K(z8S=R;+n6M19t9?uV z4N$z!-AK7`d`Y=}(A5?OT~Ij&bbf6SdH86XVQaZBAMISsb_y&8*iN~Ua+!Z5YGnQz zotNK(WlNVDJ#2$sfo*R8&*ZT482-RqQ~Ms3BUEo&W6lt=`N!z)Yl}m8hHvx7PqN4} zxOoZ1;dlSogSC&2Tj2aJDJpH9f#ET1Ewy%i2J0hO-=Mv+^QL4%<*3ZK-!S>0QTLJK zF$FMK>7DuErqw~EBK+ph&Knk1XJs^@Bj!_lQ~v6xOHb)SMUp}ZmF2|aMC4LO73=70 z&Z{Kp9LWPcjDa)?D`U(o8ILPB?~-KpQ$@1>7$L}MoWMq0N||zZjC?bW5{e*(bz(Y# zt>-jVzKQ-2yWq1PDchbJs(Jfg?oE5h;Zs0WTvy8ciQOcY*_%f**@wN9%AVW$Pbins zSc}4*v1x85=2fY&9tO{4iVD`@S9oz*-$Jv!o$K@x4oBe2qqk3@(~d@%d#Bd;Y@T=0 zEx+=ylC9&yw|eg*sh!-$BubdkdRM_|(f~C5ppYKthsv-XVn6>jGl6-0NZB^8g+0Jv z&xm}vH%d`HK0>4d>YqkSs;x_|FAFLe4K^n%DeqKhGyd!chFXIZf&7r^#}51eGy?m? zt*9e3WOMqR-csy^KIh+f3U)OSWol3-s`t$CfBy}XKCJX>h((zT({Bz$dS#kzPAx!` zPj2?-PD4=YV%<_0dgwE$x=phaLFnHO?A zd?IXwtBa&TU_MqDVW9?xmvL+uJnbj$*9zZoDn}-#E6a5s%$`5rb0uZZg@a`7XKdHS z!jKCotvy-j&`n5+T>v@bZiKB|1$!X4-iK5H8I^O-3{we<2xEfC7iLmW zh&UH-DNV0~C9OCd$9L`@a`Q*I#qhvrnFS8P9LSJ|BVbx&94%S7-h_uk;zJcQmg%aK z;YZ8*NlEELE;m&IaM6(cOn6l7>{@X~9m|)gKD6mjHj&yuQH`S zVu(kP^pj;6+dPf7*~(UdhgSsrWI)svsPoipuLdo@LrXeM9np4eJ7CNYt z8bP*)TtR^15coxIM zcN@u!xq~i~?bqN!7CqXU*2Qwd+s}4iAmcA=-&mSjKUq@d7^cm09D9!V*G-LSZ@e9e z9TrZWEJOKumcEcHsC(No=jRhpD_fOFxf1Dh8i9mr{o|@wXSp7y?~TO($3F;5Sajc( z2_7%0Y~R)-JLEc3ZP9Cz`!tyE9kzN+9H}*!S{$ux<@PP9tdqs6d=d*)YJ5~6K;TX_ zn$fgV%wnE&dDEb?)M9>Rx}rl1IU!22L;E9OYdKGyAMhkyp?Zo&U{J^V; zld*;&;BJ$=fcgb8+%1F71-p6D|Hx}U|LNAcGt>TmriK{#J3dmzyWFc5A#6}1&}Ma* z(q5&;`Huh6=p8uzR_x1w&8F|xVi|X4muhp*HTHi=|3MAwd#?X~21?#@L1(-olf=Iw zA696oGsFY$$cLu}AA=9lnf;Hongj|T(@v-#R^y2|UoqkrKR0_vjt7^$F<>j(2+IDm zd&0>;ReseLG{wm%_;AjS4XyCh*)^6A9F)6__TL#>S9>;lBjIF32p9-?*55oh(LY*q zj3MD1n%~YNR)OO+mj|86gkGAwGb(`2s;DsHp`poa27TXq>vedK*QSV}{vr7%5J9_^ zDjgswL0qHOH=_TEjLu*o?YYrgmg7`bK${UZzLw);`G&>ZI?A)jTb8XU_`~_Be%`Nr zr!DLsi4I8W!qlTO%C4AXqh0PYU_Ol`{xXclF5$)K$Ary5b%)>DQNl}U6;SM9R5jrp zM_wb&S3g6~wawngvuYS$898F#vPN!JxY5M-(7)6onsSz|W8_=;06xmI&U9MJ^c1lu zh{WFyCJ|r{A%EFs2gs#tN0pg0$vTsEu)s)erAtYureZoPNZIE(LSdeF!=Qlj$8GS~(r*)DrL zLyb9dn{@;t-77g^FCDTcb)#S%g(odEo|M}LJ@~(K!iNs!geU3e0|f%INsE~DvPsH( zM55wvyx`4vefzqiG9T!@n)S#EW#Knk!u=>3w5NV2A0w@pAtu6`sOv>pOeV1SUMP6y zyo;srJ3Fx=%!e!@hwiVWVodvK?BRx)gI7e%Rww}^ih05{i*+) z$-Ha{A>O^BX08SY>;zr0n7csmiS!R@Fzu+%NC;zS`H9SGeuRn8l?V!ejU&BTG~7=yha?hJdqYHf zvZ}pV+#^ebq7M|)c*J2nm1T2yOtrQ~mTC`;h-0ga$_s`45>?fLqYV*P3Hwp7_{Al3 zMfH5thk*`8j$%+I8px^Z*9ad%i1;D8m>D!7(&0<#{FNw<$Qs%s#xAiW1~xQi6#?v@ z?vs_8c>zhZXil-Iw5g)l%MrCMg|ka8c!^}PG5jo@sD>uu?n4#m95xaA1L;Nkxbsr{XeQ?5T5zZpbi8l9=O~6`~M;2 z*6nctt974yiVOtzf8klndxRDZRi*u3Rq8LU27DB&j=3fk0eFTy3T^ZL?oZy40=JL? zlWr^r+GB!Ge#E0r-T&30%vjhTW?(Hp+<#t?QD+D;S-2^1HjW`LMDj6&SSpI22`|JZ zTh;>)*}l_t6EKE5yi9UkGh3?P5M|q~5R(YZ!Nwzs)QzVb#iKVtadoJ>{x;GI(uRu5NN zj}Co#Odp(AUl9IHYe=b85sw2+G31_9A9!nl98AfW(Ku1~<9g~v^ksjhqMcRp{huYn zaCl)5h)^~&IfVCjL-FRl>!L9T_?5kzpw|d~{(+MG@={h%Bfjn2cfLD!%%tx}h2)!s zAR+RW=v}~*Zr5<_OSU_vMX{h@1!_~>V<`PIa^t(Vs5**Q)@V7#NTpYoJeFMqrhSNA zq_SfxNMBGOHN)&0`KW9cgnjDkDR&SFZZ zpK{4Eld5mW3r9tg$IL?3W;L7HODo_f2ReR%&QSgP>C`P?`C8n+Mc4F3GAl!CmS?oa z<8NCe??Xc}d#vHS`+=sFMsEgAv7cV9#FF=Ta&3RcV!v5L5p|Fw=ouu9W)5k|SjMSU2~cY3 zp$ohu-Bgg*>Nj#BSe?`t_sB%*Q6NL)G~yaCTd^jGC9_QU7JcktGLifaZ$P$?d3D`^ z+NYgbx~^oQV~KkAf3B(hNr&H@u?h<3ze};++suzeq>Qsk3(Wv-k@=52>d}3p%;`6_ z&4g%^uXXtU9Ht+{QfxZks>@t1IArMS1of&VP?2ic(-m!eIlVqTKKctLY`8c8Bt`s> zpEutbcqPT3yLuLZI`2m`-?UyY*hCJfR`A+ii;}R#nUpuonbDLn<(+0G{i|m%7no-c zn?`B7KX~nNB?u@C%Z?qzG6f^>+UtOm0RwI_%_AJK>#>E!8C#mCcvr^S z6Z$6|>Fl3mHUn-8aQ`|{UE==LhCm*Ar3`5h{gkKZOV&cjs=Q+9DE%^wwCl4tIGG~^ zyk5Rr-}~Lk1al{PJ(SQjh6a^zpgRB96MzW@_X)&QwasvzC)}Cz)&2clEo)Uk>Q^PZLPpE&`O$N!)>mzz(T*P!#`U)J7w5W6z;fSBjleJY zu`IF38}zq}5QPo|GW(pUv7J90^&xLv9_U!ZZB_N*^|8{NA>iSJSQ~Jils7_RMi2jC zEWi#`HbK?XLyGAml$ZJTc7=O%-og~V=5X<+<|gY1yzZOf(Xas}GJKUL)zWVIXQOfb zW){*4olDQ3NX~qTn6+pKvqWTX1t(`&^hvBnt#%kcCNpv8uy?S=&4vpc$6@?g2?Z8x zjNETU&D$ERxQC&^ksQ&kxJ_F`x+)TF3M9!)k<)1K3CxMElZOg})@tPC+D~sN5j=YM zh2jk`rAU3vzV{XtHSV!5=miRO3Oa1YOKEn+2*a`HtCEV6ZA^_r%FvF}Aks*T;~iAw zRBjzS^=+LT(^%yqJZ$cFt9P0~QD{hhPH19QSx zBFymv648faxB@Zh&^zHv$GE2;mGzbg%DB2{GDdnTh`!@$)mun%J5-n-jCJCa7YQ0356= ztb&4wPR@=dhBk=qnQI*$2AK29+};yuJ{MyhY;K$JYkQPjqsY*85dP-koY=_NvsZbaAmY%AF}+uczz{0%W3g6k$x$> z`qF*%m1}EAvG1+O3A-xGN%eN(7VSnbT;X)fquy+c*KzkJ(YMce0(i`* zwAI79WF!sLh%ZIAST#CMs#p7qhFxJBM$_?WY@MC=uek6G$%eqj4z5+Q8enPUzNC-% zQYK?YpRM~C@=)Xd$^efWRwScVxfWi21F!o;FVN}ybXJri|6-1DCJG1$h43QjKTkto z91Cu{izFI<|7YI1XqNgW%#0B&W^^r9m;0MqRVSX2VKh-)Ma`t*<*w>@u?y?<{gmuX zgype6h)N-Hk{Blhe0at84QOt8dLp~yUXN$Wb11nLJ(9WuWyfTT*5s;RD{`NDG~m1B zZsx+{MPG=XPQ^@9lIL3}IFY48=tG_Wkw#A_DglI_v^?R@Lz=L2E2aoB2ByK-N0!b4 z4_KKl0yByTyq}{)Wu_P&!C+c;0-rJc2-y7F=^}F)y_}r%E)GBfcvHN?Ss8z&+>GL< zjyCOe1KzB*yKdg2p1X0~^1Y@8A5l!G>b4k9^Y-PZ9i5pLM|Qz$V7IA&pU${<{t^Y# zzU?W-+b8wYhuq& z*xr28qeoDc2okqI%t1^(PPzDD@2zw0=5~ipwU=mh8Y$jyl`y>RA@n<^J1gfeCD%+t z=ptV&VPyXBzjGqyznQb%-6L?{vVl<@F}-UJ^mZi@73e9hmEV%OWe>viSp z{e_6cEKbUpp|V6x>o)Jb>a90_|?_Dxz)A%6}m^A z96?D|dMkr+bl7BXa8J+?q7!adeJ@8RkZ!w92Ws(eqrFV7dd44ytM%d6u$Wu8NB*rA zwIy19VtyKavUXY!CdV-z?yz09cVS!kO95zPJ$4mHU(G%pdPfK{K-%>SBAtWo$bsC1 zVcyX8-^rG+Geow>9#nml^%gWZ_}LD@)Mo{eu(<7YZB%(F)W-ywywL{)37(Pjswe9i!*x}EN`Rt&&wyZ-GPU9=xTS=mTSJY6af)g7g-mSXD811j`-JHl^gO-f1NNs$+F+#pl`uj zEJxHikUCxBgv5ixUu@Y4gQ)Uh3cGwbScU*0-JyrH@??p$H03=n-fZi=qhk1+J&kW4xzo^^$EyM*Mi$Kt}X z%opZ4gwmCTMQ~v9zJ%rJfr1%_BLuo23YRVwLb$`6^ILvl2h53t0%Tl}a-%}g7sHd^ zQw}bLeS`_EM`{A-1j7WsH|J4Z|7)R}Qc{7>yhRh#$B{yTC%g3-GQAN@lWfWs?DRWl z1%21=eK7m&RD4CcGS(GBnSTHte{%C{r4#Pm2w)UB%K)bAsh*uKwiT?26H7=+@r z5hq_*@NxiB{XkWArY`uX?@W1vbc9t-+U>!__&C!?C}kHK3P|N zdU7h@u)Zm*uRLIzG{TL7HW`Da4<3@^^MX9_BH5`#4#}3p+r9&HZzL zi(f>sm72w4md^_HnY6;J*DPk2%9*!}@u_dGm}D$SD9y!Hc}Z89zER z3|E+x4AzcJav@Lo{QB++?Y#ERekH)Q+WK##=K1b`JOK;8qR!e(Nh<+rMp|bfK~YH4 zOI;-b-hk~aBZ^hZl8w^s@oa8}(rWpGZD-4@a7u$oqvo+DTd};&p=raESE}Zp%d7jL z{g8qZ*X^A*LuIaig6Y5(XH4jX{Pg;G23OGv5lE=>{iV_tPne$F>>+}WB&WZb@l)nDeffsK4W)o5Lx;{@kdk!Hn#GtQbxNcIjc=C(te z01YR^sPw4-OSXpGp4=aaj1+#gXIreFfa*cmC*vK|L)+ey>aigM~O$vy+wycn~( z*I>c<3O*^`jyXx72X-oC0Mgd10jV1D4jX$Gva@XVTT1tjm*;Ec^H+>k@>&8-j^$+6 z8S$7imzY4xS+di0GJ%rjBL8^4)Nqhx3Y!2RNCv`Tv9sqxd1neSTOE{;la0X>vaz)M zd%y|TTtJ!CWJW|Z=QQTO0@YXSh_E9}-cws3oH*-+H=6~NY4Pd90;FUonpyEkD#Moc zmd0(h#)qipnGLmYTP<&rZI1ta^ zyH59J+PiUxkRIx40iX29sa&hR zf*xSUt({XJwENyzKx{-5avxk@G+5;XNqw80!q3Dp%_h*o-J+wpt9#m}*U{sa@|IXL z>-P7hW%9~WYvc|_Zr1FXy&rYOp1(4j+3O_zi)x}hIU!x-6rmTtD3x+Iq6h!Zo+LD8 z3P!I&zWLnY?MP786X?$S$QYv2&<2v*mIg>iQ5f*bSxOU{*}jb%h5%hoxxj+zPLBJ+ zNxw%drPJzawRR@bF@&Le7L}&xQI^lhh$(Z!kY%tUvEokCP05fvC_#9ec^nnRO7g+I zy!%ZsZ)wXEsY_HHlgNwL8$}vvzRa$$x8ifkiJv;O;`4#de<54ivTm)e&8_d*RRY7N zZH6Va14I^oO$!|on(fs7%q%T-5!sI4zY%*1#3CfqYIlm~uq@r72A5;CBhk8%*}aj` z5n8d!{EYH_^r$i}P(%;ZM+1M0n)t4DGPbkAT*{b_5gkb8&%T(FXf|W+1h0WN({rCoti+^f%_QyM z7k#%|;+VOP@Cy7t_h%mXWhFa?Q=iLT&qWB-Bry<8G~>Sv&N`?heM60>;O*uJ?$S&D zo0X!Z%&WM|Tl4!`Vac7^nm_S<@Y=w54C4E7nIW0u908imP>>KisZr!^RRIt~P;4}P z9xawIy*}4PWI(NQ1|~d^k>8j!k?-u&r8@IqTm)PH08l*VP_yk(`SSt=>wc1+Hvg4Y z%}AM#e~^t5d|ezuwlIHeZo1-WA@Q@}QZ&{7o-1^;Ui=yE$bg zDu%*G2|PJ{BDs-#liCoJ61*7F@_XV-4O`tb?($JuVz}xGCT43s>y4uB z{~oU;&Hdvf)s(DxLRU3yQBf_=ukuYvb(iJ^jV`#N+vXV=ZRr^}&G~9NX0Ql3j5HL; zlz!Xxy5|rHc3v)Oz{TIyu2G-nyZQQ@wwra`-t2@`!LcmwJ+(@pCrY5@H7P5lJv_i~ z{zZ5I8&VAuJj2K+og)+Mzaa(LHRwakQyw&I9IYe_8D!k+KqG}B9WP5Boq(DB<$JtG zG#XCkNK0uk2Qj^csx$@+_Ubp(CK#c9xU=`Z5rs_nDjL)ufX9Hx3crlNslYFZ3q$#+ zTB`i4nLOq9w6(qWsx=jweBIWuCew`8v^kvtW>j)%8H0IK1LfHGsHlv`&Sg3Ei&e^* zVHd8Qn-IOJF;5;Cn4#h1x7Rw&OqiA>5^Xa4?UsiNyk%imdPXPA^w|12K)RW=nQ|JH%YUk~pllZhZId0taJY z-q}P;UAQc|Kg$hcNJ>Y@6pxwjDaH#=7(hx$ebA;0Sod;4%;&gp_uW?U@l%~|`y)`d zLiY@#6wV6lWTuh3P;Hql8#Q|r-M-&NerKfl;BwSR%j&K-T<9m z5ZNyscJF6F=lmE!orv~3S?Tqkpc*`0$Zzao1l)tq&s!*NzBW#W5ypr&WC60Z&VN4^ zpOR_~u!J1fgDQ9i4wes!DPIh=yoNbA6_x{CI7kI~B+*&Bv{p-BVHL`s3Z0)3y>I;I-x7u>@5mNoDPw>X6J<%ghQ%y{~O)P>>+&K|;}ha0>I|h+O`HnMS=| zcqkw>hBf*md@z!6DdJ&8U@bhK_&M#zj_VRCs^MBs~H`*&pP)Xid?Sbyx;-v`{xN=A>Sfe0dFy_n0z(Fo-$BwuXtkqLIGA>8U2MN z8x~_*i|{LF#K&k;@Uqh<1KR7etWP*KpF0tijMt3>+6cfGsz@=Vat}NeIuQJD>etCl z9ZiftmpJj2FZO+ueUK;gBzm6|Ad`uA>CL_r-$`-o8)k2Jc`#{S6Z^LcROP;^Li!*a z;mPy`9wNe!v#Cb$8)C`?8sQ-fno37okbivjIz;%MlBNh^?Pwk_&G?$rxZ+7SuU6LC z`cL%+?&xGQyWlk=`#tkL;tK0T!nYAWiOms@muL1E!X$Em27|*~$fz>1Ua92%>v_|` zCqit=R^eZ?gl50ZB3Ky>01DH6SzF4kU)saBCi}=bMvgdc)y}>>q50cIWTRA$3^RAX zKGWJFxz8pp(9lZj=8nOzGAdkrvIwSVt3^(15gvdUDFcC|@(=}-rKMYL>L5Fx@2=|5 zIA)cUXA(xSJg|;FcR2m3FR&6wIZb(~W>-t9rd}PgxnC=_yI=pQ1L%9&EE7DdZ7wL} zu`U0jnRxt~_9wpVFK!_1u5V`(06PxLr*m1)>0tDWe5BVir}u_chcXamq?|JW>s}G@ zb026Pp{-81zUs<}vnq8$j6?UH%kUo!-MeREQ)Zb^t9UDOs3cNj6N-AtL^- zjRWWsu|P1DR?B*X3$RCw>9cXC+4%K+Eo_y+j>uES)9UEjBiv@4jm50OCtOqx!bE|h z`fLnX-u@RqoYb)T{VqM}dgW(Sw(?kLlSXkABoy~Y*;2AXWC;q~%xQhQiK6m-K3&_q=()sA*>LQc!t9e6lanwth7)A< z8fG#b2b=;PfMRVf9qy)TTf_d?tO|7kjoO=!uM=y8YazLF`Y6;Qm#inGd+7x)HkW+@ zHNDqRmwaUT4Sqktf0^DO2$(1}TW$69S}n=xs48t}K4$gORxr0KaA;~QcPI%ei7vX+wSU@S^mhqtvA)1q z*QyLp31jGa?%!Uq4r>o(w;1oDgI$XzvLxA>c4-)D`|($1DtV}}Ck4o8%{LV^LQ1IZ zKI@-&L4xEGI&I85N;ZM61%dfg@lrH@hVp%sQ{h!rRtzadEUD2_>MdCHR1BD(IdE$6 zBlDx)HGsRf3Pq(y(fo%;8qwc_Zc|;GzZ(}I7)gfn>4)0KB5KX~rNv^(%UVam^6Z&G z@J>H}MPL|pVTYGyLc>UKM4+(|m2cHUHY**~zJKHNQ8qPIquU zpn|DXL%wuEglI>`B)#>UgZ83NgbYVk3>_hx>oJ#VN9s!cyQ_!(tz(=dyND7@G7l#+ z=gjj_R9@(K850PQNoq3WT0W8_w4(Jh3CW31)WaL2Aiw+biS9?AaMCrvUiL3kq}Mq?rVdj!NI zj`QB@f+4!qg|{cKk{pw?cJ~jrJ$CpfB}kdBKvW~~q~YO1H09zgY(<{G42JwxO6?N~ zdmEBv&_LwS5$O_tK_W}DQa`b9_z@+9!5(~4b4u(czly5Wd(Qg!AXN#cm|75whHV1B zXcZVl{$eE4CL9<}?;FVBwwBFra|SLe`fcQYZMd(w5`wIPmIpVy#eH-PjzLChB==ml zpE#6ZUzH5PDT6o_|IPUQRPlnw7$sMwH<6ou6nHWU9VP0MtuIW#GTqSlP7}s4as}15 zRbaznIo^<&KKAx~pPp0X&^TF+fO9zZf~&K)e^(r-1D;Cg*w>%_K>Sd=S{rEo`Puhv zFZzOLnujVlwjurbb+~nXC}nav&_G^LFXfwe)|2fa#G;em^=HhOq%m7G8=u5+iO3Xp zN@5;0^l_eZ95P8F6KS~LjR;fln-{r^h^fLGO{w+y=q6**0Od+3Y--dSHE6V~(4kO+j2&!M@GT)wY9%O7vzOk+(7fokQXA>s z*Kc!o52KRzXlb6W8OI^fDs;2&U)4wk1N<4J>*!0b{)AurdulM~zAS06Q;Zdq?$H!Ccv2dNdPw0~S}BEaJ+{6t=C`i2G|=f9?Q~Xe&~KqRsEdYY#gV>P73JKjT5_?(SC|!NE7^ zh3G?2&Oq3GaZ(3A@6J3DV@h;_`<9&RCe6CWbf14-L^;x+k99Df@hp2UW&t$&f+x|3 zozJUs1Z$JsT6P^g94}{IW2XLKHx_q7JaZ~SmiI1Rinq(vlEMRRamO^rJja+LF5%ol zozfcRl~+1+I;Uu(eTdi$HMB_B)XqO08rI3Gsri3Wq&8JRgM(C6lahuO*uxP&8}8*_ z6k@tiF@zJ#m_g=7mMT7xx*iDCk7y7NPv%RKMi38=z4v4Iungr(C5)_9B(9YWZjK*Iu&YdEdw5=D)Oe4` za1!b@v&M1WD2-cO%XuNw^HwxnYUoM0Xy-0B@V02?&(GG0R6}kL9;erE68foZVGT@>EqV5F}=2X8CinR<733 zXz4H5RbI7e>SMIuv7PfbXGRqBek)=Qo&RS)^_~0Kaky?RHr;(ciVB$I&M=6dKl1bO zmsAW7-*@%$+No1R^KgmZiegMij}=`Ao}z)F*sU0r~-Z)p>Mt1`iCdUp@) zds>c2#lSR6BPPye(qE*~=Qeg;V?E!MSW*c660&U#3o<%_+Ul~II_iNdQ%9p8$(lT8{{H;{ z4yz-Q0(l_&749GOS85shdWLE11&Grd?%7SRpS+Q}bveG$s=a;72h|s6h6zC?UK6^e z_}5WmekE)|87pO02n=)uoK*FaaCLT~K0H99RM8C^0-IuAo&qyS%8Xm_1|8zGRC=?U zV|=A!>nUYI&*4X3FY3WgD;k$hM^mnV=W!3dox5jdo|v?rv^e8(v$5y-&eJX71cnZu z#~#~a%rZ0O(fn?a$w`sVgt;b`uIntd)qsc3B1pjD6*u521XHTur}H?E4R*A^#~-Fg z&?@F8a&2aw(<$(g@iDvy=tLhAZfYa;0Gs@&760Er9CiwrcL?qh+}&Lt-?``h_x$&NuU^;gRo%149BVGyyQ=q^ z<2q;N_o)i6Y7`ZcVpu}qACEnd;^UY&wK*biVw$T!FcBa@Q4imBv7G-6O;{JTkO|}p z;S%Bc>{a*uJN@}y^!3*(%6Ed(t2vM{x4_~E?A*CAX1pyG4}c-=(94*a5$rUE>ZDi_ zDhi)Co9DOMSD0al2qQE_oAW=0KTV6TUEJ+H29L7&g>-&_)jb7AMZv$*e}m6}yh=sJ zm#4KymQDn+{j6bkY*hI-`@gu)9J0KRdE;^X!A1DRoe3++?eV4wwqb)4ffu%)?yYOxW>&UGAwcHLl!!2XZ#YEuq*BMB6ldJZb?h@nBn?KEgR=7{+iQP6>03QewE3EAkIuIZ;QpX1f_CAGE00nRV36l72}egO?}E(p2j5Ze-2o}xciFURVVEM2&dHVo=vgeWeLzn z&5%^1{P+b=h!+`(G33dWe(HXsjvS9zSsbm1UPq!1S=%yNu!2?h)R$LUQcL9*X=f}w zo!$-b&S!W3TE4|Dae)eyH(LmT;OOm^PQPoM{j+*|S%&Zz56i3sR|RHL zl-Q!~a)-ZlyYrRC&-Tx4Kg`#L5Td*?yvvcG6V;gKk54+?&VHcDp>cFE_I}MUD9yPs z1u!Jq1m~b><;#b3n=T;PFkesW!HW54q|ZQsBABn*Y0TQ)7obHjhrf^L+rF^!S5EU! znY$o7gKKGZn=dC?<#ACk|{zD6~e?DQb6Uy-)2S)!QSa(t}EW_V^V_ct!gz6 z^pnXQ4MfaX^cmOT7h*Asmkt(`2?i=gNl@&iU=iZ`44(E^sv0PtCo=T| zf$H_vQlR8Za7aAt;?>wB<`oMC?BWbF(j~VL^67QUB)QXCq^p!{0vGar;yP(MyyI?! zgb9 zXprmH<)=tio~C4t#Ej4;5jrg$sw`^(;RP>iV^JE7I0{x+#;fM}qlZiupz&SP25|IC zCaF25WYu~IoHR9l7Rh|ZXk7S%R0(QC^@roMD4@RA<3bJ5^y=Pf(<)sZA4j-HM`mM_4-3MLmuhY3&A7&QFYzN|Pj;VAg!??dOvKXt2 zv1B7#y0Ioz->9F}EIqFoP-KCnfK>!Evu{e35>v)X{_noik`o556#=m&hcZU}sy$lT z@(XeU5==$O?Gn%NC{Xb?K8UnJS~Mj%IDL9zwyHBQ{oMmvk+ftig0Xi2EaEB>VtRUF z`t&f2$sC4!Y8YFwk$CxRSf?=yTO5?T=tTr)(;+1gPD$m#TI-W5YpW^>K*#O^ClZnw ze^l5Y&oR_Kw0CC#4XpzK1I$k_=ImMwq)3gG0UGoDFtY2J7+&uxIh}XHDhaSFpxep3mlghHVbpQ&B_=)44UF5NONF7R%)fnE4jjsR?c9El3l8KE38>^>T< zC>oTpkcoc81uTiXT)*kdYykNNlM;<#&+7BWSWwR2-#=qP^=b(hjtUd5%bs0vPp*it z3zZJ8g$OE=!B-ZQ60_BK>($hy zm^kbHKI>b9)o)o@I>`-YcqL$191|=Ii&}DEG9l@0-3(SX$GQvsES6m-RXGbwjyMLj z-SyrN0;MS?8(Z73%)`y$_sT%WOZcju!)4b-k1pVK{j2ruIhFkL(RN*NyiyGZb zO-xlPq^oFj^qz_Dc+aB3+R83YA4;AR?!}+-QMs79{sdBM(^m9@&(O&qgHGIpofH2$ z7YaQ$3R50`9ll0qlZ_u8Wl4-KW+GglZ6W|EXi^V$7^b8!i^?};QNL3DkvIB z?j0iN!`9G4iH?DLv6tuW{)0P&V>V)MrdJLKVO^EdB{T0ZAn4d(n(u9)Vt6Um=1j)Pb zQ!k9|ULl@BsBP0|#EqBC)0?J$m--37Fep<%LOHpxH#(`E?!XP!0Bal_YL1GxcE#Kq z(fy;3Zthoa;IzoJ&jtb)C-QtW=$gUdgDekSdCePSL8zX|2-k(kz)>SCM;fC@NLoGh zEv%Q|6wPxMN;z(UwM9{8Gt5y0wayC^6%$-Ja!fI#MTdd4ma5I54`R+LF9!{9Jt>7= zpMz3wc?%@S4VO6LEvA!t~RoVtJ3iCv}MRFr!vR<*`_Um>8-mUzWUg`6f<2hg(U?c!If3ewk6a{&ks^!s zb#A7$Zb2sdz8id!h#dYAG+O}h*)lx#`xtci?vWXoZlnrw2ZoF`#ggJexFn0*tBu*Q zN~*WgBNz~wOY!6V;zFBx913kk==3MTFwZxVX~xTplw4eRCUsx&U0m|#5v6yK1-b0~ zrSzfIseC7X!*9y$&c+8rj5ojgIqmyh$Z>0C3C9w=mqz})47*@>*9U+Cw(u3%Wz^Qk z)jX+r@eOaTAw|e9mUz&ql;gbBO?_6L_V=2sC``1CRcSb)X%wOAx259qtovh&($a2; z>?%WOuStwaP`7PB*VVbtj!hE$NnkdA4~K{}6!j_0HdUu0=|L)bT2mU1H(pYn$M79A zuH(F4ev+cHwjBw~9}8*VuH;To!Gee0Qj%Yqg0ZWcSof9!uj-Ut-WNtvf^&=V%UYXs zSzO(^#~cA%QixlKlUaBYizeP4=04Y7?nxlm&L7BSk;*#Jqt>OXO!yh@>DBfMpvzI9 z;?;qw9PX_{>$P-|aX`Y|>XF$cT9Y}T_rkIp>ChV$N$Hgw()Of_VlvSYn)zZj#IxUpMokf&x*kb6ejrDDq68k zdINAyG?0X^A}S^Tf!RDmsUf#zi0>-_{A9y^39sF8auG=}$?U}>w4(fX68Z?I)N{Dw zRy7D*j`BYO9GSu3qo>sbMC#W!wUcx0KTfjr7Fk*yg9a{`Nx*QDDT1Wg0)|$WGaFy1x|HLsBX>`8q@&jVG}EDFC-5 zn;zWD>z7Ea28*Y%=bLbni;sQ!970{#c4+>}iF$?n1pW$H;i+aUaeD#n<*TXZp3sI#H0mDmcQyC>w zme>^wV-${8=l4e&!e@>&feAq`QKB|yh}fR)>Wwn9-oxDaH>`=JJFnk61I1JuJIrf* zoYwD2+AH1<*e@d)$oMfXTyl6{G<4EFf|a2javqY+h*t^#pYDjv2evnc(qsF3>gol9*mxh&&s0Uvm;v?kSI-kd9#f!A6 z#Zy9$d0GWENECg--z}+?S}V0Mu*ttS>vfl+yQjU#vziiB>eU^&E<; zt89G_efH)-F$|gY82FLIHL7J%`?f9s(AqmcF_v-DnTbPGPFYtlY9jREOWpTZj%woE zyF{`L8UDOuEUR>o`3wF2T!-NJq5eJ)Ir;W;`lhD5ckD1?ab1eF z+k0+*uSl2I$(mY~Lmr9PQdvFM&wK_3Lkh*o=}WpB)x;~fRY=?DiMld*5>@hK-Z?U=`cO|-3{}$jfr}9fQDflz8+aUh8b{Z4 z-k-`9qJu`eP7O)QWPS|2u1o)$^s@`jn{FcVpc5jUOcu=_7pnn~m`h?Bv+9?`zX z(qL`OPX+f@FI}d!USy_rQMF9NmaSFK&@?)9`U?1A-8$gWVctB6L}OIHb|wtXDDs+i zycn`BKZaZ#zPpX(VBhykhsvE^l9o=r4QpmtS3t^M@l-h`**^WESDCI_&j~7^e6Pm4 zOZNQJ;OOnk#%~8)t{Zt!y`uU#_0qm4@k^O^0zUda=9SHwdRDr4?cB~Coy+aZfYavS z=Gt+5R&oUTI8P7#{A2*I*cJ&u+>8jSxx;_s9{GBKeclFN6sj)waE5sGu-goG`@{mq zqK4{Fqfa2euNF|fGt19Pds8A1Ee1DriyAFX?;9r?HwTdDNyHhGTdW^}X{-0hGL2lb zywLKsEPaG!KXP47>F{d%i;^gN{4yAow0Ntp!D5#kwGs|M~!;Nsc9f zZ32r+b!SFR|KGtaI{XM8c&+J`7KT5N;gDu$4w+ip7;a=2S?8v;%dSDdAkJ6#&iz*g zK~~@2Ms5E@M2$CpKj$L-z{E$p_s==CSAB!FRmBE(J>-EAqLrih!~Hx>V#r%X!|Lr| zqB#%v@j|OKFe#_(!cfNx7G^CpP?Ti7qrkuw6~MNUu}scQ?Pa}K9il&Br>9$-!tWpc zUW{0LjU4Q>rhQY0ED@aQdxrGRpNM9lXHX+=hT0Mt^xv$wr1G}0&+p`PGumd3UAEU6t*I7)SQvA$r!gq^>A;=S`4k8`b_DxYS4Yx>2Q2ZAclcVY0U&` zDhF;klKG7#!Q_Il58W2;B=d!GKHuX}v^s6gPx5)0tUU>InTZ?>g5TJ|yP^P3b*x=F z9A*owT8?7tZz6bl*d1m{FRUkc(Ccyw+MG9OC%>i6Uhh`V>gYPF?6yXm&d1;0n6Xlq z64EEp`!aULg51g*6q%nf%rNp{J2`>*EFzIoNm?Cw>@r#m&1#kgQc{E{SlJA8PWovg zy%L#dX*rr0bjzE_f!LtXj?yN7d{MrLE)_Qpx8_TEs%+@ur;1oO*2{p%*jM*KmK~)@ zJ!r?^pVv(T1In>^n6neKani z6B#K3LyShCV>DmJh~!g>5*ct)*-YvvOpkn*ChPaqr9(EtD6`Dvmm^2|H=1j7N1FB>B3lBaiRs!^ZgWLq-!>R8f&*Ywy}btN|30ThSLxo2Aycba zkA8lcu5wgu|O^8B?zghY2!n+JGsIiv?=E%X%Dvv|bMn(M<~t-^LAfeNMpI z`){|v{g0++2+zMjRaXaX&0>#+3BL+&uDBcjqx$BMpo8*eSjy)%h07d;z6cxYS@-Rj{H1k2S8PIuN0rBU@~{5aggzuuXWyccAKToS(Kut4jbv1ipOH-F=j z`BT?(S&59Z%{MKwZ|b&!zmoaUDM+T*#2xFuJQoEjjh5g57j5HY&YJb8U$usmrG=s) zK=U#cP10Z=R}{?`?|O6OhikLU19}BL>KTrLh=XWIl$RuQ>sj(diAst3NJvzK4?3G3 zW*zy;40;IAj+Vt5z{H4i+2>w;UU-y}wwXgc@#UdjL6}+?#7_7cJXqFPG=tOPD8&4Sc+B zlsZu-s2-lyb`jAEvbEbHci^=X7M%rQLL}I*mpH>$-~2gJ5E%S0tKGQd@eCAM@|Ht4 zxVMLKmrO_B;k8WTMeek|M`_T!TOK8VofYZ=sMff`P&*@*W|aNx6EDX8QRF*Pi+Jx5 zM{o(@ditj7cY4i!W{MN&r7t!*&OkEM5AP1-ci1`M2~ySYt4+00Y&h(c?{ zg@vxjVs1I-$*6}5sHOEH|CprB!V)R>{ z!;&$Xky{TYCxfX0d`Ur)_F$(9wb*Tj4CN= zBMf)Ud1&ndoh7KbTlxowJ7g^RfZA%xJe2TFF3r{REuUgZO>6qVzrdC;``gO|#Bu421faY_< zdp`VCTd`*(z}YX1Jpf*5)QUR*T~6ws<6hGK!~oOmxAA`ZPE(faM-hf^l|wb+n3@NH z3LbHlB_EqpCf<-75E@rz4++DHwIFFcTAOeO4I)ERGwaX!3-XJGWSDw$|IHqL$(O^t ztE_X?>N13;A6uDOQ_;(A&)903fcg?*_B%Ap+{z5{69uoy>vr}Y+S{wWh62mW{P}93 zSJ6vrixgQU8T-=m4jOxWJ6Hy;2zKQS_fkHYz2KgTNWNWEvM>Uf!ZVu|)Kvz~)kz!) z^zB>Y>0M*%KGZc5xseJAHzX$I=7pLth&vwG&7-UoI-XDxW{FtM(oFgpV4XER)QX_A zys$E-F0`Zm+dlu~Jrf*7pIL-I7o`vcjQCc@8A(S{l~-SX@c5;h6U2)PJ;T#`qnw`4 zXe6!2XcD{L!{iL&y+!Mr$~g}2&BXJuR(U%dyEL9>AMJrmXQf@lieTFoAv-AfbQ1-9 ztzx^cumqtY@+7N4teS)p05>jGkkuYyJfy0(Afn=$KMGfScQF)qvDM`H@&2Yg`u;|q zz0`&OyMqx3EWX%fk_YiwYXyZ#t5P=qE($5=q~M82Nkt{GKpH}3v5?Kd!LRgX5+NJk zE>Y{Qv~%H7vZ|kKl(RaY))d#U2C5&0O^mVj=J@1v zJ|8FkLK~Bi&pLb$SRG?sq?q-tjJ!n(g&+Y=*DZ z@T_4V_po7y?fKVEZTCIxQ*6WSggV}Ck9{ENjqz&4eG{$YrI?*22q^75H$ z&JN7J+6?t%vVi(mi}9r3*MK+0>M&7EQfp#Iy?!{KL=Dskwn% ziV|Vfc_wLozzb1&16dk$ZPP=w#&=u*P8_s8mabl z(2zRoZPr$cefjt+O2}}z%9BM*RbilfQuRF#4yXw=-_918q{uKF*$jajhDI^0v!=X2Y>DqaSJf|c>@VQ9aO%>t z?D&Qj{5lRL{@ksGmbNtPTS-FIm{*S)bF z*SvxmV5fWEgR5GI_Y~1R&sR6M&v={Tw^c0-La%Y>F}jz}DRn4on6oP$x>d&h-#>PV z7gy^if2mJVA0+J_QL<5OBb-1;3x*;gxR>fcHjZPhUePaU6~oQS9_YN9Jem%`XG-4& zZ?rDb2fwXy{kgc~!^$r6StLE1j@h^sana{|2F{hIhT+n#{eDF$C;l3P;s-ZoJK-ok z_(eoM66PxesiCVVWnyv?MS3QF({;Hcc>sso*Q3B1c|Vkx_^A zj(^%$Wi)r_R_2CPEMA4P+8uO6hK(39o1;$V$+l%J@Sm@vb< zi-301;mm2k_GF768avvol`{%~*Kj^3s+1oj746=ZTp`Qu0r7S6CIVJAw#Z}Oy;JD}@A_UwEjc@t zx9;`znsy#RRSGQZn=+eP^lbzUo&sm7o8q3Ah3VDUpT^_6dHe)?%J&~{i;m1lXn;_r zv6h$!6G2T(L#*JNdbbD93XEz^;E$2SBtnvSUKe2rMWw#n*4ox}vMKi83b&Oi6V|!4 z=C}G^u@R|f&@}M3%Kci0cP-h_MbsfY3dq5DdYPt(4O}BEn#VMDerB+`LU{DM!ZB=e zv3EUjS>e@@sy^FeQc)U8tMnIT69H_fsEM81T$?XT$q!c|ou(V=_+~>%USqm9HSNZF z875*$)u-aEzQg08#!rWx>Wv;I4sS;%hMLKo^vp~iW`%Y2&iiScvK{Xssf=PxG85$@ zNh0Q^gyAX);YUb@W<_FH$I@K0)epTvoRglGA(idd!6oQbwjVpwQUEY21tjzFzCz^&*KQVA?-u(A)?NyfNbxj;Eec)9x1%$d54urOqee7Ufv&lDiR!`v$hz5PQ# z7x*bWB8i@i!j4)hB11}A3KVT`3py)o6&{Pkf)@^oP%^}#e@Yw=y2bo}1|dC7v{|D$ zb!ZPActJ?N*K~Z$Sy0W$MT_e&Zy(a%E7e<-A}Mp+`sfYXdM~7h_qeS^eGvUQ^nNwy zi6PVGdhE+_v>JVo{EQxf$4&CGL>h3wSG|{Y`Hs-()qb{>Xf$D^1the=IJ0DYDB1FM zaXz4@xNRo|3(SYZPz)k)g&#wQL{7?&AO3!SlWV-NFSU~!^NTck**=0UH{m>aP! zXu9%501_F&^UXKJAv&bPr#s$po!Yv~_@2)#wfwb;-U*JY3ec84;wX%swJ5`p(-T;3 ztx)4w87Hr{^62{8P}4Bz4#MbJj_Vynl@Q%2+39&VZ;O~vH8fIxAsUCfmVeX*om(Xv zz4me+ZYS5Vw0QfIld$fC;QBIqw0!l04AhxXSCh6KE@%7ogT^(>+0+&m%k?-r%IiW; zYCcq+^c9PT1dtX$xB*1pFkY>kvmaSnwNm0P_SJGK?X7=U2v_mHy%`dxLA?;Y0gN-W z6cCrH=1zF;tgRr8>6`5`)wu9ov6t8b#B;C*)Axm6qVsKrXU%b6e}}vomH=4CMa_5B z9caH@Xsy#X)xWItw}2Xhv|}H$_4&z)vcNCua5H3Mq#h5bV zn~{5Gx4$VSzGl@bR;qMOOOMa7yRl<=kL7H@ostQ`i z4Tg$}F;Ru+iL!L@%*4zsVXa|eR6cr77W3ruirHnn(YNj`^v5qs!2>^7Al+hoAeW?A z&7sBf05cvwG0?k)yq>^!a^{yuP*q3R7plKIOB%ZMGk%=Th^>Qu?&eOzen%hq&oh1^ zQ;nfuQin*^9MuueTYK;oIpdE8-xfw@3tG+Eu^)fpVgzGbYn|R|Bf);#myi7JppUl_ z;j9m0(M$0SE%yx^o+kIbR|K9Wzr;QrjM_Z#5(or4ajU&V)yBNR(V-&G9MBIV6v7(U zkDYH~Qa8N2!K7h!qYXd}6FL(827x09IO%ui&YgAp3N7z^7Ve#3VDNjW@UR>Sc8;G< zyrTcPNB+E5uKlNQWl}GEo1-(W4!M`n8rF}T*Xz-z4`63p%JH7{>EHa)?E{<2igM_Fvn;yz9mWQ7A_ZR-q(twr`yqU6Iih^)DP<*Tuwld(&W$n|q z0Tx=c#-d z_ieq{u(Gd8D!pjHh0PGFYUynOpjo>UUxYrC^oyf{S5obrp>Xv4_q$Q4J#|htGTqAt z%-!DCAF?L1j3HiCYm2kM)zrtHYrY>J;=XU_pMKm&059>H-k7^x!mwQ|gDa zIKki^^$2|%NX}4PS%OAOvF`FhH7%ejLTenZ#e1j2UiTHqx!T}oW}J_bAW2vP@AG+R z98RdGN;nlkXH;yxC|ox<+T#Y^IwQwP{G8$Q^d=8lqxKS2>HcYZ6>{}^(gzvK6YsFD zg@vc*;komYt`S)-P(a(D??}h{O1$$=bNJ$wDa)?-va-fpL@5kz|JRM__}(V9+9;H$8F!sklD- zPq13gx}s{4x}DmCe4RJ$iX&qE$rPDfJ>@)?w)ZiwLsBl2k zp_jB29uQfmsF{F(E5;f*G<{HF4hk|Ys^~Ek7i#XoF~Asgg$Qe`fiP7wz2){JjO1oO zfJAg>=xI=$XE6#s_sryUn*LO0Cg1R0LI%AK<*4PaUrFbjYIw%X4~1s(bH{gvH5FFs8wu3zk80#6Op&}(r}Hk%a|&1qKyyalD$E# z8-*Kre*Ac7yvc@&34+`1^7HXYC+he!SWztK{ZgwaXA2@-I_XwEM`;qEm}sH!##u40R86v3 zRx*_*$$9+1FO}1eU>egdw8q=cw{dBn**)x4sJ3akHq8C(x?uKmeS`I8`g8BU#j2@dv* zKDB7AlhVQs`zBa3nf(tyeIsg-NCB__$>D+)dk(QhdvFV zcz&dT2UA~Y077n^j%otO)1j`4wH!7V9x3;nlt^N!o#7ddS{7MrYwgqBD4B>eu~eD# zn3@|OyEn4=l%hcs%Uk&!R7>+Y4uMFexh0{<;?k|c;BYKk%g0iO&~rg5*yE3Zux(oa zoOQqC>IbW{rCW)DSfr+<{mSlS&G8uW-@@Y3)GUmuW3z$>GUuS7Dcf2=h#zs zb60owqhqiyd%(U{Bn-qv@?$f6$@seO2g;e@?i(xY^9y~q8|xVtYkX5NH| z3w7{j!-AnQr?~AEtgHD6mGWD4iZXtZJQRI#R zFhmv7r?6SgnG(RU5N2oIzRi5t051DvCul9WQgCK0$aEqFEs7h#Eipz*q; z(Etf+d%p`|ZuhY4Eo3aja-~!Qizs*9)hU@hp^pAGMP@z5;^N(7gQU=`_cHC2i&N6$ z#nD-yIjbCHax#l3A&L7zuy6`H+mjFnAVW17TA^Bz)RJ-qNZk+^Q<6tJVJg)P)KU)w zzdes-s|3kpGoY3A65e=jgZx#xReE+1vk&tjQf{`Bv?$DWy@l+W!GaAD;`ETehAhtZ z4MDvL-Q~15LgVhmwGIEdzCEAI>((IXr=~7`Oj7l`TK?Kv`I78zC2G^yrgAp{aLSX4 z(w#_l+CT0tEsxJ&3K)}^&`u@fFfA`=*3N0xi4(*prIT=&79-u;)R8pGa`Xsw)Vwd4 zYQsq$plodzrffxp{g}*d^X@5X@a$mui-dQMjzErIaWv&CaPwqDj9SFl|Fmn*@iFH( zxjA+DYYK53cF|nGshMn;TX8obD$FNCf;Ho6`^i#y zcyg<#$JKf3mzG&ABF-aVnYyWRJ2I85>-B3a5eC=_)y1`f`a=+V1#ux zAGw_pZMf?KjHGq$(@h>OVzyghd)B^n8XTx5p%h-8fmPKEa<8vfat95sLF7WOt*sj} zO2P=#MO<^E_LPU?e8-+x>{W&RX95UD{y**%y=x+0tI@N=4oM9s!o>@3RC&=$+Z!$O z!a7sp3S~$}+Y)L}ut+|@8-zi?<1=k=zB4hhF*E)jjaBCOuU6%tjEwOZb~w=gR#|0X zVTh%|2Xx|lY_}P{2Vb~Dv}(!JeTAl zdFIZl;nUjjs@pgh;@jifnZkQ5ko7*y>w<4u@^HC5;5A+;`E6J1PV#X-6QPl1iSTf; zOaEezrCLt&A?~zDxjx_5iXHXNIJ#kUV1z$$3~W8@#|(!2Yb7D8l z1zaXQ#obU@LtjQYB&(y3h13;w4Rofxqcwbydwn?A5_QWZvr7xubB0P)l9PkZLNuDO}6{%6zsrp;x& zly_6OB7l)~ z44e(D?aV&=%j}=t{fyI*1^Hib{%13pnB#vs#)7l5u(C48!#QCSus}2X*FnJcX=Noa zCHP-9W_D&)mUwX|c5r5P7EX@%z<*t_u`tDVI}rnnEG#Ua^#2?Lw*T9MgNgAo=Kp5< z#Ib$${C~4CvoUh~-#BJArhmrx|H6IdDgO@}=RcCrz<&p_GqHWn%>UcQ`9CUVXXp5z zxKF+5|KZEY_PHAVJ0AxF=l}7=`*$C4Ff;v+N;p`UhJG;+G^v_Uzt}6*! zQ#%5te=jl#VFE1zZe|uvP9bJ?PDTb%K~Z)_A!bfdRv`vfCRQP44t8dd&w2Cz5BX<) z2-_J6nVT3{Il0&n(6ev~u(5KAh%hk-Fo`fQ3bQi_vavBSb8rZAGJWzhGyF3loSY3D Xo!uNwOre=M*ch0gNl8WIM4|s5iNC0k delta 22794 zcmV(`K-0hWvI6|-0kCWr4lhh)ZfA68F(5NEHaRwvzc?9x+q4kJE*6hWt%gWXMCPL_S zgs^Y+>JC$X`>60R!0;2N%w0Bbu{LYXB!uKz*k3Yl(b~D*+_32gc^*gTveEO~TIU?S zB^3OZKyJb(A7`VeAdEuWtDKfnX>ciN@lIg2E!RxFlJcN|%*aRA#FB z^{+<%zxxP#Q51eL;VBg5(M0=+`KWMxqDMI`nF#@8f8$3ss{>fR-LX2cYex=vuTF zg>WSI8M+SL#JvG$-Gtnz1f7p2prz_QgYWj(a~AzFd%=bCw(o0t0r zdQgn6gg)?SEINSqkqlhD4Sk9ucpWzauJTx}Gq)dFB~UY(kM2i%aUH&Z6bmzRV{->k z2=1{4u6iHZfp){U8~qf$hc)4N?(y7l6h&3&B6yxI^gFzd%l2%@Hc~_bmMTy^99fEg zeujR94&fyJIaw-b!XUvCuFAcM0%#zb3cY;_&in-blUxhm>$q3>(YfKs19-WUVjKMu z{Tat_H6D+rkqWYuJjks;GTd<>eCMEc!1ujy^$&0c?jlD|RNDBLO@%}&qm&;CC9&)lHg zMl=!VVFUF3ZuB5L*&cKdy$jzD(MMRoF6@D?iHq@6d^LQp#kb){@zeMj+y!@kIfOsL zpW(0YKkzAn;46|iDJCWGO_CMlO7ea3067TXL*xtcFD}fLa2c+SYv7u=rO>NQ+*bJR z;{MFX_=9{7@H|M^COjlOE&ND$MK~^M()BWuUqAg=Pi4;sS(LpwyDhsT+m-t>3c?u1 z01HK^0easG-vuz<+koPpM{i(%4R9C3mH0e-31H?5yZ|r9YXFls;`{LvtnWX;F9DX` z!GD83yGerexj(5R!^wE~UQXJ`a>fUJdJNA z*WfNvDy$LDA?M(6=s2GST)#>lA}7f?+*mvYPeu#KK>KQ9fPWS?4g4?AG5#fZw%@^( z*N7Uvmi$fB&<>19J>2tum)rn8!@Y*y<37X^{|Ndcui`L1MxNp(02Ta{KTnvBin#~S zPq^jy8nlayLP$L&Zvi?Uho1$xFa;07UvoLck#RspL%6@78_+`XU+5U{@y+NSJcpl$ zZbvnE9r_eK30zSjEDId>*Q*Iqx@oDrix&u9pZ^~YcmZ3u6oDc9N!f0|(7@g}! zHj{VBWU}q-@da$Ba0Go0-=DzFdBTfmGye{nj2d%Wa&G~3mxF}856wasqa*N?U&0+O z^QPLH=0`p&-)>on0qQ$gjF;@w+M|#FQF$S0kukh85rYE{0Dg6t5F-7lUD7HJ*2F!#Tt1hYqQ$tr;|MKz09qRhi0)^0IWQG+9z? z78MpG;<0EX90~^fK126<+?q>OWJwfwj$l+ZDml8v>`b?G^6BJ-7xtt3$yTUn?Niax zX+qWLv)goknk}rEd3H++TF(7eOUrI)^|r*i*?=1QRhgrbX6J#CNwXWzm^dBa+eRjv z%+6yB9?RgZ40Z!tTns0eqayQ1nw_}C9Mw5`_597FT1LXiy461$DrPM-$dYHqLEw|Psqj%KxFv^mK+tuv>0a;;7D z9!3W4JF+u;)se`VUAVY!#Pm&lkH)#pqay7l-QB!-liB(3#OZyH7Spn+39bR>lGNyy z&7{CHV{OD&lk+z(Xn~=LZSF*q z))w!G#jHKK!zeb&+&pD^vbeJ`o@{Cznb;mcnBoYgr8Mx?z{ z8PT#?A4cox`JF;aPnw(mfq(*%$G$kbrZrz9ru2Ux3Q{`hB{ewiz@3>)XJsWN5orXB zDfIC?wqMt;YIQeBCYR|ZYyf>|0^qi_X;?L&zqptV@U7hzngu(Z>nBdP_f0e_z5`i* z)tRPF(n62ybB+Y3(j)7gBfY1$B!QBFCz=YJnS#IW+`ggo*A(xx0A(X+1E4>%YHj2@+1(hiI@zhiXm&TT>vX4%Kpk(_S*(tJsEipgWqM!g zVJz9y52%q~reF&QGo#! z?{Pol?e-q^eCpLbnr0f_fMIwIucr7w8;*rk5oS-fCPWk^6plp;!?edP*YK$nu!1g^ zI5iwb#U+I>lqeGMdOdPscP=c83uf9sr2foIeE_0k<2*#M8^I!H;nCpO@rQilI}*2M2^7*H8$w*ZEWai z@YPpmGCi@esl`U_7y;mP*_=Z7M6J8FL zdypshffb9^8zs?tAN)L4qFyfvz^@2?!TS6~nre*VK_O6GL@5*wh5bpcKPgKmlLk}| z9$Z(OP8L5xHt&D^s^7jbwtVU(xf8EUU2mlYI+LRpq1#PM_-Ri)5{WQh)N zweG8xrf?UVs_7y;q zj?l5_aYmv+?`aq~05B9R&i@|eOHV(@Wlq1v-6-sRF5CEj68gZ3 zimcxfD@BR+UScKg&i#wgDD>uEmO&v=ppPZ75)Pm0q5vE|Wf>F_NFk_@h^dlFH(9yU zfSTC?<$MTflG5#s&9t&wcZExc?)$PlXb|Kmco&n)XiT zr^~;Q`4Ao0kUv;k%bz2U<}a34d!H1JdL<1Jqno_ALsS9|2L~Ju2H@mw`t*S`>CSz@ z#u$LFEEjzYqSL1$l#BaQrX$T}2%Dh^A<`0B7Fr*F;zDkQcsC;(HzON&+ElShyP`r@ zR7O6kLq4iQK58#Jt3183>aerg%g$^oL(ZeGq%IM(LoV~q`rTTwHv zF#?27mXwk@1He$IX3$`RZ+T@-_SBod%YL)$l?$G`=I!0W-qYLP&z^qlcI^I)8-IGo z%e!WOy@CUP)dxVv7Xelk{L*eNLZ3^9f!;*{pUZh5IiiKgRE|ef=iB ze`eD)`?rg9a@~Wt+~F5AqgFPd06Ez-a6l%5GawhcWNUOiJomocqw8gBkPQy1mq0=S zQGKvG3g93cq_s%~TCQZhRyCIFO0L!g)L34bOF&4R&FQu^T;r(F_g(3 zx54QD%PtDQ(0pl?Q%JLi%Byh-!bDbF9waM7b%}HkTslQOP|P>G*@UkHsz2$<56{=m z@SL`X2PUA*0akzj*tbtVbZDOeb5SOLV@m=Qw~=fWNhTdWk(C@Pc~%OnlqnS@8INkL zg-FMkcxVYUM;VKi672!gR_O2-Q7A27%~XB0URDSihp-2x2^r=K`rvu){L<#m7s)i_ zLpqsexe>b&5*=c)#T=&3a-#YKn`avv8tjK@?jzUOKxN|Ab%=Q70Ex@|YHg!`_G>_; zc9C|Gm#g4Y?kdl8?lOM0dyQw4TXqpau6Gaij3;Bbk&-2kbr1Kb_mca#ZPGURY3?aW z^bxPeGe95#K_If`b`KC_0A+2GcM`TRAu{#U1CMz;IvuAL-+CYM?IlkmHy*e{Fy(GM z&V$-G1!as**(&XA|U#iPU3uH`mtl11jb{V`bX$y-~cqH z(=~ZaXCdgZKSf&Z6j15gh~(ZLI&^3g9@DA8v2r-J2f2YcJSweanu$Aq_vY^6dQVkx zJ?Nru?jBO_88n1}yZS+GzdRQ;tytcSmN(<(CYyzdaVR`^2rf2~1_n;L7nkD8285z@ z_zEn%n4R`~b~-TCS9e}G;RoDlV64|p)$xZuo)lt3!;ZN` zc4Ic2f-N&W?D`Cr7>qq$-JbEf@_|-SH*q$ zaor^gO?VoapwD-8>VMb%?*6++;WggPdpMV>2t2R3J+dfC8o;uDs7YXU&?ojXp=?ST z%pDrcBD5|@>o}9w0&td62sB(Ma$+}GW+_P4KC?iHBYUw6VeGPenu*#ZZqfw)AparH zZMC%(%QZpUCw-`KTQ#iFecdY^l*o0`dWlHi^S=EK2>KJv%cJmvzsND02#)~`HpGrK z9s#0+KT3NU(1Iy{@zE_dK=4z!ukYXQ*}s32U~hq`&k$u7@8x7^Z|*okxvz&Zssx(K z{12ZbuE9yJnDZBN=`vB`NX_rb^!I<%^Mgm;#s7WZ=#oT@u=ktM_@(SfG6QehbLDNf zQvG%tXn&PJUjn+`(lFt)F{HQk*DQjFFq6wzrv zqpXav(oO9r+HK7NTn>VaaZ611SecJ3twggQF=_?oOAI zQm!mD*jt+%nHp6+!^OnNpB53zF4uXdnV>= zwNJ@^QxiaGRDmnHbDwv4JY;GG?RF8pW<_jJu}04+W<*;o)5d@Soc2*Akv8@@)avV% z9KI=(sG|dC+Xhzi*dsujz|K&PqXgT`)Z-J!7&7#p=FAaVoS;|$JC`yv1`u>L)V%x* zPvX8iY^wGTDI_(vby}7#Ela2CY6lOl0V_6tETz-Q5-}JEhxjlvYDG}J(=%UiU-9c} zmOeXq!pw8Bizc?uyY{Q^KlZPU!d~xl&vZUgKNP<^ef?D%PyO&$*}vb1-_e)ccIohy zBS+0khFdd39&20r^EvIWZ}8lD`-aQL*VHU5KWEqKRR>pgd`3CzWhgcP=X1EkiV~ZD z%~?s9#mD_8yN@u7kNXb0kBJ8iHXi{Z+P8D)xwULtQ)_Sg4Y0T674|lnvbP0=_BIk@ zTdUHo*P6ms;dudU0g&!@podW>;;WH`CZG?|aU}RmsN9M;)|@H(D3TXPUpV6EOGg}? zv~+uFVEWag{M&t$bLxWF0ZJ@P)XP_YH1zb!S86_Hq z;Hc#@i|_~;2p?t>Lof6qIqcYlVTWqNv>PQ9J>j&e;3A9mO<^KbQE7*j2&a!<5`DbMz^-6-R{=V>kCi0*-4q0PjUL4Q^fp|C`=XsqExWcYD|4ewd=T#3E z4oBj|EU=96{_$ZeHeHymT&hnsuJF$YUlExWyEJ}l_&%aX3pvhL=u(2tpaq>l3(`Sj zG#yNv5|*eeVoDL6J?Icf4wV6LoMB#~;)v22Ys?&&RO4!7wnA)u0WR==G8Xf)q4lzh zd((7GWn0l=yd?Iq1fq%AGkdLeTL_KIJK%aNm`tKZ`*PF_*1TUw#e;Z?c4oW}(NWDH zWYiK6FK9Nt84rF9kN#0tcK6E%vwNTZH7evpav6eTV!OFUCK7CHusmKFmJ6 z>(_Y3&$9o_9>ldcz7xBDzL)(N`Y8hKj|SS;2(dumQ`*#6$wYy>*9Al&TVvE%iEZK> z-fKaF^2z71d77bBrY-U!{wpiYWRRCl+e{K^-yx4|O;#Dk@#nxvpYHU9YvC%NS{?xS zC=?tm6zcJf6w^chWt{^6Uk-&Y%~!~kYPE--kLQc?T_1?Nz;m2`C`*bcDk7(-8s$z? zbp^nX0ku)#C_RK|EoTxOAOwq=OT-*O*wsy<78pDVM?m~}x=F-RG-Z;du2;eA!@B@g zF3>NzN#n^Kgb-SzV2lC|BU>)U$y%O`KgzQ)iR^ZJUMX&Q8uZ4Kw)5abGt55r<`e4U z0!!xvvkI}VeiNvFnKI~?0#h_8w27*hIuv6%!=P9usA4H=ipKBFoj_df1ZJ8gowP9X z#wtolv|ffE7zQ6~k5X%<>C9yIZz^Z80XiqMY-+GU&gpsW3tT*5)bPu3;-j7y$YO46 zcJ#V+E4Sk3Pw(vc9-VWd2uZ7{-=D18gS@${O$hen<8df|#M_F82py6@76ey@D5m*f?y8Xd=?nP9C@tBn*!YGZ;^gelr(!8yVl zZDFuO=+Lfz4tj+kHT``ulD$NxUjIh=p(EAI^#8b3$njuci;^r0Dm<>@_ISNo!0+?X z+_wlwpN5?RikNh(`3$6M2?*^wy*7d_6DeElLzb{ zC>r)VP~QSP7Dx92@jvs1GK3mO4k;}y;5I|#QJ z*Z6IJvi195;|IeDQ=19n2eViI>ci65P!)$i|HJrXqTeUK$S!#?`&yY44rG5L>^M-;Iyo&l zH@Q-|Q4!l?tAu6BO4kj-4KA@Pq;Qe4%EC~8fkG#iytaGP5rs#sIMa3&zpt>evZ4an znnqD!A&gwanY1F#q(u>Axmf@xEoxN0h~2qQtP~3*`B)T5WDz8p`M;Q=F9ujNX-dku zTFSW^HBT%*y;>@*CFs>a2XNcrH>-?ZL5o#Ee^?lc@nIpG(+dADf*s$Eab&(jQDG{7 zxb2~K+}CI6Ecti`jXz}!N_JEkr!yj4u1}B%O3*}b@gQ3f0X+nc4LOh4@HUcu`n8pF z=iPM2rR#sbCHp;m?uMZkj~RXagV{gg#h0f?%osN1?k(BpguP9B+Ae>xrtGEl^R~AP zqRi(I<%BVY5S>dj%talF% z4v7z|yr{C--CViA-Co%;V6%H;#r>fl#GY{n%k9AzsU>*Sic;vw=(FX!qc4{4j~*=l zL-74_d1MF|(jhP?@_fECE|oeOTo_NGqHs|pQ&m}8&(~L7#9vr7O>WA}mD@9atF=wq zZ?u28|CKR@)Os+lSC`g?2Nee*S5z#mAQg#fPow7!&qJP^M|jBdyytHo&eQ8O@@RA% zJyf33k);yWD%N!o4$!glh+Z$0X-cmi=JEG<5?r{OJZrflRg60C4kQv1>g_rjRi+L~ zaIT70y|u3=@M}j^IBj{DP9ZXXx=9t6QVlO^EC{tXK{hTWbJ_Zef8ww zdR!l-53rD4I?UW^;Z&rgn%SAvZ0f0ItWaJ0vUpG=MPj2!!~k=u1!x<8(RRyu7)!D# zUyLx8)M)=j<_8u%j3sr(o&$&W1`{YtEVnJuOhyOm@+8yWdt*9`T{0hkOjW`onZ{#1 znIi@Z%l4kT+&1FtnE_8{em1pJ*AiNuqBbY9NQba*U2Pe)Nu{#$2s2Sb!C)X1PNq3g z@(=)Ps7hbQHO$$w;Q5z-E?9YC-NN_g;hIr5U$?fPGqU8+EjK?qL07^hFD1gW_Ai|| zXmR`eN7Ds2Odb8Bo5pPz7x1`ar73kuzjK?GN0#3@#%jH||C-~cZaQ};e!o1SmyfN! zu;sGx=Uz#<>1I$K8mO&`B>WUutLmBQKkuki^V;EQhlFfEZ~&fv(ozA_7no|nu~KBE zsX=e6I{j`nK+_h_eb$qXWC~`QGi}0o#@B~7> zgdkxtaw?i{DrVy+0yMTJm=2n|jZn7sqK5 zn28Rd!-z-Y!urX+&L(@_Jkh{pMDy9Bbc|UNECktj%%IvDv-v}!p{TMdfsL=H4bcku+*;r&aISGy_~UbylpK7G&gqn3>w zbWP7nvT@1c^Y41S=cW9}i)k47B>89$^5=(5cgD}>0EPn;I82mqfVcz19H77ng(Msx z?f@|d&>YRi?EoGJ@H&9sPCLl=hlQyfz;FP+Lvy+V_#D7+0JmfM%ML&dP-`p@bc#R1 zA5s2)9RAo8-V#olBrKarB@#Ckj!PCM#9#tO9q2n5i|Xp36yBP8I7L#RoO@DR4Q%j~ zUW^ExHViiVQK~TllxhsBDugNJ7=%)ep;0O}*t};rN|xb7V~lRxY!yc2J}E;?h(%Ic z<2cT45bwP~oZTQ!W3>joL7b6hT%|XF+N{lgCUK44D(+0Nae9YxL~4@k@=3-ENyY`q z6h4ITR`f6;MW_*t2boT4X%a%lJ55~J7)O`M6GF%abxvk`Z0qhF!}29tlEFI8) zj5zkWgL#(Nz_iQnGK=#c#wwIo!nEkiEffNP^35ZIo?Ni{o}z1i`{1)XlQYj-_WiEu zb1vC1j8ES^?uuE{_ddV7r;Pk?(G|n)e!S-%vSZDf3HRUG^DfMYBSF6`15R?ID1Lc& zFhcR*r;TW2(y2X#>gwd$jigX%cdg6 zGbqI9%7WWb@+8zJ!yI#(69+{DW3@x_v*Ve793?7!d8YYfrnyg)uCI5FMueGAEt zZ7q`elTvs8&@fWx9z#aCM+YuST&hlU&vSn)ej54)pYZ57$a!447sV4U$v|olH2R1m z4k_>glQcDS`upkyhQpNMbsF?~WxUkySH>JYDP2EgVBN5c7Gu4^7cmO5z2=6Gi9u#U zFi|KnG014jV97sB0D72z0CW;9`iMTD&K4%qJA8~#d~9faj8J@~5(^#DtkdCBUzQF^ zA4)liFOnLi@j!HijAEItE)_BwX1cn>j2|h+Xf;|`JE2dO6`gsR+1k@bFf=c3V1XsQ zC)02wpL0cj%yMPUI7%zUbyOJ#ZQRFX2xwBc4`pye+xB1g)~W@6Z{EUz_iRTl~H=~K~_22&d)%RZCPqpYVATtU%=fPFP2gmVALttv9hf-ZH#T;DVxn(m zbW)<#w>Z|CSR<}~37#Y;BRVK{UUxV=Aw<*KxKP5oRex9~y3WTFs)Y8EXAuT-iw+}; z1hS#hF~)cM6TB;Iv;En@ldqX67Peq+{S$K`xNXls*o7-(^R3}aZj*uXJSK2 zun`Zj(T8y#a&~rI#-4eg4rt&c)4&{WJqpoS#v|r@&=|+`y77-wZ>N9Fp~v_ z?Yj?1#Vq>{7pIv)#$CR*>dQT!W&ef)e|igh@ad!Kj+3NS#)P_#GW!*D)TKKUp zT!cBS;qvT%2ibq==JR{!R6_JYSdGk-XDTz*nXZ}IO!oqLfwDkd;98)6EpT_GyUM&Ya9Ub1xO9fvC`i%Bi+2iFq)4xoI${j7N!~v2HQ0f5r=M|kt5S@n- zoyQcZn)!j{E3BU(m!&k7kD2Kp@9JL=quz~@XcZGs(Z=Zb=oQiDqX(m+H(C^38vQWJ z7e()Xh?3|}VX_GV6EfPj0<^hKlc{uk2or>LOeinx41{VKPv{<_7UTXi3lDuylwzYHO1)8&o*6aiSyAR& zh%)M>M`7&ulFN{k`)W5cs7ov1Lc0?6hbnP@CA}v-ztTxbue3dtA_YEYD6FLZ5_*-& z80$eXaAM1#eS=8jp!I{uAj;mQC}OL3Y_>IRn)6w$t!j0(+CFVhjctnXfv zHuh4JC{4TIWyyG6CVRas#N;jc5MioO5)>Vn=f>vcWBWP`GdgTnjB^|(napy^lYNwb z!DFBW(uR%4mNT~rXpk(@PdB#XB=0u?rO+ztSC|w6RcS-_>3*FPOWbB0Ddkce3;m!d z48U%&CmBa2Nw+3fsBv6grl?|ukE0^JfU1!h>M*wprbbp~Hf-2{`b-qm!frlOK^ysp zgzRacEM3;0)Ya0c>bDXI;9E#+bt2t=xWjwP)$7*OrM~y-edEs`T6yQ>YkoSz=+suW zuUilbRmX39`JQR*uU>QTU3_k0;fl7A=O!bmK^JWpcfs27qRfR?&x=f&IcZ2TQQ%ig zYtCOcbH+oL{ut%sfG_ZAfgk1vGVLT?n?*gfLfsMjv;{ z=XS7AXJaUedH?q}j+K@V7TQDXa}LP82P83X;N@e6b~s390V0vlJJC;{@ei!a!ln>uVy zrkKy#ezQw+6Z>i|3K^zmAK2G_<*_|D`>bjT85Y}D4Gtqslte6gRiwH#=FHa&On8+y z)VwYDHDfFT==Z<@{p|z#n=BnRAASM_;|wvyt&lf>EBOkQTx?us+-`96d02|-VaF?U z*ii?LTS`%Jt)3_-qcPRv)(b_YwY;b)ele~@eFBeo(WSUNvQI~TE+8d;Accr{hzL&d4dkQ~Io_0N{?Lxc7y`Eq5Z;S6LNBN`PPkkrE zZB#M{;oI?209 z3RhA9yK%rTsxBj~W{fHPBz2~-$XI7=HdI69IfUtO*aP$JNMCj4M75nMq#vPQTaLhA z+zN0k0w@WJs>(pls%}ulc+5@#`9K_AWX)B*9`hH5B%6}q^JN5oDIf@v2gW(&_5|E+ zj|@6+MwJ6_It>e^?64prSn~0**U&s});*tF)9A_-8ngC!y&exz11EJiZgDSjuXl6q zZv2#`n&VZxRK1QytjSbM8E@dF#&rf=>@d}G=>l#Mmf6X;_^Dm^r2phxCSjvvPc%12 zKtjS_EZPI`cZURjZ5@Y(1AYIih@j-r8|aV1^f#unX!7(fx2c)rrQBhdMGqrS?obyR z;5C7d53@)%i)N4MtewoV&JS&u24GfEJb6rK4T}}Zxx?Ef)2{X9Q!VLoqeHu4et~O) zBsjD~8bGhO0}Um6?K|~ey!X5?JI}}+-l>|riH7FGjrqlYOK)uSr-0fTS%pNXQ= zX~a$gW4Xa+%k}^Cp}|TPr1Xbbq>?M+@R;n2d!K3KYo6KjP~Ew^pU-x^_)Nt+e7ffc zM~vT+B|Z1Pc7V)1^&VNb>-0g8)up+wNTqNe3ge`Q4lC&{CfQ7zIkX}z>n z;t`nd4@;fWKIxDovILrZCd~<^SV&aqrv{zmNHNKd1~K3prYBIx08N+>nd}$sd>p2P z?v$uM{+5#qjtf|p|(`Rv7Ds0sT-T2kr^8bE4{Yi!Ae6uGeuanp6k5W>%uh2Z5iSa?*n5x0tONG1SOfFM6+#9 zh78+(d%^-c2i|kzGjPQ>LPxem^xA|bc?#}4x{5p zL0+#5o#|r&x_*`!?_;xUDfPvG@GXh6RbD55obbw*)&}MUD0O9uKwN=tXn8Wjr(%nR zQY;pY8O#DY78AV|9d>*n9c&zk#pvvP4hGftRjrwZ6`3U^6WKZ0@yeZFr zzK+qBx7ZzhjB0CK!}fx|{p7vuKUaKq%X5D!dOmvHjGLc*{Kf^h9&>^HGwcY*pIbdr$}%F&M7pXn2=-t5SR&H;UWq=2=xR9IXw@hGxKo zf{`}?^Mp&)91n};Skg=}t7Q68N@0P235#-ASXdw4{kyR?A#>uFFWxi{5AG` z`CiYdLrt5vz4+5?QI;lU+yv^ut01Ec^no?D+K+V}C;3`_1V5Rd%XjdiV#tcDxc!FW zMx2aY2^x|>s#3mH#&U`2$9_^`urV{(C>m@OjsHA;U-vH7vW!0CC$jN>JIk|g522m- zDBDBmyI}u!4I!}Ki4{kvrgh9v(n8@FY7K>AB41IaQU!VrOoQq3Dy*|+;`YoT=bXl6| zW!AelTAhi#AB!A_MRmGiPt26~_)c$v3f&K^#fkdzz%=jkDrdPZFY%h?18Q|zNSfjc zxg)+ZSD9Ak9;^*^*Lm(UT;;xU|AnC@Uz5Kn*zRlhw+GjXtKDmVjjIAz1#fb1Hn#Y- z_-_f^t3K^|Nq^DU8~9xPH1H31kN&SfE>Y+x&mq4n5$C-ly*I)%5$)~1JtDr|9Je7} zucm|Msv<5L@cUCfH2^za4OG*VOAWYOs-LE>xI}sZl+Y8TI`MLXB)UoCE-&E13Urex zmaEZc`N$Q%mwm*4*Nuno_TmyWDz4H)3|p3|4baAG+ypJB5e-`GtfmtZY3z!d>p&R* z^kt=r;_tpFYxes;^saH#e)O+Ys*I>PoU##ALME$^MIf#wsrp#F%l<}9@ zc@#rfK3d4mM+0(&spPuAIaLi8hK;n~$}WE8{Y*(w<}Y2@Mdz0eSU0UUJMS62yfnVh zTfmq1+_!4Oy47Uisb4=oylFDE&!5eHfNwwtkc!6bQaL33NbJTFtTg5tzy?rp15ybG zJ4hTV4I7Vtu0TuCbuhOJ=wa6*_p&ujOlP7&Vp{6Q^qymwd8cdYYG^K1+2FxLb|08< z>7e?-+<^njZ%vPlwq6GJotFEA4+;A~PIV8KRMm=LVS+ELNC*|~q+5%C>gh9CXwZ~@((IbhJY7wfdmgB!U*xNAY^1R} zn4^!qs=w;1*Y{@z(I3@_$_01GJ<7e&&5trJHCD&DNufphg20^6D)-vJM)&5xE%7JZ zs$jDD)J0!J$V(XR3o6j#JLx+zUc~g>ac*1(4-@1gd&%P{O6FS%^iqIsx_yp<5y0u@V8xP$P zeFN{sLut~Z<$`u5qVMY1TGfs3+I}mWCNjrPz&znhX(ycQe$ZWdSS}DjG?BMS(a zx}CAD#q>ItKx9fo&V(E2c-QnvqeGx3PwOgwx_jYu&p&!i%_RY!Yi0Mw1?^h`UB#dO zWX*3E&Yg4p*6h)@f1blPMDE+vdHuRa0uPck*UY~D#v9FDznZsW&J_>zFZ}84`?CM| zghnPdW!w2;n3H^{5O1?OG`*jGu0BTR8_iCW6qyxTvS3hfP{HtmW#(2>9u^)Jzc_q< zalA>sOq&^=8DAhT)Y|pM;f3*i<{N?cBk#xFC_EB4Qh3BzJxn?*582gE+6Ek}zk7!tvKqZ4*$oGnEMM(;UO zYZD7l3!PZ=(I{w(miwJ(HjF-@R#KV4^@&GqdVJVj^KU-1VAY3L&$y$%@#N}%H9vZ) zW99a2yYSP^6DMxT-TPSf)UB5c>p8_eeqjG=Z@u>0chFh7P@AQdlX9IXst$avP;E$k z2p%2RbfJg1%RMwIl+QV|*L-{II28lFu)L~DUBX2k;`y;JuP@OD`j*b*Pl9${mWFQ) zO<>VU&jz6Sl5@+h+VIkhu?Mq%6Y*jE=a=?u+dShBr+VJ|GW%6lhF;zPR^4IxUQT?& z9u%WtCQ!wR=?~GD#vHezzCdlpk4t5LNW=aROoFNbRxzpxISP2lA+AtwhEgaM2~!O` z#_PP%*HTNl)}BaNnxgMhkNzyFiW}!+Zn$w+PN^^7sla#aWN`Q%c$BN zJ02s;Vh_hUW4Rbl4L`S|eAp@pm;t>?n}U&IDuayo6@VaHTaW1k?4I zx~(vETNxKUOWn>3pnp$5vK2Tgks2Fp#f_=jF<$q$y>6NiPZu6>qRwk^JPEwqGf6JR*EoxlUIW&%r&li>+;9O>#i=Bv1H=J+t2BGpzFfLRplkd2h)Sr5fjwle{5VrPhtmW>TO?XxYVSc?^pM1fyas+7#>)9EBXJrA}eq({< z{IT_ILx$Sx+-mFW?SO&yw!~fsXQhCuyh4%ikno|vkB8#8z!fonpIc7gL8FEl+g7sa zm6);}tf{Mg2;qHTk(2M#xWCEg)$~m*W9ffL&MV!{>r`?%ClNcJ1c=7*XIoFU_sY%# zWQ&bf{JXD;+C(>Wv1M|A;(Ze|TWM>t1(QFvK=QTmnqn}l?c)}&2;@hsHlc&_qY<-f)ElJDc# z$MNGa?Pb>seiGLcdVyZ3i$BX92P_^&GHex)JF!AlmqqcniC7?!h{=fMY|8Y;@zuF8TzBc6lczn(EAMsw0xv8)8_#gSs;O1`He3KJbfV1JIL>hgzDktC3tm zmXhlTPxg|3QdETR$ZHx*&uGw3oJqw$##D>2ty|ESKY8-Y-C-V~-c#mCr8CNkCR-0q z1&h-|2Im(Bp3yUC>q_x_y6tb`>wwJ0ldfm_3z&}dEry9JUX-Rx#!Mr zn7!}X{U6U=_`L@<`wzbR`E%2sed+O=TL)6w^MhJ{u^wiYFs`r)0}2L1Dmox)MVCc? zpgrJz#x2L(aBu*IZ`Eqsh2$;543}E$XS3T(`X+PizJj6 zlP4UZaNJ@pW1{!*iG~Fksj`Sg6$y$gU?P;Vcl=am-anwff551zq4p-FuJ{Xe0!Af9 zJq^KGZlhia`4Y71KJe@&__p{nQz=BD1rNAVfs7yl%?=x0ZNQbm!> z7kjcx_;k;W^OjCoO>VZc$1QF)c?&o@f;L(gyRb`*~l+!DA4!Yve(|}H%;UQ)x9|kA41X<(Mwe}|vw6mz z=s7}VTXREwHH~9A!i}!`&o7{V0GlZc3Z*Cw9ulT+qLb)Sva-QL(pLY6F7)F&xxl=e z`d@d|xl7j!8-MZ8)g6O2@Xy^ov|`uD*>~4g-CpUbyLsyPn{T^#>K*-~)E3$U54jOI ziM~Z~sAcj35+#L*4LsL?MP4Nh)if`Wdh#BT^S1i(V^n3=k2yiMfdg27@;&_a95`@* zYdUb?^iu~8z_mB#j`H-KTskVizp<{yg61t1>V#2(&{))2M2d<^5;ck837YjM4)fC# zz)M1x#G2)1_jGS_=LZPx}V0TC1d6@)er~UF8x{r3^U7y~EVn3wQr!1BG)m$9VR_fyQ7d{>(iVk27KB5=9Y%mwB z%g2W^M%3-TTeC!|gYTuXy2xQ$J(8ICAk1f5?)FaCL&nTsKR`i1BX*yMgdo_I)yNzd z>%&h#S8QB9x8uyT#HW)Hz;T+`CNU+G9eOJhd-!H+Ommz8KK zVrg5hX?5Vr$(=+cp!IdLavGadaI$eUjlDVtPudecbPLO|`PbKCym{EXHRkY==+tSM zyz9lLaR*0N6XlgPt}j4(**9M4?LRVY#;8HuOvmfr1wC?q*vD?#5o3E@aEm3f?pFrY z6H|iiJltt>;<`Y9+-Oc`<&`p_0pE79eSn(`o*uTsep3z(oA9c^-pA??YGEJ z$EFxov9V0C`N~HS-={l!%|%bQ6$wILCmr#@&q!Q-^i`(eES3)LsvdPYVx69WzGCpQ z%}wQbzHXo+4%OdQv-|ekah=TjYX{^IPxYXdR>jO0ojXJCr5t z=%qp_E_;~d0u})@q(?ekU$7Q#m){o_zd3qpkGr-n6q+5JzZtqOVUgdW9ynI%U;QNH z#SyoZpGs|CpE2W)J-)X~ew>(xf^SQMzW1C|oV1#PJ}=dpbYa_Tx&}K+&X;lWIIG#s zmSU5;hj*5rG3qCDiqgS6b4!Ckt8x`evKzz&SD>h2QJ`!y_VSnQ*a)OXp%dQ36r54u zx{Gb~VbJe;x3>y2q3d{8xm{bNM^{vqbn~&o%Cj>axNjMZufQRv;t>;>5}L8fy*S~- z0)zbS)Od6DBNx}aA=hX&Sbvq+LpA><*R_2rV_BE&yw9cni;cd1P8AkLQZ@fF z#2lbgewA%(^-cQzijF+`Ax!XAmE!DA!SGJc7e%$zP+Q89`^hpERWa4>%Kjl4wO6+9 zYgH~7-?F{4wX%HeCuHO?b<6^P=?c^?9=u=mT41uT$tjEwgE$?-=pSYBPObEFYHPmB zsCcF-iFD@T;tE*|nmj9nZ?4C+$1fk4jwb-T0ht{eHUY!Sw?`6qEEbc!36oK@!-rEH zQz|<09(#*BmRaeWX12vAV@h@o-$n(!QUY;_5UT`N#=}~A1Lo4qC>~u6c6Q}wxtYv0 zmHfLZeVvpH8xs3>VMS|M*$&IpH-gYI@8VY{gqt>+AyLXCaNViqhfHpCM?rQcjD}ZJR-^AXzA7w! z=lqC|Pd1%_PXkO1el!b)_sE(iwR;1kYf{^fh*RtIny__sgTtis`;wLmPQxpaS=k<0 zh6=|;2d0tevQQ7|hbJ$w+w@9nvGXhsImiM;k zZq6v}o9IT#25jeK_=yBizdn`V!O+a}ewj$*uBDqVL(z#@NUWF1ip#GUrgUHtK>7a! z+n~pRv>~iDhf-w}oKX7^ulzihBRBnCj9kts*)7a(xXkQ*ZSpSZ#H$}BU!I#y5|H94 z#6c2q-|0%%1An3I8JV&bx2Hy);0yE!I+TQ2rT7N}mrofeXt7^R0wURPLkHE|$iG^#sdA zK8zM!OrfS>F`ApN6#CLeLLddXu$8w zW{*r^^L>uBS`MLGVmiM%df~B#qk3_Qic-)lV{H*Zwlev$H|v7yi2xekxw`f~v7$H> zjbk3Mav``&K`V))nM7Ak!3ug=+Crm=x9w#5uHwMD#|I=;Ul#(nKCUeoFwZ2%2zv{+ zN>Cq7Zn!g-;^VfC#CRe~k%x6pZ7sy9N9+kqlVj)_?nq|EeEw>nIV;-fbN(^lb~MB8 z-JJcm54Qp`WM-ui)WFfCtv54$68nqczWWVbFluG{0`~ysG!5C72h`m5sXw%teb>r+ z8Qg6~y}UD1y(Jw$iKMYB^Z^caW2HbXkgcGMGx?Ykeb`6ng z^2cx9wCFBuhhBo~Mn{k@PddSWOs&hUSJsD@{vyzCjy~3Ei|FB7g1fLr3hDxak>b~u z-Rr$+&_*`$XqLW&Xa{ z8Rk*Di{`XFyO$lKOE^Q}gXx^Fli(e5YrIFABVYR#3IMdTJ!mp%ylS1sfb^a-qV-k+ z80obVOBYgu%^w&0V96r}V1k<8a5w-_W@JIz!tPQ1RV6n$APH+oqwuC;gKZk@sHOjaklb4XwFpTnMNCJnog~`!*OW;gZf@kwGM$ zH}>6PZB1#;EjD>Slr$*6DVNI6s2>82nL2OgmzvOjSKw)HMrfqO`Q{LJ@!bi0ivO`| zS0hp8x2p$9@EVvd%-avVviTjrFV}kgesLq@=9z)W0*kgC8%45eq_9Mv2RiLQwVj?4w^cs`BS|-Le!KBsN zVQ|UDW#d^1cN(wB)Y4R^w<2(Kj*J^s+&kLvtAcSfRy!NLZgo+^AL#)N&3suiLTyArjX;<67KZzy;tVdznQJ+tjZSbnrmT3V+nNC zUQ6`NHTw2^xo7#J$NATP&Z!pm57AZYHGwnj67ofxLwvIrCvqCRbs9KQB9~xtait*4KZIFzHKrKlw1Nv#~nYxKV+2u^kNFn*CIwEHau)j@R*ulsnW~jWw&Dc~B0BuvU}nSZlb@z{B6X-e(r)lO;c6e9pgy zET7~&o~x5IU**dOHUpM7PunqhLHpyrIiH5LvnEo?HbcqUe`^}WTgove6s8x>%m?tP zGt1QCB418WUpvNvF9(DOU}qGm1n$b}CH2X0M8`B-%v@C$K*(zmairPVQ}R_ru3GQW zmJzCH8AjOnGf^HSv^awYMCu^9T5$`tncr*B_jo#py zZ@#_&cfTumjjAA*tTub~{b`aAS@-Ecq)j@eaSZL=*n0d}h-YUdpy8}ex06`-e*IR? zj(j;1cyY3Dz97*){@!;*fH1{u0#EzZu9= zUfng|OggBh46@8-yI|}LWEC8e`T|WNGA@=s?AwnLqsr*E0g)M##X@Mho;h}dAlFBa zo@ujjHkAfi?UZmlN$+CBDy&-*4iVkdVBHB9Dj>T}G!TBjXDv_a3oMC9g8qgKSF_n- z!t3;p;Hzk12^#u-iLwQppvGu0+qW$+@ZHP%{7y{MNTJKem6m3hXdsSV{w%@*wWGtf zlBu7m9j$oS{FVd6c_w)9L`mCI(NLZzOc6U?1PwA{+>3sv~Gw3`#OR=EE}~6@K#SN`3^gOczU>U7>PMrDv&UKjL?k; zKCIhKutb(KoOJCUiKQ>D)Am zM@ZcRH!!p^9}F(kLK`InS-5S4zp6!5krh2=oFsZdh~yb`+zp+Lk*i4pgfqh&P@>ZK zr|n#{Yb}mj>Q;n~##?R2-4VAh2U^)~F@%QQXZU@pXYNr5oFVm%w=iNvag-tE#Pke- zc#lh;Ea-Bp*{3&RsGbE^=^V2U<4B>d7#nAH!>LIMxo*mo1%Aqfn-V}9Ag zNb`B~FILilpq=nXlSDbB^kMh;b4bFHdwOZWapAKJq@M?ro#{mZg#f`yp^Elc_}9#R%Qe@i+XMQA7uxQi+Qv6*D}z&T2Ca ztDUWrXP77*d4TbmTI8gtqG4+O{n%0L^`g4@!oDU?f?2VqPQ~0(C;w_iY~GxSq?Z&u`$Jo%7SpvEaW3*R8dUc18Jg1x1O+HOwO z#R?;dHPya6xy#n~^7}k>MbMO~v2fL0hX&ztIke2SD-eUdRVz^L+D{{$U=Y*esx4ba z*k%7?B$-Ft(b?0?-S6$kPx*c^pR+&By3U%c*Lc^WL+bTRA4u}mrr5ky@EulRKQz4H z9j3uw3v@}=JCD1{kj`fjbSHiE)IV1vGuiGrR+zKr=DjhP#TXOo^S&%>ZNlTUnkB!< zK%PkzUNa!#p-6UhZlUa4F2)ThO+NDZVz!1{X@X;=alx-X)!Wy`A+K#*_t1_*{f3hA zM5p#JHu?>i4OLv^{Lt_QRuC-|%-R{T#XCe6O0D~Pw5Fc#p-5|0zAjre1b(P9{1j_) z;l9rKn;h4|CV|}dMDJ#$WNmTqp1n15jO9Q*=K;h){<1IqdirADa{EJFM|0$1i~B$L zMA|{yk$NL^{{8(AANlvJTK0HpjAtv%H@)LH;Rf>?5M-fGn93<=AAxa_Pp03Y5)qXylI%DczNgV}yDs8L{<9}wKSu(6y4l2ywHmFO#Q#BNaneId-zobhG(Eq@m@Q_lyYA%SHMfkms*?T2YO^+P~ zP>m|gx0dGn#>&9s;0W(#Z(mh!#`2cW_OK@6->btaPG<>2^_}Cq*v6M3Q3Fc>R-*o9 z)yZ_dH<(@5Ly-PvHgipPjUkW&5xvJ)u-)~!cWuC~Q^xNZS z>B_ju4jk77>qSxUSB9W%|Tc60Vi7G_lTE-U&JQNyAEBS@XaGd2(+z%e`%7Z(Xb2m-n|W zQ0qFpVaK#dkMQ#-d&qm^tsDG=j)CD>zW7BS32*1F<1fQIC^|Q8DouR}mlxNRvWB z0u_v4B7#AMlDSv_6V#Y%C4@%qL@X+OujmT(^Uq2*G_zou^f(Q#eIlBSVug6A>jq!< zb}|#2G#Lu4&KS>bQ=K}tEl0bKVk|Mc{;(LnhU5txn~?Ky@UFD&aD$0}+Jg{yS>Vv2 zc3WGXyo5_%p&3v8xp;N%puZq|qzQB6lGZhlX2p=Ov4IC1(+Ao{Ai;;v`|M+@DpW>d z8YaBN+h;h&8@F9QmaeGpcpZ4RG0FS00ex5Z^T_{6JfFqCoLhW(@V{VSk%Kp2aUd-@4 z-t!KG(XSwJh6~f9AAVWkU2P1}lAL{*N{;i6mK*1aozy*DsUcS{OusAt;27(}#4)N~ z7*gUjkIRNJMoPSdJgW4@I#iFp>Qs0`Uu= z29h7&g#SqaYub3Z`FL8{c!Bs~s5>bk{})`IVj;pW_#ZwQH=j!yC|J(H)9aE>=r1A_ zi$C^Xq?f(-pZgF(siwkFl&KGy|1fBR)olF#75Bfx;Fn=WRR0Kh;%Vc1NsBs5d4MuS zUnhj2T+sX!m*?=$!l*k z?`dO80RcfF6p()}kdUCDfFQ^g^bZV*5aj1a9j3uapa>yB7>Y0b(h`OU3!)s;9{^Ce z02~VXD;MPYuMD_=FzmlDLBxMy2nga*us`cRxd<5aQlP(Mf|p16Hw-R(d8Dd;0}+D% zoh%IhufoF6%aVU16c&J>$}^Y%2w2n2?NW$;yez7sV+zHKRV;~ Ax&QzG From d4785d4b6561e1f381abd0721e1701f4c141e2fe Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 26 Apr 2014 15:13:50 +0200 Subject: [PATCH 065/329] Use INAV as default to not break existing setups --- ROMFS/px4fmu_common/init.d/rc.mc_apps | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/rc.mc_apps b/ROMFS/px4fmu_common/init.d/rc.mc_apps index c75281fcdd..268eb9bba0 100644 --- a/ROMFS/px4fmu_common/init.d/rc.mc_apps +++ b/ROMFS/px4fmu_common/init.d/rc.mc_apps @@ -5,8 +5,8 @@ # attitude_estimator_ekf start -ekf_att_pos_estimator start -#position_estimator_inav start +#ekf_att_pos_estimator start +position_estimator_inav start mc_att_control start mc_pos_control start From ad1be765bf36a6068fda25e3c62c92e275aebc6b Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 26 Apr 2014 15:14:23 +0200 Subject: [PATCH 066/329] Fix warnings, use more efficient atan2f where it can be safely used --- src/modules/ekf_att_pos_estimator/estimator.cpp | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/src/modules/ekf_att_pos_estimator/estimator.cpp b/src/modules/ekf_att_pos_estimator/estimator.cpp index 5de22fdaec..6eceb21f8e 100644 --- a/src/modules/ekf_att_pos_estimator/estimator.cpp +++ b/src/modules/ekf_att_pos_estimator/estimator.cpp @@ -2250,21 +2250,21 @@ bool AttPosEKF::StatesNaN(struct ekf_status_report *err_report) { // check all integrators if (!isfinite(summedDelAng.x) || !isfinite(summedDelAng.y) || !isfinite(summedDelAng.z)) { err_report->statesNaN = true; - ekf_debug("summedDelAng NaN: x: %f y: %f z: %f", summedDelAng.x, summedDelAng.y, summedDelAng.z); + ekf_debug("summedDelAng NaN: x: %f y: %f z: %f", (double)summedDelAng.x, (double)summedDelAng.y, (double)summedDelAng.z); err = true; goto out; } // delta angles if (!isfinite(correctedDelAng.x) || !isfinite(correctedDelAng.y) || !isfinite(correctedDelAng.z)) { err_report->statesNaN = true; - ekf_debug("correctedDelAng NaN: x: %f y: %f z: %f", correctedDelAng.x, correctedDelAng.y, correctedDelAng.z); + ekf_debug("correctedDelAng NaN: x: %f y: %f z: %f", (double)correctedDelAng.x, (double)correctedDelAng.y, (double)correctedDelAng.z); err = true; goto out; } // delta angles if (!isfinite(summedDelVel.x) || !isfinite(summedDelVel.y) || !isfinite(summedDelVel.z)) { err_report->statesNaN = true; - ekf_debug("summedDelVel NaN: x: %f y: %f z: %f", summedDelVel.x, summedDelVel.y, summedDelVel.z); + ekf_debug("summedDelVel NaN: x: %f y: %f z: %f", (double)summedDelVel.x, (double)summedDelVel.y, (double)summedDelVel.z); err = true; goto out; } // delta velocities @@ -2381,8 +2381,8 @@ void AttPosEKF::AttitudeInit(float ax, float ay, float az, float mx, float my, f float magX, magY; float initialHdg, cosHeading, sinHeading; - initialRoll = atan2(-ay, -az); - initialPitch = atan2(ax, -az); + initialRoll = atan2f(-ay, -az); + initialPitch = atan2f(ax, -az); cosRoll = cosf(initialRoll); sinRoll = sinf(initialRoll); From 9358eb2428275f78ad5b1e06b42840927092ac89 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 26 Apr 2014 15:22:03 +0200 Subject: [PATCH 067/329] Fixed string formatting error --- src/modules/sensors/sensors.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 5315c5e387..e260aae45c 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -655,7 +655,7 @@ Sensors::parameters_update() } if (param_get(_parameter_handles.rc_map_loiter_sw, &(_parameters.rc_map_loiter_sw)) != OK) { - warnx(paramerr); + warnx("%s", paramerr); } if (param_get(_parameter_handles.rc_map_flaps, &(_parameters.rc_map_flaps)) != OK) { From 0d50b3ea866a9ca0b9271b8c861f1d9e2a61a24a Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 26 Apr 2014 16:06:25 +0200 Subject: [PATCH 068/329] Fix struct inits --- .../fw_att_pos_estimator_main.cpp | 67 +++++++++++++++---- 1 file changed, 55 insertions(+), 12 deletions(-) diff --git a/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp index ad8c1b4c00..1c943137a1 100644 --- a/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp @@ -103,8 +103,6 @@ uint32_t millis() return IMUmsec; } -static void print_status(); - class FixedwingEstimator { public: @@ -305,6 +303,25 @@ FixedwingEstimator::FixedwingEstimator() : _local_pos_pub(-1), _estimator_status_pub(-1), + _att({}), + _gyro({}), + _accel({}), + _mag({}), + _airspeed({}), + _baro({}), + _vstatus({}), + _global_pos({}), + _local_pos({}), + _gps({}), + + _gyro_offsets({}), + _accel_offsets({}), + _mag_offsets({}), + + #ifdef SENSOR_COMBINED_SUB + _sensor_combined({}), + #endif + _baro_ref(0.0f), _baro_gps_offset(0.0f), @@ -356,6 +373,10 @@ FixedwingEstimator::FixedwingEstimator() : if (fd > 0) { res = ioctl(fd, GYROIOCGSCALE, (long unsigned int)&_gyro_offsets); close(fd); + + if (res) { + warnx("G SCALE FAIL"); + } } fd = open(ACCEL_DEVICE_PATH, O_RDONLY); @@ -363,6 +384,10 @@ FixedwingEstimator::FixedwingEstimator() : if (fd > 0) { res = ioctl(fd, ACCELIOCGSCALE, (long unsigned int)&_accel_offsets); close(fd); + + if (res) { + warnx("A SCALE FAIL"); + } } fd = open(MAG_DEVICE_PATH, O_RDONLY); @@ -370,6 +395,10 @@ FixedwingEstimator::FixedwingEstimator() : if (fd > 0) { res = ioctl(fd, MAGIOCGSCALE, (long unsigned int)&_mag_offsets); close(fd); + + if (res) { + warnx("M SCALE FAIL"); + } } } @@ -538,12 +567,19 @@ FixedwingEstimator::task_main() fds[1].events = POLLIN; #endif - hrt_abstime start_time = hrt_absolute_time(); - bool newDataGps = false; bool newAdsData = false; bool newDataMag = false; + // Reset relevant structs + _gps = {}; + + #ifndef SENSOR_COMBINED_SUB + _gyro = {}; + #else + _sensor_combined = {}; + #endif + while (!_task_should_exit) { /* wait for up to 500ms for data */ @@ -624,9 +660,11 @@ FixedwingEstimator::task_main() _ekf->angRate.z = _gyro.z; } - _ekf->accel.x = _accel.x; - _ekf->accel.y = _accel.y; - _ekf->accel.z = _accel.z; + if (accel_updated) { + _ekf->accel.x = _accel.x; + _ekf->accel.y = _accel.y; + _ekf->accel.z = _accel.z; + } _ekf->dAngIMU = 0.5f * (angRate + lastAngRate) * dtIMU; _ekf->lastAngRate = angRate; @@ -672,9 +710,11 @@ FixedwingEstimator::task_main() _ekf->angRate.z = _sensor_combined.gyro_rad_s[2]; } - _ekf->accel.x = _sensor_combined.accelerometer_m_s2[0]; - _ekf->accel.y = _sensor_combined.accelerometer_m_s2[1]; - _ekf->accel.z = _sensor_combined.accelerometer_m_s2[2]; + if (accel_updated) { + _ekf->accel.x = _sensor_combined.accelerometer_m_s2[0]; + _ekf->accel.y = _sensor_combined.accelerometer_m_s2[1]; + _ekf->accel.z = _sensor_combined.accelerometer_m_s2[2]; + } _ekf->dAngIMU = 0.5f * (_ekf->angRate + lastAngRate) * _ekf->dtIMU; lastAngRate = _ekf->angRate; @@ -885,7 +925,7 @@ FixedwingEstimator::task_main() rep.kalman_gain_nan = ekf_report.kalmanGainsNaN; // Copy all states or at least all that we can fit - int i = 0; + unsigned i = 0; unsigned ekf_n_states = (sizeof(ekf_report.states) / sizeof(ekf_report.states[0])); unsigned max_states = (sizeof(rep.states) / sizeof(rep.states[0])); rep.n_states = (ekf_n_states < max_states) ? ekf_n_states : max_states; @@ -1173,6 +1213,8 @@ FixedwingEstimator::task_main() _global_pos.lat = est_lat; _global_pos.lon = est_lon; _global_pos.time_gps_usec = _gps.time_gps_usec; + _global_pos.eph = _gps.eph; + _global_pos.epv = _gps.epv; } if (_local_pos.v_xy_valid) { @@ -1185,6 +1227,7 @@ FixedwingEstimator::task_main() /* local pos alt is negative, change sign and add alt offset */ _global_pos.alt = _local_pos.ref_alt + (-_local_pos.z); + _global_pos.rel_alt = (-_local_pos.z); if (_local_pos.v_z_valid) { _global_pos.vel_d = _local_pos.vz; @@ -1259,7 +1302,7 @@ FixedwingEstimator::print_status() // 15-17: Earth Magnetic Field Vector - gauss (North, East, Down) // 18-20: Body Magnetic Field Vector - gauss (X,Y,Z) - printf("dtIMU: %8.6f dt: %8.6f IMUmsec: %d\n", _ekf->dtIMU, dt, (int)IMUmsec); + printf("dtIMU: %8.6f dt: %8.6f IMUmsec: %d\n", (double)_ekf->dtIMU, (double)dt, (int)IMUmsec); printf("dvel: %8.6f %8.6f %8.6f accel: %8.6f %8.6f %8.6f\n", (double)_ekf->dVelIMU.x, (double)_ekf->dVelIMU.y, (double)_ekf->dVelIMU.z, (double)_ekf->accel.x, (double)_ekf->accel.y, (double)_ekf->accel.z); printf("dang: %8.4f %8.4f %8.4f dang corr: %8.4f %8.4f %8.4f\n" , (double)_ekf->dAngIMU.x, (double)_ekf->dAngIMU.y, (double)_ekf->dAngIMU.z, (double)_ekf->correctedDelAng.x, (double)_ekf->correctedDelAng.y, (double)_ekf->correctedDelAng.z); printf("states (quat) [1-4]: %8.4f, %8.4f, %8.4f, %8.4f\n", (double)_ekf->states[0], (double)_ekf->states[1], (double)_ekf->states[2], (double)_ekf->states[3]); From eff15ef3f15d3fc5692b06c0669f1b758e8d744b Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 26 Apr 2014 16:08:40 +0200 Subject: [PATCH 069/329] Fix what is looking like a missing cast in CMSIS - the cast within the line would make only halfway sense if this was actually intended as double precision operation --- src/lib/mathlib/CMSIS/Include/arm_math.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/lib/mathlib/CMSIS/Include/arm_math.h b/src/lib/mathlib/CMSIS/Include/arm_math.h index 6f66f9ee3e..61d3a3b61f 100644 --- a/src/lib/mathlib/CMSIS/Include/arm_math.h +++ b/src/lib/mathlib/CMSIS/Include/arm_math.h @@ -5193,7 +5193,7 @@ void arm_rfft_fast_f32( *pIa = Ialpha; /* Calculating pIb from Ialpha and Ibeta by equation pIb = -(1/2) * Ialpha + (sqrt(3)/2) * Ibeta */ - *pIb = -0.5 * Ialpha + (float32_t) 0.8660254039 *Ibeta; + *pIb = (float32_t)-0.5 * Ialpha + (float32_t) 0.8660254039 *Ibeta; } From 2d978beefbc010d962034a0f0d588cbf46a063f0 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 26 Apr 2014 16:16:44 +0200 Subject: [PATCH 070/329] Compile fixes --- src/modules/ekf_att_pos_estimator/estimator.cpp | 2 +- .../ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp | 5 ++--- 2 files changed, 3 insertions(+), 4 deletions(-) diff --git a/src/modules/ekf_att_pos_estimator/estimator.cpp b/src/modules/ekf_att_pos_estimator/estimator.cpp index 6eceb21f8e..ffebcd4776 100644 --- a/src/modules/ekf_att_pos_estimator/estimator.cpp +++ b/src/modules/ekf_att_pos_estimator/estimator.cpp @@ -2307,7 +2307,7 @@ bool AttPosEKF::StatesNaN(struct ekf_status_report *err_report) { if (!isfinite(states[i])) { err_report->statesNaN = true; - ekf_debug("states NaN: i: %u val: %f", i, states[i]); + ekf_debug("states NaN: i: %u val: %f", i, (double)states[i]); err = true; goto out; } // state matrix diff --git a/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp index 1c943137a1..fdb5dd5883 100644 --- a/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp @@ -1213,8 +1213,8 @@ FixedwingEstimator::task_main() _global_pos.lat = est_lat; _global_pos.lon = est_lon; _global_pos.time_gps_usec = _gps.time_gps_usec; - _global_pos.eph = _gps.eph; - _global_pos.epv = _gps.epv; + _global_pos.eph = _gps.eph_m; + _global_pos.epv = _gps.epv_m; } if (_local_pos.v_xy_valid) { @@ -1227,7 +1227,6 @@ FixedwingEstimator::task_main() /* local pos alt is negative, change sign and add alt offset */ _global_pos.alt = _local_pos.ref_alt + (-_local_pos.z); - _global_pos.rel_alt = (-_local_pos.z); if (_local_pos.v_z_valid) { _global_pos.vel_d = _local_pos.vz; From 18a932fe948b06b87d65f5d13102274e5683fab6 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 26 Apr 2014 18:35:22 +0200 Subject: [PATCH 071/329] Better fake / simulation values --- src/drivers/gps/gps.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/drivers/gps/gps.cpp b/src/drivers/gps/gps.cpp index a902bdf2f5..01be80dcec 100644 --- a/src/drivers/gps/gps.cpp +++ b/src/drivers/gps/gps.cpp @@ -276,14 +276,14 @@ GPS::task_main() _report.timestamp_position = hrt_absolute_time(); _report.lat = (int32_t)47.378301e7f; _report.lon = (int32_t)8.538777e7f; - _report.alt = (int32_t)400e3f; + _report.alt = (int32_t)1200e3f; _report.timestamp_variance = hrt_absolute_time(); _report.s_variance_m_s = 10.0f; _report.p_variance_m = 10.0f; _report.c_variance_rad = 0.1f; _report.fix_type = 3; - _report.eph_m = 3.0f; - _report.epv_m = 7.0f; + _report.eph_m = 0.9f; + _report.epv_m = 1.8f; _report.timestamp_velocity = hrt_absolute_time(); _report.vel_n_m_s = 0.0f; _report.vel_e_m_s = 0.0f; From 6612cdab856674c4cbec53863c470cb4e96a9f7d Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 26 Apr 2014 18:36:09 +0200 Subject: [PATCH 072/329] Let commander be less pedantic about positioning data --- src/modules/commander/commander.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 3527117341..148a0aafa2 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -117,7 +117,7 @@ extern struct system_load_s system_load; #define STICK_ON_OFF_HYSTERESIS_TIME_MS 1000 #define STICK_ON_OFF_COUNTER_LIMIT (STICK_ON_OFF_HYSTERESIS_TIME_MS*COMMANDER_MONITORING_LOOPSPERMSEC) -#define POSITION_TIMEOUT 30000 /**< consider the local or global position estimate invalid after 30ms */ +#define POSITION_TIMEOUT 50000 /**< consider the local or global position estimate invalid after 50ms */ #define RC_TIMEOUT 500000 #define DIFFPRESS_TIMEOUT 2000000 From 925430796ed56e97bf2310423361a8e6e29e350b Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 26 Apr 2014 18:37:26 +0200 Subject: [PATCH 073/329] Reworked how we deal with altitudes --- .../fw_att_pos_estimator_main.cpp | 79 ++++++++++++------- .../fw_att_pos_estimator_params.c | 11 +++ 2 files changed, 62 insertions(+), 28 deletions(-) diff --git a/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp index fdb5dd5883..a02b2c34fb 100644 --- a/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp @@ -191,6 +191,7 @@ private: perf_counter_t _perf_airspeed; ///yawVarScale = 1.0f; @@ -568,18 +574,10 @@ FixedwingEstimator::task_main() #endif bool newDataGps = false; + bool newHgtData = false; bool newAdsData = false; bool newDataMag = false; - // Reset relevant structs - _gps = {}; - - #ifndef SENSOR_COMBINED_SUB - _gyro = {}; - #else - _sensor_combined = {}; - #endif - while (!_task_should_exit) { /* wait for up to 500ms for data */ @@ -611,6 +609,7 @@ FixedwingEstimator::task_main() if (fds[1].revents & POLLIN) { /* check vehicle status for changes to publication state */ + bool prev_hil = (_vstatus.hil_state == HIL_STATE_ON); vehicle_status_poll(); bool accel_updated; @@ -618,6 +617,12 @@ FixedwingEstimator::task_main() perf_count(_perf_gyro); + /* Reset baro reference if switching to HIL */ + if (!prev_hil && (_vstatus.hil_state == HIL_STATE_ON)) { + _baro_init = false; + _gps_initialized = false; + } + /** * PART ONE: COLLECT ALL DATA **/ @@ -813,10 +818,17 @@ FixedwingEstimator::task_main() if (baro_updated) { orb_copy(ORB_ID(sensor_baro), _baro_sub, &_baro); - _ekf->baroHgt = _baro.altitude - _baro_ref; + _ekf->baroHgt = _baro.altitude; - // Could use a blend of GPS and baro alt data if desired - _ekf->hgtMea = 1.0f * _ekf->baroHgt + 0.0f * _ekf->gpsHgt; + if (!_baro_init) { + _baro_ref = _baro.altitude; + _baro_init = true; + warnx("ALT REF INIT"); + } + + newHgtData = true; + } else { + newHgtData = false; } #ifndef SENSOR_COMBINED_SUB @@ -904,9 +916,9 @@ FixedwingEstimator::task_main() } } - // XXX trap for testing + // warn on fatal resets if (check == 1) { - errx(1, "NUMERIC ERROR IN FILTER"); + warnx("NUMERIC ERROR IN FILTER"); } // If non-zero, we got a filter reset @@ -960,7 +972,7 @@ FixedwingEstimator::task_main() // struct home_position_s home; // orb_copy(ORB_ID(home_position), _home_sub, &home); - if (!_gps_initialized && _gps.fix_type > 2) { + if (!_gps_initialized && _gps.fix_type > 2 && _gps.eph_m < _parameters.pos_stddev_threshold && _gps.epv_m < _parameters.pos_stddev_threshold) { _ekf->velNED[0] = _gps.vel_n_m_s; _ekf->velNED[1] = _gps.vel_e_m_s; _ekf->velNED[2] = _gps.vel_d_m_s; @@ -968,26 +980,37 @@ FixedwingEstimator::task_main() // GPS is in scaled integers, convert double lat = _gps.lat / 1.0e7; double lon = _gps.lon / 1.0e7; - float alt = _gps.alt / 1e3f; + float gps_alt = _gps.alt / 1e3f; - _ekf->InitialiseFilter(_ekf->velNED, math::radians(lat), math::radians(lon) - M_PI, alt); + // Set up height correctly + orb_copy(ORB_ID(sensor_baro), _baro_sub, &_baro); + _baro_gps_offset = gps_alt - _baro.altitude; + _ekf->baroHgt = _baro.altitude; + _ekf->hgtMea = 1.0f * (_ekf->baroHgt - _baro_ref); + + // Set up position variables correctly + _ekf->GPSstatus = _gps.fix_type; + _ekf->velNED[0] = _gps.vel_n_m_s; + _ekf->velNED[1] = _gps.vel_e_m_s; + _ekf->velNED[2] = _gps.vel_d_m_s; + + _ekf->gpsLat = math::radians(_gps.lat / (double)1e7); + _ekf->gpsLon = math::radians(_gps.lon / (double)1e7) - M_PI; + _ekf->gpsHgt = _gps.alt / 1e3f; + + _ekf->InitialiseFilter(_ekf->velNED, math::radians(lat), math::radians(lon) - M_PI, gps_alt); // Initialize projection _local_pos.ref_lat = _gps.lat; _local_pos.ref_lon = _gps.alt; - _local_pos.ref_alt = alt; + _local_pos.ref_alt = _baro_ref + _baro_gps_offset; _local_pos.ref_timestamp = _gps.timestamp_position; - // Store - orb_copy(ORB_ID(sensor_baro), _baro_sub, &_baro); - _baro_ref = _baro.altitude; - _ekf->baroHgt = _baro.altitude - _baro_ref; - _baro_gps_offset = _baro_ref - _local_pos.ref_alt; - map_projection_init(&_pos_ref, lat, lon); - mavlink_log_info(_mavlink_fd, "[ekf] ref: LA %.4f,LO %.4f,ALT %.2f", lat, lon, (double)alt); - warnx("[ekf] HOME/REF: LA %8.4f,LO %8.4f,ALT %8.2f V: %8.4f %8.4f %8.4f", lat, lon, (double)alt, + mavlink_log_info(_mavlink_fd, "[ekf] ref: LA %.4f,LO %.4f,ALT %.2f", lat, lon, (double)gps_alt); + warnx("HOME/REF: LA %8.4f,LO %8.4f,ALT %8.2f V: %8.4f %8.4f %8.4f", lat, lon, (double)gps_alt, (double)_ekf->velNED[0], (double)_ekf->velNED[1], (double)_ekf->velNED[2]); + warnx("GPS: eph: %8.4f, epv: %8.4f", (double)_gps.eph_m, (double)_gps.epv_m); _gps_initialized = true; @@ -1089,9 +1112,9 @@ FixedwingEstimator::task_main() _ekf->fusePosData = false; } - if (newAdsData && _ekf->statesInitialised) { + if (newHgtData && _ekf->statesInitialised) { // Could use a blend of GPS and baro alt data if desired - _ekf->hgtMea = 1.0f * _ekf->baroHgt + 0.0f * _ekf->gpsHgt; + _ekf->hgtMea = 1.0f * (_ekf->baroHgt - _baro_ref); _ekf->fuseHgtData = true; // recall states stored at time of measurement after adjusting for delays _ekf->RecallStates(_ekf->statesAtHgtTime, (IMUmsec - _parameters.height_delay_ms)); diff --git a/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_params.c b/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_params.c index cfcd99858e..d2c6e1f15e 100644 --- a/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_params.c +++ b/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_params.c @@ -258,3 +258,14 @@ PARAM_DEFINE_FLOAT(PE_MAGE_PNOISE, 0.0003f); */ PARAM_DEFINE_FLOAT(PE_MAGB_PNOISE, 0.0003f); +/** + * Threshold for filter initialization. + * + * If the standard deviation of the GPS position estimate is below this threshold + * in meters, the filter will initialize. + * + * @min 0.3 + * @max 10.0 + * @group Position Estimator + */ +PARAM_DEFINE_FLOAT(PE_POSDEV_INIT, 5.0f); From 6ab878c0218f26e5fa71053b75d7075b594b937e Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sat, 26 Apr 2014 18:38:37 +0200 Subject: [PATCH 074/329] Emit the local position against the GPS reference - this means it can jump. --- src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp index a02b2c34fb..a82e8d7044 100644 --- a/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp @@ -1203,7 +1203,7 @@ FixedwingEstimator::task_main() _local_pos.timestamp = last_sensor_timestamp; _local_pos.x = _ekf->states[7]; _local_pos.y = _ekf->states[8]; - _local_pos.z = _ekf->states[9]; + _local_pos.z = _ekf->states[9] + _baro_gps_offset; _local_pos.vx = _ekf->states[4]; _local_pos.vy = _ekf->states[5]; From 5ea1105451330aa55e0c331b99ba2ab60e6f8c15 Mon Sep 17 00:00:00 2001 From: ufoncz Date: Sun, 27 Apr 2014 15:12:05 +0200 Subject: [PATCH 075/329] changed dir from version to ver to keep it shorter added "hw_ver compare" as command option so we can replace hw_ver in future --- makefiles/config_px4fmu-v1_default.mk | 1 + makefiles/config_px4fmu-v2_default.mk | 2 +- src/systemcmds/{version => ver}/module.mk | 7 +- src/systemcmds/ver/ver.c | 175 ++++++++++++++++++++++ src/systemcmds/ver/ver.h | 64 ++++++++ src/systemcmds/version/version.c | 144 ------------------ 6 files changed, 245 insertions(+), 148 deletions(-) rename src/systemcmds/{version => ver}/module.mk (90%) create mode 100644 src/systemcmds/ver/ver.c create mode 100644 src/systemcmds/ver/ver.h delete mode 100644 src/systemcmds/version/version.c diff --git a/makefiles/config_px4fmu-v1_default.mk b/makefiles/config_px4fmu-v1_default.mk index 532e978d04..20e7ab3312 100644 --- a/makefiles/config_px4fmu-v1_default.mk +++ b/makefiles/config_px4fmu-v1_default.mk @@ -57,6 +57,7 @@ MODULES += systemcmds/config MODULES += systemcmds/nshterm MODULES += systemcmds/hw_ver MODULES += systemcmds/dumpfile +MODULES += systemcmds/ver # # General system control diff --git a/makefiles/config_px4fmu-v2_default.mk b/makefiles/config_px4fmu-v2_default.mk index 7ec6a2fc68..bbb754eb08 100644 --- a/makefiles/config_px4fmu-v2_default.mk +++ b/makefiles/config_px4fmu-v2_default.mk @@ -65,7 +65,7 @@ MODULES += systemcmds/nshterm MODULES += systemcmds/mtd MODULES += systemcmds/hw_ver MODULES += systemcmds/dumpfile -MODULES += systemcmds/version +MODULES += systemcmds/ver # # General system control diff --git a/src/systemcmds/version/module.mk b/src/systemcmds/ver/module.mk similarity index 90% rename from src/systemcmds/version/module.mk rename to src/systemcmds/ver/module.mk index 3dac09239a..2eeb80b616 100644 --- a/src/systemcmds/version/module.mk +++ b/src/systemcmds/ver/module.mk @@ -32,11 +32,12 @@ ############################################################################ # -# Show and test hardware version +# "version" nsh-command displays version infromation for hw,sw, gcc,build etc +# can be also included and used in own code via "ver.h" # -MODULE_COMMAND = version -SRCS = version.c +MODULE_COMMAND = ver +SRCS = ver.c MODULE_STACKSIZE = 1024 diff --git a/src/systemcmds/ver/ver.c b/src/systemcmds/ver/ver.c new file mode 100644 index 0000000000..f44fd1afe1 --- /dev/null +++ b/src/systemcmds/ver/ver.c @@ -0,0 +1,175 @@ +/**************************************************************************** +* +* Copyright (c) 2012-2014 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 ver.c +* +* Version command, unifies way of showing versions of HW, SW, Build, gcc +* In case you want to add new version just extend version_main function +* +* External use of the version functions is possible, include "vercmd.h" +* and use functions from header with prefix ver_ +* +* @author Vladimir Kulla +*/ + +#include +#include +#include +#include +#include + +#include "ver.h" + +// string constants for version commands +static const char sz_ver_hw_str[] = "hw"; +static const char sz_ver_hwcmp_str[]= "hwcmp"; +static const char sz_ver_git_str[] = "git"; +static const char sz_ver_date_str[] = "date"; +static const char sz_ver_gcc_str[] = "gcc"; +static const char sz_ver_all_str[] = "all"; + +static void usage(const char *reason) +{ + if (reason != NULL) { + printf("%s\n", reason); + } + + printf("usage: version {hw|hwcmp|git|date|gcc|all}\n\n"); +} + +void ver_githash(int bShowPrefix) +{ + if (bShowPrefix == 1) { + printf("FW git-hash: "); + } + printf("%s\n", FW_GIT); +} + +void ver_hwarch(int bShowPrefix) +{ + if (bShowPrefix == 1) { + printf("HW arch: "); + } + printf("%s\n", HW_ARCH); +} + +void ver_date(int bShowPrefix) +{ + if (bShowPrefix == 1) { + printf("Build date: "); + } + printf("%s %s\n", __DATE__, __TIME__); +} + +void ver_gcclong(int bShowPrefix) +{ + if (bShowPrefix == 1) { + printf("GCC used (long): "); + //printf("Built with GCC : %d.%d.%d\n", __GNUC__, __GNUC_MINOR__, __GNUC_PATCHLEVEL__); + } + printf("%s\n", __VERSION__); +} + +void ver_gccshort(int bShowPrefix) +{ + if (bShowPrefix == 1) { + printf("GCC used (short): "); + + } + printf("%d.%d.%d\n", __GNUC__, __GNUC_MINOR__, __GNUC_PATCHLEVEL__); +} + +int ver_gccnum() +{ + return (__GNUC__ * 1000) + (__GNUC_MINOR__ * 100) + __GNUC_PATCHLEVEL__; +} + +__EXPORT int ver_main(int argc, char *argv[]); + +int ver_main(int argc, char *argv[]) +{ + int ret = 1; //defaults to an error + + // first check if there are at least 2 params + if (argc >= 2) { + if (argv[1] != NULL) { + if (!strncmp(argv[1], sz_ver_hw_str, strlen(sz_ver_hw_str))) { + ver_hwarch(0); + ret = 0; + } + else if (!strncmp(argv[1], sz_ver_hwcmp_str, strlen(sz_ver_hwcmp_str))) { + if (argc >= 3 && argv[2] != NULL) { + // compare 3rd parameter with HW_ARCH string, in case of match, return 0 + ret = strcmp(HW_ARCH, argv[2]) != 0; + if (ret == 0) { + printf("hw_ver match: %s\n", HW_ARCH); + } + } else { + errx(1, "not enough arguments, try 'version hwcmp PX4FMU_1'"); + } + } + else if (!strncmp(argv[1], sz_ver_git_str, strlen(sz_ver_git_str))) { + ver_githash(0); + ret = 0; + } + else if (!strncmp(argv[1], sz_ver_date_str, strlen(sz_ver_date_str))) { + ver_date(0); + ret = 0; + } + else if (!strncmp(argv[1], sz_ver_gcc_str, strlen(sz_ver_gcc_str))) { + ver_gcclong(0); + ret = 0; + } + else if (!strncmp(argv[1], sz_ver_all_str, strlen(sz_ver_all_str))) { + printf("Pixhawk version info\n"); + ver_hwarch(1); + ver_date(1); + ver_githash(1); + ver_gcclong(1); + ret = 0; + } + else { + errx(1, "unknown command.\n"); + } + } + else { + usage("Error, input parameter NULL.\n"); + } + } + else { + usage("Error, not enough parameters."); + } + + return ret; +} diff --git a/src/systemcmds/ver/ver.h b/src/systemcmds/ver/ver.h new file mode 100644 index 0000000000..3de308c05c --- /dev/null +++ b/src/systemcmds/ver/ver.h @@ -0,0 +1,64 @@ +/**************************************************************************** +* +* Copyright (c) 2012-2014 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 vercmd.h +* +* Version command, unifies way of showing versions of hW, sW, build, gcc +* Mainly used from nsh-console, shall replace also hw_arch command +* +* External use: possible, include "vercmd.h" and use "ver_xx" functions +* +* Extension: possible, just add new else-if to version_main function +* +* @author Vladimir Kulla +*/ + +#ifndef VERCMD_H_ +#define VERCMD_H_ + + +void ver_githash(int bShowPrefix); + +void ver_hwarch(int bShowPrefix); + +void ver_date(int bShowPrefix); + +void ver_gcclong(int bShowPrefix); +void ver_gccshort(int bShowPrefix); +int ver_gccnum(); + + + + +#endif /* VERCMD_H_ */ diff --git a/src/systemcmds/version/version.c b/src/systemcmds/version/version.c deleted file mode 100644 index 37a51dbfc0..0000000000 --- a/src/systemcmds/version/version.c +++ /dev/null @@ -1,144 +0,0 @@ -/**************************************************************************** - * - * Copyright (C) 2012 PX4 Development Team. All rights reserved. - * Author: Vladimir Kulla - * - * 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 version.c - * - * Version nsh command, unified way of showing versions of HW, SW, Build, Toolchain etc - */ - -#include - -#include -#include -#include -#include - -static char sz_ver_hw_str[] = "hw"; -static char sz_ver_git_str[] = "git"; -static char sz_ver_date_str[] = "date"; -static char sz_ver_gcc_str[] = "gcc"; -static char sz_ver_all_str[] = "all"; - - -__EXPORT int version_main(int argc, char *argv[]); - -static void usage(const char *reason) -{ - if (reason != NULL) { - fprintf(stderr, "%s\n", reason); - } - - fprintf(stderr, "usage: version {hw|git|date|gcc|all}\n\n"); -} - -static void version_githash(int bShowPrefix) -{ - if (bShowPrefix == 1) { - printf("FW git-hash: "); - } - printf("%s\n", FW_GIT); -} - -static void version_hwarch(int bShowPrefix) -{ - if (bShowPrefix == 1) { - printf("HW arch: "); - } - printf("%s\n", HW_ARCH); -} - -static void version_date(int bShowPrefix) -{ - if (bShowPrefix == 1) { - printf("Build date: "); - } - printf("%s %s\n", __DATE__, __TIME__); -} - -static void version_gcc(int bShowPrefix) -{ - if (bShowPrefix == 1) { - printf("GCC used: "); - //printf("Built with GCC : %d.%d.%d\n", __GNUC__, __GNUC_MINOR__, __GNUC_PATCHLEVEL__); - } - printf("%s\n", __VERSION__); -} - -int version_main(int argc, char *argv[]) -{ - if (argc >= 2) - { - if (argv[1] != NULL) - { - if (!strncmp(argv[1], sz_ver_hw_str, strlen(sz_ver_hw_str))) - { - version_hwarch(0); - } - else if (!strncmp(argv[1], sz_ver_git_str, strlen(sz_ver_git_str))) - { - version_githash(0); - } - else if (!strncmp(argv[1], sz_ver_date_str, strlen(sz_ver_date_str))) - { - version_date(0); - } - else if (!strncmp(argv[1], sz_ver_gcc_str, strlen(sz_ver_gcc_str))) - { - version_gcc(0); - } - else if (!strncmp(argv[1], sz_ver_all_str, strlen(sz_ver_all_str))) - { - printf("Pixhawk version info\n"); - version_hwarch(1); - version_date(1); - version_githash(1); - version_gcc(1); - } - else - { - printf("unknown command: %s\n", argv[1]); - } - } - else - { - usage("Error, input parameter NULL.\n"); - } - } - else - { - usage("Error, not enough parameters."); - } - return OK; -} From bd5a0cef1aad42f8deb1d58e573631436630246e Mon Sep 17 00:00:00 2001 From: ufoncz Date: Sun, 27 Apr 2014 17:42:45 +0200 Subject: [PATCH 076/329] ver command ready including hwcmp which replaces hw_ver, removed hw_ver updated all scripts to use new ver hwcmp command q --- ROMFS/px4fmu_common/init.d/rc.io | 2 +- ROMFS/px4fmu_common/init.d/rc.logging | 2 +- ROMFS/px4fmu_common/init.d/rc.sensors | 2 +- ROMFS/px4fmu_common/init.d/rcS | 8 +-- makefiles/config_px4fmu-v1_default.mk | 1 - makefiles/config_px4fmu-v2_default.mk | 1 - makefiles/config_px4fmu-v2_test.mk | 2 +- src/systemcmds/hw_ver/hw_ver.c | 73 --------------------------- src/systemcmds/hw_ver/module.mk | 43 ---------------- src/systemcmds/ver/ver.c | 64 ++++++++--------------- src/systemcmds/ver/ver.h | 64 ----------------------- 11 files changed, 29 insertions(+), 233 deletions(-) delete mode 100644 src/systemcmds/hw_ver/hw_ver.c delete mode 100644 src/systemcmds/hw_ver/module.mk delete mode 100644 src/systemcmds/ver/ver.h diff --git a/ROMFS/px4fmu_common/init.d/rc.io b/ROMFS/px4fmu_common/init.d/rc.io index 6e8fdbc0f7..e23aebd87f 100644 --- a/ROMFS/px4fmu_common/init.d/rc.io +++ b/ROMFS/px4fmu_common/init.d/rc.io @@ -11,7 +11,7 @@ px4io recovery # Adjust PX4IO update rate limit # set PX4IO_LIMIT 400 -if hw_ver compare PX4FMU_V1 +if ver hwcmp PX4FMU_V1 then set PX4IO_LIMIT 200 fi diff --git a/ROMFS/px4fmu_common/init.d/rc.logging b/ROMFS/px4fmu_common/init.d/rc.logging index c5aef8273c..3469d5f5f5 100644 --- a/ROMFS/px4fmu_common/init.d/rc.logging +++ b/ROMFS/px4fmu_common/init.d/rc.logging @@ -5,7 +5,7 @@ if [ -d /fs/microsd ] then - if hw_ver compare PX4FMU_V1 + if ver hwcmp PX4FMU_V1 then echo "Start sdlog2 at 50Hz" sdlog2 start -r 50 -a -b 8 -t diff --git a/ROMFS/px4fmu_common/init.d/rc.sensors b/ROMFS/px4fmu_common/init.d/rc.sensors index 235be24316..1e14930fe3 100644 --- a/ROMFS/px4fmu_common/init.d/rc.sensors +++ b/ROMFS/px4fmu_common/init.d/rc.sensors @@ -22,7 +22,7 @@ then echo "[init] Using L3GD20(H)" fi -if hw_ver compare PX4FMU_V2 +if ver hwcmp PX4FMU_V2 then if lsm303d start then diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index ea5cf8deb7..3fb5fb7333 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -246,7 +246,7 @@ then echo "[init] ERROR: PX4IO not found, disabling output" # Avoid using ttyS0 for MAVLink on FMUv1 - if hw_ver compare PX4FMU_V1 + if ver hwcmp PX4FMU_V1 then set FMU_MODE serial fi @@ -260,7 +260,7 @@ then if [ $HIL == yes ] then set OUTPUT_MODE hil - if hw_ver compare PX4FMU_V1 + if ver hwcmp PX4FMU_V1 then set FMU_MODE serial fi @@ -306,7 +306,7 @@ then tone_alarm $TUNE_OUT_ERROR fi - if hw_ver compare PX4FMU_V1 + if ver hwcmp PX4FMU_V1 then if [ $FMU_MODE == pwm -o $FMU_MODE == gpio ] then @@ -381,7 +381,7 @@ then tone_alarm $TUNE_OUT_ERROR fi - if hw_ver compare PX4FMU_V1 + if ver hwcmp PX4FMU_V1 then if [ $FMU_MODE == pwm -o $FMU_MODE == gpio ] then diff --git a/makefiles/config_px4fmu-v1_default.mk b/makefiles/config_px4fmu-v1_default.mk index 20e7ab3312..61a417f308 100644 --- a/makefiles/config_px4fmu-v1_default.mk +++ b/makefiles/config_px4fmu-v1_default.mk @@ -55,7 +55,6 @@ MODULES += systemcmds/top MODULES += systemcmds/tests MODULES += systemcmds/config MODULES += systemcmds/nshterm -MODULES += systemcmds/hw_ver MODULES += systemcmds/dumpfile MODULES += systemcmds/ver diff --git a/makefiles/config_px4fmu-v2_default.mk b/makefiles/config_px4fmu-v2_default.mk index bbb754eb08..65ca24325c 100644 --- a/makefiles/config_px4fmu-v2_default.mk +++ b/makefiles/config_px4fmu-v2_default.mk @@ -63,7 +63,6 @@ MODULES += systemcmds/tests MODULES += systemcmds/config MODULES += systemcmds/nshterm MODULES += systemcmds/mtd -MODULES += systemcmds/hw_ver MODULES += systemcmds/dumpfile MODULES += systemcmds/ver diff --git a/makefiles/config_px4fmu-v2_test.mk b/makefiles/config_px4fmu-v2_test.mk index 79922374d7..84274bf754 100644 --- a/makefiles/config_px4fmu-v2_test.mk +++ b/makefiles/config_px4fmu-v2_test.mk @@ -29,7 +29,7 @@ MODULES += systemcmds/reboot MODULES += systemcmds/tests MODULES += systemcmds/nshterm MODULES += systemcmds/mtd -MODULES += systemcmds/hw_ver +MODULES += systemcmds/ver # # Library modules diff --git a/src/systemcmds/hw_ver/hw_ver.c b/src/systemcmds/hw_ver/hw_ver.c deleted file mode 100644 index 4b84523cc1..0000000000 --- a/src/systemcmds/hw_ver/hw_ver.c +++ /dev/null @@ -1,73 +0,0 @@ -/**************************************************************************** - * - * Copyright (c) 2014 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 hw_ver.c - * - * Show and test hardware version. - */ - -#include - -#include -#include -#include -#include - -__EXPORT int hw_ver_main(int argc, char *argv[]); - -int -hw_ver_main(int argc, char *argv[]) -{ - if (argc >= 2) { - if (!strcmp(argv[1], "show")) { - printf(HW_ARCH "\n"); - exit(0); - } - - if (!strcmp(argv[1], "compare")) { - if (argc >= 3) { - int ret = strcmp(HW_ARCH, argv[2]) != 0; - if (ret == 0) { - printf("hw_ver match: %s\n", HW_ARCH); - } - exit(ret); - - } else { - errx(1, "not enough arguments, try 'compare PX4FMU_1'"); - } - } - } - - errx(1, "expected a command, try 'show' or 'compare'"); -} diff --git a/src/systemcmds/hw_ver/module.mk b/src/systemcmds/hw_ver/module.mk deleted file mode 100644 index 3cc08b6a1a..0000000000 --- a/src/systemcmds/hw_ver/module.mk +++ /dev/null @@ -1,43 +0,0 @@ -############################################################################ -# -# Copyright (c) 2014 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. -# -############################################################################ - -# -# Show and test hardware version -# - -MODULE_COMMAND = hw_ver -SRCS = hw_ver.c - -MODULE_STACKSIZE = 1024 - -MAXOPTIMIZATION = -Os diff --git a/src/systemcmds/ver/ver.c b/src/systemcmds/ver/ver.c index f44fd1afe1..c932bc1624 100644 --- a/src/systemcmds/ver/ver.c +++ b/src/systemcmds/ver/ver.c @@ -34,28 +34,22 @@ /** * @file ver.c * -* Version command, unifies way of showing versions of HW, SW, Build, gcc +* Version command, unifies way of showing versions of HW, SW, Build, GCC * In case you want to add new version just extend version_main function * -* External use of the version functions is possible, include "vercmd.h" -* and use functions from header with prefix ver_ -* * @author Vladimir Kulla */ -#include #include #include -#include #include - -#include "ver.h" +#include // string constants for version commands static const char sz_ver_hw_str[] = "hw"; static const char sz_ver_hwcmp_str[]= "hwcmp"; static const char sz_ver_git_str[] = "git"; -static const char sz_ver_date_str[] = "date"; +static const char sz_ver_bdate_str[] = "bdate"; static const char sz_ver_gcc_str[] = "gcc"; static const char sz_ver_all_str[] = "all"; @@ -65,10 +59,10 @@ static void usage(const char *reason) printf("%s\n", reason); } - printf("usage: version {hw|hwcmp|git|date|gcc|all}\n\n"); + printf("usage: ver {hw|hwcmp|git|bdate|gcc|all}\n\n"); } -void ver_githash(int bShowPrefix) +static void ver_githash(int bShowPrefix) { if (bShowPrefix == 1) { printf("FW git-hash: "); @@ -76,7 +70,7 @@ void ver_githash(int bShowPrefix) printf("%s\n", FW_GIT); } -void ver_hwarch(int bShowPrefix) +static void ver_hwarch(int bShowPrefix) { if (bShowPrefix == 1) { printf("HW arch: "); @@ -84,37 +78,22 @@ void ver_hwarch(int bShowPrefix) printf("%s\n", HW_ARCH); } -void ver_date(int bShowPrefix) +static void ver_bdate(int bShowPrefix) { if (bShowPrefix == 1) { - printf("Build date: "); + printf("Build datetime: "); } printf("%s %s\n", __DATE__, __TIME__); } -void ver_gcclong(int bShowPrefix) +static void ver_gcclong(int bShowPrefix) { if (bShowPrefix == 1) { - printf("GCC used (long): "); - //printf("Built with GCC : %d.%d.%d\n", __GNUC__, __GNUC_MINOR__, __GNUC_PATCHLEVEL__); + printf("GCC toolchain: "); } printf("%s\n", __VERSION__); } -void ver_gccshort(int bShowPrefix) -{ - if (bShowPrefix == 1) { - printf("GCC used (short): "); - - } - printf("%d.%d.%d\n", __GNUC__, __GNUC_MINOR__, __GNUC_PATCHLEVEL__); -} - -int ver_gccnum() -{ - return (__GNUC__ * 1000) + (__GNUC_MINOR__ * 100) + __GNUC_PATCHLEVEL__; -} - __EXPORT int ver_main(int argc, char *argv[]); int ver_main(int argc, char *argv[]) @@ -124,37 +103,36 @@ int ver_main(int argc, char *argv[]) // first check if there are at least 2 params if (argc >= 2) { if (argv[1] != NULL) { - if (!strncmp(argv[1], sz_ver_hw_str, strlen(sz_ver_hw_str))) { + if (!strncmp(argv[1], sz_ver_hw_str, sizeof(sz_ver_hw_str))) { ver_hwarch(0); ret = 0; } - else if (!strncmp(argv[1], sz_ver_hwcmp_str, strlen(sz_ver_hwcmp_str))) { + else if (!strncmp(argv[1], sz_ver_hwcmp_str, sizeof(sz_ver_hwcmp_str))) { if (argc >= 3 && argv[2] != NULL) { // compare 3rd parameter with HW_ARCH string, in case of match, return 0 - ret = strcmp(HW_ARCH, argv[2]) != 0; + ret = strncmp(HW_ARCH, argv[2], strlen(HW_ARCH)); if (ret == 0) { - printf("hw_ver match: %s\n", HW_ARCH); + printf("hwver match: %s\n", HW_ARCH); } } else { - errx(1, "not enough arguments, try 'version hwcmp PX4FMU_1'"); + errx(1, "Not enough arguments, try 'ver hwcmp PX4FMU_V2'"); } } - else if (!strncmp(argv[1], sz_ver_git_str, strlen(sz_ver_git_str))) { + else if (!strncmp(argv[1], sz_ver_git_str, sizeof(sz_ver_git_str))) { ver_githash(0); ret = 0; } - else if (!strncmp(argv[1], sz_ver_date_str, strlen(sz_ver_date_str))) { - ver_date(0); + else if (!strncmp(argv[1], sz_ver_bdate_str, sizeof(sz_ver_bdate_str))) { + ver_bdate(0); ret = 0; } - else if (!strncmp(argv[1], sz_ver_gcc_str, strlen(sz_ver_gcc_str))) { + else if (!strncmp(argv[1], sz_ver_gcc_str, sizeof(sz_ver_gcc_str))) { ver_gcclong(0); ret = 0; } - else if (!strncmp(argv[1], sz_ver_all_str, strlen(sz_ver_all_str))) { - printf("Pixhawk version info\n"); + else if (!strncmp(argv[1], sz_ver_all_str, sizeof(sz_ver_all_str))) { ver_hwarch(1); - ver_date(1); + ver_bdate(1); ver_githash(1); ver_gcclong(1); ret = 0; diff --git a/src/systemcmds/ver/ver.h b/src/systemcmds/ver/ver.h deleted file mode 100644 index 3de308c05c..0000000000 --- a/src/systemcmds/ver/ver.h +++ /dev/null @@ -1,64 +0,0 @@ -/**************************************************************************** -* -* Copyright (c) 2012-2014 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 vercmd.h -* -* Version command, unifies way of showing versions of hW, sW, build, gcc -* Mainly used from nsh-console, shall replace also hw_arch command -* -* External use: possible, include "vercmd.h" and use "ver_xx" functions -* -* Extension: possible, just add new else-if to version_main function -* -* @author Vladimir Kulla -*/ - -#ifndef VERCMD_H_ -#define VERCMD_H_ - - -void ver_githash(int bShowPrefix); - -void ver_hwarch(int bShowPrefix); - -void ver_date(int bShowPrefix); - -void ver_gcclong(int bShowPrefix); -void ver_gccshort(int bShowPrefix); -int ver_gccnum(); - - - - -#endif /* VERCMD_H_ */ From 6a7b104c2baad7527d35736979ddd1352bf4119d Mon Sep 17 00:00:00 2001 From: TickTock- Date: Sun, 27 Apr 2014 13:12:28 -0700 Subject: [PATCH 077/329] compiles --- src/modules/commander/commander.cpp | 5 +-- src/modules/sensors/sensors.cpp | 34 +------------------ .../uORB/topics/manual_control_setpoint.h | 4 --- src/modules/uORB/topics/rc_channels.h | 4 --- 4 files changed, 2 insertions(+), 45 deletions(-) diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index d5f05c9916..50380107d0 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -1228,11 +1228,8 @@ int commander_thread_main(int argc, char *argv[]) status.set_nav_state_timestamp = hrt_absolute_time(); } else { -<<<<<<< HEAD + /* LOITER switch */ -======= - /* MISSION switch */ ->>>>>>> ad77ba26427aa9a2d8b8241fc95271667a1c0863 if (sp_man.loiter_switch == SWITCH_POS_ON) { /* stick is in LOITER position */ status.set_nav_state = NAV_STATE_LOITER; diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 9a750db12e..6449c5283b 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -255,11 +255,7 @@ private: int rc_map_mode_sw; int rc_map_return_sw; -<<<<<<< HEAD int rc_map_easy_sw; -======= - int rc_map_assisted_sw; ->>>>>>> ad77ba26427aa9a2d8b8241fc95271667a1c0863 int rc_map_loiter_sw; // int rc_map_offboard_ctrl_mode_sw; @@ -313,11 +309,7 @@ private: param_t rc_map_mode_sw; param_t rc_map_return_sw; -<<<<<<< HEAD param_t rc_map_easy_sw; -======= - param_t rc_map_assisted_sw; ->>>>>>> ad77ba26427aa9a2d8b8241fc95271667a1c0863 param_t rc_map_loiter_sw; // param_t rc_map_offboard_ctrl_mode_sw; @@ -534,11 +526,7 @@ Sensors::Sensors() : _parameter_handles.rc_map_flaps = param_find("RC_MAP_FLAPS"); /* optional mode switches, not mapped per default */ -<<<<<<< HEAD _parameter_handles.rc_map_easy_sw = param_find("RC_MAP_EASY_SW"); -======= - _parameter_handles.rc_map_assisted_sw = param_find("RC_MAP_ASSIST_SW"); ->>>>>>> ad77ba26427aa9a2d8b8241fc95271667a1c0863 _parameter_handles.rc_map_loiter_sw = param_find("RC_MAP_LOITER_SW"); // _parameter_handles.rc_map_offboard_ctrl_mode_sw = param_find("RC_MAP_OFFB_SW"); @@ -679,7 +667,7 @@ Sensors::parameters_update() } if (param_get(_parameter_handles.rc_map_failsafe, &(_parameters.rc_map_failsafe)) != OK) { - warnx(paramerr); + warnx("%s", paramerr); } if (param_get(_parameter_handles.rc_map_mode_sw, &(_parameters.rc_map_mode_sw)) != OK) { @@ -690,21 +678,12 @@ Sensors::parameters_update() warnx("%s", paramerr); } -<<<<<<< HEAD if (param_get(_parameter_handles.rc_map_easy_sw, &(_parameters.rc_map_easy_sw)) != OK) { - warnx(paramerr); - } - - if (param_get(_parameter_handles.rc_map_loiter_sw, &(_parameters.rc_map_loiter_sw)) != OK) { - warnx(paramerr); -======= - if (param_get(_parameter_handles.rc_map_assisted_sw, &(_parameters.rc_map_assisted_sw)) != OK) { warnx("%s", paramerr); } if (param_get(_parameter_handles.rc_map_loiter_sw, &(_parameters.rc_map_loiter_sw)) != OK) { warnx("%s", paramerr); ->>>>>>> ad77ba26427aa9a2d8b8241fc95271667a1c0863 } if (param_get(_parameter_handles.rc_map_flaps, &(_parameters.rc_map_flaps)) != OK) { @@ -745,11 +724,7 @@ Sensors::parameters_update() _rc.function[MODE] = _parameters.rc_map_mode_sw - 1; _rc.function[RETURN] = _parameters.rc_map_return_sw - 1; -<<<<<<< HEAD _rc.function[EASY] = _parameters.rc_map_easy_sw - 1; -======= - _rc.function[ASSISTED] = _parameters.rc_map_assisted_sw - 1; ->>>>>>> ad77ba26427aa9a2d8b8241fc95271667a1c0863 _rc.function[LOITER] = _parameters.rc_map_loiter_sw - 1; _rc.function[FLAPS] = _parameters.rc_map_flaps - 1; @@ -1502,17 +1477,10 @@ Sensors::rc_poll() manual.aux5 = get_rc_value(AUX_5, -1.0, 1.0); /* mode switches */ -<<<<<<< HEAD manual.mode_switch = get_rc_sw3pos_position(MODE, _parameters.rc_auto_th, _parameters.rc_auto_inv, _parameters.rc_assisted_th, _parameters.rc_assisted_inv); manual.easy_switch = get_rc_sw2pos_position(EASY, _parameters.rc_easy_th, _parameters.rc_easy_inv); manual.return_switch = get_rc_sw2pos_position(RETURN, _parameters.rc_return_th, _parameters.rc_return_inv); manual.loiter_switch = get_rc_sw2pos_position(LOITER, _parameters.rc_loiter_th, _parameters.rc_loiter_inv); -======= - manual.mode_switch = get_rc_switch_position(MODE); - manual.assisted_switch = get_rc_switch_position(ASSISTED); - manual.loiter_switch = get_rc_switch_position(LOITER); - manual.return_switch = get_rc_switch_position(RETURN); ->>>>>>> ad77ba26427aa9a2d8b8241fc95271667a1c0863 /* publish manual_control_setpoint topic */ if (_manual_control_pub > 0) { diff --git a/src/modules/uORB/topics/manual_control_setpoint.h b/src/modules/uORB/topics/manual_control_setpoint.h index c61987df62..b6257e22a6 100644 --- a/src/modules/uORB/topics/manual_control_setpoint.h +++ b/src/modules/uORB/topics/manual_control_setpoint.h @@ -78,11 +78,7 @@ struct manual_control_setpoint_s { switch_pos_t mode_switch; /**< mode 3 position switch (mandatory): manual, assisted, auto */ switch_pos_t return_switch; /**< land 2 position switch (mandatory): land, no effect */ -<<<<<<< HEAD switch_pos_t easy_switch; /**< assisted 2 position switch (optional): seatbelt, simple */ -======= - switch_pos_t assisted_switch; /**< assisted 2 position switch (optional): seatbelt, simple */ ->>>>>>> ad77ba26427aa9a2d8b8241fc95271667a1c0863 switch_pos_t loiter_switch; /**< mission 2 position switch (optional): mission, loiter */ }; /**< manual control inputs */ diff --git a/src/modules/uORB/topics/rc_channels.h b/src/modules/uORB/topics/rc_channels.h index df651e78d5..d99203ff65 100644 --- a/src/modules/uORB/topics/rc_channels.h +++ b/src/modules/uORB/topics/rc_channels.h @@ -64,11 +64,7 @@ enum RC_CHANNELS_FUNCTION { YAW = 3, MODE = 4, RETURN = 5, -<<<<<<< HEAD EASY = 6, -======= - ASSISTED = 6, ->>>>>>> ad77ba26427aa9a2d8b8241fc95271667a1c0863 LOITER = 7, OFFBOARD_MODE = 8, FLAPS = 9, From 269800b48c31d78fec900b4beaf3f655a8c18730 Mon Sep 17 00:00:00 2001 From: TickTock- Date: Sun, 27 Apr 2014 14:06:00 -0700 Subject: [PATCH 078/329] renamed EASY to POSHOLD and SEATBELT to ALTHOLD --- src/drivers/blinkm/blinkm.cpp | 4 +- src/modules/commander/commander.cpp | 46 +++++++++---------- .../state_machine_helper_test.cpp | 24 +++++----- src/modules/commander/px4_custom_mode.h | 4 +- .../commander/state_machine_helper.cpp | 4 +- src/modules/fixedwing_backside/fixedwing.cpp | 4 +- .../fw_pos_control_l1_main.cpp | 42 ++++++++--------- src/modules/mavlink/mavlink_messages.cpp | 8 ++-- .../mc_pos_control/mc_pos_control_params.c | 10 ++-- src/modules/segway/BlockSegwayController.cpp | 4 +- src/modules/sensors/sensor_params.c | 6 +-- src/modules/sensors/sensors.cpp | 26 +++++------ .../uORB/topics/manual_control_setpoint.h | 4 +- src/modules/uORB/topics/rc_channels.h | 2 +- src/modules/uORB/topics/vehicle_status.h | 4 +- 15 files changed, 96 insertions(+), 96 deletions(-) diff --git a/src/drivers/blinkm/blinkm.cpp b/src/drivers/blinkm/blinkm.cpp index b75c2297fa..aa9c645028 100644 --- a/src/drivers/blinkm/blinkm.cpp +++ b/src/drivers/blinkm/blinkm.cpp @@ -638,11 +638,11 @@ BlinkM::led() if(new_data_vehicle_control_mode || no_data_vehicle_control_mode < 3) { /* indicate main control state */ - if (vehicle_status_raw.main_state == MAIN_STATE_EASY) + if (vehicle_status_raw.main_state == MAIN_STATE_POSHOLD) led_color_4 = LED_GREEN; else if (vehicle_status_raw.main_state == MAIN_STATE_AUTO) led_color_4 = LED_BLUE; - else if (vehicle_status_raw.main_state == MAIN_STATE_SEATBELT) + else if (vehicle_status_raw.main_state == MAIN_STATE_ALTHOLD) led_color_4 = LED_YELLOW; else if (vehicle_status_raw.main_state == MAIN_STATE_MANUAL) led_color_4 = LED_WHITE; diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 50380107d0..a99456370c 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -434,13 +434,13 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe /* MANUAL */ main_res = main_state_transition(status, MAIN_STATE_MANUAL); - } else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_SEATBELT) { - /* SEATBELT */ - main_res = main_state_transition(status, MAIN_STATE_SEATBELT); + } else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_ALTHOLD) { + /* ALTHOLD */ + main_res = main_state_transition(status, MAIN_STATE_ALTHOLD); - } else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_EASY) { - /* EASY */ - main_res = main_state_transition(status, MAIN_STATE_EASY); + } else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_POSHOLD) { + /* POSHOLD */ + main_res = main_state_transition(status, MAIN_STATE_POSHOLD); } else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_AUTO) { /* AUTO */ @@ -455,8 +455,8 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe } 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); + /* POSHOLD */ + main_res = main_state_transition(status, MAIN_STATE_POSHOLD); } else if (base_mode & MAV_MODE_FLAG_STABILIZE_ENABLED) { /* MANUAL */ @@ -628,8 +628,8 @@ int commander_thread_main(int argc, char *argv[]) char *main_states_str[MAIN_STATE_MAX]; main_states_str[0] = "MANUAL"; - main_states_str[1] = "SEATBELT"; - main_states_str[2] = "EASY"; + main_states_str[1] = "ALTHOLD"; + main_states_str[2] = "POSHOLD"; main_states_str[3] = "AUTO"; char *arming_states_str[ARMING_STATE_MAX]; @@ -1299,8 +1299,8 @@ int commander_thread_main(int argc, char *argv[]) } // TODO remove this hack - /* flight termination in manual mode if assisted switch is on easy position */ - if (!status.is_rotary_wing && parachute_enabled && armed.armed && status.main_state == MAIN_STATE_MANUAL && sp_man.easy_switch == SWITCH_POS_ON) { + /* flight termination in manual mode if assisted switch is on poshold position */ + if (!status.is_rotary_wing && parachute_enabled && armed.armed && status.main_state == MAIN_STATE_MANUAL && sp_man.poshold_switch == SWITCH_POS_ON) { if (TRANSITION_CHANGED == failsafe_state_transition(&status, FAILSAFE_STATE_TERMINATION)) { tune_positive(armed.armed); } @@ -1557,25 +1557,25 @@ set_main_state_rc(struct vehicle_status_s *status, struct manual_control_setpoin break; case SWITCH_POS_MIDDLE: // ASSISTED - if (sp_man->easy_switch == SWITCH_POS_ON) { - res = main_state_transition(status, MAIN_STATE_EASY); + if (sp_man->poshold_switch == SWITCH_POS_ON) { + res = main_state_transition(status, MAIN_STATE_POSHOLD); if (res != TRANSITION_DENIED) { break; // changed successfully or already in this state } - // else fallback to SEATBELT - print_reject_mode(status, "EASY"); + // else fallback to ALTHOLD + print_reject_mode(status, "POSHOLD"); } - res = main_state_transition(status, MAIN_STATE_SEATBELT); + res = main_state_transition(status, MAIN_STATE_ALTHOLD); if (res != TRANSITION_DENIED) { break; // changed successfully or already in this mode } - if (sp_man->easy_switch != SWITCH_POS_ON) { - print_reject_mode(status, "SEATBELT"); + if (sp_man->poshold_switch != SWITCH_POS_ON) { + print_reject_mode(status, "ALTHOLD"); } // else fallback to MANUAL @@ -1590,9 +1590,9 @@ set_main_state_rc(struct vehicle_status_s *status, struct manual_control_setpoin break; // changed successfully or already in this state } - // else fallback to SEATBELT (EASY likely will not work too) + // else fallback to ALTHOLD (POSHOLD likely will not work too) print_reject_mode(status, "AUTO"); - res = main_state_transition(status, MAIN_STATE_SEATBELT); + res = main_state_transition(status, MAIN_STATE_ALTHOLD); if (res != TRANSITION_DENIED) { break; // changed successfully or already in this state @@ -1638,7 +1638,7 @@ set_control_mode() control_mode.flag_control_velocity_enabled = false; break; - case MAIN_STATE_SEATBELT: + case MAIN_STATE_ALTHOLD: control_mode.flag_control_manual_enabled = true; control_mode.flag_control_auto_enabled = false; control_mode.flag_control_rates_enabled = true; @@ -1649,7 +1649,7 @@ set_control_mode() control_mode.flag_control_velocity_enabled = false; break; - case MAIN_STATE_EASY: + case MAIN_STATE_POSHOLD: control_mode.flag_control_manual_enabled = true; control_mode.flag_control_auto_enabled = false; control_mode.flag_control_rates_enabled = true; diff --git a/src/modules/commander/commander_tests/state_machine_helper_test.cpp b/src/modules/commander/commander_tests/state_machine_helper_test.cpp index 8a946543d1..b440e561b5 100644 --- a/src/modules/commander/commander_tests/state_machine_helper_test.cpp +++ b/src/modules/commander/commander_tests/state_machine_helper_test.cpp @@ -317,34 +317,34 @@ bool StateMachineHelperTest::mainStateTransitionTest(void) TRANSITION_CHANGED == main_state_transition(¤t_state, new_main_state)); ut_assert("new state: manual", MAIN_STATE_MANUAL == current_state.main_state); - // MANUAL to SEATBELT. + // MANUAL to ALTHOLD. current_state.main_state = MAIN_STATE_MANUAL; current_state.condition_local_altitude_valid = true; - new_main_state = MAIN_STATE_SEATBELT; - ut_assert("tranisition: manual to seatbelt", + new_main_state = MAIN_STATE_ALTHOLD; + ut_assert("tranisition: manual to althold", TRANSITION_CHANGED == main_state_transition(¤t_state, new_main_state)); - ut_assert("new state: seatbelt", MAIN_STATE_SEATBELT == current_state.main_state); + ut_assert("new state: althold", MAIN_STATE_ALTHOLD == current_state.main_state); - // MANUAL to SEATBELT, invalid local altitude. + // MANUAL to ALTHOLD, invalid local altitude. current_state.main_state = MAIN_STATE_MANUAL; current_state.condition_local_altitude_valid = false; - new_main_state = MAIN_STATE_SEATBELT; + new_main_state = MAIN_STATE_ALTHOLD; ut_assert("no transition: invalid local altitude", TRANSITION_DENIED == main_state_transition(¤t_state, new_main_state)); ut_assert("current state: manual", MAIN_STATE_MANUAL == current_state.main_state); - // MANUAL to EASY. + // MANUAL to POSHOLD. current_state.main_state = MAIN_STATE_MANUAL; current_state.condition_local_position_valid = true; - new_main_state = MAIN_STATE_EASY; - ut_assert("transition: manual to easy", + new_main_state = MAIN_STATE_POSHOLD; + ut_assert("transition: manual to poshold", TRANSITION_CHANGED == main_state_transition(¤t_state, new_main_state)); - ut_assert("current state: easy", MAIN_STATE_EASY == current_state.main_state); + ut_assert("current state: poshold", MAIN_STATE_POSHOLD == current_state.main_state); - // MANUAL to EASY, invalid local position. + // MANUAL to POSHOLD, invalid local position. current_state.main_state = MAIN_STATE_MANUAL; current_state.condition_local_position_valid = false; - new_main_state = MAIN_STATE_EASY; + new_main_state = MAIN_STATE_POSHOLD; ut_assert("no transition: invalid position", TRANSITION_DENIED == main_state_transition(¤t_state, new_main_state)); ut_assert("current state: manual", MAIN_STATE_MANUAL == current_state.main_state); diff --git a/src/modules/commander/px4_custom_mode.h b/src/modules/commander/px4_custom_mode.h index 2144d34607..962d2804cc 100644 --- a/src/modules/commander/px4_custom_mode.h +++ b/src/modules/commander/px4_custom_mode.h @@ -12,8 +12,8 @@ enum PX4_CUSTOM_MAIN_MODE { PX4_CUSTOM_MAIN_MODE_MANUAL = 1, - PX4_CUSTOM_MAIN_MODE_SEATBELT, - PX4_CUSTOM_MAIN_MODE_EASY, + PX4_CUSTOM_MAIN_MODE_ALTHOLD, + PX4_CUSTOM_MAIN_MODE_POSHOLD, PX4_CUSTOM_MAIN_MODE_AUTO, }; diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index f09d586c74..21e36a87df 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -215,7 +215,7 @@ main_state_transition(struct vehicle_status_s *status, main_state_t new_main_sta ret = TRANSITION_CHANGED; break; - case MAIN_STATE_SEATBELT: + case MAIN_STATE_ALTHOLD: /* need at minimum altitude estimate */ if (!status->is_rotary_wing || @@ -226,7 +226,7 @@ main_state_transition(struct vehicle_status_s *status, main_state_t new_main_sta break; - case MAIN_STATE_EASY: + case MAIN_STATE_POSHOLD: /* need at minimum local position estimate */ if (status->condition_local_position_valid || diff --git a/src/modules/fixedwing_backside/fixedwing.cpp b/src/modules/fixedwing_backside/fixedwing.cpp index cfae072752..fafab9bfec 100644 --- a/src/modules/fixedwing_backside/fixedwing.cpp +++ b/src/modules/fixedwing_backside/fixedwing.cpp @@ -229,8 +229,8 @@ void BlockMultiModeBacksideAutopilot::update() _actuators.control[CH_RDR] = _manual.yaw; _actuators.control[CH_THR] = _manual.throttle; - } else if (_status.main_state == MAIN_STATE_SEATBELT || - _status.main_state == MAIN_STATE_EASY /* TODO, implement easy */) { + } else if (_status.main_state == MAIN_STATE_ALTHOLD || + _status.main_state == MAIN_STATE_POSHOLD /* TODO, implement easy */) { // calculate velocity, XXX should be airspeed, but using ground speed for now // for the purpose of control we will limit the velocity feedback between diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index 7f13df7854..d5a68e21f5 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -153,7 +153,7 @@ private: bool _setpoint_valid; /**< flag if the position control setpoint is valid */ /** manual control states */ - float _seatbelt_hold_heading; /**< heading the system should hold in seatbelt mode */ + float _althold_hold_heading; /**< heading the system should hold in althold mode */ double _loiter_hold_lat; double _loiter_hold_lon; float _loiter_hold_alt; @@ -1051,16 +1051,16 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi _att_sp.roll_reset_integral = true; } - } else if (0/* easy mode enabled */) { + } else if (0/* poshold mode enabled */) { - /** EASY FLIGHT **/ + /** POSHOLD FLIGHT **/ - if (0/* switched from another mode to easy */) { - _seatbelt_hold_heading = _att.yaw; + if (0/* switched from another mode to poshold */) { + _althold_hold_heading = _att.yaw; } - if (0/* easy on and manual control yaw non-zero */) { - _seatbelt_hold_heading = _att.yaw + _manual.yaw; + if (0/* poshold on and manual control yaw non-zero */) { + _althold_hold_heading = _att.yaw + _manual.yaw; } //XXX not used @@ -1073,39 +1073,39 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi // climb_out = true; // } - /* if in seatbelt mode, set airspeed based on manual control */ + /* if in althold mode, set airspeed based on manual control */ // XXX check if ground speed undershoot should be applied here - float seatbelt_airspeed = _parameters.airspeed_min + + float althold_airspeed = _parameters.airspeed_min + (_parameters.airspeed_max - _parameters.airspeed_min) * _manual.throttle; - _l1_control.navigate_heading(_seatbelt_hold_heading, _att.yaw, ground_speed); + _l1_control.navigate_heading(_althold_hold_heading, _att.yaw, ground_speed); _att_sp.roll_body = _l1_control.nav_roll(); _att_sp.yaw_body = _l1_control.nav_bearing(); _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _global_pos.alt + _manual.pitch * 2.0f, - seatbelt_airspeed, + althold_airspeed, _airspeed.indicated_airspeed_m_s, eas2tas, false, _parameters.pitch_limit_min, _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, _parameters.pitch_limit_min, _parameters.pitch_limit_max); - } else if (0/* seatbelt mode enabled */) { + } else if (0/* althold mode enabled */) { - /** SEATBELT FLIGHT **/ + /** ALTHOLD FLIGHT **/ - if (0/* switched from another mode to seatbelt */) { - _seatbelt_hold_heading = _att.yaw; + if (0/* switched from another mode to althold */) { + _althold_hold_heading = _att.yaw; } - if (0/* seatbelt on and manual control yaw non-zero */) { - _seatbelt_hold_heading = _att.yaw + _manual.yaw; + if (0/* althold on and manual control yaw non-zero */) { + _althold_hold_heading = _att.yaw + _manual.yaw; } - /* if in seatbelt mode, set airspeed based on manual control */ + /* if in althold mode, set airspeed based on manual control */ // XXX check if ground speed undershoot should be applied here - float seatbelt_airspeed = _parameters.airspeed_min + + float althold_airspeed = _parameters.airspeed_min + (_parameters.airspeed_max - _parameters.airspeed_min) * _manual.throttle; @@ -1124,11 +1124,11 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi climb_out = true; } - _l1_control.navigate_heading(_seatbelt_hold_heading, _att.yaw, ground_speed); + _l1_control.navigate_heading(_althold_hold_heading, _att.yaw, ground_speed); _att_sp.roll_body = _manual.roll; _att_sp.yaw_body = _manual.yaw; _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _global_pos.alt + _manual.pitch * 2.0f, - seatbelt_airspeed, + althold_airspeed, _airspeed.indicated_airspeed_m_s, eas2tas, climb_out, _parameters.pitch_limit_min, _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 678ce16455..108ef8ad46 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -124,13 +124,13 @@ void get_mavlink_mode_state(struct vehicle_status_s *status, struct position_set *mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | (status->is_rotary_wing ? MAV_MODE_FLAG_STABILIZE_ENABLED : 0); custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_MANUAL; - } else if (status->main_state == MAIN_STATE_SEATBELT) { + } else if (status->main_state == MAIN_STATE_ALTHOLD) { *mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED; - custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_SEATBELT; + custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_ALTHOLD; - } else if (status->main_state == MAIN_STATE_EASY) { + } else if (status->main_state == MAIN_STATE_POSHOLD) { *mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED; - custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_EASY; + custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_POSHOLD; } else if (status->main_state == MAIN_STATE_AUTO) { *mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED; diff --git a/src/modules/mc_pos_control/mc_pos_control_params.c b/src/modules/mc_pos_control/mc_pos_control_params.c index 104df4e59b..015d8ad160 100644 --- a/src/modules/mc_pos_control/mc_pos_control_params.c +++ b/src/modules/mc_pos_control/mc_pos_control_params.c @@ -98,7 +98,7 @@ PARAM_DEFINE_FLOAT(MPC_Z_VEL_D, 0.0f); /** * Maximum vertical velocity * - * Maximum vertical velocity in AUTO mode and endpoint for stabilized modes (SEATBELT, EASY). + * Maximum vertical velocity in AUTO mode and endpoint for stabilized modes (ALTHOLD, POSHOLD). * * @unit m/s * @min 0.0 @@ -109,7 +109,7 @@ PARAM_DEFINE_FLOAT(MPC_Z_VEL_MAX, 5.0f); /** * Vertical velocity feed forward * - * Feed forward weight for altitude control in stabilized modes (SEATBELT, EASY). 0 will give slow responce and no overshot, 1 - fast responce and big overshot. + * Feed forward weight for altitude control in stabilized modes (ALTHOLD, POSHOLD). 0 will give slow responce and no overshot, 1 - fast responce and big overshot. * * @min 0.0 * @max 1.0 @@ -154,7 +154,7 @@ PARAM_DEFINE_FLOAT(MPC_XY_VEL_D, 0.01f); /** * Maximum horizontal velocity * - * Maximum horizontal velocity in AUTO mode and endpoint for position stabilized mode (EASY). + * Maximum horizontal velocity in AUTO mode and endpoint for position stabilized mode (POSHOLD). * * @unit m/s * @min 0.0 @@ -165,7 +165,7 @@ PARAM_DEFINE_FLOAT(MPC_XY_VEL_MAX, 5.0f); /** * Horizontal velocity feed forward * - * Feed forward weight for position control in position control mode (EASY). 0 will give slow responce and no overshot, 1 - fast responce and big overshot. + * Feed forward weight for position control in position control mode (POSHOLD). 0 will give slow responce and no overshot, 1 - fast responce and big overshot. * * @min 0.0 * @max 1.0 @@ -176,7 +176,7 @@ PARAM_DEFINE_FLOAT(MPC_XY_FF, 0.5f); /** * Maximum tilt angle in air * - * Limits maximum tilt in AUTO and EASY modes during flight. + * Limits maximum tilt in AUTO and POSHOLD modes during flight. * * @unit deg * @min 0.0 diff --git a/src/modules/segway/BlockSegwayController.cpp b/src/modules/segway/BlockSegwayController.cpp index 96a443c6ea..2cb4fc900f 100644 --- a/src/modules/segway/BlockSegwayController.cpp +++ b/src/modules/segway/BlockSegwayController.cpp @@ -35,8 +35,8 @@ void BlockSegwayController::update() { // handle autopilot modes if (_status.main_state == MAIN_STATE_AUTO || - _status.main_state == MAIN_STATE_SEATBELT || - _status.main_state == MAIN_STATE_EASY) { + _status.main_state == MAIN_STATE_ALTHOLD || + _status.main_state == MAIN_STATE_POSHOLD) { _actuators.control[0] = spdCmd; _actuators.control[1] = spdCmd; diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c index dd6e21bddd..59bd99db78 100644 --- a/src/modules/sensors/sensor_params.c +++ b/src/modules/sensors/sensor_params.c @@ -605,7 +605,7 @@ PARAM_DEFINE_INT32(RC_MAP_RETURN_SW, 0); * @max 18 * @group Radio Calibration */ -PARAM_DEFINE_INT32(RC_MAP_EASY_SW, 0); +PARAM_DEFINE_INT32(RC_MAP_POSHLD_SW, 0); /** * Loiter switch channel mapping. @@ -703,7 +703,7 @@ PARAM_DEFINE_FLOAT(RC_ASSISTED_TH, 0.25f); PARAM_DEFINE_FLOAT(RC_AUTO_TH, 0.75f); /** - * Threshold for selecting easy mode + * Threshold for selecting poshold mode * * min:-1 * max:+1 @@ -716,7 +716,7 @@ PARAM_DEFINE_FLOAT(RC_AUTO_TH, 0.75f); * negative : true when channel Date: Mon, 28 Apr 2014 11:41:40 +0200 Subject: [PATCH 079/329] Reset init flags as well --- src/modules/ekf_att_pos_estimator/estimator.cpp | 13 +++++++++++++ 1 file changed, 13 insertions(+) diff --git a/src/modules/ekf_att_pos_estimator/estimator.cpp b/src/modules/ekf_att_pos_estimator/estimator.cpp index 14761831cf..6eccc4c4d2 100644 --- a/src/modules/ekf_att_pos_estimator/estimator.cpp +++ b/src/modules/ekf_att_pos_estimator/estimator.cpp @@ -2407,6 +2407,19 @@ void AttPosEKF::InitializeDynamic(float (&initvelNED)[3]) // Clear the init flag statesInitialised = false; + // Clear other flags, waiting for new data + fusionModeGPS = 0; + fuseVelData = false; + fusePosData = false; + fuseHgtData = false; + fuseMagData = false; + fuseVtasData = false; + // onGround(true), + // staticMode(true), + useAirspeed = true; + useCompass = true; + useRangeFinder = true; + ZeroVariables(); // Calculate initial filter quaternion states from raw measurements From a4724acf82e48d914bfe6397340454505e14decf Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Mon, 28 Apr 2014 17:26:42 +0200 Subject: [PATCH 080/329] autostart 10016_3dr_iris: yaw PID parameters updated --- ROMFS/px4fmu_common/init.d/10016_3dr_iris | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/10016_3dr_iris b/ROMFS/px4fmu_common/init.d/10016_3dr_iris index f11aa704eb..084dff140f 100644 --- a/ROMFS/px4fmu_common/init.d/10016_3dr_iris +++ b/ROMFS/px4fmu_common/init.d/10016_3dr_iris @@ -18,9 +18,9 @@ then param set MC_PITCHRATE_P 0.13 param set MC_PITCHRATE_I 0.0 param set MC_PITCHRATE_D 0.004 - param set MC_YAW_P 0.5 - param set MC_YAWRATE_P 0.2 - param set MC_YAWRATE_I 0.0 + param set MC_YAW_P 2.5 + param set MC_YAWRATE_P 0.25 + param set MC_YAWRATE_I 0.25 param set MC_YAWRATE_D 0.0 param set BAT_V_SCALING 0.00989 From e134537ae8f892ba3ae4a2f9c771bcfa62f905c8 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 28 Apr 2014 17:44:29 +0200 Subject: [PATCH 081/329] Added automatic declination lookup --- src/lib/geo/geo.h | 2 + src/lib/geo/geo_mag_declination.c | 135 ++++++++++++++++++++++++++++++ src/lib/geo/geo_mag_declination.h | 47 +++++++++++ src/lib/geo/module.mk | 3 +- 4 files changed, 186 insertions(+), 1 deletion(-) create mode 100644 src/lib/geo/geo_mag_declination.c create mode 100644 src/lib/geo/geo_mag_declination.h diff --git a/src/lib/geo/geo.h b/src/lib/geo/geo.h index 0a3f85d970..d987afe337 100644 --- a/src/lib/geo/geo.h +++ b/src/lib/geo/geo.h @@ -48,6 +48,8 @@ #include "uORB/topics/fence.h" #include "uORB/topics/vehicle_global_position.h" +#include "geo/geo_mag_declination.h" + __BEGIN_DECLS #include diff --git a/src/lib/geo/geo_mag_declination.c b/src/lib/geo/geo_mag_declination.c new file mode 100644 index 0000000000..7b4aa69a2f --- /dev/null +++ b/src/lib/geo/geo_mag_declination.c @@ -0,0 +1,135 @@ +/**************************************************************************** + * + * Copyright (c) 2014 MAV GEO Library (MAVGEO). 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 MAVGEO 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 geo_mag_declination.c +* +* Calculation / lookup table for earth magnetic field declination. +* +* Lookup table from Scott Ferguson +* +* XXX Lookup table currently too coarse in resolution (only full degrees) +* and lat/lon res - needs extension medium term. +* +*/ + + +/** set this always to the sampling in degrees for the table below */ +#define SAMPLING_RES 10 +#define SAMPLING_MIN_LAT -60.0f +#define SAMPLING_MAX_LAT 60.0f +#define SAMPLING_MIN_LON -180.0f +#define SAMPLING_MAX_LON 180.0f + +static const char declination_table[13][37] = \ +{ + 46, 45, 44, 42, 41, 40, 38, 36, 33, 28, 23, 16, 10, 4, -1, -5, -9, -14, -19, -26, -33, -40, -48, -55, -61, \ + -66, -71, -74, -75, -72, -61, -25, 22, 40, 45, 47, 46, 30, 30, 30, 30, 29, 29, 29, 29, 27, 24, 18, 11, 3, \ + -3, -9, -12, -15, -17, -21, -26, -32, -39, -45, -51, -55, -57, -56, -53, -44, -31, -14, 0, 13, 21, 26, \ + 29, 30, 21, 22, 22, 22, 22, 22, 22, 22, 21, 18, 13, 5, -3, -11, -17, -20, -21, -22, -23, -25, -29, -35, \ + -40, -44, -45, -44, -40, -32, -22, -12, -3, 3, 9, 14, 18, 20, 21, 16, 17, 17, 17, 17, 17, 16, 16, 16, 13, \ + 8, 0, -9, -16, -21, -24, -25, -25, -23, -20, -21, -24, -28, -31, -31, -29, -24, -17, -9, -3, 0, 4, 7, \ + 10, 13, 15, 16, 12, 13, 13, 13, 13, 13, 12, 12, 11, 9, 3, -4, -12, -19, -23, -24, -24, -22, -17, -12, -9, \ + -10, -13, -17, -18, -16, -13, -8, -3, 0, 1, 3, 6, 8, 10, 12, 12, 10, 10, 10, 10, 10, 10, 10, 9, 9, 6, 0, -6, \ + -14, -20, -22, -22, -19, -15, -10, -6, -2, -2, -4, -7, -8, -8, -7, -4, 0, 1, 1, 2, 4, 6, 8, 10, 10, 9, 9, 9, \ + 9, 9, 9, 8, 8, 7, 4, -1, -8, -15, -19, -20, -18, -14, -9, -5, -2, 0, 1, 0, -2, -3, -4, -3, -2, 0, 0, 0, 1, 3, 5, \ + 7, 8, 9, 8, 8, 8, 9, 9, 9, 8, 8, 6, 2, -3, -9, -15, -18, -17, -14, -10, -6, -2, 0, 1, 2, 2, 0, -1, -1, -2, -1, 0, \ + 0, 0, 0, 1, 3, 5, 7, 8, 8, 9, 9, 10, 10, 10, 10, 8, 5, 0, -5, -11, -15, -16, -15, -12, -8, -4, -1, 0, 2, 3, 2, 1, 0, \ + 0, 0, 0, 0, -1, -2, -2, -1, 0, 3, 6, 8, 6, 9, 10, 11, 12, 12, 11, 9, 5, 0, -7, -12, -15, -15, -13, -10, -7, -3, \ + 0, 1, 2, 3, 3, 3, 2, 1, 0, 0, -1, -3, -4, -5, -5, -2, 0, 3, 6, 5, 8, 11, 13, 15, 15, 14, 11, 5, -1, -9, -14, -17, \ + -16, -14, -11, -7, -3, 0, 1, 3, 4, 5, 5, 5, 4, 3, 1, -1, -4, -7, -8, -8, -6, -2, 1, 5, 4, 8, 12, 15, 17, 18, 16, \ + 12, 5, -3, -12, -18, -20, -19, -16, -13, -8, -4, -1, 1, 4, 6, 8, 9, 9, 9, 7, 3, -1, -6, -10, -12, -11, -9, -5, \ + 0, 4, 3, 9, 14, 17, 20, 21, 19, 14, 4, -8, -19, -25, -26, -25, -21, -17, -12, -7, -2, 1, 5, 9, 13, 15, 16, 16, \ + 13, 7, 0, -7, -12, -15, -14, -11, -6, -1, 3 +}; + +static float get_lookup_table_val(unsigned lat, unsigned lon); + +__EXPORT float get_mag_declination(float lat, float lon) +{ + /* + * If the values exceed valid ranges, return zero as default + * as we have no way of knowing what the closest real value + * would be. + */ + if (lat < -90.0f || lat > 90.0f || + lon < -180.0f || lon > 180.0f) { + return 0.0f; + } + + /* round down to nearest sampling resolution */ + int min_lat = (((int)lat) / SAMPLING_RES) * SAMPLING_RES; + int min_lon = (((int)lon) / SAMPLING_RES) * SAMPLING_RES; + + /* for the rare case of hitting the bounds exactly + * the rounding logic wouldn't fit, so enforce it. + */ + + /* limit to table bounds - required for maxima even when table spans full globe range */ + if (lat <= SAMPLING_MIN_LAT) { + min_lat = SAMPLING_MIN_LAT; + } + + if (lat >= SAMPLING_MAX_LAT) { + min_lat = (((int)lat) / SAMPLING_RES) * SAMPLING_RES - SAMPLING_RES; + } + + if (lon <= SAMPLING_MIN_LON) { + min_lon = SAMPLING_MIN_LON; + } + + if (lon >= SAMPLING_MAX_LON) { + min_lon = (((int)lon) / SAMPLING_RES) * SAMPLING_RES - SAMPLING_RES; + } + + /* find index of nearest low sampling point */ + unsigned min_lat_index = (90 + min_lat) / SAMPLING_RES; + unsigned min_lon_index = (180 + min_lat) / SAMPLING_RES; + + float declination_sw = get_lookup_table_val(min_lat_index, min_lon_index); + float declination_se = get_lookup_table_val(min_lat_index, min_lon_index + 1); + float declination_ne = get_lookup_table_val(min_lat_index + 1, min_lon_index + 1); + float declination_nw = get_lookup_table_val(min_lat_index + 1, min_lon_index); + + /* perform bilinear interpolation on the four grid corners */ + + float declination_min = ((lon - min_lon) / SAMPLING_RES) * (declination_se - declination_sw) + declination_sw; + float declination_max = ((lon - min_lon) / SAMPLING_RES) * (declination_ne - declination_nw) + declination_nw; + + return (lat - min_lat) / SAMPLING_RES * (declination_max - declination_min) + declination_min; +} + +float get_lookup_table_val(unsigned lat_index, unsigned lon_index) +{ + return declination_table[lat_index][lon_index]; +} \ No newline at end of file diff --git a/src/lib/geo/geo_mag_declination.h b/src/lib/geo/geo_mag_declination.h new file mode 100644 index 0000000000..0ac062d6d4 --- /dev/null +++ b/src/lib/geo/geo_mag_declination.h @@ -0,0 +1,47 @@ +/**************************************************************************** + * + * Copyright (c) 2014 MAV GEO Library (MAVGEO). 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 MAVGEO 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 geo_mag_declination.h +* +* Calculation / lookup table for earth magnetic field declination. +* +*/ + +#pragma once + +__BEGIN_DECLS + +__EXPORT float get_mag_declination(float lat, float lon); + +__END_DECLS diff --git a/src/lib/geo/module.mk b/src/lib/geo/module.mk index 30a2dc99fd..9500a2bccf 100644 --- a/src/lib/geo/module.mk +++ b/src/lib/geo/module.mk @@ -35,4 +35,5 @@ # Geo library # -SRCS = geo.c +SRCS = geo.c \ + geo_mag_declination.c From 90569d22bcee65b102c9f55be71806d301829cee Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 28 Apr 2014 18:34:33 +0200 Subject: [PATCH 082/329] Added support for automatic mag declination setup --- .../ekf_att_pos_estimator/estimator.cpp | 33 +++++++++++++------ src/modules/ekf_att_pos_estimator/estimator.h | 7 ++-- .../fw_att_pos_estimator_main.cpp | 13 +++++--- 3 files changed, 35 insertions(+), 18 deletions(-) diff --git a/src/modules/ekf_att_pos_estimator/estimator.cpp b/src/modules/ekf_att_pos_estimator/estimator.cpp index 8ac2d61541..1ca34ec307 100644 --- a/src/modules/ekf_att_pos_estimator/estimator.cpp +++ b/src/modules/ekf_att_pos_estimator/estimator.cpp @@ -158,7 +158,8 @@ AttPosEKF::AttPosEKF() : gpsHgt(0.0f), baroHgt(0.0f), GPSstatus(0), - VtasMeas(0.0f) + VtasMeas(0.0f), + magDeclination(0.0f) { velNED[0] = 0.0f; velNED[1] = 0.0f; @@ -2341,7 +2342,7 @@ int AttPosEKF::CheckAndBound() if (StatesNaN(&last_ekf_error)) { ekf_debug("re-initializing dynamic"); - InitializeDynamic(velNED); + InitializeDynamic(velNED, magDeclination); return 1; } @@ -2374,7 +2375,7 @@ int AttPosEKF::CheckAndBound() return 0; } -void AttPosEKF::AttitudeInit(float ax, float ay, float az, float mx, float my, float mz, float *initQuat) +void AttPosEKF::AttitudeInit(float ax, float ay, float az, float mx, float my, float mz, float declination, float *initQuat) { float initialRoll, initialPitch; float cosRoll, sinRoll, cosPitch, sinPitch; @@ -2394,6 +2395,8 @@ void AttPosEKF::AttitudeInit(float ax, float ay, float az, float mx, float my, f magY = my * cosRoll - mz * sinRoll; initialHdg = atan2f(-magY, magX); + /* true heading is the mag heading minus declination */ + initialHdg -= declination; cosRoll = cosf(initialRoll * 0.5f); sinRoll = sinf(initialRoll * 0.5f); @@ -2408,9 +2411,17 @@ void AttPosEKF::AttitudeInit(float ax, float ay, float az, float mx, float my, f initQuat[1] = sinRoll * cosPitch * cosHeading - cosRoll * sinPitch * sinHeading; initQuat[2] = cosRoll * sinPitch * cosHeading + sinRoll * cosPitch * sinHeading; initQuat[3] = cosRoll * cosPitch * sinHeading - sinRoll * sinPitch * cosHeading; + + /* normalize */ + float norm = sqrtf(initQuat[0]*initQuat[0] + initQuat[1]*initQuat[1] + initQuat[2]*initQuat[2] + initQuat[3]*initQuat[3]); + + initQuat[0] /= norm; + initQuat[1] /= norm; + initQuat[2] /= norm; + initQuat[3] /= norm; } -void AttPosEKF::InitializeDynamic(float (&initvelNED)[3]) +void AttPosEKF::InitializeDynamic(float (&initvelNED)[3], float declination) { // Clear the init flag @@ -2431,11 +2442,13 @@ void AttPosEKF::InitializeDynamic(float (&initvelNED)[3]) ZeroVariables(); + magDeclination = declination; + // Calculate initial filter quaternion states from raw measurements float initQuat[4]; Vector3f initMagXYZ; initMagXYZ = magData - magBias; - AttitudeInit(accel.x, accel.y, accel.z, initMagXYZ.x, initMagXYZ.y, initMagXYZ.z, initQuat); + AttitudeInit(accel.x, accel.y, accel.z, initMagXYZ.x, initMagXYZ.y, initMagXYZ.z, declination, initQuat); // Calculate initial Tbn matrix and rotate Mag measurements into NED // to set initial NED magnetic field states @@ -2451,9 +2464,9 @@ void AttPosEKF::InitializeDynamic(float (&initvelNED)[3]) magstate.q1 = initQuat[1]; magstate.q2 = initQuat[2]; magstate.q3 = initQuat[3]; - magstate.magN = magData.x; - magstate.magE = magData.y; - magstate.magD = magData.z; + magstate.magN = initMagNED.x; + magstate.magE = initMagNED.y; + magstate.magD = initMagNED.z; magstate.magXbias = magBias.x; magstate.magYbias = magBias.y; magstate.magZbias = magBias.z; @@ -2493,7 +2506,7 @@ void AttPosEKF::InitializeDynamic(float (&initvelNED)[3]) summedDelVel.z = 0.0f; } -void AttPosEKF::InitialiseFilter(float (&initvelNED)[3], double referenceLat, double referenceLon, float referenceHgt) +void AttPosEKF::InitialiseFilter(float (&initvelNED)[3], double referenceLat, double referenceLon, float referenceHgt, float declination) { //store initial lat,long and height latRef = referenceLat; @@ -2503,7 +2516,7 @@ void AttPosEKF::InitialiseFilter(float (&initvelNED)[3], double referenceLat, do memset(&last_ekf_error, 0, sizeof(last_ekf_error)); - InitializeDynamic(initvelNED); + InitializeDynamic(initvelNED, declination); } void AttPosEKF::ZeroVariables() diff --git a/src/modules/ekf_att_pos_estimator/estimator.h b/src/modules/ekf_att_pos_estimator/estimator.h index e118ae5734..378107b698 100644 --- a/src/modules/ekf_att_pos_estimator/estimator.h +++ b/src/modules/ekf_att_pos_estimator/estimator.h @@ -208,6 +208,7 @@ public: float innovRng; ///< Range finder innovation float varInnovVtas; // innovation variance output float VtasMeas; // true airspeed measurement (m/s) + float magDeclination; ///< magnetic declination double latRef; // WGS-84 latitude of reference point (rad) double lonRef; // WGS-84 longitude of reference point (rad) float hgtRef; // WGS-84 height of reference point (m) @@ -309,7 +310,7 @@ void OnGroundCheck(); void CovarianceInit(); -void InitialiseFilter(float (&initvelNED)[3], double referenceLat, double referenceLon, float referenceHgt); +void InitialiseFilter(float (&initvelNED)[3], double referenceLat, double referenceLon, float referenceHgt, float declination); float ConstrainFloat(float val, float min, float max); @@ -334,7 +335,7 @@ void GetLastErrorState(struct ekf_status_report *last_error); bool StatesNaN(struct ekf_status_report *err_report); void FillErrorReport(struct ekf_status_report *err); -void InitializeDynamic(float (&initvelNED)[3]); +void InitializeDynamic(float (&initvelNED)[3], float declination); protected: @@ -342,7 +343,7 @@ bool FilterHealthy(); void ResetHeight(void); -void AttitudeInit(float ax, float ay, float az, float mx, float my, float mz, float *initQuat); +void AttitudeInit(float ax, float ay, float az, float mx, float my, float mz, float declination, float *initQuat); }; diff --git a/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp index a82e8d7044..23f9e80bd8 100644 --- a/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp @@ -994,11 +994,14 @@ FixedwingEstimator::task_main() _ekf->velNED[1] = _gps.vel_e_m_s; _ekf->velNED[2] = _gps.vel_d_m_s; - _ekf->gpsLat = math::radians(_gps.lat / (double)1e7); - _ekf->gpsLon = math::radians(_gps.lon / (double)1e7) - M_PI; - _ekf->gpsHgt = _gps.alt / 1e3f; + _ekf->gpsLat = math::radians(lat); + _ekf->gpsLon = math::radians(lon) - M_PI; + _ekf->gpsHgt = gps_alt; - _ekf->InitialiseFilter(_ekf->velNED, math::radians(lat), math::radians(lon) - M_PI, gps_alt); + // Look up mag declination based on current position + float declination = math::radians(get_mag_declination(lat, lon)); + + _ekf->InitialiseFilter(_ekf->velNED, math::radians(lat), math::radians(lon) - M_PI, gps_alt, declination); // Initialize projection _local_pos.ref_lat = _gps.lat; @@ -1024,7 +1027,7 @@ FixedwingEstimator::task_main() _ekf->posNE[0] = _ekf->posNED[0]; _ekf->posNE[1] = _ekf->posNED[1]; - _ekf->InitialiseFilter(_ekf->velNED, 0.0, 0.0, 0.0f); + _ekf->InitialiseFilter(_ekf->velNED, 0.0, 0.0, 0.0f, 0.0f); } } From 11a1053b73787bb2afbaff360f720de430447455 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Mon, 28 Apr 2014 19:15:58 +0200 Subject: [PATCH 083/329] ekf_att_pos_estimator: local position reference fixed --- .../ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp b/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp index 23f9e80bd8..fb019a354e 100644 --- a/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp +++ b/src/modules/ekf_att_pos_estimator/fw_att_pos_estimator_main.cpp @@ -1004,8 +1004,8 @@ FixedwingEstimator::task_main() _ekf->InitialiseFilter(_ekf->velNED, math::radians(lat), math::radians(lon) - M_PI, gps_alt, declination); // Initialize projection - _local_pos.ref_lat = _gps.lat; - _local_pos.ref_lon = _gps.alt; + _local_pos.ref_lat = lat; + _local_pos.ref_lon = lon; _local_pos.ref_alt = _baro_ref + _baro_gps_offset; _local_pos.ref_timestamp = _gps.timestamp_position; From c179863b1e35628a75efef624d2df0b0e85ad923 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 23 Feb 2014 07:20:54 -0800 Subject: [PATCH 084/329] Improve testing infrastructure for mixer and SBUS2 --- Tools/tests-host/.gitignore | 1 + Tools/tests-host/Makefile | 57 +++++++++---------------- Tools/tests-host/board_config.h | 0 Tools/tests-host/debug.h | 5 +++ Tools/tests-host/run_tests.sh | 6 +++ Tools/tests-host/sbus2_test.cpp | 75 +++++++++++++++++++++++++++++++++ 6 files changed, 108 insertions(+), 36 deletions(-) create mode 100644 Tools/tests-host/board_config.h create mode 100644 Tools/tests-host/debug.h create mode 100755 Tools/tests-host/run_tests.sh create mode 100644 Tools/tests-host/sbus2_test.cpp diff --git a/Tools/tests-host/.gitignore b/Tools/tests-host/.gitignore index 61e0915515..1618faf581 100644 --- a/Tools/tests-host/.gitignore +++ b/Tools/tests-host/.gitignore @@ -1,2 +1,3 @@ ./obj/* mixer_test +sbus2_test \ No newline at end of file diff --git a/Tools/tests-host/Makefile b/Tools/tests-host/Makefile index 7ab1454f01..15ccf15945 100644 --- a/Tools/tests-host/Makefile +++ b/Tools/tests-host/Makefile @@ -1,47 +1,32 @@ CC=g++ -CFLAGS=-I. -I../../src/modules -I ../../src/include -I../../src/drivers -I../../src -D__EXPORT="" -Dnullptr="0" +CFLAGS=-I. -I../../src/modules -I ../../src/include -I../../src/drivers \ + -I../../src -D__EXPORT="" -Dnullptr="0" -lm -ODIR=obj -LDIR =../lib +all: mixer_test sbus2_test -LIBS=-lm +MIXER_FILES=../../src/systemcmds/tests/test_mixer.cpp \ + ../../src/systemcmds/tests/test_conv.cpp \ + ../../src/modules/systemlib/mixer/mixer_simple.cpp \ + ../../src/modules/systemlib/mixer/mixer_multirotor.cpp \ + ../../src/modules/systemlib/mixer/mixer.cpp \ + ../../src/modules/systemlib/mixer/mixer_group.cpp \ + ../../src/modules/systemlib/mixer/mixer_load.c \ + ../../src/modules/systemlib/pwm_limit/pwm_limit.c \ + hrt.cpp \ + mixer_test.cpp -#_DEPS = test.h -#DEPS = $(patsubst %,$(IDIR)/%,$(_DEPS)) +SBUS2_FILES=../../src/modules/px4iofirmware/sbus.c \ + hrt.cpp \ + sbus2_test.cpp -_OBJ = mixer_test.o test_mixer.o mixer_simple.o mixer_multirotor.o \ - mixer.o mixer_group.o mixer_load.o test_conv.o pwm_limit.o hrt.o -OBJ = $(patsubst %,$(ODIR)/%,$(_OBJ)) +mixer_test: $(MIXER_FILES) + $(CC) -o mixer_test $(MIXER_FILES) $(CFLAGS) -#$(DEPS) -$(ODIR)/%.o: %.cpp - mkdir -p obj - $(CC) -c -o $@ $< $(CFLAGS) - -$(ODIR)/%.o: ../../src/systemcmds/tests/%.cpp - $(CC) -c -o $@ $< $(CFLAGS) - -$(ODIR)/%.o: ../../src/modules/systemlib/%.cpp - $(CC) -c -o $@ $< $(CFLAGS) - -$(ODIR)/%.o: ../../src/modules/systemlib/mixer/%.cpp - $(CC) -c -o $@ $< $(CFLAGS) - -$(ODIR)/%.o: ../../src/modules/systemlib/pwm_limit/%.cpp - $(CC) -c -o $@ $< $(CFLAGS) - -$(ODIR)/%.o: ../../src/modules/systemlib/pwm_limit/%.c - $(CC) -c -o $@ $< $(CFLAGS) - -$(ODIR)/%.o: ../../src/modules/systemlib/mixer/%.c - $(CC) -c -o $@ $< $(CFLAGS) - -# -mixer_test: $(OBJ) - g++ -o $@ $^ $(CFLAGS) $(LIBS) +sbus2_test: $(SBUS2_FILES) + $(CC) -o sbus2_test $(SBUS2_FILES) $(CFLAGS) .PHONY: clean clean: - rm -f $(ODIR)/*.o *~ core $(INCDIR)/*~ \ No newline at end of file + rm -f $(ODIR)/*.o *~ core $(INCDIR)/*~ mixer_test sbus2_test \ No newline at end of file diff --git a/Tools/tests-host/board_config.h b/Tools/tests-host/board_config.h new file mode 100644 index 0000000000..e69de29bb2 diff --git a/Tools/tests-host/debug.h b/Tools/tests-host/debug.h new file mode 100644 index 0000000000..9824d13fc3 --- /dev/null +++ b/Tools/tests-host/debug.h @@ -0,0 +1,5 @@ + +#pragma once + +#include +#define lowsyslog warnx \ No newline at end of file diff --git a/Tools/tests-host/run_tests.sh b/Tools/tests-host/run_tests.sh new file mode 100755 index 0000000000..ff5ee509ac --- /dev/null +++ b/Tools/tests-host/run_tests.sh @@ -0,0 +1,6 @@ +#!/bin/sh + +make clean +make all +./mixer_test +./sbus2_test ../../../../data/sbus2/sbus2_r7008SB_gps_baro_tx_off.txt \ No newline at end of file diff --git a/Tools/tests-host/sbus2_test.cpp b/Tools/tests-host/sbus2_test.cpp new file mode 100644 index 0000000000..e3b0122267 --- /dev/null +++ b/Tools/tests-host/sbus2_test.cpp @@ -0,0 +1,75 @@ + +#include +#include +#include +#include +#include +#include +#include +#include "../../src/systemcmds/tests/tests.h" + +int main(int argc, char *argv[]) { + warnx("SBUS2 test started"); + + if (argc < 2) + errx(1, "Need a filename for the input file"); + + warnx("loading data from: %s", argv[1]); + + FILE *fp; + + fp = fopen(argv[1],"rt"); + + if (!fp) + errx(1, "failed opening file"); + + float f; + unsigned x; + int ret; + + // Trash the first 20 lines + for (unsigned i = 0; i < 20; i++) { + (void)fscanf(fp, "%f,%x,,", &f, &x); + } + + // Init the parser + uint8_t frame[30]; + unsigned partial_frame_count = 0; + uint16_t rc_values[18]; + uint16_t num_values; + bool sbus_failsafe; + bool sbus_frame_drop; + uint16_t max_channels = sizeof(rc_values) / sizeof(rc_values[0]); + + float last_time = 0; + + while (EOF != (ret = fscanf(fp, "%f,%x,,", &f, &x))) { + if (((f - last_time) * 1000 * 1000) > 3000) { + partial_frame_count = 0; + warnx("FRAME RESET\n\n"); + } + + frame[partial_frame_count] = x; + partial_frame_count++; + + //warnx("%f: 0x%02x, first: 0x%02x, last: 0x%02x, pcount: %u", (double)f, x, frame[0], frame[24], partial_frame_count); + + if (partial_frame_count == sizeof(frame)) + partial_frame_count = 0; + + last_time = f; + + // Pipe the data into the parser + hrt_abstime now = hrt_absolute_time(); + + if (partial_frame_count % 25 == 0) + sbus_parse(now, frame, &partial_frame_count, rc_values, &num_values, &sbus_failsafe, &sbus_frame_drop, max_channels); + } + + if (ret == EOF) { + warnx("Test finished, reached end of file"); + } else { + warnx("Test aborted, errno: %d", ret); + } + +} \ No newline at end of file From db304910510ed2ae8c262097fa6e8df1db965d5e Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 28 Apr 2014 19:20:14 +0200 Subject: [PATCH 085/329] Add missing newline --- Tools/tests-host/.gitignore | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Tools/tests-host/.gitignore b/Tools/tests-host/.gitignore index 1618faf581..a6ce91d694 100644 --- a/Tools/tests-host/.gitignore +++ b/Tools/tests-host/.gitignore @@ -1,3 +1,3 @@ ./obj/* mixer_test -sbus2_test \ No newline at end of file +sbus2_test From 002ff7da7e792010c4eba5903075af83cb1ebd3e Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Sun, 23 Feb 2014 07:21:13 -0800 Subject: [PATCH 086/329] Add missing header in HRT --- src/drivers/drv_hrt.h | 1 + 1 file changed, 1 insertion(+) diff --git a/src/drivers/drv_hrt.h b/src/drivers/drv_hrt.h index d130d68b34..8bfc90c64b 100644 --- a/src/drivers/drv_hrt.h +++ b/src/drivers/drv_hrt.h @@ -41,6 +41,7 @@ #include #include +#include #include #include From 3959d0c1c9cf32ac1a2fa4df63fa0f0f77cc17a5 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 28 Apr 2014 19:24:44 +0200 Subject: [PATCH 087/329] Disable sbus2_test for now, just leverage the testing infrastructure --- Tools/tests-host/sbus2_test.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Tools/tests-host/sbus2_test.cpp b/Tools/tests-host/sbus2_test.cpp index e3b0122267..134a71b809 100644 --- a/Tools/tests-host/sbus2_test.cpp +++ b/Tools/tests-host/sbus2_test.cpp @@ -63,7 +63,7 @@ int main(int argc, char *argv[]) { hrt_abstime now = hrt_absolute_time(); if (partial_frame_count % 25 == 0) - sbus_parse(now, frame, &partial_frame_count, rc_values, &num_values, &sbus_failsafe, &sbus_frame_drop, max_channels); + //sbus_parse(now, frame, &partial_frame_count, rc_values, &num_values, &sbus_failsafe, &sbus_frame_drop, max_channels); } if (ret == EOF) { From da525f29f13e5bdae717fbf189c5a3b4ab46794c Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 28 Apr 2014 19:25:13 +0200 Subject: [PATCH 088/329] Add missing header in mixer load command --- src/modules/systemlib/mixer/mixer_load.c | 1 + 1 file changed, 1 insertion(+) diff --git a/src/modules/systemlib/mixer/mixer_load.c b/src/modules/systemlib/mixer/mixer_load.c index b05273c0de..bf3428a50b 100644 --- a/src/modules/systemlib/mixer/mixer_load.c +++ b/src/modules/systemlib/mixer/mixer_load.c @@ -41,6 +41,7 @@ #include #include #include +#include #include "mixer_load.h" From 238a3636faae13c4ba52ab9581a305c7a069276e Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 28 Apr 2014 20:29:13 +0200 Subject: [PATCH 089/329] Add autodeclination --- Tools/tests-host/.gitignore | 1 + 1 file changed, 1 insertion(+) diff --git a/Tools/tests-host/.gitignore b/Tools/tests-host/.gitignore index a6ce91d694..87b314c614 100644 --- a/Tools/tests-host/.gitignore +++ b/Tools/tests-host/.gitignore @@ -1,3 +1,4 @@ ./obj/* mixer_test sbus2_test +autodeclination_test From 7aefcb7a09a12fae2dcbe2bc2360948aefbb66a4 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 28 Apr 2014 20:29:45 +0200 Subject: [PATCH 090/329] Add autodeclination testing - no-op right now --- Tools/tests-host/Makefile | 11 +++++++++-- Tools/tests-host/autodeclination_test.cpp | 19 +++++++++++++++++++ Tools/tests-host/sbus2_test.cpp | 2 +- 3 files changed, 29 insertions(+), 3 deletions(-) create mode 100644 Tools/tests-host/autodeclination_test.cpp diff --git a/Tools/tests-host/Makefile b/Tools/tests-host/Makefile index 15ccf15945..fd001e4d7f 100644 --- a/Tools/tests-host/Makefile +++ b/Tools/tests-host/Makefile @@ -3,7 +3,7 @@ CC=g++ CFLAGS=-I. -I../../src/modules -I ../../src/include -I../../src/drivers \ -I../../src -D__EXPORT="" -Dnullptr="0" -lm -all: mixer_test sbus2_test +all: mixer_test sbus2_test autodeclination_test MIXER_FILES=../../src/systemcmds/tests/test_mixer.cpp \ ../../src/systemcmds/tests/test_conv.cpp \ @@ -20,13 +20,20 @@ SBUS2_FILES=../../src/modules/px4iofirmware/sbus.c \ hrt.cpp \ sbus2_test.cpp +AUTODECLINATION_FILES=../../src/lib/geo/geo_mag_declination.c \ + hrt.cpp \ + autodeclination_test.cpp + mixer_test: $(MIXER_FILES) $(CC) -o mixer_test $(MIXER_FILES) $(CFLAGS) sbus2_test: $(SBUS2_FILES) $(CC) -o sbus2_test $(SBUS2_FILES) $(CFLAGS) +autodeclination_test: $(SBUS2_FILES) + $(CC) -o autodeclination_test $(SBUS2_FILES) $(CFLAGS) + .PHONY: clean clean: - rm -f $(ODIR)/*.o *~ core $(INCDIR)/*~ mixer_test sbus2_test \ No newline at end of file + rm -f $(ODIR)/*.o *~ core $(INCDIR)/*~ mixer_test sbus2_test autodeclination_test \ No newline at end of file diff --git a/Tools/tests-host/autodeclination_test.cpp b/Tools/tests-host/autodeclination_test.cpp new file mode 100644 index 0000000000..b1b30f5d97 --- /dev/null +++ b/Tools/tests-host/autodeclination_test.cpp @@ -0,0 +1,19 @@ + +#include +#include +#include +#include +#include +#include +#include +#include "../../src/systemcmds/tests/tests.h" + +int main(int argc, char *argv[]) { + warnx("autodeclination test started"); + + if (argc < 3) + errx(1, "Need lat/lon!"); + + + +} \ No newline at end of file diff --git a/Tools/tests-host/sbus2_test.cpp b/Tools/tests-host/sbus2_test.cpp index 134a71b809..281903cf68 100644 --- a/Tools/tests-host/sbus2_test.cpp +++ b/Tools/tests-host/sbus2_test.cpp @@ -62,7 +62,7 @@ int main(int argc, char *argv[]) { // Pipe the data into the parser hrt_abstime now = hrt_absolute_time(); - if (partial_frame_count % 25 == 0) + //if (partial_frame_count % 25 == 0) //sbus_parse(now, frame, &partial_frame_count, rc_values, &num_values, &sbus_failsafe, &sbus_frame_drop, max_channels); } From 9c81ab113e73c8862cc3f4e81411cb69dbec14ee Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 28 Apr 2014 20:42:46 +0200 Subject: [PATCH 091/329] Updated outo-test --- Tools/tests-host/Makefile | 6 +++--- Tools/tests-host/autodeclination_test.cpp | 11 ++++++++++- 2 files changed, 13 insertions(+), 4 deletions(-) diff --git a/Tools/tests-host/Makefile b/Tools/tests-host/Makefile index fd001e4d7f..f0737ef88c 100644 --- a/Tools/tests-host/Makefile +++ b/Tools/tests-host/Makefile @@ -1,7 +1,7 @@ CC=g++ CFLAGS=-I. -I../../src/modules -I ../../src/include -I../../src/drivers \ - -I../../src -D__EXPORT="" -Dnullptr="0" -lm + -I../../src -I../../src/lib -D__EXPORT="" -Dnullptr="0" -lm all: mixer_test sbus2_test autodeclination_test @@ -20,7 +20,7 @@ SBUS2_FILES=../../src/modules/px4iofirmware/sbus.c \ hrt.cpp \ sbus2_test.cpp -AUTODECLINATION_FILES=../../src/lib/geo/geo_mag_declination.c \ +AUTODECLINATION_FILES= ../../src/lib/geo/geo_mag_declination.c \ hrt.cpp \ autodeclination_test.cpp @@ -31,7 +31,7 @@ sbus2_test: $(SBUS2_FILES) $(CC) -o sbus2_test $(SBUS2_FILES) $(CFLAGS) autodeclination_test: $(SBUS2_FILES) - $(CC) -o autodeclination_test $(SBUS2_FILES) $(CFLAGS) + $(CC) -o autodeclination_test $(AUTODECLINATION_FILES) $(CFLAGS) .PHONY: clean diff --git a/Tools/tests-host/autodeclination_test.cpp b/Tools/tests-host/autodeclination_test.cpp index b1b30f5d97..6c751dc1e7 100644 --- a/Tools/tests-host/autodeclination_test.cpp +++ b/Tools/tests-host/autodeclination_test.cpp @@ -1,5 +1,6 @@ #include +#include #include #include #include @@ -7,6 +8,7 @@ #include #include #include "../../src/systemcmds/tests/tests.h" +#include int main(int argc, char *argv[]) { warnx("autodeclination test started"); @@ -14,6 +16,13 @@ int main(int argc, char *argv[]) { if (argc < 3) errx(1, "Need lat/lon!"); - + char* p_end; + + float lat = strtod(argv[1], &p_end); + float lon = strtod(argv[2], &p_end); + + float declination = get_mag_declination(lat, lon); + + printf("lat: %f lon: %f, dec: %f\n", lat, lon, declination); } \ No newline at end of file From ec50f73cbe4c88a57f92f888d764a678f6796dd2 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Mon, 28 Apr 2014 20:44:11 +0200 Subject: [PATCH 092/329] Updated geo lib C/C++ interfacing --- src/lib/geo/geo.h | 4 ++-- src/lib/geo/geo_mag_declination.c | 2 ++ 2 files changed, 4 insertions(+), 2 deletions(-) diff --git a/src/lib/geo/geo.h b/src/lib/geo/geo.h index d987afe337..e2f3da6f80 100644 --- a/src/lib/geo/geo.h +++ b/src/lib/geo/geo.h @@ -48,10 +48,10 @@ #include "uORB/topics/fence.h" #include "uORB/topics/vehicle_global_position.h" -#include "geo/geo_mag_declination.h" - __BEGIN_DECLS +#include "geo/geo_mag_declination.h" + #include #define CONSTANTS_ONE_G 9.80665f /* m/s^2 */ diff --git a/src/lib/geo/geo_mag_declination.c b/src/lib/geo/geo_mag_declination.c index 7b4aa69a2f..b96f877214 100644 --- a/src/lib/geo/geo_mag_declination.c +++ b/src/lib/geo/geo_mag_declination.c @@ -43,6 +43,8 @@ * */ +#include + /** set this always to the sampling in degrees for the table below */ #define SAMPLING_RES 10 From 31089a290ba089b2b5cbcc76ed677e3f401ffa36 Mon Sep 17 00:00:00 2001 From: TickTock- Date: Mon, 28 Apr 2014 21:47:45 -0700 Subject: [PATCH 093/329] Replaces poshold/althold with posctrl/altctrl --- .../include/mavlink/v1.0/autoquad/autoquad.h | 4 +- src/drivers/blinkm/blinkm.cpp | 4 +- src/modules/commander/commander.cpp | 46 +++++++++---------- .../state_machine_helper_test.cpp | 24 +++++----- src/modules/commander/px4_custom_mode.h | 4 +- .../commander/state_machine_helper.cpp | 4 +- src/modules/fixedwing_backside/fixedwing.cpp | 4 +- .../fw_pos_control_l1_main.cpp | 42 ++++++++--------- src/modules/mavlink/mavlink_messages.cpp | 8 ++-- .../mc_pos_control/mc_pos_control_params.c | 10 ++-- src/modules/segway/BlockSegwayController.cpp | 4 +- src/modules/sensors/sensor_params.c | 6 +-- src/modules/sensors/sensors.cpp | 26 +++++------ .../uORB/topics/manual_control_setpoint.h | 2 +- src/modules/uORB/topics/rc_channels.h | 2 +- src/modules/uORB/topics/vehicle_status.h | 4 +- 16 files changed, 97 insertions(+), 97 deletions(-) diff --git a/mavlink/include/mavlink/v1.0/autoquad/autoquad.h b/mavlink/include/mavlink/v1.0/autoquad/autoquad.h index 93e868dc35..bd3fc66e70 100644 --- a/mavlink/include/mavlink/v1.0/autoquad/autoquad.h +++ b/mavlink/include/mavlink/v1.0/autoquad/autoquad.h @@ -40,8 +40,8 @@ enum AUTOQUAD_NAV_STATUS AQ_NAV_STATUS_INIT=0, /* System is initializing | */ AQ_NAV_STATUS_STANDBY=1, /* System is standing by, not active | */ AQ_NAV_STATUS_MANUAL=2, /* Stabilized, under full manual control | */ - AQ_NAV_STATUS_ALTHOLD=4, /* Altitude hold engaged | */ - AQ_NAV_STATUS_POSHOLD=8, /* Position hold engaged | */ + AQ_NAV_STATUS_ALTCTRL=4, /* Altitude hold engaged | */ + AQ_NAV_STATUS_POSCTRL=8, /* Position hold engaged | */ AQ_NAV_STATUS_DVH=16, /* Dynamic Velocity Hold is active | */ AQ_NAV_STATUS_MISSION=32, /* Autonomous mission execution mode | */ AQ_NAV_STATUS_CEILING_REACHED=67108864, /* Craft is at ceiling altitude | */ diff --git a/src/drivers/blinkm/blinkm.cpp b/src/drivers/blinkm/blinkm.cpp index aa9c645028..974e20ca26 100644 --- a/src/drivers/blinkm/blinkm.cpp +++ b/src/drivers/blinkm/blinkm.cpp @@ -638,11 +638,11 @@ BlinkM::led() if(new_data_vehicle_control_mode || no_data_vehicle_control_mode < 3) { /* indicate main control state */ - if (vehicle_status_raw.main_state == MAIN_STATE_POSHOLD) + if (vehicle_status_raw.main_state == MAIN_STATE_POSCTRL) led_color_4 = LED_GREEN; else if (vehicle_status_raw.main_state == MAIN_STATE_AUTO) led_color_4 = LED_BLUE; - else if (vehicle_status_raw.main_state == MAIN_STATE_ALTHOLD) + else if (vehicle_status_raw.main_state == MAIN_STATE_ALTCTRL) led_color_4 = LED_YELLOW; else if (vehicle_status_raw.main_state == MAIN_STATE_MANUAL) led_color_4 = LED_WHITE; diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index a99456370c..be3e6d269a 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -434,13 +434,13 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe /* MANUAL */ main_res = main_state_transition(status, MAIN_STATE_MANUAL); - } else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_ALTHOLD) { - /* ALTHOLD */ - main_res = main_state_transition(status, MAIN_STATE_ALTHOLD); + } else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_ALTCTRL) { + /* ALTCTRL */ + main_res = main_state_transition(status, MAIN_STATE_ALTCTRL); - } else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_POSHOLD) { - /* POSHOLD */ - main_res = main_state_transition(status, MAIN_STATE_POSHOLD); + } else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_POSCTRL) { + /* POSCTRL */ + main_res = main_state_transition(status, MAIN_STATE_POSCTRL); } else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_AUTO) { /* AUTO */ @@ -455,8 +455,8 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe } else if (base_mode & MAV_MODE_FLAG_MANUAL_INPUT_ENABLED) { if (base_mode & MAV_MODE_FLAG_GUIDED_ENABLED) { - /* POSHOLD */ - main_res = main_state_transition(status, MAIN_STATE_POSHOLD); + /* POSCTRL */ + main_res = main_state_transition(status, MAIN_STATE_POSCTRL); } else if (base_mode & MAV_MODE_FLAG_STABILIZE_ENABLED) { /* MANUAL */ @@ -628,8 +628,8 @@ int commander_thread_main(int argc, char *argv[]) char *main_states_str[MAIN_STATE_MAX]; main_states_str[0] = "MANUAL"; - main_states_str[1] = "ALTHOLD"; - main_states_str[2] = "POSHOLD"; + main_states_str[1] = "ALTCTRL"; + main_states_str[2] = "POSCTRL"; main_states_str[3] = "AUTO"; char *arming_states_str[ARMING_STATE_MAX]; @@ -1299,8 +1299,8 @@ int commander_thread_main(int argc, char *argv[]) } // TODO remove this hack - /* flight termination in manual mode if assisted switch is on poshold position */ - if (!status.is_rotary_wing && parachute_enabled && armed.armed && status.main_state == MAIN_STATE_MANUAL && sp_man.poshold_switch == SWITCH_POS_ON) { + /* flight termination in manual mode if assisted switch is on posctrl position */ + if (!status.is_rotary_wing && parachute_enabled && armed.armed && status.main_state == MAIN_STATE_MANUAL && sp_man.posctrl_switch == SWITCH_POS_ON) { if (TRANSITION_CHANGED == failsafe_state_transition(&status, FAILSAFE_STATE_TERMINATION)) { tune_positive(armed.armed); } @@ -1557,25 +1557,25 @@ set_main_state_rc(struct vehicle_status_s *status, struct manual_control_setpoin break; case SWITCH_POS_MIDDLE: // ASSISTED - if (sp_man->poshold_switch == SWITCH_POS_ON) { - res = main_state_transition(status, MAIN_STATE_POSHOLD); + if (sp_man->posctrl_switch == SWITCH_POS_ON) { + res = main_state_transition(status, MAIN_STATE_POSCTRL); if (res != TRANSITION_DENIED) { break; // changed successfully or already in this state } - // else fallback to ALTHOLD - print_reject_mode(status, "POSHOLD"); + // else fallback to ALTCTRL + print_reject_mode(status, "POSCTRL"); } - res = main_state_transition(status, MAIN_STATE_ALTHOLD); + res = main_state_transition(status, MAIN_STATE_ALTCTRL); if (res != TRANSITION_DENIED) { break; // changed successfully or already in this mode } - if (sp_man->poshold_switch != SWITCH_POS_ON) { - print_reject_mode(status, "ALTHOLD"); + if (sp_man->posctrl_switch != SWITCH_POS_ON) { + print_reject_mode(status, "ALTCTRL"); } // else fallback to MANUAL @@ -1590,9 +1590,9 @@ set_main_state_rc(struct vehicle_status_s *status, struct manual_control_setpoin break; // changed successfully or already in this state } - // else fallback to ALTHOLD (POSHOLD likely will not work too) + // else fallback to ALTCTRL (POSCTRL likely will not work too) print_reject_mode(status, "AUTO"); - res = main_state_transition(status, MAIN_STATE_ALTHOLD); + res = main_state_transition(status, MAIN_STATE_ALTCTRL); if (res != TRANSITION_DENIED) { break; // changed successfully or already in this state @@ -1638,7 +1638,7 @@ set_control_mode() control_mode.flag_control_velocity_enabled = false; break; - case MAIN_STATE_ALTHOLD: + case MAIN_STATE_ALTCTRL: control_mode.flag_control_manual_enabled = true; control_mode.flag_control_auto_enabled = false; control_mode.flag_control_rates_enabled = true; @@ -1649,7 +1649,7 @@ set_control_mode() control_mode.flag_control_velocity_enabled = false; break; - case MAIN_STATE_POSHOLD: + case MAIN_STATE_POSCTRL: control_mode.flag_control_manual_enabled = true; control_mode.flag_control_auto_enabled = false; control_mode.flag_control_rates_enabled = true; diff --git a/src/modules/commander/commander_tests/state_machine_helper_test.cpp b/src/modules/commander/commander_tests/state_machine_helper_test.cpp index b440e561b5..18586053b1 100644 --- a/src/modules/commander/commander_tests/state_machine_helper_test.cpp +++ b/src/modules/commander/commander_tests/state_machine_helper_test.cpp @@ -317,34 +317,34 @@ bool StateMachineHelperTest::mainStateTransitionTest(void) TRANSITION_CHANGED == main_state_transition(¤t_state, new_main_state)); ut_assert("new state: manual", MAIN_STATE_MANUAL == current_state.main_state); - // MANUAL to ALTHOLD. + // MANUAL to ALTCTRL. current_state.main_state = MAIN_STATE_MANUAL; current_state.condition_local_altitude_valid = true; - new_main_state = MAIN_STATE_ALTHOLD; - ut_assert("tranisition: manual to althold", + new_main_state = MAIN_STATE_ALTCTRL; + ut_assert("tranisition: manual to altctrl", TRANSITION_CHANGED == main_state_transition(¤t_state, new_main_state)); - ut_assert("new state: althold", MAIN_STATE_ALTHOLD == current_state.main_state); + ut_assert("new state: altctrl", MAIN_STATE_ALTCTRL == current_state.main_state); - // MANUAL to ALTHOLD, invalid local altitude. + // MANUAL to ALTCTRL, invalid local altitude. current_state.main_state = MAIN_STATE_MANUAL; current_state.condition_local_altitude_valid = false; - new_main_state = MAIN_STATE_ALTHOLD; + new_main_state = MAIN_STATE_ALTCTRL; ut_assert("no transition: invalid local altitude", TRANSITION_DENIED == main_state_transition(¤t_state, new_main_state)); ut_assert("current state: manual", MAIN_STATE_MANUAL == current_state.main_state); - // MANUAL to POSHOLD. + // MANUAL to POSCTRL. current_state.main_state = MAIN_STATE_MANUAL; current_state.condition_local_position_valid = true; - new_main_state = MAIN_STATE_POSHOLD; - ut_assert("transition: manual to poshold", + new_main_state = MAIN_STATE_POSCTRL; + ut_assert("transition: manual to posctrl", TRANSITION_CHANGED == main_state_transition(¤t_state, new_main_state)); - ut_assert("current state: poshold", MAIN_STATE_POSHOLD == current_state.main_state); + ut_assert("current state: posctrl", MAIN_STATE_POSCTRL == current_state.main_state); - // MANUAL to POSHOLD, invalid local position. + // MANUAL to POSCTRL, invalid local position. current_state.main_state = MAIN_STATE_MANUAL; current_state.condition_local_position_valid = false; - new_main_state = MAIN_STATE_POSHOLD; + new_main_state = MAIN_STATE_POSCTRL; ut_assert("no transition: invalid position", TRANSITION_DENIED == main_state_transition(¤t_state, new_main_state)); ut_assert("current state: manual", MAIN_STATE_MANUAL == current_state.main_state); diff --git a/src/modules/commander/px4_custom_mode.h b/src/modules/commander/px4_custom_mode.h index 962d2804cc..e6274fb89d 100644 --- a/src/modules/commander/px4_custom_mode.h +++ b/src/modules/commander/px4_custom_mode.h @@ -12,8 +12,8 @@ enum PX4_CUSTOM_MAIN_MODE { PX4_CUSTOM_MAIN_MODE_MANUAL = 1, - PX4_CUSTOM_MAIN_MODE_ALTHOLD, - PX4_CUSTOM_MAIN_MODE_POSHOLD, + PX4_CUSTOM_MAIN_MODE_ALTCTRL, + PX4_CUSTOM_MAIN_MODE_POSCTRL, PX4_CUSTOM_MAIN_MODE_AUTO, }; diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 21e36a87df..3b6d951351 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -215,7 +215,7 @@ main_state_transition(struct vehicle_status_s *status, main_state_t new_main_sta ret = TRANSITION_CHANGED; break; - case MAIN_STATE_ALTHOLD: + case MAIN_STATE_ALTCTRL: /* need at minimum altitude estimate */ if (!status->is_rotary_wing || @@ -226,7 +226,7 @@ main_state_transition(struct vehicle_status_s *status, main_state_t new_main_sta break; - case MAIN_STATE_POSHOLD: + case MAIN_STATE_POSCTRL: /* need at minimum local position estimate */ if (status->condition_local_position_valid || diff --git a/src/modules/fixedwing_backside/fixedwing.cpp b/src/modules/fixedwing_backside/fixedwing.cpp index fafab9bfec..dc82ee4752 100644 --- a/src/modules/fixedwing_backside/fixedwing.cpp +++ b/src/modules/fixedwing_backside/fixedwing.cpp @@ -229,8 +229,8 @@ void BlockMultiModeBacksideAutopilot::update() _actuators.control[CH_RDR] = _manual.yaw; _actuators.control[CH_THR] = _manual.throttle; - } else if (_status.main_state == MAIN_STATE_ALTHOLD || - _status.main_state == MAIN_STATE_POSHOLD /* TODO, implement easy */) { + } else if (_status.main_state == MAIN_STATE_ALTCTRL || + _status.main_state == MAIN_STATE_POSCTRL /* TODO, implement easy */) { // calculate velocity, XXX should be airspeed, but using ground speed for now // for the purpose of control we will limit the velocity feedback between diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index d5a68e21f5..024dd98ec3 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -153,7 +153,7 @@ private: bool _setpoint_valid; /**< flag if the position control setpoint is valid */ /** manual control states */ - float _althold_hold_heading; /**< heading the system should hold in althold mode */ + float _altctrl_hold_heading; /**< heading the system should hold in altctrl mode */ double _loiter_hold_lat; double _loiter_hold_lon; float _loiter_hold_alt; @@ -1051,16 +1051,16 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi _att_sp.roll_reset_integral = true; } - } else if (0/* poshold mode enabled */) { + } else if (0/* posctrl mode enabled */) { - /** POSHOLD FLIGHT **/ + /** POSCTRL FLIGHT **/ - if (0/* switched from another mode to poshold */) { - _althold_hold_heading = _att.yaw; + if (0/* switched from another mode to posctrl */) { + _altctrl_hold_heading = _att.yaw; } - if (0/* poshold on and manual control yaw non-zero */) { - _althold_hold_heading = _att.yaw + _manual.yaw; + if (0/* posctrl on and manual control yaw non-zero */) { + _altctrl_hold_heading = _att.yaw + _manual.yaw; } //XXX not used @@ -1073,39 +1073,39 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi // climb_out = true; // } - /* if in althold mode, set airspeed based on manual control */ + /* if in altctrl mode, set airspeed based on manual control */ // XXX check if ground speed undershoot should be applied here - float althold_airspeed = _parameters.airspeed_min + + float altctrl_airspeed = _parameters.airspeed_min + (_parameters.airspeed_max - _parameters.airspeed_min) * _manual.throttle; - _l1_control.navigate_heading(_althold_hold_heading, _att.yaw, ground_speed); + _l1_control.navigate_heading(_altctrl_hold_heading, _att.yaw, ground_speed); _att_sp.roll_body = _l1_control.nav_roll(); _att_sp.yaw_body = _l1_control.nav_bearing(); _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _global_pos.alt + _manual.pitch * 2.0f, - althold_airspeed, + altctrl_airspeed, _airspeed.indicated_airspeed_m_s, eas2tas, false, _parameters.pitch_limit_min, _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, _parameters.pitch_limit_min, _parameters.pitch_limit_max); - } else if (0/* althold mode enabled */) { + } else if (0/* altctrl mode enabled */) { - /** ALTHOLD FLIGHT **/ + /** ALTCTRL FLIGHT **/ - if (0/* switched from another mode to althold */) { - _althold_hold_heading = _att.yaw; + if (0/* switched from another mode to altctrl */) { + _altctrl_hold_heading = _att.yaw; } - if (0/* althold on and manual control yaw non-zero */) { - _althold_hold_heading = _att.yaw + _manual.yaw; + if (0/* altctrl on and manual control yaw non-zero */) { + _altctrl_hold_heading = _att.yaw + _manual.yaw; } - /* if in althold mode, set airspeed based on manual control */ + /* if in altctrl mode, set airspeed based on manual control */ // XXX check if ground speed undershoot should be applied here - float althold_airspeed = _parameters.airspeed_min + + float altctrl_airspeed = _parameters.airspeed_min + (_parameters.airspeed_max - _parameters.airspeed_min) * _manual.throttle; @@ -1124,11 +1124,11 @@ FixedwingPositionControl::control_position(const math::Vector<2> ¤t_positi climb_out = true; } - _l1_control.navigate_heading(_althold_hold_heading, _att.yaw, ground_speed); + _l1_control.navigate_heading(_altctrl_hold_heading, _att.yaw, ground_speed); _att_sp.roll_body = _manual.roll; _att_sp.yaw_body = _manual.yaw; _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _global_pos.alt + _manual.pitch * 2.0f, - althold_airspeed, + altctrl_airspeed, _airspeed.indicated_airspeed_m_s, eas2tas, climb_out, _parameters.pitch_limit_min, _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 108ef8ad46..38a5433ff7 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -124,13 +124,13 @@ void get_mavlink_mode_state(struct vehicle_status_s *status, struct position_set *mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | (status->is_rotary_wing ? MAV_MODE_FLAG_STABILIZE_ENABLED : 0); custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_MANUAL; - } else if (status->main_state == MAIN_STATE_ALTHOLD) { + } else if (status->main_state == MAIN_STATE_ALTCTRL) { *mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED; - custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_ALTHOLD; + custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_ALTCTRL; - } else if (status->main_state == MAIN_STATE_POSHOLD) { + } else if (status->main_state == MAIN_STATE_POSCTRL) { *mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED; - custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_POSHOLD; + custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_POSCTRL; } else if (status->main_state == MAIN_STATE_AUTO) { *mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED; diff --git a/src/modules/mc_pos_control/mc_pos_control_params.c b/src/modules/mc_pos_control/mc_pos_control_params.c index 015d8ad160..dacdd46f0a 100644 --- a/src/modules/mc_pos_control/mc_pos_control_params.c +++ b/src/modules/mc_pos_control/mc_pos_control_params.c @@ -98,7 +98,7 @@ PARAM_DEFINE_FLOAT(MPC_Z_VEL_D, 0.0f); /** * Maximum vertical velocity * - * Maximum vertical velocity in AUTO mode and endpoint for stabilized modes (ALTHOLD, POSHOLD). + * Maximum vertical velocity in AUTO mode and endpoint for stabilized modes (ALTCTRL, POSCTRL). * * @unit m/s * @min 0.0 @@ -109,7 +109,7 @@ PARAM_DEFINE_FLOAT(MPC_Z_VEL_MAX, 5.0f); /** * Vertical velocity feed forward * - * Feed forward weight for altitude control in stabilized modes (ALTHOLD, POSHOLD). 0 will give slow responce and no overshot, 1 - fast responce and big overshot. + * Feed forward weight for altitude control in stabilized modes (ALTCTRL, POSCTRL). 0 will give slow responce and no overshot, 1 - fast responce and big overshot. * * @min 0.0 * @max 1.0 @@ -154,7 +154,7 @@ PARAM_DEFINE_FLOAT(MPC_XY_VEL_D, 0.01f); /** * Maximum horizontal velocity * - * Maximum horizontal velocity in AUTO mode and endpoint for position stabilized mode (POSHOLD). + * Maximum horizontal velocity in AUTO mode and endpoint for position stabilized mode (POSCTRL). * * @unit m/s * @min 0.0 @@ -165,7 +165,7 @@ PARAM_DEFINE_FLOAT(MPC_XY_VEL_MAX, 5.0f); /** * Horizontal velocity feed forward * - * Feed forward weight for position control in position control mode (POSHOLD). 0 will give slow responce and no overshot, 1 - fast responce and big overshot. + * Feed forward weight for position control in position control mode (POSCTRL). 0 will give slow responce and no overshot, 1 - fast responce and big overshot. * * @min 0.0 * @max 1.0 @@ -176,7 +176,7 @@ PARAM_DEFINE_FLOAT(MPC_XY_FF, 0.5f); /** * Maximum tilt angle in air * - * Limits maximum tilt in AUTO and POSHOLD modes during flight. + * Limits maximum tilt in AUTO and POSCTRL modes during flight. * * @unit deg * @min 0.0 diff --git a/src/modules/segway/BlockSegwayController.cpp b/src/modules/segway/BlockSegwayController.cpp index 2cb4fc900f..6d46db9bd1 100644 --- a/src/modules/segway/BlockSegwayController.cpp +++ b/src/modules/segway/BlockSegwayController.cpp @@ -35,8 +35,8 @@ void BlockSegwayController::update() { // handle autopilot modes if (_status.main_state == MAIN_STATE_AUTO || - _status.main_state == MAIN_STATE_ALTHOLD || - _status.main_state == MAIN_STATE_POSHOLD) { + _status.main_state == MAIN_STATE_ALTCTRL || + _status.main_state == MAIN_STATE_POSCTRL) { _actuators.control[0] = spdCmd; _actuators.control[1] = spdCmd; diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c index 59bd99db78..02890b452c 100644 --- a/src/modules/sensors/sensor_params.c +++ b/src/modules/sensors/sensor_params.c @@ -605,7 +605,7 @@ PARAM_DEFINE_INT32(RC_MAP_RETURN_SW, 0); * @max 18 * @group Radio Calibration */ -PARAM_DEFINE_INT32(RC_MAP_POSHLD_SW, 0); +PARAM_DEFINE_INT32(RC_MAP_POSCTL_SW, 0); /** * Loiter switch channel mapping. @@ -703,7 +703,7 @@ PARAM_DEFINE_FLOAT(RC_ASSISTED_TH, 0.25f); PARAM_DEFINE_FLOAT(RC_AUTO_TH, 0.75f); /** - * Threshold for selecting poshold mode + * Threshold for selecting posctrl mode * * min:-1 * max:+1 @@ -716,7 +716,7 @@ PARAM_DEFINE_FLOAT(RC_AUTO_TH, 0.75f); * negative : true when channel Date: Mon, 28 Apr 2014 22:11:02 -0700 Subject: [PATCH 094/329] Updated flight modes diagrams & comments. --- Documentation/rc_mode_switch.odg | Bin 20160 -> 20920 bytes Documentation/rc_mode_switch.pdf | Bin 39286 -> 27305 bytes .../include/mavlink/v1.0/autoquad/autoquad.h | 4 ++-- src/modules/fixedwing_backside/fixedwing.cpp | 2 +- 4 files changed, 3 insertions(+), 3 deletions(-) diff --git a/Documentation/rc_mode_switch.odg b/Documentation/rc_mode_switch.odg index 5c4b617e609a916e1b88893f3e72c31fbc152f5e..3ed379d084b834eb7fdf30ad009197deb484600f 100644 GIT binary patch delta 16014 zcmZX*1yCNr(y;sD?ry=|-QC^YJ-9>A#S?-%gy8NT+yX&@26uONmrHWK@4x52cb?kn zn%|b?M0^GK z#Aq!?SXf{!?Vq6M}Z$d8!JrM;{lO4MN<6AM;qzwgJLFl$BAeoZ?k79rvJz^hW7UqkU84SdgyFl1?oe##EpVEFe z!@!%+_$+n|u?QzuUn&?{K4!nUq{V@_xVXYcyk8zYwC&jN_gOX>A0L|9w4*{I0q`39 zm+Z*}wA;%N+(B9CqdU0m&(6<#&XS;3G!&<6tqR|Roli6nM+aJ$4`~c`$BFs$aLt6n z8iOgH1dK{Yk_n)O%6-CNuC&&Z5DBrnd5YGejbgOfJ_--RKP+hmIyvlFH1_OWH6G%` zIh5$>jSOg{XE;;#Muc#K4M46}00;UPTF`tqen{Jp+sRdoGi5beBv#{=+P3%)d!jpH z5$fDv){r6uLuAngiCAR6+<>fcMP0lYG^wPqU1N@<-G@)PdGPx9>w&-04m9gN9I;>DZ1#8B}3-(2SJfc#4EWYb%lUL9?9Tu)#C0RpMuP;)QP z3Wu~CP$SOmWA^~N$$-oqRAI&U4Oh(UI>5~vo{{Mm?6{BR*$6Cubf4K^Kl8(K^_eo& zG~DS!TX=4&jrFyb0k*hc_M8QtJF^fqJR&9!{I)>daDDYKM37_D6o+PQ480cTY!FSzc?&8(lJ%uNRp&+)m1M5RBUm(Dj$- z`V(F9Zewxr)=b5vij*1;Qm(Fz1RusB*R$=qMMmfyU#hRwfrMAy2f^AFYqzrlSJ5w|~hU&Idj^S2kYZ!iyz*_Ok2KQ#~t{v!5Ac1A;Nv&LRq^yu<|ytEbaqWdk) zNQs`(@&o)v;In)@R2eTJ@S5d_{9(T%p+Zs1JJLS|`n63A&L)DFILnz?JFh9Brd(PF(0bA)i zMF&M_OWFx?pRRH5q40Vn@K=sDLlhin!2T4iTs^jw0sNAQk{ZHXm*|G;X+PSM>Z03` zc%>Y1&O_Sv@|`Ri)EMbfHml@>+odtHL~b*!+8S`MVP3nVYO2lK*(k3&s zoLq-515l_HeW^fZnny=RS<1dmdBzNFjQyt(t2r^%I`d0z?)$a%BK-N%;nsQJHEuEI z&#EPv2VU%DxP-lK`FEzh9)t!s-7FtOPa}J7ZT8QL)<*mK)?5B$+*_QliW)>A6u22= zt9bQIY}xGpotn1H&SU?;5Pa^<+vg#`DQSBKR1tq(a!l(^ompA1=KN94s)bzt`>p@L z-JeovQtZkIG84%sMjH1Es_5E0JCv^Rz^EgBMla72suk+0&7Ry{>w<9dzkTKmlLDI* zf7wS1s!QG+68|#mCfpuywY>px@tqg#wZ|&u7p#vAx;3}*2ry&@nfb7b^5*`+O6vyj zOHz*!n1*o)QIrGv#&76VewRmzE+1JHRWg9zy$!0kRwjV7SW{lJznAQiffvU`!nPo;$WYKL12jA z4xXrJ?nHd93VjZ*mtr?EGnE?(r<@=Ai&*?P45(m3CXrd{)jc_K{FyJ$leIm9GXK9v>ey z5DjP!I56vS0x5EbUnTHJ0$#bEe3y5_HJkN0Q)~fMlqPb7X_`2^kvv>Z!M3? zyL)?vhX-e1jT<`j!#>_dNOH5&`X^&wQqo&QSB=&6j(Tc0?iu_pZRw=SOQgf`v5FHr74W;Q}ddu=Y=yr z{L!Y`kK#n+V`YWhmy5ke_`C&vz(W?^P2k`C-L?o*+kjLGTYNow^{cS8L@3f+<_7dv zQVprt`{;;P@vZy`$NizBdtpZ*W5q$_jO`m9iK$cLc>011k zup^vfu3TKI*gLO4k=7n}gI)fpRma3BZkQ-|KHM`XR9x;fq0|S`12?xo)$4UX4V9+g ziYSf5gBS!0)2}Ng{oId2JQF&uOms9n)^43Xj~g8zza!|WtGf@K4qH6roGIU<;B9}Q z@6~7Fn5VbYQ6(_}svJLokfN`@@+@<$mwABo4$!mmfTfY9hAMjYPBnw&?cCg5b>V4% z!S}ABm9LJBgTEdg?q37K!XA=r*6BDE(GL!xmjjY?qHW!c>pARHDaL)z_e&lx2Bb)> z(xq%FXi`c_ajM>WqX#Dqb;h{0K4)X^LCZa|*>mILBcW+i%^{Ej0wr;lVB%Q*L4$=% z5A_7W;5EOZmS&FF-j2Vl&D)LmF)>LXJlKg{lfl@l5i|yX4-mw!^=-3Z28Bq+-m9yQ z!XhrRS<0)9UuLgCe`<<@A;qs`8cKcXV`yG4kYIohlK%&N> zan9~1exp58M(}VYu&uQ{p}_81qK@T)iy6rHXwPP*e8(N-eH5OVyd5@b_mJ#_jeQ5U z>jhdQTbS$)b0)7mR7IFUM?lmw6x!A{u3Y()L_+n%@QstKT~(bcQf}u;%vZrctgezn z-b_O?CXbc;PX#FtK2GhLy_(hL#rLAiLfB{&eC0wdzAM6Mz?NPL_D=)ZLp@q3IIalF zT{O+U=s^20OX*Xujtv$ttYS(t--V)^IsTbUdN0h+liDc7eP=s<8DWX~A? zjzyVGTXUyIfZl^bPjbQVKzwle30#qX0IgT&eqbNspey>_@ZZ}-{hKkpPzC0&Favv| z7@*hi{cA+Qng>PRXG>pXcoVU83leL%1ZI`}31U^KYC*y19FV;z0TKiaC>Czp{R=HP z)HeAS9-lKUJ*AQn6x43hzVF)7aFeXj)cfDW6?} zXVNjnb71ooli(pN=ouwqaAIqgF-(8_P7BrhNeLe_kRMiMrBYn19Q>R?R5mq%!BGR~ z)R@8H2vmElGm%K96H9iY5UlcmeSg5dub{TC{RqLeOjrpAG6gc`5|r}xq^Thgrj^}! za=$iA_;ue+xv3a)h9sg2$T5DsJk>6Ov*e&$&Zl+|TW%D}HABZvD*0=%mPkUWI^keP zr1&xz)H-}*I^vVD!Sr0T<~KEtkuG3fE<~ zBk85Xa~k3KdlZav8U{ug89s-Y+^)Ouk!1ZE1055@rpWW^hYH04s+SF!yAcTn$BlA) zTw(Mrr#N1GiP=7w$RhVHs{M&1HBJ|e2ydeEnEhxPfNQ*Sp=lXDv^7bD3kCfD78xwK z{?EC-Ebe9yB)!||{i>O0bRqd7iT!?;lm3Urj&Xh3-cs~^7uIYvfdT975?!W3iK5m# za|b-bqX%CFt+_5KTPxr)Y7za_{pgY>SF)m^9*f+gUJv9|K;n6EU4~HiYUAnAQf3Mc z=L*z%fYBh={s52W#%?6s__R4OA}wUZ z(iJ0;>RP_-8lIr}s(^Y`*1~K#FT>*W(IjX4Z88?-e=PTlRzjys=0Vb>4QX&Pm=Je+ zFchaH?gR_zYNw1w`sJ#yT>Y-JObMdrzz^F6eCP=_`Twsa);J{UF6d!5W7A_9F!p=* zoMqN1WP)&t18}F`m?NBTS!-tYtBt!%x`vQgo!Po1l{{O5UM`es;I^?1Qy&mKc`GxP zTX|&`T=C9D4Y-ZqNj^Dp`7>RKPwwr4%nyRxwgvl?RvKQernZPo41mA$wnBUDG>X~F z4kiOOY1Wj+WWGtzifnn=0b$`@S^c%aDaq6>cCu}F;GOQ>I02YKJI+2uZ<7O|HF|X6 zD=crNsJ$@bR~UL9le~XH&ZP-mTl*}n6zUZ0)9}+PFZ(yF( z`Rtm8vdviRbh?@XfW2ze*l}&Tr;Ft=Jr7#+mW(M!O3&&(wkL*R0o@%=%>)U59LkRF zs&D*D&c58Wwky+%wzST*JZq{Z0(V(-UdMUVQd9ATWDKbUpI$dCDpZkbPpn3H`bCdp~xl@*5oKRqKNBLrL4}_Cz)2Bhlp!^2z z{W_>eUm7GAR`@j)s`=P+4DB3z@>;3bVB zC258MRXuU5#f~1e@IlVi;_6$GQ9)(!q~SL;!>Y^jh{fm?$l5vU+T$!Cs!~MtoWcfd z_kof;!e3@&<=Zb=)V~)_i!55jWEq``ZwXV}R~Y5ifL{q8hOXzw>HD!nvcsdv=T`{B zv3mnao%!>d%mlGZdt-&s)%A^xjXMJ}b+SwcNLFlhj2uX@Zt-)9$Pp@@n&$YL8-BZv zIxFhWW{7{cKV7Zw7Sv)_%c;Q8$;q&C&$qt0u$lVvMyL;pG1!$h?@LjeL%nB_a+3ax z%W$w_2$CX|bMKcsP zJlvh_6BrL^b#}OaWh!03k=cCif2GVTOIwdmbsL z0hc}&Y|qgQxUIL(cjObwpxs#8j_WcM;lR%*AfwmxVSxvaTiuH(2b;a2*@t)E&<`#a z5P~!TM=hjxhA}(8`G5W3CCXd>U4j!%T4<50?*cO;gdyz9$&7-QcGLtJf`Dv6)DMcWDfeFqZhH~Xq5M6Wvz5@ zRKM9$KK)Hr{r8}WRj1F84i7*(V|dwabcw&hA^-ZSy!7}ry6mOcm=Unb!JXLe*qP5X zX)|tEP~hvjJ-XtAp_`3XWowxPW(ei7m?96n4~vfIA!BR9MPVX(Sbeyhitu0b)Xz=R;TqexVBFpM@kSiIXD0%czM^RVW}-HH$?tgsb5B6UfpOdL<5p&fUNgRq^*C zVTyl>?nJ^jkguR5yZ{zaAOfzT@CV&*z^Qq68C82zH5y(cwiZHh*A@lQ>Dr^jP zKa;7OqWu|(aGgE3sr+bYh&DZ)(?0TZ{9X(cbR9NgF<-78-BczYbccdr2I})%(txn`xoAMzBE6Z||FKN-dA@d=jmw{T z?%O_*gF5`9cEdi!aD%5|Yk#WGqS44+zaI}egZmI@H00RgsnJ=SwK8b#wSk*veMq;= z9=pZ0l@2QU`KSmz>zP))C6g6FG*~K-STO?IOa_3i!=;O$nodD6^Gvr1XiYA)a0Law z^t6&&e1tkOX!ftfqLh1&Iz({XiM@Zd&UEmR%J4hhf=F#c zRLr)J!?N)joIqB?_LXrOp^C6ue}?6aW-gQu<}~hlKemp6`F z3jF+`VBH1#(;(_LS$#*U!}k={rgSTZEBtYB`iunV(!KIs z8zo&w`sfrC%8X|z2k+?F+%_uzVDz{V5a7I=$S&DT2{QiFOW=0mjqOA6L3uQp3nvl_ zJFTD!L>=D=vmgg-OqyA+eu&kkb8A@^rRu!|Wo8be(?1qd=E2DhRhCZcU2E3S>gUHR zx$aOH)6bB|GU$>Zu+sK}=S`GQM@IV8_F*?|u+d8h=XDPY&|HshH0qVg(aVw-0WuQ8 z709zPbUrtOf3!smMjGf+Ctq< z24M!#gP)JCcL>d88RrhWxfN{ro7k6#M}@TZC$7rMmi*GoSB9CfwL5bq*TvwbN6pT7 z_cJTETUxTnwF>-lB|dirrg5BG0e+vGNhT)eKWe*1FLPhc8k(>s?gb#?v87G2}2{TV2^1Z}(($LJhw^dR~`PYI}9W zrLPwj_#-%&0b~JLAV!{+;s}v=k3*UWi;MPtAdz3ak*_ZeOTGNA1icqoNhOIYF_Yl` z{0xAFh5h?0B2f_c1K^OD(VX(y*%f@b(vdt8W7)^=Pn~&~xjkD{#-I5$^IOXkrR385 zC=nvVdO*5xE%Dvk39~L7OG-EHk_iDnb?NDkc*w~=f3Ci}UH@c0Gv2K_xA+{t<$LCX z8zI2M@{Ri}B0eFek6tjD2cpX|d4 zd-BDCbhPkm%S!Y-+xGy$(I2x_#kyf>1F>4ojaiiG-kR~$OXXh$OozuR?`!jb=pG_j zg@Bcns%yIMPVXz^RE!xO_u$Hm^j)jrr_OBcWNaB2Up2r`7BgA&F>~mpo%P9Wzt3~f zg4k(9=*O}PlMB$P^pS%2LmZ+1Q$(4c(3cXDyszMsB#L9~(uu$I-vYe&356Hk?2;Es z$Ctg@X@0aPTjaHDIj$g3IE78>Wz{#*O|%#0VyIb`BsKHFo^+#ZwTBcfC4#)aCJpbi zX0^?rN~r^>y4D*ObzJnNF#;scQr!h_+wAjexlti#^MUBbkNATMd1izUFf zwX@R=Uy2v+cTgNw@~T|nJgL9RO+SeuQ-8wZc`QS+ukZoq5tx6mwiZ)WmB?eux|>g` zJ$nrGeI=uM^8v5q?3p-6;io{MXl|#Yw`pL=2%StHe$`UmswVZ1b?G5{yp#u^$%7;;)b*=Hu4wzE{+l?0KgDwvm0y z6X3Zi4WOy3p3V)m-EOz7D#}PKHheDUs{(ut$%(DzDK}->LIVS_1aIf%{e5-}&t@-< z&l2waWzNKe(*$v=4OQTG*lKw$4Xu9~3}tgtzV&Z0{6eXy#YmJgSAA|8)(Ep_5Yod? zgX-+y{}|!pyg%r?d4{LtN3wPd_i}B>)+wyJ;WhM(9QOxqa3ZFg{P-RUikBVgPb`o_ zQkrwb*Emf}$&{va)D|gMQirqYta_TKAUerGHrcYZ10_Q|$b-Sz(Wz0iVqyPm((dqW z%Vkx&dr99m;vvKauJI(m;K-3L`ndTmq2Q~lmmRdQ#KSryp>%$&E~OH_ynsE%+U1rWRr4jGo{^Dn+}=vwilrxquEv`7MEI`jaV0aO1o zJiW}N$>O5rRX>-Za$~QXywCh~i%mL3^tce^%Y?DePPNv$G|+}(uZ*i_loT*pUiW(; zdm)R6&pc0aTnW>K7o7S7hN-Y_Zeo>=z7U1$Yl96nT{k zalVOc;PQU6T%wimPor_&_6hLI0AC7+b{7Jnb{pR~2DMO8%pju~J+C1VRX*T$2e#I; z+nV%Oqb}QiQL}M8dH%QUh@n05QvV?&raK*+0zd$ZiT8IV<`)V_)WOF-0}|QGTX0(5 zhD=P@SuUHw#lGb5kNj{2+w(@qgEk+@H?%0&gC*K>mAw53+NA&^GazB*zAelX+&MR&GhJ^Tpdz^NowF))s;#;XIx5A{PDKRSS z>UgqZdFUi1+o&VApt+@Y)~)s=T+>;9tMW>D2P6m^mAGgib9(9;e8%pN_#Tvq_q?-i zwCB@xgDd7*kb5Mtu*jxF($qX)<$OZDG@6nl$Y3xR6UGs5M12RG6&g1Hn4eo|>o_WOmT0DN4!uO|YpvW&j>!iK5DnwFsjrw6V zRi^~`GW1$Q2n(QHzku5*INB<|Oc=p$TvAXpQISR!L?*>e=7mPRi^XrD6a@wap$)SX z6vhR9*SFhlxd4wt0~P6xn9HKQs#t2o)bx#$ti+)=7-!=5g~gEH>KgT-Fu>(EAoq!! z(8=>fzD_mx!?$ma=`s#(k3@SJ_QqF+q(d#`3%7GLKYF(tW*AOF1RJrLJU%8MZb?cU z{)zoi+0C>@!yCrAw%ca2TW_)c3zTYH4V6__y%e<8v&=W*#XfcfzX5qAsP4R4(q8&(QbT^!MXl?q zy8t?bvtj`LL3Jqh$H)bK99QV;&x~tvUqby=hBYUyJL<9NT5&a8g77Gdl?g0GPAr!N zQSbWT?UFNoSMyAc9k0u)09)9mA!GG>X;(pqb;gIN)XEpZCwcaj-R$TBP?6XJ^Tw5$ z&GDNNMX^AAE6?a1^23JThMs%q1els32nXT0oCbKt|fJnwH9rjU7a~v3~7$YYk38o zKf)v?G9vMg@OAWmSCm*eh#E|0YOvXh=B;3Ghm>5q37?Zw3od0_7&x?ztL0ncOVv^@ zYt;h`j1=a>r(M@@Av{-5E0)IbjvWOD2Qo#Ux;}`fz%hKc!YWoB@ff7jB%`mAn#EB# zOc1AfFw1#9ij)xSz{-&uo~F#%ohjBl1b;n};lK(9QH1{5MRfMMdRQ7|WC8s{1O_(; z*?dPXDmlZvB3wb!oJC6-e>TIkV!93J#6uQFk|q`g{RaOZzR_8U5Z9T zCoSk{NgGW~WJ)W_)#QsIudyl>O8EK_3(Z_bwRp!RoeoH<4+cese7Rp5r$-zssq;(s_NR3P5WRL(Q>z6+6Mmmjw}rG3aX=t#Aaj zt`aabfTrUpV<}!_)FTJ{AHm2fqC|aV$*4;|1;oM3keH(C*9d!4pGZX*{&CzQQsF7{ zg>irbzn}dJp6{Iv&$Ij5W7)NQ1P_G3U$T(|l*K0pJ(?@DH|w+_hoKX+uPR2q=M=Eb zSCEA0TjrCgD;Gm*kNck+rx^;GK4_{rPH%J6{7Q# zZsdj}yZ@kI%U3hGNi0F1oQ03dme^T_BI3*z?r3{EN25|Uy~IpIOglbKuRzWog}zt7 zX{m7GoMmv0N5Ci51~$l#J*1|guD!(*M{tpKW<7Q+lS;A zQ|;Lf+p)ZH4nY?%{GuD@3T#;ig`6mIYxvG3UQ2wgjQrSPw=v|3^J85BYCJZKq=9^C z=@oiA-Pc!QMo?Ff({L@3Y?7m+id?b4wt_n}p3V-5@3L6jJFMA9vxm#-bKY!qrRGBj zH$^B?OYNWdszq=hA0i5wg|B;`$2UL!hSQje4W3_CB2l z7_#9bGl`#&`SeF8)f5fddJcJJrajEf1ql#WtvXY`O5mLVd_J-W9`!xv9XFZt$YI<; zjbH3dPbJVYR~7_vP42AA`fN zS@I%OA`~nFRG&bwaa<=vM#h&6`VrH#E^ATmBl#`Ux%bXIN!hI{vc-|fGqexM>@XyX zA=8FPY=J=5)3-PmHNQCTt#DWPD2MQ~U-L-l zm*mEbC~{868>sgpGhlSXH*G6Q7X9(VhM$EO;d$RXfgKvcL`vxX2*LW5q%uF8K5>Hw zQN-q*17g@hl&-w;FO$$esTlUb)1d!xI)ikbtIQf*Xe|>=r5+qY`1J?Y(0nCDtmazpv{ci$jKRGk;WyleCImv*7Y|aWkCN} zN0$}Z!`lpe(SADXi=kX8zG( z=pZ3`v&aJ4SaWxM#~nWR!gG7W>CBXz&0Yf95@)K%^m7s?$m$=v z#JFioI!nQt6xBwfNsLbM1XAq#;Hf0A{&vr>VCEpbcNURBC!|J9!L=G`d>lKQ{D8{5 z0D59^8@(aLh$f%maUK_9QMVZm3FJGW@>Gx*cu-<2aRp@zq%verOw&6Fsd{2OQidJ{ zGqalFI+ebLyEdtvsF&y-Ov@HzEv3>B<;H<+f8pyjq(<7q&{F>@N?u9ZC#FDb_82%8 zIA$F)rl0w%OXFvL_OG@61K?w`v!3geqRKG0RgeeL{a^fNXFU)=^!dRyE9Z*T6fdW{ zS4|oEAf12AyMc5X(M^wVI@Tu%pc=6Xs8BVRJMyPVD%2oq)y^KRc%C^3{hvW32>YKN>Ef!nVW%Q~^1$87$ z%S$lJXxKheQaNdq@sI6H_t+R zqGQ~iPC4HnblyNYrPj^j)%@kbzUBc{)j1P|l^~3!c`Q6_hN4CFOUpN*%L3b1lNc1^ zOBgHL?+X5GtZtK8;jC8aEQ2qfW7j@+!2y}S0F3>L{r5n>r|sFuh@r|0r2v}cZ_0_c zgW_xME0NrAaeJiPR$6jZD(xW-mxNJ2+WCjQBi5GeQm^V6UYfqevX(8RRFZ2lCoWFS#>W|aIi(Q@rYnX!B_zu#Pj}7D=gDt~BNyauWGeI3 z%a7XgTg-{99d2IdKY)$k9(k9uZOh6)dxkU$vw$M?Le&xEpT~g33Dp}l6|J;dGF4BU zl)fkB=rwRT=N;`1P7yD0s3^7per6sJdjs@Bd&6Axx)9_{t;;zS~6Q;<5S8 zyi`J`AFb!>%0}rHKzrJf(K0bcWX5yHE1)6|1Ix~4;#>Z{=laYY4D^4+JSF}{L;trR zfJ^inq@41s*$*)!I$=nq|By}NoX1rd&v zrMJ80DU0zD+)Ve-(W5ELaR?cB-Qvw7#t!%vCi}cobo<%)swZ}(4`J&dmB5~kSa|Ju z>uBv&AuCI4uImoH>B}?ETiSy+vSA$xi@?}v`4V94_?4qm`Iuqk)SUma{%N^Gc{iXP z&$DagDGQ-oFUtmZymJ}STd1q6gK_vWw(eCrLCMK()aoi_ZN)vaH~_lh_&dGd_wEzc zC9@TZ0Qlko*HRy+SmlImNgJ-5Z@Ah9n4xV;hdxvMkzzg-46}Fn84_+23%O4AmIJ2l z31Z%z^gx7%k+#G4>Hny;09LHsjm^vd8fj^1B6qjYkBs)a9joHa!*Qe=wR?|F)X z-nwn&8yHT=>hfe#I*Y2*ckFm;G)rmFa61sv5jx#8yf*f%?1fGf&*ap(`avwD2K z2$ikrjc!qZ;&kLLpz8O@xULP{Z!q9BQ=UgfUJp)jwfs{+4FS~66v8e+_{gYL*lvFpKw?txf6dL;8QdAbYx<#|HMXGNk0(YtHb%A zaIyDg16(hNNZFsfl#2=H%RMQQpfJIGX?)U?p`SZtbI7R)_U@V~a#qdMO7}MKCBRI_ zQO8d9b*#55ggG*^U|t_T+j!>f&QVqgF)6=^B6^DcLd&c;9Z-4t=4@+u@%y>vbk9XQ zhwC+dbI;{9cec3R3W`QuA;x%2+*BZE2cd1O3RxVa4fYtr7SF>jC9 z6|dpui1L!B|9z@Jj5cvmMX{)o z!mPOV*nR>szN^t}XXj~%-Ax%|@kjl-Mfz$4+Y5DD4(}JD-6#65-{yIM#V?;xE|Mrb z#Ky0-aPBgJf;$kWL#?%Z?T=(HGo7Y{;347e#u?sPf!e8w0Ix4iyFPuMnwQ07k^sVUKn2vjL#1XIOYDA2@@B0WX#oQjkDZxVrLkLH(ke%6hkXN~ z0&!mlFVqL$;yVEO0J@{;tfE^1Tx?Z$TIZek0vs-+E5rJ1H<3PI;n4<%joNntnfIc{ zs8g7BQ36BpcdW`jRfEZtfc$orW=H%ao*yoxY?eiQ<}R^bwuI3;a?;_mgLZCg@hGzX^+Y1 z`Y@JO`k$q|q%_n4{T(fwr%tUgs0n}S3Z1IjWNKOb#tA07V8+yN=pBov|As}npUUkBK};7) zAk7dS)w?<9I~sxU+ELx?f0kdfS?w@NytP_=!FkC6j{KdhxVeba5%{Dsyc+O?Uw&Ok zkL}65oqgm+mV+kMDcLZ^%3whE6%WTjjmMRR>qXFMLI{V5VS+-$5x`b;&Hi@6@)^NT9ZI`A!}{u|7QQ2ay`4C z4K4n=%+PZ!7Yv~qb^adK`INJwo}Lcvdh5nl_b>~3XFhyM*PN{{NlG2m>wKe`XlTn{ z0=U718RN0_1zL6D!L=#n8My2m3sm>jTj>}p)P6cD06ffY5(vH;8Bb_`sJ4vkWAW(= zaOIss(*qLnC+E{eKSZ>*VA#jV9ciSMVm{w0 z$f6MXQ?_z{zn@xgjH@~%f4XBa&OM(`xWKRNA0lIji%%xyn#trs7!1U9J_;AY^VYx#2@?eO?Kos=V+DS#WBANl^-oggU|gS(dm;%{=pq!0 zyCQJQD;FYI)5)^DL2YZU|FQP-RZ5B_ylCm~Pgiw=yQ^a)zWtuw^gXJS7cCj_xF&); zjhs}@pN8Na*~^JC;Xt50gMjx4NVF^^*@;ax;3YoR#9i}^iDcRTy8^%qR4 z!=5_mNm`^BlcDWe&`JBd``|xp3T?i^v}pmCaq~{{>^RO~^k1WP@Os0Ni|O$8IrOVr zDEmh1>e@Pq=8`LSta7Cld$bx_3KOs@mVQF`@|!$Wb}m{>n+V1CjG?}P^%rdbf*>oA-ccURwGG2L``a4GBmkZ_yYauTG+dB){E1gY&6Jj1@V%fj(^Uj54nKP zHhdhZDeDWwDHA-c6wny`<>TD*KmCa!{!F%J>QIY z9_^mZ;w@%nw(RCX0K0%B8<+x2JH87dtM|&2nH$n=xvGuyBZ5Kna1pw3Ii#JrAY#=t zL$eAy7uh`xi~#t3xLXKyq_Mz)3Cy_@^vO^WQ6yMYDtwmk_FmY4i^MPy+N^Oh{hYvF zAt!jn7lHE@=p$Ye9h{}57F1V~d0)#7qjia+W%hdPQ^ZASqz|8SwkFI1zHpXFn>e+! zgmQLcR+k}xG z33OfTsIaa+no02!7^m^fcNE*7)~1E|C3`CjXzKz0THJiu)hT%$ZNI2c^(LF|e!p~IRUWA0SJB(H_DgziIz_0sS z)ac296i`eQnbHM~9%%tVv-VPeq#6Tav$;&Q2A&t)lWn)vvurbPw+4C-dp)f%ZZ)MU zj+g`N`O#L}G}FAa-}cLiPvlu!kCB(msjN~tNlUBw z7Yoky=hm4dS?K{ndhh6(gU^zT&Yp+j;ZKigurW0~G-~r(+`I^8qGjF$Bjxfvkc%F< z3!k6O#9{~&0IwNRZ2Q3G~PrXy|zJ9^ik(%OVcBgZE{Cyo%q+ zzU!Dwu5vG=e+jd}8F;t}^grJ_z&bH#deHK_0wSeg++_8w2?9*VL(czQq4mAT zwKiokwPz>suM!`WLESFnkCoLOHaAA5UN1)R`c-Gj3r~_2M3wWtUlL0>q=_l-N&q#dwGPnZ9P@2Vk>3azrw~nqR zrvOyPXB|%+l}}wV?@1OI99Cz#+7A}}YtoE`SZZ=9!g2*E$M~0@hgYS;{C+xXx0;Wa zzLv{~e*8J6J!8ykZ22OreRfrU=c<~V8k2vzB%glBH*29Fh=-6RVWIf*a>V;Nsl4a6 zLX<4boVGef#zQZd;jStZU58{&XzztE@d+St_jJ7S%Sd~`;uEP8QhWuH!p~JA+uxxz zUb-RF7~GcP%5k}=!~k2`(FG5CuJ*&1CG54cSq6AU-Idc&&}UxuHb!L47m zjAMDCS+jgSPw8#Y(*rn)h40S>d*8G_ol_$_uD$5(o#d_oFOTnO!7#8e37PteSp-J6`+KdB)v z_fP&k*DjCwp8t&bubwMU0H&JQAy5CW#;PFse_ACeV7{k5|ECqJ0s;5~SmGMk2P80X zOwj+yvQB(Ikw~UUhy3>u{wwEMPmxFWuj;?!0|@^c4)`Y<@j)PSCr5WnM|Y-A4)!YY z{~7mxwoFO<&nx@)#@@pR|5ehN6OR<}fxn|uZ1rkeyr+B5vw}eI|J4Kpsulu)+}yqG zE#3Zm@c&5-|M#x*Awi(O$9wnxD)Q1Gkb|YW>3{e7-&>0Ci(bJ31A$f{Kp-rX|LOz+ QVJajhDp5n*EB@8~KNYfP=l}o! delta 15149 zcmZvj19V(n)UanJwrw}IZJQ0+*tREWk|t?v+iB3)b{aG`+Ss=K^nE|C|2*rgvkzzP zUH8u3XV2Nq1;FksAd-qa1SBQ^01E)rnx)4fDM7p!3BQ1337d-W2!Fd^VPXI2O4Rt< z6?eq<&xlVz?*AGo3q=1%76Oqav;Z;xOQr$+EB^wr|3@1q^7|4JoJ8HoaaaIA99BXg zNED2vBP$`Q;hA}o_1Tg@HhT#C#V?~I#Bbz_8KFEmhp04<3*u>TEJH$8W4zuHM4+iT zQ!EgUE5Sze7ZR?xElVH)D`}Pmwyq;E0TCjwAD>MN1&>->W;uAk$EaP@qckbChUD@G z!c(G!fu_gzwZ|DXy~D1<4H#Pw?G(A4(+#Yq^425mu2;>vB}hm(`mW1X5bPhS1UGyV z@TQ{#IhtD^PhcsHI}3~Q5JRDjm#?M~eUcM0W&oA+zx!Y3BN@`(HvAKyFPwqg-3?(w!8)p?}pwdlJtux@#%Iclj8K={`?&V(4c6v zzm|&3QTutJ0zEjxm@Ma%oszV{+M4i%9(=Ki&CABm-`3#PzKqFJeTYYVUDeHmx5yBn zs?bHyKv-@1%RgE2vW`~l&!%T&N_x0j*{3#`hhj>LB{ls&CQRCFnTv~Xi_`!g#f`8g zHWjPH!|sw?N_V2Nxm2paR6hoA4O40}*be<5PMy3Yz>63PP&k61&49 zr61(0v>Dp(En!Uu9^n`~uujExe_M1VmMFiRzfj^Voa_Wub4PO(Kk0NKKaekQ81f9T zX5$1bxbM3oMDHbKf=GM%JG=n0?O;X!x9j^}RnG>Tpq&2D{(YL3C>$uAap7RkiH3C2 zuxKBUA%a7ArTRpCj?3{!POtRm*qT_IIT2I}F4zD>8HVUDQhR!No1r7XewSxA41wfL zOdZ@G(g153z=Qy_CYhb^y@@?89*yl0P-BL0;-v?1Za->=BCKOk0Fo{J0PJWC8$R_k zzC&Bj*hbwrqc)Gdq<8OZW2PJXJZ<44Vssk4mw#hA8bJ@{;1j`n^OeCYEfOGI%$sVC3-j+o#R0 zV}>Iyq1V*VH>#wEy_jU?Ri`WHblOqHfl>O=ua8~z^N3bn(Ku#hMZvoHY3|kT^A`$$ zS1SnU{HErpu3sC3cMb8f|Kke{03Zzbk#2p;4~ zt^)P&4hH~Jp!wam0WgWLYe^%_vvC;@mZAg_6*{)D9aOoO`yK_mhPK-2*ci9Zv)BAw ziEM+uK)kvSz^!2DOLI!408Ye(1eKl;2;|Jy~!Tl6`9Z2MF@QIWKA4z`l8(VddBoH((5Pe&K>y?ozi9|S}!fUkLc4_jmW+hEuVtKbGagaR^1?({2cA^3dFB6@66RP zJ)S<ellqBPUWz`Qk5*kq~hykc6-`O#nOz9v{CHQF{ z!J08e;JWn_er2Um$orV#*VsPVQ8OKYeGhiBL~LB4D+6paU`UWn(18L|Rj}C%sPKg3 zWrm(vQaLK#h_W$0U)^`}y*yu+B8)^Rbk@mHvFQJ0!sUuNb2jMCdOx&3M3B*!U%E(u zQ`+`ygxJeMU|oIS!CsI1Yk$px63FxSS!aOeBRH&oZ&g9Gyzcv*#vFS=_gY*76T&(C z`HkumnBU2ix1}~AE+J<<1f|p8N2*XYU{*!gS|ayZ%gY%jKkCoS-8{7iNsYx#$a##` zm0V=*5T%p%jm%{yI>r7hO4rju#-wc85nij~Cj|QXOT49*qsvRb=Et23&iPv0Rqhvi z?{C1Ubv*{S?uw!m;?JQ&F@Wff=$%jPY3s~jaD!4`ri!{&v@o~>fu%?iGCec>D@ms} zK0jB>6IM5;<rx8ccJ|UfCC#~;Y|LDAI0%+7llAl5l+D#GBWOkb3wRg8!gsw5n=s3!M44i2V z`bJTODjxA(T@Mr1mIy`~%K8ocjZ8x-<{n9VEzgOrK1gIt^**6HR1&Ukjp7xUB9Z<_ z8K`%i{_h9jM&xrRC857xJRKEV5V zq~g7sMxn-cF77v*CbRjc$izeU-1oR@w~vD;?~pML_pmK3C= zNQCb!zD&Dv>G!oHh!?H=D7xifBF_!TNVr69wxGNMSkjDUNgkvEhi=ARle@c_3Y%q7%_9!h zeHw56x9O<)+UD%6peQG_9@p9RQg->-T9XGveO?)%Fv4^jN#us_E4Q%6}ZIp(vaoSzyXM1X|Q>_eTOOeo!JS~?b!_%LB4(;BYHD9<}ZME6*XapB%@UvwTF z3IGZ<>w}9-&~LbYFk5@AUqC<7@&-2b3P>DO0LX0bKZ)S$>>!XMoIau{^VT*3Cf_V= z`2~S83>sztt!04`0BV!fq=k@NBp1+buAeLpiHRAo2kFV%j~f6w$K7BGg@yY7pp4CB zM*Xq~fbYiK%MOgrk^kr@3iNz`1vH^CF#+h-DuDF>mobVjog%LT$-j~4`iWEk#fS@o zi@1XDoZwB+2Y@tcovk4AMU50~Tbkj%np1QkZJrCH(#r6HJX=mVy=qapLe^j!I%{Of zN-iKXFmTx%Ld_Y8G>5da{Q)|?_QLx`=zy& z-h6L#RH~>!v^Bg)OQURQOmfrn5EI7t><_Ai2#}{&pQ+@TakA`bnMsk>X7{s@Oi*Rz zrB-{zaSDaN`jaleFVGiBsM`sXfFv#tprA^GjqH^+|L_lT4b=pLw9C~Xzg3%ApjpeOnWvK$t zIshM5>f?UixzIGL*mf#~kphzDK|=)uJaE+H?<0(mO-}x>gh{4)&{C@^A1OZ`oR87czn$UgR9$L2yd;CI z0@28tnP9A}0!X&}cQ0_OyEOa@uxFj$nfX0ALfV-!ubd%#`*NW5*x&?==JI|;mbPM* z*eyet%(WG4gI0P=i;Zi8KfQ^ZT_oT%SWY!K88!t2ws4i4BKM%&YM3UEqS{Gey+6Gg z>W!dcq$}n%fMby^h~A&X*<3)ZN)SjyEXo1|>k{DmqIf|!(Z$@UN;k5-Zkh>VhJIx& zefx}VbjI;xYTsd9zya((49D7kFAja?#NR^}tM1BqwKwjJkHS00>-b=euUlX(JX_+8t=k(ceK9NQj&)hsrI$2HZD!o?TYMyh z=7%y|`}1VzvF8{jy(y)~n!SNF>QTvet1e$OB}lfZ;MOP5{8uy+Yq8_ z%&OVdkVYPQyY*mqlU8V%2N}h2fSpJTAWbsnM?T~XzQx$tKcV*6yo5C9R7=-OoW#=v z_(5|u8qV|TVD|+`Z<w?)twc}(ajUN# z@O@Ntf}U?a|7qwQ-$Nlsz~g;5Fu2)ld#EH~R-R&{eM6R2s5-6?yEyQ%Nob2%tlLAB z6w)2$g?0-2Uh8+eb|_cx z-GO)OyMLemd=fI5pi|`0%i#$42S>ygb~U6RYAhTjr%#&T^XL_g(D8Ex<$5uA`+K#C zO$q)&PA3dQ`?uGNlL(NkAJxDn%|#yAH)Z$X-~#_c+L`e4^FAwMh@j1fqtj=&B6chJ z!g6(QK!Ez0>d~9l2iFDPL>G>0T4G(qSB}Sv!D-aet8at(q;Jp2?Qeg^W8~7U=jq-} z%&`p^K4V@Pfj0NF*Ry1Y2RG6Q}-r22&F9=&h zGBKhs;XPd5$PS>W$Ir2<^PbQAJp+y5EGq*n{!$w-ao$MVqL9%92~hN+J!sltc>}0) z1r8KQNyp}VS(mNEd3{b6&4I>X*y^b!(IA_en?%M>Pe$D_ogxr$^9H#YlkL3L_nE=- zXMEX=VHcHa6nVWrOHp6bv?Rn3n8GDMvYUPR!I>9VbO;|l0MrSI3ZsfF%uwGIFSKoi7HuYibxLp=tLZRinn2F7 zX@j<6{4B-_q-_3_}JJrygFu<^egnncL(c04Zy(N|Nh8<53mD+L> z*k%^Vvt)wjA%MYYoLaEi>mmIXuyHm|IDoA?CdH*P()(*BzNRBRPENs+GtZ+3@H4C- zgXonT<~5Wvp)UvnS_PnF^iu;s_sdEJ5FPN908<6GGGp)ml5Rnvk zk!5d10lxf!uU^0iImEE-BD~Mu1ePWpC)-Hx_#7^U3z#DffxF*bYmSowjVqDwDkJ#j z2(h9v2|Sc6P~{hA;I&#YhRQ<@xc(~r<%_yt0Api>$4J-=4-X{ze;X|5rCV)(AgeuF zBd8idN1!zo!41hR)xE=D(i0vplx`8bIv{NebgbHXXdA6qesY8b#<;8W{JbiRO%SP| zBlw#7o?N^_SBV92xM*G%wfF-K-ICnObc|G)UlHt==8!t&K(+AYByVdtIxLn7sNV;N zIA3{*OYTO~tm|sv8q&S5KiShuYX{5lAdBhuhyoo2{MuS-$0T#R615BIyGW^jnP%hY z@n};J+$#{0+jQ zm9HaS?HCX6o@6DJB&x&=zx?aV{SSw&#NVF(ObG9SiG#hXnY}BMr=9JI!HV-1PrTno zwV|auO}V!oY7f7lopy79Kn<3Whh3t^)Y_ApsC?p>)wk{VMCKQar{*{A&ujn(kPKKg zQ9-U)V~NO250aD`Bito69P)l9bgzhBVA*KS;;?eZ><_1^Ku zLHOaSg}s5<{MEIgzQdE}rX%$kTgHaDRYsl}=KHU<^UC3HCIK5OX`GAeUKCqq2e+R2 zPMOoBPvL)XJoa{F%^KZog@@X>irv61jq@+39!hj|l@@t7^)@EDDIb#Tgzh}-o(Ws} zT9oTM5i1sHWiO8T)=5%=LV1vxqzXz(Xs#(Kg!LnTSfv*f)}wmr&LU{#Wz424N;lff zWJpp(o`0xrt2eRHey}vqqI76zZ|cj^G#St;(hm9+ErWI0Jj>$oMBa#G@i_(@kyUx* zq*}0h5QKGVYawg4uCcvzT1-B<*eFl$UwZJ_#8nzTfyAT9rVT!&Z9QUsM}re7%W!W< z!KUuSZ-ZH)2x+<}JB&U&pG{7fy&R4lp2fB1tcn)nKksYa&wV(h6gefw;`0tmu@?-Unr=TN&m8B{qWUvT0kVKp z_!<-aa-9MkQd&WpVZbv7-6$%?%Q4{C+64@tRjMPZ4Y|8Wd?+`ktIGjH@Vk~aGNjvaNuL%RA?U0e9A_~=#Q%Mmy6?mpe+QelQF^6 z0Z?iYTq1N95n2+R7g>H_-x_LcxT0yLGl|iAIu<|dbi*HTkUy{}5U4)oeEe-23wD?{ z#f=?m>|3m%2+U<~G1v#c8KLB4;z>%Pg^#-O8p;=U9$}n5M5I1*qn6G>iuVa0$F9b1 zUba>iPBq#>8J7}7>_y90Ok4@_w~1JwZg!YA>k$8dL|T9on@1EVy?B8D?Q9I4PtqfzCdut)U8GK@&yvIKfitIK$K3zB zd|5Iru+U7RNPdVOKUOH#so2_Ekh}QB)z;Ud-7bB`YDyd2y~3{>{*d(%E^EcFtFG-q zpmyuGiyGZ1=K!81y3U`>kTSSKoVV?l{d69fe#SE>zgb3O*cc;A$foZ? z8NImYM0O88rwdEsuu^X0Q-RFyDLcv=AQ~RAPp!RCMw=!h>UosRB&GuqHrnA*lC7da z%fdJ>w`L{3cnlDR>1a8xh=u)ur-zd&A3`V&wmB zs0EU|oVXbJ^e$fJUz?DPw(dF_`MB>Z$mR&}s`i8DU^6}CZ2wHbXK}>Pb;)i&%Mv4f z7tLHy?*Hh;&hI(`$#HUI>{-zj$3>_cXX53177R<8vF=%GZR%mY44j&@3N}|5;MPg4ba`O>yX=>MfDj6_k zG|PE%-&V%FJ5k;vnyjg;>bmxYZC{%0YIUsxKWW@8_A1eFC{P{EMYc z5;5FxbrOiNR0{{*D&q_x?I@P#wXc_L3s5*GSWBET#Pscig?F2k{>gH(GD$uwxvdonk9Bm|s)Wi}0DufDqY^zLn57^=EHr zi`Z+~c24qwC)_Z5u#nSTXr5pcql2og%g0A{i_=9it4JhqsapH}Z^h}~Qaw~IbVQwd z&xsz4vjj#Rwl_js$X2Pl6o21sdp`y=Z98NNpExWz=pHoQugKsVmM6ZUf&E9K246Qc zotqhKBCd>4B1o9Dt0bjiiAZmd99w$2B6?BcwmP0zXu$0VnP^=7cV|uDaE(`V^2+m| zNKUUpo7%>X8hv{OkBR5K&=yKthmypV0skN3>!^-;UyL@Gw5Y4^xsL zYWVslmzFd^YtS-}0~=6tzRkisFT`2WAp&J!BTn!(PajjN7n-^O;BLTQ1PueIBa>`c zwd`YR@Yyt@dUS}abR0r!rZ*401ztI_SA8K9q5cC-&9Sd+l%~e3#O7T}LJ<9+f4Bu0 z^P`x3kTIh!k>wHRSn~YZ2)(}c2M-2SHBRA^#6V)xaM+G%_@70w(ac!W5sqI}JIRH9R;uaGjcsnD1q69ckE7@zW&