Attemp to integrate sonar (ultrasonic sensor)

Gaijin · Post by **Gaijin** » Fri Jul 06, 2012 6:26 am

Just pIcked up a nice ready made AT328 based I2C module, would it be difficult to modify the ATTiny code to run on it?

I have an HC-SRF04 kicking about, between them it could make for a very simple to build and low cost I2C sonar sensor, any thoughts

http://www.rctimer.com/index.php?gOo=goods_details.dwt&goodsid=762&productname=

xpix · Post by **xpix** » Tue Jul 31, 2012 7:51 am

wilco1967 wrote:With the TinyGPS route, the Sonar is working,, and I used the basis of Olaf Baro code (the getEstimatedAltitude part that goes in IMU) to get altitude hold....

I'm running a normal 328P on the multiwii, and a Attiny4313 to connect the HCSR04 (No GPS on this copter). I've temporarily connected all to my quad, and it is holding altitude pretty good.

Hello Wilco, i put ur patch in the new MW 2.1 in my IMU Module. The compiler dont like 'SonarAlt' ... i renamed in ur Code to 'sonarAlt'. What is happend with 'lastSonarAlt' in ur Code, u dont use it (everytime on 0) or u refresh this magic in ur Code?

My Solution:

Code: Select all

  #ifdef SONAR
    if (sonarAlt < 500){
      lastSonarAlt = sonarAlt; // save Alt for later
      temp32 = 100 - sonarAlt;
    } else {
      temp32 = 0;  // if no valid result from SONAR -> no P-Value!
    }
  #else
    temp32 = AltHold - EstAlt;
    if (abs(temp32) < 10 && abs(BaroPID) < 10) BaroPID = 0;  //remove small differences to reduce noise near zero position
  #endif

in IMU, is this so correct?

dr.tom · Post by **dr.tom** » Tue Jul 31, 2012 9:33 am

Gaijin wrote:Just pIcked up a nice ready made AT328 based I2C module, would it be difficult to modify the ATTiny code to run on it?

I have an HC-SRF04 kicking about, between them it could make for a very simple to build and low cost I2C sonar sensor, any thoughts

http://www.rctimer.com/index.php?gOo=goods_details.dwt&goodsid=762&productname=

I like the idea, I've build an attiny+sonar just for curiosity, but MWC code didn't have real use for it.
but some day when it will use it for althold, this mod with rctimer 328p would be plug&play,
easier to get, connect and program and cleaner look to integrate to other models...

+1

scanman · Post by **scanman** » Wed Aug 22, 2012 10:25 am

Hello, i have read through all the multiwii posts on sonar and i still dont really know where to start, what sonar to buy (they seem to range from $60 to $6), what code i need to modify , and where i need to wire up the sonar. would someone be so kind as to post a few simple steps to getting started?
Note: i have got the i2C nav board from rc timer wired up to a GPS using EOSBandi's code , as discussed at length in the GPS threads, so i am wondering how to connect the sonar? Do I need another i2c board? What code do i flash onto that board? do I wire it up to the same I2C port as the GPS i2C board?
Any tips would be useful before i get started!

I2C nav board link: http://www.rctimer.com/index.php?gOo=go ... oductname=

penpen77 · Post by **penpen77** » Sat Sep 01, 2012 12:13 pm

i'm using this code for my sr04 sonar/baro alt mode, juste mixing both and altering "EstAlt" as in megapirate/arducopter. No tilt correction or advanced stuff, but works fine, alt hold logic isn't altered neither code for sonar device. With a good tuned Alt hold pids value, it's working perfectly.

in MultiWii_2_1.ino

Code: Select all

//add in global decl
static int32_t BaroHome;
static int8_t BaroHome_set=0;

in IMU.ino

Code: Select all

//replace "EstAlt = BaroHigh*10/(BARO_TAB_SIZE/2);" by 

#if SONAR
  int32_t tempBaro = BaroHigh*10/(BARO_TAB_SIZE/2);
  
  if(!f.ARMED && f.BARO_MODE) { //init offset
    BaroHome_set = 1;
    BaroHome = tempBaro;
  }
  
  if(f.ARMED && BaroHome_set ==0) { // if forgotten
    BaroHome_set = 1;
    BaroHome = tempBaro;
  }

  //mix baro/sonar    
  if(SonarAlt<SONAR_SONARFULL) {
    EstAlt = BaroHome+sonarAlt;
  }
  else if(SonarAlt>=SONAR_SONARFULL && SonarAlt <SONAR_BAROFULL) {
    float fade = (float)(SONAR_BAROFULL-sonarAlt)/(SONAR_BAROFULL-SONAR_SONARFULL);
    fade = constrain(fade, 0.0, 1.0);
    EstAlt = (BaroHome+sonarAlt)*fade + tempBaro*(1-fade);
  } else {
    EstAlt = tempBaro;
  }
#else
    EstAlt = BaroHigh*10/(BARO_TAB_SIZE/2);
#endif

and in config.h

Code: Select all

     //mixing values between sonar and baro
     #define SONAR_SONARFULL 150 //full sonar reading before this point (cm)
     #define SONAR_BAROFULL 300 //full baro reading after this point (cm)

baro offset is done by activating baro mode in non armed mode or at the first arming action.

crazyal · Post by **crazyal** » Tue Sep 04, 2012 11:32 pm

If anybody is interested in using the HC-sr04 with the i2c-gps-nav, I have added some code, so the i2c-gps reads the sonar and the data can be read via the I2C bus.
It basically works the same as the tyiny-gps with sonar.
The code is on github: http://github.com/luggi
@penpen77 what pid values are you using ?

Magnetron · Post by **Magnetron** » Wed Sep 05, 2012 1:32 am

Is possible to integrate dyp-me007?

penpen77 · Post by **penpen77** » Wed Sep 05, 2012 10:57 am

@crazyal

1.2/0.0/14, i've noticed playing more with D param has better effect than playing with P,I like other mode.

@magnetron

dyp-me007 works like sr04

Magnetron · Post by **Magnetron** » Wed Sep 05, 2012 6:11 pm

I have one dyp-me007v2 and a mega2560. What pin I must use to connect to?

penpen77 · Post by **penpen77** » Mon Sep 17, 2012 11:19 am

updated code for new alt hold code (for MS561101BA)

works fine (on pos hold and at low speed), for better result, perform your althold pid tuning around 50% of sonar/baro fusion altitude.
at "high" speed, the baro-only EstAlt could increase if not wind-protected, and sonar/baro fusion could produce wrong alt reading, so be carefull...

Code: Select all

void getEstimatedAltitude(){
  static uint32_t deadLine = INIT_DELAY;

  static int16_t baroHistTab[BARO_TAB_SIZE];
  static int8_t baroHistIdx;
  static int32_t baroHigh;
 

  if (abs(currentTime - deadLine) < UPDATE_INTERVAL) return;
  uint16_t dTime = currentTime - deadLine;
  deadLine = currentTime;
 

  //**** Alt. Set Point stabilization PID ****
  baroHistTab[baroHistIdx] = BaroAlt/10;
  baroHigh += baroHistTab[baroHistIdx];
  baroHigh -= baroHistTab[(baroHistIdx + 1)%BARO_TAB_SIZE];
 
  baroHistIdx++;
  if (baroHistIdx == BARO_TAB_SIZE) baroHistIdx = 0;

  
#if SONAR
  int32_t tempBaro = EstAlt*0.6f + (baroHigh*10.0f/(BARO_TAB_SIZE - 1))*0.4f;
  
  if(!f.ARMED && f.BARO_MODE) { //init offset
    BaroHome_set = 1;
    BaroHome = tempBaro;
  }
  
  if(f.ARMED && BaroHome_set ==0) { // if forgotten
    BaroHome_set = 1;
    BaroHome = tempBaro;
  }

  //mix baro/sonar    
  if(SonarAlt<SONAR_SONARFULL) {
    EstAlt = BaroHome+SonarAlt;
  }
  else if(SonarAlt>=SONAR_SONARFULL && SonarAlt <SONAR_BAROFULL) {
    float fade = (float)(SONAR_BAROFULL-SonarAlt)/(SONAR_BAROFULL-SONAR_SONARFULL);
    fade = constrain(fade, 0.0, 1.0);
    EstAlt = (BaroHome+SonarAlt)*fade + tempBaro*(1-fade);
  } else {
    EstAlt = tempBaro;
  }
#else

    EstAlt = EstAlt*0.6f + (baroHigh*10.0f/(BARO_TAB_SIZE - 1))*0.4f;
    
#endif
  
  //P
  int16_t error = constrain(AltHold - EstAlt, -300, 300);
  error = applyDeadband16(error, 10); //remove small P parametr to reduce noise near zero position
  BaroPID = constrain((conf.P8[PIDALT] * error / 100), -150, +150);
 
  //I
  errorAltitudeI += error * conf.I8[PIDALT]/50;
  errorAltitudeI = constrain(errorAltitudeI,-30000,30000);
  BaroPID += (errorAltitudeI / 500); //I in range +/-60
 
 
  // projection of ACC vector to global Z, with 1G subtructed
  // Math: accZ = A * G / |G| - 1G
  float invG = InvSqrt(isq(EstG.V.X) + isq(EstG.V.Y) + isq(EstG.V.Z));
  int16_t accZ = (accLPFVel[ROLL] * EstG.V.X + accLPFVel[PITCH] * EstG.V.Y + accLPFVel[YAW] * EstG.V.Z) * invG - acc_1G;
  //int16_t accZ = (accLPFVel[ROLL] * EstG.V.X + accLPFVel[PITCH] * EstG.V.Y + accLPFVel[YAW] * EstG.V.Z) * invG - 1/invG;
  accZ = applyDeadband16(accZ, ACC_Z_DEADBAND);
  
  static float vel = 0.0f;
  static float accVelScale = 9.80665f / acc_1G / 10000.0f;
  
  // Integrator - velocity, cm/sec
  vel+= accZ * accVelScale * dTime;
  
  static int32_t lastBaroAlt = EstAlt;
  float baroVel = (EstAlt - lastBaroAlt) / (dTime/1000000.0f);
  baroVel = constrain(baroVel, -300, 300); // constrain baro velocity +/- 300cm/s
  baroVel = applyDeadbandFloat(baroVel, 10); // to reduce noise near zero  
  lastBaroAlt = EstAlt;
  
  // apply Complimentary Filter to keep the calculated velocity based on baro velocity (i.e. near real velocity). 
  // By using CF it's possible to correct the drift of integrated accZ (velocity) without loosing the phase, i.e without delay
  vel = vel * 0.985f + baroVel * 0.015f;
  //vel = constrain(vel, -300, 300); // constrain velocity +/- 300cm/s 
  
  //D
  BaroPID -= constrain(conf.D8[PIDALT] * applyDeadbandFloat(vel, 5) / 20, -150, 150);
}

all depencies are in previous code snippet or alt hold topic last code snippet

This is a "attemp to integrate sonar" topic, not an "early adopting perfectly working feature" topic, you have to know what you are doing, please stop pm/blame me because you crash your stuff... :shock:

Magnetron · Post by **Magnetron** » Sat Sep 22, 2012 2:31 pm

there are new on sonar / baro implementation?
Baro altitude routine only seems to be very responsive.

KasparsL · Post by **KasparsL** » Mon Sep 24, 2012 12:32 am

+1,
Is there some news for using HC-SR04 or other?

janekx · Post by **janekx** » Mon Sep 24, 2012 9:38 am

+1 for HC-SR04

penpen77 · Post by **penpen77** » Tue Sep 25, 2012 9:07 pm

sr04 and simili works since several months.....

penpen77 · Post by **penpen77** » Tue Sep 25, 2012 9:14 pm

EDIT: use code snippet instead

full code, based on last build (r1129)

althold with sonar/baro mixed works fine, tested on sr04, dypme007 and tinygps (i dont have i2c native devantech)

current define are for crius/mega board, don't forget to enable once baro mode before arming to set "baro home/zero alt level" (visible in debug[3])

sonar value are mixed before baro lpf, more robust but more "floppy", free to change before/after lpf

BTW, very low althold must be reached by slow going down, not by going up

last thing, ultrasonic doesn't like carpet, dont try inside over a carpet >_<

in config.h

hardware part
SONAR_GENERIC_ECHOPULSE is define for interrupt driven tx/trigger/pulse/echo sonar, pinout ar mapped in def.h (fallow the define)
SONAR_GENERIC_SCALE is the value for converting us to cm (not the same between sr04,me007, etc...
SONAR_GENERIC_MAX_RANGE is the range of sonar (used for validating reading, which now i remembered i've forgotten to test in code.... be carefull, add a test with 9999 value)

baro/sonar mixing part (assuming sonarAlt is correctly provided by hardware)
SONAR_TILT_CORRECTION define if sonar reading wil be corrected during tilting quad (use triggo, ressources hungry, could be optimized)
SONAR_BARO_FUSION activating fusion
SONAR_BARO_FUSION_LC low cut, below this value, cm, only sonar reading are used for EstAlt/AltHold
SONAR_BARO_FUSION_HC high cut, above this value, cm, only baro are used
SONAR_BARO_FUSION_RATIO specify amount of each sensor are used (sonar mult prior), if 0.0, its proportional, close to LC, it's more sonar, close to HC, more baro

there aren't really many heavy modification, but all is here and working so far, so good

Magnetron · Post by **Magnetron** » Tue Sep 25, 2012 11:11 pm

penpen77 wrote:full code, based on last build (r1129)

althold with sonar/baro mixed works fine, tested on sr04, dypme007 and tinygps (i dont have i2c native devantech)

current define are for crius/mega board, don't forget to enable once baro mode before arming to set "baro home/zero alt level" (visible in debug[3])

sonar value are mixed before baro lpf, more robust but more "floppy", free to change before/after lpf

BTW, very low althold must be reached by slow going down, not by going up

last thing, ultrasonic doesn't like carpet, dont try inside over a carpet >_<

in config.h

hardware part
SONAR_GENERIC_ECHOPULSE is define for interrupt driven tx/trigger/pulse/echo sonar, pinout ar mapped in def.h (fallow the define)
SONAR_GENERIC_SCALE is the value for converting us to cm (not the same between sr04,me007, etc...
SONAR_GENERIC_MAX_RANGE is the range of sonar (used for validating reading, which now i remembered i've forgotten to test in code.... be carefull, add a test with 9999 value)

baro/sonar mixing part (assuming sonarAlt is correctly provided by hardware)
SONAR_TILT_CORRECTION define if sonar reading wil be corrected during tilting quad (use triggo, ressources hungry, could be optimized)
SONAR_BARO_FUSION activating fusion
SONAR_BARO_FUSION_LC low cut, below this value, cm, only sonar reading are used for EstAlt/AltHold
SONAR_BARO_FUSION_HC high cut, above this value, cm, only baro are used
SONAR_BARO_FUSION_RATIO specify amount of each sensor are used (sonar mult prior), if 0.0, its proportional, close to LC, it's more sonar, close to HC, more baro

there aren't really many heavy modification, but all is here and working so far, so good

I would to try it.
A pinout to connect a DYP-ME007 on a Mega2560?

mahowik · Post by **mahowik** » Wed Sep 26, 2012 4:30 am

penpen77 wrote:full code, based on last build (r1129)

althold with sonar/baro mixed works fine, tested on sr04, dypme007 and tinygps (i dont have i2c native devantech)

current define are for crius/mega board, don't forget to enable once baro mode before arming to set "baro home/zero alt level" (visible in debug[3])

Hi penpen!

In general it looks ok, but i propose to add some changes
1) to avoid manual init and keep in mind additional instruction probably make sense to change this

Code: Select all

if(!f.ARMED && f.BARO_MODE) { //init offset
      BaroHome = (baroHigh*10.0f/(BARO_TAB_SIZE - 1));
    }
  
    if(f.ARMED && BaroHome ==0) { // if forgotten
      BaroHome = (baroHigh*10.0f/(BARO_TAB_SIZE - 1));
    }

to this:

Code: Select all

if(!f.ARMED) { //init offset till motors not armed
      BaroHome = BaroHome*0.9f + (baroHigh*10.0f/(BARO_TAB_SIZE - 1))*0.1f; // play with optimal coef. here
    }

thx-
Alex

penpen77 · Post by **penpen77** » Wed Sep 26, 2012 7:34 am

indeed, that was my first impl, but it's doing some probs:
- until copter armed, EstAlt value isn't correct...
- using some OSD (mobidrone for example) firsts reading after startup are used for offset, so if waiting too long for setting barohome, incorrect value are used
- if windy day, it's good to be able to "reset" barohome to avoid crazy value

in my opinion, the best option should be: auto set after XX seconds (delay define) and possibility to override manually barohome stored value
But, that's mean adding again some define (already added a lot for sonar) and mapping an aux statement for set barohome

I'd try giving value at compile time, baro home value int the code, but it was bad idea, readings have too many way of changing (inside, outside, weather, temperature, etc...)

i don't really know what the better to do, but for testing, activating althold once if not armed to store barohome (with the ability to store when arming the first time if forgotten) do the job, and doesn't need to arm for ground testing.

One other impl i've done (not in code here) but usefull, is adding a "buzzing" event if EstAlt is too low, kind of collision alarm since Alt reading is more accurate with sonar. Just some mod with lighting landing code

mahowik · Post by **mahowik** » Wed Sep 26, 2012 3:43 pm

penpen77 wrote:indeed, that was my first impl, but it's doing some probs:
- until copter armed, EstAlt value isn't correct...
- using some OSD (mobidrone for example) firsts reading after startup are used for offset, so if waiting too long for setting barohome, incorrect value are used
- if windy day, it's good to be able to "reset" barohome to avoid crazy value

in my opinion, the best option should be: auto set after XX seconds (delay define) and possibility to override manually barohome stored value
But, that's mean adding again some define (already added a lot for sonar) and mapping an aux statement for set barohome

I'd try giving value at compile time, baro home value int the code, but it was bad idea, readings have too many way of changing (inside, outside, weather, temperature, etc...)

i don't really know what the better to do, but for testing, activating althold once if not armed to store barohome (with the ability to store when arming the first time if forgotten) do the job, and doesn't need to arm for ground testing.

One other impl i've done (not in code here) but usefull, is adding a "buzzing" event if EstAlt is too low, kind of collision alarm since Alt reading is more accurate with sonar. Just some mod with lighting landing code

Strange that it's not working for you. Actually it's averaged all time altitude while motors are not armed...

Code: Select all

if(!f.ARMED) { //init offset till motors not armed
      BaroHome = BaroHome*0.9f + (baroHigh*10.0f/(BARO_TAB_SIZE - 1))*0.1f; // play with optimal coef. here
    }

But yes it takes some time for initialization. As I mentioned here new alt hold will take about 10-15 sec to init/stabilize all LPFs and CF...
- wait 10-15 sec after power on OR after ACC calibration... all LPFs, velocity integrator and CF need to be syncronized
During this time light BaroHome LPF also should be stabilized...

Also I'm sure that you know but just for your information on altitudes less then 1..2m there ground effect and baro can give minus 1..2m in measurement and become very unstable with their raw data... Probably it's reason why you was confused with init baro values... According to this to avoid baro ground effect it make sense to set SONAR_BARO_FUSION_LC at least to 150cm and from my experience sonars like dyp-me007 and hc-sr04 gives unstable values after 2..2.5m on the fly because of the acoustic noise form motors and electrical influence from power and ESC cords... so I propose

Code: Select all

      #define SONAR_BARO_FUSION_LC 150 //cm, low cut, below = full sonar
      #define SONAR_BARO_FUSION_HC 250 //cm, high cut, above = full baro

thx-
Alex

penpen77 · Post by **penpen77** » Thu Sep 27, 2012 10:22 am

it was just testing values, i've archived my working dir "as is", it's not intented to be working out the box or using perfectly tuned constants

mahowik · Post by **mahowik** » Thu Sep 27, 2012 3:26 pm

penpen77 wrote:it was just testing values, i've archived my working dir "as is", it's not intented to be working out the box or using perfectly tuned constants

It was not a critique, it was constructive discussion in terms of this feature

Also most people here are not programmers and actually have not experience to tune any parameters without exact manual/guide... so it's useful to put tuned feature at least checked in your config...
and if feature has a lot of good feedback from diff pilots it's very possible to see this in new release

KasparsL · Post by **KasparsL** » Wed Oct 10, 2012 1:03 pm

Will the support for HC-SR04 included in some release, or there is some reason not to use it, but take some i2c sonar sensor?

penpen77 · Post by **penpen77** » Thu Oct 11, 2012 2:02 pm

dunno

jhgeesink · Post by **jhgeesink** » Sat Oct 13, 2012 1:35 pm

Trying your sketch now penpen77, works now but the altitude is going up sometimes and then goes back to the correct altitude.
Motors off.

Han.

mahowik · Post by **mahowik** » Mon Oct 15, 2012 7:32 pm

Hi guys,

I have spent a lot of time this weekend to play with sonar solutions...

About implementation from penpen77. I have found our at least 2 main issues with this approach:
a) it's requires to average only correct raw sonar data readings but not 9999
b) there was not workable mix from sonar to baro because "fade" var was always null... I checked this in debug (fade*10.0f)

Code: Select all

fade = (SONAR_BARO_FUSION_HC-sonarAlt)/(SONAR_BARO_FUSION_HC-SONAR_BARO_FUSION_LC);

you should specify as float before division

Code: Select all

fade = ((float)(SONAR_BARO_FUSION_HC-sonarAlt))/(SONAR_BARO_FUSION_HC-SONAR_BARO_FUSION_LC);

I also added smooth (on the fly) ground offset correction because baro have drift and did some refactoring:

Code: Select all

int32_t tempAlt = baroHigh*10/(BARO_TAB_SIZE - 1);

  #if defined(SONAR)
  
    
    if(!f.ARMED && (sonarAlt < SONAR_BARO_FUSION_HC)) { //init offset till motors not armed
      EstAltStart = EstAltStart*0.95f + (tempAlt-sonarAlt)*0.05f;
    }
    
    //mix baro/sonar
    debug[0] = sonarAlt;
    if(EstAltStart != 0) {    
      if(sonarAlt < SONAR_BARO_FUSION_LC) {
        tempAlt = EstAltStart + sonarAlt;
      } else if(sonarAlt < SONAR_BARO_FUSION_HC) {
      
        // make smooth EstAltStart correction because baro have drift 
        EstAltStart = EstAltStart*0.98f + (tempAlt-sonarAlt)*0.02f;
      
        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);
        //debug[2] = fade*10.0f;
        
        tempAlt = (EstAltStart + sonarAlt)*fade + tempAlt*(1.0f - fade);
      }
    }
  #else
    if(!f.ARMED) { //init offset till motors not armed
      EstAltStart = EstAltStart*0.95f + tempAlt*0.05f;
    }
  #endif
  debug[1] = EstAltStart;
  
  //EstAlt = EstAlt*0.6f + tempAlt*0.4f; // additional LPF to reduce baro noise  
  EstAlt = (EstAlt*3 + tempAlt*2)/5; // additional LPF to reduce baro noise

But even after this it was not workable in real environment (e.g. when grass on ground and some sonar reading are not correct). So I started to think/implement error-based handler instead of current only-alt-based to mix/switch beetween sonar and baro and get much better results but not so satisfactory...
So I investigated solution from alexmos. Wow! It was that I have tried to get!

So I extended his sonar driver for mega board and have integrated his solution with new alt hold... then I tried to check it on the fly (but only solid on surface for now... will test more)... only one word: PERFECT!

thx-
Alex

shikra · Post by **shikra** » Tue Oct 16, 2012 12:56 pm

Alex that is very cool. looking forward to the vid.

Which sensor do you use?

penpen77 · Post by **penpen77** » Tue Oct 16, 2012 1:03 pm

yep, you need to use the code snippet of the topic, not the archive, i have mixed 2 version by mistake (current_dev and 2.1)

mahowik · Post by **mahowik** » Tue Oct 16, 2012 5:09 pm

shikra wrote:Alex that is very cool. looking forward to the vid.

Which sensor do you use?

Tried with HC-SR04 on the fly and DYP-ME007 in GUI... Still need to clean up some points and will post the code soon...

doughboy · Post by **doughboy** » Tue Oct 16, 2012 10:31 pm

question on this part of the code (from http://code.google.com/p/multiwii/sourc ... Bsonar.rar)

Code: Select all

         #ifdef SONAR_TILT_CORRECTION
          float temp = cos(float(angle[1])/10) * cos(float(angle[0])/10);            
       temp = max(temp+0.05, 0.707);
            temp = constrain(temp,0.707,1.0);
            temp = (float)sonarAlt * temp;
            if(temp<sonarAlt) sonarAlt = temp;
     #endif

The comment for angle array says

Code: Select all

static int16_t angle[2]    = {0,0};  // absolute angle inclination in multiple of 0.1 degree    180 deg = 1800

so I get the divide by 10 part. But arduino cos function takes radians
http://www.arduino.cc/en/Reference/Cos

so is the code passing angle in degrees?

if you look in other parts of the source, you will see the conversion to radians like this

Code: Select all

float cos_yaw_x = cos(heading*0.0174532925f);

where 0.0174532925 is PI/180.

Arduino.h defines the following constants

Code: Select all

#define PI 3.1415926535897932384626433832795
#define HALF_PI 1.5707963267948966192313216916398
#define TWO_PI 6.283185307179586476925286766559
#define DEG_TO_RAD 0.017453292519943295769236907684886
#define RAD_TO_DEG 57.295779513082320876798154814105

penpen77 · Post by **penpen77** » Tue Oct 16, 2012 10:56 pm

yep, mistake, wrong adaptation from apm in my work dir at the time of zipping. Use code snippet instead for bug-free/less

doughboy · Post by **doughboy** » Wed Oct 17, 2012 5:06 pm

where is the code snippet?

penpen77 · Post by **penpen77** » Wed Oct 17, 2012 5:44 pm

mahowik wrote:Hi guys,

I have spent a lot of time this weekend to play with sonar solutions...

About implementation from penpen77. I have found our at least 2 main issues with this approach:
a) it's requires to average only correct raw sonar data readings but not 9999
b) there was not workable mix from sonar to baro because "fade" var was always null... I checked this in debug (fade*10.0f)
Code: Select all
fade = (SONAR_BARO_FUSION_HC-sonarAlt)/(SONAR_BARO_FUSION_HC-SONAR_BARO_FUSION_LC);
you should specify as float before division
Code: Select all
fade = ((float)(SONAR_BARO_FUSION_HC-sonarAlt))/(SONAR_BARO_FUSION_HC-SONAR_BARO_FUSION_LC);
I also added smooth (on the fly) ground offset correction because baro have drift and did some refactoring:
Code: Select all
int32_t tempAlt = baroHigh*10/(BARO_TAB_SIZE - 1);

  #if defined(SONAR)
  
    
    if(!f.ARMED && (sonarAlt < SONAR_BARO_FUSION_HC)) { //init offset till motors not armed
      EstAltStart = EstAltStart*0.95f + (tempAlt-sonarAlt)*0.05f;
    }
    
    //mix baro/sonar
    debug[0] = sonarAlt;
    if(EstAltStart != 0) {    
      if(sonarAlt < SONAR_BARO_FUSION_LC) {
        tempAlt = EstAltStart + sonarAlt;
      } else if(sonarAlt < SONAR_BARO_FUSION_HC) {
      
        // make smooth EstAltStart correction because baro have drift 
        EstAltStart = EstAltStart*0.98f + (tempAlt-sonarAlt)*0.02f;
      
        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);
        //debug[2] = fade*10.0f;
        
        tempAlt = (EstAltStart + sonarAlt)*fade + tempAlt*(1.0f - fade);
      }
    }
  #else
    if(!f.ARMED) { //init offset till motors not armed
      EstAltStart = EstAltStart*0.95f + tempAlt*0.05f;
    }
  #endif
  debug[1] = EstAltStart;
  
  //EstAlt = EstAlt*0.6f + tempAlt*0.4f; // additional LPF to reduce baro noise  
  EstAlt = (EstAlt*3 + tempAlt*2)/5; // additional LPF to reduce baro noise  
But even after this it was not workable in real environment (e.g. when grass on ground and some sonar reading are not correct). So I started to think/implement error-based handler instead of current only-alt-based to mix/switch beetween sonar and baro and get much better results but not so satisfactory...
So I investigated solution from alexmos. Wow! It was that I have tried to get!
So I extended his sonar driver for mega board and have integrated his solution with new alt hold... then I tried to check it on the fly (but only solid on surface for now... will test more)... only one word: PERFECT!

thx-
Alex

I don't understand, the new althold algo and LPF should "smooth" the ground/reading irregularity in one hand, and in other hand, mixing statement (for now) avoid 9999 values to be integrated (but which indeed must be correctly handled)
Even with no mixing at all and no baro, the filter keep no more than 40% of max delta reading, which is close to a median filtering approch

Tilting: yep, use DEG_TO_RAD

Test Pilot Mafia · Post by **Test Pilot Mafia** » Thu Oct 18, 2012 9:36 pm

i really need a distance based alitude hold capability on my BI and TRI drones, how has this work been coming? what sonar should i buy, given i have the crius MWii lite 328p, ive got my tri working really well (till it augered in from 40 feet, upside down.)

i really apreciate those of you who code all this together, and then do the whole deveopement deal... im an EE at CSU Chico, so ive seen enormous profects before, and this multiwii stuff is so important.

I have a concern, at academic compettions, ive seen sonar fail, due to downward air turbulence, perhaps a IR distance sensor in parallel or by itself could also be implimented.... (PS ive crashed my ARdrone from 170 feet straight down due to Ultrasound failure... killed it.)

TY, Patrick Coleman
CSU Chico, Flight Club.

penpen77 · Post by **penpen77** » Tue Oct 23, 2012 7:55 pm

I have just tried last impl of "getAltEstimation" during a little fpv session after office day, with hc-sr04 and more smoothing on readings but it became very uneffective. I think it's a wrong idea to oversmooth readings... raw and "crispy" values should be keept. You could see in the vid that alt hold with sonar ignore completly small "ground bump" if too quick and it should'nt. I had also made different moving with different alt hold test, with 1/2/3m which was my LC/HC fusion values, the "middle fusion alt" was the more effective. But, i had more effective results with previous impl and raw/40% median readings.

The second thing is that i hadn't notice any significant prob with grass/ground/rock/other with hc-sr04, several time I was waching sonar telemetry readings and switching between grass/rock/moud only had a ~+/-5cm in the worst cases (before filtering/smoothing). The only erratics values was above 4/5m where tilting correction gave more wobbly readings.

http://youtu.be/esvI_-S_RGw

It's fpv style since it wasn't my first goal to perform some test but you can see that sonar loose all his reactivity. I'd put my hand under the sensor had to wait 3/4s before increasing of alt and since the two sonar and baro share the same pid loop, one can't be tuned without impacting the other

I will test an other version tomorrow if meteo is ok and uploading source here if more effective.

The other way could be to impl a new feature: keep constant distance to ground, which is no really the same as alt hold and will need an other pid impl, other behavior switch, etc.... more complexity in usage but less complexity in processing

doughboy · Post by **doughboy** » Tue Oct 23, 2012 8:05 pm

this sonar feature only works if BARO is present? (besides using it to turn on landing lights)
I was thinking alt hold within the useful distance capability of ultrasonic sensor would be available but I see it is not (I don't use barometer).
I know other FC has this feature and can make the quad fly say at 3feet above ground constantly and go up and down follwoing bumps and dips along the way.
another use is to limit the flight height to say about 10feet.

penpen77 · Post by **penpen77** » Tue Oct 23, 2012 8:25 pm

use baro at this time but for now, it's just a kind of "dirty" impl, like a baro alt with sonar precision at low alt like in arducopter or arducopterng

but it can works without baro, with minor code adjustment

but you're right in one way, sonar can't give alone the same functionnality than baro alt hold. Baro is absolute alt and sonar is relative alt.

So two school:
- using sonar to add precision at low altitude to baro alt hold (like now) with the possibility of error caused by relative/absolute variation (which is the purpose of LC/HC fusion of reading)
- using sonar to keep distance to ground independantly of absolute/baro alt readings which need to add new feature (aux, pid and all needs)

for sonar alone, the two are the same... but the second behavior could works also for "obstacle avoidance" since it's not coupled to baro, just aim with sonar horizontaly instead of verticaly

for now, my idea was, like in adc, to use lc/hc segmentation

ground <-> LowCut alt : keep distance to ground using only sonar and pid loop of baro alt old
LowCut <-> HighCut alt: a kind of baro alt hold with more precision and gradualy fusionned values for avoiding to much seek between absolute and relative altitude and as previous, same pid loop of baro alt hold
HighCut <-> sky/birds/felix baumgartner/space/whatever is above and still give reading with baro: full baro alt hold (with his pid loop)

mahowik · Post by **mahowik** » Wed Oct 24, 2012 10:49 pm

penpen77 wrote:The only erratics values was above 4/5m where tilting correction gave more wobbly readings.

Code: Select all

#ifdef SONAR_TILT_CORRECTION
          float temp = cos(float(angle[1])/10) * cos(float(angle[0])/10);            
       temp = max(temp+0.05, 0.707);
            temp = constrain(temp,0.707,1.0);
            temp = (float)sonarAlt * temp;
            if(temp<sonarAlt) sonarAlt = temp;
     #endif

It doesn't make sense to use tilt correction for sonar because raw data from sonar independent from inclinations in range of 15-25 degrees (depends on sonar ... see doc). If inclination more than sonar visibility you should catch this and switch to baro...

thx-
Alex

doughboy · Post by **doughboy** » Wed Oct 24, 2012 11:01 pm

actually, the tilt correction code is in the wrong place. I moved it to inside sonar update where it belongs.

if you do tilt correction in the main loop, it replaces sonarAlt with the corrected value, then in sonar_update, it the uses the corrected value with non corrected reading. the tilt correction should be applied to the non corrected reading.

the result seems to be fine.

which brings me to my next question. what is the unit of sonarAlt? is it the same as BaroAlt?
I added this to getEstinateAltitude method

Code: Select all

  #if defined(SONAR)
  baroHistTab[baroHistIdx] = sonarAlt/10;
  #else
  baroHstTab[baroHistIdx] = BaroAlt/10;
  #endif

along with other changes so I can switch on BARO mode, when I flight tested, once the quad got to about 4 feet or so, I swiitched on BARO mode then the quad shot straight up! I am only using sonar and no baro. I know the sonar code works because I added code to sound the buzzer if alt is less than 90CM (about 3 feet) and it works perfect.

penpen77 · Post by **penpen77** » Wed Oct 24, 2012 11:18 pm

arducopter use tilt correction so i think it worth a try

sonarAlt is in cm

but i've the strange feeling that there are several code impl/version mixed "into the wild"

depends of which version you're based but tilt correction is buggy with wrong conversion (rad_to_deg)

doughboy · Post by **doughboy** » Wed Oct 24, 2012 11:22 pm

thanks for the sonaralt unit. yes, I use tilt correction and it works fine, but I moved the code to sonar_update.

ok, I added debugs to monitor the values and they seem to be right.
if I set throttle to about 1350 then turn on the baro mode, I can see positive baropid of I move down the quad, and negative baropid if I move the quad up. maybe it was a glitch on my first test. I'll try it again.

thanks

penpen77 · Post by **penpen77** » Wed Oct 24, 2012 11:50 pm

but before all of this, i think we should choose a goal for each point (at this time and as said above, there many feature impl possibility):

for now, we have all sensor already impl in stable branch + echo/trigger generics with olaf code snippet (hcsr04, dyp me007, etc...), so we can work direcly on the feature with most of hardawre and sonarAlt is in cm in each impl

---- hardware impl
->is there other hardware to impl ? like serial sonar ? laser/lidar ?
->is error should be handled by "driver" or later by feature implementation ? (sonar are very sensitive and error generative with EMF, too close to motor, different ultrasonic absorption of ground, -> "despite of many effort, reading/shits will append, necesserly and obviously"
-> other idea ?

--- feature
there is some different request as this time so:
-> what kind of feature should we propose: keep distance to ground (relative alt hold), more alt precision (fusion with baro readings), mix of two with different marker for diferent behaviour ?
-> low alt warning (led/buzzer), trigger (airbag, landing gear) ?
-> more "general collision avoidance" like horizontal/wall detecting ?
-> other idea ?

--- implementation
it depends of feature but there is a leat a pid loop and acc_z usage
-> do we use the baro alt hold as working base (with acc_z impl) which mean sharing same pid loop ?
-> do we use tilt correction ? (since arducopter use it, i think it's not a bad idea and worth the try)
-> do we handled readings error here with lpf/hpf ? (which if shared with baro would be tricky since sonar is way less sloppy ?)

etc.... so before throwing many version and implemention, i think we should answer all theses questions and write code/impl after

mahowik · Post by **mahowik** » Thu Oct 25, 2012 2:07 am

penpen77 wrote:arducopter use tilt correction so i think it worth a try

As I remember it was commented there...
In any case I'm pretty sure and I checked this with at least hc-sr04, dyp-me007

Just do simple test... Start to tilt slowly and till 15-25 degree raw altitude value will be the same... after 15-25 it stops to return correct value at all...

thx-
Alex

penpen77 · Post by **penpen77** » Thu Oct 25, 2012 10:33 am

tried on concrete floor, with hcsr 04 and srf08, raw reading wasn't at all the same.... may be at 10cm of the floor with ground multipath reflection, but at 1m and more, raw values aren't the same at all. And with tilting correction, readings became stable and precise even with tilt more than 30°/45°

anyway you can comment/uncomment it, so feel free to use it or not, maybe in some env it will works maybe not

other point, tilting correction is maybe in the wrong place, but it could'nt be in sonar_upate too, this function is hardware dependant and tilting correction is generic

so 2 way:
- independant function called immediatly after reading (of any kind of hardware)
- feature dependant and called inside/close to feature handling

penpen77 · Post by **penpen77** » Thu Oct 25, 2012 4:04 pm

I just had a try, with sonar alone and mixing with baro, even with 1/9 ratio, it's too much, we need raw values, absolutly
it's on concret in my basement since it's a little bit too windy outside for now

I've recorder with a keychain this time with the sr04 which is the cheapest avaible for those who want to try

so, 2 thing (again)
- we need raw value without filtering/smoothing/integrating or very little one (less than 1 to 9 averaging) if we want tu use acc_z/baro alt hold implementation
- if sonar alone, pid has to be agressive, if baro/sonar fusion, and sharing same pid, sonar will be a little sloppy, or we can use some "climbing rate"

by the way, tilting works perfectly, at least on concrete, even with the "bubled beam shape of sr08", tilting quad loose less alt with tilting correction

http://youtu.be/DSQ9LSuyU9s
working but really lazy on climbing and as it share baro pid, it cant be more agressive without impacting baro alt hold (the drifting is my fault, cg was bad and not trimmed)

This is essentially the same as my older code but without mess up version and up to date with r1177. I've refactoring the code, to be working with or without baro, added some commentary/questioning for improvement, moving element where they belong (at least logicaly) and removing filtering/smoothing (sorry but inefficient)
The config.h is also clean, don't forget to enable stuff as it should be (if using generic sonar, dont forget to uncomment the 3 define and set them, and if necessary for exotic pinout, change def.h macro)

MultiWii_21_1177_sonar.rar: (120.31 KiB) Downloaded 803 times

mahowik · Post by **mahowik** » Thu Oct 25, 2012 4:29 pm

penpen77 wrote:tried on concrete floor, with hcsr 04 and srf08, raw reading wasn't at all the same.... may be at 10cm of the floor with ground multipath reflection, but at 1m and more, raw values aren't the same at all. And with tilting correction, readings became stable and precise even with tilt more than 30°/45°

Ok, i will re-check this, but remember exactly and we agreed with other developers (from russian forum) that is independent from inclination... You can see that sonar has a conus-emitter which generates divergent beam... this why it independent for some angle range...

penpen77 wrote:- we need raw value without filtering/smoothing/integrating or very little one (less than 1 to 9 averaging)

yes, but you need to take into account and exclude values when sonar has errors... this why I took error-based sonar driver (from alexmos).

penpen77 wrote:- if sonar alone, pid has to be agressive, if baro/sonar fusion, and sharing same pid, sonar will be a little sloppy, or we can use some "rate"

I have took this into account but on "bad" surface with e.g. grass it produce additional wobbles... Probably make sense to apply this if SonarErrors=0, i.e. when surface is exactly clear...so as for now I turned it off...

Code: Select all

// Increase P-term if sonar is used
  #if SONAR && defined(SONAR_BARO_PID_GAIN)
    if(SonarErrors < SONAR_ERROR_MAX) {
      error += constrain(error * (SONAR_ERROR_MAX - SonarErrors) / SONAR_ERROR_MAX * SONAR_BARO_PID_GAIN, -150, 150);
    }
  #endif

My current version 2.1_b4 will be posted with description soon in this thread: viewtopic.php?f=8&t=2371&p=25759#p25699

thx-
Alex

copterrichie · Post by **copterrichie** » Thu Oct 25, 2012 4:32 pm

Why not try two sonars (forward and aft) and compare the two readings? Yes, it is more weight.

mahowik · Post by **mahowik** » Thu Oct 25, 2012 4:45 pm

copterrichie wrote:Why not try two sonars (forward and aft) and compare the two readings? Yes, it is more weight.

you can try, but cheap sonars (like hc-sr04, dyp-me007) need to be handled with interrupt... so if you have resources on your board for this, why not

copterrichie · Post by **copterrichie** » Thu Oct 25, 2012 5:04 pm

mahowik wrote:
copterrichie wrote:Why not try two sonars (forward and aft) and compare the two readings? Yes, it is more weight.

you can try, but cheap sonars (like hc-sr04, dyp-me007) need to be handled with interrupt... so if you have resources on your board for this, why not

This is where having a housekeeping co-processor would come in handy. I believe someone used an ATiny2313 and was able to connect many, not sure if that was Tommy.

doughboy · Post by **doughboy** » Thu Oct 25, 2012 5:46 pm

tinygps only supports 1 ultrasonic sensor (at least last time I checked). atmega328 is a better option. not really worth saving $2 to deal with hassles of programming the attiny chip. or one can just use a promini (about $10 on ebay) and just solder the sensor lines to the board (if you build the board yourself, you will be spending close to $10 already anyway).

------------
Go Giants!

penpen77 · Post by **penpen77** » Thu Oct 25, 2012 5:51 pm

just have a try on grass/sand/ground/strange_orange_rubber_ground_of_athletism_stadium and had good behavior with above code. I've pushed more P and I term since i hadn't thing above my head. Still "sleepy/lazy" to climb/react, but different nature of ground hadn't so much bad influence, of course a little bit, but not as much i've expected. Even with activating at 10cm of the grass and doing some switch grass<->ground<->grass, it sill hold correctly. That's the behavior i had with the last version. In the other hand, of course i will never trust the sensor to perform some very low pass at high speed between two different kind of ground.

The only bad point is that still not avoid collision with crazy children who try to catch with hand rotating little saw of death even if you said dont touch, dont get too close"..... so i had to abort test....

http://youtu.be/Y26gE2xTxjo