Integration of SRF08 I2C sonar sensor (done)

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

Integration of SRF08 I2C sonar sensor (done)

Post by guru_florida »

Hi all,

I integrated the SRF08 sonar sensor into the latest dev 20120225. (It can also be backported easily.) The nice thing about this sensor is it has a long range (6m) and you can have up to 16 of them on the same i2c bus. No other pins are needed. The main purpose I did this is to fuse the reading with the Baro sensor - making it more accurate at low altitudes.

I'd love to see some obstacle avoidance with multiple modules here. Can you imagine really flying indoors without fear! :)

Features of the SRF08:
* Range from 3cm to 6m (actually may go to 11m but can have problems >6m)
* i2c bus, can also support 16 sensors on the same i2c bus

Features of the SRF08 code in MultiWii:
* autodetects SRF08 modules
* multiple sonar sensors already supported (up to 8)
* new sensors' i2c address is reassigned to F0-FE i2c address range once detected
* multiple sensors could be used for baro sensor fusion, side mounted or top mounted for object avoidance
* multiple sensors can be triggered together for synchronous sense, or triggered & read sequentially
* code form is very similar to ms6511 or BMP085 state machine code

First, #define SRF08 somewhere (in config.h or def.h) so this code will be enabled.

Add this code to the end of Sensors.pde but before the initSensors() function:

Code: Select all

// ************************************************************************************************************
// I2C Sonar SRF08
// ************************************************************************************************************
// first contribution from guru_florida (02-25-2012)
//
// specs are here: http://www.meas-spec.com/downloads/MS5611-01BA03.pdf
// useful info on pages 7 -> 12
#if defined(SRF08)

// the default address for any new sensor found on the bus
// the code will move new sonars to the next available sonar address in range of F0-FE so that another
// sonar sensor can be added again.
// Thus, add only 1 sonar sensor at a time, poweroff, then wire the next, power on, wait for flashing light and repeat
#if !defined(SRF08_DEFAULT_ADDRESS)
  #define SRF08_DEFAULT_ADDRESS 0xE0
#endif

#if !defined(SRF08_RANGE_WAIT)
#define SRF08_RANGE_WAIT     80000      // delay between Ping and Range Read commands
#endif

#if !defined(SRF08_RANGE_SLEEP)
#define SRF08_RANGE_SLEEP    35000      // sleep this long before starting another Ping
#endif

#if !defined(SRF08_SENSOR_FIRST)
#define SRF08_SENSOR_FIRST    0xF0    // the first sensor i2c address (after it has been moved)
#endif

#if !defined(SRF08_MAX_SENSORS)
#define SRF08_MAX_SENSORS    4        // maximum number of sensors we'll allow (can go up to 8)
#endif

#define SONAR_MULTICAST_PING

// registers of the device
#define SRF08_REV_COMMAND    0
#define SRF08_LIGHT_GAIN     1
#define SRF08_ECHO_RANGE     2


static struct {
  // sensor registers from the MS561101BA datasheet
  int32_t  range[SRF08_MAX_SENSORS];
  int8_t   sensors;              // the number of sensors present
  int8_t   current;              // the current sensor being read
  uint8_t  state;
  uint32_t deadline;
} srf08_ctx;


// read uncompensated temperature value: send command first
void Sonar_init() {
  memset(&srf08_ctx, 0, sizeof(srf08_ctx));
  srf08_ctx.deadline = 4000000;
}

// this function works like readReg accept a failed read is a normal expectation
// use for testing the existence of sensors on the i2c bus
// a 0xffff code is returned if the read failed
uint16_t i2c_try_readReg(uint8_t add, uint8_t reg) {
  uint16_t count = 255;
  i2c_rep_start(add+0);  // I2C write direction
  i2c_write(reg);        // register selection
  i2c_rep_start(add+1);  // I2C read direction
 
  TWCR = (1<<TWINT) | (1<<TWEN);
  while (!(TWCR & (1<<TWINT))) {
    count--;
    if (count==0) {              //we are in a blocking state => we don't insist
      TWCR = 0;                  //and we force a reset on TWINT register
      return 0xffff;  // return failure to read
    }
  }
 
  uint8_t r = TWDR;
  i2c_stop();
  return r; 
}

// read a 16bit unsigned int from the i2c bus
uint16_t i2c_readReg16(int8_t addr, int8_t reg) {
  i2c_rep_start(addr);
  i2c_write(reg);
  i2c_rep_start(addr + 1);
  return (i2c_readAck() <<8) | i2c_readNak();
}

void i2c_srf08_change_addr(int8_t current, int8_t moveto) {
  // to change a srf08 address, we must write the following sequence to the command register
  // this sequence must occur as 4 seperate i2c transactions!!
  //   A0 AA A5 [addr]
  i2c_writeReg(current, SRF08_REV_COMMAND, 0xA0);  delay(30);
  i2c_writeReg(current, SRF08_REV_COMMAND, 0xAA);  delay(30);
  i2c_writeReg(current, SRF08_REV_COMMAND, 0xA5);  delay(30);
  i2c_writeReg(current, SRF08_REV_COMMAND, moveto);  delay(30); // now change i2c address
  blinkLED(5,1,2);
}

// discover previously known sensors and any new sensor (move new sensors to assigned area)
void i2c_srf08_discover() {
  uint8_t addr;
  uint16_t x;

  // determine how many sensors are plugged in
  srf08_ctx.sensors=0;
  addr = SRF08_SENSOR_FIRST;
  for(int i=0; i<SRF08_MAX_SENSORS && x!=0xff; i++) {
    // read the revision as a way to check if sensor exists at this location
    x = i2c_try_readReg(addr, SRF08_REV_COMMAND);
    if(x!=0xffff) {
      // detected a sensor at this address
      srf08_ctx.sensors++;
      addr += 2;
    }
  }
 
  // do not add sensors if we are already maxed
  if(srf08_ctx.sensors < SRF08_MAX_SENSORS) {
    // now determine if any sensor is on the 'new sensor' address (srf08 default address)
    // we try to read the revision number
    x = i2c_try_readReg(SRF08_DEFAULT_ADDRESS, SRF08_REV_COMMAND);
    if(x!=0xffff) {
      // new sensor detected at SRF08 default address
      i2c_srf08_change_addr(SRF08_DEFAULT_ADDRESS, addr);  // move sensor to the next address
      srf08_ctx.sensors++;
    }
  }
}



void Sonar_update() {
  if (currentTime < srf08_ctx.deadline || (srf08_ctx.state==0 && armed)) return;
  srf08_ctx.deadline = currentTime;
  TWBR = ((16000000L / 400000L) - 16) / 2; // change the I2C clock rate to 400kHz, SRF08 is ok with this speed
  switch (srf08_ctx.state) {
    case 0:
      i2c_srf08_discover();
      if(srf08_ctx.sensors>0)
        srf08_ctx.state++;
      else
        srf08_ctx.deadline += 5000000; // wait 5 secs before trying search again
      break;
    case 1:
      srf08_ctx.current=0;
      srf08_ctx.state++;
      srf08_ctx.deadline += SRF08_RANGE_SLEEP;
      break;
#if defined(SONAR_MULTICAST_PING)
    case 2:
      // send a ping via the general broadcast address
      i2c_writeReg(0, SRF08_REV_COMMAND, 0x51);  // start ranging, result in centimeters
      srf08_ctx.state++;
      srf08_ctx.deadline += SRF08_RANGE_WAIT;
      break;
    case 3:
      srf08_ctx.range[srf08_ctx.current] = i2c_readReg16( SRF08_SENSOR_FIRST+(srf08_ctx.current<<1), SRF08_ECHO_RANGE);
      srf08_ctx.current++;
      if(srf08_ctx.current >= srf08_ctx.sensors)
        srf08_ctx.state=1;
      break;
#else
    case 2:
      // send a ping to the current sensor
      i2c_writeReg(SRF08_SENSOR_FIRST+(srf08_ctx.current<<1), SRF08_REV_COMMAND, 0x51);  // start ranging, result in centimeters
      srf08_ctx.state++;
      srf08_ctx.deadline += SRF08_RANGE_WAIT;
      break;
    case 3:
      srf08_ctx.range[srf08_ctx.current] = i2c_readReg16(SRF08_SENSOR_FIRST+(srf08_ctx.current<<1), SRF08_ECHO_RANGE);
      srf08_ctx.current++;
      if(srf08_ctx.current >= srf08_ctx.sensors)
        srf08_ctx.state=1;
      else
        srf08_ctx.state=2;
      break;
#endif
  }
}
#else
inline void Sonar_init() {}
inline void Sonar_update() {}
#endif


Add call to Sonar_init() in initSensors() function at the end of Sensors.pde:

Code: Select all

  if (ACC) {ACC_init();acc_25deg = acc_1G * 0.423;}
  Sonar_init();     /// <--- add this line!!!
}


Add a call to Sonar_update() in MultiWii.pde at line 594 (in latest dev):

Code: Select all

        #if BARO
          Baro_update();
          Sonar_update();     /// <--- add this line!!!
          break;
        #endif


You now have up to 4 sonar readings in srf08_ctx.range[0..3] ... you can support up to 8 sensors by changing SRF08_MAX_SENSORS define.

Since there is no baro/sonar sensor fusion code yet there is no use of the sensor yet. So I have also integrated into alexmos's baro alt code, looks good in the GUI, the baro output is stabalized nicely! But I havent tested it in flight yet. This is the only branch I know of that did sensor fusion. I'll post more details tomorrow.

Colin
Attachments
MultiWii_dev20120225_srf08.zip
MultiWii code with SRF08 integrated (use MultiWiiConf from original MultiWii_dev_20120225 available on code.google.com
(65.94 KiB) Downloaded 932 times

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

Re: Integration of SRF08 I2C sonar sensor (done)

Post by LuFa »

Sounds very goooooooood
have order a SRF08 Now :mrgreen:

did you plan to share the Sonar/baro fusion code also with us ?

Thanks !

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

Re: Integration of SRF08 I2C sonar sensor (done)

Post by copterrichie »

Great Job, the WiiCopter is ready for the big time!!!

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

Re: Integration of SRF08 I2C sonar sensor (done)

Post by guru_florida »

I'll share everything. The only sonar/baro fusion code I know of is alexmos's as found in thread:
viewtopic.php?f=7&t=363&p=9568#p9568

I am going to try that today.

msev
Posts: 186
Joined: Thu Apr 14, 2011 11:49 am

Re: Integration of SRF08 I2C sonar sensor (done)

Post by msev »

Where did you guys order your SRF08?

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

Re: Integration of SRF08 I2C sonar sensor (done)

Post by guru_florida »

It worked very well! I could hover 1ft off the ground and fly around, it was holding steady. I'm going to do a little more check-over and I will post the code.

msev: I ordered mine from robotshop.com in the US. They are sold out until the 8th. I just ordered 4 more from acroname.com. I am going to test out multiple sonars! :)

LenzGr
Posts: 166
Joined: Wed Nov 23, 2011 10:50 am
Location: Hamburg, Germany
Contact:

Re: Integration of SRF08 I2C sonar sensor (done)

Post by LenzGr »

Very cool. I have a SRF02 sensor that supports both I2C and serial. As soon as I've connected it, I'll give it a try!

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

Re: Integration of SRF08 I2C sonar sensor (done)

Post by LuFa »

guru_florida wrote:It worked very well! I could hover 1ft off the ground and fly around, it was holding steady. I'm going to do a little more check-over and I will post the code.

msev: I ordered mine from robotshop.com in the US. They are sold out until the 8th. I just ordered 4 more from acroname.com. I am going to test out multiple sonars! :)


please make a video :mrgreen:

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

Re: Integration of SRF08 I2C sonar sensor (done)

Post by guru_florida »

Some notes on performance at different altitudes:
1ft holds to within a few inches. A few times it almost dropped to the ground, but seriously, I am 1ft off the ground!! :)
5ft works very well. most of the time it holds still. Sometimes it's skipping like a stone on water a bit - symptoms of poor PID parameters.
7 feet It's going back to only using baro but probably just also needs some tuning. code starts weening off the sonar usage around the 2.5m range - I can increase this to 5m.

No surprise with sonar, it works better the closer to the ground you are. I think this is a really good thing, up high you're not quite as concerned about having a 1m hover range as being 1m off the ground.

According to datasheet for SRF10, code will also work unmodified. However, after reading both, buy the SRF08 still. The SRF08 supports receiving multiple ping echo's, the SRF10 only one. This may come in handy later when we mount the sonars laterally. Our first ping might be from the copter or blades themselves, so wait for the second ping or ping over a certain min-distance.

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

Re: Integration of SRF08 I2C sonar sensor (done)

Post by guru_florida »

Yes, the SRF02 should work completely as well. It also supports many sensors on the bus. I am going to get some of these too. These are going to be great for lateral sensors since they are smaller and I can fit them on the wings/arms. They are also less than half the price!

Why do some sensors have 2 tweekers/speakers? Anyone know? They look like the exact same part - not a like a speaker and a mic sort of deal.

LenzGr wrote:Very cool. I have a SRF02 sensor that supports both I2C and serial. As soon as I've connected it, I'll give it a try!

LenzGr
Posts: 166
Joined: Wed Nov 23, 2011 10:50 am
Location: Hamburg, Germany
Contact:

Re: Integration of SRF08 I2C sonar sensor (done)

Post by LenzGr »

guru_florida wrote:Yes, the SRF02 should work completely as well. It also supports many sensors on the bus.
.
Excellent, I'll take a look at your code.
I am going to get some of these too. These are going to be great for lateral sensors since they are smaller and I can fit them on the wings/arms. They are also less than half the price!

Indeed! Otherwise, the SRF02 seems to be quite similar to the SRF08 from the specs.
Why do some sensors have 2 tweekers/speakers? Anyone know? They look like the exact same part - not a like a speaker and a mic sort of deal.

Well, I assume they actually are the same parts - you can use a piezoelectric crystals both to emit vibration by applying a voltage as well as making it generate an electric field when it's being put under stress. Using two separate transducers may increase the "ping frequency", as you don't have to switch between emitting and receiving a signal?

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

Re: Integration of SRF08 I2C sonar sensor (done)

Post by guru_florida »

I just went through 4 batteries. I got the copter and alt hold tuned well. The alt hold actually works the best at 1-4 feet! lol. There is still some tweaking to the code that needs to happen when the copter is at 6 feet and above. I think the code is still relying too much on the baro then when it could still trust the sonar.

At 1-4 feet though it's awesome! Who knew you could actually fly the craft around at 1 foot and not crash. hehe. It's really smooth at the low alts. Going to the copter and pushing it down it responds instantly. It feels wierd actually, like pushing a fat mans belly...like jelly.

I'll post code tomorrow. really tired!!! I'll try to get my wife to do a vid too. (no, I mean the copter. bad.)

Colin

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

Re: Integration of SRF08 I2C sonar sensor (done)

Post by LuFa »

any news :mrgreen: ?

nhadrian
Posts: 421
Joined: Tue Oct 25, 2011 9:25 am

Re: Integration of SRF08 I2C sonar sensor (done)

Post by nhadrian »

any updates...?
My srf08 is on the way.... :D:D:D
Dear guru_florida, please share your working code for testing and development. Even if there are bugs or so!!!!!

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

Re: Integration of SRF08 I2C sonar sensor (done)

Post by guru_florida »

:) Sorry guys, I've been recovering from a major case of exhaustion. I'll post the code when I get home tonight. There is still a few little tuning things, but works good.

nhadrian wrote:any updates...?
My srf08 is on the way.... :D:D:D
Dear guru_florida, please share your working code for testing and development. Even if there are bugs or so!!!!!

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

Re: Integration of SRF08 I2C sonar sensor (done)

Post by guru_florida »

Here's the link. This code is originally from user Alexmos who coded the analog sonar and his own Alt hold implementation. He fused the sonar readings with the baro readings. If the sonar is beyond range then only the baro is used.

All I did was add the code for i2c sonar sensors instead.

You will have to go over your settings in the config.h and def.h files. Make sure to set THROTTLE_HOVER at the bottom of config.h. This is the throttle value that just about get's your copter off the ground. You'll have to get this value by starting the copter with MultiWiiConfig active. Mine was 1275.

Download the code at:
http://www.flyingeinstein.com/MultiWii-V1_9_alexmos_guru.zip

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

Re: Integration of SRF08 I2C sonar sensor (done)

Post by guru_florida »

I also tried in *very* high winds today. The copter was doing pretty good to maintain altitude even though I was having a tough time keeping the copter from blowing away. Seriously it was gusting to about 25mph winds out there! During the high gusts the copter was acting like a wing so the wind was picking it up and the copter couldnt reduce power low enough to drift back down. Funny! (Alt hold algorithm has a clamp that prevents it from reducing or powering the motors too high.)

It did a better job holding altitude than I could with alt hold off. That's saying something. I hit the ground twice myself. lol

nhadrian
Posts: 421
Joined: Tue Oct 25, 2011 9:25 am

Re: Integration of SRF08 I2C sonar sensor (done)

Post by nhadrian »

will test once having my new srf08... :D

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

Re: Integration of SRF08 I2C sonar sensor (done)

Post by LuFa »

Hey ,

with arduino 1.0 IDE i get this compiling error :

Code: Select all

MultiWii.cpp.o: In function `setup':
C:\Users\Ludwig\AppData\Local\Temp\build4385120620906669231.tmp/MultiWii.cpp:525: undefined reference to `initSonar()'
MultiWii.cpp.o: In function `annexCode()':
C:\Users\Ludwig\AppData\Local\Temp\build4385120620906669231.tmp/MultiWii.cpp:505: undefined reference to `sonarTrigger()'


any ideas how to fix the proplem ?

works the code also with the newest DEV ?

nhadrian
Posts: 421
Joined: Tue Oct 25, 2011 9:25 am

Re: Integration of SRF08 I2C sonar sensor (done)

Post by nhadrian »

Dear guru_florida!

I'm going to prepare for SRF08. I can see in manual that it needs 5V input voltage. Could you confirm that I should use the 5V I2C port on my quadrino not 3.3V one?

BR
Adrian

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

Re: Integration of SRF08 I2C sonar sensor (done)

Post by guru_florida »

Correct, the 5V I2C header. Also, you can pull the power from the servo header or the serial header if you are not using it. Since you are using the quadrino then that code should work for you without modification, def.h and config.h already setup.

nhadrian wrote:Dear guru_florida!
I'm going to prepare for SRF08. I can see in manual that it needs 5V input voltage. Could you confirm that I should use the 5V I2C port on my quadrino not 3.3V one?
BR
Adrian

Waldmensch
Posts: 31
Joined: Sat Dec 31, 2011 12:10 am

Re: Integration of SRF08 I2C sonar sensor (done)

Post by Waldmensch »

is this code also running with SRF02 without changes? http://www.robot-electronics.co.uk/htm/srf02techI2C.htm
The SRF02 is avail on the market for the half price of SRF08

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

Re: Integration of SRF08 I2C sonar sensor (done)

Post by guru_florida »

It should. I checked the datasheet and it uses the same protocol. I also bought some SRF02's and I will test them out soon and report back. Either way, I will get the SRF02's working as well if they dont already work.

Waldmensch wrote:is this code also running with SRF02 without changes? http://www.robot-electronics.co.uk/htm/srf02techI2C.htm
The SRF02 is avail on the market for the half price of SRF08

nhadrian
Posts: 421
Joined: Tue Oct 25, 2011 9:25 am

Re: Integration of SRF08 I2C sonar sensor (done)

Post by nhadrian »

Hi all,

I have just received my SRF08!
I inserted the code parts into multiwii 2.0PV1, it compiles without error. I added the line for SonarAlt.
I placed Sonaralt to debug2.

Now it looks like it works under 60 cm, over it becomes crazy. How I could set up 6 m range?


BR
Adrian

Waldmensch
Posts: 31
Joined: Sat Dec 31, 2011 12:10 am

Re: Integration of SRF08 I2C sonar sensor (done)

Post by Waldmensch »

@guru_florida:
Your code respects the Angle related horizon? If the Sensor is not parallel to ground the measure is wrong (while Pitch/Roll actions). See drawing http://torfnase.lima-city.de/22500098ab ... index.html

So tw ways:
1) waste all values measured while difference to horizon on pitch or roll (compare with ACC values) -> maybe to less valid measures
2) try to calculate the angle and the difference on ground and take the Pythagoras permanently for all measures (don't know a math algorithm) -> lot of ugly math

nhadrian
Posts: 421
Joined: Tue Oct 25, 2011 9:25 am

Re: Integration of SRF08 I2C sonar sensor (done)

Post by nhadrian »

Dear Waldmesch.

You have right, absolutely. BUT!

Who had already many flights with Baro related Altitude hold possibly had the experience that during windy day or fast forward flight, altitude could be dropped down with 2-3 meters temporary. This is because of constrained throttle behaviour, otherwise altitude always overruns. So, I think that sonar could be really useful to avoid these drops, because much "harder" PIDs could be used. When example copter reaches 20 deg angle (mine flies really fast with 20deg.....), the error will be around 7-8 percent. That means 17 cm on 2m altitude. I think this is absolutelly acceptable, because angle functions takes much processor time.

So personally my goal would be that under 5m sonar should help prevent unwanted altitude drops, with a not so complicated calculation method. Once I'll have 6m range, I'll try to modify 2.0 PV1.

If I'm not right, please correct.

BR
Adrian

PS.: I have modified the 2.0 PV code, now, when armed, actual baro alt becomes "0" and altitude during flight is measured compared to arm level. With MS5611, alt hold works as well as without this code, but now baro and sonar alt becomes comparable. And because "0" is stored at every arming, temperature drift is minimalized. I had today a flight, I hoovered (and made some forward flights) in 9 C degrees with MS5611 without noticeable changes in target altitude.

Waldmensch
Posts: 31
Joined: Sat Dec 31, 2011 12:10 am

Re: Integration of SRF08 I2C sonar sensor (done)

Post by Waldmensch »

I have mounted my SRF02 right now. Could you upload your modified 2.0 pre1 here? Or post the actual Version of your changes? Or can I use the changes posted in first Post on 2.0?

Related to the angle calculations - was just an idea. I don't know but maybe on a to large Angle ACC and leveling maybe is disabled anyway

nhadrian
Posts: 421
Joined: Tue Oct 25, 2011 9:25 am

Re: Integration of SRF08 I2C sonar sensor (done)

Post by nhadrian »

I used code parts from the first post. Only change is to add Sonaralt in the begining of multiwii.ino:

Code: Select all

#ifdef SRF08
   static int16_t SonarAlt = 0; // distance, cm (0..SONAR_MAX_DISTANCE)
#endif


and insert this line into the sonar sensor code for SonarAlt calculation:

Code: Select all

      if(srf08_ctx.current >= srf08_ctx.sensors)
        srf08_ctx.state=1;
        SonarAlt = srf08_ctx.range[0];


now you can put SonarAlt to a debug channel and see how it works...

for me it looks like it works under 60cm, so range doesn't set properly. Guru_florida, could you help what to modify for 6m range? I tried to fing the part fo your code where range is set, but till now I couldn't... :(

nhadrian
Posts: 421
Joined: Tue Oct 25, 2011 9:25 am

Re: Integration of SRF08 I2C sonar sensor (done)

Post by nhadrian »

nhadrian wrote:I used code parts from the first post. Only change is to add Sonaralt in the begining of multiwii.ino:

Code: Select all

#ifdef SRF08
   static int16_t SonarAlt = 0; // distance, cm (0..SONAR_MAX_DISTANCE)
#endif


and insert this line into the sonar sensor code for SonarAlt calculation (i added in twice, where this if line appears, because I don't know which to choose):

Code: Select all

      if(srf08_ctx.current >= srf08_ctx.sensors)
        srf08_ctx.state=1;
        SonarAlt = srf08_ctx.range[0];


now you can put SonarAlt to a debug channel and see how it works...

for me it looks like it works under 60cm, so range doesn't set properly. Guru_florida, could you help what to modify for 6m range? I tried to fing the part fo your code where range is set, but till now I couldn't... :(

Waldmensch
Posts: 31
Joined: Sat Dec 31, 2011 12:10 am

Re: Integration of SRF08 I2C sonar sensor (done)

Post by Waldmensch »

Where to add the second code block? Which module?

This block, if both lines below are depending from IF, have to be in brackets

Code: Select all

      if(srf08_ctx.current >= srf08_ctx.sensors){
        srf08_ctx.state=1;
        SonarAlt = srf08_ctx.range[0];
}


otherwise only the first line is depending of the IF above

nhadrian
Posts: 421
Joined: Tue Oct 25, 2011 9:25 am

Re: Integration of SRF08 I2C sonar sensor (done)

Post by nhadrian »

Here is my code, with this, you'll see the SonarAlt values in debug2.
Additionally, when armed, Baroalt will be "0" and Alt will be measured from this point.

BR
Adrian

PS.: that if statement works, don't aks how... :D
Attachments
Multiwii 2.0 Sonar.zip
(73.36 KiB) Downloaded 779 times

Waldmensch
Posts: 31
Joined: Sat Dec 31, 2011 12:10 am

Re: Integration of SRF08 I2C sonar sensor (done)

Post by Waldmensch »

Hmmm. I see a kind of init (Sonar LED flashing some times) on giving power to the board. But then nothing is happen. Debug2 is permanently on zero in GUI, also if armed. Do I have to define a separate AUX Switch? I only have ACC on AUX1 high and Baro on AUX2 high - nothing else.

Or could it be that it not work with SRF02?

nhadrian
Posts: 421
Joined: Tue Oct 25, 2011 9:25 am

Re: Integration of SRF08 I2C sonar sensor (done)

Post by nhadrian »

When using my code without any modifications, debug2 should show the sonar range value without anything enabled or disabled, armed or not. Please note that I defined the sonar inside Quadrino part in def.h, so probably you should define it somewhere else if not on quadrino board...

nhadrian
Posts: 421
Joined: Tue Oct 25, 2011 9:25 am

Re: Integration of SRF08 I2C sonar sensor (done)

Post by nhadrian »

Dear Guru Florida!

Now my srf08 works under 60 cm, over 60 cm valkues become crazy. Maybe some range setup issue. Could you please check out yours?
In the SRF08 manual I see that after init, range can be set up on register nr. 2. I think a sonar_range_setup or something similar should be defined and used during this init at sensor startup. For 6m it's 0xEC . Could you please add this to the code?

BTW, many thanks to your job again!!!!!!!!!

BR
Adrian

Waldmensch
Posts: 31
Joined: Sat Dec 31, 2011 12:10 am

Re: Integration of SRF08 I2C sonar sensor (done)

Post by Waldmensch »

I have defined it in config.h next to the Baro

Code: Select all

/* I2C accelerometer */
//#define MMA745
//#define ADXL345
//#define BMA020
//#define BMA180
//#define NUNCHACK  // if you want to use the nunckuk as a standalone I2C ACC without WMP
//#define LIS3LV02
#define LSM303DLx_ACC

/* I2C barometer */
#define BMP085
#define SRF08
//#define MS561101BA

/* I2C magnetometer */
//#define HMC5843
#define HMC5883
//#define AK8975


I have copy/pasted your full code (except config.h which I have modified manually) module by module in my sketch because i use the old Arduino IDE which uses *.pde instead of *.ino

Waldmensch
Posts: 31
Joined: Sat Dec 31, 2011 12:10 am

Re: Integration of SRF08 I2C sonar sensor (done)

Post by Waldmensch »

I think the SRF02 is not compatible with SRF08 :cry:

Waldmensch
Posts: 31
Joined: Sat Dec 31, 2011 12:10 am

Re: Integration of SRF08 I2C sonar sensor (done)

Post by Waldmensch »

Seems I got it running on SRF02 by undefine SONAR_MULTICAST_PING

then the #else of this code is used. This gives a compile error because SonarAlt = srf08_ctx.range[0]; must be outside (below of the if/else). It works as posted below.

Code: Select all

#if defined(SONAR_MULTICAST_PING)
    case 2:
      // send a ping via the general broadcast address
      i2c_writeReg(0, SRF08_REV_COMMAND, 0x51);  // start ranging, result in centimeters
      srf08_ctx.state++;
      srf08_ctx.deadline += SRF08_RANGE_WAIT;
      break;
    case 3:
      srf08_ctx.range[srf08_ctx.current] = i2c_readReg16( SRF08_SENSOR_FIRST+(srf08_ctx.current<<1), SRF08_ECHO_RANGE);
      srf08_ctx.current++;
      if(srf08_ctx.current >= srf08_ctx.sensors)
        srf08_ctx.state=1;
        // update alexmos's variables
      SonarAlt = srf08_ctx.range[0];
      break;
#else
    case 2:
      // send a ping to the current sensor
      i2c_writeReg(SRF08_SENSOR_FIRST+(srf08_ctx.current<<1), SRF08_REV_COMMAND, 0x51);  // start ranging, result in centimeters
      srf08_ctx.state++;
      srf08_ctx.deadline += SRF08_RANGE_WAIT;
      break;
    case 3:
      srf08_ctx.range[srf08_ctx.current] = i2c_readReg16(SRF08_SENSOR_FIRST+(srf08_ctx.current<<1), SRF08_ECHO_RANGE);
      srf08_ctx.current++;
      if(srf08_ctx.current >= srf08_ctx.sensors)
        srf08_ctx.state=1;
      else
        srf08_ctx.state=2;
       
      SonarAlt = srf08_ctx.range[0];
      break;
#endif

Waldmensch
Posts: 31
Joined: Sat Dec 31, 2011 12:10 am

Re: Integration of SRF08 I2C sonar sensor (done)

Post by Waldmensch »

Had a nice crash right now - fortunately lost just 2 Props. By activating Baro the copter move up like a rocket. I don't know why.

What i see in GUI: If Sonar is mounted to close to ground it delivers values around 255. If this value is aplied to the Baro on arming it is not a good idea. Now I have mounted the sonar around 10-12cm over ground. In GUI I see 26 which is the minimum sensor range. Not exact but better then 250 which seem an error value. I have no higher position (sensor to ground) on my copter.

So applying the Sonar value to Baro on arming should only done if Sonar value > 26 and < 50 (or minimum range + safety to maximum expected sonar mounting height over ground)

What PID is recommended for Sonar usage? This moment I have defaults for "Level" P: 9,0 - I: 0,045 - D: 100

nhadrian
Posts: 421
Joined: Tue Oct 25, 2011 9:25 am

Re: Integration of SRF08 I2C sonar sensor (done)

Post by nhadrian »

If you check out the code of jevermeister posted by goru_florida, you can see that there is difficult relation between sonar and baro values, to avoid error command. So it is not as easy as replacing baro values with sonar one.......I think you have to be patient till final solution arrives! So do I....

Now I only use Baro with success, sonar only gives the debug2 values, for monitoring of sonar working.

Waldmensch
Posts: 31
Joined: Sat Dec 31, 2011 12:10 am

Re: Integration of SRF08 I2C sonar sensor (done)

Post by Waldmensch »

What I see that with mounting SRF I get Errors on I2C. Maybe through the additional pullups in parallel to BMA020. (parallel circuit of R reduces total R)

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

Re: Integration of SRF08 I2C sonar sensor (done)

Post by dongfang »

Hi All,

I suppose some of you have met the motor noise interference issue like me. Though I'm not using an I2C sonar, but obviously my sonar won't work well if it leave the ground too high, as the sonar generate random distance data when the motor is running. (Like nhadrian said, his sonar only work under 60cm.)

This is a hardware issue, and I'm affraid whether there are any software solution.

nhadrian
Posts: 421
Joined: Tue Oct 25, 2011 9:25 am

Re: Integration of SRF08 I2C sonar sensor (done)

Post by nhadrian »

dongfang wrote:Hi All,

I suppose some of you have met the motor noise interference issue like me. Though I'm not using an I2C sonar, but obviously my sonar won't work well if it leave the ground too high, as the sonar generate random distance data when the motor is running. (Like nhadrian said, his sonar only work under 60cm.)

This is a hardware issue, and I'm affraid whether there are any software solution.


I made all my tests till now on my desktop without motors running.
I know that SRF08 sensor has adjustable range, first, range setup should tried. So I'm waiting for guru_florida inserting the range setup part into the code.
After setting to the full range and still having issues over 60 cm, we'll have to think on it!

BTW, ar-drone 1 uses sonar for alt hold, so T think sensors itself should work on multirotors!

BR
Adrian

Waldmensch
Posts: 31
Joined: Sat Dec 31, 2011 12:10 am

Re: Integration of SRF08 I2C sonar sensor (done)

Post by Waldmensch »

test it with disabled multicast:

Code: Select all

//#define SONAR_MULTICAST_PING


if it not compile check the code i posted some posts above. I havn't this 60cm issue

nhadrian
Posts: 421
Joined: Tue Oct 25, 2011 9:25 am

Re: Integration of SRF08 I2C sonar sensor (done)

Post by nhadrian »

Waldmensch wrote:test it with disabled multicast:

Code: Select all

//#define SONAR_MULTICAST_PING


if it not compile check the code i posted some posts above. I havn't this 60cm issue


I tried but still the same. Under 60-65 mm it works awesome, over values become randomly.
So I'm almost sure there is a range setup problem here.

BR
Adrian

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

Re: Integration of SRF08 I2C sonar sensor (done)

Post by guru_florida »

I went through the datasheet again and the range is set to maximum by default. I have the SRF08 and I dont have any issues over 60cm. I get up to 100 in my office here and it still pretty steady. There is lots of things to bounce off of, but it's within an acceptable range for sure. I do have my sonar shielded from the ESC. I have a solid piece of stainless steel over the back, insulated by electrical tape of course!

Here are the zipped up source files for MultiWii_2_0_preversion2 (the latest). Copy these files into your existing MultiWii_2_0_preversion2 directory. (This zip is not a complete copy of the source, only changed files.)

In this version Sonar is not used for AltHold. It is just displayed in debug3. I am working on fusing Baro and Sonar now.

Colin


Code Change List:
Sensors.pde:1191 Added Sonar sensor code
Sensors.pde:1388 (end) Added Sonar_init() call to initSensors() function
MultiWii.pde:624 Added Sonar_update() call
def.h:617 Added #ifdef testing for SRF08 define to enable "#define SONAR 1"
config.h:<eof> Added a few Sonar parameters

Careful, def.h is in there to, did you make any custom changes to def.h? You will also want to add the following parameters to the bottom of config.h:

// use the Devantech SRF i2c sensors, SRF08, SRF02
// (for now, there is no difference in the SRF0x code, but we may want to differentiate in the future.)
//#define SRF02
#define SRF08
//#define SRF10
//#define SRF235
Attachments
MultiWii_2_0_pv2-sonar(changes-only).zip
(28.61 KiB) Downloaded 712 times

nhadrian
Posts: 421
Joined: Tue Oct 25, 2011 9:25 am

Re: Integration of SRF08 I2C sonar sensor (done)

Post by nhadrian »

guru_florida wrote:In this version Sonar is not used for AltHold. It is just displayed in debug3. I am working on fusing Baro and Sonar now.


May I ask you what is your basic idea on fusing?

nhadrian
Posts: 421
Joined: Tue Oct 25, 2011 9:25 am

Re: Integration of SRF08 I2C sonar sensor (done)

Post by nhadrian »

Hi,

here is a sample graph about my sensor. Red means EstAlt (from Baro), green mneans SonarAlt in cm. As you can see, under 60-70 cm it is OK, but at 1-1,5 m it looks like it looses Echo or such. Any IDEA?

Image

BR
Adrian

alexmos
Posts: 108
Joined: Tue Aug 09, 2011 12:12 pm

Re: Integration of SRF08 I2C sonar sensor (done)

Post by alexmos »

Hi! Below few words about my experience working with sonar.I have an analog HC-SR04 but think it is suitable for all sonars.
1. A small pause (minimum 50 millisec) between two succesfull measurement strongly required. If no - echo from one will interference another. Check specifications, is this pause implemented internally or not?
2. We need to know if measure was wrong, and keep an error counter to softly swith to baro, reduce PID's and so on.
3. I was discovered strange error in real flight: above 1 meter, sonar did not reported error but gives wrong values. After some investigation, I have found that digital servo, connected on the same power BEC, cause this. After disconnecting servo, sonar start working up to 4 meters.
4. Sensivity depends on angle. Up to 0.5 meter it works good even under 45 degrees, but with increasing altitude, the working angle decreases. This another reason why we must know the percentage of errors.
5. If any frame constructions are inside sonar beam, it works unstable. Try to remove all obstacles and move sonar as far as possible from side elements. (In specification beam may be 60 degree, but it can 'see' nearby objects under 180 degree!)

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

Re: Integration of SRF08 I2C sonar sensor (done)

Post by guru_florida »

Thanks alexmos for your input. You have some great points.
1. Yes, the digital sensor requires 65ms between ranging. I am waiting 80ms, and this parameters is set in SRF08_RANGE_WAIT =80000.
3. This is a great point! The sonar sensors use a sensitive op-amp. other electrical noise could get coupled to to this. That's why I insulated mine with steel plate from inductive noise. But also it needs to have clean power, cleaner then the multiwii controller needs probably. You could try a small choke/inductor and a cap on the power line. This should filter any of this noise.
4. This makes sense. Generally, sound travels out like a pebble in a pond, so there wont be much difference in distance measured with an angled attitude. But the sonar sensor is more focused then typical sounds, and incident reflections are stronger, so attitude still has some affect. Alexmos handled this by using a gain factor for angled attitudes and I found it didnt require much gain to offset the angle difference.
---------------------

There is gain setting in the SRF08. Maybe turning down the gain would keep it from tripping on the electrical interference or sideway reflections. Also, the SRF08 can detect multiple ping reflections. Maybe I should check all values and find the one closest to the last one - that would eliminate a lot of interference.

nhadrian: Nice log btw! Good scientific approach. Is this 60cm issue happening with the quad off, no spinning motors etc?

nhadrian
Posts: 421
Joined: Tue Oct 25, 2011 9:25 am

Re: Integration of SRF08 I2C sonar sensor (done)

Post by nhadrian »

Thanks. I made this test when keeping copter in my hand in my hand, no spinning motors at all.

Post Reply