diff --git a/src/modules/fw_att_control/FixedwingAttitudeControl.cpp b/src/modules/fw_att_control/FixedwingAttitudeControl.cpp index d194edd498..9dd1bd0893 100644 --- a/src/modules/fw_att_control/FixedwingAttitudeControl.cpp +++ b/src/modules/fw_att_control/FixedwingAttitudeControl.cpp @@ -85,7 +85,7 @@ FixedwingAttitudeControl::parameters_update() } void -FixedwingAttitudeControl::vehicle_manual_poll(const float yaw_body) +FixedwingAttitudeControl::vehicle_manual_poll(const float yaw_body, const float roll) { if (_vcontrol_mode.flag_control_manual_enabled && _in_fw_or_transition_wo_tailsitter_transition) { @@ -103,7 +103,9 @@ FixedwingAttitudeControl::vehicle_manual_poll(const float yaw_body) pitch_body = constrain(pitch_body, -radians(_param_fw_man_p_max.get()), radians(_param_fw_man_p_max.get())); - const Quatf q(Eulerf(roll_body, pitch_body, yaw_body)); + float yaw_sp = yaw_body + sinf(roll) * 0.0f; + + const Quatf q(Eulerf(roll_body, pitch_body, yaw_sp)); q.copyTo(_att_sp.q_d); _att_sp.thrust_body[0] = (_manual_control_setpoint.throttle + 1.f) * .5f; @@ -252,7 +254,7 @@ void FixedwingAttitudeControl::Run() const matrix::Eulerf euler_angles(_R); - vehicle_manual_poll(euler_angles.psi()); + vehicle_manual_poll(euler_angles.psi(), euler_angles.phi()); vehicle_attitude_setpoint_poll(); @@ -294,57 +296,22 @@ void FixedwingAttitudeControl::Run() const Quatf q_sp(_att_sp.q_d); if (q_sp.isAllFinite()) { + Quatf q_current(att.q); - const Quatf q_current(att.q); - const Vector3f ez_world(0.f, 0.f, 1.f); - const Vector3f ex_c = q_current.dcm_x(); - const Vector3f ez_c = q_current.dcm_z(); - const Vector3f ez_sp = q_sp.dcm_z(); - - // Tilt-only error quaternion - Quatf q_tilt_err(ez_c, ez_sp); - - // Reduced desired attitude - Quatf q_des_red = (q_tilt_err * q_current).normalized(); - Quatf q_err = (q_current.inversed() * q_des_red).canonical(); + Quatf q_err = (q_current.inversed() * q_sp).canonical(); Vector3f e = 2.f * q_err.imag(); Vector3f body_rates_setpoint; body_rates_setpoint(0) = _proportional_gain(0) * e(0); - body_rates_setpoint(1) = _proportional_gain(1) * e(1) * 2.5f; // multiplied by 2.5 to maintain the gains - body_rates_setpoint(2) = 0.f; + body_rates_setpoint(1) = _proportional_gain(1) * e(1); + body_rates_setpoint(2) = 0.f; // yaw handled later // Turn coordination const float V = math::max(get_airspeed_constrained(), 0.1f); + const float r_tc_ff = 9.81f * 2.0f * (q_current(0) * q_current(1) + q_current(2) * q_current(3)) / V; - Vector3f x_h = ex_c - ez_world * ex_c.dot(ez_world); // forward projected to horizontal - float r_tc_ff = 0.f; + body_rates_setpoint(2) = r_tc_ff; - const float xhn = x_h.norm(); - - if (xhn > 1e-3f) { - - x_h /= xhn; - Vector3f y_h = ez_world.cross(x_h); - const float yhn = y_h.norm(); - - if (yhn > 1e-6f) { - - y_h /= yhn; - const float cos_tilt = ez_c.dot(ez_world); - const float sin_bank = ez_c.dot(y_h); - - float tan_bank = 0.f; - - if (fabsf(cos_tilt) > 0.1f) { - tan_bank = sin_bank / cos_tilt; - } - - r_tc_ff = (9.81f / V) * tan_bank * 0.6f; - } - } - - body_rates_setpoint(2) = -r_tc_ff; body_rates_setpoint(0) = math::constrain(body_rates_setpoint(0), -radians(_param_fw_r_rmax.get()), radians(_param_fw_r_rmax.get())); body_rates_setpoint(1) = math::constrain(body_rates_setpoint(1), -radians(_param_fw_p_rmax_neg.get()), radians(_param_fw_p_rmax_pos.get())); diff --git a/src/modules/fw_att_control/FixedwingAttitudeControl.hpp b/src/modules/fw_att_control/FixedwingAttitudeControl.hpp index 447281aca6..6884f7cdd6 100644 --- a/src/modules/fw_att_control/FixedwingAttitudeControl.hpp +++ b/src/modules/fw_att_control/FixedwingAttitudeControl.hpp @@ -165,7 +165,7 @@ private: WheelController _wheel_ctrl; void parameters_update(); - void vehicle_manual_poll(const float yaw_body); + void vehicle_manual_poll(const float yaw_body, const float roll); void vehicle_attitude_setpoint_poll(); void vehicle_land_detected_poll(); float get_airspeed_constrained(); diff --git a/src/modules/fw_lateral_longitudinal_control/FwLateralLongitudinalControl.cpp b/src/modules/fw_lateral_longitudinal_control/FwLateralLongitudinalControl.cpp index e6cbedfea2..30d91d2b25 100644 --- a/src/modules/fw_lateral_longitudinal_control/FwLateralLongitudinalControl.cpp +++ b/src/modules/fw_lateral_longitudinal_control/FwLateralLongitudinalControl.cpp @@ -299,7 +299,7 @@ void FwLateralLongitudinalControl::Run() float roll_body = PX4_ISFINITE(roll_sp) ? roll_sp : 0.0f; float pitch_body = PX4_ISFINITE(pitch_sp) ? pitch_sp : 0.0f; - float yaw_body = _yaw; + float yaw_body = _yaw + sinf(roll_body) * 0.45f; const float thrust_body_x = PX4_ISFINITE(throttle_sp) ? throttle_sp : 0.0f; @@ -610,6 +610,7 @@ void FwLateralLongitudinalControl::updateAttitude() { const Eulerf euler_angles(R); _long_control_state.pitch_rad = euler_angles.theta(); _yaw = euler_angles.psi(); + _roll = euler_angles.phi(); // load factor due to banking const float load_factor_from_bank_angle = 1.0f / max(cosf(euler_angles.phi()), FLT_EPSILON); diff --git a/src/modules/fw_lateral_longitudinal_control/FwLateralLongitudinalControl.hpp b/src/modules/fw_lateral_longitudinal_control/FwLateralLongitudinalControl.hpp index b2205bd5a1..7ded9002ac 100644 --- a/src/modules/fw_lateral_longitudinal_control/FwLateralLongitudinalControl.hpp +++ b/src/modules/fw_lateral_longitudinal_control/FwLateralLongitudinalControl.hpp @@ -192,6 +192,7 @@ private: hrt_abstime _time_wind_last_received{0}; SlewRate _roll_slew_rate; float _yaw{0.f}; + float _roll{0.f}; struct lateral_control_state { matrix::Vector2f ground_speed; matrix::Vector2f wind_speed;