diff options
author | Lorenz Meier <lm@inf.ethz.ch> | 2013-10-26 16:01:57 -0700 |
---|---|---|
committer | Lorenz Meier <lm@inf.ethz.ch> | 2013-10-26 16:01:57 -0700 |
commit | 59c04adada5a6b55d599cb11680ceafc7bf75614 (patch) | |
tree | 91e25b847be774f24a5ad6a0964cb08364264659 | |
parent | 2d4fc9eea1d693bafeeacc1e929d1906418ee3b0 (diff) | |
parent | 411eb1f4ef4a273bebb7a46ae095756f8619aa55 (diff) | |
download | px4-firmware-59c04adada5a6b55d599cb11680ceafc7bf75614.tar.gz px4-firmware-59c04adada5a6b55d599cb11680ceafc7bf75614.tar.bz2 px4-firmware-59c04adada5a6b55d599cb11680ceafc7bf75614.zip |
Merge pull request #475 from PX4/fw_autoland
Autolanding for fixed wing
-rw-r--r-- | Documentation/fixed_wing_control.odg | bin | 0 -> 12258 bytes | |||
-rw-r--r-- | Documentation/l1_control.odg | bin | 11753 -> 0 bytes | |||
-rw-r--r-- | src/lib/external_lgpl/tecs/tecs.h | 5 | ||||
-rw-r--r-- | src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp | 169 | ||||
-rw-r--r-- | src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c | 1 | ||||
-rw-r--r-- | src/modules/mavlink/waypoints.c | 37 |
6 files changed, 183 insertions, 29 deletions
diff --git a/Documentation/fixed_wing_control.odg b/Documentation/fixed_wing_control.odg Binary files differnew file mode 100644 index 000000000..0918edcac --- /dev/null +++ b/Documentation/fixed_wing_control.odg diff --git a/Documentation/l1_control.odg b/Documentation/l1_control.odg Binary files differdeleted file mode 100644 index 69910c677..000000000 --- a/Documentation/l1_control.odg +++ /dev/null diff --git a/src/lib/external_lgpl/tecs/tecs.h b/src/lib/external_lgpl/tecs/tecs.h index 2ae6b28bb..4a98c8e97 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 f09e44b39..ffa7915a7 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 @@ -197,6 +203,8 @@ private: float throttle_max; float throttle_cruise; + float throttle_land_max; + float loiter_hold_radius; } _parameters; /**< local copies of interesting parameters */ @@ -229,6 +237,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 */ @@ -327,7 +337,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; @@ -345,6 +356,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"); @@ -408,6 +420,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)); @@ -630,6 +644,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), @@ -639,11 +656,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); @@ -711,27 +729,87 @@ 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(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 */ + + + // 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 > -5.0f) + land_noreturn = true; + + } else { + + /* normal navigation */ + _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(); /* 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) + 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; + float airspeed_approach = (_parameters.airspeed_min + _parameters.airspeed_trim) / 2.0f; - _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _global_triplet.current.altitude, calculate_target_airspeed(_parameters.airspeed_min), + if (altitude_error > -4.0f) { + + /* 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, - true, flare_angle_rad, - _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, - math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max)); + false, flare_angle_rad, + 0.0f, _parameters.throttle_max, throttle_land, + 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); /* 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 (wp_distance < 60.0f && altitude_error > -20.0f) { + + /* 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, + math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max)); } 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), @@ -790,7 +868,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); @@ -801,7 +879,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; @@ -824,11 +902,53 @@ 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; } + } 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 **/ @@ -848,13 +968,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); @@ -867,7 +1002,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 bf4b3b32a..3bb872405 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 @@ -75,6 +75,7 @@ PARAM_DEFINE_FLOAT(FW_THR_MIN, 0.0f); PARAM_DEFINE_FLOAT(FW_THR_MAX, 1.0f); +PARAM_DEFINE_FLOAT(FW_THR_LND_MAX, 1.0f); PARAM_DEFINE_FLOAT(FW_T_CLMB_MAX, 5.0f); diff --git a/src/modules/mavlink/waypoints.c b/src/modules/mavlink/waypoints.c index ddad5f0df..7e4a2688f 100644 --- a/src/modules/mavlink/waypoints.c +++ b/src/modules/mavlink/waypoints.c @@ -398,7 +398,14 @@ void check_waypoints_reached(uint64_t now, const struct vehicle_global_position_ if (time_elapsed) { + /* 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; 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; |