diff --git a/src/modules/fw_lateral_longitudinal_control/FwLateralLongitudinalControl.cpp b/src/modules/fw_lateral_longitudinal_control/FwLateralLongitudinalControl.cpp index b1857dc622..09fed50209 100644 --- a/src/modules/fw_lateral_longitudinal_control/FwLateralLongitudinalControl.cpp +++ b/src/modules/fw_lateral_longitudinal_control/FwLateralLongitudinalControl.cpp @@ -137,12 +137,11 @@ void FwLateralLongitudinalControl::Run() 0.001f, 0.1f); _last_time_loop_ran = _local_pos.timestamp; - _long_control_limits_sub.update(); - _lateral_control_limits_sub.update(); + updateControlLimits(); - _tecs.set_speed_weight(_long_control_limits_sub.get().speed_weight); + _tecs.set_speed_weight(_long_limits.speed_weight); updateTECSAltitudeTimeConstant(checkLowHeightConditions() - || _long_control_limits_sub.get().enforce_low_height_condition, control_interval); + || _long_limits.enforce_low_height_condition, control_interval); _tecs.set_altitude_error_time_constant(_tecs_alt_time_const_slew_rate.getState()); if (_vehicle_air_data_sub.updated()) { @@ -189,13 +188,13 @@ void FwLateralLongitudinalControl::Run() PX4_ISFINITE(_long_control_sp.equivalent_airspeed_setpoint) ? _long_control_sp.equivalent_airspeed_setpoint : _performance_model.getCalibratedTrimAirspeed(), - _long_control_limits_sub.get().pitch_min, - _long_control_limits_sub.get().pitch_max, - _long_control_limits_sub.get().throttle_min, - _long_control_limits_sub.get().throttle_max, - _long_control_limits_sub.get().sink_rate_target, - _long_control_limits_sub.get().climb_rate_target, - _long_control_limits_sub.get().disable_underspeed_protection, + _long_limits.pitch_min, + _long_limits.pitch_max, + _long_limits.throttle_min, + _long_limits.throttle_max, + _long_limits.sink_rate_target, + _long_limits.climb_rate_target, + _long_limits.disable_underspeed_protection, _long_control_sp.height_rate_setpoint ); pitch_sp = PX4_ISFINITE(_long_control_sp.pitch_sp) ? _long_control_sp.pitch_sp : _tecs.get_pitch_setpoint(); @@ -245,8 +244,8 @@ void FwLateralLongitudinalControl::Run() if (PX4_ISFINITE(lateral_accel_sp)) { lateral_accel_sp = getCorrectedLateralAccelSetpoint(lateral_accel_sp); - lateral_accel_sp = math::constrain(lateral_accel_sp, -_lateral_control_limits_sub.get().lateral_accel_max, - _lateral_control_limits_sub.get().lateral_accel_max); + lateral_accel_sp = math::constrain(lateral_accel_sp, -_lateral_limits.lateral_accel_max, + _lateral_limits.lateral_accel_max); roll_sp = mapLateralAccelerationToRollAngle(lateral_accel_sp); } @@ -293,6 +292,41 @@ void FwLateralLongitudinalControl::Run() perf_end(_loop_perf); } + +void FwLateralLongitudinalControl::updateControlLimits() +{ + if (_lateral_limits.timestamp == 0) { + _lateral_limits.timestamp = _local_pos.timestamp; + _lateral_limits.lateral_accel_max = tanf(radians(_param_fw_r_lim.get())) * CONSTANTS_ONE_G; + + } + + if (_long_limits.timestamp == 0) { + setDefaultLongitudinalControlLimits(); + } + + if (_long_control_limits_sub.updated()) { + longitudinal_control_limits_s limits_in{}; + _long_control_limits_sub.copy(&limits_in); + updateLongitudinalControlLimits(limits_in); + } + + if (_lateral_control_limits_sub.updated()) { + lateral_control_limits_s limits_in{}; + _lateral_control_limits_sub.copy(&limits_in); + _lateral_limits.timestamp = limits_in.timestamp; + + if (PX4_ISFINITE(limits_in.lateral_accel_max)) { + _lateral_limits.lateral_accel_max = min(limits_in.lateral_accel_max, tanf(radians( + _param_fw_r_lim.get())) * CONSTANTS_ONE_G); + + } else { + _lateral_limits.lateral_accel_max = tanf(radians(_param_fw_r_lim.get())) * CONSTANTS_ONE_G; + } + + } +} + void FwLateralLongitudinalControl::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, @@ -659,6 +693,71 @@ float FwLateralLongitudinalControl::mapLateralAccelerationToRollAngle(float late return atanf(lateral_acceleration_sp * 1.0f / CONSTANTS_ONE_G); } +void FwLateralLongitudinalControl::setDefaultLongitudinalControlLimits() { + _long_limits.timestamp = hrt_absolute_time(); + _long_limits.equivalent_airspeed_min = _performance_model.getMinimumCalibratedAirspeed(); + _long_limits.equivalent_airspeed_max = _performance_model.getMaximumCalibratedAirspeed(); + _long_limits.pitch_min = radians(_param_fw_p_lim_min.get()); + _long_limits.pitch_max = radians(_param_fw_p_lim_max.get()); + _long_limits.throttle_min = _param_fw_thr_min.get(); + _long_limits.throttle_max = _param_fw_thr_max.get(); + _long_limits.climb_rate_target = _param_climbrate_target.get(); + _long_limits.sink_rate_target = _param_sinkrate_target.get(); + _long_limits.disable_underspeed_protection = false; + _long_limits.enforce_low_height_condition = false; +} + +void FwLateralLongitudinalControl::updateLongitudinalControlLimits(const longitudinal_control_limits_s &limits_in) { + _long_limits.timestamp = limits_in.timestamp; + if(PX4_ISFINITE(limits_in.equivalent_airspeed_min) ) { + _long_limits.equivalent_airspeed_min = math::constrain(limits_in.equivalent_airspeed_min, _performance_model.getMinimumCalibratedAirspeed(), _performance_model.getCalibratedTrimAirspeed()); + } else { + _long_limits.equivalent_airspeed_min = _performance_model.getMinimumCalibratedAirspeed(); + } + + if (PX4_ISFINITE(limits_in.equivalent_airspeed_max)) { + _long_limits.equivalent_airspeed_max = math::constrain(limits_in.equivalent_airspeed_max, _performance_model.getCalibratedTrimAirspeed(), _performance_model.getMaximumCalibratedAirspeed()); + } else { + _long_limits.equivalent_airspeed_max = _performance_model.getMaximumCalibratedAirspeed(); + } + + if (PX4_ISFINITE(limits_in.pitch_min)) { + _long_limits.pitch_min = math::constrain(limits_in.pitch_min, radians(_param_fw_p_lim_min.get()), radians(_param_fw_p_lim_max.get())); + } else { + _long_limits.pitch_min = radians(_param_fw_p_lim_min.get()); + } + + if (PX4_ISFINITE(limits_in.pitch_max)) { + _long_limits.pitch_max = math::constrain(limits_in.pitch_max, _long_limits.pitch_min, radians(_param_fw_p_lim_max.get())); + } else { + _long_limits.pitch_max = radians(_param_fw_p_lim_max.get()); + } + + if (PX4_ISFINITE(limits_in.throttle_min)) { + _long_limits.throttle_min = math::constrain(limits_in.throttle_min, _param_fw_thr_min.get(), _param_fw_thr_max.get()); + } else { + _long_limits.throttle_min = _param_fw_thr_min.get(); + } + + if (PX4_ISFINITE(limits_in.throttle_max)) { + _long_limits.throttle_max = math::constrain(limits_in.throttle_max, _long_limits.throttle_min, _param_fw_thr_max.get()); + } else { + _long_limits.throttle_max = _param_fw_thr_max.get(); + } + + if (PX4_ISFINITE(limits_in.climb_rate_target)) { + _long_limits.climb_rate_target = math::max(0.0f, limits_in.climb_rate_target); + } else { + _long_limits.climb_rate_target = _param_climbrate_target.get(); + } + + if (PX4_ISFINITE(limits_in.sink_rate_target)) { + _long_limits.sink_rate_target = math::max(0.0f, limits_in.sink_rate_target); + } else { + _long_limits.sink_rate_target = _param_sinkrate_target.get(); + } +} + extern "C" __EXPORT int fw_lat_lon_control_main(int argc, char *argv[]) { return FwLateralLongitudinalControl::main(argc, argv); diff --git a/src/modules/fw_lateral_longitudinal_control/FwLateralLongitudinalControl.hpp b/src/modules/fw_lateral_longitudinal_control/FwLateralLongitudinalControl.hpp index 2f0aaa7add..806bb096c6 100644 --- a/src/modules/fw_lateral_longitudinal_control/FwLateralLongitudinalControl.hpp +++ b/src/modules/fw_lateral_longitudinal_control/FwLateralLongitudinalControl.hpp @@ -117,12 +117,14 @@ private: uORB::SubscriptionData _vehicle_status_sub{ORB_ID(vehicle_status)}; uORB::Subscription _fw_lateral_ctrl_sub{ORB_ID(fw_lateral_control_setpoint)}; uORB::Subscription _fw_longitudinal_ctrl_sub{ORB_ID(fw_longitudinal_control_setpoint)}; - uORB::SubscriptionData _long_control_limits_sub{ORB_ID(longitudinal_control_limits)}; - uORB::SubscriptionData _lateral_control_limits_sub{ORB_ID(lateral_control_limits)}; + uORB::Subscription _long_control_limits_sub{ORB_ID(longitudinal_control_limits)}; + uORB::Subscription _lateral_control_limits_sub{ORB_ID(lateral_control_limits)}; vehicle_local_position_s _local_pos{}; fw_longitudinal_control_setpoint_s _long_control_sp{empty_longitudinal_control_setpoint}; + longitudinal_control_limits_s _long_limits{}; fw_lateral_control_setpoint_s _lat_control_sp{empty_lateral_control_setpoint}; + lateral_control_limits_s _lateral_limits{}; uORB::Publication _attitude_sp_pub; uORB::Publication _tecs_status_pub{ORB_ID(tecs_status)}; @@ -156,6 +158,10 @@ private: (ParamFloat) _param_speed_rate_standard_dev, (ParamFloat) _param_process_noise_standard_dev, + (ParamFloat) _param_climbrate_target, + (ParamFloat) _param_sinkrate_target, + (ParamFloat) _param_fw_thr_max, + (ParamFloat) _param_fw_thr_min, (ParamFloat) _param_fw_thr_slew_max, (ParamFloat) _param_fw_thrtc_sc, (ParamFloat) _param_fw_t_thr_low_hgt @@ -228,6 +234,12 @@ private: float getGuidanceQualityFactor(const vehicle_local_position_s &local_pos, const bool is_wind_valid) const; float getCorrectedLateralAccelSetpoint(float lateral_accel_sp); + + void setDefaultLongitudinalControlLimits(); + + void updateLongitudinalControlLimits(const longitudinal_control_limits_s &limits_in); + + void updateControlLimits(); }; diff --git a/src/modules/fw_pos_control/FixedwingPositionControl.cpp b/src/modules/fw_pos_control/FixedwingPositionControl.cpp index fb84080e01..2e8dd9287e 100644 --- a/src/modules/fw_pos_control/FixedwingPositionControl.cpp +++ b/src/modules/fw_pos_control/FixedwingPositionControl.cpp @@ -50,6 +50,8 @@ using matrix::wrap_pi; const fw_lateral_control_setpoint_s empty_lateral_control_setpoint = {.timestamp = 0, .course_setpoint = NAN, .airspeed_reference_direction = NAN, .lateral_acceleration_setpoint = NAN, .roll_sp = NAN, .heading_sp_runway_takeoff = NAN, .reset_integral = false}; const fw_longitudinal_control_setpoint_s empty_longitudinal_control_setpoint = {.timestamp = 0, .altitude_setpoint = NAN, .height_rate_setpoint = NAN, .equivalent_airspeed_setpoint = NAN, .pitch_sp = NAN, .thrust_sp = NAN}; +const longitudinal_control_limits_s empty_longitudinal_control_limits = {.timestamp = 0, .pitch_min = NAN, .pitch_max = NAN, .throttle_min = NAN, .throttle_max = NAN, .climb_rate_target = NAN, .sink_rate_target = NAN, .equivalent_airspeed_min = NAN, .equivalent_airspeed_max = NAN, .speed_weight = NAN, .enforce_low_height_condition = false, .disable_underspeed_protection = false }; +const lateral_control_limits_s empty_lateral_control_limits = {.timestamp = 0, .lateral_accel_max = NAN}; FixedwingPositionControl::FixedwingPositionControl(bool vtol) : ModuleParams(nullptr), WorkItem(MODULE_NAME, px4::wq_configurations::nav_and_controllers), @@ -737,8 +739,8 @@ void FixedwingPositionControl::control_idle() lateral_ctrl_sp.lateral_acceleration_setpoint = 0.0f; _lateral_ctrl_sp_pub.publish(lateral_ctrl_sp); - lateral_control_limits_s lateral_limits{.timestamp = hrt_absolute_time()}; - lateral_limits.lateral_accel_max = rollAngleToLateralAccel(radians(_param_fw_r_lim.get())); + lateral_control_limits_s lateral_limits = empty_lateral_control_limits; + lateral_limits.timestamp = hrt_absolute_time(); _lateral_ctrl_limits_pub.publish(lateral_limits); fw_longitudinal_control_setpoint_s long_contrl_sp {empty_longitudinal_control_setpoint}; @@ -747,29 +749,13 @@ void FixedwingPositionControl::control_idle() long_contrl_sp.thrust_sp = 0.0f; _longitudinal_ctrl_sp_pub.publish(long_contrl_sp); - longitudinal_control_limits_s longitudinal_control_limits{.timestamp = now}; - setDefaultLongControlLimits(longitudinal_control_limits); + longitudinal_control_limits_s longitudinal_control_limits = empty_longitudinal_control_limits; + longitudinal_control_limits.timestamp = now; longitudinal_control_limits.throttle_max = 0.f; longitudinal_control_limits.throttle_min = 0.f; _longitudinal_ctrl_limits_pub.publish(longitudinal_control_limits); } -void FixedwingPositionControl::setDefaultLongControlLimits(longitudinal_control_limits_s &longitudinal_control_limits) -const -{ - longitudinal_control_limits.equivalent_airspeed_min = _performance_model.getMinimumCalibratedAirspeed(getLoadFactor()); - longitudinal_control_limits.equivalent_airspeed_max = _performance_model.getMaximumCalibratedAirspeed(); - longitudinal_control_limits.pitch_min = radians(_param_fw_p_lim_min.get()); - longitudinal_control_limits.pitch_max = radians(_param_fw_p_lim_max.get()); - longitudinal_control_limits.throttle_min = _param_fw_thr_min.get(); - longitudinal_control_limits.throttle_max = _param_fw_thr_max.get(); - longitudinal_control_limits.climb_rate_target = _param_climbrate_target.get(); - longitudinal_control_limits.sink_rate_target = _param_sinkrate_target.get(); - longitudinal_control_limits.disable_underspeed_protection = false; - longitudinal_control_limits.enforce_low_height_condition = false; - longitudinal_control_limits.speed_weight = _param_fw_t_spdweight.get(); -} - void FixedwingPositionControl::control_auto_fixed_bank_alt_hold(const float control_interval) { @@ -792,8 +778,8 @@ FixedwingPositionControl::control_auto_fixed_bank_alt_hold(const float control_i throttle_max = _param_fw_thr_min.get(); } - longitudinal_control_limits_s longitudinal_control_limits{.timestamp = hrt_absolute_time()}; - setDefaultLongControlLimits(longitudinal_control_limits); + longitudinal_control_limits_s longitudinal_control_limits = empty_longitudinal_control_limits; + longitudinal_control_limits.timestamp = hrt_absolute_time(); longitudinal_control_limits.throttle_max = throttle_max; _longitudinal_ctrl_limits_pub.publish(longitudinal_control_limits); @@ -803,8 +789,8 @@ FixedwingPositionControl::control_auto_fixed_bank_alt_hold(const float control_i lateral_ctrl_sp.lateral_acceleration_setpoint = rollAngleToLateralAccel(roll_body); _lateral_ctrl_sp_pub.publish(lateral_ctrl_sp); - lateral_control_limits_s lateral_limits{.timestamp = hrt_absolute_time()}; - lateral_limits.lateral_accel_max = rollAngleToLateralAccel(radians(_param_fw_r_lim.get())); + lateral_control_limits_s lateral_limits = empty_lateral_control_limits; + lateral_limits.timestamp = hrt_absolute_time(); _lateral_ctrl_limits_pub.publish(lateral_limits); } @@ -817,7 +803,7 @@ FixedwingPositionControl::control_auto_descend(const float control_interval) const fw_longitudinal_control_setpoint_s fw_longitudinal_control_sp = { .timestamp = hrt_absolute_time(), - .altitude_setpoint = _current_altitude, + .altitude_setpoint = NAN, .height_rate_setpoint = -descend_rate, .equivalent_airspeed_setpoint = _performance_model.getCalibratedTrimAirspeed(), .pitch_sp = NAN, @@ -826,8 +812,8 @@ FixedwingPositionControl::control_auto_descend(const float control_interval) _longitudinal_ctrl_sp_pub.publish(fw_longitudinal_control_sp); - longitudinal_control_limits_s longitudinal_control_limits{.timestamp = hrt_absolute_time()}; - setDefaultLongControlLimits(longitudinal_control_limits); + longitudinal_control_limits_s longitudinal_control_limits = empty_longitudinal_control_limits; + longitudinal_control_limits.timestamp = hrt_absolute_time(); longitudinal_control_limits.throttle_max = (_landed || !_local_pos.v_z_valid) ? _param_fw_thr_min.get() : _param_fw_thr_max.get(); @@ -839,8 +825,8 @@ FixedwingPositionControl::control_auto_descend(const float control_interval) lateral_ctrl_sp.lateral_acceleration_setpoint = rollAngleToLateralAccel(roll_body); _lateral_ctrl_sp_pub.publish(lateral_ctrl_sp); - lateral_control_limits_s lateral_limits{.timestamp = hrt_absolute_time()}; - lateral_limits.lateral_accel_max = rollAngleToLateralAccel(radians(_param_fw_r_lim.get())); + lateral_control_limits_s lateral_limits = empty_lateral_control_limits; + lateral_limits.timestamp = hrt_absolute_time(); _lateral_ctrl_limits_pub.publish(lateral_limits); } @@ -901,8 +887,8 @@ FixedwingPositionControl::control_auto_position(const float control_interval, co const Vector2f &ground_speed, const position_setpoint_s &pos_sp_prev, const position_setpoint_s &pos_sp_curr) { const float acc_rad = _directional_guidance.switchDistance(500.0f); - float throttle_min = _param_fw_thr_min.get(); - float throttle_max = _param_fw_thr_max.get(); + float throttle_min = NAN; + float throttle_max = NAN; if (pos_sp_curr.gliding_enabled) { /* enable gliding with this waypoint */ @@ -959,8 +945,8 @@ FixedwingPositionControl::control_auto_position(const float control_interval, co _longitudinal_ctrl_sp_pub.publish(fw_longitudinal_control_sp); - longitudinal_control_limits_s longitudinal_control_limits{.timestamp = hrt_absolute_time()}; - setDefaultLongControlLimits(longitudinal_control_limits); + longitudinal_control_limits_s longitudinal_control_limits = empty_longitudinal_control_limits; + longitudinal_control_limits.timestamp = hrt_absolute_time(); longitudinal_control_limits.throttle_min = throttle_min; longitudinal_control_limits.throttle_max = throttle_max; longitudinal_control_limits.speed_weight = pos_sp_curr.gliding_enabled ? 2.0f : @@ -986,8 +972,8 @@ FixedwingPositionControl::control_auto_position(const float control_interval, co lateral_ctrl_sp.lateral_acceleration_setpoint = sp.lateral_acceleration_feedforward; _lateral_ctrl_sp_pub.publish(lateral_ctrl_sp); - lateral_control_limits_s lateral_limits{.timestamp = hrt_absolute_time()}; - lateral_limits.lateral_accel_max = rollAngleToLateralAccel(radians(_param_fw_r_lim.get())); + lateral_control_limits_s lateral_limits = empty_lateral_control_limits; + lateral_limits.timestamp = hrt_absolute_time(); _lateral_ctrl_limits_pub.publish(lateral_limits); } @@ -1011,8 +997,8 @@ FixedwingPositionControl::control_auto_velocity(const float control_interval, co fw_lateral_ctrl_sp.lateral_acceleration_setpoint = sp.lateral_acceleration_feedforward; _lateral_ctrl_sp_pub.publish(fw_lateral_ctrl_sp); - lateral_control_limits_s lateral_limits{.timestamp = hrt_absolute_time()}; - lateral_limits.lateral_accel_max = rollAngleToLateralAccel(radians(_param_fw_r_lim.get())); + lateral_control_limits_s lateral_limits = empty_lateral_control_limits; + lateral_limits.timestamp = hrt_absolute_time(); _lateral_ctrl_limits_pub.publish(lateral_limits); const fw_longitudinal_control_setpoint_s fw_longitudinal_control_sp = { @@ -1026,8 +1012,8 @@ FixedwingPositionControl::control_auto_velocity(const float control_interval, co _longitudinal_ctrl_sp_pub.publish(fw_longitudinal_control_sp); - longitudinal_control_limits_s longitudinal_control_limits{.timestamp = hrt_absolute_time()}; - setDefaultLongControlLimits(longitudinal_control_limits); + longitudinal_control_limits_s longitudinal_control_limits = empty_longitudinal_control_limits; + longitudinal_control_limits.timestamp = hrt_absolute_time(); if (pos_sp_curr.gliding_enabled) { longitudinal_control_limits.throttle_min = 0.f; @@ -1112,8 +1098,8 @@ FixedwingPositionControl::control_auto_loiter(const float control_interval, cons _lateral_ctrl_sp_pub.publish(fw_lateral_ctrl_sp); - lateral_control_limits_s lateral_limits{.timestamp = hrt_absolute_time()}; - lateral_limits.lateral_accel_max = rollAngleToLateralAccel(radians(_param_fw_r_lim.get())); + lateral_control_limits_s lateral_limits = empty_lateral_control_limits; + lateral_limits.timestamp = hrt_absolute_time(); if (_landing_abort_status) { if (pos_sp_curr.alt - _current_altitude < kClearanceAltitudeBuffer) { @@ -1141,8 +1127,8 @@ FixedwingPositionControl::control_auto_loiter(const float control_interval, cons _longitudinal_ctrl_sp_pub.publish(fw_longitudinal_control_sp); - longitudinal_control_limits_s longitudinal_control_limits{.timestamp = hrt_absolute_time()}; - setDefaultLongControlLimits(longitudinal_control_limits); + longitudinal_control_limits_s longitudinal_control_limits = empty_longitudinal_control_limits; + longitudinal_control_limits.timestamp = hrt_absolute_time(); longitudinal_control_limits.enforce_low_height_condition = enforce_low_height; if (pos_sp_curr.gliding_enabled) { @@ -1185,8 +1171,8 @@ FixedwingPositionControl::controlAutoFigureEight(const float control_interval, c _lateral_ctrl_sp_pub.publish(fw_lateral_ctrl_sp); - lateral_control_limits_s lateral_limits{.timestamp = hrt_absolute_time()}; - lateral_limits.lateral_accel_max = rollAngleToLateralAccel(radians(_param_fw_r_lim.get())); + lateral_control_limits_s lateral_limits = empty_lateral_control_limits; + lateral_limits.timestamp = hrt_absolute_time(); _lateral_ctrl_limits_pub.publish(lateral_limits); _closest_point_on_path = _figure_eight.getClosestPoint(); @@ -1202,8 +1188,8 @@ FixedwingPositionControl::controlAutoFigureEight(const float control_interval, c _longitudinal_ctrl_sp_pub.publish(fw_longitudinal_control_sp); - longitudinal_control_limits_s longitudinal_control_limits{.timestamp = hrt_absolute_time()}; - setDefaultLongControlLimits(longitudinal_control_limits); + longitudinal_control_limits_s longitudinal_control_limits = empty_longitudinal_control_limits; + longitudinal_control_limits.timestamp = hrt_absolute_time(); if (pos_sp_curr.gliding_enabled) { longitudinal_control_limits.throttle_min = 0.f; @@ -1256,8 +1242,8 @@ FixedwingPositionControl::control_auto_path(const float control_interval, const fw_lateral_ctrl_sp.lateral_acceleration_setpoint = sp.lateral_acceleration_feedforward; _lateral_ctrl_sp_pub.publish(fw_lateral_ctrl_sp); - lateral_control_limits_s lateral_limits{.timestamp = hrt_absolute_time()}; - lateral_limits.lateral_accel_max = rollAngleToLateralAccel(radians(_param_fw_r_lim.get())); + lateral_control_limits_s lateral_limits = empty_lateral_control_limits; + lateral_limits.timestamp = hrt_absolute_time(); _lateral_ctrl_limits_pub.publish(lateral_limits); const fw_longitudinal_control_setpoint_s fw_longitudinal_control_sp = { @@ -1271,8 +1257,8 @@ FixedwingPositionControl::control_auto_path(const float control_interval, const _longitudinal_ctrl_sp_pub.publish(fw_longitudinal_control_sp); - longitudinal_control_limits_s longitudinal_control_limits{.timestamp = hrt_absolute_time()}; - setDefaultLongControlLimits(longitudinal_control_limits); + longitudinal_control_limits_s longitudinal_control_limits = empty_longitudinal_control_limits; + longitudinal_control_limits.timestamp = hrt_absolute_time(); if (pos_sp_curr.gliding_enabled) { longitudinal_control_limits.throttle_min = 0.f; @@ -1306,8 +1292,8 @@ FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const flo float adjusted_min_airspeed = _performance_model.getMinimumCalibratedAirspeed(getLoadFactor()); - longitudinal_control_limits_s longitudinal_control_limits{.timestamp = hrt_absolute_time()}; - setDefaultLongControlLimits(longitudinal_control_limits); + longitudinal_control_limits_s longitudinal_control_limits = empty_longitudinal_control_limits; + longitudinal_control_limits.timestamp = hrt_absolute_time(); if (takeoff_airspeed < adjusted_min_airspeed) { // adjust underspeed detection bounds for takeoff airspeed @@ -1479,7 +1465,7 @@ FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const flo _lateral_ctrl_limits_pub.publish(lateral_limits); const float max_takeoff_throttle = (_launchDetector.getLaunchDetected() < launch_detection_status_s::STATE_FLYING) ? - _param_fw_thr_idle.get() : _param_fw_thr_max.get(); + _param_fw_thr_idle.get() : NAN; const fw_longitudinal_control_setpoint_s fw_longitudinal_control_sp = { .timestamp = hrt_absolute_time(), .altitude_setpoint = altitude_setpoint_amsl, @@ -1536,8 +1522,8 @@ FixedwingPositionControl::control_auto_landing_straight(const hrt_abstime &now, _performance_model.getMinimumCalibratedAirspeed(getLoadFactor()); float adjusted_min_airspeed = _performance_model.getMinimumCalibratedAirspeed(getLoadFactor()); - longitudinal_control_limits_s longitudinal_control_limits{.timestamp = hrt_absolute_time()}; - setDefaultLongControlLimits(longitudinal_control_limits); + longitudinal_control_limits_s longitudinal_control_limits = empty_longitudinal_control_limits; + longitudinal_control_limits.timestamp = hrt_absolute_time(); longitudinal_control_limits.enforce_low_height_condition = true; if (airspeed_land < adjusted_min_airspeed) { @@ -1743,7 +1729,7 @@ FixedwingPositionControl::control_auto_landing_straight(const hrt_abstime &now, _longitudinal_ctrl_sp_pub.publish(fw_longitudinal_control_sp); longitudinal_control_limits.throttle_min = _param_fw_thr_idle.get(); - longitudinal_control_limits.throttle_max = _landed ? _param_fw_thr_idle.get() : _param_fw_thr_max.get(); + longitudinal_control_limits.throttle_max = _landed ? _param_fw_thr_idle.get() : NAN; longitudinal_control_limits.sink_rate_target = desired_max_sinkrate; // enable direct yaw control using rudder/wheel @@ -1774,8 +1760,8 @@ FixedwingPositionControl::control_auto_landing_circular(const hrt_abstime &now, _performance_model.getMinimumCalibratedAirspeed(getLoadFactor()); float adjusted_min_airspeed = _performance_model.getMinimumCalibratedAirspeed(getLoadFactor()); - longitudinal_control_limits_s longitudinal_control_limits{.timestamp = hrt_absolute_time()}; - setDefaultLongControlLimits(longitudinal_control_limits); + longitudinal_control_limits_s longitudinal_control_limits = empty_longitudinal_control_limits; + longitudinal_control_limits.timestamp = hrt_absolute_time(); if (airspeed_land < adjusted_min_airspeed) { // adjust underspeed detection bounds for landing airspeed @@ -1874,7 +1860,7 @@ FixedwingPositionControl::control_auto_landing_circular(const hrt_abstime &now, _param_fw_thr_max.get(); const fw_longitudinal_control_setpoint_s fw_longitudinal_control_sp = { .timestamp = hrt_absolute_time(), - .altitude_setpoint = _current_altitude, + .altitude_setpoint = NAN, .height_rate_setpoint = height_rate_setpoint, .equivalent_airspeed_setpoint = target_airspeed, .pitch_sp = NAN, @@ -1926,7 +1912,7 @@ FixedwingPositionControl::control_auto_landing_circular(const hrt_abstime &now, const fw_longitudinal_control_setpoint_s fw_longitudinal_control_sp = { .timestamp = hrt_absolute_time(), - .altitude_setpoint = _current_altitude, + .altitude_setpoint = NAN, .height_rate_setpoint = -glide_slope_sink_rate, .equivalent_airspeed_setpoint = target_airspeed, .pitch_sp = NAN, @@ -1936,7 +1922,7 @@ FixedwingPositionControl::control_auto_landing_circular(const hrt_abstime &now, _longitudinal_ctrl_sp_pub.publish(fw_longitudinal_control_sp); longitudinal_control_limits.throttle_min = _param_fw_thr_idle.get(); - longitudinal_control_limits.throttle_max = _landed ? _param_fw_thr_idle.get() : _param_fw_thr_max.get(); + longitudinal_control_limits.throttle_max = _landed ? _param_fw_thr_idle.get() : NAN; longitudinal_control_limits.sink_rate_target = desired_max_sinkrate; _longitudinal_ctrl_limits_pub.publish(longitudinal_control_limits); } @@ -1970,10 +1956,10 @@ FixedwingPositionControl::control_manual_altitude(const float control_interval, // TECS may try to pitch down to gain airspeed if we underspeed, constrain the pitch when underspeeding if we are // just passed launch - const float min_pitch = (_completed_manual_takeoff) ? radians(_param_fw_p_lim_min.get()) : + const float min_pitch = (_completed_manual_takeoff) ? NAN : MIN_PITCH_DURING_MANUAL_TAKEOFF; - float throttle_max = _param_fw_thr_max.get(); + float throttle_max = NAN; // enable the operator to kill the throttle on ground if (_landed && (_manual_control_setpoint_for_airspeed < THROTTLE_THRESH)) { @@ -1982,7 +1968,7 @@ FixedwingPositionControl::control_manual_altitude(const float control_interval, const fw_longitudinal_control_setpoint_s fw_longitudinal_control_sp = { .timestamp = hrt_absolute_time(), - .altitude_setpoint = _current_altitude, + .altitude_setpoint = NAN, .height_rate_setpoint = height_rate_sp, .equivalent_airspeed_setpoint = calibrated_airspeed_sp, .pitch_sp = NAN, @@ -1991,8 +1977,8 @@ FixedwingPositionControl::control_manual_altitude(const float control_interval, _longitudinal_ctrl_sp_pub.publish(fw_longitudinal_control_sp); - longitudinal_control_limits_s longitudinal_control_limits{.timestamp = hrt_absolute_time()}; - setDefaultLongControlLimits(longitudinal_control_limits); + longitudinal_control_limits_s longitudinal_control_limits = empty_longitudinal_control_limits; + longitudinal_control_limits.timestamp = hrt_absolute_time(); longitudinal_control_limits.pitch_min = min_pitch; longitudinal_control_limits.throttle_max = throttle_max; _longitudinal_ctrl_limits_pub.publish(longitudinal_control_limits); @@ -2005,8 +1991,8 @@ FixedwingPositionControl::control_manual_altitude(const float control_interval, fw_lateral_ctrl_sp.lateral_acceleration_setpoint = sp.lateral_acceleration_feedforward; _lateral_ctrl_sp_pub.publish(fw_lateral_ctrl_sp); - lateral_control_limits_s lateral_limits{.timestamp = hrt_absolute_time()}; - lateral_limits.lateral_accel_max = rollAngleToLateralAccel(radians(_param_fw_r_lim.get())); + lateral_control_limits_s lateral_limits = empty_lateral_control_limits; + lateral_limits.timestamp = hrt_absolute_time(); _lateral_ctrl_limits_pub.publish(lateral_limits); } @@ -2022,10 +2008,10 @@ FixedwingPositionControl::control_manual_position(const float control_interval, // TECS may try to pitch down to gain airspeed if we underspeed, constrain the pitch when underspeeding if we are // just passed launch - const float min_pitch = (_completed_manual_takeoff) ? radians(_param_fw_p_lim_min.get()) : + const float min_pitch = (_completed_manual_takeoff) ? NAN : MIN_PITCH_DURING_MANUAL_TAKEOFF; - float throttle_max = _param_fw_thr_max.get(); + float throttle_max = NAN; // enable the operator to kill the throttle on ground if (_landed && (_manual_control_setpoint_for_airspeed < THROTTLE_THRESH)) { @@ -2086,7 +2072,7 @@ FixedwingPositionControl::control_manual_position(const float control_interval, const fw_longitudinal_control_setpoint_s fw_longitudinal_control_sp = { .timestamp = hrt_absolute_time(), - .altitude_setpoint = _current_altitude, + .altitude_setpoint = NAN, .height_rate_setpoint = height_rate_sp, .equivalent_airspeed_setpoint = calibrated_airspeed_sp, .pitch_sp = NAN, @@ -2095,8 +2081,8 @@ FixedwingPositionControl::control_manual_position(const float control_interval, _longitudinal_ctrl_sp_pub.publish(fw_longitudinal_control_sp); - longitudinal_control_limits_s longitudinal_control_limits{.timestamp = hrt_absolute_time()}; - setDefaultLongControlLimits(longitudinal_control_limits); + longitudinal_control_limits_s longitudinal_control_limits = empty_longitudinal_control_limits; + longitudinal_control_limits.timestamp = hrt_absolute_time(); longitudinal_control_limits.pitch_min = min_pitch; longitudinal_control_limits.throttle_max = throttle_max; _longitudinal_ctrl_limits_pub.publish(longitudinal_control_limits); @@ -2114,8 +2100,8 @@ FixedwingPositionControl::control_manual_position(const float control_interval, _lateral_ctrl_sp_pub.publish(fw_lateral_ctrl_sp); } - lateral_control_limits_s lateral_limits{.timestamp = hrt_absolute_time()}; - lateral_limits.lateral_accel_max = rollAngleToLateralAccel(radians(_param_fw_r_lim.get())); + lateral_control_limits_s lateral_limits = empty_lateral_control_limits; + lateral_limits.timestamp = hrt_absolute_time(); _lateral_ctrl_limits_pub.publish(lateral_limits); } @@ -2135,8 +2121,8 @@ void FixedwingPositionControl::control_backtransition_heading_hold() fw_lateral_ctrl_sp.airspeed_reference_direction = _backtrans_heading; _lateral_ctrl_sp_pub.publish(fw_lateral_ctrl_sp); - lateral_control_limits_s lateral_limits{.timestamp = hrt_absolute_time()}; - lateral_limits.lateral_accel_max = rollAngleToLateralAccel(radians(_param_fw_r_lim.get())); + lateral_control_limits_s lateral_limits = empty_lateral_control_limits; + lateral_limits.timestamp = hrt_absolute_time(); _lateral_ctrl_limits_pub.publish(lateral_limits); } @@ -2165,8 +2151,8 @@ void FixedwingPositionControl::control_backtransition_line_follow(const Vector2f fw_lateral_ctrl_sp.lateral_acceleration_setpoint = sp.lateral_acceleration_feedforward; _lateral_ctrl_sp_pub.publish(fw_lateral_ctrl_sp); - lateral_control_limits_s lateral_limits{.timestamp = hrt_absolute_time()}; - lateral_limits.lateral_accel_max = rollAngleToLateralAccel(radians(_param_fw_r_lim.get())); + lateral_control_limits_s lateral_limits = empty_lateral_control_limits; + lateral_limits.timestamp = hrt_absolute_time(); _lateral_ctrl_limits_pub.publish(lateral_limits); } @@ -2183,19 +2169,6 @@ FixedwingPositionControl::Run() /* only run controller if position changed */ - if (!_control_limits_published_once) { - longitudinal_control_limits_s longitudinal_control_limits{.timestamp = hrt_absolute_time()}; - setDefaultLongControlLimits(longitudinal_control_limits); - _longitudinal_ctrl_limits_pub.publish(longitudinal_control_limits); - - lateral_control_limits_s lateral_limits{.timestamp = hrt_absolute_time()}; - lateral_limits.lateral_accel_max = rollAngleToLateralAccel(radians(_param_fw_r_lim.get())); - _lateral_ctrl_limits_pub.publish(lateral_limits); - - _control_limits_published_once = true; - - } - if (_local_pos_sub.update(&_local_pos)) { const float control_interval = math::constrain((_local_pos.timestamp - _last_time_position_control_called) * 1e-6f, diff --git a/src/modules/fw_pos_control/FixedwingPositionControl.hpp b/src/modules/fw_pos_control/FixedwingPositionControl.hpp index fae929418c..622fe49c7e 100644 --- a/src/modules/fw_pos_control/FixedwingPositionControl.hpp +++ b/src/modules/fw_pos_control/FixedwingPositionControl.hpp @@ -228,8 +228,6 @@ private: vehicle_local_position_s _local_pos{}; vehicle_status_s _vehicle_status{}; - bool _control_limits_published_once{false}; - Vector2f _lpos_where_backtrans_started; bool _position_setpoint_previous_valid{false}; @@ -936,8 +934,6 @@ private: void control_idle(); - void setDefaultLongControlLimits(longitudinal_control_limits_s &longitudinal_control_limits) const; - float rollAngleToLateralAccel(float roll_body) const; };