Merge pull request #1283 from PX4/qnh

add param for qnh

3 files changed, 52 insertions, 16 deletions

diff --git a/src/drivers/ms5611/ms5611.cpp b/src/drivers/ms5611/ms5611.cpp
index 873fa62c4..889643d0d 100644
--- a/src/drivers/ms5611/ms5611.cpp
+++ b/src/drivers/ms5611/ms5611.cpp
@@ -130,7 +130,7 @@ protected:
 	float			_T;
 
 	/* altitude conversion calibration */
-	unsigned		_msl_pressure;	/* in kPa */
+	unsigned		_msl_pressure;	/* in Pa */
 
 	orb_advert_t		_baro_topic;
 
@@ -466,7 +466,7 @@ MS5611::ioctl(struct file *filp, int cmd, unsigned long arg)
 			irqrestore(flags);
 			return -ENOMEM;
 		}
-		irqrestore(flags);		
+		irqrestore(flags);
 		return OK;
 	}
 
diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c
index 7ce6ef5ef..229bfe3ce 100644
--- a/src/modules/sensors/sensor_params.c
+++ b/src/modules/sensors/sensor_params.c
@@ -214,6 +214,16 @@ PARAM_DEFINE_FLOAT(SENS_DPRES_OFF, 0.0f);
  */
 PARAM_DEFINE_FLOAT(SENS_DPRES_ANSC, 0);
 
+/**
+ * QNH for barometer
+ *
+ * @min 500
+ * @max 1500
+ * @group Sensor Calibration
+ * @unit hPa
+ */
+PARAM_DEFINE_FLOAT(SENS_BARO_QNH, 1013.25f);
+
 
 /**
  * Board rotation
diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp
index 4e8a8c01d..aac297ef8 100644
--- a/src/modules/sensors/sensors.cpp
+++ b/src/modules/sensors/sensors.cpp
@@ -143,6 +143,12 @@
 
 #define STICK_ON_OFF_LIMIT 0.75f
 
+/* oddly, ERROR is not defined for c++ */
+#ifdef ERROR
+# undef ERROR
+#endif
+static const int ERROR = -1;
+
 /**
  * Sensor app start / stop handling function
  *
@@ -235,7 +241,7 @@ private:
 	math::Matrix<3, 3>	_board_rotation;		/**< rotation matrix for the orientation that the board is mounted */
 	math::Matrix<3, 3>	_external_mag_rotation;		/**< rotation matrix for the orientation that an external mag is mounted */
 	bool		_mag_is_external;		/**< true if the active mag is on an external board */
-	
+
 	uint64_t _battery_discharged;			/**< battery discharged current in mA*ms */
 	hrt_abstime _battery_current_timestamp;	/**< timestamp of last battery current reading */
 
@@ -258,7 +264,7 @@ private:
 
 		int board_rotation;
 		int external_mag_rotation;
-		
+
 		float board_offset[3];
 
 		int rc_map_roll;
@@ -301,6 +307,8 @@ private:
 		float battery_voltage_scaling;
 		float battery_current_scaling;
 
+		float baro_qnh;
+
 	}		_parameters;			/**< local copies of interesting parameters */
 
 	struct {
@@ -354,9 +362,11 @@ private:
 
 		param_t board_rotation;
 		param_t external_mag_rotation;
-		
+
 		param_t board_offset[3];
 
+		param_t baro_qnh;
+
 	}		_parameter_handles;		/**< handles for interesting parameters */
 
 
@@ -462,12 +472,6 @@ private:
 namespace sensors
 {
 
-/* oddly, ERROR is not defined for c++ */
-#ifdef ERROR
-# undef ERROR
-#endif
-static const int ERROR = -1;
-
 Sensors	*g_sensors = nullptr;
 }
 
@@ -611,12 +615,15 @@ Sensors::Sensors() :
 	/* rotations */
 	_parameter_handles.board_rotation = param_find("SENS_BOARD_ROT");
 	_parameter_handles.external_mag_rotation = param_find("SENS_EXT_MAG_ROT");
-	
+
 	/* rotation offsets */
 	_parameter_handles.board_offset[0] = param_find("SENS_BOARD_X_OFF");
 	_parameter_handles.board_offset[1] = param_find("SENS_BOARD_Y_OFF");
 	_parameter_handles.board_offset[2] = param_find("SENS_BOARD_Z_OFF");
 
+	/* Barometer QNH */
+	_parameter_handles.baro_qnh = param_find("SENS_BARO_QNH");
+
 	/* fetch initial parameter values */
 	parameters_update();
 }
@@ -828,19 +835,38 @@ Sensors::parameters_update()
 
 	get_rot_matrix((enum Rotation)_parameters.board_rotation, &_board_rotation);
 	get_rot_matrix((enum Rotation)_parameters.external_mag_rotation, &_external_mag_rotation);
-	
+
 	param_get(_parameter_handles.board_offset[0], &(_parameters.board_offset[0]));
 	param_get(_parameter_handles.board_offset[1], &(_parameters.board_offset[1]));
 	param_get(_parameter_handles.board_offset[2], &(_parameters.board_offset[2]));
-	
+
 	/** fine tune board offset on parameter update **/
-	math::Matrix<3, 3> board_rotation_offset; 
+	math::Matrix<3, 3> board_rotation_offset;
 	board_rotation_offset.from_euler( M_DEG_TO_RAD_F * _parameters.board_offset[0],
 							 M_DEG_TO_RAD_F * _parameters.board_offset[1],
 							 M_DEG_TO_RAD_F * _parameters.board_offset[2]);
-	
+
 	_board_rotation = _board_rotation * board_rotation_offset;
 
+	/* update barometer qnh setting */
+	param_get(_parameter_handles.baro_qnh, &(_parameters.baro_qnh));
+	int	fd;
+	fd = open(BARO_DEVICE_PATH, 0);
+	if (fd < 0) {
+		warn("%s", BARO_DEVICE_PATH);
+		errx(1, "FATAL: no barometer found");
+
+	} else {
+		warnx("qnh ioctl, %lu", (unsigned long)(_parameters.baro_qnh * 100));
+		int ret = ioctl(fd, BAROIOCSMSLPRESSURE, (unsigned long)(_parameters.baro_qnh * 100));
+		if (ret) {
+			warnx("qnh could not be set");
+			close(fd);
+			return ERROR;
+		}
+		close(fd);
+	}
+
 	return OK;
 }