Browse Source

AC_AttitudeControl: multicopter specific rate_controller_run

master
Randy Mackay 9 years ago
parent
commit
7ff0fcb25d
  1. 11
      libraries/AC_AttitudeControl/AC_AttitudeControl.cpp
  2. 2
      libraries/AC_AttitudeControl/AC_AttitudeControl.h
  3. 10
      libraries/AC_AttitudeControl/AC_AttitudeControl_Multi.cpp
  4. 2
      libraries/AC_AttitudeControl/AC_AttitudeControl_Multi.h

11
libraries/AC_AttitudeControl/AC_AttitudeControl.cpp

@ -393,17 +393,6 @@ void AC_AttitudeControl::attitude_controller_run_quat(const Quaternion& att_targ @@ -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;
}
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)
{
float sin_theta = sinf(euler_rad.y);

2
libraries/AC_AttitudeControl/AC_AttitudeControl.h

@ -136,7 +136,7 @@ public: @@ -136,7 +136,7 @@ public:
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
virtual void rate_controller_run();
virtual void rate_controller_run() = 0;
// 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);

10
libraries/AC_AttitudeControl/AC_AttitudeControl_Multi.cpp

@ -217,3 +217,13 @@ void AC_AttitudeControl_Multi::update_throttle_rpy_mix() @@ -217,3 +217,13 @@ void AC_AttitudeControl_Multi::update_throttle_rpy_mix()
}
_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

@ -74,6 +74,8 @@ public: @@ -74,6 +74,8 @@ public:
// get_throttle_rpy_mix - get low throttle compensation value
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
static const struct AP_Param::GroupInfo var_info[];

Loading…
Cancel
Save