Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-03 14:38:30 -04:00
Code Issues Packages Projects Releases Wiki Activity

AC_AttitudeControl: multicopter specific rate_controller_run

Browse Source
This commit is contained in:
Randy Mackay 2016-05-23 14:03:38 +09:00
parent b7431b7d0c
commit 7ff0fcb25d
4 changed files with 13 additions and 12 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

11
libraries/AC_AttitudeControl/AC_AttitudeControl.cpp
View File

@ -393,17 +393,6 @@ void AC_AttitudeControl::attitude_controller_run_quat(const Quaternion& att_targ
_ang_vel_target_rads += Trv * _att_target_ang_vel_rads; _ang_vel_target_rads += Trv * _att_target_ang_vel_rads;
} }
void AC_AttitudeControl::rate_controller_run()
{
// move throttle vs attitude mixing towards desired (called from here because this is conveniently called on every iteration)
update_throttle_rpy_mix();
_motors.set_roll(rate_bf_to_motor_roll(_ang_vel_target_rads.x));
_motors.set_pitch(rate_bf_to_motor_pitch(_ang_vel_target_rads.y));
_motors.set_yaw(rate_bf_to_motor_yaw(_ang_vel_target_rads.z));
control_monitor_update();
}
void AC_AttitudeControl::euler_rate_to_ang_vel(const Vector3f& euler_rad, const Vector3f& euler_rate_rads, Vector3f& ang_vel_rads) void AC_AttitudeControl::euler_rate_to_ang_vel(const Vector3f& euler_rad, const Vector3f& euler_rate_rads, Vector3f& ang_vel_rads)
{ {
float sin_theta = sinf(euler_rad.y); float sin_theta = sinf(euler_rad.y);

2
libraries/AC_AttitudeControl/AC_AttitudeControl.h
View File

@ -136,7 +136,7 @@ public:
void input_att_quat_bf_ang_vel(const Quaternion& att_target_quat, const Vector3f& att_target_ang_vel_rads); void input_att_quat_bf_ang_vel(const Quaternion& att_target_quat, const Vector3f& att_target_ang_vel_rads);
// Run angular velocity controller and send outputs to the motors // Run angular velocity controller and send outputs to the motors
virtual void rate_controller_run(); virtual void rate_controller_run() = 0;
// Convert a 321-intrinsic euler angle derivative to an angular velocity vector // Convert a 321-intrinsic euler angle derivative to an angular velocity vector
void euler_rate_to_ang_vel(const Vector3f& euler_rad, const Vector3f& euler_rate_rads, Vector3f& ang_vel_rads); void euler_rate_to_ang_vel(const Vector3f& euler_rad, const Vector3f& euler_rate_rads, Vector3f& ang_vel_rads);

10
libraries/AC_AttitudeControl/AC_AttitudeControl_Multi.cpp
View File

@ -217,3 +217,13 @@ void AC_AttitudeControl_Multi::update_throttle_rpy_mix()
} }
_throttle_rpy_mix = constrain_float(_throttle_rpy_mix, 0.1f, 1.0f); _throttle_rpy_mix = constrain_float(_throttle_rpy_mix, 0.1f, 1.0f);
} }
void AC_AttitudeControl_Multi::rate_controller_run()
{
// move throttle vs attitude mixing towards desired (called from here because this is conveniently called on every iteration)
update_throttle_rpy_mix();
_motors.set_roll(rate_bf_to_motor_roll(_ang_vel_target_rads.x));
_motors.set_pitch(rate_bf_to_motor_pitch(_ang_vel_target_rads.y));
_motors.set_yaw(rate_bf_to_motor_yaw(_ang_vel_target_rads.z));
}

2
libraries/AC_AttitudeControl/AC_AttitudeControl_Multi.h
View File

@ -74,6 +74,8 @@ public:
// get_throttle_rpy_mix - get low throttle compensation value // get_throttle_rpy_mix - get low throttle compensation value
bool is_throttle_mix_min() const { return (_throttle_rpy_mix < 1.25f*_thr_mix_min); } bool is_throttle_mix_min() const { return (_throttle_rpy_mix < 1.25f*_thr_mix_min); }
// run lowest level body-frame rate controller and send outputs to the motors
void rate_controller_run();
// user settable parameters // user settable parameters
static const struct AP_Param::GroupInfo var_info[]; static const struct AP_Param::GroupInfo var_info[];