1 files changed, 58 insertions, 12 deletions

diff --git a/src/drivers/l3gd20/l3gd20.cpp b/src/drivers/l3gd20/l3gd20.cpp
index 2fcbdc0ad..9e5eb00db 100644
--- a/src/drivers/l3gd20/l3gd20.cpp
+++ b/src/drivers/l3gd20/l3gd20.cpp
@@ -54,6 +54,7 @@
 #include <stdio.h>
 #include <math.h>
 #include <unistd.h>
+#include <getopt.h>
 
 #include <systemlib/perf_counter.h>
 #include <systemlib/err.h>
@@ -68,6 +69,7 @@
 
 #include <board_config.h>
 #include <mathlib/math/filter/LowPassFilter2p.hpp>
+#include <lib/conversion/rotation.h>
 
 #define L3GD20_DEVICE_PATH "/dev/l3gd20"
 
@@ -184,7 +186,7 @@ extern "C" { __EXPORT int l3gd20_main(int argc, char *argv[]); }
 class L3GD20 : public device::SPI
 {
 public:
-	L3GD20(int bus, const char* path, spi_dev_e device);
+	L3GD20(int bus, const char* path, spi_dev_e device, enum Rotation rotation);
 	virtual ~L3GD20();
 
 	virtual int		init();
@@ -229,6 +231,8 @@ private:
 	/* true if an L3G4200D is detected */
 	bool	_is_l3g4200d;
 
+	enum Rotation		_rotation;
+
 	/**
 	 * Start automatic measurement.
 	 */
@@ -328,7 +332,7 @@ private:
 	 int 			self_test();
 };
 
-L3GD20::L3GD20(int bus, const char* path, spi_dev_e device) :
+L3GD20::L3GD20(int bus, const char* path, spi_dev_e device, enum Rotation rotation) :
 	SPI("L3GD20", path, bus, device, SPIDEV_MODE3, 11*1000*1000 /* will be rounded to 10.4 MHz, within margins for L3GD20 */),
 	_call_interval(0),
 	_reports(nullptr),
@@ -345,7 +349,8 @@ L3GD20::L3GD20(int bus, const char* path, spi_dev_e device) :
 	_gyro_filter_x(L3GD20_DEFAULT_RATE, L3GD20_DEFAULT_FILTER_FREQ),
 	_gyro_filter_y(L3GD20_DEFAULT_RATE, L3GD20_DEFAULT_FILTER_FREQ),
 	_gyro_filter_z(L3GD20_DEFAULT_RATE, L3GD20_DEFAULT_FILTER_FREQ),
-	_is_l3g4200d(false)
+	_is_l3g4200d(false),
+        _rotation(rotation)                                            
 {
 	// enable debug() calls
 	_debug_enabled = true;
@@ -823,7 +828,7 @@ L3GD20::measure()
 	// if the gyro doesn't have any data ready then re-schedule
 	// for 100 microseconds later. This ensures we don't double
 	// read a value and then miss the next value
-	if (stm32_gpioread(GPIO_EXTI_GYRO_DRDY) == 0) {
+	if (_bus == PX4_SPI_BUS_SENSORS && stm32_gpioread(GPIO_EXTI_GYRO_DRDY) == 0) {
 		perf_count(_reschedules);
 		hrt_call_delay(&_call, 100);
 		return;
@@ -916,6 +921,9 @@ L3GD20::measure()
 	report.y = _gyro_filter_y.apply(report.y);
 	report.z = _gyro_filter_z.apply(report.z);
 
+	// apply user specified rotation
+	rotate_3f(_rotation, report.x, report.y, report.z);
+
 	report.scaling = _gyro_range_scale;
 	report.range_rad_s = _gyro_range_rad_s;
 
@@ -984,7 +992,8 @@ namespace l3gd20
 
 L3GD20	*g_dev;
 
-void	start();
+void	usage();
+void	start(bool external_bus, enum Rotation rotation);
 void	test();
 void	reset();
 void	info();
@@ -993,7 +1002,7 @@ void	info();
  * Start the driver.
  */
 void
-start()
+start(bool external_bus, enum Rotation rotation)
 {
 	int fd;
 
@@ -1001,7 +1010,15 @@ start()
 		errx(0, "already started");
 
 	/* create the driver */
-	g_dev = new L3GD20(PX4_SPI_BUS_SENSORS, L3GD20_DEVICE_PATH, (spi_dev_e)PX4_SPIDEV_GYRO);
+        if (external_bus) {
+#ifdef PX4_SPI_BUS_EXT
+		g_dev = new L3GD20(PX4_SPI_BUS_EXT, L3GD20_DEVICE_PATH, (spi_dev_e)PX4_SPIDEV_EXT_GYRO, rotation);
+#else
+		errx(0, "External SPI not available");
+#endif
+	} else {
+		g_dev = new L3GD20(PX4_SPI_BUS_SENSORS, L3GD20_DEVICE_PATH, (spi_dev_e)PX4_SPIDEV_GYRO, rotation);
+	}
 
 	if (g_dev == nullptr)
 		goto fail;
@@ -1113,35 +1130,64 @@ info()
 	exit(0);
 }
 
+void
+usage()
+{
+	warnx("missing command: try 'start', 'info', 'test', 'reset'");
+	warnx("options:");
+	warnx("    -X    (external bus)");
+	warnx("    -R rotation");
+}
 
 } // namespace
 
 int
 l3gd20_main(int argc, char *argv[])
 {
+	bool external_bus = false;
+	int ch;
+	enum Rotation rotation = ROTATION_NONE;
+
+	/* jump over start/off/etc and look at options first */
+	while ((ch = getopt(argc, argv, "XR:")) != EOF) {
+		switch (ch) {
+		case 'X':
+			external_bus = true;
+			break;
+		case 'R':
+			rotation = (enum Rotation)atoi(optarg);
+			break;
+		default:
+			l3gd20::usage();
+			exit(0);
+		}
+	}
+
+	const char *verb = argv[optind];
+
 	/*
 	 * Start/load the driver.
 
 	 */
-	if (!strcmp(argv[1], "start"))
-		l3gd20::start();
+	if (!strcmp(verb, "start"))
+		l3gd20::start(external_bus, rotation);
 
 	/*
 	 * Test the driver/device.
 	 */
-	if (!strcmp(argv[1], "test"))
+	if (!strcmp(verb, "test"))
 		l3gd20::test();
 
 	/*
 	 * Reset the driver.
 	 */
-	if (!strcmp(argv[1], "reset"))
+	if (!strcmp(verb, "reset"))
 		l3gd20::reset();
 
 	/*
 	 * Print driver information.
 	 */
-	if (!strcmp(argv[1], "info"))
+	if (!strcmp(verb, "info"))
 		l3gd20::info();
 
 	errx(1, "unrecognized command, try 'start', 'test', 'reset' or 'info'");