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
:D:D
