Odd behavior of position and attitude controller as well as the physical model

[AnandaNN]

Hi @gsilano,

Thank you for this great package for simulating the Crazyflie MAV. I am running Ubuntu 18.04 with ROS Melodic and Gazebo9 and have installed and run your hovering example.

I am however experiencing some problems once I try controlling the X and Y directions. If I just change the reference from (0,0,1) to (1,1,1) i get the following response

Which is obviously not what we would want to see. Besides the position problem I would also expect that drone would have moved much further when at these roll and pitch angles for so long which could indicate a problem in the physical model.

To investigate a bit further made a new controller node which is based of your position_controller.cpp and using the crazyflie_onboard_controller.cpp. The controller uses your HoveringControl(...) to make it hover at 1 meters height and then it accepts roll and pitch values from the user instead of from a position controller. Running this I get the following response when trying to control the pitch angle:

Which looks like there could be something with the attitude controller and maybe it's gains.
Also it only manages to move 20 cm in ~7 seconds of tilting 5 degrees which doesn't seem correct.

Thank you for any help or insight you can provide.

[welcome]

Thanks for opening your first issue here! Make sure that no other issues on the same topic have already been opened!

[gsilano]

Hi @AnandaNN!! When I started developing the CrazyS ROS repository I thought it was useful to have a Crazyflie 2.0 Gazebo simulator. So, I started developing the package using the RotorS simulator as a base. The hovering_example provided in CrazyS package is just a simple cascade controller used as an example (it is inspired by this technical report) to show how the package works. Of course, this cannot be considered as a good tracking controller, but it can be seen as a reference guide on how to use the Gazebo simulator when considering the onboard Crazyflie software implementation (i.e., the state estimator and low-level controller).

As for your question, seeing the graphs, the problem may be related to the low-level control gains (see the controller_crazyflie2_with_stateEstimator.yaml and controller_crazyflie2.yaml, in the rotors_gazebo\resource folder), even if it looks like drone roll and pitch angles saturated in some way. I don't think so, but it could also be a problem in computing the error signals.

Could you help find the problem? Could you do some experiments by changing the control gains to see if the roll and pitch angles are saturated? In this way, we could rule out the reasons why the controller is not working as expected.

Please, consider also that I'm working on implementing an INDI and Mellinger's controller algorithms. More details are described in PRs #29, #28 and #27.

[AnandaNN]

Okay I will take a look at the report. As I do not need the position control for my current project I will continue looking at the roll and pitch control.

For the graphs i used the parameters from controller_crazyflie2_with_stateEstimator.yaml but I will try the test with different gains and update here how it looks. I will also try to log the errors to see if they look correct.
It is my understanding that the default controller gains should work without tuning needed?

As I understand your work, the crazyflie_onboard_controller.cpp is the code implementing the basic pid roll/pitch/yawrate/thrust and rate controller as seen (the 2 other are the onces your are developing now?):
https://www.bitcraze.io/2020/02/out-of-control/

The crazyflie_complementary_filter.cpp is the onboard attitude estimation correct?

[gsilano]

The default control gains are set to work in reality and in a not well-defined scenario (no information is on the website) that may be different for the one tested in Gazebo. Furthermore, the sensor's response (i.e., accelerometer and gyroscope) may be slightly different from the real implementation. So, without a hardware test, nobody can say how much the software platform (i.e., CrazyS) reflects the real behavior of the system. It should be similar since the official data on the Bicraze website and those on the sensors' datasheet (more details are described in the reference papers) are used but it is not easy to say how much.

Yes, the low-level controller was implemented as in the Crazyflie firmware release. As you can read from the top of the code, I also referred to the commit used for controller implementation. Please, consider that the actual implementation of the code may differ from that in CrazyS.

Yes, the crazyflie_completary_filter.cpp file contains the software implementation of the onboard attitude estimator. As I said before, remember that CrazyS is based on the 2018.01 Crazyflie firmware release that may be different from the current one.

[AnandaNN]

Okay that sounds very good. I will continue looking and post some data with different gains once I have performed the test.
Thank you very much for the clarification.

[AnandaNN]

So I did 3 tests to start where I changed one controller parameter at a time. These values were attitude Kp and Ki and the Rate controlller Kp.
Well running I also monitored the rate and attitude error and both of these seemed correct and not around zero.
The plots below show the 3 responses.

Change attitude: Kp = 5.0611 (was 1.0611)

Change attitude Ki = 1.0349 (was 0.0349)

Change rate control Kp = 3000 (was 1000)

Of course increasing the gains got it closer but it is still quite a way of even with big changes. I also started noticing it reaching maximum motor velocities even at these roll and pitch values.

[gsilano]

Looking at the graphs, I don't think the issue is in the low-level control but in the signal generator or in between (the software interface that manages to pass the data from the reference generator to the controller). I mean, even if the pitch command provided with the joystick is equal to 30 degrees there may be a saturation in the code. In fact, if you notice, the drone's roll angle is always limited to more or less 10 degrees.
To graph the signals, where do you measure them? In the script that implements the controller? From the reference topics?

If you could help me understand the origin of the problem I would be grateful. As you have seen, these days I have devoted a lot of energy to respond to the issues and work on improving the code. Unfortunately, tight deadlines force me to move away, but if you could help me together we could solve the problem or at least understand what went wrong.

[AnandaNN]

I'm not sure I would rule out something with the controllers/gains as large ki does make the pitch angle approach the reference value, albiet slowly. But on the other hand it is very odd if it is only me who is have these issues as I haven't changed any code in the controllers.

The graphs are taken from rqt_plot so using published topics.

I'm sorry I can't really point to the origin of the problem as I am unsure myself. The only guidance I can give is the steps that led me to my problem:

• Download and install CrazyS on Ubuntu 18.04 with ROS Melodic and Gazebo9
• I then ran the hovering example, which worked
• Then I tried the joy stick controlled launch you provided. Saw that the roll and pitch angles didn't reached what I referenced with my joystick (I modified the joy_crazyflie.cpp file to give a variable thrust, not only MAX_THRUST or 0, but this shouldn't change much just made it easier to manually hover it)
• This is when I started making graphs and debugging and so forth.

Unfortunately I need the simulator ready already in June for a course in drone control that I'm helping prepare, that now needs to be virtual because of the Corona virus, so for now I have shifted to using the basic rotorS drone Ardrone as this one worked after installation so that I can prepare exercises and so forth.

Before I stopped looking at CrazyS I did create a new version of the onboard controller that uses the crazyflie firmware pid controller with gains provided by bitcraze. With that version of the controller I did get the Crazyflie to follow the reference roll and pitch, but it still had problems in the physical model(moving very slow even at roll and pitch of 30 degrees).
I have created a fork of your work to: https://github.com/AnandaNN/CrazyS.git and my changes are in the ros-melodic branch. Because I was still debugging and testing things it's very hacked together and maybe not that pretty.
If you are interested in this I can write some notes to help understand what I have changed and/or added.

[gsilano]

Hmm. It's strange. I can't rule out the presence of a bug in the code because of the graphics, but I could be wrong. Since the structure of the package is very complex, it is not easy to identify the presence of a possible bug. I will try to manage the problem in my spare time and I will update the issue as soon as I have news.

I understand your point of view, and I fully agree with you. If you could write something to help me for understanding what you did, it would be fantastic for me.

[gsilano]

An update on this issue. Thanks to #50, I discovered a bug in the code. Perhaps, this could be the reason for that strange behavior. As soon as you have time, @AnandaNN please download my latest changes and run a simple test for me. Thanks!!

[AnandaNN]

Made a small update to the readme at simply describing what I added to use the firmware PID controller if you are interested.

I will look into your update and get back to you.

[AnandaNN]

@gsilano Running similar tests as I did with the first version I tried I can't really tell any difference, sorry.

[gsilano]

Thanks for the help. I'll take a look. As soon as I have updates, I will share them with you.

[GreatenAnoymous]

Hi, @AnandaNN ,I also had the same problem hovering after changing the desired postion from (0,0,1) to (1,1,1)-------the crazyflie moves slowly along X-Y and takes very very very long time to reach the goal.
This issue can be solved by tuning the gains of X-Y controller and attitude controller. The position controller itself is just fine.
As an example, attached is the yaml file of the crazyflie controller parameters I tuned. It may be not very perfect but it just works.
crazyflie_yaml.zip

# Crazyflie 2.0 vehicle parameters
mass: 0.27
inertia: {xx: 1.436e-05, xy: 0.0, xz: 0.0, yy: 1.436e-05, yz: 0.0, zz: 2.173e-05}

# angle should be expressed in rad
# direction 1 means counter clockwise, -1 clockwise
rotor_configuration:
  '0': {angle: 0, arm_length: 0.03973, rotor_force_constant: 2.8799e-08, rotor_moment_constant: 3.5909e-10, direction: -1.0}
  '1': {angle: 1.57079632679, arm_length: 0.03973, rotor_force_constant: 2.8799e-08, rotor_moment_constant: 3.5909e-10, direction: 1.0}
  '2': {angle: 3.14159265359, arm_length: 0.03973, rotor_force_constant: 2.8799e-08, rotor_moment_constant: 3.5909e-10, direction: -1.0}
  '3': {angle: -1.57079632679, arm_length: 0.03973, rotor_force_constant: 2.8799e-08, rotor_moment_constant: 3.5909e-10, direction: 1.0}

# Position controller parameters
xy_gain_kp: {x: 100, y: -100}
xy_gain_ki: {x: 1, y: -1}
attitude_gain_kp: {phi: 3.4, theta: 3.4}
attitude_gain_ki: {phi: 1.5, theta: 1.5}
rate_gain_kp: {p: 1000, q: 1000, r: 1000}
rate_gain_ki: {p: 0, q: 0, r: 96.7}
yaw_gain_kp: {yaw: 30}
yaw_gain_ki: {yaw: 0}
hovering_gain_kp: {z: 11000}
hovering_gain_ki: {z: 3500.}
hovering_gain_kd: {z: 9000}    

[marios-stam]

Hello,unfortunately I am also having the same problem and I am getting the following results when I set
Eigen::Vector3d desired_position(1.0, 0.0, 2.0);

I also tried the parameters from @GreatenAnoymous but that didn't work either.
Do you have any suggestions?
I am working on ubuntu 20.04 LTS and ros noetic ,I don't think that this problem is because of the following changes i made:

• python --> pyhton3
• cv.h --> cv2.hpp

[gsilano]

Hello everyone! I think it is time to address this problem. Unfortunately, I don't have a lot of time to invest so any help from you would be appreciated. So, please, email me to see how to face this problem.

[gsilano]

Hi there! These days, I've been working on the integration of Lee's geometric controller developed by Stefano Grossi. The code builds, but the drone does not take off. I think it's just a matter of tuning some parameters or checking the controller (i.e., position_controller_alpha.cpp file). I would be grateful if somebody could help me with this. Thanks!

[stale]

This issue has been automatically marked as stale because it has not had recent activity. It will be closed if no further activity occurs. Thank you for your contributions.

[gsilano]

I'm working on #86 to fix this issue. Updates will come soon. Closed for inactivity.