mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-07-03 00:20:34 +08:00
fw pos ctrl: calculate control interval once
- Use the same time interval for all position control logic (including TECS) - Sync naming in control methods - Remove some unused input arguments
This commit is contained in:
Thomas Stastny
@@ -414,8 +414,8 @@ FixedwingPositionControl::get_manual_airspeed_setpoint()
|
||||
}
|
||||
|
||||
float
|
||||
FixedwingPositionControl::get_auto_airspeed_setpoint(const hrt_abstime &now, const float pos_sp_cruise_airspeed,
|
||||
const Vector2f &ground_speed, float dt)
|
||||
FixedwingPositionControl::get_auto_airspeed_setpoint(const float control_interval, const float pos_sp_cruise_airspeed,
|
||||
const Vector2f &ground_speed)
|
||||
{
|
||||
// overwrite internal setpoint (e.g. set prior through MAV_CMD_DO_CHANGE_SPEED) in case
|
||||
// the current position_setpoint contains a valid airspeed setpoint
|
||||
@@ -466,9 +466,9 @@ FixedwingPositionControl::get_auto_airspeed_setpoint(const hrt_abstime &now, con
|
||||
if (!PX4_ISFINITE(_slew_rate_airspeed.getState()) || outside_of_limits) {
|
||||
_slew_rate_airspeed.setForcedValue(airspeed_setpoint);
|
||||
|
||||
} else if (dt > FLT_EPSILON) {
|
||||
} else if (control_interval > FLT_EPSILON) {
|
||||
// constrain airspeed setpoint changes with slew rate of ASPD_SP_SLEW_RATE m/s/s
|
||||
airspeed_setpoint = _slew_rate_airspeed.update(airspeed_setpoint, dt);
|
||||
airspeed_setpoint = _slew_rate_airspeed.update(airspeed_setpoint, control_interval);
|
||||
}
|
||||
|
||||
return airspeed_setpoint;
|
||||
@@ -844,16 +844,6 @@ FixedwingPositionControl::update_in_air_states(const hrt_abstime now)
|
||||
}
|
||||
}
|
||||
|
||||
float
|
||||
FixedwingPositionControl::update_position_control_mode_timestep(const hrt_abstime now)
|
||||
{
|
||||
const float dt = math::constrain((now - _last_time_position_control_called) * 1e-6f, MIN_AUTO_TIMESTEP,
|
||||
MAX_AUTO_TIMESTEP);
|
||||
_last_time_position_control_called = now;
|
||||
|
||||
return dt;
|
||||
}
|
||||
|
||||
void
|
||||
FixedwingPositionControl::move_position_setpoint_for_vtol_transition(position_setpoint_s ¤t_sp)
|
||||
{
|
||||
@@ -885,12 +875,10 @@ FixedwingPositionControl::move_position_setpoint_for_vtol_transition(position_se
|
||||
}
|
||||
|
||||
void
|
||||
FixedwingPositionControl::control_auto(const hrt_abstime &now, const Vector2d &curr_pos,
|
||||
const Vector2f &ground_speed, const position_setpoint_s &pos_sp_prev,
|
||||
const position_setpoint_s &pos_sp_curr, const position_setpoint_s &pos_sp_next)
|
||||
FixedwingPositionControl::control_auto(const float control_interval, const Vector2d &curr_pos,
|
||||
const Vector2f &ground_speed, const position_setpoint_s &pos_sp_prev, const position_setpoint_s &pos_sp_curr,
|
||||
const position_setpoint_s &pos_sp_next)
|
||||
{
|
||||
const float dt = update_position_control_mode_timestep(now);
|
||||
|
||||
update_in_air_states(now);
|
||||
|
||||
_hdg_hold_yaw = _yaw;
|
||||
@@ -911,14 +899,6 @@ FixedwingPositionControl::control_auto(const hrt_abstime &now, const Vector2d &c
|
||||
publishOrbitStatus(current_sp);
|
||||
}
|
||||
|
||||
// update lateral guidance timesteps for slewrates
|
||||
if (_param_fw_use_npfg.get()) {
|
||||
_npfg.setDt(dt);
|
||||
|
||||
} else {
|
||||
_l1_control.set_dt(dt);
|
||||
}
|
||||
|
||||
const bool was_circle_mode = (_param_fw_use_npfg.get()) ? _npfg.circleMode() : _l1_control.circle_mode();
|
||||
|
||||
/* restore TECS parameters, in case changed intermittently (e.g. in landing handling) */
|
||||
@@ -936,15 +916,15 @@ FixedwingPositionControl::control_auto(const hrt_abstime &now, const Vector2d &c
|
||||
break;
|
||||
|
||||
case position_setpoint_s::SETPOINT_TYPE_POSITION:
|
||||
control_auto_position(now, dt, curr_pos, ground_speed, pos_sp_prev, current_sp);
|
||||
control_auto_position(control_interval, curr_pos, ground_speed, pos_sp_prev, current_sp);
|
||||
break;
|
||||
|
||||
case position_setpoint_s::SETPOINT_TYPE_VELOCITY:
|
||||
control_auto_velocity(now, dt, curr_pos, ground_speed, pos_sp_prev, current_sp);
|
||||
control_auto_velocity(control_interval, curr_pos, ground_speed, pos_sp_prev, current_sp);
|
||||
break;
|
||||
|
||||
case position_setpoint_s::SETPOINT_TYPE_LOITER:
|
||||
control_auto_loiter(now, dt, curr_pos, ground_speed, pos_sp_prev, current_sp, pos_sp_next);
|
||||
control_auto_loiter(control_interval, curr_pos, ground_speed, pos_sp_prev, current_sp, pos_sp_next);
|
||||
break;
|
||||
}
|
||||
|
||||
@@ -972,11 +952,12 @@ FixedwingPositionControl::control_auto(const hrt_abstime &now, const Vector2d &c
|
||||
}
|
||||
|
||||
void
|
||||
FixedwingPositionControl::control_auto_fixed_bank_alt_hold(const hrt_abstime &now)
|
||||
FixedwingPositionControl::control_auto_fixed_bank_alt_hold(const float control_interval)
|
||||
{
|
||||
// only control altitude and airspeed ("fixed-bank loiter")
|
||||
|
||||
tecs_update_pitch_throttle(now, _current_altitude,
|
||||
tecs_update_pitch_throttle(control_interval,
|
||||
_current_altitude,
|
||||
_param_fw_airspd_trim.get(),
|
||||
radians(_param_fw_p_lim_min.get()),
|
||||
radians(_param_fw_p_lim_max.get()),
|
||||
@@ -1003,7 +984,7 @@ FixedwingPositionControl::control_auto_fixed_bank_alt_hold(const hrt_abstime &no
|
||||
}
|
||||
|
||||
void
|
||||
FixedwingPositionControl::control_auto_descend(const hrt_abstime &now)
|
||||
FixedwingPositionControl::control_auto_descend(const float control_interval)
|
||||
{
|
||||
// only control height rate
|
||||
|
||||
@@ -1011,7 +992,8 @@ FixedwingPositionControl::control_auto_descend(const hrt_abstime &now)
|
||||
// but not letting it drift too far away.
|
||||
const float descend_rate = -0.5f;
|
||||
|
||||
tecs_update_pitch_throttle(now, _current_altitude,
|
||||
tecs_update_pitch_throttle(control_interval,
|
||||
_current_altitude,
|
||||
_param_fw_airspd_trim.get(),
|
||||
radians(_param_fw_p_lim_min.get()),
|
||||
radians(_param_fw_p_lim_max.get()),
|
||||
@@ -1091,7 +1073,7 @@ FixedwingPositionControl::handle_setpoint_type(const uint8_t setpoint_type, cons
|
||||
}
|
||||
|
||||
void
|
||||
FixedwingPositionControl::control_auto_position(const hrt_abstime &now, const float dt, const Vector2d &curr_pos,
|
||||
FixedwingPositionControl::control_auto_position(const float control_interval, const Vector2d &curr_pos,
|
||||
const Vector2f &ground_speed, const position_setpoint_s &pos_sp_prev, const position_setpoint_s &pos_sp_curr)
|
||||
{
|
||||
const float acc_rad = (_param_fw_use_npfg.get()) ? _npfg.switchDistance(500.0f) : _l1_control.switch_distance(500.0f);
|
||||
@@ -1174,7 +1156,7 @@ FixedwingPositionControl::control_auto_position(const hrt_abstime &now, const fl
|
||||
}
|
||||
}
|
||||
|
||||
float target_airspeed = get_auto_airspeed_setpoint(now, pos_sp_curr.cruising_speed, ground_speed, dt);
|
||||
float target_airspeed = get_auto_airspeed_setpoint(control_interval, pos_sp_curr.cruising_speed, ground_speed);
|
||||
Vector2f curr_pos_local{_local_pos.x, _local_pos.y};
|
||||
Vector2f curr_wp_local = _global_local_proj_ref.project(curr_wp(0), curr_wp(1));
|
||||
Vector2f prev_wp_local = _global_local_proj_ref.project(prev_wp(0), prev_wp(1));
|
||||
@@ -1208,7 +1190,8 @@ FixedwingPositionControl::control_auto_position(const hrt_abstime &now, const fl
|
||||
_att_sp.apply_flaps = vehicle_attitude_setpoint_s::FLAPS_OFF;
|
||||
_att_sp.apply_spoilers = vehicle_attitude_setpoint_s::SPOILERS_OFF;
|
||||
|
||||
tecs_update_pitch_throttle(now, position_sp_alt,
|
||||
tecs_update_pitch_throttle(control_interval,
|
||||
position_sp_alt,
|
||||
target_airspeed,
|
||||
radians(_param_fw_p_lim_min.get()),
|
||||
radians(_param_fw_p_lim_max.get()),
|
||||
@@ -1220,7 +1203,7 @@ FixedwingPositionControl::control_auto_position(const hrt_abstime &now, const fl
|
||||
}
|
||||
|
||||
void
|
||||
FixedwingPositionControl::control_auto_velocity(const hrt_abstime &now, const float dt, const Vector2d &curr_pos,
|
||||
FixedwingPositionControl::control_auto_velocity(const float control_interval, const Vector2d &curr_pos,
|
||||
const Vector2f &ground_speed, const position_setpoint_s &pos_sp_prev, const position_setpoint_s &pos_sp_curr)
|
||||
{
|
||||
float tecs_fw_thr_min;
|
||||
@@ -1255,7 +1238,7 @@ FixedwingPositionControl::control_auto_velocity(const hrt_abstime &now, const fl
|
||||
Vector2f target_velocity{pos_sp_curr.vx, pos_sp_curr.vy};
|
||||
_target_bearing = wrap_pi(atan2f(target_velocity(1), target_velocity(0)));
|
||||
|
||||
float target_airspeed = get_auto_airspeed_setpoint(now, pos_sp_curr.cruising_speed, ground_speed, dt);
|
||||
float target_airspeed = get_auto_airspeed_setpoint(control_interval, pos_sp_curr.cruising_speed, ground_speed);
|
||||
|
||||
if (_param_fw_use_npfg.get()) {
|
||||
_npfg.setAirspeedNom(target_airspeed * _eas2tas);
|
||||
@@ -1274,7 +1257,8 @@ FixedwingPositionControl::control_auto_velocity(const hrt_abstime &now, const fl
|
||||
_att_sp.apply_flaps = vehicle_attitude_setpoint_s::FLAPS_OFF;
|
||||
_att_sp.apply_spoilers = vehicle_attitude_setpoint_s::SPOILERS_OFF;
|
||||
|
||||
tecs_update_pitch_throttle(now, position_sp_alt,
|
||||
tecs_update_pitch_throttle(control_interval,
|
||||
position_sp_alt,
|
||||
target_airspeed,
|
||||
radians(_param_fw_p_lim_min.get()),
|
||||
radians(_param_fw_p_lim_max.get()),
|
||||
@@ -1288,7 +1272,7 @@ FixedwingPositionControl::control_auto_velocity(const hrt_abstime &now, const fl
|
||||
}
|
||||
|
||||
void
|
||||
FixedwingPositionControl::control_auto_loiter(const hrt_abstime &now, const float dt, const Vector2d &curr_pos,
|
||||
FixedwingPositionControl::control_auto_loiter(const float control_interval, const Vector2d &curr_pos,
|
||||
const Vector2f &ground_speed, const position_setpoint_s &pos_sp_prev, const position_setpoint_s &pos_sp_curr,
|
||||
const position_setpoint_s &pos_sp_next)
|
||||
{
|
||||
@@ -1369,7 +1353,7 @@ FixedwingPositionControl::control_auto_loiter(const hrt_abstime &now, const floa
|
||||
_att_sp.apply_spoilers = vehicle_attitude_setpoint_s::SPOILERS_OFF;
|
||||
}
|
||||
|
||||
float target_airspeed = get_auto_airspeed_setpoint(now, airspeed_sp, ground_speed, dt);
|
||||
float target_airspeed = get_auto_airspeed_setpoint(control_interval, airspeed_sp, ground_speed);
|
||||
|
||||
Vector2f curr_pos_local{_local_pos.x, _local_pos.y};
|
||||
Vector2f curr_wp_local = _global_local_proj_ref.project(curr_wp(0), curr_wp(1));
|
||||
@@ -1410,7 +1394,8 @@ FixedwingPositionControl::control_auto_loiter(const hrt_abstime &now, const floa
|
||||
_tecs.set_height_error_time_constant(_param_fw_thrtc_sc.get() * _param_fw_t_h_error_tc.get());
|
||||
}
|
||||
|
||||
tecs_update_pitch_throttle(now, alt_sp,
|
||||
tecs_update_pitch_throttle(control_interval,
|
||||
alt_sp,
|
||||
target_airspeed,
|
||||
radians(_param_fw_p_lim_min.get()),
|
||||
radians(_param_fw_p_lim_max.get()),
|
||||
@@ -1422,11 +1407,10 @@ FixedwingPositionControl::control_auto_loiter(const hrt_abstime &now, const floa
|
||||
}
|
||||
|
||||
void
|
||||
FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const Vector2d &curr_pos,
|
||||
const Vector2f &ground_speed, const position_setpoint_s &pos_sp_prev, const position_setpoint_s &pos_sp_curr)
|
||||
FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const float control_interval,
|
||||
const Vector2d &curr_pos, const Vector2f &ground_speed, const position_setpoint_s &pos_sp_prev,
|
||||
const position_setpoint_s &pos_sp_curr)
|
||||
{
|
||||
const float dt = update_position_control_mode_timestep(now);
|
||||
|
||||
update_in_air_states(now);
|
||||
|
||||
_hdg_hold_yaw = _yaw;
|
||||
@@ -1435,14 +1419,6 @@ FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const Vec
|
||||
_att_sp.pitch_reset_integral = false;
|
||||
_att_sp.yaw_reset_integral = false;
|
||||
|
||||
// update lateral guidance timesteps for slewrates
|
||||
if (_param_fw_use_npfg.get()) {
|
||||
_npfg.setDt(dt);
|
||||
|
||||
} else {
|
||||
_l1_control.set_dt(dt);
|
||||
}
|
||||
|
||||
/* restore TECS parameters, in case changed intermittently (e.g. in landing handling) */
|
||||
_tecs.set_speed_weight(_param_fw_t_spdweight.get());
|
||||
_tecs.set_height_error_time_constant(_param_fw_t_h_error_tc.get());
|
||||
@@ -1491,9 +1467,8 @@ FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const Vec
|
||||
_runway_takeoff.update(now, _airspeed, _current_altitude - terrain_alt,
|
||||
_current_latitude, _current_longitude, &_mavlink_log_pub);
|
||||
|
||||
float target_airspeed = get_auto_airspeed_setpoint(now,
|
||||
_runway_takeoff.getMinAirspeedScaling() * _param_fw_airspd_min.get(), ground_speed,
|
||||
dt);
|
||||
float target_airspeed = get_auto_airspeed_setpoint(control_interval,
|
||||
_runway_takeoff.getMinAirspeedScaling() * _param_fw_airspd_min.get(), ground_speed);
|
||||
|
||||
/*
|
||||
* Update navigation: _runway_takeoff returns the start WP according to mode and phase.
|
||||
@@ -1522,7 +1497,8 @@ FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const Vec
|
||||
// update tecs
|
||||
const float takeoff_pitch_max_deg = _runway_takeoff.getMaxPitch(_param_fw_p_lim_max.get());
|
||||
|
||||
tecs_update_pitch_throttle(now, pos_sp_curr.alt,
|
||||
tecs_update_pitch_throttle(control_interval,
|
||||
pos_sp_curr.alt,
|
||||
target_airspeed,
|
||||
radians(_param_fw_p_lim_min.get()),
|
||||
radians(takeoff_pitch_max_deg),
|
||||
@@ -1560,7 +1536,7 @@ FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const Vec
|
||||
}
|
||||
|
||||
/* Detect launch using body X (forward) acceleration */
|
||||
_launchDetector.update(dt, _body_acceleration(0));
|
||||
_launchDetector.update(control_interval, _body_acceleration(0));
|
||||
|
||||
/* update our copy of the launch detection state */
|
||||
_launch_detection_state = _launchDetector.getLaunchDetected();
|
||||
@@ -1575,8 +1551,7 @@ FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const Vec
|
||||
if (_launch_detection_state != LAUNCHDETECTION_RES_NONE) {
|
||||
/* Launch has been detected, hence we have to control the plane. */
|
||||
|
||||
float target_airspeed = get_auto_airspeed_setpoint(now, _param_fw_airspd_trim.get(), ground_speed,
|
||||
dt);
|
||||
float target_airspeed = get_auto_airspeed_setpoint(control_interval, _param_fw_airspd_trim.get(), ground_speed);
|
||||
|
||||
Vector2f prev_wp_local = _global_local_proj_ref.project(prev_wp(0), prev_wp(1));
|
||||
|
||||
@@ -1610,7 +1585,8 @@ FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const Vec
|
||||
/* apply minimum pitch and limit roll if target altitude is not within climbout_diff meters */
|
||||
if (_param_fw_clmbout_diff.get() > 0.0f && altitude_error > _param_fw_clmbout_diff.get()) {
|
||||
/* enforce a minimum of 10 degrees pitch up on takeoff, or take parameter */
|
||||
tecs_update_pitch_throttle(now, pos_sp_curr.alt,
|
||||
tecs_update_pitch_throttle(control_interval,
|
||||
pos_sp_curr.alt,
|
||||
target_airspeed,
|
||||
radians(_param_fw_p_lim_min.get()),
|
||||
radians(takeoff_pitch_max_deg),
|
||||
@@ -1624,7 +1600,8 @@ FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const Vec
|
||||
_att_sp.roll_body = constrain(_att_sp.roll_body, radians(-15.0f), radians(15.0f));
|
||||
|
||||
} else {
|
||||
tecs_update_pitch_throttle(now, pos_sp_curr.alt,
|
||||
tecs_update_pitch_throttle(control_interval,
|
||||
pos_sp_curr.alt,
|
||||
target_airspeed,
|
||||
radians(_param_fw_p_lim_min.get()),
|
||||
radians(_param_fw_p_lim_max.get()),
|
||||
@@ -1679,11 +1656,10 @@ FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const Vec
|
||||
}
|
||||
|
||||
void
|
||||
FixedwingPositionControl::control_auto_landing(const hrt_abstime &now, const Vector2d &curr_pos,
|
||||
const Vector2f &ground_speed, const position_setpoint_s &pos_sp_prev, const position_setpoint_s &pos_sp_curr)
|
||||
FixedwingPositionControl::control_auto_landing(const hrt_abstime &now, const float control_interval,
|
||||
const Vector2d &curr_pos, const Vector2f &ground_speed, const position_setpoint_s &pos_sp_prev,
|
||||
const position_setpoint_s &pos_sp_curr)
|
||||
{
|
||||
const float dt = update_position_control_mode_timestep(now);
|
||||
|
||||
update_in_air_states(now);
|
||||
|
||||
_hdg_hold_yaw = _yaw;
|
||||
@@ -1692,14 +1668,6 @@ FixedwingPositionControl::control_auto_landing(const hrt_abstime &now, const Vec
|
||||
_att_sp.pitch_reset_integral = false;
|
||||
_att_sp.yaw_reset_integral = false;
|
||||
|
||||
// update lateral guidance timesteps for slewrates
|
||||
if (_param_fw_use_npfg.get()) {
|
||||
_npfg.setDt(dt);
|
||||
|
||||
} else {
|
||||
_l1_control.set_dt(dt);
|
||||
}
|
||||
|
||||
/* restore TECS parameters, in case changed intermittently (e.g. in landing handling) */
|
||||
_tecs.set_speed_weight(_param_fw_t_spdweight.get());
|
||||
|
||||
@@ -1868,7 +1836,7 @@ FixedwingPositionControl::control_auto_landing(const hrt_abstime &now, const Vec
|
||||
}
|
||||
|
||||
const float airspeed_land = _param_fw_lnd_airspd_sc.get() * _param_fw_airspd_min.get();
|
||||
float target_airspeed = get_auto_airspeed_setpoint(now, airspeed_land, ground_speed, dt);
|
||||
float target_airspeed = get_auto_airspeed_setpoint(control_interval, airspeed_land, ground_speed);
|
||||
|
||||
const float throttle_land = _param_fw_thr_min.get() + (_param_fw_thr_max.get() - _param_fw_thr_min.get()) * 0.1f;
|
||||
|
||||
@@ -1916,7 +1884,8 @@ FixedwingPositionControl::control_auto_landing(const hrt_abstime &now, const Vec
|
||||
_att_sp.yaw_body = _yaw; // yaw is not controlled, so set setpoint to current yaw
|
||||
}
|
||||
|
||||
tecs_update_pitch_throttle(now, terrain_alt + flare_curve_alt_rel,
|
||||
tecs_update_pitch_throttle(control_interval,
|
||||
terrain_alt + flare_curve_alt_rel,
|
||||
target_airspeed,
|
||||
radians(_param_fw_lnd_fl_pmin.get()),
|
||||
radians(_param_fw_lnd_fl_pmax.get()),
|
||||
@@ -1985,7 +1954,7 @@ FixedwingPositionControl::control_auto_landing(const hrt_abstime &now, const Vec
|
||||
}
|
||||
|
||||
const float airspeed_approach = _param_fw_lnd_airspd_sc.get() * _param_fw_airspd_min.get();
|
||||
float target_airspeed = get_auto_airspeed_setpoint(now, airspeed_approach, ground_speed, dt);
|
||||
float target_airspeed = get_auto_airspeed_setpoint(control_interval, airspeed_approach, ground_speed);
|
||||
|
||||
/* lateral guidance */
|
||||
if (_param_fw_use_npfg.get()) {
|
||||
@@ -2019,7 +1988,8 @@ FixedwingPositionControl::control_auto_landing(const hrt_abstime &now, const Vec
|
||||
|
||||
_att_sp.yaw_body = _yaw; // yaw is not controlled, so set setpoint to current yaw
|
||||
|
||||
tecs_update_pitch_throttle(now, altitude_desired,
|
||||
tecs_update_pitch_throttle(control_interval,
|
||||
altitude_desired,
|
||||
target_airspeed,
|
||||
radians(_param_fw_p_lim_min.get()),
|
||||
radians(_param_fw_p_lim_max.get()),
|
||||
@@ -2045,11 +2015,10 @@ FixedwingPositionControl::control_auto_landing(const hrt_abstime &now, const Vec
|
||||
}
|
||||
|
||||
void
|
||||
FixedwingPositionControl::control_manual_altitude(const hrt_abstime &now, const Vector2d &curr_pos,
|
||||
FixedwingPositionControl::control_manual_altitude(const float control_interval, const Vector2d &curr_pos,
|
||||
const Vector2f &ground_speed)
|
||||
{
|
||||
/* ALTITUDE CONTROL: pitch stick moves altitude setpoint, throttle stick sets airspeed */
|
||||
_last_time_position_control_called = now;
|
||||
|
||||
/* Get demanded airspeed */
|
||||
float altctrl_airspeed = get_manual_airspeed_setpoint();
|
||||
@@ -2079,7 +2048,8 @@ FixedwingPositionControl::control_manual_altitude(const hrt_abstime &now, const
|
||||
throttle_max = 0.0f;
|
||||
}
|
||||
|
||||
tecs_update_pitch_throttle(now, altitude_sp_amsl,
|
||||
tecs_update_pitch_throttle(control_interval,
|
||||
altitude_sp_amsl,
|
||||
altctrl_airspeed,
|
||||
radians(_param_fw_p_lim_min.get()),
|
||||
radians(_param_fw_p_lim_max.get()),
|
||||
@@ -2112,19 +2082,9 @@ FixedwingPositionControl::control_manual_altitude(const hrt_abstime &now, const
|
||||
}
|
||||
|
||||
void
|
||||
FixedwingPositionControl::control_manual_position(const hrt_abstime &now, const Vector2d &curr_pos,
|
||||
FixedwingPositionControl::control_manual_position(const float control_interval, const Vector2d &curr_pos,
|
||||
const Vector2f &ground_speed)
|
||||
{
|
||||
const float dt = update_position_control_mode_timestep(now);
|
||||
|
||||
// update lateral guidance timesteps for slewrates
|
||||
if (_param_fw_use_npfg.get()) {
|
||||
_npfg.setDt(dt);
|
||||
|
||||
} else {
|
||||
_l1_control.set_dt(dt);
|
||||
}
|
||||
|
||||
// if we assume that user is taking off then help by demanding altitude setpoint well above ground
|
||||
// and set limit to pitch angle to prevent steering into ground
|
||||
// this will only affect planes and not VTOL
|
||||
@@ -2218,7 +2178,8 @@ FixedwingPositionControl::control_manual_position(const hrt_abstime &now, const
|
||||
}
|
||||
}
|
||||
|
||||
tecs_update_pitch_throttle(now, altitude_sp_amsl,
|
||||
tecs_update_pitch_throttle(control_interval,
|
||||
altitude_sp_amsl,
|
||||
target_airspeed,
|
||||
radians(_param_fw_p_lim_min.get()),
|
||||
radians(_param_fw_p_lim_max.get()),
|
||||
@@ -2240,9 +2201,9 @@ FixedwingPositionControl::control_manual_position(const hrt_abstime &now, const
|
||||
float roll_sp_new = _manual_control_setpoint.y * radians(_param_fw_r_lim.get());
|
||||
const float roll_rate_slew_rad = radians(_param_fw_l1_r_slew_max.get());
|
||||
|
||||
if (dt > 0.f && roll_rate_slew_rad > 0.f) {
|
||||
roll_sp_new = constrain(roll_sp_new, _att_sp.roll_body - roll_rate_slew_rad * dt,
|
||||
_att_sp.roll_body + roll_rate_slew_rad * dt);
|
||||
if (control_interval > 0.f && roll_rate_slew_rad > 0.f) {
|
||||
roll_sp_new = constrain(roll_sp_new, _att_sp.roll_body - roll_rate_slew_rad * control_interval,
|
||||
_att_sp.roll_body + roll_rate_slew_rad * control_interval);
|
||||
}
|
||||
|
||||
_att_sp.roll_body = roll_sp_new;
|
||||
@@ -2303,6 +2264,10 @@ FixedwingPositionControl::Run()
|
||||
|
||||
if (_local_pos_sub.update(&_local_pos)) {
|
||||
|
||||
const float control_interval = math::constrain((_local_pos.timestamp - _last_time_position_control_called) * 1e-6f,
|
||||
MIN_AUTO_TIMESTEP, MAX_AUTO_TIMESTEP);
|
||||
_last_time_position_control_called = _local_pos.timestamp;
|
||||
|
||||
// check for parameter updates
|
||||
if (_parameter_update_sub.updated()) {
|
||||
// clear update
|
||||
@@ -2443,42 +2408,52 @@ FixedwingPositionControl::Run()
|
||||
|
||||
set_control_mode_current(_local_pos.timestamp, _pos_sp_triplet.current.valid);
|
||||
|
||||
// update lateral guidance timesteps for slewrates
|
||||
if (_param_fw_use_npfg.get()) {
|
||||
_npfg.setDt(control_interval);
|
||||
|
||||
} else {
|
||||
_l1_control.set_dt(control_interval);
|
||||
}
|
||||
|
||||
_att_sp.fw_control_yaw = false; // by default we don't want yaw to be contoller directly with rudder
|
||||
|
||||
switch (_control_mode_current) {
|
||||
case FW_POSCTRL_MODE_AUTO: {
|
||||
control_auto(_local_pos.timestamp, curr_pos, ground_speed, _pos_sp_triplet.previous, _pos_sp_triplet.current,
|
||||
control_auto(control_interval, curr_pos, ground_speed, _pos_sp_triplet.previous, _pos_sp_triplet.current,
|
||||
_pos_sp_triplet.next);
|
||||
break;
|
||||
}
|
||||
|
||||
case FW_POSCTRL_MODE_AUTO_ALTITUDE: {
|
||||
control_auto_fixed_bank_alt_hold(_local_pos.timestamp);
|
||||
control_auto_fixed_bank_alt_hold(control_interval);
|
||||
break;
|
||||
}
|
||||
|
||||
case FW_POSCTRL_MODE_AUTO_CLIMBRATE: {
|
||||
control_auto_descend(_local_pos.timestamp);
|
||||
control_auto_descend(control_interval);
|
||||
break;
|
||||
}
|
||||
|
||||
case FW_POSCTRL_MODE_AUTO_LANDING: {
|
||||
control_auto_landing(_local_pos.timestamp, curr_pos, ground_speed, _pos_sp_triplet.previous, _pos_sp_triplet.current);
|
||||
control_auto_landing(_local_pos.timestamp, control_interval, curr_pos, ground_speed, _pos_sp_triplet.previous,
|
||||
_pos_sp_triplet.current);
|
||||
break;
|
||||
}
|
||||
|
||||
case FW_POSCTRL_MODE_AUTO_TAKEOFF: {
|
||||
control_auto_takeoff(_local_pos.timestamp, curr_pos, ground_speed, _pos_sp_triplet.previous, _pos_sp_triplet.current);
|
||||
control_auto_takeoff(_local_pos.timestamp, control_interval, curr_pos, ground_speed, _pos_sp_triplet.previous,
|
||||
_pos_sp_triplet.current);
|
||||
break;
|
||||
}
|
||||
|
||||
case FW_POSCTRL_MODE_MANUAL_POSITION: {
|
||||
control_manual_position(_local_pos.timestamp, curr_pos, ground_speed);
|
||||
control_manual_position(control_interval, curr_pos, ground_speed);
|
||||
break;
|
||||
}
|
||||
|
||||
case FW_POSCTRL_MODE_MANUAL_ALTITUDE: {
|
||||
control_manual_altitude(_local_pos.timestamp, curr_pos, ground_speed);
|
||||
control_manual_altitude(control_interval, curr_pos, ground_speed);
|
||||
break;
|
||||
}
|
||||
|
||||
@@ -2614,15 +2589,12 @@ FixedwingPositionControl::get_nav_speed_2d(const Vector2f &ground_speed)
|
||||
}
|
||||
|
||||
void
|
||||
FixedwingPositionControl::tecs_update_pitch_throttle(const hrt_abstime &now, float alt_sp, float airspeed_sp,
|
||||
FixedwingPositionControl::tecs_update_pitch_throttle(const float control_interval, float alt_sp, float airspeed_sp,
|
||||
float pitch_min_rad, float pitch_max_rad,
|
||||
float throttle_min, float throttle_max, float throttle_cruise,
|
||||
bool climbout_mode, float climbout_pitch_min_rad,
|
||||
bool disable_underspeed_detection, float hgt_rate_sp)
|
||||
{
|
||||
const float dt = math::constrain((now - _last_tecs_update) * 1e-6f, MIN_AUTO_TIMESTEP, MAX_AUTO_TIMESTEP);
|
||||
_last_tecs_update = now;
|
||||
|
||||
// do not run TECS if we are not in air
|
||||
bool run_tecs = !_landed;
|
||||
|
||||
@@ -2650,7 +2622,7 @@ FixedwingPositionControl::tecs_update_pitch_throttle(const hrt_abstime &now, flo
|
||||
|
||||
} else if (_was_in_transition) {
|
||||
// after transition we ramp up desired airspeed from the speed we had coming out of the transition
|
||||
_asp_after_transition += dt * 2.0f; // increase 2m/s
|
||||
_asp_after_transition += control_interval * 2.0f; // increase 2m/s
|
||||
|
||||
if (_asp_after_transition < airspeed_sp && _airspeed < airspeed_sp) {
|
||||
airspeed_sp = max(_asp_after_transition, _airspeed);
|
||||
@@ -2706,14 +2678,21 @@ FixedwingPositionControl::tecs_update_pitch_throttle(const hrt_abstime &now, flo
|
||||
}
|
||||
|
||||
_tecs.update_pitch_throttle(_pitch - radians(_param_fw_psp_off.get()),
|
||||
_current_altitude, alt_sp,
|
||||
airspeed_sp, _airspeed, _eas2tas,
|
||||
_current_altitude,
|
||||
alt_sp,
|
||||
airspeed_sp,
|
||||
_airspeed,
|
||||
_eas2tas,
|
||||
climbout_mode,
|
||||
climbout_pitch_min_rad - radians(_param_fw_psp_off.get()),
|
||||
throttle_min, throttle_max, throttle_cruise,
|
||||
throttle_min,
|
||||
throttle_max,
|
||||
throttle_cruise,
|
||||
pitch_min_rad - radians(_param_fw_psp_off.get()),
|
||||
pitch_max_rad - radians(_param_fw_psp_off.get()),
|
||||
_param_climbrate_target.get(), _param_sinkrate_target.get(), hgt_rate_sp);
|
||||
_param_climbrate_target.get(),
|
||||
_param_sinkrate_target.get(),
|
||||
hgt_rate_sp);
|
||||
|
||||
tecs_status_publish();
|
||||
}
|
||||
|
||||
Reference in New Issue
Block a user