Position Hold with Optical Flow sensor - done

alexmos · Post by **alexmos** » Mon May 28, 2012 12:20 pm

tevada2010 wrote:Is this correct?
Set pins numbers in config.h
Code: Select all
#define OPTFLOW 5050
#define OF_SCLK A3
#define OF_SDIO A6
#define OF_NCS A7

As I know A6, A7 - may not be used as digital pins, so this config will not work.

alexmos · Post by **alexmos** » Mon May 28, 2012 12:24 pm

boke wrote:Hello!
I try to use the Adns 2051. But this one don't have the ncs-port, but a PD-Port, can i use the same way?
Greetings

All Avago sensors has different pinout, and different communication lines and even different protocol! So It is not possible to connect other sensor than ADNS-5050 and use the same code. You should read it's datasheet and re-write comunication code to get it working.

tevada2010 · Post by **tevada2010** » Mon May 28, 2012 3:59 pm

again...Is this correct?

#define OPTFLOW 5050
#define OF_SCLK D5
#define OF_SDIO D12
#define OF_NCS D13

LuFa · Post by **LuFa** » Thu May 31, 2012 6:01 pm

Hi Alexmos ,

now i have add OPTICAL_FLOW_P_GAIN and OPTICAL_FLOW_I_GAIN .

my parameter for the moment are : P 5.0 and I 0.010 , what is the right method to write this directly in the code ?
because OPTICAL_FLOW_I_GAIN 0.100 doesnt work

how to write it correct ?

krst · Post by **krst** » Fri Jun 01, 2012 7:34 am

Hi

For sale ADNS-5050 sensors

I am about to try this myself so just ordered the parts. But as the shipping was so high, almost 30$ it felt stupid to only buy one unit. So i bought a few and will get them soon. So if anyone is interested send me a PM here. Was thinking about 10$ including uninsured shipping and i can include the adns 5050 sensor and the four capacitors needed, 2 0.1 uf cheramic and 1 4.7 uf aluminium and 1 3.3uf aluminium polarised. Not trying to make any real money on this just to cover up my shipping costs which is like 10times the part cost otherwise! The creator and tread starter should have all the credits for this! Looking forward myself to this very interesting addon!

Krst

alexmos · Post by **alexmos** » Fri Jun 01, 2012 5:59 pm

krst wrote:Hi

For sale ADNS-5050 sensors

I am about to try this myself so just ordered the parts. But as the shipping was so high, almost 30$ it felt stupid to only buy one unit. So i bought a few and will get them soon. So if anyone is interested send me a PM here. Was thinking about 10$ including uninsured shipping and i can include the adns 5050 sensor and the four capacitors needed, 2 0.1 uf cheramic and 1 4.7 uf aluminium and 1 3.3uf aluminium polarised. Not trying to make any real money on this just to cover up my shipping costs which is like 10times the part cost otherwise! The creator and tread starter should have all the credits for this! Looking forward myself to this very interesting addon!

Krst

If you solder it on breadboard and add any appropriate lense - it will be more popular (Its an idea for business

). And I think Alex will faster add support for this sensor into official MWC trunc

krst · Post by **krst** » Fri Jun 01, 2012 10:45 pm

alexmos wrote:
krst wrote:Hi

For sale ADNS-5050 sensors

I am about to try this myself so just ordered the parts. But as the shipping was so high, almost 30$ it felt stupid to only buy one unit. So i bought a few and will get them soon. So if anyone is interested send me a PM here. Was thinking about 10$ including uninsured shipping and i can include the adns 5050 sensor and the four capacitors needed, 2 0.1 uf cheramic and 1 4.7 uf aluminium and 1 3.3uf aluminium polarised. Not trying to make any real money on this just to cover up my shipping costs which is like 10times the part cost otherwise! The creator and tread starter should have all the credits for this! Looking forward myself to this very interesting addon!

Krst

If you solder it on breadboard and add any appropriate lense - it will be more popular (Its an idea for business ). And I think Alex will faster add support for this sensor into official MWC trunc

Yes I was thinking to do this but not really sure how poppular this is. I am not a complete noob on microelectronics and have been done my own laser distance ranger before so know some about lensens to. But with the breadboard, work and lens i guess we would end up around 20-25 dollars I feel including shipping. But maybe there is a demand for that to?!

I guess i can just order som extra lenses to when i order for myself, the extra work is not really that large after I have figured out the first good construction. Planning to go for a lens with about 40-30deg to enable some good resolution but still be able to fly at low heigh. Which height do you think people want to fly on? It highly will influnce the type of lens, small FOV give higher heights but I assume at low heights it will see a very small field and then you really have to have some good patterna so the sensor actually sees something?!

alexmos · Post by **alexmos** » Mon Jun 04, 2012 10:49 am

About good FOV - I can only make theoretical conclusions like you. I have tested lense 9mm (FOV not good terminology because we don't know sensor dimension) and it works from zero ground to 5-10 meters. More narrow lense (longer f.distance) will receive less light and it will leads to worse sensivity. Also thay have shorter depth-of-field, so working range will be reduced. But it will be interseting to test and compare it in practice.

LuFa · Post by **LuFa** » Mon Jun 04, 2012 8:12 pm

LuFa wrote:Hi Alexmos ,

now i have add OPTICAL_FLOW_P_GAIN and OPTICAL_FLOW_I_GAIN .

my parameter for the moment are : P 5.0 and I 0.010 , what is the right method to write this directly in the code ?
because OPTICAL_FLOW_I_GAIN 0.100 doesnt work how to write it correct ?

no idea ?

Tommie · Post by **Tommie** » Mon Jun 04, 2012 8:20 pm

What's the status of integration in the shared SVN branch? I have one sensor lying around and can query it via I²C, but I'd like to see the tracking code integrated before I mount it on my copter.

alexmos · Post by **alexmos** » Tue Jun 05, 2012 6:51 am

LuFa wrote:
LuFa wrote:Hi Alexmos ,

now i have add OPTICAL_FLOW_P_GAIN and OPTICAL_FLOW_I_GAIN .

my parameter for the moment are : P 5.0 and I 0.010 , what is the right method to write this directly in the code ?
because OPTICAL_FLOW_I_GAIN 0.100 doesnt work how to write it correct ?

no idea ?

Attach you code here (all files in one archive) I will check it.

LuFa · Post by **LuFa** » Tue Jun 05, 2012 8:24 am

thank you very very mutch !!

LuFa · Post by **LuFa** » Wed Jun 06, 2012 6:38 pm

no error found ?

i think this code works and its only a proplem of the method how to write the PID Values correct directly in the code .
With your code (i think r23) the Values P 5.0 and I 0.010 works very great , but i dont know how to write this correct with #define .

Thanks

alexmos · Post by **alexmos** » Wed Jun 06, 2012 8:55 pm

I have found errors in P,I definition and usage. I have corrected 2 files: config.h and OpticalFlow.ino and uploaded in archive (other files unchanged).

P, I values are integers and relation between GUI is 0.010-> 10, 5.0 -> 50

LuFa · Post by **LuFa** » Thu Jun 07, 2012 5:14 am

thank you very mutch

the rest was ok ?

LuFa · Post by **LuFa** » Sun Jun 10, 2012 7:46 pm

Hi ,

today i have test your corrected code , but Optical flow doesnt work

Did you have found another error in my code ?

maybe its a proplem of my hardware , to exclude that i will make a second test with your code tomorrow

LuFa · Post by **LuFa** » Thu Jun 14, 2012 6:50 pm

Hi ,

today i have upload the r35 from alexmos to test the altohold
I use the MS5611 baro sensor .
It works great but i have one proplem :
The Mwii hold the alt good for the first time , after 30sek or more the Mwii beginns to move slow down and land ^^ stand for some seconds on the ground and move up to the target alt .

have anyone a idea why ?
My PID´s are : P 10 I 10 D 20

Thanks

LuFa · Post by **LuFa** » Thu Jun 14, 2012 7:52 pm

Now i have change all PID Parameter but the Proplem is not realy remove

I have found that the proplem is smaller with a Small I term and a high D term .
My Parameters for the moment are P 10 I 0.003 D 23 , thats the best result for the moment , but after some times the proplem comes back

Katch · Post by **Katch** » Thu Jun 14, 2012 8:19 pm

krst wrote:Hi

For sale ADNS-5050 sensors

I am about to try this myself so just ordered the parts. But as the shipping was so high, almost 30$ it felt stupid to only buy one unit. So i bought a few and will get them soon. So if anyone is interested send me a PM here. Was thinking about 10$ including uninsured shipping and i can include the adns 5050 sensor and the four capacitors needed, 2 0.1 uf cheramic and 1 4.7 uf aluminium and 1 3.3uf aluminium polarised. Not trying to make any real money on this just to cover up my shipping costs which is like 10times the part cost otherwise! The creator and tread starter should have all the credits for this! Looking forward myself to this very interesting addon!

Krst

PM'd

alexmos · Post by **alexmos** » Fri Jun 15, 2012 7:49 am

LuFa wrote:Now i have change all PID Parameter but the Proplem is not realy remove
I have found that the proplem is smaller with a Small I term and a high D term .
My Parameters for the moment are P 10 I 0.003 D 23 , thats the best result for the moment , but after some times the proplem comes back

Soon I will test MS5611 baro and write results. I have got the same problems in my earlier tests, try to tune pids and calibrate ACC by six points. Keep in mind that baro work much worse near ground (below 1m) because props are making high pressure and baro goes into oscillations.

LuFa · Post by **LuFa** » Fri Jun 15, 2012 5:24 pm

some tests later

i have found out that the proplem is only in low alt. around 1m .
Now i connect my Sonar Sensor and make some new tests

what did you mean with " calibrate your ACC by six points " ? how does it work ?

Thanks

alexmos · Post by **alexmos** » Fri Jun 15, 2012 10:41 pm

In my firmware I have improved ACC calibration: hold your quad in 6 positions (starting from normal) and press ACC_CALIB (or run acc calibration from sticks) six times in each position. It is important to start from the first (normal position), all other may be ordered randomly.

Dehas · Post by **Dehas** » Sat Jun 16, 2012 12:25 pm

I wonder if the module ADSN 5030 work with this code.
Another thing, I downloaded the code here
http://code.google.com/p/multiwii-alexmos/downloads/detail?name=MultiWii2_0_alexmos_r23.zip&can=2&q=#makechanges
And I have I2C errors, in debug2, with just the sonar HC-SR04.
If you have an idea!

Katch · Post by **Katch** » Sat Jun 16, 2012 3:50 pm

alexmos wrote:In my firmware I have improved ACC calibration: hold your quad in 6 positions (starting from normal) and press ACC_CALIB (or run acc calibration from sticks) six times in each position. It is important to start from the first (normal position), all other may be ordered randomly.

Am I right in thinking that it is important to make sure the quad is completely vertical during 3 4 5 and 6?

alexmos · Post by **alexmos** » Sat Jun 16, 2012 5:12 pm

Am I right in thinking that it is important to make sure the quad is completely vertical during 3 4 5 and 6?

Yes, ROLL (3,5) and PITCH (in 4,6) axis should be vertical to get maximum values for each axis.

alexmos · Post by **alexmos** » Sat Jun 16, 2012 5:15 pm

Dehas wrote:I wonder if the module ADSN 5030 work with this code.
Another thing, I downloaded the code here
http://code.google.com/p/multiwii-alexmos/downloads/detail?name=MultiWii2_0_alexmos_r23.zip&can=2&q=#makechanges
And I have I2C errors, in debug2, with just the sonar HC-SR04.
If you have an idea!

Have you configured it for your board? May be address of accelerometer or gyro is incorrect in config.h or def.h

Dehas · Post by **Dehas** » Sat Jun 16, 2012 5:33 pm

Yes everything is onfigured for my board, I use an MPU 6050 and it works well.

EDIT: I forgot, there is also a small typo in sonar.pde: "SonarErros" should be "SonarErrors" line 159.

charbot · Post by **charbot** » Sun Jun 17, 2012 6:01 pm

Is optical currently available in the latest dev version? I cant find anything about it in config.h or other tabs but is listed in the GUI. (tho' the box is black not brown like the other available sensors- I assume not a good sign)

alexmos · Post by **alexmos** » Mon Jun 18, 2012 8:01 am

Dehas wrote:Yes everything is onfigured for my board, I use an MPU 6050 and it works well.
EDIT: I forgot, there is also a small typo in sonar.pde: "SonarErros" should be "SonarErrors" line 159.

I dont understand you problem, explain more cleary. In GUI screenshot there is no I2C errors. debug1-debug4 are used to debug defferent sensors in my code. You can find by keyword '_DEBUG' in config.h wich sensor to output there (only one at a time), and debug1-debug4 in the code to find out what data is going to debug.

Thanks for typo, corrected.

alexmos · Post by **alexmos** » Mon Jun 18, 2012 8:02 am

charbot wrote:Is optical currently available in the latest dev version? I cant find anything about it in config.h or other tabs but is listed in the GUI. (tho' the box is black not brown like the other available sensors- I assume not a good sign)

Currently no support. May be box added for future releases?

Dehas · Post by **Dehas** » Mon Jun 18, 2012 11:25 am

I tried all the '_DEBUG' one by one, if I comment "# define SONAR_DEBUG", no problem but the sonar does not work, if I uncomment in debug2 I have 708, and the sonar does not work .

LuFa · Post by **LuFa** » Tue Jun 19, 2012 8:51 pm

Hi Alexmos ,

Now i have flight 20 Batterys and have make a lot of tests with Different PID Settings and with/without Sonarsensor .

My result is :

- Without Sonarsensor it is possible to minimize the Proplem but not to remove it comlete
- Thatswhy i have Connect my HC-SR04 Sonarsensor
- With Sonar its possible to remove the Proplem by reduce the I term and increase the D term , but with this the alt Hold works not so great in Higher altitude .

But i have found where the Proplem cames from :

- The MS5611 have some big jumbs (~50-100cm) in a Altitude < 30cm
- So thatswhy the Mwii reduce the Throttle and make a autoland

and stand for a few seconds ,... the Throttle goes smaller and smaller and the noise to , thatswhy the Mwii does after few seconds begin to increase the Throttle to flight to the Target altitude .

Is it Possible to use only the Sonarsensor for alt hold in a altitude < 1m ? i think in this way we can remove it .

Thanks

alexmos · Post by **alexmos** » Tue Jun 19, 2012 9:45 pm

I have wrote above that baro will not work below 1m! I simply does not turn alt hold on this altitudes without sonar. If sonar is present, it holds well on any low altitude, from 10cm to 2m.

LuFa · Post by **LuFa** » Wed Jun 20, 2012 8:48 pm

ok

did you plan to make a update of your code to the newest dev ?
because the 2.0 have the old GPS Code and it will be very nice to use the New GPS from Eosbandi with your OpticalFlow together

thanks a lot !

didlawowo69 · Post by **didlawowo69** » Wed Jun 27, 2012 1:13 pm

yep i want to test my new sensors with your code on the last dev release, so please make a new version

LuFa · Post by **LuFa** » Sat Jun 30, 2012 9:10 pm

any news about the MS5611 and your Code ?

After some morge Flights , my best PID Values are : P:20 I:0.005 D:15 , with this setup the Baro works very great but the Sonar have some proplems .
Im still in progress to find the best PID Values

Did you plan any updates or somethink else of your Altitude Code ?

justk1w1 · Post by **justk1w1** » Thu Jul 05, 2012 7:32 am

alexmos,

Is there any intention to either upgrade to the latest release of MW - or to merge the great work that you have done back into the MW. I ask this selfishly as I was looking to setup a heli with MW and it's in the current code, but unfortunately does not have the code for either the sonar or the opticalflow, where as yours has the sensors, but unfortunately not the heli code. Selfishly wanting the best of both worlds:)

alexmos · Post by **alexmos** » Thu Jul 05, 2012 3:47 pm

Sorry for long absence, I am a bit bussy now. I will merge to latest 2.0 code in the nearly future but can't say exact time.

LuFa · Post by **LuFa** » Thu Jul 05, 2012 5:12 pm

great !

cencen · Post by **cencen** » Thu Jul 12, 2012 2:34 pm

krst wrote:Hi

For sale ADNS-5050 sensors

I am about to try this myself so just ordered the parts. But as the shipping was so high, almost 30$ it felt stupid to only buy one unit. So i bought a few and will get them soon. So if anyone is interested send me a PM here. Was thinking about 10$ including uninsured shipping and i can include the adns 5050 sensor and the four capacitors needed, 2 0.1 uf cheramic and 1 4.7 uf aluminium and 1 3.3uf aluminium polarised. Not trying to make any real money on this just to cover up my shipping costs which is like 10times the part cost otherwise! The creator and tread starter should have all the credits for this! Looking forward myself to this very interesting addon!

Krst

PM'd

cencen · Post by **cencen** » Mon Aug 06, 2012 10:15 am

Excellent work Alexmos ! Your SONAR code works like a dream, many thanks for your contributions to Multiwii.

I've been trying for a while to get sonar working on my quad and at last I've managed it with your Multiwi R23 version. It holds quad incredibely steady up to almost 3 meters. I've tested with 2 sonar sensors (HC-SR04 and DYP-ME007) and both gave almost same good results.

To make life easier for the new testers I made a sort of 'to do' list:

- Sonar uses ALT PID values (VEL PID values are not used)
- I tested with Drotek 10DOFIMU with MS5611 Baro and I used the PID's 5 , 010, 020
- You need to follow the improved 6 position ACC Calibration
- You need to place the sonar at a place away from the motors. I've placed it under the center plate and works just fine.
- I disabled the THROTTLE_EXPO in config.h for better response of throttle
- I changed the value of #define SONAR_BARO_PID_GAIN from 1 to 3 for a better gain of sonar
- I disabled the #define THROTTLE_ANGLE_CORRECTION by setting it to 0
- I changed #define THROTTLE_WIND_CORRECTION from 250 to 200
- Rest of ALT HOLD command and values in config.h are not changed
- Caution: This version of Multiwii comes with already enabled serial sum receiver defined in config.h. If your receiver is not that type you should command the #define SERIAL_SUM_PPM line
- I just start flying in MAG mode and at a certain height I activate the ALT HOLD by just activating BARO (you should see the BARO green in GUI)
- DEBUG 1 shows the sonar altitude in cm/10 (not in cm as I was expecting to see, a bit confusing)

My next test step is the position hold with optical flow sensor ADNS-5050. I've already received the sensor from KRST and will make the breakout board soon to test. I'll again post here later on for the results.

Regards,

Feridun

LuFa · Post by **LuFa** » Sun Aug 12, 2012 4:50 pm

cencen , wich Acc did you use ?
did you have also make a test in a altitude >= 10m ?

cencen · Post by **cencen** » Mon Aug 13, 2012 3:19 pm

LuFa wrote:cencen , wich Acc did you use ?
did you have also make a test in a altitude >= 10m ?

ACC I've used is BMA180. I've only tested the altitude hold with SONAR up to 3m. I've not yet completed my tests with optical flow sensor. Lenses I tested were not suitable for the job therefore I ordered some new lenses with different focus lengths and looking forward to receive them. Then hopefully will continue my tests with position hold up to 10m !! I'll post here again when I have results.

Regards,

Feridun

LuFa · Post by **LuFa** » Thu Sep 13, 2012 2:13 pm

Hi ,

have anyone try to move the "SONAR_PING" to pin A3 ?
will that work ?

bg

cencen · Post by **cencen** » Thu Oct 11, 2012 8:03 pm

Hi ,

After a long while, at last I've found time to test my quad with optical sensor (ADNS-5050) and sonar (HC-SR04) and YES IT WORKS !!

Working with both sensors activated was not really easy as of with only sonar. Adjusting the parameters for the optical flow sensor is quite tricky I must say.
I had ordered a lens set of 6 with 2.8mm to 16mm focus lenghts. Took me hours to figure out the best values for the lenses with different focus lenghts. 6 and 8mm lenses gave me the best results.

* It works very well up to about 3m (max. sonar distance range) with high accuracy
* It works reasonably well up to about 6-7m with lower accuracy
* I couldn't test good results higher than 6-7m YET. I need to find a suitable place to test.

I've made the attached video with the following optical flow parameters:

#define OPTFLOW 5050
#define OF_SCLK A0
#define OF_SDIO A1
#define OF_NCS 8

/* Lense focal distance, mm (set it for your own lense)
(How to check: debug4 in GUI should not react on ROLL tilt, but react on ROLL slide) */
#define OF_FOCAL_DIST 8
/* Deadband for ROLL,PITCH sticks where position hold is enabled. Max value 100 */
#define OF_DEADBAND 10
/* Rotate I-term with heading rotation. It will well compensate wind */
#define OF_ROTATE_I
/* Low-pass filter factor to prevent shaking. Possible values 1..8. Default is 5. */
#define OF_LPF_FACTOR 2
/* Debug to GUI */
#define OF_DEBUG

My GUI values for VEL is P:3 I:10

Here is a test video https://vimeo.com/51222543

Alexmos, thanks again for your wonderful implemantation and code.

I think this is an interesting and really working function/addon for multiwii and your really working code should be added to multiwii V2.2

Are you considering to release another version for Multiwii V2.1 or implement to Mega? That will really be interesting!

Regards,

Feridun

Magnetron · Post by **Magnetron** » Thu Oct 11, 2012 9:45 pm

I have dyp-me007 and a mega2560, could someone tell me what pin I must use and what kind of software modification I must implement to test sonar on mega2560. Alexmos, could you suggest me?

Eric · Post by **Eric** » Sat Nov 17, 2012 2:54 am

It works great with my adns 5020.
But I have a question, for example, when the quad goes forwards, and then I restore the sticks, but the quad drift backward a little. It looks like the sensor record the movement (actually this is stick input) as errors and try to correct it.

So I checked the code, shall we discard the delta x/y when the sticks are not in dead band?

Eric · Post by **Eric** » Thu Jan 10, 2013 4:03 pm

Is it possible to enlarge the hole in the sensor to increase its performance in dark environment?

As it is designed to work with the assistant of an led and we could not change the shuttle speed of adns 5050 or 5020. We can enlarge the hole so that more light goes into the sensor.

cencen · Post by **cencen** » Mon Jan 14, 2013 6:39 pm

Hi Eric,

Enlarging the hole on the chip sounds quite logical to me, thought it is risky! You may try to do that if you have any spare one(s). Please post your results if you do so..

lucky13 · Post by **lucky13** » Thu Apr 03, 2014 12:04 pm

has anybody imported the optical flow code to version 2.3?