diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp b/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp index e0f4a2185f..5ac8b820b8 100644 --- a/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp +++ b/libraries/AC_AttitudeControl/AC_AttitudeControl.cpp @@ -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(); } diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl.h b/libraries/AC_AttitudeControl/AC_AttitudeControl.h index 30863b3d00..473686ccb9 100644 --- a/libraries/AC_AttitudeControl/AC_AttitudeControl.h +++ b/libraries/AC_AttitudeControl/AC_AttitudeControl.h @@ -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); diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl_Multi_6DoF.cpp b/libraries/AC_AttitudeControl/AC_AttitudeControl_Multi_6DoF.cpp index a64fad1e21..5cea0ab08b 100644 --- a/libraries/AC_AttitudeControl/AC_AttitudeControl_Multi_6DoF.cpp +++ b/libraries/AC_AttitudeControl/AC_AttitudeControl_Multi_6DoF.cpp @@ -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); } diff --git a/libraries/AC_AttitudeControl/AC_AttitudeControl_Multi_6DoF.h b/libraries/AC_AttitudeControl/AC_AttitudeControl_Multi_6DoF.h index 38ef83d71a..76deec376f 100644 --- a/libraries/AC_AttitudeControl/AC_AttitudeControl_Multi_6DoF.h +++ b/libraries/AC_AttitudeControl/AC_AttitudeControl_Multi_6DoF.h @@ -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 */