From e02c64c98fbedbe0ff7b8510711bb5b46de3c7e0 Mon Sep 17 00:00:00 2001 From: RomanBapst Date: Thu, 23 Jan 2025 16:37:50 +0300 Subject: [PATCH] addressed review comments Signed-off-by: RomanBapst --- ROMFS/px4fmu_common/init.d/rc.fw_apps | 1 + msg/FwLongitudinalControlSetpoint.msg | 2 +- src/lib/npfg/DirectionalGuidance.cpp | 114 +++++------ src/lib/npfg/DirectionalGuidance.hpp | 191 ++++++++++-------- .../FwLateralLongitudinalControl.cpp | 4 +- .../FixedwingPositionControl.cpp | 33 ++- 6 files changed, 184 insertions(+), 161 deletions(-) diff --git a/ROMFS/px4fmu_common/init.d/rc.fw_apps b/ROMFS/px4fmu_common/init.d/rc.fw_apps index 81af5e1acd..26fbf19771 100644 --- a/ROMFS/px4fmu_common/init.d/rc.fw_apps +++ b/ROMFS/px4fmu_common/init.d/rc.fw_apps @@ -16,6 +16,7 @@ control_allocator start fw_rate_control start fw_att_control start fw_pos_control start +fw_lat_control start airspeed_selector start # diff --git a/msg/FwLongitudinalControlSetpoint.msg b/msg/FwLongitudinalControlSetpoint.msg index eeaefd3e9f..346c945257 100644 --- a/msg/FwLongitudinalControlSetpoint.msg +++ b/msg/FwLongitudinalControlSetpoint.msg @@ -2,8 +2,8 @@ uint64 timestamp # Note: If not both pitch_sp and throttle_sp are finite, then either altitude_setpoint or height_rate_setpoint must be finite -float32 height_rate_setpoint # NAN if not controlled directly, used as feeforward if altitude_setpoint is finite [m/s] float32 altitude_setpoint # NAN if not controlled, MSL [m] +float32 height_rate_setpoint # NAN if not controlled directly, used as feedforward if altitude_setpoint is finite [m/s] float32 equivalent_airspeed_setpoint # [m/s] float32 pitch_sp # NAN if not controlled, overrides total energy controller [rad] float32 thrust_sp # NAN if not controlled, overrides total energy controller [0,1] diff --git a/src/lib/npfg/DirectionalGuidance.cpp b/src/lib/npfg/DirectionalGuidance.cpp index 880049e70f..4c19c82366 100644 --- a/src/lib/npfg/DirectionalGuidance.cpp +++ b/src/lib/npfg/DirectionalGuidance.cpp @@ -56,67 +56,52 @@ DirectionalGuidance::guideToPath(const Vector2f &curr_pos_local, const Vector2f const float wind_speed = wind_vel.norm(); const Vector2f path_pos_to_vehicle{curr_pos_local - position_on_path}; - _signed_track_error = unit_path_tangent.cross(path_pos_to_vehicle); + signed_track_error_ = unit_path_tangent.cross(path_pos_to_vehicle); // on-track wind triangle projections const float wind_cross_upt = wind_vel.cross(unit_path_tangent); const float wind_dot_upt = wind_vel.dot(unit_path_tangent); // calculate the bearing feasibility on the track at the current closest point - _feas_on_track = bearingFeasibility(wind_cross_upt, wind_dot_upt, airspeed, wind_speed); + feas_on_track_ = bearingFeasibility(wind_cross_upt, wind_dot_upt, airspeed, wind_speed); - const float track_error = fabsf(_signed_track_error); + const float track_error = fabsf(signed_track_error_); // update control parameters considering upper and lower stability bounds (if enabled) // must be called before trackErrorBound() as it updates time_const_ - _adapted_period = adaptPeriod(ground_speed, airspeed, wind_speed, track_error, - path_curvature, wind_vel, unit_path_tangent, _feas_on_track); + adapted_period_ = adaptPeriod(ground_speed, airspeed, wind_speed, track_error, + path_curvature, wind_vel, unit_path_tangent, feas_on_track_); //const float p_gain = pGain(adapted_period, damping_); - _time_const = timeConst(_adapted_period, damping_); + _time_const = timeConst(adapted_period_, damping_); // track error bound is dynamic depending on ground speed - _track_error_bound = trackErrorBound(ground_speed, _time_const); - const float normalized_track_error = normalizedTrackError(track_error, _track_error_bound); + track_error_bound_ = trackErrorBound(ground_speed, _time_const); + const float normalized_track_error = normalizedTrackError(track_error, track_error_bound_); // look ahead angle based solely on track proximity const float look_ahead_ang = lookAheadAngle(normalized_track_error); - _track_proximity = trackProximity(look_ahead_ang); + track_proximity_ = trackProximity(look_ahead_ang); - _bearing_vec = bearingVec(unit_path_tangent, look_ahead_ang, _signed_track_error); + bearing_vec_ = bearingVec(unit_path_tangent, look_ahead_ang, signed_track_error_); // wind triangle projections - const float wind_cross_bearing = wind_vel.cross(_bearing_vec); - const float wind_dot_bearing = wind_vel.dot(_bearing_vec); + const float wind_cross_bearing = wind_vel.cross(bearing_vec_); + const float wind_dot_bearing = wind_vel.dot(bearing_vec_); // continuous representation of the bearing feasibility - _feas = bearingFeasibility(wind_cross_bearing, wind_dot_bearing, airspeed, wind_speed); + feas_ = bearingFeasibility(wind_cross_bearing, wind_dot_bearing, airspeed, wind_speed); // we consider feasibility of both the current bearing as well as that on the track at the current closest point - const float feas_combined = _feas * _feas_on_track; + const float feas_combined = feas_ * feas_on_track_; // lateral acceleration needed to stay on curved track (assuming no heading error) const float lateral_accel_ff = lateralAccelFF(unit_path_tangent, ground_vel, wind_dot_upt, - wind_cross_upt, airspeed, wind_speed, _signed_track_error, path_curvature) * feas_combined * _track_proximity; + wind_cross_upt, airspeed, wind_speed, signed_track_error_, path_curvature) * feas_combined * track_proximity_; - return DirectionalGuidanceOutput{.course_setpoint = atan2f(_bearing_vec(1), _bearing_vec(0)), + return DirectionalGuidanceOutput{.course_setpoint = atan2f(bearing_vec_(1), bearing_vec_(0)), .lateral_acceleration_feedforward = lateral_accel_ff}; } -float -DirectionalGuidance::bearingFeasibility(float wind_cross_bearing, const float wind_dot_bearing, const float airspeed, - const float wind_speed) const -{ - if (wind_dot_bearing < 0.0f) { - wind_cross_bearing = wind_speed; - - } else { - wind_cross_bearing = fabsf(wind_cross_bearing); - } - - float sin_arg = sinf(M_PI_F * 0.5f * math::constrain((airspeed - wind_cross_bearing) / AIRSPEED_BUFFER, 0.0f, 1.0f)); - return sin_arg * sin_arg; -} - float DirectionalGuidance::adaptPeriod(const float ground_speed, const float airspeed, const float wind_speed, const float track_error, const float path_curvature, const Vector2f &wind_vel, const Vector2f &unit_path_tangent, const float feas_on_track) const @@ -178,6 +163,13 @@ float DirectionalGuidance::adaptPeriod(const float ground_speed, const float air return period; } +float DirectionalGuidance::normalizedTrackError(const float track_error, const float track_error_bound) const +{ + return math::constrain(track_error / track_error_bound, 0.0f, 1.0f); +} + + + float DirectionalGuidance::windFactor(const float airspeed, const float wind_speed) const { // See [TODO: include citation] for definition/elaboration of this approximation. @@ -189,6 +181,18 @@ float DirectionalGuidance::windFactor(const float airspeed, const float wind_spe } } +float DirectionalGuidance::periodUpperBound(const float air_turn_rate, const float wind_factor, + const float feas_on_track) const +{ + if (air_turn_rate * wind_factor > NPFG_EPSILON) { + // multiply air turn rate by feasibility on track to zero out when we anyway + // should not consider the curvature + return 4.0f * M_PI_F * damping_ / (air_turn_rate * wind_factor * feas_on_track); + } + + return INFINITY; +} + float DirectionalGuidance::periodLowerBound(const float air_turn_rate, const float wind_factor, const float feas_on_track) const { @@ -210,21 +214,10 @@ float DirectionalGuidance::periodLowerBound(const float air_turn_rate, const flo } } -float DirectionalGuidance::periodUpperBound(const float air_turn_rate, const float wind_factor, - const float feas_on_track) const +float DirectionalGuidance::trackProximity(const float look_ahead_ang) const { - if (air_turn_rate * wind_factor > NPFG_EPSILON) { - // multiply air turn rate by feasibility on track to zero out when we anyway - // should not consider the curvature - return 4.0f * M_PI_F * damping_ / (air_turn_rate * wind_factor * feas_on_track); - } - - return INFINITY; -} - -float DirectionalGuidance::timeConst(const float period, const float damping) const -{ - return period * damping; + const float sin_look_ahead_ang = sinf(look_ahead_ang); + return sin_look_ahead_ang * sin_look_ahead_ang; } float DirectionalGuidance::trackErrorBound(const float ground_speed, const float time_const) const @@ -239,9 +232,9 @@ float DirectionalGuidance::trackErrorBound(const float ground_speed, const float } } -float DirectionalGuidance::normalizedTrackError(const float track_error, const float track_error_bound) const +float DirectionalGuidance::timeConst(const float period, const float damping) const { - return math::constrain(track_error / track_error_bound, 0.0f, 1.0f); + return period * damping; } float DirectionalGuidance::lookAheadAngle(const float normalized_track_error) const @@ -249,11 +242,6 @@ float DirectionalGuidance::lookAheadAngle(const float normalized_track_error) co return M_PI_2_F * (normalized_track_error - 1.0f) * (normalized_track_error - 1.0f); } -float DirectionalGuidance::trackProximity(const float look_ahead_ang) const -{ - const float sin_look_ahead_ang = sinf(look_ahead_ang); - return sin_look_ahead_ang * sin_look_ahead_ang; -} matrix::Vector2f DirectionalGuidance::bearingVec(const Vector2f &unit_path_tangent, const float look_ahead_ang, const float signed_track_error) const @@ -267,6 +255,21 @@ matrix::Vector2f DirectionalGuidance::bearingVec(const Vector2f &unit_path_tange return cos_look_ahead_ang * unit_track_error + sin_look_ahead_ang * unit_path_tangent; } +float +DirectionalGuidance::bearingFeasibility(float wind_cross_bearing, const float wind_dot_bearing, const float airspeed, + const float wind_speed) const +{ + if (wind_dot_bearing < 0.0f) { + wind_cross_bearing = wind_speed; + + } else { + wind_cross_bearing = fabsf(wind_cross_bearing); + } + + float sin_arg = sinf(M_PI_F * 0.5f * math::constrain((airspeed - wind_cross_bearing) / AIRSPEED_BUFFER, 0.0f, 1.0f)); + return sin_arg * sin_arg; +} + float DirectionalGuidance::lateralAccelFF(const Vector2f &unit_path_tangent, const Vector2f &ground_vel, const float wind_dot_upt, const float wind_cross_upt, const float airspeed, const float wind_speed, const float signed_track_error, @@ -308,12 +311,7 @@ float DirectionalGuidance::projectAirspOnBearing(const float airspeed, const flo return sqrtf(math::max(airspeed * airspeed - wind_cross_bearing * wind_cross_bearing, 0.0f)); } -float DirectionalGuidance::getBearing() -{ - return atan2f(_bearing_vec(1), _bearing_vec(0)); -} - float DirectionalGuidance::switchDistance(float wp_radius) const { - return math::min(wp_radius, _track_error_bound * _switch_distance_multiplier); + return math::min(wp_radius, track_error_bound_ * switch_distance_multiplier_); } diff --git a/src/lib/npfg/DirectionalGuidance.hpp b/src/lib/npfg/DirectionalGuidance.hpp index ab8c3aff3a..5896d3a94e 100644 --- a/src/lib/npfg/DirectionalGuidance.hpp +++ b/src/lib/npfg/DirectionalGuidance.hpp @@ -103,11 +103,15 @@ public: */ void setRollTimeConst(float tc) { roll_time_const_ = tc; } + /* + * Set the switch distance multiplier. + */ + void setSwitchDistanceMultiplier(float mult) { switch_distance_multiplier_ = math::max(mult, 0.1f); } + /* * Set the period safety factor. */ void setPeriodSafetyFactor(float sf) { period_safety_factor_ = math::max(sf, 1.0f); } - void setSwitchDistanceMultiplier(float mult) { _switch_distance_multiplier = math::max(mult, 0.1f); } /* * [Copied directly from ECL_L1_Pos_Controller] * @@ -120,34 +124,47 @@ public: */ float switchDistance(float wp_radius) const; - float getBearing(); - private: - static constexpr float AIRSPEED_BUFFER = 1.5f; // airspeed buffer [m/s] (must be > 0) static constexpr float NPFG_EPSILON = 1.0e-6; // small number *bigger than machine epsilon static constexpr float MIN_RADIUS = 0.5f; // minimum effective radius (avoid singularities) [m] + static constexpr float AIRSPEED_BUFFER = 1.5f; // airspeed buffer [m/s] (must be > 0) + float period_{10.0f}; // nominal (desired) period -- user defined [s] float damping_{0.7071f}; // nominal (desired) damping ratio -- user defined - - float period_safety_factor_{1.5f}; // multiplied by the minimum period for conservative lower bound - bool en_period_lb_{true}; // enables automatic lower bound constraints on controller period - bool en_period_ub_{true}; // enables automatic upper bound constraints on controller period (remains disabled if lower bound is disabled) - float roll_time_const_{0.0f}; // autopilot roll response time constant [s] - - float _signed_track_error; - float _feas_on_track; - float _adapted_period; - float _time_const{7.0f}; - float _track_error_bound; - float _track_proximity; - matrix::Vector2f _bearing_vec; - float _feas; - float _switch_distance_multiplier{0.32f}; // a value multiplied by the track error boundary resulting in a lower switch distance + float _time_const{7.0f}; // time constant (computed from period_ and damping_) [s] + float adapted_period_{10.0f}; // auto-adapted period (if stability bounds enabled) [s] /* * user defined guidance settings */ + // guidance options + bool en_period_lb_{true}; // enables automatic lower bound constraints on controller period + bool en_period_ub_{true}; // enables automatic upper bound constraints on controller period (remains disabled if lower bound is disabled) + + // guidance settings + float roll_time_const_{0.0f}; // autopilot roll response time constant [s] + + // guidance parameters + float switch_distance_multiplier_{0.32f}; // a value multiplied by the track error boundary resulting in a lower switch distance + // ^as the bearing angle changes quadratically (instead of linearly as in L1), the time constant (automatically calculated for on track stability) proportional track error boundary typically over estimates the required switching distance + float period_safety_factor_{1.5f}; // multiplied by the minimum period for conservative lower bound + + /* + * internal guidance states + */ + + //bearing feasibility + float feas_{1.0f}; // continous representation of bearing feasibility in [0,1] (0=infeasible, 1=feasible) + float feas_on_track_{1.0f}; // continuous bearing feasibility "on track" + + // track proximity + float track_error_bound_{212.13f}; // the current ground speed dependent track error bound [m] + float track_proximity_{0.0f}; // value in [0,1] indicating proximity to track, 0 = at track error boundary or beyond, 1 = on track + float signed_track_error_{0.0f}; // signed track error [m] + matrix::Vector2f bearing_vec_{matrix::Vector2f{1.0f, 0.0f}}; // bearing unit vector + + /* * Cacluates a continuous representation of the bearing feasibility from [0,1]. * 0 = infeasible, 1 = fully feasible, partial feasibility in between. @@ -178,16 +195,6 @@ private: float adaptPeriod(const float ground_speed, const float airspeed, const float wind_speed, const float track_error, const float path_curvature, const matrix::Vector2f &wind_vel, const matrix::Vector2f &unit_path_tangent, const float feas_on_track) const; - /* - * Calculates a ground speed modulated track error bound under which the - * look ahead angle is quadratically transitioned from alignment with the - * track error vector to that of the path tangent vector. - * - * @param[in] ground_speed Vehicle ground speed [m/s] - * @param[in] time_const Controller time constant [s] - * @return Track error boundary [m] - */ - float trackErrorBound(const float ground_speed, const float time_const) const; /* * Returns normalized (unitless) and constrained track error [0,1]. * @@ -196,14 +203,40 @@ private: * @return Normalized track error */ float normalizedTrackError(const float track_error, const float track_error_bound) const; + /* - * Cacluates the look ahead angle as a quadratic function of the normalized - * track error. + * Cacluates an approximation of the wind factor (see [TODO: include citation]). * - * @param[in] normalized_track_error Normalized track error (track error / track error boundary) - * @return Look ahead angle [rad] + * @param[in] airspeed Vehicle true airspeed [m/s] + * @param[in] wind_speed Wind speed [m/s] + * @return Non-dimensional wind factor approximation */ - float lookAheadAngle(const float normalized_track_error) const; + float windFactor(const float airspeed, const float wind_speed) const; + + /* + * Calculates a theoretical upper bound on the user defined period to maintain + * track keeping stability. + * + * @param[in] air_turn_rate The turn rate required to track the current path + * curvature at the current true airspeed, in a no-wind condition [rad/s] + * @param[in] wind_factor Non-dimensional wind factor (see [TODO: include citation]) + * @return Period upper bound [s] + */ + float periodUpperBound(const float air_turn_rate, const float wind_factor, const float feas_on_track) const; + + /* + * Calculates a theoretical lower bound on the user defined period to avoid + * limit cycle oscillations considering an acceleration actuation delay (e.g. + * roll response delay). Note this lower bound defines *marginal stability, + * and a safety factor should be applied in addition to the returned value. + * + * @param[in] air_turn_rate The turn rate required to track the current path + * curvature at the current true airspeed, in a no-wind condition [rad/s] + * @param[in] wind_factor Non-dimensional wind factor (see [TODO: include citation]) + * @return Period lower bound [s] + */ + float periodLowerBound(const float air_turn_rate, const float wind_factor, const float feas_on_track) const; + /* * Computes a continous non-dimensional track proximity [0,1] - 0 when the * vehicle is at the track error boundary, and 1 when on track. @@ -213,6 +246,37 @@ private: * @return Track proximity */ float trackProximity(const float look_ahead_ang) const; + + /* + * Calculates a ground speed modulated track error bound under which the + * look ahead angle is quadratically transitioned from alignment with the + * track error vector to that of the path tangent vector. + * + * @param[in] ground_speed Vehicle ground speed [m/s] + * @param[in] time_const Controller time constant [s] + * @return Track error boundary [m] + */ + float trackErrorBound(const float ground_speed, const float time_const) const; + + /* + * Calculates the required controller time constant to achieve the desired + * system period and damping ratio. NOTE: actual period and damping will vary + * when following paths with curvature in wind. + * + * @param[in] period Desired system period [s] + * @param[in] damping Desired system damping ratio + * @return Time constant [s] + */ + float timeConst(const float period, const float damping) const; + + /* + * Cacluates the look ahead angle as a quadratic function of the normalized + * track error. + * + * @param[in] normalized_track_error Normalized track error (track error / track error boundary) + * @return Look ahead angle [rad] + */ + float lookAheadAngle(const float normalized_track_error) const; /* * Calculates the bearing vector and track proximity transitioning variable * from the look-ahead angle mapping. @@ -227,6 +291,16 @@ private: */ matrix::Vector2f bearingVec(const matrix::Vector2f &unit_path_tangent, const float look_ahead_ang, const float signed_track_error) const; + + /* + * Projection of the air velocity vector onto the bearing line considering + * a connected wind triangle. + * + * @param[in] airspeed Vehicle true airspeed [m/s] + * @param[in] wind_cross_bearing 2D cross product of wind velocity and bearing vector [m/s] + * @return Projection of air velocity vector on bearing vector [m/s] + */ + float projectAirspOnBearing(const float airspeed, const float wind_cross_bearing) const; /* * Calculates an additional feed-forward lateral acceleration demand considering * the path curvature. @@ -245,55 +319,6 @@ private: float lateralAccelFF(const matrix::Vector2f &unit_path_tangent, const matrix::Vector2f &ground_vel, const float wind_dot_upt, const float wind_cross_upt, const float airspeed, const float wind_speed, const float signed_track_error, const float path_curvature) const; - /* - * Cacluates an approximation of the wind factor (see [TODO: include citation]). - * - * @param[in] airspeed Vehicle true airspeed [m/s] - * @param[in] wind_speed Wind speed [m/s] - * @return Non-dimensional wind factor approximation - */ - float windFactor(const float airspeed, const float wind_speed) const; - /* - * Calculates a theoretical lower bound on the user defined period to avoid - * limit cycle oscillations considering an acceleration actuation delay (e.g. - * roll response delay). Note this lower bound defines *marginal stability, - * and a safety factor should be applied in addition to the returned value. - * - * @param[in] air_turn_rate The turn rate required to track the current path - * curvature at the current true airspeed, in a no-wind condition [rad/s] - * @param[in] wind_factor Non-dimensional wind factor (see [TODO: include citation]) - * @return Period lower bound [s] - */ - float periodLowerBound(const float air_turn_rate, const float wind_factor, const float feas_on_track) const; - /* - * Calculates a theoretical upper bound on the user defined period to maintain - * track keeping stability. - * - * @param[in] air_turn_rate The turn rate required to track the current path - * curvature at the current true airspeed, in a no-wind condition [rad/s] - * @param[in] wind_factor Non-dimensional wind factor (see [TODO: include citation]) - * @return Period upper bound [s] - */ - float periodUpperBound(const float air_turn_rate, const float wind_factor, const float feas_on_track) const; - /* - * Calculates the required controller time constant to achieve the desired - * system period and damping ratio. NOTE: actual period and damping will vary - * when following paths with curvature in wind. - * - * @param[in] period Desired system period [s] - * @param[in] damping Desired system damping ratio - * @return Time constant [s] - */ - float timeConst(const float period, const float damping) const; - /* - * Projection of the air velocity vector onto the bearing line considering - * a connected wind triangle. - * - * @param[in] airspeed Vehicle true airspeed [m/s] - * @param[in] wind_cross_bearing 2D cross product of wind velocity and bearing vector [m/s] - * @return Projection of air velocity vector on bearing vector [m/s] - */ - float projectAirspOnBearing(const float airspeed, const float wind_cross_bearing) const; }; diff --git a/src/modules/fw_lateral_longitudinal_control/FwLateralLongitudinalControl.cpp b/src/modules/fw_lateral_longitudinal_control/FwLateralLongitudinalControl.cpp index 3915c00047..2c9e3879a5 100644 --- a/src/modules/fw_lateral_longitudinal_control/FwLateralLongitudinalControl.cpp +++ b/src/modules/fw_lateral_longitudinal_control/FwLateralLongitudinalControl.cpp @@ -62,7 +62,7 @@ static constexpr uint64_t ROLL_WARNING_TIMEOUT = 2_s; static constexpr float ROLL_WARNING_CAN_RUN_THRESHOLD = 0.9f; 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}; -const fw_longitudinal_control_setpoint_s empty_longitudinal_control_setpoint = {.timestamp = 0, .height_rate_setpoint = NAN, .altitude_setpoint = NAN, .equivalent_airspeed_setpoint = NAN, .pitch_sp = NAN, .thrust_sp = NAN}; +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}; FwLateralLongitudinalControl::FwLateralLongitudinalControl(bool is_vtol) : ModuleParams(nullptr), @@ -202,8 +202,8 @@ void FwLateralLongitudinalControl::Run() fw_longitudinal_control_setpoint_s longitudinal_control_status { .timestamp = hrt_absolute_time(), - .height_rate_setpoint = _tecs.getStatus().control.altitude_rate_control, .altitude_setpoint = longitudinal_sp.altitude_setpoint, + .height_rate_setpoint = _tecs.getStatus().control.altitude_rate_control, .equivalent_airspeed_setpoint = _tecs.getStatus().true_airspeed_sp / _long_control_state.eas2tas, }; diff --git a/src/modules/fw_pos_control/FixedwingPositionControl.cpp b/src/modules/fw_pos_control/FixedwingPositionControl.cpp index 5bb206a476..1fc32b9e26 100644 --- a/src/modules/fw_pos_control/FixedwingPositionControl.cpp +++ b/src/modules/fw_pos_control/FixedwingPositionControl.cpp @@ -49,7 +49,7 @@ using matrix::Vector3f; 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, .height_rate_setpoint = NAN, .altitude_setpoint = NAN, .equivalent_airspeed_setpoint = NAN, .pitch_sp = NAN, .thrust_sp = NAN}; +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}; FixedwingPositionControl::FixedwingPositionControl(bool vtol) : ModuleParams(nullptr), WorkItem(MODULE_NAME, px4::wq_configurations::nav_and_controllers), @@ -279,7 +279,6 @@ FixedwingPositionControl::vehicle_attitude_poll() } const Eulerf euler_angles(R); - euler_angles(1); _yaw = euler_angles(2); Vector3f body_acceleration = R.transpose() * Vector3f{_local_pos.ax, _local_pos.ay, _local_pos.az}; @@ -776,8 +775,8 @@ FixedwingPositionControl::control_auto_fixed_bank_alt_hold(const float control_i { const fw_longitudinal_control_setpoint_s fw_longitudinal_control_sp = { .timestamp = hrt_absolute_time(), - .height_rate_setpoint = NAN, .altitude_setpoint = _current_altitude, + .height_rate_setpoint = NAN, .equivalent_airspeed_setpoint = _performance_model.getCalibratedTrimAirspeed(), .pitch_sp = NAN, .thrust_sp = NAN @@ -818,8 +817,8 @@ FixedwingPositionControl::control_auto_descend(const float control_interval) const fw_longitudinal_control_setpoint_s fw_longitudinal_control_sp = { .timestamp = hrt_absolute_time(), - .height_rate_setpoint = -descend_rate, .altitude_setpoint = _current_altitude, + .height_rate_setpoint = -descend_rate, .equivalent_airspeed_setpoint = _performance_model.getCalibratedTrimAirspeed(), .pitch_sp = NAN, .thrust_sp = NAN @@ -951,8 +950,8 @@ FixedwingPositionControl::control_auto_position(const float control_interval, co const fw_longitudinal_control_setpoint_s fw_longitudinal_control_sp = { .timestamp = hrt_absolute_time(), - .height_rate_setpoint = NAN, .altitude_setpoint = position_sp_alt, + .height_rate_setpoint = NAN, .equivalent_airspeed_setpoint = target_airspeed, .pitch_sp = NAN, .thrust_sp = NAN @@ -1018,8 +1017,8 @@ FixedwingPositionControl::control_auto_velocity(const float control_interval, co const fw_longitudinal_control_setpoint_s fw_longitudinal_control_sp = { .timestamp = hrt_absolute_time(), - .height_rate_setpoint = pos_sp_curr.vz, .altitude_setpoint = pos_sp_curr.alt, + .height_rate_setpoint = pos_sp_curr.vz, .equivalent_airspeed_setpoint = target_airspeed, .pitch_sp = NAN, .thrust_sp = NAN @@ -1133,8 +1132,8 @@ FixedwingPositionControl::control_auto_loiter(const float control_interval, cons const fw_longitudinal_control_setpoint_s fw_longitudinal_control_sp = { .timestamp = hrt_absolute_time(), - .height_rate_setpoint = NAN, .altitude_setpoint = pos_sp_curr.alt, + .height_rate_setpoint = NAN, .equivalent_airspeed_setpoint = target_airspeed, .pitch_sp = NAN, .thrust_sp = NAN @@ -1194,8 +1193,8 @@ FixedwingPositionControl::controlAutoFigureEight(const float control_interval, c const fw_longitudinal_control_setpoint_s fw_longitudinal_control_sp = { .timestamp = hrt_absolute_time(), - .height_rate_setpoint = NAN, .altitude_setpoint = pos_sp_curr.alt, + .height_rate_setpoint = NAN, .equivalent_airspeed_setpoint = target_airspeed, .pitch_sp = NAN, .thrust_sp = NAN @@ -1263,8 +1262,8 @@ FixedwingPositionControl::control_auto_path(const float control_interval, const const fw_longitudinal_control_setpoint_s fw_longitudinal_control_sp = { .timestamp = hrt_absolute_time(), - .height_rate_setpoint = NAN, .altitude_setpoint = pos_sp_curr.alt, + .height_rate_setpoint = NAN, .equivalent_airspeed_setpoint = target_airspeed, .pitch_sp = NAN, .thrust_sp = NAN @@ -1395,8 +1394,8 @@ FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const flo const fw_longitudinal_control_setpoint_s fw_longitudinal_control_sp = { .timestamp = hrt_absolute_time(), - .height_rate_setpoint = NAN, .altitude_setpoint = altitude_setpoint_amsl, + .height_rate_setpoint = NAN, .equivalent_airspeed_setpoint = target_airspeed, .pitch_sp = _runway_takeoff.getPitch(), .thrust_sp = _runway_takeoff.getThrottle(_param_fw_thr_idle.get()) @@ -1483,8 +1482,8 @@ FixedwingPositionControl::control_auto_takeoff(const hrt_abstime &now, const flo _param_fw_thr_idle.get() : _param_fw_thr_max.get(); const fw_longitudinal_control_setpoint_s fw_longitudinal_control_sp = { .timestamp = hrt_absolute_time(), - .height_rate_setpoint = NAN, .altitude_setpoint = altitude_setpoint_amsl, + .height_rate_setpoint = NAN, .equivalent_airspeed_setpoint = target_airspeed, .pitch_sp = NAN, .thrust_sp = NAN @@ -1672,8 +1671,8 @@ FixedwingPositionControl::control_auto_landing_straight(const hrt_abstime &now, const fw_longitudinal_control_setpoint_s fw_longitudinal_control_sp = { .timestamp = hrt_absolute_time(), - .height_rate_setpoint = height_rate_setpoint, .altitude_setpoint = altitude_setpoint, + .height_rate_setpoint = height_rate_setpoint, .equivalent_airspeed_setpoint = target_airspeed, .pitch_sp = NAN, .thrust_sp = NAN @@ -1734,8 +1733,8 @@ FixedwingPositionControl::control_auto_landing_straight(const hrt_abstime &now, const fw_longitudinal_control_setpoint_s fw_longitudinal_control_sp = { .timestamp = hrt_absolute_time(), - .height_rate_setpoint = NAN, .altitude_setpoint = altitude_setpoint, + .height_rate_setpoint = NAN, .equivalent_airspeed_setpoint = target_airspeed, .pitch_sp = NAN, .thrust_sp = NAN @@ -1875,8 +1874,8 @@ 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(), - .height_rate_setpoint = height_rate_setpoint, .altitude_setpoint = _current_altitude, + .height_rate_setpoint = height_rate_setpoint, .equivalent_airspeed_setpoint = target_airspeed, .pitch_sp = NAN, .thrust_sp = NAN @@ -1927,8 +1926,8 @@ FixedwingPositionControl::control_auto_landing_circular(const hrt_abstime &now, const fw_longitudinal_control_setpoint_s fw_longitudinal_control_sp = { .timestamp = hrt_absolute_time(), - .height_rate_setpoint = -glide_slope_sink_rate, .altitude_setpoint = _current_altitude, + .height_rate_setpoint = -glide_slope_sink_rate, .equivalent_airspeed_setpoint = target_airspeed, .pitch_sp = NAN, .thrust_sp = NAN @@ -1983,8 +1982,8 @@ FixedwingPositionControl::control_manual_altitude(const float control_interval, const fw_longitudinal_control_setpoint_s fw_longitudinal_control_sp = { .timestamp = hrt_absolute_time(), - .height_rate_setpoint = height_rate_sp, .altitude_setpoint = _current_altitude, + .height_rate_setpoint = height_rate_sp, .equivalent_airspeed_setpoint = calibrated_airspeed_sp, .pitch_sp = NAN, .thrust_sp = NAN @@ -2087,8 +2086,8 @@ FixedwingPositionControl::control_manual_position(const float control_interval, const fw_longitudinal_control_setpoint_s fw_longitudinal_control_sp = { .timestamp = hrt_absolute_time(), - .height_rate_setpoint = height_rate_sp, .altitude_setpoint = _current_altitude, + .height_rate_setpoint = height_rate_sp, .equivalent_airspeed_setpoint = calibrated_airspeed_sp, .pitch_sp = NAN, .thrust_sp = NAN