How to make the drone follow a circular trajectory Indoors

[kashishdhal]

Hello @Jaeyoung-Lim ,

Would you please point me to the part of the code where I can specify a circular trajectory to the drone to follow. I looked at the trajectoryPublisher.cpp but I was not able to understand where the trajectory is being computed. Please help!

Regards, Kashish Dhal

[Jaeyoung-Lim]

This is where the circular trajectory is being computed. Is this what you were looking for? https://github.com/Jaeyoung-Lim/mavros_controllers/blob/master/trajectory_publisher/src/shapetrajectory.cpp#L97

[kashishdhal]

Thanks for the reply @Jaeyoung-Lim ,

So these are default values: trajomega = 2.0; trajaxis << 0.0, 0.0, 1.0; trajradial << 1.0, 0.0, 0.0; trajorigin << 0.0, 0.0, 1.0;

So if I launch the mavros launch file, my drone will be executing this trajectory? And if in case, I want to provide a parameter such as radius from command line then is that possible or do I need to recompile the package by changing the default values?

[kashishdhal]

Hello @Jaeyoung-Lim ,

So we were able to change the radius of the circle by changing the following function:

void shapetrajectory::initPrimitives(Eigen::Vector3d pos, Eigen::Vector3d axis, double omega){ //Generate primitives based on current state for smooth trajectory trajorigin = pos; trajomega = omega; T_ = 2*3.14 / trajomega; trajaxis = axis; //trajradial << 2.0, 0.0, 0.0; trajradial << 0.75, 0.0, 0.0; }

And we changed omega and z from the launch file. Afterwards, we launched the mav_track_trajectory_circle.launch and we see the following console messages:

[ INFO] [1593042237.527420143]: Waiting for home pose...
[ INFO] [1593042238.028106730]: Home pose initialized to: position: 
  x: -0.059239
  y: 0.173464
  z: 0.118385
orientation: 
  x: -0.0059253
  y: -0.00803153
  z: -0.0165011
  w: -0.999814

[ INFO] [1593042238.527552080]: Got pose! Drone Ready to be armed.

I armed the drone manually and switched to offboard manually from the radio.

After doing this process, we have nice 4-5 aggressive crashes.

We also tested the off_node example provided at px4.io and that is running almost perfectly.

Also, we tried to reduce the maximum acceleration and the values of K_pos but that didn't help.

Please see if you can help!

[Jaeyoung-Lim]

@kashishdhal A log would be helpful on finding the problem.

a. You need to make sure the rate controller of the px4 side is tuned properly so that it is capable of tracking the angular rate commands you are providing. b. You need to make sure the normalized thrust inputs on the geometric_controller side is tuned properly c. Only when a,b is done you need to tune the K_pos and K_vel terms

[kashishdhal]

@Jaeyoung-Lim how to make sure a and b are tuned? Is there any process that I need to follow? Please let me know.

You need PX4 log? or ROS log?

[Jaeyoung-Lim]

@kashishdhal Both, I suggest you look at the logs too to identify what the problem is.

Have you tried looking up the px4 tuning guide yet? https://docs.px4.io/master/en/config_mc/pid_tuning_guide_multicopter.html

[kashishdhal]

@Jaeyoung-Lim , I will try tuning the gains tomomorrow and will update you after that with the logs!

[kashishdhal]

Hello @Jaeyoung-Lim is there any way to tune the gains on auto-pilot? I am not a good drone pilot. I am afraid to give step input to the drone while it is hovering. I even have hard time making it hover. Please let me know.

[Jaeyoung-Lim]

@kashishdhal I have already forwarded you to the tuning guidelines of px4. Please carefully go through it

[kashishdhal]

@Jaeyoung-Lim , I have read it carefully, all it talks about tuning the gains in either manual mode or acro mode.

However, I will try to tune in offboard mode to see the values appears in log file.

[Jaeyoung-Lim]

@kashishdhal The gains for acro mode are the gains for the attitude rate controller in px4. This is what is used in offboard mode.

[kashishdhal]

Hello @Jaeyoung-Lim ,

Sorry for the delayed reply. Actually my drone frame broke so I was waiting to get the another one.

I flew the drone in a square using offboard mode. Here is the video and logs:

https://www.youtube.com/watch?v=0Y7Wr3JIEKA https://logs.px4.io/plot_app?log=7a5a7dfa-23de-45f5-9a19-9eafdc1fca18

Do you think your controller doesn't work because we need to tune? I am working on the tuning part and will update you whenever I get them tuned.

[Jaeyoung-Lim]

What do you mean it doesnt work? On the video is it not using the controller?

[kashishdhal]

Yeah I have not recorded the video while I was using your controller. We had very aggressive crashes.

[Jaeyoung-Lim]

@kashishdhal If you didn't do any tuning yet, I guess that would be one of the reasons as we already discussed above.

[kashishdhal]

@Jaeyoung-Lim okay

[Jaeyoung-Lim]

@kashishdhal If you look at the log you posted you can see that roll angular rate and pitch angular rate tracking is not good. Therefore it is not surprising a angular rate based offboard controller (geometric_controller) is unable to stabilize the vehicle.

[kashishdhal]

okay @Jaeyoung-Lim got it. I will now try to play with these gains, moving the drone in a square. In this way I can see the commanded rates and their responses.

Do you have any other preferred method of tuning apart from the moving in a square like I am doing here?

[Jaeyoung-Lim]

@kashishdhal The document I sent you above explains the preferred tuning procedures.

[kashishdhal]

@Jaeyoung-Lim I did PID tuning in Offboard mode but I was not able to achieve perfect response. I had some delay and small oscillations as well after the tuning. I re-attempted to run your controller after tuning and again it resulted in an aggressive crash. I will re-attempt to tune the PID by going outdoors this time and tuning in manual mode, maybe that will help.

However, do you think we can make your algorithm less aggressive? Offboard Controller is able to track the waypoints pretty nicely without any crashes.

[kashishdhal]

By the way, do you think this is good tuning?

https://logs.px4.io/plot_app?log=848c2148-ea72-4fbc-9e08-4a6ed97e3bbd

[Jaeyoung-Lim]

@kashishdhal I still don't understand why you are trying to tune the PID in offboard mode...

In the document that I have sent you it says you should tune the rate controller in acromode with manual control.

Ofcourse you can make the controller less aggressive if you tune the offboard geometric_controller accordingly. But the problem with your vehicle is that the tuning on the vehicle side is not correct. If the vehicle tuning is not correct it doesn't matter what you do in offboard control, the vehicle will not be able to track the commands you are sending

[kashishdhal]

@Jaeyoung-Lim , I am turning in OFFboard mode because I am not good at flying drone from RC. I am assuming to fly in acro mode I need to be a good drone pilot which I am not and I may end up crashing.

Can one fly from mavros using ACRO ? I have never done that. Please let me know.

[Jaeyoung-Lim]

@kashishdhal Tuning in offboard mode will not work. at least you need to fly it in manual and see what the response is like to your sticks. geometric_controller is basically flying the vehicle in offboard mode in acro. However, if the vehicle is not tuned correctly how is the offboard controller supposed to fly the vehicle?

I understand flying in acro is not easy - but that is the only way you see the response to your rate commands. If you are not confident you can tune it by flying in manual mode but with this you will not be able to fully tune the vehicle since you see both the response from the attitude controller and the rate controller at the same time.

[kashishdhal]

@Jaeyoung-Lim thanks, i will try that!

[kashishdhal]

@Jaeyoung-Lim , I did some tuning in manual mode but I was never able to catch up with the delay in the rate-signal. I managed to catch up only with the overshoot and reduce noise considerably low.

However, when I run it in Acro mode then the tuning that I did previously doesn't reflect any more on rate controller response, strange! But the good thing is in Acro mode I am able to catch up with the delay and made it close to zero. So I will continue to tune in ACRO until it is tuned properly because it is not making any sense to tune in the Manual mode if it doesn't help much in the response. I am learning how to fly so it's going a bit slow but we will get there soon!

[Jaeyoung-Lim]

Seems like your making progress! Awesome!

[kashishdhal]

@Jaeyoung-Lim thanks, but do you know why running in manual mode always have a delay associated in the response of rate-controller.

My belief was in the manual mode outer PID loop sets the setpoint so how should it matter if we set the setpoint or outer loop? Is there something I am missing here?

[kashishdhal]

@Jaeyoung-Lim, I am guessing that our drone was crashing earlier because of high thrust vector and we are trying to tune the PID again and again.

But I believe that we have tuned the Inner PID loop to the best we can and not sure how to tune the thrust vector. Please see these three log files where I have used norm_thrust_const with different values. Please provide your feedback.

https://logs.px4.io/plot_app?log=3dd918d2-bd43-45ae-8424-11140541ae0a https://logs.px4.io/plot_app?log=9e8baa7a-1058-4c18-bdee-967170eda477 https://logs.px4.io/plot_app?log=43670202-04f2-434e-951b-69ca0f4132d4

[Jaeyoung-Lim]

Have you also tried changing the Kp_z and Kv_z?value?

What is the inner PID loop?

[kashishdhal]

I have not tried changing the Kp_z and Kv_z value?

By inner loop I mean attitude rate controller loop.

[kashishdhal]

Can you please guide how to change the gains you specified?

[Jaeyoung-Lim]

@kashishdhal you can change it in the launchfile

[kashishdhal]

Actually, I was asking how much should I change and in which direction?

[Jaeyoung-Lim]

@kashishdhal That is something you need to figure out yourself, since I have no knowledge of your system

[kashishdhal]

Okay, I thought you can suggest something by looking at logs I sent

[kashishdhal]

Hello @Jaeyoung-Lim ,

I attempted to play with the gains but the drone is not able to fly stably. Drone rolls and pitch too much (quite aggressively) despite of the correct tuning. I looked at the log files, when roll and pitch values are less than 45 degrees then attitude-rate tracking is pretty good. However, when roll/pitch is too high then drone is not able to follow the attitude-rate commands so well. I think that should be a normal behavior, what do you think?

So, I was wondering if I should break down the process into steps. Let's say for now I want to move my drone only in X direction in a straight line. Do you think that will be helpful? Then further we will do Y straight line. Followed by both X-Y and then finally a circle.

Can you please help me with the process? Do you think it will be a better approach? If yes, please guide me on how would I implement this? I want to take the drone to a height of 1 m then hover for couple of seconds then move in a straight line for 2 m then again hover and then land.

[kashishdhal]

And here is the log file while executing your controller (made a flip in roll axis):

https://logs.px4.io/plot_app?log=3338c90c-ae55-4e16-9870-9f88717a5a35

[kashishdhal]

Also, another question, we are running your controller in the ground station, should we consider shifting that to the on-board computer? Could that be an issue?

[Jaeyoung-Lim]

@kashishdhal The fact that the drone oscillates means that the tuning is incorrect. (The drone will not oscillate if you manage to tune the vehicle correctly and it should not depend on the roll/pitch angles)

There are multiple layers of tuning you need to do

• rate control on the px4 firmware
• attitude controller on the geometric_controller (attctrl_tau)
• position controller in the geometriccontroller (Kp, Kv_)
• Thrust gains in geometric_controller

Did you verify that all elements above are properly tuned?

Okay I missed that you were running it on the groundstation. Then you need to verify that your link latency is low enough. The implementation is intended to run on the onboard controller

[kashishdhal]

@Jaeyoung-Lim , I can say we worked on the rate controller on the px4 firmware side.

In our previous discussion, we never discussed attitude controller on the geometric_controller (attctrltau). So I didn't do that. Yes, we did play with the Kp,Kv_ values. For the thurst gain we kept norm_thrust_offset the same as 0.1 and we changed norm_thrust_const until our drone is able to hover but not aggressively. Later changed Kp,Kv values as well.

Upon looking at log file, your controller commands an attitude-rate of 100 deg/sec, do you think our drone should be able to follow that?

I can understand that there are many steps to go in that direction. So I was proposing to the tuning one by one in each direction.

If I am moving my drone in one direction then that will help me play with the gains in that particular direction. Do you think that will not be helpful? Is it possible to see if we are just able to hover using your controller?

[Jaeyoung-Lim]

Not really, since it is the dynamics that are interacting with the controller

An easier way to test is if you can create a mechanical structure that the vehicle can freely rotate and you can place the drone on the desired position, so you can check the att controller separately.

Regarding the angular velocity setpoint, that is part of what you need to tune, since it depends on your hardware. You can calculate how much angular accelerarion the vehicle can create and you can tune your offboard attitude controller accordingly

However most concerning is that you are running this offboard. This repo is intended for use of aggressive trajectory control - meaning latency in your link plays a critical role. Have you verified the latency of your link? What link are you using?

[kashishdhal]

Has anyone created such a mechanical structure? I mean a paper or a document? If I can find a reference it would be easier.

Regarding the angular velocity setpoint, I can see the drone following the set-point until it is upside down then it doesn't follow, because soon it hits the ground. Do you think this is normal for your controller to command the drone to go up-side down?

I have not tested the latency of the link. But I will switch to on-board if that could be an issue.

[Jaeyoung-Lim]

@kashishdhal It is not normal unless you commanded to do so

May be https://404warehouse.net/2015/12/20/autopilot-offboard-control-using-mavros-package-on-ros/ is useful for you

[kashishdhal]

@Jaeyoung-Lim, I sent you a video earlier making a square shape with offboard control using set-point position. I have also used velocity setpoints with px4. Do you think that link is useful for me?

That is what I am trying to say that your controller is commanding the drone to go upside down. The drone will do whatever it is commanded. So it means tuning on the geometric_controller side is needed?

[Jaeyoung-Lim]

@Jaeyoung-Lim, I sent you a video earlier making a square shape with offboard control using set-point position. I have also used velocity setpoints with px4. Do you think that link is useful for me?

Yes the link shows you the mechanical structure that I mentioned, Wasn't that what you were asking for?

@kashishdhal Yes, as I told you multiple times, it is essential that you tune the geometric_controller

[kashishdhal]

Sorry, I missed that part!

I will tune geometric_controller as per your recommendations.

[kashishdhal]

@Jaeyoung-Lim , so I upgraded the onboard computer to Raspberry Pi 4-4GB from Raspberry Pi 3B. The new version has 5.8 GHz wifi so less packet loss in localization data. And then also transferred the geometric_controller code to the onboard computer. Further reduced the values of Kp_x, Kp_y, Kv_x and Kv_y. Increased the attitude_cntrl_tau to 0.15. After all this I was able to make 3 circles with the drone.

Here is the log file: https://logs.px4.io/plot_app?log=e866fb13-95fa-44aa-9b00-b528655434d1

I found out that you are publishing the reference trajectory even when the drone is landed and waiting for take-off. During the transition from the stationery to circle, the errors are quite large which results in very large desired acceleration and most of the crashes happens at that time. Because of that reasons I had to reduce the gains to quite a good extend. Is there any possibility that the drone can be provide a smooth reference trajectory to get to the boundary of the circle first? Then follow the circle? Also, how do you estimate the acceleration produced by the drone? I tried to filter the imu data but it still doesn't make sense. Please help! Thanks!

[kashishdhal]

@Jaeyoung-Lim, so this controller is working well to follow a circular trajectory and I am able to match the commanded acceleration to the acceleration coming from IMU.

Thanks for all the support and creating such a nice package for the community. I am closing this now!

[Jaeyoung-Lim]

@kashishdhal Awesome! Congrats!