OpenCV hover-assist

Firmware/software/electronics/mechanics
arnaud
Bitcraze
Posts: 2538
Joined: Tue Feb 06, 2007 12:36 pm

Re: OpenCV hover-assist

Post by arnaud »

Hi,
I just implemented the function to send position update to the Crazyflie: https://github.com/bitcraze/crazyflie-l ... /issues/30. This function allows to update the internal kalman filter with the externally measured position.

The exact procedure depends of what you try to achieve. I have an idea to implement and test altitude hold:
- Measure the Z position of the Crazyflie using your image processing.
- Implement your image processing in a Crazyflie client tab using the example tab as a starting point: https://github.com/bitcraze/crazyflie-c ... mpleTab.py. We made a video a while ago showing the process: https://youtu.be/cutgIMfHwyQ
- Once you have Z in the client, call cf.extpos.send_extpos(0, 0, z) at the end of your image processing loop.
- Compile the crazyflie firmware with both kalman and XY decoupling: https://github.com/bitcraze/crazyflie-f ... 4b58a5d549. That can be done by adding to tools/make/config.mk:

Code: Select all

ESTIMATOR=kalman
CFLAGS += -DKALMAN_DECOUPLE_XY
then "make clean && make && make cload"
- Fly an press the alt-hold button to enjoy your super stable alt-hold!

The step after would be to measure X Y and Z (for example with 2 cameras) and disable the XY decoupling to be able to make fully autonomous flight :-).

Keep us updated and tell me if you need help. This would be very interesting to see :)
dbrgn
Member
Posts: 51
Joined: Tue Dec 16, 2014 9:42 pm

Re: OpenCV hover-assist

Post by dbrgn »

Could that function also be used for ultrasonic height stabilization from the firmware itself?

After I built my ultrasonic extension board and being able to query the board from the firmware, I never got around to implement the actual height stabilization because I didn't know how to get started...
arnaud
Bitcraze
Posts: 2538
Joined: Tue Feb 06, 2007 12:36 pm

Re: OpenCV hover-assist

Post by arnaud »

It has been done for the VL54 laser height sensor (https://github.com/bitcraze/crazyflie-firmware/pull/174).

This code compensates for the copter orientation but experience shows that you will not need to compensate for it with an ultrasound sensor. So yes, you could directly call the firmware function that is cabled to the external position: https://github.com/bitcraze/crazyflie-f ... .c#L78-L84. I think the same idea will work fine: compile your firmware with kalman filter and with XY decoupled and call stateEstimatorEnqueuePosition with (x,y,z) = (0,0,height). Depending of the precision of your sensor you can play with the stdDev (in this code it is set to 1cm).

Eventually it would be cleaner to just push height, the same way the VL53 sensor is implemented. Maybe we can have the tilt compensation as a parameter which will allow to use the same function for all height sensors.
pedro.lucas
Beginner
Posts: 7
Joined: Fri Mar 03, 2017 4:57 pm

Re: OpenCV hover-assist

Post by pedro.lucas »

Hi Arnaud,

I’m using a motion capture system to track the positions of a crazyflie and using the function cf.extpos.send_extpos(x, y, z) as you described before. I compiled the crazyflie firmware with Kalman filter and I used cf.commander.send_setpoint(y_target, x_target,0, int(z_target * 1000)) for going to an specific point (x_target, y_target, z_target) in space as the autonomousSequence.py suggests, but replacing the LPS by the MOCAP system.

The setup works perfectly, but only if the front of the crazyflie is pointing to the x_axis_world (mocap coordinate system) direction. If the CF starts in other rotation around z_axis, the CF loses control. I developed a solution around this issue, so I moved the coordinate system such way the CF always thinks that is pointing to x_axis by using the rotation that is detected; it works sometimes but other times the CF loses the balance and turns over. So my question is: Is there a way to manage correctly the Kalman filter by considering the rotation, such way the firmware calculates what is needed to move to the specific point?, maybe sending the quaternion to the Kalman filter (if that makes sense)?. It would be very helpful if you have a detailed solution since I’m a beginner and I do not know the specific functioning for some of this tools and concepts, my goal is that the CF goes from one point to another as perfect as possible. Thanks very much :)
arnaud
Bitcraze
Posts: 2538
Joined: Tue Feb 06, 2007 12:36 pm

Re: OpenCV hover-assist

Post by arnaud »

Hi,

With the LPS and simple webcam tracking Yaw is usually not measured by the location system and so the Crazyflie assumes it is started (ie. switched ON and sensor calibrated) with the front facing X. As long at you start it with the front facing X you can put it the way you want for take-off and it should work. When flying the kalman filter will estimate the real yaw orientation from the dynamic but it trust a lot the initial yaw estimate (hence it should be started facing X).

If your mocap system can measure the yaw, you should push it in the Crazyflie Kalman filter, this way you can start the way you want. If you are interested by doing that I would suggest you post a ticket in the crazyflie-firmware (https://github.com/bitcraze/crazyflie-firmware/issues) project about adding a packet for externally measured position and orientation, there we can talk a bit more precisely about how to implement it.
pedro.lucas
Beginner
Posts: 7
Joined: Fri Mar 03, 2017 4:57 pm

Re: OpenCV hover-assist

Post by pedro.lucas »

Hi Arnaud,

Thanks by the reply, I just post a ticket as you suggested (https://github.com/bitcraze/crazyflie-f ... issues/203). I hope this issue can be solved and have a very stable cf. Thanks again.
arnaud
Bitcraze
Posts: 2538
Joined: Tue Feb 06, 2007 12:36 pm

Re: OpenCV hover-assist

Post by arnaud »

Hi Pedro,

Marcus, that is much more fluent with the EKF, will answer you on github. Aparently sending orientation update is much than what I though.

I would like to understand more your problem though: if it is just a startup issue we could just "reset" the EKF with the current orientation before take-off. This would be much easier to implement and as far as I understand it would solve your problems?

/Arnaud
pedro.lucas
Beginner
Posts: 7
Joined: Fri Mar 03, 2017 4:57 pm

Re: OpenCV hover-assist

Post by pedro.lucas »

Hi Arnaud,
In the code that I’m using based on the autonomousSequence,py, there is a parameter called Kalman.resetEstimation which allows to reset the used data structures when it take-off as I can understand from the firmware (In fact I’ve been checking the firmware). As I described before I moved the coordinate system to get always the front of the CF facing to the X axis from mocap, however, I thought that there would be no problem if always (even while flying) this solution were applied, but it doesn't work at all. What I can realize is: if the CF change the front direction while is flying, the CF fails and turns over. I supposed that, considering that EKF works if always the front is facing X, when the yaw change because of a movement, it cannot restore the init yaw(which faces X) in which it starts, it is true ?, If that is the case then I think the controller should keep the init yaw (the setpoint yaw would be zero), and when the setpoint yaw is different from zero, the CF should rotate in yaw and update what is needed to be adjusted to the new orientation (all of this makes sense ?), but maybe it is something not easy. I will expose this explanation on github and I will mention Marcus, any thought about this issue would be great.
twh
Beginner
Posts: 21
Joined: Tue Dec 06, 2016 7:34 am

Re: OpenCV hover-assist

Post by twh »

arnaud wrote:Hi,
I just implemented the function to send position update to the Crazyflie: https://github.com/bitcraze/crazyflie-l ... /issues/30. This function allows to update the internal kalman filter with the externally measured position.

The exact procedure depends of what you try to achieve. I have an idea to implement and test altitude hold:
- Measure the Z position of the Crazyflie using your image processing.
- Implement your image processing in a Crazyflie client tab using the example tab as a starting point: https://github.com/bitcraze/crazyflie-c ... mpleTab.py. We made a video a while ago showing the process: https://youtu.be/cutgIMfHwyQ
- Once you have Z in the client, call cf.extpos.send_extpos(0, 0, z) at the end of your image processing loop.
- Compile the crazyflie firmware with both kalman and XY decoupling: https://github.com/bitcraze/crazyflie-f ... 4b58a5d549. That can be done by adding to tools/make/config.mk:

Code: Select all

ESTIMATOR=kalman
CFLAGS += -DKALMAN_DECOUPLE_XY
then "make clean && make && make cload"
- Fly an press the alt-hold button to enjoy your super stable alt-hold!

The step after would be to measure X Y and Z (for example with 2 cameras) and disable the XY decoupling to be able to make fully autonomous flight :-).

Keep us updated and tell me if you need help. This would be very interesting to see :)

Hi Arnaud,
I have a rough idea about using webcam as an assistant for Crazyflie but I am not sure how to implement it.

I m going to feed the absolute position to the crazyflie by using 2 webcams e.g.x = 500, y = 300, z=400 (the webcams resolution are 1000x560).
From the firmware side, I am going to enqueue this position data for kalman filter. (500, 300, 400).
and now,
If I want the Crazyflie to be able to fly vertically without drifting, which mode should I set to for x and y (velocity or absolute?)
What should be the input values for x and y? is it still 500 and 300 for the Crazyflie to be stable?

and after that, If I want the Crazyflie to be able to hover after taking off, which mode should I set to for x, y and z? (velocity or absolute?)
What should be the input values for x, y, and z?

I am abit confused about this because there are two PID controllers, which are position control and velocity control. before the 2 attitude PID controllers.
Based on my understanding of the code, If we want to do altitude hold for it, it will bypass the position controller and do velocity controller directly. However, If we want to do position hold for it, it will do both position and velocity controller. I was thinking of one case, which if I am going to only control the x and y position by using the feeds from the webcams and control the z by using its own barometer measurements. How should I do for this case?


Thank you. :)
arnaud
Bitcraze
Posts: 2538
Joined: Tue Feb 06, 2007 12:36 pm

Re: OpenCV hover-assist

Post by arnaud »

@pedro.lucas: Your messages kind of hijacked this thread so I have created a new thread to continue the discussion: viewtopic.php?f=6&t=2342.

@twh
I am currently looking at autonomous flight with a raspberry-pi camera V2 and opencv. I have that as a side-project and hopefully I can get something working soon :).

First of all you need to push meters in the kalman filter. Choose an origin and calculate the position of the Crazyflie in regards to this origin, in meters. Once that is done you will be able to push X and Y in the kalman filter, one way is to use the existing external position port. If you only get X and Y (and not Z) you can modify the part of the firmware that gets position update to only push X and Y in the EKF: https://github.com/bitcraze/crazyflie-f ... #L894-L901.

For Z with the barometer, it is not enabled yet in the EKF. So you will have to enable and test the pressure sensor.

Actually it would be easier if you could have X,Y and Z from your webcams, with 2 camera it should be possible, this way you do not have to rely on the pressure sensor for height.

Once you have the state estimator (the kalman filter) fixed, the controller will 'just work' no need to care about it. You can fly in "position hold" using a recent firmware and a recent client, we have added position hold to the client and it is activated with the same button as alt-hold.
Post Reply