diff options
Diffstat (limited to 'src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp')
| -rw-r--r-- | src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp | 254 |
1 files changed, 204 insertions, 50 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 0b147f903..cfa98e573 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 @@ -83,6 +83,7 @@ #include <systemlib/perf_counter.h> #include <systemlib/systemlib.h> #include <mathlib/mathlib.h> +#include <mavlink/mavlink_log.h> #include <ecl/l1/ecl_l1_pos_controller.h> #include <external_lgpl/tecs/tecs.h> @@ -94,6 +95,8 @@ */ extern "C" __EXPORT int fw_pos_control_l1_main(int argc, char *argv[]); +static int mavlink_fd; + class FixedwingPositionControl { public: @@ -160,7 +163,13 @@ private: /* land states */ /* not in non-abort mode for landing yet */ - bool land_noreturn; + bool land_noreturn_horizontal; + bool land_noreturn_vertical; + bool land_stayonground; + bool land_motor_lim; + bool land_onslope; + + float flare_curve_alt_last; /* heading hold */ float target_bearing; @@ -206,6 +215,15 @@ private: float throttle_land_max; float loiter_hold_radius; + + float heightrate_p; + + float land_slope_angle; + float land_slope_length; + float land_H1_virt; + float land_flare_alt_relative; + float land_thrust_lim_horizontal_distance; + } _parameters; /**< local copies of interesting parameters */ struct { @@ -240,6 +258,15 @@ private: param_t throttle_land_max; param_t loiter_hold_radius; + + param_t heightrate_p; + + param_t land_slope_angle; + param_t land_slope_length; + param_t land_H1_virt; + param_t land_flare_alt_relative; + param_t land_thrust_lim_horizontal_distance; + } _parameter_handles; /**< handles for interesting parameters */ @@ -280,13 +307,18 @@ private: void vehicle_setpoint_poll(); /** + * Get Altitude on the landing glide slope + */ + float getLandingSlopeAbsoluteAltitude(float wp_distance, float wp_altitude, float landing_slope_angle_rad, float horizontal_displacement); + + /** * Control position. */ bool control_position(const math::Vector2f &global_pos, const math::Vector2f &ground_speed, const struct mission_item_triplet_s &_mission_item_triplet); float calculate_target_airspeed(float airspeed_demand); - void calculate_gndspeed_undershoot(); + void calculate_gndspeed_undershoot(const math::Vector2f ¤t_position, const math::Vector2f &ground_speed, const struct mission_item_triplet_s &mission_item_triplet); /** * Shim for calling task_main from task_create. @@ -338,8 +370,30 @@ FixedwingPositionControl::FixedwingPositionControl() : _airspeed_valid(false), _groundspeed_undershoot(0.0f), _global_pos_valid(false), - land_noreturn(false) + land_noreturn_horizontal(false), + land_noreturn_vertical(false), + land_stayonground(false), + land_motor_lim(false), + land_onslope(false), + flare_curve_alt_last(0.0f) { + + mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); + + /* safely initialize structs */ + vehicle_attitude_s _att = {0}; + vehicle_attitude_setpoint_s _att_sp = {0}; + navigation_capabilities_s _nav_capabilities = {0}; + manual_control_setpoint_s _manual = {0}; + airspeed_s _airspeed = {0}; + vehicle_control_mode_s _control_mode = {0}; + vehicle_global_position_s _global_pos = {0}; + mission_item_triplet_s _mission_item_triplet = {0}; + accel_report _accel = {0}; + + + + _nav_capabilities.turn_distance = 0.0f; _parameter_handles.l1_period = param_find("FW_L1_PERIOD"); @@ -358,6 +412,12 @@ 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_SLLR"); + _parameter_handles.land_H1_virt = param_find("FW_LND_HVIRT"); + _parameter_handles.land_flare_alt_relative = param_find("FW_LND_FLALT"); + _parameter_handles.land_thrust_lim_horizontal_distance = param_find("FW_LND_TLDIST"); + _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"); @@ -370,6 +430,7 @@ FixedwingPositionControl::FixedwingPositionControl() : _parameter_handles.roll_throttle_compensation = param_find("FW_T_RLL2THR"); _parameter_handles.speed_weight = param_find("FW_T_SPDWEIGHT"); _parameter_handles.pitch_damping = param_find("FW_T_PTCH_DAMP"); + _parameter_handles.heightrate_p = param_find("FW_T_HRATE_P"); /* fetch initial parameter values */ parameters_update(); @@ -435,6 +496,14 @@ 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.heightrate_p, &(_parameters.heightrate_p)); + + param_get(_parameter_handles.land_slope_angle, &(_parameters.land_slope_angle)); + param_get(_parameter_handles.land_slope_length, &(_parameters.land_slope_length)); + param_get(_parameter_handles.land_H1_virt, &(_parameters.land_H1_virt)); + param_get(_parameter_handles.land_flare_alt_relative, &(_parameters.land_flare_alt_relative)); + param_get(_parameter_handles.land_thrust_lim_horizontal_distance, &(_parameters.land_thrust_lim_horizontal_distance)); + _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)); @@ -447,12 +516,13 @@ FixedwingPositionControl::parameters_update() _tecs.set_vertical_accel_limit(_parameters.vertical_accel_limit); _tecs.set_height_comp_filter_omega(_parameters.height_comp_filter_omega); _tecs.set_speed_comp_filter_omega(_parameters.speed_comp_filter_omega); - _tecs.set_roll_throttle_compensation(math::radians(_parameters.roll_throttle_compensation)); + _tecs.set_roll_throttle_compensation(_parameters.roll_throttle_compensation); _tecs.set_speed_weight(_parameters.speed_weight); _tecs.set_pitch_damping(_parameters.pitch_damping); _tecs.set_indicated_airspeed_min(_parameters.airspeed_min); _tecs.set_indicated_airspeed_max(_parameters.airspeed_max); _tecs.set_max_climb_rate(_parameters.max_climb_rate); + _tecs.set_heightrate_p(_parameters.heightrate_p); /* sanity check parameters */ if (_parameters.airspeed_max < _parameters.airspeed_min || @@ -595,17 +665,29 @@ FixedwingPositionControl::calculate_target_airspeed(float airspeed_demand) } void -FixedwingPositionControl::calculate_gndspeed_undershoot() +FixedwingPositionControl::calculate_gndspeed_undershoot(const math::Vector2f ¤t_position, const math::Vector2f &ground_speed, const struct mission_item_triplet_s &mission_item_triplet) { if (_global_pos_valid) { - /* get ground speed vector */ - math::Vector2f ground_speed_vector(_global_pos.vx, _global_pos.vy); - /* rotate with current attitude */ + /* rotate ground speed vector with current attitude */ math::Vector2f yaw_vector(_R_nb(0, 0), _R_nb(1, 0)); yaw_vector.normalize(); - float ground_speed_body = yaw_vector * ground_speed_vector; + float ground_speed_body = yaw_vector * ground_speed; + + /* The minimum desired ground speed is the minimum airspeed projected on to the ground using the altitude and horizontal difference between the waypoints if available*/ + float distance = 0.0f; + float delta_altitude = 0.0f; + if (mission_item_triplet.previous_valid) { + distance = get_distance_to_next_waypoint(mission_item_triplet.previous.lat, mission_item_triplet.previous.lon, mission_item_triplet.current.lat, mission_item_triplet.current.lon); + delta_altitude = mission_item_triplet.current.altitude - mission_item_triplet.previous.altitude; + } else { + distance = get_distance_to_next_waypoint(current_position.getX(), current_position.getY(), mission_item_triplet.current.lat, mission_item_triplet.current.lon); + delta_altitude = mission_item_triplet.current.altitude - _global_pos.alt; + } + + float ground_speed_desired = _parameters.airspeed_min * cosf(atan2f(delta_altitude, distance)); + /* * Ground speed undershoot is the amount of ground velocity not reached @@ -616,20 +698,25 @@ FixedwingPositionControl::calculate_gndspeed_undershoot() * not exceeded) travels towards a waypoint (and is not pushed more and more away * by wind). Not countering this would lead to a fly-away. */ - _groundspeed_undershoot = math::max(_parameters.airspeed_min - ground_speed_body, 0.0f); + _groundspeed_undershoot = math::max(ground_speed_desired - ground_speed_body, 0.0f); } else { _groundspeed_undershoot = 0; } } +float FixedwingPositionControl::getLandingSlopeAbsoluteAltitude(float wp_distance, float wp_altitude, float landing_slope_angle_rad, float horizontal_displacement) +{ + return (wp_distance - horizontal_displacement) * tanf(landing_slope_angle_rad) + wp_altitude; //flare_relative_alt is negative +} + bool FixedwingPositionControl::control_position(const math::Vector2f ¤t_position, const math::Vector2f &ground_speed, const struct mission_item_triplet_s &mission_item_triplet) { bool setpoint = true; - calculate_gndspeed_undershoot(); + calculate_gndspeed_undershoot(current_position, ground_speed, mission_item_triplet); float eas2tas = 1.0f; // XXX calculate actual number based on current measurements @@ -659,11 +746,15 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio _tecs.set_speed_weight(_parameters.speed_weight); /* execute navigation once we have a setpoint */ - if (_setpoint_valid) { + if (_setpoint_valid && _control_mode.flag_control_auto_enabled) { /* current waypoint (the one currently heading for) */ math::Vector2f next_wp(mission_item_triplet.current.lat, mission_item_triplet.current.lon); + /* current waypoint (the one currently heading for) */ + math::Vector2f curr_wp(mission_item_triplet.current.lat, mission_item_triplet.current.lon); + + /* previous waypoint */ math::Vector2f prev_wp; @@ -700,7 +791,7 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio } else if (mission_item_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(); @@ -715,7 +806,7 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio mission_item_triplet.current.nav_cmd == NAV_CMD_LOITER_UNLIMITED) { /* waypoint is a loiter waypoint */ - _l1_control.navigate_loiter(next_wp, current_position, mission_item_triplet.current.loiter_radius, + _l1_control.navigate_loiter(curr_wp, current_position, mission_item_triplet.current.loiter_radius, mission_item_triplet.current.loiter_direction, ground_speed); _att_sp.roll_body = _l1_control.nav_roll(); _att_sp.yaw_body = _l1_control.nav_bearing(); @@ -728,11 +819,12 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio } else if (mission_item_triplet.current.nav_cmd == NAV_CMD_LAND) { + /* 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) { + const float heading_hold_distance = 15.0f; + if (wp_distance < heading_hold_distance || land_noreturn_horizontal) { /* heading hold, along the line connecting this and the last waypoint */ @@ -741,75 +833,133 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio // target_bearing = get_bearing_to_next_waypoint(prev_wp.getX(), prev_wp.getY(), next_wp.getX(), next_wp.getY()); // } else { - if (!land_noreturn) + if (!land_noreturn_horizontal) //set target_bearing in first occurrence 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)); +// warnx("NORET: %d, target_bearing: %d, yaw: %d", (int)land_noreturn_horizontal, (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; + land_noreturn_horizontal = true; } 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); } - /* do not go down too early */ - if (wp_distance > 50.0f) { - altitude_error = (_mission_item_triplet.current.altitude + 25.0f) - _global_pos.alt; - } - - _att_sp.roll_body = _l1_control.nav_roll(); _att_sp.yaw_body = _l1_control.nav_bearing(); + + /* Vertical landing control */ + //xxx: using the tecs altitude controller for slope control for now + +// /* do not go down too early */ +// if (wp_distance > 50.0f) { +// altitude_error = (_global_triplet.current.altitude + 25.0f) - _global_pos.alt; +// } /* 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 - float flare_angle_rad = math::radians(10.0f);//math::radians(mission_item_triplet.current.param1) + float flare_angle_rad = -math::radians(5.0f);//math::radians(mission_item_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; - - if (altitude_error > -4.0f) { + float airspeed_land = 1.3f * _parameters.airspeed_min; + float airspeed_approach = 1.3f * _parameters.airspeed_min; + + float landing_slope_angle_rad = math::radians(_parameters.land_slope_angle); + float flare_relative_alt = _parameters.land_flare_alt_relative; + float motor_lim_horizontal_distance = _parameters.land_thrust_lim_horizontal_distance;//be generous here as we currently have to rely on the user input for the waypoint + float L_wp_distance = get_distance_to_next_waypoint(prev_wp.getX(), prev_wp.getY(), curr_wp.getX(), curr_wp.getY()) * _parameters.land_slope_length; + float H1 = _parameters.land_H1_virt; + float H0 = flare_relative_alt + H1; + float d1 = flare_relative_alt/tanf(landing_slope_angle_rad); + float flare_constant = (H0 * d1)/flare_relative_alt;//-flare_length/(logf(H1/H0)); + float flare_length = - logf(H1/H0) * flare_constant;//d1+20.0f;//-logf(0.01f/flare_relative_alt); + float horizontal_slope_displacement = (flare_length - d1); + float L_altitude = getLandingSlopeAbsoluteAltitude(L_wp_distance, _mission_item_triplet.current.altitude, landing_slope_angle_rad, horizontal_slope_displacement); + float landing_slope_alt_desired = getLandingSlopeAbsoluteAltitude(wp_distance, _mission_item_triplet.current.altitude, landing_slope_angle_rad, horizontal_slope_displacement); + + if ( (_global_pos.alt < _mission_item_triplet.current.altitude + flare_relative_alt) || land_noreturn_vertical) { //checking for land_noreturn to avoid unwanted climb out /* 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, _mission_item_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, - math::radians(-10.0f), math::radians(15.0f)); +// /* force TECS to only control speed with pitch, altitude is only implicitely controlled now */ +// _tecs.set_speed_weight(2.0f); /* kill the throttle if param requests it */ - throttle_max = math::min(throttle_max, _parameters.throttle_land_max); + throttle_max = _parameters.throttle_max; + + if (wp_distance < motor_lim_horizontal_distance || land_motor_lim) { + throttle_max = math::min(throttle_max, _parameters.throttle_land_max); + if (!land_motor_lim) { + land_motor_lim = true; + mavlink_log_info(mavlink_fd, "[POSCTRL] Landing, limit throttle"); + } + + } + + float flare_curve_alt = _mission_item_triplet.current.altitude + H0 * expf(-math::max(0.0f, flare_length - wp_distance)/flare_constant) - H1; + + /* avoid climbout */ + if (flare_curve_alt_last < flare_curve_alt && land_noreturn_vertical || land_stayonground) + { + flare_curve_alt = mission_item_triplet.current.altitude; + land_stayonground = true; + } + + _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, flare_curve_alt, calculate_target_airspeed(airspeed_land), + _airspeed.indicated_airspeed_m_s, eas2tas, + false, flare_angle_rad, + 0.0f, throttle_max, throttle_land, + flare_angle_rad, math::radians(15.0f)); /* 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)); + if (!land_noreturn_vertical) { + mavlink_log_info(mavlink_fd, "[POSCTRL] Landing, flare"); + land_noreturn_vertical = true; + } + //warnx("Landing: flare, _global_pos.alt %.1f, flare_curve_alt %.1f, flare_curve_alt_last %.1f, flare_length %.1f, wp_distance %.1f", _global_pos.alt, flare_curve_alt, flare_curve_alt_last, flare_length, wp_distance); - } else if (wp_distance < 60.0f && altitude_error > -20.0f) { + flare_curve_alt_last = flare_curve_alt; - /* minimize speed to approach speed */ + } else if (wp_distance < L_wp_distance) { - _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _mission_item_triplet.current.altitude, calculate_target_airspeed(airspeed_approach), + /* 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: stay on slope, alt_desired: %.1f (wp_distance: %.1f), calculate_target_airspeed(airspeed_land) %.1f, horizontal_slope_displacement %.1f, d1 %.1f, flare_length %.1f", landing_slope_alt_desired, wp_distance, calculate_target_airspeed(airspeed_land), horizontal_slope_displacement, d1, flare_length); + + if (!land_onslope) { + + mavlink_log_info(mavlink_fd, "[POSCTRL] Landing, on slope"); + land_onslope = true; + } } else { - /* normal cruise speed */ + /* intersect glide slope: + * if current position is higher or within 10m of slope follow the glide slope + * if current position is below slope -10m continue on maximum of previous wp altitude or L_altitude until the intersection with the slope + * */ + float altitude_desired = _global_pos.alt; + if (_global_pos.alt > landing_slope_alt_desired - 10.0f) { + /* stay on slope */ + altitude_desired = landing_slope_alt_desired; + //warnx("Landing: before L, stay on landing slope, alt_desired: %.1f (wp_distance: %.1f, L_wp_distance %.1f), calculate_target_airspeed(airspeed_land) %.1f, horizontal_slope_displacement %.1f", altitude_desired, wp_distance, L_wp_distance, calculate_target_airspeed(airspeed_land), horizontal_slope_displacement); + } else { + /* continue horizontally */ + altitude_desired = math::max(_global_pos.alt, L_altitude); + //warnx("Landing: before L,continue at: %.4f, (landing_slope_alt_desired %.4f, wp_distance: %.4f, L_altitude: %.4f L_wp_distance: %.4f)", altitude_desired, landing_slope_alt_desired, wp_distance, L_altitude, L_wp_distance); + } - _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _mission_item_triplet.current.altitude, calculate_target_airspeed(_parameters.airspeed_trim), + _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, altitude_desired, calculate_target_airspeed(airspeed_approach), _airspeed.indicated_airspeed_m_s, eas2tas, false, math::radians(_parameters.pitch_limit_min), _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise, @@ -818,12 +968,12 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio } else if (mission_item_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(); /* apply minimum pitch and limit roll if target altitude is not within 10 meters */ - if (altitude_error > 10.0f) { + if (altitude_error > 15.0f) { /* enforce a minimum of 10 degrees pitch up on takeoff, or take parameter */ _tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, _mission_item_triplet.current.altitude, calculate_target_airspeed(_parameters.airspeed_min), @@ -903,7 +1053,11 @@ FixedwingPositionControl::control_position(const math::Vector2f ¤t_positio /* reset land state */ if (mission_item_triplet.current.nav_cmd != NAV_CMD_LAND) { - land_noreturn = false; + land_noreturn_horizontal = false; + land_noreturn_vertical = false; + land_stayonground = false; + land_motor_lim = false; + land_onslope = false; } if (was_circle_mode && !_l1_control.circle_mode()) { |