Revamp position controller

[ataffanel]

Currently the position controller is a single position PID, it should be revamped to include a velocity loop. This would allow to use the new experimental PID for the attitude controller (see a0628ec2c).

[mikehamer]

Another very important addition would be the inclusion of feedforward terms. For example, if a trajectory is planned ahead of time, the desired position, velocity and acceleration (and therefore thrust and body angle) are known for each time instant. Giving these derivative terms to the controller as "feed forward" terms allows significant improvement in trajectory tracking.

[ataffanel]

Shall we add that as a separate ticket maybe and concentrate this one on fixing the controller for the curent setpoint structure. Or is it required to define the "feed forward" architecture first?

[stephanbro]

I've implemented a velocity controller that takes in the position PID controller output and generates the attitude setpoint information. You can see the commit here: c3c72d7414aca1c907f16acb9c7a1fc627c35500

This gives us much better control over things like the maximum velocity and we also have the potential to run the velocity control at a higher frequency.

[ataffanel]

We just tested it and saw great improvement in stability for stationary setpoint and it basically removes position overshoot. We have seen some strange trajectory when going from one setpoint to another but we have to test more and it is anyway much better than before! I have tried to set the max x/y velocity to 5m/s (being a bit greedy :) but it then starts to overshoot again.

For the height control it still oscillates a bit but previously I got it more stable by setting PID_YAW_RATE_KP at 150.0. We will test that tomorrow as well.

[stephanbro]

Glad to hear of the improvement! I haven't had a ton of opportunity to do tuning, but right now there is no D term, which could likely help out with the overshoot you were seeing.

Feel free to send me any rosbags or logs if you notice anything funny.

[mikehamer]

Reviving this discussion...

I have a student working on swarm control and he would benefit greatly from being able to send feed-forward terms (eg. a trajectory in terms of position, velocity and acceleration). I've therefore thrown together a framework for a controller and communication packets that should enable this, and am well on the way towards implementing the controller.

The packets are defined with minimizing communication in mind:

1. You shouldn't need to send a separate packet to indicate an emergency, this is therefore included in the header. It should interact with potential emergency shut-off functionality (see #168).
2. You should be able to define the control mode in the same packet that has the control input (no more setting parameters)
3. You shouldn't need to send a separate packet for an external position

This packets are shown in controller_new.h

The controller is designed to be easy to tune, with each axis modelled and tuned as a second order system with a time constant and damping factor. This is half implemented in controller_new.c, outstanding is the final conversion from desired accelerations and body angle estimate, into body rates. I'm not a quaternion master, so still wrapping my head around this one.

Happy to hear any feedback / accept any pull-requests ;-)

[mikehamer]

@mgreiff, perhaps you're also interested in this discussion.

[mgreiff]

Excellent that the discussion has been revived, and thanks for inviting me to the conversation Mike.

I am working on similar things at the moment, I think it makes sense to use the structure that you had in mind for the positional control if using simple controllers. However, I too am thinking about swarming quadcopters, in which case it would perhaps make more sense to load an entire trajectory (a priori or in real time) instead of limiting the packages to simple points. Judging by your implementation i take it you know that the quad-rotor is differentialy flat [1], and if we have well defined state trajectories in (x,y,z,yaw), then we may compute not only translational accelerations and body rates but also body accelerations and feed-forward thrust and torque in the body frame. This enables interesting controllers such as that used in the work of Mellinger and others [2][3], and I have a working implementation of both the flatness and SE(3) controller in Simulink, with a C implementation in progress. My hope was to keep the basic structure as is, with a commander for communication which sets a structure used in a state controller etc., but we can of course try to communicate directly with the state controller as you have implemented it currently.

The basic structure I've envisioned is summarized as follows:

A. Sequence Commander Load a trajectory in the flat output space constituted by a combination of (1) Points (similar to the output generated by Blender or simple setpoints) (2) Polynomial splines of arbitrary order (using either minimum snap QP-splines or simpler bezier curves) (3) Sinusoid functions defined by their amplitude, offset, frequency and phase which need not be the same across all flat outputs and can be updated in real time.

If you take a look at the referred papers, you will note that the fourth derivatives (snap) of the flat outputs are required to compute the body rates and feed forward thrust/torque-terms. My idea is therefore to LP filter the points through a fourth order system if using (1), generating known and well defined flat outputs up to and including the required higher order derivatives. Naturally, the derivatives are well known at all times in (2) and (3).

B. Flatness generator From this data, the flatness equations are applied computing a reference position, velocity, rotation, body rate, body acceleration, thrust and body torque as required.

C. State controller Implemented more or less exactly as described in [3], which is quite similar to the work you've done so far. I have these equations written down in C, but have not yet flown with it.

In conclusion, one idea is to (i) work on your current code and communicate in the controller, or to (ii) revitalize the current commander.c with a new package structure and keep the implementations separate, but if you need body rates it could make sense to (iii) devise a commander such as the suggested sequence commander, which can then be used at all times and supplemented if new data is transferred. I will upload my codes on this tomorrow, and after that we could perhaps decide on how to best move forward. What are your thoughts on this?

[1] http://authors.library.caltech.edu/28129/1/CDS97-008.pdf [2] http://www-personal.acfr.usyd.edu.au/spns/cdm/papers/Mellinger.pdf [3] https://www.math.ucsd.edu/~mleok/pdf/LeLeMc2010_quadrotor.pdf

[mgreiff]

The repository is not quite in the state I had hoped it would be, but it compiles and shows one way of implementing the flatness/SE(3)-controller. The files of potential interest are suffixed with "M_" (see M_sequenceCommander.c, M_flatnessGenerator.c and M_control.c). The SE(3) controller implementation has been tested in real (see Mellinger's PhD Thesis), and I have validated the theory by simulations. Similarly, I have derived the flatness equations by hand, verified the math in simulations and compiled a short document on the math which I can share if you are interested. Currently, the repository is on Gitlab and undergoing heavy revision, but even if we don't end up using the structure presented here, the basic equations and SE(3) controller could still be of some use.

https://gitlab.com/mgreiff/crazyflie-firmware

@ataffanel @mikehamer

[mikehamer]

Thanks for your thoughts, Marcus.

I agree that loading trajectories onto the crazyflies would be quite useful (although not to me at the moment, but perhaps in the future). It would be logical for this layer to exist on top of the new controller, which would then accept setpoints either via radio, or via onboard setpoint generation from preloaded trajectories. This would be a fairly flexible implementation allowing for many different use cases.

I'll take a look at your implementation over the next day or two and will give you some feedback.

[mikehamer]

@mgreiff Have you set up the GitLab permissions correctly? I can't seem to see anything in the repository? My username there is also mikehamer (if you would need it for the permissions).

[mgreiff]

@mikehamer Thanks for the input. You're right, however, the current code should be able to support single point trajectories in flat output space as well - and it would be interesting to see how it would work if we get the entire system up and running :) You're right, I had forgotten about the permissions, but now both you and @ataffanel should be added. Note that the codes are works in progress, and that there remains much to be done..!

If you would like to discuss something then I'm available at Skype at most times. In addition, feel free to edit the code as you wish - my time line is to validate the flatness implementation tomorrow by comparison to experiments and simulations of the dynamics. If all goes well, I'll try to fly with the entire system by the end of the week.

Have a good evening!

[mikehamer]

@mgreiff, your implementation is very thorough and quite far progressed—puts my efforts to shame! Let me know if/how/where I can help you get this working and I'd be happy to contribute / test!

[mgreiff]

@mikehamer Thanks! Quite a bit remains to be tested, tuned and debugged. As soon as the sequence generator/flatness has been validated properly (hopefully in < 4 days), we could think about trying to tune/debug the controller and power distribution.

My plan is to replicate a the Simulink flatness generation in the firmware by first having a known trajectory pre-loaded, so one thing that could be done independently and simultaneously is the loading of data through packets instead of parameters. Feel free to take a look at it if you like (and of there is time) - if you have some functionality you'd like to add/remove then don't hesitate to do so.

[ataffanel]

@mikehamer, @mgreiff, what is the status on that? Is there anything I can test or help with?

[mikehamer]

Status is good

I have implemented a quaternion-based controller for the Crazyflie, similar to the one we use here for our larger quadrocopters. The controller is working quite well, especially on new Crazyflies (with good props and motors) and I have a student who has been using it successfully for the last month.

The controller (and associated command packet):

• allows a reference to be given in terms of position, velocity and acceleration in x, y and z, as well as yaw and yaw rate
• is based on quaternions, meaning it can control the quad through all body angles
• flips are therefore possible (pad your room first... I have not tested this nor take any responsibility for broken Crazyflies)
• is very intuitive to tune, since the controller parameters are based on time constants and damping factors.

I have just cleaned it up enough such that I'm happy to release it, and I would be very thankful for people's feedback and help with debugging/improving it!

The release also includes a new model-based power distribution, a physical constants file, and a cfmath.c file to collect all the quaternion, vector, etc math that is used in the kalman filter and controller.

You can find the release at (nonlinear-controller branch): https://github.com/mikehamer/crazyflie-firmware/tree/nonlinear-controller

You will need a modified version of crazyflie-ros (enabling the new packets, full_control branch): https://github.com/mikehamer/crazyflie_ros/tree/full_control

You can use the following joystick control app to test: https://gist.github.com/mikehamer/1d3e7c3f03dfdbc3a5fc302804dcdba5

You will need the following two lines in your config.mk:

CONTROLLER = new
POWER_DISTRIBUTION = cf2

Looking forward to your feedback and improvements!

[ataffanel]

Hi Mike, I have tested and it looks really good. I am mostly impressed about the stability when commanding big velocity steps (like +3m/s to -3m/s in sequence). It is also pretty interesting that when asked for a big acceleration down, the crazyflie actually tries to flip in order to accelerate faster.

I have been able to observe that when commanding big position step the Crazyflie breaks a bit too early: https://drive.google.com/open?id=0BxnrWSPpcVHla2VHN1BxcWItWkk. Though this seems to be more related to the estimator than the controller and it should not show in more 'casual' flight trajectory.

[mikehamer]

Yes, I agree that the early braking is more likely to be a fault of the estimator. Running some tests under motion capture, it seems that the onboard velocity estimate is rather noisy and doesn't track the actual velocity very nicely. Can anyone with a motion capture system confirm this?

[mikehamer]

I just confirmed that the issue with the controller braking before the position setpoint is due to the estimator. When flying using external position measurements (mocap) to update the onboard estimator, step changes in setpoints look really smooth and the braking results in the crazyflie hovering at the correct position.

[mgreiff]

Hello Mike,

Sorry, for the late reply! I was away for a couple of weeks and then had to prepare my thesis presentation and defense, due Wednesday. I have had time to test the controller briefly, and it seems to work nicely. Excellent work!

A few comments on the implementation

• I simulated the controller some time back and recall that the heuristic for yaw rotation had little effect on general performance. So as for your comment in the code, I the heuristic can probably be ignored for now.

• When considering looping maneuvers, specifically when |arccos(R[2][2])|>pi/2, couldn't collCmd = constrain(accDes[2] / R[2][2], coll_min, coll_max); result in potential issues? It might not be all too relevant now, but it could be worth adding an exception when R[2][2] is small or switches sign, especially if loops are to be considered.

• In the referred paper, feed-forward terms in body rates are used. The code for this, concerning flatness, already exists and it could be interesting to see if it could improve the overall controller, especially during the more aggressive flights.

• The idea of sending the external position in the same packet as the reference is nice, also, the power distribution is much cleaner than what was previously done.

Moving forward A nice next step, from my point of view, would be to consider the general architecture. I would very like to support the embedded sequence evaluation, similar to what was done for the SE(3) controller. @mikehamer Could I migrate the stateControllerCrtpCB(CRTPPacket* pk) code in a separate block, similar to the sequence generator, or do you thing we should keep the implementations separate for now?

Perhaps it could be interesting to make a qualitative experimental comparison of the non-linear quaternion and SE(3) controllers for future reference in other projects?

[mikehamer]

Hi Marcus,

Not a problem, best of luck for your thesis presentation!

To your points:

1. Correct, most likely it wouldn't have much effect.

2. I'm not sure if I see what you're seeing: if the quad is upside down (R[2][2] < 0) and the desired vertical acceleration is in +z (accDes[2] > 0), this will constrain the thrust to coll_min, eg produce minimum thrust in the upside-down direction until the quad can right itself, while if the quad is the right way up (R[2][2] > 0) and -z acceleration is desired (accDes[2] < 0), then the quad will also produce coll_min. If I am not mistaken, the current line works to this purpose? Have I missed something? (certainly possible)

3. Feedforward body rates would be possible, however aren't implemented due to packet size limitations and me not needing them. We could certainly think to move the packet handling code into a separate file/module and have a control_t struct passed to the controller, exactly as you did for the flatness generator.

4. I actually don't use this at the moment, but it is a forethought to try and reduce communication to crazyflie swarms

Moving forward

• Agreed that we should see if we can integrate this with your sequence generator. I have been testing the controller quite extensively and it is very stable and quite responsive.

• Agreed that we can move the packet handling code elsewhere (and standardise the packets with the sequence generator). Important is to maintain the emergency switch in the packet.

• I would be interested in such a comparison, but no bandwidth to do so myself. The largest problem I found with the SE(3) controller is tuning: I found this very unintuitive, and relying on a non-linear combination of many gain variables which were hard to reason about (or perhaps I didn't understand the controller well enough). In addition, The controller performed nicely on a new crazyflie with new motors, however quite poorly on a damaged crazyflie. The quaternion controller seems to be more robust in this manner.

• My suggestion for moving forward is also trying to improve the estimator performance, since this affects the controller quite significantly.

[mgreiff]

Thanks!

Regarding (2). You're likely right. My concern was the switch at at R[2][2] = 0 from maximum to minimum thrust (which I, at the time of writing, thought was coll_min=0). It will probably be alright given the robustness of the attitude controller, but remains to be tested.

Regarding (3). Excellent, I'll see if I can make the feed-forward body rates optional with the pre-loaded trajectories but will not make it a priority for now.

Moving forward

• What I'll do this Thursday-Friday is to make a PR with an initial implementation of the agreed modifications, making both the non-linear controller and SE(3) optional from the config.mk file. I'll won't change the emergency switch in your packets, but we could perhaps consider putting it on a separate channel in the future.

• I agree, the estimator works very well but could be improved further. I think the first steps there are (1) simply profile the computational effort required to go from scalar to matrix updates with the current model (2) consider modelling the inertia and using the (now known) reference torques in the prediction while still having scalar updates.

[mikehamer]

Just pushed some updates which fix some issues when commanding a large downward acceleration (eg. the case where the CF flips). If anyone had time, it would be nice if you could figure out the root cause of the problem (can be excited by entering acceleration control mode and commanding xdd = 0, ydd = 0, zdd = -20), which before the final commit will cause the CF to drop from the sky and restart due to a negative square root.

https://github.com/mikehamer/crazyflie-firmware/tree/nonlinear-controller

[whoenig]

I just saw this thread. I just would like to point out that Mellinger's non-linear controller, and on-board trajectory evaluation (and generation) has been implemented as part of the Crazyswarm project (https://github.com/USC-ACTLab/crazyswarm). We are also working on integrating our changes back into the official firmware (but started with the radio and nrf changes). It would be great if we could coordinate our changes.

I see that your controller is different, so that's a nice alternative. However, it would be certainly important to decouple the controller from the CRTP packets. The external position should be used from the ext_position module and the setpoint should come from the setpoint structure.

[mgreiff]

Hello Wolfgang,

The status on the positional controller is that both the quaternion- and geometric SE(3) controllers work, though the quaternion controller seems to be a more robust alternative than Mellinger's geometric controller, especially in a UWB context.

I do agree that we should coordinate the changes and preferable decouple the controller from the CRTP packets eventually. What I'm working on now is to make the commander generic to a point where trajectories (polynomial, bezier, sinusoid, steps) may be pre-loaded, but also enable support of real-time feed of control points through the CRTP-framework.

The idea is to separate the implementations in a sequence_commander.c, which fills a setpoint_t structure (extended to include FF-terms). The geometric, quaternion or any other positional controller may then be compiled from the config.mk file, and will all be invoked using the stateController(&control, &state, &setpoint). The computed control signals for the power distribution will be of thrust [N] and torques [Nm].

My workload increased to new heights this past week, but I hope to submit a PR with the above changes tonight or tomorrow - the code is almost in a good state!

@whoenig What do you think about this? Would you like to add/change anything? @mikehamer What do you think about using the ext_position module for positional updates?

[mikehamer]

Agreed that we should

• decouple the CRTP packets from the controller implementation, standardise the controller interface, and have the controller implementation selectable in the config.mk
• standardise the power distribution interface (ie. the output of the controller), this should be based on physical quantities (thrust [N] and torque [Nm]) since we can then leverage a model-based power distribution. This is already implemented in my power_distribution_cf2.c

To standardise controller interface and define a generic packet, we need to decide what exactly should be in the packet. In my opinion, the current quaternion controller packets are a fairly good example. They include:

• Ability to select different control modes for each axis (eg. pos, vel, acc control). For example, in my quaternion controller I can achieve a very smooth takeoff and landing by using velocity control on X,Y,Z to command a vertical ascent/descent at 0.5m/s for half a second, before switching the X and Y axes to position control such that the CF stabilises itself in X and Y. I finally switch Z to position mode when the quadrocopter has ascended to within a few cm of the desired takeoff height. This results in a very smooth takeoff without the hack of manually adjusting the position setpoint.
• Feedforward terms (velocity reference, acceleration reference, body rate feedforward)
• Ability for an emergency shutdown

For me, the above are the minimum requirement for any generic packet. The external position in the control packet was added as an afterthought for my application and is not necessary in a generic packet—we have after all the ext_position module.

I don't think we should overload the same packets to send data to the sequence generator. This should be a separate subsystem on a separate CRTP port, and should be possible to disable entirely in the config.mk

[whoenig]

I think the plan is good. I like the idea for using standardized units for power distribution.

A few more comments:

• For takeoff/landing we just have a canned trajectory (polynomial based), which we can dynamically stretch based on the desired take-off height and duration. This is very efficient and smooth. I can see that your different modes help as well, but I would consider that more hacky and less autonomous (we can broadcast take-off and all CFs will plan and execute their take-off trajectory).
• For emergency, one can just send a CRTP packet with thrust 0, or not send anything (the CF will shut-down after it didn't receive a new command for x ms).
• I think we should leverage Arnauds work on the new generic commander packet, rather than inventing something new: https://github.com/bitcraze/crazyflie-firmware/blob/master/src/modules/src/crtp_commander_generic.c; I agree that we need to add a version with feedforward terms.
• I agree that it would be nice to be able to disable/enable certain parts. However, this could also be just an #ifdef; In particular, I am worried that if we have a separate sequence generator certain behaviors might be "weird". For example, a manual backup pilot should be always able to take over, no matter if a trajectory or set-point is executed. With the current solution there is a mediator in place already. With a new sequence generator we would need to be careful to mediate and/or switch between the two modes correctly. If we just add it to the new generic crtp_commander things should work out without additional mediation.
• for ext_position I am planning to add a "broadcast"-friendly version (which can pack 2 poses per packet) and supports quaternions. This is useful for Mocaps to correct for yaw-drifts more efficiently. Also, for swarms, the broadcasting allows to use fewer radios in total.

[mgreiff]

I have today worked on combining Mike's quaternion controller with the SE(3) controller in a unified application using a sequence commander. Here is a brief update on the implementation relating to your comments @mikehamer @whoenig @ataffanel . It includes what has been done so far and what I will PR tomorrow once I have verified that the flight works with all controllers using the UWB network.

I think we should leverage Arnauds work on the new generic commander packet. (---) If we just add it to the new generic crtp_commander things should work out without additional mediation.

Agreed. I'm currently using the trajectory/fullcontrol/synchronization as we defined in the full_control branch, but I will work from Malmö tomorrow together with Arnaud and possibly make use of the generic packages instead.

For emergency, one can just send a CRTP packet with thrust 0.

I would rather suggest that we use a packet where we explicitly state that the emergency should be requested, making the implementation less ambiguous and the emergency reversible. Alternatively, we could send a packet to a separate channel and force an assertion for a hard emergency. What do you think?

For takeoff/landing we just have a canned trajectory (polynomial based).

Agreed. Do you think it would make sense to have some sort of event-TOC in the commander? Such that all settings such as controller->enable or more high level commands such as "takeoff", "take image" or "find nearest landing spot" are listed and given an integer ID with a boolean status? If so, these could be set through parameters or the generic packages, but also parametrized in time such that a positional trajectory will terminate in - for instance - a polynomial landing maneuver without user interaction switching between controller modes if needed. What do you think?

[whoenig]

• Agreed with the emergency state as separate packet (could be part of the crtp_commander though)
• For trajectories we used so far the following interface: https://github.com/USC-ACTLab/crazyflie-firmware/blob/crazyswarm_master/src/modules/interface/packetdef.h#L106-L201. Each of the "high-level" commands is parameterized. Right now, we require the host to send new commands accordingly, i.e. there is no sequencing on-board. I think doing so is probably overkill. However, we would like to use the new SD-card deck to store pre-defined command sequences. Then a high-level command could be "play sequence from sd-card with id 7". Each sequence on SD-card would most likely be just a high-level command (or trajectory) with parameters and a duration (for when the next entry is supposed to be executed).

[ataffanel]

For Emergency there is a "stop" packet in the new commander: https://github.com/bitcraze/crazyflie-firmware/blob/master/src/modules/src/crtp_commander_generic.c#L71. It sends a 0 setpoint that should stops all motors. Is that good enough for now or do we need a more proper E-stop that would sets the Crazyflie in emergency mode, preventing any further setpoint to start the motors, and guaranteeing that the motors are off (bypassing all the stabilizer loop)?

[mikehamer]

A few thoughts, in which I define an emergency to be a state in which power to the motors is shut off:

• An emergency should be triggerable both from other modules on the CF, and from a CRTP packet.
  • This is why, in my implementation, the enable/disable was part of the control structure that is passed around
• An emergency should be an explicit emergency and should require a different action to leave the emergency state (eg. an emergency reset packet/bit/whatever).
  • Sending a thrust of 0 is therefore not suitable
  • since the controller sending a different thrust would reenable the motors
  • since this would make setting an emergency by other modules on the CF difficult
  • and since defining an "emergency" as zero thrust requires the controller on the computer to track the emergency state to ensure it doesn't send a non-zero thrust until the emergency is over
• Having an explicit emergency would allow a supervisor program to run on the computer, inspecting the state of a CF and trigger an emergency for given conditions (eg. flight outside the flight boundary), irrespective of the operation of the primary control program.
• An emergency should not stop other modules from running—eg. LPS, estimator, CRTP, etc. In this regard it is very different to a processor fault or an assert.
• An emergency should be resettable without resetting the CF (motivating the above point)
• For maximum safety, there need to be guarantees that if the CF is receiving packets, it will stop in the case of an emergency.
  • This is the reason for having an emergency bit in the header of the control packet, since if the control packet is received with the emergency bit, the CF will stop. And if no packet is received within a defined duration, the CF will stop. If triggering an emergency is moved to a different packet, there is an edge case scenario whereby control packets are received, but the emergency packet is not received, and the CF therefore keeps flying. This is, however, most likely a minor issue.
  • However, I appreciate that when executing onboard trajectories/sequences, there may be very little communication from the computer to the CF, in which case an emergency packet is advantageous.

In summary, my thoughts are:

• @ataffanel we require an explicit control disabled mode, not simply a thrust=0. Refer to how control->enable is implemented in my branch
• A separate set emergency packet is probably the best approach when sequences are considered, however, perhaps there can also be a "set emergency" bit in the control packet header, if we have space. Triggering either would trigger an emergency.

[4rzael]

Hello,

I'm working on a project where I need a new controller, with trajectory planning.

Any updates on this work ?

Is the mikehammer's one the most advanced at the moment, or should I use crazyswarm at the moment ?

Can I help in any way ?

[binxxx]

Hello,

I have explored Mike's new controller for several days. But I haven't found a way to use crazyflie_ros with branch full_control to control the crazyflie. Could anyone give some examples, just like whoenig did in the crazyflie_demo to use this modified ROS package? That would be much helpful.

Thanks

[mikehamer]

Have a look in the joystick control app that I linked earlier (https://gist.github.com/mikehamer/1d3e7c3f03dfdbc3a5fc302804dcdba5)

Namely you'll need the following lines somewhere:

from crazyflie_driver.msg import FullControl
control_pub = rospy.Publisher("full_control", FullControl, queue_size=1, latch=True)

cmd = FullControl()
cmd.enable = True
cmd.xmode = 0b111 # position control
cmd.ymode = 0b111
cmd.zmode = 0b111
cmd.x = [0,0,0] # [x position, x velocity, x acceleration]
cmd.y = [0,0,0]
cmd.z = [0,0,0]
cmd.yaw = [0,0]

control_pub.publish(cmd)

[binxxx]

Hi, Mike. Thank you for your reply.

So the controller in crazyflie_ros is not used right now? But if you want to hold the position, there would be a position controller with the actual position as the feedback to calculate the roll, yaw, pitch. But I don't see any msg to be published as the actual position. And another part that confuses me is that the usage of "full_control_external_position" and "extenal_position". What are these two parts be used for? I didn't see it been used in the joystick control app.

[mikehamer]

Correct, the controller in crazyflie_ros is not used. Position control is performed onboard the quadrocopter, based on the state estimate provided by the onboard Kalman filter.

The joystick control app was written for a crazyflie flying using an LPS system, hence the CF is able to measure its own position and does not need an external position to be provided (hence why you can't find this code in the joystick control app).

I have, however, also run this controller under a vicon system. In this case the vicon position needs to be sent to the Crazyflie using the external_position topic. It will then be incorporated in the estimate of the onboard Kalman filter.

[suarez-ramon]

Hello,

I am just starting with the CF quadrotor and I had some questions. Is it possible with this implementation to maintain a given altitude and control linear velocities in x and y independently? Or do i have to send commands in thrust as well if im publishing on the cmd_vel topic?

[ataffanel]

Hi @suarez-ramon, Are you running the PID controller or Mike's controller? With Mike's controller you can send the Z position and X,Y velocity. Using the 'master branch' PID controller you would need to implement your own commander packet if you want to send Z position and X/Y velocity, or you can integrate the position on PC side and send X/Y/Z position setpoint using the existing packets.

[suarez-ramon]

Hello @ataffanel , I am interested in using Mike's controller. The issue is im new to this and not very sure of the procedure. Im interested in using ROS to communicate to the CF. There are three packages that Mike mentions, one of these is the Firmware and another is the ROS packages.

Do I have to flash the CF2 to be able to use this or is running the code for ROS enough? If I have to flash the CF2, how do I do this? in the readme it doesnt specify that well....

Thanks!

[suarez-ramon]

I was able to build the firmware and flash it using make cload... I can run the crazy_flie ROS demo files publishing to /full_control... the only problem is that it has some weird behaivior. Just launching and enabling control in accelerations first as it says in joystick_controller starts the motors and it teds to make a flip wanting to be upside down. moving the joystick does something but very little. If the commands for accelerations are at 0 in x,y and z, how come it sends thrust to the motors?

[ataffanel]

If you ask for 0 acceleration, the controller will start the motor and try to achieve 0 acceleration (the controller is designed to fly, the motor should be ON to fly), so this is not very strange. Is your position estimate good? What system are you using to calculate the position estimate?

[suarez-ramon]

@ataffanel I am using optitrack system. I have been able to launch it with the optitrack (after figuring out that I had to update Kalman filter before enabling). It does as expected and takes off, but the problem is it drifts a lot to one side and never reaches the desired position, the one by default 0,0,1m... Is this a problem of parameter tuning on the controller? What parameters are tunable in the controller if this is the problem?

I am sending the optitrack position to the "/external_position" topic and sending the commands to the "/full_control" topic. I am not really sure the difference between /full_control and /full_control_external_position... perhaps I am wrong there?

In order to build the firmware I had to do a make with the following options:

make CLOAD=1 DEBUG=1 PLATFORM=CF2 CONTROLLER=new POWER_DISTRIBUTION=cf2 ESTIMATOR=kalman cload

and cload for flashing the CF2... This was the only way to build without errors... Are these parameters correct?

Thanks!

[ataffanel]

Your build parameters look correct (you can add them to tool/makes/config.mk to make it easier). Have you checked that the position estimate is correct?

If you are setting the controller in position mode, the drift is not normal. If you are setting it in velocity or acceleration only control mode then I have observed some drift before.

You can look in the new controller implementation, the parameter that can be tuned are well documented.

[suarez-ramon]

I'll check the position estimate from the optitrack. Another thing I get errors with the set_param for igain_yaw, imax_yaw and igain_rate... It says it cant find them.

[ataffanel]

You should check the output of the Kalman filter to see if it is not diverging (log variables kalman.stateX/Y/Z). For the parameters you can see the list of exported parameters with rosparam or with the Crazyflie client.

[suarez-ramon]

how do you see these Log variables?

[suarez-ramon]

@ataffanel As you suggested I checked the output of the Kalman filter with the log variables measured.x, measured.y and measured.z which are the ones in estimator_kalman.c in the firmware. As was expected, these are not good at all... I am using ground truth with the Optitrack and the values are really bad. I dont know if its normal behavior but when I take off the values stop publishing to the rostopic. What can this error be to have such a bad estimate of the position if i am passing ground truth thru the external_position topic?

Thanks!

[ataffanel]

If the estimate becomes bad when you take off it could be that you are not starting with the front of the Crazyflie facing X? Anyway, the position estimate is then your problem and we are now very far from the topic of this ticket, I suggest you post on the forum, we can continue the discussion there: https://forum.bitcraze.io/viewforum.php?f=18&sid=0cb8fd627d16f7737b68fa60b617fd82

[krichardsson]

Closing this issue since we now have support for adding more controllers, as has been used for the Mellinger controller. Would still be nice to get Mikes controller in though, adding separate issue for this.