diff --git a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp b/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp index 7cac39a131..0249cef561 100644 --- a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp +++ b/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp @@ -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(); } diff --git a/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp b/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp index d6c1ba568a..d5626963cd 100644 --- a/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp +++ b/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp @@ -358,14 +358,6 @@ private: */ void update_in_air_states(const hrt_abstime now); - /** - * @brief Updates the time since the last position control call. - * - * @param now Current system time [us] - * @return Time since last position control call [s] - */ - float update_position_control_mode_timestep(const hrt_abstime now); - /** * @brief Moves the current position setpoint to a value far ahead of the current vehicle yaw when in a VTOL * transition. @@ -375,21 +367,17 @@ private: void move_position_setpoint_for_vtol_transition(position_setpoint_s ¤t_sp); uint8_t handle_setpoint_type(const uint8_t setpoint_type, const position_setpoint_s &pos_sp_curr); - void 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); + void 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); - void control_auto_fixed_bank_alt_hold(const hrt_abstime &now); - void control_auto_descend(const hrt_abstime &now); + void control_auto_fixed_bank_alt_hold(const float control_interval); + void control_auto_descend(const float control_interval); - void control_auto_position(const hrt_abstime &now, const float dt, const Vector2d &curr_pos, - const Vector2f &ground_speed, + void 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); - void control_auto_loiter(const hrt_abstime &now, const float dt, const Vector2d &curr_pos, - const Vector2f &ground_speed, + void 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); - void control_auto_velocity(const hrt_abstime &now, const float dt, const Vector2d &curr_pos, - const Vector2f &ground_speed, + void 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); /** @@ -397,26 +385,38 @@ private: * * @param now Current system time [us] * @param curr_pos Current 2D local position vector of vehicle [m] + * @param control_interval Time since the last position control update [s] * @param ground_speed Local 2D ground speed of vehicle [m/s] * @param pos_sp_prev previous position setpoint * @param pos_sp_curr current position setpoint */ - void 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); - void control_auto_landing(const hrt_abstime &now, const Vector2d &curr_pos, + void 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); + + void 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); - void control_manual_altitude(const hrt_abstime &now, const Vector2d &curr_pos, const Vector2f &ground_speed); - void control_manual_position(const hrt_abstime &now, const Vector2d &curr_pos, const Vector2f &ground_speed); + void control_manual_altitude(const float control_interval, const Vector2d &curr_pos, const Vector2f &ground_speed); + void control_manual_position(const float control_interval, const Vector2d &curr_pos, const Vector2f &ground_speed); float get_tecs_pitch(); float get_tecs_thrust(); float get_manual_airspeed_setpoint(); - float get_auto_airspeed_setpoint(const hrt_abstime &now, const float pos_sp_cru_airspeed, const Vector2f &ground_speed, - float dt); + + /** + * @brief Returns an indicated airspeed setpoint for auto modes. + * + * Adjusts the setpoint for wind, accelerated stall, and slew rates. + * + * @param control_interval Time since the last position control update [s] + * @param pos_sp_cru_airspeed The commanded cruise airspeed from the position setpoint [s] + * @param ground_speed Vehicle ground velocity vector [m/s] + * @return Indicated airspeed setpoint [m/s] + */ + float get_auto_airspeed_setpoint(const float control_interval, const float pos_sp_cru_airspeed, + const Vector2f &ground_speed); void reset_takeoff_state(bool force = false); void reset_landing_state(); @@ -431,7 +431,7 @@ private: /* * Call TECS : a wrapper function to call the TECS implementation */ - void tecs_update_pitch_throttle(const hrt_abstime &now, float alt_sp, float airspeed_sp, + void 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,