From e539a89e225c1d6ab127d7953bceec7efecefbdf Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Fri, 1 Nov 2013 17:35:09 +0100 Subject: autoland slope parameters, variable rename for bugfix and code clarity, printfs --- .../fw_pos_control_l1/fw_pos_control_l1_main.cpp | 41 ++++++++++++++++------ 1 file changed, 30 insertions(+), 11 deletions(-) (limited to 'src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp') 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 a90899528..a4b277888 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 @@ -206,6 +206,9 @@ private: float throttle_land_max; float loiter_hold_radius; + + float land_slope_angle; + float land_slope_length; } _parameters; /**< local copies of interesting parameters */ struct { @@ -240,6 +243,9 @@ private: param_t throttle_land_max; param_t loiter_hold_radius; + + param_t land_slope_angle; + param_t land_slope_length; } _parameter_handles; /**< handles for interesting parameters */ @@ -363,6 +369,9 @@ FixedwingPositionControl::FixedwingPositionControl() : _parameter_handles.throttle_cruise = param_find("FW_THR_CRUISE"); _parameter_handles.throttle_land_max = param_find("FW_THR_LND_MAX"); + _parameter_handles.land_slope_angle = param_find("FW_LND_ANG"); + _parameter_handles.land_slope_length = param_find("FW_LND_SLL"); + _parameter_handles.time_const = param_find("FW_T_TIME_CONST"); _parameter_handles.min_sink_rate = param_find("FW_T_SINK_MIN"); _parameter_handles.max_sink_rate = param_find("FW_T_SINK_MAX"); @@ -440,6 +449,11 @@ FixedwingPositionControl::parameters_update() param_get(_parameter_handles.pitch_damping, &(_parameters.pitch_damping)); param_get(_parameter_handles.max_climb_rate, &(_parameters.max_climb_rate)); + + param_get(_parameter_handles.land_slope_angle, &(_parameters.land_slope_angle)); + param_get(_parameter_handles.land_slope_length, &(_parameters.land_slope_length)); + + _l1_control.set_l1_damping(_parameters.l1_damping); _l1_control.set_l1_period(_parameters.l1_period); _l1_control.set_l1_roll_limit(math::radians(_parameters.roll_limit)); @@ -673,7 +687,7 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio if (_setpoint_valid) { /* current waypoint (the one currently heading for) */ - math::Vector2f next_wp(global_triplet.current.lat / 1e7f, global_triplet.current.lon / 1e7f); + math::Vector2f curr_wp(global_triplet.current.lat / 1e7f, global_triplet.current.lon / 1e7f); /* previous waypoint */ math::Vector2f prev_wp; @@ -711,7 +725,7 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio } else if (global_triplet.current.nav_cmd == NAV_CMD_WAYPOINT) { /* waypoint is a plain navigation waypoint */ - _l1_control.navigate_waypoints(prev_wp, next_wp, current_position, ground_speed); + _l1_control.navigate_waypoints(prev_wp, curr_wp, current_position, ground_speed); _att_sp.roll_body = _l1_control.nav_roll(); _att_sp.yaw_body = _l1_control.nav_bearing(); @@ -726,7 +740,7 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio global_triplet.current.nav_cmd == NAV_CMD_LOITER_UNLIMITED) { /* waypoint is a loiter waypoint */ - _l1_control.navigate_loiter(next_wp, current_position, global_triplet.current.loiter_radius, + _l1_control.navigate_loiter(curr_wp, current_position, global_triplet.current.loiter_radius, global_triplet.current.loiter_direction, ground_speed); _att_sp.roll_body = _l1_control.nav_roll(); _att_sp.yaw_body = _l1_control.nav_bearing(); @@ -741,7 +755,7 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio /* Horizontal landing control */ /* 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()); + float wp_distance = get_distance_to_next_waypoint(current_position.getX(), current_position.getY(), curr_wp.getX(), curr_wp.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) { @@ -766,7 +780,7 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio } else { /* normal navigation */ - _l1_control.navigate_waypoints(prev_wp, next_wp, current_position, ground_speed); + _l1_control.navigate_waypoints(prev_wp, curr_wp, current_position, ground_speed); } _att_sp.roll_body = _l1_control.nav_roll(); _att_sp.yaw_body = _l1_control.nav_bearing(); @@ -787,9 +801,9 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio float airspeed_land = _parameters.airspeed_min; float airspeed_approach = (_parameters.airspeed_min + _parameters.airspeed_trim) / 2.0f; - const float landing_slope_angle_rad = 20.0f/180.0f*M_PI_F; //xxx: param - const float landingslope_length = 64.0f; - const float L_wp_distance = cosf(landing_slope_angle_rad) * landingslope_length; + float landing_slope_angle_rad = math::radians(_parameters.land_slope_angle); + float landingslope_length = _parameters.land_slope_length; + float L_wp_distance = cosf(landing_slope_angle_rad) * landingslope_length; float L_altitude = getLandingSlopeAbsoluteAltitude(L_wp_distance, _global_triplet.current.altitude, landing_slope_angle_rad); float landing_slope_alt_desired = getLandingSlopeAbsoluteAltitude(wp_distance, _global_triplet.current.altitude, landing_slope_angle_rad); @@ -812,15 +826,17 @@ 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)); + warnx("Landing: land with minimal speed"); + } else if (wp_distance < L_wp_distance) { - /* minimize speed to approach speed, stay on glide slope */ + /* minimize speed to approach speed, stay on landing slope */ _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, landing_slope_alt_desired, 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)); - + warnx("Landing: after L, stay on landing slope, alt_desired: %.4f (wp_distance: %.4f)", landing_slope_alt_desired, wp_distance); } else { /* normal cruise speed @@ -832,12 +848,15 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio if (_global_pos.alt > landing_slope_alt_desired - 10.0f) { /* stay on slope */ altitude_desired = landing_slope_alt_desired; + warnx("Landing: before L, stay on landing slope, alt_desired: %.4f (wp_distance: %.4f)", altitude_desired, wp_distance); } else if (_global_pos.alt < landing_slope_alt_desired - 10.0f && _global_pos.alt > L_altitude) { /* continue horizontally */ altitude_desired = _global_pos.alt; //xxx: dangerous, but we have the altitude < L_altitude protection + warnx("Landing: before L,continue horizontal at: %.4f, (landing_slope_alt_desired %.4f, wp_distance: %.4f, L_altitude: %.4f)", altitude_desired, landing_slope_alt_desired, wp_distance, L_altitude); } else { /* climb to L_altitude */ altitude_desired = L_altitude; + warnx("Landing: before L, below L, climb: %.4f (wp_distance: %.4f)", altitude_desired, wp_distance); } _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, altitude_desired, calculate_target_airspeed(_parameters.airspeed_trim), @@ -849,7 +868,7 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio } else if (global_triplet.current.nav_cmd == NAV_CMD_TAKEOFF) { - _l1_control.navigate_waypoints(prev_wp, next_wp, current_position, ground_speed); + _l1_control.navigate_waypoints(prev_wp, curr_wp, current_position, ground_speed); _att_sp.roll_body = _l1_control.nav_roll(); _att_sp.yaw_body = _l1_control.nav_bearing(); -- cgit v1.2.3