From 1913ae19f166d61038e436eff73efa42ac4d8f13 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 17 Aug 2014 10:45:30 +0200 Subject: add param for qnh --- src/drivers/ms5611/ms5611.cpp | 4 ++-- src/modules/sensors/sensor_params.c | 10 ++++++++++ src/modules/sensors/sensors.cpp | 40 +++++++++++++++++++++++++++++-------- 3 files changed, 44 insertions(+), 10 deletions(-) (limited to 'src') 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..d8e1fe9c8 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -235,7 +235,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 +258,7 @@ private: int board_rotation; int external_mag_rotation; - + float board_offset[3]; int rc_map_roll; @@ -301,6 +301,8 @@ private: float battery_voltage_scaling; float battery_current_scaling; + float baro_qnh; + } _parameters; /**< local copies of interesting parameters */ struct { @@ -354,9 +356,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 */ @@ -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,36 @@ 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) { + errx(ret, "qnh could not be set"); + } + } + close(fd); + return OK; } -- cgit v1.2.3 From 91d8cb2edcda1fde3c1765a659b2445592ed03f1 Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 17 Aug 2014 13:45:51 +0200 Subject: sensors: move close to correct position --- src/modules/sensors/sensors.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) (limited to 'src') diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index d8e1fe9c8..0d51667d0 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -862,8 +862,8 @@ Sensors::parameters_update() if (ret) { errx(ret, "qnh could not be set"); } + close(fd); } - close(fd); return OK; } -- cgit v1.2.3 From 86ae2e489fdd3656faa38202f44e4b53342e450b Mon Sep 17 00:00:00 2001 From: Thomas Gubler Date: Sun, 17 Aug 2014 15:21:27 +0200 Subject: sensors: do not exit task on error --- src/modules/sensors/sensors.cpp | 16 +++++++++------- 1 file changed, 9 insertions(+), 7 deletions(-) (limited to 'src') diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 0d51667d0..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 * @@ -466,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; } @@ -860,7 +860,9 @@ Sensors::parameters_update() warnx("qnh ioctl, %lu", (unsigned long)(_parameters.baro_qnh * 100)); int ret = ioctl(fd, BAROIOCSMSLPRESSURE, (unsigned long)(_parameters.baro_qnh * 100)); if (ret) { - errx(ret, "qnh could not be set"); + warnx("qnh could not be set"); + close(fd); + return ERROR; } close(fd); } -- cgit v1.2.3