Removed duplicate and fix SRF02 driver

ROMFS/px4fmu_common/init.d/rc.sensors

@@ -366,6 +366,12 @@ then
 	mb12xx start
 fi
 
+# srf02 sonar sensor
+if param greater SENS_EN_SRF02 0
+then
+	srf02 start
+fi
+
 # teraranger one tof sensor
 if param greater SENS_EN_TRANGER 0
 then

src/drivers/drv_range_finder.h

@@ -46,7 +46,6 @@
 
 #define RANGE_FINDER_BASE_DEVICE_PATH	"/dev/range_finder"
 #define RANGE_FINDER0_DEVICE_PATH	"/dev/range_finder0"
-#define MB12XX_MAX_RANGEFINDERS	12	// Maximum number of Maxbotix sensors on bus
 
 /*
  * ioctl() definitions

src/drivers/mb12xx/mb12xx.cpp

@@ -81,6 +81,7 @@
 #define MB12XX_DEVICE_PATH	"/dev/mb12xx"
 
 /* MB12xx Registers addresses */
+#define MB12XX_MAX_RANGEFINDERS	12	// Maximum number of Maxbotix sensors on bus
 
 #define MB12XX_TAKE_RANGE_REG	0x51		/* Measure range Register */
 #define MB12XX_SET_ADDRESS_1	0xAA		/* Change address 1 Register */

src/drivers/srf02/srf02.cpp

@@ -35,6 +35,7 @@
  * @file srf02.cpp
  *
  * Driver for the SRF02 sonar range finder adapted from the Maxbotix sonar range finder driver (mb12xx).
+ * Based on the documentation of the srf08 found at http://www.robot-electronics.co.uk/htm/srf08tech.html
  */
 
 #include <px4_config.h>
@@ -76,22 +77,37 @@
 
 /* Configuration Constants */
 #define SRF02_BUS 		PX4_I2C_BUS_EXPANSION
-#define SRF02_BASEADDR 	0x70 /* 7-bit address. 8-bit address is 0xE0 */
+#define SRF02_BASEADDR 	(0xFE >> 1) /* 7-bit address. 8-bit address is 0xFE */
 #define SRF02_DEVICE_PATH	"/dev/srf02"
+#define SRF02_MAX_RANGEFINDERS	16
 
-/* MB12xx Registers addresses */
+/* SRF02 Registers addresses */
 
-#define SRF02_TAKE_RANGE_REG	0x51		/* Measure range Register */
-#define SRF02_SET_ADDRESS_0	0xA0		/* Change address 0 Register */
-#define SRF02_SET_ADDRESS_1	0xAA		/* Change address 1 Register */
-#define SRF02_SET_ADDRESS_2	0xA5		/* Change address 2 Register */
+/*Write*/
+#define SRF02_CMD_REG 0x00 /* Command Register */
+#define SRF02_GAIN_REG 0x01 /* Max Gain Register */
+#define SRF02_RGN_REG 0x02 /* Range Register */
+/*Read*/
+#define SRF02_VERSION 0x00 /* Software Revision */
+#define SRF02_LIGHT 0x01 /* Light Sensor */
+#define SRF02_ECHO_1 0x02 /*1st Echo High Byte */
+
+
+
+/* SRF02 Commands */
+#define SRF02_RANGING_MODE_IN	0x50 /* Ranging Mode - Result in inch */
+#define SRF02_RANGING_MODE_CM	0x51 /* Ranging Mode - Result in centimeters */
+#define SRF02_RANGING_MODE_MS	0x52 /* Ranging Mode - Result in micro-seconds */
+#define SRF02_SET_ADDRESS_0	0xA0		/* 1st in sequence to change I2C address */
+#define SRF02_SET_ADDRESS_1	0xAA		/* 2st in sequence to change I2C address */
+#define SRF02_SET_ADDRESS_2	0xA5		/* 3st in sequence to change I2C address */
 
 /* Device limits */
 #define SRF02_MIN_DISTANCE 	(0.20f)
-#define SRF02_MAX_DISTANCE 	(7.65f)
+#define SRF02_MAX_DISTANCE 	(6.00f)
 
-#define SRF02_CONVERSION_INTERVAL 	100000 /* 60ms for one sonar */
-#define TICKS_BETWEEN_SUCCESIVE_FIRES 	100000 /* 30ms between each sonar measurement (watch out for interference!) */
+#define SRF02_CONVERSION_INTERVAL 	100000 /* 67ms or longer for default range and gain setting*/
+#define TICKS_BETWEEN_SUCCESIVE_FIRES 	100000 /* To Do (watch out for interference!) */
 
 
 #ifndef CONFIG_SCHED_WORKQUEUE
@@ -116,7 +132,7 @@ class SRF02 : public device::I2C
 	void				print_info();
 
 protected:
-	virtual int			probe();
+//  virtual int			probe();
 
 private:
 	uint8_t _rotation;
@@ -199,7 +215,7 @@ class SRF02 : public device::I2C
 extern "C" __EXPORT int srf02_main(int argc, char *argv[]);
 
 SRF02::SRF02(uint8_t rotation, int bus, int address) :
-	I2C("MB12xx", SRF02_DEVICE_PATH, bus, address, 100000),
+	I2C("SRF02", SRF02_DEVICE_PATH, bus, address, 100000),
 	_rotation(rotation),
 	_min_distance(SRF02_MIN_DISTANCE),
 	_max_distance(SRF02_MAX_DISTANCE),
@@ -278,17 +294,17 @@ SRF02::init()
 	// XXX we should find out why we need to wait 200 ms here
 	usleep(200000);
 
-	/* check for connected rangefinders on each i2c port:
-	   We start from i2c base address (0x70 = 112) and count downwards
-	   So second iteration it uses i2c address 111, third iteration 110 and so on*/
-	for (unsigned counter = 0; counter <= MB12XX_MAX_RANGEFINDERS; counter++) {
-		_index_counter = SRF02_BASEADDR - counter;	/* set temp sonar i2c address to base adress - counter */
-		set_address(_index_counter);			/* set I2c port to temp sonar i2c adress */
+	/* check for connected srf02 at each possible i2c address. Counting down from the highest */
+	for (unsigned counter = 0; counter <= SRF02_MAX_RANGEFINDERS; counter++) {
+		_index_counter = SRF02_BASEADDR - counter;
+		set_address(_index_counter);
+
+		DEVICE_DEBUG("Looking for SRF02 at 0x%02x", _index_counter << 1);
 		int ret2 = measure();
 
 		if (ret2 == 0) { /* sonar is present -> store address_index in array */
 			addr_ind.push_back(_index_counter);
-			DEVICE_DEBUG("sonar added");
+			DEVICE_DEBUG("Sonar added");
 			_latest_sonar_measurements.push_back(200);
 		}
 	}
@@ -306,10 +322,10 @@ SRF02::init()
 
 	/* show the connected sonars in terminal */
 	for (unsigned i = 0; i < addr_ind.size(); i++) {
-		DEVICE_LOG("sonar %d with address %d added", (i + 1), addr_ind[i]);
+		DEVICE_LOG("Sonar %d at 0x%02x added", (i + 1), addr_ind[i]);
 	}
 
-	DEVICE_DEBUG("Number of sonars connected: %d", addr_ind.size());
+	DEVICE_DEBUG("At total of %d are connected", addr_ind.size());
 
 	ret = OK;
 	/* sensor is ok, but we don't really know if it is within range */
@@ -318,12 +334,13 @@ SRF02::init()
 	return ret;
 }
 
+/*
 int
 SRF02::probe()
 {
-	return measure();
+  return measure();
 }
-
+*/
 void
 SRF02::set_minimum_distance(float min)
 {
@@ -535,8 +552,8 @@ SRF02::measure()
 	 */
 
 	uint8_t cmd[2];
-	cmd[0] = 0x00;
-	cmd[1] = SRF02_TAKE_RANGE_REG;
+	cmd[0] = SRF02_CMD_REG;
+	cmd[1] = SRF02_RANGING_MODE_CM;
 	ret = transfer(cmd, 2, nullptr, 0);
 
 	if (OK != ret) {
@@ -557,7 +574,7 @@ SRF02::collect()
 
 	/* read from the sensor */
 	uint8_t val[2] = {0, 0};
-	uint8_t cmd = 0x02;
+	uint8_t cmd = SRF02_ECHO_1;
 	perf_begin(_sample_perf);
 
 	ret = transfer(&cmd, 1, nullptr, 0);
@@ -816,18 +833,20 @@ test()
 	sz = read(fd, &report, sizeof(report));
 
 	if (sz != sizeof(report)) {
-		err(1, "immediate read failed");
+		err(1, "Single measurement test failed");
 	}
 
-	warnx("single read");
-	warnx("measurement: %0.2f m", (double)report.current_distance);
-	warnx("time:        %llu", report.timestamp);
+	warnx("Single Measurement Test");
+	warnx("Distance:\t%0.2f m", (double)report.current_distance);
+	warnx("time:\t%llu", report.timestamp);
 
 	/* start the sensor polling at 2Hz */
 	if (OK != ioctl(fd, SENSORIOCSPOLLRATE, 2)) {
 		errx(1, "failed to set 2Hz poll rate");
 	}
 
+	warnx("Periodic Polling Test");
+
 	/* read the sensor 5x and report each value */
 	for (unsigned i = 0; i < 5; i++) {
 		struct pollfd fds;
@@ -848,19 +867,17 @@ test()
 			err(1, "periodic read failed");
 		}
 
-		warnx("periodic read %u", i);
-		warnx("valid %u", (float)report.current_distance > report.min_distance
-		      && (float)report.current_distance < report.max_distance ? 1 : 0);
-		warnx("measurement: %0.3f", (double)report.current_distance);
-		warnx("time:        %llu", report.timestamp);
+		warnx("Measurement [%u]", i);
+		warnx("Distance:\t%0.2f m", (double)report.current_distance);
+		warnx("time:\t%llu", report.timestamp);
 	}
 
 	/* reset the sensor polling to default rate */
 	if (OK != ioctl(fd, SENSORIOCSPOLLRATE, SENSOR_POLLRATE_DEFAULT)) {
 		errx(1, "failed to set default poll rate");
 	}
 
-	errx(0, "PASS");
+	warnx("Test Successful");
 }
 
 /**
@@ -904,6 +921,14 @@ info()
 
 } /* namespace */
 
+static void
+srf02_usage()
+{
+	PX4_INFO("usage: srf02 command");
+	PX4_INFO("command:");
+	PX4_INFO("\tstart|stop|reset|test");
+}
+
 int
 srf02_main(int argc, char *argv[])
 {
@@ -961,5 +986,7 @@ srf02_main(int argc, char *argv[])
 		srf02::info();
 	}
 
-	errx(1, "unrecognized command, try 'start', 'test', 'reset' or 'info'");
+	srf02_usage();
+
+	return PX4_OK;
 }