Flipping Arducopter

When I was learning to fly my quad I couldn't wait to develop the skills to start doing acrobatics. I had no idea that arducopter was so limited in this area. Here's the stock options:

Stabilized Acro (AXIS_ENABLED=1, ACRO_BAL_ROLL = x, ACRO_BAL_PITCH = y)

In this mode, you get your familiar stabilize 'earth framed' controls, and the ACRO_BAL parameters set a decay rate on how quickly the copter returns to level when you let off the controls. It's not so bad, but:

  • You are limited to +/-90 degree pitch to avoid gimbal lock. No pitch flips for you!
  • Yaw is earth based, so yaw and roll become the same control at +/-90 degree pitch. (no flipping the tail around if you chose to do so.)
  • The ACRO_BAL seems to limit the roll, because the further you roll, the harder it tries to return to level. I found I needed to drop those right to zero to do roll flips, but getting the exit angle right is really hard for a newbie like myself. 

Body Frame Acro (AXIS_ENABLED=0)

In this mode, the stick sets the rate you want to rotate on each axis. You get true 3D axis control, but it is very unstable and hard to fly. A small gust of wind tilts the copter and that's it's new holding position - the integrators in the rate controllers try to get it back, but since the control input is 'don't move' the Kp value kind of cancels it out. In this mode, you also don't have access to the ACRO_BAL safety net. 

So I thought the first thing I needed to do was improve the holding power in Body Frame Acro mode. I achieved this by, instead of directly passing the pilot rates to the rate controllers, I rotated a reference and used the stabilize controllers to target that. So far so good. The trick here was to use the gyro drift compensated omega vector to un-rotate my reference which basically put an outer angular controller around the rate controllers. You get the angular holding performance similar to stabilize mode - except at any random angle. I tried it out, it worked just as expected, but I continued to crash my copter because I'm a really bad pilot who can't get the exit angle correct or remember to flip back into stabilize mode before it's too late. 

In trying to solve this puzzle, I ran into some posts about MultiWii's HORIZON mode and basically what they've done is combined level mode with acro mode, and your stick position sets the mix between the two. At 0, you are in 100% level mode, at 100% you are in 100% acro mode. This sounded pretty good, but I was a little skeptical with how it would actually perform when getting inverted. For example, if you were passing +90 degree pitch at less than full stick, the sudden roll switch from 0 to 180 would cause the roll level controller to start rolling to copter when you didn't want it to. Also, as you cross the 90 degree pitch point, the pitch angle then looks like it's decreasing from 90, but the pitch level controller would be pulling in the wrong direction. I didn't see how they dealt with this in the MultiWii code, but perhaps their gimbal code works differently. For arducopter, this is what I did:

  • I set up the stick based proportional mix just like MultiWii
  • For the roll level controller, I made it's output blend towards the acro controller rate as pitch approached +/-90 degrees. At 90 degree pitch, there is no roll level control to blend in, and the stable acro controller keeps things in line while passing through the discontinuity. 
  • For the pitch level controller, I made it target the closest 'level' point - either up right or upside down. So if you pitch past 90 and let go of the stick, the pitch leveler will actually pull you over to 180 pitch. Sounds bad, but as you approach full inversion, the roll controller will right you on it's axis instead. More importantly, of you roll past 90 on purpose, the pitch controller will happily hold your pitch angle - inverted or not. 

I just tested this new algorithm out and it's perfect for a newbie like me. Forward, backward, and side flips where smooth and easy. Since the level/acro rates are blended and fighting each other, the only tuning was to get the acro Kp up high enough to make it feel natural. 

--Bob

You need to be a member of diydrones to add comments!

Join diydrones

Email me when people reply –

Replies

  • Hello

    is this code already in V3.0.1 or it must be loaded separately

    Thank you

  • Can someone please summarize what i have to do so i can roll and pitch flip on 2.9.1? i would be gratefull

  • I realize there's another solution in the works, but I updated my code and was pretty pleased with how this flew...

    What's new (most stolen directly from Leonard's solution):

     - Pitch leveling using pitch/yaw mix.

     - Leveling power is set using g.acro_balance, which I also added as a tuning parameter.

     - Acro target is direct instead of blended, removing the expo feel.

     - Leveling is still scaled by deflection, but now yaw deflection is included in the decision.

     - Bleed time constant reduced from 50%/sec to about 28%/sec.

     - An excessive and redundant amount of floating point math in there that I should optimize out...

    #define _BLEED 300
    #define BLEED_CONST (_BLEED/(_BLEED + 1.0))

    // use max stick deflection of roll, pitch or yaw to determine level mix
    float CalcLevelMix()
    {
      float r = ((float)labs(g.rc_1.control_in)) / MAX_INPUT_ROLL_ANGLE;
      float p = ((float)labs(g.rc_2.control_in)) / MAX_INPUT_PITCH_ANGLE;
      float y = ((float)labs(g.rc_4.control_in)) / MAX_INPUT_YAW_ANGLE;

      p = max(y,max(r,p));

      return 1.0 - p;
    }

    // Roll with rate input and stabilized to body frame
    static void
    get_roll_rate_stabilized_bf(int32_t roll_stick_angle)
    {
      float angle_error = 0;
      int32_t acro_rate;
      int32_t level_rate;

      // Bleed axis error
      roll_axis = roll_axis * BLEED_CONST;

      // convert the input to the desired roll rate
      acro_rate = roll_stick_angle * g.acro_p;

      // now calculate level rate for blending (similar to MultiWiii horizon mode)
      // ignore roll level as we approach pitch +-90 (cos_pitch_x)
      level_rate = constrain(wrap_180(roll_stick_angle - ahrs.roll_sensor), -4500, 4500) * cos_pitch_x * CalcLevelMix() * (g.acro_balance_roll/100.0);

      // convert the input to the desired roll rate
      roll_axis += (acro_rate + level_rate - (omega.x * DEGX100)) * ins.get_delta_time();

      // angle error with maximum of +- max_angle_overshoot
      angle_error = constrain(roll_axis, -MAX_BF_ROLL_OVERSHOOT, MAX_BF_ROLL_OVERSHOOT);

      // update roll_axis to be within max_angle_overshoot of our reference
      roll_axis = angle_error;

      if (motors.armed() == false || ((g.rc_3.control_in == 0) && !ap.failsafe)) {
        angle_error = 0;
        roll_axis = 0;
        acro_rate = 0;
        level_rate = 0;
      }

      // set body frame target for rate controller
      set_roll_rate_target(g.pi_stabilize_roll.get_p(angle_error) + acro_rate, BODY_FRAME);
    }

    // Pitch with rate input and stabilized to body frame
    static void
    get_pitch_rate_stabilized_bf(int32_t pitch_stick_angle)
    {
      float angle_error = 0;
      int32_t acro_rate;
      int32_t level_rate;

      // Bleed axis error
      pitch_axis = pitch_axis * BLEED_CONST;

      // convert the input to the desired pitch rate
      acro_rate = pitch_stick_angle * g.acro_p;

      // now calculate level rate for blending (similar to MultiWiii horizon mode)
      level_rate = constrain(wrap_180(pitch_stick_angle - ahrs.pitch_sensor), -4500, 4500) * cos_roll_x * CalcLevelMix() * (g.acro_balance_pitch/100.0);

      // convert the inputs to the desired pitch rate
      pitch_axis += (acro_rate + level_rate - (omega.y * DEGX100)) * ins.get_delta_time();

      // angle error with maximum of +- max_angle_overshoot
      angle_error = constrain(pitch_axis, -MAX_BF_PITCH_OVERSHOOT, MAX_BF_PITCH_OVERSHOOT);

      // update pitch_axis to be within max_angle_overshoot of our current heading
      pitch_axis = angle_error;

      if (motors.armed() == false || ((g.rc_3.control_in == 0) && !ap.failsafe)) {
        angle_error = 0;
        pitch_axis = 0;
        acro_rate = 0;
        level_rate = 0;
      }

      // set body frame target for rate controller
      set_pitch_rate_target(g.pi_stabilize_pitch.get_p(angle_error) + acro_rate, BODY_FRAME);
    }

    // Yaw with rate input and stabilized to body frame
    static void
    get_yaw_rate_stabilized_bf(int32_t yaw_stick_angle, int32_t pitch_stick_angle)
    {
      float angle_error = 0;
      int32_t acro_rate;
      int32_t level_rate;

      // Bleed axis error
      yaw_axis = yaw_axis * BLEED_CONST;

      // convert the input to the desired yaw rate
      acro_rate = yaw_stick_angle * g.acro_p;

      // now calculate level rate for blending (similar to MultiWiii horizon mode)
      level_rate = constrain(wrap_180(pitch_stick_angle - ahrs.pitch_sensor), -4500, 4500) * (-sin_roll) * CalcLevelMix() * (g.acro_balance_pitch/100.0);

      // convert the input to the desired yaw rate
      yaw_axis += (acro_rate + level_rate - (omega.z * DEGX100)) * ins.get_delta_time();

      // angle error with maximum of +- max_angle_overshoot
      angle_error = constrain(yaw_axis, -MAX_BF_YAW_OVERSHOOT, MAX_BF_YAW_OVERSHOOT);

      // update yaw_axis to be within max_angle_overshoot of our current heading
      yaw_axis = angle_error;

      if (motors.armed() == false || ((g.rc_3.control_in == 0) && !ap.failsafe)) {
        angle_error = 0;
        yaw_axis = 0;
        acro_rate = 0;
        level_rate = 0;
      }

      // set body frame target for rate controller
      set_yaw_rate_target(g.pi_stabilize_yaw.get_p(angle_error) + acro_rate, BODY_FRAME);

    }

  • Fumbled around with editing and deleting my posting so I mixed up order a bit, sorry for that!

    Thank you for the Loiter issue info so I won´t touch my Rate PIDs!

  • Developer

    I put the acro code and a summary of it here:

    http://diydrones.com/forum/topics/arducopter-3-0-ready-for-wider-use

  • I can't thank you guys enough for posting the code for this. Can't wait to see what this turns into. 

  • Hey Bob nice work! I'd like to show my support for an improved BF acro mode like this. Moreover, an acro mode that has the possibility to run in pure rate mode (aka no levelling), similar to the one R_Lefebvre wrote, would be essential to the development of arducopter IMO. 

  • Developer

    Here is a snippet of the code:



    // Roll with rate input and stabilized in the earth frame
    static void
    get_roll_rate_stabilized_bf(int32_t stick_angle)
    {
        static float angle_error = 0;

        // convert the input to the desired body frame roll rate
        roll_axis += stick_angle * g.acro_p;

        // add automatic correction
        int32_t correction_rate = g.pi_stabilize_roll.get_p(angle_error);

        // Calculate integrated body frame rate error
        angle_error += (roll_axis - (omega.x * DEGX100)) * G_Dt;

        // don't let angle error grow too large
        angle_error = constrain_float(angle_error, - MAX_ROLL_OVERSHOOT, MAX_ROLL_OVERSHOOT);

        #if FRAME_CONFIG == HELI_FRAME
            if (!motors.motor_runup_complete) {
                   angle_error = 0;
            }
        #else
            if (!motors.armed() || g.rc_3.servo_out == 0) {
                angle_error = 0;
            }
        #endif // HELI_FRAME

        // set body frame targets for rate controller
        set_roll_rate_target(roll_axis + correction_rate, BODY_FRAME);

        // Calculate trainer mode earth frame rate command
        int32_t roll_angle = wrap_180_cd(ahrs.roll_sensor);

        int32_t target_rate = 0;

        if (g.acro_trainer_enabled) {
            if (roll_angle > 4500){
                target_rate =  g.pi_stabilize_roll.get_p(4500-roll_angle);
            }else if (roll_angle < -4500) {
                target_rate =  g.pi_stabilize_roll.get_p(-4500-roll_angle);
            }
        }
        roll_angle   = constrain_int32(roll_angle, -3000, 3000);
        target_rate -=  (roll_angle * g.acro_balance_roll)/100;

        // add earth frame targets for rate controller
        set_roll_rate_target(target_rate, BODY_EARTH_FRAME);
    }

    The only thing I really need to point out is that the earth frame level correction is added at the beginning of the next loop. Also the scaling of the earth frame level is not currently inversely proportional to stick input. I will try this out tomorrow and hopefully get a clone ready to download over the weekend.

    Sorry for the rough code but this is a work in progress.

  • Developer

    I did a full test flight today. Everything worked very well.

    Rolls, left and right. Flips forward and back. Flips while moving forward. Rolls while moving forward.

    Generally thrashing around was fine too, although I need to work on my coordinated turns.

    Vertical descents and the associated wobble didn't cause it to move any more than stabilise would.

    Overall I am very happy. Now we just need to sort out exactly what people want in the way of customisation. By this I mean the training modes and throttle input.

  • Developer

    Robert, Bob,

    Great work!!! As you know I have been thinking about this for a while and although I had a very nice solution I like this concept better.

    What I am implementing is basically Roberts design with the current earth frame acro trainer additions included. This means that this should behave the same as the current earth frame acro but in the body frame.

    I will let you both know when I have something to look at and give feedback on.

    Thanks again for your help and contributions!!

This reply was deleted.

Activity

DIY Drones via Twitter
21 hours ago
DIY Robocars via Twitter
RT @Heavy02011: @diyrobocars : A Home-brew computer club* for Connected Autonomous Driving on Jan 23rd, 2021 https://www.meetup.com/Connected-Autonomous-Driving/events/275728684/ #Meetu…
21 hours ago
DIY Robocars via Twitter
21 hours ago
David Hori liked Isabella Domi's profile
Wednesday
DIY Robocars via Twitter
RT @Heavy02011: ⁦@diyrobocars⁩ Autonomous Driving Assembly at #rC3. join us at https://rc3.world/rc3/assembly/diyrobocars-f1tenth/ ⁦@f1tenth⁩ ⁦@DAVGtech⁩ ⁦@DWalmroth⁩…
Monday
DIY Robocars via Twitter
RT @chr1sa: New car designs coming for our next @DIYRobocars @donkey_car virtual race on the 23rd. Choose any one you want at race time Le…
Monday
DIY Robocars via Twitter
RT @RoboticMasters: Thanks to @EllerbachMaxime and the Sydney Uni Capstone Students the @donkey_car @diyrobocars simulator is getting a ma…
Monday
DIY Robocars via Twitter
Jan 6
DIY Robocars via Twitter
Dec 28, 2020
DIY Robocars via Twitter
An interesting line-following simulator to use with with your robocars: https://github.com/ron-grant/LFS
Dec 23, 2020
DIY Robocars via Twitter
Dec 23, 2020
DIY Robocars via Twitter
An improved version of the @IntelAIResearch OpenBot: https://diyrobocars.com/2020/12/14/an-improved-version-of-the-intel-openbot/
Dec 14, 2020
DIY Drones via Twitter
An improved version of the Intel OpenBot https://ift.tt/2KkHT8Q
Dec 14, 2020
DIY Robocars via Twitter
Dec 10, 2020
DIY Robocars via Twitter
RT @KN_Richardson: Delivery 📦😬 Early xmas present from @ccbdcdt ! Sims ready, will put h/w together over Christmas break. Didn’t need to mo…
Dec 7, 2020
DIY Robocars via Twitter
RT @RoboticMasters: Did we mention there's new car models in the #donkeycar simulator too? Here is an RB16 from team @redbull @donkey_ca…
Dec 7, 2020
More…