Attemp to integrate sonar (ultrasonic sensor)

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:

penpen77 · Post by **penpen77** » Sat Jun 15, 2013 6:34 pm

Sorry, i wasn't come here since several weeks

If you want only code lines to change, juste make a diff of files (btw, i dont remenber if mwc still on svn or switch to git, but both have diff tools/compare fonctionnality)

Tvixen · Post by **Tvixen** » Sun Jun 16, 2013 6:25 pm

SvenH wrote: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

Hi Sven
I did try ur code yesterday and i works well on my HC-SR04, but i have the same spikes as you. It jumps from 0 to 58, and back again every second. (see picture)

I will try to move the echo pin to another pin, to see if it will help.

Thx Tvixen

QSXE · Post by **QSXE** » Thu Jun 20, 2013 1:47 pm

Hey Sven,
I received my new 4th sonar sensor and guess what, everything, expect my quad frame which is broken

, is working fine now. Damn thats abso-bloody-lutely super awesome!!! :mrgreen:

Thanks a lot for your competent help dude!

best regards,
Felix;)

SvenH · Post by **SvenH** » Thu Jun 20, 2013 4:44 pm

Hi Felix,

that's really awesome! You had got so unbelievable bad luck with your sonars that I hope you gonna have much fun now (except of that your quad is broken :-(

).
I very happy you got it to work now :-)

kind regards,
Sven

QSXE · Post by **QSXE** » Mon Jun 24, 2013 4:31 pm

Hey Sven,

My sonar is working fine and I get quite accurate values on dbg 3. But unfortuntaley the alt value is frozen at 0 all the time, which is pretty anoing It doesn't marter what value dbg 3 has.

I am quite sure that this strange behaviour is just cased by a small scripting issue. May I forgot to uncoment somehting, but sadly I just don't get it...

Best regards,
Felix

schnupperm · Post by **schnupperm** » Wed Jun 26, 2013 12:35 am

Hi,

I'm using a HC-SR04 on a HK multiWii pro board and got the same problem as Sven described in this post viewtopic.php?f=7&t=1033&start=200#p36745. For some tests I used this sonar only
code

Code: Select all

int trigPin = 9;
int echoPin = 10;

unsigned long currentTime;
unsigned long loopTime;

unsigned long currentTime2;
unsigned long loopTime2;

long duration;
long distance;

void setup() {
Serial.begin (115200);
pinMode(trigPin, OUTPUT);
pinMode(echoPin, INPUT);
}

void loop() {

currentTime = millis();
if(currentTime >= (loopTime + 200)){
ping();
loopTime = currentTime; // Updates loopTime
}
}

void ping(){

digitalWrite(trigPin, HIGH);
currentTime2 = millis();
if(currentTime2 >= (loopTime2 + 10)){
digitalWrite(trigPin, LOW);
duration = pulseIn(echoPin, HIGH);
distance = (duration/2) / 29.1;
if (distance >= 200 || distance <= 0){
Serial.println("Out of range");
}
else {
Serial.print(distance);
Serial.println(" cm");
}

with the same result: readings between 9 cm and 56 cm.

Yesterday I loaded the same sketch on an arduino nano board and found the sensor working much better:
readings between 1cm and 170cm and beyond that distance "Out of range".

Any ideas?

best regards
frank

SvenH · Post by **SvenH** » Wed Jun 26, 2013 9:34 am

Hi QSXE,

so that sounds weird, does the behaviour change when you arm/disarm the copter? I think there was something depending on this in the
code but I don't know it exactly. Could you test the alt-hold till now with a copter?

So for my case I got it to work as you know but I was not happy with this afterwards. After fighting the PID controller values I switched to ArduPiratesNG two weeks ago and everything except my bluetooth module works properly. Even the alt-hold with sonar works very good.

Kind regards
Sven

picstart · Post by **picstart** » Sat Aug 10, 2013 8:03 pm

The HCSR04 is an inexpensive device. As with all sonars it pings and then an echo comes back or not. It is probably only useful up to 3 meters though the specs may say 7 meters. The issue that has been pointed out before is that by nature this is a blocking process. Since an echo can come back at any time the processor can't go off and do anything else. Because echos have the potential of not coming back at all either due to high reflection angles relative to the ping path or no reflector at all, a maximum wait is established. HCSR04 would set this at 40ms. This is a huge amount of blocking time if done within the multiwii code. It is not that you can't get away with it, it is just not prudent since the flyer can't do a thing during this long lockout. Now if it is turned off by the BARO after a few meters ascent and on within a few meters during descent it may not be to risky. However, if you take off from the ground and land on a roof the BARO doesn't detect the roof but the sonar will. The prudent approach has to be to off load the echo latency to a more capable process; namely an interrupt driven process based on the HSCR04 rise of the echo pin and a further interrupt on the fall of that pin. The time difference between the interrupts is proportional to the distance ( range). The multiwii then just has to poll to get an immediate sonar range based on the last completed echo. This poll after ranging allows the opportunity of averaging the results if several ranging can be made between the multiwii as master asking for a result ( very likely to be true at short ranges due to short return times).. The next issue is the comparative reliance on either the BARO or the sonar. It is obvious the BARO is the winner at any range beyond a few ( 2 or 3) meters. The BARO probably shouldn't be used within 2 meters of take off or landing; so a firm change over point is very viable.

The HCSR04 is a 5v device but the trigger just needs 3v. The device's ping needs the full 5v to be accurate. Further the ping fan out is about 15 deg so at 2 meters any substantial pitch or roll can cause the echo to reflect off ground and not make it back.

I'm a newbie to this but I see the auto landing (take off) like cruise control on a car...move the throttle above the slow auto climb and the throttle wins adjust the pitch or roll or maybe even yaw and the sonar based cruise control is off. Throttle is the gas pedal of the car and pitch roll and yaw the equivalent of the brake pedal as far as cruise control goes.

wareck · Post by **wareck** » Mon Aug 26, 2013 7:18 pm

hi every ones !

This piece of code drive me crazy :twisted:

I put the patch on my file as explain un page 20.
I've always got an error in this part of code:

Code: Select all

ISR(SONAR_GEP_EchoPin_PCINT_vect) {
  if (SONAR_GEP_EchoPin_PIN & (1<<SONAR_GEP_EchoPin_PCINT)) { 
    SONAR_GEP_startTime = micros();
 }

Every time I compile the code, arduino say to me that
"void__vector_9 previously defined"

I must uncomment that line to make the code working

Code: Select all

#define SONAR_GEP_EchoPin_PCINT_vect     PCINT0_vect  // PCINT0-7 belog PCINT0_vect

If I uncoment this line, sonar don't make "tik tik" (it's normal, when I uncoment the line, echo interrupt is disabled)

Can someone help me ?

Olivier

wareck · Post by **wareck** » Mon Aug 26, 2013 7:26 pm

Oupps...
Sorry I found the problem...

May help someones like me becoming crazy...

The interrupt Handler ISR for PCINT0_vect not working with board based on ATMEGA32U.
This because this Handler is already used for PWM ouput...

Compiling code for an Atmega328p or mega works great

U.Sentenza · Post by **U.Sentenza** » Tue Aug 27, 2013 8:31 pm

Hello to all,
although absurd, I do the same:
to eliminate the peaks is possible to calculate the vertical acceleration of the sonar and eliminate the bogus values?

Tomek · Post by **Tomek** » Thu Sep 05, 2013 3:51 am

Hi !
What about old fashion Maxbotix LV-EZ1, it can in "Free run operation can continually measure and output range information"
http://www.maxbotix.com/documents/MB1010_Datasheet.pdf
Tom

U.Sentenza · Post by **U.Sentenza** » Thu Sep 05, 2013 12:06 pm

Hello,
to reduce the glitch I used the following code to add in "Sonar_Update"

Code: Select all

        // --------------------------------------------------------------------------------------------------------------
        #define M_PI 3.1415926535f
        #define fCut 10  
        static uint16_t previousTimeSonar;
        uint16_t currentTimeSonar = micros();
        float dTimeSonar;

        dTimeSonar = (currentTimeSonar - previousTimeSonar) / 1000000.0;
        previousTimeSonar = currentTimeSonar;  
  
        float RC = 1 / (2 * M_PI * fCut);
        tempSonarAlt = last_tempSonarAlt + ((float)dTimeSonar / (RC + dTimeSonar)) * (tempSonarAlt - last_tempSonarAlt);
        // update state
        last_tempSonarAlt    = tempSonarAlt;
        // --------------------------------------------------------------------------------------------------------------

of course also be added to the line

Code: Select all

  volatile int16_t last_tempSonarAlt = 0;

in MultiWii.cpp

and the line

Code: Select all

  extern volatile int16_t last_tempSonarAlt;

in Multiwii.h

penpen77 · Post by **penpen77** » Tue Sep 17, 2013 5:01 pm

I don't use MWC anymore, so i don't have the code under my eyes, but from what i remember with each version of the code i've given here

1. Noisy /glitch
just increase LPF. Default use a 6/4 integrator from previous reading at the same frequency than baro (1/5 exec time of main loop i think). You can rise the update frequency and lower the part of last readings/new reading (like, 1 new for 9 from history). The code from getAltitude / Sonar_Update is very simple

HCSR4 is a very noisy sensor, cheap, but noisy.

2. acc_z usage
One of the version in this thread use the ACC_Z values (beta 2.0/2.1 i think, it was a the same period of "alt hold" research with acc_z). It's only few line of code located in "getAltitude" which use the pid integrator of baro
It was working correctly from what i remember. The only prob was on some frame/config where PID of althold for baro was too slopy/nervous for althold with sonar.

3.void__vector_9 previously defined
It's because your conf use an already defined ISR on designed PIN

Entropia · Post by **Entropia** » Fri Sep 20, 2013 9:46 pm

Hi,
I'm a desperate noob, trying to understanding the code and modifying, I'm not able to use my HC-SR04 sonar.
I have a Chinese MultiWii, similar to this one:

http://www.rctimer.com/product_761.html

it is recognized by setting

Code: Select all

#define GY_521          // Chinese 6  DOF with  MPU6050, LLC

in config.h.

I tried to integrate the code at page 4 (http://www.multiwii.com/forum/viewtopic.php?f=7&t=1033&start=30) modifying it to fit MultiWii_2.2.

I don't know why, the ISR interrupt function is never called, and I have no idea how to debug it.

In attachment you have the code.
I have connected the sonar like suggested in page 4...

Thank you very much for help!

Michele

penpen77 · Post by **penpen77** » Fri Sep 20, 2013 9:59 pm

i'm not sure the code will work with 328 based arduino

Entropia · Post by **Entropia** » Fri Sep 20, 2013 10:33 pm

penpen77 wrote:i'm not sure the code will work with 328 based arduino

Ahh ok, why do you think it?

Entropia · Post by **Entropia** » Fri Sep 20, 2013 11:19 pm

Ok I found that only PINs 2-3 can generate interrupts (http://arduino.cc/en/Main/ArduinoBoardProMini), is it possible to use one of the two? Just to try the sonar, even if in the end I will probably not have enough pins to fly...

rishanrdx · Post by **rishanrdx** » Mon Sep 23, 2013 1:56 pm

i have this http://www.rctimer.com/syssite/home/sho ... ig/761.jpg
and i need to connect SR04 sonar to this board
i have tried page 4 with multi wii 2.2 but didn't works
help help .............
how to connect pin s to se v2.0 board and code config for the 2.2
please help help .............

Entropia · Post by **Entropia** » Mon Sep 23, 2013 2:18 pm

Apparently it is not possible to do it, your microcontroller is ATMEGA328P and according to penpen77 this code will not work. I have made unsuccessfull attempts last week end, I have not enough knowledge of electronics to work it out by myself.
Now I'm spending some time reading the Arduino learning section (http://arduino.cc/en/Reference/HomePage) in particular the Interrupts section.

Do you know C/C++? If you know, we can try together to work it out...

rishanrdx · Post by **rishanrdx** » Mon Sep 23, 2013 4:30 pm

what about I2C port ? can i use SR04 with it

Entropia · Post by **Entropia** » Tue Sep 24, 2013 10:43 am

Look at page 2, there's a guy who bought a simple I2C shield...

U.Sentenza · Post by **U.Sentenza** » Tue Sep 24, 2013 12:49 pm

@ PENPEN77
hello,
I tried to make some changes to your code, try to see if it works ...

Please note I have not tried it!

U.Sentenza · Post by **U.Sentenza** » Tue Sep 24, 2013 3:42 pm

I forgot, the pins to be used are A2 and 13!

scanman · Post by **scanman** » Wed Oct 02, 2013 4:21 pm

I see many people are getting the problem shown below with the HC-SR04 sonar. The problem is that it only reads distance from 9 to 56 cm. After some experimenting with the basic "pure" sonar code in so many examples (no multi wii code) , I reproduced the problem on a standard Ardunio Uno board - the sonar only reads from 9 to 56 cm.
I then put the same sonar and same code (changed a few pins) onto a "arduino mega 256 " Crius AIOP purchased from here: http://www.rctimer.com/product_765.html

The problem went away - the sonar now reads up to 2M as expected.

So the problem is obviously that some boards cannot handle the Arduino "pulseIn" command correctly.

schnupperm wrote:Hi,

I'm using a HC-SR04 on a HK multiWii pro board and got the same problem as Sven described in this post viewtopic.php?f=7&t=1033&start=200#p36745. For some tests I used this sonar only
code

with the same result: readings between 9 cm and 56 cm.

scanman · Post by **scanman** » Thu Oct 03, 2013 6:39 am

actually , the problem arose as well on the "arduino mega 256 " Crius AIOP board today: the sonar would not read more than 50cm, so my theory is not complete: it turns out it is not a symptom of the pulseIn command or the specific board, but more a symptom of power - when i plug in other current draining sensors to the board (like LED's etc), it cannot supply enough power to the HC-SR04 sonar and the performance of the sonar drops, as soon as i unplug the other sesnors, the performance improves. This is running off USB, so when plugging in to a battery or LiPo, i am sure that enough current will be supplied.

So: to those of you who are getting the 50cm maximum range of the sonar, try plugging in a battery instead of only using the usb power and you might get better results!

Greg Covey · Post by **Greg Covey** » Mon Oct 07, 2013 12:05 am

scanman,

Is the sonar only used for landing lights in Multiwii r2.2? I do not see an option for the HC-SR04 sonar in config.h so is there external code for this?

Thank you.

/* Sonar */ // for visualization purpose currently - no control code behind
//#define SRF02 // use the Devantech SRF i2c sensors
//#define SRF08
//#define SRF10
//#define SRF23

/******************************* Landing lights *********************************/
/* Landing lights
Use an output pin to control landing lights.
They can be switched automatically when used in conjunction
with altitude data from a sonar unit. */
//#define LANDING_LIGHTS_DDR DDRC
//#define LANDING_LIGHTS_PORT PORTC
//#define LANDING_LIGHTS_BIT PORTC0
//#define LANDING_LIGHTS_INVERT

/* altitude above ground (in cm) as reported by sonar */
//#define LANDING_LIGHTS_AUTO_ALTITUDE 50

/* adopt the flasher pattern for landing light LEDs */
//#define LANDING_LIGHTS_ADOPT_LED_FLASHER_PATTERN

Johnnynunes · Post by **Johnnynunes** » Tue Oct 29, 2013 1:44 am

I'm a bit confused... Did you get it to work on Multiwii pro board with multiwii v2.2? If yes, witch code did you change?

Tks in advance

phybest · Post by **phybest** » Mon Jan 06, 2014 8:36 am

Entropia wrote:Look at page 2, there's a guy who bought a simple I2C shield...

Hi All, I also have a MWC SE 2.5 328P board, software version is MWC2.2. I found there has a UART interface on this board. I also found a sonar module called US-100. this sonar module support two work method, switch by jump pin. Can i use this sonar module to my SE 2.5 board use UART interface? and what should i do with the code? Thanks.

1.jpg: (22.91 KiB) Not downloaded yet

wareck · Post by **wareck** » Thu Jan 16, 2014 9:58 pm

I re-open this post.
I wan't to finalise this project.
I build a very cheap adaptator, wich works with an ATTIN85.
So now my sonar, works in I2C and no problems with interrupt or lazy timing.

But there is a problem in multiwii 2.3, I think loop for SRF08 was not merged and doesn't work anymore.
My chip on a multiwii 2.2 works (I have only sonar infos in debug for now)

Tomorow I'll put shematics and firmware for the attiny85

Olivier