aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AC_AttitudeControl: Support MAVLINK_MSG_ID_SET_ATTITUDE_TARGET

Browse Source
This commit is contained in:
Leonard Hall 2022-01-11 16:47:09 -05:00 committed by Andrew Tridgell
parent 94c117fe62
commit 7a9a0bfb3f
4 changed files with 30 additions and 27 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

42
libraries/AC_AttitudeControl/AC_AttitudeControl.cpp
View File

@ -210,40 +210,40 @@ void AC_AttitudeControl::reset_rate_controller_I_terms_smoothly()
// trust vector drops below 2*AC_ATTITUDE_THRUST_ERROR_ANGLE. At this point the heading is also corrected.
// Command a Quaternion attitude with feedforward and smoothing
void AC_AttitudeControl::input_quaternion(Quaternion attitude_desired_quat)
// attitude_desired_quat: is updated on each time_step (_dt) by the integral of the angular velocity
void AC_AttitudeControl::input_quaternion(Quaternion& attitude_desired_quat, Vector3f ang_vel_target)
{
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
// this function is not currently used, this is a reminder that we need to add a 6DoF implementation
AP_HAL::panic("input_quaternion not implemented AC_AttitudeControl_Multi_6DoF");
#endif
// calculate the attitude target euler angles
_attitude_target.to_euler(_euler_angle_target.x, _euler_angle_target.y, _euler_angle_target.z);
Quaternion attitude_error_quat = _attitude_target.inverse() * attitude_desired_quat;
Vector3f attitude_error_angle;
attitude_error_quat.to_axis_angle(attitude_error_angle);
// Limit the angular velocity
ang_vel_limit(ang_vel_target, radians(_ang_vel_roll_max), radians(_ang_vel_pitch_max), radians(_ang_vel_yaw_max));
if (_rate_bf_ff_enabled) {
// When acceleration limiting and feedforward are enabled, the sqrt controller is used to compute an euler
// angular velocity that will cause the euler angle to smoothly stop at the input angle with limited deceleration
// and an exponential decay specified by _input_tc at the end.
_ang_vel_target.x = input_shaping_angle(wrap_PI(attitude_error_angle.x), _input_tc, get_accel_roll_max_radss(), _ang_vel_target.x, _dt);
_ang_vel_target.y = input_shaping_angle(wrap_PI(attitude_error_angle.y), _input_tc, get_accel_pitch_max_radss(), _ang_vel_target.y, _dt);
_ang_vel_target.z = input_shaping_angle(wrap_PI(attitude_error_angle.z), _input_tc, get_accel_yaw_max_radss(), _ang_vel_target.z, _dt);
// Limit the angular velocity
ang_vel_limit(_ang_vel_target, radians(_ang_vel_roll_max), radians(_ang_vel_pitch_max), radians(_ang_vel_yaw_max));
// Convert body-frame angular velocity into euler angle derivative of desired attitude
ang_vel_to_euler_rate(_euler_angle_target, _ang_vel_target, _euler_rate_target);
_ang_vel_target.x = input_shaping_angle(wrap_PI(attitude_error_angle.x), _input_tc, get_accel_roll_max_radss(), _ang_vel_target.x, ang_vel_target.x, radians(_ang_vel_roll_max), _dt);
_ang_vel_target.y = input_shaping_angle(wrap_PI(attitude_error_angle.y), _input_tc, get_accel_pitch_max_radss(), _ang_vel_target.y, ang_vel_target.y, radians(_ang_vel_pitch_max), _dt);
_ang_vel_target.z = input_shaping_angle(wrap_PI(attitude_error_angle.z), _input_tc, get_accel_yaw_max_radss(), _ang_vel_target.z, ang_vel_target.z, radians(_ang_vel_yaw_max), _dt);
} else {
_attitude_target = attitude_desired_quat;
// Set rate feedforward requests to zero
_euler_rate_target.zero();
_ang_vel_target.zero();
_ang_vel_target = ang_vel_target;
}
// calculate the attitude target euler angles
_attitude_target.to_euler(_euler_angle_target.x, _euler_angle_target.y, _euler_angle_target.z);
// Convert body-frame angular velocity into euler angle derivative of desired attitude
ang_vel_to_euler_rate(_euler_angle_target, _ang_vel_target, _euler_rate_target);
// rotate target and normalize
Quaternion attitude_desired_update;
attitude_desired_update.from_axis_angle(ang_vel_target * _dt);
attitude_desired_quat = attitude_desired_quat * attitude_desired_update;
attitude_desired_quat.normalize();
// Call quaternion attitude controller
attitude_controller_run_quat();
}

5
libraries/AC_AttitudeControl/AC_AttitudeControl.h
View File

@ -149,7 +149,8 @@ public:
void inertial_frame_reset();
// Command a Quaternion attitude with feedforward and smoothing
virtual void input_quaternion(Quaternion attitude_desired_quat);
// attitude_desired_quat: is updated on each time_step (_dt) by the integral of the angular velocity
virtual void input_quaternion(Quaternion& attitude_desired_quat, Vector3f ang_vel_target);
// Command an euler roll and pitch angle and an euler yaw rate with angular velocity feedforward and smoothing
virtual void input_euler_angle_roll_pitch_euler_rate_yaw(float euler_roll_angle_cd, float euler_pitch_angle_cd, float euler_yaw_rate_cds);
@ -295,7 +296,7 @@ public:
// calculates the velocity correction from an angle error. The angular velocity has acceleration and
// deceleration limits including basic jerk limiting using smoothing_gain
static float input_shaping_angle(float error_angle, float input_tc, float accel_max, float target_ang_vel, float desired_ang_vel, float max_ang_vel, float dt);
static float input_shaping_angle(float error_angle, float smoothing_gain, float accel_max, float target_ang_vel, float dt){ return input_shaping_angle(error_angle, smoothing_gain, accel_max, target_ang_vel, 0.0f, 0.0f, dt); }
static float input_shaping_angle(float error_angle, float input_tc, float accel_max, float target_ang_vel, float dt){ return input_shaping_angle(error_angle, input_tc, accel_max, target_ang_vel, 0.0f, 0.0f, dt); }
// limits the acceleration and deceleration of a velocity request
static float input_shaping_ang_vel(float target_ang_vel, float desired_ang_vel, float accel_max, float dt);

7
libraries/AC_AttitudeControl/AC_AttitudeControl_Multi_6DoF.cpp
View File

@ -145,8 +145,9 @@ void AC_AttitudeControl_Multi_6DoF::input_angle_step_bf_roll_pitch_yaw(float rol
}
// Command a Quaternion attitude with feedforward and smoothing
// not used anywhere in current code, panic in SITL so this implementaiton is not overlooked
void AC_AttitudeControl_Multi_6DoF::input_quaternion(Quaternion attitude_desired_quat) {
// attitude_desired_quat: is updated on each time_step (_dt) by the integral of the angular velocity
// not used anywhere in current code, panic in SITL so this implementation is not overlooked
void AC_AttitudeControl_Multi_6DoF::input_quaternion(Quaternion& attitude_desired_quat, Vector3f ang_vel_target) {
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
AP_HAL::panic("input_quaternion not implemented AC_AttitudeControl_Multi_6DoF");
#endif
@ -154,7 +155,7 @@ void AC_AttitudeControl_Multi_6DoF::input_quaternion(Quaternion attitude_desired
_motors.set_lateral(0.0f);
_motors.set_forward(0.0f);
AC_AttitudeControl_Multi::input_quaternion(attitude_desired_quat);
AC_AttitudeControl_Multi::input_quaternion(attitude_desired_quat, ang_vel_target);
}

3
libraries/AC_AttitudeControl/AC_AttitudeControl_Multi_6DoF.h
View File

@ -19,8 +19,9 @@ public:
}
// Command a Quaternion attitude with feedforward and smoothing
// attitude_desired_quat: is updated on each time_step (_dt) by the integral of the angular velocity
// not used anywhere in current code, panic so this implementaiton is not overlooked
void input_quaternion(Quaternion attitude_desired_quat) override;
void input_quaternion(Quaternion& attitude_desired_quat, Vector3f ang_vel_target) override;
/*
override input functions to attitude controller and convert desired angles into thrust angles and substitute for osset angles
*/