aboutsummaryrefslogtreecommitdiff
path: root/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
diff options
context:
space:
mode:
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.cpp169
1 files changed, 152 insertions, 17 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 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 &current_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 &current_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 &current_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 &current_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 &current_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 &current_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 &current_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 &current_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;
}
generated by cgit v1.2.3 (git 2.39.1) at 2024-07-07 19:13:06 +0000