fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

autoland slope parameters, variable rename for bugfix and code clarity, printfs

Browse Source
This commit is contained in:
Thomas Gubler 2013-11-01 17:35:09 +01:00
parent 116ee348a2
commit e539a89e22
2 changed files with 33 additions and 11 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

41
src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
View File

@ -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 &current_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 &current_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 &current_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 &current_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 &current_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 &current_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 &current_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 &current_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 &current_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();

3
src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c
View File

@ -111,3 +111,6 @@ PARAM_DEFINE_FLOAT(FW_T_PTCH_DAMP, 0.0f);
PARAM_DEFINE_FLOAT(FW_T_SINK_MAX, 5.0f);
PARAM_DEFINE_FLOAT(FW_LND_ANG, 10.0f);
PARAM_DEFINE_FLOAT(FW_LND_SLL, 64.0f);