From 4f683db7328e23374467a10d880c7c0f4e40d982 Mon Sep 17 00:00:00 2001
From: ChristophTobler <christoph.tob@gmail.com>
Date: Tue, 19 Sep 2017 13:45:56 +0200
Subject: [PATCH] sf0x: update for new orientation convention

add possibility to specify orientation and adapt to new defaults and use px4_getopt
---
 src/drivers/sf0x/sf0x.cpp | 50 ++++++++++++++++++++++++++++-----------
 1 file changed, 36 insertions(+), 14 deletions(-)

diff --git a/src/drivers/sf0x/sf0x.cpp b/src/drivers/sf0x/sf0x.cpp
index afee467b70..3ccd4ddfd5 100644
--- a/src/drivers/sf0x/sf0x.cpp
+++ b/src/drivers/sf0x/sf0x.cpp
@@ -40,6 +40,7 @@
  */
 
 #include <px4_config.h>
+#include <px4_getopt.h>
 
 #include <sys/types.h>
 #include <stdint.h>
@@ -89,7 +90,7 @@
 class SF0X : public device::CDev
 {
 public:
-	SF0X(const char *port = SF0X_DEFAULT_PORT);
+	SF0X(const char *port = SF0X_DEFAULT_PORT, uint8_t rotation = distance_sensor_s::ROTATION_DOWNWARD_FACING);
 	virtual ~SF0X();
 
 	virtual int 			init();
@@ -107,6 +108,7 @@ protected:
 
 private:
 	char 				_port[20];
+	uint8_t _rotation;
 	float				_min_distance;
 	float				_max_distance;
 	int                 _conversion_interval;
@@ -177,8 +179,9 @@ private:
  */
 extern "C" __EXPORT int sf0x_main(int argc, char *argv[]);
 
-SF0X::SF0X(const char *port) :
+SF0X::SF0X(const char *port, uint8_t rotation) :
 	CDev("SF0X", RANGE_FINDER0_DEVICE_PATH),
+	_rotation(rotation),
 	_min_distance(0.30f),
 	_max_distance(40.0f),
 	_conversion_interval(83334),
@@ -631,7 +634,7 @@ SF0X::collect()
 
 	report.timestamp = hrt_absolute_time();
 	report.type = distance_sensor_s::MAV_DISTANCE_SENSOR_LASER;
-	report.orientation = 8;
+	report.orientation = _rotation;
 	report.current_distance = distance_m;
 	report.min_distance = get_minimum_distance();
 	report.max_distance = get_maximum_distance();
@@ -789,7 +792,7 @@ namespace sf0x
 
 SF0X	*g_dev;
 
-void	start(const char *port);
+void	start(const char *port, uint8_t rotation);
 void	stop();
 void	test();
 void	reset();
@@ -799,7 +802,7 @@ void	info();
  * Start the driver.
  */
 void
-start(const char *port)
+start(const char *port, uint8_t rotation)
 {
 	int fd;
 
@@ -808,7 +811,7 @@ start(const char *port)
 	}
 
 	/* create the driver */
-	g_dev = new SF0X(port);
+	g_dev = new SF0X(port, rotation);
 
 	if (g_dev == nullptr) {
 		goto fail;
@@ -972,43 +975,62 @@ info()
 int
 sf0x_main(int argc, char *argv[])
 {
+	// check for optional arguments
+	int ch;
+	uint8_t rotation = distance_sensor_s::ROTATION_DOWNWARD_FACING;
+	int myoptind = 1;
+	const char *myoptarg = NULL;
+
+
+	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")) {
-		if (argc > 2) {
-			sf0x::start(argv[2]);
+	if (!strcmp(argv[myoptind], "start")) {
+		if (argc > myoptind + 1) {
+			sf0x::start(argv[myoptind + 1], rotation);
 
 		} else {
-			sf0x::start(SF0X_DEFAULT_PORT);
+			sf0x::start(SF0X_DEFAULT_PORT, rotation);
 		}
 	}
 
 	/*
 	 * Stop the driver
 	 */
-	if (!strcmp(argv[1], "stop")) {
+	if (!strcmp(argv[myoptind], "stop")) {
 		sf0x::stop();
 	}
 
 	/*
 	 * Test the driver/device.
 	 */
-	if (!strcmp(argv[1], "test")) {
+	if (!strcmp(argv[myoptind], "test")) {
 		sf0x::test();
 	}
 
 	/*
 	 * Reset the driver.
 	 */
-	if (!strcmp(argv[1], "reset")) {
+	if (!strcmp(argv[myoptind], "reset")) {
 		sf0x::reset();
 	}
 
 	/*
 	 * Print driver information.
 	 */
-	if (!strcmp(argv[1], "info") || !strcmp(argv[1], "status")) {
+	if (!strcmp(argv[myoptind], "info") || !strcmp(argv[1], "status")) {
 		sf0x::info();
 	}