Attitude controller not tracking command roll and pitch angles
bobbyvanthiel opened this issue · 5 comments
Dear @gsilano,
Thank you for sharing this CrazyFlie simulator.
I am adapting the crazyflie2_with_joy.launch example to work with a custom controller I build. It sends roll/pitch/yawrate/thrust command topic to the roll_pitch_yawrate_thrust_crazyflie_node.cpp at 80 Hz. The main issue is that the provided attitude controller does not properly track the commanded roll & pitch angles. Perhaps issue #43 is related to this behavior.
My setup:
- Ubuntu 18.04
- ROS Melodic and Gazebo9
- Results refer to the crazyflie2_with_joy.launch file
The attitude controller runs in the roll_pitch_yawrate_thrust_crazyflie.cpp among the other lower-level controller if I am not mistaken. This file has #define SAMPLING_TIME 0.01 which suggests this control loop runs at 100 Hz. However, I experienced that the attitude controller actually runs at ~240 Hz (when the roll/pitch/yawrate/thrust command topics are send at 80 Hz). Upon halving the custom command rate to 40 Hz I observe that the attitude controller also changes frequency but not with the same proportion. In other words, (80 Hz → ~240 Hz), (40 Hz → ~180 Hz) with (custom topic rate → attitude update rate).
This suggests that the attitude controller does not run at a fixed frequency, but is dependent on the control commands’ frequency, but also other (unkown) factors have an influence.
Moreover, the CrazyS paper suggests that the attitude controller runs at a fixed 250 Hz. 250 Hz, which corresponds to 1 / 250 = 0.004 seconds. Then why is the macro roll_pitch_yawrate_thrust_crazyflie_node.cpp set to #define SAMPLING_TIME 0.01, as this would correspond to 100 Hz. This will likely have a large influence on the attitude controller performance, as the integrator error is multiplied with SAMPLING_TIME.
Furthermore, are the control gains from the crazyflie2_with_joy.launch exactly the same as in the ones defined in the Crazyflie firmware of Bitcraze?
Lastly, do you think it is better to use crazyflie2_hovering_example.launch as a starting point, and adapt it such that it accepts roll/pitch/yawrate/thrust command topics. If so, where would you think we can best adapt this example such that it accepts these topics?
Thanks for opening your first issue here! Make sure that no other issues on the same topic have already been opened!
Hi @bobbyvanthiel! First of all, thanks for your valuable feedback and comments. I really appreciated them.
Yeah, it might be. This issue certainly needs further investigation. However, currently due to some project commitments, I don't have enough time to explore the code and figure out if and what is wrong with it. So, as I said in #43, any help would be welcome.
Anyway, have you tried to force the controller to run at a given frequency (e.g., 0.01 s) using a callback function? As in the position_controller_node.cpp file ((here is the link)[https://github.com/gsilano/CrazyS/blob/master/rotors_control/src/nodes/position_controller_node.cpp#L579]), if the problem is in the working frequency, it should be easily fixable. Also, as it's been a long time since my first release, it would be nice to update the code in CrazyS repository with the latest release, i.e., 2021.03, of the firmware.
As for the previous question, it has been a long time since I released CrazyS. At that time, I was working with the 2018.01 release of the firmware. Therefore, we can assume that for sure something has changed in these two years. Hence, I cannot answer the question about the gains.
I don't know what your plan is, but if at the end of the day you would like to test your controller with the physical vehicle, I'd rather start checking out the differences with the onboard firmware. Afterward, if there are any changes with the parameters, and finally use the crazyflie2_hovering_example.launch file as the basis for your control chain.
I hope this helps. Please, let me know if you have any further comments or questions.
Thanks for your response! I dove a little further into the roll_pitch_yawrate_thrust_crazyflie.cpp's control loop frequencies. All controllers seem to run at around 161 Hz (per simulation second). This is also true for the position_controller.cpp from the crazyflie2_hovering_example.launch. Opposed to what I said earlier, this frequency is fixed. Therefore my first question is if all controllers operate at a fixed frequency compared to simulation time to real-time? My simulation runs at a real-time factor of 0.6.
Secondly, could you please elaborate on the frequency of 161 Hz? Your paper suggests that the all controllers should run at 250 Hz, except for the rate controller, which should run at 500 Hz. However, on the top of the controller files, SAMPLE TIME 0.01; is stated. This would imply a frequency of 100 Hz.
How did you measure the working frequencies? Did you use the rostopic hz (more info here) to measure the publishing rate on topics of interest?
Although errors may exist in the code, the callback functions triggered by ros::TimerEvent should guarantee, within certain limits, the working frequencies of the controllers.
To speed up the real-time factor, you could remove not necessary plots from the launch file. The real-time factor is an existing problem that I ran into. I'm aware that other simulation environments, such as MRS System, do not have such issues. However, we should delve into the RotorS implementation (it is used as a basis for CrazyS), to see where this error comes from.
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.