Position Hold with Optical Flow sensor - done

LuFa · Post by **LuFa** » Mon Apr 23, 2012 7:25 pm

alexmos wrote:
LuFa wrote:Hi Alexmos ,

i have see that you know move your code to 2.0

I have a idea :

What do you think about a Opticalflow and GPS Positionhold fusion ? somethink like :
Code: Select all
PositionHold_Angle = (GPS_Anlge + OpticalFlow_Angle) / 2 
what do you think about this idea ?

Thanks !
There is no fusion in your example: these angles are summed right now, and you can set weight of 1/2 (or any you want) for each sensor by PIDs.

And some news:
1. I have merged all my code with 2.0 and tested in flight. All seems to be working.
2. I have implemented ADNS-3080. See config.h for details. Latest code in /branches/v20/MultiWii: http://code.google.com/p/multiwii-alexm ... 2FMultiWii

I have not tested it because have not ADNS-3080 sensor, so try debug it yourself

ok cool

can you tell me were i have to paste this code "idea" ?
i like to test optical flow and gps at the same time

i think i have found a bug in your code , normaly if the copter hover at 1meter high the gui display "1.0"
but with your code the gui show "10.0" , is that right or a bug =?
thanks so mutch !!!!!!!

alexmos · Post by **alexmos** » Mon Apr 23, 2012 8:29 pm

In my code this is already done: GPS angle and optical flow angle are summed:

Code: Select all

      errorAngle = constrain(2*rcCommand[axis] - GPS_angle[axis],-500,+500) - angle[axis] + accTrim[axis]; //16 bits is ok here
      #ifdef OPTFLOW
         errorAngle-= optflow_angle[axis];
      #endif

so no need to change anithing to get it working together with GPS. You should only configure PIDS to change proportional multiplyers for each angle.

In my code altitude is multiplied 10 times before output to GUI, so 1m = 10m in GUI. All ok there.

Tifani · Post by **Tifani** » Mon Apr 23, 2012 8:30 pm

Thanks a lot !
I did not connect NCS
Tom

LuFa · Post by **LuFa** » Tue Apr 24, 2012 12:29 pm

Hi ,

today i have test your code .
Optical Flow works perfect !

but i have some proplems with the Barosensor , its not possible to find the right PID Values .
With Mwii 2.0 standart PID i havent this proplem

Can you tell me how i can use the same Barocode like multiwii 2.0 =?

Thanks

alexmos · Post by **alexmos** » Tue Apr 24, 2012 1:02 pm

Wich PID's you are using for Optical Flow? I have found that low values ( P<=5, I<=0.020) gives best results.

For my altitude hold code, P=10, I=0.010, D=20..30 is good working. But ACC fusion is used so precize calibration required.

I will add baro code from oficial MWC 2.0 as will have enough time..

LuFa · Post by **LuFa** » Tue Apr 24, 2012 6:04 pm

I have add the SRF08 code into yours

maybe this is the proplem of the altitute hold ?

i have change the sonar code to this :

Code: Select all


#ifdef SONAR_SRF08

inline void sonarUpdate() {

SonarAlt = SRF08_Alt; 

}

#endif

but i think this is not the proplem .

tomorrow i will test your Sonar/Baro PIDS

what is your SONAR_BARO_PID_GAIN ?

For Optical Flow i use P:6.3 and I: 0.010
Optical flow works very great with this value on my Quad

If the Alt Hold are activate , i think i have feel somethink like a Vario Alt Control if i move the throttle stick , is that right ?

Next question is , is EstAlt also Multiplied by 10 ?

thanks !

alexmos · Post by **alexmos** » Wed Apr 25, 2012 11:03 am

LuFa,
your sonar integration is incorrect

As you can se, in my code sonar distance calculated quite simply: uint16_t dist = sonarData/58;
All other more-than-50 lines of code - is error detection to achieve good Sonar/Baro fusion and protection from sonar inaccurate working. Try to search how often SonarErrors used in my code and you can understand that it is very important.

My SONAR_BARO_PID_GAIN = 1 - it twice output from PID controller for good sonar data.

"Vario Alt Control if i move the throttle stick" - yes, you are right. It will beep and blink leds if you have it connected while alt is changing.

EstAlt is as it was, in cm. 100 = 1m

Please publish you Optical Flow experience here (some pictures and words) for other people. What lense have you used?

LuFa · Post by **LuFa** » Wed Apr 25, 2012 8:37 pm

Hey ,

no proplem , tomorrow i will make some Pictures of my OpticalFlow Sensor

i use a 8mm lens from a old CCD Camera and it works very great !

Can you tell me how i can implement the SRF08 correct ?
I use the Sonar Code from the newest Dev version .

Does your OpticalFlow only work with GPS Position Hold or also with GPS Comming Home ?
Because at Comming home , i think the OpticalFlow will compensate the ComingHome Angle because of the movement at the way to Home .

Next week i will start to move my Hardware into a Haxacopter .
Does the OpticalFlow Code use any interrupts or so on , wich are need for Hexacopter ?
I know that any times ago your Sonarcode have use a interrupt wich is also need for Hexacopter , thatswhy i use the SRF08 with i2c bus

i hope its not the same proplem with OpticalFlow code

Thanks so mutch for your great Project and Help !

alexmos · Post by **alexmos** » Thu Apr 26, 2012 8:28 am

LuFa,

I can't give advise about SRF08.. You may get AltHold code from dev 2.0 and completely replace my code. To integrete into my code, you should set 2 variables for sonar reading: SonarAlt (in cm) and SonarErrors (0..SONAR_MAX_ERROR). If SonarErrors (or somewhat simular) is not defined in dev release - it is a big mistake of developers

OpticalFlow is disabled in GPSHome mode:

Code: Select all

   // enable   OPTFLOW   only in   LEVEL   mode and if   GPS   is not used
   if(accMode ==   1   && optflowMode ==   1   && GPSModeHome ==   0) {

OpticalFlow does not use any timers and interrupts so may work with hexacopters, too.

didlawowo69 · Post by **didlawowo69** » Thu Apr 26, 2012 10:49 am

Very interesting project
but where i can buy an low cost sensors for this implementation, cause 40$ on diydrone, is pretty expensive i think.

and now your code is integrated in MW 2.0 or not ?

LuFa · Post by **LuFa** » Thu Apr 26, 2012 6:03 pm

yes , the newest code is based on Mw 2.0 .

You can buy the adns5050 for example here : http://www.digikey.de/product-search/de ... dns%205050

@Alexmos

Wich Altitute Variable is use for calculate the Opticalflow Angle ?
because , i try to start to move your alt code to the newest dev code .
can you tell me which parts i should change ?

Thanks !

p25o1 · Post by **p25o1** » Fri Apr 27, 2012 11:32 am

hi alexmos,

i managed to get the ADNS-3080 optical sensor from diydrones.

can you advice on the connection diagram plz.

looking forward to get it up and running and test with you guys

thanks

alexmos · Post by **alexmos** » Fri Apr 27, 2012 12:09 pm

LuFa wrote:yes , the newest code is based on Mw 2.0 .

You can buy the adns5050 for example here : http://www.digikey.de/product-search/de ... dns%205050

@Alexmos

Wich Altitute Variable is use for calculate the Opticalflow Angle ?
because , i try to start to move your alt code to the newest dev code .
can you tell me which parts i should change ?

Thanks !

Hi! You need an altutude from ground. In my code, It is estimated altitude from Sonar+Baro+ACC. But you can use only Baro Alt, but first you must fix altitude on the ground and remove it further. I store GroundAlt on every motor arms.

alexmos · Post by **alexmos** » Fri Apr 27, 2012 12:11 pm

p25o1 wrote:hi alexmos,

i managed to get the ADNS-3080 optical sensor from diydrones.

can you advice on the connection diagram plz.

looking forward to get it up and running and test with you guys

thanks

Hi! I have added code for ADNS-3080 to latest revision but it is still not working. If you have skills to debug it yourself, you can try. Connect all pins described in config.h for it (except RESET) to any unused Arduino legs.

p25o1 · Post by **p25o1** » Fri Apr 27, 2012 1:37 pm

i'm using a quadrino rev 1e board
http://flyingeinstein.com/index.php/tutorials/knowit

can i do the following

#define OPTFLOW 3080
define OF_SCLK A0
define OF_MISO A1
define OF_MOSI D8
define OF_NCS A6

A0, A1, D8 are available on the ezconnect cable
A6 is available on the board.

alexmos · Post by **alexmos** » Fri Apr 27, 2012 3:02 pm

A0, A1 - used by gimbal servos, disable it function first in config.h or def.h
D8 - used by buzzer, disable it too in def.h
A6 - don't know what is it. check if it supports digital write/read.

LuFa · Post by **LuFa** » Sun Apr 29, 2012 3:25 pm

Hi Alexmos

Today i have change your alt hold code to this from Mwii2.0 .
Now i have two Outputs : BaroAlt and BaroAltCal , BaroAltCal is the Altitude over Ground .
Can you tell me where i have to put BaroAltCal into your OpticalFlow code for right Movement Calculation ?

Thanks

alexmos · Post by **alexmos** » Sun Apr 29, 2012 4:05 pm

You should find EstAlt variable in OpticalFlow.pde and replace it to yours BaroAltCal

LuFa · Post by **LuFa** » Sun Apr 29, 2012 4:40 pm

Code: Select all

alt   =   constrain((int16_t)EstAlt, 30, 300)   *   min(avgSqual.res,50) * 2;   // 16   bit   ok:   300   *   50 * 2 = 30000;

this one ? is this stored in cm ?

alexmos · Post by **alexmos** » Sun Apr 29, 2012 6:53 pm

LuFa wrote:

Code: Select all

alt   =   constrain((int16_t)EstAlt, 30, 300)   *   min(avgSqual.res,50) * 2;   // 16   bit   ok:   300   *   50 * 2 = 30000;

this one ? is this stored in cm ?

Yes, exactly

didlawowo69 · Post by **didlawowo69** » Wed May 02, 2012 7:15 am

LuFa wrote:yes , the newest code is based on Mw 2.0 .

You can buy the adns5050 for example here : http://www.digikey.de/product-search/de ... dns%205050

@Alexmos

Wich Altitute Variable is use for calculate the Opticalflow Angle ?
because , i try to start to move your alt code to the newest dev code .
can you tell me which parts i should change ?

Thanks !

is there any other solution to get a flowsensor with a breakout board ? i'm not very good in electronics

alexmos · Post by **alexmos** » Wed May 02, 2012 8:09 am

didlawowo69,
currently there are 2 boards based on ADNS-3080 (from diydrones) and sensor from Flymentor, both still not working with my code. Wait may be somebody will implement and test them.

LuFa · Post by **LuFa** » Wed May 02, 2012 8:49 am

Hi Alexmos ,

works your optical Flow also with Arduino Mega ?

alexmos · Post by **alexmos** » Wed May 02, 2012 9:56 am

I have not tested but it should work.

LuFa · Post by **LuFa** » Sun May 06, 2012 8:34 pm

Hi Alexmos ,

can you tell me how i can set the PID Values for OpticalFlow directly into the Code ?
Because i like to use the Vel Parameter for my alt hold with BMP085 .

And , what did you think about a MaxAngle Parameter ?
I think its great to eliminate a Flip when the Mwii take wrong sensor data .

Thanks

boke · Post by **boke** » Tue May 22, 2012 6:42 pm

Hello,
have somebody a working version of this config?
The videos looks great are u still developing in this way?
Greetings!

LuFa · Post by **LuFa** » Tue May 22, 2012 6:54 pm

Hi ,

look at the googlecode site from Alexmos , there you can find the newest version witch is working

im try to port it to the newest dev , but i have proplems to find a option to set the PID values direct into the code like :

#define OPTICFLOW P-Gain 10.0
#define OPTICFLOW I-Gain 5.0
.....

alexmos · Post by **alexmos** » Tue May 22, 2012 7:33 pm

In the OpticalFlow.pde, find P8[PIDVEL] and I8[PIDVEL] and replace to your values. But defines you wrote is wrong. It should be:
#define OPTFLOW_P_GAIN 50
#define OPTFLOW_I_GAIN 50

(in GUI they are divided by 10, so 5 in GUI will be 50 in code)

boke · Post by **boke** » Tue May 22, 2012 7:56 pm

Hey, thx for answering!
Did u guys use the Flow with a Sonar?
Greetings

LuFa · Post by **LuFa** » Wed May 23, 2012 5:00 pm

yes , i allready fly with the OpticalFlow version from Alexmos and its work very good !

at the moment i work at a fusion of alexmos opticalflow code and the newest multiwii dev code .

@alexmos , i have some questions :

- I have make all chanches in Multiwii.ino for OpticalFLow
- have add OpticalFlow.h
- add OpticalFlow.ino
- and add digitalWriteFast.h

- now iam at the Point were i have to add all variables for OpticalFlow Calculation :
- i have allready add Altitude in Meter (with offset to get the real alt)
- Directly PI defines (#define OPTFLOW_GAIN_P 50 ..... )

now i have to add all the rest of it .
Alexmos , can you tell me wich Variables are your code need to (MagValue , IMUValue .....?) ?

Thanks !

alexmos · Post by **alexmos** » Wed May 23, 2012 5:35 pm

The best way - try to compile and you will see which variable missed. Almous all optflow code is in OpticalFlow.pde

tevada2010 · Post by **tevada2010** » Thu May 24, 2012 12:08 am

Hello alexmos
Can I use only ADNS-5050 or HC-SR04 one or the other?
or Both?

alexmos · Post by **alexmos** » Thu May 24, 2012 8:04 am

Yes - simply disable in config.h sensor that you not needed.

didlawowo69 · Post by **didlawowo69** » Thu May 24, 2012 10:07 am

hi alexmos,
so your code work with ultrasonic sensors like HC SR04 ?

alexmos · Post by **alexmos** » Thu May 24, 2012 10:29 am

Yes, HC SR04 is working for me. But I was written code only for 328p chip (check your flight control board).

tevada2010 · Post by **tevada2010** » Thu May 24, 2012 11:26 am

How maximum height ADNS-5050 can work? and How maximum height sonar can work?

didlawowo69 · Post by **didlawowo69** » Thu May 24, 2012 1:57 pm

alexmos wrote:Yes, HC SR04 is working for me. But I was written code only for 328p chip (check your flight control board).

like arduino pro mini ?

LuFa · Post by **LuFa** » Thu May 24, 2012 7:21 pm

Hi Alexmos

Now i have bring to work the OpticalFlow code with the newest Dev code

But i have one Proplem with the Newest Dev and also your code :

If I Activate AltHold the Mwii Hold the Alt Perfekct for the first 20sek. , after that the Mwii does begin to move down and do nearly landing ^^

I have the same pid-settings like you for the moment , wich parameter can i change to eliminate the effect ?

boke · Post by **boke** » Thu May 24, 2012 10:10 pm

Hello, this sound like your I-value is to small. Put it a little higher, but only a little, that means if you have 0.030 than try 0.035.
Pease give me a feedback if it works.
Greetings.

alexmos · Post by **alexmos** » Fri May 25, 2012 7:09 am

tevada2010 wrote:How maximum height ADNS-5050 can work? and How maximum height sonar can work?

Sonar works up to 2-3m, optical flow - up to 5m. Above it works to, but proportionaly decrease strenght.

alexmos · Post by **alexmos** » Fri May 25, 2012 7:34 am

didlawowo69 wrote:
alexmos wrote:Yes, HC SR04 is working for me. But I was written code only for 328p chip (check your flight control board).

like arduino pro mini ?

Yes exactly. Also Crius MWC and many other has this chip

alexmos · Post by **alexmos** » Fri May 25, 2012 7:56 am

LuFa wrote:Hi Alexmos

Now i have bring to work the OpticalFlow code with the newest Dev code
But i have one Proplem with the Newest Dev and also your code :

If I Activate AltHold the Mwii Hold the Alt Perfekct for the first 20sek. , after that the Mwii does begin to move down and do nearly landing ^^

I have the same pid-settings like you for the moment , wich parameter can i change to eliminate the effect ?

It is hard to give any advice, because you copy-paste my code and may mistake enywhere. Try to output some vars to GUI and check how it works.

tevada2010 · Post by **tevada2010** » Fri May 25, 2012 1:18 pm

Is this video you use ADNS-5050 only or use with sonar both?

http://www.youtube.com/watch?feature=pl ... dZnxTz1Y_g

alexmos · Post by **alexmos** » Fri May 25, 2012 1:55 pm

On this both Optflow and Sonar sensors are used.

tevada2010 · Post by **tevada2010** » Fri May 25, 2012 2:08 pm

alexmos wrote:On this both Optflow and Sonar sensors are used.

ADNS-5050 for Position Hold?
Sonar for Alt Hold?

I have upload your code my radio tx can't to use.

alexmos · Post by **alexmos** » Fri May 25, 2012 6:00 pm

tevada2010 wrote:
alexmos wrote:On this both Optflow and Sonar sensors are used.

ADNS-5050 for Position Hold?
Sonar for Alt Hold?

I have upload your code my radio tx can't to use.

Yes, ADNS-5050 and sonar HC-SR04

I am using SumPPM, if you using separate channels from Rx - set in in config.h And check ALL other parameters carefully because config is tuned for my setup.

tevada2010 · Post by **tevada2010** » Sat May 26, 2012 3:43 am

How connect ADNS-5050 to pin's multiwii Crius SE?
SCLK=>pin?
SDIO=>pin?
NCS=>pin?
VDD=>+5V
NRESET=>+5V

alexmos · Post by **alexmos** » Sat May 26, 2012 10:22 am

For Crius SE I have definitions:
#define OPTFLOW 5050
#define OF_SCLK PITCHPIN
#define OF_SDIO YAWPIN
#define OF_NCS ROLLPIN

PITCHPIN,YAWPIN,ROLLPIN - marked on the board for RX connection. But you may choose any of free pins and set it numbers in config.h
Also VDD +5V, NRESET +5V

tevada2010 · Post by **tevada2010** » Sat May 26, 2012 3:09 pm

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

boke · Post by **boke** » Sat May 26, 2012 9:04 pm

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