[UPDATES] Bicopter stabilization algorith (PLEASE HELP NEEDED!!!)

Hey guys,

I am building a bicopter from scratch

Using:
Arduino nano
1 mpu6050
1 hc05 for communication with an android app
2 brushless motors 2300kv
2 12a esc
2 9g servos
1 11.1v 2200 mah lipo battery

I was able to stabilize the roll using a PID, but i can not figure out how to stabilize the pitch using the servos.

I tried a PID but not sure if that is the way to go.

I still can not make a take off

I will attache the code later today since i do not have it right now

I add a couple of images of the bicopter so you have an idea of what i am doing

I hope someone can give me some guidance

Thanks!!

Marco

1 Like

You already created a thread for this topic:

why start a new one? Even if there is a good reason you should still post a link to the other thread so people can see that you have already been provided some information on the subject.

pert:
You already created a thread for this topic:
http://forum.arduino.cc/index.php?topic=462312
why start a new one? Even if there is a good reason you should still post a link to the other thread so people can see that you have already been provided some information on the subject.

Yes you are right, my mistake. Sorry for that

As pert mention i created this topic when i started the project

Now i made adjustment and i wanted to create a topic with all the information.

i am also adding the code here.

I am thinking of changing the copter frame in order to put the motors on the top closer between each other.

something like the image i am attaching.

Any opinion?

Thanks and sorry again,
Marco

twincopter_v1.ino (7.94 KB)

Update:

I change the frame as it is shown inthe sketch. You can see how it looks like on the attachment. I think it is more stable now.

Still not able to do a take off.

Based on same test that i made the problem seems to be that response from the servos is to fast so the shoot to stabilize the pitch angle is npt gentle enough. I dont know if my code is correct.

Can someone help ke with that?

I will upload a video soon also

Thanks
,Marco

Hi Marco,

In your code you're only using the gyroscope X and Y axis for roll and pitch, you'll also need to read the Z axis for yaw.

For the motors use the analogWrite() function rather than the servo library. To use analogWrite() at 490Hz on the Nano use digtal pins 3, 11, 9 or 10. (Pins 5 and 6 operate at a higher frequency, too high for the ESCs). The servo library operates at 50Hz and this frequency is not enough to allow the motor ESCs to stabilise the dualcopter.

Finally, I'd comment out the accelerometer and complementary filter code for now. Before you can implement auto-level you'll need to set-up the dualcopter using only the gyroscope, with the input from the pilot converted to degrees/s. This is variously called aero/rate/gyro mode and will ensure that you have your inner gyroscope roll, pitch and yaw rate PIDs tuned correctly. This is necessary before moving on to auto-level. For the PIDs, initially tune using the P term only. You'll be able to fly using the P term alone. Add the I and D terms after you get airborne.

To test the dualcopter I'd first remove the props and test it on the bench. You'll know when it's working properly, because with zero pilot input (0 degrees/s on roll, pitch and yaw) and a little bit of throttle, the motors and servos will oppose the aircraft's motion in an attempt to stabilise it.

Your dualcopter looks cool, by the way, something out of the ordinary.

MartinL:
Hi Marco,

In your code you’re only using the gyroscope X and Y axis for roll and pitch, you’ll also need to read the Z axis for yaw.

Yes i konow i am just focusing first in pitch and roll, thanks for that!

MartinL:
For the motors use the analogWrite() function rather than the servo library. To use analogWrite() at 490Hz on the Nano use digtal pins 3, 11, 9 or 10. (Pins 5 and 6 operate at a higher frequency, too high for the ESCs). The servo library operates at 50Hz and this frequency is not enough to allow the motor ESCs to stabilise the dualcopter.

I made a switch i put the servos on 5 and 6 and the esc on 9 and 10. Can you give me an example on how to use the analovwrite? Thanks!

MartinL:
Finally, I’d comment out the accelerometer and complementary filter code for now. Before you can implement auto-level you’ll need to set-up the dualcopter using only the gyroscope, with the input from the pilot converted to degrees/s. This is variously called aero/rate/gyro mode and will ensure that you have your inner gyroscope roll, pitch and yaw rate PIDs tuned correctly. This is necessary before moving on to auto-level. For the PIDs, initially tune using the P term only. You’ll be able to fly using the P term alone. Add the I and D terms after you get airborne.

Do you mean using the raw values from the gyro??

I just hope to make a successful take off

Looking forward to reading your anwers.

Marco

Hi Marco,

I made a switch i put the servos on 5 and 6 and the esc on 9 and 10. Can you give me an example on how to use the analovwrite? Thanks!

The analogWrite() function has two parameters: pin number and value. The value is a number between 0 and 255, so it's necessary to map/convert from your 0us - 2000us ESC values to 0 - 255.

For example to set the motor PWM to mid-throttle on digital pin 9 (50% duty cycle):

analogWrite(9, 127);

Here's a link to the analogWrite (PWM) page: analogWrite() - Arduino Reference.

Do you mean using the raw values from the gyro??

I used the wrong terminology, I meant just using the gyroscope's values coverted to degrees per second.

Another thing, the MPU6050 has an in built low pass (anti-aliasing) filter. For multi-rotor applications this has to be set at around 20Hz to 40Hz. Using Jeff Rowberg's I2CDev library, you can set it with the following line in your setup() code:

mpu.setDLPFMode(MPU6050_DLPF_BW_20);

Finally, I'd also strongly recommend adding to your code an ARM/DISARM interlock for your motors. This can take the form of a button, stick movement or arm switch to allow you to remotely enable and disable your motors before and after flight. It just ensures that your motors won't burst into life if you accidentally knock the throttle stick (or whatever you're using to control the throttle), while connecting or disconnecting the battery.

The analogWrite() function has two parameters: pin number and value. The value is a number between 0 and 255, so it's necessary to map/convert from your 0us - 2000us ESC values to 0 - 255.

I will do the map. And use analogwrite
I thought that analogWrite would also need some delay in order to generate the same signal than writemiliseconds.

I used the wrong terminology, I meant just using the gyroscope's values coverted to degrees per second.

Another thing, the MPU6050 has an in built low pass (anti-aliasing) filter. For multi-rotor applications this has to be set at around 20Hz to 40Hz. Using Jeff Rowberg's I2CDev library, you can set it with the following line in your setup() code:

Thanks for this, i will remove the filter code and use what you suggested.

mpu.setDLPFMode(MPU6050_DLPF_BW_20);

Finally, I'd also strongly recommend adding to your code an ARM/DISARM interlock for your motors. This can take the form of a button, stick movement or arm switch to allow you to remotely enable and disable your motors before and after flight. It just ensures that your motors won't burst into life if you accidentally knock the throttle stick (or whatever you're using to control the throttle), while connecting or disconnecting the battery.

I will use a button for this on my android app. I have already got a finger cut because of what you mentioned :slight_smile:

I will do the adjustments today and let you know the results

Thanks!!!

I thought that analogWrite would also need some delay in order to generate the same signal than writemiliseconds.

Using AnalogWrite() there's minimal delay, as it will output the new value on the next PWM timer cycle. At 490Hz the selected pin will automatically produce a new pulse every 2.04ms (1/490Hz).

Using AnalogWrite() there’s minimal delay, as it will output the new value on the next PWM timer cycle. At 490Hz the selected pin will automatically produce a new pulse every 2.04ms (1/490Hz).

I made the code adjustments that you suggested. I still wasn’t able to check the code in arduino, but do you mind having a look at it?

Any other suggestion is welcome

Thanks a lot,
Marco

twincopter_v1.ino (8.23 KB)

Hi Marco,

I had a look at your code.

AnalogWrite() is not a member function of the servo library, but a function in it's own right, so you can just call it without the "motor1" and "motor3" prefix:

analogWrite(9, 127);

You're assigning the accelerometer Z-axis rather than the gyroscope Z-axis, as an input of your yaw rate PID.

MartinL:
Hi Marco,

I had a look at your code.

AnalogWrite() is not a member function of the servo library, but a function in it's own right, so you can just call it without the "motor1" and "motor3" prefix:

analogWrite(9, 127);

You're assigning the accelerometer Z-axis rather than the gyroscope Z-axis, as an input of your yaw rate PID.

You are right!!

I will make the adjustments and test them.

Thanks!!!

Marco

MartinL:
You’re assigning the accelerometer Z-axis rather than the gyroscope Z-axis, as an input of your yaw rate PID.

I have read that i would need a compass to get that value is that right? or is there a way to get z-axis gyro from mpu6050?

I have read that i would need a compass to get that value is that right? or is there a way to get z-axis gyro from mpu6050?

A compass isn't necessary for piloted modes such as rate or auto-level. You only need a compass if you're implementing GPS automated flight modes such as loiter or return to home, where the flight computer needs to know its heading.

The MPU6050 contains a 3-axis gyroscope that has a Z-axis for yaw, it's just that your code isn't yet reading it from the device.

With regard to your question, it means is that you can use your accelerometer X, Y and Z axes (with some trigonometry) to calculate the roll and pitch angle, but the accelerometer itself is unable to measure yaw angle (heading), for that you need a compass. In other words the accelerometer has no means of knowing where magnetic north is located.

To implement rate mode you'll need the gyroscope X, Y and Z axes, as inputs to your rate roll, pitch and yaw PIDs.

To implement rate mode you’ll need the gyroscope X, Y and Z axes, as inputs to your rate roll, pitch and yaw PIDs.
[/quote]

Should i add this?

Wire.requestFrom(MPU, 6, true);
GyZ = Wire.read() << 8 | Wire.read();

OR this

mpu.getMotion6(& AcX, & AcY, & AcZ, &GyX, &GyY, &GyZ);

Thanks

Hi Marco,

Either way.

In my first flight controller I used getMotion6, as it's just an easier and cleaner way to get all the data from both the gyro and accelerometer.

MartinL:
Hi Marco,

Either way.

In my first flight controller I used getMotion6, as it’s just an easier and cleaner way to get all the data from both the gyro and accelerometer.

Thanks

I will try both and see how it works.

Hopefully tomorrow i will be able to test this adjustments.

I attached both codes just in case anybody want to have a look at them and give me some advices.

I got rid of the aggressive kp ki and kd params

I will keep you posted.

Thanks!!!

twincopter_v2.ino (6.99 KB)

twincopter_v3.ino (6.03 KB)

MartinL:
Hi Marco,

Either way.

In my first flight controller I used getMotion6, as it’s just an easier and cleaner way to get all the data from both the gyro and accelerometer.

I am testing the values using

mpu.getMotion6(&AcX, &AcY, &AcZ, &GyX, &GyY, &GyZ);

And the values i am getting are crazy

And using analogwrite doen’t make the motors move. I found this

analogWrite(escPin, High);
delay(1000);
analogWrite(escPin, Low);
delay(19000);

And that do not work neither

Any suggestions?

Hi Marco,

And the values i am getting are crazy

What values are you getting from the getMotion6() function?

And using analogwrite doen't make the motors move.

I found the best way to approach this was to remove the props and write small sketch that feeds you throttle through to analogWrite() directly, without any roll, pitch, yaw, MPU or PID code. That way you'll know if it's working correctly or not.

The construction of a flight controller can really be broken down into a collection of mini projects:

  1. Arm/Disarm Interlock
  2. Receiver
  3. Gyroscope
  4. PIDs
  5. Motor/Servo Mixing and Output

I found it much easier think about and solve each of these topics in isolation with number of small test sketches, before gradually bringing them together at the end.

MartinL:
Hi Marco,

What values are you getting from the getMotion6() function?

I found the best way to approach this was to remove the props and write small sketch that feeds you throttle through to analogWrite() directly, without any roll, pitch, yaw, MPU or PID code. That way you'll know if it's working correctly or not.

The construction of a flight controller can really be broken down into a collection of mini projects:

  1. Arm/Disarm Interlock
  2. Receiver
  3. Gyroscope
  4. PIDs
  5. Motor/Servo Mixing and Output

I found it much easier think about and solve each of these topics in isolation with number of small test sketches, before gradually bringing them together at the end.

I am creating separate sketches to test mpu, bluetooth, and motors.

I am having a hard time figuring out how to control the brushless motors using analogwrite. Do you have a short example on how to arm them and how to power them?

Thanks,
Marco