From 3124e76aaab5a5b93c55d1a5ea979ef7bf85bcbc Mon Sep 17 00:00:00 2001 From: Azarakhsh Keipour Date: Sun, 26 Jul 2020 01:23:53 -0400 Subject: [PATCH] Omni Pos-Ctrl: Added (unfiltered) roll and pitch angle estimation mode --- .../PositionControl/ControlMath.cpp | 9 +++++++++ .../mc_pos_control/mc_pos_control_main.cpp | 15 ++++++++++++--- 2 files changed, 21 insertions(+), 3 deletions(-) diff --git a/src/modules/mc_pos_control/PositionControl/ControlMath.cpp b/src/modules/mc_pos_control/PositionControl/ControlMath.cpp index cfeaad1864..d8b5a076b6 100644 --- a/src/modules/mc_pos_control/PositionControl/ControlMath.cpp +++ b/src/modules/mc_pos_control/PositionControl/ControlMath.cpp @@ -87,6 +87,15 @@ void thrustToAttitude(const Vector3f &thr_sp, const float yaw_sp, const matrix:: } case 6: { // Estimate the optimal roll and pitch to counteract the wind (used for omnidirectional vehicles) + bodyzToAttitude(-thr_sp, yaw_sp, att_sp); + att_sp.thrust_body[2] = -thr_sp.length(); + + // Set the roll angle + omni_att_roll = att_sp.roll_body; + + // Set the pitch angle + omni_att_pitch = att_sp.pitch_body; + break; } diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp index 29db1afb13..41aac99c0f 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -143,8 +143,11 @@ private: home_position_s _home_pos{}; /**< home position */ landing_gear_s _landing_gear{}; int8_t _old_landing_gear_position{landing_gear_s::GEAR_KEEP}; + float _param_tilt_angle = 0, _param_tilt_dir = 0; // Keeping the last parameter values in degrees float _tilt_angle = 0, _tilt_dir = 0; // Keeping the currently used values in radians + float _param_roll_angle = 0, _param_pitch_angle = 0; // Keeping the last parameter values in degrees + float _tilt_roll = 0, _tilt_pitch = 0; // Keeping the currently used values in radians DEFINE_PARAMETERS( // Position Control @@ -404,6 +407,14 @@ MulticopterPositionControl::parameters_update(bool force) _tilt_angle = math::radians(_param_tilt_angle); _tilt_dir = math::radians(_param_tilt_dir); } + + if (abs(_param_omni_att_roll.get() - _param_roll_angle) > FLT_EPSILON + || abs(_param_omni_att_pitch.get() - _param_pitch_angle) > FLT_EPSILON) { + _param_roll_angle = _param_omni_att_roll.get(); + _param_pitch_angle = _param_omni_att_pitch.get(); + _tilt_roll = math::radians(_param_roll_angle); + _tilt_pitch = math::radians(_param_pitch_angle); + } } return OK; @@ -674,10 +685,8 @@ MulticopterPositionControl::Run() vehicle_attitude_setpoint_s attitude_setpoint{}; attitude_setpoint.timestamp = time_stamp_now; - float omni_att_roll = _param_omni_att_roll.get(); - float omni_att_pitch = _param_omni_att_pitch.get(); _control.getAttitudeSetpoint(matrix::Quatf(att.q), _param_omni_att_mode.get(), _param_omni_dfc_max_thr.get(), - _tilt_angle, _tilt_dir, omni_att_roll, omni_att_pitch, _param_omni_proj_axes.get(), attitude_setpoint); + _tilt_angle, _tilt_dir, _tilt_roll, _tilt_pitch, _param_omni_proj_axes.get(), attitude_setpoint); // Part of landing logic: if ground-contact/maybe landed was detected, turn off // controller. This message does not have to be logged as part of the vehicle_local_position_setpoint topic.