From 08f1e6a9dc86c03015cb2a83dc94c3412d6d9eb4 Mon Sep 17 00:00:00 2001 From: px4dev Date: Thu, 13 Feb 2014 09:20:33 -0800 Subject: [PATCH 01/23] Fix base clock frequencies for timers 9/10/11 (not currently used). Thanks to xiazibin@gmail.com for pointing these out. --- nuttx-configs/px4fmu-v2/include/board.h | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/nuttx-configs/px4fmu-v2/include/board.h b/nuttx-configs/px4fmu-v2/include/board.h index 850043ddf54..3bede8a1f78 100755 --- a/nuttx-configs/px4fmu-v2/include/board.h +++ b/nuttx-configs/px4fmu-v2/include/board.h @@ -141,9 +141,9 @@ #define STM32_APB2_TIM1_CLKIN (2*STM32_PCLK2_FREQUENCY) #define STM32_APB2_TIM8_CLKIN (2*STM32_PCLK2_FREQUENCY) -#define STM32_APB2_TIM9_CLKIN (2*STM32_PCLK1_FREQUENCY) -#define STM32_APB2_TIM10_CLKIN (2*STM32_PCLK1_FREQUENCY) -#define STM32_APB2_TIM11_CLKIN (2*STM32_PCLK1_FREQUENCY) +#define STM32_APB2_TIM9_CLKIN (2*STM32_PCLK2_FREQUENCY) +#define STM32_APB2_TIM10_CLKIN (2*STM32_PCLK2_FREQUENCY) +#define STM32_APB2_TIM11_CLKIN (2*STM32_PCLK2_FREQUENCY) /* Timer Frequencies, if APBx is set to 1, frequency is same to APBx * otherwise frequency is 2xAPBx. From 1be3ea4f4eac121a627c194fefbf202586f6f66f Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Sun, 16 Feb 2014 12:18:22 +0100 Subject: [PATCH 02/23] MPU6000: gyro topic was not initialized --- src/drivers/mpu6000/mpu6000.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/src/drivers/mpu6000/mpu6000.cpp b/src/drivers/mpu6000/mpu6000.cpp index bf80c9cff21..ac75682c471 100644 --- a/src/drivers/mpu6000/mpu6000.cpp +++ b/src/drivers/mpu6000/mpu6000.cpp @@ -1353,6 +1353,7 @@ MPU6000::print_info() MPU6000_gyro::MPU6000_gyro(MPU6000 *parent) : CDev("MPU6000_gyro", MPU_DEVICE_PATH_GYRO), _parent(parent), + _gyro_topic(-1), _gyro_class_instance(-1) { } From a6af669399b8f12e497aff6292ec35dfd541d903 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sun, 16 Feb 2014 14:43:00 +0100 Subject: [PATCH 03/23] navigator: switch to READY instead of LOITER if landed --- src/modules/navigator/navigator_main.cpp | 66 +++++++++++++----------- 1 file changed, 35 insertions(+), 31 deletions(-) diff --git a/src/modules/navigator/navigator_main.cpp b/src/modules/navigator/navigator_main.cpp index 260356eca99..5952d667fe5 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -305,6 +305,12 @@ private: void start_land(); void start_land_home(); + /** + * Fork for state transitions + */ + void request_loiter_or_ready(); + void request_mission_if_available(); + /** * Guards offboard mission */ @@ -699,24 +705,17 @@ Navigator::task_main() } else { /* MISSION switch */ if (_vstatus.mission_switch == MISSION_SWITCH_LOITER) { - dispatch(EVENT_LOITER_REQUESTED); + request_loiter_or_ready(); stick_mode = true; } else if (_vstatus.mission_switch == MISSION_SWITCH_MISSION) { - /* switch to mission only if available */ - if (_mission.current_mission_available()) { - dispatch(EVENT_MISSION_REQUESTED); - - } else { - dispatch(EVENT_LOITER_REQUESTED); - } - + request_mission_if_available(); stick_mode = true; } if (!stick_mode && _vstatus.return_switch == RETURN_SWITCH_NORMAL && myState == NAV_STATE_RTL) { /* RETURN switch is in normal mode, no MISSION switch mapped, interrupt if in RTL state */ - dispatch(EVENT_LOITER_REQUESTED); + request_mission_if_available(); stick_mode = true; } } @@ -733,17 +732,11 @@ Navigator::task_main() break; case NAV_STATE_LOITER: - dispatch(EVENT_LOITER_REQUESTED); + request_loiter_or_ready(); break; case NAV_STATE_MISSION: - if (_mission.current_mission_available()) { - dispatch(EVENT_MISSION_REQUESTED); - - } else { - dispatch(EVENT_LOITER_REQUESTED); - } - + request_mission_if_available(); break; case NAV_STATE_RTL: @@ -770,12 +763,7 @@ Navigator::task_main() } else { /* on first switch to AUTO try mission by default, if none is available fallback to loiter */ if (myState == NAV_STATE_NONE) { - if (_mission.current_mission_available()) { - dispatch(EVENT_MISSION_REQUESTED); - - } else { - dispatch(EVENT_LOITER_REQUESTED); - } + request_mission_if_available(); } } } @@ -1397,6 +1385,28 @@ Navigator::set_rtl_item() _pos_sp_triplet_updated = true; } +void +Navigator::request_loiter_or_ready() +{ + if (_vstatus.condition_landed) { + dispatch(EVENT_READY_REQUESTED); + + } else { + dispatch(EVENT_LOITER_REQUESTED); + } +} + +void +Navigator::request_mission_if_available() +{ + if (_mission.current_mission_available()) { + dispatch(EVENT_MISSION_REQUESTED); + + } else { + request_loiter_or_ready(); + } +} + void Navigator::position_setpoint_from_mission_item(position_setpoint_s *sp, mission_item_s *item) { @@ -1555,13 +1565,7 @@ Navigator::on_mission_item_reached() /* loiter at last waypoint */ _reset_loiter_pos = false; mavlink_log_info(_mavlink_fd, "[navigator] mission completed"); - - if (_vstatus.condition_landed) { - dispatch(EVENT_READY_REQUESTED); - - } else { - dispatch(EVENT_LOITER_REQUESTED); - } + request_loiter_or_ready(); } } else if (myState == NAV_STATE_RTL) { From 92578e1fc3a3b011aa361ecee65d1bcce1593f6b Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sun, 16 Feb 2014 19:34:44 +0100 Subject: [PATCH 04/23] navigator: ignore min altitude on MC, loiter always at current altitude --- 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 5952d667fe5..577c767a863 100644 --- a/src/modules/navigator/navigator_main.cpp +++ b/src/modules/navigator/navigator_main.cpp @@ -1059,7 +1059,7 @@ Navigator::start_loiter() float min_alt_amsl = _parameters.min_altitude + _home_pos.alt; /* use current altitude if above min altitude set by parameter */ - if (_global_pos.alt < min_alt_amsl) { + if (_global_pos.alt < min_alt_amsl && !_vstatus.is_rotary_wing) { _pos_sp_triplet.current.alt = min_alt_amsl; mavlink_log_info(_mavlink_fd, "[navigator] loiter %.1fm higher", (double)(min_alt_amsl - _global_pos.alt)); From 44c095b9e5717c707faa3528b861bad8eb7ac94a Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sun, 16 Feb 2014 19:46:57 +0100 Subject: [PATCH 05/23] commander: allow arming from RC with safety enabled in HIL mode --- 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 8129dddb30d..6d14472f360 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -430,7 +430,7 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe arming_res = TRANSITION_NOT_CHANGED; if (base_mode & MAV_MODE_FLAG_SAFETY_ARMED) { - if ((safety->safety_switch_available && !safety->safety_off) && status->hil_state == HIL_STATE_OFF) { + if (safety->safety_switch_available && !safety->safety_off && status->hil_state == HIL_STATE_OFF) { print_reject_arm("NOT ARMING: Press safety switch first."); arming_res = TRANSITION_DENIED; @@ -516,7 +516,7 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe transition_result_t arming_res = TRANSITION_NOT_CHANGED; if (!armed->armed && ((int)(cmd->param1 + 0.5f)) == 1) { - if (safety->safety_switch_available && !safety->safety_off) { + if (safety->safety_switch_available && !safety->safety_off && status->hil_state == HIL_STATE_OFF) { print_reject_arm("NOT ARMING: Press safety switch first."); arming_res = TRANSITION_DENIED; @@ -1156,7 +1156,7 @@ int commander_thread_main(int argc, char *argv[]) if (status.arming_state == ARMING_STATE_STANDBY && sp_man.yaw > STICK_ON_OFF_LIMIT && sp_man.throttle < STICK_THRUST_RANGE * 0.1f) { if (stick_on_counter > STICK_ON_OFF_COUNTER_LIMIT) { - if (safety.safety_switch_available && !safety.safety_off) { + if (safety.safety_switch_available && !safety.safety_off && status.hil_state == HIL_STATE_OFF) { print_reject_arm("NOT ARMING: Press safety switch first."); } else if (status.main_state != MAIN_STATE_MANUAL) { From 7f7ce77d616679aed7a923c37fff8ef7a6261da2 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sun, 16 Feb 2014 20:14:39 +0100 Subject: [PATCH 06/23] mc_att_control: parameters descriptions added --- .../mc_att_control/mc_att_control_params.c | 119 ++++++++++++++++++ 1 file changed, 119 insertions(+) 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 27a45b6bbc2..488107d5856 100644 --- a/src/modules/mc_att_control/mc_att_control_params.c +++ b/src/modules/mc_att_control/mc_att_control_params.c @@ -41,16 +41,135 @@ #include +/** + * Roll P gain + * + * Roll proportional gain, i.e. desired angular speed in rad/s for error 1 rad. + * + * @min 0.0 + * @group Multicopter Attitude Control + */ PARAM_DEFINE_FLOAT(MC_ROLL_P, 6.0f); + +/** + * Roll rate P gain + * + * Roll rate proportional gain, i.e. control output for angular speed error 1 rad/s. + * + * @min 0.0 + * @group Multicopter Attitude Control + */ PARAM_DEFINE_FLOAT(MC_ROLLRATE_P, 0.1f); + +/** + * Roll rate I gain + * + * Roll rate integral gain. Can be set to compensate static thrust difference or gravity center offset. + * + * @min 0.0 + * @group Multicopter Attitude Control + */ PARAM_DEFINE_FLOAT(MC_ROLLRATE_I, 0.0f); + +/** + * Roll rate D gain + * + * Roll rate differential gain. Small values help reduce fast oscillations. If value is too big oscillations will appear again. + * + * @min 0.0 + * @group Multicopter Attitude Control + */ PARAM_DEFINE_FLOAT(MC_ROLLRATE_D, 0.002f); + +/** + * Pitch P gain + * + * Pitch proportional gain, i.e. desired angular speed in rad/s for error 1 rad. + * + * @unit 1/s + * @min 0.0 + * @group Multicopter Attitude Control + */ PARAM_DEFINE_FLOAT(MC_PITCH_P, 6.0f); + +/** + * Pitch rate P gain + * + * Pitch rate proportional gain, i.e. control output for angular speed error 1 rad/s. + * + * @min 0.0 + * @group Multicopter Attitude Control + */ PARAM_DEFINE_FLOAT(MC_PITCHRATE_P, 0.1f); + +/** + * Pitch rate I gain + * + * Pitch rate integral gain. Can be set to compensate static thrust difference or gravity center offset. + * + * @min 0.0 + * @group Multicopter Attitude Control + */ PARAM_DEFINE_FLOAT(MC_PITCHRATE_I, 0.0f); + +/** + * Pitch rate D gain + * + * Pitch rate differential gain. Small values help reduce fast oscillations. If value is too big oscillations will appear again. + * + * @min 0.0 + * @group Multicopter Attitude Control + */ PARAM_DEFINE_FLOAT(MC_PITCHRATE_D, 0.002f); + +/** + * Yaw P gain + * + * Yaw proportional gain, i.e. desired angular speed in rad/s for error 1 rad. + * + * @unit 1/s + * @min 0.0 + * @group Multicopter Attitude Control + */ PARAM_DEFINE_FLOAT(MC_YAW_P, 2.0f); + +/** + * Yaw rate P gain + * + * Yaw rate proportional gain, i.e. control output for angular speed error 1 rad/s. + * + * @min 0.0 + * @group Multicopter Attitude Control + */ PARAM_DEFINE_FLOAT(MC_YAWRATE_P, 0.3f); + +/** + * Yaw rate I gain + * + * Yaw rate integral gain. Can be set to compensate static thrust difference or gravity center offset. + * + * @min 0.0 + * @group Multicopter Attitude Control + */ PARAM_DEFINE_FLOAT(MC_YAWRATE_I, 0.0f); + +/** + * Yaw rate D gain + * + * Yaw rate differential gain. Small values help reduce fast oscillations. If value is too big oscillations will appear again. + * + * @min 0.0 + * @group Multicopter Attitude Control + */ PARAM_DEFINE_FLOAT(MC_YAWRATE_D, 0.0f); + +/** + * Yaw feed forward + * + * Feed forward weight for manual yaw control. 0 will give slow responce and no overshot, 1 - fast responce and big overshot. + * + * @min 0.0 + * @max 1.0 + * @group Multicopter Attitude Control + */ PARAM_DEFINE_FLOAT(MC_YAW_FF, 0.5f); From 4b41a398e71d177661cc5127427448d55bab944e Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sun, 16 Feb 2014 21:05:54 +0100 Subject: [PATCH 07/23] mc_pos_control: parameters descriptions added --- .../mc_pos_control/mc_pos_control_params.c | 146 +++++++++++++++++- 1 file changed, 145 insertions(+), 1 deletion(-) 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 9eb56545d75..0082a5e6a6d 100644 --- a/src/modules/mc_pos_control/mc_pos_control_params.c +++ b/src/modules/mc_pos_control/mc_pos_control_params.c @@ -39,20 +39,164 @@ #include -PARAM_DEFINE_FLOAT(MPC_THR_MIN, 0.0f); +/** + * Minimum thrust + * + * Minimum vertical thrust. It's recommended to set it > 0 to avoid free fall with zero thrust. + * + * @min 0.0 + * @max 1.0 + * @group Multicopter Position Control + */ +PARAM_DEFINE_FLOAT(MPC_THR_MIN, 0.1f); + +/** + * Maximum thrust + * + * Limit max allowed thrust. + * + * @min 0.0 + * @max 1.0 + * @group Multicopter Position Control + */ PARAM_DEFINE_FLOAT(MPC_THR_MAX, 1.0f); + +/** + * Proportional gain for vertical position error + * + * @min 0.0 + * @group Multicopter Position Control + */ PARAM_DEFINE_FLOAT(MPC_Z_P, 1.0f); + +/** + * Proportional gain for vertical velocity error + * + * @min 0.0 + * @group Multicopter Position Control + */ PARAM_DEFINE_FLOAT(MPC_Z_VEL_P, 0.1f); + +/** + * Integral gain for vertical velocity error + * + * Non zero value allows hovering thrust estimation on stabilized or autonomous takeoff. + * + * @min 0.0 + * @group Multicopter Position Control + */ PARAM_DEFINE_FLOAT(MPC_Z_VEL_I, 0.02f); + +/** + * Differential gain for vertical velocity error + * + * @min 0.0 + * @group Multicopter Position Control + */ 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). + * + * @min 0.0 + * @group Multicopter Position Control + */ 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. + * + * @min 0.0 + * @max 1.0 + * @group Multicopter Position Control + */ PARAM_DEFINE_FLOAT(MPC_Z_FF, 0.5f); + +/** + * Proportional gain for horizontal position error + * + * @min 0.0 + * @group Multicopter Position Control + */ PARAM_DEFINE_FLOAT(MPC_XY_P, 1.0f); + +/** + * Proportional gain for horizontal velocity error + * + * @min 0.0 + * @group Multicopter Position Control + */ PARAM_DEFINE_FLOAT(MPC_XY_VEL_P, 0.1f); + +/** + * Integral gain for horizontal velocity error + * + * Non-zero value allows to resist wind. + * + * @min 0.0 + * @group Multicopter Position Control + */ PARAM_DEFINE_FLOAT(MPC_XY_VEL_I, 0.02f); + +/** + * Differential gain for horizontal velocity error. Small values help reduce fast oscillations. If value is too big oscillations will appear again. + * + * @min 0.0 + * @group Multicopter Position Control + */ 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). + * + * @min 0.0 + * @group Multicopter Position Control + */ 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. + * + * @min 0.0 + * @max 1.0 + * @group Multicopter Position Control + */ PARAM_DEFINE_FLOAT(MPC_XY_FF, 0.5f); + +/** + * Maximum tilt + * + * Limits maximum tilt in AUTO and EASY modes. + * + * @min 0.0 + * @max 1.57 + * @group Multicopter Position Control + */ PARAM_DEFINE_FLOAT(MPC_TILT_MAX, 1.0f); + +/** + * Landing descend rate + * + * @min 0.0 + * @group Multicopter Position Control + */ PARAM_DEFINE_FLOAT(MPC_LAND_SPEED, 1.0f); + +/** + * Maximum landing tilt + * + * Limits maximum tilt on landing. + * + * @min 0.0 + * @max 1.57 + * @group Multicopter Position Control + */ PARAM_DEFINE_FLOAT(MPC_LAND_TILT, 0.3f); From 2c589ea374da5fc3b368d9195a80bc20365c86d5 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Sun, 16 Feb 2014 21:11:27 +0100 Subject: [PATCH 08/23] navigator: parameters descriptions cleanup --- src/modules/navigator/navigator_params.c | 20 ++++++++++++-------- 1 file changed, 12 insertions(+), 8 deletions(-) diff --git a/src/modules/navigator/navigator_params.c b/src/modules/navigator/navigator_params.c index ec7a4e2296a..87be737c425 100644 --- a/src/modules/navigator/navigator_params.c +++ b/src/modules/navigator/navigator_params.c @@ -53,7 +53,7 @@ */ /** - * Minimum altitude + * Minimum altitude (fixed wing only) * * @group Navigation */ @@ -67,32 +67,36 @@ PARAM_DEFINE_FLOAT(NAV_MIN_ALT, 50.0f); PARAM_DEFINE_FLOAT(NAV_ACCEPT_RAD, 10.0f); /** - * Loiter radius. + * Loiter radius (fixed wing only) * * @group Navigation */ PARAM_DEFINE_FLOAT(NAV_LOITER_RAD, 50.0f); /** + * Enable onboard mission. + * * @group Navigation */ PARAM_DEFINE_INT32(NAV_ONB_MIS_EN, 0); /** - * Default take-off altitude. + * Take-off altitude. + * + * Even if first waypoint has altitude less then NAV_TAKEOFF_ALT, system will climb to NAV_TAKEOFF_ALT on takeoff, then go to waypoint. * * @group Navigation */ -PARAM_DEFINE_FLOAT(NAV_TAKEOFF_ALT, 10.0f); // default TAKEOFF altitude +PARAM_DEFINE_FLOAT(NAV_TAKEOFF_ALT, 10.0f); /** * Landing altitude. * - * Slowly descend from this altitude when landing. + * Stay at this altitude after RTL descending. Slowly descend from this altitude if autolanding allowed. * * @group Navigation */ -PARAM_DEFINE_FLOAT(NAV_LAND_ALT, 5.0f); // slow descend from this altitude when landing +PARAM_DEFINE_FLOAT(NAV_LAND_ALT, 5.0f); /** * Return-to-land altitude. @@ -101,12 +105,12 @@ PARAM_DEFINE_FLOAT(NAV_LAND_ALT, 5.0f); // slow descend from this altitude when * * @group Navigation */ -PARAM_DEFINE_FLOAT(NAV_RTL_ALT, 30.0f); // min altitude for going home in RTL mode +PARAM_DEFINE_FLOAT(NAV_RTL_ALT, 30.0f); /** * Return-to-land delay. * - * Delay after descend before landing. + * Delay after descend before landing in RTL mode. * If set to -1 the system will not land but loiter at NAV_LAND_ALT. * * @group Navigation From 9665d38940dec371be3dd1c5e711a670162e2a30 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Mon, 17 Feb 2014 12:51:13 +0400 Subject: [PATCH 09/23] navigator: parameters descriptions cleanup --- src/modules/navigator/navigator_params.c | 37 +++++++++++++++++------- 1 file changed, 26 insertions(+), 11 deletions(-) diff --git a/src/modules/navigator/navigator_params.c b/src/modules/navigator/navigator_params.c index 87be737c425..9ef359c6dc8 100644 --- a/src/modules/navigator/navigator_params.c +++ b/src/modules/navigator/navigator_params.c @@ -55,70 +55,85 @@ /** * Minimum altitude (fixed wing only) * + * Minimum altitude above home for LOITER. + * + * @unit meters * @group Navigation */ PARAM_DEFINE_FLOAT(NAV_MIN_ALT, 50.0f); /** - * Waypoint acceptance radius. + * Waypoint acceptance radius * + * Default value of acceptance radius (if not specified in mission item). + * + * @unit meters + * @min 0.0 * @group Navigation */ PARAM_DEFINE_FLOAT(NAV_ACCEPT_RAD, 10.0f); /** - * Loiter radius (fixed wing only) + * Loiter radius (fixed wing only) * + * Default value of loiter radius (if not specified in mission item). + * + * @unit meters + * @min 0.0 * @group Navigation */ PARAM_DEFINE_FLOAT(NAV_LOITER_RAD, 50.0f); /** - * Enable onboard mission. + * Enable onboard mission * * @group Navigation */ PARAM_DEFINE_INT32(NAV_ONB_MIS_EN, 0); /** - * Take-off altitude. + * Take-off altitude * - * Even if first waypoint has altitude less then NAV_TAKEOFF_ALT, system will climb to NAV_TAKEOFF_ALT on takeoff, then go to waypoint. + * Even if first waypoint has altitude less then NAV_TAKEOFF_ALT above home position, system will climb to NAV_TAKEOFF_ALT on takeoff, then go to waypoint. * + * @unit meters * @group Navigation */ PARAM_DEFINE_FLOAT(NAV_TAKEOFF_ALT, 10.0f); /** - * Landing altitude. + * Landing altitude * - * Stay at this altitude after RTL descending. Slowly descend from this altitude if autolanding allowed. + * Stay at this altitude above home position after RTL descending. Land (i.e. slowly descend) from this altitude if autolanding allowed. * + * @unit meters * @group Navigation */ PARAM_DEFINE_FLOAT(NAV_LAND_ALT, 5.0f); /** - * Return-to-land altitude. + * Return-To-Launch altitude * - * Minimum altitude for going home in RTL mode. + * Minimum altitude above home position for going home in RTL mode. * + * @unit meters * @group Navigation */ PARAM_DEFINE_FLOAT(NAV_RTL_ALT, 30.0f); /** - * Return-to-land delay. + * Return-To-Launch delay * * Delay after descend before landing in RTL mode. * If set to -1 the system will not land but loiter at NAV_LAND_ALT. * + * @unit seconds * @group Navigation */ PARAM_DEFINE_FLOAT(NAV_RTL_LAND_T, -1.0f); /** - * Enable parachute deployment. + * Enable parachute deployment * * @group Navigation */ From 7d80f05b4d857b933cf7aca106ac55a7111e3b35 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Mon, 17 Feb 2014 12:53:06 +0400 Subject: [PATCH 10/23] mc_att_control: more strict conditions for integrating --- src/modules/mc_att_control/mc_att_control_main.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) 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 db5e2e9bb06..8de0f0b39f1 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -86,7 +86,7 @@ extern "C" __EXPORT int mc_att_control_main(int argc, char *argv[]); #define MIN_TAKEOFF_THROTTLE 0.3f #define YAW_DEADZONE 0.05f -#define RATES_I_LIMIT 0.5f +#define RATES_I_LIMIT 0.3f class MulticopterAttitudeControl { @@ -658,7 +658,7 @@ MulticopterAttitudeControl::control_attitude_rates(float dt) _rates_prev = rates; /* update integral only if not saturated on low limit */ - if (_thrust_sp > 0.1f) { + if (_thrust_sp > 0.2f) { for (int i = 0; i < 3; i++) { if (fabsf(_att_control(i)) < _thrust_sp) { float rate_i = _rates_int(i) + _params.rate_i(i) * rates_err(i) * dt; From 838290fa63cf5592e5c9d7590d0f359f5f81aac3 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Mon, 17 Feb 2014 17:12:25 +0400 Subject: [PATCH 11/23] mc_att_control: pref counter name fixed --- src/modules/mc_att_control/mc_att_control_main.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 8de0f0b39f1..38fde0cace6 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -262,7 +262,7 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() : _actuators_0_pub(-1), /* performance counters */ - _loop_perf(perf_alloc(PC_ELAPSED, "fw att control")) + _loop_perf(perf_alloc(PC_ELAPSED, "mc att control")) { memset(&_v_att, 0, sizeof(_v_att)); From 71d4c7fed8bc809fc38ba54147156fc834d38846 Mon Sep 17 00:00:00 2001 From: Julian Oes Date: Wed, 5 Feb 2014 19:03:26 +0100 Subject: [PATCH 12/23] Startup: Hex vs Hexa --- ROMFS/px4fmu_common/init.d/rcS | 2 +- ROMFS/px4fmu_common/mixers/{FMU_hex_+.mix => FMU_hexa_+.mix} | 0 ROMFS/px4fmu_common/mixers/{FMU_hex_x.mix => FMU_hexa_x.mix} | 0 3 files changed, 1 insertion(+), 1 deletion(-) rename ROMFS/px4fmu_common/mixers/{FMU_hex_+.mix => FMU_hexa_+.mix} (100%) rename ROMFS/px4fmu_common/mixers/{FMU_hex_x.mix => FMU_hexa_x.mix} (100%) diff --git a/ROMFS/px4fmu_common/init.d/rcS b/ROMFS/px4fmu_common/init.d/rcS index 50ac9759a5c..d127853683a 100644 --- a/ROMFS/px4fmu_common/init.d/rcS +++ b/ROMFS/px4fmu_common/init.d/rcS @@ -465,7 +465,7 @@ then set MAV_TYPE 2 # Use mixer to detect vehicle type - if [ $MIXER == FMU_hex_x -o $MIXER == FMU_hex_+ ] + if [ $MIXER == FMU_hexa_x -o $MIXER == FMU_hexa_+ ] then set MAV_TYPE 13 fi diff --git a/ROMFS/px4fmu_common/mixers/FMU_hex_+.mix b/ROMFS/px4fmu_common/mixers/FMU_hexa_+.mix similarity index 100% rename from ROMFS/px4fmu_common/mixers/FMU_hex_+.mix rename to ROMFS/px4fmu_common/mixers/FMU_hexa_+.mix diff --git a/ROMFS/px4fmu_common/mixers/FMU_hex_x.mix b/ROMFS/px4fmu_common/mixers/FMU_hexa_x.mix similarity index 100% rename from ROMFS/px4fmu_common/mixers/FMU_hex_x.mix rename to ROMFS/px4fmu_common/mixers/FMU_hexa_x.mix From e407766cc7c9f2c2d06deb064b9a1c89cb17a203 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Mon, 17 Feb 2014 23:56:31 +0400 Subject: [PATCH 13/23] mc_att_control: minor cleanup and refactoring, move some class attributes to local variables --- .../mc_att_control/mc_att_control_main.cpp | 43 +++++++++---------- 1 file changed, 20 insertions(+), 23 deletions(-) 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 38fde0cace6..76ee2c311c7 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -71,7 +71,6 @@ #include #include #include -#include #include #include #include @@ -84,8 +83,8 @@ */ extern "C" __EXPORT int mc_att_control_main(int argc, char *argv[]); -#define MIN_TAKEOFF_THROTTLE 0.3f #define YAW_DEADZONE 0.05f +#define MIN_TAKEOFF_THRUST 0.2f #define RATES_I_LIMIT 0.3f class MulticopterAttitudeControl @@ -135,15 +134,13 @@ private: perf_counter_t _loop_perf; /**< loop performance counter */ - math::Matrix<3, 3> _R_sp; /**< attitude setpoint rotation matrix */ - math::Matrix<3, 3> _R; /**< rotation matrix for current state */ math::Vector<3> _rates_prev; /**< angular rates on previous step */ math::Vector<3> _rates_sp; /**< angular rates setpoint */ math::Vector<3> _rates_int; /**< angular rates integral error */ float _thrust_sp; /**< thrust setpoint */ math::Vector<3> _att_control; /**< attitude control vector */ - math::Matrix<3, 3> I; /**< identity matrix */ + math::Matrix<3, 3> _I; /**< identity matrix */ bool _reset_yaw_sp; /**< reset yaw setpoint flag */ @@ -262,7 +259,7 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() : _actuators_0_pub(-1), /* performance counters */ - _loop_perf(perf_alloc(PC_ELAPSED, "mc att control")) + _loop_perf(perf_alloc(PC_ELAPSED, "mc_att_control")) { memset(&_v_att, 0, sizeof(_v_att)); @@ -276,15 +273,13 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() : _params.rate_i.zero(); _params.rate_d.zero(); - _R_sp.identity(); - _R.identity(); _rates_prev.zero(); _rates_sp.zero(); _rates_int.zero(); _thrust_sp = 0.0f; _att_control.zero(); - I.identity(); + _I.identity(); _params_handles.roll_p = param_find("MC_ROLL_P"); _params_handles.roll_rate_p = param_find("MC_ROLLRATE_P"); @@ -535,16 +530,17 @@ MulticopterAttitudeControl::control_attitude(float dt) _thrust_sp = _v_att_sp.thrust; /* construct attitude setpoint rotation matrix */ + math::Matrix<3, 3> R_sp; if (_v_att_sp.R_valid) { /* rotation matrix in _att_sp is valid, use it */ - _R_sp.set(&_v_att_sp.R_body[0][0]); + R_sp.set(&_v_att_sp.R_body[0][0]); } else { /* rotation matrix in _att_sp is not valid, use euler angles instead */ - _R_sp.from_euler(_v_att_sp.roll_body, _v_att_sp.pitch_body, _v_att_sp.yaw_body); + R_sp.from_euler(_v_att_sp.roll_body, _v_att_sp.pitch_body, _v_att_sp.yaw_body); /* copy rotation matrix back to setpoint struct */ - memcpy(&_v_att_sp.R_body[0][0], &_R_sp.data[0][0], sizeof(_v_att_sp.R_body)); + memcpy(&_v_att_sp.R_body[0][0], &R_sp.data[0][0], sizeof(_v_att_sp.R_body)); _v_att_sp.R_valid = true; } @@ -561,23 +557,24 @@ MulticopterAttitudeControl::control_attitude(float dt) } /* rotation matrix for current state */ - _R.set(_v_att.R); + math::Matrix<3, 3> R; + R.set(_v_att.R); /* all input data is ready, run controller itself */ /* try to move thrust vector shortest way, because yaw response is slower than roll/pitch */ - math::Vector<3> R_z(_R(0, 2), _R(1, 2), _R(2, 2)); - math::Vector<3> R_sp_z(_R_sp(0, 2), _R_sp(1, 2), _R_sp(2, 2)); + math::Vector<3> R_z(R(0, 2), R(1, 2), R(2, 2)); + math::Vector<3> R_sp_z(R_sp(0, 2), R_sp(1, 2), R_sp(2, 2)); /* axis and sin(angle) of desired rotation */ - math::Vector<3> e_R = _R.transposed() * (R_z % R_sp_z); + math::Vector<3> e_R = R.transposed() * (R_z % R_sp_z); /* calculate angle error */ float e_R_z_sin = e_R.length(); float e_R_z_cos = R_z * R_sp_z; /* calculate weight for yaw control */ - float yaw_w = _R_sp(2, 2) * _R_sp(2, 2); + float yaw_w = R_sp(2, 2) * R_sp(2, 2); /* calculate rotation matrix after roll/pitch only rotation */ math::Matrix<3, 3> R_rp; @@ -600,15 +597,15 @@ MulticopterAttitudeControl::control_attitude(float dt) e_R_cp(2, 1) = e_R_z_axis(0); /* rotation matrix for roll/pitch only rotation */ - R_rp = _R * (I + e_R_cp * e_R_z_sin + e_R_cp * e_R_cp * (1.0f - e_R_z_cos)); + R_rp = R * (_I + e_R_cp * e_R_z_sin + e_R_cp * e_R_cp * (1.0f - e_R_z_cos)); } else { /* zero roll/pitch rotation */ - R_rp = _R; + R_rp = R; } - /* R_rp and _R_sp has the same Z axis, calculate yaw error */ - math::Vector<3> R_sp_x(_R_sp(0, 0), _R_sp(1, 0), _R_sp(2, 0)); + /* R_rp and R_sp has the same Z axis, calculate yaw error */ + math::Vector<3> R_sp_x(R_sp(0, 0), R_sp(1, 0), R_sp(2, 0)); math::Vector<3> R_rp_x(R_rp(0, 0), R_rp(1, 0), R_rp(2, 0)); e_R(2) = atan2f((R_rp_x % R_sp_x) * R_sp_z, R_rp_x * R_sp_x) * yaw_w; @@ -616,7 +613,7 @@ MulticopterAttitudeControl::control_attitude(float dt) /* for large thrust vector rotations use another rotation method: * calculate angle and axis for R -> R_sp rotation directly */ math::Quaternion q; - q.from_dcm(_R.transposed() * _R_sp); + q.from_dcm(R.transposed() * R_sp); math::Vector<3> e_R_d = q.imag(); e_R_d.normalize(); e_R_d *= 2.0f * atan2f(e_R_d.length(), q(0)); @@ -658,7 +655,7 @@ MulticopterAttitudeControl::control_attitude_rates(float dt) _rates_prev = rates; /* update integral only if not saturated on low limit */ - if (_thrust_sp > 0.2f) { + if (_thrust_sp > MIN_TAKEOFF_THRUST) { for (int i = 0; i < 3; i++) { if (fabsf(_att_control(i)) < _thrust_sp) { float rate_i = _rates_int(i) + _params.rate_i(i) * rates_err(i) * dt; From 55f5f1815f6a8f2efb969cea2eb061bdc2d9b92a Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Mon, 17 Feb 2014 23:57:23 +0400 Subject: [PATCH 14/23] mc_att_control: remove rate limiting to run at 250Hz --- src/modules/mc_att_control/mc_att_control_main.cpp | 3 --- 1 file changed, 3 deletions(-) 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 76ee2c311c7..e92b7f37580 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -692,9 +692,6 @@ MulticopterAttitudeControl::task_main() _manual_control_sp_sub = orb_subscribe(ORB_ID(manual_control_setpoint)); _armed_sub = orb_subscribe(ORB_ID(actuator_armed)); - /* rate limit attitude updates to 200Hz, failsafe against spam, normally runs at the same rate as attitude estimator */ - orb_set_interval(_v_att_sub, 5); - /* initialize parameters cache */ parameters_update(); From 2b4af6635708be2248abf53edb65a9223fedcd6a Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Mon, 17 Feb 2014 23:58:13 +0400 Subject: [PATCH 15/23] mc_att_control: poll vehicle_rates_setpoint if attitude controller disabled --- src/modules/mc_att_control/mc_att_control_main.cpp | 10 ++++++---- 1 file changed, 6 insertions(+), 4 deletions(-) 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 e92b7f37580..b5df83d8596 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -761,10 +761,12 @@ MulticopterAttitudeControl::task_main() } } else { - /* attitude controller disabled */ - // TODO poll 'attitude_rates_setpoint' topic - _rates_sp.zero(); - _thrust_sp = 0.0f; + /* attitude controller disabled, poll rates setpoint topic */ + vehicle_rates_setpoint_poll(); + _rates_sp(0) = _v_rates_sp.roll; + _rates_sp(1) = _v_rates_sp.pitch; + _rates_sp(2) = _v_rates_sp.yaw; + _thrust_sp = _v_rates_sp.thrust; } if (_v_control_mode.flag_control_rates_enabled) { From 969f564a132e7b14e94028798ca5a4d6c5f13244 Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Mon, 17 Feb 2014 23:58:58 +0400 Subject: [PATCH 16/23] mc_att_control: code style fixed --- src/modules/mc_att_control/mc_att_control_main.cpp | 9 +++++---- 1 file changed, 5 insertions(+), 4 deletions(-) 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 b5df83d8596..01902ed0cfb 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -531,6 +531,7 @@ MulticopterAttitudeControl::control_attitude(float dt) /* construct attitude setpoint rotation matrix */ math::Matrix<3, 3> R_sp; + if (_v_att_sp.R_valid) { /* rotation matrix in _att_sp is valid, use it */ R_sp.set(&_v_att_sp.R_body[0][0]); @@ -762,10 +763,10 @@ MulticopterAttitudeControl::task_main() } else { /* attitude controller disabled, poll rates setpoint topic */ - vehicle_rates_setpoint_poll(); - _rates_sp(0) = _v_rates_sp.roll; - _rates_sp(1) = _v_rates_sp.pitch; - _rates_sp(2) = _v_rates_sp.yaw; + vehicle_rates_setpoint_poll(); + _rates_sp(0) = _v_rates_sp.roll; + _rates_sp(1) = _v_rates_sp.pitch; + _rates_sp(2) = _v_rates_sp.yaw; _thrust_sp = _v_rates_sp.thrust; } From 3eca9f9b6b866e1c7ad8b57d16ad16cb073d6085 Mon Sep 17 00:00:00 2001 From: Stefan Rado Date: Mon, 17 Feb 2014 21:11:03 +0100 Subject: [PATCH 17/23] Updated logging/conv.zip with latest script versions from Tools directory. --- ROMFS/px4fmu_common/logging/conv.zip | Bin 10087 -> 9922 bytes 1 file changed, 0 insertions(+), 0 deletions(-) diff --git a/ROMFS/px4fmu_common/logging/conv.zip b/ROMFS/px4fmu_common/logging/conv.zip index 7cb837e5666f51eca7ed803dfef4581f01bec1f3..7f485184c6d453a422f795489fe64684e9b179e3 100644 GIT binary patch delta 9806 zcmZ{KQ*fYN*K9JeZQHgrv6G22v27<$Y;)pF>||owp4hf+ocFE2>U`f{bHQiclD7%pS+-wEEqUC$TtvZ5He2V`qL1Zit5OXsS5Eu{;5F2}QV|zO{M%xPk zYx^~+#4V^_Uo-VkAx5I3Udsxkl1W$yWPp!UJ{nIL*wim6^!E0)@_&j@ULis*HC2)E zzGu;Xbosi{RTzGQq?r=N*ID9Qnr21g83egz#{Qy{%T>bir4gvDdmBN9pySc<;~5i$ z)64MA${@xcd~A+6FoMf7l$OQ|NYLn6g7=cOxUC7Thb4Uj-H49$IU>njpXZ%J;MRv5 z$6fMcTpDhq9-kG7fUH&*>h0WK5Hv;MjM0uUKqj{?AyJ~OsmyzzgROJez>+6QA7HTo z6S{7)Tk|-1rEFhkvZ7$Ul&AdY4ZHE%Q5m5`3vVQ(SC$~$m?8Tt0L*a%&eZG^wo+{q zi%tSewht&CZO|Z0O4KWikq*d{xDhD<#dbH*XMIc3-o7R8~*C;PklB#(@5c#nRIh7BcK6)^!%MqkXoIkTLe8Bbrj`HdF!pmQdHTP z1@ijHpJc~1om`E4N)thu`yDi_@w`^zN`J$DD_ttU@mJr-sVfuGja>`C%7|V7df1V_ z6Aijn?}@(Da(djE{dsidvshA;vTW1-xhFTb&^T`u6ylH#aR%CL&;RjhcJYb{`z3#@ z35v(ghV{2L9P7fX#t9w_wAj?6T2p|RSZ~Z%77%LE$;+~6c>hr7r8I~HV9!+5aWbiL zE{Xd~v0)v}Q1M6W4<_jQ!GlYI@007-i=5+szRAC0b!M`RZGf+;sraTFok)|vatUkV zF^#!4#-mPbAS->Fh-YKwMm@(y4gc-mC_hji-}XBUx|w1OV3xbGGf57CIFI3cO#G^I{?Q~7^1(Y4NNvr^%TiRa6n9MNW495(D)2)DjG(coeYbKc1v`S4}ngD4Te30!ML2rYXB)8 zw-ckHWxN$WE$R{cDFSUe`;P*{K()?QZQvc`!TB}n+C8nzH0+E}Gp7eV`67)ST*50zm6e|iEBn)*nJOk6I25txKgmw4uMlyc zB|;prys8xHsbJy3Ltq*i-Yk_21p~FR7o7ZrxzN`|ltep>&2gh7P;u+A3xtHuRo-Aj z0jUIIYznH?3HE%vCW_>E+7ZCINA!WcJgB@>W;@Dh|3=j*V>KBhQMFmAB6YikEGz}q zg0#~QS+Ds`>26C5fU}#fd|59X%AQUQ=r4Jy_?o`Vow}q}8uemWo~XR|{MEl5$w?ejQ@4w=WX zLO1Pg&82tLMXUgKwT458RSlj4i74jzaC<~c=vH=mQ)3_SH{)2+H?P%nm8~O2A=^R( z_zVq|n(-GZ4+W@jcW#GZ0?;kco|a2EsABPwC!xlX#!%p6sGo~UY0p9cL3l`jm081d zw?<%@{K(W4L4-QoUkL0Km#f~PvAUXQG*{*`SITs%8|;z8SRc`k2DeC)Y&M$35|>Oj zYDPU`ECGNNQh~-Y=z_8S8&VB|;Iaxm&i0_SuH#?W03(FM3t8`3R}lJcuA^}-@hqDr z{>?d)7R89DSyfwf>W?l`me;`cw2e*8__MMHG$VKu5m747dzx##Q`Skd%)=d0taWS8 zv$#0Cw{)zvO7}Bt|I{tJ_C1>fD?LwoNY2!RnkBGEJ1K?Jamvj6ff?8LBZTQZ5(t_z zrgyE;ttnfQ&Z(1DQZ7Xw|4rdx^*NiRma?x&i`-;yWJ;cEe(MsIvDD;VH4>Jy)M*{(HWIB1*2J9l%nTkHhFX+@J%#;YfCE zv$?QnFS5A$5UP}rZm+`@KjT(_xxs3_{kM55d`ot*Xk(;&!6~MGek}|cK4rOaea3>z z0NL7N1Yy$cjsoiK^qKct7ipbe5l<5 z%JxI0&+{GJCzzg#YkOfm04uFMv7rqk#~DN2M6R()ye-iZ<2y@CW#qcLg(IJ?9=5am zO^~FFj)5Tf%h`mQ_A-{R8-CAbM?8=kseNm3s^5&Bo(#^%aZqrkwPj$paoLk|S!uyE z$axe%Ub${}E}G^V6veC;oLsCFMHXKY?6Ay2v0%l~LElwKEHz8%;_YI45M(-*2x^?6sj%X>aVphRxBZ`1afoN&J2iP2RVVl zoi40`DoDgW54uj6Tcx@?;_y+lt|qtQYIzRn!LmxqaVqs|mKS60xNmRw^-tI=HU8Ay zc`FiM`101uQ9;RZkhg^DZWrTQl)f+^9fH{`rUL22rHlBXvy$6&PP|?Ann-#9lMy6+ zSNK@VCmIwiAw3R5Cce}sLk^I8Ky8FjaEm*zXdNbf#nS^wH;-_v(>3E)$H_;- zswqzED!n9^Kr>~QaMOq%#zAzM+lNivwx^_QY-bu&^5Q1w{z*EmeN}x;$mAzqM68l- z_4Xs`@i}#%n+7H-(eL?%RQZ{|PR%E(3B~z6e}KE zl8qiT+MX8fuMsh6j8VAIz3Y1$RJ8lH4lHAY$)R$Sb};FHkcFS1v{fUjGO%if&2rHu zrmWyuwgxHT;}sa}$3`XXwaC#NNXl{;L7F5V&VU2&yWAeK9w4;kW7 zKR+CslaWejJD%cRZd-=$0BFzkcKdOF^P4FEZ%4nO?L|gg98HfS);RcSReVA5`>$f55**inY?@=DYywqDwA4ghSgQtlz0>N8CwzHUzrnHg5J>SN+qfyQR zQG*pEjf07FE^KG#YA933`3j0yud?#juHr;2CMhl6CF%v<6R+8qUGH!S)w*-V^h?v% ztE_=*LTkPQ6R64y2cP48N`=^o3NieZCXXu}g$ zE4TM-6~~y0J1=VNoAqTcas2QxIS&(K37j!S2(?|UGg!dBD_Odoex&Q0jzTkaeFie1 zDvTO95Mm5@39I=1&E!=C$yW#ya!t2~b?k#B=mz?$i^kK03odDcC7RSkteI)B4TGvZ zLdF0a`HwfsMg`bcH&?>Oy7Z%=VVw}ammNympRKgOk4RO&a5or<=g{`h4cF6?`kt-M%}|V z@$d^&w?q=Q++lZaI1F-Zlkxg!YUtnUsTXh&Ixi($ZlE$*^CR^4f8F@LR4Z?h#OaET zIM*EgmBzq@w4{YL={3I3I^;c>?XNx}4i4I-sT=(Tu5h>II>}la2o7Q&lPX@>|>KSrS!l1aQ$Khb9mQ z7nmXm$}UKC^DOnw6kP3^`E~jDfKKcGbPXaDdZ@(Iq_No?2KK5RK|(YCg?FJ zK7QKufXbPhW5 zSedhtbso$nfy$ca+Zb{#-|De^7;ik5W>6ssWctQS8P+YT;rOK{u&a5l|DAbUU(N3w zqlWgqTxFUY`}Wn4GRj6bi3lGDXvE_eV9b_|6<-Tfdk&K3%*_Z7TV$mk_ev;dmM+TM z(cd9cWy|73w1$c)AWM8LuID$-sP^W-eSk!Z|F|r5=Ili`y(&p@;uNYHNOQCc@W54p zLCzG#@+Xz05WCGKd3Uzr&V6eANF5AXZCO7s;lG&p{kl1R7YR>iqZwSD zw^Q{{rMt@s7nJ|#gZCG^CW-V8w2YCLYoEdTbm>h(>1l?toa%UwD95B-qs8@Hhddcq zCH7r|Jh4YlOr&USjWEdo60gN6jp!!nJ?oG%wERqo*z3{d1SiArT)~o1lyU#*@H^Kb z<$F4r68*S)mla%*#7oNvJ7gz>D&HgS7QZ!^6U4Mh(xZ#G<2pNi7pmr%t zAgedVUfgG;Knz&;5w4xQ!p^)3of#Im1&41`5tCc>}7ebEqfB2^!1T)Iu#Aix5$#69bcl|xDI11-+C_5C(iJ4OSz zQ-A{{(vXRlcMsI$SG_#;$8SZgSkz(O0Zt8A)WhBZw_eQyhdtv7%gry88sDSX(M=aI zNhjW8NHD@xJ4Q^b?P+opF14L%lbw}V5PxxhmVse<`yC@>n?(oKVDjQnKikM`Tp6O70R|O+F6cqfA*X6NsNNt+EOA+0DrB9r?6IEd@ z2pDk!4c!H8pZBv*XtL+i%p!Hqi8Qrafs9#Te1dx-3J}wX3Dh0a({D2v7`>M7fp|8~ z6W-c6c8a;^&ItndQ+K`9#tEPtMGr46fz?2lHJ6Cp!~(C!x{Aecl0;SUDrTXyDY;6Y zyQVS-@!T1Q13XWU-jOJ$f4{jLnumq!1IG8p6&zCMuv?r68t)Pm#jG8gVeorQ;W-s| z+;l!ot3eK;ySuK~vtR7E18gD=$}xWTW^hTE2q%_ye$MdZ{)O|2#nQ{k!5%XM%IQEE z={Pd?A(*dzz%eKt2229Cb+F_Q^J4pk-dbE@2-x++cj;6VvHH{T6uI&4Ia})?g@n|D zz3N*&h-X>E3F3u=;N)f7z*$0~%t7%<9^oh=MOk_a#W7TiEzdm4s;$73(R0_`)*6rT z9j^H#bH9woL@K?P6Fq)fq&q6aaaEvq$Ns?Thndr(Q_YBtOo&}FRk?GI83%)S{to$~ zu3PMg_RG6SH+8*?HDm;;JE~MBLGR(2$nz7&U#FdIvhMtrIIj$kNpOOAkQ@&af00+A z+}X}!Bt~9f`;A_z+1c;YyX0*t*PT`92q*2<$>R-avIC5>pYYE9a>@OpcL8{Q_id2h z3{MlZ^VmO3V$7Y(1AETIBvtk! z8qaB_^pr$3g`Wwo3%>SN_9y;&r=pP!??ih*;b8F1gn*uLTq&?d2NQF@PCjMmDs=tB zEa3B0PFDL8<70GX)j8}gZU6{@X=6Rd#^RGq;FJC?yjzkon+{0E{R`&8LiAllz!1Yk z9ojpcecqcSBj{6JcgEemuPNdaBDV{B|v_^hjoTKjH<}WQZ!*%%<_{T z*;7l$JNA>uPsT$+S60|RaR|QO-dYZ&N9>LC?atI6DfF0r6#q;QmGJ>qVtWo3Ql>)@ zYf)q9LDVOEw)!U*W_lw!lN!bfN)%3{i#wnuR2QXX)xLk&rb_cO(`Zp+x}bjIyN4jG zzZ&pCrQE8%Pb&T$ueUp+71(Pnfw~`!6Zk!C-MSpql@i*?GmPYN9g}E}bU0y>!WEc9 z@Vlnr7LMB|%GO5x&Sywc3fzsKwGAN{Ihr6&RhF1L#yk~_7qNFypbO(Z)(HZAaqg;u zKZxXA;+0VC5!K{ULg?b6f}8XucC->rFHk}|b1+zp-=sfDn3NjOZO=|n9_5BzYyX*A z^pl8a6WIXGiv+Z24C#*zcfn3L zjmee$098Wo78vE%%=PI>X=F7?D9pnCK1)k7xgzt;NQGR7@QE5A; z1wLMPKovGi^`uhv+XU%2o_)H-`ET)4qg7i>6C3)RvdI0S4jZ%p)j~Wn+KreGoIhy` zP~7-2zTg3f4vAgU3}J2mh>z>M8{ee%ntM=WCu01y#EEhSy%jxv-gHpe@6S1JDw3ve zAMN?`5%2|l$oo3NeqEdb^P{>E()qpj1@#|$+VHogSX7XLAOH^n^3DnZg7}X;?QHUo zLCvaf;%e)_=-}C@^JTx*g!jMl5M%q>+ystBcaXjkjDLb5ChCPCRUTGKFm#WM_Sf#-#+I?a1q8GIrHH95!pb$C@CdU zh)f92+cuJ0s+p-t8>uI=YsRw!q8P)8XMaI`?{h*=LJxy8Lk~SOu#DwT^HWdR+jxj$ zqe#rzVave>um*1MvtwX5vx0(}KpREdWRf5dAt?*aD%TU`<9{^NO7FY;G^-)7;`Mtj zFQQ*bXu`;Tu(ni1E)4(TSq7EU0inD1N0i)yIYP73NcO}?C8TMB zu#dT3@(>q-p}gH=;dys?en%FpK`d#s@*LEhNKIR>$qAZ2>&WM@6d~I+7tx!Ut3cKD z;(;}Uc3+*MDcOF8hdLw&;?5z&v4_1cTSgLF@GmA2pVl+QYN_O5SGjg8K*GNqMlPik z3B#n7(DzTz5o{7&zKR@s#J}!^nq&MIdw!eJ*oZLP)0q0Llsp*GD zvOqUJ7Hz7IhqteWs+!HtZtm7cCPf8+ryDcB>#a+yyUp)w=N=bWx;hxg8vNTl*vtf5 za^d>XJ?{KBCGL8|P#$f4=}H?EXf$)Q%cjBUZI;|Dj&92l^%|9CvMt1*sGmhhUCuCXl)Pa?&Oa76vpE9yONX+Vj0w^dHm; z#G9gble(N~HyNY|xQ@^(69t_}fAzL#=4brjO)8iO2rj{YoF{TA?Ssg~Zy|*|ha6JQq^Kk3a|u2=;wbuaYYc_^{76k? zSmaS=a?vdP$qezsq|`yN(Uspp!@j?r`&W?+n$sBIM5+W&Aab?o{GwdQps8Q$eV2W1 zlf{)n-XsIepb+x5c&QD&bwAh-4_zvEm90%-lzcP;sMW`U_PkqTd}N5Oks6?$AyWc}ewhPEFY`6Kc2%*y>D=AhbW(OSAYuMl zLUt5xK&IV`6E8%~*jJ2dVT=B8Rjx8({Vh#A4|hIe3mW?e-XfvbRDWheA?BUUBYRN( zrU5*tM3KE=9NdZgL~9znZ^Q!?EH0-?Es7u!$P~KY`#1^_W7>d zh!_WN3iy$UPZ zTZL9c`VI^dlA$%Ndvf8LXaVG6U-?35=!}bBvtE@sYcGQOs6iLhKZz`4Za$>A37D^OEk1h&1%Hc8BFRO;r~*A$ZQgu(%D382g|w3u$FUdx8Q zR79MHTy!_m-%3@}k|=69Wm!YqIVcHe7i_*Pum74sTq=WxS{$p>kT(|@BzDaqi6zjg zq}O}dj&Lx_thK}dA}S%Jth>n!-qI%A}Zh%oQ)c8_+^ve zGJ+-$G{au+`G-~N&>iMhp-qojbzvh1AqyyA;#0~7M-NN!V=Y#g912It8ra7(JH2_k zvHk+}gTl9+|1}$5bNOU!+HY-{TV#smxq~k6Lv0QV?S|_V|)|CeSgYnriZNVDnJ0zKI)*xtnDL-+h|32p=63~ZGcyRfpy789 zH5f)U28%UvThHqB*&3ltYXIwqxv*V%9)ZoNlDWq^>WBxks(5Ih_3O8R8-nLUn@&uP zQ#Z?7^(?-T(7*e|7yL_8ZuijR;}Io`ay?JiBlJ$gKp_jBtEahi!G%nNLg@%s@>+wSDVxQxDHqtveEdS{U2ZQ#7Em)Gi8tq3#Akz> z>66Xz(I&gB#ezVn)%WXjw-;LmpJm}nX$WIH@Op0q-05MoeJM)Za9V&$6=5C8zxU;O zpQu}C^i z1MMeVe^jf_?|qM0YamB8tJp^DPKBUJ?g1i^Dgy#*m7wFYGHaq@z%?b=%qHQ6VxV>w z(Pm}JwCkcbGXWm?kgqV)VH2q|q#WcrUypplV#5SkQ$@$Lk3{e##O`Gjm(b>E0U|Xc zuawoNH^N@c`Yg19IBeGc$lEg%QL$iS`5?sVnC3aJj9~ru0q_ipRy;eP=r_k-2;T8q z?ih4}5mD1Vk?V{sp2zBAW4RL(wiw=eJgN;l^Az+I?x!3>dUuDvHQBRc$(9E7 z_2q}maOfsOY`MHgp;Lr(>&C4ivLtmn-WFKQ{e2aq@Kr$SMd=TcW?4ZlF>R!o_7Xub zZd->OL=c^FOP_2#vnpcd$D$1#h>n#uS#z?jrt{S_ORB5Q+~=- zH3(4qB|x@6KfzSYB6mgi7w(MV^SwLw*cnef0;A^j&3X%BkK}&eC|WrKw`te~^__br&( zHmhQVoOfrRJ>-sCsiR_tNuCsTE$aN(Nq*q0k+Mh?j?sO&M%NPcUy9FtF1hP-amV<4 zU67i+RVD>-_`h+>*z%1V&h6I6c4-f)A$?4 zA^^E!=MM|0^mAz)WR|}SHp3m|eZ*@MJg|Ic##ofthm5xCk%pnIWJzxcRvm_$4@Zj4 zjZdhzX!D5&p5a$5i!E`#xq6;3Bz=GD;lWP(c*SYDC|c`*Z?`77P1zfP&6#^$f3t|n z36|hlUariyi?++(Q?~_Is^I&jbL^~ADF|pOLcP0&Cm}f0iA+6!9C^jp<8RYL@f3hg zjU+&|_WVAVB%U-|jn{TbA4TDL9BwJpdr5W3r;MG{ali28ljZV@s%{lBceA(}q+#lx zP`FLiaqgZxpIQHAeFdcd%&4%vC~AtWTXp5w%gg5<@rHUFIx2d zh+&_>NG5x0g`a&njhAmm1$qFgd6q%rZa*s(L3uotE&UUQ@Xy}TI3UV-qfq`S-EHgT z6qD^zzdt-373lPQ!vx@$pP z9Ru^nUPD=1Rg&xS*EVhM&lo@HpTDB|t=_}}ZF^M7Mm z{|)=!0@>kz6!O2*|AXWDC)V}9_40p%|6j!X4|bRAziFx@3jz6W5A2_+`1gOxe_#Ix DCnVIZ literal 10087 zcmaL7V{j!v*Zq6qi8b-WwoYu@b~3Ruv27bCwrx&qn-kl{1b3eIR^58)|K5A6t7~_E z=vCEK>&x2vrzisf`5gcNd;_41Ac~;1hZwjZ007J^000u;2f)e1#@?L8z{JJYfx*E; zO%)aZK`{rbEVu%z?BWg&0E2u40|5T#eYduc{RTVwSLf&Vs=&Ob3ipT&^VL98!Sy1Y z`jRQoLtoBNXx?aYwOR?Vo0Bcn=lM9cNHVfu(FC6YGInbJ!=0DlI%Mz9e(ia~%Nx6q zfl3ScYmK(K4KOmwMkJ7^rm`bXNH9?@2Lod)Dp+lHynv?9F6TIVDKiV;rFkIKYz;3Bf5Ch-qX|N?qkU9?pV<-K9z`#ZbF6`CW_Fi+w2FcLJukytO^TKNyog*Lu3Qj0$T}|*x*y#1#v+PKorgM^R zci~g&oh8_tM7z^CS6E7wz_-$h)(;mciFG37LA`wkS-0%~oJ8JYPK(q+;$$+}g^O5* zQbUwNC6r(`ItwsY5&3YpaT?7{tX*5Z8Ucz)2=0?cNd6PRL*XexdQnJf+{VsQX0v{FM`>qGo((^C9 zoQa$tpBp0biCrt#z`waxLtQ%8>Cw9jl=iIRlszR~JIPo`;~ei~`+Uj!kU$U-O)J>5papx6c~e1D-*AboN=+cARG*U2`IK;r z`dBCl>^h2Giz5tO-#;q$%LM2uImOFTF z15Tl2KU2flP8UMuZ0IQMfdE;xIr1Vx0MiW7vI=BEJUgqlMso3ED2~d;fTGyTPYxg; z9-6ci8j)y=qRNm|_k;7W;z9R>sv+M;G|o!!ry4r$uT&H-P`xgZOV7b+xMlJ3UHAt| z@7~zlQ}`x{vTAg);NzH{a`jkJEw8Y^Aqow%jTzZ6eJX`%$d+i6O(s1yy~SbG7)l%1 zU(w688v4u)Egv_do{)S*&dm!u;3+hy7QZUW@i9_CtH83k%?;#8{3Zc&kDCRzBRx1g z8yx@Iln4M4SsiA=;>rui0I&E5fR+6#r$DL_^wrW~&v_!&RXQ>ur zZ*mMrdrSxh;upU$c#_rI5yyqBBu`RsFqv_g8ZWI-DqQ98XN2c)Lp5Zyq1#n*GB$jB zmnCIsWFz9e7G?WsWp=9L6kZ)^m%WNtwFKj4zfVAXsAKLHu*%Dy!S-}J@U*lcw$6+0mql^qO1PMlw9WCzcc5TmM zFB76W{8^t4>+G*F8kc||q0jo!2o_r^p+}&&p&9+Knt1+gLJrI_$tJMo;Kh;$c|?RB69 zO@YF&0_P~_y{kw{t#mtt+WFgEw^QxQpj_{+?oPHH=-8Z6V0iG~kRkI>x3a zL{Mx}cWib{tDX{_YD`_MAgqCDbflvmIW#frWum#j6n0?2MR4AQ_+ZNU$vAfE5S590 zV7-e{p)gTEWpB9}*6+N>G7du#Gqvc>nj3kwn1VtpOdRuIqr0yVf#rTP*8ZkytGX0Z z!=}Art+{j;bJfYDpo4pfYudZd&p6Y1z6v>f=Je*3TK7w1G=j=O9>IdXn5Tlz!GCOY z6{>`>ekR}{AXS^b8)^Lw;iAPx$5xKPXT7B35;++&v7#;PvA@0IXIC>qOMOuF;-ElF z`q#s{mduj0M~7Xd57Rb*bk2KtE7$_)(CFcp4W-#$(Z;nLolzwp=}0x54^g`gg4~DE zo$Am_ZlV;tsmc5%OAK&qGL>+fQpe6z|HoyS@gI?i1;fz=_$(%g40`W7GP*)C50(sJ z>%_S3*ZgA!_ye50zRwHdi!%rHdVy=B+*A*cs_{%~{3hmA&dPS?^mgV@cknhTBWp{s zm?@IJsV%zgv=(YyH8%y(+gU3Fg{?|y%AwM&P%Umg>r$bsnkEL18!kSF@0_om3wnE! zuZXmy-hR)zAZ(Q|1$I;6=pF|AYsS^f8evZTvnp3CTxpe)w@3t|Xozy5i>6@y0D9&T zkkB{gISID5nRHt+Sd|=zqZx9|%)pg=zAOy@fJlGa6`UJ-$|Do z>BxN%eHD8r#_!`l5x=y5&u^jsRD=%~0Qoc;IFg>w-^;?I)cz6pt$IS*6iVrHUfjIK zpLZ6=>RO#oBdy00(Q(&?5!*9cH(Bu&Wvz7CqQL#p)27bGjnJd)h(UpL52WN_&o~IC z$Zj}s>eD=;d_l9?lvkFWdd}%>k99_D&_5qK;3s~_nMWv+LQb@pk5GhtJ0LNk3Vn4!u|;lO6lX*&?%l6pECcO_?=dSHVj38fimtMTIZ) z9$?i!LT6UI?Iv4+TA?zyZ`oBd5Grj>?ut$diMKdSiH)xYN?t7qkK0{)p4cCT!IQwa zDYZ)Zt9eI&>Xl9S_N%ewPskVEnRnh^6wP zngawH0)ad-_pK|_B7q5TPskoUP;2zi3TesJFl5LoYGjg?ub`>H@A!@ch1T)oZI4&b z{^jXm&zspibReOBvNGtht7Cg>n8262R@V*R%ad`)!pZ0M*Hzam=kt5}3QRvP%HqH~ zyq)mV&U1we66s7OV>cSLBJ{OLM-g}qEmHG7{6>!uB>85==vmiePnZF)pPooL!(>P> zmdXS}Qf<~05u?8$l$%*tgB-J4f&7--_i#o1W@nTsra>FLvd?6*Hxh>e=*R;MgSt95 zf=C4(ZIFoJSt_Ag>r3<_w=KmbpGAI4IaG&Fj5XLm8N(Dw6$&3g! zQBwnSZU+s`_$D3qg1S}_uOS=iV|+g~r*J6}OX9>QQ7Sz3^>gOVM~DBC$d z9^G%3Io;Wp+`a|I?QcO6#5Vuo)2#o)g39c{^B$CE!BG|+R4sy6^5_#d`@Q*R}%M;35W8W^=(wGmS9CpEpjc`pauEG-<_5ah)ED?Bp!olU7x91Lh0{ik zpBf7QFgF7LVE=`ef6>L*-p-Z5_J49q@c(;mx#Y99-;hk)hDraLZG;I?79R6ll_v@7 zvohhDtD(n9%#u~N#s-Q>Z7LSUA`3WreVsb=e*UgJgIOz*jF3P0fy8MUhi!t$9;R?$b$?bb*I>mPS|;jD%ymor?2LkH+B$W$ zcpJ<%-l+YZJbFkNW{#yX=xDeoi|pK1E3vcOf(OYY?RJxEC*aF!-fZ9(@QeJ;QGqaa z@bzY3>hRTp1l1-8=NM4S#MJpJcZwslsv}SR^>U|!`w5j zZ{88}_vg=5Xy0Ait(M#QM>*GagGFhR%}m+1Amsgz{^EE!3KVNT&4Os&j(mxCiLYrO z_)6s#$#41%GMNR4vEB)pi*i&blTv+qL*zp;SZh#c^u804*)%WLxP*@Tc<>c z3yT|H@8&(k&-J5mXK+VB!z!#D-8YdVU9=j7oQY+0m5k{NkmKPZf-G_w)7Zy6OR8=* zHldXXV3KKbX+2c`iPVZJ2ypgj0iCDTT55BKoMudaBb-!^n+_d#QrI@_qDw`%3~m31 zZn?(}MteUvxukN5Z5pVjCsWO?`E5n3j^O4wrgVF|N^e_u_cts)tznSa5eY8lt+4e& z#RwCMp?2tTtrT7eG%ILmwr zeq?F~j8|;4JHeGYbtSA!q)26k*qUyJQV!A|0W)Fl;bSjwcG+k!zB9pXVO4sX`UaW$ z5>BQH7CN51#9Q90qZX8~E2`lR<$`5~HN8%@Eo!^d2-5I|9T)7*1$!jOMfz|T3u?iN zM;98@wirL5dMszG5x!kLfcN)6T{$nl%6jH{7BVKq)dgH*y;EDPOa6|d!~EPbwOZTl zfJvoff-?O}ju?>eXD1n9W>lNJ`Nm3iT6_Th zuAbv8VOP+99bJD$$l`0ykrTw^_Rz$78s7lniVhTfiB91|2kYhD`N{LGQ!d>c`72-3 ziSUn;&~LqP=GLQug^C^_Vgh%f6y0!wu6`1 z)G-Cym3Ipwr?=-9*XHM!`LnC5M8J5-O5oJw`c)hk!k;n@oX5V{gj=lHGx)pmm23ow zzd7`vtDQXK4HMHdeofnlFdt`mXhZp|H=>#MKM>?@F&I=s4wy`>=g+ z42`nS4p=M_lRn?B%y8?Mtqe;Y+8@=CzKbv*of5u*jg!R)%!gxpi*QK97m2BxYf)0& zKb*wz;I?}Ne_M_Qe+4zX8oml~1?Hnc`6Ww-Jod*S2leG5OsZwPSR=EKd1z^gpumyr z%=q^lP0{IATc1nZl_q{sDrets`+rQX#svvPN>i&x0F7lqeasD}=`O+2mZ5k$kN!V|1c^&YLFmZk9*~uZ~`^pd z4@rMp=IHIuQsKaNtu#-!c)IyUj7DAEi>)<>kk(jbA-U_c^UKKHP&Gr+iiRiDERd-aq zh+P|5&?=vd1lEO5l}SL%lD@Ye#7PX^oYQtw>jBc-Z=_4(sXCe%|a5Z%X@_c`t~jZpY!8Khe=AhRrA} ztx`6GI;tYfd6Ax|U5%-O4AliIh7rMSS_~nWQ>}ubO_!*RxYc6svR_&7iPOS*W)rT{ z{sTocP^EJj(@gPCp1K-?;j%PaEh@@)!S@RWS(M>C-QGa)YG41P1XAB$`1cv0Lp|ML zd8wKflNxdLocgrDNtC1?R@gGMwVW( zv#~D9Jn7-T0$WdE1#5?-NtV=~N*Q^+xmWZQNqPky8P4;T^IgQw66Gt+JN8S?HP@lR z!u}C*GlG>z)s;bj=%uSps-m2MY8 z8mS}lGXRe!RTk&Sqq-(Ci@8+K@|j%V*I9Gx$iq44a=E zZRXBV%M80qLPqTLfy=5ZQV}Gfdm#%dE8|~I#D^GWrsN{OQ$nG^m zZEE`6?L*6KK{A+aiIt{keG)%dAC#ZmfoiNC*~vMOb>QlZa?@hOLoTZaCI~e2X?Q|q z8$a|@mh`}Asz+6%;na*;Wur}OO}=q%^wOZFY0z5C4#~S25o22u6r|HY*9*TLf9^QH zWB}I>1`gA8kw$#zu8Rj=&3;UWX+!eNSn8sbLMZ`SYL zn=Pl`fs}ErPIr&4@;z(QW38QB9-F?IV`BrCS-dt$_q#AGInEUOK(APurk7-s+4fGN zHr4><)@Vcq;2CoXVg_Qv`^v^FJ^V`Hc;hv^`vKVJU#=7#-n-r(2)r($PRC7jQEmlj z$R{q?gCXC1LXXw^)Kk+ls#xyf%%`&rFw{xuoHE^CSVM=tTUd4C!(g8lP_$ zFJG2gNt(vm76ED}n);19GcJq}s5LC@kB44z&p_k3#$47Ju_|NKVrqfCL#79(Jac)003n)Ho5FmCLLucf?$~yaQULXV?J$|;#>DWYy{&u;Rb!6rrStyGTf}a7Q z^5Rk1NzOB|+}6?OAVt|z#Ad*TiPImk|F51y=(vY|R zx4ksxT6MpP8$#i?wb|6e^>RJM=;nTftxj2fcz4tdXJ2kTErLqm3H>V|ai z4}S}`&C5B;yKTD4SCF)uhmH16Y)e`o<|Bz*nn5y?IO07&+{>X16>u4J_Up_bw?(+GRfjY;Xj`uoKS-D<5` z-L_GmuLRUze5{M-j=IsV1hJ#LdnjrV6t(I$qdFvrRgbTf&da^gs}bmuG6D8ew+ajmg4rk>39hFvuwM4 zt|j}Ornf`Sha${!v_32_{G@XjJ^xmi*x^dv_H^9G*LG-4sZRz`E3(*MJNED|o*nX0zN$Qs-T%wj8mXE&byq zMD8g+;Np{?)ouGtqQWP^JD*~KsG<&7Uv}WMb>vxaWeK6XPJ=T}IUV+=f&06!wXjS; z4i9DT=9-&=zXHp3y1%$uc`yzjY)9n#GvoruL$-4k>(jX}3AMKs#&WvqU8-2Q%%-yJ z;1^yIqe8%gEnd-YCCPD$v1;B#OX+^BjPPn;g&hG2U@G>Be#B8N+4}C#$GmIAx8?JEk|jlYP2*=hR+NO+=l=0vr=~!=)MWas;{X{(2!D>!x-DX zh9-EiLLq7~jzi|l^s|V0GJ-BEf7f7lBvBu3)@VefffTE|hUsHc{~$s^4WZmkCb_nS zAB-0BXf##|C3y(6xY#l9U0&}R^XE+Y8qX7j%V>PPC8LVz7PP#AlD1}4M0y54v|v<> z{Z}6CBBuP)i0kbes+#taHsP4ZbRm~_s*$X31U75CX*VjcjJO3O2eWn=3&y%H*5W)zUjYIcQx^7 z1YYh-y(2JsRXgW{21biscnn9&A|0WTBLq#?WiIp{OKbuXXp>ZS|?(zog175NCKj6hNK{q;Q5dP9?Fpevn3hD+iR`2!Klnfuz*MP{iSAXQ5-4S7^yCK^u?fO` zZzF$Aq9YA8TS}wNNcAh-SdPvnp`RGhJ9L9n!Pkwq?rDLGcfOrMa z`|{D(#~T|sHb8FhUH0SV!KqU;u0$O7?9qp9$6MFVG+gT54blp1MM)eQeGVFV&~Uz2 z>rS1?PU~M`WOS)o)bJ{0L$GViE0f{M|C}0{DYUKj{>(E-)+%NT5{=-26*ZQqd-X*5 z^pabY<$M{frMfE2q0na)f+6##B)~C@|C}jxW_$~ZnkIbVho8U9%KiFt`uTXAJvrk( zT-^2J&!-66wf|Jz$b07BM?F6WHhw=s+a!*?;yA9jP4D?bCtPQMpOf+QfcY&z32ye7 z;=*Ql9@z6{>w4dE?6IyJ)-A)i6vG~I_gC|&i*1Z2k;>SgBAGmRI|>}~^;kekOZXzY zEKYTPK8;b(KX!sZuMiu%BdAo>{+7HtD&%yxQv*L>%KiY|OXHlchZg)V&TL1oo3qKc z-C<%hX>Pw5GK|MTu6SiuPk)A^Ajt~Ks=!G&BgUjqPexIv+20ZHZ&z}w>~p_&7O)EO z1caB#HdXkPuX$ygOFYt;Fp_<$PZHL{eXgC_M-C51+D6VDnV-v&l)`;%PS96J*c62M z<9srzb=K0{yA?BBO>t>%O?kjyD`|5P@eRpwwE$J1fZt)6mH9#FUX+$85(?S#NwRlH z@y6MUs$z@VW*LJJNr@ppg(!Dterc@r(kUhs_6rriCudD#%^QO{JEUjauTH^y8eICb z6;AH88J_a&i;e83Zz1r#3lBDexLCCgWAe-;PH3y&!I2D*pu8E!?r=y^S8qp8*inR; zTzTc`19pHpT1Y3HkUGyV>a#rK7>b$-ziV$1LT~U+y;Rp^;@j~)np$i3!Vh>T#)eBF zW185bV8h%cr>&{hP~JKeCNeq?pp4`*2wz42K1X(F6n>GXQGM8jUVbVz{KXhvk{ir+@oGBgyzl+>}9XlZEtoo3hR6PsES$x z2jT_EH3IBNa6{OY$9A)v?=KPX)>WCCa$7gScMIL5ooD-ldDd$>j)yg0e;%;G6tV0| zcMcg(HR?fll5ETzda7@uI{j&swj^Ax$|eMZ>8i@ECJh_{jVdgvl|w-qv1moI7>JhF zX*Ku)ZOserygHlC%Y<1CtFR1jTYp}TGs4T+yZ}Rrw-SDB6u4u|BaV50EEe9i{?H;U ziHj|Z`?^-#@!wg)W~w&Ik5-$X87L^nuDZ>T=>om$`3ogC*nH7L5Ubj2DT(X`wA}-2 zn2K2(g&`f4GCyykS(XU>&a$2r1YDs@kGpdz6?pZ6DDM&@@ZJX0;Sqpd0ms=_TyHd1 zRb91(^(}JdsB{ylxj&?HH)V=L6Z!H)=6Uz0#WMe_Sf8@5Pbt?@RQ4H_&4#TvOu3RM)JP-W(sg zDW4d=z4ipO+`p zY00=CwTB`Ti@zpM^yNz_AK5{LwLR^|qgDL|KeD}D$J1r#9J0rkIJsD_Q6}`JVzrVpx?F1eg)3m zA&*Bp69#oz7;{xpIdZ}Kyk~N#WiM`-SC34mxkcqWvBArxV3%!-5Y>Kgie# zdN920HKh$PHt4-z!5ObncNxV_o$?)|?AQ?5HJ{3DYba@|K(SA!?MU}Y$@xlUXI2iF zec=!9S^B*7UG~eL9fd+YCGR?brr$){5FbzW1Tu5~@-d`vYbZrUTQ%Ugd z$BF-tn8O~yCZe4kSDgNmN73>$V8n_Fpl5z!5m|phF_BSLBE;i5D996k)xXYTHNh^T z`~oP-fPoW2{D1Lf*#EdQ0N|gC07U`+IsKos8PxwxoBbc8*?(*P@3GH+Yj*!5+5T$? x^iS?TL!kd3x&Q6#|CXaB`{(!n^7%i#|M>iW=|MsN=Q9xh*8RV#Pw}7I{{>mxP2T_j From d9031844da2ed4c085db57d9793ef1b5b1913c3e Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Tue, 18 Feb 2014 00:13:55 +0400 Subject: [PATCH 18/23] Unused includes removed --- src/modules/mc_att_control/mc_att_control_main.cpp | 2 -- src/modules/mc_pos_control/mc_pos_control_main.cpp | 2 -- .../position_estimator_inav/position_estimator_inav_main.c | 3 --- 3 files changed, 7 deletions(-) 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 01902ed0cfb..24226880ed3 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -53,11 +53,9 @@ #include #include #include -#include #include #include #include -#include #include #include #include diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp index 25d34c872d2..b06655385a8 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -51,7 +51,6 @@ #include #include #include -#include #include #include #include @@ -68,7 +67,6 @@ #include #include #include -#include #include #include #include diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.c b/src/modules/position_estimator_inav/position_estimator_inav_main.c index bf4f7ae9743..ad363efe064 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -42,14 +42,11 @@ #include #include #include -#include #include #include #include #include #include -#include -#include #include #include #include From 98ee73463f428b420d4aeb44af4a7a90d8d40e41 Mon Sep 17 00:00:00 2001 From: Stefan Rado Date: Mon, 17 Feb 2014 21:14:40 +0100 Subject: [PATCH 19/23] Removed obsolete ROMFS folder for removed logging build. The logging makefile was removed in 9a54c7c6. --- ROMFS/px4fmu_logging/init.d/rcS | 88 -------------------------- ROMFS/px4fmu_logging/logging/conv.zip | Bin 10087 -> 0 bytes 2 files changed, 88 deletions(-) delete mode 100644 ROMFS/px4fmu_logging/init.d/rcS delete mode 100644 ROMFS/px4fmu_logging/logging/conv.zip diff --git a/ROMFS/px4fmu_logging/init.d/rcS b/ROMFS/px4fmu_logging/init.d/rcS deleted file mode 100644 index 7b885671978..00000000000 --- a/ROMFS/px4fmu_logging/init.d/rcS +++ /dev/null @@ -1,88 +0,0 @@ -#!nsh -# -# PX4FMU startup script for logging purposes -# - -# -# Try to mount the microSD card. -# -echo "[init] looking for microSD..." -if mount -t vfat /dev/mmcsd0 /fs/microsd -then - echo "[init] card mounted at /fs/microsd" - # Start playing the startup tune - tone_alarm start -else - echo "[init] no microSD card found" - # Play SOS - tone_alarm error -fi - -uorb start - -# -# Start sensor drivers here. -# - -ms5611 start -adc start - -# mag might be external -if hmc5883 start -then - echo "using HMC5883" -fi - -if mpu6000 start -then - echo "using MPU6000" -fi - -if l3gd20 start -then - echo "using L3GD20(H)" -fi - -if lsm303d start -then - set BOARD fmuv2 -else - set BOARD fmuv1 -fi - -# Start airspeed sensors -if meas_airspeed start -then - echo "using MEAS airspeed sensor" -else - if ets_airspeed start - then - echo "using ETS airspeed sensor (bus 3)" - else - if ets_airspeed start -b 1 - then - echo "Using ETS airspeed sensor (bus 1)" - fi - fi -fi - -# -# Start the sensor collection task. -# IMPORTANT: this also loads param offsets -# ALWAYS start this task before the -# preflight_check. -# -if sensors start -then - echo "SENSORS STARTED" -fi - -sdlog2 start -r 250 -e -b 16 - -if sercon -then - echo "[init] USB interface connected" - - # Try to get an USB console - nshterm /dev/ttyACM0 & -fi \ No newline at end of file diff --git a/ROMFS/px4fmu_logging/logging/conv.zip b/ROMFS/px4fmu_logging/logging/conv.zip deleted file mode 100644 index 7cb837e5666f51eca7ed803dfef4581f01bec1f3..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 10087 zcmaL7V{j!v*Zq6qi8b-WwoYu@b~3Ruv27bCwrx&qn-kl{1b3eIR^58)|K5A6t7~_E z=vCEK>&x2vrzisf`5gcNd;_41Ac~;1hZwjZ007J^000u;2f)e1#@?L8z{JJYfx*E; zO%)aZK`{rbEVu%z?BWg&0E2u40|5T#eYduc{RTVwSLf&Vs=&Ob3ipT&^VL98!Sy1Y z`jRQoLtoBNXx?aYwOR?Vo0Bcn=lM9cNHVfu(FC6YGInbJ!=0DlI%Mz9e(ia~%Nx6q zfl3ScYmK(K4KOmwMkJ7^rm`bXNH9?@2Lod)Dp+lHynv?9F6TIVDKiV;rFkIKYz;3Bf5Ch-qX|N?qkU9?pV<-K9z`#ZbF6`CW_Fi+w2FcLJukytO^TKNyog*Lu3Qj0$T}|*x*y#1#v+PKorgM^R zci~g&oh8_tM7z^CS6E7wz_-$h)(;mciFG37LA`wkS-0%~oJ8JYPK(q+;$$+}g^O5* zQbUwNC6r(`ItwsY5&3YpaT?7{tX*5Z8Ucz)2=0?cNd6PRL*XexdQnJf+{VsQX0v{FM`>qGo((^C9 zoQa$tpBp0biCrt#z`waxLtQ%8>Cw9jl=iIRlszR~JIPo`;~ei~`+Uj!kU$U-O)J>5papx6c~e1D-*AboN=+cARG*U2`IK;r z`dBCl>^h2Giz5tO-#;q$%LM2uImOFTF z15Tl2KU2flP8UMuZ0IQMfdE;xIr1Vx0MiW7vI=BEJUgqlMso3ED2~d;fTGyTPYxg; z9-6ci8j)y=qRNm|_k;7W;z9R>sv+M;G|o!!ry4r$uT&H-P`xgZOV7b+xMlJ3UHAt| z@7~zlQ}`x{vTAg);NzH{a`jkJEw8Y^Aqow%jTzZ6eJX`%$d+i6O(s1yy~SbG7)l%1 zU(w688v4u)Egv_do{)S*&dm!u;3+hy7QZUW@i9_CtH83k%?;#8{3Zc&kDCRzBRx1g z8yx@Iln4M4SsiA=;>rui0I&E5fR+6#r$DL_^wrW~&v_!&RXQ>ur zZ*mMrdrSxh;upU$c#_rI5yyqBBu`RsFqv_g8ZWI-DqQ98XN2c)Lp5Zyq1#n*GB$jB zmnCIsWFz9e7G?WsWp=9L6kZ)^m%WNtwFKj4zfVAXsAKLHu*%Dy!S-}J@U*lcw$6+0mql^qO1PMlw9WCzcc5TmM zFB76W{8^t4>+G*F8kc||q0jo!2o_r^p+}&&p&9+Knt1+gLJrI_$tJMo;Kh;$c|?RB69 zO@YF&0_P~_y{kw{t#mtt+WFgEw^QxQpj_{+?oPHH=-8Z6V0iG~kRkI>x3a zL{Mx}cWib{tDX{_YD`_MAgqCDbflvmIW#frWum#j6n0?2MR4AQ_+ZNU$vAfE5S590 zV7-e{p)gTEWpB9}*6+N>G7du#Gqvc>nj3kwn1VtpOdRuIqr0yVf#rTP*8ZkytGX0Z z!=}Art+{j;bJfYDpo4pfYudZd&p6Y1z6v>f=Je*3TK7w1G=j=O9>IdXn5Tlz!GCOY z6{>`>ekR}{AXS^b8)^Lw;iAPx$5xKPXT7B35;++&v7#;PvA@0IXIC>qOMOuF;-ElF z`q#s{mduj0M~7Xd57Rb*bk2KtE7$_)(CFcp4W-#$(Z;nLolzwp=}0x54^g`gg4~DE zo$Am_ZlV;tsmc5%OAK&qGL>+fQpe6z|HoyS@gI?i1;fz=_$(%g40`W7GP*)C50(sJ z>%_S3*ZgA!_ye50zRwHdi!%rHdVy=B+*A*cs_{%~{3hmA&dPS?^mgV@cknhTBWp{s zm?@IJsV%zgv=(YyH8%y(+gU3Fg{?|y%AwM&P%Umg>r$bsnkEL18!kSF@0_om3wnE! zuZXmy-hR)zAZ(Q|1$I;6=pF|AYsS^f8evZTvnp3CTxpe)w@3t|Xozy5i>6@y0D9&T zkkB{gISID5nRHt+Sd|=zqZx9|%)pg=zAOy@fJlGa6`UJ-$|Do z>BxN%eHD8r#_!`l5x=y5&u^jsRD=%~0Qoc;IFg>w-^;?I)cz6pt$IS*6iVrHUfjIK zpLZ6=>RO#oBdy00(Q(&?5!*9cH(Bu&Wvz7CqQL#p)27bGjnJd)h(UpL52WN_&o~IC z$Zj}s>eD=;d_l9?lvkFWdd}%>k99_D&_5qK;3s~_nMWv+LQb@pk5GhtJ0LNk3Vn4!u|;lO6lX*&?%l6pECcO_?=dSHVj38fimtMTIZ) z9$?i!LT6UI?Iv4+TA?zyZ`oBd5Grj>?ut$diMKdSiH)xYN?t7qkK0{)p4cCT!IQwa zDYZ)Zt9eI&>Xl9S_N%ewPskVEnRnh^6wP zngawH0)ad-_pK|_B7q5TPskoUP;2zi3TesJFl5LoYGjg?ub`>H@A!@ch1T)oZI4&b z{^jXm&zspibReOBvNGtht7Cg>n8262R@V*R%ad`)!pZ0M*Hzam=kt5}3QRvP%HqH~ zyq)mV&U1we66s7OV>cSLBJ{OLM-g}qEmHG7{6>!uB>85==vmiePnZF)pPooL!(>P> zmdXS}Qf<~05u?8$l$%*tgB-J4f&7--_i#o1W@nTsra>FLvd?6*Hxh>e=*R;MgSt95 zf=C4(ZIFoJSt_Ag>r3<_w=KmbpGAI4IaG&Fj5XLm8N(Dw6$&3g! zQBwnSZU+s`_$D3qg1S}_uOS=iV|+g~r*J6}OX9>QQ7Sz3^>gOVM~DBC$d z9^G%3Io;Wp+`a|I?QcO6#5Vuo)2#o)g39c{^B$CE!BG|+R4sy6^5_#d`@Q*R}%M;35W8W^=(wGmS9CpEpjc`pauEG-<_5ah)ED?Bp!olU7x91Lh0{ik zpBf7QFgF7LVE=`ef6>L*-p-Z5_J49q@c(;mx#Y99-;hk)hDraLZG;I?79R6ll_v@7 zvohhDtD(n9%#u~N#s-Q>Z7LSUA`3WreVsb=e*UgJgIOz*jF3P0fy8MUhi!t$9;R?$b$?bb*I>mPS|;jD%ymor?2LkH+B$W$ zcpJ<%-l+YZJbFkNW{#yX=xDeoi|pK1E3vcOf(OYY?RJxEC*aF!-fZ9(@QeJ;QGqaa z@bzY3>hRTp1l1-8=NM4S#MJpJcZwslsv}SR^>U|!`w5j zZ{88}_vg=5Xy0Ait(M#QM>*GagGFhR%}m+1Amsgz{^EE!3KVNT&4Os&j(mxCiLYrO z_)6s#$#41%GMNR4vEB)pi*i&blTv+qL*zp;SZh#c^u804*)%WLxP*@Tc<>c z3yT|H@8&(k&-J5mXK+VB!z!#D-8YdVU9=j7oQY+0m5k{NkmKPZf-G_w)7Zy6OR8=* zHldXXV3KKbX+2c`iPVZJ2ypgj0iCDTT55BKoMudaBb-!^n+_d#QrI@_qDw`%3~m31 zZn?(}MteUvxukN5Z5pVjCsWO?`E5n3j^O4wrgVF|N^e_u_cts)tznSa5eY8lt+4e& z#RwCMp?2tTtrT7eG%ILmwr zeq?F~j8|;4JHeGYbtSA!q)26k*qUyJQV!A|0W)Fl;bSjwcG+k!zB9pXVO4sX`UaW$ z5>BQH7CN51#9Q90qZX8~E2`lR<$`5~HN8%@Eo!^d2-5I|9T)7*1$!jOMfz|T3u?iN zM;98@wirL5dMszG5x!kLfcN)6T{$nl%6jH{7BVKq)dgH*y;EDPOa6|d!~EPbwOZTl zfJvoff-?O}ju?>eXD1n9W>lNJ`Nm3iT6_Th zuAbv8VOP+99bJD$$l`0ykrTw^_Rz$78s7lniVhTfiB91|2kYhD`N{LGQ!d>c`72-3 ziSUn;&~LqP=GLQug^C^_Vgh%f6y0!wu6`1 z)G-Cym3Ipwr?=-9*XHM!`LnC5M8J5-O5oJw`c)hk!k;n@oX5V{gj=lHGx)pmm23ow zzd7`vtDQXK4HMHdeofnlFdt`mXhZp|H=>#MKM>?@F&I=s4wy`>=g+ z42`nS4p=M_lRn?B%y8?Mtqe;Y+8@=CzKbv*of5u*jg!R)%!gxpi*QK97m2BxYf)0& zKb*wz;I?}Ne_M_Qe+4zX8oml~1?Hnc`6Ww-Jod*S2leG5OsZwPSR=EKd1z^gpumyr z%=q^lP0{IATc1nZl_q{sDrets`+rQX#svvPN>i&x0F7lqeasD}=`O+2mZ5k$kN!V|1c^&YLFmZk9*~uZ~`^pd z4@rMp=IHIuQsKaNtu#-!c)IyUj7DAEi>)<>kk(jbA-U_c^UKKHP&Gr+iiRiDERd-aq zh+P|5&?=vd1lEO5l}SL%lD@Ye#7PX^oYQtw>jBc-Z=_4(sXCe%|a5Z%X@_c`t~jZpY!8Khe=AhRrA} ztx`6GI;tYfd6Ax|U5%-O4AliIh7rMSS_~nWQ>}ubO_!*RxYc6svR_&7iPOS*W)rT{ z{sTocP^EJj(@gPCp1K-?;j%PaEh@@)!S@RWS(M>C-QGa)YG41P1XAB$`1cv0Lp|ML zd8wKflNxdLocgrDNtC1?R@gGMwVW( zv#~D9Jn7-T0$WdE1#5?-NtV=~N*Q^+xmWZQNqPky8P4;T^IgQw66Gt+JN8S?HP@lR z!u}C*GlG>z)s;bj=%uSps-m2MY8 z8mS}lGXRe!RTk&Sqq-(Ci@8+K@|j%V*I9Gx$iq44a=E zZRXBV%M80qLPqTLfy=5ZQV}Gfdm#%dE8|~I#D^GWrsN{OQ$nG^m zZEE`6?L*6KK{A+aiIt{keG)%dAC#ZmfoiNC*~vMOb>QlZa?@hOLoTZaCI~e2X?Q|q z8$a|@mh`}Asz+6%;na*;Wur}OO}=q%^wOZFY0z5C4#~S25o22u6r|HY*9*TLf9^QH zWB}I>1`gA8kw$#zu8Rj=&3;UWX+!eNSn8sbLMZ`SYL zn=Pl`fs}ErPIr&4@;z(QW38QB9-F?IV`BrCS-dt$_q#AGInEUOK(APurk7-s+4fGN zHr4><)@Vcq;2CoXVg_Qv`^v^FJ^V`Hc;hv^`vKVJU#=7#-n-r(2)r($PRC7jQEmlj z$R{q?gCXC1LXXw^)Kk+ls#xyf%%`&rFw{xuoHE^CSVM=tTUd4C!(g8lP_$ zFJG2gNt(vm76ED}n);19GcJq}s5LC@kB44z&p_k3#$47Ju_|NKVrqfCL#79(Jac)003n)Ho5FmCLLucf?$~yaQULXV?J$|;#>DWYy{&u;Rb!6rrStyGTf}a7Q z^5Rk1NzOB|+}6?OAVt|z#Ad*TiPImk|F51y=(vY|R zx4ksxT6MpP8$#i?wb|6e^>RJM=;nTftxj2fcz4tdXJ2kTErLqm3H>V|ai z4}S}`&C5B;yKTD4SCF)uhmH16Y)e`o<|Bz*nn5y?IO07&+{>X16>u4J_Up_bw?(+GRfjY;Xj`uoKS-D<5` z-L_GmuLRUze5{M-j=IsV1hJ#LdnjrV6t(I$qdFvrRgbTf&da^gs}bmuG6D8ew+ajmg4rk>39hFvuwM4 zt|j}Ornf`Sha${!v_32_{G@XjJ^xmi*x^dv_H^9G*LG-4sZRz`E3(*MJNED|o*nX0zN$Qs-T%wj8mXE&byq zMD8g+;Np{?)ouGtqQWP^JD*~KsG<&7Uv}WMb>vxaWeK6XPJ=T}IUV+=f&06!wXjS; z4i9DT=9-&=zXHp3y1%$uc`yzjY)9n#GvoruL$-4k>(jX}3AMKs#&WvqU8-2Q%%-yJ z;1^yIqe8%gEnd-YCCPD$v1;B#OX+^BjPPn;g&hG2U@G>Be#B8N+4}C#$GmIAx8?JEk|jlYP2*=hR+NO+=l=0vr=~!=)MWas;{X{(2!D>!x-DX zh9-EiLLq7~jzi|l^s|V0GJ-BEf7f7lBvBu3)@VefffTE|hUsHc{~$s^4WZmkCb_nS zAB-0BXf##|C3y(6xY#l9U0&}R^XE+Y8qX7j%V>PPC8LVz7PP#AlD1}4M0y54v|v<> z{Z}6CBBuP)i0kbes+#taHsP4ZbRm~_s*$X31U75CX*VjcjJO3O2eWn=3&y%H*5W)zUjYIcQx^7 z1YYh-y(2JsRXgW{21biscnn9&A|0WTBLq#?WiIp{OKbuXXp>ZS|?(zog175NCKj6hNK{q;Q5dP9?Fpevn3hD+iR`2!Klnfuz*MP{iSAXQ5-4S7^yCK^u?fO` zZzF$Aq9YA8TS}wNNcAh-SdPvnp`RGhJ9L9n!Pkwq?rDLGcfOrMa z`|{D(#~T|sHb8FhUH0SV!KqU;u0$O7?9qp9$6MFVG+gT54blp1MM)eQeGVFV&~Uz2 z>rS1?PU~M`WOS)o)bJ{0L$GViE0f{M|C}0{DYUKj{>(E-)+%NT5{=-26*ZQqd-X*5 z^pabY<$M{frMfE2q0na)f+6##B)~C@|C}jxW_$~ZnkIbVho8U9%KiFt`uTXAJvrk( zT-^2J&!-66wf|Jz$b07BM?F6WHhw=s+a!*?;yA9jP4D?bCtPQMpOf+QfcY&z32ye7 z;=*Ql9@z6{>w4dE?6IyJ)-A)i6vG~I_gC|&i*1Z2k;>SgBAGmRI|>}~^;kekOZXzY zEKYTPK8;b(KX!sZuMiu%BdAo>{+7HtD&%yxQv*L>%KiY|OXHlchZg)V&TL1oo3qKc z-C<%hX>Pw5GK|MTu6SiuPk)A^Ajt~Ks=!G&BgUjqPexIv+20ZHZ&z}w>~p_&7O)EO z1caB#HdXkPuX$ygOFYt;Fp_<$PZHL{eXgC_M-C51+D6VDnV-v&l)`;%PS96J*c62M z<9srzb=K0{yA?BBO>t>%O?kjyD`|5P@eRpwwE$J1fZt)6mH9#FUX+$85(?S#NwRlH z@y6MUs$z@VW*LJJNr@ppg(!Dterc@r(kUhs_6rriCudD#%^QO{JEUjauTH^y8eICb z6;AH88J_a&i;e83Zz1r#3lBDexLCCgWAe-;PH3y&!I2D*pu8E!?r=y^S8qp8*inR; zTzTc`19pHpT1Y3HkUGyV>a#rK7>b$-ziV$1LT~U+y;Rp^;@j~)np$i3!Vh>T#)eBF zW185bV8h%cr>&{hP~JKeCNeq?pp4`*2wz42K1X(F6n>GXQGM8jUVbVz{KXhvk{ir+@oGBgyzl+>}9XlZEtoo3hR6PsES$x z2jT_EH3IBNa6{OY$9A)v?=KPX)>WCCa$7gScMIL5ooD-ldDd$>j)yg0e;%;G6tV0| zcMcg(HR?fll5ETzda7@uI{j&swj^Ax$|eMZ>8i@ECJh_{jVdgvl|w-qv1moI7>JhF zX*Ku)ZOserygHlC%Y<1CtFR1jTYp}TGs4T+yZ}Rrw-SDB6u4u|BaV50EEe9i{?H;U ziHj|Z`?^-#@!wg)W~w&Ik5-$X87L^nuDZ>T=>om$`3ogC*nH7L5Ubj2DT(X`wA}-2 zn2K2(g&`f4GCyykS(XU>&a$2r1YDs@kGpdz6?pZ6DDM&@@ZJX0;Sqpd0ms=_TyHd1 zRb91(^(}JdsB{ylxj&?HH)V=L6Z!H)=6Uz0#WMe_Sf8@5Pbt?@RQ4H_&4#TvOu3RM)JP-W(sg zDW4d=z4ipO+`p zY00=CwTB`Ti@zpM^yNz_AK5{LwLR^|qgDL|KeD}D$J1r#9J0rkIJsD_Q6}`JVzrVpx?F1eg)3m zA&*Bp69#oz7;{xpIdZ}Kyk~N#WiM`-SC34mxkcqWvBArxV3%!-5Y>Kgie# zdN920HKh$PHt4-z!5ObncNxV_o$?)|?AQ?5HJ{3DYba@|K(SA!?MU}Y$@xlUXI2iF zec=!9S^B*7UG~eL9fd+YCGR?brr$){5FbzW1Tu5~@-d`vYbZrUTQ%Ugd z$BF-tn8O~yCZe4kSDgNmN73>$V8n_Fpl5z!5m|phF_BSLBE;i5D996k)xXYTHNh^T z`~oP-fPoW2{D1Lf*#EdQ0N|gC07U`+IsKos8PxwxoBbc8*?(*P@3GH+Yj*!5+5T$? x^iS?TL!kd3x&Q6#|CXaB`{(!n^7%i#|M>iW=|MsN=Q9xh*8RV#Pw}7I{{>mxP2T_j From 1afa53a159ee247a1c57ca59912077c5076c3997 Mon Sep 17 00:00:00 2001 From: Stefan Rado Date: Mon, 17 Feb 2014 21:15:35 +0100 Subject: [PATCH 20/23] Cleanup: Moved sdlog2 file conversion scripts to separate folder. --- Tools/{ => sdlog2}/README.txt | 0 Tools/{ => sdlog2}/logconv.m | 1070 ++++++++++++++--------------- Tools/{ => sdlog2}/sdlog2_dump.py | 0 3 files changed, 535 insertions(+), 535 deletions(-) rename Tools/{ => sdlog2}/README.txt (100%) rename Tools/{ => sdlog2}/logconv.m (97%) rename Tools/{ => sdlog2}/sdlog2_dump.py (100%) diff --git a/Tools/README.txt b/Tools/sdlog2/README.txt similarity index 100% rename from Tools/README.txt rename to Tools/sdlog2/README.txt diff --git a/Tools/logconv.m b/Tools/sdlog2/logconv.m similarity index 97% rename from Tools/logconv.m rename to Tools/sdlog2/logconv.m index c416b8095e8..e19c97fa3ce 100644 --- a/Tools/logconv.m +++ b/Tools/sdlog2/logconv.m @@ -1,535 +1,535 @@ -% This Matlab Script can be used to import the binary logged values of the -% PX4FMU into data that can be plotted and analyzed. - -%% ************************************************************************ -% PX4LOG_PLOTSCRIPT: Main function -% ************************************************************************ -function PX4Log_Plotscript - -% Clear everything -clc -clear all -close all - -% ************************************************************************ -% SETTINGS -% ************************************************************************ - -% Set the path to your sysvector.bin file here -filePath = 'log001.bin'; - -% Set the minimum and maximum times to plot here [in seconds] -mintime=0; %The minimum time/timestamp to display, as set by the user [0 for first element / start] -maxtime=0; %The maximum time/timestamp to display, as set by the user [0 for last element / end] - -%Determine which data to plot. Not completely implemented yet. -bDisplayGPS=true; - -%conversion factors -fconv_gpsalt=1; %[mm] to [m] -fconv_gpslatlong=1; %[gps_raw_position_unit] to [deg] -fconv_timestamp=1E-6; % [microseconds] to [seconds] - -% ************************************************************************ -% Import the PX4 logs -% ************************************************************************ -ImportPX4LogData(); - -%Translate min and max plot times to indices -time=double(sysvector.TIME_StartTime) .*fconv_timestamp; -mintime_log=time(1); %The minimum time/timestamp found in the log -maxtime_log=time(end); %The maximum time/timestamp found in the log -CurTime=mintime_log; %The current time at which to draw the aircraft position - -[imintime,imaxtime]=FindMinMaxTimeIndices(); - -% ************************************************************************ -% PLOT & GUI SETUP -% ************************************************************************ -NrFigures=5; -NrAxes=10; -h.figures(1:NrFigures)=0.0; % Temporary initialization of figure handle array - these are numbered consecutively -h.axes(1:NrAxes)=0.0; % Temporary initialization of axes handle array - these are numbered consecutively -h.pathpoints=[]; % Temporary initiliazation of path points - -% Setup the GUI to control the plots -InitControlGUI(); -% Setup the plotting-GUI (figures, axes) itself. -InitPlotGUI(); - -% ************************************************************************ -% DRAW EVERYTHING -% ************************************************************************ -DrawRawData(); -DrawCurrentAircraftState(); - -%% ************************************************************************ -% *** END OF MAIN SCRIPT *** -% NESTED FUNCTION DEFINTIONS FROM HERE ON -% ************************************************************************ - - -%% ************************************************************************ -% IMPORTPX4LOGDATA (nested function) -% ************************************************************************ -% Attention: This is the import routine for firmware from ca. 03/2013. -% Other firmware versions might require different import -% routines. - -%% ************************************************************************ -% IMPORTPX4LOGDATA (nested function) -% ************************************************************************ -% Attention: This is the import routine for firmware from ca. 03/2013. -% Other firmware versions might require different import -% routines. - -function ImportPX4LogData() - - % ************************************************************************ - % RETRIEVE SYSTEM VECTOR - % ************************************************************************* - % //All measurements in NED frame - - % Convert to CSV - %arg1 = 'log-fx61-20130721-2.bin'; - arg1 = filePath; - delim = ','; - time_field = 'TIME'; - data_file = 'data.csv'; - csv_null = ''; - - if not(exist(data_file, 'file')) - s = system( sprintf('python sdlog2_dump.py "%s" -f "%s" -t"%s" -d"%s" -n"%s"', arg1, data_file, time_field, delim, csv_null) ); - end - - if exist(data_file, 'file') - - %data = csvread(data_file); - sysvector = tdfread(data_file, ','); - - % shot the flight time - time_us = sysvector.TIME_StartTime(end) - sysvector.TIME_StartTime(1); - time_s = uint64(time_us*1e-6); - time_m = uint64(time_s/60); - time_s = time_s - time_m * 60; - - disp([sprintf('Flight log duration: %d:%d (minutes:seconds)', time_m, time_s) char(10)]); - - disp(['logfile conversion finished.' char(10)]); - else - disp(['file: ' data_file ' does not exist' char(10)]); - end -end - -%% ************************************************************************ -% INITCONTROLGUI (nested function) -% ************************************************************************ -%Setup central control GUI components to control current time where data is shown -function InitControlGUI() - %********************************************************************** - % GUI size definitions - %********************************************************************** - dxy=5; %margins - %Panel: Plotctrl - dlabels=120; - dsliders=200; - dedits=80; - hslider=20; - - hpanel1=40; %panel1 - hpanel2=220;%panel2 - hpanel3=3*hslider+4*dxy+3*dxy;%panel3. - - width=dlabels+dsliders+dedits+4*dxy+2*dxy; %figure width - height=hpanel1+hpanel2+hpanel3+4*dxy; %figure height - - %********************************************************************** - % Create GUI - %********************************************************************** - h.figures(1)=figure('Units','pixels','position',[200 200 width height],'Name','Control GUI'); - h.guistatepanel=uipanel('Title','Current GUI state','Units','pixels','Position',[dxy dxy width-2*dxy hpanel1],'parent',h.figures(1)); - h.aircraftstatepanel=uipanel('Title','Current aircraft state','Units','pixels','Position',[dxy hpanel1+2*dxy width-2*dxy hpanel2],'parent',h.figures(1)); - h.plotctrlpanel=uipanel('Title','Plot Control','Units','pixels','Position',[dxy hpanel1+hpanel2+3*dxy width-2*dxy hpanel3],'parent',h.figures(1)); - - %%Control GUI-elements - %Slider: Current time - h.labels.CurTime=uicontrol(gcf,'style','text','Position',[dxy dxy dlabels hslider],'String','Current time t[s]:','parent',h.plotctrlpanel,'HorizontalAlignment','left'); - h.sliders.CurTime=uicontrol(gcf,'style','slider','units','pix','position',[2*dxy+dlabels dxy dsliders hslider],... - 'min',mintime,'max',maxtime,'value',mintime,'callback',@curtime_callback,'parent',h.plotctrlpanel); - temp=get(h.sliders.CurTime,'Max')-get(h.sliders.CurTime,'Min'); - set(h.sliders.CurTime,'SliderStep',[1.0/temp 5.0/temp]); - h.edits.CurTime=uicontrol(gcf,'style','edit','position',[3*dxy+dlabels+dsliders dxy dedits hslider],'String',get(h.sliders.CurTime,'value'),... - 'BackgroundColor','white','callback',@curtime_callback,'parent',h.plotctrlpanel); - - %Slider: MaxTime - h.labels.MaxTime=uicontrol(gcf,'style','text','position',[dxy 2*dxy+hslider dlabels hslider],'String','Max. time t[s] to display:','parent',h.plotctrlpanel,'HorizontalAlignment','left'); - h.sliders.MaxTime=uicontrol(gcf,'style','slider','units','pix','position',[2*dxy+dlabels 2*dxy+hslider dsliders hslider],... - 'min',mintime_log,'max',maxtime_log,'value',maxtime,'callback',@minmaxtime_callback,'parent',h.plotctrlpanel); - h.edits.MaxTime=uicontrol(gcf,'style','edit','position',[3*dxy+dlabels+dsliders 2*dxy+hslider dedits hslider],'String',get(h.sliders.MaxTime,'value'),... - 'BackgroundColor','white','callback',@minmaxtime_callback,'parent',h.plotctrlpanel); - - %Slider: MinTime - h.labels.MinTime=uicontrol(gcf,'style','text','position',[dxy 3*dxy+2*hslider dlabels hslider],'String','Min. time t[s] to dispay :','parent',h.plotctrlpanel,'HorizontalAlignment','left'); - h.sliders.MinTime=uicontrol(gcf,'style','slider','units','pix','position',[2*dxy+dlabels 3*dxy+2*hslider dsliders hslider],... - 'min',mintime_log,'max',maxtime_log,'value',mintime,'callback',@minmaxtime_callback,'parent',h.plotctrlpanel); - h.edits.MinTime=uicontrol(gcf,'style','edit','position',[3*dxy+dlabels+dsliders 3*dxy+2*hslider dedits hslider],'String',get(h.sliders.MinTime,'value'),... - 'BackgroundColor','white','callback',@minmaxtime_callback,'parent',h.plotctrlpanel); - - %%Current data/state GUI-elements (Multiline-edit-box) - h.edits.AircraftState=uicontrol(gcf,'style','edit','Units','normalized','position',[.02 .02 0.96 0.96],'Min',1,'Max',10,'String','This shows the current aircraft state',... - 'HorizontalAlignment','left','parent',h.aircraftstatepanel); - - h.labels.GUIState=uicontrol(gcf,'style','text','Units','pixels','position',[dxy dxy width-4*dxy hslider],'String','Current state of this GUI',... - 'HorizontalAlignment','left','parent',h.guistatepanel); - -end - -%% ************************************************************************ -% INITPLOTGUI (nested function) -% ************************************************************************ -function InitPlotGUI() - - % Setup handles to lines and text - h.markertext=[]; - templinehandle=0.0;%line([0 1],[0 5]); % Just a temporary handle to init array - h.markerline(1:NrAxes)=templinehandle; % the actual handle-array to the lines - these are numbered consecutively - h.markerline(1:NrAxes)=0.0; - - % Setup all other figures and axes for plotting - % PLOT WINDOW 1: GPS POSITION - h.figures(2)=figure('units','normalized','Toolbar','figure', 'Name', 'GPS Position'); - h.axes(1)=axes(); - set(h.axes(1),'Parent',h.figures(2)); - - % PLOT WINDOW 2: IMU, baro altitude - h.figures(3)=figure('Name', 'IMU / Baro Altitude'); - h.axes(2)=subplot(4,1,1); - h.axes(3)=subplot(4,1,2); - h.axes(4)=subplot(4,1,3); - h.axes(5)=subplot(4,1,4); - set(h.axes(2:5),'Parent',h.figures(3)); - - % PLOT WINDOW 3: ATTITUDE ESTIMATE, ACTUATORS/CONTROLS, AIRSPEEDS,... - h.figures(4)=figure('Name', 'Attitude Estimate / Actuators / Airspeeds'); - h.axes(6)=subplot(4,1,1); - h.axes(7)=subplot(4,1,2); - h.axes(8)=subplot(4,1,3); - h.axes(9)=subplot(4,1,4); - set(h.axes(6:9),'Parent',h.figures(4)); - - % PLOT WINDOW 4: LOG STATS - h.figures(5) = figure('Name', 'Log Statistics'); - h.axes(10)=subplot(1,1,1); - set(h.axes(10:10),'Parent',h.figures(5)); - -end - -%% ************************************************************************ -% DRAWRAWDATA (nested function) -% ************************************************************************ -%Draws the raw data from the sysvector, but does not add any -%marker-lines or so -function DrawRawData() - % ************************************************************************ - % PLOT WINDOW 1: GPS POSITION & GUI - % ************************************************************************ - figure(h.figures(2)); - % Only plot GPS data if available - if (sum(double(sysvector.GPS_Lat(imintime:imaxtime)))>0) && (bDisplayGPS) - %Draw data - plot3(h.axes(1),double(sysvector.GPS_Lat(imintime:imaxtime))*fconv_gpslatlong, ... - double(sysvector.GPS_Lon(imintime:imaxtime))*fconv_gpslatlong, ... - double(sysvector.GPS_Alt(imintime:imaxtime))*fconv_gpsalt,'r.'); - title(h.axes(1),'GPS Position Data(if available)'); - xlabel(h.axes(1),'Latitude [deg]'); - ylabel(h.axes(1),'Longitude [deg]'); - zlabel(h.axes(1),'Altitude above MSL [m]'); - grid on - - %Reset path - h.pathpoints=0; - end - - % ************************************************************************ - % PLOT WINDOW 2: IMU, baro altitude - % ************************************************************************ - figure(h.figures(3)); - plot(h.axes(2),time(imintime:imaxtime),[sysvector.IMU_MagX(imintime:imaxtime), sysvector.IMU_MagY(imintime:imaxtime), sysvector.IMU_MagZ(imintime:imaxtime)]); - title(h.axes(2),'Magnetometers [Gauss]'); - legend(h.axes(2),'x','y','z'); - plot(h.axes(3),time(imintime:imaxtime),[sysvector.IMU_AccX(imintime:imaxtime), sysvector.IMU_AccY(imintime:imaxtime), sysvector.IMU_AccZ(imintime:imaxtime)]); - title(h.axes(3),'Accelerometers [m/s²]'); - legend(h.axes(3),'x','y','z'); - plot(h.axes(4),time(imintime:imaxtime),[sysvector.IMU_GyroX(imintime:imaxtime), sysvector.IMU_GyroY(imintime:imaxtime), sysvector.IMU_GyroZ(imintime:imaxtime)]); - title(h.axes(4),'Gyroscopes [rad/s]'); - legend(h.axes(4),'x','y','z'); - plot(h.axes(5),time(imintime:imaxtime),sysvector.SENS_BaroAlt(imintime:imaxtime),'color','blue'); - if(bDisplayGPS) - hold on; - plot(h.axes(5),time(imintime:imaxtime),double(sysvector.GPS_Alt(imintime:imaxtime)).*fconv_gpsalt,'color','red'); - hold off - legend('Barometric Altitude [m]','GPS Altitude [m]'); - else - legend('Barometric Altitude [m]'); - end - title(h.axes(5),'Altitude above MSL [m]'); - - % ************************************************************************ - % PLOT WINDOW 3: ATTITUDE ESTIMATE, ACTUATORS/CONTROLS, AIRSPEEDS,... - % ************************************************************************ - figure(h.figures(4)); - %Attitude Estimate - plot(h.axes(6),time(imintime:imaxtime), [sysvector.ATT_Roll(imintime:imaxtime), sysvector.ATT_Pitch(imintime:imaxtime), sysvector.ATT_Yaw(imintime:imaxtime)] .*180./3.14159); - title(h.axes(6),'Estimated attitude [deg]'); - legend(h.axes(6),'roll','pitch','yaw'); - %Actuator Controls - plot(h.axes(7),time(imintime:imaxtime), [sysvector.ATTC_Roll(imintime:imaxtime), sysvector.ATTC_Pitch(imintime:imaxtime), sysvector.ATTC_Yaw(imintime:imaxtime), sysvector.ATTC_Thrust(imintime:imaxtime)]); - title(h.axes(7),'Actuator control [-]'); - legend(h.axes(7),'ATT CTRL Roll [-1..+1]','ATT CTRL Pitch [-1..+1]','ATT CTRL Yaw [-1..+1]','ATT CTRL Thrust [0..+1]'); - %Actuator Controls - plot(h.axes(8),time(imintime:imaxtime), [sysvector.OUT0_Out0(imintime:imaxtime), sysvector.OUT0_Out1(imintime:imaxtime), sysvector.OUT0_Out2(imintime:imaxtime), sysvector.OUT0_Out3(imintime:imaxtime), sysvector.OUT0_Out4(imintime:imaxtime), sysvector.OUT0_Out5(imintime:imaxtime), sysvector.OUT0_Out6(imintime:imaxtime), sysvector.OUT0_Out7(imintime:imaxtime)]); - title(h.axes(8),'Actuator PWM (raw-)outputs [µs]'); - legend(h.axes(8),'CH1','CH2','CH3','CH4','CH5','CH6','CH7','CH8'); - set(h.axes(8), 'YLim',[800 2200]); - %Airspeeds - plot(h.axes(9),time(imintime:imaxtime), sysvector.AIRS_IndSpeed(imintime:imaxtime)); - hold on - plot(h.axes(9),time(imintime:imaxtime), sysvector.AIRS_TrueSpeed(imintime:imaxtime)); - hold off - %add GPS total airspeed here - title(h.axes(9),'Airspeed [m/s]'); - legend(h.axes(9),'Indicated Airspeed (IAS)','True Airspeed (TAS)','GPS Airspeed'); - %calculate time differences and plot them - intervals = zeros(0,imaxtime - imintime); - for k = imintime+1:imaxtime - intervals(k) = time(k) - time(k-1); - end - plot(h.axes(10), time(imintime:imaxtime), intervals); - - %Set same timescale for all plots - for i=2:NrAxes - set(h.axes(i),'XLim',[mintime maxtime]); - end - - set(h.labels.GUIState,'String','OK','BackgroundColor',[240/255 240/255 240/255]); -end - -%% ************************************************************************ -% DRAWCURRENTAIRCRAFTSTATE(nested function) -% ************************************************************************ -function DrawCurrentAircraftState() - %find current data index - i=find(time>=CurTime,1,'first'); - - %********************************************************************** - % Current aircraft state label update - %********************************************************************** - acstate{1,:}=[sprintf('%s \t\t','GPS Pos:'),'[lat=',num2str(double(sysvector.GPS_Lat(i))*fconv_gpslatlong),'°, ',... - 'lon=',num2str(double(sysvector.GPS_Lon(i))*fconv_gpslatlong),'°, ',... - 'alt=',num2str(double(sysvector.GPS_Alt(i))*fconv_gpsalt),'m]']; - acstate{2,:}=[sprintf('%s \t\t','Mags[gauss]'),'[x=',num2str(sysvector.IMU_MagX(i)),... - ', y=',num2str(sysvector.IMU_MagY(i)),... - ', z=',num2str(sysvector.IMU_MagZ(i)),']']; - acstate{3,:}=[sprintf('%s \t\t','Accels[m/s²]'),'[x=',num2str(sysvector.IMU_AccX(i)),... - ', y=',num2str(sysvector.IMU_AccY(i)),... - ', z=',num2str(sysvector.IMU_AccZ(i)),']']; - acstate{4,:}=[sprintf('%s \t\t','Gyros[rad/s]'),'[x=',num2str(sysvector.IMU_GyroX(i)),... - ', y=',num2str(sysvector.IMU_GyroY(i)),... - ', z=',num2str(sysvector.IMU_GyroZ(i)),']']; - acstate{5,:}=[sprintf('%s \t\t','Altitude[m]'),'[Barometric: ',num2str(sysvector.SENS_BaroAlt(i)),'m, GPS: ',num2str(double(sysvector.GPS_Alt(i))*fconv_gpsalt),'m]']; - acstate{6,:}=[sprintf('%s \t','Est. attitude[deg]:'),'[Roll=',num2str(sysvector.ATT_Roll(i).*180./3.14159),... - ', Pitch=',num2str(sysvector.ATT_Pitch(i).*180./3.14159),... - ', Yaw=',num2str(sysvector.ATT_Yaw(i).*180./3.14159),']']; - acstate{7,:}=sprintf('%s \t[','Actuator Ctrls [-]:'); - %for j=1:4 - acstate{7,:}=[acstate{7,:},num2str(sysvector.ATTC_Roll(i)),',']; - acstate{7,:}=[acstate{7,:},num2str(sysvector.ATTC_Pitch(i)),',']; - acstate{7,:}=[acstate{7,:},num2str(sysvector.ATTC_Yaw(i)),',']; - acstate{7,:}=[acstate{7,:},num2str(sysvector.ATTC_Thrust(i)),',']; - %end - acstate{7,:}=[acstate{7,:},']']; - acstate{8,:}=sprintf('%s \t[','Actuator Outputs [PWM/µs]:'); - %for j=1:8 - acstate{8,:}=[acstate{8,:},num2str(sysvector.OUT0_Out0(i)),',']; - acstate{8,:}=[acstate{8,:},num2str(sysvector.OUT0_Out1(i)),',']; - acstate{8,:}=[acstate{8,:},num2str(sysvector.OUT0_Out2(i)),',']; - acstate{8,:}=[acstate{8,:},num2str(sysvector.OUT0_Out3(i)),',']; - acstate{8,:}=[acstate{8,:},num2str(sysvector.OUT0_Out4(i)),',']; - acstate{8,:}=[acstate{8,:},num2str(sysvector.OUT0_Out5(i)),',']; - acstate{8,:}=[acstate{8,:},num2str(sysvector.OUT0_Out6(i)),',']; - acstate{8,:}=[acstate{8,:},num2str(sysvector.OUT0_Out7(i)),',']; - %end - acstate{8,:}=[acstate{8,:},']']; - acstate{9,:}=[sprintf('%s \t','Airspeed[m/s]:'),'[IAS: ',num2str(sysvector.AIRS_IndSpeed(i)),', TAS: ',num2str(sysvector.AIRS_TrueSpeed(i)),']']; - - set(h.edits.AircraftState,'String',acstate); - - %********************************************************************** - % GPS Plot Update - %********************************************************************** - %Plot traveled path, and and time. - figure(h.figures(2)); - hold on; - if(CurTime>mintime+1) %the +1 is only a small bugfix - h.pathline=plot3(h.axes(1),double(sysvector.GPS_Lat(imintime:i))*fconv_gpslatlong, ... - double(sysvector.GPS_Lon(imintime:i))*fconv_gpslatlong, ... - double(sysvector.GPS_Alt(imintime:i))*fconv_gpsalt,'b','LineWidth',2); - end; - hold off - %Plot current position - newpoint=[double(sysvector.GPS_Lat(i))*fconv_gpslatlong double(sysvector.GPS_Lat(i))*fconv_gpslatlong double(sysvector.GPS_Alt(i))*fconv_gpsalt]; - if(numel(h.pathpoints)<=3) %empty path - h.pathpoints(1,1:3)=newpoint; - else %Not empty, append new point - h.pathpoints(size(h.pathpoints,1)+1,:)=newpoint; - end - axes(h.axes(1)); - line(h.pathpoints(:,1),h.pathpoints(:,2),h.pathpoints(:,3),'LineStyle','none','Marker','.','MarkerEdge','black','MarkerSize',20); - - - % Plot current time (small label next to current gps position) - textdesc=strcat(' t=',num2str(time(i)),'s'); - if(isvalidhandle(h.markertext)) - delete(h.markertext); %delete old text - end - h.markertext=text(double(sysvector.GPS_Lat(i))*fconv_gpslatlong,double(sysvector.GPS_Lon(i))*fconv_gpslatlong,... - double(sysvector.GPS_Alt(i))*fconv_gpsalt,textdesc); - set(h.edits.CurTime,'String',CurTime); - - %********************************************************************** - % Plot the lines showing the current time in the 2-d plots - %********************************************************************** - for i=2:NrAxes - if(isvalidhandle(h.markerline(i))) delete(h.markerline(i)); end - ylims=get(h.axes(i),'YLim'); - h.markerline(i)=line([CurTime CurTime] ,get(h.axes(i),'YLim'),'Color','black'); - set(h.markerline(i),'parent',h.axes(i)); - end - - set(h.labels.GUIState,'String','OK','BackgroundColor',[240/255 240/255 240/255]); -end - -%% ************************************************************************ -% MINMAXTIME CALLBACK (nested function) -% ************************************************************************ -function minmaxtime_callback(hObj,event) %#ok - new_mintime=get(h.sliders.MinTime,'Value'); - new_maxtime=get(h.sliders.MaxTime,'Value'); - - %Safety checks: - bErr=false; - %1: mintime must be < maxtime - if((new_mintime>maxtime) || (new_maxtimeCurTime) - set(h.labels.GUIState,'String','Error: Mintime cannot be bigger than CurTime! CurTime set to new mintime.','BackgroundColor','red'); - mintime=new_mintime; - CurTime=mintime; - bErr=true; - end - %3: MaxTime must be >CurTime - if(new_maxtime - %find current time - if(hObj==h.sliders.CurTime) - CurTime=get(h.sliders.CurTime,'Value'); - elseif (hObj==h.edits.CurTime) - temp=str2num(get(h.edits.CurTime,'String')); - if(tempmintime) - CurTime=temp; - else - %Error - set(h.labels.GUIState,'String','Error: You tried to set an invalid current time! Previous value restored.','BackgroundColor','red'); - end - else - %Error - set(h.labels.GUIState,'String','Error: curtime_callback','BackgroundColor','red'); - end - - set(h.sliders.CurTime,'Value',CurTime); - set(h.edits.CurTime,'String',num2str(CurTime)); - - %Redraw time markers, but don't have to redraw the whole raw data - DrawCurrentAircraftState(); -end - -%% ************************************************************************ -% FINDMINMAXINDICES (nested function) -% ************************************************************************ -function [idxmin,idxmax] = FindMinMaxTimeIndices() - for i=1:size(sysvector.TIME_StartTime,1) - if time(i)>=mintime; idxmin=i; break; end - end - for i=1:size(sysvector.TIME_StartTime,1) - if maxtime==0; idxmax=size(sysvector.TIME_StartTime,1); break; end - if time(i)>=maxtime; idxmax=i; break; end - end - mintime=time(idxmin); - maxtime=time(idxmax); -end - -%% ************************************************************************ -% ISVALIDHANDLE (nested function) -% ************************************************************************ -function isvalid = isvalidhandle(handle) - if(exist(varname(handle))>0 && length(ishandle(handle))>0) - if(ishandle(handle)>0) - if(handle>0.0) - isvalid=true; - return; - end - end - end - isvalid=false; -end - -%% ************************************************************************ -% JUST SOME SMALL HELPER FUNCTIONS (nested function) -% ************************************************************************ -function out = varname(var) - out = inputname(1); -end - -%This is the end of the matlab file / the main function -end +% This Matlab Script can be used to import the binary logged values of the +% PX4FMU into data that can be plotted and analyzed. + +%% ************************************************************************ +% PX4LOG_PLOTSCRIPT: Main function +% ************************************************************************ +function PX4Log_Plotscript + +% Clear everything +clc +clear all +close all + +% ************************************************************************ +% SETTINGS +% ************************************************************************ + +% Set the path to your sysvector.bin file here +filePath = 'log001.bin'; + +% Set the minimum and maximum times to plot here [in seconds] +mintime=0; %The minimum time/timestamp to display, as set by the user [0 for first element / start] +maxtime=0; %The maximum time/timestamp to display, as set by the user [0 for last element / end] + +%Determine which data to plot. Not completely implemented yet. +bDisplayGPS=true; + +%conversion factors +fconv_gpsalt=1; %[mm] to [m] +fconv_gpslatlong=1; %[gps_raw_position_unit] to [deg] +fconv_timestamp=1E-6; % [microseconds] to [seconds] + +% ************************************************************************ +% Import the PX4 logs +% ************************************************************************ +ImportPX4LogData(); + +%Translate min and max plot times to indices +time=double(sysvector.TIME_StartTime) .*fconv_timestamp; +mintime_log=time(1); %The minimum time/timestamp found in the log +maxtime_log=time(end); %The maximum time/timestamp found in the log +CurTime=mintime_log; %The current time at which to draw the aircraft position + +[imintime,imaxtime]=FindMinMaxTimeIndices(); + +% ************************************************************************ +% PLOT & GUI SETUP +% ************************************************************************ +NrFigures=5; +NrAxes=10; +h.figures(1:NrFigures)=0.0; % Temporary initialization of figure handle array - these are numbered consecutively +h.axes(1:NrAxes)=0.0; % Temporary initialization of axes handle array - these are numbered consecutively +h.pathpoints=[]; % Temporary initiliazation of path points + +% Setup the GUI to control the plots +InitControlGUI(); +% Setup the plotting-GUI (figures, axes) itself. +InitPlotGUI(); + +% ************************************************************************ +% DRAW EVERYTHING +% ************************************************************************ +DrawRawData(); +DrawCurrentAircraftState(); + +%% ************************************************************************ +% *** END OF MAIN SCRIPT *** +% NESTED FUNCTION DEFINTIONS FROM HERE ON +% ************************************************************************ + + +%% ************************************************************************ +% IMPORTPX4LOGDATA (nested function) +% ************************************************************************ +% Attention: This is the import routine for firmware from ca. 03/2013. +% Other firmware versions might require different import +% routines. + +%% ************************************************************************ +% IMPORTPX4LOGDATA (nested function) +% ************************************************************************ +% Attention: This is the import routine for firmware from ca. 03/2013. +% Other firmware versions might require different import +% routines. + +function ImportPX4LogData() + + % ************************************************************************ + % RETRIEVE SYSTEM VECTOR + % ************************************************************************* + % //All measurements in NED frame + + % Convert to CSV + %arg1 = 'log-fx61-20130721-2.bin'; + arg1 = filePath; + delim = ','; + time_field = 'TIME'; + data_file = 'data.csv'; + csv_null = ''; + + if not(exist(data_file, 'file')) + s = system( sprintf('python sdlog2_dump.py "%s" -f "%s" -t"%s" -d"%s" -n"%s"', arg1, data_file, time_field, delim, csv_null) ); + end + + if exist(data_file, 'file') + + %data = csvread(data_file); + sysvector = tdfread(data_file, ','); + + % shot the flight time + time_us = sysvector.TIME_StartTime(end) - sysvector.TIME_StartTime(1); + time_s = uint64(time_us*1e-6); + time_m = uint64(time_s/60); + time_s = time_s - time_m * 60; + + disp([sprintf('Flight log duration: %d:%d (minutes:seconds)', time_m, time_s) char(10)]); + + disp(['logfile conversion finished.' char(10)]); + else + disp(['file: ' data_file ' does not exist' char(10)]); + end +end + +%% ************************************************************************ +% INITCONTROLGUI (nested function) +% ************************************************************************ +%Setup central control GUI components to control current time where data is shown +function InitControlGUI() + %********************************************************************** + % GUI size definitions + %********************************************************************** + dxy=5; %margins + %Panel: Plotctrl + dlabels=120; + dsliders=200; + dedits=80; + hslider=20; + + hpanel1=40; %panel1 + hpanel2=220;%panel2 + hpanel3=3*hslider+4*dxy+3*dxy;%panel3. + + width=dlabels+dsliders+dedits+4*dxy+2*dxy; %figure width + height=hpanel1+hpanel2+hpanel3+4*dxy; %figure height + + %********************************************************************** + % Create GUI + %********************************************************************** + h.figures(1)=figure('Units','pixels','position',[200 200 width height],'Name','Control GUI'); + h.guistatepanel=uipanel('Title','Current GUI state','Units','pixels','Position',[dxy dxy width-2*dxy hpanel1],'parent',h.figures(1)); + h.aircraftstatepanel=uipanel('Title','Current aircraft state','Units','pixels','Position',[dxy hpanel1+2*dxy width-2*dxy hpanel2],'parent',h.figures(1)); + h.plotctrlpanel=uipanel('Title','Plot Control','Units','pixels','Position',[dxy hpanel1+hpanel2+3*dxy width-2*dxy hpanel3],'parent',h.figures(1)); + + %%Control GUI-elements + %Slider: Current time + h.labels.CurTime=uicontrol(gcf,'style','text','Position',[dxy dxy dlabels hslider],'String','Current time t[s]:','parent',h.plotctrlpanel,'HorizontalAlignment','left'); + h.sliders.CurTime=uicontrol(gcf,'style','slider','units','pix','position',[2*dxy+dlabels dxy dsliders hslider],... + 'min',mintime,'max',maxtime,'value',mintime,'callback',@curtime_callback,'parent',h.plotctrlpanel); + temp=get(h.sliders.CurTime,'Max')-get(h.sliders.CurTime,'Min'); + set(h.sliders.CurTime,'SliderStep',[1.0/temp 5.0/temp]); + h.edits.CurTime=uicontrol(gcf,'style','edit','position',[3*dxy+dlabels+dsliders dxy dedits hslider],'String',get(h.sliders.CurTime,'value'),... + 'BackgroundColor','white','callback',@curtime_callback,'parent',h.plotctrlpanel); + + %Slider: MaxTime + h.labels.MaxTime=uicontrol(gcf,'style','text','position',[dxy 2*dxy+hslider dlabels hslider],'String','Max. time t[s] to display:','parent',h.plotctrlpanel,'HorizontalAlignment','left'); + h.sliders.MaxTime=uicontrol(gcf,'style','slider','units','pix','position',[2*dxy+dlabels 2*dxy+hslider dsliders hslider],... + 'min',mintime_log,'max',maxtime_log,'value',maxtime,'callback',@minmaxtime_callback,'parent',h.plotctrlpanel); + h.edits.MaxTime=uicontrol(gcf,'style','edit','position',[3*dxy+dlabels+dsliders 2*dxy+hslider dedits hslider],'String',get(h.sliders.MaxTime,'value'),... + 'BackgroundColor','white','callback',@minmaxtime_callback,'parent',h.plotctrlpanel); + + %Slider: MinTime + h.labels.MinTime=uicontrol(gcf,'style','text','position',[dxy 3*dxy+2*hslider dlabels hslider],'String','Min. time t[s] to dispay :','parent',h.plotctrlpanel,'HorizontalAlignment','left'); + h.sliders.MinTime=uicontrol(gcf,'style','slider','units','pix','position',[2*dxy+dlabels 3*dxy+2*hslider dsliders hslider],... + 'min',mintime_log,'max',maxtime_log,'value',mintime,'callback',@minmaxtime_callback,'parent',h.plotctrlpanel); + h.edits.MinTime=uicontrol(gcf,'style','edit','position',[3*dxy+dlabels+dsliders 3*dxy+2*hslider dedits hslider],'String',get(h.sliders.MinTime,'value'),... + 'BackgroundColor','white','callback',@minmaxtime_callback,'parent',h.plotctrlpanel); + + %%Current data/state GUI-elements (Multiline-edit-box) + h.edits.AircraftState=uicontrol(gcf,'style','edit','Units','normalized','position',[.02 .02 0.96 0.96],'Min',1,'Max',10,'String','This shows the current aircraft state',... + 'HorizontalAlignment','left','parent',h.aircraftstatepanel); + + h.labels.GUIState=uicontrol(gcf,'style','text','Units','pixels','position',[dxy dxy width-4*dxy hslider],'String','Current state of this GUI',... + 'HorizontalAlignment','left','parent',h.guistatepanel); + +end + +%% ************************************************************************ +% INITPLOTGUI (nested function) +% ************************************************************************ +function InitPlotGUI() + + % Setup handles to lines and text + h.markertext=[]; + templinehandle=0.0;%line([0 1],[0 5]); % Just a temporary handle to init array + h.markerline(1:NrAxes)=templinehandle; % the actual handle-array to the lines - these are numbered consecutively + h.markerline(1:NrAxes)=0.0; + + % Setup all other figures and axes for plotting + % PLOT WINDOW 1: GPS POSITION + h.figures(2)=figure('units','normalized','Toolbar','figure', 'Name', 'GPS Position'); + h.axes(1)=axes(); + set(h.axes(1),'Parent',h.figures(2)); + + % PLOT WINDOW 2: IMU, baro altitude + h.figures(3)=figure('Name', 'IMU / Baro Altitude'); + h.axes(2)=subplot(4,1,1); + h.axes(3)=subplot(4,1,2); + h.axes(4)=subplot(4,1,3); + h.axes(5)=subplot(4,1,4); + set(h.axes(2:5),'Parent',h.figures(3)); + + % PLOT WINDOW 3: ATTITUDE ESTIMATE, ACTUATORS/CONTROLS, AIRSPEEDS,... + h.figures(4)=figure('Name', 'Attitude Estimate / Actuators / Airspeeds'); + h.axes(6)=subplot(4,1,1); + h.axes(7)=subplot(4,1,2); + h.axes(8)=subplot(4,1,3); + h.axes(9)=subplot(4,1,4); + set(h.axes(6:9),'Parent',h.figures(4)); + + % PLOT WINDOW 4: LOG STATS + h.figures(5) = figure('Name', 'Log Statistics'); + h.axes(10)=subplot(1,1,1); + set(h.axes(10:10),'Parent',h.figures(5)); + +end + +%% ************************************************************************ +% DRAWRAWDATA (nested function) +% ************************************************************************ +%Draws the raw data from the sysvector, but does not add any +%marker-lines or so +function DrawRawData() + % ************************************************************************ + % PLOT WINDOW 1: GPS POSITION & GUI + % ************************************************************************ + figure(h.figures(2)); + % Only plot GPS data if available + if (sum(double(sysvector.GPS_Lat(imintime:imaxtime)))>0) && (bDisplayGPS) + %Draw data + plot3(h.axes(1),double(sysvector.GPS_Lat(imintime:imaxtime))*fconv_gpslatlong, ... + double(sysvector.GPS_Lon(imintime:imaxtime))*fconv_gpslatlong, ... + double(sysvector.GPS_Alt(imintime:imaxtime))*fconv_gpsalt,'r.'); + title(h.axes(1),'GPS Position Data(if available)'); + xlabel(h.axes(1),'Latitude [deg]'); + ylabel(h.axes(1),'Longitude [deg]'); + zlabel(h.axes(1),'Altitude above MSL [m]'); + grid on + + %Reset path + h.pathpoints=0; + end + + % ************************************************************************ + % PLOT WINDOW 2: IMU, baro altitude + % ************************************************************************ + figure(h.figures(3)); + plot(h.axes(2),time(imintime:imaxtime),[sysvector.IMU_MagX(imintime:imaxtime), sysvector.IMU_MagY(imintime:imaxtime), sysvector.IMU_MagZ(imintime:imaxtime)]); + title(h.axes(2),'Magnetometers [Gauss]'); + legend(h.axes(2),'x','y','z'); + plot(h.axes(3),time(imintime:imaxtime),[sysvector.IMU_AccX(imintime:imaxtime), sysvector.IMU_AccY(imintime:imaxtime), sysvector.IMU_AccZ(imintime:imaxtime)]); + title(h.axes(3),'Accelerometers [m/s²]'); + legend(h.axes(3),'x','y','z'); + plot(h.axes(4),time(imintime:imaxtime),[sysvector.IMU_GyroX(imintime:imaxtime), sysvector.IMU_GyroY(imintime:imaxtime), sysvector.IMU_GyroZ(imintime:imaxtime)]); + title(h.axes(4),'Gyroscopes [rad/s]'); + legend(h.axes(4),'x','y','z'); + plot(h.axes(5),time(imintime:imaxtime),sysvector.SENS_BaroAlt(imintime:imaxtime),'color','blue'); + if(bDisplayGPS) + hold on; + plot(h.axes(5),time(imintime:imaxtime),double(sysvector.GPS_Alt(imintime:imaxtime)).*fconv_gpsalt,'color','red'); + hold off + legend('Barometric Altitude [m]','GPS Altitude [m]'); + else + legend('Barometric Altitude [m]'); + end + title(h.axes(5),'Altitude above MSL [m]'); + + % ************************************************************************ + % PLOT WINDOW 3: ATTITUDE ESTIMATE, ACTUATORS/CONTROLS, AIRSPEEDS,... + % ************************************************************************ + figure(h.figures(4)); + %Attitude Estimate + plot(h.axes(6),time(imintime:imaxtime), [sysvector.ATT_Roll(imintime:imaxtime), sysvector.ATT_Pitch(imintime:imaxtime), sysvector.ATT_Yaw(imintime:imaxtime)] .*180./3.14159); + title(h.axes(6),'Estimated attitude [deg]'); + legend(h.axes(6),'roll','pitch','yaw'); + %Actuator Controls + plot(h.axes(7),time(imintime:imaxtime), [sysvector.ATTC_Roll(imintime:imaxtime), sysvector.ATTC_Pitch(imintime:imaxtime), sysvector.ATTC_Yaw(imintime:imaxtime), sysvector.ATTC_Thrust(imintime:imaxtime)]); + title(h.axes(7),'Actuator control [-]'); + legend(h.axes(7),'ATT CTRL Roll [-1..+1]','ATT CTRL Pitch [-1..+1]','ATT CTRL Yaw [-1..+1]','ATT CTRL Thrust [0..+1]'); + %Actuator Controls + plot(h.axes(8),time(imintime:imaxtime), [sysvector.OUT0_Out0(imintime:imaxtime), sysvector.OUT0_Out1(imintime:imaxtime), sysvector.OUT0_Out2(imintime:imaxtime), sysvector.OUT0_Out3(imintime:imaxtime), sysvector.OUT0_Out4(imintime:imaxtime), sysvector.OUT0_Out5(imintime:imaxtime), sysvector.OUT0_Out6(imintime:imaxtime), sysvector.OUT0_Out7(imintime:imaxtime)]); + title(h.axes(8),'Actuator PWM (raw-)outputs [µs]'); + legend(h.axes(8),'CH1','CH2','CH3','CH4','CH5','CH6','CH7','CH8'); + set(h.axes(8), 'YLim',[800 2200]); + %Airspeeds + plot(h.axes(9),time(imintime:imaxtime), sysvector.AIRS_IndSpeed(imintime:imaxtime)); + hold on + plot(h.axes(9),time(imintime:imaxtime), sysvector.AIRS_TrueSpeed(imintime:imaxtime)); + hold off + %add GPS total airspeed here + title(h.axes(9),'Airspeed [m/s]'); + legend(h.axes(9),'Indicated Airspeed (IAS)','True Airspeed (TAS)','GPS Airspeed'); + %calculate time differences and plot them + intervals = zeros(0,imaxtime - imintime); + for k = imintime+1:imaxtime + intervals(k) = time(k) - time(k-1); + end + plot(h.axes(10), time(imintime:imaxtime), intervals); + + %Set same timescale for all plots + for i=2:NrAxes + set(h.axes(i),'XLim',[mintime maxtime]); + end + + set(h.labels.GUIState,'String','OK','BackgroundColor',[240/255 240/255 240/255]); +end + +%% ************************************************************************ +% DRAWCURRENTAIRCRAFTSTATE(nested function) +% ************************************************************************ +function DrawCurrentAircraftState() + %find current data index + i=find(time>=CurTime,1,'first'); + + %********************************************************************** + % Current aircraft state label update + %********************************************************************** + acstate{1,:}=[sprintf('%s \t\t','GPS Pos:'),'[lat=',num2str(double(sysvector.GPS_Lat(i))*fconv_gpslatlong),'°, ',... + 'lon=',num2str(double(sysvector.GPS_Lon(i))*fconv_gpslatlong),'°, ',... + 'alt=',num2str(double(sysvector.GPS_Alt(i))*fconv_gpsalt),'m]']; + acstate{2,:}=[sprintf('%s \t\t','Mags[gauss]'),'[x=',num2str(sysvector.IMU_MagX(i)),... + ', y=',num2str(sysvector.IMU_MagY(i)),... + ', z=',num2str(sysvector.IMU_MagZ(i)),']']; + acstate{3,:}=[sprintf('%s \t\t','Accels[m/s²]'),'[x=',num2str(sysvector.IMU_AccX(i)),... + ', y=',num2str(sysvector.IMU_AccY(i)),... + ', z=',num2str(sysvector.IMU_AccZ(i)),']']; + acstate{4,:}=[sprintf('%s \t\t','Gyros[rad/s]'),'[x=',num2str(sysvector.IMU_GyroX(i)),... + ', y=',num2str(sysvector.IMU_GyroY(i)),... + ', z=',num2str(sysvector.IMU_GyroZ(i)),']']; + acstate{5,:}=[sprintf('%s \t\t','Altitude[m]'),'[Barometric: ',num2str(sysvector.SENS_BaroAlt(i)),'m, GPS: ',num2str(double(sysvector.GPS_Alt(i))*fconv_gpsalt),'m]']; + acstate{6,:}=[sprintf('%s \t','Est. attitude[deg]:'),'[Roll=',num2str(sysvector.ATT_Roll(i).*180./3.14159),... + ', Pitch=',num2str(sysvector.ATT_Pitch(i).*180./3.14159),... + ', Yaw=',num2str(sysvector.ATT_Yaw(i).*180./3.14159),']']; + acstate{7,:}=sprintf('%s \t[','Actuator Ctrls [-]:'); + %for j=1:4 + acstate{7,:}=[acstate{7,:},num2str(sysvector.ATTC_Roll(i)),',']; + acstate{7,:}=[acstate{7,:},num2str(sysvector.ATTC_Pitch(i)),',']; + acstate{7,:}=[acstate{7,:},num2str(sysvector.ATTC_Yaw(i)),',']; + acstate{7,:}=[acstate{7,:},num2str(sysvector.ATTC_Thrust(i)),',']; + %end + acstate{7,:}=[acstate{7,:},']']; + acstate{8,:}=sprintf('%s \t[','Actuator Outputs [PWM/µs]:'); + %for j=1:8 + acstate{8,:}=[acstate{8,:},num2str(sysvector.OUT0_Out0(i)),',']; + acstate{8,:}=[acstate{8,:},num2str(sysvector.OUT0_Out1(i)),',']; + acstate{8,:}=[acstate{8,:},num2str(sysvector.OUT0_Out2(i)),',']; + acstate{8,:}=[acstate{8,:},num2str(sysvector.OUT0_Out3(i)),',']; + acstate{8,:}=[acstate{8,:},num2str(sysvector.OUT0_Out4(i)),',']; + acstate{8,:}=[acstate{8,:},num2str(sysvector.OUT0_Out5(i)),',']; + acstate{8,:}=[acstate{8,:},num2str(sysvector.OUT0_Out6(i)),',']; + acstate{8,:}=[acstate{8,:},num2str(sysvector.OUT0_Out7(i)),',']; + %end + acstate{8,:}=[acstate{8,:},']']; + acstate{9,:}=[sprintf('%s \t','Airspeed[m/s]:'),'[IAS: ',num2str(sysvector.AIRS_IndSpeed(i)),', TAS: ',num2str(sysvector.AIRS_TrueSpeed(i)),']']; + + set(h.edits.AircraftState,'String',acstate); + + %********************************************************************** + % GPS Plot Update + %********************************************************************** + %Plot traveled path, and and time. + figure(h.figures(2)); + hold on; + if(CurTime>mintime+1) %the +1 is only a small bugfix + h.pathline=plot3(h.axes(1),double(sysvector.GPS_Lat(imintime:i))*fconv_gpslatlong, ... + double(sysvector.GPS_Lon(imintime:i))*fconv_gpslatlong, ... + double(sysvector.GPS_Alt(imintime:i))*fconv_gpsalt,'b','LineWidth',2); + end; + hold off + %Plot current position + newpoint=[double(sysvector.GPS_Lat(i))*fconv_gpslatlong double(sysvector.GPS_Lat(i))*fconv_gpslatlong double(sysvector.GPS_Alt(i))*fconv_gpsalt]; + if(numel(h.pathpoints)<=3) %empty path + h.pathpoints(1,1:3)=newpoint; + else %Not empty, append new point + h.pathpoints(size(h.pathpoints,1)+1,:)=newpoint; + end + axes(h.axes(1)); + line(h.pathpoints(:,1),h.pathpoints(:,2),h.pathpoints(:,3),'LineStyle','none','Marker','.','MarkerEdge','black','MarkerSize',20); + + + % Plot current time (small label next to current gps position) + textdesc=strcat(' t=',num2str(time(i)),'s'); + if(isvalidhandle(h.markertext)) + delete(h.markertext); %delete old text + end + h.markertext=text(double(sysvector.GPS_Lat(i))*fconv_gpslatlong,double(sysvector.GPS_Lon(i))*fconv_gpslatlong,... + double(sysvector.GPS_Alt(i))*fconv_gpsalt,textdesc); + set(h.edits.CurTime,'String',CurTime); + + %********************************************************************** + % Plot the lines showing the current time in the 2-d plots + %********************************************************************** + for i=2:NrAxes + if(isvalidhandle(h.markerline(i))) delete(h.markerline(i)); end + ylims=get(h.axes(i),'YLim'); + h.markerline(i)=line([CurTime CurTime] ,get(h.axes(i),'YLim'),'Color','black'); + set(h.markerline(i),'parent',h.axes(i)); + end + + set(h.labels.GUIState,'String','OK','BackgroundColor',[240/255 240/255 240/255]); +end + +%% ************************************************************************ +% MINMAXTIME CALLBACK (nested function) +% ************************************************************************ +function minmaxtime_callback(hObj,event) %#ok + new_mintime=get(h.sliders.MinTime,'Value'); + new_maxtime=get(h.sliders.MaxTime,'Value'); + + %Safety checks: + bErr=false; + %1: mintime must be < maxtime + if((new_mintime>maxtime) || (new_maxtimeCurTime) + set(h.labels.GUIState,'String','Error: Mintime cannot be bigger than CurTime! CurTime set to new mintime.','BackgroundColor','red'); + mintime=new_mintime; + CurTime=mintime; + bErr=true; + end + %3: MaxTime must be >CurTime + if(new_maxtime + %find current time + if(hObj==h.sliders.CurTime) + CurTime=get(h.sliders.CurTime,'Value'); + elseif (hObj==h.edits.CurTime) + temp=str2num(get(h.edits.CurTime,'String')); + if(tempmintime) + CurTime=temp; + else + %Error + set(h.labels.GUIState,'String','Error: You tried to set an invalid current time! Previous value restored.','BackgroundColor','red'); + end + else + %Error + set(h.labels.GUIState,'String','Error: curtime_callback','BackgroundColor','red'); + end + + set(h.sliders.CurTime,'Value',CurTime); + set(h.edits.CurTime,'String',num2str(CurTime)); + + %Redraw time markers, but don't have to redraw the whole raw data + DrawCurrentAircraftState(); +end + +%% ************************************************************************ +% FINDMINMAXINDICES (nested function) +% ************************************************************************ +function [idxmin,idxmax] = FindMinMaxTimeIndices() + for i=1:size(sysvector.TIME_StartTime,1) + if time(i)>=mintime; idxmin=i; break; end + end + for i=1:size(sysvector.TIME_StartTime,1) + if maxtime==0; idxmax=size(sysvector.TIME_StartTime,1); break; end + if time(i)>=maxtime; idxmax=i; break; end + end + mintime=time(idxmin); + maxtime=time(idxmax); +end + +%% ************************************************************************ +% ISVALIDHANDLE (nested function) +% ************************************************************************ +function isvalid = isvalidhandle(handle) + if(exist(varname(handle))>0 && length(ishandle(handle))>0) + if(ishandle(handle)>0) + if(handle>0.0) + isvalid=true; + return; + end + end + end + isvalid=false; +end + +%% ************************************************************************ +% JUST SOME SMALL HELPER FUNCTIONS (nested function) +% ************************************************************************ +function out = varname(var) + out = inputname(1); +end + +%This is the end of the matlab file / the main function +end diff --git a/Tools/sdlog2_dump.py b/Tools/sdlog2/sdlog2_dump.py similarity index 100% rename from Tools/sdlog2_dump.py rename to Tools/sdlog2/sdlog2_dump.py From a8b3381ca9675890b0d36e98a4453ac18d19df3e Mon Sep 17 00:00:00 2001 From: marco Date: Mon, 17 Feb 2014 21:29:14 +0100 Subject: [PATCH 21/23] BL-Ctrl 3.0 fix --- src/drivers/mkblctrl/mkblctrl.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/drivers/mkblctrl/mkblctrl.cpp b/src/drivers/mkblctrl/mkblctrl.cpp index ec5f77d7478..705e98eea48 100644 --- a/src/drivers/mkblctrl/mkblctrl.cpp +++ b/src/drivers/mkblctrl/mkblctrl.cpp @@ -705,7 +705,7 @@ MK::mk_check_for_blctrl(unsigned int count, bool showOutput, bool initI2C) Motor[i].State |= MOTOR_STATE_PRESENT_MASK; // set present bit; foundMotorCount++; - if (Motor[i].MaxPWM == 250) { + if ((Motor[i].MaxPWM & 252) == 248) { Motor[i].Version = BLCTRL_NEW; } else { From 4cfaf928e12ac8927b3980506f31c05a1ab899ce Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Tue, 18 Feb 2014 16:11:46 +0400 Subject: [PATCH 22/23] px4io: bug in failsafe fixed --- src/modules/px4iofirmware/mixer.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/px4iofirmware/mixer.cpp b/src/modules/px4iofirmware/mixer.cpp index f39fcf7ec6b..6a44294619c 100644 --- a/src/modules/px4iofirmware/mixer.cpp +++ b/src/modules/px4iofirmware/mixer.cpp @@ -179,7 +179,7 @@ mixer_tick(void) ((r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED) /* and there is valid input via or mixer */ && (r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK) ) /* or direct PWM is set */ || (r_status_flags & PX4IO_P_STATUS_FLAGS_RAW_PWM) - /* or failsafe was set manually */ || (r_setup_arming & PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM) + /* or failsafe was set manually */ || ((r_setup_arming & PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM) && !(r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK)) ) ); From 84d9e3479f3d2bb29b22906be0dfe65d73b5dd0f Mon Sep 17 00:00:00 2001 From: Anton Babushkin Date: Tue, 18 Feb 2014 23:42:02 +0400 Subject: [PATCH 23/23] mc_pos_control: unused variables removed --- src/modules/mc_pos_control/mc_pos_control_main.cpp | 1 - 1 file changed, 1 deletion(-) diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp index b06655385a8..b4bac53d66b 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -730,7 +730,6 @@ MulticopterPositionControl::task_main() } else { /* run position & altitude controllers, calculate velocity setpoint */ math::Vector<3> pos_err; - float err_x, err_y; get_vector_to_next_waypoint_fast(_global_pos.lat, _global_pos.lon, _lat_sp, _lon_sp, &pos_err.data[0], &pos_err.data[1]); pos_err(2) = -(_alt_sp - alt);