From cb7aabf93291ee90c5768c1b8b0680c57817ac87 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Sat, 6 Feb 2021 14:11:17 -0500 Subject: [PATCH] fw_pos_control_l1: cleanup body velocity and acceleration usage - minimize storing unnecessary state (msg copies) --- src/lib/tecs/TECS.cpp | 13 +-- src/lib/tecs/TECS.hpp | 6 +- .../FixedwingPositionControl.cpp | 101 +++++++++--------- .../FixedwingPositionControl.hpp | 33 +++--- 4 files changed, 75 insertions(+), 78 deletions(-) diff --git a/src/lib/tecs/TECS.cpp b/src/lib/tecs/TECS.cpp index d470e8d675..2c444cf191 100644 --- a/src/lib/tecs/TECS.cpp +++ b/src/lib/tecs/TECS.cpp @@ -56,9 +56,7 @@ static constexpr float DT_MAX = 1.0f; ///< max value of _dt allowed before a fil * which is used by the airspeed complimentary filter. */ void TECS::update_vehicle_state_estimates(float equivalent_airspeed, const float speed_deriv_forward, - bool altitude_lock, - bool in_air, - float altitude, float vz) + bool altitude_lock, bool in_air, float altitude, float vz) { // calculate the time lapsed since the last update uint64_t now = hrt_absolute_time(); @@ -265,7 +263,7 @@ void TECS::_update_energy_estimates() _SKE_rate = _tas_state * _speed_derivative;// kinetic energy rate of change } -void TECS::_update_throttle_setpoint(const float throttle_cruise, const matrix::Dcmf &rotMat) +void TECS::_update_throttle_setpoint(const float throttle_cruise) { // Calculate demanded rate of change of total energy, respecting vehicle limits. // We will constrain the value below. @@ -287,8 +285,7 @@ void TECS::_update_throttle_setpoint(const float throttle_cruise, const matrix:: // Adjust the demanded total energy rate to compensate for induced drag rise in turns. // Assume induced drag scales linearly with normal load factor. // The additional normal load factor is given by (1/cos(bank angle) - 1) - const float cosPhi = constrain(sqrtf((rotMat(0, 1) * rotMat(0, 1)) + (rotMat(1, 1) * rotMat(1, 1))), 0.1f, 1.0f); - STE_rate_setpoint = STE_rate_setpoint + _load_factor_correction * (1.0f / cosPhi - 1.0f); + STE_rate_setpoint = STE_rate_setpoint + _load_factor_correction * (_load_factor - 1.f); STE_rate_setpoint = constrain(STE_rate_setpoint, _STE_rate_min, _STE_rate_max); @@ -533,7 +530,7 @@ void TECS::_update_STE_rate_lim() _STE_rate_min = - _min_sink_rate * CONSTANTS_ONE_G; } -void TECS::update_pitch_throttle(const matrix::Dcmf &rotMat, float pitch, float baro_altitude, float hgt_setpoint, +void TECS::update_pitch_throttle(float pitch, float baro_altitude, float hgt_setpoint, float EAS_setpoint, float equivalent_airspeed, float eas_to_tas, bool climb_out_setpoint, float pitch_min_climbout, float throttle_min, float throttle_max, float throttle_cruise, float pitch_limit_min, float pitch_limit_max) { @@ -580,7 +577,7 @@ void TECS::update_pitch_throttle(const matrix::Dcmf &rotMat, float pitch, float _update_energy_estimates(); // Calculate the throttle demand - _update_throttle_setpoint(throttle_cruise, rotMat); + _update_throttle_setpoint(throttle_cruise); // Calculate the pitch demand _update_pitch_setpoint(); diff --git a/src/lib/tecs/TECS.hpp b/src/lib/tecs/TECS.hpp index 3e47406630..2c39d931ce 100644 --- a/src/lib/tecs/TECS.hpp +++ b/src/lib/tecs/TECS.hpp @@ -81,7 +81,7 @@ public: /** * Update the control loop calculations */ - void update_pitch_throttle(const matrix::Dcmf &rotMat, float pitch, float baro_altitude, float hgt_setpoint, + void update_pitch_throttle(float pitch, float baro_altitude, float hgt_setpoint, float EAS_setpoint, float equivalent_airspeed, float eas_to_tas, bool climb_out_setpoint, float pitch_min_climbout, float throttle_min, float throttle_setpoint_max, float throttle_cruise, float pitch_limit_min, float pitch_limit_max); @@ -126,6 +126,7 @@ public: void set_throttle_slewrate(float slewrate) { _throttle_slewrate = slewrate; } void set_roll_throttle_compensation(float compensation) { _load_factor_correction = compensation; } + void set_load_factor(float load_factor) { _load_factor = load_factor; } void set_ste_rate_time_const(float time_const) { _STE_rate_time_const = time_const; } void set_speed_derivative_time_constant(float time_const) { _speed_derivative_time_const = time_const; } @@ -212,6 +213,7 @@ private: float _integrator_gain_throttle{0.0f}; ///< integrator gain used by the throttle demand calculation float _integrator_gain_pitch{0.0f}; ///< integrator gain used by the pitch demand calculation float _vert_accel_limit{0.0f}; ///< magnitude of the maximum vertical acceleration allowed (m/sec**2) + float _load_factor{0.0f}; ///< additional normal load factor float _load_factor_correction{0.0f}; ///< gain from normal load factor increase to total energy rate demand (m**2/sec**3) float _pitch_speed_weight{1.0f}; ///< speed control weighting used by pitch demand calculation float _height_error_gain{0.2f}; ///< height error inverse time constant [1/s] @@ -324,7 +326,7 @@ private: /** * Update throttle setpoint */ - void _update_throttle_setpoint(float throttle_cruise, const matrix::Dcmf &rotMat); + void _update_throttle_setpoint(float throttle_cruise); /** * Detect an uncommanded descent diff --git a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp b/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp index d75603d8fc..49a1b3b4b4 100644 --- a/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp +++ b/src/modules/fw_pos_control_l1/FixedwingPositionControl.cpp @@ -182,10 +182,10 @@ void FixedwingPositionControl::airspeed_poll() { bool airspeed_valid = _airspeed_valid; + airspeed_validated_s airspeed_validated; - if ((_param_fw_arsp_mode.get() == 0) && _airspeed_validated_sub.update()) { + if ((_param_fw_arsp_mode.get() == 0) && _airspeed_validated_sub.update(&airspeed_validated)) { - const airspeed_validated_s &airspeed_validated = _airspeed_validated_sub.get(); _eas2tas = 1.0f; //this is the default value, taken in case of invalid airspeed if (PX4_ISFINITE(airspeed_validated.calibrated_airspeed_m_s) @@ -235,21 +235,37 @@ FixedwingPositionControl::manual_control_setpoint_poll() void FixedwingPositionControl::vehicle_attitude_poll() { - if (_vehicle_attitude_sub.update(&_att)) { - /* set rotation matrix and euler angles */ - _R_nb = Quatf(_att.q); + vehicle_attitude_s att; + + if (_vehicle_attitude_sub.update(&att)) { + vehicle_angular_velocity_s angular_velocity{}; + _vehicle_angular_velocity_sub.copy(&angular_velocity); + const Vector3f rates{angular_velocity.xyz}; + + Dcmf R{Quatf(att.q)}; // if the vehicle is a tailsitter we have to rotate the attitude by the pitch offset // between multirotor and fixed wing flight if (_vtol_tailsitter) { - Dcmf R_offset = Eulerf(0, M_PI_2_F, 0); - _R_nb = _R_nb * R_offset; + const Dcmf R_offset{Eulerf{0.f, M_PI_2_F, 0.f}}; + R = R * R_offset; + + _yawrate = rates(0); + + } else { + _yawrate = rates(2); } - const Eulerf euler_angles(_R_nb); - _roll = euler_angles(0); - _pitch = euler_angles(1); - _yaw = euler_angles(2); + const Eulerf euler_angles(R); + _pitch = euler_angles(1); + _yaw = euler_angles(2); + + _body_acceleration = R.transpose() * Vector3f{_local_pos.ax, _local_pos.ay, _local_pos.az}; + _body_velocity = R.transpose() * Vector3f{_local_pos.vx, _local_pos.vy, _local_pos.vz}; + + // update TECS load factor + const float load_factor = 1.f / cosf(euler_angles(0)); + _tecs.set_load_factor(load_factor); } } @@ -303,18 +319,13 @@ FixedwingPositionControl::calculate_target_airspeed(float airspeed_demand, const // groundspeed undershoot if (!_l1_control.circle_mode()) { - - // rotate ground speed vector with current attitude - Vector2f yaw_vector(_R_nb(0, 0), _R_nb(1, 0)); - yaw_vector.normalize(); - - const float ground_speed_body = yaw_vector * ground_speed; - /* * This error value ensures that a plane (as long as its throttle capability is * not exceeded) travels towards a waypoint (and is not pushed more and more away * by wind). Not countering this would lead to a fly-away. */ + const float ground_speed_body = _body_velocity(0); + if (ground_speed_body < _param_fw_gnd_spd_min.get()) { airspeed_demand += max(_param_fw_gnd_spd_min.get() - ground_speed_body, 0.0f); } @@ -328,7 +339,7 @@ FixedwingPositionControl::calculate_target_airspeed(float airspeed_demand, const void FixedwingPositionControl::tecs_status_publish() { - tecs_status_s t = {}; + tecs_status_s t{}; switch (_tecs.tecs_mode()) { case TECS::ECL_TECS_MODE_NORMAL: @@ -616,14 +627,14 @@ FixedwingPositionControl::control_position(const hrt_abstime &now, const Vector2 float throttle_max = 1.0f; /* save time when airplane is in air */ - if (!_was_in_air && !_vehicle_land_detected.landed) { + if (!_was_in_air && !_landed) { _was_in_air = true; _time_went_in_air = now; _takeoff_ground_alt = _current_altitude; } /* reset flag when airplane landed */ - if (_vehicle_land_detected.landed) { + if (_landed) { _was_in_air = false; } @@ -944,7 +955,7 @@ FixedwingPositionControl::control_position(const hrt_abstime &now, const Vector2 /* throttle limiting */ throttle_max = _param_fw_thr_max.get(); - if (_vehicle_land_detected.landed && (fabsf(_manual_control_setpoint_airspeed) < THROTTLE_THRESH)) { + if (_landed && (fabsf(_manual_control_setpoint_airspeed) < THROTTLE_THRESH)) { throttle_max = 0.0f; } @@ -964,7 +975,7 @@ FixedwingPositionControl::control_position(const hrt_abstime &now, const Vector2 fabsf(_manual_control_setpoint.r) < HDG_HOLD_MAN_INPUT_THRESH) { /* heading / roll is zero, lock onto current heading */ - if (fabsf(_vehicle_rates_sub.get().xyz[2]) < HDG_HOLD_YAWRATE_THRESH && !_yaw_lock_engaged) { + if (fabsf(_yawrate) < HDG_HOLD_YAWRATE_THRESH && !_yaw_lock_engaged) { // little yaw movement, lock to current heading _yaw_lock_engaged = true; @@ -1046,7 +1057,7 @@ FixedwingPositionControl::control_position(const hrt_abstime &now, const Vector2 /* throttle limiting */ throttle_max = _param_fw_thr_max.get(); - if (_vehicle_land_detected.landed && (fabsf(_manual_control_setpoint_airspeed) < THROTTLE_THRESH)) { + if (_landed && (fabsf(_manual_control_setpoint_airspeed) < THROTTLE_THRESH)) { throttle_max = 0.0f; } @@ -1107,7 +1118,7 @@ FixedwingPositionControl::control_position(const hrt_abstime &now, const Vector2 } else { /* Copy thrust and pitch values from tecs */ - if (_vehicle_land_detected.landed) { + if (_landed) { // when we are landed state we want the motor to spin at idle speed _att_sp.thrust_body[0] = min(_param_fw_thr_idle.get(), throttle_max); @@ -1179,8 +1190,7 @@ FixedwingPositionControl::control_takeoff(const hrt_abstime &now, const Vector2f if (_runway_takeoff.runwayTakeoffEnabled()) { if (!_runway_takeoff.isInitialized()) { - Eulerf euler(Quatf(_att.q)); - _runway_takeoff.init(now, euler.psi(), _current_latitude, _current_longitude); + _runway_takeoff.init(now, _yaw, _current_latitude, _current_longitude); /* need this already before takeoff is detected * doesn't matter if it gets reset when takeoff is detected eventually */ @@ -1240,7 +1250,7 @@ FixedwingPositionControl::control_takeoff(const hrt_abstime &now, const Vector2f } /* Detect launch using body X (forward) acceleration */ - _launchDetector.update(now, _vehicle_acceleration_sub.get().xyz[0]); + _launchDetector.update(now, _body_acceleration(0)); /* update our copy of the launch detection state */ _launch_detection_state = _launchDetector.getLaunchDetected(); @@ -1672,10 +1682,16 @@ FixedwingPositionControl::Run() vehicle_attitude_poll(); vehicle_command_poll(); vehicle_control_mode_poll(); - _vehicle_land_detected_sub.update(&_vehicle_land_detected); + + if (_vehicle_land_detected_sub.updated()) { + vehicle_land_detected_s vehicle_land_detected; + + if (_vehicle_land_detected_sub.update(&vehicle_land_detected)) { + _landed = vehicle_land_detected.landed; + } + } + _vehicle_status_sub.update(&_vehicle_status); - _vehicle_acceleration_sub.update(); - _vehicle_rates_sub.update(); Vector2f curr_pos((float)_current_latitude, (float)_current_longitude); Vector2f ground_speed(_local_pos.vx, _local_pos.vy); @@ -1740,7 +1756,7 @@ void FixedwingPositionControl::reset_takeoff_state(bool force) { // only reset takeoff if !armed or just landed - if (!_control_mode.flag_armed || (_was_in_air && _vehicle_land_detected.landed) || force) { + if (!_control_mode.flag_armed || (_was_in_air && _landed) || force) { _runway_takeoff.reset(); @@ -1785,7 +1801,7 @@ FixedwingPositionControl::tecs_update_pitch_throttle(const hrt_abstime &now, flo _last_tecs_update = now; // do not run TECS if we are not in air - bool run_tecs = !_vehicle_land_detected.landed; + bool run_tecs = !_landed; // do not run TECS if vehicle is a VTOL and we are in rotary wing mode or in transition // (it should also not run during VTOL blending because airspeed is too low still) @@ -1849,28 +1865,15 @@ FixedwingPositionControl::tecs_update_pitch_throttle(const hrt_abstime &now, flo /* Using tecs library */ float pitch_for_tecs = _pitch - radians(_param_fw_psp_off.get()); - // calculate speed derivative in forward direction, which is used by complimentary filter to calculate filtered airspeed - Vector3f speed_deriv_body = _R_nb.transpose() * Vector3f(_local_pos.ax, _local_pos.ay, _local_pos.az); - float speed_deriv_forward; - - // tailsitters use the multicopter frame as reference, in fixed wing - // we need to use the fixed wing frame - if (_vtol_tailsitter) { - speed_deriv_forward = -speed_deriv_body(2); - - } else { - speed_deriv_forward = speed_deriv_body(0); - } - /* tell TECS to update its state, but let it know when it cannot actually control the plane */ - bool in_air_alt_control = (!_vehicle_land_detected.landed && + bool in_air_alt_control = (!_landed && (_control_mode.flag_control_auto_enabled || _control_mode.flag_control_offboard_enabled || _control_mode.flag_control_velocity_enabled || _control_mode.flag_control_altitude_enabled)); /* update TECS vehicle state estimates */ - _tecs.update_vehicle_state_estimates(_airspeed, speed_deriv_forward, (_local_pos.timestamp > 0), in_air_alt_control, + _tecs.update_vehicle_state_estimates(_airspeed, _body_acceleration(0), (_local_pos.timestamp > 0), in_air_alt_control, _current_altitude, _local_pos.vz); /* scale throttle cruise by baro pressure */ @@ -1889,7 +1892,7 @@ FixedwingPositionControl::tecs_update_pitch_throttle(const hrt_abstime &now, flo } } - _tecs.update_pitch_throttle(_R_nb, pitch_for_tecs, + _tecs.update_pitch_throttle(pitch_for_tecs, _current_altitude, alt_sp, airspeed_sp, _airspeed, _eas2tas, climbout_mode, climbout_pitch_min_rad, diff --git a/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp b/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp index 9c2271490e..28ef26b1d7 100644 --- a/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp +++ b/src/modules/fw_pos_control_l1/FixedwingPositionControl.hpp @@ -76,7 +76,6 @@ #include #include #include -#include #include #include #include @@ -102,8 +101,6 @@ using matrix::Vector2f; using matrix::Vector3f; using matrix::wrap_pi; -using uORB::SubscriptionData; - using namespace launchdetection; using namespace runwaytakeoff; using namespace time_literals; @@ -150,16 +147,17 @@ private: uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; - uORB::Subscription _control_mode_sub{ORB_ID(vehicle_control_mode)}; ///< control mode subscription + uORB::Subscription _airspeed_validated_sub{ORB_ID(airspeed_validated)}; + uORB::Subscription _control_mode_sub{ORB_ID(vehicle_control_mode)}; uORB::Subscription _global_pos_sub{ORB_ID(vehicle_global_position)}; - uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)}; ///< notification of manual control updates + uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)}; uORB::Subscription _pos_sp_triplet_sub{ORB_ID(position_setpoint_triplet)}; uORB::Subscription _vehicle_air_data_sub{ORB_ID(vehicle_air_data)}; - uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)}; ///< vehicle attitude subscription - uORB::Subscription _vehicle_command_sub{ORB_ID(vehicle_command)}; ///< vehicle command subscription - uORB::Subscription _vehicle_land_detected_sub{ORB_ID(vehicle_land_detected)}; ///< vehicle land detected subscription - uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)}; ///< vehicle status subscription - uORB::SubscriptionData _vehicle_rates_sub{ORB_ID(vehicle_angular_velocity)}; + uORB::Subscription _vehicle_angular_velocity_sub{ORB_ID(vehicle_angular_velocity)}; + uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)}; + uORB::Subscription _vehicle_command_sub{ORB_ID(vehicle_command)}; + uORB::Subscription _vehicle_land_detected_sub{ORB_ID(vehicle_land_detected)}; + uORB::Subscription _vehicle_status_sub{ORB_ID(vehicle_status)}; uORB::Publication _attitude_sp_pub; uORB::Publication _pos_ctrl_status_pub{ORB_ID(position_controller_status)}; ///< navigation capabilities publication @@ -168,16 +166,11 @@ private: manual_control_setpoint_s _manual_control_setpoint {}; ///< r/c channel data position_setpoint_triplet_s _pos_sp_triplet {}; ///< triplet of mission items - vehicle_attitude_s _att {}; ///< vehicle attitude setpoint vehicle_attitude_setpoint_s _att_sp {}; ///< vehicle attitude setpoint vehicle_control_mode_s _control_mode {}; ///< control mode vehicle_local_position_s _local_pos {}; ///< vehicle local position - vehicle_land_detected_s _vehicle_land_detected {}; ///< vehicle land detected vehicle_status_s _vehicle_status {}; ///< vehicle status - SubscriptionData _airspeed_validated_sub{ORB_ID(airspeed_validated)}; - SubscriptionData _vehicle_acceleration_sub{ORB_ID(vehicle_acceleration)}; - double _current_latitude{0}; double _current_longitude{0}; float _current_altitude{0.f}; @@ -199,6 +192,8 @@ private: hrt_abstime _control_position_last_called{0}; ///< last call of control_position + bool _landed{true}; + /* Landing */ bool _land_noreturn_horizontal{false}; bool _land_noreturn_vertical{false}; @@ -237,12 +232,12 @@ private: float _airspeed{0.0f}; float _eas2tas{1.0f}; - float _groundspeed_undershoot{0.0f}; ///< ground speed error to min. speed in m/s - - Dcmf _R_nb; ///< current attitude - float _roll{0.0f}; float _pitch{0.0f}; float _yaw{0.0f}; + float _yawrate{0.0f}; + + matrix::Vector3f _body_acceleration{}; + matrix::Vector3f _body_velocity{}; bool _reinitialize_tecs{true}; ///< indicates if the TECS states should be reinitialized (used for VTOL) bool _is_tecs_running{false};