Add support for rotations of PX4flow

5 files changed, 92 insertions, 15 deletions

diff --git a/src/drivers/drv_px4flow.h b/src/drivers/drv_px4flow.h
index ab640837b..5aed3f02b 100644
--- a/src/drivers/drv_px4flow.h
+++ b/src/drivers/drv_px4flow.h
@@ -32,7 +32,7 @@
  ****************************************************************************/
 
 /**
- * @file Rangefinder driver interface.
+ * @file PX4Flow driver interface.
  */
 
 #ifndef _DRV_PX4FLOW_H
diff --git a/src/drivers/drv_sensor.h b/src/drivers/drv_sensor.h
index 8e8b2c1b9..5e4598de8 100644
--- a/src/drivers/drv_sensor.h
+++ b/src/drivers/drv_sensor.h
@@ -82,8 +82,19 @@
 #define SENSORIOCGQUEUEDEPTH	_SENSORIOC(3)
 
 /**
- * Reset the sensor to its default configuration.
+ * Reset the sensor to its default configuration
  */
 #define SENSORIOCRESET		_SENSORIOC(4)
 
-#endif /* _DRV_SENSOR_H */
\ No newline at end of file
+/**
+ * Set the sensor orientation
+ */
+#define SENSORIOCSROTATION	_SENSORIOC(5)
+
+/**
+ * Get the sensor orientation
+ */
+#define SENSORIOCGROTATION	_SENSORIOC(6)
+
+#endif /* _DRV_SENSOR_H */
+
diff --git a/src/drivers/px4flow/px4flow.cpp b/src/drivers/px4flow/px4flow.cpp
index 62308fc65..99a9cc02a 100644
--- a/src/drivers/px4flow/px4flow.cpp
+++ b/src/drivers/px4flow/px4flow.cpp
@@ -62,6 +62,8 @@
 #include <systemlib/perf_counter.h>
 #include <systemlib/err.h>
 
+#include <conversion/rotation.h>
+
 #include <drivers/drv_hrt.h>
 #include <drivers/drv_px4flow.h>
 #include <drivers/device/ringbuffer.h>
@@ -125,7 +127,7 @@ struct i2c_integral_frame f_integral;
 class PX4FLOW: public device::I2C
 {
 public:
-	PX4FLOW(int bus, int address = I2C_FLOW_ADDRESS);
+	PX4FLOW(int bus, int address = I2C_FLOW_ADDRESS, enum Rotation rotation = (enum Rotation)0);
 	virtual ~PX4FLOW();
 
 	virtual int 		init();
@@ -154,6 +156,8 @@ private:
 	perf_counter_t		_sample_perf;
 	perf_counter_t		_comms_errors;
 	perf_counter_t		_buffer_overflows;
+	
+	enum Rotation				_sensor_rotation;
 
 	/**
 	 * Test whether the device supported by the driver is present at a
@@ -199,7 +203,7 @@ private:
  */
 extern "C" __EXPORT int px4flow_main(int argc, char *argv[]);
 
-PX4FLOW::PX4FLOW(int bus, int address) :
+PX4FLOW::PX4FLOW(int bus, int address, enum Rotation rotation) :
 	I2C("PX4FLOW", PX4FLOW_DEVICE_PATH, bus, address, 400000), //400khz
 	_reports(nullptr),
 	_sensor_ok(false),
@@ -208,7 +212,8 @@ PX4FLOW::PX4FLOW(int bus, int address) :
 	_px4flow_topic(-1),
 	_sample_perf(perf_alloc(PC_ELAPSED, "px4flow_read")),
 	_comms_errors(perf_alloc(PC_COUNT, "px4flow_comms_errors")),
-	_buffer_overflows(perf_alloc(PC_COUNT, "px4flow_buffer_overflows"))
+	_buffer_overflows(perf_alloc(PC_COUNT, "px4flow_buffer_overflows")),
+	_sensor_rotation(rotation)
 {
 	// enable debug() calls
 	_debug_enabled = false;
@@ -358,8 +363,12 @@ PX4FLOW::ioctl(struct file *filp, int cmd, unsigned long arg)
 			return OK;
 		}
 
-	case SENSORIOCGQUEUEDEPTH:
-		return _reports->size();
+	case SENSORIOCSROTATION:
+		_sensor_rotation = (enum Rotation)arg;
+		return OK;
+
+	case SENSORIOCGROTATION:
+		return _sensor_rotation;
 
 	case SENSORIOCRESET:
 		/* XXX implement this */
@@ -535,6 +544,9 @@ PX4FLOW::collect()
 	report.gyro_temperature = f_integral.gyro_temperature;//Temperature * 100 in centi-degrees Celsius
 
 	report.sensor_id = 0;
+	
+	/* rotate measurements to flight controller frame */
+	rotate_3f(_sensor_rotation, report.pixel_flow_x_integral, report.pixel_flow_y_integral, report.ground_distance_m);
 
 	if (_px4flow_topic < 0) {
 		_px4flow_topic = orb_advertise(ORB_ID(optical_flow), &report);
diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c
index 229bfe3ce..f5f6094d1 100644
--- a/src/modules/sensors/sensor_params.c
+++ b/src/modules/sensors/sensor_params.c
@@ -262,6 +262,42 @@ PARAM_DEFINE_FLOAT(SENS_BARO_QNH, 1013.25f);
 PARAM_DEFINE_INT32(SENS_BOARD_ROT, 0);
 
 /**
+ * PX4Flow board rotation
+ *
+ * This parameter defines the rotation of the PX4FLOW board relative to the platform.
+ * Possible values are:
+ *    0 = No rotation
+ *    1 = Yaw 45°
+ *    2 = Yaw 90°
+ *    3 = Yaw 135°
+ *    4 = Yaw 180°
+ *    5 = Yaw 225°
+ *    6 = Yaw 270°
+ *    7 = Yaw 315°
+ *    8 = Roll 180°
+ *    9 = Roll 180°, Yaw 45°
+ *   10 = Roll 180°, Yaw 90°
+ *   11 = Roll 180°, Yaw 135°
+ *   12 = Pitch 180°
+ *   13 = Roll 180°, Yaw 225°
+ *   14 = Roll 180°, Yaw 270°
+ *   15 = Roll 180°, Yaw 315°
+ *   16 = Roll 90°
+ *   17 = Roll 90°, Yaw 45°
+ *   18 = Roll 90°, Yaw 90°
+ *   19 = Roll 90°, Yaw 135°
+ *   20 = Roll 270°
+ *   21 = Roll 270°, Yaw 45°
+ *   22 = Roll 270°, Yaw 90°
+ *   23 = Roll 270°, Yaw 135°
+ *   24 = Pitch 90°
+ *   25 = Pitch 270°
+ *
+ * @group Sensor Calibration
+ */
+PARAM_DEFINE_INT32(SENS_FLOW_ROT, 0);
+
+/**
  * Board rotation Y (Pitch) offset
  *
  * This parameter defines a rotational offset in degrees around the Y (Pitch) axis. It allows the user
diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp
index cdcb428dd..d8df2c8ff 100644
--- a/src/modules/sensors/sensors.cpp
+++ b/src/modules/sensors/sensors.cpp
@@ -63,6 +63,7 @@
 #include <drivers/drv_rc_input.h>
 #include <drivers/drv_adc.h>
 #include <drivers/drv_airspeed.h>
+#include <drivers/drv_px4flow.h>
 
 #include <systemlib/systemlib.h>
 #include <systemlib/param/param.h>
@@ -263,6 +264,7 @@ private:
 		float diff_pres_analog_scale;
 
 		int board_rotation;
+		int flow_rotation;
 		int external_mag_rotation;
 
 		float board_offset[3];
@@ -361,6 +363,7 @@ private:
 		param_t battery_current_scaling;
 
 		param_t board_rotation;
+		param_t flow_rotation;
 		param_t external_mag_rotation;
 
 		param_t board_offset[3];
@@ -614,6 +617,7 @@ Sensors::Sensors() :
 
 	/* rotations */
 	_parameter_handles.board_rotation = param_find("SENS_BOARD_ROT");
+	_parameter_handles.flow_rotation = param_find("SENS_FLOW_ROT");
 	_parameter_handles.external_mag_rotation = param_find("SENS_EXT_MAG_ROT");
 
 	/* rotation offsets */
@@ -831,8 +835,22 @@ Sensors::parameters_update()
 	}
 
 	param_get(_parameter_handles.board_rotation, &(_parameters.board_rotation));
+	param_get(_parameter_handles.flow_rotation, &(_parameters.flow_rotation));
 	param_get(_parameter_handles.external_mag_rotation, &(_parameters.external_mag_rotation));
 
+	/* set px4flow rotation */
+	int	flowfd;
+	flowfd = open(PX4FLOW_DEVICE_PATH, 0);
+	if (flowfd >= 0) {
+ 		int flowret = ioctl(flowfd, SENSORIOCSROTATION, _parameters.flow_rotation);
+ 		if (flowret) {
+ 			warnx("flow rotation could not be set");
+			close(flowfd);
+			return ERROR;
+ 		}
+		close(flowfd);
+ 	}	
+	
 	get_rot_matrix((enum Rotation)_parameters.board_rotation, &_board_rotation);
 	get_rot_matrix((enum Rotation)_parameters.external_mag_rotation, &_external_mag_rotation);
 
@@ -850,20 +868,20 @@ Sensors::parameters_update()
 
 	/* update barometer qnh setting */
 	param_get(_parameter_handles.baro_qnh, &(_parameters.baro_qnh));
-	int	fd;
-	fd = open(BARO_DEVICE_PATH, 0);
-	if (fd < 0) {
+	int	barofd;
+	barofd = open(BARO_DEVICE_PATH, 0);
+	if (barofd < 0) {
 		warn("%s", BARO_DEVICE_PATH);
 		errx(1, "FATAL: no barometer found");
 
 	} else {
-		int ret = ioctl(fd, BAROIOCSMSLPRESSURE, (unsigned long)(_parameters.baro_qnh * 100));
-		if (ret) {
+		int baroret = ioctl(barofd, BAROIOCSMSLPRESSURE, (unsigned long)(_parameters.baro_qnh * 100));
+		if (baroret) {
 			warnx("qnh could not be set");
-			close(fd);
+			close(barofd);
 			return ERROR;
 		}
-		close(fd);
+		close(barofd);
 	}
 
 	return OK;