fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-20 14:09:06 +08:00
Code Issues Packages Projects Releases Wiki Activity

Merge remote-tracking branch 'upstream/qnh' into obcfailsafe

Browse Source
This commit is contained in:
Thomas Gubler 2014-08-17 10:46:31 +02:00
commit a2cab83391
3 changed files with 44 additions and 10 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
src/drivers/ms5611/ms5611.cpp
View File

@ -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;
}

10
src/modules/sensors/sensor_params.c
View File

@ -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

40
src/modules/sensors/sensors.cpp
View File

@ -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;
}