Attemp to integrate sonar (ultrasonic sensor)

rortega · Post by **rortega** » Fri Nov 23, 2012 8:50 pm

Here you are the video: http://youtu.be/4fwnxL15yCQ
Sorry about the quality, duration, ... Its all I can record with my smartfone while it is on the ground ...

jy0933 · Post by **jy0933** » Fri Nov 23, 2012 10:59 pm

KasparsL wrote:Why the HC-SR04 sonar is not a option in official dev. versions? It is quite a pain to try to integrate it every time!

same question here... since sr04 is much cheaper and easier to get... why isnt it in the official fork?

excalibur · Post by **excalibur** » Sat Nov 24, 2012 10:26 am

Hello,

is it possible to get information about how i can use it with my CRIUS AIO V1.0. The official Trigger Output should be PIN9 i think und the Echo Input PIN10. If i simply change them in def.h nothing happens and in debug mode there are no informations about measering. Must there been additional changes?

Code: Select all

#if defined(SONAR_GENERIC_ECHOPULSE)
  #define SONAR_GEP_TriggerPin             12   <<< PIN9
  #define SONAR_GEP_TriggerPin_PINMODE_OUT pinMode(SONAR_GEP_TriggerPin,OUTPUT);
  #define SONAR_GEP_TriggerPin_PIN_HIGH    PORTB |= 1<<6;
  #define SONAR_GEP_TriggerPin_PIN_LOW     PORTB &= ~(1<<6);
  #define SONAR_GEP_EchoPin                11   <<<< PIN10
  #define SONAR_GEP_EchoPin_PINMODE_IN     pinMode(SONAR_GEP_EchoPin,INPUT);
  #define SONAR_GEP_EchoPin_PCINT          PCINT5
  #define SONAR_GEP_EchoPin_PCICR          PCICR |= (1<<PCIE0); // PCINT 0-7 belong to PCIE0
  #define SONAR_GEP_EchoPin_PCMSK          PCMSK0 = (1<<SONAR_GEP_EchoPin_PCINT); // Mask Pin PCINT5 - all other PIns PCINT0-7 are not allowed to create interrupts!
  #define SONAR_GEP_EchoPin_PCINT_vect     PCINT0_vect  // PCINT0-7 belog PCINT0_vect
  #define SONAR_GEP_EchoPin_PIN            PINB  // PCINT0-7 belong to PINB
#endif

best regards
Christian

penpen77 · Post by **penpen77** » Sat Nov 24, 2012 11:14 am

i'm using a crius for my test, the default def.h I left in the archive is made for crius pinout

excalibur · Post by **excalibur** » Sat Nov 24, 2012 11:25 am

Hmm, but why doesn't that works for me? Is it right, that in debug-mode Debug1 should show the altitude?

Do you also use the connections marked here as trigger and echo?

rortega · Post by **rortega** » Sat Nov 24, 2012 6:49 pm

A new and better video that shows how it flies in ANGLE+SONAR/BARO+MAG mode: https://www.youtube.com/watch?v=eiv7_8GAN20

And this other only with acro mode: https://www.youtube.com/watch?v=J0Mz-SKEU-4

rortega · Post by **rortega** » Sat Nov 24, 2012 9:46 pm

Excalibur, I reply to you in the public thread, so it will be usefull for others:

Here you are how it's defined in def.h file:

Code: Select all

#if defined(SONAR_GENERIC_ECHOPULSE)
  #define SONAR_GEP_TriggerPin             12
  #define SONAR_GEP_TriggerPin_PINMODE_OUT pinMode(SONAR_GEP_TriggerPin,OUTPUT);
  #define SONAR_GEP_TriggerPin_PIN_HIGH    PORTB |= 1<<6;
  #define SONAR_GEP_TriggerPin_PIN_LOW     PORTB &= ~(1<<6);
  #define SONAR_GEP_EchoPin                11
  #define SONAR_GEP_EchoPin_PINMODE_IN     pinMode(SONAR_GEP_EchoPin,INPUT);
  #define SONAR_GEP_EchoPin_PCINT          PCINT5
  #define SONAR_GEP_EchoPin_PCICR          PCICR |= (1<<PCIE0); // PCINT 0-7 belong to PCIE0
  #define SONAR_GEP_EchoPin_PCMSK          PCMSK0 = (1<<SONAR_GEP_EchoPin_PCINT); // Mask Pin PCINT5 - all other PIns PCINT0-7 are not allowed to create interrupts!
  #define SONAR_GEP_EchoPin_PCINT_vect     PCINT0_vect  // PCINT0-7 belog PCINT0_vect
  #define SONAR_GEP_EchoPin_PIN            PINB  // PCINT0-7 belong to PINB
#endif

So, yo only need to connect Triger to signal pin 12, and Echo to signal pin 11, exactly the pins with same numbers as you see on the board. Also you need to connect vcc and gnd wires to the vcc pin and gnd pin 12 or 11, any of them is the same.

The image linked inside the thread of the forum is not correct, it use other pins. If you use the code above as it is defined, you must use pins 11 and 12 of your board. Thats all.

Don't forget to define generic sonar in config.h:

Code: Select all

/* Generic sonar: hc-sr04, srf04, dyp-me007, all generic sonar with echo/pulse pin 
         default pulse is PH6/12, echo is PB4/11
      */
      #define SONAR_GENERIC_ECHOPULSE 
      #define SONAR_GENERIC_SCALE 58 //scale for ranging conversion (hcsr04 is 58)
      //#define SONAR_GENERIC_MAX_RANGE 500 //cm (could be more)
      #define SONAR_GENERIC_MAX_RANGE 450

I use 450 cm as max range because this is the value in the specs from the supplier where i bought it.

Sorry for not sending an image of my quad, but I have to disassemble many parts to do a good photo, but if you do what I say it will work nice.

rortega · Post by **rortega** » Sat Nov 24, 2012 9:51 pm

Perhaps, if you upload megapirates code to your board you must use "the official pins 9 and 10". But for the code of this thread, use 11 and 12.

penpen77 · Post by **penpen77** » Sat Nov 24, 2012 10:08 pm

you can use others pin, but in this case, you'll have to change the other parameters/defs SONAR_GEP_TriggerPin_xxx (bit shifting, interrupt, etc...). The DEFINE list in GENERIC_ECHO_PULSE is made to make this easy: refere to arduino 2560 pinout: 12 is PB6 (so shifting 6), 11 is pb5, pcint5 so pcint0-7 range, so PCINT0/PCIE0 flag. Adapt that with your pin selection (but without overriding pin/io/interrupt used by multiwii "core")

vpb · Post by **vpb** » Sun Nov 25, 2012 2:37 am

Has someone used more than 1 sonar to implement object avoidance test?

excalibur · Post by **excalibur** » Sun Nov 25, 2012 8:31 pm

Thanks to all, it's working now. Thought that i have to use PIN 9 and 10 in any case. Next step is to get code for autolanding by sonar.

P.S.: Where can i get more information about all the definitions that are used to define the pinouts, so that i would be able to change or add pin's? Are there describtions or tutorials?

Code: Select all

  #define SONAR_GEP_TriggerPin_PIN_HIGH    PORTB |= 1<<6;
  #define SONAR_GEP_TriggerPin_PIN_LOW     PORTB &= ~(1<<6);
  #define SONAR_GEP_EchoPin_PINMODE_IN     pinMode(SONAR_GEP_EchoPin,INPUT);
  #define SONAR_GEP_EchoPin_PCINT          PCINT5
  #define SONAR_GEP_EchoPin_PCICR          PCICR |= (1<<PCIE0); // PCINT 0-7 belong to PCIE0
  #define SONAR_GEP_EchoPin_PCMSK          PCMSK0 = (1<<SONAR_GEP_EchoPin_PCINT); // Mask Pin PCINT5 - all other PIns PCINT0-7 are not allowed to create interrupts!
  #define SONAR_GEP_EchoPin_PCINT_vect     PCINT0_vect  // PCINT0-7 belog PCINT0_vect
  #define SONAR_GEP_EchoPin_PIN            PINB  // PCINT0-7 belong to PINB

excalibur · Post by **excalibur** » Tue Nov 27, 2012 5:01 am

Goog morning,

back from testing and several problems detected. First of all, when i arm my copter it goes some times directly in failsafe mode as if there is not enough "power" left for the rest of the board. Sometimes it works. Is it possible that gps and sonar together need to much current?
Next prob was, that as i switched on level+bara it suddenly pitches backward. All qyros, acc and mag were calibrated but he still wants to drift in one direction backwards.

best regards Christian

Paisley · Post by **Paisley** » Wed Nov 28, 2012 12:12 am

Sorry for asking, but I can't find it in the thread... does there exist a working I²C-sonarcode? I really don't like the idea of connecting the sonar directly to the controller and want to use I²C to do so...

eXc · Post by **eXc** » Thu Dec 06, 2012 1:13 pm

Hello,

so i have tested a while today because of the goog wheather. But the sonar function isn't realy working with my crius. In the MWC i can see the debugged values but the activated baro function doesn't work.
Do i have to activated angle or horizon as well? Is it normal, that when i have activated baro and then change my throttle stick, that the baro button in MWC blinks?

best regards
Christian

jy0933 · Post by **jy0933** » Mon Dec 10, 2012 9:23 pm

GLB gets some TTL serial sonar now...
http://www.goodluckbuy.com/uart-ttl-ser ... dule-.html

i wonder except the connection.. what other differences are between ttl/i2c/ trig-echo?

copterrichie · Post by **copterrichie** » Mon Dec 10, 2012 9:34 pm

jy0933 wrote:GLB gets some TTL serial sonar now...
http://www.goodluckbuy.com/uart-ttl-ser ... dule-.html

i wonder except the connection.. what other differences are between ttl/i2c/ trig-echo?

In other to get this one to work, a serial port or Softserial is required.

kataventos · Post by **kataventos** » Sat Dec 15, 2012 6:31 pm

Hi,

I ordered HC-SR04... this one http://www.goodluckbuy.com/ultrasonic-w ... -sr04.html

Since we are using trigger and echo, almost any ultrasonic sensor with this will do the job, or not?

Cheers,
KV

EDIT: Yes... I saw now that some are already using it! Thanks.

rortega · Post by **rortega** » Tue Dec 18, 2012 8:42 pm

My last video testing POSHOLD and RTH with GPS and serial sonar HC-SR04 integrated in r12340 version.

http://www.youtube.com/watch?v=Jmue9jEYwc8

simone76 · Post by **simone76** » Wed Dec 19, 2012 2:21 pm

penpen77 wrote:if you want to merge change, take care of crappy piece of code added to "emulate" Baro mode presence without baro

can you point this workaround you added?
tnx

Lithium_Bob · Post by **Lithium_Bob** » Fri Feb 01, 2013 7:24 am

Hi all. I have been trying without success to get my Crius AIOP V1.1 board going with sonar. I am new to editing code so don't know very much about it. I'm using the HC-SR04 from Hobby King. I'm flying a quad in X mode. Can somebody post a copy of their working sketch please. Thanks in advance.

Lithium Bob.

elriri · Post by **elriri** » Fri Feb 08, 2013 1:59 pm

Hi all. I have been trying without success to get my Crius AIOP V1.1 board going with sonar. I am new to editing code so don't know very much about it. I'm using the HC-SR04 from Hobby King. I'm flying a quad in X mode. Can somebody post a copy of their working sketch please. Thanks in advance.

Lithium Bob.

Same problem for me !

maccoa · Post by **maccoa** » Sun Feb 17, 2013 1:51 am

hello all. sorry (google translation).

very good topic! I tested "MultiWii_21_1177_sonar" arduino mega 2560 on a HC-SR04 sonar. it works very well.

Now I would like to install it on my FC with the latest version of install multiwii.

I test:

Excalibur, I reply to you in the public thread, so it will be usefull for others:

Here you are how it's defined in def.h file:

CODE: SELECT ALL
#if defined(SONAR_GENERIC_ECHOPULSE)
#define SONAR_GEP_TriggerPin 12
#define SONAR_GEP_TriggerPin_PINMODE_OUT pinMode(SONAR_GEP_TriggerPin,OUTPUT);
#define SONAR_GEP_TriggerPin_PIN_HIGH PORTB |= 1<<6;
#define SONAR_GEP_TriggerPin_PIN_LOW PORTB &= ~(1<<6);
#define SONAR_GEP_EchoPin 11
#define SONAR_GEP_EchoPin_PINMODE_IN pinMode(SONAR_GEP_EchoPin,INPUT);
#define SONAR_GEP_EchoPin_PCINT PCINT5
#define SONAR_GEP_EchoPin_PCICR PCICR |= (1<<PCIE0); // PCINT 0-7 belong to PCIE0
#define SONAR_GEP_EchoPin_PCMSK PCMSK0 = (1<<SONAR_GEP_EchoPin_PCINT); // Mask Pin PCINT5 - all other PIns PCINT0-7 are not allowed to create interrupts!
#define SONAR_GEP_EchoPin_PCINT_vect PCINT0_vect // PCINT0-7 belog PCINT0_vect
#define SONAR_GEP_EchoPin_PIN PINB // PCINT0-7 belong to PINB
#endif

So, yo only need to connect Triger to signal pin 12, and Echo to signal pin 11, exactly the pins with same numbers as you see on the board. Also you need to connect vcc and gnd wires to the vcc pin and gnd pin 12 or 11, any of them is the same.

The image linked inside the thread of the forum is not correct, it use other pins. If you use the code above as it is defined, you must use pins 11 and 12 of your board. Thats all.

Don't forget to define generic sonar in config.h:

CODE: SELECT ALL
/* Generic sonar: hc-sr04, srf04, dyp-me007, all generic sonar with echo/pulse pin
default pulse is PH6/12, echo is PB4/11
*/
#define SONAR_GENERIC_ECHOPULSE
#define SONAR_GENERIC_SCALE 58 //scale for ranging conversion (hcsr04 is 58)
//#define SONAR_GENERIC_MAX_RANGE 500 //cm (could be more)
#define SONAR_GENERIC_MAX_RANGE 450

I use 450 cm as max range because this is the value in the specs from the supplier where i bought it.

Sorry for not sending an image of my quad, but I have to disassemble many parts to do a good photo, but if you do what I say it will work nice.

but it does not work.

someone could explain to me as how to integrate it into the dev version of MultiWii version of r1342 (2013/02/14)

I tried to compare the two versions to see changes, but I admit that it is beyond my compétances

merci

jy0933 · Post by **jy0933** » Sat Feb 23, 2013 4:03 am

when using a no baro board... how do i enable alt hold?

it seems if i turn on baro... it is not really working

thanks

kataventos · Post by **kataventos** » Wed Mar 06, 2013 8:38 pm

Hi,

I would too like to have this running on latest developments, is it so much to ask for some organization here?
What about to answer to people requests? I too have an extremely expensive HC SR04 :mrgreen:

and I would like to use it sometime.

Even with latest great developments on alt code I think that this continues to be the best and safe/accurate way to auto land. Besides this, I just don´t get why it is not available (HC SR04) on the MW code (even for monitoring proposes) and using dedicated/or other pins for MEGA Boards using trigger and echo, since it is proved to work fine with older MW versions.

I sincerely hope that some of the developers that have done a good job on this in past can answer me and to other users questions, at least to say that no more work should be done on this, then questions will stop or someone can continue with developments! OK?

Tank you.
Cheers,
KV

basti · Post by **basti** » Tue Mar 12, 2013 10:09 am

I'm just getting into multiwii/multicopters, it's been a bit of a steep learning curve

I'm using a hobbyking multiwii board on my tricopter, which has an atmega328p. On this board pin 11 is already used for one of the esc's so I was wondering what would have to be done to this code to use a different pin as the echo?

Code: Select all

#if defined(SONAR_GENERIC_ECHOPULSE)
#define SONAR_GEP_TriggerPin 12
#define SONAR_GEP_TriggerPin_PINMODE_OUT pinMode(SONAR_GEP_TriggerPin,OUTPUT);
#define SONAR_GEP_TriggerPin_PIN_HIGH PORTB |= 1<<6;
#define SONAR_GEP_TriggerPin_PIN_LOW PORTB &= ~(1<<6);
#define SONAR_GEP_EchoPin 11
#define SONAR_GEP_EchoPin_PINMODE_IN pinMode(SONAR_GEP_EchoPin,INPUT);
#define SONAR_GEP_EchoPin_PCINT PCINT5
#define SONAR_GEP_EchoPin_PCICR PCICR |= (1<<PCIE0); // PCINT 0-7 belong to PCIE0
#define SONAR_GEP_EchoPin_PCMSK PCMSK0 = (1<<SONAR_GEP_EchoPin_PCINT); // Mask Pin PCINT5 - all other PIns PCINT0-7 are not allowed to create interrupts!
#define SONAR_GEP_EchoPin_PCINT_vect PCINT0_vect // PCINT0-7 belog PCINT0_vect
#define SONAR_GEP_EchoPin_PIN PINB // PCINT0-7 belong to PINB
#endif

I have plenty of pins free as I'm using ppm in.

Ok figured it out after a bit of trial and mostly error

this is the code segment I used in def.h:

Code: Select all

#if defined(SONAR_GENERIC_ECHOPULSE)
  #define SONAR_GEP_TriggerPin             12
  #define SONAR_GEP_TriggerPin_PINMODE_OUT pinMode(SONAR_GEP_TriggerPin,OUTPUT);
  #define SONAR_GEP_TriggerPin_PIN_HIGH    PORTB |= (1<<4);
  #define SONAR_GEP_TriggerPin_PIN_LOW     PORTB &= ~(1<<4);
  #define SONAR_GEP_EchoPin                8 // using pin 8 instead of 11
  #define SONAR_GEP_EchoPin_PINMODE_IN     pinMode(SONAR_GEP_EchoPin,INPUT);
  #define SONAR_GEP_EchoPin_PCINT          PCINT0  //pin 8 is PCINT0 instead of pin 11 which is PCINT5 
  #define SONAR_GEP_EchoPin_PCICR          PCICR |= (1<<PCIE0); // PCINT 0-7 belong to PCIE0
  #define SONAR_GEP_EchoPin_PCMSK          PCMSK0 = (1<<SONAR_GEP_EchoPin_PCINT); // Mask Pin PCINT0 - all other PIns PCINT0-7 are not allowed to create interrupts!
  #define SONAR_GEP_EchoPin_PCINT_vect     PCINT0_vect  // PCINT0-7 belog PCINT0_vect
  #define SONAR_GEP_EchoPin_PIN            PINB  // PCINT0-7 belong to PINB
#endif

That's in the code from this post viewtopic.php?f=7&t=1033&start=160#p26227

I'm using this board http://www.hobbyking.com/hobbyking/stor ... _Port.html which has pin8 unused as well as spare breakout pads/holes so I soldered a wire from pin8 to one of those.
I also added in some code to make the P term 5x more sensitive when using the sonar for a much more responsive and precise altitude hold indoors. Works like a treat now, with my not so good flying skills I can now hover in my kitchen!

One thing that tripped me up for a few hours was that the sonar didn't work well at all when powering the board from usb - readings all over the place. Fine when a flight battery is connected though so that's good.

Thanks to everyone contributing to this, it's awesome to be able to go in and chop'n'change stuff!

BeringBullet · Post by **BeringBullet** » Thu Apr 04, 2013 9:41 pm

is this a dead topic, I have a HC SR04 would love to use if for more accorded alt data but I see a lot of question and no replays?

basti · Post by **basti** » Sat Apr 06, 2013 9:46 am

What are you wanting to know? I'm fairly new to this stuff so I might not be able to help but I can try

Mine's all going now, I made some changes to the code and I'm pretty happy with it.

shadow29 · Post by **shadow29** » Sat Apr 06, 2013 6:37 pm

Does anyone have the code for multiwii 2.2? I would be grateful for entire code

penpen77 · Post by **penpen77** » Fri Apr 12, 2013 9:32 pm

Yes, this is a (kind of) dead topic since none of questions about request for feature has been answered.... For now, this is just a fusion/stand alone alt hold usinh baro+sonar or sonar alone

I've join the sketch i'm using, building and tuning are as described before in the thread. Beware of config.h which isn't clean

For any update of incoming version or "nightlybuild", the sonar code (generic driver or alt hold part) isn't intrusive, so a simple diff is enough to merge both sonar code and newly mwc.

copterrichie · Post by **copterrichie** » Fri Apr 12, 2013 9:34 pm

I think this is sad because I have seen some of the videos posted with the sonar working. I can not promise when, but I will try it on one of my copters.

penpen77 · Post by **penpen77** » Sat Apr 13, 2013 11:29 am

this is the same code as before, nothing as changed, it's work as shown in video

kataventos · Post by **kataventos** » Thu Apr 18, 2013 3:01 pm

Hi Penpen77,

it´s been a while since my last visit here. Good to know that you were able to get some time to it thanks.

Can you please post here the patches you have worked on? It would be nice because every time I need to "change version" is a pain in the ass because I already have the FRSky patch and other stuff going on, and with Multiwii 2.2 most of the stuff had to be changed...

Thank you again for your time on this.

Fly safe.
Cheers,
KV

QSXE · Post by **QSXE** » Tue May 21, 2013 9:00 pm

It seems like my multiwii pro flight controll doesn't have pin 11 and 12.

Here a picture of the pinouts:

Caused by I have changed the Signalpins to 9 and 10.

Code: Select all

#if defined(SONAR_GENERIC_ECHOPULSE)
  #define SONAR_GEP_TriggerPin             10
  #define SONAR_GEP_TriggerPin_PINMODE_OUT pinMode(SONAR_GEP_TriggerPin,OUTPUT);
  #define SONAR_GEP_TriggerPin_PIN_HIGH    PORTB |= 1<<6;
  #define SONAR_GEP_TriggerPin_PIN_LOW     PORTB &= ~(1<<6);
  #define SONAR_GEP_EchoPin                9
  #define SONAR_GEP_EchoPin_PINMODE_IN     pinMode(SONAR_GEP_EchoPin,INPUT);
  #define SONAR_GEP_EchoPin_PCINT          PCINT5
  #define SONAR_GEP_EchoPin_PCICR          PCICR |= (1<<PCIE0); // PCINT 0-7 belong to PCIE0
  #define SONAR_GEP_EchoPin_PCMSK          PCMSK0 = (1<<SONAR_GEP_EchoPin_PCINT); // Mask Pin PCINT5 - all other PIns PCINT0-7 are not allowed to create interrupts!
  #define SONAR_GEP_EchoPin_PCINT_vect     PCINT0_vect  // PCINT0-7 belog PCINT0_vect
  #define SONAR_GEP_EchoPin_PIN            PINB  // PCINT0-7 belong to PINB
#endif

Sadly this doesn't helps at. Sonar still doesn't works.

After searching for a while I found this:

penpen77 wrote:you can use others pin, but in this case, you'll have to change the other parameters/defs SONAR_GEP_TriggerPin_xxx (bit shifting, interrupt, etc...). The DEFINE list in GENERIC_ECHO_PULSE is made to make this easy: refere to arduino 2560 pinout: 12 is PB6 (so shifting 6), 11 is pb5, pcint5 so pcint0-7 range, so PCINT0/PCIE0 flag. Adapt that with your pin selection (but without overriding pin/io/interrupt used by multiwii "core")

May this could resolve my problem but unfortunately my English is too bad to understand what the qoutation above should exactly mean.

It would be great pleasure if anyone could help me with editing pinouts, cause having a working sonar sensor would be just awesome.

CU QSXE

SvenH · Post by **SvenH** » Thu May 23, 2013 11:04 pm

Hi QSXE,

in case that you have got the HK multiWii pro board I can show you, how I got this to work.
I used a ISP pin for the echo and a motor pin for the trigger of the sonar since I dont need them (I have a X-Quad).

Within the def header file I set the pins as shown next:

Code: Select all

#if defined(SONAR_GENERIC_ECHOPULSE)
  #define SONAR_GEP_TriggerPin               10 // --> D10
  #define SONAR_GEP_TriggerPin_PINMODE_OUT   pinMode(SONAR_GEP_TriggerPin,OUTPUT);
  #define SONAR_GEP_TriggerPin_PIN_HIGH      digitalWrite(SONAR_GEP_TriggerPin, HIGH); //PORTB |= 1<<4;
  #define SONAR_GEP_TriggerPin_PIN_LOW       digitalWrite(SONAR_GEP_TriggerPin, LOW); //PORTB &= ~(1<<4);
  
  #define SONAR_GEP_EchoPin                  52 // --> ISP-Pin SCK
  #define SONAR_GEP_EchoPin_PINMODE_IN       pinMode(SONAR_GEP_EchoPin,INPUT);
  #define SONAR_GEP_EchoPin_PCINT            PCINT1
  #define SONAR_GEP_EchoPin_PCICR            PCICR |= (1<<PCIE0); // PCINT 0-7 belong to PCIE0
  #define SONAR_GEP_EchoPin_PCMSK            PCMSK0 = (1<<SONAR_GEP_EchoPin_PCINT); // Mask Pin PCINT5 - all other PIns PCINT0-7 are not allowed to create interrupts!
  #define SONAR_GEP_EchoPin_PCINT_vect       PCINT0_vect  // PCINT0-7 belog PCINT0_vect
  #define SONAR_GEP_EchoPin_PIN              PINB  // PCINT0-7 belong to PINB
#endif

Then I connected the pins as you can see in the following image:

Using this setup, the HC-SR04 worked with my HK MultiWii pro board.
I hope this can help you

Kind regards,
Sven

kataventos · Post by **kataventos** » Sat May 25, 2013 2:24 am

Hi all,

After my requests for the ones who had the code working to post the patch for people that like me want to know exactly what´s running inside without success, I decided to do it.

Here is the entire code for generic trigger/echo sonar´s (Example_HC-SR04) I am using Crius AIO Pro.

REMEMBER: Use non official patches at your own risc.

@ noobies
When you see commented (//) look for the line on sketch and add as written below. Else do as written. The (...) change the add/function.

Main.ino

Code: Select all

//#if BARO && (!defined(SUPPRESS_BARO_ALTHOLD))
 #if (BARO||SONAR) && (!defined(SUPPRESS_BARO_ALTHOLD))  //add
...
static int32_t  BaroHome=0; // add after line 227
...
//#if BARO  // Change on two places
#if (BARO||SONAR)   //add
...
//#if BARO && (!defined(SUPPRESS_BARO_ALTHOLD))
#if (BARO||SONAR) && (!defined(SUPPRESS_BARO_ALTHOLD))    //add

IIMU.ino

Code: Select all

//#if BARO
#if BARO || SONAR 
...
//add after line 274
#if BARO
//add after line 284
 #endif
...
//EstAlt = (EstAlt * 6 + BaroAlt * 2) >> 3; // additional LPF to reduce baro noise (faster by 30 µs) // comment
// add as follows the entire function
#if BARO && !SONAR //baro alone
    EstAlt = (EstAlt * 6 + BaroAlt * 2) >> 3; // additional LPF to reduce baro noise (faster by 30 µs)
  #elif SONAR && !BARO  //sonar alone
    // LOG: for now, keep the last good reading and no more than max alt
    if(sonarAlt <0 || sonarAlt> SONAR_MAX_HOLD) sonarAlt = lastSonarAlt;
    else lastSonarAlt = sonarAlt;
    
    // LOG: need for LPF ? if yes, value ?
    EstAlt = EstAlt*SONAR_BARO_LPF_LC + sonarAlt*(1-SONAR_BARO_LPF_LC);
  #elif SONAR && BARO  //fusion
    // LOG: I would like some manually way to set offset....
    // LOG: if you take off from a chair/desk/something higher than the "real" ground, when switching to sonar and low cut fusion
    // LOG: the home offset will be higher than the ground and maybe mess up things...
    if(!f.ARMED) { //init offset till motors not armed
      BaroHome = (EstAlt * 6 + BaroAlt * 2) >> 3; // play with optimal coef. here
    }
    debug[3] = BaroHome;
    if(sonarAlt <0 || sonarAlt> SONAR_MAX_HOLD) sonarAlt = lastSonarAlt;
    else lastSonarAlt = sonarAlt;
    
    if(sonarAlt < SONAR_BARO_FUSION_LC) {
      // LOG: same as sonar alone
      // LOG: need for LPF ? if yes, value ?
      // LOG: trying 1/9 ratio (same as sonar alone, and as we share same pid conf than baro, we can't have two separate config, 1/9 is too much for my config, need raw values)
      EstAlt = EstAlt*SONAR_BARO_LPF_LC + (BaroHome+sonarAlt)*(1-SONAR_BARO_LPF_LC);
   } else if(sonarAlt < SONAR_BARO_FUSION_HC) {
        
      float fade = SONAR_BARO_FUSION_RATIO;
      if(fade==0.0) fade = ((float)(SONAR_BARO_FUSION_HC-sonarAlt))/(SONAR_BARO_FUSION_HC-SONAR_BARO_FUSION_LC);
      fade = constrain(fade, 0.0f, 1.0f);
        
      // LOG: will LPF should be faded too ? sonar is less sloppy than baro and will be oversmoothed
      // LOG: try same as baro alone 6/4 ratio (same as above about smoothing)
      EstAlt = EstAlt*SONAR_BARO_LPF_HC + ((BaroHome+sonarAlt)*fade + (BaroAlt)*(1-fade))*(1-SONAR_BARO_LPF_HC);
    } else {
      // LOG:same as baro
      EstAlt = (EstAlt * 6 + BaroAlt * 2) >> 3; // additional LPF to reduce baro noise (faster by 30 µs)
    }
  #endif
  
  debug[1] = AltHold;

LCD.ino

Code: Select all

// change as followed 4 times
//#if  BARO && (!defined(SUPPRESS_BARO_ALTHOLD))
#if (BARO||SONAR) && (!defined(SUPPRESS_BARO_ALTHOLD))
...
// change as followed 2 times
//#if BARO
#if (BARO||SONAR)

Sensors.ino

Code: Select all

//add as follows after line 1702
#elif defined(SONAR_GENERIC_ECHOPULSE) 

volatile unsigned long SONAR_GEP_startTime = 0;
volatile unsigned long SONAR_GEP_echoTime = 0;
volatile static int32_t  tempSonarAlt=0;

void Sonar_init()
{
  SONAR_GEP_EchoPin_PCICR;
  SONAR_GEP_EchoPin_PCMSK;
  SONAR_GEP_EchoPin_PINMODE_IN;
  SONAR_GEP_TriggerPin_PINMODE_OUT;
  Sonar_update();
}

ISR(SONAR_GEP_EchoPin_PCINT_vect) {
  if (SONAR_GEP_EchoPin_PIN & (1<<SONAR_GEP_EchoPin_PCINT)) { 
 SONAR_GEP_startTime = micros();
  }
  else {
    SONAR_GEP_echoTime = micros() - SONAR_GEP_startTime;
    if (SONAR_GEP_echoTime <= SONAR_GENERIC_MAX_RANGE*SONAR_GENERIC_SCALE)                                  
      tempSonarAlt = SONAR_GEP_echoTime / SONAR_GENERIC_SCALE;
    else
      tempSonarAlt = -1;
  }
}

void Sonar_update()
{
    sonarAlt=1+tempSonarAlt;
    SONAR_GEP_TriggerPin_PIN_LOW;
    delayMicroseconds(2);
    SONAR_GEP_TriggerPin_PIN_HIGH;
    delayMicroseconds(10);
    SONAR_GEP_TriggerPin_PIN_LOW;
}

Serial.ino

Code: Select all

//#if BARO && (!defined(SUPPRESS_BARO_ALTHOLD))
#if (BARO||SONAR) && (!defined(SUPPRESS_BARO_ALTHOLD))

config.h

Code: Select all

// add after line 181
/* Generic sonar: hc-sr04, srf04, dyp-me007, all generic sonar with echo/pulse pin 
         default pulse is PH6/12, echo is PB4/11
      */
      #define SONAR_GENERIC_ECHOPULSE 
      #define SONAR_GENERIC_SCALE 58 //scale for ranging conversion (hcsr04 is 58)
      #define SONAR_GENERIC_MAX_RANGE 500 //cm (could be more)
...
//add after line 618
/************************* Sonar alt hold / precision / ground collision keeper *******/
    #define SONAR_MAX_HOLD 400 //cm, kind of error delimiter, for now to avoid rocket climbing, only usefull if no baro
    
    //if using baro + sonar       
    #define SONAR_BARO_FUSION_LC 100 //cm, baro/sonar readings fusion, low cut, below = full sonar
    #define SONAR_BARO_FUSION_HC 400 //cm, baro/sonar readings fusion, high cut, above = full baro
    #define SONAR_BARO_FUSION_RATIO 0.0 //0.0-1.0,  baro/sonar readings fusion, amount of each sensor value, 0 = proportionnel between LC and HC
    #define SONAR_BARO_LPF_LC 0.9f 
    #define SONAR_BARO_LPF_HC 0.9f

def.h

Code: Select all

//add after line 1453
#if defined(SONAR_GENERIC_ECHOPULSE)
  #define SONAR_GEP_TriggerPin             12
  #define SONAR_GEP_TriggerPin_PINMODE_OUT pinMode(SONAR_GEP_TriggerPin,OUTPUT);
  #define SONAR_GEP_TriggerPin_PIN_HIGH    PORTB |= 1<<6;
  #define SONAR_GEP_TriggerPin_PIN_LOW     PORTB &= ~(1<<6);
  #define SONAR_GEP_EchoPin                11
  #define SONAR_GEP_EchoPin_PINMODE_IN     pinMode(SONAR_GEP_EchoPin,INPUT);
  #define SONAR_GEP_EchoPin_PCINT          PCINT5
  #define SONAR_GEP_EchoPin_PCICR          PCICR |= (1<<PCIE0); // PCINT 0-7 belong to PCIE0
  #define SONAR_GEP_EchoPin_PCMSK          PCMSK0 = (1<<SONAR_GEP_EchoPin_PCINT); // Mask Pin PCINT5 - all other PIns PCINT0-7 are not allowed to create interrupts!
  #define SONAR_GEP_EchoPin_PCINT_vect     PCINT0_vect  // PCINT0-7 belog PCINT0_vect
  #define SONAR_GEP_EchoPin_PIN            PINB  // PCINT0-7 belong to PINB
...
//#if defined(SRF02) || defined(SRF08) || defined(SRF10) || defined(SRC235) || defined(TINY_GPS_SONAR)
#if defined(SRF02) || defined(SRF08) || defined(SRF10) || defined(SRC235) || defined(TINY_GPS_SONAR) || defined(SONAR_GENERIC_ECHOPULSE) //add
#endif

Have fun and fly safe.

Cheers,
KV

QSXE · Post by **QSXE** » Sun May 26, 2013 12:45 pm

Thanks Sven, pinouts are working now. Helped me quite a lot.

I am using this http://dx.com/p/ultrasonic-sensor-dista ... ino-148659 US-20 sonar whith a max range of 700cm instead of a hcsr04. Unfortunately it seems like the delay between every singe trig is too short for this sonar type. I only get wrong measurement like -0.27m all the time. I alredy have tried to chance the sonar scale but this didn't helped at all. Have anybody an idea where I could change the delay between the trigger impulse in the script?

Kindly regards
QSXE

SvenH · Post by **SvenH** » Wed May 29, 2013 1:38 pm

Hi QSXE,

I'm glad to hear that. Unfortunalety I can't help you on your current problem, but maby you can help me a little bit with my HC-SR04.
My pins are working (as you got yours to work) but I never went further so I never tried the alt-hold. Now after your question I wanted
to test it and I'm facing the problem, that the altitude is calculated correctly only in a range up to 50cm. If increase the distance between the sonar
sensor and the ground there is only a noisy signal around 50cm, even if I'm much higher.

You can see the problem here in these screenshot, I put the tmpSonarAlt into debug[2], so I could see the raw sonar value.
I just pulled my Quad up to 1,80m from the ground and this was the resulting signal. I just can't find the error, since the sensor is looking directly to the ground and the
code is very simple :-(

Did the sensor work on your MultiWii pro Board properly with these pins?
Maby you or someone else has an advice?

Thanks a lot and kind regards
Sven

QSXE · Post by **QSXE** » Fri May 31, 2013 7:16 pm

Hey Sven,

Today my HC-SR04 brand new arrived

. Of course I made some testing but unfortunately it seems like my HC-SR04 doesn't work properly. If i connect it to my Hobbyking Multiwii Pro board I just hear a strange noise but no trigger impulses like by my US-20 Sonar. Also double checking the code helped anything.

Would it be possible to upload your entire Multiwii code as an attachment? Cause in order to this it would be abe to compare messurements and see if I got the same odd measurement like you with my HC-SR04. Aditional i could also give it a try with my US-20 sensor.

Perhaps changing the trigger or/and the echo pin helps. You never know....

Best regards
QSXE

SvenH · Post by **SvenH** » Sat Jun 01, 2013 10:02 am

Hi QSXE,

so I already tried another pin. I tried the D10 and D9 pin but it was the same result. I tried these pins because I loaded ArdupirateNG for testing reason to the MultiWii pro board.
With this sketch the sonar was working perfectly, but this software is to fat in my opinion. While testing the ArduPirateNG sketch I also recognized that the sonar sound was very cyclic. Using the MultiWii sketch I always get a noisy sound from the sonar.

So I next want to look into the ArduPirateNG sketch to find out how they solved this. At the moment I don't get it since the code for the sonar is so clear :-D

Here you can get the sketch I used to test the sonar on my Quad, in this case the echo pin is D10 and the trigger pin is D9 (like it was while testing ArduPirateNG):
https://dl.dropboxusercontent.com/u/7763161/MultiWii.rar

Im very interested in hearing from your results if you would test it!

Btw OT: Are you german? I think I once saw you nickname on the fpv-community.de site ;-)

kind regards
Sven

QSXE · Post by **QSXE** » Sat Jun 01, 2013 1:18 pm

Thanks a lot for your sketch.

Of course I have already tested it with both of my ultrasonic sensors. Sadly the results of my HC-SR04 aren't significantly different. I just get values between 0 and 55 and strange noise. The US-20 sonar doesn't want to deliver any values, but at least the sound pulses are quite cyclic an clear. ( Almost like by MegapirateNG )

You are using the hextronic HC-SR04, right?

OT: Jepp I am fpv-commuity member. Just wondering about your fpv-community nickname.....

best regards,
QSXE

SvenH · Post by **SvenH** » Sat Jun 01, 2013 4:49 pm

Hi QSXE,

so maby I think I got it to work (without looking to MegaPirateNG).
In my code there are some Arduino keywords for setting the pins high and low (digitalWrite), I think they aren't used correctly for doing this.
So I changed my definition by hand like it was in the original posting. Now the range works although there are some peaks from time to time.
But I think this is not critical because of the the fading algorithm.

So here is the code (Trigger: D9, Echo: D10):

Code: Select all

  #define SONAR_GEP_TriggerPin             9 // D9
  #define SONAR_GEP_TriggerPin_PINMODE_OUT pinMode(SONAR_GEP_TriggerPin,OUTPUT);
  #define SONAR_GEP_TriggerPin_PIN_HIGH    PORTH |= 1<<6;
  #define SONAR_GEP_TriggerPin_PIN_LOW     PORTH &= ~(1<<6);
  
  #define SONAR_GEP_EchoPin                10 // D10
  #define SONAR_GEP_EchoPin_PINMODE_IN     pinMode(SONAR_GEP_EchoPin,INPUT);
  #define SONAR_GEP_EchoPin_PCINT          PCINT4
  #define SONAR_GEP_EchoPin_PCICR          PCICR |= (1<<PCIE0); // PCINT 0-7 belong to PCIE0
  #define SONAR_GEP_EchoPin_PCMSK          PCMSK0 = (1<<SONAR_GEP_EchoPin_PCINT);
  #define SONAR_GEP_EchoPin_PCINT_vect     PCINT0_vect  // PCINT0-7 belog PCINT0_vect
  #define SONAR_GEP_EchoPin_PIN            PINB  // PCINT0-7 belong to PINB

Maby you can check whether this works for you too?

OT: Ok then I was remembering correctly ;-)

Im totally new to the fpv stuff and just registered there 4 weeks ago. My nickname there is SvenHenning ;-)

Liebe Grüße
Sven

QSXE · Post by **QSXE** » Sat Jun 01, 2013 6:02 pm

Hi Sven,

Thank you quite much for your awesome help

I have already replaced the "old" def.h code with your updated one. Unfortunately I still get strange results as showen below and a lot of noise if HC-SR04 is connectet.

Could it be that you also modified perhaps just a small pice of code at IIMU.ino or Sensors.ino?

Grüße,
Felix

SvenH · Post by **SvenH** » Sun Jun 02, 2013 1:54 pm

Hey Felix,

I tested it now on the Copter and it worked (jippi ^^).
But I had some strange errors, so first to your question: No except of what I gave to you, I didnt change anything.

But the strange thing what lead me to the solution was:
Normally I connected the Quad by a Serial to Bluetooth device to my PC to view the altitude value.
Yesterday, when I wrote everything works great, I connected the Quad by USB. I could determine that when I had power supply by the USB connection the HC-SR04 was giving right values. Otherwise, when powering by the battery it showed this strange behavior. So what I did for testing was that I was connected to the Quad via bluetooth and at the same time I had the USB cable plugged in. Now in this case the HC-SR04 worked properly and gave correct values. Then while everything worked, I removed the usb cable and boom the strange behavior was back in the same second.

I mesured the pins where I picked up the voltage (see the first picture) and found out that the voltage was too low, maby there is a voltage regulator before this connection.
I now connected the power supply of the HC-SR04 (GND and VCC) to free ESC pins (+/- pins of D12) and now it works

Another error I was facing was that the VCC wire was broken and sometimes had no connection which I didnt recognized....

So maby you could just plug the VCC and GND wires to the pins of D12 and look if this works for you?

Kind regards,
Sven

sorg · Post by **sorg** » Sun Jun 02, 2013 4:41 pm

kataventos wrote:Hi all,

After my requests for the ones who had the code working to post the patch for people that like me want to know exactly what´s running inside without success, I decided to do it.

Here is the entire code for generic trigger/echo sonar´s (Example_HC-SR04) I am using Crius AIO Pro.

<snip>

Thankyou kataventos.

I am trying that.
however, some modification are not clear enough and a real patch file would have been more explicit.
Fore example in multiwii , there is a lot of " If Baro" and you suggest to hange two of them. Which ones ?
In def.h, you have addes a #endif. Where ?

QSXE · Post by **QSXE** » Sun Jun 02, 2013 6:36 pm

Hi Sven,

Congratulation you really git r done. Thats quite awesome

And you are absolutely right there is a ams1117 voltage regulator befor all three serial ports.

Although after switching VCC and GND to pin D10 and powering the board with an UBEC the Sonar trigger impulses are clearer and there is only a tiny little bit of supply noise left the sonar sounds still a bit strange. But may thats normal for the HC-SR04. Sadly I still get no useable values neither on D9 and D 10 nor D44 and D45. Sometimes the values are just 0 or something between 60 and 70 all the time. Thats such a huge disapointment :cry:

In order to this I also testet my three sonars on megapirate to make sure that they are really working properly and I got almost the same crabby results. Considering this it seems like the strange values are caused by a hardware failure .Damn I think its time for a summer job

BTW: Don't be afraid of raising i2c speed at config.h. The MultiWii Pro board handles 400khz easily.

Best regards,
Felix

QSXE · Post by **QSXE** » Tue Jun 04, 2013 6:05 pm

Damn I am just soo stupid... closed tab during writing a post. :oops:

However I made some testings today.I connected a radio chanel to D10 and defined it as AUX3PIN. Fortunately it seems like my D10 signal pin is working just fine as it can be sawen at the srceenshot below.

Nontheless I still get any sonar data. Values are only 0 or something between 60 and 80 if the board is powered with the USB connestor ( maybe some electrical noise ). That's just so disapointing.

I don't know may all of my three sonar sensors are broken. I just don't know... :cry:

Best regards,
Felix

SvenH · Post by **SvenH** » Tue Jun 04, 2013 9:24 pm

Hi Felix,

that sounds very strange, I just can't imagine that all of your sonar sensors are broken...
That would be very dissapointing.

What do you think of this idea:
You could download an exemplary sketch which just enables the sonar functionality for arduino testing reason. (example: click)

Then you could really check whether the sensors are bad or there is some issue with the board/software ?

Kind regards
Sven

QSXE · Post by **QSXE** » Wed Jun 05, 2013 1:04 pm

Hey Sven,

now I upoad an exemplary sonar sketch as recommended and just get the same "mindblowing" results. Serial monitor shows outof range all the time. But if i connect a radio port to the echo pin values proportinal to stick commands are shown, as it could be sawn in the screenshot below.

It quite looks like that really all of my 3 Sonar sensors are broken

May I should invest a 10 psc. lot from aliexpress :mrgreen:

best regards,
Felix

SvenH · Post by **SvenH** » Fri Jun 07, 2013 8:26 am

Hi Felix,
that's totally weird (If you think of the probabillity that all of your 3 sonars are dead.... :roll:

).
Maby you will have more luck with a new one (or more :mrgreen:

), please let me know about it

kind regards
Sven

QSXE · Post by **QSXE** » Sat Jun 15, 2013 1:29 pm

Still waiting for my new sonar... :roll: