From 71ac33596836519a341001bb48a8835b8af75cd3 Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Wed, 16 Oct 2013 21:43:11 +0200 Subject: [PATCH 1/5] Small improvements to autoland, ensure that throttle can be shut down close to touch down. Depends on accurate land WP altitude --- src/lib/external_lgpl/tecs/tecs.h | 5 +++ .../fw_pos_control_l1_main.cpp | 32 +++++++++++++++---- .../fw_pos_control_l1_params.c | 1 + 3 files changed, 31 insertions(+), 7 deletions(-) diff --git a/src/lib/external_lgpl/tecs/tecs.h b/src/lib/external_lgpl/tecs/tecs.h index 2ae6b28bb4..4a98c8e974 100644 --- a/src/lib/external_lgpl/tecs/tecs.h +++ b/src/lib/external_lgpl/tecs/tecs.h @@ -94,6 +94,11 @@ public: // Rate of change of velocity along X body axis in m/s^2 float get_VXdot(void) { return _vel_dot; } + + float get_speed_weight() { + return _spdWeight; + } + // log data on internal state of the controller. Called at 10Hz // void log_data(DataFlash_Class &dataflash, uint8_t msgid); diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index cd4a0d58ef..3697af225e 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -196,6 +196,8 @@ private: float throttle_max; float throttle_cruise; + float throttle_land_max; + float loiter_hold_radius; } _parameters; /**< local copies of interesting parameters */ @@ -227,6 +229,8 @@ private: param_t throttle_max; param_t throttle_cruise; + param_t throttle_land_max; + param_t loiter_hold_radius; } _parameter_handles; /**< handles for interesting parameters */ @@ -342,6 +346,7 @@ FixedwingPositionControl::FixedwingPositionControl() : _parameter_handles.throttle_min = param_find("FW_THR_MIN"); _parameter_handles.throttle_max = param_find("FW_THR_MAX"); _parameter_handles.throttle_cruise = param_find("FW_THR_CRUISE"); + _parameter_handles.throttle_land_max = param_find("FW_THR_LND_MAX"); _parameter_handles.time_const = param_find("FW_T_TIME_CONST"); _parameter_handles.min_sink_rate = param_find("FW_T_SINK_MIN"); @@ -404,6 +409,8 @@ FixedwingPositionControl::parameters_update() param_get(_parameter_handles.throttle_max, &(_parameters.throttle_max)); param_get(_parameter_handles.throttle_cruise, &(_parameters.throttle_cruise)); + param_get(_parameter_handles.throttle_land_max, &(_parameters.throttle_land_max)); + param_get(_parameter_handles.time_const, &(_parameters.time_const)); param_get(_parameter_handles.min_sink_rate, &(_parameters.min_sink_rate)); param_get(_parameter_handles.max_sink_rate, &(_parameters.max_sink_rate)); @@ -625,6 +632,9 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio _tecs.update_50hz(baro_altitude, _airspeed.indicated_airspeed_m_s, _R_nb, accel_body, accel_earth); float altitude_error = _global_triplet.current.altitude - _global_pos.alt; + /* no throttle limit as default */ + float throttle_max = 1.0f; + /* AUTONOMOUS FLIGHT */ // XXX this should only execute if auto AND safety off (actuators active), @@ -634,11 +644,12 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio /* get circle mode */ bool was_circle_mode = _l1_control.circle_mode(); + /* restore speed weight, in case changed intermittently (e.g. in landing handling) */ + _tecs.set_speed_weight(_parameters.speed_weight); + /* execute navigation once we have a setpoint */ if (_setpoint_valid) { - float altitude_error = _global_triplet.current.altitude - _global_pos.alt; - /* current waypoint (the one currently heading for) */ math::Vector2f next_wp(global_triplet.current.lat / 1e7f, global_triplet.current.lon / 1e7f); @@ -712,16 +723,23 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio /* apply minimum pitch (flare) and limit roll if close to touch down, altitude error is negative (going down) */ // XXX this could make a great param - if (altitude_error > -20.0f) { - float flare_angle_rad = math::radians(15.0f);//math::radians(global_triplet.current.param1) + if (altitude_error > -10.0f) { + + float flare_angle_rad = math::radians(10.0f);//math::radians(global_triplet.current.param1) + + /* set the speed weight to 0.0 to push the system to control altitude with pitch */ + _tecs.set_speed_weight(0.0f); _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _global_triplet.current.altitude, calculate_target_airspeed(_parameters.airspeed_min), _airspeed.indicated_airspeed_m_s, eas2tas, - true, flare_angle_rad, + false, flare_angle_rad, _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max)); + /* kill the throttle if param requests it */ + throttle_max = math::min(throttle_max, _parameters.throttle_land_max); + /* limit roll motion to prevent wings from touching the ground first */ _att_sp.roll_body = math::constrain(_att_sp.roll_body, math::radians(-20.0f), math::radians(20.0f)); @@ -785,7 +803,7 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio _loiter_hold = true; } - float altitude_error = _loiter_hold_alt - _global_pos.alt; + altitude_error = _loiter_hold_alt - _global_pos.alt; math::Vector2f loiter_hold_pos(_loiter_hold_lat, _loiter_hold_lon); @@ -862,7 +880,7 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio } _att_sp.pitch_body = _tecs.get_pitch_demand(); - _att_sp.thrust = _tecs.get_throttle_demand(); + _att_sp.thrust = math::min(_tecs.get_throttle_demand(), throttle_max); return setpoint; } diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c b/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c index d210ec7126..9b64cb047a 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c @@ -72,6 +72,7 @@ PARAM_DEFINE_FLOAT(FW_THR_MIN, 0.0f); PARAM_DEFINE_FLOAT(FW_THR_MAX, 1.0f); +PARAM_DEFINE_FLOAT(FW_THR_LND_MAX, 0.0f); PARAM_DEFINE_FLOAT(FW_T_CLMB_MAX, 5.0f); From 013579cffd70f46788a356043563340731beabae Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 17 Oct 2013 07:54:04 +0200 Subject: [PATCH 2/5] More improvements on landing --- .../fw_pos_control_l1_main.cpp | 72 ++++++++++++++++--- .../fw_pos_control_l1_params.c | 2 +- 2 files changed, 64 insertions(+), 10 deletions(-) diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index 3697af225e..348a17ba63 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -727,15 +727,17 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio if (altitude_error > -10.0f) { float flare_angle_rad = math::radians(10.0f);//math::radians(global_triplet.current.param1) + float land_pitch_min = math::radians(5.0f); + float throttle_land = _parameters.throttle_min + (_parameters.throttle_max - _parameters.throttle_min) * 0.1f; - /* set the speed weight to 0.0 to push the system to control altitude with pitch */ - _tecs.set_speed_weight(0.0f); + // /* set the speed weight to 0.0 to push the system to control altitude with pitch */ + // _tecs.set_speed_weight(0.0f); - _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _global_triplet.current.altitude, calculate_target_airspeed(_parameters.airspeed_min), + _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _global_triplet.current.altitude, _parameters.airspeed_min, _airspeed.indicated_airspeed_m_s, eas2tas, false, flare_angle_rad, - _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, - math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max)); + 0.0f, _parameters.throttle_max, throttle_land, + land_pitch_min, math::radians(_parameters.pitch_limit_max)); /* kill the throttle if param requests it */ throttle_max = math::min(throttle_max, _parameters.throttle_land_max); @@ -814,7 +816,7 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio _att_sp.yaw_body = _l1_control.nav_bearing(); /* climb with full throttle if the altitude error is bigger than 5 meters */ - bool climb_out = (altitude_error > 5); + bool climb_out = (altitude_error > 3); float min_pitch; @@ -842,6 +844,43 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio _att_sp.roll_reset_integral = true; } + } else if (0/* easy mode enabled */) { + + /** EASY FLIGHT **/ + + if (0/* switched from another mode to easy */) { + _seatbelt_hold_heading = _att.yaw; + } + + if (0/* easy on and manual control yaw non-zero */) { + _seatbelt_hold_heading = _att.yaw + _manual.yaw; + } + + /* climb out control */ + bool climb_out = false; + + /* user wants to climb out */ + if (_manual.pitch > 0.3f && _manual.throttle > 0.8f) { + climb_out = true; + } + + /* if in seatbelt mode, set airspeed based on manual control */ + + // XXX check if ground speed undershoot should be applied here + float seatbelt_airspeed = _parameters.airspeed_min + + (_parameters.airspeed_max - _parameters.airspeed_min) * + _manual.throttle; + + _l1_control.navigate_heading(_seatbelt_hold_heading, _att.yaw, ground_speed); + _att_sp.roll_body = _l1_control.nav_roll(); + _att_sp.yaw_body = _l1_control.nav_bearing(); + _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _global_pos.alt + _manual.pitch * 2.0f, + seatbelt_airspeed, + _airspeed.indicated_airspeed_m_s, eas2tas, + false, _parameters.pitch_limit_min, + _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, + _parameters.pitch_limit_min, _parameters.pitch_limit_max); + } else if (0/* seatbelt mode enabled */) { /** SEATBELT FLIGHT **/ @@ -861,13 +900,28 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio (_parameters.airspeed_max - _parameters.airspeed_min) * _manual.throttle; + /* user switched off throttle */ + if (_manual.throttle < 0.1f) { + throttle_max = 0.0f; + /* switch to pure pitch based altitude control, give up speed */ + _tecs.set_speed_weight(0.0f); + } + + /* climb out control */ + bool climb_out = false; + + /* user wants to climb out */ + if (_manual.pitch > 0.3f && _manual.throttle > 0.8f) { + climb_out = true; + } + _l1_control.navigate_heading(_seatbelt_hold_heading, _att.yaw, ground_speed); - _att_sp.roll_body = _l1_control.nav_roll(); - _att_sp.yaw_body = _l1_control.nav_bearing(); + _att_sp.roll_body = _manual.roll; + _att_sp.yaw_body = _manual.yaw; _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _global_pos.alt + _manual.pitch * 2.0f, seatbelt_airspeed, _airspeed.indicated_airspeed_m_s, eas2tas, - false, _parameters.pitch_limit_min, + climb_out, _parameters.pitch_limit_min, _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, _parameters.pitch_limit_min, _parameters.pitch_limit_max); diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c b/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c index 9b64cb047a..c39df9ae31 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c @@ -72,7 +72,7 @@ PARAM_DEFINE_FLOAT(FW_THR_MIN, 0.0f); PARAM_DEFINE_FLOAT(FW_THR_MAX, 1.0f); -PARAM_DEFINE_FLOAT(FW_THR_LND_MAX, 0.0f); +PARAM_DEFINE_FLOAT(FW_THR_LND_MAX, 1.0f); PARAM_DEFINE_FLOAT(FW_T_CLMB_MAX, 5.0f); From 95aba0d70eae6a9aba55d31f223b852df1f82dcb Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Thu, 17 Oct 2013 09:36:20 +0200 Subject: [PATCH 3/5] Almost perfect landing approach, needs touch-down fine tuning --- .../fw_pos_control_l1_main.cpp | 47 +++++++++++++++---- src/modules/mavlink/waypoints.c | 37 ++++++++++----- 2 files changed, 62 insertions(+), 22 deletions(-) diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index 348a17ba63..219c6ffa6b 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -635,6 +635,9 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio /* no throttle limit as default */ float throttle_max = 1.0f; + /* not in non-abort mode for landing yet */ + bool land_noreturn = false; + /* AUTONOMOUS FLIGHT */ // XXX this should only execute if auto AND safety off (actuators active), @@ -717,23 +720,39 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio } else if (global_triplet.current.nav_cmd == NAV_CMD_LAND) { - _l1_control.navigate_waypoints(prev_wp, next_wp, current_position, ground_speed); + /* switch to heading hold for the last meters, continue heading hold after */ + + float wp_distance = get_distance_to_next_waypoint(current_position.getX(), current_position.getY(), next_wp.getX(), next_wp.getY()); + + if (wp_distance < 5.0f || land_noreturn) { + + /* heading hold, along the line connecting this and the last waypoint */ + float target_bearing = get_bearing_to_next_waypoint(prev_wp.getX(), prev_wp.getY(), next_wp.getX(), next_wp.getY()); + _l1_control.navigate_heading(target_bearing, _att.yaw, ground_speed); + + if (altitude_error > -20.0f) + land_noreturn = true; + + } else { + + /* normal navigation */ + _l1_control.navigate_waypoints(prev_wp, next_wp, current_position, ground_speed); + } + _att_sp.roll_body = _l1_control.nav_roll(); _att_sp.yaw_body = _l1_control.nav_bearing(); /* apply minimum pitch (flare) and limit roll if close to touch down, altitude error is negative (going down) */ // XXX this could make a great param - if (altitude_error > -10.0f) { + float flare_angle_rad = math::radians(10.0f);//math::radians(global_triplet.current.param1) + float land_pitch_min = math::radians(5.0f); + float throttle_land = _parameters.throttle_min + (_parameters.throttle_max - _parameters.throttle_min) * 0.1f; + float airspeed_land = (_parameters.airspeed_min + _parameters.airspeed_trim) / 2.0f; - float flare_angle_rad = math::radians(10.0f);//math::radians(global_triplet.current.param1) - float land_pitch_min = math::radians(5.0f); - float throttle_land = _parameters.throttle_min + (_parameters.throttle_max - _parameters.throttle_min) * 0.1f; + if (altitude_error > -5.0f) { - // /* set the speed weight to 0.0 to push the system to control altitude with pitch */ - // _tecs.set_speed_weight(0.0f); - - _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _global_triplet.current.altitude, _parameters.airspeed_min, + _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _global_triplet.current.altitude, calculate_target_airspeed(_parameters.airspeed_min), _airspeed.indicated_airspeed_m_s, eas2tas, false, flare_angle_rad, 0.0f, _parameters.throttle_max, throttle_land, @@ -743,7 +762,15 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio throttle_max = math::min(throttle_max, _parameters.throttle_land_max); /* limit roll motion to prevent wings from touching the ground first */ - _att_sp.roll_body = math::constrain(_att_sp.roll_body, math::radians(-20.0f), math::radians(20.0f)); + _att_sp.roll_body = math::constrain(_att_sp.roll_body, math::radians(-10.0f), math::radians(10.0f)); + + } else if (altitude_error > -20.0f) { + + _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _global_triplet.current.altitude, calculate_target_airspeed(airspeed_land), + _airspeed.indicated_airspeed_m_s, eas2tas, + false, flare_angle_rad, + _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, + math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max)); } else { diff --git a/src/modules/mavlink/waypoints.c b/src/modules/mavlink/waypoints.c index ddad5f0df6..adaf814047 100644 --- a/src/modules/mavlink/waypoints.c +++ b/src/modules/mavlink/waypoints.c @@ -399,6 +399,13 @@ void check_waypoints_reached(uint64_t now, const struct vehicle_global_position_ if (time_elapsed) { if (cur_wp->autocontinue) { + + /* safeguard against invalid missions with last wp autocontinue on */ + if (wpm->current_active_wp_id == wpm->size - 1) { + /* stop handling missions here */ + cur_wp->autocontinue = false; + } + cur_wp->current = 0; float navigation_lat = -1.0f; @@ -419,13 +426,16 @@ void check_waypoints_reached(uint64_t now, const struct vehicle_global_position_ navigation_frame = MAV_FRAME_LOCAL_NED; } + /* guard against missions without final land waypoint */ /* only accept supported navigation waypoints, skip unknown ones */ do { + /* pick up the last valid navigation waypoint, this will be one we hold on to after the mission */ if (wpm->waypoints[wpm->current_active_wp_id].command == (int)MAV_CMD_NAV_WAYPOINT || wpm->waypoints[wpm->current_active_wp_id].command == (int)MAV_CMD_NAV_LOITER_TURNS || wpm->waypoints[wpm->current_active_wp_id].command == (int)MAV_CMD_NAV_LOITER_TIME || - wpm->waypoints[wpm->current_active_wp_id].command == (int)MAV_CMD_NAV_LOITER_UNLIM) { + wpm->waypoints[wpm->current_active_wp_id].command == (int)MAV_CMD_NAV_LOITER_UNLIM || + wpm->waypoints[wpm->current_active_wp_id].command == (int)MAV_CMD_NAV_TAKEOFF) { /* this is a navigation waypoint */ navigation_frame = cur_wp->frame; @@ -434,17 +444,20 @@ void check_waypoints_reached(uint64_t now, const struct vehicle_global_position_ navigation_alt = cur_wp->z; } - if (wpm->current_active_wp_id == wpm->size - 1 && wpm->size > 1) { - /* the last waypoint was reached, if auto continue is - * activated keep the system loitering there. - */ - cur_wp->command = MAV_CMD_NAV_LOITER_UNLIM; - cur_wp->param3 = 20.0f; // XXX magic number 20 m loiter radius - cur_wp->frame = navigation_frame; - cur_wp->x = navigation_lat; - cur_wp->y = navigation_lon; - cur_wp->z = navigation_alt; - cur_wp->autocontinue = false; + if (wpm->current_active_wp_id == wpm->size - 1) { + + /* if we're not landing at the last nav waypoint, we're falling back to loiter */ + if (wpm->waypoints[wpm->current_active_wp_id].command != (int)MAV_CMD_NAV_LAND) { + /* the last waypoint was reached, if auto continue is + * activated AND it is NOT a land waypoint, keep the system loitering there. + */ + cur_wp->command = MAV_CMD_NAV_LOITER_UNLIM; + cur_wp->param3 = 20.0f; // XXX magic number 20 m loiter radius + cur_wp->frame = navigation_frame; + cur_wp->x = navigation_lat; + cur_wp->y = navigation_lon; + cur_wp->z = navigation_alt; + } /* we risk an endless loop for missions without navigation waypoints, abort. */ break; From 5fafe45745238ae377ed748c6f561a6cbf02221a Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 18 Oct 2013 10:14:42 +0200 Subject: [PATCH 4/5] Updated docs --- Documentation/fixed_wing_control.odg | Bin 0 -> 12258 bytes Documentation/l1_control.odg | Bin 11753 -> 0 bytes 2 files changed, 0 insertions(+), 0 deletions(-) create mode 100644 Documentation/fixed_wing_control.odg delete mode 100644 Documentation/l1_control.odg diff --git a/Documentation/fixed_wing_control.odg b/Documentation/fixed_wing_control.odg new file mode 100644 index 0000000000000000000000000000000000000000..0918edcac86ac046cda417e534551bb91480c219 GIT binary patch literal 12258 zcmd6NcT`l%((jOigwLT_piGisGy;f0RY$lfJmw4y)-o54lV!y zaD8C|00281JE)VpJrrzj53_-QoooIQT&f4DJEwmT>N`H;&8AX)KT0CBK~V& z;=gK(e|Pl9KW;D^xUCeEwUd**I1uRS>dNaX$csQ)0r|zm#DL#hnf_F@g#1g--Wdt| zu4M@ULSfMBt~&zxdHI1qbgv`)r$Jro{u+`!66y$rJAKddKgIaf=2t8P0`ae)uk-Oo zy1s`lz{e*9{NwkJs2%NWVE@$PwJr|?VP_9^vax{uC8uDxjom+mxz_#O(_f>uLRwnF z{uTExgCfAkCkV6xT7sRxJT5j+SLPp)|Iv^C-dnMM>#Zdc?D}6V`1yd>I!p|Ipo{-9 zQ3aT!f6U73;ggp9K8O$`^m|`;EWu7tX#qZdK^}fS9)1BGegSbl0dXNQN#JkvBrPH0 ze>D)%5fBs?77-T`kp%u|{KEs((#FXKZpCBijQs9N`hkvssG6{d_MgcA(f(I6h&u!Z zb(9wTH|;PL|8x+!Xx^K4+OIO)Adg;e|Y`Zv+a02 z@g1FPAPnDAg|N8B=7Aub;Z9OaAR#88wB(=T_-E#S{Q4Kd_W}810m8<{{=T9Cei1wX z0N@C9x=y3x^&&KFH0!tkBJ$cckJ&xQ@MJhw&1SC*JM176ez7b?nF&yBL{XG|}89=EcK4)%*Yg z^isRFHYANVsnAN_2YTZjQaQ{3kIma)l6P~*=zDhb^r%fhg;#PAA#V2_2AWKYF zx%V9LTZ-?GQNBd0H~>VXR4*~j4+FebUO|ITiRujBM|bYU?fCd^lwlZ(qYl9at#)6$d4gF`F073RVH+n*ix(kWs&jSc3_#-b;9o54ZFDR2MtRfO=^q zvBhdt;jJ1px8f|`$cV`mH=^#-fLiy11wZ{gBDS(X^v_sd>V?WWcce2I*h+(E! z^ynL#q4)SRw+5X7&6O;OJ6yiiN!h>E#|q6jZnjxfcAfwYt7N6mHoWRZL-^?+#7$QO zrN6ihYE9y|^=gMoa$(J4o`Fpv9Frl4fzBbO2`A2Cw6=4jRdF>_w!>wGMQlRAethWo zwTp5$qrDnchp2ILTwK@HAXj_cBA0*8iF}(&3^q>P_=Ih>^A-J$RCT=Uw#{n&;106K zqqsT<^hB8dbnr|R{Vo}95bZ=O|FD@DH`d&@Q()0zVY2f9G}9HoA72E?b1bW$8({5( z_2v}VvEyqM?9T+UlKc$ATWV7Gz@J28dA*>PGv;r=;(Vr1uad%H(Z`Onki>y>Gj+Hh zYqs*SqiuANA@6W^A6O2Wn|PVdIZEQi?O@Se01am_S`Xn3%+-E;>H<-7e;}0@Y`Y$N z;*vcfH&85wrlOwkJmI#Qgx2Ka%$aWwK8BpARnvvy;G@nVru3V?fQFuc28hn{OHupE zwk%-Z93M2)#O(!OE4w^_-G1!bw?zJ)L*#a_1K;u_Dsx^!65!)-Ho-8v3RAcCEVRa-{hNHY;VbT(@Z}<5D_~9WvRSaFi^PR=ZbMBK!qT)nX zY_>TC>rSJ@l>4wZk9sLXb>^ccT|7Kby97}(*1Z)4fWdJSyBTrBf_F1dI0)|P_X>w( z&qCT>e{%6i{Q3a(MrJ8j-U*S(L>t3l76c_9=PM^L%}(Bn#X+6 z*17pT;cXe{r&+)Q6)fy7)rZT37yy7RGXU`S3+W92002S2ovy$8-0WbRrjwD2wd9_I z`b4R!e94wEWUS=^x)D@(nC`C$s8{J2dQ|Q~`>G#V_Qot3eWloo4w75~?qQuIGMq%2 zXocPGef}jU&Ly7)8eeCY>fz1{RH|$CbZo&JDyCrfA~TCDgNH{`sAuNW4*(TpJilo= z$|Fj=_U(j{Pe`{dsMW?*fT~qWWVt($=`IN-mUZ!2&a`H7@C9~?jD~~fVNOWm;>NPq z&KYsd(Vo!TN~c5zCA>EZtF*VJH;oNn7JqO?oE~%ED6GmW_n4@8!*L_MV9ZK$_T8~k zdOy=0&pM=b3tOk>Tu_TiL}OgZ+kM634h$-4>=59N zoK-L7x|_*M(;leG^)xSSzkay>9lnBaQYj_+050M zP~=9me8Vd?-q;J+*_{p|N@q20&Wucx9l27BpYthA+xx4&Iq;t`@_EA}zaHp?&?bV? zXtUq!8VVJq>7L;X9o4#9aj6b?!pJHg=0sJgozTM*IG_aT@}Bg$ z4fmeVeA-*IFH*PHNd)RSZ!%J?vMdU`HO-4D`y9g7q=-DU`>M9Y_0Eez);uzoOY$A* zBi{E(Sc>n?-&7-)XRJS)In#+03_x6~cG&Oly^H7^QS=&mqimtBG3_SK`KiCqnya-Z zOnX*^$c=V|H_~hOApsV#TmK;52_hf>mL3b(kP1QdF35%fx#rMro7fR|UY5oTMSR7) zq-8jFPszUA$S~W{Q{mBri4C^*>lQm2R&Z~EbjFV=aRJk#GOfPn;H;fj0}n4uvL%6YSt zjCOEl2D|L7ekLDV&IL-I2t8SD?i{TVM5YjVSOAZquZT))28Wo#xJpb_kV`*%GVTRO z4#@6Zrr-&prXF3s*hq2FS^Iw7GfQ`$)e*uzT=~cfMJ{d|nWvMN-{eJPJYF0OXiyA` zlQYnQB92nI;05_l8che3;}m&%j8{(Q zvnK=0u%}v*!LF~PTx_FK4czhXb9}O0B45k0dza-=u;l~rUajjDKXT!OQBa-*pO7&+ zCxcCwL5F6QOkVSYYv$&+x)&8bFe$P(IzGK>MjIvXLUL8=Us_7eO)+rk1t0q|vX;EmwKOWuCuQuOAVp>1GA0gxR zwwQgR4EFd%VSm$5*aeP3e@YrlED4uP3P6jf>}+o(r)3EL(^Kh;zCh9|eIoo9x<X%8`GJX5FX12An4Ybfo99r?yS=a6?Lvvvu;7|;?7GF0PAfu_>7I`QVA zgTebRX#f)5o%pXGC8^jSRx>Bok)cF9H}b2$yQnw^Lh)=!w2 zEBzGLx^{1Ih^z>OiyGC>Uj}OCk~ej$3~Tx-ys^()z?Palo(^utOt-jwrB}C&911&n zljIwZ`iYvmOV<5Lp;P&m4m{=RY-0BEY}?x@yq+ULk|nd&=aO;E}`EZy2~G9%p~(Bc`*b3dEP zt#7QYe?v)_*ih;SbvD_qJY~ftc@@SLJ(%Br{>eXrdOcWQv6r4&-30IC%jqy7{_^bY z(j;nDFPB&@!+Ri$fn~Lup4^_GTs`(Gp**PfQ|k3_tKsGSNk>SCL+WDugd&g^#y)RFWNd$9ag+K1@CK7jc~M#kR2Y&0OsjM{)1K{QS-q#DNHlx5ABWk9NJUJ|`V)l|9E}ls73R zpMP`ICG@QB%cZqDT~Zf3!IJQ#Lvt;c?o^gp9D4I>ZZH0O9H=>yA!l@5HC%j%ZlNBE zlJnFMoukk}ni6Od9Hfy;WO-i&7(s72xNtjkn;F{=&yvB9huNDaeQ!}TG`8aTySs|1 z#H>S#6od+X%2+4nkMR1zPvF?=vj-=C;@`cNS!$_Gm z)|ZtUOzMb+$8%5cCu6EB=q5)-^*>UQRy5ERPk0D2uh_x_vl>$COw}`DwnB?0Gkm+U z8=mWyvJ@}ag6XHpH|?5i64jjboYu%MCMgh)K&P1DKADJcU_;@4F{*6?Mi>4IO>wpr z8l3&e0f#W+!<8nm{F|@(TBqf$aD|INF>)#Y4XAFyqfv&u_6=94E#(F%LN801H=3+= z!2^Gs_Y%oNKxn)-ea0;VNq?R|^WGU9@*;~enfg&mxo=mUGrDc^!bpt$I$4^Ct)ObH zy6U@pH4ZSEEn$&V*-`5!jk19m5t4Pw4K)V&Z;qW{w1i9~utN^aI}!+n1?1 z=c+GX(m#5bWZ_Rqi!KQgxB&e(Zjq<3C1n3MVXnXnw`Q2f>X|Vo^?90wR-F0SOd$Ey^V#V z4Qg^NPCD|8%|BX~!Sa+p-l0IGsUG*m@Yn=!8;L;_KYib%`uX%avJ*l?VIXD85w}X_ zNP^LthNiKV1X?< zETr}>k+7N^4-F37BAWfEmUc?^X52;~_Lm)rN=!?1ZNsm`VN66?59pS}dKl3iy3&i% zr9xHN3lV5GA>wjh9T@nn>5Y5%>)x}gZoEXPOC%%q?{mSXkQs{d=f1a-%i4)<5ok#? z8K8Fu74KZ(4lWHA(~Szaz~}pZ1N>$MCDz(0O@Rwmoi%kS8=!@89_0sO{br#b?|ZdI zDs8@d1Wjc!AA7*ALL6HiAeKBHyG9N=@@q9`w_5+lti-0Ad`rW zuD2vwtOqen;#C-d+T;!ssEbhUjxitjjj~dAs+ny)$wJAJwx~T6D$44>PjVHQU$j61cE&)oWEv>hF)&_DGK5ju<%_nQ$BMA-f z(l&Nz-g$dTxtbzgd}kiVQIXaAWVKfYNu0amb?AM~ z=pk>d?Gp(B0I9?ujQ+=AkLP30F;)x!;QFEj0027H&UO}Xuno)+==9^3*B)*a@<3ey zAD0>z007`CDavU90Dvk000n@J3IG6}+)WP$0MIv-qcziF!)Dc(hyo_DfH(ZTgo*Ln!5ZFL4QcMOqrvKr7a@bcLFA%x}1BBG@wl zrD31-JlN&mQJIBIsnOFfHXU5x9DV4voUfCYFEnV0qA>9#+#5M&H)x5PXcS&TE(ssG zdWoHEed#$dp7Yli9@qh`F2-sAa|_qMmCa-CmY0`L9=CX3UG6Ur9xfUZmOhD+YKHF?I z$h92wZG1k}bl#Y2wrZ;+YP`EIx1{uB(aHYUi{LcrzSq0+-8c~t!P6#Wr8FDj{3OJ` z3i)0d^JI4pt@8S&a#XK+bMer;!=HYuH`0jTyKxf6wHS~YWoO!$YILcxR@1C2Bryx4 z7$1>N71M59*K%0D@Vd0>#m%d^EpsK}Daw;3-Dq&~w9h96}wCTqCIRr(~&dsD=I@2$=}8AmQ3leFrG8YbgAeBpo_8RafO;NV7?VawwZK&w4`)jU9Lp-q2FJRy?;4$0?-2hfO`n|BO9wf zPwpHAfHrnuE2tw-*2c*WZ0`vCdt>U~7~3Q4o$c+QaOeM_of859vj@YWu>Yam^|_q0 zqXiiGU+idq7bg;7g@ii(Uo5ZRJpZ1a>q!5*zu#G1r{t&A?+yUjf8z@bfk0t@9D4#G z&Pe2+d)jE z<`L;Qm3!P>?@qPc=+bsFCNpKg31w@d8zO<0XUO?xb}crxnH^+OoZURbJQE1p^f-uq zY56V==DwKNtB#5e8dTVQ6&19+L(5g#+&q}zs3520-t6QbvMtUM<)*rSIC61valG>B z;`FOVsoV3RU~UUKU8-JI&M$E>Bfh=vb$o3(#vxDNIw~FoW{ibNz~lCZ%c0ay#$~*#+pZZL`&r@n!BzK&p6l6rHd(-Q*K&Oe|C04o!5ZR(Bowyy&sD5cm)LzfK=(1 z3;)SZKv$`4#BBcIhamUTB;2po;esvMff?Rf?P-Z*qN_wxg%)9s;^~582ke)+M9?_y z4cT@^Yg-*b0++-CniuH$Ma`eZ9&x`V+N@S_<*|<$>9H$2bk)HYb(htu#pBd3stXZa zjJ|5AJsO*R@+ClETsbqKQMJT`eLf6d5_h-F^7Iw+p=X|sFUEM7bKD4%b3IWB3-Cq} zUMBxMW~gC|MNJ^NE?uuOy~D*uj`2v*gN%-z`9QfLWg@&9j-j*yd_Iy`uep&x0lN0Z z#5>5bU=o6whe_RXokq^g;~JWwE6I+2hDt_n7?3FWjT~<|5g&_gmTZWZRdeF_aaq0> zQ3RR^-oAfdDB?-kWQok8U%Yu!5TBknqPOcL#7b1e|JDsWx?&{)e!}O44{8ZA(*;o3 z{g^e>&h6qjR}~moUy*A?@xLnNY7ETy=);6vq~*gqdpT6YlFf~Gc0rd{wnUsl{jiRQ zwU~il73WP%XS*O*>fr{N%=DTbJTG2GlI2Qra&HR#wJkZ z$jdF`;n2&=@#zz@ra_+wRjdtjG1Uw*n1=i`KM z?E1Ed`V>)|DYx+$1$z?Rd2pmT7NGl}$BA__xyVA#_N_hHD2X$SKY;ip@~#Zh+1uxi z+{B!Gi%w2Dsy#z&OcPjP*o%1PF*fBh(0Z-|)w4H$20=Dp8wfkn7tH}+WVfW>Jc zF{7>u&-B&#{>1ovQILUj-m;~T0$%Bzk*&iU5}lht;}^9j1TH(%AY+HGsiZDy+^EDNu|Fx8pcmXDdX*e*U#HCbXZ}zvSxwz?8Rh-AR^VK3;A6HO*f8KiB@*yiptRR^gQ@m#td!{I)@%iJo)#?>; zM#PD@X2se%A|H^c#|H|{zFNtq)ubdQ6DdT3<3+pzRS((~w4YNIbB}Q`C@0WqrOhNK zO@DQ->L$e@=kh}ZO_;JL7p9u^ZC{;sdtY5lK@N=YD5@WLIPq*!gC||l%VypAn~ElM zYV5G-ur|UKmFK)66}KZocuQE(y}2ql@~jUWp($K*Tk~8r_ZVC=0N-y zx%e>tLZP`Li-?ZwXW-iDWmgc0xmIR>%AwE3KaepX@HG)0rRzG-ZU$3(oMwmXnWp3@ z?07gnl~L*=kE+qgU)^ocP63+V0Z2`8jwz{eP{EXCQl0O)a_<-F)NFfHCw`qdIH7j$ zy8p(55C)%MUQHiSZcEE&9(-ga_D0~g8*VbDcjwOLtT7#rX}HXNHk7F8$er^|YK74N*7L~$@VyYz-CT9(Nprq$9?V^y{MAkOq&r#(o& z)1-bwQ8_Eh@a2_8|6%(BmW;Jk#WlTMwaKOhZt1joEL^TM9_NZwOzc?uo-7Th5OZx& z2jta6d*(h<4s`YgR?|MFnReMxHO?y0P~oe0^}M$l6u{%vv)S#5YmE>PIL}efqq=JDclG#78+&%?R@bpU8L5u zilIH{18?1QlQphQa{^)&KrLO}~{p!{5xUj=ipIOglt0?nD` zTY)b>8FV^W#WxEAr8X@(2V`7lY@0$uiuw;-ShgKj53jltl4ZVs7G2Z|669k800x3T zK8t?$fkO@e0Mzd5+~rYHmj~K`;Wm$;j^FDAinZUu+Zphhj=7k&e2epNSU15V&FNx%Z!mfKQOes4Xj)LzX~d>$ za@TDkDVFE)>70%UB{qFXxm8^OrA%Zri_I$9!)0d|9~SSUzQole9(33wTzmT46G#T{ zzC)w)@#H6Q4)JQ$&uDd05-Z^aHh4n<52ntp@xD@FUKIETN%`IWN$J#p*Xm zKPlZmMM?P$%1?^-2g;8*d+qxNPrD|0KV|ecD1Srw{@Z-|YX>*$E@Q3#wUHWx7xL!$r%3aRCSyvyZU|?RW5dnw*AanpA?z)U0@P7c)gApMB literal 0 HcmV?d00001 diff --git a/Documentation/l1_control.odg b/Documentation/l1_control.odg deleted file mode 100644 index 69910c677459aae4664a08b1bbeb6aaed48721ad..0000000000000000000000000000000000000000 GIT binary patch literal 0 HcmV?d00001 literal 11753 zcmeIYcT`hb6E+?}5RhIK&`_n9&^yvadPjN)0YXV23B3u@K|llnkuJSS?;uK15Ru+N zdM_eH`fuUAzIyNLeSi1+{{CjIvsQA>%rmoRpG@|%_h_nMUL^+rZ~%Z%Jyp3hOzt)g z003}#;Q#;tN0=iN?coFkJ2^SPAYe2M0q1drTX7@6C>V+x;RJ90}SAg!z% z{uB2vlfuW#%g6pMS{@?q?9Vm5((A#{@df>=N4j814A@)49(p|gy?>GWbTAS*g+v^9w?tE zm=7et#b;>=wG!sz;uYcpSwV%kz@ox}d?FAo5o=x{A*(-K|Mc>Q*Z=&sQI{`13JrtM zeNPp_@)Da1f^dPOB^mgI7}B}SEh%k1=k=$Nm3>RAY)dy{HWS$Nkov*;lLe8}!}v&L z(N|#_9VzNxV}R9rK7~VehXb+Q8l_jnJ+9ER?M4lV6pORGmfT}AN59BjO?Tb$UORUR z`zpGhzL`aZ^;%CYK-)7^qHmziq14>%#l0ONQe|lJQ-GDRhagVMCR^%mhQ=-mfe-2Bx`U}C#un++Os67@%!yw)4j1ta>ms?RJtUCm>@ zeU4O|5`?^XibmDQ7ObL$zf|BRZ$Z~S>_=1U5ho^!DFc%QU1-%teKUw#6q26`Zcm7s z=Lrz50|mQ$+_C%;uFq}s++SMxMqbE3DJQY#_95>6$~~w0JKWT+ zso(T8{aAJUHeUA<-z45bF9#~KK9VMW=jP*$znAgCiB!Gs)j}%=NxVGpDb^%OS-Oe7 zl4Wx=YlulWW9=&VfXdyDZtrc^gN{5REy0MJ75An|xAE&DzTvTp+pLS!PC4 z7g~A2ZxFDu;B~yy51Uau_Fuc%ywRpxI>Bv|Rv1o^O~c2)VS42HB`h%7#$X($3|SAK zE&Upm!UaiTq&O-y9gx`{td_6(B<%jwf2;4`FCV8b-bTvTttKIbt^qiVOQJC?Gt!QZ-3qnna!1$MI>%j{%!V zLhcrMndtnis}0?@%ece4+jKG%2##W)JOkV0%z*h?5WTUaGVzG~llkir90hx_xpyd2 zZ!3v5MFai9E3ZiBUM*3z|CZmDbgOU}6kuD%a(pAvEPr)UO>j}m#XPg5*kEbwi5Xd4 z<Nm3sws@^)Zo5n|nHJcxBNq>?%!s)fa3NfblmUU=%H&t|m`9jmeIszP>_#@B z^>VnJp&2DFmLN{1WJ*?5Gk3=2$==rH1aX(Y*iWHJX^X4t_y|@c(?w+_J9n@T+QICPGgCpyjuD@t=G00;t(zFhLTJ34HdP9P`Vk@)l)sDdl<_s45L z;A3O5KGSWes*1lRzS!kisO{k_Q>!_Ob>%8R0p`aLebnNu?1@c#3k3|_#p@m(u^ zCZka`f#NA}ga@~)>9gIt+&oX!a{&ibA8O`Q+>%geVA-+?)`*4qrP~IjeO0Y<{zB?VAd(47v;ASpd z)Qiyw?%MG zsa;PEZ^tu9oW<@Fx8%mI1f3Jodda*-iLN^{*4a~48UvLsdN*_*nbxb|za1>>y|uZY zcPtj&MYo;jm9>^issP~E;R?B>!PlvuH$3pe~6_s<&vdpt3C{4PPC&@#z zdl7vub&-cUi7`S25tD~*1p(r^*TgXSv=EsVXOH^})#t-j9hh@o9?RX)&NsfUNboA- zddf3(Iz8ba4)e+?K~;k=^Y#o3-rAl32~q~(XVx}Nl%5pKcyqzlu>s};mrpG56RwJav|C;q(H!CZ#F6|1ph98jyekYr54+8&4N#>1k z=zaVnvzN{bZY3MT6g&P@C7vlueZc(C$7>yZ7<8wOdU0CrHO20XjXOei-8jJpn)bLB zLkg)SL+f-#CUOh1!rV-%p4W0gpWY2Tyfam(DQ*xefScnF^_<&|z>1La2rgzcmr%L! zytOeK$$R(=e(n_`7qTNzQO0aS?I!qe#;v>XA(~-NH7|=7tJWJ7t<^0+1_!U|Eg$O{ zJ)5Hkhp-a9DMxYKQa0P~58Wa${W=!>M&SM!wNSAg>jf=g!fRUud6%~!zE%o-JU*|f zqO!fkph$$h+MOvPb5}p_fx;P#QWK>`mwI+A93mRU!`mFoT|}Y)KEB|Um!L|7ZDZp? zO{$1Ulh)^k_NxA-m*E`CUAFQOUll0+jzeD#m-po*$PP7oK`pxh&@1tlf)s{kDb@lPe8KAm2SH=eZ38T>-HI8 z>Q2a6*TIQtF!9Z*vTw%3Ggld~9d&5(>v2dEZ9QWU>v^o6)DLt$V@U3Pe)1w*!_9N& zrR3Bq!!f4OYbFk_=Fsi}V|jSF2CKg9+~;aajYF56$!|x1a|b$aTC1Ac#e`%F;I%Bv z!I`b0%FqJAZA&c6whctQO+>CFWpBzGHHl5Hb`&o_T16sFB5l!EhnB)+aRanYkvuh5C)W_tpLt|gza4s z$-tF9;y2Wy%7Gsk8LZNnWH)SaNO~lc1Bo&EL?}n<+CD$f$NH8#_-x8ChE0h(C*n=_>pdv8 zsRo1UrW_Uouj|&D(sH1L*$M{kdKlG1A2#cRDJBKo8bIT35Obd?>(HEY_t+|%HH9(2zA)gy+WHq{PLPpSYlUjVoZ>_awD|^~l@^e9(J&(I%GS@oErJLS`D+pM`ikL3I z8%AFoYu&)A`=Ikk=kaLH`#S%dQ&h*@uV}ni2lVkmcZ63?y_ks(&`Eb3grW5Kg`<)N z=MHoVz>3!sl;w>3>9@xE1`bC*g^9wIYBss$0?Z&EBb~)jHE9eC^T7_I8UAQKxRoQ# z`yzL)UC3n2_L^ET(r)n6v{2-mZEm@!>AR$wdmZkA@U0EBq2vbd?U$;#kbvoXiA3nA zL#DpK>1!}1u9k%9b&LkggGXpPPwW|Eog3D^CIjGoFB>TNZnY|ZHVam4z_mMIHFCSY|~|eS91GRq*3mvGP(E>S{@i?d6c-n{#NKazVI=xt)SE z?SV&>kikjac<<1;z>;mqGc(7zd-sKJ9q(%&OZX?V`Q+>+_1CaA)ek%5t4$eYMbCeu ze_NCA&?%)SSNt9WXN^X*?D+l*mYuHm1GnZ|2U3aM-C5k1%y!;g-JyLGyfaokg!PIj zjcaxS?GZmMWIMdAuykktqf2K>v|-)1jA=qj>Yibz;mN%mK_65n4_`K~e}AXgE6vv= zy)c*3$^c!YC*5=EVi|8k-6RNO)ZBKu%b!QTIFPf2QgOl?U}E>90--sYSd!+-;1@Ao_VrltH%<(H(Z>%`)SR~)@=%P zJUuiWU3bVwv=&)%0nQn-EXUdRPcomy_~Na({0R?KO*fyYN}}E}$~@rrPGdv{<&rL`LQ9H>;Kx zL?hZb+^2u9k1Y}jxW;t$q#`9twkRx2U?Fh6s_H$cFmK?54)+_ zeIvK298)li@2S>XSN!mHTCB0>P*K`&lJ=|bMoFt|4eeArpvAA)ll)3J3FPw^wHylW zeBcn(169O8EGfoi$k%Y4iCI*uaNgWN-B_d5!nRAK=N@Fk!C%UwwR_S-k`>zeumf9h zvYZ%v_sdRH(qjOQJV7w0HC>d4s%Uu%_^v#;S$`Pr2f^e+jo6)|cWq1Wt_po2e*JMQ z8BccjQ4B0vG<-mMSujU6E@Fgd+ZKzM3dqPkO`;)Y^o6_)>p?HJKa{#Xt1XEB6Sf=Y zea@{!_2_Eao{T%W0g9X-+YKTB*|vcjOYCrd42J+Olb|pqe|v{?_RgzF&=Ux;yc(HS z1Y@fjzLllRhb7ALS60IEd6+G7_IDL|b=;I?>OlLZ9i<`9wk#H>7$2QnSg^<4{-y0<5j`Qw)4^z`%_Sod+m zRC|H(ZnSxX6t-YU-D&-P@i1SHaeem)_sC-@?ENUHpyUGd&2BVPj~Jki=BzSSAfMh4 zgf8(CF>AAvv0WG&Xusej_i@Nhl@6x-cC=sdLbjl^A)XwP>wl0D?c04QWh6Ht9uyZw z?x!!H;@g+A*9XZm9muFswI8-WAyJ25odPdf>1w|J3T9V5n;p@3tN zC(}=Er&#A`K=@Z6QIf(mN;THO%~f5DgPq;OVtztirw5IBIzUGcGOO7cpR|bE`VyE$ z(GZ9FzE^XS>sg&DUZ8=hEmVYvev5|B9?gBL0~L2}Ptg>qdwU>TTfC$)l(?ttMdjX; zqH=mfK+EO0=N?EUhxjr$1pHqlpG zDDA$E-}H%!sr@eN9%w=`Cq(Obw0H4Nv@CFdi*`!KR8TZU>Ht;uKI>Dbd^bxpM~Bu5 zmFm`q`+;Uc}ufCFoP}2cLbK@>I9s^B$^`VAGT4d4lX{m*J)v-;w=*kBMQ*?e zu3Nl*oTn8+%c)BXWv8j;o&fRFO#ad8=tdAo4ppq{Q}%V?OU%%H$3-c9qRhghB^Iw{ zw++-Vn_A(%mh`ew1C9OEMV^}92>(3mv%|irI>Y18I~N_BTP=qdbN@sFT|4IlC^@rzT;1|I#%qIOg(S$xi~t0#y(a=Q0G zQ;qU(LR6&22u=)TX}JzJ=7-o`dZ;JncpYecFzrs1FoI7gfsI`V+_oT@o{O0;OOg-! zCJWw`Qjsf_CLe;IqB4>XL0zdTu3x3@L^Z{C$0Q$WwaGgqMb-NOgI2-*N_!xA;jG&u zorDG!lkew`^G0owR>FJ|d2_rhf!0y0w?{(W&of7_KT7zA%JrLK_Wy4EDC_@MnK&i7 zc~#xHXf(?8@<>5hL0!H?)*|4q$DO|%HUNMC06-1_w}#pLd18a&J2V2}-~@(49sUpPF3)3JP?lii|FEO|U7SdS4HAm_ z|FOLM%l~_NE+hTl{(fh5nUbGYzdQJ0`He3y1Oj#V=4(lsm)Y>Sgzgbm`fjAc#0Q#2 zH!RDpxMH_{+^CaLiEF=gpPaQ|D%Ic}eF*I?rN^U2)tE-5aOqe!R7GbEz@%}D?7Z0!(-eQTvY=}91o;Dg1GflmlDis6glMO?|cw$?HSgQDCcW+}rV z?HD{=I+OT>d9#Ia^zOQ^q*%k&2qtgmP0FmVsvK@Q&}Wev@ek(4mkg~j*S$7durkhP zCemCzn0L!iY*U1~W$BA6ypOBxdqokZdf3(#_HJ2pi6={AebcmhGdS#FPJ{DVYanaq z2$9~CyO>R%t`W#@>2=jOFULKOqDEhfy-I}^*t>s7rx%3b$NPrEJSO(~7)ERqi)kXW z9UJ3QQJNflW=Gu-24zpZ)|4coy(`I7Q#$2GUF$EfJITOh2{_M%fwmR>Lt6h&8Wg2T*IXEU=2&ZADAR$>^l8vHn8CNdnqTW(+!TB+RFsI1(Lfj zAVRzFWd0%A>>5N|&8v}iC5Ai4+g&L%+s5=F@>kj_b$KtINKd-AG9}8lT2UrCTRf8p z1d$h%apFmNH2at{V_n?S3@H5IvpVJA>PCg-HuF{_e{jb7_=(Dy6P4-A6vUZ|YfA$1 z>{Ef2dqm`sgC#!84i#_GDPf!+LU0qzeUFpISQEGPosWDZxxIq@?A-oH6diKUmf1OD zivmpS?Zci4w3QhTjRncDUMExH<9@hf1rU!O`ivXx2iE>vIqzDURy_SO3!RhPDo~3z z-*~;MkeVRBT1If5I6<_8EOzesSoi0)$m|=V&R+AySls;7UXM$B6tf?uurmcMXyx0N zg^A8PnSWao)AZZ;zy>XfZ)5XFG4psD$0NMM-ll@$XBE-z70tL6Ok|s=P1aM3m4K<`bHq-XDW3*u=fXGsa{LpEf2^$oqc8GLNOv!>7qbv zB1+aQsN7Qxx(XiJmS%Pd28E1JZ@0Vt2 zY2TD1Z4J&0E=|s7KgPWqwv;^x!KYfME(NoK?t_TS$Uk?xNgt@;D$xe-pFiYJ+zr)u zY*|&z^q$DnenI@&ErLZzmW4!>3OvQpDy>}6vI%~5rfD6eW=eU(^*T#--!m~w!$$`SMlr>izSt+pNk zAn6l)*;neq467eH$!rTN=pfngM>0p2#PLU2v}fxkOiA5j%pbh6as8^PO;j7_-C!~w z`dnO1T^r(7Xf^|>B)V?}ucqlulU}_~R)iCFC!ScWTnY7~1~L9FTRncog*Kd_TB-KE zEE0M>uPOJ7;}i3VK$O|s!i(znF#j87{x40}4Yo*XG>T6ek8bPloC%pbFQv#e1Ych^ zIF9`cjM&4c&FpAk+Vx*kC<;FlrMR7)qz^ECJEXG>z!Ge43l#%hXWXWnsyc4uCj0JCvZWKo(%>AGC_9%=Ag;@^*5}_UdWJ{!)*1mfGM4Zvqf}} zI{MMKH3QOag80`BOH`55Ihq5g_RO;)z^zIQsTza;K`FM8lK**WuHfML4)SIG-m$yR z`;C&;)cy~elXbS#itef+T#J(f;cIceaYCOih((F(>~bX4S8|bW1rGMqZ*JC)t7ya= zTA%nnOW;U#NE;ZKcVqD~YsSEk6lWh&R%5&Aa9`RKE$7DhwNSTa+sid^X?kxF+oMCF z+>6KoKEgPg-Y**1@<~3mxA(pkh4wdE5W#skNo`Aw#F#V4mnsR{#z@3~Q@W)CmgKsZS>LGX-q?c>WksjVt+&0soGj@z zwjZlPWad(5wAxe?V<)m-k-+Ouc#x0$FgSQ2sg}l!jp}~s)g(9Qwndr9< zEg$LKaMNG)`r0=T#o>BX*KM|}qq@LZ(t_zhq|$N$?4LZRx?Ck)>C)b)z3d$1|FKH? z-Fk-t005{d=*n;@t1I$2g5fZ0DC+y!aj{M-{4E`R(;)}LmVa?BF7qb1zd8K`p!k9u z7av1M&`+8)-!Ug&Cu`pta?)fYIk3_DQJ*FBy}QF0haZ?C{UD_^1DX~Ref)USExChD zK$0moA)OtSSOV)t%CGA2DQ6;Mm|&}z7RxTK51IT9x|3F)b6s^9gX>Iv^Mqt@e|2tj zIUE}oV->4boy4q@6kmxffZ>1QqX-Z*(t?q(Y<%NPwQn-+VE}I`2JXJ2*lMqFH$Xlo1REyk{+Z^K$mmO{>&}9=_c}pqpER6 zCH8gZeXSMR`I+Ri==Iiy`!`u|C1^?#a5&C#``%=23D1oHr+hFklUe{+>?FMc08C?E zCiMyiIpCk$Gyf_-nl=A!_4gjkpQ3fy;`wJs<`1hMQ}}B;z-8mqPwBt<*EXuZTmHJ` zciArRQz-HN(me3P@Sk=Be~JU;KkpCz9p$Ih9~|tb+`aYRl;gh`*H* z-+=wf(Ecq>)o*ZqWo-WzXY)5WzcRRg#<}EhKZW`Be<$g$jP9S2E(y_3G5-zHPloqz zQR07t@{{rXf%4;>z4ZNqwq0_*pVIvslz(7=|LcAF>i`*m|DCFz{O_-!{aPz88Q@Pb s1pdkge|Y~fq+iRyWhMP7_gMd-u4<}aVPC2d1Bd}aR{?+&wo9Y`4}A~OqW}N^ From 40610c7d484d077dc10726628c3adbe01edd91df Mon Sep 17 00:00:00 2001 From: Lorenz Meier Date: Fri, 18 Oct 2013 10:38:51 +0200 Subject: [PATCH 5/5] Fixes, but approach needs proper design --- .../fw_pos_control_l1_main.cpp | 66 ++++++++++++++----- src/modules/mavlink/waypoints.c | 12 ++-- 2 files changed, 57 insertions(+), 21 deletions(-) diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp index 219c6ffa6b..9ed74f0caf 100644 --- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp +++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp @@ -158,6 +158,12 @@ private: float _launch_alt; bool _launch_valid; + /* land states */ + /* not in non-abort mode for landing yet */ + bool land_noreturn; + /* heading hold */ + float target_bearing; + /* throttle and airspeed states */ float _airspeed_error; ///< airspeed error to setpoint in m/s bool _airspeed_valid; ///< flag if a valid airspeed estimate exists @@ -329,7 +335,8 @@ FixedwingPositionControl::FixedwingPositionControl() : _airspeed_error(0.0f), _airspeed_valid(false), _groundspeed_undershoot(0.0f), - _global_pos_valid(false) + _global_pos_valid(false), + land_noreturn(false) { _nav_capabilities.turn_distance = 0.0f; @@ -635,9 +642,6 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio /* no throttle limit as default */ float throttle_max = 1.0f; - /* not in non-abort mode for landing yet */ - bool land_noreturn = false; - /* AUTONOMOUS FLIGHT */ // XXX this should only execute if auto AND safety off (actuators active), @@ -722,15 +726,26 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio /* switch to heading hold for the last meters, continue heading hold after */ - float wp_distance = get_distance_to_next_waypoint(current_position.getX(), current_position.getY(), next_wp.getX(), next_wp.getY()); - - if (wp_distance < 5.0f || land_noreturn) { + float wp_distance = get_distance_to_next_waypoint(prev_wp.getX(), prev_wp.getY(), current_position.getX(), current_position.getY()); + //warnx("wp dist: %d, alt err: %d, noret: %s", (int)wp_distance, (int)altitude_error, (land_noreturn) ? "YES" : "NO"); + if (wp_distance < 15.0f || land_noreturn) { /* heading hold, along the line connecting this and the last waypoint */ - float target_bearing = get_bearing_to_next_waypoint(prev_wp.getX(), prev_wp.getY(), next_wp.getX(), next_wp.getY()); + + + // if (global_triplet.previous_valid) { + // target_bearing = get_bearing_to_next_waypoint(prev_wp.getX(), prev_wp.getY(), next_wp.getX(), next_wp.getY()); + // } else { + + if (!land_noreturn) + target_bearing = _att.yaw; + //} + + warnx("NORET: %d, target_bearing: %d, yaw: %d", (int)land_noreturn, (int)math::degrees(target_bearing), (int)math::degrees(_att.yaw)); + _l1_control.navigate_heading(target_bearing, _att.yaw, ground_speed); - if (altitude_error > -20.0f) + if (altitude_error > -5.0f) land_noreturn = true; } else { @@ -739,6 +754,12 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio _l1_control.navigate_waypoints(prev_wp, next_wp, current_position, ground_speed); } + /* do not go down too early */ + if (wp_distance > 50.0f) { + altitude_error = (_global_triplet.current.altitude + 25.0f) - _global_pos.alt; + } + + _att_sp.roll_body = _l1_control.nav_roll(); _att_sp.yaw_body = _l1_control.nav_bearing(); @@ -748,15 +769,21 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio float flare_angle_rad = math::radians(10.0f);//math::radians(global_triplet.current.param1) float land_pitch_min = math::radians(5.0f); float throttle_land = _parameters.throttle_min + (_parameters.throttle_max - _parameters.throttle_min) * 0.1f; - float airspeed_land = (_parameters.airspeed_min + _parameters.airspeed_trim) / 2.0f; + float airspeed_land = _parameters.airspeed_min; + float airspeed_approach = (_parameters.airspeed_min + _parameters.airspeed_trim) / 2.0f; - if (altitude_error > -5.0f) { + if (altitude_error > -4.0f) { - _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _global_triplet.current.altitude, calculate_target_airspeed(_parameters.airspeed_min), + /* land with minimal speed */ + + /* force TECS to only control speed with pitch, altitude is only implicitely controlled now */ + _tecs.set_speed_weight(2.0f); + + _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _global_triplet.current.altitude, calculate_target_airspeed(airspeed_land), _airspeed.indicated_airspeed_m_s, eas2tas, false, flare_angle_rad, 0.0f, _parameters.throttle_max, throttle_land, - land_pitch_min, math::radians(_parameters.pitch_limit_max)); + math::radians(-10.0f), math::radians(15.0f)); /* kill the throttle if param requests it */ throttle_max = math::min(throttle_max, _parameters.throttle_land_max); @@ -764,9 +791,11 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio /* limit roll motion to prevent wings from touching the ground first */ _att_sp.roll_body = math::constrain(_att_sp.roll_body, math::radians(-10.0f), math::radians(10.0f)); - } else if (altitude_error > -20.0f) { + } else if (wp_distance < 60.0f && altitude_error > -20.0f) { - _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _global_triplet.current.altitude, calculate_target_airspeed(airspeed_land), + /* minimize speed to approach speed */ + + _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _global_triplet.current.altitude, calculate_target_airspeed(airspeed_approach), _airspeed.indicated_airspeed_m_s, eas2tas, false, flare_angle_rad, _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, @@ -774,6 +803,8 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio } else { + /* normal cruise speed */ + _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _global_triplet.current.altitude, calculate_target_airspeed(_parameters.airspeed_trim), _airspeed.indicated_airspeed_m_s, eas2tas, false, math::radians(_parameters.pitch_limit_min), @@ -866,6 +897,11 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio } } + /* reset land state */ + if (global_triplet.current.nav_cmd != NAV_CMD_LAND) { + land_noreturn = false; + } + if (was_circle_mode && !_l1_control.circle_mode()) { /* just kicked out of loiter, reset roll integrals */ _att_sp.roll_reset_integral = true; diff --git a/src/modules/mavlink/waypoints.c b/src/modules/mavlink/waypoints.c index adaf814047..7e4a2688f6 100644 --- a/src/modules/mavlink/waypoints.c +++ b/src/modules/mavlink/waypoints.c @@ -398,13 +398,13 @@ void check_waypoints_reached(uint64_t now, const struct vehicle_global_position_ if (time_elapsed) { - if (cur_wp->autocontinue) { + /* safeguard against invalid missions with last wp autocontinue on */ + if (wpm->current_active_wp_id == wpm->size - 1) { + /* stop handling missions here */ + cur_wp->autocontinue = false; + } - /* safeguard against invalid missions with last wp autocontinue on */ - if (wpm->current_active_wp_id == wpm->size - 1) { - /* stop handling missions here */ - cur_wp->autocontinue = false; - } + if (cur_wp->autocontinue) { cur_wp->current = 0;