Attemp to integrate sonar (ultrasonic sensor)

LuFa
Posts: 160
Joined: Fri Jan 27, 2012 7:56 pm

Re: Attemp to integrate sonar (ultrasonic sensor)

Post by LuFa »

penpen77 wrote:Sorry, i thought i joined the zip....

dependency:
TinyWire (attiny implementation of USI/TWI)
Arduino ATTiny (arduino core for attiny)

You may find "smartDigitalWrite" in code, it's just digitalWrite but without PWM check (size issue for me in attiny2313)

i2c address is arbitrary, you can choose what you want
switch MODE "define" to switch MODE behaviour

A rewrite in pure atmel C shouldn't be a problem and should save a lot of memory space

TinySonarI2C.zip


did you have also the code for Multiwii to juse your ATiny Sonar ?

copterrichie
Posts: 2261
Joined: Sat Feb 19, 2011 8:30 pm

Re: Attemp to integrate sonar (ultrasonic sensor)

Post by copterrichie »

FYI: here is the link for the TinyWire: http://arduino.cc/playground/Code/USIi2c

And the Link for the Arduino ATTINY: http://code.google.com/p/arduino-tiny/

LuFa
Posts: 160
Joined: Fri Jan 27, 2012 7:56 pm

Re: Attemp to integrate sonar (ultrasonic sensor)

Post by LuFa »

ok ,

Is the code for the Multiwii Flight Software also ready ?

i cant wait to test it :D

Vallekano
Posts: 25
Joined: Sun Dec 25, 2011 7:07 pm

Re: Attemp to integrate sonar (ultrasonic sensor)

Post by Vallekano »

I managed to run the HC-SR04 sonar with interruptions.

I'll try when I have time to integrate it into the code to work with the bario. But my knowledge of code MultiWii are very low. If anyone has more knowledge and is able to integrate ...

Code: Select all

// setup pins and variables
#define HC_SR04_echoPin 3 //  (digital 3)
#define HC_SR04_trigPin 7 //  (digital 7)

volatile unsigned long Sonar_starTime = 0;
volatile unsigned long Sonar_endTime = 0;

void Sonar_changeDetected()
{
  if (digitalRead(HC_SR04_echoPin) == HIGH) {
    Sonar_starTime = micros(); // Apunto el tiempo de inicio
  }
  else {
    Sonar_endTime = micros();
  } 
}


void Sonar_init()
{
  // Sonar init
   pinMode(HC_SR04_trigPin, OUTPUT);
   pinMode(HC_SR04_echoPin, INPUT);        // Set echo pin as input
   
   attachInterrupt(1, Sonar_changeDetected, CHANGE);
}

void Sonar_update()
{
  digitalWrite(HC_SR04_trigPin, LOW);      // Send 2ms LOW pulse to ensure we get a nice clean pulse
  delayMicroseconds(2);
  digitalWrite(HC_SR04_trigPin, HIGH); // send 10 microsecond pulse
  delayMicroseconds(10); // wait 10 microseconds before turning off
  digitalWrite(HC_SR04_trigPin, LOW); // stop sending the pulse
 
  Sonar_starTime = micros(); // Apunto el tiempo de inicio
}


unsigned long Sonar_read()
{
  return (Sonar_endTime - Sonar_starTime) / 58;
}






// Test
void setup()
{
  Serial.begin(9600);
  Sonar_init();
}

void loop()
{
  Serial.print(Sonar_read(), DEC);
  Serial.println(" cm");
 
  Sonar_update();
 
  delay(100);
}

penpen77
Posts: 73
Joined: Tue Jan 24, 2012 10:45 pm

Re: Attemp to integrate sonar (ultrasonic sensor)

Post by penpen77 »

Oula....

guys, i wrote this during my lunch break yesterday..... there isn't any real working code, it's just "proof of concept"

by the way, this should/might/may work by hacking Barometer handler.

And if i could understand WHY thoses f*****g i2c_*** function never work when i try using it
It works perfectly with Wire library, but when swapping, stop working.....

EDIT: working impl next post

Code: Select all

#if defined(HCSR04) // put it in BARO define section

#define HCSR04_ADDRESS 0x02 //my i2c address for SR04

void  Baro_init() {
  //rien
  Wire.begin(); 
}

void Baro_update() {
  Wire.requestFrom(HCSR04_ADDRESS,1);
  if(Wire.available())   BaroAlt = (uint8_t)Wire.receive()*100; //centimeter ???
 
  /* DONT WORK..... ffffuuuuuuu
  i2c_rep_start(HCSR04_ADDRESS/*+1*/); (//with or without 7bit, +/-1, register, voodoo, same results...)
  BaroAlt = (uint8_t)i2c_readNak()*100;    //centimeter ??? 
  */
}
#endif


barometer substitute on conf
border of my desk, 70cm above the ground so....
border of my desk, 70cm above the ground so....
Last edited by penpen77 on Tue Feb 07, 2012 7:28 pm, edited 1 time in total.

User avatar
EOSBandi
Posts: 802
Joined: Sun Jun 19, 2011 11:32 am
Location: Budapest, Hungary
Contact:

Re: Attemp to integrate sonar (ultrasonic sensor)

Post by EOSBandi »

wire lib does not handle repeated stop on i2c, multiwii code uses it, so it has it's own i2c implementation.

penpen77
Posts: 73
Joined: Tue Jan 24, 2012 10:45 pm

Re: Attemp to integrate sonar (ultrasonic sensor)

Post by penpen77 »

i know that, it's just that i can't achieve mw i2c sequence working...... the Wire part is just for debugging, check if data is here and correct. It's not intended to fly with that kind of code....

Like i said before, it's just "poc", if someone has a correct i2c request/reading sequence, i could write a real handler, independants of Barometer part (or having a switching behaviour).

penpen77
Posts: 73
Joined: Tue Jan 24, 2012 10:45 pm

Re: Attemp to integrate sonar (ultrasonic sensor)

Post by penpen77 »

need define PULLUPS.....

so

Code: Select all

// ************************************************************************************************************
// I2C HCSR04 UltraSonic range finder Wrapper (EXPERIMENTAL, USE WITH CAUTION, NOT TESTED, BLABLABLABLABLA)
// ************************************************************************************************************
#if defined(HCSR04)
#define HCSR04_ADDRESS 0x09 //your i2c wrapper address

void  Baro_init() {
}

void Baro_update() {
  TWBR = ((16000000L / I2C_SPEED) - 16) / 2;
  i2c_rep_start((HCSR04_ADDRESS<<1)+1); //read
  BaroAlt = i2c_readNak()*100;  //centimeter ?
}
#endif


in sensor file and add a define HCSR04 in config.h for BARO turned to 1

This override barometer data. Not tested yet....

penpen77
Posts: 73
Joined: Tue Jan 24, 2012 10:45 pm

Re: Attemp to integrate sonar (ultrasonic sensor)

Post by penpen77 »

Tests results: seems to work, so far so good.... but i thinks it needs a real dedicated handler. Hacking barometer impl' for ultrasonic rf makes the quad a bit too much '"floaty" with some kind of inertia that i was unable to cancel by only tweaking alt params.

LuFa
Posts: 160
Joined: Fri Jan 27, 2012 7:56 pm

Re: Attemp to integrate sonar (ultrasonic sensor)

Post by LuFa »

sounds greate :)

i have try to run your ATTinySonarI2C on a arduino mini pro , did you can chanche your code for it ??

Vallekano
Posts: 25
Joined: Sun Dec 25, 2011 7:07 pm

Re: Attemp to integrate sonar (ultrasonic sensor)

Post by Vallekano »

I managed to operate the HC-SR04 Ultrasonic Sonar, without use of any auxiliary device.

I integrate it into the MultiWii 1.9 code.

To make it work we need to use the D8 and D12 ports so we can not use AUX2.

Here is a sample image. The measured distance is seen in "debug3" and is measured in centimeters. Cycle time unaffected.

MultiWiiConf.png


Now we have to do something with this :lol: . I will try an autolanding...

Changes:

In "Serial.pde" replace:

Code: Select all

serialize16(0);                 // debug3

by

Code: Select all

serialize16(SonarAlt);         // debug3


In "MultiWii_1_9.pde" replace:

Code: Select all

static int32_t  BaroAlt;

by

Code: Select all

static int32_t  BaroAlt;
static int32_t  SonarAlt;


and replace

Code: Select all

 
if (BARO) Baro_update();

by

Code: Select all

if (BARO) Baro_update();
if (SONAR) Sonar_update();



In config.h Add

Code: Select all

/* HC-SR04 Ultrasonic Sonar */
#define HCSR04




In def.h add

Code: Select all

#if defined(HCSR04)
  #define SONAR 1
  #define BUZZERPIN_PINMODE          ;
  #define BUZZERPIN_ON               ;
  #define BUZZERPIN_OFF              ;
  #define POWERPIN_PINMODE           ;
  #define POWERPIN_ON                ;
  #define POWERPIN_OFF               ;
  #define HCSR04_EchoPin         8
  #define HCSR04_TriggerPin      12
#else
  #define SONAR 0
#endif


In Sensors.pde add

Code: Select all

// ************************************************************************************************************
// HC-SR04 Ultrasonic Sonar
// ************************************************************************************************************

#if defined(HCSR04)

volatile unsigned long HCSR04_starTime = 0;
volatile unsigned long HCSR04_echoTime = 0;
volatile unsigned int HCSR04_waiting_echo = 0;
unsigned int HCSR04_current_loops = 0;

// The cycle time is between 3000 and 6000 microseconds
// The recommend cycle period for sonar request should be no less than 50ms -> 50000 microseconds
// A reading every 18 loops (50000 / 3000 aprox)
unsigned int HCSR04_loops = 18;

void Sonar_init()
{
  // Pin change interrupt control register - enables interrupt vectors
  PCICR  |= (1<<PCIE0); // D8
 
  // Pin change mask registers decide which pins are enabled as triggers
  PCMSK0 = (1<<PCINT0); // pin 8
 
  pinMode(HCSR04_TriggerPin, OUTPUT);
   
  Sonar_update();
}

ISR(PCINT0_vect) {
    uint8_t pin = PINB;
    if (pin & 1<<PCINT0) {     //indicates if the bit 0 of the arduino port [B0-B7] is at a high state
      HCSR04_starTime = micros();
    }
    else {
      HCSR04_echoTime = micros() - HCSR04_starTime; // Echo time in microseconds
     
      if (HCSR04_echoTime <= 25000) {     // valid distance
        SonarAlt = HCSR04_echoTime / 58;
      }
      else
      {
      // No valid data
        SonarAlt = 9999;
      }
      HCSR04_waiting_echo = 0;
    }
}


void Sonar_update()
{
  HCSR04_current_loops++;
 
  if (HCSR04_waiting_echo == 0 && HCSR04_current_loops >= HCSR04_loops)
  {
    // Send 2ms LOW pulse to ensure we get a nice clean pulse
    digitalWrite(HCSR04_TriggerPin, LOW);     
    delayMicroseconds(2);
   
    // send 10 microsecond pulse
    digitalWrite(HCSR04_TriggerPin, HIGH);
    // wait 10 microseconds before turning off
    delayMicroseconds(10);
    // stop sending the pulse
    digitalWrite(HCSR04_TriggerPin, LOW);
   
    HCSR04_waiting_echo = 1;
    HCSR04_current_loops = 0;
  }
}
#endif


and change:

Code: Select all

  if (MAG) Mag_init();

by

Code: Select all

  if (MAG) Mag_init();
  if (SONAR) Sonar_init();

copterrichie
Posts: 2261
Joined: Sat Feb 19, 2011 8:30 pm

Re: Attemp to integrate sonar (ultrasonic sensor)

Post by copterrichie »

You know, this may just work. I had tried using the Ultrasonic library but I would experience random reboots. The way you have this coded, I don't see that happening. Going to give this a try.

Thanks

LuFa
Posts: 160
Joined: Fri Jan 27, 2012 7:56 pm

Re: Attemp to integrate sonar (ultrasonic sensor)

Post by LuFa »

have anyere a idear how to switch between SonarAlt and BaroAlt automaticly ?

sorry for my english :oops:

Vallekano
Posts: 25
Joined: Sun Dec 25, 2011 7:07 pm

Re: Attemp to integrate sonar (ultrasonic sensor)

Post by Vallekano »

LuFa wrote:have anyere a idear how to switch between SonarAlt and BaroAlt automaticly ?

sorry for my english :oops:


The problem is that Baro give us a absolute altitude (sea level) and de sonar give us a relative altitude (ground).

mahowik
Posts: 332
Joined: Sun Apr 10, 2011 6:26 pm

Re: Attemp to integrate sonar (ultrasonic sensor)

Post by mahowik »

LuFa wrote:have anyere a idear how to switch between SonarAlt and BaroAlt automaticly ?


I have used the following... i got the main idea from megapiratNG:

Code: Select all

#define MAX_SONAR_RANGE 400 
#define BARO_TO_SONAR MAX_SONAR_RANGE+150 // Due to low baro accuracy
#define SONAR_TO_BARO_FADE (MAX_SONAR_RANGE/3) // 33% of Max sonar range
#define SONAR_TO_BARO_FADE_FROM (MAX_SONAR_RANGE - SONAR_TO_BARO_FADE) //4m-33% = 2.67m start value to fading from sonar to baro

int32_t  currAlt;
  if(SONAR){
    sonarTempDistance = (sonarTempDistance - (sonarTempDistance >> 3)) + sonarDistance;
    sonarDistance = sonarTempDistance >> 3;
 
    if((BaroAlt - BaroAltOffset) < BARO_TO_SONAR){
      float scale = (sonarDistance - SONAR_TO_BARO_FADE_FROM) / SONAR_TO_BARO_FADE;
      scale = constrain(scale, 0, 1);
      currAlt = ((float)sonarDistance * (1.0 - scale)) + ((float)(BaroAlt - BaroAltOffset) * scale);
    }
    else{
      currAlt = (BaroAlt - BaroAltOffset);
    }
  }
  else{
    // no sonar altitude
    currAlt = (BaroAlt - BaroAltOffset);
  }


also sorry for BIG delay... i stopped the investigation of the current idea because as i have found out that my motors make a noise which "push" the echo signal from sonar and it not possible to test with altitude more than 50-80cm... so if someone has mega1280+DYP_ME007_sonar you can try to do something with that.
some notes and condition before use this:
1. u should have tuned alt-hold (i.e. based on baro only) before, because it uses the same alt estimator and pid regulator
2. BaroAltOffset remembered on gyro and acc calibration
3. alt-hold should work based on sonar data in range 0 - 2.67m,
then 2.67-4m for mix of sonar+baro,
and >4m => based on baro only
Attachments
MultiWii_1_9_a2.zip
(52.95 KiB) Downloaded 987 times

LuFa
Posts: 160
Joined: Fri Jan 27, 2012 7:56 pm

Re: Attemp to integrate sonar (ultrasonic sensor)

Post by LuFa »

Hi Mahowik ,

I have download your .zip folder ;)
But in your code are any parts commented out , should it work without ? or is need to put it in the code ?

works your code only with arduino mega or also with Mini Pro ?

Vallekano
Posts: 25
Joined: Sun Dec 25, 2011 7:07 pm

Re: Attemp to integrate sonar (ultrasonic sensor)

Post by Vallekano »

Well, another change and altitude is implemented with sonar:

In MultiWii_1.9 change

Code: Select all

static int32_t  EstAlt;             // in cm

by

Code: Select all

static int32_t  EstAlt;             // in cm
static int32_t  InitialAlt = 0;             // in cm



In IMU.pde change getEstimatedAltitude method

Code: Select all

void getEstimatedAltitude(){
  static uint8_t inited = 0;
  static int16_t AltErrorI = 0;
  static float AccScale  = 0.0f;
  static uint32_t deadLine = INIT_DELAY;
  int16_t AltError;
  int16_t InstAcc;
  int16_t Delta;
 
  if (currentTime < deadLine) return;
  deadLine = currentTime + UPDATE_INTERVAL;
  // Soft start

  if (!inited) {
    inited = 1;
    EstAlt = BaroAlt;
   
    // Apunto la altura de despegue
    InitialAlt = BaroAlt;
   
    EstVelocity = 0;
    AltErrorI = 0;
    AccScale = 100 * 9.80665f / acc_1G;
  }
 
  // Implementacion del sonar
  if(SONAR && (SonarAlt < 9999)) {
    EstAlt = InitialAlt + SonarAlt;
  }
  else {
     // Estimation Error
     AltError = BaroAlt - EstAlt;
     AltErrorI += AltError;
     AltErrorI=constrain(AltErrorI,-25000,+25000);
     // Gravity vector correction and projection to the local Z
     //InstAcc = (accADC[YAW] * (1 - acc_1G * InvSqrt(isq(accADC[ROLL]) + isq(accADC[PITCH]) + isq(accADC[YAW])))) * AccScale + (Ki) * AltErrorI;
     #if defined(TRUSTED_ACCZ)
      InstAcc = (accADC[YAW] * (1 - acc_1G * InvSqrt(isq(accADC[ROLL]) + isq(accADC[PITCH]) + isq(accADC[YAW])))) * AccScale +  AltErrorI / 1000;
     #else
      InstAcc = AltErrorI / 1000;
     #endif
    
     // Integrators
     Delta = InstAcc * dt + (Kp1 * dt) * AltError;
     EstAlt += (EstVelocity/5 + Delta) * (dt / 2) + (Kp2 * dt) * AltError;
     EstVelocity += Delta*10;
  }
}



When you start taking the absolute altitude (sea level). Is the altitude of takeoff.

If there is a valid measurement of sonar then the current altitude is the altitude of takeoff + sonar measurement

The problem I have is that will not work well if the ground is not level where we fly and we approach the ground at another point that is at another altitude as the takeoff.

LuFa
Posts: 160
Joined: Fri Jan 27, 2012 7:56 pm

Re: Attemp to integrate sonar (ultrasonic sensor)

Post by LuFa »

mahowik wrote:
LuFa wrote:have anyere a idear how to switch between SonarAlt and BaroAlt automaticly ?


I have used the following... i got the main idea from megapiratNG:

Code: Select all

#define MAX_SONAR_RANGE 400 
#define BARO_TO_SONAR MAX_SONAR_RANGE+150 // Due to low baro accuracy
#define SONAR_TO_BARO_FADE (MAX_SONAR_RANGE/3) // 33% of Max sonar range
#define SONAR_TO_BARO_FADE_FROM (MAX_SONAR_RANGE - SONAR_TO_BARO_FADE) //4m-33% = 2.67m start value to fading from sonar to baro

int32_t  currAlt;
  if(SONAR){
    sonarTempDistance = (sonarTempDistance - (sonarTempDistance >> 3)) + sonarDistance;
    sonarDistance = sonarTempDistance >> 3;
 
    if((BaroAlt - BaroAltOffset) < BARO_TO_SONAR){
      float scale = (sonarDistance - SONAR_TO_BARO_FADE_FROM) / SONAR_TO_BARO_FADE;
      scale = constrain(scale, 0, 1);
      currAlt = ((float)sonarDistance * (1.0 - scale)) + ((float)(BaroAlt - BaroAltOffset) * scale);
    }
    else{
      currAlt = (BaroAlt - BaroAltOffset);
    }
  }
  else{
    // no sonar altitude
    currAlt = (BaroAlt - BaroAltOffset);
  }


also sorry for BIG delay... i stopped the investigation of the current idea because as i have found out that my motors make a noise which "push" the echo signal from sonar and it not possible to test with altitude more than 50-80cm... so if someone has mega1280+DYP_ME007_sonar you can try to do something with that.
some notes and condition before use this:
1. u should have tuned alt-hold (i.e. based on baro only) before, because it uses the same alt estimator and pid regulator
2. BaroAltOffset remembered on gyro and acc calibration
3. alt-hold should work based on sonar data in range 0 - 2.67m,
then 2.67-4m for mix of sonar+baro,
and >4m => based on baro only


is it possible to juse D8 and D12 for the ultrasonic sensor ?
will be cool , bacause i have no mega :(

Vallekano
Posts: 25
Joined: Sun Dec 25, 2011 7:07 pm

Re: Attemp to integrate sonar (ultrasonic sensor)

Post by Vallekano »

LuFa wrote:is it possible to juse D8 and D12 for the ultrasonic sensor ?
will be cool , bacause i have no mega :(


Have you read my posts?

I use D8 and D12

LuFa
Posts: 160
Joined: Fri Jan 27, 2012 7:56 pm

Re: Attemp to integrate sonar (ultrasonic sensor)

Post by LuFa »

yes , but i have a DYP_ME007 ultrasonic sensor , and i think this one doesnt work with your code :?
if yes , than i will test your code :D

Vallekano
Posts: 25
Joined: Sun Dec 25, 2011 7:07 pm

Re: Attemp to integrate sonar (ultrasonic sensor)

Post by Vallekano »

LuFa wrote:yes , but i have a DYP_ME007 ultrasonic sensor , and i think this one doesnt work with your code :?
if yes , than i will test your code :D


I think it would work well, but in the code instead of dividing by 58, I think you'd have to divide by 118.

LuFa
Posts: 160
Joined: Fri Jan 27, 2012 7:56 pm

Re: Attemp to integrate sonar (ultrasonic sensor)

Post by LuFa »

Vallekano wrote:
I think it would work well, but in the code instead of dividing by 58, I think you'd have to divide by 118.



great ! :D
Is your newest update with switch between Baro and Sonar ? and with Baro Offset ?
i will test it tomorrow :)

LuFa
Posts: 160
Joined: Fri Jan 27, 2012 7:56 pm

Re: Attemp to integrate sonar (ultrasonic sensor)

Post by LuFa »

now i have test your code , Vallekano ;)

is it normal , that the SonarAlt Value does only refresh at the start ?
because when i power up the copter , is see the right Sonar distance , but it does not update the value on the GUI (degug3 window)
it shows only the first distance .

Next question is , should the Baro Alt be Zero because of the offset and Sonar ?

Steeze McQueen
Posts: 4
Joined: Fri Jan 27, 2012 10:09 pm

Re: Attemp to integrate sonar (ultrasonic sensor)

Post by Steeze McQueen »

Vallekano wrote:I use D8 and D12



Thank you for getting the HC-SR04 working! I think I'm being a bit thick today, plus this is my first arduino project so I'm still learning. My question is, what pins are you using off the HC-SR04? I see a Vcc, Trig, Echo, and GND. How do those correspond to pins D8 and D12 on the arduino? Also, and you may not know this, but I'm using a seeeduino mega board, not a pro mini. In that case, how do the pins D8 and D12 on a pro mini correspond to pins on the arduino mega?

Vallekano
Posts: 25
Joined: Sun Dec 25, 2011 7:07 pm

Re: Attemp to integrate sonar (ultrasonic sensor)

Post by Vallekano »

Steeze McQueen wrote:
Vallekano wrote:I use D8 and D12

Thank you for getting the HC-SR04 working! I think I'm being a bit thick today, plus this is my first arduino project so I'm still learning. My question is, what pins are you using off the HC-SR04? I see a Vcc, Trig, Echo, and GND. How do those correspond to pins D8 and D12 on the arduino? Also, and you may not know this, but I'm using a seeeduino mega board, not a pro mini. In that case, how do the pins D8 and D12 on a pro mini correspond to pins on the arduino mega?


Sorry for the delay.

VCC -> 5V
GND -> GND
ECHO -> D8
TRIGGER -> D12


if i have time this afternoon or tomorrow i'll record a video of MultiWiiconf

Steeze McQueen
Posts: 4
Joined: Fri Jan 27, 2012 10:09 pm

Re: Attemp to integrate sonar (ultrasonic sensor)

Post by Steeze McQueen »

Thanks Vallekano! I found a resource elsewhere that seems to indicate D8 and D12 are mapped to #32 and #37 on the Seeeduino, so hopefully I'll have some success when I try hooking this all up tonight.

mr_swell
Posts: 3
Joined: Thu Sep 22, 2011 11:48 am

Re: Attemp to integrate sonar (ultrasonic sensor)

Post by mr_swell »

@mahowik
Good news! Sonar driver works now! :)
Cycle time is ok also.

hi all,
maybe a stupid question, but where the code in multiwii must be inserted?
Jo

User avatar
Bledi
Posts: 187
Joined: Sat Sep 10, 2011 6:36 pm

Re: Attemp to integrate sonar (ultrasonic sensor)

Post by Bledi »

with the EOSBandi GPS I2C, a good solution is to add this sensor to the "extension" arduino board : this promini will do GPS and sonar to I2C conversion.

User avatar
guru_florida
Posts: 45
Joined: Sat Mar 26, 2011 4:51 am

Re: Attemp to integrate sonar (ultrasonic sensor)

Post by guru_florida »

I integrated the SRF08 I2C sonar sensor into the latest dev. Details here:
viewtopic.php?f=7&t=1282

I also integrated into alexmos's code that has baro/sonar sensor fusion but I havent tested it yet. Looks good in the GUI.

penpen77
Posts: 73
Joined: Tue Jan 24, 2012 10:45 pm

Re: Attemp to integrate sonar (ultrasonic sensor)

Post by penpen77 »

the code for SRF08 works perfectly with my i2c shield for HC-SR04, just need some tweaking with address

User avatar
guru_florida
Posts: 45
Joined: Sat Mar 26, 2011 4:51 am

Re: Attemp to integrate sonar (ultrasonic sensor)

Post by guru_florida »

penpen77 wrote:the code for SRF08 works perfectly with my i2c shield for HC-SR04, just need some tweaking with address


I dont get it, the SR04 is not an i2c sensor, how could the SRF08 code work for it?

My code will try to change the i2c address of the sensor so that multiple SRF08's can be put on the same bus. If there are other i2c sonar sensors out there I can take a look at how they do the address change, or simply have an option not to do the address moving but then only 1 sonar sensor can be present.

C

penpen77
Posts: 73
Joined: Tue Jan 24, 2012 10:45 pm

Re: Attemp to integrate sonar (ultrasonic sensor)

Post by penpen77 »

i2c shield for SR04 in the beginning of the thread, page 2, wrap echo/trigger blocking call request into i2c stream non-blocking event. With this "shield/wrapper", you have a decent working i2c sonar for ~5€

Blocking event like echo/trigger will degrade loop execution time, especially if there is several call at each execution (to have a median value, SR04 returned values aren't always accurate). And you don't have to sacrify 2 I/Os

so, with your code, just need to change i2c address and tweak srf08 i2c response/request handler.

User avatar
guru_florida
Posts: 45
Joined: Sat Mar 26, 2011 4:51 am

Re: Attemp to integrate sonar (ultrasonic sensor)

Post by guru_florida »

I see now, thanks. I took a look at the code. You will want to set the address of the sonar to 0xF0. This is where my code will look for the first sensor. The shield code ignores the register byte in i2c transactions so that is good in the sense my code tries to read register 2, the shield will return the sonar code anyway. The only issue I see is my code reads a 16bit value. So it will read from the sonar shield twice, You will want to reduce that read call to 8bit.

Note: The shield code specifies the i2c address as 0x09. This would be the 7bit address (i.e. not including r/w bit), So to my code this equals 0x12 address.

So, try this:
1. Do one of:
a. Recompile the shield code to change i2c address from 0x09 to Ox78, or (0x78 = 0xF0 in 8bit i2c addr format)
b. Change my code and redefine SRF08_SENSOR_FIRST parameter from 0xF0 to 0x12...my code will then look for sensors at 0x12, 0x014, 0x16, etc.
1. Uncomment the SONAR_MULTICAST_PING define.
2. Change i2c_readReg16 to i2creadReg8 in "case 3" within the sonar's state machine code. There are actually 2 locations, but with SONAR_MULTICAST_PING disabled only the bottom of the two matter. The full line is:
srf08_ctx.range[srf08_ctx.current] = i2c_readReg16(SRF08_SENSOR_FIRST+(srf08_ctx.current<<1), SRF08_ECHO_RANGE);
change to:
srf08_ctx.range[srf08_ctx.current] = i2c_readReg8(SRF08_SENSOR_FIRST+(srf08_ctx.current<<1), SRF08_ECHO_RANGE);



penpen77 wrote:i2c shield for SR04 in the beginning of the thread, page 2, wrap echo/trigger blocking call request into i2c stream non-blocking event. With this "shield/wrapper", you have a decent working i2c sonar for ~5€

Blocking event like echo/trigger will degrade loop execution time, especially if there is several call at each execution (to have a median value, SR04 returned values aren't always accurate). And you don't have to sacrify 2 I/Os

so, with your code, just need to change i2c address and tweak srf08 i2c response/request handler.

User avatar
Bledi
Posts: 187
Joined: Sat Sep 10, 2011 6:36 pm

Re: Attemp to integrate sonar (ultrasonic sensor)

Post by Bledi »

I just order an SR04. I ll try to use your works to add it to the arduino use for I2C GPS (EOSBandi)

haley0918
Posts: 28
Joined: Tue Mar 06, 2012 9:09 am

Re: Attemp to integrate sonar (ultrasonic sensor)

Post by haley0918 »

Bledi wrote:I just order an SR04. I ll try to use your works to add it to the arduino use for I2C GPS (EOSBandi)


I'm interested to include the SR04 into I2C_GPS_NAV too, possibly tying multiple SR04 together under one I2C address
Otherwise the arduino is kind of a waste for GPS only

Tifani
Posts: 63
Joined: Sun Nov 06, 2011 5:15 pm

Re: Attemp to integrate sonar (ultrasonic sensor)

Post by Tifani »

Hi !
Why not Ultrasonic Range Finder - Maxbotix LV-EZ1 ???
(I have one)
Regards
Tom

User avatar
guru_florida
Posts: 45
Joined: Sat Mar 26, 2011 4:51 am

Re: Attemp to integrate sonar (ultrasonic sensor)

Post by guru_florida »

Tifani wrote:Hi !
Why not Ultrasonic Range Finder - Maxbotix LV-EZ1 ???
Tom


It could work. The A6 and A7 input line is unused.

Twister
Posts: 2
Joined: Sat Feb 18, 2012 6:03 pm

Re: Attemp to integrate sonar (ultrasonic sensor)

Post by Twister »

Is the DYP-ME007 working on a flyduino mega?

dongfang
Posts: 5
Joined: Tue Mar 13, 2012 4:33 pm

Re: Attemp to integrate sonar (ultrasonic sensor)

Post by dongfang »

Hi All,

I think Vallekano's code do not work in my board(Arduino Nano) as it miss one "pinMode(HCSR04_EchoPin, INPUT);" command.

And I've merged Vallekano's code and mahowik's code together, so:

1. It still use D8 for TX pin, and D12 for RX pin
2. alt-hold data based on sonar data in range 0 - 200cm; 200cm-500cm for mix of sonar+baro; above 500cm:based on baro only
3. EstAlt will still use multiwii 1.9's existing code when sonar do not have valid data, (though I don't understand the Algorithm, and how to set Kp1)
4. I removed some unnecessary code


Till now, below code works for me very well: (please use Vallekano's patch first, and change the code below)

Sensors.pde:

Code: Select all

/////////////////////////////////////////
// ************************************************************************************************************
// HC-SR04 Ultrasonic Sonar
// ************************************************************************************************************

#if defined(HCSR04)

volatile unsigned long HCSR04_starTime = 0;
volatile unsigned long HCSR04_echoTime = 0;
volatile static int32_t  tempSonarAlt=0;

void Sonar_init()
{
  PCICR  |= (1<<0); // D8
  PCMSK0 = (1<<0); // pin 8
  pinMode(HCSR04_EchoPin, INPUT);
  pinMode(HCSR04_TriggerPin, OUTPUT);
  Sonar_update();
}

ISR(PCINT0_vect) {
  uint8_t pin = PINB;
  if (pin & 1<<PCINT0) {
    HCSR04_starTime = micros();
  }
  else {
    HCSR04_echoTime = micros() - HCSR04_starTime;
    if (HCSR04_echoTime <= 25000)
      tempSonarAlt = HCSR04_echoTime / 58;
    else
      tempSonarAlt = 9999;
  }
}


void Sonar_update()
{
    SonarAlt=tempSonarAlt;
    static int32_t sonarTempDistance=0;
    sonarTempDistance = (sonarTempDistance - (sonarTempDistance >> 3)) + SonarAlt;
    SonarAlt= sonarTempDistance >> 3;
   
    digitalWrite(HCSR04_TriggerPin, LOW);     
    delayMicroseconds(2);
    digitalWrite(HCSR04_TriggerPin, HIGH);
    delayMicroseconds(10);
    digitalWrite(HCSR04_TriggerPin, LOW);

}

#endif


IMU.pde

Code: Select all

void getEstimatedAltitude(){
  static uint8_t inited = 0;
  static int16_t AltErrorI = 0;
  static float AccScale  = 0.0f;
  static uint32_t deadLine = INIT_DELAY;
  int16_t AltError;
  int16_t InstAcc;
  int16_t Delta;

  if (currentTime < deadLine) return;
  deadLine = currentTime + UPDATE_INTERVAL;
  // Soft start

  if (!inited) {
    inited = 1;
    EstAlt = BaroAlt;

    // Apunto la altura de despegue
    InitialAlt = BaroAlt;
    if(SONAR && (SonarAlt < 9999)){
      InitialAlt-=SonarAlt;
    }

    EstVelocity = 0;
    AltErrorI = 0;
    AccScale = 100 * 9.80665f / acc_1G;
  }

  // Implementacion del sonar
  if(SONAR && (SonarAlt < 9999)) {
    BaroAlt=Fang_getAdjustedBaroAltBySONAR(BaroAlt,SonarAlt,InitialAlt) ;
  }

  // Estimation Error
  AltError = BaroAlt - EstAlt;
  AltErrorI += AltError;
  AltErrorI=constrain(AltErrorI,-25000,+25000);
  // Gravity vector correction and projection to the local Z
  //InstAcc = (accADC[YAW] * (1 - acc_1G * InvSqrt(isq(accADC[ROLL]) + isq(accADC[PITCH]) + isq(accADC[YAW])))) * AccScale + (Ki) * AltErrorI;
#if defined(TRUSTED_ACCZ)
  InstAcc = (accADC[YAW] * (1 - acc_1G * InvSqrt(isq(accADC[ROLL]) + isq(accADC[PITCH]) + isq(accADC[YAW])))) * AccScale +  AltErrorI / 1000;
#else
  InstAcc = AltErrorI / 1000;
#endif

  // Integrators
  Delta = InstAcc * dt + (Kp1 * dt) * AltError;
  if(SONAR && (SonarAlt < 9999)){
    EstAlt=BaroAlt;
  }
  else{
    EstAlt += (EstVelocity/5 + Delta) * (dt / 2) + (Kp2 * dt) * AltError;
  }
  EstVelocity += Delta*10;
}

int Fang_getAdjustedBaroAltBySONAR(int BaroAlt, int SonarAlt,int InitialBarAlt)
{
#define SONAR_FADE_START_RANGE 200
#define SONAR_FADE_END_RANGE 500

  if(SonarAlt<SONAR_FADE_START_RANGE)
    return SonarAlt+InitialBarAlt;
  else if (SonarAlt>=SONAR_FADE_END_RANGE)
    return BaroAlt;
  else{
    float mixlevel=(SonarAlt-SONAR_FADE_START_RANGE)*1.0/(SONAR_FADE_END_RANGE-SONAR_FADE_START_RANGE);
    return (SonarAlt+InitialBarAlt)*(1-mixlevel)+BaroAlt*mixlevel;
  }
}
Last edited by dongfang on Wed Mar 14, 2012 12:31 am, edited 1 time in total.

dongfang
Posts: 5
Joined: Tue Mar 13, 2012 4:33 pm

Re: Attemp to integrate sonar (ultrasonic sensor)

Post by dongfang »

Another things is, my sonar(called SRF-06) has the same sample code to the DYP-ME007, and I can use Vallekano's HCSR04 code in it.

That's means SRF-06, DYP-ME007 and HCSR04 are mostly the same to developer (maybe they are the same).

Vallekano
Posts: 25
Joined: Sun Dec 25, 2011 7:07 pm

Re: Attemp to integrate sonar (ultrasonic sensor)

Post by Vallekano »

dongfang wrote:Hi All,

I think Vallekano's code do not work in my board(Arduino Nano) as it miss one "pinMode(HCSR04_EchoPin, INPUT);" command.

And I've merged Vallekano's code and mahowik's code together, so:

1. It still use D8 for TX pin, and D12 for RX pin
2. alt-hold data based on sonar data in range 0 - 200cm; 200cm-500cm for mix of sonar+baro; above 500cm:based on baro only
3. EstAlt will still use multiwii 1.9's existing code when sonar do not have valid data, (though I don't understand the Algorithm, and how to set Kp1)
4. I removed some unnecessary code


Till now, below code works for me very well: (please use Vallekano's patch first, and change the code below)

Sensors.pde:

Code: Select all

/////////////////////////////////////////
// ************************************************************************************************************
// HC-SR04 Ultrasonic Sonar
// ************************************************************************************************************

#if defined(HCSR04)

volatile unsigned long HCSR04_starTime = 0;
volatile unsigned long HCSR04_echoTime = 0;
volatile static int32_t  tempSonarAlt=0;

void Sonar_init()
{
  PCICR  |= (1<<0); // D8
  PCMSK0 = (1<<0); // pin 8
  pinMode(HCSR04_EchoPin, INPUT);
  pinMode(HCSR04_TriggerPin, OUTPUT);
  Sonar_update();
}

ISR(PCINT0_vect) {
  uint8_t pin = PINB;
  if (pin & 1<<PCINT0) {
    HCSR04_starTime = micros();
  }
  else {
    HCSR04_echoTime = micros() - HCSR04_starTime;
    if (HCSR04_echoTime <= 25000)
      tempSonarAlt = HCSR04_echoTime / 58;
    else
      tempSonarAlt = 9999;
  }
}


void Sonar_update()
{
    SonarAlt=tempSonarAlt;
    static int32_t sonarTempDistance=0;
    sonarTempDistance = (sonarTempDistance - (sonarTempDistance >> 3)) + SonarAlt;
    SonarAlt= sonarTempDistance >> 3;
   
    digitalWrite(HCSR04_TriggerPin, LOW);     
    delayMicroseconds(2);
    digitalWrite(HCSR04_TriggerPin, HIGH);
    delayMicroseconds(10);
    digitalWrite(HCSR04_TriggerPin, LOW);

}

#endif


IMU.pde

Code: Select all

void getEstimatedAltitude(){
  static uint8_t inited = 0;
  static int16_t AltErrorI = 0;
  static float AccScale  = 0.0f;
  static uint32_t deadLine = INIT_DELAY;
  int16_t AltError;
  int16_t InstAcc;
  int16_t Delta;

  if (currentTime < deadLine) return;
  deadLine = currentTime + UPDATE_INTERVAL;
  // Soft start

  if (!inited) {
    inited = 1;
    EstAlt = BaroAlt;

    // Apunto la altura de despegue
    InitialAlt = BaroAlt;
    if(SONAR && (SonarAlt < 9999)){
      InitialAlt-=SonarAlt;
    }

    EstVelocity = 0;
    AltErrorI = 0;
    AccScale = 100 * 9.80665f / acc_1G;
  }

  // Implementacion del sonar
  if(SONAR && (SonarAlt < 9999)) {
    oldBaroAlt=BaroAlt;
    BaroAlt=Fang_getAdjustedBaroAltBySONAR(BaroAlt,SonarAlt,InitialAlt) ;
  }

  // Estimation Error
  AltError = BaroAlt - EstAlt;
  AltErrorI += AltError;
  AltErrorI=constrain(AltErrorI,-25000,+25000);
  // Gravity vector correction and projection to the local Z
  //InstAcc = (accADC[YAW] * (1 - acc_1G * InvSqrt(isq(accADC[ROLL]) + isq(accADC[PITCH]) + isq(accADC[YAW])))) * AccScale + (Ki) * AltErrorI;
#if defined(TRUSTED_ACCZ)
  InstAcc = (accADC[YAW] * (1 - acc_1G * InvSqrt(isq(accADC[ROLL]) + isq(accADC[PITCH]) + isq(accADC[YAW])))) * AccScale +  AltErrorI / 1000;
#else
  InstAcc = AltErrorI / 1000;
#endif

  // Integrators
  Delta = InstAcc * dt + (Kp1 * dt) * AltError;
  if(SONAR && (SonarAlt < 9999)){
    EstAlt=BaroAlt;
  }
  else{
    EstAlt += (EstVelocity/5 + Delta) * (dt / 2) + (Kp2 * dt) * AltError;
  }
  EstVelocity += Delta*10;
}

int Fang_getAdjustedBaroAltBySONAR(int BaroAlt, int SonarAlt,int InitialBarAlt)
{
#define SONAR_FADE_START_RANGE 200
#define SONAR_FADE_END_RANGE 500

  if(SonarAlt<SONAR_FADE_START_RANGE)
    return SonarAlt+InitialBarAlt;
  else if (SonarAlt>=SONAR_FADE_END_RANGE)
    return BaroAlt;
  else{
    float mixlevel=(SonarAlt-SONAR_FADE_START_RANGE)*1.0/(SONAR_FADE_END_RANGE-SONAR_FADE_START_RANGE);
    return (SonarAlt+InitialBarAlt)*(1-mixlevel)+BaroAlt*mixlevel;
  }
}


I have forgotten it a bit but will try your updates this week.

dongfang
Posts: 5
Joined: Tue Mar 13, 2012 4:33 pm

Re: Attemp to integrate sonar (ultrasonic sensor)

Post by dongfang »

Well, I found the same issue that mahowik met: the motor noise is too big, and Sonar return wrong distance data to the board. I've tried to isolate the noise but failed.

It's sonar's issue...

adam
Posts: 15
Joined: Sun Oct 02, 2011 4:26 pm
Location: Taiwan

Re: Attemp to integrate sonar (ultrasonic sensor)

Post by adam »

Vallekano wrote:
Steeze McQueen wrote:
Vallekano wrote:I use D8 and D12

Thank you for getting the HC-SR04 working! I think I'm being a bit thick today, plus this is my first arduino project so I'm still learning. My question is, what pins are you using off the HC-SR04? I see a Vcc, Trig, Echo, and GND. How do those correspond to pins D8 and D12 on the arduino? Also, and you may not know this, but I'm using a seeeduino mega board, not a pro mini. In that case, how do the pins D8 and D12 on a pro mini correspond to pins on the arduino mega?


Sorry for the delay.

VCC -> 5V
GND -> GND
ECHO -> D8
TRIGGER -> D12


if i have time this afternoon or tomorrow i'll record a video of MultiWiiconf


Hi Vallekano ,

I am ready to join HC-SR04 group now, But the "D12" make me confused!
The "D12" provide power to wii sensor like picture below, How can I do, Thank You.

Image

Vallekano
Posts: 25
Joined: Sun Dec 25, 2011 7:07 pm

Re: Attemp to integrate sonar (ultrasonic sensor)

Post by Vallekano »

I use an Arduino ONE that has an output of 3.3V. I think the Arduino Pro Mini (5V version) also has an output of 3.3V that can be used instead of pin 12.

If you do not have that output you will have to use a LM117 to pass the 5V to 3.3V, as illustrated in the diagrams.

dr.tom
Posts: 141
Joined: Fri Mar 30, 2012 4:46 pm
Location: Croatia
Contact:

Re: Attemp to integrate sonar (ultrasonic sensor)

Post by dr.tom »

Did someone try the HC-SR04 sonar @ 2.0 code?

adam
Posts: 15
Joined: Sun Oct 02, 2011 4:26 pm
Location: Taiwan

Re: Attemp to integrate sonar (ultrasonic sensor)

Post by adam »

Vallekano wrote:I use an Arduino ONE that has an output of 3.3V. I think the Arduino Pro Mini (5V version) also has an output of 3.3V that can be used instead of pin 12.

If you do not have that output you will have to use a LM117 to pass the 5V to 3.3V, as illustrated in the diagrams.


Hi Vallekano ,

Thanks for your help.

I follow your steps, Now can get altitude reading from DEBUG3, How can I get altitude hold ? Next for autolanding...

My QuadX , wmp + HCSR04, No Baro.

User avatar
kos
Posts: 286
Joined: Thu Feb 16, 2012 4:51 am
Location: Fr

Re: Attemp to integrate sonar (ultrasonic sensor)

Post by kos »

copterrichie wrote:This is great news and one step closer to auto landing in my opinion.


thing that is still required is a 'switch' to be activated when all landing gears touch the ground .. i just realized there is two unused inputs (1) on the nunchuck that could be used for that purpose.



1 : https://raw.github.com/todbot/wiichuck_ ... ck_funcs.h

Code: Select all

//  groundContact()
static int nunchuck_zbutton() {
      return ((nunchuck_buf[5] >> 0) & 1) ? false : true;
}

// crashContact()
static int nunchuck_cbutton() {
      return ((nunchuck_buf[5] >> 1) & 1) ? false : true; 
}

dr.tom
Posts: 141
Joined: Fri Mar 30, 2012 4:46 pm
Location: Croatia
Contact:

Re: Attemp to integrate sonar (ultrasonic sensor)

Post by dr.tom »

I think the better way to detect
-landing : absence of gyro & acc variations (due to model firmly standing od the ground)
-crash : high G readings on acc & gyro followed by absence of acc and gyro variations

seems to me like DJI has somethign like that, in case of a crash it stops motors from running,
without switches, because if model falls sideways and breaks, no switch on landing legs woud activate either

and by managing it in the code, the system is 'cleaner', no switches & wires going down the frame and landing gears...

copterrichie
Posts: 2261
Joined: Sat Feb 19, 2011 8:30 pm

Re: Attemp to integrate sonar (ultrasonic sensor)

Post by copterrichie »

kos wrote: thing that is still required is a 'switch' to be activated when all landing gears touch the ground .. i just realized there is two unused inputs (1) on the nunchuck that could be used for that purpose.



1 : https://raw.github.com/todbot/wiichuck_ ... ck_funcs.h

Code: Select all

//  groundContact()
static int nunchuck_zbutton() {
      return ((nunchuck_buf[5] >> 0) & 1) ? false : true;
}

// crashContact()
static int nunchuck_cbutton() {
      return ((nunchuck_buf[5] >> 1) & 1) ? false : true; 
}


One idea would be to read the Z Axis of the Accelerometer, it should return to 1G on touchdown.

User avatar
kos
Posts: 286
Joined: Thu Feb 16, 2012 4:51 am
Location: Fr

Re: Attemp to integrate sonar (ultrasonic sensor)

Post by kos »

dr.tom wrote:and by managing it in the code, the system is 'cleaner', no switches & wires going down the frame and landing gears...



there are still some cases where a mechanical switch is preferable ?

- landing on a not so flat ground , Z Axis will not return 1g
- crashed against a wall : as the copter fall down after hitting the wall , acc and gyro variations can be read from sensors

also maybe by reading the throttle input we can make autolanding a little more clever :

Code: Select all

if (AUTOLANDING && ACC && ( SONAR || BARO)){
  if (autoLanding && !failSafe){
    if (! groundContact){ 
      Alt =- 0.10;
      grounded = 0;
   }else{
     GROUND_LED_PIN_ON;
     if (!grounded) {
         grounded =1;
     }
     if ( rcData[THROTTLE]<MINCHECK){
       grounded++;
     if (grounded>20){
       armed=0;
       autoLanding=0;
    }
}

}

Post Reply