-
Notifications
You must be signed in to change notification settings - Fork 17.3k
New issue
Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.
By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.
Already on GitHub? Sign in to your account
Plane: added fast attitude recovery for inverted flight in Q modes #28320
base: master
Are you sure you want to change the base?
Conversation
@IamPete1 @priseborough @lthall I have a chat to Leonard about this, and he suggested we run this function all the time, not just when starting VTOL control. |
ensures we can recover from inverted flight quickly
when we start the VTOL motor stabilisation with an attitude beyond normal attitude limits we will reset the thrust angle target to give faster recovery
e93be80
to
2ab01b4
Compare
The algorithm appears to be working as expected looking at the initialisation of the set-point in the log. |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Nice autotest.
this needs more discussion |
Vector3f gyro = _ahrs.get_gyro_latest(); | ||
|
||
// angle_delta is the estimated rotation that the aircraft will experience during the correction | ||
Vector3f angle_delta = Vector3f{gyro.x / get_angle_roll_p().kP(), gyro.y / get_angle_pitch_p().kP(), gyro.z / get_angle_yaw_p().kP() }; |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Dont' really want to assign from the object created on the stack
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
... and could be const?
Vector3f gyro = _ahrs.get_gyro_latest(); | ||
|
||
// angle_delta is the estimated rotation that the aircraft will experience during the correction | ||
Vector3f angle_delta = Vector3f{gyro.x / get_angle_roll_p().kP(), gyro.y / get_angle_pitch_p().kP(), gyro.z / get_angle_yaw_p().kP() }; |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Vector3f angle_delta = Vector3f{gyro.x / get_angle_roll_p().kP(), gyro.y / get_angle_pitch_p().kP(), gyro.z / get_angle_yaw_p().kP() }; | |
const Vector3f angle_delta{ | |
gyro.x / get_angle_roll_p().kP(), | |
gyro.y / get_angle_pitch_p().kP(), | |
gyro.z / get_angle_yaw_p().kP() | |
}; |
const Vector3f thrust_vector_up{0.0f, 0.0f, -1.0f}; | ||
|
||
// Rotating [0,0,-1] by attitude_body expresses (gets a view of) the thrust vector in the inertial frame | ||
Vector3f thrust_vec = attitude_body * thrust_vector_up; |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
Vector3f thrust_vec = attitude_body * thrust_vector_up; | |
const Vector3f thrust_vec {attitude_body * thrust_vector_up}; |
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
... similarly elsewhere
This makes recovery to level a lot faster from an upset attitude when using a Q mode as a recovery mode. It resets the target thrust angle to be within the limits set for the vehicle, which prevents the input shaping from trying to keep the vehicle inverted
builds on #28316
thanks to @lthall
With GriffinPro in RealFlight
without this change: