From d5ddc2b4897f11dcd9a15a792c4a586fc5e6889d Mon Sep 17 00:00:00 2001
From: ChristophTobler <christoph.tob@gmail.com>
Date: Tue, 19 Sep 2017 14:11:27 +0200
Subject: [PATCH] srf02_i2c: update for new orientation convention

add possibility to specify orientation and adapt to new defaults and use px4_getopt
---
 src/drivers/srf02_i2c/srf02_i2c.cpp | 47 +++++++++++++++++++++--------
 1 file changed, 35 insertions(+), 12 deletions(-)

diff --git a/src/drivers/srf02_i2c/srf02_i2c.cpp b/src/drivers/srf02_i2c/srf02_i2c.cpp
index 356d432cab..e071eed8c4 100644
--- a/src/drivers/srf02_i2c/srf02_i2c.cpp
+++ b/src/drivers/srf02_i2c/srf02_i2c.cpp
@@ -41,6 +41,7 @@
 
 #include <px4_config.h>
 #include <px4_defines.h>
+#include <px4_getopt.h>
 
 #include <drivers/device/i2c.h>
 
@@ -101,7 +102,8 @@
 class SRF02_I2C : public device::I2C
 {
 public:
-	SRF02_I2C(int bus = SRF02_I2C_BUS, int address = SRF02_I2C_BASEADDR);
+	SRF02_I2C(uint8_t rotation = distance_sensor_s::ROTATION_DOWNWARD_FACING, int bus = SRF02_I2C_BUS,
+		  int address = SRF02_I2C_BASEADDR);
 	virtual ~SRF02_I2C();
 
 	virtual int 		init();
@@ -118,6 +120,7 @@ protected:
 	virtual int			probe();
 
 private:
+	uint8_t _rotation;
 	float				_min_distance;
 	float				_max_distance;
 	work_s				_work;
@@ -196,8 +199,9 @@ private:
  */
 extern "C" { __EXPORT int srf02_i2c_main(int argc, char *argv[]);}
 
-SRF02_I2C::SRF02_I2C(int bus, int address) :
+SRF02_I2C::SRF02_I2C(uint8_t rotation, int bus, int address) :
 	I2C("MB12xx", SRF02_DEVICE_PATH, bus, address, 100000),
+	_rotation(rotation),
 	_min_distance(SRF02_MIN_DISTANCE),
 	_max_distance(SRF02_MAX_DISTANCE),
 	_reports(nullptr),
@@ -573,7 +577,7 @@ SRF02_I2C::collect()
 	struct distance_sensor_s report;
 	report.timestamp = hrt_absolute_time();
 	report.type = distance_sensor_s::MAV_DISTANCE_SENSOR_ULTRASOUND;
-	report.orientation = 8;
+	report.orientation = _rotation;
 	report.current_distance = distance_m;
 	report.min_distance = get_minimum_distance();
 	report.max_distance = get_maximum_distance();
@@ -722,7 +726,7 @@ namespace  srf02_i2c
 
 SRF02_I2C	*g_dev;
 
-void	start();
+void	start(uint8_t rotation);
 void	stop();
 void	test();
 void	reset();
@@ -732,7 +736,7 @@ void	info();
  * Start the driver.
  */
 void
-start()
+start(uint8_t rotation)
 {
 	int fd;
 
@@ -741,7 +745,7 @@ start()
 	}
 
 	/* create the driver */
-	g_dev = new SRF02_I2C(SRF02_I2C_BUS);
+	g_dev = new SRF02_I2C(rotation, SRF02_I2C_BUS);
 
 	if (g_dev == nullptr) {
 		goto fail;
@@ -903,38 +907,57 @@ info()
 int
 srf02_i2c_main(int argc, char *argv[])
 {
+	// check for optional arguments
+	int ch;
+	int myoptind = 1;
+	const char *myoptarg = NULL;
+	uint8_t rotation = distance_sensor_s::ROTATION_DOWNWARD_FACING;
+
+
+	while ((ch = px4_getopt(argc, argv, "R:", &myoptind, &myoptarg)) != EOF) {
+		switch (ch) {
+		case 'R':
+			rotation = (uint8_t)atoi(myoptarg);
+			PX4_INFO("Setting distance sensor orientation to %d", (int)rotation);
+			break;
+
+		default:
+			PX4_WARN("Unknown option!");
+		}
+	}
+
 	/*
 	 * Start/load the driver.
 	 */
-	if (!strcmp(argv[1], "start")) {
-		srf02_i2c::start();
+	if (!strcmp(argv[myoptind], "start")) {
+		srf02_i2c::start(rotation);
 	}
 
 	/*
 	 * Stop the driver
 	 */
-	if (!strcmp(argv[1], "stop")) {
+	if (!strcmp(argv[myoptind], "stop")) {
 		srf02_i2c::stop();
 	}
 
 	/*
 	 * Test the driver/device.
 	 */
-	if (!strcmp(argv[1], "test")) {
+	if (!strcmp(argv[myoptind], "test")) {
 		srf02_i2c::test();
 	}
 
 	/*
 	 * Reset the driver.
 	 */
-	if (!strcmp(argv[1], "reset")) {
+	if (!strcmp(argv[myoptind], "reset")) {
 		srf02_i2c::reset();
 	}
 
 	/*
 	 * Print driver information.
 	 */
-	if (!strcmp(argv[1], "info") || !strcmp(argv[1], "status")) {
+	if (!strcmp(argv[myoptind], "info") || !strcmp(argv[myoptind], "status")) {
 		srf02_i2c::info();
 	}