diff --git a/ArduCopter/Attitude.cpp b/ArduCopter/Attitude.cpp index f79a555c62d88..3e71782c18c00 100644 --- a/ArduCopter/Attitude.cpp +++ b/ArduCopter/Attitude.cpp @@ -15,7 +15,9 @@ void Copter::run_rate_controller() pos_control->set_dt(last_loop_time_s); // run low level rate controllers that only require IMU data - attitude_control->rate_controller_run(); + attitude_control->rate_controller_run(); + // reset sysid and other temporary inputs + attitude_control->rate_controller_target_reset(); } /************************************************************* diff --git a/ArduPlane/quadplane.cpp b/ArduPlane/quadplane.cpp index c4b66a7eddd79..a3c27cc4fd35a 100644 --- a/ArduPlane/quadplane.cpp +++ b/ArduPlane/quadplane.cpp @@ -1968,6 +1968,8 @@ void QuadPlane::motors_output(bool run_rate_controller) attitude_control->set_dt(last_loop_time_s); pos_control->set_dt(last_loop_time_s); attitude_control->rate_controller_run(); + // reset sysid and other temporary inputs + attitude_control->rate_controller_target_reset(); last_att_control_ms = now; } diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp b/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp index ef3317fe6bd2d..fbfa3305be080 100644 --- a/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp +++ b/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp @@ -9,10 +9,12 @@ extern const AP_HAL::HAL& hal; // default gains for Plane # define AC_ATTITUDE_CONTROL_INPUT_TC_DEFAULT 0.2f // Soft #define AC_ATTITUDE_CONTROL_ANGLE_LIMIT_MIN 5.0 // Min lean angle so that vehicle can maintain limited control + #define AC_ATTITUDE_CONTROL_AFTER_RATE_CONTROL 0 #else // default gains for Copter and Sub # define AC_ATTITUDE_CONTROL_INPUT_TC_DEFAULT 0.15f // Medium #define AC_ATTITUDE_CONTROL_ANGLE_LIMIT_MIN 10.0 // Min lean angle so that vehicle can maintain limited control + #define AC_ATTITUDE_CONTROL_AFTER_RATE_CONTROL 1 #endif AC_AttitudeControl *AC_AttitudeControl::_singleton; @@ -185,18 +187,45 @@ float AC_AttitudeControl::get_slew_yaw_max_degs() const return MIN(_ang_vel_yaw_max, _slew_yaw * 0.01); } +// get the latest gyro for the purposes of attitude control +// Counter-inuitively the lowest latency for rate control is achieved by running rate control +// *before* attitude control. This is because you want rate control to run as close as possible +// to the time that a gyro sample was read to minimise jitter and control errors. Running rate +// control after attitude control might makes sense logically, but the overhead of attitude +// control calculations (and other updates) compromises the actual rate control. +// +// In the case of running rate control in a separate thread, the ordering between rate and attitude +// updates is less important, except that gyro sample used should be the latest +// +// Currently quadplane runs rate control after attitude control, necessitating the following code +// to minimise latency. +// However this code can be removed once quadplane updates it's structure to run the rate loops before +// the Attitude controller. +const Vector3f AC_AttitudeControl::get_latest_gyro() const +{ +#if AC_ATTITUDE_CONTROL_AFTER_RATE_CONTROL + // rate updates happen before attitude updates so the last gyro value is the last rate gyro value + // this also allows a separate rate thread to be the source of gyro data + return _rate_gyro; +#else + // rate updates happen after attitude updates so the AHRS must be consulted for the last gyro value + return _ahrs.get_gyro_latest(); +#endif +} + // Ensure attitude controller have zero errors to relax rate controller output void AC_AttitudeControl::relax_attitude_controllers() { + // take a copy of the last gyro used by the rate controller before using it + Vector3f gyro = get_latest_gyro(); // Initialize the attitude variables to the current attitude _ahrs.get_quat_body_to_ned(_attitude_target); _attitude_target.to_euler(_euler_angle_target); _attitude_ang_error.initialise(); // Initialize the angular rate variables to the current rate - _ang_vel_target = _ahrs.get_gyro(); + _ang_vel_target = gyro; ang_vel_to_euler_rate(_attitude_target, _ang_vel_target, _euler_rate_target); - _ang_vel_body = _ahrs.get_gyro(); // Initialize remaining variables _thrust_error_angle = 0.0f; @@ -208,6 +237,8 @@ void AC_AttitudeControl::relax_attitude_controllers() // Reset the I terms reset_rate_controller_I_terms(); + // finally update the attitude target + _ang_vel_body = gyro; } void AC_AttitudeControl::reset_rate_controller_I_terms() @@ -471,6 +502,7 @@ void AC_AttitudeControl::input_euler_rate_roll_pitch_yaw(float euler_roll_rate_c attitude_controller_run_quat(); } +// Fully stabilized acro // Command an angular velocity with angular velocity feedforward and smoothing void AC_AttitudeControl::input_rate_bf_roll_pitch_yaw(float roll_rate_bf_cds, float pitch_rate_bf_cds, float yaw_rate_bf_cds) { @@ -511,6 +543,7 @@ void AC_AttitudeControl::input_rate_bf_roll_pitch_yaw(float roll_rate_bf_cds, fl attitude_controller_run_quat(); } +// Rate-only acro with no attitude feedback - used only by Copter rate-only acro // Command an angular velocity with angular velocity smoothing using rate loops only with no attitude loop stabilization void AC_AttitudeControl::input_rate_bf_roll_pitch_yaw_2(float roll_rate_bf_cds, float pitch_rate_bf_cds, float yaw_rate_bf_cds) { @@ -531,9 +564,12 @@ void AC_AttitudeControl::input_rate_bf_roll_pitch_yaw_2(float roll_rate_bf_cds, _attitude_target.to_euler(_euler_angle_target); // Convert body-frame angular velocity into euler angle derivative of desired attitude ang_vel_to_euler_rate(_attitude_target, _ang_vel_target, _euler_rate_target); + + // finally update the attitude target _ang_vel_body = _ang_vel_target; } +// Acro with attitude feedback that does not rely on attitude - used only by Plane acro // Command an angular velocity with angular velocity smoothing using rate loops only with integrated rate error stabilization void AC_AttitudeControl::input_rate_bf_roll_pitch_yaw_3(float roll_rate_bf_cds, float pitch_rate_bf_cds, float yaw_rate_bf_cds) { @@ -554,7 +590,7 @@ void AC_AttitudeControl::input_rate_bf_roll_pitch_yaw_3(float roll_rate_bf_cds, _attitude_ang_error.from_axis_angle(attitude_error); } - Vector3f gyro_latest = _ahrs.get_gyro_latest(); + Vector3f gyro_latest = get_latest_gyro(); attitude_ang_error_update_quat.from_axis_angle((_ang_vel_target - gyro_latest) * _dt); _attitude_ang_error = attitude_ang_error_update_quat * _attitude_ang_error; _attitude_ang_error.normalize(); @@ -582,8 +618,11 @@ void AC_AttitudeControl::input_rate_bf_roll_pitch_yaw_3(float roll_rate_bf_cds, // Compute the angular velocity target from the integrated rate error _attitude_ang_error.to_axis_angle(attitude_error); - _ang_vel_body = update_ang_vel_target_from_att_error(attitude_error); - _ang_vel_body += _ang_vel_target; + Vector3f ang_vel_body = update_ang_vel_target_from_att_error(attitude_error); + ang_vel_body += _ang_vel_target; + + // finally update the attitude target + _ang_vel_body = ang_vel_body; } // Command an angular step (i.e change) in body frame angle @@ -612,6 +651,25 @@ void AC_AttitudeControl::input_angle_step_bf_roll_pitch_yaw(float roll_angle_ste attitude_controller_run_quat(); } +// Command an rate step (i.e change) in body frame rate +// Used to command a step in rate without exciting the orthogonal axis during autotune +// Done as a single thread-safe function to avoid intermediate zero values being seen by the attitude controller +void AC_AttitudeControl::input_rate_step_bf_roll_pitch_yaw(float roll_rate_step_bf_cd, float pitch_rate_step_bf_cd, float yaw_rate_step_bf_cd) +{ + // Update the unused targets attitude based on current attitude to condition mode change + _ahrs.get_quat_body_to_ned(_attitude_target); + _attitude_target.to_euler(_euler_angle_target); + // Set the target angular velocity to be zero to minimize target overshoot after the rate step finishes + _ang_vel_target.zero(); + // Convert body-frame angular velocity into euler angle derivative of desired attitude + _euler_rate_target.zero(); + + Vector3f ang_vel_body {roll_rate_step_bf_cd, pitch_rate_step_bf_cd, yaw_rate_step_bf_cd}; + ang_vel_body = ang_vel_body * 0.01f * DEG_TO_RAD; + // finally update the attitude target + _ang_vel_body = ang_vel_body; +} + // Command a thrust vector and heading rate void AC_AttitudeControl::input_thrust_vector_rate_heading(const Vector3f& thrust_vector, float heading_rate_cds, bool slew_yaw) { @@ -788,33 +846,35 @@ void AC_AttitudeControl::attitude_controller_run_quat() thrust_heading_rotation_angles(_attitude_target, attitude_body, attitude_error, _thrust_angle, _thrust_error_angle); // Compute the angular velocity corrections in the body frame from the attitude error - _ang_vel_body = update_ang_vel_target_from_att_error(attitude_error); + Vector3f ang_vel_body = update_ang_vel_target_from_att_error(attitude_error); // ensure angular velocity does not go over configured limits - ang_vel_limit(_ang_vel_body, radians(_ang_vel_roll_max), radians(_ang_vel_pitch_max), radians(_ang_vel_yaw_max)); + ang_vel_limit(ang_vel_body, radians(_ang_vel_roll_max), radians(_ang_vel_pitch_max), radians(_ang_vel_yaw_max)); // rotation from the target frame to the body frame Quaternion rotation_target_to_body = attitude_body.inverse() * _attitude_target; // target angle velocity vector in the body frame Vector3f ang_vel_body_feedforward = rotation_target_to_body * _ang_vel_target; - + Vector3f gyro = get_latest_gyro(); // Correct the thrust vector and smoothly add feedforward and yaw input _feedforward_scalar = 1.0f; if (_thrust_error_angle > AC_ATTITUDE_THRUST_ERROR_ANGLE * 2.0f) { - _ang_vel_body.z = _ahrs.get_gyro().z; + ang_vel_body.z = gyro.z; } else if (_thrust_error_angle > AC_ATTITUDE_THRUST_ERROR_ANGLE) { _feedforward_scalar = (1.0f - (_thrust_error_angle - AC_ATTITUDE_THRUST_ERROR_ANGLE) / AC_ATTITUDE_THRUST_ERROR_ANGLE); - _ang_vel_body.x += ang_vel_body_feedforward.x * _feedforward_scalar; - _ang_vel_body.y += ang_vel_body_feedforward.y * _feedforward_scalar; - _ang_vel_body.z += ang_vel_body_feedforward.z; - _ang_vel_body.z = _ahrs.get_gyro().z * (1.0 - _feedforward_scalar) + _ang_vel_body.z * _feedforward_scalar; + ang_vel_body.x += ang_vel_body_feedforward.x * _feedforward_scalar; + ang_vel_body.y += ang_vel_body_feedforward.y * _feedforward_scalar; + ang_vel_body.z += ang_vel_body_feedforward.z; + ang_vel_body.z = gyro.z * (1.0 - _feedforward_scalar) + ang_vel_body.z * _feedforward_scalar; } else { - _ang_vel_body += ang_vel_body_feedforward; + ang_vel_body += ang_vel_body_feedforward; } // Record error to handle EKF resets _attitude_ang_error = attitude_body.inverse() * _attitude_target; + // finally update the attitude target + _ang_vel_body = ang_vel_body; } // thrust_heading_rotation_angles - calculates two ordered rotations to move the attitude_body quaternion to the attitude_target quaternion. diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl.h b/libraries/AC_AttitudeControl/AC_AttitudeControl.h index 70271ac9cc48f..319c2269d2471 100644 --- a/libraries/AC_AttitudeControl/AC_AttitudeControl.h +++ b/libraries/AC_AttitudeControl/AC_AttitudeControl.h @@ -11,6 +11,7 @@ #include #include #include +#include #define AC_ATTITUDE_CONTROL_ANGLE_P 4.5f // default angle P gain for roll, pitch and yaw @@ -151,6 +152,7 @@ class AC_AttitudeControl { // set the rate control input smoothing time constant void set_input_tc(float input_tc) { _input_tc.set(constrain_float(input_tc, 0.0f, 1.0f)); } + // rate loop visible functions // Ensure attitude controller have zero errors to relax rate controller output void relax_attitude_controllers(); @@ -177,6 +179,24 @@ class AC_AttitudeControl { // handle reset of attitude from EKF since the last iteration void inertial_frame_reset(); + // Command euler yaw rate and pitch angle with roll angle specified in body frame + // (implemented only in AC_AttitudeControl_TS for tailsitter quadplanes) + virtual void input_euler_rate_yaw_euler_angle_pitch_bf_roll(bool plane_controls, float euler_roll_angle_cd, + float euler_pitch_angle_cd, float euler_yaw_rate_cds) {} + + ////// begin rate update functions ////// + // These functions all update _ang_vel_body which is used as the rate target by the rate controller. + // Since _ang_vel_body can be seen by the rate controller thread all these functions only set it + // at the end once all of the calculations have been performed. This avoids intermediate results being + // used by the rate controller when running concurrently. _ang_vel_body is accessed so commonly that + // locking proves to be moderately expensive, however since this is changing incrementally values combining + // previous and current elements are safe and do not have an impact on control. + // Any additional functions that are added to manipulate _ang_vel_body should follow this pattern. + + // Calculates the body frame angular velocities to follow the target attitude + // This is used by most of the subsequent functions + void attitude_controller_run_quat(); + // Command a Quaternion attitude with feedforward and smoothing // attitude_desired_quat: is updated on each time_step (_dt) by the integral of the body frame angular velocity virtual void input_quaternion(Quaternion& attitude_desired_quat, Vector3f ang_vel_body); @@ -186,37 +206,47 @@ class AC_AttitudeControl { // Command an euler roll, pitch and yaw angle with angular velocity feedforward and smoothing virtual void input_euler_angle_roll_pitch_yaw(float euler_roll_angle_cd, float euler_pitch_angle_cd, float euler_yaw_angle_cd, bool slew_yaw); - // Command euler yaw rate and pitch angle with roll angle specified in body frame - // (implemented only in AC_AttitudeControl_TS for tailsitter quadplanes) - virtual void input_euler_rate_yaw_euler_angle_pitch_bf_roll(bool plane_controls, float euler_roll_angle_cd, - float euler_pitch_angle_cd, float euler_yaw_rate_cds) {} - // Command an euler roll, pitch, and yaw rate with angular velocity feedforward and smoothing virtual void input_euler_rate_roll_pitch_yaw(float euler_roll_rate_cds, float euler_pitch_rate_cds, float euler_yaw_rate_cds); + // Fully stabilized acro // Command an angular velocity with angular velocity feedforward and smoothing virtual void input_rate_bf_roll_pitch_yaw(float roll_rate_bf_cds, float pitch_rate_bf_cds, float yaw_rate_bf_cds); - // Command an angular velocity with angular velocity feedforward and smoothing + // Rate-only acro with no attitude feedback - used only by Copter rate-only acro + // Command an angular velocity with angular velocity smoothing using rate loops only with no attitude loop stabilization virtual void input_rate_bf_roll_pitch_yaw_2(float roll_rate_bf_cds, float pitch_rate_bf_cds, float yaw_rate_bf_cds); + // Acro with attitude feedback that does not rely on attitude - used only by Plane acro // Command an angular velocity with angular velocity smoothing using rate loops only with integrated rate error stabilization virtual void input_rate_bf_roll_pitch_yaw_3(float roll_rate_bf_cds, float pitch_rate_bf_cds, float yaw_rate_bf_cds); // Command an angular step (i.e change) in body frame angle virtual void input_angle_step_bf_roll_pitch_yaw(float roll_angle_step_bf_cd, float pitch_angle_step_bf_cd, float yaw_angle_step_bf_cd); + // Command an angular rate step (i.e change) in body frame rate + virtual void input_rate_step_bf_roll_pitch_yaw(float roll_rate_step_bf_cd, float pitch_rate_step_bf_cd, float yaw_rate_step_bf_cd); + // Command a thrust vector in the earth frame and a heading angle and/or rate virtual void input_thrust_vector_rate_heading(const Vector3f& thrust_vector, float heading_rate_cds, bool slew_yaw = true); + virtual void input_thrust_vector_heading(const Vector3f& thrust_vector, float heading_angle_cd, float heading_rate_cds); void input_thrust_vector_heading(const Vector3f& thrust_vector, float heading_cd) {input_thrust_vector_heading(thrust_vector, heading_cd, 0.0f);} + ////// end rate update functions ////// + // Converts thrust vector and heading angle to quaternion rotation in the earth frame Quaternion attitude_from_thrust_vector(Vector3f thrust_vector, float heading_angle) const; // Run angular velocity controller and send outputs to the motors virtual void rate_controller_run() = 0; + // reset target loop rate modifications + virtual void rate_controller_target_reset() {} + + // optional variant to allow running with different dt + virtual void rate_controller_run_dt(const Vector3f& gyro, float dt) { AP_BoardConfig::config_error("rate_controller_run_dt() must be defined"); }; + // Convert a 321-intrinsic euler angle derivative to an angular velocity vector void euler_rate_to_ang_vel(const Quaternion& att, const Vector3f& euler_rate_rads, Vector3f& ang_vel_rads); @@ -244,12 +274,6 @@ class AC_AttitudeControl { // Return the angle between the target thrust vector and the current thrust vector. float get_att_error_angle_deg() const { return degrees(_thrust_error_angle); } - // Set x-axis angular velocity in centidegrees/s - void rate_bf_roll_target(float rate_cds) { _ang_vel_body.x = radians(rate_cds * 0.01f); } - - // Set y-axis angular velocity in centidegrees/s - void rate_bf_pitch_target(float rate_cds) { _ang_vel_body.y = radians(rate_cds * 0.01f); } - // Set z-axis angular velocity in centidegrees/s void rate_bf_yaw_target(float rate_cds) { _ang_vel_body.z = radians(rate_cds * 0.01f); } @@ -349,9 +373,6 @@ class AC_AttitudeControl { // Calculates the body frame angular velocities to follow the target attitude void update_attitude_target(); - // Calculates the body frame angular velocities to follow the target attitude - void attitude_controller_run_quat(); - // thrust_heading_rotation_angles - calculates two ordered rotations to move the attitude_body quaternion to the attitude_target quaternion. // The maximum error in the yaw axis is limited based on the angle yaw P value and acceleration. void thrust_heading_rotation_angles(Quaternion& attitude_target, const Quaternion& attitude_body, Vector3f& attitude_error, float& thrust_angle, float& thrust_error_angle) const; @@ -454,6 +475,9 @@ class AC_AttitudeControl { // Return the yaw slew rate limit in radians/s float get_slew_yaw_max_rads() const { return radians(get_slew_yaw_max_degs()); } + // get the latest gyro for the purposes of attitude control + const Vector3f get_latest_gyro() const; + // Maximum rate the yaw target can be updated in Loiter, RTL, Auto flight modes AP_Float _slew_yaw; @@ -520,7 +544,7 @@ class AC_AttitudeControl { Vector3f _ang_vel_target; // This represents the angular velocity in radians per second in the body frame, used in the angular - // velocity controller. + // velocity controller and most importantly the rate controller. Vector3f _ang_vel_body; // This is the angular velocity in radians per second in the body frame, added to the output angular diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl_Logging.cpp b/libraries/AC_AttitudeControl/AC_AttitudeControl_Logging.cpp index 80d7512742efa..d9327f6abf382 100644 --- a/libraries/AC_AttitudeControl/AC_AttitudeControl_Logging.cpp +++ b/libraries/AC_AttitudeControl/AC_AttitudeControl_Logging.cpp @@ -30,20 +30,20 @@ void AC_AttitudeControl::Write_ANG() const // Write a rate packet void AC_AttitudeControl::Write_Rate(const AC_PosControl &pos_control) const { - const Vector3f &rate_targets = rate_bf_targets(); + const Vector3f rate_targets = rate_bf_targets() * RAD_TO_DEG; const Vector3f &accel_target = pos_control.get_accel_target_cmss(); - const Vector3f &gyro_rate = _rate_gyro; + const Vector3f gyro_rate = _rate_gyro * RAD_TO_DEG; const struct log_Rate pkt_rate{ LOG_PACKET_HEADER_INIT(LOG_RATE_MSG), time_us : _rate_gyro_time_us, - control_roll : degrees(rate_targets.x), - roll : degrees(gyro_rate.x), + control_roll : rate_targets.x, + roll : gyro_rate.x, roll_out : _motors.get_roll()+_motors.get_roll_ff(), - control_pitch : degrees(rate_targets.y), - pitch : degrees(gyro_rate.y), + control_pitch : rate_targets.y, + pitch : gyro_rate.y, pitch_out : _motors.get_pitch()+_motors.get_pitch_ff(), - control_yaw : degrees(rate_targets.z), - yaw : degrees(gyro_rate.z), + control_yaw : rate_targets.z, + yaw : gyro_rate.z, yaw_out : _motors.get_yaw()+_motors.get_yaw_ff(), control_accel : (float)accel_target.z, accel : (float)(-(_ahrs.get_accel_ef().z + GRAVITY_MSS) * 100.0f), diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl_Multi.cpp b/libraries/AC_AttitudeControl/AC_AttitudeControl_Multi.cpp index 91378c8bfa067..5beb517fde739 100644 --- a/libraries/AC_AttitudeControl/AC_AttitudeControl_Multi.cpp +++ b/libraries/AC_AttitudeControl/AC_AttitudeControl_Multi.cpp @@ -439,35 +439,49 @@ void AC_AttitudeControl_Multi::update_throttle_rpy_mix() _throttle_rpy_mix = constrain_float(_throttle_rpy_mix, 0.1f, AC_ATTITUDE_CONTROL_MAX); } -void AC_AttitudeControl_Multi::rate_controller_run() +void AC_AttitudeControl_Multi::rate_controller_run_dt(const Vector3f& gyro, float dt) { + // take a copy of the target so that it can't be changed from under us. + Vector3f ang_vel_body = _ang_vel_body; + // boost angle_p/pd each cycle on high throttle slew update_throttle_gain_boost(); // move throttle vs attitude mixing towards desired (called from here because this is conveniently called on every iteration) update_throttle_rpy_mix(); - _ang_vel_body += _sysid_ang_vel_body; + ang_vel_body += _sysid_ang_vel_body; - _rate_gyro = _ahrs.get_gyro_latest(); + _rate_gyro = gyro; _rate_gyro_time_us = AP_HAL::micros64(); - _motors.set_roll(get_rate_roll_pid().update_all(_ang_vel_body.x, _rate_gyro.x, _dt, _motors.limit.roll, _pd_scale.x) + _actuator_sysid.x); + _motors.set_roll(get_rate_roll_pid().update_all(ang_vel_body.x, gyro.x, dt, _motors.limit.roll, _pd_scale.x) + _actuator_sysid.x); _motors.set_roll_ff(get_rate_roll_pid().get_ff()); - _motors.set_pitch(get_rate_pitch_pid().update_all(_ang_vel_body.y, _rate_gyro.y, _dt, _motors.limit.pitch, _pd_scale.y) + _actuator_sysid.y); + _motors.set_pitch(get_rate_pitch_pid().update_all(ang_vel_body.y, gyro.y, dt, _motors.limit.pitch, _pd_scale.y) + _actuator_sysid.y); _motors.set_pitch_ff(get_rate_pitch_pid().get_ff()); - _motors.set_yaw(get_rate_yaw_pid().update_all(_ang_vel_body.z, _rate_gyro.z, _dt, _motors.limit.yaw, _pd_scale.z) + _actuator_sysid.z); + _motors.set_yaw(get_rate_yaw_pid().update_all(ang_vel_body.z, gyro.z, dt, _motors.limit.yaw, _pd_scale.z) + _actuator_sysid.z); _motors.set_yaw_ff(get_rate_yaw_pid().get_ff()*_feedforward_scalar); + _pd_scale_used = _pd_scale; + + control_monitor_update(); +} + +// reset the rate controller target loop updates +void AC_AttitudeControl_Multi::rate_controller_target_reset() +{ _sysid_ang_vel_body.zero(); _actuator_sysid.zero(); - - _pd_scale_used = _pd_scale; _pd_scale = VECTORF_111; +} - control_monitor_update(); +// run the rate controller using the configured _dt and latest gyro +void AC_AttitudeControl_Multi::rate_controller_run() +{ + Vector3f gyro_latest = _ahrs.get_gyro_latest(); + rate_controller_run_dt(gyro_latest, _dt); } // sanity check parameters. should be called once before takeoff diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl_Multi.h b/libraries/AC_AttitudeControl/AC_AttitudeControl_Multi.h index 55359a1edb6fb..c1e9b15a250a3 100644 --- a/libraries/AC_AttitudeControl/AC_AttitudeControl_Multi.h +++ b/libraries/AC_AttitudeControl/AC_AttitudeControl_Multi.h @@ -76,6 +76,8 @@ class AC_AttitudeControl_Multi : public AC_AttitudeControl { bool is_throttle_mix_min() const override { 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_dt(const Vector3f& gyro, float dt) override; + void rate_controller_target_reset() override; void rate_controller_run() override; // sanity check parameters. should be called once before take-off diff --git a/libraries/AC_AutoTune/AC_AutoTune_Multi.cpp b/libraries/AC_AutoTune/AC_AutoTune_Multi.cpp index 213fc41a2b416..5e7aa6c5d9836 100644 --- a/libraries/AC_AutoTune/AC_AutoTune_Multi.cpp +++ b/libraries/AC_AutoTune/AC_AutoTune_Multi.cpp @@ -1254,23 +1254,22 @@ void AC_AutoTune_Multi::twitch_test_run(AxisType test_axis, const float dir_sign attitude_control->input_rate_bf_roll_pitch_yaw(0.0, 0.0, 0.0); } } else { - attitude_control->input_rate_bf_roll_pitch_yaw_2(0.0, 0.0, 0.0); // Testing rate P and D gains so will set body-frame rate targets. // Rate controller will use existing body-frame rates and convert to motor outputs // for all axes except the one we override here. switch (test_axis) { case AxisType::ROLL: // override body-frame roll rate - attitude_control->rate_bf_roll_target(dir_sign * target_rate + start_rate); + attitude_control->input_rate_step_bf_roll_pitch_yaw(dir_sign * target_rate + start_rate, 0.0f, 0.0f); break; case AxisType::PITCH: // override body-frame pitch rate - attitude_control->rate_bf_pitch_target(dir_sign * target_rate + start_rate); + attitude_control->input_rate_step_bf_roll_pitch_yaw(0.0f, dir_sign * target_rate + start_rate, 0.0f); break; case AxisType::YAW: case AxisType::YAW_D: // override body-frame yaw rate - attitude_control->rate_bf_yaw_target(dir_sign * target_rate + start_rate); + attitude_control->input_rate_step_bf_roll_pitch_yaw(0.0f, 0.0f, dir_sign * target_rate + start_rate); break; } }