Bug (and suggested fix): SimpleFlight angle rate controller unstable at high pitch or roll

[RichardHopkirk]

Hi there. I've noticed what I think is a bug in the simple flight angle-rate controller. I've believe I've fixed it locally. If you agree it's a bug, and if you'd like me to push the fix, I'm happy to open a pull request. But thought I'd raise the issue first to check as it'll be my first contribution to this project.

Steps to recreate:

• Increase the maximum pitch or roll angle from the default pi/5.5 (33 degrees) (AngleLevelPid::max_limit in firmware/Params.hpp)
• Fly aggressively
• The drone will become unstable and often flip over, crashing to the ground.

I believe this is due to a bug in the angle rate controller confusing euler angle rates with body rates (i.e. angular velocity expressed in the body frame)

The goal for the angle rate controller comes from the angle level controller, and is expressed in euler angle rates (as the state estimator provides angles to the angle level controller as euler angles). However the the angle rate controller targets body rates not euler angle rates (as the state estimator provides angular velocity to the angle rate controller in the body frame (kinematics_->twist.angular), not as euler rates).

This isn't a problem at low pitch and roll as the body and euler rates are so similar, but at high pitch and roll they differ significantly, particularly yaw (Euler yaw is rotation about world z axis, not body z axis), leading to instability.

To fix the issue I added some code to transform the goal in the angle rate controller from euler rates into body rates. It seems stable on my build up to higher angles (I limit at 45 degrees). The drone still lacks sufficient thrust to maintain altitude in aggressive flying (as per the comment in params.hpp - I happen to correct this in the drone model by increasing available thrust), but it doesn't become unstable.

Do let me know if you agree with my assessment - I may have missed something.

And if you do agree let me know if you'd like me to open a pull request.

Best wishes,

Richard

[ctoumieh]

Hi,

I'm facing the same issue. Do you mind sharing the code you modified?

Cheers, CT

[RichardHopkirk]

Hi CT,

The main thing is the following change to the update method in angleratecontroller.hpp (AirLib/include/vehicles/multirotor/firmwares/simple_flight/firmware/AngleRateController.hpp), note it's a bit of a hack at the moment due to the use of statics, but you hopefully get the idea.

The maths to do the conversion is in the switch statement. There's plenty of derivations online, but one example can be found in section 1.3 of this Mech Eng course from MIT - see equation 17.

I'll try and put together a pull request with a better modification if I get a moment.

Best wishes,

Richard

virtual void update() override
    {
        /// Centralised angle rate goals - to deal with cross-talk between euler and body rates
        /// NB THIS STATIC PREVENTS MORE THAN ONE DRONE IN THE SYSTEM
        /// Long term we should pass a pointer to this from the Cascade Controller, and have it live as a member there
        /// ToDo - Richard
        static Axis3r euler_rate_goal_(0,0,0);
        static Axis3r body_rate_goal_(0, 0, 0);

                IAxisController::update();

        euler_rate_goal_[axis_] = goal_->getGoalValue()[axis_];
        Axis3r euler_angles = state_estimator_->getAngles();
        Axis3r body_rate = state_estimator_->getAngularVelocity();

        // The rate controller works in body rates, but goals are provided in euler rates not body rates
        // So we need to turn euler rates into body rates here using T matrix
        // in order to give the rate controller the correct target
        TReal phi = euler_angles[0];
        TReal sPhi = sin(phi);
        TReal cPhi = cos(phi);

        TReal theta = euler_angles[1];
        TReal sTheta = sin(theta);
        TReal cTheta = cos(theta);

        TReal phiDotGoal = euler_rate_goal_[0];
        TReal thetaDotGoal = euler_rate_goal_[1];
        TReal psiDotGoal = euler_rate_goal_[2];

        switch (axis_) {
        case 0: // roll
            body_rate_goal_[0] = phiDotGoal - sTheta * psiDotGoal;
            break;
        case 1: // pitch
            body_rate_goal_[1] = cPhi * thetaDotGoal + sPhi * cTheta * psiDotGoal;
            break;
        case 2: // yaw
            body_rate_goal_[2] = -sPhi * thetaDotGoal + cPhi * cTheta * psiDotGoal;
            break;
        default:
            throw std::invalid_argument("axis must be 0, 1 or 2");
        }

                pid_->setGoal(body_rate_goal_[axis_]);
                pid_->setMeasured(body_rate[axis_]);
                pid_->update();

                output_ = pid_->getOutput();
    }

[stale[bot]]

This issue has been automatically marked as stale because it has not had activity from the community in the last year. It will be closed if no further activity occurs within 20 days.

[RichardHopkirk]

Github just reminded me that this issue is now quite old, so should be closed. I'll close now - but if there's interest do say and it can be can reopened.
If I get time I'll try to do a PR for the change. I still use it locally, and I think it's still valid on the latest master.

[wmy929]

Hi Richard, Your improvement has helped me a lot. I'm facing the same problem. I want to make the quadrotor more aggressive and increase the maximum thrust available. But when I changed CT and CP in RotorParams.hpp, my quadrotor would keep flying to the sky at the beginning of the simulation. I guess it is because of idle throttle (50% in Simpleflight), but I don't know how to solve it. May I ask how you increase the max available thrust?

[xxEoD2242]

Github just reminded me that this issue is now quite old, so should be closed. I'll close now - but if there's interest do say and it can be can reopened. If I get time I'll try to do a PR for the change. I still use it locally, and I think it's still valid on the latest master.

Hey @RichardHopkirk,

As AirSim was shutdown, we decided to continue the open source project here: Colosseum. I will take a look at your suggestions and generate a PR, unless you would like to do that but either way, I'd like to solve this bug in the Angle Rate controller.

[RichardHopkirk]

Hi Richard, Your improvement has helped me a lot. I'm facing the same problem. I want to make the quadrotor more aggressive and increase the maximum thrust available. But when I changed CT and CP in RotorParams.hpp, my quadrotor would keep flying to the sky at the beginning of the simulation. I guess it is because of idle throttle (50% in Simpleflight), but I don't know how to solve it. May I ask how you increase the max available thrust?

Hi there, I haven't adjusted max available thrust, but your suggestion sounds reasonable (adjusting the values in rotorparams.hpp). I'm afraid I don't know why your quadrotor is ascending, but keep poking around in simple flight and I'm sure you'll solve it.

[RichardHopkirk]

Hey @RichardHopkirk,

As AirSim was shutdown, we decided to continue the open source project here: Colosseum. I will take a look at your suggestions and generate a PR, unless you would like to do that but either way, I'd like to solve this bug in the Angle Rate controller.

Hi Tyler, thanks for your message. Do feel free to create a PR yourself, and feel free to reach out if you need a hand.