Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-28 10:43:58 -04:00
Code Issues Packages Projects Releases Wiki Activity

AC_AttitudeControl: add helper functions to retrieve rotation matrices

Browse Source
This commit is contained in:
Jonathan Challinger 2015-12-02 15:47:52 -08:00 committed by Randy Mackay
parent e22fe34259
commit 3d4bd92b48
2 changed files with 55 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

37
libraries/AC_AttitudeControl/AC_AttitudeControl.cpp
View File

@ -647,6 +647,43 @@ float AC_AttitudeControl::sqrt_controller(float error, float p, float second_ord
} }
} }
void AC_AttitudeControl::get_rotation_vehicle_to_ned(Matrix3f& m)
{
m = _ahrs.get_dcm_matrix();
}
void AC_AttitudeControl::get_rotation_ned_to_vehicle(Matrix3f& m)
{
get_rotation_vehicle_to_ned(m);
m = m.transposed();
}
void AC_AttitudeControl::get_rotation_reference_to_ned(Matrix3f& m)
{
m.from_euler(_att_target_euler_rad.x,_att_target_euler_rad.y,_att_target_euler_rad.z);
}
void AC_AttitudeControl::get_rotation_ned_to_reference(Matrix3f& m)
{
get_rotation_reference_to_ned(m);
m = m.transposed();
}
void AC_AttitudeControl::get_rotation_vehicle_to_reference(Matrix3f& m)
{
Matrix3f Tvn;
Matrix3f Tnr;
get_rotation_vehicle_to_ned(Tvn);
get_rotation_ned_to_reference(Tnr);
m = Tnr * Tvn;
}
void AC_AttitudeControl::get_rotation_reference_to_vehicle(Matrix3f& m)
{
get_rotation_vehicle_to_reference(m);
m = m.transposed();
}
float AC_AttitudeControl::max_rate_step_bf_roll() float AC_AttitudeControl::max_rate_step_bf_roll()
{ {
float alpha = _pid_rate_roll.get_filt_alpha(); float alpha = _pid_rate_roll.get_filt_alpha();

18
libraries/AC_AttitudeControl/AC_AttitudeControl.h
View File

@ -214,6 +214,24 @@ public:
static const struct AP_Param::GroupInfo var_info[]; static const struct AP_Param::GroupInfo var_info[];
protected: protected:
// Retrieve a rotation matrix from the vehicle body frame to NED earth frame
void get_rotation_vehicle_to_ned(Matrix3f& m);
// Retrieve a rotation matrix from NED earth frame to the vehicle body frame
void get_rotation_ned_to_vehicle(Matrix3f& m);
// Retrieve a rotation matrix from reference (setpoint) body frame to NED earth frame
void get_rotation_reference_to_ned(Matrix3f& m);
// Retrieve a rotation matrix from NED earth frame to reference (setpoint) body frame
void get_rotation_ned_to_reference(Matrix3f& m);
// Retrieve a rotation matrix from vehicle body frame to reference (setpoint) body frame
void get_rotation_vehicle_to_reference(Matrix3f& m);
// Retrieve a rotation matrix from reference (setpoint) body frame to vehicle body frame
void get_rotation_reference_to_vehicle(Matrix3f& m);
// Update _att_target_euler_rad.x by integrating a 321-intrinsic euler roll angle derivative // Update _att_target_euler_rad.x by integrating a 321-intrinsic euler roll angle derivative
void update_att_target_and_error_roll(float euler_roll_rate_rads, Vector3f &att_error_euler_rad, float overshoot_max_rad); void update_att_target_and_error_roll(float euler_roll_rate_rads, Vector3f &att_error_euler_rad, float overshoot_max_rad);